集群仿真基于matlab固定翼无人机集群仿真演示平台含Matlab源码 1497期

Posted 2021-12-09 紫极神光

一、无人机集群仿真简介

一个小型无人机集群仿真演示平台，使用matlab和simulink搭建。
给出的例子是5架的，当然如果你愿意花时间，也可以把它扩展到10架，20架甚至更多。
输入：5架飞机的规划路径
输出：每架无人机每个时刻的13个状态量

13个状态量：
世界坐标系x,y,z方向上的位置
机体坐标系上的速度u,v,w
滚转角，俯仰角，偏航角
滚转角速率，俯仰角速率，偏航角速率
时间
2 平台搭建的初衷
最近有需要把协同控制的算法扩展到固定翼无人机上的需求，但是协同控制算法一般考虑的都是一阶二阶积分器，单车模型。即使考虑固定翼模型，也只是简单的固定翼动力学模型。
但是真实的固定翼无人机飞控模型是十分复杂的，具有很强的非线性。
那么我怎么证明我提出的固定翼协同控制算法，或者规划算法是有效的呢。此时就需要使用一个较为真实的固定翼飞控来模拟真实的无人机飞行状态。这就是我搭建这个平台的缘由。
其实Matlab是自带一个固定翼无人机的仿真工具的，但是官方给出的文档较少，用起来也不是很方便，动画展示也只能展示一架飞机，总之就是不好用。
搭建的代码主要参考兰德尔的《小型无人机理论与应用》，里面的飞控代码原理我完全不懂，我的工作就是把它们整合起来。并能够在一个画面里面展示多架。

3.关于怎么使用
仿真平台可以被分为两部分，一块是计算部分uavA1，另一块是展示部分uavShow。直接运行main.m文件就可以了。
其实也可以把计算和展示同步进行，实时的计算然后展示，但是个人认为这样会比较卡，并且飞机越多越卡，会影响展示的流畅程度。

3.1 计算部分
我是依次计算每一架飞机的随着时间变化的状态，然后把它们储存在 x1.mat 文件中(x1可以为x2,x3…表示为第几架飞机)。

• CalAlluavs.m 计算部分代码
  
  3.2 演示部分
• ShowAlluavs.m 演示部分代码
  每一架飞机的数据都储存在x, path, waypoint数据中。

4 怎么读入路径文件
文件夹data中提供了一些5架飞机的路径文件可以使用。
如果你想计算自己的路径数据，你可以依照下面的步骤：

1. uavA1/getWpp.m -> load ‘5jia.mat’
2. uavA1/para_chap1.m -> load ‘5jia.mat’
   找到对应的代码文件中的load,修改’5jia.mat’就行了。
   getWpp.m 读取路径
   para_chap1.m 读取飞机的初始位置
   simulink的时间需要根据你的路径文件的长短调整，如果你的飞机明显没有跑完你的路径，那么你需要把时间调的长一些。
   关于 getWpp( ) 函数

function [num_waypoints , wpp] = getWpp(P,uav)
    load '5jia3.mat'

    a1 = 2;
    a2 = 400;
    num_waypoints = a2/a1;       
    wpp = [];
    i1 = uav;

    for i = 1:a1:a2
        x = [];
        x = Xplot2(i,6*i1-5:6*i1-3);
        x(3) = -x(3);
        x = [x -9999 Xplot2(i,6*i1-2)];
        wpp = [wpp;x];    
    end

5jia3.mat 文件里面储存的是5架飞机的状态量
400x30
400是400步长的状态 30是5x6的5架飞机的6个状态量
分别是：
三个方向的位置：x y z
飞机的速度：v
速度的两个角度theta 俯仰 phi 偏航（速度是没有滚转角的）
14-20行:对路径进行处理，传递给wpp
x = Xplot2(i,6i1-5:6i1-3)中 6i1-5:6i1-3 是 三个方向的位置 x y z
而 6*i1-2 就是速度
a1和a2 是我传递状态的一个离散步长，比方说 我有400步长的状态需要传递，然后我并不想每一个都传递过去，那么我就隔一个传递过去，也就是a1=2.隔两个传递过去a1=3.
如果你想到读取自己的路径文件，你可以仿照我的5jia3.mat文件的格式去生成，也可以自己去设置路径文件。
你可以看到，我只是传递了位置和速度，并没有传递速度的角度。所以后续飞控在跟踪的时候，只是按照参考速度去跟踪位置点的。至于具体的姿态，是飞控自己生成的。

