精选【自动驾驶】路径规划算法Dijkstra与A*

Spgroc

发布时间 2023.06.20阅读数 2892 评论数 7

在本文中，我们将主要介绍Dijkstra算法和A*算法，从成本计算的角度出发，并逐步展开讨论。我们将从广度优先搜索开始，然后引入Dijkstra算法，与贪心算法进行比较，最终得出A*算法。

成本计算

在路径规划中，成本计算的一个主要因素是距离。距离可以作为一种衡量路径长短的度量指标，通常使用欧几里得距离、曼哈顿距离或其他合适的距离度量方法来计算。本文主要介绍欧几里得距离与曼哈顿距离。

欧几里得距离（Euclidean distance）是一种常用的距离度量方法，用于衡量两个点之间的直线距离。欧几里得距离可以在二维或多维空间中计算，针对二维空间的两个点 $A(X_A,Y_A)$ , $B(X_B,Y_B)$ ，其计算公式如下：

曼哈顿距离，也叫出租车或城市街区距离，因为网格的限制，要考虑所有行驶的方向。与欧几里得距离不同，曼哈顿距离是通过在坐标轴上的垂直和水平线段之和来计算距离。在路径规划中，曼哈顿距离经常被用作成本计算的指标之一，特别适用于离散的网格地图或需要沿街区移动的情况。对于二维空间中的两个点两个点 $A(X_A,Y_A)$ , $B(X_B,Y_B)$ ,曼哈顿距离计算公式为：

广度优先搜索

广度优先搜索（Breadth First Search，BFS ）是一种图遍历算法，按照广度方向逐层遍历所有可达节点。

BFS的基本思想是通过维护一个队列，逐层访问节点。具体步骤如下：

将起始节点放入队列中，并标记为已访问。
当队列非空时，执行以下步骤：
- 从队列中取出一个节点，记为当前节点，并标记为已访问。
- 如果该节点是目标节点，则返回结果。
- 将当前节点的所有未访问过的邻居节点放入队列中。
如果队列为空，则表示已经遍历完所有可达节点，算法结束。

算法框图

算法框图BFS

实现效果如下：

result

广度优先搜索是一种基本的图搜索算法，它按照图的广度方向逐层遍历所有可达节点。然而，BFS并不考虑边的权重，它只关注节点的层级关系。因此，对于成本计算来说，BFS并不适用。这里为了实现到目标点的搜索，采用了曼哈顿距离计算初始点的行进成本。

代码

def searching(self):
    """
        Breadth-first Searching.
        :return: path, visited order
        """

    self.PARENT[self.s_start] = self.s_start  # 开始节点的父节点
    self.g[self.s_start] = 0  # 开始节点的成本
    self.g[self.s_goal] = math.inf  # 目标节点的成本
    # 统一成本搜索，起点的成本是0
    heapq.heappush(self.OPEN,
                   (0, self.s_start))

    while self.OPEN:
        _, s = heapq.heappop(self.OPEN)  # 弹出最小的元素，优先级较高
        self.CLOSED.append(s)  # 将节点加入被访问元素队列，已访问

        if s == self.s_goal:  # 到达目标点，即停止
            break

            for s_n in self.get_neighbor(s):  # 得到s的邻居节点
                new_cost = self.g[s] + self.cost(s, s_n)  
                # 计算当前邻居节点s_n的成本=g(s)节点s的成本+s到s_n之间的成本

                if s_n not in self.g:  # 当前节点没有访问过
                    self.g[s_n] = math.inf  # 起点到节点s_n的成本为无穷

                    if new_cost < self.g[s_n]:  # conditions for updating Cost
                        self.g[s_n] = new_cost
                        self.PARENT[s_n] = s

                        # bfs, add new node to the end of the openset
                        # 将新的节点添加到队列的末尾
                        prior = self.OPEN[-1][0] + 1 if len(self.OPEN) > 0 else 0
                        heapq.heappush(self.OPEN, (prior, s_n))
                        self.f[s_n] = prior

                        return self.extract_path(self.PARENT), self.CLOSED, self.f

Dijkstra算法

迪杰斯特拉算法(Dijkstra)算法是一种单源最短路径算法，用于在加权图中找到从起点到所有其他节点的最短路径。它基于贪心策略，每次选择当前距离起点最近的节点，并通过该节点更新与它相邻的节点的距离。具体步骤如下：

