怎么测距：针孔加地平面

针孔模型：像素与射线的一一对应

针孔相机把三维世界投影到二维图像。设相机坐标系 $O_c X_c Y_c Z_c$，光心在原点，光轴沿 $+Z_c$，图像平面在 $Z_c = f$（焦距）处。空间点 $\mathbf{p}^c = [X_c, Y_c, Z_c]^\top$ 投影到归一化像平面：

$$\mathbf{u}_{\text{norm}} = \begin{bmatrix} X_c / Z_c \\ Y_c / Z_c \\ 1 \end{bmatrix}$$

再乘以内参矩阵 $K$ 得到像素坐标：

$$\mathbf{u} = K \,\mathbf{u}_{\text{norm}}, \qquad K = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}$$

其中 $f_x, f_y$ 是以像素为单位的焦距，$c_x, c_y$ 是主点。测距是反过来用：已知一个像素 $\mathbf{u}$，先解出归一化方向 $\mathbf{u}_{\text{norm}} = K^{-1}\mathbf{u}$，就得到一条从光心射出的射线方向。这就是”像素反投影成射线”。

地平面假设：射线求交得到距离

单条射线只能定方向，定不了深度（沿射线任何位置都投影到同一像素）。补上”障碍物站在地面上”这个几何约束，深度就被钉死了。

设相机光心离地高度 $h$，相机相对地面的俯仰角 $\theta$（光轴向下倾 $\theta$，水平为 0）。地面在相机系里的方程是：

$$Y_c = -h + (\text{光轴倾角修正}) \;\Longleftrightarrow\; \text{地平面法向 } \mathbf{n} \cdot (\mathbf{p}^c - \mathbf{o}) = 0$$

像素反投影得到的射线 $\mathbf{r}(t) = \mathbf{o} + t\,\mathbf{d}$（$\mathbf{d} = K^{-1}\mathbf{u}$ 已归一化方向，$t \ge 0$ 是深度参数）。射线与地平面相交：

$$\mathbf{n}\cdot(\mathbf{o} + t\,\mathbf{d} - \mathbf{o}_\text{ground}) = 0 \;\Longrightarrow\; t = \frac{-\mathbf{n}\cdot(\mathbf{o} - \mathbf{o}_\text{ground})}{\mathbf{n}\cdot\mathbf{d}}$$

简化到二维侧视（只看高度-前向平面），设射线相对水平方向的俯角为 $\alpha$，则有经典三角关系：

$$d = \frac{h}{\tan\alpha}$$

这里 $d$ 是障碍物”脚”的前向水平距离。只用”脚”这个点，所以障碍物无论多高多大都不进入公式——这是该方法简洁的根本原因。

为什么这个方法对姿态极度敏感

关键在 $\alpha$。远处的物体俯角 $\alpha$ 很小，射线几乎贴着地面掠过。对 $d = h / \tan\alpha$ 微分（$\alpha$ 用弧度）：

$$\frac{\partial d}{\partial \alpha} = -\frac{h}{\sin^2\alpha} \quad\Longrightarrow\quad |\Delta d| \approx \frac{h}{\sin^2\alpha}\,|\Delta\alpha|$$

$\alpha \to 0$ 时 $\sin\alpha \to 0$，$\Delta d$ 被平方级放大。$\alpha$ 由”相机俯仰角 + 像素在画面里的垂直位置”共同决定——其中相机俯仰角来自 IMU。俯仰差零点几度，远处距离就能偏出好几米，这是后续所有标定误差分析的源头。

易错点

• 地平面假设只在物体确实落地时成立。脚被遮挡、悬空物体、上下坡、台阶，都会让 $d = h/\tan\alpha$ 失真——它不是普适测距，是带强假设的测距。
• 相机相对地面的俯仰角是动态量。车辆加减速、颠簸都会让车身（ hence 相机）俯仰变化，地面”在哪里”必须用 IMU 实时重算，标定一次是不够的。
• 内参与外参哪个错都会让 $\alpha$ 错：内参错让射线方向 $\mathbf{d}$ 偏，外参错让”地面方程”摆错位置，二者殊途同归地污染 $d$。

参考

• 针孔相机模型（维基百科）
• Camera matrix / intrinsic matrix

测距准不准，就看标定准不准

误差放大的总账

地平面测距的核心公式（见 B04）：

$$d = \frac{h}{\tan\alpha}, \qquad \alpha = \theta_{\text{cam}} + \alpha_{\text{pixel}}$$

其中 $\theta_{\text{cam}}$ 是相机俯仰角（IMU 给出），$\alpha_{\text{pixel}}$ 是该像素相对光轴的俯角（由内参和像素位置算出）。误差来源有三类，破坏 $h$、$\theta_{\text{cam}}$、$\alpha_{\text{pixel}}$ 中的任一个，都会让 $d$ 出错。

三类误差，谁最致命

标定项	影响的量	量级敏感性
相机内参 $K$、畸变	$\alpha_{\text{pixel}}$ 方向偏	远处被放大，但单像素角度误差有限
相机安装高度 $h$	分子 $h$	线性：$h$ 错 1%，$d$ 错 1%，远近一视同仁
相机俯仰角 $\theta_{\text{cam}}$	分母角度	平方级：远处爆炸性放大

俯仰角之所以最敏感，是因为它直接进入分母的 $\tan$。对全微分：

$$\Delta d \approx \frac{1}{\tan\alpha}\,\Delta h \;-\; \frac{h}{\sin^2\alpha}\,\Delta\theta$$

第二项在 $\alpha \to 0$（远处）时趋向无穷。这就是”远处的射线几乎贴着地面掠过，俯仰角差一度，距离就可能偏出好几米”的数学来源。

一个直觉数字

取 $h = 1$ m，远处像素对应 $\alpha = 1° \approx 0.0175$ rad。若俯仰角误差 $\Delta\theta = 0.5° \approx 0.0087$ rad：

$$|\Delta d| \approx \frac{h}{\sin^2\alpha}\,|\Delta\theta| \approx \frac{1}{(0.0175)^2}\times 0.0087 \approx 28 \text{ m}$$

（线性近似在 $\alpha$ 很小时偏夸张，但量级已经说明问题：俯仰误差在远处是放大器，不是乘法器。）这也是为什么后续对 IMU 姿态、相机-IMU 外参的标定精度要求都压在零点几度甚至更小。

易错点

• 不要把”高度误差”和”俯仰误差”混为一谈。高度误差是线性的、各距离等比例；俯仰误差是非线性的、随距离平方级放大。标定资源应优先砸在俯仰上。
• 内参误差看似温和，但远距离时一个小角度误差也能折算成大距离误差，且畸变在画面边缘更明显——远处的像素常落在画面边缘。

参考

• 单目距离估计的尺度敏感性（perspective-n-point 思想）

坐标系全景

这套系统里距离最终要在”地面系”里表达，而原始测量分别落在 IMU 体坐标系和两个相机坐标系里。标定要做的，就是把这些系两两对齐。

四个坐标系

坐标系	记号	谁的测量落在这里
地面系 / 世界系	$W$（或 $G$，ground）	最终距离、位置
IMU 体坐标系	$B$（body）	加速度计、陀螺仪原始输出
相机 0 系	$C_0$（cam0）	cam0 的像素反投影射线
相机 1 系	$C_1$（cam1）	cam1 的像素反投影射线

坐标系变换的记法

把一个点从 $A$ 系变到 $B$ 系，用一个齐次变换 $T_{BA} \in SE(3)$：

$$\begin{bmatrix} \mathbf{p}^B \\ 1 \end{bmatrix} = T_{BA} \begin{bmatrix} \mathbf{p}^A \\ 1 \end{bmatrix} = \begin{bmatrix} R_{BA} & \mathbf{t}_{BA} \\ \mathbf{0}^\top & 1 \end{bmatrix} \begin{bmatrix} \mathbf{p}^A \\ 1 \end{bmatrix}$$

