机器人 / 系统课时
逆传感器模型如何更新占据概率
逆传感器模型如何更新占据概率
机器人收到激光距离后,需要判断射线经过的哪些栅格可通行、哪里可能有障碍,以及哪些区域尚未观测。本课用一束命中射线和一束无返回射线,推导逆传感器模型的对数几率更新,并运行一个可验证的 Python 小地图,观察重复证据与概率饱和。
学习目标
- 区分激光束上的空闲格、命中格和未观测格。
- 用对数几率将多次量测合并为占据概率。
- 验证无返回处理、更新限幅和未知标记的区别。
激光束怎样提供占据证据
把二维地图划成边长 0.20 m 的栅格,每个格子保存“被障碍占据”的概率。初值取 ,表示没有证据,而不是已经测出一半被占据。
假设激光位于栅格 (0, 0),一束射线向右,在 (3, 0) 命中墙壁。先把量测端点变换到 map 坐标系,再用 Bresenham 等离散射线遍历方法找出经过的格子:
(1, 0)、(2, 0)位于命中点之前,获得空闲证据。(3, 0)是有效回波所在格,获得占据证据。(4, 0)在障碍后方,不更新;射线外未扫到的格子也不更新。
若设备明确把某次无回波解释为“在可信有效距离内未探测到障碍”,这束射线可以只更新空闲格,不能把最大距离端点当作命中点。无回波是否可靠,须结合雷达驱动规定;NaN 或越界距离不能直接当作空闲证据。

对数几率让多帧更新变成加法
逆传感器模型给出“在当前位姿 与观测 下,一个格子被占据的概率”。常见栅格实现把格子近似独立处理,并用对数几率 存储概率:
单次观测提供的是相对于初始先验的新增证据,因而递推为:
其中 。若先验为 ,则 ;使用其他先验时,不能省略减去 的项。没有被当前射线观测到的格子不执行更新。
本例设置命中概率 、空闲概率 。每次命中让 增加约 ,每次空闲让 减少约 。一个格子先后命中两次时,,换回概率约为 ,并不是简单把两次 相加。
这些数值是演示用的逆模型参数,不代表所有雷达的测量精度。实际配置要结合噪声、反射材质、定位误差和多帧关联进行标定。
用 Python 验证射线更新
以下代码使用 Python 3.9+ 标准库,直接以整数栅格坐标输入射线;不依赖 ROS 2。它把命中前的格子更新为空闲,把有效命中终点更新为占据,并把对数几率限制在 [-2.0, 3.5]。
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}")预期输出:
(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, 2) 始终没被射线扫到,保持先验。运行时只要保存以上代码为 inverse_grid.py,执行 python inverse_grid.py 即可核对各格概率。
怎样避免地图被错误证据锁死
限幅采用 ,对应约 至 的占据概率范围。这样重复击中一面墙不会让占据概率变成严格的 1;墙移走后,新空闲证据还能逐步把它清除。上下限与单次更新强度应当一起调节,不应直接把示例值当成通用配置。
还要分清两个容易混淆的状态:从未观测的格子与观测后正负证据恰好抵消的格子,概率都可能为 。因此示例额外维护 observed 集合;按常见 nav_msgs/msg/OccupancyGrid 导出约定,未观测写 -1,已观测可把 映射为 0~100 的占据值,具体范围约定由地图生产者与消费者共同确认。
接入 ROS 2 时,先检查 ROS 2 LaserScan 消息定义 中的 range_min、range_max,剔除无效量测;再按时间戳将激光端点变换到 map 并求栅格索引。若地图出现墙后空闲区、重影墙或运动障碍长期不消失,依次检查射线终点分类、TF 与时间对齐、是否反复累计相关量测,以及对数几率限幅是否过宽。