无人驾驶全局路径规划之RRT算法

根据查阅网上关于RRT算法的原理及代码，再结合自己的理解，使用MATLAB实现RRT算法生成路径的整个过程。RRT的原理部分我不再赘述，网上介绍的文章还是很多的。
下面主要展示自己编的RRT算法，并使用MATLAB自带的RRT路径规划函数进行简单比对。

RRT算法自编代码实现

clear;
close all;
C=zeros(30,30);%一般默认权值大于0.65的网格为有障碍物的网格
%注意障碍物的权值按行列生成，而路径曲线是以左下角为原点生成，注意两者的区别
%C为生成整个网格地图的各个网格权值的矩阵
C(15:18,10:16)=0.9*ones(4,7);
C(21:26,15:27)=0.9*ones(6,13);
C(6:12,3:15)=0.9*ones(7,13);
%生成带权值的网格地图
costmap = vehicleCostmap(C,'CellSize',1);%每个网格边长1米
%设置障碍物的膨胀范围
ccConfig =inflationCollisionChecker('CenterPlacements',[0.2 0.5 0.8],'InflationRadius',0);
costmap.CollisionChecker = ccConfig;
%画出整个地图
plot(costmap);hold on;
set(gca,"XTick",0:1:30);
set(gca,"YTick",0:1:30);
grid on;%hold on;
legend("off");

%检测有障碍物的区域
occMat = checkOccupied(costmap);

x_start=[1,1];%起点坐标
goal=[27,27];%终点坐标
grow_distance=1;%生长步长
goal_radius=1.5;%搜索停止阈值，如果新生成的节点距离终点距离小于1.5米，则停止搜索路径
%画出起点
plot(x_start(1),x_start(2),'k>','MarkerFaceColor','g','MarkerSize',9);hold on;
%画出终点
plot(goal(1),goal(2),'ko','MarkerFaceColor','r','MarkerSize',9);hold on;
%使用结构体存储子节点，父节点，以及父节点在子节点中的位置
tree.child=[];
tree.parent=[];
tree.parent_idx=[];
%初始化结构体tree
tree.child=x_start;
tree.parent=x_start;
tree.parent_idx=1;

flag=1;
while flag
    %在地图范围内随机生成一个点(rd_x，rd_y)
    rd_x=30*rand(1);
    rd_y=30*rand(1);
    fix_rd_x=fix(rd_x)+1;
    fix_rd_y=fix(rd_y)+1;
    %调用自定义函数cal_distance，计算新随机点离子节点tree.child中哪个子节点最近，
    %返回最近的那个子节点行号、两者的连线和x坐标轴的夹角、最短距离
    [angle,min_idx,distance]=cal_distance(rd_x,rd_y,tree);
    
    %随机点的坐标转换为权值矩阵上网格所在的行列，并判断该点是否在障碍物上，
    %如果该点不在障碍物上，并且最短距离大于grow_distance（防止在计算新节点时超边界），则生成新节点
    if (occMat(31-fix_rd_y,fix_rd_x)==false) && (distance>grow_distance)%此处注意路径曲线上的点坐标和障碍物权值矩阵之间对应关系的转换
        
        %沿着上述夹角方向按生长步长生成新节点
        new_node_x=tree.child(min_idx,1)+grow_distance*cos(angle);
        new_node_y=tree.child(min_idx,2)+grow_distance*sin(angle);
        new_node=[new_node_x,new_node_y];
        %沿着上述夹角方向，在生长步长内生成100个中间点，点越多，则检测碰撞的精度越高，当然计算量也会越大
        L=linspace(0,grow_distance,100);
        direc_x=double(rd_x>tree.child(min_idx,1))*2-1;%判断新节点相对于父节点在x轴上的方向（正方向或负方向）
        direc_y=double(rd_y>tree.child(min_idx,2))*2-1;%判断新节点相对于父节点在y轴上的方向（正方向或负方向）
        x_cha=tree.child(min_idx,1)+direc_x*(L*abs(cos(angle)));%100个中间点的横坐标
        y_cha=tree.child(min_idx,2)+direc_y*(L*abs(sin(angle)));%100个中间点的纵坐标
        cha_occMat=[];
        %计算线段上所有这100个点是否位于障碍物上
        for j=1:length(x_cha)
            cha_occMat=[cha_occMat,double(occMat(31-(fix(y_cha(j))+1),fix(x_cha(j))+1)==false)];  
        end
        %所有的点都不处在障碍物上，才将上述生产的新节点新增到结构体中
        if sum(cha_occMat)==length(x_cha)
            tree.child(end+1,:)=new_node;
            tree.parent(end+1,:)=[tree.child(min_idx,1),tree.child(min_idx,2)];
            tree.parent_idx(end+1)=min_idx;
            plot(rd_x,rd_y,'.r');hold on;%画出随机生成的点
            plot(new_node_x,new_node_y,'.g');hold on;%画出新节点
            plot([tree.child(min_idx,1),new_node_x],[tree.child(min_idx,2),new_node_y],'b-');hold on;%画出新节点及其父节点的连线
        end
        %如果新节点到终点的距离小于搜索停止阈值，则停止搜索，
        %并将终点加入到tree中
        if sqrt((new_node_x-goal(1,1))^2+(new_node_y-goal(1,2))^2)<=goal_radius
            tree.child(end+1,:)=goal;
            tree.parent(end+1,:)=new_node;
            tree.parent_idx(end+1)=size(tree.parent,1)-1;
            break;
        end
    end 
