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

本文继续总结各传感器的算法

1、传感器的信号处理

这是指**传感器模组内部（Edge端）**的处理逻辑。在数据传输到智驾主芯片（如 Orin-X）之前，传感器内部的 ISP、DSP、FPGA 或 MCU 已经对原始物理信号进行了大量的预处理。

这些算法决定了传感器输出数据的质量（如信噪比、动态范围、点云精度）。

传感器	核心算力单元	关键内部算法	目的
camera	isp	Demosaic, HDR合成, Tone Mapping	将光信号变为人/机器可看的图像
lidar	fpga	TDC计时, 多回波甄别, 强度校正	将光脉冲变为精确的 XYZ 坐标
radar	dsp	3D-FFT, CFAR	从电磁波中提取距离、速度、角度
uss	asic	余振消除, TVG	过滤近处杂波，探测远处障碍
gnss	baseband	PLL/DLL跟踪, RTK解算	消除大气误差，实现厘米级定位
imu	mcu	温度补偿, 低通滤波	消除温漂和震动，输出纯净的惯性数据

1.1 摄像头

摄像头的信号处理算法前文已经有介绍。

摄像头的原始数据是光电二极管产生的 RAW 数据（Bayer Pattern）。模组内的 ISP (Image Signal Processor) 需要运行以下核心算法将其转化为可视图像：

黑电平校正 (BLC - Black Level Correction): 扣除暗电流噪声，确保全黑环境下像素值为 0。
镜头阴影校正 (LSC - Lens Shading Correction): 修复由于镜头光学特性导致的"中间亮、四周暗"现象。
坏点矫正 (DPC - Defect Pixel Correction): 识别并用周围像素插值修复传感器上的坏点。
去马赛克 (Demosaicing / Debayering): 将拜耳阵列（只有 R, G, G, B 单色信息）通过插值算法还原为全彩 RGB 图像。
自动白平衡 (AWB - Auto White Balance): 核心算法（如灰度世界法、完美反射法），在不同光源色温下还原白色。
高动态范围合成 (HDR / WDR): （车规级最关键）
多帧合成: 将长、中、短曝光的三帧图像对齐并融合。
Tone Mapping (色调映射): 将合成后的高位宽（如 20bit）数据压缩到显示或传输位宽（12bit/8bit），同时保留亮部和暗部细节。
降噪 (Denoising): 2D/3D 空域和时域降噪，去除低照度下的噪点。

1.2 Lidar

LiDAR 接收的是光电探测器（APD/SPAD）输出的微弱电压脉冲信号，内部处理主要由 FPGA 完成。

TDC (Time-to-Digital Converter) / ADC 采样:
将光脉冲的飞行时间（ToF）转化为数字计数值，精度通常在皮秒级。
波形处理与峰值检测 (Peak Detection):
在回波波形中寻找能量峰值时刻。算法需要区分真实回波和太阳光噪声。
多回波处理 (Multi-Echo Processing):
算法识别出雨滴、雾气产生的微弱前回波，和墙壁/车辆产生的强后回波。通常输出"最强回波"和"最后回波"。
强度计算 (Intensity Calculation):
根据回波脉冲的宽度（Pulse Width）或幅度，计算目标物体的反射率（Intensity），用于识别路牌或车道线。
坐标转换:
利用内部标定表，将 (距离,垂直角度,水平角度)(距离,垂直角度,水平角度) 转化为笛卡尔坐标 (x,y,z)(x,y,z)。

硬件层面：ToF/相位测距 + 旋转或 MEMS 扫描 + 姿态融合，决定原始回波的时间、角度、姿态信息。
算法层面：坐标变换 → 去噪/下采样 → 地面分割（RANSAC） → 聚类 → 表面重建 → 配准。每一步都有成熟的实现（PCL、Open3D、ROS）。
深度学习层面：扩散/分数匹配模型（LiDARGen、RangeLDM）能够在保持物理约束的前提下生成或增强点云，已成为数据增强和仿真生成的热点。
实现建议：在实际项目中，先使用传统管线完成实时点云获取与基本滤波；随后在离线或仿真阶段引入深度学习生成模型，以提升数据多样性和稀疏‑密集转换效果。

