一种基于语义信息串联的履带车辆的控制方法及系统

技术领域

本发明涉及车辆控制领域，特别是涉及一种基于语义信息串联的履带车辆的控制方法及系统。

背景技术

地面无人机动履带车辆能够代替人类在越野环境下完成诸如危险物资运输、灾情救援、战场环境侦察等任务，减轻或避免人类在越野危险环境中的负担与消耗。地面无人机动履带车辆一般由环境感知、运动规划、运动控制、底盘执行四个基本模块组成。

前期的研究针对地面无人机动履带车辆中的各个模块均开展了相应的工作，但各个模块之的串联方式比较弱化，使地面无人机动履带车辆模块之间的输出信息比较单一，进而，造成地面无人机动履带车辆在越野环境中的通行能力比较低。

因此，为解决上述问题，亟需提供一种新的控制方法或系统，提升地面无人机动平台在越野环境中的通行能力。

发明内容

本发明的目的是提供一种基于语义信息串联的履带车辆的控制方法及系统，能够提升地面无人机动平台在越野环境中的通行能力。

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

一种基于语义信息串联的履带车辆的控制方法，包括：

根据环境感知模块获取的激光雷达数据提取激光雷达点云信息；并根据环境感知模块获取的相机图像提取图像语义信息；

将激光雷达点云信息和图像语义信息进行融合，并根据融合后的信息、全局拓扑参考路径以及履带车辆的几何通过性参数确定三维语义栅格地图；所述三维语义栅格地图为带有局部可通行度语义栅格地图；几何通过性参数包括：最大坡度，垂直墙跨越高度以及发生倾覆的最大横坡角度；

运动规划模块根据三维语义栅格地图、全局拓扑参考路径以及运动基元簇中的运动基元的扩展代价对运动基元簇中的运动基元进行拼接与过渡，确定带有行为语义信息的轨迹；运动基元簇包括：通过采样不同的目标位姿形成的运动基元；运动基元为履带车辆在设定时间内，在履带车辆的运动学约束、平滑过渡约束、平台-地形约束以及平台行为约束下，履带车辆从起始位姿到目标位姿的轨迹；扩展代价包括：运动基元与全局拓扑参考路径的横纵向偏差、碰撞风险代价、平滑代价以及通行度代价；

运动控制模块利用带有行为语义信息的轨迹对控制目标函数的权重系数进行实时更新，并根据更新后的控制目标函数和带有行为语义信息的轨迹在执行机构的机械约束、平台的运动学约束以及平台动力学约束下，基于模型预测控制算法进行履带车辆的控制；所述控制目标函数根据轨迹跟踪控制的横向偏差、航向偏差以及控制量变化率进行构建。

可选地，所述将激光雷达点云信息和图像语义信息进行融合，并根据融合后的信息、全局拓扑参考路径以及履带车辆的几何通过性参数确定三维语义栅格地图，具体包括：

通过环境感知模块中激光雷达与相机之间的坐标转换关系，将图像语义信息投影至激光雷达点云信息，确定激光雷达语义点云；激光雷达语义点云为融合后的信息；

根据激光雷达语义点云进行可通行区域的第一次粗提取；

根据第一次粗提取后的激光雷达语义点云信息提取点云特征；点云特征包括：道路的不平度信息、纵坡坡度以及横坡坡度；

根据点云特征与几何通过性参数进行可通行区域的第二次粗提取；

根据第二次粗提取后的激光雷达语义点云信息的点云特征，采用高斯混合聚类方法，确定点云特征对应位置的可通行度；进而根据全局拓扑参考路径，采用八叉树以及贝叶斯核推理，确定三维语义栅格地图。

可选地，所述运动规划模块根据三维语义栅格地图、全局拓扑参考路径以及运动基元库中的运动基元的扩展代价对运动基元库中的运动基元进行拼接与过渡，确定带有行为语义信息的轨迹，具体包括：

根据履带车辆的参考速度以及履带车辆前方可通行区域的通行度确定运动基元簇；

根据运动基元簇中的运动基元的扩展代价，确定最优扩展的运动基元；

将所有的最优扩展的运动基元进行拼接与过渡，确定带有行为语义信息的轨迹。

