【无人机三维路径规划】基于优化算法的无人机三维避障航迹规划

一、规划原理简述

       首先，对含有障碍物的空间环境进行建模，再对空间进行栅格处理，将空间划分成由一个个网格单元组成的网格空间。每个网格单元被赋予了一定的属性（0或1），用来表示障碍物或物体已走过的路径。将空间看作一个三维数组，每个网格位置根据其在三维空间上的位置用三维数组行列信息进行表示。

       接着，利用优化算法（本文采用蜻蜓算法）计算目标函数，实现对无人机的航迹规划问题求解。具体内容如下：

• 初始化

1. 将起点S加入CLOS列表，CLOSE列表用于存储已经探索过的节点或是障碍物的位置。
2. 计算起点周围可达节点的评价函数值，并将这些节点及其信息（包括评价函数值）加入OPEN列表，OPEN列表用于存储待探索的节点。

注：评价函数由两部分组成：f(n) = g(n) + h(n)。g(n) 表示从起点到节点n的实际代价；h(n) 是从节点n到目标节点的估计代价。

• 扩展节点

1. 在OPEN列表中找到评价函数值最小的节点作为当前节点n
2. 将当前节点n从OPEN列表移除，并添加到CLOSE列表，表示该节点已被无人机经过
3. 计算当前节点n周围可达的子节点n+1的评价函数值。如果子节点已经在OPEN列表中，比较新旧评价函数值，如果新值更小，则更新该节点在OPEN列表中的信息；否则不做改动。
4. 将新发现的子节点及其信息添加到OPEN列表中，等待后续探索。

• 重复上述过程，直到节点n到达目标节点

       最后，根据最终优化得到的OPEN列表信息（无人机的路径）作出相关的仿真路径规划图。具体的仿真示意图如下：

图.1   将障碍物和三维空间进行栅格化处理后的空间示意图  

图.2   三维航迹的俯视图 

图.3   三维航迹的仿真视图

算法存在的不足：

• 网格大小对规划路径的影响较大，例如，当设定的网格单元过小时会造成计算变得复杂且路径也会变得十分曲折。后续仍需对该问题进行改进。
• 利用优化算法求解路径经常由于局部收敛导致无法找到全局最优的路径，因此，在精度和稳定性上需要作出一定的改进。
• 该方法目前只考虑了单个无人机的路径规划问题，然而目前的研究早已趋向研究多个无人机的协同路径规划问题。后续可以通过节点赋予时间信息，如（x,y,z,t），进而实现无人机在空间上的协同（无人机之间在同一时刻应避免相互碰撞）和时间上的协同（到达目标的时间差/时间顺序）。

参考文献：

[1]高九州,张焯.基于改进A*算法的无人机三维空间避障路径规划[J].计算机测量与控制,2023,31(12):203-209+223.DOI:10.16526/j.cnki.11-4762/tp.2023.12.030.

二、程序展示

MATLAB程序如下：

clc;
clear;
close all;
warning off

%% 载入数据
data.S=[130,760,2];       % 起点
data.E=[950,320,10];      % 终点
data.Obstacle=xlsread('data.xls');  % 导入表格中障碍物的信息
data.numObstacles=length(data.Obstacle(:,1));  % 障碍物的数量
data.mapSize=[1000,1000,20];    % 地图尺寸
data.unit=[50,50,1];            % 地图精度，每个栅格在空间的三维大小
data.S0=ceil(data.S./data.unit);    % 将起点坐标从实际单位转换为栅格单位
data.E0=ceil(data.E./data.unit);    % 将终点坐标从实际单位转换为栅格单位
data.mapSize0=data.mapSize./data.unit;  % 将地图尺寸从实际单位转换为栅格单位，得到地图在栅格单位下的尺寸
data.map=zeros(data.mapSize0);    % 初始化了一个全零矩阵


%% 画出环境的图（起点，终点，障碍物，坐标轴标签） ，同时对data.map赋值障碍物属性
figure

