【路径规划】模拟退火优化遗传算法求解避障路径规划问题【Matlab 106期】

一、简介

路径规划主要是让目标对象在规定范围内的区域内找到一条从起点到终点的无碰撞安全路径。路径规划中有静态路径规划以及动态路径规划，本文所讨论的问题仅针对静态路径规划。具体问题描述如下：
给定起点、终点和障碍物等环境信息，如图1.1所示，利用演化计算方法得到最优轨迹，并分析不同参数选择对结果的影响。本文采用遗传算法和模拟退火混合遗传算法进行求解，得到最优路径。
1 遗传算法
1.1 遗传算法原理
遗传算法（Genetic Algorithm，GA）[1]是进化计算的一部分，是模拟达尔文的遗传选择和自然淘汰的生物进化过程的计算模型，是一种通过模拟自然进化过程搜索最优解的方法。该算法简单、通用，鲁棒性强，适于并行处理。基本遗传算法流程包括种群初始化、选择、交叉、变异等操作，如图1所示。

遗传算法的进化过程从完全随机生成的种群开始，随后逐代向适应度更高的种群进化。在进化的每一代中，整个种群的适应度按照一定规则被评价，基于个体的适应度高低从当前种群中随机选择多个个体，通过个体自然选择和基因突变操作而产生新种群，生成的新种群在算法的下一次迭代中成为当前种群。在遗传算法中，需解决的问题的解通常被成为基因个体，它通常以参数列表的形式进行标示，被称作染色体或基因串。其中染色体通常以编码形式体现，即表达为简单的字符串或数字串[2]。

针对本问题，使用遗传算法求解最优路径需要按照特定规则初始化种群，并且根据问题所给的限定条件设计合适的适应度计算方法。下面的内容会详细说明这两个步骤，而算法中的其他操作，如选择、交叉、变异分别采用经典的轮盘赌、单点交叉和随机变异方法，这里不再赘述。

1.2 编码

1.3 适应度函数
适应度函数要有效反映每一个体与问题的最优个体之间的差距。遗传算法在进化搜索中基本不利用外部信息,仅以适应度函数为依据,利用种群中每个个体的适应度值来进行搜索。本问题的目标是路径总长度为最短，路径总长度的倒数就可以作为适应度函数：



2 混合遗传算法
为了提高遗传算法运行效率和求解质量，这里引入模拟退火算法的思想，模拟退火(Stimulated Annealing, SA)具有较强的局部寻优能力，并能使搜索过程避免陷入局部最优解[3-4]，采用模拟退火遗传算法（Stimulated Annealing Genetic Algorithm，SAGA）求解路径优化问题。
具体来说，模拟退火遗传算法是对适应度函数的改进，在前期（高温）减少个体间的适应度差异，后期（低温）放大个体间适应度差异，突出优秀个体，完成适应度拉伸的作用，在一定程度上弥补了遗传算法在前期容易早熟、后期容易停滞的缺点[5]。具体的适应度值计算公式如式（6）和（7）所示：

二、源代码

%%%%%%模拟退火遗传算法(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;
%====================================
%%输入参数：种群——x,y横纵坐标
%%输出参数：种群中每个个体的适应度值
%%说明：
%     逐个计算种群中个体的适应度值，判断该个体所连成线路与圆形障碍物之间的关系，
%     将满足条件的路径的距离倒数作为适应度值
%====================================

function fitval=fitness(x,y)
    %obstacle
    xobs=[1.5 4.0 1.2];
    yobs=[6.5 3.0 1.5];
    robs=[1.5 1.0 0.8];
    [n, xn] = size(x);
    for i=1:1:n
        cnt_line = 0;  %记录穿过障碍物的线段数
        
        %%拆分相邻的两个点，便于后续计算
        for m=1:1:xn-1
            x1(m)=x(i,m);
            y1(m)=y(i,m);
        end
        for n=2:1:xn
            x2(n-1)=x(i,n);
            y2(n-1)=y(i,n);
        end
        
        %%判断线段与障碍物的关系
        for m = 1:size(x1,2)
            A = y2(m) - y1(m);
            B = x1(m) - x2(m);
            C = y1(m)*(x2(m)-x1(m)) - x1(m)*(y2(m)-y1(m));
            for ii = 1:3
                if ((x1(m)-xobs(ii))^2 + (y1(m)-yobs(ii))^2) < robs(ii)^2 || ((x2(m)-xobs(ii))^2 + (y2(m)-yobs(ii))^2) < robs(ii)^2
                    % disp('有点在圆内')
                    cnt_line = cnt_line + 1;
                    continue;
                else            
                    d = abs(A*xobs(ii)+B*yobs(ii)+C) / sqrt(A^2+B^2);
                    if d==0
                        d1 = (xobs(ii)-x1(m))^2 + (yobs(ii)-y1(m))^2;
                        d2 = (x2(m)-x1(m))^2 + (y2(m)-y1(m))^2;
                        if d1 < d2
                            cnt_line = cnt_line + 1;   % 该线段过圆心且穿过圆
                        end

                    elseif d < robs(ii)
                        cos1 = (x2(m)-x1(m))*(xobs(ii)-x1(m)) + (y2(m)-y1(m))*(yobs(ii)-y1(m));
                        cos2 = (x1(m)-x2(m))*(xobs(ii)-x2(m)) + (y1(m)-y2(m))*(yobs(ii)-y2(m));
                        if cos1>0 && cos2>0
                            cnt_line = cnt_line + 1;    % 该线段与圆交与两点
                        end
                    else
                        continue;
                    end
                end
            end
         
        end

三、运行结果

四、备注

版本：2014a
完整代码或代写加QQ912100926

标签：种群,避障,end,route,路径,模拟退火,ii,适应度,遗传算法
来源： https://blog.csdn.net/m0_54742769/article/details/117073056