mpc控制器推导

运动模型建模

考虑如下车辆运动模型：

该运动模型推导参见[[多车辆运动学公式推导]]。其中 为车辆速度(矢量，未必为车头方向)， 为车辆纵向加速度， 为转弯的角速度， 为前轮转角， 为车辆航向角。 上式中选取后轮轮轴中心，作为车辆坐标原点。 若以车辆重心为坐标原点，则其运动方程如下：

其中 为重心处的速度与车辆纵轴的的夹角, 为汽车重心到后轮的距离， 为汽车重心到前轮的距离。

运动模型线性化

定义系统状态 , 控制向量为 。 则上面运动方程可以写为：

运动状态方程离散化

采用前向欧拉法对上述线性方程进行离散化。

问题一：是否意味着输入的trajectory上的点也是时间均匀的

solution 1: 将路径点上的速度设置为相同（比如1m/s）

solution 2: 在路径点上带上时间点信息(这样就知道dt了)

问题二： 线性化传递函数离线性化点越远，则偏差越大此时A矩阵是否为变值

优化目标函数推导

形式一(Condensed Format)

定义未来p个周期(预测步长)内预测的系统状态为：

定义到达未来p个周期内预测的系统输入为：

则由上述离散状态转移方程可以写出未来p个状态的状态转移方程：

将上式写为矩阵形式：

定义预定的控制目标(步长为p)为：

定义优化目标代价函数为：

将上面的 的状态转移方程带入上式，整理得以下二次型：

其中：

上式可调用线性求解器进行求解析解，对 可以添加输入限制。

TODO: 这种类型的实现目前还不多，可以尝试自己封装一个库。

形式二(Non-Condensed Format), 构造法

参考

• MPC cast to osqp
• osqp求解器 该形式直接利用 进行构造，容易实现。下面简要描述构造过程： OSQP求解器求解的是一类QPs(quadratic programs)问题,其形式如下：

其中 为优化变量， 为半正定矩阵 ， 为约束矩阵。 而上述MPC的通常代价函数为：

如果考虑也可以将 中的 , 为上一步的控制序列的求解结果，当做已知量即可以根据后文很方便地进行构造，为了说明的方便此处省略该项的代价。

其中 为终端约束(terminal constraint)， 通常是为了让求解结果收敛到优化目标 ; 为过程约束(stage constraint), 其维度为4x4; 为对控制量及控制量变化的约束其维度为2x2; 为预测步长; 为控制步长; 为初始状态。

注意： 形式二中的代价函数将 作为求解器要优化的变量，而形式一则只优化 序列。

构造代价函数为OSQP求解形式

下面将上述优化目标 及其约束构造为式(1)要求的形式(为了理解方便，后面的矩阵都将注释维度, x维度假定为4x1, u维度假定为2x1): 1. 构造 矩阵,式(2)中 可以展开并忽略常数项可以整理为如下形式： 设

上面的式子便为osqp求解器要求的形式。

约束矩阵的构建

事实上，约束矩阵即为(2)式中的约束条件的矩阵表达，其中值得注意的是其中等式约束的表达也采用了不等式的形式:

构造的约束矩阵 , 如下:

优化目标求解

对于形式一，可以利用线性求解器求得解析解。

对于形式二， 可以利用QP(Quadratic Programming)问题求解器求最优解（本质为凸优化问题）