@ -1,351 +1,346 @@
# 同济大学SuperPower战队25赛季自瞄算法开源
# MOVE_AI
## 本项目亮点
- 无ROS依赖*，
- 完整工作流，包含开发、编译、调试、部署全流程。
- 模块化架构，包含各模块的功能说明、代码实现、以及独立的测试程序。
- 我们提出了“轨迹视角下的自瞄理论”， ， ， （ ， ， ）
适用于 RoboMaster 机器人的视觉自瞄系统，参考同济大学 Superpower 战队 25 年开源设计，适配 MOVE。
*为实现哨兵“追击”功能，
## 项目结构
```
├── calibration/ # 标定工具
├── configs/ # 配置文件（ ）
├── src/
│ ├── component/ # 通用组件（ ）
│ ├── device/ # 设备驱动（ ）
│ ├── module/
│ │ ├── auto_aim/ # 自瞄模块（检测、解算、跟踪、瞄准、规划）
│ │ ├── auto_buff/ # 打符模块
│ │ └── omniperception/ # 全向感知模块（哨兵用）
│ └── task/
│ ├── *.cpp # 各兵种主程序
│ └── test/ # 测试用例
```
## 1 功能介绍
自瞄的定义和意义。我们对自瞄的定义为“针对移动装甲板目标的自动瞄准和自动火控软件”。当操作手切换成自瞄模式后，自瞄会接管云台的控制权，通过对敌方运动轨迹的预测和弹道解算，控制云台进行追踪；同时，自瞄还会接管发射机构的控制权，判断开火时机。自瞄的意义在于提高我方作战能力，实现短击杀时间、高命中率的作战效果。
## 环境要求
自瞄的模块组成。经过多年的发展， ，
图1.1 自瞄模块划分
- Ubuntu 22.04
- 运算平台： （ ）
- 相机：海康 MV-CS016-10UC + 6mm 镜头
- 下位机： （ ） （ ）
- ROS2 Humble（ ， ）
自瞄是分布式软件。识别器、估计器、决策器由于算法较为复杂，一般运行在算力较高的小电脑上；出于对实时性和稳定性的考量，控制器一般运行在嵌入式单片机上。模块划分解耦了视觉组和电控组的职责，明确了分工，提高了开发效率。
## 依赖安装
本项目包含了本队自瞄相关的视觉组部分的全部代码，包括但不限于：多线程相机驱动程序、电控通信协议和具体实现、图像-四元数对齐算法、相机内参标定和手眼标定程序、装甲板识别算法（ ）
1. SDK：
- [HikRobot MVS SDK ](https://www.hikrobotics.com/cn2/source/support/software/MVS_STD_GML_V2.1.2_231116.zip )
- [MindVision SDK ](https://mindvision.com.cn/category/software/sdk-installation-package/ )（可选）
- [OpenVINO 2024 ](https://docs.openvino.ai/2024/get-started/install-openvino/install-openvino-archive-linux.html )
- [Ceres Solver ](http://ceres-solver.org/installation.html )
本项目建立在往年[sp_vision_23](https://github.com/TongjiSuperPower/sp_vision_23)、[sp_vision_24](https://github.com/TongjiSuperPower/sp_vision_24)项目的基础之上。其中， ， ， （ ， ） ， （ ） ， （
本项目在上述基础上改进了识别器和决策器。其中，识别器由传统的图像处理方案更换为其它战队开源的基于神经网络的四点检测模型[1,2]，以期提高识别器的召回能力。而决策器则由基于经验主义的分段决策逻辑改为基于轨迹优化的规划器，简化代码的同时，实现了更优的云台控制效果和开火决策逻辑，其思路会在后文详细介绍。
## 2 效果展示
轨迹跟随效果。本项目创新性采用“自瞄轨迹规划算法”， ， ， ，
图2.1 轨迹跟随效果（ ， ）
调试场景演示。本项目容易上手， ， ， ， ， ，
https://github.com/user-attachments/assets/606c2907-2f11-4392-b3fe-7ee4b7b6fd29
视频2.1 调试场景演示
量化指标统计。如图2.2所示， ， ， ， ， ， ， ，
图2.2 国赛MVP结算数据
## 3 详细信息
### 3.1 项目环境
操作系统：
运算平台： （ ， ）
相机型号：
镜头型号：
下位机型号： （ ）
IMU型号：
通信方式： （ ） （ ）
辅助工具： （ ） （ ）
### 3.2 编译方式
1. 安装依赖项：
- [MindVision SDK ](https://mindvision.com.cn/category/software/sdk-installation-package/ )或[HikRobot SDK](https://www.hikrobotics.com/cn2/source/support/software/MVS_STD_GML_V2.1.2_231116.zip)
- [OpenVINO ](https://docs.openvino.ai/2024/get-started/install-openvino/install-openvino-archive-linux.html )
- [Ceres ](http://ceres-solver.org/installation.html )
- 其余：
2. 系统依赖：
```bash
sudo apt install -y \
git \
g++ \
cmake \
can-utils \
libopencv-dev \
libfmt-dev \
libeigen3-dev \
libspdlog-dev \
libyaml-cpp-dev \
libusb-1.0-0-dev \
nlohmann-json3-dev \
openssh-server \
screen
git g++ cmake can-utils \
libopencv-dev libfmt-dev libeigen3-dev \
libspdlog-dev libyaml-cpp-dev libusb-1.0-0-dev \
nlohmann-json3-dev openssh-server screen
```
2. 编译 ：
3. ROS2 依赖（可选，用于哨兵）：
```bash
cmake -B build
make -C build/ -j`nproc`
# 安装 ROS2 Humble
sudo apt install ros-humble-desktop
# 编译 rm_msgs 包
cd ~/rm_msgs
source /opt/ros/humble/setup.bash
colcon build
```
3. 运行demo:
## 编译与运行
### 标准编译（ ）
```bash
cmake -B build
make -C build/ -j$(nproc)
./build/auto_aim_test # 运行测试
```
### ROS2编译（ ）
```bash
# 设置ROS2环境
source /opt/ros/humble/setup.bash
source ~/rm_msgs/install/setup.bash
# 编译
cmake -B build
make -C build/ -j$(nproc)
# 运行ROS2版本程序
./build/sentry_mpc configs/sentry.yaml
./build/auto_aim_debug_mpc_ros configs/standard3.yaml
./build/capture_ros configs/calibration.yaml -o assets/img_with_q
```
**注意**： ， ，
## 可执行目标
### 主程序（ ）
| 目标 | 说明 | 配置文件 |
|------|------|----------|
| `standard` | 步兵自瞄 | `configs/standard3.yaml` |
| `standard_mpc` | 步兵自瞄（
| `sentry_mpc` | 哨兵自瞄（ ）
| `uav` | 无人机自瞄 + 打符 | `configs/uav.yaml` |
| `uav_debug` | 无人机调试（含可视化） | `configs/uav.yaml` |
| `mt_standard` | 多线程步兵 | 需指定 |
| `balance_infantry` | 平衡步兵 | 需指定 |
| `balance_infantry_mpc` | 平衡步兵（ ）
| `auto_aim_debug_mpc` | 自瞄 MPC 调试 | 需指定 |
| `auto_aim_debug_mpc_ros` | 自瞄 MPC 调试（ ）
| `auto_buff_debug` | 打符调试 | 需指定 |
| `auto_buff_debug_mpc` | 打符 MPC 调试 | 需指定 |
| `mt_auto_aim_debug` | 多线程自瞄调试 | 需指定 |
**注意**： `auto_aim_debug_mpc_ros` 需要ROS2环境，
### 标定工具（ ）
| 目标 | 说明 | 通信方式 |
|------|------|----------|
| `capture` | 采集标定图像 | 串口 |
| `capture_ros` | 采集标定图像（ ）
| `calibrate_camera` | 相机内参标定 | - |
| `calibrate_handeye` | 手眼标定 | - |
| `calibrate_robotworld_handeye` | 机器人-世界手眼标定 | - |
| `split_video` | 视频拆帧 | - |
**注意**： ，
### 测试用例（ ）
| 目标 | 说明 |
|------|------|
| `auto_aim_test` | 自瞄全流程测试 |
| `auto_buff_test` | 打符全流程测试 |
| `camera_test` | 相机基础测试 |
| `camera_detect_test` | 相机 + 检测测试 |
| `camera_thread_test` | 相机多线程测试 |
| `cboard_test` | C 板通信测试 |
| `gimbal_test` | 云台通信测试 |
| `gimbal_response_test` | 云台响应测试 |
| `fire_test` | 发射测试 |
| `dm_test` | 达妙 IMU 测试 |
| `handeye_test` | 手眼标定测试 |
| `detector_video_test` | 离线视频检测测试 |
| `planner_test` | MPC 规划器测试 |
| `planner_test_offline` | MPC 规划器离线测试 |
| `usbcamera_test` | USB 相机测试 |
| `usbcamera_detect_test` | USB 相机 + 检测测试 |
| `multi_usbcamera_test` | 多 USB 相机测试 |
| `minimum_vision_system` | 最小视觉系统 |
## 串口设置
1. 授予权限：
```bash
./build/auto_aim_test
```
4. 注册自启：
1. 确保已安装`screen`:
```
sudo apt install screen
```
2. 创建`.desktop`文件:
```
mkdir ~/.config/autostart/
touch ~/.config/autostart/sp_vision.desktop
```
3. 在该文件中写入:
```
[Desktop Entry]
Type=Application
Exec=/home/rm/Desktop/sp_vision_25/autostart.sh
Name=sp_vision
```
注: [Exec ](https://specifications.freedesktop.org/desktop-entry-spec/desktop-entry-spec-latest.html )必须为绝对路径.
4. 确保`autostart.sh`有可执行权限:
```
chmod +x autostart.sh
```
5. USB2CAN设置（ ）
1. 创建`.rules`文件:
```
sudo touch /etc/udev/rules.d/99-can-up.rules
```
2. 在该文件中写入:
```
ACTION=="add", KERNEL=="can0", RUN+="/sbin/ip link set can0 up type can bitrate 1000000"
ACTION=="add", KERNEL=="can1", RUN+="/sbin/ip link set can1 up type can bitrate 1000000"
6. 使用GPU推理（ ）
```
mkdir neo
cd neo
wget https://github.com/intel/intel-graphics-compiler/releases/download/igc-1.0.13463.18/intel-igc-core_1.0.13463.18_amd64.deb
wget https://github.com/intel/intel-graphics-compiler/releases/download/igc-1.0.13463.18/intel-igc-opencl_1.0.13463.18_amd64.deb
wget https://github.com/intel/compute-runtime/releases/download/23.09.25812.14/intel-level-zero-gpu-dbgsym_1.3.25812.14_amd64.ddeb
wget https://github.com/intel/compute-runtime/releases/download/23.09.25812.14/intel-level-zero-gpu_1.3.25812.14_amd64.deb
wget https://github.com/intel/compute-runtime/releases/download/23.09.25812.14/intel-opencl-icd-dbgsym_23.09.25812.14_amd64.ddeb
wget https://github.com/intel/compute-runtime/releases/download/23.09.25812.14/intel-opencl-icd_23.09.25812.14_amd64.deb
wget https://github.com/intel/compute-runtime/releases/download/23.09.25812.14/libigdgmm12_22.3.0_amd64.deb
wget https://github.com/intel/compute-runtime/releases/download/23.09.25812.14/ww09.sum
sha256sum -c ww09.sum
sudo dpkg -i *.deb
```
注：如果使用 GPU 异步推理（ ） ， ×
7. 串口设置
1. 授予权限
```
sudo usermod -a -G dialout $USER
```
2. 获取端口 ID（ ）
```
2. 获取端口 ID（ ） ：
```bash
udevadm info -a -n /dev/ttyACM0 | grep -E '({serial}|{idVendor}|{idProduct})'
```
将 /dev/ttyACM0 替换为实际设备名。
3. 创建 udev 规则文件
```
3. 创建 udev 规则：
```bash
sudo touch /etc/udev/rules.d/99-usb-serial.rules
```
然后在文件中写入如下内容（用真实 ID 替换示例，
写入（用实际 ID 替换）：
```
SUBSYSTEM=="tty", ATTRS{idVendor}=="1234", ATTRS{idProduct}=="1234", ATTRS{serial}=="A1234567", SYMLINK+="gimbal"
```
4. 重新加载 udev 规则
```
4. 重新加载规则：
```bash
sudo udevadm control --reload-rules
sudo udevadm trigger
```
5. 检查结果
```
5. 验证：
```bash
ls -l /dev/gimbal
# Expected output (example):
# lrwxrwxrwx 1 root root 7 Jul 21 10:00 /dev/gimbal -> ttyACM0
```
### 3.3 数据流图
视觉相关模块如图3.1所示。其中，相机线程产生图像、时间戳，通过下位机线程获取对应的云台姿态四元数；图像经过识别器，获得装甲板的四个顶点像素坐标，以及其图案类别；估计器根据装甲板信息，获得目标单位的运动状态；决策器则根据当前的目标运动状态信息，预测目标的运动轨迹，从而判断最佳瞄准位置和最佳开火时机，形成指令发送给下位机；最后控制器和执行机构则根据该指令进行执行，从而完成一个完整的自瞄流程。
图3.1 数据流图
## 通信协议
### 3.4 软件架构
各兵种需要实现的功能往往有多个，但是每个功能不能作为独立的程序。例如步兵需要自瞄和打符，显然，这两个功能都需要从相机获取图像，但是相机只能被一个程序打开，这会导致另一个程序无法正常工作（相机被占用）。
### 1. CBoard 协议（
为了解决这个问题， （ ） ： ， ， （ ） ； （ ） ， ， （ ） ， ， ， ， ， ， 。
图3.2 软件架构
通过 SocketCAN 与 RoboMaster C 型开发板通信，
**发送帧 — 控制命令（ ）
### 3.5 文件结构
```
sp_vision_25
├── assets // 包含demo素材、网络权重等
│ └── ...
├── calibration // 标定相关程序
│ ├── calibrate_camera.cpp // 相机内参标定程序
│ ├── calibrate_handeye.cpp // 手眼标定程序
│ ├── calibrate_robotworld_handeye.cpp // 手眼标定程序（同时计算标定板位置）
│ └── capture.cpp // 相机标定数据采集程序
├── CMakeLists.txt // CMake配置文件
├── configs // 每台机器人的YAML配置文件
│ └── ...
├── io // 硬件抽象层，
│ └── ...
├── src // 应用层，
│ └── ...
├── tasks // 功能层，
│ ├── auto_aim // 自瞄相关算法实现
│ │ └── ...
│ ├── auto_buff // 打符相关算法实现
│ │ └── ...
│ └── omniperception // 全向感知相关算法实现
│ │ └── ...
├── tests
│ ├── auto_aim_test.cpp // 自瞄录制视频测试程序
│ ├── auto_buff_test.cpp // 打符录制视频测试程序
│ ├── camera_detect_test.cpp // 识别器测试程序（工业相机）
│ ├── camera_test.cpp // 相机测试程序
│ ├── camera_thread_test.cpp // 相机线程测试程序
│ ├── cboard_test.cpp // C板测试程序
│ ├── detector_video_test.cpp // 识别器测试程序（视频）
│ ├── dm_test.cpp // 达妙IMU测试程序
│ ├── fire_test.cpp // 开火测试程序
│ ├── gimbal_response_test.cpp // 云台响应测试程序
│ ├── gimbal_test.cpp // 云台通信测试程序
│ ├── handeye_test.cpp // 手眼标定测试程序
│ ├── minimum_vision_system.cpp // 最小视觉系统测试程序
│ ├── multi_usbcamera_test.cpp // 多USB摄像头测试程序
│ ├── planner_test_offline.cpp // 规划器测试程序（离线）
│ ├── planner_test.cpp // 规划器测试程序（实车）
│ ├── publish_test.cpp // ROS发送测试程序
│ ├── subscribe_test.cpp // ROS接收测试程序
│ ├── topic_loop_test.cpp // ROS话题循环测试程序
│ ├── usbcamera_detect_test.cpp // 识别器测试程序（ ）
│ ├── usbcamera_test.cpp // USB相机测试程序
│ └── ...
└── tools // 工具层，
├── crc.hpp // CRC校验
├── exiter.hpp // 退出检测
├── extended_kalman_filter.hpp // 扩展卡尔曼滤波器
├── img_tools.hpp // 图像处理工具
├── logger.hpp // 日志记录器
├── math_tools.hpp // 数学工具
├── plotter.hpp // 曲线图绘制工具
├── recorder.hpp // 视频录制器
├── thread_safe_queue.hpp // 线程安全队列
├── trajectory.hpp // 弹道解算
├── yaml.hpp // YAML配置文件解析器
└── ...
8 bytes, Big-Endian
[0] : control (uint8) 0=不控制, 1=控制
[1] : shoot (uint8) 0=不射击, 1=射击
[2-3] : yaw (int16) 缩放 1e4, 单位 rad
[4-5] : pitch (int16) 缩放 1e4, 单位 rad
[6-7] : horizon_distance (int16) 缩放 1e4（ ）
```
**接收帧1 — 四元数（ ）
## 4 轨迹视角下的自瞄理论
### 4.1 引言
在自瞄的研发和测试过程中，我们发现决策器和控制器是目前的短板。
决策器实现繁琐。不同兵种、不同敌方移动状态（平移、低速小陀螺、高速小陀螺）需要不同的“自瞄行为”，例如：英雄只瞄准旋转中心位置，根据时间误差判断是否开火；平移和低速时，步兵的瞄准位置则持续跟随敌方装甲板移动，同时依据位置误差判断是否开火，而高速时则退化为类似英雄的情况。不同的自瞄行为需要不同的代码实现，而且需要设计不同的判断条件，这一方面提高了代码维护的成本，另一方面也引入额外参数，增大了调试的负担。
控制调参困难。传统控制指标的输入一般为阶跃或斜坡函数，而自瞄控制器的输入更类似于三角波，且周期和幅值随敌方运动状态变化而变化，我们无法找到合适的理论将二者联系起来，因此调参过程中更依赖于经验猜测而不是理论指导。此外，我们无法判断云台的控制效果上限在哪里，甚至对于其影响因素都缺乏充分的认识。这导致电控调参十分“坐牢”，调试一台机器人需要消耗大量时间和资源，并且调试后的机器人表现效果参差不齐。
因此，我们希望能够找到一个“理论”，统一不同决策逻辑、精简代码，同时提高我们对于自瞄以及控制的认识，指导我们更加高效、可靠地调试自瞄，最终提高整体的自瞄效果。
### 4.2 前置概念
命中率。命中率越高，意味着所发射的子弹中命中装甲板的数量越多，造成的伤害越高，是用来衡量自瞄效果最常见的指标。然而，“不谈射频的命中率是没有意义的”[3]，因为如果自瞄只在最有把握的时候射击，只要时间足够长，早晚能打死对面，所以除了命中率还需要考虑击杀时间。
击杀时间。击杀时间衡量了打死敌方单位所需的时间，是衡量自瞄好坏的“金标准”：一方面，装甲板击打检测和发射机构射频均存在上限，杜绝了依靠超高射频刷击杀时间的可行性；另一方面，该指标便于测量，仅需要秒表和靶机即可，比赛时也可以通过回放视频来快速估算。在测量该指标时，需要控制敌方血量、距离和运动状态等变量，这样才能保证时间的可比较性。
目标运动状态。目标运动状态由估计器计算得出，用于决策器预测目标在一段时间后的位置。目前主流的估计算法为基于扩展卡尔曼滤波的整车估计器[3,4]， ， ， ， ， ： ， ， ； ， ， ，
目标轨迹和云台轨迹。已知目标运动状态， ， ， ， ， ， ， ， ，
射击轨迹。子弹出膛后需经过子弹飞行时间t_fly后到达装甲板， ， ：
```math
yaw_{shoot}(t) = yaw_{target}(t + t_{fly})
```
如果t时刻云台角度yaw_gimbal和对应的射击角度yaw_shoot满足一定的误差范围（ ） ， ， ， ， ，
图4.1 轨迹示意图
### 4.3 什么是好自瞄
在轨迹视角下， ， （ ） ， ：
```math
击杀时间 = \frac{血量}{DPS}
8 bytes, Big-Endian
[0-1] : x (int16) 缩放 1e4
[2-3] : y (int16) 缩放 1e4
[4-5] : z (int16) 缩放 1e4
[6-7] : w (int16) 缩放 1e4
四元数顺序: wxyz，
```
```math
DPS = 单位时间射击窗口占比 \times 射频 \times 单发子弹伤害
**接收帧2 — 子弹速度和模式（ ）
```
不难看出， ， ， ， ，
轨迹重合度和云台控制能力息息相关。云台最大加速度的高低决定了云台控制能力的好坏，云台能产生的瞬时加速度越大，跟随轨迹变化的能力越强，轨迹重合度越高。云台最大加速度计算公式如下：
```math
云台最大加速度 = \frac{云台电机最大扭矩}{云台惯量}
8 bytes, Big-Endian
[0-1] : bullet_speed (int16) 缩放 1e2, 单位 m/s
[2] : mode (uint8) 0=idle, 1=auto_aim, 2=small_buff, 3=big_buff, 4=outpost
[3] : shoot_mode (uint8) 0=left, 1=right, 2=both（ ）
[4-5] : ft_angle (int16) 缩放 1e4, 单位 rad（ ）
```
其中，云台电机最大扭矩一般由电机厂家给出，云台惯量通过系统辨识[5]得出。除了云台最大加速度，控制算法的设计也会影响最终的云台控制效果，为了尽可能降低控制难度，我们采取了多种措施：
1. 我们提出了“自瞄轨迹规划器”，根据云台最大加速度优化射击轨迹，使其更容易被云台跟随。
2. 我们计算了轨迹对应的速度和加速度，作为前馈量一并发送给电控，提高云台的响应。
3. 我们采用了计算力矩控制算法[6]， ，
除了击杀时间，命中率也可以在轨迹视角下进行分析。我们可以将云台轨迹划分为“重合射击轨迹段”和“偏离射击轨迹段”，二者的比例决定了理论命中率上限：
```math
命中率 \le 重合度 = \frac{重合射击轨迹段}{(重合射击轨迹段 + 偏离射击轨迹段)}
### 2. Gimbal 协议（串口）
通过 USB 串口与达妙 MC02 通信，帧头 `{'S', 'P'}` ，
**发送帧 — VisionToGimbal（ ）
```
[0-1] : head (uint8[2]) = {'S', 'P'}
[2] : mode (uint8) 0=不控制, 1=控制不开火, 2=控制且开火
[3-6] : yaw (float) rad
[7-10] : yaw_vel (float) rad/s
[11-14] : yaw_acc (float) rad/s²
[15-18] : pitch (float) rad
[19-22] : pitch_vel (float) rad/s
[23-26] : pitch_acc (float) rad/s²
[27-28] : crc16 (uint16) Little-Endian
```
云台在移动过程中，不可避免地会出现偏离射击轨迹的情况，当该情况发生时，应停止开火，减少弹丸的浪费，保证命中率。在自瞄轨迹规划器中，我们将上述情况作为开火决策的依据，在保证击杀时间的同时提高命中率。
击杀时间越短、命中率越高，自瞄效果越好。前者依赖良好的瞄准位置决策，后者则依赖良好的开火决策。我们从轨迹视角出发，用自瞄轨迹规划器替代传统自瞄决策器：通过考虑云台控制的能力，使得决策后的瞄准位置的更具可行性；通过考虑射击轨迹和规划后轨迹的偏差，结合开火延迟时间，使得开火决策更加简洁清晰，无需额外的分段参数，调试时更加方便。
**接收帧 — GimbalToVision（ ）
### 4.4 轨迹规划器
轨迹突变问题和提前减速策略。小陀螺时装甲板会发生切换， ， ， ， ： ， ， ，
图4.2 提前减速策略示意图
```
[0-1] : head (uint8[2]) = {'S', 'P'}
[2] : mode (uint8) 0=IDLE, 1=AUTO_AIM, 2=SMALL_BUFF, 3=BIG_BUFF
[3-6] : q[0] (float) 四元数 w
[7-10] : q[1] (float) 四元数 x
[11-14] : q[2] (float) 四元数 y
[15-18] : q[3] (float) 四元数 z
[19-22] : yaw (float) rad
[23-26] : yaw_vel (float) rad/s
[27-30] : pitch (float) rad
[31-34] : pitch_vel (float) rad/s
[35-38] : bullet_speed (float) m/s
[39-40] : bullet_count (uint16) 子弹累计发射次数
[41-42] : crc16 (uint16) Little-Endian
```
该策略的实现方式不止一种，我们尝试了两种方案：
1. 隐式搜索， ， （ ） （ ） ， ， ， ， ， ， ， ，
2. 显式搜索， ， ， ， ， ， ， ， ，
### 3. DM IMU 协议（串口）
由于时间有限，方案二仅进行了仿真验证，未上车测试，国赛上场采用了方案一。
达妙 IMU， ， ，
开火延迟问题和提前开火决策。从开火命令发送到子弹出膛（ ） ， ， ， ， ，
```
帧1 [0-18] : 加速度 (accx, accy, accz) float, CRC16
帧2 [19-37] : 角速度 (gyrox, gyroy, gyroz) float, CRC16
帧3 [38-56] : 欧拉角 (roll, pitch, yaw) float, 单位°, CRC16
预测时间偏移量。射击轨迹提前于目标轨迹的时间称为预测时间，预测时间除了子弹飞行时间外，还需考虑各个环节引入的时间延迟，包括：
- 图像传输延迟：图像时间戳对应接收完成的时刻，需要图像传输至小电脑的时间。
- 图像处理延迟：神经网络推理耗时无法忽略，需要考虑图像开始处理到形成决策命令的时间。
- 下位机通信延迟：向下位机传输决策命令所需的时间。
- 下位机控制延迟：控制云台到目标位置所需的时间。
每帧结构: 帧头(0x55 0xAA) + slave_id(0x01) + reg + 3×
四元数由 ZYX 欧拉角生成: q = Rz(yaw) * Ry(pitch) * Rx(roll)
```
这里不需要考虑开火延迟，因为在射击轨迹上任意时间发射均会命中[3]。在实际代码实现中，除了图像处理延迟可以直接计算外，我们把其余延迟时间的总和作为一个调试参数[8]， ，
### 4. ROS2 通信（哨兵专有）
**rm_msgs 自定义消息**
## 5 未来优化方向
将轨迹规划器部署到更多兵种上。由于时间有限，国赛中仅步兵使用了轨迹规划器。我们非常期待其在英雄（射频低）、哨兵（惯量大）、无人机（射频高）上的表现。
| 方向 | 话题 | 消息类型 | 内容 |
|------|------|----------|------|
| 发布 | `data_aim` | `rm_msgs/DataAim` | 视觉控制指令 |
| 订阅 | `data_mcu` | `rm_msgs/DataMCU` | MCU状态数据 |
边跑边打。目前的自瞄并没有考虑自身的移动，无法边跑边打。在本赛季，步兵经常需要从狗洞冲下去杀对面静止的英雄，而此时自瞄会认为对面在移动，导致前几发打不准，限制了操作手的发挥。我们计划引入轮式里程计信息，改善在该场景下的表现。
**DataAim 消息定义（视觉 → MCU）
```
uint8 mode # 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
float32 yaw # 目标偏航角 (rad)
float32 yaw_vel # 偏航角速度 (rad/s)
float32 yaw_acc # 偏航角加速度 (rad/s²)
float32 pitch # 目标俯仰角 (rad)
float32 pitch_vel # 俯仰角速度 (rad/s)
float32 pitch_acc # 俯仰角加速度 (rad/s²)
```
## 参考文献
[1] Alan Day.【RM2024赛季-识别模型】深圳大学-RobotPilots[EB/OL]. RoboMaster论坛. https://bbs.robomaster.com/article/54091, 2025.
**DataMCU 消息定义（
[2] gaoxin.【RM2024-识别训练网络及推理代码开源】北京科技大学Reborn[EB/OL]. RoboMaster论坛. https://bbs.robomaster.com/article/9655, 2024.
```
uint8 mode # 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
float32 q0 # 四元数 w
float32 q1 # 四元数 x
float32 q2 # 四元数 y
float32 q3 # 四元数 z
float32 yaw # 偏航角 (rad)
float32 yaw_vel # 偏航角速度 (rad/s)
float32 pitch # 俯仰角 (rad)
float32 pitch_vel # 俯仰角速度 (rad/s)
float32 bullet_speed # 弹速 (m/s)
uint16 bullet_count # 子弹累计发送次数
```
[3] 方俊杰. Linear Modelled Top Detector[EB/OL]. GitHub. https://github.com/julyfun/rm.cv.fans, 2023.
**ROS2 程序使用说明**
[4] 陈君. rm_vision[EB/OL]. GitHub. https://github.com/chenjunnn/rm_vision, 2023.
1. 编译 rm_msgs 包：
```bash
cd ~/rm_msgs
source /opt/ros/humble/setup.bash
colcon build
```
[5] 桂凯. 系统辨识心得分享[EB/OL]. 知乎. https://www.zhihu.com/question/57405191/answer/153098166, 2017.
2. 编译视觉程序：
```bash
cd /home/robofish/MOVE_AI
source /opt/ros/humble/setup.bash
source ~/rm_msgs/install/setup.bash
cmake -B build
make -C build/ sentry_mpc capture_ros -j$(nproc)
```
[6] Lynch K M, Park F C. Modern Robotics: Mechanics, Planning, and Control[M]. Cambridge: Cambridge University Press, 2017.
3. 运行程序：
```bash
# 运行哨兵自瞄
source /opt/ros/humble/setup.bash
source ~/rm_msgs/install/setup.bash
./build/sentry_mpc configs/sentry.yaml
[7] Nguyen K, Schoedel S, Alavilli A, et al. TinyMPC: Model-predictive control on resource-constrained microcontrollers[C]//2024 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2024: 1-7.
# 运行标定采集
./build/capture_ros configs/calibration.yaml -o assets/img_with_q
```
[8] 王洪玺, 计泽贤, 张兰勇. 基于卡尔曼滤波的目标识别跟踪与射击系统设计[J]. Journal of Ordnance Equipment Engineering, 2022, 43(11).
**ROS2 兼容性说明**
- 项目支持条件编译，
- 如果未安装ROS2，
- 如果安装了ROS2和rm_msgs， ：
- `sentry_mpc` : 使用ROS2通信的哨兵自瞄程序
- `capture_ros` : 使用ROS2通信的标定采集程序
- ROS2版本与串口版本功能完全相同，
## 项目成员
王骁扬、杨佳轩、奚睿豪、俞选涛、吴圳楠、杨瑞灵、程翔宇
### 5. 协议总览
## Star History
[](https://www.star-history.com/#TongjiSuperPower/sp_vision_25& Date)
| 协议 | 接口 | 速率 | 帧长 | 校验 | 适用设备 |
|------|------|------|------|------|----------|
| CBoard | CAN | 1Mbps | 8B | — | C 板 (STM32F407) |
| Gimbal | 串口 | 可配置 | 29/43B | CRC16 | 达妙 MC02 (STM32H7) |
| DM IMU | 串口 | 921600 | 57B | CRC16 | 达妙 IMU |
| ROS2 | DDS | — | 可变 | DDS | 哨兵导航系统 |
