集群仿真基于matlab匈牙利算法无人机队形重构集群仿真含Matlab源码 1498期

Posted 2021-12-09 紫极神光

一、无人机集群仿真简介（附课程报告）

1 研究背景
四旋翼无人机具有成本较低、设备简单、飞行时间灵活等特点，近些年被广泛应用于军事和民用领域，如目标侦察、应急救援、农业植保、无人机灯光表演。随着任务复杂度的增加，单架无人机往往难以满足任务需求，因此无人机集群控制及其应用由此成为目前的研究热点，多无人机集群能够提高执行任务效率和灵活度。无人机队形变换控制方法是实现多无人机编队飞行的前提，集群无人机队形重构问题是我们要考虑的一个重要问题，让每架无人机都能从初始位置无碰撞的到达最终位置，进而保证队形重构过程中代价最小或能耗最优。其中目标分配问题最多利用匈牙利算法进行解决，但是在多无人机轨迹规划时普遍存在计算难度大、规划时间增长、规划效率难以满足实际需求的问题。因此，探索计算简便、效率高的多无人机路径规划算法是目前迫切需要的。

2 任务要求
（1）实验描述
十九架无人机组成的编队如图2.1所示，由F队形切换至Z队形，如果忽略本身以及外在约束条件的情况下，将会有多种不同的移动方案。但实际情形下无人机飞行距离越长，耗能也就越多，部分无人机到达目标的距离过长，消耗电力过多，从而提前降落。

图2.1： 编队实景图
使用传统匈牙利算法来解决队形变换的目标分配问题，其编队切换仿真图如下图，其中绿色方块表示无人机在F队形中的位置，红色圆点表示Z队形中无人机集群所要到达的各目标航点，黑色直线表示经分配后无人机由初始位置到达目标位置的直线路径。

图2.2： 匈牙利算法路径图
由上图可以看出，部分无人机到达目的地的直线路径过长，而大部分无人机初始位置与目标位置重合耗能少。移动路径长短不一，造成无人机耗能相差较大。所以我们需要寻求一种移动方式，使各个无人机移动路径长度趋于一致。
（2）实验要求
1、改进分配方案，使无人机在编队切换过程中飞行路径相近，飞行至目标航点时间一致，避免无人机耗能相差较大的问题。
2、集群无人机空中个别无人机能量不足或故障情况需退出，编队无人机数目发生变化，为维持编队队形需重新规划各无人机所在位置。

二、部分源代码

% 该程序用到
%  main.m :
%            主函数  

%  calc.m  ：
%            位置计算函数，实际上是将矩阵的点绘制到固定坐标位置
%        例如矩阵 array_f 中第一行第二列中的1 表示一架无人机。在模型场景中
%        他的位置实际上是(90, 30)，这个计算过程就是通过这个函数计算得到，知道他的功能就行。
%
%  move.m :
%            位置移动函数，该函数主要功能是做点的位置移动，不需要理会。
%
% my_function.m: 
%           算法函数，需要完成的算法函数，具体要完成什么在函数中有说明

% 


clc
clear all

symbol   = 'bo';  % 打点颜色符号(b. 蓝点； bo蓝圈)
symbol1  = 'wo';  % 打点颜色符号(w. 白点； wo白圈)
   
dt = 1; % 采样步距
v  = 1; % 速度


% 设置两点之间距离
width = 10;

% 做 8*8 矩阵 19 个点  字符：F  初始矩阵
array_f = [0 1 1 1 1 1 1 0;
           0 0 1 0 0 0 0 0;
           0 0 1 0 0 1 0 0;
           0 0 1 1 1 1 0 0;
           0 0 0 0 0 1 0 0;
           0 0 1 0 0 0 0 0;
           0 0 1 0 0 0 0 0;
           0 1 1 1 0 0 0 0];
       
% 做 8*8 矩阵 19 个点  字符：Z  目标矩阵
array_z = [0 1 1 1 1 1 1 0;
           0 1 0 0 0 1 0 0;
           0 0 0 0 1 0 0 0;
           0 0 0 1 0 0 0 0;
           0 0 0 1 0 0 0 0;
           0 0 1 0 0 0 0 0;
           0 0 0 0 0 1 0 0;
           1 1 1 1 1 1 0 0];

%场景的范围       
xmin = 0;xmax = width * 8 + 20;ymin = 0;ymax = width * 8 + 20; 


% 创建一个空的坐标图
axis([xmin xmax ymin ymax]); %设定坐标范围
figure(1);
hold on ; %保留绘图内容