可以参考下文：

激光点云系列之一：详解激光雷达点云数据的处理过程

车载激光雷达（LiDAR）信号处理的典型流程

激光发射与扫描

激光器（如EEL、VCSEL）产生短脉冲激光，经过发射光学系统（透镜、分光器）向外投射。

扫描模块（机械转镜、MEMS、光学相控阵）按照预设的角度序列快速改变发射方向，实现对周围空间的逐点扫描。

光信号回收

目标物体表面反射的激光被接收光学系统聚焦到光电探测器（APD、SiPM 等），转换为微弱的光电流信号。

前端模拟处理

接收信号经过低噪声放大器（TIA）放大，随后进行模拟滤波（带通/低通）去除高频噪声和干扰。

部分系统在此阶段加入自动增益控制（AGC）以适应不同反射强度。

模数转换（A/D）或时间‑数字转换（TDC）‍

放大后的模拟信号被 ADC（或专用的 TDC）采样，得到数字化的回波幅度和到达时间信息。

对于飞行时间（ToF）测距，关键是精确捕获脉冲往返的时间差 Δt。

初步数据预处理

去噪与背景剔除：滤除多余回波、强光干扰、无效点（如大气散射、透明物体）。

多回波分离：对同一激光脉冲返回的多次回波进行分类（地面、障碍物、透明体）。

坐标转换与点云生成

将每个回波的距离、水平/垂直角度（由扫描模块提供）转换为车辆坐标系下的三维坐标 (x, y, z)。

形成原始点云（raw point cloud），通常以标准格式（如 Velodyne、Apollo）输出。

点云校准与配准

通过内部标定参数（激光器‑接收器时延、扫描角度误差、外参）对点云进行几何校正，确保空间精度。

与车载 IMU、GNSS 等其他传感器数据进行时间同步。

点云过滤与分割

地面剔除：基于高度阈值或平面拟合去除道路地面点。

聚类/分割：使用欧氏聚类、DBSCAN、Voxel‑grid 等算法将点云划分为独立目标簇。

目标检测与特征提取

对每个簇提取形状、尺寸、反射强度等特征，或直接使用深度学习模型（如 PointNet、SECOND）进行物体类别识别（车辆、行人、障碍物等）。

目标跟踪

基于卡尔曼滤波、匈牙利匹配或更高级的多目标跟踪（MOT）算法，对检测到的目标在连续帧之间进行关联，输出位置、速度、加速度等运动状态。

高层感知融合

将 LiDAR 点云结果与摄像头、毫米波雷达、超声波等传感器的数据进行融合，提升感知鲁棒性与精度。

融合后输出统一的环境模型，供路径规划、决策控制使用。

输出与接口

处理后的感知信息以标准消息（如 ROS2 / Apollo / AUTOSAR）发送至车载计算平台，供后续的自动驾驶软件栈使用。

要点概览

硬件层：激光发射 → 扫描 → 接收 → 前端放大/滤波 → ADC/TDC。

数字层：A/D 转换 → 预处理 → 坐标映射 → 校准 → 点云生成。

感知层：过滤/分割 → 目标检测 → 跟踪 → 多传感器融合。

以上步骤构成了车载激光雷达从信号发射到最终感知输出的完整链路，涵盖了硬件信号链、数字信号处理以及高层感知算法的主要环节。

1.3 Radar

雷达是计算密集型传感器，模组内部进行大量的频域变换。

