基于栅格地图——遗传算法的机器人最优路径规划 - Matlab在线 - matlabol

中文说明：

采用栅格对机器人的工作空间进行划分，再利用优化算法对机器人路径优化，是采用智能算法求最优路径的一个经典问题。目前，采用蚁群算法在栅格地图上进行路径优化取得比较好的效果，而利用遗传算法在栅格地图上进行路径优化在算法显得更加难以实现。

利用遗传算法处理栅格地图的机器人路径规划的难点主要包括：1保证路径不间断，2保证路径不穿过障碍。

用遗传算法解决优化问题时的步骤是固定的，就是种群初始化，选择，交叉，变异，适应度计算这样，那么下面我就说一下遗传算法求栅格地图中机器人路径规划在每个步骤的问题、难点以及解决办法。

1、种群初始化

首先要知道遗传算法种群初始化的定义。遗传算法种群初始化是生成一定种群数量的个体，每个个体是一个可行解。这里要注意是可行解，对于该问题也就是说是一个可行路径，也就是说应该是一个从路径起点到路径终点的，且不经过障碍的路径。怎样进行初始化种群得到可行路径是遗传算法求栅格路径的第一个难点也是最大难点。

方法

路径初始化可以分为两步：第一步生成必经节点路径，什么是必经节点路径？举个例子，比如对于10*10的栅格，从左下角编号1到右上角编号100的路径必经过第2行的一个点，第3行一个点……第9行的一个点，这是很显然的，那么我们在第二行的自由栅格中随机取一个节点、第3行的自由栅格中取一个节点……第9行自由栅格取一个节点，那么这就行程了一个必经点路径，当然这个路径也是间断的。所以需要第二步，第二步就是连接这些间断节点，而这个问题是该算法中最难的问题，因为在连接路径中，太难绕开障碍了，而且如果你绕开了障碍物，会发现失去了方向连不回来了。因此，在连接节点中采用中点连接法，但是中点，要取得有技巧，可在中点处取4或3个栅格，然后在这些栅格中找到自由栅格，等概率选择，如果有最坏得情形，这些中点处栅格全都是障碍，则在这些栅格中等概率选择一个作为路径一点，因此该方法保证了路径的连续性，但也有可能存在经过障碍的路径，而这种障碍的路径可以在适应度函数中进行惩罚。

上述初始化路径的两步结束后，进行简化路径操作，即如果路径中有两个相同的节点，则去掉相同节点之间的一段，这个很容易理解。

至此初始化路径结束，你已经成功了一大步！

2、选择

选择和遗传算法的选择是一样的，无需特殊改动，就是根据适应度进行选择。

3、交叉

交叉采用重复点交叉，这个也比较好理解，比如一条路径为[1 12 13 24 25 36 45 56 ]，另一条路径为[ 1 14 25 35 46 56]，这两条路径有一个公共节点25，进行交叉变成[1 12 13 24 25 35 46 56]和[1 14 25 36 45 56].

4、变异

变异的方法使用随机生成选择两个节点，并去掉这两个节点之间的路径，之后采用上述的中带你插入方法生成连续路径。

5、适应度计算

适应度计算路径长度和穿过障碍的个数，求和即可，这个比较间断不多说，下面展示一下我写的程序的效果。

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

clc;

clear all

close all;

% 输入数据,即栅格地图

G= [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 0 0 0 1 1 0 0 0;

0 1 1 0 0 0 0 0 0 0 1 1 1 1 0 1 1 0 0 0;

0 0 0 0 0 0 1 1 1 0 0 1 1 1 0 1 0 0 0 0;

0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 1 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 1 1 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 0 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 1 1 1 0 0 0 0 1 1 1 1 0;

0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0;

0 0 1 1 0 0 0 0 0 0 1 1 1 1 1 0 0 0 0 0;

0 0 1 1 0 0 1 1 0 0 1 0 0 0 0 0 0 0 0 0;

0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 1 1 1 1 0;

0 0 1 0 1 0 0 0 0 0 0 1 0 0 1 0 0 1 1 0;

0 0 1 1 1 0 1 1 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 0 0 0 0 0 0];

