本發(fā)明涉及兩輪自平衡機(jī)器人控制領(lǐng)域，尤其涉及一種基于部分已知參數(shù)的兩輪自平衡機(jī)器人控制系統(tǒng)。

背景技術(shù)：

兩輪自平衡機(jī)器人是一種利用傳感器感知自身狀態(tài)，然后通過控制算法控制馬達(dá)轉(zhuǎn)動，從而實(shí)現(xiàn)自平衡。近年來，隨著兩輪自平衡機(jī)器人技術(shù)不斷完善以及成本不斷降低，逐漸成為更多人接受的代步工具，使兩輪自平衡機(jī)器人開始從實(shí)驗(yàn)研究階段轉(zhuǎn)變?yōu)榇蟊娦偷拇焦ぞ?，其所面臨的環(huán)境和任務(wù)也越來越復(fù)雜。

目前市場上有各種類型的平衡機(jī)器人，大多使用PID控制算法，該算法通過采集兩輪自平衡機(jī)器人當(dāng)前角度并計(jì)算與目標(biāo)角度的偏差，在將這個偏差進(jìn)行比例、積分、微分運(yùn)算計(jì)算出馬達(dá)控制量從而實(shí)現(xiàn)兩輪自平衡機(jī)器人自平衡。這種算法簡單實(shí)用但并不是最理想的控制器，因?yàn)樵趶?fù)雜的運(yùn)行環(huán)境中，該算法在很多時候處理的并不是很好，比如，該方法在外界存在干擾時，就會使控制出現(xiàn)抖震，在干擾特別大時，還會使平衡車失去平衡；同時，PID算法使用比例、積分、微分這三個成員進(jìn)行線性組合也是不合理的，這種線性組合的方式會使其在系統(tǒng)魯棒性和系統(tǒng)穩(wěn)定性上無法兩者兼顧，提高魯棒性會使穩(wěn)定性降低，反之提高穩(wěn)定性則降低魯棒性。

與此同時，兩輪自平衡機(jī)器人在使用過程中會逐漸老化，或者其運(yùn)行環(huán)境發(fā)生巨大變化時，其固有參數(shù)會隨之發(fā)生變化，比如，轉(zhuǎn)子(輪胎)轉(zhuǎn)動慣量J_m，會隨著摩擦力變化而改變，以及其他一些物理參量也會在使用過程中發(fā)生變化。雖然這些固有參數(shù)的變化是緩慢的，但長期積累也會對控制器的輸出造成影響，從而使系統(tǒng)變得不穩(wěn)定，然而，現(xiàn)有技術(shù)的控制器并未考慮上述因素對其造成的影響。

故，針對目前現(xiàn)有技術(shù)中存在的上述缺陷，實(shí)有必要進(jìn)行研究，以提供一種方案，解決現(xiàn)有技術(shù)中存在的缺陷。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的是提供一種基于部分已知參數(shù)的兩輪自平衡機(jī)器人控制系統(tǒng)，能夠在外界條件發(fā)生變化或在有些參數(shù)發(fā)生攝動的情況下依然保持兩輪自平衡機(jī)器人擁有良好的性能。

為了克服現(xiàn)有技術(shù)的缺陷，本發(fā)明采用的技術(shù)方案為：

一種基于部分已知參數(shù)的兩輪自平衡機(jī)器人控制系統(tǒng)，包括傳感器測量模塊、主控芯片、速度手柄和電機(jī)系統(tǒng)，其中，所述主控芯片與所述傳感器測量模塊、速度手柄和電機(jī)系統(tǒng)相連接，通過所述速度手柄獲取望速度通過所述傳感器測量模塊獲取實(shí)際速度實(shí)際角度θ和角速度

所述主控芯片內(nèi)設(shè)置控制器，所述控制器根據(jù)所采集的自平衡機(jī)器人的運(yùn)動參數(shù)進(jìn)行信號處理并控制輸出電壓U驅(qū)動電機(jī)系統(tǒng)運(yùn)動；

