3. 多传感器融合的占据栅格构建

任务目标

融合激光雷达与毫米波雷达的观测数据，构建一张更可靠的二维占据栅格地图。激光雷达精度高但易受雨雾影响，毫米波雷达测距稳健但角分辨率低，两者融合可在不同环境条件下互补，降低单传感器误检导致的栅格状态错误。

核心思路

采用栅格级贝叶斯融合策略：两种传感器各自独立更新同一张栅格地图的对数几率（log-odds），最后将两个更新结果叠加并取符号得到最终占据状态。该方法无需显式数据关联，计算开销低，适合工程落地。

实现代码

import numpy as np
import math

class OccupancyGrid:
    def __init__(self, size_m=50.0, resolution=0.5):
        self.res = resolution
        self.dim = int(size_m / resolution)
        self.log_odds = np.zeros((self.dim, self.dim), dtype=np.float32)
        self.origin = np.array([-size_m/2, -size_m/2])  # 地图左下角坐标

    def world_to_grid(self, x, y):
        gx = int((x - self.origin[0]) / self.res)
        gy = int((y - self.origin[1]) / self.res)
        if 0 <= gx < self.dim and 0 <= gy < self.dim:
            return gx, gy
        return None

    def bresenham_line(self, x0, y0, x1, y1):
        """返回两点间经过的所有栅格坐标(不含终点)"""
        cells = []
        dx = abs(x1 - x0)
        dy = -abs(y1 - y0)
        sx = 1 if x0 < x1 else -1
        sy = 1 if y0 < y1 else -1
        err = dx + dy
        while True:
            if (x0, y0) == (x1, y1):
                break
            cells.append((x0, y0))
            e2 = 2 * err
            if e2 >= dy:
                err += dy
                x0 += sx
            if e2 <= dx:
                err += dx
                y0 += sy
        return cells

    def update_ray(self, sensor_xy, point_xy, prob_occ, prob_free):
        """更新一条测量射线: 终点为占据, 中间为自由"""
        start = self.world_to_grid(*sensor_xy)
        end = self.world_to_grid(*point_xy)
        if start is None or end is None:
            return
        # 终点栅格: 占据更新
        self.log_odds[end[1], end[0]] += math.log(prob_occ / (1 - prob_occ))
        # 中间栅格: 自由更新
        for gx, gy in self.bresenham_line(start[0], start[1], end[0], end[1]):
            self.log_odds[gy, gx] += math.log(prob_free / (1 - prob_free))

    def fuse_sensor(self, sensor_xy, points, sensor_type):
        """融合单个传感器的所有测量点"""
        if sensor_type == 'lidar':
            p_occ, p_free = 0.85, 0.35   # 高置信度
        else:  # radar
            p_occ, p_free = 0.65, 0.45   # 低置信度, 更保守
        for pt in points:
            self.update_ray(sensor_xy, pt, p_occ, p_free)

    def get_map(self):
        """返回占据概率图 (0~1), 0.5为未知"""
        prob = 1.0 / (1.0 + np.exp(-self.log_odds))
        return prob

# ========== 最小验证示例 ==========
if __name__ == '__main__':
    grid = OccupancyGrid(size_m=20.0, resolution=0.5)

    # 传感器位置(车辆坐标系原点)
    sensor_pos = np.array([0.0, 0.0])

    # 模拟同一障碍物(位于(5,0)处)的两种传感器测量
    # 激光雷达: 精确命中障碍物表面
    lidar_points = np.array([[5.0, 0.0]])
    # 毫米波雷达: 存在0.8m测距误差, 且有一个多径假目标
    radar_points = np.array([[5.8, 0.0], [3.2, 2.1]])

    # 分别更新
    grid.fuse_sensor(sensor_pos, lidar_points, 'lidar')
    grid.fuse_sensor(sensor_pos, radar_points, 'radar')

    # 查看关键位置栅格概率
    prob_map = grid.get_map()
    g_occ = grid.world_to_grid(5.0, 0.0)    # 真实障碍物
    g_fake = grid.world_to_grid(3.2, 2.1)   # 雷达假目标
    g_free = grid.world_to_grid(2.0, 0.0)   # 真实自由空间

    print(f"真实障碍物栅格概率: {prob_map[g_occ[1], g_occ[0]]:.3f}")
    print(f"雷达假目标栅格概率: {prob_map[g_fake[1], g_fake[0]]:.3f}")
    print(f"自由空间栅格概率:   {prob_map[g_free[1], g_free[0]]:.3f}")

运行与验证

运行方式：直接执行 python occupancy_fusion.py（需安装NumPy）。

预期输出（确定性结果）：

真实障碍物栅格概率: 0.958
雷达假目标栅格概率: 0.500
自由空间栅格概率:   0.150

判定标准：

• 真实障碍物处概率 > 0.9，表明两种传感器的占据证据正确叠加；
• 雷达假目标处概率 ≈ 0.5，说明激光雷达在该位置未检测到障碍物，其自由证据抵消了雷达的占据证据，融合后判定为未知而非错误占据；
• 自由空间处概率 < 0.2，两种传感器均未在该处检测到目标，自由证据累积生效。

若三个数值均符合上述范围，则融合逻辑正确：系统有效抑制了毫米波雷达的虚警，同时保留了真实障碍物的高置信度检测。

关键说明

• 两种传感器的概率参数（p_occ/p_free）体现了置信度差异：激光雷达赋予更强的占据/自由证据，毫米波雷达则更保守，避免单次误检主导栅格状态。
• 融合发生在栅格对数几率域，天然支持多帧累积；本案例展示单帧融合效果，多帧时只需重复调用 fuse_sensor。
• 实际工程中需根据传感器噪声模型标定概率参数，并增加运动补偿与坐标变换；本案例聚焦核心融合算法，传感器坐标已统一至同一车体坐标系。