% 初始化矩阵 

 %  该矩阵用于保存开始坐标位置 实际上是一个二维矩阵，矩阵的索引号就是 ID 号 第一位元素为 x 轴坐标，第二位元素为 y 轴坐标
 %   例如： id_sta_addr(7, 1) 表示 ID7 的无人机 X 轴坐标位置； 同理 id_sta_addr(7, 2)表示 ID7 的无人机 Y 轴坐标位置
id_sta_addr = zeros(19,2);  

%  该矩阵用于保存结束坐标位置
id_sto_addr = zeros(19,2);

%  该矩阵用于无人机在飞行过程中的临时坐标信息
id_cur_addr = zeros(19,2);

% 该矩阵用于保存所有无人机的最终要飞行的时间 是一个一维数组， 同上索引和就是ID号
% 例如 id_tm(1) 表示 ID1的无人机 飞行的时间
id_tm = zeros(19, 1);  

% 临时变量
index = 1;

% 给飞机实时编号，行扫描 安置无人机初始位置 实际上就是按比例显示F  这个函数不需要关心
% 他的工作就是 扫描 8*8 的矩阵，然后将矩阵中为 1 元素的位置按比例在图纸上用圆圈描绘出来
% 其中 两无人机的 位置宽带设置由 width 决定，如果 width = 10 ，则表示两无人机位置宽度为10个单位宽度
% 在该循环中 calc(i, j, width) 函数 是将 'F' 按比例放大并安置无人机到模型当中
% 其中 ID 的扫描顺序为: 第一行从左边第一列开始，到最后一列依次定义。
% 例如：array_f 中第一行第二列的 定义为 ID1;  array_f 中第一行第七列的 定义为 ID6; 以此类推
for i = 1: 8
	for j = 1: 8 
        % start 坐标
        if array_f(i, j) == 1
            % 做矩阵点位置应该实际场景
            [id_sta_addr(index, 1), id_sta_addr(index, 2)] = calc(i, j, width);
            % 安置飞机
            plot(id_sta_addr(index, 1), id_sta_addr(index, 2), 'bo');
            index = index + 1;            
        end
	end
end

% 算法部分  完成 my_function 函数即可
% temp_info 保存着各个ID的变化信息
temp_info = my_function(array_f, array_z);




pause(2);



% % 测试部分%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
% id_sto_addr = id_sta_addr;
% 
% % calc(2, 2, width); 中(2,2)表示无人机终止位置坐标，width表示位宽
% %id_sto_addr(7, 1)； 中(8,1)表示8 表示 ID， 1 表示 X坐标， 2 表示Y坐标
% % 表示 ID 7的点 移动到(2,2) 的位置
% [id_sto_addr(7, 1) , id_sto_addr( 7, 2)] = calc(2, 2, width);
% [id_sto_addr(8, 1) , id_sto_addr( 8, 2)] = calc(3, 5, width);
% [id_sto_addr(9, 1) , id_sto_addr( 9, 2)] = calc(2, 6, width);
% [id_sto_addr(10, 1), id_sto_addr(10, 2)] = calc(4, 4, width);
% [id_sto_addr(11, 1), id_sto_addr(11, 2)] = calc(5, 4, width);
% [id_sto_addr(12, 1), id_sto_addr(12, 2)] = calc(8, 5, width);
% [id_sto_addr(13, 1), id_sto_addr(13, 2)] = calc(7, 6, width);
% [id_sto_addr(14, 1), id_sto_addr(14, 2)] = calc(8, 6, width);
% [id_sto_addr(16, 1), id_sto_addr(16, 2)] = calc(8, 1, width);
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%计算 各个无人机运行时间
% 假设无人机 开始位置设置为（x1, y1），终止位置设置为(x2,y2) 则他们运行时间为:
%                   t =  sqrt((x1 - x2)^2 + (y1 - y2)^2) / v  （时间 = 路程 / 速度）
% 其中 sqrt 表示开根号
% 这里 使用一个循环则表示计算各个无人机 改变位置需要消耗的时间


 % 前面使用的 for 循环已经 计算出所有飞机飞行或改变位置的消耗时间
 % 由于无人机的时间有长有短，所以要得到最后变化的队形，肯定是按照最长时间计算变化时间
 %这里就是获取最大的变化时间
max_tm = max(id_tm);

% 该部分有两重循环， 第一重循环是表示 时间 扫描表示的是时间更新 其中 dt 表示无人机飞行过程中的更新时间 默认dt = 1
% 如果 dt = 1则表示 1s 更新一次飞行状态
% 可以不用理会 这个  写算法用不到
 for t=0:dt:max_tm
     
    % 扫描19个点
    for index = 1:19
        % 单点移动
        [id_cur_addr(index, 1), id_cur_addr(index, 2)] = move(index,       ...
                                                              id_sta_addr, ...
                                                              id_sto_addr, ...
                                                              id_cur_addr, ...
                                                              t,           ...
                                                              id_tm,       ...
                                                              v);

    end
  

