RRT—— 快速扩展随机算法

算法步骤

1. 生成随机点 Xrand

2. 寻找距离 Xrand 最近的节点 Xnear

当前只有根节点，后期的话有了其他新的节点，则找最近的节点

3. 生成新的节点 Xnew

树的具体生长：将Xnear和Xrand连接起来作为树具体的生长方向，设置步长(stepsize)作为树枝的长度，产生新的节点 Xnew

4. 再次采样 Xrand，寻找距离最近的节点 Xnear

结果发现生长的枝会穿越障碍物，会抛弃这次生长

5. 继续采样，随着采样的进行越来越靠近终点

设置提前停止的方法：每次生成Xnew节点，都会把该点和终点进行连线，判断之间的距离是否小于步长且没有障碍物，这样就直接把该点和终点连起来。

6. 以一定概率选择终点最为采样点

@[TOC]算法讲解

class RRT

def _init_ 初始化

    def __init__(self, obstacleList, randArea = [-2,18], expandDis = 2.0, goalSampleRate = 10, maxIter = 200):

        self.start = None
        self.goal = None
        self.min_rand = randArea[0] # 采样范围赋值给最小值和最大值
        self.max_rand = randArea[1] # randArea = [-2, 18]
        self.expand_dis = expandDis # 设定步长为2
        self.goal_sample_rate = goalSampleRate # 目标采样率，有10%的概率每次将终点作为采样点
        self.max_iter = maxIter # 设置最大迭代次数为200
        self.obstacle_list = obstacleList # 设置障碍物位置和大小
        self.node_list = None # 存储RRT树上节点
1
2
3
4
5
6
7
8
9
10
11

• 障碍物设置

# 设置障碍物 (x,y,r)最后参数是半径
obstacleList = [  (3,  3,  1.5), (12, 2,  3), (3,  9,  2), (9,  11, 2),]
1
2

def rrt_planning

class Node 设置节点

class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.cost = 0.0
        self.parent = None
1
2
3
4
5
6

def sample 获取采样点

• random.randint(start, stop)：返回指定范围内的整数
• random.uniform 返回一个随机浮点数 N ，当 a <= b 时 a <= N <= b ，当 b < a 时 b <= N
  <= a

    def sample(self):
        # randint 是在指定的某个区间内的一个值, 产生(0,100)的随机整数, ,[low,high]左闭右闭
        if random.randint(0, 100) > self.goal_sample_rate: # goal_sample_rate = 10
           # random.uniform 返回一个随机浮点数 N ，当 a <= b 时 a <= N <= b ，当 b < a 时 b <= N <= a
           rnd = [random.uniform(self.min_rand, self.max_rand), random.uniform(self.min_rand, self.max_rand)]
           # random.uniform(a,b) 返回a、b之间的随机浮点值, [a,b], min_read = -2 max_read = 18
        else:  # goal point sampling
            rnd = [self.goal.x, self.goal.y] # 以终点作为采样点，返回终点坐标，goal = [15, 12]
        return rnd
1
2
3
4
5
6
7
8
9

def get_nearest_list_index 找到距离采样点最近的节点下标

    def get_nearest_list_index(nodes, rnd):
        # 遍历当前所有的节点，计算采样点和每个节点的距离
        dList = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in nodes]
        minIndex = dList.index(min(dList)) # 得到列表里距离值最小的下标，dList是列表格式，index最小值的下标
        return minIndex # 返回下标
1
2
3
4
5

theta 求解最近节点和采样点之间的角度，生成新节点方向

theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
1

def get_new_node 得到新的结点

# 生成新的节点, n_ind表示离采样点最近节点的下标，nearestNode表示离采样点最近的节点(用x,y表示)
    def get_new_node(self, theta, n_ind, nearestNode):
        newNode = copy.deepcopy(nearestNode) # 拷贝最近的节点作为新的节点，然后再增加x，y轴方向的坐标

        # 进行生长，生长的距离(设置为2)乘以余弦正弦值，得到节点的坐标，expand_dis = 2
        newNode.x += self.expand_dis * math.cos(theta)
        newNode.y += self.expand_dis * math.sin(theta)

        newNode.cost += self.expand_dis # cost值等于长度，记录长度
        newNode.parent = n_ind # 记录这个新节点的父节点，就是 nearestNode 的下标
        return newNode
