路径规划——RRT算法实现

RRT——rapidly exploring random tree，即快速扩展随机树，是基于随机采样的路径规划算法，以起点为根节点，在搜索空间中随机产生节点，通过进行步长计算、碰撞检测等操作来产生新的子节点，直到采样至终点为止。伪代码如图所示：

图源： https://www.cnblogs.com/long5683/p/11795987.html

matlab代码实现：
main函数：

clc
clear
close 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;%距离目标点多远可以终止搜索
%绘制初始图像
figure
plot(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 equal
xlim([0,10]);
ylim([0,6]);
for obs_index=1:size(obs,2)%绘制障碍物
    pause(0.1);
    hold on
    plot(obs(obs_index).x,obs(obs_index).y)
    fill(obs(obs_index).x,obs(obs_index).y,'b')
end
StepSize=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 ~feasible
        T.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个节点的索引
    else
        i=i-1;
        continue;
    end
    pause(0.1);
    hold on
    plot(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');
end
line([T.vertex(i).x,goal(1)],[T.vertex(i).y,goal(2)]);%最后一个点与终点连线
path=[goal(1),goal(2)];
route=T.vertex(i);
%绘制路线
while 1
    path=[path;route.x,route.y];
    if route.index==1
        break
    end
    route=route.parent;
end
for i=1:size(path,1)-1
    line([path(i,1),path(i+1,1)],[path(i,2),path(i+1,2)],'Color','r','linewidth',3)
end
disp('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_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

运行结果如下图所示：

终点处的圆表示的是终止搜索范围，当采样点进入该范围内，即可直接连接至终点，不过，程序中并未对此步骤进行碰撞检测，可以根据自己的需要进行修改完善。