自动驾驶中的传感器技术85——Sensor Fusion（8）

现在是PVT

这里的 PVT 指的是全球卫星导航系统（GNSS）直接输出的最终解算结果：

P (Position): 位置（经度、纬度、海拔）
V (Velocity): 速度（北向速度、东向速度、天向速度）
T (Time): 时间（UTC 时间、接收机钟差）

PVT 融合的核心逻辑是：将 GNSS 芯片已经算好的"位置和速度"，与 IMU（惯性测量单元）的"加速度和角速度"进行融合。

1. 单传感器 GNSS 基础定位算法

算法	关键原理	适用场景	备注
伪距定位	通过测量卫星到接收机的传播时间（伪距）求解三维坐标。	开阔环境、对精度要求不高（≈1 m）	受多路径、离子层、对流层误差影响大。
载波相位定位	利用载波相位的相对测量，可实现亚米甚至厘米级精度。	高精度需求的定位（RTK、PPP）	需要整数模糊度解算，计算复杂。
PPP（Precise Point Positioning）‍	采用精密星历、模型校正，单站即可实现厘米级定位。	全球范围、无基准站的高精度定位	对卫星可见性要求高，收敛时间相对较长。
RTK（Real‑Time Kinematic）‍	基站提供差分观测，快速消除共模误差，实现厘米级定位。	城市道路、车道保持、自动泊车等	需要基站或网络RTK服务，基站遮挡时性能下降。

这些算法是车载 GNSS 感知的"底层"。在实际系统中往往与后续的多传感器融合一起使用，以提升鲁棒性。

2. 多传感器融合（GNSS + IMU + 其他）

2.1 融合模式

融合方式	说明	常用实现
松耦合（Loose Coupling）‍	GNSS 先独立解算出位置/速度，再与 IMU 进行卡尔曼滤波融合。	适用于 GNSS 信号较好、对实时性要求不高的场景。
紧耦合（Tight Coupling）‍	将原始 GNSS 伪距、载波相位直接与 IMU 观测一起进入滤波器。	在 GNSS 信号遮挡或弱视情况下仍能保持定位（）。
深耦合（Deep Coupling）‍	GNSS 基带处理（如相位解算）与 IMU 融合在同一芯片内部完成，误差模型更紧密。	适用于高端车规级芯片，提升抗干扰能力。

2.2 常用滤波框架

扩展卡尔曼滤波（EKF）

通过线性化雅可比矩阵实现非线性系统状态估计。
在车载 GNSS/IMU 融合中最为常见，能够实时提供位置、速度、姿态。。
无迹卡尔曼滤波（UKF）
通过 sigma 点传播避免显式求雅可比，适用于高度非线性场景。
粒子滤波（PF）
采用蒙特卡罗采样处理多模态分布，尤其在 GNSS 信号出现大尺度遮挡时表现更稳健。
因子图优化（FGO）
将 GNSS、IMU、里程计等观测构建为因子图，使用非线性最小二乘求解，可实现全局一致性和后端优化。

2.3 鲁棒 EKF 及自适应技术

抗差 EKF（Robust EKF）‍：在观测残差中加入 Huber、M‑估计等抗差函数，抑制多路径、粗差的影响。。
自适应 EKF（Adaptive EKF）‍：根据残差统计动态调节过程噪声 Q 与观测噪声 R，提升在卫星数量不足或几何分布差的环境下的稳定性。。
双窗口 Sage‑Husa 抗差 EKF：进一步结合双窗口残差评估，实现更强的异常检测与切换。。

这些鲁棒/自适应方案在城市峡谷、隧道等 GNSS 信号受阻的车载场景中被广泛验证，可将定位误差从分米级压缩至厘米级。

3. 多系统 GNSS 融合（GPS + BDS + Galileo + GLONASS）

通过统一的时间、坐标框架，将四大系统的观测一起输入 EKF，实现更高的可见卫星数和几何强度。。
在中长基线（>10 km）情况下，结合 EKF 对大气误差进行实时估计，可显著提升 RTK 固定率并缩短收敛时间

