基于采样的路径规划算法总结：RRT-Matlab实现

任务说明

• 在一张大小800*800具有障碍物的地图里实现RRT算法

算法流程

• 流程图

• 流程描述

1. Sample()函数在地图上随机采样一个点Xrand

2. 遍历树T得到距离Xrand最近的点Xnear

3. 扩展Xnear得到Xnew，检查Xnew以及其Edget是否与障碍物发生碰撞

4. 将Xnew插入到树中

5. 判断Xnew是否在树附近

1. 是，则回查将X_near和X_new之间的路径画出来

2. 否，返回步骤1，直到找到目标点

技术实现

所需matlab函数

• imread(filename) 从filename指定文件读取图像

• rgb2gray(RGB_filename) 将彩色图片转换为灰度图

• imshow(gray) 在图窗中显示灰度图像

• size(A,dim) 返回维度dim的长度

• rand(1,1) 返回一个1\*1维度的数据，在0~1之间均匀产生一个数

• 函数句柄

• 创建函数句柄：f = @myfunction 例如，有一个名为myfunction的函数，创建一个名为f的句柄，f相当于函数的别名

• 匿名函数：匿名函数是基于单行表达式的 MATLAB 函数，不需要程序文件。构造指向匿名函数的句柄，方法是定义 `anonymous_function` 函数主体，以及指向匿名函数 `arglist` 的以逗号分隔的输入参数列表。语法为：h = @(arglist)anonymous_function

• arrayfun(func,A) 将函数func应用于A中的每一个元素

• 将函数应用于结构体数组的字段 S(1).f1 = rand(1,5) S(2).f1 = rand(1,10) // A = arrayfun(@(x) mean(x.f1),S)

• atan2(Y,X) 求反正切

• tic ... toc 求...运行时间

• disp('字符串') 显示字符串

• X.^2 求X的二次幂

• r.*[... ] 乘以数组中每个元素

• sqrt() 求平方根

• ceil() Y = ceil(x)将 `X` 的每个元素四舍五入到大于或等于该元素的最接近整数

算法实现

1. 初始化

1. 每个树的节点用struct表示，每个struct要记录当前节点在图像中的位置、parent在图像中的位置（用于回查路径）、当前节点距离parent的步长Edget、上一个节点在T中的位置（用于回查路径）

2. 如何得到800*800地图上的一个随机采样点？

1. 用 边长*rand(1,1) ，这样就能分别得到x与y轴的值 x_rand =[xL* rand, yL*rand]

3. 节点如何表示？如何遍历树得到x_near？

1. 使用arrayfun遍历每个struct节点，计算每个节点到x_rand的距离，最后得到一个数组；用min()求出最小距离对应的index即可

4. 如何延长x_near得到x_new?

1. 用atan2求出x_near与x_rand之间的角度，通过角度求出x_new位置

5. 如何检查collision free？

1. 主要思想是对X_new和X_near之间的线段上的点全部检验，看是否超出边界、穿越障碍物posCheck = startPose + r.*[sin(dir) cos(dir)];

1. 由于落点可能不准确，可采用夹逼定理的思想，将posCheck往大了估计，并且将posCheck往小的估计，若都满足要求，则说明这个点满足要求

6. 如何判断Xnew在Xgoal附近？

1. 计算两点间的距离，在一定范围内则说明近似到达目标点，退出循环

1. 无论是否找到都要将Xnew与Xnear之间的路径画上

7. 如何反向查询

1. 根据indPrev反查

Matlab代码实现

RRT.m

