改進(jìn)ERTS平滑算法在位置跟蹤中的應(yīng)用研究

薛永勝 張 華 霍建文

(西南科技大學(xué)信息工程學(xué)院，四川 綿陽(yáng) 621010)

改進(jìn)ERTS平滑算法在位置跟蹤中的應(yīng)用研究

薛永勝 張 華 霍建文

(西南科技大學(xué)信息工程學(xué)院，四川 綿陽(yáng) 621010)

針對(duì)標(biāo)準(zhǔn)ERTS平滑算法在位置和姿態(tài)估計(jì)中計(jì)算復(fù)雜、效率低、精度不高等問(wèn)題，提出了利用奇異值分解法改進(jìn)ERTS平滑算法優(yōu)化位置和姿態(tài)數(shù)據(jù)的新方法。對(duì)系統(tǒng)采集到的位置和姿態(tài)信息進(jìn)行前向擴(kuò)展卡爾曼濾波，降低系統(tǒng)噪聲的初步影響；對(duì)濾波后的均方誤差陣進(jìn)行奇異值分解，并降低后向遞推增益和預(yù)測(cè)值計(jì)算量，提高了預(yù)測(cè)精度，有效增強(qiáng)了系統(tǒng)的抗干擾性和穩(wěn)定性。Turtlebot移動(dòng)機(jī)器人平臺(tái)的試驗(yàn)效果證明該算法在位置和姿態(tài)估計(jì)中的高效性和穩(wěn)定性。

擴(kuò)展卡爾曼濾波 奇異值分解法 最優(yōu)平滑算法 最優(yōu)估計(jì) 位置跟蹤

0 引言

精確、穩(wěn)定的位置和姿態(tài)測(cè)算是移動(dòng)機(jī)器人自主導(dǎo)航的核心課題之一。在諸多移動(dòng)機(jī)器人位置和姿態(tài)定位算法中，預(yù)測(cè)、濾波和平滑三類(lèi)最優(yōu)估計(jì)[1]是提升移動(dòng)機(jī)器人位姿受信息信噪比的有效方法，可為控制系統(tǒng)提供更精準(zhǔn)、穩(wěn)定的位置和姿態(tài)參數(shù)。

相對(duì)于濾波估計(jì)而言，平滑估計(jì)能夠獲得優(yōu)于濾波的估計(jì)精度。平滑估計(jì)一般可分為固定點(diǎn)平滑、固定滯后平滑和固定區(qū)間平滑。其中，固定區(qū)間平滑的應(yīng)用范圍最為廣泛，但固定區(qū)間平滑估計(jì)在后向遞推計(jì)算中，需要進(jìn)行協(xié)方差陣的求逆運(yùn)算和兩正定矩陣的相減運(yùn)算，因此存在計(jì)算效率低和數(shù)值穩(wěn)定性差的問(wèn)題[1]。為解決這些問(wèn)題，本文介紹一種改進(jìn)的ERTS定區(qū)間平滑算法。

1 ERTS平滑算法

為保證移動(dòng)機(jī)器人在移動(dòng)過(guò)程中傳感器提供的位置和姿態(tài)數(shù)據(jù)穩(wěn)定可靠，實(shí)際引入了ERTS平滑算法削弱噪聲對(duì)系統(tǒng)的影響。該算法對(duì)位置的姿態(tài)數(shù)據(jù)先進(jìn)行EKF濾波，之后采用RTS平滑算法對(duì)數(shù)據(jù)進(jìn)行最優(yōu)平滑優(yōu)化。系統(tǒng)框圖如圖1所示。

圖1 系統(tǒng)框圖

1.1 前向EKF濾波器

擴(kuò)展卡爾曼濾波器于20世紀(jì)60年代提出，是一種歷史悠久、應(yīng)用廣泛的非線(xiàn)性高斯次優(yōu)濾波算法。該算法在卡爾曼濾波算法的基本框架上，通過(guò)對(duì)非線(xiàn)性函數(shù)的泰勒展開(kāi)式進(jìn)行一階線(xiàn)性化截?cái)啵⒁源私品蔷€(xiàn)性狀態(tài)后驗(yàn)均值和協(xié)方差[2]。常用的非線(xiàn)性離散隨機(jī)系統(tǒng)模型如下。

xk+1=f[x(k),u(k)]+ω(t)

(1)

(2)

將系統(tǒng)方程進(jìn)行線(xiàn)性化[2]表示：

(3)

(4)

