关键词:
RRT快速搜索随机树英文全称Rapid-exploration Random Tree,和PRM类似,也是一种路径规划算法。
和PRM类似,算法也需要随机撒点,不过不同的是,该算法不是全局随机撒点,而是一次撒一个点,然后判断当前搜索树与随机点距离,然后找到搜索树距离随机点最近的节点,向该随机点方向扩展。这里随机点有一定的概率是终点,所以搜索树最终是能够到达终点的。
算法流程如下:
1. 首先确定地图与起始结束点位置,设置搜索树,这里定义了一个随机点列表和一个随机点索引前驱列表代表搜索树。
2. 随机撒一个点,该点有可能是最终点,也有可能是全局中的一个随机点,设为nextp。
3. 找到搜索树中距离nextp最近的节点,从该节点向nextp方向扩展step距离,生成新的路径。
4. 判断新生成的路径是否通过障碍物或者该路径已经被搜索过,如果都没有则该路径加入到搜索树中,否则重新生成随机点。
5. 不断循环直到搜索树最终节点距离终点小于一定阈值,搜索结束,根据前驱列表画出搜索路径。
matlab代码如下:
main.m:
clear all; close all; clc; img = imread(‘map.png‘); %空间地图 imshow(img); hold on; [h,w]=size(img); p=ginput(); %选取起始与结束位置 plot(p(:,1),p(:,2),‘r.‘); pc = p(1,:); %随机节点列表 step = 20; %随机扩展步长 parent = 1; %所有节点前驱,初始节点前驱为自己 while norm(pc(end,:)-p(2,:))>step %搜索到距离结束节点一定距离停止 if rand()<0.3 %按30%概率随机搜索,70%概率朝着结束位置搜索 nextp = [rand()*h rand()*w]; else nextp = p(2,:); end diff = repmat(nextp,length(pc(:,1)),1)-pc; %计算节点树与待搜索节点距离 [~,ind] = min(sqrt(diff(:,1).^2+diff(:,2).^2)); %找到距离带搜索节点最小的节点树节点 direct = atan2(nextp(1)-pc(ind,1),nextp(2)-pc(ind,2)); sin_dir = sin(direct); cos_dir = cos(direct); newp = pc(ind,:) + step*[sin_dir cos_dir]; %向着待搜索节点方向扩展节点树 isobs = check_obs(img,newp,pc(ind,:)); %判断该路径是否有障碍物 if isobs==1 %有障碍物重新搜索 continue; end diff = repmat(newp,length(pc(:,1)),1)-pc; %判断该路径是否已搜索过,如果已搜索过,则重新搜索 if min(sqrt(diff(:,1).^2+diff(:,2).^2))<sqrt(step) continue; end pc=[pc;newp]; %将新节点加入节点树 parent = [parent;ind]; %设置新节点的前驱 line([pc(ind,1) pc(parent(ind),1)],[pc(ind,2) pc(parent(ind),2)]); end line([pc(ind,1) p(2,1)],[pc(ind,2) p(2,2)],‘color‘,‘r‘); ind = length(pc); while ind~=1 ind = parent(ind); %不断搜索当前节点的父节点 line([pc(ind,1) pc(parent(ind),1)],[pc(ind,2) pc(parent(ind),2)],‘color‘,‘r‘); end
check_obs.m:
function isobs = check_obs(img,p1,p2) [h w]=size(img); d = norm(p1-p2); direct = atan2(p1(1)-p2(1),p1(2)-p2(2)); sin_dir = sin(direct); cos_dir = cos(direct); for r=0:d p = floor(p2 + r*[sin_dir cos_dir]); y = p(2); x = p(1); if y>=1 && y<=h && x>=1 && x<=w if img(y,x) ==0 isobs = 1; return; end end end isobs = 0; end
结果如下:
原图:
算法结果:
rrt路径规划算法
...,不适合解决多自由度机器人在复杂环境中的规划。基于快速扩展随机树(RRT/ rapidlyexplo 查看详情
路径规划基于matlabrrt算法避障路径规划含matlab源码1220期(代码片段)
...碍物,避免路径陷入局部极小值,收敛速度快。本文通过matlab实现RRT算法,解决二维平面的路径规划问题。2地图为了方便算法的实现,使 查看详情
ptal2-004这是二叉搜索树吗?-判断是否是对一棵二叉搜索树或其镜像进行前序遍历的结果团体程序设计天梯赛-练习集(代码片段)
L2-004 这是二叉搜索树吗? (25 分) 一棵二叉搜索树可被递归地定义为具有下列性质的二叉树:对于任一结点,其左子树中所有结点的键值小于该结点的键值;其右子树中所有结点的键值大于等于该结点的键值;其... 查看详情
路径规划基于matlabrrt算法机器人最短路径规划含matlab源码1391期(代码片段)
...碍物,避免路径陷入局部极小值,收敛速度快。本文通过matlab实现RRT算法,解决二维平面的路径规划问题。2地图为了方便算法的实现,使 查看详情
prm路径规划算法
...法:如人工势场法、单元分解法、随机路标图(PRM)法、快速搜索树(RRT)法等。传统的人工势场、单元分解法需要对空间中的障碍物进行精确建模,当环境中的障碍物较为复杂时,将导致规划算法计算量较大。基于随机采样技... 查看详情
[rrt_algorithm]rrt算法以及伪代码
...rrt算法是基于采样的路径规划算法,与A*算法的基于搜索是不同的。算法实现过程中遇到了一些困难,类似距离计算公式写错等低级错误,也有路径探索方向写错等算法理解错误,这些我都将在后续文章中提出。... 查看详情
java示例代码_使用数组(Java)平衡现有的随机二叉搜索树(BST)
java示例代码_使用数组(Java)平衡现有的随机二叉搜索树(BST) 查看详情
pat天梯赛练习题l3-010.是否完全二叉搜索树(完全二叉树的判断)
L3-010.是否完全二叉搜索树时间限制400ms 内存限制 65536kB 代码长度限制 8000B 判题程序 Standard 作者 陈越 将一系列给定数字顺序插入一个初始为空的二叉搜索树(定义为左子树键值大,右子树键值... 查看详情
[数据结构]快速排序+折半搜索(代码片段)
1数据结构的练习与巩固2/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////3//折半搜索4int 查看详情
C中的二叉搜索树导致堆损坏错误
...】:所以我是一名Python程序员,我正在尝试自学C。就像练习一样,我一直在尝试用C实现一个简单的二叉搜索树。我从来不需要处理内存分配或之前的指针,它导致了很多错误。我的程序一直给我退出代码-1073740940(0xC0000374),我... 查看详情
leetcode练习(python):动态规划类:第95题:不同的二叉搜索树ii:给定一个整数n,生成所有由1...n为节点所组成的二叉搜索树。
题目:不同的二叉搜索树II:给定一个整数n,生成所有由1...n为节点所组成的二叉搜索树。思路:遍历每一个节点,并且得到每个节点的左右子树,然后获得每个子树的样子就可以得出来了。自己想了半天没法实现,参考了一下... 查看详情
leetcode练习(python):动态规划类:第95题:不同的二叉搜索树ii:给定一个整数n,生成所有由1...n为节点所组成的二叉搜索树。
题目:不同的二叉搜索树II:给定一个整数n,生成所有由1...n为节点所组成的二叉搜索树。思路:遍历每一个节点,并且得到每个节点的左右子树,然后获得每个子树的样子就可以得出来了。自己想了半天没法实现,参考了一下... 查看详情
练习题(21.7.26)(代码片段)
二叉搜索树题意:给你要判断的个数n,告诉你初始序列,根据这个序列可以构造出一棵二叉搜索树。然后给你n个序列,每个序列格式跟第一个序列一样,请判断这两个序列是否能组成同一棵二叉搜索树。这个... 查看详情
单目标优化求解基于matlab秃鹰算法(bes)求解最优目标问题含matlab源码1546期(代码片段)
一、秃鹰算法(BES)简介1秃鹰搜索优化算法秃鹰遍布于北美洲地区,飞行中视力敏锐,观察能力优秀.以捕食鲑鱼为例,秃鹰首先会基于个体和种群到鲑鱼的浓度来选择搜索空间,朝一个特定区域飞行;其次在选定搜索空间内搜... 查看详情
直通bat算法精讲附程序源码
课程内容第1章免费试看2视频|2练习字符串和二叉树问题免费试看1.1二叉树打印免费1.2二叉树打印练习题免费1.3字符串免费1.4两串旋转练习题免费第2章排序4视频|16练习详细介绍常见的排序算法过程,以及各个排序算法稳定性、时... 查看详情
数学建模基于matlabgui随机节点的生成树含matlab源码1919期(代码片段)
...1:完整代码已上传我的资源:【数学建模】基于matlabGUI随机节点的生成树【含Matlab源码1919期】获取代码方式2:通过订阅紫极神光博客付费专栏,凭支付凭证,私信博主,可获得此代码。备注:订阅紫... 查看详情
#yyds干货盘点#leetcode腾讯精选练习50题:二叉搜索树中第k小的元素
题目:给定一个二叉搜索树的根节点root,和一个整数k,请你设计一个算法查找其中第 k 个最小元素(从1开始计数)。 示例1:输入:root=[3,1,4,null,2],k=1输出:1示例2:输入:root=[5,3,6,2,4,null,null,1],k=3输出:3代码实现:... 查看详情
天梯赛练习l3-010是否完全二叉搜索树(30分)数组建树模拟(代码片段)
题目分析:本题的要求是将n个数依次插入一个空的二叉搜索树(左大右小,且没有重复数字),最后需要输出其层次遍历以及判断是否是完全二叉搜索树,通过观察我们发现,如果这个树是用数组建立的,那么最后输出的时候... 查看详情