中共二十大指出，加快建设农业强国，把握精准农业技术是现代化农业建设中不可或缺的一部分。提高农业机械化和智能化水平可以大幅度提高劳动生产率、资源利用率和土地产出率，无人农机可为此提供装备支撑^[1]。然而，无人农机在实际生产中会受到环境等因素影响 ^[2-3]。近年来，传感器技术与定位技术的高速发展使得农机作业的复杂情境可通过仿真建模来实现较高程度的还原^[4-7]，在仿真环境中的测试可分析并解决一部分的技术难点；同时，经过多次仿真测试验证后再开展实际试验的工作模式可适当规避复杂自然环境中的偶然影响因素并提高作业效率^[8]。因此构建一个一体化应用系统，模拟农机在实际环境中的运作情况是农业自动化作业研究的重要方法。

路径规划是农业机械自动导航的关键技术之一^[9]，可分为全局路径和局部路径规划。全局路径规划是在已知环境中进行，更新和修正静态的全局路径^[10]，动态调整路径躲避障碍物。局部路径规划算法针对环境完全未知或部分已知，侧重于作业时的局部环境信息，将在实时路况发生变化时直接影响自主导航的避障性能，进而影响对全局路径规划的修正效果。

郑凯林等^[11]在Stage仿真平台中测试机器人路径规划与导航避障效果，采用直接插入模型的方法，选择了仿真平台列表中的可用模型。王宾^[12]通过调用快速扩展随机树算法(Rapidly-exploring random trees，RRT)节点实现机器人避障，模型采用Gazebo软件列表中可用模型搭建的虚拟机器人模型。郑凯强^[13]提出一种软件定义无人车框架设计，对不同导航算法的可行性进行验证，使用3D Max软件中配套组件进行环境的三维建模，为无人车试验提供环境模型。李献雯^[14]使用单目摄像机采集实际环境路面场景数据，并采用运动恢复结构(Structure from motion，SFM)算法逆向生成实际环境三维模型，该方法虽然能根据作业环境的实际特征仿真并复现环境，但摄像机采集图像的处理过程比较复杂，且可获取点云有限，不能很好地描述较为复杂的地形地貌特征。

上述几种模拟仿真系统未能根据实际作业机器人的物理特性与碰撞属性实现模型仿真，忽略了实际作业机器人自身及引起的影响因素；对于实际地形环境的建模未能真实地体现地形地貌特征，欠缺对机器人行驶的平稳性与抗干扰性的考虑。针对上述缺点与农业机械化精准作业的高效性要求，本文设计一种无人驾驶农机避障路径跟踪仿真验证方法，以无人农机为基础，集成真实复杂地形环境仿真、实际作业农机仿真和路径规划算法植入构建一个一体化仿真验证应用系统。本文基于农机阿克曼结构约束改进了定时弹性带(Time elastic band，TEB)局部路径规划算法，实现无人农机的避障路径跟踪仿真，并在仿真验证应用系统中测试该算法下的路径规划与自主导航避障效果。

1.   材料与方法

1.1   试验材料

农机自主导航系统实现路径跟踪需实际行驶路径与规划路径的横向和航向偏差保持在一定的精度范围之内^[15]，以满足农业生产的需要。

机器人操作系统(Robot operating system， ROS)为基于Linux操作系统的机器人软件框架，是一个开源机器人软件平台，通过整合不同研究成果实现算法的发布与代码的重用，可实现硬件抽象、底层驱动程序管理、进程间消息传递、程序发行包管理等功能。该平台已实现了硬件驱动、模拟仿真、运动规划、运动控制、环境感知等方面的开发。其核心是提供一种点对点的通信机制，进程之间可以直接实现通信并进行数据交换，不需要中转。ROS是进程的分布式框架，使得异构的运算平台从基站服务器到小型的ARM嵌入式系统都可以成为机器人系统的一部分，分散计算压力、降低功能模块间的耦合度。

