【路径规划】基于matlab果蝇优化算法机器人路径规划【含Matlab源码 677期】

一、获取代码方式

获取代码方式1：
通过订阅紫极神光博客付费专栏，凭支付凭证，私信博主，可获得此代码。

获取代码方式2：
完整代码已上传我的资源：【路径规划】基于matlab果蝇优化算法机器人路径规划【含Matlab源码 677期】

备注：
订阅紫极神光博客付费专栏，可免费获得1份代码（有效期为订阅日起，三天内有效）；

二、果蝇优化算法简介

果蝇优化算法（FOA）是一种基于果蝇觅食行为推演出寻求全局优化的新方法。果蝇本身在感官知觉上优于其他物种，尤其是嗅觉和视觉上。果蝇的嗅觉器官能很好的搜集漂浮在空气中的各种气味，甚至能够嗅到40公里以外的食物源。然后，飞到食物位置附近后亦可使用敏锐的视觉发现食物和同伴聚集的位置，并且向该方向飞去。
果蝇算法可应用于求解最优解。

果蝇群体迭代搜寻食物的步骤如下：
（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

  

 

  
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
 

四、运行结果

五、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.

文章来源: qq912100926.blog.csdn.net，作者：海神之光，版权归原作者所有，如需转载，请联系作者。

原文链接：qq912100926.blog.csdn.net/article/details/115417347