基于中心差分卡爾曼濾波器的快速SLAM算法

田 翔，張 亮，陳耀武

(浙江大學數字技術及儀器研究所，浙江杭州310027，cyw@mail.bme.zju.edu.cn)

移動機器人同時定位與地圖創建 SLAM (Simultaneous Localization and Mapping)問題［1－2］是指在未知環境中移動機器人利用自身定位信息創建環境地圖，同時利用已創建的環境地圖進行自身定位.經過十多年的研究和發展已經產生了許多SLAM算法，典型的包括基于擴展卡爾曼濾波的SLAM算法(EKF-SLAM)，基于粒子濾波器的SLAM算法(FASTSLAM1、FASTSLAM2)，基于稀疏信息矩陣濾波器的SLAM算法(EIF-SLAM)等［3－8］.由于在EKF-SLAM中狀態向量和狀態向量協方差都需要維護所有已檢測的路標，因此在路標密集環境下SLAM速度很慢，從而限制其在實際中的應用.另外由于機器人SLAM中的預測過程和測量過程都是非線性模型，所以在EKFSLAM算法中使用了一級泰勒展開中對預測過程和測量過程進行線性近似，但是當非線性方程的非線性嚴重時會導致EKF-SLAM算法的一致性出現問題［9］.在FASTSLAM中SLAM的速度和一致性受粒子數目的影響，粒子數目過小可能導致SLAM結果發散，粒子數目過多會導致SLAM速度太慢從而限制其在實際中的應用.

本文提出基于中心差分卡爾曼濾波器的快速SLAM算法對預測過程和測量過程的線性近似精度上進行了改進，使用Stirling插值方法可以對非線性過程近似到二級泰勒展開，從而提高SLAM的精確性，并且使用已檢測過的路標統計信息對由已檢測路標和機器人位置組成的狀態向量和狀態協方差矩陣進行動態調整，以保證它們的大小限制在固定范圍內，從而提高了SLAM速度.本文算法同EKF-SLAM和FASTSLAM進行分析發現在內存占用上要優于EKF-SLAM和FASTSLAM.通過對在稀疏路標環境和密集路標環境下的SLAM結果進行比較表明在SLAM精確性和一致性上都優于FASTSLAM2.0算法.

1 中心差分卡爾曼濾波器SLAM

中心差分卡爾曼濾波器是一種對非線性過程使用Stirling多項式插值公式近似，同時將其和卡爾曼濾波器結合所組成的濾波器.基于此濾波器對移動機器人進行SLAM稱為CDKFSLAM.

1.1 Stirling多項式插值

式中:u和δ分別為操作符.

對于式(1)如果只考慮到二級近似而忽略之后的所有高階項則可以簡化為

式(2)中操作符u和δ的定義為

對式(2)中的操作符使用式(3)進行替換可得

對比式(5)和對f(x)在ˉx處的泰勒展開可以看出使用Stirling插值公式對非線性方程f的近似可以達到二級泰勒展開，同時選取合適的參數h還可以近似到更高級別的泰勒展開.

當表達式(1)中的輸入隨機變量x為多維變量時，式(2)可以重新被表示為

式(7)中操作符up和δp為

式中:ep為第p項元素為1其余項元素為0的N維向量.

式(6)中輸入隨機變量x經過非線性過程f后輸出的均值為

式中:sp為輸入變量x的協方差Cholesky系數的第p項.

式(6)中輸入隨機變量x經過非線性過程f后輸出的協方差以及輸出和輸入x之間的互協方差分別為

綜合考慮式(9)、式(11)以及式(12)可以看出對非線性方程使用Stirling插值公式的二級展開近似后，非線性方程的輸出均值、協方差以及輸出和輸入的互協方差完全由2N+1個輸入點經過非線性方程f后的輸出決定.這些輸入點統稱為Sigma點［12－13］.結合式(9)、式(11)和式(12)同卡爾曼濾波器組成中心差分卡爾曼濾波器，并使用此濾波器對SLAM中的狀態預測方程，測量方程進行估計.

1.2 SLAM狀態向量預測

在CDKFSLAM過程中，狀態向量和狀態向量協方差分別為

狀態向量預測是指根據機器人當前狀態向量SV，同時參考機器人速度和以及噪聲參數預測機器人下一時刻的位置.狀態向量預測方程為

式中:V為機器人的運動速度，Δt為時刻k和時刻k+1的時間間隔，Gn為方位角噪聲，WB為機器人的輪間距.

由式(15)可知為了根據k時刻的狀態向量預測k+1時刻的狀態向量需要使用k時刻的狀態向量SVk，機器人速度參數V，機器人方位角噪聲參數，時間間隔以及機器人輪間距.由于機器人速度和方位角參數受噪聲影響，因此在使用Stirling多項式插值方法對式(15)進行非線性近似時對輸入Sigma點的構造需要考慮機器人速度和角速度噪聲.使用式(9)和式(11)對狀態向量的均值和協方差預測時的輸入Sigma點來進行選擇

