丘陵山区田间道路智能农业装备的自主避障算法研究*

林先卬， 李云伍， 赵颖， 王月强， 杨洪涛

1. 西南大学 工程技术学院， 重庆 400715； 2. 长安汽车智能化研究院， 重庆 404135

为贯彻实施《“十四五”全国农业机械化发展规划》， 西南地区正在加快推动农业机械化、 智能化、 绿色化， 丘陵山区田间道路智能农业装备的自动行驶及避障技术是西南地区现代农业发展需重点解决的技术问题.

在国内， 张强等[1]针对山地环境通过离散化和空间映射， 把三维路径规划问题转为二维平面路径规划问题. 蒋浩[2]基于RTK技术研制了农业机械自动导航系统. 李云伍等[3]采用RTK-GNSS系统和视觉导航系统实现丘陵山区田间道路自主转运车的自主导航行驶， 但面对道路上的新增障碍物不能有效自主避障. 劳彩莲等[4]提出了基于改进A星算法与动态窗口法结合的温室机器人路径规划算法， 在保证平滑性和实时性的基础上进行全局最优路径避障， 但环境复杂度增加和地面打滑， 导致实际目标点到设定的目标点误差越来越大. 张漫等[5]对农业机械全局路径规划算法与局部路径算法进行对比， 提出每种算法各有特点， 需根据具体作业环境和作业要求选用不同的算法.

在国外， Luo等[6]在葡萄园中提出了一种称为Agrobpp-bridge的解决方案， 可从卫星图像中自主提取占用网格和拓扑图， 减少了人为干预. Backman等[7]采用二维激光雷达提取导航跟踪线， 采用GPS和惯性测量单元识别农用机械的前进方位. Liu等[8]提出了一种基于ROS平台的非线性模型预测的独立路径规划控制器， 用于农用拖拉机避障. AI-Mashhadani等[9]基于ROS平台运用DWA算法在温室环境进行仿真避障， 但未进行实际试验验证. Qin等[10]运用Kinect深度摄像头计算农业机器人前方的点云数据个数， 判断是否存在障碍物并计算障碍物点云质心.

上述研究都没有充分解决丘陵山区田间道路智能农业装备的避障问题， 面对可见度差、 天气变化复杂等工况， 依靠单传感器进行避障可靠性不高， 基于多源信息融合的避障方案还很少[11]. 本文基于全球导航卫星系统(Global Navigation Satellite System， GNSS)和惯性导航系统(Inertial Navigation System， INS)以及激光雷达， 首先利用改进的反演模型算法和滚动更新设计， 通过激光雷达实时建立二维动态栅格地图， 提供局部范围的障碍物位置信息； 其次， 基于GNSS/INS定位系统提供的全局路径和实时定位信息， 结合二维动态栅格地图和改进A星算法形成局部避障的感知规划框架； 最后， 对改进的A星算法仿真分析， 并对避障系统进行实车避障试验， 验证该实时避障系统的可靠性和安全性.

1 激光点云数据处理

图1 二维障碍物激光图

激光雷达通过激光源发射激光击打到障碍物上， 再利用接收器接收反射回来的激光， 通过激光射出到反射回来的时间计算出障碍物到激光源的位置， 同时通过判断接收器接收到的信号强度来判断该障碍物的折射率.

由于二维栅格图包含二维坐标系下的障碍物位置信息， 因此， 需要把激光雷达发布的三维点云信息转化为二维激光束， 得到的激光束主要包含以下两个属性： 长度、 发射角度， 把点云图转化为二维激光扫描图， 如图1所示， 三维点云数据平面化处理为二维障碍物栅格图的创建提供了基础数据.

2 基于改进反演模型的动态栅格地图

2.1 传统反演模型原理

图2 位姿已知的栅格图构建模型

在激光雷达位姿已知情形下构建栅格图的数学模型如图2所示：

从图2看出，m={mi}为所有栅格的集合，zt，zt-1，zt+1分别为t时刻、t-1时刻、t+1时刻的激光测量值；xt-1，xt，xt+1分别为t-1时刻、t时刻、t+1时刻的机器人(激光雷达)的位姿[12]， 因而单个栅格被占用的概率值可描述如下：

p(mi|z1：t，x1：t)

(1)

式中：p表示概率值； 1表示栅格地图构建的起始时刻；z1：t表示1到t时刻累计激光测量值；x1：t表示1到t时刻的位姿；t表示建图结束时刻. 假如地图中有1 000个左右的栅格单元， 那么整个地图可能会有21 000种结果， 后验概率的计算量非常大， 因此采用静态二值滤波器来计算整个地图的概率[13].

