基于噪聲縮放的自適應UKF-SLAM算法

王祖麟,秦 菘,梁毓明

(江西理工大學電氣工程與自動化學院,江西贛州341000)

基于噪聲縮放的自適應UKF-SLAM算法

王祖麟,秦 菘,梁毓明

(江西理工大學電氣工程與自動化學院,江西贛州341000)

針對擴展卡爾曼濾波(EKF)算法在移動機器人同時定位和環境建模(SLAM)中的缺點,即非線性系統簡單線性化所導致的系統狀態方程的不準確性、雅克比矩陣的計算所導致的計算復雜化以及噪聲模型不確定性所導致的濾波穩定性降低等問題,提出一種對噪聲自適應的UKF-SLAM算法。該算法通過對噪聲縮放進而改變噪聲模型,利用觀測殘差序列準確估計觀測噪聲模型協方差,運用預測的新息協方差和IAE開窗法求其系統狀態噪聲縮放因子,從而準確估計系統狀態噪聲模型協方差,實現對不確定的噪聲模型能夠自適應UKF-SLAM算法。UKF的Sigma點采樣策略是比例對稱采樣。實驗結果證明,該方法相對EKF算法和UKF算法具有較高的定位精度和自適應能力。

同時定位和環境建模;無跡卡爾曼濾波;噪聲縮放;在線自適應;比例對稱采樣;開窗法

1 概述

實現智能移動機器人的關鍵技術之一就是移動機器人同時定位和環境建模(Simultaneous Location and Mapping,SLAM),即一方面機器人通過自身傳感器實現對周圍環境模型的建立,另一方面機器人依靠所建立的環境模型估計自身所在環境中的位置[1]。SLAM可以理解為機器人同時估計其位置和環境模型特征的估計問題。

目前最經典的算法就是擴展卡爾曼濾波(Extend Kalman Filtering,EKF)[2],該算法是通過把機器人的系統模型進行泰勒展開,將其高階忽略,把非線性系統變為線性系統,但是當其高階項無法忽略時,該系統就會出現較大誤差,并且濾波器很難穩定,最終導致濾波器無法收斂,使得所建立的環境模型精度較低[3]。此外,EKF算法必須獲取系統噪聲和觀測噪聲的先驗估計特性,在實際中是很難實現的,況且如果這些先驗數據不準確或者信息量不足,則會造成濾波器估計精度降低。

針對以上EKF問題,人們提出了很多算法解決這個問題。文獻[4]提出無跡卡爾曼濾波器(UKF)濾波器,該算法是一種用采樣策略將非線性逼近線性的辦法。UKF[5]是以UT(Unscented Transformation)變換為基礎,采用KF線性濾波框架,其采樣形式為確定性采樣,不存在PF的粒子退化問題,對非線性分布統計量的計算精度至少為2階。但是UT變換采樣點到中心點的距離會隨著系統位數的增加而越來越遠,進而出現非局部效應。文獻[6]提出了比例對稱采樣可以解決采樣非局部效應。文獻[7]提出了基于平方根容積卡爾曼濾波的SLAM算法。文獻[8]提出了基于線段特征匹配的EKF-SLAM算法。

在實際過程中,UKF也會存在噪聲模型不準確對濾波器精度和穩定性的影響,并且主要是濾波增益和噪聲模型不能在線調節,不具備在線自適應能力。考慮了以上UKF算法的缺點,文獻[9-10]提出了將UKF和粒子濾波器相結合的算法,文獻[11]提出了通過調節UKF參數的改進辦法。本文提出基于噪聲縮放的UKF自適應SLAM算法,該算法利用機器人觀測殘差序列的均值和協方差改變觀測噪聲協方差值;并運用IAE開窗法和新息預測值得到系統狀態噪聲協方差縮放因子,進而求取系統噪聲方差。

2 問題描述

SLAM的基本含義是機器人定位和環境建模同時進行,一方面機器人利用已經創建好的環境模型矯正機器人的位置狀態,從而提高機器人的定位精度;另一方面機器人依靠可靠的機器人位置,建立出精度更高的環境模型。其問題可以描述為:當機器人進入一個未知環境中,k時刻的機器人位置狀態為s(k),k時刻的地圖特征為m(k),故機器人的狀態向量表示為X(k)=(s(k),m(k))T,u(k)表示時刻k-1到k時刻的運動控制輸入,Z(k)表示為k時刻的系統觀測向量。SLAM其本質可以理解為在貝葉斯框架下的機器人定位和環境模型的預估問題, SLAM用概率描述可以表示為:

