雙輪差速移動機器人軌跡跟蹤混合控制算法研究*

許　萬，曹　松，羅　西，王　琪，楊　維

(湖北工業(yè)大學(xué) 機械工程學(xué)院，武漢　430068)

0　引言

隨著國家對工業(yè)智能化的重視，輪式移動機器人在服務(wù)、物流、探測等領(lǐng)域具有極廣的應(yīng)用前景，而對具有非完整系統(tǒng)的移動機器人的運動控制是關(guān)注的熱點。軌跡跟蹤問題作為其控制的重要研究問題，受到研究者的廣泛關(guān)注。軌跡跟蹤是使移動機器人的實際軌跡快速且穩(wěn)定地跟蹤上期望軌跡，問題的解決即是要設(shè)計出合理的控制器，以滿足移動機器人的性能要求。而在實際應(yīng)用中，由于移動機器人自身的結(jié)構(gòu)和系統(tǒng)以及外部的不確定因素，要達到理想的跟蹤效果，就顯得困難重重。

文獻[1-2]提出了一種基于Backstepping的控制方法，計算出合適的目標(biāo)速度，并在此基礎(chǔ)上加上最小均方算法作為觀測器，實現(xiàn)了最小化軌跡跟蹤誤差。文獻[3-4]運用終端滑動模態(tài)技術(shù)，提出基于滑?？刂频能壽E跟蹤控制策略，采用基于反算法的有限時間控制方法，提高了軌跡跟蹤的收斂性能。由于滑模變結(jié)構(gòu)具有不連續(xù)性的本質(zhì)，引起控制系統(tǒng)的抖振將是不可避免的。文獻[5-7]則針對抖振和收斂速度，提出了新型滑模變控制策略，在解決了系統(tǒng)中未知參數(shù)帶來的影響基礎(chǔ)上，抖振現(xiàn)象也被有效地抑制了，且較好地消除了跟蹤誤差。文獻[8]提出了基于優(yōu)化方法的模糊反演控制器在軌跡跟蹤問題中的應(yīng)用，利用動力學(xué)模型設(shè)計出力矩的控制律，達到了良好的跟蹤效果。

以上所述的控制算法，部分只運用了移動機器人的運動學(xué)模型，若應(yīng)用于重載移動機器人則存在明顯不足；部分算法結(jié)合了動力學(xué)模型，但是均涉及到力矩法，需要精確的力矩模型。本文在上述的研究基礎(chǔ)上，針對雙輪差速移動機器人，提出了基于前饋運動學(xué)和基于干擾觀測器的解耦動力學(xué)的軌跡跟蹤混合控制算法，該算法無需精確模型，即可取得良好的軌跡跟蹤性能。在運動學(xué)控制器中，采用位姿輸入的前饋控制，保證軌跡跟蹤的快速收斂；在動力學(xué)控制器中，采用前饋解耦補償器，將力矩控制律的設(shè)計轉(zhuǎn)變?yōu)樾碌妮o助量控制律的設(shè)計，加入干擾觀測器，增強控制器的魯棒性，運用具有積分鏈?zhǔn)浇Y(jié)構(gòu)微分器，抑制求導(dǎo)帶來的噪聲影響，在速度上保證移動機器人的跟蹤性能。最后將該算法運用到工程應(yīng)用中，驗證其可行性。

1　移動機器人系統(tǒng)建模

1.1　移動機器人運動學(xué)建模

圖1　雙輪差速移動機器人運動學(xué)模型

在理想情況下，具有非完整約束的移動機器人相對于局部坐標(biāo)系中YR軸的速度分量為0，即移動機器人的一個運動約束方程為：

(1)

則移動機器人的運動學(xué)模型滿足等式：

(2)

1.2　移動機器人動力學(xué)建模

