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

Method and experiment of map building and path planning for mobile robot in orchard based on improved A algorithm

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

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.