plot3(data.S(:,1),data.S(:,2),data.S(:,3),'o','LineWidth',1,...
    'MarkerEdgeColor','g',...
    'MarkerFaceColor','g',...
    'MarkerSize',8)
hold on
plot3(data.E(:,1),data.E(:,2),data.E(:,3),'h','LineWidth',1,...
    'MarkerEdgeColor','g',...
    'MarkerFaceColor','g',...
    'MarkerSize',8)

for i=1:data.numObstacles
    x=1+data.Obstacle(i,1);
    y=1+data.Obstacle(i,2);
    z=1+data.Obstacle(i,3);
    long=data.Obstacle(i,4);
    wide=data.Obstacle(i,5);
    pretty=data.Obstacle(i,6);
    [V,F] = DrawCuboid(long, wide, pretty, x,y,z);  % 画
    x0=ceil(x/data.unit(1));
    y0=ceil(y/data.unit(2));
    z0=ceil(z/data.unit(3));
    long0=ceil(long/data.unit(1));
    wide0=ceil(wide/data.unit(2));
    pretty0=ceil(pretty/data.unit(3));
    data.map(x0:x0+long0,y0:y0+wide0,z0:z0+pretty0)=1;
end

legend('起点','终点')
title('三维地形地图')
grid on
axis equal

%% 画出障碍物的点状分布图
index=find(data.map==1);
[p1,p2,p3] = ind2sub(size(data.map), index);
figure
plot3(data.S0(:,1),data.S0(:,2),data.S0(:,3),'o','LineWidth',1,...
    'MarkerEdgeColor','g',...
    'MarkerFaceColor','g',...
    'MarkerSize',8)
hold on
plot3(data.E0(:,1),data.E0(:,2),data.E0(:,3),'h','LineWidth',1,...
    'MarkerEdgeColor','g',...
    'MarkerFaceColor','g',...
    'MarkerSize',8)
plot3(p1,p2,p3,'.','LineWidth',1,...
    'MarkerEdgeColor','k',...
    'MarkerFaceColor','g',...
    'MarkerSize',10)
legend('起点','终点')
title('三维地形地图')
grid on
axis equal
xlabel('x（km）')
ylabel('y（km）')
zlabel('z（km）')

%% 生成可移动方向：确定 物体质点 下一步走的位置
% 离散的空间中，将物体的移动设为一个固定的步长，而三维空间中一个点的邻近域有26个；
% 这个基础单位可以沿着三个正交轴（x、y、z）以及它们的反方向（-x、-y、-z）移动
temp=[1,0,-1];  
direction=[];
% 生成 3*3*3 的矩阵（元素均为temp的某个元素）
for i=1:3
    for j=1:3
        for k=1:3
            direction=[direction;temp(i),temp(j),temp(k)];
        end
    end
end
position=find(direction(:,1)==0 & direction(:,2)==0 & direction(:,3)==0);
direction(position,:)=[];  % 移除矩阵中全0的一行
data.direction=direction;

%% 优化算法的参数设置
numAgent=38;         % 种群个体数 size of population
Max_iter=100;        % 最大迭代次数 maximum number of interation
lb=0;         % 下限
ub=1;         % 上限
dim=prod(data.mapSize0);    % 优化变量个数，即向量三个元素的乘积
fobj=@(x) aimFcn(x,data);   % 定义一个匿名函数，表示目标函数

%% 使用优化算法求解
% 加载文件中的算法函数，将优化算法DA的函数和initialization的函数放
% 入该路径下optimization文件夹中
addpath(genpath("optimization"));   

index = 1; % 对比其他算法时用到
tic  % 开始计时
[bestY,bestX,recording]=DA(numAgent,Max_iter,lb,ub,dim,fobj);   % 蜻蜓算法DA，bestX 最优解的变量值，bestY 最优解的适应度值

% 得到最终的结果
[~,result]=aimFcn(bestX,data);

