基于迭代平方根CKF的SLAM算法

高 偉，張 亞，孫 騫，關 勁（.哈爾濱工程大學自動化學院，5000哈爾濱；.海軍裝備部，00036北京）

基于迭代平方根CKF的SLAM算法

高 偉1，張 亞1，孫 騫1，關 勁2
（1.哈爾濱工程大學自動化學院，150001哈爾濱；2.海軍裝備部，100036北京）

在大尺度環境中，平方根容積卡爾曼同步定位與地圖構建算法的非線性誤差嚴重制約了算法的定位精度，為解決這一問題，提出了一種基于迭代平方根容積卡爾曼濾波的改進算法，該算法結合迭代理論，對平方根容積卡爾曼濾波的量測更新過程進行迭代更新，充分利用最新的觀測信息，降低濾波的估計誤差，從而構建精確的地圖并獲得高精度的定位信息.仿真實驗結果表明，采用本算法后，x軸和y軸方向上的位置誤差均在1.5m以內，估計結果明顯優于SRCKFSLAM、CKF-SLAM和EKF-SLAM算法；添加不同的環境噪聲后進行仿真實驗，該算法所取得的位置誤差相比仍是最小的.利用該算法可以有效地減小非線性誤差造成的影響，提高SLAM的定位精度.

迭代理論；同步定位與地圖構建；非線性誤差；平方根容積卡爾曼濾波；迭代平方根容積卡爾曼濾波

同步定位與地圖構建（simultaneous localization and mapping，SLAM）是指運載體在完全未知的環境中，依靠自身攜帶的傳感器對周圍環境進行觀測和識別，并利用獲得的觀測信息自主創建環境地圖，同時利用該地圖對運載體進行定位與導航［1-4］.目前SLAM問題的解決方法主要以概率估計為主，其中應用最為廣泛的方法是基于擴展卡爾曼濾波（extended Kalman filter，EKF）的SLAM算法［5-7］.但是該算法在線性化過程中會引入較大的截斷誤差，并且計算量較大，不適用于復雜環境.針對上述不足，Julier等提出了基于無跡卡爾曼濾波（unscented Kalman filter，UKF）的SLAM算法，但當系統為高維時，UKF濾波過程中的協方差容易出現非正定情況，從而導致濾波數值不穩定且有可能發散［8-9］.為克服UKF-SLAM算法中存在的問題，很多學者開始研究基于容積卡爾曼濾波（cubature Kalman filter，CKF）和基于平方根CKF（square root cubature Kalman filter，SRCKF）的SLAM算法［1，10-11］.由于CKF是通過對一組具有相同權重的cubature點經過非線性系統方程轉換來預測下一時刻系統的狀態，從而避免了“維數災難”.與CKF不同，SRCKF算法在濾波過程中直接以協方差矩陣的平方根形式進行遞推更新，這樣不僅可以降低計算復雜度，獲得更高的計算效率，而且能夠保證了協方差矩陣的對稱性和非負定性，有效地避免濾波器的發散，提高濾波的收斂速度和數值穩定性［12-13］.但是在大尺度復雜環境中，觀測過程中大量路標特征的加入會使得系統初始誤差增大，量測方程高度非線性化，從而導致cubature點的非線性近似誤差逐漸增大，降低了SRCKF-SLAM算法的估計精度，最終使得狀態估計和地圖構建誤差增大［14-15］.為解決上述不足，本文在SRCKF算法的基礎上，結合迭代方法，提出一種基于迭代平方根容積卡爾曼濾波（iterated square root cubature Kalman filter，ISRCKF）的SLAM算法.該算法汲取了SRCKF濾波的優點并充分利用最新觀測信息，在大尺度復雜環境中可有效地降低系統狀態的估計誤差，提高地圖構建的準確性及導航精度.

1 同步定位與地圖構建

SLAM問題的本質是在整個路徑中對系統狀態的估計問題，系統狀態包括載體的狀態和觀測到的環境中路標特征的狀態［1］.設系統狀態量x和協方差矩陣P分別為

其中：xv為載體的狀態量，包括載體在x軸和y軸的位置以及載體的航向角；xm為環境中路標特征的狀態量，包括各路標特征在x軸和y軸的位置（xi，yi），其中i=1，2，…，l，l為路標的索引；Pvv為載體的協方差矩陣；Pmm為地圖的協方差矩陣；Pvm為載體與地圖的關聯協方差矩陣．

