code-test-drone/docs/物理追踪思路.md

# 视觉跟踪控制律设计（FlightController 实现期规格）

## 1. 文档状态与范围

### 1.1 文档状态

- 本文档为**实现期规格文档**，由早期草稿《物理追踪实现路线 v1.0》重写而来：
  原文档中与既有设计体系冲突的表述（摄像头型号、飞控直连、控制时机、
  拦截设备绑定、雷达距离语义等）已全部修正，合理的控制思路
  （水平角控制、垂直控制、距离控制）保留并纳入本规格。
- 当前相关代码均为骨架占位实现，与 `docs/开发进度.md`（第 6 步未开始）一致：
  - `src/control/flight_controller.cpp`（IFlightController Stub）
  - `src/state_machine/mission_state_machine.cpp`（IMissionStateMachine Stub）
  - `src/perception/target_estimator.cpp`（ITargetEstimator Stub）
- 本文档中所有阈值、限幅值、PID 参数**只列占位，不填具体数值**，
  一律标注"待定/配置化"（需求分析.md NFR-007）。

### 1.2 范围与状态机对应

本文档定义视觉跟踪阶段（位置→速度控制切换之后）的控制律：

| 状态机阶段 | 本文档覆盖内容 |
|------------|----------------|
| S5 GPS_APPROACH | 仅提供上下文（位置引导阶段，控制律不在本文档范围） |
| S6 VISUAL_HANDOVER | 切换后首条限幅指令的产生要求（切换条件本身见 状态机设计.md S6） |
| S7 VISUAL_TRACKING | **本文档核心**：水平航向、垂直/高度、距离三路控制律 |
| S8 OBSTACLE_HOLD | 控制律输出被障碍策略接管（限速/刹停），见第 9 章 |

### 1.3 文档定位与引用关系

本文档定位为**视觉跟踪控制律设计（FlightController 实现期规格）**。
与以下内容重叠的部分一律引用、不复述：

| 引用文档 | 引用章节 | 本文档不重复的内容 |
|----------|----------|--------------------|
| `docs/状态机设计.md` | S6、S7、§4.2、§7 | 切换守卫条件、目标丢失转移、可替换策略接口 |
| `docs/感知与避障设计.md` | §3、§5、§6、§9 | 标定流程、稳定锁定、雷达关联条件、降级原则 |
| `docs/视觉追踪技术路线.md` | §1.2、§2 | TargetTracker 算法细节、Coast/判丢行为约定 |
| `docs/系统架构设计.md` | §6、§8 | 数据流、控制单出口、时间与控制周期 |
| `docs/数据接口文档.md` | §2.2、§3、§4.9~§4.12、§5 | 消息字段定义、接口签名、Topic 一览 |
| `docs/需求分析.md` | FR-033、FR-041、FR-051、NFR-007、NFR-008、§7 | 需求条文本身 |
| `include/common/types.h` | 全文 | 消息类型与 Topic 常量的权威定义 |
| `target_tracker/target_tracker.md` | §2、§7 | 追踪库接口与未来接线指引 |

## 2. 输入输出与数据流

### 2.1 输入

控制律在状态机与控制线程中按固定周期消费以下快照（Topic 名称常量见
`include/common/types.h` 的 `drone::common::topics` 命名空间）：

| Topic 常量 | 消息类型 | 用途 |
|------------|----------|------|
| `kEstimatedTarget` | `TargetState` | 目标估计状态（位置/速度/置信度/最近观测时间），控制律主输入 |
| `kFlightState` | `FlightStateSnapshot` | 本机姿态、速度、局部位置，用于坐标转换与反馈 |
| `kLaserRange` | `LaserRangeSample` | 机头前向单点距离；**仅在相机—雷达标定完成且空间关联成立时**才可作为目标距离，否则仅为前向安全距离（需求分析.md FR-041） |
| `kGroundTarget` | `GroundStationTarget` | 地面站目标状态，用于丢失校验与降级回退（非视觉控制主输入） |

### 2.2 输出与控制链路

