【路径规划】(3) RRT 算法求解最短路，附python完整代码

大家好，今天和各位分享一下机器人路径规划中的 RRT 算法，感兴趣的点个关注，文末有 python 代码，那我们开始吧。

---

1. 算法介绍

RRT 算法是由学者 S.M.LaValle 提出来的路径规划算法。该算法是将空间中的一个起始点当作根节点，在空间中利用增量方式进行随机采样，然后向外延伸扩展增加叶节点。重复该步骤直到获得一个从起点到目标点的树，最后回溯找到从根节点到目标点的无碰撞的路径。 

快速搜索随机树（RRT）算法是一种增量式采样的搜索方法，将它应用于路径规划时不需要任何参数整定，也不需要事先对环境信息进行处理和存储，具有良好的适应性，适合解决复杂环境以及多种约束条件的路径规划问题。 

算法优点：

（1）RRT 算法不需要启发式函数，可以在存在复杂障碍物的环境里快速规划出一条安全路径，且更适合实际应用，目前常被用于无人机的运动规划中。

（2）RRT 的快速迭代生长方式决定了其在高维非凸空间的搜索优势。

算法缺点：

（1）稳定性不佳，且内存消耗较大。扩展树生长方向随机，重复规划结果却有很大差异。

（2）随机性采样导致相对效率较低。由于快速扩展随机树有随机采样性，没有目标启发导向，因此在采样过程中有很多无效采样节点，致使效率相对变低。

（3）路径曲折冗余点较多，光滑性不佳。在 RRT 生成的路径中有许多无用的点，这些冗余点使得路径长度增多。

（4）无法在动态障碍物中有效检测路径，不适合于动态环境规划与避障。

---

2. 算法原理

传统 RRT 算法计算流程可分为两个阶段：

第一个阶段。在任务环境中，以算法初始点作为根节点，通过随机采样、逐步迭代地方式，向任务环境中进行扩展，形成一棵随机树，通过判定采样点是否包含目标点或在目标范围内，来判断是否完成随机树的构造，结束采样；

第二阶段。在随机树中从目标点依次往回遍历，直到寻回根节点，即可得到一条从初始点到目标点的有效路径。RRT的具体扩展方式如下图所示。 

首先，以初始点 Pinit 作为随机树的根节点，在任务环境中随机选取一个采样点 Prand，从节点树中找出离采样点 Prand 最近的一个子节点 Pnear，以点 Pnear 为端点，沿着 Pnear——Prand 方向扩展一定的步长 ，将另一端点记为 Pnew，

若 Pnear 与 Pnew 的连线并未与环境障碍发生交叉，则将点 Pnew 和线段 Pnear--Pnew 加入到随机树中，成为新的子节点和新的路径；

若线段 Pnear 与 Pnew 和障碍发生交叉碰撞，则舍弃点 Pnew，重新选取新的采样点 Prand。重复上述步骤，直至点 Pnew与目标点 Pgoal 不存在障碍，且步长不大于，则将 Pgoal 加入到随机树中，至此，随机树构造完成。

而计算所得的可行路径便是从离 Pgoal 最近的一个子节点 Pnew 依次向上搜索父节点，直到初始点 Pinit，基于 RRT 算法的路径规划结束，其主要流程框图如下图所示。

RRT仿真结果如下图所示，左图为快速扩展随机树随机采样过程，右图为最终提取到的运动路径规划轨迹。设置起始点（10，10）和终止点位置（490，490）坐标。

---

3. 代码实现

如下图，绿色线条代表RRT生长的树枝，红色线条代表搜索到的最优路径

完整代码如下，每段都有注释，有问题在评论区留言

import math
import random
import matplotlib.pyplot as plt
import numpy as np

