RRT*-Smart论文解读

jucat

已于 2024-03-13 00:11:08 修改

阅读量1.1k

点赞数 9

分类专栏：算法文章标签：算法自动驾驶无人机

于 2023-11-12 02:08:55 首次发布

本文链接：https://blog.csdn.net/jucat/article/details/134356668

版权

算法专栏收录该内容

21 篇文章 4 订阅

订阅专栏

论文

RRT*-Smart论文

RRT，RRT*，RRT*-Smart 对比论文

算法实现

算法在 ROS 平台实现效果在 RRT*Smart全局规划器

简介

传统 RRT 算法可以较快搜索到路径，但不能保证路径最优。后来改进的 RRT* 可以渐进向最优解收敛，但实际上收敛速度很慢。RRT*-Smart 给出了路径优化和智能采样方法，提高搜索路径优化的收敛速度。

论文简述

第一节

RRT-Smart* 不会纯粹地随机采样，而是利用 RRT* 找到的第一条路径作拓展搜索空间，使用智能采样提高路径优化的收敛速度。

第二节 RRT* 介绍

RRT* 是基于增量采样的算法，可以非常快速地找到初始路径，该路径在后续执行时候优化。

数学描述为在 X 空间中，障碍空间 X_obs 在 X 中，自由空间 X_free = X / X_obs，X_goal 是目标空间。RRT* 在输入为 u:[0,T]，满足约束条件下，产生可行驶路径 x(T) ∈ X_free，从 x(0)=x_init 到目标 x(T)=goal。同时 RRT* 维护树 T=(V，E)，包含在 X_free 空间采样的顶点 V，以及将顶点连接在一起的边 E。

X_free = X / X_obs 是集合的概念，X_free 是 X 和 X_obs 的补集。

补集/：两个集中非共同元素组成的集合。也叫反交集。

e.g. (1,2,3) / (1,3,4) = (2,4)

逆集\：第二个集合减去第一个集合的元素。也叫反差集。

e.g. (1,2,3) \ (1,3,4) = (4)

RRT* 伪代码：

初始化树
插入第一个树节点，初始节点应该是机器当前位姿
循环迭代 N 次
1. 随机采样得到一个位置 Z_rand
2. 在树 T 中找到离 Z_rand 最近的节点 Z_nearest
3. 由 Z_nearest 到 Z_rand 在满足约束条件下，根据控制 U_new 产生一个新的节点 Z_new
4. 判断 Z_new 是否可以通过，无碰撞的话执行下面步骤
  1. 在 Z_new 一定范围内搜索树 T 的一些邻近节点 Z_near，邻近节点可能会有多个
  2. 计算从 Z_init 开始经过 Z_near 到 Z_new 的路径总代价，选择代价最小的邻近节点 Z_min 为 Z_new 的父节点
  3. 把 Z_new 插入到树 T 中，改 Z_min 为 Z_new 的父节点
  4. 树 T 路径调整，邻近节点尝试把 Z_new 作为父节点并计算代价，总代价低则更新 Z_new 为父节点
返回树 T

邻近节点搜索半径有个参考公式：

$k=\gamma (\frac{log(n)}{n})^{\frac{1}{d}}$

γ 是常量参数，d 是维度，n 是迭代数

当 d = 2，k = 10 时候

当 d = 3，k = 10 时候

RRT* 图解

随机采样点 rand 的最近节点是 F，F 向 rand 运行得到下一个节点 new
以节点 new 为圆心在附近范围 r 内搜索树 T，找到邻近节点 B C F G
计算从 new 到邻近节点再到起点 A 的路径，得到 A - B - new 路径代价最小，把节点 B 设置为 new 的父节点
除了节点 B 外的邻近节点，如果改其父节点为 new 的总路径代价比原来总代价低，把邻近节点的父节点改为 new
新路线 A - B - new - G 代价 12 比原来路线 A - D - F - G 代价 13 低，把 G 的父节点改为 new

第三节 RRT*-Smart

执行与 RRT* 类似，但在优化路径时候补充了新方法。

有了初始路径后，会从目标 goal 开始，反向直连前方节点，直到检测到碰撞或者代价变高停止。然后从停止的节点开始反向直连其前方节点，如此反复。如果路径成本更低，就更新路径树。这个直连的节点就作为信标节点 Z_beacons。

有了信标节点 Z_beacons，会在其半径 R_beacons 范围内随机采样一些 Z_rand 节点。不仅可以优化路径代价，还可以优化在障碍物附近的路径。

因此 RRT*-Smart 加速了优化收敛并降低路径成本。

偏置比 b： $BiasingRatio=(\frac{n}{z_{free}})*B$ ，B 是一个需要调节的参数

RRT*-Smart 伪代码：

初始化树
插入第一个树节点，初始节点应该是机器当前位姿
循环迭代 N 次
1. 有信标后，以常数 b 的时间间隔进行偏差采样：
  1. 偏差采样
2. 不需要信标采样：
  1. 随机采样
3. 在树 T 中找到离 Z_rand 最近的节点 Z_nearest
4. 由 Z_nearest 到 Z_rand 在满足约束条件下，根据控制 U_new 产生一个新的节点 Z_new
5. 判断 Z_new 是否可以通过，无碰撞的话执行下面步骤
  1. 在 Z_new 一定范围内搜索树 T 的一些邻近节点 Z_near，邻近节点可能会有多个
  2. 计算从 Z_init 开始经过 Z_near 到 Z_new 的路径总代价，选择代价最小的邻近节点 Z_min 为 Z_new 的父节点
  3. 把 Z_new 插入到树 T 中，改 Z_min 为 Z_new 的父节点
  4. 树 T 路径调整，邻近节点尝试把 Z_new 作为父节点并计算代价，总代价低则更新 Z_new 为父节点
6. 如果找到了初始通向目标 goal 的路径
  1. 返回当前迭代次数给到 n
  2. 通过直接连接路径中彼此可见的节点，计算从 Z_init 到 Z_goal 的优化路径并返回其成本
7. 如果优化路径比之前路径代价更低
  1. 产生信标 Z_beacons