基于A*算法的无人车路径规划

在排除掉无人车送快递，以及酒店服务，还有哪些应用场景呢？

猜猜看：

import numpy as np
import heapq
import matplotlib.pyplot as plt
from noise import pnoise2


def generate_random_terrain(width, height, scale=100.0, octaves=6, persistence=0.5, lacunarity=2.0):
    # 生成随机地形
    terrain = np.zeros((height, width))
    for y in range(height):
        for x in range(width):
            terrain[y][x] = pnoise2(x / scale, y / scale, octaves=octaves, persistence=persistence,
                                    lacunarity=lacunarity, repeatx=width, repeaty=height, base=0)
    # 归一化地形高度
    terrain = (terrain - np.min(terrain)) / (np.max(terrain) - np.min(terrain))
    # 将地形高度映射到0-10之间
    terrain = terrain * 10
    return terrain


def add_obstacles(terrain, obstacle_density=0.1):
    # 随机添加障碍物
    obstacles = np.random.rand(*terrain.shape) < obstacle_density
    terrain[obstacles] = -1
    return terrain


def heuristic(node, goal, dem):
    # 启发函数，考虑高程差和障碍物
    distance = np.linalg.norm(np.array(node) - np.array(goal))
    elevation_diff = abs(dem[node[0], node[1]] - dem[goal[0], goal[1]])
    if dem[node[0], node[1]] == -1:
        return np.inf
    return distance + elevation_diff * 0.1


def distance_cost(current, neighbor, dem):
    # 计算从当前节点到邻居节点的移动代价，考虑高程变化
    distance = np.linalg.norm(np.array(neighbor) - np.array(current))
    elevation_diff = abs(dem[neighbor[0], neighbor[1]] - dem[current[0], current[1]])
    if dem[neighbor[0], neighbor[1]] == -1:
        return np.inf
    return distance + elevation_diff * 0.1


def a_star_search(dem, start, goal):
    # 初始化
    open_list = []
    closed_list = np.zeros(dem.shape, dtype=bool)
    came_from = {}
    g_score = np.full(dem.shape, np.inf)
    f_score = np.full(dem.shape, np.inf)
    g_score[start] = 0
    f_score[start] = heuristic(start, goal, dem)
    heapq.heappush(open_list, (f_score[start], start))

    while open_list:
        _, current = heapq.heappop(open_list)

        if np.array_equal(current, goal):
            return reconstruct_path(came_from, current)

        closed_list[current] = True

        # 检查邻居节点
        for dx, dy in [(-1, 0), (1, 0), (0, -1), (0, 1)]:
            neighbor = (current[0] + dx, current[1] + dy)

            if not (0 <= neighbor[0] < dem.shape[0] and 0 <= neighbor[1] < dem.shape[1]):
                continue

            if closed_list[neighbor] or dem[neighbor] == -1:
                continue

            tentative_g_score = g_score[current] + distance_cost(current, neighbor, dem)

            if tentative_g_score < g_score[neighbor]:
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g_score
                f_score[neighbor] = tentative_g_score + heuristic(neighbor, goal, dem)

                if neighbor not in [node[1] for node in open_list]:
                    heapq.heappush(open_list, (f_score[neighbor], neighbor))

    return []  # 未找到路径


def reconstruct_path(came_from, current):
    # 重构路径
    path = []
    while current in came_from:
        path.append(current)
        current = came_from[current]
    path.append(start)
    path.reverse()
    return path


# 生成随机地形
width, height = 100, 100
dem = generate_random_terrain(width, height, scale=100.0, octaves=6, persistence=0.5, lacunarity=2.0)
dem = add_obstacles(dem, obstacle_density=0.1)

start = (10, 10)
goal = (90, 90)

path = a_star_search(dem, start, goal)

# 可视化
plt.figure(figsize=(10, 10))

# 绘制DEM地形图
plt.imshow(dem, cmap='terrain', interpolation='nearest')

# 绘制障碍物
obstacles = np.where(dem == -1)
plt.scatter(obstacles[1], obstacles[0], color='black', marker='s', s=100)

# 绘制路径
if path:
    path_x, path_y = zip(*path)
    plt.plot(path_x, path_y, color='red', linewidth=2, label='Path')
    plt.scatter(start[1], start[0], color='blue', marker='o', s=100, label='Start')
    plt.scatter(goal[1], goal[0], color='green', marker='x', s=100, label='Goal')

# 添加图例
plt.legend()

# 显示图形
plt.title('A* Path Planning on Complex DEM')
plt.xlabel('X')
plt.ylabel('Y')
plt.grid(True)
plt.show()

Matplotlib库将生成的地形、障碍物和寻找到的路径可视化。地形以颜色渐变的形式显示，障碍物以黑色方块表示，而路径则以红色线条表示，起点和终点分别用蓝色圆圈和绿色叉号标记。