【Carsim Simulink自动驾驶仿真】基于MPC的轨迹跟踪控制

薯一个蜂蜜牛奶味的愿

已于 2022-05-09 12:45:21 修改

阅读量1.3w

点赞数 65

分类专栏：自动驾驶仿真学习文章标签：算法 matlab 自动驾驶

于 2022-04-01 21:22:30 首次发布

本文链接：https://blog.csdn.net/w_w_y/article/details/123903969

版权

自动驾驶仿真学习专栏收录该内容

5 篇文章 50 订阅

订阅专栏

如果对Carsim的基础使用还不了解，可以参考：【Carsim Simulink自动驾驶仿真】基于MPC的速度控制
如果对MPC算法原理不清楚，可以参考：如何理解MPC模型预测控制理论
项目介绍：
教程为北理工的无人驾驶车辆模型预测控制第2版。所用的仿真软件为Carsim2020.0和MatlabR2021a。使用MPC控制思想对车辆进行轨迹跟踪控制，并给出仿真结果。

整整弄了两天，踩了无数的坑，所以篇幅比较大，如果还有什么其他问题，欢迎一起讨论。

从网上下载下来的代码运行不了，所以本文的代码是经过修改和调试的。

效果图

因为参数经过了调整，所以效果上和书上不一样。

直线跟踪

在这里插入图片描述
图像为：路径，速度，误差和前轮偏角

前轮偏角的局部放大图为：

3m/s

在这里插入图片描述

5m/s

在这里插入图片描述

10m/s

在这里插入图片描述
速度的具体图像为：

圆跟踪

在这里插入图片描述
图像为：路径，速度和前轮偏角(误差不知道该在Carsim中怎么画，知道的大佬请私信，呜呜呜)：

轨迹放大之后：

前轮偏角为：

前轮偏角放大之后：
在这里插入图片描述

3m/s

在这里插入图片描述
速度为：

5m/s

在这里插入图片描述
速度为：

10m/s

在这里插入图片描述
速度为：

MATLAB框架搭建

书中给出的MATLAB框架如下：
在这里插入图片描述
因为对于这种方法输出需要再另外处理。所以我这里直接将速度乘3.6，角度也在程序中直接处理了。对应的框架图为：

代码的数学遗漏问题与修改

书中的代码和下载下来的代码不一样，所以还是检查一下这两个地方，

运动学模型少L

在书中，推导运动学模型的时候，最后一个数字分母是有一个l的：
在这里插入图片描述
但是在代码中，这个地方少了，需要加上。

改写之后的目标函数线性项少系数

在这里插入图片描述
这里的2在书中没有少，但是下载下来的代码中没有这个系数。如果没有的可以加一下。

代码中的其他问题及讨论

输入维度不匹配的情况

看一下S-Function的输入是3个还是5个，输出是2个还是直接给出5个，对应的代码中也应该和其一样。具体在S-Function中是初始化的第一个函数中的：

    sizes.NumOutputs      =5;%[speed,steering]
    sizes.NumInputs       =3;

索引超出数组范围

这个是我的主要问题，之所以报错，是因为求解器求解不出来优化问题，方法的话我找了一些，然后自己也试了一些：

1.变量的范围

这里有两种情况：

第一种情况是检查变量范围delta_umin=[-0.05;-0.0082;];如果当中的-0.05是0.05，将其加一个负号。
第二种情况其实扩大变量的范围可以求解，但是这里不建议，如果你在扩大范围之后可以求解，那么建议还是将其变回原来的数，更改其他值。

2.换一个求解器

使用interior-point-convex，而不是active-set

%options = optimset('Algorithm','active-set');
options = optimset('Algorithm','interior-point-convex');

3.初始值全部设为0

检查初始化函数mdlInitializeSizes中的x0和U是否都为零。注意，这里的U表示的就是控制量与参考控制的差值。

function [sys,x0,str,ts] = mdlInitializeSizes
%找到以下两个值
x0 = [0;0;0];
U=[0;0];

4.对于kesi(4)和kesi(5)的赋值（这个是我的问题）

这个我认为争议比较大，但是改了之后确实没有报错了。
先说一下，按照理论：
在这里插入图片描述
kesi(4)和kesi(5)就是这里的控制变量 $\tilde{u}$ 。而这个 $\tilde{u}$ 根据定义是：

是当前速度与参考速度的插值，所以这里我们应该将Carsim中的5个变量全部输入到S-Funciton中，然后进行如下操作：

    U(1) = u(4)/3.6 - vd1;
    steer_SW = u(5)*pi/180;
    steering_angle = steer_SW/18;
    U(2) = steering_angle - vd2;
    kesi(4)=U(1); % vel-vel_ref
    kesi(5)=U(2); % steer_angle - steering_ref

