LPC：localization planning and control，IN：lntegrated Navigation

一、核心定义

组合导航：将两种或多种导航源的信息，通过融合算法（滤波为主）取长补短，得到更高精度、更高可靠性、全场景可用的载体位姿（位置、速度、姿态）。

本质：单一导航系统都有固有缺陷，组合就是互补，抑制各自误差。
在自动驾驶 / 机器人 LPC 链路里，组合导航属于定位模块核心，输出高频位姿给到规划与控制。

最经典：GNSS/INS 组合导航（GNSS+IMU）

行业默认说组合导航，大多指 GNSS+INS；其他还有 LiDAR+IMU、VIO（视觉 + IMU）等也属于广义组合导航。

---

二、两大基础导航源原理与误差特性

1. INS 惯性导航系统（IMU 为核心）

IMU = 加速度计 + 陀螺仪

• 陀螺仪：测量载体三轴角速度 → 积分得到姿态（roll/pitch/yaw）
• 加速度计：测量三轴比力，扣除重力后积分得到速度，再积分得到位置

优点

1. 完全自主：不需要外部信号、不需要地图，不依赖环境；隧道、室内、遮挡场景都能输出数据
2. 输出频率极高：100~1000Hz，满足底盘控制高频需求
3. 短时精度极好，信号连续，无跳变

致命缺点：误差随时间积分发散（漂移）

• 陀螺仪零偏、加速度计零偏、噪声，每一个微小误差经过两次积分，位置误差会随时间平方增长。
• 低成本 IMU：几十秒位置就飘到不可用；战术级可以维持几分钟；光纤惯导能几十分钟。

一句话：IMU 短期稳，长期飘

2. GNSS 卫星导航（GPS / 北斗 / GLONASS/Galileo）

接收机接收卫星信号，解算大地坐标、速度。

优点

1. 绝对位置无漂移，长时间稳定，误差不随时间累积
2. 可输出全局坐标

缺点

1. 依赖卫星信号：城市峡谷、树荫、隧道、地下、室内，容易丢星、多路径效应，直接失效或者出现跳点
2. 输出频率低：普通接收机 1~10Hz；RTK 一般 10~20Hz
3. 信号受干扰，会出现突变野值（不是缓慢漂移，是突然跳几米）

一句话：GNSS 长期稳，容易断，低频

组合的底层逻辑

IMU 负责高频连续插值，GNSS 作为观测修正项，不断去估计并补偿 IMU 的零偏漂移；
GNSS 丢失时，IMU 独立维持一段时间位姿推算；GNSS 恢复后立刻重新校正漂移。

---

三、GNSS/INS 三种组合架构

1. 松耦合 Loosely Coupled（LC）

两个系统独立解算：INS 单独输出位姿；GNSS 接收机单独输出位置 / 速度。
滤波器输入：GNSS 解出来的位置、速度，作为观测值，去修正 INS。

流程：

IMU 原始数据 → INS 机械编排 → INS 位姿
GNSS 原始数据 → GNSS 解算 → GNSS 位置速度
EKF 融合：INS状态，观测 = GNSS 位置、速度

优点

• 结构简单，模块化，可独立调试；GNSS、INS 各自可以单独校验
• 工程落地最多，很多商用组合导航默认松耦合

缺点

• GNSS 接收机内部先做一次解算，会损失原始信息；GNSS 少于 4 颗卫星时，GNSS 无法输出位置，融合直接失效

2. 紧耦合 Tightly Coupled（TC）

不使用 GNSS 已经解算好的位置，直接拿卫星原始观测值：伪距、伪距率送入滤波器。
滤波器状态：INS 状态 + 卫星相关误差项。

优点

• 卫星数量不足 4 颗时，只要还有 1~3 颗卫星，依然可以利用残差做观测约束，抑制 IMU 漂移；
• 抗多路径能力更强，信号弱场景鲁棒性更好

缺点

• 复杂度高；需要解析 GNSS 原始电文、伪距；调试难度大；计算量更大

自动驾驶、无人机高端方案大量使用紧耦合

3. 超紧耦合 Deeply Coupled / Ultra-tight Coupled

深入到 GNSS 接收机信号跟踪环路层面，IMU 信息辅助卫星信号载波跟踪。
IMU 预测载体运动，辅助锁相环，在信号极弱、高动态场景防止载波失锁。

• 优点：高动态、强干扰场景性能最强
• 缺点：难度极大，接收机底层固件改造；民用很少，导弹、军机应用多

总结选型：
普通机器人：松耦合够用；自动驾驶车（城市峡谷）：优先紧耦合

---

四、状态方程与滤波框架（EKF 为主，组合导航标准）

组合导航最常用：扩展卡尔曼滤波 EKF；高端产品用 UKF、粒子滤波（极少）。

1. 状态向量（15 维是行业标准 15-state EKF）

X = [ δφ,δθ,δψ,  // 姿态误差 3维
      δvx,δvy,δvz,// 速度误差 3维
      δx,δy,δz,   // 位置误差 3维
      εbx,εby,εbz,// 陀螺零偏 3维
      ∇bx,∇by,∇bz ] // 加计零偏 3维

