Matlab仿真----无人机集群协同对抗仿真案例（2020数学建模D题）_无人机集群系统数学模型

自动驾驶行业交流群 简介

《自动驾驶行业交流》群里有参加竞赛的同行，把问题发出来了。有兴趣的同学，可以加群看大家的讨论。
https://zhuanlan.zhihu.com/p/101366366

自动驾驶技术是实现更高程度的智能生活的技术基础，目前已经广泛应用于无人车/船/机。
自动驾驶广泛应用于无人车、船、机。欢迎大家交流分享无人系统行业的技术发展（通信，感知(图像视觉、雷达、精密定位（无线、硬件）)，机器学习，建模与仿真）、招聘与求职、创业与融资。
另外，每年组织两次行业羽毛球比赛，促进线下交流。

群主从事无人系统(车和船)研究和开发10年，目前正编写{无人系统—感知 导航 控制}

我一直做自动驾驶的路径仿真，对这个问题，是轻车熟路。遂来试试水。

<无人机集群协同对抗>试题回顾

试题简述

新一代人工智能技术和自主技术快速走向战场，将催生新型作战力量，颠覆传统战争模式，未来战争必将是智能化战争。无人机集群作战作为智能作战的重要形式，正在崭露头角。通过多架无人机协同侦察、协同探测、协同跟踪、协同攻击、协同拦截等，共同完成较复杂的作战任务。

现考虑红、蓝双方的无人机集群在平面区域内的协同对抗问题。蓝方作为进攻方，希望突破红方无人机的拦截，成功抵达目的地遂行军事行动；红方则希望在给定的区域内完成对蓝方无人机的拦截，阻止蓝方的突防。本赛题讨论的对抗区域约定为图1所示的矩形区域ABCD，攻击纵深即BC之间的距离为L=50km,蓝方无人机的飞行轨迹不能越过AD\BC边，即考虑的是攻击通道（突防走廊）带宽有一个限定约束的情形，通道带宽即AB之间的距离记为M。蓝方无人机的速度为_e = 250m/s，最小转弯半径为500m；红方无人机的速度为350m/s,最小转弯半径为350m；红蓝双方无人机的速度保持不变，运动的方向可根据机动策略的需要随时改变，但受转弯半径的限制。

原题目，从这里下载。

问题一

如图3所示，对抗伊始红方2个无人机集群的圆周中心分别位于G1和G2，圆周半径为100m，其中DG1 = 20km, G1G2 = 30km, G2C = 20km. 试建模分析蓝方无人机处于矩形区域ABCD内哪些位置时，无论红方无人机采用什么样的追击策略，蓝方无人机总能采用合适的策略以躲避红方的拦截，实现成功突防；讨论蓝方无人机相应的最优突防策略。

问题二

如图4所示，对抗伊始蓝方突防无人机位于边界AB的中心点，红方2个无人机集群的圆周中心分别位于G1和G2，圆周半径为100m，其中，G1和G2位于边界CD上，具体位置根据需要确定。

试建模分析是否存在一个通道带宽M的下限Mmin，当实际通道带宽M比M_min大时，蓝方无人机一定能突破红方无人机集群的拦截；给出此种情形下蓝方无人机时间最短的突防策略。

问题三

红方每架运载机可分两个波次共发射10架无人机，组成两个无人机集群遂行拦截任务，每个无人机集群的无人机数量不少于3架。每一波次发射时无人机集群初始队形如图2所示的圆周构型，运载机与圆周中心的距离为2km，随后无人机集群的队形可根据需要调整，但要求满足相应的间距约束。如图5所示，对抗伊始，蓝方无人机位于边界AB的中心，通道带宽M = 70km；红方两架运载机分别位于边界CD上G1点 和G2点，并开始发射第一波次的无人机集群，运载机和无人机集群中心具体位置根据需要确定。

运载机第二波次发射无人机集群时，必须保证运载机与第一波次发射的无人机集群满足间距上的约束。讨论红方两架运载机两个波次发射的无人机数量、每架运载机第二波次发射的时刻和位置以及第二波次发射的无人机集群的中心位置，以实现最优的拦截效果；进一步具体建模分析是否存在一个通道带宽M的上限Mmax，当实际通道带宽M小于M_max时，无论蓝方无人机采用什么样的突防策略，红方无人机集群均存在相应的拦截策略，在区域ABCD内成功阻止蓝方无人机的突防。

问题四