結(jié)合文獻(xiàn)[2]～[3]中離散EKF濾波器的實(shí)現(xiàn)步驟，首先初始化濾波器，即當(dāng)k=0時(shí)，有：

(5)

(6)

當(dāng)K=1，2，…時(shí)，通過(guò)迭代運(yùn)算估計(jì)各時(shí)刻目標(biāo)的狀態(tài)向量：

xk+1|k=f[x(k,k),u(k)]

(7)

Pk+1|k=JPk|kJT+Qk

(8)

(9)

(10)

(11)

xk+1|k+1=xk+1|k+Wk+1?k+1

(12)

(13)

相對(duì)傳統(tǒng)的低通濾波器和卡爾曼濾波器，擴(kuò)展卡爾曼濾波器不僅避免了狀態(tài)方程線(xiàn)性化過(guò)程中帶來(lái)的誤差和不穩(wěn)定影響，而且其具有自適應(yīng)性的反饋增益矩陣，使得系統(tǒng)偏差在統(tǒng)計(jì)意義上達(dá)到最小，使系統(tǒng)能同時(shí)保證穩(wěn)定性和響應(yīng)速度[4]。

1.2RTS平滑算法

RTS平滑算法原理如圖2所示。

圖2 RTS平滑算法原理圖

RTS平滑算法是一種利用某一時(shí)間區(qū)間內(nèi)的所有量測(cè)信息對(duì)所有狀態(tài)進(jìn)行估計(jì)的離線(xiàn)處理算法。它能夠獲得優(yōu)于濾波的估計(jì)精度[1,5]，廣泛應(yīng)用于聲音信號(hào)處理、目標(biāo)跟蹤和導(dǎo)航制導(dǎo)等領(lǐng)域[1,6-9]。該算法包括前向?yàn)V波和后向遞推兩個(gè)部分[1]。

圖2中，上標(biāo)“+”和“-”分別表示濾波的更新過(guò)程和預(yù)測(cè)過(guò)程；“F”和“S”分別表示前向?yàn)V波過(guò)程和后向遞推平滑過(guò)程。前向?yàn)V波過(guò)程利用固定時(shí)間區(qū)間[0N]內(nèi)所有采樣值進(jìn)行EKF濾波，并存儲(chǔ)式(7)～(13)中的狀態(tài)估計(jì)xk+1|k+1、估計(jì)均方差誤差陣Pk+1|k+1和預(yù)測(cè)均方差誤差陣Pk+1|k。后向遞推公式[15]如下：

xk|N=xk+KS(xk+1|N-xk+1|k)

(14)

(15)

其中后向遞推的平滑增益矩陣為：

(16)

1.3 算法改進(jìn)

由于RTS算法中存在大量(n×n)矩陣相乘，為減少運(yùn)算過(guò)程的存儲(chǔ)和計(jì)算量，采用可靠的矩陣分解算法奇異值分解法[5]對(duì)Pk|N和Pk+1|N進(jìn)行定義，得：

(17)

(18)

定義Mk滿(mǎn)足：

(19)

將上式帶入式(16)化簡(jiǎn)為：

(20)

為進(jìn)行奇異值分解運(yùn)算，將Mk求逆得：

(21)

(22)

對(duì)上式兩邊分別左乘各自轉(zhuǎn)置陣后得到：

(23)

由式(20)和式(22)得到：

uk=V″kDk=(D″k)-1

(24)

同理可實(shí)現(xiàn)式(17)的奇異值分解運(yùn)算，即：

uk|N=V″kDk|N=D″k

(25)

將式(23)和式(24)帶入式(17)可得改進(jìn)后的后向遞推的平滑增益矩陣：

(26)

由式(23)～(25)及式(15)組成了最優(yōu)RTS平滑算法，而配合前向EKF濾波過(guò)程產(chǎn)生的狀態(tài)估計(jì)和誤差矩陣即為本文改進(jìn)的ERTS平滑算法。

2 應(yīng)用及仿真分析

本文研究的移動(dòng)機(jī)器人模型是基于兩輪驅(qū)動(dòng)的Turtlebot移動(dòng)機(jī)器人，其位姿傳感器型號(hào)及參數(shù)如表1所示。其中，ticks/enc表示每圈脈沖數(shù)。

表1 Turtlebot部分傳感器參數(shù)