可选地，所述运动控制模块利用带有行为语义信息的轨迹对控制目标函数的权重系数进行实时更新，并根据更新后的控制目标函数和带有行为语义信息的轨迹在执行机构的机械约束、平台的运动学约束以及平台动力学约束下，基于模型预测控制算法进行履带车辆的控制，之前还包括：

获取履带车辆的运动学模型；

根据轨迹跟踪控制的横向偏差、航向偏差以及控制量变化率构建控制目标函数。

一种基于语义信息串联的履带车辆的控制系统，包括：

信息获取单元，用于根据环境感知模块获取的激光雷达数据提取激光雷达点云信息；并根据环境感知模块获取的相机图像提取图像语义信息；

三维语义栅格地图确定单元，用于将激光雷达点云信息和图像语义信息进行融合，并根据融合后的信息、全局拓扑参考路径以及履带车辆的几何通过性参数确定三维语义栅格地图；所述三维语义栅格地图为带有局部可通行度语义栅格地图；几何通过性参数包括：最大坡度，垂直墙跨越高度以及发生倾覆的最大横坡角度；

带有行为语义信息的轨迹确定单元，用于运动规划模块根据三维语义栅格地图、全局拓扑参考路径以及运动基元簇中的运动基元的扩展代价对运动基元簇中的运动基元进行拼接与过渡，确定带有行为语义信息的轨迹；运动基元簇包括：通过采样不同的目标位姿形成的运动基元；运动基元为履带车辆在设定时间内，在履带车辆的运动学约束、平滑过渡约束、平台-地形约束以及平台行为约束下，履带车辆从起始位姿到目标位姿的轨迹；扩展代价包括：运动基元与全局拓扑参考路径的横纵向偏差、碰撞风险代价、平滑代价以及通行度代价；

履带车辆的控制单元，用于运动控制模块利用带有行为语义信息的轨迹对控制目标函数的权重系数进行实时更新，并根据更新后的控制目标函数和带有行为语义信息的轨迹在执行机构的机械约束、平台的运动学约束以及平台动力学约束下，基于模型预测控制算法进行履带车辆的控制；所述控制目标函数根据轨迹跟踪控制的横向偏差、航向偏差以及控制量变化率进行构建。

可选地，所述三维语义栅格地图确定单元具体包括：

激光雷达语义点云确定子单元，用于通过环境感知模块中激光雷达与相机之间的坐标转换关系，将图像语义信息投影至激光雷达点云信息，确定激光雷达语义点云；激光雷达语义点云为融合后的信息；

第一次粗提取子单元，用于根据激光雷达语义点云进行可通行区域的第一次粗提取；

点云特征提取子单元，用于根据第一次粗提取后的激光雷达语义点云信息提取点云特征；点云特征包括：道路的不平度信息、纵坡坡度以及横坡坡度；

第二次粗提取子单元，用于根据点云特征与几何通过性参数进行可通行区域的第二次粗提取；

三维语义栅格地图确定子单元，用于根据第二次粗提取后的激光雷达语义点云信息的点云特征，采用高斯混合聚类方法，确定点云特征对应位置的可通行度；进而根据全局拓扑参考路径，采用八叉树以及贝叶斯核推理，确定三维语义栅格地图。

可选地，所述带有行为语义信息的轨迹确定单元具体包括：

运动基元簇确定子单元，用于根据履带车辆的参考速度以及履带车辆前方可通行区域的通行度确定运动基元簇；

最优扩展的运动基元确定子单元，用于根据运动基元簇中的运动基元的扩展代价，确定最优扩展的运动基元；

带有行为语义信息的轨迹确定子单元，用于将所有的最优扩展的运动基元进行拼接与过渡，确定带有行为语义信息的轨迹。

可选地，还包括：

运动学模型确定单元，用于获取履带车辆的运动学模型；

控制目标函数构建单元，用于根据轨迹跟踪控制的横向偏差、航向偏差以及控制量变化率构建控制目标函数。

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