4. 环境感知与 NLOS / 多路径抑制

机器学习/深度学习检测：利用卷积神经网络或图神经网络对 GNSS 原始观测的信噪比、伪距残差等特征进行分类，自动标记 NLOS（非视线）观测并剔除。
多频、多星座协同：不同频段的观测在多路径特性上存在差异，融合后可通过差分方法抑制多路径误差。

虽然上述技术在学术论文中已有初步验证（如 2023‑2024 年的多模态感知研究），在车载产品中正逐步落地。

5. 与高精度地图、SLAM 的闭环感知

地图匹配（Map‑Matching）‍：将 GNSS 轨迹投影到高精度道路网络或车道中心线，实现位置纠偏。。
视觉/激光 SLAM 融合：在 GNSS 完全失效的隧道或地下场景，使用视觉里程计或激光雷达构建局部地图，并通过因子图与 GNSS/IMU 融合实现"相对定位 + 绝对校正"。。

这种感知闭环在 L4/L5 自动驾驶系统中是实现全场景定位的关键。

6. 在智能驾驶系统中的定位角色

车载 GNSS 感知是智能驾驶感知层的基石，直接支撑以下功能：

车道保持与路径规划：提供全局坐标基准，配合高精度地图实现车道级定位。。
V2X（车路协同）定位：通过 C‑V2X 与 GNSS 融合，实现车间相对定位与协同决策。。
冗余安全定位：在多传感器融合框架下，GNSS 与 IMU、雷达、摄像头形成冗余链路，满足功能安全（ASIL D）要求。。

7. 典型算法实现流程（示例）

原始数据预处理

GNSS：伪距、载波相位、卫星状态（SNR、Eph）过滤。
IMU：零偏校准、噪声模型（Allan 方差）估计。
状态模型建立
车辆动力学（六自由度）+ IMU 误差状态。
观测模型构建
紧耦合 GNSS 伪距/相位 → 观测方程。
IMU 加速度、角速度 → 预测方程。
滤波/优化
EKF（或鲁棒 EKF）进行递推预测‑更新。
若需要全局一致性，可在后端使用因子图优化（FGO）进行滑动窗口平滑。
异常检测与剔除
基于残差的 χ² 检验、Huber 损失或机器学习分类。
输出融合定位
位置（经纬度/ENU）、速度、姿态、置信度。
同时输出 GNSS 质量指示（PDOP、卫星数）供上层决策使用。

8. 发展趋势与挑战

趋势	说明
多频、多星座、全局导航	5G/6G 与 GNSS 网络协同，提供更高的可用性。
因子图与图优化的实时化	结合 GPU/FPGA 加速，实现毫秒级全局优化。
深度学习驱动的 NLOS 检测	端侧模型轻量化，实时剔除异常观测。
车路协同定位（V2X‑GNSS 融合）‍	利用路侧基站、RSU 提供短时差分或伪距校正。
安全可靠性验证	通过仿真+实车闭环测试，满足 ISO 26262 / 功能安全等级。

结语车载 GNSS 感知算法已经从单一的伪距定位演进为多传感器、鲁棒、实时优化的综合体系。核心技术包括 RTK/PPP 高精度解算、紧耦合 GNSS‑IMU 融合（EKF、UKF、PF、因子图）、抗差/自适应滤波以及与高精度地图/SLAM 的闭环结合。随着多频多星座 GNSS、车路协同和 AI 驱动的异常检测进一步成熟，车载 GNSS 感知将在 L4/L5 自动驾驶的全场景定位中发挥决定性作用。

基础原理与数据模型

传感器组成：三轴加速度计 + 三轴陀螺仪（部分型号还包括三轴磁力计），提供线性加速度、角速度（以及磁场）信息。
姿态表示：常用四元数、方向余弦矩阵（DCM）或欧拉角（Yaw‑Pitch‑Roll）来描述车辆姿态，避免奇异点并便于数值积分。
误差模型：IMU 零偏、比例因子、噪声等误差通常采用一阶高斯马尔可夫（Gauss‑Markov）模型描述，便于在滤波器中进行状态预测。