(2)

式中：p(m|z1：t，x1：t)表示整个地图所占用的概率值；p(mi|z1：t，x1：t)表示每个栅格占用的概率值. 单个栅格经过t时刻后的概率值为

(3)

同理， 可得到该栅格未被占用状态时的概率值p(mi|z1：t，x1：t)， 两者相比：

(4)

为了提高栅格值更新的计算效率， 采用log函数把计算机运算困难的乘法变成运算简单的加法. 覆盖栅格地图的算法伪代码如表1所示.

表1 覆盖栅格地图算法伪代码

表中：

(5)

(6)

(7)

式中：lt-1，i表示栅格mi在t-1时刻状态值；l0表示栅格mi先验值， 即对栅格地图中每个栅格赋的初始值. 函数inv_sensor_model应用了反演测量模型的对数型式， 如下式：

(8)

式中：p(mi|zt，xt)表示栅格被占用的概率值；p(mi|zt，xt)表示未被占用的概率值.

图3 Bresenham’s算法示意图

基于此， 采用Bresenham’s算法作为反演模型的算法将实时收到的激光转化为栅格占用值， 如图3所示. 图中x坐标和y坐标系为栅格地图的坐标系，x′和y′坐标系为移动装备的行驶坐标系， 将激光雷达安装在行驶平台的顶部. 图中激光束刚好射到黑色栅格中的物体上， 激光束经过的蓝色栅格为空闲.

结合图3， 反演模型的算法流程如式(9). 首先， 给每个栅格设置一个初始值m0， 遍历该帧激光流的所有光束， 被击中栅格的更新状态方程为

(9)

激光扫过的空闲栅格更新状态方程为

(10)

2.2 栅格地图优化

在丘陵山区田间道路中， 往往会出现新增的静态障碍物， 这也是田间路径规划所需考虑的重要因素. 传统反演模型适合构建全局栅格地图， 对于丘陵山区， 室外建图存在激光反射率较差和算法难以回环检测的问题， 所建地图无法满足丘陵山区导航的实用需求. 为了解决上述难点， 提出利用改进的反演模型来构建动态栅格地图并加入滚动更新原理， 提供局部范围内的障碍物位置信息[14-15]. 根据试验平台和参数调试结果， 动态栅格地图的大小确定为8×8 m.

图4 改进的反演模型

2.2.1 反演模型优化

为了保证避障系统的感知部分的实时性能， 减少计算量， 在构建动态栅格地图前首先对激光雷达数据的激光束反射点位置进行判断， 再分别进行处理. 如图4所示， 其中x和y为大地坐标系，x′和y′为激光雷达装载平台坐标系， laser_1和laser_2分别为两束不同方向和位置的激光.

在图4中， laser_1激光束反射点位于地图范围内， laser_1激光束击中栅格和途径的栅格按反演模型更新栅格值， 同时击中栅格到边界栅格也视为占用； laser_2激光束反射点位置位于地图范围外， 则laser_2激光所经过的栅格全部更新为空闲状态.

图5 坐标转换示意图

2.2.2 滚动更新设计

引入坐标转换原理， 把历史帧的动态栅格地图占用信息作为参考量融入当前帧的动态栅格地图， 保证了动态栅格地图中的栅格占用位置信息是多帧激光雷达数据中障碍物位置信息位姿矩阵变换所得， 使得局部栅格地图中的占用位置的准确度概率得到了提升. 滚动更新原理如图5所示， 动态栅格地图随着车身转动和移动后，P点在o′x′y′坐标系下的位置为(x′，y′)， 那么转换到原坐标系oxy下， 坐标位置(x，y)表示为：

(11)

式(11)中θ为两个坐标系之间转动的角度，a，b分别为o′x′y′坐标系对比原坐标系oxy移动的横向距离和纵向距离.

坐标转动后， 图中蓝色点P此刻在o′x′y′坐标系下该时刻所在栅格初始值为P点转动前在原坐标系oxy点(x，y)所在栅格的栅格占用值. 即：

p(x′，y′)=p(x，y)

(12)

式中：p表示该坐标所在栅格的栅格占用值. 坐标转换提高了动态栅格地图稳定性， 保证不同时刻栅格地图在同一位置的占用概率的准确性.

3 改进A星算法

