【MPC】追踪锥形螺旋：深蓝路径规划课程P8作业

课程笔记：

【模型预测控制MPC】：深蓝路径规划课程P8笔记-CSDN博客

一、作业描述

之前，我们已经讨论了如何设计一个基于二次规划的MPC，以允许单轴三积分器从任意状态移动到状态空间的中心，a.k.a的位置、速度和加速度为零。

现在请设计一个基于二次规划的MPC来跟踪一个三轴三积分器（基本上是一个四旋翼模型）的圆锥形螺旋：

二、MATLAB代码设计 

课程里面可以参考的MPC代码设计，先读懂这个：

自己设计代码：

初始条件：

p_0 = [0 8 20];v_0 = [0 0 0];a_0 = [0 0 0];

MPC向前预测4秒，分为20步，每次步0.2s，并更新PVA：

K=20;dt=0.2;

P=[];V=[];A=[];

生成参考信号：

for t=0.2:0.2:40
    %% Construct the reference signal
    % 理想的参考信号
    for i = 1:20
        tref = t + i*0.2;
        r=0.25*tref;
        w = 0.7;
        pt(i,1) = r*sin(w*tref);
        vt(i,1) = r*cos(w*tref);
        at(i,1) = -r*sin(w*tref);
        
        pt(i,2) = r*cos(w*tref);
        vt(i,2) = -r*sin(w*tref);
        at(i,2) = -r*cos(w*tref);
        
        pt(i,3) = 20 - 0.5*tref;
        vt(i,3) = -0.5;
        at(i,3) = 0;
    end

X、Y轴上限制条件相同是对称的，其MPC函数为：

function j = xy_axis_mpc(K,dt,p_0,v_0,a_0,pt,vt,at)
w1 = 100;
w2 = 1;
w3 = 1;
w4 = 1;
%% Construct the prediction matrix
[Tp, Tv, Ta, Bp, Bv, Ba] = getPredictionMatrix(K,dt,p_0,v_0,a_0);

%% Construct the optimization problem
H = w4*eye(K)+w1*(Tp'*Tp)+w2*(Tv'*Tv)+w3*(Ta'*Ta);
F = w1*(Bp-pt)'*Tp+w2*(Bv-vt)'*Tv+w3*(Ba-at)'*Ta;

A = [Tv;-Tv;Ta;-Ta];
b = [6*ones(20,1)-Bv;6*ones(20,1)+Bv;3*ones(20,1)-Ba;3*ones(20,1)+Ba];
%% Solve the optimization problem
J = quadprog(H,F,A,b,[],[],-3*ones(20,1),3*ones(20,1));

%% Apply the control
j = J(1);

Z轴MPC函数定义：

function j = z_axis_mpc(K,dt,p_0,v_0,a_0,pt,vt,at)
w1 = 100;
w2 = 1;
w3 = 1;
w4 = 1;
%% Construct the prediction matrix
[Tp, Tv, Ta, Bp, Bv, Ba] = getPredictionMatrix(K,dt,p_0,v_0,a_0);

%% Construct the optimization problem
H = w4*eye(K)+w1*(Tp'*Tp)+w2*(Tv'*Tv)+w3*(Ta'*Ta);
F = w1*(Bp-pt)'*Tp+w2*(Bv-vt)'*Tv+w3*(Ba-at)'*Ta;

A = [Tv;-Tv;Ta;-Ta];
b = [6*ones(20,1)-Bv;1*ones(20,1)+Bv;3*ones(20,1)-Ba;1*ones(20,1)+Ba];
%% Solve the optimization problem
J = quadprog(H,F,A,b,[],[],-2*ones(20,1),2*ones(20,1));

%% Apply the control
j = J(1);
end

执行MPC：

function j = z_axis_mpc(K,dt,p_0,v_0,a_0,pt,vt,at)
w1 = 100;
w2 = 1;
w3 = 1;
w4 = 1;
%% Construct the prediction matrix
[Tp, Tv, Ta, Bp, Bv, Ba] = getPredictionMatrix(K,dt,p_0,v_0,a_0);

%% Construct the optimization problem
H = w4*eye(K)+w1*(Tp'*Tp)+w2*(Tv'*Tv)+w3*(Ta'*Ta);
F = w1*(Bp-pt)'*Tp+w2*(Bv-vt)'*Tv+w3*(Ba-at)'*Ta;

A = [Tv;-Tv;Ta;-Ta];
b = [6*ones(20,1)-Bv;1*ones(20,1)+Bv;3*ones(20,1)-Ba;1*ones(20,1)+Ba];
%% Solve the optimization problem
J = quadprog(H,F,A,b,[],[],-2*ones(20,1),2*ones(20,1));

%% Apply the control
j = J(1);
end

根据MPC生成的点前进：

function [p_0,v_0,a_0] = forward(p_0,v_0,a_0,j,dt)
p_0 = p_0 + v_0*dt + 0.5*a_0*dt^2 + 1/6*j*dt^3;
v_0 = v_0 + a_0*dt + 0.5*j*dt^2;
a_0 = a_0 + j*dt;


    for i=1:3
       [p_0(i),v_0(i),a_0(i)] = forward(p_0(i),v_0(i),a_0(i),j(i),dt);
    end

记录点：

    %% Log the states
    P = [P;p_0 pt(1,:)];
    V = [V;v_0 vt(1,:)];
    A = [A;a_0 at(1,:)];

 生成图像：

%% Plot the result
plot(P);
grid on;
legend('x','y','z');
figure;
plot3(P(:,1),P(:,2),P(:,3));
axis equal;
grid on;

三、结果