遗传算法之路径规划matlab代码（栅格地图）含详细注释

2023-03-31

路径 pop 栅格

遗传算法本人在另一篇博文中已经有记载，本次将遗传算法用于路径规划的代码记录于此，用于大家一起学习一起进步，如果有用，欢迎点赞。1.基于遗传算法的栅格法机器人路径规划main.m%基于遗传算法的栅格法机器人路径规划%jubobolv369clc;clear;%输入数据,即栅格地图.20行20列Grid

遗传算法本人在另一篇博文中已经有记载，本次将遗传算法用于路径规划的代码记录于此，用于大家一起学习一起进步，如果有用，欢迎点赞。

1.基于遗传算法的栅格法机器人路径规划main.m


% 基于遗传算法的栅格法机器人路径规划
%jubobolv369
clc;
clear;
% 输入数据,即栅格地图.20行20列
Grid=  [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];
 
start_num = 0;    % 起点编号
end_num = 399;    % 终点序号
NP = 200;       % 种群数量
max_gen = 50;  % 最大进化代数
pc = 0.8;      % 交叉概率
pm = 0.2;      % 变异概率
a = 1;         % 路径长度比重
b = 7;         % 路径顺滑度比重
z = 1;         
new_pop1 = {}; % 元胞数组，存放路径
[y, x] = size(Grid);
% 起点所在列（从左到右编号1.2.3...）
start_column = mod(start_num, x) + 1; 
% 起点所在行（从上到下编号行1.2.3...）
start_row = fix(start_num / x) + 1;  %Y = fix(X) 将 X 的每个元素朝零方向四舍五入为最近的整数
% 终点所在列、行
end_column = mod(end_num, x) + 1;
end_row = fix(end_num / x) + 1;
 
%% 种群初始化step1，必经节点,从起始点所在行开始往上，在每行中挑选一个自由栅格，构成必经节点
pass_num = end_row - start_row + 1;  %每条路径的节点个数
pop = zeros(NP, pass_num);%生成种群数量*节点个数的矩阵，用于存放每个个体的路径
for i = 1 : NP  %每个个体（每行）循环操作：
    pop(i, 1) = start_num;   %每行第一列都为起点(存入起点的编号)
    j = 1;
    % 此for循环用于寻找除去起点和终点所在行以外每行中的自由栅格
    for row_i = start_row+1 : end_row-1   %栅格的第二行到倒数第二行循环
        j = j + 1;
        % 存放栅格里当前行中的自由栅格序号
        free = []; 
        for column_i = 1 : x   %从第一列到第二十列中
            % 栅格对应的序号
            num = (column_i - 1) + (row_i - 1) * x;
% 如果该栅格为非障碍物
            if Grid(row_i, column_i) == 0
                % 把此栅格的编号加入free矩阵中
                free = [free num];
            end
        end     % 栅格一行里的自由栅格查询结束，自由栅格的编号存在了向量中
 
        free_num = length(free);
        % 产生小于等于本行自由栅格数量的一个随机整数
        index = randi(free_num); %X = randi(imax) 返回一个介于 1 和 imax 之间的伪随机整数标量。
        % 将栅格中当前行的自由栅格矩阵free中第index个栅格编号作为当前种群的第j个节点
        pop(i, j) = free(index);
      end  %该个体的每一行的路径节点产生完成,存入了pop的第i行中
    pop(i, end) = end_num; %pop的每行第最后一列都为终点(存入终点的编号)
    
 
 
 
 
%% 种群初始化step2将上述必经节点联结成无间断路径
    single_new_pop = generate_continuous_path(pop(i, :), Grid, x);
  
    if ~isempty(single_new_pop)%如果这一行种群的路径不是空的，将这行路径存入元胞数组中。
       new_pop1(z, 1) = {single_new_pop};
        z = z + 1;
    end
end
 
%% 计算初始化种群的适应度
% 计算路径长度
path_value = cal_path_value(new_pop1, x);
% 计算路径平滑度
path_smooth = cal_path_smooth(new_pop1, x);
fit_value = a .* path_value .^ -1 + b .* path_smooth .^ -1;
 
