迪杰斯特拉算法(Dijkstra)是由荷兰计算机科学家狄克斯特拉于1959年提出的，因此又叫狄克斯特拉算法。是从一个顶点到其余各顶点的最短路径算法，解决的是有权图中最短路径问题。迪杰斯特拉算法主要特点是从起始点开始，采用贪心算法的策略，每次遍历到始点距离最近且未访问过的顶点的邻接节点，直到扩展到终点为止。它可以找到从一个起始节点到其他所有节点的最短路径。
一、算法介绍
Dijkstra算法采用贪心策略，通过逐步扩展已知最短路径集合来逐步确定最短路径。它使用一个距离数组来记录从起始节点到其他节点的当前最短距离，并通过不断更新距离数组来逐步确定最短路径。
二、算法描述
1. 创建一个距离数组dist[]，用于记录起始节点到其他节点的当前最短距离。初始化dist[]，将起始节点的距离设为0，其他节点的距离设为无穷大。
2. 创建一个集合visited[]，用于记录已经确定最短路径的节点。
3. 重复以下步骤，直到visited[]包含所有节点：
a. 从未访问的节点中选择一个距离最小的节点u，将其加入visited[]。
b. 对于节点u的所有邻居节点v，更新其距离数组dist[]：
- 如果通过节点u可以获得更短的路径，则更新dist[v]为新的最短距离。
4. 最终，dist[]数组中记录了起始节点到其他所有节点的最短距离。
三、算法流程
1. 初始化dist[]数组和visited[]集合。
2. 将起始节点的距离设为0。
3. 重复以下步骤，直到visited[]包含所有节点：
a. 从未访问的节点中选择一个距离最小的节点u。
b. 将节点u加入visited[]。
c. 对于节点u的所有邻居节点v，更新其距离数组dist[]：
- 如果通过节点u可以获得更短的路径，则更新dist[v]为新的最短距离。
4. 返回dist[]数组作为最短路径结果。
四、部分代码
import matplotlib.pyplot as plt
import math
show_animation = True
class Dijkstra:
def __init__(self, ox, oy, resolution, robot_radius):
"""
Initialize map for a star planning
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
resolution: grid resolution [m]
rr: robot radius[m]
"""
self.min_x = None
self.min_y = None
self.max_x = None
self.max_y = None
self.x_width = None
self.y_width = None
self.obstacle_map = None
self.resolution = resolution
self.robot_radius = robot_radius
self.calc_obstacle_map(ox, oy)
self.motion = self.get_motion_model()
class Node:
def __init__(self, x, y, cost, parent_index):
self.x = x # index of grid
self.y = y # index of grid
self.cost = cost
self.parent_index = parent_index # index of previous Node
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(
self.cost) + "," + str(self.parent_index)
五、部分结果
六、完整Python代码
见下方联系方式