% 保存结果
Optimal_results{1,index}="DA";      % 算法名字
Optimal_results{2,index}=recording; % 迭代曲线
Optimal_results{3,index}=bestY;     % 最佳函数值
Optimal_results{4,index}=bestX;     % 最佳变量值
Optimal_results{5,index}=result;    % 优化结果
Optimal_results{6,index}=toc;       % 运行时间

% index = index +1;  % 为下一个算法做准备
 
%% 输出结果

color_mat = hsv(size(Optimal_results, 2));
figure
hold on
for i=1:size(Optimal_results, 2)
    plot(Optimal_results{2, i},'-o','LineWidth',1)
end
xlabel('迭代次数')
ylabel('适应度值')
title('适应度函数曲线')
set (gcf,'position',[488   342   500    200]); % 设置图形窗口的位置和大小
legend(Optimal_results{1, :}); % 添加图例
box on % 使坐标轴的框线可见

% Optimal_results{5,:}里放的是优化结果，Optimal_results{1, :}里是算法名字
for ii=1:size(Optimal_results, 2)
    DrawPic(Optimal_results{5, ii},data,Optimal_results{1, ii})
end
rmpath(genpath("optimization")); % 将算法文件夹从路径中移除


data.S=[108,940,2];     % 起点
data.E=[980,132,1];      % 终点
data.S0=ceil(data.S./data.unit);    % 将起点坐标从实际单位转换为栅格单位
data.E0=ceil(data.E./data.unit);    % 将终点坐标从实际单位转换为栅格单位


%% 需要的函数

%% 画出路径和障碍物
function DrawPic(result1,data,str)
figure
plot3(data.S0(:,1)*data.unit(1),data.S0(:,2)*data.unit(2),data.S0(:,3)*data.unit(3),'o','LineWidth',1.5,...
    'MarkerEdgeColor','g',...
    'MarkerFaceColor','g',...
    'MarkerSize',8)
hold on
plot3(data.E0(:,1)*data.unit(1),data.E0(:,2)*data.unit(2),data.E0(:,3)*data.unit(3),'h','LineWidth',1.5,...
    'MarkerEdgeColor','g',...
    'MarkerFaceColor','g',...
    'MarkerSize',8)
plot3(result1.path(:,1).*data.unit(1),result1.path(:,2).*data.unit(2),result1.path(:,3).*data.unit(3),'-','LineWidth',1.5,...
    'MarkerEdgeColor','g',...
    'MarkerFaceColor','g',...
    'MarkerSize',10)
for i=1:data.numObstacles
    x=1+data.Obstacle(i,1);
    y=1+data.Obstacle(i,2);
    z=1+data.Obstacle(i,3);
    long=data.Obstacle(i,4);
    wide=data.Obstacle(i,5);
    pretty=data.Obstacle(i,6);
    
    x0=ceil(x/data.unit(1))*data.unit(1);
    y0=ceil(y/data.unit(2))*data.unit(2);
    z0=ceil(z/data.unit(3))*data.unit(3);
    long0=ceil(long/data.unit(1))*data.unit(1);
    wide0=ceil(wide/data.unit(2))*data.unit(2);
    pretty0=ceil(pretty/data.unit(3))*data.unit(3);
    [V,F] = DrawCuboid(long0, wide0, pretty0, x0,y0,z0); % 调用函数画出障碍物
end
legend('起点','终点','location','north')
grid on
%axis equal
xlabel('x（km）')
ylabel('y（km）')
zlabel('z（km）')
title([str, '最优结果:', num2str(result1.fit)])

end

%% 画出障碍物
function  [V,F] = DrawCuboid(long, wide, pretty, x,y,z)
    V = [x y z; x+long y z; x y+wide z; x+long y+wide z; x y z+pretty; x+long y z+pretty; x y+wide z+pretty; x+long y+wide z+pretty];
    F = [1  2  4  3; 5  6  8  7; 1  2  6  5; 3  4  8  7; 1  5  7  3; 2  6  8  4]; 
    FC=[127,127,127]./255;  % 障碍外观颜色
    patch('Vertices',V,'Faces',F,'FaceColor',FC);
end

%% 用于规划路径的目标函数
function [fit,result]=aimFcn(x,data)

