目录
1 概述
RRT是一种基于概率搜索的寻路算法，与PRM类似，但PRM是一次性随机撒下所有点，然后去构造图，而RRT每次随机采样一个点，然后向前扩展一段距离，形成一个新的节点，不断重复此过程，最终形成一棵树，树的起点就是设定的路径起始点，最后由终点回溯形成一条可行路径。
算法伪代码如下：
具体实现步骤：
首先随机选择一个点qrand，找最近的节点qnear，并向前扩展一段得到qnew，判断qnew和qnear连线是否与障碍物发生碰撞，若不碰撞，则将qnew和其父节点qnear加入Ei，新生成的结果如下图所示：
然后重复上一步骤，继续寻找下一个点：
直到找到目标点：
2 MATLAB代码
主要由三部分构成：
构造RRT算法：
function [GEd,qw]=build_RRT(qset, NumNodes, dq, QLIST)
qinit=qset(:,1); qgoal=qset(:,2);
G=qinit; GE=[];
% 障碍物个数
Obstacle_Num = size(QLIST,1);
%% 绘制障碍物模型
for i=1:Obstacle_Num
Q = [QLIST(i,1:4);QLIST(i,5:8)];
fill(Q(1,:),Q(2,:),'k');
hold on
end
fig_num=1;
for k= 1:NumNodes
qrand=[rand*20; rand*20];
C(:,k)=qrand;
qnear= Nearest_Vertex( C(:,k) , G);
qnew= qnear + dq*(C(:,k)-qnear)/norm(C(:,k)-qnear);
if Obstacle_Num==0
G=[G,qnew];
GE= [GE, [qnear;qnew]];
plot([qnear(1),qnew(1)],[qnear(2),qnew(2)])
pause(0.1);
else
for i=1:Obstacle_Num
Q = [QLIST(i,1:4);QLIST(i,5:8)];
if isequal(1,isIntersection( qnear,qnew,Q ) )
break;
elseif i==Obstacle_Num
% 如果到最后一个障碍物都不碰撞，则将新节点及其父节点保存进GE
G=[G,qnew];
GE= [GE, [qnear;qnew]];
% 保存gif，与算法无关
plot([qnear(1),qnew(1)],[qnear(2),qnew(2)])
pause(0.1);
hold on
F=getframe(gcf);
I=frame2im(F);
[I,map]=rgb2ind(I,256);
if (fig_num==1)
imwrite(I,map,'test.gif','gif','Loopcount',inf,'DelayTime',0.2);
else
imwrite(I,map,'test.gif','gif','WriteMode','append','DelayTime',0.2);
end
fig_num = fig_num+1;
end
end
end
if isequal(qnew,qgoal) || norm(qnew-qgoal)<dq
GE= [GE, [qnew;qgoal]];
plot([qnear(1),qnew(1)],[qnear(2),qnew(2)])
hold on
break
end
end
qw=G;
GEd=GE;
end
function vv=Nearest_Vertex(q,G)
d= inf;
n= size(G,2);
for i=1:n
if norm(G(:,i) - q) < d && ~isequal(G(:,i),q)
vnew= G(:,i);
d= norm(G(:,i)-q);
end
end
vv= vnew;
end判断新节点与父节点连线是否与障碍物相撞检测算法：
function IS_INTERSECT_FLAG = isIntersection( P0,P1,Q )
%ISINTERSECTION 此处显示有关此函数的摘要
% 此处显示详细说明
% P0，P1：起始点和终点
% Q：四边形
% IS_INTERSECT_FLAG：为1则有交叉，为0则没有交叉
IS_INTERSECT_FLAG = 0;
% 判断Q为边数
N = size(Q,2);
% 扩充Q，方便后续相关参数的计算
QQ = [Q,Q(:,1)];
% 对P0，P1两个点的x坐标和y坐标分别排序
x_P_sort = sort([P0(1),P1(1)]);
y_P_sort = sort([P0(2),P1(2)]);
% 判断P0和P1是否在Q中
[P0_in,P0_on] = inpolygon(P0(1),P0(2),QQ(1,:),QQ(2,:));
[P1_in,P1_on] = inpolygon(P1(1),P1(2),QQ(1,:),QQ(2,:));
if (P0_in||P0_on) || (P1_in||P1_on)
IS_INTERSECT_FLAG=1;
disp('xxx1');
elseif (isequal(P0,P1))
% 如果两个点都不在Q中，且两个点重合，则不可能和Q相交
IS_INTERSECT_FLAG=0;
disp('xxx2');
else
% 两个点不重合且都不在Q中，计算其与Q交点
% 计算P0-P1连线参数
if (P1(1)-P0(1)==0)
k_P = inf;
b_P = 0;
else
k_P = (P1(2)-P0(2))/(P1(1)-P0(1));
b_P = P0(2)-k_P*P0(1);
end
% 循环四条边判断是否有交叉
for i=1:N
% 计算Q的第i条边的参数 y = k_Q*x+b_Q
if (QQ(1,i+1)-QQ(1,i)==0)
k_Q(i) = inf;
b_Q(i) = 0;
else
k_Q(i) = (QQ(2,i+1)-QQ(2,i))/(QQ(1,i+1)-QQ(1,i));
b_Q(i) = QQ(2,i)-k_Q(i)*QQ(1,i);
end
% 如果平行则不交叉，但不代表不重合
if (k_Q(i)==k_P)
% 如果四点共线，仍然视为相交
if k_P==inf
if (Q(1,i)==P0(1))
IS_INTERSECT_FLAG=1;
break;
end
else
if (b_P==b_Q(i))
IS_INTERSECT_FLAG=1;
break;
end
end
else
if k_P==inf
% 如果不平行则一定交叉，计算交点
X_INTERSECT = P0(1);
Y_INTERSECT = k_Q(i)*X_INTERSECT+b_Q(i);
% 判断交点是否在两条线段之间
x_Q_sort = sort([QQ(1,i),QQ(1,i+1)]);
y_Q_sort = sort([QQ(2,i),QQ(2,i+1)]);
if (X_INTERSECT>=x_Q_sort(1)&&X_INTERSECT<=x_Q_sort(2)&&Y_INTERSECT>=y_Q_sort(1)&&Y_INTERSECT<=y_Q_sort(2))
if (X_INTERSECT>=x_P_sort(1)&&X_INTERSECT<=x_P_sort(2)&&Y_INTERSECT>=y_P_sort(1)&&Y_INTERSECT<=y_P_sort(2))
IS_INTERSECT_FLAG=1;
plot(X_INTERSECT,Y_INTERSECT,'c+');
disp('xxx3');
break;
end
end
elseif k_Q(i)==inf
disp('xxx4');
% 如果不平行则一定交叉，计算交点
X_INTERSECT = Q(1,i);
Y_INTERSECT = k_P*X_INTERSECT+b_P;
% 判断交点是否在两条线段之间
x_Q_sort = sort([QQ(1,i),QQ(1,i+1)]);
y_Q_sort = sort([QQ(2,i),QQ(2,i+1)]);
if (X_INTERSECT>=x_Q_sort(1)&&X_INTERSECT<=x_Q_sort(2)&&Y_INTERSECT>=y_Q_sort(1)&&Y_INTERSECT<=y_Q_sort(2))
if (X_INTERSECT>=x_P_sort(1)&&X_INTERSECT<=x_P_sort(2)&&Y_INTERSECT>=y_P_sort(1)&&Y_INTERSECT<=y_P_sort(2))
IS_INTERSECT_FLAG=1;
plot(X_INTERSECT,Y_INTERSECT,'c+');
break;
end
end
else
% 如果不平行则一定交叉，计算交点
X_INTERSECT = (b_P-b_Q(i))/(k_Q(i)-k_P);
Y_INTERSECT = k_Q(i)*X_INTERSECT+b_Q(i);
% 判断交点是否在两条线段之间
x_Q_sort = sort([QQ(1,i),QQ(1,i+1)]);
y_Q_sort = sort([QQ(2,i),QQ(2,i+1)]);
if (X_INTERSECT>=x_Q_sort(1)&&X_INTERSECT<=x_Q_sort(2)&&Y_INTERSECT>=y_Q_sort(1)&&Y_INTERSECT<=y_Q_sort(2))
if (X_INTERSECT>=x_P_sort(1)&&X_INTERSECT<=x_P_sort(2)&&Y_INTERSECT>=y_P_sort(1)&&Y_INTERSECT<=y_P_sort(2))
IS_INTERSECT_FLAG=1;
plot(X_INTERSECT,Y_INTERSECT,'c+');
disp('xxx5');
break;
end
end
end
end
end
end主函数：
%% Prob 1. Run RRT
close all
clear
clc
qset=[3 19;3 19]; % 1st column: q_initial, 2nd: q_goal
NumNodes=5000;
dq=1;
% 多边形表示的障碍物
Q1=[1 2 2 1;1 1 2 2]; % 障碍物
Q2=[10 15 15 10;
10 10 15 15]; % 障碍物
Q3=[5 10 10 5;
5 5 10 10]; % 障碍物
Q4=[2 4 4 2;14 14 16 16]; % 障碍物
Q5=[4 6 6 4;16 16 18 18]; % 障碍物
Q6=[16 18 18 16;4 4 6 6]; % 障碍物
Q7=[12 14 14 12;2 2 4 4]; % 障碍物
Q8=[6 8 8 6;12 12 14 14]; % 障碍物
% 障碍物列表
QLIST = [Q1(1,1:4),Q1(2,1:4);
Q2(1,1:4),Q2(2,1:4);
Q3(1,1:4),Q3(2,1:4);
Q4(1,1:4),Q4(2,1:4);
Q5(1,1:4),Q5(2,1:4);
Q6(1,1:4),Q6(2,1:4);
Q7(1,1:4),Q7(2,1:4);
Q8(1,1:4),Q8(2,1:4);];
% 设置地图范围
MAX_X = 20;
MAX_Y = 20;
% 绘制地图，起始点和终止点
figure(1)
set(gcf, 'Renderer', 'painters');
set(gcf, 'Position', [500, 50, 700, 700]);
set(gca, 'XTick', 0:1:MAX_X);
set(gca, 'YTick', 0:1:MAX_Y);
grid on;
axis equal;
axis ([0 MAX_X 0 MAX_Y ]);
hold on;
% 绘制起始点和终点
LL = 0.2;
fill([qset(1,1)-LL,qset(1,1)+LL,qset(1,1)+LL,qset(1,1)-LL],[qset(2,1)-LL,qset(2,1)-LL,qset(2,1)+LL,qset(2,1)+LL],'r'); % 起始点
fill([qset(1,2)-LL,qset(1,2)+LL,qset(1,2)+LL,qset(1,2)-LL],[qset(2,2)-LL,qset(2,2)-LL,qset(2,2)+LL,qset(2,2)+LL],'b'); % 终点
% 生成RRT
[GE,qw]=build_RRT(qset, NumNodes, dq, QLIST); %GE is [qnear ; qnew]
%the first and last columns have q_init and q_goal
qc1=GE(1:2,:); qc2=GE(3:4,:);
qc1=flip(qc1,2); % 父节点
qc2=flip(qc2,2); % 子节点
qpath=[qc1(:,1)]; % 目标节点的父节点
while ~isequal(qpath(:,1),qset(:,1))
for i=1:size(GE,2)
if isequal(qpath(:,1), qc2(:,i))
qpath=[qc1(:,i) qpath];
break;
end
end
end
qpath= [qpath qset(:,2)];
disp(qpath)
plot(qpath(1,:),qpath(2,:),'b--o')
hold on3 运行结果
该算法搜索时间过长，应该是没有加启发函数，扩展方向比较随机导致的。
4 github链接
5 参考
GitHub - gn7550/RRT-PRM-codes: RRT and PRM algorithms I have written
在上述参考代码中，改进了障碍物检测部分的算法实现，以及多障碍物检测逻辑
5391
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
