RRT——rapidly exploring random tree,即快速扩展随机树,是基于随机采样的路径规划算法,以起点为根节点,在搜索空间中随机产生节点,通过进行步长计算、碰撞检测等操作来产生新的子节点,直到采样至终点为止。伪代码如图所示:
图源: https://www.cnblogs.com/long5683/p/11795987.html
matlab代码实现: Near函数产生新的随机采样点,同时还要计算树上与该点距离最近的节点,并返回其在树上的索引值。
function [index,new_point]=Near(T,x_range,y_range,StepSize)%产生新的节点,并返回树上距该点最近点的索引
rand_point=[x_range(2)*rand, y_range(2)*rand];%产生随机点
min_dist=sqrt((rand_point(1)-T.vertex(1).x)^2+(rand_point(2)-T.vertex(1).y)^2);
index=1;
for i=1:size(T.vertex,2)
dist=sqrt((rand_point(1)-T.vertex(i).x)^2+(rand_point(2)-T.vertex(i).y)^2);
if dist<min_dist
min_dist=dist;
index=i;
end
end
if min_dist<=StepSize%在步长范围内直接使用该点
new_point(1)=rand_point(1);
new_point(2)=rand_point(2);
else
theta = atan2(rand_point(1)-T.vertex(index).x,rand_point(2)-T.vertex(index).y);
new_point(1)=T.vertex(index).x+StepSize*sin(theta);
new_point(2)=T.vertex(index).y+StepSize*cos(theta);
end
check_Collision函数用于对新生成的节点进行碰撞检测,方法是在新生成的节点与树上最近点之间进行定步长采样,计算采样点是否与障碍物发生碰撞,因此采样步长需要足够小,防止误判。
function feasible=check_Collision(obs,new_point,x,y)
min_dist=sqrt((new_point(1)-x)^2+(new_point(2)-y)^2);
theta = atan2(new_point(1)-x,new_point(2)-y);
feasible=false;
for k=1:size(obs,2)
for step=0:0.001:min_dist
check_point.x=x+step*sin(theta);
check_point.y=y+step*cos(theta);
feasible=isinPolygon(check_point,obs(k));
if feasible
return;
end
end
end
判断指定点与凸多边形的关系可见以下文章,此处不再赘述:
https://blog.csdn.net/qq_36250209/article/details/123763849
function in_out=isinPolygon(Q,obs)%判断一个点是否在凸多边形之外——利用目标点与多边形各顶点构成的面积与多边形面积进行计算判断
%如果面积之和大于多边形面积,则在外部,否则在内部
%将多边形面积计算转化为各个三角形的面积计算,可以简化计算过程
%输入:目标点的坐标,凸多边形的顶点坐标 最大边数为五边形 输入的多边形顶点按逆时针或顺时针顺序输入
%输出:判断结果
S1=calc_triangle(Q.x,Q.y,obs.x(1),obs.y(1),obs.x(2),obs.y(2));
S2=calc_triangle(Q.x,Q.y,obs.x(2),obs.y(2),obs.x(3),obs.y(3));
S3=calc_triangle(Q.x,Q.y,obs.x(3),obs.y(3),obs.x(4),obs.y(4));
S4=calc_triangle(Q.x,Q.y,obs.x(4),obs.y(4),obs.x(5),obs.y(5));
S = calc_triangle(obs.x(1),obs.y(1),obs.x(2),obs.y(2),obs.x(3),obs.y(3))+ ...
calc_triangle(obs.x(1),obs.y(1),obs.x(3),obs.y(3),obs.x(4),obs.y(4));
if S==S1+S2+S3+S4
in_out=true;
else
in_out=false;
end
end
function S=calc_triangle(x0,y0,x1,y1,x2,y2)%计算三角形面积——利用三角形各点与横坐标垂直连线构成的梯形面积相加减计算三角形面积
S=0.5*abs(x0*y1+x1*y2+x2*y0-x0*y2-x1*y0-x2*y1);
end
运行结果如下图所示: 终点处的圆表示的是终止搜索范围,当采样点进入该范围内,即可直接连接至终点,不过,程序中并未对此步骤进行碰撞检测,可以根据自己的需要进行修改完善。
|