【路径规划】基于果蝇优化算法实现机器人路径规划matlab源码-优快云博客

本文链接：https://blog.youkuaiyun.com/Matlab_dashi/article/details/140115113

一、简介

果蝇优化算法(FOA)是一种基于果蝇觅食行为推演出寻求全局优化的新方法。果蝇本身在感官知觉上优于其他物种，尤其是嗅觉和视觉上。果蝇的嗅觉器官能很好的搜集漂浮在空气中的各种气味，甚至能够嗅到40公里以外的食物源。然后，飞到食物位置附近后亦可使用敏锐的视觉发现食物和同伴聚集的位置，并且向该方向飞去。
果蝇算法可应用于求解最优解。【路径规划】基于果蝇优化算法实现机器人路径规划matlab源码_matlab
果蝇群体迭代搜寻食物的步骤如下：
(1)随机初始化果蝇群体位置。
Init X_axis
Init Y_axis

(2)赋予果蝇个体利用嗅觉搜寻食物的随机距离与方向。
Xi = X_axis + Random Value
Yi = Y_axis + Random Value

(3)由于无法得知食物的位置，因此先估计与原点的距离(Dist)，再计算味道浓度判定值(S)，此值为距离的倒数。
Disti = sqrt(Xi^2 + Yi^2)
Si = 1 / Disti

(4)味道浓度判定值(S)代入味道浓度判定函数(或称为Fitness function)以求出该果蝇个体位置的味道浓度(Smelli)。
Smelli = Function(Si)

(5)找出该果蝇群体中味道浓度最高的果蝇(求极大值)。
[bestSmell bestIndex] = max(Smell)

(6)保留最佳味道浓度值与x、y的坐标，此时果蝇群体利用视觉往该位置飞去。
Smellbest = bestSmell
X_axis = X(bestIndex)
Y_axis = Y(bestIndex)

(7)进入迭代寻优，重复执行步骤2-5，并判断味道浓度是否优于前一迭代味道浓度，若是则实行步骤6。

二、源代码

%% 清空环境
clc;
clear all;
close all;
tic
%% 障碍物数据
position = load('barrier1.txt');
% plot([0,100],[0,100],'.');
axis([0 100 0 100]);
hold on
B = load('barrier1.txt');
xlabel('km','fontsize',12)
ylabel('km','fontsize',12)
title('二维规划空间','fontsize',12)
%圆形障碍物数据
% r=10; theta=0:pi/100:2*pi;
% x=r*cos(theta)+65; y=r*sin(theta)+50;
% sizep = size(position);
% sizet = size(theta);
% for kk=1:sizet(2)
%     position(sizep(1)+kk,1)=x(kk);
%     position(sizep(1)+kk,2)=y(kk);
%     B(sizep(1)+kk,1)=x(kk);
%     B(sizep(1)+kk,2)=y(kk);
% end


%% 描述起点和终点
Start = [0,0];
Target = [100,100];
plot([Start(1),Target(1)],[Start(2),Target(2)],'.');

% 图形标注
text(Start(1)+3,Start(2)+4,'S');
text(Target(1)-3,Target(2)-4,'T');
 
%% 描绘障碍物图形
fill(position([2,7,8,3],1),position([2,7,8,3],2),[0,0,0]);
fill(position([4,9,10,5],1),position([4,9,10,5],2),[0,0,0]);
fill(position([11,15,16,13],1),position([11,15,16,13],2),[0,0,0]);
fill(position([17,20,21,18],1),position([17,20,21,18],2),[0,0,0]);

%% 描绘线及中点
% 下载链路端点数据
L = load('lines1.txt');

v = zeros(size(L)); %存储中点的坐标
for i=1:13   %%19表示连线数目即中点数目
    plot([position(L(i,1),1),position(L(i,2),1)],[position(L(i,1),2)...
        ,position(L(i,2),2)],'color','black','LineStyle','--');
    v(i,:) = (position(L(i,1),:)+position(L(i,2),:))/2;
    plot(v(i,1),v(i,2),'*');
    text(v(i,1)+2,v(i,2),strcat('v',num2str(i)));
end

%% 描绘可行路径
sign = load('matrix1.txt');%邻接矩阵
[n,m]=size(sign);

for i=1:n
    
    if i == 1
        for k=1:m-1
            if sign(i,k) == 1
                plot([Start(1),v(k-1,1)],[Start(2),v(k-1,2)],'color',...
                    'black','Linewidth',1,'LineStyle','-');
            end
        end
        continue;
    end
    
    for j=2:i
        if i == m
            if sign(i,j) == 1
                plot([Target(1),v(j-1,1)],[Target(2),v(j-1,2)],'color',...
                    'black','Linewidth',1,'LineStyle','-');
            end
        else
            if sign(i,j) == 1
                plot([v(i-1,1),v(j-1,1)],[v(i-1,2),v(j-1,2)],...
                    'color','black','Linewidth',1,'LineStyle','-');
            end
        end
    end
