lesson12 自瞄框架简介#

detector#

既然要实现自瞄，我们首先要达到能够高精度、高鲁棒性地识别出装甲板,显而易见，我们需要一些视觉特征来分辨出这是装甲板来进行识别。

这是3号装甲板的图片，我们可以看到，装甲板明显的视觉特征有两条竖直方向的红/蓝灯条+中间的数字（哨兵是图案），因此我们要针对这些特征做出识别。

现在社区有两种主流方案：一是传统视觉方案，通过opencv的一系列预处理和画轮廓在图像中提取出灯条+取灯条中间的ROI识别出数字；二是神经网络方案，训练一个识别灯条四个角点+数字的神经网络并部署于端侧。

1. 传统视觉

现在假设我们识别红色装甲板，首要任务是提取出红色灯条，红色便是明显的提取方向。转灰度图后一我们可以在RGB空间下R通道减去B通道并二值化来分离出红色，同时考虑上G通道来确保R通道最大；二我们也可以在HSV空间下针对红色的HSV区间进行提取。

当我们提取出红色的二值化图像，往往会发现图像中存在空洞或噪点，我们就需要使用滤波和形态学运算来优化二值化图像。我们通常使用高斯滤波GaussianBlur()来平滑处理减小噪点并保留特征，然后进行形态学处理膨胀dilate()和腐蚀erode()，进行闭操作使一些空洞闭合。

然后我们要根据预处理后的二值化图像提取出灯条的轮廓，我们使用canny算法进行边缘检测然后findContours()提取出轮廓。

最后我们要在提取出的轮廓中筛选出真正灯条的轮廓，我们可以从几何关系、拓扑关系等等方面上进行约束提取出灯条轮廓，如面积/周长大小约束、与矩形相似性约束等等。

数字现在主要使用CNN数字分类模型进行识别。

2. 神经网络

由于社区中有很多开源的装甲板识别模型，因此我们主要的工作是部署神经网络到端侧，当然识别流程中我们会根据一些先验信息划出网络检测的ROI然后网络输出装甲板置信度、颜色类别、装甲板编号类别、四个角点坐标，从而得到letterbbox。

市面上有很多的神经网络框架，主要分为训练框架和推理框架。训练框架常用PyTorch，易上手;推理框架根据硬件平台，Nvidia选TensorRT，Intel选OpenVINO，通用架构有ONNX（也主要作为中间格式在各框架格式中转换）。训练框架与推理框架之间可能会出现一些算子的不兼容，在部署网络时要对不兼容的算子进行处理，避免推理输出过慢（不处理就都用CPU了，那要GPU/NPU干什么😡）

网络端侧部署的主要手段是三板斧量化、剪枝、蒸馏。量化就是把高精度模型（FP32）映射到低精度（FP16、INT8）的过程，量化后模型更轻量化、计算更快。量化方法一是PTQ（训练后量化），仅需要在量化后用数据集校准，但一般精度损失相对较高；二是QAT（量化感知训练），就是在训练过程中模拟量化误差，使得精度损失极少。

剪枝是通过移除模型中不重要的权重、神经元、通道甚至整个层来减少参数量和计算量，而蒸馏是不直接改变模型结构，利用一个大型教师模型来指导小型学生模型的训练。

网络端侧部署是一件比较吃经验的技术，容易踩到一些坑，一般推荐大家做一个benchmark来比较修改前后的模型，避免精度损失过多。

一些神经网络教程： 【吴恩达机器学习经典名课【中英字幕】】 https://www.bilibili.com/video/BV164411S78V/?share_source=copy_web&vd_source=67c7cf3f00d689ea393ebc93fd94efd5 （理论）

【【中英字幕】吴恩达深度学习课程第一课 — 神经网络与深度学习】 https://www.bilibili.com/video/BV164411m79z/?share_source=copy_web&vd_source=67c7cf3f00d689ea393ebc93fd94efd5 （理论）

【YOLO环境配置】 https://www.bilibili.com/video/BV182bZzMEYD/?share_source=copy_web&vd_source=67c7cf3f00d689ea393ebc93fd94efd5 （YOLO系列实操，同时理解训练和推理方法）

由于传统视觉方案受光照影响较大，且鲁棒性不强，而神经网络鲁棒性高，所以当前主流使用神经网络方案，传统灯条识别作为观测信息补充，而且在神经网络识别灯条角点后也需要传统方案修正角点（亮度质心、PCA 等），同时得到神经网络的鲁棒性和传统的高精度

tracker#

