%% 清空環境 clc;clear%% 障礙物數據 position = load('barrier.txt'); plot([0,200],[0,200],'.'); hold on B = load('barrier.txt'); xlabel('km','fontsize',12) ylabel('km','fontsize',12) title('二維規劃空間','fontsize',12) %% 描述起點和終點 S = [20,180]; T = [160,90]; plot([S(1),T(1)],[S(2),T(2)],'.');% 圖形標注 text(S(1)+2,S(2),'S'); text(T(1)+2,T(2),'T');%% 描繪障礙物圖形 fill(position(1:4,1),position(1:4,2),[0,0,0]); fill(position(5:8,1),position(5:8,2),[0,0,0]); fill(position(9:12,1),position(9:12,2),[0,0,0]); fill(position(13:15,1),position(13:15,2),[0,0,0]);% 下載鏈路端點數據 L = load('lines.txt');%% 描繪線及中點 v = zeros(size(L)); for i=1:20plot([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('matrix.txt'); [n,m]=size(sign);for i=1:nif i == 1for k=1:m-1if sign(i,k) == 1plot([S(1),v(k-1,1)],[S(2),v(k-1,2)],'color',...'black','Linewidth',2,'LineStyle','-');endendcontinue;endfor j=2:iif i == mif sign(i,j) == 1plot([T(1),v(j-1,1)],[T(2),v(j-1,2)],'color',...'black','Linewidth',2,'LineStyle','-');endelseif sign(i,j) == 1plot([v(i-1,1),v(j-1,1)],[v(i-1,2),v(j-1,2)],...'color','black','Linewidth',2,'LineStyle','-');endendend end path = DijkstraPlan(position,sign); j = path(22); plot([T(1),v(j-1,1)],[T(2),v(j-1,2)],'color','yellow','LineWidth',3,'LineStyle','-.'); i = path(22); j = path(i); count = 0; while trueplot([v(i-1,1),v(j-1,1)],[v(i-1,2),v(j-1,2)],'color','yellow','LineWidth',3,'LineStyle','-.');count = count + 1;i = j;j = path(i);if i == 1 || j==1break;end end plot([S(1),v(i-1,1)],[S(2),v(i-1,2)],'color','yellow','LineWidth',3,'LineStyle','-.');count = count+3; pathtemp(count) = 22; j = 22; for i=2:countpathtemp(count-i+1) = path(j);j = path(j); end path = pathtemp; % path = [1 9 8 7 13 14 12 22];%% 蟻群算法參數初始化 pathCount = length(path)-2; %經過線段數量 pheCacuPara=2; %信息素計算參數 pheThres = 0.8; %信息素選擇閾值 pheUpPara=[0.1 0.0003]; %信息素更新參數 qfz= zeros(pathCount,10); %啟發值phePara = ones(pathCount,10)*pheUpPara(2); %信息素 qfzPara1 = ones(10,1)*0.5; %啟發信息參數 qfzPara2 = 1.1; %啟發信息參數 m=10; %種群數量 NC=500; %循環次數 pathk = zeros(pathCount,m); %搜索結果記錄 shortestpath = zeros(1,NC); %進化過程記錄%% 初始最短路徑 dijpathlen = 0; vv = zeros(22,2); vv(1,:) = S; vv(22,:) = T; vv(2:21,:) = 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); end LL = dijpathlen;%% 經過的鏈接線 lines = zeros(pathCount,4); for i = 1:pathCountlines(i,1:2) = B(L(path(i+1)-1,1),:);lines(i,3:4) = B(L(path(i+1)-1,2),:); endbestPath=zeros(pathCount,1); %% 循環搜索 for num = 1:NC%% 螞蟻迭代尋優一次for i=1:pathCountfor k=1:mq = rand();qfz(i,:) = (qfzPara2-abs((1:10)'/10-qfzPara1))/qfzPara2; %啟發信息if q<=pheThres%選擇信息素最大值arg = phePara(i,:).*(qfz(i,:).^pheCacuPara);j = find(arg == max(arg));pathk(i,k) = j(1);else % 輪盤賭選擇arg = phePara(i,:).*(qfz(i,:).^pheCacuPara);sumarg = sum(arg);qq = (q-pheThres)/(1-pheThres);qtemp = 0;j = 1;while qtemp < qqqtemp = qtemp + (phePara(i,j)*(qfz(i,j)^pheCacuPara))/sumarg;j=j+1;endj=j-1;pathk(i,k) = j(1);end% 信息素更新phePara(i,j) = (1-pheUpPara(1))*phePara(i,j)+pheUpPara(1)*pheUpPara(2);endend%% 計算路徑長度len = zeros(1,k);for k=1:mPstart = S;Pend = lines(1,1:2) + (lines(1,3:4)-lines(1,1:2))*pathk(1,k)/10;for l=1:pathCountlen(1,k) = len(1,k)+sqrt(sum((Pend-Pstart).^2));Pstart = Pend;if l<pathCountPend = lines(l+1,1:2) + (lines(l+1,3:4)-lines(l+1,1:2))*pathk(l+1,k)/10;endendPend = T;len(1,k) = len(1,k)+sqrt(sum((Pend-Pstart).^2));end%% 更新信息素% 尋找最短路徑minlen = min(len);minlen = minlen(1);minant = find(len == minlen);minant = minant(1);% 更新全局最短路徑if minlen < LLLL = minlen;bestPath=pathk(:,minant);end% 更新信息素for i=1:pathCountphePara(i,pathk(i,minant)) = (1-pheUpPara(1))* phePara(i,pathk(i,minant))+pheUpPara(1)*(1/minlen);endshortestpath(num) = minlen; end %% 畫圖 Pstart = S; Pend = lines(1,1:2) + (lines(1,3:4)-lines(1,1:2))*bestPath(1)/10; plot([Pstart(1),Pend(1)],[Pstart(2),Pend(2)],'color','blue','LineWidth',3,'LineStyle','-.'); for l=1:pathCountPstart = Pend;if l<pathCountPend = lines(l+1,1:2) + (lines(l+1,3:4)-lines(l+1,1:2))*bestPath(l+1)/10;plot([Pstart(1),Pend(1)],[Pstart(2),Pend(2)],'color','blue','LineWidth',3,'LineStyle','-.');end end Pend = T; plot([Pstart(1),Pend(1)],[Pstart(2),Pend(2)],'color','blue','LineWidth',3,'LineStyle','-.');figure; plot(1:NC,shortestpath,'color','blue'); hold on % plot(1:NC,djpathlen,'color','red'); ylabel('路徑總長度'); xlabel('迭代次數');

?D206

總結

以上是生活随笔為你收集整理的【蚁群路径规划】基于MATLAB的蚁群算法的二维路径规划的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。