本发明所提供的一种基于语义信息串联的履带车辆的控制方法及系统，环境感知模块通过相机与激光雷达的融合感知方法，利用图像语义信息与激光雷达点云特征，结合平台通过性几何参数确定三维语义栅格地图；相较于平面二值栅格地图能够提供更多的环境通行度信息。运动规划模块根据三维语义栅格地图、全局拓扑参考路径以及运动基元簇中的运动基元的扩展代价对运动基元簇中的运动基元进行拼接与过渡，确定带有行为语义信息的轨迹；在运动规划中更多考虑平台姿态，平台稳定性的影响，提升了无人系统在复杂越野环境中的通行稳定性。运动控制模块利用带有行为语义信息的轨迹对控制目标函数的权重系数进行实时更新，并根据更新后的控制目标函数和带有行为语义信息的轨迹在执行机构的机械约束、平台的运动学约束以及平台动力学约束下，基于模型预测控制算法进行履带车辆的控制；即通过语义信息串联环境感知模块，运动规划模块，运动控制模块，丰富了无人驾驶系统模块之间的输出信息，提升了地面无人机动平台在越野环境的通行能力。

附图说明

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

图1为本发明所提供的一种基于语义信息串联的履带车辆的控制方法流程示意图；

图2为基于语义信息串联的履带车辆框架示意图；

图3为三维可通行度语义栅格地图；

图4为图像语义信息投影至激光雷达点云信息的效果图；

图5为履带车辆纵向受力简图；

图6为履带车辆横向受力简图；

图7为履带车辆跨越垂直墙过程简图；

图8为车体水平坐标系示意图；

图9为控制参数语义匹配原则示意图；

图10为本发明所提供的一种基于语义信息串联的履带车辆的控制系统结构示意图。

具体实施方式

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

本发明的目的是提供一种基于语义信息串联的履带车辆的控制方法及系统，能够提升地面无人机动平台在越野环境中的通行能力。

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

图1为本发明所提供的一种基于语义信息串联的履带车辆的控制方法流程示意图，图2为基于语义信息串联的履带车辆框架示意图，如图1和图2所示，本发明所提供的一种基于语义信息串联的履带车辆的控制方法，包括：

S101，根据环境感知模块获取的激光雷达数据提取激光雷达点云信息；并根据环境感知模块获取的相机图像提取图像语义信息；激光雷达点云信息包括障碍物、起伏地形以及地表粗糙度；

S102，将激光雷达点云信息和图像语义信息进行融合，并根据融合后的信息、全局拓扑参考路径以及履带车辆的几何通过性参数确定三维语义栅格地图，并如图3所示；所述三维语义栅格地图为带有局部可通行度语义栅格地图；几何通过性参数包括：最大坡度，垂直墙跨越高度以及发生倾覆的最大横坡角度；

首先基于BiSeNet V2网络在数据集上训练来提取图像特征完成输入图像语义分割，在图像分割出的可通行语义的类别中，如土路，草地等。之后基于传感器坐标系转换关系将图像分割结果转化为语义点云。激光雷达与相机的相对位姿可以表示成旋转矩阵

S102具体包括：

通过环境感知模块中激光雷达与相机之间的坐标转换关系，将图像语义信息投影至激光雷达点云信息，确定激光雷达语义点云；激光雷达语义点云为融合后的信息；从点云中筛选了一定距离和高度范围内的点投影到图像并覆盖在原始像素上，投影后点的像素的颜色设置为像素位置对应的类别颜色，并赋予同样的分类信息，如图3所示，左侧为原始点云，右侧分别为原始图像。即不可通行语义类别如车辆、灌木等，直接赋值为不可通行区域，后采用激光雷达语义点云进行可通行区域的粗提取。

根据激光雷达语义点云进行可通行区域的第一次粗提取；

根据第一次粗提取后的激光雷达语义点云信息提取点云特征；点云特征包括：道路的不平度信息、纵坡坡度以及横坡坡度；

根据点云特征与几何通过性参数进行可通行区域的第二次粗提取；

根据第二次粗提取后的激光雷达语义点云信息的点云特征，采用高斯混合聚类方法，确定点云特征对应位置的可通行度；进而根据全局拓扑参考路径，采用八叉树以及贝叶斯核推理，确定三维语义栅格地图。

几何通过性参数确定过程为：

考虑平台（履带车辆）不发生倾覆的纵向稳定性，其极限上坡角

