輪式移動機器人快速軌跡跟蹤

倪洪杰，王宏霞，俞 立

(浙江工業(yè)大學 信息工程學院，杭州 310023)

移動機器人集動態(tài)決策與規(guī)劃、行為控制與執(zhí)行等多種功能于一體，能在惡劣環(huán)境、危險環(huán)境、災難環(huán)境中代替人類以更低的成本完成許多重要工作.因此，在軍事、工業(yè)等眾多領域具有廣泛的應用，一直是廣大科研工作者的研究熱點.

移動機器人主要包括履帶式、蛇形式、腿式、跳躍式、復合式和輪式等.與其他類型的移動機器人相比，盡管輪式移動機器人的運動穩(wěn)定性受路況影響較大，但其具有承載大、驅(qū)動和控制較方便、自重輕、行走速度快、機構簡單、工作效率高、機動靈活、運動噪聲低等多種優(yōu)點，因此成為本文及諸多學者的研究對象.然而，作為一種典型的非完整系統(tǒng)，輪式移動機器人的軌跡跟蹤控制問題迄今仍然被很多人關注.早期的軌跡跟蹤控制方法主要集中在針對運動學方程提供虛擬控制器使機器人能夠漸近跟蹤給定的參考軌跡，如反演控制[1]、 滑模控制[2]、預測控制等[3-4].隨后，許多工作將這些非線性控制方法與其他方法相結合，同時考慮機器人的動力學模型，提出了一些具有魯棒性和實用性的機器人軌跡跟蹤控制方法，如與自適應補償相結合的研究工作[5-6]、與神經(jīng)動力學優(yōu)化方法相結合的研究工作[7]以及與其他方法相結合的研究工作[8-11].此外，隨著人工智能理論的發(fā)展，還涌現(xiàn)出了一些新的軌跡跟蹤控制方法，如基于機器學習、神經(jīng)網(wǎng)絡、模糊控制設計的軌跡跟蹤控制方法[12-13]，基于視覺伺服的軌跡跟蹤控制方法[14-15]，基于多體動力學的保辛瞬時最優(yōu)控制方法[16]等.

與以上文獻的研究重點不同，本文主要研究如何利用預演信息克服機器人響應能力的有限性，以保證機器人快速跟蹤給定的參考軌跡.事實上，已有的大量研究亦表明：合理利用預演信息設計控制器能夠有效地改善控制系統(tǒng)的性能如暫態(tài)性能、閉環(huán)性能等，該事實在文獻[17-22]中都有詳細的闡述與證明；另一方面，大自然及人類社會有許多成熟的利用預演控制的實際系統(tǒng)，比如，文獻[18]曾提到，司機如果能利用更多的前方路況及時調(diào)整車速及車子的前進方向，就能夠提供更安全、舒適的駕駛.基于此，再考慮到軌跡跟蹤問題中輪式移動機器人參考軌跡已知的特點，本文提出一種基于預演控制策略的輪式移動機器人軌跡跟蹤控制方法.不同于其他控制方法，由于該控制策略有效地利用了當前及將來的參考信息，因而能夠達到更好的軌跡跟蹤效果.

1 輪式移動機器人的動力學模型

假設絕對坐標系XOY固定在平面內(nèi)，輪式移動機器人的運動示意如圖1所示.其后輪為隨動輪，僅用于支撐車體，無導向作用.兩個前輪為獨立驅(qū)動輪，各采用一臺直流伺服電機驅(qū)動.當兩個前輪被輸入不同的驅(qū)動電壓時，它們將產(chǎn)生速度差，繼而可調(diào)整機器人前進的方向及軌跡.

圖1 輪式移動機器人的運動示意

首先，為方便閱讀，我們在表1中定義移動機器人建模所涉及到的相關參數(shù)和變量.其中，下標l和r被用于區(qū)別左右輪的同一參數(shù)或變量.

表1 輪式移動機器人變量和參數(shù)的定義

