兩輪機(jī)器人在坡面上的運(yùn)動(dòng)平衡控制*

阮曉鋼， 李亞磊， 朱曉慶

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

0 引 言

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

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

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

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

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

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

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

3)兩輪機(jī)器人在坡面上運(yùn)動(dòng)時(shí)，車輪只有滾動(dòng)沒(méi)有滑動(dòng)；

4)忽略傳感器噪聲。

以機(jī)器人兩輪輪軸中心為原點(diǎn)建立笛卡爾直角坐標(biāo)系，如圖1所示，x軸為兩輪機(jī)器人沿著坡面的前進(jìn)方向，y軸與兩輪軸線方向一致，且以指向左輪軸心的方向?yàn)檎?，z軸為垂直斜坡向上。大地坐標(biāo)系為X1，Y1，Z1。

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

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

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

拉格朗日方程公式為

(1)

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

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

1)系統(tǒng)動(dòng)能

(2)

2)系統(tǒng)勢(shì)能

U=MgLcosθb.

(3)

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

(4)

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

(5)

引入電機(jī)模型

(6)

帶入方程(1)得到一個(gè)二階非線性微分方程，即為兩輪機(jī)器人在坡面上的動(dòng)力學(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è)計(jì)

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

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

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

(8)

增益調(diào)整型

(9)

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

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

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

(10)

3 仿真實(shí)驗(yàn)

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

圖2 兩輪機(jī)器人在坡面上的Simulink模型

3.1 姿態(tài)平衡控制實(shí)驗(yàn)

圖3 兩輪機(jī)器人在坡面上的自平衡仿真實(shí)驗(yàn)

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

3.2 魯棒性實(shí)驗(yàn)

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

3.2.1 脈沖干擾實(shí)驗(yàn)

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

圖4 脈沖擾動(dòng)時(shí)兩輪機(jī)器人的姿態(tài)控制

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

3.2.2 階躍干擾實(shí)驗(yàn)

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

圖5 階躍擾動(dòng)時(shí)兩輪機(jī)器人的姿態(tài)平衡控制

3.2.3 隨機(jī)噪聲實(shí)驗(yàn)

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

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

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

3.3 靜態(tài)平衡控制實(shí)驗(yàn)

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

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

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

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

4 結(jié)束語(yǔ)

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

參考文獻(xiàn):

[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] 李 航,孫厚芳,韓建海,等.兩輪機(jī)器人行走機(jī)構(gòu)的建模與實(shí)驗(yàn)[J].北京理工大學(xué)學(xué)報(bào),2004,24(12):1058-1061.

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

[4] 趙書尚,韓建海,李濟(jì)順,等.平行雙輪電動(dòng)車及其控制系統(tǒng)的研究[J].機(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ī)器人建模與分析[J].機(jī)器人,2010,32(1):138-144.

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

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