基于自適應(yīng)EKF的移動(dòng)機(jī)器人軌跡跟蹤控制器

于　晨，馮顯英

(山東大學(xué) 機(jī)械工程學(xué)院 高效潔凈機(jī)械制造教育部重點(diǎn)實(shí)驗(yàn)室，濟(jì)南　250061)

0　引言

隨著輪式移動(dòng)機(jī)器人應(yīng)用越發(fā)廣泛，人們對(duì)其自動(dòng)化程度的要求不斷提高。而對(duì)軌跡跟蹤性能的改善是提高其自動(dòng)化程度的一個(gè)重要基礎(chǔ)。因此，眾多科研人員進(jìn)行了大量的研究[1]。軌跡跟蹤控制的目標(biāo)是通過(guò)調(diào)節(jié)機(jī)器人的運(yùn)動(dòng)速度和運(yùn)動(dòng)方向，使機(jī)器人沿期望的軌跡運(yùn)動(dòng)。但在實(shí)際運(yùn)行過(guò)程中，移動(dòng)機(jī)器人具有時(shí)變、非線性的動(dòng)力學(xué)特征，同時(shí)存在傳感器測(cè)量誤差的問(wèn)題。要實(shí)現(xiàn)移動(dòng)機(jī)器人有效、精確地軌跡跟蹤控制，需要解決兩個(gè)方面的問(wèn)題，一是能夠準(zhǔn)確實(shí)時(shí)得到當(dāng)前機(jī)器人位姿；二是根據(jù)當(dāng)前機(jī)器人位姿能夠準(zhǔn)確迅速調(diào)整移動(dòng)機(jī)器人，從而跟隨預(yù)定軌跡移動(dòng)。

對(duì)于工作于室外的導(dǎo)航系統(tǒng)，通常采用INS/GPS組合導(dǎo)航，但GPS定位只能用于室外定位，限制了其在室內(nèi)應(yīng)用。本文采用陀螺儀、加速度傳感器和地磁傳感器三者的組合，來(lái)獲取機(jī)器人姿態(tài)；通過(guò)編碼器獲取機(jī)器人速度信息，使用自適應(yīng)擴(kuò)展卡爾曼濾波算法，實(shí)現(xiàn)在沒(méi)有GPS的情況下獲取移動(dòng)機(jī)器人相對(duì)位姿數(shù)據(jù)。之后構(gòu)造了具有全局穩(wěn)定性的閉環(huán)控制器控制移動(dòng)機(jī)器人，實(shí)現(xiàn)對(duì)給定軌跡的跟蹤，通過(guò)仿真實(shí)驗(yàn)驗(yàn)證了其有效性和正確性。

1　移動(dòng)機(jī)器人位姿獲取

由于傳感器存在一定的固有缺陷，因此使用單一的傳感器很難準(zhǔn)確的移動(dòng)機(jī)器人位姿，需要利用多個(gè)傳感器互相補(bǔ)充得到較為準(zhǔn)確的數(shù)據(jù)。如結(jié)合視覺(jué)傳感器與指北針獲取移動(dòng)機(jī)器人偏航角[3]?；蚶每柭鼮V波，為每個(gè)傳感器安排權(quán)值，實(shí)現(xiàn)多傳感器融合[4，8]。但該算法要求系統(tǒng)必須為線性系統(tǒng)。還有許多設(shè)計(jì)中采用互補(bǔ)濾波算法實(shí)現(xiàn)兩傳感器的數(shù)據(jù)融合。但一般的互補(bǔ)濾波，權(quán)重參數(shù)是固定的，在這一方面明顯弱于可以自動(dòng)調(diào)整參數(shù)的卡爾曼濾波。因此，本文采用可用于非線性系統(tǒng)的擴(kuò)展卡爾曼濾波算法(EKF)實(shí)現(xiàn)數(shù)據(jù)融合。

使用EKF算法融合陀螺儀與加速度傳感器數(shù)據(jù)已經(jīng)有了大量應(yīng)用[2],為避免上述文獻(xiàn)中采用歐拉角作為系統(tǒng)狀態(tài)時(shí)的大量三角函數(shù)運(yùn)算，本文取系統(tǒng)狀態(tài)為X。

(1)

其中，

(2)

(3)

其中，

(4)

Ok為關(guān)于角速度的反對(duì)稱矩陣[5]：

(5)

