一种基于SLAM的果园机器人导航方法

摘要

本发明公开了一种基于SLAM的果园机器人导航方法，果园机器人采用树干方位测量模块、里程计测量模块感知果园环境，并使用基于非线性优化的SLAM方案对感知数据进行融合，构建以二维树干坐标表征的果园地图，从而实现专用于果园环境下的移动机器人高精度建图与全局定位功能；所述果园地图的特征在于包含果园中所有树干的中心二维坐标，及每棵树干的轮廓拟合圆形半径，上述果园地图构建完毕后，还用于实现机器人在果园环境中的路径规划。本发明提出的导航方法能够实现机器人在果园环境中的高精度全局定位与“点到点”式地路径规划，从而达成对果园中各植株及其周边土壤环境进行差异化管理的目标，促进机器人技术在精准农业领域的应用。

说明书

技术领域

本发明属于农业机器人技术领域，具体涉及一种果园机器人基于SLAM(Simultaneous Localization and Mapping)的果园机器人导航方法。

背景技术

精准农业被视为未来农业的发展方向，同时机器人技术的兴起也大大提升了农业的生产效率。果园环境中，精准农业理念下的机器人需对每一植株个体及其周边土壤环境执行高分辨率地信息收集任务，并根据每一植株生长状况定制不同的植保方案，以此显著地节约肥、水、药等资源，并提高优质果率。此外机器人还需自动执行果实采摘、运输等任务。

现有的果园导航方法多是使用传感器提取机器人两侧树行的中心线，并将其作为机器人的目标运动轨迹，这类方法仅能够满足机器人在果园中自主巡航的功能，而在精准农业领域，需要实现厘米级地高精度全局定位与“点到点”式地路径规划，才能实现对各植株及其周边土壤环境的差异化管理。因此，果园环境下机器人的高精度定位与路径规划是必须要实现的技术。

发明内容

本发明提供了一种基于SLAM的果园机器人导航方法，用以解决精准农业理念下机器人在果园环境下的高精度全局定位与路径规划问题。

本发明采用如下技术方案来实现的：

一种基于SLAM的果园机器人导航方法，包括：

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

本发明进一步的改进在于，树干方位测量模块使用的传感器包括一个单目相机与一个单线激光雷达，其中由单目相机获取环境图像，并基于由深度学习方法训练得到的树干检测模型，实时地检测机器人周围树干在图像中的位置，并标定相机与激光雷达的外参矩阵，将雷达感知的点云投影到图像中，获取图像中树干像素的深度信息，从而得到树干相对机器人的方位。

本发明进一步的改进在于，里程计测量模块使用的传感器包括机器人底盘的轮速计与陀螺仪，这两种传感器的数据通过航迹推算方法计算机器人位姿变换的一种测量。

本发明进一步的改进在于，SLAM方案采用基于非线性优化的SLAM技术将果园建图问题建模成非线性最小二乘问题，求解变量为机器人建图过程中的全运动轨迹与果园地图，步骤为：

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

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

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

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

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

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

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

本发明进一步的改进在于，用于机器人路径规划时，将上述方法构建的果园地图转化成二值栅格地图，每个栅格的值表示其内部是否包含障碍物，即是否包括树干。

本发明进一步的改进在于，将果园移动机器人的路径规划分为两部分：全局路径规划器与局部轨迹规划器，全局路径规划器用于在果园栅格地图中搜索一条从机器人当前位姿到给定任务位姿的无碰撞最短路径，该路径使用一系列离散的轨迹点表达，局部轨迹规划器用于根据全局路径规划器输出的全局路径与激光雷达感知的障碍物信息，将全局路径优化成一条符合机器人运动学约束，且能躲避果园中动态障碍物的局部轨迹，并实现对机器人底盘的速度控制。

与现有技术相比，本发明至少具有以下有益的技术效果：

本发明提出了一种适用于精准农业的果园机器人高精度建图与全局定位方法，该方法能够从多种传感器数据中提取树干方位信息、机器人底盘里程信息，并通过SLAM技术融合这些信息，最终创建了以二维树干坐标与半径表征的果园地图。另外，本发明还将此地图应用于机器人在果园中的重定位任务。这种建图定位方法能够对果园中的植株与机器人进行厘米级高精度定位，助力精准农业在果园环境下的落地。

进一步的，本发明根据所提出的果园地图形式，配套地提出了一种果园机器人路径规划方法。该路径规划方法不仅能找到从机器人当前位姿到目标任务位姿的最短路径，还能够满足机器人底盘的运动学约束，并且可以躲避动态的障碍物。本发明能够顺利地完成“点到点”式地路径规划，助力达成精准农业机器人自动对果园中植株进行差异化管理的目标。

附图说明

为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为使用本发明提出果园导航方法的具体实施例的系统框架图；

图2为本发明实施例中机器人建图定位过程的示意图；

图3为本发明实施例使用的果园环境下高精度建图方法的流程框图；

图4为本发明实施例使用极大似然方法判断树干编号的流程框图；

图5为本发明实施例使用的果园环境下高精度全局定位方法的流程框图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1，使用本发明提出果园导航方法的具体实施例的系统框架图。

系统中包含两个计算设备，其中工控机107需要执行三个复杂功能模块的计算任务：树干方位测量模块1073、建图与定位模块1072、路径规划模块1071；而STM32单片机106需要执行两个简单功能模块的计算任务：里程计测量模块1062、底盘速度控制模块1061。工控机107与STM32单片机106通过串口进行数据通讯。

