具体实施方式

下面结合附图对本发明的结构原理和工作原理作具体的描述：

参见图1，图1为本发明一实施例的导航定位方法流程图。本发明基于视觉的果园机械导航定位方法，在卫星信号丢失的情况下启用视觉导航定位，在信号正常的情况下直接使用卫星导航，视觉导航定位包括如下步骤：

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

步骤S200、所述果园机械1通过深度摄像头3采集果树二维码的信息得到视野内每颗果树4的位置坐标(X_n,Y_n)，所述果树二维码为预先生成的果树4位置坐标信息并贴于对应的果树4树干上，即在导航定位开始之前，将果树4的卫星(GPS/北斗)位置坐标信息、经纬度信息生成二维码提前贴于对应果树4的树干，以便导航的时候深度摄像头3实时采集；

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

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

本实施例还可包括：

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

其中，步骤S400中，所述深度摄像头3通过图像处理的方法识别所述果树4的树干位置，所述深度摄像头3生成所述果园机械1的前方三维地图，并计算所述树干位置的中心与所述果园机械1之间的相对距离d_n。可参照至少两组所述相对距离的信息和位置坐标推算所述果园机械1的绝对位置，并校正所述视觉里程计累积误差。其中，所述果园机械1的绝对位置(X₀,Y₀)通过如下方程联立求解推算：

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

在步骤S300中，所述视觉里程计基于特征点匹配或像素灰度值差异，利用所述三维轮廓与所述深度摄像头3采集到的图像的对应关系，求解相邻帧所述图像的所述深度摄像头3的位姿。采用拓展卡尔曼滤波器或非线性优化方法求解相邻帧所述图像的所述深度摄像头3的位姿。且所述果园机械1将所述果树4的世界坐标位置作为初始位置，以实现准确的定位导航。

当所述卫星导航信号丢失后，所述深度摄像头3实时采集所述果园环境中果树4的图像信息和深度信息，生成所述果园机械1的前方三维环境图，同时根据相邻的所述前方三维环境图的图像信息估计所述深度摄像头3的运动，通过所述视觉里程计得到所述果园机械1相对所述初始位置的位姿并推算所述果园机械1的相对位置。所述视觉里程计每一间隔时间进行一次绝对位置校正，以防止所述视觉里程计的累积误差造成位置偏移影响定位精度。

参见图2，图2为本发明一实施例的导航定位方法原理图。如图所示，当果园机械1在果树4行间沿导航路径5行驶时，通过深度摄像头3采集到的图像和深度信息，采用视觉里程计实时推算当前果园机械1相对于运动起始点的位姿估计，推算果园机械1相对位置。预先将果园环境中果树4的位置坐标信息生成二维码贴于果树4树干，通过深度摄像头3采集树干三维轮廓和二维码信息，返回树干距离和果树4世界坐标，推算果园机械1绝对位置，校正视觉里程计累积误差。

在本发明一实施例中，果园机械导航定位方法的具体工作过程如下：

系统初始化后判断卫星信号是否丢失，如果信号丢失，将此时的世界坐标位置作为初始位置，深度摄像头3获取图像数据；

所述深度摄像头3可以实时采集图像信息和深度图，生成果园机械1前方三维环境，通过视觉里程计得到果园机械1相对初始位置的位姿估计，推算相对位置；其中，所述深度摄像头3获取图像数据，通过图像处理的方法识别果树4树干位置，结合深度图确定果园机械1与果树4中心点的距离。将果树4的位置坐标信息生成二维码贴于果树4的树干，深度摄像头3采集二维码信息得到果树4的位置坐标。参照几组(至少2组)果树4距离信息和位置坐标推算果园机械1绝对位置，校正视觉里程计累积误差。绝对位置校正每过一段时间进行一次，防止位姿估计累积误差过大。当卫星信号恢复时，将最后一次绝对位置校正作为初始点，卫星定位信息作为当前点进行一次视觉里程计校正，进而保证导航路径5的连续性和准确性。

其中，所述视觉里程计是移动机器人系统的一种自我定位方法，按照是否需要提取特征，可分为特征点法和直接法。视觉里程计工作时，基于特征点匹配或像素灰度值差异，利用已知三维结构与图像的对应关系，求解相邻帧图像的摄像机位姿估计，求解方法较多，本实施例优选通过拓展卡尔曼滤波器和非线性优化两大类方法来解决。

求解过程可总结为两个基本方程：

