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

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

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

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

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

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

1 同步定位與地圖構建

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

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

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

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

2 基于SRCKF的SLAM算法

假設離散非線性系統(tǒng)為

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

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

其中：n為系統(tǒng)狀態(tài)向量的維數(shù).

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

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

估計濾波增益與狀態(tài)估計值分別為

狀態(tài)誤差協(xié)方差矩陣平方根為

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

3 基于ISRCKF的SLAM算法

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

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

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

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

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

4 算法的實驗驗證與分析

4.1 實驗條件

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

圖1 仿真實驗環(huán)境示意

載體的運動模型為

其中：［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時刻的系統(tǒng)誤差，且有W～N（0，Q）．

觀測模型為

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

實驗參數(shù)設置如下：載體的初始狀態(tài)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°；最大迭代次數(shù)設為20次．系統(tǒng)噪聲Q和觀測噪聲R分別為

4.2 實驗結果與分析

在上述實驗環(huán)境下，分別對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）中矩形區(qū)域的局部放大圖.

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

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

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

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

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

5 結 論

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

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

［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］.哈爾濱工業(yè)大學學報，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］.哈爾濱工業(yè)大學學報，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.