算法背景
RRT*算法是一种渐近最优的路径规划算法，其是快速扩展随机树（RRT）算法的优化版本。RRT*算法通过不断地迭代和优化，最终可以得到一条从起点到目标点的最优路径。与RRT算法相比，RRT*算法的主要不同之处在于其对已构建的路径进行优化，以提高搜索效率和精度。RRT算法适用于各种复杂环境和动态系统，能够有效地解决高维空间和复杂约束的路径规划问题。
基本原理
RRT*算法与RRT算法的区别主要在于重新选择父节点和重新布线两个方面：
1、重新选择父节点：需为新节点在作者自己设定的范围内重新选择父节点，对新生成的New节点进行判断，如下图所示：
比较原本路径和用更改后的2号父节点路径，发现原本路径最优，无需更改路径。
比较原本路径和用更改后的1号父节点路径，发现1号父节点路径最优，更改父节点为1号。
最终更改路径为下图所示：
2、重新布线：对范围内的节点重新连接（rewire）
在对上图进行第一步重新选择父节点之后为Xnew找到最好的父节点Xnear。第二步再为范围内已经在树上的节点进行重连使之找到更好的路径，往往通过这两步一直迭代，最后可以找到一条渐进最优的路线，当然选取步长越短越接近最优。
其中，Xnew为新节点，最优的父节点为Xnear，再以Xnew为父节点判断范围内的其他节点连接是否具有更好的结果。
若是Xnew连接X1，没有原本的Xnear到X1路径好，所以不会重新连接；若是Xnew连接X2，会比原本路径X1连接X2要好，那么我们就会进行重连，把原本的X1连接X2路径删掉，把X2的父节点重新设置成Xnew，将二者连接起来，最终得到下图结果。
案例实操
对起点start=[0, 0],终点goal=[15, 12]进行路径规划，其中四个圆形障碍物为(3, 3, 1.5),
(12, 2, 3),(3, 9, 2),(9, 11, 2)，前两个参数是圆心坐标，第三个参数是半径。
【实验代码】
import copy
import math
import random
import time
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation as Rot
import numpy as np
show_animation = True
class RRT:
def __init__(self, obstacleList, randArea,
expandDis=2.0, goalSampleRate=10, maxIter=200):
self.start = None
self.goal = None
self.min_rand = randArea[0]
self.max_rand = randArea[1]
self.expand_dis = expandDis
self.goal_sample_rate = goalSampleRate
self.max_iter = maxIter
self.obstacle_list = obstacleList
self.node_list = None
def rrt_planning(self, start, goal, animation=True):
start_time = time.time()
self.start = Node(start[0], start[1])
self.goal = Node(goal[0], goal[1])
self.node_list = [self.start]
path = None
for i in range(self.max_iter):
rnd = self.sample()
n_ind = self.get_nearest_list_index(self.node_list, rnd)
nearestNode = self.node_list[n_ind]
# steer
theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
newNode = self.get_new_node(theta, n_ind, nearestNode)
noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)
if noCollision:
self.node_list.append(newNode)
if animation:
self.draw_graph(newNode, path)
if self.is_near_goal(newNode):
if self.check_segment_collision(newNode.x, newNode.y,
self.goal.x, self.goal.y):
lastIndex = len(self.node_list) - 1
path = self.get_final_course(lastIndex)
pathLen = self.get_path_len(path)
print("current path length: {}, It costs {} s".format(pathLen, time.time()-start_time))
if animation:
self.draw_graph(newNode, path)
return path
def rrt_star_planning(self, start, goal, animation=True):
start_time = time.time()# 计时
self.start = Node(start[0], start[1])
self.goal = Node(goal[0], goal[1])# 存储起点与终点
self.node_list = [self.start]# 把起点加入node_list树中，设置起点为根节点
path = None
lastPathLength = float('inf')#当前找到路径的长度，初始化为无穷大
for i in range(self.max_iter):
rnd = self.sample()#sample()采样函数
n_ind = self.get_nearest_list_index(self.node_list, rnd)#返回最近节点下标
nearestNode = self.node_list[n_ind]#设置为最近节点nearestNode
# steer
theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)#计算偏转角度，节点与目标节点之间的围成三角形。
newNode = self.get_new_node(theta, n_ind, nearestNode)#生长过程
noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)#判断是否碰撞，若无加入到树里面
if noCollision:
#为新节点重新选择父节点
nearInds = self.find_near_nodes(newNode)#找到范围内的所有节点下标
newNode = self.choose_parent(newNode, nearInds)#将范围内的节点与新节点放在这个函数里，重新寻找父节点
#为范围里的其他节点进行链接
self.node_list.append(newNode)#将更新的的新节点加入树列表node_list
self.rewire(newNode, nearInds)#重连
if animation:#画图仿真
self.draw_graph(newNode, path)
if self.is_near_goal(newNode):#判断与终点距离是否结束
if self.check_segment_collision(newNode.x, newNode.y,
self.goal.x, self.goal.y):
lastIndex = len(self.node_list) - 1
tempPath = self.get_final_course(lastIndex)
tempPathLen = self.get_path_len(tempPath)
if lastPathLength > tempPathLen:#与RRT算法不同，如果当前路径比我们之前记录的路径还好，则可多次更新
path = tempPath
lastPathLength = tempPathLen
print("current path length: {}, It costs {} s".format(tempPathLen, time.time()-start_time))
return path
def informed_rrt_star_planning(self, start, goal, animation=True):
start_time = time.time()
self.start = Node(start[0], start[1])
self.goal = Node(goal[0], goal[1])
self.node_list = [self.start]
# max length we expect to find in our 'informed' sample space,
# starts as infinite
cBest = float('inf')
path = None
# Computing the sampling space
cMin = math.sqrt(pow(self.start.x - self.goal.x, 2)
+ pow(self.start.y - self.goal.y, 2))
xCenter = np.array([[(self.start.x + self.goal.x) / 2.0],
[(self.start.y + self.goal.y) / 2.0], [0]])
a1 = np.array([[(self.goal.x - self.start.x) / cMin],
[(self.goal.y - self.start.y) / cMin], [0]])
e_theta = math.atan2(a1[1], a1[0])
# 论文方法求旋转矩阵（2选1）
# first column of identity matrix transposed
# id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3)
# M = a1 @ id1_t
# U, S, Vh = np.linalg.svd(M, True, True)
# C = np.dot(np.dot(U, np.diag(
# [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])),
# Vh)
# 直接用二维平面上的公式（2选1）
C = np.array([[math.cos(e_theta), -math.sin(e_theta), 0],
[math.sin(e_theta), math.cos(e_theta), 0],
[0, 0, 1]])
for i in range(self.max_iter):
# Sample space is defined by cBest
# cMin is the minimum distance between the start point and the goal
# xCenter is the midpoint between the start and the goal
# cBest changes when a new path is found
rnd = self.informed_sample(cBest, cMin, xCenter, C)
n_ind = self.get_nearest_list_index(self.node_list, rnd)
nearestNode = self.node_list[n_ind]
# steer
theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
newNode = self.get_new_node(theta, n_ind, nearestNode)
noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)
if noCollision:
nearInds = self.find_near_nodes(newNode)
newNode = self.choose_parent(newNode, nearInds)
self.node_list.append(newNode)
self.rewire(newNode, nearInds)
if self.is_near_goal(newNode):
if self.check_segment_collision(newNode.x, newNode.y,
self.goal.x, self.goal.y):
lastIndex = len(self.node_list) - 1
tempPath = self.get_final_course(lastIndex)
tempPathLen = self.get_path_len(tempPath)
if tempPathLen < cBest:
path = tempPath
cBest = tempPathLen
print("current path length: {}, It costs {} s".format(tempPathLen, time.time()-start_time))
if animation:
self.draw_graph_informed_RRTStar(xCenter=xCenter,
cBest=cBest, cMin=cMin,
e_theta=e_theta, rnd=rnd, path=path)
return path
def sample(self):
if random.randint(0, 100) > self.goal_sample_rate:
rnd = [random.uniform(self.min_rand, self.max_rand), random.uniform(self.min_rand, self.max_rand)]
else: # goal point sampling
rnd = [self.goal.x, self.goal.y]
return rnd
def choose_parent(self, newNode, nearInds): #选取父节点
if len(nearInds) == 0:#圈里面没有候选节点，直接返回
return newNode
dList = []
for i in nearInds:#遍历范围内的所有节点作为父节点
dx = newNode.x - self.node_list[i].x
dy = newNode.y - self.node_list[i].y
d = math.hypot(dx, dy)
theta = math.atan2(dy, dx)
if self.check_collision(self.node_list[i], theta, d):#判断是否遇到障碍物，如果没遇到计算总长度，如果遇到记为无穷大
dList.append(self.node_list[i].cost + d)
else:
dList.append(float('inf'))
minCost = min(dList)#从生成路径dList中找到最短的作为 minCost
minInd = nearInds[dList.index(minCost)]#其作为父节点的索引
if minCost == float('inf'):#若全都是无穷大都遇到障碍物，直接返回
print("min cost is inf")
return newNode
newNode.cost = minCost
newNode.parent = minInd#更新父节点和其下标
return newNode
def find_near_nodes(self, newNode):
n_node = len(self.node_list)#获得当前节点的个数
r = 50.0 * math.sqrt((math.log(n_node) / n_node))#r表示红框范围，根据节点数动态变化的，越到后面节点数越多，范围就要变小，不然算法复杂度过高
d_list = [(node.x - newNode.x) ** 2 + (node.y - newNode.y) ** 2#计算所有节点与当前newNode的距离保存在 d_list
for node in self.node_list]
near_inds = [d_list.index(i) for i in d_list if i <= r ** 2]#如果这个距离小于所定义的半径就放回下标near_inds
return near_inds
def informed_sample(self, cMax, cMin, xCenter, C):
if cMax < float('inf'):
r = [cMax / 2.0,
math.sqrt(cMax ** 2 - cMin ** 2) / 2.0,
math.sqrt(cMax ** 2 - cMin ** 2) / 2.0]
L = np.diag(r)
xBall = self.sample_unit_ball()
rnd = np.dot(np.dot(C, L), xBall) + xCenter
rnd = [rnd[(0, 0)], rnd[(1, 0)]]
else:
rnd = self.sample()
return rnd
@staticmethod
def sample_unit_ball():
a = random.random()
b = random.random()
if b < a:
a, b = b, a
sample = (b * math.cos(2 * math.pi * a / b),
b * math.sin(2 * math.pi * a / b))
return np.array([[sample[0]], [sample[1]], [0]])
@staticmethod
def get_path_len(path):
pathLen = 0
for i in range(1, len(path)):
node1_x = path[i][0]
node1_y = path[i][1]
node2_x = path[i - 1][0]
node2_y = path[i - 1][1]
pathLen += math.sqrt((node1_x - node2_x)
** 2 + (node1_y - node2_y) ** 2)
return pathLen
@staticmethod
def line_cost(node1, node2):
return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2)
@staticmethod
def get_nearest_list_index(nodes, rnd):
dList = [(node.x - rnd[0]) ** 2
+ (node.y - rnd[1]) ** 2 for node in nodes]
minIndex = dList.index(min(dList))
return minIndex
def get_new_node(self, theta, n_ind, nearestNode):
newNode = copy.deepcopy(nearestNode)
newNode.x += self.expand_dis * math.cos(theta)
newNode.y += self.expand_dis * math.sin(theta)
newNode.cost += self.expand_dis
newNode.parent = n_ind
return newNode
def is_near_goal(self, node):
d = self.line_cost(node, self.goal)
if d < self.expand_dis:
return True
return False
def rewire(self, newNode, nearInds):
n_node = len(self.node_list)#存储范围内节点个数
for i in nearInds:
nearNode = self.node_list[i]#遍历范围内节点
d = math.sqrt((nearNode.x - newNode.x) ** 2
+ (nearNode.y - newNode.y) ** 2)#计算newNode与范围内节点的距离d
s_cost = newNode.cost + d#总路径长度为在newNode的长度上加上d
if nearNode.cost > s_cost:#判断新路径s_cost与原本路径nearNode.cost，没有碰撞且距离更短的话更新父节点
theta = math.atan2(newNode.y - nearNode.y,
newNode.x - nearNode.x)
if self.check_collision(nearNode, theta, d):
nearNode.parent = n_node - 1#父节点下标是减一，是由于下标从零开始的
nearNode.cost = s_cost#更新路径
@staticmethod
def distance_squared_point_to_segment(v, w, p):
# Return minimum distance between line segment vw and point p
if np.array_equal(v, w):
return (p - v).dot(p - v) # v == w case
l2 = (w - v).dot(w - v) # i.e. |w-v|^2 - avoid a sqrt
# Consider the line extending the segment,
# parameterized as v + t (w - v).
# We find projection of point p onto the line.
# It falls where t = [(p-v) . (w-v)] / |w-v|^2
# We clamp t from [0,1] to handle points outside the segment vw.
t = max(0, min(1, (p - v).dot(w - v) / l2))
projection = v + t * (w - v) # Projection falls on the segment
return (p - projection).dot(p - projection)
def check_segment_collision(self, x1, y1, x2, y2):
for (ox, oy, size) in self.obstacle_list:
dd = self.distance_squared_point_to_segment(
np.array([x1, y1]),
np.array([x2, y2]),
np.array([ox, oy]))
if dd <= size ** 2:
return False # collision
return True
def check_collision(self, nearNode, theta, d):
tmpNode = copy.deepcopy(nearNode)
end_x = tmpNode.x + math.cos(theta) * d
end_y = tmpNode.y + math.sin(theta) * d
return self.check_segment_collision(tmpNode.x, tmpNode.y, end_x, end_y)
def get_final_course(self, lastIndex):
path = [[self.goal.x, self.goal.y]]
while self.node_list[lastIndex].parent is not None:
node = self.node_list[lastIndex]
path.append([node.x, node.y])
lastIndex = node.parent
path.append([self.start.x, self.start.y])
return path
def draw_graph_informed_RRTStar(self, xCenter=None, cBest=None, cMin=None, e_theta=None, rnd=None, path=None):
plt.clf()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if rnd is not None:
plt.plot(rnd[0], rnd[1], "^k")
if cBest != float('inf'):
self.plot_ellipse(xCenter, cBest, cMin, e_theta)
for node in self.node_list:
if node.parent is not None:
if node.x or node.y is not None:
plt.plot([node.x, self.node_list[node.parent].x], [
node.y, self.node_list[node.parent].y], "-g")
for (ox, oy, size) in self.obstacle_list:
plt.plot(ox, oy, "ok", ms=30 * size)
if path is not None:
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.plot(self.start.x, self.start.y, "xr")
plt.plot(self.goal.x, self.goal.y, "xr")
plt.axis([-2, 18, -2, 15])
plt.grid(True)
plt.pause(0.01)
@staticmethod
def plot_ellipse(xCenter, cBest, cMin, e_theta): # pragma: no cover
a = math.sqrt(cBest ** 2 - cMin ** 2) / 2.0
b = cBest / 2.0
angle = math.pi / 2.0 - e_theta
cx = xCenter[0]
cy = xCenter[1]
t = np.arange(0, 2 * math.pi + 0.1, 0.1)
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
rot = Rot.from_euler('z', -angle).as_matrix()[0:2, 0:2]
fx = rot @ np.array([x, y])
px = np.array(fx[0, :] + cx).flatten()
py = np.array(fx[1, :] + cy).flatten()
plt.plot(cx, cy, "xc")
plt.plot(px, py, "--c")
def draw_graph(self, rnd=None, path=None):
plt.clf()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.node_list:
if node.parent is not None:
if node.x or node.y is not None:
plt.plot([node.x, self.node_list[node.parent].x], [
node.y, self.node_list[node.parent].y], "-g")
for (ox, oy, size) in self.obstacle_list:
# self.plot_circle(ox, oy, size)
plt.plot(ox, oy, "ok", ms=30 * size)
plt.plot(self.start.x, self.start.y, "xr")
plt.plot(self.goal.x, self.goal.y, "xr")
if path is not None:
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.axis([-2, 18, -2, 15])
plt.grid(True)
plt.pause(0.01)
class Node:
def __init__(self, x, y):
self.x = x
self.y = y
self.cost = 0.0
self.parent = None
def main():
print("Start rrt planning")
# create obstacles
obstacleList = [
(3, 3, 1.5),
(12, 2, 3),
(3, 9, 2),
(9, 11, 2),
]
# obstacleList = [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2),
# (9, 5, 2), (8, 10, 1)]
# Set params
rrt = RRT(randArea=[-2, 18], obstacleList=obstacleList, maxIter=200)
# path = rrt.rrt_planning(start=[0, 0], goal=[15, 12], animation=show_animation)
path = rrt.rrt_star_planning(start=[0, 0], goal=[15, 12], animation=show_animation)
# path = rrt.informed_rrt_star_planning(start=[0, 0], goal=[15, 12], animation=show_animation)
print("Done!!")
if show_animation and path:
plt.show()
if __name__ == '__main__':
main()
【运行结果】
【注】如果在更新之后有更好的路径就会覆盖之前随机出来的路径。所以上结果中前三次虽然也满足条件，但没有最后一个距离更短更优。
扫一扫
1888
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
