一種兩輪自平衡機器人滑模自適應(yīng)控制方法
【專利摘要】本發(fā)明公開了一種兩輪自平衡機器人滑模自適應(yīng)控制方法，包括以下步驟：通過傳感器測量模塊采集自平衡機器人運動參數(shù)；在主控芯片中設(shè)置滑模自適應(yīng)控制器，該滑模自適應(yīng)控制器根據(jù)實時輸入的角度參量θ和角速度控制輸出電壓U從而驅(qū)動電機系統(tǒng)運動；所述滑模自適應(yīng)控制器的輸出方程為：U＝?(K+φ)X。采用本發(fā)明的技術(shù)方案，所使用的方法能夠?qū)ν饨绛h(huán)境進行自適應(yīng)同時能夠最大程度降低外界環(huán)境中各種干擾對兩輪自平衡機器人的影響并且不損失魯棒性，同時本發(fā)明還會利用機器學習的方法對一些長期積累因素進行磨合使兩輪自平衡機器人具有最優(yōu)的性能，從而保證了安全性與穩(wěn)定性。
【專利說明】
一種兩輪自平衡機器人滑模自適應(yīng)控制方法
技術(shù)領(lǐng)域
[0001 ]本發(fā)明涉及兩輪自平衡機器人控制領(lǐng)域，尤其涉及一種兩輪自平衡機器人滑模自 適應(yīng)控制方法。
【背景技術(shù)】
[0002] 兩輪自平衡機器人是一種利用傳感器感知自身狀態(tài)，然后通過控制算法控制馬達 轉(zhuǎn)動，從而實現(xiàn)自平衡。近年來，隨著兩輪自平衡機器人技術(shù)不斷完善以及成本不斷降低， 逐漸成為更多人接受的代步工具，使兩輪自平衡機器人開始從實驗研究階段轉(zhuǎn)變?yōu)榇蟊娦?的代步工具，其所面臨的環(huán)境和任務(wù)也越來越復雜。
[0003] 目前市場上有各種類型的平衡機器人，大多使用PID控制算法，該算法通過采集兩 輪自平衡機器人當前角度并計算與目標角度的偏差，在將這個偏差進行比例、積分、微分運 算計算出馬達控制量從而實現(xiàn)兩輪自平衡機器人自平衡。這種算法簡單實用但并不是最理 想的控制器，因為在復雜的運行環(huán)境中，該算法在很多時候處理的并不是很好，比如，該方 法在外界存在干擾時，就會使控制出現(xiàn)抖震，在干擾特別大時，還會使平衡車失去平衡；同 時，PID算法使用比例、積分、微分這三個成員進行線性組合也是不合理的，這種線性組合的 方式會使其在系統(tǒng)魯棒性和系統(tǒng)穩(wěn)定性上無法兩者兼顧，提高魯棒性會使穩(wěn)定性降低，反 之提高穩(wěn)定性則降低魯棒性。也就是說使用PID算法的平衡車如果把魯棒性調(diào)高，則它具有 很強的保持直立的能力但一旦角度偏差過大則容易使其失去控制，從而造成危險的結(jié)果， 如果把穩(wěn)定性調(diào)高則，則會使其魯棒性降低，從而導致平衡車承受負載的能力下降。總而言 之，PID算法魯棒性不夠好，響應(yīng)速度不夠快，面對較大的擾動時，系統(tǒng)不穩(wěn)定，當外部路面 條件變化的時候，不能自適應(yīng)較復雜的外部環(huán)境以及大范圍負載的變化，使系統(tǒng)的抖振非 常大。
[0004] 故，針對目前現(xiàn)有技術(shù)中存在的上述缺陷，實有必要進行研究，以提供一種方案， 解決現(xiàn)有技術(shù)中存在的缺陷。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明的目的是一種兩輪自平衡機器人滑模自適應(yīng)控制方法，使建模過程更加精 簡且全面、增強系統(tǒng)的魯棒性、提高系統(tǒng)的響應(yīng)速度;能夠應(yīng)對較大的外部擾動;能夠自適 應(yīng)外部環(huán)境以及大范圍負載的變化;能夠自動檢測負載的加入;系統(tǒng)中參數(shù)的值更加精確； 速度控制方式多樣化。
[0006] 為了克服現(xiàn)有技術(shù)存在的缺陷，本發(fā)明的技術(shù)方案為：
[0007] -種兩輪自平衡機器人滑模自適應(yīng)控制方法，包括以下步驟：
[0008] 通過傳感器測量模塊采集自平衡機器人運動參數(shù)，該運動參數(shù)至少包括角速度信 號和加速度信號；
[0009] 根據(jù)角速度信號和加速度信號得出角度參量Θ和角速度力；
[0010] 在主控芯片中設(shè)置滑模自適應(yīng)控制器，所述滑模自適應(yīng)控制器根據(jù)實時輸入的角 度參量Θ和角速度冷控制輸出電壓U從而驅(qū)動電機系統(tǒng)運動；
[0011 ]所述滑模自適應(yīng)控制器的輸出方程為：
[0012] υ = -(Κ+Φ )Χ；
[0013] 其中，X為角度參量Θ和角速度#的集合，K是通過極點計算出來的參數(shù)矩陣；Φ取 值是根據(jù)以下公式?jīng)Q定：
[0014]
是自適應(yīng)速率，e為角度誤差參數(shù)，C取[0 0 I 1]。
[0015] 優(yōu)選地，所述滑模自適應(yīng)控制器還包括機器學習表(Map)，所述機器學習表(Map) 根據(jù)輸入的角度參量Θ和角速度6調(diào)節(jié)輸出電壓U。
[0016]優(yōu)選地，所述滑模自適應(yīng)控制器還包括機器學習模塊，所述學習模塊根據(jù)輸入的 角度參量Θ和角速度#更新所述機器學習表(Map)。
[0017]優(yōu)選地，所述滑模自適應(yīng)控制器的輸出方程為：
[0018] υ = _(Κ+Φ )X+MaD(X):其中，
[0019]
[0020] 優(yōu)選地，通過陀螺儀采集角速度信號，所述陀螺儀的型號為L3G420D。
[0021]優(yōu)選地，通過加速度計采集加速度信號，所述加速度計的型號為LSM303D。
[0022] 優(yōu)選地，通過通訊模塊實現(xiàn)自平衡機器人與外部設(shè)備進行數(shù)據(jù)通訊。
[0023] 優(yōu)選地，通過設(shè)置轉(zhuǎn)向桿線性霍爾傳感器實現(xiàn)自平衡機器人轉(zhuǎn)向控制。
[0024] 優(yōu)選地，所述主控芯片采用DSP芯片。
[0025] 優(yōu)選地，所述通訊模塊為無線數(shù)據(jù)傳輸模塊。
[0026] 與現(xiàn)有技術(shù)相比較，本發(fā)明所使用的方法能夠?qū)ν饨绛h(huán)境進行自適應(yīng)同時能夠最 大程度降低外界環(huán)境中各種干擾對兩輪自平衡機器人的影響并且不損失魯棒性，同時本發(fā) 明還會利用機器學習的方法對一些長期積累因素(如兩輪自平衡機器人的機械特性或人為 的操作習慣)進行磨合使兩輪自平衡機器人具有最優(yōu)的性能，從而保證了安全性與穩(wěn)定性。 [0027] 說明書附圖
[0028] 圖1為本發(fā)明兩輪自平衡機器人滑模自適應(yīng)控制方法的流程框圖；
[0029] 圖2為本發(fā)明采用的倒立擺模型結(jié)構(gòu)；
[0030] 圖3為本發(fā)明中滑模自適應(yīng)控制器的結(jié)構(gòu)框圖；
[0031] 圖4為本發(fā)明中兩輪自平衡機器人控制系統(tǒng)的結(jié)構(gòu)框圖；
[0032] 圖5為本發(fā)明中兩輪自平衡機器人控制系統(tǒng)的執(zhí)行流程圖；
[0033]圖6為仿真運行一段時間后地機器學習庫的狀態(tài)圖；
[0034]圖7為仿真時本發(fā)明在一定仿真條件的角度誤差圖；
[0035]圖8為仿真時本發(fā)明沒有機器學習模塊情況下在一定仿真條件的角度誤差圖；
[0036] 圖9為與在同樣的仿真條件下傳統(tǒng)PID算法的角度誤差圖。
【具體實施方式】
[0037] 參見圖1，所示為本發(fā)明一種兩輪自平衡機器人滑模自適應(yīng)控制方法的流程框圖， 包括以下步驟：
[0038] 步驟SI:在自平衡機器人的主控芯片中設(shè)置滑模自適應(yīng)控制器;其中，滑模自適應(yīng) 控制器的輸出方程為：
[0039] υ = -(Κ+Φ )X；
[0040] 其中，X為角度參量Θ和角速度力的集合，K是通過極點計算出來的參數(shù)矩陣；Φ取值 是根據(jù)以下公式?jīng)Q定：
[0041 ]
是自適應(yīng)速率，e為角度誤差參數(shù)，C取[0 0 I 1]。
[0042] 步驟S2:通過傳感器模塊采集自平衡機器人運動參數(shù)，該運動參數(shù)至少包括角速 度信號和加速度信號，通過陀螺儀和加速度計采集角速度信號和加速度信號，其中，陀螺儀 的型號為L3G420D，加速度計的型號為LSM303D。
[0043] 主控芯片獲取角速度信號和加速度信號，通過頂U算法計算出4元數(shù)，再通過4元數(shù) 還原出三個方位的歐拉角。從而計算得到角度參量Θ和角速度冷。
[0044] 步驟S3:滑模自適應(yīng)控制器獲取傳感參數(shù)角度參量Θ和角速度&，根據(jù)上述輸出方 程控制輸出電壓進而驅(qū)動電機系統(tǒng)運動。電機系統(tǒng)運動使角度參量Θ和角速度^發(fā)生改變 并反饋至滑模自適應(yīng)控制器又調(diào)節(jié)輸出電壓U，不斷循環(huán)該過程，使自平衡機器人始終能夠 回歸自平衡狀態(tài)。
[0045] 采用上述技術(shù)方案，能夠最大程度降低外界環(huán)境中各種干擾。
[0046] 在步驟1中采用的滑模自適應(yīng)控制器的設(shè)計原理如下：
[0047] 兩輪自平衡機器人的系統(tǒng)可以等效看作是一個倒立擺模型，倒立擺模型為現(xiàn)有技 術(shù)通用的動力模型。從能量和動量角度分析，利用拉格朗日動力學理論，可以得到以下描 述：
[0048] (!)
[0049] U = -mgl+mglcos9 (2)
[0050] (1)式和(2)式中，m為車身質(zhì)量，Mw為轉(zhuǎn)子(輪胎)質(zhì)量，1為擺桿長度、J e3為平衡車轉(zhuǎn) 動慣量、Jm為轉(zhuǎn)子(輪胎)轉(zhuǎn)動慣量，這些參量都為自平衡機器人的固有參量，取決于自平衡 機器人機械架構(gòu);在倒立擺模型下的不同機械架構(gòu)，上述參量會發(fā)生變化。
[0051] 其中，Xw為路程、為速度、Θ為角度和^為角速度為自平衡機器人的運動參量，這 些數(shù)據(jù)可以通過傳感器采集到。
[0052]在兩輪自平衡機器人控制中，Θ變化范圍很小所以COS0可以近似為l，si n0可以近 似為Θ，然后根據(jù)(1)、（ 2)兩個方程聯(lián)立可以得到：
[0061 ]由公式(7)可以選擇合適的系統(tǒng)控制極點設(shè)計出控制器(一般p = [ VI，v2，v3，v4 ]， ￥1，￥2，￥3，￥4〈0利用11^1：1313中口13〇6函數(shù)計算出1(，1( = 口13〇6(厶，13，口）），從而得到下式：
[0062] U = -K · X (8)
[0063] 其中，K是通過選擇合適極點計算出來的參數(shù)矩陣，：
[0064] 式(8)的控制器輸出方程能夠很好的實現(xiàn)直立自平衡，并且在控制精度上遠優(yōu)于 PD控制，在極端情況中不易失控，但這個控制器缺少對外界條件的適應(yīng)力，因此，本發(fā)明在 此基礎(chǔ)上還對控制器進行進一步優(yōu)化。
[0065] 參見圖3,所示為滑模自適應(yīng)控制器的原理框圖，為了提高控制器的自適應(yīng)能力， 根據(jù)滑?？刂评碚撛诳刂破骷尤牖⒘俊；５淖畲髢?yōu)點在于魯棒性強并且對于外界條 件變化引起的參數(shù)攝動具有很強的免疫力。于是將控制器設(shè)計為：
[0066] υ = _(Κ+Φ)Χ (9)
[0067] 其4
Φ取值是根據(jù)以下公式?jīng)Q定：
[0068]
(.10)
[0069] 滑模參量Φ會根據(jù)實際采集值與誤差進行不斷累加和更新，當控制力度不足或過 渡時Φ就會發(fā)生變化增大或減少，從而一直保持控制器輸出最優(yōu)結(jié)果，從而實現(xiàn)對參數(shù)攝 動的抵抗作用。其中，sign為符號函數(shù)，γ是自適應(yīng)速率，為常量，實際調(diào)試中選取合適值;e 為角度誤差參數(shù)，采集角度和期望角度的差值。
[0070] 為了使控制器輸出方程穩(wěn)定，必須符合李雅普諾夫穩(wěn)定性原理，證明如下：
[0071] (7)式動力學方程可簡化為：
[0072]
[0073]
[0074]將重新設(shè)計的控制器(9)式代入動力學方程，可以得到：
[0075]
[0076]
[0077]其中V為控制器輸入向量。
[0078] 定義矩陣P，若存在矩陣Q滿足ATP+PA = -Q，PB = C，C取[0 0 I 1]，這里只要滿足矩 陣A是滿秩，就可以證明Q存在。通過Matlab計算出r (A) = 4。
[0079] 由此可見矩陣A為滿秩矩陣。然后構(gòu)造能量函數(shù)：
[0080]
[0081]
[0082]
[0083]由此證明了該控制器滿足李雅普諾夫穩(wěn)定性原理。
[0084]通過上述技術(shù)方案，顯著提高了自適應(yīng)能力，但對在使用過程中的長期變化會每 次都重新匹配，比如兩輪自平衡機器人的機械特性或人為的操作習慣，無法智能匹配。 [0085]為了解決上述技術(shù)問題，本發(fā)明根據(jù)機器學習的理論，對兩輪自平衡機器人的控 制器進行進一步優(yōu)化使其能夠適應(yīng)人的各種習慣，在滑模自適應(yīng)控制器中增加機器學習表 (Map)，機器學習表(Map)根據(jù)環(huán)境因素和駕駛習慣在出廠時預設(shè)的一張或多張參數(shù)表，機 器學習表(Map)根據(jù)輸入的角度參量Θ和角速度S查找參數(shù)值，從而能夠調(diào)節(jié)輸出電壓U。 [00 86]在一種優(yōu)選實施方式中，在滑模自適應(yīng)控制器還設(shè)置機器學習模塊，學習模塊根 據(jù)輸入的角度參量Θ和角速度#更新機器學習表(Map)，使機器學習表(Map)始終存儲最佳參 數(shù)，從而能夠適應(yīng)人的各種習慣。
[0087]由此，控制器的輸出方程如下：
[0088] υ = -(Κ+Φ )X+Map(X) (16)
[0089] 該設(shè)計是在(9)式控制器輸出方程的基礎(chǔ)上進行擴展，其中Map(X)是通過機器學 習的方法對控制器進行優(yōu)化的參數(shù)表。該優(yōu)化主要針對兩輪自平衡機器人系統(tǒng)在實際情況 中的非線性因素，例如人的主觀控制，從而改善駕駛操控。
[0091]
[0090] 該控制器前段已在之前通過穩(wěn)定性證明，只需要證明后端，定義能量函數(shù)：
[0092] ;
[0093] -
[0094] ：
[0095] 〈
[0096] J
[0097] j
[0098] Map本質(zhì)上是一張查找表，該方法會對每一個Xb根據(jù)誤差和能量進行單獨修正，從 而使控制器在運行時能量I LI能夠收斂到區(qū)間（vmin，Vmax)內(nèi)從而使控制不會產(chǎn)生過渡響應(yīng)， 實際調(diào)試中在平衡車加速和減速時，由于加速和減速不平滑因此會產(chǎn)生較大的I L I該方法， 該方法使直立車加減速平滑化，給人一種比較柔和的感覺尤其是在起步和停車過程中，這 種感覺會更加明顯。
[0099] 參見圖4,所述為本發(fā)明兩輪自平衡機器人的系統(tǒng)框圖，包括傳感器測量模塊、主 控芯片、通訊模塊、轉(zhuǎn)向桿線性霍爾傳感器和電機系統(tǒng)，其中，傳感器測量模塊至少包括陀 螺儀和加速度計，分別用于采集角速度信號和加速度信號，其中，陀螺儀的型號為L3G420D， 加速度計的型號為LSM303D;主控芯片采用DSP芯片，在其中設(shè)置滑模自適應(yīng)控制器;通訊模 塊采用串口通信模塊或者無線數(shù)據(jù)傳輸模塊，用于與外部設(shè)備進行數(shù)據(jù)通訊，以便于系統(tǒng) 調(diào)試和維修檢測;轉(zhuǎn)向桿線性霍爾傳感器用于實現(xiàn)自平衡機器人轉(zhuǎn)向控制；電機系統(tǒng)至少 包括無刷電機及其驅(qū)動電路。
[0100] 本發(fā)明通過傳感器模塊獲取兩輪自平衡機器人姿態(tài)，通過轉(zhuǎn)向桿上的線性霍爾傳 感器獲取轉(zhuǎn)向期望信號，然后通過DSP芯片進行運算處理，分別計算出兩只無刷電機的控制 量，從而實現(xiàn)自平衡。在此同時本發(fā)明還將一些系統(tǒng)參數(shù)通過通訊模塊發(fā)送出去，以便實時 監(jiān)測與觀測。
[0101] 參見圖5,所述為本發(fā)明兩輪自平衡機器人的系統(tǒng)執(zhí)行流程圖，該系統(tǒng)在開始執(zhí)行 后首先進行初始化，然后分兩條不同頻率的任務(wù)，一條是方向控制，執(zhí)行周期為20ms;另一 條為本發(fā)明的平衡控制，執(zhí)行周期為5ms。其中平衡控制首先通過傳感器(陀螺儀和加速度 計)采集角速度信號和加速度信號，然后通過姿態(tài)計算計算出兩輪平衡車角度，然后根據(jù)角 度計算出自適應(yīng)律，并通過自適應(yīng)控制器計算出自適應(yīng)控制輸出，接著根據(jù)Map查找出最優(yōu) 的Map輸出，然后根據(jù)采集到的信號更新Map庫，再將Map輸出與自適應(yīng)輸出進行疊加從而計 算出平衡控制輸出，最后將直立控制和方向控制的控制輸出進行疊加然后濾波從而控制電 機輸出。
[0102] 參見圖6,所示為仿真運行一段時間后地機器學習庫的狀態(tài)圖，其中仿真條件是在 其輸入信號加載峰峰值為1頻率為3kHz的g高頻擾動和幅度為0.01的噪聲信號。在運行一段 時間后，其運行時出現(xiàn)的每個角度與角速度狀態(tài)都經(jīng)過機器學習、更新演化出如圖6所示的 機器學習 Map表，可以看出在經(jīng)機器學習以后，Map表對改機器學習的高頻擾動和噪聲信號 的反應(yīng)更為均勻，從而實現(xiàn)對控制器進行進一步的優(yōu)化。
[0103] 參見圖7,所示為本發(fā)明在輸入信號加載峰峰值為1頻率為3kHz的g高頻擾動和幅 度為0.01的噪聲信號仿真時的角度誤差變化曲線，圖8為在同樣仿真條件下移除機器學習 功能時的角度誤差變化曲線，圖9為在同樣仿真條件下傳統(tǒng)PID算法的角度誤差變化曲線。 從圖7,圖8和圖9的仿真結(jié)果對比可以很明顯的發(fā)現(xiàn)本發(fā)明的性能明顯要優(yōu)于傳統(tǒng)PID算 法，從圖7和圖8的對比，可以清楚的算出本發(fā)明中的機器學習模塊可以對與控制中的細節(jié) 部分進行進一步地改善從而增強了本發(fā)明的自適應(yīng)控制性能。
[0104] 以上實施例的說明只是用于幫助理解本發(fā)明的方法及其核心思想。應(yīng)當指z出，對 于本技術(shù)領(lǐng)域的普通技術(shù)人員來說，在不脫離本發(fā)明原理的前提下，還可以對本發(fā)明進行 若干改進和修飾，這些改進和修飾也落入本發(fā)明權(quán)利要求的保護范圍內(nèi)。
[0105] 對所公開的實施例的上述說明，使本領(lǐng)域?qū)I(yè)技術(shù)人員能夠?qū)崿F(xiàn)或使用本發(fā)明。 對這些實施例的多種修改對本領(lǐng)域的專業(yè)技術(shù)人員來說將是顯而易見的，本發(fā)明中所定義 的一般原理可以在不脫離本發(fā)明的精神或范圍的情況下，在其它實施例中實現(xiàn)。因此，本發(fā) 明將不會被限制于本發(fā)明所示的這些實施例，而是要符合與本發(fā)明所公開的原理和新穎特 點相一致的最寬的范圍。
【主權(quán)項】
1. 一種兩輪自平衡機器人滑模自適應(yīng)控制方法，其特征在于，包括以下步驟： 通過傳感器測量模塊采集自平衡機器人運動參數(shù)，該運動參數(shù)至少包括角速度信號和 加速度信號； 根據(jù)角速度信號和加速度信號得出角度參量9和角速度砝； 在主控芯片中設(shè)置滑模自適應(yīng)控制器，所述滑模自適應(yīng)控制器根據(jù)實時輸入的角度參 量9和角速度0控制輸出電壓U從而驅(qū)動電機系統(tǒng)運動； 所述滑模自適應(yīng)控制器的輸出方程為： U = -(K+<i) )x； 其中，X為角度參量0和角速度6的集合，K是通過極點計算出來的參數(shù)矩陣；(i)取值是根 據(jù)以下公式?jīng)Q定： 於=_-.吻《(|/(|);^(：-/1，丫是自適應(yīng)速率，6為角度誤差參數(shù)，(：取[0 0 11]。2. 根據(jù)權(quán)利要求1所述的兩輪自平衡機器人滑模自適應(yīng)控制方法，其特征在于，所述滑 模自適應(yīng)控制器還包括機器學習表(Map)，所述機器學習表(Map)根據(jù)輸入的角度參量0和 角速度g調(diào)節(jié)輸出電壓U。3. 根據(jù)權(quán)利要求2所述的兩輪自平衡機器人滑模自適應(yīng)控制方法，其特征在于，所述滑 模自適應(yīng)控制器還包括機器學習模塊，所述學習模塊根據(jù)輸入的角度參量9和角速度#更新 所述機器學習表(Map)。4. 根據(jù)權(quán)利要求3所述的兩輪自平衡機器人滑模自適應(yīng)控制方法，其特征在于，所述滑 模自適應(yīng)控制器的輸出方程為： U = -(K+<i) )X+Map(X);其中，5. 根據(jù)權(quán)利要求1所述的兩輪自平衡機器人滑模自適應(yīng)控制方法，其特征在于，通過陀 螺儀采集角速度信號，所述陀螺儀的型號為L3G420D。6. 根據(jù)權(quán)利要求1所述的兩輪自平衡機器人滑模自適應(yīng)控制方法，其特征在于，通過加 速度計采集加速度信號，所述加速度計的型號為LSM303D。7. 根據(jù)權(quán)利要求1所述的兩輪自平衡機器人滑模自適應(yīng)控制方法，其特征在于，通過通 訊模塊實現(xiàn)自平衡機器人與外部設(shè)備進行數(shù)據(jù)通訊。8. 根據(jù)權(quán)利要求1所述的兩輪自平衡機器人滑模自適應(yīng)控制方法，其特征在于，通過設(shè) 置轉(zhuǎn)向桿線性霍爾傳感器實現(xiàn)自平衡機器人轉(zhuǎn)向控制。9. 根據(jù)權(quán)利要求1所述的兩輪自平衡機器人滑模自適應(yīng)控制方法，其特征在于，所述主 控芯片采用DSP芯片。10. 根據(jù)權(quán)利要求7所述的兩輪自平衡機器人滑模自適應(yīng)控制方法，其特征在于，所述 通訊模塊為無線數(shù)據(jù)傳輸模塊。
【文檔編號】B25J9/16GK106041934SQ201610529407
【公開日】2016年10月26日
【申請日】2016年6月30日
【發(fā)明人】陳龍, 胡華, 滿志紅, 黃明, 馬學條
【申請人】杭州電子科技大學