控制律不直接写飞控。链路严格遵循 系统架构设计.md §6 的控制单出口约束：

```text
状态机与控制线程（控制律，本文档范围）
  -> ControlIntent（控制意图，kControlIntent）
  -> IFlightController（意图 -> 设定值转换与限幅）
  -> Px4Setpoint（kPx4Setpoint）
  -> IPx4Link（PX4 通信线程）
  -> 飞控串口
```

- 只有 PX4 通信线程可以写飞控串口；控制律只提交意图
  （系统架构设计.md §6、数据接口文档.md §4.9）。
- 视觉跟踪阶段主要使用：
  - `ControlIntentType::kVelocityHeading`（速度与航向意图）：
    `target_x/target_y/target_z` 为速度分量（坐标系由 `header.frame_id` 给出），
    `yaw_deg` 为目标航向，`max_speed_mps` 携带速度限制；
  - `ControlIntentType::kBrakeHover`（刹停/悬停意图）：目标丢失判丢、
    障碍制动等场景；
  - `ControlIntentType::kManualOverride`（禁止自动输出/人工接管意图）：
    遥控器接管时停止任务控制输出。
- `Px4Setpoint` 使用 `SetpointType::kVelocity`（速度设定值，分量写入
  `x`、`y`、`z`；垂直通道不再使用 `SetpointType::kAttitude`，理由见
  5.2 设计修订）；航向写入 `yaw_deg` 或 `yaw_rate_dps`；
  无效设定值（如刹停完成）也需发送，避免过期指令（`valid` 标志语义见
  `include/common/types.h`）。

### 2.3 坐标系编码

所有空间数据消息显式使用 `MessageHeader.frame_id` 编码
（数据接口文档.md §2.2、`include/common/types.h` 文件头约定）：

```text
0 = 未知
1 = WGS84 经纬度
2 = 机体（前右下）
3 = 局部NED
4 = 局部ENU
```

控制律内部计算统一在**机体坐标系（frame_id = 2）**下进行
（视线角→机体前/右/下分量），输出的速度意图若采用局部系
（frame_id = 3），必须随 `FlightStateSnapshot` 的姿态/航向完成旋转，
并在 `header.frame_id` 中显式标注；禁止混用未标注坐标系的数据。

## 3. 坐标与标定前提

### 3.1 转换链路

```text
目标像素位置 (u, v)
  ->（相机内参 + 畸变校正）-> 归一化像平面方向
  ->（相机→机体固定外参）-> 机体视线方向
  ->（结合当前机体姿态）-> 水平偏航误差 / 垂直俯仰误差
  ->（雷达关联成立时叠加距离）-> 机体系相对位置 (Δ前, Δ右, Δ下)
```

视觉像素只有结合相机模型和对应时刻的机体姿态，才能转换为目标相对机体
的视线方向（感知与避障设计.md §3）。

### 3.2 标定前提与待定项

实现视觉控制前必须完成（感知与避障设计.md §3、需求分析.md §7）：

- 相机内参与畸变标定（**待定**：相机内参、畸变参数未标定）；
- 相机坐标系到机体坐标系的固定外参（**待定**：外参未标定）；
- 激光波束在相机图像中的投影位置或区域（**待定**：相机—雷达标定未完成）；
- 图像、检测、光流、雷达和 PX4 姿态的时间对齐；
- 目标和本机高度基准（**待定**，需求分析.md §7）。

摄像头为 SIYI A8mini，经网口以 RTSP 输出 H.265、1080p、25 FPS 视频
（需求分析.md §3、开发思路.md）；画面宽度 `W`、高度 `H` 与帧率以实际
解码配置为准。标定完成前，控制律只允许在 SITL/台架中以录制数据回放
验证数学链路，不得上实飞。

## 4. 水平航向控制

### 4.1 目标与原理

目标：让机头在水平方向对准目标。由目标像素水平偏移反求偏航修正量，
经限幅后作为航向指令。

