基于无模型自适应控制的设施园艺自动导航车辆路径跟踪

陈娟 姚立健 徐丽君 颜松

摘要：为实现设施园艺内农业机械的自动导航，提出一种基于无模型自适应控制（model-free adaptive control，简称MFAC）车辆路径跟踪方法。采用4个超宽带（UWB）基站组建室内无线定位系统，并与WT901C光纤陀螺仪组合使用，获取试验样车与期望路径之间的横向偏差和航向偏差。建立预瞄偏差模型，并采用无模型自适应控制器来动态确定纯追踪算法中的前视距离，最终实现路径跟踪。对比试验结果表明，在4种不同的初始状态下进行直线跟踪时，本研究方法的平均偏差分别为0.102、0.079、0.181、0.084 m，達到稳定状态后，其稳态偏差变化范围在0.013～0.033 m之间;在矩形路径跟踪时，平均偏差为0.139 m，本研究方法明显优于采用固定前视距离的纯追踪模型，满足农业设施内狭窄空间中运输作业的精度要求，可为设施园艺农机导航提供新思路。

关键词：无模型自适应控制;自动导航;超宽带;设施园艺;路径跟踪

中图分类号：TP242.6   文献标志码： A

文章编号：1002-1302（2022）08-0187-06

近年来我国设施园艺产业发展迅速，总面积现已位列世界首位[1]。在设施园艺生产中，农产品在设施内的运输时间约占整个作业时间20%以上[2]，因此研发能自动导航运输的装备成为设施园艺研究的热点之一。

目前，位姿获取与路径跟踪控制是农机自动导航的2个关键技术。全球定位系统（GPS）[3-6]、机器视觉[7-9]作为传统的定位手段经常被使用在田间农机位姿解算、作物行导航线提取等导航研究中。但在农业设施内无法接收到清晰的GPS信号，而机器视觉的实时性和全局路径规划能力还有待提高。超宽带（UWB）是利用纳秒级或亚纳秒级脉宽实现短距高速传输数据的无线通信技术，具有时间分辨率高、发送功率小等优点，被越来越多地运用到室内高精度定位研究中。如戴波等通过对UWB技术的静态、动态定位性能进行试验与分析，提出基于多时段协同定位方法，解决仓库货物堆垛遮挡、金属货罐和液体等干扰的问题[10]。符世琛等提出一种基于UWB测距技术的掘进机自主定位导向方法，可有效扩大掘进机的测量范围，减少工人井下作业时间[11]。林相泽等采用信息到达时间差（time difference of arrival，简称 TDOA）技术以及动静态优化算法，为设施环境下农用车辆精细作业提供了准确定位[12]。

在路径跟踪控制方面，纯追踪算法[13]因其控制参数少、预见性强等优点获得广泛应用。研究人员采用模糊控制[14]、线性方程[15]等方法动态调整前视距离，以提高路径跟踪精度。在模糊控制中，隶属度函数的构建对专家经验依赖较大，因此很难精准确定。对于线性方程算法，其数学表达式只能满足当前假设条件，而这些假设条件在实际环境中存在一定局限性。无模型自适应控制（model-free adaptive control，简称MFAC）是一种利用动态线性化模型解决复杂非线性系统的控制器，其控制过程无需构建复杂的数学模型，仅仅利用受控系统的I/O数据驱动，具有控制算法难度低、无需假设条件等优点，现已广泛应用在无人驾驶[16]、自动泊车系统[17]以及多自由度机器人外骨骼控制[18]等领域。

因此，本研究在前述研究基础上，采用超宽带技术构建室内定位系统以精确获取车辆横向偏差和航向偏差，通过预瞄偏差模型对纯追踪算法进行改进，并提出一种基于MFAC的前视距离动态确定方法并验证其路径跟踪方面的有效性。

1 试验样车结构

