高鹏，姜军生，白阳，宋健

(1.山东科技大学机械电子工程学院，山东青岛，266590；2.潍坊学院机电与车辆工程学院，山东潍坊，261061)

0 引言

水果产业在整个农业经济中占有很大比例，随着城乡居民生活水平的提升，人们对水果的消费需求日益增加，但是果园自动化程度较低与劳动力资源缺乏制约了水果产业的进一步发展，所以将智能机器人引入果园代替人工作业显得尤为重要[1-4]。果农们需要定期在果园内进行除草、修枝、喷药、施肥等工作，将机器人技术应用在果园环境中，可以把果农们从这些高温、有毒等危险的任务中解放出来，而果园机器人实现智能化的前提是可以在没有人类干预的情况下精确感知和自主规划决策到达目标地点，随后执行下一步特定的果园工作。

自主导航技术在城市、道路等场景中已广泛应用，但在农业领域发展缓慢，其原因在于果园环境相对复杂，大量不规则的树冠和杂乱的枝叶增加了机器人感知和定位的难度。随着近年来增强现实、无人驾驶技术的兴起，SLAM技术也越来越受到关注。相比于相机，激光雷达将自身作为光源感知周围环境能够实时获取三维点云数据，感知范围大，在黑暗环境中工作更有优势，克服了相机光学错觉和测距不足的缺陷。主流的三维激光SLAM算法主要包括LOAM、A-LOAM、LeGO-LOAM等[5-6]，这些算法未针对果园环境进行特定优化，所以在果园复杂环境中容易出现地图漂移、特征点提取困难、噪声点过多等问题。除了建立地图和准确定位外，机器人控制器还要快速规划出没有障碍物阻挡的可行路线，通过路径生成及优化算法，最终得到实际机器人可以运行的、符合动力学约束的、花费代价最小的光滑路线。常用的全局规划有Dijkstra、A*、JPS、PRM、RRT等算法[7-14]。除此之外，机器人还需具备主动避障能力，这需要实时局部规划算法来实现，如Trajectory Rollous、DWA、TEB等，上述全局规划算法在果园复杂环境中容易找不到最优路径，本文对A*算法进行了改进，解决了原始算法得不到最短路径的问题。

本文提出一种果园自主导航系统，采用基于图优化的紧耦合LiDAR-IMU里程计算法实现机器人建图与定位功能，对数据进行降维处理，融合广度优先搜索和改进启发式函数的A*算法完成路径规划并进行跟踪控制，应用到实际履带式移动机器人上进行自主导航效果测试。

1 机器人平台

果园移动机器人由车载硬件系统和程序控制系统两部分组成，如图1所示，机器人整体尺寸(长×宽×高)为1 020 mm×750 mm×405 mm，为了使移动机器人在果园环境内有更好的地形适应能力及轨迹跟踪能力，移动机器人底盘部分采用履带传动机构，可原地转弯，采用克里斯蒂和玛蒂尔达四轮组合平衡悬挂系统，左右各装有独立油压阻尼避震器，具有很强的吸震承载能力，可负载100 kg重物爬越30°的斜坡。配备2 kW 大容量锂电池和高精度伺服电机，可在6.5 km/h的运行速度下连续作业30 min。机器人顶部装有16线机械式激光雷达，扫描频率为10 Hz，最大量程150 m，测量精度小于2 cm，水平角度分辨率0.18°。惯性测量单元(IMU)为基于MEMS惯性传感技术的三加速度计，可输出加速度、角速度、欧拉角位姿信息，在振动和磁场扭曲状态下仍然具有高精确度跟踪性能。机器人还搭载了高精度定位测向系统，使机器人可在遮挡严重及高频动态环境下定位与测向得到快速准确响应，并且融合绝对位姿信息补偿IMU模块带来的累计误差，辅助记录机器人位置信息，用于对比验证建图与定位效果。

图1 果园机器人硬件平台

机器人控制器放置在内仓中，控制器为NUC10i7FNK微型计算机，由Intel core i7-10710U CPU和16GB DDR4 RAM组成，安装了Ubuntu16.04LTS操作系统与ROS kinetic开源机器人开发框架，使用C++编程语言，构建开机自启动程序，运行建图定位和轨迹规划算法及相关功能包，通过CAN通信协议与电机驱动器通讯下达速度指令完成轨迹跟踪，机器人程序控制系统整体流程图如图2所示。