end                                                                  %到此能画出MAKLINK链路图
v1=zeros(15,2);  %13个中点加上起始点和终止点                         %v1存放包括S和T的总共22个结点，作为DijkstraPlan的参数
v1(1,:)=[0,0];
v1(15,:)=[100,100];
for i=2:14
    v1(i,:)=v(i-1,:);
end
path = DijkstraPlan(v1,sign);
j = path(15);
plot([Target(1),v(j-1,1)],[Target(2),v(j-1,2)],'color','black','LineWidth',3,'LineStyle',':');
i = path(15);
j = path(i);
count = 0;
while true
    plot([v(i-1,1),v(j-1,1)],[v(i-1,2),v(j-1,2)],'color','black','LineWidth',3,'LineStyle',':');
    count = count + 1;
    i = j;
    j = path(i);
    if i == 1 || j==1
        break;
    end
end
plot([Start(1),v(i-1,1)],[Start(2),v(i-1,2)],'color','black','LineWidth',3,'LineStyle',':');
count = count+3;
pathtemp(count) = 15;
j = 15;
for i=2:count
    pathtemp(count-i+1) = path(j);
    j = path(j);
end
path = pathtemp;                     %到此能在规划空间中用黄线描绘出次优最短路径
pathCount = length(path)-2;          %经过线段数量，相当于维数D

%% 经过的链接线
lines = zeros(pathCount,4);
for i = 1:pathCount                  %lines用于存放各个结点所在链接线的起点和终点
    lines(i,1:2) = B(L(path(i+1)-1,1),:);
    lines(i,3:4) = B(L(path(i+1)-1,2),:);
end

% 初始最短路径
dijpathlen = 0;
vv = zeros(15,2);
vv(1,:) = Start;
vv(15,:) = Target;
vv(2:14,:) = v;
for i=1:pathCount+1
dijpathlen = dijpathlen + sqrt((vv(path(i),1)-vv(path(i+1),1))^2+(vv(path(i),2)-vv(path(i+1),2))^2);
end
LL = dijpathlen;                   %得出的LL为次优最短路径的长度

%% 果蝇优化算法参数初始化
popsize=200;
maxgen=200;
gen=0;
D=zeros(popsize,pathCount);
S=zeros(popsize,pathCount);
bestgensmell=zeros(1,maxgen);

%% 果蝇位置初始化及食物浓度计算
%*** 随机初始果蝇群体位置。
X_axis=1+1*rands(1,pathCount);
Y_axis=1+1*rands(1,pathCount);

% *** 果蝇寻优开始，利用嗅觉寻找食物。
for p=1:popsize
    X(p,:)=X_axis+2*rand()-1;
    Y(p,:)=Y_axis+2*rand()-1;

    for i=1:pathCount
    %*** 求出与原点之距离
%     D(p,i)=(X(p,i)^2+Y(p,i)^2)^0.5;
%     %*** 味道浓度为距离之倒数，先求出味道浓度判定值。
%     S(p,i)=1/D(p,i);
    S(p,i)=(X(p,i)+Y(p,i))*1.0/2;
    %%%%%%
    if(S(p,i)>1)%%范围限定在0到1之间
            S(p,i)=1;
    end
        if(S(p,i)<0)
            S(p,i)=0;
        end

    end
    
end


    %*** 利用味道浓度判定函数求出味道浓度
for p=1:popsize
    w=S(p,:);
    Smell(p)=distance(w,pathCount,lines,Start,Target);
end

%% 初始化公告板
[bestsmell, bestindex]=min(Smell);
%*** 利用视觉寻找伙伴聚集味道浓度最高之处，做法是保留最佳值初始位置及初始味道浓度。
X_axis=X(bestindex,:);
Y_axis=Y(bestindex,:);
bestS=S(bestindex,:);
SmellBest=bestsmell;

%% 果蝇搜索过程
%*** 果蝇迭代寻优
for gen=1:maxgen
    %*** 利用嗅觉寻找食物
    for p=1:popsize
    %*** 初始果蝇个体飞行距离
    X(p,:)=X_axis+2*rand()-1;
    Y(p,:)=Y_axis+2*rand()-1;

    for i=1:pathCount
    %*** 求出与原点之距离
%     D(p,i)=(X(p,i)^2+Y(p,i)^2)^0.5;
%     %*** 味道浓度为距离之倒数，先求出味道浓度判定值。
%     S(p,i)=1/D(p,i);
    S(p,i)=(X(p,i)+Y(p,i))*1.0/2;
    %%%%%%
    if(S(p,i)>1)%%范围限定在0到1之间
            S(p,i)=1;
    end
        if(S(p,i)<0)
            S(p,i)=0;
        end
    end
    
    end