三輪移動(dòng)機(jī)器人的精確運(yùn)動(dòng)控制設(shè)計(jì)

文相容，周宇生

(貴州大學(xué) 數(shù)學(xué)與統(tǒng)計(jì)學(xué)院，貴陽(yáng) 550025)

輪式移動(dòng)機(jī)器人是一種通過(guò)驅(qū)動(dòng)輪子轉(zhuǎn)動(dòng)來(lái)實(shí)現(xiàn)運(yùn)動(dòng)功能的非完整約束結(jié)構(gòu)，具有結(jié)構(gòu)簡(jiǎn)單、能耗低和運(yùn)動(dòng)靈活等優(yōu)點(diǎn)[1]。輪式移動(dòng)機(jī)器人按輪子數(shù)量可分為獨(dú)輪、兩輪、三輪、四輪和多輪移動(dòng)機(jī)器人。在實(shí)際應(yīng)用中，會(huì)根據(jù)需要采用不同的輪式移動(dòng)機(jī)器人。獨(dú)輪和兩輪機(jī)器人占地面積小且運(yùn)動(dòng)最為靈活，但豎直方向部分容易翻倒，是需要自平衡控制的倒立擺結(jié)構(gòu)[2]。三輪移動(dòng)機(jī)器人雖然靈活性不如獨(dú)輪和兩輪，但結(jié)構(gòu)穩(wěn)定不容易翻倒，且靈活性比四輪移動(dòng)機(jī)器人要強(qiáng)很多。因此兼具穩(wěn)定性和靈活性?xún)?yōu)點(diǎn)的三輪移動(dòng)機(jī)器人在實(shí)際生活工作中應(yīng)用非常廣泛。

目前關(guān)于三輪移動(dòng)機(jī)器人的控制研究大都側(cè)重于其運(yùn)動(dòng)學(xué)方程[3-6]，通過(guò)將其運(yùn)動(dòng)學(xué)方程轉(zhuǎn)化為鏈?zhǔn)綐?biāo)準(zhǔn)型，設(shè)計(jì)速度控制器跟蹤給定的目標(biāo)速度[7-9]。但在實(shí)際中一般是通過(guò)驅(qū)動(dòng)力或力矩來(lái)實(shí)現(xiàn)控制目的，所以設(shè)計(jì)力或力矩控制器更符合實(shí)際。文[10]針對(duì)高速運(yùn)動(dòng)情況下，對(duì)輪式移動(dòng)機(jī)器人進(jìn)行了運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)模型重建，并利用反演控制方法設(shè)計(jì)了抗側(cè)滑運(yùn)動(dòng)學(xué)反演控制器，同時(shí)利用逆動(dòng)力學(xué)控制方法設(shè)計(jì)了抗擾動(dòng)逆動(dòng)力學(xué)控制器，完成了機(jī)器人對(duì)預(yù)定軌跡的跟蹤。一般情況下，實(shí)際運(yùn)動(dòng)任務(wù)是控制三輪移動(dòng)機(jī)器人精確沿給定目標(biāo)軌跡曲線運(yùn)動(dòng)，而動(dòng)力學(xué)方程只是單純描述其運(yùn)動(dòng)速度和驅(qū)動(dòng)力或力矩之間的關(guān)系。因此，需要將給定的目標(biāo)軌跡曲線轉(zhuǎn)化為速度目標(biāo)形式，這樣就可以將原來(lái)的運(yùn)動(dòng)控制問(wèn)題轉(zhuǎn)化為基于動(dòng)力學(xué)方程的一般跟蹤控制問(wèn)題，從而使得問(wèn)題變得簡(jiǎn)單且容易實(shí)現(xiàn)。文[11]利用平面曲線基本理論，將兩輪式倒立擺的目標(biāo)軌跡曲線轉(zhuǎn)化為速度目標(biāo)形式，結(jié)合其動(dòng)力學(xué)方程為兩輪式倒立擺設(shè)計(jì)了力矩控制器，精確實(shí)現(xiàn)往返運(yùn)動(dòng)控制。