根據(jù)表1定義的機器人參數(shù)與變量，在輪式移動機器人的質(zhì)心與幾何中心重合的前提下，根據(jù)力矩平衡原理、牛頓第二運動定律以及雙輪驅(qū)動輪式移動機器人的速度調(diào)節(jié)原理，推導出如下的輪式移動機器人的動力學模型：

(1)

(2)

其中：

定義如下變量

x1=[vω]T,u1=[urul]T.

則輪式移動機器人的動力學方程(1)、(2)可表示成:

(3)

式(3)中

獲得了輪式移動機器人的動力學方程，只是完成了預演控制跟蹤的第一步.考慮到輪式移動機器人對預演控制跟蹤策略的執(zhí)行問題，系統(tǒng)(3)被離散化成:

(4)

A=I+TA1,B=TB1.

式中I是維數(shù)相容的單位陣.

關于輪式移動機器人動力學方程(3)的推導，本文假設了輪式移動機器人的質(zhì)心與幾何中心重合.事實上，質(zhì)心與幾何中心不重合并不會影響動力學方程(3)的線性特性.

2 輪式移動機器人的運動學模型

為了進行輪式移動機器人的軌跡跟蹤，有必要給出輪式移動機器人的運動學方程.

記[x(t)y(t)θ]T為t時刻輪式移動機器人的位姿，(x(t),y(t))為t時刻輪式移動機器人的幾何中心在笛卡爾坐標系XOY中的坐標，θ是輪式移動機器人的航向角.依然考慮輪式移動機器人的質(zhì)心與幾何中心重合的情況，假設輪式移動機器人側向不打滑，即其不能沿著驅(qū)動輪軸線的方向運動，故機器人沿驅(qū)動輪軸線方向的速度為零，滿足純滾動無滑動的非完整約束

(5)

根據(jù)約束條件(5)，輪式移動機器人的運動學模型為:

(6)

相似地，對于輪式移動機器人的質(zhì)心與幾何中心不重合的情況，輪式移動機器人的約束條件和運動學模型也可以根據(jù)坐標轉(zhuǎn)換關系建立，所不同的是，約束方程和運動學方程都引入了與質(zhì)心和幾何中心之間距離相關的項，該項的引入亦不影響本文預演追蹤策略的執(zhí)行.至此，雙輪驅(qū)動輪式移動機器人的建模完成.

3 輪式移動機器人的軌跡跟蹤控制

基于運動學模型，輪式移動機器人的軌跡跟蹤問題可敘述為：尋找控制器u使輪式移動機器人的軌跡 [x(t)y(t)θ]T跟蹤預先給定的參考軌跡[xr(t)yr(t)θr]T.

為方便理解，將參考軌跡看成一個虛擬的輪式移動機器人的軌跡.令vr與θr分別為虛擬輪式移動機器人的線速度和方位角，則其運動學模型為：

假設1虛擬輪式移動機器人的線速度vr和角速度ωr不同時為零，即被跟蹤的虛擬輪式移動機器人始終處于移動或轉(zhuǎn)動狀態(tài).

下面首先根據(jù)運動學模型提供能夠保證輪式移動機器人跟蹤參考軌跡的虛擬控制器.其次，以虛擬控制器為橋梁，將參考軌跡轉(zhuǎn)化成可利用的預演信息，建立預演信息的離散線性模型，方便預演控制器的設計與執(zhí)行.

3.1 輪式移動機器人的虛擬控制器

根據(jù)運動學模型(6),輪式移動機器人的軌跡跟蹤誤差模型為：

進一步的推導表明，軌跡跟蹤誤差滿足如下方程：

(7)

為尋找虛擬控制器，輪式移動機器人的軌跡跟蹤問題此時可重述為: 對于較小的初始軌跡誤差，尋找有界的虛擬控制器[vω]T,使得

limt→(ex,ey,eθ)=(0,0,0),

(8)

limt→(v,ω)=(vr,ωr),

(9)

同時成立.

