于 晨,馮顯英
(山東大學(xué) 機(jī)械工程學(xué)院 高效潔凈機(jī)械制造教育部重點(diǎn)實(shí)驗(yàn)室,濟(jì)南 250061)
隨著輪式移動(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)證了其有效性和正確性。
由于傳感器存在一定的固有缺陷,因此使用單一的傳感器很難準(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)行位姿控制。
為實(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ò)滑??刂坡筛欀虚g指令信號(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)框圖
忽略移動(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)
引理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)
由于滑??刂凭哂蟹蔷€性和對(duì)擾動(dòng)不敏感的特性,因此姿態(tài)控制環(huán)采用滑??刂?。設(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)
仿真軌跡為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ì)。
本文針對(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.
(編輯李秀敏)