隨著科學(xué)技術(shù)的發(fā)展，輪式移動(dòng)機(jī)器人的精確軌跡跟蹤變得越來(lái)越重要，目前關(guān)于精確軌跡跟蹤控制的研究主要側(cè)重于先進(jìn)控制器的設(shè)計(jì)，文[12]通過(guò)間接自適應(yīng)模糊滑?？刂品椒?，構(gòu)造基于旋轉(zhuǎn)子系統(tǒng)和縱向子系統(tǒng)的復(fù)合控制，實(shí)現(xiàn)速度跟蹤和傾角穩(wěn)定。文[13]設(shè)計(jì)了基于反步法的自適應(yīng)控制，實(shí)現(xiàn)了輪式倒立擺的輸出跟蹤穩(wěn)定。文[14]為解決平臺(tái)與安裝機(jī)器人之間的擾動(dòng)、非線性和非完整約束之間的動(dòng)力學(xué)耦合問(wèn)題，提出了一種基于反步法的自適應(yīng)滑?？刂破饔糜谝苿?dòng)機(jī)械臂的軌跡跟蹤。從這些文獻(xiàn)的仿真結(jié)果來(lái)看，無(wú)論控制設(shè)計(jì)方法多么先進(jìn)，實(shí)際軌跡總是與目標(biāo)軌跡曲線有一定的偏差，特別是在剛開(kāi)始的一段時(shí)間內(nèi)。主要原因是實(shí)際運(yùn)動(dòng)速度與目標(biāo)速度之間的初始速度誤差比較大，導(dǎo)致整個(gè)運(yùn)動(dòng)過(guò)程中的累積位置誤差越來(lái)越大。因此，必須根據(jù)運(yùn)動(dòng)任務(wù)設(shè)計(jì)合適的速度跟蹤目標(biāo)，盡量減少初始速度誤差。通過(guò)分析其側(cè)向運(yùn)動(dòng)速度為零這一非完整約束，利用微分幾何理論得到目標(biāo)軌跡曲線與速度目標(biāo)之間的一一對(duì)應(yīng)關(guān)系。引入一個(gè)微分同胚的變量變換，將前向速度目標(biāo)轉(zhuǎn)化為和實(shí)際初始速度更接近的形式，通過(guò)減少初始速度誤差來(lái)盡量減少三輪移動(dòng)機(jī)器人運(yùn)動(dòng)時(shí)的累積位置誤差。最后通過(guò)仿真說(shuō)明所提軌跡跟蹤目標(biāo)設(shè)計(jì)和魯棒控制設(shè)計(jì)方法的有效性。

1 三輪移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)方程

如圖1所示，三輪移動(dòng)機(jī)器人的后2輪上各裝有一個(gè)驅(qū)動(dòng)馬達(dá)，同時(shí)前輪通過(guò)控制方向盤(pán)來(lái)實(shí)現(xiàn)轉(zhuǎn)向。當(dāng)后2輪中心點(diǎn)C(x，y)隨著三輪移動(dòng)機(jī)器人在平面上做連續(xù)光滑運(yùn)動(dòng)時(shí)，點(diǎn)C的運(yùn)動(dòng)軌跡是一條光滑曲線，此時(shí)點(diǎn)C在與切向垂直方向的側(cè)向速度為0，如若不然，其軌跡曲線會(huì)出現(xiàn)不光滑的尖點(diǎn)。因此三輪式移動(dòng)機(jī)器人在平面上做光滑曲線運(yùn)動(dòng)時(shí)一般受到如下非完整約束

圖1 三輪移動(dòng)機(jī)器人結(jié)構(gòu)簡(jiǎn)圖

(1)

(2)

根據(jù)方程(2)中的第二式可以知道，軌跡曲線的切向量長(zhǎng)度就是點(diǎn)C沿該曲線運(yùn)動(dòng)的前向速度。因此，可以將方程(2)改寫(xiě)為

(3)

此外，當(dāng)三輪移動(dòng)機(jī)器人的后2輪不側(cè)滑不空轉(zhuǎn)時(shí)，需要滿足下面2個(gè)完整約束

(4)

其中：θ是三輪移動(dòng)機(jī)器人的偏航角；θl和θr分別是左右兩輪的轉(zhuǎn)角；r是輪子的半徑。

(5)

然后將方程(2)的第二式代入方程(5)并結(jié)合方程(1)，整理得到三輪移動(dòng)機(jī)器人后兩輪所受的運(yùn)動(dòng)約束方程

(6)

