可视图法路径规划python代码

可视图法路径规划是一种常用的路径规划算法，它通过将地图转化为图形表示，并利用图的搜索算法来找到最优路径。下面是一个简单的可视图法路径规划的Python代码示例：

import numpy as np
import matplotlib.pyplot as plt

# 定义地图
map = np.array([
    [0, 0, 0, 0, 0],
    [0, 1, 1, 0, 0],
    [0, 1, 1, 0, 0],
    [0, 0, 0, 0, 0],
])

# 定义起点和终点
start = (0, 0)
goal = (3, 4)

# 定义移动方向（上、下、左、右）
actions = [(-1, 0), (1, 0), (0, -1), (0, 1)]

# 初始化路径
path = []
path.append(start)

# 可视图法路径规划算法
def visualize_planning(map, start, goal):
    queue = []
    queue.append(start)
    visited = set()
    visited.add(start)

    while queue:
        current = queue.pop(0)
        if current == goal:
            break

        for action in actions:
            next_pos = (current[0] + action[0], current[1] + action[1])
            if next_pos[0] >= 0 and next_pos[0] < map.shape[0] and next_pos[1] >= 0 and next_pos[1] < map.shape[1]:
                if map[next_pos[0]][next_pos[1]] == 0 and next_pos not in visited:
                    queue.append(next_pos)
                    visited.add(next_pos)
                    path.append(next_pos)

    if current != goal:
        print("无法找到路径")
    else:
        print("找到路径")

# 调用可视图法路径规划函数
visualize_planning(map, start, goal)

# 绘制地图和路径
plt.imshow(map, cmap='gray')
plt.plot([p[1] for p in path], [p[0] for p in path], 'r')
plt.show()

这段代码实现了一个简单的可视图法路径规划算法。首先定义了一个地图，其中0表示可通行的区域，1表示障碍物。然后定义了起点和终点的坐标。接着使用队列和集合来实现广度优先搜索算法，找到起点到终点的最优路径。最后使用matplotlib库绘制地图和路径。