CVRM 2023

东莞理工学院 RoboMaster 战队 2023 赛季视觉代码框架。

Author: 彭淐

[TOC]

0、前言

  • 本赛季的视觉程序在研发时，充分考虑到实验室的实际经费和其他资源可支配情况下，制定了“整体框架不变，小步快跑迭代”的规则
  • 在 2022 赛季末，从底层通讯到感知算法彻底重构了步兵机器人的算法部分代码，其改进优势着重表现在更加合理的程序业务流程、更好的程序鲁棒性、复杂环境下更优秀的目标检测效果和全新的滤波器，并且添加了易于追溯程序问题的日志系统。同时为了降低不同兵种之间的算法部署难度，添加了配置文件读取功能，不同兵种之间通用一套代码，显著降低了在代码维护方面的人力资源成本。
  • 本篇旨在总结工作、交流技术，由于笔者水平有限，文中难免存在一些不足和错误之处，诚请各位批评指正。受限于时间原因，部分内容将以后进行补充。如果你想要系统地学习相关内容，请阅读相关书籍。

1、部署

运算 设备 Intel NUC 10 i7FNK、DJI Manifold 2
相机 大恒MER-139-210U3、MindVision MV-SUA134GC-T 均搭配 6mm 的镜头
相机位置 跟随云台，依附在枪管下方，倒置
操作系统 Ubuntu 18.04 LTS
开发环境 Qt Creator 5.12.0、支持C++11以上的C++编译器（推荐g++ 7.4）
采集帧率 150fps
分辨率 默认640x480（更改需重新标定相机）
串口 USB 转 TTL 串口，波特率为115200
陀螺仪板 自研陀螺仪、官方C型开发板

部署前需要检查并更改

  • 相机内参与畸变参数
  • 相机与IMU间的变换矩阵 $T_{ci}$ 和 $T_{ic}$
  • $Yaw$ 和 $Pitch$ 固定补偿量
  • 相机至枪管的 $x、y、z$ 轴偏移量。通常将x轴平移量设置为0，y轴向前，z轴向天空。y,z轴的值应当让机械在3维图纸上测，至于值的正负，可以改变一下符号，看最终的坐标输出自行判断。
  • 图像输入源

第三方库

  • fmt 安装方式：编译安装

  • Ceres-Solve计算库 安装方式：编译安装

  • Eigen 安装方式：sudo apt-get install libeigen3-dev

  • OpenCV4.4.0 安装方式：编译安装

  • OpenVINO 2021.4（针对四点模型，只适用于Intel，AMD无法使用openvino的推理框架，因为有一些算子不被处理器或指令集支持

    ）：官方文档

  • gcc/g++：升级方式：添加 ppa 升级

常见问题1

Cmake提示找不到Eigen3的方案

1
sudo ln -s /usr/include/eigen3/Eigen /usr/include/Eigen

工业相机

  • 启动需要5s左右，等待指示灯为绿色时即可正常使用

  • 发热情况较为严重，时间过久温度过高可能会触发保护机制造成相机离线。此外，使用非官方USB线也可能会导致相机离线

  • 前往官网找到linux版本的相机驱动（USB3.0），根据官方文档进行安装迈德威视相机驱动
    大恒相机驱动

  • 根据实际环境调整曝光范围、Gain增益范围、Gamma增益范围、白平衡

  • 由于使用了数字识别，务必保证光照充足，图像上数字清晰可见。光照不足时，调整摄像头曝光或增益数值，直到数字清晰可见。务必保证摄像头焦距正确，同时镜片干净无污物

  • 认真检查相机焦距和光圈是否已经固定住，通常将光圈调整到最大，然后将焦距调整到能清晰看到2- 6m左右距离的装甲板数字(使用GalaxyView)。(超过这个范围，就算识别到，PNP解算误差也很大，而且操作手也不会打弹丸，所以不用过多纠结识别距离)。

  • 配置:

    1
    2
    3
    LIBS += /usr/lib/libgxiapi.so \
            /usr/lib/libdximageproc.so \
            -lMVSDK

编译调试

  1. 创建build编译文件夹，与ACERMVision.pro在同一路径下
  2. 用Qt Creator打开ACERMVision/ACERMVision.pro
  3. 将构建目录导向到build，解决方案配置使用Release，若出现异常（如无法打开工业相机）再改为Debug

输入源设置

考虑到真实比赛环境及调试需求，可以选择不同的图像输入源进行操作，目前支持以下的图像输入源：

  • 视频文件：使用视频或文件作为输入源。将source设置为文件路径即可，例如："/home/ace/video/1.mp4"
  • 普通USB相机：使用USB相机作为输入源。将source设置为openCV中设备编号即可，例如："/dev/video0"
  • 工业摄像机：使用工业摄像机作为输入源，目前，实现了对大恒工业相机和迈德威视相机的支持

开机自启

在赛场上，只有三分钟准备时间，因此需要让程序在 PC 开机之后自启动，这就需要写一个自启动脚本，并且，为了防止程序异常中断，使用看门狗来保护程序，原理就是不断检测程序是否在运行，若没有运行，则立刻运行，如果在运行，就隔一段时间再去检测。同样，只需要修改一下路径就可以运行。（如果在 PC 意外断电的时候，会有几率将二进制文件给损坏，导致程序无法运行，因此为了保险起见，在开机时重新 make 一下程序），为了防止极端情况，在程序意外中断 10 次之后直接重启 PC （一般不会遇到这种情况）。以下为示例脚本：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
!/bin/bash
sec=1
cnt=0
PROC_NAME=ACE //进程名字，取决于你自己make后的名称，也可以在终端输入top来查找
Thread=`ps -ef | grep $PROC_NAME | grep -v "grep"` //判断用到，具体用法自行百度
cd /home/ace/ACERMVision/build/  //进入文件里面
make clean && make -j        //清除并重新make一下，防止文件损坏（-j提高效率）
echo 123456 | sudo -S sudo chmod +777 /dev/ttyUSB0 //用自动输入密码并开启ttyusb权限
./main //运行

while [ 1 ] //循环，记得大括号里面的1两边都要空格
do
count=`ps -ef | grep $PROC_NAME | grep -v "grep" | wc -l` //判定进程运行否，是则继续，否则重新启动
echo "Thread count: $count"
if [ $count -gt 1 ];then  //-gt 大于1情况下 进程没被杀害
	echo "The $PROC_NAME is still alive!"
	sleep $sec 
else  //进程被杀害
	echo "Starting $PROC_NAME"
	cd ~
	echo 123456 | sudo -S sudo chmod +777 /dev/ttyUSB0 //12345678是我minipc上的密码
	echo 123456 | sudo -S sudo chmod +777 /dev/ttyUSB1
	cd /home/ace/ACERMVision/build/  //移动到你要编译运行的文件夹里面
	./main	
	echo "$PROC_NAME has started!"
		sleep $sec
fi  
done

只需将 watchDog.sh 添加到 ubuntu 系统的 StartUp Applications 中就可以实现开机自启。如果重启之后自启动失败，可考虑是否是未赋予该脚本权限所导致的。

此外，所有的调试信息输出都将在一定程度上影响算法的实时性，因此在发布终版程序时，尽量关闭所有调试选项。

2、流程图

机器人功能解耦

