1. 一组让人头疼的真实传感器数据滤波到底在解什么题先说个我自己的经历。有次在园区里调试一台轮式巡检机器人底盘装的是超声波测距模块用来做近距离避障。静止状态下模块读数在 38cm 到 46cm 之间来回跳偶尔还蹦出个 120cm。旁边同事看了一眼说数据滤波一下不就行了我苦笑——滤波这两个字看着简单真做起来能把人绕进去好几天。这个系列写到第五篇前面聊过感知系统怎么从原始数据里提取特征、怎么做状态估计的框架。到了滤波这一篇要处理的核心问题其实是你手里拿到的传感器数据到底有多可信怎么在充满噪声的观测中恢复出系统真实的状态很多刚开始接触智能感知的同学对滤波的理解停留在把曲线弄平滑。真到了工程现场你会发现滤波远不止是画一条好看的曲线。它要解决三件事去噪把测量噪声从信号里剥离让数据反映真实物理量。预测传感器还没来得及给出下一帧数据时系统得先猜出状态该怎么变比如帧间间隙、传感器丢帧。融合多个传感器各有优缺点怎么取长补短给出一个比任何单一传感器都可靠的估计。更关键的是现代滤波算法比如卡尔曼滤波输出的不只是一个修正后的数值还带一个不确定性估计。这个方差信息太重要了——下游的规划模块看到的不只是前方 42cm 有障碍物还有这个 42cm 到底有多可信。置信度不够的时候机器人就该降速而不是莽撞地冲过去。所以这篇不打算写成一堂数学课而是按照我实际做感知系统时的思路来先搞清楚噪声从哪来再选合适的滤波手段最后处理那些教科书里根本不会写、但现场一定会遇到的幺蛾子问题。2. 滤波算法选型低通、互补、卡尔曼、粒子谁适合你的场景很多教程一上来就是卡尔曼滤波的五个公式我觉得这是本末倒置。你连自己手里是哪种噪声、系统是线性还是非线性都没搞清楚套公式只会得到一个看起来在动但完全不可信的曲线。2.1 经典频域滤波器适合离线分析不适合在线状态估计最常见的低通滤波本质是允许低频信号通过、衰减高频噪声。比如一阶 RC 低通y[k] α * x[k] (1-α) * y[k-1]α 由采样周期和截止频率决定。实现简单到离谱嵌入式里一个全局变量就能搞定。但它的问题也很明显相位延迟。信号经过低通滤波器之后会变钝真实突变被抹平这对避障、跟踪这种对实时性要求高的场景非常致命。高通滤波反过来会丢掉直流分量和缓变趋势适合去除基线漂移。带通滤波则是两者的结合。我的建议是频域滤波器适合做数据预处理比如剔除电源纹波干扰、去除imu 的高频振动噪声但不适合做最终的状态估计——因为它在频域上做文章对状态这个概念没有建模能力更没法融合多个传感器。2.2 互补滤波姿态解算里的穷人的卡尔曼做无人机、平衡车姿态解算时互补滤波是永远不会过时的手段原理一句话就能说清陀螺仪短时间准但会漂移加速度计长时间准但噪声大两者互补。陀螺仪积分得到的姿态角高频特性好但积分会累积漂移。加速度计解算出的姿态角低频段可靠但高频噪声大。互补滤波就是让陀螺仪姿态通过高通、加速度计姿态通过低通再相加。工程实现更简单到令人发指angle 0.98 * (angle gyro_rate * dt) 0.02 * accel_angle0.98 和 0.02 就是要调的互补系数。系数越大越信任陀螺仪积分姿态越平滑但越容易漂移系数越小越信任加速度计。在嵌入式资源紧张的板子上互补滤波是性价比之王。但你发现没有它本质上是一个人为设定的固定权重权重不会根据实时数据质量变化。陀螺仪今天漂移严重了明天加速度计噪声变大了它都一概不知。它适合姿态解算但没法推广到GPSIMU轮速计这种多源异构融合场景。2.3 从卡尔曼到粒子滤波选型的本质是你对模型的信任度卡尔曼滤波做的事情用一个类比就通透了。想象你开车进隧道GPS信号没了。你只能靠车速和方向盘角度往前猜——这就是预测。出了隧道GPS恢复你会把GPS位置和刚才猜的位置做一个权衡得到一个最终位置——这就是更新。卡尔曼滤波干的就是这个而且它能算出猜和测各自的信任度用最优比例去加权。经典卡尔曼滤波有个铁约束系统必须是线性的噪声必须是高斯分布。真实世界哪有这么乖。车辆运动模型加几个转弯、加速度突变就非线性了激光雷达测到走路的人、超声波的镜面反射噪声分布根本不是高斯。这时候有两个选择扩展卡尔曼滤波EKF把非线性函数在当前估计值处做一阶泰勒展开强行线性化。实现简单但如果非线性程度太强误差会大到收敛不了。无迹卡尔曼滤波UKF用一组精心挑选的采样点sigma point直接通过非线性函数传播精度到二阶计算量增加有限。粒子滤波彻底放弃高斯假设用一堆带权重的随机粒子逼近任意分布。非高斯、非线性通吃但粒子数量一大计算量非常酸爽在嵌入式实时系统里要掂量着用。算法线性假设高斯假设计算量典型场景互补滤波不需要不需要极低姿态解算、平衡车经典卡尔曼需要需要低GPS导航、线性系统状态估计EKF弱化需要低机器人定位、SLAM前端UKF不需要需要中强非线性但仍是高斯噪声粒子滤波不需要不需要高全局定位、非高斯噪声实际工程里80% 的感知滤波需求卡尔曼或者 EKF 就够用了。粒子滤波更多出现在重定位、绑架恢复这类多峰分布场景。上来就上粒子滤波多半会把简单问题搞复杂。3. 卡尔曼滤波核心那个比例系数决定了你信模型还是信测量卡尔曼滤波之所以是感知系统的定海神针因为它做的不是平均两个数据源而是按方差最小化原则动态地算出一个最优权重。这个权重就是卡尔曼增益 K。3.1 五个公式背后的物理直觉卡尔曼滤波就两个阶段五个公式。预测阶段用模型往前推x_pred A * x_prev B * u P_pred A * P_prev * A^T Q更新阶段用观测修正K P_pred * H^T * (H * P_pred * H^T R)^-1 x_upd x_pred K * (z - H * x_pred) P_upd (I - K * H) * P_pred符号理解起来很简单x是状态向量比如位置、速度、姿态角。A是状态转移矩阵描述上一时刻的状态如何演变成当前时刻。B是控制输入矩阵u是外部输入比如油门、转向。P是状态协方差矩阵描述我对当前状态估计有多不确定。Q是过程噪声协方差描述模型本身有多不可靠。H是观测矩阵把状态映射到观测空间。R是观测噪声协方差描述传感器读数有多不可靠。z是传感器实际读数。真正决定滤波性格的就是 Q 和 R。R 小说明传感器很可靠卡尔曼增益 K 会变大滤波器会更激进地追随测量值Q 小说明模型很可靠K 变小滤波器会更平滑、更信任预测。3.2 一维温度估计手把手感受 Q 和 R 的拉扯假设你用温度计测室温状态只有温度一个系统匀速不变A1没有控制输入B0。那么卡尔曼滤波退化成极易理解的形态import numpy as np # 初始化 x 24.0 # 初始温度估计 P 1.0 # 初始不确定性 Q 0.01 # 过程噪声模型非常稳定室温不会跳变 R 4.0 # 观测噪声温度计噪声比较大 measurements [23.9, 24.5, 25.1, 24.8, 25.3, 26.0, 26.4] for z in measurements: # 预测温度保持不变 x_pred x P_pred P Q # 更新计算卡尔曼增益 K P_pred / (P_pred R) # 修正估计 x x_pred K * (z - x_pred) P (1 - K) * P_pred print(fmeasurement{z:.2f}, K{K:.3f}, estimated{x:.3f}, P{P:.3f})跑一下就能看到一开始 K 很大因为 P 初始设得大滤波迅速靠近测量值几轮之后 P 收敛K 稳定在一个小值滤波变得平滑。这个过程特别像人做判断——一开始啥都不知道多听传感器的后来自信了就开始坚持自己的判断。3.3 多维扩展时最常见的三个坑从一维到多维公式还是一样的但工程实现上会踩不少坑矩阵维度对不上。A 是 n×nP 是 n×nH 是 m×nR 是 m×m。任何一个维度错误运行时直接抛矩阵乘法异常。建议先把各维度写在注释里。P 初始值别设太大也别设太小。设太大前期滤波会剧烈震荡设太小滤波器觉得自己很懂后面真实数据来了也改不过来收敛极其缓慢。一般按可接受的范围设比如位置不确定给 1~10速度不确定给 0.1~1。Q 和 R 不是拍脑袋的要能自圆其说。R 可以拿传感器静态数据算方差Q 更抽象可以用目标最大加速度来推算——比如车辆最大加速度 3m/s²采样周期 0.1s那么速度的过程噪声方差大约是(3 * 0.1)^2 0.09。这个推导过程比瞎调强得多。4. 传感器融合实战IMU GPS 的融合链路是怎么搭起来的滤波最大的价值在融合。拿最常见的 IMU GPS 做车辆定位举例这条链路踩过的坑比公式本身多得多。4.1 融合前的三个对齐一个都不能省时间对齐IMU 频率通常 100Hz~200HzGPS 只有 10Hz而且 GPS 输出还有延迟从接收到解算完毕可能差几十到几百毫秒。融合前必须先做时间戳同步否则你融合的是一对不同时刻的数据结果必然漂移。坐标系对齐IMU 给的是机体坐标系下的加速度和角速度GPS 给的是全局经纬度。做融合前必须转到同一个坐标系通常转到局部笛卡尔坐标系比如以起点为原点。采样频率对齐高频的 IMU 负责预测阶段低频的 GPS 负责更新阶段。如果 GPS 在两次更新之间来了就只做预测不更新。4.2 一个简化的位置速度融合模型车辆在平面上运动状态向量选位置 (px, py) 和速度 (vx, vy) 四个量x [px, py, vx, vy]^T状态转移矩阵 A 描述匀速运动模型import numpy as np dt 0.1 # IMU采样周期100Hz A np.array([ [1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1] ]) # 过程噪声主要来自加速度突变 sigma_a 2.0 # 加速度扰动标准差 m/s^2 Q np.array([ [0.25*dt**4, 0, 0.5*dt**3, 0], [0, 0.25*dt**4, 0, 0.5*dt**3], [0.5*dt**3, 0, dt**2, 0], [0, 0.5*dt**3, 0, dt**2] ]) * sigma_a**2 # 观测矩阵GPS只能观测位置观测不到速度 H np.array([ [1, 0, 0, 0], [0, 1, 0, 0] ]) # 观测噪声GPS定位的标准差静态时能测出来 sigma_gps 3.0 # 单位米 R np.array([ [sigma_gps**2, 0], [0, sigma_gps**2] ])这套模型跑起来滤波轨迹比纯 GPS 平滑得多而且能输出速度估计。最让人意外的是当 GPS 短暂失效时滤波器靠 IMU 预测还能维持一段时间的可用输出只是 P 会随时间发散——这个不确定性在涨的信息比输出本身还重要。4.3 松耦合还是紧耦合一个经常被问住的工程选择题搭融合架构时一定会面临这个抉择松耦合每个传感器先各自做定位比如 GPS 单独解算位置、IMU 单独推算姿态再把二者的输出喂给卡尔曼滤波融合。实现简单模块间解耦传感器坏了可以独立降级。这是中小型项目最稳妥的起步方案。紧耦合直接把原始观测比如 GPS 的伪距、多普勒频移和 IMU 原始数据一起塞进滤波器。精度上限高抗干扰能力强但实现复杂任何一个小环节出错都难以排查。我的建议是没有充分理由先做松耦合。你先把整条数据链路跑通、滤波器调稳再考虑精度瓶颈是否真的在耦合方式上。很多时候你以为是耦合不够紧导致的精度问题最后发现是时间戳没对齐。5. 跑测试时最容易翻车的滤波问题发散、延迟、野值滤波器看着公式简单一到真机测试就脾气尽显。下面这几个问题我几乎在每次滤波落地项目里都会遇到。5.1 滤波发散状态估计突然飞到天上去现象很好认滤波输出曲线突然炸开位置或角度变成了不可能出现的数值。排查链路一般是这样先看初值是否给得太离谱。P 初始设太大前期增益高少量野值就能把状态拉飞。再看 R 是否给得太小。R 太小意味着滤波器极端信任传感器读数一个异常的 GPS 跳变比如多径效应导致位置突跳 50 米直接就把状态拽走了。最后检查数值稳定性。P 矩阵随着迭代可能失去对称正定性浮点误差导致计算增益时求逆会出问题。工程上的应对手段是加一个极小值到对角线上或者定期对称化P (P P.T) / 2 P np.eye(P.shape[0]) * 1e-95.2 滤波延迟数据是平滑了但总是慢半拍滤波的本质是用历史信息做加权延迟某种意义上不可避免。如果发现下游控制明显感觉反应迟钝往往是 Q 设得太小滤波器过分相信模型结果真实变化到来时它反应半天。解决办法适当增大 Q让滤波器对新数据更敏感。检查传感器本身的延迟。GPS 解算延迟、图像处理管线延迟这些在滤波模型里应该作为时间戳补偿处理否则你等于拿着一堆过期数据喂卡尔曼。如果延迟来源于低通预滤波试试把预滤波的截止频率调高或者去掉预滤波把噪声交给卡尔曼处理——卡尔曼本身就是最优滤波器没必要在前面再叠一层。5.3 野值滤波永远救不了假数据这是我觉得最需要强调的一点。卡尔曼滤波假设噪声是高斯分布的小扰动但真实传感器经常给出完全离谱的点状异常超声波打到楔形面上反射出超远距离、GPS 在城市峡谷里瞬间跳变、激光雷达打到玻璃上穿透。这时候先别急着调滤波参数先加野值检测把异常数据挡在滤波外面。最常用的是马氏距离检验innovation z - H x_pred # 新息 S H P_pred H.T R # 新息协方差 mahalanobis innovation.T np.linalg.inv(S) innovation if mahalanobis chi2_threshold: # 比如卡方分布5%显著性水平 # 正常进入滤波更新 else: # 野值丢弃或只做预测这里卡方阈值查表就行二维观测取约 5.99三维取约 7.81。这个机制加进去滤波的鲁棒性提升立竿见影。5.4 调参的顺序比调参本身更重要很多人一上来就 Q、R 一起调调了几天也没头绪。我的建议是固定一个、调另一个并且按下面的顺序来先调 R。用传感器静止数据算方差这是有物理依据的不要拍脑袋。再调 Q 的量级。先设一个与 R 同量级的初始值然后观察滤波输出——如果太肉增大 Q如果太躁减小 Q。最后微调初始 P。P 只影响前几十帧的收敛过程不影响稳态不要花太多时间。每次只改一个参数记录输出曲线变化形成对比。这一步看起来笨实际是最高效的。另外提醒一句仿真环境和真机环境差别巨大。仿真里调好的 Q、R上台真机基本都要再调。因为仿真噪声是人为设置的而真机噪声往往是有色噪声、时变的、还带各种干扰。所以别在仿真里追求完美曲线差不多就行留出时间在现场调。最后分享一点心得滤波这个东西做久了你会发现它并不神秘——它本质上是在表达我有多信模型、多信传感器然后按最优方式做加权。真正让滤波发挥威力的不是公式背得多熟而是对传感器噪声、系统模型、时间戳这些脏活的敬畏。还有一个容易被忽略的点滤波输出要永远带方差一起用。下游模块只有拿到了不确定性才能真正理解这个数据值不值得信任。下次做滤波的时候建议你从记录方差曲线开始你会看到比状态曲线本身更多的信息。