路径规划——RRT*算法

路径规划------RRT*算法

算法原理

RRT Star 算法是一种渐近最优的路径规划算法,它是 RRT 算法的优化版本。RRT Star 算法通过不断地迭代和优化,最终可以得到一条从起点到目标点的最优路径。

在学习RRT Star 算法之前最好先学习一下RRT原始算法:RRT算法

与RRT 算法相比,RRT Star 算法的主要不同之处在于它对已构建的路径进行优化,以提高搜索效率和精度。RRT Star算法与RRT算法的区别主要在于重新选择父节点和重新布线两个方面。

1.重新选择父节点过程

在新产生的节点node_new附近以定义的半径范围内寻找"近邻",作为替换父节点的备选。依次计算"近邻"节点到起点的路径代价加上到每个"近邻"的路径代价,具体过程如下图:

在上图中的这种情况下,以节点4作为node_new节点的父节点时路径代价是最小的,因此当前随机树的结构无需改动。

如果是下图的情况:

此时,以节点6作为node_new节点的父节点时的路径代价是最小的,因此进行随机树结构改动为如下图所示:

2.重新布线过程

在为node_new节点重新选择父节点后,为了进一步使随机树节点间的代价尽量小,为随机树重新布线。如果"近邻节点"的父节点改为node_new节点可以减小路径代价,则进行更改。如下图所示:

原来的路径情况:

将近邻节点的父节点改为node_new节点后的路径情况:

可以发现将节点6和8的父节点改为node_new后路径代价大于原来的路径代价,因此不需要改变节点6和8的父节点。

而对于节点4和7,将其父节点改为node_new后路径代价小于原来的路径代价,因此需要将节点4和7的父节点改为node_new,则新生成的随机树如下图所示:

这两部分的实现代码:

python 复制代码
for node_n in node_list:
    new_dist = self.distance(node_n,node_new)
    if new_dist < self.search_r and not self.isCollision(node_n, node_new):

        cost = node_n.cost + new_dist
        if node_new.cost > cost:
            # find who is the best parent of node_new
            node_new.parent = node_n.position
            node_new.cost = cost
            else:
                cost = node_new.cost + new_dist
                if node_n.cost > cost:
                    # find whose best parent is node_new
                    node_n.parent = node_new.position
                    node_n.cost = cost
                    else:
                        continue
相关推荐
在所不辞兄2 小时前
为什么数字孪生驱动的产线标定需要在线更新机制,而不能一次标定后长期沿用?
人工智能·神经网络·其他·算法·机器学习
AvocadoZeroOneOI6 小时前
B4500 [GESP202603 三级] 凯撒密码 详细解题报告
c++·算法
cjz38998 小时前
双指针技巧:覆盖式双指针(原地覆盖)
算法
橘子1312 小时前
【高级数据库】关系代数基本概念
算法
电科_银尘1 天前
【大模型算法与工程 15】手写Self-Attention:Q、K、V与缩放点积
算法
郝学胜-神的一滴1 天前
Effective STL 条款03:容器中的对象复制要廉价且正确
开发语言·数据结构·c++·算法·rpc·stl
秋天的怀念_David1 天前
算法一千零一夜 · 第三十三夜 · 跳表
java·程序人生·算法
Pniubi1 天前
力扣21合并两个有序链表
算法·leetcode·职场和发展
AIgorithmGEEK1 天前
std::unordered_map 与 std::unordered_set:接口、算法技巧与权衡 (速查)
c++·算法·哈希表·负载因子·桶·unordered_map·unordered_set
上海潭龙东海科技有限公司1 天前
智慧堆场动态模型三维实时孪生重构智能管控
大数据·网络·数据库·算法·重构