1. Occupancy Grid Map 基础构建
任务目标
本案例使用2D激光雷达扫描数据，通过占据栅格地图（Occupancy Grid Map）算法构建环境地图。你将实现从激光雷达到栅格概率更新的完整链路，理解传感器测量如何转化为栅格单元的占据概率，并最终输出可可视化的地图。
核心原理
占据栅格地图将环境离散化为均匀网格，每个栅格单元存储其被障碍物占据的概率。激光雷达光束传播时：光束经过的栅格单元（射线上的自由空间）降低占据概率，光束终点所在的栅格单元（击中点）提高占据概率。概率更新采用对数几率（log-odds）形式避免数值下溢：
l_new = l_prior + l_measurement其中l = log(p/(1-p))，测量模型赋予击中栅格正更新值、自由栅格负更新值。
环境依赖
需要Python 3.8+，以及numpy、matplotlib库。无需ROS或其他机器人框架，本案例使用模拟激光雷达数据完成闭环。
完整实现
import numpy as np
import matplotlib.pyplot as plt
class OccupancyGridMap:
def __init__(self, width_m, height_m, resolution):
"""初始化栅格地图，坐标原点在地图中心"""
self.resolution = resolution # 米/格
self.width = int(width_m / resolution)
self.height = int(height_m / resolution)
# 使用log-odds存储，0对应概率0.5
self.log_odds = np.zeros((self.height, self.width))
self.origin_x = width_m / 2.0
self.origin_y = height_m / 2.0
# 测量更新参数
self.l_hit = 1.5 # 击中栅格更新值
self.l_free = -0.8 # 自由栅格更新值
self.l_max = 5.0 # 概率上限（log-odds）
self.l_min = -5.0 # 概率下限
def world_to_map(self, x, y):
"""世界坐标转栅格索引"""
mx = int((x + self.origin_x) / self.resolution)
my = int((y + self.origin_y) / self.resolution)
return mx, my
def is_valid(self, mx, my):
return 0 <= mx < self.width and 0 <= my < self.height
def raycast(self, x0, y0, x1, y1):
"""Bresenham直线遍历，返回射线经过的所有栅格坐标（不含终点）"""
mx0, my0 = self.world_to_map(x0, y0)
mx1, my1 = self.world_to_map(x1, y1)
cells = []
dx = abs(mx1 - mx0)
dy = -abs(my1 - my0)
sx = 1 if mx0 < mx1 else -1
sy = 1 if my0 < my1 else -1
err = dx + dy
mx, my = mx0, my0
while True:
if mx == mx1 and my == my1:
break
cells.append((mx, my))
e2 = 2 * err
if e2 >= dy:
err += dy
mx += sx
if e2 <= dx:
err += dx
my += sy
return cells
def update_from_scan(self, robot_x, robot_y, ranges, angle_min, angle_increment):
"""更新一次激光扫描数据"""
for i, r in enumerate(ranges):
if r <= 0 or r > 10: # 跳过无效和超量程点
continue
theta = angle_min + i * angle_increment
hit_x = robot_x + r * np.cos(theta)
hit_y = robot_y + r * np.sin(theta)
# 更新自由栅格（射线经过的单元）
free_cells = self.raycast(robot_x, robot_y, hit_x, hit_y)
for mx, my in free_cells:
if self.is_valid(mx, my):
self.log_odds[my, mx] = np.clip(
self.log_odds[my, mx] + self.l_free, self.l_min, self.l_max)
# 更新击中栅格
hx, hy = self.world_to_map(hit_x, hit_y)
if self.is_valid(hx, hy):
self.log_odds[hy, hx] = np.clip(
self.log_odds[hy, hx] + self.l_hit, self.l_min, self.l_max)
def get_probability_map(self):
"""将log-odds转换为概率值用于显示"""
return 1 - 1 / (1 + np.exp(self.log_odds))
def generate_simulated_scan(robot_x, robot_y, obstacles):
"""生成模拟激光雷达数据：返回距离数组和角度参数"""
angles = np.arange(-np.pi/2, np.pi/2, np.radians(1)) # 180度视场
ranges = []
for theta in angles:
min_dist = 10.0 # 最大量程
for (ox, oy, radius) in obstacles:
# 计算射线与圆的最短距离（简化：射线方向与圆心距离）
dx = ox - robot_x
dy = oy - robot_y
proj = dx * np.cos(theta) + dy * np.sin(theta)
if proj < 0:
continue
perp_dist = abs(-dx * np.sin(theta) + dy * np.cos(theta))
if perp_dist < radius:
dist = proj - np.sqrt(radius**2 - perp_dist**2)
if dist > 0:
min_dist = min(min_dist, dist)
ranges.append(min_dist)
return np.array(ranges), angles[0], angles[1] - angles[0]
# ============ 主程序：构建地图 ============
if __name__ == "__main__":
# 创建地图：10m x 10m，分辨率0.1m
grid_map = OccupancyGridMap(10.0, 10.0, 0.1)
# 定义环境障碍物（圆心坐标，半径）
obstacles = [(2.0, 1.5, 0.4), (-1.5, 2.0, 0.5), (0.0, -2.0, 0.3)]
# 模拟机器人沿轨迹移动并采集数据
trajectory = [(0, 0), (1, 0), (2, 0), (2, 1), (1, 1), (0, 1)]
for rx, ry in trajectory:
ranges, a_min, a_inc = generate_simulated_scan(rx, ry, obstacles)
grid_map.update_from_scan(rx, ry, ranges, a_min, a_inc)
# 输出地图概率并可视化
prob_map = grid_map.get_probability_map()
plt.imshow(prob_map, cmap='gray_r', origin='lower',
extent=[0, 10, 0, 10])
plt.colorbar(label='Occupancy Probability')
plt.xlabel('X (m)')
plt.ylabel('Y (m)')
plt.title('Occupancy Grid Map')
plt.savefig('occupancy_grid.png', dpi=100)
print("地图已保存为 occupancy_grid.png")
print(f"地图尺寸: {grid_map.width} x {grid_map.height} 栅格")
print(f"占据概率范围: [{prob_map.min():.3f}, {prob_map.max():.3f}]")
运行与验证
运行方式：将上述代码保存为occupancy_grid.py，在终端执行：
python occupancy_grid.py验证方法：
- 程序正常结束且无报错，输出地图尺寸为100×100栅格。
- 生成
occupancy_grid.png图像，图中应清晰显示三个高概率（白色）圆形区域，对应三个障碍物位置，周围环境为低概率（黑色）自由空间。 - 障碍物位置与代码中定义一致：
(2.0, 1.5)、(-1.5, 2.0)、(0.0, -2.0)，且圆形轮廓完整。 - 概率值范围在0到1之间，障碍物中心概率接近1.0，远处自由空间接近0.0。
关键说明
本案例使用模拟激光数据验证算法正确性，实际使用时只需将generate_simulated_scan替换为真实雷达驱动接口（如ROS的sensor_msgs/LaserScan），并调用update_from_scan即可。Bresenham射线遍历确保自由空间更新高效准确，log-odds累积方式保证多帧数据融合时数值稳定。若需调整地图分辨率或建图范围，修改OccupancyGridMap初始化参数即可。