1075 lines
59 KiB
Markdown
1075 lines
59 KiB
Markdown
# 视觉跟踪控制律（visual_track_control）技术路线
|
||
|
||
> 本文档把 `visual_track_control/` 子项目已实现的控制律抽象为**与编程语言无关**的
|
||
> 技术路线：只讲逻辑、数据结构、算法与实现方式，不依赖 C++ 语法特性。
|
||
> 可作为任意语言（C++/Rust/Python 仿真/嵌入式 C）重新实现或移植的规格说明。
|
||
>
|
||
> **文档关系**（三份文档各司其职，不重复抄录）：
|
||
>
|
||
> | 文档 | 定位 | 权威性 |
|
||
> |------|------|--------|
|
||
> | `docs/物理追踪思路.md` | 控制律**物理与行为规格**（为什么这样控） | 数学与行为的唯一权威 |
|
||
> | `docs/视觉跟踪控制律技术路线.md`（本文） | **语言无关实现路线**（逻辑怎么落地） | 实现结构与算法流程的权威 |
|
||
> | `visual_track_control/视觉跟踪控制律实现文档.md` | **C++ 实现说明**（构建、日志、排查） | 当前 C++ 落地的权威 |
|
||
>
|
||
> 文中出现的字段名（如 `track_time_ms`、`yaw_rate_dps`）取自当前实现，仅作为
|
||
> **跨文档可追溯的标识符**，不构成语言绑定；重写时可自由改名，但语义与单位必须一致。
|
||
>
|
||
> ⚠️ **全部数值均为占位值**：相机内参/畸变未标定、增益与 PID 未整定。标定完成前
|
||
> 本路线**仅限回放/SITL 验证数学链路，不得用于实飞**（见第 11 章红线）。
|
||
|
||
---
|
||
|
||
## 目录
|
||
|
||
1. [概述与系统定位](#1-概述与系统定位)
|
||
2. [输入输出接口定义](#2-输入输出接口定义)
|
||
3. [核心构件与状态清单](#3-核心构件与状态清单)
|
||
4. [坐标系与符号约定](#4-坐标系与符号约定)
|
||
5. [单周期控制流程（主流程伪代码）](#5-单周期控制流程主流程伪代码)
|
||
6. [各通道算法细节](#6-各通道算法细节)
|
||
7. [安全语义：超龄、判丢、Coast、无距离](#7-安全语义超龄判丢coast无距离)
|
||
8. [输出打包与链路](#8-输出打包与链路)
|
||
9. [配置参数全量表](#9-配置参数全量表)
|
||
10. [设计取舍记录（为什么不是别的做法）](#10-设计取舍记录为什么不是别的做法)
|
||
11. [实现与重写指引](#11-实现与重写指引)
|
||
12. [验证路线与待定项](#12-验证路线与待定项)
|
||
|
||
---
|
||
|
||
## 1. 概述与系统定位
|
||
|
||
### 1.1 模块职责
|
||
|
||
本模块是**视觉跟踪阶段（状态机 S7 VISUAL_TRACKING）的单周期控制律**，职责可归纳为
|
||
"三算一限一降级一打包"：
|
||
|
||
1. **三路控制律计算**（每拍一次，固定周期）
|
||
- **水平航向**：目标像素偏移 → 水平视线角 β → 增益 → 偏航指令；
|
||
- **垂直高度**：目标像素偏移 → 垂直视线角 α → 垂直速度指令；
|
||
- **距离**：双目目标距离与期望射程之差 Δd → PID → 前向速度指令。
|
||
2. **统一限幅**：每个通道内联完成"变化率限幅 + 幅值限幅"，任何输出都不允许
|
||
跳过限幅直达链路（含切换后的**首条**指令）。
|
||
3. **安全降级**：输入超龄、目标判丢、距离不可用时，输出降级意图而不是继续
|
||
用旧数据算正常指令。
|
||
4. **单出口打包**：控制意图 → MAVLink 字节流 → 注入的传输实现；控制律本身
|
||
不认识串口、电台或飞控。
|
||
|
||
### 1.2 在系统中的位置
|
||
|
||
```text
|
||
┌─────────────┐ 检测框 ┌──────────────┐ 追踪结果 ┌───────────────────┐
|
||
│ YOLO 识别 │────────────▶│ TargetTracker│───────────▶│ 本模块：控制律 │
|
||
│ （感知线程）│ 每帧 │ （滤波器） │ 主目标+ │ TrackingControlLaw│
|
||
└─────────────┘ └──────────────┘ Coast标志 └─────────┬─────────┘
|
||
│
|
||
┌─────────────┐ 目标距离 │ 控制意图
|
||
│ 双目立体测距│──────────────────────┐ ▼
|
||
└─────────────┘ │ ┌──────────────────────┐
|
||
┌─────────────┐ 姿态/航向/速度 ├─────────────▶│ 打包网关 │
|
||
│ PX4 链路 │──────────────────────┘ 控制快照 │ （控制意图→MAVLink） │
|
||
└─────────────┘ （由调用方采集打戳） └─────────┬────────────┘
|
||
▲ │ 字节流
|
||
│ MAVLink offboard 指令流 ▼
|
||
└────────────────────────────────────────────── ┌──────────────────┐
|
||
│ 传输抽象实现 │
|
||
│ （串口/电台/回环）│
|
||
└──────────────────┘
|
||
```
|
||
|
||
**上游依赖**：追踪库提供主目标中心与 `tracked` / `is_predicted` 两个行为标志
|
||
（追踪库技术路线见 `docs/视觉追踪技术路线.md`）；双目模块提供目标距离
|
||
（检测框自身的立体测距，前向分量 Z）与有效标志 `distance_valid`；飞控链路
|
||
提供自机姿态（至少航向角）。
|
||
|
||
**方位源（接入层约定）**：双目模组两枚相机分列模组中线两侧（各偏 ±B/2），
|
||
单目像素存在近场视差偏差（3m 处约 0.57°）。匹配成功时接入层应以**虚拟双目
|
||
中心**像素 `x_mid = (xL + xR) / 2` 填充 `track.center`（推导见
|
||
`docs/双目深度估计思路.md` §11.3）。虚拟中心相机与单目共用同一 FOV 配置，
|
||
本模块"像素偏移 → 视线角"逻辑与配置**零改动**。
|
||
|
||
**下游依赖**：只有一个——传输抽象。打包与传输分离，控制律不直接写飞控。
|
||
|
||
**上层归属**：状态机（S6/S7/S8 切换、目标丢失策略、障碍接管）在**本模块之外**。
|
||
本模块只按输入标志产生"当拍应该做什么"，不决定"要不要继续视觉闭环"。
|
||
|
||
### 1.3 设计特点
|
||
|
||
| 特点 | 含义 | 带来的约束 |
|
||
|------|------|-----------|
|
||
| **纯值输入 / 纯值输出** | 每拍传入一个不可变快照，返回一个输出值对象 | 无隐藏输入、无回调、可离线回放 |
|
||
| **单线程固定周期** | 由控制线程按配置频率调用，构件内部**无锁** | 不得从多线程并发调用同一实例 |
|
||
| **热路径零动态分配** | 所有系数、缓冲在构造期备妥；每拍只做算术 | 便于实时性分析与嵌入式移植 |
|
||
| **热路径零日志** | 仅在输出模式发生切换时打一条日志 | 日志量与状态变化次数成正比，不与频率成正比 |
|
||
| **配置全量外置** | 所有阈值/增益/限幅来自启动期加载的配置对象 | 代码内**不得**出现任何写死的控制参数 |
|
||
| **无状态纯函数构件** | 视线角转换、超龄判定、坐标旋转均为纯函数 | 可独立单元测试，无需夹具 |
|
||
| **单一 offboard 消息类型** | 全周期只发一种消息（ID 84） | 避免消息类型逐拍翻转账扰动飞控（见 §10.1） |
|
||
|
||
### 1.4 明确不做的事（边界）
|
||
|
||
- **不做感知**：不跑检测/光流/双目立体匹配，只消费调用方填入的快照；
|
||
- **不做距离有效性判定**：`distance_valid` 由双目链路自身的一致性校验给出，
|
||
track_id 一致性校验为调用方约定（§12.2），本模块只读取标志；
|
||
- **不做状态机仲裁**：不判断何时进入/退出视觉跟踪，不实现目标丢失策略；
|
||
- **不做链路 IO**：不打开串口、不管理重连、不解析任何入向 MAVLink 消息；
|
||
- **不做配置热更新**：启动期加载一次，调参重启生效；
|
||
- **不做多目标**：只处理追踪库选定的主目标。
|
||
|
||
---
|
||
|
||
## 2. 输入输出接口定义
|
||
|
||
### 2.1 输入：控制快照（每拍一个，由调用方采集并打戳）
|
||
|
||
| 分组 | 字段 | 单位 / 类型 | 语义与约束 |
|
||
|------|------|------------|-----------|
|
||
| 视觉 | `track.center` | 像素 (x, y) | 主目标中心；原点为画面**左上角**，x 右正、y 下正；双目匹配成功时由接入层填虚拟双目中心 `x_mid`（§1.2） |
|
||
| 视觉 | `track.tracked` | 布尔 | 是否存在有效轨道；`false` → 判丢刹车 |
|
||
| 视觉 | `track.is_predicted` | 布尔 | 本拍是否为滤波器预测值（Coast）；`true` → 清距离积分 |
|
||
| 视觉 | `track_time_ms` | 单调时钟 ms | **取样瞬间**打戳；`<= 0` 视为从未取样，直接判超龄 |
|
||
| 距离 | `target_distance_m` | m | 双目目标距离（前向分量 Z）；`<= 0` 视为不可用 |
|
||
| 距离 | `distance_valid` | 布尔 | 本帧测距可用标志：距离来自目标检测框自身的双目测距；**无效帧的距离不得当作目标距离** |
|
||
| 距离 | `distance_time_ms` | 单调时钟 ms | 双目距离取样瞬间打戳 |
|
||
| 姿态 | `attitude_present` | 布尔 | 姿态源当前是否可用；`false` → 禁止控制 |
|
||
| 姿态 | `attitude_time_ms` | 单调时钟 ms | 姿态取样瞬间打戳 |
|
||
| 姿态 | `yaw_rad` | rad (NED) | 自机航向；**参与计算**：位置式航向合成 + 输出端坐标旋转 |
|
||
| 姿态 | `roll_rad` / `pitch_rad` | rad (FRD) | 预留（机体补偿），当前不参与计算 |
|
||
| 姿态 | `yaw_rate_radps` | rad/s | 预留（机体补偿），当前不参与计算 |
|
||
| 自机速度 | `velocity_x/y/z_mps` | m/s | 预留，接入主工程后由飞控状态填充 |
|
||
|
||
**关键约定：追踪结果自身不带时间戳。** 追踪库输出的是"最新一帧的结果"，
|
||
调用方必须在读取结果的**那一瞬间**用单调时钟打戳，写入 `track_time_ms`。
|
||
所有超龄判定一律采用"取样时刻 + 有效期"模型（§7.1），不使用"消息序号"或
|
||
"到达时刻"。
|
||
|
||
**时钟要求：单一单调时钟源。** 快照内三类时间戳与 `Update()` 的 `now_ms`
|
||
必须来自同一个不受墙钟调整影响的时钟（如系统启动后毫秒数）。混用墙钟会
|
||
导致 `now < sample` 的不可核实情形，实现按"宁可降级"处理（§7.1）。
|
||
|
||
### 2.2 输出：控制意图（每拍一个，已限幅）
|
||
|
||
| 字段 | 单位 | 语义 |
|
||
|------|------|------|
|
||
| `mode` | 枚举 | `kVelocityHeading` / `kBrakeHover` / `kDegradedHold` |
|
||
| `valid` | 布尔 | 是否可交付链路发送；`kDegradedHold` 恒为 `false` |
|
||
| `vx_mps` | m/s | **局部 NED 北向**速度（机体系前向经航向旋转得到） |
|
||
| `vy_mps` | m/s | **局部 NED 东向**速度（机体系侧向恒 0，旋转后可非零） |
|
||
| `vz_mps` | m/s | **NED 下正**垂直速度 |
|
||
| `yaw_deg` | deg (NED) | **绝对**目标航向，回绕到 [-180, 180)；仅位置式模式非零 |
|
||
| `yaw_rate_dps` | deg/s | 偏航角速度指令（相对量）；仅速率式模式非零 |
|
||
| `degraded_reason` | 枚举 | `kNone` / `kAttitudeStale` / `kVisionStale` / `kNoDistanceExit` |
|
||
| `coast_active` | 布尔 | 本拍处于 Coast 预测跟踪 |
|
||
|
||
**输出坐标系已在库内完成旋转**：速度分量对外一律是局部 NED（北/东/地），
|
||
与打包时声明的坐标框一致。调用方**不得**再做二次旋转。
|
||
|
||
### 2.3 输出模式与可发送性矩阵
|
||
|
||
| 模式 | `valid` | 速度分量 | 航向字段 | 链路动作 |
|
||
|------|---------|---------|---------|---------|
|
||
| `kVelocityHeading` | `true` | 三路限幅后旋转值 | 按配置的航向模式**二选一**非零 | 发送速度+航向指令 |
|
||
| `kBrakeHover` | `true` | 全零 | 全零（掩码忽略 yaw 与 yaw_rate） | 发送零速刹车 |
|
||
| `kDegradedHold` | `false` | 全零 | 全零 | **不发送**，仅告警/记录降级原因 |
|
||
|
||
### 2.4 传输抽象契约（唯一出向依赖）
|
||
|
||
```text
|
||
接口 CommandTransport:
|
||
Send(data: 字节序列, len: 长度) -> 布尔
|
||
返回 true = 已交付底层链路
|
||
返回 false = 发送失败（调用方可计数/告警，控制律不重试）
|
||
```
|
||
|
||
契约要点：
|
||
|
||
- **只吃字节流**：串口、电台、回环打印、文件录制均可注入实现；换传输层
|
||
不改动打包与控制律；
|
||
- **同步交付**：实现方不得长期持有传入的字节指针（同步拷贝或同步写出），
|
||
因此打包侧可以复用同一块固定缓冲；
|
||
- **无反向通道**：本模块不接收任何入向消息，传输实现无需提供接收能力。
|
||
|
||
---
|
||
|
||
## 3. 核心构件与状态清单
|
||
|
||
控制律由 1 个主控律 + 6 个构件组成。下表列出**每个构件需要持久化的状态**，
|
||
这是重写/移植时最容易漏掉的部分——状态漏了，行为就变了。
|
||
|
||
| 构件 | 职责 | 构造期预计算 | 运行期持久状态 | 复位入口 |
|
||
|------|------|-------------|--------------|---------|
|
||
| 主控律 | 八步流程编排 | 各构件实例、配置值拷贝 | 上拍时刻 `last_now_ms`、上拍模式 `last_mode`、无距离计时（`exit` 门限用，§7.5） | `Reset()` 清全部 |
|
||
| 视线角转换器 ×2 | 像素偏移 → β / α | 针孔系数 `2·tan(θ/2)/size` | 无（纯函数） | 不需要 |
|
||
| 距离 PID | Δd → 前向速度 | PID 参数、输出限幅 | 积分累计、上拍误差、低通后微分值、上次输出、"是否存在有效微分历史"标志 | `ResetIntegral()` 只清积分；`Reset()` 清全部 |
|
||
| 变化率限幅器 ×4 | 限制每拍变化量 | 变化率上限 | 上次输出值 | `Reset(value)` |
|
||
| 静态限幅 | 幅值钳制 | 无 | 无（纯函数） | 不需要 |
|
||
| 超龄判定器 | 输入是否过期 | 无 | 无（纯函数） | 不需要 |
|
||
| 坐标旋转 | 机体系 → NED | 无 | 无（纯函数） | 不需要 |
|
||
|
||
**四个变化率限幅器分别守护什么**：
|
||
|
||
| 限幅器 | 守护量 | 单位 | 生效通道 |
|
||
|--------|-------|------|---------|
|
||
| 航向角限幅器 | 位置式航向修正量的变化率 | deg/s（见 §12.2 量纲待澄清项） | 水平航向（位置式） |
|
||
| 航向角速率限幅器 | 偏航角速度指令的变化率 | deg/s² | 水平航向（速率式，默认） |
|
||
| 前向速度限幅器 | 前向加速度 | m/s² | 距离通道 |
|
||
| 垂直速度限幅器 | 垂直加速度 | m/s² | 垂直通道 |
|
||
|
||
**为什么正好是 4 个**：机体系侧向速度恒为 0（本控制律不做侧移），因此
|
||
`ay_mps2` 配置项当前为预留、无对应限幅器实例。
|
||
|
||
---
|
||
|
||
## 4. 坐标系与符号约定
|
||
|
||
### 4.1 三个坐标系
|
||
|
||
| 坐标系 | 轴定义 | 用途 |
|
||
|--------|-------|------|
|
||
| 像素系 | 原点左上角，x 右正、y 下正 | 追踪库输出的原始语义 |
|
||
| 机体系 (FRD) | x 前、y 右、z 下 | **控制律内部计算**（三路通道各自算出机体量） |
|
||
| 局部 NED | x 北、y 东、z 地 | **对外输出与打包声明** |
|
||
|
||
### 4.2 像素偏移与视线角符号
|
||
|
||
```text
|
||
x_offset = center.x − W/2 # 目标偏右 → 正
|
||
y_offset = center.y − H/2 # 目标偏下 → 正
|
||
|
||
β = 水平视线角（弧度）：与 x_offset 同号 # 目标偏右 → β 正 → 需右转
|
||
α = 垂直视线角（弧度）：与 y_offset 同号 # 目标偏下 → α 正
|
||
```
|
||
|
||
**垂直符号链**（NED 下正约定）：目标偏低 → α 为正 → 期望向下运动 → NED 垂直
|
||
速度 `vz` 为正（下正）。因此垂直通道**不需要额外取反**，增益为正值即可。
|
||
|
||
**水平符号链**（速率式）：目标偏右 → β 为正 → 偏航角速度指令为正。按 NED
|
||
航向"自北顺时针（右转为正）"约定，正角速度即右转，趋近目标。因此水平通道
|
||
同样不需要额外取反。
|
||
|
||
### 4.3 机体系 → NED 旋转（只随航向角）
|
||
|
||
```text
|
||
给定机体速度 (v_fwd, v_right, v_down) 与自机航向 ψ（rad，自北顺时针为正）：
|
||
|
||
v_north = v_fwd·cos ψ − v_right·sin ψ
|
||
v_east = v_fwd·sin ψ + v_right·cos ψ
|
||
v_down = v_down # 绕下轴旋转不影响垂直分量
|
||
```
|
||
|
||
**为什么只随航向旋转、不含滚转/俯仰**：速度设定值的姿态耦合项由飞控内环
|
||
处理，这是 offboard 速度设定值的标准做法；滚转/俯仰对水平速度指令是二阶小量。
|
||
|
||
**正方向自检**（实现时必须通过的两条断言）：
|
||
|
||
| 输入 | ψ | 期望输出 | 说明 |
|
||
|------|---|---------|------|
|
||
| 机体前向 (1, 0, 0) | 90°（机头朝东） | (0, 1, 0) = 东 | 前向随航向转向东 ✓ |
|
||
| 机体右向 (0, 1, 0) | 0°（机头朝北） | (0, 1, 0) = 东 | 机体右侧即地理东 ✓ |
|
||
|
||
### 4.4 限幅与旋转的先后顺序（正确性论证）
|
||
|
||
**顺序：限幅在机体系完成 → 旋转在输出端执行。**
|
||
|
||
论证：绕下轴旋转是**正交变换**，保持向量范数不变，即
|
||
`√(v_north² + v_east²) = √(v_fwd² + v_right²)`。因此机体系内满足的幅值约束
|
||
在旋转后仍然成立，先限幅后旋转**不破坏限幅语义**；反过来（先旋转后限幅）
|
||
则会让限幅作用在地理轴上，与"机体能力限制"的物理含义不符。
|
||
|
||
推论：输出的 `vy_mps` **可以非零**（机体系侧向恒 0，旋转后东向分量非零），
|
||
这不是缺陷，调用方不得把它当作异常。
|
||
|
||
### 4.5 姿态不可用时的处理
|
||
|
||
旋转与位置式航向合成都依赖 `yaw_rad`。因此**姿态过期/缺失必须在任何坐标
|
||
运算之前拦截**（流程第 ① 步），直接进入降级；不允许"用上一拍航向凑合旋转"，
|
||
因为那会产生坐标系标注与实际不符的输出（规格明令禁止混用未标注坐标系数据）。
|
||
|
||
---
|
||
|
||
## 5. 单周期控制流程（主流程伪代码）
|
||
|
||
### 5.1 节拍推算
|
||
|
||
```text
|
||
函数 计算dt(now_ms) -> 秒:
|
||
dt ← 1.0 / 配置.control.frequency_hz # 默认：配置频率兜底
|
||
若 last_now_ms > 0 且 now_ms > last_now_ms: # 时钟正常前进才信任差值
|
||
dt ← min( (now_ms − last_now_ms) / 1000.0, DT_MAX )
|
||
last_now_ms ← now_ms
|
||
返回 dt
|
||
|
||
常量 DT_MAX ← 0.5 秒 # 异常长间隔上限
|
||
```
|
||
|
||
**三条兜底规则的动机**：
|
||
|
||
| 情形 | 处理 | 动机 |
|
||
|------|------|------|
|
||
| 首拍（`last_now_ms == 0`） | 用配置频率推算 dt | 无上拍参考，不允许 dt=0 触发除零 |
|
||
| 时钟回拨或同值（`now <= last`） | 用配置频率推算 dt | 差值不可信；不引入额外状态去纠错 |
|
||
| 间隔异常长（`> 0.5 s`） | 截断到 0.5 s | 防止限幅器一步放过巨大跳变 |
|
||
|
||
### 5.2 八步主流程
|
||
|
||
```text
|
||
函数 Update(快照, now_ms) -> 控制意图:
|
||
dt ← 计算dt(now_ms)
|
||
输出 ← 新建控制意图()
|
||
输出.mode ← kVelocityHeading # 正常路径默认；分支可覆盖为降级/刹车
|
||
|
||
# ---- ① 超龄判定（最高优先级：数据不可信就不算） ----
|
||
若 (非 快照.attitude_present) 或 已超龄(快照.attitude_time_ms, now_ms, 配置.attitude_stale_ms):
|
||
输出.mode ← kDegradedHold; 输出.degraded_reason ← kAttitudeStale
|
||
返回 收尾并记录模式切换(输出, "姿态过期/缺失，禁止控制")
|
||
|
||
若 已超龄(快照.track_time_ms, now_ms, 配置.visual_stale_ms):
|
||
输出.mode ← kDegradedHold; 输出.degraded_reason ← kVisionStale
|
||
返回 收尾并记录模式切换(输出, "视觉追踪结果超龄")
|
||
|
||
# ---- ② 判丢 → 零速刹车 ----
|
||
若 非 快照.track.tracked:
|
||
无距离计时.清除() # 刹车期间不计 kExit 时长
|
||
若 last_mode ≠ kBrakeHover: # 仅在"进入沿"清状态
|
||
距离PID.全清(); 四个限幅器.复位(0)
|
||
输出.mode ← kBrakeHover; 输出.valid ← true # 速度/航向字段保持全零
|
||
返回 收尾并记录模式切换(输出, "判丢，零速刹车")
|
||
|
||
# ---- ③ Coast → 清距离积分 ----
|
||
若 快照.track.is_predicted:
|
||
距离PID.清积分() # 只清积分，保留微分历史
|
||
输出.coast_active ← true
|
||
# 航向通道为纯比例映射，无积分项，无需清理
|
||
|
||
# ---- ④ 像素偏移 → 视线角 ----
|
||
x_offset ← 快照.track.center.x − 配置.image_width / 2
|
||
y_offset ← 快照.track.center.y − 配置.image_height / 2
|
||
β_deg ← 水平视线角转换器(x_offset) × 弧度转度 # 针孔模型，见 §6.1
|
||
α_deg ← 垂直视线角转换器(y_offset) × 弧度转度
|
||
|
||
# ---- ⑤ 水平航向通道 ----
|
||
若 配置.heading.mode == 速率式:
|
||
目标角速率 ← 配置.heading.gain × β_deg
|
||
限速后 ← 航向角速率限幅器.推进(目标角速率, dt) # 先变化率
|
||
输出.yaw_rate_dps ← 钳制(限速后, ±配置.yaw_rate_limit_dps) # 再幅值
|
||
否则: # 位置式
|
||
目标修正量 ← 配置.heading.gain × β_deg
|
||
修正量 ← 航向角限幅器.推进(目标修正量, dt)
|
||
# ID 84 的 yaw 字段是"绝对航向"，必须与当前航向合成，不能直发修正量
|
||
输出.yaw_deg ← 回绕180( 快照.yaw_rad × 弧度转度 + 修正量 )
|
||
|
||
# ---- ⑥ 垂直速度通道（统一速度支路） ----
|
||
目标垂直速度 ← 配置.vertical.vz_gain_mps_per_deg × α_deg
|
||
限速后 ← 垂直速度限幅器.推进(目标垂直速度, dt) # 加速度限幅
|
||
输出.vz_mps ← 钳制(限速后, ±配置.vertical.vz_limit_mps)
|
||
|
||
# ---- ⑦ 距离通道 ----
|
||
距离可用 ← 快照.distance_valid
|
||
且 快照.target_distance_m > 0
|
||
且 非 已超龄(快照.distance_time_ms, now_ms, 配置.distance_stale_ms)
|
||
|
||
若 距离可用:
|
||
无距离计时.清除() # 任一有效帧重置 kExit 计时
|
||
Δd ← 快照.target_distance_m − 配置.distance.d_exp_m # 正=偏远，需接近
|
||
前向原始 ← 距离PID.更新(Δd, dt)
|
||
限速后 ← 前向速度限幅器.推进(前向原始, dt) # 加速度限幅
|
||
# 非对称限幅：接近可以快，后退必须慢
|
||
输出.vx_mps ← 钳制(限速后, −配置.retreat_velocity_limit_mps,
|
||
+配置.approach_velocity_limit_mps)
|
||
否则:
|
||
距离PID.清积分() # 防饱和
|
||
按 配置.distance.no_distance_action 分支:
|
||
保持: 输出.vx_mps ← 0; 前向速度限幅器.复位(0)
|
||
限速接近: 输出.vx_mps ← 前向速度限幅器.推进(配置.no_distance_approach_limit_mps, dt)
|
||
退出近距操作:
|
||
若 非 无距离计时.进行中: 无距离计时.启动(now_ms) # 首个无效拍起计时
|
||
若 now_ms − 无距离计时.起始时刻 ≥ 配置.no_distance_exit_grace_ms:
|
||
输出.mode ← kDegradedHold
|
||
输出.degraded_reason ← kNoDistanceExit
|
||
返回 收尾并记录模式切换(输出, "持续无距离达门限，退出近距操作")
|
||
# grace 期内按"限速接近"执行（复用加速度限幅，vx 平滑无跳变）
|
||
输出.vx_mps ← 前向速度限幅器.推进(配置.no_distance_approach_limit_mps, dt)
|
||
|
||
# ---- ⑧ 机体系 → 局部 NED 旋转（输出端） ----
|
||
旋转机体到NED(输出.vx_mps, 输出.vy_mps, 输出.vz_mps, 快照.yaw_rad)
|
||
|
||
输出.valid ← true
|
||
返回 收尾并记录模式切换(输出, 输出.coast_active ? "Coast 预测跟踪" : "正常跟踪")
|
||
```
|
||
|
||
### 5.3 提前返回的语义差异（易错点）
|
||
|
||
三条提前返回路径**跳过第 ⑧ 步旋转**，这是安全的：这些路径的速度分量全为零，
|
||
零向量旋转后仍是零向量。但重写时必须注意：
|
||
|
||
| 返回路径 | 是否清 PID | 是否清限幅器 | 速度是否旋转 | `valid` |
|
||
|---------|-----------|------------|------------|---------|
|
||
| 姿态超龄降级 | 否 | 否 | 否 | `false` |
|
||
| 视觉超龄降级 | 否 | 否 | 否 | `false` |
|
||
| 判丢刹车 | 是（进入沿） | 是（进入沿） | 否 | `true` |
|
||
| 无距离退出（grace 期满） | 是（仅积分） | 否 | 否 | `false` |
|
||
| 正常/Coast | 视情况清积分 | 否 | **是** | `true` |
|
||
|
||
**降级路径不清状态的理由**：降级只是"这一拍不输出"，输入恢复后应当接着上次的
|
||
积分/限幅历史平滑续接；若每次降级都清状态，输入抖动会导致指令反复从零起爬。
|
||
|
||
**刹车路径必须清状态的理由**：判丢是一次真实的控制中断，恢复跟踪时的首条指令
|
||
必须从零渐变（避免位置→速度切换突变），因此积分与全部限幅器历史归零。
|
||
|
||
### 5.4 模式切换日志（唯一日志点）
|
||
|
||
```text
|
||
函数 收尾并记录模式切换(输出, 原因文本) -> 控制意图:
|
||
若 输出.mode ≠ last_mode:
|
||
记录 INFO 日志: "[ControlLaw] 状态切换: {旧模式名} -> {新模式名} ({原因文本})"
|
||
last_mode ← 输出.mode
|
||
返回 输出
|
||
```
|
||
|
||
同一模式连续多拍**不重复打印**。这是本模块唯一的运行期日志；每拍的三路通道
|
||
数学计算不打任何日志。
|
||
|
||
---
|
||
|
||
## 6. 各通道算法细节
|
||
|
||
### 6.1 像素 → 视线角：全程针孔模型
|
||
|
||
```text
|
||
构造期（水平、垂直各一个实例）:
|
||
校验 size_px > 0，否则报参数错误
|
||
校验 0 < fov_rad < π，否则报参数错误
|
||
系数 ← 2 · tan(fov_rad / 2) / size_px # 预计算，热路径不再算 tan
|
||
|
||
每拍:
|
||
视线角(弧度) ← atan( offset_px × 系数 )
|
||
```
|
||
|
||
| 通道 | size | fov | 含义 |
|
||
|------|------|-----|------|
|
||
| 水平 | `image_width` (W) | `fov_h_deg` (θ_h) | 偏移 → β |
|
||
| 垂直 | `image_height` (H) | `fov_v_deg` (θ_v) | 偏移 → α |
|
||
|
||
**为什么不用线性近似**：线性式 `angle ≈ offset · θ/size` 只在小偏移成立。
|
||
若做"小偏移走线性、大偏移走针孔"的双路切换，两式在切换边界**函数值不相等**
|
||
（θ=60°、边界比例 0.2 处约 8% 角度台阶），目标跨越边界的那一拍会向控制环路
|
||
注入一次角度突变 → 限幅器被迫吸收 → 机体抽动。`atan` 的计算代价在此频率下
|
||
可忽略，故全程使用精确针孔式，配置项也随之少一个（原 `linear_ratio` 已移除）。
|
||
|
||
**未建模项（红线相关）**：镜头畸变、相机安装外参（俯仰/偏航安装角）、
|
||
像素非正方形均未补偿。标定完成前，视线角存在系统性偏差。
|
||
|
||
### 6.2 水平航向通道
|
||
|
||
**两种输出映射，二选一，由配置决定**：
|
||
|
||
| 模式 | 输出字段 | 量纲 | 优点 | 风险 |
|
||
|------|---------|------|------|------|
|
||
| **速率式（默认）** | `yaw_rate_dps` | (deg/s)/deg = 1/s | 指令天然是**相对量**，无坐标系歧义；不依赖自机航向精度 | 稳态需 β→0 才停转，存在小幅极限环可能 |
|
||
| 位置式 | `yaw_deg` | deg/deg（无量纲） | 稳态无累积漂移 | 输出是**绝对 NED 航向**，必须与当前航向合成；依赖新鲜姿态 |
|
||
|
||
**限幅顺序：先变化率、后幅值**（不可颠倒）：
|
||
|
||
```text
|
||
目标 ← 增益 × β_deg
|
||
限速 ← 变化率限幅器.推进(目标, dt) # 限制"每拍能变多少"
|
||
输出 ← 钳制(限速, ±幅值上限) # 限制"最大能到多少"
|
||
```
|
||
|
||
颠倒顺序会导致：幅值限幅先切掉超限部分，变化率限幅器记录的"上次输出"变成
|
||
被切过的值，下一步以此为基准继续推进，实际变化率可超过配置上限。
|
||
|
||
**位置式必须合成绝对航向**：链路消息的 `yaw` 字段语义是绝对 NED 航向。
|
||
若直接把修正量填进去，等于命令机体"转向修正角那个方向"（例如修正量 5° 会被
|
||
理解为"转向北偏东 5°"），与当前航向无关——这是典型的坐标系语义误用。正确做法：
|
||
|
||
```text
|
||
输出.yaw_deg ← 回绕180( 当前航向(度) + 限幅后修正量 )
|
||
|
||
函数 回绕180(deg):
|
||
d ← fmod(deg + 180, 360)
|
||
若 d < 0: d ← d + 360
|
||
返回 d − 180 # 结果落在 [-180, 180)
|
||
```
|
||
|
||
### 6.3 垂直通道：统一速度支路
|
||
|
||
```text
|
||
目标垂直速度 ← vz_gain_mps_per_deg × α_deg
|
||
限速 ← 垂直速度限幅器.推进(目标垂直速度, dt) # 即垂直加速度限幅
|
||
输出.vz_mps ← 钳制(限速, ±vz_limit_mps)
|
||
```
|
||
|
||
**小误差自然映射为小垂直速度**：α = 1° 且增益 0.1 → vz = 0.1 m/s，行为连续，
|
||
不存在"小误差走一条支路、大误差走另一条支路"的模式翻转。原设计的双支路
|
||
（小误差发姿态微调消息）已取消，理由见 §10.1。
|
||
|
||
### 6.4 距离通道
|
||
|
||
```text
|
||
Δd ← 实测距离 − 期望射程 d_exp
|
||
Δd > 0：目标比期望远 → 前向接近（vx 为正）
|
||
Δd < 0：目标比期望近 → 减速/后退（vx 为负）
|
||
|
||
vx ← 非对称钳制( 加速度限幅( PID(Δd) ), −后退限速, +接近限速 )
|
||
```
|
||
|
||
**非对称限幅的物理动机**：接近方向允许较快（追击需要），后退方向必须保守
|
||
（后退意味着放弃射程优势，且后方通常未观测）。默认占位值 3 m/s vs 1 m/s。
|
||
|
||
**PID 输出限幅取两者较大值**：PID 内部用对称限幅 `±max(接近, 后退)` 做
|
||
anti-windup 判据，外部再做非对称钳制。这样内部限幅不会先于外部约束生效，
|
||
保证 anti-windup 判据基于"真实饱和边界"。
|
||
|
||
**可用性三重门**（缺一不可）：`distance_valid` 成立 **且** 距离为正 **且** 未超龄。
|
||
双目测距取自目标检测框自身，不存在"回波打到别处"的跨模态关联问题；标志为
|
||
`false` 表示本帧测距不可用（有效域外/未匹配/一致性校验失败），不得把任何数值
|
||
当作目标距离。
|
||
|
||
### 6.5 PID 内部算法
|
||
|
||
```text
|
||
函数 更新(error, dt) -> 输出:
|
||
若 dt <= 0: 返回 上次输出 # 异常节拍保护
|
||
|
||
# ---- 积分：候选值先钳位 ----
|
||
积分候选 ← 积分累计 + error × dt
|
||
若 积分限幅 > 0: 积分候选 ← 钳制(积分候选, ±积分限幅)
|
||
|
||
# ---- 微分：一阶低通 ----
|
||
若 存在有效微分历史:
|
||
微分原始 ← (error − 上拍误差) / dt
|
||
微分 ← 滤波系数 × 上次滤波微分 + (1 − 滤波系数) × 微分原始
|
||
否则:
|
||
微分 ← 0 # 首拍不做微分（避免冲击）
|
||
上次滤波微分 ← 微分; 上拍误差 ← error; 存在有效微分历史 ← true
|
||
|
||
# ---- 位置式合成 ----
|
||
原始输出 ← kp·error + ki·积分候选 + kd·微分
|
||
|
||
# ---- anti-windup：条件积分 ----
|
||
已饱和 ← (输出限幅 > 0) 且 |原始输出| > 输出限幅
|
||
增量推深饱和 ← (error × dt) × 原始输出 > 0 # 本拍积分增量方向与饱和同向
|
||
若 非 (已饱和 且 增量推深饱和):
|
||
积分累计 ← 积分候选 # 否则冻结本拍累加
|
||
|
||
# ---- 输出限幅 ----
|
||
上次输出 ← (输出限幅 > 0) ? 钳制(原始输出, ±输出限幅) : 原始输出
|
||
返回 上次输出
|
||
```
|
||
|
||
**条件积分判据的三种候选与取舍**（这是 anti-windup 最容易写错的地方）：
|
||
|
||
| 判据 | 饱和时行为 | 误差反号后 | 结论 |
|
||
|------|-----------|-----------|------|
|
||
| `积分项与输出同向` | 冻结 | **仍冻结**（积分残留同号）→ 无法回卷 | ✗ 会留下饱和残留 |
|
||
| `原始输出 × error > 0` | 冻结 | 可回卷，但大 kp 项会掩盖饱和方向 → 回卷被拖延 | △ 不够干脆 |
|
||
| **`(error × dt) × 原始输出 > 0`** | 冻结 | 增量立即反向 → **首拍即回卷** | ✓ 采用 |
|
||
|
||
**两种复位的语义差别**（必须分开实现）：
|
||
|
||
| 方法 | 清空内容 | 保留内容 | 使用场景 |
|
||
|------|---------|---------|---------|
|
||
| 清积分 | 积分累计 | 微分历史、上次输出 | Coast 每拍、距离不可用每拍 |
|
||
| 全清 | 积分、上拍误差、滤波微分、上次输出、微分历史标志 | 参数 | 判丢进入刹车、停止视觉闭环 |
|
||
|
||
Coast 只清积分而不清微分：预测值仍在连续演化，微分历史有效，清掉会让微分项
|
||
在恢复跟踪瞬间失去参考、产生一次冲击。
|
||
|
||
### 6.6 变化率限幅器
|
||
|
||
```text
|
||
函数 推进(target, dt) -> 输出:
|
||
若 dt <= 0 或 变化率上限 <= 0: 返回 当前输出 # 异常节拍/冻结保护
|
||
最大步长 ← 变化率上限 × dt
|
||
当前输出 ← 钳制(target, 当前输出 − 最大步长, 当前输出 + 最大步长)
|
||
返回 当前输出
|
||
```
|
||
|
||
**两个必须知道的性质**：
|
||
|
||
1. **不可能超调**：输出被钳制在以"上次输出"为中心、`±最大步长` 为半径的区间内，
|
||
因此单步变化严格不超过配置上限。调试时若观察到"限幅后数值超过目标"，
|
||
一定是别处的问题，不是限幅器超调。
|
||
2. **初值为 0 即天然实现首条指令渐变**：构造时输出置 0，切换进入视觉跟踪后的
|
||
第一条指令会从 0 按加速度限幅爬升，**不需要专门写切换分支**。这也是"判丢
|
||
刹车时把限幅器复位到 0"的原因——复位后恢复跟踪同样从零爬升。
|
||
|
||
---
|
||
|
||
## 7. 安全语义：超龄、判丢、Coast、无距离
|
||
|
||
### 7.1 超龄判定（纯函数，三条判据）
|
||
|
||
```text
|
||
函数 已超龄(取样时刻, 当前时刻, 有效期) -> 布尔:
|
||
若 取样时刻 <= 0: 返回 true # 从未取样
|
||
若 当前时刻 < 取样时刻: 返回 true # 时钟源错配/打戳错误，年龄不可信
|
||
返回 (当前时刻 − 取样时刻) > 有效期
|
||
```
|
||
|
||
**第二条判据是"宁可降级"的显式表达**：单一单调时钟下不应出现 `now < sample`。
|
||
一旦出现，说明取样与判定用了不同时钟源（或打戳位置错误），此时年龄**不可核实**，
|
||
不能因为"看起来没超龄"就继续用它算正常指令。
|
||
|
||
**三个有效期独立配置**，反映各输入的更新特性：
|
||
|
||
| 输入 | 有效期占位值 | 依据 |
|
||
|------|-------------|------|
|
||
| 视觉追踪结果 | 200 ms | 与推理帧率/追踪输出节拍匹配，约 4 拍（20 Hz） |
|
||
| 自机姿态 | 300 ms | 姿态链路更新较快，但缺失即禁止坐标运算 |
|
||
| 双目距离 | 500 ms | 双目随推理帧更新，且距离不可用有独立降级动作 |
|
||
|
||
### 7.2 降级优先级（顺序即语义）
|
||
|
||
```text
|
||
姿态不可用 > 视觉超龄 > 判丢 > 无距离（exit 档，持续达门限）
|
||
```
|
||
|
||
| 优先级 | 检查 | 理由 |
|
||
|-------|------|------|
|
||
| 1 | 姿态过期/缺失 | 连坐标旋转与绝对航向合成都做不了，任何空间量都不可信（规格：禁止空间转换和控制） |
|
||
| 2 | 视觉超龄 | 视觉是"是否判丢"的唯一来源；数据过期时连 `tracked` 标志本身都不可信，因此**必须先于判丢**处理 |
|
||
| 3 | 判丢 | 视觉可信且明确报告无轨道 → 零速刹车 |
|
||
| 4 | 无距离 | 只有在"视觉可信且正在跟踪"前提下才谈得上距离降级；`exit` 档还需持续无距离达门限（§7.5），默认档为 `slow_approach`（§10.8） |
|
||
|
||
### 7.3 判丢刹车与状态清理
|
||
|
||
```text
|
||
触发: tracked == false（且视觉未超龄、姿态可用）
|
||
动作: 输出 kBrakeHover（速度全零、航向全零）、valid = true
|
||
进入沿清：距离 PID 全清 + 四个限幅器复位到 0
|
||
```
|
||
|
||
**为什么用"进入沿"而不是每拍清**：每拍清会把限幅器输出反复钉在 0，虽然结果
|
||
相同，但会掩盖"进入刹车"这一事件；用进入沿判定还能保证只清一次，语义清晰。
|
||
判定依据是"上拍模式 ≠ 刹车"，因此不需要额外的布尔标志。
|
||
|
||
**为什么刹车要 `valid = true`**：刹车是一条**真实要发出去的指令**（零速），
|
||
与降级"什么都不发"有本质区别。
|
||
|
||
### 7.4 Coast（预测跟踪）
|
||
|
||
```text
|
||
触发: is_predicted == true（追踪库用滤波器预测值补帧）
|
||
动作: 输出照常计算（目标中心为预测值），仅清距离 PID 积分；coast_active = true
|
||
```
|
||
|
||
**为什么 Coast 期间仍照常算航向与垂直**：Coast 是短暂补帧（数帧量级），预测中心
|
||
连续可用，此时停止控制反而会造成机体停顿—恢复的抖动。清积分的理由是：预测期间
|
||
误差不反映真实世界，积分累加会把预测偏差固化成饱和量。
|
||
|
||
**Coast 期间距离通常不可用**：目标短暂消失时双目失去匹配（`distance_valid ==
|
||
false`），第 ⑦ 步走无距离分支；默认 `slow_approach` 下前向按低速定速推进，
|
||
方位闭环继续（§7.5）。这是设计上的自然降级，不需要额外分支。
|
||
|
||
### 7.5 无距离三档动作
|
||
|
||
| 配置值 | 行为 | 适用场景 |
|
||
|--------|------|---------|
|
||
| `slow_approach`（默认） | 以配置的低速限幅缓速接近（仍经加速度限幅） | **默认档**：双目是机载唯一距离源，有效域外无距离是常态而非异常；即远场策略"方位闭环 + 定速接近"（D4） |
|
||
| `hold` | 前向速度清零，限幅器复位到 0 | 安全占位语义：不知道距离就不前冲 |
|
||
| `exit` | 持续无距离达 `no_distance_exit_grace_ms` 才整体降级保持（`kDegradedHold` / `kNoDistanceExit`），不发送 | 距离是安全前提（如避撞）时的可配置安全策略；持续无距离 ≈ 传感器级故障 |
|
||
|
||
三档中只有 `exit` 会中断整拍输出，且**延迟到 grace 期满**；其余情况（含 `exit`
|
||
的 grace 期）仍输出航向与垂直通道（继续把目标保持在画面中心）。
|
||
|
||
**`exit` 门限语义**（替代旧版"首个无效拍即退出"）：
|
||
|
||
- `distance_available` 首次为 `false` 起计时，任一有效距离帧**重置**计时；
|
||
- grace 期内前向按 `slow_approach` 低速推进，复用既有加速度限幅，`vx` 平滑
|
||
过渡无跳变；
|
||
- 判丢刹车期间不计时（每拍清计时状态），`Reset()` 同样清除；
|
||
- `no_distance_exit_grace_ms = 0` 保留旧版行为（首个无效拍即退出）。
|
||
|
||
---
|
||
|
||
## 8. 输出打包与链路
|
||
|
||
### 8.1 控制意图 → 消息映射
|
||
|
||
| 模式 | 消息 | 关键填法 |
|
||
|------|------|---------|
|
||
| `kVelocityHeading` | ID 84 `SET_POSITION_TARGET_LOCAL_NED`（速度模式） | 位置三字段填 0 并被掩码忽略；加速度三字段填 0 并被掩码忽略；速度填 NED 三分量；航向按掩码三分支二选一 |
|
||
| `kBrakeHover` | ID 84（同上，全零速度） | 速度全零，位置/加速度/航向/航向角速率字段**全部**被掩码忽略 |
|
||
| `kDegradedHold` | —— | **不打包、不发送** |
|
||
|
||
当前实现已收敛为**只使用一种消息类型**（ID 84）。原设计中的姿态目标消息
|
||
（ID 82）随姿态微调支路一并取消（§10.1）。
|
||
|
||
### 8.2 忽略掩码语义与位定义
|
||
|
||
掩码语义：**置位 = 忽略该字段**。基础位组合（速度模式必备）：
|
||
|
||
```text
|
||
基础掩码 = 忽略位置 x/y/z + 忽略加速度 afx/afy/afz
|
||
= 1 + 2 + 4 + 64 + 128 + 256 = 455 = 0x01C7
|
||
```
|
||
|
||
航向两位取自**本仓库随附的 MAVLink 定义**（v3，`common.xml` / `common.h`）：
|
||
|
||
| 枚举 | 值 | 位序 |
|
||
|------|---|------|
|
||
| `YAW_IGNORE` | 1024 = `0x0400` | bit 10 |
|
||
| `YAW_RATE_IGNORE` | 2048 = `0x0800` | bit 11 |
|
||
|
||
> ⚠️ **版本偏移风险（必须实测确认）**：这两个值之所以是 1024/2048，是因为该
|
||
> 版本的枚举在 `AZ_IGNORE=256` 之后插入了 `FORCE_SET=512`。**较旧的 MAVLink
|
||
> 版本没有 `FORCE_SET`，其 `YAW_IGNORE=512`、`YAW_RATE_IGNORE=1024`**，
|
||
> 位序整体低一位。若飞控端（PX4 1.11.3）随附的是旧枚举，本模块发出的掩码会被
|
||
> 解读为"使用 yaw、忽略 yaw_rate"，即把速率式指令误解为绝对航向 0（指向正北）。
|
||
> **上线前必须在飞控端核对枚举值，或在链路侧抓包比对行为。**
|
||
|
||
### 8.3 三个掩码与三分支选择
|
||
|
||
| 掩码 | 组成 | 语义 |
|
||
|------|------|------|
|
||
| 速度+航向角 | 基础 \| `0x0800` | 使用 `yaw`，忽略 `yaw_rate`（位置式） |
|
||
| 速度+角速率 | 基础 \| `0x0400` | 使用 `yaw_rate`，忽略 `yaw`（速率式，默认） |
|
||
| 刹车 | 基础 \| `0x0C00` | `yaw` 与 `yaw_rate` 均忽略 |
|
||
|
||
**分支选择规则（含"两者皆零"的处理）**：
|
||
|
||
```text
|
||
有角速率 ← |yaw_rate_dps| > 极小阈值
|
||
有航向角 ← |yaw_deg| > 极小阈值
|
||
|
||
若 (非 有角速率) 且 有航向角: 用 速度+航向角掩码
|
||
否则: 用 速度+角速率掩码
|
||
```
|
||
|
||
**为什么"两者皆零"必须落到角速率掩码**：此时若选航向角掩码，`yaw = 0` 会被
|
||
飞控当作**有效绝对航向 0（正北）**执行——机体无故转向正北。落到角速率掩码配
|
||
零角速率，语义是"保持当前航向"，才是期望行为。这是速率式默认配置下的常态
|
||
路径（β≈0 时角速率自然为 0），影响面很大。
|
||
|
||
### 8.4 单位换算清单（打包边界上的全部换算）
|
||
|
||
| 量 | 控制律内部 | 链路上 | 换算 |
|
||
|----|-----------|-------|------|
|
||
| 航向角 | deg | rad | × π/180 |
|
||
| 偏航角速率 | deg/s | rad/s | × π/180 |
|
||
| 速度 | m/s | m/s | 无 |
|
||
| 时间戳 | 单调 ms | 系统启动后 ms | 直接传入（由调用方保证语义一致） |
|
||
|
||
**内部统一用度、边界统一转弧度**的理由：控制参数（视场角、增益、限幅）在
|
||
度单位下便于人工整定与阅读；弧度只是链路协议要求。换算点集中在打包层，
|
||
控制律内部不出现弧度制航向。
|
||
|
||
### 8.5 打包与传输分离
|
||
|
||
```text
|
||
控制律 ──控制意图──▶ 打包网关 ──字节流──▶ 传输抽象 ──▶ 物理链路
|
||
（懂协议） （只懂字节）
|
||
```
|
||
|
||
分离带来的三个好处：
|
||
|
||
1. **可回放/可录制**：把传输实现换成"写文件"即可离线录制全量指令流，
|
||
换成"回环打印"即可台架观测，控制律零改动；
|
||
2. **链路可替换**：串口 ↔ 电台 ↔ 网络透明切换；
|
||
3. **协议知识集中**：所有 MAVLink 细节只存在于打包层一处，控制律与配置层
|
||
完全不感知协议。
|
||
|
||
**与实现语言相关的工程约束**（重写时可忽略，但当前 C++ 落地必须遵守）：
|
||
随附的 MAVLink C 库把辅助函数以外部链接直接定义在头文件中（`MAVLINK_HELPER`
|
||
默认为空宏），**多个编译单元包含同一协议头会产生重复定义链接错误**。因此
|
||
协议头只允许被打包层那**一个**编译单元包含，对外头文件不得出现任何协议类型，
|
||
打包/发送缓冲以固定尺寸字节存储承载并在编译期断言尺寸足够。
|
||
|
||
### 8.6 发送节奏
|
||
|
||
- 控制频率由配置给定（占位 20 Hz），**每拍一条**指令；
|
||
- offboard 模式要求指令流**持续**：断流超过飞控侧超时阈值会触发 offboard
|
||
失效保护。因此 `kDegradedHold`（不发送）会让飞控自行进入失效保护——
|
||
这是有意的"交还控制权"语义，但见 §12.2 的待确认项；
|
||
- 本模块不做重传、不做发送失败重试；发送失败仅返回布尔由调用方计数告警。
|
||
|
||
---
|
||
|
||
## 9. 配置参数全量表
|
||
|
||
配置在**启动期一次性加载**，运行期只读。缺字段、类型错误、非法取值一律
|
||
在加载期报错并**携带完整字段路径**（如 `heading.gain`），不允许带着半套
|
||
默认值进入运行期。
|
||
|
||
### 9.1 control 组：周期与超龄
|
||
|
||
| 键 | 单位 | 占位值 | 作用 |
|
||
|----|------|-------|------|
|
||
| `frequency_hz` | Hz | 20.0 | 控制频率；同时作为 dt 兜底值 |
|
||
| `visual_stale_ms` | ms | 200 | 视觉结果超龄阈值 |
|
||
| `distance_stale_ms` | ms | 500 | 双目距离超龄阈值 |
|
||
| `attitude_stale_ms` | ms | 300 | 自机姿态超龄阈值 |
|
||
|
||
### 9.2 camera 组：画面与视场
|
||
|
||
| 键 | 单位 | 占位值 | 作用 |
|
||
|----|------|-------|------|
|
||
| `image_width` | px | 1920 | 画面宽 W（β 计算与 x 偏移基准） |
|
||
| `image_height` | px | 1080 | 画面高 H（α 计算与 y 偏移基准） |
|
||
| `fov_h_deg` | deg | 60.0 | 水平视场角 θ_h |
|
||
| `fov_v_deg` | deg | 35.0 | 垂直视场角 θ_v |
|
||
|
||
> 画面尺寸必须与追踪库实际使用的画面尺寸一致；若推理在缩放图上运行，
|
||
> 这里填**缩放后**的尺寸，否则视线角会系统性偏差。
|
||
|
||
### 9.3 heading 组：水平航向
|
||
|
||
| 键 | 单位 | 占位值 | 作用 |
|
||
|----|------|-------|------|
|
||
| `mode` | 枚举 | `rate` | `rate` = 速率式（默认）/ `position` = 位置式 |
|
||
| `gain` | 见 §6.2 | 1.0 | 视线角 → 航向指令增益 |
|
||
| `yaw_rate_limit_dps` | deg/s | 30.0 | 偏航角速度幅值上限 |
|
||
| `yaw_slew_limit_dps2` | deg/s² | 60.0 | 航向指令变化率上限（两种模式共用，量纲待澄清） |
|
||
|
||
### 9.4 vertical 组：垂直
|
||
|
||
| 键 | 单位 | 占位值 | 作用 |
|
||
|----|------|-------|------|
|
||
| `vz_gain_mps_per_deg` | (m/s)/deg | 0.1 | 视线角 → 垂直速度增益 |
|
||
| `vz_limit_mps` | m/s | 2.0 | 垂直速度幅值上限 |
|
||
|
||
> 原双支路所需的"切换阈值"与"姿态微调增益/限幅"三项已随支路取消而移除。
|
||
|
||
### 9.5 distance 组：距离
|
||
|
||
| 键 | 单位 | 占位值 | 作用 |
|
||
|----|------|-------|------|
|
||
| `d_exp_m` | m | 10.0 | 期望射程（由拦截设备接口给定） |
|
||
| `kp` / `ki` / `kd` | —— | 0.5 / 0.05 / 0.1 | PID 三增益 |
|
||
| `integral_limit` | m·s | 2.0 | 积分项限幅（<=0 表示不限幅） |
|
||
| `derivative_filter_coef` | ∈[0,1) | 0.2 | 微分一阶低通系数（0 = 不滤波） |
|
||
| `approach_velocity_limit_mps` | m/s | 3.0 | 接近速度上限 |
|
||
| `retreat_velocity_limit_mps` | m/s | 1.0 | 后退速度上限 |
|
||
| `no_distance_action` | 枚举 | `slow_approach` | `hold` / `slow_approach` / `exit`（默认档依据见 §10.8） |
|
||
| `no_distance_approach_limit_mps` | m/s | 0.5 | `slow_approach` 档（含 `exit` 的 grace 期）的限速值 |
|
||
| `no_distance_exit_grace_ms` | ms | 3000 | `exit` 档持续无距离门限（0 = 首个无效拍即退出） |
|
||
|
||
### 9.6 accel_limit 组：加速度
|
||
|
||
| 键 | 单位 | 占位值 | 作用 |
|
||
|----|------|-------|------|
|
||
| `ax_mps2` | m/s² | 2.0 | 前向加速度上限（距离通道限幅器） |
|
||
| `ay_mps2` | m/s² | 2.0 | 侧向（**预留**：本控制律不做侧移，无对应限幅器） |
|
||
| `az_mps2` | m/s² | 1.5 | 垂直加速度上限（垂直通道限幅器） |
|
||
|
||
---
|
||
|
||
## 10. 设计取舍记录（为什么不是别的做法）
|
||
|
||
### 10.1 取消垂直"姿态微调"双支路，统一走速度支路
|
||
|
||
**原设计**：垂直误差小于切换阈值时发**姿态目标消息**做微调，大于阈值时发
|
||
**速度设定值**。
|
||
|
||
**取消理由（三条，任一条都足以否决）**：
|
||
|
||
1. **丢弃同周期的距离通道输出**：姿态消息里没有前向速度字段。一旦本拍切成
|
||
姿态消息，距离通道算出的 `vx` 就被整拍丢弃 → 目标在画面中心附近时逼近
|
||
**停滞**（而"目标居中"恰恰是最常见的稳态）。
|
||
2. **消息类型逐拍翻转扰动飞控**：目标在切换阈值附近抖动时，offboard 会收到
|
||
交替的两种消息类型，飞控侧设定值来源反复切换，行为难以预测。
|
||
3. **需要额外整定一套参数**：切换阈值、微调增益、姿态角限幅、姿态角速率限幅
|
||
四项全部需要标定，而收益仅是"小误差时更细腻"。
|
||
|
||
**替代方案的效果**：小误差经增益线性映射为小垂直速度（1° → 0.1 m/s），行为
|
||
连续、无模式翻转、全周期单一消息类型，且少 4 个待标定参数。
|
||
|
||
### 10.2 航向默认改为速率式
|
||
|
||
**理由**：速率式指令是**相对量**，不存在"这是绝对航向还是修正量"的坐标系歧义；
|
||
位置式的 `yaw` 字段是绝对 NED 航向，一旦合成环节出错（漏加当前航向、姿态过期
|
||
用了旧航向），后果是机体转向一个完全无关的地理方向——**故障模式危险且不直观**。
|
||
速率式最坏情况是转速不对，不会指向错误方位。
|
||
|
||
**代价**：稳态需 β→0 才停转，可能有小幅极限环（可通过死区或降低增益抑制）。
|
||
位置式保留为可配置选项，但**姿态接线未验证前不得用于实飞**。
|
||
|
||
### 10.3 输出端补齐机体系 → NED 旋转
|
||
|
||
**原状态**：速度分量以机体系语义直发，而打包声明的坐标框是局部 NED——
|
||
**声明与实际不符**。机头不朝北时，"前向 3 m/s"会被飞控理解为"北向 3 m/s"，
|
||
机体航向一变，速度方向就错。
|
||
|
||
**修正**：在控制律输出端（第 ⑧ 步）随快照航向旋转为 NED，使对外语义与打包
|
||
声明一致。限幅仍在机体系完成（正交变换保范，见 §4.4）。
|
||
|
||
### 10.4 视线角全程针孔、取消线性双路
|
||
|
||
见 §6.1：双路切换边界处约 8% 角度台阶，会向控制环注入突变；`atan` 代价可忽略。
|
||
同时消除了一个待标定参数。
|
||
|
||
### 10.5 PID anti-windup 采用"增量方向"条件积分
|
||
|
||
见 §6.5 判据对比表：另两种判据要么导致误差反号后无法回卷（积分残留），
|
||
要么被大比例项掩盖饱和方向、拖延回卷。
|
||
|
||
### 10.6 限幅器初值为 0 兼作首条指令突变抑制
|
||
|
||
见 §6.6：不需要为"位置模式 → 速度模式切换"写专门分支，限幅器从零起步天然
|
||
满足"首条视觉速度设定值必须经过限幅"的要求；判丢刹车时复位到 0，恢复跟踪
|
||
同样从零爬升。
|
||
|
||
### 10.7 降级路径不清状态、刹车路径清状态
|
||
|
||
见 §5.3：降级是"这一拍不输出"，恢复后应平滑续接；刹车是真实控制中断，
|
||
恢复后必须从零渐变。两者语义不同，清理策略必须不同。
|
||
|
||
### 10.8 距离输入从雷达切换为双目：语义简化、默认动作与退出门限（2026-09-10）
|
||
|
||
**背景**：目标距离的机载唯一来源改为双目立体测距（决策 D2，替代激光雷达；
|
||
见 `docs/双目深度估计思路.md`）。
|
||
|
||
1. **关联标志语义简化（S2）**：`associated_to_target`（相机—雷达空间关联）
|
||
→ `distance_valid`（本帧测距可用）。双目距离来自目标检测框自身，"回波未
|
||
打到目标"的跨模态关联问题消失，标志退化为纯粹的本帧可用性（域外/未匹配/
|
||
一致性校验失败）；track_id 一致性校验转为调用方约定（§12.2）。
|
||
2. **默认无距离动作 `hold` → `slow_approach`（D4）**：单雷达时代无距离多为
|
||
瞬时丢关联，`hold` 占位安全；双目时代有效域外无距离是**常态**，`hold` 会让
|
||
远场逼近停滞。`slow_approach` 即"方位闭环 + 定速接近"远场策略的库内承载。
|
||
3. **`exit` 增加持续门限（S1）**：旧版首个无效拍即降级，会把单帧丢检误判为
|
||
传感器级故障；改为持续无距离达 `no_distance_exit_grace_ms` 才退出，grace 期
|
||
按 `slow_approach` 行为执行（复用加速度限幅，无跳变），任一有效帧重置。
|
||
`exit` 语义收敛为"持续无距离 ≈ 传感器级故障"。
|
||
|
||
---
|
||
|
||
## 11. 实现与重写指引
|
||
|
||
### 11.1 必须持久化的状态（漏一个就变行为）
|
||
|
||
| 状态 | 所属 | 初值 | 何时改变 |
|
||
|------|------|------|---------|
|
||
| 上拍时刻 | 主控律 | 0（表示首拍） | 每拍写入当前时刻 |
|
||
| 上拍输出模式 | 主控律 | 降级保持 | 仅模式切换时写入（并触发日志） |
|
||
| 无距离计时（进行中 + 起始时刻） | 主控律 | 未计时 | 首个无距离拍启动（`exit` 档）；有效距离帧 / 判丢刹车 / 复位() 清除 |
|
||
| 积分累计 | PID | 0 | 每拍（除冻结）；清积分/全清时归零 |
|
||
| 上拍误差 | PID | 0 | 每拍 |
|
||
| 低通后微分 | PID | 0 | 每拍 |
|
||
| 上次输出 | PID | 0 | 每拍 |
|
||
| 微分历史有效标志 | PID | false | 首次更新后置 true；全清时回 false |
|
||
| 上次输出 ×4 | 四个限幅器 | 0 | 每拍；复位时归零 |
|
||
|
||
**构造期预计算量**（不应在热路径重复计算）：针孔系数 ×2、PID 参数拷贝、
|
||
限幅器变化率上限 ×4、配置值拷贝。
|
||
|
||
### 11.2 时序与线程模型
|
||
|
||
```text
|
||
推理侧（每帧，独立线程）:
|
||
追踪库.更新(检测框, 画面尺寸) # 内部维护轨道与主目标
|
||
|
||
控制线程（固定周期，配置频率）:
|
||
结果 ← 追踪库.取最新结果() # 线程安全拷贝
|
||
快照 ← 采集(结果, 双目距离, 姿态)
|
||
快照.track_time_ms ← 当前单调时钟() # 就在取结果的那一刻打戳
|
||
意图 ← 控制律.更新(快照, 当前单调时钟())
|
||
按 意图.mode 分发:
|
||
速度航向 → 网关.发送速度航向(意图, 启动后毫秒)
|
||
零速刹车 → 网关.发送刹车(启动后毫秒)
|
||
降级保持 → 不发送（记录降级原因/告警）
|
||
|
||
停止视觉闭环（状态机决定）:
|
||
控制律.复位() # 清积分、清限幅器、清节拍与模式历史、清无距离计时
|
||
```
|
||
|
||
**约束**：
|
||
|
||
- 控制律实例**只由控制线程调用**，构件无锁；
|
||
- 采集快照与打戳必须尽量贴近（打戳晚于采集会把采集耗时算进"新鲜度"）；
|
||
- 追踪库取结果必须是**拷贝**而非引用，避免推理线程并发改写；
|
||
- 复位与更新不得并发。
|
||
|
||
### 11.3 数值与精度注意事项
|
||
|
||
| 事项 | 处理方式 | 理由 |
|
||
|------|---------|------|
|
||
| 角度回绕 | 位置式合成后回绕到 [-180, 180) | 链路侧与人工读数都按此区间 |
|
||
| 浮点"是否为零" | 用极小阈值（如 1e-9）比较，不用 `== 0` | 掩码分支选择依赖"航向/角速率是否有效" |
|
||
| dt 异常 | `dt <= 0` → 保持上次输出；`dt > 0.5 s` → 截断 | 除零保护与限幅器保护 |
|
||
| 首拍微分 | 不做微分（无历史） | 避免首拍微分冲击 |
|
||
| 除法基准 | 画面尺寸、`dt` 均在构造期/入口校验为正 | 防除零 |
|
||
| 配置校验 | 加载期全量校验并抛错（含字段路径） | 不允许带非法参数进入运行期 |
|
||
|
||
### 11.4 资源与日志纪律
|
||
|
||
- **热路径零动态分配**：所有缓冲与系数在构造期备妥（若实现语言有 GC，
|
||
也应避免每拍新建对象造成抖动）；
|
||
- **热路径零日志**：仅模式切换打一条 INFO；
|
||
- **异常日志节流**：若实现中加入异常日志，必须按"第 1 次与每满 N 次"节流，
|
||
20 Hz 下不节流会在数秒内刷满日志；
|
||
- **RAII / 确定性释放**：传输实现、配置源在构造期注入，生命周期由调用方保证。
|
||
|
||
### 11.5 可测试性设计（构件边界即测试边界）
|
||
|
||
| 构件 | 是否纯函数 | 测试要点 |
|
||
|------|-----------|---------|
|
||
| 超龄判定 | 是 | 三条判据各一例；边界（恰好等于有效期不超龄） |
|
||
| 坐标旋转 | 是 | §4.3 两条正方向自检；旋转保范；垂直分量不变 |
|
||
| 视线角转换 | 是 | 中心偏移为 0；针孔结果与线性近似明显偏离（证明未走线性）；非法参数报错 |
|
||
| 静态限幅 | 是 | 上/下界、界内不变 |
|
||
| 变化率限幅器 | 否（有状态） | 单步不超限；**不可超调**；复位后从零爬升；`dt<=0` 保持 |
|
||
| PID | 否（有状态） | 各项独立；积分钳位；**误差反号后能回卷**（旧错误判据在此会留在同侧饱和值）；两种复位语义 |
|
||
| 主控律 | 否（有状态） | 八步流程各分支；降级优先级；模式切换日志只打一次 |
|
||
| 打包网关 | 否 | 字节头（起始字节、长度、消息 ID）；掩码三分支；单位换算；**两者皆零走角速率掩码** |
|
||
|
||
**测试策略**：构件级用纯函数/小状态测试覆盖数学与边界；主控律级用构造快照
|
||
驱动分支覆盖；打包级**直接解析字节**验证线格式（不依赖协议库的解码函数，
|
||
避免"用同一份错误理解自我验证"）。
|
||
|
||
---
|
||
|
||
## 12. 验证路线与待定项
|
||
|
||
### 12.1 分阶段验证路线
|
||
|
||
| 阶段 | 目标 | 方法 | 通过判据 |
|
||
|------|------|------|---------|
|
||
| 1. 构件级 | 数学正确 | 单元测试（无硬件） | 全部用例通过 |
|
||
| 2. 控制律级 | 分支与时序正确 | 构造快照驱动 + 端到端合成场景回放 | 三分支（正常/Coast/判丢）行为符合 §7 |
|
||
| 3. 链路级 | 线格式正确 | 注入"回环打印/写文件"传输实现，解析字节 | 帧头、消息 ID、掩码、单位换算符合 §8 |
|
||
| 4. SITL | 闭环行为 | 仿真飞控 + 回放/合成目标 | 目标居中收敛、无抖动、无积分饱和 |
|
||
| 5. 台架 | 实链路 | 真实串口/电台 + 飞控上电不转桨 | offboard 建立、指令被接受、无失效保护 |
|
||
| 6. 实飞 | 标定整定 | 逐项标定增益与限幅 | **仅在 1–5 全部通过后** |
|
||
|
||
当前已完成阶段 1–3（构建零告警、单元测试全绿、端到端示例三阶段完整运行）。
|
||
阶段 4 起需要飞控与标定数据，属后续工作。
|
||
|
||
### 12.2 待定项清单
|
||
|
||
| 项 | 现状 | 需要谁确认 | 风险 |
|
||
|----|------|-----------|------|
|
||
| **全部数值参数** | 占位值 | 标定 + 台架整定 | 高：未标定不得实飞 |
|
||
| **忽略掩码位序** | 按随附 MAVLink v3 枚举（`YAW_IGNORE=1024`、`YAW_RATE_IGNORE=2048`） | 核对飞控端 PX4 1.11.3 随附枚举 | **高**：旧枚举无 `FORCE_SET=512`，位序低一位 → 速率指令被误解为绝对航向 0（§8.2） |
|
||
| **坐标框取值** | 局部 NED | 台架实测 | 中：与旋转语义必须一致 |
|
||
| **降级时是否应主动刹车** | 降级**不发送**，依赖飞控 offboard 断流失效保护 | 与安全策略负责人确认 | 中：断流后飞控行为（悬停/返航）取决于其配置，不由本模块决定 |
|
||
| **`yaw_slew_limit_dps2` 量纲** | 两种航向模式共用一个键；速率式下含义为 deg/s²，位置式下实际含义为 deg/s | 启用位置式前拆分为两个键或重命名 | 低（默认速率式，位置式当前休眠） |
|
||
| **侧向通道** | `ay_mps2` 预留、机体侧向恒 0 | 若需要侧移则新增通道 | 低 |
|
||
| **机体补偿** | 快照的滚转/俯仰/角速度字段预留未用 | 接入主工程姿态源后评估 | 低 |
|
||
| **相机标定** | 内参/畸变/安装外参未标定，针孔模型无畸变补偿 | 标定 | 中：视线角系统性偏差 |
|
||
| **track_id 一致性校验** | 调用方约定：装配层校验双目距离与追踪主目标的 `track_id` 一致并告警，库内不实现（S3） | 主程序装配层落地时加一处校验 | 中：错配会把其他目标的距离当作本目标，距离通道朝错误目标调射程 |
|
||
|
||
### 12.3 红线
|
||
|
||
1. **未标定不得实飞**：配置与代码默认值中所有数值均为占位，标定与实测整定
|
||
完成前，仅限 SITL / 台架 / 录制数据回放验证数学链路。
|
||
2. **不得混用未标注坐标系的数据**：对外输出的速度分量已声明为局部 NED，
|
||
任何改动都必须同步更新声明与打包侧坐标框。
|
||
3. **不得用超龄数据生成正常指令**：超龄只能触发降级或告警。
|
||
4. **不得绕过限幅**：任何新增通道都必须内联"变化率 + 幅值"双重限幅，
|
||
包括切换后的首条指令。
|
||
5. **`distance_valid == false` 的距离不得当作目标距离**（v1.0 仅单目检出时
|
||
不发布距离，方位退化为该目偏移，见 `docs/双目深度估计思路.md` §11.3）。
|
||
|
||
---
|
||
|
||
## 附：与追踪库的行为衔接速查
|
||
|
||
| 追踪库输出 | 本模块响应 | 位置 |
|
||
|-----------|-----------|------|
|
||
| `tracked == true`、`is_predicted == false` | 三路正常计算 | §5.2 |
|
||
| `tracked == true`、`is_predicted == true` | 三路照常计算 + 清距离积分 + `coast_active` | §7.4 |
|
||
| `tracked == false` | 零速刹车 + 清积分与限幅器历史 | §7.3 |
|
||
| 结果超过 `visual_stale_ms` 未更新 | 降级保持（视觉超龄），**不刹车**（标志不可信） | §7.2 |
|
||
| 主目标切换（追踪库内部选举） | 本模块无感知：像素偏移自然跳变，由限幅器吸收 | §6.6 |
|