ROBO-520 03/29/22 随手记（RRT*, Informed RRT）

参考资料：机器人路径规划、轨迹优化系列课程_哔哩哔哩_bilibili

一. RRT* 

用上的函数：

（1）random.uniform()

 random.uniform(x, y)方法将随机生成一个实数，它在 [x,y] 范围内。

   https://www.jb51.net/article/157225.htm

（2）math.hypot()

   hypot() 返回欧几里德范数 sqrt(x*x + y*y)

   https://www.runoob.com/python/func-number-hypot.html

（3）copy.deepcopy（）

   把复制对象完全复制创建一个新的对象

   https://blog.youkuaiyun.com/qq_32907349/article/details/52190796

代码分析：

初始化:

    obstacle_List = [
        (3,  3,  1.5),
        (12, 2,  3),
        (3,  9,  2),
        (9,  11, 2),
    ]

用圆来表示障碍物，参数为（圆心坐标，半径）

class Node:

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

Node class, 其中记录 （坐标，到此处的cost，母节点）

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

初始的距离默认为float‘inf'以便于更新，path初始化为None，将开始节点加入至Node_List

步骤：

在最大迭代次数以内

（1）生成随机的点：

    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:  # goal point sampling
            rnd = [self.goal.x, self.goal.y]
        return rnd

采用10%的概率取终点为生成点，否则用random.uniform函数生成一个区间的随机点，结果得到随机点的坐标

（2）寻找随机点附近最近的点的index，并且  得到它的x，y数据

    n_ind = self.get_nearest_list_index(self.node_list, rnd)

    def get_nearest_list_index(nodes, rnd):
        dList = [(no