设目标中心像素横坐标偏移为 `x_offset`（目标中心相对画面中心，右正），
图像宽度为 `W`，相机**水平视场角**为 `θ_h`（注意：`θ_h` 仅表示水平
视场角，与俯仰姿态角无关，不得复用同一符号）。

针孔模型下的精确表述：

```
β = atan( x_offset * 2 * tan(θ_h / 2) / W )
```

当目标靠近画面中心（|x_offset| 相对 W 较小）时，可用线性近似：

```
β ≈ x_offset * ( θ_h / W )
```

线性近似的适用条件：

- 偏移量处于画面中心附近（具体适用范围**待定**，由标定与实测确定）；
- 已完成畸变校正（畸变在画面边缘放大近似误差）；
- 大偏移场景应使用针孔模型表述，或直接采用归一化像平面方向。

机体姿态变化时，像素偏移同时包含本机转动与目标运动两部分，
控制律需结合 `FlightStateSnapshot` 的姿态与角速度做补偿
（实现期细化，补偿结构**待定**）。

### 4.2 输出映射

- 航向指令映射到 `ControlIntentType::kVelocityHeading` 的 `yaw_deg`，
  或由 `Px4Setpoint` 的 `yaw_deg` / `yaw_rate_dps` 表达；
- 采用位置式（β→目标航向）还是速率式（β→偏航角速度）由实现期
  按 PX4 1.17.0 Offboard 行为确定（**待定**）；
- 偏航角速度与方向变化率必须限幅（需求分析.md FR-033），
  限幅值见第 8 章。

## 5. 垂直 / 高度控制

### 5.1 误差来源

垂直误差由两个来源合成：

1. 像素垂直偏移：目标中心相对画面中心的纵坐标偏移 `y_offset`（下正），
   图像高度 `H`，相机**垂直视场角** `θ_v`。与第 4 章同理，垂直视线角：

   ```
   α = atan( y_offset * 2 * tan(θ_v / 2) / H )        （针孔模型）
   α ≈ y_offset * ( θ_v / H )                          （中心附近线性近似）
   ```

2. 距离信息：只有雷达与目标完成空间关联（见 3.2 与第 6.3 节）时，
   才可由测距距离 `d` 换算高度差：

   ```
   Δh = d * sin(α)
   ```

   （等价于早期草稿中的 `Δh = d × cos(90° − α)`；此处 α 为垂直视线角，
   与水平视场角 θ_h、垂直视场角 θ_v 明确区分命名。）

雷达未关联时，垂直通道仍由角度误差驱动（统一速度支路，见 5.2），
只是缺少高度差校核，不得假设已知高度差。

### 5.2 控制策略（设计修订：双支路已合并为统一速度支路）

**现行设计**：垂直控制采用统一速度支路——垂直视线角 α 直接映射为
垂直速度设定值，全程保持 `ControlIntentType::kVelocityHeading` 的
`SetpointType::kVelocity` `z` 分量输出，无误差分支切换：

```
vz = clamp( slew( k_vz * α ), -vz_limit, +vz_limit )
```

其中 `k_vz` 为角度-速度增益，`slew` 为垂直加速度限幅（第 8 章），
`clamp` 为垂直速度限幅。小角度误差自然映射为小垂直速度，大小误差
表现为连续的增益、饱和与限速，不产生模式跳变。

**设计修订记录**：原草稿按角度误差大小分两支——小误差微调姿态角
（`SetpointType::kAttitude`，姿态限幅要求见需求分析.md FR-033），
大误差用垂直速度调整高度，两支按角度误差切换阈值切换。参考实现
`reference-c++/visual_track_control`（`tracking_control_law.cpp`
垂直速度通道）已将两支**合并**为上述统一速度支路，Python 移植
（`utils/visual_track_control`）沿用同一实现。合并原因：输出切换为
姿态设定值时，同一周期的距离通道前向速度会随输出模式切换被丢弃，
导致水平逼近停滞；统一速度支路消除了该问题，且无需维护切换阈值。

垂直角度-速度增益 `k_vz` 与垂直速度限幅均**待定/配置化**（第 8 章）。

## 6. 距离控制

### 6.1 目标