SLAM問題即可轉化為求取系統狀態向量的后驗概率密度問題，設k時刻系統狀態向量和觀測量分別為xk和zk，根據貝葉斯公式，狀態量的后驗概率密度為［2］

其中：uk為控制輸入，p（xv，k｜xv，k-1，uk）為運動模型，p（zk｜xv，k，xm）為量測模型．

2 基于SRCKF的SLAM算法

假設離散非線性系統為

其中：xk為k時刻系統的狀態向量，zk為k時刻系統狀態的量測值，f（·）和h（·）分別為非線性的狀態方程和量測方程，Wk-1～N（0，Q）和Vk～N（0，R）分別為系統噪聲和觀測噪聲．

對于系統（1），SRCKF選取2n（n為系統狀態向量的維數）個具有同等權值的容積點計算高斯權重分布，然后通過時間更新和量測更新即可得到SRCKF濾波算法，其中容積點集（ξi，wi）為

其中：n為系統狀態向量的維數.

基于SRCKF的SLAM算法步驟如下［1，14］.

時間更新．假設p（xk-1）=N（x＾k-1｜k-1，Pk-1｜k-1）已知，利用Cholesky分解協方差矩陣Pk-1｜k-1= S ST，計算cubature點X= k-1｜k-1k-1｜k-1i，k-1｜k-1+通過非線性狀態方程傳播=狀態預測=估計預測誤差協方差矩陣的平方根為

估計濾波增益與狀態估計值分別為

狀態誤差協方差矩陣平方根為

通過上述遞推，在給定條件后即可實現對SLAM的觀測更新，完成對系統狀態的估計，從而確定載體在環境中的位置并實現地圖的構建.但是在大尺度復雜環境中，隨著觀測的進行，大量路標特征的加入會使得系統的初始誤差較大，量測方程的非線性度越來越高，cubature點的非線性近似誤差也會逐漸增大，從而導致SRCKF-SLAM算法的濾波估計精度降低，甚至發散.

3 基于ISRCKF的SLAM算法

針對SRCKF-SLAM算法中存在的問題，本文利用迭代更新算法，提出了基于ISRCKF的SLAM算法.該算法的核心思想是：利用狀態量預測值和協方差平方根的預測值，將量測更新過程利用迭代更新進行多次迭代，充分利用最新的觀測信息，從而有效減小估計誤差，提高算法的估計精度.

基于ISRCKF的SLAM算法過程描述如下.時間更新.在k時刻，按照第2節基于SRCKF的SLAM算法計算系統狀態預測x＾k｜k-1和狀態誤差協方差矩陣平方根Sk｜k-1．

量測更新．量測更新是以x＾k｜k-1和Sk｜k-1為初始值的迭代過程．記第j次迭代的估計值和方差的平方根分別為x＾（j）和S（j），則量測更新的初始值為

對于j=0，1，2，…，Nmax（其中Nmax為最大迭代次數），計算新的cubature點

按照下式進行更新，可以得到j次迭代的狀態和方差矩陣的平方根矩陣為

4 算法的實驗驗證與分析

4.1 實驗條件

為了驗證本文所提SLAM算法的有效性和可行性，通過MATLAB仿真驗證.仿真環境采用悉尼大學學者Tim Bailey發布的開源SLAM仿真器中設置的實驗環境.該實驗環境在一個250 m× 200 m的室外環境區域中，其中包括人為設置的17個載體運行路徑點和135個靜止的路標特征，載體從（0，0）處開始逆時針沿路徑點確定的軌跡運行，具體的實驗環境如圖1所示.

圖1 仿真實驗環境示意

載體的運動模型為

其中：［xvx，k，xvy，k，xvφ，k］T為k時刻載體的位姿，xv，k+1為載體在k+1時刻的位姿，ΔT為采樣時間，vk+1和θk+1分別為k+1時刻載體的速度和航向角，WB為兩軸間的軸距，Wk+1為k+1時刻的系統誤差，且有W～N（0，Q）．

觀測模型為

其中：（xi，yi）為探測到的第i個路標特征的位置坐標，ri和αi分別為傳感器觀測到的第i個路標特征與載體之間的距離及其與載體前進方向的夾角，Vk為k時刻的量測誤差，且Vk～N（0，R）．