結合輪式移動機器人的誤差方程(7),選擇

作為時變系統(tǒng)(7)的一個候選Lyapunov函數(shù).不難推出，

選擇具有可調(diào)參數(shù)k1與k2的虛擬非線性狀態(tài)反饋控制律：

(10)

下面證明在虛擬控制器(10)的調(diào)節(jié)下，軌跡跟蹤誤差閉環(huán)系統(tǒng)解的漸近穩(wěn)定性.將虛擬控制器(10)代入軌跡跟蹤誤差系統(tǒng)(7)可得閉環(huán)系統(tǒng):

(11)

考慮系統(tǒng)(11)相關的不變集合

當ex=0,eθ=0時，根據(jù)式(11)可推得:

(12)

聯(lián)合假設1與式(12)可得，ey=0.這意味著不變集合S中僅包含零點.根據(jù)文獻[23]，閉環(huán)跟蹤誤差系統(tǒng)(11)在零點漸近穩(wěn)定，故式(8)成立.再結合式(10)，可推得式(9)亦成立.

注1若考慮虛擬機器人的線速度和角速度同時為零的情況，可參考文獻[11]，引入如下的變量

以克服輪式移動機器人的欠驅(qū)動特性[24]帶來的控制器設計困難，其中，k(t)=λ1vr,λ1為大于零的常數(shù)，滿足λ1|vr|max≤1，|vr|max為|vr|的極大值.本文重點強調(diào)使用預演信息設計控制器使機器人快速跟蹤給定軌跡，因而未對這類情況進行深入討論.

3.2 虛擬控制器的離散線性模型

由于輪式移動機器人的參考軌跡即虛擬機器人的軌跡可能是各種不同類型的曲線，結合式(10)則意味著：機器人的虛擬控制器可能是不同類型的曲線.因此，在設計預演控制器之前，本文先為虛擬控制器建立統(tǒng)一的狀態(tài)空間模型.

令yd(k)是虛擬控制器(10)在k時刻的采樣值，則虛擬控制器可建模成

xd(k+1)=Adxd(k)+Bdwd(k),
yd(k)=Cdxd(k).

(13)

其中，wd(k)可理解為虛擬控制系統(tǒng)的外部輸入，wd(k)=yd(k+Np);Np是給定的正整數(shù)，能夠表示對將來虛擬控制信息的使用量，即使用了多少將來的虛擬控制信息，稱為預演窗口寬度或預演長度;系數(shù)矩陣

觀察該模型的狀態(tài)xd(k)，不難發(fā)現(xiàn)，其中不僅包含當前時刻虛擬控制器的信息yd(k)，也包含將來時刻的虛擬控制器信息yd(k+1),yd(k+2),…,yd(k+Np-1).這對虛擬控制器可提前離線計算的移動舞臺機器人是可行的.該模型的建立，為利用將來的虛擬控制器信息設計預演控制器鋪平了道路.

注2Np越大，意味著可利用越多將來的虛擬控制器信息設計控制器.盡管如此，也應意識到：Np并非越大越好.這是因為：一方面，離當前時刻越久遠的將來虛擬控制信息，設計控制器時它的增益實際上越小，因而對跟蹤控制效果的貢獻也相對越小；另一方面，輪式移動機器人處理器的處理能力也將制約預演控制器的跟蹤性能.利用的將來虛擬控制信息越多，意味著虛擬控制器的模型(13)維度越高，這將給后續(xù)控制增益的求解帶來越高的運算代價.

注3由于虛擬控制器可能是任意曲線，因此，本文為虛擬控制器建立離散線性模型的方法可以推廣建立任意已知信號的離散線性模型.

3.3 最優(yōu)預演控制器的設計

至此，輪式移動機器人軌跡跟蹤的控制問題實際上轉(zhuǎn)化成了虛擬速度的跟蹤問題.為了使跟蹤誤差盡可能小，本文設計了一個最優(yōu)預演控制器，以最小化下面的代價函數(shù)

