Rapidly-exploring Random Tree

• 介绍

  • 通过在状态空间内随机撒点，控制路径树的生长点和生长方向

• 算法流程

  输入：x_init, x_goal, Map
  Tree.init()
  for i = 1 to n
    x_rand = Sample(Map)
    x_near = Near(x_rand, Tree)
    x_new = Steer(x_near, x_rand, StepSize)
    e[i] = Edge(x_new, x_steer)
    if CollisionFree(Map, e[i])
      Tree.addNode(x_new)
      Tree.addEdge(e[i])
    if x_new == x_goal
      Success()
  

• 优劣势

  • 比RMP更加具有方向性
  • 非最优解
  • 效率低
  • 在整个空间中采样

• 改进方式

  • 用KDTree存储路径树中的点，加速查找x_near
  • Bidirectional RRT，从起点终点一起生长，直到两者相交

RRT*

• 算法流程

  输入：x_init, x_goal, Map
  Tree.init()
  for i = 1 to n
    x_rand = Sample(Map)
    x_near = Near(x_rand, Tree)
    x_new = Steer(x_near, x_rand, StepSize)
    if CollisionFree(Map, e[i])
      xs_near = NearC(Tree, x_new)
      x_min = ChooseParent(xs_near)
      e[i] = Edge(x_new, x_min)
      Tree.addNode(x_new)
      Tree.addEdge(e[i])
      Tree.rewire()
    if x_new == x_goal
      Success()
  

• 细节展示

• ChooseParent，如图所示，x_new是由x_2生长出来的，但不是直接将x_2作为x_new的父，而是从一个固定的半径范围内选择到达x_new的总路程最短的节点作为父节点。

• Tree.rewire()

Kinodynamic-RRT*

• 核心

  • 主体结构与RRT*相似，但细节需要改进
  • 采样(Sample)
    • 不同于在欧式空间内采样，该方法要求在全状态空间内采样(位置、速度、加速度、时间)
  • 求Tree上最近节点
    • 不同于用欧式距离衡量远近，这里用状态转移的代价来衡量，简单的说可以用状态转移的 jerk + T。如果采样的时候没有对T进行采样，还需要加一步求最优T。这个也就是个OBVP过程。

• 技巧

  • 在找Tree中最近节点的时候，实际上对树中的每个节点都求了一次OBVP，效率低下。
  • 为了节省时间，我们可以设置一个cost tolerance r。
  • 求解能够在消耗cost小于r的前提下到达x_rand的全状态边界范围(范围内的Tree上的节点构成后向可达集)，和x_rand所能到达的全状态边界范围(范围内的Tree上的节点构成前向可达集)
  • Near()和ChooseParent()操作都在前后两个可达集里面进行，减小了遍历范围。
  • Rewire()操作在前向可达集中操作

AnyTime-RRT*

• 核心

  • Keep optimizing the leaf RRT tree when the robot executes the current trajectory Anytime Fashion
  • 先快速构建一个RRT,获得一个可行解并记录其代价.之后算法会继续采样,但仅将有利于降低可行解代价的结点插入树中,从而逐渐获得较优的可行解

• 优势

  • 提高实时性

Informed RRT*

• 核心

  • 先快速构建一个RRT，获得一个可行路径。在可行路径的外包椭圆内继续采样点，构建新的，代价更低的路径
  • 不断循环上一个步骤，通过缩小采样空间，提高了效率