# PX4 AprilTag 移动目标跟随演示

使用无人机下视 RGB 摄像头识别小车顶部的 AprilTag，通过姿态补偿和相对位置外环让 PX4 SITL 持续跟随移动目标。控制链不使用 GPS、LiDAR、SLAM、光流位置或 Gazebo 真值。

<p align="center">
  <a href="https://albert17github.github.io/PX4-AprilTag-Follow-Demo/">在线图形报告</a> ·
  <a href="docs/REPRODUCE.md">从零复现</a> ·
  <a href="docs/ARCHITECTURE.md">架构与解算</a> ·
  <a href="docs/VALIDATION.md">验证数据</a> ·
  <a href="CONTRIBUTING.md">参与开发</a> ·
  <a href="LICENSE">Apache-2.0</a>
</p>

![无人机下视相机识别到小车顶部的 AprilTag，绿色框表示该检测可用于控制](reports/assets/apriltag-overlay-tracking.png)

## 这个项目实现了什么

- 一辆差速小车顶部刚性固定朝上的 `tagStandard41h12` AprilTag。
- 无人机使用 `640×480` 下视 RGB 摄像头和上游 `apriltag_ros` 获取带尺度的标签相对位姿。
- 控制器按照图像时间戳插值 PX4 姿态，再应用相机外参，补偿机身 roll/pitch 对观察方向的影响。
- 水平通道使用受限 PD 相对位置外环，输出标准 MAVLink `SET_ATTITUDE_TARGET`；PX4 原有姿态和角速度内环保持不变。
- Gazebo 3D 世界与紧凑的 AprilTag 标注相机窗口会同时显示。
- 小车既可以运行脚本化 figure-eight，也可以用键盘实时控制。

这里的 AprilTag 是**移动目标的相对观测**，不是地图坐标，也不是静止定位信标，因此不会写入 PX4 EKF。

## 信号链

```mermaid
flowchart LR
  Camera[下视 RGB 摄像头] --> Detector[apriltag_ros]
  Detector --> Gate[ID / hamming / 时效门控]
  Attitude[PX4 图像时刻姿态] --> Comp[外参和姿态补偿]
  Gate --> Comp
  Comp --> PD[受限相对位置 PD]
  Baro[PX4 气压高度] --> Safety[高度与丢标安全]
  PD --> Setpoint[SET_ATTITUDE_TARGET]
  Safety --> Setpoint
  Setpoint --> PX4[PX4 姿态 / 角速度内环]
```

| 可以进入控制器 | 明确禁止进入控制器 |
|---|---|
| AprilTag 位姿和检测质量 | Gazebo model/world pose |
| 图像源时间戳与 `CameraInfo` | GPS、local/global position |
| 相机固定外参 | LiDAR、SLAM、光流位置 |
| PX4 attitude、state、气压高度 | 独立 evaluator 的仿真真值 |

Gazebo 真值只允许独立 evaluator 读取，用于完成运行后的误差评分，不向控制器发布任何数据。

## 环境要求

- Ubuntu 24.04 x86_64 图形桌面，`DISPLAY` 可通过 X11/XWayland 访问
- 至少 4 个逻辑 CPU，建议 8 个以上
- 12 GiB 内存与 4 GiB swap
- 首次安装预留至少 20 GiB；完成后的项目运行时约 13 GiB
- 主机工具见 [docs/REPRODUCE.md](docs/REPRODUCE.md)

项目会在仓库内构建独立 Ubuntu rootfs，并安装 ROS 2 Jazzy、Gazebo Harmonic、AprilTag、MAVROS 和 PX4 v1.17.0。主机不需要预装 ROS，也不需要 Docker。

## 快速开始

```bash
git clone https://github.com/albert17github/PX4-AprilTag-Follow-Demo.git
cd PX4-AprilTag-Follow-Demo
./scripts/check_environment.sh --setup
./setup.sh
./demo.sh
```

`./demo.sh` 会打开 Gazebo 与 `AprilTag Camera Detection` 窗口，从小车旁边起飞，以 `0.25 m/s` 的 figure-eight 跟随 60 秒，然后自动下降、disarm 并清理进程。在虚拟机中，Gazebo 窗口可能需要 30–90 秒才出现，也可能位于终端后面，可使用 `Alt+Tab` 切换。

如果上一次终端异常关闭：

```bash
./demo.sh --status
./demo.sh --stop
./demo.sh
```

## 用键盘控制小车