fprintf('在任意两无人机距离为 %d 个单位时， 最大运行时间为 %f \\n', width, ...
    function [M,Perf_select,cost,Mean_square_erro] = zq_ave(Perf)
%Perf =  [ 1.0000    1.4142    4.1231    2.2361    2.8284    3.6056    4.4721    5.0000    4.1231    5.0990;
%    3.0000    3.1623    1.0000    3.6056    2.8284    2.2361    2.0000    3.0000    5.0000    5.8310;
%    2.2361    2.0000    1.0000    2.2361    1.4142    1.0000    1.4142    2.2361    3.6056    4.4721;
%    2.2361    1.4142    2.2361    1.0000         0        1.0000    2.0000    2.2361    2.2361    3.1623;
%    3.1623    2.2361    2.8284    1.4142    1.0000    1.4142    2.2361    2.0000    1.4142    2.2361;
%    4.0000    3.0000    4.2426    2.0000    2.2361    2.8284    3.6056    3.1623         0        1.0000;
 %   5.8310    5.0000    4.0000    4.2426    3.6056    3.1623    3.0000    2.0000    3.1623    3.0000;
%    6.3246    5.3852    7.0711    4.4721    5.0000    5.6569    6.4031    5.8310    2.8284    2.2361;
%    6.3246    5.3852    5.0990    4.4721    4.1231    4.0000    4.1231    3.1623    2.8284    2.2361;
%    6.7082    5.8310    5.0000    5.0000    4.4721    4.1231    4.0000    3.0000    3.6056    3.1623;

 %    ];

 size_P = size(Perf);%返回一个行向量，第一个元素是矩阵行，第二个是列，
 M = zeros(1,size_P(1));%返回一个1*10的零矩阵

Perf_num = zeros(size(Perf));%返回一个10*10的零矩阵

t = size_P(1);%t为10
min_mean = zeros(t);%10*10的矩阵
ave = 0;
for i=1:t 
    min_r = min(Perf(:,t));%A(x,y)表示二维矩阵第x行第y列位置的元素，x为:则表示所有的行。因此，A(:,1)就表示A的第1列的所有元素   1
    ave_vl = mean(mean(Perf(:,t)));%mean是求均值  
 
%     min_mean(i) = (min_r+ave)/2;
    min_mean(i) = 0.2*min_r+0.8*ave_vl;        %  调节最小、平均之间的比例
 
    ave = ave + ave_vl;
end

ave = ave/t;

%  求与平均值的差值 的矩阵
for j=1:t
    for i=1:t        
        Perf_num(i,j) = abs(Perf(i,j) - min_mean(j));%10*10的矩阵=
    end
end

%  将列最小值变为0
% function [Perf_num] = zero_mark(Perf_num)
    for j=1:t
        min_num = min(Perf_num(:,j));%找出第j列最小的元素
        for i=1:t
            if Perf_num(i,j) == min_num
                Perf_num(i,j) = 0;
                break;
            end
        end  
    end
%end
%zero_mark(Perf_num);

%重新标0
zeros_mark_num = zeros(1,10);
n = 1;
for i=1:t
    for j=1:t
        if Perf_num(i,j) == 0
            if find(zeros_mark_num == i)
                Perf_num = zero_mark(Perf_num,Perf,ave);
            else
                zeros_mark_num(n) = i;
                n = n+1;
            end

        end
    end
end          

for i=1:t
    for j=1:t
        if Perf_num(i,j) == 0
            if find(zeros_mark_num == i)
                Perf_num = zero_mark(Perf_num,Perf,ave);
            else
                zeros_mark_num(n) = i;
                n = n+1;
            end

        end
    end
end  


cost = 0;
Mean_square_erro = 0;
Perf_select = zeros(1,t);

for i=1:t
    for j=1:t
        if Perf_num(i,j)==0
            M(i) = j;
            Perf_select(i) = Perf(j,i);%做了修改
            cost = cost + Perf(j,i);
        end
    end
end

mean_Perf_select = mean(Perf_select)
for i = 1:t
    Mean_square_erro = Mean_square_erro + (Perf_select(i)-mean_Perf_select)^2;
end
Mean_square_erro = Mean_square_erro/t;
  
end

三、运行结果

四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.
[3]巫茜,罗金彪,顾晓群,曾青.基于改进PSO的无人机三维航迹规划优化算法[J].兵器装备工程学报. 2021,42(08)