所述控制器至少包括角度魯棒控制器、角度滑?？刂破?、速度魯棒控制器和速度滑?？刂破?，其中，所述速度魯棒控制器和速度滑模控制器根據(jù)速度誤差e_v獲取期望角度θ_r；

角度魯棒控制器和角度滑模控制器根據(jù)角度誤差e_θ調(diào)節(jié)輸出電壓U；

其中，所述控制器的輸出方程為：

其中，U₀為角度滑模控制器的輸出量，U₁為角度魯棒控制器的輸出量；

所述角度魯棒控制器的輸出方程為：

所述角度滑模控制器的輸出方程為：

其中，期望角度θ_r滿足為速度滑?？刂破鞯妮敵隽?，為速度魯棒控制器的輸出量；

所述速度魯棒控制器的輸出方程為：

所述速度滑模控制器的輸出方程為：

優(yōu)選地，所述速度手柄用于輸出線性霍爾信號并發(fā)送給所述主控芯片。

優(yōu)選地，傳感器測量模塊至少包括陀螺儀、加速度計(jì)和編碼器，所述陀螺儀、加速度計(jì)和編碼器與所述主控芯片相連接，所述編碼器用于采集速度信號；所述陀螺儀用于采集角速度信號，所述加速度計(jì)用于采集加速度信號和地磁信號，所述主控芯片接收速度信號、角速度信號加速度信號和地磁信號，并進(jìn)行信號處理后獲取實(shí)際速度實(shí)際角度θ和角速度

優(yōu)選地，所述陀螺儀的型號為L3G420D。

優(yōu)選地，所述加速度計(jì)的型號為LSM303D。

優(yōu)選地，還包括通訊模塊，所述通訊模塊用于實(shí)現(xiàn)自平衡機(jī)器人與外部設(shè)備進(jìn)行數(shù)據(jù)通訊。

優(yōu)選地，還設(shè)置轉(zhuǎn)向桿線性霍爾傳感器，所述轉(zhuǎn)向桿線性霍爾傳感器用于實(shí)現(xiàn)自平衡機(jī)器人轉(zhuǎn)向控制。

優(yōu)選地，所述主控芯片采用DSP芯片。

優(yōu)選地，所述通訊模塊采用無線數(shù)據(jù)傳輸模塊。

與現(xiàn)有技術(shù)相比較，本發(fā)明能夠利用部分已知參數(shù)實(shí)現(xiàn)優(yōu)化和改善兩輪自平衡機(jī)器人的性能，即便在外界條件發(fā)生劇烈變化或在有些參數(shù)發(fā)生攝動的情況下，依然能夠保持兩輪自平衡機(jī)器人的穩(wěn)定控制。

說明書附圖

圖1為本發(fā)明基于部分已知參數(shù)的兩輪自平衡機(jī)器人控制系統(tǒng)的結(jié)構(gòu)框圖；

圖2為本發(fā)明所采用的控制器的結(jié)構(gòu)框圖；

圖3為本發(fā)明中倒立擺模型的結(jié)構(gòu)框圖；

圖4為本發(fā)明中兩輪自平衡機(jī)器人控制系統(tǒng)的執(zhí)行流程圖；

圖5為仿真時輸入信號的參數(shù)攝動規(guī)律圖；

圖6為仿真時本發(fā)明速度跟蹤與傳統(tǒng)PID速度跟蹤對比圖；

圖7為仿真時本發(fā)明速度誤差與傳統(tǒng)PID速度誤差對比圖；

圖8為仿真時本發(fā)明角度跟蹤與傳統(tǒng)PID角度跟蹤對比圖；

圖9為仿真時本發(fā)明角度誤差與傳統(tǒng)PID角度誤差對比圖；

具體實(shí)施方式