根據貝葉斯定理可知:

其中,η為歸一化常數;p(Z(k)|X(k))為機器人觀測模型;p(X(k)|u(k),Z(k))為機器人運動模型,也就是根據觀測模型的概率和運動模型的概率分布遞歸的求其機器人的位置狀態和環境地圖模型。

針對第1節中所提到的EKF-SALM算法的不足和缺點,文獻[12]提出了UKF算法,其算法的核心是采用SUT變換和確定性采樣策略逼近非線性系統。基本的原理是在系統狀態分布中選取一定量的采樣點,使這些采樣點的期望值和協方差等于系統狀態分布的期望值和協方差;然后將這些點帶入到非線性系統函數中,就得到了相應的非線性函數點集,最后通過得到的點集求取變換后的期望值和協方差。

根據以上關于SLAM問題的描述,設機器人的狀態方程和觀測方程分別為:

其中,系統狀態噪聲均值為w(k);協方差為Q(k);系統的觀測噪聲均值為v(k);協方差為R(k)。

在利用UT變換算法時,首要任務是確定Sigma點的采樣策略。目前提出的采樣策略有單行采樣,對稱采樣等[13]。為了解決非局部效應,本文采用的是比例對稱采樣[6-7],利用UKF算法求其狀態向量的均值和協方差Px,假設X(k)為n維向量,那么選取采樣個數為L=2n+1按照比例對稱策略進行Sigma點近似,其遵循如下的條件函數來確保能夠準確地抓取Xi的必要特征。

遵循上述條件函數,求得比例修正的對稱采樣Sigma點χi′為:

那么X(k)對應的均值和協方差矩陣可近似的表示為:

以上就是UKF算法對均值和協方差的非線性變換計算,根據算法可以發現相對于EKF算法有至少存在2階的估計精度,不需要計算雅克比矩陣,算法比EKF更容易實現;而相對于粒子濾波器,采用的粒子點是較少的,計算量更小。

3 UKF-SLAM算法

系統狀態噪聲和觀測噪聲的協方差對于UKF算法在SLAM提高濾波精度,定位精度和地圖建模精度尤為重要。因為Q(k)和R(k)直接影響著當前時刻狀態信息和上一時刻狀態信息之間的權值。而確定R(k)和Q(k)的一般是依靠系統狀態噪聲和觀測噪聲的先驗知識。根據前面介紹的UKF算法,如果均值X(0)和協方差P(0)取值存在誤差,那么UKF算法的第2步Sigma點測采用就會受到影響,進而影響算法的預測部分;而且經系統的觀測方程,使得觀測量Z(k)會存在一定的誤差。

經過以上分析可以看出如果R(k)和Q(k)的不確定性將對整個算法的濾波精度產生較大影響,進而影響SLAM的定位精度和地圖建模精度。

針對在SLAM中UKF算法出現的系統狀態噪聲和觀測噪聲模型不確定性,所導致的UKF算法收斂速度變慢,估計精度降低,甚至出現濾波發散等問題,本文提出了一種在線自適應調整噪聲模型并同時控制系統噪聲模型異常干擾源對濾波效果產生影響的算法。

3.1 噪聲自適應的UKF算法

根據UKF算法和機器人的狀態方程和觀測方程,首先定義觀測新息值矩陣d(k)和觀測殘差值矩陣ε(k):

其中,d(k)是由k時刻的預測狀態值和輸入值所推算出來的;而ε(k)是由k時刻的更新狀態值和輸入值推算出來的。可以看出d(k)僅僅使用了預測值來求,所以帶有噪聲不確定性帶來的誤差,可以反映出系統所受到的擾動。

根據觀測噪聲模型和UKF算法,對ε(k)進行處理,得到其均值和協方差如下所示:

其中,P(k)為系統方程的協方差矩陣的更新值,根據UKF算法可以求出,進而可以估計出觀測噪聲協方差矩陣R(k)。

基于觀測協方差的自適應算法一般采用的是開窗法,有IAE濾波和RAE濾波2種[14],IAE濾波使用m個歷史新息值來推算當前新息的協方差矩陣估計值(k):

一個最優的濾波器應該滿足,通過上面開窗法IAE預測的k時刻的協方差值(k)和UKF算法所預測的P(k|k-1)基本保持一致。即:

由于觀測模型噪聲矩陣R(k)不確定性帶來的誤差,將會導致(k)和預測的系統狀態協方差P(k|k-1)并不相等。即:

