基于密度自适应的RANSAC非结构化环境下果园机器人导航

褚福春，宫金良，张彦斐

(1 山东理工大学 机械工程学院, 山东 淄博 255000； 2 山东理工大学农业工程与食品科学学院, 山东 淄博 255000)

我国作为水果生产大国，水果种植面积与产量均为世界第一。但是果树种植过程中存在着自动化水平低、管理不规范和采摘效率低等问题。针对这一系列问题，加大对果园智能装备的发展力度，尤其是加大对丘陵山区果园管理装备的改造已迫在眉睫。提高果园生产作业的自动化水平，不仅可以提高水果产量，还能够减少人工劳动力，降低生产成本，对我国许多水果种植地区具有比较重要的现实意义，因此研究出一款应用于果园的自主作业机器人势在必行[1-5]。

智慧农业是农业信息化发展从数字化到网络化再到智能化的高级阶段,对农业发展具有里程碑意义,已成为世界现代农业发展的趋势。为进一步提高生产效率和水果品质,农业机械装备的智能化是未来发展的必然趋势。随着智慧农业的不断发展，智能化机器人技术被广泛应用于果园除草、剪枝、采摘等作业环节，有效地减轻了果农的劳动强度，提高了作业效率与质量[6-9]。机器人在果园等非结构化的环境下行间行驶的安全性是研究重点，合理、可靠、安全的导航路径规划方法是保证机器人安全行驶的重要手段之一[10-11]。传统的GPS导航方案，在果园中由于受到果树遮挡，卫星信号弱，无法进行高精度定位[12]。激光雷达作为一种新兴的导航定位传感器，具有精度高、扫描速度快和抗干扰能力强等特点，在果园环境感知中得到了越来越广泛的应用[13-15]。

本文提出一种果园机器人导航技术，基于三维激光雷达和惯性测量单元(Inertial measurement unit，IMU)等传感器，采集果园三维点云数据，利用改进的随机采样一致性 (Random sample consensus，RANSAC)算法，拟合推算得到导航线。根据三维点云信息，提出换行策略，实现换行作业。基于果园机器人的差速转向结构和纯追踪模型进行轨迹跟踪，实现果园机器人在非结构化环境下的自主导航。

1 果园导航方法

首先使用16线激光雷达采集果园点云数据，在经过点云滤波处理后，降低了点云密度。然后进行地面点云的分割，去除地面点云后通过改进的RANSAC拟合出果树行直线，根据平行直线之间的关系，进一步推算得到导航线，结合横向偏差、航向偏差以及激光雷达IMU融合定位数据，通过纯跟踪控制算法调整机器人位姿。在行驶到行末触发换行判定程序，确定机器人是否需要换行，完成换行后继续执行果树行内导航程序。如此，便实现果园机器人的自主导航，主要导航技术框架如图1所示。

图1 果园机器人导航技术框架Fig.1 Framework of orchard robot navigation technology

1.1 果树行内直线行驶

果树行直线提取的方法通常是在获取当前帧的果树点云后，将该帧点云投影到世界坐标系中，然后将该帧点云分割成左右两树行的点云集，再从左右树行点云集中拟合出两侧果树行的直线，常用的获取方法有RANSAC算法和最小二乘法(Least squares method，LSM)。

LSM算法是一种被普遍使用的数学优化方法，该方法通过最小误差平方和来求解点云数据中的最佳函数参数[16]。采用LSM算法拟合果树行直线，当点云坐标满足f(Q)取到最小值，即：

式中，Q=[k b]T为果树行直线的参数矩阵，k为果树行直线的斜率，b为果树行直线的截距；X′为点云X轴坐标值组成的矩阵，Y′为点云Y轴坐标值组成的矩阵。

1981年有学者提出RANSAC算法，最初用于解决定位的问题，用迭代法求解含有局部点的观测数据的数学模型参数[17-18]。RANSAC算法通过对观测值随机抽样估算参数，该算法的简要步骤为：

1)随机从数据集中选取n个样本数据，建立参数模型M；

2)计算数据集中所有数据与参数模型M的误差，若误差小于阈值，加入内点集；

3)如果当前内点集元素个数大于最优内点集，则更新最优内点集，同时更新迭代次数h，h的表达式为：

式中，η为置信度，一般取0.995，λ为内点的比例，m为计算模型所需要的最少样本数；