开放系统互连参考模型 (Open System Interconnect, OSI）是国际标准化组织(ISO)和国际电报电话咨询委员会(CCITT)联合制定的开放系统互连参考模型，为开放式互连信息系统提供了一种功能结构的框架。它从低到高分别是：物理层、数据链路层、网络层、传输层、会话层、表示层和应用层。OSI将计算机网络中各个模块进行解耦并实现各个层级之间的各种协议。

参考OSI的设计模式，可以将机器人系统解耦为五个部分：

主要实现的是上位机处理的会话层、决策层及数据融合与处理层。

软硬件架构

本赛季的视觉算法程序结构设计是以功能模块化和方便多人协作的思路来进行设计，实现低耦合高内聚设计要求，极其注重代码复用性，将代码模块化并参数化，基本上参照了单例模式进行设计。

整体分为四个板块：配置参数、硬件驱动、功能实现、工具包。

  • 配置参数板块主要包含了各个功能模块所需的参数配置xml文件，初始化、条件判断、相机标定参数都储存于此，相较于以前参数位于代码头文件内，减少了因编译所花费的时间，提高了调试效率。
  • 硬件驱动板块主要包含了工业相机的驱动程序、串口收发协议等外设驱动，属于驱动层的内容，内容配置完毕后不需再进行重复修改，解耦的同时也减少了重复造轮子的所花费的时间。
  • 功能实现板块主要包含了装甲板识别和击打、能量机关识别和击打、弹道补偿算法等各种识别功能和控制算法，内容由多个设计者分别完成并汇总于此，要求模块独立设计，方便后期调试和维护，可以使功能快速实现并测试，进行快速的迭代。
  • 工具包板块主要包含了帧率测试包和其他第三方调用工具包等，对整体程序提供相应的测试需要和功能拓展。

对于不同机器人只需要根据配置文件加载不同数量和种类的数据。做到了一套代码给所有机器人同时使用，如：在开发好步兵的基础模块后，开发双云台哨兵时，只需要编写配置.xml文件 ，除了决策部分的开发，不需要书写或修改任何一行代码。

各个模块之间通过公共的数据结构进行信息交互，所以在出现问题时只需要定位到相应的模块，对其里面的内容进行修改即可，不会影响到其他模块的内容。并且，在各个模块的判断处都设置了相应的状态提示信息于终端进行显示，可以根据终端输出的信息判断问题出现的位置，快速排查问题。

消息管理器

基于双向队列设计消息管理器控制数据的发布和订阅，对标准库进行二次封装，提供简单的多线程同步功能。即不同功能之间的通信是通过发布者-订阅者的形式进行的。订阅者和发布者之间相互不关心实现方式。可以协调不同速度的线程的同步关系，又可以采取多线程加速，提高代码并行度。

由于IMU和下位机发送的频率远高于相机采集和算法处理，因此一般为它们预留大小至少为相机图像buffer 5~10倍的数据队列，以方便进行时间戳对齐。如果算法时间开销大于相机采集速度，应该增大共享数据队列的容量保证其能够进行同时读写，用流水线处理掩盖算法多出来的latency。

这里选择使用三个buffer，实际上如果算法处理速度较慢，可以在算法的内部子环节中插入额外的FIFO buffer以掩盖延迟的时间开销。从下位机处的数据接收可以采用轮询或者中断的方式，根据需要自行选择。

对于线程的数据交互，利用共同的时间轴确定内部数据的时间顺序，消费者线程始终读取FIFO的头部，生产者线程不断向尾部填充，buffer size大于1的时候不会出现读写冲突。buffer size等于1的时候为数据添加互斥锁，为了提高效率防止阻塞，在消费者拿走数据后立刻解锁，让生产者能够将数据放入buffer。

命名共享对象

针对多线程编程的几个痛点：

  • 共享数据直接使用全局变量：代码不具有良好的模块化结构，文件依赖关系繁杂。
  • 共享数据使用堆上动态分配对象：shared_ptr在各个函数中层层传递，代码结构容易混乱。

使用命名共享对象的方式，省去shared_ptr的层层传递。命名共享对象，通过对象类型和对象名称唯一确定一个共享对象，通过字符串哈希来寻找对应的共享对象。

各线程流程

每个线程都有自己的分工。由主线程创建所有子线程并维护。

  • 数据接收线程：接收下位机发送的机器人状态数据和操作手的模式切换指令，并将接收到的机器人状态数据存入缓冲区，供图像生产线程使用。同步记录时间，每次在收到数据的同时将时间信息传入，在允许的误差范围内同步相机帧和云台位姿。
  • 数据发送线程：读取任务线程缓冲区的待发送数据，并将其发送至下位机。并实时监控串口设备工作状态，需 要时对串口设备复位
  • 图像生产线程 ：从图像传感器中采集并生成 RGB 图像数据；从缓冲区内读取数据接受线程的机器人状态数据，与当前图像数据一同打包存入缓冲区供主线程使用
  • 图像消费线程 ：从缓冲区中读取图像生产线程打包的目前图像与机器人状态数据；根据目前的机器人状态数据选择进入自瞄模式或者能量机关模式，得到装甲端点的像素坐标以及是否连续识别相同目标等基本信息，将基本信息传入角度解算函数通过PNP姿态估计得到目标在相机坐标系下的坐标，然后再根据当前目标状态（第一次发现该目标、连续识别相同目标和掉帧缓冲）执行不同的应对方案，待相应模式主函数执行完毕后将处理数据打包压入缓冲区，送交数据发送线程

装甲板自瞄算法流程

能量机关算法流程

  • 设计提前退出的选项可以大大减少遍历次数，利用 continue 让整体条件筛选的结构更加清晰，方便排查哪个条件出现问题，同时避免出现大量 if 和 {} 的冗余结构
  • 对条件进行了一定的剔除或变更为比例关系，即形态不变特征，防止赛场上出现炸程序的现象

在处理形态学特征的时候，可能希望寻找一些由不同区块的轮廓组成的目标，在比赛中常见的即装甲板灯条的匹配，其在正常曝光下从远处看是两个对称的梯形，一般将其视作一个轮廓进行处理。当希望匹配处于同一个物体上的两个或多个形态特征（对于装甲板识别就是找到处于同一块装甲板上的灯条），应该寻找对旋转/尺度/平移都有不变性的特征。

3-1、装甲板识别方案（传统特征提取）

ROI设置

在图像处理领域，感兴趣区域(ROI) 是从图像中选择的一个图像区域，这个区域是你的图像分析所关注的重点。圈定该区域以便进行进一步处理。

不难发现，如果在算法循环中对全图进行计算，将会使计算资源浪费在非有效装甲板区域，进而拖慢整个进程的计算效率。为了优化算法，设置了ROI跟踪算法，即在上一个有效目标的周围区域进行装甲板检测。但是在整个实际对抗环境中， 无论是装甲板在视野中消失，还是装甲板被击中时灯条闪烁都会造成该帧图像里无法识别装甲板，但两种情况下的ROI有效性是不同的。所以为区分两种情况，设置一个丢失阈值。当无法识别有效装甲板的次数超过某个阈值时认为该目标消失。

此外，在一些预处理操作之前，可以将图像进行分割并分配合适个数的线程，以实现多行并行二值化和一些形态学处理等操作，最后再合并图像，以提升处理速度。

图像预处理

图像预处理算法通常可以分为以下四种类型

  1. 使用灰度图进行二值化，最后对轮廓内的像素判断颜色。
  2. 使用敌方颜色的通道减去己方颜色通道(或者绿通道)。
  3. 以上两者混合，即使用 (灰度图 & 通道相减图) 然后再二值化。
  4. 使用HSV进行通道二值化分割。

最后预处理的方案选择了第一种，理由是使用灰度图能够避免灯条断开的问题(如果在较高曝光或者较高增益的情况下，灯条中心像素值较高，使灯条中心发白，如果使用通道相减或者HSV方法，可能会导致二值化后的图形断开)，虽然仅仅使用灰度图使后面对每个轮廓的颜色校验增加了一定的计算量，而且也会导致筛选出来的灯条变多。

**为了让便于分离灯条与其他光线，一般将曝光设置得很低。如下图所示，**由于一般工业相机的动态范围不够大，导致若要能够清晰分辨装甲板的数字，得到的相机图像中灯条中心就会过曝，灯条中心的像素点的值往往都是 R=B。根据颜色信息来进行二值化效果不佳，并且将图像变换到 HSV 或 HLS 颜色空间都会比变换到灰度图耗时大，因此选择直接通过灰度图进行二值化，单独取数字ROI区域进行图像增强。必要情况搭配滤光片辅助。
原图 通过颜色二值化 通过灰度二值化
  1. 考虑到发光灯条亮度显著高于室内环境光，判断灯条颜色这里采用了对轮廓内的的R/B值求和，判断两和的的大小的方法，若 sum_r > sum_b 则认为是红色灯条，反之则认为是蓝色灯条
  2. 对灰度图进行二值化，判断灰度灯条轮廓的其中一个点是否在RGB颜色光圈的轮廓内利用pointPolygonTest函数
  3. 这个方法可以将绝大多数不发光的物体排除在外，在后面处理轮廓和轮廓筛选环节能节省较多时间。
  4. 如果颜色阈值分离和相机参数（当然和相机本身的性能有关）配合得很好，在很多时候可以省去形态学操作，大大降低时间开销，毕竟形态学操作和滤波一样是逐像素的算子。

自适应阈值

二值化的关键在于阈值，我们采用了基于正片叠底+ 迭代法的自适应阈值，将原图像素 bgr 三通道的权重转化为 0.1、0.2、0.7，接着找出图像最大灰度。最终值：最大灰度 x 0.6+平均灰度 x 0.4，就是二值化的阈值。

拟合灯条

  1. 找出目标轮廓主要利用了 findContours 函数中的 contours 。首先定义了一个容器 std::vector<std::vector<cv::Point>> contours循环遍历contours[i]筛选出满足几何条件（轮廓周长、轮廓高宽比和旋转矩形角度等）、颜色条件和点集数大于5的轮廓。可以高效的筛除形状不满足的亮斑
  2. 装甲板距离相机的距离会导致灯条长度的改变，但是不论距离远近，处于同一块装甲板上的灯条的长度比始终应当保持不变，这就是一个尺度不变特征，显然灯条的宽度比也同样是一个很好的衡量指标。其他特征同理。
  3. 之后将找到的轮廓用旋转矩形标识出来利用的是椭圆拟合矩形函数 fitEllipse 。原因是椭圆拟合出来的角度更加准确，且表示的角度范围在 -180°~180° 之间，可以减少额外的转换运算。利用椭圆拟合矩形的角度，灯条的角度限定在-30°~30° 之间
  4. 将符合条件的灯条轮廓进行位置排序

拟合装甲板

通过两两灯条匹配，根据几何条件（倾斜弧度、左右灯条高宽比、灯条高度差、灯条角度差和装甲板长宽比等）筛选出装甲板并保存到armor_中。再通过平均颜色强度，排除装甲板中心较亮的装甲板。

以上得到的装甲板符合装甲板的基本特征。装甲集合进行二次筛选去除粘连装甲，如下图所示其中序号2为正确装甲而序号1和3为错误装甲，二次筛选利用共用灯条和装甲板数字等信息去除错误装甲。具体几何特征为：

  • 两灯条角度差
  • 两灯条连接端点后应接近矩形而非平行四边形
  • 两灯条间距应在一定范围内
  • 两灯条长度比应在一定范围内

数字识别

数字图案实质上就是黑色背景+白色图案。通过计算得到数字识别的区域，然后对该区域进行数字识别。数字识别使用机器学习，用SVM进行分类，得到装甲板ID，筛掉没有数字的和工程ID，得到目标装甲板。

伽马校正

首先，对于装甲板中的ROI区域，需要对其进行伽马校正以增强数字区域亮度，之后进行二值化操作，以获得待放入SVM识别的图像数据。伽马校正的计算公式为：
$$
V_{\text {out }}=A V_{i n}^{\gamma}
$$
其中系数A一般取1，$V_{in}$和为$V_{out}$为像素点的灰度值。如下图所示，当$ \gamma < 1$时，在低灰度值区域内，像素值动态范围变大，对比度和图像整体的灰度值变大。当时，$ \gamma > 1$ 时，在低灰度值区域内，像素值动态范围变小，对比度和图像整体的灰度值变小。由于装甲板图片在低曝光下进行采集，需要令$ \gamma < 1$来增加图像亮度。经过不同光照环境下的大量测试，在本程序中取$ \gamma = 0.15$。

gamma值越低，数字越亮，但噪点也会越多,故gamma值需要调节到一个较为合适的值，至少肉眼能够清晰分辨数字。

数字识别区域变换如图所示
原图 透视变换 伽马校正 二值化

有时候希望增大图像的对比图突出边缘和纹路等细节，那可以使用直方图均衡化的技巧，其将一张分布不均匀（常常是有大块明暗区域）的图像的直方图变得更接近均匀分布。这能够大大提升图像的对比度，使得明暗的差异增大，更好地分离需要的部分。

稳定的数字识别是基础，这样就可以做优先级打击序列，以及针对特定目标的打击。

多层感知机（MLP）

网络结构中定义了两个隐藏层和一个分类层，将二值化后的数字展平成 20x28=560 维的输入，送入网络进行分类。

SVM

LeNet

装甲板优先级

通过对装甲板到图像中心点的距离进行排序，选择离图像中心点最近的装甲板。便于操作手进行选择装甲板击打。

为了防止画面中出现多个装甲板时，最优装甲板位置判断反复横跳。当自瞄在当前帧检测到某一个装甲板为最优目标装甲板，保存该装甲板在图片上的外接矩阵信息，下一帧根据保存的外接矩阵信息截取特定 ROI 传入自瞄。若传入的 ROI 检测不到装甲板，则扩大 ROI 的大小全图搜索，此时将与 ROI 有关的变量全部清零。

有操作手的地面车辆，自瞄优先级一般定义为：

1
2
初始化：离相机光心最近的装甲板 
预测：上一次击打的装甲板 > 同 id 装甲板 > 重置预测器

对于没有操作手的车辆，自瞄优先级更为复杂，定义为：

1
2
初始化：离相机光心最近的装甲板
预测：优先级 id 逆转 = 斩杀线 > 上一次击打的装甲板 > 同 id 装甲板 > 重置预测器

工程机器人没有攻击能力，且血量高，击打无明显收益，所以加入限制条件自动忽略掉工程机器人。

跟踪器

共有四个状态：

  • NO_FOUND 目标未识别：跟踪器完全丢失目标
  • DETECTING 目标识别中：短暂识别到目标，需要更多帧识别信息才能进入跟踪状态
  • TRACKING 目标跟踪中：跟踪器正常跟踪目标中，通过卡尔曼滤波器预测目标
  • LOST 目标丢失：跟踪器短暂丢失目标

工作流程：

  • 初始化：

    跟踪器默认选择离相机光心最近的目标作为跟踪对象，这种方式比较贴近FPS游戏中辅助瞄准的手感。选择目标后初始化卡尔曼滤波器，初始状态设为当前目标位置。

  • 更新:

    在下文卡尔曼部分说明。

如对方机器人开启小陀螺时，一块装甲板消失的时候另一块装甲板会恰好出现；装甲板被击打后会发生闪烁，于 0.1 s 后亮起；又或是目标装甲板被物体短暂遮挡后再次出现。因此跟踪器的更新应该要考虑目标丢失和目标短暂丢失的情况。一般设置一个阈值，当连续数帧都没有出现的时候删除该跟踪器。

陀螺检测

识别

采用普通的自瞄算法击打处于陀螺状态的敌方机器人，由于高速旋转下枪管识别的方向不断切换，造成枪管一直抖动，击打方向不稳，命中率较低的现象。 因此需要做陀螺检测。

判断敌方是否位于陀螺状态，最直接的方法就是操作手按一个按钮然后反馈给程序，再执行反陀螺策略。但是这个对操作手的要求比较高，本身比赛中就要考虑很多情况，还要按一个按钮，也太不友好了，果断pass掉这种方法。

基于自瞄状态下，可以把陀螺识别模块看作一个系统，这样就可以将该部分的功能分解为两个部分来看，即激励与响应。

激励部分：

当在一定时间内存在较多目标ID时，便触发陀螺状态检测。通过历史数据判断当前跟踪的装甲是否是同一块装甲板。如果装甲板出现切换，则计算切换前后的角度差和移动位置差，进而确定旋转方向。当同一旋转方向的次数达到一定次数，便视作该车辆进行了一次陀螺动作，进行激励。

分数响应部分：

  • 初值为0的情况。此时，为该目标ID的陀螺分数赋初值。
  • 初值与目前值相反的情况。出现该情况说明目标车辆旋转方向改变，或者出现陀螺切换状态的误识别，为目前陀螺旋转分数乘上一个衰减系数。
  • 初值与目前值同号的情况。出现该种情况说明有较大可能出现了陀螺状态，为目前陀螺旋转分数乘上一个增益系数。

假设整个陀螺状态为一阶马尔可夫过程，只取决于上一时刻的分数：

  • 分数低于低阈值且陀螺状态已确定。从分数map中移除该分数，并重置该ID的陀螺状态，取消反陀螺模式。
  • 分数低于低阈值且陀螺状态未确定。进行分数衰减。
  • 分数高于低阈值且陀螺状态已确定。进行分数衰减。若陀螺分数大于高阈值，则强制使分数等于高阈值，避免分数爆炸，进入反陀螺模式。
  • 分数高于低阈值且陀螺状态未确定。进行分数衰减，并确定目标ID的陀螺状态。

击打策略

击打目标原计划采用是根据装甲板中心多帧位置与旋转矩阵，解算陀螺旋转中心与旋转半径并瞄准旋转轨迹圆心上最近的一点，在角度阈值达到一定范围内开火。但这样的方法很难对旋转和平移复合运动的陀螺进行准确击打，因此并未采用。

由于没有时间调参，目前所使用的反陀螺是保底版的反陀螺（命中率约50%）。因为装甲板切换是在陀螺旋转时出现，所以记录下切换前的惯性坐标和切换后的惯性坐标。对记录的前后坐标进行多次储存，最后对储存的坐标取平均值获得新的惯性坐标。基本上会识别为敌方陀螺的中心位置而不再跟随装甲板移动 。只有惯性坐标超过窗口边界值才会使识别的位置发生变化。（让操作手决定开火时机，减少云台晃动以提升操作体验）

3-2、装甲板识别方案（四点模型）

时下RM赛场上的自瞄算法分为两个流派：传统特征提取和神经网络。 基于卷积神经网络的目标检测算法非常依赖电脑的GPU性能。不过，Intel推出的OpenVINO部署平台（仅仅支持Intel的cpu）能够通过优化CPU的运算来提高神经网络的性能。

我们采用了 黑箱 形式的神经网络 YOLO 目标检测系列。仅需要输入原始图像，而不需要经过任何人工的操作，即可由训练好的神经网络自动进行目标检测并标注出位置，实现目标跟踪。该方案至少节省了以下几个步骤：颜色处理，像素处理， ROI筛选，装甲板 确认，灯条拟合等。使用卷积神经网络和目标检测模型，不仅能大幅减少算法开发人员的负担，还能提升识别准确率，为操作手操作提供更准确和有效的帮助。

数据集

  • RoboMaster组委会在2019年开源了一组赛场数据集，同时也提供了YOLO-v3利用该数据集的训练示例。不过官方提供的数据集有一个问题，这些图片都是从赛场直播相机中截取的，大部分是俯视视角（比较适合雷达、哨兵和无人机），并且亮度都较高。川大&沈航则是搭建了一个RMCV视觉开源数据站 ；云南大学&中国科学院深圳先进技术研究院也建立了自己的数据集并开源：RM-DATASET。
  • 常见的数据集规范有COCO和VOC，也有简单的txt(yolo格式)。打开数据集的标注简单浏览，就会看到类别信息、grond truth的中心坐标和宽高这几个数据，每条数据对应一张图片中的一个对象。有些数据集采用的是绝对坐标，有些采用的是归一化到[0,1]区间的相对坐标，它们各有各的优劣。
  • 官方的数据集还有对机器人的标注如工程车、步兵机器人等，如果我们只是识别装甲板可以把这些多余标注利用一个文本处理脚本去除，通过一些简单的正则表达式知识就可以完成。
  • 需要一些序列化数据处理的知识，和对常见api使用的掌握。如COCO使用json来保存标注，VOC使用xml保存标注，YOLO系列则一般用txt文本进行保存。网络上也有大量的不同标注文件转换的脚本。
  • 标注工具：https://github.com/xinyang-go/LabelRoboMaster

数据增强

在装甲板的定位任务中，中间的数字/图案特征起了非常重要的作用（尤其是解耦颜色和分类之后更甚，即使两者没有解耦，数字特征仍然是区分装甲板和其他类灯条的发光物体最重要的部分），而灯条则是区分数字/图案和其他类似物体（特别是场地中的定位标签、大块白色不明物体，都常常被识别成装甲板）。因此，针对这两种情况，推荐加入一些上述的负样本，如场地上的灯条、定位标签、没有贴贴纸的装甲板、熄灭的装甲板（可以作为其他类，颜色中加入熄灭）。

多尺度可以很好地增加多样性，根据数据集中的样本形状选择合适的缩放大小，和前述一样，除有特殊需求外最大尺度不宜超过画幅的1/4；小尺度的目标也要保证再resize后的图片里至少占有共40个以上的像素（COCO把长或宽小于32的目标视为小目标），除非有特殊需求。

旋转增强大概开到20~30，不排除数据集里本来就有一定角度的装甲板（爬坡、视角不同等），开太大容易学到倒的装甲板，甚至把一些场地光效和不知道是什么的玩意识别成装甲板。

伸展和收缩增强应该根据图像预处理的pipeline和相机的分辨率进行设置！这一点非常重要，此参数的设置取决于图像在被输入网络的时候是怎么被resize的。是直接不保留原图比例暴力resize成输入大小，还是保持比例加padding？抑或是保持一边的比例… 伸缩的参数尽量和预处理设置的一致。

另外几个关于像素值的增强（对比度、色调、亮度、饱和度等），也要保持在可能出现的场景范围内。相机调参往往不会特别剧烈，而且至少要看清装甲板图案又不能让灯条过曝，因此这几种增强也要根据工况进行调整。饱和度注意不要调太低不然会变成灰色。尤其是色调，如果变化得太大可能会导致紫色装甲板被识别成红色或蓝色！。

椒盐噪声作为一种常见的噪声类型，在图像中经常出现。同时为图像中添加椒盐噪声，也能模拟装甲板贴纸出现小幅度破损的情况。因此，在Mosaic数据增强与HSV数据增强外，尝试了为图像中随机增添椒盐噪声进行数据增强的方法。 在添加椒盐噪声后，网络分类准确率有了一定的上升，说明添加椒盐噪声的数据增强方法对提高网络分类准确率有一定的帮助。

训练

https://github.com/RangerOnMars/TUP-NN-Train

baseline

YOLOX是截至本文完成前Objection Detection领域anchor-free+one-stage算法的state-of-the-art ，采用解耦检测头，配合 SimOTA 数据增强，在 COCO 数据集上达到了 SOTA 的表现，且其轻量化版本 YOLOX-Nano 在保证推理速度的前提下，也有着不俗的精度表现，这正是所需要的，因此选择了 YOLOX-Nano 作为网络的 baseline。

但因为设备性能限制与检测需求的不同，直接使用原版的 YOLOX-Nano 并不能满足我们的需求，所以需要对网络进行修改。

网络修改

常规深度学习的目标检测算法只能识别出目标的外接矩形，这给后续算法中的单目测距带来了困难。故基于YOLOX-Nano进行修改，并对网络进行了一些改进，以提高网络推理的速度和精度，让网络符合使用实际需求。 网络的输出是：四个点的坐标+class+confidence。 以最小化回归得到的角点和数据集中角点之间距离的MSE（mean square error）为优化目标对loss function进行修改。

  • CoordConv 由 Uber 团队于 2018 年提出，通过显式地为特征图 concat 上位置编码后再送入卷积，从而提升卷积层进行数值回归能力，这一模块对于对位置信息高度敏感的任务有一定的帮助，我们也在网络中添加了这一模块以提升角点回归精度，为网络输入中增加xy坐标特征图，提升装甲板角点的回归精度。

  • Focus 层的概念提出于 YOLOv5，是一种采用切片操作把高分辨率的图片/特征图拆分成多个低分辨率的图片/特征图的下采样操作，但近期有研究指出 Focus 层在移动端设备的表现并不理想，因为其涉及到大规模的 concat 操作，内存开销较大。借鉴 yolov5s6 的修改，将网络的头部的 Focus 层更换为 6*6 卷积，修改后的 Focus 层既保证了较大的感受野，又减小了内存开销，提高了推理速度。

  • ShuffleNetV2 是一款轻量级网络，对移动设备推理进行了特殊优化，并提出了指导轻量级网络设计的四条准则。遵照这几条准则，将 YOLOX-Nano 的 backbone 部分换成了 ShuffleNetV2，并进行了一些其他微调，以加快推理速度。

  • 将检测头原本的reg分支由回归bbox的(xywh)改为装甲板四点(x1y1x2y2x3y3x4y4)，形式上类似关键点检测，并使用WingLoss作为关键点回归损失函数，并进行训练。

  • 为检测头增加color分支，将分类数由“类别*颜色”改为”类别+颜色“。有效降低分类维度，在数据集总量不变的前提下，提高各类样本数，降低网络学习难度。

若进入手动ROI模式，手动缩小送入网络的区域，可以更进一步的提高识别极限距离。装甲板处理优先级同上。

基于CNN的目标检测方法最显著的特点就是泛化性能好，对于复杂的光线条件能够通过扩充相应数据集的方法来解决，也可以加入负样本防止误识别。其部署也不是什么难事，对于新队伍来说更加友好（直接使用官方数据集然后找一个开源的算法也能取得不错的效果）。并且习得特征（训练得到的卷积核和全连接的权重）的表示能力要大大强于人类专家手工设计的特征。在当下的RoboMaster赛场上，神经网络算法已经和传统识别分庭抗礼，之后的事，大家应该也能猜想到结果。

流程

  • 检测近期是否检测到目标，若检测到目标则根据最近目标位置截取 ROI。然后将已经过 ROI 裁剪的图像进行 resize 至 416*416 像素大小，之后送入 CNN 的 InputBlob，进行推理并对推理结果解码得到装甲板的类别和四点坐标

可用的运算平台是无法同时满足大分辨率输入和速度的要求的，当下的最优选择大概是416x416（追求速度）、512x512（权衡）或640x640（精度）。

  • 根据角点坐标拟合旋转矩形并计算 ROI 区域。为防止网络的置信度波动造成的装甲板不稳定识别，装甲板的置信度放行条件我们采用双阈值，大于低阈值而小于高阈值的装甲板需在历史相同位置存在过才放行。 然后根据装甲板类型和长宽比确定大小装甲板

防抖层

这是一个没有技术的算法代码，但其对于模型误识别有很好的消除作用。读懂这段代码对逻辑有一定考验（即比较费脑，还容易忘），并需要对实际作战和需要解决的问题有足够的思考。 反映在代码中都是一些 bool 变量和 if 判断。防抖代码仅是一种保障，应该以模型正确率为主 。

第一层：双阈值

上一次存在过装甲板的图像区域或惯性坐标系中的附近区域的装甲板置信度提升，即阈值降低。

第二层：颜色

装甲板被击打后将会熄灭，因此灰色的装甲板是需要考虑的。因此，放行条件：上一次击打的装甲板的同号灰色装甲板在车辆存在血量时放行。

第三层：误识别

上一次击打的装甲板作为本次击打的最高优先级，不考虑其 id ，而只考虑图像位置和世界坐标位置匹配：避免 id 抖动。

优缺点

传统视觉对于定位四个点在多数情况下比神经网络更精确，但是神经网络的模型鲁棒性更强，可以适应不同的场地和曝光。

此模型的优势在于：

  • 适应不同环境和曝光：在比赛场地几乎不需要调参，心情愉悦，鲁棒性强
  • 能够直接给出装甲板的数字，做车辆区分，便于决策层
  • 能够部分抗遮挡，在装甲板部分不可见，尤其是一个甚至两个灯条不可见时进行识别，增大识别成功率

此模型的劣势在于：

  • 一旦更改标签图案将使整个模型失效
  • 数据集制作困难且样本少，导致过拟合、无识别等问题
  • 速度可能略逊一筹

模型虽然还有优化空间，但配合代码中的防抖层已经完全可用，效果不输于传统模型。

小结

自从2021年上交四点模型横空出世，很多人认为深度学习是RM装甲板识别的最优解。但是，今年哈工程使用传统视觉识别也展现出了惊人的鲁棒性，这值得令我们深思，是传统视 觉不行，还是我们自己本身对传统视觉的理解并不够。深度学习虽然能避免曝光的影响，但是也会使模型不透明，导致出现一些莫名其妙的误识别(比如视觉群大佬们讨论的在前哨站附近的装甲板全部识别为前哨站)。所以我认为，我们既要开发深度学习方向的装甲板识别，也要将这个从RM诞生至此的

传统算法彻底吃透。

4、能量机关方案

能量机关分为大小能量机关，小能量机关可激活时间为比赛开始⼀分钟后至第三分钟（即倒计时 5:59- 4:00），大能量机关可激活时间为比赛开始四分钟后（即倒计时 2:59）。小能量机关激活buff为1.5倍攻击力，⼤能量机关为2倍攻击力和50%防御。

能量机关的扇叶相比装甲板要复杂一些，未击打过的扇叶中间有一个箭头型流动的灯条，末端则是一个闭合的矩形边框。已经激活的装甲板的两侧灯条也会亮起，并且中间的灯条停止流动。到这里，两种状态的区别已经很明显了。能量机关的中心还有一个机甲大师的标志性icon：右下角有个小闪电的”R“。

图像预处理

  1. 在能量机关激活点几乎是正对能量机关的，光线干扰会比较小，有利于预处理的进行。原图 RGB 通道分离相减，得到对应颜色范围的图像，二值化处理，得到颜色二值化图像。具体颜色可以从电控通信兵种信息数据获取到，比如敌方颜色为红色，则能量机关颜色为蓝色
  2. 原图转化为灰度图，二值化处理。将灰度图和颜色图取交集。
  3. 适当的膨胀操作，将流水灯和装甲板区域大致处理成锤子的形状，方便后续进行轮廓筛选处理
  4. 同样的，对输入的图片先进行ROI处理。根据上一帧预置的一个矩形范围进行合理的缩放来对图片进行ROI的截取。提高预处理速度
  5. 由于相机采样本身存在噪声，同时自然环境中也有各种光噪声干扰，因此我们常常对二值化图像进行滤波来减小噪声。 针对扇叶下端的流动条缝隙，使用形态学运算消除。
  6. 流程如图所示

寻找待击打装甲板

能量机关激活点观察能量机关，视觉干扰较小，各种限制条件尽可能放宽松些

  1. 已经激活的扇叶内有3个子轮廓，未激活的扇叶只有1个子轮廓。采用轮廓检测函数findContours()检测图像里的所有轮廓，并建立两个等级关系。findContours函数中hierarchy，定义为vector<Vec4i>hirerarchy，每个元素用以储存一个“用四个整形变量表示的轮廓之间的关系” 的向量。hierarchy[i][0]~hierarchy[i][3]，分别表示第i个轮廓的后一个轮廓，前一个轮廓，父轮廓，内嵌轮廓的索引编号。如果当前轮廓没有对应的后一个轮廓、前一个轮廓、父轮廓、内嵌轮廓的话，则hierarchy[i][0]~hierarchy[i][3]的相应位被设置为默认值-1。

    采用CV_RETR_CCOMP检测所有的轮廓，所有轮廓只建立两个等级关系，外围为顶层，若外围内的内围轮廓还包含了其他的轮廓信息，则内围内的所有轮廓均归属于顶层。

  2. 查找装甲板

    • 将没有父轮廓的轮廓排除（hierarchy[i][3]<0）（拓扑关系约束）
    • 轮廓面积与其最小外接矩形的面积之比（矩形相似性约束）
    • 轮廓面积（面积约束）
    • 轮廓最小外接矩形的长宽比（矩形相似性约束）
    • 矩形区域的亮度

  3. 查找扇叶

    上述步骤得到的装甲板轮廓的父轮廓为候选扇叶，对候选扇叶进行条件筛选。同上

    • 轮廓最小外接矩形的长宽比（矩形相似性约束）
    • 轮廓面积（面积约束）
    • 轮廓面积与其最小外接矩形的面积之比（矩形相似性约束）
    • 矩形区域的亮度

    最终得到每个装甲板对应的扇叶

  4. 将每个装甲板最小包围矩形和其对应的扇叶最小包围矩形组成一个类对象，对每个类对象进行条件筛选。

    • 扇叶矩形面积与装甲板矩形面积的比值
    • 扇叶矩形中心与装甲板矩形中心的距离

    最终得到待击打的扇叶（锤子形）

  5. 装甲板编号说明

    根据大小扇叶的比值关系来进行筛选得到可以用于判断扇叶状态的目标后，对旋转矩形的四个顶点进行排序处理：令半径较大的两个点分别为 0 和 1，令半径较小的两个点为 2 和 3，并以顺时针的方向依次排序，如图所示

最终的排序方式如上如圆圈中所示，这里的排序方法利用的是 RotateRect 类里面的特性，RotateRect 类所标识的矩形顶点编号如上图所示，y 值最大的为 0 点，其余按照顺时针一次编号，若出现两个相同的 y 值，则计 x 值最小的为 0 点。只要判断装甲板的两条长边的中心点跟 rect_big 中心点的距离近，就可以顺利得到装甲板四点的正确的顺序。

6.判断是否激活

  • 方法一：用扇叶轮廓面积与装甲板轮廓面积所得到的比值来判断是否激活
  • 方法二：通过两旁的灯杆。利用装甲板两条宽边向圆心方向延一段比例的距离，建立两个小块 roi 区域，计算 roi 区域的平均值，若 roi 区域内的值满足一定条件，则表明目标矩形是激活的（装甲板旁边有两个杆）否则未激活。

寻找R标圆心

目标筛选之后，得到未打击扇叶的几何中心。而这个几何中心与能量机关旋转中心、扇叶最外层轮廓的几何中点三点在一条直线上，并且具有一定的比例关系。查找圆心是为了计算目标的运行轨迹，通过目标往圆心区域创建搜索区域，经过合理膨胀腐蚀之后的R标成了一团， 通过以下条件进行筛选

  • 轮廓面积
  • 轮廓外接矩形的长宽比
  • 外接矩形中心是否在候选圆心区域

计算预测角度

⼤能量机关的运动预测是该任务中的难点、重点，主要需要对其运动函数的振幅 、 角频率 、 相位进行估计。

计算旋转方向

以圆心为基点向X轴正方向做射线，将此射线作为基准线。然后将获取的目标矩形中心点与R矩形中心点连接成线，计算此线段与基准线的夹角。由于程序运行速度快，帧率高，为了防止采样频率过高从而导致放大了低频噪声，采集三十组待击打装甲板和圆心连线的角度数据，通过取众数法，暂时使用当前的旋转方向进行预测，并存储方向的符号，当这个方向的符号大于错误的符号并且这个数量达到一个值的时候，就可以存储这个方向并沿用到整个过程结束。

计算旋转角速度

未激活扇叶每隔 2.5s 进行切换，所以需要对是否切换叶片进行判断。只需要采集同一片未激活扇叶的旋转数据，只要切换就将相关数据清零。常规方法是使用帧差法，我们假定已经知道两帧或者几帧之间的装甲板和 的相对位置关系又知道采样的时间，我们就可以直接算出来风车的在这一段时间的角速度。
$$
\omega=\frac{1}{T}\left(\theta_{2}-\theta_{1}\right)
$$
由于装甲板定位的噪声（四个点会飘动）以及中心确定的误差，角速度难免有波动，常常可以达到0.5~0.7 rad/s级，需要通过粒子滤波对其测量值进行处理。

粒子滤波

粒子滤波(Particle Filter))的思想基于蒙特卡洛方法(Monte Carlo methods)，它是利用粒子集来表示概率，可以用在任何形式的状态空间模型上。不再追求用高斯分布去近似真实分布，直接用一组点经过模型转换后再采样，用此后验数据来近似任意分布。

  • 算法