x=reshape(x,data.mapSize0); % 将输入的向量 x 按照 data.mapSize0 定义的尺寸重塑成三维数组
% 初始化变量
S=data.S0;  % 当前位置 S
E=data.E0;
path=S;
map=data.map;

flag=x*0;

while sum(S==E)~=3    % 直到当前位置 S 和结束位置 E 完全匹配，结束循环
    % 当前位置 S 按照方向向量进行移动
    nextN=repmat(S,length(data.direction(:,1)),1)+data.direction;  % 下一步的位置 nextN
    
    % 移除超过地图边界的移动
    % 当 nextN 小于等于0或大于地图的尺寸data.mapSize0时，删除该位置
    flag=nextN(:,1)*0;       
    for i=1:length(nextN(:,1))
        for j=1:3
            if nextN(i,j)<=0 ||nextN(i,j)>data.mapSize0(j)  
                flag(i)=1;
            end
        end
    end
    position=find(flag==1);
    nextN(position,:)=[];
    
    % 移除碰撞障碍物的移动
    flag=nextN(:,1)*0;
    for i=1:length(nextN(:,1))
        no1=nextN(i,1);
        no2=nextN(i,2);
        no3=nextN(i,3);
        if map(no1,no2,no3)==1  % 表示该位置有障碍物
            flag(i)=1;
        end
    end
    position=find(flag==1);
    nextN(position,:)=[];

    % 移除没法走的移动
    % 如果 nextN 变为空矩阵（即没有任何有效的下一步），则退回上一个路径位置，并从路径列表中移除最后一个位置
    if isempty(nextN)
        S=path(end-1,:);
        path(end,:)=[];
        continue;
    end
    
    % 计算每一步的成本和优先级
    D1=nextN(:,1)*0;
    D2=nextN(:,1)*0;
    pri=nextN(:,1)*0;
    for i=1:length(nextN(:,1))
        no1=nextN(i,1);
        no2=nextN(i,2);
        no3=nextN(i,3);
        D1(i)=norm(nextN(i,:)-S);
        D2(i)=norm(nextN(i,:)-E);
        pri(i)=x(no1,no2,no3);
    end
    % 找到具有最小成本的位置，将其添加到路径列表 path 中，再更新当前位置
    [~,no]=min((D1+D2).*pri.^0.5);
    path=[path;nextN(no,:)];
    S=nextN(no,:);
    map(S(1),S(2),S(3))=1;  % 标记走过的位置，防止重复访问
end

% 循环结束，计算路径的总长度 D
D=0;
for i=1:length(path(:,1))-1
    D=D+norm(path(i,:)-path(i+1,:));
end
% 适应度值==路径长度
fit=D; 
if nargout>1
    result.fit=fit;    % 总目标
    result.path=path;
end

end

该程序的优化算法代码（可替换为其他优化算法）：

%___________________________________________________________________%
%  Dragonfly Algorithm (DA) source codes demo version 1.0           %
%                                                                   %
%  Developed in MATLAB R2011b(7.13)                                 %
%                                                                   %
%  Author and programmer: Seyedali Mirjalili                        %
%                                                                   %
%         e-Mail: ali.mirjalili@gmail.com                           %
%                 seyedali.mirjalili@griffithuni.edu.au             %
%                                                                   %
%       Homepage: http://www.alimirjalili.com                       %
%                                                                   %
%   Main paper:                                                     %
%                                                                   %
%   S. Mirjalili, Dragonfly algorithm: a new meta-heuristic         %
%   optimization technique for solving single-objective, discrete,  %
%   and multi-objective problems, Neural Computing and Applications % 
%   DOI: http://dx.doi.org/10.1007/s00521-015-1920-1                %
%                                                                   %
%___________________________________________________________________%