p_start = 0; % 起始序号

p_end =399; % 终止序号

NP = 200; % 种群数量

max_gen = 2000; % 最大进化代数

pc = 0.5; % 交叉概率

pm = 0.5; % 变异概率

a = 1; % 路径长度比重

b = 7; % 路径顺滑度比重

%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);

min_value=1000;

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

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

% 计算初始化种群的适应度

% 计算路径长度

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, G, 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);

if min(path_value)<min_value

min_value=min(path_value);

min_path_value(1, i) = min(path_value);

else

min_path_value(1, i) = min_value;

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(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')

English Description：

It is a classic problem to use the intelligent algorithm to find the optimal path by using the grid to divide the workspace of the robot and then using the optimization algorithm to optimize the path of the robot. At present, using ant colony algorithm to optimize the path on the grid map has achieved good results, while using genetic algorithm to optimize the path on the grid map is more difficult to achieve. the difficulties of robot path planning using genetic algorithm to deal with raster map mainly include: 1. Ensure that the path is uninterrupted; 2. Ensure that the path does not pass through obstacles. when using genetic algorithm to solve optimization problems, the steps are fixed, that is, population initialization, selection, crossover, mutation and fitness calculation. In this way, let me talk about the problems, difficulties and solutions of genetic algorithm in each step of robot path planning in grid map. 1, population initialization firstly, we need to know the definition of population initialization of genetic algorithm. Genetic algorithm population initialization is to generate a certain number of individuals, each individual is a feasible solution. Here, we should pay attention to the feasible solution. For this problem, it is a feasible path, that is, it should be a path from the beginning of the path to the end of the path, and it does not pass through obstacles. How to initialize the population to get the feasible path is the first and the most difficult point of genetic algorithm to find the grid path. method Path initialization can be divided into two steps: the first step is to generate the required node path, what is the required node path? For example, for a 10 * 10 grid, the path from number 1 in the lower left corner to number 100 in the upper right corner must pass through a point in line 2 and a point in line 3 A point in line 9, which is obvious, then we randomly take a node from the free grid in line 2 and a node from the free grid in line 3 In line 9, the free grid takes a node, so this is a necessary path. Of course, this path is discontinuous. So we need the second step. The second step is to connect these discontinuous nodes, and this problem is the most difficult problem in the algorithm, because in the connection path, it is too difficult to bypass the obstacles, and if you bypass the obstacles, you will find that you lose the direction and can't connect back. Therefore, the midpoint connection method is used in the connection node, but the midpoint can be obtained skillfully by taking 4 or 3 grids at the midpoint, and then finding the free grid in these grids, and making equal probability selection. If in the worst case, the grids at the midpoint are all obstacles, then one of them is selected as the path point with medium probability, so this method ensures the path stability However, there may be paths through obstacles, which can be punished in the fitness function. after the above two steps of initializing the path, simplify the path operation, that is, if there are two identical nodes in the path, remove a segment between the same nodes, which is easy to understand. so far, the initialization path is over, and you have made a big step! 2, selection selection and genetic algorithm selection are the same, without special changes, it is based on fitness selection. 3, crossover crossover uses repeated point crossover, which is also easy to understand, for example, one path is [1 213 24 25 36 45 56], the other path is [1 14 25 35 46 56], these two paths have a common node 25, which can be crossed into [1 113 24 25 35 46 56] and [1 14 25 36 45 56]. 4, variation 4 Mutation method uses random generation to select two nodes, and remove the path between the two nodes, and then uses the above method to generate a continuous path. 5, fitness calculation fitness calculation path length and the number of obstacles through, summation can, this comparison does not need to say, the following shows the effect of the program I wrote. < p < p < p < p < p < p < p < p < p < p < p < p < p < p < p < p < p 0 0 0;

0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 1 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 1 1 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 0 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 1 1 1 0 0 0 0 1 1 1 1 0;

0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0;

0 0 1 1 0 0 0 0 0 0 1 1 1 1 1 0 0 0 0 0;

0 0 1 1 0 0 1 1 0 0 1 0 0 0 0 0 0 0 0 0;

0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 1 1 1 1 0;

0 0 1 0 1 0 0 0 0 0 0 1 0 0 1 0 0 1 1 0;

0 0 1 1 1 0 1 1 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 0 0 0 0 0 0];

