从RRT到RRT*,再到Informed RRT*,路径规划算法怎么写

article/2025/10/24 7:49:07

从RRT到RRT*,再到Informed RRT*,路径规划算法怎么写

  • 1、RRT算法
    • 1.1 假设
    • 1.2 RRT算法步骤与实现
    • 1.3 伪代码
  • 2、RRT*算法
  • 3、Informed RRT*算法

做个正直的人

RRT中文名字是“快速搜索随机树”(Rapidly-exploring Random Tree),是一种比较常用的基于采样的静态路径规划算法。

我们可以这么理解:小明住在北京,小红住在南京,有一天小红给小明发了一条短信,短信上说只要小明从北京来南京见小红,小红就做小明的女朋友。单身青年小明一看给激动坏了,当即收拾东西要来南京。

1、RRT算法

1.1 假设

为了说明RRT算法,我们作出如下假设:

  • 小明不知道从北京直达南京的路线;

  • 在每一个城市,只能够获得到达相邻城市的路线,而不能到达不相邻的城市;

  • 小明看不懂地图,只有一个列出了中国所有城市的城市清单。

1.2 RRT算法步骤与实现

小明的舍友甲为小明提出了一种找到从北京到达南京的算法——RRT算法。
小明只能拿着这个清单,重复如下过程:

    (1)随机选择一个城市,(2)看看这个城市是不是可达,(3)离南京(目标位置)多远,如果到达目标位置就终止算法(4)在已经选择过得城市中哪一个离它最近,就把那个城市设置为他的父节点。

不断的重复这个过程,这一个随机树就会不断的生长,直到到达目标位置。从下图可以看出经过许许多多次随机选择之后,目标城市就会变成这个随机树的叶节点,也就是说,小明就能找到一条从北京到南京的可行路径(仅仅是可行而已,很大概率上不会是最优)。
RRT算法说明示意图
比如上面这幅图就是小明使用RRT算法找到的一条从北京到南京的路线。

1.3 伪代码

我们可以把RRT算法用伪代码的形式写出来,我认为伪代码用英语描述能够更加准确精炼的表达算法的步骤和精髓,所以我用英语来写伪代码:

//RRT算法伪代码——作者黄唐,2021.4.9于南京//initialization
Obtain map, 0 stands for free space and 1 obstacle.
Set start and goal in this map .
Set step_size which means how long we will search from the closest pos to the 
random pos obtained from sampling.(Why we need to set a step_size? Because the sample may be far away from our tree and we don't want to produce a very long edge.)
Set max iteration that means how many times we crop our tree.
Set current iteration as 0.
Set a flag which flags whether we have reached the goal.
Set goal_radius, if the distance from new_point to the goal is less than. 
goal_radius we think we have reached the goal.
If we don't set this goal_radius we may never reach the goal because the 
probability that we sample the goal is 0.
//make plan
We need a vector to memorize the position the we have detected
Push the start into the vector and set its parent as -1 and its index 0 which 
means it is the root of our tree while(we haven't reached the goal and the max iteration){Sample in the map and get a random pos as random_pointGet the closest pos as closest_point in the vector(tree) whose distance to random_point is min among all members in the vectorMove from the closest_point to the random_point and produce a new point as new_point in this edge whose distance to closest_point is step_size if (the edge from closest_point to new_point is safe which means there is no collision with any obstacle){Push the new_point into the vector and set its parent as closest_pointcurrent iteration ++if (the distance from new_point to goal < goal_radius){if (the edge from new_point to goal is safe){flag =trueSet new_point as parent of the goal}}//RRT*算法的rewrite和relink写在这里哟}else{Abandon this sample and continue}
} 
return the index of new_point as the parent of the goal//build plan 
if (the index of parent of the goal>=0){push the goal at the front of the path current_parent =the index of parent of the goalwhile(the index of current_parent>0){Push the current_parent at the front of the pathcurrent_parent= index of parent of current_parent}Push start at the front of the pathreturn path
}
else {We can't obtain a path from start to goalreturn -1
}

懒得看英语的可以直接看中文的伪代码:

//初始化
拿到一幅地图,0表示自由移动的空间,1表示障碍物
设定初始位置start和目标位置goal
设定生长步长step_size,为什么要设置这么个东西呢?比如说采样点离我们现有的树实在是太远了,直接把这
一个点加入到树中就会产生一条特别长的边,我们可不想有这么长的边
设定一个标志flag,如果我们到达了目标位置就设置它为true,我们就终止循环,不再继续搜索路径
设置一个变量goal_radius来形成一个以goal为圆心,goal_radius为半径的圆(三维空间就是球,更高维就
是超球),因为如果我们不设置这个东西,我们的算法可能永远也不会找到到达目标的路径,因为在平面上采样到
一个点的概率是0。//makeplan
声明一个向量vector来存储我们的树中当前有哪些节点,以及这些节点的位置、索引和父节点
把起始位置压入vector中作为RRT的根节点
把目标位置的父节点索引parent_of_goal设置为-1
while(flag!=true && 未达到最大迭代次数){随机采样一个点作为random_point遍历RRT(也就是vector)找到距离采样点最近的那个节点,记为closest_point从closest_point向random_point搜索step_size距离,将这一点记为new_point检查从closest_point到new_point这一段路径是不是安全,也就是会不会哈障碍物发生碰撞if(不会发生碰撞){将这一个新节点new_point压入到RRT中,并将他的父节点设置为closest_point迭代次数++检查这一个新节点是不是位于以goal为圆心的圆内if(在圆内){检查new_point到goal是不是安全if(安全){设置flag为true,表示我们到达目标区域了设置parent_of_goal为new_point的索引}else{do nothing}}//RRT*的rewrite和relink写在这里哟}else{不安全,舍弃这一个点,从新采样}
}
return parent_of_goal//生成路径
if(parent_of_goal >0){从goal不断的向父节点回溯,直到回到根节点,也就是start,这一条回溯路径就是RRT算法找到的路径
}
else{没有找到路径,失败
}

2、RRT*算法

小明的舍友乙指出,甲的RRT算法找到的路径并不够优秀,舍友乙对舍友甲的算法作出了改进,提出了一种RRT*算法。

下面讲讲RRT*算法。加了个*,很明显这是对RRT的一种改进,就像A算法和A*算法一样。从前面那副图中我们可以看出来RRT只能找出一条可行路径,并不能保证找到一条最优路径,换句话说,RRT会让小明走很多弯路,这会让小明更晚收获他的爱情。所以乙提出了一种RRT*算法。RRT*算法在RRT算法的基础上增加了两步:rewriterandom relink。也就是重写和随机重连。

先解释一下什么是重写,什么是重连。

重写就是,在新节点new_point加入到树中之后,重新为它选择父节点,好让它到起始点的路径长度(代价)更小。

随机重连是在重写完成之后,对新节点new_point附近一定范围内的节点进行重连。重连就是,检查一下如果把new_point附近的这些节点的父节点设置为new_point,这些节点的代价会不会减小。如果能够减小,就把这些节点的父节点更改为new_point;否则,就不更改。
(论文的作者已经证明RRT*算法是一种渐进最优的算法,也就是说当时间趋于无穷大的时候,这个算法就能够找到最优的路径。)

由于RRT算法不考虑距离,RRT*算法考虑每一个节点到出发点的距离,为此每一个节点会增加一个属性:distance_to_start,即到出发点的距离。相应的在为每一个节点选择父节点的时候,新节点的距离等于父节点的距离加上父节点到子节点的直线距离,即:
c h i l d . d i s t a n c e _ t o _ s t a r t = p a r e n t . d i s t a n c e _ t o _ s t a r t + g e t D i s t a n c e ( p a r e n t , c h i l d ) child.distance\_to\_start=parent.distance\_to\_start+getDistance(parent,child) child.distance_to_start=parent.distance_to_start+getDistance(parent,child)
原本RRT的代码不用更改,只需要把rewriterandom relink 加进去就可以。
rewrite重写的伪代码如下所示:

遍历整个树,
获得到新节点new_point的距离小于一定阈值(比如1.5倍的步长,也就是1.5*step_size)的所有节点
将这些节点加入到一个名为candidate_parent_of_newpoint的列表中,
为了方便,这些节点的distance不再用来存储到出发点的距离,而是用来存储如果把该节点设置为newpointt
的父节点的话,newpoint到出发点的距离。
找到candidate_parent_of_newpoint列表中具有最小distance的那个节点,返回他的索引index,
将新节点newpoint的父节点设置为index。

random relink的伪代码如下所示:

遍历整个列表,对每一个节点执行如下动作{if(该节点到newpoint的距离小于一定的阈值,比如1.6倍的步长,也就是1.6*step_size){if(该节点现在的distance>把该节点的父节点更新为newpoint之后的distance){把该节点的父节点设置为newpoint,并更新该节点的distance值更新以该节点为根节点的子树中的每个节点的distance。我用队列结构实现了一个,但是算法效率很低,正在想办法改进。}}else{do nothing}
}

3、Informed RRT*算法

使用了这个RRT*算法之后,小明发现找到的路径明显变得优秀多了。但是室友丙这时候站出来说话了,他说,RRT*算法是找到了一条更优的路径,但是它的计算量太大了,导致算法的效率比较低,我给改进了一下。舍友丙提出了Informed RRT*算法。

Informed RRT*顾名思义,是加入了一些已经informed的信息。实际上informed RRT的思路非常简单,它仅仅是对RRT和RRT*的采样函数做了一些限制。在没有搜索到任何一条可达路径之前,informed RRT*算法就是RRT*算法,在找到了一条可达路径之后,informed 就不再采用原来的采样函数,而是采用一个椭圆采样函数。
在这里插入图片描述
我们假定上图中的蓝色线是一个非常非常标准的椭圆。
假设目前已经找到一个可达的路径:北京——石家庄——济南——徐州——南京。但是这时候,informed RRT*算法并不会终止,而是会在这一条路径的基础上进行优化。

informed RRT*的做法是,在找到一条可达路径之后,把这一条路径的长度作为cMax,把出发点和目标点之间的距离作为cMin。这样做的目的就是,构造一个椭圆出来,把出发点和目标点作为椭圆的两个焦点F1和F2,这一条可达路径的长度做作为2a。因为我们知道,椭圆上的点P满足如下关系:
∣ P F 1 ∣ + ∣ P F 2 ∣ = 2 a |PF1|+|PF2|=2a PF1+PF2=2a
此后我们就只在这一个椭圆内部采样,而不会再去椭圆外部采样。因为很明显,我已经找到了一个可行路径,比如上图中的路径,那么这个时候,我再去采样西安或者兰州,并不会对我现有路径的优化产生什么作用,除了增大计算量以外。该算法每发现一个更小的cMax就会更新一次cMax,直到cMax满足一定的条件或者达到最大迭代次数才会终止。比如当 c M a x < 1.2 c M i n cMax<1.2cMin cMax<1.2cMin 时终止算法。
乍一看,这个改进似乎没什么了不起的,仅仅是优化了一下采样函数而已嘛。但是这仅仅是在二维空间中进行采样而已,当遇到更高维问题比如六自由度的机械臂的运动规划是在六维空间中,一般的移动机械臂甚至可以达到12个自由度,这种时候,对采样空间进行限制带来的好处是巨大的。
就这样,小明在三位热心舍友的帮助下成功的找打了一条非常不错的去南京的路线,不知道小明会不会如愿以偿呀?
关于如何在该椭圆内部采样,其实可以用坐标变换的方法,在单位圆内采样是很容易实现的,然后把该采样点通过坐标变换变换到椭圆内就可以了。


http://chatgpt.dhexx.cn/article/W9A8Ydvl.shtml

相关文章

【机器人路径规划算法RRT和RRG】

路径规划算法 RRT路径规划RRG路径规划 RRT路径规划 RRT算法&#xff1a;Rapid-exploration Random Tree 快速搜索随机数算法&#xff0c;是一种在完全已知的环境中通过随机采样扩展搜索的算法。 RRT算法是概率完备的&#xff0c;只要规划时间足够长&#xff0c;确实存在一条路…

ROS移动机器人基于RRT(快速探索随机树)算法 rrt_exploration实现真实机器人自主探索建图

仿真机器人加真实机器人功能包下载链接移动机器人项目组项目-机器学习代码类资源-CSDN下载 博主为了图方便&#xff0c;就直接使用了古月老师的仿真包了&#xff0c;博主先和自己的朋友先在真实的机器人上实现了这个功能&#xff0c;再在仿真上来实现了一下。 也可以先去zhang…

RT-Thread 简介

1.RT-Thread 概述 RT-Thread&#xff0c;全称是Real Time-Thread&#xff0c;顾名思义&#xff0c;它是一个嵌入式实时多线程操作系统&#xff0c; 基本属性之一是支持多任务&#xff0c;允许多个任务同时运行并不意味着处理器在同一时刻真地执行了多个任务。 事实上&#xff…

路径规划 | 随机采样算法:PRM、RRT、RRT-Connect、RRT*

基于图搜索的路径规划算法主要用于低维度空间上的路径规划问题&#xff0c;它在这类问题中往往具有较好的完备性&#xff0c;但是需要对环境进行完整的建模工作&#xff0c;在高维度空间中往往会出现维数灾难。为了解决这些问题&#xff0c;本文将介绍基于随机采样的路径规划算…

基于matlab的RRTRRT*算法实现以及可视化

学习记录-基于采样的路径规划算法 内容来源RRT主要步骤动态效果展示优缺点&#xff1a;自己进行的改进尝试 RRT*主要步骤NearCChooseParentrewire总结及动态效果图 Informed RRT*其他优化RRT的方式总结 内容来源 记录学习深蓝路径规划课程-基于采样的路径规划一节的作业和笔记…

RRT基本概念

原文地址 快速探索随机树&#xff08;RRT&#xff09;是一种通过随机构建空间填充树来有效搜索非凸&#xff0c;高维空间的算法。树是从搜索 空间中随机抽取的样本逐步构建的&#xff0c;并且本质上倾向于朝向大部分未探测区域生长。 RRT由Steven M. LaValle 和James J. Kuf…

SQLSERTVER安装教程

很久没有安装过这个了&#xff0c;今天安装有点生疏了&#xff0c;这里记录一下分享 分为三块块1、下载地址&#xff0c;2、安装图解 &#xff0c;3、安装失败问题 1、sqlserver 2008 r2 百度下载地址链接&#xff1a;下载 cn_sql_server_2008_r2_enterprise_x86 Microsoft…

sqlserver安装目录_SQL Server 2016数据库安装

SQL SERVER 2016较之前的SQL安装有些不同,下面详细介绍如何将SQL SERVER 2016安装到Windows的服务器。 一、第一阶段,SQL安装 1.首先具备SQL SERVER 2016的安装介质。一般可能是下载的为ISO光盘镜像文件。在Windows Server 2016操作系统和Windows 10的系统中可以使用鼠标的右…

Sql Server安装时遇到polybase问题

错误&#xff1a;以域格式&#xff08;域\用户名&#xff09;指定账户。对于本地用户&#xff0c;请采用&#xff08;本地主机\用户名&#xff09;格式。 在安装时选了polybase&#xff0c;需要手动输入账户&#xff0c;如果不需要该服务或没有账户&#xff0c;可以不要勾选po…

SQL Server 安装教程

目录 第一阶段&#xff1a;安装SQL Server向导 第二阶段&#xff1a;安装SQL Server 第三阶段&#xff1a;安装SQL Server管理工具 运行SSM 参考链接 第一阶段&#xff1a;安装SQL Server向导 以下以中文版为例&#xff1a; 中文版官网&#xff1a;https://www.microsoft.com/…

SQL Server 基础操作(一)安装数据库

Windows server 2012 R2系统 安装SQL Server 2008数据库 1.创建虚拟机---安装Windwos server 2012 R2 操作系统 2.安装windows server 2012 R2系统完成后&#xff0c;更换SQL Server 2008 iso镜像 3.安装.NET Framework 3.5&#xff0c;一直点击下一步在.NET Framework 3.5选项…

sqlserver 2016 安装

1、环境介绍 操作系统&#xff1a;windows server 2016 sqlserver版本&#xff1a; sqlserver 2016 下载地址&#xff1a; https://msdn.itellyou.cn/ 2、双击下载下来的镜像&#xff0c;打开setup开始安装 3、选择全新安装 4、选择输入秘钥&#xff0c;下一不 5、接受许可…

SQLserver的安装

SQLserver的安装 一、SQLserver的安装步骤 1.SQLserver的下载&#xff1a;官网下载网址 下载Developer版本即可。 2.运行完成后安装类型选择“基本” &#xff0c;之后选择合适的语言和安装位置。 3.显示“成功完成安装”后&#xff0c;不急于点击完成退出&#xff0c;应点…

SQL Server无法安装问题

SQL Server无法安装问题 一、软件安装“无法使用此产品的安装源,请确认安装源存在并且你可以访问它安装过程中遇到无法访问您试图使用的功能所在的网络位置问题一、软件安装“无法使用此产品的安装源,请确认安装源存在并且你可以访问它 原因:之前版本卸载没有卸载干净(主要…

mysql 2005 安装教程_sql2005 安装教程 图文

SQL2005安装安装步骤 安装Microsoft SQL Server 2005 数据库步骤&#xff1a; 第一步&#xff1a;将Microsoft SQL Server 2000安装光盘放入光驱中&#xff0c;在光驱目录下&#xff0c;点击Setup.exe安装程序开始安装过程&#xff0c; 或使用镜像安装文件。选择“基于X86的操作…

SQLserver2005 安装

解压cs_sql server_2005_ent_x64_dvd.iso镜像文件。打开Servers文件夹找到setup.exe双机点击安装。出现程序兼容助手提示&#xff0c;点击运行程序。 3、用户许可协议&#xff0c;选择我接受&#xff0c;点击下一步。 4、安装必要组件&#xff0c;点击安装。 5、安装必要组件&a…

Elasticsearch插件:elasticsearch-sql安装和使用

使用此插件&#xff0c;您可以使用熟悉的SQL语法查询elasticsearch。您还可以在SQL中使用ES函数。 有两种方法可以使用此插件&#xff1a; 使用其余的api http://localhost:9200/_sql?sqlselect * from indexName limit 10 2. 或者通过浏览器访问 http://localhost:9200/…

sqlserver2012安装教程

前言&#xff1a; 我们实验室开发前端界面一般用.net&#xff0c;然后数据库用微软的Sqlserver,搭配起来做一些系统框架还是很方便的。记得本科的时候安装Sqlserver的时候好像出了点问题&#xff0c;不知道是不是因为先安装了VS&#xff0c;然后这一次我打算先安装Sqlserver&am…

SQLServer2008安装教程

因为对接老系统的数据&#xff0c;上面使用的SQLServer2008&#xff0c;所以本机也需要SQLServer2008作对接。 首当其冲的就是SQLServer2008的安装。 1.下载sqlServer2008的安装包 2.在安装包中点击setup.exe 2.选择安装&#xff0c;再选择全新安装 3.安装规则检测&#xff…

SQL server安装问题汇总

SQLTOC 欢迎使用Markdown编辑器 安装SQL Server遇到的几个问题 1.安装过程中最后出现“数据库引擎服务安装失败”&#xff0c;报错代码&#xff1a;1722&#xff0c;安装时选择“全新SQL server安装或向现有安装添加功能”&#xff0c;安装成功了。 2.安装过程出现“以前的某…