L3G4200D具有功耗低、精度高、使用靈活的特點(diǎn)，實(shí)際移動(dòng)中僅用單軸進(jìn)行航向參數(shù)測(cè)量；編碼器精度高、體積小，其作為位置反饋的重要設(shè)備。為實(shí)現(xiàn)對(duì)移動(dòng)機(jī)器人的位置跟蹤控制，對(duì)機(jī)器人模型進(jìn)行簡(jiǎn)化，如圖3所示。

圖3 Turtlebot機(jī)器人簡(jiǎn)化模型

(27)

其中D為小車(chē)前后輪間的間距。為了實(shí)現(xiàn)位置跟蹤控制系統(tǒng)的閉環(huán)控制，控制系統(tǒng)加入陀螺儀和編碼器對(duì)系統(tǒng)運(yùn)動(dòng)狀態(tài)進(jìn)行測(cè)量，其距離ri和偏轉(zhuǎn)角度θi測(cè)量方程為：

(28)

式中:x、y為當(dāng)前移動(dòng)機(jī)器人本體在的坐標(biāo)；xi、yi為上一次移動(dòng)機(jī)器人本體的坐標(biāo)。

設(shè)置采樣周期T=10ms，Wk為[-1.0,1.0]的白噪聲，vk為[-0.02,0.02]的白噪聲信號(hào),設(shè)角度跟蹤輸出指令為sint,t=kT,仿真時(shí)間10s，在軟件MatlabR2012a進(jìn)行系統(tǒng)仿真。

2.1 結(jié)果比較

為驗(yàn)證RTS平滑算法應(yīng)用到目標(biāo)跟蹤的有效性，選取分段數(shù)N=10,結(jié)合文獻(xiàn)[10]～[12]中的標(biāo)準(zhǔn)ERTS平滑算法和EKF濾波器進(jìn)行輸出效果對(duì)比，如圖4所示。

圖4 不同算法跟蹤比較

由圖4可見(jiàn)，單獨(dú)的EKF濾波器效果在估計(jì)上存在較大波動(dòng)，綜合RTS平滑算法后在數(shù)據(jù)最優(yōu)估計(jì)上更加平滑，有效削弱了噪聲的影響。不同算法的跟蹤誤差曲線(xiàn)如圖5所示。

圖5 不同算法跟蹤誤差比較

三種算法在Matlab R2012a環(huán)境下的計(jì)算時(shí)間和誤差對(duì)比如表2所示。

表2 不同平滑器算法性能比較

由表2分析知，三種算法均能有效地抑制噪聲信號(hào)。單獨(dú)的EKF濾波器波動(dòng)較大。標(biāo)準(zhǔn)的ERTS平滑算法在EKF基礎(chǔ)上添加了RTS平滑算法，多耗費(fèi)0.118 s，精度提高52.0%，改進(jìn)后的ERTS平滑算法定位精度相較于其他兩者分別提高了54.6%、13.6%，并且在接近EKF的計(jì)算耗時(shí)內(nèi)大幅提升了系統(tǒng)的精度和穩(wěn)定性。將本文改進(jìn)算法應(yīng)用到Turtlebot機(jī)器人本體上，大大提高了其位置跟蹤的精確度，驗(yàn)證結(jié)果如表3所示。

表3 Turtlebot機(jī)器人平臺(tái)算法驗(yàn)證結(jié)果

2.2 抗干擾分析

由于控制系統(tǒng)中總存在著干擾和噪聲，現(xiàn)代控制對(duì)系統(tǒng)的魯棒性要求較高。為驗(yàn)證濾波器的抗干擾能力，將測(cè)量噪聲和系統(tǒng)噪聲均增強(qiáng)2倍，分別對(duì)標(biāo)準(zhǔn)ERTS和本文改進(jìn)算法進(jìn)行跟蹤效果驗(yàn)證，結(jié)果如圖6所示。

圖6 增強(qiáng)Wk和vk后的跟蹤結(jié)果

由圖6分析可知，系統(tǒng)在噪聲干擾下改進(jìn)的ERTS平滑算法可以較好地穩(wěn)定系統(tǒng)的輸出，提高系統(tǒng)的跟蹤精度。

3 結(jié)束語(yǔ)

本文采用奇異值分解法對(duì)標(biāo)準(zhǔn)的ERTS平滑算法進(jìn)行化簡(jiǎn)改進(jìn)，以獲得移動(dòng)機(jī)器人位置和姿態(tài)的最優(yōu)估計(jì)值，實(shí)現(xiàn)精準(zhǔn)導(dǎo)航定位。

