RRT* 算法是在 RRT 的基础上做出了一些改进,主要改进的点有两点:
-
新结点生成后,优化其父结点。
在生成新结点
new_node
后,首先设置一个搜索区域的半径,搜索该区域中的树结点,并计算其中的每一个结点到新结点new_node
的距离。# 改进一:当生成新的结点时,判断在一定范围内的结点是否有比当前父结点路径更有的结点 near_inds = self.find_near_nodes(new_node) new_node = self.choose_parent(new_node, near_inds)
其中第一步:找到该区域中的树结点。用函数
find_near_nodes()
函数实现,具体代码如下:def find_near_nodes(self, new_node): """ 获取新结点一定范围内的树中的结点 """ n_node = len(self.node_list) r = 50.0 * math.sqrt((math.log(n_node) / n_node)) # 一个渐变的距离 d_list = [(node.x - new_node.x) ** 2 + (node.y - new_node.y) ** 2 for node in self.node_list] near_inds = [d_list.index(i) for i in d_list if i <= r ** 2] return near_inds
第二步:从该区域中选择 cost 最小的作为结点作为其新的父结点。用函数
choose_parent()
函数实现,具体代码如下:def choose_parent(self, new_node, near_inds): if len(near_inds) == 0: return new_node d_list = [] for i in near_inds: dx = new_node.x - self.node_list[i].x dy = new_node.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): d_list.append(self.node_list[i].cost + d) else: d_list.append(float('Inf')) min_cost = min(d_list) min_ind = near_inds[d_list.index(min_cost)] if min_cost == float('Inf'): print("min cost is inf") return new_node new_node.cost = min_cost new_node.parent = min_ind return new_node
在判断新结点和树中结点能否相连时,需要再次检测该路径是否有碰撞,用函数
check_collision()
函数来实现:def check_collision(self, near_node, theta, d): tmp_node = copy.deepcopy(near_node) end_x = tmp_node.x + math.cos(theta) * d end_y = tmp_node.y + math.sin(theta) * d return self.check_segment_collision(tmp_node.x, tmp_node.y, end_x, end_y)
-
新结点生成后,优化周围点的连接关系。
优化代码如下:
def rewire(self, new_node, near_inds): n_node = len(self.node_list) for i in near_inds: near_node = self.node_list[i] d = math.sqrt((near_node.x - new_node.x) ** 2 + (near_node.y - new_node.y) ** 2) s_cost = new_node.cost + d if near_node.cost > s_cost: theta = math.atan2(new_node.y - near_node.y, new_node.x - near_node.x) if self.check_collision(near_node, theta, d): near_node.parent = n_node - 1 near_node.cost = s_cost
RRT* 的二维环境仿真实验结果为:
完整代码如下:
import random
import math
import copy
import time
import matplotlib.pyplot as plt
import numpy as np
class RRT:
# 初始化
def __init__(self,
obstacle_list, # 障碍物
rand_area, # 采样的区域
expand_dis=2.0, # 步长
goal_sample_rate=10, # 目标采样率
max_iter=200): # 最大迭代次数
self.start = None
self.goal = None
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.expand_dis = expand_dis
self.goal_sample_rate = goal_sample_rate
self.max_iter = max_iter
self.obstacle_list = obstacle_list
self.node_list = None
def rrt_planning(self, start, goal, animation=True):
self.start = Node(start[0], start[1])
self.goal = Node(goal[0], goal[1])
self.node_list = [self.start]
path = None
last_path_length = float('inf')
for i in range(self.max_iter):
# 1. 在环境中随机采样点
rnd = self.sample()
# 2. 找到结点树中距离采样点最近的结点
n_ind = self.get_nearest_list_index(self.node_list, rnd)
nearest_node = self.node_list[n_ind]
# 3. 在采样点的方向生长一个步长,得到下一个树的结点。
theta = math.atan2(rnd[1] - nearest_node.y, rnd[0] - nearest_node.x)
new_node = self.get_new_node(theta, n_ind, nearest_node)
# 4. 检测碰撞,检测到新生成的结点的路径是否会与障碍物碰撞
no_collision = self.check_segment_collision(new_node.x, new_node.y, nearest_node.x, nearest_node.y)
if no_collision:
# 改进一:当生成新的结点时,判断在一定范围内的结点是否有比当前父结点路径更有的结点
near_inds = self.find_near_nodes(new_node)
new_node = self.choose_parent(new_node, near_inds)
self.node_list.append(new_node)
# 改进二:范围内的结点重新相连
self.rewire(new_node, near_inds)
# 一步一绘制
if animation:
time.sleep(1)
self.draw_graph(new_node, path)
# 判断新结点是否临近目标点
if self.is_near_goal(new_node):
if self.check_segment_collision(new_node.x, new_node.y,
self.goal.x, self.goal.y):
last_index = len(self.node_list) - 1
temp_path = self.get_final_course(last_index) # 回溯路径
temp_path_length = self.get_path_len(temp_path) # 计算路径的长度
if last_path_length > temp_path_length:
path = temp_path
last_path_length = temp_path_length
print("当前的路径长度为:{}".format(last_path_length))
if animation:
self.draw_graph(new_node, 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:
rnd = [self.goal.x, self.goal.y]
return rnd
@staticmethod
def get_nearest_list_index(nodes, rnd):
""" 计算树中距离采样点距离最近的结点 """
d_list = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2
for node in nodes]
min_index = d_list.index(min(d_list))
return min_index
def get_new_node(self, theta, n_ind, nearest_node):
""" 计算新结点 """
new_node = copy.deepcopy(nearest_node)
new_node.x += self.expand_dis * math.cos(theta)
new_node.y += self.expand_dis * math.sin(theta)
new_node.cost += self.expand_dis
new_node.parent = n_ind
return new_node
def check_segment_collision(self, x1, y1, x2, y2):
""" 检测碰撞 """
for (ox, oy, radius) 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 <= radius ** 2:
return False
return True
@staticmethod
def distance_squared_point_to_segment(v, w, p):
""" 计算线段 vw 和 点 p 之间的最短距离"""
if np.array_equal(v, w): # 点 v 和 点 w 重合的情况
return (p - v).dot(p - v)
l2 = (w - v).dot(w - v) # 线段 vw 长度的平方
t = max(0, min(1, (p - v).dot(w - v) / l2))
projection = v + t * (w - v)
return (p - projection).dot(p - projection)
def find_near_nodes(self, new_node):
""" 获取新结点一定范围内的树中的结点 """
n_node = len(self.node_list)
r = 50.0 * math.sqrt((math.log(n_node) / n_node))
d_list = [(node.x - new_node.x) ** 2 + (node.y - new_node.y) ** 2
for node in self.node_list]
near_inds = [d_list.index(i) for i in d_list if i <= r ** 2]
return near_inds
def choose_parent(self, new_node, near_inds):
""" 选择处在区域中的当前树的结点中 cost 最小的结点作为其新的父结点"""
if len(near_inds) == 0:
return new_node
d_list = []
for i in near_inds:
dx = new_node.x - self.node_list[i].x
dy = new_node.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):
d_list.append(self.node_list[i].cost + d)
else:
d_list.append(float('Inf'))
min_cost = min(d_list)
min_ind = near_inds[d_list.index(min_cost)]
if min_cost == float('Inf'):
print("min cost is inf")
return new_node
new_node.cost = min_cost
new_node.parent = min_ind
return new_node
def check_collision(self, near_node, theta, d):
tmp_node = copy.deepcopy(near_node)
end_x = tmp_node.x + math.cos(theta) * d
end_y = tmp_node.y + math.sin(theta) * d
return self.check_segment_collision(tmp_node.x, tmp_node.y, end_x, end_y)
def rewire(self, new_node, near_inds):
""" 范围内的结点重连"""
n_node = len(self.node_list)
for i in near_inds:
near_node = self.node_list[i]
d = math.sqrt((near_node.x - new_node.x) ** 2 + (near_node.y - new_node.y) ** 2)
s_cost = new_node.cost + d
if near_node.cost > s_cost:
theta = math.atan2(new_node.y - near_node.y, new_node.x - near_node.x)
if self.check_collision(near_node, theta, d):
near_node.parent = n_node - 1
near_node.cost = s_cost
def draw_graph(self, rnd=None, path=None):
plt.clf()
# 绘制新的结点
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')
# 绘制起点、终点
plt.plot(self.start.x, self.start.y, "og")
plt.plot(self.goal.x, self.goal.y, "or")
# 绘制障碍物
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.axis([-2, 18, -2, 15])
plt.grid(True)
plt.pause(0.01)
def is_near_goal(self, node):
d = self.line_cost(node, self.goal)
if d < self.expand_dis:
return True
return False
@staticmethod
def line_cost(node1, node2):
return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2)
def get_final_course(self, last_index):
""" 回溯路径 """
path = [[self.goal.x, self.goal.y]]
while self.node_list[last_index].parent is not None:
node = self.node_list[last_index]
path.append([node.x, node.y])
last_index = node.parent
path.append([self.start.x, self.start.y])
return path
@staticmethod
def get_path_len(path):
""" 计算路径的长度 """
path_length = 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]
path_length += math.sqrt((node1_x - node2_x) ** 2 + (node1_y - node2_y) ** 2)
return path_length
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!')
show_animation = True
start = [0, 0]
goal = [15, 12]
# 障碍物 (x, y, radius)
obstacle_list = [
(3, 3, 1.5),
(12, 2, 3),
(3, 9, 2),
(9, 11, 2)
]
rrt = RRT(rand_area=[-2, 18], obstacle_list=obstacle_list, max_iter=200)
path = rrt.rrt_planning(start=[0, 0], goal=[15, 12], animation=show_animation)
print('Done!')
if show_animation and path:
plt.show()
if __name__ == '__main__':
main()
如果对您有帮助,记得在下方点赞哟!也欢迎在下方评论区留言讨论,谢谢!