将网格所有位置的状态初始化为
$$
\left{x_{0}^{(i)}, w_{0}^{(i)}\right}, w_{0}^{(i)}=\frac{1}{N_{s}}, \forall i \quad x_{0}^{(i)}=[x, y, \dot{x}, \dot{y}]^{T}
$$
对于 t = 1 到22根据权重重新采样粒子2

使用预测模型进行 2 采样22

使用观察模型为每个粒子分配权重2

标准化2

输出估计2

结束

粒子滤波适用于非线性，非高斯系统，且采样数越多，估计越准。预测效果与采样粒子数有关，但粒子数较大时计算量较大。

在非线性系统相比卡尔曼滤波具有较大的优越性。很适合用来对坐标这种非线性的变量进行滤波。 使用Eigen库实现，噪声采用高斯假设，假设各维度噪声相互独立。经过矩阵化计算优化，耗时在 1ms 内，处于可以接受的水平

计算预测角度

即在T秒内，能量机关转过的角度。(T：发弹延时+弹丸滞空时间+云台移动时间等)。需要在连续多帧获取数据后，模拟出旋转的轨迹，计算出对应的提前量，将目标矩形预置到打击位置。

  • 小符匀速运动：

转速固定为 10 $rad/s$ ，$T$秒内转动的角度为 $60° *T$，此即预测角度。

  • 大符变速运动：