1
2
3
4
5
6
7
8
9
10
11

def check_segment_collision 判断障碍物是否在Xnew和Xnear的线段上

    def check_segment_collision(self, x1, y1, x2, y2): # 把生成的新节点和最近的节点输入进来
        for (ox, oy, size) in self.obstacle_list: # 遍历所有障碍物
            # 判断直线和圆是否相切，只需要判断圆心到直线的距离
            # 输入 Xnew, Xnear, 障碍物圆心
            # array处理向量的模块
            dd = self.distance_squared_point_to_segment(
                np.array([x1, y1]),
                np.array([x2, y2]),
                np.array([ox, oy]))
            if dd <= size ** 2: # 距离大于半径则没有碰撞
                return False  # collision
        return True
1
2
3
4
5
6
7
8
9
10
11
12

def distance_squared_point_to_segment 计算点到直线的距离(重难点)

• 点到直线的公式(用向量表示)
• 下面公式的值是一个比例，vw与vo的长度比例 = 向量vw的自乘和向量 vp与 vw乘积的比值

python中dot函数用法

• dot()函数可以通过NumPy库调用，也可以由数组实例对象进行调用。例如：a.dot(b) 与 np.dot(a,b)效果相同。但矩阵积计算不遵循交换律,np.dot(a,b) 和 np.dot(b,a) 得到的结果是不一样的

# 计算点到直线的距离
    def distance_squared_point_to_segment(v, w, p):
        # Return minimum distance between line segment vw and point p
        # 如果端点v,w是同一个点,则距离就是p到v/w的距离，变成点到点的距离
        if np.array_equal(v, w):
            # a.dot(b) 与 np.dot(a, b) 效果相同
            return (p - v).dot(p - v) # v == w case，返回距离的平方(向量(p-v)的x^2+y^2)
        l2 = (w - v).dot(w - v)  # i.e. |w-v|^2 -  avoid a sqrt 计算w和v的线段长度的平方
        # Consider the line extending the segment,
        # parameterized as v + t (w - v).
        # We find projection of point p onto the line.
        # It falls where t = [(p-v) . (w-v)] / |w-v|^2
        # We clamp t from [0,1] to handle points outside the segment vw.
        t = max(0, min(1, (p - v).dot(w - v) / l2)) # (p - v) 与 (w - v)做点积，就是向量pv与向量wv做乘积，t是个比例
        projection = v + t * (w - v)  # Projection falls on the segment 图中的o点
        return (p - projection).dot(p - projection) # 返回p 和 o点的距离的平方
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

def 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
1
2
3
4
5

def line_cost 计算两点间的距离

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

def get_final_course 找路径

    def get_final_course(self, lastIndex): # 找路径，传入终点值
        path = [[self.goal.x, self.goal.y]]
        while self.node_list[lastIndex].parent is not None: # 当parent = 0是就找到终点了
            node = self.node_list[lastIndex]
            path.append([node.x, node.y])
            lastIndex = node.parent
        path.append([self.start.x, self.start.y])
        return path
1
2
3
4
5
6
7
8

RRT 完整代码

import copy
import math
import random
import time

import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation as Rot
import numpy as np

show_animation = True


