【路径规划】基于UKF和MPC实现无人机编队路径避碰matlab源码

一、无迹卡尔曼滤波

无迹卡尔曼滤波不同于扩展卡尔曼滤波，它是概率密度分布的近似，由于没有将高阶项忽略，所以在求解非线性时精度较高。

UT变换的核心思想：近似一种概率分布比近似任意一个非线性函数或非线性变换要容易。

原理：

假设n维随机向量x:N(x均值，Px)，x通过非线性函数y=f(x)变换后得到n维的随机变量y。通过UT变换可以比较高的精度和较低的计算复杂度求得y的均值和方差Px。

UT的具体过程如下：

（1）计算2n+1个Sigma点及其权值：

根号下为矩阵平方根的第i列

依次为均值、方差的权值

式中：

α决定Sigma点的散步程度，通常取一小的正值；k通常取0；β用来描述x的分布信息，高斯情况下，β的最优值为2。

（2）计算Sigma点通过非线性函数f()的结果：

从而得知

由于x的均值和方差都精确到二阶，计算得到y的均值和方差也精确到二阶，比线性化模型精度更高。

二、MPC

三、部分代码

%% ALLAH
​
function [t,x,u] = uav_model_ekf
%%  global parameters
global T mpciterations
​
%%  inputs
mpciterations = 20;
N             = 11;
T             = 6;
tmeasure      = 0.0;
xmeasure      = [-1500,50,0,-1500,270,0,-1500,400,0];
u0            = 0*ones(9,N);
​
%%  solve mpc problem
[t,x,u] = mpc_control(@runningcosts, @terminalcosts, @constraints, ...
         @terminalconstraints, @linearconstraints, @system, ...
         mpciterations, N, T, tmeasure, xmeasure, u0);
end
​
function dx = system(t,x,u,T)
global x_pre
x_pre = x;
V = 100;
y_psi_1 = atan2(x(2),x(1));
y_psi_2 = atan2(x(5),x(4));
y_psi_3 = atan2(x(8),x(7));
dx(1) = V*cos(y_psi_1) + u(1);
dx(2) = V*sin(y_psi_1) + u(2);
dx(3) = u(3);
dx(4) = V*cos(y_psi_2) + u(4);
dx(5) = V*sin(y_psi_2) + u(5);
dx(6) = u(6);
dx(7) = V*cos(y_psi_3) + u(7);
dx(8) = V*sin(y_psi_3) + u(8);
dx(9) = u(9);
end
​
function cost = runningcosts(t, x, u)
%   inputs
    h_pi = 10;
    zeta_ob = 400;
    zeta_t = 1;
    a = 0.5;
    b = 0.5;
    n = 3;
    Rs = 50;
    Gamma = eye(9)/1e6;
    %   call obstacle function
    [x_obs,y_obs,z_obs] = obstacle_function(t);
    %   call target function
    [xt,yt,zt] = target_function;
    %   estimate states with ekf
    q = 0.1;
    r = 0.1;
    Q = q^2*eye(9);
    R = r^2;
    f = @(x)(x);
    h = @(x)(x(1));
    z = h(ones(9,1));
    P = eye(9);
    xhat = ekf(f,reshape(x,[],1),P,h,z,Q,R);
    %   distance to the obstacles
    d1_x = xhat(1) - x_obs;
    d1_y = xhat(2) - y_obs;
    d1_z = xhat(3) - z_obs;
    d2_x = xhat(4) - x_obs;
    d2_y = xhat(5) - y_obs;
    d2_z = xhat(6) - z_obs;
    d3_x = xhat(7) - x_obs;
    d3_y = xhat(8) - y_obs;
    d3_z = xhat(9) - z_obs;
    d1 = sqrt(d1_x.^2+d1_y.^2+d1_z.^2);
    d2 = sqrt(d2_x.^2+d2_y.^2+d2_z.^2);
    d3 = sqrt(d3_x.^2+d3_y.^2+d3_z.^2);
    do(1) = min(d1);
    do(2) = min(d2);
    do(3) = min(d3);
    dt(1) = sqrt((x(1)-xt)^2+(x(2)-yt)^2+(x(3)-zt)^2);
    dt(2) = sqrt((x(4)-xt)^2+(x(5)-yt)^2+(x(6)-zt)^2);
    dt(3) = sqrt((x(7)-xt)^2+(x(8)-yt)^2+(x(9)-zt)^2);
    %   J0
    Jo = 0;
    for tau = 1:h_pi
        for i = 1:n
            if do(i) < Rs
                pe = 1;
            else
                pe = 0;
            end
        end
        Jo = Jo + pe;
    end
    Jo = zeta_ob*Jo;
    %   Jt
    Jt1 = 0;
    for i = 1:n
        for tau = 1:h_pi
            Jt1 = Jt1 + zeta_t*dt(i);
        end
    end
    Jt2 = 0;
    for i = 1:n
        for j = 1:n
            if j ~= i
                for tau = 1:h_pi
                    Jt2 = Jt2 + abs(dt(i) - dt(j));
                end
            end
        end
    end


    Jt = a*Jt1 + b*Jt2;
    %   Ju
    Ju = u'*Gamma*u;
    %   total cost
    cost = Jo + Jt + Ju;
end
​
function cost = terminalcosts(t, x)
    cost = 0.0;
end
​
function [c,ceq] = constraints(t, x, u)
    global T x_pre
    dx = system(t,x,u,T);
    c = [];
    ceq = x - x_pre - dx*T;
end
​
function [c,ceq] = terminalconstraints(t, x)
    c = [];
    ceq = [];
end
​
function [A, b, Aeq, beq, lb, ub] = linearconstraints(t, x, u)
    A   = [];
    b   = [];
    Aeq = [];
    beq = [];
    lb  = [];
    ub  = [];
end

四、仿真结果

4 参考文献

[1] Saito M , Yamakita M . MPC for System with Backlash using UKF[C]// Japan Joint Automatic Control Conference. The Japan Joint Automatic Control Conference, 2006.

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

部分理论引用网络文献，若有侵权联系博主删除。