基于MPC的智能小车轨迹跟踪控制

基于MPC的智能小车轨迹跟踪控制

引用

CSDN

1.

https://blog.csdn.net/lcX301415926535/article/details/139686456

本文介绍了一种基于模型预测控制（MPC）的智能小车轨迹跟踪控制方法。通过样条插值法生成参考轨迹，并在MATLAB中实现MPC控制器，成功实现了小车对参考轨迹的跟踪。文章详细介绍了算法原理、代码实现及实验结果，适合对控制理论感兴趣的读者学习参考。

本次实验仿真采用样条插值法生成参考轨迹，样条插值（Spline Interpolation）即使用spline函数对给定点集point进行样条插值。样条插值是一种通过一系列给定的点来生成平滑曲线的方法。在这里，spline函数被用于对point中的x和y坐标分别进行插值，生成更密集的点的坐标，这些点位于原始点之间，从而形成了平滑的曲线，即形成参考轨迹。

图2.1 样条插值生成参考轨迹

随后把生成的参考轨迹导入MATLAB中，在主循环里把控制输入量v和航向角delta输入控制器进行控制输入计算：在每个控制周期，根据当前时刻的参考轨迹点和当前小车的状态输出量x、y和姿态角yaw，控制器通过优化未来的轨迹预测，考虑小车的动力学模型和可能的约束，来确定最优的控制输入；MPC控制器不仅计算当前时刻的控制输入，还预测未来一段时间内的轨迹。其通过离散时间步的滚动预测，其中每一步都基于当前的控制输入和系统模型来预测下一时刻的状态。即把离散的参考轨迹滚动计算得到一段时间内的预测轨迹，达到控制小车实现循迹效果，如图2.2所示，细的曲线为参考轨迹，粗的曲线为小车实际行走的轨迹，可以看出：实际行走的轨迹与参考轨迹基本重合，说明MPC算法对于小车的轨迹跟踪控制是有效的。

图2.2 轨迹跟踪展示

下面是具体的代码实现：

path_design.m

%design path from points by spline
point = [0, 0;
    10, 5;
    20,10;
    30,15;
    40,20;
    48, 28;
    50,40;
    65, 55;
    73, 61;
    83, 70;
    86,75;
    90,81;
    90,90];
s = 1:1:length(point);
px_spline = spline(s, point(:,1), 1:0.01:length(point));
py_spline = spline(s, point(:,2), 1:0.01:length(point));
p_spline = [px_spline', py_spline'];
%insert yaw
yaw = zeros(length(px_spline), 1);
for i = 2:length(px_spline)-1
    x_forward = px_spline(i+1);
    x_backward = px_spline(i-1);
    y_forward = py_spline(i+1);
    y_backward = py_spline(i-1);
    yaw(i) = atan2(y_forward-y_backward, x_forward-x_backward);
end
yaw(1) = yaw(2);
yaw(end) = yaw(end-1);
%plot with attitude
arrow_scale = 0.01;
figure(101);
plot(point(:,1), point(:,2), 'bo-'); hold on;
quiver(px_spline', py_spline', cos(yaw)*arrow_scale, sin(yaw)*arrow_scale);
plot(px_spline, py_spline,'r-'); grid on; hold off;
%save path
path = [px_spline', py_spline', yaw];
save('path', 'path')  

main.m

