遗传算法的求解路径规划问题python代码 遗传算法解决路径规划

文章目录

• 一、遗传算法

• 1.1 编码与解码
• 1.2 选择算子-轮盘赌法
• 1.3 交叉算子
• 1.4 变异算子
• 1.5 遗传算法流程
• 1.6 基于遗传算法的栅格法机器人路径规划

• 二、采用模拟退火算法改善适应度函数

一、遗传算法

遗传算法 (Genetic AIgorithm, 简称 GA)起源于对生物系统所进行的计算机模拟研究，是一种随机全局搜索优化方法，它模拟了自然选择和遗传中发生的复制、交叉 (crossover) 和变异 (mutation)现象，从任一初始种群 (PopuIation) 出发，通过随机选择、交叉和变异操作，产生一群更适合环境的个体，使群体进化到搜索空间中越来越好的区域，这样一代一代不断繁衍进化，最后收敛到一群最适应环境的个体(lndividual) ，从而求得问题的优质解。

1.1 编码与解码

实现遗传算法的第一步就是明确对求解问题的编码和解码方式。所谓编码就是把要传递的信息按照一定的规则进行组织，所谓解码就是把收到的信息按照一定的规则进行解忻，且这个规则必须是编码者和解码者事先都知道或约定好的。

对于函数优化问题，一般有两种编码方式，各具优缺点

• 实数编码：直接用实数表示基因，容易理解且不需要解码过程、但容易过早收敛，从而陷入局部最优
• 二进制编码：稳定性高，种群多样性大，需要的存储空间大，需要解码且难以理解。

1.2 选择算子-轮盘赌法

轮盘赌选择法是依据个体的适应度值计算每个个体在子代中出现的概率，并按照此概率随机选择个体构成子代种群，因此该方法也被称为适应度比例法。轮盘赌选择策略的出发点是适应度值越好的个体被选择的概率越大。因此，在求解最大化问题的时候，我们可以直接采用适应度值来进行选择。但是在求解最小化问题的时候，我们必须首先将问题的适应度函数进行转换(如采用倒数或相反数)以将问题转化为最大化问题。

1.3 交叉算子

交叉操作是指从种群中随机选择两个个体，通过两个染色体的交换组合，把父串的优秀特征遗传给子串，从而产生新的优秀个体。体现了信息交换的思想。在实际应用中，使用率最高的是单点交叉算子，该算子在配对的染色体中随机的选择一个交叉位置，然后在该交叉位置对配对的染色体进行基因位变。其他交叉算子包括：双点交叉或多点交叉，均匀交叉，算术交叉。

1.4 变异算子

对种群中的每一个个体，以变异概率改变一个或多个基因座上的基因值为其他的等位基因。同生物界中一样，变异发生的概率很低，变异为新个体的产生了机会。

变异率一般可取0.001~0.1。变异率不能取得太大，如果大于0.5，遗传算法就会退化为随机搜索。

1.5 遗传算法流程

• 对于一个实际问题，首先通过编码将解空间的数据表示为基因型串结构
• 然后进行种群初始化，随机产生N个初始串结构数据，每个串结构数据称为一个个体，N个个体构成了一个群体。
• 适应度表明个体或解的优劣性。
• 选择的原则是适应性强的个体，为下一代贡献一个或多个后代的概率大。
• 交叉操作可以得到新一代个体，新个体组合体现了其父辈个体的特性。
• 变异过程以一定的概率随机地改变串结构数据中某个串的值。
• 新种群满足条件时，停止进化。

1.6 基于遗传算法的栅格法机器人路径规划

机器人控制过程中，路径规划是其中重要的一环。因此本文采用遗传算法对机器人移动时的路径进行优化，最终选取出一条最优路径。采用栅格图法为机器人移动的地图进行建模，并根据遗传算法机理，在MATLAB上仿真实现了最优路径的选取。

首先对其进行编码，用0~24表示每个方格，如图所示路径编码为(0,1,7,13,19,24)。

种群初始化

• 每行选择一个栅格
• 判断相邻栅格是否连续

• 不连续时进行插入栅格操作，直到连续

选择：如图所示有①②两条路径，对应两个个体。选择路径长度和路径平滑性作为评价指标：

交叉：如上图所示有①②两条路径，对应两个个体。通过交换交叉点前后的路径可形成新的路径。

变异：随机选择路径中的两个栅格。 采用种群初始化中的方法在两个栅格间产生新路径。