實驗參數設置如下：載體的初始狀態xv，0=［0，0，0］T，采樣時間ΔT=0.025 s，速度v= 3 m／s，速度誤差σv=0.25 m／s，航向角誤差σφ=2°，最大角速度20°／s；量測的采樣時間為0.2 s，最大觀測距離為30 m，距離誤差σr= 0.2 m，角度誤差σα=1°；最大迭代次數設為20次．系統噪聲Q和觀測噪聲R分別為

4.2 實驗結果與分析

在上述實驗環境下，分別對ISRCKF-SLAM、SRCKF-SLAM、CKF-SLAM和EKF-SLAM算法進行了20次獨立重復仿真實驗，并對實驗結果的均方根誤差（rootmean square error，RMSE）進行對比分析.

圖2為4種SLAM算法對路徑的估計結果對比圖，圖3為SRCKF-SLAM和ISRCKF-SLAM算法對路標的估計結果對比圖.其中圖2（b）、3（b）分別對應圖2（a）、3（a）中矩形區域的局部放大圖.

由圖2、3可知，在運載體導航定位估計方面，ISRCKF-SLAM算法與SRCKF-SLAM、CKFSLAM和EKF-SLAM算法相比，估計路徑和運載體真實路徑契合度高，即ISRCKF-SLAM算法估計的精度高于其他3種算法.

圖2 ISRCKF-SLAM和SRCKF-SLAM、CKFSLAM、EKF-SLAM算法估計路徑對比

圖3 ISRCKF-SLAM和SRCKF-SLAM算法估計路標對比

圖4 、5為ISRCKF-SLAM、SRCKF-SLAM、CKF-SLAM和EKF-SLAM算法對載體運行路徑x軸、y軸方向上的位置估計誤差對比曲線．

圖4 4種SLAM算法下x軸方向位置估計誤差對比

圖5 4種SLAM算法下y軸方向位置估計誤差對比

由圖4、5的實驗結果可知利用ISRCKFSLAM算法所得到的x軸和y軸方向位置估計誤差均小于1.5 m；SRCKF-SLAM算法和CKFSLAM算法精度略低于ISRCKF-SLAM算法，位置估計誤差均在4m以內；KF-SLAM算法的估計精度最差，位置估計精度在16m以內.因此SRCKFSLAM算法的估計精度優于其他3種算法，理論分析一致.由圖4可以看出，雖然SRCKF-SLAM算法在前6 000步之前的估計精度均高于ISRCKF-SLAM，但后半部分誤差迅速增大.這是由于隨著觀測的進行，大量路標特征的加入使得系統狀態初始誤差增大，并且系統高度非線性化，從而引起cubature點的非線性近似誤差逐漸增大，系統的非線性誤差越來越大，進而導致SRCKF-SLAM算法誤差增大.而本文所提出的ISRCKF-SLAM算法通過迭代充分利用了最新的觀測信息，有效降低了估計誤差，提高了算法的估計精度.綜上所述，在運載體運動的全過程中，ISRCKF-SLAM算法的定位估計精度高、誤差小，且數值穩定性好，從而驗證了ISRCKF-SLAM算法的有效性和優越性.

為了充分驗證ISRCKF-SLAM算法的穩定性，添加不同的環境噪聲重新進行仿真，其他參數不變.設觀測噪聲服從以下混合高斯分布，即

圖6為ISRCKF-SLAM算法對載體運行路徑x軸、y軸方向上的位置估計誤差曲線．

圖6 ISRCKF-SLAM算法下運載體位置估計誤差

從圖6可以看出，在添加不同環境噪聲后，ISRCKF-SLAM算法仍然能夠較好地估計出運載體的運動軌跡，可取得較高的定位精度.

5 結 論

1）提出了一種基于ISRCKF的SLAM算法，該算法將迭代理論與平方根CKF濾波方法相結合，充分利用最新的觀測信息，有效地解決了在復雜環境下量測信息增加導致cubature點的分布逐漸失真的問題，從而保證了濾波的數值穩定性，大大提高了系統狀態的估計精度.

2）在相同的仿真環境中，對不同的SLAM算法進行仿真實驗對比.仿真結果表明，所提出的ISRCKF-SLAM算法的估計誤差小于與其他幾種SLAM算法，通過仿真實驗驗證了本算法的穩定性.

［1］王宏健，傅桂霞，邊信黔，等.基于SRCKF的移動機器人同步定位與地圖構建［J］.機器人，2013，35（2）：200-207.

