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

1、 第9 期 田 翔，等： 基于中心差分卡爾曼濾波器的快速 SLAM 算法 1459 動過程中速度噪聲方差為 0. 3 m， 方位角噪聲方 差為 3 ； 傳感器測量過程中測量距離噪聲方差為 0. 1 m， 角度方差為 1 CDKFSLAM 中狀態(tài)向量中 的最大路標數(shù)目 MAXLM 為 10 ， 權重計算時所參 FASTSLAM2. 0 中粒 考的路標序列數(shù)目 m = 10 ， 子數(shù)目取 PMAX = 10 10 cdkfslam cdkfslam ekfsiam cdkfslam cdkfslam MAXLM=5 MAXLM=10 PMAX=5 PMAX=10 次 MMSE （ Minimum

2、Mean Square Error） 比較 圖 4 為 CDKFSLAM 和 FASTSLAM2. 0 在不同時刻時狀 態(tài)向 量 預 測 和 狀 態(tài) 向 量 更 新 所 消 耗 的 時 間 從 FASTSLAM2. 0 所消耗的時間要 圖 4 中可以看出， 大于 CDKFSLAM 15 FASTSLAM MMSE CDKFSLAM MMSE 8 10 內存占用量/104 6 %MMSE 5 4 2 0 0 20 40 60 80 100 120 140 160 180 LN 200 %10 20 t/s 30 40 圖3 稀 疏 路 標 環(huán) 境 下 CDKFSLAM 和 FASTSLAM 的

3、MMSE 比較 0.16 FASTSLAM with 100 particles FASTSLAM with 50 particles FASTSLAM with 10 particles CDKFSLAM 圖1 %100 CDKFSLAM、 EKFSLAM 和 FASTSLAM 內存比較 每個步驟的耗時/s 0.12 0.08 0.04 60 20 y/m -20 0 50 100 t/s 150 200 -60 圖4 稀疏路標環(huán)境下 CDKFSLAM 和 FASTSLAM 的消耗 時間比較 -100 -150 -100 0 x/m （ a） SLAM 稀疏路標環(huán)境 -50 50 100 在

4、 密 集 路 標 環(huán) 境 下， 圖 5 中 為 每 次 MMSE （ Minimum Mean Square Error ） 比較 圖 6 為 CDKFSLAM 和 FASTSLAM2. 0 在不同時刻狀態(tài)向量 預測和狀態(tài)向量更新所消耗的時間 從圖 6 中可 以看出 FASTSLAM 所消耗的時間要大于 CDKFSLAM ， CDKFSLAM 所消 耗 的 時 與稀疏環(huán)境相比 ， 6 FASTSLAM MMSE CDKFSLAM MMSE %100 60 20 y/m 5 -20 %MMSE 4 3 2 -60 -100 -150 -100 -50 0 x/m 50 100 150 1 0 %

5、10 20 t/s 30 40 （ b） SLAM 密集路標環(huán)境 圖2 稀疏路標環(huán)境下和密集路標環(huán)境下 SLAM 的比較 在 稀 疏 路 標 環(huán) 境 下 使 用 CDKFSLAM 和 FASTSLAM2. 0 分別進行 45 次實驗 圖 3 中為每 圖5 密 集 路 標 環(huán) 境 下 CDKFSLAM 和 FASTSLAM 的 MMSE 比較 1460 哈 爾 濱 工 業(yè) 大 學 學 %40 30 報 第 42 卷 間增大， 這是因為在密集環(huán)境下動態(tài)調整狀態(tài)向 量 SV 和 PV 的頻率加大的緣故 4 一致性分析 一致性 20 10 0 使 用 NEES （ Normalized Estimat

6、ion Error Squared） 作為對移動機器人 SLAM 過程中定位結 NEES 定義為 果的一致性分析依據， （ k） ） T P（ k | k） （ k） = （ x（ k） x （ x（ k） x （ k） ） 1 （ 43 ） -10 0 20 40 60 80 100 120140 160 180 200 t/s （ a） CDKFSLAM 的一致性數(shù)據描述 x （ k） 為機 式中： x（ k） 為機器人 k 時刻實際位置， P （ k | k） 為機器人 k 時刻 器人 k 時刻的估計位置， 的位置協(xié)方差 由于機器人位置向量維數(shù)為 3 ， 因 此可知 （ k） 符合自由度

7、為 3 的卡方分布 FASTSLAM with 100 particles FASTSLAM with 50 particles FASTSLAM with 10 particles CDKFSLAM %40 30 一致性數(shù)據 0.8 每個步驟的耗時/s 20 10 0 -10 0.6 0.4 0.2 0 0 50 100 t/s 150 200 250 20 40 60 80 100 120140 160 180 200 t/s （ b） FASTSLAM 的一致性數(shù)據描述 圖6 密集路標環(huán)境下 CDKFSLAM 和 FASTSLAM 的消耗 時間比較 圖7 稀疏路標環(huán)境下使用 CDKFSL