main

% 基于遗传算法的栅格法机器人路径规划
clc;
clear;
% 输入数据,即栅格地图
G=  [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
     0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
     0 0 1 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;
     0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
     0 1 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 0 1 1 1 0 1 0 1 1 0 0 0 0 0 0;
     0 1 1 1 0 0 0 0 0 0 1 0 1 1 0 0 0 0 0 0;
     0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 0 0 1 1 0 1 1 1 1 0 0 0 0 0 0;
     0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 1 1 1 1 0;
     0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0;
     0 0 1 1 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 0;
     0 0 1 1 0 0 1 1 1 0 1 0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 0 1 1 1 0 1 0 0 0 0 0 0 1 1 0; 
     0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 1 1 0;
     0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0;
     0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0];
%G = [0 0 0 1 0;
%     0 0 0 0 0;
%     0 0 1 0 0;
%     1 0 1 0 0;
%     0 0 0 0 0;];
 
p_start = 0;   % 起始序号
p_end = 399;   % 终止序号
NP = 200;      % 种群数量
max_gen = 50;  % 最大进化代数
pc = 0.8;      % 交叉概率
pm = 0.2;      % 变异概率
%init_path = [];
z = 1;  
new_pop1 = {}; % 元包类型路径
[y, x] = size(G);
% 起点所在列（从左到右编号1.2.3...）
xs = mod(p_start, x) + 1; 
% 起点所在行（从上到下编号行1.2.3...）
ys = fix(p_start / x) + 1;
% 终点所在列、行
xe = mod(p_end, x) + 1;
ye = fix(p_end / x) + 1;

% 种群初始化step1，必经节点,从起始点所在行开始往上，在每行中挑选一个自由栅格，构成必经节点
pass_num = ye - ys + 1;
pop = zeros(NP, pass_num);
for i = 1 : NP
    pop(i, 1) = p_start;
    j = 1;
    % 除去起点和终点
    for yk = ys+1 : ye-1
        j = j + 1;
        % 每一行的可行点
        can = []; 
        for xk = 1 : x
            % 栅格序号
            no = (xk - 1) + (yk - 1) * x;
            if G(yk, xk) == 0
                % 把点加入can矩阵中
                can = [can no];
            end
        end
        can_num = length(can);
        % 产生随机整数
        index = randi(can_num);
        % 为每一行加一个可行点
        pop(i, j) = can(index);
    end
    pop(i, end) = p_end;
    %pop
    % 种群初始化step2将上述必经节点联结成无间断路径
    single_new_pop = generate_continuous_path(pop(i, :), G, x);
    %init_path = [init_path, single_new_pop];
    if ~isempty(single_new_pop)
       new_pop1(z, 1) = {single_new_pop};
        z = z + 1;
    end
end

mean_path_value = zeros(1, max_gen);
min_path_value = zeros(1, max_gen);
% 循环迭代操作
for i = 1 : max_gen
    % 计算适应度值
    % 计算路径长度
    path_value = cal_path_value(new_pop1, x)
    % 计算路径平滑度
    path_smooth = cal_path_smooth(new_pop1, x)
    fit_value = 1 .* path_value .^ -1 + 1 .* path_smooth .^ -1;
    mean_path_value(1, i) = mean(path_value);
    [~, m] = max(fit_value);
    min_path_value(1, i) = path_value(1, m);
    
    % 选择操作
    new_pop2 = selection(new_pop1, fit_value);
    % 交叉操作
    new_pop2 = crossover(new_pop2, pc);
    % 变异操作
    new_pop2 = mutation(new_pop2, pm, G, x);
    % 更新种群
    new_pop1 = new_pop2;
    
end
% 画每次迭代平均路径长度和最优路径长度图
figure(1)
plot(1:max_gen,  mean_path_value, 'r')
hold on;
plot(1:max_gen, min_path_value, 'b')
legend('平均路径长度', '最优路径长度');
min_path_value(1, end)
% 在地图上画路径
[~, min_index] = max(fit_value);
min_path = new_pop1{min_index, 1};
figure(2)
hold on;
title('遗传算法机器人运动轨迹'); 
xlabel('坐标x'); 
ylabel('坐标y');
DrawMap(G);
[~, min_path_num] = size(min_path);
for i = 1:min_path_num
    % 路径点所在列（从左到右编号1.2.3...）
    x_min_path(1, i) = mod(min_path(1, i), x) + 1; 
    % 路径点所在行（从上到下编号行1.2.3...）
    y_min_path(1, i) = fix(min_path(1, i) / x) + 1;
