机器人 / 系统课时

逆传感器模型如何更新占据概率

逆传感器模型如何更新占据概率

机器人收到激光距离后,需要判断射线经过的哪些栅格可通行、哪里可能有障碍,以及哪些区域尚未观测。本课用一束命中射线和一束无返回射线,推导逆传感器模型的对数几率更新,并运行一个可验证的 Python 小地图,观察重复证据与概率饱和。

学习目标

  • 区分激光束上的空闲格、命中格和未观测格。
  • 用对数几率将多次量测合并为占据概率。
  • 验证无返回处理、更新限幅和未知标记的区别。

激光束怎样提供占据证据

把二维地图划成边长 0.20 m 的栅格,每个格子保存“被障碍占据”的概率。初值取 p0=0.5p_0=0.5,表示没有证据,而不是已经测出一半被占据。

假设激光位于栅格 (0, 0),一束射线向右,在 (3, 0) 命中墙壁。先把量测端点变换到 map 坐标系,再用 Bresenham 等离散射线遍历方法找出经过的格子:

  1. (1, 0)、(2, 0) 位于命中点之前,获得空闲证据。
  2. (3, 0) 是有效回波所在格,获得占据证据。
  3. (4, 0) 在障碍后方,不更新;射线外未扫到的格子也不更新。

若设备明确把某次无回波解释为“在可信有效距离内未探测到障碍”,这束射线可以只更新空闲格,不能把最大距离端点当作命中点。无回波是否可靠,须结合雷达驱动规定;NaN 或越界距离不能直接当作空闲证据。

激光命中与无返回时栅格获得的不同证据

对数几率让多帧更新变成加法

逆传感器模型给出“在当前位姿 xtx_t 与观测 ztz_t 下,一个格子被占据的概率”。常见栅格实现把格子近似独立处理,并用对数几率 LL 存储概率:

L(p)=ln⁡p1−p,p=11+e−LL(p)=\ln\frac{p}{1-p},\qquad p=\frac{1}{1+e^{-L}}

单次观测提供的是相对于初始先验的新增证据,因而递推为:

Lt,i=Lt−1,i+ln⁡p(mi=1∣zt,xt)1−p(mi=1∣zt,xt)−L0L_{t,i}=L_{t-1,i}+\ln\frac{p(m_i=1\mid z_t,x_t)}{1-p(m_i=1\mid z_t,x_t)}-L_0

其中 L0=L(p0)L_0=L(p_0)。若先验为 0.50.5,则 L0=0L_0=0;使用其他先验时,不能省略减去 L0L_0 的项。没有被当前射线观测到的格子不执行更新。

本例设置命中概率 phit=0.7p_{hit}=0.7、空闲概率 pfree=0.3p_{free}=0.3。每次命中让 LL 增加约 0.84730.8473,每次空闲让 LL 减少约 0.84730.8473。一个格子先后命中两次时,L≈1.6946L\approx1.6946,换回概率约为 0.8450.845,并不是简单把两次 0.70.7 相加。

这些数值是演示用的逆模型参数,不代表所有雷达的测量精度。实际配置要结合噪声、反射材质、定位误差和多帧关联进行标定。

用 Python 验证射线更新

以下代码使用 Python 3.9+ 标准库,直接以整数栅格坐标输入射线;不依赖 ROS 2。它把命中前的格子更新为空闲,把有效命中终点更新为占据,并把对数几率限制在 [-2.0, 3.5]。

python
import math

HIT = math.log(0.7 / 0.3)
FREE = math.log(0.3 / 0.7)
LOW, HIGH = -2.0, 3.5
grid = {}
observed = set()

def rasterize(start, end):
    x, y = start
    tx, ty = end
    dx, dy = abs(tx - x), abs(ty - y)
    sx = 1 if x < tx else -1
    sy = 1 if y < ty else -1
    error = dx - dy
    while True:
        yield (x, y)
        if (x, y) == (tx, ty):
            return
        e2 = 2 * error
        if e2 > -dy:
            error -= dy
            x += sx
        if e2 < dx:
            error += dx
            y += sy

def update_cell(cell, delta):
    observed.add(cell)
    grid[cell] = max(LOW, min(HIGH, grid.get(cell, 0.0) + delta))

def update_ray(start, end, hit):
    # 不使用当前射线更新传感器自身所在的起点格
    cells = list(rasterize(start, end))[1:]
    # 仅有真实命中时才为最后一个格子增加占据证据
    for cell in (cells[:-1] if hit else cells):
        update_cell(cell, FREE)
    if hit and cells:
        update_cell(cells[-1], HIT)

def probability(cell):
    return 1 / (1 + math.exp(-grid.get(cell, 0.0)))

if __name__ == "__main__":
    # 两次命中,接着模拟一束已确认无遮挡的无返回射线
    for _ in range(2):
        update_ray((0, 0), (3, 0), True)
    update_ray((0, 0), (3, 1), False)
    for cell in ((3, 0), (1, 0), (3, 1), (0, 2)):
        flag = "已观测" if cell in observed else "未观测"
        print(f"{cell} {flag} p={probability(cell):.3f}")
    for _ in range(10):
        update_ray((0, 0), (3, 0), True)
    print(f"命中饱和 p={probability((3, 0)):.3f}")

预期输出:

text
(3, 0) 已观测 p=0.845
(1, 0) 已观测 p=0.119
(3, 1) 已观测 p=0.300
(0, 2) 未观测 p=0.500
命中饱和 p=0.971

(1, 0) 经三次空闲证据后本应低于 0.1190.119,但被下限截住;(0, 2) 始终没被射线扫到,保持先验。运行时只要保存以上代码为 inverse_grid.py,执行 python inverse_grid.py 即可核对各格概率。

怎样避免地图被错误证据锁死

限幅采用 L←min⁡(3.5,max⁡(−2.0,L+ΔL))L\leftarrow\min(3.5,\max(-2.0,L+\Delta L)),对应约 0.1190.119 至 0.9710.971 的占据概率范围。这样重复击中一面墙不会让占据概率变成严格的 1;墙移走后,新空闲证据还能逐步把它清除。上下限与单次更新强度应当一起调节,不应直接把示例值当成通用配置。

还要分清两个容易混淆的状态:从未观测的格子与观测后正负证据恰好抵消的格子,概率都可能为 0.50.5。因此示例额外维护 observed 集合;按常见 nav_msgs/msg/OccupancyGrid 导出约定,未观测写 -1,已观测可把 pp 映射为 0~100 的占据值,具体范围约定由地图生产者与消费者共同确认。

接入 ROS 2 时,先检查 ROS 2 LaserScan 消息定义 中的 range_min、range_max,剔除无效量测;再按时间戳将激光端点变换到 map 并求栅格索引。若地图出现墙后空闲区、重影墙或运动障碍长期不消失,依次检查射线终点分类、TF 与时间对齐、是否反复累计相关量测,以及对数几率限幅是否过宽。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

逆传感器模型如何更新占据概率

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通