基于水下机器人EKF-SLAM的数据关联算法研究
|
摘要
导航技术是AUV实现自治的关键技术之一,高精度的导航和定位对其安全航行和高效率完成任务具有决定性的作用。同时定位与地图构建(SLAM, Simultaneous Localization and Mapping)利用所携带的外部传感器感知环境,并利用提取的信息同时进行水下地图构建和自身定位。由于水下环境极其复杂,可使用的外部传感器仅限于声呐、水下照相机等,且获得的观测信息噪声大、干扰多,所以对SLAM的数据关联提出了很高的要求。本文主要对水下机器人SLAM的数据关联算法进行了深入的研究和分析。
论文介绍了AUV的导航方式以及SLAM对于水下定位的重要意义和发展现状；介绍了航行环境的描述方法以及SLAM的实现方法,分析了存在的技术难点；讨论了SLAM算法的性质,阐述了AUV基于扩展卡尔曼滤波的SLAM算法原理,建立了相关的仿真平台；重点研究了几种数据关联方法：最近邻算法、最大可能性算法、连续兼容最近邻算法和联合兼容算法,并提出了一种基于蚁群优化算法改进的最大可能性算法；结合各种数据关联方法,在逐渐增加量测噪声和过程噪声以及变化特征点间间隔的仿真场景中,进行了对比试验；并将某型AUV在水池中做直线运动获得的声呐图像数据融入SLAM仿真平台中,处理得出了相关的结果。
试验结果表明：相对于单纯推位方法,SLAM可以提高系统的定位精度,也验证了算法在水下导航应用上的可行性,数据关联算法的优劣受到诸多因素的影响,如地图中特征间的间隔等。提出的新算法在保证实时性的同时,能够有效的提高关联正确率,具有一定的可行性。本论文所研究的工作,对于智能水下机器人自主导航的研究和SLAM技术的应用具有一定的参考意义。
Navigation and localization with high precision is vital for the safety of the AUV and its effective completion of missions. Simultaneous Localization and Mapping (SLAM) algorithm allows the vehicle using on-board sensors to sense the environment and extract useful information to construct a feature map while locating itself through the map. However, the available sensors are limited within sonar and TV due to the complexity of underwater environment which brings much disturbance on obtained information. So there is a great demand on performance of data association. The research of this thesis emphasized on data association of SLAM.
This paper gives a summary of current development of the worldwide SLAM research and its significance to AUV. The expressions of environment where an AUV will navigate are discussed. The details of SLAM principles realized by Extended Kalman Filter (EKF) are displayed. Relative simulative scenes with increasing measure noise and process noise and different feature separations are built up Four typical data association algorithms are elaborated and a new method based on Ant Colony Optimization is proposed. Comparison simulations for each association algorithm combined with different experiment scenes are researched and analyzed. What's more, the data of sonar images from some AUV during certain tank test are used in the simulation.
The results demonstrated that SLAM has a superior localization precision to pure dead reckoning and the feasibility of SLAM in underwater navigation is verified. The effect of association algorithm is influenced by various factors such as noise and feature separation. The presented algorithm proves to be excellent with high correct association rate and good real-time. The research will be favorable for the application of SLAM and AUV navigation.
论文介绍了AUV的导航方式以及SLAM对于水下定位的重要意义和发展现状；介绍了航行环境的描述方法以及SLAM的实现方法,分析了存在的技术难点；讨论了SLAM算法的性质,阐述了AUV基于扩展卡尔曼滤波的SLAM算法原理,建立了相关的仿真平台；重点研究了几种数据关联方法：最近邻算法、最大可能性算法、连续兼容最近邻算法和联合兼容算法,并提出了一种基于蚁群优化算法改进的最大可能性算法；结合各种数据关联方法,在逐渐增加量测噪声和过程噪声以及变化特征点间间隔的仿真场景中,进行了对比试验；并将某型AUV在水池中做直线运动获得的声呐图像数据融入SLAM仿真平台中,处理得出了相关的结果。
试验结果表明：相对于单纯推位方法,SLAM可以提高系统的定位精度,也验证了算法在水下导航应用上的可行性,数据关联算法的优劣受到诸多因素的影响,如地图中特征间的间隔等。提出的新算法在保证实时性的同时,能够有效的提高关联正确率,具有一定的可行性。本论文所研究的工作,对于智能水下机器人自主导航的研究和SLAM技术的应用具有一定的参考意义。
Navigation and localization with high precision is vital for the safety of the AUV and its effective completion of missions. Simultaneous Localization and Mapping (SLAM) algorithm allows the vehicle using on-board sensors to sense the environment and extract useful information to construct a feature map while locating itself through the map. However, the available sensors are limited within sonar and TV due to the complexity of underwater environment which brings much disturbance on obtained information. So there is a great demand on performance of data association. The research of this thesis emphasized on data association of SLAM.
This paper gives a summary of current development of the worldwide SLAM research and its significance to AUV. The expressions of environment where an AUV will navigate are discussed. The details of SLAM principles realized by Extended Kalman Filter (EKF) are displayed. Relative simulative scenes with increasing measure noise and process noise and different feature separations are built up Four typical data association algorithms are elaborated and a new method based on Ant Colony Optimization is proposed. Comparison simulations for each association algorithm combined with different experiment scenes are researched and analyzed. What's more, the data of sonar images from some AUV during certain tank test are used in the simulation.
The results demonstrated that SLAM has a superior localization precision to pure dead reckoning and the feasibility of SLAM in underwater navigation is verified. The effect of association algorithm is influenced by various factors such as noise and feature separation. The presented algorithm proves to be excellent with high correct association rate and good real-time. The research will be favorable for the application of SLAM and AUV navigation.
引文
[1]郑宏.移动机器人导航和SLAM系统研究.上海交通大学硕士学位论文.2007：1-7页
[2]Durrant-Whyte H F. Where am I? A tutorial on mobile vehicle localization. Industrial Robot,1994,21(2):11-16P
[3]吕丹,戴学丰,刘树东.一种同时定位与地图构建的仿真系统.微计算机信息.2007,第23卷第(2-2)期
[4]Hans Jacob Sverdrup Feder. Simultaneous Stochastic Mapping and Localization. Massachusetts Institute of Technology.1999
[5]E.S.Maloney, editor. Dutton's Navigation and Piloting. Annapolis, MD: Naval Institute Press,1985
[6]E. Geyer, P. Creamer, J. D'Appolito, and R. Gains. Characteristics and capabilities of navigation systems for unmanned untethered submersibles. In Proc Int. Symp. on Unmanned Untethered Submersible Technology, Pages 320-347,1987
[7]J. Tard'os, J. Neira, P. Newman, and J. Leonard, "Robust mapping and localization in indoor environments using sonar data," Int. J. Robotics Research, vol.21, no.4, pp.311-330,2002
[8]P.M. Newman. On the Structure and Solution of the Simultaneous Localisation and Map Building Problem. PhD thesis, Australian Centre for Field Robotics. The University of Sydney, March 1999
[9]J.J. Leonard, R.N. Carpenter, and H.J.S. Feder. Stochastic mapping using forward look sonar. Robotica,19:341,2001
[10]J.A. Castellanos, J.M.M Montiel, J. Neira, and J.D. Tardos. Sensor influence in the performance of simultaneous mobile robot localization and map building. In P. Corke and J. Trevelyan, editors, Experimental Robotics IV, pp.287-296. Spring-Verlag,2000
[11]M.W.M.G.Dissanayake, P. Newman, H.F.Durrant-Whyte, S.Clark, and M.Csorba. An experimental and theoretical investigation into simultaneous localization and map building. Experimental Robotics IV, pp.265-274,2000
[12]H.J.S.Feder, J.J.Leonard, and C.M.Smith. Adaptive mobile robot navigation and mapping. International Journal of Robotics Research, Special Issue on Field and Service Robotics,18(7):650-668,1999
[13]W.D.Rencken. Concurrent localization and map building for mobile robots using ultrasonic sensors. In IEEE/RSJ Intl. Workshop on Intelligent Robots and Systems, volume 3, pp.2192-2197,1993
[14]P.Mourtarlier and R.Chatila, Stochastic Multisensory Data Fusion for Mobile Robot Location and Environment Modelling, Proc. Int. Symnp. On Robotics Research, Tokyo,1989
[15]王卫华,陈卫东,席裕庚.基于不确定信息的移动机器人地图创建研究进展.机器人.2001,6(23)：563—568页
[16]A. Amir, A. Efrat, P.Indyk, and H.Samet. Efficient Regular Data Structures and Algorithms for Location and Proximity Problems. In IEEE Symposium on Foundations of Computer Science, pp.160-170,1999
[17]A. Elfes. Occupancy grids:A probabilistic framework for robot perception and navigation. PhD thesis, Camegie Mellon University,1989
[18]K.Chong and L. Kleeman. Mobile robot map building from an advanced sonar array and accurate odometry. Intl. Journal of Robotics Research, 18(1):20-36,1999
[19]J.J.Leonard, H.F. Durrant-Whyte, and I.J. Cox. Dynamic map building for an autonomous mobile robot. Intl. Journal of Robotics Research, 11(4):286-298,1992
[20]O.Aycard, F.Charpillet, D. Fohr, and J.Mari. Place Learning and Recognition Using Hidden Markov Models. In Proceedings of the IEEE International Conference on Intelligent Robots and Systems,pp.1741-1746.1997
[21]R. Simmons and S. Koenig. Probabilistic Robot Navigation in Partially Observable Environments. International Joint Conference on Artificial Intelligence, pp.1080-1087,1995
[22]殷波.移动机器人同时定位与地图创建方法研究.中国海洋大学博士学位论文.2006.27-28页
[23]陈卫东,张飞.移动机器人的同步自定位与地图创建研究进展,控制理论与应用22(3)：455-460,2005
[24]Arulanmpalam S, Maskell S, Gordon N, et al. A tutorial on particle filters for on-line non-linear/non-Gaussian Bayesian tracking. IEEE Transactions on Signal Processing.2002,50(2):174-188
[25]迟建男,徐心和.移动机器人即使定位与地图创建问题研究.机器人,2004,26(1)：92-96
[26]Murphy K P. Bayesian map learning in dynamic environments. In Advances in Neural Information Processing System,2002,12:1015-1021, MIT Press
[27]H. F. Durrant-whyte. Uncertain geometry in robotics. IEEE Journal of Robotics and Automation,4(1):23-31, February 1988
[28]R.Smith, M.Self, and P.Cheeseman. Estimating uncertain spatial relationships in robotics. In Autonomous robot vehicle, pages 167-193. Springer-Verlag New York, Inc.,1990
[29]P.M. Newman and J. Leonard. Pure range-only sub-sea SLAM. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 1921-1926, Taipei, Taiwan, September 2003
[30]J.D. Tardos, J.Newman, and J.Leonard. Robust mapping and localization in indoor environments using sonar data. The Int. Journal of Robotics Research,21(4):311-330,April 2002
[31]David Ribas. Towards Simultaneous Localization & Mapping for an AUV using an Imaging Sonar. University de Girona.2005
[32]王璐,蔡自兴.未知环境中移动机器人并发建图与定位(CML)研究进展.机器人.2004,26(4)：380-384
[33]罗荣华,洪炳熔.移动机器人同时定位与地图构建研究进展.机器人,2004,26(2)：182-186
[34]J.Weber, K-W.Jorg and E. von Puttkamer, "APR-Global Scan Matching Using Anchor Point Relationships", in Proc. the 6th International Conference on Intelligent Autonomous Systems, pp.471-478,2000
[35]P.Jensfelt and H.I.Christensen, Laser based pose tracking, in Proc. IEEE Int. Conf. on Robotics and Automation, pp.2994-3000,1999
[36]M. Wada, KS Yoon and H.Hashimoto, High Accuracy Road Vehicle State Estimation Using Extended Kalman Filter, Proceedings of 3rd IEEE International Conference on Intelligent Transportation Systems, pp. 282-287,2000
[37]徐则中.移动机器人的同时定位和地图构建.浙江大学博士学位论文.2004.71-75页
[38]赵晶.水下机器人同时定位与地图构建研究.哈尔滨工程大学硕士论文.2005.23-25页
[39]张志涌.精通MATLAB北京航空航天大学出版社.2003
[40]王文晶.EKF-SLAM算法在水下航行器定位中的研究应用.哈尔滨工程大学硕士学位论文.2007.27-35页
[41]S.Thrun. Robotic mapping:A survey. In G. Lakemeyer and B. Nebel, editors. Exploring Artificial Intelligence in the New Millenium. Morgan Kaufmann,2002
[42]唐守正.多元统计分析方法.中国林业出版社.1986：211-223页
[43]陆璇.应用统计.清华大学出版社.1999：135-137页
[44]强志庄.水下机器人定位标图并行技术研究.哈尔滨工程大学硕士学位论文.2004：23-25页
[45]Aron J. Cooper. A Comparison of Data Association Techniques for Simultaneous Localization and Mapping. Massachusetts Institute of Technology.2005.23-26p
[46]Jose Neira. Consensus in Data Association. SLAM Summer School, Oxford.2006
[47]Jose Neira and Juan D. Tardos. Data Association Stochastic Mapping Using the Joint Compatibility Test. IEEE Transactions on Robotics and Automation. volume 17,2001
[48]Y. Bar-Shalonm and T.E. Fortmann. Tracking and Data Association. Academic Press, Ilic, Orlando, FL,1988
[49]Tim Bailey. Mobile Robot Localisation and Mapping in Extensive Outdoor Environments. PhD thesis, The University of Sydney,2002
[50]黄席樾,张著洪,何传江,胡小兵,马笑潇.现代智能算法理论及应用.北京,科学出版社.2004：23-50页.
[51]张军,胡晓敏,罗旭耀等译.蚁群优化.北京,清华大学出社.2006：78-96页
[2]Durrant-Whyte H F. Where am I? A tutorial on mobile vehicle localization. Industrial Robot,1994,21(2):11-16P
[3]吕丹,戴学丰,刘树东.一种同时定位与地图构建的仿真系统.微计算机信息.2007,第23卷第(2-2)期
[4]Hans Jacob Sverdrup Feder. Simultaneous Stochastic Mapping and Localization. Massachusetts Institute of Technology.1999
[5]E.S.Maloney, editor. Dutton's Navigation and Piloting. Annapolis, MD: Naval Institute Press,1985
[6]E. Geyer, P. Creamer, J. D'Appolito, and R. Gains. Characteristics and capabilities of navigation systems for unmanned untethered submersibles. In Proc Int. Symp. on Unmanned Untethered Submersible Technology, Pages 320-347,1987
[7]J. Tard'os, J. Neira, P. Newman, and J. Leonard, "Robust mapping and localization in indoor environments using sonar data," Int. J. Robotics Research, vol.21, no.4, pp.311-330,2002
[8]P.M. Newman. On the Structure and Solution of the Simultaneous Localisation and Map Building Problem. PhD thesis, Australian Centre for Field Robotics. The University of Sydney, March 1999
[9]J.J. Leonard, R.N. Carpenter, and H.J.S. Feder. Stochastic mapping using forward look sonar. Robotica,19:341,2001
[10]J.A. Castellanos, J.M.M Montiel, J. Neira, and J.D. Tardos. Sensor influence in the performance of simultaneous mobile robot localization and map building. In P. Corke and J. Trevelyan, editors, Experimental Robotics IV, pp.287-296. Spring-Verlag,2000
[11]M.W.M.G.Dissanayake, P. Newman, H.F.Durrant-Whyte, S.Clark, and M.Csorba. An experimental and theoretical investigation into simultaneous localization and map building. Experimental Robotics IV, pp.265-274,2000
[12]H.J.S.Feder, J.J.Leonard, and C.M.Smith. Adaptive mobile robot navigation and mapping. International Journal of Robotics Research, Special Issue on Field and Service Robotics,18(7):650-668,1999
[13]W.D.Rencken. Concurrent localization and map building for mobile robots using ultrasonic sensors. In IEEE/RSJ Intl. Workshop on Intelligent Robots and Systems, volume 3, pp.2192-2197,1993
[14]P.Mourtarlier and R.Chatila, Stochastic Multisensory Data Fusion for Mobile Robot Location and Environment Modelling, Proc. Int. Symnp. On Robotics Research, Tokyo,1989
[15]王卫华,陈卫东,席裕庚.基于不确定信息的移动机器人地图创建研究进展.机器人.2001,6(23)：563—568页
[16]A. Amir, A. Efrat, P.Indyk, and H.Samet. Efficient Regular Data Structures and Algorithms for Location and Proximity Problems. In IEEE Symposium on Foundations of Computer Science, pp.160-170,1999
[17]A. Elfes. Occupancy grids:A probabilistic framework for robot perception and navigation. PhD thesis, Camegie Mellon University,1989
[18]K.Chong and L. Kleeman. Mobile robot map building from an advanced sonar array and accurate odometry. Intl. Journal of Robotics Research, 18(1):20-36,1999
[19]J.J.Leonard, H.F. Durrant-Whyte, and I.J. Cox. Dynamic map building for an autonomous mobile robot. Intl. Journal of Robotics Research, 11(4):286-298,1992
[20]O.Aycard, F.Charpillet, D. Fohr, and J.Mari. Place Learning and Recognition Using Hidden Markov Models. In Proceedings of the IEEE International Conference on Intelligent Robots and Systems,pp.1741-1746.1997
[21]R. Simmons and S. Koenig. Probabilistic Robot Navigation in Partially Observable Environments. International Joint Conference on Artificial Intelligence, pp.1080-1087,1995
[22]殷波.移动机器人同时定位与地图创建方法研究.中国海洋大学博士学位论文.2006.27-28页
[23]陈卫东,张飞.移动机器人的同步自定位与地图创建研究进展,控制理论与应用22(3)：455-460,2005
[24]Arulanmpalam S, Maskell S, Gordon N, et al. A tutorial on particle filters for on-line non-linear/non-Gaussian Bayesian tracking. IEEE Transactions on Signal Processing.2002,50(2):174-188
[25]迟建男,徐心和.移动机器人即使定位与地图创建问题研究.机器人,2004,26(1)：92-96
[26]Murphy K P. Bayesian map learning in dynamic environments. In Advances in Neural Information Processing System,2002,12:1015-1021, MIT Press
[27]H. F. Durrant-whyte. Uncertain geometry in robotics. IEEE Journal of Robotics and Automation,4(1):23-31, February 1988
[28]R.Smith, M.Self, and P.Cheeseman. Estimating uncertain spatial relationships in robotics. In Autonomous robot vehicle, pages 167-193. Springer-Verlag New York, Inc.,1990
[29]P.M. Newman and J. Leonard. Pure range-only sub-sea SLAM. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 1921-1926, Taipei, Taiwan, September 2003
[30]J.D. Tardos, J.Newman, and J.Leonard. Robust mapping and localization in indoor environments using sonar data. The Int. Journal of Robotics Research,21(4):311-330,April 2002
[31]David Ribas. Towards Simultaneous Localization & Mapping for an AUV using an Imaging Sonar. University de Girona.2005
[32]王璐,蔡自兴.未知环境中移动机器人并发建图与定位(CML)研究进展.机器人.2004,26(4)：380-384
[33]罗荣华,洪炳熔.移动机器人同时定位与地图构建研究进展.机器人,2004,26(2)：182-186
[34]J.Weber, K-W.Jorg and E. von Puttkamer, "APR-Global Scan Matching Using Anchor Point Relationships", in Proc. the 6th International Conference on Intelligent Autonomous Systems, pp.471-478,2000
[35]P.Jensfelt and H.I.Christensen, Laser based pose tracking, in Proc. IEEE Int. Conf. on Robotics and Automation, pp.2994-3000,1999
[36]M. Wada, KS Yoon and H.Hashimoto, High Accuracy Road Vehicle State Estimation Using Extended Kalman Filter, Proceedings of 3rd IEEE International Conference on Intelligent Transportation Systems, pp. 282-287,2000
[37]徐则中.移动机器人的同时定位和地图构建.浙江大学博士学位论文.2004.71-75页
[38]赵晶.水下机器人同时定位与地图构建研究.哈尔滨工程大学硕士论文.2005.23-25页
[39]张志涌.精通MATLAB北京航空航天大学出版社.2003
[40]王文晶.EKF-SLAM算法在水下航行器定位中的研究应用.哈尔滨工程大学硕士学位论文.2007.27-35页
[41]S.Thrun. Robotic mapping:A survey. In G. Lakemeyer and B. Nebel, editors. Exploring Artificial Intelligence in the New Millenium. Morgan Kaufmann,2002
[42]唐守正.多元统计分析方法.中国林业出版社.1986：211-223页
[43]陆璇.应用统计.清华大学出版社.1999：135-137页
[44]强志庄.水下机器人定位标图并行技术研究.哈尔滨工程大学硕士学位论文.2004：23-25页
[45]Aron J. Cooper. A Comparison of Data Association Techniques for Simultaneous Localization and Mapping. Massachusetts Institute of Technology.2005.23-26p
[46]Jose Neira. Consensus in Data Association. SLAM Summer School, Oxford.2006
[47]Jose Neira and Juan D. Tardos. Data Association Stochastic Mapping Using the Joint Compatibility Test. IEEE Transactions on Robotics and Automation. volume 17,2001
[48]Y. Bar-Shalonm and T.E. Fortmann. Tracking and Data Association. Academic Press, Ilic, Orlando, FL,1988
[49]Tim Bailey. Mobile Robot Localisation and Mapping in Extensive Outdoor Environments. PhD thesis, The University of Sydney,2002
[50]黄席樾,张著洪,何传江,胡小兵,马笑潇.现代智能算法理论及应用.北京,科学出版社.2004：23-50页.
[51]张军,胡晓敏,罗旭耀等译.蚁群优化.北京,清华大学出社.2006：78-96页