无人驾驶模型预测控制（六）-加入规划层的轨迹跟踪控制

结合规划层的轨迹跟踪控制系统

思想：局部规划+轨迹跟踪
局部规划：低精度模型+长周期
跟踪控制层：高精度模型+短周期

基于MPC规划层需满足的条件：

1. 满足运动学、动力学约束
2. 能避障

基于MPC控制层需满足的条件：

1. 实际跟踪轨迹与期望轨迹偏差尽量小
2. 优化出的控制量满足执行机构的限制
3. 不打滑，不侧翻

基于MPC的规划器

模型：点质量动力学模型
参考点的选择：（考虑目标点信息时）离车最近的轨迹上的点。
避障函数：惩罚离障碍物的距离

轨迹重规划层的模型预测控制器目标函数

五次多项式轨迹拟合
实现规划层和控制层的顺利对接。
将局部规划层的离散点拟合为五次多项式。

非线性二次规划计算
算法跟之前程序的过程基本一致。

1. 模型状态空间方程
2. 迭代预测（以前是写出了预测方程，现在每一步以上一步为基础预测）
3. 非线性优化问题直接求解（之前都是转化为2次规划问题）

点评：还是预测方程更清晰一点，迭代方法写起来多写点。（本质相同）
预测方程就是迭代方法的公式化。

基于MPC方法的路径跟踪控制器

编程思想还是老套路：
状态方程+线性化+离散化+预测方程+约束线性化+非线性目标函数转为二次规划+求解最优问题。