基于改进A*算法的果园移动机器人建图定位与路径规划方法与试验

doi:10.13733/j.jcam.issn.20955553.2022.01.021

摘要/Abstract

摘要： 为实现果园移动机器人室外自主导航，对机器人建图定位和路径规划进行研究。在建图与定位方面，提出一种紧耦合激光—惯性里程计方法，通过匹配提取到的关键帧点云线面特征来完成机器人的位姿估计与地图构建，将处理过的不同传感器的测量信息融入因子图进行优化得到全局一致位姿，运用滑动窗口的方法保证系统实时性，对历史关键帧进行边缘化处理以保留位姿约束关系。在路径规划方面，引入启发式函数权重系数，减小A*算法本身的贪心程度来避免得到路径次优解，使用DWA局部规划方法实现动态环境下的避障功能。为验证算法可靠性，在实际果园中对机器人进行试验。试验结果表明，横向偏差与纵向偏差平均值不超过10 cm，航向误差平均值不超过10°，满足果园机器人自主导航需求。

关键词: 果园机器人, 自主导航, 多传感器融合, 激光雷达, SLAM, 路径规划

Abstract: To realize autonomous outdoor navigation for mobile robots in orchards, robot map building and localization and path planning were investigated. A tightly coupled laserinertial odometer method was proposed for mapping and positioning. The pose estimation and map construction of the robot were completed by matching the line and plane features extracted from the keyframe point cloud. The measurement information of different sensors processed was integrated into the factor graph for optimization to obtain the consistent global pose. The sliding window method was used to ensure the realtime performance of the system, and the historical keyframes were marginalized to preserve the pose constraint relationship. In the aspect of path planning, the weight coefficient of the heuristic function was introduced to reduce the greedy degree of the A* algorithm to avoid obtaining suboptimal path solutions. The DWA local planning method was used to realize the obstacle avoidance function in a dynamic environment. To verify the reliability of the algorithm, the robot was tested in a real orchard. The test results showed that the average of lateral deviation and longitudinal deviation does not exceed 10 cm, and the average of heading error does not exceed 10°, which meets the demand of autonomous navigation of the orchard robot.

Key words: orchard robot, autonomous navigation, multisensor fusion, LiDAR, SLAM, path planning

中图分类号:

TP24

高鹏, 姜军生, 白阳, 宋健. 基于改进A*算法的果园移动机器人建图定位与路径规划方法与试验[J]. 中国农机化学报, 2022, 43(1): 142-149.

Gao Peng, Jiang Junsheng, Bai Yang, Song Jian.. Method and experiment of map building and path planning for mobile robot in orchard based on improved A* algorithm[J]. Journal of Chinese Agricultural Mechanization, 2022, 43(1): 142-149.

参考文献

