代码详解——NMPC之加入控制平顺性惩罚项

clear all;
%%车辆参数初始化
l=1;%轴距
global v1;
global deltaf1;
v1=1;
deltaf1=0;
%% 控制参数初始化
Nx=3;%状态量个数
Np=25;%预测时域
Nc=3;%控制时域
%% 车辆位置初始化
State_Initial(1,1)=0;%x
State_Initial(2,1)=0;%y
State_Initial(3,1)=pi/6;%phi
%% 参考轨迹参数初始化
N=1000;%参考轨迹点数量
T=0.05;%采样周期
global Xrefg;
global Yrefg;
global PHIrefg;
%%参考轨迹生成
for k=1:1:N+Np
    if k<200
        Xrefg(k,1)=k*T;  %横坐标
        Yrefg(k,1)=2;  %纵坐标
        PHIrefg(k,1)=0;  %航向角
    elseif k<514
        Xrefg(k,1)=10+10*sin( 0.1*(k-200)*T);
        Yrefg(k,1)=12-10*cos(0.1*(k-200)*T);
        PHIrefg(k,1)=0.1*(k-200)*T;
    elseif k<714
        Xrefg(k,1)=20;
        Yrefg(k,1)=12+(k-514)*T;
        PHIrefg(k,1)=1.57;
    else
        Xrefg(k,1)=20;
        Yrefg(k,1)=22;
        PHIrefg(k,1)=1.57;
    end
end
%% 开始仿真
for j=1:1:N-300
    %读取参考路径
    Xref=zeros(Np,1);
    Yref=zeros(Np,1);
    PHIref=zeros(Np,1);
    for Nref=1:1:Np
        Xref(Nref,1)=Xrefg(j+Nref-1,1);
        Yref(Nref,1)=Yrefg(j+Nref-1,1);
        PHIref(Nref,1)=PHIrefg(j+Nref-1,1);
    end
    %%约束条件
    for i=1:1:Nc
        lb(2*i-1)=0.8;
        lb(2*i)=-0.44;
        ub(2*i-1)=1.2;
        ub(2*i)=0.44;
    end
    %%选取求解算法
    options = optimset('Algorithm','active-set'); %选择active-set为求解算法
    %%求解算法预留
    A=[];
    b=[];
    Aeq=[];
    beq=[];
    %%求解
    [A,fval,exitflag]=fmincon(@(x)NMPC(x,State_Initial,Np,Nc,T,Xref,Yref,PHIref,v1,deltaf1),[0;0;0;0;0;0],A,b,Aeq,beq,lb,ub,[],options);%有约束求解，需要有2*Nc个0
    %%获得控制输入
    v_actual=A(1);
    deltaf_actual=A(2);
    v1=v_actual;
    deltaf1=deltaf_actual;
    %%车辆位置代入
    X00(1)=State_Initial(1,1);
    X00(2)=State_Initial(2,1);
    X00(3)=State_Initial(3,1);
    %%代入控制输入后，解算下一时刻车辆位置
    Xref=dsolve('Dx-v_actual*cos(z)=0','Dy-v_actual*sin(z)=0','Dz-v_actual*tan(deltaf_actual)/l=0','x(0)=X00(1)','y(0)=X00(2)','z(0)=X00(3)');
    t=T;
    %%更新车辆位置
    State_Initial(1,1)=eval(Xref.x);
    State_Initial(2,1)=eval(Xref.y);
    State_Initial(3,1)=eval(Xref.z);
    %%绘图
    figure(1)
    plot(State_Initial(1,1),State_Initial(2,1),'b*');
    hold on;
    plot(Xrefg(j,1),Yrefg(j,1),'ro');
    hold on;
    axis([-5 25 -5 25])
end
NMPC.m更新为：
function cost = NMPC(x,State_Initial,Np,Nc,T,Xref,Yref,PHIref,v1,delta_f1)
%%车辆长度
l=1;
%%预测时域内初始位置代入
X=State_Initial(1,1);
Y=State_Initial(2,1);
PHI=State_Initial(3,1);
%%循环，获得预测位姿
for i=1:1:Np
    if i==1
        v(i,1)=x(1);
        delta_f(i,1)=x(2);
        dv(i,1)=v(i,1)-v1;
        ddelta_f(i,1)=delta_f(i,1)-delta_f1;
        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
        if i<Nc
            v(i,1)=x(2*i-1);
            delta_f(i,1)=x(2*i);
        else
            v(i,1)=x(2*Nc-1);
            delta_f(i,1)=x(2*Nc);
        end
        dv(i,1)=v(i,1)-v(i-1,1);
        ddelta_f(i,1)=delta_f(i,1)-delta_f(i-1,1);
        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
    %%解算预测位姿和参考轨迹的误差
    X_error(i,1)=X_predict(i,1)-Xref(i,1);
    Y_error(i,1)=Y_predict(i,1)-Yref(i,1);
    PHI_error(i,1)=PHI_predict(i,1)-PHIref(i,1);
end
%cost=Y_error'*Y_error+X_error'*X_error; 也可以不加航向误差
cost=Y_error'*Y_error+X_error'*X_error+0.01*PHI_error'*PHI_error+0.1*dv'*dv+0.1*ddelta_f'*ddelta_f;
end