MOVE_AI

适用于 RoboMaster 机器人的视觉自瞄系统，参考同济大学 Superpower 战队 25 年开源设计，适配 MOVE。

项目结构

├── calibration/          # 标定工具
├── configs/              # 配置文件（yaml）
├── src/
│   ├── component/        # 通用组件（EKF、弹道、日志、绘图等）
│   ├── device/           # 设备驱动（相机、串口、CAN、IMU）
│   ├── module/
│   │   ├── auto_aim/     # 自瞄模块（检测、解算、跟踪、瞄准、规划）
│   │   ├── auto_buff/    # 打符模块
│   │   └── omniperception/  # 全向感知模块（哨兵用）
│   └── task/
│       ├── *.cpp         # 各兵种主程序
│       └── test/         # 测试用例

环境要求

• Ubuntu 22.04
• 运算平台：Intel NUC（i7-1260P / i7-1165G7）
• 相机：海康 MV-CS016-10UC + 6mm 镜头
• 下位机：RoboMaster C 型开发板（STM32F407）/ 达妙 MC02（STM32H7）
• ROS2 Humble（可选，用于哨兵ROS2通信）

依赖安装

1. SDK：

   • HikRobot MVS SDK
   • MindVision SDK（可选）
   • OpenVINO 2024
   • Ceres Solver

2. 系统依赖：

   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
   

3. ROS2 依赖（可选，用于哨兵）：

   # 安装 ROS2 Humble
   sudo apt install ros-humble-desktop
   
   # 编译 rm_msgs 包
   cd ~/rm_msgs
   source /opt/ros/humble/setup.bash
   colcon build
   

编译与运行

标准编译（不含ROS2）

cmake -B build
make -C build/ -j$(nproc)
./build/auto_aim_test    # 运行测试

ROS2编译（哨兵专用）

# 设置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

注意：CMake会自动检测ROS2环境，如果检测到ROS2和rm_msgs包，会自动启用ROS2支持并编译相关程序。

可执行目标

主程序（task）

目标	说明	配置文件
standard	步兵自瞄	configs/standard3.yaml
standard_mpc	步兵自瞄（MPC 规划）	需指定
sentry_mpc	哨兵自瞄（ROS2通信）	需指定
uav	无人机自瞄 + 打符	configs/uav.yaml
uav_debug	无人机调试（含可视化）	configs/uav.yaml
mt_standard	多线程步兵	需指定
balance_infantry	平衡步兵	需指定
balance_infantry_mpc	平衡步兵（MPC规划）	需指定
auto_aim_debug_mpc	自瞄 MPC 调试	需指定
auto_aim_debug_mpc_ros	自瞄 MPC 调试（ROS2）	需指定
auto_buff_debug	打符调试	需指定
auto_buff_debug_mpc	打符 MPC 调试	需指定
mt_auto_aim_debug	多线程自瞄调试	需指定

注意：sentry_mpc 和 auto_aim_debug_mpc_ros 需要ROS2环境，只在检测到ROS2时才会编译。

标定工具（calibration）

目标	说明	通信方式
capture	采集标定图像	串口
capture_ros	采集标定图像（ROS2）	ROS2话题
calibrate_camera	相机内参标定	-
calibrate_handeye	手眼标定	-
calibrate_robotworld_handeye	机器人-世界手眼标定	-
split_video	视频拆帧	-

注意：capture_ros 需要ROS2环境，只在检测到ROS2时才会编译。

测试用例（test）

目标	说明
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. 授予权限：

   sudo usermod -a -G dialout $USER
   

2. 获取端口 ID（serial, idVendor, idProduct）：

   udevadm info -a -n /dev/ttyACM0 | grep -E '({serial}|{idVendor}|{idProduct})'
   

3. 创建 udev 规则：

   sudo touch /etc/udev/rules.d/99-usb-serial.rules
   

   写入（用实际 ID 替换）：

   SUBSYSTEM=="tty", ATTRS{idVendor}=="1234", ATTRS{idProduct}=="1234", ATTRS{serial}=="A1234567", SYMLINK+="gimbal"
   

4. 重新加载规则：

   sudo udevadm control --reload-rules
   sudo udevadm trigger
   

5. 验证：

   ls -l /dev/gimbal
   

通信协议

1. CBoard 协议（CAN 总线）

通过 SocketCAN 与 RoboMaster C 型开发板通信，CAN ID 由 yaml 配置。

发送帧 — 控制命令（CAN ID: 0xff）

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 — 四元数（CAN ID: 0x100 / 0x01）

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，验证 x²+y²+z²+w² ≈ 1

接收帧2 — 子弹速度和模式（CAN ID: 0x101 / 0x110）

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（无人机专有）

2. Gimbal 协议（串口）

通过 USB 串口与达妙 MC02 通信，帧头 {'S', 'P'}，CRC16 校验。

发送帧 — VisionToGimbal（29 bytes, packed）

[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（43 bytes, packed）

[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

3. DM IMU 协议（串口）

达妙 IMU，串口 921600 bps，Modbus RTU 格式，57 bytes 三帧合一。

帧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×float(uint32) + crc16 + 帧尾
四元数由 ZYX 欧拉角生成: q = Rz(yaw) * Ry(pitch) * Rx(roll)

4. ROS2 通信（哨兵专有）

rm_msgs 自定义消息

方向	话题	消息类型	内容
发布	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²)

DataMCU 消息定义（MCU → 视觉）

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     # 子弹累计发送次数

ROS2 程序使用说明

1. 编译 rm_msgs 包：

   cd ~/rm_msgs
   source /opt/ros/humble/setup.bash
   colcon build
   

2. 编译视觉程序：

   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)
   

3. 运行程序：

   # 运行哨兵自瞄
   source /opt/ros/humble/setup.bash
   source ~/rm_msgs/install/setup.bash
   ./build/sentry_mpc configs/sentry.yaml
   
   # 运行标定采集
   ./build/capture_ros configs/calibration.yaml -o assets/img_with_q
   

ROS2 兼容性说明

• 项目支持条件编译，自动检测ROS2环境
• 如果未安装ROS2，只编译串口/CAN版本的程序
• 如果安装了ROS2和rm_msgs，会额外编译ROS2版本：
  • sentry_mpc: 使用ROS2通信的哨兵自瞄程序
  • capture_ros: 使用ROS2通信的标定采集程序
• ROS2版本与串口版本功能完全相同，只是通信方式不同

5. 协议总览

协议	接口	速率	帧长	校验	适用设备
CBoard	CAN	1Mbps	8B	—	C 板 (STM32F407)
Gimbal	串口	可配置	29/43B	CRC16	达妙 MC02 (STM32H7)
DM IMU	串口	921600	57B	CRC16	达妙 IMU
ROS2	DDS	—	可变	DDS	哨兵导航系统