接下來(lái)，分析三輪移動(dòng)機(jī)器人前輪所受到的約束。

如圖2所示，P(xp,yp)點(diǎn)具有雙重身份。一方面當(dāng)P點(diǎn)作為前輪中心，在不發(fā)生側(cè)滑運(yùn)動(dòng)時(shí)，其側(cè)向速度為0，即滿足方程

圖2 P點(diǎn)速度示意圖

(7)

另一方面當(dāng)P點(diǎn)作為中間體的前端點(diǎn)時(shí)，該點(diǎn)的速度滿足如下關(guān)系

(8)

由方程(7)和(8)得到

(9)

方程(9)是一個(gè)重要的非完整約束，它描述了前輪轉(zhuǎn)向和后輪運(yùn)動(dòng)速度之間的關(guān)系。將方程(3)的第一式代入方程(9)，則方程(9)可改寫(xiě)為

(10)

這里φ是前輪操舵轉(zhuǎn)向角，將其看作一個(gè)控制變量，因?yàn)樗峭ㄟ^(guò)人為控制方向盤(pán)轉(zhuǎn)向得到的。

結(jié)合方程(3)、(5)和(9)，將三輪移動(dòng)機(jī)器人的所有運(yùn)動(dòng)約束總結(jié)為

(11)

2 三輪移動(dòng)機(jī)器人的動(dòng)力學(xué)方程

令三輪移動(dòng)機(jī)器人的廣義坐標(biāo)為q=(x,y,θ,θr,θl)T，方程(11)可以表示為以下矩陣形式

(12)

其中

利用非完整力學(xué)系統(tǒng)的Euler-Lagrange方程，三輪移動(dòng)機(jī)器人的動(dòng)力學(xué)方程為

(13)

方程(13)中的F(q)選取為

F(q)S=0。

(14)

接下來(lái)需要計(jì)算三輪移動(dòng)機(jī)器人的動(dòng)能。根據(jù)圖1所示，得到如下位置關(guān)系

其中，(xwl,ywl),(xwr,ywr)分別是左右輪的中心坐標(biāo)。此時(shí)三輪移動(dòng)機(jī)器人上的各中心點(diǎn)速度分別表示為

因此，三輪移動(dòng)機(jī)器人的后2輪的前向動(dòng)能和轉(zhuǎn)動(dòng)動(dòng)能之和表示為

同樣地，可以將三輪移動(dòng)機(jī)器人的前輪和中間體的前向動(dòng)能和轉(zhuǎn)動(dòng)動(dòng)能之和分別表示為

其中：MB是中間體質(zhì)量；Mw是輪子質(zhì)量；Iwd是輪子繞z軸方向轉(zhuǎn)動(dòng)慣量；Iw是輪子繞輪軸方向轉(zhuǎn)動(dòng)慣量；IB是中間體繞z軸方向轉(zhuǎn)動(dòng)慣量。

將Lagrange函數(shù)L=Tw+TB+Tf代入方程(13)，然后按狀態(tài)變量的各階導(dǎo)數(shù)整理得到

(15)

其中：

將方程(12)兩邊同時(shí)關(guān)于t求導(dǎo)，然后代入方程(15)得

(16)

在方程(16)的左右兩邊同時(shí)左乘ST(q)，結(jié)合方程(14)消去等式中的Lagrange乘子得

整理上式，最終可以將三輪移動(dòng)機(jī)器人動(dòng)力學(xué)方程表示為如下?tīng)顟B(tài)方程形式

(17)

其中

β1=a2r2MB+4a2r2Mw+2d2r2Mw+4r2IB+4a2Iw+8r2Iwd，

β2=(2d2r2Mw+4r2IB+8Iwdr2-3a2r2MB-8a2r2Mw-8a2Iw)cosφ-β1。

3 三輪移動(dòng)機(jī)器人的軌跡跟蹤運(yùn)動(dòng)控制

3.1 速度跟蹤目標(biāo)設(shè)計(jì)

(18)

對(duì)方程(18)兩邊同時(shí)關(guān)于t求導(dǎo)得

(19)

將方程(18)和(19)交叉相乘并相減得

因此中心點(diǎn)C運(yùn)動(dòng)速度與運(yùn)動(dòng)軌跡曲線之間的關(guān)系式為