树干方位测量模块1073通过单目相机105获取机器人周围环境图像，并基于由深度学习方法训练得到的树干检测模型10731，实时地检测机器人周围树干在图像中的位置，在本实施例中采用的深度学习算法为Faster R-CNN。经过基于Faster R-CNN的树干检测10732后，可得到树干在图像中的BoundingBox，即树干在图像中的位置。树干方位测量模块1073通过单线激光雷达104获取树干的深度信息，单线激光雷达的点云数据表达形式为一系列(l

里程计测量模块1062收集电机编码器102与陀螺仪103的感知数据，并通过航迹推算方法计算机器人位姿变换的一种测量，可以设相邻时刻由里程计获得的机器人二维位姿变换测量结果为u

建图与定位模块1072基于SLAM技术实现果园机器人的高精度建图与全局定位功能。首先在果园地图未知的情况下，首先运行建图步骤10721，构建果园地图10722，并存储在工控机中；然后建图结束后，即可以使用定位步骤10723实现机器人在果园环境中的高精度全局定位功能。同时，机器人在地图坐标系中位姿是路径规划模块1071的输入数据。建图步骤10721的流程框图在图3，定位步骤10723的流程框图在图5，后文会有进一步描述。

路径规划模块1071分为两部分：全局路径规划器10711与局部轨迹规划器10712。在本实施例中使用的全局路径规划算法是A*算法，使用该算法，需将果园地图10722栅格化。全局路径规划器在接收到目标任务位姿后，会在果园地图中搜索一条从机器人当前位姿到给定任务位姿的无碰撞最短路径，即一系列离散的轨迹点。但需要注意的是，全局路径无法预知机器人在行驶过程中是否会碰到地图中所没有的障碍物，即动态障碍物；此外，A*算法将机器人建模成一个质点，其规划的路径不一定满足机器人运动学约束(如差速式底盘无法做到横向移动，而A*规划出的路径却可能是横移的)。因此需要使用局部轨迹规划器10712对全局路径进行优化。而在本发明实例中使用的局部轨迹规划算法是Timed-Elastic-Band(TEB)算法。TEB算法仅优化从当前位姿开始的一段全局路径，并重复执行，直到机器人到达目标位姿。TEB算法将路径优化成能够躲避动态障碍物的轨迹，最终输出速度指令给底盘速度控制模块1061执行。另外，局部轨迹规划器10712需要获取激光雷达104感知的障碍物信息以及里程计测量模块1062的里程计测量。

底盘速度控制模块1061接收到局部轨迹规划器10712发送的速度控制指令后，会将其分解为底盘电机的转速控制指令，并通过电机驱动器101执行。

请参阅图2，为本发明实施例中机器人建图定位过程的示意图。

其中矩形表示机器人，而圆形则是果园中各树干的轮廓拟合圆形，本发明实施例将机器人建图起点时的位姿作为地图坐标系X

图中虚线矩形202表示上一时刻t-1机器人在地图坐标系中的状态，其位姿为x

图中204的圆形表示果园中一棵树干，该树干中心在地图坐标系中坐标为p

要注意，果园中的树干半径大小不一，因此需要分别求解，而本发明实施例使用的优化求解方法在迭代求非线性最小二乘问题时需要初值，因此，需对果园中一部分树干的半径进行人工测量，并描述为一个高斯分布。在建图过程中，每观测到一棵果园地图中还没有的树干时，需设置该树干半径的初值，可从高斯分布中随机采样获得。

若树干方位测量模块没有噪声且能获取准确无误的机器人位姿，则应满足等式z

请参阅图3，为本发明实施例使用的果园环境下高精度建图方法的流程框图，即图1中建图与定位模块1072中基于非线性优化的SLAM系统10721的具体算法流程。

301、建图开始，在本发明实施例中，首先设置机器人的初始位姿

此时最小二乘问题的待求解变量仅有机器人的初始位姿x

302、获取树干方位测量z

303、判断里程计测量u

304、设当前时刻t的机器人位姿为x

305、极大似然法判断树干编号，(极大似然方法的具体流程见图4，后文会有更进一步说明)极大似然法判断树干编号。若是已观测树干j，损失函数加入误差项

306、本发明实施例采用高斯牛顿方法作为非线性优化求解器，求解使损失函数f(x

最小二乘问题的求解结果

307、由用户决定建图过程是否结束，若结束则进入步骤308，否则回到步骤302。

308、输出果园地图，也就是果园中所有树干中心的二维坐标，以及每棵树干轮廓拟合圆形的半径，即

请参阅图4，为本发明实施例使用极大似然方法判断树干编号的流程框图。

401、输入上一时刻机器人位姿

402、使用运动方程估计t时刻机器人在地图坐标系中位姿：

403、计算地图已有的J棵树干是当前观测树干的误差，即

其中能够得到误差最小值的树干编号为：

404、判断

405、卡方检测不满足，即观测到了地图中还未有的树干，设置其编号为J+1，从统计半径高斯分布中随机采样作为该树半径初值

406、卡方检测满足，即认为当前观测树干的编号为k。随后进入步骤407。

407、返回t时刻被观测树干的编号。

请参阅图5，为本发明实施例使用的果园环境下高精度全局定位方法的流程框图，即图1中建图与定位模块1072中基于非线性优化的定位系统10723的具体算法流程。

501、定位开始，在本发明实施例中，首先设置机器人的初始位姿

此时最小二乘问题的待求解变量仅有机器人的初始位姿x

502、获取树干方位测量z

503、判断里程计测量u

504、设当前时刻t的机器人位姿为x

505、极大似然法判断树干编号为j，并在损失函数中添加由树干方位测量模块提供的误差项

506、本发明实施例采用高斯牛顿方法作为非线性优化求解器，求解使损失函数f(x

本发明实施例在定位系统中使用的非线性最小二乘问题只求解最近五个时刻机器人轨迹点位姿，因此损失函数中与最近五个时刻机器人轨迹点变量不相关的误差项需被删除。求解结果

507、将

508、由于u

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。