路径规划最全综述+代码+可视化绘图（Dijkstra算法+A*算法+RRT算法等）
路径规划综述
1. 背景介绍
路径规划是指在给定的环境中找到从起点到终点的最佳路径的过程。它在现实生活中有着广泛的应用，包括无人驾驶、物流配送、机器人导航等领域。随着人工智能和计算机技术的发展，路径规划技术也在不断地得到改进和应用。
路径规划中常见的算法可以分为两类：基于搜索的规划和基于采样的规划。
基于搜索的规划包括
- Breadth-First Searching (BFS)、
- Depth-First Searching (DFS)、
- Best-First Searching、
- Dijkstra’s、
- A*、
- Bidirectional A*、
- Anytime Repairing A*、
- Learning Real-time A* (LRTA*)、
- Real-time Adaptive A* (RTAA*)、
- Lifelong Planning A* (LPA*)、
- Dynamic A* (D*)、
- D* Lite 和
- Anytime D*
等算法。这些算法通过搜索图形结构来找到最短或最优的路径，其中 A* 是最为常用和经典的算法之一。
优缺点比较
- BFS（Breadth-First Searching）
优点：可找到最短路径；适用于无权图。
缺点：时间复杂度高；空间复杂度高。
- DFS（Depth-First Searching）
优点：空间复杂度低。
缺点：可能会陷入死循环；不一定能找到最短路径。
- Best-First Searching
优点：速度快；可以处理启发式信息。
缺点：可能会陷入局部最优解。
- Dijkstra’s
优点：可以找到最短路径；适用于有权图。
缺点：时间复杂度高；不能处理负权边。
- A*
优点：速度快；可以处理启发式信息；可以找到最短路径。
缺点：可能会陷入局部最优解。
- Bidirectional A*
优点：速度快；可以找到最短路径。
缺点：需要存储两个搜索树；可能会出现问题，例如搜索空间过大或搜索树生长过慢。
- Anytime Repairing A*
优点：可以在任何时候停止搜索并返回最佳路径；可以处理启发式信息。
缺点：可能会陷入局部最优解。
- LRTA* (Learning Real-time A*)
优点：可以处理动态环境；可以处理启发式信息。
缺点：需要进行实时计算，可能会导致性能问题。
- RTAA* (Real-time Adaptive A*)
优点：可以处理动态环境；可以处理启发式信息。
缺点：需要进行实时计算，可能会导致性能问题。
- LPA* (Lifelong Planning A*)
优点：可以在不同的时间段进行搜索；可以处理启发式信息。
缺点：需要存储大量的搜索树。
class Node:
def __init__(self, n):
self.x = n[0]
self.y = n[1]
self.parent = None
class RrtStarSmart:
def __init__(self, x_start, x_goal, step_len,
goal_sample_rate, search_radius, iter_max):
self.x_start = Node(x_start)
self.x_goal = Node(x_goal)
self.step_len = step_len
self.goal_sample_rate = goal_sample_rate
self.search_radius = search_radius
self.iter_max = iter_max
self.env = env.Env()
self.plotting = plotting.Plotting(x_start, x_goal)
self.utils = utils.Utils()
self.fig, self.ax = plt.subplots()
self.delta = self.utils.delta
self.x_range = self.env.x_range
self.y_range = self.env.y_range
self.obs_circle = self.env.obs_circle
self.obs_rectangle = self.env.obs_rectangle
self.obs_boundary = self.env.obs_boundary
self.V = [self.x_start]
self.beacons = []
self.beacons_radius = 2
self.direct_cost_old = np.inf
self.obs_vertex = self.utils.get_obs_vertex()
self.path = None
def planning(self):
n = 0
b = 2
InitPathFlag = False
self.ReformObsVertex()
for k in range(self.iter_max):
if k % 200 == 0:
print(k)
if (k - n) % b == 0 and len(self.beacons) > 0:
x_rand = self.Sample(self.beacons)
else:
x_rand = self.Sample()
x_nearest = self.Nearest(self.V, x_rand)
x_new = self.Steer(x_nearest, x_rand)
if x_new and not self.utils.is_collision(x_nearest, x_new):
X_near = self.Near(self.V, x_new)
self.V.append(x_new)
if X_near:
# choose parent
cost_list = [self.Cost(x_near) + self.Line(x_near, x_new) for x_near in X_near]
x_new.parent = X_near[int(np.argmin(cost_list))]
# rewire
c_min = self.Cost(x_new)
for x_near in X_near:
c_near = self.Cost(x_near)
c_new = c_min + self.Line(x_new, x_near)
if c_new < c_near:
x_near.parent = x_new
if not InitPathFlag and self.InitialPathFound(x_new):
InitPathFlag = True
n = k
if InitPathFlag:
self.PathOptimization(x_new)
if k % 5 == 0:
self.animation()
self.path = self.ExtractPath()
self.animation()
plt.plot([x for x, _ in self.path], [y for _, y in self.path], '-r')
plt.pause(0.01)
plt.show()
def PathOptimization(self, node):
direct_cost_new = 0.0
node_end = self.x_goal
while node.parent:
node_parent = node.parent
if not self.utils.is_collision(node_parent, node_end):
node_end.parent = node_parent
else:
direct_cost_new += self.Line(node, node_end)
node_end = node
node = node_parent
if direct_cost_new < self.direct_cost_old:
self.direct_cost_old = direct_cost_new
self.UpdateBeacons()
def UpdateBeacons(self):
node = self.x_goal
beacons = []
while node.parent:
near_vertex = [v for v in self.obs_vertex
if (node.x - v[0]) ** 2 + (node.y - v[1]) ** 2 < 9]
if len(near_vertex) > 0:
for v in near_vertex:
beacons.append(v)
node = node.parent
self.beacons = beacons
def ReformObsVertex(self):
obs_vertex = []
for obs in self.obs_vertex:
for vertex in obs:
obs_vertex.append(vertex)
self.obs_vertex = obs_vertex
def Steer(self, x_start, x_goal):
dist, theta = self.get_distance_and_angle(x_start, x_goal)
dist = min(self.step_len, dist)
node_new = Node((x_start.x + dist * math.cos(theta),
x_start.y + dist * math.sin(theta)))
node_new.parent = x_start
return node_new
def Near(self, nodelist, node):
n = len(self.V) + 1
r = 50 * math.sqrt((math.log(n) / n))
dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in nodelist]
X_near = [nodelist[ind] for ind in range(len(dist_table)) if dist_table[ind] <= r ** 2 and
not self.utils.is_collision(node, nodelist[ind])]
return X_near
def Sample(self, goal=None):
if goal is None:
delta = self.utils.delta
goal_sample_rate = self.goal_sample_rate
if np.random.random() > goal_sample_rate:
return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
return self.x_goal
else:
R = self.beacons_radius
r = random.uniform(0, R)
theta = random.uniform(0, 2 * math.pi)
ind = random.randint(0, len(goal) - 1)
return Node((goal[ind][0] + r * math.cos(theta),
goal[ind][1] + r * math.sin(theta)))
def SampleFreeSpace(self):
delta = self.delta
if np.random.random() > self.goal_sample_rate:
return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
return self.x_goal
- D* (Dynamic A*)
优点：可以处理动态环境；可以处理启发式信息。
缺点：需要存储大量的搜索树。
- D* Lite
优点：可以处理动态环境；可以处理启发式信息；空间复杂度低。
缺点：可能会陷入局部最优解。
- Anytime D*
优点：可以在任何时候停止搜索并返回最佳路径；可以处理动态环境；可以处理启发式信息。
缺点：可能会陷入局部最优解。
基于采样的规划则是利用随机采样的方法来生成路径
其中最常见的算法是
-
RRT、
-
RRT-Connect、
-
Extended-RRT、
-
Dynamic-RRT、
-
RRT*、
-
Informed RRT*、
-
RRT* Smart、
-
Anytime RRT*、
-
Closed-Loop RRT*、
-
Spline-RRT*、
-
Fast Marching Trees (FMT*) 和
-
Batch Informed Trees (BIT*)
等算法。这些算法适用于复杂环境中的路径规划，如机器人导航、无人驾驶和物流配送等领域。
优缺点
- RRT (Rapidly-Exploring Random Trees)
优点：适用于高维空间；能够有效处理复杂环境；运算速度较快。
缺点：无法保证找到最优解；生成的路径可能不是最短路径。
- RRT-Connect
优点：可以保证找到可行路径；适用于多机器人路径规划问题。
缺点：路径质量可能较差；可能收敛速度较慢。
- Extended-RRT
优点：能够处理非完整动力学系统；适用于多机器人协同规划。
缺点：路径质量可能较差；运算速度较慢。
- Dynamic-RRT
优点：能够处理动态环境中的路径规划问题；适用于移动机器人和无人机等领域。
缺点：运算速度较慢；路径质量可能较差。
- RRT* (Rapidly-Exploring Random Trees Star)
优点：能够找到最优路径；路径质量较高。
缺点：运算速度较慢；可能需要大量的存储空间。
- Informed RRT*
优点：结合了启发式信息，能够加速搜索过程；能够找到近似最优解。
缺点：运算速度较慢；路径质量可能较差。
- RRT* Smart
优点：通过智能采样策略提高搜索效率；能够找到最优路径。
缺点：运算速度较慢；路径质量可能较差。
- Anytime RRT*
优点：可以在任何时候停止搜索并返回当前的最佳路径；能够找到近似最优解。
缺点：路径质量可能较差；需要进行实时计算。
- Closed-Loop RRT*
优点：能够处理非完整动力学系统和约束条件；路径质量较高。
缺点：运算速度较慢；可能需要大量的存储空间。
# --------Visualization specialized for dynamic RRT
def visualization(self):
if self.ind % 100 == 0 or self.done:
V = np.array(self.V)
Path = np.array(self.Path)
start = self.env.start
goal = self.env.goal
# edges = []
# for i in self.Parent:
# edges.append([i, self.Parent[i]])
edges = np.array([list(i) for i in self.Edge])
ax = plt.subplot(111, projection='3d')
# ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))
# ax.view_init(elev=0., azim=90.)
ax.view_init(elev=90., azim=0.)
ax.clear()
# drawing objects
draw_Spheres(ax, self.env.balls)
draw_block_list(ax, self.env.blocks)
if self.env.OBB is not None:
draw_obb(ax, self.env.OBB)
draw_block_list(ax, np.array([self.env.boundary]), alpha=0)
draw_line(ax, edges, visibility=0.75, color='g')
draw_line(ax, Path, color='r')
# if len(V) > 0:
# ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', )
ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')
ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')
# adjust the aspect ratio
set_axes_equal(ax)
make_transparent(ax)
# plt.xlabel('s')
# plt.ylabel('y')
ax.set_axis_off()
plt.pause(0.0001)
if __name__ == '__main__':
rrt = dynamic_rrt_3D()
rrt.Main()
- Spline-RRT*
优点：通过样条插值提高路径质量；能够找到平滑的路径。
缺点：运算速度较慢；可能需要大量的存储空间。
- Fast Marching Trees (FMT*)
优点：运算速度快；能够找到最短路径。
缺点：路径质量可能较差；在高维空间中效果可能不理想。
- Batch Informed Trees (BIT*)
优点：通过批量采样提高搜索效率；能够找到最优路径。
缺点：运算速度较慢；可能需要大量的存储空间。
2. 常见的路径规划算法
2.1 Dijkstra算法
Dijkstra算法是一种用于图中寻找最短路径的算法，它可以应用于有向图或无向图。该算法通过不断更新起点到各个顶点的最短路径来找到最终的最短路径。Dijkstra算法的时间复杂度为O(V^2)，其中V为顶点数，但可以通过优先队列实现最小堆来优化时间复杂度。
2.2 A*算法
A算法是一种启发式搜索算法，它结合了Dijkstra算法和贪婪最佳优先搜索算法的优点。A算法通过估计从当前节点到目标节点的代价来动态调整搜索方向，从而更快地找到最佳路径。A*算法在很多实际应用中表现出色，并且具有较高的效率和准确性。
2.3 RRT算法
RRT（Rapidly-exploring Random Tree）算法是一种适用于高维空间的路径规划算法，它通过随机采样和不断扩展树形结构来搜索路径。RRT算法适用于具有复杂空间结构的环境，并且在机器人导航和运动规划中有着广泛的应用。
3. 路径规划在无人驾驶中的应用
无人驾驶技术作为当今人工智能领域的热点之一，路径规划在其中扮演着至关重要的角色。无人驾驶车辆需要通过传感器获取周围环境信息，并利用路径规划算法来决定车辆的行驶路线，以确保安全和高效地到达目的地。由于道路交通环境的复杂性，路径规划算法需要考虑到实时交通状况、障碍物避让、交通规则等因素，因此对路径规划算法的要求也更加严格。
4. 路径规划在物流配送中的应用
随着电商行业的快速发展，物流配送成为了一个备受关注的领域。路径规划在物流配送中的应用不仅可以提高配送效率，还可以降低成本。通过合理的路径规划，配送车辆可以在最短的时间内覆盖更多的配送点，从而提高送货效率。同时，路径规划算法还需要考虑到配送点的时效性、交通拥堵情况等因素，以提供最优的配送方案。
5. 路径规划的挑战与未来发展
随着人工智能和计算机技术的不断发展，路径规划领域也面临着一些挑战。例如，在复杂的城市环境中，路径规划需要考虑到人行道、交通信号灯、行人车辆等多种因素，这对算法的精度和实时性提出了更高的要求。未来，路径规划技术可能会结合更多的传感器数据和深度学习技术，以提高路径规划的效率和准确性。
结语
路径规划作为人工智能领域中的重要应用之一，对于实现智能化的交通系统和物流配送具有重要意义。随着技术的不断进步，路径规划算法将会在更多的领域发挥作用，为人们的生活带来便利和安全。