基于车辆运动学模型的路径跟踪仿真及程序

基于车辆运动学模型设计车辆横纵向运动控制器，控制车辆从当前位置自动行驶到指定位置，属于点对点的自主导航控制系统，比如控制自主车辆从当前点泊入指定的车位上。通过本文，了解简单的车辆运动学建模方法和坐标系变换方法，能够利用simulink工具和雅克比矩阵将非线性模型线性化，并能设计基于线性化模型的二次型最优控制器，最后实现车辆运动学模型的闭环仿真与分析。

运动学模型

图1 车辆运动学模型

则车辆运动学模型为：

其中，u1和u2分别是车辆的纵向和横向控制输入。

图2 坐标系变换

根据(1)和(2)可得车辆在相对坐标系下的运动学模型为：

因此，该车辆运动学模型是一个多输入的非线性模型，其两个输入分别为车辆的纵向（油门/刹车）和横向（方向盘）控制输入。

横纵向解耦控制

由车辆运动学模型(3)可知，该系统是一个横纵向耦合的非线性系统，设计控制系统的难度较大，因此可将横纵向系统解耦，分别设计各自子系统的控制器。

1 纵向控制器

针对纵向控制系统，根据(3)可得解耦后的车辆纵向运动学模型为：

车辆纵向控制系统的目的是车辆的纵向行驶速度跟踪，车辆初始速度为0，到达指定位置时的速度也为0，当开始行驶时的目标速度为Vset，当车辆即将到达目标位置时其目标速度为0。车辆纵向运动控制系统是一个简单的单输入单输出系统，可以采用如下所示的PID控制策略实现车辆速度跟踪控制：

其中，Kp、Ki、Kd分别为误差的比例系数、积分系数、微分系数，速度跟踪误差为

其中，目标参考速度为

其中，d0为判断车辆是否接近目标位置的距离阈值，d为车辆当前位置到目标位置的距离，其计算公式为

在车辆运动过程中，可将Vref视作常数，则速度跟踪误差的导数为

令跟踪误差的积分q为：

则根据(5)可得控制输入为

根据(9)和(10)可得增广的闭环系统为

其状态空间模型为

显然，该系统是一个二阶的双积分系统，其特征方程为

其中，λ为系统的特征根。当PID控制器(5)中的系数满足如下条件时，纵向速度跟踪系统是渐进稳定的，可实现车辆期望行驶速度的稳定跟踪。

2 横向控制器

针对横向控制系统，其目的是车辆的横向运动轨迹跟踪，根据(3)可得解耦后的车辆横向运动学模型为：

将该模型线性化后得：

其中，车辆横向状态z和系统矩阵A、B分别为

利用线性二次型调节器（LQR）方法实现系统(17)的状态反馈控制，选取最优控制的二次型性能指标为

要求寻找最优控制u2(t)使得性能指标J最小。其中，假设输入u2(t)无约束，Q为对称半正定矩阵，R为对称正定矩阵。

设控制器增益为K，则横向反馈控制策略为：

根据连续系统无限时间调节器的变分求解结果可得，控制器增益为

其中，P是如下Riccati方程的解：

利用Lyapunov稳定性理论分析LQR控制系统的稳定性。针对线性化的模型(17)，其闭环系统为

定义二次型Lyapunov函数

对Lyapunov函数求导可得

将系统状态方程(23)代入上式可得

将黎卡提方程(22)代入上式可得

黎卡提方程(22)的求解问题已经是一个较成熟的研究，且有诸多计算程序可以借鉴，接下来介绍三种最常用的方法。

方法一：简单迭代算法。

参考离散系统的无限时间状态调节器的递推黎卡提方程，方程(22)的求解等价于如下递归方程的求解，因此，当给定初值P0=Q后，则可以利用如下的迭代公式求解黎卡提方程(14)：

利用上述迭代法求解黎卡提方程和最优控制算法可以用MATLAB来实现，代码如下：

方法二：利用lqr函数。

MATLAB控制系统工具箱中提供了求解黎卡提代数方程的函数lqr，调用的格式为

［K,P,E] =lqr(A, B, Q, R)

其中，输入矩阵为A、B、Q、R，返回矩阵K为状态反馈矩阵，P为黎卡提代数方程的解，E为闭环系统的零极点。

方法三：利用care函数。

也可采用MATLAB控制系统工具箱中care函数求解黎卡提代数方程，其调用方法如下：

［P,E,K]= care(A,B,Q,R,zeros(size(B)),eye(size(A)))

其中，采用care函数的优点在于可设置P的终值条件，而采用lqr函数不能设置黎卡提代数方程的边界条件。

采用MATLAB工具箱中的lqr和care函数可以求解线性时不变（LTI）系统的状态反馈控制增益，但是针对线性时变系统或线性变参（LPV）模型的控制器实时求解时，方法一中的迭代法是常用的解决方法。

闭环系统仿真

（1）simulink仿真模型搭建

首先在simulink环境下搭建车辆运动学模型以及坐标系变换模型，如下图所示。

图3  车辆运动学模型

图4 坐标系变换模型

然后搭建整个闭环仿真模型，如下图所示，包括横纵向解耦控制器，其中，纵向运动采用PID控制，横向运动采用LQR控制。

图5  车辆运动学闭环仿真模型

（2）横向运动学系统非线性模型的线性化

线性化模型(17)可根据求雅克比矩阵求得，也可以根据所建立的simulink仿真模型求得。在simulink仿真模型中先对非线性模型的输入和输出分别添加open-loop input和open-loop output节点，如图5所示，然后利用simulink的linear analysis工具可直接求得线性化之后的系统模型，或者利用命令行函数求解simulink非线性仿真模型的线性化模型，所用函数命令如下所示。

model='unicyclemodelcontrol';sys=linearize(model,getlinio(model))

其中，unicyclemodelcontrol是在simulink环境下所搭建的非线性仿真模型。

（3）控制器求解

MATLAB软件提供了lqr函数可以非常方便的求解二次型最优控制问题。设定车辆初始位置为(0,0)，初始航向角为0，初始速度为5m/s，则针对线性化的系统模型求解LQR控制器，求解程序如下。

A=[0,5;0,0];B=[0;1];Q=10*eye(2);R=1;K=lqr(A,B,Q,R)

求得控制器增益矩阵为K=[3.1623,6.4516]。

（4）系统仿真

设定目标位置为(100,5)，目标航向角为0.01，仿真结果如下图所示。由图可见，所设计的横纵向控制器可以控制车辆行驶至指定的位置并保持目标航向角。

图6  车辆行驶轨迹

图7 车辆行驶航向角

附件：可点击如下链接获取本文中的仿真模型（包括固定点跟踪和可变轨迹跟踪两种仿真场景）

​​项目2程序.rar​​  

文章转载自公众号：智能运载装备研究所