CN111338354A - 履带式车辆轨迹跟踪控制方法、装置及系统 - Google Patents

Abstract

本发明公开了一种履带式车辆轨迹跟踪控制方法、装置及系统。该履带式车辆轨迹跟踪控制方法包括以下步骤：基于轨迹跟踪误差模型，根据Lyapunov方法获取第一控制律；其中，轨迹跟踪误差模型是基于履带式车辆的运动学模型建立的；基于轨迹跟踪误差模型，根据反演滑模控制方法获取第二控制律；结合第一控制律和第二控制律推导出融合控制律；根据融合控制律控制履带式车辆履带的转速。本发明在通过Lyapunov控制方法和反演滑模控制方法结合推导出融合控制律能够提高车辆控制速度和控制精度，取得更好的控制效果。

Description

履带式车辆轨迹跟踪控制方法、装置及系统

技术领域

本发明涉及机械领域，具体涉及一种履带式车辆轨迹跟踪控制方法、装置及系统。

背景技术

近年来，中国设施农业发展迅速，设施农业面积已居世界首位，但当前国内设施农业机械化水平仍然较低。而由于农用履带车辆本身体积小、对土壤压实影响小、能适应复杂恶劣的农业作业环境等优点，使得其很适合应用于设施农业的生产作业中，并提高设施农业机械化水平。为此，农用履带式车辆得到了广泛应用。

为了实现农用履带式车辆轨迹跟踪，国内外学者已经进行了深入研究，建立了运动学模型和轨迹跟踪误差模型。现有的农用履带式车辆轨迹跟踪方法包括基于Lyapunov稳定性理论的控制方法、通过终端滑动模态技术设计的控制方法、反演模糊滑模控制方法等。上述方法能够实现期望轨迹的有限时间跟踪，但至少存在以下问题中的一种或几种：稳态误差较大、奇异性问题、抗干扰能力差、控制精度低、控制速度低。

因此，如何提高履带式车辆的自动化程度，提高轨迹跟踪的控制精度、控制速度和抗干扰能力成为了本领域技术人员亟待解决的技术问题和始终研究的重点。

发明内容

有鉴于此，本发明实施例提供了一种履带式车辆轨迹跟踪控制方法、装置及系统，以解决现有履带式车辆控制精度低、控制速度慢、抗干扰能力差的问题。

为此，本发明实施例提供了如下技术方案：

本发明第一方面提供了一种履带式车辆轨迹跟踪控制方法，包括以下步骤：

基于轨迹跟踪误差模型，根据Lyapunov方法获取第一控制律；其中，所述轨迹跟踪误差模型是基于履带式车辆的运动学模型建立的；

基于所述轨迹跟踪误差模型，根据反演滑模控制方法获取第二控制律；

结合所述第一控制律和所述第二控制律推导出融合控制律；

根据所述融合控制律控制所述履带式车辆履带的转速。

进一步地，该履带式车辆轨迹跟踪控制方法还包括：

获取驱动电机的各项参数；

根据所述各项参数修正所述融合控制律。

进一步地，所述履带式车辆的运动学模型为：

其中，x、y分别为所述履带式车辆质心在全局坐标系(X，O，Y)中的横坐标和纵坐标，θ为车身纵向与X轴的夹角，v为所述履带式车辆的线速度，ω为所述履带式车辆的角速度，v_L为所述履带式车辆左侧履带的线速度，v_R为所述履带式车辆右侧履带的线速度，L为所述履带式车辆两侧履带之间的距离，d为所述履带式车辆的履带宽度，N_L为所述履带式车辆左侧驱动电机的转速，N_R为所述履带式车辆右侧驱动电机的转速，r为所述履带式车辆驱动轮的半径，i为所述履带式车辆减速器传动比。

进一步地，所述履带式车辆的轨迹跟踪误差模型为：

其中，x_e为所述履带式车辆实际位置与参考位置的横向误差，y_e为所述履带式车辆实际位置与参考位置的纵向误差，θ_e为所述履带式车辆的航向角误差，v_d为所述履带式车辆的参考速度，ω_d为所述履带式车辆的参考角速度。

进一步地，所述第一控制律为：

其中，ω₁为第一控制输入角速度，v₁为第一控制输入速度，m₁、m₂、m₃均为正常数。

进一步地，所述第二控制律为：

