Dijkstra

图像来自 路径规划与轨迹跟踪系列算法学习_第1讲_Dijkstra算法_哔哩哔哩_bilibili

dijkstra算法可以求从初始点到其余点的最短路径，分为两个数组，一个数组S记录最优路径点，另一个数组U记录各个节点到初始点的距离，通过迭代跟新S,U，直到S中满节点就可以得到最路径。

关键代码

while True:
            open_set, closed_set = dict(), dict() #分别是U和S
            open_set[self.calc_index(start_node)] = start_node
            c_id = min(open_set, key=lambda o: open_set[o].cost)
            current = open_set[c_id]


            if current.x == goal_node.x and current.y == goal_node.y:#是否到达目标
                print("Find goal")
                goal_node.parent_index = current.parent_index
                goal_node.cost = current.cost
                break

            # Remove the item from the open set
            del open_set[c_id]

            # Add it to the closed set
            closed_set[c_id] = current     #加入代价最小的点

            # expand search grid based on motion model
            for move_x, move_y, move_cost in self.motion:  #方向寻找
                node = self.Node(current.x + move_x,
                                 current.y + move_y,
                                 current.cost + move_cost, c_id)
                n_id = self.calc_index(node)

                if n_id in closed_set:  #已经在S集中，代表着已经是最优的
                    continue

                if not self.verify_node(node): #超出地图外
                    continue

                if n_id not in open_set:
                    open_set[n_id] = node  # Discover a new node
                else:
                    if open_set[n_id].cost >= node.cost:
                        # This path is the best until now. record it!
                        open_set[n_id] = node