【路径规划】基于matlab遗传优化模拟退火算法求解避障路径规划问题matlab代码

2022/1/11 1:06:58

编程Tag： 路径 route matlab plot fit 模拟退火 bestx0

本文主要是介绍【路径规划】基于matlab遗传优化模拟退火算法求解避障路径规划问题matlab代码，对大家解决编程问题具有一定的参考价值，需要的程序猿们随着小编来一起学习吧！

1 简介

近年来, 路径规划一直是机器人学中的研究热点之一.它研究的目的是在有障碍物的环境中, 为机器人寻找从起始点到目标点并且能够避开障碍物的合理、有效的路径.其中涉及到环境信息的获取、环境的表示、路径执行、知识的获取等 .国内外已有不少学者对移动机器人路径规划问题作了深入研究, 并提出了多种路径规划方法.例如, 可视图法[ 1-2] 、图搜索法、人工势场法[ 3] 等.但这些方法的搜索空间比较大, 存在组合爆炸的问题, 难以满足实时搜索的要求, 或者存在局部极小值问题, 使规划失败. 遗传算法由于具有算法鲁棒、灵活, 在种群中搜索不易落入局部最小点等优点[ 4] , 近几年被广泛采用, 但它在实际应用过程中也可能会产生早熟现象、局部寻优能力差等问题 .而模拟退火算法却具有较强的局部搜索能力, 所以使用遗传算法与模拟退火相结合的方法, 是解决上述问题的有效途径.本文中将这两个算法结合构成遗传模拟退火算法, 它兼备了很强的全局和局部搜索能力的优点, 在变量数目较大时尤其突出.

2 部分代码

%%%%%%模拟退火遗传算法(SAGA)%%%%%%%%%%%

clear
%%%设置超参数
p_crs = 0.7;   %交叉概率
p_mut = 0.1;   %变异概率
ratio = 0.5;   %选择操作中父辈的比例
pop_num = 5000;  %种群规模
chrom_len = 7;  %染色体长度，这里代表路线的点数
iteration = 40;
T0 = 100;  %初始温度
A = 0.8;  %退火速度

% 一个个体就是一条路线
[x,y]=popinit(pop_num,chrom_len);   %产生初始种群   
fit=saga_fitness(x,y, T0);      %计算种群适应度
[bestx0,besty0,fit0]=best(x,y,fit); 
d0 = 0; %初始路径长度
for j=1:1:size(bestx0,2)-1
   d0 = d0 + sqrt((bestx0(1,j+1)-bestx0(1,j)).^2 + ...
      (besty0(1,j+1)-besty0(1,j)).^2);     %该个体（即路线）的路径长度
end

for i=1:1:iteration           %设置进化代数
  [Parentx,Parenty]=select(x, y, fit, ratio);      %选择
  [Kidx,Kidy]=crossover(Parentx,Parenty,p_crs);       %交叉
  [Kidx,Kidy]=mutation(Kidx,Kidy,p_mut);              %变异
   x = [Parentx; Kidx];    % 得到新的种群
   y = [Parentx; Kidy];
   x(:,chrom_len)=1.5;   % 保留终点
   y(:,chrom_len)=8.9;
   
   T = T0 * A^(i-1);  % 当前温度
   fit = saga_fitness(x,y,T);   % 计算进化后的适应度
  [bestx,besty,bestfit]=best(x,y,fit);   %选择每一代中的最佳个体
   route_x(i,:)=bestx;                     %保存该最佳个体
   route_y(i,:)=besty;
   route_fit(i)=bestfit;
   for j=1:1:size(bestx,2)-1
       dd(j)=sqrt((bestx(1,j+1)-bestx(1,j)).^2 + ...
          (besty(1,j+1)-besty(1,j)).^2);     %该个体（即路线）的路径长度
   end
   d(i) = sum(dd);   %有问题
   fprintf('%dth 代进化完成...\n', i)
%     plot(bestx,besty,'r-');
end
route_fit = [fit0, route_fit];   %加上初始种群中最优个体
route_x = [bestx0; route_x];
route_y = [bestx0; route_y];
d = [d0, d];

[final_fit,idx]=max(route_fit);            %所有代中的的最佳路线
final_routex=route_x(idx,:);
final_routey=route_y(idx,:);
final_distance = min(d)             %最佳路径长度

%==========画图，可视化路线、进化过程==============
% start point
xs=0;
ys=0;
% Destination
xt=1.5;
yt=8.9;
%obstacle
xobs=[1.5 4.0 1.2];
yobs=[6.5 3.0 1.5];
robs=[1.5 1.0 0.8];
theta=linspace(0,2*pi,100);
max_area = 0;
for k=1:numel(xobs)
   fill(xobs(k)+robs(k)*cos(theta),yobs(k)+robs(k)*sin(theta),[0.5 0.7 0.8]);  % 后一个参数表示RGB值
   text(xobs(k), yobs(k), num2str(k))
   hold on;
end
plot(xs,ys,'bs','MarkerSize',12,'MarkerFaceColor','y');
plot(xt,yt,'kp','MarkerSize',16,'MarkerFaceColor','g');
grid on;
hold on;

%%画出最短路径的路线
plot(final_routex,final_routey,'r*-',  'linewidth', 1.5);
legend('障碍物1','障碍物2','障碍物3','起点', '终点', '最短路线')
set(gca,'FontSize',16);
hold off;

% 进化过程中适应度曲线
figure,
% plot(0:1:size(route_fit,2)-1, route_fit, 'linewidth', 1.2)
plot(d, 'linewidth', 1.2)
% ylim([0.08,0.1])
title(['变异率=',num2str(p_mut),',交叉率=', num2str(p_crs), '的进化曲线']);
legend('最短路径长度值', 'Location', 'northeast');
set(gca,'FontSize',16);


d_saga = d;
save('d_saga');

load('d_saga.mat');
figure,
plot(d, 'linewidth', 1.2), hold on,
plot(d_saga, 'linewidth', 1.2);
title(['变异率=',num2str(p_mut),',交叉率=', num2str(p_crs), '的进化曲线']);
legend('SAGA最优路径值', 'GA最优路径值', 'Location', 'northeast');
set(gca,'FontSize',16);