式(16)和式(17)可以用來判斷系統噪聲模型是否發生改變,分以下2種情況討論觀測模型是否發生變化。

(1)如果系統的狀態噪聲模型沒有發生改變,式(13)是成立的。

根據以上分析,只要知道UKF算法k時刻預測狀態協方差值P(k|k-1)和利用開窗法LAE所預測的k時刻的協方差值(k),那么就可以根據式(13)和式(14)判斷系統狀態噪聲模型有沒有發生變化,進而可以知道系統狀態噪聲模型的具體變化情況。

定義縮放因子β(k)為:

根據式(16),系統狀態噪聲的協方差Q(k)可以表示為:

縮放因子β(k)取平方根是為了噪聲更加平滑,使得整個系統噪聲模型更加平滑,其可以取大于1也可以取小于1的數值,使得更加充分的調節噪聲模型。

3.2 噪聲自適應的UKF-SLAM算法

根據UKF算法原理和噪聲自適應的UKF算法原理,從而實現噪聲自適應的UKF-SLAM算法,具體步驟如下:

(1)系統初始化,初始化k=0時刻的均值、協方差。

(2)對于i=1,2,…,2n,進行Sigma點采樣。采用策略是比例對稱采樣。可參考式(4)到式(8);

(4)首先按式(11)確定系統觀測噪聲協方差R(k),然后根據觀測模型對第(3)步的預測部分進行更新,設Pzz為觀測模型的協方差,Pxz為系統狀態模型和觀測模型之間的協方差,K(k)為濾波增益。

其更新方程如下:

4 系統建模與實驗分析

4.1 系統建模

系統的模型主要包括機器人坐標中的模型、機器人運動模型、機器人控制模型、傳感器觀測模型、系統噪聲模型,本實驗中采用的模型來自于悉尼大學的經典數據庫[15],其中機器人坐標中的模型如圖1所示。

圖1 機器人坐標中的模型

機器人運動模型如下:

機器人觀測模型如下:

那么k時刻m個特征觀測量為機器人的觀測向量Z(k),如下所示:

4.2 實驗環境

實驗是在PC機上用MATLAB編寫軟件仿真進行的,實驗所構造的環境區域為40×40的地圖。機器人從起始狀態S(0)=(0.005,0.005,0.001)開始步進,其仿真參數如表1所示。

表1 仿真參數

記NAUKF-SLAM為噪聲自適應UKF-SLAM算法。對于UKF-SLAM算法和NAUKF-SLAM算法,用到的參數設定為:α取1,β取2,κ取3-n。

4.3 實驗結果分析

在以上的實驗環境,設定系統狀態噪聲δd(k)為2 cm,δθ(k)為0.5°,觀測噪聲δd(k)為0.1 m,δθ(k)為 1°。對 EKF-SLAM、UKF-SLAM 和 NAUKFSLAM算法分別進行仿真實驗,并對以上實驗結果進行分析和比較。

3種算法分別在系統狀態噪聲和觀測噪聲在服從上述噪聲分布的情況下,機器人步進2 000次建立的路徑估計曲線和地圖特征估計點,其仿真效果分別如圖2～圖4所示。該仿真圖是在同一地圖中進行的,機器人真實路徑和地圖路標是相同的,其中實線為機器人實際行走的軌跡;虛線為算法估計的路徑;+表示算法估計的路標;◇表示實際路標的位置。

圖2 EKF-SLAM算法仿真結果

圖3 UKF-SLAM算法仿真結果

圖4 NAUKF-SLAM算法仿真結果

從效果圖可以看出UKF-SLAM算法對機器人位姿的估計和對地圖的建模比EKF更加準確,因為UKF-SLAM算法至少可以達到2階的定位精度和環境建模精度;而NAUKF-SLAM算法對機器人路徑估計基本和實際的運動路線相吻合,并且從地圖特征的可信區域可以看出NAUKF-SLAM對地圖特征的估計和實際的地圖特征基本重合,實現了對機器人位姿的精確估計和環境模型的精確建模,因為在NAUKF-SLAM算法中所建立的噪聲自適應模型可以實時的調節其噪聲值。此外,由于提供的噪聲是服從高斯分布的白噪聲,所以3種算法最后都是收斂的,精度都逐漸提高。

