# Robot path generation from a Genetic Algorithm

The project presented in this post was accomplished during the Artificial Intelligence class given by professor Caroline. It consisted of using the Genetic Algorithm (GA) to make a robot (from the iRobot Matlab toolbox) navigate from a start point to an end point without hitting any obstacles (walls).

A GA flowchart is shown below to quickly resume the method. At the beginning, a new population of possible solutions is created, each individual of this population can be the solution for a given problem. The population size is strictly fixed to a selected size. Next, an evaluation (or fitness) is applied to each individual of the population, to know how well their solution fits into the problem solution. From there, the process of “evolution” begins, a new generation is created by selecting the best individuals, reproducing them (crossover) and applying a chance of mutation for each individual. At the end, this new generation is evaluated and the process of creating a new generation starts again. The algorithm stops when the evaluation of the population is good enough or the number of generations specified is reached. The robot path generation was adapted based on the explanation given of the GA. A brief summary of the developed code in this post is shown:

1. The start population is created from the possible paths that the robot can follow. Each path (individual) is composed of a sequence of cartesian coordinates (x,y), the path is constructed from the interconnection of those coordinates. It is important to check if the path is valid (no obstacles in the way).
2. The evaluation (fitness) of the population is retrieved based on the success of the path to reach the destination and the distance traveled from the start point to the finish point.
3. A new generation is started by selecting the new individuals randomly (Roulette selection) until a selected percentage of the population size. The individuals with the best evaluation have more chances to be selected (Elitism).
4. The last empty spots of the new population are filled with the reproduction (crossover) of two individuals selected the same way as step 3. The reproduction generates a single child which has one part of the first selected individual and another part of the second individual from a common intersection of both.
5. With the new population filled, a chance of mutation is applied to each individual. Each Cartesian coordinate have a really small chance of being replaced by another random one. It is also important to check if this new random point is valid to create a path. Mutation is applied to avoid the global minimum solution to the problem.
6. The population is evaluated again, and the algorithm repeats from step 2 until the requirements are matched.
```population_size = 100;
path_size = 10;
%Start point coordinates
start_x = 3;
start_y = 3;
%End point
desired_x = -3;
desired_y = -3;
%survive rate for next generation
keep_alive = 90;
%Number of generations
epoch = 10;
%Mutation chance for each node
mutation_chance = 0.0005; %For each point in the path
```

At the beginning of the Matlab code (shown above), the user can specify the parameters of the GA such as the population size, the number of points on each path, the start point, the destination point, how many individuals to keep alive on the next generation, the number of generations for the problem and the chance of mutation for each point in each individual. Those parameters are very important to determine the convergence speed to the solution.

With the parameters set, the algorithm begins by creating the population. A function receives the start point, the population size and the path size to do so:

```%creates population
population = generate_population(start_x,start_y,population_size,path_size)
```

The “generate_population” function generates the number of individuals equal to the population size set. Each individual is constructed by two successive dots, checking if the path between is valid with the “valid_point” function.

```function [ population ] = generate_population( start_x, start_y, population_size, path_size )

population = cell(population_size,1);
for j = 1:population_size
i = 0;
x = start_x;
y = start_y;
prev_x = x;
prev_y = y;

while i < path_size

temp_x = prev_x;
temp_y = prev_y;
move_x = rand*10 - 5;
move_y = rand*10 - 5;

isvalid = valid_point(temp_x,temp_y,move_x,move_y);
if isvalid == 1
x = [x;move_x];
y = [y;move_y];
%plot(x,y);
prev_x = move_x;
prev_y = move_y;
i = i+1;
end
end
population{j} = [x y];
end

end
```

The “valid_point” function checks if there is a wall between two points. It uses the logic provided at https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection to check if the lines produced by the walls and the pair of points intersect, meaning a possible colision. The map files generated by the iRobot GUI creates the walls with straight lines between two points. The code also considers a margin with respect to the robot width.

```function [ value ] = valid_point( start_x, start_y, move_x, move_y )

walls = fopen('test_GA.txt','r');
line = 'clan';
while ~feof(walls)
line = fgetl(walls);
spl = strsplit(line);
if strcmp(spl(1),'wall')
wx1 = cellfun(@str2num,spl(2));
wy1 = cellfun(@str2num,spl(3));
wx2 = cellfun(@str2num,spl(4));
wy2 = cellfun(@str2num,spl(5));
%line
%[start_x,start_y,move_x,move_y]
%Robot margin from center
rm = 0.2;
valid_flag = lines_intersect(start_x,start_y,move_x,move_y,wx1,wy1,wx2,wy2);
%Avoid the robot of hitting a wall
valid_flag = valid_flag | lines_intersect(start_x,start_y,move_x,move_y,wx1+rm,wy1,wx2+rm,wy2);
valid_flag = valid_flag | lines_intersect(start_x,start_y,move_x,move_y,wx1-rm,wy1,wx2-rm,wy2);
valid_flag = valid_flag | lines_intersect(start_x,start_y,move_x,move_y,wx1,wy1+rm,wx2,wy2+rm);
valid_flag = valid_flag | lines_intersect(start_x,start_y,move_x,move_y,wx1,wy1-rm,wx2,wy2-rm);
if valid_flag == 1
value = 0;
fclose(walls);
return
end
end
end
fclose(walls);
value = 1;
end
```