其中，ω₂为第二控制输入角速度，v₂为第二控制输入速度，S₁＝x_e,S₂＝θ_e+arctan(v_dy_e)，j₁、j₂、p₁、p₂均为正常数。

进一步地，所述融合控制律为：

其中，k₁，k₂，k₃，k₄，k₅，k₆，δ₁，δ₂均为正常数。

进一步地，所述各项参数包括所述驱动电机的最大转速和最大加速度。

进一步地，根据所述各项参数修正所述融合控制律包括限定角速度和线速度的范围为：

其中，N_max为驱动电机的最大转速，M_max为驱动电机的最大加速度。

进一步地，根据所述各项参数修正所述融合控制律包括限定最大加速度和最大角加速度范围为：

其中，α_max为最大角加速度，β_max为最大线加速度。

本发明第二方面提供了一种履带式车辆轨迹跟踪控制装置，包括：

第一数据处理模块，用于基于轨迹跟踪误差模型，根据Lyapunov方法获取第一控制律；其中，所述轨迹跟踪误差模型是基于履带式车辆的运动学模型建立的；

第二数据处理模块，用于基于所述轨迹跟踪误差模型，根据反演滑模控制方法获取第二控制律；

第三数据处理模块，用于结合所述第一控制律和所述第二控制律推导出融合控制律；

控制模块，用于根据所述融合控制律控制所述履带式车辆履带的转速。

本发明第三方面提供了一种履带式车辆轨迹跟踪控制系统，包括：

经纬度获取模块、航向角获取模块、上位机、左电机控制器和右电机控制器；

所述经纬度获取模块用于获取所述履带式车辆的经纬度，并发送给所述上位机；

所述航向角获取模块用于获取所述履带式车辆的航向角，并发送给所述上位机；

所述上位机用于获取所述履带式车辆的参考轨迹，并根据本发明第一方面任一实施例所述的履带式车辆轨迹跟踪控制方法确定所述履带式车辆左侧驱动电机的转速和右侧驱动电机的转速；所述上位机还用于将所述履带式车辆左侧驱动电机转速发送给所述左电机控制器，并将所述履带式车辆右侧驱动电机的转速发送给所述右电机控制器；

所述左电机控制器用于控制左电机的转速；

所述右电机控制器用于控制右电机的转速。。

本发明实施例技术方案，具有如下优点：

本发明实施例提供了一种履带式车辆轨迹跟踪控制方法。现有的基于Lyapunov的控制方法和反演滑模控制方法存在控制速度低、控制精度低的问题。本发明在通过，Lyapunov控制方法和反演滑模控制方法结合推导出融合控制律能够提高控制速度和控制精度，取得更好的控制效果。

附图说明

为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是根据本发明实施例的履带式车辆轨迹跟踪控制方法流程图；

图2是根据本发明优选实施例的履带式车辆轨迹跟踪控制方法流程图；

图3是根据本发明实施例的运动学模型示意图；

图4是根据本发明实施例的履带式车辆轨迹跟踪控制系统结构框图；

图5是根据本发明实施例的东向误差变化对比曲线图；

图6是根据本发明实施例的北向误差变化对比曲线图；

图7是根据本发明实施例的航向角误差变化对比曲线图；

图8是根据本发明实施例的直线跟踪轨迹图；

图9是根据本发明实施例的圆形轨迹跟踪图；

图10是根据本发明实施例的圆形跟踪误差变化图；

图11是根据本发明实施例的田间轨迹跟踪图；

图12是根据本发明实施例的田间跟踪误差图。

具体实施方式

下面结合说明书附图对本发明提供的一种履带式车辆轨迹跟踪控制方法、装置及系统的技术方案进行清楚、完整地描述，显然地，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

在本发明的描述中，需理解的是，术语“第一”、“第二”、“第三”等仅用于描述目的，而不能将其理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”、“第三”的特征可以明示或隐含地包括一个或者更多个特征。在本发明的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。

在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

在本发明中，“示例性”一词用来表示“用作例子、例证或说明”。本发明中被描述为“示例性”的任何实施例不一定被解释为比其它实施例更优选或更具优势。为了使本领域任何技术人员能够实现和使用本发明，给出了以下描述。在以下描述中，本发明为了解释的目的而列出了细节。应当明白的是，本领域普通技术人员可以认识到，即使在不使用这些特定细节的情况下也可以实现本发明。在其它的实例中，不会对公知的结构和过程进行详细阐述，以避免不必要的细节使本发明的描述变得晦涩。因此，本发明并非旨在限于所示的实施例，而是应与符合本发明所公开的原理和特征的最广范围相一致。