考慮一個具有n個自由度并且受到m(1≤m

(3)

結(jié)合第1.1節(jié)可推算出：

(4)

(5)

從式(5)可以看出，在動力學(xué)模型中，控制輸入量是力矩τ。

1.3　移動機器人位姿誤差建模

(6)

通過坐標(biāo)轉(zhuǎn)換，可得到在局部坐標(biāo)系中雙輪差速移動機器人的位姿誤差e為：

(7)

圖2　移動機器人在局部坐標(biāo)系中的位姿誤差示意圖

2　移動機器人軌跡跟蹤控制器設(shè)計

2.1　運動學(xué)控制器設(shè)計

結(jié)合式(2)，對局部位姿誤差，即式(7)進行求導(dǎo)可得：

(8)

移動機器人運動學(xué)模型的輸入可設(shè)計為：

(9)

等式右邊的前半部分作為前饋輸入，后半部分v1和v2則作為新的反饋閉環(huán)控制系統(tǒng)的輸入。

把式(9)代入式(8)，并在平衡點(e1=e2=e3=0,v1=v2=0)處進行線性化，可以得到以局部位姿誤差為狀態(tài)變量的狀態(tài)方程：

(10)

對式(10)所描述的輸入V設(shè)計新的閉環(huán)系統(tǒng)控制律：

V=Ke

(11)

因該狀態(tài)方程有三個狀態(tài)變量和兩個輸入，則增益矩陣K的維度為2×3。

對式(11)定義的控制律可設(shè)計為：

(12)

若只考慮運動學(xué)模型，則該控制器的結(jié)構(gòu)框圖設(shè)計如圖3所示：

圖3　基于前饋運動學(xué)控制器的結(jié)構(gòu)框圖

2.2　動力學(xué)控制器設(shè)計

本文在動力學(xué)控制器中串聯(lián)了一個前饋解耦補償器；同時，引入帶有擾動觀測器[10]的二自由度控制系統(tǒng)[11]。

(13)

基于前饋補償器解耦的轉(zhuǎn)換控制框圖如圖4所示：

圖4　基于前饋補償器解耦的轉(zhuǎn)換控制框圖

由圖4可以看出，將對力矩τ控制律的設(shè)計轉(zhuǎn)換成為對輔助控制矢量z控制律的設(shè)計，本文所設(shè)計的輔助控制矢量z的控制律為：

(14)

式中，Kp=diag(kpv,kpω)，Kd=diag(kdv,kdω)

其動力學(xué)控制器的結(jié)構(gòu)框圖，如圖5所示。

圖5　帶有觀測器的解耦動力學(xué)控制器的結(jié)構(gòu)框圖

從圖5可以看出，經(jīng)過前饋解耦的動力學(xué)控制器組成還包括前饋控制器與PD反饋控制器組成的二自由度控制系統(tǒng)和擾動觀測器。其中，前饋控制對運動學(xué)控制器的速度輸出做了系統(tǒng)逆運算，計算得到的系統(tǒng)輸入能夠使動力學(xué)控制器的速度輸出精確跟蹤運動學(xué)控制器的速度輸出并且能夠保證系統(tǒng)內(nèi)部穩(wěn)定，從而提高了系統(tǒng)的跟蹤性能。同理，擾動觀測器動力學(xué)控制器的速度輸出做了系統(tǒng)逆運算，再與輔助控制輸入作比較，觀測出等效擾動，再反饋到輔助控制的輸入端，從而抵消外部擾動，進一步保證了系統(tǒng)的魯棒性。另外，在控制律式(14)中需要對輸入信號求導(dǎo)，本文采用二階線性積分鏈?zhǔn)轿⒎制鱗12]來實現(xiàn)，其定義如下：

(15)

式中，N(t)為待微分的輸入信號；x1為對信號進行跟蹤；x2為信號一階導(dǎo)數(shù)的估計；微分器的初始值為x1(0)=0，x2(0)=0。

噪聲一般存在于信號中進入微分器，由式(15)可以看出，只有微分器底層的微分方程中存在噪聲。在工程應(yīng)用中，具有積分鏈?zhǔn)浇Y(jié)構(gòu)的微分器，對含有噪聲的信號求導(dǎo)時，噪聲只含在微分器的最后一層，通過積分作用，信號一階導(dǎo)數(shù)中的噪聲能夠被更充分地抑制。

2.3　混合控制器設(shè)計及穩(wěn)定性分析

2.3.1混合控制器設(shè)計

針對雙輪差速移動機器人軌跡跟蹤問題，本文提出了一種基于前饋運動學(xué)和帶有干擾觀測器的解耦動力學(xué)的混合控制算法，把動力學(xué)控制器作為內(nèi)環(huán)加入到運動學(xué)控制器中，并給出混合控制器系統(tǒng)框圖，如圖6所示。

圖6　非完整移動機器人軌跡跟蹤控制器系統(tǒng)框圖

2.3.2穩(wěn)定性分析

本文對2.3.1節(jié)提出的混合控制器，用Lyapunov(李雅普諾夫)函數(shù)直接法來分析其穩(wěn)定性。

