遗传算法之路径规划matlab代码（栅格地图）含详细注释 |
您所在的位置：网站首页 › 遗传算法matlab程序博客 › 遗传算法之路径规划matlab代码（栅格地图）含详细注释 |
遗传算法之路径规划matlab代码（栅格地图）含详细注释
|
遗传算法本人在另一篇博文中已经有记载，本次将遗传算法用于路径规划的代码记录于此，用于大家一起学习 一起进步，如果有用，欢迎点赞。 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; end3.计算路径长度函数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 end4.计算路径平滑度函数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 1 path_smooth(1, i) = path_smooth(1, i) + 30; %若等于1，说明此栅格与隔一个的栅格是上下或左右相邻，其路径不如直接从此格到邻格，显然冗余了。 elseif c2 = 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 end8.创建具有障碍物的栅格地图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; % 沿每个坐标轴使用相同的数据单位，保持一致
链接: https://pan.baidu.com/s/19Z0huAtdR5x2WmBQdAbhHg 提取码: uzdu |
如果急看，可至此处，压缩包包含代码和本人学习时候的草稿。喜欢请点赞哦。【本文地址】
公司简介
联系我们
今日新闻 |
点击排行 |
|
推荐新闻 |
图片新闻 |
|
专题文章 |