大符转速按照三角函数呈周期变化。速度目标函数为
$$
spd = A sin(\omega x + \varphi) + C
$$
单位为 $rad/s$ ，$x$ 的单位为 $s$ ，$A$ 的取值范围为 0.780~1.045，$\omega$ 的取值范围为 1.884~2.000，$C$ 始终满足 $C = 2.090 - A$。每次大能量机关进入可激活状态时，所有参数重置，其中 $x$ 重置为 0，$A$ 和 $\omega$ 重置为取值范围内任意值。能量机关的实际转速与速度目标函数的时间误差在 500ms 内。

代价函数的构建

采取非线性最小二乘法函数拟合的方法进行预测，使用Ceres库进行非线性最小二乘拟合。

常规的写法是需要构造 CostFunction 结构体用来描述代价函数，并且需要重载括号运算符分为两个阶段：
$$
\begin{array}{l}
f_{\text {cost }}=\hat{y}-A \sin (\omega \hat{x}+\varphi)-B \
f_{\text {cost }}=\hat{y}-\hat{A} \sin (\widehat{\omega} \hat{x}+\varphi)-\hat{B}
\end{array}
$$

  • 第一阶段是整个函数的拟合，共有

$$
A,C,\omega,\varphi
$$

四个参数需要拟合。进行非线性最小二乘求解，该阶段由于拟合参数多，耗时较长，帧率会有显著降低，暂未到达可以进行打击的阶段。

  • 第二阶段，该阶段下假设原函数拟合基本正确，相位上可能稍有一定偏差。因此，第二阶段只对相位进行拟合。同时由于只需拟合相位一个参数，拟合残差项数也相应减少，这一阶段会在击打大符时一直持续。根据曲线拟合中 RMSE 指标对拟合结果进行评估，若评估误差过大，超过设定的最大阈值，则依旧沿用之前的参数；反之，则使用此次拟合的参数。之后根据弹速和发弹延迟给定积分上下限，对函数进行积分，计算角度提前量。

拟合成功后，再根据延迟时间（子弹飞行、云台延时等）求解预置角度：

假设初始时间为 $t_{1}$，末尾时间为 $t_{2}$，对速度目标函数进行积分，总共转过的角度差：
$$
\begin{aligned}
\Delta \theta=\int_{t_{1}}^{t_{2}} s p d &=-\frac{A}{\omega}\left[\cos \left(\omega t_{2}+\varphi\right)-\cos \left(\omega t_{1}+\varphi\right)\right]+C\left(t_{2}-t_{1}\right) \
&=\frac{2 A}{\omega}\left[\sin \frac{\omega\left(t_{1}+t_{2}\right)+2 \varphi}{2} \sin \frac{\omega\left(t_{2}-t_{1}\right)}{2}\right]+C\left(t_{2}-t_{1}\right)
\end{aligned}
$$
计算出R矩形中心到目标矩形中心的距离作为构造的直角三角形的斜边。利用三角函数和角度预置量计算出最终的目标矩形中心点。

此外，由于比赛规定的大符参数是在一定范围内取值，可以利用 Ceres::Problem 类中 SetParameterLowerBound 的 SetParameterUpperBound 和两个函数进行限制优化上下限。

拟合结果的验证

对于拟合结果是需要验证的，如果拟合的数据与预期差距较大，是不能直接对相应的结果进行预测的。常用的验证手段是 MAE(平均绝对误差) 和 RMSE(均方根误差)：
$$
\begin{aligned}
M A E &=\frac{1}{m} \sum_{i=1}^{m}\left|h\left(x_{i}\right)-y_{i}\right| \
R M S E &=\sqrt{\frac{1}{m} \sum_{i=1}^{m}\left(h\left(x_{i}\right)-y_{i}\right)^{2}}
\end{aligned}
$$
MAE是一个线性分数，所有的个体差异在平均值上的权重相等，而RMSE会放大对于错误对最终值的影响。而对于该场景中，不能放过对于错误较大的结果进行预

测，所以采用RMSE进行验证。

细节

Ceres的官方文档里详细介绍了非线性优化问题和其工作原理，并编写了相应的Tutorial以供入门使用， 这里不再赘述。确定优化问题，编写损失函数，然后选择优化方式就能开始求解了。

需要注意的问题是，Ceres在求解的时候（尤其是样本点数多，模型又很复杂时），对于CPU的占用往往很高，其内部对多线程调用进行了大量的优化，因此可能会导致其他线程的CPU时下降，使得识别线程等性能下降。因此要合理选择待优化参数，并分阶段解决问题以降低此情况的影响。

实时进行插值补偿，防止空帧所造成的影响。

关于预测量时间 t ，由于我们是位于⼀个相对固定的距离进行击打，且击打⽬标⾯积较⼤，所以使用了⼀个固定的预测时间。但弹道补偿的时间依然是由弹道补偿模型计算得出。

目标丢失归中

由于相机非广角镜头，在自瞄能量机关过程中会出现目标丢失的情况，那么就涉及到决策问题，是选择继续等待目标出现，还是选择自动寻找目标。 对此决策方案如下：

操作手设定云台修正位置。
若短时间内目标丢失，则以未丢失前R标的位置为云台移动方向。
若长时间目标丢失，则通知下位机将云台位置回正到操作手预设位置。

5、Camera-IMU 传感器系统

视觉的输入来源有两个，一个是工业相机，读入的是图片和时间戳，图片初始大小 640x480 ，时间戳单位 ms ，另一个是 C板的陀螺仪。陀螺仪是个姿态传感器，它一般固定在云台上某个远离振动源的位置。陀螺仪可以反馈当前云台的角度数据（Yaw轴、Pitch轴），也可以反馈三轴角速度和加速度数据。目前用到的数据是陀螺仪解算出来的四元数。

标定方法

需要将惯性坐标系和相机坐标系进行标定，来确定相机坐标系的转动量。可以参考的外参标定方法如下：

  1. IMU单独标定
  2. 相机单独标定
  3. 相机与IMU之间相对平移量的标定

参考链接：https://tool.52pika.cn/articles/6

下图展示了联合标定的结果，对应的数据通过文件进行了保存。

四元数姿态表示

姿态描述的是机体坐标系 (b 系) 相对惯性坐标系 (n 系) 的旋转关系。常见的描述方法有三种，每种各有其优缺点，包括欧拉角，方向余弦矩阵和四元数。

四元数的定义与复数非常相似，主要区别是复数只有一个虚部，而四元数有三个，故四元数可以写成下面这种形式：
$$
\mathbf q=q_{0}^{}+q_{1}^{} i+q_{2}^{} j+q_{3}^{} k \quad(q_{0}^{}, q_{1}^{}, q_{2}^{}, q_{3}^{} \in \mathbb{R})
$$
其中：
$$
i^{2}=j^{2}=k^{2}=i j k=-1
$$
四元数可以看作基 {1，i，j，k}的线性组合，同样的，四元数也可以写成向量形式：
$$
\mathbf q=\left[\begin{array}{l}
{q_{0}^{}} \
{q_{1}^{}} \
{q_{2}^{}} \
{q_{3}^{}}
\end{array}\right]
$$
值得注意的是，只有单位四元数可用于描述姿态。

另外的，也可以将实部与虚部分开，即通过一个三维向量表示虚部，从而将四元数表示为标量与向量的有序对形式：
$$
q=[s, \mathbf{v}] \quad\left(\mathbf{v}=\left[\begin{array}{l}
x \
y \
z
\end{array}\right], s, x, y, z \in \mathbb{R}\right)
$$
利用四元数表示姿态：该部分省略推导过程，具体可以参考https://krasjet.github.io/quate

通过罗德里格旋转可推导出四元数表示旋转的定理：任意向量 v 绕着以单位向量定义的旋转轴 u 旋转 θ 度后的 v’ 可以使用四元数乘法来获得：
$$
v^{\prime}=q v q^{}=q v q^{-1}
$$
其中：
$$
v=[0, \mathbf{v}], \quad q=\left[\cos \left(\frac{1}{2} \theta\right), \sin \left(\frac{1}{2} \theta\right) \mathbf{u}\right]
$$
得出四元数表示旋转的一般形式，即三个四元数相乘，通过四元数乘法的矩阵表示形式，可以将四元数旋转公式表示为矩阵形式：
$$
q v q^{}=\left[\begin{array}{cccc}
1 & 0 & 0 & 0 \
0 & 1-2 q_{2}^{2}-2 q_{3}^{2} & 2 q_{1} q_{2}-2 q_{0} q_{3} & 2 q_{0} q_{2}+2 q_{1} q_{3} \
0 & 2 q_{1} q_{2}+2 q_{0} q_{3} & 1-2 q_{1}^{2}-2 q_{3}^{2} & 2 q_{2} q_{3}-2 q_{0} q_{1} \
0 & 2 q_{1} q_{3}-2 q_{0} q_{2} & 2 q_{0} q_{1}+2 q_{2} q_{3} & 1-2 q_{1}^{2}-2 q_{2}^{2}
\end{array}\right] v
$$
矩阵的第一行和第一列不会对v进行任何变换，所以可以将其压缩为3维方阵，即可得到矩阵旋转公式：任意向量 v 沿着以单位向量定义的旋转轴 u 旋转 θ 角度后的 v’ 可以使用矩阵乘法来获得：
$$
\mathbf{v}^{\prime}=\left[\begin{array}{lll}
1-2\left(q_{2}^{2}+q_{3}^{2}\right) & 2\left(q_{1} q_{2}-q_{0} q_{3}\right) & 2\left(q_{1} q_{3}+q_{0} q_{2}\right) \
2\left(q_{1} q_{2}+q_{0} q_{3}\right) & 1-2\left(q_{1}^{2}+q_{3}^{2}\right) & 2\left(q_{2} q_{3}-q_{0} q_{1}\right) \
2\left(q_{1} q_{3}-q_{0} q_{2}\right) & 2\left(q_{2} q_{3}+q_{0} q_{1}\right) & 1-2\left(q_{1}^{2}+q_{2}^{2}\right)
\end{array}\right] \mathbf{v}
$$
改写为矩阵乘法形式可得到通过四元数表示的 b 系到 n 系的坐标变换矩阵：
$$
\boldsymbol C_{b}^{n}=\left[\begin{array}{lll}
{1-2\left(q_{2}^{2}+q_{3}^{2}\right)} & {2\left(q_{1} q_{2}-q_{0} q_{3}\right)} & {2\left(q_{1} q_{3}+q_{0} q_{2}\right)} \
{2\left(q_{1} q_{2}+q_{0} q_{3}\right)} & {1-2\left(q_{1}^{2}+q_{3}^{2}\right)} & {2\left(q_{2} q_{3}-q_{0} q_{1}\right)} \
{2\left(q_{1} q_{3}-q_{0} q_{2}\right)} & {2\left(q_{2} q_{3}+q_{0} q_{1}\right)} & {1-2\left(q_{1}^{2}+q_{2}^{2}\right)}
\end{array}\right]
$$
其中：
$$
q_{0}=\cos \left(\frac{1}{2} \theta\right), q_{1}=\sin \left(\frac{1}{2} \theta\right) u_{x}, q_{2}=\sin \left(\frac{1}{2} \theta\right) u_{y}, q_{3}=\sin \left(\frac{1}{2} \theta\right) u_{z}
$$
也可写成有序数对形式，即上面旋转公式中的：
$$
q=\left[\cos \left(\frac{1}{2} \theta\right), \sin \left(\frac{1}{2} \theta\right) \mathbf{u}\right]

