路径跟踪，轨迹跟踪的算法有基于运动学和基于动力学的，其中LQR算法基于车辆动力学网上有很多，毕竟百度开源了apollo，该算法通过简单改进后的实车效果还是非常棒的，之前通过dSPACE MicroAutoBox做了试验，横向误差都是厘米级。由于动力学模型需要估计车辆重量，轮胎侧偏刚度，还有绕Z轴转动惯量，需要估计的参数非常多，相对而言，运动学则简单了很多，不需要估计很多参数，且听某FSAC车队的大佬说，运动学模型非常稳定，于是我动手用Simulink搭了一下，跟各位分享一下建模过程，因为在网上找的基本都是MPC，感觉不用MPC都不好意思出门了，就是找不到基于运动学的LQR算法，所以动手实践下，搭起来也非常简单，并且年后找个机会实车测试一下。

运动学模型很多博客都提到过，这里分享个视频，如果不清楚运动学模型推导的可以仔细跟着做一遍。 【控制】模型预测控制 MPC 【合集】Model Predictive Control 这里，我就通过图片的方式直接呈现运动学模型，该运动学模型需要线性化和离散化，因为实际系统都是离散的，所以根据需要的采样时间进行离散化即可。

其中，Vr是车辆后轴速度，φ是车辆横摆角，δf是车辆前轮转角，l是轴距。其中基于运动学几何关系的前轮转角与转弯半径关系非常重要，通过下式就可以算出期望的前轮转角。

然后把上面方程线性化，离散化后，直接得出如下方程：

将模型变为了偏差的方程，分别是状态变量和控制变量都是偏差值：实际值减去期望的偏差值。那么可以发现，系统矩阵和控制矩阵都是时变的，因此必须在每个控制时刻都进行黎卡提方程的求解得到控制量，注意，此刻的控制量是变化量，必须加上期望值，才是最后的控制量，输入给车辆。这里和动力学模型是大不同的。 期望速度很好理解，提前给定就行。前轮转角则是通过车辆轴距和当前车辆与路径最近点的曲率值乘积的反正切值。有了上述模型，则可以进行建模与仿真。

这里给各位分享一下，我是如何搭建这个模型的，并且给出整个控制算法的代码，该代码是可以进行Code Generation的。下面就是仿真模型，该模型分为三部分。第一部分为控制算法，第二部分为车辆模型，第三部分是可视化界面，这部分可有可无。Matlab版本：R2019b。

先看一下车辆模型，为了方便用的车辆动力学工具箱的3dof 车辆模型搭建。 模型输入：前轮转角(rad); 车辆纵向速度(m/s) 模型输出：车辆后轴位置横坐标x(m);车辆后轴位置横坐标 y(m);车辆横摆角(rad)。 其余输出为可视化界面使用，有纵向速度vx,侧向速度vy，以及横摆角速度φdot。 rate trasition模块是因为整个模型使用变步长求解，因为定步长求解这个车辆模型很慢。但是控制算法是原子子系统，10ms运行一次。因为运动学离散时间为10ms。memory模块是为了避免代数环。 控制算法部分如图所示，控制算法通过m函数编写,将该子系统设置为原子子系统采样时间10ms。

该控制算法的输入有车辆当前后轴横坐标x，纵坐标y以及横摆角φ。车辆期望的路径通过离散点的形式存入Basic/Model workspace并通过Constant模块给出，包括路径的横纵坐标，每个坐标的航向角以及每个坐标的曲率。车辆期望速度通过Constant模块给出。 模型输出控制车速以及前轮转角(rad)。该车速是期望车速，按理说应该通过纵向控制输出加速度，后反算成转矩给车辆发动机或者电机。这里省略纵向控制直接将车速给到车辆模型。 下面直接给出M函数的代码。如果对代码有疑问的同学，可以私聊我进行讨论。