參見圖1，所示為本發(fā)明一種基于部分已知參數(shù)的兩輪自平衡機(jī)器人控制系統(tǒng)的結(jié)構(gòu)框圖，包括傳感器測量模塊、主控芯片、通訊模塊、轉(zhuǎn)向桿線性霍爾傳感器、速度手柄和電機(jī)系統(tǒng)，主控芯片與傳感器測量模塊、通訊模塊、轉(zhuǎn)向桿線性霍爾傳感器、速度手柄和電機(jī)系統(tǒng)相連接；其中，傳感器測量模塊至少包括陀螺儀、加速度計(jì)和編碼器，編碼器安裝在電機(jī)系統(tǒng)中，用于測量電機(jī)的轉(zhuǎn)速，主控芯片通過編碼器獲取實(shí)際速度陀螺儀用于采集角速度信號以及加速度計(jì)用于采集加速度信號和地磁信號，主控芯片由此獲取實(shí)際角度θ和角速度其中，陀螺儀的型號為L3G420D，加速度計(jì)的型號為LSM303D；速度手柄用于輸出的線性霍爾信號，主控芯片通過該線性霍爾信號獲取期望速度主控芯片采用DSP芯片，并在其中設(shè)置通過上述方法設(shè)計(jì)的控制器；通訊模塊采用串口通信模塊或者無線數(shù)據(jù)傳輸模塊，用于與外部設(shè)備進(jìn)行數(shù)據(jù)通訊，以便于系統(tǒng)調(diào)試和維修檢測；轉(zhuǎn)向桿線性霍爾傳感器用于實(shí)現(xiàn)自平衡機(jī)器人轉(zhuǎn)向控制；電機(jī)系統(tǒng)至少包括無刷電機(jī)及其驅(qū)動電路。

主控芯片內(nèi)設(shè)置控制器，控制器根據(jù)所采集的自平衡機(jī)器人的運(yùn)動參數(shù)進(jìn)行信號處理并控制輸出電壓U驅(qū)動電機(jī)系統(tǒng)運(yùn)動，運(yùn)動參數(shù)至少包括期望速度實(shí)際速度實(shí)際角度θ和角速度

參見圖2，所示為控制器的原理框圖，至少包括角度魯棒控制器、角度滑?？刂破鳌⑺俣若敯艨刂破骱退俣然？刂破?，其中，

以期望速度和實(shí)際速度的速度誤差e_v作為速度魯棒控制器和速度滑模控制器的輸入信號，獲取期望角度θ_r；

以期望角度θ_r和實(shí)際角度θ的角度誤差e_θ和角速度作為角度魯棒控制器和角度滑?？刂破鞯妮斎胄盘?，根據(jù)角度誤差e_θ調(diào)節(jié)控制輸出電壓U從而驅(qū)動電機(jī)系統(tǒng)運(yùn)動。其中，控制器的輸出方程為：

其中，U₀為角度滑?？刂破鞯妮敵隽浚琔₁為角度魯棒控制器的輸出量；

進(jìn)一步的，角度魯棒控制器的輸出方程為：

進(jìn)一步的，角度滑?？刂破鞯妮敵龇匠虨椋?/p>

其中，期望角度θ_r滿足為速度滑模控制器的輸出量，為速度魯棒控制器的輸出量；

速度魯棒控制器的輸出方程為：

速度滑?？刂破鞯妮敵龇匠虨椋?/p>

上述控制器的設(shè)計(jì)原理如下：

兩輪自平衡機(jī)器人的系統(tǒng)可以等效看作是一個倒立擺模型，參見圖3，所示的倒立擺模型為現(xiàn)有技術(shù)通用的動力模型結(jié)構(gòu)。從能量和動量角度分析，利用拉格朗日動力學(xué)理論，可以得到以下描述：

U＝-mgl+mglcosθ (2)

(1)式和(2)式中，m為車身質(zhì)量，M_w為轉(zhuǎn)子(輪胎)質(zhì)量，l為擺桿長度、J_e為平衡車轉(zhuǎn)動慣量、J_m為轉(zhuǎn)子(輪胎)轉(zhuǎn)動慣量，這些參量都為自平衡機(jī)器人的固有參量，取決于自平衡機(jī)器人機(jī)械架構(gòu)；在倒立擺模型下的不同機(jī)械架構(gòu)，上述參量會發(fā)生變化。

其中，X_w為路程、為速度、θ為角度和為角速度為自平衡機(jī)器人的運(yùn)動參量，這些數(shù)據(jù)可以通過傳感器采集到。