```bash
./demo.sh --keyboard
```

保持启动终端处于焦点，并按住运动键。这里的“前进”始终是小车橙色车头方向，不是固定的屏幕上方。

| 按键 | 动作 |
|---|---|
| `W` / `I` / `↑` | 前进 |
| `S` / `,` / `↓` | 后退 |
| `A` / `J` / `←` | 原地左转 |
| `D` / `L` / `→` | 原地右转 |
| `U` / `O` | 前进左弧线 / 前进右弧线 |
| `M` / `.` | 后退左弧线 / 后退右弧线 |
| `Space` / `K` | 立即停车 |
| `Q` | 结束遥控，无人机下降并 disarm |

运动键超过 `0.4 s` 没有续按时，小车会自动发布零速度。可在安全范围内修改速度和最长时间：

```bash
./demo.sh --keyboard --speed 0.30 --turn-rate 0.9 --max-duration 600
```

## 飞机是怎样跟随的

跟随控制不是二分控制，也没有积分项。水平轴使用标签相对位置误差和有限差分速度构成受限 PD，经过低通、加速度和 `8°` 倾角限制后生成姿态/推力设定值。PX4 内部继续完成姿态与角速度闭环。

机身倾斜时，摄像头看到的方向也会改变。控制器不会简单“减去倾角”，而是按照图像曝光时刻完成：

```text
camera optical pose
        → camera 固定外参
        → body FRD 相对向量
        → 图像时刻 PX4 attitude 旋转
        → gravity-aligned NED 相对向量
```

详细坐标、时间同步和控制公式见 [docs/ARCHITECTURE.md](docs/ARCHITECTURE.md)。

## 丢标行为

- 短暂丢标后，水平追逐指令快速衰减并回正。
- 等待期间使用已有气压高度做简单 PD，不把它描述成水平位置保持。
- 10 秒内重新识别到 Tag，可以恢复 `TRACK`。
- 持续丢标达到 10 秒后，锁存受控下降流程。
- 确认 disarm 后继续保持零推力 3 秒，再退出控制进程。

心跳、姿态时效、OFFBOARD、高度包线和看门狗等安全限制仍具有更高优先级。

## 已验证结果

| 场景 | 条件 | 结果 |
|---|---|---|
| 静态感知 | 1–3 m、偏移和不同 yaw | 5/5 场景，100% 有效，hamming 0 |
| 姿态补偿 | roll/pitch ±15° | 水平 RMSE `0.459 m → 0.000957 m` |
| 静止飞行 | 连续 60 秒 | XY RMS `0.096 m`，p95 `0.227 m` |
| 移动目标 | `0.5 m/s` figure-eight，120 秒 | 视觉 RMS `0.319 m`，p95 `0.560 m` |
| 独立真值评分 | 同一轮 120 秒运行 | RMS `0.324 m`，p95 `0.574 m` |
| 持续丢标 | 遮挡超过 10 秒 | `9.94 s` 后下降，disarm 后 `3.00 s` 退出 |
| 控制输出 | 移动目标正式运行 | `50.000 Hz`，最大倾角 `6.24°` |

机器可读结果位于 [reports/status.json](reports/status.json)，测试方法与运行来源见 [docs/VALIDATION.md](docs/VALIDATION.md)，完整图形证据见[在线报告](https://albert17github.github.io/PX4-AprilTag-Follow-Demo/)。

## 常用命令

```bash
./demo.sh                 # 图形化脚本跟随
./demo.sh --keyboard      # 图形化跟随，键盘控制小车
./demo.sh --gate1         # 静态感知扫描，不解锁
./demo.sh --gate2         # 姿态补偿台架，不解锁
./demo.sh --gate3         # 姿态设定值合同，不解锁
./demo.sh --gate5         # 移动目标飞行回归
./demo.sh --gate6         # 遮挡与失效回归
./demo.sh --verify        # 单测、文档、图片、运行时和证据检查
./demo.sh --stop          # 受控停止或清理异常残留
```

## 范围与限制

这是仿真参考实现，不是经过飞行认证的真机软件。当前不提供避障、全局导航、多标签切换或在移动小车车顶精准降落。控制增益、推力和安全限制只适用于仓库中的 x500 SITL 模型，不能未经独立安全验证直接用于真机。

修改任何会解锁仿真的路径前，请先阅读 [docs/SAFETY.md](docs/SAFETY.md)。贡献方式见 [CONTRIBUTING.md](CONTRIBUTING.md)。