兩輪機器人在坡面上的運動平衡控制*

阮曉鋼， 李亞磊， 朱曉慶

(北京工業(yè)大學(xué) 人工智能與機器人研究所, 北京 100124)

0 引 言

兩輪機器人是自平衡機器人家族中的一員，兩輪機器人的平衡問題類似于倒立擺的平衡問題，不同的是,倒立擺只能在二維空間中運動，而兩輪機器人是可以在三維空間中運動。目前，關(guān)于兩輪機器人的運動控制研究，大多是在平面上的運動控制，在坡面上運動的兩輪機器人由于受到重力的影響，其平衡控制變得更為復(fù)雜。

2002年，瑞士聯(lián)邦工業(yè)大學(xué)的Grasser F[1]通過解耦分解運動簡化了兩輪機器人的控制，研制出可以實現(xiàn)自平衡的兩輪機器人Joe，Joe能夠穩(wěn)定地做U型回轉(zhuǎn)和零半徑回轉(zhuǎn)。2003年，美國南衛(wèi)理工會大學(xué)的Anderson教授制作了兩輪機器人nBot，nBot采用基于狀態(tài)反饋的控制方法，具有魯棒的運動平衡控制性能，能在崎嶇道路行進，但是不具備自主起立的功能。北京理工大學(xué)的李航、河南科技大學(xué)的韓建海和趙書尚等人[2～4]，通過極點配置方法設(shè)計了閉環(huán)狀態(tài)反饋控制器，實現(xiàn)了兩輪機器人在平面上的平衡控制。2006年，韓國的Kim Y[5]運用牛頓力學(xué)的方法建立了兩輪機器人在平面上的動力學(xué)模型，對兩輪機器人在斜坡上的運動進行了分析討論，但是并沒有給出斜坡上適用的數(shù)學(xué)模型。2010年，北京工業(yè)大學(xué)的李欣源等人[6]成功研制出具有柔性腰椎的柔性兩輪自平衡機器人“原人”，提出了一種基于非線性PD的三環(huán)控制方法來控制兩輪機器人在平面上的姿態(tài)平衡和運動控制，其控制能力達到了國際領(lǐng)先水平。

本文首先利用拉格朗日方程方法建立了兩輪機器人在坡面上的動力學(xué)模型，然后設(shè)計了一種非線性PD控制器控制兩輪機器人在坡面上的姿態(tài)平衡。與傳統(tǒng)的線性PD控制器進行多項仿真實驗對比，在動態(tài)性能、魯棒性方面，非線性PD控制器有著更好的控制效果，最后加入速度控制，與姿態(tài)控制構(gòu)成雙環(huán)的PD控制，實現(xiàn)了兩輪機器人在坡面上的靜態(tài)平衡。

1 兩輪機器人在坡面上的動力學(xué)建模

兩輪機器人在坡面上運動時，由于受到重力作用的影響，兩輪機器人的平衡控制變得更為復(fù)雜。每個控制任務(wù)本身就是一個動力學(xué)任務(wù)，本文利用Lagrange方程方法，以能量方式建立兩輪機器人在坡面上的動力學(xué)模型。建模的假設(shè)條件[7]：

1)兩輪機器人的車輪直徑相等；

2)機器人的機身、車輪均為剛體；

3)兩輪機器人在坡面上運動時，車輪只有滾動沒有滑動；

4)忽略傳感器噪聲。

以機器人兩輪輪軸中心為原點建立笛卡爾直角坐標系，如圖1所示，x軸為兩輪機器人沿著坡面的前進方向，y軸與兩輪軸線方向一致，且以指向左輪軸心的方向為正，z軸為垂直斜坡向上。大地坐標系為X1，Y1，Z1。

圖1 機器人系統(tǒng)的受力分析圖

表1中列出了機器人動力學(xué)建模的參數(shù)定義。

表1 兩輪機器人的參數(shù)定義

拉格朗日方程公式為

(1)

