【路径规划】基于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

四、仿真结果

五、参考文献

[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.

六、代码下载