其中 $R_{BA} \in SO(3)$ 是 3×3 旋转，$\mathbf{t}_{BA} \in \mathbb{R}^3$ 是平移。下标读法：$T_{BA}$ 表示”从 $A$ 系到 $B$ 系的变换”，即把 $A$ 系下表达的坐标转成 $B$ 系下的坐标。它的逆 $T_{BA}^{-1} = T_{AB}$。

这套系统里要标定的变换链

要把 cam0 的射线落到地面系，需要把链乘起来：

$$T_{W C_0} = T_{W B}\, T_{B C_0}$$

• $T_{W B}$：IMU 体坐标系到地面系。$R_{WB}$ 就是 IMU 实时解算出的姿态（俯仰/横滚/航向），$\mathbf{t}$ 里包含相机离地高度等信息。这是动态量，每一帧都不同。
• $T_{B C_0}$：相机 0 相对 IMU 的安装位姿。这是静态量，相机和 IMU 刚性固定在同一块板上，一次标定终身使用（除非松动）。这就是相机-IMU 联合标定要解的”空间外参”。

cam1 同理：$T_{W C_1} = T_{W B}\, T_{B C_1}$。两相机之间的相对外参 $T_{C_0 C_1} = T_{B C_0}^{-1}\, T_{B C_1}$ 也可由此推出。

标定到底在求什么

把上面拆开看，标定分三层：

1. 各传感器自己的参数：相机内参 $K$ 与畸变；IMU 的零偏、标度、轴向（IMU”内参”，见 B10/B13）和噪声参数（Allan 方差，见 B11）。
2. 静态空间外参：$T_{B C_0}$、$T_{B C_1}$（相机-IMU），$T_{C_0 C_1}$（相机-相机）。这些是 $SE(3)$ 里的固定元素。
3. 时间偏移 $t_d$：两个传感器时钟之间的固定延迟，让”同一时刻”对得上。

第 2、3 项是这套联合标定的主角。把它们求准了，$T_{W C_0}$ 才能正确把 cam0 射线摆到地面系，地平面求交得到的距离才可信。

易错点

• 下标顺序是最大坑。$T_{AB}$ 和 $T_{BA}$ 是互逆，写反一次整条链就废。养成习惯：变换链写成 $T_{C A} = T_{C B}\, T_{B A}$，相邻下标必须”咬合”（$B$ 接 $B$）。
• 旋转和平移的地位不对称。对地平面测距，旋转外参（决定射线朝向）远比平移外参（决定光心位置）敏感——因为 $h$ 量级是米而 $d$ 量级也是米，平移误差最多贡献米级；旋转误差却被 $\sin^{-2}\alpha$ 放大（见 B05）。
• $T_{WB}$ 是动态的，不要把它当成标定结果。标定给的是 $T_{BC}$，运行时再用 IMU 实时姿态去拼出当前帧的 $T_{WC}$。

参考

• 刚体变换与齐次坐标
• 旋转群 SO(3) 与欧氏群 SE(3)

相机内参与两相机的角色

内参矩阵 K 决定”像素对应哪条射线”

相机内参把归一化像平面坐标映射到像素坐标（见 B04）：

$$\mathbf{u} = K \,\mathbf{u}_{\text{norm}}, \qquad K = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}$$

• $f_x, f_y$：以像素为单位的焦距。物理焦距 $f$（毫米）除以像元尺寸（毫米/像素）得到 $f_x = f / d_x$、$f_y = f / d_y$。
• $c_x, c_y$：主点（光轴穿过图像的位置），通常接近图像中心但不严格在中心。

反投影求射线方向时求逆：$\mathbf{u}_{\text{norm}} = K^{-1}\mathbf{u}$。$K$ 是上三角，逆存在且容易算：

$$K^{-1} = \begin{bmatrix} 1/f_x & 0 & -c_x/f_x \\ 0 & 1/f_y & -c_y/f_y \\ 0 & 0 & 1 \end{bmatrix}$$

射线方向就是 $\mathbf{d} = [u_{\text{norm},x},\, u_{\text{norm},y},\, 1]^\top$（再归一化）。内参错了，所有像素的射线方向就一起偏。

畸变：把直的拍弯

真实镜头不完美，针孔模型之外还要加畸变。最常用的是径向+切向模型（Brown-Conrady / OpenCV 的 distCoeffs）：

$$\begin{aligned} x_{\text{dist}} &= x(1 + k_1 r^2 + k_2 r^4 + k_3 r^6) + 2 p_1 x y + p_2(r^2 + 2x^2) \\ y_{\text{dist}} &= y(1 + k_1 r^2 + k_2 r^4 + k_3 r^6) + p_1(r^2 + 2y^2) + 2 p_2 x y \end{aligned}$$

其中 $(x, y)$ 是归一化坐标，$r^2 = x^2 + y^2$；$k_1, k_2, k_3$ 是径向畸变（鱼眼、桶形/枕形），$p_1, p_2$ 是切向畸变（镜头与传感器不平行）。边缘像素畸变最大，而地平面测距关心的远处障碍物恰好常落在画面边缘——所以畸变必须标，否则远处射线方向偏得离谱。

畸变方向：标定是反着用的，要把检测到的畸变像素 $\mathbf{u}$ 先去畸变回理想的 $\mathbf{u}_{\text{norm}}$，再求射线。鱼眼镜头要用等距/全向模型（如 Kannala-Brandt，Kalibr 里叫 equidistant/equi）。

两个相机为什么朝不同方向

这套系统的两个相机视野几乎不重叠，是为了拼出更大的水平视野（周视），不是为了双目立体。各看一片区域、各做各的地平面测距。

但那点小重叠也不是没用：标定时让标定板同时出现在两相机画面里，就能求出 $T_{C_0 C_1}$（相机间相对外参），这正是多相机标定的串联原理（见 kalibr 项目）。

易错点

• 内参和相机装在哪无关，只跟镜头+传感器有关。换镜头、对焦、变焦（如果可变）都要重标。固定焦距镜头一般一次标定长期有效，除非受剧烈温度/冲击。
• 主点 $(c_x, c_y)$ 不是图像几何中心，标定才准。把它默认设成 $(W/2, H/2)$ 会引入系统偏差。
• 鱼眼镜头硬套 pinhole-radtan 模型，画面边缘的射线方向永远偏——畸变模型选错是常见翻车点。

参考

• 相机内参与针孔模型
• 镜头畸变（径向/切向，Brown-Conrady）
• OpenCV 相机标定文档

IMU 内部测什么

IMU（Inertial Measurement Unit，惯性测量单元）里其实是两种独立的微机电传感器，各管一类量。

加速度计：测”比力”，不是纯加速度

加速度计测量的是比力（specific force）$\mathbf{f}^B$，即”非引力加速度”。它在 IMU 体坐标系 $B$ 下输出。关键关系：

$$\mathbf{f}^B = \mathbf{a}^B - \mathbf{g}^B$$

其中 $\mathbf{a}^B$ 是相对惯性系的运动加速度（在 $B$ 系下的表达），$\mathbf{g}^B$ 是重力在 $B$ 系下的表达。负号是约定（“比力”定义为 $\mathbf{a} - \mathbf{g}$，也有教材用 $\mathbf{g} - \mathbf{a}$，要看符号约定）。

最易错的点：静止时 $\mathbf{a}=0$，加速度计输出的是 $-\mathbf{g}^B$，即一个向上的、大小为 $g\approx9.8\,\text{m/s}^2$ 的”加速度”。这是因为加速度计感受到支撑它抵抗重力的反力。所以”静止时它读出重力”。

正因为静止时能”看到”重力方向，加速度计可以定出俯仰角（pitch，绕 $Y$ 轴）和横滚角（roll，绕 $X$ 轴）——这两个角决定了重力在 IMU 系里的分解。但它无法定航向角（yaw，绕 $Z$ 轴），因为绕重力轴转，重力方向不变。