% You can simply define your cost in a seperate file and load its handle to fobj 
% The initial parameters that you need are:
%__________________________________________
% fobj = @YourCostFunction
% dim = number of your variables
% Max_iteration = maximum number of generations
% SearchAgents_no = number of search agents
% lb=[lb1,lb2,...,lbn] where lbn is the lower bound of variable n
% ub=[ub1,ub2,...,ubn] where ubn is the upper bound of variable n
% If all the variables have equal lower bound you can just
% define lb and ub as two single number numbers

% To run DA: [Best_score,Best_pos,cg_curve]=DA(SearchAgents_no,Max_iteration,lb,ub,dim,fobj)
%__________________________________________

function [Best_score,Best_pos,cg_curve]=DA(SearchAgents_no,Max_iteration,lb,ub,dim,fobj)


cg_curve=zeros(1,Max_iteration); % 收敛曲线

if size(ub,2)==1
    ub=ones(1,dim)*ub;
    lb=ones(1,dim)*lb;
end

%The initial radius of gragonflies' neighbourhoods
r=(ub-lb)/10;
Delta_max=(ub-lb)/10;

Food_fitness=inf;
Food_pos=zeros(dim,1);

Enemy_fitness=-inf;
Enemy_pos=zeros(dim,1);

X=initialization(SearchAgents_no,dim,ub,lb);
Fitness=zeros(1,SearchAgents_no);

DeltaX=initialization(SearchAgents_no,dim,ub,lb);

