在RM战队待了快三年，从小登到老登，学了不少关于智能机器人的技术。现在的RM比赛技术大爆炸，不少团队的自瞄都能达到很精准的水平。写下这个“过时的自瞄”系列，算是给自己一个交代吧。
机器人自瞄系统的整体设计
自瞄的整体流程如上，首先相机获取图像后经过预处理，然后送入检测模型，得到装甲板灯条交点的2D图像坐标，接着通过PnP算法计算出3D空间坐标，接着通过坐标系转换到世界坐标系，在世界坐标系下对目标进行运动状态的估计，最后进行运动状态的预测和弹道补偿，最后将xyz坐标系转换到pitch，yaw，distance，发送给下位机控制云台进行瞄准。
相机选型
在RoboMaster比赛中，识别的速度尽量越快越好，即摄像头帧率尽量越高越好。而通常这些摄像头的帧率一方面受摄像头自身硬件属性影响例如做工、型号、快门类型等等。一方面受计算平台处理能力因素（需要注意的是你的编译运行方式也可以影响，例如VSxxxx DEBUG模式与RELEASE模式），另一方面则受分辨率的增加而减少（带宽限制）
我们采用的是工业相机：大恒水星二代的相机与镜头的组合。CMOS相机，彩色摄像头。具体焦距不同机器人用的不一样，步兵使用的是6-8mm的镜头，英雄使用的是8-12mm的镜头，无人机使用的是12-16mm的镜头。焦距越大，视野越小，识别距离越远。焦距越小，视野越大，识别距离越近。
NUC选型
在RoboMaster比赛中，计算平台的选择也是非常重要的。计算平台的性能直接影响到图像处理的速度和精度。我们使用的是Intel NUC13（部分机器人用的是NUC11，NUC12），搭载Intel i5-1340P处理器。实则跑我们的识别模型处理速度在10ms以内，完全够用。
灯条角点识别
关于灯条交点的识别上赛季做了很多迭代，最终还是用了改进的YOLO-X算法。通过将YOLO-X的检测头改为四个角点的坐标进行回归训练。
模型输出：机器人的颜色+角点坐标
在绝大多数情况下，模型的性能是足够的，检测足够精准，足够鲁棒。
为什么没有直接输出机器人的类别呢？？？？？？？？？？
为什么没有直接输出机器人的类别呢？？？？？？？？？？
为什么没有直接输出机器人的类别呢？？？？？？？？？？
重要的事情说三遍，受限于数据集的不平衡和模型的性能，多加个类别损失模型收敛会更慢，并且分类效果也一般。于是决定只判断颜色，后面用一层轻量化的神经网络输出类别。
为了能够获得更多装甲板的数据集，我们还基于Unity做了数据集的仿真生成，生成了大概2W张图像数据，极大地丰富了数据集。
数字分类
之前采用的数字分类是Opencv内置的SVM分类器，效果还不错，但是鲁棒性不够好：比如对前哨战、基地、哨兵的分类不够理想。遂自己搭了个轻量化的网络进行数字分类，训练过程中进行了魔鬼式的数据增强，最后对各类的分类效果都还不错。
首先将灯条的四个角点进行透视变换，得到一个固定大小的图像(64×64），然后送入分类网络进行分类。
PnP解算
通过机器人装甲板检测，获取了敌方机器人装甲板在图像坐标系下的像素坐标，通过已知的装甲板的实际尺寸，结合相机的内参，可以通过PnP算法计算出装甲板在相机坐标系下的空间坐标。
相机成像原理
相机成像原理是基于凸透镜成像。如图，工业相机主要由两部分组成，分别为镜头和相机本体，镜头由多个镜片组成，多个镜片可以等效为一个凸透镜，相机本体主要为CMOS传感器，负责感受光信号。相机的成像本质就是镜头将物体发出的光聚集在相机本体的CMOS传感器上，CMOS传感器通过把光信号转化为电信号，最终转化为数字信号，变成我们所熟知的图片。具体成像原理如下。
实际物体一点会向等效凸透镜发出光线，其中穿过凸透镜光心点的光线经过凸透镜光心不会发生折射。平行射入凸透镜的光线会发生折射，所有通过凸透镜射向凸透镜的光线，透过凸透镜会交于一点。在相机对焦较好的情况下，这一点就是相机本体感光面，即CMOS传感器所在的位置。由于相机成像多为倒像，这不利于进行图像的分析，所以我们可以把成像面的像通过一个等效三角形，把三维世界的物体在相机光心点后侧的倒像，等效为在光心点前侧的正像进行图像的分析。
相机模型中各坐标系之间的变换
根据相机的成像原理，可以分析出三维坐标系下的物体的空间坐标与图像的像素坐标系下的像素坐标之间的变换关系。在阐述以上关系之前先定义以下四个坐标系。
- 世界坐标系：空间中某一点为原点，以一个特定方位构建一个三维的笛卡尔坐标系。
- 相机空间坐标系：以相机光心为原点，以相机正前方向为Z轴，X和Y轴平行且同方向于图像像素坐标系的X和Y轴的三维坐标系。
- 相机图像坐标系：以相机成像面中心为原点，X和Y轴平行且同方向于图像像素坐标系的X和Y轴的二维图像坐标系。
- 像素坐标系：以相机成像面右上角为原点，X轴为从左向右为正方向，Y轴为从上到下为正方向。
通过使用旋转和平移等欧式变换的方法，把在世界坐标系下的点转化为相机空间坐标系下的点，这两个坐标系之间的变换关系由相机的外参矩阵进行描述
其中为相机的内参矩阵，描述相机空间坐标系到像素坐标系的变换关系，而矩阵为相机的外参数矩阵描述世界坐标系与相机空间坐标系的变换关系。想要通过一个相机获取目标在相机空间坐标系下的位置，本质上就是计算相机的外参数矩阵的平移矩阵。
PnP算法
PNP算法本质上就是在已知相机内存矩阵的条件下，通过多组对应的像素坐标系下的坐标点与真实世界坐标系下的坐标点，推导或者拟合出两者之间的关系，由于相机内参矩阵已知，那么可以通过用相机的内参数矩阵对求解或者拟合的结果求逆变换得到相机的外参数矩阵，从而得到目标在相机空间坐标系下的坐标。
在实际装甲板定位中，由于之间在装甲板检测中，获取了装甲板上两根灯条A，B，C，D点的像素坐标，由于装甲板大小恒定，可以通过机械测量，计算出以装甲板的中心位置为原点，坐标轴下的，A，B，C，D四点物理坐标，通过上述PNP方法，求解相机的外参数矩阵，以此获得在相机空间坐标系下装甲板中心的空间坐标。
通过PNP算法获取了在相机空间坐标系下的识别对象的三维空间坐标，在此基础上将目标在相机空间坐标系下的坐标变换到以机器人云台转轴为原点的惯性坐标系下：
- 相机空间坐标系：该坐标系上文所定义的相机空间坐标系定义相同。
- 云台坐标系：该坐标系原点位于机器人云台转轴交点处，坐标轴方向为以机器人云台pitch从坐标系原点到pitch轴发射原点为x轴正方向，从原点指向上侧为z轴正方向的一个笛卡尔坐标系。
- 惯性坐标系：该坐标系原点与机器人云台坐标系原点相同，坐标系方向与世界坐标系方向相同。 在实现坐标变换时，首先通过平移变换将相机空间坐标系下的坐标变换到云台坐标系，然后利用机器人云台的IMU数据将云台坐标系下的坐标进行旋转变换变换到惯性坐标系下。