class RRT:

    class Node:  # 创建节点
        def __init__(self, x, y):
            self.x = x  # 节点坐标
            self.y = y
            self.path_x = [] # 路径，作为画图的数据
            self.path_y = []
            self.parent = None #父节点

    class AreaBounds:
        """区域大小
        """
        def __init__(self, area):
            self.xmin = float(area[0])
            self.xmax = float(area[1])
            self.ymin = float(area[2])
            self.ymax = float(area[3])

    def __init__(self,
            start,
            goal,
            obstacle_list,
            rand_area,
            expand_dis=1.0,  # 树枝长度
            goal_sample_rate=5,
            max_iter=500,
            play_area=None,
            robot_radius=0.0,
            ):
        """
        Setting Parameter

        start:起点 [x,y]
        goal:目标点 [x,y]
        obstacleList:障碍物位置列表 [[x,y,size],...]
        rand_area: 采样区域 x,y ∈ [min,max]
        play_area: 约束随机树的范围 [xmin,xmax,ymin,ymax]
        robot_radius: 机器人半径
        expand_dis: 扩展的步长
        goal_sample_rate: 采样目标点的概率，百分制.default: 5，即表示5%的概率直接采样目标点

        """
        self.start = self.Node(start[0], start[1]) # 根节点(0,0)
        self.end = self.Node(goal[0], goal[1]) # 终点(6,10)
        self.min_rand = rand_area[0]  # -2  树枝生长区域xmin
        self.max_rand = rand_area[1]  # 15  xmax

        if play_area is not None:
            self.play_area = self.AreaBounds(play_area)  # 树枝生长区域，左下(-2,0)==>右上(12,14)
        else:
            self.play_area = None  # 数值无限生长

        self.expand_dis = expand_dis  # 树枝一次的生长长度
        self.goal_sample_rate = goal_sample_rate  # 多少概率直接选终点
        self.max_iter = max_iter  # 最大迭代次数
        self.obstacle_list = obstacle_list  # 障碍物的坐标和半径
        self.node_list = []  # 保存节点
        self.robot_radius = robot_radius  # 随机点的搜索半径

    # 路径规划
    def planning(self, animation=True,camara=None):

        # 将起点作为根节点x_{init}​，加入到随机树的节点集合中。
        self.node_list = [self.start]  # 先在节点列表中保存起点
        for i in range(self.max_iter):
            # 从可行区域内随机选取一个节点x_{rand}
            rnd_node = self.sample_free()  

            # 已生成的树中利用欧氏距离判断距离x_{rand}​最近的点x_{near}。
            # 从已知节点中选择和目标节点最近的节点
            nearest_ind = self.get_nearest_node_index(self.node_list, rnd_node)  # 最接近的节点的索引
            nearest_node = self.node_list[nearest_ind]  # 获取该最近已知节点的坐标

            # 从 x_{near} 与 x_{rand} 的连线方向上扩展固定步长 u，得到新节点 x_{new}
            new_node = self.steer(nearest_node, rnd_node, self.expand_dis)
            
            # 如果在可行区域内，且x_{near}与x_{new}之间无障碍物
            # 判断新点是否在规定的树的生长区域内，新点和最近点之间是否存在障碍物
            if self.is_inside_play_area(new_node, self.play_area) and \
               self.obstacle_free(new_node, self.obstacle_list, self.robot_radius):
               # 都满足才保存该点作为树节点
                self.node_list.append(new_node)

            # 如果此时得到的节点x_new到目标点的距离小于扩展步长，则直接将目标点作为x_rand。
            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)
                # 如果最新点和终点之间没有障碍物True
                if self.obstacle_free(final_node, self.obstacle_list, self.robot_radius):
                    # 返回最终路径
                    return self.generate_final_course(len(self.node_list) - 1)

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

        return None  # cannot find path

    # 距离最近的已知节点坐标，随机坐标，从已知节点向随机节点的延展的长度
    def steer(self, from_node, to_node, extend_length=float("inf")):
        # d已知点和随机点之间的距离，theta两个点之间的夹角
        d, theta = self.calc_distance_and_angle(from_node, to_node)

        # 如果$x_{near}$与$x_{rand}$间的距离小于步长，则直接将$x_{rand}$作为新节点$x_{new}$
        if extend_length >= d:  # 如果树枝的生长长度超出了随机点，就用随机点位置作为新节点
            new_x = to_node.x
            new_y = to_node.y
        else:  # 如果树生长长度没达到随机点长度，就截取长度为extend_length的节点作为新节点
            new_x = from_node.x + math.cos(theta)*extend_length  # 最近点 x + cos * extend_len
            new_y = from_node.y + math.sin(theta)*extend_length  # 最近点 y + sin * extend_len

        new_node = self.Node(new_x,new_y)  # 初始化新节点
        new_node.path_x = [from_node.x]  # 最近点
        new_node.path_y = [from_node.y]  # 
        new_node.path_x.append(new_x)  # 新点
        new_node.path_y.append(new_y)

        new_node.parent = from_node  # 根节点变成最近点，用来指明方向

        return new_node



    def generate_final_course(self, goal_ind):  # 终点坐标的索引
        """生成路径
        Args:
            goal_ind (_type_): 目标点索引

        Returns:
            _type_: _description_
        """
        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):
        """计算(x,y)离目标点的距离
        """
        dx = x - self.end.x  # 新点的x-终点的x
        dy = y - self.end.y
        return math.hypot(dx, dy)  # 计算新点和终点之间的距离

    def sample_free(self):
        # 以（100-goal_sample_rate）%的概率随机生长，(goal_sample_rate)%的概率朝向目标点生长
        if random.randint(0, 100) > self.goal_sample_rate:  # 大于5%就不选终点方向作为下一个节点
            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, camera=None):
        if camera==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")
            if self.robot_radius > 0.0:
                self.plot_circle(rnd.x, rnd.y, self.robot_radius, '-r')
        # 画已生成的树
        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)

        # 如果约定了可行区域，则画出可行区域
        if self.play_area is not None:
            plt.plot([self.play_area.xmin, self.play_area.xmax,
                self.play_area.xmax, self.play_area.xmin,
                self.play_area.xmin],
                [self.play_area.ymin, self.play_area.ymin,
                self.play_area.ymax, self.play_area.ymax,
                self.play_area.ymin],
                "-k")

        # 画出起点和目标点
        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)
        if camera!=None:
            camera.snap()
    # 静态方法无需实例化，也可以实例化后调用，静态方法内部不能调用self.的变量
    @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):  # 已知节点list，随机的节点坐标
        # 计算所有已知节点和随机节点之间的距离
        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 is_inside_play_area(node, play_area):

        if play_area is None:
            return True  # no play_area was defined, every pos should be ok

        if node.x < play_area.xmin or node.x > play_area.xmax or \
           node.y < play_area.ymin or node.y > play_area.ymax:
            return False  # outside - bad
        else:
            return True  # inside - ok

    @staticmethod
    def obstacle_free(node, obstacleList, robot_radius): # 目标点，障碍物中点和半径，移动时的占地半径

        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+robot_radius)**2:
                return False  # collision

        return True  # safe

    @staticmethod
    def calc_distance_and_angle(from_node, to_node):
        """计算两个节点间的距离和方位角
        Args:
            from_node (_type_): _description_
            to_node (_type_): _description_
        Returns:
            _type_: _description_
        """
        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):
    print("start " + __file__)
    fig = plt.figure(1)

    # camera = Camera(fig) # 保存动图时使用
    camera = None # 不保存动图时，camara为None
    show_animation = True
    # ====Search Path with RRT====
    obstacleList = [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2),
                    (9, 5, 2), (8, 10, 1)]  # [x, y, radius]
    # Set Initial parameters
    rrt = RRT(
        start=[0, 0],  # 起点位置
        goal=[gx, gy],  # 终点位置
        rand_area=[-2, 15],  # 树枝可生长区域[xmin,xmax]
        obstacle_list=obstacleList,  # 障碍物
        play_area=[-2, 12, 0, 14],  # 树的生长区域，左下[-2,0]==>右上[12,14]
        robot_radius=0.2  # 搜索半径
    )
    path = rrt.planning(animation=show_animation,camara=camera)

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

        # 绘制最终路径
        if show_animation:
            rrt.draw_graph(camera=camera)
            plt.grid(True)
            plt.pause(0.01)  
            plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
            if camera!=None:
                camera.snap()
                # animation = camera.animate()
                # animation.save('trajectory.gif')
            plt.show()


if __name__ == '__main__':
    main()