end
hold on;
plot(x_min_path, y_min_path, 'r')

selection

% 用轮盘赌法选择新的个体
% 输入变量：pop元胞种群，fitvalue：适应度值
% 输出变量：newpop选择以后的元胞种群
function [new_pop] = selection(pop, fit_value)
%构造轮盘
[px, ~] = size(pop);
total_fit = sum(fit_value);
p_fit_value = fit_value / total_fit;
p_fit_value = cumsum(p_fit_value);    % 概率求和
% 随机数从小到大排列
ms = sort(rand(px, 1));
fitin = 1;
newin = 1;
while newin <= px
    if(ms(newin)) < p_fit_value(fitin)
        new_pop{newin, 1} = pop{fitin, 1};
        newin = newin+1;
    else
        fitin = fitin+1;
    end
end

mutation

% 变异操作
% 函数说明
% 输入变量：pop：种群，pm：变异概率
% 输出变量：newpop变异以后的种群
function [new_pop] = mutation(pop, pm, G, x)
[px, ~] = size(pop);
new_pop = {};
for i = 1:px
    % 初始化最大迭代次数
    max_iteration = 0;
    single_new_pop = pop{i, 1};
    [~, m] = size(single_new_pop);
    % single_new_pop_slice初始化
    single_new_pop_slice = [];
    if(rand < pm)
        while isempty(single_new_pop_slice)
            % 生成2-（m-1）的两个随机数,并排序
            mpoint = sort(round(rand(1,2)*(m-3)) + [2 2]);
            single_new_pop_slice = [single_new_pop(mpoint(1, 1)-1) single_new_pop(mpoint(1, 2)+1)];
            single_new_pop_slice = generate_continuous_path(single_new_pop_slice, G, x);
            %max_iteration = max_iteration + 1;
            if max_iteration >= 100000
                break
            end
        end
        if max_iteration >= 100000
            new_pop{i, 1} = pop{i, 1};
        else
            new_pop{i, 1} = [single_new_pop(1, 1:mpoint(1, 1)-1), single_new_pop_slice(2:end-1), single_new_pop(1, mpoint(1, 2)+1:m)];
        end
        % single_new_pop_slice再次初始化
        single_new_pop_slice = [];
    else
        new_pop{i, 1} = pop{i, 1};
    end
end

generate_continuous_path

% 将必经节点联结成无间断路径
function [single_new_pop] = generate_continuous_path(single_pop, G, x)
i = 1;
single_new_pop = single_pop;
[~, single_path_num] = size(single_new_pop);
while i ~= single_path_num
    % 点i所在列（从左到右编号1.2.3...）
    x_now = mod(single_new_pop(1, i), x) + 1; 
    % 点i所在行（从上到下编号行1.2.3...）
    y_now = fix(single_new_pop(1, i) / x) + 1;
    % 点i+1所在列、行
    x_next = mod(single_new_pop(1, i + 1), x) + 1;
    y_next = fix(single_new_pop(1, i + 1) / x) + 1;
    
    % 初始化最大迭代次数
    max_iteration = 0;
    
    % 判断点i和i+1是否连续,若不连续插入值
    while max(abs(x_next - x_now), abs(y_next - y_now)) > 1
        x_insert = floor((x_next + x_now) / 2);
        y_insert = floor((y_next + y_now) / 2);
        
        % 插入栅格为自由栅格
        if G(y_insert, x_insert) == 0  
            % 栅格序号
            num_insert = (x_insert - 1) + (y_insert - 1) * x;
            % 插入新序号
            single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];
            
        % 插入栅格为障碍物栅格
        else   
            % 往左走
            if G(y_insert, x_insert - 1) == 0 && ((x_insert - 2) + (y_insert - 1) * x ~= single_new_pop(1, i)) && ((x_insert - 2) + (y_insert - 1) * x ~= single_new_pop(1, i+1))
                x_insert = x_insert - 1;
                % 栅格序号
                num_insert = (x_insert - 1) + (y_insert - 1) * x;
                % 插入新序号
                single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];
                               
            % 往右走    
            elseif G(y_insert, x_insert + 1) == 0 && (x_insert + (y_insert - 1) * x ~= single_new_pop(1, i)) && (x_insert + (y_insert - 1) * x ~= single_new_pop(1, i+1))
                x_insert = x_insert + 1;
                % 栅格序号
                num_insert = (x_insert - 1) + (y_insert - 1) * x;
                % 插入新序号
                single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];
                
            % 向上走
            elseif G(y_insert + 1, x_insert) == 0 && ((x_insert - 1) + y_insert * x ~= single_new_pop(1, i)) && ((x_insert - 1) + y_insert * x ~= single_new_pop(1, i+1))
                y_insert = y_insert + 1;
                % 栅格序号
                num_insert = (x_insert - 1) + (y_insert - 1) * x;
                % 插入新序号
                single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];

            % 向下走
            elseif  G(y_insert - 1, x_insert) == 0 && ((x_insert - 1) + (y_insert - 2) * x ~= single_new_pop(1, i)) && ((x_insert - 1) + (y_insert-2) * x ~= single_new_pop(1, i+1))
                y_insert = y_insert - 1;
                % 栅格序号
                num_insert = (x_insert - 1) + (y_insert - 1) * x;
                % 插入新序号
                single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];
                
            % 其他情况舍去此路径
            else
                %break_pop = single_new_pop
                single_new_pop = [];
                break
            end    
        end
        
        x_next = x_insert;
        y_next = y_insert;
        max_iteration = max_iteration + 1;
        if max_iteration > 20000
            single_new_pop = [];
            break
        end
        
    end
    
    if isempty(single_new_pop)
        break
    end
    
    [~, single_path_num] = size(single_new_pop);
    i = i + 1;
