【路徑規(guī)劃】基于UKF和MPC實(shí)現(xiàn)無(wú)人機(jī)編隊(duì)路徑避碰matlab源碼
?一、無(wú)跡卡爾曼濾波
無(wú)跡卡爾曼濾波不同于擴(kuò)展卡爾曼濾波，它是概率密度分布的近似，由于沒(méi)有將高階項(xiàng)忽略，所以在求解非線性時(shí)精度較高。
UT變換的核心思想：近似一種概率分布比近似任意一個(gè)非線性函數(shù)或非線性變換要容易。
原理：
假設(shè)n維隨機(jī)向量x:N(x均值，Px)，x通過(guò)非線性函數(shù)y=f(x)變換后得到n維的隨機(jī)變量y。通過(guò)UT變換可以比較高的精度和較低的計(jì)算復(fù)雜度求得y的均值和方差Px。
UT的具體過(guò)程如下：
（1）計(jì)算2n+1個(gè)Sigma點(diǎn)及其權(quán)值：
? 根號(hào)下為矩陣平方根的第i列
? 依次為均值、方差的權(quán)值
?式中：
?α決定Sigma點(diǎn)的散步程度，通常取一小的正值；k通常取0；β用來(lái)描述x的分布信息，高斯情況下，β的最優(yōu)值為2。
（2）計(jì)算Sigma點(diǎn)通過(guò)非線性函數(shù)f()的結(jié)果：
從而得知
由于x的均值和方差都精確到二階，計(jì)算得到y(tǒng)的均值和方差也精確到二階，比線性化模型精度更高。
二、MPC
模型預(yù)測(cè)控制是一種基于模型的閉環(huán)優(yōu)化控制策略。
預(yù)測(cè)控制算法的三要素：內(nèi)部（預(yù)測(cè)）模型、參考軌跡、控制算法?，F(xiàn)在一般則更清楚地表述為內(nèi)部（預(yù)測(cè)）模型、滾動(dòng)優(yōu)化、反饋控制。?
大量的預(yù)測(cè)控制權(quán)威性文獻(xiàn)都無(wú)一例外地指出, 預(yù)測(cè)控制最大的吸引力在于它具有顯式處理約束的能力, 這種能力來(lái)自其基于模型對(duì)系統(tǒng)未來(lái)動(dòng)態(tài)行為的預(yù)測(cè), 通過(guò)把約束加到未來(lái)的輸入、輸出或狀態(tài)變量上, 可以把約束顯式表示在一個(gè)在線求解的二次規(guī)劃或非線性規(guī)劃問(wèn)題中.?
模型預(yù)測(cè)控制具有控制效果好、魯棒性強(qiáng)等優(yōu)點(diǎn)，可有效地克服過(guò)程的不確定性、非線性和并聯(lián)性，并能方便的處理過(guò)程被控變量和操縱變量中的各種約束。
三、部分代碼
%% ?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
四、仿真結(jié)果?
?