陀螺仪：测角速度

陀螺仪测量 IMU 体坐标系相对惯性系的角速度 $\boldsymbol{\omega}^B = [\omega_x, \omega_y, \omega_z]^\top$（单位 rad/s）。姿态要通过积分得到：

$$R_{WB}(t) \approx R_{WB}(0)\,\exp\big(\boldsymbol{\omega}^B \,\Delta t\big)$$

（$\exp$ 是 $SO(3)$ 上的矩阵指数，详见 B09。）陀螺仪擅长捕捉快速变化，但积分会漂移：常数零偏 $\boldsymbol{\epsilon}$ 积分后变成随时间线性增长的姿态误差 $\Delta\theta = \boldsymbol{\epsilon}\,t$。

为什么要把两者融合

传感器	擅长	弱点
加速度计	长期定重力方向（俯仰、横滚）准	短期受运动加速度污染，分不清”重力”和”加速”
陀螺仪	短期动态准、不受运动加速度干扰	长期积分漂移

两者互补：长期靠加速度计”拉回”重力方向，短期靠陀螺仪补上快速变化。这就是互补滤波、Mahony、Madgwick、以及基于 EKF 的姿态融合的共同思路。融合输出的姿态 $R_{WB}$，是地平面测距最关键的输入。

易错点

• 加速度计读的不是纯运动加速度。想从它得到真实加速度，要加上重力 $\mathbf{a} = \mathbf{f} + \mathbf{g}$。直接拿读数做二次积分定位会爆炸性发散。
• 静止时加速度计定不了 yaw。所有靠重力定姿态的方法（互补滤波等）对 yaw 都无约束，yaw 必须靠陀螺积分或磁力计/视觉。
• 加速度计”分不清重力还是加速”在车辆加减速时尤其严重——这时俯仰角估计要更信任陀螺仪短时积分。

参考

• Inertial measurement unit (维基百科)
• 比力 / specific force
• MEMS 加速度计与陀螺仪原理

IMU 怎么给出姿态

姿态是 3D 旋转 $R_{WB} \in SO(3)$，表示 IMU 体坐标系 $B$ 相对世界系 $W$（这里取重力对齐的系）的朝向。从 IMU 算出 $R_{WB}$ 有两条互补的路径。

路径一：加速度计定重力方向（定俯仰、横滚）

静止或匀速时运动加速度 $\mathbf{a}\approx 0$，加速度计读数 $\mathbf{f}^B \approx -\mathbf{g}^B$，即重力的反向。世界系里重力沿 $-Z_W$，所以 $\mathbf{g}^W = [0,0,-g]^\top$。把它转到 $B$ 系：

$$\mathbf{g}^B = R_{WB}^{-1}\,\mathbf{g}^W = R_{WB}^\top\,\mathbf{g}^W$$

读出 $\mathbf{f}^B = [f_x, f_y, f_z]^\top$ 后（取 $-\mathbf{f}^B \approx \mathbf{g}^B$），可直接解俯仰角 $\phi$（pitch）和横滚角 $\psi$（roll）：

$$\phi = \arctan2\!\left(-f_x,\; \sqrt{f_y^2 + f_z^2}\right), \qquad \psi = \arctan2\!\left(f_y,\; f_z\right)$$

（符号约定随轴定义而变，原理一致：用重力的两个分量反推两个姿态角。）这条路径长期稳定（重力永远在那），但短期受运动加速度污染——车辆一加速，$\mathbf{a}\ne 0$，$\mathbf{f}$ 里混入运动项，解出的角就不准。

路径二：陀螺仪积分（短期准，长期漂）

陀螺仪给角速度 $\boldsymbol{\omega}^B$。姿态更新用 $SO(3)$ 上的指数映射：

$$R_{WB}(t+\Delta t) = R_{WB}(t)\,\exp\!\big(\boldsymbol{\omega}^B\,\Delta t\big)$$

其中 $\exp(\cdot)$ 把角速度×时间（一个旋转向量）转成旋转矩阵。若旋转向量为 $\boldsymbol{\theta} = \boldsymbol{\omega}\,\Delta t$，模长 $\theta = \|\boldsymbol{\theta}\|$，方向 $\mathbf{k} = \boldsymbol{\theta}/\theta$（罗德里格斯公式 Rodrigues’ formula）：

$$\exp(\boldsymbol{\theta}) = I + \sin\theta\,[\mathbf{k}]_\times + (1-\cos\theta)\,[\mathbf{k}]_\times^2$$

$[\mathbf{k}]_\times$ 是 $\mathbf{k}$ 的反对称矩阵。陀螺积分短期非常准（几百毫秒内几乎无误差），但陀螺有零偏和噪声，积分误差随时间累积：常数零偏 $\boldsymbol{\epsilon}$ 积出 $\Delta R \propto \boldsymbol{\epsilon}\,t$ 的姿态漂移。

融合：互补滤波的本质

两条路径频域上互补——加速度计低频准（重力是 DC 信号）、陀螺高频准。互补滤波（complementary filter）用一个高通加一个低通：

$$\hat{R}_{WB} \;\xleftarrow{\text{更新}}\; \text{陀螺积分预测} \;+\; k\cdot\big(\text{加速度计观测的重力方向} - \text{预测的重力方向}\big)$$

直觉：长期相信加速度计（拉回真实重力），短期相信陀螺（追上快速变化）。Mahony、Madgwick、基于 EKF 的 AHRS 都是这个思路的不同实现（EKF 还能把零偏当状态一起估）。前提是 IMU 本身是准的——零偏标掉、标度正确，否则融合从源头就被污染。

易错点

• 欧拉角有奇异性（gimbal lock）。俯仰接近 ±90° 时 roll/yaw 退化，工程上要么用四元数，要么用旋转矩阵做更新，最后再转欧拉角给人看。
• 加速度计定姿态的前提是”运动加速度小”。猛烈加减速时不能信加速度计，要临时加大对陀螺的信任权重——这正是融合算法自动做的事。
• 陀螺零偏不是常数，会随温度、开机漂（见 B10/B11）。不标定就积分，姿态几秒到几分钟内就会偏到不可用。

参考

• 互补滤波与姿态估计
• 罗德里格斯旋转公式（Rodrigues’ rotation formula）
• Mahony & Hamel 互补滤波论文

IMU 的误差项

理想的 IMU 在静止、无运动时应输出零角速度、零加速度（除重力）。真实 MEMS IMU 远不完美，输出要乘一个”误差模型”才能还原真值。把陀螺和加速度计的真实值记作 $\boldsymbol{\omega}, \mathbf{f}$，测量值记作 $\tilde{\boldsymbol{\omega}}, \tilde{\mathbf{f}}$，通用模型是：

$$\tilde{\mathbf{m}} = \mathbf{T}\,\mathbf{S}\,\big(\mathbf{m}_{\text{真}} + \mathbf{b} + \mathbf{n}\big)$$

各项分别对应下面四类误差。

四类典型误差

误差	记号	物理含义	后果
零偏 bias	$\mathbf{b}$	输入为零时输出不为零	最致命：陀螺零偏积分 → 姿态线性漂
标度误差 scale	$\mathbf{S} = \text{diag}(s_x,s_y,s_z)$	真实值乘个比例才等于输出	大角速度/大加速度时误差放大
轴不对齐 misalignment	$\mathbf{T}$（非对角项）	三轴非理想正交，互相串扰	一个轴的运动污染另一个轴
随机噪声 noise	$\mathbf{n}$	每次读数在真值附近抖	高频抖动，需滤波/融合抑制

$\mathbf{S}$ 是对角标度矩阵，$\mathbf{T}$ 是接近单位阵的轴对齐矩阵（非对角项很小）。两者合起来 $\mathbf{M} = \mathbf{T}\mathbf{S}$ 是一个”内参矩阵”，描述传感器的确定性失真。完整模型常写成：

