路径规划最全综述+代码+可视化绘图（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* 是最为常用和经典的算法之一。

优缺点比较

1. BFS（Breadth-First Searching）
   优点：可找到最短路径；适用于无权图。

缺点：时间复杂度高；空间复杂度高。

2. DFS（Depth-First Searching）
   优点：空间复杂度低。
   

缺点：可能会陷入死循环；不一定能找到最短路径。

3. Best-First Searching
   优点：速度快；可以处理启发式信息。

缺点：可能会陷入局部最优解。

4. Dijkstra’s
   优点：可以找到最短路径；适用于有权图。

缺点：时间复杂度高；不能处理负权边。

5. A*
   优点：速度快；可以处理启发式信息；可以找到最短路径。

缺点：可能会陷入局部最优解。

6. Bidirectional A*
   优点：速度快；可以找到最短路径。

缺点：需要存储两个搜索树；可能会出现问题，例如搜索空间过大或搜索树生长过慢。

7. Anytime Repairing A*
   优点：可以在任何时候停止搜索并返回最佳路径；可以处理启发式信息。
   

缺点：可能会陷入局部最优解。

8. LRTA* (Learning Real-time A*)
   优点：可以处理动态环境；可以处理启发式信息。

缺点：需要进行实时计算，可能会导致性能问题。

9. RTAA* (Real-time Adaptive A*)
   优点：可以处理动态环境；可以处理启发式信息。

缺点：需要进行实时计算，可能会导致性能问题。

10. 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

11. D* (Dynamic A*)
    优点：可以处理动态环境；可以处理启发式信息。

缺点：需要存储大量的搜索树。

12. D* Lite
    优点：可以处理动态环境；可以处理启发式信息；空间复杂度低。

缺点：可能会陷入局部最优解。

13. 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*)

等算法。这些算法适用于复杂环境中的路径规划，如机器人导航、无人驾驶和物流配送等领域。

优缺点

1. RRT (Rapidly-Exploring Random Trees)
   优点：适用于高维空间；能够有效处理复杂环境；运算速度较快。
   

缺点：无法保证找到最优解；生成的路径可能不是最短路径。

2. RRT-Connect
   优点：可以保证找到可行路径；适用于多机器人路径规划问题。

缺点：路径质量可能较差；可能收敛速度较慢。

3. Extended-RRT
   优点：能够处理非完整动力学系统；适用于多机器人协同规划。

缺点：路径质量可能较差；运算速度较慢。

4. Dynamic-RRT
   优点：能够处理动态环境中的路径规划问题；适用于移动机器人和无人机等领域。
   

缺点：运算速度较慢；路径质量可能较差。

5. RRT* (Rapidly-Exploring Random Trees Star)
   优点：能够找到最优路径；路径质量较高。

缺点：运算速度较慢；可能需要大量的存储空间。

6. Informed RRT*
   优点：结合了启发式信息，能够加速搜索过程；能够找到近似最优解。

缺点：运算速度较慢；路径质量可能较差。

7. RRT* Smart
   优点：通过智能采样策略提高搜索效率；能够找到最优路径。

缺点：运算速度较慢；路径质量可能较差。

8. Anytime RRT*
   优点：可以在任何时候停止搜索并返回当前的最佳路径；能够找到近似最优解。

缺点：路径质量可能较差；需要进行实时计算。

9. 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()

10. Spline-RRT*
    优点：通过样条插值提高路径质量；能够找到平滑的路径。

缺点：运算速度较慢；可能需要大量的存储空间。

11. Fast Marching Trees (FMT*)
    优点：运算速度快；能够找到最短路径。
    

缺点：路径质量可能较差；在高维空间中效果可能不理想。

12. 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. 路径规划的挑战与未来发展

随着人工智能和计算机技术的不断发展，路径规划领域也面临着一些挑战。例如，在复杂的城市环境中，路径规划需要考虑到人行道、交通信号灯、行人车辆等多种因素，这对算法的精度和实时性提出了更高的要求。未来，路径规划技术可能会结合更多的传感器数据和深度学习技术，以提高路径规划的效率和准确性。

结语

路径规划作为人工智能领域中的重要应用之一，对于实现智能化的交通系统和物流配送具有重要意义。随着技术的不断进步，路径规划算法将会在更多的领域发挥作用，为人们的生活带来便利和安全。