图2 程序控制系统整体流程图

2 果园机器人建图与定位算法

2.1 激光里程计

2.1.1 点云畸变补偿

由于IMU误差随时间的积累，需要与其他传感器融合，从而不断相互矫正输出位姿，由于机器人不断运动和激光雷达本身机械结构的影响，还需要进行帧间点云畸变补偿，如图3所示。首先要计算出该帧每个激光点相对于坐标原点的坐标变化，然后将其作为补偿量对激光点坐标进行补偿，计算公式如式(1)所示。

图3 果树点云畸变

式中：T0——起始时刻雷达位姿；

Ti——第i个激光点采集时雷达的位姿；

Pi——第i个激光点坐标；

2.1.2 线面特征提取

对点云进行线面特征提取匹配策略，计算激光点相对于水平面的倾角，按照线数对激光束分割，根据雷达到激光线上前后5个点与当前点的距离来计算曲率，按照曲率大小筛选角点与平面上的点，考虑到不同距离下激光点的分布疏密不同，进行归一化处理。

(2)

式中：c——曲率；

X——当前激光点坐标；

Xi——第i个点坐标。

根据线和面的不重合度反推出雷达位姿变化[15-16]，由于当前真实位姿未知，假设机器人上一时刻到当前时刻为匀速运动，根据两帧位置计算出机器人速度、角速度，可得到当前机器人的猜测位姿，利用猜测位姿将当前帧点云转换到上一帧点云坐标系下，点云转换方程如式(3)所示。

(3)

pi——第k帧的点；

U——旋转矩阵；

t——平移量。

(4)

(5)

式中：dε——当前帧激光点到上一帧直线的距离；

dΗ——当前帧激光点到平面的距离；

pa、pb——上下相邻激光线上的激光点坐标；

pm、pl、pj——同一平面上按顺时针排序的激光点坐标。

2.2 GNSS数据处理

高精度GNSS信息可提供自身绝对坐标来辅助移动机器人完成准确建图和定位初始化操作，在点云地图中加入GNSS的6DOF姿态作为索引，使机器人在先验地图的初始位置能够计算先验位姿与当前位置的距离完成当前时刻定位，由于定位信息坐标系不同，需要将GNSS与机器人坐标系进行统一，将GNSS系统提供的经纬度坐标转化为机器人载体本身的笛卡尔坐标系下的坐标，转换坐标如式(6)～式(8)所示。

x=(R+H)cosαcosβ

(6)

y=(R+H)cosαsinβ

(7)

(8)

式中：α，β——经纬度信息；

H——海拔高度；

R——地球半径；

a，b——赤道半径和极地半径。

转换得到的坐标与机器人初始姿态仿射对齐到先验地图中完成重定位。

2.3 基于图优化的融合定位方案

基于滤波的融合方案包含了马尔可夫假设，设定过去和未来数据都是独立的，当前时刻状态只与上一时刻状态有关，不受其他时刻影响，导致先前时刻的误差会累积到当前时刻，而基于优化的融合方案，采用scan to map的方式，将之前所有时刻的状态以关键帧的形式保存下来[17]，不断利用观测数据和回环迭代修正累积误差，可以消除整个轨迹的误差，构建因子图，将各个传感器的测量值作为因子加入到系统中。为保证实时性，引入滑动窗口和边缘化方法，不断往滑动窗口内添加新的帧，通过舒尔补模型把先前的关键帧约束保留下来，并进行Hessian矩阵稀疏化处理，以进一步提高系统实时性。

3 果园机器人路径规划算法

3.1 加入启发式函数权重的A*算法

A*算法是在Dijkstra算法的基础上增加了贪心算法，不再像Dijkstra那样毫无目的地搜索，使得路径搜索更具有导向性，更加高效地找到目标节点，两者搜索方法都是基于广度优先搜索，BFS逐层辐射式扩展节点，构建搜索树，将扩展与未扩展节点分别放入open list与close list队列容器中，当目标节点被发现时，停止扩展新的节点，将最终节点作为子节点不断迭代找出所有的父节点，从而回溯出最短路径[18-20]。但单纯的A*算法有时会盲目倾向目标节点导致路径距离变长，为解决此问题，改进后的A*节点扩展规则f(n)公式表达如式(9)所示。