如图6所示，通道带宽M=100km，蓝方3架突防无人机组成突防集群从矩形边界AB一侧开始突防（任2架突防无人机的间距需大于30m），红方5架运载机各携带10架无人机，从边界CD一侧同时开始遂行协同拦截任务。红方每架运载机分两个波次发射无人机，分别组成两个无人机集群，每个集群的无人机数量不少于3架；每架运载机第一波次发射无人机的时刻为初始对抗时刻，与所属无人机集群几何构型圆周中心的距离为2km。红方运载机初始位置、红方运载机发射的第一个波次的无人机集群中心位置、红方运载机发射第二波次无人机集群的时刻和位置、第二波次发射的无人机集群中心位置、两个波次无人机数量以及蓝方突防无人机初始位置根据需要确定。蓝方希望尽可能多的无人机突防成功，红方则希望成功拦截尽可能多的蓝方无人机。试讨论红方最优拦截策略和蓝方最优突防策略。

数学建模解题过程

分为两个部分：条件假设；程序设计。

解题思路· 假设条件

%% 1 假设条件，设计各自的运动策略；

% A 运动速度恒定，方向可以改变；

% B 假设蓝色无人机在飞行过程中，红色无人机（集群）的覆盖效果，始终以蓝色无人机为目标，实时调整方向；

% C 红色无人机（集群）的覆盖效果，以最大可靠覆盖为准则；

% D 强力突防策略下，蓝色无人机将在100s后，被拦截。

% E 蓝色无人机，必须采用欺骗的手段，将红色无人机吸引到一侧后，高速机动到另外一个区域，才有可能成功突防。

% F 转弯半径，能够限制运动轨迹，具体的数学表达，要考虑仿真步长

% G 蓝色无人机，初始位置在AB线上。如果以BA为Y轴的话，设置蓝色无人机的初始位置为（0, ）

为解答D题，进行了一下主要程序的实现。共233种不同情景。

解题过程已经在整理，目标是形成论文后，投稿。

待论文发表后，将在此博文中，详细介绍模型。

第一题相关代码和图示