end

DrawMap

%创建具有障碍物的栅格地图
%矩阵中1代表黑色栅格

function G = DrawMap(G)
b = G;
b(end+1,end+1) = 0;
colormap([1 1 1;0 0 0]);  % 创建颜色
pcolor(0.5:size(G,2) + 0.5, 0.5:size(G,1) + 0.5, b); % 赋予栅格颜色
set(gca, 'XTick', 1:size(G,1), 'YTick', 1:size(G,2));  % 设置坐标
axis image xy;  % 沿每个坐标轴使用相同的数据单位，保持一致

crossover

%交叉变换
%输入变量：pop：父代种群，pc：交叉的概率
%输出变量：newpop：交叉后的种群
function [new_pop] = crossover(pop, pc)
[px,~] = size(pop);
% 判断路径点数是奇数或偶数
parity = mod(px, 2);
new_pop = {};
for i = 1:2:px-1
        singal_now_pop = pop{i, 1};
        singal_next_pop = pop{i+1, 1};
        [lia, lib] = ismember(singal_now_pop, singal_next_pop);
        [~, n] = find(lia == 1);
        [~, m] = size(n);
    if (rand < pc) && (m >= 3)
        % 生成一个2-m-1之间的随机数
        r = round(rand(1,1)*(m-3)) +2;
        crossover_index1 = n(1, r);
        crossover_index2 = lib(crossover_index1);
        new_pop{i, 1} = [singal_now_pop(1:crossover_index1), singal_next_pop(crossover_index2+1:end)];
        new_pop{i+1, 1} = [singal_next_pop(1:crossover_index2), singal_now_pop(crossover_index1+1:end)];
        
    else
        new_pop{i, 1} =singal_now_pop;
        new_pop{i+1, 1} = singal_next_pop;
    end
if parity == 1
    new_pop{px, 1} = pop{px, 1};
end
end

cal_path_value

% 计算路径长度函数
function [path_value] = cal_path_value(pop, x)
[n, ~] = size(pop);
path_value = zeros(1, n);
for i = 1 : n
    single_pop = pop{i, 1};
    [~, m] = size(single_pop);
    for j = 1 : m - 1
        % 点i所在列（从左到右编号1.2.3...）
        x_now = mod(single_pop(1, j), x) + 1; 
        % 点i所在行（从上到下编号行1.2.3...）
        y_now = fix(single_pop(1, j) / x) + 1;
        % 点i+1所在列、行
        x_next = mod(single_pop(1, j + 1), x) + 1;
        y_next = fix(single_pop(1, j + 1) / x) + 1;
        if abs(x_now - x_next) + abs(y_now - y_next) == 1
            path_value(1, i) = path_value(1, i) + 1;
        else
            path_value(1, i) = path_value(1, i) + sqrt(2);
        end
    end
end

cal_path_smooth