图1是根据本发明实施例的履带式车辆轨迹跟踪控制方法流程图。如图1所示，该方法包括以下步骤：

S101：基于轨迹跟踪误差模型，根据Lyapunov方法获取第一控制律；其中，轨迹跟踪误差模型是基于履带式车辆的运动学模型建立的；

本实施例中，履带式车辆包括两侧履带。该履带式车辆通过两侧电机分别联接蜗轮蜗杆减速器再连接驱动轮，两侧履带由驱动轮独立驱动。两侧履带可以实现任何差速控制。两侧履带相同，整车的质心和几何中心重合，两侧履带不发生侧滑。车辆的位置由车辆质心在全局坐标下的横坐标、纵坐标和航向角表示。航向角为车身纵向与X轴的夹角。

本实施例中，轨迹跟踪误差为车辆实际位置与参考位置的误差。履带式车辆轨迹跟踪控制方法就是寻找准确的左右侧驱动电机的转速，使得误差为0。

本实施例中，第一控制律为基于Lyapunov稳定性理论的状态反馈控制律。由于线速度控制律中没有y轴误差项，在y轴初始误差较大而x轴初始误差较小时，其收敛速度较慢。

S102：基于轨迹跟踪误差模型，根据反演滑模控制方法获取第二控制律；

S103：结合第一控制律和第二控制律推导出融合控制律；

本实施例中，融合控制律为根据期望轨迹确定期望输入速度和角速度，进而确定左侧驱动电机的转速和右侧驱动电机的转速。

S104：根据融合控制律控制履带式车辆履带的转速。

区别于现有技术，本发明提供了一种结合了Lyapunov方法和反演滑模控制方法的融合控制律，解决了现有的基于Lyapunov的控制方法和反演滑模控制方法存在的控制速度低、控制精度低的问题。本发明提供的控制方法能够提高控制速度和控制精度，取得更好的控制效果，提高农用履带车的自动化程度。

图2是根据本发明优选实施例的履带式车辆轨迹跟踪控制方法流程图。如图2所示，该方法包括如下步骤：

S201：基于轨迹跟踪误差模型，根据Lyapunov方法获取第一控制律；其中，轨迹跟踪误差模型是基于履带式车辆的运动学模型建立的；

S202：基于轨迹跟踪误差模型，根据反演滑模控制方法获取第二控制律；

S203：结合第一控制律和第二控制律推导出融合控制律；

S204：获取驱动电机的各项参数；

本实施例中，该履带式车辆两侧的驱动电机相同。各项参数包括驱动电机的最大转速和最大加速度。

S205：根据各项参数修正融合控制律；

本实施例中，修正融合控制律包括限定最大角速度、最大线速度、最大线加速度和最大角加速度；

S206：根据融合控制律控制履带式车辆履带的转速。

本实施例通过驱动电机的各项参数来修正融合控制律，考虑了车辆本身存在的物理限制，提高了控制的准确性和有效性。

图3是根据本发明实施例的运动学模型示意图；如图3所示，在一个具体的实施方式中，假设履带式车辆在二维平面内运动，两侧履带完全相同。M为车辆的质心和几何中心，L为两侧履带之间的距离，d为单侧履带宽度，v为质心M沿车身纵向前进的线速度，ω为车体转动的角速度。在全局坐标系(X，O，Y)中，车辆的位置由向量p＝(x，y，θ)^T描述，参考车辆的位置由向量p_d＝(x_d，y_d，θ_d)^T描述。其中x、y分别为车辆质心在全局坐标系下的横坐标和纵坐标，描述车辆的地理位置；θ为车辆车身纵向与X轴的夹角即航向角，描述车辆的运动姿态，向量p_d的对应数据描述了参考车辆的地理位置和航向角信息。建立履带式车辆的运动学模型为：

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

式中：v_L、v_R分别为左侧和右侧履带的线速度(m/s)，N_L、N_R分别为左右侧驱动电机的转速,r为驱动轮半径，i为减速器传动比。

通过坐标转换后，得到的轨迹跟踪误差模型为:

式中，x_e和y_e分别为基于车辆随动坐标系下实际位置与参考位置的x轴、y轴误差，θ_e为航向角误差，对式(4)进行微分，可以得到轨迹跟踪误差微分方程(5)。

所以，车辆的轨迹跟踪控制就是寻找准确的期望电机转速N_L、N_R，使得在该控制律下，车辆能快速跟踪参考轨迹，即使得x_e＝0、y_e＝0、θ_e＝0，亦使得X轴、Y轴误差为0。

构造一个正定的Lyapunov函数：

式中，K₁>0且有界。对该Lyapunov函数进行微分，可得：

第一控制律选取为：

式中，

K₂＝m₃，K₂>0，K₃>0且都有界。

由于状态变量和控制输入都是有界的，

也是有界的，

具有一致连续性。随着时间推移

会收敛到0，即x_e和θ_e会收敛道0，y_e也会收敛到0。按照μ₁的控制率可以使履带式车辆在给定任意初始误差时，逐渐收敛于平衡点，实现对期望轨迹的跟踪。在x_e误差较小，y_e误差较大时收敛速度慢。

根据反演(backstepping)思想来设计滑模切换函数，当x_e＝0时，取Lyapunov函数

设θ_e＝-arctan(v_dy_e)，结合式(5)对式(7)求导可得：

当且仅当x_e＝0且v_dy_e＝0时“＝”成立，又v_d≠0，所以y_e＝0，进而得到θ_e＝0，因此可以得到结论：当x_e＝0，θ_e收敛到-arctan(v_dy_e)时，系统达到收敛。

所以选择切换函数：

选取指数趋近律:

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

设α＝arctan(v_dy_e)，由式(6)、(9)、(11)可得：

整理式(12)可得第二控制律为：

其中

将第一控制律(6)和第二控制律(13)结合，得到一种基于Lyapunov方法和反演滑模的融合控制律如下：

其中S₁＝x_e,S₂＝θ_e+arctan(v_dy_e)，k₁，k₂，k₃，k₄，k₅，k₆，δ₁，δ₂均为正常数。

而由式(2)和式(3)可得：

所以式(14)和式(15)组成了履带式车辆的融合控制律。

履带式车辆的驱动电机本身存在最大转速N_max和最大加速度M_max，所以车辆也存在最大线速度v_max和最大角速度ω_max，以及相应的最大加速度α_max和β_max，因此可对式(14)的控制律给出如下的修正表达式：

式中，v₁,、ω₁为考虑运动受限后的控制律，v_pre、ω_pre为前一时刻车辆的转向角速度和线速度，Δt为控制的周期。

两侧驱动电机的最大转速都为N_max，所以v_max和ω_max实际上也是由两侧电机的最大转速和转速差决定的：

整理得

驱动电机的最大加速度为M_max，结合式(2)可得：

由于角速度ω_l指令和线速度v_l指令共同决定了两侧电机转速，所以最大角速度ω_max和最大线速度v_max也需要根据实际要求结合式(19)给出，而最大加速度α_max和β_max也需要根据实际要求结合式(20)决定。

在本实施例中还提供了一种履带式车辆轨迹跟踪控制装置，该装置用于实现上述实施例及优选实施方式，已经进行过说明的不再赘述。如以下所使用的，术语“模块”可以实现预定功能的软件和/或硬件的组合。尽管以下实施例所描述的装置较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。

本实施例提供一种履带式车辆轨迹跟踪控制装置，包括第一数据处理模块，用于基于轨迹跟踪误差模型，根据Lyapunov方法获取第一控制律；其中，轨迹跟踪误差模型是基于履带式车辆的运动学模型建立的；第二数据处理模块，用于基于轨迹跟踪误差模型，根据反演滑模控制方法获取第二控制律；第三数据处理模块，用于结合第一控制律和第二控制律推导出融合控制律；控制模块，用于根据融合控制律控制履带式车辆履带的转速。

本发明还公开了一种履带式车辆轨迹跟踪控制系统，包括：经纬度获取模块、航向角获取模块、上位机、左电机控制器和右电机控制器；

经纬度获取模块用于获取履带式车辆的经纬度，并发送给上位机；

航向角获取模块用于获取履带式车辆的航向角，并发送给上位机；