距离控制使目标进入拦截射程。**射程进入条件由拦截设备接口
（`IInterceptionDevice`）与配置给出**，本文档不假设任何具体拦截方式；
具体捕网或其他拦截方式不得进入通用逻辑（需求分析.md FR-051、
状态机设计.md §7）。射程 `d_exp` 为配置项，数值**待定**。

### 6.2 控制律

设目标距离为 `d`（来源见 6.3），射程为 `d_exp`：

```
距离误差：Δd = d - d_exp
前向速度指令 = PID(Δd)
```

- 对 `Δd` 做 PID 得到机体前向速度指令，采用速度指令实现
  （`ControlIntentType::kVelocityHeading` 的速度分量 →
  `SetpointType::kVelocity`）；
- `d > d_exp` 时前向接近，`d < d_exp` 时减速/后退，接近末端的速度
  必须按限幅约束收敛；
- PID 参数（Kp/Ki/Kd、积分限幅、微分滤波）全部**待定/配置化**。

### 6.3 距离来源与降级

- 雷达距离**不得直接当作目标距离**：`LaserRangeSample` 表达机头前向
  单点距离，不得自动解释为任意视觉目标的距离
  （`include/common/types.h` 注释、通信与数据定义.md §7）。
  仅当相机—雷达标定完成、视觉区域覆盖雷达投影点、时间足够接近且
  测量质量有效时，才可关联为目标距离（需求分析.md FR-041、
  感知与避障设计.md §3）；
- 关联失败时，雷达距离仅作为**前向安全距离**，供障碍策略
  （第 9 章）使用；距离通道此时按配置降级：保持当前速度、
  限速接近或退出近距操作（具体降级动作**待定**）；
- 视觉估计的距离趋势（目标估计器输出）可作为辅助输入，
  融合权重**待定**。

### 6.4 与追踪库的行为衔接

沿用 视觉追踪技术路线.md §1.2 的上游行为约定：

- 追踪库输出 `is_predicted == true`（Coast 预测中）时，距离与航向
  PID 的**积分项清空**，防止积分饱和；
- 追踪库输出 `tracked == false`（判丢）时，执行**零速刹车**
  （`ControlIntentType::kBrakeHover`），并按 状态机设计.md §4.2
  的目标丢失转移处理；
- 停止视觉闭环时调用追踪库 `Reset()` 清空状态。

## 7. 控制周期与时序

### 7.1 固定控制周期

- 控制线程按**固定周期持续运行**，不等待任意传感器消息后被动执行；
  视觉跟踪控制律在每个控制周期内持续执行，而非"检测完成后才进行"
  （系统架构设计.md §8）；
- 每次控制计算使用同一时刻附近的 PX4、视觉、雷达和目标数据快照；
- 超龄数据只能触发降级或告警，不能继续生成正常控制指令
  （超龄判定使用 `MessageHeader.receive_time_ms` + `valid_for_ms`）；
- 控制周期频率与各输入超龄阈值**待定/配置化**（系统架构设计.md §8）。

### 7.2 与 target_tracker 的衔接

目标追踪库（`target_tracker/`，已实现，见 `target_tracker/target_tracker.md`）
承担像素级平滑、Coast 与主目标锁定：

- 推理侧每帧调用 `TargetTracker::Update()`，控制线程（约 20Hz 速度闭环）
  读取 `TrackResult`（`LastResult()` 线程安全拷贝）；
- 控制线程消费 `TrackResult` 的平滑中心点计算第 4/5 章角度误差，
  并执行 6.4 的 Coast/判丢约定；
- `TrackResult` 属于追踪库自有类型，**尚未进入
  `include/common/types.h`**；`target_tracker/target_tracker.md` §7
  提议新增 `TrackedTarget` 消息与 `kTrackedTarget` 主题，
  该提议当前**未实现**，接入主工程时需先在数据接口文档与
  types.h 中正式定义。

## 8. 限幅、阈值与配置化

所有阈值集中于此并进入配置文件，不得散落在代码中
（需求分析.md NFR-007）。视觉跟踪的速度、加速度和方向变化
必须限幅（需求分析.md FR-033）。