总共 15 维状态：姿态误差、速度误差、位置误差、陀螺零偏、加速度计零偏

滤波器不直接估计姿态 / 位置本身，而是估计 INS 的误差量，属于误差状态卡尔曼滤波 ES-EKF（Error State EKF，组合导航首选！）
ES-EKF 是重点：INS 做主状态正向积分，EKF 只估计误差，再把误差反馈回 INS 修正，相比直接 EKF 数值稳定性强很多。

2. 两大步骤（EKF 循环）

1. 预测步（时间更新）
   用 IMU 数据，INS 机械编排，向前递推状态与协方差矩阵；IMU 高频驱动。
2. 更新步（观测更新）
   收到 GNSS 观测（松耦合：pos/vel；紧耦合：伪距伪距率），计算残差，更新误差状态，然后反馈修正 INS 姿态、速度、位置和 IMU 零偏。

GNSS 断联期间：只有预测步，没有观测更新，协方差矩阵持续变大，误差慢慢积累，纯惯导推算。

---

五、误差源详细拆解

INS 侧误差

1. 确定性误差：零偏、刻度因子、安装角（IMU 本体和载体底盘之间的安装偏差，必须标定！）
2. 随机误差：角度随机游走、速率随机游走；Allan 方差用来标定 IMU 随机噪声参数，滤波 Q 矩阵依据 Allan 方差设置。

GNSS 侧误差

星历误差、电离层 / 对流层延迟、多路径效应、接收机噪声；RTK 可以大幅降低位置观测噪声。

时间同步误差

工程头号大坑！LPC 定位模块高频踩坑点，IMU 和 GNSS 时间戳不对齐（哪怕几十 ms），在高动态运动下，引入巨大观测残差，直接导致姿态估计跑偏。
解决方案：PTP 硬件时间同步，或者 PPS 秒脉冲给 IMU 打时间戳。

---

六、GNSS/INS 工作模式切换

1. GNSS 正常锁定（≥4 星）：融合模式，GNSS 持续修正 IMU 零偏，位姿高精度。
2. GNSS 短暂失锁：纯惯导推算，误差随时间增长；惯导等级决定能撑多久。
   • MEMS IMU（低成本）：失锁几秒位置就飘；
   • 战术级 IMU：几十秒；光纤惯导：分钟级。
3. GNSS 信号恢复：滤波器重新接收观测，快速收敛，修正累积漂移。

自动驾驶痛点：连续长隧道，GNSS 长时间丢失，纯惯导漂移超标，此时需要引入 LiDAR / 视觉地图匹配作为外部观测，构成多源组合导航：GNSS+IMU+LiDAR。

---

七、广义组合导航拓展（LPC 定位里多源融合）

1. GNSS+IMU + 轮速里程计：无人车底盘，轮速做辅助观测，抑制低速 IMU 漂移
2. LIO LiDAR+IMU：激光惯性组合，无 GNSS 场景，室内 / 隧道
3. VIO Visual-Inertial Odometry 视觉惯性：相机 + IMU
4. GNSS+IMU+LiDAR：车载全场景组合定位（当前自动驾驶主流方案）

本质不变：多个传感器，各自做状态预测或者观测，用 ES-EKF / 因子图优化融合。因子图优化（如 GTSAM）现在在 SLAM 组合定位也大量使用，和 EKF 区别：EKF 递推，因子图保留历史窗口，做滑窗优化。

---

八、组合导航输出，如何接入 LPC（定位→规划→控制）

组合导航输出：

• 全局坐标系下：经度纬度 / 局部笛卡尔坐标 x,y,z
• 三轴速度、三轴姿态 roll/pitch/yaw（航向角）
• 输出状态协方差矩阵
  协方差代表当前定位不确定性：GNSS 丢失时协方差变大，上层规划模块可以读取协方差，自动降速、限制运动范围，实现安全决策。

输出频率：IMU 驱动 100Hz 位姿输出；GNSS 观测更新 10Hz。

规划模块拿到连续高频位姿，生成轨迹；控制模块高频获取位姿跟踪轨迹。

---

九、关键指标评价

1. 定位精度：水平定位误差、高度误差；GNSS RTK 可达厘米级
2. 姿态精度：航向角 yaw（这个最难！MEMS 惯导航向漂移最严重）、横滚俯仰
3. 失锁续航时间：GNSS 丢失后，位置误差保持在阈值内的时长
4. 输出频率、延迟、时间同步精度
5. 抗干扰能力：多路径、遮挡下稳定性

十、工程常见坑总结

1. IMU 安装角没有标定，姿态永久偏；
2. 时间戳不同步，融合结果震荡；
3. 滤波 Q/R 矩阵调参错误：Q 太大相信 IMU 噪声，R 太大不信任 GNSS；
4. 坐标系混淆：IMU 系、载体系、ENU 局部坐标系、WGS84 大地坐标系转换出错；
5. 只看位置精度，忽略航向角误差，直接导致车辆控制跑偏。

---