[发明专利]一种基于SLAM的果园机器人导航方法

权利要求书

1.一种基于SLAM的果园机器人导航方法，其特征在于，包括：

果园机器人采用树干方位测量模块、里程计测量模块感知果园环境，并使用基于非线性优化的SLAM方案对感知数据进行融合，构建以二维树干坐标表征的果园地图，从而实现专用于果园环境下的移动机器人高精度建图与全局定位功能；所述果园地图的特征在于包含果园中所有树干的中心二维坐标，及每棵树干的轮廓拟合圆形半径，上述果园地图构建完毕后，还用于实现机器人在果园环境中的路径规划。

2.根据权利要求1所述的一种基于SLAM的果园机器人导航方法，其特征在于，树干方位测量模块使用的传感器包括一个单目相机与一个单线激光雷达，其中由单目相机获取环境图像，并基于由深度学习方法训练得到的树干检测模型，实时地检测机器人周围树干在图像中的位置，并标定相机与激光雷达的外参矩阵，将雷达感知的点云投影到图像中，获取图像中树干像素的深度信息，从而得到树干相对机器人的方位。

3.根据权利要求1所述的一种基于SLAM的果园机器人导航方法，其特征在于，里程计测量模块使用的传感器包括机器人底盘的轮速计与陀螺仪，这两种传感器的数据通过航迹推算方法计算机器人位姿变换的一种测量。

4.根据权利要求1所述的一种基于SLAM的果园机器人导航方法，其特征在于，SLAM方案采用基于非线性优化的SLAM技术将果园建图问题建模成非线性最小二乘问题，求解变量为机器人建图过程中的全运动轨迹与果园地图，步骤为：

I：建图开始，设置机器人初始位姿，初始化非线性最小二乘问题；

II：获取当前时刻树干方位测量模块观测到的树干方位信息；

III：若里程计模块观测到此时机器人位姿相对上一时刻的变换超过设定阈值，则进入步骤IV，否则重复步骤II；

IV：将当前时刻机器人轨迹点加入最小二乘问题待求解变量中，并将当前时刻由里程计测量得到的误差项加入到最小二乘问题的损失函数中；

V：使用极大似然估计方法判断被观测树干的编号，若该树干从未被观测过，则在地图中加入该树干，并初始化其中心二维坐标与半径，将由当前时刻树干方位测量得到的误差项加入到最小二乘问题的损失函数中；

VI：使用非线性优化方法迭代求解该最小二乘问题，最终得到机器人全运动轨迹与果园地图的解，求解结果会作为下一次循环时求解器使用的迭代初值，最后重新进入步骤II；

VII：建图结束，输出果园地图，果园地图构建结束后，定位系统依靠该地图进行重定位，地图在定位过程中不会更新。

5.根据权利要求1所述的一种基于SLAM的果园机器人导航方法，其特征在于，用于机器人路径规划时，将上述方法构建的果园地图转化成二值栅格地图，每个栅格的值表示其内部是否包含障碍物，即是否包括树干。

6.根据权利要求1所述的一种基于SLAM的果园机器人导航方法，其特征在于，将果园移动机器人的路径规划分为两部分：全局路径规划器与局部轨迹规划器，全局路径规划器用于在果园栅格地图中搜索一条从机器人当前位姿到给定任务位姿的无碰撞最短路径，该路径使用一系列离散的轨迹点表达，局部轨迹规划器用于根据全局路径规划器输出的全局路径与激光雷达感知的障碍物信息，将全局路径优化成一条符合机器人运动学约束，且能躲避果园中动态障碍物的局部轨迹，并实现对机器人底盘的速度控制。