mean_path_value = zeros(1, max_gen);
min_path_value = zeros(1, max_gen);
%% 循环迭代操作
for i = 1 : max_gen
    % 选择操作
    new_pop2 = selection(new_pop1, fit_value);
    % 交叉操作
    new_pop2 = crossover(new_pop2, pc);
    % 变异操作
    new_pop2 = mutation(new_pop2, pm, Grid, x);
    % 更新种群
    new_pop1 = new_pop2;
    % 计算适应度值
    % 计算路径长度
    path_value = cal_path_value(new_pop1, x)
    % 计算路径平滑度
    path_smooth = cal_path_smooth(new_pop1, x)
    fit_value = a .* path_value .^ -1 + b .* path_smooth .^ -1
    mean_path_value(1, i) = mean(path_value);
    [~, m] = max(fit_value);
    min_path_value(1, i) = path_value(1, m);
end
%% 画每次迭代平均路径长度和最优路径长度图
figure(1)
plot(1:max_gen,  mean_path_value, 'r')
hold on;
title(['a = ', num2str(a)', '，b = ',num2str(b)','的优化曲线图']); 
xlabel('迭代次数'); 
ylabel('路径长度');
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(['a = ', num2str(a)', '，b = ',num2str(b)','遗传算法机器人运动轨迹']); 
xlabel('坐标x'); 
ylabel('坐标y');
DrawMap(Grid);
[~, 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')

2.将必经节点联结成无间断路径，如果结点间不连续，则插入节点使其连续generate_continuous_path.m


% 将必经节点联结成无间断路径，如果结点间不连续，则插入节点使其连续。
%jubobolv369
function [single_new_pop] = generate_continuous_path(single_pop, Grid, x)
i = 1;
single_new_pop = single_pop;  %传入的某行的初始路径，有20个路径节点
[~, single_path_num] = size(single_new_pop);
%遍历该行的所有节点，使其连续
while i ~= single_path_num
%%定位第i、i+1个节点的坐标
    % 路径中第i个栅格在地图的列（从左到右编号1.2.3...）
    column_now = mod(single_new_pop(1, i), x) + 1; 
    % 路径中第i个栅格在地图的行（从上到下编号行1.2.3...）
    row_now = fix(single_new_pop(1, i) / x) + 1;
    % 路径中第i+1个栅格在地图的列、行
    column_next = mod(single_new_pop(1, i + 1), x) + 1;
    row_next = fix(single_new_pop(1, i + 1) / x) + 1;
    
    % 初始化最大迭代次数
    max_iteration = 0;
    
    %% 判断点i和i+1是否连续,若不连续插入值(如果前后两节点的X坐标与Y坐标的差中较大值不等于1，说明不连续)
while max(abs(column_next - column_now), abs(row_next - row_now)) ~= 1
%取两节点的中点作为插入点,见forGA_word.xls-sheet1
%插入点的横坐标 x_insert，纵坐标 y_insert
        x_insert = floor((column_next + column_now) / 2);%Y = floor(X) 将 X 的每个元素四舍五入到小于或等于该元素的最接近整数。
        y_insert = floor((row_next + row_now) / 2);
        
        % 插入栅格为自由栅格
        if Grid(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 Grid(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 Grid(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 Grid(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  Grid(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
        
        column_next = x_insert;
        row_next = y_insert;
        max_iteration = max_iteration + 1;
%如果可以不断的增加新节点，但增加次数超过20000次，则舍弃此路径
        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

3.计算路径长度函数cal_path_value.m


%% 计算路径长度函数
%jubobolv369
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);
    %路径有m个栅格，需要计算m-1次
    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;
        %如果相邻两个栅格为上下或左右，路径长度加1，否则为对角线，长度加根号2
        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

4.计算路径平滑度函数cal_path_smooth.m


%% 计算路径平滑度函数
%jubobolv369
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);
    %路径有m个栅格，需要计算m-1次
    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;
        %path_smooth(1, i) = path_smooth(1, i) + abs(atan(abs(x_now - x_next1)/abs(y_now - y_next1))-atan(abs(x_next2 - x_next1)/abs(y_next2 - y_next1)));
        %a2 = (x_now - x_next1)^2 + (y_now - y_next1)^2;
        %b2 = (x_next2 - x_next1)^2 + (y_next2 - y_next1)^2;
        c2 = (x_now - x_next2)^2 + (y_now - y_next2)^2;
        %angle = (a2 + c2 - b2) / (2 * sqrt(a2) *  sqrt(c2));
        %若大于4小于等于8，说明此栅格与隔一个的栅格隔一行或一列且列或行相邻
        if c2 < 8 && c2 > 4
            path_smooth(1, i) = path_smooth(1, i) + 5;
        %若大于1小于等于4，说明此栅格与隔一个的栅格为对角，也可能或同行或同列垮了一格
        elseif c2 <= 4 && c2 > 1
            path_smooth(1, i) = path_smooth(1, i) + 30;
        %若等于1，说明此栅格与隔一个的栅格是上下或左右相邻，其路径不如直接从此格到邻格，显然冗余了。
        elseif    c2 <= 1
            path_smooth(1, i) = path_smooth(1, i) + 5000;
        %否则不设置值，也即值为0，此时此栅格与隔一个的栅格是正方形对角的关系，最好。
        end
    end
end

5.用轮盘堵法选择新的个体selection.m


%% 用轮盘堵法选择新的个体
% 输入变量：pop元胞种群，fitvalue：适应度值
% 输出变量：newpop选择以后的元胞种群
%jubobolv369
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);    % B = cumsum(A) 从 A 中的第一个其大小不等于 1 的数组维度开始返回 A 的累积和。
% 随机数从小到大排列
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

6.交叉操作crossover.m


%%交叉操作
%输入变量：pop：父代种群，pc：交叉的概率
%输出变量：newpop：交叉后的种群
%jubobolv369
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};
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
       %%         A = [5 3 4 2];                %%
       %%         B = [2 4 4 4 6 8];            %%
       %%        [Lia,Locb] = ismember(A,B)     %%
       %%         Lia = 1x4 logical array       %%A的每个元素若B中存在则该位为1 否则为零
       %%                 0   0   1   1         %%
       %%         Locb = 1×4                   %%每个相同的元素在B中的索引
       %%                 0     0     2     1   %%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        [lia, locb] = ismember(singal_now_pop, singal_next_pop);%[Lia,Locb] = ismember(A,B)确定 A 的哪些元素同时也在 B 中及其在 B 中的相应位置。
        [~, n] = find(lia == 1);%要查找特定的整数值，使用 == 运算符。返回找到的值在lia中的索引
        [~, m] = size(n);
        %如果随机数小于交叉概率且A中有三个以上路径节点与B中的相同
    if (rand < pc) && (m >= 3)
        % 生成一个2到m-1之间的随机数，也就是除去开头和结尾，在两条路径的相同节点中随机选取一个节点用于交叉
        r = round(rand(1,1)*(m-3)) +2;%Y = round(X) 将 X 的每个元素四舍五入为最近的整数
        crossover_index1 = n(1, r);%
        crossover_index2 = locb(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的条件进行了是否交叉的处理，所以最后一条仍然不变。
if parity == 1
    new_pop{px, 1} = pop{px, 1};
end
end

7.变异操作mutation.m


%% 变异操作
% 函数说明
% 输入变量：pop：种群，pm：变异概率
% 输出变量：newpop变异以后的种群
%jubobolv369
function [new_pop] = mutation(pop, pm, Grid, 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, Grid, 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

8.创建具有障碍物的栅格地图DrawMap.m


%创建具有障碍物的栅格地图
%矩阵中1代表黑色栅格
%jubobolv369
function Grid = DrawMap(Grid)
b = Grid;
b(end+1,end+1) = 0;
colormap([1 1 1;0 0 0]);  % 创建颜色
pcolor(0.5:size(Grid,2) + 0.5, 0.5:size(Grid,1) + 0.5, b); % 赋予栅格颜色
set(gca, 'XTick', 1:size(Grid,1), 'YTick', 1:size(Grid,2));  % 设置坐标
axis image xy;  % 沿每个坐标轴使用相同的数据单位，保持一致