8. 从理解到实现：亲手写一个迷你 gmapping

上一篇：7. 实战：跑起来与调参 ｜ 下一篇：9. 局限与进阶：gmapping之外的世界

目标：用 Python 把 第 5 篇 的算法图变成 ~200 行可跑代码。不追求工程性能，追求每一行都能对应到原理。

一、总体框架

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
import numpy as np

class MiniGmapping:
    def __init__(self, n_particles=30, res=0.05, map_size=400):
        self.particles = [Particle() for _ in range(n_particles)]
        self.res = res                        # 栅格分辨率 5cm
        self.grid = np.zeros((map_size, map_size))  # log-odds 占用栅格

    def step(self, odom_delta, scan):
        """每帧调用: 里程计增量 + 一圈激光"""
        for p in self.particles:
            p.pose = self.sample_proposal(p, odom_delta, scan)  # ① 改进提议分布
            p.weight *= self.obs_likelihood(p.pose, scan)       # ② 权重
            self.update_map(p, scan)                             # ③ 建图
        if self.neff() < len(self.particles) * 0.5:              # ④ 改进二
            self.resample()

四个方法恰好就是 第 5 篇 的四个模块。下面逐个填肉。

二、模块①：运动模型 + 改进提议分布

2.1 朴素版（先跑通）：里程计采样

1
2
3
4
5
6
7
8
def sample_motion(self, p, dx, dy, dth):
    # 里程计误差: 平移误差 ∝ 平移量, 旋转误差 ∝ 旋转量
    noise_d  = np.random.normal(0, 0.05 * abs(dx + dy) + 0.01)
    noise_th = np.random.normal(0, 0.03 * abs(dth) + 0.005)
    th = p.pose[2] + dth + noise_th
    return np.array([p.pose[0] + dx*np.cos(th) - dy*np.sin(th) + noise_d*np.cos(th),
                     p.pose[1] + dx*np.sin(th) + dy*np.cos(th) + noise_d*np.sin(th),
                     wrap(th)])

2.2 升级版：加扫描匹配（对应 gmapping 改进一）

1
2
3
4
5
6
7
8
9
10
def sample_proposal(self, p, odom_delta, scan):
    pose0 = self.sample_motion(p, *odom_delta)      # 先按里程计走一步
    best, score = self.scan_match(pose0, scan, p)   # 在粒子的"似然场"上爬山对齐
    if score < min_score:                           # 匹配失败 -> 退回朴素版
        return pose0
    # 在匹配点附近撒 K 个候选, 拟合高斯, 从中采样
    cands = [perturb(best) for _ in range(K)]
    w = [self.score(c, scan, p) * self.motion_prob(c, pose0) for c in cands]
    mu, Sigma = weighted_gaussian_fit(cands, w)     # 加权均值+协方差
    return np.random.multivariate_normal(mu, Sigma) # ⭐ 改进提议分布落地

2.3 扫描匹配：爬山版（~30 行核心）

思路：在网格上小步挪动位姿，找激光得分最高的位置（就是拼图对齐）：

1
2
3
4
5
6
7
8
9
10
11
12
def scan_match(self, pose, scan, p, max_iter=5):
    step = 0.05
    for _ in range(max_iter):
        best_gain, best_move = 0, None
        for dx, dy, dth in [(-step,0,0),(step,0,0),(0,-step,0),(0,step,0),
                            (0,0,-0.03),(0,0,0.03)]:
            cand = pose + np.array([dx, dy, dth])
            gain = self.score(cand, scan, p) - self.score(pose, scan, p)
            if gain > best_gain: best_gain, best_move = gain, (dx,dy,dth)
        if best_move is None: break
        pose = pose + np.array(best_move)
    return pose, self.score(pose, scan, p)

打分函数（似然场思想）：把激光点按位姿投到地图上，离最近的障碍物越近得分越高：

1
2
3
4
5
6
7
def score(self, pose, scan, p):
    s = 0
    for r, a in scan.points:
        hit = pose.transform(r, a)                # 激光点 -> 全局坐标
        d = distance_to_nearest_occupancy(p, hit) # 到粒子地图最近障碍的距离
        s += np.exp(-d**2 / (2*sigma_l**2))       # 高斯核打分
    return s

三、模块③：建图 = Bresenham 光线投射 + log-odds 更新

这是最值得亲手写的一块（第 2 篇讲过原理）：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
OCC, FREE = 0.9, -0.4      # 命中加多少 / 穿过减多少 (log-odds)

def update_map(self, p, scan):
    for r, a in scan.points:
        end = p.pose.transform(r, a)
        for cell in bresenham(p.pose[:2], end):   # 沿光束逐格
            self.grid[cell] += FREE               # 穿过 -> 更空闲
        self.grid[to_cell(end)] += OCC            # 命中 -> 更占用
    np.clip(self.grid, -5, 5, out=self.grid)      # 防饱和

def bresenham(a, b):
    """经典直线栅格化: 返回 a->b 穿过的所有格子"""
    # 标准网格直线算法, ~15 行, 网上一搜即得
    ...

def to_prob(cell):  # log-odds -> [0,1] 占用概率, 画图用
    return 1 - 1/(1 + np.exp(self.grid[cell]))

为什么用 log-odds 而不是概率？ 贝叶斯更新在 log 域就是纯加减法（乘法取对数变加法），且天然夹在 [0,1] 不发散。这也是 第 2 篇 “乘系数”的工程实现。

四、模块②④：权重与重采样（第 3/5 篇直接翻译）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
def obs_likelihood(self, pose, scan):
    # 简化: 直接用扫描匹配得分(改进提议分布下本就近似均匀)
    return np.exp(self.score(pose, scan) - norm_const)

def neff(self):
    w = np.array([p.weight for p in self.particles])
    w /= w.sum()
    return 1.0 / np.sum(w**2)                 # 第3篇公式原样

def resample(self):
    w = normalize([p.weight for p in self.particles])
    idx = np.random.choice(len(self.particles), len(self.particles), p=w)
    self.particles = [deepcopy(self.particles[i]) for i in idx]  # 地图一起复制
    for p in self.particles: p.weight = 1.0 / len(self.particles)

五、怎么验证它 work？

  1. 数据源：ROS bag（/scan + /tf）或 MIT Cartographer 数据集；
  2. 可视化循环：matplotlib 每帧画 栅格图 + 粒子云，肉眼看粒子云收缩、地图长出来；
  3. 对照实验（强烈推荐，加深理解）：
    • 把 sample_proposal 换回朴素 sample_motion -> 观察需要多少粒子才不丢（论文改进一的体感）；
    • 把 Neff 判断删掉、每帧重采样 -> 观察长走廊/对称场景怎么退化（改进二的体感）。

六、优化方向（从”能跑”到”能看”）

优化 对应 gmapping 的设计 收益
地图共享树（copy-on-write） HierarchicalArray2D 重采样内存 ↓↓（第 6 篇）
似然场预计算查找表 ScanMatcher 内部 打分 O(1)/点
活跃区域更新（只更新激光扫到的格子） active area 建图耗时 ↓
多分辨率地图（粗定位+细匹配） gmapping 的 coarse-fine 扫描匹配更快更稳

📚 参考：源代码解析（实现与 openslam 结构对照）