一种无人履带车辆及其轨迹跟踪控制方法和系统

摘要

本发明涉及一种无人履带车辆及其轨迹跟踪控制方法和系统，属于车辆轨迹跟踪控制技术领域。本发明先基于无人履带车辆的运动学模型生成的轨迹跟踪控制器的目标函数，以及基于这一函数和无人履带车辆的动力学模型生成的轨迹跟踪控制器的约束函数形成MPC模型，然后，根据无人履带车辆的位姿特征、道路的曲率特征和实时工况状态信息生成参考轨迹后，将参考轨迹和车辆轨迹跟踪响应状态信息输入MPC模型得到参数组合，接着，将参数组合输入MLP神经网络得到控制参数，最后，根据控制参数完成无人履带车辆轨迹的跟踪控制，进而有效的提升无人履带车辆轨迹跟踪的工况适应性，实现轨迹跟踪精度、车辆行驶稳定性以及计算时间成本的较好均衡。

权利要求

1.一种无人履带车辆的轨迹跟踪控制方法，其特征在于，包括：

步骤100：获取无人履带车辆的运动学模型和无人履带车辆的动力学模型；其中，基于传统的无人履带车辆运动学模型，通过对无人履带车辆接地段的运动学分析，建立了基于瞬时转向中心模型的无人履带车辆的运动学模型；

所述基于瞬时转向中心模型的无人履带车辆的运动学模型为：

；

其中，ξ为无人履带车辆的状态向量， 为无人履带车辆的横向速度， 为无人履带车辆的纵向速度， 为无人履带车辆的横摆角速度，vj1为一侧履带的卷绕速度，vj2为另一侧履带的卷绕速度，φ为履带车辆的几何中心在固连于地面的笛卡尔坐标系下的航向角，为一侧履带的横向方向， 为另一侧履带的横向方向；

所述无人履带车辆的动力学模型为：

其中，xc为无人履带车辆的重心与几何中心的横向距离，yc为无人履带车辆的重心与几何中心的纵向距离，Fcx为离心力的横向分量、Fcy为离心力的纵向分量，Fr1为一侧履带接地段所受的地面变形阻力，Fr2为另一侧履带接地段所受的地面变形阻力，Fy1为一侧履带制动力，Fy2为另一侧履带驱动力，Fx1为一侧履带所受的横向阻力的合力、Fx2为另一侧履带所受的横向阻力的合力，m为无人履带车辆的整车质量，δ为无人履带车辆旋转质量换算系数，Mμ为转向阻力矩，Mμ=Mμ1+Mμ2，Mμ1为一侧履带所受的转向阻力矩、Mμ2为另一侧履带所受的转向阻力矩，J为无人履带车辆绕z’轴的转动惯量，B为履带中心距， 为车辆的横向加速度，为车辆的纵向加速度， 为车辆的横摆角加速度；

步骤101：基于所述无人履带车辆的运动学模型生成轨迹跟踪控制器的目标函数；所述轨迹跟踪控制器的目标函数为：其中，J(ξ(k)，Uk)为轨迹跟踪控制器的目标函数，ξ(k)为k时刻的车辆状态向量，Uk为控制序列，Np为预测时域的个数、Nc为控制时域的个数，且Nc≤Np，i为个数变量，le(k+i)为k+i时刻的轨迹跟踪的横向偏差，he(k+i)为k+i时刻的轨迹跟踪的航向偏差，v(k+i)为k+i时刻的实际车速，ldesired(k+i)为k+i时刻的期望横向偏差，hdesired(k+i)为k+i时刻的期望航向偏差，vdesired(k+i)为k+i时刻的期望车速，Q、R、P、M和N均为权重系数矩阵，vj1(*)为*时刻的一侧履带的卷绕速度，vj2(*)为*时刻的另一侧履带的卷绕速度；

步骤102：基于所述无人履带车辆的动力学模型和所述轨迹跟踪控制器的目标函数生成轨迹跟踪控制器的约束函数；所述轨迹跟踪控制器的约束函数包括：车辆纵向速度约束、转向中心纵向偏移量约束、电机转速约束和电机功率约束；

步骤103：基于所述轨迹跟踪控制器的目标函数和所述轨迹跟踪控制器的约束函数生成MPC模型；

步骤104：根据无人履带车辆的位姿特征、道路的曲率特征和实时工况状态信息生成参考轨迹；

步骤105：将所述参考轨迹和车辆轨迹跟踪响应状态信息输入所述MPC模型得到参数组合；所述车辆轨迹跟踪响应状态信息通过实验获得；所述参数组合包括：轨迹曲率特征向量、无人履带车辆姿态参考向量和无人履带车辆速度信息向量；

