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.

source: http://techeffigytutorials.blogspot.com.br/2015/02/the-genetic-algorithm-explained.html

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:

- 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).
- 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.
- 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).
- 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.
- 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.
- 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[1].

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 load('last_generation'); [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 [x y th]= OverheadLocalizationCreate(serPort); %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 [x y th]= OverheadLocalizationCreate(serPort); 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 finalRad= 0; % 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.

Parabéns pelo ótimo trabalho!

Muito bem estruturado e com ótima didática, finalizando com o vídeo.

LikeLiked by 1 person