f(n)=g(n)+εh(n)

(9)

式中：g(n)——从起点到节点n的累积代价之和；

h(n)——当前节点到目标节点所需花费代价的估计值；

D——起始点到终点的距离；

d——当前点到终点的距离。

权重系数ε与机器人的工作环境和目标点位置有关，根据目标点不同距离不断调整ε值，因此ε并不是固定值。当ε=0时，A*算法完全退化为Dijkstra算法，生成的路径一定为最优路径。在牺牲一定效率的情况下，权衡搜索速度与路径距离找到最优ε值，减小启发式函数在评价函数内的比重进而减小算法贪婪程度，避免算法过于贪婪，结合了Dijkstra的路径最优性和贪心算法的导向性，解决了生成路径不是最优的问题，如图4所示。

(a)当ε=1时计算出的路径

3.2 动态窗口

本研究采用动态窗口算法完成机器人局部路径规划，在全局地图上使用A*算法进行全局规划后，机器人接收到速度指令开始运动，将局部规划问题用数学描述为带约束的优化问题，其中约束主要包括差速模型的非完整约束，机器人自身的动力学约束以及障碍物约束等，简化履带式移动机器人为差分轮式底盘数学模型。

为保证机器人在安全区域内行驶，需要满足在撞到障碍物前以最大加速度将当前速度vn与当前角速度ωn减为0，则下一时刻动态窗口VD满足式(17)。

(17)

根据线速度和角速度采样点数对连续的速度矢量空间进行离散化处理，得到离散采样点，通过采样点预测生成机器人下一时刻的轨迹，如图5所示，引入评价函数对预测轨迹进行评分，选取最靠近全局路径的轨迹。

图5 速度离散采样示意图

4 果园机器人自主导航性能测试试验

4.1 果园环境建图试验

建立准确且特征明显的三维点云地图是机器人完成定位与导航等后续工作的关键，为验证本研究建图方法的可靠性，在真实果园环境中采集激光雷达原始点云数据、IMU数据。试验区长30 m，宽25 m，以0.5 m/s 的速度在果树行间行走，在同一数据源下运行目前主流的A-LOAM、LeGO-LOAM、LIO-mapping建图算法与本研究方法进行对比，建图效果如图6所示。

(a)A-LOAM

结果表明，A-LOAM算法得到的地图离群点数量多于其他算法，LeGO-LOAM与LIO-mapping虽然占用资源少，运行速度快，但点云密度稀疏，果树树干特征不明显，机器人在不平整路况下运行地图出现漂移情况导致建图失败。本研究建图算法虽然占用计算资源较大，但建图精度高，果树树干特征明显，适合转化为二维栅格地图，相比于A-LOAM、LeGO-LOAM和LIO-mapping算法准确性与鲁棒性均有所提高。

在建图过程中统计各算法点云地图中离群点数量，如图7所示，本文算法在建图开始的10 s内离群点数量剧增，是由于初始阶段点云数据过少，没有足够的线面特征进行匹配，导致点云地图杂乱点较多。

图7 各算法离散点数量统计

4.2 果园机器人定位精度试验

为了验证机器人定位算法精度，选取了没有枝叶遮挡的卫星信号较好区域进行定位对比试验，在上位机上发送指令使机器人按照S型路线行走，以 0.5 m/s 的速度进行直线行走，以0.2 m/s的速度进行270°转弯，记录GNSS测量值与机器人系统基于先验地图的重定位数据，将GNSS得到的测量值作为真值，使GNSS和雷达的定位数据统一到相同坐标系下，如图8所示。

图8 GNSS与本文算法定位坐标对比

采用欧式范数和轨迹插值计算来量化误差，公式描述如式(10)～式(12)所示。

Ex(t)=x(t)G-x(t)S

(10)

Ey(t)=y(t)G-y(t)S

(11)

(12)

式中：x(t)G、y(t)G——GNSS测量值；

x(t)S、y(t)S——机器人定位算法得到的坐标值。