单体惯性导航（INS）算法

步骤	说明
时间更新（预测）‍	根据 IMU 的加速度和角速度，利用刚体动力学方程对位置、速度、姿态进行积分（惯性导航机械化）。
误差状态传播	将误差模型（零偏、比例因子）加入状态向量，使用线性化的误差动力学方程进行传播。
姿态更新	采用四元数或 DCM 的递推公式，保持数值稳定性。
漂移校正	纯惯性导航在数十秒后会出现显著漂移，需要外部约束（如 GNSS、车轮里程计）进行校正。

传感器融合框架

3.1 扩展卡尔曼滤波（EKF）

核心思路：将 IMU 误差状态与外部观测（GNSS 伪距、速度、车轮里程计）放入同一状态向量，利用 EKF 的线性化预测‑更新步骤实现紧耦合融合。
常见约束：
零速度更新（ZUPT）‍：车辆停稳时强制速度为零，抑制漂移。
非完整约束（Non‑holonomic）‍：车辆侧向和垂直速度近似为零，提升横向定位精度。
零积分航向率（ZIHR）‍：在直线行驶段约束航向变化率。

3.2 无迹卡尔曼滤波（UKF）

对高度非线性系统（如四元数姿态）提供更精确的统计线性化，适用于高动态驾驶场景。

3.3 粒子滤波（PF）

在多模态误差分布或 GNSS 完全失效（隧道）时，可通过大量粒子保持姿态/位置的概率分布，提升鲁棒性。

3.4 互补滤波（Complementary Filter）

轻量级实现，利用低频 GNSS/里程计信息校正高频 IMU 积分误差，常用于嵌入式平台。

紧耦合 GNSS‑IMU 融合（车载定位）

紧耦合结构：在滤波器内部直接使用原始 GNSS 伪距、伪距速率（Doppler）以及时间差分载波相位（TDCP）作为观测，结合 IMU 状态进行联合估计。
优势：
在 GNSS 信号遮挡（城市峡谷、隧道）期间仍能保持连续定位，误差在 5‑6 m 量级。
通过 TDCP 计算航向角，可在无 GNSS 时提供可靠的航向约束。
实现要点：

状态向量包含位置、速度、姿态误差、IMU 零偏、比例因子、GNSS 时钟误差。
观测方程对伪距、Doppler、TDCP 进行线性化，加入噪声模型（SNR‑dependent）。
滤波步骤：初始化 → 时间更新 → 多源观测更新（GNSS、ZUPT、非完整约束） → 重置/再初始化。
多传感器融合的高级应用

融合方案	关键算法	适用场景
视觉‑惯性里程计（VIO）‍	EKF‑VIO、MSCKF、OpenVINS	高精度定位、SLAM、低速城市道路
激光雷达‑惯性融合（LIO）‍	LIO‑SAM、FAST‑LIO	高速公路、复杂城市环境、夜间/雨雾
车轮里程计‑IMU 融合	卡尔曼滤波 + 轮速约束	低速泊车、短时 GNSS 失效
深度学习校正	RNN/LSTM 对 IMU 零偏进行预测、端到端姿态估计	高动态运动、传感器退化检测

近年来，随着高精度 MEMS 与 AI‑驱动的自适应滤波技术成熟，车载 IMU 感知正向端到端学习‑融合方向发展，能够在极端噪声环境下自动调节滤波参数，实现更稳健的定位与姿态估计。

实际工程实现要点
标定：在出厂前完成加速度计、陀螺仪的温度补偿与轴间对准；现场可通过静态/动态标定进一步降低零偏漂移。
计算平台：实时 EKF/UKF 计算量约 10‑20 µs（单核 DSP），适合车规级 MCU；深度学习模型需在车载 GPU/AI 加速器上运行。
容错设计：在 GNSS 完全失效时切换至纯惯性死算（Dead‑reckoning）或 VIO/LIO；使用冗余 IMU（双 IMU）提升可靠性。
软件架构：推荐采用 ROS 2 或 AUTOSAR Adaptive 中间件，实现模块化的传感器数据流、滤波器节点与状态发布。
核心架构：松耦合 (Loose Coupling)