试验样车由锂电池（48 V、20 Ah）供电，采用前轮转向、后轮驱动的4轮结构，轮距和轴距分别为550、840 mm（图1）。前轮由L298N驱动器驱动电动推杆实现转向，后轮电机和AQMD3608BLS驱动器驱动实现车辆行驶。通过霍尔编码器将电机的位移量转换成脉冲数，来检测前轮转向角（α）和后轮速度（v），进而实现闭环控制。选用4个UWB无线传感器（I-UWB LPS PA型）组建室内定位系统，并与WT901C光纤陀螺仪（测量范围-180°～180°，测量精度为0.1°）组合使用，获得样车的位置和姿态信息。主控制器采用STM32F767IGT6芯片，接收UWB定位系统和光纤陀螺仪传来的位姿信息，换算成样车相对于期望路径的横向偏差（d）和航向偏差（θ）。根据纯追踪算法计算样车前轮转向角（α），从而达到设施内道路路径跟踪的目的。

2 样车位姿解算

在试验样车自动导航中，需要实时获取车辆位置和姿态信息。车辆的姿态信息由WT901C光纤陀螺仪直接读取，位置信息则通过UWB定位系统获取，具体解算流程如下：采用4个UWB无线传感器（基站）组建室内无线定位坐标系（图2）。将移动信标安装在试验样车上，当样车在定位系统中移动时信标与4个基站建立通讯，根据信号到达时间（time of arrival，简称TOA）原理[10]，信标到每个基站的距离（r）可由如下方程组表示：

（x1-x）2+（y1-y）2=r21;

（x2-x）2+（y2-y）2=r22;

（x3-x）2+（y3-y）2=r23;

（x4-x）2+（y4-y）2=r24。（1）

式中：（x1，y1）、（x2，y2）、（x3，y3）、（x4，y4）分别为基站1、2、3、4的坐标;（x，y）为移动信标的坐标。r1、r2、r3、r4为各基站到信标的距离，其计算方式如下：

ri=（xi-x）2+（yi-y）2=cti。（2）

式中：c为光速，3×108 m/s;ti为信号在移动标签到

第i个基站之间的传输时间，i=1，2，3，4。将式（1）中的后3个方程分别与第1个方程相减，可得：

[x y]T=Z·C。（3）

式中：Z=2x1-x2 y1-y2

x1-x3 y1-y3

x1-x4 y1-y4-1;

C=r22-r12+x12+y12-x22-y22

r32-r12+x12+y12-x32-y32

r42-r12+x12+y12-x42-y42。

式（3）为超静定方程组，其中各基站的坐标（xi，yi）及移动标签到各定位基站的距离（ri）均可以由UWB定位系统获得。采用加权最小二乘法（weighted least squares，简称 WLS）[19]解该超静定方程组，得到移动标签坐标[x，y]T的最优解。[x，y]T的定位精度与基站的分布、标签的位置、障碍物等密切相关。经过反复调试，当4个基站组成15 m×20 m的矩形时，此区域内能获得良好的定位精度。

3 路径跟踪控制

3.1 预瞄偏差模型

自动导航是通过对样车横向控制来实现路径跟踪。参考驾驶员习惯，本研究引入预瞄点和前视距离的概念。如图3所示，B点为车辆后轮中心（A）到期望跟踪直线P1P2的投影，设C点为车辆预瞄点，根据弧长预瞄方法[20]，BC即为前视距离（L）。

图3中，β为车身方向与AC连线的夹角，定义该角度为预瞄偏差角，并设当车在跟踪路径的左侧时角度为正。根据几何知识，预瞄偏差模型的前视距离（L）为

L=dtan（β+θ）。（4）

式中：d为横向偏差，m;θ为航向偏差，°。横向偏差（d）和航向偏差（θ）的正负号定义如下：在路径跟踪中，横向偏差d为左正右负;根据时针运行方向，航向偏差θ为逆正顺负。由式（4）可知，前视距离L由横向偏差d、航向偏差θ和预瞄偏差角β决定，θ、d可由光纤陀螺仪和UWB定位系统得到，因此预瞄偏差角β是影响前视距离L的唯一参数。

3.2 纯追踪原理

纯追踪算法[13]是一种基于几何模型的路径跟踪方法，通过设置预瞄点计算前轮转向角α，并规划车辆从当前位置到预瞄点的圆弧轨迹（如图3中的圆弧AC）来实现路径跟踪，该方法简单直观且具有预见性，在计算过程中无需考虑车辆的动力学模型。由纯追踪原理，可得前轮转向角α为

α=-acrtan2l（dcosθ+Lsinθ）L2+d2。（5）

式中：l为车辆轴距，m。