二、部分源代码

close all;clear;
addpath 'data'
addpath 'uavA1'
% addpath 'uavShow'

% If you want to use other planning 5 trajectories files
% You should change as followws：
% 1. getWpp.m      load '5jia.mat'
% 2. para_chap1.m  load '5jia.mat'
% Change the name of '5jia.mat'

% And the simulink time also needs to adjusted if too long or long short  
%----------------
uavW = 1;
save('uavW.mat','uavW');
sim('New_mavsim_chap12');

ii = 1;
eval(['x' num2str(ii) '= x;'])
eval(['path' num2str(ii) '= path;'])
eval(['waypoints' num2str(ii) '= waypoints;'])

save('x1.mat','x1','path1','waypoints1');
%----------------
clear;
uavW = 2;
uavi = uavW;
save('uavW.mat','uavW');
sim('New_mavsim_chap12');

ii = 2;
eval(['x' num2str(ii) '= x;'])
eval(['path' num2str(ii) '= path;'])
eval(['waypoints' num2str(ii) '= waypoints;'])
save('x2.mat','x2','path2','waypoints2');
%----------------
clear;

save('uavW.mat','uavW');
sim('New_mavsim_chap12');

ii = 3;
eval(['x' num2str(ii) '= x;'])
eval(['path' num2str(ii) '= path;'])
eval(['waypoints' num2str(ii) '= waypoints;'])
save('x3.mat','x3','path3','waypoints3');
%----------------
clear;
uavW = 4;

save('uavW.mat','uavW');
sim('New_mavsim_chap12');


eval(['x' num2str(ii) '= x;'])
eval(['path' num2str(ii) '= path;'])
eval(['waypoints' num2str(ii) '= waypoints;'])
save('x4.mat','x4','path4','waypoints4');
%----------------
clear;

save('uavW.mat','uavW');
sim('New_mavsim_chap12');


% Show part
%----------------
close all;clear;
load 'x1'
load 'x2'
load 'x3'
load 'x4'
load 'x5'

addpath 'uavShow'

sim('mavsim_show');

% some variables needed
u = P.x_trim(4);
v = P.x_trim(5);
w = P.x_trim(6);
Va_trim = sqrt(u^2 + v^2 + w^2);
theta_trim = P.x_trim(8);
alpha_trim = atan(w/u);
delta_e_trim = P.u_trim(1);
delta_t_trim = P.u_trim(4);

% Roll attitude hold
P.delta_a_max = 45 * pi / 180;
P.phi_max = 15 * pi / 180;
P.roll_max = 45 * pi / 180;
a_phi1 = -0.5 * P.rho * Va_trim^2 * P.S_wing * P.b * P.C_p_p * P.b / (2 * Va_trim);
a_phi2 = 0.5 * P.rho * Va_trim^2 * P.S_wing * P.b * P.C_p_delta_a;
P.kp_phi = P.delta_a_max / P.phi_max * sign(a_phi2);
omega_phi = sqrt(abs(a_phi2) * P.delta_a_max / P.phi_max);
zeta_phi = 1.2;   % design parameter
P.kd_phi = (2 * zeta_phi * omega_phi - a_phi1) / a_phi2;
P.ki_phi = 0.2;    % design parameter

% Course hold
W_chi = 8;  % design parameter
omega_chi = 1 / W_chi * omega_phi;
zeta_chi = 0.707;   % design parameter
Vg = P.Va0;
P.kp_chi = 2 * zeta_chi * omega_chi * Vg / P.gravity;
P.ki_chi = omega_chi^2 * Vg / P.gravity;

% Pitch attitude hold
P.delta_e_max = 45 * pi / 180;
P.e_theta_max = 10 * pi / 180;
a_theta1 = -P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_q * P.c / (2 * P.Jy * 2 * Va_trim);
a_theta2 = -P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_alpha / (2 * P.Jy);
a_theta3 = P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_delta_e / (2 * P.Jy);
P.kp_theta = P.delta_e_max / P.e_theta_max * sign(a_theta3);
omega_theta = sqrt(a_theta2 + P.delta_e_max / P.e_theta_max * abs(a_theta3));
zeta_theta = 0.707;     % design parameter
P.kd_theta = (2 * zeta_theta * omega_theta - a_theta1) / a_theta3;
P.theta_max = 20 * pi / 180;
P.pitch_max = 40 * pi / 180;