ξk為測(cè)量噪聲，姿態(tài)的測(cè)量方程為：

zk=H·Xk+ξk

(6)

可得測(cè)量轉(zhuǎn)移矩陣為：

傳統(tǒng)EKF算法中測(cè)量噪聲協(xié)方差Rk采用通過(guò)離線測(cè)量確定的常值，但在傳感器噪聲統(tǒng)計(jì)特征未知或測(cè)量噪聲隨時(shí)間發(fā)生變化的情況下，便難以實(shí)現(xiàn)對(duì)系統(tǒng)狀態(tài)的有效估計(jì)。為了改進(jìn)這一缺陷，需要Rk能夠自動(dòng)調(diào)整。但測(cè)量噪聲實(shí)際方差往往無(wú)法直接獲得，不便采用直接調(diào)節(jié)Rk的方式實(shí)現(xiàn)自適應(yīng)狀態(tài)估計(jì)。參考文獻(xiàn)[9]中自適應(yīng)卡爾曼算法，設(shè)VK為濾波器的新息狀態(tài);Pv,k為新息方差理論值。

(7)

(8)

可以看出新息中包含有測(cè)量噪聲ξk，新息隨測(cè)量噪聲變化而變化，故可將新息的統(tǒng)計(jì)特征作為調(diào)整濾波算法的依據(jù)。Vk的實(shí)測(cè)方差[9]：

(9)

其中，k-M+1為新息序列長(zhǎng)度，可以證明Cv,k為Pv,k的最大似然估計(jì)[7]。由于Pv,k與卡爾曼增益Kk的分母的表達(dá)式相同，因此可將帶入上式，為簡(jiǎn)化運(yùn)算，取Cv,k的遞推形式：

(10)

得到自適應(yīng)EKF算法:

(11)

經(jīng)過(guò)上述算法對(duì)傳感器信號(hào)的處理，可得到機(jī)器人在慣性系中的姿態(tài)矩陣：

(12)

得到移動(dòng)機(jī)器人當(dāng)前相對(duì)初始點(diǎn)的位姿：

(13)

通過(guò)EKF算法得到的移動(dòng)機(jī)器人位姿數(shù)據(jù)將反饋至位姿控制器進(jìn)行位姿控制。

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

為實(shí)現(xiàn)移動(dòng)機(jī)器人對(duì)軌跡的跟蹤，本文構(gòu)建了雙環(huán)軌跡追蹤控制器,見(jiàn)圖1。位置控制為外環(huán)，位置控制環(huán)通過(guò)產(chǎn)生連續(xù)的控制律u1,u2，在控制移動(dòng)機(jī)器人行進(jìn)速度的同時(shí)生成中間指令信號(hào)θd，傳遞給姿態(tài)控制環(huán)。姿態(tài)控制環(huán)則通過(guò)滑模控制律跟蹤中間指令信號(hào)θd。為抑制姿態(tài)控制環(huán)性能對(duì)整個(gè)閉環(huán)系統(tǒng)穩(wěn)定性的影響，本文采用雙曲正切函數(shù)設(shè)計(jì)控制律，實(shí)現(xiàn)全局穩(wěn)定的軌跡跟蹤控制[6]。

圖1　控制器結(jié)構(gòu)框圖

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

忽略移動(dòng)機(jī)器人在低速轉(zhuǎn)彎時(shí)離心力對(duì)機(jī)器人的影響，建立如式(14)所示的近似運(yùn)動(dòng)模型，見(jiàn)圖2，式中v和ω分別為機(jī)器人的線速度和角速度,其值可通過(guò)左右兩側(cè)在k時(shí)刻速度vR與vL確定。b為機(jī)器人左右兩側(cè)履帶地面接觸點(diǎn)之間的距離。

圖2　輪式移動(dòng)機(jī)器人理想模型

(14)

(15)

(xe,ye)為位置誤差，θe為姿態(tài)誤差，可以把移動(dòng)機(jī)器人位姿誤差方程描述為：

(16)

2.2　位置控制律設(shè)計(jì)

引理1[6]，對(duì)任意η∈R且|η|<∞,若η滿足η=-αtanh(k·η) (α,k>0)，則在t→∞時(shí)，η→0。

(17)

取

(18)

由u2/u1= tanθ可得到滿足位置控制律所要求目標(biāo)角度θd=arctan(u2/u1)，為避免當(dāng)前偏轉(zhuǎn)角θ與θd之間差異對(duì)系統(tǒng)穩(wěn)定性的影響，據(jù)引理1，若設(shè)計(jì)控制律:

(19)

則可實(shí)現(xiàn)xe→∞，ye→0?？傻脤?shí)際位置控制律為:

(20)

離散化后可得：

(21)

(22)

2.3　姿態(tài)控制律設(shè)計(jì)

由于滑?？刂凭哂蟹蔷€性和對(duì)擾動(dòng)不敏感的特性，因此姿態(tài)控制環(huán)采用滑?？刂啤ＴO(shè)計(jì)滑模函數(shù)為：

s=θe

(23)

取指數(shù)趨近律：

(24)

為了減弱抖動(dòng)，采用飽和函數(shù)sat(t)代替sgn(s),從而得到如下的姿態(tài)控制律：

(25)

(26)

由于位置控制環(huán)輸出θd為離散的θd,k，本文使用非線性微分跟蹤器[10]對(duì)其跟蹤并提取其近似微分信號(hào)。離散形式的非線性微分跟蹤器為：

(27)

(28)

(29)

ωk=r2,k-k-k·(θk-r1,k)-η·sat(θk-r1,k)

(30)

3　仿真驗(yàn)證

仿真軌跡為yd=2·sin(0.5·xd)+0.5·xd;控制器控制頻率100Hz，采樣時(shí)間T=0.01s 陀螺儀漂移為2 °/ h ,陀螺儀白噪聲功率0.01 , 加速度計(jì)白噪聲功率0.02 , 地磁傳感器白噪聲功率0.02，移動(dòng)機(jī)器人初始線速度v=1.80 , 白噪聲功率0.01。微分追蹤器δ=400,d0=0.002 。在對(duì)本文的控制器進(jìn)行仿真之外，將位置環(huán)的控制算法改為傳統(tǒng)的PID控制之后，也進(jìn)行了仿真，以此作為對(duì)照。使用Matlab Simulink工具仿真結(jié)果如圖3～圖6所示。

圖3　曲線軌跡跟蹤效果

(a) 全局穩(wěn)定控制律

(b) PID控制圖4　軌跡跟蹤誤差

由于實(shí)際姿態(tài)角θ與目標(biāo)姿態(tài)角θd會(huì)存在偏差，通過(guò)圖3b與圖4b可以看出，當(dāng)位置環(huán)采用非全局穩(wěn)定的PID算法時(shí)，因其控制的效果嚴(yán)重依靠?jī)?nèi)環(huán)——姿態(tài)環(huán)的收斂速度，因此雖然能夠在一定程度上跟蹤目標(biāo)軌跡，但其位置誤差相對(duì)較大，并存在震蕩。而當(dāng)位置控制環(huán)使用具有全局穩(wěn)定特性的控制律時(shí)，因其保證了xe與ye在任何時(shí)間內(nèi)有界，因此通過(guò)圖3a與圖4a可以看出位置誤差相對(duì)PID控制明顯減小并且不存在震蕩。

(a) 速度估計(jì)

(b) 速度估計(jì)誤差與測(cè)量誤差圖5　速度估計(jì)效果

(a) 姿態(tài)角估計(jì)

(b) 姿態(tài)角估計(jì)誤差與測(cè)量誤差圖6　姿態(tài)角估計(jì)效果

根據(jù)圖5與圖6可以發(fā)現(xiàn)，相對(duì)于傳統(tǒng)的卡爾曼濾波，本文在不需要測(cè)量噪聲的先驗(yàn)知識(shí)的情況下，即在測(cè)量噪聲協(xié)方差R未知的情況下，通過(guò)計(jì)算新息理論方差的極大似然估計(jì)Cv,k代替R修正濾波增益的值。速度估計(jì)誤差絕對(duì)值的平均值(MAD)與姿態(tài)角軌跡誤差絕對(duì)值的平均值相比測(cè)量值均減小了65%以上。實(shí)現(xiàn)了對(duì)移動(dòng)機(jī)器人狀態(tài)的良好估計(jì)。

4　結(jié)論