% 计算路径平滑度函数
function [path_smooth] = cal_path_smooth(pop, x)
[n, ~] = size(pop);
path_smooth = zeros(1, n);
for i = 1 : n
    single_pop = pop{i, 1};
    [~, m] = size(single_pop);
    for j = 1 : m - 2
        % 点i所在列（从左到右编号1.2.3...）
        x_now = mod(single_pop(1, j), x) + 1; 
        % 点i所在行（从上到下编号行1.2.3...）
        y_now = fix(single_pop(1, j) / x) + 1;
        % 点i+1所在列、行
        x_next1 = mod(single_pop(1, j + 1), x) + 1;
        y_next1 = fix(single_pop(1, j + 1) / x) + 1;
        % 点i+2所在列、行
        x_next2 = mod(single_pop(1, j + 2), x) + 1;
        y_next2 = fix(single_pop(1, j + 2) / x) + 1;
        % cos A=(b?+c?-a?)/2bc
        b2 = (x_now - x_next1)^2 + (y_now - y_next1)^2;
        c2 = (x_next2 - x_next1)^2 + (y_next2 - y_next1)^2;
        a2 = (x_now - x_next2)^2 + (y_now - y_next2)^2;
        cosa = (c2 + b2 - a2) / (2 * sqrt(b2) *  sqrt(c2));
        angle = acosd(cosa);
        
        if angle < 170 && angle > 91
            path_smooth(1, i) = path_smooth(1, i) + 3;
        elseif angle <= 91 && angle > 46
            path_smooth(1, i) = path_smooth(1, i) + 15;
        elseif angle <= 46
            path_smooth(1, i) = path_smooth(1, i) + 20;
        end
    end
end

运行效果：

二、采用模拟退火算法改善适应度函数

为了提高遗传算法运行效率和求解质量，这里引入模拟退火算法的思想，模拟退火(Stimulated Annealing, SA)具有较强的局部寻优能力，并能使搜索过程避免陷入局部最优解，采用模拟退火遗传算法（Stimulated Annealing Genetic Algorithm，SAGA）求解路径优化问题。

模拟退火遗传算法是对适应度函数的改进，在前期（高温）减少个体间的适应度差异，后期（低温）放大个体间适应度差异，突出优秀个体，完成适应度拉伸的作用，在一定程度上弥补了遗传算法在前期容易早熟、后期容易停滞的缺点。
具体的适应度值计算公式如下所示：

其中，$M$为种群大小， $f_i$为第𝑖个个体的适应度，$g$为遗传代数，$T$为温度， $T_0$为初始温度，$A$为退火速度（本文取$T_0 = 100, A = 0.8$）

SAGA_main

%%%%%%模拟退火遗传算法(SAGA)%%%%%%%%%%%
clc;
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_ga.mat');
figure,
plot(d, 'linewidth', 1.2), hold on,
plot(d_ga, 'linewidth', 1.2);
title(['变异率=',num2str(p_mut),',交叉率=', num2str(p_crs), '的进化曲线']);
legend('SAGA最优路径值', 'GA最优路径值', 'Location', 'northeast');
set(gca,'FontSize',16);

select

%====================================
%%输入参数：种群、其适应度、选择比例
%%输出参数：父辈种群——x,y横纵坐标
%%说明：
%     按照输入参数的比例，从种群中选择父代
%====================================
function [parentPopx, parentPopy]=select(popx, popy, fitness, ratio)
    totalfit=sum(fitness); %适应值之和
    accP=cumsum(fitness/totalfit); %概率累计和

    %轮盘赌选择算法
    for n=1:round(ratio*size(popx,1))
        mat=find(accP>rand); %找到比随机数大的累积概率
        if isempty(mat)
            continue
        end
        
        parentPopx(n,:)=popx(mat(1),:);%将首个比随机数大的累积概率的位置的个体遗传下去
        parentPopy(n,:)=popy(mat(1),:);
    end
end

saga_fitness

%====================================
%%输入参数：种群——x,y横纵坐标, 当前温度T
%%输出参数：种群中每个个体的适应度值
%%说明：
%     逐个计算种群中个体的适应度值，判断该个体所连成线路与圆形障碍物之间的关系，
%     将满足条件的路径的距离倒数作为适应度值
%====================================