式中:V為機器人移動速度參數，Vn為機器人移動速度噪聲均值，Gn為機器人方位角噪聲均值，VV為機器人速度噪聲協方差，GV為方位角噪聲協方差.由于機器人移動速度參數和機器人方位角噪聲參數的維數都為1，所以式(18)中SN=3+ 2n+1+1，輸入Sigma點的總數為2SN+1，在這里機器人移動速度噪聲和方位角噪聲都近似認為符合高斯分布，因此參數h取值為

將式(18)中各個輸入向量代入式(15)可以獲得各個輸入向量經過狀態向量預測方程的輸出，同時結合式(9)和式(11)獲得預測狀態向量和其協方差

1.3 SLAM狀態向量更新

在機器人運動過程中接收到傳感器測量數據時先根據Stirling插值方式對測量方程的輸出進行估計，再使用卡爾曼濾波器對狀態向量更新，測量方程為

同樣在使用Stirling插值方式對測量方程輸出進行估計時需要考慮到測量過程的距離測量噪聲和方位角測量噪聲，因此此時輸入Sigma點的選擇為

式中:Dn為距離測量噪聲的均值，An為方位角測量噪聲的均值，DV為距離測量噪聲的協方差，AV為方位角測量噪聲的協方差;由于DV和AV的維數都為1，所以式(22)中的SM=3+2n+1+1，輸入Sigma點總數為2SM+1，參數h取值為

在測量數據過程中，由于距離測量噪聲和方位角測量噪聲與狀態向量SV互不相關并且測量式(19)的輸出即為機器人和路標之間的相對距離和相對方位角，所以對測量方程的輸入Sigma點的選取可以簡化為

同時對均值和協方差的估計需要由式(9)、式(11)修正為

式中:Noise.mean為噪聲均值所組成的向量，Noise.cov為噪聲協方差所組成的協方差矩陣.從式(25)和式(26)可以看出此時噪聲參數對于非線性輸出的估計可以認為是單純的附加量.

相對于使用式(20)～式(22)對測量方程輸出的估計使用式(23)～式(28)時輸入Sigma點數目從2(3+2n+1+1)+1降低為2(3+2n)+ 1，從而降低了時間占用量.當使用式(23)～式(28)獲得對第i個路標的測量數據向量MVk+1和測量數據向量協方差矩陣 PMVk+1后，根據式(12)獲取測量方程的輸入變量和測量方程輸出MVk+1的協方差為

1.4 狀態向量及狀態協方差動態調整

由狀態向量和狀態向量協方差矩陣可知隨著已檢測到路標數目的增大狀態向量SV和狀態向量協方差PV的維數都會逐漸增大，這樣會嚴重影響SLAM的速度.因此根據已測量到的路標統計信息提出一種動態狀態向量和狀態向量協方差調整的方法.假設k時刻之前所檢測到的路標索引序列為

式中:MLi為在第i時刻所檢測到的路標索引序列.根據檢測路標統計信息計算每個路標對于當前時刻狀態向量的影響權重，同時依據此權重對當前狀態向量和狀態向量協方差實現動態調整.

為簡單起見，在k時刻計算各個路標權重時只考慮到k之前的m個時刻的路標統計信息，m取值范圍為1～k，距離當前時刻k越近的路標索引序列所對應的權重系數越大.這樣k時刻所參考的路標索引序列為

每次路標索引的權重系數滿足wk＞wk－1＞… ＞wk－m+2＞wk－m+1，權重系數為

在時刻k各個路標的權重為

式中:i為路標索引.

式中:當路標i包括在檢測路標索引序列MLj中時其值為1，否則為0.

為將狀態向量SV和狀態向量協方差矩陣PV控制在固定大小內，定義閾值MAXLM，MAXLM為在SV和PV中所包含的最大路標數.當SV和PV中路標數目大于MAXLM時首先通過式(35)獲取每個路標在此時刻的權重，然后在SV和PV中找到權重最小的路標并將其從SV和PV中刪除，直至SV和PV中所包含路標數目小于等于MAXLM.當在后續時刻重新檢測到被刪除過的路標時將其作為未被檢測過的路標對待.

1.5 初始狀態及其協方差確定

由于在初始狀態時還未檢測到任何路標，因此初始狀態向量SV1和狀態向量協方差PV1中只包括機器人位置信息.

SV1和PV1組成的式(37)滿足自由度為3的卡方分布，根據一致性理論［14］可知當式(37)的值位于雙邊概率為95%的區域時認為其符合一致性要求.由卡方分布可知此時雙邊概率區域為［0.22 9.35］.

