自主式水下机器人同时定位与地图构建算法的研究
摘要
自主式水下机器人AUV(Autonomous Underwater Vehicle)代表了未来水下机器人技术的研究方向,是目前研究工作的热点。而导航问题仍是AUV所面临的主要技术挑战之一,在导航问题中,定位问题又是移动机器人的基本问题,是指移动机器人通过携带的传感器完成对内部状态的检测或对外部环境的感知,从而估算其自身位置和姿态的过程。
在未知的环境中,机器人的定位与构图是融为一体相互关联的,即同时定位和地图构建SLAM (Simultaneous Localization and Mapping)。问题可描述为:一个自主移动机器人从未知环境中的未知位置出发,在机器人航行过程中根据自身携带的传感器采集的数据实现自身位置的确定,同时增量式地构建全局环境地图。
本文首先介绍了SLAM算法的基础即卡尔曼滤波器理论,然后阐述了在单纯声纳更新的SLAM算法基础上引入了其他传感器更新环节的SLAM算法,即多传感器更新的SLAM算法,详细介绍了其实现流程,并从理论上说明其能够提高了机器人定位和构图精度。接着介绍了试验的前期准备,即AUV平台携带的传感器以及SLAM算法中用于构建环境地图的特征的提取。提取精确可靠的环境特征是SLAM算法准确度的保障,而环境特征的表示则依据机器人航行的环境。然而直接提取的环境特征可能比较密集,影响了SLAM算法的效率和精度,因此需对特征点进行去噪声和稀疏化处理,噪声是针对声纳自身的噪声及其环境背景噪声,而稀疏化处理则是针对声纳发射的单个波束及其多个波束间的冗余信息的。
最后通过湖试和海试试验验证了多传感器更新的SLAM算法的有效性。湖试试验的结果表明了该算法在机器人的定位和构图精度方面优于仅声纳更新的SLAM算法,而较长距离的海试试验进一步验证了该算法的有效性,且说明了该算法的定位和构图精度满足自主式水下机器人的航行要求。
Autonomous underwater vehicle (AUV) is the current research focus, which represents the future development direction of underwater vehicle technology. However, navigation is still the one of the major technical challenges of AUV, in which localization is the fundamental problem, which refers to a process of estimating robot’s pose by detecting the internal state of robot and perceiving the external surrounding environment through carried sensors.
Localization and mapping is integrated and interrelated in unknown environment, which is simultaneous localization and mapping (SLAM). SLAM can be described as: an autonomous mobile robot leaves from an unknown location in an unknown environment, relies on sensors to build a global environment map gradually, and then calculates its location by this map.
This article first introduces the theory basis of SLAM algorithm, which is EKF, then add other sensors update to only sonar update SLAM, namely SLAM with multi-sensor update, illustrates its flow and certifies it can improve accuracy of localization and mapping for the robot from theory. Next, experimental preparation is introduced, which includes sensors carried in AUV platform and features extraction for map construction in SLAM. Extracting reliable and accurate environmental features is prerequisite for the precision of SLAM algorithm, while features representing is based on environment where the robot navigates. But the original features of direct extraction from environment are generally too dense to affect the efficiency and accuracy of SLAM algorithm, and then denoising and sparseness of features are adapted. Noise involves noise of sonar itself and environmental background noise, and sparseness aims in redundant information of each beam and adjacent beams of sonar launches.
Finally, validity of SLAM with multi-sensor update is verified by lake and sea experiments. Lake experiment result proves that accuracy of localization and mapping of this algorithm is superior to SLAM only with sonar update, and longer sea trial experiment further verifies validity of SLAM algorithm. In addition, it also shows the accuracy of localization and mapping meets the requirements of autonomous navigation of robot.
在未知的环境中,机器人的定位与构图是融为一体相互关联的,即同时定位和地图构建SLAM (Simultaneous Localization and Mapping)。问题可描述为:一个自主移动机器人从未知环境中的未知位置出发,在机器人航行过程中根据自身携带的传感器采集的数据实现自身位置的确定,同时增量式地构建全局环境地图。
本文首先介绍了SLAM算法的基础即卡尔曼滤波器理论,然后阐述了在单纯声纳更新的SLAM算法基础上引入了其他传感器更新环节的SLAM算法,即多传感器更新的SLAM算法,详细介绍了其实现流程,并从理论上说明其能够提高了机器人定位和构图精度。接着介绍了试验的前期准备,即AUV平台携带的传感器以及SLAM算法中用于构建环境地图的特征的提取。提取精确可靠的环境特征是SLAM算法准确度的保障,而环境特征的表示则依据机器人航行的环境。然而直接提取的环境特征可能比较密集,影响了SLAM算法的效率和精度,因此需对特征点进行去噪声和稀疏化处理,噪声是针对声纳自身的噪声及其环境背景噪声,而稀疏化处理则是针对声纳发射的单个波束及其多个波束间的冗余信息的。
最后通过湖试和海试试验验证了多传感器更新的SLAM算法的有效性。湖试试验的结果表明了该算法在机器人的定位和构图精度方面优于仅声纳更新的SLAM算法,而较长距离的海试试验进一步验证了该算法的有效性,且说明了该算法的定位和构图精度满足自主式水下机器人的航行要求。
Autonomous underwater vehicle (AUV) is the current research focus, which represents the future development direction of underwater vehicle technology. However, navigation is still the one of the major technical challenges of AUV, in which localization is the fundamental problem, which refers to a process of estimating robot’s pose by detecting the internal state of robot and perceiving the external surrounding environment through carried sensors.
Localization and mapping is integrated and interrelated in unknown environment, which is simultaneous localization and mapping (SLAM). SLAM can be described as: an autonomous mobile robot leaves from an unknown location in an unknown environment, relies on sensors to build a global environment map gradually, and then calculates its location by this map.
This article first introduces the theory basis of SLAM algorithm, which is EKF, then add other sensors update to only sonar update SLAM, namely SLAM with multi-sensor update, illustrates its flow and certifies it can improve accuracy of localization and mapping for the robot from theory. Next, experimental preparation is introduced, which includes sensors carried in AUV platform and features extraction for map construction in SLAM. Extracting reliable and accurate environmental features is prerequisite for the precision of SLAM algorithm, while features representing is based on environment where the robot navigates. But the original features of direct extraction from environment are generally too dense to affect the efficiency and accuracy of SLAM algorithm, and then denoising and sparseness of features are adapted. Noise involves noise of sonar itself and environmental background noise, and sparseness aims in redundant information of each beam and adjacent beams of sonar launches.
Finally, validity of SLAM with multi-sensor update is verified by lake and sea experiments. Lake experiment result proves that accuracy of localization and mapping of this algorithm is superior to SLAM only with sonar update, and longer sea trial experiment further verifies validity of SLAM algorithm. In addition, it also shows the accuracy of localization and mapping meets the requirements of autonomous navigation of robot.
引文
[1] http://baike.baidu.com/view/2860.htm
[2]杨柯.大尺度未知海底环境下的AUV同时定位与地图构建方法研究:[硕士学位论文].青岛:中国海洋大学,2009
[3]任春云.自主式水下机器人的同时定位与地图构建算法及实现:[硕士学位论文].青岛:中国海洋大学,2010
[4]王文晶. EKF-SLAM算法在水下航行器定位中的应用研究:[硕士学位论文].哈尔滨:哈尔滨工程大学,2007
[5]王培勋.基于子地图连接的机器人同时定位与地图构建研究:[硕士学位论文].青岛:中国海洋大学,2010
[6] http://news.163.com/08/0319/08/47CQLJ9F000125LI.html
[7]于妮妮.基于EKF的AUV同时定位与构图方法研究:[硕士学位论文].青岛:中国海洋大学,2009
[8] Fox D, Burgard W. Monte Carlo localization for mobile robots. Proceedings of the IEEE International Conference on Robotics and Automation. USA: IEEE.1999.1322-1328
[9] Durrant Wh-qe H F. Where am I? A tutorial on mobile vehicle localization. Industrial Robot, 1994. 11-16.
[10] M. Dissanayake, P. Newman, S. Clark, H. Durrant-Whyte, and M. Csorba. A solution to the simultaneous localization and mapping (slam) problem. IEEE Transactions on Robotics and Automation, 2001
[11]王文晶.基于重力和环境特征的水下导航定位方法研究:[博士学位论文].哈尔滨:哈尔滨工程大学,2009
[12] Tim Bailey. Mobile Robot Localization and Mapping in Extensive Outdoor Environments. PhD thesis, University of Sydney, Australian. Centre for Field Robotics, 2002
[13] Thrun S, Fox D, Burgard W, et al. Robust Monte Carlo localization for mobile robots. Artificial Intelligence, 2001, 128(1-2):99-141
[14]王璐,蔡自兴.未知环境中移动机器人并发建图与定位(CML)的研究进展,机器人,2004
[15] Hahnel D, Schulz D, Burgard W. Map building with mobile robots in populated environments. Advanced Robotics , 2003 ,17 (7) : 579 -598
[16] Howard A, Matari’c MJ, Sukhatme GS. Localization for Mobile Robot Teams: a MaximumLikelihood approach. USA: University of Sourthern California , 2001
[17] Guivant J, Nebot E. Improved computational and memory requirements of simultaneous localization and map building algorithms. Proceedings of the 2002 IEEE International Conference on Robotics &Automation. Washington, DC: 2002. 2731-2736
[18]赵璇.基于粒子滤波的机器人定位及目标跟踪:[硕士学位论文].青岛:中国海洋大学,2008
[19] Dellaert F, Fox D, Burgard W, et al. Monte Carlo localization for mobile robots . Proceedings of the IEEE International Conference on Robotics and Automation. Detroit: 1999, 1322-1328
[20] Fox D. Adapting the sample size in particle filters through KLD-Sgnl-piing. The International Journal of Robotie Research, 2003, 22(12):985-1004
[21]陈白帆.动态环境下移动机器人同时定位与建图研究:[博士学位论文]长沙:中南大学,2009
[22] S. Thrun,W. Burgard and D. Fox.Probabilistic Robotics.The MIT press, 2005
[23]张洁.基于声纳的水下机器人同时定位与地图构建技术研究:[硕士学位论文].青岛:中国海洋大学,2010
[24] S. Thrun, Y. Liu, D. Koller, A. Y. Ng, Z. Ghahramani and H. Durrant-Whyte. Simultaneous Localization Mapping with Sparse Extended Information Filters. The International Journal of Robotics Research, 2004, 23(7): 693-716
[25]苏立.室内环境下移动机器人双目视觉SLAM研究:[硕士学位论文].西安:西安理工大学,2010
[26]谭民,王硕,曹志强.多机器人系统.清华大学出版社.2005
[27]周福娜,周梅,文成林.多传感器数据融合效果分析.河南大学学报(自然科学版) 2003,33(2):33-36
[28] Thrun S, Beetz M, Burgard W, et al. Probabilistic algorithms and the interactive museumtour-guided robot Minerva. International Journal of Robotics Research, 2000, 19(11) : 972 - 1000
[29] Bo He, Ke Yang, Shuai Zhao. Underwater Simultaneous Localization and Mapping Based on EKF and Point Features. ICMA, 2009.
[30]马莹巍,孙艳霞.未知环境下移动机器人地图构建的研究发展.中国电力教育. 2009,S1
[31] David Ribas ROMAGóS. Underwear SLAM for Structured Environment Using an Imaging Sonar: [phd thesis]. Girona: Universitat of Girona, 2008
[32] Luyue Huang, Bo He, Tao Zhang, An Autonomous Navigation Algorithm for Underwater Vehicles Based on Inertial Measurement Units and Sonar, Proceedings of 2nd International Asia conf. on Informatics in Control, Automation and Robotics, March, 2010
[33] Ioseba Tena Ruiz, S’ebastien de Raucourt, Yvan Petillot and David M.Lane. Concurrent Mapping and Localization Using Sidescan Sonar. IEEE JOURNAL OF OCEANIC
[34] David Ribas, Jos’eNeira, Pere Ridao, & Tard′os, J. D., SLAM using an imaging sonar for partially structured environments. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Beijing, China, 2006.
[35] I. Tena, S. de Raucourt, Y. Petillot and D. Lane. Concurrent mapping and localization using sidescan sonar. IEEE Journal of Oceanic Engineering, 2004, 29(2): 442-456
[36] H. Durrant-Whyte and T. Bailey. Simultaneous Localization and Mapping (SLAM): Part I, the essential algorithms. Robotics and Automation,2006,13(2): 99-108
[37] Guivant, J. E. and Nebot, E. M. Optimization of the Simultaneous Localization and Map Building Algorithm for Real Time Implementation. IEEE Trans. Robot. Automat, 2001, 17(3): 242–257
[38] Leonard, J. J. and Durrant-Whyte, H. F. Directed Sonar Sensing for Mobile Robot Navigation. London: Kluwer Academic Pub, 1992
[39]赵鹏飞,徐万里,罗佳.数据关联算法综述及性能评估.武器制造. 2009,09
[40] Jing Luo, Bo He, Data Association for AUV Localization Conference on Measuring Tech. and Mechatronics Automation. 2010
[41] http://baike.baidu.com/view/79592.htm
[42] Tritech公司. Super Seaking DST Sonar技术参考资料,2007
[43] LinkQuest Inc. User Guide, 2006
[44]星网宇达公司.定位定向位姿导航产品选型手册,2008
[45]陈书海,傅录祥.实用数字图像处理.北京:科学出版社, 2005
[46]李介谷等.图像处理技术.上海:上海交通大学出版社,1988
[47] Kenneth R. Castleman, (朱志刚等译).数字图像处理.电子工业出版社,2002:58-60
[48]黄爱民,安向京,骆力等.数字图像处理与分析基础.北京:中国水利水电出版社, 2005
[49]王彪.声纳图像的处理及目标识别技术研究:[硕士学位论文].西安:西北师范大学, 2005
[50]陈亮.基于图像处理的水下目标识别方法研究:[硕士学位论文].哈尔滨:哈尔滨工程大学,2006
[51]贾云得.机器视觉.北京:科学出版社,2000
[52]李启虎.声纳信号处理引论.北京:海洋出版社,2000
[53]吴翠江.水下机器人声视觉仿真系统研究:[硕士学位论文].哈尔滨:哈尔滨工程大学,2004
[54] Shujuan Chen, Bo He, Shu Tian, Pei Song, Guijie Liu. Simultaneous Localization and Mapping in Structured Environment Based on Point Feature. 2010 3rd International Conference on Computer and Electrical Engineering, 2010,456-460
[55] David Ribas. Abandoned Marina Dataset. Experiment performed at the Fluvia Nautic abandoned marina near St Pere Pescador (Spain), 2007
[2]杨柯.大尺度未知海底环境下的AUV同时定位与地图构建方法研究:[硕士学位论文].青岛:中国海洋大学,2009
[3]任春云.自主式水下机器人的同时定位与地图构建算法及实现:[硕士学位论文].青岛:中国海洋大学,2010
[4]王文晶. EKF-SLAM算法在水下航行器定位中的应用研究:[硕士学位论文].哈尔滨:哈尔滨工程大学,2007
[5]王培勋.基于子地图连接的机器人同时定位与地图构建研究:[硕士学位论文].青岛:中国海洋大学,2010
[6] http://news.163.com/08/0319/08/47CQLJ9F000125LI.html
[7]于妮妮.基于EKF的AUV同时定位与构图方法研究:[硕士学位论文].青岛:中国海洋大学,2009
[8] Fox D, Burgard W. Monte Carlo localization for mobile robots. Proceedings of the IEEE International Conference on Robotics and Automation. USA: IEEE.1999.1322-1328
[9] Durrant Wh-qe H F. Where am I? A tutorial on mobile vehicle localization. Industrial Robot, 1994. 11-16.
[10] M. Dissanayake, P. Newman, S. Clark, H. Durrant-Whyte, and M. Csorba. A solution to the simultaneous localization and mapping (slam) problem. IEEE Transactions on Robotics and Automation, 2001
[11]王文晶.基于重力和环境特征的水下导航定位方法研究:[博士学位论文].哈尔滨:哈尔滨工程大学,2009
[12] Tim Bailey. Mobile Robot Localization and Mapping in Extensive Outdoor Environments. PhD thesis, University of Sydney, Australian. Centre for Field Robotics, 2002
[13] Thrun S, Fox D, Burgard W, et al. Robust Monte Carlo localization for mobile robots. Artificial Intelligence, 2001, 128(1-2):99-141
[14]王璐,蔡自兴.未知环境中移动机器人并发建图与定位(CML)的研究进展,机器人,2004
[15] Hahnel D, Schulz D, Burgard W. Map building with mobile robots in populated environments. Advanced Robotics , 2003 ,17 (7) : 579 -598
[16] Howard A, Matari’c MJ, Sukhatme GS. Localization for Mobile Robot Teams: a MaximumLikelihood approach. USA: University of Sourthern California , 2001
[17] Guivant J, Nebot E. Improved computational and memory requirements of simultaneous localization and map building algorithms. Proceedings of the 2002 IEEE International Conference on Robotics &Automation. Washington, DC: 2002. 2731-2736
[18]赵璇.基于粒子滤波的机器人定位及目标跟踪:[硕士学位论文].青岛:中国海洋大学,2008
[19] Dellaert F, Fox D, Burgard W, et al. Monte Carlo localization for mobile robots . Proceedings of the IEEE International Conference on Robotics and Automation. Detroit: 1999, 1322-1328
[20] Fox D. Adapting the sample size in particle filters through KLD-Sgnl-piing. The International Journal of Robotie Research, 2003, 22(12):985-1004
[21]陈白帆.动态环境下移动机器人同时定位与建图研究:[博士学位论文]长沙:中南大学,2009
[22] S. Thrun,W. Burgard and D. Fox.Probabilistic Robotics.The MIT press, 2005
[23]张洁.基于声纳的水下机器人同时定位与地图构建技术研究:[硕士学位论文].青岛:中国海洋大学,2010
[24] S. Thrun, Y. Liu, D. Koller, A. Y. Ng, Z. Ghahramani and H. Durrant-Whyte. Simultaneous Localization Mapping with Sparse Extended Information Filters. The International Journal of Robotics Research, 2004, 23(7): 693-716
[25]苏立.室内环境下移动机器人双目视觉SLAM研究:[硕士学位论文].西安:西安理工大学,2010
[26]谭民,王硕,曹志强.多机器人系统.清华大学出版社.2005
[27]周福娜,周梅,文成林.多传感器数据融合效果分析.河南大学学报(自然科学版) 2003,33(2):33-36
[28] Thrun S, Beetz M, Burgard W, et al. Probabilistic algorithms and the interactive museumtour-guided robot Minerva. International Journal of Robotics Research, 2000, 19(11) : 972 - 1000
[29] Bo He, Ke Yang, Shuai Zhao. Underwater Simultaneous Localization and Mapping Based on EKF and Point Features. ICMA, 2009.
[30]马莹巍,孙艳霞.未知环境下移动机器人地图构建的研究发展.中国电力教育. 2009,S1
[31] David Ribas ROMAGóS. Underwear SLAM for Structured Environment Using an Imaging Sonar: [phd thesis]. Girona: Universitat of Girona, 2008
[32] Luyue Huang, Bo He, Tao Zhang, An Autonomous Navigation Algorithm for Underwater Vehicles Based on Inertial Measurement Units and Sonar, Proceedings of 2nd International Asia conf. on Informatics in Control, Automation and Robotics, March, 2010
[33] Ioseba Tena Ruiz, S’ebastien de Raucourt, Yvan Petillot and David M.Lane. Concurrent Mapping and Localization Using Sidescan Sonar. IEEE JOURNAL OF OCEANIC
[34] David Ribas, Jos’eNeira, Pere Ridao, & Tard′os, J. D., SLAM using an imaging sonar for partially structured environments. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Beijing, China, 2006.
[35] I. Tena, S. de Raucourt, Y. Petillot and D. Lane. Concurrent mapping and localization using sidescan sonar. IEEE Journal of Oceanic Engineering, 2004, 29(2): 442-456
[36] H. Durrant-Whyte and T. Bailey. Simultaneous Localization and Mapping (SLAM): Part I, the essential algorithms. Robotics and Automation,2006,13(2): 99-108
[37] Guivant, J. E. and Nebot, E. M. Optimization of the Simultaneous Localization and Map Building Algorithm for Real Time Implementation. IEEE Trans. Robot. Automat, 2001, 17(3): 242–257
[38] Leonard, J. J. and Durrant-Whyte, H. F. Directed Sonar Sensing for Mobile Robot Navigation. London: Kluwer Academic Pub, 1992
[39]赵鹏飞,徐万里,罗佳.数据关联算法综述及性能评估.武器制造. 2009,09
[40] Jing Luo, Bo He, Data Association for AUV Localization Conference on Measuring Tech. and Mechatronics Automation. 2010
[41] http://baike.baidu.com/view/79592.htm
[42] Tritech公司. Super Seaking DST Sonar技术参考资料,2007
[43] LinkQuest Inc. User Guide, 2006
[44]星网宇达公司.定位定向位姿导航产品选型手册,2008
[45]陈书海,傅录祥.实用数字图像处理.北京:科学出版社, 2005
[46]李介谷等.图像处理技术.上海:上海交通大学出版社,1988
[47] Kenneth R. Castleman, (朱志刚等译).数字图像处理.电子工业出版社,2002:58-60
[48]黄爱民,安向京,骆力等.数字图像处理与分析基础.北京:中国水利水电出版社, 2005
[49]王彪.声纳图像的处理及目标识别技术研究:[硕士学位论文].西安:西北师范大学, 2005
[50]陈亮.基于图像处理的水下目标识别方法研究:[硕士学位论文].哈尔滨:哈尔滨工程大学,2006
[51]贾云得.机器视觉.北京:科学出版社,2000
[52]李启虎.声纳信号处理引论.北京:海洋出版社,2000
[53]吴翠江.水下机器人声视觉仿真系统研究:[硕士学位论文].哈尔滨:哈尔滨工程大学,2004
[54] Shujuan Chen, Bo He, Shu Tian, Pei Song, Guijie Liu. Simultaneous Localization and Mapping in Structured Environment Based on Point Feature. 2010 3rd International Conference on Computer and Electrical Engineering, 2010,456-460
[55] David Ribas. Abandoned Marina Dataset. Experiment performed at the Fluvia Nautic abandoned marina near St Pere Pescador (Spain), 2007