田 翔，張 亮，陳耀武

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

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

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

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

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

1.1 Stirling多項式插值

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

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

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

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

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

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

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

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

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

式中:sp為輸入變量x的協(xié)方差Cholesky系數(shù)的第p項.

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

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

1.2 SLAM狀態(tài)向量預測

在CDKFSLAM過程中，狀態(tài)向量和狀態(tài)向量協(xié)方差分別為

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

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

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

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

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

1.3 SLAM狀態(tài)向量更新

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

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

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

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

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

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

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

1.4 狀態(tài)向量及狀態(tài)協(xié)方差動態(tài)調(diào)整

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

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

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

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

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

式中:i為路標索引.

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

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

1.5 初始狀態(tài)及其協(xié)方差確定

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

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

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

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

2 內(nèi)存分析

在CDKFSLAM算法中所占用的內(nèi)存由狀態(tài)向量SV，狀態(tài)向量協(xié)方差PV以及參考路標序列MLref組成.當環(huán)境中路標總體數(shù)目為LN時，CDKFSLAM占用的最大內(nèi)存為

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

EKFSLAM算法中內(nèi)存占用量由狀態(tài)向量和狀態(tài)向量協(xié)方差組成，其占用最大內(nèi)存為

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

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

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

3 實驗

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

圖1 CDKFSLAM、EKFSLAM和FASTSLAM內(nèi)存比較

圖2 稀疏路標環(huán)境下和密集路標環(huán)境下SLAM的比較

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

圖3 稀疏路標環(huán)境下CDKFSLAM和FASTSLAM的MMSE比較

圖4 稀疏路標環(huán)境下CDKFSLAM和FASTSLAM的消耗時間比較

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

圖5 密集路標環(huán)境下CDKFSLAM和FASTSLAM的MMSE比較

4 一致性分析

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

圖6 密集路標環(huán)境下CDKFSLAM和FASTSLAM的消耗時間比較

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

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

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

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

表1 CDKFSLAM和FASTSLAM一致性分析

5 結(jié)論

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

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

3)從實驗結(jié)果可以看出本文算法的SLAM MMSE結(jié)果要優(yōu)于FASTSLAM，從一致性分析中可以看出在稀疏環(huán)境和密集環(huán)境下CDKFSLAM的一致性都優(yōu)于粒子數(shù)目為10的FASTSLAM;另外從一致性結(jié)果和MMSE比較可以看出對SLAM結(jié)果的評價時使用一致性評價要比使用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，F(xiàn)L: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.