栅格法路径规划障碍物碰撞代码

以下是一个简单的栅格法路径规划障碍物碰撞的 Python 代码示例：

import numpy as np

# 定义地图大小和障碍物位置
map_size = (10, 10)
obstacles = [(2, 2), (2, 3), (2, 4), (3, 4), (4, 4), (5, 4), (6, 4), (6, 3), (6, 2)]

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

# 定义栅格地图
grid_map = np.zeros(map_size)
for obstacle in obstacles:
    grid_map[obstacle] = 1

# 定义 A* 算法
def astar(start, goal, grid_map):
    open_list = [start]
    closed_list = []
    g_score = {start: 0}
    f_score = {start: get_heuristic(start, goal)}

    while open_list:
        current = min(open_list, key=lambda x: f_score[x])
        if current == goal:
            return reconstruct_path(start, goal, came_from)
        
        open_list.remove(current)
        closed_list.append(current)

        for neighbor in get_neighbors(current, grid_map):
            if neighbor in closed_list:
                continue
            tentative_g_score = g_score[current] + get_distance(current, neighbor)
            if neighbor not in open_list:
                open_list.append(neighbor)
            elif tentative_g_score >= g_score[neighbor]:
                continue

            came_from[neighbor] = current
            g_score[neighbor] = tentative_g_score
            f_score[neighbor] = tentative_g_score + get_heuristic(neighbor, goal)

    return None

# 定义获取邻居栅格的函数
def get_neighbors(current, grid_map):
    neighbors = []
    for i in range(-1, 2):
        for j in range(-1, 2):
            if i == 0 and j == 0:
                continue
            neighbor = (current[0]+i, current[1]+j)
            if neighbor[0] < 0 or neighbor[0] >= grid_map.shape[0] or neighbor[1] < 0 or neighbor[1] >= grid_map.shape[1]:
                continue
            if grid_map[neighbor] == 1:
                continue
            neighbors.append(neighbor)
    return neighbors

# 定义获取两点之间的距离函数
def get_distance(start, end):
    return np.sqrt((start[0]-end[0])**2 + (start[1]-end[1])**2)

# 定义获取启发式函数的函数
def get_heuristic(start, goal):
    return get_distance(start, goal)

# 定义重构路径的函数
def reconstruct_path(start, goal, came_from):
    path = [goal]
    while path[-1] != start:
        path.append(came_from[path[-1]])
    path.reverse()
    return path

# 运行 A* 算法
came_from = {}
path = astar(start, goal, grid_map)

# 输出路径和地图
print('Path:', path)
print('Map:')
print(grid_map)

在这个代码中，我们首先定义了地图大小和障碍物位置，然后定义了起点和终点。接下来，我们定义了栅格地图，并将障碍物的占据网格视为不可通过的状态。然后，我们定义了 A* 算法，并使用启发式函数和邻居栅格函数来搜索最优路径。最后，我们运行 A* 算法并输出路径和地图。