基于点云地图的果园作业机器人无人驾驶方法

技术领域

本发明涉及导航技术领域。更具体地说，本发明涉及一种基于点云地图的果园作业机器人无人驾驶方法。

背景技术

多功能农业机器人得到广泛地应用，使得农业机器人在广阔的田野上，越来越多地代替了手工完成各种农活。农业中果园生产的任务种类繁杂，，需要大量的人力和物力，同时非精准的果园管理方式会产生大量的无效投入和生态污染，增加了水果价格。针对这些情况，研制适合果园作业的智能精准型机器人势在必行。机器人要代替人工，在果园中自主作业，环境地图的构建是机器人完成定位与自主导航的首要任务，利用构建出的地图可以帮助机器人进行路径规划、预判果树位置、完成地头转向等任务。

在果园中，机器人的自主工作需要在两行果树之间的过道内完成，即沿着过道对每棵树进行作业，一行树作业完成了则转弯到另一行过道内工作，直至走完整片果园。机器人在工作过程中，一般会位于树冠以下，如果单单依靠GPS来定位，会出现卫星信号被树冠遮挡、多路径效应和射频干扰等问题，最终导致机器人位误差较大，甚至无法定位。果园行间的距离大多只有3m左右，利用GPS很难获取精准的行间定位导航效果；电磁导航定位能有较好的定位导航效果，但是成本较高。

发明内容

本发明的一个目的是解决至少上述问题和/或缺陷，并提供至少后面将说明的优点。

为了实现根据本发明的这些目的和其它优点，提供了一种基于点云地图的果园作业机器人无人驾驶方法，包括：

步骤一、通过差分GPS对果园区域周边的树木树冠、路标分别进行定位，标明树木和路标的绝对位置；

步骤二、采用激光雷达扫描记录关键特征树干并记录相应特征信号，得到对应的点云地图；

步骤三、作业机器人在进入果园前，获取当前的GPS信号，以在点云地图上标记出实时位置；

步骤四、作业机器人在果园中行进时，通过激光雷达不断扫描树干信息，以对应点云地图上的GPS信号点，以在自主行进过程中实时推算机器人行进位姿，进而修正机器人的行进方向；

步骤五、作业机器人运动到果园的每行尽头时，基于点云地图选择调头模式，直到对果园所有行完成遍历。

优选的是，在步骤一中，所述绝对位置的获取方式被配置为包括：

S1、数据采集，在移动机器人上设置搭载GPS天线且高于树冠的高架，在高架中部搭载激光雷达对封闭结构化的果园进行全局扫描，使每颗树干均被扫描，且扫描时GPS天线在每一树冠顶端停留时间配置为大于十秒；

S2、数据处理，移动机器人上的处理器将每一树干停留时间内的GPS点提取出来，通过粒子滤波算法剔除掉偏移点，以得到各树干绝对位置对应的GPS信号点。

优选的是，在步骤二中，所述点云地图的获取方式被配置为包括：

S3、处理器计算相邻两树坐标之间的中点，将其连接起来以获得可通行路径；

S4、通过将激光雷达扫描树干与地面特征获得的点云信息bag在ros下进行播放，并使用Lio-Sam算法进行加载，将其逐帧保存下来，存为pcd文件，将最后一张文件作为地图；

S5、将GPS信号点通过选择稀疏控制点的多标记点云标注形式加入到地图中，以得到对应的点云地图。

优选的是，在步骤四中，所述自主行进方式被配置为在机器人进入行间GPS信号消失后，通过激光雷达扫描树干获取相应的GPS信号点标签，通过激光雷达测量左右两侧树干距离信息，以计算出中心线位置，比对可通行节点沿中心线自主行驶。

优选的是，在步骤四中，实时推算机器人行进位姿，进而修正机器人的行进方向被配置为包括：

S6、激光雷达扫描树干，通过扫描到的点云信息与点云地图进行特征匹配，以获取当前位置的GPS信息；

S7、通过将GPS信息与IMU通过与扩展卡尔曼滤波EKF进行数据融合，获得准确的定位数据，而在机器人驶出行间遮挡消失，差分GPS信号恢复后，通过定位数据矫正此时的位置信息，以使在下一步转向进行之前机器人的位姿正确。

优选的是，调头模式的选择方式被配置为包括：

S8、机器人运动到果园行尽头时，通过扫描到行头两棵树的信息获得掉头信息；

S9、在机器人的雷达安装中线与行头树干平行时，检测机器人与围墙的距离，以通过扫描边界线的距离来选择半圆弧掉头法、隔行掉头法、3/4圆弧掉头法中的任意一种掉头方式进入新的行继续行驶，直到遍历所有行。