之前我从来没有怀疑过这里，直到我看到书中的代码和网上下载的不一样的时候，我就尝试了一下书上的代码，即将前四行全部注释掉：

%    U(1) = u(4)/3.6 - vd1;
%    steer_SW = u(5)*pi/180;
%    steering_angle = steer_SW/18;
%    U(2) = steering_angle - vd2;
    kesi(4)=U(1); % vel-vel_ref
    kesi(5)=U(2); % steer_angle - steering_ref

这样改之后我的代码就不会出现问题了。

一些理解

首先我们要清楚，kesi(4,5)是控制减去参考控制，求解器解出来的X是前后两次的控制差值。
我们先看能跑对的程序，大致过程可以简化成设置值，求解，给出输出三步：

U = [0,0]
for 循环：
#第一步：设置kesi
    kesi(4)=U(1); % vel-vel_ref
    kesi(5)=U(2); % steer_angle - steering_ref
#第二步：求解
	[X,~,~]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,[],options);
#第三步：给出输出
	u_dot(1)=X(1);
    u_dot(2)=X(2);
    U(1)=kesi(4)+u_dot(1);%用于存储上一个时刻的控制量
    U(2)=kesi(5)+u_dot(2);
    u_real(1) = U(1) + vd1;
    u_real(2) = U(2) + vd2;

这里列一个表格：

时间	求解之前的U，也就是上一步的U	kesi	u_dot	求解之后的U	输出
k-1	$u_{k-1}-u_{k-1}^{ref}$	$u_{k-1}-u_{k-1}^{ref}$	$u_k-u_{k}^{ref}-u_{k-1}+u_{k-1}^{ref}$	$u_{k-1}-u_{k-1}^{ref}+u_k-u_{k}^{ref}-u_{k-1}+u_{k-1}^{ref}=u_k-u_{k}^{ref}$	u_k
之后就可以征程循环。注意这里的初始值设置为[0,0]是合理的。

我出错的程序：

U = [0,0]
for 循环：
#第一步：设置kesi
    U(1) = u(4)/3.6 - vd1;
    steer_SW = u(5)*pi/180;
    steering_angle = steer_SW/18;
    U(2) = steering_angle - vd2;
    kesi(4)=U(1); % vel-vel_ref
    kesi(5)=U(2); % steer_angle - steering_ref
#第二步：求解
	[X,~,~]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,[],options);
#第三步：给出输出
	u_dot(1)=X(1);
    u_dot(2)=X(2);
    U(1)=kesi(4)+u_dot(1);%用于存储上一个时刻的控制量
    U(2)=kesi(5)+u_dot(2);
    u_real(1) = U(1) + vd1;
    u_real(2) = U(2) + vd2;

建立同样的表格，这里唯一不一样的是，之前的输入其实是记忆到了程序里面，这里的输入变成了从程序中获取：

时间	求解之前的U，也就是上一步的U	kesi	u_dot	求解之后的U	输出
k-1	$u_{k-1}^{input}-u_{k-1}^{ref}$	$u_{k-1}^{input}-u_{k-1}^{ref}$	$u_k-u_{k}^{ref}-u_{k-1}^{input}+u_{k-1}^{ref}$	$u_k-u_{k}^{ref}$	u_k
k	$u_{k}^{input}-u_{k}^{ref}$
但是按照道理来说，这个和上一个表格是一样的，这里不一样的是速度输入之后需要进行变换，输出的时候也要进行变换，这变换之后会存在的误差比较大。还有一种可能，这里其实输入进来的是速度和方向盘转角，这个方向盘转角和轮胎转角之间有一个系数，这个系数这里设置的18，但是具体设置多少，也没有具体的深究。
总之我个人感觉可能是上一时刻的输出 $u_k$ 和这一时刻的输入 $u_k^{input}$ 可能不一样。希望各位大佬也给出一些解释。

参数的选取

参数这里有4个，预测步长，控制步长，Q和R。这里Q和R都是对角阵，表格中的数值是对角阵中的数，而不是Q和R的实际值。
这里我的参数为：

预测步长( $N_p$ )	控制步长(N_c)	Q	R
100	30	1	100

我先调整的Q和R，当轨迹不贴合的时候，调大Q,当速度收敛太慢，震荡太久的时候调整R。根据我的经验Q取1就好，之后就根据速度图像的震荡和你的评判标准进行选择就好。
对于预测步长和控制步长，预测的越久，说明这个智能体越有远见，超调就会相对小一点，个人感觉控制步长太多不是什么好的事情，因为本身希望汽车进行相对较少的控制。

