1 理论基础

1.1 三维路径规划问题概述

1.2 三维空间抽象建模

2案例背景

2.1问题描述

        采用蚁群算法在跨度为21 km×21 km的一片海域中搜索从起点到终点,并且避开所有障碍物的路径,为了方便问题的求解,取该区域内最深点的高度为0,其他点高度根据和最深点高度差依次取得。路径规划起点坐标为(1,10,800),终点坐标为(21,4,1 000),规划环境和起点、终点如图3所示。整个搜索空间为21 km×21 km的海域,其中,起点坐标为(1,10,800),终点坐标为(21,4,1 000)。
        基于蚁群算法的三维路径搜索算法的算法流程如图4所示。

2.3 信息素更新

2.4可视搜索空间

        取α轴方向作为三维路径规划的主方向,水下机器人沿工轴方向前进,为了降低规划复杂程度,将水下机器人的运动简化为前向运动、横向运动和纵向运动三种运动方式。在前向运动一定单位长度距离Lx,max情况下,设定机器人最大横向移动允许距离为Ly,max,最大纵向移动距离为Lz,max。这样,当蚂蚁沿着α轴方向前进时,当位于点H(i,j,k)时,对下一个点的搜索就存在一个可视区域，可视区域如图5所示。

        这样,当蚂蚁由当前点向下一个点移动时,可搜索的区域限制在蚂蚁搜索可视区域之内,简化了搜索空间,提高了蚁群算法的搜索效率。

2.5 蚁群搜索策略

3 MATLAB程序

3.1 启发值计算函数

function qfz=CacuQfz(Nexty,Nexth,Nowy,Nowh,endy,endh,abscissa,HeightData)
%% 该函数用于计算各点的启发值
%Nexty Nexth    input    下个点坐标
%Nowy Nowh      input    当前点坐标
%endy endh      input    终点坐标
%abscissa       input    横坐标
%HeightData     input    地图高度
%qfz            output   启发值%% 判断下个点是否可达
if HeightData(Nexty,abscissa)<Nexth*200S=1;
elseS=0;
end%% 计算启发值
%D距离
D=50/(sqrt(1+(Nowh*0.2-Nexth*0.2)^2+(Nexty-Nowy)^2)+sqrt((21-abscissa)^2 ...+(endh*0.2-Nexth*0.2)^2+(endy-Nowy)^2));
%计算高度
M=30/abs(Nexth+1);
%计算启发值
qfz=S*M*D;

3.2适应度计算函数

function fitness=CacuFit(path)
%% 该函数用于计算个体适应度值
%path       input     路径
%fitness    input     路径[n,m]=size(path);
for i=1:nfitness(i)=0;for j=2:m/2%适应度值为长度加高度fitness(i)=fitness(i)+sqrt(1+(path(i,j*2-1)-path(i,(j-1)*2-1))^2 ...+(path(i,j*2)-path(i,(j-1)*2))^2)+abs(path(i,j*2));end
end

3.3 路径搜索

function [path,pheromone]=searchpath(PopNumber,LevelGrid,PortGrid,pheromone,HeightData,starty,starth,endy,endh)
%% 该函数用于蚂蚁蚁群算法的路径规划
%LevelGrid     input    横向划分格数
%PortGrid      input    纵向划分个数
%pheromone     input    信息素
%HeightData    input    地图高度
%starty starth input    开始点
%path          output   规划路径
%pheromone     output   信息素%% 搜索参数
ycMax=2;   %蚂蚁横向最大变动
hcMax=2;   %蚂蚁纵向最大变动
decr=0.9;  %信息素衰减概率%% 循环搜索路径
for ii=1:PopNumberpath(ii,1:2)=[starty,starth];  %记录路径NowPoint=[starty,starth];      %当前坐标点%% 计算点适应度值for abscissa=2:PortGrid-1%计算所有数据点对应的适应度值kk=1;for i=-ycMax:ycMaxfor j=-hcMax:hcMaxNextPoint(kk,:)=[NowPoint(1)+i,NowPoint(2)+j];if (NextPoint(kk,1)<20)&&(NextPoint(kk,1)>0)&&(NextPoint(kk,2)<20)&&(NextPoint(kk,2)>0)qfz(kk)=CacuQfz(NextPoint(kk,1),NextPoint(kk,2),NowPoint(1),NowPoint(2),endy,endh,abscissa,HeightData);qz(kk)=qfz(kk)*pheromone(abscissa,NextPoint(kk,1),NextPoint(kk,2));kk=kk+1;elseqz(kk)=0;kk=kk+1;endendend%选择下个点sumq=qz./sum(qz);pick=rand;while pick==0pick=rand;endfor i=1:25pick=pick-sumq(i);if pick<=0index=i;break;endendoldpoint=NextPoint(index,:);%更新信息素pheromone(abscissa+1,oldpoint(1),oldpoint(2))=0.5*pheromone(abscissa+1,oldpoint(1),oldpoint(2));%路径保存path(ii,abscissa*2-1:abscissa*2)=[oldpoint(1),oldpoint(2)];    NowPoint=oldpoint;endpath(ii,41:42)=[endy,endh];
end

3.4 主函数