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

printf(">_<!");

已于 2023-03-17 20:43:09 修改

阅读量1.7k

点赞数 6

分类专栏：路径规划学习总结文章标签：动态规划图搜索算法 matlab Powered by 金山文档

于 2023-03-17 20:41:41 首次发布

本文链接：https://blog.csdn.net/NPC_Hacket/article/details/129627676

版权

路径规划学习总结专栏收录该内容

1 篇文章 0 订阅

订阅专栏

任务说明

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

算法流程

流程图

流程描述

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

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

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

将Xnew插入到树中

判断Xnew是否在树附近

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

否，返回步骤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` 的每个元素四舍五入到大于或等于该元素的最接近整数

算法实现

初始化

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

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

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

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

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

如何延长x_near得到x_new?

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

如何检查collision free？

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

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

如何判断Xnew在Xgoal附近？

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

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

如何反向查询

根据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