为了提高丘陵山区田间道路局部路径规划算法的鲁棒性， 需融合GNSS/INS定位信息以及动态栅格地图， 于是采用基于图搜索算法进行改进. 常用的基于图搜索算法有： A星算法、 Dijkstra算法、 GBFS算法， 而A星算法保证路径最优同时具有高搜索效率[16-17]， 因此， 运用改进代价函数的A星算法， 把GNSS/INS系统定位信息预存为参考路径， 作为局部路径规划的参考线.

图6 参考线为基础的s-l坐标系

3.1 搜索点与代价函数改进

图6为以参考线为基准建立的s-l坐标系， 在参考线的法向上离散一定数量的搜索点， 在图6中用蓝色点示意. 改进算法的思路是： 首先， 根据行驶装备的位置信息搜索参考线中与装备位置相近的点作为路径规划的起点； 其次， 找出在参考线与装备行驶方向的栅格地图交界点， 在该交界点的l方向的离散点都可以作为局部规划终点； 最后， 在离散点从起点开始搜索[18].

A星算法的评价函数为

f(Vi)=g(Vi)+h(Vi)

(13)

式中：f(Vi)表示起始节点经由当前节点和目标节点构成的评价函数；g(Vi)表示起始节点到当前节点的代价；h(Vi)表示当前节点与目标节点之间的估算代价[19]， 实际为当前节点与s轴投影的弧长差Δs：

h(Vi)=sgoal-s(Vi)

(14)

式中：sgoal表示s-l坐标系的局部目标点在s轴上对应的弧长；s(Vi)表示当前节点对应的参考线最近点的弧长.

g(Vi)由递归得到：

g(Vi)=g(Vi-1)+c(Vi-1，Vi)

(15)

式中：Vi-1代表节点Vi的父节点；c(Vi-1，Vi)代表父节点到当前节点的代价值， 使局部规划路径尽可能靠近参考线和保持安全， 本研究采用多权重代价函数去约束路径.c(Vi-1，Vi)代价函数主要为两部分：

c(Vi-1，Vi)=woco(Vi)+wdcd(Vi)

(16)

式中：co(Vi)表示当前节点与参考线的偏移代价；cd(Vi)表示当前节点与障碍物的安全代价；wo，wd分别为各代价函数对应的权重系数.

为了保证局部路径尽可能接近全局路径， 偏移量是决定局部路径能回到全局路径的重要参数， 设在全局路径的法向的横向偏移量为l， 则横向偏移代价为横向偏移量的绝对值与最大横向偏移量lr的比值， 即：

co(Vi)=|l|/lr

(17)

通过局部路径规划的节点在动态局部栅格地图上与障碍物的距离信息， 判断该距离是否在安全距离内， 若节点不在安全距离内则引入安全代价函数， 则：

cd(Vi)=(ds-di)/ds

(18)

式中：ds表示节点与障碍物之间的安全距离；di表示该节点到障碍物之间的距离. 考虑丘陵山区田间道路的横坡对规划轨迹曲率的影响， 经过多次参数调优， 式(16)中wo和wd取值分别为0.45和0.55时效果较好.

3.2 改进A星算法的仿真分析

在ROS系统进行算法离线仿真， 采用3张不同栅格地图作为规划的场景， 白色为可行区域， 黑色为被障碍物占用区域， 地图分辨率为0.1 m×0.1 m. 分别在3种不同场景设置同样的起点、 终点， 对比改进A星算法与传统A星算法， 其仿真结果如图7所示.

图7 改进A星与传统A星算法在3种场景中的仿真结果

如图7所示， 箭头表示起点和终点， 图7中的(a)，(b)，(c)图为改进A星算法的仿真结果， 其中设有基于红色点的参考路径， 绿色曲线为规划的结果. 图7中的(d)，(e)，(f)图为传统A星算法的仿真结果， 无参考路径， 粉红色曲线为规划的结果. 在3个场景中， 改进A星算法与传统A星算法的起点和终点设置一致， 运行20次， 得到两种路径规划算法的规划时间、 与参考线的偏离度、 与障碍物之间的最小距离， 取其平均值， 结果如表2所示.

表2 两种路径规划算法的对比

根据表2对比结果可知： 改进的A星算法单次规划的平均时间为18 ms， 与参考线的平均偏离度为31.49%， 与障碍物的最小距离的平均值为0.46 m， 较传统A星算法在安全性、 平滑性方面都有明显的提升， 且与参考线保持靠近.

4 试验与分析

