RRT算法

%% 此代码有一种情况会死循环
% 起始点周围全是障碍物时会出现死循环的情况，应该加一些代码进行判断，是这种请款直接跳出循环（算法处的while）

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

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

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

% 起点、终点、障碍物区域
startPos = 2;
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; 

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;

function samplePoint = getSamplePoint(field, treeNodes)
[rows, cols] = size(field);
%field(treeNodes(:,1)) = 3; %这句话多余，因为matlab是值传递不是引用  
while true
    samplePoint = randi([1,rows*cols]);
    if field(samplePoint) == 1
        break;
    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个点的行列坐标
% 注意，为了行列坐标与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,:);

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