步骤106：将所述参数组合输入MLP神经网络得到控制参数；

步骤107：根据所述控制参数完成无人履带车辆轨迹的跟踪控制。

2.一种无人履带车辆的轨迹跟踪控制系统，其特征在于，包括：

模型获取模块，用于获取无人履带车辆的运动学模型和无人履带车辆的动力学模型；

其中，基于传统的无人履带车辆运动学模型，通过对无人履带车辆接地段的运动学分析，建立了基于瞬时转向中心模型的无人履带车辆的运动学模型；

所述基于瞬时转向中心模型的无人履带车辆的运动学模型为：

；

其中，ξ为无人履带车辆的状态向量， 为无人履带车辆的横向速度， 为无人履带车辆的纵向速度， 为无人履带车辆的横摆角速度，vj1为一侧履带的卷绕速度，vj2为另一侧履带的卷绕速度，φ为履带车辆的几何中心在固连于地面的笛卡尔坐标系下的航向角，为一侧履带的横向方向， 为另一侧履带的横向方向；

所述无人履带车辆的动力学模型为：

其中，xc为无人履带车辆的重心与几何中心的横向距离，yc为无人履带车辆的重心与几何中心的纵向距离，Fcx为离心力的横向分量、Fcy为离心力的纵向分量，Fr1为一侧履带接地段所受的地面变形阻力，Fr2为另一侧履带接地段所受的地面变形阻力，Fy1为一侧履带制动力，Fy2为另一侧履带驱动力，Fx1为一侧履带所受的横向阻力的合力、Fx2为另一侧履带所受的横向阻力的合力，m为无人履带车辆的整车质量，δ为无人履带车辆旋转质量换算系数，Mμ为转向阻力矩，Mμ=Mμ1+Mμ2，Mμ1为一侧履带所受的转向阻力矩、Mμ2为另一侧履带所受的转向阻力矩，J为无人履带车辆绕z’轴的转动惯量，B为履带中心距， 为车辆的横向加速度，为车辆的纵向加速度， 为车辆的横摆角加速度；

目标函数生成模块，用于基于所述无人履带车辆的运动学模型生成轨迹跟踪控制器的目标函数；所述轨迹跟踪控制器的目标函数为：其中，J(ξ(k)，Uk)为轨迹跟踪控制器的目标函数，ξ(k)为k时刻的车辆状态向量，Uk为控制序列，Np为预测时域的个数、Nc为控制时域的个数，且Nc≤Np，i为个数变量，le(k+i)为k+i时刻的轨迹跟踪的横向偏差，he(k+i)为k+i时刻的轨迹跟踪的航向偏差，v(k+i)为k+i时刻的实际车速，ldesired(k+i)为k+i时刻的期望横向偏差，hdesired(k+i)为k+i时刻的期望航向偏差，vdesired(k+i)为k+i时刻的期望车速，Q、R、P、M和N均为权重系数矩阵，vj1(*)为*时刻的一侧履带的卷绕速度，vj2(*)为*时刻的另一侧履带的卷绕速度；

约束函数生成模块，用于基于所述无人履带车辆的动力学模型和所述轨迹跟踪控制器的目标函数生成轨迹跟踪控制器的约束函数；所述轨迹跟踪控制器的约束函数包括：车辆纵向速度约束、转向中心纵向偏移量约束、电机转速约束和电机功率约束；

MPC模型生成模块，用于基于所述轨迹跟踪控制器的目标函数和所述轨迹跟踪控制器的约束函数生成MPC模型；

参考轨迹生成模块，用于根据无人履带车辆的位姿特征、道路的曲率特征和实时工况状态信息生成参考轨迹；

参数组合确定模块，用于将所述参考轨迹和车辆轨迹跟踪响应状态信息输入所述MPC模型得到参数组合；所述车辆轨迹跟踪响应状态信息通过实验获得；所述参数组合包括：轨迹曲率特征向量、无人履带车辆姿态参考向量和无人履带车辆速度信息向量；

控制参数确定模块，用于将所述参数组合输入MLP神经网络得到控制参数；

跟踪控制模块，用于根据所述控制参数完成无人履带车辆轨迹的跟踪控制。

3.一种无人履带车辆，其特征在于，包括如权利要求2所述的轨迹跟踪控制系统。

说明书

一种无人履带车辆及其轨迹跟踪控制方法和系统

技术领域

[0001] 本发明涉及车辆轨迹跟踪控制技术领域，特别是涉及一种无人履带车辆及其轨迹跟踪控制方法和系统。

背景技术