(20)

由于φ和u1之間有約束關(guān)系，本文將φ和u2看作控制變量。觀察發(fā)現(xiàn)方程(17)是解耦的，可以先通過(guò)第二個(gè)方程將u1設(shè)計(jì)出來(lái)。然后結(jié)合方程(9)有

(21)

再利用方程(21)將控制器φ設(shè)計(jì)出來(lái)。然后將控制變量φ代入方程(17)的第一式，設(shè)計(jì)控制器u2實(shí)現(xiàn)最終的軌跡跟蹤任務(wù)。

從上式可以看出，當(dāng)參數(shù)變換后，前向速度目標(biāo)也是隨之而改變的。因此，上式中含有曲線相對(duì)曲率的第二式才是對(duì)軌跡曲線的本質(zhì)刻畫(huà)。

其中，α>0,β>0，r>0，并且滿足

(22)

由方程(22)有r=l,α=lβ2。因此函數(shù)φ(η)可以設(shè)計(jì)為

φ(η)=-(βη+1)le-βη+l,

其中l(wèi)是目標(biāo)軌跡曲線長(zhǎng)度。

(23)

其中：s=φ(t)=-(βt+1)le-βt+l.

3.2 偏航轉(zhuǎn)速控制系統(tǒng)

由于方程(17)是解耦的，先考慮其偏航轉(zhuǎn)速控制系統(tǒng)

(24)

偏航轉(zhuǎn)速控制系統(tǒng)(24)比較簡(jiǎn)單，容易設(shè)計(jì)反饋跟蹤控制器。為了加強(qiáng)它的魯棒性，采用積分滑模思想處理不確定性因素的影響[15]。

其中d1(t)是偏航轉(zhuǎn)速誤差系統(tǒng)受到的不確定性因素。

其中k是誤差反饋控制增益參數(shù)。

再將軌跡跟蹤目標(biāo)設(shè)計(jì)成積分滑模面S1(ω(t))=0，且

其中G1>0是合適的常數(shù)。這樣，切換控制部分設(shè)計(jì)為

其中:μ是滑?？刂茀?shù)；ε是外部擾動(dòng)最大振幅。因此，u1設(shè)計(jì)為

u1(t)=u10(t)+u11(t)。

(25)

最后，結(jié)合方程(21)和(25)，最終設(shè)計(jì)的操舵角控制器為

(26)

3.3 前向速度控制系統(tǒng)

接下來(lái)考慮前向速度控制系統(tǒng)

(27)

對(duì)系統(tǒng)(27)利用反饋線性化思想處理，設(shè)

(28)

得到一個(gè)簡(jiǎn)單的控制系統(tǒng)

令X=[x1,x2]T=[s,υ]T，前向速度控制系統(tǒng)改寫(xiě)為

(29)

再將控制方程(29)轉(zhuǎn)化成誤差系統(tǒng)，令

則前向速度誤差系統(tǒng)為

其中d2(t)是前向速度誤差系統(tǒng)受到的不確定性因素。

為了使得前向速度誤差盡量小，可以將前向速度誤差的權(quán)重取得足夠大[11]。利用線性二次型最優(yōu)控制理論，h(t)設(shè)計(jì)為

h(t)=-R-1BT[P(t)Y(t)+b(t)],

其中：P(t)∈2×2，b(t)∈2滿足方程

-PA-ATP+PBR-1BTP-Q=0，

為了增強(qiáng)反饋控制的魯棒性，同樣采用積分滑模思想來(lái)處理不確定性因素。系統(tǒng)(29)的控制器基礎(chǔ)部分設(shè)計(jì)為

h20(t)=-R-1BT[P(t)Y(t)+b(t)]。

引入積分滑模函數(shù)

其中，G2是適當(dāng)?shù)某＞仃嚒?/p>

積分滑模面設(shè)計(jì)為S2(X(t))=0，因此系統(tǒng)(29)的控制最終設(shè)計(jì)為

h2(t)=h20(t)+h21(t),

(30)

其中：h21是切換的不連續(xù)控制，用來(lái)補(bǔ)償系統(tǒng)不確定性因素和外部擾動(dòng)的影響，

h21(t)=-(μ+ε||Y||1)(G2B)-1sgn(S2(X(t)))。