速度问题

这里有一个小问题，书中的速度在初始时刻是没有减速的：
在这里插入图片描述
而在我做的过程中，速度在一开始都有减少：

这里我找到了问题，是两个前轮的速度在一开始不一样，但是按照道理，输入都是一样的，出现这样的差异不知道怎么解决。

附录：全代码(修改过后的)

function [sys,x0,str,ts] = MPCtracking(t,x,u,flag)
    %控制量为速度和前轮偏角，模型是用运动学模型
    switch flag
        case 0
            [sys,x0,str,ts]=mdlInitializeSizes;
        case 2
            sys = mdlUpdates(t,x,u);
        case 3
            sys = mdlOutputs(t,x,u);
        case {1,4,9}
            sys = [];
        otherwise
            error(['unhandled flag =', num2str(flag)])
    end
%End of MPCtracking

%==============================================================
% 初始化
%==============================================================
function [sys,x0,str,ts] = mdlInitializeSizes

    sizes = simsizes;
    sizes.NumContStates   =0;
    sizes.NumDiscStates   =3;
    sizes.NumOutputs      =5;%[speed,steering]
    sizes.NumInputs       =3;
    sizes.DirFeedthrough  =1;
    sizes.NumSampleTimes  =1;
    sys = simsizes(sizes);
    x0 = [0;0;0];
    global U;%存储目前的控制变量[vel,delta]
    U=[0;0];
    
    str = [];
    ts = [0.05,0]; %采样时间：[period,offset]
%End of mdlInitializeSizes

%==============================================================
% 更新离散状态
%==============================================================
function sys = mdlUpdates(t,x,u)
  
sys = x;
%End of mdlUpdate.

%==============================================================
% 计算输出，核心输出
%==============================================================
function sys = mdlOutputs(t,x,u)
    global a b u_dot;
    global U;
    global kesi;

    
    tic
    Nx = 3;%状态量的个数
    Nu = 2;%控制量的个数
    Np = 100;%预测步长
    Nc = 30;%控制步长
    Row = 10; %松弛因子
    yaw_angle =u(3)*pi/180;%CarSim输出的Yaw angle为角度，角度转换为弧度
    
    
%==================================================
%最后调参
%==================================================
    u_bound1 = 0.2;
    u_bound2 = 0.436;
    delta1 = 0.05;
    delta2 = 0.0082;
    Q_a = 1;
    R_a = 100;
%==================================================
%构建路径
%==================================================
%     %直线路径
%     vd1 = 10;%ref_velocity
%     vd2 = 0;%ref_steering
%     r(1)=vd1*t; %ref_x
%     r(2)=5;%ref_y
%     r(3)=0;%ref_heading_angle


%     %半径为35m的圆形轨迹, 圆心为(0, 35), 速度为3m/s
%     r(1)=25*sin(0.12*t);
%     r(2)=25+10-25*cos(0.12*t);
%     r(3)=0.12*t;
%     vd1=3;
%     vd2=0.104;
	%半径为25m的圆形轨迹, 圆心为(0, 35), 速度为10m/s
     r(1)=25*sin(0.4*t);
     r(2)=25+10-25*cos(0.4*t);
     r(3)=0.4*t;
     vd1=10;
     vd2=0.104;
%     %半径为25m的圆形轨迹, 圆心为(0, 35), 速度为5m/s
%      r(1)=25*sin(0.2*t);
%      r(2)=25+10-25*cos(0.2*t);
%      r(3)=0.2*t;
%      vd1=5;
%      vd2=0.104;
%==================================================
%构建结束
%==================================================
   
    %添加转化后的自变量kesi
    kesi=zeros(Nx+Nu,1);
    kesi(1) = u(1)-r(1);%u(1)==X(1),x_offset
    kesi(2) = u(2)-r(2);%u(2)==X(2),y_offset
    heading_offset = yaw_angle - r(3); %u(3)==X(3),heading_angle_offset


    if (heading_offset < -pi)
        heading_offset = heading_offset + 2*pi;
    end
    if (heading_offset > pi)
        heading_offset = heading_offset - 2*pi;
    end
    kesi(3)=heading_offset;

%     U(1) = u(4)/3.6 - vd1;
%     steer_SW = u(5)*pi/180;
%     steering_angle = steer_SW/18;
%     U(2) = steering_angle - vd2;
    kesi(4)=U(1); % vel-vel_ref
    kesi(5)=U(2); % steer_angle - steering_ref

    T = 0.05;

    L = 2.6;%轮间距

    %矩阵初始化
    t_d = r(3);
    %系数

    Q = Q_a*eye(Nx*Np,Nx*Np);