为了验证动态栅格地图和改进的A星算法在丘陵山区环境用于避障及自主导航的可靠性， 分别进行了局部避障和全局自主导航的实车试验. 图8所示为无人履带车驾驶平台， 无人履带车驾驶平台搭载了GNSS/INS融合系统、 激光雷达和电脑， 采用激光雷达和GNSS/INS融合系统为无人履带车完成避障和导航功能.

图8 无人履带车驾驶平台

4.1 局部路径规划试验

通过摆放锥形筒添加障碍物， 履带车以1 m/s的速度匀速驶向障碍物， 使用机器人操作系统显示平台(RVIZ)来显示障碍物场景下动态栅格地图中的路径规划情况.

如图9所示， 图中黑色地图部分代表是障碍物阻挡区域， 栅格地图中前方的黑色区域表示车体前方的障碍物， 栅格地图中后方黑色区域与实况对应的部分为激光雷达后方的车体， 白色区域为可行驶区域， 图中心红色箭头代表规划起点车体位姿， 图边界点红色箭头代表局部目标点车体位姿， 局部目标点是从全局的预存路径中选取的.

图9(a)是障碍物还未进入到动态栅格地图范围内时的局部路径规划状态， 图9(b)，图9(c)，图9(d)是随着履带车驶向障碍物， 不同时刻的局部路径规划结果. 规划路径结果发送给底层控制模块， 两侧电机转速得到控制. 由试验结果可知， 该算法在实际使用过程中， 避障效果良好， 生成的路径较平滑， 在CPU为i5-8250U的算力下， 生成动态栅格地图并进行路径规划周期时间为50～100 ms， 满足实时避障要求.

图9 试验避障效果图

4.2 全局自主行驶与避障试验

为验证基于动态栅格地图局部规划路径算法的工程实用性， 采用单点预瞄算法对规划的路径进行跟踪控制， 用编码器和PID算法对底层电机转速进行调整， 使指令转速与实际电机转速的误差减小.

图10为实车试验环境卫星图， 此实车试验环境为铺有1.5 m宽的水泥路面非结构化道路， 选取40 m长的直线路段和曲线路段， 无人驾驶履带车以1 m/s匀速行驶进行自主导航与避障.

图10 实车试验环境卫星图

由图11(a)可得， 直线路段下， 3次避障都成功完成， 在前两次连续避障中， 与参考线横向偏移量较大， 最大偏移量为1 m， 最后一次避障经过长距离跟踪后， 与参考线横向偏移量明显减小， 说明底层PID反馈控制逐渐稳定； 图11(b)中， 实际行驶路径曲率范围为-0.015～0.015 m-1， 最大值为0.015 99 m-1， 行驶较平顺.

图11 直线路段实际行驶路径与路径曲率图

图12(a)中， 曲线路段实际行驶路径与预存路径偏移量较直线有所增加， 但顺利完成了3次避障. 图12(b)中， 实际行驶路径曲率在-0.026～0.025 m-1， 最大值为-0.026 94 m-1， 整个过程比直线路段曲率增大， 但仍满足丘陵山区规避静态障碍物的路径的平滑性要求.

图12 曲线路段实际行驶路径与路径曲率图

综上所述， 在直线和曲线路段两种工况， 基于动态栅格地图的局部路径规划算法均避障成功， 并继续沿着全局路径跟踪行驶， 规划路径的曲率满足智能农业装备对行驶轨迹的平滑性要求.

5 总结

本研究针对丘陵山区田间道路环境特点， 运用优化的Brensman’s算法将激光雷达的点云数据进行栅格化并加入滚动更新设计， 实时处理了移动平台下的数据噪声问题， 让动态栅格地图更加稳定. 同时， 考虑丘陵山区田间道路的横坡对行驶横向加速度的影响， 运用改进的A星算法结合GNSS/INS系统预存路径的偏移量、 与障碍物之间的距离设计了代价函数. 最后， 运用单点预瞄算法对生成的路径进行路径跟踪并进行了实车试验， 得出以下结论：

1) 动态栅格地图通过优化的Bresenham’s算法和滚动更新原理， 能够实时和稳定地更新障碍物位置， 并更精准地检测到障碍物位置.

2) 改进的A星算法更安全、 平滑， 与动态栅格地图耦合更紧密. 改进的A星算法因在参考线上的离散点搜索， 规划时间更短， 规划路径始终与参考线靠近且与障碍物保持一定的安全距离.

3) 试验实际行驶路径曲率在-0.026～0.025 m-1， 曲线路段行驶轨迹曲率波动较直线路段有所增加， 但仍满足丘陵山区田间道路的智能农业装备自主行驶对平滑性、 安全性的要求.