[发明专利]一种基于视觉的果园机械导航定位方法

权利要求书

1.一种基于视觉的果园机械导航定位方法，其特征在于，包括如下步骤：

S100、系统初始化后判断果园机械是否接收到卫星导航信号，若卫星导航信号正常则直接使用卫星导航；若卫星导航信号丢失则采用如下视觉导航定位；

S200、所述果园机械通过深度摄像头采集果树二维码的信息得到视野内每颗果树的位置坐标(X_n,Y_n)，所述果树二维码为预先生成的果树位置坐标信息并贴于对应的果树树干上；

S300、所述果园机械在果树行间行驶并通过深度摄像头采集到的图像和深度信息，采用视觉里程计实时推算所述果园机械相对于运动起始点的位姿，并推算该果园机械的相对位置；以及

S400、通过所述深度摄像头采集果树树干的三维轮廓和对应的所述果树二维码的信息，返回所述果树的树干位置和世界坐标，推算所述果园机械的绝对位置，并校正所述视觉里程计的累积误差；

其中，步骤S300中，所述视觉里程计基于特征点匹配或像素灰度值差异，利用所述三维轮廓与所述深度摄像头采集到的图像的对应关系，求解相邻帧所述图像的所述深度摄像头的位姿；所述果园机械将所述果树的世界坐标位置作为初始位置，以实现准确的定位导航；所述卫星导航信号丢失后，所述深度摄像头实时采集果园环境中果树的图像信息和深度信息，生成所述果园机械的前方三维环境图，同时根据相邻的所述前方三维环境图的图像信息估计所述深度摄像头的运动，通过所述视觉里程计得到所述果园机械相对所述初始位置的位姿并推算所述果园机械的相对位置。

2.根据权利要求1所述的基于视觉的果园机械导航定位方法，其特征在于，步骤S400中，所述深度摄像头通过图像处理的方法识别所述果树的树干位置，所述深度摄像头生成所述果园机械的前方三维地图，并计算所述树干位置的中心与所述果园机械之间的相对距离d_n。

3.根据权利要求2所述的基于视觉的果园机械导航定位方法，其特征在于，参照至少两组所述相对距离的信息和位置坐标推算所述果园机械的绝对位置，并校正所述视觉里程计累积误差。

4.根据权利要求3所述的基于视觉的果园机械导航定位方法，其特征在于，所述果园机械的绝对位置(X₀,Y₀)通过如下方程联立求解推算：

其中，X₁,X₂,……,X_n，Y₁,Y₂,……,Y_n为视野内1-n棵果树对应的位置坐标，d₁,d₂,……,d_n分别为1-n棵果树的树干位置中心与所述果园机械之间的相对距离。

5.根据权利要求1所述的基于视觉的果园机械导航定位方法，其特征在于，采用拓展卡尔曼滤波器或非线性优化方法求解相邻帧所述图像的所述深度摄像头的位姿。

6.根据权利要求1所述的基于视觉的果园机械导航定位方法，其特征在于，所述视觉里程计每一间隔时间进行一次绝对位置校正，以防止所述视觉里程计的累积误差造成位置偏移影响定位精度。

7.根据权利要求1或6所述的基于视觉的果园机械导航定位方法，其特征在于，还包括：

S500、判断卫星导航信号是否恢复，若卫星导航信号正常，则恢复使用卫星导航，并将最近一次所述果园机械的绝对位置校正作为初始点，卫星定位信息作为当前点进行所述视觉里程计的累积误差校正，以保证导航路径的连续性和准确性。