本文針對(duì)輪式移動(dòng)機(jī)器人在軌跡跟蹤過(guò)程中存的兩個(gè)實(shí)際問(wèn)題，首先使用自適應(yīng)EKF算法構(gòu)造了位姿估計(jì)器，在不需要傳感器噪聲先驗(yàn)知識(shí)的情況下獲取了移動(dòng)機(jī)器人相對(duì)準(zhǔn)確的位姿數(shù)據(jù)。與傳統(tǒng)卡爾曼濾波相比，由于不需要測(cè)量噪聲協(xié)方差R，因此省去了確定R的過(guò)程，同時(shí)避免了測(cè)量噪聲協(xié)方差對(duì)位姿數(shù)據(jù)估計(jì)的影響。

其次，在得到移動(dòng)機(jī)器人當(dāng)前估計(jì)位姿后，采用由位置環(huán)與姿態(tài)環(huán)構(gòu)成的軌跡跟蹤控制器對(duì)移動(dòng)機(jī)器人進(jìn)位置跟蹤，其中作為外環(huán)的位置環(huán)采用了全局穩(wěn)定的控制律，相比于非全局穩(wěn)定的控制律對(duì)內(nèi)環(huán)的收斂速度依賴性較小，即當(dāng)移動(dòng)機(jī)器人實(shí)際姿態(tài)角與目標(biāo)姿態(tài)角相差較大時(shí)，本文的控制器依然能夠?qū)崿F(xiàn)對(duì)目標(biāo)軌跡的有效跟蹤。

最后，為進(jìn)一步轉(zhuǎn)化為實(shí)際控制器中的控制程序，整套算法均采用了離散形式的位姿估計(jì)器與位姿控制律，以離散形式的非線性微分跟蹤器銜接位置環(huán)與姿態(tài)環(huán)。仿真結(jié)果驗(yàn)證了整套位姿控制器的正確性。但仿真結(jié)果也顯示，由于本文使用的傳感器不能反饋移動(dòng)機(jī)器人當(dāng)前位置數(shù)據(jù)，而是由速度與姿態(tài)角去推算位置，因此并沒(méi)有避免位置誤差的累積。仿真結(jié)果顯示，30s后Y方向產(chǎn)生了約1%的累計(jì)誤差。因此在對(duì)位置誤差要求嚴(yán)格的應(yīng)用中，需要定時(shí)或定點(diǎn)對(duì)移動(dòng)機(jī)器人位置進(jìn)行重新標(biāo)定。

[參考文獻(xiàn)]

[1] Kolmanovsky I, Mcclamroch N H. Developments in nonholonomic control problems[J]. IEEE Control Systems, 1995, 15(6):20-36.

[2] Martin P, Salaün E. The true role of accelerometer feedback in quadrotor control[C]// IEEE International Conference on Robotics and Automation. IEEE, 2010:1623-1629.

[3] 鐘映春, 劉阿明, 謝仁亮. 基于機(jī)器視覺(jué)的室內(nèi)移動(dòng)機(jī)器人導(dǎo)航偏轉(zhuǎn)角的確定[J]. 組合機(jī)床與自動(dòng)化加工技術(shù), 2017(4):1-4.

[4] Sun S L, Deng Z L. Multi-sensor optimal information fusion Kalman filter[M]. Pergamon Press, Inc. 2004.

[5] CSDN. 角速度與旋轉(zhuǎn)矩陣的轉(zhuǎn)換關(guān)系及思考[EB/OL]. http://blog.csdn.net/gh234505/article/details/51214089, 2016-04-21.

[6] Ailon A, Zohar I. Controllers for trajectory tracking and string-like formation in Wheeled Mobile Robots with bounded inputs[C]// Melecon 2010-2010, IEEE Mediterranean Electrotechnical Conference. IEEE Xplore, 2010:1563-1568.

[7] 卞鴻巍,金志華,王俊璞,等. 組合導(dǎo)航系統(tǒng)新息自適應(yīng)卡爾曼濾波算法[J]. 上海交通大學(xué)學(xué)報(bào),2006,40(6):1000-1003,1009.

[8] 王素青, 黃文龍. 基于Android的雙輪機(jī)器人控制系統(tǒng)研究與設(shè)計(jì)[J]. 組合機(jī)床與自動(dòng)化加工技術(shù), 2016(12):60-64.

[9] 趙龍, 吳康. 新型自適應(yīng)Kalman濾波算法及其應(yīng)用[J]. 壓電與聲光, 2009, 31(6):908-911.

[10] 韓京清, 袁露林. 跟蹤-微分器的離散形式[J]. 系統(tǒng)科學(xué)與數(shù)學(xué), 1999, 19(3):268-273.

(編輯李秀敏)