机器人路径规划

基于python计算从起始节点到目标节点的最短路径，使用Dijkstra算法和A*算法进行比较。最后，它会绘制出这两种算法找到的最短路径。

import networkx as nx
import matplotlib.pyplot as plt
import numpy as np


def generate_node_positions(num_nodes):
    """生成随机节点位置"""
    return np.random.rand(num_nodes, 2)


def create_undirected_graph(node_positions):
    """创建无向图"""
    G = nx.Graph()
    for i in range(len(node_positions)):
        G.add_node(i, pos=(node_positions[i][0], node_positions[i][1]))
    for i in range(len(node_positions)):
        for j in range(i + 1, len(node_positions)):
            distance = np.linalg.norm(node_positions[i] - node_positions[j])
            if distance < 0.2:
                G.add_edge(i, j, weight=distance)
    return G


def calculate_shortest_path(G, start_node, end_node, algorithm='dijkstra'):
    """计算最短路径"""
    if algorithm == 'dijkstra':
        path = nx.dijkstra_path(G, source=start_node, target=end_node)
    elif algorithm == 'astar':
        heuristic = lambda n1, n2: np.linalg.norm(np.array(G.nodes[n1]['pos']) - np.array(G.nodes[n2]['pos']))
        path = nx.astar_path(G, source=start_node, target=end_node, heuristic=heuristic)
    else:
        raise ValueError("Unsupported algorithm")
    return path


def draw_routes(G, dijkstra_path, astar_path):
    """绘制路线，增加图形美化，并排显示Dijkstra和A*路径"""
    pos = nx.get_node_attributes(G, 'pos')
    fig, axs = plt.subplots(1, 2, figsize=(20, 8))  # 创建包含两个子图的窗口

    paths = [dijkstra_path, astar_path]
    titles = ["Dijkstra Path", "A* Path"]

    for ax, path, title in zip(axs, paths, titles):
        # 绘制节点，设置节点大小
        node_sizes = [50 if node in path else 300 for node in G.nodes()]
        nx.draw_networkx_nodes(G, pos, node_size=node_sizes, node_color='lightblue', alpha=0.7, ax=ax)

        # 绘制边，设置透明度
        nx.draw_networkx_edges(G, pos, edgelist=G.edges(), edge_color='gray', alpha=0.5, ax=ax)

        # 突出显示路径边，设置颜色和宽度
        path_edges = list(zip(path, path[1:]))
        nx.draw_networkx_edges(G, pos, edgelist=path_edges, edge_color='r', width=3, alpha=0.9, ax=ax)

        # 绘制标签，设置字体大小和颜色
        nx.draw_networkx_labels(G, pos, font_size=12, font_color='black', ax=ax)

        ax.set_title(title, fontsize=16)
        ax.axis('off')  # 关闭坐标轴

    plt.tight_layout()  # 自动调整子图参数,使之填充整个图像区域
    plt.show()


if __name__ == "__main__":
    num_nodes = 100
    node_positions = generate_node_positions(num_nodes)
    G = create_undirected_graph(node_positions)

    start_node = 1
    end_node = 99

    dijkstra_path = calculate_shortest_path(G, start_node, end_node, 'dijkstra')
    astar_path = calculate_shortest_path(G, start_node, end_node, 'astar')

    draw_routes(G, dijkstra_path, astar_path)