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

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

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

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

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

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

同理，可得到该栅格未被占用状态时的概率值$p\left(\rightharpoondown m_i \mid z_{1: t}, x_{1: t}\right) $，两者相比：

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

表中：

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

式中：p(m_i|z_t，x_t)表示栅格被占用的概率值；$p\left(\rightharpoondown m_i \mid z_t, x_t\right) $示未被占用的概率值.

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

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

式中：m⁺表示更新后的栅格值；m^-表示更新前的栅格值；$\log \frac{p(z=1 \mid m=1)}{p(z=1 \mid m=0)} $表示在测量值z=1(击中栅格)的后验概率.

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

式中：$\log \frac{p(z=0 \mid m=1)}{p(z=0 \mid m=0)} $表示在测量值z=0(未击中栅格)的后验概率.

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

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

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

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

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

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

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

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

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

A星算法的评价函数为

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

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

g(V_i)由递归得到：

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

式中：c_o(V_i)表示当前节点与参考线的偏移代价；c_d(V_i)表示当前节点与障碍物的安全代价；w_o，w_d分别为各代价函数对应的权重系数.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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