ADC 采样与预处理: 对混频后的中频（IF）信号进行采样。
距离 FFT (Range FFT - 1D FFT):
对 Chirp 信号进行快速傅里叶变换，频率峰值对应目标的距离。
多普勒 FFT (Doppler FFT - 2D FFT):
在多个 Chirp 之间进行 FFT，计算相位变化率，从而解算出目标的相对速度。
角度 FFT (Angle FFT - 3D FFT / DBF):
在多个天线通道之间进行 FFT，或使用 DBF (数字波束形成)、超分辨率算法 (MUSIC/ESPRIT) 计算目标的方位角和俯仰角。
CFAR (恒虚警率检测 - Constant False Alarm Rate): （核心算法）
动态设定阈值，在充满杂波（地面反射、底噪）的环境中提取出真实的目标点。
聚类与跟踪 (Clustering & Tracking):
将杂乱的点云聚类成物体，并使用卡尔曼滤波（Kalman Filter）跟踪物体的运动轨迹（输出 Object List）。

Radar的信号处理算法前文已经有介绍。

距离 FFT（Range FFT）：对每个 Chirp 的采样点在快速时间维做 1‑D FFT，得到距离谱。

多普勒 FFT（Doppler FFT）：对同一目标在多帧（慢时间）上的距离谱做 2‑D FFT，得到距离‑速度（R‑D）谱。

角度 FFT / 数字波束形成（Beamforming）：利用 MIMO 天线阵列的空间采样，在空间维做 FFT 或采用波束形成算法，得到 DOA（到达方向）‍ 信息。

典型实现为 "Range‑FFT → Doppler‑FFT → Angle‑FFT"，形成三维数据立方体（Range‑Doppler‑Angle）。

检测与阈值控制（CFAR）：对 R‑D‑Angle 立方体的每个点执行恒虚警率（CFAR）‍ 检测，动态设定阈值并抑制杂波。只保留满足阈值的点目标（点云）‍，显著降低虚警率。CFAR 是雷达链路中关键的噪声抑制与目标筛选步骤。

后处理：聚类、跟踪与输出，峰值提取 & 非相干累积，对同一目标在多个天线或多帧上的峰值进行相干或非相干累加，提高 SNR。聚类（如 DBSCAN）‍将相邻的点目标聚合为目标簇（即目标的点云），得到每个目标的距离、速度、角度等属性。目标跟踪基于卡尔曼滤波 / 多假设跟踪（MHT）‍ 等算法，对每帧的目标簇进行状态预测与关联，输出稳定的轨迹信息。

1.4 USS

超声波主要处理声波的包络信号。

余振消除 (Ring-down Cancellation):
算法屏蔽探头刚发射完波后的高频余振信号，消除近距离盲区内的假阳性。
TVG (时变增益控制 - Time Varying Gain):
随着时间推移（距离增加），回波信号会衰减。算法需要随时间线性增加放大器的增益，以探测远处的物体。
自适应阈值比较:
根据环境噪声水平，动态调整判定障碍物的电压阈值。
回波宽度分析:
根据接收到的回波持续时间，粗略判断障碍物是墙壁（宽回波）还是细杆（窄回波）。

车载超声波传感器的信号处理流程概览

发射脉冲生成

控制电路向换能器（Transducer）发送宽度为几微秒、频率约 40 kHz 的超声波脉冲（Burst）。

脉冲由专用驱动芯片（如 TI PGA460）产生，能够在短时间内提供高功率的正弦波列。

声波传播与回波

超声波在空气中以声速传播，遇到障碍物后产生反射回波。回波的到达时间与障碍物距离成正比。

接收前端（模拟前端）‍

换能器接收：换能器把回波转换为微弱的电信号。

前置放大：信号首先经过低噪声放大器（LMP7711 等）提升幅度。

带通滤波：两级带通滤波器（≈ 40 kHz）去除非目标频段的噪声。

自动增益控制（AGC）‍：根据回波强度动态调节增益，保证后续电路工作在合适范围。

阈值检测与包络提取

放大、滤波后的模拟信号进入阈值比较器，只有超过设定阈值的部分被保留下来，形成数字化的回波脉冲。

包络检测（Envelope）或整流后得到的脉冲宽度即为往返时间的粗略指示。

时间测量（TOF）‍

计时器在发射脉冲的瞬间启动，在阈值检测到回波上升沿时停止，记录发射‑接收时间差（Time‑of‑Flight）。

对于多目标或多回波，DSP 可记录多个峰值及对应宽度，以实现多目标检测。

