1. 从赛前一次云台抽风说起RoboMaster电控为什么绕不开卡尔曼滤波1.1 那段让人抓狂的赛前调试前年区域赛前一周我们队的英雄云台在低速跟踪时会周期性抖一下幅度不大但很致命——打小陀螺的命中率直接掉了三成。拆开看代码姿态解算用的是最朴素的互补滤波陀螺仪和加速度计各给一个系数就完事了参数调了三天要么响应慢得像喝多了要么噪声大到电机跟着啸叫。后来把加速度计和陀螺仪的数据丢进一个两状态的卡尔曼滤波器二十分钟后云台就稳了那种原来是这样的感觉至今记得很清楚。这个故事几乎所有打过RoboMaster的队伍都会遇到。云台要跟目标、底盘要估计速度、发射机构要控制摩擦轮转速、超级电容要算剩余能量每一个环节背后都在跟带噪声的传感器数据打交道。而卡尔曼滤波就是处理这类问题最趁手的一把工具。这篇内容我会从中科大战队的实际电控开发出发把卡尔曼滤波在RM里的落地思路、公式含义、代码实现和调参经验一次讲透无论你是刚进队的新人还是已经打过一届的老电控都能从里面找到能直接抄的东西。1.2 卡尔曼滤波到底替电控解决了什么问题一句话它能把不准的模型预测和不准的传感器测量揉在一起得到一个比两者都准的估计值。放到RM场景里陀螺仪短时间准但会漂移加速度计长期不漂但受震动干扰极大编码器能测速但有量化误差和延迟电流传感器受电机换向噪声影响严重。单靠任何一个都做不出稳定的控制而卡尔曼滤波恰好给出了一个数学上最优的融合方式。它解决的核心痛点有三个。第一是噪声抑制原始传感器信号高频抖动大直接拿去做PID的微分项会把噪声放大成一个咆哮的电机。第二是延迟补偿很多传感器比如某些IMU的加速度计、视觉解算的角度输出频率低或者有固定延迟用滤波器可以在两帧之间做预测把控制周期填满。第三是不可观测量估计比如底盘的实际对地速度很难直接测但可以通过轮速和IMU加速度融合估计出来这对打滑抑制和里程计至关重要。1.3 这篇内容适合谁看需要什么基础如果你是刚接手云台电控的新人建议先把矩阵运算和状态空间表达这两个概念补一下至少要能看懂状态向量乘矩阵是什么意思。有单片机C语言基础、会写PID、会用定时器采IMU这就够了。我下面讲公式时会尽量用大白话代码全部是能直接编译进STM32工程的风格。硬件上假设你手里有一块常见的六轴或九轴IMUMCU有FPU最好没有的话浮点运算也能跑只是要注意主频。2. 卡尔曼滤波的数学思想把那些吓人的公式讲成人话2.1 预测和更新一场反复横跳的自我修正卡尔曼滤波的整个流程说白了就两个动作在循环预测和更新。预测是根据我上一步的估计推测这一步大概在哪更新是拿传感器实测值来纠正我猜的位置。你可以把它想象成开车进隧道进隧道前你知道自己的速度于是预测出了隧道口的位置预测出来一看路牌发现偏了就按偏差修正自己的位置估计更新。两个动作一轮一轮跑估计值就越来越贴近真实。关键在于卡尔曼滤波不只是修正位置这一个值它同时维护一个叫协方差的东西代表我对当前估计有多不确定。预测时不确定度变大因为模型有误差更新时不确定度变小因为有了新信息。滤波器会根据模型的不确定度和测量的不确定度谁更小来决定更相信谁。这就是它比固定系数的互补滤波高级的地方——系数是动态算出来的而不是拍脑袋定的。2.2 五个核心公式逐个掰开揉碎标准卡尔曼滤波有五个方程我按RM电控的使用顺序讲。状态预测x(k|k-1) A·x(k-1|k-1) B·u(k)。意思是这一步的预测状态等于上一步估计状态经过状态转移矩阵A演化再加上控制输入u通过矩阵B施加的影响。在云台角度解算里A通常是一个随时间步长变化的矩阵x里放角度和陀螺零偏u就是陀螺仪测到的角速度。协方差预测P(k|k-1) A·P(k-1|k-1)·Aᵀ Q。这一步是在传播不确定性Q是过程噪声矩阵代表你对模型有多不信任。Q越大说明你认为模型越不准滤波器就会越依赖传感器。卡尔曼增益K(k) P(k|k-1)·Hᵀ·(H·P(k|k-1)·Hᵀ R)⁻¹。这个是整个滤波器的灵魂。H是观测矩阵把状态映射到能被传感器观测的空间R是观测噪声矩阵。K的本质是一个权重它告诉滤波器这次测量值该信几分。R越大测量越不可靠K越小滤波器越相信自己预测的值。状态更新x(k|k) x(k|k-1) K(k)·(z(k) - H·x(k|k-1))。括号里那一项叫新息就是实测值减去预测的观测量也就是预测偏差。用这个偏差乘上增益去修正状态估计。协方差更新P(k|k) (I - K(k)·H)·P(k|k-1)。修正完状态也要修正不确定度。这一步保证了滤波器不会一直自信满满也不会一直摇摆不定。你不需要在工程里手推这五个公式但理解它们的物理含义很关键——尤其是Q和R它们直接决定了滤波器的性格后面调参会反复用到。2.3 和互补滤波、低通滤波比它到底强在哪很多队伍一开始用的是互补滤波两个系数一加一减实现只要几行代码调起来也直观。它在云台角度这种陀螺仪积分漂移加速度计抖动的经典场景下确实能用问题是系数固定遇到不同的运动状态就露馅静止时希望滤波重一点压噪声高速旋转时又希望响应快一点别滞后。固定系数只能取折中。低通滤波更简单就是压高频但它天然带来相位滞后用在控制回路里会吃掉相位裕度容易引起振荡。卡尔曼滤波的增益是动态的运动剧烈、预测不准时自动加大增益追传感器运动平稳时自动减小增益压噪声。这种自适应是它最大的优势。当然它也不是没有代价。计算量比互补滤波大得多状态维数一高对MCU就是负担建模不对、Q和R乱设滤波器照样能发散。所以我的建议是简单场景用互补滤波够用就别硬上卡尔曼但在云台、底盘估计、超级电容能量管理这类对精度要求高的环节卡尔曼值得投入。3. 云台角度解算卡尔曼滤波的第一次实战3.1 传感器选型与坐标系约定这一步千万别省先说硬件。常见的六轴IMU三轴陀螺三轴加速度是最低配置如果还要做绝对航向就需要加磁力计或视觉。但RM赛场上电机多、电流大碳纤维车体又是导电的磁场环境非常脏磁力计基本是靠不住的我个人的做法是云台偏航角用陀螺积分为主、视觉或碰撞线做慢速校正不用磁力计。坐标系一定要提前约定好并写进注释。我们的约定是X轴指车头前方Y轴指左侧Z轴向上陀螺仪和加速度计都按右手定则。碳板车身会把IMU的地和车架连在一起布线时信号线要远离电机线束最好双绞或加屏蔽否则你会发现加速度计的数据里全是电机换向的噪声后面滤波再强也救不回来。IMU安装位置尽量靠近云台旋转中心减少离心加速度对加速度计的污染。3.2 状态量怎么选为什么是角度加零偏最经典的两状态模型状态向量x [角度, 陀螺零偏]ᵀ。为什么要把陀螺零偏也放进状态里估计因为陀螺仪的零偏会随温度变化慢慢漂如果当成常数写死积分出来的角度就会慢慢跑偏。把它当成一个待估计的缓慢变化量放进状态滤波器会在运行过程中自动把它辨识出来这样角度的长期稳定性会好很多。状态转移矩阵A就是[1, -dt; 0, 1]。第一行表示角度 上一时刻角度 - 零偏×dt 陀螺测量×dt第二行表示零偏保持不变。观测矩阵H取[1, 0]因为我们只用加速度计解算出的角度作为观测量零偏是不可直接观测的。这里有个细节加速度计只有在运动加速度很小的时候解算出的倾角才可信所以观测量前面通常要加一个可信度判断运动剧烈时把R调大甚至临时不更新。3.3 能直接抄进工程的C代码实现先给一个最通用的单状态版本方便新手理解核心逻辑typedef struct { float x; // 状态估计 float P; // 估计协方差 float Q; // 过程噪声 float R; // 观测噪声 } KF1D; float KF1D_Update(KF1D *kf, float z) { // 1. 预测不确定度增加 kf-P kf-Q; // 2. 计算卡尔曼增益 float K kf-P / (kf-P kf-R); // 3. 更新状态 kf-x K * (z - kf-x); // 4. 更新协方差 kf-P * (1.0f - K); return kf-x; }这段代码用在比如电池电压滤波、单个轮速估计上都非常合适。真正做云台角度用两状态版本更稳typedef struct { float angle; // 角度估计 float bias; // 陀螺零偏估计 float P[2][2]; // 协方差矩阵 float Q_angle; // 角度过程噪声 float Q_bias; // 零偏过程噪声 float R_measure; // 观测噪声 float dt; // 采样周期, 单位秒 } KalmanAngle; float KalmanAngle_Update(KalmanAngle *kf, float newAngle, float newRate) { // ---------- 预测 ---------- float rate newRate - kf-bias; kf-angle kf-dt * rate; kf-P[0][0] kf-dt * (kf-dt * kf-P[1][1] - kf-P[0][1] - kf-P[1][0] kf-Q_angle); kf-P[0][1] - kf-dt * kf-P[1][1]; kf-P[1][0] - kf-dt * kf-P[1][1]; kf-P[1][1] kf-Q_bias * kf-dt; // ---------- 更新 ---------- float S kf-P[0][0] kf-R_measure; // 新息协方差 float K0 kf-P[0][0] / S; float K1 kf-P[1][0] / S; float y newAngle - kf-angle; // 新息 kf-angle K0 * y; kf-bias K1 * y; float P00_temp kf-P[0][0]; float P01_temp kf-P[0][1]; kf-P[0][0] - K0 * P00_temp; kf-P[0][1] - K0 * P01_temp; kf-P[1][0] - K1 * P00_temp; kf-P[1][1] - K1 * P01_temp; return kf-angle; }这段的推导思路在业界流传很广核心就是把零偏当状态估出来。注意dt必须是真实采样周期如果你的IMU中断有抖动最好用定时器实测的dt别用理论值否则滤波器会周期性偏。3.4 Q和R怎么调拿数据和现象说话参数整定是新手最头疼的地方我给一套实操方法。先整R。让云台静止把加速度计解算出的角度打印出来看它的波动幅度。波动大说明测量噪声大R就设大一点通常从0.01到0.5之间取。判断标准是静止时滤波输出的角度不能跟着加速度计的抖动跳。再整Q_angle和Q_bias。Q_angle代表你相信角度模型的演化有多不确定Q_bias代表零偏变化有多快。经验上Q_angle从0.001起调Q_bias从0.003起调。Q_angle设大滤波器响应快但噪声大Q_bias设大零偏跟踪快但可能把真实角度信号也当成漂移给吃掉。用现象反推。如果角度长期缓慢漂移说明零偏估计不够加大Q_bias。如果跟随目标时有明显滞后加大Q_angle或者减小R。如果输出高频抖动减小Q_angle或者加大R。我一般会在上位机用曲线把原始陀螺积分、加速度计解算、滤波输出三条线画在一起一眼就能看出谁的问题。注意调参时一定要用真实运动数据回放不能在静止状态下调好了就上赛场。云台快速旋转时离心加速度会污染加速度计这时候如果不动态加大R滤波器会把错误的倾角当真的用云台会突然摆头。4. 底盘、超级电容和更多RM场景的卡尔曼应用4.1 底盘速度估计与打滑抑制底盘电控里轮速编码器能测出轮子转速但在打滑时轮速和实际对地速度完全对不上麦克纳姆轮全向移动还涉及多个轮子的速度合成。单纯用轮速做里程计一场比赛下来位置误差能积累到几十厘米。做法是把IMU的加速度积分和轮速融合轮速作为观测量加速度作为控制输入用一个线性卡尔曼在二维X、Y速度上估计底盘实际速度。底盘场景的模型是v(k) v(k-1) a·dt观测量是z v_odom由轮速换算非常简单。难点在于打滑检测当轮速变化率和加速度计读数严重不一致时说明在打滑此时把R临时放大让滤波器不信里程计主要靠加速度积分撑过去。实测下来融合后的速度在急停急转时明显更真实配合功率控制能减少电机过载。4.2 超级电容能量状态估计RM的功率控制离不开超级电容。规则给的是功率上限超了要扣血所以每辆车都在用电容把瞬时大功率缓冲掉。电控要做的是实时估计电容的SOC剩余能量据此决定放电策略。SOC最直接的办法是安时积分SOC SOC₀ - ∫I·dt / 额定容量。但电流传感器有零偏和噪声积分久了误差就累积了。这里用一个单状态卡尔曼非常合适状态是SOC控制输入是实测电流观测量是电容端电压通过开路电压和SOC的关系反推。电压测量在电容内阻大、电流突变时会有明显偏差正好用卡尔曼动态权衡。整定的关键是把电流传感器的噪声方差估准一般做成标定流程上电时清零并记录一段静置数据反推零偏。提示电容电压和SOC的关系是非线性的做模型时至少用分段线性近似别当成理想电容用U Q/C超级电容在大电流下电压跌落很明显直接套公式误差很大。4.3 多模块联调时的接口约定卡尔曼滤波不是孤立的它跟控制的接口设计很关键。我们的约定是滤波器统一跑在1kHz的姿态任务里输入是IMU原始数据和上一拍控制量输出是滤波后的角度、角速度、零偏通过共享内存或者全局结构体给控制层用。控制层不做任何滤波避免重复滤波引起相位混乱。另外要注意初始化。滤波器上电时协方差要设成一个较大的初值让它快速收敛状态初值可以从传感器各取一次直接赋值。云台上电那几秒钟通常是自检状态云台不响应遥控等P[0][0]降到某个阈值以下再放行这样能避免上电瞬间的乱摆。碳板车体上如果IMU和主控地不干净初始化时角度可能直接就是一个错的值所以上电自检里加一条角度是否在合理范围的判断很有必要。5. 常见问题排查与踩坑实录5.1 三类典型症状发散、滞后、抖动发散最常见的原因是符号错误或dt错误。状态转移矩阵里零偏项的符号如果写反滤波器会越跑越偏表现为角度单调漂移且无法收敛。dt用错比如实际1ms写成10ms也会造成等效增益错乱最终发散。排查方法很简单把预测和更新分开打印看协方差是否单调增大。滞后是Q设得太小或R设得太大导致的滤波器过度相信模型不信任测量。表现为云台跟随目标时慢半拍打快速移动目标时命中率低。解决办法是加大Q_angle或减小R代价是噪声变大需要在两者间找平衡。抖动则相反R太小或Q太大滤波器把传感器噪声放大了。表现为静止时电机就有细微的高频啸叫长时间会把电机和齿轮磨坏。先检查机械和布线确认不是硬件噪声后再调参数。5.2 常见问题速查表现象可能原因排查方向处理办法角度缓慢漂移不收敛零偏未被有效估计检查状态里是否含零偏加大Q_bias检查符号快速旋转后角度错乱加速度计被离心力污染看运动时R是否固定动态调大R或禁用该帧观测静止时电机高频啸叫R太小或Q太大打印滤波输出频谱减小Q或加大R跟随目标慢半拍Q太小或R太大对比滤波与原始信号延迟加大Q_angle上电瞬间云台乱摆协方差初值太小看收敛速度加大P初值并加自检等待电流估计偏差大电流传感器零偏静置标定上电清零并做温补底盘速度里程计漂移打滑未检测对比轮速与加速度打滑时临时放大R5.3 几条只能靠比赛喂出来的经验第一条滤波器不是越复杂越好。有队伍一上来就上扩展卡尔曼处理四元数结果参数多到调不动最后还不如老老实实做两状态。把简单的模型调明白比堆一个调不动的复杂模型强得多。第二条一定要做数据回放。赛场上时间紧不可能在车上慢慢试。我的习惯是把IMU原始数据存到SD卡或者通过无线回传到上位机离线用Python复现滤波调参调到满意再烧回去。这样一次上电就能验证效率高很多。第三条注意数值类型和运算顺序。在MCU上做矩阵运算浮点精度损失比想象中大尤其是协方差矩阵做减法时可能出现微小的负值长期会破坏正定性。可以在每次更新后强制让P的对角元素不小于一个极小正数或者用float的平方根形式UD分解实现稳定性会好很多。第四条滤波器只管估计别让它替你做决策。很多新手喜欢把滤波后的角度直接喂给PID然后指望它解决所有抖动问题。其实抖动可能来自机械间隙、来自电流环、来自控制周期不匹配滤波只解决传感器噪声这一环。定位问题时先把滤波旁路用原始信号跑一遍控制往往能发现真正的原因在别处。第五条从零偏估计的角度说碳板车身的温升对IMU零偏影响非常明显。比赛打久了电机发热车体温度上来陀螺零偏会漂这时候能不能快速跟上就看Q_bias设得够不够。我个人的经验是把Q_bias设在刚好能在一个控制周期内对零偏变化做出反应的量级太大反而会把真实角速度吃掉。最后再分享一个我们队用的小技巧给滤波器加一个健康度监控。每次更新后看新息y的大小如果连续若干拍y都超过某个阈值比如三倍标准差就认为测量异常临时把R放大或者跳过这次更新。这个机制在对付偶发的IMU数据跳变上非常好用相当于给滤波器加了一层保险比赛里少翻好几次车。