Matlab复现RRT算法

快速搜索随机树(Rapid-exploration Random Tree,RRT)算法是一种在完全己知的环境中通过随机采样扩展搜索的算法

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

优点：最主要的优点就是快，因此在多自由度机器人的规划问题中发挥着较大的作用，比如机械臂的规划算法基本都是以RRT为基础的。

缺点：规划的路径通常不最优、路径不平滑等。

算法思想:

MATLAB编写RRT算法的几个核心思想：

• 在随机采样撒点时，可以利用randi函数，随机生成一个[l,n]范围的整数，代表采样栅格；
• 如何根据子节点扩展方向确定对应的栅格？首先把父节点邻近的16个节点作为备选的子节点，从正下方编号为1，逆时针依次增加，如图1。然后把2π角度划分为16份，根据所确定的父节点与采样点
  的连线可以可以计算一个角度，根据这个角度在16份角度的范围，从而确定被选中的子节点。同时考虑到行列坐标与x/y坐标的不对应，在计算连线角度时采用atan2函数，而不用atan函数，如图2.
• 如何判断父节点与子节点的连线是否跨过障碍物？如图3，父子节点的连线与障碍物的最短距离d大于栅格单位长度的一半，表明未跨过障碍物。并且仅需讨论蓝色虚线框内的障碍物栅格即可。

源代码：

init.m

function [field,cmap] = defColorMap(rows, cols)
cmap = [1 1 1; ...       % 1-白色-空地
    0 0 0; ...           % 2-黑色-静态障碍
    1 0 0; ...           % 3-红色-已经搜索过的点
    1 1 0;...            % 4-黄色-起始点 
    1 0 1;...            % 5-品红-目标点
    0 1 0; ...           % 6-绿色-到目标点的规划路径   
    0 1 1];              % 7-青色-动态规划的路径

% 构建颜色MAP图
colormap(cmap);

% 定义栅格地图全域，并初始化空白区域
field = ones(rows, cols);

% 障碍物区域
obsRate = 0.3;
obsNum = floor(rows*cols*obsRate);
obsIndex = randi([1,rows*cols],obsNum,1);
field(obsIndex) = 2;

getChildNode.m：得到生长节点

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个点的行列坐标
% 注意，为了行列坐标与x/y坐标匹配，从父节点的下节点逆时针开始定义，依次编号
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

% 遍历周围的16个点，判断角度位于哪一个范围
for i = 1:15
    if theta >= theta_set(i) && theta < theta_set(i+1)
        childNodeIdx = i;
        break
    end
end

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

getSamplePoint.m: 随机生成采样点：

function samplePoint = getSamplePoint(field, treeNodes)
[rows, cols] = size(field);
field(treeNodes(:,1)) = 3;
while true
    samplePoint = randi([1,rows*cols]);
    if field(samplePoint) == 1
        break;
    end
end

judgeObs.m: 判断障碍物

function flag = judgeObs(field, parentNode, childNode)

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
            

RRT.m: 生成路径：

% 基于栅格地图的机器人路径规划算法
% 第4节：RRT算法
clc
clear
close all

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

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

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


%% 算法

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

while true
    % 初始化parentNode和childNode
    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
    
    % 判断该子节点是否已经存在于treeNodes，未在则追加到treeNodes
    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 == startPos
        break;
    else
        idx = find(treeNodes(:,1) == parentNode);
    end    
end

% 路径信息反映到field中
field(treeNodes(:,1)) = 3;
field(path_opt) = 6;
field(startPos) = 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;

运行结果：

将绿色部分连接就得到了基于RRT算法的最短路径（红色代表算法运行过程中生成的随机采样点），可以看到，这个路径并不是最优的而是相对较优的，需要我们继续对搜索的路径做平滑处理