迪杰斯特拉算法(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代码
见下方联系方式
标签:index,路径,dist,Python,self,机器人,算法,节点 From: https://blog.csdn.net/2401_82411023/article/details/136885190