路径规划常用算法

一、最短路径算法

最短路径是指从起点到终点距离最短的路径。在路径规划中，最短路径算法是最基本的算法之一。最短路径算法主要分为两类：单源最短路径和多源最短路径。单源最短路径是指从一个给定的起点到其他所有点的最短路径。常见的单源最短路径算法有：

1. Dijkstra算法

function Dijkstra(graph, start, end):
    dist = {start: 0}
    queue = [start]
    while queue:
        vertex = queue.pop(0)
        for neighbor in graph[vertex].keys():
            path = dist[vertex] + graph[vertex][neighbor]
            if neighbor not in dist or path < dist[neighbor]:
                dist[neighbor] = path
                if neighbor == end:
                    return dist[end]
                queue.append(neighbor)
    return -1

Dijkstra算法可以求解从单个源点出发到其他所有节点的最短路径问题，但该算法要求必须所有边的权值必须非负，并且求得的最短路径不一定是全局最优解。

2. Bellman-Ford算法

function BellmanFord(graph, start, end):
    dist = {start: 0}
    for i in range(len(graph) - 1):
        for u in graph.keys():
            for v in graph[u].keys():
                if dist.get(v, float('inf')) > dist[u] + graph[u][v]:
                    dist[v] = dist[u] + graph[u][v]
    for u in graph.keys():
        for v in graph[u].keys():
            if dist.get(v, float('inf')) > dist[u] + graph[u][v]:
                return -1
    return dist[end]

Bellman-Ford算法可处理具有负权边的图，并且可以检测负环。时间复杂度高于Dijkstra算法，通常用于处理边权为负数的情况。

3. A*算法

function AStar(graph, start, end, heuristic):
    closed = set()
    open = {start: 0}
    came_from = {}
    g_score = {start: 0}
    f_score = {start: heuristic(start, end)}
    while open:
        current = min(open, key=open.get)
        if current == end:
            return reconstruct_path(came_from, end)
        del open[current]
        closed.add(current)
        for neighbor in graph[current].keys():
            if neighbor in closed:
                continue
            tentative_g_score = g_score[current] + graph[current][neighbor]
            if neighbor not in open or tentative_g_score < g_score[neighbor]:
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g_score
                f_score[neighbor] = tentative_g_score + heuristic(neighbor, end)
                if neighbor not in open:
                    open[neighbor] = f_score[neighbor]
    return -1

A*算法是一种启发式搜索算法，可以快速找到单源最短路径。该算法使用估价函数来评估节点离目标节点的距离，并将已知最短路径从起点到此节点的代价视为g(x)，并将估计到达目标节点的最短距离视为h(x)，利用f(x) = g(x) + h(x)来评价节点x。

二、最小生成树算法

最小生成树是指包含了给定的无向图中所有节点的生成树，并且树上所有边的边权之和最小。最小生成树算法主要包括：

1. Kruskal算法

function Kruskal(graph):
    mst = []
    sets = {i: {i} for i in graph.keys()}
    edges = [(i, j, weight) for i in graph.keys() for j, weight in graph[i].items() if i < j]
    edges.sort(key=lambda x: x[2])
    for edge in edges:
        u, v, weight = edge
        if sets[u] != sets[v]:
            mst.append(edge)
            sets[u] |= sets[v]
            sets[v] = sets[u]
    return mst

Kruskal算法是一种基于贪心的算法，每一步都选择当前能够加入最小生成树的边，直到得到一棵生成树为止。

2. Prim算法

function Prim(graph):
    mst = []
    closed = set([next(iter(graph.keys()))])
    while len(closed) < len(graph):
        min_edge = (None, None, float('inf'))
        for u in closed:
            for v, weight in graph[u].items():
                if v not in closed and weight < min_edge[2]:
                    min_edge = (u, v, weight)
        mst.append(min_edge)
        closed.add(min_edge[1])
    return mst

Prim算法是一种从定点出发的算法，从当前已经选择进入最小生成树的节点出发寻找到未覆盖节点的最短路径，并将这条最短路径的另一个节点加入集合中。

三、最短路搜索算法

最短路搜索算法是在给定的地图上寻找最短路径的算法，常用于自动驾驶、路径规划等应用场景。最短路搜索算法包括：

1. BFS算法

function BFS(graph, start, end):
    queue = [(start, [start])]
    visited = set()
    while queue:
        (vertex, path) = queue.pop(0)
        if vertex == end:
            return path
        if vertex in visited:
            continue
        visited.add(vertex)
        for neighbor in graph[vertex].keys():
            if neighbor not in visited:
                queue.append((neighbor, path + [neighbor]))
    return -1

BFS算法是一种广度优先搜索算法，每一次搜索都会先遍历当前节点的所有相邻节点，再遍历相邻节点的所有相邻节点，依次扩散，直到找到目标节点为止。

2. DFS算法

function DFS(graph, start, end):
    stack = [(start, [start])]
    visited = set()
    while stack:
        (vertex, path) = stack.pop()
        if vertex == end:
            return path
        if vertex in visited:
            continue
        visited.add(vertex)
        for neighbor in graph[vertex].keys():
            if neighbor not in visited:
                stack.append((neighbor, path + [neighbor]))
    return -1

DFS算法是一种深度优先搜索算法，每次搜索都会尽可能递归地深入到当前节点的子节点，直到达到目标节点或者无法逆行的终点为止。