本试验采用ROS中的Gazebo仿真软件作为仿真平台，该软件可展示作业机器人的各种物理性能如重力、尺寸、惯性、材料、碰撞等。试验环境为崎岖颠簸的田地，需考虑到农机在行驶时由于车速过快或转弯程度过大而翻车的情况，Gazebo仿真环境可以充分体现惯性参数以及碰撞参数对农机行驶平稳性能的影响，为后续研究提供参考。此外，Gazebo还能提供传感器功能，例如，为农机配置的激光雷达模块和惯性测量传感器(Inertial measurement unit，IMU)模块等。本试验采用ROABAG播放采集的实际农田地形环境三维点云数据包，并采用EVO软件进行路径轨迹的误差评估。

无人农机模型为洋马YR-10D型直播机，四轮驱动式小农机，可播种10行，行距25 cm，有11条排水沟，其中，深排水沟3条、浅排水沟8条，适用于南方田间作业，可代表典型水田作业时的农机情况。根据该农机及现有的转向控制模型建模，在仿真验证应用系统中多次测试，完成测试算法的有效性验证后，即可运用该算法为洋马YR-10D型直播机在实际农田中的作业提供路径规划与自主导航服务。

洋马YR-10D型直播机型号为2BDZ-10D(G4)(YR10D)，长、宽、高分别为3430、2640、2330 mm，整机质量为860 kg，发动机型号为3TNM74F-SC，结构型式为水冷3缸4冲程柴油发动机，标定功率为16.0 kW，转速为3200 r/min；行走部驱动型式为四轮驱动，采用HMT高效液压无极变速方式，变速级数为前进2(播种1)、后退1×HMT无极变速。播种方式为自然落种，播种深度为表面落种，种子箱容量为120 L(12 L/个×10个)，行数为10行，行距为250 mm，每穴播种量为5~10粒，有10个外槽轮型式的排种器、10个排种沟开沟器、11个蓄水沟开沟器，采用机械自动平衡装置。该直播机的作业效率(理论值)为0.7035 hm²/h。

采用Velodyne16线激光雷达采集地形环境数据，通过返回测量点到雷达信号接收端的距离与角度计算出各采集点在以雷达为原点坐标系的三维坐标。如图1所示，激光雷达测量机构绕2个互相垂直的轴线旋转(绕Z轴旋转 ± 180°，绕X轴旋转 ± 15°)。

图  1  3D激光雷达测量目标示意图

P为测量点的坐标； O_Z为雷达信号接收端坐标原点； $\alpha $为方位角； $\theta $为仰角

Figure  1.  Schematic diagram of 3D LiDAR measurement target

P is the coordinate of the measurement point; O_Z is the coordinate origin of the radar signal receiving end; $\alpha $ is the azimuth angle; $\theta $ is the elevation angle

下载: 全尺寸图片 幻灯片

测量时系统预先设定范围并扫描，最终得出测量目标的三维坐标P(x, y, z)的计算公式：

式中，l为测量点P到雷达信号接收端坐标原点O_Z的距离，m；$\alpha $为方位角，(°)； $\theta $为仰角，(°)。

1.2   试验方法

为验证避障路径跟踪仿真验证方法的有效性，本文提出一种基于阿克曼结构动力学约束的改进TEB算法，并测试算法的性能。设计避障路径跟踪有效性对比试验和避障路径跟踪平顺性验证试验2组试验进行验证。

在行驶路径上实时添加障碍物，分别采用DWA算法、改进TEB路径跟踪算法，仿真验证无人驾驶农机实时避障效果，进行避障路径跟踪有效性试验。测试时，在行驶路途中分别在距农机不同位置处放置圆柱体障碍物，观察农机是否能准确规划路径避开障碍物，避免陷入障碍物死区或者由于规划路径紊乱而失控。设定一系列农机与Gazebo仿真环境中圆柱障碍物距离，每段距离在仿真环境进行10次导航测试，观察其在行进过程中遇到障碍物时是否绕开障碍物并继续往设定目标点前进。

避障路径跟踪平顺性验证试验主要考察无人驾驶农机避障行驶的平顺性和路径跟踪精度。测试时设置相同的起点与终点，在Gazebo仿真环境中行驶途中放置障碍物，获取农机导航避障过程中实际行驶路径坐标数据，以及由局部路径规划器根据突然添加的障碍物对全局路径更新修正所得规划路径的坐标数据，分析二者轨迹误差。

2.   无人驾驶农机及田间环境建模

2.1   农田环境建模