式中，

考虑平台不发生下滑的纵向稳定性，极限下滑角

式中，

因此，平台的纵向上下坡稳定角分别为

另外，不同尺寸大小的平台对纵坡的连续长度应该有不同的考虑，在本文中认为纵坡长度应大于等于履带接地长度。

考虑平台不发生倾覆的横向稳定性，平台横向受力简图如图6所示。图中，

当平台在横坡上开始倾覆时，重力作用线通过左侧履带外侧边缘，由此可得平台的极限横坡角

考虑平台不发生侧滑的横向稳定性，极限下滑角

式中，

因此，平台的横向稳定角为

不同尺寸大小的平台对横坡的连续长度也应该有不同的考虑，在认为横坡长度应大于等于履带中心距。

履带车辆跨越垂直墙时，分为前段履带接触壁缘、前端负重轮升上壁缘、重心前进至与垂直壁缘线重合的位置以及平缓地与垂直壁的顶部平面接触四个阶段，过程简图如图7所示。

对于第一阶段，即前段履带接触壁缘时，若要通过此垂直墙，则有：

式中，

对于第三阶段，即重心前进至与垂直壁缘线重合的位置时，有：

式中，

因此，平台能跨越的垂直墙最高高度为

点云特征的提取过程为：

考虑点云中一查询点

式中，

在不均匀点局部描述符的基础上，进一步提取点云的局部特征向量

通过利用高斯混合聚类方法（GMM）训练出聚类模型，并对输入点云进行在线实时估计，分为三类，即易通行、可通行和难通行。

在点云特征的提取过程中，首先利用平台搭载惯导对点云位姿进行矫正，之后在平台水平坐标系计算所有点云特征，所有环境地形特征全部基于当前时刻平台所处位置进行判断。

点云特征的高斯聚类模型

其中，

其中，领近区域选取平台几何形状占据的区域。将特征向量进行归一化处理，如式（16），利用GMM进行多元聚类，得到易通行、可通行和难通行三类可通行度标签。

在实际在线感知过程中将点云的特征向量输入至GMM模型得到当前点云的对应环境的可通行度。

最后采用八叉树的数据管理地图数据，采用基于贝叶斯核推理的栅格属性推理，对三维栅格的可通行性属性进行建模，从而对点云缺失部分的栅格的属性进行推理，以实现连续的三维建图。

S103，运动规划模块根据三维语义栅格地图、全局拓扑参考路径以及运动基元簇中的运动基元的扩展代价对运动基元簇中的运动基元进行拼接与过渡，确定带有行为语义信息的轨迹；运动基元簇包括：通过采样不同的目标位姿形成的运动基元；运动基元为履带车辆在设定时间内，在履带车辆的运动学约束、平滑过渡约束、平台-地形约束以及平台行为约束下，履带车辆从起始位姿到目标位姿的轨迹；扩展代价包括：运动基元与全局拓扑参考路径的横纵向偏差、碰撞风险代价、平滑代价以及通行度代价；

S103基于最优控制理论的二维运动基元生成与在线基于三维语义栅格地图与全局参考路径的运动基元选择扩展。通过将二维运动基元向三维环境进行投影，最终输出具备三维姿态信息的轨迹

其中，

S103具体包括：

根据履带车辆的参考速度以及履带车辆前方可通行区域的通行度确定运动基元簇；

根据运动基元簇中的运动基元的扩展代价，确定最优扩展的运动基元；

将所有的最优扩展的运动基元进行拼接与过渡，确定带有行为语义信息的轨迹。

S103之前还包括：构建离线的运动基元库，具体的构建过程为：

运动基元的生成考虑履带车辆的基本运动学约束、平滑过渡约束、平台-地形约束、平台行为约束，以生成运动基元尽可能平滑为目标，离线生成用于在线规划时所用的运动基元库。生成运动基元的优化问题定义如式（17）。

其中，

双侧独立驱动无人履带车辆的运动学微分约束

其中，

平台行为约束主要对生成运动基元的起始与目标状态进行限制，常见的行为如类直线行驶，类直角弯转向，类U形弯大角度转向，调头，双移线，上下坡等行为。起始与目标状态的约束如式（19）所示。

其中，

