农用履带机器人轨迹跟踪控制系统设计与试验

匡文龙，沈文龙,，姬长英*，田光兆，顾宝兴，刘 朋

（1.南京农业大学工学院，南京 210031；2.淮安信息职业技术学院汽车工程学院，江苏 淮安 223003）

近年来，中国设施农业发展迅速，设施农业面积居世界首位[1]，但国内设施农业机械化水平较低。农用履带机器人体积小、对土壤压实影响小、可适应复杂恶劣农业作业环境，提高设施农业机械化水平。

针对轨迹跟踪控制问题，国内外学者已开展深入研究，Kanayama 等率先提出一种基于Lyapunov函数跟踪控制律，并成功应用于移动机器人系统[2]。Amidi 等提出3 种较为实用跟踪控制方法，包括一种纯追踪轨迹跟踪控制法，但稳态误差较大[3]。李世华等通过终端滑动模态技术设计控制律，实现期望轨迹有限时间跟踪，但存在奇异性问题[4]。李世华等提出连续状态反馈跟踪控制律，解决奇异性问题并推广到动力学模型中，优化控制方法[5-6]。吴卫国等基于后退方法提出具有全局渐进稳定性跟踪控制律[7]。徐俊艳等在这种控制律基础上，引入具有双曲正切特性虚拟反馈量，简化控制器，提高收敛路径平滑性[8]。叶涛等采用变结构控制方法，提高控制鲁棒性[9]。张鑫等针对非完整机器人系统动力学模型，基于反演技术和自适应滑模控制思想设计控制律，提高抗干扰能力，进一步研究一类质心与几何中心不重合情况下轨迹跟踪问题[10-11]。Mehrjerdi 等提出一种自适应指数滑模控制方法，克服外部干扰并提高控制器针对不确定性性能，降低抖振[12]。Hwang 等将模糊控制和自适应控制律与滑模控制律进一步结合，并作田间试验，试验结果表明控制精度良好[13]。Asif等提出一种带积分器新型自适应滑模动态控制器，提升在连续扰动和不确定性情况下控制效果[14]。宋立业等利用自适应径向基神经网络调节滑模控制中增益系数，有效消除不确定性参数和未知扰动影响[15]。Wu 等在传统模糊滑模控制器（SMC）基础上提出一种新型反演模糊滑模控制器（BFSMC），具有更高准确性、快速性、平滑性和鲁棒性[16]。尤波等通过建立一种模糊快速双幂次趋近律滑模控制策略，提高系统鲁棒性且削弱系统抖振现象[17]。Zhai等提出一种不受约束快速非奇异终端滑模面，并建立误差动态系统二阶和三阶子系统[18]。然而，大多控制律仅作仿真验证，部分控制律设计复杂，难以运用到实际控制中。

有学者针对履带式移动机器人轨迹跟踪控制开展研究，焦俊等推演履带移动机器人运动学模型，基于模糊理论滑模控制，建立由电机驱动方程和运动方程构成的级联系统，通过变倾斜参数自适应控制满足作业要求，并以田间试验验证控制效果[19-20]。

本文针对前期试制农用履带机器人，提出一种新型控制方法，结合Lyapunov 方法和滑模控制技术，考虑机器人运动学约束，得到最终修正控制律。基于该控制律设计轨迹跟踪控制器并应用于机器人，并验证该控制器准确性和有效性。

1 农用履带机器人运动学模型和轨迹误差模型

前期设计制造农业履带机器人（后简称机器人）通过两侧电机分别联接蜗轮蜗杆减速器再连接驱动轮，两侧履带由驱动轮独立驱动，可以在两侧电机转速范围内以任何差速实现控制并完成多种作业。