% Airspeed hold using Throttle
a_V1 = P.rho * Va_trim * P.S_wing / P.mass ...
       * (P.C_D_0 + P.C_D_alpha * alpha_trim + P.C_L_delta_e * delta_e_trim) ...
       + P.rho * P.S_prop / P.mass * P.C_prop * Va_trim;
a_V2 = P.rho * P.S_prop / P.mass * P.C_prop * P.k_motor^2 * delta_t_trim;
a_V3 = P.gravity * cos(theta_trim - alpha_trim);
omega_v = 5;   % design parameter
zeta_v = 0.707;     % design parameter
P.delta_t_max = 1;
P.delta_t_min = 0;
P.ki_v = omega_v^2 / a_V2;
P.kp_v = (2 * zeta_v * omega_v - a_V1) / a_V2;

% Airspeed hold using Pitch
W_v2 = 7;   % design parameter
zeta_v2 = 0.707;    % design parameter
omega_v2 = 1 / W_v2 * omega_theta;
K_theta_dc = P.kp_theta * a_theta3 / (a_theta2 + P.kp_theta * a_theta3);
P.ki_v2 = -omega_v2^2/(K_theta_dc * P.gravity);
P.kp_v2 = (a_V1 - 2 * zeta_v2 * omega_v2) / (K_theta_dc * P.gravity);

% Altitude hold using Pitch
W_h = 10;   % design parameter
omega_h = 1 / W_h * omega_theta;
Va = P.Va0;
zeta_h = 1.2;     % design parameter
P.h_max = 1000;
P.h_min = 0;
P.ki_h = omega_h^2 / (K_theta_dc * Va);
P.kp_h = 2 * zeta_h * omega_h / (K_theta_dc * Va);
function drawPathError(uu)

    % process inputs to function
    pn       = uu(1);       % inertial North position     
    pe       = uu(2);       % inertial East position
    pd       = uu(3);       % inertial Down position
    u        = uu(4);       % body frame velocities
    v        = uu(5);       
    w        = uu(6);       
    phi      = uu(7);       % roll angle         
    theta    = uu(8);       % pitch angle     
    psi      = uu(9);       % yaw angle     
    p        = uu(10);      % roll rate
    q        = uu(11);      % pitch rate     
    r        = uu(12);      % yaw rate    
    t        = uu(13);      % time
    NN = 13;
    flag_path = uu(1+NN);      % path flag
    r_path    = [uu(3+NN); uu(4+NN); uu(5+NN)];
    q_path    = [uu(6+NN); uu(7+NN); uu(8+NN)];
    c_orbit   = [uu(9+NN); uu(10+NN); uu(11+NN)];
    rho_orbit = uu(12+NN);
    lam_orbit = uu(13+NN);


    % define persistent variables 
    persistent aircraft_handle;  % figure handle for MAV
    persistent Vertices
    persistent Faces
    persistent facecolors

    % first time function is called, initialize plot and persistent vars
    if t==0,

        figure(4), clf
        S = 1000;
        switch flag_path,
            case 1,
                XX = [r_path(1), r_path(1)+S*q_path(1)];
                YY = [r_path(2), r_path(2)+S*q_path(2)];
                ZZ = [r_path(3), r_path(3)+S*q_path(3)];
            case 2,
                N = 100;
                th = [0:2*pi/N:2*pi];
                XX = c_orbit(1) + rho_orbit*cos(th);
                YY = c_orbit(2) + rho_orbit*sin(th);
                ZZ = c_orbit(3)*ones(size(th));
        end

三、运行结果

四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.
[3]巫茜,罗金彪,顾晓群,曾青.基于改进PSO的无人机三维航迹规划优化算法[J].兵器装备工程学报. 2021,42(08)
[4]邓叶,姜香菊.基于改进人工势场法的四旋翼无人机航迹规划算法[J].传感器与微系统. 2021,40(07)
[5]马云红,张恒,齐乐融,贺建良.基于改进A*算法的三维无人机路径规划[J].电光与控制. 2019,26(10)
[6]焦阳.基于改进蚁群算法的无人机三维路径规划研究[J].舰船电子工程. 2019,39(03)