路径规划——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 patchesclass 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 = obstaclesself.board_size = board_sizeself.max_try = max_try # Number of iterationsself.max_dist = max_dist # Maximum sampling distanceself.goal_sample_rate = goal_sample_rateself.env = envself.inflation = 1self.searched = []class Node:def __init__(self,position,parent,cost) -> None:self.position = positionself.parent = parentself.cost = costdef run(self):cost,path,expand = self.plan()self.searched = expandself.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 nodenode_rand = self.get_random_node()# Get the nearest neighbor nodenode_near = self.get_nearest_neighbor(list(nodes_forward.values()),node_rand)# Get the new nodenode_new = self.get_new_node(node_rand,node_near)if node_new:nodes_forward[node_new.position] = node_newnode_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 extendingwhile True:for node_position, node in nodes_back.items():if node.position == node_new.position:print("final")cost, path = self.extractPath(node_new, nodes_back, nodes_forward)expand = self.get_expand(list(nodes_back.values()), list(nodes_forward.values()))print("Exploring {} nodes.".format(iter))return cost, path, expandnode_new_b2 = self.get_new_node(node_new,node_new_b)if node_new_b2:nodes_back[node_new_b2.position] = node_new_b2node_new_b = node_new_b2else:breakif len(nodes_back) < len(nodes_forward):nodes_forward, nodes_back = nodes_back, nodes_forwardreturn 0, None, Nonedef get_random_node(self):"""Return a random node."""if random.random()>self.goal_sample_rate:node = self.Node((random.uniform(0,self.env.height),random.uniform(0,self.env.width)),None,0)else:node = self.goalreturn nodedef get_nearest_neighbor(self,node_list,node):"""Return node that is nearest to 'node' in node_list"""dist = [self.distance(node, n) for n in node_list]node_near = node_list[int(np.argmin(dist))]return node_neardef get_new_node(self,node_rand,node_near):"""Return node found based on node_near and node_rand."""dx = node_rand.position[0] - node_near.position[0]dy = node_rand.position[1] - node_near.position[1]dist = math.hypot(dx,dy)theta = math.atan2(dy, dx)d = min(self.max_dist,dist)position = ((node_near.position[0]+d*math.cos(theta)),node_near.position[1]+d*math.sin(theta))node_new = self.Node(position,node_near,node_near.cost+d)if self.isCollision(node_new, node_near):return Nonereturn node_newdef isCollision(self,node1,node2):"""Judge collision from node1 to node2 """if self.isInObstacles(node1) or self.isInObstacles(node2):return Truefor rect in self.env.obs_rectangle:if self.isInterRect(node1,node2,rect):return Truefor circle in self.env.obs_circle:if self.isInterCircle(node1,node2,circle):return Truereturn Falsedef distance(self,node1,node2):dx = node2.position[0] - node1.position[0]dy = node2.position[1] - node1.position[1]return math.hypot(dx,dy)def isInObstacles(self,node):"""Determine whether it is in obstacles or not."""x,y = node.position[0],node.position[1]for (ox,oy,w,h) in self.env.boundary:if ox-self.inflation<x<ox+w+self.inflation and oy-self.inflation<y<oy+h+self.inflation:return Truefor (ox,oy,w,h) in self.env.obs_rectangle:if ox-self.inflation<x<ox+w+self.inflation and oy-self.inflation<y<oy+h+self.inflation:return Truefor (ox,oy,r) in self.env.obs_circle:if math.hypot(x-ox,y-oy)<=r+self.inflation:return Truereturn Falsedef isInterRect(self,node1,node2,rect):""""Judge whether it will cross the rectangle when moving from node1 to node2"""ox,oy,w,h = rectvertex = [[ox-self.inflation,oy-self.inflation],[ox+w+self.inflation,oy-self.inflation],[ox+w+self.inflation,oy+h+self.inflation],[ox-self.inflation,oy+h+self.inflation]]x1,y1 = node1.positionx2,y2 = node2.positiondef cross(p1,p2,p3):x1 = p2[0]-p1[0]y1 = p2[1]-p1[1]x2 = p3[0]-p1[0]y2 = p3[1]-p1[0]return x1*y2 - x2*y1for v1,v2 in combinations(vertex,2):if max(x1,x2) >= min(v1[0],v2[0]) and min(x1,x2)<=max(v1[0],v2[0]) and \max(y1,y2) >= min(v1[1],v2[1]) and min(y1,y2) <= max(v1[1],v2[1]):if cross(v1,v2,node1.position) * cross(v1,v2,node2.position)<=0 and \cross(node1.position,node2.position,v1) * cross(node1.position,node2.position,v2)<=0:return Truereturn Falsedef isInterCircle(self,node1,node2,circle):"""Judge whether it will cross the circle when moving from node1 to node2"""ox,oy,r = circledx = node2.position[0] - node1.position[0]dy = node2.position[1] - node1.position[1]# print("isInterCircle-dx:",dx)# print("isInterCircle-dy:",dy)d = dx * dx + dy * dyif d==0:return False# Projectiont = ((ox - node1.position[0]) * dx + (oy - node1.position[1]) * dy) / d# The projection point is on line segment ABif 0 <= t <= 1:closest_x = node1.position[0] + t * dxclosest_y = node1.position[1] + t * dy# Distance from center of the circle to line segment ABdistance = math.hypot(ox-closest_x,oy-closest_y)return distance <= r+self.inflationreturn Falsedef extractPath(self, node_middle, nodes_back, nodes_forward):""""Extract the path based on the closed set."""if self.start.position in nodes_back:nodes_forward, nodes_back = nodes_back, nodes_forward# forwardnode = nodes_forward[node_middle.position]path_forward = [node.position]cost = node.costwhile node.position != self.start.position:node_parent = nodes_forward[node.parent.position]node = node_parentpath_forward.append(node.position)# backwardnode = nodes_back[node_middle.position]path_back = []cost += node.costwhile node.position != self.goal.position:node_parent = nodes_back[node.parent.position]node = node_parentpath_back.append(node.position)path = list(reversed(path_forward))+path_backreturn cost, pathdef get_expand(self, nodes_back, nodes_forward):expand = []tree_size = max(len(nodes_forward), len(nodes_back))for tr in range(tree_size):if tr < len(nodes_forward):expand.append(nodes_forward[tr])if tr < len(nodes_back):expand.append(nodes_back[tr])return expanddef visualize(self, cost, path):"""Plot the map."""...

完整代码：PathPlanning