MPC轨迹跟踪控制

CHD_Apple

已于 2023-11-03 12:10:21 修改

阅读量2.2k

点赞数 5

分类专栏：智能驾驶文章标签：算法人工智能机器学习自动驾驶目标跟踪

于 2023-10-16 16:07:58 首次发布

本文链接：https://blog.csdn.net/CHD_Apple/article/details/133815146

版权

智能驾驶专栏收录该内容

18 篇文章 4 订阅

订阅专栏

1、车辆运动学方程

这里具体的推导就不说明了，可以得到如下状态方程：

也即是如下非线性方程：

2、线性化

模型预测控制的核心思想是建立基于误差的状态方程来推导，针对上面方程我们采用在任意参考点线性化的方法，即：对于参考系统，任意时刻的状态量和控制量满足

其中，

将此式与相减，可得：

具体表示为：

线性化后模型为：

A、B分别为：

3、离散化

在由上述推导得到的线性化方程后，该状态方程是连续的，不能直接用于模型预测控制器的设计，所以有必要对其进行离散化处理。经过离散化后我们就能实现了由当前的状态量误差和当前时刻的控制量误差推导得到下一时刻的状态量误差。

这里采用向前欧拉法：

T为系统采样时间，程序里设定了为0.1 ，等式左右两边简单变形可得：

4、预测方程推导

1、合并状态量

2、下一时刻状态推导

新构造的状态量 ξ矩阵的大小为：(Nx,Nu)， Nx,Nu分别是状态量个数和控制量个数。

上面A矩阵中的 0矩阵的大小为：(Nu,Nx)() ；

A矩阵的大小为：(Nx+Nu,Nx+Nu)

B矩阵的大小为：(Nx+Nu,Nu)

Δu(k)矩阵的大小为：(Nu,1)

输出方程为：

3、迭代推导

因为，C矩阵的大小为：(Nx,Nx+Nu) ，

A矩阵的大小为：(Nx+Nu,Nx+Nu)

B矩阵的大小为：(Nx+Nu,Nu)

Δu(k)矩阵的大小为：(Nu,1)

所以 $A^{n}$ 矩阵的大小为：(Nx+Nu,Nx+Nu)

C $A^{n}$ 矩阵的大小为：(Nx,Nx+Nu)

C $A^{n}$ B矩阵的大小为：(Nx,Nu)

总结规律可得：

Ψ_cell矩阵大小为：(Np,1) ，

Ψ矩阵大小为：(Np∗Nx,Nx∗Nu)

Y_cell矩阵的大小为(Np,1)，Y矩阵的大小(Nx∗Np,1)，

Θ_cell矩阵大小为：(Np,Nc)，又因C $A^{n}$ B矩阵的大小为：(Nx,Nu)，

所以Θ矩阵的大小为(Nx∗Np,Nc∗Nu)，且其中每个00矩阵的大小为：(Nx,Nu)，

ΔU_cell矩阵的大小为：(Nc,1)，ΔUΔ矩阵的大小为：(Nc∗Nu,1)。

5、目标函数优化

优化的目标函数为：

定义系统输出量参考值：

matlab中二次规划的标准形式为：

所以进行转化可得：

因为Θ_cell矩阵大小为：(Np,Nc)，Θ矩阵的大小为：(Nx∗Np,Nc∗Nu)

Q_cell元胞的大小为：(Np,Np)，Q矩阵大小为：(Nx∗Np,Nx∗Np)

所以ΘTQΘ矩阵的大小为：(Nc∗Nu,Nc∗Nu)

H矩阵中右上角的0矩阵大小为：(Nc∗Nu,1)，左下角的0矩阵的大小为：(1,Nc∗Nu)

6、约束条件

7、carsim配置

1、输入配置

2、输出配置

8、simulink模型

9、mpc_geo1代码

首先在工作区输入edit sfuntmpl，这是一份模板，先复制一份，改名为mpc_geo1，具体可参考simulink中S-Function的说明及应用举例_simulink s-function-CSDN博客

function [sys,x0,str,ts] = mpc_geo1(t,x,u,flag)

switch flag,

  %%%%%%%%%%%%%%%%%%
  % Initialization %
  %%%%%%%%%%%%%%%%%%
  case 0,
    [sys,x0,str,ts]=mdlInitializeSizes;

  %%%%%%%%%%
  % Update %
  %%%%%%%%%%
  case 2,
    sys=mdlUpdate(t,x,u);

  %%%%%%%%%%%
  % Outputs %
  %%%%%%%%%%%
  case 3,
    sys=mdlOutputs(t,x,u);

  %%%%%%%%%%%%%
  % Terminate %
  %%%%%%%%%%%%%
  case {1,4,9},
    sys=[];

  %%%%%%%%%%%%%%%%%%%%
  % Unexpected flags %
  %%%%%%%%%%%%%%%%%%%%
  otherwise
    DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));

end

% end sfuntmpl

%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%
function [sys,x0,str,ts]=mdlInitializeSizes

%
% call simsizes for a sizes structure, fill it in and convert it to a
% sizes array.
%
% Note that in this example, the values are hard coded.  This is not a
% recommended practice as the characteristics of the block are typically
% defined by the S-function parameters.
%
sizes = simsizes;

sizes.NumContStates  = 0;
sizes.NumDiscStates  = 3;
sizes.NumOutputs     = 2;
sizes.NumInputs      = 3;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;   % at least one sample time is needed
global U;%定义全局变量U用来保存控制量偏差（v- vr;  delta - deltar）
U = [0;0];
sys = simsizes(sizes);

%
% initialize the initial conditions
%
x0  = [0;0;0];%状态量初始化，即X、Y和phi都等于0

%
% str is always an empty matrix
%
str = []; % 保留变量一般都为空

