本体感觉传感器

机器人传感器分为两大类。本体感觉传感器（Proprioceptive sensor）测量机器人自身的内部状态——即它自身的运动——而外部感觉传感器（exteroceptive sensor）（相机、LiDAR、RADAR、声呐）测量的是外部环境。在SLAM中你会不断遇到的两种本体感觉传感器是：

• IMU（惯性测量单元）：以高频率（100-1000 Hz）测量线加速度 $\mathbf{a}_m$（三轴加速度计）和角速度 $\boldsymbol{\omega}_m$（三轴陀螺仪）。测量值会受到白噪声以及缓慢变化的偏置（bias） $\mathbf{b}^a, \mathbf{b}^g$ 的污染,这些偏置必须在线估计;加速度计还会感受到重力,必须加以补偿。
• 轮式编码器：在差速驱动或阿克曼转向的车辆上统计车轮转数,通过车辆的运动学模型得到线速度和角速度。在干净的室内地面上精度很高,但在室外会因车轮打滑、地面不平和轮胎变形而退化。

本体感觉测量为SLAM系统的运动模型提供输入,

$$\mathbf{x}_{t+1} = f(\mathbf{x}_t, \mathbf{u}_t) + \mathbf{w}_t, \qquad \mathbf{w}_t \sim \mathcal{N}(\mathbf{0}, Q_t),$$

即根据当前状态预测下一状态。它们的典型优势与劣势：

由于本体感觉传感器会漂移但从不会”失效”,而外部感觉传感器精度高但可能失效(模糊、低纹理、遮挡),这两类传感器天然互补——这正是视觉-惯性里程计的全部前提,其中IMU预积分将关键帧之间数百次惯性测量累积成一个单一的相对运动约束。

测量模型

标准IMU测量模型明确给出了噪声结构。设机体坐标系为 $B$,世界坐标系为 $W$,机体姿态为 $R_{WB}$：

$$\boldsymbol{\omega}_m = \boldsymbol{\omega} + \mathbf{b}^g + \mathbf{n}^g, \qquad \mathbf{a}_m = R_{WB}^T(\mathbf{a}_W - \mathbf{g}) + \mathbf{b}^a + \mathbf{n}^a$$

有三点需要注意。加速度计测量的是**比力（specific force）**而不是加速度：重力 $\mathbf{g}$ 出现在模型内部,因此静止的IMU读数约为 $9.81\,\text{m/s}^2$ 向上,且姿态误差会将重力泄漏进估计出的加速度中——$1°$ 的倾角误差会注入约 $9.81 \sin(1°) \approx 0.17\,\text{m/s}^2$ 的虚假水平加速度,经二次积分后几秒内就会累积成数米的误差。噪声项 $\mathbf{n}$ 被建模为白高斯噪声;偏置项 $\mathbf{b}$ 则被建模为缓慢的随机游走,这就是为什么估计器要携带偏置状态并不断更新它们。而且一切都以机体坐标系表示——使用它需要用到你正试图估计的那个姿态本身,这正是惯性导航被耦合成一个联合估计问题的原因。

对于差速驱动底盘上的轮式编码器(车轮半径 $r$,轮距 $L$,测得的车轮角速度 $\omega_l, \omega_r$)：

$$v = \frac{r(\omega_r + \omega_l)}{2}, \qquad \omega = \frac{r(\omega_r - \omega_l)}{L},$$

其噪声的主要来源并非编码器分辨率(这方面表现优异),而是模型违背：打滑、侧滑、地面不平、轮胎压缩变形——这些误差是系统性的、依赖地形的,而不是白噪声。

它们漂移得有多快?

一个有用的粗略层级：仅靠陀螺仪的姿态漂移随时间线性增长;由加速度计得到的速度漂移随时间线性增长(再加上姿态误差导致的重力泄漏);位置漂移则由加速度计误差引起二次增长,由陀螺仪引起的倾角误差引起三次增长。这就是为什么消费级MEMS IMU无法单独提供超过几秒钟的位置估计,而战术级/导航级设备(光纤陀螺)能够维持数分钟到数小时——但代价是成本、体积和功耗高出几个数量级。轮式里程计的漂移随行驶距离而非时间增长,因此静止的轮式机器人根本不会漂移——这也是轮式+IMU融合在室内如此有效的原因之一。

常见陷阱

• 忽略偏置 ——把 $\mathbf{a}_m, \boldsymbol{\omega}_m$ 当作无偏测量在数据表上”行得通”,但在真实机器人上会失败;偏置会随温度变化,也会在每次开关机之间发生变化,因此必须将其作为状态量而非常数处理。
• 重力初始化不当 ——VIO系统在静止时利用加速度计测得的重力来估计初始姿态;若在运动中(或在振动平台上)进行初始化,会破坏倾角估计,进而污染下游的一切。
• 振动与混叠 ——电机和螺旋桨会注入超出IMU采样假设范围的振动;机械减振和适当的低通滤波与算法本身同等重要。
• 运动学参数同样需要标定 ——车轮半径和轮距误差会在”直线”轨迹中产生系统性的曲率;需要对其标定(并在更换轮胎后重新标定)。

对SLAM的意义

几乎每一个已部署的SLAM系统都是本体感觉+外部感觉融合：手机和头显上的VIO(相机+IMU)、仓储机器人上的轮式-惯性里程计、汽车上的LiDAR-惯性里程计。了解本体感觉传感器测量的是什么、漂移速度有多快、其噪声如何建模,是理解第6级中卡尔曼滤波和因子图融合的先决条件。

相关条目

• 外部感觉传感器
• IMU
• 里程计
• IMU噪声模型