[0002] 近年来，地面无人平台在港口运输、疫情防控、野外搜救以及战场环境中得到了越来越多的应用，轨迹跟踪控制作为无人驾驶的关键技术，其发展也得到了越来越多学者的关注，这其中以模型预测的控制方法应用较多。然而，目前大多数都是基于定参数的轨迹跟踪控制方法，很难保证车辆在复杂多变工况下的跟踪精度和稳定性。基于此，有学者提出了自适应参数调整的方法，包括更精确的车辆模型参数在线估计、更合理的控制系统结构设计以及更高效的优化算法架构设计等。由于越野工况下地面‑履带的相互作用关系难以精确表征，因此基于车辆模型参数在线估计的方法很难为系统提供稳定、可靠的输入，且在线估计对系统的实时资源配置要求较高。部分研究成果以MPC为基本框架，通过增加前馈、数据学习等方式提升轨迹跟踪性能，但是缺乏在实际高速履带平台的验证。

发明内容

[0003] 为解决现有技术存在的上述问题，本发明提供了一种无人履带车辆及其轨迹跟踪控制方法和系统。

[0004] 为实现上述目的，本发明提供了如下方案：

[0005] 一种无人履带车辆的轨迹跟踪控制方法，包括：

[0006] 获取无人履带车辆的运动学模型和无人履带车辆的动力学模型；

[0007] 基于所述无人履带车辆的运动学模型生成轨迹跟踪控制器的目标函数；

[0008] 基于所述无人履带车辆的动力学模型和所述轨迹跟踪控制器的目标函数生成轨迹跟踪控制器的约束函数；

[0009] 基于所述轨迹跟踪控制器的目标函数和所述轨迹跟踪控制器的约束函数生成MPC模型；

[0010] 根据无人履带车辆的位姿特征、道路的曲率特征和实时工况状态信息生成参考轨迹；

[0011] 将所述参考轨迹和车辆轨迹跟踪响应状态信息输入所述MPC模型得到参数组合；所述车辆轨迹跟踪响应状态信息通过实验获得；所述参数组合包括：轨迹曲率特征向量、无人履带车辆姿态参考向量和无人履带车辆速度信息向量；

[0012] 将所述参数组合输入MLP神经网络得到控制参数；

[0013] 根据所述控制参数完成无人履带车辆轨迹的跟踪控制。

[0014] 优选地，基于传统的无人履带车辆运动学模型，通过对无人履带车辆接地段的运动学分析，建立了基于瞬时转向中心模型的无人履带车辆的运动学模型。

[0015] 优选地，所述基于瞬时转向中心模型的无人履带车辆的运动学模型为：

[0016] ；

[0017] 其中，ξ为无人履带车辆的状态向量， 为无人履带车辆的横向速度， 为无人履带车辆的纵向速度， 为无人履带车辆的横摆角速度，vj1为一侧履带的卷绕速度，vj2为另一侧履带的卷绕速度，φ为履带车辆的几何中心在固连于地面的笛卡尔坐标系下的航向角， 为一侧履带的横向方向， 为另一侧履带的横向方向。

[0018] 优选地，所述无人履带车辆的动力学模型为：

[0019]

[0020] 其中，xc为无人履带车辆的重心与几何中心的横向距离，yc为无人履带车辆的重心与几何中心的纵向距离，Fcx为离心力的横向分量、Fcy为离心力的纵向分量，Fr1为一侧履带接地段所受的地面变形阻力，Fr2为另一侧履带接地段所受的地面变形阻力，Fy1为一侧履带制动力，Fy2为另一侧履带驱动力，Fx1为一侧履带所受的横向阻力的合力、Fx2为另一侧履带所受的横向阻力的合力，m为无人履带车辆的整车质量，δ为无人履带车辆旋转质量换算系数，Mμ为转向阻力矩，Mμ=Mμ1+Mμ2，Mμ1为一侧履带所受的转向阻力矩、Mμ2为另一侧履带所受的转向阻力矩，J为无人履带车辆绕z’轴的转动惯量，B为履带中心距， 为车辆的横向加速度， 为车辆的纵向加速度， 为车辆的横摆角加速度。

[0021] 优选地，所述轨迹跟踪控制器的目标函数为：

[0022]

[0023] 其中，J(ξ(k)，Uk)为轨迹跟踪控制器的目标函数，ξ(k)为k时刻的车辆状态向量，Uk为控制序列，Np为预测时域的个数、Nc为控制时域的个数，且Nc≤Np，i为个数变量，le(k+i)为k+i时刻的轨迹跟踪的横向偏差，he(k+i)为k+i时刻的轨迹跟踪的航向偏差，v(k+i)为k+i时刻的实际车速，ldesired(k+i)为k+i时刻的期望横向偏差，hdesired(k+i)为k+i时刻的期望航向偏差，vdesired(k+i)为k+i时刻的期望车速，Q、R、P、M和N均为权重系数矩阵，vj1(*)为*时刻的一侧履带的卷绕速度，vj2(*)为*时刻的另一侧履带的卷绕速度。

