>
Home

登龙（DLonng）

选择大于努力

自动驾驶规划算法 - RRT、GoalRRT、RRTStar 等基本原理

版权声明：本文为 DLonng 非原创文章，可以随意转载，但必须在明确位置注明出处！

RRT

基本原理

RRT(Rapidly-Exploring Random Trees) 快速随机扩展树，是一种单一查询路径规划算法。RRT 将根节点作为搜索的起点，然后通过随机撒点采样增加叶子节点的方式，生成一个随机扩展树，当新采样的叶子节点进入目标范围内，就得到了从起点位置到目标位置的路径，原理图如下：

算法基本步骤：

  1. 首先，在构型空间内随机(一般使用均匀分布)生成一个节点 q_rand
  2. 然后在已知的路径中找到和 q_rand 距离最近的节点 q
  3. 继续在线段 q 和 q_rand 之间添加一个节点 q_new，并保证 q 和 q_new之间的距离为 step_size`
  4. 最后检测 q_new 是否碰到障碍物，如果碰到则舍弃它，继续选择下一个 q_new
  5. 重复上述过程，直到路径上最后一个节点距离目标位置在一定阈值范围内，则找到了最终路径
    • 并不能保证最后一次延伸能够刚好到达终点的位置，因此需要设置阈值范围

步长参数的作用

  • 步长的设置显然也会影响树的形状
    • 当步长太大时，可能由于太过笨拙而无法成功绕过障碍物，且在终点附近来回跳动
    • 当步长过小时，生长的速度显然会有所减慢（因为同样的距离要生长更多次），且采样点非常密集
    • 一般来说，空间越复杂，步长越小
    • 生长步长一定要比判断是否为同一个采样点的阈值要大
      • 如果步长小于采样点阈值，则采样出来的新节点与最近的节点距离太近（小于设定的阈值），就被认为是相同节点，这就导致采样失败了

完备性

随机采样的 RRT 路径规划算法适用于高维度空间，它们以概率完备性(当时间接近无限时一定有解)来代替完备性，从而提高搜索效率

  • RRT 算法是概率完备的，只要路径存在且规划的时间足够长，就一定能确保找到一条路径解
    • 注意「且规划的时间足够长」这一前提条件，说明了如果规划器的参数设置不合理（如搜索次数限制太少、采样点过少等）导致算法很快终止，就可能找不到解
  • 补充：概率完备，意思就是说假如在规划的起点和终点之间存在有效路径，也就是路径规划是有解的话，那么只要规划的时间足够长、采样点足够多，必然可以找到有效的路径

特点

  • RRT 是一种可以对非凸高维空间快速搜索的算法，可以很容易的处理包含障碍物和差分运动约束的场景
  • RRT 可以有效地考虑到移动机器人的非完整约束限制并且可以有效的搜索整个解空间
    • 完整约束
      • 全向机器人（能够前后左右平移），忽略了移动机器人的转向轮转向角的限制
    • 非完整性约束
      • 非全向机器人（差速和阿克曼），非全向机器人不能左右平移，且阿克曼底盘有最小转向半径约束
  • RRT 的一大显著特征就是它具备探索空间的能力，即从一点（根）出发向外探索拓展的特征
  • PRM 算法在一开始就通过抽样在地图上构建出完整的无向图，再进行图搜索，而 RRT 算法则是从某个点出发一边搜索，一边采样
  • RRT 每次在地图内随机选择生长方向，有一定的概率会向着目标点延伸，也有一定的概率会随机在地图内选择一个方向延伸

伪代码

算法伪代码

-----------------------------------------------------
Algorithm: RRT Algorithm
-----------------------------------------------------
Input: X, x_init, x_goal, n, step_size
Output: A path connecting x_start to x_goal
T.init(x_start);
for i = 1 to n do
    x_rand <- sample_state(X);
    x_nearest <- nearest_neighbor(T, x_rand);
    x_new <- extend(x_nearest, x_rand, step_size);
    if CollisionFree(x_nearest, x_new) then
        T.add_vertex(x_new);
        T.add_edge(x_nearest, x_new);
    end
    if x_new == x_goal then
        success();
    end
end
-----------------------------------------------------

demo 伪代码：

1. 设置起始点和目标点、障碍物列表
2. 调用 RRT 算法
	1.1 生成一个随机位置 q_rand
	1.2 找到和新生成的随机节点 q_rand 距离最近的节点 q_nearest
	1.3 利用反正切计算角度 theta = atan2(q_rand.y - q_nearest.y, q_rand.x - q_nearest.x)
	1.4 利用角度和步长计算新坐标 q_new(q_nearest.x + step_size * cos(theta), q_nearest.y + step_size * sin(theta))
	1.5 把新节点 q_new 的父节点设置为 q_nearest
	1.6 如果 CollisionCheck(q_new) == true 舍弃 q_new，返回步骤 1 重新选择下一个随机位置
	1.7 如果新节点到目标节点的距离 sqrt(q_new, goal) < step_size 则跳出循环
3. 根据父节点的指针反向画出规划的路径

复杂度

???

优缺点

  • 优点
    • 无需对系统进行建模
    • 无需对搜索区域进行几何划分
    • 在搜索空间的覆盖率高，搜索的范围广，可以尽可能的探索未知区域
  • 缺点
    • 随机选择采样点导致效率低
    • 无法保证路径是最优，路径不光滑（转折点角度大或者数量多），对机器人移动而言不是最优路径（路径跟踪难度大）

RRT 的改进

基于目标的快速 RRT

  • 改进的目的：解决随机选择采样点导致效率低的问题

    • 原始 RRT 随机树搜索漫无目的，在整个度量空间生成随机点进行扩展，直到恰好有节点扩展到目标附近才结束搜索，生成最终路径
    • 这导致搜索树经常扩展到一些离目标很远的无用区域，导致效率低下
    • 因此我们希望随机树尽可能向着目标方向搜索，以加快搜索速度

  • 改进的方法：人为引导随机点的采样

    • 在产生随机点 q_rand时，以一定的目标概率 p 选取目标点 q_goal 作为随机点 q_rand = q_goal
    • 这相当于以一定的目标概率驱使随机树向着目标方向扩展
    • 目标概率 p 对随机树拓展的影响
      • p 越小(p = 0.01)，树的分支也就越多（过度随机采样，原始 RRT），这会导致生长缺乏方向性，是一种「碰运气」式的搜索
      • p 越大(p = 0.9)，产生分支的太少（过度趋向目标采样，类似最佳优先的贪心搜索），会导致很难绕过障碍物找到目标点
        • p = 1.0 无法绕开障碍物

  • 改进后的缺点：

    • 如果目标概率的不断加大，当障碍物遮挡目标时容易陷入局部搜索无法跳出（随机产生的分支太少了）
    • 因此为了保持随机树对未知空间的扩展能力，概率通常不宜选择的过大(通常0.05 - 0.1)
  • -----------------------------------------------------
Algorithm: Goal RRT Algorithm
-----------------------------------------------------
Input: X, x_init, x_goal, n, step_size
Output: A path connecting x_start to x_goal
T.init(x_start);
for i = 1 to n do
    // 与 RRT 的不同之处
    x_rand <- sample_random_state(X);
    x_nearest <- nearest_neighbor(T, x_rand);
    x_new <- extend(x_nearest, x_rand, step_size);
    if CollisionFree(x_nearest, x_new) then
        T.add_vertex(x_new);
        T.add_edge(x_nearest, x_new);
    end
    if x_new == x_goal then
        success();
    end
end
-----------------------------------------------------

  • // Goal RRT
function sample_random_state()
p = rand(0, 1)
// P_Goal 为目标采样概率，如 0.5
if (p < P_Goal)
    q_rand = q_goal;
else
    q_rand = random_state();

RRT Connect

  • RRT Connect 算法从初始状态点和目标状态点同时扩展随机树从而实现对状态空间的快速搜索
    • 改进的目的也是为了加速

RRT Connect

RRT*

  • 改进的目的：解决 RRT 算法难以求解最优的可行路径的问题（路径不光滑，对机器人移动而言不是最优路径）

  • 改进的方法：

    • RRT* 算法是渐进优化的，随着迭代次数和采样点的增加，得到的路径越来越优，迭代的时间越久，就越可以得到相对满意的路径，因此要想得出相对满意的优化路径，需要一定的运算时间
    • 优化分为 2 个步骤
      • 重新为 x_new 选择父节点
        • 降低起点到 x_new 的路径代价
      • 重新对随机树进行布线
        • 为进一步降低随机树节点之间的路径代价
        • 如果以整体的眼光看，并不是每一个重新布线的节点都会出现在最终生成的路径中，但在生成随机树的过程中，每一次的重布线都尽可能的为最终路径代价减小创造机会
      • 这两个过程相辅相成，重新选择父节点使新生成的节点路径代价尽可能小，重布线使得生成新节点后的随机树减小路径代价

  • 步骤 1：重新为 x_new 选择父节点

    • 在新产生的节点 x_new 附近以定义的半径范围 r 内寻找所有的近邻节点 x_neighbor，作为替换 x_new 原始父节点 x_rand 的备选
      • 半径 r 如何计算？没看太懂。。。
    • 依次计算起点 x_init 到每个近邻节点 x_neighbor 的路径代价加上近邻节点 x_neighbor 到 x_new 的路径代价
    • 取路径代价最小的近邻节点 x_neighbor_mincost 作为 x_new 新的父节点

  • 步骤 2：重新对随机树进行布线

    • 重布线的过程：如果近邻节点 x_neighbor 的父节点改为新产生的节点 x_new 可以减小路径代价，则进行更改
  • 半径 R 对算法的影响
    • R 太小（R = 7）：每次优化的邻居点很少，优化力度太小，路径类似与 RRT
    • R 太大（R = 700）：每次优化的邻居点非常多(比如包括起点)，优化力度太大，使用欧式距离作为总代价
      • 对步骤 1 的影响：导致新采样的 x_new 的父节点总是被优化为起点（因为总成本是用到起点的欧式距离衡量，新采样的点到起点的总成本是最小的，两点之间直线最短）
      • 对步骤 2 的影响：范围太大导致对所有点都进行优化，而如果包含起点会直接把很多采样点的父节点优化为起点，因为采样点到起点的距离是最短的，中间不用经过其他采样点。
      • 对路径的影响：采样点很少，起点区域的树枝很密集，因为很多采样点的父节点被优化为起点了
    • R 正常（R = 70） 
-----------------------------------------------------
Algorithm: RRT* Algorithm
-----------------------------------------------------
Input: X, x_start, x_goal, n, step_size, r
Ouput: path connecting x_start to x_goal
T.init(x_start);
for i = 1 to n do
    x_rand <- sample_random_state(X);
    x_nearest <- nearest_neighbor(T, x_rand);
    x_new <- extend(x_nearest, x_rand, step_size);

    if CollisionFree(x_nearest, x_new) then
        T.add_vertex(x_new);
        T.add_edge(x_nearest, x_new);
        X_near <- near_neighbors(T, x_new, r);
        // 重新为 x_new 选择父节点
        // 相当于为 x_new 选择是否将 x_near 作为新的父节点
        for x_near in X_near do
            // x_near 为父节点、x_new 为子节点
            rewire_rrt_star(x_near, x_new);
        end
        // 重新对随机树进行布线
        // 相当于为 x_near 选择是否将 x_new 作为新的父节点
        for x_near in X_near do
            // x_new 为父节点、x_near 为子节点
            rewire_rrt_star(x_new, x_near);
        end
    end
    if x_new == x_goal then
        success();
    end
end
-----------------------------------------------------


-----------------------------------------------------
Algorithm: rewire_rrt_star Algorithm
-----------------------------------------------------
Input: x_potential_parent, x_child
Ouput: T
if CollisionFree(x_potential_parent, x_child) then
    // 计算父子节点之间的路径代价
    c <- cost(x_potential_parent, x_child);
	// 起点到父节点的路径代价 + 父子节点间的路径代价 < 起点到子节点间的路径代价
    if (cost_from_start(x_potential_parent) + c < cost_from_start(x_child)) then
        // 建立父子节点的指针关系
        T.parent(x_child) <- x_potential_parent
    end
end
-----------------------------------------------------

RRT*-Connect

就是双向的 RRT*。。。

Matlab 仿真代码

参考博客

本文原创首发于微信公号「登龙」，分享机器学习、算法编程、Python、机器人技术等原创文章，扫码即可关注！

DLonng at 04/13/21

‹ ›