2. 贝叶斯更新在占据栅格中的应用

任务目标

在占据栅格地图中，单个栅格的状态（占据/空闲）需要根据多次传感器测量进行递归更新。本案例实现贝叶斯公式对栅格占据概率的更新，并显式处理传感器噪声（假阳性与假阴性）。最终输出每个栅格在多次测量后的后验占据概率。

核心原理与实现

设栅格占据先验概率为 P(occ)，传感器模型由两个噪声参数描述：p_sensor_occ_given_occ（栅格实际占据时传感器返回占据的概率，即1-假阴性率）和 p_sensor_occ_given_free（栅格实际空闲时传感器返回占据的概率，即假阳性率）。

贝叶斯更新采用对数几率（log-odds）形式避免概率接近0或1时的数值下溢，且计算高效：

l_new = l_prior + l_measurement

其中 l_measurement 根据本次传感器读数（占据或空闲）取不同值。更新后通过 sigmoid 函数将 log-odds 转换回概率。

关键完整代码

import math

class BayesianGridUpdater:
    """对单个栅格进行贝叶斯占据概率更新（log-odds形式）"""
    
    def __init__(self, prior_prob=0.5, p_occ_given_occ=0.9, p_occ_given_free=0.1):
        """
        prior_prob: 先验占据概率
        p_occ_given_occ: 实际占据时传感器返回占据的概率（1-假阴性率）
        p_occ_given_free: 实际空闲时传感器返回占据的概率（假阳性率）
        """
        self.prior = prior_prob
        self.p_occ_given_occ = p_occ_given_occ
        self.p_occ_given_free = p_occ_given_free
        
        # 预计算传感器模型的log-odds
        self.l_occ = math.log(p_occ_given_occ / (1 - p_occ_given_occ))
        self.l_free = math.log(p_occ_given_free / (1 - p_occ_given_free))
        
        # 初始化栅格状态为先验log-odds
        self.l = math.log(prior_prob / (1 - prior_prob))
    
    def update(self, measurement):
        """
        根据一次传感器测量更新栅格状态
        measurement: True表示传感器检测到占据，False表示检测到空闲
        """
        if measurement:
            self.l += self.l_occ
        else:
            self.l += self.l_free
    
    def get_probability(self):
        """将当前log-odds转换回占据概率"""
        return 1.0 / (1.0 + math.exp(-self.l))
    
    def reset(self):
        """重置为先验状态"""
        self.l = math.log(self.prior / (1 - self.prior))

# ========== 最小验证闭环 ==========
if __name__ == "__main__":
    # 场景：真实栅格为占据，传感器噪声参数：假阳性率0.2，假阴性率0.1
    # 即 p_occ_given_occ=0.9, p_occ_given_free=0.2
    grid = BayesianGridUpdater(prior_prob=0.5, p_occ_given_occ=0.9, p_occ_given_free=0.2)
    
    # 模拟10次测量：真实占据下，传感器以0.9概率返回True，0.1概率返回False
    # 这里使用固定序列模拟（非随机），保证可复现
    measurements = [True, True, False, True, True, True, False, True, True, True]
    
    for i, m in enumerate(measurements):
        grid.update(m)
        prob = grid.get_probability()
        print(f"第{i+1:2d}次测量后占据概率: {prob:.4f}")
    
    # 验证：经过多次占据测量后，概率应显著高于先验0.5
    final_prob = grid.get_probability()
    assert final_prob > 0.8, f"期望概率>0.8，实际{final_prob:.4f}"
    print(f"\n最终占据概率: {final_prob:.4f}，验证通过")

运行与验证

运行方式：将上述代码保存为 bayesian_grid.py，在Python 3环境中执行：

python bayesian_grid.py

预期输出与判定标准：

• 程序打印10次更新后的概率值，数值从0.5逐步上升（中间因噪声测量出现小幅回落）。
• 最终概率应大于0.8（本序列中约0.95），断言通过即表示贝叶斯更新正确地将多次含噪测量融合为先验概率的修正。
• 若将 measurements 全部改为 False（模拟空闲栅格），最终概率应趋近于0，可自行修改验证对称性。

关键说明

• log-odds形式将贝叶斯公式的乘法转换为加法，避免概率边界处的数值问题，是占据栅格建图的标准实现方式。
• 传感器噪声参数 p_occ_given_occ 和 p_occ_given_free 需根据实际传感器标定获得，本案例直接给定以聚焦贝叶斯更新本身。
• 本实现针对单个栅格；扩展到多栅格时，对每个栅格独立维护一个 BayesianGridUpdater 实例即可。