基于线性误差模型研究铲运机的路径跟踪与控制

沈琦

（201620 上海市 上海工程技术大学 机械与汽车工程学院）

0 引言

随着采矿深度的增加，采矿条件也愈加恶劣,对人员安全的威胁程度也逐渐提高，遥控采矿、自动化矿山开采技术应运而生[1-2]。地下铲运机是地下自动采矿的关键设备，路径跟踪是地下铲运机实现自动化作业的基本任务之一，路径跟踪与控制的目的是使车辆以尽可能小的误差跟踪期望路径，而地下空间多为狭窄受限的巷道空间，因此铲运机的路径跟踪与轨迹特性相较于普通车辆难度更大。

本文运用 MATLAB 编写工作装置运动仿真程序，通过程序仿真分析，得到工作装置的轨迹、速度运动学特性曲线，为提高工作装置设计精度和设计效率提供了重要的分析数据，也为工作装置的优化设计奠定了基础。

1 铲运机运动学分析

1.1 转向特点

铲运机是一种采用铰接连接的工程车辆，采用“折腰式”设计[3]，前后车体作为相对独立的部分存在，通过销轴连接，这种连接方式可保证前后车体在同一水平面内转动，从而完成铲运机的转向，同时车体间液压缸的伸缩亦可达到转向的目的。

1.2 转向半径分析

铲运机属于铰接式转向车辆，通过转向油缸的伸缩来改变铰接角，进而使铲运机转向。其转向角度取决于液压缸伸缩长度，当两油缸伸缩量相等时，铲运机沿直线行驶，转向半径是铲运机行驶的重要参数。如图1 所示，前后车桥两轴线相交于C点，该点即为铲运机转向速度瞬心，铲运机围绕该点做无侧滑的转向运动。

图1 中，O 点——铲运机的铰接点，即连接前后车体的销轴所在点；A 点，B点——前后车桥中点；α——转向角，与∠C 大小相同，铲运机为直线行驶时α=0°；lf，lr——前后车桥中心到铰接点的距离；Rf，Rr——转向半径，计算公式如式（1）、式（2）。

同理可得Rr：

2 铲运机模型建立

2.1 模型推导

以前车体作为分析中心，该点速度与铲运机前进方向一致，有利于后续分析与计算。参考Corke等人提出的经典铲运机的运动学模型[4-5]。由于铲运机通常在低速情况下行驶，所以此处可以忽略加速和制动及其它偏离确切运动学模型的影响，将前车体行驶速度和铰接角转向速度为输入量的情况下，计算前车体航向角的角速度表达式。

由刚体的平面运动学[6]可知，刚体中的任意一点速度可通过基点分解成平动和转动，其中平动速度与所选基点（牵连点）速度不同，而平面绕基点转动的角速度、角加速度与基点的选取无关。由此可知，与前车体角速度一致，与后车体角速度一致。由于在O 点处两刚体具有相同速度，分别选取基点A 与基点B 表达O 点的速度，根据刚体平面运动学中基点法可得：

如图2 所示，θf为前车体航向角，铲运机逆时针转向时，航向角角速度为正，铰接角α亦为正。图中所处航向角与铰接角α均为正。将4 个速度向量分别沿平行于前车体车桥方向与垂直于前车体车桥方向进行分解，可以建立等式如式（4）。

其中，与顺时针转动时，航向角为负且不断减小，铰接角变大的同时，前车体航向角的变化率更大，同时求解

此处，将vA和vB改写成vf和vr。假设铲运机处于无侧滑情况下，故有

2.2 模型验证

借鉴Altafini 和Corke 的文献[7-8]验证运动学方程的准确性。由于处于无侧滑情况下，可列方程：

得到关于的表达式为

由此通过2 种方法证明所得运动学模型方程是正确的。同时还可确定铲运机运行时的速度瞬心：

当铰接角转向速度=0 时，铲运机处于稳态转向状态时，铲运机围绕前后车桥的交叉点旋转。速度瞬心表达式为：

通过比较可发现前后车桥交叉点与速度瞬心重合，即当=0 时，铲运机的行驶曲率圆心位于前后车桥的交叉点处，再一次验证了运动学公式的准确性。

铲运机运动学模型状态空间表达式为

3 LEMPC 路径跟踪控制

基于铲运机的运动学方程设计了LEMPC 控制预测模型，MPC 控制具备以下特点：（1）预瞄功能，根据模型预测未来的状态及参考状态的偏差；（2）可处理MIMO 状况；（3）约束功能，对控制量、输出量、控制增量等进行约束，提高系统稳定性；（4）实时更新功能，每个时刻得到最优输入。

MPC 控制已广泛应用于移动机器人及自动驾驶中，而铲运机由于结构特殊，关于路径跟踪的研究还较少。

图3 为模型预测控制的计算流程。

对参考轨迹的当前参考点进行泰勒展开及雅克比线性化（Jacobian Linearization）：

式（15）、式（17）中下角标ref 代表参考点（ωref即铰接角的角速度）

处理得新的状态空间表达式为

输出量即为状态变量，输出方程为

列出预测方程，定义预测时域为Np，控制时域为Nc，且Nc≤Np。做如下假设：在控制域之外，输入为0，即u（k+i）=0，i=Nc，Nc+1，Nc+2，…，Np-1，预测矩阵表达为

为使铲运机快速平稳地行驶在预定轨迹上，将二次型的标准形式作为目标函数来求解输入，取[ΔUTε]T作为二次型的求解对象，目标函数矩阵形式表示为

式中：Q，R——输出量的调整矩阵；ε——松弛因子。Ψξ（k）对应当前时刻k 为已知值，将其定义为e，eT，QQ，e——常数。

由于铲运机的物理条件限制，还需要对输入速度、铰接角速度及铰接角大小进行约束：

本文使用MATLAB 嵌入的quadprog 函数对目标函数求解，目标函数输入量为[ΔUTε]T。控制增量Δu=ΔU+Δuref。

则约束表达为

只存在铰接角一个输出量的表达式为

铰接角约束为

公式中二次型目标函数的约束Ax≤b。

基于铲运机误差模型的预测控制已搭建完成，根据目标函数求解得到每个周期的[ΔUTε]T，取其中的第一项，则需要的输入为

4 仿真验证

运用MATLAB/Simulink 2021a 软件设计仿真环境[9]。仿真系统中设计了铲运机的运动学模型及MPC 控制器，如图4 所示。在实际控制中，车速和铰接角角速度的变化有时滞存在，因此在MPC控制器输出端添置单位延迟环节，采用积极集法求解函数quadprog，期望路径为一条直线。

控制器中各参数设定见表1。

表1 控制器参数Tab.1 Controller parameters

控制器权重参数为

图5 为仿真结果。

可以看出，铲运机铲装路线与参考路径基本吻合，残差很小，对于地下铲运机的控制而言，误差可以接受。

5 结语

本文通过参考Corke 的经典运动学模型,推导出铲运机运动学模型表达式，运用 MATLAB 进行运动仿真分析，为提高工作装置设计精度和设计效率提供了重要的分析数据。以地下铲运机为例建立的跟踪轨迹推算数学模型也同样适用于其他铰接式车辆。该跟踪轨迹推算模型为地下铲运机导航控制器的设计提供了参考。