移动机器人视觉动态定位的稳健高斯混合模型
|
摘要
针对动态场景中运动路标点严重影响传统视觉自主定位算法精度,甚至产生定位失效的问题,提出一种顾及动态路标点的稳健高斯混合模型。在传统图优化视觉定位模型的基础上,增加"运动指数"描述图优化模型中路标点的运动概率,把传统图优化高斯模型增强为高斯混合模型,以约束运动路标点对图优化结果的影响;为增强模型对噪声的稳健性,采用方差膨胀模型约束残差方程;详细推导了该高斯混合模型的期望-最大化求解方法,把该问题转化为经典迭代最小二乘问题进行解算。仿真试验和真实数据试验表明:强动态场景中,提出的算法绝对精度指标和相对精度指标均优于传统优化算法;静态或弱动态场景中,提出的算法仍与传统优化算法定位性能相当。本文方法可有效减小场景中运动路标点对优化结果的影响,更适用于移动机器人的自主定位。
In dynamic environments,the moving landmarks can make the accuracy of traditional vision-based localization worse or even failure.To solve this problem,a robust Gaussian mixture model for vision-based localization with dynamic landmarks is proposed.The motion index is added to the traditional graph-based visionbased localization model to describe landmarks' moving probability,changing the classic Gaussian model to Gaussian mixture model,which can reduce the influence of moving landmarks for optimization results.To improve the algorithm's robustness to noise,the covariance inflation model is employed in residual equations.The expectation maximization method for solving the Gaussian mixture problem is derived in detail,transforming the problem into classic iterative least square problem.Experimental results demonstrate that in dynamic environments,the proposed algorithm outperforms the traditional method both in absolute accuracy and relative accuracy,while maintains high accuracy in static environments.The proposed method can effectively reduce the influence of the moving landmarks in dynamic environments,which is more suitable for the autonomous localization of mobile robots.
In dynamic environments,the moving landmarks can make the accuracy of traditional vision-based localization worse or even failure.To solve this problem,a robust Gaussian mixture model for vision-based localization with dynamic landmarks is proposed.The motion index is added to the traditional graph-based visionbased localization model to describe landmarks' moving probability,changing the classic Gaussian model to Gaussian mixture model,which can reduce the influence of moving landmarks for optimization results.To improve the algorithm's robustness to noise,the covariance inflation model is employed in residual equations.The expectation maximization method for solving the Gaussian mixture problem is derived in detail,transforming the problem into classic iterative least square problem.Experimental results demonstrate that in dynamic environments,the proposed algorithm outperforms the traditional method both in absolute accuracy and relative accuracy,while maintains high accuracy in static environments.The proposed method can effectively reduce the influence of the moving landmarks in dynamic environments,which is more suitable for the autonomous localization of mobile robots.
引文
[1] PERSSON M,PICCINI T,FELSBERG M,et al.Robust Stereo Visual Odometry from Monocular Techniques[C]∥Proceedings of 2015IEEE Intelligent Vehicles Symposium.Seoul:IEEE,2015:686-691.
[2]黄楠楠,刘贵喜,张音哲,等.无人机视觉导航算法[J].红外与激光工程,2016,45(7):269-277.HUANG Nannan,LIU Guixi,ZHANG Yinzhe,et al.Unmanned Aerial Vehicle Vision Navigation Algorithm[J].Infrared and Laser Engineering,2016,45(7):269-277.
[3] WU Meiqing,LAM S K,SRIKANTHAN T.A Framework for Fast and Robust Visual Odometry[J].IEEE Transactions on Intelligent Transportation Systems,2017,18(12):3433-3448.
[4] QU Xiaozhi,SOHEILIAN B,PAPARODITIS N.Landmark Based Localization in Urban Environment[J].ISPRS Journal of Photogrammetry and Remote Sensing,2018,140:90-103.
[5]陈锐志,陈亮.基于智能手机的室内定位技术的发展现状和挑战[J].测绘学报,2017,46(10):1316-1326.DOI:10.11947/j.AGCS.2017.20170383.CHEN Ruizhi,CHEN Liang.Indoor Positioning with Smartphones:The State-of-the-art and the Challenges[J].Acta Geodaetica et Cartographica Sinica,2017,46(10):1316-1326.DOI:10.11947/j.AGCS.2017.20170383.
[6] STRASDAT H,DAVISON A J,MONTIEL J M M,et al.Double Window Optimisation for Constant Time Visual SLAM[C]∥Proceedings of 2011International Conference on Computer Vision.Barcelona,Spain:IEEE,2011:2352-2359.
[7] MUR-ARTAL6)R,MONTIEL J M M,TARDS J D.ORBSLAM:A Versatile and Accurate Monocular SLAM System[J].IEEE Transactions on Robotics,2015,31(5):1147-1163.
[8] CADENA C,CARLONE L,CARRILLO H,et al.Past,Present,and Future of Simultaneous Localization and Mapping:Toward the Robust-perception Age[J].IEEE Transactions on Robotics,2016,32(6):1309-1332.
[9] CARLONE L,KIRA Z,BEALL C,et al.Eliminating Conditionally Independent Sets in Factor Graphs:A Unifying Perspective Based on Smart Factors[C]∥Proceedings of2014 IEEE International Conference on Robotics and Automation.Hong Kong,China:IEEE,2014:4290-4297.
[10] KAESS M,JOHANNSSON H,ROBERTS R,et al.iSAM2:Incremental Smoothing and Mapping with Fluid Relinearization and Incremental Variable Reordering[C]∥Proceedings of 2011IEEE International Conference on Robotics and Automation.Shanghai,China:IEEE,2011:3281-3288.
[11] KMMERLE R,GRISETTI G,STRASDAT H,et al.G2o:A General Framework for Graph Optimization[C]∥Proceedings of 2011IEEE International Conference on Robotics and Automation.Shanghai,China:IEEE,2011:3607-3613.
[12] SONG Shiyu,CHANDRAKER M,GUEST C C.Parallel,Real-time Monocular Visual Odometry[C]∥Proceedings of 2013IEEE International Conference on Robotics and Automation.Karlsruhe,Germany:IEEE,2013:4698-4705.
[13] ENGEL J,STCKLER J,CREMERS D.Large-scale Direct SLAM with Stereo Cameras[C]∥Proceedings of 2015IEEE/RSJ International Conference on Intelligent Robots and Systems.Hamburg:IEEE,2015.
[14]刘浩敏,章国锋,鲍虎军.基于单目视觉的同时定位与地图构建方法综述[J].计算机辅助设计与图形学学报,2016,28(6):855-868.LIU Haomin,ZHANG Guofeng,BAO Hujun.A Survey of Monocular Simultaneous Localization and Mapping[J].Journal of Computer-aided Design and Computer Graphics,2016,28(6):855-868.
[15]曹天扬,蔡浩原,方东明,等.结合图像内容匹配的机器人视觉导航定位与全局地图构建系统[J].光学精密工程,2017,25(8):2221-2232.CAO Tianyang,CAI Haoyuan,FANG Dongming,et al.Robot Vision System for Keyframe Global Map Establishment and Robot Localization Based on Graphic Content Matching[J].Optics and Precision Engineering,2017,25(8):2221-2232.
[16] PERERA S,PASQUAL A.Towards Realtime Handheld Mono SLAM in Dynamic Environments[C]∥Proceedings of the 7th International Symposium on Advances in Visual Computing.Las Vegas,NV:Springer,2011:313-324.
[17] TAN Wei,LIU Haomin,DONG Zilong,et al.Robust Monocular SLAM in Dynamic Environments[C]∥Proceedings of 2013IEEE International Symposium on Mixed and Augmented Reality.Adelaide,SA,Australia:IEEE,2013:209-218.
[18] WANG C C,THORPE C,THRUN S.Online Simultaneous Localization and Mapping with Detection and Tracking of Moving Objects:Theory and Results from a Ground Vehicle in Crowded Urban Areas[C]∥Proceedings of 2003IEEE International Conference on Robotics and Automation.Taipei,Taiwan,China:IEEE,2003:842-849.
[19] HOCHDORFER S,NEUMANN H,SCHLEGEL C.Landmark Rating and Selection for SLAM in Dynamic Environments[C]∥Proceedings of the 13th International Conference on Intelligent Autonomous Systems 13.Cham:Springer,2014:401-414.
[20] AGARWAL P,TIPALDI G D,SPINELLO L,et al.Robust Map Optimization Using Dynamic Covariance Scaling[C]∥Proceedings of 2013IEEE International Conference on Robotics and Automation.Karlsruhe,Germany:IEEE,2013:62-69.
[21] OLSON E,AGARWAL P.Inference on Networks of Mixtures for Robust Robot Mapping[J].The International Journal of Robotics Research,2013,32(7):826-840.
[22] SNDERHAUF N,PROTZEL P.Switchable Constraints for Robust Pose Graph SLAM[C]∥Proceedings of 2012IEEE/RSJ International Conference on Intelligent Robots and Systems.Vilamoura,Portugal:IEEE,2012:1879-1884.
[23] ROGERS J G,TREVOR A J B,NIETO-GRANDA C,et al.SLAM with Expectation Maximization for Moveable Object Tracking[C]∥Proceedings of 2010IEEE/RSJ International Conference on Intelligent Robots and Systems.Taipei,Taiwan,China:IEEE,2010:2077-2082.
[24] KAESS M,RANGANATHAN A,DELLAERT F.iSAM:Incremental SmoothingandMapping[J]. IEEE Transactions on Robotics,2008,24(6):1365-1378.
[25] GEIGER A,LENZ P,URTASUN R.Are We Ready for Autonomous Driving?The KITTI Vision Benchmark Suite[C]∥Proceedings of 2012IEEE Conference on Computer Vision and Pattern Recognition.Providence,RI:IEEE,2012:3354-3361.
[2]黄楠楠,刘贵喜,张音哲,等.无人机视觉导航算法[J].红外与激光工程,2016,45(7):269-277.HUANG Nannan,LIU Guixi,ZHANG Yinzhe,et al.Unmanned Aerial Vehicle Vision Navigation Algorithm[J].Infrared and Laser Engineering,2016,45(7):269-277.
[3] WU Meiqing,LAM S K,SRIKANTHAN T.A Framework for Fast and Robust Visual Odometry[J].IEEE Transactions on Intelligent Transportation Systems,2017,18(12):3433-3448.
[4] QU Xiaozhi,SOHEILIAN B,PAPARODITIS N.Landmark Based Localization in Urban Environment[J].ISPRS Journal of Photogrammetry and Remote Sensing,2018,140:90-103.
[5]陈锐志,陈亮.基于智能手机的室内定位技术的发展现状和挑战[J].测绘学报,2017,46(10):1316-1326.DOI:10.11947/j.AGCS.2017.20170383.CHEN Ruizhi,CHEN Liang.Indoor Positioning with Smartphones:The State-of-the-art and the Challenges[J].Acta Geodaetica et Cartographica Sinica,2017,46(10):1316-1326.DOI:10.11947/j.AGCS.2017.20170383.
[6] STRASDAT H,DAVISON A J,MONTIEL J M M,et al.Double Window Optimisation for Constant Time Visual SLAM[C]∥Proceedings of 2011International Conference on Computer Vision.Barcelona,Spain:IEEE,2011:2352-2359.
[7] MUR-ARTAL6)R,MONTIEL J M M,TARDS J D.ORBSLAM:A Versatile and Accurate Monocular SLAM System[J].IEEE Transactions on Robotics,2015,31(5):1147-1163.
[8] CADENA C,CARLONE L,CARRILLO H,et al.Past,Present,and Future of Simultaneous Localization and Mapping:Toward the Robust-perception Age[J].IEEE Transactions on Robotics,2016,32(6):1309-1332.
[9] CARLONE L,KIRA Z,BEALL C,et al.Eliminating Conditionally Independent Sets in Factor Graphs:A Unifying Perspective Based on Smart Factors[C]∥Proceedings of2014 IEEE International Conference on Robotics and Automation.Hong Kong,China:IEEE,2014:4290-4297.
[10] KAESS M,JOHANNSSON H,ROBERTS R,et al.iSAM2:Incremental Smoothing and Mapping with Fluid Relinearization and Incremental Variable Reordering[C]∥Proceedings of 2011IEEE International Conference on Robotics and Automation.Shanghai,China:IEEE,2011:3281-3288.
[11] KMMERLE R,GRISETTI G,STRASDAT H,et al.G2o:A General Framework for Graph Optimization[C]∥Proceedings of 2011IEEE International Conference on Robotics and Automation.Shanghai,China:IEEE,2011:3607-3613.
[12] SONG Shiyu,CHANDRAKER M,GUEST C C.Parallel,Real-time Monocular Visual Odometry[C]∥Proceedings of 2013IEEE International Conference on Robotics and Automation.Karlsruhe,Germany:IEEE,2013:4698-4705.
[13] ENGEL J,STCKLER J,CREMERS D.Large-scale Direct SLAM with Stereo Cameras[C]∥Proceedings of 2015IEEE/RSJ International Conference on Intelligent Robots and Systems.Hamburg:IEEE,2015.
[14]刘浩敏,章国锋,鲍虎军.基于单目视觉的同时定位与地图构建方法综述[J].计算机辅助设计与图形学学报,2016,28(6):855-868.LIU Haomin,ZHANG Guofeng,BAO Hujun.A Survey of Monocular Simultaneous Localization and Mapping[J].Journal of Computer-aided Design and Computer Graphics,2016,28(6):855-868.
[15]曹天扬,蔡浩原,方东明,等.结合图像内容匹配的机器人视觉导航定位与全局地图构建系统[J].光学精密工程,2017,25(8):2221-2232.CAO Tianyang,CAI Haoyuan,FANG Dongming,et al.Robot Vision System for Keyframe Global Map Establishment and Robot Localization Based on Graphic Content Matching[J].Optics and Precision Engineering,2017,25(8):2221-2232.
[16] PERERA S,PASQUAL A.Towards Realtime Handheld Mono SLAM in Dynamic Environments[C]∥Proceedings of the 7th International Symposium on Advances in Visual Computing.Las Vegas,NV:Springer,2011:313-324.
[17] TAN Wei,LIU Haomin,DONG Zilong,et al.Robust Monocular SLAM in Dynamic Environments[C]∥Proceedings of 2013IEEE International Symposium on Mixed and Augmented Reality.Adelaide,SA,Australia:IEEE,2013:209-218.
[18] WANG C C,THORPE C,THRUN S.Online Simultaneous Localization and Mapping with Detection and Tracking of Moving Objects:Theory and Results from a Ground Vehicle in Crowded Urban Areas[C]∥Proceedings of 2003IEEE International Conference on Robotics and Automation.Taipei,Taiwan,China:IEEE,2003:842-849.
[19] HOCHDORFER S,NEUMANN H,SCHLEGEL C.Landmark Rating and Selection for SLAM in Dynamic Environments[C]∥Proceedings of the 13th International Conference on Intelligent Autonomous Systems 13.Cham:Springer,2014:401-414.
[20] AGARWAL P,TIPALDI G D,SPINELLO L,et al.Robust Map Optimization Using Dynamic Covariance Scaling[C]∥Proceedings of 2013IEEE International Conference on Robotics and Automation.Karlsruhe,Germany:IEEE,2013:62-69.
[21] OLSON E,AGARWAL P.Inference on Networks of Mixtures for Robust Robot Mapping[J].The International Journal of Robotics Research,2013,32(7):826-840.
[22] SNDERHAUF N,PROTZEL P.Switchable Constraints for Robust Pose Graph SLAM[C]∥Proceedings of 2012IEEE/RSJ International Conference on Intelligent Robots and Systems.Vilamoura,Portugal:IEEE,2012:1879-1884.
[23] ROGERS J G,TREVOR A J B,NIETO-GRANDA C,et al.SLAM with Expectation Maximization for Moveable Object Tracking[C]∥Proceedings of 2010IEEE/RSJ International Conference on Intelligent Robots and Systems.Taipei,Taiwan,China:IEEE,2010:2077-2082.
[24] KAESS M,RANGANATHAN A,DELLAERT F.iSAM:Incremental SmoothingandMapping[J]. IEEE Transactions on Robotics,2008,24(6):1365-1378.
[25] GEIGER A,LENZ P,URTASUN R.Are We Ready for Autonomous Driving?The KITTI Vision Benchmark Suite[C]∥Proceedings of 2012IEEE Conference on Computer Vision and Pattern Recognition.Providence,RI:IEEE,2012:3354-3361.