垄间无人运输车自主导航纠偏方法*

随连杰 ， 刘义亭 ， 郁汉琪

（1.南京工程学院研究生院，江苏 南京 211167；2.南京工程学院自动化学院，江苏 南京 211167；3.东南大学信息科学与工程学院，江苏 南京 210096；4.南京工程学院工业中心，江苏 南京 211167）

0 引言

《中国制造2025》将农业机械自动化列入我国发展的十大重点领域，旨在2025年全面实现农业产品结构升级并突破高端产品技术应用[1]。农业机器人是结合机器人技术、自动控制技术、无线传输技术等技术共同开发出来的一种具体的以农业生产为背景的生产技术[2-4]。近年来，随着物联网技术、深度学习、大数据等理论知识的普及，农业机器人也逐渐产生并完善，在具体的农业生产中也有应用[5]。Plessen[6]提出了两种规划模式，通过使用减轻重量的控制方法和单次田间覆盖方式来减轻土壤压实。Jensen等[7]提出了一种用于农业作业中运输单元的路径规划方法，涉及田间和田间运输，以优化时间或行程标准。Zhang等[8]举例说明了一种路径模式，通过避免在地头耗费时间，引导履带式联合收割机从外到内工作。此外，地头转弯是农场工作中的一个重要考虑因素，在很大程度上影响工作效率[9]。相应地，各种自动导航系统由研究人员和制造商开发研制，以促进农业生产的自动化[10-11]。

无人运输车在进行温室大棚垄间作业时，单纯只靠激光雷达一种传感器，容易出现两侧预留空间不够而导致导航失败的情况，甚至产生碰撞危险。因此，设计融合激光测距仪、激光雷达以及陀螺仪三种传感器的导航纠偏系统，该系统适用于温室大棚内垄间作业时自动纠偏的导航运输车设备。

1 整机结构与硬件设备

1.1 总体架构

如图1所示，农业无人运输车导航系统可由三个模块构成：1）PC端从机与工控机交互模块，该模块主要功能是将建图与导航过程可视化；2）激光雷达、里程计、激光测距仪和IMU传感器模块，该模块主要功能是为导航系统提供机器人位姿信息与环境信息；3）建图与导航功能模块，该模块主要功能是通过SLAM与路径规划算法实现机器人建图与导航。

图1 农业无人运输车总体架构图

1.2 硬件设计

本文所用的导航无人运输车的硬件结构为两轮差速驱动结构，如图2所示。左右驱动轮线速度的计算公式如下：

式中，φ1表示右驱动轮转速，单位m/s；φ2表示左驱动轮转速，单位m/s；r表示驱动轮半径，单位m。

则无人运输车的导航线速度Vd和导航角速度ωd分别为：

式中，L表示导航无人车中心到两轮的距离，单位m。

如图2所示，陀螺仪安装在导航无人车的前方中心，用于测量导航无人车的三轴角速度。激光测距仪分别安装在导航无人车的右前、右后、左前、左后位置，用于测量导航无人车与两侧田垄的距离。

图2 无人运输车硬件结构示意图

2 融合定位与纠偏策略

2.1 扩展卡尔曼滤波融合IMU

系统矩阵和测量矩阵的线性化将导致误差协方差矩阵和卡尔曼增益成为状态估计的函数。当卡尔曼滤波器使用先前的最优估计而不是预定义值应用于线性化时，则称为扩展卡尔曼滤波器[12]。最佳估计值用于初始化卡尔曼滤波器，然后将新的估计值作为下一时期的输入值。因此，扩展卡尔曼滤波器使用闭环滤波器的架构。在闭环校正结构中，将估计的误差反馈给IMU进行校正，这样可以保持系统模型的线性假设；相反，在开环系统中，误差或校正状态不会在每次迭代时反馈给IMU。因此，本文采用扩展卡尔曼滤波的闭环架构来进行融合，将其运用到非线性系统的过程如下所示。

1）状态模型：

式中，^xk表示k时刻的状态向量；uk表示控制向量，如加速度、角加速度等；Qk表示预测状态的高斯噪声的协方差阵，它用来衡量模型的准确度，模型越准确其值越小表示状态向量的协方差矩阵，代表着状态向量每个元素之间的关系。

2）更新模型：

式中，Rk表示测量值的高斯噪声的协方差阵，代表着传感器测量的误差。Fk和Hk分别是f和h的偏导雅可比矩阵，可由式（10）、（11）表示：