After the population is created, it is applied an evaluation function for the solution measurement, passing the population as the main parameter to the function:

```%Evaluates population fitness
fitness = get_fitness(desired_x,desired_y,population_size,path_size,population);
```

The fitness function evaluates each individual based on the total distance traveled and the proximity to the goal destination. It weights the proximity distance higher to make it more valuable (that is the point of the algorithm). The function returns an array which each index is indirectly linked to the population array. For example, population{1} has fitness.

```function [ fitness ] = get_fitness( desired_x,desired_y,population_size,path_size, population )
fitness = [];
for i = 1:population_size
dude = population{i};
dist_fit = 0;
for j = 1:path_size
dist_fit = dist_fit + sqrt(sum((dude(j,:) - dude(j+1,:)) .^ 2));
end
goal_fit = sqrt(sum((dude(path_size+1,:) - [desired_x desired_y]) .^ 2));
%goal_fit
%dist_fit/(path_size+1)
%make the goal more important than the distance travelled
final_fit = 2*goal_fit + dist_fit/(path_size+1);

fitness = [fitness ; final_fit];
end

end
```

Subsequently, the fitness array is sorted to keep the best individuals in an ascending order.

```%organizes best fitness specimen
[out,id_ranking]=sort(fitness);
```

Upon the creation and evaluation of the new population, the process of evolution BEGINS!! “next_gen” is an auxiliary variable that represents the next generation. The new generation will select the best individuals of the previous one, fill the left spaces with crossovers of the last generation, mutate all the new individuals by chance and start the whole process again until a stop condition.

```next_gen = population;

for j = 1:epoch

%Selection and reproduction
%Elite selection
for i = 1:keep_alive
next_gen{i} = population{id_ranking(i)};
end
for i = (keep_alive+1):population_size
next_gen{i} = cross_over(population,fitness,path_size);
end
%Mutation
next_gen = mutate_population(next_gen,population_size,path_size,mutation_chance);

fitness = get_fitness(desired_x,desired_y,population_size,path_size,next_gen);
mean(fitness)
[out,id_ranking]=sort(fitness);
population = next_gen;
end
save('demo','population','fitness');
```

The crossover function is highlighted bellow, it starts by selecting two individuals with the “select_specimen” function to be reproduced. It tries to select two different individuals because the population tends to have the same individuals at the end when the solution has converged. Then, it randomly chooses a common division point of both selected individuals, so the child has one part of each parent.

```function [ cross_over_result ] = cross_over( population_input, fitness_input, path_size )

population = population_input;
fitness = fitness_input;

valid_cross = 0;
while valid_cross == 0
s1 = select_specimen(fitness);
s2 = s1;
debug_count = 5;
%Avoid same paths
while s2 == s1 && debug_count > 1
debug_count = debug_count - 1;
s2 = select_specimen(fitness);
end
if debug_count == 0
%Needs to check if its the last position
s2 = s1+1;
end

div_point = round(rand*(path_size-1) + 1);

s1x = population{s1}(div_point,1,:);
s1y = population{s1}(div_point,2,:);
s2x = population{s2}(div_point+1,1,:);
s2y = population{s2}(div_point+1,2,:);

valid_cross = valid_point(s1x,s1y,s2x,s2y);
end
cross_over_result = [population{s1}([1:div_point],:) ; population{s2}([div_point+1:path_size+1],:)];

end
```

To select the specimen, the developed code normalizes the fitness array by dividing each index by the sum of all indexes. The result is a vector witch the sum of all values equals 1. Then, the array is modified again so the first value is the first percentage and the last value is 1. For example, [0.1 0.2 0.4 0.3] becomes [0.1 0.3 0.7 1.0]. Subsequently, a random number is generated and the individual is selected based on the interval that the random number has fallen.

```function [ index ] = select_specimen( population_fitness )

%Roaller coster (roleta) method to find the specimen
%Weighted to the most probable to be accepted
fitness = population_fitness;
fitness = fitness/sum(fitness);
%sum(fitness)
fitness = max(fitness) - fitness;
if sum(fitness)==0
index = round(rand*(size(fitness)-1) + 1);
index = index(1);
return
end
fitness = fitness/sum(fitness);
for i = 2:size(fitness)
fitness(i) = fitness(i)+fitness(i-1);
end
%sum(fitness)

%fitness
alea = rand;
for i = 1:size(fitness)
if alea < fitness(i)
index = i;
break
end
end
end
```

Finally, the “mutate_population” function applies a chance of mutation on each individual set of points based on the specifications at the beginning of this implementation. It also needs to check if the point changed creates a valid path without obstacles so the robot can walk through.