现在我们成功实现了detector，做到了精确识别装甲板，但是由于云台响应和弹丸飞行都需要时间，这段时间敌方装甲板仍然在运动，因此我们需要跟踪装甲板估计它的运动状态，然后考虑一系列时间延迟算出提前量来做到精确打击。

运动状态估计的前提是我们需要解算出装甲板的位姿，现在我们已知像素坐标系下的灯条四个角点的坐标，且已知装甲板的物理尺寸以及各个参数，这完美符合PNP算法的需求。PNP可以通过已知的3D点以及其对应的2D点+相机内参和畸变参数得出相机坐标系下的装甲板的位姿，这样我们得到了观测值6dof位姿$（x,y,z,roll,pitch,yaw）$，但是roll和pitch根据规则手册几乎已知，因此我们只需要4dof$(x,y,z,yaw)$。

光得到装甲板的位姿还不够，我们还需要速度才能估计运动状态，预测之后装甲板的位姿。我们可以先假设一个装甲板匀速运动的模型，在该假设下可以得到对于x轴的运动状态$[x,vx]^T$，但是我们凭空假设得出来的$vx$肯定是不准确的，我们需要对$vx$进行修正。这就建模成了一个最优化问题，如何通过一系列观测值和预测值最优化$vx$。我们先提出最简单的线性卡尔曼kalman滤波，它通过当前的状态$[x,vx]^T$，在匀速运动模型下预测$\Delta t$后的运动状态，在观测到实际$\Delta t$后的状态根据观测值更新$vx$，理论上最后达到收敛。kalman滤波的关键参数K（卡尔曼增益）描述了你更相信预测值还是观测值，因为噪声的存在（比如识别误差，PNP解算抖动等等），观测值不一定很准。观测噪声大，K变小，滤波器更相信匀速运动模型下的预测；观测很准，K变大，滤波器更相信实际观测。

理论上这样的流程就能够得出装甲板的运动状态，但是实际上这样的算法仅仅只能跟踪低速运动目标，对于高速小陀螺的装甲板几乎无法跟踪。我们可以看看目前的算法有哪些问题：一是观测噪声较大，装甲板位姿的解算完全依赖PNP，PNP抖动就会造成巨大噪声；二是噪声的物理描述使用XYZ笛卡尔坐标系，而噪声的主要来源是yaw和pitch的角度误差以及深度估计误差，使用YPD坐标系描述更合适；三是当前仅做到对单一装甲板的跟踪，不适合高速小陀螺下快速切板的场景；四是线型的KF不符合高速小陀螺下装甲板的非线性圆周运动场景。

我们一步一步来，滤波器换选ESEKF做非线性优化，将对单装甲板的4dof观测改为13dof整车观测，PCA角点精修+uv观测减小噪声（在像素坐标系下肯定是用UVL来描述噪声，如果仍然在相机坐标系下噪声描述改为更适合的YPD），增加灯条观测获取更多约束信息。

EKF（拓展卡尔曼滤波）是在线性KF的基础上改进，在当前估计值附近对非线性函数做一阶泰勒展开，将非线性系统局部近似为线性系统，然后套用 KF。在EKF下，我们主要要设置调节的参数有$x_{0}$、$P_{0}$、$Q$（过程噪声协方差）、$R$（观测噪声协方差）。$x_{0}$为滤波开始时的状态估计值，$P_{0}$为初始估计的不确定性，影响收敛速度，$Q$表示模型的不确定性，越大表示越不信任模型预测，$R$表示传感器噪声，越大表示越不信任观测。我们主要调节$Q$和$R$，它们极大影响滤波性能，也因此我们需要对噪声的准确描述。

ESEKF（迭代扩展卡尔曼滤波）是在更新步骤中多次迭代——每次用更新后的状态重新计算观测矩阵 H，再做一次更新。收敛后（通常 2-5 次迭代），相当于在真值附近做线性化——比只在预测值附近展开一次要精确得多。

13dof整车观测（cx, cy, cz，vcx, vcy, vcz，rot_x, rot_y, rot_z，vyaw，log_r1， log_r2, h）多维度约束。cx, cy, cz表示车体中心的位置；vcx, vcy, vcz表示车体中心速度；rot_x, rot_y, rot_z表示车体姿态的 SO(3) 旋转向量；vyaw表示yaw角速度；log_r1表示偶数装甲板到车体中心的半径对数；log_r2表示奇数装甲板到车体中心的半径对数；h表示装甲板高度差（可有效观测装甲板高度不一、车辆结构不对称的车辆）

