1. Introduction to the RRT Algorithm
The RRT algorithm is an algorithm that randomly samples the state space. By performing collision detection on the sampled 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 spaces and with complex constraints.
Similar to PRM, this method is probabilistically complete but not optimal. It can easily handle problems with obstacles and differential constraints (nonholonomic and dynamic) and is widely used in robot path planning.
2. RRT Algorithm Principle
2.1 Algorithm Flow
(1) Set the initial point and the target point, and set the state sampling space yourself.
(2) Random sampling is performed to obtain sampling points. If the sampling point is inside an obstacle, random sampling is performed again.
(3) If the sampling point is not within an obstacle, calculate the distance between the sampling point and all nodes in the set (already generated nodes), find the nearest node, and then move from node to node with a step size to generate a new node. If the line connecting node and node passes through an obstacle, then resample randomly.
(4) After repeated iterations, a random expansion tree is generated. When a child node in the random expansion tree enters the target area we have specified, 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 pseudocode with the algorithm flow described above.
2.3 Algorithm Flowchart
3. MATLAB implementation of the RRT algorithm
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, adjust as needed if(distance(D(1),D(2),90,90)>(D(3)+5)) % Set a certain distance from the target point to prevent excessive obstruction of the robot's arrival at the target point k=0;
end
for t=1:p % The distance between obstacles cannot be too narrow; you can adjust it yourself to test 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; % 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 using a sequential list data structure. 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 region Xrand=round(rand(1,2)*100)+1;% State sampling space: both x and y 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)%Check if the random sampling point is inside the obstacle k=1;%Sampling failed break;
end
end
Xrand=round(rand(1,2)*100);%Resample end
min=10000;
for i=1:size(T,2)/2 % Iterate through the set of generated nodes 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 % Iterate through the set of generated nodes 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 the 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 traversed 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. Defects 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 performance in exploring narrow channels. As shown in the comparison below, it may not be able to find the exit.
(4) Without heuristic information, RRT is like a headless fly, exploring space entirely by luck, as shown in the figure below.
To address the aforementioned shortcomings, many variations of the RRT algorithm have emerged, which will be introduced in later articles.