本发明至少包括以下有益效果：其一，本发明针对果园中机器人依赖GPS定位存在不能定位的情况，本发明可以让机器人在行间无GPS情况下，利用果园添加位置标签的点云地图，利用点云地图和惯性导航的辅助实现机器人精准出行、转弯和寻找下一行的功能，满足机器人在果园中连续工作的需求，在果园中丢失GPS信号时，保证定位准确，成本可控。

其二，本发明针对一般规划中无法精准控制机器人掉头这一情况，本发明可以使得机器人通过当前位置与果树行行头边界线距离的不同，选择不同的掉头策略，极大地提高掉头成功率，提高导航效率。

本发明的其它优点、目标和特征将部分通过下面的说明体现，部分还将通过对本发明的研究和实践而为本领域的技术人员所理解。

附图说明

图1为本发明的一个实施例中技术路线图；

图2为本发明导航系中力学编排建模图；

图3为本发明激光雷达与惯性信息融合示意图；

图4为本发明的另一个实施例中半圆弧掉头法的示意图；

图5为本发明的另一个实施例中隔行掉头法的示意图；

图6为本发明的另一个实施例中3/4圆弧掉头法的示意图。

具体实施方式

下面结合附图对本发明做进一步的详细说明，以令本领域技术人员参照说明书文字能够据以实施。

应当理解，本文所使用的诸如“具有”、“包含”以及“包括”术语并不配出一个或多个其它元件或其组合的存在或添加。

本发明针对机器人在密植果园内行驶场景，出现GPS无法精准定位和行间无法精准调头的问题，提供了一种基于点云地图和惯性导航辅助的方法。该发明保证了机器人能够在果园这种半结构化环境下(果树行列分布清晰)，自主完成作业。具体来说本发明基于点云地图的果园无人驾驶方法如下：

步骤1：基于树干和地标制作点云地图

通过差分GPS实现果园区域周边树木树冠的定位，然后对路标进行定位，标明树木和路标的绝对位置。使用16线激光雷达扫描记录关键特征树干记录特征，制作出高精度拓扑地图。具体操作如下：(1)数据采集：在移动机器人上焊接一轻型高架，高架一定要高于树冠。高架顶端搭载GPS天线，中部搭载16线激光雷达对封闭结构化果园进行全局扫描，沿着围墙开始，蛇形逐行进入果园，直至走完全部可通行区域。行进过程中保持匀速，确保树干被全部扫描，且GPS天线在每一树冠顶端停留时间超过十秒。(2)数据处理：对于每一树干，从机器人停止开始，到机器人重新移动结束，将时间内的GPS点提取出来通过粒子滤波算法剔除掉偏移点，求出树干实际位置。如此循环，直至所有树干都已被计算。通过计算相邻两树坐标之间的中点，将其连接起来即可获得可通行路径。使用激光雷达扫描树干与地面特征获得的点云信息bag在ros下进行播放，使用Lio-Sam算法进行加载，将其逐帧保存下来，存为pcd文件，最后一张文件即为我们需要的点云地图。将之前处理后的GPS信号点通过选择稀疏控制点的多标记点云标注的形式加进地图中，如此，即可通过激光雷达进行相对定位的方式对机器人进行绝对定位，在无GPS信号的情况下获取GPS坐标点，本步骤构建基于树干和地标的点云地图辅助GPS进行导航，可获取较大的检测范围，同时果园环境也具有较好的半结构化特征，同时通过地图导航的方法经济成本较低，符合农业发展的需要，而果园中除去树干，还有围墙、台阶、沟渠等，以上三者被统称为地标，具有辨识度的东西都可被称为地标，用以后续地图匹配时，作为特征点进行对比，以使机器人能在行进中避开地标。

步骤2：机器人行间自主行驶

机器人进入行间之前，获取GPS信号，在建好的点云地图上标记出自己的位置。进入行间，GPS信号消失，通过激光雷达扫描树干获取，获取相应的GPS点标签，通过左右两侧树干距离信息，计算出中心线位置，比对可通行节点沿中心线自主行驶。

通过点云地图获取到左右两侧边线之后，求其边线方程。左边线方程为：y＝k

随后控制机器人沿可通行道路中心线行驶。实时对比d1与d2的大小，通过预先设定规则进行转向控制，迫使机器人时刻保持沿可通行道路中心线行驶。转向控制规则如公式(2)所示：

至此，机器人能够在果园行间自主行驶，直到行走到果园行间的出口。

步骤3：推算机器人的位姿

对惯性导航系统进行建模(如图2所示)，并完成惯导系统在导航坐标系下导航参数的解算，最后使用惯性导航系统做初步的定位。

由于知晓点云地图的每一个点的绝对位置，故可以将载体坐标系转到默认的东北天坐标系下。