2.2 纠偏判断方法

横向偏移量DP通过同侧两个激光测距仪所测距离与目标距离设定值作差得到，横向偏移量DP的计算公式如下：

式中，C1、C2表示同侧激光测距仪所测得的数据，单位m；Dset表示右侧目标距离设定值，为0.5 m。

航向偏移量AP通过同侧两个激光测距仪所测距离的差值与同侧激光测距仪的距离相除后的反正切值得到，航向偏移量AP的计算公式如下：

式中，pdist表示同侧激光测距仪的距离，单位m。

将计算所得到的横向偏移量DP与初始设定的距离标准值Dm相比较，同时根据路况信息对是否需要纠偏作出判断。根据多次测量比较，最终距离标准值Dm设置为30 mm。若横向偏移量DP大于距离标准值Dm，同时路况信息判断该路况为非转弯路段，则判定该导航无人车未进行直线前进，需要在Y方向上控制横向偏移量，直到所测100组激光测距仪的数据的横向偏移量绝对值小于或等于距离标准值的数量大于等于50组。

图3为无人运输车在进行Y方向上调偏时，同侧激光测距仪测量数据以及横向偏差量数据。如图3（a）所示，右侧前后两个激光测距仪测量距离在未出现导航调偏系统介入时，测量的距离均保持在距离设定值0.5 m上下波动。当前后两侧激光测距仪测量距离骤减，同时图3（b）中横向偏移量DP绝对值大于Dm，则纠偏系统开始介入纠偏，直至右侧前后两个激光测距仪测量距离开始返回距离设定值，横向偏移量绝对值小于或等于距离标准值的数量大于等于50组，调偏结束。

图3 同侧激光测距仪测量距离及横向偏差

在进行Y方向上的纠偏时，给定无人车以斜45°方向进行纠偏，则此时的调偏角速度ωt为：

式中，N表示此次调偏采集激光测距仪数据的次数；Δt表示相邻两次采集数据时间差，单位s。

纠偏所需要的调偏线速度Vt计算公式如下：

式中，ΔD表示横向偏差量，单位m；Δt表示相邻两次采集激光测距仪数据的时间差，单位s。

将计算所得到的航向偏移量AP与初始设定的航向标准值Am进行比较，同时根据路况信息对是否需要纠偏作出判断，根据多次测量比较，最终航向标准值Am设置为5°。若航向偏移量AP大于航向标准值Am，同时路况信息判断该路况为非转弯路段，则判定该导航无人车已偏航，需要在Z方向上控制航向偏移量，直到陀螺仪的数据的航向偏移量回零。

如图4所示，导航无人车发生航向偏移时的300组航向偏移量数据，一旦导航无人车航向偏移量大于航向标准值，纠偏系统介入并进行Z方向上的调偏，即图中一号区域。在进行Z方向上的调偏时，纠偏系统只通过计算旋转角速度发送给导航控制话题move_base，不发送线速度，即纠偏系统介入时的导航无人车会停下来进行原地旋转调整，直至与两侧障碍物平行，纠偏结束。纠偏所需要的Z方向上的纠偏角速度ωt′计算公式如下：

图4 航向纠偏时航向偏移量数据

式中，ΔA表示航向偏差量，单位°；Δt′表示相邻两次采集陀螺仪数据的时间差，单位s；ωt′表示Z方向上的纠偏角速度，单位rad/s。

2.3 导航调偏方法

导航无人运输车调偏流程如下。

步骤1：以导航无人车自身坐标系建立车身坐标系，导航无人车中心为原点，导航无人车前进方向为X方向，左右方向为Y方向，上下方向为Z方向；

步骤2：安装陀螺仪和激光测距仪，通过陀螺仪和激光测距仪实时采集导航无人车位姿信息；

步骤3：根据陀螺仪和激光测距仪实时采集的数据，判断路况信息并计算横向偏移量DP和航向偏移量AP；

步骤4：主控单元根据路况信息和偏移量判断无人导航无人车是否需要进行纠偏，如需纠偏，则进一步计算纠偏所需要的调偏线速度Vt和调偏角速度ωt，结合导航角速度ωd和导航线速度Vd，得到目标线速度Vx和目标角速度ω，进入步骤5；

步骤5：通过移动控制节点将目标线速度Vx和目标角速度ω发布至速度话题上；