为建立运动学模型，合理假设其仅平面内运动、两侧履带完全相同、整机质心与几何中心重合在M 点、两侧履带不发生侧滑。机器人几何结构和轨迹跟踪如图1所示：M为机器人质心和几何中心，L 为机体宽度，d 为单侧履带宽度，v 为质心M 沿机身纵向前进线速度，ω 为机体转动角速度。在全局坐标系（X，O，Y）中，机器人位置由向量p=（x y θ）T描述，参考机器人位置由向量pd=（xdydθd）T描述。其中x、y分别为机器人质心在全局坐标系下横坐标和纵坐标，描述机器人地理位置；θ为机器人机身纵向与X轴夹角即航向角，描述机器人运动姿态，向量pd对应数据描述参考机器人地理位置和航向角信息。

由此建立全局坐标系下机器人运动学模型为:

其中，线速度v和角速度ω由两侧履带速度及其中心距决定，两侧履带速度由两侧驱动电机转速、传动比和驱动轮半径共同决定，则有：

式中，vL、vR分别为左侧和右侧履带线速度（m·s-1），NL、NR分别为左右侧驱动电机转速（r·s-1），r为驱动轮半径（m），i为蜗轮蜗杆减速器传动比。

通过坐标转换后，得到轨迹跟踪误差公式（4）:

式中，xe和ye分别为基于机器人随动坐标系下实际位置与参考位置x 轴、y 轴误差，θe为航向角误差，对式（4）作微分，可得到轨迹跟踪误差微分方程（5）：

所以，机器人轨迹跟踪控制即寻找准确期望电机转速NL、NR，使得在该控制律下，机器人快速跟踪参考轨迹，xe=0、ye=0、θe=0，亦使X轴、Y轴误差为0。

2 基于Lyapunov 方法和滑模控制技术轨迹跟踪控制律设计

跟踪控制律设计，考虑经典基于Lyapunov 方法控制律，由于线速度控制律中无ye项[21]，在ye初始误差较大，而xe初始误差较小时，收敛速度较慢；基于反演（Backstepping）思想设计滑模控制律；提出一种基于Lyapunov 方法和反演滑模控制融合控制律。

2.1 基于Lyapunov方法控制律设计

卞永明等提出一种基于Lyapunov 稳定性理论状态反馈控制算法并给出其稳定性证明[22]。其控制律表达式如式6所示，仿真结果表明，对于履带式移动机器人具有较好控制效果，这类方法在xe误差较小，ye误差较大时，收敛速度较慢。

2.2 反演滑模控制律设计

引理1[7]：对于任意x∈R 且|x|＜∞，有φ(x)=xsin(arctanx)≥0，当且仅当x=0 时“=”成立。

根据引理1，引入反演（Backstepping）思想设计滑模切换函数，当xe=0时，取Lyapunov函数

设θe=-arctan（vdye），结合式（5）对式（7）求导可得：

当且仅当xe=0 且vdye=0 时“=”成立，又vd≠0，所以ye=0，得到θe=0，因此可得到结论：当xe=0，θe收敛到-arctan（vdye）时，系统达到收敛。

选择切换函数：

为加快收敛速度，选取指数趋近律:

为削弱抖振，采用连续函数代替符号函数，从而式（10）改写为：

设α=arctan（vdye），由式（5）（9）（11）可得：

整理式（12）可得控制律为：

2.3 融合控制律律设计

将式（6）和式（13）结合，得到一种基于Lyapunov方法和反演滑模技术融合控制律如下：

其中，S1=xe，S2=θe+arctan(vdye)， k1，k2，k3，k4，k5，k6, δ1，δ2均为正常数。

而由式（2）和式（3）可得：

所以式（14）和式（15）组成机器人轨迹跟踪控制律。

3 运动受限修正控制律

前期设计制造机器人驱动电机本身存在最大转速Nmax和最大加速度Mmax，机器人也存在最大线速度vmax和最大角速度ωmax及相应最大加速度αmax和βmax，对式（14）控制律给出修正表达式：

式中，v1、ω1为考虑运动受限后线速度和转向角速度控制指令，vpre、ωpre为前一时刻机器人线速度和转向角速度，Δt为控制周期。

两侧驱动电机最大转速均为Nmax，vmax和ωmax实际上由两侧电机最大转速和转速差决定：

整理得

