Adaptive RRT – ARRT (with code)

Hi all
Back in 2010 I was taking an AI class where we learned about Rapidly Exploring Random Trees for motion planning. For my final project I developed an adaptive version of the RRT that I named ARRT. I decided to post it here to share with others. The post below is based from the final presentation that I gave in the class.

RRT is a Probabilistic search algorithm. The basic algorithm is as follows:

– From initial/current point, pick a new point “A” that:
• with probability p heads towards the goal
• with probability (1-p) head towards a random point
– Extend a line segment a fixed distance towards “A”.
• If it intersects an obstacle start again by picking a new point.
• If it does NOT intersect an obstacle set “A” as the current point and run again.
– Finish when at goal

The problem with the above algorithm is that p and the amount extended are static value. So:
– If there are a lot of obstacles you want both values to be small
– If there are no obstacles you want each value to be large

The solution is Adaptive RRT or ARRT where we can change p and the extend length based on the results from the prior timestep. This lets the algorithm adapt to worlds with a lot or no obstacles and improve its performance
• At each iteration if the previous point that we tried reaching was bad we decrease p and decrease the extend distance
• If the point was good then we increase p and increase the extend distance
• If a map has an obstacle free region followed by lots of obstacles within a few iterations of not having valid points it will change p and the extend length accordingly.

So if we use ARRT what type of results can you expect? Lets see some sample cases below.

e is the extend distance per cycle

You can see that the total path length (ie. number of segments) is shortest with ARRT and the number of iterations to reach the solution is also less (making this computationally faster). For the RRT p is run at 50% towards the goal and also 90% which should do very well since there are no obstacles. Note that these results are the average of 20 iterations.

Similar to the test with no obstacles that ARRT does better. For the RRT I test it with 50% and 20%. The 20% is due to the fact that there are more obstacles to deal with in this case and we want to try more random directions.

This case is more extreme however we still show how ARRT overall does the best. This data was averaged over 400 computed paths.

Here is some code (I know it is Matlab…). Sorry for the lack of tabs, wordpress/html does weird things when I try to add them. If you put the code below back into Matlab you can use the auto-indent feature to make it look good again.

% Adaptive RRT
% For AI spring 2010

function [lenPath iteration]=rrt()
%%%%%%%% Parameters to Set %%%%%%%%%%
p=0.5;
extend=2;
p_increment=0.05;
extend_increment=1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% use adaptive RRT
adaptive=1;

%%%%%%%% create world %%%%%%%%
%start=[1,0];
%goal=[9,7];
%map=[0,1;0,1];
%map=[map;2,0;2,1;2,2;2,3;2,4;2,5;2,6];
%map=[map;4,10;4,9;4,8;4,7;4,6;4,5;4,4];
%map=[map;6,0;6,1;6,2;6,3;6,4;6,5;6,6;6,8;6,9;6,10];
%map=[map;8,7];
%dimensions=[10,10];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%% create world %%%%%%%%
start=[5,10];
goal=[40,40];
for ii=1:500
map(ii,1)=round(rand()*50);
map(ii,2)=round(rand()*50);
endfor
dimensions=[50,50];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

iteration=0;
current=start;
path=[current p extend iteration]; % logs the final path

reachedGoal=0;
while (reachedGoal==0)
iteration=iteration + 1;
if (iteration > 10000)
reachedGoal=1;
iteration
endif;

if (rand()<= p) nextPoint=newPoint(current,goal,extend); else randomLocation=[round(rand()*dimensions(1)) round(rand()*dimensions(2))]; nextPoint=newPoint(current,randomLocation,extend); endif [valid,temp]=newPointValid(current,nextPoint,goal,extend,map,dimensions); if (valid == 0) if (adaptive==1) [p extend]=modifyParams(-1,p, extend, p_increment, extend_increment,dimensions); endif; elseif (valid == 1) if (adaptive==1) [p extend]=modifyParams(1,p, extend, p_increment, extend_increment,dimensions); endif; current=nextPoint; path=[path;current,p,extend,iteration]; elseif (valid ==2) reachedGoal=1; current=temp; path=[path;current,p,extend,iteration]; endif end displayMapWithPath(map,path,start,goal,dimensions); lenPath=length(path); endfunction % returns 1 if valid, 0 if invalid, 2 if goal function [ret,temp]=newPointValid(current,nextPoint,goal,extend,map,dimensions) ret=1; % check if out of bounds if ((nextPoint(1) < 0) | (nextPoint(2) < 0)) ret=0; endif; if ((nextPoint(1) > dimensions(1)) | (nextPoint(2) > dimensions(2))) ret=0; endif;

% check if intersects obstacle or goal
for (ii=1:extend)
temp=newPoint(current,nextPoint,ii);
for jj=1:length(map)
if ((temp(1) == map(jj,1)) & (temp(2) == map(jj,2)))
ret=0;
break;
endif
endfor

if ((temp(1) == goal(1)) & (temp(2) == goal(2)))
ret=2;
break;
endif
endfor
endfunction

function nextPoint=newPoint(current,goal,extend)
% to avoid divide by 0
numerator=goal(2) – current(2);
denomonator=goal(1) – current(1);
slope=atan2(numerator,denomonator);
[x y]=pol2cart(slope,extend);
nextPoint=[round(current(1)+x) round(current(2)+y)];
endfunction

function displayMapWithPath(map,path,start,goal,dimensions)
figure(1)
clf;
hold on;
title(‘Adaptive RRT Path’)
axis([0 dimensions(1) 0 dimensions(2)]);

%plot start
plot(start(1),start(2),’o’);
%plot goal
plot(goal(1),goal(2),’x’);

% plot obstacles
[m n]=size(map);
for ii=1:m
plot(map(ii,1),map(ii,2),’*’);
endfor

% plot path
plot(path(:,1),path(:,2));

figure(2)
clf;
plot(path(:,5),path(:,3));
title(‘p at each valid point’);xlabel(‘iteration’),ylabel(‘p’);

figure(3)
clf;
plot(path(:,5),path(:,4));
title(‘extend length at each valid point’);xlabel(‘iteration’),ylabel(‘extend length’);
endfunction

function [p extend]=modifyParams(type,p, extend, p_increment, extend_increment,dimensions)
p=p +(type * p_increment);
extend=extend + (type * extend_increment);

% check the bounds for p and extend
if (p <= p_increment) p=p_increment; endif; if (p >= (1-p_increment)) p=(1-p_increment); endif;
if (extendmax_extend) extend=max_extend; endif;
endfunction

I hope this adaptation of RRT is useful for you. As you can see above making parameters adaptive can be a good way to improve the performance of an algorithm. If you have other ideas leave them in the comments below.

Liked it? Take a second to support David Kohanbash on Patreon!