上位机用于获取履带式车辆的参考轨迹，并根据上述任一实施例的履带式车辆轨迹跟踪控制方法确定履带式车辆左侧驱动电机的转速和右侧驱动电机的转速。上位机还用于将履带式车辆左侧驱动电机转速发送给左电机控制器，并将履带式车辆右侧驱动电机的转速发送给右电机控制器。左电机控制器用于控制左电机的转速。右电机控制器用于控制右电机的转速。

图4是根据本发明实施例的履带式车辆轨迹跟踪控制系统结构框图。如图4所示，履带式车辆轨迹跟踪控制系统主要由硬件部分和软件部分组成。硬件部分包括南方S82RTK-GPS移动站、北微传感BW-AH50传感器、科亚电动机控制器、上位机和电源。南方S82RTK-GPS移动站用于获取车辆在WGS-84坐标系下的经纬度，北微传感BW-AH50传感器用于获取车辆的航向角，上位机用于接收和处理经纬度和航向角数据，并计算得到电机控制命令再发给电机控制器，电机控制器用于接收上位机发出的控制命令并完成对左右电机控制。软件部分基于C#程序语言建立，先将获取的GPS数据中的纬度和经度数据提取出来，再利用坐标转换程序将WGS-84坐标系下的经纬度数据转化为高斯-克吕格投影坐标系^[26]下的东向坐标和北向坐标，同时将航向角传感器中的航向角即车身纵向与赤道的夹角提取出来，然后将获得东坐标、北坐标和航向角转换用于轨迹追踪控制律，通过式(16)、(17)、(18)得到左右电机转速并发给电机控制器。

本发明通过实验进一步验证了融合控制律的有效性。

试验场地在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。其驱动电机的最大转速N_max为25r/s，最大加速度M_max为25r/s²，驱动轮半径为0.2m，车体两侧履带之间的距离L为0.7m，履带宽度d为0.25m，根据式(19)和式(20)，选取最大线速度v_max为0.5m/s，其最大加速度α_max适当圆整后为0.6m/s²；最大角速度ω_max为0.25rad/s，最大角加速度β_max适当圆整减小后为0.8rad/s²。南方S82RTK-GPS移动站的平面精度为0.01m，输出频率设置为为5Hz；北微传感BW-AH50传感器的航向精度为0.05rad，输出频率设置为5Hz；科亚电机控制器内置PID控制，调节实际转速等于指令转速。

首先进行了直线轨迹跟踪试验，以车辆的初始位置为全局坐标系下的相对零点(0m,0m)，航向角为0.25π，初始线速度0m/s，初始角速度为0rad/s；则参考车辆的初始位置是相对坐标系下的(0m,2m)，航向角为0rad,线速度为0.3m/s，转向角度为0rad/s；试验时间设置为60s，分别用融合控制律、Lyapunov控制律、反演滑模控制律进行了试验。在Lyapunov控制律中取控制参数m₁＝0.5,m₂＝0.5,m₃＝0.6；在反演滑模控制律中，取参数p₁＝0.4，p₂＝0.02，j₁＝0.001，p₃＝0.4，p₄＝0.02，j₂＝0.001；在融合控制律中，取参数k₁＝1.0，k₂＝0.02，k₃＝0.3，k₄＝0.02，k₅＝0.9，k₆＝0.25，δ₁＝δ₂＝0.001。得到的东向误差变化对比曲线图、北向误差变化对比曲线图、航向角误差变化对比曲线图如图5、6、7所示。图8是得到的直线跟踪轨迹图。图8a是融合控制率的直线跟踪轨迹图。图8b是基于Lyapunov方法控制率的直线跟踪轨迹图。图8c是反演滑模控制率的直线跟踪轨迹图。

由图5可以看出，由于初始航向角偏差的影响，东方向的误差一开始有些许增大，但10s后3种控制律基本都能使误差绝对值稳定在0.1m以内。由图6可以看出，北坐标误差一开始迅速减小，而其绝对值减小到0.1m以内，融合控制律所用时间最低为12s左右，Lyapunov控制律为18s左右，反演滑模控制律为20s左右；而最终收敛情况下，融合控制律的误差范围为(-0.02m，0.02m)，Lyapunov控制律的误差范围为(-0.01m，0.04m)，反演滑模控制律的误差范围为(-0.05m，0.05m)，融合控制律的控制效果更好。由图7可以看出，在车辆启动后，航向角误差也迅速减小，而其绝对值减小到0.1rad以内，融合控制律所用的时间为10s，Lyapunov控制律所用时间为14s，反演滑模控所用时间为11s，在收敛稳定后，3种控制率的航向角的误差范围都为(-0.03rad，0.03rad)；由图8可以看出，三种控制律最终都能使车辆成功跟踪参考轨迹。综上可以看出，三种控制律中，融合控制律的收敛速度最快，收敛稳定后，融合控制律的效果也稍优于另外两种控制律，证明了融合控制律的优越性。