安装Velodyne16线激光雷达的农机巡检作业进行3D数据的测量和采集，该雷达是可以同时发射并接收多束激光雷达的机械式旋转测距雷达^[16]，每秒可输出高达30万个点数据，具备 ± 15°的垂直视场、360°的水平视场，测量距离半径达100 m以上。即时定位与地图构建(Simultaneous localization and mapping， SLAM)指机器人在试验区域移动的过程中根据位姿估计以及地图进行定位并在此基础上构建增量式地图。基于Cartographer SLAM算法^[17]，通过在试验区域遍历完成三维环境点云的采集，创建三维点云地图。Cartographer SLAM算法通过非线性最小二乘法实现，处理闭环检测数据，获取后端优化后的静态全局地图，使用里程计数据结合传感器接收的激光数据、IMU数据^[18]进行位姿估计，其中IMU辅助激光雷达去除运动畸变部分并进行积分，其积分值作为激光点进行迭代最近邻点(Iterative closest point， ICP)配准的初值并据此迭代求解得到载体位姿与局部子图位姿，激光帧经叠加后形成子图。局部SLAM模块据此进行实时位姿更新与局部子图的建立，全局SLAM实现回环检测功能，根据局部子图优化形成全局地图。

试验农田位于广州市增城区，尺寸为230 m×123 m。首先在ROS系统中搭建SLAM工作环境，运行SLAM功能包，将激光雷达装于无人农机适当位置后，运行建图程序，随着无人农机在试验区域内行驶，激光雷达采集各处点云数据。运行ROS BAG指令开始录制数据，当无人农机在试验区域内遍历完成后，停止录制数据。播放数据包，进行坐标变换的处理，即可得到PCD格式的点云地图。

由于农机行驶采集点云数据过程中获取信息非常频繁，导致2帧数据有大量的重叠。如图2所示，点云信息数据量庞大，计算开销大，因此本文使用Cloudcompare软件对三维点云图进行降采样(Downsample)处理，实现数据匹配以及点云噪声点的滤除，子样本保持原始点云的其他性质。选择八叉树方式根据不同区域点云密度相应调节削减程度，即密度大的区域的点云数据被削减的程度更大。经处理后，点云数由原来的3408484被削减为13473。

图  2  农田环境点云图

Figure  2.  Point cloud map of farmland environment

下载: 全尺寸图片 幻灯片

在点云图的原始三维坐标的基础上计算并配置法向量，并对方向不一的法向量进行最小生成树二次处理，重新定位点云所有法向量后即可呈现清晰正反亮暗两面效果。通过Meshlab软件重建为3D地形模型，然后导入Gazebo仿真环境中，作为仿真地形环境模型。针对不同的作业情境，使用激光雷达采集三维点云数据，通过上述三维重建方法生成不同的地形环境仿真模型，即可集成至应用系统进行相应测试，具备一定通用性。

2.2   无人驾驶农机仿真建模

2.2.1   阿克曼转向模型

轮式农机多采用阿克曼转向结构，这种结构可以通过构建各轴车轮转向的传动机构约束^[19]减小无人农机在行驶过程中的阻力和轮胎的磨损，使各车轮在转向中处于纯滚动无侧滑的状态，有利于提升转向响应速度和农机行驶的平稳性^[20]。

当农机转向时，车轮运动如图3所示，农机的基坐标旋转中心为后轮转轴中点O，当转向轮前轮转弯程度到达极限时，由外转向轮中心引出的垂线与后轮旋转轴所在直线的交点，与旋转中心之间的距离，称为最小转弯半径(R)，可以反映农机通过最小曲率半径弯曲道路和在狭窄路面掉头行驶的能力。为实现纯滚动运动，须使得2个前轮运动方向的法线与通过车轮转动实现前后移动的后轮运动方向的法线，即后轮旋转轴的延长线相交于一点。

图  3  阿克曼结构转向示意图

α为左前轮转向角；β为右前轮转向角；K为后轮两轮距； L为前后轮旋转轴轴距；R为转弯半径；v_l为左后轮线速度；v_r为右后轮线速度

Figure  3.  Schematic diagram of ackermann structure turning

α is the left front wheel steering angle; β is the right front wheel steering angle; K is the wheelbase of the two rear wheels; L is the wheelbase of rotation axis for front and rear wheel; R is the turning radius; v_l is the linear speed of the left rear wheel; v_r is the linear speed of the right rear wheel

