RRT是一种多维空间中有效率的规划方法。它以一个初始点作为根节点,通过随机采样增加叶子节点的方式,生成一个随机扩展树,当随机树中的叶子节点包含了目标点或进入了目标区域,便可以在随机树中找到一条由从初始点到目标点的路径。RRT方法是概率完备且不最优的。

function BuildRRT(qinit, K, Δq)T.init(qinit)for k = 1 to Kqrand = Sample()  -- chooses a random configurationqnearest = Nearest(T, qrand) -- selects the node in the RRT tree that is closest to qrandif  Distance(qnearest, qgoal) < Threshold thenreturn trueqnew = Extend(qnearest, qrand, Δq)  -- moving from qnearest an incremental distance in the direction of qrandif qnew ≠ NULL thenT.AddNode(qnew)return falsefunction Sample() -- Alternatively,one could replace Sample with SampleFree(by using a collision detection algorithm to reject samples in C_obstaclep = Random(0, 1.0)if 0 < p < Prob thenreturn qgoalelseif Prob < p < 1.0 thenreturn RandomNode()

RRT 伪代码

  初始化时随机树T只包含一个节点:根节点qinit。首先Sample函数从状态空间中随机选择一个采样点qrand(4行);然后Nearest函数从随机树中选择一个距离qrand最近的节点qnearest(5行);最后Extend函数通过从qnearest向qrand扩展一段距离,得到一个新的节点qnew(8行)。如果qnew与障碍物发生碰撞,则Extend函数返回空,放弃这次生长,否则将qnew加入到随机树中。重复上述步骤直到qnearest和目标点qgaol距离小于一个阈值,则代表随机树到达了目标点,算法返回成功(6~7行)。为了使算法可控,可以设定运行时间上限或搜索次数上限(3行)。如果在限制次数内无法到达目标点,则算法返回失败。
  为了加快随机树到达目标点的速度,简单的改进方法是:在随机树每次的生长过程中,根据随机概率来决定qrand是目标点还是随机点。在Sample函数中设定参数Prob,每次得到一个0到1.0的随机值p,当0<p<Prob的时候,随机树朝目标点生长行;当Prob<p<1.0时,随机树朝一个随机方向生长。

  上述算法的效果是随机采样点会“拉着”树向外生长,这样能更快、更有效地探索空间(The effect is that the nearly uniformly distributed samples “pull” the tree toward them, causing the tree to rapidly explore C-Space)。随机探索也讲究策略,如果我们从树中随机取一个点,然后向着随机的方向生长,那么结果是什么样的呢?见下图(Left: A tree generated by applying a uniformly-distributed random motion from a randomly chosen tree node does not explore very far. Right: A tree generated by the RRT algorithm using samples drawn randomly from a uniform distribution. Both trees have 2000 nodes )。可以看到,同样是随机树,但是这棵树并没很好地探索空间。

下面是具体的代码

%% RRT parameters
map=im2bw(imread('map.bmp')); % input map read from a bmp file. for new maps write the file name here
source=[490 490]; % source position in Y, X format
goal=[10 900]; % goal position in Y, X format
stepsize = 20;  % size of each step of the RRT
threshold = 20; % nodes closer than this threshold are taken as almost the same
maxFailedAttempts = 10000;
display = true; % display of RRTif ~feasiblePoint(source,map), error('source lies on an obstacle or outside map'); end
if ~feasiblePoint(goal,map), error('goal lies on an obstacle or outside map'); end
if display,imshow(map);rectangle('position',[1 1 size(map)-1],'edgecolor','k'); endtic;  % tic-toc: Functions for Elapsed TimeRRTree = double([source -1]); % RRT rooted at the source, representation node and parent index
failedAttempts = 0;
counter = 0;
pathFound = false;while failedAttempts <= maxFailedAttempts  % loop to grow RRTs
%% chooses a random configurationif rand < 0.5sample = rand(1,2) .* size(map);   % random sampleelsesample = goal; % sample taken as goal to bias tree generation to goalend%% selects the node in the RRT tree that is closest to qrand[A, I] = min( distanceCost(RRTree(:,1:2),sample) ,[],1); % find the minimum value of each columnclosestNode = RRTree(I(1),1:2);%% moving from qnearest an incremental distance in the direction of qrandtheta = atan2(sample(1)-closestNode(1),sample(2)-closestNode(2));  % direction to extend sample to produce new nodenewPoint = double(int32(closestNode(1:2) + stepsize * [sin(theta)  cos(theta)]));if ~checkPath(closestNode(1:2), newPoint, map) % if extension of closest node in tree to the new point is feasiblefailedAttempts = failedAttempts + 1;continue;endif distanceCost(newPoint,goal) < threshold, pathFound = true; break; end % goal reached[A, I2] = min( distanceCost(RRTree(:,1:2),newPoint) ,[],1); % check if new node is not already pre-existing in the treeif distanceCost(newPoint,RRTree(I2(1),1:2)) < threshold, failedAttempts = failedAttempts + 1; continue; end RRTree = [RRTree; newPoint I(1)]; % add nodefailedAttempts = 0;if display, line([closestNode(2);newPoint(2)],[closestNode(1);newPoint(1)]);counter = counter + 1; M(counter) = getframe; end % Capture movie frame
end% getframe returns a movie frame, which is a structure having two fields
if display && pathFound, line([closestNode(2);goal(2)],[closestNode(1);goal(1)]); counter = counter+1;M(counter) = getframe; endif display, disp('click/press any key'); waitforbuttonpress; end
if ~pathFound, error('no path found. maximum attempts reached'); end%% retrieve path from parent information
path = [goal];
prev = I(1);
while prev > 0path = [RRTree(prev,1:2); path];prev = RRTree(prev,3);
endpathLength = 0;
for i=1:length(path)-1, pathLength = pathLength + distanceCost(path(i,1:2),path(i+1,1:2)); end % calculate path length
fprintf('processing time=%d \nPath Length=%d \n\n', toc, pathLength);
imshow(map);rectangle('position',[1 1 size(map)-1],'edgecolor','r');
line(path(:,2),path(:,1));

rrt_main.m

%% distanceCost.m
function h=distanceCost(a,b)h = sqrt(sum((a-b).^2, 2));

distanceCost.m

%% checkPath.m
function feasible=checkPath(n,newPos,map)
feasible=true;
dir=atan2(newPos(1)-n(1),newPos(2)-n(2));
for r=0:0.5:sqrt(sum((n-newPos).^2))posCheck=n+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;endif ~feasiblePoint(newPos,map), feasible=false; end
end

checkPath.m

%% feasiblePoint.m
function feasible=feasiblePoint(point,map)
feasible=true;
% check if collission-free spot and inside maps
if ~(point(1)>=1 && point(1)<=size(map,1) && point(2)>=1 && point(2)<=size(map,2) && map(point(1),point(2))==1)feasible=false;
end

feasiblePoint.m

结果如图

[matlab] 7.快速搜索随机树(RRT---Rapidly-exploring Random Trees) 路径规划相关推荐

  1. 快速搜索随机树(RRT---Rapidly-exploring Random Trees)入门及在Matlab中演示

    转载:http://blog.csdn.net/gpeng832/article/details/71249198?locationNum=1&fps=1   快速搜索随机树(RRT -Rap ...

  2. 快速搜索随机树算法(RRT)和A*算法

    1.快速搜索随机树算法 1.1 简述快速搜索随机树算法 快速生长随机树算法需要知道整个环境信息,通过随机采样的方法并通过一系列判断来找到起点和终点之间的一条可行路线.这种方法在概率上是可以找到一条可行 ...

  3. 左上角到右下角的路径 oj_【机器人路径规划】快速扩展随机树(RRT)算法

    内容无关:最近的课题内容和机器人运动规划方法有关,我把学习的内容整理成为工具箱上传到了我的github仓库,稍后将会发一篇说明介绍使用方法. XM522706601/robotics_tutorial ...

  4. 【运动规划】RRT快速搜索随机树 Rapidly Exploring Random Tree

    Randomized Kinodynamic Planning Steven M. LaValle James J. Kuffner, Jr. 1. Exploring the State Space ...

  5. rapidly exploring random tree(快速搜索随机树)路径规划

    直接上程序 %% 流程初始化 clear all; close all; pic_num=1;%制作gif所需参数 x_I=1; y_I=1; % 设置初始点 x_G=600; y_G=600; % ...

  6. RRT(快速搜索随机树)

    RRT 理论: 抛X_rand,先得X_near,再得X_new 以上是算法流程图. close all; clear all;% params for function [vertices, edg ...

  7. 基于快速探索随机树RRT和Dubins路径规划与避障研究(Matlab代码实现)

  8. RRT(Rapidly-Exploring Random Trees)算法详解及python实现

    RRT(Rapidly-Exploring Random Trees)算法详解及python实现 前言 一.原理 二.伪代码 三.代码详解 总结 前言 快速探索随机树(RRT):作为一种随机数据结构, ...

  9. rrt算法流程图_RRT算法移动机器人路径规划(快速扩展随机树).pdf

    ( ) 第 34 卷 第 5期 南京理工大学学报 自然科学版 Vo l. 34 No. 5 20 10年 10 月 Journal of N anj ing Un iversity of Scienc ...

最新文章

  1. zbg是什么意思_zbg的含义,zbg是什么的缩写,zbg的词语,zbg代表的意思
  2. 以后版本网卡命名规则
  3. 解决 Flex navigateToURL 中文乱码问题
  4. Spring Boot笔记-对dto数据传输对象及物联网公司主创建主键的认识
  5. 关于在vue中结合数组方法的this的指向问题
  6. node.js + express 初体验【hello world】
  7. 【华为云技术分享】GeminiDB for Cassandra 流功能介绍
  8. C++ lower_bound 与 upper_bound 函数
  9. 一个关于从1到100的加法算法
  10. CPU读/写一个存储单元
  11. shell基础之多功能nginx(安装、重启、停止等)
  12. 侵入式框架和非侵入式框架的区别
  13. angular中的$q服务
  14. 基于matlab的图像复原,MATLAB在图像复原中的应用
  15. 高数_第6章无穷级数_函数的幂级数展开式_马克劳林级数
  16. python-opencv图像处理之车牌识别+区域划分+车牌提取+保存图片(2)
  17. 【ArcGIS风暴】中国756个气象台站分布Shapefile数据下载
  18. wps office 2013 利用wps文字制作一张漂亮的座位表
  19. 【直击】腾讯SaaS加速器首期复试现场:122位CEO争夺30席入场券,激烈!
  20. 5_02_GLib库入门与实践_日志和调试

热门文章

  1. 卷积核大小对网络参数和计算量的影响
  2. batch—size的大小对loss收敛的影响
  3. activiti适配人大金仓数据库修改方法
  4. 2022-2028全球急救和创伤单架行业调研及趋势分析报告
  5. 灰帽python之旅_灰帽 Python之旅10
  6. 第三代计算机的操作系统,操作系统基础
  7. linux 文件系统被占用,解决umount的时候文件系统被占用的两个命令 fuser 和 lsof
  8. iBase4J-JAVA分布式开源框架
  9. Matlab学习手记——制作GIF动图
  10. 关于WIN10“网络重置”后连不上网的解决方法