其中，L=T-U，為拉格朗日算子，T為系統(tǒng)的總動能，U為系統(tǒng)的總勢能，D為耗散能,qs為系統(tǒng)的廣義坐標，定義系統(tǒng)的廣義坐標為qs=[q1,q2,q3]T=[θb,φl,φr]T，Qs為廣義力。

通過運動學(xué)關(guān)系推導(dǎo)可以得到:

1)系統(tǒng)動能

(2)

2)系統(tǒng)勢能

U=MgLcosθb.

(3)

3)系統(tǒng)耗散能

(4)

4)系統(tǒng)的廣義力

(5)

引入電機模型

(6)

帶入方程(1)得到一個二階非線性微分方程，即為兩輪機器人在坡面上的動力學(xué)方程

(7)

其中

cosθbcos2θcosθs-cosθsinθbsinθs)

n1=-MgLsinθb-M2L2sinθbcosθb+2μwb-

sinθbcos2θcosθs+cosθbsinθscosθ),

sinθbcos2θcosθs+cosθbsinθscosθ),

sinθbcos2θcosθs+cosθbsinθscosθ),

2 非線性PD控制器設(shè)計

兩輪機器人主要通過陀螺儀和傾角儀檢測實時的姿態(tài)，不可避免地有噪聲信號，這些信號經(jīng)過積分環(huán)節(jié)隨時間累積，反而會增大靜態(tài)誤差而影響控制效果，所以，選擇控制器為PD控制，而非PID控制。

根據(jù)控制原理的不同，非線性PID控制器可分為兩類[8]：直接控制量(direct-action)型和增益調(diào)整(gain-sche-duling)型。數(shù)學(xué)表達式分別為直接控制量型

u(DA)=uP(x,θP)+uI(x,θI)+uD(x,θD).

(8)

增益調(diào)整型

(9)

其中，u,e分別為控制量輸入與控制響應(yīng)誤差信號；x,θ分別為非線性函數(shù)的自變量向量與參數(shù)向量；uP(·),uI(·),uD(·)分別代表比例、積分、微分3個非線性分量函數(shù)，而KP(·),KI(·),KD(·)分別代表比例、積分、微分3個非線性增益函數(shù)。

兩輪機器人的控制量是兩輪電機的輸入電壓，當(dāng)e=0，輸入電壓為0；當(dāng)e=maxe，輸入電壓為電機最大值；結(jié)合文獻[3]中分析關(guān)于比例控制分量uP(·)方面的先驗知識充分，本文選擇如式(8)的直接控制量型非線性PID控制器，構(gòu)造一種PD控制器，具體形式為

τB=PDNLB(θb)=KP(θb)+KD(b)

(10)

3 仿真實驗

在Matlab環(huán)境中，基于S函數(shù)方法，建立兩輪機器人在坡面上的Simulink控制模型如圖2所示。

圖2 兩輪機器人在坡面上的Simulink模型

3.1 姿態(tài)平衡控制實驗

圖3 兩輪機器人在坡面上的自平衡仿真實驗

實驗結(jié)果可見，兩輪機器人在坡面上運動時，線性PD控制器和非線性PD控制器都可以實現(xiàn)姿態(tài)的平衡控制，不過線性PD控制器在機身傾角小角度時候反應(yīng)不夠靈敏，達到穩(wěn)定狀態(tài)需要時間長，非線性PD控制器有更好的快速性和穩(wěn)定性。另外,從實驗中可以看出機器人最后的傾角都沒有到達0°，因為控制器中沒有加入速度控制環(huán)，所以,機器人會提供一定的機身傾角，保持在坡面上的勻速運動狀態(tài)。

3.2 魯棒性實驗

在相同初始條件下，姿態(tài)平衡控制的過程中，加入脈沖擾動，階躍擾動和隨機噪聲，分別對線性PD和非線性PD控制器進行仿真實驗，測試2種控制器的魯棒性。

3.2.1 脈沖干擾實驗

在3 s時對機身加入持續(xù)時間1 s、幅值為5的脈沖擾動，分別對2種控制器進行實驗，結(jié)果如圖4所示。

圖4 脈沖擾動時兩輪機器人的姿態(tài)控制