在运动基元生成过程中，由于平台执行机构的机械结构约束，以及平台-地形之间关系，在运动基元生成过程需要满足不等式约束如式（20）所示。

其中，

由于运动规划最终的输出轨迹是由运动基元序列组成，所以在离线生成基元时需要考虑横向和纵向参量的平滑过渡，约束形式如式（21）。

运动基元生成的目标函数从横纵向两个方面考虑了横纵参量的平滑度，离散化的目标函数表示如式（22）。

其中，

本文全局规划采用基于谷歌地球卫星地图制作的环境拓扑地图，在环境拓扑地图的基础上利用A*搜索，完成经过必经点的全局规划，输出具有良好引导效果的全局参考轨迹。

因此被选择的运动基元上的轨迹点需要与参考轨迹匹配点在横向偏差、航向偏差、速度偏差尽可能小，代价计算如式（23）所示。

其中，

碰撞风险代价的计算首先将地面无人平台利用覆盖圆的方法对地面无人平台外形结构进行近似，采用6覆盖圆圆心距离最近障碍物的距离与覆盖圆半径的差值作为运动基元的碰撞风险代价，如式（24）。

运动基元的平滑度评价采用综合曲率和长度的轨迹能量函数，该值越小反应该运动基元的能量越低，轨迹越平滑，该代价值计算如式（25）。

其中，

在环境感知提供的三维语义栅格地图下，运动基元的通行度代价计算包含两个方面，一方面是将运动基元轨迹点匹配栅格附近平台外形结构大小区域的拟合平面法向量与垂直方向的夹角，另一方面是与匹配区域通行度代价的均值。拟合平面法向量计算方法如式（26）。

其中：

其中：

行为基元的最终选择是对多目标进行综合评价，选择代价的计算如式（28）。

其中，

在线基元选择的原则是选择总代价值最小的运动基元，通过选择基元向前扩展，当运动基元的末状态与规划终点区域后完成扩展，形成最终的具有时空信息的三维轨迹。

在选择过程中，相邻运动基元之间的过渡基元为变速直线运动，采用三次Hermite插值完成过渡段中速度的生成。

S104，运动控制模块利用带有行为语义信息的轨迹对控制目标函数的权重系数进行实时更新，并根据更新后的控制目标函数和带有行为语义信息的轨迹在执行机构的机械约束、平台的运动学约束以及平台动力学约束下，基于模型预测控制算法进行履带车辆的控制；所述控制目标函数根据轨迹跟踪控制的横向偏差、航向偏差以及控制量变化率进行构建。

S104之前还包括：

获取履带车辆的运动学模型；

采用基于转速控制的方法对进行轨迹跟踪控制，控制量为左右侧履带的速度，即

其中，

根据轨迹跟踪控制的横向偏差、航向偏差以及控制量变化率构建控制目标函数。运动基元组成的轨迹包括道路的坡度、路面不平度、曲率特征等语义信息，以上信息对运动控制模型约束以及目标函数设计上提供更多的参考信息。在进行模型预测轨迹跟踪控制模型的建立过程中，主要考虑执行机构的机械约束、平台的运动学约束以及平台动力学约束。根据道路类型、曲率特征、平台速度等信息，对电机外特性以及履带车辆坡道转向的动力学分析，增加关于驱动系统后备功率的约束，保证极限工况下履带车辆的轨迹跟踪的稳定性，当平台在坡道上进行低速匀速转向时，结合参考轨迹上平台姿态与规划速度，对车辆在坡道上跟踪行驶时所需的功率和力矩进行预测，结合电机外特性的边界值，对车辆的控制量进行约束，使得平台可以在坡道上进行稳定的轨迹跟踪。最终建立如式（30）的约束不等式。

其中，

基于模型预测控制方法的运动控制模块根据运动规划模块提供的由行为运动基元组成的具有语义分类的参考轨迹，采用多组控制目标函数权重系数对不同语义分类轨迹进行跟踪。

根据控制目标函数如式（31）中所考虑的轨迹跟踪控制的横向偏差、航向偏差以及控制量变化率为指标反应的轨迹跟踪稳定性等构建最优轨迹跟踪控制精度的评价体系，从而得出不同控制目标函数权重系数以及不同的系数比例对最终轨迹跟踪控制效果的影响规律，建立不同语义轨迹下的控制策略数据集。

