路径规划——RRT-Connect算法

路径规划——RRT-Connect算法

算法原理

RRT-Connect算法是在RRT算法的基础上进行的扩展，引入了双树生长，分别以起点和目标点为树的根节点同时扩展随机树从而实现对状态空间的快速搜索。在此算法中以两棵随机树建立连接为路径规划成功的条件。并且，在搜索过程中使用了贪婪搜索的方法，在搜索的过程中，两棵树是交替扩展的，与RRT算法不同的是，RRT-Connect算法并不是每次扩展都会进行随机采样，而是第一棵树先随机采样进而扩展一个新的节点node_new，然后第二棵树利用node_new节点往相同的方向进行多次扩展直到扩展失败才会开始下一轮的交替扩展或者与另一棵树能够建立连接了从而满足路径规划完成的条件。

这种双向的RRT算法比原始RRT算法的搜索速度更快，因为除了使用双树扩展搜索，两棵树在扩展时还是朝着对方的方向进行扩展的，并不是完全随机的。

具体的算法流程可结合上述原理以及RRT算法的实现流程。

算法实现

"""
  @File: rrt-connect.py
  @Brief: RRT-Connect algorithm for pathplanning
 
  @Author: Benxiaogu
  @Github: https://github.com/Benxiaogu
  @CSDN: https://blog.csdn.net/weixin_51995147?type=blog
 
  @Date: 2024-11-13
"""
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 RRTConnect:
    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) -> None:
            self.position = position
            self.parent = parent
            self.cost = cost


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

    def plan(self):
        nodes_forward = {
   self.start.position: self.start}
        nodes_back = {
   self.goal.position: self.goal}

        for iter in range(self.max_try):
            # Generate a random node
            node_rand = self.get_random_node()
            # Get the nearest neighbor node
            node_near = self.get_nearest_neighbor(list(nodes_forward.values()),node_rand)
            # Get the new node
            node_new = self.get_new_node(node_rand,node_near)
            
            if node_new:
                nodes_forward[node_new.position] = node_new
                node_near_b = self.get_nearest_neighbor(list(nodes_back.values()), node_new)
                node_new_b = self.get_new_node(node_new,node_near_b)

                if node_new_b:
                    nodes_back[node_new_b.position] = node_new_b
                    # Greedy extending
                    while