目录
1 RRT算法的简介
2 RRT算法原理
2.1 算法流程
2.2 算法伪代码
2.3 算法流程图
3 RRT算法matlab实现
3.1 测试地图
3.2 distance函数
3.3 RRT算法
3.4 动画效果
4 RRT的缺陷
1 RRT算法的简介
天下武功唯快不破,快是 RRT 的最大优势。RRT 的思想是快速扩张一群像树一样的路径以探索空间的大部分区域,找到可行的路径。RRT 算法是一种对状态空间随机采样的算法,通过对采样点进行碰撞检测,避免了对空间的精确建模带来的大计算量,能够有效地解决高维空间和复杂约束的路径规划问题。与PRM类似,该方法是概率完备且非最优的。可以轻松处理障碍物和差分约束(非完整和动力学)的问题,并被广泛应用于机器人路径规划。
2 RRT算法原理
2.1 算法流程
(1)设定初始点 与目标点 ,自行设定状态采样空间
(2)进行随机采样得到采样点,如果采样点在障碍物内,则重新随机采样
(3)若不在障碍物内,计算该采样点 与集合 (已经生成的节点) 中的所有节点之间的距离,得到离得最近的节点 ,再从节点 以步长 走向节点 ,生成一个新的节点 ,若 与 的连线 经过障碍物,则重新随机采样
(4)经过反复迭代,生成一个随机扩展树,当随机扩展树中的子节点进入了我们规定的目标区域,便可以在随机扩展树中找到一条由从初始点到目标点的路径。
2.2 算法伪代码
可以将伪代码与上述算法流程对照起来看
2.3 算法流程图
3 RRT算法matlab实现
3.1 测试地图
-
%随机生成障碍物
-
function [f,n1]=ob(n)
-
f=[];%储存障碍物信息
-
n1=n;%返回障碍物个数
-
p=0;
-
for i=1:n
-
k=1;
-
while(k)
-
D=[rand(1,2)*60+15,rand(1,1)*1+3];%随机生成障碍物的坐标与半径,自行调整
-
if(distance(D(1),D(2),90,90)>(D(3)+5)) %与目标点距离一定长度,防止过多阻碍机器人到达目标点
-
k=0;
-
end
-
for t=1:p %障碍物之间的距离不能过窄,可自行调整去测试
-
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
-
%画出障碍物
-
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;%目前生成的障碍物个数
-
end
-
hold all;
3.2 distance函数
-
function f=distance(x,y,x1,y1)
-
f=sqrt((x-x1)^2+(y-y1)^2);
3.3 RRT算法
-
clc
-
clear all
-
[f,n1]=ob(10);%随机生成障碍物
-
Xinit=[1,1];%定义初始点
-
Xgoal=[90,90];%定义目标点
-
plot(Xinit(1),Xinit(2),'ro');
-
plot(Xgoal(1),Xgoal(2),'ko');
-
T=[Xinit(1),Xinit(2)];%已生成节点集合用顺序表的数据结构存储
-
Xnew=Xinit;
-
D(1)=0;%初始节点的父节点指向0
-
while distance(Xnew(1),Xnew(2),Xgoal(1),Xgoal(2))>3 %进入目标区域
-
Xrand=round(rand(1,2)*100)+1;%状态采样空间:横纵坐标均为整数,范围1~100
-
k=1;%进入循环
-
while k==1
-
k=0;%初始化采样成功
-
for i=1:n1
-
if distance(Xrand(1),Xrand(2),f(i*3-2),f(i*3-1))<(f(i*3)+1)%判断随机采样点是否在障碍物内
-
k=1;%采样不成功
-
break;
-
end
-
end
-
Xrand=round(rand(1,2)*100);%重新采样
-
end
-
min=10000;
-
for i=1:size(T,2)/2 %遍历已生成节点集合
-
if distance(T(2*i-1),T(2*i),Xrand(1),Xrand(2))<min %得到最近的节点
-
min=distance(T(2*i-1),T(2*i),Xrand(1),Xrand(2));
-
Xnear=[T(2*i-1),T(2*i)];
-
end
-
end
-
stepsize=3/distance(Xnear(1),Xnear(2),Xrand(1),Xrand(2));%以一定步长前进,这里选择3
-
Xnew=[Xrand(1)*stepsize+Xnear(1)*(1-stepsize),Xrand(2)*stepsize+Xnear(2)*(1-stepsize)];
-
t=0;%初始状态未碰撞
-
if Xnear(1)>Xnew(1)
-
caiyang=-0.001;
-
else
-
caiyang=0.001;
-
end
-
for i=Xnear(1):caiyang:Xnew(1)%均匀采样进行碰撞检测
-
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;%代表碰撞
-
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 %遍历已生成节点集合
-
if (T(i*2-1)==Xnear(1))&&(T(i*2)==Xnear(2)) %得到最近的节点的索引
-
D(size(T,2)/2)=i;%记录父节点
-
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); %通过链表回溯
-
if D(i)~=0
-
jg=[D(i),jg];%存储最短路径通过的节点
-
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 动画效果
4 RRT的缺陷
(1)很明显RRT算法得到的路径不是最优的
(2)RRT算法未考虑运动学模型
(3)RRT算法对于狭小的通道的探索性能不好,如下图的对比,有可能探索不到出口
(4)没有启发信息的RRT像无头苍蝇,探索空间完全靠运气,如下图
针对上述缺陷,又出现了很多RRT算法的变种,以后的文章中会介绍。