驱动电机最大加速度为Mmax，结合式（2）（18）（19）可得∶

角速度ωl指令和线速度vl指令共同决定两侧电机转速，所以最大角速度ωmax和最大线速度vmax及最大角加速度αmax和线加速度βmax需要根据实际要求结合式（19）（20）确定。

4 仿真及结果分析

为检验设计运动受限修正控制律有效性，运用Simulink 搭建仿真模型，3 种控制律均作仿真试验。以直线为参考轨迹，其初始位姿为[0 2 0]T，线速度为0.3 m·s-1，角速度为0 rad·s-1，仿真时间确定为60 s，取样时间设定为0.2 s。线速度最大值设定为0.5 m·s-1，其加速度最大值设定为0.5 m·s-2；角速度最大值设定为0.25 rad·s-1，其加速度最大值设定为0.25 rad·s-2。机器人实际初始位姿为[0 0 pi/4]T，初始线速度为0 m·s-1，初始角速度0 rad·s-1。基于Lyapunov 方法控制律中，取仿真参数m1=0.5，m2=0.5，m3=0.6；基于反演（Backstepping）滑模控制律中，取仿真参数p1=0.4，p2=0.02，p3=0.4，p4=0.02，j1=j2=0.001；基于融合控制律中，取仿真参数k1=1，k2=0.02，k3=0.3，k4=0.02，k5=0.9，k6=0.25，δ1=δ2=0.001。由仿真模型得到直线跟踪轨迹见图2。

由图2可知，3种控制律均使误差得到收敛并成功跟踪参考轨迹，其中融合控制律控制效果明显优于基于Lyapunov方法控制律和反演滑模控制律，3种控制律在稳定后xe、ye、θe均＜0.01。

5 试验结果与分析

为进一步验证融合轨迹跟踪控制律正确性和有效性，设计基于该控制律控制器并应用于机器人，开展野外试验。

5.1 轨迹跟踪控制器设计

轨迹跟踪控制器主要由硬件部分和软件部分组成。硬件部分包括南方S82RTK-GPS移动站、北微传感BW-AH50传感器、电动机控制器、上位机和电源等。南方S82RTK-GPS移动站在网络模式下可动态获取机器人在WGS-84坐标系下经过多颗卫星对比得到的经纬度，北微传感BW-AH50传感器用于动态获取机器人航向角，上位机用于接收和处理经纬度和航向角数据，并利用轨迹跟踪控制律计算得到电机控制命令再发给电机控制器，电机控制器用于接收上位机发出控制命令并完成对左右电机控制，电源则保证各个元器件供电。软件部分基于C#程序语言建立，先将获取的GPS 数据中纬度和经度数据提取，再利用坐标转换程序将WGS-84 坐标系下经纬度数据转化为高斯-克吕格投影坐标系[23]下东向坐标和北向坐标，同时将航向角传感器中航向角即机器人纵向与赤道夹角提取，将东向坐标、北向坐标和航向角转换用于轨迹追踪控制律，通过式（16～18）得到左右电机转速并发给电机控制器，其主要结构如图3 所示。其中，南方S82RTK-GPS 移动站平面精度为0.01 m，输出频率设置为5 Hz；北微传感BW-AH50传感器在俯仰角小于40°时航向角精度为0.05 rad，输出频率设置为5 Hz；电机控制器内置闭环PID控制，可以通过霍尔传感器得到实际转速并与指令转速比较，再利用PID 控制器实现实际转速和指令转速统一。

试验场地在WGS-84 坐标系下经纬度大致为（118.68°E 32.13°N），在高斯-克吕格3度带投影坐标系下，其中央经度线为120°，带号为40，由于高斯-克吕格投影坐标以中央经线投影为纵轴（x），赤道投影为横轴（y），其横纵轴与x、y 轴和传统几何相反，所以建立以赤道投影为X 轴（东向为正），120°中央经度线投影为Y 轴（北向为正）全局坐标系，将点在高斯-克吕格3 度带投影坐标系下x 值和y 值对调即可得到点在全局坐标系下坐标，即X轴对应东向坐标，Y轴对应北向坐标。