下载: 全尺寸图片 幻灯片

根据上述模型中几何结构得出以下约束：

式中， L为前后轮旋转轴轴距，m；R为转弯半径，m；K为后轮两轮距，m； $\alpha $为左前轮转向角，(°)； $\beta $为右前轮转向角，(°)。因此转弯半径受前后轮转轴轴距、后轮两轮距、转向角限制，该控制策略限制了转弯半径的最小值即最大转弯程度。根据上述几何关系可以得到农机在进行转弯运动时的线速度和角速度(以左轮为例，右轮类同)：

式中，$\dot x$为x轴方向的线速度，m/s；$\dot y$为y轴方向的线速度，m/s； $\dot \theta $为角速度，(°)/s；v为农机的总线速度。

阿克曼结构中四连杆是闭环机械结构，无法通过ROS系统中机器人建模URDF文件描述^[21]，因此在实际的导出模型中，通过设置车轮关节替代四连杆的机械结构实现前轮转向控制。

基于无人农机配置阿克曼结构农机Solidworks模型，并根据实际运动状态对作用于运动控制的连杆与关节进行配置，包括连杆父子关系、关节运动形式、关节运动中心或旋转轴的配置等，通过插件Solidworks to URDF Exporter将Solidworks模型导出至ROS系统Gazebo仿真环境中。

2.2.2   运动控制

在ROS仿真环境中，实现运动控制包括运动控制器配置和关节控制消息发布。为四轮配置速度控制器，并为前轮配置位置控制器，将四轮转速、前轮转向角消息的指令信号发布到对应的运动关节以驱动关节运动。

农机的运动信息包括坐标、转向角(${\psi}$)、线速度等，运动学模型如下所示。

农机在t时刻状态向量为：

式中，$ ({x_t},{y_t}) $为农机在t时刻的坐标，$ {\psi _t} $为农机在t时刻的转向角，$ {\dot x_t}、{\dot y_t} $分别为农机在t时刻X和Y轴方向线速度。

则农机在t+1时刻沿全局坐标系的速度为：

式中，$\ddot x$、$\ddot y$分别为农机在t时刻沿全局坐标系的X轴、Y轴方向的加速度，${\dot x_{{\rm{r}},t}}$和${\dot y_{{\rm{r}},t}}$分别表示右轮在t时刻X轴和Y轴方向的线速度，${\dot x_{{\rm{l}},t}}$和${\dot y_{{\rm{l}},t}}$分别显示左轮在t时刻X轴和Y轴方向的线速度，$\Delta t$为时间增量。

则农机在t+1时刻的位移、转向角计算公式为：

综上所述，农机运动学模型为：

农机模型机械结构如图4所示，采用ros_control插件处理所接收的转速、转向角信息。速度控制器在接收转速指令信号后将驱动对应车轮关节转动实现前后移动；位置控制器在接收转向角指令信号后作用于转向轮关节驱动连杆摆动带动前轮转向实现转向控制。各节点之间关系如图5所示。

图  4  农机模型机械结构

Figure  4.  Mechanical structure of agricultural machinery models

下载: 全尺寸图片 幻灯片

图  5  运动控制系统节点关系

Figure  5.  Node relationships in motion control system

下载: 全尺寸图片 幻灯片

农机运动控制的核心是对该阿克曼转向结构中各关节的控制，因此在该仿真验证应用系统中的测试结果亦适用于具备相同阿克曼转向结构的农机。由于该应用系统试验平台为ROS系统，采用了话题通信机制，当需要对不同农机进行作业效果测试时，可通过话题的转换实现农机模型模块化的移植与集成。

2.2.3   路径跟踪控制

全局路径规划基于全局地图进行。在获取里程计数据、激光雷达数据以及融合IMU传感器数据后，对/map坐标系下的农机进行基于自适应蒙特卡洛定位算法^[22](Adaptive monte carlo localization，AMCL)的位姿估计。

AMCL算法是一种多传感器融合定位算法，使用贝叶斯概率实现利用栅格、格子等描述移动机器人的状态空间，递归计算出概率分布^[23]。将里程计信息作为预测量，并融合IMU获取的精确角度信息来弥补里程计中较大的角度累积误差。移动机器人在任意状态下都可推算出自己位姿，AMCL算法通过概率统计的方法利用粒子滤波在已知地图中估计农机位姿，具体流程如下：

