基于模型预测控制MPC的无人机轨迹跟踪

模型预测控制MPC

算法主要思想是用优化的方法解决控制和规划问题。 MPC主要分为三部分

建模：系统建模和问题建模 预测：参数空间 状态空间 输入空间 控制：选择最好的策略

算法过程

系统建模 问题建模 优化 控制

基本的MPC过程用MATLAB代码实现

主函数 hw8.m

clear all;
clc;
close all;

//初始化状态
p_0 = [0 8 20];
v_0 = [0 0 0];
a_0 = [0 0 0];
K = 20;
dt = 0.2;
omega = 0.5;
velocity = 0.5;

P=[];
V=[];
A=[]; 
k=0;
//离散时间总时间T为：dt=0.2s
for t=0.2:0.2:40
    k = k+1;
    //预测过程 同样离散时间 ，建立tracking曲线，包含曲线各点的pva信息
    //参考线为Z向速度恒定的圆锥螺旋曲线
    // x = r sin(wt)  y = r cos(wt) z =20 - 0.5t
    //代码中的 v  a 要求导得到 
    for i= 1:20
        tref = t + 0.2 * i;
        
        r= 0.25*tref;
        
        pt(i,1) = r * sin(omega * tref);
        vt(i,1) = omega * r * cos(omega * tref);
        at(i,1) = -omega * omega * r * sin(omega * tref);
        
        pt(i,2) = r * cos(omega * tref);
        vt(i,2) = omega * r * sin(omega * tref);
        at(i,2) = -omega * omega * r * cos(omega * tref);
        
        pt(i,3) = 20 - velocity * tref;
        vt(i,3) = -velocity;
        at(i,3) = 0;
    end
    //再xyz三个维度求解MPC
    j(1) = getMPC(K,dt,p_0(1),v_0(1),a_0(1),pt(:,1),vt(:,1),at(:,1));
    j(2) = getMPC(K,dt,p_0(2),v_0(2),a_0(2),pt(:,2),vt(:,2),at(:,2));
    j(3) = getMPC(K,dt,p_0(3),v_0(3),a_0(3),pt(:,3),vt(:,3),at(:,3));
    //控制部分 将得到的最优j推到pva信息 
    for i =1:3
        p_0(i)  = p_0(i)  +v_0(i) *dt + 0.5*a_0(i) *dt^2 + 1/6*j(i) *dt^3;
        v_0(i)  = v_0(i)  +a_0(i) *dt + 0.5*j(i) *dt^2;
        a_0(i)  = a_0(i) +j(i) *dt;
    end
    //记录pva 以便后来画出曲线
    P = [P;k p_0 pt(1,:)];
    V = [V;k v_0 vt(1,:)];
    A = [A;k a_0 at(1,:)];
end
//画图
figure
scatter3(P(:,5),P(:,6),P(:,7));
hold on
plot3(P(:,2),P(:,3),P(:,4),--);
legend(refierence,tracking);

figure
plot(P(:,1),P(:,2),-,P(:,1),P(:,5),--);
legend(x,xref)
figure
plot(P(:,1),P(:,3),-,P(:,1),P(:,6),--);
legend(y,yref)
figure
plot(P(:,1),P(:,4),-,P(:,1),P(:,7),--);
legend(z,zref)

figure
plot(V(:,1),V(:,2),V(:,1),V(:,5));
legend(vx,vxref);
figure
plot(V(:,1),V(:,3),V(:,1),V(:,6));
legend(vy,vyref);
figure
plot(V(:,1),V(:,4),V(:,1),V(:,7));
legend(vz,vzref);

figure
plot(A(:,1),A(:,2),A(:,1),A(:,5));
legend(ax,axref);
figure
plot(A(:,1),A(:,3),A(:,1),A(:,6));
legend(ay,ayref);
figure
plot(A(:,1),A(:,4),A(:,1),A(:,7));
legend(az,zxref);

getMPC函数

function j = getMPC(K,dt,p_0,v_0,a_0,pt,vt,at)
   //H矩阵的权重值
    w1 = 100;
    w2 = 1;
    w3 = 1;
    w4 = 1;
	//预测矩阵
    [Tp,Tv,Ta,Bp,Bv,Ba] = getPredictioinMatrix(K,dt,p_0,v_0,a_0);
	//构建H F A b 这里把约束条件vmax amax带入进b
    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];
   
   //二次规划求解 
    J = quadprog(H,F,A,b);
    j=J(1);
end

预测矩阵函数 getPrectioninMAtrix.m

function [Tp, Tv, Ta, Bp, Bv, Ba] = getPredictioinMatrix(K,dt,p_0,v_0,a_0)
Ta = zeros(K);
Tv = zeros(K);
Tp = zeros(K);

for i=1:K
    Ta(i,1:i)=ones(1,i)*dt;
end

for i=1:K
    for j=1:i
        Tv(i,j) = (i-j+0.5)*dt^2;
    end
end

for i=1:K
    for j=1:i
        Tp(i,j) = ((i-j+1)*(i-j)/2 + 1/6) * dt^3;
    end
end

Ba = ones(K,1)*a_0;
Bv = ones(K,1)*v_0;
Bp = ones(K,1)*p_0;

for i=1:K
    Bv(i) = Bv(i) + i*dt*a_0;
    Bp(i) = Bp(i) + i*dt*v_0+i^2/2*a_0*dt^2;
end

结果