由式（5）可知，前视距离L决定前轮转向角α的大小。前视距离较大时，车辆会沿数个曲率较小的弧线慢慢靠近期望路径，振荡小但收敛快速性较差;当前视距离较小时，车辆会沿曲率较大的弧线快速靠近期望路径，但车辆会产生振荡。因此，可以通过控制β不断收敛来获得动态的前视距离L，进而控制前轮转向角α来调整车辆位姿逐步向期望路径收敛。本研究将设计一种MFAC算法，根据实时预瞄偏差β信息对前视距离L进行动态调整。

4 无模型自适应控制系统

4.1 控制系统框架

如图4所示，β*（k）为当前时刻（k）预瞄偏差角的期望输出，通常为0°。通过位姿和转角传感器获取车辆前一时刻（k-1）时的状态数据β（k-1）、α（k-1），与β*（k）比对后，输入MFAC，产生当前时刻（k）的预瞄偏差角[β（k）]，将β（k）带入预瞄偏差模型[式（4）]，就可求出此刻最合适的前视距离 [L（k）]，再将前视距离带入式（5），就可得到此刻理想的前轮转角[α*（k）]，以此控制车辆调整姿态。不断循环便可将车辆逐步收敛到期望路径上。因此，子系统MFAC是整个控制系统设计的重点。

4.2 MFAC设计

由式（4）和式（5）可知，前轮转向角α和预瞄偏差角β存在非线性关系，可用一般离散时间描述 k+1时刻的β值：

β（k+1）=f[β（k），…，β（k-kβ），α（k），…，α（k-kα）]。（6）

式中：f（·）是未知非线性函数;kα和kβ是须追溯参数的时间宽度。

根据文献[21]，式（6）须满足以下3种假设：

假设1，子系统MFAC输入输出变量α和β均须可测、可控、有边界，且输出变量β会由系统输入信号驱动迭代逐渐接近期望输出信号β*;

假设2，f（·）关于输入信号中α（k）的偏导数是连续的;

假设3，子系统MFAC满足广义Lipschitz相关条件。

因此符合基于紧格式的动态线性化模型，使得子系统MFAC转换为

Δβ（k+1）=（k）Δα（k）。（7）

式中：Δα（k）=α（k）-α（k-1）;Δβ（k+1）=β（k+1）-β（k）;（k）为伪偏导数，|（k）|≤b，且b是一個正数。

为保证前轮转向角α急剧转向引起车辆失稳以及消除路径跟踪误差，本研究引入输入准则函数和估计准则函数，公式如下：

J[α（k）]=|β*（k+1）-β（k+1）|2+λ|α（k）-α（k+1）|2;（8）

J[（k）]=|β（k）-β（k-1）-（k）Δα（k-1）|2+μ|（k）-（k-1）|2。（9）

式中：μ>0;λ>0;β*为期望输出。将式（7）代入工（8）中，对α（k）求导等于0;同时式（9）对 （k） 求极值，得其近似估计值：

^（k）=^（k-1）+η[Δβ（k）-^（k-1）Δα（k-1）]Δα（k-1）μ+|Δα（k-1）|2;（10）

α（k）=α（k-1）+（k）[β*（k+1）-β（k）]λ+|（k）|2。（11）

式中：步长因子η∈（0，2]。

为了使估计算法具有较好的适应性和收敛性，（k）须满足如下约束条件

|（k）|2≤ε‖Δα（k-1）≤ε;

sign[（k）]≠sign[（1）];

（k）=（1）。（12）

式中：ε取正數最小值。由式（10）、式（11）和式（12）可知，该系统需要设定起始时刻的前视距离 L（1）、估计伪偏导 ^（1）以及λ、μ、η。

5 试验结果与讨论

5.1 实车试验

为检验本研究算法的有效性，在浙江农林大学官塘农场温室中进行实车试验。试验环境及场地布置如图1和图2所示。试验样车的采样频率和车速分别为10 Hz、1 m/s。采用最佳固定前视距离（经计算机反复仿真，取0.8 m）的纯追踪模型（简称L-fixed法）进行对比试验。对各性能指标的定义如下：平均偏差是从试验开始到结束所有横向偏差绝对值的平均值，m;平均标准差是各个偏差偏离平均偏差距离的均值，m;稳定距离是指样车首次收敛至稳定点（距离期望路径±0.02 m以内）时样车所行驶的纵向距离，m;调整时间是指从初始状态行驶到稳定点所需的时间，s;稳定距离和调整时间这2个指标反映路径跟踪的收敛速度;稳态偏差是指从稳定点向后的平均横向偏差，m，该指标反映了路径跟踪精度。