该型农用履带机器人用于设施农业犁耕作业环境时，为保证足够驱动力矩，减速器减速比i为50。其驱动电机最大转速Nmax为25 r·s-1，最大加速度Mmax为25 r·s-2，驱动轮半径为0.2 m，机体宽度L为0.7 m，履带宽度d 为0.25 m，根据式（19～20），选取最大线速度vmax为0.5 m·s-1，其最大加速度αmax选为0.5 m·s-2；最大角速度ωmax为0.25 rad·s-1，最大角加速度βmax选为0.25 rad·s-2。

5.2 直线轨迹跟踪试验

首先开展直线轨迹跟踪试验，以机器人初始位置为全局坐标系下相对零点（0 m，0 m），航向角为0.25π rad，初始线速度0 m·s-1，初始角速度为0 rad·s-1；则参考机器人初始位置在相对坐标系下（0 m 2 m），航向角为0 rad，线速度为0.3 m·s-1，转向角度为0 rad·s-1；试验时间设置为60 s，分别以Lyapunov 控制律、反演滑模控制律、融合控制律开展试验。参数取值与仿真试验相同，在Lyapunov 控制律中取控制参数m1=0.5，m2=0.5， m3=0.6；在反演滑模控制律中，取参数p1=0.4， p2=0.02，j1=0.001，p3=0.4，p4=0.02，j2=0.001；在融合控制律中，取参数k1=1.0，k2=0.02，k3=0.3，k4=0.02，k5=0.9，k6=0.25，δ1=δ2=0.001。

得到轨迹跟踪曲线、东向、北向和航向角误差如图4～6所示。

由图4可知，由于初始航向角偏差影响，东向初始误差增大，但10 s 后3 种控制律可使误差绝对值稳定在0.1 m内。

由图5可知，北向初始误差迅速减小，绝对值减至0.1 m 内，融合控制律所用时间最低为12 s，Lyapunov 控制律为18 s，反演滑模控制律为20 s；而最终收敛情况下，融合控制律误差范围为（-0.02 m，0.02 m），Lyapunov 控制律误差范围为（-0.01 m，0.04 m），反演滑模控制律误差范围为（-0.05 m，0.05 m），融合控制律控制效果更好。

由图6可知，机器人启动后，航向角误差迅速减小，绝对值减至0.1 rad 以内，融合控制律用时间为10 s，Lyapunov 控制律用时为14 s，反演滑模控用时为11 s，在收敛稳定后，3 种控制率航向角误差范围为（-0.03 rad，0.03 rad）。

由图7 可知，3 种控制律最终可使机器人成功跟踪参考轨迹，且与直线轨迹跟踪仿真图大体相同。可见，3种控制律中，融合控制律收敛速度最快，收敛稳定后，融合控制律效果优于另外两种控制律，证明融合控制律优越性。

5.3 圆形轨迹跟踪试验

为进一步验证融合控制律有效性，设计圆形跟踪轨迹开展试验，依然取参数k1=1.0，k2=0.02， k3=0.3， k4=0.02， k5=0.9， k6=0.25， δ1=δ2=0.001。设圆形轨迹圆心为参考坐标系原点，圆半径为3 m；则参考机器人初始位置为（3.0 m，0 m），航向角为0.5 π rad，线速度为0.24 m·s-1，角速度为0.08 rad·s-1；实际机器人初始位置为（3.5 m，0 m），航向角为0.25π rad，初始线速度0 m·s-1，角速度为0 rad·s-1；试验时间设置为80 s，得到轨迹图如图8所示；东向、北向和航向角误差如图9所示。

由图8可知，经过30 s控制后，实际机器人轨迹与参考轨迹接近重合，速度限制和初始航向角初始误差较大导致耗费时间稍长。