初始化粒子群，定义一个大小为m的粒子群，其中每个粒子代表农机在环境中的一个位姿信息，如果初始位姿未知，则每个粒子的位置都是随机的；模拟粒子运动，得出系统初始状态分布$ N\left(x_{0}, P_{0}\right) $，并在农机状态空间分布中采集n个粒子，如式(14)所示：

式中，x为粒子。

计算粒子权重，根据粒子权重长期变化均值${w_{{\rm{slow}}}}$和短期变化均值${w_{{\rm{fast}}}}$，加入经验测量似然$ {w_{{\rm{avg}}}} $并通过设定好的长期变化率$ \alpha_{ {{\rm{slow }}}} $和短期变化率$ \alpha_{ {{\rm{fast}} }} $对上述2种均值进行修正，根据式(15)更新：

其中，加入随机粒子、重复性采样的概率由式(16)确定：

粒子群重采样时为了减少无效计算量，AMCL利用库尔贝克−莱布勒散度(Kullback-leibler divergence，KLD)采样，计算粒子及对应的数目：

式中，$\delta $ 为标准正态分布下的 $\alpha $ 分位点，即满足条件P(x≤$\delta $)=$\alpha $的实数； $ z_{1-\delta} $是上界为 $1-\delta$ 的标准正态分布置信度；$\varepsilon $ 表示一个非常小的正数的阈值。

计算机器人在t时刻的位姿，即粒子集的数学期望：

在上述位姿估计信息的基础上，即可基于Gmapping SLAM算法构建出试验区域的二维全局地图。

将所建全局地图应用于导航中的全局规划，如图6所示，农机在导航行驶时要求避免与代价地图接触，系统根据位姿估计实时更新路径规划，对障碍物的检测识别将会减小局部地图局部路径规划器代价地图覆盖范围，进而对全局路径进行修正，实现实时避障。

图  6  导航过程示意图

全局路径以紫色曲线表示，局部路径规划以农机行驶时沿实际运动方向的绿色短线表示

Figure  6.  Navigation process diagram

The global path is represented by a purple curve, while the local path planning is represented by a green short line along the actual movement direction of theagricultural machinery during operation

下载: 全尺寸图片 幻灯片

本文基于move_base算法实现导航，默认局部路径规划算法为动态窗口法(Dynamic windows approaches， DWA)，通过$ V_{\rm{s}}、 V_{\rm{d}}、 V_{{\rm{a}}} $ 3种约束条件精确化减小速度空间的搜索范围，最终获取该时刻机器人的无碰撞速度，在该速度约束空间内机器人不会发生碰撞。然后通过评价函数评价可选轨迹，选取最优解^[24]。但是该算法并未充分考虑针对本文农机阿克曼动力学结构的非完整约束动力学情况，当遇到障碍物时，该算法无法决策出适用于阿克曼转向结构的有效避障路径，完成有效地避障操作。

在局部路径规划中，动力学模型将成为更重要的影响因素，因此，本文提出基于阿克曼机械结构约束的改进TEB局部路径规划算法，不断在适量空间中获取实时转向控制信息并将其发送至前轮的位置控制器，完成所设定的各项约束。相比于move_base导航算法默认采用的DWA局部路径规划算法，该算法实现了对具有非完整约束的阿克曼无人农机动力学的考虑，当遇到障碍物时可实现车轮转向关节、驱动关节准确并快速地响应，决策出有效的避障路径。

TEB算法通过在全局路径中根据设定约束插入N个控制点对全局路径修正，这些控制点是一些规划的各个时刻的离散位姿数据，这些离散位姿用$P $表示，表示为式(19)。

式中，每个点的位姿用$ p_{i}=\left(x_{i}, y_{i}, \psi_{i}\right) $表示，分别描述所处位置的横坐标、纵坐标和转向角。

在2个位姿点之间设置时间间隔$ \Delta t_{i} $。将所有相邻位姿点时间间隔集合用$T $表示：

因此，TEB路径定义为2个序列的元组($M $)：