初始化：初始化变量和数据结构，创建一个包含所有节点的集合，并为每个节点设置一个距离值。将起始节点的父节点设置为自身，将起始节点的距离值设置为0，其他节点的距离值设置为无穷大（表示尚未找到最短路径）。将起始节点以成本0的优先级推入优先队列OPEN中。
主循环：当OPEN非空时：
- 弹出优先级最小（成本最低）的节点(_, s)，其中_为忽略的值，s为当前节点。
- 将当前节点s添加到CLOSED列表中，表示已访问。
- 检查当前节点是否为目标节点。如果是，则跳出循环。
- 对于当前节点的所有邻居节点，计算通过当前节点到达邻居节点的距离，并与邻居节点的当前距离值进行比较。如果计算得到的距离值小于邻居节点的当前距离值，则更新邻居节点的距离值为新的更小值，并将邻居节点s_n以新的成本作为优先级推入优先队列OPEN中
循环结束后，可以通过从目标节点回溯到起始节点，在PARENT字典中提取最短路径。

算法框图

Dijkstra算法流程图

实现效果如下：

result

Dijkstra算法能够正确地找到起始节点到其他所有节点的最短路径。它基于贪婪策略，每次选择当前最短路径的节点，通过逐步更新节点的距离值，最终找到最短路径。

代码

    def searching(self):
        """
        Breadth-first Searching.
        :return: path, visited order
        """

        self.PARENT[self.s_start] = self.s_start  # 开始节点的父节点
        self.g[self.s_start] = 0  # 开始节点的成本
        self.g[self.s_goal] = math.inf  # 目标节点的成本

        # 统一成本搜索，起点的成本是0
        heapq.heappush(self.OPEN,
                       (0, self.s_start))

        while self.OPEN:  # open_list
            _, s = heapq.heappop(self.OPEN)  # 弹出最小的元素，优先级较高
            self.CLOSED.append(s)  # 将节点加入被访问元素队列

            if s == self.s_goal:  # 到达目标点，即停止
                break

            for s_n in self.get_neighbor(s):  # 得到s的邻居节点
                new_cost = self.g[s] + self.cost(s, s_n)  # 计算当时邻居节点s_n的成本=g(s)节点s的成本+s到s_n之间的成本

                if s_n not in self.g:   # 当前节点没有访问过
                    self.g[s_n] = math.inf  # 起点到节点s_n的成本为无穷

                if new_cost < self.g[s_n]:  # 预估节点s_n成本<g(s_n)从起始节点到节点s_n的成本
                    self.g[s_n] = new_cost  # 更新成本
                    self.PARENT[s_n] = s  # s_n的父节点为s

                    # best first set the heuristics as the priority
                    # 将到当前节点的成本设为优先级排序指标
                    heapq.heappush(self.OPEN, (new_cost, s_n))
                    self.f[s_n] = new_cost

        return self.extract_path(self.PARENT), self.CLOSED, self.f

贪婪算法：

贪婪算法（Greedy Algorithm）是一种常见的算法设计策略，其基本思想是在每一步选择当前最优解，而不考虑整体的最优解。贪婪算法通常以局部最优解为目标，通过不断做出局部最优选择来达到整体最优解。贪婪算法在路径规划问题中，根据当前位置到目标位置的成本作为启发式评估准则，选择最近的节点作为下一步移动的目标。具体步骤如下：

初始化：设置起始节点，将起始节点的父节点设置为起始节点本身，并将起始节点和目标节点的成本初始化为无穷大，将起始节点加入开放列表，其优先级根据启发式函数值确定。
主循环：当OPEN非空时：
- 从OPEN列表中弹出具有最高优先级的节点，将其加入已访问列表（CLOSED）中。
- 检查当前节点是否为目标节点。如果是，则跳出循环。
- 获取当前节点的邻居节点，从邻居节点中选择距离目标节点最近的节点，将选择的节点加入OPEN列表，并将该节点作为当前节点。
循环结束后，通过从目标节点回溯到起始节点，在PARENT字典中提取最短路径。

算法框图

贪婪算法流程图

实现效果如下：

result

贪婪最佳优先搜索算法的局限性在于它过度依赖启发式函数（heuristic function），该函数用于估计节点到目标节点的距离。由于启发式函数的估计可能不准确或不全面，算法可能会在搜索过程中陷入局部最优解，导致得到的路径并不是最短的。