步骤6：导航无人车根据速度话题上的速度，分别计算出左驱动轮和右驱动轮所需要的脉冲个数，进行导航无人车自主导航纠偏。

3 实验及数据分析

3.1 导航路线规划实验

全局路径规划算法使用传统的A*算法，其原理重点是从初始状态向外扩展，计算每个相邻状态的评估函数值，选择评估函数值最小的状态作为下一个遍历状态。然后重复扩展过程，直到遍历目标状态。如图5所示，蓝色路径（虚线）为A*算法导航规划路径，可以明显地看出该算法为了获取更短的搜索时间和最短路径，没有着重注意与障碍物的最小距离，导致其与障碍物的距离较小，容易发生碰撞。红色路径（实线）为导航纠偏系统介入时的导航规划路径，可以清楚地看出在路径规划时，纠偏系统会避免与障碍物有过小的距离，在保证安全距离的前提下，指导导航系统在田垄中心线行驶，避免产生碰撞危险。

图5 垄间导航路径规划图

3.2 路况信息识别实验

课题组设计的导航纠偏系统通过读取陀螺仪和两侧激光测距仪的数据来判断此时所处的路况信息以及导航调偏效果反馈。图6～图9为导航纠偏系统介入转弯路况时陀螺仪和两侧激光测距仪输出数据图。

如图6、图7所示，在导航无人运输车进行转弯操作时，陀螺仪PITCH轴和ROLL轴输出信号趋于平稳，即导航无人运输车转弯操作时没有X方向和Y方向的速率变化。如图8所示，陀螺仪YAW轴输出信号中有一段极速变化的数据，即此时导航无人运输车Z轴旋转角速率快速增加，这与图9两侧激光测距仪输出距离图中右侧距离快速增加、左侧距离快速下降的一段相对应。同时，根据激光测距仪左侧距离减小、右侧距离增加，可以判断此时为左转操作。

图6 陀螺仪PITCH轴输出信号图

图7 陀螺仪ROLL轴输出信号图

图8 陀螺仪YAW轴输出信号图

图9 两侧激光测距仪输出距离图

3.3 导航纠偏效果实验

本实验是通过将自主导航时的最大横向偏差和最大航向偏差与导航纠偏系统介入时的最大横向偏差和最大航向偏差对比，进而可以明显地看出本导航纠偏系统对于垄间无人运输车的纠偏效果。

如表1所示，在自主导航实验中，最大横向偏差为68.50 mm，最大航向偏差为21.7°，平均最大横向偏差为54.84 mm，平均最大航向偏差为17.38°。可以看出此时的偏差较大，对于农业温室大棚环境，相邻田垄间距固定且狭窄，导航无人运输车会与两侧田垄距离较近，有时会直接与田垄碰撞，导致导航失败，同时会有一定危险。在有导航纠偏系统介入实验中，最大横向偏差为38.02 mm，最大航向偏差为16.1°，平均最大横向偏差为24.88 mm，平均最大航向偏差为9.7°。导航无人运输车会尽量保持在两侧田垄的中线上行驶，虽然存在一定偏差，但能根据陀螺仪和两侧激光测距仪传感器传输回来的数据及时对偏航状态进行调整，不会导致导航系统意外终止，避免垄间导航无人运输车与两侧田垄相碰撞的危险。

表1 自主导航与导航调偏介入时偏差数据对比

4 结论

课题组首先对垄间导航无人车系统进行总体框架设计，并分析了其硬件结构；其次利用扩展卡尔曼滤波融合IMU对导航无人进行先验定位；然后分析通过陀螺仪和激光测距仪两种传感器数据的策略，可以对此时的路况信息进行准确的判断，计算偏差并介入导航控制系统，最终提高了无人运输车导航纠偏的准确性。

实验表明，应用课题组提出的导航纠偏方法，规划路线会与两侧保持一定安全距离。10次测试中，最大横向偏差为38.02 mm，最大航向偏差为16.1°，平均最大横向偏差为24.88 mm，平均最大航向偏差为9.7°。导航纠偏系统介入，可以综合调整距离和方向偏差，保证垄间导航无人运输车可以在相邻田垄中线附近行驶，避免垄间导航无人运输车与两侧田垄相碰撞，为垄间导航无人运输车的偏差调整提供了一种准确、高效、可靠的控制方法。