為了驗證NAUKF-SLAM算法對噪聲的自適應性,3種算法在同一個地圖中進行實驗:地圖大小為40 m×40 m,機器人運行時間為500 s,特征點數為40個。為了更好的比較3種算法性能,求取算法的估計值和真實值之間的誤差。算法的估計值包括對機器人位置的估計、機器人姿態的估計以及對環境特征點的估計。每種算法都是在同一地圖上進行N次實驗,采樣歸一化誤差來表示其估計值和真實值之間的關系,如對機器人位置的估計誤差關系為:E(S(k),S*(k)),S(k)表示機器人實際的路徑,S*(k)表示算法估計的機器人路徑,其公式如下:

其中,N=50。

首先在上述的噪聲不變的情況下,添加(0,1)正態分布的環境噪聲,對3種算法進行實驗,其3種誤差分別對應圖5～圖7。雖然添加了高斯噪聲,造成了對系統噪聲模型和觀測模型的影響,但是NAUKF-SLAM的定位精度和環境特征點估計精度都較好,體現出其對噪聲的自適應性。

圖5 高斯分布噪聲下機器人位置平均估計誤差

圖6 高斯分布噪聲下機器人姿態平均估計誤差

圖7 高斯分布噪聲下特征點平均估計誤差

為了進一步驗證NAUKF-SLAM對噪聲的自適應性,取2種不同的噪聲對其實驗。

添加(0,1)均勻分布的環境噪聲,然后對3種算法進行實驗,其3種誤差如圖8～圖10所示。從圖中可以看出3種算法的誤差都有所增加,但是NAUKF-SLAM算法誤差曲線基本在其他2種算法曲線下,其定位精度和地圖建模精度更高。

圖8 均勻分布噪聲下機器人位置平均估計誤差

圖9 均勻分布噪聲下機器人姿態平均估計誤差

添加參數為0.01的指數分布的環境噪聲,其3種誤差分別對應圖11～圖13。雖然加入了指數噪聲,3種算法的誤差也都有所增加,但是NAUKFSLAM誤差曲線基本在其他2種算法誤差曲線以下,其定位精度和地圖建模精度上更高。

圖10 均勻分布噪聲下特征點平均估計誤差

圖11 指數分布噪聲下機器人位置平均估計誤差

圖12 指數分布噪聲下機器人位姿平均估計誤差

圖13 指數分布噪聲下特征點平均估計誤差

綜合以上的實驗分析,可以看出隨著噪聲的增大或者減小,UKF-SLAM算法其誤差比EKF-SLAM算法較小,這是因為EKF-SLAM算法僅僅保證了1階的精度,而UKF-SLAM至少有2階精度的存在。NAUKF-SLAM算法通過實時計算觀測殘差值的均值和協方差改變觀測噪聲協方差R(k),利用開窗法和新息預測值實時計算出狀態噪聲縮放因子β(k)進而改變系統狀態噪聲協方差Q(k),以實現更加精確的機器人定位和地圖建模。

5 結束語

針對EKF算法在SLAM中由于噪聲模型的改變所導致的精度降低甚至發散的問題,本文提出了一種通過改變噪聲模型的自適應的UKF-SLAM算法(NAUKF-SLAM),通過實時觀測新息的均值和協方差來自適應觀測噪聲模型R(k),以及通過IAE開窗法和新息協方差預測值計算出縮放因子β(k),進而自適應系統狀態噪聲模型Q(k),以達到對系統噪聲模型的改變,或者有其他環境噪聲加入時,能夠自適應地調節其噪聲模型,進而提高機器人SLAM精度。實驗結果表明,本文算法SLAM精度優于EKFSLAM和UKF-SLAM算法,一定程度上滿足了SLAM的精度要求。但是該算法對系統噪聲Q(k)是全局縮放的,其縮放因子β(k)沒有分配到每個單獨的狀態分量,可能對其精度有一定的影響,因此,下一步將在本文算法基礎上,對UKF和粒子濾波器結合同時定位和環境建模進行研究。

[1] Durrant-Whyte H,Bailey T.Simultaneous Localization and Mapping:Part I[J].IEEE Robotics and Automation Magazine,2006,13(2):99-108.

[2] Dissanayake M W M G,Newman P,Clark S,et al.A Solution to the Simultaneous Localization and Mapbuilding(SLAM)Problem[J].Transactions of Robotics and Automation,2001,17(3):229-241.

[3] Singh S.Noise Impact on Time-series Forecasting Using an Intelligent Pattern Matching Technique[J].Pattern Recognition,1999,32(8):1389-1398.

[4] Andrade-Cetto J,Vidal-Calleja T,Sanfeliu A.Unscented Transfor mation ofVehicleStatesin Slam[C]// Proceedings of IEEE International Conference on Robotics and Automation.Barcelona,Spain:[s.n.],2005:324-329.