[0024] 优选地，所述轨迹跟踪控制器的约束函数包括：车辆纵向速度约束、转向中心纵向偏移量约束、电机转速约束和电机功率约束。

[0025] 对应于上述提供的无人履带车辆的轨迹跟踪控制方法，本发明还提供了一种无人履带车辆的轨迹跟踪控制系统，该轨迹跟踪控制系统包括：

[0026] 模型获取模块，用于获取无人履带车辆的运动学模型和无人履带车辆的动力学模型；

[0027] 目标函数生成模块，用于基于所述无人履带车辆的运动学模型生成轨迹跟踪控制器的目标函数；

[0028] 约束函数生成模块，用于基于所述无人履带车辆的动力学模型和所述轨迹跟踪控制器的目标函数生成轨迹跟踪控制器的约束函数；

[0029] MPC模型生成模块，用于基于所述轨迹跟踪控制器的目标函数和所述轨迹跟踪控制器的约束函数生成MPC模型；

[0030] 参考轨迹生成模块，用于根据无人履带车辆的位姿特征、道路的曲率特征和实时工况状态信息生成参考轨迹；

[0031] 参数组合确定模块，用于将所述参考轨迹和车辆轨迹跟踪响应状态信息输入所述MPC模型得到参数组合；所述车辆轨迹跟踪响应状态信息通过实验获得；所述参数组合包括：轨迹曲率特征向量、无人履带车辆姿态参考向量和无人履带车辆速度信息向量；

[0032] 控制参数确定模块，用于将所述参数组合输入MLP神经网络得到控制参数；

[0033] 跟踪控制模块，用于根据所述控制参数完成无人履带车辆轨迹的跟踪控制。

[0034] 此外，还提供了一种无人履带车辆，该无人履带车辆包括有上述轨迹跟踪控制系统。

[0035] 根据本发明提供的具体实施例，本发明公开了以下技术效果：

[0036] 本发明提供的无人履带车辆的轨迹跟踪控制方法，以无人履带车辆在越野工况下的大量行驶数据为依托，以轨迹跟踪的高精度、高稳定性、低时间成本为目标，完成最优参数组合的离线提取和最优控制参数的在线预测，为基于模型预测的轨迹跟踪控制器提供时变双参数输入（控制时域长度、时间步长），实现无人履带车辆越野工况下的自适应轨迹跟踪控制，进而有效的提升无人履带车辆轨迹跟踪的工况适应性，实现轨迹跟踪精度、车辆行驶稳定性以及计算时间成本的较好均衡。

附图说明

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

[0038] 图1为本发明提供的无人履带车辆的轨迹跟踪控制方法的流程图；

[0039] 图2为本发明实施例提供的实施无人履带车辆的轨迹跟踪控制方法的数据框图；

[0040] 图3为本发明实施例提供的MLP算法原理图；

[0041] 图4为本发明提供的无人履带车辆的轨迹跟踪控制系统的结构示意图。

具体实施方式

[0042] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

[0043] 本发明的目的是提供一种无人履带车辆及其轨迹跟踪控制方法和系统，以为无人履带车辆轨迹的跟踪控制提供稳定、可靠的输入，同时降低无人履带车辆轨迹跟踪控制的资源配置，进而能够提升无人履带车辆轨迹跟踪控制性能。

[0044] 为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

[0045] 如图1所示，本发明提供的无人履带车辆的轨迹跟踪控制方法，包括：

[0046] 步骤100：获取无人履带车辆的运动学模型和无人履带车辆的动力学模型。其中，基于传统的无人履带车辆运动学模型，通过对无人履带车辆接地段的运动学分析，建立了基于瞬时转向中心模型的无人履带车辆的运动学模型。建立得到的基于瞬时转向中心模型的无人履带车辆的运动学模型为：

[0047] ；

[0048] 其中，ξ为无人履带车辆的状态向量， 为无人履带车辆的横向速度， 为无人履带车辆的纵向速度， 为无人履带车辆的横摆角速度，vj1为一侧履带的卷绕速度，vj2为另一侧履带的卷绕速度。

[0049] 无人履带车辆的动力学模型为：

[0050]

