基于运动学模型的履带无人车辆轨迹跟踪控制器设计方法与流程

本发明涉及车辆控制技术领域，具体涉及一种基于运动学模型的轨迹跟踪控制器的设计方法。

背景技术：

目前国内外研究学者对履带式自主平台的轨迹跟踪控制进行了大量研究，但存在以下几点不足：

1.研究对象都是低速运动的小型履带式移动机器人，仿真车速不超过2m/s。并且仿真时给定的轨迹过于单一，普遍是跟踪一段直线或者圆形轨迹。因此控制算法在车辆高速运动条件下跟踪复杂轨迹时的准确性有待进一步验证。

2.控制算法主要基于理想的车辆运动学模型进行构建，没有考虑到打滑对车辆运动造成的影响，在复杂道路条件下跟踪精度会有所降低。

3.仿真结果没有体现出跟踪过程中车辆速度和加速度的变化情况，评价指标只有跟踪轨迹的精确度，缺乏对车辆行驶稳定性和跟踪速度的考虑。

由于模型预测控制算法能够有效结合车辆的约束条件对目标函数进行反复的在线实时优化，并能根据系统实际状态与预测控制之间的误差对预测值进行实时修正，能够有效克服系统不确定性产生的扰动，消除模型误差，非常适合于求解不能精确建立数学模型且存在约束条件的控制系统。因而基于模型预测控制算法来设计履带无人车辆的轨迹跟踪控制器，可使无人车辆在高速运动和非结构化道路环境下能够准确、稳定的跟踪参考轨迹。

技术实现要素：

有鉴于此，本发明提供了一种基于运动学模型的履带无人车辆轨迹跟踪控制器设计方法，该方法建立的控制器对连续和离散点路径都具有良好的跟踪能力，特别是在低速跟踪过程中，行驶速度稳定在期望速度附近，直线跟踪稳态误差趋近于0，行驶轨迹平顺。

一种基于运动学模型的履带无人车辆轨迹跟踪控制器设计方法，该方法的实现步骤如下：

第一步：根据考虑滑移的履带车辆运动学模型建立状态空间方程；

第二步：根据设定的目标函数进行标准二次型转化；

第三步：在下一个控制周期t内系统通过目标函数的优化过程计算出新的控制序列，反复进行滚动优化，从而实现轨迹跟踪控制。

进一步地，所述第一步中建立状态空间方程的过程如下：

考虑滑移的履带车辆运动学模型如式(1.1)所示

其中，

将其表示为如下的非线性模型：

其中，μ(t)＝[ωl,ωr]^t；

将式(1.3)在工作点(ξr,μr)处进行一阶近似泰勒展开线性系统，得

将工作点(ξr,μr)代入模型(1.3)，式(1.4)减式(1.3)，得线性化误差方程

其中，

应用一阶差商的方法将式(1.5)线性时变系统进行离散化，得到

其中，

其中，t为采样时间，r为主动轮半径，b为履带中心距，il为左侧履带滑移率，ir为右侧履带滑移率，α为车辆打滑角；

选取系统输出量为：

设定则式(1.6)所示的状态空间方程可以表示为：

其中，各矩阵表示如下：

其中，ak,t由式(1.7)可以得到，bk,t由式(1.8)可以得到；

为了进一步降低模型的计算复杂度，需要做出以下定义：

进一步地，所述第二步中设定的目标函数为

目标函数转化为如下标准二次型。

其中，已知状态量个数n＝3，控制量个数m＝2，输出量个数p＝3，设定预测时域hp＝135，控制时域hc＝35，控制周期t＝0.1s，可以得到；

对于目标函数的优化求解需要符合车辆运动学和动力学约束条件，为了发挥履带车辆的运动性能并能保证车辆的稳定性，设定如下约束条件：

(a)两侧履带转速约束

umin-u(t-1)≤mδu(t)≤umax-u(t-1)(1.20)

其中，u(t-1)为前一时刻车辆反馈的两侧履带转速；

(b)转速增量约束

δumin≤δu(t)≤δumax(1.21)

其中，

(c)纵向加速度约束

a1δu≤b1(1.22)

其中，

(d)侧向加速度约束

其中，v为上一时刻测得的车辆纵向速度，ωl(t-1),ωr(t-1)分别为上一时刻测得两侧履带转速；

根据以上建立的目标优化函数和设定的约束条件，将线性时变预测控制问题转化为如下的标准二次规划问题；

在每个控制周期t内对式(1.25)表示的标准二次型进行优化求解，得到一个控制增量的序列：取序列中的第一个元素作为控制输入量，即：

有益效果：