首先，取k2>0，式(12)可寫為:

v1=-k1e1

v2=-k2vde2-k3e3

(16)

移動機器人的實際速度與運動學(xué)控制器的輸出期望速度的誤差為速度跟蹤誤差，定義為:

(17)

式(14)可化簡為:

(18)

最后選取局部位姿誤差e1、e2、e3和速度跟蹤誤差ev、ew作為混合系統(tǒng)的狀態(tài)變量，則混合系統(tǒng)的狀態(tài)方程可寫為：

(19)

由式(19)可得，混合系統(tǒng)在e1=e2=e3=ev=ew=0處具有唯一的平衡狀態(tài)。選取總的Lyapunov函數(shù)為:

(20)

假設(shè)該函數(shù)對所有局部位姿誤差e1、e2、e3和速度跟蹤誤差ev、eω都具有一階偏導(dǎo)數(shù)。當(dāng)且僅當(dāng)e1=e2=e3=ev=eω=0時，恒有V(e′)=0；當(dāng)e1、e2、e3、ev、ew不全為零時，V(e′)>0，則V(e′)為正定的。

對式(20)求導(dǎo)并結(jié)合式(19)得：

(21)

3　仿真和樣機驗證

3.1　仿真

表1　軌跡跟蹤控制器參數(shù)選取

圖7所示為仿真的效果圖：

(a) 軌跡跟蹤示意圖

(b) 通過混合控制算法得出的速度

(c) 軌跡跟蹤位姿誤差圖7　移動機器人軌跡跟蹤控制器仿真圖

圖7b所示為通過軌跡跟蹤混合控制算法計算得出的移動機器人右輪速度vR、左輪速度vL、線速度v和角速度ω，可看出，均能夠快速穩(wěn)定跟蹤上給定的速度值。圖7c所示為位姿誤差，可得，存在初始誤差的情況下，移動機器人通過控制器能夠快速使位姿誤差收斂，且趨于0；在持續(xù)1.5s的外部干擾下，結(jié)合圖7a，機器人保持較好的穩(wěn)定性，在干擾過后位姿誤差能夠快速收斂。

3.2　樣機驗證

對于3.1節(jié)的仿真結(jié)果，是將雙輪差速移動機器人處于理想環(huán)境下得到的，而在實際應(yīng)用中，系統(tǒng)自身和外界因素的影響是不可忽視的。

圖10　移動機器人軌跡跟蹤控制器驗證圖

為了驗證本文所提出的軌跡跟蹤控制算法的可行性，結(jié)合仿真的設(shè)計，將算法移植到實際的移動機器人控制中。

3.2.1樣機構(gòu)成

圖8所示為本文研究對象的整體控制系統(tǒng)示意圖。該系統(tǒng)為自動導(dǎo)航系統(tǒng)，其中，負責(zé)路徑規(guī)劃的上位機通過WLAN(Wireless Local Area Networks)與地面控制系統(tǒng)進行交互；移動機器人的實際位姿由激光掃描儀NAV350獲取并通過RJ45與主控制器通訊；主控制器為基于Windows XPe的嵌入式控制器，主控制器與驅(qū)動器、驅(qū)動器與驅(qū)動電機之間通過CAN (Controller Area Network)總線進行通訊。圖9所示為本文所研發(fā)的雙輪差速移動機器人車體，車體有四個車輪，前兩輪為驅(qū)動輪，后兩輪為萬向輪，激光掃描儀安裝在兩驅(qū)動輪中軸線的正上方。移動機器人車體參數(shù)見表2。

圖8　控制系統(tǒng)示意圖

圖9　雙輪差速移動機器人

表2　移動機器人車體參數(shù)

3.2.2驗證結(jié)果