function fitval=saga_fitness(x,y,T)
    %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
        
        %%%计算适应度
        if cnt_line > 0
            f(i)=0;       %路线穿过障碍物的个体适应度置零
        else
            for j=1:1:xn-1
                d(j)=sqrt((x(i,j+1)-x(i,j)).^2+(y(i,j+1)-y(i,j)).^2);     %该个体（即路线）的路径长度
            end
            f(i)=1.0/sum(d);       %取距离的倒数作为个体适应度
        end
        record_cnt(i,:) = cnt_line;
    end
    
    I = find(f ~= 0);
    sumf = sum(exp(f(I) ./ T)) + n - size(I, 1);
    f(I) = exp(f(I)./T) ./ sumf;
    fitval=f';
%     disp(['不满足条件的最小线段数为：',num2str(min(record_cnt, [], 1))])

end

popinit

%====================================
%%输入参数：种群数量，染色体长度
%%输出参数：初始种群
%%说明：
%     种群中的个体由一系列点组成，该点的x，y坐标分开存放，
%     除了起点和终点，其余点随机生成并从小到大排序
%====================================
function [x,y]=popinit(popsize,chromlength)

    x=5.0*rand(popsize,chromlength);
    y=8.9*rand(popsize,chromlength);
    x(:,1)=0;  % 设置起点
    y(:,1)=0;

    for i=1:1:size(x,1) 
        x(i,:)=sort(x(i,:));
        y(i,:)=sort(y(i,:));
    end 
    
    x(:,chromlength)=1.5;  %设置终点
    y(:,chromlength)=8.9;

end

mutation

%====================================
%%输入参数：父代种群，变异概率
%%输出参数：子代种群
%%说明：
%     随机替换，最后需要从小到大排序
%====================================
function [kidx, kidy]=mutation(x, y, p)
    [m, n] = size(x);
    kidx = x;
    kidy = y;
    for i = 1:1:m
        if(rand < p)
            point = round(rand*n);
            % 避免越界
            if point <= 1
                point = 2;
            end
            if point == n
               point = n-1;
            end
            % 变异：随机数替换
            kidx(i,point) = round(rand*n);
            kidy(i,point) = round(rand*n);
        end
    end
    for i = 1:1:m
        kidx(i,:) = sort(kidx(i,:));
        kidy(i,:) = sort(kidy(i,:));
    end 
end

fitness

%====================================
%%输入参数：种群——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
        
        %%%计算适应度
        if cnt_line > 0
            f(i)=0;       %路线穿过障碍物的个体适应度置零
        else
            for j=1:1:xn-1
                d(j)=sqrt((x(i,j+1)-x(i,j)).^2+(y(i,j+1)-y(i,j)).^2);     %该个体（即路线）的路径长度
            end
            f(i)=1.0/sum(d);       %取距离的倒数作为个体适应度
        end
        record_cnt(i,:) = cnt_line;
    end
    fitval=f';
%     disp(['不满足条件的最小线段数为：',num2str(min(record_cnt, [], 1))])

end

crossover

%====================================
%%输入参数：父代种群，交叉概率
%%输出参数：子代种群
%%说明：
%     单点交叉，最后需要从小到大排序
%====================================
function [kidx, kidy]=crossover(x, y, p)
    [m,n] = size(x);
    kidx = ones(size(x)); 
    kidy = ones(size(y)); 
    for i = 1:2:m-1
        if(rand < p)
            point = round(rand*n);
            kidx(i,:) = [x(i,1:point),x(i+1,point+1:n)];
            kidx(i+1,:) = [x(i+1,1:point),x(i,point+1:n)];
            kidy(i,:) = [y(i,1:point),y(i+1,point+1:n)];
            kidy(i+1,:) = [y(i+1,1:point),y(i,point+1:n)];
        else
            kidx(i,:) = x(i,:);
            kidx(i+1,:) = x(i+1,:);
            kidy(i,:) = y(i,:);
            kidy(i+1,:) = y(i+1,:);
        end
    end
    for i = 1:1:m
        kidx(i,:) = sort(kidx(i,:));
        kidy(i,:) = sort(kidy(i,:));
    end 
end

best

%====================================
%%输入参数：种群、其适应度
%%输出参数：种群中的最佳个体，及其适应度
%%说明：
%     选择最佳个体，便于后续分析
%====================================
function [bestx,besty,bestfit]=best(x,y,fitval)
    bestx = x(1,:);
    besty = y(1,:);
    bestfit = fitval(1);
    for i = 2:size(x,1)
        if fitval(i) > bestfit
            bestx = x(i,:);
            besty = y(i,:);
            bestfit=fitval(i);
        end
    end
end

运行效果