RRT路径规划算法在二维仿真环境中的应用 -- Python代码实现

在上一节中，介绍了 RRT 算法的原理，这一节将一步步实现 RRT 路径规划算法在二维环境中的路径规划，来进一步加深对 RRT 算法的理解。

二维环境的搭建

我们将搭建下图所示的二维环境，绿色点为起点（0，0），红色点为目标点（15, 12），黑色的圆表示障碍物。

实现上述环境的代码如下：

start = [0, 0]      # 起点
goal = [15, 12]     # 终点

# 障碍物 （x, y, radiu）
obstacle_list = [
    (3, 3, 1.5),
    (12, 2, 3),
    (3, 9, 2),
    (9, 11, 2)
]
plt.axis([-2, 18, -2, 15])
for (ox, oy, size) in obstacle_list:
    plt.plot(ox, oy, "ok", ms=30 * size)

    plt.plot(start[0], start[1], "og")
    plt.plot(goal[0], goal[1], "or")
plt.show()
plt.pause(0.01)

RRT 路径规划算法

1. 算法初始化

   class RRT:
       
       # 初始化
       def __init__(self, 
                    obstacle_list,         # 障碍物
                    rand_area,             # 采样的区域
                    expand_dis=2.0,        # 步长
                    goal_sample_rate=10,   # 目标采样率
                    max_iter=200):         # 最大迭代次数
          
           self.start = None
           self.goal = None
           self.min_rand = rand_area[0]
           self.max_rand = rand_area[1]
           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 = None
   

2. 路径规划

   • 将起点和终点结点化，方便计算该结点到起点的路径距离以及后面的路径回溯。

     def rrt_planning(self, start, goal, animation=True):
         self.start = Node(start[0], start[1])
         self.goal = Node(goal[0], goal[1])
         self.node_list[self.start]
         path = None
     

     结点化的代码如下：

     class Node:   
         def __init__(self, x, y):
             self.x = x
             self.y = y
             self.cost = 0.0
             self.parent = None
     

   • 开始在环境中循环采样点，在此有一个小的采样技巧，为了使 RRT 树能朝着目标点的方向生长，在采样时，以一定的概率采样目标点。

     rnd = self.sample()             # 在环境中随机采样点
     

     采样函数如下：

     def sample(self):
         if random.randint(0, 100) > self.goal_sample_rate:
             rnd = [random.uniform(self.min_rand, self.max_rand), 
                    random.uniform(self.min_rand, self.max_rand)]
         else:
             rnd = [self.goal.x, self.goal.y]
         return rnd
     

   • 从结点树中找到距离采样点最近的结点。

     n_ind = self.get_nearest_list_index(self.node_list, rnd)
     nearest_node = self.node_list[n_ind]
     

     def get_nearest_list_index(nodes, rnd):
         """ 计算树中距离采样点距离最近的结点 """
         d_list = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 
                   for node in nodes]
         min_index = d_list.index(min(d_list))
         return min_index
     

   • 生成新的下一个结点。在找到树中距离采样点最近的结点后，可以计算两个结点的连线和水平的方向的角度，再根据步长的大小，即可计算出下一个树结点的位置。

     

     theta = math.atan2(rnd[1] - nearest_node.y, rnd[0] - nearest_node.x)
     new_node = self.get_new_node(theta, n_ind, nearest_node)
     

     def get_new_node(self, theta, n_ind, nearest_node):
         """ 计算新结点 """
         new_node = copy.deepcopy(nearest_node)
     
         new_node.x += self.expand_dis * math.cos(theta)
         new_node.y += self.expand_dis * math.sin(theta)
     
         new_node.cost += self.expand_dis
         new_node.parent = n_ind
     
         return new_node
     

   • 检测碰撞。检测新生成的结点的路径是否会与障碍物碰撞

     no_collision = self.check_segment_collision(new_node.x, new_node.y, nearest_node.x, nearest_node.y)
     

     其中检测碰撞的函数如下：

     def check_segment_collision(self, x1, y1, x2, y2):
         for (ox, oy, radius) in self.obstacle_list:
             dd = self.distance_squared_point_to_segment(
                 np.array([x1, y1]),
                 np.array([x2, y2]),
                 np.array([ox, oy])
             )
             if dd <= radius ** 2:
                 return False
         return True
     

     其中 distance_squared_point_to_segment()函数的功能为：求点到线段的最短距离，代码如下：

     def distance_squared_point_to_segment(v, w, p):
         """ 计算线段 vw 和 点 p 之间的最短距离"""
         if np.array_equal(v, w):    # 点 v 和 点 w 重合的情况
             return (p - v).dot(p - v)
     
         l2 = (w - v).dot(w - v)     # 线段 vw 长度的平方
         t = max(0, min(1, (p - v).dot(w - v) / l2))
         projection = v + t * (w - v)
         return (p - projection).dot(p - projection)
     

     

   • 如果没有与障碍物发生碰撞，则将新结点加入到树中，并绘制新结点以及生长的新枝干。再判断新结点是否是目标点的邻接结点。

     if no_collision:
         self.node_list.append(new_node)
     
         # 一步一绘制
         if animation:
             self.draw_graph(new_node, path)
     
         # 判断新结点是否临近目标点
         if self.is_near_goal(new_node):
             if self.check_segment_collision(new_node.x, new_node.y,
                                                 self.goal.x, self.goal.y):
                 last_index = len(self.node_list) - 1
                 path = self.get_final_course(last_index)        # 回溯路径
                 path_length = self.get_path_len(path)           # 计算路径的长度
                 print("当前的路径长度为：{}".format(path_length))
     
                 if animation:
                     self.draw_graph(new_node, path)
                 return path
     

     其中，is_near_goal()是判断新结点是否邻近目标点的函数，其代码如下：

     def is_near_goal(self, node):
         d = self.line_cost(node, self.goal)
         if d < self.expand_dis:
             return True
         return False
     

     line_cost() 函数是如果新生成的结点邻近目标结点的情况下，该结点到目标结点之间的距离。其代码如下：

     def line_cost(node1, node2):
         return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2)
     

     get_final_course()是获取最终从起点到终点的路径的函数。其代码如下：

     def get_final_course(self, last_index):
         """ 回溯路径 """
         path = [[self.goal.x, self.goal.y]]
         while self.node_list[last_index].parent is not None:
             node = self.node_list[last_index]
             path.append([node.x, node.y])
             last_index = node.parent
         path.append([self.start.x, self.start.y])
         return path
     

     get_path_len()是求取路径的总长度的函数，其代码如下：

     def get_path_len(path):
         """ 计算路径的长度 """
         path_length = 0
         for i in range(1, len(path)):
             node1_x = path[i][0]
             node1_y = path[i][1]
             node2_x = path[i - 1][0]
             node2_y = path[i - 1][1]
             path_length += math.sqrt((node1_x - node2_x) ** 2 + (node1_y - node2_y) ** 2)
         return path_length
     

     draw_graph() 为绘制地图以及结点路径等函数，使之可视化。其代码如下：

     def draw_graph(self, rnd=None, path=None):
         plt.clf()
     
         # 绘制新的结点
         if rnd is not None:
             plt.plot(rnd.x, rnd.y, '^k')
     
         # 绘制路径
         for node in self.node_list:
             if node.parent is not None:
                 if node.x or node.y is not None:
                     plt.plot([node.x, self.node_list[node.parent].x],
                              [node.y, self.node_list[node.parent].y],
                              '-g')
     
         # 绘制障碍物
         for (ox, oy, size) in self.obstacle_list:
             plt.plot(ox, oy, "ok", ms=30 * size)
     
         # 绘制起点、终点
         plt.plot(self.start.x, self.start.y, "og")
         plt.plot(self.goal.x, self.goal.y, "or")
     
         # 绘制路径
         if path is not None:
             plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
     
         # 绘制图的设置
         plt.axis([-2, 18, -2, 15])
         plt.grid(True)
         plt.pause(0.01)
     

   最终运行结果如下：