%    Q(Nx*Np,Nx*Np) = Q_a*0.01;
    R = R_a*eye(Nu*Nc);
    %运动学方程
    u_dot = zeros(Nx,1);%%%%这里应该没有x
    a = [1 0 -vd1*sin(t_d)*T;
         0 1 vd1*cos(t_d)*T;
         0 0 1;];
    b = [cos(t_d)*T 0;
      sin(t_d)*T 0;
      tan(vd2)*T/L vd1*T/(L*(cos(vd2))^2);];
    %构建新的状态空间
    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 j=1:1:Np
        PHI_cell{j,1}=C*A^j;
        for k=1:1:Nc
            if k<=j
                THETA_cell{j,k}=C*A^(j-k)*B;
            else 
                THETA_cell{j,k}=zeros(Nx,Nu);
            end
        end
    end
    PHI=cell2mat(PHI_cell);%size(PHI)=[Nx*Np Nx+Nu]
    THETA=cell2mat(THETA_cell);%size(THETA)=[Nx*Np Nu*(Nc+1)]

    H_cell=cell(2,2);
    H_cell{1,1}=THETA'*Q*THETA+R;
    H_cell{1,2}=zeros(Nu*Nc,1);
    H_cell{2,1}=zeros(1,Nu*Nc);
    H_cell{2,2}=Row;
    H=cell2mat(H_cell);
%    H=(H+H')/2;

    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);

    %% 以下为约束生成区域
 %不等式约束
    A_t=zeros(Nc,Nc);%见falcone论文 P181
    for p=1:1:Nc
        for q=1:1:Nc
            if q<=p 
                A_t(p,q)=1;
            else 
                A_t(p,q)=0;
            end
        end 
    end 
    A_I=kron(A_t,eye(Nu));%对应于falcone论文约束处理的矩阵A,求克罗内克积
    Ut=kron(ones(Nc,1), U);%

    umin=[-u_bound1;  -u_bound2];%[min_vel, min_steer]维数与控制变量的个数相同
    umax=[u_bound1;   u_bound2]; %[max_vel, max_steer],%0.436rad = 25deg

    delta_umin = [-delta1;  -delta2]; % 0.0082rad = 0.47deg
    delta_umax = [delta1;  delta2];

    Umin=kron(ones(Nc,1),umin);
    Umax=kron(ones(Nc,1),umax);
    A_cons_cell={A_I zeros(Nu*Nc, 1); -A_I zeros(Nu*Nc, 1)};
    b_cons_cell={Umax-Ut;-Umin+Ut};
    A_cons=cell2mat(A_cons_cell);%（求解方程）状态量不等式约束增益矩阵，转换为绝对值的取值范围
    b_cons=cell2mat(b_cons_cell);%（求解方程）状态量不等式约束的取值

   % 状态量约束
    delta_Umin = kron(ones(Nc,1),delta_umin);
    delta_Umax = kron(ones(Nc,1),delta_umax);
    lb = [delta_Umin; 0];%（求解方程）状态量下界
    ub = [delta_Umax; 10];%（求解方程）状态量上界
    %尝试初始值
    %x0 = zeros(Nc*Nu+1,1);
    %% 开始求解过程
    %options = optimset('Algorithm','active-set');
    options = optimset('Algorithm','interior-point-convex');  
    warning off all  % close the warnings during computation   
    [X,~,~]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,[],options);

    %% 计算输出   
    u_dot(1)=X(1);
    u_dot(2)=X(2);
    U(1)=kesi(4)+u_dot(1);%用于存储上一个时刻的控制量
    U(2)=kesi(5)+u_dot(2);
    u_real(1) = U(1) + vd1;
    u_real(2) = U(2) + vd2;
    
    sys= [u_real(1)*3.6; u_real(2)*180/pi; u_real(2)*180/pi;0;0]; % vel, steering, x, y
    toc
    
% End of mdlOutputs.

薯一个蜂蜜牛奶味的愿

关注

65
点赞
踩
326

收藏

觉得还不错? 一键收藏
打赏
27
评论
【Carsim Simulink自动驾驶仿真】基于MPC的轨迹跟踪控制

项目介绍：教程为北理工的无人驾驶车辆模型预测控制第2版。所用的仿真软件为Carsim2020.0和MatlabR2021a。使用MPC控制思想对车辆进行轨迹跟踪控制，并给出仿真结果。从网上下载下来的代码运行不了，所以本文的代码是经过修改和调试的。
复制链接

扫一扫