［１］　周建军, 周文彬, 刘建东, 等. 果园机器人自动导航技术研究进展［J］. 计算机与数字工程, 2019, 47(3): 571-576, 586.
Zhou Jianjun, Zhou Wenbin, Liu Wei, et al. Review of research on automatic navigation robot in orchard ［J］. Computer & Digital Engineering, 2019, 47(3): 571-576, 586.
［2］　闫全涛, 李丽霞, 邱权, 等. 小型移动式农业机器人研究现状及发展趋势［J］. 中国农机化学报, 2019, 40(5): 178-186.
Yan Quantao, Li Lixia, Qiu Quan, et al. Research status and development trends of smallmobile agricultural robots ［J］. Journal of Chinese Agricultural Mechanization. 2019, 40(5): 178-186.
［3］　李会宾, 史云. 果园采摘机器人研究综述［J］. 中国农业信息, 2019, 31(6): 1-9.
Li Huibin, Shi Yun. Review on orchard harvesting robots ［J］. China Agricultural Information, 2019, 31(6): 1-9.
［4］　赵映, 肖宏儒, 梅松, 等. 我国果园机械化生产现状与发展策略［J］. 中国农业大学学报, 2017, 22(6): 116-127.
Zhao Ying, Xiao Hongru, Mei Song,et al. Current status and development strategies of orchard mechanization production in China ［J］. Journal of China Agricultural University, 2017, 22(6): 116-127.
［5］　Su Y, Wang T, Shao S, et al. GR-LOAM: LiDAR-based sensor fusion SLAM for ground robots on complex terrain ［J］. Robotics and Autonomous Systems, 2021, 140(5): 103759.
［6］　Belkin I, Abramenko A, D Yudin. Realtime Lidarbased localization of mobile ground robot ［J］. Procedia Computer Science, 2021, 186: 440-448.
［7］　Dijkstra E W. A note on two problems in connexion with graphs ［J］. Numerische Mathematik, 1959, 1(1): 269-271.
［8］　Soltani A R, Tawfik H, Goulermas J Y, et al. Path planning in construction sites: Performance evaluation of the Dijkstra, A*, and GA search algorithms ［J］. Advanced Engineering Informatics, 2002, 16(4): 291-303.
［9］　邱磊. 利用跳点搜索算法加速A*寻路［J］. 兰州理工大学学报, 2015, 41(3): 102-107.
Qiu Lei. Speedup of A* pathfinding with jump point search algorithm ［J］. Journal of Lanzhou University of Technology, 2015, 41(3): 102-107.
［10］　张文, 刘勇, 张超凡, 等. 基于方向A*算法的温室机器人实时路径规划［J］. 农业机械学报, 2017, 48(7): 22-28.
Zhang Wen, Liu Yong, Zhang Chaofan, et al. Realtime path planning of greenhouse robot based on directional A* algorithm ［J］. Transactions of the Chinese Society for Agricultural Machinery, 2017, 48(7): 22-28.
［11］　王洪斌, 尹鹏衡, 郑维, 等. 基于改进的A*算法与动态窗口法的移动机器人路径规划［J］. 机器人, 2020, 42(3): 346-353.
Wang Hongbin, Yin Pengheng, Zheng Wei, et al. Mobile robot path planning based on improved A* algorithm and dynamic window method ［J］. Robot, 2020, 42(3): 346-353.
［12］　孙炜, 吕云峰, 唐宏伟, 等. 基于一种改进A*算法的移动机器人路径规划［J］. 湖南大学学报(自然科学版), 2017, 44(4): 94-101.
Sun Wei, Lü Yunfeng, Tang Hongwei, et al. Mobile robot path planning based on an improved A* algorithm ［J］. Journal of Hunan University (Natural Sciences), 2017, 44(4): 94-101.
［13］　Park J J, Kim H S, Song J B. Collisionfree path planning for a redundant manipulator based on PRM and potential field methods ［J］. Journal of Institute of Control Robotics & Systems, 2011, 17(4): 7-8.
［14］　 LaValle, S. M. Randomized Kinodynamic planning ［J］. International Journal of Robotics & Research, 1999, 15(5): 378-400.
［15］　易杰, 李蹊, 易辉成, 等. 基于点对特征的三维点云匹配算法［J］. 传感器与微系统, 2019, 38(9): 115-117.
Yi Jie, Li Xi, Yi Huicheng, et al. 3D point cloud matching algorithm based on point pair feature ［J］. Transducer and Microsystem Technologies, 2019, 38(9): 115-117.
［16］　林俊钦, 韩宝玲, 罗庆生, 等. 基于NDT匹配和改进回环检测的SLAM研究［J］. 光学技术, 2018, 44(2): 152-157.
Lin Junqin, Han Baoling, Luo Qingsheng, et al. 3D SLAM of mobile robot based on NDT match and improved loopclosure detection ［J］. Optical Technique, 2018, 44(2): 152-157.
［17］　Dine A, Elouardi A, Vincke B, et al. Graphbased SLAM embedded implementation on lowcost architectures: A practical approach ［J］. ProceedingsIEEE International Conference on Robotics and Automation, 2015, 2015: 4612-4619.
［18］　王殿君. 基于改进A*算法的室内移动机器人路径规划［J］. 清华大学学报(自然科学版), 2012, 52(8): 1085-1089.
Wang Dianjun. Indoor mobilerobot path planning based on an improved A* algorithm ［J］. Journal of Tsinghua University (Science and Technology), 2012, 52(8): 1085-1089.
［19］　涂亮杰, 李林升, 林国湘. 果园移动机器人的全局最优路径规划研究［J］. 南华大学学报(自然科学版), 2017, 31(4): 71-74.
Tu Liangjie, Li Linsheng, Lin Guoxiang. Orchard global optimal path planning for mobile robot research ［J］. Journal of University of South China (Science & Technology), 2017, 31(4): 71-74.
［20］　顾辰. 改进的A*算法在机器人路径规划中的应用［J］. 电子设计工程, 2014, 22(19): 96-98, 102.
Gu Chen. Application of improved A* algorithm in robot path planning ［J］. Electronic Design Engineering, 2014, 22(19): 96-98, 102.

