基于车辆动力学的横向控制学习

最新推荐文章于 2024-06-27 06:36:23 发布

芜湖起飞哦

最新推荐文章于 2024-06-27 06:36:23 发布

阅读量116

点赞数

文章标签：学习

本文链接：https://blog.csdn.net/weixin_51724291/article/details/135066357

版权

为了记录一下学习过程与遇到的错误，如有错误欢迎讨论。

1.LQR（参考忠厚老实的老王）https://www.bilibili.com/video/BV1Py4y1B7ab/?spm_id_from=333.999.0.0&vd_source=33be2788fe5983a88368a312539f40fb

规划参数接口

参考轨迹给出不同点的x_r,y_r,\theta_r,k_r,通过carsim反馈的位置搜索找到最近的投影点进行误差计算

function [k,err] = fcn( x,y,phi,vx,vy,phi_dot,xr,yr,thetar,kappar)
    n=length(xr);
    dmin=(x-xr(1))^2+(y-yr(1))^2;
    min=1;
    for i=1:n
        d=(x-xr(i))^2+(y-yr(i))^2;
        if d<dmin
            dmin=d;
            min=i;
        end
    end
    dmin=min;
    tor=[cos(thetar(dmin));sin(thetar(dmin))];
    nor=[-sin(thetar(dmin));cos(thetar(dmin))];
    d_err=[x-xr(dmin);y-yr(dmin)];
    ed=nor'*d_err;
    es=tor'*d_err;
    %theta=thetar(dmin);
    theta=thetar(min)+kappar(min)*es;
    ed_dot=vy*cos(phi-theta)+vx*sin(phi-theta);
    ephi=sin(phi-theta);
    s_dot=(vx*cos(phi-theta)-vy*sin(phi-theta))/(1-kappar(dmin)*ed);
    ephi_dot=phi_dot-kappar(dmin)*s_dot;
    k=kappar(dmin);
    err=[ed;ed_dot;ephi;ephi_dot];
end

在后边横纵向控制中，规划接口被改为带有时间序列的x_r,y_r,\theta_r,k_r，也就是可以通过clock模块得到目标点的位置信息。下图为规划的道路+代码。

LQR求解过程

求解Raccati黎卡提方程，过程公式未推导，总而言之就是利用lqr工具箱得到反馈矩阵K，则控制量为-K*x

a=1.015;
b=2.910-1.015;
m=1270+88.6;
I=1536.7;
Cf=-17580*2;
Cr=Cf;
k=zeros(5000,4);
%%这里是规定0.01s的时长递进，最大速度是50m/s，离线的lqr可以通过查找速度来得到对应的K，从而得到u=-kx
for i=1:5000
    vx=0.01*i;
    A=[0,1,0,0;
       0,(Cf+Cr)/(m*vx),-(Cf+Cr)/m,(a*Cf-b*Cr)/(m*vx);
       0,0,0,1;
       0,(a*Cf-b*Cr)/(I*vx),-(a*Cf-b*Cr)/I,(a^2*Cf+b^2*Cr)/(I*vx)
        ];
    B=[0;-Cf/m;0;-a*Cf/I];
    Q=eye(4);
    R=100;
    k(i,:)=lqr(A,B,Q,R);
end
k1 = k(:,1)';
k2 = k(:,2)';
k3 = k(:,3)';
k4 = k(:,4)';%%转置是为了将k1，k2，k3，k4从工作区导入simulink中

这里的K矩阵的通过车速进行查表得到的，再加上一个动力学模型中的前馈过程，得到前轮转角u。

2.MPC

动力学模型与误差计算参考的是忠厚老实的老王。代码参考https://blog.csdn.net/CHD_Apple/article/details/133919745

一般看到的是增量形势的增广式动力学模型，这里采用的是直接求解前轮转角的动力学模型。

这里有几点需要注意的。

预测时域与控制时域的区别

一般来说控制时域Nc小于预测时域Np，dr_can的书中取的是相等。尽管从x(k+Np)=A*x(k+Np-1)+B*u(k+Np-1)推断来看，预测到Np步需要Np-1步的控制量，然而我们可以利用Nc步的控制量代替后边的Nc+1,Nc+2......Np-1步，这样可以减少一点计算量，更多的可以利用热启动，也就是启发式来减少计算量。

增量与非增量的区别

