PMW3901这颗光流模块我前前后后调了快一个月。第一次上电看到串口里飞出来的原始速度数据时我一度以为传感器坏了——明明放在桌面上纹丝不动x轴数据却在±15之间疯狂跳动偶尔还蹦出几个±80的尖峰。把这样的数据直接丢给飞控的位置环后果可想而知电机嗡嗡响、机身原地颤抖、定点变成了“抖点”。后来才明白PMW3901输出的光流速度本质上是一帧一帧图像像素位移的差分结果天生就带着噪声。直接使用原始数据做闭环控制就是让控制器去追一个“抽风”的信号。下面这套处理思路我实测下来比较稳限幅加一阶低通EMA再加一点滑动平均的思路给同样被光流速度噪声折磨的朋友直接抄作业。我会把参数怎么定、代码怎么写、坑在哪里都讲清楚适合正在做无人机定高定点、机器人循迹避障的嵌入式开发者参考。1. 为什么PMW3901的原始数据会“抽风”拿到数据先观察1.1 PMW3901到底输出的是什么PMW3901是一款基于图像匹配的光流传感器内部有个小摄像头以固定帧率拍摄地面图像然后通过算法比较前后两帧图像中特征点的位移最终输出x、y两个方向的像素位移量。我这边通过SPI读取数据更新频率约50Hz也就是每20ms能拿到一组新的位移数据。理解一个点很重要PMW3901输出的并不是“直接可用的速度”而是“两帧图像之间的像素位移差”。要得到真正的速度还需要结合安装高度、镜头视场角、帧率做换算。但滤波这件事和单位没关系原始位移差和换算后的速度噪声特征是相同的所以调滤波时直接用raw值处理最后再乘换算系数就行。1.2 原始数据的“病”到底长什么样我在调的时候把原始数据通过串口打出来用串口绘图软件画成波形问题一目了然。第一个问题是静态跳动。传感器放在桌面不动输出却不是零而是在±15附近随机波动光照差一点的环境能到±30。这个跳动属于传感器算法本身的误差因为图像传感器有噪声特征点匹配结果会存在亚像素级的抖动这些抖动经过差分放大后就表现成了速度跳动。第二个问题是偶发尖峰。序列里偶尔会冒出±60甚至±80的突变值这些尖峰往往只有一帧但危害极大。PID控制器一旦看到这种尖峰微分项会瞬间输出一个很大的控制量机身就会猛抖一下。尖峰来源比较复杂可能是图像匹配失败、地面纹理突然变化也可能是读取时序不对导致的脏数据。第三个问题是相邻帧的跳变幅度不均匀。即便没有任何尖峰原始数据的曲线也是“锯齿状”的相邻两帧的差值忽大忽小。如果把这个信号直接接到速度环相当于给系统注入了一堆高频噪声飞控来回修正、越修越乱表现就是机身高频抖动和电机啸叫。所以我一直跟身边人说拿到PMW3901的第一天不要写任何控制算法先把原始数据录下来看波形。这一步能帮你省下后面至少三天的排查时间。滤波之前先搞清楚噪声的特征是静态随机噪声为主还是尖峰为主还是在运动时有周期性波动这直接决定了你该选哪种滤波方案。2. 几种常见滤波算法怎么选别一上来就上卡尔曼很多新手一提到滤波就想到卡尔曼滤波但PMW3901这种场景其实没必要。数据是单维度、慢变化、噪声有明确特征用几个简单算法组合就能处理得很好而且代码量小、CPU占用低、参数直观好调。我自己试过好几种方案下面挨个说下使用感受。2.1 滑动平均滤波思路最直白但要注意延迟滑动平均就是把最近N个采样值丢进一个环形缓冲区每次输出这N个值的平均值。它的优点是实现极其简单对周期性噪声和高频抖动有很好的抑制效果缺点是会带来固定的相位延迟N越大越平滑、延迟越大。打个比方你走在路上想判断自己的平均步速看最后一步肯定不准因为抬脚和落地的瞬间速度差异很大但如果你统计最近10步的平均速度结果就稳得多。不过代价是你感受到的“当前速度”实际上滞后了5步。滑动平均就是这个思路。在实际使用中N取5到10比较常见。N5时延迟约40msN10时延迟约90ms。如果后续还有PID速度环N尽量别超过8否则相位滞后会让系统变得迟钝甚至引发震荡。2.2 一阶低通滤波EMA代码最少参数最直观EMA的公式很简单y y_prev α × (x - y_prev)其中α是一个0到1之间的系数。它本质上是给历史数据做指数加权平均离当前越近的数据权重越大离得越远权重越小。α越大新数据占的权重越大输出响应越快但滤波效果越差α越小输出越平滑但滞后越严重。EMA比滑动平均的代码量还少只需要一个状态变量不需要缓冲区。我特别推荐先用它做第一版因为调参非常直观觉得噪声大就调小α觉得反应慢就调大α来回试几次就能找到合适的点。它唯一的弱点是抵抗尖峰的能力不足——一个很大的尖峰虽然会被削弱但依然会明显影响输出值所以最好在前面加一道限幅保护。2.3 中值滤波和限幅滤波对付尖峰的两把刀中值滤波是在滑窗内排序后取中间值对付离群尖峰效果极好但排序需要额外的计算开销在低端MCU上要谨慎使用。限幅滤波则简单粗暴如果当前值和上一帧值的差超过了设定阈值就认为这是无效数据直接丢弃或沿用上一帧的值。我在PMW3901上强烈建议用“增量限幅”就是限制相邻两帧之间的变化量而不是限制速度的绝对值。为什么因为速度信号本身是非平稳的飞行器快速机动时真实速度可能很大如果限制绝对值会直接把真实的大速度信号给截断。而噪声的主要特征恰恰是相邻帧之间出现不合理的巨大跳变所以限制增量更符合物理直觉。2.4 我的最终组合增量限幅 一阶低通EMA经过几轮实测对比我最终采用了“增量限幅 EMA”两阶段方案。第一道限幅负责把偶发尖峰和异常跳变挡在门外第二道EMA负责把剩余的随机噪声抹平。中间加不加滑动平均看需求和算力我后来为了更顺滑又在EMA之后加了一个N3的滑动平均实测效果更好一点但也能明显感觉到延迟变大了一点点。滤波方案尖峰抑制平滑效果计算量延迟表现代码复杂度滑动平均一般较好低固定、随N增大低EMA一阶低通弱较好极低指数型、可调极低中值滤波很强一般中固定、随N增大中增量限幅很强无极低几乎无极低限幅 EMA很强好低较低、可调低这个组合的另一个好处是便于调试。限幅阈值负责“剔除明显不合理的数据”EMA系数负责“决定平滑和响应的平衡点”两者互不干扰调起来思路非常清晰。3. 滤波参数手把手整定限幅阈值、EMA系数、窗口大小到底怎么定3.1 先统一数据读取节拍保证每次都是“新数据”这一步看似和滤波无关但实际上是滤波有效的前提。我第一次踩坑就是因为SPI读取频率不稳定有时候20ms读一次有时候40ms读一次时间间隔不一致直接导致相邻两帧的位移差被放大滤波效果差得离谱。正确的做法是固定读取节拍。我在代码里用20ms的定时器中断去触发数据读取读取前先检查PMW3901的DataReady状态位确保寄存器里的数据是当前这一帧的新数据而不是上一帧的旧数据。这一步非常重要否则你滤的是“时快时慢的脏数据”参数怎么调都白搭。3.2 增量限幅阈值怎么定用统计数据说话增量限幅阈值的设定我不建议拍脑袋。我的做法是先不滤波让传感器在目标场景中静止和正常运动各录500到1000组数据然后计算相邻两帧差值的分布。具体来说把所有相邻帧的差值统计出来算标准差σ阈值取5倍到6倍σ左右。比如我这边静止时相邻帧差值标准差约3正常推动时约12到15峰值偶尔到40那么我取60作为阈值。阈值设得太小会把真实运动截断设得太大则起不到剔除尖峰的作用。还有一个经验如果场景固定、光照稳定阈值一旦定下来基本不用再动。但如果你的使用环境差异很大室内室外切换可以把阈值做成参数在飞控里预留调试接口起飞前根据环境自动选择或手动调整。3.3 EMA系数α怎么算先算时间常数再转成系数EMA系数的选择可以从时间常数τ理解。τ表示滤波输出跟上输入变化的大致时间尺度τ越小响应越快τ越大越平滑。换算公式是α 1 - exp(-dt/τ)工程上也可以近似成α ≈ dt / (τ dt)其中dt是采样周期。以我这边dt20ms为例。如果希望系统对速度变化的响应时间常数约100ms那么α 0.02 / (0.1 0.02) ≈ 0.167这个值偏保守。如果希望响应更快一点τ取50msα ≈ 0.286。我实际在室内慢速飞行场景下试了一圈下来发现α取0.3到0.4最舒服噪声压得住、动作也跟手。α取0.3和0.4的差别体现在波形上就是“更平滑”和“更及时”之间的取舍。大于0.7基本等于没滤小于0.1输出就会明显发肉。如果今天不想算公式就直接从0.5开始试观察静态晃动幅度再观察动态跟随性来回调两次就能找到合适的区间。3.4 滑动平均窗口大小怎么选延迟换平滑要有上限如果需要在EMA之后再加一层滑动平均窗口N的取值要结合延迟来定。滑动平均带来的延迟约等于(N - 1) / 2 × dtdt20ms时N5延迟40msN10延迟90ms。这个延迟需要和你的控制系统带宽做权衡。我的经验是如果后面接的是PID速度环N不要超过5如果只是做数据显示、导航路径规划N可以取8到10。还有个小技巧可以做一个“去极值滑动平均”在窗口内先去掉最大值和最小值再对剩下的值求平均。这样滑动平均本身也具备一定的尖峰抑制能力和前面的增量限幅叠加起来效果很强。3.5 完整示例代码一个可以抄的C语言实现下面是我实际用的滤波代码做了精简核心逻辑都在。结构体里包含了滤波器的全部状态方便维护和扩展。typedef struct { float ema_value; // 上一帧的EMA输出 float alpha; // EMA系数比如0.35 int16_t last_raw; // 上一帧的限幅输出 int16_t max_step; // 增量限幅阈值比如60 } flow_filter_t; int16_t flow_filter_update(flow_filter_t *f, int16_t raw) { // 第一步增量限幅 int16_t delta raw - f-last_raw; if (delta f-max_step) { delta f-max_step; } else if (delta -f-max_step) { delta -f-max_step; } int16_t limited f-last_raw delta; f-last_raw limited; // 第二步EMA一阶低通 f-ema_value f-ema_value f-alpha * ((float)limited - f-ema_value); // 返回滤波后的值保留两位小数的四舍五入 return (int16_t)(f-ema_value 0.5f); } void flow_filter_reset(flow_filter_t *f, int16_t first_raw) { f-last_raw first_raw; f-ema_value (float)first_raw; }注意几个细节。第一first_raw初始化的时候EMA输出和last_raw都要赋值成同一个值否则启动瞬间会有一次从0跳变到真实值的“阶跃”。第二增量限幅用的是“相对上一帧”的增量不是绝对值的上下限这样在高速运动时不会截断真实信号。第三调用时机很关键必须每次读到新数据后立刻调用并且保证读取间隔均匀否则滤波效果会打折。3.6 关于最终速度换算的建议滤波后的raw值可以直接作为速度环的输入吗可以但不同单位会影响PID参数整定。我建议把滤波后的raw值乘一个换算系数转成标准单位m/s或cm/s再接进控制环。公式大致是speed raw × scale / height其中scale是传感器的像素到物理距离的换算系数height是传感器离地面的高度单位要和scale一致。这个换算在滤波之后做因为滤波过程对raw和换算后的速度是等价的提前换或推后换不影响滤波效果。但推后换算有个好处所有滤波参数的调试都和单位无关代码在不同机型之间移植时只需要改最后的换算系数即可。4. 实测效果滤前滤后的数据长什么样4.1 静态测试桌面放置传感器不动我把PMW3901固定在小车上放在桌面静止记录500帧数据。原始数据均值约2.3标准差17.6最大尖峰跑到82。这个状态别说做控制光看数据都觉得离谱。加了增量限幅阈值60和EMAα0.35之后同一段数据均值约1.5标准差降到3.2最大幅度不超过±8。如果再加一个N3的滑动平均标准差能进一步降到2.5左右但响应会慢半拍。实际数据如下指标原始数据限幅EMAα0.35限幅EMAN3滑动平均标准差17.63.22.5最大绝对值8286肉眼观感锯齿尖峰仍有轻微毛刺平滑干净4.2 动态测试手动快速移动传感器静止数据好看还不够关键看动态。我用手拿着小车以大约0.3到0.5m/s的速度左右移动观察滤波输出能否跟上真实速度变化。EMA系数α0.35时滤波输出相比原始数据大约滞后2到4帧也就是40到80ms。人眼能从波形上看出略有滞后但飞控速度环完全感知不到这种程度的延迟。滑动平均N3时额外增加约20ms依然在可接受范围内。N10时延迟明显变大波形看起来很“肉”快速往返时滤波输出几乎变成了正弦波的缩小版相位滞后严重。这里有个关键体会滤波的滞后并不全是坏事。速度环本身是积分关系的反馈回路适当平滑输入能减少高频激励PID输出更稳定。但滞后过大会造成相位裕度下降严重时飞控会“越想稳越震荡”。所以参数整定的目标不是“波形最好看”而是“整机闭环最稳定”。4.3 整机表现从“筛糠”到定点换了滤波之后我重新做了定点测试。原地悬停时机身的位移偏差从原来的±30cm降到了±5cm左右电机声音从尖锐的“啸叫”变成平稳的低鸣。人为推一下飞机它能在1秒左右恢复稳定而之前至少要3到4秒期间还会来回摆两下。还有一个意外收获滤波之后因为位置环拿到的速度反馈更干净了我把速度环PID的P值上调了将近30%都没有引发震荡整体响应比滤波之前反而更快了。这说明合理滤波不是“牺牲响应换稳定”而是“去掉脏数据让控制器敢用更大的增益”。5. 坑我都替你踩过了滤波之后还是抖问题出在哪5.1 滤波后依旧剧烈跳动的排查顺序如果你把滤波代码写好了数据还是抖别急着怀疑滤波参数先按下面顺序排查。第一个是数据读取节拍。我前面强调过读取间隔不均匀会让相邻帧差分被缩放噪声特征完全改变。用逻辑分析仪抓一下片选信号和中断时间戳确认读取周期稳定。第二个是数据有效性。PMW3901有数据更新标志位读取之前一定要检查。第三个是SPI配置。时钟极性和相位配错可能读到半新半旧的数据表现就是隔三差五出现一个巨大尖峰。限幅能挡掉一部分但治标不治本正确配置之后尖峰频率会明显下降。最后才是滤波参数问题。如果前三步都没问题再回去看限幅阈值是不是太小EMA系数是不是太大。5.2 响应太“肉”先怀疑限幅再怀疑α表现是手动快速晃动传感器滤波输出跟不上幅度明显小于真实值。这种情况九成是限幅阈值设小了真实的大增量被当成异常值截断。检测方法很简单把限幅阈值临时改成一个很大的值比如1000然后晃动传感器看波形幅度是否恢复。如果恢复了说明阈值设太小调大一些如果波形还是偏小那才是EMA系数太小或滑动平均窗口太大需要增大α或减小N。记住一个原则限幅负责“砍尖峰”EMA负责“抹毛刺”两个功能千万别混为一谈。5.3 静止时输出不为零还在缓慢漂移这个问题我一开始也以为是滤波没滤干净查了半天发现锅不在滤波器。PMW3901在低纹理环境纯色地板、白墙下图像匹配质量会严重下降输出出现零偏甚至随机漫游。解决办法有两个方向。一是改善环境条件室内尽量选择纹理丰富的表面实在不行可以贴一些随机图案的地贴。二是在代码里加零偏校准上电后让设备静止2到3秒采集这段时间的滤波输出做平均作为零偏在正式运行时减掉。这个方法对静态漂移效果很明显但对随机漫游无能为力。5.4 光流定点的“慢速爬行”现象用过光流定点的朋友应该熟悉这个现象飞机看似稳住了但会以极低的速度缓慢漂移过几分钟就飘出去很远。这其实是滤波和位置环的共同作用滤波后的速度对低频小偏差不敏感位置环又不够敏感导致系统“看不见”低速漂移。我的解决方案是“分路处理”速度环输入用滤波后的速度但位置环的位移修正来自原始速度积分前的一路高通分量。简单说就是把原始速度分成低频和高频两部分用低频做位置积分用高频做稳定性阻尼两者对PID的意义不同。这个方法稍微进阶一些但在做高质量定点时值得一试。5.5 最后的经验参数一定要留调试接口我吃过最大的亏是每调一次参数就重新烧一次固件效率极低。后来我把限幅阈值、EMA系数、滑动平均窗口全部做成可配置项通过串口命令行实时修改改完立刻生效。这样一来整定参数变成了一件“边看波形边拧旋钮”的事情十几分钟就能调完一组满意参数比烧固件快了一个数量级。PMW3901的滤波本身并不复杂难的是搞明白噪声长什么样、需要滤到什么程度、引入多少延迟是可接受的。先把原始数据录下来用串口绘图软件看几遍波形再动手写代码远远比自己闭门调参快得多。这套“限幅 EMA”的方案我后续又移植到好几个项目上换传感器、换主控核心逻辑一直没变稳定性也一直在线。