RRT——rapidly exploring random tree，即快速扩展随机树，是基于随机采样的路径规划算法，以起点为根节点，在搜索空间中随机产生节点，通过进行步长计算、碰撞检测等操作来产生新的子节点，直到采样至终点为止。伪代码如图所示：
图源： /long5683/p/11795987.html
matlab代码实现：
main函数：
clcclearclose all%障碍物顶点初始化obs(1).index=1;obs(1).x=[3,4,3,2,3];obs(1).y=[1,2,3,3,1];obs(2).index=2;obs(2).x=[6,7,7,5,6];obs(2).y=[3,3,5,5,3];%10*6的空间x_range=[0,10];%横轴范围约束y_range=[0,6];%纵轴范围约束init=[1,1];%起始点坐标%第一个节点赋值T.vertex(1).x=init(1);T.vertex(1).y=init(2);T.vertex(1).parent=T.vertex(1);%起始点的父节点是自身T.vertex(1).index=1;%节点的索引goal=[9,5];%目标点坐标end_radiu=1;%距离目标点多远可以终止搜索%绘制初始图像figureplot(init(1),init(2),'o','MarkerSize',5,'Color','g')%绘制起点hold on plot(goal(1),goal(2),'o','MarkerSize',5,'Color','b')%绘制终点rectangle('position',[goal(1)-end_radiu,goal(2)-end_radiu,2*end_radiu,2*end_radiu],'curvature',[1,1]);%绘制终止域范围axis equalxlim([0,10]);ylim([0,6]);for obs_index=1:size(obs,2)%绘制障碍物pause(0.1);hold onplot(obs(obs_index).x,obs(obs_index).y)fill(obs(obs_index).x,obs(obs_index).y,'b')endStepSize=0.6;%采样步长i=1;while sqrt((T.vertex(i).x-goal(1))^2+(T.vertex(i).y-goal(2))^2)>end_radiu %如果未达到终止条件，则继续进行采样i=i+1;%索引更新[index,new_point]=Near(T,x_range,y_range,StepSize);%生成随机点，并返回树上距离随机点最近的点索引feasible=check_Collision(obs,new_point,T.vertex(index).x,T.vertex(index).y);%碰撞检测，true表示发生碰撞if ~feasibleT.vertex(i).x=new_point(1);%给新的可行节点赋值T.vertex(i).y=new_point(2);T.vertex(i).parent=T.vertex(index);T.vertex(index).son=T.vertex(i);T.vertex(i).index=i;%第i个节点的索引elsei=i-1;continue;endpause(0.1);hold onplot(T.vertex(i).x,T.vertex(i).y,'*','MarkerSize',5,'Color','r');line([T.vertex(i).x,T.vertex(index).x],[T.vertex(i).y,T.vertex(index).y],'Color','g');endline([T.vertex(i).x,goal(1)],[T.vertex(i).y,goal(2)]);%最后一个点与终点连线path=[goal(1),goal(2)];route=T.vertex(i);%绘制路线while 1path=[path;route.x,route.y];if route.index==1breakendroute=route.parent;endfor i=1:size(path,1)-1line([path(i,1),path(i+1,1)],[path(i,2),path(i+1,2)],'Color','r','linewidth',3)enddisp('done!')
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_distmin_dist=dist;index=i;endendif min_dist<=StepSize%在步长范围内直接使用该点new_point(1)=rand_point(1);new_point(2)=rand_point(2);elsetheta = 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_distcheck_point.x=x+step*sin(theta);check_point.y=y+step*cos(theta);feasible=isinPolygon(check_point,obs(k));if feasiblereturn;endendend
判断指定点与凸多边形的关系可见以下文章，此处不再赘述：
/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+S4in_out=true;elsein_out=false;endend
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
运行结果如下图所示：
终点处的圆表示的是终止搜索范围，当采样点进入该范围内，即可直接连接至终点，不过，程序中并未对此步骤进行碰撞检测，可以根据自己的需要进行修改完善。