end

%画出生成的路径
tem1=length(tree.parent_idx);%从终点所在的行开始搜索
%zuiyou_idx存储生成路径的点所在tree中的行号
zuiyou_idx=tem1;%首先将终点所在的行加入进去
while tem1>1 %如果没有到起点的行号（也就是第一行），则一直找下去
    zuiyou_idx(end+1)=tree.parent_idx(tem1);%该行的parent_idx存着该行父节点parent在子节点child中的行号
    tem1=tree.parent_idx(tem1);%使用当前行的parent_idx更新tem1   
end
zuiyou_lujing=tree.child(zuiyou_idx,:);
%画出所生成的路径
plot(zuiyou_lujing(:,1),zuiyou_lujing(:,2),'-r',"LineWidth",2);
hold off;

其中自定义函数如下：

function [angle, min_idx,distance] = cal_distance(rd_x, rd_y, tree)
    distance = [];
    i = 1;
    while i<=size(tree.child,1)
        dx = rd_x - tree.child(i,1);
        dy = rd_y - tree.child(i,2);
        d = sqrt(dx^2 + dy^2);
        distance(i) = d;
        i = i+1;
    end
    [distance, min_idx] = min(distance);
    angle = atan2(rd_y - tree.child(min_idx,2),rd_x - tree.child(min_idx,1));
end

运行代码几次的展示结果如下：

MATLAB自带RRT算法实现

C=zeros(30,30);%一般默认权值大于0.65的网格为有障碍物的网格
%注意障碍物的权值按行列生成，而路径曲线是以左下角为原点生成，注意两者的区别
%C为生成整个网格地图的各个网格权值的矩阵
C(15:18,10:16)=0.9*ones(4,7);
C(21:26,15:27)=0.9*ones(6,13);
C(6:12,3:15)=0.9*ones(7,13);
%生成带权值的网格地图
costmap = vehicleCostmap(C,'CellSize',1);%每个网格边长1米
%设置障碍物的膨胀范围
vdims = vehicleDimensions(2.2,0.6,1.5, ...
    'FrontOverhang',0.37,'RearOverhang',0.32);
numCircles = 3;
ccConfig =inflationCollisionChecker(vdims,numCircles,'CenterPlacements',[0.2 0.5 0.8], ...
    'InflationRadius',0);
costmap.CollisionChecker = ccConfig;
%画出整个地图
figure,plot(costmap);hold on;
set(gca,"XTick",0:1:30);
set(gca,"YTick",0:1:30);
grid on;%hold on;
legend("off");

startPose = [1, 1, 0]; % 起始点位置及姿态
goalPose  = [27, 27, 90]; %终点位置及姿态

planner = pathPlannerRRT(costmap);
refPath = plan(planner,startPose,goalPose);%规划路径

plot(planner);legend("off"); 
hold off

运行代码几次的展示结果如下：



注：主要思路参考自这位大神https://blog.csdn.net/m0_55205668/article/details/123922046