4)如果迭代次数大于h，则退出，否则迭代次数加1，并重复上述步骤。

RANSAC算法较LSM算法具有较大的优势，尤其是在行距较大的果园中导航精度更高，鲁棒性更好。本文基于传统RANSAC算法，结合果园机器人的实际应用场景，提出一种基于点云密度自适应的RANSAC算法，即使存在部分点云缺失的情况，也能较好地拟合出果树行直线。传统的RANSAC算法固定阈值，在拟合点云较为稀疏的地方可能会出现较大偏差。改进后的RANSAC算法具体步骤为：

1)将当前帧的左右两侧果树行点云ξi进行聚类分割，并去除地面和杂草点云的干扰，得到左右两侧点云集合ξli和ξri；

2)一般果园都是按照平行直线种植，本文设定左、右2条平行直线分别为Ll和Lr：

式中，kl、kr为果树行直线斜率，bl，br为果树行直线截距；

3)分别从左右两侧点云集合ξli和ξri中随机选取2点，确定构成果树行直线的参数；

4)根据点云密度设定不同初始阈值Ri，从而分割出内外点，选取出内点集合，将最大内点集合的阈值设为最佳阈值Rbest，将其他内点集合的阈值更新为Rbest，进而确定与直线Li的几何距离不大于Rbest的点云集合ξ(Li)，即为L的最大一致集合。设定内点(xi,yi)，拟合直线Ax+By+C=0，则有:

5)重复n次随机选择，得到直线L1，L2，…，Ln和相应的一致集合ξ(L1)，ξ(L2)，…，ξ(Ln)；

6)确定最大一致集合，将最大一致集合中的点拟合成一条直线即为最佳直线L，通过ξli和ξri获取的最佳直线分别为Ll和Lr，即获取到最佳斜率kl和kr，最佳截距bl和br；

7)在获取到左右两侧果树行最佳直线后，根据平行直线关系，进一步提取出导航线Lc，果园机器人通过纯跟踪算法沿导航线行驶，完成果树行内的自主导航。其中，Lc的计算公式为：

由于Ll、Lr和Lc为三条平行直线，故k=kl=kr。

1.2 行末换行策略

一般果园的树行两端的特征很明显，即行首和行末都不存在果树，因此存在一定的转向空间。当激光雷达检测到果园机器人两侧大片点云数据缺失的时候，可以判定机器人已到达换行区域，工控机下发换行指令，给定机器人底盘特定的速度指令，保证每次换行转弯动作的准确性，默认左转，可根据果园实际情况进行调整。完成换行动作后，继续向前行驶一段距离dts，确保其完全进入到下一行，然后继续执行树行跟踪的指令，具体过程如图2。

图2 果园机器人换行策略Fig.2 Line breaking strategy of orchard robot

设定转弯半径为Rt，换行时的路径为Sth，则

2 运动控制

2.1 基本运动模型

本试验采用的是差速转向机器人底盘，同侧两车轮通过链传动共用一个电机来驱动，故同侧前后两轮轮速大小相等。可直接接收来自工控机发送的线速度v和角速度 ω(主要是指x轴方向上的线速度和z轴方向上的角速度)，实现机器人的各种运动。图3中XOY为建立的大地坐标系，v为机器人瞬时线速度，机器人正前方为正；vr为右侧轮速度；vl为左侧轮速度；ω为瞬时角速度，逆时针方向为正；θ为姿态角；£为轮距；p为机器人中心位置；Ll、Lr为左、右两侧果树行；机器人的位姿可用矢量P=[x,y,θ]T表示。其中：

图3 运动模型Fig.3 Motion model

机器人主要有3种运动状态：

1)vl≠vr，方向相同且均不为0时，机器人做圆周运动，此时

2)vl=-vr时 ，机器人原地转动，此时v=0；

3)vr=vl时，机器人直线行驶，此时 ω =0。

在驱动轮与地面接触运动为纯滚动的情况下，机器人的运动学模型可以表示为：

2.2 融合定位

IMU以100 Hz的频率输出加速度和角速度数据,激光雷达以10 Hz的频率输出点云数据。每收到一帧点云数据,系统首先扩增一个代表当前激光雷达位姿初始估计的状态向量,然后使用优化后的点云观测模型对特征进行提取和跟踪;每收到一帧IMU数据,根据运动模型对IMU状态进行估计,同时更新系统状态的协方差矩阵;最终通过扩展卡尔曼滤波算法融合IMU与激光雷达的观测数据更新位姿状态,以100 Hz的频率输出结果。相邻测量时刻间的状态量定义如下：

