随机快速搜索树RRT

机器人的规划问题的本质，是在已知或者部分已知环境的情况下，规划一条从起点到终点的无碰撞的路径。规划算法种类有很多，比如基于图的搜索算法、人工势场法、基于采样的规划方法等。今天主要介绍的就是一种典型的基于采样的规划方法：随机快速搜索树（RRT）。
RRT是一种在完全已知的环境中通过采样扩展搜索的算法，相比较于基于图的搜索算法，最主要的优点就是快，因此在多自由度机器人的规划问题中发挥着较大的作用，比如机械臂的规划算法基本都是以RRT为基础的。但同时他也有比较明显的缺点，比如通常不最优、规划的路径非常不平滑等。但这些缺点的存在使得后面还有很多对RRT算法的改进，毕竟人无完人，也不能使用一个RRT就能应对所有规划问题。
RRT算法是概率完备的，就是说如果规划时间足够长，如果确实存在一条可行路径，RRT是可以找出这条路径的。但这里存在限制条件，如果规划时间不够长，迭代次数较少，有可能不能找出实际存在的路径。
从名字来看，RRT，全名Rapid-exploration Random Tree。Rapid-exploration指的是RRT的效果，可以快速进行搜索，Random指的是搜索的方式，通过在环境中随机采样的方式探索整个环境。Tree指的是已搜索的位置通过一棵树来存储，每个位置都有自己的父节点和子节点。搜索完成的路径通常是从树的根节点到一个叶节点的路径。
如下图所示，蓝色圆圈为障碍，起点为0，0，终点为6，10。使用RRT来规划一条无碰撞路径，绿色为规划时维护的树。

下面根据RRT算法的步骤进行详细描述。

（1）初始化
由于RRT需要知道完整的环境地图，所以在初始化时要提供环境地图，以进行后续的碰撞检测。
然后将起点作为树的根节点进行存储

（2）随机采样
在确定起点和地图之后，可以进入随机采样环节。随机采样理论上是在整个地图中随机采样，但是实际上我们更关心终点附近的情况，因此采样策略为以一定概率在整个地图中随机采样，剩余部分概率直接选取终点为采样点。

（3）树节点扩展
在选取采样点之后，需要有一种策略将采样点映射到树中，并且将映射点添加为树的新的节点。这里采用的策略是寻找距离采样点最近的树中的节点，将其作为新扩展的节点的父节点，然后以该父节点为基础，向采样点的方向延申一定距离，将延伸后的节点作为新节点并加入树中。这个步骤的前提条件是新节点以及父节点到新节点的路径不发生碰撞，如果发生碰撞，则放弃该采样点和新节点。
另外，父节点向采样点方向延申的距离有一定灵活空间，这里采取设置一个阈值，如果采样点到父节点距离大于阈值，则延申阈值长度，如果小于阈值，则直接将采样点作为新节点。

（4）终止条件
一般情况下，在树节点扩展的方法基础上，可以将扩展的新节点是终点作为终止条件，为了避免不存在路径导致的死循环问题，增加最大迭代次数作为另一个终止条件。

RRT的性能与他的参数设置有很大关系，比如扩展新节点时的步长阈值、选择终点作为采样点的概率等等。这里以不同步长阈值为例，区别可以通过下图看出。

这里给出python源码：

import math
import random
import matplotlib.pyplot as plt
import numpy as np
show_animation = True

