1. 为什么RM电控要学卡尔曼滤波1.1 从一场实战惨案说起先说一个我亲眼在RM赛场看过的场景某支队伍的自瞄系统在实验室里打得贼准步兵绕场三圈云台纹丝不动弹着点跟用尺子量过似的。结果一到正式比赛换上赛场灯光目标装甲板在视野里抖成了筛子云台跟着目标疯狂左右摇摆子弹全打在装甲板边缘弹飞。下场之后队员围在一起查了一个通宵发现问题的根源根本不是视觉算法崩了而是视觉给出来的目标位置数据包含大量噪声电控侧拿到这些带毛刺的坐标之后直接喂给了PID云台被噪声带着走自然没办法稳定输出。这就是典型的“差一个滤波器”的现场。很多RM队伍把大量精力花在机械结构、视觉识别、电控PID上却忽略了数据进入控制器之前的预处理环节。实际上在RoboMaster赛场上视觉识别出的目标中心坐标、裁判系统给出的装甲板血量、测速模块返回的弹速、陀螺仪积分出来的姿态角没有任何一个数据是干净无噪声的。而卡尔曼滤波恰恰就是解决这一类问题最成熟、最系统的工具。1.2 卡尔曼滤波在RM赛场的三大用途认真算一笔账RM电控里面卡尔曼滤波能落地的场景非常多但最典型的不外乎三个方向。第一个方向是目标预测。自瞄系统识别出敌方装甲板的位置之后由于视觉处理帧率有限、通信存在延迟云台实际执行瞄准时目标早就移动了。这时候卡尔曼滤波可以对目标的运动状态做建模预测出几十毫秒后目标的位置把云台提前甩过去提高命中率。第二个方向是数据平滑。裁判系统返回的模块血量、弹仓剩余子弹数、底盘功率等数据在赛场上因为通信干扰会剧烈跳动直接拿来控制很容易导致误判。卡尔曼滤波可以结合物理模型和观测数据给出一个相对平滑且可靠的状态估计。第三个方向是多传感器融合。步兵车的底盘里程计和IMU陀螺仪各有所长里程计短时间准确但会打滑陀螺仪长时间稳定但有漂移。卡尔曼滤波可以把两者的优势结合起来输出一个比任何单一传感器都靠谱的位姿估计。这三个方向背后都是同一个数学工具。而这个工具的地基就是概率统计。2. 概率统计基础状态估计的“世界观”2.1 随机变量与噪声模型测量为什么总是“不准”要理解卡尔曼滤波第一件事是转变观念不要把一个测量值当成一个确定的数而要把它当成一个随机变量。举个例子你用测速轮给步兵车测速测出来2.65 m/s这个数并不是真实速度的完美映射。轮子和地面之间可能有微小滑动编码器分辨率有限线材接触电阻会波动单片机ADC采样有量化误差等等。所有这些因素叠加在一起导致测量结果在真实值附近上下浮动。统计学家把这种浮动叫做“噪声”。如果用数学语言描述测量值z和真实值x之间的关系可以写成z x v其中v就是噪声通常建模为一个均值为0的高斯随机变量。均值为0意味着噪声的平均效果不偏不倚不会系统性地把结果往一个方向带高斯分布则意味着大部分噪声都集中在真实值附近偏离太远的概率很小。这就是卡尔曼滤波世界观的第一课**永远不要精确描述一个测量值用均值和方差来描述它。**均值告诉你这个数大概在什么位置方差告诉你这个数有多可信。一个方差很大的测量即使均值看起来合理也不能过分相信它。2.2 高斯分布与协方差从单个量到多个量单变量的高斯分布大家都很熟悉概率密度函数长这样f(x) (1 / sqrt(2π σ²)) · exp(-(x - μ)² / (2σ²))这里面μ是均值σ²是方差。方差越小曲线越瘦高说明数据越集中测量越可靠方差越大曲线越矮胖说明数据越分散测量越不靠谱。但RM里面几乎没有单变量的估计问题。云台瞄准要同时估计敌方的x、y坐标和运动速度底盘状态估计要同时关心x坐标、y坐标、朝向角、速度、角速度。这时候就需要多变量高斯分布登场。多维高斯分布不再用一个数μ来描述中心位置而是用一个向量μ来描述均值用一个矩阵Σ来描述各个变量之间的协方差关系。这个矩阵叫做协方差矩阵Σ [[σ₁², σ₁₂], [σ₂₁, σ₂²]]对角线上的元素是每个变量的方差非对角线上的元素是变量之间的协方差。协方差描述的是两个变量的相关性如果敌方装甲板x坐标增大时y坐标倾向于同步增大协方差就是正的如果x增大时y倾向于减小协方差就是负的如果两者独立变化协方差就是零。协方差矩阵在卡尔曼滤波里无处不在。预测步误差变大体现在协方差矩阵变大观测更新之后状态更确定体现在协方差矩阵缩小。可以说卡尔曼滤波的整个迭代过程就是均值向量和协方差矩阵的交替更新。2.3 条件概率与贝叶斯视角观测到数据之后该信谁卡尔曼滤波还有一个绕不开的概率论概念条件概率和贝叶斯定理。条件概率描述的是“在已知某个条件的情况下事件发生的概率”记作P(A|B)读作“在B发生的条件下A发生的概率”。贝叶斯定理则把这个关系反过来P(A|B) P(B|A) · P(A) / P(B)把它翻译到状态估计的语言里P(状态 | 观测) P(观测 | 状态) · P(状态) / P(观测)左边是“看到观测数据之后状态取某个值的概率”叫做后验概率右边的P(状态)是观测之前对状态分布的猜测叫做先验概率P(观测|状态)是给定状态后产生这个观测的概率叫做似然。不要被这些术语吓到用生活类比一下就通了。假设你在宿舍里等队友送电池过来平时他走那条路大概需要3分钟这是一开始设定的“先验”。突然听到楼道里传来急促的脚步声这就是“观测”。听到脚步声之后你对“他马上就要到”这件事的把握就增大了这就是“后验”。如果脚步声越来越近你的把握就越来越大如果脚步声又远了你的把握就会回落。卡尔曼滤波的本质就是在每一帧里重复做贝叶斯更新用运动模型做预测得到一个先验估计用传感器观测计算似然再把两者按可信度加权融合得到当前帧的后验估计。权重怎么分配这就引出了著名的卡尔曼增益。3. 从贝叶斯到卡尔曼滤波的数学之路3.1 状态空间模型把问题“形式化”在动手写卡尔曼滤波代码之前先把数学模型搭起来。卡尔曼滤波假设系统可以用一个状态空间模型来描述包含两个核心方程。第一个是状态方程描述状态如何随时间演化x_k A · x_{k-1} B · u_k w_k其中x_k是k时刻的状态向量A是状态转移矩阵描述上一时刻的状态如何影响当前时刻u_k是控制输入B是控制矩阵w_k是过程噪声建模为高斯分布N(0, Q)Q是过程噪声协方差矩阵。第二个是观测方程描述传感器如何观测状态z_k H · x_k v_k其中z_k是测量值向量H是观测矩阵把状态空间映射到测量空间v_k是测量噪声建模为高斯分布N(0, R)R是测量噪声协方差矩阵。拿云台瞄准的例子说如果要估计敌方装甲板的平面位置(x, y)和运动速度(vx, vy)状态向量就是x [x, y, vx, vy]^T。如果假设目标做匀速直线运动状态转移矩阵A就是A [[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]视觉识别给出的装甲板像素中心坐标映射到云台坐标系之后就是观测值z [x_obs, y_obs]^T观测矩阵H就是把状态向量中的位置分量提取出来H [[1, 0, 0, 0], [0, 1, 0, 0]]形式化之后问题就变得很清晰了每收到一帧新的观测数据就利用状态方程预测一下目标现在大概在哪然后用观测值修正一下这个预测把两者按可信度加权。剩下的问题就是“权重怎么算”这就要引出卡尔曼滤波的五大核心公式。3.2 预测步先验是怎么来的卡尔曼滤波的每一次迭代分为预测步和更新步。预测步做的事情是根据上一帧的后验估计和运动模型推算当前帧状态的先验估计。公式有两个x_pred A · x_est B · uP_pred A · P_est · A^T Q第一个公式很好理解就是把上一帧的最优估计带入运动模型外推一步。第二个公式稍微复杂一点它描述的是状态不确定性的传递上一帧的估计有一个协方差矩阵P_est经过线性变换A之后不确定性变成A · P_est · A^T然后还要加上过程噪声Q因为运动模型本身不是完美准确的目标可能会加速、转弯、被其他车挡住这些模型没有刻画的因素都会增加不确定性。用我最开始提到的宿舍等队友的例子来类比你预测他1分钟后到这个预测有一个方差。如果预测的时间越远这个方差就应该越大因为过程中可能发生的意外变数越多。Q就是用来描述这个“过程中变数”的量。Q越大说明你越不信任运动模型预测的协方差矩阵会膨胀得越快。3.3 更新步后验是怎么融合的更新步是卡尔曼滤波的精髓它的任务是把预测值和观测值融合起来。公式有三个K P_pred · H^T · (H · P_pred · H^T R)^(-1)x_est x_pred K · (z - H · x_pred)P_est (I - K · H) · P_pred其中K就是卡尔曼增益。没有K的时候你手里有两条信息预测出来的状态x_pred带协方差P_pred和传感器读到的观测z带噪声R。如果观测噪声很小、预测不确定性很大就应该多信观测反过来如果观测噪声很大、预测已经很准就应该多信预测。卡尔曼增益K就是那个根据两个不确定性的相对大小自动调节信任比例的“调音台”。可以再看一个简洁的解释角度两个高斯分布相乘结果仍然是一个高斯分布。预测分布乘以观测似然分布得到的新的高斯分布的均值正好就等于x_est而且它的协方差P_est比任何参与相乘的分布都要小。更严谨地讲高斯相乘之后的结果均值落在两个峰之间偏向方差更小那个峰。卡尔曼增益K的数值大小决定了融合后的状态在预测和观测之间偏向谁。拆开来理解这几个更新公式z - H · x_pred是“观测残差”意思是我预测目标应该在某个位置但传感器说在另一个位置差了多少。乘以K之后得到一个修正量加到预测值上得到融合后的估计。x_est x_pred K · (z - H · x_pred)这个结构在控制领域非常经典它本质上是一个带增益的误差修正回路。卡尔曼滤波不是简单地取平均而是一个自适应的反馈校正过程。每一帧迭代都在重复“预测-校正”的循环状态向量和协方差矩阵像滚雪球一样持续更新。4. 卡尔曼滤波的RM实战场景4.1 云台瞄准目标检测数据抖得像心电图怎么办回到文章开头那个翻车场景。自瞄系统给出的目标中心坐标在皮卡丘模式下飘得厉害每帧之间可能跳好几个像素。直接把这些坐标发给云台俯仰/偏航电机PID就会跟着噪声来回抽动云台不仅瞄不准还容易发热过载。用卡尔曼滤波解决这个问题的思路是这样的把目标的平面速度当成隐含状态通过连续多帧坐标数据估计出目标的位置和运动趋势。这样做的好处是即使某两帧的坐标因为图像噪声偏离真实位置滤波器也会认为这个偏离不符合目标的运动趋势给它一个较低的权重从而把毛刺平滑掉。具体实施时可以先在视觉和电控之间增加一个数据通道通过串口或CAN总线把目标坐标发送给电控侧在电控侧一般是STM32上实现一个四维状态量的卡尔曼滤波器。目标位置用视觉数据目标速度则完全靠滤波器自己估计。这样云台收到的坐标就是经过平滑处理的不再跳变。如果嫌STM32上手动实现容易出bug还可以先用MATLAB或Python把算法仿真调好再移植到单片机。我个人的经验是在单片机上做纯手写矩阵运算版本存4x4浮点矩阵跑一次完整的预测加更新在STM32F4上大约只需要几十微秒完全不影响控制周期。4.2 弹道预测与击打时机RM赛场上弹速有限子弹飞行需要时间。假设子弹以15 m/s的速度飞行目标在3米开外横向运动飞完这3米大约需要0.2秒。如果目标以2 m/s的速度横向移动0.2秒内就移动了0.4米这足以让弹着点错开装甲板。卡尔曼滤波在这里的角色是根据历史帧的目标位置估计出目标当前的运动速度从而外推目标在0.2秒之后的位置。外推的位置才是云台实际需要瞄准的点。实现的时候要额外注意通信延迟。视觉识别到目标到电控真正发出发射指令中间经过串口通信、解析、PID响应等环节总共可能延迟几十毫秒。这个延迟在高速运动的目标面前不可忽略需要在卡尔曼滤波预测目标位置时额外把通信延迟补偿进去。补偿方法不复杂在每次预测时把预测的时间步长从当前帧间隔改成当前帧间隔加上总延迟时间。也就是把dt改成dt latency。这样预测出来的是目标“子弹到达时刻”的位置而不是“当前时刻”的位置瞄准精度能提升一个量级。4.3 测速轮与陀螺仪数据融合底盘里程计和IMU陀螺仪都是RM电控里常用的定位传感器但各有各的毛病。测速轮编码器里程计短期精度很高但轮子打滑、轮胎磨损会引入累积误差跑久了位置会漂陀螺仪的角速度测量短期会漂但经过积分得到的角度在长时间内相对稳定。卡尔曼滤波很适合把这两者融合成一套完整位姿估计。状态向量取[tx, ty, yaw, vx, vy, omega]观测用里程计给的[tx, ty, yaw]和陀螺仪给的omega。里程计在yaw上往往不太准因为轮式车辆转小弯的时候轮子打滑会破坏轮速和实际转向角的关系而陀螺仪的yaw积分相对稳定正好可以弥补这个短板。融合后的位姿比任何单一传感器都稳后续的导航、瞄准预测、功率控制都可以在这个基础上进行。我见过不少队伍用这个方案做步兵车的低成本航迹推算在比赛场地这种规则平面上跑完整场误差非常可观。5. 调参经验与常见问题5.1 噪声矩阵R和Q到底怎么定这是所有第一次接触卡尔曼滤波的人必问的问题。网上能搜到一堆“根据经验”、“试出来的”这种答案实际上是有规律可循的。R代表测量噪声协方差矩阵它描述的是传感器测量值中含有的噪声水平。最简单的标定方法把传感器固定住采集一堆静态测量数据计算这些数据的方差那就是R的一个比较合理的估计。比如用视觉测距模块测量一个固定距离的目标采集200个数据点求方差填进R即可。Q代表过程噪声协方差矩阵它描述的是运动模型与实际运动之间的偏差。这个一般更难直接用实验标定因为它反映的是“模型没刻画的东西”比如目标的突然加速、转弯、被撞击。通常的做法是先设一个较小的初始值让滤波器的输出与真值对比不断调整Q直到输出既平滑又不明显滞后。调参的时候有个铁律要记住R越大说明传感器越不可靠滤波器越倾向相信预测Q越大说明模型越不可靠滤波器越倾向相信观测。很多人调参调不明白就是把R和Q的方向搞反了。还有个常用技巧是用试凑法的时候先固定R只用Q去调平滑程度这样调试空间降为了一维会快很多。5.2 发散、滞后与初始化问题卡尔曼滤波在实际工程中会遇到的三大典型问题。第一个是发散。表现为状态估计逐渐偏离真实值甚至飞上天。常见原因是Q设得太小模型过度自信导致滤波器无法用观测数据修正累积的模型误差或者R设得太小过度相信单帧观测被某个异常观测带偏。排查思路是先检查协方差矩阵P的数值是否异常增长再看观测残差序列的均值是否偏离零。第二个是滞后。很多队伍滤波之后发现云台动作“慢半拍”这是因为调参时为了平滑噪声把Q调得太小滤波器对目标真实运动趋势的响应被压得太慢。滞后和噪声是一对矛盾压噪声必然会增加滞后唯一出路是建更好的运动模型。比如匀速模型滞后了就上匀加速模型匀加速模型还滞后就加一个角速度估计状态。第三个是初始化。P_est的初始值不要设成零矩阵否则滤波器一开始就认为自己对状态的估计完全确定后续预测和更新基本不会修正初始值导致前几百毫秒的估计非常拉胯。通常的做法是把初始P设成一个较大的对角阵比如对应位置的方差设成100表示初始状态极度不确定让滤波器在开头快速收敛。5.3 新手必看的几个坑第一个坑在回调函数里做矩阵求逆。H·P·H^T R这一步需要求逆如果这个矩阵的行列式很小数值上可能会不稳定。实际工程中这个矩阵一般是正的对称矩阵用LDUT分解或者直接调用ARM的CMSIS-DSP库做矩阵运算不要自己写高斯消元法。第二个坑数据类型选择。STM32上做矩阵运算浮点精度尽量用float双精度会吃掉大量CPU时间在控制周期紧张的嵌入式环境里不值得。如果对精度有更高要求可以考虑在关键节点用双精度做中间量最后再转回float。第三个坑采样时间不固定。RM赛场上视觉帧率可能会在30到60帧之间波动如果滤波器的dt一直写死预测步的精度会大打折扣。正确做法是每次预测前用定时器或者系统时钟读取实际的时间差作为dt这样即使帧率波动滤波器也能维持稳定。第四个坑频繁的矩阵运算会让代码变得很长很乱阅读性差debug困难。我建议封装成几个独立的函数至少包括predict()和update()状态量、协方差矩阵全部用结构体管理方便二次修改。6. 从概率视角看状态估计这件事在RM电控这个领域待得越久越能感受到卡尔曼滤波不是一个孤立的算法而是一整套看待数据和不确定性的思维范式。以前的我很喜欢把传感器返回的每个数值都当作“真值”来用直到一次调试中发现视觉给出的目标坐标在短时间内剧烈抖动当时第一反应是去查视觉算法后来才意识到任何传感器都存在噪声真正需要做的是在电控侧对数据做合理建模和滤波。从那以后我再也不纠结于单个数据点的准确性而是把每个测量值都看作“均值加噪声”把系统的状态看作一个概率分布。这种思维转变比学会卡尔曼滤波本身的公式重要得多。如果你正在准备RM电控或者已经在队里负责云台、底盘的控制代码建议先认真啃一遍概率统计的基础概念再把卡尔曼滤波的五个公式亲手推到一遍。推公式的过程可能很痛苦但推完之后再去调参、改模型思路会清晰很多。还有一个小技巧动手写代码前先在Python或者MATLAB里用仿真数据跑一遍算法把每个中间变量打出来看一眼理解每一步在做什么再移植到单片机上调试效率会翻倍。