結(jié)合方程(28)和(30)，前向速度控制器設(shè)計(jì)為

(31)

4 仿真實(shí)現(xiàn)

考慮目標(biāo)軌跡曲線是單位圓，假設(shè)初始時(shí)刻三輪移動(dòng)機(jī)器人是靜止的，且目標(biāo)軌跡曲線為

由曲線基本理論，目標(biāo)軌跡曲線的曲率為k(t)=1。

根據(jù)方程(20)，軌跡曲線的速度目標(biāo)形式表示為

(32)

再根據(jù)方程(23)，改進(jìn)的速度跟蹤目標(biāo)設(shè)計(jì)為

(33)

假設(shè)三輪移動(dòng)機(jī)器人的各部分參數(shù)選擇如下

M=3 kg,Mw=2 kg,a=1 m，r=0.1 m，Iw=0.01 kgm2,Iwd=0.005 kgm2,I=0.01 kgm2。

l=π/2,β=0.5,k=2,μ=0.01,ε=2,G1=1.8,G2=diag(100,1)。

若前向速度誤差控制系統(tǒng)和偏航轉(zhuǎn)速誤差控制系統(tǒng)分別受到如下未知擾動(dòng)影響

且將φ(t)設(shè)計(jì)為

此時(shí)，關(guān)于控制器(26)和(31)中的所有量就都已知了，實(shí)驗(yàn)如圖3、4所示。

圖3 前向速度誤差圖

從圖3和圖4可以看出，采用速度目標(biāo)(32)會(huì)使得初始速度誤差不為零，而改進(jìn)后的速度跟蹤目標(biāo)(33) 從初始時(shí)刻開(kāi)始速度誤差為零。如若初始速度誤差過(guò)大，自然會(huì)導(dǎo)致三輪移動(dòng)機(jī)器人在整個(gè)運(yùn)動(dòng)過(guò)程中累積位置誤差也很大，最終導(dǎo)致實(shí)際運(yùn)動(dòng)軌跡曲線嚴(yán)重偏離目標(biāo)軌跡曲線。如圖5所示，由于筆者設(shè)計(jì)控制器時(shí)采用了積分滑模的思想，所設(shè)計(jì)的滑模控制器具有很強(qiáng)的魯棒性。因此即使誤差控制系統(tǒng)存在不確定性因素的影響，采用速度目標(biāo)(33)和積分滑?？刂破鲿r(shí)，得到的實(shí)際運(yùn)動(dòng)軌跡幾乎和目標(biāo)軌跡重合，反之采用速度目標(biāo)(32)時(shí)，即使同樣采用積分滑?？刂疲玫降膶?shí)際軌跡曲線也會(huì)嚴(yán)重偏離目標(biāo)軌跡曲線，這主要是由于初始速度誤差導(dǎo)致的累積位置誤差太大所致。

圖4 偏航轉(zhuǎn)速誤差圖

圖5 三輪移動(dòng)機(jī)器人在平面上的實(shí)際運(yùn)動(dòng)軌跡

5 結(jié) 論

通過(guò)分析三輪移動(dòng)機(jī)器人的運(yùn)動(dòng)約束方程和運(yùn)動(dòng)規(guī)律，針對(duì)給定的目標(biāo)軌跡曲線，給出了一種新的速度跟蹤目標(biāo)和控制器設(shè)計(jì)方法。該方法可以通過(guò)選擇合適的光滑函數(shù)，為三輪移動(dòng)機(jī)器人設(shè)計(jì)合適的前向速度和偏航轉(zhuǎn)速目標(biāo)，使得在控制過(guò)程中能夠極大地減少誤差控制系統(tǒng)的初始速度誤差，從而達(dá)到減少累積位置誤差的目的。仿真結(jié)果表明，采用文中所提出的速度跟蹤目標(biāo)和控制方法，可以使得三輪移動(dòng)機(jī)器人能精確地沿著給定的目標(biāo)軌跡曲線運(yùn)動(dòng)。

提出的精確軌跡運(yùn)動(dòng)控制方法完全可以推廣到三維空間中的軌跡曲線跟蹤運(yùn)動(dòng)，并應(yīng)用于航天航空、機(jī)器人、機(jī)械制造等需要精確軌跡運(yùn)動(dòng)控制的領(lǐng)域。