圖10b所示為移動機器人的位姿跟蹤誤差，圖中可以明確看出局部坐標(biāo)XR軸方向上的誤差e1在除去起點位置之外有3個跳變的點，這是由于在程序設(shè)計時認為當(dāng)移動機器人到達某個范圍之內(nèi)即表示到達了該點，而實際并沒有完全到達點的坐標(biāo)位置，所以會有一個跳變誤差，即使這樣，機器人的位姿誤差仍可以快速收斂于0，跟蹤上期望軌跡。從圖10b可以看出，移動機器人穩(wěn)定運行時的左右擺差為±9mm，停車誤差可控制在以終點為圓心，9mm為半徑的范圍內(nèi)。圖10c、圖10d所示為移動機器人實際的左右輪速度、線速度和角速度與理論的左右輪速度、線速度和角速度，由于初始位姿和初始速度值的不同，導(dǎo)致在運行起始階段實際速度有一定的波動，但機器人的實際速度在6s左右可跟蹤上理論速度，體現(xiàn)了快速收斂的性質(zhì)，同時通過實際速度與理論速度曲線的擬合程度可看出，移動機器人在速度的跟蹤上具有良好的實時性和跟蹤效果。圖10e所示為移動機器人的速度跟蹤誤差，可看出，從起始位置開始通過快速的動態(tài)調(diào)整過程后，速度跟蹤誤差穩(wěn)定在0附近。綜上所述，結(jié)合圖10a的實際軌跡跟蹤圖，說明本文提出的基于前饋運動學(xué)和帶有干擾觀測器的解耦動力學(xué)的混合軌跡跟蹤控制算法具有良好的跟蹤效果和抗干擾能力，可行性也得到了驗證。

4　總結(jié)

本文將雙輪差速移動機器人作為研究對象，在充分考慮系統(tǒng)自身和外部干擾因素的影響下，系統(tǒng)地介紹了基于前饋運動學(xué)和帶有干擾觀測器的解耦動力學(xué)的混合控制算法，在無需得到精確力矩模型的情況下，解決了移動機器人的軌跡跟蹤問題。仿真結(jié)果表明，基于該算法所設(shè)計的軌跡跟蹤控制器可實現(xiàn)良好的跟蹤性能和魯棒性，驗證了其有效性。驗證結(jié)果表明，存在初始誤差時，移動機器人可快速調(diào)整到穩(wěn)定狀態(tài)，達到了良好的跟蹤性能；存在外部干擾時，也可以平穩(wěn)運行，具備了較好的抗干擾能力，實現(xiàn)了較好的控制效果，驗證了該控制器的可行性。對于同樣具有非完整系統(tǒng)的研究,本文提出的算法具有一定的借鑒價值。

[參考文獻]

[1] Kanayama Y, Kimura Y, Miyazaki F, et al. A stable tracking control method for an autonomous mobile robot[C]// IEEE International Conference on Robotics and Automation, 1990. Proceedings. IEEE Xplore, 1991:384-389.

[2] Anushree R, Prasad B K S. Design and development of novel control strategy for trajectory tracking of mobile robot: Featured with tracking error minimization[C]// IEEE India Conference. IEEE, 2016:1-6.

[3] 李世華,田玉平.移動小車的軌跡跟蹤控制[J].控制與決策,2000,15(5):626-628.

[4] 張揚名,劉國榮,楊小亮.基于滑模變結(jié)構(gòu)的移動機器人軌跡跟蹤控制[J].計算機工程,2013,39(5):160-164.

[5] 席雷平,陳自力,齊曉慧.具有抖振抑制特性的機械臂快速滑模變結(jié)構(gòu)控制[J].電機與控制學(xué)報, 2012, 16(7):97-102.

[6] Jia P, Song C, Zhang X. Control and simulation of adaptive global trajectory tracking for nonholonomic mobile robots with parameter uncertainties[C]// International Symposium on Instrumentation & Measurement, Sensor Network and Automation. IEEE, 2012:314-317.

[7] 王宗義,李艷東,朱玲.非完整移動機器人的雙自適應(yīng)神經(jīng)滑?？刂芠J].機械工程學(xué)報,2010,46(23):16-22.

[8] Swadi S M, Tawfik M A, Abdulwahab E N, et al. Fuzzy-Backstepping Controller Based on Optimization Method for Trajectory Tracking of Wheeled Mobile Robot[C]// Uksim-Amss, International Conference on Computer Modelling and Simulation，2016:147-152.

[9] 唐述博. 非完整移動機器人點鎮(zhèn)定和軌跡跟蹤控制研究[D]. 大連:大連理工大學(xué),2005.

[10] Kempf C J, Kobayashi S. Disturbance observer and feedforward design for a high-speed direct-drive positioning table[J]. IEEE Transactions on Control Systems Technology, 2002, 7(5):513-526.

[11] 彭超.基于系統(tǒng)逆的魯棒二自由度控制方法研究[D].成都:電子科技大學(xué),2012.

[12] 王新華,劉金琨.微分器設(shè)計與應(yīng)用[M].北京:電子工業(yè)出版社,2010.

(編輯李秀敏)