clc
clear
close all
load path.mat
%%初始参数
Kp=1.0;
dt=0.1; %时间步长
L=2.9;  %轴距
max_steer=60*pi/180;
target_v=10.0/3.6;
%%参考轨迹的相关参数
%定义参考轨迹
refPos_x=path(:,1);
refPos_y=path(:,2);
refPos=[refPos_x,refPos_y];
%计算航向角和曲率
diff_x=diff(refPos_x);
diff_x(end+1)=diff_x(end);
diff_y=diff(refPos_y);
diff_y(end+1)=diff_y(end);
derivative1=gradient(refPos_y)./abs(diff_x);    %计算一阶导数
derivative2=del2(refPos_y)./abs(diff_x);        %计算二阶导数
refHeading=atan2(diff_y,diff_x);                %航向角
refK=abs(derivative2)./(1+derivative1.^2).^(3/2);   %计算曲率
refPos_x=refPos_x'; %转置
refPos_y=refPos_y';
refPos_yaw=atan(derivative1');
refPos_k=refK';
%绘图
figure
plot(refPos_x,refPos_y,'r-');
hold on
%主程序
x=0.1;
y=-0.1;
yaw=0.1;
v=0.1;
U=[0.01;0.01];
ind=0;
pos_actual=[x;y];
while ind<length(refPos_x)
    %调用MPC控制器
    [Delta,v,ind,e,U]=mpc_control(x,y,yaw,refPos_x,refPos_y,refPos_yaw,refPos_k,dt,L,U,target_v);
    
    %误差太大，退出程序
    if abs(e)>3
        fprintf('误差过大，退出程序！\n')
        break
    end
    %选择P控制器
    a=Kp*(target_v-v);
    %更新状态量
    [x,y,yaw,v]=updateState(x,y,yaw,v,a, Delta,dt,L,max_steer);
    % pos_actual(end+1,:)=[x,y];
   plot(x,y,'bo')
   pause(0.01);
end   
%保存
path_MPC=pos_actual;
save path_MPC.mat path_MPC  

mpc_control.m

function [Delta_real,v_real,idx,error_y,U]=mpc_control(x,y,yaw,refPos_x,refPos_y,refPos_yaw,refPos_k,dt,L,U,target_v)
%%MPC预设参数
Nx=3;   %状态量个数
Nu=2;   %控制量个数
Np=60;  
Nc=30;
row=10;
Q=100*eye(Np*Nx);
R=1*eye(Nc*Nu);
%控制量约束条件
umin=[-0.2;-0.54;];%维数与控制变量的个数相同(速度与前轮转角的最大/小值)
umax=[0.2;0.332];
delta_umin=[-0.05;-0.64;];
delta_umax=[0.05;0.64];
%%原运动学误差状态空间方程的相关矩阵
%计算参考控制量
[idx,error_y]=calc_target_index(x,y,refPos_x,refPos_y);
curvature=refPos_k(idx);
Delta_r=atan(L*curvature);  %参考前轮转角
v_r=target_v;
%实际状态量与参考状态量
X_real=[x,y,yaw];
Xr=[refPos_x(idx),refPos_y(idx),refPos_yaw(idx)];
%a,b两个矩阵
a=[1    0   -v_r*sin(yaw)*dt;
   0    1   v_r*cos(yaw)*dt;
   0    0   1;];
b = [cos(yaw)*dt   0;     
    sin(yaw)*dt   0;      
    tan(yaw)*dt/L   v_r*dt/(L*(cos(Delta_r)^2))];
%%新的状态空间方程的相关矩阵
%新的状态量
    kesi=zeros(Nx+Nu,1);
    kesi(1:Nx)=X_real-Xr;
    kesi(Nx+1:end)=U;
    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=[eye(Nx),zeros(Nx,Nu)];
  %PHI矩阵
    PIH_cell=cell(Np,Nc);
    for i=1:Np
        PIH_cell{i,1}=C*A^i;
    end
    PHI=cell2mat(PIH_cell);
   %THETA矩阵
   THETA_cell=cell(Np,Nc);
   for i=1:Np
       for j=1:Nc
           if j<=i
               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(Nu*Nc,1);
    H_cell{2,1}=zeros(1,Nu*Nc);
    H_cell{2,2}=row;%松弛因子加入实现最优解
    H=cell2mat(H_cell);
%E矩阵
     E=PHI*kesi;
%g矩阵     
    g_cell=cell(1,1);
    g_cell{1,1}=E'*Q*THETA;
    g_cell{1,2}=0;
    g=cell2mat(g_cell);
%%约束 条件相关矩阵
%A_I矩阵
A_I=zeros(Nc,Nc);
for i=1:Nc
    A_I(i,1:i)=1;
end
A_I=kron(A_I,eye(Nu));
 %Ut矩阵
 Ut=kron(ones(Nc,1),U);
 %控制量与控制量变化量的约束
  Umin=kron(ones(Nc,1),umin);
  Umax=kron(ones(Nc,1),umax);
  delta_Umin=kron(ones(Nc,1),delta_umin);
  delta_Umax=kron(ones(Nc,1),delta_umax);
  %用于quadprog函数不等式约束Ax<=b的矩阵A
  A_cons_cell={A_I,zeros(Nu*Nc,1);
     -A_I,zeros(Nu*Nc,1) };
  A_cons=cell2mat(A_cons_cell);
  %向量b
  b_cons_cell={Umax-Ut;
     -Umin+Ut}; 
  b_cons=cell2mat(b_cons_cell);
  %deltaU的上下界约束
  lb=[delta_Umin;0];%（求解方程）状态量下界，包含控制时域内控制增量和松弛因子
  ub=[delta_Umax;1];%（求解方程）状态量上界，包含控制时域内控制增量和松弛因子
  
  %%开始求解过程
  options=optimoptions('quadprog','Display','iter','MaxIterations',100,'TolFun',1e-10);
  delta_U=quadprog(H,g,A_cons,b_cons,[],[],lb,ub,[],options);
  %%计算输出
  delta_v_tilde=delta_U(1);
  delta_Delta_tilde=delta_U(2);
  %更新控制量
  U(1)=kesi(4)+delta_v_tilde;
  U(2)=kesi(5)+delta_Delta_tilde;
  v_real=U(1)+v_r;
  Delta_real=U(2)+Delta_r;
end

updateState.m

function [x,y,yaw,v]=updateState(x,y,yaw,v,a,Delta,dt,L,max_steer)
Delta=max(min(max_steer,Delta),-max_steer);
x=x+v*cos(yaw)*dt;
y=y+v*sin(yaw)*dt;
yaw=yaw+v/L*tan(Delta)*dt;
v=v+a*dt;
end  

calc_target_index.m

function [target_idx,error]=calc_target_index(x,y,refPos_x,refPos_y)
i=1:length(refPos_x)-1;
dist=sqrt((refPos_x(i)-x).^2+(refPos_y(i)-y).^2);
[value,target_idx]=min(dist);
if y<refPos_y(target_idx)
    error=-value;
else
    error=value;
end
end

参考文献：

• 无人驾驶车辆模型预测控制第二版
• 路径规划与轨迹跟踪系列算法学习_第13讲_模型预测控制(MPC)法_哔哩哔哩_bilibili