雙輪差速移動(dòng)機(jī)器人軌跡跟蹤混合控制算法研究*

許　萬(wàn)，曹　松，羅　西，王　琪，楊　維

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

0　引言

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

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

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

1　移動(dòng)機(jī)器人系統(tǒng)建模

1.1　移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)建模

圖1　雙輪差速移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型

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

(1)

則移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型滿足等式：

(2)

1.2　移動(dòng)機(jī)器人動(dòng)力學(xué)建模

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

(3)

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

(4)

(5)

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

1.3　移動(dòng)機(jī)器人位姿誤差建模

(6)

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

(7)

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

2　移動(dòng)機(jī)器人軌跡跟蹤控制器設(shè)計(jì)

2.1　運(yùn)動(dòng)學(xué)控制器設(shè)計(jì)

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

(8)

移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型的輸入可設(shè)計(jì)為：

(9)

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

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

(10)

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

V=Ke

(11)

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

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

(12)

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

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

2.2　動(dòng)力學(xué)控制器設(shè)計(jì)

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

(13)

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

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

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

(14)

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

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

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

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

(15)

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

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

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

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

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

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

2.3.2穩(wěn)定性分析

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

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

v1=-k1e1

v2=-k2vde2-k3e3

(16)

移動(dòng)機(jī)器人的實(shí)際速度與運(yùn)動(dòng)學(xué)控制器的輸出期望速度的誤差為速度跟蹤誤差，定義為:

(17)

式(14)可化簡(jiǎn)為:

(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ù)對(duì)所有局部位姿誤差e1、e2、e3和速度跟蹤誤差ev、eω都具有一階偏導(dǎo)數(shù)。當(dāng)且僅當(dāng)e1=e2=e3=ev=eω=0時(shí)，恒有V(e′)=0；當(dāng)e1、e2、e3、ev、ew不全為零時(shí)，V(e′)>0，則V(e′)為正定的。

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

(21)

3　仿真和樣機(jī)驗(yàn)證

3.1　仿真

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

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

(a) 軌跡跟蹤示意圖

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

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

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

3.2　樣機(jī)驗(yàn)證

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

圖10　移動(dòng)機(jī)器人軌跡跟蹤控制器驗(yàn)證圖

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

3.2.1樣機(jī)構(gòu)成

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

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

圖9　雙輪差速移動(dòng)機(jī)器人

表2　移動(dòng)機(jī)器人車體參數(shù)

3.2.2驗(yàn)證結(jié)果

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

4　總結(jié)

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

[參考文獻(xiàn)]

[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] 李世華,田玉平.移動(dòng)小車的軌跡跟蹤控制[J].控制與決策,2000,15(5):626-628.

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

[5] 席雷平,陳自力,齊曉慧.具有抖振抑制特性的機(jī)械臂快速滑模變結(jié)構(gòu)控制[J].電機(jī)與控制學(xué)報(bào), 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] 王宗義,李艷東,朱玲.非完整移動(dòng)機(jī)器人的雙自適應(yīng)神經(jīng)滑模控制[J].機(jī)械工程學(xué)報(bào),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] 唐述博. 非完整移動(dòng)機(jī)器人點(diǎn)鎮(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è)計(jì)與應(yīng)用[M].北京:電子工業(yè)出版社,2010.

(編輯李秀敏)