获取到实时速度后，获得推测位置。但是，这种根据速度推算而获得的距离虽然在短时内精度很高，但是由于系统零偏误差的一直存在，导致误差会持续累积，且果园路面并不平坦，颠簸也会导致误差，所以必须使用雷达进行数据融合。

使用激光雷达扫描树干，通过扫描到的点云信息与我们建好的点云地图进行特征匹配，获取当前位置，由于步骤1添加了位置信息标签，故可以获得绝对位置，相当于无视树冠的影响而直接获取准确的GPS信息，通过此GPS信息与IMU通过EKF进行数据融合，获得准确的定位数据，当机器人驶出行间，遮挡消失，差分GPS信号恢复，通过此信号直接矫正此时的位置信息，确保下一步转向进行之前机器人的位姿正确，其具体流程如图3所示，在本步骤中，采用合适的算法和外界辅助，可以推算出机器人与果园行的位置和姿态，根据当前机器人的位姿信息，及时生成机器人转向的控制信号，以应对不同的作业模式，具体来说，现有技术通常为IMU与GPS的数据融合，通常不涉及激光雷达。本方案如上所述，在行间通过将GPS信号添加进激光点云地图里的方式，取代GPS，与IMU数据进行融合，获取位姿信息。在行外信号恢复，通过差分GPS进行位置校准。

在实际操作中，由于树冠的遮挡，导致GPS信号很弱，激光雷达作为主要的定位方式，故选用准确度更高的NDT算法进行配准。NDT是基于栅格化地图的算法，基于2D场景下的主要算法思路是将参考帧点云地图分为一个个小块(cell)，计算下一时刻的目标帧得到的点云，变换到参考坐标，落到对应参考帧的cell中的概率。就是同一个环境点，不同时刻检测结果，变换到同一坐标，落在同一个cell。通过这种关系得到评分指标，最优化得到R，t。将雷达与惯导的推测结果通过扩展卡尔曼滤波进行融合，即可输出精确的位置结果，驶出行间后，再次通过扩展卡尔曼滤波进行信息处理，得出精确的位置信息，以解决在密植的果园环境中，采用在地头处规划转弯路径，然后利用GPS测量得到机器人的位置，存在GPS信号丢失和定位误差较大，造成机器人转弯具有极大的不稳定性，且GPS的后续的定位费用较高的技术问题；以及现有技术中构建了拓扑地图，但针对惯性导航的误差较大，如何实现机器人精准地进入下一行，其机器视觉仅用于检测拓扑点之后的强行更新位置，精度差强人意，不能完整地实现果园环境下机器人自主行驶的技术问题。

步骤4：基于点云地图选择调头模式

机器人运动到果园行的尽头时，通过扫描到行头两棵树的特定信息获得掉头信息，当机器人的雷达安装中线与行头树干平行时，即检测此时与围墙的距离，选择合适的掉头方式进入新的果园行继续行驶。掉头方式通过扫描边界线的距离来判断。

正常情况下，前方预留位置充足，选择传统如图4所示的半圆弧掉头方法。

当果树行宽度小于机器人的转弯半径时，并且行头边界线宽度也小于转弯半径时，此时传统的半圆弧掉头策略已经无法满足如此狭小的空间，因此只能采取如图5所示的隔行掉头法进入新的果树行。

当以上两种掉头方法都不适用，且机器人掉头反方向空间较大，行头边界线宽度满足要求时，可以采用如图6所示的3/4圆弧掉头法。

以上三种方法可以满足绝大部分果树行间的掉头场景。

下表将说明三种掉头方式的适用范围(约束条件)，其中r表示机器人最小转弯半径，d表示行头与围墙的距离，L表示行间距。

采用本发明方法进行的果园融合定位方法，通过添加GPS坐标点标签的点云地图与惯性导航系统相结合，可以准确且快速的得到机器人的位姿信息，并通过坐标点判断应该前进还是转向，通过航向角判断掉头是否成功，在实际操作中，如地图坐标系选取东北天坐标系，当机器人从南向北驶入行间，航向角应为正北，当机器人完成掉头操作，进行换行行驶时，航向角应为正南方向。当航向角变成正南时，通过激光雷达扫描获取信息，探得左右行间距，并通过行外GPS坐标信息进行比对，判断机器人的位姿是否正确，以此判断是否掉头成功。

以上方案只是一种较佳实例的说明，但并不局限于此。在实施本发明时，可以根据使用者需求进行适当的替换和/或修改。

这里说明的设备数量和处理规模是用来简化本发明的说明的。对本发明的应用、修改和变化对本领域的技术人员来说是显而易见的。

尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方式中所列运用。它完全可以被适用于各种适合本发明的领域。对于熟悉本领域的人员而言，可容易地实现另外的修改。因此在不背离权利要求及等同范围所限定的一般概念下，本发明并不限于特定的细节和这里示出与描述的图例。