%
% initialize the array of sample times
%
ts  = [0.1,0];%采样时间和偏置

% Specify the block simStateCompliance. The allowed values are:
%    'UnknownSimState', < The default setting; warn and assume DefaultSimState
%   c 'DefaultSimState', < Same sim state as a built-in block
%    'HasNoSimState',   < No sim state
%    'DisallowSimState' < Error out when saving or restoring the model sim state
simStateCompliance = 'UnknownSimState';

% end mdlInitializeSizes
%=============================================================================

%=============================================================================
% mdlUpdate
% Handle discrete state updates, sample time hits, and major time step
% requirements.
%=============================================================================
%
function sys=mdlUpdate(t,x,u)

sys = x; %更新下一个离散状态即x(k+1)

% end mdlUpdate

%
%=============================================================================
% mdlOutputs
% Return the block outputs.
%=============================================================================
%
function sys=mdlOutputs(t,x,u)
  global a b delat_u_piao;
  global U;
  global kesai;
  Nx = 3;%状态量个数
  Nu = 2;%控制量个数
  Np = 50;%预测时域
  Nc = 30;%控制时域
  Row = 10;%松弛因子
  t_d = u(3);%实际的横摆角
  %%参考路径半径为25m,速度为10m/s
  r(1) = 25*sin(0.2*t);%参考的x
  r(2) = 35-25*cos(0.2*t);%参考的y
  r(3) = 0.2*t;%参考横摆角
  vd1 = 5;%参考速度
  vd2 = 2.91/25;%参考前轮转角delta = l/r
  %%构造新的状态量矩阵
  kesai = zeros(Nx + Nu,1);
  kesai(1) = u(1) - r(1);
  kesai(2) = u(2)- r(2);
  kesai(3) = t_d - r(3);
  kesai(4) = U(1);
  kesai(5) = U(2);
  T = 0.1;
  T_all = 40;
  L = 2.91;
  %%矩阵初始化
  delta_u_piao= zeros(Nu,1);
  Q = 1000*eye(Nx*Np);
  R = 10*eye(Nu*Nc);
  td1 = r(3);
  a = [1,0,-T*vd1*sin(td1);
      0,1,T*vd1*cos(td1);
      0,0,1];
  b = [T*cos(td1),0;
      T*sin(td1),0;
      (T*tan(vd2))/L,T*vd1/(L*(cos(vd2))^2)];
  %%预测方程初始化即构建AB矩阵
  A_cell = cell(2,2);
  B_cell = cell(2,1);
  A_cell{1,1} = a;
  A_cell{1,2} = b;
  A_cell{2,1} = zeros(Nu,Nx);
  A_cell{2,2} = eye(Nu);
  B_cell{1,1} = b;
  B_cell{2,1} = eye(Nu);
  A = cell2mat(A_cell);
  B = cell2mat(B_cell);
  C = [1,0,0,0,0;
      0,1,0,0,0;
      0,0,1,0,0];
  %%构建预测方程矩阵
  PHI_cell = cell(Np,1);
  THETA_cell = cell(Np,Nc);
  for i = 1:Np
       PHI_cell{i,1} =  C*A^i;
     for j = 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
  PHI = cell2mat(PHI_cell);
  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);
  H = 0.5*(H+H');
  ERROR = PHI*kesai;
  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:Nc
      for j = 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));
  U_t = kron(ones(Nc,1),U);
  umin = [-0.2;-0.54];
  umax = [0.2;0.332];
  U_min = kron(ones(Nc,1),umin);
  U_max = kron(ones(Nc,1),umax);
   A_cons_cell={A_I,zeros(Nu*Nc, 1); -A_I,zeros(Nu*Nc, 1)};
   b_cons_cell={U_max-U_t;-U_min+U_t};
   A_cons=cell2mat(A_cons_cell);%（求解方程）状态量不等式约束增益矩阵，转换为绝对值的取值范围
   b_cons=cell2mat(b_cons_cell);%（求解方程）状态量不等式约束的取值
    %%上下界约束
   M= 10;%松弛因子上界
   delta_umin = [-0.05;  -0.0082]; % 0.0082rad = 0.47deg
   delta_umax = [0.05;  0.0082];
   delta_Umin = kron(ones(Nc,1),delta_umin);
   delta_Umax = kron(ones(Nc,1),delta_umax);
   lb = [delta_Umin;0];
   ub = [delta_Umax;M];
   %%开始求解
   options = optimset('Algorithm','interior-point-convex');
   [X,fval,exitflag] = quadprog(H,f,A_cons,b_cons,[],[],lb,ub,[],options);
   %%计算输出
   delta_u_piao(1) = X(1);
   delta_u_piao(2) = X(2);
   U(1) = kesai(4) + delta_u_piao(1);
   U(2) = kesai(5) + delta_u_piao(2);
   u_real(1) = vd1 + U(1);
   u_real(2) = vd2 + U(2);
sys = [u_real(1);u_real(2)];

10、仿真效果

这里可以看到基本可以跟踪轨迹，这里面的Q 和R设置的很随意Np和Nc也是随意设置

这里我随意调节了 Np和Nc,就会有很大不同

12、可视化

t = 0:0.1:30;
x =25*sin(0.2*t);
y = 35-25*cos(0.2*t);
plot(x,y)
hold on 
plot(out.x,out.y)
legend('参考轨迹','实际轨迹')
 xlabel('横向位移');
ylabel('纵向位移');

11、参考链接

参考链接：

线性时变模型预测控制推导详解_时变模型预测控制线性化时是对当天状态进行线性化还是针对当前应到达的轨迹位置线-CSDN博客

《无人驾驶车辆模型预测控制》（第二版）第四章详细学习——算法部分_无人驾驶车辆模型预测控制 pdf_总系学不废的博客-CSDN博客