在兩輪自平衡機(jī)器人控制中，θ變化范圍很小所以cosθ可以近似為1，sinθ可以近似為θ，然后根據(jù)(1)、(2)兩個方程聯(lián)立可以得到：

寫成狀態(tài)空間形式：

然后我們可以另于是動力學(xué)模型可簡化為

即簡寫形式：

由狀態(tài)空間方程，可得到：

令

a₄₃＝a₄₃₀+Δa₄₃

a₂₃＝a₂₃₀+Δa₂₃

b₁＝b₁₀+Δb₁

b₂＝b₂₀+Δb₂ (9)

其中a₄₃₀、a₂₃₀、b₁₀、b₂₀為已知部分，Δa₄₃、Δa₂₃、Δb₁、Δb₂為未知部分。

于是公式可重寫為：

其中，

P₁＝Δa₄₃+Δb₂U

P₂＝Δa₂₃+Δb₁U (11)

定義角度誤差：

e_θ＝θ-θ_r (12)

代入(10)式可得：

考慮不確定性因素公式(13)可簡化為：

其中，

為了使系統(tǒng)穩(wěn)定且誤差能夠被快速穩(wěn)定，將角度魯棒控制器可被設(shè)計(jì)為：

其中k₁₁與k₁₂的選取必須滿足赫維茲穩(wěn)定性判據(jù)。

在控制部分中U主要由兩部分組成，即U滿足：

將公式(17)與(16)代入到公式(14)中可得到：

令

這里η₁₁、η₁₂分別為Δa₄₃、Δb₂的上界。

引入滑模變量：

于是本發(fā)明就可以設(shè)計(jì)出角度滑?？刂破鳎?/p>

應(yīng)用李雅普諾夫穩(wěn)定性原理，構(gòu)造能量函數(shù)：

當(dāng)s₁＝0時取“＝”。由此可知，該控制方法能夠使兩輪自平衡機(jī)器人保持穩(wěn)定。

在直立得到控制的前提下，才可以對速度進(jìn)行控制，根據(jù)模型可以構(gòu)建速度與角度關(guān)系θ_r＝β·e_v(23)，其中e_v＝v-v_r(24)

這里，v和v_r表示的是瞬時速度，而上述中期望速度和實(shí)際速度表示的是平均速度，兩者實(shí)際的物理意義是相同。

根據(jù)公式(23)與(10)可以得到：

不考慮誤差的情況下，公式可以簡化為:

其中

為了使速度可以有效被控制，設(shè)計(jì)速度魯棒控制器為：

根據(jù)赫維茲穩(wěn)定性判據(jù)可以確定k₂₁，k₂₂的值。

速度控制輸出量和角度控制一樣由兩部分組成，即

將公式(29)與(28)代入公式(26)可以得到：

為了使速度能夠有效收斂，利用滑?？刂萍夹g(shù)，設(shè)計(jì)滑模變量：

s₂＝e_v+λ₂·e_iv

其速度滑?？刂破鳛?/p>

根據(jù)李雅普諾夫穩(wěn)定性原理構(gòu)造能量函數(shù)：

由此證明，速度控制也是收斂并能夠使系統(tǒng)穩(wěn)定的。

采用上述技術(shù)方案，通過差量作為反饋實(shí)現(xiàn)角度魯棒控制器、角度滑?？刂破?、速度魯棒控制器和速度滑?？刂破鞯妮敵隹刂?，從而對于實(shí)際兩輪自平衡機(jī)器人中的一些參數(shù)估計(jì)存在偏差時，控制器依然能夠保持兩輪自平衡機(jī)器人擁有良好的性能。在這些參數(shù)偏差較小時期魯棒控制器起主導(dǎo)作用，在偏差較大時滑模控制器起主導(dǎo)作用，兩者結(jié)合就可以應(yīng)對外界參數(shù)發(fā)生的變化，如兩輪自平衡機(jī)器人使用過程中的老化或者運(yùn)行環(huán)境而導(dǎo)致系統(tǒng)固有參數(shù)發(fā)生變化。