for iter=1:Max_iteration
    
    r=(ub-lb)/4+((ub-lb)*(iter/Max_iteration)*2);
    
    w=0.9-iter*((0.9-0.4)/Max_iteration);
       
    my_c=0.1-iter*((0.1-0)/(Max_iteration/2));
    if my_c<0
        my_c=0;
    end
    
    s=2*rand*my_c; % Seperation weight
    a=2*rand*my_c; % Alignment weight
    c=2*rand*my_c; % Cohesion weight
    f=2*rand;      % Food attraction weight
    e=my_c;        % Enemy distraction weight
    
    for i=1:SearchAgents_no %Calculate all the objective values first
        Fitness(1,i)=fobj(X(:,i)');
        if Fitness(1,i)<Food_fitness
            Food_fitness=Fitness(1,i);
            Food_pos=X(:,i);
        end
        
        if Fitness(1,i)>Enemy_fitness
            if all(X(:,i)<ub') && all( X(:,i)>lb')
                Enemy_fitness=Fitness(1,i);
                Enemy_pos=X(:,i);
            end
        end
    end
    
    for i=1:SearchAgents_no
        index=0;
        neighbours_no=0;
        
        clear Neighbours_DeltaX
        clear Neighbours_X
        %find the neighbouring solutions
        for j=1:SearchAgents_no
            Dist2Enemy=distance(X(:,i),X(:,j));
            if (all(Dist2Enemy<=r) && all(Dist2Enemy~=0))
                index=index+1;
                neighbours_no=neighbours_no+1;
                Neighbours_DeltaX(:,index)=DeltaX(:,j);
                Neighbours_X(:,index)=X(:,j);
            end
        end
        
        % Seperation%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Eq. (3.1)
        S=zeros(dim,1);
        if neighbours_no>1
            for k=1:neighbours_no
                S=S+(Neighbours_X(:,k)-X(:,i));
            end
            S=-S;
        else
            S=zeros(dim,1);
        end
        
        % Alignment%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Eq. (3.2)
        if neighbours_no>1
            A=(sum(Neighbours_DeltaX')')/neighbours_no;
        else
            A=DeltaX(:,i);
        end
        
        % Cohesion%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Eq. (3.3)
        if neighbours_no>1
            C_temp=(sum(Neighbours_X')')/neighbours_no;
        else
            C_temp=X(:,i);
        end
        
        C=C_temp-X(:,i);
        
        % Attraction to food%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Eq. (3.4)
        Dist2Food=distance(X(:,i),Food_pos(:,1));
        if all(Dist2Food<=r)
            F=Food_pos-X(:,i);
        else
            F=0;
        end
        
        % Distraction from enemy%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Eq. (3.5)
        Dist2Enemy=distance(X(:,i),Enemy_pos(:,1));
        if all(Dist2Enemy<=r)
            Enemy=Enemy_pos+X(:,i);
        else
            Enemy=zeros(dim,1);
        end
        
        for tt=1:dim
            if X(tt,i)>ub(tt)
                X(tt,i)=lb(tt);
                DeltaX(tt,i)=rand;
            end
            if X(tt,i)<lb(tt)
                X(tt,i)=ub(tt);
                DeltaX(tt,i)=rand;
            end
        end
        
        if any(Dist2Food>r)
            if neighbours_no>1
                for j=1:dim
                    DeltaX(j,i)=w*DeltaX(j,i)+rand*A(j,1)+rand*C(j,1)+rand*S(j,1);
                    if DeltaX(j,i)>Delta_max(j)
                        DeltaX(j,i)=Delta_max(j);
                    end
                    if DeltaX(j,i)<-Delta_max(j)
                        DeltaX(j,i)=-Delta_max(j);
                    end
                    X(j,i)=X(j,i)+DeltaX(j,i);
                end
            else
                % Eq. (3.8)
                X(:,i)=X(:,i)+Levy(dim)'.*X(:,i);
                DeltaX(:,i)=0;
            end
        else
            for j=1:dim
                % Eq. (3.6)
                DeltaX(j,i)=(a*A(j,1)+c*C(j,1)+s*S(j,1)+f*F(j,1)+e*Enemy(j,1)) + w*DeltaX(j,i);
                if DeltaX(j,i)>Delta_max(j)
                    DeltaX(j,i)=Delta_max(j);
                end
                if DeltaX(j,i)<-Delta_max(j)
                    DeltaX(j,i)=-Delta_max(j);
                end
                X(j,i)=X(j,i)+DeltaX(j,i);
            end 
        end
        
        Flag4ub=X(:,i)>ub';
        Flag4lb=X(:,i)<lb';
        X(:,i)=(X(:,i).*(~(Flag4ub+Flag4lb)))+ub'.*Flag4ub+lb'.*Flag4lb;
        
    end
    Best_score=Food_fitness;
    Best_pos=Food_pos;
    
    cg_curve(iter)=Best_score;
end
end
%%
function o = distance(a,b)

for i=1:size(a,1)
    o(1,i)=sqrt((a(i)-b(i))^2);
end
end
%%
function Positions=initialization(SearchAgents_no,dim,ub,lb)

Boundary_no= size(ub,2); % numnber of boundaries

% If the boundaries of all variables are equal and user enter a signle
% number for both ub and lb
if Boundary_no==1
    ub_new=ones(1,dim)*ub;
    lb_new=ones(1,dim)*lb;
else
     ub_new=ub;
     lb_new=lb;   
end

% If each variable has a different lb and ub
    for i=1:dim
        ub_i=ub_new(i);
        lb_i=lb_new(i);
        Positions(:,i)=rand(SearchAgents_no,1).*(ub_i-lb_i)+lb_i;
    end

Positions=Positions';
end
%%
function o=Levy(d)

beta=3/2;
%Eq. (3.10)
sigma=(gamma(1+beta)*sin(pi*beta/2)/(gamma((1+beta)/2)*beta*2^((beta-1)/2)))^(1/beta);
u=randn(1,d)*sigma;
v=randn(1,d);
step=u./abs(v).^(1/beta);

% Eq. (3.9)
o=0.01*step;
end

初始化代码：

function Positions=initialization(SearchAgents_no,dim,ub,lb)

Boundary_no= size(ub,2); % numnber of boundaries

% If the boundaries of all variables are equal and user enter a signle
% number for both ub and lb
if Boundary_no==1
    Positions=rand(SearchAgents_no,dim).*(ub-lb)+lb;
end

% If each variable has a different lb and ub
if Boundary_no>1
    for i=1:dim
        ub_i=ub(i);
        lb_i=lb(i);
        Positions(:,i)=rand(SearchAgents_no,1).*(ub_i-lb_i)+lb_i;
    end
end

注：转载请声明原作者地址，谢谢！！！！！！