MPC轨迹跟踪控制（动力学版本）

1、二自由度状态方程

A 、B矩阵和定义的误差如下：

2、离散化、预测推导、目标函数优化、约束条件设计

这里可以参考上篇博客，基于运动学的推导过程，核心思想一样，也就是误差方程不同。

3、carsim设置 

1、道路设置 

这里选取了一个U行道路，也可以选取其他道路比如老王的道路

2、输入设置

3、输出设置 

这里的4是道路曲率、5是参考的纵向位置、7是参考的横向位置 、10是与参考路径偏差

4、simulink模型 

1、误差计算模块 

2、s-function具体代码

function [sys,x0,str,ts] = mpc_dyn1(t,x,u,flag)
switch flag
  case 0
    [sys,x0,str,ts]=mdlInitializeSizes;
  case 2
    sys=mdlUpdate(t,x,u);
  case 3
    sys=mdlOutputs(t,x,u);
  case {1,4,9}
    sys=[];
  otherwise
    DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));

end
% 
function [sys,x0,str,ts]=mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 4;
sizes.NumOutputs     = 1;
sizes.NumInputs      = 5;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;   % at least one sample time is needed
sys = simsizes(sizes);
x0  = [0.0001;0.0001;0.0001;0.0001];
str = [];
ts  = [0.01 0];
global U
U=[0];
simStateCompliance = 'UnknownSimState';

function sys=mdlUpdate(t,x,u)

sys = x;

function sys=mdlOutputs(t,x,u)
%% 定义全局变量
global A1 B1 delta_u_piao U kesi;
Nx=4;%状态量4个，横向误差，航向角误差，侧向速度误差，横摆角速度误差
Nu=1;%控制量1个前轮转角
Np=80;%预测步长50
Nc=50;%控制步长30
Row=10;%松弛因子
T=0.01;%采用时间0.01
T_all = 40;
%%车辆参数
 m = 1270 + 88.6 +54.4 ;
 a = 1.015;
 b = 2.91 - a;
 L = 2.91;
 cf = -148970;
 cr = -82204;
 Iz = 1536.7;
 vx=u(1);
%%构造新的状态矩阵
kesi=zeros(Nx+Nu,1);
kesi(1)=u(2);
kesi(2)=u(3);
kesi(3)=u(4);
kesi(4)=u(5);
kesi(5)=U;
%%初始化矩阵
q=[100,0,0,0;
   0,1,0,0;
   0,0,1,0;
   0,0,0,1];
Q=kron(eye(Np),q);
R=10*eye(Nu*Nc);
% A1 =[1,T,0,0;
%     0,1+T* (cf + cr)/(m*vx),-T*(cf + cr)/m,T*(a*cf - b*cr)/(m*vx);
%     0,0,1,T;
%     0,T*(a*cf - b*cr)/(Iz*vx),-T*(a*cf - b*cr)/Iz,T*(a*a*cf+b*b*cr)/(Iz*vx)];%
%  B1=[0;
%     T*-cf/m;
%     0;
%     -T*a*cf/Iz];
A2=[0,vx,1,0;
   0,0,0,1;
   0,0,(cf+cr)/m/vx,(a*cf-b*cr)/m/vx-vx;
   0,0,(a*cf-b*cr)/Iz/vx,(a^2*cf+b^2*cr)/Iz/vx];
B2=[0;0;-cf/m;-a*cf/Iz];
A1=A2*T+eye(Nx);%雅可比矩阵计算
B1=B2*T;
%%构建预测方程
A_cell=cell(2,2);
A_cell{1,1}=A1;
A_cell{1,2}=B1;
A_cell{2,1}=zeros(Nu,Nx);
A_cell{2,2}=eye(Nu);
A=cell2mat(A_cell);
B_cell=cell(2,1);
B_cell{1,1}=B1;
B_cell{2,1}=eye(Nu);
B=cell2mat(B_cell);
C_cell=cell(1,2);
C_cell{1,1}=eye(Nx);
C_cell{1,2}=zeros(Nx,Nu);
C=cell2mat(C_cell);
%% 预测时域矩阵升维
PHI_cell=cell(Np,1);
for i=1:1:Np
    PHI_cell{i,1}=C*A^i;
end
PHI=cell2mat(PHI_cell);
THETA_cell=cell(Np,Nc);
for i=1:1:Np
    for j=1:1:Nc
        if  i>=j
            THETA_cell{i,j}=C*A^(i-j)*B;
        else
            THETA_cell{i,j}=zeros(Nx,Nu);
        end
    end
end
THETA=cell2mat(THETA_cell);
%%构建二次规划矩阵
H_cell=cell(2,2);
H_cell{1,1}=THETA'*Q*THETA+R;
H_cell{1,2}=zeros(Nc*Nu,1);
H_cell{2,1}=zeros(1,Nc*Nu);
H_cell{2,2}=Row;
H=cell2mat(H_cell);
error=PHI*kesi;
f_cell=cell(1,2);
f_cell{1,1}=2*error'*Q*THETA;
f_cell{1,2}=0;
f=cell2mat(f_cell);
f = f';
%%构建不等式约束
A_t=zeros(Nc,Nc);%工具人矩阵
for i=1:1:Nc
    for j=1:1:Nc
        if i>=j
            A_t(i,j)=1;
        else
            A_t(i,j)=0;
        end
    end
end
A_I=kron(A_t,eye(Nu));
Ut=kron(ones(Nc,1),U);
umax=0.44;
umin=-0.44;
delta_umax=0.005;
delta_umin=-0.005;
Umax=kron(ones(Nc,1),umax);
Umin=kron(ones(Nc,1),umin);
delta_Umax=kron(ones(Nc,1),delta_umax);
delta_Umin=kron(ones(Nc,1),delta_umin);
A_cons_cell=cell(2,2);
A_cons_cell{1,1}=A_I;
A_cons_cell{1,2}=zeros(Nu*Nc,1);
A_cons_cell{2,1}=-A_I;
A_cons_cell{2,2}=zeros(Nu*Nc,1);
A_cons=cell2mat(A_cons_cell);
B_cons_cell=cell(2,1);
B_cons_cell{1,1}=Umax-Ut;
B_cons_cell{2,1}=-Umin+Ut;
B_cons=cell2mat(B_cons_cell);
%%上下界约束
M = 10;
lb=[delta_Umin;0];
ub=[delta_Umax;10];
%% 二次规划问题
options=optimset('Algorithm','interior-point-convex');
[X,fval,exitflag] =quadprog(H,f,A_cons,B_cons,[],[],lb,ub,[],options);
%% 赋值输出
 delta_u_piao=X(1);
 U=kesi(5)+delta_u_piao;
 u_real = U;
sys = u_real;

5、仿真结果 

横向偏差图 

前轮转角 

轨迹对比

可以看到，仿真精度还是很高的。