路径规划——RRT算法

路径规划——RRT算法

算法原理

RRT算法的全称是快速扩展随机树算法(Rapidly Exploring Random Tree)，它的思想是选取一个初始点作为根节点，通过随机采样，增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，边可以在随机树中通过回溯的方式，找到这条从初始点到目标点的路径。

此算法的重点随机采样+步⻓限制+碰撞检测

算法流程：
1.初始化：以起点start为根节点，创建一棵树（通常用二叉树表示），树的根节点表示起始位置。
2.随机采样：在搜索空间中随机生成一个点x_rand。这个点可能在自由空间中，也可能在障碍物中。
3.寻找最近的节点：在当前的树中找到距离x_rand最近的节点x_near。
4.扩展树：从x_near沿着指向x_rand的方向移动一小步，生成一个新的节点x_new。如果x_new在自由空间中（即不与障碍物碰撞），则将x_new加入到树中，并将x_near和n_new用一条边连接。
5.检查目标：检查x_new是否在目标区域附近，这里的“附近”可以设置一个搜索距离来量化。如果是，则生成一条路径从起点到x_new，并结束算法。
6.迭代：重复步骤2~步骤5，直到找到目标点goal，或达到预设的迭代次数。

由于RRT采用随机采样的方法，RRT生成的路径通常不一定是最优路径，但可以通过多次运行RRT或结合其他优化算法来获得更优路径。

算法实现

import numpy as np
import random
import math
from itertools import combinations
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import matplotlib.patches as patches

class RRT:
    def __init__(self,start,goal,obstacles,board_size,max_try,max_dist,goal_sample_rate,env) -> None:
        self.start = self.Node(start,None,0)
        self.goal = self.Node(goal,None,0)
        self.obstacles = obstacles
        self.board_size = board_size
        self.max_try = max_try # Number of iterations
        self.max_dist = max_dist # Maximum sampling distance
        self.goal_sample_rate = goal_sample_rate
        self.env = env
        self.inflation = 1
        self.searched = []

    class Node:
        def __init__(self,position,parent,cost):
            self.position = position
            self.parent = parent
            self.cost = cost

    def run(self):
        cost,path = self.plan()
        self.visualize(cost,path)

    def plan(self):
        self.searched.append(self.start)
        closed_list = {
   self.start.position: self.start}
        # plan max_try times
        for i in range(self.max_try):
            node_rand = self.get_random_node()
            # visited
            if node_rand.position in closed_list:
                continue
            # Get the nearest neighbor node
            node_near = self.get_nearest_neighbor(list(closed_list.values()),node_rand)
            # Get the new node
            node_new = self.get_new_node(node_rand,node_near)
            if node_new