本发明的控制器在高速和低速条件下对连续和离散点路径都具有良好的跟踪能力，特别是在低速跟踪过程中，行驶速度稳定在期望速度附近，直线跟踪稳态误差趋近于0，行驶轨迹平顺；此外，纵向和侧向加速度以及横摆加速度和角速度都能限定在约束范围，车辆具有良好的行驶稳定性，跟踪效果能够满足车辆自主行驶的要求。

附图说明

图1为本发明实现的步骤流程图；

图2为参考轨迹和仿真轨迹变化曲线图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

如附图1所示，本发明提供了一种基于运动学模型的履带无人车辆轨迹跟踪控制器设计方法，该方法的实现步骤如下：

第一步：根据考虑滑移的履带车辆运动学模型建立状态空间方程；

考虑滑移的履带车辆运动学模型如式(1.1)所示。

其中，

将其表示为如下的非线性模型：

其中，

将式(1.3)在工作点(ξr,μr)处进行一阶近似泰勒展开线性系统，得

将工作点(ξr,μr)代入模型(1.3)，式(1.4)减式(1.3)，得线性化误差方程

其中，

应用一阶差商的方法将式(1.5)线性时变系统进行离散化，得到

其中，

其中，t为采样时间，r为主动轮半径，b为履带中心距，il为左侧履带滑移率，ir为右侧履带滑移率，α为车辆打滑角；

选取系统输出量为：

设定则式(1.6)所示的状态空间方程可以表示为：

其中，各矩阵表示如下：

其中，ak,t由式(1.7)可以得到，bk,t由式(1.8)可以得到；

为了进一步降低模型的计算复杂度，需要做出以下定义：

第二步：根据设定的目标函数进行标准二次型转化；

目标函数转化为如下标准二次型。

其中，已知状态量个数n＝3，控制量个数m＝2，输出量个数p＝3，设定预测时域hp＝135，控制时域hc＝35，控制周期t＝0.1s，可以得到；

对于目标函数的优化求解需要符合车辆运动学和动力学约束条件，为了发挥履带车辆的运动性能并能保证车辆的稳定性，设定如下约束条件：

(a)两侧履带转速约束

umin-u(t-1)≤mδu(t)≤umax-u(t-1)(1.20)

其中，u(t-1)为前一时刻车辆反馈的两侧履带转速；

(b)转速增量约束

δumin≤δu(t)≤δumax(1.21)

其中，

(c)纵向加速度约束

a1δu≤b1(1.22)

其中，

(d)侧向加速度约束

其中，v为上一时刻测得的车辆纵向速度，ωl(t-1),ωr(t-1)分别为上一时刻测得两侧履带转速；

根据以上建立的目标优化函数和设定的约束条件，将线性时变预测控制问题转化为如下的标准二次规划问题；

在每个控制周期t内对式(1.25)表示的标准二次型进行优化求解，得到一个控制增量的序列：取序列中的第一个元素作为控制输入量，即：

第三步：在下一个控制周期t内系统通过目标函数的优化过程计算出新的控制序列，反复进行滚动优化，从而实现轨迹跟踪控制。

为了验证轨迹跟踪控制器在高速和复杂路径条件下的轨迹跟踪能力，设计了高速和低速条件下复杂连续曲线和离散点的跟踪仿真实验。无人车辆相关结构参数如表1所示

表1履带无人车辆结构参数

其中m为整车质量，l为履带接地长，b为履带中心距，b为履带板宽度，iz为车辆绕z轴的转动惯量，f为滚动阻力系数，μ为履带与地面之间的摩擦系数，k为履带与地面之间的剪切模量，r为主动轮半径，cx、cy分别为质心与车辆局部坐标系的横向和纵向距离。

其中，航向角的定义为：x轴正方向为0°，y轴正方向为90°，逆时针方向依次增加，取值范围为[0°，360°)。

进行2m/s速度下连续曲线的轨迹跟踪仿真：仿真车速设置为2m/s，仿真时间为90s，采用四阶龙格库塔算法，仿真步长设置为0.1s。在0-20s内跟踪直线，20s-50s内跟踪圆形，50s-100s跟踪双移线，得到的仿真结果：

如附图2所示，控制器具有良好的路径覆盖程度，在直线跟踪时，y方向的误差趋近于0。跟踪圆形轨迹和双移线轨迹的误差较大，原因在于车辆前进方向不断改变，车辆难以预测未来一段时间的状态变化，这可以通过增加预测时域hp或减小仿真步长t来提高跟踪精度，但相应的会增加计算时间，降低控制系统的实时性，预测时域和仿真步长需要进行多次仿真实验方可确定。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。