由于机器人定位算法精度与建图精度紧密相关，在机器人转向时点云地图会出现漂移现象，位置与航向定位误差随之增加。机器人开始运行后，对定位误差进行采样，航向偏离向左为正，向右为负，偏差分布如图9所示。

(a)位置偏差分布

结果表明该定位算法定位精度较好，欧氏距离定位误差取绝对值后最大值为0.191 2 m，最小值为0.014 3 m，航向角度误差取绝对值后最大值为7.991 1°，最小值为0.033 6°，机器人转弯时与直行时相比位置偏差明显增加，但位置偏差总体幅度小于0.17 m，航向误差小于13°，能够满足机器人定位需求。

4.3 果园机器人路径规划仿真试验

为验证本文算法较原始算法得到的路径更短，在Matlab仿真环境下进行路径规划仿真，构建面积大小为900 m2的二维栅格仿真地图，模拟果园环境，如图10所示，其中黑色表示障碍物，白色为可行区域，图内三角形所在位置为机器人起始位置，圆形位置为机器人终点位置。仿真结果表示改进后算法较原始算法路径长度减少2.164 m，路径更加平缓，曲折线段减少4处，更适合机器人实际运行，在真实果园环境中测试，机器人采用改进后的路径规划算法到达目标点所用的时间更短，运行路径较为改进前减少5.77 m，运行平稳，未出现转弯幅度过大的现象。

(a)A*算法仿真结果

4.4 果园机器人导航偏差测试试验

将得到的三维点云地图转化为可用于导航的二维栅格地图，上位机界面如图11所示。机器人可在白色区域内自由移动，黑色栅格为障碍物层，灰色栅格表示膨胀层，运行move_base导航框架，在上位机上规定好目标点与目标姿态后，机器人规划出全局路径向目标点移动，如果中途检测到障碍物，机器人会规划出局部路径进行避障。

图11 自主导航过程中上位机界面

向机器人系统发送不同速度指令，跟踪规划路径分别以0.2 m/s、0.5 m/s和0.8 m/s的速度在果园环境中完成自主导航，在运行过程中设置20个中间目标导航点用于测量机器人系统导航偏差，相邻导航点间隔一米，以机器人的初始位置作为坐标原点，机器人坐标系符合右手坐标系，将GNSS测量值作为真值，测量机器人系统实际导航路径与目标路径的横向偏差、纵向偏差，规定机器人到达导航点左侧为正横向偏差，右侧为负横向偏差，到达导航点前方为正纵向偏差，后方为负纵向偏差。在各运行速度下重复试验3次，以各项试验数据的平均值作为测量点处的偏差，如表1所示。

表1 机器人位置偏差

除了导航的横向偏差与纵向偏差外，机器人实际位姿与目标位姿采用航向角误差衡量，机器人到达目标点后实际航向在目标航向左侧为正偏差，在目标航向右侧为负偏差，航向误差如表2所示。

表2 机器人姿态偏差

随着果园机器人的运行速度增加，位置横向偏差、纵向偏差以及航向误差均有所增加，原因在于运行速度过快会影响建图效果以及定位精度，但三种速度下横向偏差与纵向偏差平均值不超过10 cm，标准差不超过7 cm，航向误差平均值不超过10°，标准差不超过6°。试验结果证明该系统精度满足果园机器人在真实果园内自主导航作业的要求。

5 结论

1)针对果园自动化程度低的问题，本文提出一种果园机器人自主导航系统。针对果树点云畸变特性，计算出补偿量对点云进行补偿以消除点云畸变，采用提取线面特征的点云匹配方式计算各时刻相对位姿变化，避免了在果园复杂环境内地图漂移的现象，并提高了激光里程计的准确性，使用基于图优化的融合方案实现建图与定位功能。

2)对原始A*算法进行改进，针对果园环境降低了原始算法的贪婪程度，实际测试中路径长度减少5.77 m，路径更加平缓，更适合机器人实际运动，加入了动态窗口算法，使机器人具有了局部避障的能力。

3)将本文的建图定位与路径规划算法应用到果园机器人上，在真实果园内进行自主导航试验，结果表明，以0.5 m/s的速度运行时导航效果最好，横向偏差与纵向偏差平均值不超过10 cm，航向误差平均值不超过10°。满足果园机器人作业要求。