# 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)
- 待补充...

## 目标

感谢同济 SuperPower 战队！自瞄算法参考同济的 sp_vision_26，整车决策和节点图系统为自有设计。

## 软件架构

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

### UART 收发节点

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

### IMU 收发节点

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

### 中央节点

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

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

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

### 自瞄节点

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

### 导航节点

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

导航思想简单描述：
0. IMU 被安装在哨兵云台的 YAW 转盘上，坐标轴系向前为 IMU 接口中对应 X 轴，向右为 Y 轴，向下为 Z 轴。由于底盘作动在代码中设置为跟随云台朝向，因此控制量可以直接同步到 IMU 的这个坐标系中观察作用。
1. 将本届 RMUL 地图简化为一个 2D 平面地图，整体轮廓为矩形，面积 $(12\times 8)  m^2$。设置原点为矩形左下角，矩形长（X 轴） 12 米，高（Y 轴）8 米，基地范围为矩形 \[(0,6),(1.5,8)\]，有两墙在图中近似为矩形，一个位于 \[(2.5,1.5),(4.5,8)\] 另一个位于其相对场地的几何中心的中心对称位置，不可跨越，不可通过。“增益点”范围为矩形 \[(4.5,2.5),(7,9.5)\]。
2. 将机器人简化为一个中心点圆，IMU 位于中心点，圆直径为 0.75 米。开局时，因为没有对地图的重定位手段，人为放置机器人，使得机器人中心点圆与基地矩形下、右边缘相切，使云台朝向场地的 Y 轴方向，至此机器人与场地的 X、Y 轴坐标系映射完成。
3. 后续依据此映射和 IMU 的数据，计算出机器人的当前位置，并根据当前状态规划一个绕过墙，前往“增益点”内的路径。目前的方法是直接计算出能使机器人中心点圆通过的路径并一直按该路径行动（设置几个路径点，中心点圆覆盖到路径点时认为路径部分完成）