class RRT:

    class Node:
        def __init__(self, x, y):
            self.x = x
            self.y = y
            self.path_x = [] # to define the path from parent to current---
            self.path_y = [] # the purpose is to check collision of the path
            self.parent = None

    def __init__(self, start, goal, obstacle_list, rand_area,
                 expand_dis=5, path_resolution=0.5, goal_sample_rate=5, max_iter=500):
        """
        Setting Parameter
        start:Start Position [x,y]
        goal:Goal Position [x,y]
        obstacleList:obstacle Positions [[x,y,size],...]
        randArea:Random Sampling Area [min,max]
        """
        self.start = self.Node(start[0], start[1])
        self.end = self.Node(goal[0], goal[1])
        self.min_rand = rand_area[0]
        self.max_rand = rand_area[1]
        self.expand_dis = expand_dis
        self.path_resolution = path_resolution
        self.goal_sample_rate = goal_sample_rate
        self.max_iter = max_iter
        self.obstacle_list = obstacle_list
        self.node_list = []

    def planning(self, animation=True):
        """
        rrt path planning
        animation: flag for animation on or off
        """

        self.node_list = [self.start]
        for i in range(self.max_iter):
            rnd_node = self.get_random_node()
            nearest_ind = self.get_nearest_node_index(self.node_list, rnd_node)
            nearest_node = self.node_list[nearest_ind]

            new_node = self.steer(nearest_node, rnd_node, self.expand_dis)

            if self.check_collision(new_node, self.obstacle_list):
                self.node_list.append(new_node)

            if animation and i % 5 == 0:
                self.draw_graph(rnd_node)

            if self.calc_dist_to_goal(self.node_list[-1].x, self.node_list[-1].y) <= self.expand_dis:
                final_node = self.steer(self.node_list[-1], self.end, self.expand_dis)
                if self.check_collision(final_node, self.obstacle_list):
                    self.node_list.append(final_node)
                    return self.generate_final_course(len(self.node_list)-1)

        return None 

    def steer(self, from_node, to_node, extend_length=float("inf")):
        '''extend the node list'''
        new_node = self.Node(from_node.x, from_node.y)
        d, theta = self.calc_distance_and_angle(new_node, to_node)

        new_node.path_x = [new_node.x]
        new_node.path_y = [new_node.y]

        if extend_length > d:
            extend_length = d

        n_expand = math.floor(extend_length / self.path_resolution)

        for _ in range(n_expand):
            new_node.x += self.path_resolution * math.cos(theta)
            new_node.y += self.path_resolution * math.sin(theta)
            new_node.path_x.append(new_node.x)
            new_node.path_y.append(new_node.y)

        d, _ = self.calc_distance_and_angle(new_node, to_node)
        if d <= self.path_resolution:
            new_node.path_x.append(to_node.x)
            new_node.path_y.append(to_node.y)

        new_node.parent = from_node

        return new_node

    def generate_final_course(self, goal_ind):
        path = [[self.end.x, self.end.y]]
        node = self.node_list[goal_ind]
        while node.parent is not None:
            path.append([node.x, node.y])
            node = node.parent
        path.append([node.x, node.y])

        return path

    def calc_dist_to_goal(self, x, y):
        dx = x - self.end.x
        dy = y - self.end.y
        return math.hypot(dx, dy)

    def get_random_node(self):
        if random.randint(0, 100) > self.goal_sample_rate:
            rnd = self.Node(random.uniform(self.min_rand, self.max_rand),
                            random.uniform(self.min_rand, self.max_rand))
        else:  # goal point sampling
            rnd = self.Node(self.end.x, self.end.y)
        return rnd

    def draw_graph(self, rnd=None):
        plt.clf()
        # for stopping simulation with the esc key.
        plt.gcf().canvas.mpl_connect('key_release_event',
                                     lambda event: [exit(0) if event.key == 'escape' else None])
        if rnd is not None:
            plt.plot(rnd.x, rnd.y, "^k")
        for node in self.node_list:
            if node.parent:
                plt.plot(node.path_x, node.path_y, "-g")

        for (ox, oy, size) in self.obstacle_list:
            self.plot_circle(ox, oy, size)

        plt.plot(self.start.x, self.start.y, "xr")
        plt.plot(self.end.x, self.end.y, "xr")
        plt.axis("equal")
        plt.axis([-2, 15, -2, 15])
        plt.grid(True)
        plt.pause(0.01)

    @staticmethod
    def plot_circle(x, y, size, color="-b"):  # pragma: no cover
        deg = list(range(0, 360, 5))
        deg.append(0)
        xl = [x + size * math.cos(np.deg2rad(d)) for d in deg]
        yl = [y + size * math.sin(np.deg2rad(d)) for d in deg]
        plt.plot(xl, yl, color)

    @staticmethod
    def get_nearest_node_index(node_list, rnd_node):
        dlist = [(node.x - rnd_node.x) ** 2 + (node.y - rnd_node.y)
                 ** 2 for node in node_list]
        minind = dlist.index(min(dlist))

        return minind

    @staticmethod
    def check_collision(node, obstacleList):

        if node is None:
            return False

        for (ox, oy, size) in obstacleList:
            dx_list = [ox - x for x in node.path_x]
            dy_list = [oy - y for y in node.path_y]
            d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)]

            if min(d_list) <= size ** 2:
                return False  # collision

        return True  # safe

    @staticmethod
    def calc_distance_and_angle(from_node, to_node):
        dx = to_node.x - from_node.x
        dy = to_node.y - from_node.y
        d = math.hypot(dx, dy)
        theta = math.atan2(dy, dx)
        return d, theta


def main(gx=6.0, gy=10.0):
    obstacleList = [
        (5, 5, 1),
        (3, 6, 2),
        (3, 8, 2),
        (3, 10, 2),
        (7, 5, 2),
        (9, 5, 2),
        (8, 10, 1),
        (16, 0, 3)
    ]  # [x, y, radius]
    rrt = RRT(start=[0, 0],
              goal=[gx, gy],
              rand_area=[-2, 15],
              obstacle_list=obstacleList)
    path = rrt.planning(animation=show_animation)

    if path is None:
        print("Cannot find path")
    else:
        print("found path!!")

        if show_animation:
            rrt.draw_graph()
            plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
            plt.grid(True)
            plt.show()
            
if __name__ == '__main__':
    main()

以上代码主要借鉴于规划算法代码