5.1.1 试验1 样车分别从4种不同的初始状态[（d，θ）分别为（0.7 m，-70°）、（-0.8 m，80°）、（0.9 m，-20°）、（-0.5 m，40°）]出发，跟踪全长为15 m的直线路径。本研究方法（简称L-MFAC法）中，系统初始时刻[L（1）]取最佳固定前视距离，即为0.8 m，估计伪偏导^=0.5，λ=18，μ=1，η=1。L-MFAC法和L-fixed法直线追踪的对比试验轨迹与数据统计分别如图5和表1所示。

5.1.2 试验2 为进一步验证样车在设施园艺狭小空间中转向能力，设计了19 m×9 m的矩形跟踪路线。出发的初始位置为（0，0），依次经过点（19，0）、点（19，9）、点（0，9）后回到点（0，0），初始横向、航向偏差均为0°（图6）。考虑到系统的响应时间及试验样机的速度，设定距离下一条路径不足 2.0 m 时更新路径。矩形追踪的对比试验结果与数据统计分别如图6和表2所示。

5.2 试验结果与分析

如图5、表1所示，在UWB定位系统位姿偏差可测范围内，4种状态下本研究基于MFAC的纯追踪模型直线跟踪的平均偏差分别为0.102、0.079、0.181、0.084 m，而固定前视距离L-fixed纯追踪模型平均偏差分别为0.141、0.122、0.198、0.118 m。当试验样机行驶到稳定状态时，本研究 L-MFAC 法的稳态偏差分别为0.013、0.015、0.018、0.033 m，而作为对比的L-fixed法分别为0.067、0.038、0.024、0.081 m。由此可知，L-MFAC法平均能够降低一半以上的稳态误差率;在试验2中，本研究的L-MFAC法和对比的L-fixed法平均偏差分别为0.139、0.265 m（图6、表2），表明本研究方法具有更好的跟踪精度。

由表1可知，本研究的L-MFAC法和L-fixed法路径跟踪稳态偏差的标准差分别为0.008、0.010、0.013、0.018 m和0.017、0.013、0.019、0.023 m，前者均值为0.012 m，而后者均值为 0.018 m，前者明显小于后者。说明本研究基于MFAC的纯追踪模型转向较为平缓且稳定，可以提高运输车在设施园艺抗侧翻性能。

收敛性能的快慢是检验试验样机路径跟踪的重要性能之一，即调整时间是表明受控系统的收敛性能。由表1可知，本研究所用L-MFAC法的调整时间为5.6、6.1、7.6、6.8 s，对应的稳定距离分别为4.210、4.970、6.110、5.590 m。而L-fixed法调整时间分别为6.6、6.4、10.4、8.1 s，对应稳定距离分别为5.570、5.020、8.800、6.260 m。可知，本研究所用L-MFAC法确定前视距离具有较好的适应性，收敛效果要优于作为对比的L-fixed法。

由表2可知，本研究的L-MFAC法平均偏差的标准差为0.111m，而L-fixed法平均偏差的标准差为0.208 m，稳定性能提高了46.6%。同时由图6可知，最大偏差均出现在转弯处，这是由于试验样机存在最小转弯半径且更换当前路径，因此试验样机需要重新收敛至最新路径。

6 总结与展望

本研究采用UWB无线传感器组建室内定位系统，并与光纤陀螺仪组合使用，精准获取移动平台的位姿偏差信息，能够解决在设施园艺环境下农业机械的定位与导航难题。

利用无模型自适应算法方法动态确定纯追踪模型的前视距离，并设计直线跟踪和矩形路径跟踪2个试验。试验结果表明，在直线路径跟踪下，其稳态偏差均值为0.020 m;在矩形路径跟踪下，平均偏差和标准差分别为0.139、0.111 m，可以满足园艺设施内作业平台的导航要求。