TEB路径约束包括机器人动力学约束、最快路径约束、速度约束、路径避障约束等。其中，速度约束($ f_{ {{\rm{vel }}}} $)限制机器人的最大线速度与角速度，速度限制以损失的形式实现，线速度$\dot{x}_i 、 \dot{y}_i$ 以及角速度$\dot{\theta}_i$根据向后差分可得：

最快路径约束($f_{{\rm{t}}}$)计算公式为：

路径避障约束$f_{ {{\rm{path}} }}$与$f_{{{\rm{obs}}}}$，在TEB算法规划局部路径时，须以跟踪全局路径为基础，通过f_path实现；并基于此避开实时障碍物，即全局路径吸引TEB路径规划点，通过f_obs实现。约束以惩罚函数行驶实现如式(27)、(28)所示。

式中，$d_{\text {min }}$为TEB局部路径规划点和障碍物的最小距离，m；$r_{p \max }$为TEB局部路径规划点与全局路径规划点的最大距离，m ；$r_{o \min }$为TEB局部路径规划点与全局路径规划点的最小距离，m ；S表示缩放系数；n是多项式阶数；$\varepsilon $ 为边界值附近的1个小位移，为约束增加裕度。

针对阿克曼机械结构，结合式(2)、(3)，在局部路径算法中对农机的最小转弯半径等几何参数加以约束，针对实际地势起伏不平的田地环境，有效提高农机行驶过程中对规划路径的跟踪程度以及避障的有效性。

以上TEB约束的求解采用图优化方法，将每一个状态和时间间隔作为顶点，各种约束作为建立超图的元素进行多目标优化，并使用g2o框架求解。

测试时将农机模型的基坐标模型起点设定为[−5.5，0]，终点设为[5.5，0]，农机模型的前、后轮距离为1.10 m，由式 (2)、(3)可知该农机模型的最小转弯半径应为2.177 m。多次测试调试后，整定的参数为：改进TEB算法中最小期望距离为2 m，轨迹时间分辨率为0.1 s，最大线速度为2 m/s，线加速度为0.5 m/s²，倒车最大速度为0.5 m/s，最大角速度为286.48 (°)/s，角加速度为143.24 (°)/s²，考虑优化全局路径子集最大长度为1 m，到达目标允许误差范围为0.5 m，到达目标角度允许误差范围为0.5 m。

3.   结果与分析

3.1   避障路径跟踪有效性测试结果

3.1.1   DWA路径跟踪效果

无人农机与障碍物距离为3.8~8.0 m区间内，分别进行了10次避障测试，测试结果如图7所示。改进TEB算法的最短有效避障距离达4.1 m；DWA算法，由于距离为8.0 m时，避障成功率尚未达到0.5，因此最短有效避障距离大于8.0 m。

图  7  避障成功率

Figure  7.  Obstacle avoidance success rate

下载: 全尺寸图片 幻灯片

在DWA算法避障的测试中，避障失败的主要表现有2种，一是由于缺少全局指引，在局部最小测试中陷入局部最优而无法脱离圆柱体障碍物；二是由于农机根据规划路径行驶时转弯半径过小，即转弯程度过大，绕开障碍物后偏航严重，无法回到全局路径中。

当农机与障碍物距离区间为3.8~4.5 m时，避障失败的原因均为无法绕过障碍物，DWA算法在局部距离最短测试中陷入了局部最优而无法脱离圆柱体侧面的凹形障碍物；当距离为6.0 m，避障失败情况中有2次是由于农机根据规划路径行驶时转弯半径过小，即转弯程度过大，绕开障碍物后偏航严重，局部路径规划无法将运动控制拉回至全局路径，农机无法回到向目标点行进的路径当中；当距离为8.0 m时，偏航情况出现6次。

3.1.2   阿克曼机械结构约束的TEB避障效果

采用基于阿克曼结构约束的改进TEB算法，最短可避障距离相较DWA算法提升至少3.9 m。避障过程如图8所示，规划农机自起点至终点的全局路径；当靠近障碍物时，如8b所示，根据局部路径规划，修正全局路径以避障；如图8c所示，农机根据规划路径成功避障到达设定目标点，未偏离全局路径。因此，测试结果表明本文提出的基于阿克曼动力学约束的改进TEB局部路径规划算法的避障路径跟踪有效性良好。

