Analysis of the Principle of RRT Algorithm for Autonomous Driving

1 Introduction to RRT algorithm

The only way to defeat all martial arts is to be fast, and speed is the biggest advantage of RRT. The idea of ​​RRT is to quickly expand a group of tree-like paths to explore most areas of the space and find feasible paths.

The RRT algorithm is an algorithm for random sampling of state space. By performing collision detection on the sampling points, it avoids the large amount of computation caused by precise modeling of the space and can effectively solve path planning problems in high-dimensional space and complex constraints.

Similar to PRM, this method is probabilistically complete and non-optimal. It can easily handle problems with obstacles and differential constraints (non-holonomic and dynamic), and is widely used in robot path planning.

2 RRT Algorithm Principle

2.1 Algorithm Flow

(1) Set the initial point and target point, and set the state sampling space by yourself

(2) Perform random sampling to obtain the sampling point. If the sampling point is within the obstacle, re-random sampling is performed.

(3) If it is not within the obstacle, calculate the distance between the sampling point and all nodes in the set (the generated nodes), get the nearest node, and then walk from node to node with a step length of , generate a new node. If the line connecting and passes through an obstacle, re-randomly sample

(4) After repeated iterations, a random expansion tree is generated. When a child node in the random expansion tree enters the target area we specify, a path from the initial point to the target point can be found in the random expansion tree.

2.2 Algorithm Pseudocode

You can compare the pseudo code with the above algorithm flow

2.3 Algorithm Flowchart

3 RRT algorithm MATLAB implementation

3.1 Test Map

% Randomly generate obstacles

function [f,n1]=ob(n)

f=[];%Store obstacle information

n1=n;%Return the number of obstacles

p=0;

for i=1:n

k=1;

while(k)

D=[rand(1,2)*60+15,rand(1,1)*1+3];% Randomly generate the coordinates and radius of the obstacle and adjust it by yourself

if(distance(D(1),D(2),90,90)>(D(3)+5)) %The distance from the target point is a certain length to prevent too much obstruction for the robot to reach the target point

k=0;

end

for t=1:p % The distance between obstacles cannot be too narrow, you can adjust it to test it yourself

if(distance(D(1),D(2),f(3*t-2),f(3*t-1))<=(D(3)+f(3*t)+5))

k=1;

end

end

end

%Draw obstacles

aplha=0:pi/40:2*pi;

r=D(3);

x=D(1)+r*cos(aplha);

y=D(2)+r*sin(aplha);

fill(x,y,'k');

axis equal;

hold on;

xlim([0,100]);ylim([0,100]);

f=[f,D];

p=p+1;%The number of obstacles currently generated

end

hold all;

3.2 distance function

function f=distance(x,y,x1,y1)

f=sqrt((x-x1)^2+(y-y1)^2);

3.3 RRT algorithm

clc

clear all

[f,n1]=ob(10);% Randomly generate obstacles

Xinit=[1,1];%Define the initial point

Xgoal=[90,90];%Define the target point

plot(Xinit(1),Xinit(2),'ro');

plot(Xgoal(1),Xgoal(2),'ko');

T=[Xinit(1),Xinit(2)];% The generated node set is stored in the data structure of the sequence table

Xnew=Xinit;

D(1)=0;%The parent node of the initial node points to 0

while distance(Xnew(1),Xnew(2),Xgoal(1),Xgoal(2))>3 % enter the target area

Xrand=round(rand(1,2)*100)+1;%State sampling space: horizontal and vertical coordinates are integers, ranging from 1 to 100

k=1;%Enter the loop

while k==1

k=0;% Initialization sampling successful

for i=1:n1

if distance(Xrand(1),Xrand(2),f(i*3-2),f(i*3-1))<(f(i*3)+1)% determines whether the random sampling point is within the obstacle

k=1;% sampling failed

break;

end

end

Xrand=round(rand(1,2)*100);% resampling

end

min=10000;

for i=1:size(T,2)/2 %Traverse the generated node set

if distance(T(2*i-1),T(2*i),Xrand(1),Xrand(2))Xnew(1)

caiyang=-0.001;

else

caiyang=0.001;

end

for i=Xnear(1)Xnew(1)% uniform sampling for collision detection

for j=1:n1

if distance(f(3*j-2),f(3*j-1),i,Xnear(2)+(i-Xnear(1))/(Xnew(1)-Xnear(1))*( Xnew(2)-Xnear(2)))<=(f(3*j)+1)

t=1;% represents collision

break;

end

end

if t==1

break;

end

end

if t==0

T=[T,Xnew(1),Xnew(2)];

for i=1:size(T,2)/2 %Traverse the generated node set

if (T(i*2-1)==Xnear(1))&&(T(i*2)==Xnear(2)) %Get the index of the nearest node

D(size(T,2)/2)=i;%Record parent node

break;

end

end

plot([Xnew(1),Xnear(1)],[Xnew(2),Xnear(2)],'b-');hold on;pause(0.005);

plot(Xnew(1),Xnew(2),'r.');xlim([0,100]);ylim([0,100]);

end

end

i=size(T,2)/2;

jg=[i];

while D(i)

i=D(i); %Backtrack through the linked list

if D(i)~=0

jg=[D(i),jg];%Store the nodes passed by the shortest path

end

end

Fx=T(jg(1)*2-1);Fy=T(jg(1)*2);

i=2;

while jg(i)~=size(T,2)/2

x=T(jg(i)*2-1);

y=T(jg(i)*2);

plot([x,Fx],[y,Fy],'g-');hold on;pause(0.05);

Fx=x;Fy=y;

i=i+1;

end

plot([T(jg(i)*2-1),Fx],[T(jg(i)*2),Fy],'g-');hold on;pause(0.05);

3.4 Animation Effects

4 Disadvantages of RRT

(1) It is obvious that the path obtained by the RRT algorithm is not optimal.

(2) The RRT algorithm does not consider the kinematic model

(3) The RRT algorithm has poor exploration performance for narrow channels. As shown in the following figure, it is possible that the exit cannot be explored.

(4) RRT without inspiration information is like a headless fly, and exploring space depends entirely on luck, as shown in the following figure

In response to the above defects, many variants of the RRT algorithm have emerged, which will be introduced in future articles.