[0051] 其中，xc为无人履带车辆的重心与几何中心的横向距离，yc为无人履带车辆的重心与几何中心的纵向距离，Fcx为离心力的横向分量、Fcy为离心力的纵向分量，Fr1为一侧履带接地段所受的地面变形阻力，Fr2为另一侧履带接地段所受的地面变形阻力，Fy1为一侧履带制动力，Fy2为另一侧履带驱动力，Fx1为一侧履带所受的横向阻力的合力、Fx2为另一侧履带所受的横向阻力的合力，m为无人履带车辆的整车质量，δ为无人履带车辆旋转质量换算系数，Mμ为转向阻力矩，Mμ=Mμ1+Mμ2，Mμ1为一侧履带所受的转向阻力矩、Mμ2为另一侧履带所受的转向阻力矩，J为无人履带车辆绕z’轴的转动惯量，B为履带中心距， 为车辆的横向加速度， 为车辆的纵向加速度， 为车辆的横摆角加速度。

[0052] 步骤101：基于无人履带车辆的运动学模型生成轨迹跟踪控制器的目标函数。其中，轨迹跟踪控制器的目标函数为：

[0053]

[0054] 其中，J(ξ(k)，Uk)为轨迹跟踪控制器的目标函数，ξ(k)为k时刻的车辆状态向量，Uk为控制序列，Np为预测时域的个数、Nc为控制时域的个数，且Nc≤Np，i为个数变量，le(k+i)为k+i时刻的轨迹跟踪的横向偏差，he(k+i)为k+i时刻的轨迹跟踪的航向偏差，v(k+i)为k+i时刻的实际车速，ldesired(k+i)为k+i时刻的期望横向偏差，hdesired(k+i)为k+i时刻的期望航向偏差，vdesired(k+i)为k+i时刻的期望车速，Q、R、P、M和N均为权重系数矩阵，vj1(*)为*时刻的一侧履带的卷绕速度，vj2(*)为*时刻的另一侧履带的卷绕速度。

[0055] 步骤102：基于无人履带车辆的动力学模型和轨迹跟踪控制器的目标函数生成轨迹跟踪控制器的约束函数。轨迹跟踪控制器的约束函数包括：车辆纵向速度约束、转向中心纵向偏移量约束、电机转速约束和电机功率约束。其中各约束条件的具体设置过程在下述实施例中予以说明。

[0056] 步骤103：基于轨迹跟踪控制器的目标函数和轨迹跟踪控制器的约束函数生成MPC模型。

[0057] 步骤104：根据无人履带车辆的位姿特征、道路的曲率特征和实时工况状态信息生成参考轨迹。

[0058] 步骤105：将参考轨迹和车辆轨迹跟踪响应状态信息输入MPC模型得到参数组合。车辆轨迹跟踪响应状态信息通过实验获得。参数组合包括：轨迹曲率特征向量、无人履带车辆姿态参考向量和无人履带车辆速度信息向量。

[0059] 步骤106：将参数组合输入MLP神经网络得到控制参数。

[0060] 步骤107：根据控制参数完成无人履带车辆轨迹的跟踪控制。

[0061] 下面基于PSO‑MLP算法，以控制时域长度tl和控制时间步长ts为优化目标，以离线和在线相结合的方式，对上述提供的无人履带车辆的轨迹跟踪控制方法进行详细说明。离线部分，首先对不同控制参数下的履带车辆轨迹跟踪响应状态信息进行提取，得到包含运动基元类型、参考轨迹Sr、轨迹跟踪响应信息、车辆状态ξ(k)以及控制参数的车辆历史数据集合DH。然后，基于改进PSO粒子群算法得到不同运动基元下的最优控制参数组合，并构建训练数据集，利用Adam学习率自适应优化算法完成对MLP神经网络模型的训练。在线部分，首先对运动基元类型和车辆状态进行特征匹配，基于MLP神经网络对控制时间步长ts和控制时域长度tl进行预测，为轨迹跟踪控制模块提供自适应双参数输入。然后，结合自适应参数输入、车辆模型、约束条件c以及代价函数等建立轨迹跟踪控制器，得到最优控制量u，实现了双侧独立电驱动履带平台的自适应轨迹跟踪控制。基于如图2所示的数据处理框架，该方法的具体实施过程如下：

[0062] 步骤1：建立履带车辆运动学模型。

[0063] 首先对履带车辆的相关坐标系进行定义：oxy为固连于地面的空间笛卡尔坐标系，坐标轴x、y所在平面与路面重合； 为固连于履带车辆并随履带车辆共同运动的空间笛卡尔坐标系， 位于履带车辆的几何中心， 为车辆的横向方向， 为车辆的纵向方向。 为履带车辆的瞬时转向中心， 分别为低速侧履带、高速侧履带的瞬时转向中心。

[0064] 为了给步骤3中模型预测控制器目标函数提供可靠的模型支撑、准确的履带车辆状态空间表达，需要首先建立履带车辆的运动学模型。传统的履带车辆运动学模型可以表示为：

[0065]                      （1）

