RRT算法

RRT--快速搜索随机树算法是一种在完全已知的环境中通过随机采样扩展搜索的算法

其特点是，概率完备，如果规划时间足够长，如果确实存在一条可行的最优路径，RRT是可以找出来的，但这里存在限制条件，如果规划时间不够，迭代次数较少，有可能无法找到实际存在路径。

主要优点是快，因此在多自由度机器人的规划问题中发挥着较大的作用，例如机械臂的移动规划算法，缺点是规划的路径通常不最优，路径不平滑

使用randi函数随机撒点，生成的随机数表示栅格的线性索引。根据子节点扩展方向确定对应的栅格，将父节点邻近的16个节点作为备选子节点，然后通过父节点与采样点的连线计算角度，然后根据角度在16份角度中的范围，确定被选中的子节点，同时考虑到行列坐标与x/y坐标的不对应，在计算连线角度时采用atan2函数。

生成随机数

function samplePoint = getSamplePoint(field,treeNodes)
%UNTITLED10 此处提供此函数的摘要
[rows,cols] = size(field);
field(treeNodes(:,1)) = 3;
    while true
        samplePoint = randi([1,rows*cols]);
        if field(samplePoint) == 1
            break;
        end
    end
end

获取子节点

function childNode = getChildNode(field,parentNode, samplePoint )
%定义生长步长为2个栅格，选取父节点周边16个节点作为备选子节点
%根据父节点与随机采样点的角度，确定生长的子节点

[rows,cols] = size(field);
[row_samplePoint,col_samplePoint] = ind2sub([rows,cols], samplePoint ) ;
[row_parentNode,col_parentNode] = ind2sub([rows,cols],parentNode);

%定义16个点的行列坐标
%注意，为了行列坐标与xy坐标匹配，从父结点的下节点逆时针开始定义
childNode_set = [ row_parentNode+2 , col_parentNode;
    row_parentNode+2,col_parentNode+1;
    row_parentNode+2,col_parentNode+2;
    row_parentNode+1,col_parentNode+2;
    row_parentNode,col_parentNode+2;
    row_parentNode-1,col_parentNode+2;
    row_parentNode-2,col_parentNode+2;
    row_parentNode-2,col_parentNode+1;
    row_parentNode-2,col_parentNode;
    row_parentNode-2,col_parentNode-1;
    row_parentNode-2,col_parentNode-2;
    row_parentNode-1,col_parentNode-2; 
    row_parentNode,col_parentNode-2;
    row_parentNode+1,col_parentNode-2; 
    row_parentNode+2,col_parentNode-2;
    row_parentNode+2,col_parentNode-1];

%计算16个子节点的角度范围集，和当前随机点的角度范围

theta_set = linspace(0,2*pi,16);
theta = atan2((col_samplePoint - col_parentNode), ...
    (row_samplePoint - row_parentNode));

%若theta位于第三四象限，加上2*pi
 if theta < 0
     theta =theta + 2*pi;
 end
 %遍历周围的十六个点，判断角度位于那个范围
 for i = 1:15
     if theta >= theta_set(i) && theta <theta_set(i+1)
         childNodeIdx = i;
         break
     end
 end

 %选中的子节点
 childNode = childNode_set(childNodeIdx,:);
  


end

 判断子节点是否在障碍物上

function flag = judgeObs(field,parentNode,childNode)
%UNTITLED 此处提供此函数的摘要
flag = 0;
[rows, cols] = size(field);

% 判断子节点是否在障碍物上
obsIdx = find(field == 2);
if ismember(childNode, obsIdx)
    flag = 1;
    return
end
%判断父节点与子节点的连线是否跨过障碍物
[parentNode(1),parentNode(2)] = ind2sub([rows,cols],parentNode);
[childNode(1),childNode(2)] = ind2sub([rows,cols],childNode);

P2 = parentNode;
P1 = childNode;
row_min = min([P1(1),P2(1)]);
row_max = max([P1(1),P2(1)]);
col_min = min([P1(2),P2(2)]);
col_max = max([P1(2),P2(2)]);
for row =  row_min: row_max
    for col = col_min:col_max
        if field(row,col) == 2
            P = [row,col];

            % 直接接地计算障碍物节点距离P1和P2构成的连线的距离
            d = abs(det([P2-P1;P-P1]))/norm(P2-P1);
            if d< 0.5
                flag = 1;
                return
            end
        end
    end
end




end

 

RRT算法

%基于栅格地图的机器人路径规划算法

clc
clear
close all


%% 障碍物、空白区域、起始点、目标点定义

%行数和列数
rows = 30;
cols = 50;
[field,camp] = defColorMap(rows,cols);

%起点、终点、障碍物区域
starPos = 2;
goalPos = rows*cols-2;
field(starPos) = 4;
field(goalPos) = 5;

 %% 算法

 %定义树节点，第一列放节点编号，第二列放该节点的父节点
 treeNodes = [starPos,0];

 while true
     %初始化parentNode和chileNode
     parentNode = [];
     childNode = [];
     %在地图空间随机采样
     samplePoint = getSamplePoint(field,treeNodes);

     %依次遍历每一个树节点到采样点的距离，取最小值对应的树节点
     for i = 1:size(treeNodes,1)
         [row_treeNode,col_treeNode] = ind2sub([rows,cols],treeNodes(i,1));
         [row_samplePoint,col_samplePoint] = ind2sub([rows,cols],samplePoint);
         dist(i) = norm([row_treeNode,col_treeNode] - [row_samplePoint,col_samplePoint]);
     end
     [~,idx] = min(dist);
     parentNode = treeNodes(idx,1);

     %生成新的子节点，行列坐标
     childNode = getChildNode(field,parentNode,samplePoint);
     %判断该子节点是否超过地图限制
     if childNode(1) < 1 || childNode(1) > rows  || childNode(2) < 1 || childNode(2) > cols
         continue
     else

         %转为线性索引
         childNode = sub2ind([rows,cols],childNode(1),childNode(2));
     end

     %判断父节点与子节点的连线是否跨过障碍物
     flag = judgeObs(field, parentNode,childNode);
     if flag
         continue
     end

     %判断该子节点是否已经存在于tree Nodes，未在则追加到treeeNodes
     if ismember(childNode,treeNodes(:,1))
         continue
     else
         treeNodes(end+1,:) = [childNode,parentNode];
     end

     %判断子节点是否位于目标区域

     [row_childNode,col_childNode] = ind2sub([rows,cols],childNode);
     [row_goalPos,col_goalPos] = ind2sub([rows,cols],goalPos);
     if abs(row_childNode - row_goalPos) + abs(col_childNode - col_goalPos) < 2
         break
     end
 end



 %% 找出最优目标路径
 %最优路径
 path_opt = [];
 idx = size(treeNodes,1);
 while true
     path_opt(end+1) = treeNodes(idx,1);
     parentNode = treeNodes(idx,2);
     if parentNode == starPos
         break
     else
         idx = find (treeNodes(:,1)== parentNode);
     end
 end

 %路径信息反馈到field中
 field(treeNodes(:,1)) = 3;
 field(path_opt) = 6;
 field(starPos) = 4;
 field(goalPos) = 5;


  %% 画栅格图
image(1.5,1.5,field);
grid on;
set(gca,'gridline','-','gridcolor','k','linewidth',2,'GridAlpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
axis image;