秀容月明像老師似地說個不停，喬瞧望著他，想笑，忍住了，說：“我不知道怎么用腳趾去剔。秀容月明，還是你來吧，我擔心又把藕弄斷了。”

(14)

在代價函數(shù)(14)中，

(15)

基于虛擬控制器的狀態(tài)空間模型，前述利用預演信息設計控制器的輪式移動機器人最優(yōu)追蹤問題將被重述成一個線性二次調(diào)節(jié)問題.該問題受到如下線性狀態(tài)空間方程的約束

xa(k+1)=Aaxa(k)+Bau(k)+Bdawd(k),

(16)

其中：

根據(jù)式(4)、(15)以及(16)，跟蹤誤差亦可以表示成

因此，跟蹤性能(13)也可以重新表述成

(17)

定理1考慮系統(tǒng)(16)，能夠使代價函數(shù)(17)最小的最優(yōu)預演控制器如下：

u(k)=-Kxa(k)-Nr(k)=

(18)

其中：

(19)

S滿足如下的代數(shù)Riccati方程，

證明與無已知輸入的標準線性二次調(diào)節(jié)問題的最優(yōu)解的推導類似，上述結果可根據(jù)極大值原理和繁復的代數(shù)運算推出.不同之處在于：這種情況下，由極大值原理產(chǎn)生的最優(yōu)狀態(tài)及伴隨狀態(tài)之間呈仿射關系而非線性關系.因此，詳細的證明此處不再贅述.

觀察(18)可以發(fā)現(xiàn)：最優(yōu)的控制器不僅跟xa(k)有關，還跟r(k)有關.根據(jù)(19)，r(k)包含了未來所有已知輸入，即它包含了所有的wd(s),s≥k，等價于r(k)包含了所有的yd(s),s≥k+Np.

另一方面，由于輪式移動機器人的處理器能力有限，使用太多的將來虛擬控制可能使跟蹤效果得不償失.為此，可以在最優(yōu)跟蹤性能和處理速度之間進行一個權衡，將控制器(18)修改為

(20)

結合(16)來觀察(20),不難發(fā)現(xiàn)，xa(k)中不僅包含當前時刻的虛擬控制信息，也包含了k+1,k+2,…,k+Np-1時刻的虛擬控制信息.

注4控制器(18)與(20)不是直接、顯式地利用將來的參考軌跡，而是以虛擬控制器為橋梁，通過使用將來的虛擬控制信息，把將來的參考軌跡充分利用起來.

4 數(shù)值仿真

為了驗證本文所提預演控制算法的有效性,本文對兩輪驅(qū)動輪式移動機器人進行了軌跡跟蹤仿真研究，觀察了最優(yōu)預演控制策略驅(qū)動下的輪式移動機器人的實際跟蹤效果.

仿真實驗中，輪式移動機器人的主要物理參數(shù)取值為：M=50 kg,r=0.125 m,c=0.05 kg·m2/s,Iv=10 kg·m2,Iω=0.05 kg·m2,L=0.5 m.先后以直線、圓、類正弦三類典型曲線為參考軌跡，驗證了本文所提跟蹤策略的有效性.為了節(jié)省空間，下文僅展示對類正弦曲線的跟蹤效果圖.

設輪式移動機器人的初始位姿誤差為(-0.3,0.4,-π/10)，待跟蹤軌跡由參考速度vr=1 m/s,ωr=2cos(πt/2)rad/s及θr=4sin(πt/2)/π rad生成.

1)如圖2所示，在虛擬控制器(10)的作用下，輪式移動機器人的軌跡跟蹤誤差在3 s左右收斂到了零.在實際控制器(18)(如圖3所示)的調(diào)節(jié)下，圖4中機器人的線速度和角速度僅需大約1 s就快速地跟蹤了虛擬控制器(10)，這保證了實際控制器(18)驅(qū)動下的輪式移動機器人也能在3 s內(nèi)跟蹤到參考軌跡(如圖5).對比圖2與圖5可知：研究虛擬控制器與研究實際控制器有本質(zhì)區(qū)別；同時也表明本文所提出的最優(yōu)預演跟蹤策略基本能夠保證實際控制器與虛擬控制器有相同的追蹤效果.