其中，

通过试验数据积累和分析，在平台能够进行稳定轨迹跟踪控制的可行域内，平台的轨迹跟踪效果主要受到横向跟踪权重

如图9所示，平台在跟踪具有语义分类的参考轨迹时，在不同语义属性的轨迹上采用不同的控制参数，提升轨迹跟踪的精度与稳定性。

图10为本发明所提供的一种基于语义信息串联的履带车辆的控制系统结构示意图，如图10所示，本发明所提供的一种基于语义信息串联的履带车辆的控制系统，包括：

信息获取单元1001，用于根据环境感知模块获取的激光雷达数据提取激光雷达点云信息；并根据环境感知模块获取的相机图像提取图像语义信息；

三维语义栅格地图确定单元1002，用于将激光雷达点云信息和图像语义信息进行融合，并根据融合后的信息、全局拓扑参考路径以及履带车辆的几何通过性参数确定三维语义栅格地图；所述三维语义栅格地图为带有局部可通行度语义栅格地图；几何通过性参数包括：最大坡度，垂直墙跨越高度以及发生倾覆的最大横坡角度；

带有行为语义信息的轨迹确定单元1003，用于运动规划模块根据三维语义栅格地图、全局拓扑参考路径以及运动基元簇中的运动基元的扩展代价对运动基元簇中的运动基元进行拼接与过渡，确定带有行为语义信息的轨迹；运动基元簇包括：通过采样不同的目标位姿形成的运动基元；运动基元为履带车辆在设定时间内，在履带车辆的运动学约束、平滑过渡约束、平台-地形约束以及平台行为约束下，履带车辆从起始位姿到目标位姿的轨迹；扩展代价包括：运动基元与全局拓扑参考路径的横纵向偏差、碰撞风险代价、平滑代价以及通行度代价；

履带车辆的控制单元1004，用于运动控制模块利用带有行为语义信息的轨迹对控制目标函数的权重系数进行实时更新，并根据更新后的控制目标函数和带有行为语义信息的轨迹在执行机构的机械约束、平台的运动学约束以及平台动力学约束下，基于模型预测控制算法进行履带车辆的控制；所述控制目标函数根据轨迹跟踪控制的横向偏差、航向偏差以及控制量变化率进行构建。

所述三维语义栅格地图确定单元1002具体包括：

激光雷达语义点云确定子单元，用于通过环境感知模块中激光雷达与相机之间的坐标转换关系，将图像语义信息投影至激光雷达点云信息，确定激光雷达语义点云；激光雷达语义点云为融合后的信息；

第一次粗提取子单元，用于根据激光雷达语义点云进行可通行区域的第一次粗提取；

点云特征提取子单元，用于根据第一次粗提取后的激光雷达语义点云信息提取点云特征；点云特征包括：道路的不平度信息、纵坡坡度以及横坡坡度；

第二次粗提取子单元，用于根据点云特征与几何通过性参数进行可通行区域的第二次粗提取；

三维语义栅格地图确定子单元，用于根据第二次粗提取后的激光雷达语义点云信息的点云特征，采用高斯混合聚类方法，确定点云特征对应位置的可通行度；进而根据全局拓扑参考路径，采用八叉树以及贝叶斯核推理，确定三维语义栅格地图。

所述带有行为语义信息的轨迹确定单元1003具体包括：

运动基元簇确定子单元，用于根据履带车辆的参考速度以及履带车辆前方可通行区域的通行度确定运动基元簇；

最优扩展的运动基元确定子单元，用于根据运动基元簇中的运动基元的扩展代价，确定最优扩展的运动基元；

带有行为语义信息的轨迹确定子单元，用于将所有的最优扩展的运动基元进行拼接与过渡，确定带有行为语义信息的轨迹。

本发明所提供的一种基于语义信息串联的履带车辆的控制系统，还包括：

运动学模型确定单元，用于获取履带车辆的运动学模型；

控制目标函数构建单元，用于根据轨迹跟踪控制的横向偏差、航向偏差以及控制量变化率构建控制目标函数。

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

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