增量的动力学模型会利用一个kesai向量来增广控制量，从而得到控制量增量的动力学模型。因此相比直接求解控制量而言，增量方程的代码会多一段求解新的A和B矩阵，随后再升维。
利用s-function与simulink模型可以直接得到车辆的状态更新，mdlupdate好像没有什么用

function [sys,x0,str,ts] = mpc(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      = 6;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1; 
global U;%定义全局变量U用来保存控制量
U = [0];
sys = simsizes(sizes);
 
x0  = [0.0001;0.0001;0.0001;0.0001];%状态量初始化
str = []; % 保留变量一般都为空
ts  = [0.1,0];%采样时间和偏置
simStateCompliance = 'UnknownSimState';
 
%状态更新，但是好像没什么用。。。后边的状态更新都是从输入向量得到的
function sys=mdlUpdate(t,x,u)
 
sys = x; %更新下一个离散状态即x(k+1)
 
%输出
function sys=mdlOutputs(t,x,u)
  global A B C;
  global kesai;
  Nx = 4;%状态量个数
  Nu = 1;%控制量个数
  Np = 20;%预测时域
  Nc = 15;%控制时域
   a=1.015;
   b=2.910-1.015;
   m=1270+88.6;
   I=1536.7;
   Cf=-148970;
   Cr=-82204;
   dt=0.1;
   vx=u(1);%常量
   kesai=zeros(Nx,1);
   kesai(1)=u(2);
   kesai(2)=u(3);
   kesai(3)=u(4);
   kesai(4)=u(5);
   phi_dot=u(6);
    if vx == 0
    % 处理 vx 为零的情况，可以设置一个默认值或采取其他合适的措施
    vx = 1e-6; % 一个很小的非零值
   end
   A1=[0,1,0,0;
       0,(Cf+Cr)/(m*vx),-(Cf+Cr)/m,(a*Cf-b*Cr)/(m*vx);
       0,0,0,1;
       0,(a*Cf-b*Cr)/(I*vx),-(a*Cf-b*Cr)/I,(a*a*Cf+b*b*Cr)/(I*vx)];
   B1=[0;-Cf/m;0;-a*Cf/I];
   C1=[0;(a*Cf-b*Cr)/(m*vx)-vx;0;(a*a*Cf+b*b*Cr)/(I*vx)];
   A=A1*dt+eye(4);%离散化
   B=B1*dt;
   C=C1*dt*(phi_dot-u(4));
   q=[1000,0,0,0;
   0,10,0,0;
   0,0,100,0;
   0,0,0,10];
  Q=kron(eye(Np),q);
  R = 10*eye(Nu*Nc);
  %%构建预测方程矩阵
  PHI_cell = cell(Np,1);
  THETA_cell = cell(Np,Nc);
  cc_cell=cell(Np,1);
  for i = 1:Np
       PHI_cell{i,1} =  A^i;
     for j = 1:Nc 
         if i>= j
             THETA_cell{i,j} = A^(i-j)*B;
         else
             THETA_cell{i,j} = zeros(Nx,Nu);
         end
     end
  end
  cc_cell{1,1} = C;
  for i=2:Np
      cc_cell{i,1} = cc_cell{i-1,1} + A^(i-1)*C;
  end
  PHI = cell2mat(PHI_cell);
  THETA = cell2mat(THETA_cell);
  cc = cell2mat(cc_cell);
  %%构造二次规划的矩阵
  H_cell = cell(1,1);
  H_cell{1,1} = THETA'*Q*THETA + R;
  H = cell2mat(H_cell);
  H = 0.5*(H+H');%保证正定
  f_cell = cell(1,1);
  err = PHI*kesai+cc;
  f_cell{1,1} = err'*Q*THETA;
  f = cell2mat(f_cell);
  f = f';%这里是为了对应matlab中的quadprog，可以help看一下

%     %上下界约束
   % M= 10;%松弛因子上界
   u_umin = -1; % 0.0082rad = 0.47deg
   u_umax = 1;
   U_Umin = kron(ones(Nc,1),u_umin);
   U_Umax = kron(ones(Nc,1),u_umax);
   lb = -1;
   ub = 1;
  
   %%开始求解
   options = optimset('Algorithm','interior-point-convex');
   %options = optimset('Algorithm','active-set');
   X = quadprog(H,f,[],[],[],[],lb,ub,[],options);
  
   %%计算输出
   out = X(1);
   sys =out ;

可以看到横向控制的结果