温度补偿与声速校正

车内温度传感器提供实时温度 T，依据公式 C = C₀ + 0.607 × T（C₀≈332 m/s）计算实际声速，并对距离结果进行校正。

数字滤波与数据融合

采样得到的数字回波信号经低通/滑动平均滤波、动态评估等模块进一步抑制噪声，提升测距精度。

若使用多组传感器，还会进行数据融合（如加权平均或卡尔曼滤波）以获得更稳健的障碍物信息。

距离计算与障碍判定

根据 TOF 与校正后的声速计算距离 d = C × TOF / 2。

依据预设阈值（安全距离、盲区等）判断是否需要触发刹车或避障控制。

数据输出与车载网络

处理后的距离、目标状态等信息通过 CAN 总线、LIN 总线或以太网发送至车辆的高级驾驶辅助系统（ADAS）或主控 ECU。

部分系统还会将原始或处理后的波形数据上报给远程诊断平台用于后续分析。

车载超声波雷达的信号处理链条从发射‑接收‑前端调理‑ADC 开始，经过时间差测距、噪声抑制、温度补偿等基础步骤后，进入特征提取（差值法、Hough 变换、自适应滤波）阶段，以实现车位边缘检测、障碍物识别等功能，最终将结果通过车载通信总线输出供控制单元使用。上述流程兼顾硬件实现与算法层面的细化，能够在复杂的城市道路和停车场环境中提供可靠的距离感知。

1.5 GNSS

捕获与跟踪 (Acquisition & Tracking):
DLL (延迟锁定环): 跟踪伪码（C/A码），测量信号传播时间（计算伪距）。
PLL (锁相环): 跟踪载波相位，测量多普勒频移和载波相位值（用于高精度定位）。
PVT 解算 (Position, Velocity, Time):
通过最小二乘法或卡尔曼滤波，利用 4 颗以上卫星的伪距解算接收机位置。
RTK 引擎 (Real-Time Kinematic) - 高精度模组特有:
接收基准站的差分数据，利用整周模糊度解算 (Ambiguity Resolution, 如 LAMBDA 算法)，消除电离层、对流层误差，实现厘米级定位。

多频多星座天线捕获

车载天线需同时覆盖 L1、L2、L5 等频段，采用右旋圆极化（RHCP）以抑制多径。

天线的相位中心稳定性直接影响厘米级定位精度。

低噪声放大（LNA）与滤波

接收到的微弱射频信号首先经 LNA 放大，随后通过带通滤波器去除无关频段噪声。

下变频与模数转换（ADC）‍

放大后的射频信号在本振（LO）驱动下下变频至中频（IF），随后经高速 ADC 采样得到数字中频信号。

该过程构成 GNSS 接收机的 RF 前端，为后续基带处理提供高质量的数字信号。

基带信号处理（Baseband Processing）

子模块	主要功能	关键技术
信号捕获（Acquisition）‍	在搜索空间（码相位、频率偏移）中快速定位卫星信号的初始锁定。	采用并行搜索或 FFT‑based 方法，实现毫秒级捕获。
信号跟踪（Tracking）‍	持续锁定已捕获的卫星信号，实时估计码相位、载波相位、频率漂移。	采用 DLL（码环）和 PLL（载波环）闭环控制。
导航信息解码	从卫星的导航电文中提取星历、时钟、系统参数等。	采用卷积解码、纠错（LDPC、Turbo）等技术。
伪距/相位观测值提取	计算每颗可见卫星的伪距（C/A 码）和相位观测，用于后续定位。	需要对多路径、噪声进行滤波。

上述四个子模块构成基带处理的核心框架，形成从原始数字信号到可用观测值的转换链路。

误差校正与高精度定位

差分/实时动态定位（RTK）‍

车载终端接收基准站（或网络基准）发送的 RTCM 差分修正信息，实时消除卫星钟差、轨道误差、对流层延迟等。

通过双频或多频观测，可进一步抑制电离层延迟，实现厘米级定位。

模型校正

