薛永勝 張 華 霍建文

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

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

薛永勝 張 華 霍建文

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

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

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

0 引言

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

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

1 ERTS平滑算法

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

圖1 系統(tǒng)框圖

1.1 前向EKF濾波器

擴展卡爾曼濾波器于20世紀60年代提出，是一種歷史悠久、應(yīng)用廣泛的非線性高斯次優(yōu)濾波算法。該算法在卡爾曼濾波算法的基本框架上，通過對非線性函數(shù)的泰勒展開式進行一階線性化截斷，并以此近似非線性狀態(tài)后驗均值和協(xié)方差[2]。常用的非線性離散隨機系統(tǒng)模型如下。

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

(1)

(2)

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

(3)

(4)

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

(5)

(6)

當K=1，2，…時，通過迭代運算估計各時刻目標的狀態(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)

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

1.2RTS平滑算法

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

圖2 RTS平滑算法原理圖

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

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

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

(14)

(15)

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

(16)

1.3 算法改進

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

(17)

(18)

定義Mk滿足：

(19)

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

(20)

為進行奇異值分解運算，將Mk求逆得：

(21)

(22)

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

(23)

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

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

(24)

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

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

(25)

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

(26)

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

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

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

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

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

圖3 Turtlebot機器人簡化模型

(27)

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

(28)

式中:x、y為當前移動機器人本體在的坐標；xi、yi為上一次移動機器人本體的坐標。

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

2.1 結(jié)果比較

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

圖4 不同算法跟蹤比較

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

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

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

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

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

表3 Turtlebot機器人平臺算法驗證結(jié)果

2.2 抗干擾分析

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

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

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

3 結(jié)束語

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

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

[1] 宮曉琳,張蓉,房建成.固定區(qū)間平滑算法及其在組合導(dǎo)航系統(tǒng)中的應(yīng)用[J].中國慣性技術(shù)學(xué)報,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]于翔川.基于非線性濾波的目標跟蹤算法研究[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é)檢測技術(shù)與自動化裝置專業(yè)在讀碩士研究生;主要研究方向為工業(yè)智能控制、智能檢測技術(shù)與自動化裝置。

TP202

A

10.16086/j.cnki.issn1000-0380.201504005

四川省科技廳科技支撐計劃項目(編號：2014RZ0049)；

2014四川省科技支撐計劃項目(編號：2014GZ0021)。

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