p_ Start = 0;% start sequence number p_ End = 399;% stop sequence number NP = 200;% population number Max_ Gen = 2000;% max evolution algebra PC = 0.5;% crossover probability PM = 0.5;% mutation probability A = 1;% path length proportion b = 7;% path smoothness proportion % init_ path = [];

z = 1;

new_ Pop1 = {};% metapacket type path [y, x] = size (g); % start column (numbered from left to right 1.2.3...) XS = mod (P_ Start, x) + 1; % start line (number line 1.2.3 from top to bottom...) ys = fix (P_ Start / x) + 1; % column and row of the end point Xe = mod (P_ end, x) + 1;

ye = fix(p_ End / x) + 1; % the population initializes step 1 and must pass through nodes. Starting from the line at the starting point, a free grid is selected in each line to form the required node pass_ num = ye - ys + 1;

pop = zeros(NP, pass_ num);

min_ value=1000;

for i = 1 : NP

pop(i, 1) = p_ Start; J = 1; % remove start and end points for YK = ys + 1: ye-1 J = j + 1; % feasible points of each line can =]; for XK = 1: x % grid number no = (XK - 1) + (YK - 1) * x; if G (YK, XK) = = 0 % add points to can matrix can = [can no]; end end can_ Num = length (can); % generates random integer index = Randi (can)%_ Add a feasible point for each line pop (I, J) = can (index); end pop (I, end) = P_ End; % pop % population initialization Step2 connects the above nodes into a continuous path 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 % calculate the fitness of the initial population % calculate the path length path_ value = cal_ path_ value(new_ Pop1, x); % calculate path smoothness 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); % loop iteration operation for I = 1: Max_ Gen % select action New_ pop2 = selection(new_ pop1, fit_ Value); % cross operation New_ pop2 = crossover(new_ Pop2, PC); % mutation operation new_ pop2 = mutation(new_ Pop2, PM, G, x); % renewal population new_ pop1 = new_ Pop2; % calculate fitness value % calculate path length Path_ value = cal_ path_ value(new_ Pop1, x); % calculate path smoothness 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);

if min(path_ value)<min_ value

min_ value=min(path_ value);

min_ path_ value(1, i) = min(path_ value);

else

min_ path_ value(1, i) = min_ Value; end end % draw the average path length and optimal path length graph of each iteration figure (1) plot (1: max)_ gen, mean_ path_ All of the 39; r-39; r-39; r-39; r-39; (r-39; r-39; (r-39; r-39; (r-39; r-39; r-39; r-39; (r-39; r-39;) all of the details of all of the title ([\#;#####; a = \35; 39; 39; 39; \35#####\\\\\\\\\\\\\\\\\\\\\abel (& 39; path length & 39;); plot (1: max)_ gen, min_ path_ Value, &# 39; B & # 39;) Legend (&# 39; average path length & # 39;, &# 39; optimal path length & # 39;); min_ path_ Value (1, end) % draw path on map [~, min]_ index] = max(fit_ value);

min_ path = new_ pop1{min_ All of the details (2) figure (2) title ([\d3d-39; a = \353d39; 39; 39; a = \\\\\35; 39; 39;; 39; 39; 39; 39; \35\35\35\\\\\\\\\\\\\353d3d 3D 3D 3D 3D 3D 3d#39; coordinate y & #39;); drawmap (g); [~, min]_ path_ num] = size(min_ path);

for i = 1:min_ path_ Num % path point column (numbered from left to right 1.2.3...) x_ min_ path(1, i) = mod(min_ Path (1, I), x) + 1; % the line of the path point (line 1.2.3... Numbered from top to bottom) y_ min_ path(1, i) = fix(min_ path(1, i) / x) + 1;

end

hold on;

plot(x_ min_ path, y_ min_ path, 'r')

基于栅格地图——遗传算法的机器人最优路径规划我要分享

代码描述

代码预览

基于栅格地图——遗传算法的机器人最优路径规划我要分享

代码描述

代码预览

相关推荐