电离层模型（如 Klobuchar、NeQuick）用于单频设备的粗校正。

对流层模型（如 Saastamoinen）结合气压、温度、湿度进行延迟补偿。

多路径抑制：利用天线相位中心特性、信号质量监测（C/N0）以及后处理滤波（如卡尔曼滤波）降低多径误差。

输出与接口

实时定位结果

位置（经纬度、海拔）、速度（北向、东向、垂直）以及时间戳。

通过 CAN、Ethernet、NMEA0183、RTCM 等标准接口向车载 ECU、HMI、云平台推送。

质量监控

输出 C/N0、PDOP、HDOP、卫星可视度、解算状态等诊断信息，供系统自检与故障诊断使用。

安全冗余

多星座、多频、双天线冗余设计，确保在复杂城市峡谷或隧道环境下仍能保持定位可用性。

车载 GNSS 的信号处理流程从天线捕获 → RF 前端 → 基带获取/跟踪 → 导航解码 → 差分误差校正 → 多传感器融合 → PVT 解算 → 标准接口输出，形成一个闭环的高可靠性定位系统。每一步都围绕实时性、抗干扰、精度提升进行设计，满足自动驾驶、车队管理、电子收费等多场景需求

如果需要原始观测量，则为

车载 GNSS（车规级卫星定位）在接收到卫星信号后，首先会把信号转化为原始观测量，这些观测量是未经差分、滤波或融合处理的最底层数据。常见的原始观测量包括：

伪距观测（Pseudo‑Range）‍ 通过测距码（如 GPS 的 C/A 码、BDS 的 B1I 码等）计算出卫星到接收机的时间延迟，再乘以光速得到的距离值。伪距是定位的基础量，但受卫星钟差、接收机钟差、大气延迟和多径等误差影响较大。
载波相位观测（Carrier‑Phase）‍ 比较接收机内部产生的参考载波与卫星发射的载波之间的相位差，得到的相位计数可以转化为距离。相位观测的精度远高于伪距，是实现厘米级 RTK、PPP 等高精度定位的关键。
多普勒观测（Doppler）‍ 直接测量卫星信号的频率偏移，用于计算相对速度并在滤波或惯导融合时提供速度信息。多普勒观测还能帮助平滑伪距、检测相位跳变等。
信号强度（C/N₀）及锁定信息记录每颗卫星信号的载噪比（C/N₀）和锁定状态，用于评估观测质量、剔除弱信号或进行信号质量加权。部分车规级模块（如 UM620N、MXT900D）会把这些信息一起输出，以便后端解算器进行质量控制。

在车载系统中，这些原始观测量通常以 RTCM（如 1005/1006/1007/1019）或 RINEX 格式实时输出，供车载定位算法（单点定位、差分 RTK、紧耦合 GNSS/INS）直接使用。通过对伪距、相位和多普勒的联合处理，能够在城市峡谷、隧道入口等复杂环境下仍保持亚米甚至厘米级的定位精度。

1.6 IMU

IMU 的原始数据非常"脏"，必须经过滤波。

低通滤波 (Low-Pass Filtering, LPF):
滤除车辆发动机震动、路面颠簸带来的高频机械噪声（通常滤掉 >50Hz 或 >100Hz 的信号）。
温度补偿 (Temperature Compensation): （最重要）
MEMS 陀螺仪和加速度计对温度极敏感。内部 MCU 会读取温度传感器，查表或使用多项式拟合，修正零偏（Bias）和比例因子（Scale Factor）的温漂。
正交化与对准:
利用出厂标定矩阵，修正 X/Y/Z 轴非正交误差（Cross-axis sensitivity）。
AHRS 解算 (部分模组包含):
利用互补滤波或扩展卡尔曼滤波 (EKF)，融合加速度计（修正倾角）和陀螺仪（积分姿态），直接输出欧拉角（Pitch, Roll）。

传感器采集（原始数据）

传感器种类：3 轴加速度计、3 轴陀螺仪（有时再加 3 轴磁力计）。

