路径规划——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 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, expand
node_new_b2 = self.get_new_node(node_new,node_new_b)
if node_new_b2:
nodes_back[node_new_b2.position] = node_new_b2
node_new_b = node_new_b2
else:
break
if len(nodes_back) < len(nodes_forward):
nodes_forward, nodes_back = nodes_back, nodes_forward
return 0, None, None
def 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.goal
return node
def 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_near
def 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 None
return node_new
def isCollision(self,node1,node2):
"""
Judge collision from node1 to node2
"""
if self.isInObstacles(node1) or self.isInObstacles(node2):
return True
for rect in self.env.obs_rectangle:
if self.isInterRect(node1,node2,rect):
return True
for circle in self.env.obs_circle:
if self.isInterCircle(node1,node2,circle):
return True
return False
def 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 True
for (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 True
for (ox,oy,r) in self.env.obs_circle:
if math.hypot(x-ox,y-oy)<=r+self.inflation:
return True
return False
def isInterRect(self,node1,node2,rect):
""""
Judge whether it will cross the rectangle when moving from node1 to node2
"""
ox,oy,w,h = rect
vertex = [[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.position
x2,y2 = node2.position
def 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*y1
for 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 True
return False
def isInterCircle(self,node1,node2,circle):
"""
Judge whether it will cross the circle when moving from node1 to node2
"""
ox,oy,r = circle
dx = 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 * dy
if d==0:
return False
# Projection
t = ((ox - node1.position[0]) * dx + (oy - node1.position[1]) * dy) / d
# The projection point is on line segment AB
if 0 <= t <= 1:
closest_x = node1.position[0] + t * dx
closest_y = node1.position[1] + t * dy
# Distance from center of the circle to line segment AB
distance = math.hypot(ox-closest_x,oy-closest_y)
return distance <= r+self.inflation
return False
def 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
# forward
node = nodes_forward[node_middle.position]
path_forward = [node.position]
cost = node.cost
while node.position != self.start.position:
node_parent = nodes_forward[node.parent.position]
node = node_parent
path_forward.append(node.position)
# backward
node = nodes_back[node_middle.position]
path_back = []
cost += node.cost
while node.position != self.goal.position:
node_parent = nodes_back[node.parent.position]
node = node_parent
path_back.append(node.position)
path = list(reversed(path_forward))+path_back
return cost, path
def 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 expand
def visualize(self, cost, path):
"""
Plot the map.
"""
...
完整代码：PathPlanning