为进一步验证融合控制律的有效性，又设计了圆形跟踪轨迹，进行了试验，依然取参数k₁＝1.0，k₂＝0.02，k₃＝0.3，k₄＝0.02，k₅＝0.9，k₆＝0.25，δ₁＝δ₂＝0.001。设圆形轨迹的圆心为参考坐标系原点，圆半径为3m；则参考车辆的初始位置为(3.0m,0m)，航向角为0.5πrad，线速度为0.24m/s，角速度为0.08rad/s；实际车辆的初始位置和航向角为(3.5m，0m)，航向角为0.25πrad，初始线速度0m/s，角速度为0rad/s；试验时间设置为80s，得到的轨迹图如图9所示；东坐标、北坐标和航向角误差如图10所示。

从图9可以看出，经过30s的控制后,实际车辆轨迹基本与参考轨迹重合，耗费时间稍长主要是速度限制和初始航向角误差较大导致的。由图10可以看出东坐标误差在一开始的9s内继续变大，而后迅速减小，并在20s左右有到达超调顶峰，然后在28s左右时误差绝对值小于0.1m，保持稳定后误差范围为(-0.05m，0.05m)；北坐标误差在14s左右到达最大值，然后迅速减小，然后在29s左右时误差绝对值小于0.1m，保持稳定后误差范围为(-0.04m，0.04m)；航向角误差一开始迅速减小，后有一定超调，在30s左右误差绝对值小于0.1rad，保持稳定后误差范围为(0rad，0.07rad)。

由于是针对农用履带电动车辆设计的轨迹跟踪控制器，所以参考设计了田间作业时参考轨迹，并进行了试验，依然取参数k₁＝1.0，k₂＝0.02，k₃＝0.3，k₄＝0.02，k₅＝0.9，k₆＝0.25，δ₁＝δ₂＝0.001。以实际车辆的初始位置为相对坐标系原点(0m，0m)，初始航向角为0rad，初始线速度为0m/s，初始角速度为0rad/s；则参考轨迹的初始位置为(1.5m，0m)，航向角为0rad。参考轨迹中，直线时线速度为0.5m/s，角速度为0rad/s；曲线时线速度为0.3m/s，角速度为0.125rad/s；由于实验场地条件限制，直线距离设定为9m，转弯的圆半径设定为2.4m，即行间距为4.8m。试验时间设置为210s，得到的轨迹跟踪图如图11所示，东坐标、北坐标以及航向角误差如图12所示。

从图11可以看出，在直线行驶段时车辆能够基本满足作业要求，而在转向时，由于角速度由0rad/s切换到0.125rad/s再到0rad/s，而所以会有一定的误差，但是基本能完成作业要求。由图12可以看出，直线行驶段时，稳定后东坐标和北坐标误差范围为(-0.06m，0.09m),航向角误差范围为(-0.1rad，0.1rad)；转向时,东坐标和北坐标误差范围为(-0.2m，0.3m)，航向角误差范围为(-0.19rad，0.22rad)。

以上实验结果表明，通过本发明设计的履带式车辆轨迹跟踪控制方法，履带式车辆能快速跟踪到参考轨迹完成田间作业，跟踪误差也能令人满意。

虽然结合附图描述了本发明的实施例，但是本领域技术人员可以在不脱离本发明的精神和范围的情况下作出各种修改和变型，这样的修改和变型均落入由所附权利要求所限定的范围之内。

Claims (12)

1.一种履带式车辆轨迹跟踪控制方法，其特征在于包括以下步骤：

基于轨迹跟踪误差模型，根据Lyapunov方法获取第一控制律；其中，所述轨迹跟踪误差模型是基于履带式车辆的运动学模型建立的；

基于所述轨迹跟踪误差模型，根据反演滑模控制方法获取第二控制律；

结合所述第一控制律和所述第二控制律推导出融合控制律；

根据所述融合控制律控制所述履带式车辆履带的转速。