%***************************************
%Author: Chaoqun Wang
%Date: 2019-10-15
%***************************************
%% 流程初始化
clc
clear all; close all;
x_I=1; y_I=1; % 设置初始点
x_G=700; y_G=700; % 设置目标点（可尝试修改终点）
Thr=50; % 设置目标点阈值，表示当前节点到达目标点方圆50内，就算作到达目标点
Delta= 30; % 设置扩展步长
x_goal = [x_G y_G];
%% 建树初始化
T.v(1).x = x_I; % T是我们要做的树，v是节点，这里先把起始点加入到T里面来
T.v(1).y = y_I;
T.v(1).xPrev = x_I; % 起始节点的父节点仍然是其本身
T.v(1).yPrev = y_I;
T.v(1).dist=0; % 从父节点到该节点的距离，这里可取欧氏距离
T.v(1).indPrev = 0; %
%% 开始构建树，作业部分
% getDistance
getDist = @(x1,y1,x2,y2) sqrt((x1-x2)^2+(y1-y2)^2);
% end of getDistance
figure(1);
ImpRgb=imread('newmap.png');
Imp=rgb2gray(ImpRgb);
imshow(Imp)
xL=size(Imp,2);%地图x轴长度
yL=size(Imp,1);%地图y轴长度
hold on
plot(x_I, y_I, 'ro', 'MarkerSize',10, 'MarkerFaceColor','r');
plot(x_G, y_G, 'go', 'MarkerSize',10, 'MarkerFaceColor','g');% 绘制起点和目标点
count=1;
bFind = false;
tic
for iter = 1:3000
%Step 1: 在地图中随机采样一个点x_rand，这个点为图中的坐标
%提示：用（x_rand(1),x_rand(2)）表示环境中采样点的坐标
x_rand=[0 + (xL - 0) * rand(1,1), 0 + (yL - 0) * rand(1,1) ];
%Step 2: 遍历树，从树中找到最近邻近点x_near
%提示：x_near已经在树T里
distArr = arrayfun(@(dot) getDist(x_rand(1),x_rand(2),dot.x,dot.y),T.v);
[~,index] = min(distArr);
x_near=[T.v(index).x,T.v(index).y];
%Step 3: 扩展得到x_new节点
%提示：注意使用扩展步长Delta
theta = atan2(x_rand(2)-x_near(2),x_rand(1)-x_near(1));
x_new=[x_near(1)+Delta*cos(theta), x_near(2) + Delta*sin(theta)];
%检查节点是否是collision-free
if ~collisionChecking(x_near,x_new,Imp)
continue;
end
count=count+1;
%Step 4: 将x_new插入树T
%提示：新节点x_new的父节点是x_near
T.v(count).x = x_new(1); % T是我们要做的树，v是节点
T.v(count).y = x_new(2);
T.v(count).xPrev = x_near(1);
T.v(count).yPrev = x_near(2);
T.v(count).dist=Delta; % 距离不变吗？这个步长其实比较短，可以接受
T.v(count).indPrev = index; % 为什么要标记呢？这样得到parent在T中的位置，方便回查
%Step 5:检查是否到达目标点附近
%提示：注意使用目标点阈值Thr，若当前节点和终点的欧式距离小于Thr，则跳出当前for循环
if getDist(x_new(1),x_new(2),x_G,y_G) < Thr
plot([x_near(1),x_new(1)],[x_near(2),x_new(2)],'r');
hold on;
bFind = true;
break;
end
%Step 6:将x_near和x_new之间的路径画出来
%提示 1：使用plot绘制，因为要多次在同一张图上绘制线段，所以每次使用plot后需要接上hold on命令
%提示 2：在判断终点条件弹出for循环前，记得把x_near和x_new之间的路径画出来
plot([x_near(1),x_new(1)],[x_near(2),x_new(2)],'r');
hold on;
pause(0.05); %暂停一会，使得RRT扩展过程容易观察
end
toc
%% 路径已经找到，反向查询
if bFind
path.pos(1).x = x_G; path.pos(1).y = y_G;
path.pos(2).x = T.v(end).x; path.pos(2).y = T.v(end).y;
pathIndex = T.v(end).indPrev; % 终点加入路径
j=0;
while 1
path.pos(j+3).x = T.v(pathIndex).x;
path.pos(j+3).y = T.v(pathIndex).y;
pathIndex = T.v(pathIndex).indPrev;
if pathIndex == 1
break
end
j=j+1;
end % 沿终点回溯到起点
path.pos(end+1).x = x_I; path.pos(end).y = y_I; % 起点加入路径
for j = 2:length(path.pos)
plot([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y], 'b', 'Linewidth', 3);
end
else
disp('Error, no path found!');
end

collosionChecking.m

function feasible=collisionChecking(startPose,goalPose,map)
feasible=true;
dir=atan2(goalPose(1)-startPose(1),goalPose(2)-startPose(2));
for r=0:0.5:sqrt(sum((startPose-goalPose).^2))% 求两点平方差的小技巧
posCheck = startPose + r.*[sin(dir) cos(dir)];
% 估值小技巧：若往大的估计成立，往小的估计也成立，那么夹在中间的必定成立，用于判断点的位置
if ~(feasiblePoint(ceil(posCheck),map) && feasiblePoint(floor(posCheck),map) && ...% 换行小技巧
feasiblePoint([ceil(posCheck(1)) floor(posCheck(2))],map) && feasiblePoint([floor(posCheck(1)) ceil(posCheck(2))],map))
feasible=false;break;
end
if ~feasiblePoint([floor(goalPose(1)),ceil(goalPose(2))],map), feasible=false; end
end
function feasible=feasiblePoint(point,map)
feasible=true;
if ~(point(1)>=1 && point(1)<=size(map,2) && ... % x in map
point(2)>=1 && point(2)<=size(map,1) && ... % y in map
map(point(2),point(1))==255) % x,y is Free
feasible=false;
end

结果

改进点

1. 碰撞检测

1. 需要对每个点判断，十分浪费时间

2. 最近节点搜索

1. 对每个点都计算距离，资源消耗严重，可采用kd-tree优化

3. 轨迹都是线段，不利于运动学约束下的机器人移动

4. 采样点为全局均匀采样，没有目的性

参考

• 代码来源:https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots

• 相关知识来源:深蓝学院<<移动机器人运动规划>>

• http://msl.cs.uiuc.edu/rrt/

• https://arxiv.org/pdf/1308.0189.pdf

• http://pracsyslab.org/sst_software

• http://homepages.laas.fr/jcortes/Papers/jaillet_aaaiWS08.pdf

• https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=6606360

• https://robotics.cs.unc.edu/publications/Ichnowski2012_IROS.pdf

• https://github.com/zychaoqun