代码

  def searching(self):
        self.PARENT[self.s_start] = self.s_start  # 开始节点的父节点
        self.h[self.s_start] = math.inf  # 开始节点的成本
        self.h[self.s_goal] = math.inf  # 目标节点的成本
        # heappush 函数能够按照 h 值的大小来维护堆的顺序，这意味着self.OPEN堆中的节点将按照 h 值的升序排列，h 值较小的节点将具有较高的优先级。
        heapq.heappush(self.OPEN,
                       (self.heuristic(self.s_start), self.s_start))

        while self.OPEN:  # 当不为空时，即存在未探索区域
            _, s = heapq.heappop(self.OPEN)  # 弹出最小的元素，优先级较高
            self.CLOSED.append(s)  # 将节点加入被访问元素队列

            if s == self.s_goal:  # stop condition，到达目标点，即停止
                break
            for s_n in self.get_neighbor(s):  # 得到s的邻居节点
                new_cost = self.heuristic(s_n) + self.cost(s, s_n)  # 计算当时邻居节点s_n的成本=g(s)节点s的成本+s到s_n之间的成本

                if s_n not in self.h:  # 下一个节点没有遍历过
                    self.h[s_n] = math.inf  # 起点到节点s_n的成本为无穷

                if new_cost < self.h[s_n]:  # 预估节点s_n成本<g(s_n)从起始节点到节点s_n的成本
                    self.h[s_n] = new_cost  # 更新成本函数
                    self.PARENT[s_n] = s  # s_n的父节点为s
                    heapq.heappush(self.OPEN, (self.heuristic(s_n), s_n))  # 把成本h放入堆栈
                    self.h[s_n] = self.heuristic(s_n)

        return self.extract_path(self.PARENT), self.CLOSED, self.h

A*算法

Dijkstra算法没有考虑到目标节点的位置，因此可能会浪费时间在探索那些与目标节点相距较远的方向上。贪婪最佳优先搜索算法会优先选择离目标节点更近的节点进行扩展。这样做的好处是它能够更快地找到到达目标节点的路径，但无法保证找到的路径是最短路径，因为它只考虑了节点到目标节点的距离，没有综合考虑到起点到目标节点的实际距离。

A*算法是一种综合了Dijkstra算法和贪婪最佳优先搜索的启发式搜索算法。A*算法同时使用了节点到起点的实际距离（表示为g值）和节点到目标节点的估计距离（表示为h值）。它通过综合考虑这两个值来评估节点的优先级，并选择优先级最高的节点进行扩展。A算法通过选择合适的启发式函数来平衡搜索的速度和路径的优劣。当启发式函数满足一定条件时，A算法能够保证找到最短路径。

Dijkstra与贪婪搜索算法对比

在路径规划中，贪婪算法关注的是当前节点到目标节点的距离（启发式函数值），它倾向于选择离目标节点最近的节点作为下一步。Dijkstra算法关注的是从起点到各个节点的距离，通过不断更新节点的最短距离来逐步扩展路径。

A*算法的成本函数是由两部分组成：g(n)和h(n)。

g(n)表示从起点到达节点n的实际距离（也称为已知最短路径的代价），表示为g(n)。——Dijkstra
h(n)表示从节点n到目标节点的预估距离（也称为启发式函数），表示为h(n)。——贪婪搜索

A算法使用这两个值来评估节点的优先级。具体地，A算法为每个节点计算一个估计总代价f(n)，计算公式为：

其中，f(n)表示从起点经过节点n到达目标节点的预估总代价。

frame_path

具体步骤如下：

初始化：设置起始节点，将起始节点的父节点设置为起始节点本身，将起始节点的成本设置为0，将目标节点的成本设置为无穷大，将起始节点加入到OPEN列表中，使用节点的f值作为优先级。
主循环：当OPEN非空时：
- 从OPEN列表中弹出具有最高优先级的节点，将其加入已访问列表（CLOSED）中。
- 检查当前节点是否为目标节点。如果是，则跳出循环。
- 获取当前节点的邻居节点。
- 对于每个邻居节点，执行以下步骤：
  - 计算从起始节点经过当前节点到达邻居节点的实际距离，即g值。
  - 如果邻居节点不在g字典中，将其g值初始化为无穷大。
  - 如果计算得到的g值小于邻居节点的当前g值，更新邻居节点的g值为新的更小值，并将当前节点设为邻居节点的父节点。
  - 计算邻居节点的启发式函数值，即h值。
  - 将邻居节点加入OPEN列表，并根据f值（f = g + h）确定其优先级。