2)考慮到線性時不變的機器人動力學模型過于理想，討論當機器人的系統(tǒng)參數(shù)隨機變化時，本文所提跟蹤策略的調(diào)節(jié)效果.因為輪胎與地面的摩擦系數(shù)c、電機與傳動機構的驅(qū)動增益k往往會隨工作環(huán)境及工況的變化而變化，本文通過數(shù)值實驗展示這兩個參數(shù)的隨機變化對機器人跟蹤誤差的影響.

以下考慮參數(shù)c和k受到強弱兩種干擾后，本文所提出的控制策略的跟蹤效果.假設參數(shù)c和k均受到方差為σ的乘性隨機噪聲w的干擾，即干擾后的摩擦系數(shù)和增益分別為(1+w)c和(1+w)k.

圖2 虛擬控制器產(chǎn)生的軌跡跟蹤誤差

圖3 實際控制器

圖4 線速度與角速度的跟蹤誤差

圖5 實際控制器驅(qū)動下的軌跡跟蹤誤差

2.1)當σ=0.5時，實際控制器(18)如圖6所示，在該控制器作用下，機器人的線速度和角速度的跟蹤誤差如圖7所示，機器人的實際軌跡跟蹤誤差如圖8所示.

圖6 實際控制器

圖7 線速度與角速度跟蹤誤差

圖8 實際控制器驅(qū)動下的軌跡跟蹤誤差

2.2)當σ=1時，實際控制器(18)如圖9所示，在該控制器的作用下，機器人的線速度和角速度跟蹤誤差如圖10所示，機器人的實際軌跡跟蹤誤差如圖11所示.

將情況1)與情況2.1)、情況2.2)的實驗結果比較可知，當動力學系統(tǒng)參數(shù)受到的隨機干擾較小(即隨機干擾的方差σ=0.5)時，無論跟蹤時間還是跟蹤誤差，都與參數(shù)無擾動時基本一致.隨著隨機干擾的增強(即隨機干擾的方差σ=1)，速度與軌跡跟蹤誤差都明顯增大，由圖11可見，實際跟蹤軌跡有更明顯的縱向偏差.這說明該控制策略具有對參數(shù)c和k的小擾動不敏感的優(yōu)點.

圖9 實際控制器

圖10 線速度與角速度的跟蹤誤差

圖11 實際控制器驅(qū)動下的軌跡跟蹤誤差

5 結 論

1)圍繞如何充分利用已知的將來參考軌跡設計控制器使輪式移動機器人有效跟蹤參考軌跡的問題, 本文提出了最優(yōu)預演控制跟蹤策略，該策略將包含當前及將來參考軌跡及參考速度的信息于當前時刻用于機器人軌跡跟蹤，為機器人提供了足夠的處理及響應時間，因而能夠保證機器人快速有效地進行軌跡跟蹤；

2)基于運動學模型提出了一個輪式移動機器人的虛擬控制器，建立了該虛擬控制器的離散線性狀態(tài)空間模型，為利用將來的參考信息設計跟蹤策略做好了準備，該思路可以用于建立任意已知信號的離散線性狀態(tài)空間模型；

3)基于虛擬控制器的離散線性模型及離散化的輪式移動機器人動力學模型，將速度跟蹤問題等價轉(zhuǎn)化成一個特殊的、具有已知輸入項的線性系統(tǒng)最優(yōu)二次調(diào)節(jié)問題，利用極大值原理予以解決;

4)以類正弦曲線為參考軌跡進行數(shù)值實驗，考慮了機器人參數(shù)的隨機擾動對本文所提控制器對跟蹤效果的影響，驗證了本文所提算法的有效性.