式中：w表示世界坐标系；u表示IMU坐标系；i表示测量时刻；S为系统状态量；Sut为t时刻IMU状态量，包括相对于世界坐标系的平移、速度和旋转，分别以p、v和四元数q表示；ua和ug分别为加速度计和陀螺仪的零偏。

本文提出的以激光雷达为主，融合IMU信息的定位方案，可实现果园机器人的厘米级定位，对非结构化环境下的导航十分重要。

2.3 轨迹跟踪

果园机器人通过纯跟踪算法结合传感器数据确定前视距离，最终将机器人位姿信息上传至工控机，通过对比当前帧与前一帧位姿变化，确定横向偏差和航向偏差是否超过设定阈值，工控机下发速度指令给单片机，驱动左右两侧电机，保证机器人能够按照预定导航线轨迹行驶，完成树行跟踪的目标。

纯追踪算法是一种模拟人类驾驶习惯的几何追踪模型，根据车辆的行驶速度和路径信息设定轨迹上的一个路径点，计算车辆当前位置距离预设目标点所需转过的角度，控制车辆向路径点行驶[19]。具体跟踪方法是：在目标轨迹K上选择一个点B作为跟踪点，则机器人从当前位置到达点B的轨迹可以视为一段半径为R的圆弧轨迹，对点B跟踪的关键在于对圆弧轨迹半径R的求解，图4中 α为航向偏差，f为前视距离，d为横向偏差。综合各参数，可计算得到轨迹半径R。

图4 果园机器人轨迹跟踪示意图Fig.4 Orchard robot trajectory tracking diagram

具体计算步骤为：

在求得轨迹半径R之后、线速度v给定的情况下可以求解出角速度 ω，计算公式为:

2.4 路径跟踪仿真结果分析

纯追踪算法中前视距离的大小将会直接影响机器人的轨迹跟踪效果，前视距离较大时，机器人将会以较小的曲率向路径行驶，但系统响应较慢，可能会产生“抄近路”的问题，影响控制精度；当前视距离较小时，机器人将会以较大曲率向路径行驶，机器人会因此频繁转向，产生较大的振荡，降低机器人的稳定性。因此，前视距离的大小对导航十分重要。

通过Python3.5 IDLE进行路径跟踪仿真，结合果园机器人差速运动模型和纯跟踪算法，初始位姿设置为 (0，-1，0)，前视距离分别设置为 2、3、4 m，在0.8 m/s速度下进行仿真，结果如图5所示。

图5 不同前视距离(f )下路径跟踪仿真结果Fig.5 Path tracking results at different forward-viewing distances(f)

从仿真结果中可以看出，前视距离为2 m时，机器人在曲率较大的路径处的实际运动轨迹较为曲折，实际运动轨迹和目标轨迹出现明显偏差；前视距离为4 m时，机器人虽然会以平滑的运动轨迹行驶，但也出现了实际轨迹与目标轨迹存在较大偏差的问题；只有当前视距离为3 m时，机器人能够较好地追踪目标轨迹，故设定机器人前视距离为 3 m。

3 试验验证及结果

3.1 硬件系统组成

图6a为果园机器人整体框架，由机器人底盘、工控机、Velodyne 16线激光雷达和9轴IMU组成。机器人长、宽和高分别为60、45和25 cm，整体采用差速转向的结构设计，使其在果园中具有良好的通行能力。机器人左右两侧各安装一个驱动电机，通过链传动的方式带动四轮转动。图6b为果园机器人实物图，除导航所必须的硬件结构，还配备了高清摄像头，可以实时获取果园真实画面。为方便各感器融合，定义果园机器人坐标系，以IMU中心为坐标原点，机器人正前方为x轴方向，平行于车轴且指向左侧的方向为y轴正方向，过坐标原点垂直指向正上方为z轴正方向。

图6 果园机器人基本组成Fig.6 Basic composition of orchard robot

3.2 试验方案