假设果园机械1正携带某种传感器和相机在未知环境里运动，因为相机通常是在某些时刻采集数据，所以就把导航时一段连续时间的运动看成离散时刻t＝1,……，k当中发生的事情。在这些时刻，用x表示果园机械1自身的位置，于是各时刻的位置就记为x₁，……，x_k，它们构成了果园机械1运动过程的轨迹。假设地图由多个路标组成，而每一个时刻，传感器会测量得到一部分路标点，设路标点一共有n个，用y₁，……，y_n表示它们。

第一个方程x_k＝f(x_k-1,u_k,w_k)为运动方程，通常果园机械1会携带一个测量自身运动的传感器，比如编码器或惯性传感器。这个传感器可以测量有关运动的读数，但不一定直接就是位置之差，还可能是加速度、角速度等信息。然而无论是什么传感器，都可使用一个通用的、抽象的数学模型来表示，即：

x_k＝f(x_k-1,u_k,w_k)

公式中，u_k是运动传感器的读数，w_k为噪声。这里用一个一般函数f来描述这个过程，但不具体指明f的作用方式，因此整个函数可以指代任意的运动传感器，成为一个通用的方程。

第二个方程z_k,j＝h(y_j,x_k,v_k,j)为观测方程，观测方程的描述是，当果园机械1在x_k位置上看到某个路标点y_j，产生了一个观测数据z_k,j，v_k,j是这次观测里的噪声，观测使用的传感器形式很多，如深度摄像头3、激光雷达等，同样用一个抽象函数h来描述这个关系。

z_k,j＝h(y_j,x_k,v_k,j)

已知运动测量读数u，以及传感器读数z时，求解定位问题(估计x)和建图问题(估计y)。

其中，定位问题就是通过获取已知运动测量读数u以及传感器读数z，求解相邻图像间(视频每帧之间)果园机械1的相对运动，估算果园机械1相较于起始点的自身位置x(根据每两帧图像推算出的位置，记为x₁，……，x_k)。建图问题指果园机械1在k时刻与x_k处探测到某一个路标y_j，考虑如何用数学语言来描述y_j的空间位置，也就是估计y。

注意，上述公式中涉及的x，y与果树二维码及果园机械1绝对位置中的X，Y含义不同，果树二维码中的X，Y是通过卫星定位获取的位置坐标信息，果园机械1绝对位置是基于果树二维码的位置坐标通过联立方程求算的果园机械1的绝对位置坐标，在此以大小写不同以示二者的区别。

参见图3，图3为本发明一实施例的计算果园机械绝对位置原理图，本发明的果园机械1的绝对位置计算原理如图3所示。运动测量读数u_k可通过卫星定位模块2或编码器、IMU等传感器读取，编码器与电机轴连接，用于测量电机轴位置、角度、圈数等信息；卫星定位模块2或IMU安装于车架，尽量靠近机身重心，用于工作时测量机身三轴姿态角以及加速度。编码器和IMU仅用于举例，传感器类型不限于这两种，只要可测量机器自身运动的传感器均可。传感器读数z_k,j可通过深度摄像头3、激光雷达等传感器读取，本实施例采用深度摄像头3采集z_k,j，安装于果园机械1的顶部靠前位置。预先将果树4的位置坐标信息生成果树二维码贴于果树4树干，果园机械1可通过深度摄像头3采集二维码信息得到视野内每颗果树4的位置坐标(X_n,Y_n)。通过图像处理的方法识别果树4的树干位置，深度摄像头3可生成果园机械1前方三维地图，进而可以计算到果树4中心与果园机械1的相对距离d_n。

参照几组(至少2组)果树4距离信息和位置坐标，便可通过平面几何的方式推算出果园机械1绝对位置(X₀,Y₀)，进而校正视觉里程计的累积误差。

求解过程可总结为一个基本方程：

可通过方程联立求解推算果园机械1绝对位置(X₀,Y₀)。

本发明可将果园机械在果园中的导航定位分为两部分：当果园机械在果树行间行驶时，通过深度摄像头采集到的图像和深度信息，采用视觉里程计实时推算当前果园机械相对于运动起始点的位姿估计；将果园环境中果树的位置坐标信息生成二维码贴于果树树干，通过深度摄像头采集树干三维轮廓和二维码信息，返回树干距离和果树世界坐标，推算果园机械绝对位置。既可以实时计算果园机械连续运动的相对位姿估计，也可以得到果园机械世界坐标，校正视觉里程计位姿累积误差，实现果园机械丢失卫星信号时的准确定位。可以让果园机械在丢失卫星信号时实现定位导航，克服由于果园浓密树冠对卫星信号的遮挡导致的定位导航效果不好的问题。

当然，本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。