超宽带定位精度易受到外界环境的影响，而在本研究中只考虑移动标签与定位基站之间无遮挡的情况，属于理想使用环境，在后续研究中，可针对有遮挡物时的定位测量，分析不同遮挡物对定位精度的控制。另外，可考虑选用4轮独立驱动独立转向新型结构，以减少大角度转向时的车体打滑现象。

参考文献：

[1]齐 飞，魏晓明，张跃峰. 中国设施园艺装备技术发展现状与未来研究方向[J]. 农业工程学报，2017，33（24）：1-9.

[2]Jang W J，Lewis G，Hoachuck J，et al. Vibration-reducing path tracking control for a strawberry transport robot[C]//American Society of Agricultural and Biological Engineers .Proceedings of 2014 ASABE and CSBE/SCGAB annual international meeting. Montreal，2014：141914011.

[3]刘柯楠，吴普特，朱德兰，等. 基于GPS的太阳能平移式喷灌机自主导航系统设计与试验[J]. 农业工程学报，2016，32（16）：89-94.

[4]郭 娜，胡静涛，王 鹤. 基于GPS导航的插秧机作业控制系统[J]. 农业机械学报，2013，44（1）：200-204.

[5]黎永键，赵祚喜，高俊文. 基于GPS/SINS组合的农业导航定位系统设计与研究[J]. 农机化研究，2014，36（3）：16-22.

[6]Yin X，Du J，Noguchi N，et al. Development of autonomous navigation system for rice transplanter[J]. International Journal of Agricultural and Biological Engineering，2018，11（6）：89-94.

[7]李云伍，徐俊杰，王铭枫，等. 丘陵山区田间道路自主行驶转运车及其视觉导航系统研制[J]. 农业工程学报，2019，35（1）：52-61.

[8]Lee I N，Lee K H，Lee J H，et al. Autonomous greenhouse sprayer navigation using automatic tracking algorithm[J]. Applied Engineering in Agriculture，2015，31（1）：17-21.

[9]孟庆宽，何 洁，仇瑞承，等. 基于机器视觉的自然环境下作物行识别与导航线提取[J]. 光学学报，2014，34（7）：180-186.

[10]戴 波，吕 昕，刘学君，等. 基于改进的超宽带危化品仓储堆垛定位技术[J]. 上海交通大学学报，2016，50（6）：963-967，974.

[11]符世琛，李一鸣，杨健健，等. 基于超宽带技术的掘进机自主定位定向方法研究[J]. 煤炭学报，2015，40（11）：2603-2610.

[12]林相泽，王 祥，林彩鑫，等. 基于超宽带的温室农用车辆定位信息采集与优化[J]. 农业机械学报，2018，49（10）：23-29，45.

[13]姚立健，Pitla S K，楊自栋，等. 基于超宽带无线定位的农业设施内移动平台路径跟踪研究[J]. 农业工程学报，2019，35（2）：17-24.

[14]汪小旵，鲁 伟，陈 满，等. 基于改进纯追踪模型的温室采摘运输自动跟随系统[J]. 农业机械学报，2016，47（12）：8-13.

[15]李 革，王 宇，郭刘粉，等. 插秧机导航路径跟踪改进纯追踪算法[J]. 农业机械学报，2018，49（5）：21-26.

[16]王建邦，袁智勇，陈 波，等. 变电站巡检机器人数据驱动无模型自适应控制[J]. 电测与仪表，2019，56（19）：114-120.

[17]侯忠生，董航瑞，金尚泰.基于坐标补偿的自动泊车系统无模型自适应控制[J]. 自动化学报，2015，41（4）：823-831.

[18]Wang X F，Li X，Wang J H，et al. Data-driven model-free adaptive sliding mode control for the multi degree-of-freedom robotic exoskeleton[J]. Information Sciences，2016，327：246-257.

[19]刘书静，罗海勇，吴 彬，等. 基于最小二乘测距定位算法信标最优部署模型[J]. 计算机学报，2013，36（3）：546-556.

[20]李 爽，徐延海，陈 静，等. 基于弧长预瞄的车辆侧向跟踪控制研究[J]. 汽车工程，2019，41（6）：668-675.

[21]王卫红，侯忠生，霍海波，等. 基于数据驱动方法的控制器设计及其参数整定[J]. 系统科学与数学，2010，30（6）：792-805.