反小陀螺算法研发
判断进入小陀螺#
目标小陀螺时，装甲板相对于车身坐标系的朝向角会有剧烈且规律的变化，检验这一时间序列数据的稳定性可以作为进入小陀螺的判据。
简单统计方法：
将这一时间序列数据前后分成两份（如果序列够长可以分成更多份），分别计算它们的方差和均值。如果这一时间序列是稳定的，那么每一份的均值和方差应该大致相同；如果均值差或方差差异大于某一阈值，可以认为目标进入了小陀螺。假设检验方法：
通过检验单位根，若存在则认为不平稳，实现难度比第一种大，Python 有一些对应的库可以参考实现。滤波方法，也就是我主要介绍的方法。
但是注意，姿态角需要转换到车身坐标系下再使用，否则相机和云台自身运动会明显影响判断。
反小陀螺#
如果只对装甲板坐标做普通滤波，就没有区分车身平动和装甲板绕车身旋转。对方高速小陀螺时，滤波器会把旋转运动误认为很大的平动速度，导致云台甩头。把车身和装甲板的运动分开建模，可以很好地解决这一问题。
运动学建模#
由于规则限制，车辆对侧装甲板基本对称，所以一辆车小陀螺时通常只有两种半径。下面对其中一种半径进行分析。
上图是车身的运动学模型。为了便于推导，下面仍用 X/Y 表示水平面上的两个方向；在实际代码中，这两个方向对应机器人坐标系下的 X/Z 平面。沿用原图中的符号约定，车身中心和装甲板坐标有以下关系：
从起始时刻经过时间 \(\Delta t\) 到达时刻 \(i\) 后，有：
$$ X_C+V\cos(\theta_V)\Delta t+L_1\sin(\theta_R+\omega \Delta t)=X_i \tag{3} $$ $$ Y_C+V\sin(\theta_V)\Delta t-L_1\cos(\theta_R+\omega \Delta t)=Y_i \tag{4} $$可以看到方程组的参数包括 \(X_i,Y_i,X_C,Y_C,L_1,\theta_R,\theta_V,V,\omega\)，我们直接观测到的是 \(X_i,Y_i,\theta_R\)。
考虑到实际滤波已由 pitch、yaw 滤波改为对空间坐标滤波，可以把速度拆成两个方向的分量，消去 \(\theta_V\)，此时方程变为：
但此时仍然存在非线性的 \(\sin(\theta_R+\omega \Delta t)\) 和 \(\cos(\theta_R+\omega \Delta t)\)，它们对求解造成了很大的麻烦。考虑使用一阶泰勒展开将其化为线性形式：
$$ \sin(\theta_R+\omega \Delta t) \approx \sin(\theta_R)+\cos(\theta_R)\omega \Delta t $$ $$ \cos(\theta_R+\omega \Delta t) \approx \cos(\theta_R)-\sin(\theta_R)\omega \Delta t $$因为我们的采样周期很短（通常小于 5ms），\(\Delta t\) 很小，此时使用一阶泰勒展开替代非线性部分仍有较好的近似效果。线性化后的表达式如下：
$$ X_C+V_x\Delta t+L_1(\sin(\theta_R)+\cos(\theta_R)\omega \Delta t)=X_i \tag{7} $$ $$ Y_C+V_y\Delta t-L_1(\cos(\theta_R)-\sin(\theta_R)\omega \Delta t)=Y_i \tag{8} $$现在观测方程已经可以线性化处理，使用整车模型 EKF 来估计车身中心、平动速度、车身朝向、角速度和半径。
轨迹匹配的改进#
轨迹匹配同时考虑了装甲板在图像上的位置、装甲板朝向角和数字识别结果。
首先会过滤掉倾斜角过大的装甲板，只保留朝向角在约 45 度以内的观测值。这样可以避免侧向装甲板 PnP 误差过大，把错误姿态送入滤波器。
随后对每条已有轨迹和每块新识别装甲板计算匹配代价。代价主要由两部分组成：图像中心距离和朝向角差。图像距离会除以装甲板高度做归一化，避免近大远小带来的尺度影响；如果数字识别结果与轨迹数字不同，则会加重惩罚，降低错误匹配概率。最后使用 Auction 分配算法做整体匹配，而不是每条轨迹各自贪心选择最近目标。
轨迹状态也会随匹配结果更新：连续匹配到目标时进入正常状态；短时间没匹配到时进入短暂丢失状态，仅做预测；丢失太久后才清空轨迹。这样可以减少高速小陀螺时频繁切轨迹的问题。
预测小陀螺#
如果仍然尝试击打当前正在观察的装甲板，命中率通常不会高。因为识别、滤波、云台响应和弹丸飞行都存在延迟，快速小陀螺会在这段时间内把当前装甲板转走。更合理的思路是利用当前观测到的装甲板估计整车旋转规律，再预测下一块即将转到枪口前方的装甲板。
实际实现没有为四块装甲板强行创建固定轨迹，而是由轨迹匹配自然维护多条轨迹。当当前锁定轨迹切换到同数字的新轨迹时，代码会认为发生了装甲板切换：旧轨迹上的整车滤波器会转移到新轨迹上，同时记录两次装甲板朝向之间的角度差。这个角度差后续会作为相邻装甲板间隔的估计，用来判断下一块装甲板什么时候进入可击打窗口。
半径并不是完全依赖测量自由收敛。滤波器会估计半径，但代码对半径做了范围限制，避免 PnP 测距噪声把半径带到不合理的值。初始时也会给一个经验半径，让滤波器从相对可信的位置开始收敛。
需要注意的是，只有朝向角较小、观测质量较好的装甲板才适合参与更新。倾斜角过大的装甲板会明显影响 PnP 姿态和距离，送入滤波器反而会破坏预测结果。
击打小陀螺#
击打逻辑可以简单理解成三步：先判断目标是否真的在转，再决定锁哪一块装甲板，最后只在合适的时间窗口里允许开火。
首先，视觉侧会根据车体模型估计目标的旋转速度。只有连续几帧都表现出明显旋转，才认为进入小陀螺；如果旋转速度降下来，也不会立刻退出，而是等状态稳定后再退出。这样可以避免因为一两帧抖动导致反复切换状态。
进入小陀螺后，系统不会死追当前看到的那块装甲板。因为从识别、解算、云台动作到弹丸飞行都有延迟，等子弹到达时，当前装甲板很可能已经转走了。实现的思路是：当前看到的装甲板主要用来估计整车的运动和旋转规律，真正击打时要瞄准即将转到枪口前方的那块装甲板。
锁定目标时，代码会优先选择更靠近瞄准中心、距离更合适、并且已经稳定识别的轨迹。如果操作手已经锁定了某个数字，后续会尽量保持攻击同数字目标。当前目标转走或短暂丢失时，会根据旋转方向切到下一条合适轨迹，而不是重新随便选一个最近的装甲板。
预测阶段也分层处理。轨迹刚建立时直接使用当前解算结果，避免滤波器还没收敛就乱预测；轨迹稳定后先使用滤波结果；再稳定一些才加入弹丸飞行时间和云台响应延迟，预测装甲板转到可击打位置时的瞄准角。
具体开火有两种思路：普通小陀螺主要采用定点击打，也就是瞄准车体中心附近的固定射线，等装甲板自己转进可命中的角度范围再打；特殊目标可以采用跟随击打，即直接预测下一块装甲板的位置并跟过去。无论哪种方式，视觉侧都会持续输出 pitch 和 yaw，但只有预测到装甲板进入命中窗口时，才会把击打标志发给电控，让电控允许发弹。