注意: 这个项目是玩玩的，实际使用不用这样做,如果你觉的有用，那非常好；但如果你觉得这个项目是作者纯闲着没事干，那么你猜对了（

大家好啊，最近台风天，不知道大家怎么样，我这儿被淹惨了(；へ：)

反正被困在家里没事情干，我就继续玩玩之前一直想干而且很酷的项目---GM6020的速度融合

6020反馈的速度精度只有1RPM，这就导致在在某些场景下对6020的控制存在困难。直接对编码器进行微分+滑动窗口能较好的解决这个问题，而且效果还行，但是这样会带来一些延时和抖动，在高速场景不太可靠~~而且不是很酷~~等问题，所以我就想尝试一些不一样的东西。

我搭建了一个简易的测试平台，使用了一个2500P/R的E6B2作为参考（有点偏心，但最终缓解了一点）

微信图片_2026-08-15_121920_022.png

一. 变噪卡尔曼

变噪卡尔曼是指卡尔曼滤波器中的观测噪声或过程噪声不是固定值，而是根据当前运行状态动态调整

普通卡尔曼通常使用固定噪声： $R=\text{constant}$

变噪卡尔曼则使用： $R_k=f(\text{当前速度、工况或传感器状态})$

之所以我要用变噪卡尔曼的原因是因为我发现在低速情况下速度的微分更可靠，而高速情况下6020的速度反馈更可靠，因此当前系统的噪声不是固定的，是随速度变化的，因此我选择了变噪卡尔曼

二. 各个物理模型

1. 状态模型

状态向量为： $x_k= \begin{bmatrix} \theta_k\\ \omega_k \end{bmatrix}$

$\theta_k$ ：6020多圈位置, 单位 rad

$\omega_k$ ：GM6020 角速度, 单位 rad/s

状态转移模型为：

$x_k=F_kx_{k-1}+\omega_k$

$F_k= \begin{bmatrix} 1 & \Delta t\\ 0 & 1 \end{bmatrix}$

展开后：

$\theta_k=\theta_{k-1}+\omega_{k-1}\Delta t$

$\omega_k=\omega_{k-1}$

假设速度在短时间内保持不变，实际加减速被放入过程噪声中

2. 观测模型

当前观测的东西只有两个：

6020的多圈位置
6020的原始速度反馈

观测向量： $z_k= \begin{bmatrix} \theta_{\text{GM6020}}\\ \omega_{\text{GM6020}} \end{bmatrix}$

观测矩阵： $H= \begin{bmatrix} 1 & 0\\ 0 & 1 \end{bmatrix}$

3. 过程噪声模型

当前采用白噪声加速度模型： $Q_k=\sigma_a^2 \begin{bmatrix} \frac{\Delta t^3}{3} & \frac{\Delta t^2}{2}\\ \frac{\Delta t^2}{2} & \Delta t \end{bmatrix}$

4. 变噪观测模型

由于当前噪声不是固定值，而是根据6020的速度大小变化，我在这里做了以下几件事：

先计算速度比例：

$r=\operatorname{clamp} \left( \frac{|\omega_{\text{raw}}|} {\omega_{\text{transition}}}, 0,1 \right)$

随后使用 smoothstep：

$s(r)=r^2(3-2r)$

这个函数的作用是让噪声平滑过渡

有了 $s(r)$ 之后就可以处理位置噪声和速度噪声了，直接上一个插值

位置观测噪声：

$\sigma_{\theta}(r) = \sigma_{\theta,\text{low}} + (\sigma_{\theta,\text{high}}-\sigma_{\theta,\text{low}})s(r)$

速度观测噪声：

$\sigma_{\omega}(r) = \sigma_{\omega,\text{low}} + (\sigma_{\omega,\text{high}}-\sigma_{\omega,\text{low}})s(r)$

这样做了之后就能在高速时提高原始速度观测权重并降低位置观测权重，低速则反之

这样我的R矩阵就构建出来了: $R_k= \begin{bmatrix} \sigma_{\theta,k}^2 & 0\\ 0 & \sigma_{\omega,k}^2 \end{bmatrix}$

三. 最终结果

我的测试平台搭建有点问题，编码器和6020有些偏心，而且电流控6020还是空载状态，所以这个速度控起来和大便一样....

fkkkkkk,我太菜了....

区间	原始6020速度变化 STD	Kalman变化 STD
5~20 RPM	0.466	0.236 RPM
40~80 RPM	1.299	1.094 RPM
≥120 RPM	2.387	1.925 RPM

区间	原始6020速度RMSE	Kalman目标RMSE
5~20 RPM	4.403	4.277 RPM
40~80 RPM	5.875	5.618 RPM
≥120 RPM	6.370	5.923 RPM
静止	0.043	0.127 RPM

以上数据的参考对象都是外置编码器E6B2

可以看出:

除静止段外，Kalman 的目标 RMSE 和 STD 均低于6020原始速度，

而且平滑度可以得到改善，综合来看还可以(?

本人专业是工业设计，高数/线代都没上过课，自己补了一点......

如果有问题，请尽情提出，但是嘴下留情(；へ：)

这里附上6020速度控制和卡尔曼的C代码

chassis_task.c