路径规划——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:
closed_list[node_new.position] = node_new
self.searched.append(node_new)
dist = self.distance(node_new,self.goal)
# Found goal successfully
if dist <= self.max_dist and not self.isCollision(node_new,self.goal):
self.searched.append(self.goal)
self.goal.parent = node_new
self.goal.cost = node_new.cost + self.distance(self.goal,node_new)
closed_list[self.goal.position] = self.goal
cost, path= self.extractPath(closed_list)
print("Exploring {} nodes.".format(i))
return cost,path
return 0,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.Node(self.goal.position,None,0)
return node
def get_nearest_neighbor(self,node_list,node) -> 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):
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]
# Projection
t = ((ox - node1.position[0]) * dx + (oy - node1.position[1]) * dy) / (dx * dx + dy * dy)
# 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,closed_list):
""""
Extract the path based on the closed list.
"""
node = closed_list[self.goal.position]
path = [node.position]
cost = node.cost
while node != self.start:
parent = node.parent
node_parent = closed_list[parent.position]
node = node_parent
path.append(node.position)
return cost,path
def visualize(self, cost, path):
"""
Plot the map.
"""
....
结果图：
