# Amadeus 26

本项目为 Simba Robotics 为 2026 年机甲大师高校联盟赛编写的哨兵机器人机载计算机识别、导航、决策算法。

## 依赖

### 硬件

每一条后括号内为与主机的连接方式。

- MV-SUA133GC-T 工业相机 (USB)
- NUC10i7 微型计算机 (主机)
- FDI Systems DETA10 IMU 模块 (USB - CP210x 串口芯片 - UART) `/dev/ttyCH340`
- CH340 USB 转 UART 模块 (USB - 模块 - UART) `/dev/ttyIMU`

### 软件

- ROS2 Humble
- OpenCV 4.13 with Contrib
- Eigen 3
- MVSDK (工业相机 SDK)
- 待补充...

## 软件架构

ROS2 基本架构为节点。以每个节点的发布、解析、接收数据为例说明。
一共有几项：
- 功能：描述该节点有什么功能
- 连接：描述该节点需要使用的物理连接硬件的软件接口和参数
- 接收：描述该节点从直连硬件中接收的数据
- 订阅：描述该节点从其他节点获取的数据
- 发布：描述该节点使用 ROS 发布的数据
- 发送：描述该节点使用直连硬件发送的数据

### UART 收发节点

- 功能：主机对外通信的封装节点。物理上读取收发模块
- 连接：CH340，协商 UART 通信速率 115200，`/dev/ttyCH340`
- 接收：CH340.RX -> 裁判系统数据
- 订阅：中央节点 -> 控制指令
- 发送：CH340.TX -> 控制指令
- 发布：裁判系统数据（血量、比赛当前状态）
        CH340 、IMU 连接 OK 标志

### IMU 收发节点

- 功能：IMU 与主机通信的封装节点。物理上读取 IMU（DETA10）数据
- 连接：IMU_CP210x，协商 UART 通信速率 921600，`/dev/ttyIMU`
- 接收：IMU_CP210x -> 六轴加速度、陀螺仪数据
- 发布：IMU 数据（加速度、陀螺仪）

### 中央节点

- 功能：综合各类信息判断状态与转移，是控制指令发送的唯一来源
- 订阅：自瞄节点 -> 是否有装甲板、自瞄移动指令、开火指令
        收发节点 -> 裁判系统.血量, 裁判系统.比赛的进程，USB2CAN 连接
- 发布：当前状态，敌方装甲板颜色（蓝/红），控制指令

中央节点负责状态转移的判断。整车的决策被设计为一个有限状态机，分为四个状态：索敌、击打、进点、回家。

```mermaid
stateDiagram-v2
    [*] --> 等待: 开始
    等待 --> 进点: 通信 OK 且 比赛开始 且 血量 = 100%
    进点 --> 索敌: 到达点内
    索敌 --> 攻击: 搜寻到装甲板
    攻击 --> 索敌: 无装甲板
    索敌 --> 回家: 血量 < 50%
    攻击 --> 回家: 血量 < 50%
    回家 --> 等待: 到达基地
```

### 自瞄节点

- 功能：在视野内搜寻装甲板，自动瞄准并计算何时发射
- 连接：MV 工业相机
- 接收：工业相机句柄与帧数据
- 发布: 自瞄运动指令，视野内是否有装甲板

### 导航节点

- 功能：根据 IMU 数据计算出当前位置，根据状态决定目标位置并发出移动到该位置所需的指令
- 接收：收发节点 -> IMU.加速度, IMU.六轴陀螺仪(AHRS)
        中央节点 -> 状态
- 发布：导航运动指令

考虑到 IMU 的实际情况，导航建模思路如下：
- 地图范围：一个 $8 \times 12 m$ 的矩形，机器人运动的范围。以左下角为坐标原点 $ (0,0) $，地图对角为 $ (12,8) $，坐标单位为米。
- 障碍区：不可跨越，不可侵入，由地图中两个矩形组成，第一个矩形的两个角点为 $ (2.5,1.5) ~ (4.5,8) $，第二个为 $ (7.5,0) ~ (9.5,6.5) $。
- 基地：同样为场地中角点描述的矩形，我方基地为 $ (0,6) ~ (1.5,8) $。
- 增益点：同样为场地中角点描述的矩形，范围为 $ (4.5,2.5) ~ (7.5,5.5) $.
- 机器人：视为一直径 $ 0.75 m $ 的圆，圆心位置为 IMU 安装位置。开始时，圆心位于 $ (1.235,6.265) $。
- 机器人的朝向和位置计算：IMU 安装于机器人圆心，开始时，IMU X 轴正方向（机器人朝向）与场地 Y 轴负方向相同，Y 轴正方向与场地 X 轴负方向相同，IMU Z 轴则朝向地面。当机器人朝向发生改变时，IMU 轴系的朝向也会相应发生改变，因此需要结合陀螺仪数据才能正确反向解算加速度计带来的加速度信息，从而完成惯性导航。
- 根据已测量到的 AHRS 返回的数据，yaw 轴在 360° 范围内值为 $ (-3,3) $（左转为增，增到 3 再左转则为 -3）。根据最开始静止测量到的 yaw 值可以确定 **场地朝向**（因为开始时机器人始终朝向赛场坐标系 Y 轴负方向）。