完整代码如下：

import random
import math
import copy
import time

import matplotlib.pyplot as plt
import numpy as np


class RRT:

    # 初始化
    def __init__(self,
                 obstacle_list,         # 障碍物
                 rand_area,             # 采样的区域
                 expand_dis=2.0,        # 步长
                 goal_sample_rate=10,   # 目标采样率
                 max_iter=200):         # 最大迭代次数

        self.start = None
        self.goal = None
        self.min_rand = rand_area[0]
        self.max_rand = rand_area[1]
        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 = None

    def rrt_planning(self, start, goal, animation=True):
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        self.node_list = [self.start]
        path = None

        for i in range(self.max_iter):
            # 1. 在环境中随机采样点
            rnd = self.sample()

            # 2. 找到结点树中距离采样点最近的结点
            n_ind = self.get_nearest_list_index(self.node_list, rnd)
            nearest_node = self.node_list[n_ind]

            # 3. 在采样点的方向生长一个步长，得到下一个树的结点。
            theta = math.atan2(rnd[1] - nearest_node.y, rnd[0] - nearest_node.x)
            new_node = self.get_new_node(theta, n_ind, nearest_node)

            # 4. 检测碰撞，检测到新生成的结点的路径是否会与障碍物碰撞
            no_collision = self.check_segment_collision(new_node.x, new_node.y, nearest_node.x, nearest_node.y)
            if no_collision:
                self.node_list.append(new_node)

                # 一步一绘制
                if animation:
                    time.sleep(1)
                    self.draw_graph(new_node, path)

                # 判断新结点是否临近目标点
                if self.is_near_goal(new_node):
                    if self.check_segment_collision(new_node.x, new_node.y,
                                                    self.goal.x, self.goal.y):
                        last_index = len(self.node_list) - 1
                        path = self.get_final_course(last_index)        # 回溯路径
                        path_length = self.get_path_len(path)           # 计算路径的长度
                        print("当前的路径长度为：{}".format(path_length))

                        if animation:
                            self.draw_graph(new_node, path)
                        return path

    def sample(self):
        """ 在环境中采样点的函数，以一定的概率采样目标点 """
        if random.randint(0, 100) > self.goal_sample_rate:
            rnd = [random.uniform(self.min_rand, self.max_rand),
                   random.uniform(self.min_rand, self.max_rand)]
        else:
            rnd = [self.goal.x, self.goal.y]
        return rnd

    @staticmethod
    def get_nearest_list_index(nodes, rnd):
        """ 计算树中距离采样点距离最近的结点 """
        d_list = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2
                  for node in nodes]
        min_index = d_list.index(min(d_list))
        return min_index

    def get_new_node(self, theta, n_ind, nearest_node):
        """ 计算新结点 """
        new_node = copy.deepcopy(nearest_node)

        new_node.x += self.expand_dis * math.cos(theta)
        new_node.y += self.expand_dis * math.sin(theta)

        new_node.cost += self.expand_dis
        new_node.parent = n_ind

        return new_node

    def check_segment_collision(self, x1, y1, x2, y2):
        """ 检测碰撞 """
        for (ox, oy, radius) in self.obstacle_list:
            dd = self.distance_squared_point_to_segment(
                np.array([x1, y1]),
                np.array([x2, y2]),
                np.array([ox, oy])
            )
            if dd <= radius ** 2:
                return False
        return True

    @staticmethod
    def distance_squared_point_to_segment(v, w, p):
        """ 计算线段 vw 和 点 p 之间的最短距离"""
        if np.array_equal(v, w):    # 点 v 和 点 w 重合的情况
            return (p - v).dot(p - v)

        l2 = (w - v).dot(w - v)     # 线段 vw 长度的平方
        t = max(0, min(1, (p - v).dot(w - v) / l2))
        projection = v + t * (w - v)
        return (p - projection).dot(p - projection)

    def draw_graph(self, rnd=None, path=None):

        plt.clf()

        # 绘制新的结点
        if rnd is not None:
            plt.plot(rnd.x, rnd.y, '^k')

        # 绘制路径
        for node in self.node_list:
            if node.parent is not None:
                if node.x or node.y is not None:
                    plt.plot([node.x, self.node_list[node.parent].x],
                             [node.y, self.node_list[node.parent].y],
                             '-g')

        # 绘制起点、终点
        plt.plot(self.start.x, self.start.y, "og")
        plt.plot(self.goal.x, self.goal.y, "or")

        # 绘制障碍物
        for (ox, oy, size) in self.obstacle_list:
            plt.plot(ox, oy, "ok", ms=30 * size)

        # 绘制路径
        if path is not None:
            plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')

        # 绘制图的设置
        plt.axis([-2, 18, -2, 15])
        plt.grid(True)
        plt.pause(0.01)

    def is_near_goal(self, node):
        d = self.line_cost(node, self.goal)
        if d < self.expand_dis:
            return True
        return False

    @staticmethod
    def line_cost(node1, node2):
        return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2)

    def get_final_course(self, last_index):
        """ 回溯路径 """
        path = [[self.goal.x, self.goal.y]]
        while self.node_list[last_index].parent is not None:
            node = self.node_list[last_index]
            path.append([node.x, node.y])
            last_index = node.parent
        path.append([self.start.x, self.start.y])
        return path

    @staticmethod
    def get_path_len(path):
        """ 计算路径的长度 """
        path_length = 0
        for i in range(1, len(path)):
            node1_x = path[i][0]
            node1_y = path[i][1]
            node2_x = path[i - 1][0]
            node2_y = path[i - 1][1]
            path_length += math.sqrt((node1_x - node2_x) ** 2 + (node1_y - node2_y) ** 2)
        return path_length


class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.cost = 0.0
        self.parent = None


def main():

    print('Start RRT planning!')
    show_animation = True
    start = [0, 0]
    goal = [15, 12]
    # 障碍物 (x, y, radius)
    obstacle_list = [
        (3, 3, 1.5),
        (12, 2, 3),
        (3, 9, 2),
        (9, 11, 2)
    ]

    rrt = RRT(rand_area=[-2, 18], obstacle_list=obstacle_list, max_iter=200)
    path = rrt.rrt_planning(start=[0, 0], goal=[15, 12], animation=show_animation)
    print('Done!')
    if show_animation and path:
        plt.show()


if __name__ == '__main__':
    main()

我是火山&飘雪。如果本文对您有帮助，记得在下方点赞哟！也欢迎在下方评论区留言交流，谢谢！