RRT规划算法

基于快速扩展随机树（RRT / rapidly exploring random tree）的路径规划算法，通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题。

#!/usr/bin/python
# _*_ coding: UTF-8 _*_
import matplotlib.pyplot as plt
import random
import math
import copy
from scipy import optimize
show_animation = True
import numpy as np

class Node（object）:
    “““
    RRT Node
    “““

    def __init__（self x y）:
        self.x = x
        self.y = y
        self.parent = None


class RRT（object）:
    “““
    Class for RRT Planning
    “““

    def __init__（self start near_points far_points obstacle_list rand_area）:
        “““
        Setting Parameter

        start:Start Position [xy]
        goal:Goal Position [xy]
        obstacleList:obstacle Positions [[xysize]...]
        randArea:random sampling Area [minmax]

        “““
        self.start_Ta = Node（start[0] start[1]）
        self.end = Node（near_points[0] near_points[1]）

        self.start_Tb = Node（far_points[0] far_points[1]）


        self.min_rand_x = rand_area[0]
        self.max_rand_x = rand_area[1]
        self.min_rand_y = rand_area[2]
        self.max_rand_y = rand_area[3]        
        self.expandDis = 1.0
        self.goalSampleRate = 0.05  # 选择终点的概率是0.05
        self.maxIter = 500
        self.obstacleList = obstacle_list
        self.nodeList = []


    def random_node（self mean cov）:
        “““
        产生随机节点
        start point-----> near points
        Far point ------> near points
        :return:
        “““

        node_x node_y  = np.random.multivariate_normal（mean cov 1）.T
        node = [node_x node_y]

        return node

    @staticmethod
    def get_nearest_list_index（node_list rnd）:
        “““
        :param node_list:
        :param rnd:
        :return:
        “““

        d_list = [math.sqrt（（node.x - rnd[0]） ** 2 + （node.y - rnd[1]） ** 2） for node in node_list]
        ‘‘‘
        theta_list = [abs（math.atan（（node.y - rnd[1]）/（node.x - rnd[0]））） for node in node_list]
        

        w_dis = 0.5
        w_theta = 0.5

        theta_list_normlize = [w_theta*（math.pi-theta）/math.pi for theta in theta_list]
        d_list_normlize = [w_dis*（20-distance）/20 for distance in d_list]

        metric_list = np.array（d_list_normlize） + np.array（theta_list_normlize）
        metric_list = metric_list.tolist（）
        ‘‘‘
        
        min_index = d_list.index（min（d_list））
        return min_index

    @staticmethod
    def collision_check（new_node obstacle_list）:
        a = 1
        for （ox oy size） in obstacle_list:
            dx = ox - new_node.x
            dy = oy - new_node.y
            d = math.sqrt（dx * dx + dy * dy）
            if d <= size:
                a = 0  # collision

        return a  # safe


    def planning（self flag）:
        “““
        Path planning

        animation: flag for animation on or off
        “““
        if flag == 0:
            start = self.start_Ta
            mean = [510]
            cov = [[10][01]]

        else:
            #far_dictribution
            start = self.start_Tb
            mean = [205]
            cov = [[200][01