［2］袁贛南，王丹丹，魏延輝，等.水下石油管道漏油檢測定位的粒子濾波SLAM算法［J］.中國慣性技術學報，2013，21（2）：204-208.

［3］杜航原，郝燕玲，高忠強，等.基于魯棒非線性卡爾曼濾波的自適應SLAM算法［J］.宇航學報，2012，33（5）：620-627.

［4］張國良，湯文俊，敬斌，等.輔助粒子濾波算法改進的UFastSLAM算法［J］.哈爾濱工業大學學報，2012，44（11）：123-128.

［5］WILLIAMS B，CUMMINS M，NEIRA J，et al.A comparison of loop closing techniques in monocular SLAM［J］.IEEE Transactions on Robotics and Autonomous Systems，2009，57（12）：1188-1197.

［6］王宏健，王晶，邊信黔，等.基于組合EKF的自主水下航行器SLAM［J］.機器人，2012，34（1）：56-64.

［7］田翔，張亮，陳耀武.基于中心差分卡爾曼濾波器的快速SLAM算法［J］.哈爾濱工業大學學報，2010，42（9）：1454-1461.

［8］JULIER S，UHLMANN J，DURRANTF.A newmethod for the nonlinear transformation of means and covariances in filters and estimators［J］.IEEE Transactions on Automatic Control，2000，45（3）：477-482.

［9］YAN Xuejun，ZHAO Chunxia，XIAO Jizhong.A novel FastSLAM algorithm based on iterated unscented Kalman filter［C］／／Proceeding of IEEE International Conference on Robotics and Biomimetics.Phuket：IEEE，2011：1906-1911.

［10］PAKKI K，CHANDRA B，GU D，et al.Cubature Kalman filter based localization andmapping［C］／／18th IFAC World Congress.Laxenburg，Australia：IFAC，2011：2121-2125.

［11］ARASARATNAM I，HAYKIN S.Cubature Kalman filters［J］.IEEE Transactions on Automatic Control，2009，54（6）：1254-1269.

［12］GAOWei，ZHANG Ya，WANG Jianguo.A strapdown interial navigation system／Beidou／Doppler velocity log integrated navigation algorithm based on a cubature Kalman filter［J］.Sensors，2014，14（1）：1511-1527.

［13］YU Fei，SUN Qian，LüChongyang，et al.A SLAM algorithm based on adaptive cubature Kalman filter［J］. Mathematical Problems in Engineering，2014，2014：1-11.

［14］郝燕玲，楊峻巍，陳亮，等.基于SRCKF的水下航行器動基座初始對準技術［J］.華中科技大學學報：自然科學版，2012，40（2）：123-127.

［15］SMIDL V，PEROUTKA Z.Advantages of square-root extended Kalman filter for sensorless control of AC drives［J］.IEEE Transactions on Industrial Electronics，2012，59（11）：4189-4196.

（編輯 魏希柱）

Simultaneous localization and mapping based on iterated square root cubature Kalman filter

GAOWei1，ZHANG Ya1，SUN Qian1，GUAN Jin2
（1.College of Automation，Harbin Engineering University，150001Harbin，China；2.Navy Equipment Department，100036 Beijing，China）

In large-scale conditions，the large nonlinear error of simultaneous localization and mapping（SLAM）based on square root cubature Kalman filter（SRCKF）is a serious constraint to high positional accuracy.To solve this problem，an improved SLAM algorithm based on iterated square root cubature Kalman filter（ISRCKF）is proposed.Utilizing the iteration theory，the newest observation information is in full use. Thus the estimation errors of the new algorithm will be decreased noticeably，an accurate environmentmap will be established and high-precision localization will be obtained as well.The simulation results show that the location errors of x axis and y axis are both less than 1.5 m by the new algorithm.The estimating accuracy of the new algorithm is higher than that of SRCKF-SLAM，CKF-SLAM and EKF-SLAM algorithms.Adding different environmental noises，the position errors of ISRCKF are the smallest.

iterated theory；SLAM；nonlinear error；SRCKF；ISRCKF

U666.1

A

0367-6234（2014）12-0120-05

2013-09-13.

國家自然科學基金（51179039）；武器裝備預研基金（9140A09040211CB0102）.

高 偉（1977—），男，教授，博士生導師.

張 亞，yzhang＠hrbeu.edu.cn.