2.根据权利要求1所述的履带式车辆轨迹跟踪控制方法，其特征在于，还包括：

获取驱动电机的各项参数；

根据所述各项参数修正所述融合控制律。

3.根据权利要求2所述的履带式车辆轨迹跟踪控制方法，其特征在于，所述履带式车辆的运动学模型为：

其中，x、y分别为所述履带式车辆质心在全局坐标系(X，O，Y)中的横坐标和纵坐标，θ为车身纵向与X轴的夹角，v为所述履带式车辆的线速度，ω为所述履带式车辆的角速度，v_L为所述履带式车辆左侧履带的线速度，v_R为所述履带式车辆右侧履带的线速度，L为所述履带式车辆两侧履带之间的距离，d为所述履带式车辆的履带宽度，N_L为所述履带式车辆左侧驱动电机的转速，N_R为所述履带式车辆右侧驱动电机的转速，r为所述履带式车辆驱动轮的半径，i为所述履带式车辆减速器传动比。

4.根据权利要求3所述的履带式车辆轨迹跟踪控制方法，其特征在于，所述履带式车辆的轨迹跟踪误差模型为：

其中，x_e为所述履带式车辆实际位置与参考位置的横向误差，y_e为所述履带式车辆实际位置与参考位置的纵向误差，θ_e为所述履带式车辆的航向角误差，v_d为所述履带式车辆的参考速度，ω_d为所述履带式车辆的参考角速度。

5.根据权利要求4所述的履带式车辆轨迹跟踪控制方法，其特征在于，所述第一控制律为：

其中，ω₁为第一控制输入角速度，v₁为第一控制输入速度，m₁、m₂、m₃均为正常数。

6.根据权利要求5所述的履带式车辆轨迹跟踪控制方法，其特征在于，所述第二控制律为：

其中，ω₂为第二控制输入角速度，v₂为第二控制输入速度，S₁＝x_e,S₂＝θ_e+arctan(v_dy_e)，j₁、j₂、p₁、p₂均为正常数。

7.根据权利要求6所述的履带式车辆轨迹跟踪控制方法，其特征在于，所述融合控制律为：

其中，k₁，k₂，k₃，k₄，k₅，k₆，δ₁，δ₂均为正常数。

8.根据权利要求7所述的履带式车辆轨迹跟踪控制方法，其特征在于，所述各项参数包括所述驱动电机的最大转速和最大加速度。

9.根据权利要求8所述的履带式车辆轨迹跟踪控制方法，其特征在于，根据所述各项参数修正所述融合控制律包括限定角速度和线速度的范围为：

其中，N_max为驱动电机的最大转速。

10.根据权利要求8所述的履带式车辆轨迹跟踪控制方法，其特征在于，根据所述各项参数修正所述融合控制律包括限定最大加速度和最大角加速度范围为：

其中，α_max为最大角加速度，β_max为最大线加速度，M_max为驱动电机的最大加速度。

11.一种履带式车辆轨迹跟踪控制装置，其特征在于，包括：

第一数据处理模块，用于基于轨迹跟踪误差模型，根据Lyapunov方法获取第一控制律；其中，所述轨迹跟踪误差模型是基于履带式车辆的运动学模型建立的；

第二数据处理模块，用于基于所述轨迹跟踪误差模型，根据反演滑模控制方法获取第二控制律；

第三数据处理模块，用于结合所述第一控制律和所述第二控制律推导出融合控制律；

控制模块，用于根据所述融合控制律控制所述履带式车辆履带的转速。

12.一种履带式车辆轨迹跟踪控制系统，其特征在于，包括：经纬度获取模块、航向角获取模块、上位机、左电机控制器和右电机控制器；

所述经纬度获取模块用于获取所述履带式车辆的经纬度，并发送给所述上位机；

所述航向角获取模块用于获取所述履带式车辆的航向角，并发送给所述上位机；

所述上位机用于获取所述履带式车辆的参考轨迹，并根据权利要求1-10任一项所述的履带式车辆轨迹跟踪控制方法确定所述履带式车辆左侧驱动电机的转速和右侧驱动电机的转速；所述上位机还用于将所述履带式车辆左侧驱动电机转速发送给所述左电机控制器，并将所述履带式车辆右侧驱动电机的转速发送给所述右电机控制器；

所述左电机控制器用于控制左电机的转速；

所述右电机控制器用于控制右电机的转速。

Images