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

成本计算

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

广度优先搜索

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

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

1.将起始节点放入队列中，并标记为已访问。

2.当队列非空时，执行以下步骤：

从队列中取出一个节点，记为当前节点，并标记为已访问。如果该节点是目标节点，则返回结果。将当前节点的所有未访问过的邻居节点放入队列中。

3.如果队列为空，则表示已经遍历完所有可达节点，算法结束。

算法框图

实现效果如下：

广度优先搜索是一种基本的图搜索算法，它按照图的广度方向逐层遍历所有可达节点。然而，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)算法是一种单源最短路径算法，用于在加权图中找到从起点到所有其他节点的最短路径。它基于贪心策略，每次选择当前距离起点最近的节点，并通过该节点更新与它相邻的节点的距离。具体步骤如下：

1.初始化：初始化变量和数据结构，创建一个包含所有节点的集合，并为每个节点设置一个距离值。将起始节点的父节点设置为自身，将起始节点的距离值设置为0，其他节点的距离值设置为无穷大（表示尚未找到最短路径）。将起始节点以成本0的优先级推入优先队列OPEN中。

2.主循环：当OPEN非空时：

弹出优先级最小（成本最低）的节点(_, s)，其中_为忽略的值，s为当前节点。将当前节点s添加到CLOSED列表中，表示已访问。检查当前节点是否为目标节点。如果是，则跳出循环。对于当前节点的所有邻居节点，计算通过当前节点到达邻居节点的距离，并与邻居节点的当前距离值进行比较。如果计算得到的距离值小于邻居节点的当前距离值，则更新邻居节点的距离值为新的更小值，并将邻居节点s_n以新的成本作为优先级推入优先队列OPEN中

3.循环结束后，可以通过从目标节点回溯到起始节点，在PARENT字典中提取最短路径。

算法框图

点击【自动驾驶】路径规划算法Dijkstra与A* - 古月居可查看全文