從圖4中可以看出，兩輪機器人機身在受到脈沖擾動時，非線性PD控制器能更好的控制機器人機身的平衡，受到脈沖的影響較小。

3.2.2 階躍干擾實驗

在3 s時，對機器人機身加入幅值為20的階躍擾動(圖5)，實驗結(jié)果說明:線性PD控制器已經(jīng)不能有效地控制機器人機身的平衡，機身傾角開始發(fā)散，控制失敗。非線性PD控制器仍然能夠控制機器人機身的近似平衡，有效地抑制了階躍干擾，得到很好的控制效果。

圖5 階躍擾動時兩輪機器人的姿態(tài)平衡控制

3.2.3 隨機噪聲實驗

在獲得的機器人機身傾角數(shù)據(jù)加上周期為2 s，最大值為1°的正弦噪聲信號，得到控制結(jié)果如圖6所示。

圖6 加入隨機噪聲時兩輪機器人的姿態(tài)平衡控制

線性PD控制器下的兩輪機器人由于噪聲作用，機身傾角也隨著振蕩，控制效果不佳。而非線性PD控制器能夠很好的過濾噪聲，受到的影響很小，仍然可以保持機器人的機身平衡。

3.3 靜態(tài)平衡控制實驗

與在平面上運動不同，兩輪機器人在坡面上運動時，不加速度控制環(huán)節(jié)，只能保持勻速運動下的平衡，不能保持靜態(tài)平衡。所以,有必要在姿態(tài)平衡的控制同時加入速度控制環(huán)，構(gòu)成雙環(huán)控制，才可以讓機器人在坡面上保持靜態(tài)平衡。

對非線性PD姿態(tài)控制器，加入速度控制環(huán)節(jié)后，得到實驗結(jié)果如圖7所示

圖7 兩輪機器人在坡面上的靜態(tài)平衡控制

在加入速度控制環(huán)節(jié)后，兩輪機器人在坡面上的傾角和輪子轉(zhuǎn)速都可以等于0。以姿態(tài)平衡為主，速度控制為輔的雙環(huán)PD控制下，兩輪機器人在坡面上實現(xiàn)了靜態(tài)平衡。

4 結(jié)束語

本文分析兩輪機器人在斜坡上的受力情況，利用拉格朗日方法建立了兩輪機器人在坡面上的動力學(xué)模型，基于模型在Matlab中搭建仿真環(huán)境，分別設(shè)計了兩輪機器人姿態(tài)平衡的線性PD和非線性PD控制器，通過仿真實驗，驗證了非線性PD控制器相比線性PD控制器，有更好的動態(tài)性能和魯棒性。最后結(jié)合姿態(tài)平衡控制器加入速度控制環(huán)節(jié)，實現(xiàn)兩輪機器人在坡面上的靜態(tài)平衡。

參考文獻:

[1] Grasser F,D’Arrigo A,Colombi S.JOE:A mobile inverted pendulum[J].IEEE Trans on Industrial Electronics,2002,49(1):107-114.

[2] 李 航,孫厚芳,韓建海,等.兩輪機器人行走機構(gòu)的建模與實驗[J].北京理工大學(xué)學(xué)報,2004,24(12):1058-1061.

[3] 韓建海,李 航,白玉川,等.平行雙輪電動車的建模與控制仿真[J].中國機械工程,2005,16(8):683-686.

[4] 趙書尚,韓建海,李濟順,等.平行雙輪電動車及其控制系統(tǒng)的研究[J].機電工程,2005,22(1):12-15.

[5] KimY,Kim S H,Kwak Y K.Dynamic analysis of a nonholonomic two-wheeled inverted pendulum robot[J].Journal of Intelligent and Robotic Systems,2005,44:25-46.

[6] 李欣源,阮曉鋼,任紅格.柔性雙輪機器人建模與分析[J].機器人,2010,32(1):138-144.

[7] 王曉宇.兩輪自平衡機器人的研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2009:2-26.

[8] 胡包鋼.非線性PID控制器研究—比例分量的非線性方法[J].自動化學(xué)報，2006,32(2):219-227.