```function [ mutated_population ] = mutate_population( population,population_size,path_size,mutation_chance )

next_gen = population;
for j = 1:population_size
for i = 1:path_size
r_num = rand;
valid_before = 0;
valid_after = 0;
if r_num < mutation_chance
while valid_before == 0 || valid_after == 0
mutation_x = rand*8 -4;
mutation_y = rand*8 -4;
%Searches for last position
%First position doesnt change
if i < path_size
valid_before = valid_point(next_gen{1}(i,1,:), next_gen{1}(i,2,:),mutation_x,mutation_y);
valid_after = valid_point(mutation_x,mutation_y,next_gen{1}(i+2,1,:), next_gen{1}(i+2,2,:));
if valid_before == 1 && valid_after == 1
next_gen{1}(i+1,:) = [mutation_x mutation_y];
end
else
valid_before = valid_point(next_gen{1}(i,1,:), next_gen{1}(i,2,:),mutation_x,mutation_y);
if valid_before == 1
next_gen{1}(i+1,:) = [mutation_x mutation_y];
end
%To make possible the exit of the while loop
valid_after = 1;
end
end
end
end
end
mutated_population = next_gen;
end
```

With the “ideal” population generated, the code below gets the best individual of the population by its final fitness and uses it to reach the destination from a start point. Again, each individual is a set of points that represents a path by tracing each subsequent point. The algorithm uses the “OverheadLocalizationCreate()” function to keep track of the angle that the robot is looking and calculates the difference to look straight to the desired next point to follow by using the arctan() of the difference of coordinates x and y from the robot position and the desired point position. Then, it reaches the point with the “travelDist()” function who receives the euclidian distance between the robot and the destination point.

```function finalRad= ExampleControlProgram(serPort)<span 				data-mce-type="bookmark" 				id="mce_SELREST_start" 				data-mce-style="overflow:hidden;line-height:0" 				style="overflow:hidden;line-height:0" 			>﻿</span>

% Set constants for this program
maxDuration= 1200;  % Max time to allow the program to run (s)
maxFwdVel= 0.5;     % Max allowable forward velocity with no angular
% velocity at the time (m/s)
i=0;
% Initialize loop variables
tStart= tic;        % Time limit marker

% Enter main loop

[out i] = min(fitness);
plot(population{i}(:,1),population{i}(:,2),'O');
hold on
plot(population{i}(:,1),population{i}(:,2));
pop_size = size(population{1});
pop_size = pop_size(1);
% Check for bugs on sensors reading
for j = 1:pop_size

%th

[population{i}(j,1,:) population{i}(j,2,:)];
[x y]

qy = sign(y-population{i}(j,2,:))
qx = sign(x-population{i}(j,1,:))
%Third cartesian plan
if qx == -1 && qy == -1
angle = (atan((y-population{i}(j,2,:))/(x-population{i}(j,1,:)))) - (pi/2);
angle = (pi/2) + angle;
%Fourth cartesian plan
elseif qx == 1 && qy == -1
angle = (atan((y-population{i}(j,2,:))/(x-population{i}(j,1,:))));
angle = angle + pi;
%Second cartesian plan0
elseif qx == -1 && qy == 1
angle = (pi/2) - (atan((y-population{i}(j,2,:))/(x-population{i}(j,1,:))));
angle = (pi/2) - angle;
%First cartesian plan
elseif qx == 1 && qy == 1
angle = (atan((y-population{i}(j,2,:))/(x-population{i}(j,1,:))));
angle = angle - pi;
else
angle = 0;
end
if angle > 3.14
angle = 3.14;
elseif angle < -3.14             angle = -3.14;         end         %turn_angle = (360/(2*pi))*(angle-th);         %turnAngle(serPort,5,turn_angle);         while (abs(angle-th)) > 0.1
angle
th
SetDriveWheelsCreate(serPort,0.04,-0.04);
pause(0.2);
end
%SetDriveWheelsCreate(serPort,0.0,0.0);
distance = sqrt(sum(([x y] - population{i}(j,:)) .^ 2));
travelDist(serPort,0.5,distance);
end
% Specify output parameter

% Stop the robot
SetFwdVelAngVelCreate(serPort,0,0)
end
```

The source code can be accessed at:

https://github.com/YangTavares/Deep_learning/tree/master/iRobot_run_Genetic_Algorithm/iRobot_YangT

## Results:

The next images illustrate some of the results got for different maps on iRobot environment, with a selected start and end coordinates.   The attached video demonstrates the developed software working inside the iRobot environment. At the start of the video, it is shown how the population is created, each new path is plotted on the window. Then, the robot follows the best-generated path with the Genetic Algorithm.

A robot navigation with a Multi-Layer Perceptron can be accessed here.

## 1 thought on “Robot path generation from a Genetic Algorithm”

1. Parabéns pelo ótimo trabalho!
Muito bem estruturado e com ótima didática, finalizando com o vídeo.

Liked by 1 person