```%%  MultiAgentBesieging_RoutePlaningSimu
% Steps:
% 1 假设条件，设计各自的运动策略；
% 2 规划函数和子函数，做好分工；
% 3 红蓝军主体的运动策略；
% 4 红军的子飞行器的运动策略；
% 5 统计计算仿真的相关参数
% 6 仿真开始
% 7 条件判断---子函数
% 8 迭代重新执行
% unmanned aerial vehicle
% Author: ruogu7(380545156)
% date: sep, 17th.
% Latest update: sep, 17th.

close all;
clear all;
clc
% 计时
tic
diary '.\path\output_0917_simu_log.txt'
diary on;

%% 1 假设条件，设计各自的运动策略；
% A 运动速度恒定，方向可以改变；
% B 假设蓝色无人机在飞行过程中，红色无人机（集群）的覆盖效果，始终以蓝色无人机为目标，实时调整方向；
% C 红色无人机主机的速度恒定不变，但红色小型无人机的速度和方向可以保持灵活变化；
% D 红色无人机（集群）的覆盖效果，以各自对蓝色无人机形成最大可靠覆盖为准则；当红色无人机间距缩小时，以二者的联合覆盖最大为覆盖准则；
% E 强力突防策略下，蓝色无人机将在100s后，被拦截。
% F 蓝色无人机，必须采用欺骗的手段，将红色无人机吸引到一侧后，高速机动到另外一个区域，才有可能成功突防。
% G 转弯半径，能够限制运动轨迹，具体的数学表达，要考虑仿真步长
% H 蓝色无人机，初始位置在AB线上。如果以BA为Y轴的话，设置蓝色无人机的初始位置为（0, ）

%% 2 规划函数和子函数，做好分工；
% 主函数：初始化设置、主控流程、
% 子函数1：基于红色无人机主机的位置，规划小型无人机的位置,使其有效覆盖长度最大。
% 子函数2：计算当前有效覆盖长度（垂直方向）
% 子函数3：计算当前有效覆盖长度（垂直方向）

close all;
clear all;
clc;

%% 参数
% 仿真步长
% 模型一：蓝色无人机初始位置，AB中间，一直往右飞行；红色无人机集群始终朝着蓝色无人机飞行。
% 步长为0.2s
step_length = 3;
speed_blue_UAV = 0.250;
speed_red_UAV = 0.200;
Besiege_distance = 1
% UAV集群的半径
r_red_uav = 0.5;

% 先画框架
fig = figure;
frame = getframe(fig); % 获取frame

set(gcf,'outerposition',get(0,'screensize'));
axis equal;

FlyBox = [0 0;
    0 50;
    50 50;
    50 0;
    0 0];
pause(1)
% plot(FlyBox(1:end-1,1),FlyBox(1:end-1,2),'gp', 'MarkerSize',20,'MarkerEdgeColor','b','MarkerFaceColor',[0 1 0]);%方形点填充颜色
plot(FlyBox(1:end-1,1),FlyBox(1:end-1,2),'gp', 'MarkerSize',20,'MarkerEdgeColor','b','MarkerFaceColor',[0 1 0]);%方形点填充颜色
hold on;
plot(FlyBox(:,1),FlyBox(:,2),'b');
% plot(FlyBox(:,1),FlyBox(:,2),'b', 'MarkerSize',15);
hold on;

% 蓝色UAV的初始位置
Blue_UAV_Location = [0 25]; % x水平,y竖直
% 红色UAV的初始位置，
Red_1_UAV_Location = [50 40]; % 中心（舰载机1）FY01
Red_1_1_UAV_Location = [50+r_red_uav*cos(18*pi/180) 40+ r_red_uav*sin(18*pi/180)];  % 无人机FY0101的位置
Red_1_2_UAV_Location = [50+r_red_uav*cos(90*pi/180) 40+ r_red_uav*sin(90*pi/180)];  % 无人机FY0102的位置
Red_1_3_UAV_Location = [50+r_red_uav*cos(162*pi/180) 40+ r_red_uav*sin(162*pi/180)];  % 无人机FY0103的位置
Red_1_4_UAV_Location = [50+r_red_uav*cos(234*pi/180) 40+ r_red_uav*sin(234*pi/180)];  % 无人机FY0104的位置
Red_1_5_UAV_Location = [50+r_red_uav*cos(306*pi/180) 40+ r_red_uav*sin(306*pi/180)];  % 无人机FY0105的位置
Red_2_UAV_Location = [50 10]; % 中心（舰载机2）FY02
Red_2_1_UAV_Location = [50+r_red_uav*cos(18*pi/180) 10+ r_red_uav*sin(18*pi/180)];  % 无人机FY0201的位置
Red_2_2_UAV_Location = [50+r_red_uav*cos(90*pi/180) 10+ r_red_uav*sin(90*pi/180)];  % 无人机FY0202的位置
Red_2_3_UAV_Location = [50+r_red_uav*cos(162*pi/180) 10+ r_red_uav*sin(162*pi/180)]; % 无人机FY0203的位置
Red_2_4_UAV_Location = [50+r_red_uav*cos(234*pi/180) 10+ r_red_uav*sin(234*pi/180)];  % 无人机FY0204的位置
Red_2_5_UAV_Location = [50+r_red_uav*cos(306*pi/180) 10+ r_red_uav*sin(306*pi/180)];  % 无人机FY0205的位置

Flag_Track = 1;
Figure_num = 0;

while Flag_Track && Figure_num<85
    % 当前无人机的位置
    Blue_UAV_Location_temp = [Blue_UAV_Location(end,1) Blue_UAV_Location(end,2)];
    Red_1_UAV_Location_temp = [Red_1_UAV_Location(end,1) Red_1_UAV_Location(end,2)]
    Red_2_UAV_Location_temp = [Red_2_UAV_Location(end,1) Red_2_UAV_Location(end,2)]
    
    % 计算红色无人机的飞行方向
    [azimuth_Red_1, dist_Red_1 ] = GetAzimuth_2points(Blue_UAV_Location_temp, Red_1_UAV_Location_temp)
    [azimuth_Red_2, dist_Red_2 ] = GetAzimuth_2points(Blue_UAV_Location_temp, Red_2_UAV_Location_temp)
    
    % 更新蓝色无人机的位置
    Blue_UAV_Location_new_x = Blue_UAV_Location_temp(1) + step_length * speed_blue_UAV * cos(0);
    Blue_UAV_Location_new_y = Blue_UAV_Location_temp(2) + step_length * speed_blue_UAV * sin(0);
    Blue_UAV_Location_new = [Blue_UAV_Location_new_x Blue_UAV_Location_new_y];
    Blue_UAV_Location = [Blue_UAV_Location;Blue_UAV_Location_new];
    
    % 计算红色无人机群1的位置
    Red_1_UAV_Location_new_x = Red_1_UAV_Location_temp(1) + step_length * speed_red_UAV * cos((360-(azimuth_Red_1-90))*pi/180);
    Red_1_UAV_Location_new_y = Red_1_UAV_Location_temp(2) + step_length * speed_red_UAV * sin((360-(azimuth_Red_1-90))*pi/180);
    Red_1_UAV_Location_new = [Red_1_UAV_Location_new_x Red_1_UAV_Location_new_y];
    
    Red_1_1_UAV_Location_new = [Red_1_UAV_Location_new_x+r_red_uav*cos(18*pi/180) Red_1_UAV_Location_new_y+r_red_uav*sin(18*pi/180)];  % 无人机FY0101的位置
    Red_1_2_UAV_Location_new = [Red_1_UAV_Location_new_x+r_red_uav*cos(90*pi/180) Red_1_UAV_Location_new_y+r_red_uav*sin(90*pi/180)];  % 无人机FY0102的位置
    Red_1_3_UAV_Location_new = [Red_1_UAV_Location_new_x+r_red_uav*cos(162*pi/180) Red_1_UAV_Location_new_y+r_red_uav*sin(162*pi/180)];  % 无人机FY0103的位置
    Red_1_4_UAV_Location_new = [Red_1_UAV_Location_new_x+r_red_uav*cos(234*pi/180) Red_1_UAV_Location_new_y+r_red_uav*sin(234*pi/180)];  % 无人机FY0104的位置
    Red_1_5_UAV_Location_new = [Red_1_UAV_Location_new_x+r_red_uav*cos(306*pi/180) Red_1_UAV_Location_new_y+r_red_uav*sin(306*pi/180)];  % 无人机FY0105的位置
    
    % 更新红色无人机群1的位置
    Red_1_UAV_Location = [Red_1_UAV_Location;Red_1_UAV_Location_new];
    Red_1_1_UAV_Location = [Red_1_1_UAV_Location;Red_1_1_UAV_Location_new];
    Red_1_2_UAV_Location = [Red_1_2_UAV_Location;Red_1_2_UAV_Location_new];
    Red_1_3_UAV_Location = [Red_1_3_UAV_Location;Red_1_3_UAV_Location_new];
    Red_1_4_UAV_Location = [Red_1_4_UAV_Location;Red_1_4_UAV_Location_new];
    Red_1_5_UAV_Location = [Red_1_5_UAV_Location;Red_1_5_UAV_Location_new];
    
    % 计算红色无人机群2的位置
    Red_2_UAV_Location_new_x = Red_2_UAV_Location_temp(1) + step_length * speed_red_UAV * cos((360-(azimuth_Red_2-90))*pi/180);
    Red_2_UAV_Location_new_y = Red_2_UAV_Location_temp(2) + step_length * speed_red_UAV * sin((360-(azimuth_Red_2-90))*pi/180);
    Red_2_UAV_Location_new = [Red_2_UAV_Location_new_x Red_2_UAV_Location_new_y];
    
    Red_2_1_UAV_Location_new = [Red_2_UAV_Location_new(1)+r_red_uav*cos(18*pi/180) Red_2_UAV_Location_new(2)+r_red_uav*sin(18*pi/180)];  % 无人机FY0201的位置
    Red_2_2_UAV_Location_new = [Red_2_UAV_Location_new(1)+r_red_uav*cos(90*pi/180) Red_2_UAV_Location_new(2)+r_red_uav*sin(90*pi/180)];  % 无人机FY0202的位置
    Red_2_3_UAV_Location_new = [Red_2_UAV_Location_new(1)+r_red_uav*cos(162*pi/180) Red_2_UAV_Location_new(2)+r_red_uav*sin(162*pi/180)]; % 无人机FY0203的位置
    Red_2_4_UAV_Location_new = [Red_2_UAV_Location_new(1)+r_red_uav*cos(234*pi/180) Red_2_UAV_Location_new(2)+r_red_uav*sin(234*pi/180)];  % 无人机FY0204的位置
    Red_2_5_UAV_Location_new = [Red_2_UAV_Location_new(1)+r_red_uav*cos(306*pi/180) Red_2_UAV_Location_new(2)+r_red_uav*sin(306*pi/180)];  % 无人机FY0205的位置
    
    % 更新红色无人机群2的位置
    Red_2_UAV_Location = [Red_2_UAV_Location;Red_2_UAV_Location_new];
    
    Red_2_1_UAV_Location = [Red_2_1_UAV_Location;Red_2_1_UAV_Location_new];  % 无人机FY0201的位置
    Red_2_2_UAV_Location = [Red_2_2_UAV_Location;Red_2_2_UAV_Location_new];  % 无人机FY0202的位置
    Red_2_3_UAV_Location = [Red_2_3_UAV_Location;Red_2_3_UAV_Location_new];  % 无人机FY0203的位置
    Red_2_4_UAV_Location = [Red_2_4_UAV_Location;Red_2_4_UAV_Location_new];  % 无人机FY0204的位置
    Red_2_5_UAV_Location = [Red_2_5_UAV_Location;Red_2_5_UAV_Location_new];  % 无人机FY0205的位置
    
    %% 路径可视化
    % 蓝色无人机
    hold on;
    plot(Blue_UAV_Location(:,1),Blue_UAV_Location(:,2),'b');
    hold on;
    
    % 红色UAV集群1,画线
    plot(Red_1_UAV_Location(:,1),Red_1_UAV_Location(:,2),'r');
    hold on;
    
    % 红色UAV集群1的UAV，画点
    plot(Red_1_1_UAV_Location(end,1),Red_1_1_UAV_Location(end,2),'pr');
    hold on;
    plot(Red_1_2_UAV_Location(end,1),Red_1_2_UAV_Location(end,2),'pr');
    hold on;
    plot(Red_1_3_UAV_Location(end,1),Red_1_3_UAV_Location(end,2),'pr');
    hold on;
    plot(Red_1_4_UAV_Location(end,1),Red_1_4_UAV_Location(end,2),'pr');
    hold on;
    plot(Red_1_5_UAV_Location(end,1),Red_1_5_UAV_Location(end,2),'pr');
    hold on;
    
    % 红色UAV集群2,画线
    plot(Red_2_UAV_Location(:,1),Red_2_UAV_Location(:,2),'r');
    hold on;
    
    % 红色UAV集群2的UAV，画点
    plot(Red_2_1_UAV_Location(end,1),Red_2_1_UAV_Location(end,2),'pr');
    hold on;
    plot(Red_2_2_UAV_Location(end,1),Red_2_2_UAV_Location(end,2),'pr');
    hold on;
    plot(Red_2_3_UAV_Location(end,1),Red_2_3_UAV_Location(end,2),'pr');
    hold on;
    plot(Red_2_4_UAV_Location(end,1),Red_2_4_UAV_Location(end,2),'pr');
    hold on;
    plot(Red_2_5_UAV_Location(end,1),Red_2_5_UAV_Location(end,2),'pr');
    hold on;
    
    
    
    Point_Attack = Blue_UAV_Location(end,:);
    Point_Array_Besiege = [Red_1_1_UAV_Location_new;
        Red_1_2_UAV_Location_new;
        Red_1_3_UAV_Location_new;
        Red_1_4_UAV_Location_new;
        Red_1_5_UAV_Location_new;
        Red_2_1_UAV_Location_new;
        Red_2_2_UAV_Location_new;
        Red_2_3_UAV_Location_new;
        Red_2_4_UAV_Location_new;
        Red_2_5_UAV_Location_new;
        ];
    
    besiege_status = check_besiege_status(Point_Attack, Point_Array_Besiege, Besiege_distance)
    
    if besiege_status
        % 结束后续的循环
        Flag_Track = 0
        % 画出拦截的位置
        plot_circle( Point_Attack(1),Point_Attack(2),1 );
    end
    
    Figure_num = Figure_num + 1
    % 截图
    % imwrite(ImageWith_BBox, strcat('.\PeoplewithLJDetected_0728\','PeoplewithLJDetected_',num2str(Flag_Track),'.png'));
    % imwrite(ImageWith_BBox, num2str(Flag_Track),'.png'));
    % print(['Frame_' num2str(Flag_Track)], '-dpng', '-r200');
    % print(['Frame_' num2str(Figure_num)], '-dpng');
    frame = getframe(fig); 
    img = frame2im(frame); % 将frame变换成imwrite函数可以识别的格式;
    imwrite(img,strcat('.\figure\Frame_',num2str(Figure_num),'.png')); 
    
end

% 第一次计时
toc

%% make the vedio of dynamic object tracking
GifFileName = 'Q1_dynamicSim_Condition_1.gif';
delay = 0.5;                             % Delay between frames (s)
for jj= 1:Figure_num % 1039 % iii-1
    [A, ~] = imread(['.\figure\Frame_' num2str(jj) '.png']);
    [X, map] = rgb2ind(A, 64); % 128
    if jj == 1
        imwrite(X, map, GifFileName, 'gif', 'LoopCount', inf, 'DelayTime', delay)
    else
        imwrite(X, map, GifFileName, 'gif', 'WriteMode', 'append', 'DelayTime', delay)
    end
end

% % 设置输出视频的参数
AviFileName = 'Q1_dynamicSim_Condition_1.mp4';
if (exist ('AviFileName', 'file'))
    delete  AviFileName.mp4;
end
aviobj = VideoWriter(AviFileName,'MPEG-4' );         % 创建一个avi视频文件对象，并初始化为空；
aviobj.FrameRate = 5;                      % 帧率， 0～12，即秒出现的图像 帧数

open (aviobj);   % 打开aviobj，开始写入数据
for jjjj= 1:85 % 1039 % iii-1
    jjjj
    frames = imread(['.\figure\Frame_' num2str(jjjj) '.png']);
    writeVideo(aviobj,frames);
end
close(aviobj);% 关闭创建视频

% 第二次计时
toc
diary off;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257

仿真框架现在搭建起来了。蓝色UAV、红色UAV集群的位置计算方式，已经有了。

资源链接

% 当前无人机的位置
    Blue_UAV_Location_temp = [Blue_UAV_Location(end,1) Blue_UAV_Location(end,2)];
    Red_1_UAV_Location_temp = [Red_1_UAV_Location(end,1) Red_1_UAV_Location(end,2)];
    Red_2_UAV_Location_temp = [Red_2_UAV_Location(end,1) Red_2_UAV_Location(end,2)];
    
    % 计算红色无人机的飞行方向
    [azimuth_Red_1, dist_Red_1 ] = GetAzimuth_2points(Blue_UAV_Location_temp, Red_1_UAV_Location_temp);
    [azimuth_Red_2, dist_Red_2 ] = GetAzimuth_2points(Blue_UAV_Location_temp, Red_2_UAV_Location_temp);
    
    % 更新蓝色无人机的位置
    Blue_UAV_Location_new_x = Blue_UAV_Location_temp(1) + step_length * speed_blue_UAV * cos(0);
    Blue_UAV_Location_new_y = Blue_UAV_Location_temp(2) + step_length * speed_blue_UAV * sin(0);
    Blue_UAV_Location_new = [Blue_UAV_Location_new_x Blue_UAV_Location_new_y];
    Blue_UAV_Location = [Blue_UAV_Location;Blue_UAV_Location_new];
    
    % 计算红色无人机群1的位置
    Red_1_UAV_Location_new_x = Red_1_UAV_Location_temp(1) + step_length * speed_red_UAV * cos((360-(azimuth_Red_1-90))*pi/180);
    Red_1_UAV_Location_new_y = Red_1_UAV_Location_temp(2) + step_length * speed_red_UAV * sin((360-(azimuth_Red_1-90))*pi/180);
    Red_1_UAV_Location_new = [Red_1_UAV_Location_new_x Red_1_UAV_Location_new_y];
    
    Red_1_1_UAV_Location_new = [Red_1_UAV_Location_new_x+r_red_uav*cos(18*pi/180) Red_1_UAV_Location_new_y+r_red_uav*sin(18*pi/180)];  % 无人机FY0101的位置
    Red_1_2_UAV_Location_new = [Red_1_UAV_Location_new_x+r_red_uav*cos(90*pi/180) Red_1_UAV_Location_new_y+r_red_uav*sin(90*pi/180)];  % 无人机FY0102的位置
    Red_1_3_UAV_Location_new = [Red_1_UAV_Location_new_x+r_red_uav*cos(162*pi/180) Red_1_UAV_Location_new_y+r_red_uav*sin(162*pi/180)];  % 无人机FY0103的位置
    Red_1_4_UAV_Location_new = [Red_1_UAV_Location_new_x+r_red_uav*cos(234*pi/180) Red_1_UAV_Location_new_y+r_red_uav*sin(234*pi/180)];  % 无人机FY0104的位置
    Red_1_5_UAV_Location_new = [Red_1_UAV_Location_new_x+r_red_uav*cos(306*pi/180) Red_1_UAV_Location_new_y+r_red_uav*sin(306*pi/180)];  % 无人机FY0105的位置
    
    % 更新红色无人机群1的位置
    Red_1_UAV_Location = [Red_1_UAV_Location;Red_1_UAV_Location_new];
    Red_1_1_UAV_Location = [Red_1_1_UAV_Location;Red_1_1_UAV_Location_new];
    Red_1_2_UAV_Location = [Red_1_2_UAV_Location;Red_1_2_UAV_Location_new];
    Red_1_3_UAV_Location = [Red_1_3_UAV_Location;Red_1_3_UAV_Location_new];
    Red_1_4_UAV_Location = [Red_1_4_UAV_Location;Red_1_4_UAV_Location_new];
    Red_1_5_UAV_Location = [Red_1_5_UAV_Location;Red_1_5_UAV_Location_new];
    
    % 计算红色无人机群2的位置
    Red_2_UAV_Location_new_x = Red_2_UAV_Location_temp(1) + step_length * speed_red_UAV * cos((360-(azimuth_Red_2-90))*pi/180);
    Red_2_UAV_Location_new_y = Red_2_UAV_Location_temp(2) + step_length * speed_red_UAV * sin((360-(azimuth_Red_2-90))*pi/180);
    Red_2_UAV_Location_new = [Red_2_UAV_Location_new_x Red_2_UAV_Location_new_y];
    
    Red_2_1_UAV_Location_new = [Red_2_UAV_Location_new(1)+r_red_uav*cos(18*pi/180) Red_2_UAV_Location_new(2)+r_red_uav*sin(18*pi/180)];  % 无人机FY0201的位置
    Red_2_2_UAV_Location_new = [Red_2_UAV_Location_new(1)+r_red_uav*cos(90*pi/180) Red_2_UAV_Location_new(2)+r_red_uav*sin(90*pi/180)];  % 无人机FY0202的位置
    Red_2_3_UAV_Location_new = [Red_2_UAV_Location_new(1)+r_red_uav*cos(162*pi/180) Red_2_UAV_Location_new(2)+r_red_uav*sin(162*pi/180)]; % 无人机FY0203的位置
    Red_2_4_UAV_Location_new = [Red_2_UAV_Location_new(1)+r_red_uav*cos(234*pi/180) Red_2_UAV_Location_new(2)+r_red_uav*sin(234*pi/180)];  % 无人机FY0204的位置
    Red_2_5_UAV_Location_new = [Red_2_UAV_Location_new(1)+r_red_uav*cos(306*pi/180) Red_2_UAV_Location_new(2)+r_red_uav*sin(306*pi/180)];  % 无人机FY0205的位置
    
    % 更新红色无人机群2的位置
    Red_2_UAV_Location = [Red_2_UAV_Location;Red_2_UAV_Location_new];
    
    Red_2_1_UAV_Location = [Red_2_1_UAV_Location;Red_2_1_UAV_Location_new];  % 无人机FY0201的位置
    Red_2_2_UAV_Location = [Red_2_2_UAV_Location;Red_2_2_UAV_Location_new];  % 无人机FY0202的位置
    Red_2_3_UAV_Location = [Red_2_3_UAV_Location;Red_2_3_UAV_Location_new];  % 无人机FY0203的位置
    Red_2_4_UAV_Location = [Red_2_4_UAV_Location;Red_2_4_UAV_Location_new];  % 无人机FY0204的位置
    Red_2_5_UAV_Location = [Red_2_5_UAV_Location;Red_2_5_UAV_Location_new];  % 无人机FY0205的位置
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53

效果如下：

一些过程中的函数

函数： 判断是否被成功拦截

%%  besiege_status = check_besiege_status(Point_Attack, Point_Array_Besiege，Besiege_distance)
% Point_Attack:x,y
% Point_Array_Besiege:[x,y] * N
% Output: besiege_status = 0, 未被拦截；1，被拦截
% function: 判断Point_Array_Besiege中,是否存在两个点，距离Point_attack的距离，均小于拦截距离Besiege_distance
% Steps:
% 1 获取Point_Array_Besiege的维度；
% 2 统计Point_Attack与获取Point_Array_Besiege的维度中，各点的距离；
% 3 检索最小距离的两个值；
% 4 对比Besiege_distance；
% 5 统计个数。根据个数，来对besiege_status进行赋值
% Author: ruogu7(380545156)
% date: sep, 18th.
% Latest update: sep, 18th.

close all;
clear all;
clc
% 计时
tic
diary '.\path\output_check_besiege_status_log.txt'
diary on;

Num_Array_Besiege = size(Point_Array_Besiege,1)
distance_array = zeros(1,Num_Array_Besiege);
ii = 0;
if Num_Array_Besiege < 2
    return 0;
end 
for ii = 1:Num_Array_Besiege
    P_local = [Point_Attack(1)-Point_Array_Besiege(ii,1) Point_Attack(2)-Point_Array_Besiege(ii,2)];
    distance_array(ii) = sqrt(P_local(1)^2+P_local(2)^2);
end

% 从distance_array中，挑选两个最小的数
[distance_array_sorted,i]=sort(distance_array)
if (distance_array_sorted(1) < Besiege_distance) && (distance_array_sorted(2) < Besiege_distance) 
    besiege_status = 1;
else
    besiege_status = 0;
end

diary off;
toc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44

函数： 判断两点的相对方位

function [azimuth, dist ] = GetAzimuth_2points(point1, point2)
%% 功能：求Point1相对与Point2的相对方位和距离
% point的格式：[x,y]
% azimuth: 指北方向为0度，然后顺时针为正，直到360度，保留1位小数
% point1 = [-1 1];
% point2 = [1 -1];
% [azimuth, dist ] = GetAzimuth_2points(point1, point2)

P_local = [point1(1)-point2(1) point1(2)-point2(2)];

dist = sqrt(P_local(1)^2+P_local(2)^2);

sin_value = P_local(2)/dist;
cos_value = P_local(1)/dist;

if (P_local(1)>=0) && (P_local(2)>=0)  % 第一象限
    azimuth = 90-asin(P_local(2)/dist) * 180/pi;
    return;
end
if (P_local(1)<=0) && (P_local(2)>=0)   % 第二象限
    azimuth = asin(P_local(2)/dist)*180/pi+270 
    return;
end
if (P_local(1)<=0) && (P_local(2)<=0)   % 第三象限
    azimuth = 180 + asin(-P_local(1)/dist) * 180 / pi;
    return;
end
if (P_local(1)>=0) && (P_local(2)<=0)   % 第四象限
    azimuth = 180 - asin(P_local(1)/dist) * 180 / pi;
    return;
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

函数： 判断无人机是否被 成功拦截

function besiege_status = check_besiege_status(Point_Attack,Point_Array_Besiege,Besiege_distance)
% Point_Attack:x,y
% Point_Array_Besiege:[x,y] * N
% Output: besiege_status = 0, 未被拦截；1，被拦截
% function: 判断Point_Array_Besiege中,是否存在两个点，距离Point_attack的距离，均小于拦截距离Besiege_distance
% Steps:
% 1 获取Point_Array_Besiege的维度；
% 2 统计Point_Attack与获取Point_Array_Besiege的维度中，各点的距离；
% 3 检索最小距离的两个值；
% 4 对比Besiege_distance；
% 5 统计个数。根据个数，来对besiege_status进行赋值
% Author: ruogu7(380545156)
% date: sep, 18th.
% Latest update: sep, 18th.

Num_Array_Besiege = size(Point_Array_Besiege,1)
distance_array = zeros(1,Num_Array_Besiege);
ii = 0;
if Num_Array_Besiege < 2
    besiege_status = 0;
    return;
end 
for ii = 1:Num_Array_Besiege
    P_local = [Point_Attack(1)-Point_Array_Besiege(ii,1) Point_Attack(2)-Point_Array_Besiege(ii,2)];
    distance_array(ii) = sqrt(P_local(1)^2+P_local(2)^2);
end

% 从distance_array中，挑选两个最小的数
[distance_array_sorted,i] = sort(distance_array);
if (distance_array_sorted(1) < Besiege_distance) && (distance_array_sorted(2) < Besiege_distance) 
    besiege_status = 1;
else
    besiege_status = 0;
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34

文献资源

该试题的全部过程，详见知乎博客，键连接

补充说明

关于图示说明

红色表示红色无人机集群；

蓝线表示蓝色无人机的轨迹。

黄色圆圈表示蓝色无人机被红色无人机集群拦截的位置。被拦截后，仿真程序就执行结束。所以，黄色圆圈，出现的时间比较短。

关于赛后工作

本人对数学建模比较感兴趣，同时，工作方向也是无人机无人艇的算法开发。

对数模D题的投入，是为了继续无人艇路径规划算法开发，并最终写投一篇路径规划的论文。

有兴趣的朋友，可以交流，一起署名发表。

关于文献

2020 IEEE Cooperative Occupancy Decision Making of Multi-UAV in Beyond-Visual-Range Air Combat- A Game Theory Approach.pdf
2017 厦门 无人机集群对抗技术新进展.pdf
2014 UAV path planning using artificial potential field method updated by optimal control theory

关于自动驾驶行业交流群

欢迎加入《自动驾驶行业交流群》，一起探讨自动驾驶有关的技术问题：感知、规划、仿真。