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] = init(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算法的最短路径（红色代表算法运行过程中生成的随机采样点），可以看到，这个路径并不是最优的而是相对较优的，需要我们继续对搜索的路径做平滑处理
你只管出发，旅途自有风景~~
