无人驾驶车辆模型预测控制 - 3_4_3

the comparison result of Np=15 and Np=25.

predict path when Np=25

clc;
clear all;
%% 参考轨迹生成
tic
Nx=3;%状态量个数
Np=25;%预测时域
Nc=2;%控制时域
l=1;
N=100;%参考轨迹点数量
T=0.05;%采样周期
Xref=zeros(Np,1);
Yref=zeros(Np,1);
PHIref=zeros(Np,1);

%% 对性能函数各参数的初始化
State_Initial=zeros(Nx,1);%state=[y_dot,x_dot,phi,Y,X],此处为给定初始值
State_Initial(1,1)=0;%x 
State_Initial(2,1)=0;%y
State_Initial(3,1)=pi/6;%phi

Q=100*eye(Np+1,Np+1);
R=100*eye(Np+1,Np+1);

%% 开始求解
for j=1:1:N
    lb=[0.8;-0.44;0.8;-0.44];
    ub=[1.2;0.44;1.2;0.44];
    A=[];
    b=[];
    Aeq=[];
    beq=[];
    for Nref=1:1:Np
        Xref(Nref,1)=(j+Nref-1)*T;
        Yref(Nref,1)=2;
        PHIref(Nref,1)=0;
    end
    x0=[0;0;0;0];
    options = optimset('Algorithm','active-set');
    [A,fval,exitflag]=fmincon(@(x)MY_costfunction(x,State_Initial,Np,Nc,T,Xref,Yref,PHIref,Q,R),x0,A,b,Aeq,beq,lb,ub,[],options);%有约束求解，但速度慢
    %[A,fval,exitflag]=fminbnd(@(x)MY_costfunction(x,State_Initial,Np,Nc,T,Yref,Q,R,S),lb,ub);%只有上下界约束，但容易陷入局部最小
    %[A,fval,exitflag]=fminsearch(@(x)MY_costfunction(x,State_Initial,Np,Nc,T,Xref,Yref,PHIref,Q,R),[0;0;0;0]);%无约束求解，速度最快
    v_actual=A(1);
    deltaf_actual=A(2);
    fval;
    exitflag;
    X00(1)=State_Initial(1,1);
    X00(2)=State_Initial(2,1);
    X00(3)=State_Initial(3,1);
    XOUT=dsolve('Dx-v_actual*cos(z)=0','Dy-v_actual*sin(z)=0','Dz-v_actual*tan(deltaf_actual)=0','x(0)=X00(1)','y(0)=X00(2)','z(0)=X00(3)');
    t=T;
    State_Initial(1,1)=eval(XOUT.x);
    State_Initial(2,1)=eval(XOUT.y);
    State_Initial(3,1)=eval(XOUT.z);
    
    figure(1)
    plot(State_Initial(1,1),State_Initial(2,1),'r*');
    axis([0 5 0 3]);
    hold on;
    plot([0,5],[2,2],'r-');
    hold on;
    
    figure(2)
    plot(j*T,v_actual,'b*');
    axis([0 5 0 2]);
    hold on;
    figure(3)
    plot(j*T, deltaf_actual*180/pi,'r*');
    axis([0 5 -35 35]);
    hold on;
 
end 
 toc

function cost = MY_costfunction(x,State_Initial,Np,Nc,T,Xref,Yref,PHIref,Q,R)
%MY_COSTFUNCTION 此处显示有关此函数的摘要
%   此处显示详细说明
cost=1;
l=1;
%============================
% initialize
%=============================
X=State_Initial(1,1);
Y=State_Initial(2,1);
PHI=State_Initial(3,1);

X_predict=zeros(Np,1);
Y_predict=zeros(Np,1);
PHI_predict=zeros(Np,1);

X_error=zeros(Np+1,1);
Y_error=zeros(Np+1,1);
PHI_error=zeros(Np+1,1);

v=zeros(Np,1);
delta_f=zeros(Np,1);
%===================
%state update
%===================
%x(1)=A(1);

for i=1:1:Np
    if i==1
        v(i,1)=x(1);
        delta_f(i,1)=x(2);
        X_predict(i,1)=X+T*v(i,1)*cos(PHI);
        Y_predict(i,1)=Y+T*v(i,1)*sin(PHI);
        PHI_predict(i,1)=PHI+T*v(i,1)*tan(delta_f(i,1))/l;
    else
        v(i,1)=x(3);%控制时域为2；
        delta_f(i,1)=x(4);
        X_predict(i,1)=X_predict(i-1)+T*v(i,1)*cos(PHI_predict(i-1));
        Y_predict(i,1)=Y_predict(i-1)+T*v(i,1)*sin(PHI_predict(i-1));
        PHI_predict(i,1)=PHI_predict(i-1)+T*v(i,1)*tan(delta_f(i,1))/l;
    end
%==============
%calcate trajectory error
%===============
    X_real=zeros(Np+1,1);
    Y_real=zeros(Np+1,1);
    X_real(1,1)=X;
    X_real(2:Np+1,1)=X_predict;
    Y_real(1,1)=Y;
    Y_real(2:Np+1,1)=Y_predict;
    X_error(i,1)=X_real(i,1)-Xref(i,1);
    Y_error(i,1)=Y_real(i,1)-Yref(i,1);
    PHI_error(i,1)=PHI_predict(i,1)-PHIref(i,1);
end
figure(1)
plot(X_real(1:Np+1),Y_real(1:Np+1),'g-');
hold on;
%=============
%calculate function value
%=============
cost=cost+Y_error'*R*Y_error+X_error'*Q*X_error;

end

查看速度

前轮转角

• 可以发现 ，速度和前轮转角存在阶跃，因为模型未对控制变量的增量进行限制。