栅格法路径规划算法python

栅格法路径规划算法是一种基于离散化地图的路径规划方法。在Python中，你可以使用numpy库来处理地图数据，以及使用matplotlib库来可视化结果。下面是一个简单的栅格法路径规划算法的示例代码：

import numpy as np
import matplotlib.pyplot as plt

def grid_path_planning(start, goal, obstacles, map_size):
    # 创建地图
    grid_map = np.zeros(map_size)
    grid_map[start[0], start[1]] = 1
    grid_map[goal[0], goal[1]] = 2
    for obstacle in obstacles:
        grid_map[obstacle[0], obstacle[1]] = -1

    # 栅格法路径规划
    queue = [start]
    visited = set()
    while queue:
        current = queue.pop(0)
        visited.add(current)
        if current == goal:
            break
        neighbors = [(current[0] + 1, current[1]), (current[0] - 1, current[1]),
                     (current[0], current[1] + 1), (current[0], current[1] - 1)]
        for neighbor in neighbors:
            if neighbor[0] < 0 or neighbor[0] >= map_size[0] or neighbor[1] < 0 or neighbor[1] >= map_size[1]:
                continue
            if neighbor in obstacles or neighbor in visited:
                continue
            queue.append(neighbor)
            visited.add(neighbor)
            grid_map[neighbor[0], neighbor[1]] = grid_map[current[0], current[1]] + 1

    # 可视化路径
    path = [goal]
    current = goal
    while current != start:
        neighbors = [(current[0] + 1, current[1]), (current[0] - 1, current[1]),
                     (current[0], current[1] + 1), (current[0], current[1] - 1)]
        for neighbor in neighbors:
            if neighbor[0] < 0 or neighbor[0] >= map_size[0] or neighbor[1] < 0 or neighbor[1] >= map_size[1]:
                continue
            if grid_map[neighbor[0], neighbor[1]] == grid_map[current[0], current[1]] - 1:
                path.append(neighbor)
                current = neighbor
                break

    # 返回路径和地图
    return path, grid_map

# 测试
start = (0, 0)
goal = (5, 5)
obstacles = [(2, 2), (3, 2), (4, 2), (2, 4), (3, 4), (4, 4)]
map_size = (8, 8)
path, grid_map = grid_path_planning(start, goal, obstacles, map_size)

# 可视化结果
plt.imshow(grid_map.T, origin='lower', cmap='Greys')
plt.scatter([start[0]], [start[1]], color='green', marker='o', label='Start')
plt.scatter([goal[0]], [goal[1]], color='red', marker='o', label='Goal')
for obstacle in obstacles:
    plt.scatter([obstacle[0]], [obstacle[1]], color='black', marker='s', label='Obstacle')
plt.plot([point[0] for point in path], [point[1] for point in path], color='blue', linewidth=2, label='Path')
plt.legend()
plt.show()

这段代码演示了一个简单的栅格法路径规划算法的实现，你可以根据需要修改地图的大小、起点、终点以及障碍物位置，然后运行代码查看结果。希望对你有帮助！