定位传感器全解：IMU、轮速计、GNSS 与 RTK
为什么需要多传感器
自动驾驶和机器人对定位的要求可以概括为四个字：连续、精准。没有任何单一传感器能同时满足高频率、低漂移、全局绝对、全天候这四个维度。工程上的做法是让不同传感器各司其职，互补短板：
| 需求 | 谁来满足 |
|---|---|
| 高频连续状态传播 | IMU |
| 地面运动约束 | 轮速计 |
| 全局绝对位置 | GNSS |
| 厘米级绝对精度 | RTK |
本文按 “传感器是什么 → 输出长什么样 → 怎么用于定位” 的线索，逐个拆解。
IMU（Inertial Measurement Unit，惯性测量单元）
物理原理
IMU 的核心是两组传感器：
- 三轴陀螺仪（Gyroscope）：测量绕三个正交轴的角速度 $\omega_x, \omega_y, \omega_z$，单位 rad/s
- 三轴加速度计（Accelerometer）：测量三个正交轴的比力（specific force）$a_x, a_y, a_z$，单位 m/s$^2$
比力 = 真实加速度 + 重力加速度。静止放在桌面上的 IMU，加速度计读数不是零，而是约 $(0, 0, 9.81)$ m/s$^2$——它测到的是桌面对它的支持力。
部分模组还集成磁力计和气压计，但做定位最核心的始终是陀螺仪和加速度计。
数据格式
IMU 典型输出频率：100–500 Hz，高性能惯导可达 1000 Hz。
ROS 中对应 sensor_msgs/Imu：
| |
关键字段说明：
| 字段 | 含义 | 备注 |
|---|---|---|
angular_velocity | 三轴角速度 | 陀螺仪直接输出 |
linear_acceleration | 三轴比力 | 包含重力分量 |
orientation | 姿态四元数 | 仅 AHRS 模组提供，纯 IMU 无此输出 |
*_covariance | 9 元素协方差矩阵 | 用于滤波器加权 |
IMU 如何推算定位
IMU 的定位链条是 捷联惯导解算（Strapdown Mechanization）。IMU 自身并不直接输出"我在哪"，它只告诉你"我刚才怎么转了、怎么加速了"。要从这些瞬时测量量推算出姿态、速度和位置，必须做积分。整个过程分两层。
第一层：角速度 → 姿态
陀螺仪每个时刻输出三轴角速度 $\boldsymbol{\omega} = (\omega_x, \omega_y, \omega_z)^T$（rad/s），描述的是"此刻 IMU 绕三个轴分别转多快"。
要得到姿态（即 IMU 此刻朝哪个方向），需要对角速度做时间积分。用四元数 $q$ 表示姿态时，微分方程为：
$$ \dot{q} = \frac{1}{2} q \otimes \boldsymbol{\omega} $$直觉理解：每个瞬间的角速度告诉你"往哪个方向转了一点点"，把这些微小旋转一个个累乘起来，就得到了从初始姿态到当前姿态的完整旋转。离散实现中，常用的近似是：
$$ q(t + \Delta t) \approx q(t) \otimes \Delta q, \quad \Delta q \approx \begin{pmatrix} 1 \\\\ \frac{1}{2}\boldsymbol{\omega} \Delta t \end{pmatrix} $$即每一步用当前角速度乘以时间步长，构造一个微小旋转四元数 $\Delta q$，右乘到当前姿态上。
第二层：加速度 → 速度 → 位置
加速度计输出的是 比力 $\mathbf{f}^b$（单位 m/s$^2$），注意它包含了重力。比如 IMU 静止放在桌面上时读数约为 $(0, 0, 9.81)$——测到的是桌面的支持力，而不是真实运动加速度。
要得到真实运动加速度，需要三步：
- 坐标系旋转：加速度计的读数是在 IMU 自身坐标系（body 系，上标 $b$）下的。用第一层求出的姿态旋转矩阵 $R_b^n$，把它转到导航坐标系（上标 $n$，通常是 ENU 或 NED）：
- 减去重力：导航坐标系下重力是已知常量 $\mathbf{g}^n$（ENU 下约为 $(0, 0, -9.81)$），减掉它才是真实运动加速度：
- 两次积分：对真实加速度积分一次得速度，再积分一次得位置：
整条链路可以总结为：陀螺仪积分一次得姿态，加速度计借助姿态去重力后积分两次得速度和位置。积分次数越多，误差累积越快——这也是 IMU 漂移问题的根源。
IMU 的根本问题：漂移
积分意味着误差会累积。陀螺仪的微小零偏 $b_g$ 经过积分后变成姿态误差，姿态误差又会导致加速度投影方向出错，进而使速度和位置误差以 $t$、$t^2$、$t^3$ 的阶次增长。
这就是为什么 IMU 不能独立做长时间定位——它是定位系统里的"高频传播器"，必须配合外部观测来修正漂移。
IMU 的等级划分
不同价位的 IMU 性能差异巨大：
| 等级 | 陀螺零偏稳定性 | 典型应用 | 代表产品 |
|---|---|---|---|
| 消费级 | > 10 °/h | 手机、无人机 | BMI088、ICM-42688 |
| 工业级 | 1–10 °/h | 量产 ADAS | ADIS16470 |
| 战术级 | 0.1–1 °/h | Robotaxi、采图车 | STIM300、HGuide i300 |
| 导航级 | < 0.01 °/h | 军用、航空 | HG1700 |
零偏稳定性直接决定了纯惯导续航时间——消费级 IMU 几秒钟就发散到不可用，战术级可以扛几十秒到数分钟。
轮速计（Wheel Odometry）
物理原理
轮速计的核心测量元件是安装在车轮上的 编码器（Encoder）。编码器每转一圈会产生固定数量的脉冲信号（记为 $N_{rev}$，即 ticks_per_rev），系统通过计数脉冲就能知道车轮转了多少。
从脉冲到车速的推导分三步：
第一步：脉冲数 → 转过的圈数
在一个采样间隔 $\Delta t$ 内，编码器累计产生了 $\Delta \text{ticks}$ 个脉冲，那么车轮转过的圈数为：
$$ \Delta n = \frac{\Delta \text{ticks}}{N_{rev}} $$第二步：圈数 → 行驶距离
车轮每转一圈，车辆前进一个轮胎周长 $2\pi r$（$r$ 是轮胎有效滚动半径）。所以这段时间内车辆行驶的距离为：
$$ \Delta s = \Delta n \cdot 2\pi r = \frac{\Delta \text{ticks}}{N_{rev}} \cdot 2\pi r $$第三步：距离 → 速度
距离除以时间就是速度：
$$ v = \frac{\Delta s}{\Delta t} = \frac{\Delta \text{ticks}}{N_{rev}} \cdot \frac{2\pi r}{\Delta t} $$也可以等价地写成 $v = \omega_{wheel} \cdot r$，其中 $\omega_{wheel}$ 是车轮角速度（rad/s）。
如果有四轮独立轮速和转向角，还可以利用 阿克曼转向运动学模型 推算车辆的瞬时转弯半径和航向变化率——左右轮的速度差隐含了车辆的转弯信息。
数据格式
轮速数据通常通过 CAN 总线从底盘 ESC/ABS 模块获取，频率 50–100 Hz。
| |
不同车型给出的具体字段差异很大，有的只给单车速，有的给四轮脉冲计数，需要结合 CAN DBC 文件解析。
轮速在定位中的作用
轮速最重要的价值是提供 非完整性约束（Non-holonomic Constraint）——对于正常行驶的地面车辆：
- 纵向速度为主
- 侧向速度近似为零（不会横着滑）
- 垂向速度近似为零（贴地行驶）
这组约束在滤波器中相当于一个持续生效的"伪观测"，能有效抑制 IMU 的侧向和垂向漂移。
轮速的局限性：
- 打滑 / 空转：急加速、湿滑路面、ABS 介入时轮速不可信
- 轮胎半径变化：胎压、磨损、载重都会影响有效半径
- 无法提供绝对位置：只能做相对推算
GNSS（Global Navigation Satellite System，全球卫星导航系统）
概念澄清
| 术语 | 含义 |
|---|---|
| GPS | 美国的 Global Positioning System，只是 GNSS 的一个子集 |
| GNSS | 所有卫星导航系统的统称 |
| BDS | 中国北斗卫星导航系统 |
| Galileo | 欧盟卫星导航系统 |
| GLONASS | 俄罗斯卫星导航系统 |
现代接收机通常标注"多星多频 GNSS"，表示同时使用多个星座的多个频段。多星座意味着可见卫星更多、几何分布更好、定位更稳定。
定位原理
GNSS 定位的本质是 三边测量（Trilateration）：
- 接收机测量与每颗卫星之间的伪距（通过信号传播时间乘以光速）
- 至少 4 颗卫星（3 个空间维度 + 1 个钟差）联立方程，解出接收机位置
其中 $\rho_i$ 是伪距，$c \cdot \delta t$ 是接收机钟差项，$\epsilon_i$ 包含电离层延迟、对流层延迟、多径等误差。
数据格式
GNSS 的数据输出分两个层级：
高层导航结果——定位模块已解算好的位置/速度
最通用的是 NMEA 0183 协议，ASCII 文本格式：
| |
GGA 报文字段拆解：
| 字段 | 示例值 | 含义 |
|---|---|---|
| UTC 时间 | 123519 | 12:35:19 UTC |
| 纬度 | 3113.825,N | 31°13.825’N |
| 经度 | 12128.422,E | 121°28.422’E |
| Quality | 4 | 定位质量（1=单点，4=RTK Fix，5=RTK Float） |
| 卫星数 | 22 | 参与解算的卫星数量 |
| HDOP | 0.8 | 水平精度衰减因子 |
| 海拔 | 12.53 | 米 |
常用 NMEA 语句：
| 语句 | 内容 |
|---|---|
| GGA | 定位质量、坐标、高度、卫星数、DOP |
| RMC | 推荐最小导航信息：坐标、速度、航向 |
| GSA | 参与定位的卫星编号、PDOP/HDOP/VDOP |
| GSV | 可见卫星详情：仰角、方位角、信噪比 |
| VTG | 地面速度与航向 |
原始观测量——用于高精度定位算法
| |
原始观测量通常以厂家二进制协议传输（如 u-blox UBX、NovAtel OEM），效率更高、信息更全。业务层一般不直接处理原始观测，而是交给定位引擎内部的 GNSS 解算模块。
GNSS 的特点
优势：提供 WGS84 下的全局绝对坐标，误差不随时间累积
劣势：
- 频率低：常见 1–20 Hz
- 城市峡谷多径效应严重，定位跳变
- 隧道、地库、高架桥下完全丢失
- 单点定位精度仅米级
RTK（Real-Time Kinematic，实时动态载波相位差分）
原理
RTK 不是一种新传感器，而是一种 提升 GNSS 精度的技术。
普通 GNSS 基于伪距测量，精度受限于码相位分辨率（米级）。RTK 的核心改进是利用 载波相位观测，其波长约 19 cm（L1 频段），理论分辨率远高于伪距。
但载波相位观测有一个固有难题——整周模糊度（Integer Ambiguity）：接收机只能测量不足一个波长的相位差，无法直接知道信号经历了多少个完整波长。RTK 的关键就是解算出这个整数。
工作流程：
- 基准站（已知精确坐标的参考站或 CORS 网络虚拟站）持续观测卫星
- 基准站将自身观测数据通过网络实时发送给 流动站（车端接收机），传输协议通常是 NTRIP，数据格式为 RTCM
- 流动站做差分：基准站和流动站对同一颗卫星的观测误差高度相关（电离层、对流层、星历误差），做差后大幅消除
- 在差分观测量上解算整周模糊度，解算成功即获得 Fix 解
定位状态
| 状态 | Quality 值 | 精度 | 含义 |
|---|---|---|---|
| Single | 1 | 米级 | 普通单点定位，无差分 |
| DGPS | 2 | 亚米级 | 伪距差分 |
| Float | 5 | 分米级 | 载波相位差分进行中，整周模糊度未固定 |
| Fix | 4 | 厘米级 | 整周模糊度固定，最高精度 |
RTK 数据格式
输入端（改正数据）：车端通过 NTRIP 协议从基站/CORS 网络接收 RTCM 3.x 格式的差分数据流。RTCM 是二进制协议，包含基站观测量、基站坐标、多星座多频改正等。
输出端（定位结果）：依然是 NMEA 报文或厂家二进制协议，但定位质量字段会变化。典型输出：
| |
RTK 的局限
RTK 不等于"永远厘米级"：
- 需要稳定的网络链路接收改正数据
- 卫星信号质量必须足够好（开阔天空）
- 城市高楼、高架、林荫道会导致 Fix 退化为 Float 甚至 Single
- 多径效应会让整周模糊度固定失败
- 改正数据龄期过大（基站数据延迟）会降低精度
坐标系链路
GNSS/RTK 输出的是 WGS84 经纬高，而定位系统内部需要统一的笛卡尔坐标。典型转换链路：
$$ \text{WGS84 经纬高} \xrightarrow{\text{地理→地心}} \text{ECEF} \xrightarrow{\text{地心→局部}} \text{ENU / NED} \xrightarrow{\text{局部→地图}} \text{Map 坐标系} $$- ECEF（Earth-Centered Earth-Fixed）：以地球质心为原点的直角坐标系
- ENU（East-North-Up）/ NED（North-East-Down）：以某个参考点为原点的局部切平面坐标系，便于和 IMU、轮速等传感器的输出统一
传感器融合：怎么组合做定位
各传感器的分工
把四种传感器的特性放在一起看：
| 传感器 | 频率 | 输出性质 | 优势 | 致命弱点 |
|---|---|---|---|---|
| IMU | 100–500 Hz | 角速度 + 比力 | 高频、连续、不依赖外部信号 | 积分漂移 |
| 轮速计 | 50–100 Hz | 轮速 / 脉冲数 | 约束地面运动、低速稳定 | 打滑、无绝对位置 |
| GNSS | 1–20 Hz | 全局经纬高 | 绝对位置、不漂移 | 低频、易受遮挡 |
| RTK | 1–20 Hz | 高精经纬高 | 厘米级绝对精度 | 依赖网络和天空 |
它们的互补关系可以概括为一句话：IMU 做连续传播，轮速做运动约束，GNSS/RTK 做全局校正。
误差状态卡尔曼滤波（ESKF）
ESKF 是车载组合导航最经典的框架。
状态向量通常包含 15–21 维：
$$ \delta \mathbf{x} = \begin{pmatrix} \delta \mathbf{p} \\\\ \delta \mathbf{v} \\\\ \delta \boldsymbol{\theta} \\\\ \delta \mathbf{b}_g \\\\ \delta \mathbf{b}_a \end{pmatrix} $$| 分量 | 维度 | 含义 |
|---|---|---|
| $\delta \mathbf{p}$ | 3 | 位置误差 |
| $\delta \mathbf{v}$ | 3 | 速度误差 |
| $\delta \boldsymbol{\theta}$ | 3 | 姿态误差（小角度近似） |
| $\delta \mathbf{b}_g$ | 3 | 陀螺零偏误差 |
| $\delta \mathbf{b}_a$ | 3 | 加速度计零偏误差 |
工作循环：
- 预测（Prediction）：每来一帧 IMU（200 Hz），用捷联惯导方程传播标称状态（姿态、速度、位置），同时传播误差状态的协方差
- 更新（Update）：当轮速、GNSS、RTK 数据到达时，计算观测残差，用卡尔曼增益修正误差状态，然后将修正量注入标称状态
- 零偏估计：GNSS/RTK 的绝对位置观测使得滤波器能持续估计并补偿 IMU 的陀螺零偏 $\mathbf{b}_g$ 和加速度计零偏 $\mathbf{b}_a$，这是抑制漂移的关键
为什么用"误差状态"而不是直接滤波：姿态存在于 $SO(3)$ 流形上，直接对旋转矩阵或四元数做加减没有物理意义。误差状态 EKF 对小量误差在切空间里做线性化，数值更稳定、线性化近似更准确。
因子图优化
更先进的系统使用因子图（Factor Graph）做滑动窗口优化：
- 节点：各时刻的状态（位置、速度、姿态、零偏）
- 因子（边）：
- IMU 预积分因子（连接相邻状态）
- GNSS/RTK 因子（绝对位置约束）
- 轮速因子（速度约束）
- 视觉/激光雷达因子（如果有）
优势是能处理更多异步约束、全局一致性更好；代价是计算量更大、工程实现更复杂。
典型方案
量产乘用车（成本敏感）
| 传感器 | 配置 |
|---|---|
| IMU | 工业级，200 Hz |
| 轮速 | 四轮 CAN，50 Hz |
| GNSS | 单天线，多星座，10 Hz |
| 融合算法 | ESKF |
| 精度 | 亚米级 ~ 米级 |
Robotaxi / 采图车（精度优先）
| 传感器 | 配置 |
|---|---|
| IMU | 战术级，200–400 Hz |
| 轮速 | 四轮 CAN + 高精度编码器 |
| GNSS | 双天线 RTK，10–20 Hz |
| 融合算法 | ESKF 或因子图 |
| 精度 | 厘米级（RTK Fix 条件下） |
离线真值系统（数据闭环）
在离线环境下，可以使用 PPK（Post-Processing Kinematic） 替代实时 RTK，并结合 RTS Smoother（前向+后向平滑），利用所有传感器数据前后双向滤波，压榨出最高精度的连续位姿真值，用于感知和预测模型的评估。
工程落地的关键前置条件
再好的融合算法也建立在三个基础之上：
时间同步
IMU 200 Hz、轮速 50 Hz、GNSS 10 Hz——频率不同、时钟源不同。如果时间戳对不齐，融合结果会严重抖动甚至发散。
常见同步手段：
- GNSS PPS（秒脉冲）硬件触发
- NTP/PTP 网络时间协议
- 硬件时间戳 + 软件插值
外参标定
各传感器的安装位置不同，必须知道它们相对车体坐标系的刚体变换：
- IMU 到车体的旋转和平移
- GNSS 天线到车体参考点的 杠杆臂（Lever Arm）——天线通常装在车顶，IMU 在车身中部，转弯时杠杆臂会产生明显的位置偏差
- 轮速参考点（后轴中心）到车体原点的偏移
杠杆臂补偿
GNSS 天线测到的是天线自身的位置，而定位系统需要的是车体参考点的位置。两者之间的偏差就是杠杆臂 $\mathbf{l}$：
$$ \mathbf{p}_{body} = \mathbf{p}_{antenna} - R \cdot \mathbf{l} $$如果不补偿杠杆臂，车辆转弯时 GNSS 位置和 IMU 推算位置之间会出现系统性偏差，滤波器会被带偏。