在PCA对灯条角点进行精修后，为了减小PNP这一噪声来源，我们把原来的2D图像 -> PnP位姿 -> 3D下对位姿滤波，改为PNP初始化整车状态 -> 预测装甲板/灯条图像位置 ->真实图像观测 -> 计算重投影残差 -> 更新整车状态，直接在uv观测进行优化，PNP只会在初始化时使用。同时我们增加对单个灯条的观测，不仅观测当前最正对相机的装甲板左右灯条，还有左右相邻装甲板可能露出的灯条来增加约束。

这样我们就有效地减小噪声，充分利用可观测的信息，得到较好的整车状态估计，并稳定地跟踪装甲板。

tracker这一部分主要就是进行后端优化，我也顺便讲讲整体上的优化处理选择。在后端优化中，我们通常考虑一段更长时间内的状态估计问题，而且不仅使用过去的信息更新自己的状态，也会用未来的信息来更新，这种处理方法叫做批量。否则，如果当前的状态只由过去的时刻决定，甚至只由前一个时可决定，则叫做渐进。

我们如果假设存在马尔可夫性，一阶马氏性认为k时刻状态只与k-1时刻状态有关，而与之前的无关，那我们就得到以EKF为代表的滤波器方法；如果仍然考虑k时刻状态与之前所有状态的关系，将得到非线性优化为主的优化框架（因子图图优化）。

滤波器方法计算量较为固定，计算速度快，但没有考虑到过去的观测信息，一般认为优化效果不如图优化；图优化方法在全局（相对较长时间）上利用观测信息，但是计算量不固定且计算量大，不太适合实时计算（虽然通过滑动窗口法等一堆优化可以达到实时计算）。在视觉SLAM中当今的主流方案是使用图优化，但是在RM场景下对自瞄实时性要求较高，因此普遍采用滤波器（但我也看到吉大佬做出了基于因子图的自瞄🥰）

planner#

现在我们成功获得了整车的运动状态估计，可以预测未来某个时间点装甲板的位置，现在我们需要做的是如何让云台成功地响应偏转至指定位姿。很显然我们不能够直接把yaw和pitch发给云台，云台是做不到瞬间响应的，它不能瞬间摆动到任意角度，云台存在着最大角速度和最大角加速度，而且受到库伦摩擦（恒定阻力，与速度方向相反）、粘滞阻尼（与速度成正比的阻力）和转动惯量（阻止角加速度的惯性力）的共同作用。所以我们需要在物理约束之下，规划出一条云台可以实际响应的平滑轨迹，同时保证精度。

首先我们需要知道应该瞄准哪里，在车辆各种的运动状态下，低速选择单板瞄准，高速选择双板瞄准，超高速直接瞄准车辆中心。然后我们需要估算飞行时间，把它加入延迟考虑之中。根据当前装甲板的位置和弹道模型估计飞行时间，然后按飞行时间预测车辆位姿，然后再重新解算，逐步迭代飞行时间直至收敛。这样我们把装甲板位姿转换为（yaw，pitch，aim_point，aim_id），并加上yaw和pitch的偏置。

我们同时维护两条轨迹，目标轨迹（理论上应该瞄准的位置控制轨迹）、控制轨迹（考虑云台运动能力后实际执行的位置），并观测以当前时刻前后一段时间来进行规划。

规划器的实现有MPC和类MINCO实现。MPC 将 yaw 和 pitch 分别看作两个独立的二阶系统，优化目标是角度跟踪误差+ 角速度跟踪误差+ 角加速度大小，使用小规模ADMM求解器（每次迭代的计算量极小且固定）来在线求解，最后MPC在每个时间点输出yaw 角度、角速度、角加速度和pitch 角度、角速度、角加速度。

类MINCO轻量实现参考MINCO的分段多项式参数化思想，从固定参考点→ 估计节点速度和加速度→ 用五次多项式连接相邻节点→ 检查切板段最大加速度→ 必要时扩大切板过渡时间，从而使轨迹具有位置、速度、加速度连续性。

在最后的火控阶段，我们会对比当前控制轨迹与当前目标轨迹进行开火判断，并在允许的开火误差延迟区间内持续检查控制轨迹，如果偏离目标就静止开火。

整体上的自瞄架构就介绍完毕了，当然里面还有许多算法有值得优化的地方，工程设计上也有值得优化的地方。我推荐大家在看完后去继续学习《了解CV和RoboMaster视觉组》（视觉圣经😍）：https://pan.baidu.com/s/1GakDJP3pDnNib364TDAEBg?pwd=0416 和上科大十等星佬的自瞄教程：https://fcn47qghdcqf.feishu.cn/wiki/Hcw1wxTMZicx0xkinuQcKHetn5d?from=from_copylink 。这里面都大体介绍了自瞄的实现，并且详细地介绍了原理。