文中利用Turtlebot移動(dòng)機(jī)器人平臺(tái)采集到的位置和姿態(tài)信息進(jìn)行前向擴(kuò)展卡爾曼濾波器初步濾波后，通過(guò)奇異值分解法化簡(jiǎn)的RTS平滑算法獲得位置和姿態(tài)最優(yōu)估計(jì)。仿真結(jié)果和試驗(yàn)證明該算法比單獨(dú)的擴(kuò)展卡爾曼濾波器和標(biāo)準(zhǔn)ERTS平滑算法具有更好的精確性和抗干擾能力。

[1] 宮曉琳,張蓉,房建成.固定區(qū)間平滑算法及其在組合導(dǎo)航系統(tǒng)中的應(yīng)用[J].中國(guó)慣性技術(shù)學(xué)報(bào),2012,20(6):687-693.

[2] Jetto L,Longhi S.Development and experimental validation of an adaptive extended kalman filter for the localization of mobile robots[C]//IEEE Transactions and Automation on Robotics,2009：143-150.

[3] Zhu Jihua,Zheng Nanning,Yuan Zejian,et al.A SLAM algorithm based on the central difference kalman filter[C]//Intelligent Vehicles Symposium,IEEE，2009：123-128.

[4] Zhang Haitao,Rong Jian,Zhong Xiaochun.The performance comparison and algorithm analysis of first/second order EKF and smoother for GPS/DR navigation[C]//11th IEEE International Conference on Communication Technology,2008:432-437.

[5] Sarkka S,Viikari V,Huusko M,et al.Phase-based UHF RFID tracking with nonlinear kalman filtering and smoothing[J].IEEE Sensors Journal，2012，12(5):904-910.

[6] Nassar S.Improving the inertial navigation system(INS) error model for INS and INS/DGPS applications[D].Alberta:University of Calgary，2003.

[7] 秦永元.慣性導(dǎo)航[M].北京：科學(xué)出版社，2005：10-230.

[8] Lei Xuan,Yang Jing.Application of RTS optimal smoothing algorithm in satellite attitude determination[C]//2011 2nd International Conference on Intelligent Control and Information Proceedings,2011:978-982.

[9] Nassar S,Liu Hang,El-Sheimy N.Two-Filter smoothing for accurate ins-gps land-vehicle navigation in urban centers[J].IEEE Trans.on Vehicular Technology,2010,59(9):4256-4267.

[10]于翔川.基于非線(xiàn)性濾波的目標(biāo)跟蹤算法研究[D].西安：西安電子科技大學(xué)，2009.

[11]Razali S,Watanabe K,Maeyama S,et al.An unscented rauch-tung-striebelsmoother for a bearing only tracking problem[C]//2010 Int.Conf.on ICCAS,2010:1281-1286.

[12]Simo Sarkka.Unscented rauch-tung-striebelsmoother[J].IEEE Trans.on Automatic Control,2008,53(3):845-849.

Application Research on the Improved ERTS Smoothing Algorithm in Position Tracking

To overcome the disadvantages of standard ERTS smoothing algorithm in position and attitude estimation, e.g., complexity, low efficiency, and poor precision, etc., the new improved ERTS smoothing algorithm by adopting singularity valve decomposition is proposed for optimizing position and attitude data. After forward extended Karman filtering for the information of position and attitude collected in the system, the initial impact ofthe system noise is reduced; the singularity value decomposition is conducted for the MSE matrix after filtering, thus the backward recursion gain and the calculated amount of the predicted valueare decreased, and the prediction accuracy is improved; as well as the anti-interference and stability of the system are effectively strengthened. The experimental result on Turtlebot mobile robot platform verifies the high effectiveness and stability of this algorithm in position and attitude estimation.

Extended Kalman filter Singular value decomposition Optimal smoothing algorithm Optimal estimation Position tracking

薛永勝(1989-)，男，現(xiàn)為西南科技大學(xué)檢測(cè)技術(shù)與自動(dòng)化裝置專(zhuān)業(yè)在讀碩士研究生;主要研究方向?yàn)楣I(yè)智能控制、智能檢測(cè)技術(shù)與自動(dòng)化裝置。

TP202

A

10.16086/j.cnki.issn1000-0380.201504005

四川省科技廳科技支撐計(jì)劃項(xiàng)目(編號(hào)：2014RZ0049)；

2014四川省科技支撐計(jì)劃項(xiàng)目(編號(hào)：2014GZ0021)。

修改稿收到日期：2014-10-19。