class RRT:

    def __init__(self, obstacleList, randArea = [-2,18], expandDis = 2.0, goalSampleRate = 10, maxIter = 200):

        self.start = None
        self.goal = None
        self.min_rand = randArea[0] # 采样范围赋值给最小值和最大值
        self.max_rand = randArea[1] # randArea = [-2, 18]
        self.expand_dis = expandDis # 设定步长为2
        self.goal_sample_rate = goalSampleRate # 目标采样率，有10%的概率每次将终点作为采样点
        self.max_iter = maxIter # 设置最大迭代次数为200
        self.obstacle_list = obstacleList # 设置障碍物位置和大小
        self.node_list = None # 存储RRT树上节点

    # start = [0, 0], goal = [15, 12], animation = show_animation (开头设置show_animation = True)
    def rrt_planning(self, start, goal, animation = True):
        start_time = time.time() # 进行计时，统计时间
        self.start = Node(start[0], start[1]) # 将起点和终点以node节点的方式存储起来,start[0]表示起点x值
        self.goal = Node(goal[0], goal[1]) # Node 是一个类 class Node
        self.node_list = [self.start] # 将起点加入到node列表中，起点作为根节点
        path = None

        for i in range(self.max_iter): # 进行for循环，到最大迭代次数200
            rnd = self.sample()  # 获取采样点rnd
            # 求采样点和节点的距离，返回采样点和各个节点距离值最小的节点的下标 n_ind
            n_ind = self.get_nearest_list_index(self.node_list, rnd)
            nearestNode = self.node_list[n_ind] # 使用下标找到距离采样点最近的节点 Xnear

            # steer
            theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) # 求解最近节点和采样点之间的角度
            newNode = self.get_new_node(theta, n_ind, nearestNode) # 生长过程，生成新的节点

            noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)
            if noCollision: # 没有碰撞，就可以把新的节点加入到树里面
                self.node_list.append(newNode)
                if animation:
                    self.draw_graph(newNode, path)

                # 判断是否到了终点附近
                if self.is_near_goal(newNode):
                    if self.check_segment_collision(newNode.x, newNode.y,
                                                    self.goal.x, self.goal.y): # 判断路径是否与障碍物发生碰撞
                        lastIndex = len(self.node_list) - 1 # 没有碰撞的话就找到终点，下标从0开始，长度减1
                        path = self.get_final_course(lastIndex)
                        pathLen = self.get_path_len(path)
                        print("current path length: {}, It costs {} s".format(pathLen, time.time()-start_time))

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

    def rrt_star_planning(self, start, goal, animation=True):
        start_time = time.time()
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        self.node_list = [self.start]
        path = None
        lastPathLength = float('inf')

        for i in range(self.max_iter): # 迭代次数
            rnd = self.sample() # 获取采样点
            # 找离采样点最近的节点，最开始找的离初始点近的结点(有步长)
            n_ind = self.get_nearest_list_index(self.node_list, rnd)
            nearestNode = self.node_list[n_ind] # 由索引得到距离最近的节点

            # steer
            theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) # 求解采样点和最近的节点的角度值
            newNode = self.get_new_node(theta, n_ind, nearestNode) # 由角度、最近点的父节点的索引值、最近节点得到新的节点

            # 判断是否有障碍物
            noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)
            if noCollision:
                nearInds = self.find_near_nodes(newNode)
                newNode = self.choose_parent(newNode, nearInds)

                self.node_list.append(newNode)
                self.rewire(newNode, nearInds)

                if animation:
                    self.draw_graph(newNode, path)

                if self.is_near_goal(newNode):
                    if self.check_segment_collision(newNode.x, newNode.y,
                                                    self.goal.x, self.goal.y):
                        lastIndex = len(self.node_list) - 1

                        tempPath = self.get_final_course(lastIndex)
                        tempPathLen = self.get_path_len(tempPath)
                        if lastPathLength > tempPathLen:
                            path = tempPath
                            lastPathLength = tempPathLen
                            print("current path length: {}, It costs {} s".format(tempPathLen, time.time()-start_time))

        return path

    def informed_rrt_star_planning(self, start, goal, animation=True):
        start_time = time.time()
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        self.node_list = [self.start]
        # max length we expect to find in our 'informed' sample space,
        # starts as infinite
        cBest = float('inf')
        path = None

        # Computing the sampling space
        cMin = math.sqrt(pow(self.start.x - self.goal.x, 2)
                         + pow(self.start.y - self.goal.y, 2))
        xCenter = np.array([[(self.start.x + self.goal.x) / 2.0],
                            [(self.start.y + self.goal.y) / 2.0], [0]])
        a1 = np.array([[(self.goal.x - self.start.x) / cMin],
                       [(self.goal.y - self.start.y) / cMin], [0]])

        e_theta = math.atan2(a1[1], a1[0])

        # 论文方法求旋转矩阵（2选1）
        # first column of identity matrix transposed
        # id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3)
        # M = a1 @ id1_t
        # U, S, Vh = np.linalg.svd(M, True, True)
        # C = np.dot(np.dot(U, np.diag(
        #     [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])),
        #            Vh)

        # 直接用二维平面上的公式（2选1）
        C = np.array([[math.cos(e_theta), -math.sin(e_theta), 0],
                      [math.sin(e_theta), math.cos(e_theta),  0],
                      [0,                 0,                  1]])

        for i in range(self.max_iter):
            # Sample space is defined by cBest
            # cMin is the minimum distance between the start point and the goal
            # xCenter is the midpoint between the start and the goal
            # cBest changes when a new path is found

            rnd = self.informed_sample(cBest, cMin, xCenter, C)
            n_ind = self.get_nearest_list_index(self.node_list, rnd)
            nearestNode = self.node_list[n_ind]

            # steer
            theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
            newNode = self.get_new_node(theta, n_ind, nearestNode)

            noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)
            if noCollision:
                nearInds = self.find_near_nodes(newNode)
                newNode = self.choose_parent(newNode, nearInds)

                self.node_list.append(newNode)
                self.rewire(newNode, nearInds)

                if self.is_near_goal(newNode):
                    if self.check_segment_collision(newNode.x, newNode.y,
                                                    self.goal.x, self.goal.y):
                        lastIndex = len(self.node_list) - 1
                        tempPath = self.get_final_course(lastIndex)
                        tempPathLen = self.get_path_len(tempPath)
                        if tempPathLen < cBest:
                            path = tempPath
                            cBest = tempPathLen
                            print("current path length: {}, It costs {} s".format(tempPathLen, time.time()-start_time))
            if animation:
                self.draw_graph_informed_RRTStar(xCenter=xCenter,
                                                cBest=cBest, cMin=cMin,
                                                e_theta=e_theta, rnd=rnd, path=path)

        return path

    def sample(self):
        # randint 是在指定的某个区间内的一个值, 产生(0,100)的随机整数, ,[low,high]左闭右闭
        if random.randint(0, 100) > self.goal_sample_rate: # goal_sample_rate = 10
           # random.uniform 返回一个随机浮点数 N ，当 a <= b 时 a <= N <= b ，当 b < a 时 b <= N <= a
           rnd = [random.uniform(self.min_rand, self.max_rand), random.uniform(self.min_rand, self.max_rand)]
           # random.uniform(a,b) 返回a、b之间的随机浮点值, [a,b], min_read = -2 max_read = 18
        else:  # goal point sampling
            rnd = [self.goal.x, self.goal.y] # 以终点作为采样点，返回终点坐标，goal = [15, 12]
        return rnd

    def choose_parent(self, newNode, nearInds):
        if len(nearInds) == 0:
            return newNode

        dList = []
        for i in nearInds:
            dx = newNode.x - self.node_list[i].x
            dy = newNode.y - self.node_list[i].y
            d = math.hypot(dx, dy)
            theta = math.atan2(dy, dx)
            if self.check_collision(self.node_list[i], theta, d):
                dList.append(self.node_list[i].cost + d)
            else:
                dList.append(float('inf'))

        minCost = min(dList)
        minInd = nearInds[dList.index(minCost)]

        if minCost == float('inf'):
            print("min cost is inf")
            return newNode

        newNode.cost = minCost
        newNode.parent = minInd

        return newNode

    def find_near_nodes(self, newNode):
        n_node = len(self.node_list)
        r = 50.0 * math.sqrt((math.log(n_node) / n_node))
        d_list = [(node.x - newNode.x) ** 2 + (node.y - newNode.y) ** 2
                  for node in self.node_list]
        near_inds = [d_list.index(i) for i in d_list if i <= r ** 2]
        return near_inds

    def informed_sample(self, cMax, cMin, xCenter, C):
        if cMax < float('inf'):
            r = [cMax / 2.0,
                 math.sqrt(cMax ** 2 - cMin ** 2) / 2.0,
                 math.sqrt(cMax ** 2 - cMin ** 2) / 2.0]
            L = np.diag(r)
            xBall = self.sample_unit_ball()
            rnd = np.dot(np.dot(C, L), xBall) + xCenter
            rnd = [rnd[(0, 0)], rnd[(1, 0)]]
        else:
            rnd = self.sample()

        return rnd

    @staticmethod
    def sample_unit_ball():
        a = random.random()
        b = random.random()

        if b < a:
            a, b = b, a

        sample = (b * math.cos(2 * math.pi * a / b),
                  b * math.sin(2 * math.pi * a / b))
        return np.array([[sample[0]], [sample[1]], [0]])

    @staticmethod
    def get_path_len(path):
        pathLen = 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]
            pathLen += math.sqrt((node1_x - node2_x)
                                 ** 2 + (node1_y - node2_y) ** 2)

        return pathLen

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

    @staticmethod
    def get_nearest_list_index(nodes, rnd):
        # 遍历当前所有的节点，计算采样点和每个节点的距离
        dList = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in nodes]
        minIndex = dList.index(min(dList)) # 得到列表里距离值最小的下标，dList是列表格式，index最小值的下标
        return minIndex # 返回下标

    # 生成新的节点, n_ind表示离采样点最近节点的下标，nearestNode表示离采样点最近的节点(用x,y表示)
    def get_new_node(self, theta, n_ind, nearestNode):
        newNode = copy.deepcopy(nearestNode) # 拷贝最近的节点作为新的节点，然后再增加x，y轴方向的坐标

        # 进行生长，生长的距离(设置为2)乘以余弦正弦值，得到节点的坐标，expand_dis = 2
        newNode.x += self.expand_dis * math.cos(theta)
        newNode.y += self.expand_dis * math.sin(theta)

        newNode.cost += self.expand_dis # cost值等于长度，记录长度
        newNode.parent = n_ind # 记录这个新节点的父节点，就是 nearestNode 的下标
        return newNode

    def is_near_goal(self, node):
        d = self.line_cost(node, self.goal) # 计算新加入点和终点的距离
        if d < self.expand_dis: # 距离小于步长的话，则表示和终点非常接近，可以直接和终点连起来
            return True
        return False

    def rewire(self, newNode, nearInds):
        n_node = len(self.node_list)
        for i in nearInds:
            nearNode = self.node_list[i]

            d = math.sqrt((nearNode.x - newNode.x) ** 2
                          + (nearNode.y - newNode.y) ** 2)

            s_cost = newNode.cost + d

            if nearNode.cost > s_cost:
                theta = math.atan2(newNode.y - nearNode.y,
                                   newNode.x - nearNode.x)
                if self.check_collision(nearNode, theta, d):
                    nearNode.parent = n_node - 1
                    nearNode.cost = s_cost

    @staticmethod
    # 计算点到直线的距离
    def distance_squared_point_to_segment(v, w, p):
        # Return minimum distance between line segment vw and point p
        # 如果端点v,w是同一个点,则距离就是p到v/w的距离，变成点到点的距离
        if np.array_equal(v, w):
            # a.dot(b) 与 np.dot(a, b) 效果相同
            return (p - v).dot(p - v) # v == w case，返回距离的平方(向量(p-v)的x^2+y^2)
        l2 = (w - v).dot(w - v)  # i.e. |w-v|^2 -  avoid a sqrt 计算w和v的线段长度的平方
        # Consider the line extending the segment,
        # parameterized as v + t (w - v).
        # We find projection of point p onto the line.
        # It falls where t = [(p-v) . (w-v)] / |w-v|^2
        # We clamp t from [0,1] to handle points outside the segment vw.
        t = max(0, min(1, (p - v).dot(w - v) / l2)) # (p - v) 与 (w - v)做点积，就是向量pv与向量wv做乘积，t是个比例
        projection = v + t * (w - v)  # Projection falls on the segment 图中的o点
        return (p - projection).dot(p - projection) # 返回p 和 o点的距离的平方

    def check_segment_collision(self, x1, y1, x2, y2): # 把生成的新节点和最近的节点输入进来
        for (ox, oy, size) in self.obstacle_list: # 遍历所有障碍物
            # 判断直线和圆是否相切，只需要判断圆心到直线的距离
            # 输入 Xnew, Xnear, 障碍物圆心
            # array处理向量的模块
            # dd 表示两点距离的平方
            dd = self.distance_squared_point_to_segment(
                np.array([x1, y1]),
                np.array([x2, y2]),
                np.array([ox, oy]))
            if dd <= size ** 2: # 距离的平方大于半径的平方则没有碰撞
                return False  # collision
        return True

    def check_collision(self, nearNode, theta, d):
        tmpNode = copy.deepcopy(nearNode)
        end_x = tmpNode.x + math.cos(theta) * d
        end_y = tmpNode.y + math.sin(theta) * d
        return self.check_segment_collision(tmpNode.x, tmpNode.y, end_x, end_y)

    def get_final_course(self, lastIndex): # 找路径，传入终点值
        path = [[self.goal.x, self.goal.y]]
        while self.node_list[lastIndex].parent is not None: # 当parent = 0是就找到终点了
            node = self.node_list[lastIndex]
            path.append([node.x, node.y])
            lastIndex = node.parent
        path.append([self.start.x, self.start.y])
        return path

    def draw_graph_informed_RRTStar(self, xCenter=None, cBest=None, cMin=None, e_theta=None, rnd=None, path=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[0], rnd[1], "^k")
            if cBest != float('inf'):
                self.plot_ellipse(xCenter, cBest, cMin, e_theta)

        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)

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

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

    @staticmethod
    def plot_ellipse(xCenter, cBest, cMin, e_theta):  # pragma: no cover

        a = math.sqrt(cBest ** 2 - cMin ** 2) / 2.0
        b = cBest / 2.0
        angle = math.pi / 2.0 - e_theta
        cx = xCenter[0]
        cy = xCenter[1]
        t = np.arange(0, 2 * math.pi + 0.1, 0.1)
        x = [a * math.cos(it) for it in t]
        y = [b * math.sin(it) for it in t]
        rot = Rot.from_euler('z', -angle).as_matrix()[0:2, 0:2]
        fx = rot @ np.array([x, y])
        px = np.array(fx[0, :] + cx).flatten()
        py = np.array(fx[1, :] + cy).flatten()
        plt.plot(cx, cy, "xc")
        plt.plot(px, py, "--c")

    def draw_graph(self, rnd=None, path=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 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:
            # self.plot_circle(ox, oy, size)
            plt.plot(ox, oy, "ok", ms=30 * size)

        plt.plot(self.start.x, self.start.y, "xr")
        plt.plot(self.goal.x, self.goal.y, "xr")

        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)


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")

    # create obstacles
    obstacleList = [  # 设置障碍物 (x,y,r)最后参数是半径
        (3,  3,  1.5),
        (12, 2,  3),
        (3,  9,  2),
        (9,  11, 2),]

    # obstacleList = [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2),
    #                 (9, 5, 2), (8, 10, 1)]

    # Set params 类RRT,采样范围[-2,18]，障碍物，最大迭代次数
    rrt = RRT(randArea = [-2, 18], obstacleList = obstacleList, maxIter = 200)
    path = rrt.rrt_planning(start = [0, 0], goal = [15, 12], animation = show_animation) # 传入起点和终点
    # path = rrt.rrt_star_planning(start=[0, 0], goal=[15, 12], animation=show_animation)
    # path = rrt.informed_rrt_star_planning(start=[0, 0], goal=[15, 12], animation=show_animation)
    print("Done!!")

    if show_animation and path:
        plt.show()


if __name__ == '__main__':
    main()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468