【自动驾驶】基于采样的路径规划算法——RRT*（含python实现）
文章目录
- 参考资料
- 1. 基本概念
-
- 1.1 基于随机采样的路径规划算法
- 1.2 RRT*算法概述
- 1.3 算法伪代码
- 2. python代码实现
参考资料
- 路径规划 | 随机采样算法
- 基于采样的运动规划算法-RRT
- Rapidly-exploring random tree
- 移动机器人路径规划算法综述
- 路径规划算法
- Robotic Path Planning: RRT and RRT*
- Sampling-based Algorithms for Optimal Motion Planning
- Anytime Motion Planning using the RRT*
- RRT*基本原理
前文回顾
- 基于采样的路径规划算法——PRM（含python实现）
- 基于采样的路径规划算法——RRT（含python实现）
- 基于采样的路径规划算法——RRT-Connect（含python实现）
1. 基本概念
1.1 基于随机采样的路径规划算法
-
基于随机采样的路径规划算法适用于高维度空间，它们以概率完备性(当时间接近无限时一定有解)来代替完备性，从而提高搜索效率。
-
基于随机采样的路径规划算法又分为单查询算法(single-query path planning)以及渐近最优算法(asymptotically optimal path planning)，前者只要找到可行路径即可，侧重快速性，后者还会对找到的路径进行逐步优化，慢慢达到最优，侧重最优性。单查询方法包括概率路图算法(Probabilistic Road Map, PRM)、快速随机扩展树算法(Rapidly-exploring Random Tree, RRT)、RRT-Connect算法等，渐近最优算法有RRT*算法等。
1.2 RRT*算法概述
RRT*算法是一种渐近最优算法，属于RRT算法的优化。
渐近最优的意思是随着迭代次数的增加，得出的路径是越来越优化的，因此要想得出相对满意的优化路径，需要一定的运算时间。
算法流程与RRT算法流程基本相同，不同之处主要在于两个地方：
-
首先，重新为 x n e w x_{new} xnew选择父节点。
- 不同于RRT中直接选择 x n e a r e s t x_{nearest} xnearest作为 x n e w x_{new} xnew的父节点，我们需要重新为 x n e w x_{new} xnew选择父节点，使得 x n e w x_{new} xnew到起点的cost能够最小。至于cost的定义，可以是路径的长度。
- 父节点的选择可以是该节点附近相连的所有点，一般是在新产生的节点 x n e w x_{new} xnew 附近以定义的半径范围 r r r内寻找所有的近邻节点 X n e a r X_{near} Xnear，作为替换 x n e w x_{new} xnew 原始父节点 x n e a r e s t x_{nearest} xnearest 的备选
- 我们需要依次计算起点到每个近邻节点 X n e a r X_{near} Xnear 的路径代价 加上 近邻节点 X n e a r X_{near} Xnear 到 x n e w x_{new} xnew 的路径代价，取路径代价最小的近邻节点 x m i n x_{min} xmin作为 x n e w x_{new} xnew 新的父节点
-
其次，就是在重新选完父节点后，为该节点的所有近邻节点重新布线，即
rewire，布线的原则是使所有节点到起点的cost最小
示意过程如下：
1.3 算法伪代码
算法伪代码如下
伪代码中的一些必要解释：
SampleFree：在可行的区域内随机采样Nearest(G,x_{rand})：在已生成的树中利用欧氏距离判断距离 x r a n d x_{rand} xrand 最近的点 x n e a r e s t x_{nearest} xnearest。Steer(x_{nearest},x_{rand}): 从 x n e a r e s t x_{nearest} xnearest与 x r a n d x_{rand} xrand的连线方向上扩展固定步长得到新节点 x n e w x_{new} xnew（如果 x n e a r e s t x_{nearest} xnearest与 x r a n d x_{rand} xrand间的距离小于步长，则直接将 x r a n d x_{rand} xrand作为新节点 x n e w x_{new} xnew）Card(V)：cardinality of V——应该理解成集合 V V V的大小Line(x_nearest, x_new)：两个点之间的直线距离（欧氏距离）CollisionFree和ObstacleFree：都是判断两个节点之间是否有障碍物
2. python代码实现
代码主体来自pythonRobotics，由于许多代码跟RRT的代码是重复的，所以直接继承了RRT类。完整的代码见github仓库。
import math
import os
import sys
import matplotlib.pyplot as plt
from celluloid import Camera # 保存动图时用，pip install celluloid
sys.path.append("../RRT")
try:
from rrt_planning import RRT
except ImportError:
raise
show_animation = True
class RRTStar(RRT):
"""
Class for RRT Star planning
"""
class Node(RRT.Node):
def __init__(self, x, y):
super().__init__(x, y)
self.cost = 0.0
def __init__(self,
start,
goal,
obstacle_list,
rand_area,
expand_dis=3.0,
goal_sample_rate=20,
max_iter=500,
connect_circle_dist=50.0,
search_until_max_iter=False,
robot_radius=0.0):
"""
Setting Parameter
start:Start Position [x,y]
goal:Goal Position [x,y]
obstacleList:obstacle Positions [[x,y,size],...]
randArea:Random Sampling Area [min,max]
"""
super().__init__(start, goal, obstacle_list, rand_area, expand_dis,
goal_sample_rate, max_iter,
robot_radius=robot_radius)
self.connect_circle_dist = connect_circle_dist
self.goal_node = self.Node(goal[0], goal[1])
self.search_until_max_iter = search_until_max_iter
def planning(self, animation=True, camera=None):
"""
rrt star path planning
animation: flag for animation on or off .
"""
self.node_list = [self.start]
for i in range(self.max_iter):
print("Iter:", i, ", number of nodes:", len(self.node_list))
rnd = self.sample_free()
nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
new_node = self.steer(self.node_list[nearest_ind], rnd,
self.expand_dis)
near_node = self.node_list[nearest_ind]
# 计算代价,欧氏距离
new_node.cost = near_node.cost + math.hypot(new_node.x-near_node.x, new_node.y-near_node.y)
if self.obstacle_free(new_node, self.obstacle_list, self.robot_radius):
near_inds = self.find_near_nodes(new_node) # 找到x_new的邻近节点
node_with_updated_parent = self.choose_parent(new_node, near_inds) # 重新选择父节点
# 如果父节点更新了
if node_with_updated_parent:
# 重布线
self.rewire(node_with_updated_parent, near_inds)
self.node_list.append(node_with_updated_parent)
else:
self.node_list.append(new_node)
if animation and i % 5 ==0:
self.draw_graph(rnd, camera)
if ((not self.search_until_max_iter) and new_node): # if reaches goal
last_index = self.search_best_goal_node()
if last_index is not None:
return self.generate_final_course(last_index)
print("reached max iteration")
last_index = self.search_best_goal_node()
if last_index is not None:
return self.generate_final_course(last_index)
return None
def choose_parent(self, new_node, near_inds):
"""
在新产生的节点 $x_{new}$ 附近以定义的半径范围$r$内寻找所有的近邻节点 $X_{near}$，
作为替换 $x_{new}$ 原始父节点 $x_{near}$ 的备选
我们需要依次计算起点到每个近邻节点 $X_{near}$ 的路径代价 加上近邻节点 $X_{near}$ 到 $x_{new}$ 的路径代价，
取路径代价最小的近邻节点$x_{min}$作为 $x_{new}$ 新的父节点
Arguments:
--------
new_node, Node
randomly generated node with a path from its neared point
There are not coalitions between this node and th tree.
near_inds: list
Indices of indices of the nodes what are near to new_node
Returns.
------
Node, a copy of new_node
"""
if not near_inds:
return None
# search nearest cost in near_inds
costs = []
for i in near_inds:
near_node = self.node_list[i]
t_node = self.steer(near_node, new_node)
if t_node and self.obstacle_free(t_node, self.obstacle_list, self.robot_radius):
costs.append(self.calc_new_cost(near_node, new_node))
else:
costs.append(float("inf")) # the cost of collision node
min_cost = min(costs)
if min_cost == float("inf"):
print("There is no good path.(min_cost is inf)")
return None
min_ind = near_inds[costs.index(min_cost)]
new_node = self.steer(self.node_list[min_ind], new_node)
new_node.cost = min_cost
return new_node
def search_best_goal_node(self):
dist_to_goal_list = [self.calc_dist_to_goal(n.x, n.y) for n in self.node_list]
goal_inds = [
dist_to_goal_list.index(i) for i in dist_to_goal_list
if i <= self.expand_dis
]
safe_goal_inds = []
for goal_ind in goal_inds:
t_node = self.steer(self.node_list[goal_ind], self.goal_node)
if self.obstacle_free(t_node, self.obstacle_list, self.robot_radius):
safe_goal_inds.append(goal_ind)
if not safe_goal_inds:
return None
min_cost = min([self.node_list[i].cost for i in safe_goal_inds])
for i in safe_goal_inds:
if self.node_list[i].cost == min_cost:
return i
return None
def find_near_nodes(self, new_node):
"""
1) defines a ball centered on new_node
2) Returns all nodes of the three that are inside this ball
Arguments:
---------
new_node: Node
new randomly generated node, without collisions between
its nearest node
Returns:
-------
list
List with the indices of the nodes inside the ball of
radius r
"""
nnode = len(self.node_list) + 1
r = self.connect_circle_dist * math.sqrt((math.log(nnode) / nnode))
# if expand_dist exists, search vertices in a range no more than
# expand_dist
if hasattr(self, 'expand_dis'):
r = min(r, self.expand_dis)
dist_list = [(node.x - new_node.x)**2 + (node.y - new_node.y)**2
for node in self.node_list]
near_inds = [dist_list.index(i) for i in dist_list if i <= r**2]
return near_inds
def rewire(self, new_node, near_inds):
"""
For each node in near_inds, this will check if it is cheaper to
arrive to them from new_node.
In such a case, this will re-assign the parent of the nodes in
near_inds to new_node.
Parameters:
----------
new_node, Node
Node randomly added which can be joined to the tree
near_inds, list of uints
A list of indices of the self.new_node which contains
nodes within a circle of a given radius.
Remark: parent is designated in choose_parent.
"""
for i in near_inds:
near_node = self.node_list[i]
edge_node = self.steer(new_node, near_node)
if not edge_node:
continue
edge_node.cost = self.calc_new_cost(new_node, near_node)
no_collision = self.obstacle_free(
edge_node, self.obstacle_list, self.robot_radius)
improved_cost = near_node.cost > edge_node.cost
if no_collision and improved_cost:
near_node.x = edge_node.x
near_node.y = edge_node.y
near_node.cost = edge_node.cost
near_node.path_x = edge_node.path_x
near_node.path_y = edge_node.path_y
near_node.parent = edge_node.parent
self.propagate_cost_to_leaves(new_node)
def calc_new_cost(self, from_node, to_node):
d, _ = self.calc_distance_and_angle(from_node, to_node)
return from_node.cost + d
def propagate_cost_to_leaves(self, parent_node):
for node in self.node_list:
if node.parent == parent_node:
node.cost = self.calc_new_cost(parent_node, node)
self.propagate_cost_to_leaves(node)
def main():
print("Start " )
fig = plt.figure(1)
camera = Camera(fig) # 保存动图时使用
# camera = None # 不保存动图时，camara为None
show_animation = True
# ====Search Path with RRT====
obstacle_list = [
(5, 5, 1),
(3, 6, 2),
(3, 8, 2),
(3, 10, 2),
(7, 5, 2),
(9, 5, 2),
(8, 10, 1),
(6, 12, 1),
] # [x,y,size(radius)]
# Set Initial parameters
rrt_star = RRTStar(
start=[0, 0],
goal=[6, 10],
rand_area=[-2, 15],
obstacle_list=obstacle_list,
expand_dis=3,
robot_radius=0.8)
path = rrt_star.planning(animation=show_animation,camera=camera)
if path is None:
print("Cannot find path")
else:
print("found path!!")
# Draw final path
if show_animation:
rrt_star.draw_graph(camera=camera)
plt.plot([x for (x, y) in path], [y for (x, y) in path], 'r--')
plt.grid(True)
if camera!=None:
camera.snap()
animation = camera.animate()
animation.save('trajectory.gif')
plt.show()
if __name__ == '__main__':
main()
实现效果如下：