路径规划最全综述+代码+可视化绘图(Dijkstra算法+A*算法+RRT算法等)-1

简介: 路径规划最全综述+代码+可视化绘图(Dijkstra算法+A*算法+RRT算法等)-1

路径规划综述


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)
    优点:可找到最短路径;适用于无权图。


缺点:时间复杂度高;空间复杂度高。

  1. DFS(Depth-First Searching)
    优点:空间复杂度低。

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

  1. Best-First Searching
    优点:速度快;可以处理启发式信息。

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


  1. Dijkstra’s
    优点:可以找到最短路径;适用于有权图。

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


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

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


  1. Bidirectional A*
    优点:速度快;可以找到最短路径。

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


7.Anytime Repairing A*

优点:可以在任何时候停止搜索并返回最佳路径;可以处理启发式信息。


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

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

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

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

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


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


  1. D* (Dynamic A*)
    优点:可以处理动态环境;可以处理启发式信息。

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


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

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


  1. Anytime D*
    优点:可以在任何时候停止搜索并返回最佳路径;可以处理动态环境;可以处理启发式信息。

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


路径规划最全综述+代码+可视化绘图(Dijkstra算法+A*算法+RRT算法等)-2

https://developer.aliyun.com/article/1446470?spm=a2c6h.13148508.setting.19.68a34f0egwu157

相关文章
|
17天前
|
存储 人工智能 算法
数据结构与算法细节篇之最短路径问题:Dijkstra和Floyd算法详细描述,java语言实现。
这篇文章详细介绍了Dijkstra和Floyd算法,这两种算法分别用于解决单源和多源最短路径问题,并且提供了Java语言的实现代码。
50 3
数据结构与算法细节篇之最短路径问题:Dijkstra和Floyd算法详细描述,java语言实现。
|
1天前
|
算法 测试技术 开发者
在Python开发中,性能优化和代码审查至关重要。性能优化通过改进代码结构和算法提高程序运行速度,减少资源消耗
在Python开发中,性能优化和代码审查至关重要。性能优化通过改进代码结构和算法提高程序运行速度,减少资源消耗;代码审查通过检查源代码发现潜在问题,提高代码质量和团队协作效率。本文介绍了一些实用的技巧和工具,帮助开发者提升开发效率。
7 3
|
13天前
|
存储 缓存 算法
如何通过优化算法和代码结构来提升易语言程序的执行效率?
如何通过优化算法和代码结构来提升易语言程序的执行效率?
|
14天前
|
搜索推荐
插入排序算法的讲解和代码
【10月更文挑战第12天】插入排序是一种基础的排序算法,理解和掌握它对于学习其他排序算法以及数据结构都具有重要意义。你可以通过实际操作和分析,进一步深入了解插入排序的特点和应用场景,以便在实际编程中更好地运用它。
|
24天前
|
算法 数据可视化 新制造
Threejs路径规划_基于A*算法案例完整版
这篇文章详细介绍了如何在Three.js中完整实现基于A*算法的路径规划案例,包括网格构建、路径寻找算法的实现以及路径可视化展示等方面的内容。
49 0
Threejs路径规划_基于A*算法案例完整版
|
6天前
|
缓存 分布式计算 监控
优化算法和代码需要注意什么
【10月更文挑战第20天】优化算法和代码需要注意什么
13 0
|
18天前
|
数据可视化 搜索推荐 Python
Leecode 刷题笔记之可视化六大排序算法:冒泡、快速、归并、插入、选择、桶排序
这篇文章是关于LeetCode刷题笔记,主要介绍了六大排序算法(冒泡、快速、归并、插入、选择、桶排序)的Python实现及其可视化过程。
10 0
|
24天前
|
存储 算法 机器人
Threejs路径规划_基于A*算法案例V2
这篇文章详细介绍了如何在Three.js中使用A*算法进行高效的路径规划,并通过三维物理电路的实例演示了路径计算和优化的过程。
45 0
|
27天前
|
存储 算法 程序员
迪杰斯特拉(Dijkstra)算法(C/C++)
迪杰斯特拉(Dijkstra)算法(C/C++)
|
27天前
|
算法 Java 测试技术
数据结构 —— Java自定义代码实现顺序表,包含测试用例以及ArrayList的使用以及相关算法题
文章详细介绍了如何用Java自定义实现一个顺序表类,包括插入、删除、获取数据元素、求数据个数等功能,并对顺序表进行了测试,最后还提及了Java中自带的顺序表实现类ArrayList。
15 0