无人驾驶车辆模型预测控制第六章核心

在第六章中，模型预测控制器主要由带避障功能的轨迹重规划模块和轨迹跟踪控制模块构成。轨迹重规划模块接受来自传感器的障碍物信息以及来自全局规划的参考轨迹信息，通过算法规划出局部参考轨迹，再发送给跟踪控制模块。轨迹跟踪控制模块接受来自规划层的局部参考轨迹，输出前轮偏角来对车辆进行控制。

轨迹规划层代码 

function [sys,x0,str,ts] = MPC_TrajPlanner(t,x,u,flag)
% 主函数，状态量=[y_dot,x_dot,phi,Y,X]，控制量为前轮偏角ay
switch flag,
 case 0  %flag=0表示处于初始化状态，此时用函数mdlInitializeSizes进行初始化
  [sys,x0,str,ts] = mdlInitializeSizes; % Initialization
 case 2  %flag=2表示此时要计算下一个离散状态
  sys = mdlUpdates(t,x,u); % Update discrete states
 case 3 %flag=3表示此时要计算输出
  sys = mdlOutputs(t,x,u); % Calculate outputs
 case {1,4,9} % Unused flags
    % flag=1表示此时要计算连续状态的微分
    %flag=4表示此时要计算下一次采样的时间，只在离散采样系统中有用，主要用于变步长的设置
    %flag=9表示此时系统要结束，一般来说写上在mdlTerminate函数中写上sys=[]就可
  sys = [];
 otherwise
  error(['unhandled flag = ',num2str(flag)]); % Error handling
end
% End of dsfunc.

%===