基于中心差分粒子滤波的SLAM算法

1 36 ò 1 2 Ï g Ä z Æ � Vol. 36, No. 2 2010 2 � ACTA AUTOMATICA SINICA February, 2010 Äu¥%�©âfÈÅ� SLAMŽ{ 6Uu 1 xHw 1 �Lê 1 Ü r 1 Á ‡ �é£ÄÅì<Ӟ½ †/ãMï (Simultaneous localization and mapping, SLAM) ¥� FastSLAM Ž{,  3š‚5Xڂ5z?nÚOŽäŒ'Ý �":, �©JÑ Äu Sterling õ‘ª�Š?nš‚5XÚ� SLAM {. T {Äu Rao-Blackwellized âfÈŵe, |^¥%�©ÈŐ{�)U?�ïÆ©Ù¼ê, Jp Åì< ^�O�°Ý; |^¥%�©ÈÅ�©zAÆڍ#/ã¥�AÆ, Jp /ãMï�°Ý; �é¢SA^¥3JbAÆ�œ¹JÑ ˜ «k��/ã+n{. 3Ó�âfê�œ¹e, T{U? SLAM (J�°Ý. Äu•ýÚ¢Sêâ�¢�(J�y T{�k�5. '… Ӟ½ †/ãMï, Rao-Blackwellized âfÈÅ, ¥%�©ÈÅì, ïÆ©Ù¼ê DOI 10.3724/SP.J.1004.2010.00249 A SLAM Algorithm Based on Central Difference Particle Filter ZHU Ji-Hua1 ZHENG Nan-Ning1 YUAN Ze-Jian1 ZHANG Qiang1 Abstract There are two serious drawbacks in FastSLAM (Simultaneous localization and mapping), which are the deriva- tion of the Jacobian matrices and the linear approximations of nonlinear functions. To overcome the serious drawbacks of the previous frameworks, this paper provides a robust SLAM algorithm based on the Sterling polynomial interpolation. It uses the central difference filter (CDF) to compute the proposal distribution in Rao-Blackwellized particle filter, then to initialize and update each feature state. For practical application, an effective mechanism for feature management is proposed. This approach improves the state estimation accuracy, and requires a smaller number of particles than previous approaches. Both simulation and experimental results are used to validate the effectiveness of the proposed algorithm. Key words Simultaneous localization and mapping, Rao-Blackwellized particle filter, central difference filter (CDF), proposal distribution £ÄÅì<3™‚¸¥‚g��‘�Da ì¼�‚¸�a&E, 48/Mﱌ‚¸�/ ã, Ӟ|^¤Mï�/ã¢yg�½ , =Ӟ ½ Ú/ãMï¯K´£ÄÅì<¢yý�gÌ �'…. TïÄ©uþ­V 80 “¥Ï[1], 8 ® kõ«)ûd¯K�{ [2] , ̇)šVǐ{ ÚÄuVÇ�O{, ¿±ÄuVÇ�O{ǑÌ 6 [3−4]. ��VÇ�O{´Äu*�k�ùÈÅ � SLAM Ž{[5−6], �daŽ{3J±)û�ê â'é¯KÚ�'uAƇê�g�OŽþ† ;þ¯K. C 5, Doucet �JÑ |^ Rao- Blackwellized âfÈÅ)û SLAM (Simultaneous localization and mapping) ¯K[7] �k�{. § ÂvFÏ 2008-11-12 ¹^FÏ 2009-04-03 Manuscript received November 12, 2008; accepted April 3, 2009 I[­:Ä:ïÄu�Oy (973 Oy) (2007CB311005) ÚI[g ,‰ÆÄ7 (90820017) ℄Ï Supported by National Basic Research Program of China (973 Program) (2007CB311005) and National Natural Science Foun- dation of China (90820017) 1. ÜS�όÆ<óœU†Åì<ïĤÜS 710049 1. Institute of Artificial Intelligence and Robotics, Xi′an Jiao- tong University, Xi′an 710049 Œ±ŠâI‡)¤AÆ/ã!»‚/ã, ¿…é� /)û êâ'é¯K. �duda{¥, z‡ âf¥;Xˆg�Åì<½ (JÚ/ã, �¡ �X�'uâfê�OŽþÚ;þ¯K. Ïd, 3 ØKǑ SLAM (J°Ý� Je, XÛü$ SLAM L§¥¤I��âfê´�©ïÄ�­:. Äu Rao-Blackwellized âfÈŵe, Motemerlo �J Ñ ·^uMïAÆ/ã�k�{: FastSLAM Ž{ [8−9]. T{|^âfÈÅ�OÅì<�G� &E, ¿|^õ‡*�k�ùÈÅì©O�Oz‡ AÆ�G�&E. þã{duþæ^�V�mC qš‚5XÚ¿I‡OŽäŒ'Ý , Ïd�O( J�Ø��Œ¿…OŽ„¡. ǑdkÆöJÑ | ^�›k�ùÈÅ [10] O“ FastSLAM Ž{¥�* �k�ùÈÅ�{ [11−12], T{?˜Úü$ /ãMï¤I�âfê, �I‡Šâ²�(½ºÝ �›C† (Scaled unscented transformation, SUT) ¥¤I� 3 ‡™ëê, ëê�ÀJòKǑ½ Ú /ãMï�°Ý. �©JÑ Äu Sterling õ‘ª�Š[13−14] ? 更多技术文章，论文请登录www.srvee.com 内容版权归作者所有 250 g Ä z Æ � 36ò nš‚5XÚ� SLAM {. T{3|^¥%� ©ÈŐ{�)U?�ïÆ©Ù¼êÄ:ƒþ, | ^¥%�©ÈŐ{�©zAÆڍ#/ã¥�A Æ. d , Ǒ GØ¢SA^¥�JbAÆ, �©J Ñ ˜«k��/ã+nüÑ. 3Ó�âfêe, ù U?�„–Jp £ÄÅì<Ӟ½ Ú/ãM ï�°Ý. 1 FastSLAMŽ{ b�ΘΘΘ L«/ãG�Cþ (·Ž), θθθk L«1 k ‡AÆ. Åì<3 t žǑ�G�CþǑ xxxt, *ÿ(J Ǒ zzzt, (t−1)žǑ� tžǑ�››Ñ\Ǒ uuut. 3VÇ Ø¥, SLAMŒ£ãǑ: ‰½ 1 žǑ� t žǑp§O Öê uuut = {uuu1, · · · ,uuut−1} Ú 1 žǑ� t žǑDaì *ÿ zzzt = {zzz1, · · · , zzzt}, 48/|^�“dÈÅì� �£ÄÅì<�´» xxxt = {xxx1, · · · ,xxxt} ڂ¸/ã �éÜ��VǗݼê p(xxxt,ΘΘΘ|zzzt,uuut). 3 SLAM ¯K¥, ‰½Åì<�^��O� Je, ØÓAÆ ƒm��O´ƒpÕá� [8−9] . Šâ SLAM ù˜A :, Œ±òéÜ��VÇ—Ý p(xxxt,ΘΘΘ|nt, zzzt,uuut) © )¤±e/ª: p(xxxt,ΘΘΘ|Y ) = p(xxxt|Y ) N∏ k=1 p(θθθk|Y ) (1) Ù¥ Y “L (nt, zzzt,uuut), nt = {n1, · · · , nt} Ǒêâ 'éCþ. ª (1) ´|^âfÈÅì¢y SLAM � nØÄ:ڝâ. ©z [8] JÑ Äuª (1) � FastSLAM Ž{. TŽ{Äu Rao-Blackwellized âfÈŵe, |^ âfÈÅì�OÅì<�G�, ¿^õ‡*�k� ùÈÅì©O�Oz‡AÆ�G�. dŽ{¥�z ‡âf©O¹ Tâf��Š, Åì<�´»& EÚAÆ�G�: XXX [m] t = 〈 w [m] t ,xxx t,[m],µµµ [m] 1,t ,Σ [m] 1,t , · · · ,µµµ[m]N,t,Σ[m]N,t 〉 (2) Ù¥, xxxt,[m] “L1 m ‡âf'uÅì<�´»� O(J, µµµ [m] k,t ´1 k ‡AÆ�G��O(J, Σ [m] k,t Ǒ1 k ‡AÆ�G��OƐ�Ý . FastSLAM Ž{) 1.0 Ú 2.0 ü‡‡�[8−9]. ö�ïÆ© Ù¼êK\ � p§O�Öê, ™ā#� *ÿ&E. duDaì�°Ý˜„�pup§O� °Ý, ùò�—âfÈÅ�Ä��Ǜ©$[15] . Ä u#�*ÿ&E, �ö¼� U?�ïÆ©Ù¼ ê, Ïdü$ /ãMï¤I�âfê. ddŒ, FastSLAM Ž{�'…3uXÛOŽâf��Š w [m] t : du8I¼ê p(xxx t,ΘΘΘ|nt, zzzt,uuut) (½, Ïd� �ïÆ©Ù¼êòU?TŽ{��O(J. 3þãŽ{¥, z‡âfÑ;Xˆg'uÅ ì<$Ä;,�&E, ±9õ‡ÕáAÆG��� O. ÏLÚ\��ä�;(�[9] , Œü$Ž{¤I �;˜m. daŽ{é�/)û SLAM ¯K¥ �êâ'é¯K. , TŽ{3X‚5z?nš ‚5XÚ�"€, ù«?nªî­/KǑ Åì