根據上述要求，當速度噪聲滿足N～(0，VV)，方位角噪聲滿足N～(0，AV)時，初始狀態向量和狀態向量協方差取值為

此時式(38)和式(39)滿足一致性要求.

2 內存分析

在CDKFSLAM算法中所占用的內存由狀態向量SV，狀態向量協方差PV以及參考路標序列MLref組成.當環境中路標總體數目為LN時，CDKFSLAM占用的最大內存為

式中:(3+2MAXLM)為狀態向量SV大小，(3+ 2MAXLM)2為狀態向量協方差PV大小，mLN為參考路標序列大小.

EKFSLAM算法中內存占用量由狀態向量和狀態向量協方差組成，其占用最大內存為

在FASTSLAM中，內存占用量和粒子數目有關.每個粒子所占用內存由機器人位置向量，機器人位置協方差，各個路標位置向量以及各個路標位置協方差組成.假設粒子數目為PMAX，其占用最大內存為

式中:(3+32)為機器人位置向量和位置向量協方差大小，(2+22)為每個路標位置向量和位置向量協方差大小.

對比式(40)～式(42)可知CDKFSLAM和FASTSLAM的內存占用和環境中路標總數LN成線性關系，EKFSLAM的內存占用和LN成二次型關系.圖1描述了 CDKFSLAM、EKFSLAM和FASTSLAM的內存占用情況隨路標數目LN變化的關系，其中，m取值為10.從圖1中可以看出CDKFSLAM對內存的占用要少于EKFSLAM和FASTSLAM.

3 實驗

本實驗在稀疏路標環境和密集路標環境下使用CDKFSLAM和FASTSLAM2.0分別進行測試.稀疏路標環境大小為250 m×200 m，包含35個路標;密集路標環境大小為250 m×200 m，包含135個路標.移動機器人輪間距為4 m，所使用傳感器最大檢測距離為30 m，檢測角度范圍為0～180°.SLAM過程中狀態向量預測的頻率為400 Hz，測量數據的獲取頻率為50 Hz.機器人運動過程中速度噪聲方差為0.3 m，方位角噪聲方差為3°;傳感器測量過程中測量距離噪聲方差為0.1 m，角度方差為1°.CDKFSLAM中狀態向量中的最大路標數目MAXLM為10，權重計算時所參考的路標序列數目m=10，FASTSLAM2.0中粒子數目取PMAX=10.

圖1 CDKFSLAM、EKFSLAM和FASTSLAM內存比較

圖2 稀疏路標環境下和密集路標環境下SLAM的比較

在稀疏路標環境下使用 CDKFSLAM和FASTSLAM2.0分別進行45次實驗.圖3中為每次MMSE(Minimum Mean Square Error)比較.圖4為CDKFSLAM和FASTSLAM2.0在不同時刻時狀態向量預測和狀態向量更新所消耗的時間.從圖4中可以看出，FASTSLAM2.0所消耗的時間要大于CDKFSLAM.

圖3 稀疏路標環境下CDKFSLAM和FASTSLAM的MMSE比較

圖4 稀疏路標環境下CDKFSLAM和FASTSLAM的消耗時間比較

在密集路標環境下，圖5中為每次MMSE (Minimum Mean Square Error)比較.圖6為CDKFSLAM和FASTSLAM2.0在不同時刻狀態向量預測和狀態向量更新所消耗的時間.從圖6中可以看出FASTSLAM所消耗的時間要大于CDKFSLAM，與稀疏環境相比，CDKFSLAM所消耗的時間增大，這是因為在密集環境下動態調整狀態向量SV和PV的頻率加大的緣故.

圖5 密集路標環境下CDKFSLAM和FASTSLAM的MMSE比較

4 一致性分析

使用 NEES(NormalizedEstimationError Squared)作為對移動機器人SLAM過程中定位結果的一致性分析依據，NEES定義為

圖6 密集路標環境下CDKFSLAM和FASTSLAM的消耗時間比較

對CDKFSLAM和FASTSLAM2.0分別進行45次實驗，使用45次的平均NEES作為一致性分析依據.由卡方分布屬性可知符合自由度為45×3的卡方分布，當考慮雙邊概率為95%的區域作為一致性區域時可得位于此區域內時表示當前機器人位置估計滿足一致性要求，否則表示機器人位置估計不滿足一致性要求.圖7(a)為稀疏路標環境下通過45次實驗所得的CDKFSLAM的一致性數據，圖7(b)為粒子數為10時FASTSLAM2.0的一致性數據.圖8(a)為密集路標環境下CDKFSLAM的一致性數據，圖8(b)為粒子數為10時FASTSLAM2.0的一致性數據.的雙邊區域為［2.32 3.75］，當