采样频率：典型 100 Hz--200 Hz，部分高端 6 轴 IMU 可达 1 kHz。

硬件路径：传感器 → 模数转换（ADC） → 数字信号输出。

参考：6 轴 IMU 功能框图中展示了原始传感器数据进入数字滤波前的采样过程。

前端信号调理（低通/带通滤波）

低通滤波（LPF）‍：抑制高频噪声，常采用巴特沃斯或 FIR 低通。

带通/数字滤波：对陀螺仪进行动态漂移抑制，对加速度计进行重力分离。

示例：在可穿戴系统中使用 5 阶巴特沃斯低通滤波器对原始 IMU 数据进行降噪；在车载系统的预处理流程中，先经过低通滤波再进行后续处理。

校准与补偿

步骤	目的	常用方法
偏置校准	消除静态零点误差（bias）	零速/静止时测量并扣除
尺度因子校准	修正传感器灵敏度误差	通过已知加速度/角速度标定
轴间错位（mis‑alignment）‍	消除坐标轴不正交	矩阵变换校正
温度补偿	抑制温度漂移	在线温度传感器 + 多项式补偿

这些校准步骤在 G‑INS MW‑AHRS 手册的信号处理流程图中被明确列出。

重力与姿态分离

重力补偿：从加速度计测量中减去重力分量（利用姿态估计得到的重力方向）。
姿态积分：陀螺仪角速度积分得到姿态增量；加速度计经重力补偿后用于姿态更新（互补滤波）。

在 Strapdown INS/KF 流程中，先进行重力补偿再进入滤波器进行姿态估计。

坐标系转换

传感器坐标 → 车辆坐标：使用校准得到的旋转矩阵（R）将传感器坐标系数据映射到车体坐标系。
导航坐标（NED/ECEF）‍：在融合 GNSS 时进一步转换到全球导航坐标系。

6 轴 IMU 处理链路中提到的"旋转矩阵"步骤即完成此类坐标变换。

融合与滤波（核心估计）

方法	适用场景	关键特性
互补滤波	低算力、实时姿态估计	高通滤波陀螺仪 + 低通滤波加速度计，权衡短期动态与长期稳态
扩展卡尔曼滤波（EKF）‍	多传感器融合（IMU+GNSS+磁力计+车轮编码器）	线性化状态预测、观测更新，能够进行零速更新（ZUPT）
无迹卡尔曼滤波（UKF）‍	高非线性运动（激烈转向、颠簸）	通过 sigma 点传播更准确
深度学习/决策树	复杂场景下的异常检测或模式识别	与传统滤波器结合使用，提高鲁棒性

车载系统常用的"高级卡尔曼滤波器"在多篇文献中出现，如 IMU 与 GNSS 融合的 INS/KF 框架；6 轴 IMU 的处理流程也展示了基于卡尔曼滤波的融合模块。

输出与接口

姿态（欧拉角 / 四元数）‍、线速度、位置通过 CAN、Ethernet、RS‑232 等总线输出。
状态更新频率：姿态 100 Hz，位置/速度 10 Hz--20 Hz（视系统需求而定）。
健康监测：实时监控传感器温度、噪声水平，若异常则触发故障诊断。

常见实现要点
先校准后滤波：校准误差若在滤波前未消除，会导致滤波器收敛偏差。
分层融合：先在本地（IMU 内部）完成姿态互补滤波，后在车载 ECU 中进行 EKF 融合 GNSS、里程计等。
时间同步：所有传感器（IMU、GNSS、车轮编码器）必须统一时间基准，常用硬件时间戳或 PTP。
冗余设计：关键安全系统（如自动刹车）建议使用双 IMU 冗余并进行一致性检查。

综上所述，车载 IMU 的信号处理流程大致为：原始采样 → 前端低通/带通滤波 → 偏置/尺度/温度校准 → 重力补偿与坐标转换 → 互补/卡尔曼滤波融合 → 姿态/速度/位置输出。每一步都有成熟的实现方案，可根据具体平台的算力、精度需求以及外部传感器配置进行灵活组合。