基于姿态阈值滤波的单目视觉农业机械定位方法
|
摘要
精确定位是农业机械实现自动导航的重要前提,提出了一种基于单目摄像机的导航定位算法。算法通过对捕获的每帧图像进行特征点检测与跟踪,基于3D-2D特征点配对方法,还原了车辆行驶过程中的姿态信息。同时,假设车辆前方小区域近似平坦,摄像机离地高度近似固定不变,算法实时求解车辆位移的绝对尺度因子,配合姿态阈值滤波器,对所求姿态信息进行在线修正。实验选择在三种不同的场景进行,以RTK-GPS定位数据作为对比,三种场景的平均相对位置偏差分别为5.459 9%、8.373 1%、6.443 94%,三种不同场景的航向角变化平均值分别为7.717 7°、5.738 9°、3.438 3°。结果表明算法可实现农业机械的自动定位,在短距离范围内具有一定的可靠性。
Precise localization is an important premise of autonomous navigation for agricultural vehicles.We propose a localization algorithm based on monocular camera.After features are detected and tracked through multiple frames,vehicle poses are estimated based on 3 D-2 Dcorrespondences.Furthermore,the translation absolute scale is calculated based on the assumption that the ground patches are locally flat and the camera is moving at a known and fixed height over the ground.Finally,the poses are refined by the pose threshold filter.Compared with the RTK-GPS data,the average relative position errors of the three different experimental terrains are 5.459 9%,8.373 1%and 6.443 94%,and the average heading errors are 7.717 7°,5.738 9°and 3.438 3°.The results show that the algorithm is feasible for agricultural vehicles localization.
Precise localization is an important premise of autonomous navigation for agricultural vehicles.We propose a localization algorithm based on monocular camera.After features are detected and tracked through multiple frames,vehicle poses are estimated based on 3 D-2 Dcorrespondences.Furthermore,the translation absolute scale is calculated based on the assumption that the ground patches are locally flat and the camera is moving at a known and fixed height over the ground.Finally,the poses are refined by the pose threshold filter.Compared with the RTK-GPS data,the average relative position errors of the three different experimental terrains are 5.459 9%,8.373 1%and 6.443 94%,and the average heading errors are 7.717 7°,5.738 9°and 3.438 3°.The results show that the algorithm is feasible for agricultural vehicles localization.
引文
[1]Luo Xi-wen,Ou Ying-gang,Zhao Zuo-xi,et al.Research and development of intelligent flexible chassis for precision farming[J].Transactions of the Chinese Society of Agricultural Engineering(Transactions of the CSAE),2005,21(2):83-85.(in Chinese)
[2]Li M,Imou K,Wakabayashi K,et al.Review of research on agricultural vehicle autonomous guidance[J].Int J Agric&Biol Eng,2009,2(3):1-16.
[3]Subramanian V,Burks T F,Arroyo A A.Development of machine vision and laser radar based autonomous vehicle guidance systems for citrus grove navigation[J].Computers and Electronics in Agriculture,2006,53(2):130-143.
[4]Luo Xi-wen,Zhang Zhi-gang,Zhao Zuo-xi,et al.Design of DGPS navigation control system for Dongfanghong X-804tractor[J].Transactions of the Chinese Society of Agricultural Engineering(Transactions of the CSAE),2009,25(11):139-145.(in Chinese)
[5]Chen Yan,Zhang Man,Ma Wen-qiang,et al.Positioning method of integrated navigation based on GPS and machine vision[J].Transactions of the Chinese Society of Agricultural Engineering(Transactions of the CSAE),2011,27(3):126-130.(in Chinese)
[6]Zhang Hui-chun,Zheng Jia-qiang,Zhou Hong-ping.Positioning accuracy analysis of RBN DGPS applied in precision forestry[J].Transactions of the Chinese Society of Agricultural Engineering(Transactions of the CSAE),2011,27(7):210-213.(in Chinese)
[7]Wan Li-qin,Qiu Bai-jing.Experiment and study of the positioning accuracy of DGPS in precision agriculture[J].Journal of Agricultural Mechanization Research,2003(1):115-117.(in Chinese)
[8]Liu Pei,Chen Jun,Zhang Ming-ying.Automatic control system of orchard tractor based on laser navigation[J].Transactions of the Chinese Society of Agricultural Engineering(Transactions of the CSAE),2011,27(3):196-199.(in Chinese)
[9]Wang Ling,Liu Si-yao,Lu Wei,et al.Laser detection method for cotton orientation in robotic cotton picking[J].Transactions of the Chinese Society of Agricultural Engineering(Transactions of the CSAE),2014,30(14):42-47.(in Chinese)
[10]Feng Juan,Liu Gang,Si Yong-sheng,et al.Construction of laser vision system for apple harvesting robot[J].Transactions of the Chinese Society of Agricultural Engineering(Transactions of the CSAE),2013,29(1):32-36.(in Chinese)
[11]Xue Jin-lin,Zhang Shun-shun.Navigation of an agricultural robot based on laser radar[J].Transactions of the Chinese Society for Agricultural Machinery,2014,45(9):55-60.(in Chinese)
[12]Li Yong-jian.Robot Monto Carlo self-localization method based on combination of vision sensors and laser range finder[J].Transactions of the Chinese Society for Agricultural Machinery,2012,43(1):170-174.(in Chinese)
[13]Feng Juan,Liu Gang,Si Yong-sheng,et al.Algorithm based on image processing technology to generate navigation directrix in orchard[J].Transactions of the Chinese Society for Agricultural Machinery,2012,43(7):185-189.(in Chinese)
[14]Wu Jia-yi,Yang Qing-hua,Bao Guan-jun,et al.Algorithm of path navigation line for robot in forestry environment based on machine vision[J].Transactions of the Chinese Society for Agricultural Machinery,2009,40(7):176-179.(in Chinese)
[15]Xiong Jun-tao,Zou Xiang-jun,Chen Li-juan,et al.Visual position of picking manipulator for disturbed litchi[J].Transactions of the Chinese Society of Agricultural Engineering(Transactions of the CSAE),2012,28(14):36-41.(in Chinese)
[16]Jiang Huan-yu,Peng Yong-shi,Shen Chuan,et al.Recognizing and locating ripe tomatoes based on binocular stereo vision technology[J].Transactions of the Chinese Society of Agricultural Engineering(Transactions of the CSAE),2008,24(8):279-283.(in Chinese)
[17]Zhou Jun,Chen Qin,Liang Quan.Vision navigation of agricultural mobile robot based on reinforcement learning[J].Transactions of the Chinese Society for Agricultural Machinery,2014,45(2):53-58.(in Chinese)
[18]Zhang Hao,Chen Yong,Wang Wei,et al.Positioning method for tea picking using active computer vision[J].Transactions of the Chinese Society for Agricultural Machinery,2014,45(9):61-65.(in Chinese)
[19]Meng Qing-kuan,Zhang Man,Qiu Rui-cheng,et al.Navigation line detection for farm machinery based on improved genetic algorithm[J].Transactions of the Chinese Society for Agricultural Machinery,2014,45(10):39-46.(in Chinese)
[20]Li Yu-bo,Zhu Xiao-zhou,Lu Hui-min,et al.Review on visual odometry technology[J].Application Research of Computers,2012,29(8):2801-2804.(in Chinese)
[21]Nister D,Naroditsky O,Bergen J.Visual odometry[C]∥Proc of IEEE International on Conference on Computer Vision and Pattern Recognition,2004:652-659.
[22]Olson C,Matthies L,Schoppers M,et al.Robust stereo egomotion for long distance navigation[C]∥Proc of IEEE International Conference on Computer Vision and Pattern Recognition,2000:453-458.
[23]Sünderhauf N,Konolige K,Lacroix S,et al.Visual odometry using sparse bundle adjustment on an autonomous outdoor vehicle[C]∥Proc of Autonome Mobile System,2005:157-163.
[24]Zhang J,Singh S,Kantor G.Robust monocular visual odometry for a ground vehicle in undulating terrain[C]∥Proc of the 8th International Conference on Field and Service Robots(FSR),2014:311-326.
[25]Xu Yun-xi,Xiang Zhi-yu,Liu Ji-lin.Initial motion estimation and inliers detection based on particle swarm optimization for stereo odometry[J].Control Theory&Applications,2015,32(1):93-100.(in Chinese)
[26]Wang Ya-long,Zhang Qi-zhi,Zhou Ya-li.Design of 3Dvisual odometry based on Kinect[J].Journal of Computer Applications,2014,34(8):2371-2374.(in Chinese)
[27]Xia Ling-nan,Zhang Bo,Wang Ying-guan,et al.Robot localization algorithm based on inertial sensor and video odometry[J].Chinese Journal of Scientific Instrument,2013,34(1):166-172.(in Chinese)
[28]Scaramuzza D,Fraundorfer F,Siegwart R.Real-time monocular visual odometry for on-road vehicles with 1-point RANSAC[C]∥Proc of IEEE International Conference on Robotics and Automation,2009:4293-4299.
[29]Scaramuzza D,Siegwart R.Appearance-guided monocular omnidirectional visual odometry for outdoor ground vehicles[J].IEEE Transactions on Robotics,2008,24(5):1015-1026.
[30]Neubeck A,Gool L V.Efficient non-maximum suppression[C]∥Proc of ICPR,2006:850-855.
[31]Longuet-Higgins H.A computer algorithm for reconstructing a scene from two projections[J].Nature,1981,293(10):133-135.
[32]Shewchuk J R.Triangle:Engineering a 2Dquality mesh generator and delaunay triangualtor[M]∥Applied Computational Geometry Towards Geometric Engineering.Berlin:Springer,2012:203-222.
[33]Moreno-Noguer F,Lepetit V,Fua P.Accurate non-iterative O(n)solution to the PnP problem[C]∥Proc of IEEE International Conference on Computer Vision,2007:1-8.
[34]SBA:A generic sparse bundle adjustment C/C++package based on the Levenberg-Marquardt algorithm[CP/OL].[2015-03-17].http://users.ics.forth.gr/~lourakis/sba/.
[35]Kitt B,Rehder J,Chambers A,et al.Monocular visual odometry using aplanar road model to solve scale ambiguity[C]∥Proc of European Conference on Mobile Robots,2011:43-48.
[36]Damien’s Matlab library[CP/OL].[2015-03-17].http://www.montefiore.ulg.ac.be/~dteney/dml.htm.
[1]罗锡文,区颖刚,赵祚喜,等.农业智能作业平台的模型的研制[J].农业工程学报,2005,21(2):83-85.
[4]罗锡文,张智刚,赵祚喜,等.东方红X-804拖拉机的DGPS自动导航控制系统[J].农业工程学报,2009,25(11):139-145.
[5]陈艳,张漫,马文强,等.基于GPS和机器视觉的组合导航定位方法[J].农业工程学报,2011,27(3):126-130.
[6]张慧春,郑加强,周宏平.精确林业GPS信标差分定位精度分析[J].农业工程学报,2011,27(7):210-213.
[7]万丽芹,邱白晶.GPS在精确农业应用中定位精度的实验研究[J].农机化研究,2003(1):115-117.
[8]刘沛,陈军,张明颖.基于激光导航的果园拖拉机自动控制系统[J].农业工程学报,2011,27(3):196-199.
[9]王玲,刘思瑶,卢伟,等.面向采摘机器人的棉花激光定位算法[J].农业工程学报,2014,30(14):42-47.
[10]冯娟,刘刚,司永胜,等.苹果采摘机器人激光视觉系统的构建[J].农业工程学报,2013,29(1):32-36.
[11]薛金林,张顺顺.基于激光雷达的农业机器人导航控制研究[J].农业机械学报,2014,45(9):55-60.
[12]李永坚.融合视觉和激光测距的机器人Monte Carlo自定位方法[J].农业机械学报,2012,43(1):170-174.
[13]冯娟,刘刚,司永胜,等.果园视觉导航基准线生成算法[J].农业机械学报,2012,43(7):185-189.
[14]吴佳艺,杨庆华,鲍官军,等.基于机器视觉的林间导航路径生成算法[J].农业机械学报,2009,40(7):176-179.
[15]熊俊涛,邹湘军,陈丽娟,等.采摘机械手对扰动荔枝的视觉定位[J].农业工程学报,2012,28(14):36-41.
[16]蒋焕煜,彭永石,申川,等.基于双目立体视觉技术的成熟番茄识别与定位[J].农业工程学报,2008,24(8):279-283.
[17]周俊,陈钦,梁泉.基于强化学习的农业移动机器人视觉导航[J].农业机械学报,2014,45(2):53-58.
[18]张浩,陈勇,汪巍,等.基于主动计算机视觉的茶叶采摘定位技术[J].农业机械学报,2014,45(9):61-65.
[19]孟庆宽,张漫,仇瑞承,等.基于改进遗传算法的农机具视觉导航线检测[J].农业机械学报,2014,45(10):39-46.
[20]李宇波,朱效洲,卢惠民,等.视觉里程计技术综述[J].计算机应用研究.2012,29(8):2801-2805.
[25]许允喜,项志宇,刘济林.立体视觉里程计中基于微粒群优化的初始运动估计和内点检测[J].控制理论与应用,2015,32(1):93-100.
[26]王亚龙,张奇志,周亚丽.基于Kinect的三维视觉里程计的设计[J].计算机应用,2014,34(8):2371-2374.
[27]夏凌楠,张波,王营冠,等.基于惯性传感器和视觉里程计的机器人定位[J].仪器仪表学报,2013,34(1):166-172.
[2]Li M,Imou K,Wakabayashi K,et al.Review of research on agricultural vehicle autonomous guidance[J].Int J Agric&Biol Eng,2009,2(3):1-16.
[3]Subramanian V,Burks T F,Arroyo A A.Development of machine vision and laser radar based autonomous vehicle guidance systems for citrus grove navigation[J].Computers and Electronics in Agriculture,2006,53(2):130-143.
[4]Luo Xi-wen,Zhang Zhi-gang,Zhao Zuo-xi,et al.Design of DGPS navigation control system for Dongfanghong X-804tractor[J].Transactions of the Chinese Society of Agricultural Engineering(Transactions of the CSAE),2009,25(11):139-145.(in Chinese)
[5]Chen Yan,Zhang Man,Ma Wen-qiang,et al.Positioning method of integrated navigation based on GPS and machine vision[J].Transactions of the Chinese Society of Agricultural Engineering(Transactions of the CSAE),2011,27(3):126-130.(in Chinese)
[6]Zhang Hui-chun,Zheng Jia-qiang,Zhou Hong-ping.Positioning accuracy analysis of RBN DGPS applied in precision forestry[J].Transactions of the Chinese Society of Agricultural Engineering(Transactions of the CSAE),2011,27(7):210-213.(in Chinese)
[7]Wan Li-qin,Qiu Bai-jing.Experiment and study of the positioning accuracy of DGPS in precision agriculture[J].Journal of Agricultural Mechanization Research,2003(1):115-117.(in Chinese)
[8]Liu Pei,Chen Jun,Zhang Ming-ying.Automatic control system of orchard tractor based on laser navigation[J].Transactions of the Chinese Society of Agricultural Engineering(Transactions of the CSAE),2011,27(3):196-199.(in Chinese)
[9]Wang Ling,Liu Si-yao,Lu Wei,et al.Laser detection method for cotton orientation in robotic cotton picking[J].Transactions of the Chinese Society of Agricultural Engineering(Transactions of the CSAE),2014,30(14):42-47.(in Chinese)
[10]Feng Juan,Liu Gang,Si Yong-sheng,et al.Construction of laser vision system for apple harvesting robot[J].Transactions of the Chinese Society of Agricultural Engineering(Transactions of the CSAE),2013,29(1):32-36.(in Chinese)
[11]Xue Jin-lin,Zhang Shun-shun.Navigation of an agricultural robot based on laser radar[J].Transactions of the Chinese Society for Agricultural Machinery,2014,45(9):55-60.(in Chinese)
[12]Li Yong-jian.Robot Monto Carlo self-localization method based on combination of vision sensors and laser range finder[J].Transactions of the Chinese Society for Agricultural Machinery,2012,43(1):170-174.(in Chinese)
[13]Feng Juan,Liu Gang,Si Yong-sheng,et al.Algorithm based on image processing technology to generate navigation directrix in orchard[J].Transactions of the Chinese Society for Agricultural Machinery,2012,43(7):185-189.(in Chinese)
[14]Wu Jia-yi,Yang Qing-hua,Bao Guan-jun,et al.Algorithm of path navigation line for robot in forestry environment based on machine vision[J].Transactions of the Chinese Society for Agricultural Machinery,2009,40(7):176-179.(in Chinese)
[15]Xiong Jun-tao,Zou Xiang-jun,Chen Li-juan,et al.Visual position of picking manipulator for disturbed litchi[J].Transactions of the Chinese Society of Agricultural Engineering(Transactions of the CSAE),2012,28(14):36-41.(in Chinese)
[16]Jiang Huan-yu,Peng Yong-shi,Shen Chuan,et al.Recognizing and locating ripe tomatoes based on binocular stereo vision technology[J].Transactions of the Chinese Society of Agricultural Engineering(Transactions of the CSAE),2008,24(8):279-283.(in Chinese)
[17]Zhou Jun,Chen Qin,Liang Quan.Vision navigation of agricultural mobile robot based on reinforcement learning[J].Transactions of the Chinese Society for Agricultural Machinery,2014,45(2):53-58.(in Chinese)
[18]Zhang Hao,Chen Yong,Wang Wei,et al.Positioning method for tea picking using active computer vision[J].Transactions of the Chinese Society for Agricultural Machinery,2014,45(9):61-65.(in Chinese)
[19]Meng Qing-kuan,Zhang Man,Qiu Rui-cheng,et al.Navigation line detection for farm machinery based on improved genetic algorithm[J].Transactions of the Chinese Society for Agricultural Machinery,2014,45(10):39-46.(in Chinese)
[20]Li Yu-bo,Zhu Xiao-zhou,Lu Hui-min,et al.Review on visual odometry technology[J].Application Research of Computers,2012,29(8):2801-2804.(in Chinese)
[21]Nister D,Naroditsky O,Bergen J.Visual odometry[C]∥Proc of IEEE International on Conference on Computer Vision and Pattern Recognition,2004:652-659.
[22]Olson C,Matthies L,Schoppers M,et al.Robust stereo egomotion for long distance navigation[C]∥Proc of IEEE International Conference on Computer Vision and Pattern Recognition,2000:453-458.
[23]Sünderhauf N,Konolige K,Lacroix S,et al.Visual odometry using sparse bundle adjustment on an autonomous outdoor vehicle[C]∥Proc of Autonome Mobile System,2005:157-163.
[24]Zhang J,Singh S,Kantor G.Robust monocular visual odometry for a ground vehicle in undulating terrain[C]∥Proc of the 8th International Conference on Field and Service Robots(FSR),2014:311-326.
[25]Xu Yun-xi,Xiang Zhi-yu,Liu Ji-lin.Initial motion estimation and inliers detection based on particle swarm optimization for stereo odometry[J].Control Theory&Applications,2015,32(1):93-100.(in Chinese)
[26]Wang Ya-long,Zhang Qi-zhi,Zhou Ya-li.Design of 3Dvisual odometry based on Kinect[J].Journal of Computer Applications,2014,34(8):2371-2374.(in Chinese)
[27]Xia Ling-nan,Zhang Bo,Wang Ying-guan,et al.Robot localization algorithm based on inertial sensor and video odometry[J].Chinese Journal of Scientific Instrument,2013,34(1):166-172.(in Chinese)
[28]Scaramuzza D,Fraundorfer F,Siegwart R.Real-time monocular visual odometry for on-road vehicles with 1-point RANSAC[C]∥Proc of IEEE International Conference on Robotics and Automation,2009:4293-4299.
[29]Scaramuzza D,Siegwart R.Appearance-guided monocular omnidirectional visual odometry for outdoor ground vehicles[J].IEEE Transactions on Robotics,2008,24(5):1015-1026.
[30]Neubeck A,Gool L V.Efficient non-maximum suppression[C]∥Proc of ICPR,2006:850-855.
[31]Longuet-Higgins H.A computer algorithm for reconstructing a scene from two projections[J].Nature,1981,293(10):133-135.
[32]Shewchuk J R.Triangle:Engineering a 2Dquality mesh generator and delaunay triangualtor[M]∥Applied Computational Geometry Towards Geometric Engineering.Berlin:Springer,2012:203-222.
[33]Moreno-Noguer F,Lepetit V,Fua P.Accurate non-iterative O(n)solution to the PnP problem[C]∥Proc of IEEE International Conference on Computer Vision,2007:1-8.
[34]SBA:A generic sparse bundle adjustment C/C++package based on the Levenberg-Marquardt algorithm[CP/OL].[2015-03-17].http://users.ics.forth.gr/~lourakis/sba/.
[35]Kitt B,Rehder J,Chambers A,et al.Monocular visual odometry using aplanar road model to solve scale ambiguity[C]∥Proc of European Conference on Mobile Robots,2011:43-48.
[36]Damien’s Matlab library[CP/OL].[2015-03-17].http://www.montefiore.ulg.ac.be/~dteney/dml.htm.
[1]罗锡文,区颖刚,赵祚喜,等.农业智能作业平台的模型的研制[J].农业工程学报,2005,21(2):83-85.
[4]罗锡文,张智刚,赵祚喜,等.东方红X-804拖拉机的DGPS自动导航控制系统[J].农业工程学报,2009,25(11):139-145.
[5]陈艳,张漫,马文强,等.基于GPS和机器视觉的组合导航定位方法[J].农业工程学报,2011,27(3):126-130.
[6]张慧春,郑加强,周宏平.精确林业GPS信标差分定位精度分析[J].农业工程学报,2011,27(7):210-213.
[7]万丽芹,邱白晶.GPS在精确农业应用中定位精度的实验研究[J].农机化研究,2003(1):115-117.
[8]刘沛,陈军,张明颖.基于激光导航的果园拖拉机自动控制系统[J].农业工程学报,2011,27(3):196-199.
[9]王玲,刘思瑶,卢伟,等.面向采摘机器人的棉花激光定位算法[J].农业工程学报,2014,30(14):42-47.
[10]冯娟,刘刚,司永胜,等.苹果采摘机器人激光视觉系统的构建[J].农业工程学报,2013,29(1):32-36.
[11]薛金林,张顺顺.基于激光雷达的农业机器人导航控制研究[J].农业机械学报,2014,45(9):55-60.
[12]李永坚.融合视觉和激光测距的机器人Monte Carlo自定位方法[J].农业机械学报,2012,43(1):170-174.
[13]冯娟,刘刚,司永胜,等.果园视觉导航基准线生成算法[J].农业机械学报,2012,43(7):185-189.
[14]吴佳艺,杨庆华,鲍官军,等.基于机器视觉的林间导航路径生成算法[J].农业机械学报,2009,40(7):176-179.
[15]熊俊涛,邹湘军,陈丽娟,等.采摘机械手对扰动荔枝的视觉定位[J].农业工程学报,2012,28(14):36-41.
[16]蒋焕煜,彭永石,申川,等.基于双目立体视觉技术的成熟番茄识别与定位[J].农业工程学报,2008,24(8):279-283.
[17]周俊,陈钦,梁泉.基于强化学习的农业移动机器人视觉导航[J].农业机械学报,2014,45(2):53-58.
[18]张浩,陈勇,汪巍,等.基于主动计算机视觉的茶叶采摘定位技术[J].农业机械学报,2014,45(9):61-65.
[19]孟庆宽,张漫,仇瑞承,等.基于改进遗传算法的农机具视觉导航线检测[J].农业机械学报,2014,45(10):39-46.
[20]李宇波,朱效洲,卢惠民,等.视觉里程计技术综述[J].计算机应用研究.2012,29(8):2801-2805.
[25]许允喜,项志宇,刘济林.立体视觉里程计中基于微粒群优化的初始运动估计和内点检测[J].控制理论与应用,2015,32(1):93-100.
[26]王亚龙,张奇志,周亚丽.基于Kinect的三维视觉里程计的设计[J].计算机应用,2014,34(8):2371-2374.
[27]夏凌楠,张波,王营冠,等.基于惯性传感器和视觉里程计的机器人定位[J].仪器仪表学报,2013,34(1):166-172.