first commit

2026-09-02 15:13:13 +08:00
commit cb00322a50
107 changed files with 10918 additions and 0 deletions
@@ -0,0 +1,371 @@
+# 视觉跟踪控制律设计（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 章）；航向写入 `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 控制策略
+
+垂直控制按角度误差大小分两支（沿用原草稿思路，阈值待定）：
+
+- **角度误差较小**：直接微调姿态角（`SetpointType::kAttitude`），
+  仅作辅助，必须有严格姿态角与姿态变化率限幅（需求分析.md FR-033）；
+- **角度误差较大**：调整高度，使用速度设定值（`SetpointType::kVelocity`
+  的 `z` 分量，局部 NED 下为垂直速度）趋近目标高度。
+
+角度误差切换阈值、姿态角限幅、垂直速度限幅均**待定**（第 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、积分限幅 | 待定 |
+| 航向通道增益与限幅 | 偏航角速度上限、方向变化率上限 | 待定 |
+| 垂直速度限幅 | 上升/下降速度上限 | 待定 |
+| 姿态角限幅 | 姿态微调支路的角与角速率上限 | 待定 |
+| 前向速度限幅 | 接近/后退速度上限，随距离收敛 | 待定 |
+| 加速度限幅 | 各轴加速度上限 | 待定 |
+| 控制周期频率 | 控制线程固定周期 | 待定 |
+| 各输入超龄阈值 | 视觉/雷达/姿态/目标估计 | 待定 |
+| 线性近似适用范围 | 画面中心偏移比例 | 待定 |
+
+首条视觉速度设定值必须经过限幅，避免位置→速度切换产生突变
+（状态机设计.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）。