无人机编队自主避障系统

部分代码如下：

function [ ] = formation2 (start,goal)
% close all;                 % 4follower and 1 leader
countmax=500000;
% goal=[500,500,50];
stop_goal=goal;
obstacleR=0.5;% 冲突判定用的障碍物半径
%% 初始化 位置pose、速度V、加速度控制量control
% start = [
%         300,290,0,0,0;
%         300,280,0,0,0;
%         300,270,0,0,0;
%         300,260,0,0,0;
%         300,300,0,0,0;
%         300,250,0,0,0;
%         300,240,0,0,0;
%         300,230,0,0,0;
%         300,220,0,0,0;
%         300,210,0,0,0;]; 
 %% follower相对leader的位置
% 评价函数参数 [heading,dist,velocity,predictDT]
evalParam=[0.05,0.2,0.1,3.0];
% 模拟实验的结果
result1=[];
result2=[];
result3=[];
result4=[];
result5=[];
result6=[];
result7=[];
result8=[];
result9=[];
result10=[];
x1=start(1,:)';
x2=start(2,:)';
x3=start(3,:)';
x4=start(4,:)';
x5=start(5,:)';%x5为长机
x6=start(6,:)';
x7=start(7,:)';
x8=start(8,:)';
x9=start(9,:)';
x10=start(10,:)';
% Main loop
handles = [];
uav_num =10;%获得无人机的数量

%     h1 = figure;
% plot3(410,410,20);
% max_edge_limit=410;
% set(gca,'XLim',[0 max_edge_limit]);
% set(gca,'YLim',[0 max_edge_limit]);
% set(gca,'ZLim',[0 max_edge_limit]);
% set(0,'CurrentFigure',h1);
% xlabel("x");
% ylabel("y");
% zlabel("z");
for i = 1:uav_num
    handle = drawBody(start(i,1),start(i,2),start(i,3),0,0,0,[]);
    handles = [handles,handle];
end

%% 功能：人工势场法避障程序
%% 输入：起点(start_num*3)，终点(goal_num*3)，障碍物(obs_num*3),障碍物半径(obs_num*1)
%% 输出：无
%% 时间：2022.11.8
function APFM_main(start,goal,obs_point,obs_radius)

%Path Planning
obstacles = transpose(obs_point);
iteration = 2000;                         %Iterations count
current_pos = transpose(start);
goal = transpose(goal);
previous_pos = current_pos;             %Intialising Previous position of the UAV  
Krep = 0.1;                             %Gain factor of repulsive potential field
Katt = 0.04;                             
delta = 0;
data_points = zeros(iteration,3); % storing the iteration values positions of UAV
F = zeros(3,length(obstacles));
Urep = 0;
figure(1)
title('Path Planning of a UAV')
for i=1:iteration
    p_Fr = 0;
    robot_height = current_pos(3,1);
    goal_height = goal(3,1);
    flag = 0;
    Fatt = potential_attraction(Katt, current_pos, goal);
    for k = 1: length(obstacles)
        % Measuring the horizantal distance between UAV and centre axis of the building 
        rou = sqrt((current_pos(1,1)-obstacles(1,k))^2+(current_pos(2,1)-obstacles(2,k))^2); 
        % differentiation of variable rou 
        d_rou = [current_pos(1,1)-obstacles(1,k); current_pos(2,1)-obstacles(2,k)]/rou; 
        
        % Threshold value to judge whether the UAV near to buidling or not?
        zeta = 3.5*obs_radius(k,1);
        n = 2;
        if rou<=zeta
            if robot_height <= obstacles(3,k)
                % Flag - that tells the UAV to move in xy plane
                % no increment in height. 
                flag = 1;
                Frep1 = Krep*((1/rou)-(1/zeta))*(1/rou^2)*dist_factor(current_pos, goal, n, flag)*d_rou;
                Frep2 = -(n/2)*Krep*((1/rou)-(1/zeta))^2*dist_factor(current_pos, goal, n-1, flag)*diff_distance_factor(current_pos, goal, n, flag);
                F(:,k) = vertcat(Frep1,0)+Frep2; 
                Fatt = Katt*[goal(1,1)-current_pos(1,1);goal(2,1)-current_pos(2,1); 0];
            else
                F(:,k) = 0;
            end
        elseif rou > zeta
            
            F(:,k) = 0;
        end
    end
    Frep = sum(F,2); % summation of all repulsive forces
    Ft = Fatt + Frep;
    if flag == 1
        flag = 0;
        % changing the 'z' axis based on the UAV position if robot is near to obstracle it only moves in xy plane.
        Ft(3,1) = 0; 
    end
    previous_pos = current_pos;
    current_pos = current_pos+Ft;
    data_points(i,:)=transpose(current_pos);
    title(sprintf('Iterations %d', i))
    % Plotting the UAV position in real time
    plot3(current_pos(1,1), current_pos(2,1), current_pos(3,1),"Marker","*","Color","black");
    pause(0.07)
    drawnow
end
%Plotting of graphs
% for i = 1:length(obstacles)
%     % Plotting other usefull plots to analyse the UAV behaviour
%     potential_plots(x,y,obstacles(:,i));
% end
end