PVT 融合处于组合导航融合层级中的最上层（也是最简单的一层）。

输入 A: GNSS 接收机输出的 PVT 结果。
注意：这里不涉及卫星的伪距或载波相位，只用芯片吐出来的最终经纬度。
输入 B: IMU 输出的原始数据（比力/加速度 + 角速度）。
融合算法: 扩展卡尔曼滤波 (EKF)。
预测步骤 (Time Update): 利用 IMU 的高频数据进行积分（航位推算），预测下一时刻的位置和速度。
更新步骤 (Measurement Update): 当 GNSS 的 PVT 数据到来时（通常 1Hz - 10Hz），将其作为"观测值"，计算观测残差（Measurement Residual），用来修正 IMU 的推算误差，并估计 IMU 的零偏（Bias）。

PVT 融合的优缺点

相比于之前提到的"Raw Data 级融合（紧耦合）"，PVT 融合有非常鲜明的特点：

优点：

工程实现简单 (Easy Integration):

GNSS 模块是一个独立的"黑盒"。你只需要解析标准的 NMEA 协议（如 $GNGGA,$ GNRMC）拿到经纬度即可，不需要懂卫星信号处理算法。
模块化强 (Modular):
你可以随意更换 GNSS 接收机的品牌（如从 u-blox 换到和芯星通），只要它们都输出 PVT，后端的融合算法几乎不用改。
算力消耗小:
卡尔曼滤波的状态量较少，对车载 MCU 压力很小。

缺点：

抗干扰能力弱 (Less Robust):

级联失效问题： GNSS 芯片内部需要至少 4 颗卫星才能解算出 PVT。如果进入城市峡谷，只能看到 3 颗卫星，GNSS 芯片就会停止输出 PVT（或输出无效值）。此时，整个融合系统就失去了校正源，只能靠 IMU 硬抗（并在几秒后飘飞）。
对比：紧耦合（Raw Data 融合）即使只有 2 颗卫星，也能利用这 2 颗卫星的伪距来约束 IMU，不会彻底失效。
无法处理多路径效应 (Multipath):
在玻璃幕墙附近，GNSS 算出的 PVT 可能会因为信号反射产生几十米的跳变。因为是"松耦合"，融合算法很难判断这个跳变是真是假，容易被带偏。
不可逆的精度损失:
GNSS 芯片内部解算 PVT 时已经使用了一次卡尔曼滤波，这引入了过程噪声相关性。再在外部做一次融合，理论上不符合卡尔曼滤波关于噪声独立性的假设（Incestuous Data Problem），限制了最终精度的上限。

应用场景

车载导航/车机 (IVI): 绝大多数普通的车载导航都使用 PVT 融合。当车辆进入隧道丢失 GPS 时，利用车速脉冲和简易 IMU 维持几分钟的图标移动。
低速/低成本机器人: 扫地机、割草机等。
入门级 ADAS (L2): 对于车道级定位要求不高的功能，PVT 融合配合高精地图匹配（Map Matching）通常够用。

总结：PVT 融合 vs. SPP/RTK

在实际应用中，PVT 融合中的 "PVT" 来源有两种精度级别：

SPP-PVT 融合 (单点定位):

普通 GPS 模块，精度 3-5 米。融合 IMU 主要是为了平滑轨迹，填补 1Hz GPS 之间的空隙，让导航光标看起来顺滑。
RTK-PVT 融合 (高精度):
GNSS 模块内部集成了 RTK 引擎，直接输出厘米级的 PVT。
融合算法利用 IMU 主要是为了解决**短时遮挡（如过桥下、树荫）**时的定位连续性问题。

一句话概括：PVT 融合就是把 GNSS 当作一个位置传感器，用它的读数来校准 IMU 的积分漂移，是目前最通用的低成本组合导航方案。