由图9 可知，东方向初始误差9 s 内继续变大，而后迅速减小，并在20 s到达超调顶峰，28 s时误差绝对值小于0.1 m，保持稳定后误差范围为（-0.05 m，0.05 m）；北向误差在14 s 到达最大值，然后迅速减小，在29 s 时误差绝对值小于0.1 m，保持稳定后误差范围为（-0.04 m，0.04 m）；航向角误差迅速减小，后有超调，30 s 误差绝对值小于0.1 rad，保持稳定后误差范围为（0 rad，0.07 rad）。

5.4 田间轨迹跟踪试验

因针对农用履带机器人设计轨迹跟踪控制器，所以参考设计田间作业时参考轨迹[24]并开展试验，依然取参数k1=1.0，k2=0.02，k3=0.3，k4=0.02，k5=0.9，k6=0.25，δ1=δ2=0.001。以实际机器人初始位置为相对坐标系原点（0 m，0 m），初始航向角为0 rad，初始线速度为0 m·s-1，初始角速度为0 rad·s-1；则参考轨迹初始位置为（1.5 m，0 m），航向角为0 rad。参考轨迹中，直线时线速度为0.5 m·s-1，角速度为0 rad·s-1；曲线时线速度为0.3 m·s-1，角速度为0.125 rad·s-1；由于实验场地条件限制，直线距离设定为9 m，转弯圆半径设定为2.4 m，即行间距为4.8 m。试验时间设置为210 s，得到轨迹跟踪图如图10 所示，东向、北向以及航向角误差如图11所示。

由图10 可知，在直线行驶段时机器人可满足作业要求，转向时由于角速度由0 rad·s-1切换到0.125 rad·s-1再到0 rad·s-1，存在一定误差，但可满足作业要求。

由图11 可知，直线行驶段时，稳定后东方向和北方向误差范围为（-0.06 m，0.09 m），航向角误差范围为（-0.1 rad，0.1 rad）；转向时，东方向和北方向误差范围为（-0.2 m，0.3 m），航向角误差范围为（-0.19 rad，0.22 rad）。

本文设计控制器作用下，农用履带机器人快速跟踪到参考轨迹完成田间作业，跟踪误差较小。跟踪稳定后误差原因为：北微传感BW-AH50航向角传感器精度较低，易受外部磁场影响；机器人本身质心和几何中心不完全重合；控制器频率仅为5 Hz，数据采集和命令发送有时间滞后；外部未知干扰因素影响。

6 结 论

本研究设计一种农用履带机器人轨迹跟踪控制系统，构建机器人轨迹跟踪运动学模型，提出融合Lyapunov 方法和反演滑模技术的轨迹跟踪控制律，并提出考虑运动学约束的修正控制律。利用Simulink 搭建直线轨迹跟踪仿真模型，用3 种控制律作对比测试，仿真结果表明，融合控制律可实现直线轨迹跟踪且收敛速度更快。利用RTK-Gps、倾角传感器、电机控制器、上位机等搭建履带机器人轨迹跟踪控制系统，作直线参考轨迹、圆形参考轨迹、田间参考轨迹实际试验。以直线为参考轨迹试验表明，与仿真测试结果相同，融合控制律控制效果优于其余两种控制律，收敛情况下东向、北向和航向角误差范围分别为（-0.03 m，0.06 m）（-0.02 m，0.02 m）（-0.03 rad，0.03 rad）；以圆形为参考轨迹试验表明，收敛情况下东向、北向和航向角误差范围分别为（-0.05 m，0.05 m）（-0.04 m，0.04 m）（0 rad，0.03 rad）；以田间路径为参考轨迹试验表明，直线行驶收敛情况下东向、北向和航向角误差范围分别为（-0.06 m，0.09 m）（-0.06 m，0.09 m）（0.1 rad，0.1 rad）。试验证明3 种参考轨迹下，农用履带机器人可在设计轨迹跟踪控制器作用下沿参考轨迹行驶，误差在可接受范围内。本研究仅在未配套农机具情况下试验，后续研究中还应进一步配套不同农机具验证控制效果，完善农用履带机器人轨迹跟踪控制系统。