[0066] 其中， 分别为履带车辆的几何中心在固连于地面的笛卡尔坐标系下的横坐标、纵坐标、航向角； 分别为车辆的横向速度、纵向速度； 为车辆的横摆角速度，定义逆时针为正。

[0067] 传统的履带车辆运动学模型并没有考虑到履带车辆的滑移滑转特性，而滑移滑转特性是履带车辆在行进过程中不可避免的，因此传统的履带车辆运动学模型精度较差。基于传统的履带车辆运动学模型，通过对履带车辆以及履带接地段的运动学分析，建立了基于瞬时转向中心模型进行履带车辆运动学模型，可以真实的反应履带车辆的运动学特性，为轨迹跟踪控制方法提供可靠、稳定的模型支撑，基于瞬时转向中心的履带车辆运动学微分方程可以表示为：

[0068]       （2）

[0069] 其中，ξ为无人履带车辆的状态向量， 为无人履带车辆的横向速度， 为无人履带车辆的纵向速度， 为无人履带车辆的横摆角速度，vj1为一侧履带的卷绕速度，vj2为另一侧履带的卷绕速度，φ为履带车辆的几何中心在固连于地面的笛卡尔坐标系下的航向角， 为一侧履带的横向方向， 为另一侧履带的横向方向。

[0070] 上述建立的履带车辆运动学模型是后续生成MPC控制器状态空间的基础，该模型的精确性也决定了模型在进行仿真计算时的结果与真实情况的准确性，反映了仿真结果的可靠性。

[0071] 步骤2：建立履带车辆动力学模型

[0072] 为了提升履带车辆模型的精度，保证履带车辆在行进过程中的动力性、安全性，需要结合动力学模型对步骤4中轨迹跟踪控制器约束设置施加安全约束。为了简化建模，进行如下所示的假设：

[0073] （1）车辆在转向过程中，不考虑履带的沉陷作用和推土作用；

[0074] （2）履带载荷沿着履带长度方向均匀分布；

[0075] （3）在进行阻力分析时，忽略空气阻力的作用；

[0076] （4）在转向过程中的行驶阻力系数与直驶过程相同；

[0077] 基于达朗贝尔原理，对车辆几何中心建立履带车辆动力学方程组，如下所示：

[0078] （3）

[0079] 其中，xc为无人履带车辆的重心与几何中心的横向距离，yc为无人履带车辆的重心与几何中心的纵向距离，Fcx为离心力的横向分量、Fcy为离心力的纵向分量，Fr1为一侧履带接地段所受的地面变形阻力，Fr2为另一侧履带接地段所受的地面变形阻力，Fy1为一侧履带制动力，Fy2为另一侧履带驱动力，Fx1为一侧履带所受的横向阻力的合力、Fx2为另一侧履带所受的横向阻力的合力，m为无人履带车辆的整车质量，δ为无人履带车辆旋转质量换算系数，Mμ为转向阻力矩，Mμ=Mμ1+Mμ2，Mμ1为一侧履带所受的转向阻力矩、Mμ2为另一侧履带所受的转向阻力矩，J为无人履带车辆绕z’轴的转动惯量，B为履带中心距， 为车辆的横向加速度， 为车辆的纵向加速度， 为车辆的横摆角加速度。

[0080] 步骤3：建立模型预测控制器目标函数

[0081] 对于步骤1中建立的履带车辆运动学微分方程（公式（2）），使用矩阵代数中有微分方程到状态空间的方法，可以建立该履带车辆的状态空间表达式。另外，在进行代价函数的构建时，需要综合考虑轨迹跟踪精度、稳定性。

[0082] 为求得在k时刻对于车辆的最优控制序列，需要求使得代价函数取最小值的控制量，从而为步骤6中的最优参数组合数据集的建立提供轨迹跟踪响应状态信息，因此基于离散非线性模型的模型预测控制(MPC)优化问题可以描述为：

[0083]                             （4）

[0084] 优化问题当中的目标函数是进行控制量求解的必须条件，目标函数中各个项目的设置也会直接影响轨迹跟踪的效果以及步骤6中轨迹跟踪效应状态信息的构建。

[0085] 满足时域约束条件：

[0086]               （5）

[0087] 其中目标函数为：

[0088]  （6）

