【路径规划】基于果蝇优化算法实现机器人路径规划matlab源码
发布日期:2021-05-04 12:56:13
浏览次数:30
分类:技术文章
本文共 5499 字,大约阅读时间需要 18 分钟。
一、简介
果蝇优化算法(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 onB = 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 endend %到此能画出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,:);endpath = 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; endendplot([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);endpath = 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+1dijpathlen = dijpathlen + sqrt((vv(path(i),1)-vv(path(i+1),1))^2+(vv(path(i),2)-vv(path(i+1),2))^2);endLL = 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
三、源代码
四、备注
完整代码或者代写添加QQ1535704183
转载地址:https://blog.csdn.net/qq_34763204/article/details/116379640 如侵犯您的版权,请留言回复原文章的地址,我们会给您删除此文章,给您带来不便请您谅解!
发表评论
最新留言
做的很好,不错不错
[***.243.131.199]2024年03月12日 22时03分08秒
关于作者
喝酒易醉,品茶养心,人生如梦,品茶悟道,何以解忧?唯有杜康!
-- 愿君每日到此一游!
推荐文章
c语言中malloc数组,如何在C中对malloc()数组进行一行赋值?
2019-04-21
c语言调存储过程,写留言板–调用存储过程出问题
2019-04-21
c语言编程max,C语言编程题及答案.doc
2019-04-21
android增删改查布局,Android之父_增删改查
2019-04-21
html获取文件路径_HTML 文件路径
2019-04-21
mysql in 有序_mysql中的in排序 mysql按in中顺序来排序
2019-04-21
mysql 行转列 显示_mysql 行转列 (结果集以坐标显示)
2019-04-21
mysql 查询姓王_MySQL查询语句练习题,测试足够用了
2019-04-21
mysql多实例脚本_mysql多实例脚本
2019-04-21
python如何生成excel文件夹_用python脚本通过excel生成文件夹树结构
2019-04-21
mysql加密复制_MySQL主从复制使用SSL加密
2019-04-21
python启动远端 exe_python打包exe开机自动启动的实例(windows)
2019-04-21