其中 \mathbf{u}=\left[\begin{array}{l}u_{x} \ u_{y} \ u_{z}\end{array}\right]
$$
得到四元数后，可以通过四元数的值反解出机体坐标系的欧拉角，同样的这里省略推导过程直接给出公式：
$$
\left{\begin{array}{l}
\theta=\arcsin \left[2\left(q_{0} q_{2}-q_{1} q_{3}\right)\right] \
\gamma=\arctan \left(\frac{q_{0} q_{3}+q_{1} q_{2}}{1-2\left(q_{2}^{2}+q_{3}^{2}\right)}\right) \
\psi=\arctan \left(\frac{q_{0} q_{1}+q_{2} q_{3}}{1-2\left(q_{1}^{2}+q_{2}^{2}\right)}\right)
\end{array}\right.
$$

惯性坐标系

将预判打击视为一个运动学问题，那么需要一个参考系。但相机坐标系随着相机运动而运动，在跟随目标时坐标系也发生了变化，在相机坐标系下预判难度较大。引入惯性坐标系的概念。以Yaw轴为例，Yaw轴角度有一个零点，在忽略陀螺仪零漂的情况下，这个零点是相对固定不会改变的。这个零点可以类比为指南针的南极，无论如何移动它，始终指向一个方向。

对于已经解算出目标的相机系坐标 $\boldsymbol r_{c} =\boldsymbol {}\left[\begin{array}{ccc}x_{c}&y_{c}&z_{c}\end{array}\right]^T$。为满足命中率要求，云台在跟踪过程中需对目标进行运动预测，而运动预测需要估计目标在惯性坐标系 (n系) 的运动状态。

根据相机的安装位置，目标在云台坐标系 ( b系) 的坐标可由 $\boldsymbol r_{c} $经过旋转平移后得到：
$$
\begin{equation}
\boldsymbol r_{b} =\boldsymbol {}\left[\begin{array}{ccc}x_{b}&y_{b}&z_{b}\end{array}\right]^T = \boldsymbol R_c^b \boldsymbol r_c + \boldsymbol M_c^b
\end{equation}
$$
其中 $\boldsymbol R_c^b $ 为旋转矩阵、 $\boldsymbol M_c^b $为平移矩阵。 计算时一般只使用平移矩阵。

考虑到目标坐标与姿态信息时间上并不同步，故需根据其时间戳确定两者时间偏移关系。由于图像采集耗时 3ms、目标识别与解算耗时1~3ms，因此需根据时间偏移关系从历史姿态信息中找出与最新目标坐标时间对应的四元数，利用四元数解算得到从云台系 ( b系) 到惯性坐标系 ( n系) 的坐标变换矩阵，进而将目标坐标由b系变换到n 系以得到惯性坐标系下的目标坐标。
$$
\begin{equation}
\boldsymbol r_{n} =\boldsymbol {}\left[\begin{array}{ccc}x_{n}&y_{n}&z_{n}\end{array}\right]^T = \boldsymbol C_b^n \boldsymbol r_b
\end{equation}
$$
其中：
$$
\boldsymbol C_{b}^{n}=\left[\begin{array}{lll}
{1-2\left(q_{2}^{2}+q_{3}^{2}\right)} & {2\left(q_{1} q_{2}-q_{0} q_{3}\right)} & {2\left(q_{1} q_{3}+q_{0} q_{2}\right)} \
{2\left(q_{1} q_{2}+q_{0} q_{3}\right)} & {1-2\left(q_{1}^{2}+q_{3}^{2}\right)} & {2\left(q_{2} q_{3}-q_{0} q_{1}\right)} \
{2\left(q_{1} q_{3}-q_{0} q_{2}\right)} & {2\left(q_{2} q_{3}+q_{0} q_{1}\right)} & {1-2\left(q_{1}^{2}+q_{2}^{2}\right)}
\end{array}\right]
$$
惯性坐标系的原点跟随自身机器人运动，此时视原点静止，将自身机器人的运动与目标的运动叠加在一起，即可构建物理模型进行预判打击。在获得惯性坐标系下的坐标后，先进行一波简单的数据剔除与插值。即剔除明显错误的数据并用上一次正确的数据做插值，可以保证云台预判的流畅性。

还有一些细节需要注意。IMU的采样频率一般为1kHz或500Hz（以C型开发板板载的BMI088为例），而装甲板识别和解算的帧率显然远低于前者（100~200Hz）。 需要对齐两者采集数据的时间戳保证相机系到绝对系的位姿变换正确。 目前只做了软同步，硬同步有待研究。

时间戳同步（TODO）

相机提供了硬件触发功能，这样能够让相机以固定的时间间隔触发采样，保证两帧之间的时间相同，以便于和机器人的控制进行时间线同步。视觉算法处理、数据传输、电控算法处理、再到控制执行机构动作，最后子弹在空中飞行——这几个过程中都有时间延迟，累加之后算是非常可观。高速的算法和确定的延迟时间是打造预测算法的基础。

延时测量

子弹飞行时间最少将占据2/3以上的时间，因此距离的解算和弹道模型是否准确是超前量估计最重要的问题。最暴力的方法是只考虑子弹飞行时间，将剩下所有开销视作黑箱，通过实弹测试获取数据，把超前量当作距离目标的函数进行拟合。总时长最高可以达到0.5~0.7s。倘若将当前时刻识别到的装甲板相对于云台的位置原原本本地发送给下位机进行控制，在识别到装甲板时刻之后的这段时间，对方机器人很可能已经移动到其他位置了（以步兵机器人为例，最快移动速度甚至可以达到6m/s，平均移动速度也有2~3m/s）。

延迟环节 延迟时间 重要性 备注
相机曝光 由设置的参数决定，典型时间为 3~8h 忽略
和下位机通信 和线程设计有关，一般有波动，范围在3~15 ms 不重要 接收IMU数据
装甲板识别 传统算法约13 ms，神经网络约520 ms 不重要
距离解算 PnP典型开销为1 ms 忽略
和下位机通信 和线程设计有关，一般有波动，范围在3~15 ms 不重要 发送控制数据
CAN通信(电机) ~2 ms 忽略
电机动作 和电机控制算法的性能有关，~100 ms 较重要
弹链空程 ~100 ms，取决于弹速 较重要
子弹飞行时间 取决于弹速和距离，300~700 ms 极其重要

由于视觉指令是增量式的，电控在接收到视觉指令时应当使用此延迟之前的电机的角度加上视觉要求的转动角度，不然在电机快速移动情况下就会出现震荡的现象，而这个问题，在视觉和电控同步时间戳，并使用同一块陀螺仪之后，就消失了。

解决方案

时间同步协议的核心是保证相机采集图像的时间确定以及采集时刻云台角度已知。因此，电控端按照一定周期触发相机采图并同时保存时间戳和角度信息至队列中，之后发送给视觉端。视觉端相机被触发后接收电控信息，开始识别图像并根据通信信息进行目标状态预测。之后将预测的模型参数、弹道信息和原时间戳发送给电控端。电控端接收信息计算延时并根据模型对状态转移矩阵的时间间隔进行延时后预测其位置并控制云台跟踪目标。具体流程如下图所示：

当云台转动速度过大或转向频率较高时，陀螺仪测量的滞后会导致对目标运动状态判断很大的误差，即将机器人于云台自身运动耦合到了对方机器人的运动中。这里在触发相机并同步测量角度的基础上，改为触发相机若干毫秒后再进行陀螺仪数据获取。这样相对来看就是通过提前的相机触发来抵消了陀螺仪的滞后测量。

6、角度解算

坐标系分类

涉及的坐标系有图像（像素）坐标系、相机坐标系、惯性坐标系、枪管坐标系、世界坐标系。需要对各个坐标系有清晰的认知。

像素坐标系

即一张图像中描述像素的坐标系。是一个二维空间中的坐标系。在OpenCV中向右为$x$轴正方向，向下为$y$轴正方向。在实际使用中，我们常用齐次形式描述像素坐标系，即一个像素坐标系中的点$(x,y)$常常被表示为$\left[\begin{matrix} x \ y \ 1 \end{matrix}\right]^T$ ,在这种方法的表示下，你也可以认为像素坐标系位于相机坐标系中平面$z=1$上。 我们也称这个平面为归一化平面。

相机坐标系

以相机为原点，$z$轴朝前的右手系。在相机发生平移和旋转运动时，相机坐标系和相机一起运动。必须说明的是，相机坐标系一般为右手系，但其各个坐标轴的朝向可以自行定义，需要注意与像素坐标系保持一致。（这里由于像素坐标系y轴朝下，故相机坐标系中y轴也朝下）。

相机的成像公式中,$\left[\begin{matrix}X_c \ Y_c \ Z_c \end{matrix}\right]^T$即为相机坐标系中的点。相机的成像过程即为将相机坐标系中的点投影到像素坐标系的过程。

惯性坐标系

在相机坐标系中有提到，当相机发生旋转运动时，相机坐标系也会随之一起运动。因此，当相机在发生旋转运动时，想要知道物体和相机的相对位置关系变化将会变得更困难。 对此，我们想要找到一个坐标系，在相机旋转时保持不变，这就是惯性坐标系。惯性系通过固定在相机上的陀螺仪，实时解算相机的位姿，进而得到一个不随相机旋转的坐标系。

由于惯性坐标系和相机坐标系之间的关系为旋转关系，因此惯性坐标系中的三个坐标轴的关系与相机坐标系相同。

相关推导已在上文给出。

世界坐标系

即使是惯性坐标系也会在相机运动时发生平移运动，而世界坐标系是一个与相机没有任何直接关联，建立在外部世界中的一个被认为静止的参考系。

事实上，许多时候视觉定位就是在解算相机坐标系与世界坐标系的相对位置关系。两者之间经过了一次平移和一次旋转变换。矩阵形式如下：
$$
\left[\begin{matrix} X_c \ Y_c \ Z_c \end{matrix}\right] = \ \left[\begin{matrix} R & t \ 0^T & 1 \end{matrix}\right] \times \ \left[\begin{matrix} X_w \ Y_w \ Z_w \end{matrix}\right]
$$
世界坐标系的三个坐标轴关系需要与相机坐标系保持一致（同为左手系或者同为右手系），但是世界坐标系的建立可以依据实际问题选择最方便研究的方式建立。

下图为一个机器人云台， $O_{w}$$X_{w}$$Y_{w}$$Z_{w}$ 为惯性坐标系，原点在云台pitch，yaw轴的交点上，云台零位（枪管水平指向前方）时枪管方向为 $Z$ 轴正向，竖直向下为 $Y$ 轴正向， $X$ 轴按照右手系判断，$O_{c}$$X_{c}$$Y_{c}$$Z_{c}$ 为相机坐标系，原点在相机光心， $Z$ 轴沿相机轴线向前， $X$ 轴水平向右， $Y$ 轴由右手系确定。云台零位时，相机坐标系与固定坐标系三个轴分别平行。但云台旋转一定角度后固定坐标系不变，相机坐标系会随相机运动，与相机固连。枪管坐标系同理。

四个坐标系之间的关系：世界坐标系 – [平移] –> 惯性坐标系 – [旋转] –> 相机坐标系 – [投影] –> 像素坐标系

坐标系变换

三维空间中的两种刚体变换和它们的组合：平移+旋转=位姿变换。三维空间内任意两个刚体系都可以通过如下两步变换实现。首先是平移，令初始坐标原点为：
$$
C_{0}=\left[\begin{array}{l}
x_{0} \
y_{0} \
z_{0}
\end{array}\right]
$$
平移之后得到 :
$$
C_{0}=\left[\begin{array}{l}
x_{0} \
y_{0} \
z_{0}
\end{array}\right]C_{1}=\left[\begin{array}{l}
x_{1} \
y_{1} \
z_{1}
\end{array}\right]+\left[\begin{array}{l}
x_{0} \
y_{0} \
z_{0}
\end{array}\right]
$$
把旋转矩阵拆成绕三个轴的旋转：
$$
R_{x}(\beta)=\left[\begin{array}{ccc}
1 & 0 & 0 \
0 & \cos \beta & -\sin \beta \
0 & \sin \beta & \cos \beta
\end{array}\right] \quad R_{y}(\theta)=\left[\begin{array}{ccc}
\cos \theta & 0 & \sin \theta \
0 & 1 & 0 \
-\sin \theta & 0 & \cos \theta
\end{array}\right] \quad R_{z}(\alpha)=\left[\begin{array}{ccc}
\cos \alpha & -\sin \alpha & 0 \
\sin \alpha & \cos \alpha & 0 \
0 & 0 & 1
\end{array}\right]
$$
将旋转施加到平移后的坐标系，得到变换结果：
$$
C_{2}=R_{x}(\beta) R_{y}(\theta) R_{z}(\alpha) C_{1}
$$
由于平移变换不是三维向量空间中的线性变换，因此无法用矩阵表示，为了方便起见增加一个额外的维度形成齐次坐标系（齐次表示方法最大的优势在于它保留了常数项，因此可以通过矩阵运算进行点的平移操作。 ），用四维矩阵描述三维空间中的任意刚体变换：
$$
C_{2}=R_{x}(\beta) R_{y}(\theta) R_{z}(\alpha) C_{1}\left[\begin{array}{l}
x^{\prime} \
y^{\prime} \
z^{\prime} \
1
\end{array}\right]=\left[\begin{array}{ll}
R & t \
0^{T} & 1
\end{array}\right]\left[\begin{array}{l}
x \
y \
z \
1
\end{array}\right]=T\left[\begin{array}{l}
x \
y \
z \
1
\end{array}\right]
$$
矩阵 T 称为变换矩阵（Transform Matrix）。

PnP算法

如果场景 (或物体) 的三维结构已知，利用多个控制点在三维场景中的坐标及其在图像中的透视投影坐标即可求解出摄像机坐标系与表示三维场景结构的世界坐标系之间的绝对位姿关系，包括绝对平移向量 𝑡 以及旋转矩阵 𝑅，该类求解方法统称为 N 点透视位姿求解 (Perspective-N-Point，PNP 问题)

在 OpenCV 提供的 pnp 姿态解算算法中，SOLVEPNP_ITERATIVE 方法只能用 4 个共面的特征点来解位姿，使用 5 个特征点或 4 点非共面的特征点都得不到正确的位姿。

相机成像模型

一个物体从世界坐标系（绝对系）到像素坐标系的转换过程：
$$
\left[\begin{array}{l}
u \
v \
1
\end{array}\right]=\color{purple}\frac{1}{Z_{c}}\color{green}\left[\begin{array}{ccc}
\frac{1}{d x} & 0 & u_{0} \
0 & \frac{1}{d y} & v_{0} \
0 & 0 & 1
\end{array}\right]\color{blue}\left[\begin{array}{cccc}
f & 0 & 0 & 0 \
0 & f & 0 & 0 \
0 & 0 & 1 & 0
\end{array}\right]\color{red}\left[\begin{array}{cc}
R & t \
0^{T} & 1
\end{array}\right]\color{black}\left[\begin{array}{c}
X_{w} \
Y_{w} \
Z_{w} \
1
\end{array}\right]
$$

  • 红色是物体从世界坐标系变换到相机坐标系的位姿变换矩阵；
  • 蓝色是根据小孔成像模型将物体从三维”压扁“成二维的伸缩变换矩阵（没有旋转，没有平移，但在x和y上都乘上了缩放系数f，即焦距），即相机坐标系到图像坐标系；
  • 绿色是图像坐标系转到像素坐标系的平移（图像原点在左上角）+缩放矩阵（离散化）；
  • 紫色是在投影过程中丢失的维度，也是比例缩放系数（离得越远的物体越小，越近越大）。

红色矩阵内的参数也称作相机外参，蓝色和绿色矩阵则是相机内部的变换信息，称作内参，一般通过标定得到。标准化的相机（啥也不能动，固定好的）则由厂家提供内参（当然自己要再标定也可以，一般没有厂家给的准）。

实际上在成像的过程中，镜头由于种种原因会产生畸变，不能得到蓝色矩阵这般简单的变换，因此需要通过标定来确定畸变的类型和畸变参数以最大程度消除其影响。

应用

需要准确知道三维空间坐标位置，同时也知道对应图像平面坐标的点。装甲板世界坐标系为灯条尺寸，而非实际的装甲板尺寸。需要注意的是，由于采集的数据可能存在灯条光晕， 参数应该适当放大。对于透视投影来说，若单目摄像头的内部参数已知 (通过相机标定获得内参)，要使得 PNP 问题有确定解，需要至少三组控制点，而当 控制点在同一三维平面上时，需要至少四组控制点。可得平移矩阵为：
$$
t V e c=\left[\begin{array}{c}
t_{x} \
t_{y} \
t_{z}
\end{array}\right]
$$
转角计算公式如下：
$$
\begin{array}{c}
\tan p i t c h=\frac{t_{y}}{\sqrt{t_{y}{ }^{2}+t_{z}^{2}}} \
\tan y a w=\frac{t_{x}}{t_{z}}
\end{array}
$$
坐标系是以相机为坐标中心的左手坐标系。通过平移向量可以得出目标在坐标系中的坐标位置可以得到 yaw 和 pitch 的相对值， 以及装甲板中心在相机系的坐标 。

能进行SolvePNP得出相对可靠的数据的前提是摄像头已经进行了较为精细的校准。目前使用的校准方式是传统的棋盘格照片结合ROS内置的相机校准应用进行校准。根据多次实验结合SolvePNP本身求解原理，发现 SolvePNP结果对于像素变化非常敏感，即进行SolvePNP解算前的的输入图像分辨率越高，最终解算得出的结果会更加可靠和稳定。然而受限于算法优化能力和上位机本身算力，目前可以接受的最大分辨率（能保持稳定性能的分辨率）为640×480。这个限制条件间接导致了解算结果的不稳定性。

为了降低输出结果的噪声，对相机进行低曝光处理。这带来的好处是更加稳定的数据，但是也对人工调教和场地灯光提出了更高的要求。在深圳的实际场地中，赛场上光照不充足和不稳定性对于最终的解算结果影响较大。

预测击打

考虑到云台发射的17mm弹丸初速度为15m/s，飞行时间和云台响应的延迟是无法忽略不计的，此外实际比赛中机器人在操作手或者自动化运动程序的控制下的运动是比较复杂的，例如，操作手经常会控制机器人做出闪避、加速、制动等机动动作，自动机器人通过加装储能反弹装置和变速控制进行机动。因此在云台跟踪目标的过程中通过运动预测确定云台期望姿态角对高命中率是至关重要的。要实现运动预测需要选取合适的坐标系以及运动学模型，进而对对目标运动状态进行估计与预测。

总共设计了四种滤波器，可根据实际调试效果选用：

滤波器 运动学模型 目标表达形式
卡尔曼 匀速直线 惯性坐标
卡尔曼 匀加速直线 惯性坐标
卡尔曼 匀速直线 Yaw绝对角度
扩展卡尔曼 当前统计模型 惯性坐标转球面坐标

卡尔曼滤波器介绍

卡尔曼滤波器通俗理解

bilibili-KF&EKF详细视频

编程依据

普通卡尔曼 KF

卡尔曼滤波就相当于一个带有权重的低通滤波，即调整观测值的权重(测量值)和估计值的权重，是更相信观测值还是更相信估计值的一个过程，观测值权重+估计值权重 = 修正值，即最优估计。卡尔曼滤波有马尔可夫性，只考虑了上一个时间节点，而并没有考虑$0 \to k-1$项数据的特征。

卡尔曼滤波是一种递归过程，包含两个更新过程：时间更新和观测更新，其中时间更新主要包括状态预测和协方差预测，主要是对系统的预测，而观测更新主要包括计算卡尔曼增益、状态更新（修正）和协方差更新（修正）。

预测是基于上一时刻状态估计当前状态，而修正则是综合当前时刻的估计状态与观测状态，估计出最优的状态。 考虑到自瞄任务中对于目标只有观测没有控制，所以输入－控制模型 $B$ 和控制器向量 $u$ 可忽略。用观测值来更新预测值，类似于形成一个闭环的反馈，通过设置预测量和观测量的协方差矩阵（类似于噪声，虽然这么说不严谨），让预测器不会超前和滞后。


$$
预测：

\begin{array}{l}
x_{k \mid k-1}=F * x_{k-1 \mid k-1} \
P_{k \mid k-1}=F * P_{k-1 \mid k-1} * F^{T}+Q
\end{array}

更新:

\begin{array}{l}
K=P_{k \mid k-1} * H^{T} *\left(H * P_{k \mid k-1} * H^{T}+R\right)^{-1} \
x_{k \mid k}=x_{k \mid k-1}+K *\left(z_{k}-H * x_{k \mid k-1}\right) \
P_{k \mid k}=(I-K * H) * P_{k \mid k-1}
\end{array}
$$
以上为卡尔曼滤波的全部流程，主要分为两步：预测系统状态矩阵和 协方差矩阵；根据测量更新系统状态矩阵和协方差矩阵。

卡尔曼滤波的适用条件主要有：

  • 应用系统必须是线性的，对于非线性模型并没有良好的效果；
  • 对测量造成影响的噪声必须是符合高斯分布的白噪声。

若想用KF来修正之前的数据，那么卡尔曼滤波器相当于一个插值或平滑数据的装置；若想通过KF来融合当前采集的数据并且消除噪声，那么它是一个估计器，即利用后验信息来修正概率模型；若想通过它来预测t+1时刻的值即通过修改状态转移矩阵用历史数据来推测将来值，这时候就是一个预测器。

扩展卡尔曼 EKF

核心思想与传统 Kalman 模型类似。使用 EKF 而非传统 Kalman 模型的理由是：解藕观测变量，消除变量间的相关性。场上的车辆的运动大概率不符合线性运动规律，且运动噪声也可能不符合高斯分布。

原理

非线性模型和线性模型最直接的区别在于：状态转移方程和观测方程由线性变为非线性。 这一特点在数学公式下可以表现得比较直接。 线性情况下，我们可以用矩阵运算直接表示出状态转移方程和观测方程： (X’ = AX + U) (Z = CX + V) 。而在非线性情况下，我们失去了矩阵工具，因此只能通过函数来表示： (X’ = F(X) + U) (Z = H(X) + V)

而EKF的核心思想就在于，通过数学上的近似手段，把函数$F(x)$和函数$H(x)$转化为矩阵运算$FX$和$HX$。

引入问题：对于一个函数$F(x)$，已知$F(x_0)$，如何近似求解$F(x_0 + \Delta x)$。 我们对$F(x)$在$x_0$进行泰勒展开：
$$
F(x) = F(x_0) + \frac{F’(x_0)}{1!} (x - x_0) + \frac{F’‘(x_0)}{2!}(x - x_0)^2 + \frac{F^{(3)}(x)}{3!}(x-x_0)^3 + \frac{F^{(n)}(\epsilon)}{n!}(x-x_0)^n
$$
一般来说，在非线性度较低的情况下，近似只需取一阶泰勒展开即可。 因此，我们可以得到这样的近似结果：
$$
F(x_0+\Delta x) \approx F(x_0) + F’(x_0)\Delta x
$$
我们可以用这样的近似方式，将状态转移方程和观测方程近似为状态转移矩阵和观测矩阵。 设状态转移矩阵为$F$，那么$F_{ij}$的值就是第$i$个变量的状态转移方程$F_i$对第$j$个变量的偏导。

一般来说，假设我们的状态向量为：
$$
X = \left[\begin{matrix} x_1 \ x_2 \ … \ x_n \end{matrix}\right]
$$
那么我们会有这样的转移方程组：
$$
x_i’ = F_{x_i}(x_1, x_2, …, x_n)
$$
通过一阶泰勒展开近似，可以得到这样的近似转移方程组：
$$
x_i’ = \sum_{j}{\frac{dF_i}{dx_j}x_j}
$$
如果这样的公式描述不够直观，我们可以用一个例子更清晰地描述。 以函数$f(x)=kx^2$为例： 对于他，我们可以列写状态向量：
$$
X = \left[\begin{matrix} f(x) \ x \ k \ \end{matrix}\right]
$$
$X_i$的转移方程为
$$
f^*(x)=k(x+\Delta x) ^ 2
$$

$$
x^* = x + \Delta x
$$

$$
k^* = k
$$

对它们使用一阶泰勒展开近似：
$$
f^*(x) = 0 + (2kx+2k) x + (x + \Delta x)^2 k
$$

$$
x^* = 0 + 1 * x + 0
$$

$$
k^* = 0 + 0 + 1 * k
$$

那么我们就可以用一个矩阵来表示这个转移了：
$$
F = \left[\begin{matrix} 0 & (2kx+2k) & (x + \Delta x)^2 \ 0 & 1 & 0 \ 0 & 0 & 1 \ \end{matrix}\right]
$$

$$
X_k = F X_{k-1}
$$

可以看到在EKF算法的过程中，我们需要对变量求导得到矩阵$F$和矩阵$H$。 程序自动求导实现起来并不容易，因此我们一般使用ceres工具库的Jet类型辅助完成求导的工作。

应用

相机解算出的观测量其实是 yaw 和 pitch 的相对值，三轴坐标并非相互独立。传统 Kalman 模型的一个限制条件就是：系统是一个线性系统，而定义一个 yaw 和 pitch 匀角速运动的车辆显然不符合实际的物理模型（意味着车在一个球面上运动），而实际的运动情况是，受制于电机和惯性，车辆在较长时间内都近似匀速直线运动，而这个模型是定义在惯性坐标系中的（相机坐标系会有自身的转动，不可取），意味着从观测量（yaw、pitch）到预测量（虽然最终转换成 yaw 、 pitch ，但本质是 xyz ）是一个非线性系统。

此外，枪管正对的角度不一定是陀螺仪的0度(你可以想象一下敌方装甲板在你枪管的正前方，但你计算得到的y值为负)，计算得到的x,y值方差实际上是会跟随枪管正前对着的陀螺仪角度变化的。并且，直接拿yaw,pitch当测量值，最后也要把状态估计量转换为yaw,pitch再发送给下位机，这个模型把你最后转换为yaw和pitch过程的估计误差也考虑在内了，理论上更加完整合理。

因此量测模型的设计与量测噪声矩阵的确定应考虑原始信息噪声模型， 为了避免观测量之间的噪声相互叠加损害预测器的工作，使用了 EKF ，它最大的优势是可以运用于非线性系统。这样，预测将会更加准确且收敛更快。

假设车辆在较短的时间内的运动都是匀速直线运动模型，模型公式如下：
$$
\begin{array}{l}
x=x^{\prime}+v_{x} * \Delta t \
y=y^{\prime}+v_{y} * \Delta t \
z=z^{\prime}
\end{array}
$$
其中 z 轴假设不变。

在大多数情况下，EKF已经能够满足我们的大多数需求，如果硬要说它的不足的话：

  • EKF算法具有卡尔曼滤波一类的算法共同的不足，只由$k-1$项推断第$k$项而不考虑历史结果
  • EKF在线性性极差的环境下表现不够优秀

当前统计模型

实际比赛中机器人在操作手或者自动化运动程序的控制下的运动是比较复杂的，使用匀速或者匀加速运动模型很难对其运动状态进行描述并预测。例如，操作手经常会控制机器人做出闪避、加速、制动等机动动作，自动机器人通过加装储能反弹装置和变速控制进行机动。

Singer 模型

Singer 模型认为机动目标的加速度 $a(t) $是一个零均值、指数自相关的一阶马尔可夫过程，其自相关函数为：
$$
R_{a}(\tau)=E[a(t+\tau) * a(t)]=\sigma_{m}^{2} * e^{-\alpha *|\tau|}
$$
其中，$ \sigma_{m}^{2}$是目标加速度的方差，$α$是目标机动常数，它是目标机动频率的倒数。 机动加速度可以用输入为白噪声的一阶时间相关模型来描述，即:
$$
\dot{a}(t)=-\alpha * a(t)+w(t)
$$
其中，$w(t) $为白噪声，其均值为零，方差为$ 2\alpha\sigma_{m}^{2}$ 。

对于采样间隔 T 对系统进行离散化后对应的离散时间系统状态方程为：
$$
\mathrm{X}(\mathrm{k}+1) = \mathrm{F}(\mathrm{k}) * \mathrm{X}(\mathrm{k})+\mathrm{W}(\mathrm{k})
$$
其中 $F(k)$为状态转移矩阵：
$$
\mathrm{F}(\mathrm{k})=\left[\begin{array}{ccc}
1 & \mathrm{~T} & \frac{\left(\alpha \mathrm{T}-1+\mathrm{e}^{-\alpha \mathrm{T}}\right)}{\alpha^{2}} \
0 & 1 & \frac{\left(1-\mathrm{e}^{-\alpha \mathrm{T}}\right)}{\alpha} \
0 & 0 & \mathrm{e}^{-\alpha \mathrm{T}}
\end{array}\right]
$$
$W(k)$为过程噪声矩阵：
$$
\mathrm{W}(\mathrm{k})=2 * \alpha * \sigma_{m}^{2} *\left[\begin{array}{lll}
q_{11} & q_{12} & q_{13} \
q_{12} & q_{22} & q_{23} \
q_{13} & q_{23} & q_{33}
\end{array}\right]
$$
其中：
$$
\left{\begin{array}{l}
q_{11}=\frac{1}{2 \alpha^{5}}\left[1-\mathrm{e}^{-2 a T}+2 \alpha T+\frac{2 \alpha^{3} T^{3}}{3}-2 \alpha^{2} T^{2}-4 \alpha T \mathrm{e}^{-a T}\right] \
q_{12}=\frac{1}{2 \alpha^{4}}\left[\mathrm{e}^{-2 \alpha T}+1-2 \mathrm{e}^{-\alpha T}+2 \alpha T \mathrm{e}^{-\alpha T}-2 \alpha T+\alpha^{2} T^{2}\right] \
q_{13}=\frac{1}{2 \alpha^{3}}\left[1-\mathrm{e}^{-2 \alpha T}-2 \alpha T \mathrm{e}^{-a T}\right] \
q_{22}=\frac{1}{2 \alpha^{3}}\left[4 \mathrm{e}^{-a T}-3-\mathrm{e}^{-2 \alpha T}+2 \alpha T\right] \
q_{23}=\frac{1}{2 \alpha^{2}}\left[\mathrm{e}^{-2 \alpha T}+1-2 \mathrm{e}^{-\alpha T}\right] \
q_{33}=\frac{1}{2 \alpha}\left[1-\mathrm{e}^{-2 \alpha T}\right]
\end{array}\right.
$$
当目标机动频率为无穷大时， Singer 模型近似为匀加速模型，当目标机动频率趋于 0 时，Singer 模型近似为匀速模型。因此，Singer 模型可以表征处于匀速到匀加速模型之间的目标机动模型，在目标机动情况未知的情况下它能够自适应调整自身结构进行运动模型的拟合。

模型优化

Singer 模型假设加速度均值为 0 高斯分布这一先验条件并不能适合于大多数目标机动模型，可以通过修正的瑞利分布描述目标加速度变化的概率密度函数，即通过假设当前机动目标的加速度值的一定范围传递给下一次机动目标的加速度值。

类似的，当前统计模型假设目标运动模型为：
$$
\mathrm{X}(\mathrm{k}+1)=\mathrm{F}(\mathrm{k}) * \mathrm{X}(\mathrm{k})+\mathrm{G}(\mathrm{k}) * \overline{\mathrm{a}}(\mathrm{k})+\mathrm{W}(\mathrm{k})
$$
其中 $G(k)$为输入控制矩阵：
$$
G(k)=\left[\begin{array}{c}
\frac{1}{\alpha}\left(-\mathrm{T}+\frac{\alpha \mathrm{T}^{2}}{2}+\frac{1-\mathrm{e}^{-\alpha \mathrm{T}}}{\alpha}\right) \
\mathrm{T}-\frac{1-\mathrm{e}^{-\alpha \mathrm{T}}}{\alpha} \
1-\mathrm{e}^{-\alpha \mathrm{T}}
\end{array}\right]
$$
$ \overline{\mathrm{a}}(\mathrm{k})$为上一时刻机动加速度的滤波值：
$$
\overline{\mathrm{a}}(\mathrm{k})=\mathrm{E}\left{\mathrm{a}(\mathrm{k}) \mid \mathrm{Z}^{\mathrm{k}-1}\right}=\mathrm{E}\left{\mathrm{a}(\mathrm{k}-1) \mid \mathrm{Z}^{\mathrm{k}-1}\right}=\mathrm{a}(\mathrm{k}-1 \mid \mathrm{k}-1)
$$
$W(k)$为过程噪声矩阵：
$$
\mathrm{W}(\mathrm{k})=2 * \alpha * \sigma_{a}^{2} *\left[\begin{array}{lll}
q_{11} & q_{12} & q_{13} \
q_{12} & q_{22} & q_{23} \
q_{13} & q_{23} & q_{33}
\end{array}\right]
$$
$ \sigma_{a}^{2}$为机动加速度方差：
$$
\sigma_{a}^{2}=\frac{4-\pi}{\pi}\left[a_{\max }-\overline{\mathrm{a}}(\mathrm{k})^{2}\right]
$$

曲线拟合

除了卡尔曼滤波外，还利用Ceres库对运动曲线拟合，作为备选方案。曲线拟合的方法残差项较少，拟合参数较少时耗时也处于可以接收的水平，并且可以

通过拟合后的 RMSE 指标来对曲线拟合结果进行评价，误差较大时可以禁用。其可对已知运动模型的物体进行较为精确的运动建模，但较为依赖于预设曲线模型，若分布与预设模型距离不符时，拟合效果可能较差。

卡方检验

若在跟踪过程中识别的目标发生了变化，卡尔曼滤波器得到的位置量测会与当前位置估计有显著差异，不适合再进行量测更新，需要重新对滤波器进行初始化。

定义残差：
$$
\boldsymbol{e}{k}=\boldsymbol{z}{k}-\boldsymbol{H}{k} \boldsymbol{x}{k}^{-}
$$
正常情况下，残差符合期望为0的正态分布，其方差为：
$$
\boldsymbol{D}{k}=E\left[\boldsymbol{e}{k} \boldsymbol{e}{k}^{T}\right]=\boldsymbol{H}{k} \boldsymbol{P}{k}^{-} \boldsymbol{H}{k}^{T}+\boldsymbol{R}{k}
$$
当发生目标切换时，残差的值会显著大于正常情况，故定义检测函数：
$$
r{k}=\boldsymbol{e}{k}^{T} \boldsymbol{D}{k}^{-1} \boldsymbol{e}_{k}
$$
若其大于阈值，说明位置量测与位置先验估计值有较大差别，即发生了目标切换。发生目标切换后应对滤波器进行初始化以迅速重新收敛。

应用

当第一次发现该目标时将各个保留值进行初始化，连续识别则连续迭代滤波计算预测量，掉帧缓冲则按上一帧位置和速度继续估算目标位置，达到补帧的效果 。

外部直接调用predict()获得预测后的装甲坐标，内部判断调用initFilter()初始化卡尔曼滤波器，并用第一帧输入的装甲绝对坐标进行多次预测使其收敛。判断当前帧和上一帧装甲相对距离，根据设置的阈值(target_change_threshold=0.25m)或者卡方检验判断装甲板是否切换，装甲板切换需重新初始化滤波器。各种状态的判定由装甲板跟踪器决定

移动预测之前穿插反陀螺打击，具体流程在 solve_angle.cpp 中，整个逻辑用 /// 注释。

同时限于云台响应等延迟（在跟随运动目标时存在100ms左右的延时），预判时也给予了延时补偿，这个延时补偿会造成目标在突然变向时云台反应不及（也包含云台自身惯性的原因），这个问题解决的根源还是在于云台响应。

预测问题是一个复杂的系统工程，需要对多个模块从信息的获取到处理加工做大量的优化。在拥有稳定的决策数据源情况下，目标的预测才能够取得较好的效果，否则一切都是无源之水。

弹道补偿

曲线拟合模型

实现简单，计算量少，效果鲁棒。 但是该方法需要大量的测试， 且当拟合变量维数较多，或现实场景数据点存在着训练集未测量到的点，容易出现过拟合现象，导致曲线拟合结果不具有实际的物理意义，存在一定的风险。

抛物线模型

迭代模型

通过位姿解算，已知装甲板相对于枪口水平距离 L，竖直高度 H，需要根据抛物线模型计算击打角度和弹丸飞行时间。对于弹道解算使用公式计算和迭代器计算的方法。

对于使用迭代器进行预测，由于比赛中击打距离较近，存在一个先验知识：枪口上抬则击打点上偏。基于此，只需计算击打点与期望点在竖直方向上面的误差并将其负反馈到云台瞄准角度上面即可。迭代器工作流程如下图所示，当误差小于 1cm 时输出结果。

弹道模型未考虑竖直方向空气阻力，大角度弹道误差较大。而加入竖直方向空气阻力后竖直方向位移表达式较复杂，因为空气阻力大小正比于速度平方，而非速度，故空气阻力方向由符号函数得到，即存在非线性环节，无法得到通用的解析表达式，需分情况讨论（上抛并于下坠前击中目标、上抛并于下坠后击中目标、下抛）。且在计算弹道补偿时未考虑目标距离的变化率，因此对于高速接近或远离的目标打击效果较差，应考虑目标在惯性坐标系平面的完整运动进行迭代求解飞行时间与弹道补偿（暴力迭代或构建新方程迭代求解均可）。

掉帧处理

打中敌方机器人时，其装甲板上的灯条实际上会闪一下，而这个闪一下的过程，相机识别到就是没有装甲板，因此这时候枪管就不会移动。对于不断左右移动的敌方机器人（如哨兵），如因为闪一下的时间而不动，就会跟不上敌人，这很明显是想要避免的情况。而掉帧处理就是专门用来处理上面的情况。

掉帧处理的方法有很多种，下面简单介绍一种：

首先需要测试灯条闪一下的时间对应到相机这边最多是多少帧。然后要记录当前连续掉帧的次数，如果在阈值范围内（一般设置在上一步测试的最大帧数再大一点，比如笔者设置了8），就认为它是在掉帧，这时候发送掉帧前的信息即可（或者乘一个系数缓和一下），目的是不要让枪管骤然停下来。直到超过上述帧数才发送未识别到目标，这样子防止了灯条被打灭就失去目标导致云台突然停一下的问题，打灭了之后云台依旧会向前运动，以保证云台控制的流畅性。

7、串口异步通信

虚拟串口

STM32 微控制器自身提供的 USB Device 特性可以被配置为 Virtual ComPort，即 USB 虚拟串口。使用 USB 虚拟串口相较于一般外置独立USB-TTL 设备具有以下优点：

  • 可以提供更高的 Full Duplex 传输带宽（根据 ST 官方文档，带宽大约是 12Mbps）
  • 物理传输层为 USB 协议，在数据完整性和校验方面提供了更高的稳定性和优势
  • Linux中会自动识别为“/dev/ttyACMX”，其中 X为设备数。不需要实现额外的驱动即可实现串口通讯

此外，为了防止由上位机或下位机意外断电或者接口松动而导致的信号不稳定、程序终止或崩溃问题，我们实现了热插拔功能。日后可以利用其高速与无需进行数据校验的优势做更多扩展，例如下位机运算压力分流，下位机数据记录等。

通信协议

下位机发送至上位机数据

头帧 数据1 数据2 数据3 数据4 数据5 数据6 尾帧
0xFF 敌方颜色（蓝/红) 击打模式（装甲板自瞄/能量机关） 弹速（m/s） 四元数（顺序为wxyz） 云台yaw 云台pitch 0xFE

上位机发送至下位机数据

头帧 数据1 数据2 数据3 数据n 尾帧
0xFF yaw解算值 pitch解算值 空 …

其他

  1. 当视觉有控制指令的时候，需要通过合适的协议告诉电控，再由电控控制实际的电机运动。同样，视觉也需要通过电控了解裁判系统的各种信息和状态量。串口通信的实现分为两种，一种是通过 USB 接入 TTL 模块，变化为 4pin 线接入主控板 uart （电控的电脑），或者是通过运算平台的 4pin 口直接接入电控的 4pin uart 口等
  2. 总的通信带宽是有限的，比如 115200 比特率，意味着 1s 只能发送或接收 115200 个比特，也就是说，超过这一数据量就会发生数据丢失或重叠。
  3. 为了提高数据传输的效率，当有浮点数类型数据需要传输时，根据数据类型的特点，适当截取小数点后固定几位,将浮点数转化成整数类型进行传输，可缩短数据长度，并且避免浮点数传输时发生异常，解析成非法浮点数。类似数据会在协议中标注，如 A*100，就代表将数据 A 只保留两位小数，乘以100进行传输，下位机使用时将收到的数据除以100即可。 尽量精简数据包体量以提高传输速度 。
  4. 视觉发送数据的帧率不能过低，过低会导致云台响应缓慢
  5. 串口发送的速率不要一味的追求过高，过高的串口速率，传递信号中的错误信号也会被放大

8、调试方法

相机参数

  • 曝光：每一帧图像的感光时间，其值愈大则画面的整体亮度越大，曝光时间过长过短都可能会出现宽容度不够的情况（一片雪白或是漆黑无比），选择正确的曝光是算法能否奏效的关键。如传统的灯条特征选取算法就要求恰当的低曝光以保证灯条不会出现过曝而显现白色但同时又要能够看清装甲板中间的数字以便进行模板匹配、svm分类或其他数字识别；运用卷积神经网络的目标检测算法则需要相机采集到的画面能尽量接近数据集中图片的情况，一般来说需要高一些的曝光。
  • 白平衡：设定白色是怎么样的“白”，本意是不管在任何光源下都让原本呈现白色的物体通过增加偏置而还原为白色。涉及到色温和颜色空间的概念，调节此参数即调整RGB三原色的混合比例。
  • 增益：调节感光单元在进行电荷信号放大时的增益（gain,一般是用dB表示的,这需要你注意数量级），对于图像的亮度和各颜色信息的保存都有影响。在低曝光的时候可以有效提高成像质量，但同时也可能提高噪声（不规则噪声信号也会被放大）。
  • 对比度：图像中明暗区域最亮的白和最暗的黑之间不同亮度层级的测量，差异范围越大代表对比越大,在高动态范围和高宽容度的时候，提高对比度可以凸显图像中亮度不同部分的区别，相当于用一把刻度更精细的尺子去测量物体能够得到更精细度量信息。某些情况下，提高对比度所指的则是直接增加亮暗之间的差别，让亮部更亮，暗部更暗。
  • 值得注意的是相机中内置自动曝光，自动增益这些功能，这些算法(也包括你所能查到的大部分自动曝光算法)通常是以图像整体观感作为前提实现的， 而我们感兴趣的区域只有装甲板那一小部分，故这些功能都是价值不大的，不太建议去探索。
  • 其他参考相机官方文档

串口通信

  • 先判断是否成功读写数据，再检查数据格式和数据内容是否吻合
  • 开机未赋予串口权限: （加上自启动就不必处理） 手动赋予权限的方法：sudo chmod 777 /dev/ttyUSB0
  • 串口线的两个位置接反了：适用于导致偶尔能收到一次信息，其他时候都收不到的情况
  • 通信协议出现问题：可以找到以前版本的通信去测试，同时可以看传输函数里面的变量类型是否正常或者看看有没有加传输函数send
  • 可以适当加入 HINT

角度解算

  1. 从机械图纸得到相机坐标系到枪口坐标系的平移矩阵
  2. 先关闭弹道补偿，根据激光红点位置微调固定补偿，使得在不同距离都能将红点对准装甲板中心
  3. 打开弹道补偿，在不同距离不同高度不同射速下测试子弹命中位置
  4. 子弹弹道并不一定是图像的正中心，需要根据实车进行相应的调整，修改子弹弹道的中心散布点

大符

转速函数拟合

调节history_len，在拟合时间与参数精度中取得平衡

粒子滤波

  1. vector_len:所需预测量的维度，但需注意维度灾难的存在，高维空间下样本的稀疏性会使得滤波器效果显著降低。
  2. num_particle:字面意思，粒子的数量，理论上来说粒子数越大效果越好，粒子数量为无穷时即为真实分布。
  3. process_noise:过程噪声矩阵，会在重采样时为粒子叠上方差为该矩阵对角线元素的高斯分布，增加粒子多样性。
  4. observe_noise:观测噪声矩阵，对角线元素即为重要性采样函数（高斯分布右半侧）的协方差。一般来说，不超过过程噪声的两倍

普通卡尔曼

  1. 测距精度达到要求，先调试PID参数，完成后开始调试预测
  2. 提前测算自瞄循环的帧率，主要调试predict_coe预测量和control_freq控制频率
  3. 对于较快速的线性运动，有时需要增大target_change_threshold避免滤波器重复初始化造成跟随过程中的抖动，但该阈值过大容易在装甲板切换时预测出错
  4. 当我们要更信任观测值时，那么我们需要将增益增大，所以可以将观测噪声R调小
  5. 当我们要更信任估计值时，那么我们需要将增益调小，所以可以将观测噪声R调大，也可以将Q调大
  6. 如果想要增高一个预测量的实时性，那么可以减小他的协方差矩阵，但后果是这个变量的预测输出会变的更抖
  7. 如果想要提高一个预测量的平滑度，那么可以增大他的协方差矩阵，但后果是这个变量的实时性会变低

扩展卡尔曼

  1. 测距精度达到要求，先调试PID参数，完成后开始调试预测
  2. 对于较快速的线性运动，有时需要增大target_change_threshold避免滤波器重复初始化造成跟随过程中的抖动，但该阈值过大容易在装甲板切换时预测出错
  3. 修改 EKF.yml 中的预测： Q00 - Q44 以及 R00 - R22 。其中 Q 对应预测值的协方差矩阵对角线，R 对应观测值的协方差矩阵对角线，值和实际的物理量的对应关系如下：
1
2
3
4
5
6
7
8
Q00 -> 预测量 x ：用于调整对直线模型中 x 轴方向运动属于直线运动的的置信度，一般预测不用调整
Q11 -> 预测量 x_v ：用于调整 x	 轴速度的波动量，减小效果：击打匀速目标不会一会超前一会落后，增大效果：更新速度快，例如变向
Q22 -> 预测量 y ：用于调整对直线模型中 y 轴方向运动属于直线运动的的置信度，一般预测不用调整
Q33 -> 预测量 y_v ：用于调整 y 轴速度的波动量，减小效果：击打匀速目标不会一会超前一会落后，增大效果：更新速度快，例如变向；y 轴（pitch）一般不用调整，很容易打中
Q44 -> 预测量 z ：用于调整对直线模型中 z 轴不变的置信度
R00 -> 观测量 yaw：固定不动
R11 -> 观测量 pitch：固定不动
R22 -> 观测量 distance：用于调整距离的波动大小，一般距离的波动非常大，调成三位数，距离更新快

一般固定一组参数，比如 R ，调整 Q ， Q00 和 Q22 一起调， Q11 和 Q33 ，一般每对相同， Q44 单独调。需要发子弹看效果。如果遇到落点一直超前或落后（不是震荡或非震荡引起），调整发射延迟。

卡尔曼矩阵优化（TODO）

测量噪声

敌方装甲板不动，自己动云台，保存相关的原始坐标值，然后利用Matlab或者Python计算出方差，最后填入该矩阵的对角线中。 值得注意的是,由于相机的小孔成像模型，如果误差一个像素的话，敌方装甲板在离自己不同距离所计 算得到的distance方差是完全不同的。因此，一种显然的想法是根据distance的计算值动态改变 distance的方差。可以测出不同距离的distance方差，然后用一条直线拟合出来，每次都不断计算更新distance的方差。yaw和pitch的方差与距离无关，故设置固定值即可。

过程噪声

9、代码规范

编码风格主要参考Google C++ 风格指南(中文版/英文原版)，并在此基础上添加了部分战队特有风格。此规范为对以上材料的补充和改写。如果与参考指南冲突，则以本规范为准。

C++

  • 使用 C++17
  • 不使用 using namespace 的用法
  • 尽可能使用 C++ 标准库 (std::)
  • 避免使用 C 风格的函数，比如： FLT_EPSILON 和 (int)()- 而是使用 std::numeric_limits<double>::epsilon() 和 static_cast<int>()
  • 当功能在标准库中不可用时，鼓励使用 Boost 库
  • 弃用函数使用 [[deprecated\]] 属性，添加一条有用的消息来描述如何处理这种情况：
1
[[deprecated("use bar instead")]] void foo() {}

面向对象的编程思维

  • 单一职责原则SRP：类的功能要单一，体积不要过于庞大。
  • 开放封闭原则OCP：对扩展开放，对修改关闭。
  • 合成复用原则：多组合，少继承，降低耦合
  • 依赖倒置原则DIP：具体依赖抽象。面向对象、抽象编程不要面向细节、过程编程。
  • 接口分离原则ISP：接口定义的标准尽可能单一致力于对接口的多实现

命名风格

更偏向使用完整的、描述性的变量名称而不是简短的字母 - 例如 robot_state 优于 rs

风格 例子
文件名 小驼峰 solve_angle.hpp
类名 大驼峰 class AngleSolver{}
方法 小驼峰 void setTargetSize(double width, double height);
void setCameraParam(const cv::Mat & camera_matrix, const cv::Mat &dist_coeff);
变量 小写+下划线 cv::Mat cam_matrix;
cv::Mat distortion_coeff;
double width_target;
double height_target;
宏 大写+下划线 #define TRUNC_ABS(a) ((a) > 0 ? (a) : 0); 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
//正常语序（前置定语）：
class ArmorClassifier;
//类别优先（后置定语）：
ArmorClassifier classifier_arm;
//适用范围优先：
enum ArmorColor {
    ARMOR_RED  = 0，
    ARMOR_BLUE = 1
};
enum FlowerColor {
    FLOWER_RED   = 0，
    FLOWER_BLUE  = 1，
    FLOWER_GREEN = 2
};

函数

参数

  • 尽可能地用const保护起来，并且使用引用&防止意外修改
  • 应以const类型作为函数数据类型

常用前缀

名称 说明
initXX() 初始化相关方法,使用init为前缀标识，如初始化布局initView()
isXX() checkXX()方法返回值为boolean型的请使用is或check为前缀标识
getXX() 返回某个值的方法，使用get为前缀标识
processXX() 对数据进行处理的方法，使用process为前缀标识，或使用Proc为后缀标识
saveXX() 与保存数据相关的，使用save为前缀标识
resetXX() 对数据重组的，使用reset前缀标识
clearXX() 清除数据相关的
removeXXX() 清除数据相关的
writeXX() 写入文件相关
readXX() 读入文件相关

注释规范

场景 规范
代码块 统一规范：
//-----------------------------------【标题】----------------
// 描述：
//-----------------------------------------------------------------
方法 统一规范
/**
* @brief 这里书写该方法的简介，包括其作用等
* @param 第一个参数说明
* @param第二个参数说明
* @return 返回值说明
*/
宏定义 例： #define SHOW_IMAGE //如果注释掉则不显示图片，有利于加快运行速度
成员变量 如果变量名不能很好的显示其含义或有特殊用途，就需要注释
其他 添加大量注释以解释复杂的代码部分

其他常用规范

  • ”，“之后要留空格，for语句中”;“后需要留空格

  • 所有头文件都必须使用#define 来进行保护，防止头文件被多重包含

    命名格式：<FILE>_H_

  • 将小于10行的工具函数定义为内联函数，内联函数规模不可超过十行，且应尽量使用lambda表达式等技巧构造函数

  • 赋值操作符、比较操作符、算数操作符、逻辑操作符等二元操作符的前后应留空格

  • 其他书写规范尽量与Visual Studio中默认书写规范一致

  • 对于有问题未解决的地方，使用 TODO(username): description

  • 如果变量类型不能从上下文中立即变得清晰（例如从 make_shared<...>），避免使用 auto

  • 在编写较大型的项目的时候，采用配置文件的方法来保存常用的、可能会修改的参数，避免程序中出现一些magic number，提高程序的可维护性和可读性

10、团队协作

组间沟通

在大家成长为RM全栈选手之前，组和组之间的技术认知壁垒是一定存在的；

在机械同学的眼里，视觉各类传感器的散热情况、走线情况、防护需求很可能被忽略。作为视觉队员，应当在机器人设计之初就应当主动引导机械队员设计出合理的结构。

这里有一份问询清单，大家可以选择性的食用：

  1. 是否符合《机器人制作手册规范》
  2. 相机视野是否存在遮挡？
  3. 是否预留布线活动余量？接口线头保护风险评估？
  4. 是否有合理的重力配平？
  5. 俯仰角是否满足任务需求？
  6. 是否有磕碰硬件的风险，是否有保护措施？
  7. 相机、PC、降压模块、IMU等模块紧急检修和更换难度？是否完成模块化?

很多新手同学在前期调研设计时候会忽略的一件事情，就是不认真看往年比赛视频或者仅仅当个爽片去看了，其实里面的细节还是挺多的，哪怕是3分钟准备阶段，都可以看到很多设计的细节，结合比赛多注意一些强队的功能实现。

强烈建议视觉同学按图纸自行搭建基础的场地点，多找一些点位观察视野情况，车辆设计不是机械抄个开源就完事，你的车需要多少俯仰角，相机可以安装在哪里，视觉同学最好准备好相关调研数据。

(比如20交车的下摄像头其实在面对激活点的敌方是看不到的，视野情况还不如摄像头在枪管上面)，很多时候战术的设计，就依赖于此，例如摄像头高度不够，你就没法在环高后面打对面哨兵，摄像头不能倾斜，你就没法超远程吊射。

做好场地调研，以便于更好的知道视觉适合在哪些地方发挥作用。这也是为前期机器人设计定位提供关键数据。

组内学习

前期可以多看一些开源代码，熟悉整个项目的流程，知道每一个模块是怎么设计的，干了什么，不同学校之间的代码差异在哪，各自的特点是什么，能从中学习到什么知识，有什么更好的思路，这都是阅读源代码的时候应当思考的。

中期知识学得差不多之后，可以开始思考当前代码缺了什么功能，或者自己有哪些想法。如果有想法，就思考想法的可行性，如果可行，就开始构建代码框架，然后去实现。如果实现了，就放到车上进行测试。如果测试发现了问题，就多录一些视频素材调试，然后对代码框架进行改进。以此完成一个想法的落地和迭代过程。

  • 比如识别陀螺状态算法的设计，初版的数据结构跟最终的还是有很大差别的，中途是通过不断测试发现问题然后在原有代码框架上不断补充。
  • 如果是设计复杂算法的话，建议先把逻辑理清楚了再写，建议使用Xmind，ProcessOn，幕布等思维导图平台先把整个逻辑写出来。否则很容易写着写着不知道自己在干嘛只好又从头开始，浪费大量时间。

后期基本就是不断测试，优化算法的过程，因为不同的环境对于参数的要求是不同的。比如下午调好的一套参数，到了晚上因为光线的原因可能就不适用了。

问题解决

学习一定要循序渐进，如果某些知识要求一些前置知识，请务必把基础夯实再开始学习前者，否则很容易不知其所云。另外，一定要成体系地进行学习，尽量一次性建立该学科或该领域的主干，不要总是四处寻找碎片化的知识。当你遇到一个难以解决的bug或者异常，请先问问自己，我是否有相关领域的基础知识，如果有了再尝试查找资料解决；如果没有，请立刻着手学习，否则你可能根本无法解决这个问题，或是即使查到了也不理解它到底是如何帮助你修复这个bug的，只是按部就班地根据其解决步骤机械地复制罢了。对于一些一直无法理解的公式推导或者知识，同理。

另外，在上述遇到问题的情况下，你自己可能无法解决（但是也请仔细看一看报错信息，这并不困难，而且很多时候其实很快就能定位问题所在，不要看到一片红色的error就不知所措），这时候就要利用搜索引擎或者向它人求助。一般来说，技术类的问题，Google可以得到比较好的结果，bing次之，最好不要不用baidu。搜索的时候，请使用陈述句而不要加入语气词/问号/疑问词等。如“C++ vector的初始化”就比“怎么样初始化一个vector”好得多。善用搜索引擎的规则搜索，大部分搜索引擎都支持类似正则表达式的功能，可以限定搜索内容，请参考：搜索引擎实用语法。请有意识的在相关的平台寻找对应的知识，在该软件平台的官方网站/说明文档中寻找解答是一个很好的方法。对于编程问题，StackOverflow是比较合适的选择，系统出现的问题如Ubuntu就去askubuntu。谨慎使用CSDN上的解决方法，很多完全不奏效甚至会造成意想不到的后果，并且质量良莠不齐，存在大规模的复制黏贴。How-To-Ask-Questions-The-Smart-Way详细地介绍了提问的艺术，良好的问题描述有助于它人了解你的情况。

Commit Lint

  • feat: new feature
  • fix: modify the issue
  • refactor: code refactoring
  • docs: documentation changes
  • style: code formatting changes
  • test: test case modifications
  • chore: other changes, such as build process, dependency management

11、优化方向

  1. 继续研究反小陀螺算法，提高反小陀螺算法的自适应程度，自动化程度，提高反小陀螺算法击打命中率
  2. 在调节定点打击目标时需要调节相机相对云台的偏移量，当前主要调节参数为两个轴（pitch、yaw）的偏移和三个坐标系（x、y、z）的偏移，调节方法可能存在一定问题，十分繁琐，希望能找到一个更行之有效的方法解决这一问题
  3. 当前滤波的波形显示使用OpenCV绘制，没有做上位机的参数调节，导致调节卡尔曼参数的时候很繁琐，希望新的赛季能设计出更便于调节的上位机
  4. 添加程序重置函数，防止程序出现异常
  5. 大符添加自动控制和复位部分，减少操作手在目标丢失时需要手动对准的操作
  6. 扩展卡尔曼滤波中，使用匀速模型描述目标运动，即将目标加速度建模为期望为0的高斯白噪声。若目标发生机动，即目标做变加速运动时，这种加速度的描述方式就显得太过粗糙。因此如何建模或描述目标加速度是个关键问题，可以考虑采用Singer模型或当前统计模型等运动模型代替匀速模型。另外目标实际运动存在旋转等非匀速直线运动，故可以考虑利用更复杂的运动模型以提高对目标复杂运动的估计效果。
  7. 粒子滤波中，重采样十分频繁。但显然当目标运动状态改变不十分剧烈的时候，没有必要进行重采样，目前的样本即可较好的表示出目标分布，这种时候重采样只会使得目标产生不必要的抖动。此外，添加过程噪声的方法十分粗暴，没有考虑到目标权重的影响，可以考虑为权重大的目标添加较小的噪声，以求减小抖动
  8. 希望在框架的底层实现通用的类模板和接口，后续随着功能的不断更新以及赛事规则的更改，能够仅仅只添加的相应的 任务（task） 和 重写（overwriting） 类的方法，实现机器人的功能。

12、参考文献

[1]Harry-hhj et al. (2021) 上海交通大学 RoboMaster 战队 2021 赛季视觉代码框架[SourceCode]. https://github.com/Harry-hhj/CVRM2021-sjtu

[2]Megvii (2021) YOLOX[SourceCode]. https://github.com/Megvii-BaseDetection/YOLOX

[3]Ningning Ma， Xiangyu Zhang， Hai-Tao Zheng， Jian Sun. ShuffleNet V2: Practical Guidelines for Efficient CNN Architecture Design[C].Salty Lake:Proceedings of the European Conference on Computer Vision (ECCV)，2018

[4]Rosanne Liu， Joel Lehman， Piero Molino， Felipe Petroski Such， Eric Frank， Alex Sergeev， Jason Yosinski. An Intriguing Failing of Convolutional Neural Networks and the CoordConv Solution[C].Montreal:Conference on Neural Information Processing Systems(NeurIPS)，2018

[5]RangerOnMars et al. (2022) 沈阳航空航天大学 RoboMaster 战队 2022 赛季视觉代码框架[SourceCode]. https://github.com/tup-robomaster/TUP-InfantryVision-2022

[6]EVO-Napo et al. (2021) 桂林电子科技大学 RoboMaster 战队 2021 赛季视觉代码框架[SourceCode]. https://github.com/freezing00/Baldr

[7]NZqian et al. (2021) 西北工业大学 RoboMaster 战队 2021 赛季视觉代码框架[SourceCode]. https://github.com/NZqian/WMJ2021

[8]fqjun et al. (2021)华南理工大学广州学院 RoboMaster 战队 2021 赛季视觉代码框架[SourceCode]. https://github.com/wildwolf-team/WolfVision

[9]Google (2010) Ceres Solver[Docs]. http://ceres-solver.org/index.html

[10]Robomaster (2019) RoboRTS—Tutorial[PDF]. https://github.com/RoboMaster/RoboRTS-Tutorial/blob/master/pdf/projectile_model.pdf

[11]Sebastian Thurn et al.Probabilistic Robotics[M].Boston.The MIT Press,2000

[12] 《学习OpenCV3》, Adrian Kaehler ,Gary Bradski，清华大学出版社， 2018-7.

[13] 《Robomaster——关于视觉组，你想要了解的都在这里》,Coo㏒ ∮的CSDN博客,https://blog.csdn.net/weixin_42754478/article/details/108159529

[14] RoboMaster_Raring_Ringtail的博客-CSDN博客,https://blog.csdn.net/u010750137/category_8994384.html

[15] Harry’s Blog,https://harry-hhj.github.io/

[16] RoboMaster创梦之翼战队自瞄系统设计，王洪玺，哈尔滨工程大学

[17] 了解CV和RoboMaster视觉组，曾庆铖，湖南大学 https://github.com/HNUYueLuRM/vision_tutorial

[18] Robomaster (2019) RoboRTS—Tutorial[PDF]. https://github.com/RoboMaster/RoboRTS-Tutorial/blob/master/pdf/projectile_model.pdf

[19] Sebastian Thurn et al.Probabilistic Robotics[M].Boston.The MIT Press,2000

[20] Ningning Ma, Xiangyu Zhang, Hai-Tao Zheng, Jian Sun. ShuffleNet V2: Practical Guidelines for Efficient CNN Architecture Design[C].Salty Lake:Proceedings of the European Conference on Computer Vision (ECCV),2018

[21] Rosanne Liu, Joel Lehman, Piero Molino, Felipe Petroski Such, Eric Frank, Alex Sergeev, Jason Yosinski. An Intriguing Failing of Convolutional Neural Networks and the CoordConv Solution[C].Montreal:Conference on Neural Information Processing Systems(NeurIPS),2018

13、附录