表1是對圖7和圖8中位于一致性區間內點數的統計，比較可以看出無論是在稀疏路標環境下還是在密集路標環境下使用CDKFSLAM所得的機器人位置一致性都要優于粒子數目為10的FASTSLAM2.0.對比圖3和圖5可以看出對CDKFSLAM而言在密集路標環境下的MMSE均值要優于稀疏路標環境下的結果，但從一致性結果(圖7，圖8和表1)上來看卻恰恰相反，在稀疏環境下CDKFSLAM的一致性要優于密集環境下的結果.這是由于在MMSE計算過程中并沒有考慮協方差矩陣，由此可以看出在對SLAM結果的分析過程中使用一致性分析要比使用MMSE更加合理.

圖7 稀疏路標環境下使用CDKFSLAM和FASTSLAM所得機器人位置一致性

圖8 密集路標環境下使用CDKFSLAM和FASTSLAM所得機器人位置一致性

表1 CDKFSLAM和FASTSLAM一致性分析

5 結論

1)選取合適的參數時使用Striling多項式插值方法對非線性近似可以達到二級甚至更高級的近似，從而提高了SLAM結果的一致性.

2)使用已經測量到的路標統計信息估計各個路標對當前狀態向量的影響權重，使用各個路標權重動態調整狀態向量和狀態協方差將其控制在固定大小內雖然在一定程度上丟失了一些歷史信息，但可以使其在密集型路標環境下達到較快的SLAM速度從而可以提高實際應用的可行性.

3)從實驗結果可以看出本文算法的SLAM MMSE結果要優于FASTSLAM，從一致性分析中可以看出在稀疏環境和密集環境下CDKFSLAM的一致性都優于粒子數目為10的FASTSLAM;另外從一致性結果和MMSE比較可以看出對SLAM結果的評價時使用一致性評價要比使用MMSE更加合理.

［1］SMITH R，SELF M，CHESSEMAN P.Estimating uncertain spatial relationships in robots［C］//Autonomous Robot Vehicles.New York，NY:Springer-Verlag New York，Inc，1990:167－193.

［2］DURRANT-WHYTE H，BAILEY T.Simultaneous localization and mapping:Part I［J］.IEEE Robotics and Automation Magazine，2006，13(2):99－110.

［3］BAILEY T，DURRANT-WHYTE H.Simultaneous localization and mapping:Part II［J］.IEEE Robotics and Automation Magazine，2006，13(3):108－117.

［4］KIM C，SAKTHIVEL R，CHUNG W K.Unscented FastSLAM:A robust and efficient solution to the slam problem［J］.IEEE Transactions on Robotics，2008，24(4):808－820.

［5］BAILEY T，NIETO J，NEBOT E.Consistency of the FastSLAM algorithm［C］//Proceeding of IEEE International Conference on Robotics and Automation.Orlando，FL:IEEE，2006:424－429.

［6］MONTEMERLO M，THRUN S，KOLLER D，et al.FastSLAM 2.0:An improved particle filtering algorithm for simultaneous localization and mapping that provably converges［C］//Proceeding of the International Conference on Artificial Intelligence.Acapulco，Mexico: Springer，2003:1151－1156.

［7］THRUN S，KOLLER D，GHAHRAMANI Z，et al.Simultaneous Mapping and Localization with Sparse Extended Information Fulters:Theory and Initial Results［R］.［S.l.］:CMU，2002.

［8］THRUN S，LIU Y F，KOLLER D，et al.Simultaneous mapping and localization with sparse extended information filters［J］.The International Journal of Robotics Research，2004，23(7/8):693－716.

［9］WAN E A，Van der MERWE R.The unscented kalman filter for nonlinear estimation［C］//Proceedings of A-daptive Systems for Signal Processing，Communications and Control Symposium.Lake Louise，Alta:IEEE，2000:153－158.

［10］NORGAARD M，POULSEN N，RAVN O.New developments in state estimation for nonlinear systems［J］.Automatica，2000，36(11):1627－1638.

［11］NORGAARD M，POULSEN N，RAVN O.Advances in Derivative-Free State Estimation for Nonlinear Systems［R］.［S.l.］:Denmark，2004.

［12］Van der MERWE R.Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models［D］.Oregon:Oregon Health＆Science University，2004.

［13］WANG X，ZHANG H.A UPF-UKF framework for SLAM［C］//Proceedings of International Conference on Robotics and Automation.Roma:IEEE，2007:1664－1669.

［14］BAR-SHALOM Y，RONG L X，KIRUBARAJAN T.Estimation with Applications To Tracking and Navigation［M］.New York，NY:Wiley-Interscience Publication，2001.