[5] Juliter S J.The SphericalSimplex Unscented Transformation[C]//Proceedings of American Control Conference.Denver,USA:[s.n.],2003:2430-2434.

[6] Wan E A,vander Merwe R.The Unscented Kalman Filter, in Kalman Filtering and Neural Networks[EB/OL].(2004-03-10).http://www.cse.ogi.edu/PacSoft/projects/sec/ wan01b.ps.

[7] 康軼非.平方根容積卡爾曼濾波在移動機器人SLAM中的應用[J].機器人,2013,35(2):186-193.

[8] 張國良,湯文俊.基于線段特征匹配的EKF-SLAM算法[J].控制工程,2012,10(6):119-124.

[9] Wang X,Zhang H.A UPF-UKF Framework for SLAM [C]//Proceedings of IEEE International Conference on Roboticsand Automation.Piscataway,USA:IEEE, 2007:1664-1669.

[10] Kim C,SakthivelR,ChungW K.Unscented Fast SLAM:A Robust and Efficient Solution to the SLAM Problem[J].IEEE Transactions on Robotics,2008,24 (4):808-820.

[11] Shojaie K,ShahriA M.Iterated Unscented SLAM Algorithm for Nacigation of an Autonomous Mobile Robot[C]//Proceedings of IEEE/RSJ Internation Conference on Intelligent Robots and Systems.Piscataway, USA:IEEE Press,2008:1582-1587.

[12] Julier S J,Uhlmann J K.Unscented Filtering and Nonlinear Estimation[J].Proceedings of the IEEE, 2004,92(3):401-422.

[13] Julier S J,Uhlmann J K.A Consistant,Debiased Method for Converting Between Polar and Cartesian Coordinate Systems[C]//Proceedings of the 11th International Symposium on Aerospace Defense Sensing,Simulation and Controls.Orlando,USA:Press,1997:110-121.

[14] Mohamed A H,Schwarz K P.Adaptive Kalman Filtering for INS/GPS[J].Journal of Geodesy,1999,73(4): 193-203.

[15] Australian Centre for Field Robotics.Source Code [EB/OL].(2008-06-10).http://www-personal.acfr. usyd.edu.au/tbailey/.

編輯 索書志

Adaptive UKF-SLAM Algorithm Based on Noise Scaling

WANG Zu-lin,QIN Song,LIANG Yu-ming
(Faculty of Electronic Engineering and Automation,Jiangxi University of Science and Technology,Ganzhou 341000,China)

For Extend Kalman Filtering(EKF)algorithm disadvantage on the Simultaneous Location and Mapping

(SLAM),that is simple linearization of nonlinear systems resulting from the inaccuracy of the system state equation,Jacobi matrix calculation resulting from computational complexity,and noise uncertainty caused by the filtering reduced stability and other issues,this paper proposes a noise adaptive UKF-SLAM algorithm.In order to achieve adaptive UKF-SLAM algorithm,the paper scales the noise to change the noise model.Using the observed innovation sequence to accurately estimate the covariance of the measurement noise model.And using a new message convariance and IAE fenestration to find the system noise scaling factor,and thus accurately setimates the convariance of the system state noise model,it achieves a adaptive UKF-SLAM algorithm.The sampling strategy of UKF Sigma points is scaling symmetric sampling.Experimental results show that the algorithm has a high accuracy on SLAM compared with the EKF-SLAM and UKF-SLAM.

Simultaneous Location and Mapping(SLAM);Unscented Kalman Filtering(UKF);noise scaling;online adaptive;scale symmetric sampling;windowing method

1000-3428(2014)10-0143-07

A

TP18

10.3969/j.issn.1000-3428.2014.10.028

國家自然科學基金資助項目(61262013);江西省教育廳科技計劃基金資助項目(GJ11133)。

王祖麟(1954-),男,教授,主研方向:智能機器人,嵌入式系統;秦 菘(通訊作者),碩士;梁毓明,講師、博士。

2013-10-16

2013-12-03E-mail:chinaqinsong@sina.cn

中文引用格式:王祖麟,秦 菘,梁毓明.基于噪聲縮放的自適應UKF-SLAM算法[J].計算機工程,2014,40(10): 143-149,154.

英文引用格式:Wang Zulin,Qin Song,Liang Yuming.Adaptive UKF-SLAM Algorithm Based on Noise Scaling[J]. Computer Engineering,2014,40(10):143-149,154.