为了验证果园机器人自主导航系统的实际效果，在山东理工大学兰玉彬教授团队与淄博市沂源县山东中以现代智慧农业有限公司共建的智慧化无人果园试验基地进行了试验。该果园果树行距约为 4 m，果树高约 3.5 m，行距约 4 m。图7a为果园真实环境，可以看到果树枝叶茂盛，对GPS信号遮挡十分严重。

图7 点云预处理Fig.7 Point cloud pretreatment

通过三维激光雷达获取点云数据(图7b)后，首先进行预处理降低点云密度，借助滤波算法去除地面和杂草的干扰，采用平面模型拟合当前的地面。由于现实的地面并不是一个“完美的”平面，尤其是在果园这种环境中，地面非常不平整，要很好地完成地面分割，就必须要处理存在一定坡度变化的地面情况,即不能将这种坡度的变化视为非地面，不能因为坡度的存在而引入噪声。一种简单的处理方法就是沿着x方向(车头的方向)将空间分割成若干个子平面，然后对每个子平面使用地面平面拟合算法从而得到能够处理陡坡的地面分割方法。对于给定的点云集合设为P，最终将会被分割为地面点云集Pg和非地面点云集合Png，如图7c所示。

分割出地面点云后，利用滤波算法将其过滤掉，处理后的结果如图8a。通过聚类算法将果树行点云簇进行聚类，最终会形成一个将点云包裹的三维Bounding Box，其主要使用了一种基本的数据结构KD树，它是在一个欧几里德空间中组织点的基本数据结构，本质上是一个每个节点都为k对应维点的二叉树，聚类后的效果如图8b。

基于传统的RANSAC算法，提出一种密度自适应的改进方法，根据不同点云集设置不同初始阈值，筛选出最大内点集合，更新并确定最佳阈值，经过多次迭代，更高效且更准确地拟合出果树行直线，根据平行直线关系，推算导航线(图8c)。由图8d可以看到，即使在部分点云缺失和存在离群果树点云的情况下，该算法还是能较好地推算出导航线。

图8 拟合导航线Fig.8 Fitting navigation lines

通过多次试验，统计果园机器人实际行驶轨迹与导航线之间的横向偏差和航向偏差，验证本方案的可行性。对比LSM算法、传统RANSAC算法和改进后的RANSAC算法分别在0.8和1.0 m/s的不同速度下产生的横向偏差和航向偏差，每隔10 m进行一次记录，进行5次记录，并取平均值。

3.3 试验结果

综合多次试验结果，对比LSM算法、传统RANSAC算法和改进RANSAC算法，当机器人以0.8 m/s的速度行驶，改进后的RANSAC算法产生的横向偏差的最大值和平均值分别为0.10 m和0.084 m，航向偏差的最大值和平均值分别为1.5°和1.34°，相较LSM算法和传统RANSAC算法均为最小，具体结果见表1。但当机器人以1.0 m/s的速度行驶时，各项偏差均明显增大，具体结果见表2。

表1 速度0.8 m/s下3种算法产生的偏差结果Table 1 The deviation results generated by the three algorithms under the speed of 0.8 m/s

表2 速度1.0 m/s下3种算法产生的偏差结果Table 2 The deviation results generated by the three algorithms under the speed of 1.0 m/s

4 结论

果园机器人通过激光雷达采集点云数据，经过点云降噪、分割地面与非地面点云和果树行点云聚类等一系列处理后，依靠改进的RANSAC算法拟合果树行最佳直线，根据平行直线关系，推算得到导航线。通过纯跟踪算法对果园机器人进行运动控制，实现果园机器人的自主导航。基于本文提出的行末换行策略，实现机器人换行作业。通过咨询果园管理专业人员，了解到果园机器人在行距为4 m的果园进行巡检、除草等作业时，横向偏差不得大于0.15 m、航向偏差不得大于2°。本研究试验结果显示：当机器人以0.8 m/s的速度在果园行驶作业时，改进后的RANSAC算法产生的横向偏差不超过0.10 m，航向偏差不超过1.5°。因此，本文提出的导航方法基本满足生产要求，能够极大地提高果园管理的自动化水平，具有比较重要的现实意义。

未来将在保证定位导航精度的前提下，稳步提升果园机器人的行驶速度，提高作业效率。融合视觉算法，提高果园机器人的环境感知能力，实现对杂草和障碍物的识别，进而提高控制精度，使机器人在非标准化果园也能够正常行驶。