图  8  避障路径规划

图中紫色曲线为农机自起点至终点的全局路径；绿色曲线为局部路径修正的全局路径

Figure  8.  Path planning of obstacle avoidance

The purple curve in the figure is the global path planned from the starting point to the end point of the agricultural maehinery; The green curve is the global path corrected by the local path

下载: 全尺寸图片 幻灯片

3.2   避障路径平顺性测试结果

距离实时障碍物4.5、5.0 m时，农机实际行驶路径对规划路径的跟踪情况如图9所示。由图9可知实际路径和规划路径的相对误差随着时间推进逐渐逼近0。由“3.1”可知本算法最短有效避障距离可达4.1 m，因此图9a中实际行驶路径与规划路径的相对误差明显大于图9b。图9b中农机图像尾端Y轴方向坐标相对误差稳定值约为0.7 m；图9a中两轴方向坐标相对误差稳定值接近0。距离障碍物5.0 m时，实际行驶路径与规划路径误差均值为0.4305 m，均方根误差为0.3151 m，均小于0.45 m；距离障碍物4.0 m 时，实际行驶路径与规划路径均方根误差为1.3538 m，误差均值为1.6126 m，均小于1.65 m，农机实际行驶路径对规划路径的跟踪状况良好、精度较高。

图  9  距障碍物不同距离时实际行驶路径与规划路径的相对误差

Figure  9.  Relative errors between the actual driving path and planned path of obstacles at different distances

下载: 全尺寸图片 幻灯片

由上述试验结果可知，农机在田地仿真环境中导航作业时，若障碍物距离大于5 .0 m时，可控误差均值≤0.4305 m，均方根误差≤0.3151 m；若障碍物距离为4.5~5.0 m时，可控误差均值≤1.3538 m，均方根误差≤1.6126 m。上述结果表明基于阿克曼结构约束改进的TEB算法对局部路径的规划控制效果良好，满足农机自动导航系统对路径规划的精度要求，验证了算法的可行性。

4.   结论

设计了一种无人驾驶农机避障路径跟踪仿真验证方法，集成真实复杂地形环境仿真、实际作业农机仿真、路径规划算法植入构建一个一体化仿真验证应用系统，该应用系统易于扩充，是一个通用的仿真验证应用系统，适用于各种地形地貌的作业环境。基于三维SLAM技术采集环境点云数据实现农田地形环境仿真建模，有利于体现测试算法下农机对复杂路面的适应性；适用于不同机械结构的农机，通过ROS系统话题转换实现农机模型模块化的通用移植与集成。

在仿真验证应用系统中，基于洋马YR-10D型无人驾驶直播机研究了阿克曼结构模型和无人驾驶农机动力学模型，测试结果及结论适用于具备相同阿克曼转向结构的农机。基于阿克曼结构和无人驾驶动力学约束，设计了改进型TEB避障路径跟踪算法，验证了无人农机在农田环境中作业的路径规划效果。

避障路径跟踪有效性测试结果表明该算法规划下最短有效避障距离达4.1 m，较传统DWA动态窗口法提升了3.9 m，验证了其有效性。避障路径平顺性测试结果表明，在局部路径规划采用基于阿克曼结构动力学约束的改进TEB算法，农机自主导航行驶时可较好地跟踪规划路径，满足精度要求。障碍物距离大于5.0 m时，可控误差均值≤0.4305 m，均方根误差≤0.3151 m；障碍物距离小于5.0 m大于4.5 m时，可控误差均值≤1.3538 m，均方根误差≤1.6126 m。

各项测试结果表明，基于阿克曼结构动力学约束的TEB算法具有较强的作业能力以及较高的作业精度，满足无人驾驶农机避障路径跟踪仿真验证的需求，后续可应用该算法于无人农机在实际农田环境的避障路径跟踪。模型的惯性参数与碰撞参数未能很好地调试和校正，导致无人农机行驶在地势起伏不平的农田仿真环境中平稳性欠佳，后续研究应进一步调整校正。为无人农机模型配置的阿克曼前轮转向轮结构在实际运动时仍有缺陷，无人农机在仿真环境中行驶时出现震荡现象，该部分机械结构设计有待进一步完善。