$$\tilde{\mathbf{m}} = \mathbf{M}\,(\mathbf{m}_{\text{真}} + \mathbf{b}) + \mathbf{n}$$

为什么零偏最致命

零偏 $\mathbf{b}$ 是常数（或慢变）偏移。对陀螺仪，姿态由角速度积分得到（见 B09）：

$$\Delta\theta_{\text{误差}} = \int_0^t \boldsymbol{\epsilon}\,d\tau = \boldsymbol{\epsilon}\,t$$

一个没标掉的常数零偏 $\boldsymbol{\epsilon}$ 让估计姿态随时间线性漂移。哪怕只有 $0.01$ rad/s（约 $0.57°/\text{s}$），10 秒就偏 $5.7°$。而 B05 已说明：俯仰角误差在地平面测距里被 $\sin^{-2}\alpha$ 放大，远处距离爆炸性偏离。所以标陀螺零偏是 IMU 标定的头号任务。

标度误差和轴不对齐是乘性的，只在运动幅度大时才显著，但它们同样要标——尤其是廉价 MEMS，标度误差能到 1% 量级。

噪声：随机但可统计

随机噪声 $\mathbf{n}$ 通常是高斯白噪声（见 B11 的 Allan 方差建模）。它单次读数抖动大，但积分/平均会减小（白噪声方差随 $1/N$ 下降），所以融合滤波能压制它。它不像零偏那样累积，危险性低于零偏。

标定要做什么

确定性误差（$\mathbf{b}, \mathbf{S}, \mathbf{T}$）是可标定、可扣除的——只要测出来，运行时从读数里减掉/除掉即可。随机噪声（$\mathbf{n}$）不可扣除，只能建模其统计参数（Allan 方差），让融合算法知道该多信任 IMU。这两类标定分别见 B13 和 B11。

易错点

• 零偏会随温度和时间变化（turn-on to turn-on 漂、in-run 漂），开机一次标定不能终身使用。好的做法是把零偏当运行时状态，用 EKF/VIO 在线估计。
• 标度误差是乘性的，只在信号大时才暴露。低动态场景标定可能漏掉，要在转台或多姿态激励下才能激发。
• 陀螺零偏污染姿态，加速度计零偏污染重力方向估计（hence 俯仰/横滚）——两个传感器的零偏都不能放过。

参考

• IMU 误差模型（bias / scale factor / misalignment）
• MEMS 陀螺仪误差源

噪声模型与 Allan 方差

IMU 的噪声不是一个数，而是不同时间尺度上不同性质的随机过程。Allan 方差（Allan variance） 是把它们逐个分离、量化的标准工具。

为什么需要 Allan 方差

普通方差无法区分这些噪声：白噪声和零偏随机游走都让读数”看起来在抖”，但它们的危害完全不同。Allan 方差的思路是换一个观察尺度——把长时间静置的数据按不同窗口长度 $\tau$ 分组，看组与组之间的均值波动，从而把”快噪声”和”慢噪声”分到不同的 $\tau$ 段。

Allan 方差的定义

设 IMU 静置采样得到序列 $\omega(t)$（这里以陀螺为例，加速度计同理）。取聚类时间（averaging time）$\tau$，把序列按长度 $\tau$ 分成不重叠的组，第 $k$ 组的组内均值为 $\bar{\omega}_k(\tau)$。Allan 方差定义为相邻两组均值之差的方差的一半：

$$\sigma_A^2(\tau) = \frac{1}{2}\,\mathbb{E}\!\left[\big(\bar{\omega}_{k+1}(\tau) - \bar{\omega}_k(\tau)\big)^2\right]$$

工程实现里用有限样本求平均。把它画成 $\log\sigma_A$ 对 $\log\tau$ 的双对数曲线（Allan deviation plot），不同斜率的线段对应不同物理来源的噪声。

双对数曲线怎么读

每种噪声在 $\log\sigma$-$\log\tau$ 图上贡献一段固定斜率的直线，叠成一条折线：

噪声类型	曲线斜率	对应参数	物理含义
角度随机游走 N（白噪声）	$-1/2$	$\sigma_A = N/\sqrt{\tau}$	高频抖动，传感器热噪声
零偏不稳定性 B	$0$（平台）	$\sigma_A = B$	中频，零偏的随机波动
速率随机游走 K	$+1/2$	$\sigma_A = K\sqrt{\tau/3}$	零偏本身的慢漂

直觉解读：

• 短 $\tau$（快速分组）：每组只覆盖很短时间，白噪声来不及被平均掉，主导方差，斜率 $-1/2$。
• 中 $\tau$：白噪声被组内平均压下去，零偏不稳定性成为主导，曲线进入水平平台。
• 长 $\tau$：慢漂（速率随机游走）让相邻组的系统均值越拉越远，方差重新上升，斜率 $+1/2$。

读图就是：$-1/2$ 段外推到 $\tau=1$ 读 $N$；平台高度读 $B$；$+1/2$ 段斜率读 $K$。

融合算法怎么用这些参数

VIO / EKF / IMU 预积分把 IMU 噪声建模为两个量：

• 白噪声谱密度 $N$（连续时间单位 $\text{rad}/(\text{s}\sqrt{\text{Hz}})$ 或 $\text{m}/(\text{s}^2\sqrt{\text{Hz}})$）：决定连续时间角速度/加速度的随机扰动强度。离散化时乘 $\sqrt{\Delta t}$。
• 零偏随机游走 $K$：把零偏建模为 $\dot{\mathbf{b}} = \mathbf{w}_b$，$\mathbf{w}_b$ 是方差为 $K^2$ 的白噪声。零偏因此是”随机游走”过程，融合算法把它当待估状态，扩散协方差由 $K$ 决定。

标定给出的就是这两个数。Kalibr 要求在 imu.yaml 里填 gyroscope_noise_density、gyroscope_random_walk（以及加速度计对应项），它们直接控制 EKF 对 IMU 的信任权重。

怎么实测

把 IMU 长时间静置（通常几小时，越长越能量到慢漂段），采原始数据，跑工具（imu_utils、allan_variance_ros、Kalibr 的 imu_analyzers）算 Allan 方差并拟合 $N, B, K$。采样要够长——要测到 $\tau \sim 10^3$ s 量级的平台和上升段，至少录几小时，否则慢噪声参数估不准。

Allan 方差的核心实现

把原始序列 $\{\omega_i\}_{i=0}^{N-1}$（采样间隔 $\tau_0$）按聚类时间 $\tau = m\tau_0$（$m$ 个样本一组）分组，算每组均值 $\bar\omega_k$，再求相邻组差值的方差：

import numpy as np