8、AM 和 FASTSLAM 所 得機器人位置一致性 %40 30 20 10 0 -10 對 CDKFSLAM 和 FASTSLAM2. 0 分 別 進 行 45 次實驗， 使用 45 次的平均 NEES 作為一致性分 45 i =1 當考慮雙邊概率為 95 % 度為 45 3 的卡方分布， 的區(qū)域作為一致性區(qū)域時可得 （ k） 的雙邊區(qū)域 2. 32 3. 75 ， 為 當 （ k） 位于此區(qū)域內時表示當 否則表示機 前機器人位置估計滿足一致性要求， 器人位置估計不滿足一致性要求 圖 7 （ a ） 為稀 疏路標環(huán)境下通過 45 次實驗所得的 CDKFSLAM 的一 致 性 數(shù) 據， 圖 7

9、 （ b ） 為 粒 子 數(shù) 為 10 時 FASTSLAM2. 0 的一致性數(shù)據 圖 8 （ a ） 為密集路 標環(huán)境下 CDKFSLAM 的一致性數(shù)據， 圖 8 （ b） 為 粒子數(shù)為 10 時 FASTSLAM2. 0 的一致性數(shù)據 1 （ k） = i （ k） 45 i = 1 45 一致性 析依據 由卡方分布屬性可知 i （ k） 符合自由 0 20 40 60 80 100 120140 160 180 200 t/s （ a） CDKFSLAM 的一致性數(shù)據描述 %40 30 一致性數(shù)據 20 10 0 （ 44 ） 表 1 是對圖 7 和圖 8 中位于一致性區(qū)間內點 數(shù)的統(tǒng)計

10、， 比較可以看出無論是在稀疏路標環(huán)境 下還是在密集路標環(huán)境下使用 CDKFSLAM 所得 的機器人位置一致性都要優(yōu)于粒子數(shù)目為 10 的 FASTSLAM2. 0 對比圖 3 和圖 5 可以看出對 CDKFSLAM 而言在密集路標環(huán)境下的 MMSE 均值要 優(yōu)于稀疏路標環(huán)境下的結果， 但從一致性結果 圖8 -10 0 20 40 60 80 100 120140 160 180 200 t/s （ b） FASTSLAM 的一致性數(shù)據描述 密集路標環(huán)境下使用 CDKFSLAM 和 FASTSLAM 所 得機器人位置一致性 第9 期 田 翔，等： 基于中心差分卡爾曼濾波器的快速 SLAM 算法

11、1461 （ 圖 7， 圖 8 和表 1 ） 上來看卻恰恰相反， 在稀疏環(huán) 境下 CDKFSLAM 的一致性要優(yōu)于密集環(huán)境下的 結果 這是由于在 MMSE 計算過程中并沒有考慮 協(xié)方差矩陣， 由此可以看出在對 SLAM 結果的分 析過程中使用一致性分析要比使用 MMSE 更加 合理 表1 環(huán)境 一致性區(qū) 域內點數(shù) 總點數(shù) 百分比 / % 4 KIM C， SAKTHIVEL R， CHUNG W K Unscented FastSLAM： A robust and efficient solution to the slam problemJ IEEE Transactions on Robo

12、tics，2008 ， 24 （ 4 ） ： 808 820 5 BAILEY T，NIETO J，NEBOT E Consistency of the FastSLAM algorithmC/ / 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

13、for simultaneous localization and mapping that provably converges C/ / Proceeding of the International Conference on Artificial Intelligence Springer， 2003 ： 1151 1156 7 THRUN S，KOLLER D，GHAHRAMANI Z， et al Simultaneous Mapping and Localization with Sparse Extended Information Fulters： Theory and In

14、itial 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 estimati

15、on C/ / Proceedings of Adaptive 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

16、 O Advances in DerivativeFree State Estimation for Nonlinear Systems R S l ： Denmark， 2004 12Van der MERWE R SigmaPoint Kalman Filters for Probabilistic Inference in Dynamic StateSpace Models D Oregon： Oregon Health Science University， 2004 13WANG X， ZHANG H A UPFUKF framework for SLAM C/ / Proceedi

17、ngs of International Conference on Robotics and Automation Roma： IEEE， 2007 ： 1664 1669 14BARSHALOM Y，RONG L X，KIRUBARAJAN T Estimation with Applications To Tracking and Navigation M New York，NY： WileyInterscience Publication， 2001 Acapulco， Mexico： CDKFSLAM 和 FASTSLAM 一致性分析 稀疏路標環(huán)境下 密集路標環(huán)境下 CDKFSLAM

18、 FASTSLAM2. 0 CDKFSLAM FASTSLAM2. 0 6 375 8 779 72 41 0 8 779 0 1 628 8 779 18 54 0 8 779 0 5 結 論 1 ） 選取合適的參數(shù)時使用 Striling 多項式插 值方法對非線性近似可以達到二級甚至更高級的 近似， 從而提高了 SLAM 結果的一致性 2 ） 使用已經測量到的路標統(tǒng)計信息估計各 個路標對當前狀態(tài)向量的影響權重， 使用各個路 標權重動態(tài)調整狀態(tài)向量和狀態(tài)協(xié)方差將其控制 在固定大小內雖然在一定程度上丟失了一些歷史 信息， 但可以使其在密集型路標環(huán)境下達到較快 的 SLAM 速度從而可以提高實際應用的可行性 3 ） 從實驗結果可以看出本文算法的 SLA