[0089] 其中，ξ(k)为k时刻的车辆状态向量，ξ(k)∈Rn，u(k)为k时刻的控制输入向量，um b(k)∈R ，Uk为控制序列，yc(k)为k时刻的控制输出向量，yb(k)为约束输出向量，yb(k)∈R ，umin为控制量下限值，umax为控制量上限值，Δumin为控制量变化率的下限值，Δumax为控制量变化率的上限值，Np为预测时域的个数，Nc为控制时域的个数，且Nc≤Np，ymin(k+i)为k+i时刻约束输出的下限值，ymax(k+i)为k+i时刻约束输出的上限值，le(k+i)为k+i时刻的轨迹跟踪的横向偏差，he(k+i)为k+i时刻的轨迹跟踪的航向偏差，v(k+i)为k+i时刻的实际车速，ldesired(k+i)为k+i时刻的期望横向偏差，hdesired(k+i)为k+i时刻的期望航向偏差，vdesired(k+i)为k+i时刻的期望车速，Q、R、P、M和N均为权重系数矩阵，vj1(*)为*时刻的一侧履带的卷绕速度，vj2(*)为*时刻的另一侧履带的卷绕速度。

[0090] 步骤4：轨迹跟踪控制器约束设置

[0091] 基于步骤1中的履带车辆运动学模型，进行步骤3中轨迹跟踪控制器的设计，由于并未考虑到履带车辆的动力学特性以及驱动电机的外特性，履带车辆可能会出现侧翻、侧滑、后备功率不足等情况。为了保证车辆的动力性和安全性，需要借助履带车辆的动力学模型以及电机外特性，为轨迹跟踪控制器提供可靠的约束设置。

[0092] 基于步骤2中建立的履带车辆动力学模型及其约束条件，为保证履带车辆不发生侧翻，车速需要满足：

[0093]                        （7）

[0094] 其中，g为重力加速度，R为履带车辆转向半径，hg为重心高度，β为车辆的侧倾角度，kr为侧翻阈值系数。

[0095] 为了保证车辆在转弯时，不发生失控的情况，车辆的转向中心纵向偏移量需要满足：

[0096]                              （8）

[0097] 其中，L为履带接地段长度，xc为无人履带车辆的重心与几何中心的横向距离，yc为无人履带车辆的重心与几何中心的纵向距离，λc为转向中心纵向偏移量。

[0098] 整理得到使履带车辆安全行进的转向中心纵向速度的约束条件：

[0099]                     （9）

[0100] 其中， 为路面附着系数； 为车辆俯仰角度，ks为侧滑阈值系数。

[0101] 在规划轨迹已知的条件下，由步骤2中的履带车辆动力学模型，在入弯之前，根据当前弯道对于车辆转向中心纵向速度的约束，对车辆转向的转矩需求进行预测：

[0102]                               （10）

[0103]                                （11）

[0104]                   （12）

[0105] 其中，T1、T2分别为低速侧、高速侧履带转矩需求，η为机械传动效率，i为综合传动比，r为驱动轮半径，T(vy,ρ)为基于规划轨迹的电机最大转矩需求，ρ为道路的曲率。这些参数表征了履带车辆转弯时需要的驱动力，结合实际电机的外特性曲线，能够对于临界极限转矩工况进行预测，防止极限工况下出现轨迹不可控等危险行为。

[0106] 将电机的转矩和功率作为约束输出，并设置转矩保护系数kT和后备功率保护系数kp，可得：

[0107]                        （13）

[0108]                        （14）

[0109] 其中， 分别为当前电机转速下，电机可以提供的最大转矩，最大功率， 为当前工况下的电机功率的需求。

[0110] 步骤5：参考轨迹的提取

[0111] 轨迹规划模块根据车辆位姿特征、道路的曲率特征，基于最优控制理论进行履带车辆运动基元的生成，并结合感知模块提供的实时工况状态信息进行运动基元的拼接，得到具备车辆位姿信息以及运动语义信息的参考轨迹，对车辆的期望路径进行预测：

[0112]                （15）

[0113] 其中， 为参考轨迹点在车体坐标下的坐标， 为参考轨迹的车辆三维姿态信息， 为参考轨迹的曲率； 为车辆的参考速度， 为履带车辆运动基元的类型。研究中结合算法验证的实际需求，基于车辆位姿及道路特征，将车辆运动基元分为类直线运动基元 、类小曲率转向运动基元 、类单移线运动基元 、类U型弯运动基元 四类。

[0114] 步骤6：基于改进PSO算法的最优参数组合提取

[0115] 基于步骤5中提取出来的参考轨迹以及基于实车试验得到的车辆轨迹跟踪响应状态信息将输入到MPC模型（步骤3、步骤4）的控制时域长度、时间步长作为粒子。

[0116] 适应度函数即目标函数的表达形式如式下所示：

[0117]          （16）

[0118] 其中， 为第i个粒子在第t轮迭代的适应度函数值， 为横向跟踪偏差的均值， 为航向跟踪偏差的均值， 分别为内、外侧履带卷绕速度控制量变化
率的均值， 为迭代求解时间的均值； 分别为对应项的权重
系数。

[0119] 粒子的最新位置和速度需要利用状态更新方程进行迭代求解，其状态更新方程如下式所示：