參見圖4，所述為本發(fā)明兩輪自平衡機(jī)器人的系統(tǒng)執(zhí)行流程圖，該系統(tǒng)在開始執(zhí)行后首先進(jìn)行初始化，然后分兩條不同頻率的任務(wù)，一條是方向控制，執(zhí)行周期為20ms；另一條為本發(fā)明的平衡控制，執(zhí)行周期為5ms。其中平衡控制首先通過傳感器(陀螺儀和加速度計(jì))采集角速度信號和加速度信號，檢測速度手柄的速度輸入，然后通過姿態(tài)計(jì)算計(jì)算出兩輪自平衡機(jī)器人角度，然后通過速度魯棒控制器和速度滑?？刂破饔?jì)算出速度輸出，速度輸出計(jì)算完成后直接用作角度控制的期望信號，然后計(jì)算出角度輸出，最后將直立控制和方向控制的控制輸出進(jìn)行疊加然后濾波從而控制電機(jī)輸出。

參見圖5是在進(jìn)行仿真時為了模擬實(shí)際情況，并且測試極端狀況對實(shí)際模型中的a₄₃加入1/60Hz的正弦震蕩并對包括a₄₃在內(nèi)的所有參數(shù)加入幅度為1的噪聲信號。本發(fā)明仿真時使用與傳統(tǒng)PID進(jìn)行對比，并且本發(fā)明與傳統(tǒng)PID是在相等地仿真條件下進(jìn)行。

參見圖6與圖7，在圖6中左圖為本發(fā)明速度跟蹤曲線，右圖為傳統(tǒng)PID跟蹤曲線，其中虛線為速度期望信號，實(shí)線為實(shí)際速度。在圖7中左圖為本發(fā)明速度誤差變化曲線，右圖為傳統(tǒng)PID速度跟蹤誤差變化曲線；由兩者對比可以很明顯地發(fā)現(xiàn)本發(fā)明速度輸出在外界參數(shù)發(fā)生攝動地情況下比較平滑穩(wěn)定，而傳統(tǒng)PID會由于系統(tǒng)本非線性產(chǎn)生抖動。

參見圖8與圖9，在圖8中左圖為本發(fā)明角度跟蹤曲線，右圖為傳統(tǒng)PID角度跟蹤曲線，其中虛線為速度控制部分輸出地期望角度參考信號，實(shí)線為實(shí)際角度響應(yīng)情況。在圖9中左圖為本發(fā)明角度誤差變化曲線，右圖為傳統(tǒng)PID角度誤差變化曲線。由本發(fā)明角度仿真波形與傳統(tǒng)PID仿真波形對比可以明顯發(fā)現(xiàn)本發(fā)明在速度可控地情況下角度依然可以得到有效控制，而傳統(tǒng)PID在速度可控時角度控制并不完美產(chǎn)生了較大地相位差并且在角度幅度控制上傳統(tǒng)PID也無法有效精確控制。

以上實(shí)施例的說明只是用于幫助理解本發(fā)明的方法及其核心思想。應(yīng)當(dāng)指出，對于本技術(shù)領(lǐng)域的普通技術(shù)人員來說，在不脫離本發(fā)明原理的前提下，還可以對本發(fā)明進(jìn)行若干改進(jìn)和修飾，這些改進(jìn)和修飾也落入本發(fā)明權(quán)利要求的保護(hù)范圍內(nèi)。

對所公開的實(shí)施例的上述說明，使本領(lǐng)域?qū)I(yè)技術(shù)人員能夠?qū)崿F(xiàn)或使用本發(fā)明。對這些實(shí)施例的多種修改對本領(lǐng)域的專業(yè)技術(shù)人員來說將是顯而易見的，本發(fā)明中所定義的一般原理可以在不脫離本發(fā)明的精神或范圍的情況下，在其它實(shí)施例中實(shí)現(xiàn)。因此，本發(fā)明將不會被限制于本發(fā)明所示的這些實(shí)施例，而是要符合與本發(fā)明所公開的原理和新穎特點(diǎn)相一致的最寬的范圍。