def allan_variance(data, fs):
    """data: 静置采样序列, fs: 采样率 (Hz)"""
    tau0 = 1.0 / fs
    N = len(data)
    taus, avars = [], []
    for m in [2**k for k in range(0, int(np.log2(N//2)) + 1)]:  # 二进尺度
        tau = m * tau0
        # 累计和法（O(N)）：cluster mean via cumulative sum
        K = N // m
        clusters = data[:K*m].reshape(K, m).mean(axis=1)   # 每组均值
        diffs = np.diff(clusters)                            # 相邻组之差
        avar = 0.5 * np.mean(diffs**2)                       # σ²_A(τ) = ½ E[(Δ)²]
        taus.append(tau); avars.append(avar)
    return np.array(taus), np.sqrt(np.array(avars))          # 返回 τ 和 σ_A(τ)

# 双对数图上拟合：-1/2 段外推到 τ=1 读 N；平台读 B；+1/2 段斜率读 K

m 取 2 的幂（octave 间隔）是为了在 log-log 图上均匀分布点。用累积和（cumulative sum）技巧可以把朴素 $O(N^2)$ 降到 $O(N)$——对几小时的百万级样本很关键。

易错点

• Allan 方差给出的是噪声参数，不是”总噪声大小”。它分出的是各种成分各自的强度，不能直接当成”读数误差”。
• 白噪声密度 N 和随机游走 K 不要混。前者是测量噪声（陀螺角速度抖），后者是零偏漂移速率（$\dot{\mathbf{b}}$ 的强度）。在 EKF 里它们对应完全不同的协方差注入方式。
• 单位陷阱：连续时间谱密度 vs 离散时间方差差一个 $\sqrt{\Delta t}$ 因子。不同工具/文档的约定不同，填参数前务必核对单位约定，否则 EKF 权重错一个量级。

参考

• Allan 方差（维基百科）
• IEEE STD 1293-1998 惯性传感器噪声模型标准
• Kalibr IMU 噪声模型文档

不标定的代价

把一个没标掉的陀螺零偏，沿着”零偏 → 姿态 → 地面方向 → 测距”这条链追下去，看清误差怎么放大。

链条第一环：零偏积分成姿态漂

陀螺零偏 $\boldsymbol{\epsilon}$（rad/s）被当作真实角速度积分（见 B09）：

$$\Delta R(t) \;\approx\; \exp\!\big(\boldsymbol{\epsilon}\,t\big) \;\Longrightarrow\; \Delta\theta_{\text{姿态}}(t) = \|\boldsymbol{\epsilon}\|\,t$$

姿态误差随时间线性增长。设未标掉的零偏只有 $0.001$ rad/s（约 $0.057°/\text{s}$，廉价 MEMS 完全可能）。1 分钟后姿态偏 $3.4°$。其中俯仰分量 $\Delta\phi$ 直接进入地平面测距。

链条第二环：俯仰偏 → 地面歪

俯仰角 $\phi$ 决定相机相对地面的倾角（见 B04/B05）。姿态偏了 $\Delta\phi$，地平面方程跟着转 $\Delta\phi$，射线与地面的俯角 $\alpha$ 偏了 $|\Delta\alpha| \approx |\Delta\phi|$。

链条第三环：俯角偏 → 距离平方级放大

地平面测距 $d = h/\tan\alpha$。对 $\alpha$ 求导（B05 已得）：

$$\Delta d \approx -\frac{h}{\sin^2\alpha}\,\Delta\alpha$$

代入数字看远处。$h=1$ m，远处 $\alpha = 2° \approx 0.035$ rad，俯仰误差 $\Delta\phi = 0.5° \approx 0.0087$ rad：

$$|\Delta d| \approx \frac{1}{(0.035)^2}\times 0.0087 \approx 7.1 \text{ m}$$

而同一误差在近处（$\alpha = 30°$）只有 $|\Delta d| \approx \frac{1}{(0.5)^2}\times 0.0087 \approx 0.035$ m。同样的姿态误差，近处忽略不计，远处偏出几米。

为什么”近处看不出来”是危险的

近处 $\alpha$ 大，$\sin^2\alpha$ 接近 1，误差被压得很小，看起来”系统很准”。但系统的真实使用场景（避障、跟车）恰恰关心远处的障碍物——那里 $\alpha$ 小，误差爆炸。所以用近处目标验收标定，会掩盖问题；必须用远处已知距离目标检验。

这条链决定了标定精度要求

倒推：要让 50 m 处测距误差 < 1 m，$\alpha$ 大约 $1.15°$（$\tan\alpha = h/d = 1/50$），则允许的俯仰误差：

$$|\Delta\phi| \lesssim \frac{|\Delta d|\,\sin^2\alpha}{h} = \frac{1 \times (0.02)^2}{1} = 4\times10^{-4}\text{ rad} \approx 0.023°$$

这就是为什么 IMU 零偏要标到 $10^{-4}\sim10^{-3}$ rad/s 量级、相机-IMU 俯仰外参要标到零点零几度——不是洁癖，是远处测距硬性倒推出来的。

易错点

• 不要只看近处验收。近处一切正常不代表远处没问题，误差的”远处放大”是几何决定的，不是 bug。
• 零偏危害随时间和距离双重放大：时间上积分线性漂，距离上平方级放大。两个因素叠加，长时间运行 + 远距离 = 最坏情况。
• 加速度计零偏同样污染俯仰（通过重力方向估计），不能只盯着陀螺。

参考

• Allan 方差与零偏漂移（IMU 噪声）
• IMU 误差传播到姿态/位置（误差状态卡尔曼滤波）

IMU 内参怎么标

IMU 标定分两类完全不同的任务：噪声参数（随机，不可扣除，只建模）和确定性参数（零偏、标度、轴向，可扣除）。两者用不同方法标。

第一类：噪声参数——Allan 方差法

把 IMU 长时间静置（数小时，恒温），采集原始数据，按 B11 的方法算 Allan 方差，拟合出：

• 角度/速度随机游走 $N$（白噪声谱密度）
• 零偏不稳定性 $B$
• 速率/加速度随机游走 $K$

这组数填进融合算法的噪声协方差（如 Kalibr 的 imu.yaml），决定 EKF 该多信任 IMU。这一类不能”标掉”，只能”量出来供算法使用”。实测要点：

• 静置时间要够长。要量到 $\tau \sim 10^3$ s 的平台和上升段，至少录 3–10 小时，否则慢漂参数 $K$ 估不准。
• 环境恒温。温度漂移会污染 Allan 曲线，让 $B$ 段假性抬升。
• 用专门工具拟合，如 imu_utils（基于对 Allan 双对数曲线分段拟合 $N, K$）、allan_variance_ros。

第二类：确定性参数——多姿态/转台法

零偏 $\mathbf{b}$、标度 $\mathbf{S}$、轴不对齐 $\mathbf{T}$ 是确定性失真，可标定后从读数里扣除。核心思路：用已知的真值去反解 IMU 模型参数。

最简单的零偏标定：六面静置/多姿态静置。利用”静止时加速度计只应测到重力、大小恒为 $g$“这个约束。把 IMU 摆到 6 个不同朝向（每个轴朝上/朝下），每次加速度计读数的合力都应等于 $\pm g$。把所有读数收集起来，最小二乘解出零偏和标度：

$$\min_{\mathbf{b}, \mathbf{S}} \sum_i \big\|\, \mathbf{S}^{-1}(\tilde{\mathbf{f}}_i - \mathbf{b}) - \mathbf{g}_i^B \,\big\|^2$$

其中 $\mathbf{g}_i^B$ 是第 $i$ 个姿态下重力在 IMU 系的已知方向。多姿态越多，参数（含轴不对齐 $\mathbf{T}$）越能解出来。

转台标定精度最高：把 IMU 装在精密速率转台上，给已知角速度 $\omega_{\text{ref}}$，比对陀螺读数 $\tilde{\boldsymbol{\omega}}$，反解标度和零偏。这是工业级 IMU 出厂标定方式，廉价 MEMS 项目通常用多姿态法近似。

第三类：相机-IMU 联合标定里的在线零偏

即便离线标了零偏，运行时它仍会缓慢漂移（零偏随机游走，见 B11）。所以 VIO/EKF 把零偏当状态变量在线估计：

$$\text{状态} = [\text{位姿}, \text{速度}, \mathbf{b}_g, \mathbf{b}_a, \ldots]^\top$$

视觉观测定期修正零偏估计，IMU 在两次观测间做预测。这把”离线标定 + 在线补偿”结合：离线给初值和噪声参数，在线追慢漂。

工具

• imu_utils（ETH/同济等开源）：录静置 rosbag，算 Allan 方差，输出 $N, K$ 拟合结果。
• Kalibr：除相机-IMU 联合标定外，要求用户自行提供 imu.yaml（噪声参数），本身不估噪声参数。
• imu_tk / Inertial Sensor Calibration：多姿态数据标零偏、标度、轴向。

易错点

• Allan 方差静置不够长是最常见的错。录 30 分钟就想估 $K$，曲线根本没到 $+1/2$ 段，参数虚低。
• 多姿态标定没激励全轴向。只在几个姿态静置，某些标度/不对齐项欠约束，解出来不可靠。要覆盖六个方向且姿态分布均匀。
• 离线零偏 ≠ 运行时零偏。温度、开机随机性让零偏每次开机都不同，离线标定值只能做初值，真正可信的是在线估计。

参考

• Kalibr IMU 噪声模型与标定流程
• Allan 方差（维基百科）
• imu_utils（开源 Allan 方差工具）

空间外参的直觉

空间外参 $T_{BC}$（相机系到 IMU 体坐标系的变换）描述的是：相机和 IMU 虽然刚性拧在同一块板上，但它们各自的坐标系朝向不同、原点错位，这个固定差异就是 $T_{BC}$。

外参的几何含义

$T_{BC} \in SE(3)$ 拆成旋转 $R_{BC}$ 和平移 $\mathbf{t}_{BC}$：

$$\mathbf{p}^B = R_{BC}\,\mathbf{p}^C + \mathbf{t}_{BC}$$

• $R_{BC}$：相机系坐标轴相对 IMU 系的朝向差。相机光轴（$+Z_C$）在 IMU 系里指向某个固定方向；相机的”右""下”在 IMU 系里也是固定方向。
• $\mathbf{t}_{BC}$：相机光心相对 IMU 原点的位置（在 IMU 系里表达）。

外参是静态量——只要相机和 IMU 没相对位移（没松动、没热变形），$T_{BC}$ 一次标定终身有效。这是它和 IMU 实时姿态 $R_{WB}$（动态量）的关键区别。

为什么旋转外参最敏感

IMU 给出的是 $R_{WB}$（IMU 在世界系里的姿态）。要用它摆正相机看到的地面，需要把变换链乘起来（见 B06）：

$$T_{WC} = T_{WB}\,T_{BC}$$

也就是 $R_{WC} = R_{WB}\,R_{BC}$。如果 $R_{BC}$ 标错了 $\Delta R$，相机系的姿态就整体偏 $\Delta R$，等效于相机俯仰角偏 $\Delta\phi$。这个 $\Delta\phi$ 直接进入 B05 的误差公式：

$$|\Delta d| \approx \frac{h}{\sin^2\alpha}\,|\Delta\phi|$$

远处平方级放大。所以旋转外参的精度要求是零点零几度（B12 倒推过）。相比之下平移 $\mathbf{t}_{BC}$ 只影响光心位置，量级是几厘米，对距离的贡献是米级以下——远不如旋转敏感。

直觉：外参是”翻译器”

可以把 $R_{BC}$ 想成一个翻译器：IMU 报告”地面法向是这个方向”（在 IMU 系里），但相机听不懂 IMU 系，得经过 $R_{BC}$ 翻译成相机系里的方向，才能去和相机射线求交。翻译器错一两度，地面就被摆歪一两度，远处的射线-地面交点大幅前后挪动。

易错点

• 外参标错不会在近处暴露。和所有俯仰误差一样，近处 $\sin\alpha$ 大、误差被压住，看起来没事，远处才爆炸。验收要用远处已知距离目标。
• 平移和旋转的标定难度不对称。旋转要靠充分的旋转激励（绕各轴转动，见 B19）才能约束；平移只要有视差（标定板不同距离）即可。纯旋转标定能解好 $R_{BC}$ 但 $\mathbf{t}_{BC}$ 欠约束。
• 外参假设刚性。如果相机或 IMU 受冲击松动，$T_{BC}$ 变了，旧标定失效——这是看不见的故障源，运行时残差异常增大要怀疑硬件松动。

参考

• 相机-IMU 外参标定（Furgale 经典论文）
• Kalibr 相机-IMU 标定文档

时间偏移的直觉

相机和 IMU 各有各的时钟、各自的触发与曝光机制。同一物理时刻，相机图像的时间戳 $t_{\text{cam}}$ 和 IMU 的时间戳 $t_{\text{imu}}$ 之间存在一个固定偏移 $t_d$：

$$t_{\text{cam}} = t_{\text{imu}} + t_d$$

$t_d$ 通常在几毫秒到几十毫秒量级（曝光延迟、传输延迟、不同时钟源、不同驱动栈造成）。标定就是把这个固定 $t_d$ 解出来，运行时用它把两路数据对齐到同一时刻。

静止时无所谓，运动时致命

静止时所有时刻的姿态都一样，时间对错不影响——拿哪个时刻的姿态去配这一帧图像都对。但车辆一旦运动（颠簸、转弯、加减速），姿态随时间变化。若图像时间戳错了 $t_d$，配上的就是另一个时刻的姿态：

$$\Delta\phi_{\text{时间}} \approx \dot{\phi}\cdot t_d$$

$\dot{\phi}$ 是俯仰角变化率（rad/s）。颠簸时俯仰能瞬时变化几 rad/s，$t_d = 30$ ms 就贡献 $\sim 0.06$ rad $\approx 3.4°$ 的俯仰错位——又进入 B05 的平方级放大。运动越快、颠簸越剧烈，时间错位的误差越大。

时间偏移和空间外参耦合

时间偏移产生的姿态错位 $\dot{\phi}\,t_d$，在效果上和空间外参旋转误差 $\Delta R_{BC}$ 几乎无法区分——两者都表现为”地面方向偏了一个固定角度”。这给标定带来麻烦：如果不让设备充分运动（$\dot{\phi}$ 大），$t_d$ 的可观性很差，容易和 $R_{BC}$ 混淆。这就是为什么联合标定要求充分的旋转激励（见 B19）——既激发空间外参，也让时间偏移变得可观。

为什么是固定偏移而不是随机

工程上把 $t_d$ 当常数来标定，这是合理的近似：

• 大多数固定延迟源（曝光时间、传输缓冲、驱动时戳逻辑）是稳定的。
• 真正的时戳抖动（jitter）通常很小（亚毫秒），且当作噪声处理。

所以一次标定出一个 $t_d$ 长期使用。若硬件/驱动改了（换相机、改曝光参数、改驱动），$t_d$ 可能变，要重标。

标定思路

联合标定把 $t_d$ 当作待估参数，和 $T_{BC}$ 一起放进优化（见 B18）。原理：让设备做一段运动，相机和 IMU 都观测到同一运动。改变 $t_d$ 会改变”这一帧用哪一时刻的 IMU 姿态”，从而改变重投影误差。优化找让总重投影误差最小的 $t_d$。运动越剧烈，重投影误差对 $t_d$ 越敏感，$t_d$ 解得越准。

易错点

• 静止数据标不了 $t_d$。$\dot\phi=0$ 时 $t_d$ 完全不可观。采标定数据必须有运动，且运动要含角速度（平移不行）。
• 时间偏移和空间外参旋转几乎共线。没有充分的、含旋转的运动激励，两者会互相吸收误差，解出来都不准。B19 讲的”充分激励”就是为了打破这个耦合。
• 卷帘快门（rolling shutter）相机的 $t_d$ 更复杂——同一帧图像不同行对应不同时刻，要把 $t_d$ 和行时间一起建模，否则快速运动下边缘行错位严重。

参考

• 相机-IMU 时间同步（Furgale 时间偏移标定）
• Kalibr 时间偏移与卷帘快门标定

联合标定的原理

联合标定的核心思想是：同一段物理运动，相机和 IMU 各自测出来，本该完全吻合。标定就是去找一组空间外参 $T_{BC}$ 和时间偏移 $t_d$，让这两条运动曲线在空间和时间上严丝合缝地对齐。

两条独立的运动测量

让设备做一段运动（手持晃动或装在机械臂上）：

1. 相机侧：标定板角点（或场景特征）在图像里的轨迹，隐含了相机的运动。已知 $T_{BC}$ 后，相机运动可以反推 IMU 应该感受到的运动。
2. IMU 侧：陀螺/加速度计直接测出角速度和加速度，积分得到姿态和速度轨迹。

这两条都是同一物理运动的测量，理想情况下应该一致。

对齐判据：重投影误差最小

Kalibr 采用连续时间优化方法（continuous-time estimation）。把相机轨迹用三次 B 样条（cubic B-spline）参数化，使它在任意时刻（含两帧之间的时刻）都可插值——这正好用来处理 IMU 高频、相机低频的时间错位问题。优化目标是让 IMU 预测的姿态/位置与视觉观测一致：

$$\min_{T_{BC},\, t_d,\, \text{样条控制点}} \; \underbrace{\sum_k \big\| \mathbf{u}_k - \pi(K\, T_{CB}\, T_{BW}(t_k+t_d)\, \mathbf{p}_k^W)\big\|^2_{\Sigma}}_{\text{视觉重投影项}} \;+\; \underbrace{\sum_j \big\| \tilde{\boldsymbol{\omega}}_j - \boldsymbol{\omega}_{\text{样条}}(t_j)\big\|^2 + \big\|\tilde{\mathbf{f}}_j - \mathbf{f}_{\text{样条}}(t_j)\big\|^2}_{\text{IMU 残差项}}$$

直觉：

• 视觉项：在 $t_k + t_d$ 时刻（注意时间偏移进入这里），从样条取出 IMU 姿态，经 $T_{BC}$ 变到相机系，把 3D 点 $\mathbf{p}_k^W$ 投影成像素，与实测像素 $\mathbf{u}_k$ 比，差越小越好。
• IMU 项：样条曲线在每个 IMU 采样时刻 $t_j$ 应能还原出陀螺/加速度计读数。

$T_{BC}$、$t_d$、样条控制点一起迭代优化，让总残差最小。收敛时，对齐那一刻的 $T_{BC}$ 和 $t_d$ 就是解。

为什么时间偏移能解出来

$t_d$ 进入视觉项的时间参数。改变 $t_d$，视觉项用的 IMU 姿态从样条的不同位置取值，投影位置变化，重投影误差变化。运动越快，样条在时间上越陡，$t_d$ 微小变化引起投影大变化——$t_d$ 越可观。这就是为什么静态数据解不出 $t_d$，必须运动（见 B17/B19）。

为什么空间外参能解出来

$T_{BC}$ 把 IMU 姿态变换到相机系。如果 $R_{BC}$ 错了，所有帧的相机姿态系统性地偏一个固定旋转，重投影误差下不去。优化搜索让重投影误差最小的 $R_{BC}$，原理和多相机外参串联一致（见 kalibr 项目），只是这里多了一个 IMU 的约束。

与多相机标定的区别

多相机标定只用视觉重投影误差（见 kalibr B08/B09）。相机-IMU 联合标定多了 IMU 残差项，它带来两个好处：

1. 解出 $t_d$：纯视觉没有时间维度（图像是离散快照），加 IMU 才有时间对齐概念。
2. 更强约束：IMU 提供连续的高频运动信息，约束了帧间运动，让 $T_{BC}$（尤其平移部分）解得更稳。

易错点

• 联合标定要求相机已标好内参。内参错了，反推的相机姿态错，连带 $T_{BC}$、$t_d$ 都错。流程上先标内参再标外参，不要混在一起。
• 样条拟合需要充分运动覆盖。运动太单一（只平移、只绕一个轴），样条在某些自由度欠约束，外参对应分量不可观。
• 优化是迭代的、非线性的，初值重要。先用一个粗略 $T_{BC}$（手工量或厂商给出）做初值，否则可能陷局部极小。

参考

• Furgale et al. - Unified Temporal and Spatial Calibration (TRO 2013)
• Rehder et al. - Spatio-temporal laser-visual-inertial calibrator (连续时间 B 样条标定)
• Kalibr 连续时间标定原理

为什么要充分激励

联合标定的参数（空间外参 $T_{BC}$、时间偏移 $t_d$）只有在运动”激发”了它们对应的可观方向时，才能被可靠解出。运动太单一，某些参数数学上不可观，标出来的是噪声而不是真值——这叫退化运动（degenerate motion）。

退化运动：参数不可观的本质

可观性（observability）指：改变某个参数，是否能改变观测（重投影误差）。如果不能，这个参数就淹没在噪声里。几种典型退化：

运动	不可观的参数	原因
纯静止	全部，尤其 $t_d$	没有运动，姿态不变，时间错位无影响
纯平移（不转）	$R_{BC}$、$t_d$	平移不产生角速度，旋转外参和时间偏移都不敏感
匀速直线	同上	加速度为零，加速度计约束失效
只绕一个轴转	该轴外的旋转分量	其他轴的旋转外参没被激发
绕 IMU 单一轴匀速转	$t_d$ 与该轴 $R_{BC}$ 耦合	两者在该运动下数学共线，无法区分

为什么旋转外参需要绕各轴都转

$R_{BC}$ 有三个自由度（如欧拉角 roll/pitch/yaw）。只绕 $Z$ 轴转，只能激发”IMU $Z$ 轴 ↔ 相机某轴”的对应关系，roll/pitch 方向的 $R_{BC}$ 分量没被约束。必须绕三个轴都转（摇头 pitch、点头 yaw、翻滚 roll），让每个旋转自由度都出现在观测里，$R_{BC}$ 三维才能同时解准。

为什么平移外参需要视差

平移外参 $\mathbf{t}_{BC}$（光心相对 IMU 的位置）靠视差约束：相机平移时，远处和近处的标定板点产生不同的相对运动，量级与 $\mathbf{t}_{BC}$ 相关。纯旋转没有视差（所有距离点的相对运动一样），$\mathbf{t}_{BC}$ 不可观。所以采数据要既转又平移，且平移要让标定板在画面里产生明显位移。

为什么时间偏移需要角速度

$t_d$ 通过”姿态随时间变化”才变得敏感（见 B17）：$\Delta\phi \approx \dot\phi\,t_d$。$\dot\phi$ 大（剧烈转动），$t_d$ 微小变化引起投影大位移，$t_d$ 可观。静止或纯平移（$\dot\phi\approx 0$）时 $t_d$ 不可观，且会和旋转外参耦合到无法分离。

充分激励的动作要领

为了让所有参数同时可观，采集运动应包含：

• 三轴旋转：绕 $X, Y, Z$ 三个轴都做正负方向的转动（摇头、点头、翻滚）。
• 三轴平移：前后、左右、上下移动，制造视差。
• 变速度（含加速度）：非匀速，让加速度计也有约束。匀速段加速度为零，约束失效。
• 覆盖画面：标定板在相机视野里各位置、各距离都出现，激发内参各区域。

这就是”让设备充分地摇头、点头、翻滚加平移”的技术含义——不是仪式感，是可观性的硬要求。

易错点

• “动得很猛”不等于”充分激励”。如果只在一个方向狂甩，照样退化。要的是自由度覆盖，不是幅度。
• 退化运动不会报错，只会给出看似收敛但数值不对的结果（残差也低，因为不可观方向残差本就不变）。这是最隐蔽的坑——看残差大小验收不够，还要看参数是否稳定（多次标定一致）。
• 太剧烈的运动有运动模糊（相机曝光期间姿态变化大），角点检测精度下降。要在”充分激励”和”避免模糊”间取平衡：动作丰富但速度适中。

参考

• 可观性分析（observability of camera-IMU calibration）
• Furgale - 时空联合标定与激励要求
• Kalibr 标定数据采集建议

标定误差怎样变成测距误差

把各类标定误差代入地平面测距公式 $d = h/\tan\alpha$，分别算出它们各自折算成多少距离误差，把账算清楚。

俯仰角误差：平方级，最致命

俯仰角 $\phi$ 进入射线-地面俯角 $\alpha$。误差传递（B05 已推导）：

$$|\Delta d| \approx \frac{h}{\sin^2\alpha}\,|\Delta\phi|$$

由于 $\alpha \approx h/d$（远处小角），$\sin\alpha \approx h/d$，代入得：

$$|\Delta d| \approx \frac{h}{(h/d)^2}\,|\Delta\phi| = \frac{d^2}{h}\,|\Delta\phi|$$

距离误差正比于距离的平方，除以相机高度。 这就是”距离误差大致随距离的平方除以相机高度增长”。代入例子：$h=1$ m，$\Delta\phi = 1° = 0.0175$ rad，$d = 50$ m：

$$|\Delta d| \approx \frac{50^2}{1}\times 0.0175 \approx 44 \text{ m}$$

（线性近似在极端远处偏夸张，实际有饱和，但量级说明 50 m 外几度俯仰误差就是灾难。）俯仰误差的来源是 IMU 姿态误差和相机-IMU 旋转外参误差，这正是 B10/B16 重视它们的原因。

内参误差：射线方向偏，远处放大

内参 $K$ 错让像素反投影的射线方向 $\mathbf{d} = K^{-1}\mathbf{u}$ 偏。设射线方向误差角 $\Delta\beta$，效果和俯仰误差对称——也进入 $\alpha$：

$$|\Delta d| \approx \frac{h}{\sin^2\alpha}\,|\Delta\beta|$$

远处放大规律相同。内参误差主要来自焦距 $f$、主点、畸变系数标不准，且畸变在画面边缘最严重，远处的障碍物常落在画面边缘，受影响最大。

时间偏移误差：乘速度，动态偏移

时间偏移 $t_d$ 标不准（或运行时变化），在运动中产生姿态错位 $\Delta\phi \approx \dot\phi\,t_d$，再代入俯仰误差公式：

$$|\Delta d_{\text{时间}}| \approx \frac{h}{\sin^2\alpha}\,|\dot\phi\,t_d|$$

它额外乘了角速度 $\dot\phi$。静止时为零，剧烈颠簸/转弯时巨大。这是纯动态误差——静止标定场景看不出来，但车辆实际行驶时会暴露。

高度误差：线性，均匀

相机高度 $h$ 标不准，误差是线性的：

$$|\Delta d| = \frac{|\Delta h|}{\tan\alpha}$$

由于 $\tan\alpha \approx h/d$，化简 $|\Delta d|/d \approx |\Delta h|/h$。高度相对误差等于距离相对误差：高度错 1%，所有距离都错 1%。不像俯仰那样随距离平方爆炸，但仍要标准（实测光心高度，不能拿名义值）。

总账：越远越快，误差越大

误差源	误差公式	对距离 $d$ 的依赖	对速度的依赖
俯仰 $\Delta\phi$	$d^2/h \cdot \Delta\phi$	平方	无（除非时间引起）
内参 $\Delta\beta$	$d^2/h \cdot \Delta\beta$	平方	无
时间 $t_d$	$d^2/h \cdot \dot\phi\,t_d$	平方	正比速度
高度 $\Delta h$	$d \cdot \Delta h/h$	线性	无

一句话：俯仰、内参、时间这三类都被距离平方放大；时间还被速度放大。越远、越快，标定的每一点误差都被放得越大。 这就是为什么避障系统（关心远距离）对标定精度的要求比近距应用严苛得多。

易错点

• $d^2$ 依赖意味着远处误差远超直觉。50 m 处 0.5° 俯仰误差，按上面公式是十几到几十米——不是”几厘米”，别低估。
• 线性近似在 $\alpha$ 极小时失效（公式发散）。真实系统有最大有效距离（超过后测距完全不可信），不要把公式外推到无穷。
• 时间误差只在运动时出现，静态验收完全看不出。标定好不好要在动态、远距离场景检验。

参考

• 单目地平面测距的距离-俯仰敏感性
• Allan 方差与 IMU 噪声传播到姿态

怎么判断标定好不好

标定收敛不代表标定正确——退化运动下也能给出一个”收敛”但错误的解（见 B19）。验收要看几个独立的指标，互相印证。

指标一：重投影误差（视觉项）

把标定结果代入，用解出的外参 + IMU 姿态把 3D 标定板角点投影回图像，与实测像素比对：

$$e_{\text{reproj}} = \big\| \mathbf{u}_{\text{实测}} - \pi(K\,T_{CB}\,T_{BW}\,\mathbf{p}^W) \big\| \quad (\text{像素})$$

合格标准通常亚像素（< 0.3–0.5 px）。这个指标反映内参 + 外参 + IMU 姿态的联合精度。如果重投影误差大，至少有一项错了。

注意：重投影误差只覆盖有图像观测的那些时刻，不能反映帧间 IMU 轨迹质量。所以要配下面的 IMU 项一起看。

指标二：IMU 残差（IMU 项）

联合标定的 IMU 残差项（见 B18）衡量样条轨迹与陀螺/加速度计读数的一致性：

$$e_{\text{imu}} = \big\|\tilde{\boldsymbol{\omega}}_j - \boldsymbol{\omega}_{\text{样条}}(t_j)\big\|, \qquad \big\|\tilde{\mathbf{f}}_j - \mathbf{f}_{\text{样条}}(t_j)\big\|$$

这些残差应该接近 IMU 的噪声水平（由 B11 的 Allan 方差参数决定）。如果残差远大于噪声谱密度，说明样条没能拟合上 IMU——要么 $T_{BC}$/$t_d$ 错，要么 IMU 噪声参数填错。

指标三：残差的随机性（最容易被忽略）

残差分布应该是随机的，不应有结构。 把重投影残差画成散点图（按图像位置、按时间、按帧），如果点呈随机噪声样、无偏置无规律，说明模型够好；如果残差有明显的系统性图案（比如图像边缘系统性偏向一边、某些时间段残差突变），说明模型没标对——可能是畸变模型选错、$t_d$ 标偏、或者有未建模的效应（卷帘快门、温度漂移）。

这是比”残差数值小”更严格的判据。一个数值小但有结构的残差，往往藏着系统性误差。

指标四：已知距离目标的绝对验证

最直接、最可信的验收：放一个距离已知的标定物（如 5 m、10 m、20 m 处的标志），用系统测距，对比真实值。这跳过所有间接指标，直接检验最终目的——测距准不准。务必覆盖远处目标，因为 B21 已说明远处误差被平方级放大，近处正常不代表远处没问题。

指标五：预测一致性（时间维度）

用 IMU 预测下一时刻的姿态，把标定板（或场景特征）按预测姿态投影回图像，看是否对齐。如果短期预测投影与实测吻合，说明 $T_{BC}$、$t_d$、IMU 噪声参数都对——三者共同决定了预测精度。这是运行时 VIO 工作原理的缩影，能通过说明系统可以上线。

怎么用这套指标

指标	检验什么	通过标准
重投影误差	内参 + 外参 + 视觉时刻姿态	亚像素，随机无结构
IMU 残差	样条 vs IMU 读数	接近噪声水平
残差随机性	模型是否完备	无系统性图案
已知距离目标	最终测距精度	远近都达标
预测一致性	运行时可用性	短期投影对齐

单一指标通过不能算数。重投影亚像素 + IMU 残差接近噪声 + 残差无结构 + 远近测距都准，几个条件同时成立，标定才算靠谱。

易错点

• 只看平均重投影误差会被掩盖。少数大残差被多数小残差平均掉。要看分布（直方图、最大值、散点），不只看均值。
• 残差有结构时，先怀疑模型再怀疑数据。畸变模型不匹配镜头（鱼眼用 radtan）、$t_d$ 未建模、卷帘快门未建模，都会留下结构化残差。
• 已知距离验证只在近处做没意义。近处 B05/B21 证明误差被压住，必须放远处目标。

参考

• 重投影误差与标定评估（OpenCV）
• Kalibr 标定报告解读