[1]	顾健, 张荣昕, 戴相龙, 韩鑫, 兰玉彬, 孔凡霞. 基于SLAM的无人机飞行参数设定方法研究#br#[J]. 中国农机化学报, 2022, 43(8): 166-172.
[2]	耿丽杰, 顾健, 别晓婷, 冉维旭, 兰玉彬, . 基于Scan Context与NDT-ICP相融合的果园建图方法研究[J]. 中国农机化学报, 2022, 43(7): 44-50.
[3]	李雪峰, 李涛, 邱权, 樊正强, 孙娜. 果园移动机器人自主导航研究进展[J]. 中国农机化学报, 2022, 43(5): 156-164.
[4]	南风, 曹光乔, 李亦白, 陈聪, 刘东. 基于小麦收割机作业路线行走方向的调度优化研究[J]. 中国农机化学报, 2022, 43(4): 98-105.
[5]	代玉梅, 张瑞玲, 马黎. 改进A*算法的采摘机器人路径规划与跟踪控制[J]. 中国农机化学报, 2022, 43(3): 138-145.
[6]	李岩舟, 石奕峰, 涂伟, 覃锋, 潘柳颖, 何艳洲. 基于激光雷达的全生长周期甘蔗地行间自主导航研究[J]. 中国农机化学报, 2022, 43(3): 153-158.
[7]	姜赛珂, 张美娜, 李雪, 祁雁楠, 吕晓兰, . 设施内自主移动装备导航控制技术研究进展[J]. 中国农机化学报, 2022, 43(3): 159-169.
[8]	王潇, 张美娜, Zhou Jianfeng, 孙传亮, 吴茜, 曹静. LiDAR传感器及技术在农业场景的应用进展综述[J]. 中国农机化学报, 2022, 43(11): 155-164.
[9]	王新彦, 张凯, 盛冠杰, 易政洋. 基于单线激光雷达的割草机器人建图方法研究[J]. 中国农机化学报, 2022, 43(10): 51-56.
[10]	杨涛;李晓晓;. 机器视觉技术在现代农业生产中的研究进展[J]. 中国农机化学报, 2021, 42(3): 171-181.
[11]	杨保海;任全会;李海生;. 复杂环境下果园机器人路径规划方法研究[J]. 中国农机化学报, 2021, 42(2): 134-138.
[12]	石志刚, 梅松, 邵毅帆, 万如, 宋志禹, 谢铭露, 李燕. 基于人工势场法的移动机器人路径规划研究现状与展望[J]. 中国农机化学报, 2021, 42(12): 182-188.
[13]	杨涛, 李晓晓. 农机自动驾驶系统研究进展与行业竞争环境分析^*[J]. 中国农机化学报, 2021, 42(11): 222-231.
[14]	张美娜;范道全;雷哓晖;Andreas Herbst;吕晓兰;. 基于插值算法的靶标三维激光点云数据补偿研究[J]. 中国农机化学报, 2020, 41(8): 59-64.
[15]	耿乾;毛鹏军;李鹏举;黄传鹏;方骞;张家瑞;. 障碍物分类识别的果园机器人避障方法研究[J]. 中国农机化学报, 2020, 41(8): 170-177.