最近整理RM电控组的代码库时又把卡尔曼滤波翻出来看了好几遍。不少新队员第一次跑测速模块看到示波器上无规律乱跳的读数第一反应就是写个一阶低通滤波压一压。这个思路本身没有错可一旦到了比赛场上要算弹道补偿、要卡射速节奏的时候一个响应慢半拍的滤波器很容易让全队陷入“越调越不稳”的泥潭。带队伍这几年我一个很深的感受是电控要学的东西很多但卡尔曼滤波绝对是最值得花时间彻底搞懂的那一块。它不是玄学也不是代码库里的黑盒本质是一套用数学来回答“什么时候该信传感器、什么时候该信模型”的框架。这篇内容主要面向RM电控组里准备接手云台、测速模块或者裁判系统数据的队员也会适合那些已经在用低通滤波但总觉得响应和噪声顾此失彼的朋友。我会按照自己实际跑过的流程来写先从RM里几个典型场景讲清楚卡尔曼滤波到底在解决什么问题再给出STM32上的一维和二维C代码实现接着讲Q/R参数怎么调、现场有哪些坑最后简单聊一聊如何把同一套思路扩展到云台姿态融合上。1. 思路拆解卡尔曼滤波在RM机器人的切入场景1.1 三个典型场景测速、云台、底盘电控里的传感器数据大部分能简化成两类问题一类是“直接能测但测不准”另一类是“不能直接测只能靠多个信号拼出来”。卡尔曼滤波对这两类问题都有用但用得对不对差别很大。我在RM机器人上见过最值得上卡尔曼滤波的场景基本可以分成三个。第一个是弹丸初速估计。测速模块通过轮子或光电对管给出弹丸通过两个检测点的时间差再用距离除以时间换出速度。问题在于时间差本身有量化误差轮子打滑、弹丸姿态变化、摩擦轮温度变化都会让速度读数跳变。如果直接拿这个瞬时速度去做弹道解算弹着点会飘得非常厉害。第二个是云台姿态估计。陀螺仪直接积分会有漂移加速度计静止时很准、运动时噪声又大得离谱。卡尔曼滤波可以把两者的优势互补得到更平滑、更可信的姿态角让云台PID环路的反馈信号不再带着一堆毛刺。第三个是底盘功率控制。裁判系统给出的实时功率有噪声电调反馈的电流也不一定准如果直接拿去闭环容易把功率环调出低频振荡。用一个状态量去融合“指令”和“反馈”输出给限功率算法底盘的爆发力会明显更稳。这三个场景本质上是同一个数学结构我有一组状态量比如速度、角度、功率状态之间的变化规律我大致知道这就是模型我也有若干观察手段也就是传感器但两边都带噪声。卡尔曼滤波要做的就是找一个随时间变化的最优权重把模型预测和传感器测量合起来。1.2 核心思想用不确定度动态分配信任理解卡尔曼滤波我推荐用一个“两位专家开会”的类比。你面前有两位专家一位是模型预测专家他根据上一帧状态和运动学规律告诉你“现在弹丸速度应该在15.2 m/s左右”另一位是传感器测量专家他根据测速模块实时读数告诉你“这一发打出来的瞬时速度是16.8 m/s”。两人意见不一致你到底听谁的如果模型专家平时的预测方差很大传感器专家读数又很稳你会偏向传感器反过来如果传感器读数跳得离谱你会更相信模型。卡尔曼滤波做的事情非常简单它把两位专家的“可信程度”用方差量化每一帧都按方差大小算出最优加权系数。这个系数就是卡尔曼增益K。这个思路最值钱的地方在于它不是拍一个固定权重而是每一帧动态计算。低速打弹时测速模块的量化误差相对明显K会自动小一点滤波结果更偏向模型平滑值高速连续射击时测量时间差变小、量化误差占比升高K继续自适应调整。这比写死一个alpha的低通滤波科学得多因为低通滤波永远只能用同一个平滑系数去应对不同噪声环境。1.3 为什么比低通滤波更适合RM电控很多队员会问低通滤波简单粗暴为什么非要上卡尔曼。我的回答是低通滤波不是不能用但它本质上是个单参数工具只能按频率成份做加权它不知道你的物理模型是什么。云台角速度用一阶低通拉一下响应变慢了PID相位裕度就会被吃掉测速数据拉到很平滑弹丸真实速度变化时又跟不上。卡尔曼滤波的优势在于它可以显式地把物理规律写进状态转移矩阵。弹丸速度的变化率、云台角度的惯性、底盘功率的功率变化趋势这些都是模型信息。引入模型信息之后滤波不仅在修噪声它还在“预测未来”。在RM这种需要极限响应和控制带宽的场合多一个可用的预测值往往就意味着云台少一帧丢失、弹道多打中一个移动目标。不过我也要说一句公道话卡尔曼滤波不是银弹。如果你的传感器本身线性度差、时序乱、采样周期抖动严重上什么滤波都是白搭。滤波能做的是在信号Topology已经稳定的前提下把估计精度和响应速度同时往前推一步。2. 从公式到代码一维和二维卡尔曼的完整落地2.1 状态空间写法和五个公式还是拿弹丸测速举例子。我们假设弹丸在很短的一段测量区间里速度变化不大把它建模成一个带扰动的缓慢变化状态x_k x_{k-1} w_kz_k x_k v_kx是真实速度z是测速模块观测出的速度w是过程噪声代表“速度并不严格恒定”这个事实带来的扰动v是测量噪声代表测速模块本身的误差。两者的方差分别记为Q和R。完整卡尔曼滤波每帧要算五个公式预测两步更新三步预测 x_pred A * x_prev P_pred A * P_prev * A^T Q更新 K P_pred * H^T * (H * P_pred * H^T R)^(-1) x_new x_pred K * (z - H * x_pred) P_new (I - K * H) * P_pred在一维弹丸速度这个例子里A1H1公式会化简得非常清爽x_pred x_prev P_pred P_prev Q K P_pred / (P_pred R) x_new x_pred K * (z - x_pred) P_new (1 - K) * P_pred这套化简后的版本特别好理解K只取决于P和Q、R的相对大小。P大、R小说明测量可信K趋近于1P小、R大说明模型可信K趋近于0。整个收敛过程实际上就是P和K从初始值逐步进入稳态的过程。2.2 一维版本STM32上的极简实现我直接贴一段在RM工程里用过的写法。一维情况下不需要矩阵运算库就是几个浮点乘除放在测速模块的中断回调或者5ms周期任务里单次运行最多耗时几百纳秒基本不占用CPU时间。typedef struct { float x; // 状态弹丸速度估计值 float P; // 状态方差 float Q; // 过程噪声方差 float R; // 测量噪声方差 } Kalman1D_t; void Kalman1D_Init(Kalman1D_t *kf, float init_x, float init_P, float Q, float R) { kf-x init_x; kf-P init_P; kf-Q Q; kf-R R; } float Kalman1D_Update(Kalman1D_t *kf, float z) { // 预测 float P_pred kf-P kf-Q; // 更新 float K P_pred / (P_pred kf-R); kf-x kf-x K * (z - kf-x); kf-P (1.0f - K) * P_pred; return kf-x; }这里有两个特别容易被新手忽略的地方。第一第一次调用前要合理设置P的初值。P0本质上是“我对初始状态x0有多不确信”。如果完全不确定就设一个大值比如100.0让滤波器在前几帧快速收敛如果已经知道弹丸速度大概是15 m/s可以把P0设成0.1这种小值但后面我会说大多数场景宁可大一点。第二Q和R不是随便拍出来的。它们直接决定滤波的“性格”Q决定模型跟踪快速变化的灵敏度R决定平滑程度。这两个参数怎么调第三章会专门展开。2.3 二维版本速度加加速度的矩阵推导弹丸在被摩擦轮带动的过程中存在明显的加速段。一维模型在速度快速上升时会有跟踪滞后表现就是滤波值总是慢半拍。解决办法是把状态向量扩展成“速度加速度”两个量x [v, a]^T状态转移方程这样写[ v_pred ] [ 1 dt ] [ v_prev ] [ a_pred ] [ 0 1 ] [ a_prev ]这个模型的意思是速度的变化由加速度决定加速度本身则做缓慢随机游走。P矩阵是2x2的协方差矩阵Q矩阵也要相应扩展成2x2Q [ Q_v 0 ] [ 0 Q_a ]观测方程仍然只测速度z [1 0] x v。也就是说加速度不是直接被测出来的而是滤波器根据速度的变化趋势“反推”出来的隐藏变量。二维的好处是能比一维更积极地跟踪速度爬升代价是要多调一个Q_a参数而且协方差矩阵交叉项一旦初始化不当滤波结果可能抖动甚至发散。我的建议是先把一维跑通确认数据流和调参逻辑再上二维。上来就上高维滤波出了问题你根本分不清是数学错了、代码错了还是参数错了。2.4 二维版C代码速度加速度联合估计下面这段是二维版本的核心更新代码我保留了全部计算过程方便你对照公式看。代码里用显式变量写出协方差矩阵的四个元素没有引入矩阵库读起来更直观。typedef struct { float v; // 速度估计 float a; // 加速度估计 float P00, P01; // 协方差矩阵第一行 float P10, P11; // 协方差矩阵第二行 float Q_v, Q_a; // 过程噪声 float R; // 测量噪声 float dt; // 周期 } Kalman2D_t; void Kalman2D_Init(Kalman2D_t *k, float init_v, float init_a, float init_P, float Qv, float Qa, float R, float dt) { k-v init_v; k-a init_a; k-P00 init_P; k-P01 0.0f; k-P10 0.0f; k-P11 init_P; k-Q_v Qv; k-Q_a Qa; k-R R; k-dt dt; } float Kalman2D_Update(Kalman2D_t *k, float z) { float dt k-dt; // 预测状态 float v_pred k-v k-a * dt; float a_pred k-a; // 预测协方差 P_pred A * P * A^T Q float P00_pred k-P00 2.0f * dt * k-P01 dt * dt * k-P11 k-Q_v; float P01_pred k-P01 dt * k-P11; float P10_pred k-P10 dt * k-P11; float P11_pred k-P11 k-Q_a; // 更新增益 float S P00_pred k-R; float K0 P00_pred / S; float K1 P10_pred / S; // 更新状态 float innov z - v_pred; k-v v_pred K0 * innov; k-a a_pred K1 * innov; // 更新协方差 P_new (I - K*H) * P_pred k-P00 (1.0f - K0) * P00_pred; k-P01 (1.0f - K0) * P01_pred; k-P10 P10_pred - K1 * P00_pred; k-P11 P11_pred - K1 * P01_pred; return k-v; }注意协方差矩阵按定义是对称的所以P01和P10在理论上永远相等。中间如果出现数值漂移导致轻微不对称不会立刻让算法崩掉但强行把它俩写成相等也不是好习惯。保持代码按照标准公式算数值上会让滤波更稳定。3. 实战调参Q与R的物理含义和调试流程3.1 把Q和R当作可解释的物理量很多同学把Q和R当成两个旋钮在那里乱拧总觉得拧到一个神秘数值就功德圆满。你得换个角度理解它们。过程噪声Q代表“模型本身有多不可信”。如果你认为弹丸速度在测量区间内几乎恒定Q应该很小如果弹丸速度会因为摩擦轮打滑或供弹机构卡顿而剧烈变化Q就要适当放大。测量噪声R代表“传感器读数有多不可信”测速模块硬件输出抖动明显时R要调大。量纲上如果状态是速度单位是m/s那么R就是速度测量噪声的方差单位是(m/s)^2。Q则与过程扰动在单个周期内的能量有关。有些人会用(最大加速度*dt)^2这种公式去估算Q我觉得做精细调参时可以算但起步阶段没必要太精确。先扫数量级再微调。这里有个很直观的观察法。Q很小、R很大的时候滤波结果非常平滑但真实速度发生变化时输出会有明显滞后Q很大、R很小的时候滤波结果几乎贴着原始测量走平滑效果约等于无。最优参数通常落在中间区域既保留一定平滑度又不至于在关键时刻反应太慢。3.2 五步定参流程与判据我调QM和R一般按下面这个顺序来大家可以当作模板用。先确定一个估计范围。拿测速模块来说测量速度一般在10到30 m/s把R先粗略定在测量方差的量级上比如0.25换算成标准差就是0.5 m/s。然后让Q从0.001到10之间按指数步长扫。录制一段覆盖低速、中速、高速、急停的数据把原始测量和滤波结果画在同一张图里。没有上位机的话用串口把数据导到Excel里画也行。观察滞后程度。如果滤波结果比真实速度变化慢很多就增大Q或者减小R。观察平滑度。如果输出仍然抖得像没滤波一样就减小Q或者增大R。重复第三步和第四步直到你觉得“响应够快且曲线足够干净”。这个流程看着笨但它能帮你建立对Q/R的直觉。等到你对某个传感器的噪声特性足够熟悉基本上一两次就能定位到量级。3.3 起步参数参考表下面这张表是一些常见场景的起步参数可以直接拿去跑但不代表你不用再看真实数据。每个机器人传感器硬件不同、安装方式不同上电噪声也不同最终参数一定要现场标定。应用场景状态量Q起步建议R起步建议备注弹丸测速速度v0.01~0.10.1~1.0连续射击时可以考虑R自适应弹丸测速进阶速度v加速度aQ_v: 0.01~0.1, Q_a: 1~100.1~1.0跟踪快速爬升更积极云台角度估计角度theta角速度omegaQ_theta: 1e-4~1e-3, Q_omega: 1e-3~1e-21e-2~1e-1取决于陀螺仪零漂程度底盘功率闭环功率p1e-2~1e-11~10功率波动大时Q要放大这张表的价值在于给出数量级参考。很多人调参失败不是因为灵敏度不够而是因为初始参数差了三个数量级导致要么滤波器完全追不上测量要么干脆发散。3.4 进阶用残差做R自适应固定Q/R能覆盖大部分场景但RM比赛现场很残酷。测速模块可能因为供电波动变差云台陀螺仪可能因为振动加剧导致角速度噪声增大。固定参数的滤波器会在一段时间后失配。解决思路是实时估算测量噪声。计算innovation残差也就是测量值与预测值的差innov z - H * x_pred理论上如果模型和参数都正确innovation应该是一个均值为零、方差为S HPH^T R的随机序列。我们可以用滑动窗口统计innovation的经验方差反过来修正R。这个思路实现不复杂效果却很明显。我写过一版简单的自适应R维护一个长度为N的循环缓冲区每次更新后推入当前innovation定期计算方差再根据方差与理论S的比值去缩放R。实测下来连续射击时滤波输出明显更稳弹道解算的落点散布也小了一圈。这个技巧很值得电控组师弟师妹们去实现性价比极高。4. 现场问题与排查技巧实录4.1 先查时间戳再查数学卡尔曼滤波的预测步骤本质是用上一帧状态推导到当前帧。如果某一次测量因为中断延迟晚到了两个周期滤波器会拿“过期测量”去做更新整套状态会被带偏到天边去。我在STM32上遇到过几次“滤波结果发散但数学明明没错”的问题最后都是时序引起的。排查思路很简单给每次测量打一个us级时间戳更新时检查当前测量时间戳和上一帧的时间间隔。如果和预期周期明显不符要么丢弃这帧数据要么把状态转移矩阵里的dt换成实际间隔。很多时候滤波炸了不是数学错了是时间错了。4.2 传感器性能漂移与R失配有一次打弹道测试连续射击的后半段解算出来的着弹点总是偏上。排查了很久最后发现测速模块在连续射击时发热光电对管响应延迟升高R变大但代码里还是固定值滤波器就过度相信了已经变差的测量。这个案例让我印象很深。从那以后我对所有滤波代码的要求都是R可以被动态调整而不是一个写死的常量。比赛中要警惕传感器性能漂移卡尔曼滤波不只是一个离线调好的固定算法更应该被看成一个能感知传感器健康度的在线估计器。4.3 初始化P0太小导致长时间不收敛新手最容易犯的错是上来把P0设成和Q差不多大。比如Q是0.01就顺手把P0也等于0.01结果滤波结果头几十帧都拖着一条明显的“温吞”尾巴怎么等都不收敛。记住P0代表先验不确定度宁可设大十倍也不要设小。P01和P0100在跑一秒数据后通常会收敛到同一个稳态但P0设小的代价是收敛慢。到了比赛第一发弹就要精准的场景这几十帧的收敛时间就是致命的。保险起见初始化时把P0设成一个比Q大两三数量级的数比如Q0.01时P010基本不会出错。4.4 调试手段把内部状态全部发出来实战中最有效的排查方法是把x、P、K、innovation这些内部量全部通过串口发到上位机。很多人只发最终滤波结果一旦发现结果不对根本看不出是哪一步出了问题。你把中间量打出来配合波形可视化工具就能迅速判断是预测错、更新错还是参数错。我们组内部定了一个规矩滤波模块必须提供debug输出接口至少能打印状态向量和协方差对角元素。这个习惯帮我们省了很多对线时间。你在调Q/R时光看滤波输出是看不出P在怎么走的但P的变化能告诉你滤波器是不是已经进入稳态K的变化能告诉你当前到底是更相信模型还是更相信测量。4.5 float与double选型STM32F4这类带FPU的单片机上float计算是硬件加速的精度对绝大多数RM场景足够。只有在做低漂移IMU姿态解算或者需要长时间积分的时候才需要考虑double。要注意的是有些芯片的FPU是单精度double运算是软件模拟性能差距能到几十倍。我见过有人在全代码里用了double结果主循环执行时间暴涨整机控制周期直接拖垮。嵌入式上能float就float能用定点就不碰浮点这是原则。5. 从单状态滤波走向多传感器融合5.1 云台姿态估计怎么套用同一套框架弹丸测速是一维或二维状态估计云台姿态估计则更容易让人晕因为涉及角度、角速度和角加速度还常常有多个传感器。但拆开看结构还是那套状态量选偏航角和偏航角速度陀螺仪角速度作为模型预测依据视觉或加速度计解算出的角度作为测量。一个简化模型是[ theta_pred ] [ 1 dt ] [ theta_prev ] [ 0 ] [ omega_pred ] [ 0 1 ] [ omega_prev ] [ gyro_bias_est ]陀螺仪的零漂可以作为状态量一并估计这样滤波器不仅能输出平滑角度还能在线估计出陀螺仪零偏这比用固定补尝值靠谱得多。很多RM队伍的云台angle滤波就是从这个模型起步的效果比互补滤波更有延展性。如果你要融合IMU里加速度计和陀螺仪的数据记得先做坐标变换和重力分量补偿不然测量方程里会平白多出一堆系统误差。我的经验是先把加速度计在静止时测得的向量和重力向量做对齐再拿到滤波器里当观测这样出来的姿态角才不会有明显静差。5.2 非线性与EKF什么时候需要升级标准卡尔曼滤波只适用于线性系统也就是状态转移和观测都是线性关系。RM里很多真实系统是非线性的比如云台的旋转矩阵、弹道解算时的空气阻力模型、视觉测距里的透视投影关系。这类非线性问题最简单常用的扩展方式是EKF也就是扩展卡尔曼滤波。做法是先对非线性函数做一阶泰勒展开用雅可比矩阵替代线性系统中的A和H然后继续跑标准卡尔曼流程。EKF思路简单工程上非常常见但它的线性化误差在小角度、近距离等场景下通常可以接受一旦状态误差大或者系统强非线性就可能不收敛。我自己用得最多的判断标准是如果系统能在正常工作点附近用线性模型较好地描述就用标准卡尔曼或EKF如果机器人要做大范围翻滚、快速掉头这些极端机动可能就要考虑UKF甚至粒子滤波了。不过对RM电控而言99%的场景EKF已经完全够用没必要一上来就追求更高阶算法。5.3 电控组后续可以扩展的方向卡尔曼滤波在RM电控里还能扩展出不少方向。比如视觉目标预测用一个恒速或恒加速模型对敌方机器人的位置做外推弥补云台转向过程的滞后再比如电源管理融合裁判系统功率、电池电压和电调电流提前预测功率上限让机器人能在不扣血的条件下打出更高射速。关于“合集”这两个字我在标题里也写了卡尔曼滤波本身不是孤立存在的它在不同模块里的不同形态值得电控组以合集的思路去沉淀。测速模块的滤波、云台姿态的融合、功率闭环的状态估计三套代码可以共用同一个矩阵运算层参数可以按场景分别标定。形成一个标准化的滤波模板比每个队员各写各的、彼此不通用要高效得多。最后再分享一个小技巧。如果你要教新人入门卡尔曼滤波不要让他直接看多变量矩阵推导。先拉着他在点一份测速模块的数据处理任务从一维卡尔曼写起跑起来、画出来、调起来等他发现一维不够用了再自然地引入二维状态向量。这样不仅学习曲线平滑而且他上手后就会自己去找Q/R的物理含义而不是在那里背公式。我第一次带队员走完这个流程后后续让他去写云台姿态融合基本上不用我多解释他自己就能把模型搭出来这就是实战路线最大的好处。