第七篇（下），MPC工程化总结

function y = Fcn_MPC(Ca_f, Ca_r, mass_f, mass_r, WheelBase, VehicleSpeed_mps, ...
                     LatErr, LatErr_d, AngleErr, AngleErr_d, ...
                     ts)

WholeMass = mass_f + mass_r;
l_f = WheelBase * (1 - mass_f/WholeMass);
l_r = WheelBase * (1 - mass_r/WholeMass);
Iz = l_f*l_f*mass_f + l_r*l_r*mass_r;

dim_state = 4; %状态4个
dim_ctrl = 1; %控制1个

A = zeros(4,4);
A(1,2) = 1;
A(2,2) = (Ca_f+Ca_r) / WholeMass / VehicleSpeed_mps * -1;
A(2,3) = (Ca_f+Ca_r) / WholeMass;
A(2,4) = (Ca_r*l_r-Ca_f*l_f) / WholeMass / VehicleSpeed_mps;
A(3,4) = 1;
A(4,2) = (Ca_r*l_r-Ca_f*l_f) / Iz / VehicleSpeed_mps;
A(4,3) = (Ca_f*l_f-Ca_r*l_r) / Iz;
A(4,4) = (Ca_f*l_f*l_f+Ca_r*l_r*l_r) / Iz / VehicleSpeed_mps * -1;
A = (eye(4)-ts*0.5*A) \ (eye(4)+ts*0.5*A); % Apollo的双线性离散化

B = [0 0 0 0]';
B(2) = Ca_f / WholeMass;
B(4) = Ca_f * l_f / Iz;
B = B * ts;

Q = zeros(4,4);
Q(1,1) = 2;
Q(2,2) = 2;
Q(3,3) = 1;
Q(4,4) = 1;

R = 0.1;

% 当前状态量
x0 = [LatErr, LatErr_d, AngleErr, AngleErr_d]';

% 预测步长
Np=10;

% 增广
A_ext = zeros(dim_state*Np, dim_state);
B_ext = zeros(dim_state*Np, Np);
Q_ext = zeros(dim_state*Np);
R_ext = zeros(dim_ctrl*Np);
for i=1:1:Np
    At_tmp = A^i;
    for row = 1 : 1 : dim_state
        for col = 1 : 1 : dim_state
            A_ext(dim_state*(i-1)+row, col) = At_tmp(row,col);
        end
    end

    for j=1:1:i
        Bt_tmp = (A^(i-j)) * B;
        for row = 1 : 1 : dim_state
            B_ext(dim_state*(i-1)+row, j) = Bt_tmp(row,1);
        end
    end

    for row = 1 : 1 : dim_state
        for col = 1 : 1 : dim_state
            Q_ext(dim_state*(i-1)+row, dim_state*(i-1)+col) = Q(row,col); % Qt/Rt是对角Q/R阵，这里用row_Q其实代表方阵维数，QR都是方阵
        end
    end

    for row = 1 : 1 : dim_ctrl
        for col = 1 : 1 : dim_ctrl
            R_ext(dim_ctrl*(i-1)+row, dim_ctrl*(i-1)+col) = R(row,col); % Qt/Rt是对角Q/R阵，这里用row_Q其实代表方阵维数，QR都是方阵
        end
    end
end

% 控制量ut的初始值
persistent p_u0;
if isempty(p_u0)
    p_u0 = zeros(Np,1);
end

% quadprog()所需参数
H = 2*(B_ext'*Q_ext*B_ext + R_ext);
f = (2*x0'*A_ext'*Q_ext*B_ext)';
qp_A = [B_ext; -B_ext];
state_constr = zeros(dim_state*Np, 1); % state_constr也要做增广，以匹配公式中的维度
constr_tmp = [0.5 1 0.5/180*pi 1/10]';
for i = 1 : 1 : Np
    for j = 1 : 1 : dim_state
        state_constr(dim_state*(i-1)+j) = constr_tmp(j);
    end
end
b = [state_constr-A_ext*x0; state_constr+A_ext*x0];% b是根据对状态的约束得来的，
                                                   % [0.5 1 0.5/180*pi 1/10]，即横向误差约束到半米以内、变化率1m/s以内、
                                                   % HandingAngleError(也就是HeadingAngle)在0.5°以内、HeadingAngleErrorRate在0.1rad以内
lb = -20/180*pi*ones(Np,1); % 控制量ut的上下限，即方向盘转角
ub = 20/180*pi*ones(Np,1); % ±20°
opts = optimoptions('quadprog','Algorithm','active-set');
% quadprog()所需参数

% solve qp problem
[u, fval, exitflag]=quadprog(H, f, qp_A, b, [], [], lb, ub, p_u0, opts);

p_u0 = [u(2:Np); u(Np)];

y = u(1);