| 项目 | 说明 | 状态 |
|------|------|------|
| 射程 `d_exp` | 由拦截设备接口与配置给出 | 待定 |
| 距离通道 PID 参数 | Kp / Ki / Kd、积分限幅 | 待定 |
| 航向通道增益与限幅 | 偏航角速度上限、方向变化率上限 | 待定 |
| 垂直角度-速度增益 | 垂直视线角到垂直速度的映射增益（5.2 统一速度支路） | 待定 |
| 垂直速度限幅 | 上升/下降速度上限 | 待定 |
| 前向速度限幅 | 接近/后退速度上限，随距离收敛 | 待定 |
| 加速度限幅 | 各轴加速度上限 | 待定 |
| 控制周期频率 | 控制线程固定周期 | 待定 |
| 各输入超龄阈值 | 视觉/雷达/姿态/目标估计 | 待定 |
| 线性近似适用范围 | 画面中心偏移比例 | 待定 |

首条视觉速度设定值必须经过限幅，避免位置→速度切换产生突变
（状态机设计.md S6）。

## 9. 异常与降级

### 9.1 目标丢失

- 判丢后执行零速刹车并调用 `ITargetLostStrategy`：短时预测
  （预测时长与不确定度有上限）、最新有效 `kGroundTarget` 校验、
  整机转向搜索或退回 `GPS_APPROACH`（状态机设计.md §4.2、
  感知与避障设计.md §5）；
- 卡尔曼预测不得无限期生成控制指令（需求分析.md FR-034）。

### 9.2 传感器降级

按 感知与避障设计.md §9 的降级原则，与本文档控制律相关者：

| 故障 | 视觉跟踪阶段动作 |
|------|------------------|
| 摄像头失效 | 退出视觉跟踪 |
| 光流失效 | 降低跟踪可信度和速度 |
| 雷达失效 | 降低速度或退出近距操作；距离通道仅保留角度级控制 |
| 地面站目标过期 | 可按策略短时视觉跟踪 |
| PX4 姿态过期 | 禁止空间转换和控制，立即退出正常控制 |

### 9.3 障碍物规避

前向危险时控制律输出被障碍策略接管：清除视觉速度意图，限速、刹停并
悬停（S8 OBSTACLE_HOLD，状态机设计.md S8、感知与避障设计.md §7）。
雷达未关联时的前向安全距离即该策略的主输入。危险解除后必须重新
验证入口条件才可恢复视觉控制（需求分析.md FR-044）。

## 10. 待定项与验证计划

### 10.1 待定项清单

| 待定项 | 依据 |
|--------|------|
| 相机内参、畸变参数、相机—机体—雷达外参 | 需求分析.md §7 |
| 目标和本机高度基准 | 需求分析.md §7 |
| 第 8 章全部阈值、限幅与 PID 参数 | 需求分析.md §7、NFR-007 |
| 雷达—目标空间关联的具体判据参数 | 感知与避障设计.md §3 |
| 航向控制位置式/速率式选择 | PX4 1.17.0 Offboard 实测 |
| 机体姿态补偿结构 | 实现期细化 |
| `TrackedTarget` / `kTrackedTarget` 消息定义 | target_tracker.md §7 提议，未入库 |

### 10.2 验证路径

按 需求分析.md NFR-008 与 系统架构设计.md §11 逐级验证：

1. **开发机单元测试**：控制律纯函数（角度换算、PID、限幅）用
   Google Test 覆盖；录制数据回放验证像素→视线角数学链路；
2. **PX4 SITL**：在仿真中验证设定值流、限幅行为、丢失/降级分支；
3. **串口台架**：真实飞控 + 模拟数据，验证时序、超龄判定与
   控制权仲裁；
4. **受控实飞**：按状态机设计验证 S6→S7 切换与跟踪效果。

必测故障项包含旧目标位置、旧视频帧、雷达异常、遥控器接管、
算力板控制输出中断（系统架构设计.md §11）。