基于无人驾驶车辆轨迹跟踪的模型预测控制（MPC）

羽哥

分类：建模仿真

发布时间 2022.02.15阅读数 5343 评论数 0

1. 车辆建模

1.1 运动学模型

车体运动学建模假设了车体不存在打滑的情况,也就是假设了滑移角 $\theta_{vf}$ 和 $\theta_{vr}$ 为0，主要解决的问题是车体坐标系和车轮转角之间的关系。

可以将车体假设为单车模型，如上图所示。车体坐标系的原点假设为M点，也就是车体的质心上。我们需要解决的问题是如何驱动轮速度 $V_{d}$ 和转向轮角度 $\delta_{f}$ 转换为车体坐标系M下线速度 $v_{x},v_{y}$ 和角速度 $\tilde{\varphi}$ ，这也就是所谓的正运动学问题，同理逆运动学问题就是将车体坐标系M下线速度 $v_{x},v_{y}$ 和角速度 $\tilde{\varphi}$ 转换为驱动轮速度v和转向轮角度 $\delta_{f}$ 。现假设后轮驱动前轮转向，则运动学模型如下：

1.2 里程计模型

里程计模型根据编码器的反馈位置，可以分为三种形式，如果想对编码器数据进行充分利用，也可以进行数据的滤波融合。现根据使用的编码器数据分别给出三种里程计模型公式：

1.3 轮胎模型

如下图所示分别代表了前轮和后轮在车体坐标系下的状态，图中向量V代表了前轮和后轮的运动方向,图中的坐标系和车体坐标系平行， $\theta_{vf}$ 和 $\theta_{vr}$ 分别代表前轮和后轮运动方向与车体坐标轴方向的夹角, $F_{ry}$ 和 $F_{fy}$ 分别代表摩擦力产生的y轴方向力。

1.4 动力学模型

车体动力学模型主要研究的是车体在加速度产生的惯性力和车轮打滑产生的摩擦力的影响下车体速度的计算。首先搬公式：

根据牛顿定律：

横向加速度：

摩擦力：

$l_{f}\tilde{\varphi}$ 为前轮绕重心自转产生的线速度：

$l_{r}\tilde{\varphi}$ 为后轮绕重心自转产生的线速度：

由于摩擦角很小，采用近似计算：

将（3），（4），（5），（8），（9）代入到（1）得到：

将（4），（5），（8），（9）代入到（2）得到：

故合并（10），（11）得到动力方程为：

2. 车辆方向盘控制误差模型

车体方向盘控制误差模型解决的是，在速度恒定的情况，如何控制转向角 $\delta$ ，使车体按期望的角速度旋转，从而保证车体横向的控制误差最小。假设期望的角速度为 $\tilde{\varphi}_{des}$ ，则期望的横向加速度为 $a_{ydes}$ 。

横向位移加速度误差为：

车体的旋转角度误差为：

横向位移速度误差：

航向角速度误差：

航向角加速度误差：

车体旋转角加速度误差：

最终的控制误差状态方程为：

3. 车辆运动控制误差状态转移方程

无人驾驶车辆运动控制状态转移方程，综合考虑横纵向控制误差，在前面(22)式中，描述了车辆横向控制的状态转移方程，为了综合考虑纵向误差，现假设纵向位移误差为 $es$ ，速度误差为 $ev$ ，油门所控制的加速度为 $a$ ，期望的航向角速度 $\tilde{\varphi}_{des}$ 用 $\tilde{\varphi}$ 表示则有: