在第六章中，模型预测控制器主要由带避障功能的轨迹重规划模块和轨迹跟踪控制模块构成。轨迹重规划模块接受来自传感器的障碍物信息以及来自全局规划的参考轨迹信息，通过算法规划出局部参考轨迹，再发送给跟踪控制模块。轨迹跟踪控制模块接受来自规划层的局部参考轨迹，输出前轮偏角来对车辆进行控制。
在我们的S-Funcion中第一部分为主函数，主要完成程序的初始化，设置 S 函数的输入、输出、状态变量个数等。第二部分为初始化子函数，包括变量设置、采样周期设置等。第三部分为更新离散状态变量的子函数。第四部分为计算输出的子函数，是规划层控制的主体，包括状态变量的定义、预测和控制时域的设定、参考轨迹生成、状态变量矩阵变换、障碍物设置、函数求解等。第五部分为求代价函数的功能子函数最后输出局部参考路径。
该部分代码已在carsim2016、carsim8.02联合matlab2019a运行成功！
详解见我的b站链接：https://www.bilibili.com/video/BV1HQ4y1R7a8/
需要全套代码可加up微信：Jess-L-S-J
工况设置
模型搭建
轨迹重规划结果 ：