结合规划层的轨迹跟踪控制系统
思想:局部规划+轨迹跟踪
局部规划:低精度模型+长周期
跟踪控制层:高精度模型+短周期

基于MPC规划层需满足的条件:
- 满足运动学、动力学约束
- 能避障
基于MPC控制层需满足的条件:
- 实际跟踪轨迹与期望轨迹偏差尽量小
- 优化出的控制量满足执行机构的限制
- 不打滑,不侧翻
基于MPC的规划器
模型:点质量动力学模型
参考点的选择:(考虑目标点信息时)离车最近的轨迹上的点。
避障函数:惩罚离障碍物的距离

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

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