第2章 · 传感器标定与同步

相机内参标定、IMU零偏与尺度、多传感器时间戳同步——这些是SLAM系统里最基础也最容易翻车的环节。我见过太多团队在标定上省事，结果后端怎么调都救不回来。这一章咱们就把这些硬骨头啃干净。

2.1 相机内参标定（张正友法）

相机内参说白了就是描述“光线怎么穿过镜头映射到像素”的那几个数。焦距、主点、畸变参数——任何一个偏了，你后面三角化出来的点都是歪的。张正友法到今天依然是工业界最稳的方案，用棋盘格拍十几张照片就能搞定。

我个人习惯在标定时多拍几个角度：倾斜、旋转、远近都要覆盖。尤其注意棋盘格要占画面 1/3 以上，不然角点检测容易飘。我遇到过有人只拍了 6 张正对相机的图，结果标出来的畸变参数完全不对，地图直接扭曲。

            核心步骤：

            ① 打印高对比度棋盘格（建议 9×6 内角点）

            ② 从不同角度采集 15~20 张图像

            ③ 提取亚像素角点，计算单应性矩阵

            ④ 利用闭合解求内参，再用最大似然优化

            ⑤ 计算畸变系数（径向 + 切向）

# 伪代码示意 (OpenCV)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
ret, corners = cv2.findChessboardCorners(gray, (9,6), None)
corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)

标定完成后一定要做重投影误差检查。我一般要求平均误差 < 0.3 像素，如果超过 0.5 像素，大概率是图像模糊或棋盘格不平整，重新拍吧。

2.2 IMU零偏与尺度估计

IMU 的零偏（bias）是 VIO 系统里最磨人的小妖精。你想想看，加速度计零偏如果偏了 0.1 m/s²，积分 10 秒位置就漂了好几米。陀螺零偏更致命，角度误差会直接让视觉匹配失效。

我常用的方法是 Allan 方差分析，把 IMU 静止放置 1~2 小时，采集数据后画出 Allan 曲线，读出零偏不稳定性和随机游走。不过在实际工程里，我更依赖在线标定——初始化阶段把 IMU 和视觉对齐，同时估计零偏。

千万不要把 IMU 放在桌面上就以为零偏是固定的。温度变化、振动都会改变 bias。我曾在无人机项目里吃过亏：起飞前标定好好的，飞了 3 分钟 bias 漂了 20%，最后不得不加温度补偿模型。

尺度估计主要针对加速度计和陀螺仪的尺度因子。一般用六面法或转台标定，但如果你没有转台，也可以用多位置静态法。说白了就是让 IMU 在不同姿态下静止，利用重力矢量约束来解尺度。

参数	典型值（消费级IMU）	标定后精度
加速度零偏	±50 mg	< 1 mg
陀螺零偏	±5 °/s	< 0.1 °/s
加速度尺度误差	1~3%	< 0.1%
陀螺尺度误差	0.5~2%	< 0.05%

如果你用 Kalibr 工具箱做联合标定，记得把 IMU 的噪声密度和随机游走先设对，不然优化会陷入局部最优。我一般先跑 Allan 方差拿到先验，再扔给 Kalibr 做视觉-惯性标定。

2.3 多传感器时间戳硬同步与软同步方案

时间同步是 SLAM 系统里最容易忽略但又最致命的一环。相机和 IMU 如果时间差 10 ms，在快速运动下位置误差就能到厘米级。你想想看，视觉特征和 IMU 数据对不上，预积分全白做。

硬同步： 用硬件触发线（比如相机外触发 + IMU 的 PPS 信号）把时间基准统一到纳秒级。我在自动驾驶项目里用过 STM32 输出 PWM 触发相机，同时给 IMU 一个同步脉冲，精度能到 100 μs 以内。但硬同步需要硬件支持，不是所有传感器都有触发接口。

软同步： 没有硬件触发怎么办？靠软件对齐时间戳。常见做法是让所有传感器共用同一个系统时钟（比如 NTP 或 PTP 协议），然后通过插值或优化对齐。我习惯用 IMU 时间戳作为基准，把相机图像时间戳通过线性插值对齐到 IMU 时间轴上。

            软同步常用技巧：

            · 使用 PTP（IEEE 1588）同步多台设备，精度可达亚毫秒级

            · 相机曝光时间中点作为有效时间戳（不是触发时刻）

            · IMU 数据用高精度定时器打戳，避免软件延迟抖动

            · 在优化中把时间偏移作为待估参数（比如 Kalibr 的 time offset）

# 软同步插值示例 (Python)
def sync_imu_to_camera(imu_ts, imu_data, cam_ts):
    # 对每个相机时间戳，找到最近的IMU数据并线性插值
    synced = []
    for ct in cam_ts:
        idx = np.searchsorted(imu_ts, ct)
        if idx == 0 or idx == len(imu_ts):
            continue
        t0, t1 = imu_ts[idx-1], imu_ts[idx]
        ratio = (ct - t0) / (t1 - t0)
        interp_data = imu_data[idx-1] + ratio * (imu_data[idx] - imu_data[idx-1])
        synced.append(interp_data)
    return np.array(synced)

我曾经在一个轮式机器人上只用软件时间戳，结果发现相机驱动和 IMU 驱动用了不同的时钟源（一个用 CLOCK_MONOTONIC，一个用 gettimeofday），偏差最大到 50 ms。后来统一用 clock_gettime(CLOCK_MONOTONIC_RAW) 才解决。记住：所有传感器必须用同一个时钟源！

嗯，这里还要提一句：如果你用 ROS，时间同步可以用 message_filters 的 ApproximateTime 策略，但千万别依赖它做高精度同步。它只能保证时间戳相近，真正的硬同步还是得靠硬件。