[0120] (17)

[0121]                           （18）

[0122] 其中，t、t+1为迭代次数， 、 分别为第i个粒子在第t、t+1轮迭代的速度， 分别为第i个粒子在第t、t+1轮迭代的位置； 为第i个粒子累
计到第t轮迭代的局部最优值， 为粒子群内所有粒子累计到第t轮迭代的全局最优值，c1、c2为学习因子，分别代表认知系数和社会系数，通常c1=c2∈（0,4），r1、r2为(0,1)之间的随机数。 为第t轮迭代的惯性因子，w>0，为了解决算法早熟以及迭代后期易在最优解附近震荡的情况，这里采用线性可变权重系数的方法进行权重系数的设置：

[0123]                     （19）

[0124] 其中，wmax、wmin分别为权重系数的最大值和最小值，tmax为最大迭代次数。

[0125] 步骤7：基于MLP的最优控制参数预测

[0126] 基于步骤6中提取的最优参数组合，进行MLP神经网络的离线训练和最优控制参数在线预测。本模块的工作原理如图3所示。

[0127] 其中， 为轨迹曲率特征向量；为车辆姿态参考向量， 为车
辆速度信息向量。

[0128] MLP神经网络具体算法流程如下所示：

[0129] （1）初始化网络深度l，模型权重系数矩阵 ，i∈(1,...,l)，模型的偏置参数等参数。模型的激活函数选择tanh函数：

[0130]                          （20）

[0131] （2）基于模型输入、模型参数进行模型输出的求解，并得到损失函数 。

[0132]                                 （21）

[0133]              （22）

[0134]                                 （23）

[0135] 其中， 为第k层隐藏单元的向量，x为模型输入； 为输入x时的神经网络输出。

[0136] （3）反向传播计算。

[0137] 未达到迭代终止条件时，需要根据误差的梯度方向对不同网络深度的权重系数矩阵和偏置参数进行更新，然后再重复步骤（2），直至达到迭代收敛条件。

[0138] 本发明采用Adam学习率自适应优化算法进行MLP神经网络的训练，具体流程如表1所示。

[0139]

[0140] 基于上述描述，相对于现有技术，本发明提供的无人履带车辆的轨迹跟踪控制方法具有以下优点：

[0141] 1、本发明可以保证越野工况下履带车辆轨迹跟踪的行驶安全性，通过建立的运动学模型和安全约束，考虑了车辆侧翻、侧滑失控、电机外特性边界等执行器约束，可以保证履带车辆工作在安全区间。

[0142] 2、本发明可以提升越野工况下履带车辆轨迹跟踪的精度、稳定性、计算时间成本等综合性能。其中，通过将轨迹跟踪的精度、控制量变化率作为惩罚项进行控制量的求解，保证较高的跟踪精度和较为平滑的控制量变化。通过以跟踪精度、行驶稳定性和计算时间成本为目标，进行最优控制参数的求解，保证车辆轨迹跟踪性能的均衡，提升越野工况下履带车辆轨迹跟踪控制的工况适应性。

[0143] 对应于上述提供的无人履带车辆的轨迹跟踪控制方法，本发明还提供了一种无人履带车辆的轨迹跟踪控制系统，如图3所示，该轨迹跟踪控制系统包括：

[0144] 模型获取模块400，用于获取无人履带车辆的运动学模型和无人履带车辆的动力学模型。

[0145] 目标函数生成模块401，用于基于无人履带车辆的运动学模型生成轨迹跟踪控制器的目标函数。

[0146] 约束函数生成模块402，用于基于无人履带车辆的动力学模型和轨迹跟踪控制器的目标函数生成轨迹跟踪控制器的约束函数。

[0147] MPC模型生成模块403，用于基于轨迹跟踪控制器的目标函数和轨迹跟踪控制器的约束函数生成MPC模型。

[0148] 参考轨迹生成模块404，用于根据无人履带车辆的位姿特征、道路的曲率特征和实时工况状态信息生成参考轨迹。

[0149] 参数组合确定模块405，用于将参考轨迹和车辆轨迹跟踪响应状态信息输入MPC模型得到参数组合。车辆轨迹跟踪响应状态信息通过实验获得。参数组合包括：轨迹曲率特征向量、无人履带车辆姿态参考向量和无人履带车辆速度信息向量。

[0150] 控制参数确定模块406，用于将参数组合输入MLP神经网络得到控制参数。

[0151] 跟踪控制模块407，用于根据控制参数完成无人履带车辆轨迹的跟踪控制。

[0152] 此外，还提供了一种无人履带车辆，该无人履带车辆包括有上述轨迹跟踪控制系统。

[0153] 本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

[0154] 本发明中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。