循环结束后，通过从目标节点回溯到起始节点，在PARENT字典中提取最短路径。

算法框图

A_算法流程图

实现效果如下：

A*算法的效率和质量受启发式函数的选择影响较大。合理选择启发式函数能够提供更好的搜索引导，但不同问题可能需要设计不同的启发式函数。

代码

 def searching(self):
        """
        A_star Searching.
        :return: path, visited order
        """

        self.PARENT[self.s_start] = self.s_start  # 开始节点的父节点
        self.g[self.s_start] = 0  # 开始节点的成本
        self.g[self.s_goal] = math.inf  # 目标节点的成本

        # heappush 函数能够按照 f 值的大小来维护堆的顺序，这意味着self.OPEN堆中的节点将按照 f 值的升序排列，f 值较小的节点将具有较高的优先级。
        heapq.heappush(self.OPEN,
                       (self.f_value(self.s_start), self.s_start))

        while self.OPEN:  # 当不为空时，即存在未探索区域
            _, s = heapq.heappop(self.OPEN)  # 弹出最小的元素，优先级较高
            self.CLOSED.append(s)  # 将节点加入被访问元素队列

            if s == self.s_goal:  # stop condition，到达目标点，即停止
                break
            for s_n in self.get_neighbor(s):  # 得到s的邻居节点
                new_cost = self.g[s] + self.cost(s, s_n)  # 计算当时邻居节点s_n的成本=g(s)节点s的成本+s到s_n之间的成本

                if s_n not in self.g:
                    self.g[s_n] = math.inf  # 起点到节点s_n的成本为无穷

                if new_cost < self.g[s_n]:  # 预估节点s_n成本<g(s_n)从起始节点到节点s_n的成本
                    self.g[s_n] = new_cost  # 更新成本函数
                    self.PARENT[s_n] = s  # s_n的父节点为s
                    heapq.heappush(self.OPEN, (self.f_value(s_n), s_n))  # 把成本h+g放入队列
                    self.h[s_n] = self.heuristic(s_n)
        return self.extract_path(self.PARENT), self.CLOSED, self.g, self.h

参考

A*（A星）算法介绍：https://www.bilibili.com/read/cv17763077?spm_id_from=333.1007.0.0
PathPlanning：https://github.com/zhm-real/PathPlanning

A*与JPS寻路算法的实现：http://xiexuefeng.cc/lab/369.html
Introduction to the A* Algorithm：https://www.redblobgames.com/pathfinding/a-star/introduction.html

路径规划自动驾驶 Dijkstra A*

原创文章作者： Spgroc 。如若转载，请注明出处：古月居 https://www.guyuehome.com/43539

打赏 2

上一篇：使用强化学习与障碍函数完成避障任务

下一篇：【控制算法】ADRC自抗扰控制

精选【自动驾驶】路径规划算法Dijkstra与A*

Spgroc

成本计算

广度优先搜索

算法框图

实现效果如下：

代码

Dijkstra算法

算法框图

实现效果如下：

代码

贪婪算法：

算法框图

实现效果如下：

代码

A*算法

Dijkstra与贪婪搜索算法对比

算法框图

实现效果如下：

代码

参考

为你推荐

2022年最新成果——Patchwork++地面分割

多传感器融合综述---FOV与BEV

精选hrust工程化学习(六)----最近邻搜索

关于机器人状态估计(12)-VIO/VSLAM的稀疏与稠密

【自动驾驶】强化学习DDPG玩赛车游戏

CUDA入门和网络加速学习(四)

评论（7）

关于作者

Spgroc

24

0

399

6

精选【机器人】PyBullet四足机器狗仿真

精选【Tello无人机】实物轨迹跟踪控制

【自动驾驶】Carsim无人车横向LQR控制仿真

相关推荐

精选经典文献阅读之--Vision-based Large-scale 3D Semantic Mapping...(自动驾驶的大规模三维视觉语义地图的构建)

自动驾驶|3D目标检测：AVOD(二)

图像处理基础篇-形态学处理-边缘检测(matlab仿真与图像处理系列第4期）

精选myAgv智能移动底盘的slam算法学习以及动态避障

经典文献阅读之--NeRF-SLAM（单目稠密重建）

车辆运动学建模(二)

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

TA的专栏

控制算法

强化学习

自动驾驶

机器人

专栏导航

谢谢大爷打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板