460 lines
28 KiB
Markdown
460 lines
28 KiB
Markdown
# 视觉跟踪控制律实现文档（visual_track_control）
|
||
|
||
> 独立静态库子工程，实现 `../docs/物理追踪思路.md`（下称"规格"）定义的
|
||
> 视觉跟踪阶段（S7 VISUAL_TRACKING）控制律：水平航向、垂直速度、
|
||
> 距离 PID 三路控制 + 统一限幅 + Coast 清积分 / 判丢刹车 + 超龄降级，
|
||
> 输出端随机体航向完成机体系→局部 NED 旋转，经 `MavlinkCommandGateway`
|
||
> 打包为 MAVLink 字节流，交由注入的 `ICommandTransport` 实现送达。
|
||
>
|
||
> 本文档按 `../AGENTS.md`"实现文档规范"六要素组织。三份文档分工明确、
|
||
> 互不重复：
|
||
>
|
||
> - `../docs/物理追踪思路.md`——控制律**物理与行为规格**（唯一权威），
|
||
> 本文档不重复抄录公式推导；
|
||
> - `../docs/视觉跟踪控制律技术路线.md`——**语言无关实现路线**：单周期八步
|
||
> 伪代码、构件与状态清单、坐标系与符号约定、打包掩码取舍、重写指引；
|
||
> - 本文档——**C++ 落地说明**：公共头一览、构建与测试方式、日志行为、
|
||
> 排查与修改要点。
|
||
>
|
||
> 追踪库行为约定见 `../target_tracker/target_tracker.md`，同样只引用不复述。
|
||
|
||
---
|
||
|
||
## 一、功能职责与边界
|
||
|
||
命名空间 `drone::vtc`，头文件保护一律 `#pragma once`。
|
||
|
||
**做什么**：
|
||
|
||
- **三路控制律**（`TrackingControlLaw::Update()` 单周期计算）：
|
||
- 水平航向（规格 §4）：像素偏移 → 水平视线角 β → 增益 → 变化率限幅 →
|
||
角速度限幅，按 `HeadingMode` 配置输出速率式 `yaw_rate_dps`（默认）
|
||
或位置式 `yaw_deg`（绝对 NED 航向，见 §3.8）；
|
||
- 垂直速度（规格 §5）：垂直视线角 α → 统一速度支路 → `vz_mps`
|
||
垂直速度设定值（原"小误差姿态微调/大误差垂直速度"双支路已取消，
|
||
原因见 `vtc_types.h` 的 `ControlMode` 注释与 §3.8）；
|
||
- 距离通道（规格 §6）：`Δd = d − d_exp` → `PidController` → 前向速度
|
||
`vx_mps`；目标距离无效/超龄时按 `NoDistanceAction` 配置降级
|
||
（hold / slow_approach / exit，exit 需持续无距离达
|
||
`no_distance_exit_grace_ms` 门限，见 §3.8）。
|
||
- **统一限幅**（规格 §8）：所有输出通道内联完成限幅——`Clamp` 静态限幅 +
|
||
`SlewRateLimiter` 变化率限幅（航向修正/角速度、前向/垂直加速度），
|
||
全部在机体系完成。
|
||
- **坐标系旋转**（规格 §2.3）：输出端用 `RotateBodyToNed()` 把机体系
|
||
速度（前/右/下）随快照航向 ψ 旋转为局部 NED（北/东/地），与网关的
|
||
`MAV_FRAME_LOCAL_NED` 一致；仅绕下轴旋转（滚转/俯仰对速度指令为
|
||
二阶小量，由飞控内环处理）。
|
||
- **Coast 清积分**（规格 §6.4）：`is_predicted == true` 时清空距离 PID
|
||
积分项（`ResetIntegral()`），防积分饱和。
|
||
- **判丢刹车**（规格 §6.4）：`tracked == false` 时输出 `kBrakeHover`
|
||
零速刹车，并清积分与全部限幅器历史（保证恢复跟踪后指令从零渐变）。
|
||
- **超龄降级**（规格 §7.1、§9.2）：姿态过期/缺失 → `kDegradedHold`
|
||
（`kAttitudeStale`，禁止控制）；视觉结果超龄 → `kDegradedHold`
|
||
（`kVisionStale`）；判定器为无状态 `StalenessChecker::IsStale()`。
|
||
- **MAVLink 打包**（规格 §2.2）：`MavlinkCommandGateway` 把
|
||
`ControlOutput` 打包为 `SET_POSITION_TARGET_LOCAL_NED`（ID 84）字节流。
|
||
- **配置化**（规格 §8、需求分析 NFR-007）：全部阈值集中在
|
||
`TrackingConfig`，由 `TrackingConfig::LoadFromJson()` 启动期一次性加载，
|
||
代码不写死任何参数。
|
||
|
||
**不做什么**（边界）：
|
||
|
||
- **不做感知**：不运行检测/光流/雷达驱动，只消费调用方填入的
|
||
`ControlSnapshot`；
|
||
- **不做状态机仲裁**：S6/S8 切换、目标丢失策略（`ITargetLostStrategy`）、
|
||
障碍接管均在状态机层，本库只按输入标志产生当拍输出；
|
||
- **不写串口/传输**：库内无任何链路代码，字节流交给 `ICommandTransport`
|
||
实现方；
|
||
- **不解析 MAVLink 输入**：只有打包发送方向，无 `mavlink_parse_*`；
|
||
- **不做机体补偿**：`ControlSnapshot` 的 `roll_rad` / `pitch_rad` /
|
||
`yaw_rate_radps` / `velocity_*` 字段为预留输入（规格 §4.1）；当前仅
|
||
`yaw_rad` 参与计算（NED 旋转与位置式航向合成）；
|
||
- **不做热更新**：配置启动期加载一次，调参重启生效。
|
||
|
||
---
|
||
|
||
## 二、接口与数据流
|
||
|
||
### 2.1 公共头一览
|
||
|
||
| 头文件 | 内容 |
|
||
|--------|------|
|
||
| `include/visual_track_control/vtc_types.h` | `ControlMode`、`DegradedReason`、`ControlSnapshot`、`ControlOutput` |
|
||
| `include/visual_track_control/vtc_config.h` | `HeadingMode`、`NoDistanceAction`、各分组配置结构、`TrackingConfig` |
|
||
| `include/visual_track_control/tracking_control_law.h` | `TrackingControlLaw`（主控律） |
|
||
| `include/visual_track_control/pid_controller.h` | `PidParams`、`PidController` |
|
||
| `include/visual_track_control/rate_limiter.h` | `Clamp()`、`SlewRateLimiter`（header-only） |
|
||
| `include/visual_track_control/staleness_checker.h` | `StalenessChecker`（header-only） |
|
||
| `include/visual_track_control/line_of_sight.h` | `PixelToAngle`（像素→视线角，全程针孔模型） |
|
||
| `include/visual_track_control/frame_transform.h` | `RotateBodyToNed()`（机体系→局部 NED，header-only） |
|
||
| `include/visual_track_control/i_command_transport.h` | `ICommandTransport`（传输抽象） |
|
||
| `include/visual_track_control/mavlink_command_gateway.h` | `MavlinkCommandGateway`（MAVLink 打包） |
|
||
|
||
### 2.2 关键字段（以代码为准）
|
||
|
||
`enum class ControlMode`：`kVelocityHeading`（速度航向主输出）/
|
||
`kBrakeHover`（零速刹车）/ `kDegradedHold`（降级保持，禁止正常控制）。
|
||
（原 `kAttitudeTrim` 姿态微调模式已取消，见 §3.8。）
|
||
|
||
`enum class DegradedReason`：`kNone` / `kAttitudeStale`（姿态过期）/
|
||
`kVisionStale`（视觉超龄）/ `kNoDistanceExit`（持续无距离达门限且配置
|
||
退出近距操作）。
|
||
|
||
`ControlSnapshot`（输入，纯值类型）：
|
||
|
||
| 字段 | 含义 |
|
||
|------|------|
|
||
| `track` | `drone::tracker::TrackResult` 值拷贝（追踪库主目标输出，字段定义见 `../target_tracker/target_tracker.md` §二） |
|
||
| `track_time_ms` | 追踪结果取样单调时钟（毫秒）；**<=0 视为从未取样，直接判超龄** |
|
||
| `target_distance_m` | 目标距离（米，双目立体测距前向分量 Z） |
|
||
| `distance_valid` | 距离有效标志；**无效不得当目标距离**（规格 §6.3、需求分析 FR-041）。距离来自目标检测框自身的双目测距，无跨模态关联问题 |
|
||
| `distance_time_ms` | 距离取样单调时钟 |
|
||
| `attitude_present` / `attitude_time_ms` | 姿态源可用性与取样时刻（过期→禁止控制，规格 §9.2） |
|
||
| `yaw_rad` | 自机航向（弧度，NED）；**参与计算**：输出端机体系→NED 旋转（§2.3）与位置式绝对航向合成 |
|
||
| `roll_rad` / `pitch_rad` / `yaw_rate_radps` | 自机姿态其余分量（弧度，FRD）；预留，机体补偿未实现（规格 §4.1） |
|
||
| `velocity_x_mps` / `velocity_y_mps` / `velocity_z_mps` | 自机速度分量（预留，接入主工程时由 `FlightStateSnapshot` 填充） |
|
||
|
||
`ControlOutput`（输出，已限幅）：
|
||
|
||
| 字段 | 含义 |
|
||
|------|------|
|
||
| `mode` / `valid` | 输出模式 / 指令是否可用于发送 |
|
||
| `vx_mps` / `vy_mps` / `vz_mps` | **局部 NED 速度分量（北/东/地，米/秒）**：各通道在机体系计算并限幅后，输出端随 `snapshot.yaw_rad` 旋转得到；旋转为正交变换，合速度大小不变。机体系侧向恒为 0，旋转后 `vy_mps` 可非零 |
|
||
| `yaw_deg` | 位置式目标**绝对航向**（度，NED，右转为正）：由当前航向 + 变化率限幅后的修正量合成，回绕到 [-180, 180)（ID 84 的 yaw 字段为绝对值） |
|
||
| `yaw_rate_dps` | 速率式偏航角速度指令（度/秒，右转为正；天然是相对量，无坐标系歧义） |
|
||
| `degraded_reason` | 降级原因（仅 `kDegradedHold` 有意义） |
|
||
| `coast_active` | 本周期处于 Coast（`is_predicted == true`） |
|
||
|
||
`TrackingConfig` 六分组：`control`（`frequency_hz`、`visual_stale_ms`、
|
||
`distance_stale_ms`、`attitude_stale_ms`）、`camera`（`image_width`、
|
||
`image_height`、`fov_h_deg`、`fov_v_deg`）、`heading`（`mode`、`gain`、
|
||
`yaw_rate_limit_dps`、`yaw_slew_limit_dps2`）、`vertical`
|
||
（`vz_gain_mps_per_deg`、`vz_limit_mps`）、`distance`（`d_exp_m`、
|
||
`kp`/`ki`/`kd`、`integral_limit`、`derivative_filter_coef`、
|
||
`approach_velocity_limit_mps`、`retreat_velocity_limit_mps`、
|
||
`no_distance_action`、`no_distance_approach_limit_mps`、
|
||
`no_distance_exit_grace_ms`）、`accel_limit`
|
||
（`ax_mps2`、`ay_mps2`、`az_mps2`）。JSON 键名与字段一一对应，见
|
||
`config/track_control.json`。`heading.mode` 默认速率式（`"rate"`）；
|
||
`distance.no_distance_action` 默认 `slow_approach`（双目有效域外无距离
|
||
是常态，对齐 D4 远场定速接近策略，见《双目测距方案讨论纪要》S1）；
|
||
相机组原 `linear_ratio` 键已随线性/针孔双路一并移除（§3.5）。
|
||
|
||
### 2.3 ICommandTransport 契约
|
||
|
||
```cpp
|
||
class ICommandTransport {
|
||
public:
|
||
virtual bool Send(const std::uint8_t* data, std::size_t len) = 0;
|
||
};
|
||
```
|
||
|
||
- **只吃字节流**：串口、电台、回环打印均可注入实现；后续重写传输层
|
||
只换实现，不动打包与控制律；
|
||
- 返回 `true` = 已交付底层链路，`false` = 发送失败（调用方可计数/告警）；
|
||
- 实现方保证不长期持有 `data` 指针（同步拷贝或同步写出）。
|
||
|
||
### 2.4 数据流向
|
||
|
||
```text
|
||
推理侧每帧: TargetTracker::Update(detections, frame_shape)
|
||
控制线程(固定周期, 规格 §7.1):
|
||
TargetTracker::LastResult() # 线程安全拷贝
|
||
→ 取样瞬间用单调时钟打戳填入 ControlSnapshot.track_time_ms
|
||
→ TrackingControlLaw::Update(snapshot, now_ms) # 单周期计算
|
||
→ 按 ControlOutput.mode 分发:
|
||
kVelocityHeading → MavlinkCommandGateway::SendVelocityHeading()
|
||
kBrakeHover → MavlinkCommandGateway::SendBrakeHover()
|
||
kDegradedHold → 不发送（仅告警/降级）
|
||
→ mavlink_msg_to_send_buffer → ICommandTransport::Send(字节流)
|
||
停止视觉闭环: TrackingControlLaw::Reset()（规格 §6.4）
|
||
```
|
||
|
||
---
|
||
|
||
## 三、关键实现点
|
||
|
||
### 3.1 传输接口只吃字节流的双重解耦
|
||
|
||
打包（`MavlinkCommandGateway`）与传输（`ICommandTransport`）分离：
|
||
网关只负责 `ControlOutput` → MAVLink 字节，不感知链路形态；传输实现
|
||
只感知字节，不感知消息语义。对应规格 §2.2 控制单出口约束在独立库中的
|
||
映射——控制律不直接写飞控。
|
||
|
||
### 3.2 TrackResult 无时间戳，由调用方取样打戳
|
||
|
||
`TrackResult` 是追踪库自有类型，本身不含时间戳（规格 §7.2）。约定：
|
||
调用方在读取 `LastResult()` 的**瞬间**用单调时钟打戳填入
|
||
`track_time_ms`；超龄判定一律用"取样时间 + `valid_for_ms`"
|
||
（`StalenessChecker::IsStale(sample_time_ms, now_ms, valid_for_ms)`）：
|
||
|
||
- `sample_time_ms <= 0` 视为从未取样，直接判超龄；
|
||
- `now_ms < sample_time_ms`（时间戳不一致：时钟源错配/打戳错误）同样
|
||
判超龄——单一单调时钟下不应发生，宁可降级也不信任不可核实的取样。
|
||
|
||
### 3.3 PID anti-windup 策略（条件积分）
|
||
|
||
`PidController::Update()`（位置式）三层防护：
|
||
|
||
1. **积分钳位**：候选积分值先钳制在 ±`integral_limit`；
|
||
2. **anti-windup（条件积分）**：输出饱和（`|raw| > output_limit`）且
|
||
**本拍积分增量仍在推深饱和**（增量 `error·dt` 与原始输出 `raw`
|
||
同号）时，冻结本步积分累加（不采纳候选积分值）；误差反号后增量反向，
|
||
积分立即恢复回卷，无饱和残留。判据刻意基于"增量方向"而非"积分项与
|
||
输出同向"：后者在误差反号后仍会冻结回卷；也不用 `raw·error` 的
|
||
变体含混表述，实现即 `(error*dt_s) * raw > 0`；
|
||
3. **输出限幅**：最终输出钳制在 ±`output_limit`。
|
||
|
||
微分带一阶低通（`derivative_filter_coef ∈ [0,1)`，0 = 不滤波）；
|
||
`dt_s <= 0` 时保持上次输出（异常节拍保护）。`ResetIntegral()` 只清积分，
|
||
保留微分历史与上次输出；`Reset()` 清全部，均 O(1)（Coast 清积分，
|
||
规格 §6.4）。
|
||
|
||
### 3.4 SlewRateLimiter 兼作首条指令突变抑制
|
||
|
||
`SlewRateLimiter` 保存上次输出，单步变化不超过 `rate_limit · dt_s`；
|
||
构造时初值置 0，天然实现规格要求的"首条视觉速度设定值必须经过限幅"
|
||
（状态机设计 S6）：切换进入视觉跟踪后首条指令从零按加速度限幅渐变，
|
||
无需专门的切换分支。判丢进刹车时调用 `Reset()` 归零，恢复跟踪后同样
|
||
从零渐变。主控律共 4 个限幅器：`yaw_deg_limiter_` / `yaw_rate_limiter_`
|
||
（度/秒²）、`vx_limiter_` / `vz_limiter_`（米/秒²）。
|
||
|
||
### 3.5 像素→角度：全程针孔模型
|
||
|
||
`PixelToAngle` 构造期预计算针孔系数 `2·tan(θ/2)/size`，热路径仅一次
|
||
乘加 + `atan`：`angle = atan(offset·2·tan(θ/2)/size)`（规格 §4.1/§5.1）。
|
||
原"小偏移线性近似/大偏移针孔"双路切换已取消：线性式与针孔式在切换边界
|
||
两侧不连续（θ=60°、边界比例 0.2 处约 8% 角度台阶），会向控制环路注入
|
||
突变；`atan` 代价可忽略，全程采用精确针孔式。配置中 `linear_ratio` 键
|
||
随之移除。水平 β（`PixelToAngle(W, θ_h)`）与垂直 α（`PixelToAngle(H, θ_v)`）
|
||
各一个实例，构造期校验参数合法性（尺寸、视场角），非法抛
|
||
`std::invalid_argument`。
|
||
|
||
### 3.6 全工程仅 mavlink_command_gateway.cpp 一个 TU 包含 MAVLink 头
|
||
|
||
c_library_v2 的 `mavlink_helpers.h` 中 **`MAVLINK_HELPER` 默认为空宏**，
|
||
辅助函数直接以外部链接定义在头文件中；多个翻译单元包含 `common/mavlink.h`
|
||
会产生**多重定义**链接错误。因此：
|
||
|
||
- `common/mavlink.h` 只被 `src/mavlink_command_gateway.cpp` 包含
|
||
（该文件也是全库唯一的 MAVLink 知识落点）；单元测试同样**不得**包含
|
||
该头，按 MAVLink v2 线格式手工解码字节流断言（见 §5.3）；
|
||
- 公共头 `mavlink_command_gateway.h` 不出现任何 `mavlink_*` 类型，
|
||
打包/发送缓冲用成员固定字节存储承载（`msg_storage_` / `tx_buffer_`，
|
||
各 512 字节，`.cpp` 中 `static_assert` 校验 ≥ `sizeof(mavlink_message_t)`
|
||
与 `MAVLINK_MAX_PACKET_LEN`）；
|
||
- 打包用 `mavlink_msg_*_pack`（无通道状态、纯静态内联、可重入），
|
||
不使用 `_pack_chan` 通道 API；
|
||
- CMake 中 MAVLink include 目录设为 `SYSTEM PRIVATE`：压第三方告警噪音，
|
||
且不泄漏给下游。
|
||
|
||
同理，`nlohmann/json` 只被 `src/vtc_config.cpp` 一个 TU 包含。
|
||
|
||
### 3.7 配置启动期一次加载，不做热更新
|
||
|
||
`TrackingConfig::LoadFromJson()` 仅在启动期调用一次：解析结果拷贝进
|
||
`TrackingConfig` 值对象后即与 JSON 脱钩，热路径绝不触碰 JSON 对象；
|
||
调参重启即可。若未来需要热更新，扩展点为**原子指针交换**：持有
|
||
`std::atomic<const TrackingConfig*>`，后台解析新配置成功后原子换指针，
|
||
控制线程每拍读取当前指针快照使用（当前未实现，不引入额外复杂度）。
|
||
|
||
### 3.8 主控律单周期流程
|
||
|
||
`Update()` 按固定流程执行（`last_now_ms_` 推算 dt，首拍与异常节拍
|
||
（时钟回拨/`now_ms` 未前进）用 `1.0 / control.frequency_hz` 兜底，
|
||
dt 上限 `kMaxDtS = 0.5` 秒）：
|
||
|
||
① 超龄判定（姿态过期/缺失 → `kDegradedHold`；视觉超龄 → `kDegradedHold`）
|
||
→ ② 判丢刹车（清积分与限幅器历史）→ ③ Coast 清距离积分
|
||
→ ④ 像素偏移 → β/α（针孔模型）→ ⑤ 水平航向（按 `HeadingMode` 分支）
|
||
→ ⑥ 垂直统一速度支路（α → `vz_gain·α` → 加速度限幅 → 速度限幅）
|
||
→ ⑦ 距离通道（`distance_valid` 且未超龄才可用；否则清积分并按
|
||
`no_distance_action` 降级）→ ⑧ 机体系→NED 旋转（`RotateBodyToNed`，
|
||
随 `snapshot.yaw_rad`）→ 置 `valid = true` 输出。
|
||
|
||
三个设计决策记录：
|
||
|
||
- **垂直双支路合并（A1）**：规格 §5.2 原设"小误差姿态微调（`kAttitudeTrim`
|
||
→ ID 82）/ 大误差垂直速度"双支路。姿态微调支路会把整个输出切成姿态
|
||
消息，导致同周期的距离通道前向速度被丢弃（逼近停滞），且双消息类型
|
||
逐拍翻转会扰动 PX4 offboard。现垂直通道一律走速度支路：小误差经
|
||
`vz_gain` 线性映射为小垂直速度，行为连续、全周期单一 offboard 消息。
|
||
- **位置式航向输出绝对值（A2）**：ID 84 的 `yaw` 字段是**绝对** NED
|
||
航向。若把 β 修正量直接填入，飞控会转向"修正角"方向而非目标方向。
|
||
位置式分支输出 `WrapDeg180(当前航向 + 限幅修正量)`；速率式
|
||
（默认）输出角速度指令，天然相对量、无此问题。
|
||
- **kExit 持续无距离门限（纪要 S1）**：换双目后有效域外无距离是常态
|
||
（纪要 D4 远场定速接近），首个无效拍即退出会频繁降级。`kExit` 现需
|
||
连续无距离达 `no_distance_exit_grace_ms` 才降级（语义收敛为"疑似
|
||
传感器级故障"），grace 期间按 `kSlowApproach` 行为执行（经加速度
|
||
限幅，vx 平滑过渡），任一有效距离帧重置计时；`no_distance_exit_grace_ms
|
||
= 0` 保留旧"立即退出"行为。`no_distance_action` 默认值随之
|
||
`kHold` → `kSlowApproach`。判丢刹车期间不计无距离时长，`Reset()`
|
||
清计时状态。
|
||
|
||
### 3.9 MAVLink 消息映射（规格 §2.2）
|
||
|
||
全库仅 ID 84 `SET_POSITION_TARGET_LOCAL_NED` 一种消息（原 `kAttitudeTrim`
|
||
→ ID 82 `SET_ATTITUDE_TARGET` 已随姿态微调支路移除）：
|
||
|
||
| 模式 | 消息 | 要点 |
|
||
|------|------|------|
|
||
| `kVelocityHeading` | ID 84 | `coordinate_frame = MAV_FRAME_LOCAL_NED`（速度分量已是 NED 语义）。速度模式 `type_mask` 基础位 `kMaskVelocityBase = 0x01C7`（忽略位置与加速度），航向三分支：角速率非零 → `kMaskVelocityYawRate`（再忽略 yaw）；仅航向角非零 → `kMaskVelocityYaw`（再忽略 yaw_rate）；**两者均零 → 速率掩码配零角速率（保持当前航向）**——旧实现把零角速率落到航向掩码，等于误发绝对航向 0（指向正北） |
|
||
| `kBrakeHover` | ID 84 | `kMaskBrake = 0x0DC7`：速度全零，位置/加速度/航向/角速率字段全部忽略 |
|
||
|
||
---
|
||
|
||
## 四、日志行为
|
||
|
||
遵循 `../AGENTS.md` 日志纪律：**库内仅状态切换点打日志，热路径零日志、
|
||
零堆分配**。
|
||
|
||
| 位置 | 日志 | 触发条件 |
|
||
|------|------|----------|
|
||
| `tracking_control_law.cpp`（`Update()` 内 `FinishAndLog`） | `spdlog::info` "[ControlLaw] 状态切换: X -> Y (原因)" | **仅当 `ControlMode` 相对 `last_mode_` 发生变化**时打印（刹车进出、降级切换、进入/退出 Coast 跟踪）；同一模式连续多拍不重复打印 |
|
||
| `tracking_control_law.cpp` | —— | 每拍正常计算（含三路通道数学）**不打任何日志** |
|
||
| `mavlink_command_gateway.cpp` / `pid_controller.cpp` / `line_of_sight.cpp` / `vtc_config.cpp` | —— | 不打日志（配置错误走异常，见第六章） |
|
||
|
||
示例程序（`examples/track_control_demo.cpp`，非生产代码）另有两处
|
||
**演示观测日志**，均带节流防刷屏：
|
||
|
||
- `LoggingTransport::Send()`：字节统计与包头十六进制摘要，**秒级节流**
|
||
（距上次打印 ≥ 1000 ms 才输出）；
|
||
- 主循环状态摘要：**每 10 拍一次**（20 Hz 下即半秒一次）。
|
||
|
||
---
|
||
|
||
## 五、构建、测试与运行方式
|
||
|
||
### 5.1 构建
|
||
|
||
`../AGENTS.md` 规定构建环境为 WSL2 Ubuntu 24.04。**当前开发机 WSL 仅
|
||
装有 Ubuntu 22.04（发行版名 `Ubuntu`）**，本库的构建与测试验证在
|
||
22.04（cmake 3.22 / g++ 11.4）完成，行为与 24.04 无差异；24.04 发行版
|
||
安装后应按其复核一遍（见 §6.4）。
|
||
|
||
```bash
|
||
cmake -S visual_track_control -B build_vtc -DBUILD_TESTING=OFF
|
||
cmake --build build_vtc -j$(nproc)
|
||
./build_vtc/track_control_demo
|
||
```
|
||
|
||
- **为什么 `-DBUILD_TESTING=OFF`**：本库以 `add_subdirectory` 零改动复用
|
||
`target_tracker`，而 `target_tracker/CMakeLists.txt` 中 `include(CTest)`
|
||
会把 `BUILD_TESTING` 默认置为 `ON`，从而连带构建追踪库的 3 个测试
|
||
可执行文件。本子工程不关心其测试，显式关闭以加快构建（追踪库自身的
|
||
测试方式见 `../target_tracker/target_tracker.md` §五）。
|
||
- **本库自身测试不复用 `BUILD_TESTING`**：由独立选项 `VTC_BUILD_TESTS`
|
||
（默认 `ON`）控制，与上面的关闭互不干扰。
|
||
- **示例无需 `LD_LIBRARY_PATH`**：`target_tracker` 为 SHARED 库，
|
||
构建期 `CMAKE_BUILD_RPATH` 已指向构建目录，`track_control_demo`
|
||
可直接运行。
|
||
- 示例缺省配置路径由编译期宏 `VTC_DEMO_DEFAULT_CONFIG` 内置为
|
||
`config/track_control.json` 的绝对路径；也可命令行传入
|
||
`track_control_demo [配置文件路径]`。
|
||
|
||
### 5.2 示例演示内容
|
||
|
||
单线程模拟完整链路（合成运动检测框 → `TargetTracker::Update()` →
|
||
`LastResult()` + 打戳 → `TrackingControlLaw::Update()` → Gateway →
|
||
`LoggingTransport` 字节打印），按配置频率固定节拍依次走三阶段
|
||
（对应规格 §6.4）：
|
||
|
||
1. **正常跟踪（6 秒）**：持续喂入向画面中心漂移的合成检测框，双目
|
||
距离有效，输出速率式速度航向意图（vx 按加速度限幅渐变至接近速度上限，
|
||
角速度随 β 收敛而减小）；
|
||
2. **Coast（4 秒）**：停止喂检测 → `is_predicted == true` → 清积分、
|
||
继续预测跟踪；同时置距离无效，演示 §6.3 无距离降级路径（配置为
|
||
`slow_approach`：按限速值缓速前向）；
|
||
3. **判丢刹车（3 秒）**：连续丢帧超过追踪阈值 → `tracked == false` →
|
||
零速刹车。
|
||
|
||
示例将追踪库判丢阈值放大为 `kDemoMaxLostFrames = 60` 帧（默认 8 帧在
|
||
20 Hz 下 Coast 仅 0.4 秒不便观察），**仅演示专用**，正式参数以追踪库
|
||
规格为准。程序跑完总拍数后自行调用 `TrackingControlLaw::Reset()` 并
|
||
正常退出（返回 0），无需手动终止。
|
||
|
||
### 5.3 单元测试（Google Test）
|
||
|
||
测试源文件位于 `tests/`，覆盖 8 个测试套件：
|
||
|
||
| 测试文件 | 覆盖要点 |
|
||
|----------|----------|
|
||
| `rate_limiter_test.cpp` | `Clamp` 边界；`SlewRateLimiter` 单步限幅、`dt<=0`/冻结保护、`Reset` |
|
||
| `staleness_checker_test.cpp` | 从未取样、时钟回拨（`now < sample`）、恰在有效期边界（临界判新鲜） |
|
||
| `frame_transform_test.cpp` | ψ=0/90° 基向量映射、合速度守恒（正交性）、`v_down` 不变 |
|
||
| `line_of_sight_test.cpp` | 零偏移、半幅=半视场、奇对称、单调性、与线性近似的偏离（证明未走线性支路）、构造期非法参数抛异常 |
|
||
| `pid_controller_test.cpp` | P/I/D 分量、积分钳位、输出限幅、抗饱和冻结、**误差反号回卷回归**（旧判据钉在 +5，条件积分到 -5）、微分低通、`dt<=0`、`ResetIntegral`/`Reset` |
|
||
| `tracking_control_law_test.cpp` | 姿态缺失/过期、视觉超龄、从未取样、姿态比视觉更严的优先级、判丢刹车、Coast 标志、速率/位置两种航向模式（含绝对航向回绕）、垂直支路与限幅、距离通道无距离降级动作（含 kExit 门限、grace 有效帧重置、零门限立即退出）、距离超龄回落、首拍变化率限幅（S6）、NED 旋转、`Reset` 后从零渐变 |
|
||
| `vtc_config_test.cpp` | 真实配置加载、position 模式可接受、默认降级动作为 slow_approach、缺文件/缺字段/缺分组/类型错/枚举非法/数值越界/grace 为负均抛异常且消息携带字段名 |
|
||
| `mavlink_command_gateway_test.cpp` | 手工解码 MAVLink v2 字节流（不含 `mavlink.h`，构建约束见 §3.6）：帧头、ID 84、三种掩码分支（含"双零 → 速率掩码"回归）、坐标基准字节、目标系统/组件字节、传输失败传播 |
|
||
|
||
运行方式（构建后）：
|
||
|
||
```bash
|
||
./build_vtc/vtc_unit_tests # 直接跑
|
||
ctest --test-dir build_vtc --output-on-failure # 或经 ctest（gtest_discover_tests 注册）
|
||
```
|
||
|
||
依赖：系统安装 Google Test（`libgtest-dev`，FindGTest 模块模式，无需
|
||
CMake config 包）；未找到时自动跳过测试构建并打印状态，不影响库与示例。
|
||
`vtc_config_test` 通过编译期宏 `VTC_TEST_CONFIG` 读取真实配置文件
|
||
`config/track_control.json`——**修改该配置文件时须同步核对测试断言**。
|
||
|
||
### 5.4 部署提示
|
||
|
||
`target_tracker` 为 SHARED 库（`libtarget_tracker.so.1`）：部署
|
||
`track_control_demo` 或未来接入主工程时，必须将该 so 一并随带
|
||
（目录式部署与 RPATH 约定见 `../target_tracker/target_tracker.md` §7）。
|
||
本库 `visual_track_control` 自身为 STATIC 库，链接进可执行文件，无需随带。
|
||
|
||
---
|
||
|
||
## 六、排查 / 修改要点
|
||
|
||
### 6.1 配置缺字段 / 非法值报错定位
|
||
|
||
`LoadFromJson()` 缺字段、类型错误、非法取值均抛 `std::runtime_error`，
|
||
**异常消息携带完整字段名**（如 `配置缺失字段或类型错误: heading.gain`、
|
||
`配置非法(必须为正): control.frequency_hz`、
|
||
`配置非法: distance.no_distance_action 仅支持 hold / slow_approach / exit`），
|
||
按异常文本直接定位到 `config/track_control.json` 的对应分组与键即可；
|
||
文件打开/解析失败则携带文件路径。`PixelToAngle` 构造期参数非法（尺寸
|
||
非正、视场角不在 (0, π)）抛 `std::invalid_argument`，同样见消息文本。
|
||
|
||
### 6.2 MAVLink 消息待实测适配项
|
||
|
||
以下各项均以 **PX4 1.11.3 Offboard 实测为准**（规格 §10.1 待定项），
|
||
全部集中在 `src/mavlink_command_gateway.cpp`，修改时只动这一个文件：
|
||
|
||
| 适配项 | 位置 | 现状 |
|
||
|--------|------|------|
|
||
| `coordinate_frame` | `SendVelocityHeading()` / `SendBrakeHover()` 的 `mavlink_msg_set_position_target_local_ned_pack` 调用 | 固定取 `MAV_FRAME_LOCAL_NED`；速度分量已由主控律旋转为局部 NED 语义（§3.8 步骤⑧），语义是否匹配待实测确认 |
|
||
| 速度模式 `type_mask` | 匿名命名空间 `kMaskVelocityBase` / `kMaskVelocityYaw` / `kMaskVelocityYawRate` / `kMaskBrake` | 掩码位组合待实测确认 |
|
||
|
||
### 6.3 接入主工程时需补齐的事项
|
||
|
||
1. **机体补偿（可选）**：机体系→NED 旋转已在库内完成（仅绕下轴按
|
||
`yaw_rad`，规格 §2.3）；`roll_rad` / `pitch_rad` / `yaw_rate_radps` /
|
||
`velocity_*` 字段仍为预留，规格 §4.1 的机体补偿启用时由姿态源填充；
|
||
2. **`TrackedTarget` 主题接线**：`TrackResult` 尚未进入主工程
|
||
`include/common/types.h`；需先按 `../target_tracker/target_tracker.md`
|
||
§7 与规格 §7.2 正式定义 `TrackedTarget` 消息与 `kTrackedTarget` 主题，
|
||
由接线部件完成 `TrackResult` → `TrackedTarget` 转换与控制线程订阅；
|
||
3. **传输实现替换**：把示例的 `LoggingTransport` 换成真实链路实现
|
||
（`ICommandTransport` 注入），打包与控制律零改动；
|
||
4. **距离有效标志来源**：`distance_valid` 由双目链路（StereoRanger）
|
||
给出——距离来自目标检测框自身，控制律不自行判定；多目标场景下
|
||
调用方填 `ControlSnapshot` 时应校验 `StereoTargetDistance.track_id`
|
||
与 `TrackResult.track_id` 一致（v1.0 单目标为调用方约定，纪要 S3，
|
||
库内不加代码）。
|
||
|
||
### 6.4 红线声明
|
||
|
||
- `config/track_control.json` 与 `TrackingConfig` 默认值中**所有数值均为
|
||
占位值**（对应规格第 8 章待定项与需求分析 §7 未标定前提）：相机内参/
|
||
畸变/外参未标定、PID 与限幅参数未整定。标定与实测整定完成前，本库
|
||
**仅限 SITL/台架录制数据回放验证数学链路，不得用于实飞**。
|
||
- 构建环境差异：`../AGENTS.md` 规定 WSL2 Ubuntu 24.04，当前开发机仅
|
||
装有 Ubuntu 22.04，验证在 22.04 完成；安装 24.04 后需复核构建与测试。
|