commit cb00322a501c718fb50dc9db516ed9e711995fad Author: 一喵 Date: Wed Sep 2 15:13:13 2026 +0800 first commit diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..10b731c --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,5 @@ +# 默认忽略的文件 +/shelf/ +/workspace.xml +# 基于编辑器的 HTTP 客户端请求 +/httpRequests/ diff --git a/.idea/code_drone-test.iml b/.idea/code_drone-test.iml new file mode 100644 index 0000000..d0876a7 --- /dev/null +++ b/.idea/code_drone-test.iml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/inspectionProfiles/Project_Default.xml b/.idea/inspectionProfiles/Project_Default.xml new file mode 100644 index 0000000..130a849 --- /dev/null +++ b/.idea/inspectionProfiles/Project_Default.xml @@ -0,0 +1,27 @@ + + + + \ No newline at end of file diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000..49af811 --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.zcode/plans/plan-sess_b7314a3c-d6f0-45f0-9768-58161c9cf637.md b/.zcode/plans/plan-sess_b7314a3c-d6f0-45f0-9768-58161c9cf637.md new file mode 100644 index 0000000..2c6af37 --- /dev/null +++ b/.zcode/plans/plan-sess_b7314a3c-d6f0-45f0-9768-58161c9cf637.md @@ -0,0 +1,49 @@ +# C++ → Python 移植计划（reference-c++ → utils） + +## 范围与环境 +- 移植 `reference-c++/target_tracker` 与 `reference-c++/visual_track_control` 两个子项目为 Python，放入 `utils/`；**不改动 reference-c++ 任何文件**。 +- Python 解释器：`D:\program file\conda\envs\pytorch\python.exe`（3.12.13，mavsdk 3.15.3 / numpy 2.4.3 已装）。需向该环境 `pip install pytest`。 +- 行为保真优先：单位、符号约定、限幅顺序、超龄/判丢/Coast 语义、异常消息文本全部与 C++ 版及规格文档一致。 + +## 目录布局 +``` +utils/ +├── README.md # 唯一文档（结构、运行、MAVSDK 映射决策、实机接线片段） +├── conftest.py # pytest sys.path 注入，使两包可顶层导入 +├── target_tracker/ # 包1：追踪库（仅依赖 stdlib + numpy） +│ ├── __init__.py # 导出 TargetTracker/TrackResult/Detection/FrameShape 等 +│ ├── kalman_box_filter.py # 8 维恒速卡尔曼（numpy 矩阵；Q 用状态值、R 用 abs×2 的规格差异保留） +│ ├── tracklet.py # 轨道 + ToCenterSize/ToXYXY/ComputeIoU +│ ├── target_tracker.py # 六步主流程 + 贪婪关联 + 主目标锁定；LastResult 用 threading.Lock +│ └── tests/ # pytest 移植 27 条（kalman 5 / tracklet 12 / tracker 10，含 §9.4 六条验收） +├── visual_track_control/ # 包2：控制律（不 import target_tracker） +│ ├── __init__.py +│ ├── vtc_types.py # TrackObservation/ControlSnapshot/ControlOutput/枚举（frozen dataclass） +│ ├── vtc_config.py # stdlib json 加载，校验异常消息与 C++ 一致（含字段路径） +│ ├── line_of_sight.py / pid_controller.py / rate_limiter.py / staleness_checker.py / frame_transform.py +│ ├── tracking_control_law.py # 八步单周期主流程，logging 仅模式切换打点 +│ ├── offboard_sender.py # OffboardSender 协议 + YawIntegrator + LoopbackSender（演示/测试） +│ ├── mavsdk_sender.py # MAVSDK 实现（mavsdk 懒加载，其余模块零感知） +│ ├── config/track_control.json # 从 C++ 原样拷贝 +│ └── tests/ # pytest 移植 8 套 +└── examples/ + └── track_control_demo.py # 唯一同时 import 两包的胶水点 +``` + +## 关键设计 +1. **解耦边界**：C++ 版 ControlSnapshot 内嵌 tracker::TrackResult；Python 改为控制律自有的最小结构 `TrackObservation(tracked, is_predicted, center)`。visual_track_control 全程不出现 target_tracker 导入，demo 负责字段拷贝。两包内部一律相对导入、可独立搬移；mavsdk 只在 mavsdk_sender.py 内懒加载。 +2. **MAVSDK 映射（已确认方案）**：统一 `set_velocity_ned(VelocityNedYaw)`。 + - 速率式：`YawIntegrator` 把 yaw_rate 积分为绝对航向（wrap180，dt 取发送侧单调时钟差）→ VelocityNedYaw(vn, ve, vd, 积分航向)； + - 位置式：直接用 out.yaw_deg；刹车：零速 + 保持积分航向；降级：不发送。 + - 控制律输出仍与 C++ 一致（机体系计算限幅→输出端旋转为 NED），NED 分量直接对应 MAVSDK 字段；OffboardError 捕获后返回 False（对齐 C++ Send 布尔语义）。offboard.start()/stop() 生命周期不属于 sender，README 给接线片段。 +3. **数值**：全程 Python float（双精度），卡尔曼用 numpy（4×4 求逆 + 1e-9 正则化）；保留检测入口 NaN/Inf 过滤、置信度 NaN→−∞ 排序等防御逻辑；时间戳 `time.monotonic_ns()//10**6`。 +4. **MAVSDK sender 测试**：用 fake offboard 对象（鸭子类型）验证分发与积分行为，不 import mavsdk；gateway 相关 C++ 字节级用例改为验证 sender 收到的 VelocityNedYaw 内容。 + +## 验证与交付 +1. `pip install pytest`（pytorch 环境）→ 两包测试全绿； +2. 运行 demo（asyncio，20Hz，三阶段：正常 6s → Coast 4s → 判丢 3s，演示判丢阈值 60 帧），输出语义与 C++ 版一致； +3. mavsdk_sender 冒烟测试（fake offboard，不连实机）； +4. 撰写 utils/README.md。 + +## 实施顺序 +pip 安装 pytest → target_tracker 包+测试 → visual_track_control 包+测试 → sender 层+测试 → demo → README → 汇总报告。 \ No newline at end of file diff --git a/docs/物理追踪思路.md b/docs/物理追踪思路.md new file mode 100644 index 0000000..5dc77c0 --- /dev/null +++ b/docs/物理追踪思路.md @@ -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）。 diff --git a/docs/视觉跟踪控制律技术路线.md b/docs/视觉跟踪控制律技术路线.md new file mode 100644 index 0000000..3307769 --- /dev/null +++ b/docs/视觉跟踪控制律技术路线.md @@ -0,0 +1,1029 @@ +# 视觉跟踪控制律（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`）；雷达提供机头前向单点距离与 +**关联标志**；飞控链路提供自机姿态（至少航向角）。 + +**下游依赖**：只有一个——传输抽象。打包与传输分离，控制律不直接写飞控。 + +**上层归属**：状态机（S6/S7/S8 切换、目标丢失策略、障碍接管）在**本模块之外**。 +本模块只按输入标志产生"当拍应该做什么"，不决定"要不要继续视觉闭环"。 + +### 1.3 设计特点 + +| 特点 | 含义 | 带来的约束 | +|------|------|-----------| +| **纯值输入 / 纯值输出** | 每拍传入一个不可变快照，返回一个输出值对象 | 无隐藏输入、无回调、可离线回放 | +| **单线程固定周期** | 由控制线程按配置频率调用，构件内部**无锁** | 不得从多线程并发调用同一实例 | +| **热路径零动态分配** | 所有系数、缓冲在构造期备妥；每拍只做算术 | 便于实时性分析与嵌入式移植 | +| **热路径零日志** | 仅在输出模式发生切换时打一条日志 | 日志量与状态变化次数成正比，不与频率成正比 | +| **配置全量外置** | 所有阈值/增益/限幅来自启动期加载的配置对象 | 代码内**不得**出现任何写死的控制参数 | +| **无状态纯函数构件** | 视线角转换、超龄判定、坐标旋转均为纯函数 | 可独立单元测试，无需夹具 | +| **单一 offboard 消息类型** | 全周期只发一种消息（ID 84） | 避免消息类型逐拍翻转账扰动飞控（见 §10.1） | + +### 1.4 明确不做的事（边界） + +- **不做感知**：不跑检测/光流/雷达驱动，只消费调用方填入的快照； +- **不做目标关联**：相机—雷达空间关联由感知融合层给出布尔标志，本模块只读取； +- **不做状态机仲裁**：不判断何时进入/退出视觉跟踪，不实现目标丢失策略； +- **不做链路 IO**：不打开串口、不管理重连、不解析任何入向 MAVLink 消息； +- **不做配置热更新**：启动期加载一次，调参重启生效； +- **不做多目标**：只处理追踪库选定的主目标。 + +--- + +## 2. 输入输出接口定义 + +### 2.1 输入：控制快照（每拍一个，由调用方采集并打戳） + +| 分组 | 字段 | 单位 / 类型 | 语义与约束 | +|------|------|------------|-----------| +| 视觉 | `track.center` | 像素 (x, y) | 主目标中心；原点为画面**左上角**，x 右正、y 下正 | +| 视觉 | `track.tracked` | 布尔 | 是否存在有效轨道；`false` → 判丢刹车 | +| 视觉 | `track.is_predicted` | 布尔 | 本拍是否为滤波器预测值（Coast）；`true` → 清距离积分 | +| 视觉 | `track_time_ms` | 单调时钟 ms | **取样瞬间**打戳；`<= 0` 视为从未取样，直接判超龄 | +| 雷达 | `radar_distance_m` | m | 机头前向单点距离；`<= 0` 视为不可用 | +| 雷达 | `associated_to_target` | 布尔 | 相机—雷达关联成立标志；**未关联不得当作目标距离** | +| 雷达 | `radar_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` | `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: + 若 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) + + # ---- ⑦ 距离通道 ---- + 距离可用 ← 快照.associated_to_target + 且 快照.radar_distance_m > 0 + 且 非 已超龄(快照.radar_time_ms, now_ms, 配置.radar_stale_ms) + + 若 距离可用: + Δd ← 快照.radar_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) + 退出近距操作: 输出.mode ← kDegradedHold + 输出.degraded_reason ← kNoDistanceExit + 返回 收尾并记录模式切换(输出, "距离未关联，退出近距操作") + + # ---- ⑧ 机体系 → 局部 NED 旋转（输出端） ---- + 旋转机体到NED(输出.vx_mps, 输出.vy_mps, 输出.vz_mps, 快照.yaw_rad) + + 输出.valid ← true + 返回 收尾并记录模式切换(输出, 输出.coast_active ? "Coast 预测跟踪" : "正常跟踪") +``` + +### 5.3 提前返回的语义差异（易错点） + +三条提前返回路径**跳过第 ⑧ 步旋转**，这是安全的：这些路径的速度分量全为零， +零向量旋转后仍是零向量。但重写时必须注意： + +| 返回路径 | 是否清 PID | 是否清限幅器 | 速度是否旋转 | `valid` | +|---------|-----------|------------|------------|---------| +| 姿态超龄降级 | 否 | 否 | 否 | `false` | +| 视觉超龄降级 | 否 | 否 | 否 | `false` | +| 判丢刹车 | 是（进入沿） | 是（进入沿） | 否 | `true` | +| 无距离退出 | 是（仅积分） | 否 | 否 | `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 判据基于"真实饱和边界"。 + +**可用性三重门**（缺一不可）：关联标志成立 **且** 距离为正 **且** 未超龄。 +"未关联的雷达距离"只是机头前方某个障碍物的距离，把它当作目标距离会导致 +朝着错误目标调整射程。 + +### 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 +姿态不可用 > 视觉超龄 > 判丢 > 无距离（退出档） +``` + +| 优先级 | 检查 | 理由 | +|-------|------|------| +| 1 | 姿态过期/缺失 | 连坐标旋转与绝对航向合成都做不了，任何空间量都不可信（规格：禁止空间转换和控制） | +| 2 | 视觉超龄 | 视觉是"是否判丢"的唯一来源；数据过期时连 `tracked` 标志本身都不可信，因此**必须先于判丢**处理 | +| 3 | 判丢 | 视觉可信且明确报告无轨道 → 零速刹车 | +| 4 | 无距离 | 只有在"视觉可信且正在跟踪"前提下才谈得上距离降级 | + +### 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 期间距离通常不可用**：目标短暂消失往往伴随雷达关联失效，因此第 ⑦ 步会 +走无距离分支（默认"保持"→ 前向清零）。这是设计上的自然降级，不需要额外分支。 + +### 7.5 无距离三档动作 + +| 配置值 | 行为 | 适用场景 | +|--------|------|---------| +| `hold`（默认） | 前向速度清零，限幅器复位到 0 | 安全占位语义：不知道距离就不前冲 | +| `slow_approach` | 以配置的低速限幅缓速接近（仍经加速度限幅） | 需要持续压迫、且低速可接受风险 | +| `exit` | 整体降级保持（`kDegradedHold` / `kNoDistanceExit`），不发送 | 距离是安全前提（如避撞），无距离必须退出近距操作 | + +三档中只有 `exit` 会中断整拍输出；`hold` 与 `slow_approach` 仍输出航向与垂直 +通道（继续把目标保持在画面中心）。 + +--- + +## 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 | 视觉结果超龄阈值 | +| `radar_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` | 枚举 | `hold` | `hold` / `slow_approach` / `exit` | +| `no_distance_approach_limit_mps` | m/s | 0.5 | `slow_approach` 档的限速值 | + +### 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：降级是"这一拍不输出"，恢复后应平滑续接；刹车是真实控制中断， +恢复后必须从零渐变。两者语义不同，清理策略必须不同。 + +--- + +## 11. 实现与重写指引 + +### 11.1 必须持久化的状态（漏一个就变行为） + +| 状态 | 所属 | 初值 | 何时改变 | +|------|------|------|---------| +| 上拍时刻 | 主控律 | 0（表示首拍） | 每拍写入当前时刻 | +| 上拍输出模式 | 主控律 | 降级保持 | 仅模式切换时写入（并触发日志） | +| 积分累计 | 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 | 若需要侧移则新增通道 | 低 | +| **机体补偿** | 快照的滚转/俯仰/角速度字段预留未用 | 接入主工程姿态源后评估 | 低 | +| **相机标定** | 内参/畸变/安装外参未标定，针孔模型无畸变补偿 | 标定 | 中：视线角系统性偏差 | +| **雷达关联判据** | 由感知融合层给出标志，本模块不实现 | 融合层设计 | 中：关联错误 = 距离通道朝错误目标调射程 | + +### 12.3 红线 + +1. **未标定不得实飞**：配置与代码默认值中所有数值均为占位，标定与实测整定 + 完成前，仅限 SITL / 台架 / 录制数据回放验证数学链路。 +2. **不得混用未标注坐标系的数据**：对外输出的速度分量已声明为局部 NED， + 任何改动都必须同步更新声明与打包侧坐标框。 +3. **不得用超龄数据生成正常指令**：超龄只能触发降级或告警。 +4. **不得绕过限幅**：任何新增通道都必须内联"变化率 + 幅值"双重限幅， + 包括切换后的首条指令。 +5. **未关联的雷达距离不得当作目标距离**。 + +--- + +## 附：与追踪库的行为衔接速查 + +| 追踪库输出 | 本模块响应 | 位置 | +|-----------|-----------|------| +| `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 | diff --git a/docs/视觉追踪技术路线.md b/docs/视觉追踪技术路线.md new file mode 100644 index 0000000..5374bd2 --- /dev/null +++ b/docs/视觉追踪技术路线.md @@ -0,0 +1,685 @@ +# 多目标追踪过滤器（TargetTracker）技术路线 + +> 本文档将 `drone_controller/target_tracker.py` 的实现抽象为与编程语言无关的技术路线， +> 可作为 C++（或任意语言）重新实现的规格说明。所有算法细节、参数默认值均以现有代码 +> 实际实现为准，未包含代码中不存在的功能。 + +--- + +## 目录 + +1. [概述与系统定位](#1-概述与系统定位) +2. [输入输出接口定义](#2-输入输出接口定义) +3. [核心数据结构](#3-核心数据结构) +4. [卡尔曼滤波算法细节](#4-卡尔曼滤波算法细节) +5. [检测-轨迹关联流程](#5-检测-轨迹关联流程) +6. [轨迹生命周期与 ID 管理](#6-轨迹生命周期与-id-管理) +7. [主流程伪代码（每帧处理）](#7-主流程伪代码每帧处理) +8. [关键参数表](#8-关键参数表) +9. [C++ 移植要点与模块划分建议](#9-c-移植要点与模块划分建议) + +--- + +## 1. 概述与系统定位 + +### 1.1 模块职责 + +本模块是一个**轻量级多目标追踪过滤器**，位于无人机视觉闭环控制链路中 +"目标检测"与"飞控闭环"之间，承担三类职责： + +1. **平滑**：检测器（YOLO 推理）输出的边界框逐帧抖动，通过恒定速度卡尔曼滤波 + 输出平滑后的目标中心点与边界框，供下游速度闭环（PID）消费。 +2. **抗短暂丢失（Coast 预测）**：目标短暂消失（如 2~5 帧）时，不立即判丢， + 而是用滤波器预测值继续输出目标位置（`is_predicted = true`）， + 避免控制输出突变导致机体抽搐。 +3. **ID 稳定与主目标锁定**：多目标场景下通过"中心距离 + IoU 加权代价"的贪婪 + 关联维持稳定 ID；选定主目标（primary）后持续锁定，直到该轨道真正死亡， + 避免被新出现的高置信度目标抢走导致"甩头"。 + +### 1.2 在系统中的位置 + +``` + 摄像头/RTSP ──► NPU 推理(YOLO) ──► 后处理检测框 + │ + ▼ + ┌──────────────────┐ + │ TargetTracker │ ◄── 每帧调用 update() + │ (本模块) │ + └──────────────────┘ + │ 平滑中心点 / 框 / 状态标志 + ┌────────────────┼────────────────┐ + ▼ ▼ ▼ + 20Hz 控制线程 Coast 期间 tracked=false + PID → 速度指令 清空 PID 积分 触发零速刹车 +``` + +- **调用时机**：由推理主循环在每一帧检测完成后调用一次 `update()`； + 结果由独立的控制线程（约 20Hz 速度闭环）读取消费。 +- **上游行为约定**（来自调用方，供移植时对齐）： + - 输出 `is_predicted == true`（Coast 中）时，调用方会清空 PID 积分项， + 防止积分饱和（windup）； + - 输出 `tracked == false`（彻底丢失）时，调用方执行零速刹车； + - 停止视觉闭环时调用方会调用 `reset()` 清空全部追踪状态。 + +### 1.3 设计特点 + +- 恒定速度模型，8 维状态向量，纯线性卡尔曼滤波（无 UKF/EKF）。 +- 关联采用**贪婪最邻近**（按轨道置信度降序逐一匹配），不使用匈牙利算法； + 代价函数为中心距离与 (1−IoU) 的加权和。 +- 所有坐标使用**原始像素坐标**（与检测后处理输出空间一致），不做归一化。 +- 时间步长以"帧"为单位（dt = 1），与帧率无关，参数按约 30fps、640×480 画面调优。 + +--- + +## 2. 输入输出接口定义 + +### 2.1 主要接口 + +| 接口 | 说明 | +|------|------| +| `构造(max_lost_frames, max_association_dist, min_hits, dist_weight, iou_weight)` | 创建追踪器，参数见[第 8 节](#8-关键参数表) | +| `update(detections, frame_shape) → TrackResult` | 每帧调用一次，输入检测列表，输出主目标追踪结果 | +| `get_last_result() → TrackResult` | 获取上一帧结果快照（线程安全读取场景使用） | +| `reset()` | 清空所有轨道、ID 计数器、锁定状态与缓存结果 | +| `active_count() → int` | 当前存活轨道数量 | +| `active_tracks() → {track_id: Tracklet}` | 当前所有轨道的只读快照（浅复制） | + +### 2.2 输入：detections + +每帧检测列表，每个元素为 6 元组： + +``` +[x1, y1, x2, y2, conf, cls_id] +``` + +| 字段 | 类型 | 含义 | +|------|------|------| +| x1, y1 | 浮点 | 边界框左上角（像素坐标） | +| x2, y2 | 浮点 | 边界框右下角（像素坐标） | +| conf | 浮点 | 检测置信度 ∈ [0,1] | +| cls_id | 整数 | 类别编号 | + +- 允许传入空列表（表示本帧无任何检测），此时所有轨道进入 Coast 计数。 +- `frame_shape = (height, width)`，可选。仅在"无实测轨道、只有 Coast 轨道" + 时用于选取主目标的兜底排序（选最接近画面中心的轨道）；未提供时按 + 640×480 的画面中心 (320, 240) 计算。 + +### 2.3 输出：TrackResult + +`update()` 每帧返回一个结构体（代码中以字典表示），字段如下： + +| 字段 | 类型 | 含义 | +|------|------|------| +| tracked | 布尔 | 是否存在有效主目标。false 时调用方应刹车 | +| primary_id | 整数或空 | 主目标轨道 ID；无目标时为空 | +| center | (cx, cy) 或空 | 平滑后的目标中心点（像素坐标） | +| box | [x1,y1,x2,y2] 或空 | 平滑后的边界框 | +| confidence | 浮点 | 当前置信度（Coast 期间继承最近一次实测的置信度） | +| is_predicted | 布尔 | true = 当前帧处于 Coast（仅有预测、无实测） | +| lost_frames | 整数 | 主目标连续未命中帧数 | +| n_active | 整数 | 当前活跃轨道总数 | +| raw | [x1,y1,x2,y2] 或空 | 关联到的原始检测框；Coast 时为空 | + +丢失（无主目标）时的返回值：`tracked=false`，其余定位字段为空，`lost_frames=0`。 + +--- + +## 3. 核心数据结构 + +模块由三层结构组成： + +``` +TargetTracker（多目标管理器） + └── Tracklet × N（单条轨道：生命周期 + 辅助状态） + └── KalmanBoxFilter（单个 8 维恒定速度卡尔曼滤波器） +``` + +### 3.1 KalmanBoxFilter（单目标滤波器） + +| 成员 | 类型 | 含义 | +|------|------|------| +| x | 数组[8] | 状态向量 [cx, cy, w, h, vx, vy, vw, vh] | +| P | 矩阵 8×8 | 状态协方差 | +| F | 矩阵 8×8 | 状态转移矩阵（常量） | +| H | 矩阵 4×8 | 观测矩阵（常量） | +| initialized | 布尔 | 是否已用首次观测初始化 | + +### 3.2 Tracklet（轨道） + +| 成员 | 类型 | 含义 | +|------|------|------| +| track_id | 整数 | 全局唯一轨道 ID | +| kf | KalmanBoxFilter | 本轨道的滤波器 | +| lost_count | 整数 | 当前连续未匹配帧数 | +| max_lost | 整数 | 最大允许连续丢失帧数（构造时注入） | +| confidence | 浮点 | 最近一次匹配的置信度 | +| cls_id | 整数 | 最近一次匹配的类别 ID | +| age | 整数 | 轨道已存在的总帧数（创建时为 1） | +| hits | 整数 | 累计成功匹配次数（创建时为 1） | +| last_center | (cx, cy) | 最近一次中心点（实测或预测） | +| last_box | [x1,y1,x2,y2] 或空 | 最近一次边界框（实测或预测） | + +派生状态（只读属性）： + +- `is_dead := lost_count > max_lost`（可回收） +- `is_coasting := lost_count > 0`（处于纯预测状态） + +### 3.3 TargetTracker（管理器） + +| 成员 | 类型 | 含义 | +|------|------|------| +| max_lost_frames 等 5 项 | 浮点/整数 | 配置参数，见第 8 节 | +| next_id | 整数 | 下一个可分配的轨道 ID（从 0 递增，不复用） | +| tracks | 哈希表 {track_id → Tracklet} | 所有存活轨道 | +| frame_count | 整数 | 已处理帧数（仅统计用） | +| locked_primary_id | 整数或空 | 当前锁定的主目标轨道 ID | +| last_result | TrackResult | 上一帧结果缓存（供跨线程读取） | + +--- + +## 4. 卡尔曼滤波算法细节 + +### 4.1 状态空间模型 + +状态向量（8 维）与观测向量（4 维）： + +\[ +\mathbf{x} = [c_x,\ c_y,\ w,\ h,\ v_x,\ v_y,\ v_w,\ v_h]^T, +\qquad +\mathbf{z} = [c_x,\ c_y,\ w,\ h]^T +\] + +其中 \(c_x, c_y\) 为框中心像素坐标，\(w, h\) 为框宽高，后四项为对应速度 +（单位：像素/帧）。 + +**状态转移**（恒定速度模型，dt = 1 帧）： + +\[ +\mathbf{x}_k = F\,\mathbf{x}_{k-1},\quad +F = \begin{bmatrix} I_4 & I_4 \\ 0 & I_4 \end{bmatrix} +\] + +即前 4 维（位置/尺寸）各自加上对应速度：`F[i][i+4] = 1 (i=0..3)`，其余为单位阵。 + +**观测模型**（直接观测位置与尺寸，不观测速度）： + +\[ +\mathbf{z}_k = H\,\mathbf{x}_k,\quad +H = [\,I_4 \mid 0\,] \in \mathbb{R}^{4\times 8} +\] + +### 4.2 噪声参数（经验值，针对 640×480 画面调优） + +| 常量 | 值 | 用途 | +|------|----|------| +| STD_POS | 1/50 = 0.02 | 位置过程噪声比例系数 | +| STD_VEL | 1/200 = 0.005 | 速度过程噪声比例系数 | +| INI_POS | 2.0 | 初始位置协方差（对角） | +| INI_VEL | 100.0 | 初始速度协方差（对角） | + +**关键设计：噪声与目标尺寸成比例（动态 Q/R）。** +预测时过程噪声 Q 按当前状态中的目标宽高缩放： + +\[ +\sigma_{pos} = [\tfrac{w}{50},\ \tfrac{h}{50},\ \tfrac{w}{50},\ \tfrac{h}{50}], +\quad +\sigma_{vel} = [\tfrac{w}{200},\ \tfrac{h}{200},\ \tfrac{w}{200},\ \tfrac{h}{200}] +\] + +\[ +Q = \mathrm{diag}\big(\sigma_{pos}^2,\ \sigma_{vel}^2\big) +\] + +（Q 为 8×8 对角阵，前 4 个对角元来自 \(\sigma_{pos}\)，后 4 个来自 \(\sigma_{vel}\)； +每个分量先平方再上对角。） + +更新时观测噪声 R 同样按当前状态的宽高缩放，并额外乘 2 增强平滑： + +\[ +R = \mathrm{diag}\big(2\,(\tfrac{|w|}{50})^2,\ 2\,(\tfrac{|h|}{50})^2,\ 2\,(\tfrac{|w|}{50})^2,\ 2\,(\tfrac{|h|}{50})^2\big) +\] + +> 实现注意：R 中使用 `abs(w)`、`abs(h)` 防御负尺寸；Q 中直接使用状态值。 +> 该差异与代码保持一致即可，正常情形下尺寸为正，二者等价。 + +### 4.3 初始化（initiate） + +用首次观测 \([c_x, c_y, w, h]\) 初始化： + +- 状态：\(\mathbf{x} = [c_x, c_y, w, h, 0, 0, 0, 0]^T\)（速度置零） +- 协方差：\(P = \mathrm{diag}(2, 2, 2, 2, 100, 100, 100, 100)\) + （位置置信度中等，速度置信度低，允许前几帧快速收敛速度估计） + +### 4.4 预测（predict） + +未初始化时返回空。否则： + +\[ +\mathbf{x} \leftarrow F\,\mathbf{x}, \qquad +P \leftarrow F\,P\,F^T + Q +\] + +返回预测的位置/尺寸 \([c_x, c_y, w, h]\)（状态向量前 4 维的副本）。 + +### 4.5 更新（update） + +未初始化时直接返回。给定观测 \(\mathbf{z} = [c_x, c_y, w, h]\)： + +\[ +S = H\,P\,H^T + R +\] + +\[ +K = P\,H^T\,S^{-1} +\] + +\[ +\mathbf{y} = \mathbf{z} - H\,\mathbf{x} \quad \text{（残差 / innovation）} +\] + +\[ +\mathbf{x} \leftarrow \mathbf{x} + K\,\mathbf{y}, \qquad +P \leftarrow (I_8 - K\,H)\,P +\] + +> 实现注意：S 为 4×4 矩阵，代码中使用通用矩阵求逆。C++ 中可用 +> 4×4 专用求逆（如高斯消元或 LDL 分解），无需引入大型线性代数库。 +> 代码未对位置/尺寸做画面边界钳制，也未做负尺寸约束，移植时保持一致 +> （若需增强鲁棒性可另行添加，但属于行为变更）。 + +--- + +## 5. 检测-轨迹关联流程 + +### 5.1 代价函数 + +对每条轨道（预测后的中心 \((p_x, p_y)\)、预测框 \(B_t\)）与每个检测 +（中心 \((d_x, d_y)\)、检测框 \(B_d\)），定义加权代价： + +\[ +\mathrm{cost} = w_{dist}\cdot \frac{\lVert (p_x,p_y)-(d_x,d_y) \rVert_2}{D_{max}} +\;+\; w_{iou}\cdot (1 - \mathrm{IoU}(B_t, B_d)) +\] + +- \(\lVert\cdot\rVert_2\) 为中心点欧氏距离（像素）； +- \(D_{max}\) 为距离软阈值（`max_association_dist`，默认 200 px）， + 用于把距离项归一化到与 IoU 项同量级；实际使用时取 + \(\max(D_{max},\ 10^{-3})\) 防止除零； +- 默认 \(w_{dist} = w_{iou} = 0.5\)。 + +**匹配有效性判据**：仅当 `cost < 1.0` 时视为有效匹配 +（实现上：每条轨道的最优代价初始化为 1.0，只有严格小于 1.0 的候选才会被记录）。 + +### 5.2 IoU 计算 + +轴对齐框 \([x_1,y_1,x_2,y_2]\) 的标准交并比： + +\[ +\mathrm{IoU}(A,B) = \frac{\mathrm{area}(A \cap B)}{\mathrm{area}(A) + \mathrm{area}(B) - \mathrm{area}(A \cap B)} +\] + +交集宽高需与 0 取最大值；任一框为空、交集面积 ≤ 0 或并集 ≤ 0 时返回 0。 + +### 5.3 贪婪关联算法（非匈牙利） + +按**轨道置信度从高到低**排序后逐一贪心匹配（高置信轨道优先抢占检测）： + +``` +ASSOCIATE(tracks, detections): + 若 tracks 或 detections 为空： + 返回 matches=[], unmatched_tracks=全部轨道, unmatched_dets=全部检测 + + 预计算每个检测的中心与框 + + sorted_tracks ← 按 track.confidence 降序排序的轨道列表 + assigned_dets ← 空集合; assigned_tracks ← 空集合; matches ← [] + + 对 sorted_tracks 中每条轨道 t： + 若 t.id ∈ assigned_tracks：跳过 + best_cost ← 1.0; best_det ← 空 + 对每个未分配的检测 d： + dist ← 欧氏距离(t.last_center, d.center) + norm_dist ← dist / max(Dmax, 1e-3) + iou ← IoU(t.last_box, d.box) + cost ← w_dist × norm_dist + w_iou × (1 − iou) + 若 cost < best_cost： + best_cost ← cost; best_det ← d + 若 best_det 非空： + matches.append((t.id, best_det.idx)) + assigned_dets.add(best_det.idx); assigned_tracks.add(t.id) + + unmatched_tracks ← tracks 中不在 assigned_tracks 的轨道 + unmatched_dets ← detections 中不在 assigned_dets 的检测 + 返回 (matches, unmatched_tracks, unmatched_dets) +``` + +复杂度：O(M×N)（M 条轨道 × N 个检测），另有 O(M log M) 排序。 +在机载小目标数量（通常个位数）下开销可忽略。 + +> 说明：代码明确选择贪心而非匈牙利最优匹配——贪心实现简单、无第三方依赖， +> 配合"置信度优先 + 距离/IoU 加权"在无人机跟随单主目标的场景下已足够稳定。 + +--- + +## 6. 轨迹生命周期与 ID 管理 + +### 6.1 生命周期状态 + +代码中没有显式状态枚举，但由字段组合可归纳出四个阶段： + +| 阶段 | 判定条件 | 行为 | +|------|----------|------| +| 新建（未确认） | `hits < min_hits` | 正常参与关联与预测，但主目标选举时优先级低于已确认轨道 | +| 已确认 | `hits ≥ min_hits` | 主目标选举的优先候选池 | +| Coast（滑行预测） | `0 < lost_count ≤ max_lost` | 无实测，输出滤波器预测值，`is_predicted=true` | +| 死亡 | `lost_count > max_lost` | 当帧即从轨道表中删除 | + +注意两点（与代码一致）： + +1. **min_hits 不阻止轨道参与关联或对外输出**，它只影响"重新选择主目标"时 + 的候选池优先级（见 6.3）。这与 SORT 中 tentative 轨道不输出的做法不同。 +2. **死亡判定使用严格大于**：`lost_count > max_lost`，即默认配置下允许 + 连续 8 帧 Coast，第 9 帧未命中才回收。 + +### 6.2 ID 分配策略 + +- 全局计数器 `next_id` 从 0 开始，每创建一个新轨道自增 1； +- **ID 永不复用**；`reset()` 时计数器归零； +- 未匹配到任何现有轨道的检测，每个都会创建一个新轨道（无预过滤， + 不要求检测置信度下限——上游检测器已完成阈值过滤）。 + +### 6.3 主目标（primary）锁定与选举 + +对外每帧只输出**一个**主目标，供控制回路使用。选择规则： + +``` +SELECT_PRIMARY: + 若轨道表为空：清除锁定，返回 tracked=false + + ① 锁定延续：若 locked_primary_id 非空且该轨道仍存活： + 直接以该轨道输出（Coast 中照样输出，is_predicted=true） + —— 不被其他更高置信度目标抢占 + + ② 否则（无锁定 / 锁定目标已死亡被回收）→ 重新选举并锁定： + measured ← 所有 lost_count == 0 的轨道（本帧有实测） + coasting ← 其余存活轨道 + + 若 measured 非空： + confirmed ← measured 中 hits ≥ min_hits 者 + pool ← confirmed（若为空则退回 measured） + chosen ← pool 中 confidence 最大者 + 否则若 coasting 非空： + chosen ← coasting 中离画面中心最近者（欧氏距离平方排序） + is_predicted ← true + 否则： + 清除锁定，返回 tracked=false + + locked_primary_id ← chosen.id + 以 chosen 输出结果 +``` + +锁定的**唯一解除途径**：锁定轨道死亡被回收时清除 `locked_primary_id` +（在轨道删除的同一处处理），或调用 `reset()`。 + +### 6.4 生命周期状态图 + +```mermaid +graph TB + A[未匹配检测] --> B[新建轨道 hits=1 lost=0] + B -->|每帧匹配成功| C{hits >= min_hits?} + C -->|否, 选举低优先级| B + C -->|是, 已确认| D[正常跟踪 lost=0] + D -->|某帧未匹配| E[Coast lost_count+1 输出预测值] + E -->|重新匹配| D + E -->|lost_count > max_lost| F[死亡回收 删除轨道] + B -->|连续未匹配| E + F -->|若是锁定主目标| G[清除锁定 下帧重新选举] +``` + +--- + +## 7. 主流程伪代码（每帧处理） + +`update(detections, frame_shape)` 的完整单帧流程： + +``` +UPDATE(detections, frame_shape): + frame_count += 1 + + # ---- 步骤 1：所有存活轨道先行预测（推进一帧）---- + 对 tracks 中每条轨道 t： + t.predict() # 卡尔曼预测；更新 t.last_center / t.last_box；age += 1 + + # ---- 步骤 2：构造检测中心表并关联 ---- + 若 detections 非空： + det_centers ← [] + 对每个检测 i = [x1,y1,x2,y2,conf,cls]： + cx ← (x1+x2)/2; cy ← (y1+y2)/2 + w ← x2−x1; h ← y2−y1 + det_centers.append((i, cx, cy, w, h, conf, cls)) + (matches, unmatched_tracks, unmatched_dets) ← ASSOCIATE(tracks, det_centers) + 否则： + matches ← []; unmatched_tracks ← 全部轨道; unmatched_dets ← [] + + # ---- 步骤 3：用匹配到的检测更新轨道 ---- + 对每个匹配 (tid, did)： + 由 det_centers[did] 重建原始框 raw_box = [cx−w/2, cy−h/2, cx+w/2, cy+h/2] + tracks[tid].update(raw_box, conf, cls) + # 内部：框→[cx,cy,w,h] → kf.update；lost_count←0；hits+1；age+1 + # 更新 last_center / last_box / confidence / cls_id + + # ---- 步骤 4：未匹配轨道计丢失，死亡者回收 ---- + 对每个 tid ∈ unmatched_tracks： + tracks[tid].mark_miss() # lost_count+1, age+1 + 若 tracks[tid].is_dead： # lost_count > max_lost + 若 tid == locked_primary_id：locked_primary_id ← 空 + 从 tracks 中删除 tid + + # ---- 步骤 5：未匹配检测 → 创建新轨道 ---- + 对每个 did ∈ unmatched_dets： + 以 (next_id, [cx,cy,w,h], conf, cls, max_lost=max_lost_frames) 创建 Tracklet + new_track.last_box ← 由中心/宽高重建的框 # 保证创建当帧即可参与输出 + tracks[next_id] ← new_track + next_id += 1 + + # ---- 步骤 6：选取/延续主目标，组装输出 ---- + result ← SELECT_PRIMARY(frame_shape) + last_result ← result # 缓存供跨线程读取 + 返回 result +``` + +其中 `Tracklet.predict()` 与 `Tracklet.update()`： + +``` +TRACKLET_PREDICT(): + pred ← kf.predict() + 若 pred 非空： + last_center ← (pred.cx, pred.cy) + last_box ← center_size → [x1,y1,x2,y2] + age += 1 + 返回 (last_center, last_box) + +TRACKLET_UPDATE(box=[x1,y1,x2,y2], conf, cls): + cx ← (x1+x2)/2; cy ← (y1+y2)/2 + w ← x2−x1; h ← y2−y1 + kf.update([cx, cy, w, h]) + last_center ← (cx, cy); last_box ← box + confidence ← conf; cls_id ← cls + lost_count ← 0; hits += 1; age += 1 +``` + +每帧处理的时序图： + +```mermaid +graph TB + A[接收本帧检测] --> B[全部轨道卡尔曼预测] + B --> C{有检测?} + C -->|是| D[中心距离+IoU 贪婪关联] + C -->|否| E[全部轨道标记 miss] + D --> F[匹配轨道: 卡尔曼更新] + D --> G[未匹配轨道: miss 计数] + D --> H[未匹配检测: 新建轨道] + G --> I{lost > max_lost?} + I -->|是| J[回收轨道 必要时解除锁定] + I -->|否| K[主目标选择/锁定延续] + E --> I + F --> K + H --> K + J --> K + K --> L[输出 TrackResult] +``` + +--- + +## 8. 关键参数表 + +以下默认值全部取自代码实际实现。 + +### 8.1 追踪器配置参数 + +| 参数 | 默认值 | 含义与调优建议 | +|------|--------|----------------| +| max_lost_frames | 8 | 最大连续丢失帧数（约 250~300ms @ 30fps）。越大越能容忍遮挡，但 Coast 漂移时间越长；判丢后下游刹车 | +| max_association_dist | 200.0（像素） | 中心距离软阈值，用于归一化距离代价。目标运动快/帧率低时调大 | +| min_hits | 3 | 轨道"确认"所需累计匹配次数，仅影响主目标选举优先级 | +| dist_weight | 0.5 | 关联代价中归一化距离项权重 | +| iou_weight | 0.5 | 关联代价中 (1−IoU) 项权重。两项建议之和保持 1.0，使 cost<1.0 判据语义稳定 | + +> 工程部署时以上参数可由外部配置文件注入（项目中即通过配置文件的 +> `target_tracker` 段传入，未配置项回落到代码默认值）。 + +### 8.2 卡尔曼滤波内部常量 + +| 常量 | 值 | 含义 | +|------|----|------| +| dt | 1.0 | 状态转移步长，以"帧"为单位 | +| STD_POS | 1/50 | 位置过程/观测噪声比例系数（乘以目标宽高） | +| STD_VEL | 1/200 | 速度过程噪声比例系数（乘以目标宽高） | +| INI_POS | 2.0 | 初始位置协方差 | +| INI_VEL | 100.0 | 初始速度协方差 | +| R 缩放因子 | 2.0 | 观测噪声额外加倍，增强输出平滑 | +| 匹配代价上限 | 1.0 | cost ≥ 1.0 判为不可匹配 | +| Coast 兜底画面中心 | (320, 240) | frame_shape 缺省时按 640×480 计算 | + +--- + +## 9. C++ 移植要点与模块划分建议 + +### 9.1 模块 / 类划分草案 + +与代码三层结构一一对应： + +``` +target_tracker/ + ├── kalman_box_filter.h/.cpp // class KalmanBoxFilter + ├── tracklet.h/.cpp // struct/class Tracklet + └── target_tracker.h/.cpp // class TargetTracker + struct TrackResult +``` + +```cpp +// ---- 基础几何量 ---- +struct BoxXYXY { double x1, y1, x2, y2; }; +struct BoxCXCYWH{ double cx, cy, w, h; }; + +// ---- 单目标滤波器 ---- +class KalmanBoxFilter { + double x[8]; // 状态向量 + double P[8][8]; // 协方差 + static constexpr double F[8][8] = /* I4 右上 I4 */; + static constexpr double H[4][8] = /* [I4 | 0] */; + bool initialized = false; + + void initiate(const BoxCXCYWH& z); + bool predict(BoxCXCYWH& out); // 未初始化返回 false + void update(const BoxCXCYWH& z); +}; + +// ---- 轨道 ---- +struct Tracklet { + int track_id; + KalmanBoxFilter kf; + int lost_count = 0; + int max_lost; + double confidence; + int cls_id; + int age = 1; + int hits = 1; + std::pair last_center; + std::optional last_box; // 首帧预测前可能为空（创建时即赋实测框） + + bool is_dead() const { return lost_count > max_lost; } + bool is_coasting() const { return lost_count > 0; } +}; + +// ---- 输出结构 ---- +struct TrackResult { + bool tracked = false; + std::optional primary_id; + std::pair center; // tracked=false 时无效 + std::optional box; + double confidence = 0.0; + bool is_predicted = false; + int lost_frames = 0; + int n_active = 0; + std::optional raw; +}; + +// ---- 管理器 ---- +class TargetTracker { +public: + struct Config { + int max_lost_frames = 8; + double max_association_dist = 200.0; + int min_hits = 3; + double dist_weight = 0.5; + double iou_weight = 0.5; + }; + explicit TargetTracker(const Config& cfg); + TrackResult update(const std::vector& dets, + std::optional frame_shape); + TrackResult last_result() const; // 需要跨线程时加锁/原子快照 + void reset(); +}; +``` + +### 9.2 STL 容器选择 + +| 代码中的结构 | C++ 建议 | +|--------------|----------| +| 轨道哈希表 `{id: Tracklet}` | `std::unordered_map`；若需按 ID 有序遍历可用 `std::map` | +| 关联中的已分配集合 | `std::unordered_set`，或目标数量少时直接用 `std::vector` 位标记 | +| 按置信度排序的轨道视图 | 复制 ID/指针到 `std::vector` 后 `std::sort`（不直接排哈希表） | +| 检测列表 | `std::vector`，`Detection` 为 POD 结构体 | +| 结果缓存 | `TrackResult` 值类型；跨线程用互斥锁保护的拷贝，或双缓冲原子交换 | + +### 9.3 数值计算 + +- 矩阵规模极小（8×8、4×8、4×4），**无需引入 Eigen/OpenCV**： + 用固定大小二维数组或 `std::array` 手写矩阵乘法即可。 +- 唯一需要通用实现的是 4×4 矩阵求逆（S 矩阵）。可用高斯-约当消元或 + 利用 S 接近对称正定的特性做 Cholesky/LDL 分解；建议加极小量 + 正则化（对角加 1e-9）防奇异。 +- 距离计算用 `std::hypot`；IoU 为纯标量运算。 +- 全程 `double` 精度，与代码一致（代码为 float64）。 + +### 9.4 行为一致性检查清单 + +移植后建议用以下用例对齐行为： + +1. **空检测帧序列**：连续输入空检测，主目标应输出 `is_predicted=true` + 共 `max_lost_frames` 帧，第 `max_lost_frames + 1` 帧 `tracked=false` + （因死亡判定为 `lost_count > max_lost`）。 +2. **ID 单调性**：多次进出画面的目标应获得严格递增、不复用的 ID。 +3. **锁定不被抢占**：锁定目标 Coast 期间出现更高置信度新目标， + `primary_id` 不变；锁定目标死亡后切换到新目标。 +4. **min_hits 语义**：新轨道（hits 未达 min_hits）在无其他已确认实测轨道时 + 也可被选为主目标（候选池回退逻辑）。 +5. **cost 边界**：恰好 `cost = 1.0` 不构成匹配（严格小于）。 +6. **置信度优先**：两轨道竞争同一检测时，置信度高者获胜。 + +### 9.5 线程模型注意 + +原系统中追踪器在推理主线程更新，结果由 20Hz 控制线程读取。C++ 移植若 +保持该模型，仅需保护 `last_result` 的读写（互斥锁或无锁双缓冲）， +追踪器内部状态不必加锁（单线程写）。若改为多线程调用 `update()`， +则需整体加锁。 diff --git a/reference-c++/target_tracker/CMakeLists.txt b/reference-c++/target_tracker/CMakeLists.txt new file mode 100644 index 0000000..ab5c384 --- /dev/null +++ b/reference-c++/target_tracker/CMakeLists.txt @@ -0,0 +1,85 @@ +cmake_minimum_required(VERSION 3.16) + +# ============================================================================= +# target_tracker —— 视觉追踪库（独立动态库工程） +# +# 轻量级多目标追踪过滤器：恒定速度卡尔曼滤波 + 贪婪关联 + 主目标锁定。 +# 规格见 docs/视觉追踪技术路线.md；实现文档见 target_tracker.md。 +# 零第三方依赖（不引入 spdlog / Eigen / OpenCV），仅链接 C++ 标准库。 +# ============================================================================= + +project(target_tracker VERSION 1.0.0 LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# ---- 动态库目标 ---- +add_library(target_tracker SHARED + src/kalman_box_filter.cpp + src/tracklet.cpp + src/target_tracker.cpp) +target_include_directories(target_tracker PUBLIC + $) +target_compile_features(target_tracker PUBLIC cxx_std_17) + +# 版本与符号：VERSION 1.0.0 / SOVERSION 1；默认符号可见性（不加 +# visibility=hidden 与导出宏），全部公共类直接对外导出。 +set_target_properties(target_tracker PROPERTIES + VERSION 1.0.0 + SOVERSION 1) + +# 编译告警：与主工程一致（非 MSVC：-Wall -Wextra -Wpedantic） +if(MSVC) + target_compile_options(target_tracker PRIVATE /W4 /permissive-) +else() + target_compile_options(target_tracker PRIVATE -Wall -Wextra -Wpedantic) +endif() + +# ---- 单元测试（GTest；缺失时仅提示不失败，与主工程策略一致） ---- +include(CTest) +if(BUILD_TESTING) + find_package(GTest QUIET) + if(GTest_FOUND) + include(GoogleTest) + + # 测试可执行文件直接链接共享库 target_tracker； + # BUILD_RPATH 指向库输出目录，保证 ctest 无需 LD_LIBRARY_PATH 即可运行 + set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR};$ORIGIN") + + # 卡尔曼滤波器测试：初始化状态、静止收敛、匀速速度收敛与预测超前、未初始化行为 + add_executable(kalman_box_filter_test tests/kalman_box_filter_test.cpp) + target_link_libraries(kalman_box_filter_test PRIVATE + target_tracker GTest::gtest_main) + if(MSVC) + target_compile_options(kalman_box_filter_test PRIVATE /W4 /permissive-) + else() + target_compile_options(kalman_box_filter_test PRIVATE -Wall -Wextra -Wpedantic) + endif() + gtest_discover_tests(kalman_box_filter_test) + + # 轨道与几何工具测试：IoU 边界、IsDead/IsCoasting、Predict/Update/MarkMiss 计数 + add_executable(tracklet_test tests/tracklet_test.cpp) + target_link_libraries(tracklet_test PRIVATE + target_tracker GTest::gtest_main) + if(MSVC) + target_compile_options(tracklet_test PRIVATE /W4 /permissive-) + else() + target_compile_options(tracklet_test PRIVATE -Wall -Wextra -Wpedantic) + endif() + gtest_discover_tests(tracklet_test) + + # 追踪器测试：逐条覆盖文档 §9.4 六条行为一致性验收用例 + add_executable(target_tracker_test tests/target_tracker_test.cpp) + target_link_libraries(target_tracker_test PRIVATE + target_tracker GTest::gtest_main) + if(MSVC) + target_compile_options(target_tracker_test PRIVATE /W4 /permissive-) + else() + target_compile_options(target_tracker_test PRIVATE -Wall -Wextra -Wpedantic) + endif() + gtest_discover_tests(target_tracker_test) + else() + message(STATUS "GoogleTest not found; target_tracker tests will not be built") + endif() +endif() diff --git a/reference-c++/target_tracker/include/target_tracker/kalman_box_filter.h b/reference-c++/target_tracker/include/target_tracker/kalman_box_filter.h new file mode 100644 index 0000000..d5998a9 --- /dev/null +++ b/reference-c++/target_tracker/include/target_tracker/kalman_box_filter.h @@ -0,0 +1,87 @@ +#pragma once + +// ============================================================================= +// kalman_box_filter.h —— 单目标 8 维恒定速度卡尔曼滤波器 +// +// 对应规格：docs/视觉追踪技术路线.md §4（卡尔曼滤波算法细节）、§8.2（内部常量） +// 状态向量 x[8] = [cx, cy, w, h, vx, vy, vw, vh]（§4.1） +// 观测向量 z[4] = [cx, cy, w, h]（§4.1） +// 时间步长 dt = 1 帧，与帧率无关（§1.3） +// +// 实现约束（§9.3）：矩阵规模极小（8×8 / 4×8 / 4×4），手写定长矩阵运算， +// 全程 double 精度，不引入 Eigen/OpenCV 等线性代数库。 +// ============================================================================= + +#include +#include + +namespace drone { +namespace tracker { + +/// 轴对齐边界框（中心点 + 宽高表示，卡尔曼观测空间） +struct BoxCXCYWH { + double cx = 0.0; // 框中心 x（像素） + double cy = 0.0; // 框中心 y（像素） + double w = 0.0; // 框宽（像素） + double h = 0.0; // 框高（像素） +}; + +/// 单目标恒定速度卡尔曼滤波器（§3.1 / §4） +class KalmanBoxFilter { +public: + KalmanBoxFilter() = default; + + /// 是否已用首次观测初始化（§3.1 initialized 字段） + bool Initialized() const { return initialized_; } + + /// 用首次观测 [cx, cy, w, h] 初始化（§4.3）： + /// 状态速度置零；P = diag(2,2,2,2,100,100,100,100)。 + void Initiate(const BoxCXCYWH& z); + + /// 预测推进一帧（§4.4）：未初始化返回 std::nullopt； + /// 否则 x ← F·x，P ← F·P·Fᵀ + Q，返回状态前 4 维 [cx,cy,w,h] 副本。 + std::optional Predict(); + + /// 观测更新（§4.5）：未初始化时直接返回； + /// S = H·P·Hᵀ + R，K = P·Hᵀ·S⁻¹，x ← x + K(z−Hx)，P ← (I−KH)P。 + void Update(const BoxCXCYWH& z); + + /// 状态向量只读访问（[cx, cy, w, h, vx, vy, vw, vh]，测试与排查用） + const std::array& State() const { return x_; } + + // ---- 内部常量（§4.2 / §8.2，经验值，针对 640×480 画面调优） ---- + static constexpr double kStdPos = 1.0 / 50.0; // STD_POS：位置噪声比例系数 + static constexpr double kStdVel = 1.0 / 200.0; // STD_VEL：速度噪声比例系数 + static constexpr double kIniPos = 2.0; // INI_POS：初始位置协方差 + static constexpr double kIniVel = 100.0; // INI_VEL：初始速度协方差 + static constexpr double kRScale = 2.0; // R 额外缩放因子（增强平滑，§8.2） + +private: + std::array x_{}; // 状态向量（§3.1） + std::array, 8> P_{}; // 状态协方差 8×8（§3.1） + bool initialized_ = false; // 是否已初始化（§3.1） + + // 状态转移矩阵 F（常量，§4.1）：I4 + 右上 I4，即 F[i][i]=1、F[i][i+4]=1 + static constexpr std::array, 8> kF = {{ + {{1, 0, 0, 0, 1, 0, 0, 0}}, + {{0, 1, 0, 0, 0, 1, 0, 0}}, + {{0, 0, 1, 0, 0, 0, 1, 0}}, + {{0, 0, 0, 1, 0, 0, 0, 1}}, + {{0, 0, 0, 0, 1, 0, 0, 0}}, + {{0, 0, 0, 0, 0, 1, 0, 0}}, + {{0, 0, 0, 0, 0, 0, 1, 0}}, + {{0, 0, 0, 0, 0, 0, 0, 1}}, + }}; + + // 观测矩阵 H（常量，§4.1）：[I4 | 0]（4 行 × 8 列）， + // 直接观测位置与尺寸、不观测速度 + static constexpr std::array, 4> kH = {{ + {{1, 0, 0, 0, 0, 0, 0, 0}}, + {{0, 1, 0, 0, 0, 0, 0, 0}}, + {{0, 0, 1, 0, 0, 0, 0, 0}}, + {{0, 0, 0, 1, 0, 0, 0, 0}}, + }}; +}; + +} // namespace tracker +} // namespace drone diff --git a/reference-c++/target_tracker/include/target_tracker/target_tracker.h b/reference-c++/target_tracker/include/target_tracker/target_tracker.h new file mode 100644 index 0000000..856796e --- /dev/null +++ b/reference-c++/target_tracker/include/target_tracker/target_tracker.h @@ -0,0 +1,123 @@ +#pragma once + +// ============================================================================= +// target_tracker.h —— 多目标追踪过滤器（TargetTracker）对外接口 +// +// 对应规格：docs/视觉追踪技术路线.md +// §2 输入输出接口定义（Detection / TrackResult） +// §3.3 TargetTracker（管理器） +// §5 检测-轨迹关联（贪婪最邻近，§5.3） +// §6 轨迹生命周期与 ID 管理（主目标锁定与选举，§6.3） +// §7 主流程（每帧 Update 六步） +// +// 线程模型（§9.5）：Update() 由单线程（推理主循环）调用；库内仅对 +// last_result 缓存加一把 std::mutex，LastResult() 返回受保护拷贝。 +// 本库零第三方依赖（不依赖 spdlog / Eigen / OpenCV）。 +// ============================================================================= + +#include +#include +#include +#include +#include + +#include "tracklet.h" + +namespace drone { +namespace tracker { + +/// 单个检测框（§2.2 输入 detections 元素：[x1,y1,x2,y2,conf,cls_id]） +struct Detection { + double x1 = 0.0; // 边界框左上角 x（像素坐标） + double y1 = 0.0; // 边界框左上角 y + double x2 = 0.0; // 边界框右下角 x + double y2 = 0.0; // 边界框右下角 y + double conf = 0.0; // 检测置信度 ∈ [0,1] + int cls_id = 0; // 类别编号 +}; + +/// 画面尺寸（height, width，§2.2）：仅在"无实测轨道、只有 Coast 轨道" +/// 时用于选取离画面中心最近的轨道；缺省按 640×480 中心 (320,240)。 +struct FrameShape { + int height = 0; + int width = 0; +}; + +/// 每帧追踪输出（§2.3） +struct TrackResult { + bool tracked = false; // 是否存在有效主目标；false 时调用方应刹车 + std::optional primary_id; // 主目标轨道 ID；无目标时为空 + std::pair center{0.0, 0.0}; // 平滑后的目标中心点（tracked=false 时无效） + std::optional box; // 平滑后的边界框 + double confidence = 0.0; // 当前置信度（Coast 期间继承最近实测值） + bool is_predicted = false; // true = Coast（仅预测、无实测） + int lost_frames = 0; // 主目标连续未命中帧数 + int n_active = 0; // 当前活跃轨道总数 + std::optional raw; // 关联到的原始检测框；Coast 时为空（§2.3） +}; + +/// 多目标追踪过滤器（§1.1 / §3.3） +class TargetTracker { +public: + /// 追踪器配置参数（§8.1，默认值取自规格） + struct Config { + int max_lost_frames = 8; // 最大连续丢失帧数（严格大于才判死，§6.1） + double max_association_dist = 200.0; // 中心距离软阈值 Dmax（像素，§5.1） + int min_hits = 3; // 轨道"确认"所需累计匹配次数（§6.1） + double dist_weight = 0.5; // 关联代价距离项权重（§5.1） + double iou_weight = 0.5; // 关联代价 (1−IoU) 项权重（§5.1） + }; + + /// 默认配置构造（等价于 Config{}，§8.1 默认值） + TargetTracker(); + + /// 指定配置构造 + explicit TargetTracker(const Config& cfg); + + /// 每帧调用一次（§7 六步主流程）：输入检测列表与可选画面尺寸， + /// 输出主目标追踪结果并缓存（供 LastResult 跨线程读取）。 + TrackResult Update(const std::vector& detections, + std::optional frame_shape = std::nullopt); + + /// 上一帧结果快照（线程安全：返回受互斥锁保护的拷贝，§9.5） + TrackResult LastResult() const; + + /// 清空所有轨道、ID 计数器、锁定状态与缓存结果（§2.1）；next_id 归零（§6.2） + void Reset(); + + /// 当前存活轨道数量（§2.1） + int ActiveCount() const; + + /// 当前所有轨道的只读快照（§2.1；值拷贝） + std::unordered_map ActiveTracks() const; + + /// 配置只读访问 + const Config& GetConfig() const { return config_; } + +private: + /// 贪婪关联结果（§5.3）：matches 元素为 (track_id, 检测索引) + struct AssociationResult { + std::vector> matches; + std::vector unmatched_track_ids; + std::vector unmatched_det_indices; + }; + + /// 贪婪最邻近关联（§5.3）：按轨道 confidence 降序逐一匹配， + /// cost = w_dist·dist/max(Dmax,1e-3) + w_iou·(1−IoU)，仅 cost 严格小于 + /// best_cost（初始 1.0）才匹配，即 cost ≥ 1.0 判为不可匹配（§5.1）。 + AssociationResult AssociateDetections(const std::vector& detections); + + /// 主目标选择/锁定延续（§6.3 SELECT_PRIMARY） + TrackResult SelectPrimary(std::optional frame_shape); + + Config config_; // 配置参数（§3.3） + int next_id_ = 0; // 下一个可分配 ID，从 0 递增不复用（§6.2） + std::unordered_map tracks_; // 所有存活轨道（§3.3） + long long frame_count_ = 0; // 已处理帧数（仅统计用，§3.3） + std::optional locked_primary_id_; // 当前锁定的主目标 ID（§3.3） + TrackResult last_result_; // 上一帧结果缓存（§3.3） + mutable std::mutex last_result_mutex_; // 仅保护 last_result_（§9.5） +}; + +} // namespace tracker +} // namespace drone diff --git a/reference-c++/target_tracker/include/target_tracker/tracklet.h b/reference-c++/target_tracker/include/target_tracker/tracklet.h new file mode 100644 index 0000000..2a147f8 --- /dev/null +++ b/reference-c++/target_tracker/include/target_tracker/tracklet.h @@ -0,0 +1,93 @@ +#pragma once + +// ============================================================================= +// tracklet.h —— 轨道（Tracklet）与基础几何工具 +// +// 对应规格：docs/视觉追踪技术路线.md +// §3.2 Tracklet（轨道）字段定义与派生状态 +// §5.2 IoU 计算 +// §7 主流程伪代码末尾 TRACKLET_PREDICT / TRACKLET_UPDATE +// +// 几何量说明：BoxCXCYWH（卡尔曼观测空间 [cx,cy,w,h]）定义于 +// kalman_box_filter.h，本文件通过 include 复用并同时提供 BoxXYXY +// （像素坐标 [x1,y1,x2,y2]）与两种表示的互转函数。 +// 本库只依赖 C++ 标准库，无任何第三方依赖。 +// ============================================================================= + +#include +#include + +#include "kalman_box_filter.h" + +namespace drone { +namespace tracker { + +// ---- 基础几何量（§9.1 模块/类划分草案） ---- + +/// 轴对齐边界框（像素坐标，左上/右下角表示） +struct BoxXYXY { + double x1 = 0.0; + double y1 = 0.0; + double x2 = 0.0; + double y2 = 0.0; +}; + +/// [x1,y1,x2,y2] → [cx,cy,w,h]（§7 TRACKLET_UPDATE 中的转换） +BoxCXCYWH ToCenterSize(const BoxXYXY& box); + +/// [cx,cy,w,h] → [x1,y1,x2,y2]（§7 TRACKLET_PREDICT 中的 center_size 逆变换） +BoxXYXY ToXYXY(const BoxCXCYWH& box); + +/// 标准轴对齐 IoU（§5.2）： +/// 任一框为空、交集面积 ≤ 0 或并集 ≤ 0 时返回 0。 +double ComputeIoU(const BoxXYXY& a, const BoxXYXY& b); + +// ============================================================================= +// Tracklet —— 单条轨道：生命周期计数 + 辅助状态 + 卡尔曼滤波器（§3.2） +// ============================================================================= +struct Tracklet { + /// 构造即创建轨道（§7 步骤 5 / §6.1 新建阶段）： + /// 用首次观测初始化滤波器，age=1、hits=1、lost_count=0， + /// last_center / last_box 立即由观测重建（保证创建当帧即可参与输出）。 + Tracklet(int id, const BoxCXCYWH& measurement, double conf, int cls, + int max_lost_frames); + + int track_id = 0; // 全局唯一轨道 ID（§3.2） + KalmanBoxFilter kf; // 本轨道的滤波器（§3.2） + int lost_count = 0; // 当前连续未匹配帧数（§3.2） + int max_lost = 0; // 最大允许连续丢失帧数（构造时注入，§3.2） + double confidence = 0.0; // 最近一次匹配的置信度（§3.2） + int cls_id = 0; // 最近一次匹配的类别 ID（§3.2） + int age = 1; // 轨道已存在的总帧数（创建时为 1，§3.2） + int hits = 1; // 累计成功匹配次数（创建时为 1，§3.2） + std::pair last_center{0.0, 0.0}; // 最近一次中心点（实测或预测） + std::optional last_box; // 最近一次边界框（实测或预测，§3.2） + std::optional last_raw_box; // 最近一次实测关联到的原始检测框； + // Coast 期间保留不更新，仅内部参考； + // 对外输出语义按 §2.3：Coast 时 raw 置空 + + // ---- 派生状态（§3.2 只读属性） ---- + + /// is_dead := lost_count > max_lost（严格大于，§6.1 注记 2） + bool IsDead() const { return lost_count > max_lost; } + + /// is_coasting := lost_count > 0（处于纯预测状态，§3.2） + bool IsCoasting() const { return lost_count > 0; } + + // ---- 生命周期方法（§7 末尾伪代码） ---- + + /// TRACKLET_PREDICT：卡尔曼预测推进一帧。 + /// 预测成功时更新 last_center / last_box 且 age+1。 + void Predict(); + + /// TRACKLET_UPDATE：框 [x1,y1,x2,y2] → [cx,cy,w,h] → kf.update； + /// lost_count 清零、hits+1、age+1，更新 + /// last_center / last_box / last_raw_box / confidence / cls_id。 + void Update(const BoxXYXY& box, double conf, int cls); + + /// 未匹配计丢失：lost_count+1、age+1（§7 步骤 4）。 + void MarkMiss(); +}; + +} // namespace tracker +} // namespace drone diff --git a/reference-c++/target_tracker/src/kalman_box_filter.cpp b/reference-c++/target_tracker/src/kalman_box_filter.cpp new file mode 100644 index 0000000..66e040a --- /dev/null +++ b/reference-c++/target_tracker/src/kalman_box_filter.cpp @@ -0,0 +1,235 @@ +// ============================================================================= +// kalman_box_filter.cpp —— 单目标 8 维恒定速度卡尔曼滤波器实现 +// +// 对应规格：docs/视觉追踪技术路线.md §4（算法细节）、§8.2（内部常量）、 +// §9.3（数值计算：定长数组手写矩阵乘法，4×4 高斯-约当求逆 + 1e-9 正则化）。 +// 关键公式逐行注释并标注对照文档小节号。 +// ============================================================================= + +#include "target_tracker/kalman_box_filter.h" + +#include +#include + +namespace drone { +namespace tracker { +namespace { + +// ---- 定长矩阵基础运算（§9.3：无需引入 Eigen/OpenCV） ---- + +/// 8×8 矩阵乘法：C = A·B +std::array, 8> MatMul8(const std::array, 8>& a, + const std::array, 8>& b) { + std::array, 8> c{}; + for (int i = 0; i < 8; ++i) { + for (int k = 0; k < 8; ++k) { + if (a[i][k] == 0.0) continue; // 稀疏常量矩阵加速，结果不变 + for (int j = 0; j < 8; ++j) { + c[i][j] += a[i][k] * b[k][j]; + } + } + } + return c; +} + +/// 8×8 矩阵转置：R = Aᵀ +std::array, 8> MatTranspose8( + const std::array, 8>& a) { + std::array, 8> r{}; + for (int i = 0; i < 8; ++i) { + for (int j = 0; j < 8; ++j) { + r[i][j] = a[j][i]; + } + } + return r; +} + +/// 4×4 矩阵求逆：高斯-约当消元 + 部分主元（§9.3）。 +/// 输入矩阵先对角加 1e-9 正则化防奇异（§9.3 建议）。 +/// 返回的 bool 表示是否求逆成功（主元退化时返回 false）。 +bool Invert4x4(const std::array, 4>& m, + std::array, 4>& out) { + constexpr double kRegularization = 1e-9; // 正则化极小量（§9.3） + + // 增广矩阵 [M + εI | I]，消元后左半变为单位阵、右半即为逆矩阵 + std::array, 4> aug{}; + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + aug[i][j] = m[i][j] + (i == j ? kRegularization : 0.0); + aug[i][j + 4] = (i == j) ? 1.0 : 0.0; + } + } + + for (int col = 0; col < 4; ++col) { + // 部分主元：选取当前列绝对值最大行交换，提高数值稳定性 + int pivot = col; + for (int r = col + 1; r < 4; ++r) { + if (std::abs(aug[r][col]) > std::abs(aug[pivot][col])) { + pivot = r; + } + } + if (std::abs(aug[pivot][col]) < 1e-12) { + return false; // 主元退化（正则化后理论上不可达） + } + if (pivot != col) { + std::swap(aug[pivot], aug[col]); + } + // 主元行归一化 + const double d = aug[col][col]; + for (int j = 0; j < 8; ++j) { + aug[col][j] /= d; + } + // 消去其余行（高斯-约当：一次消成对角单位阵） + for (int r = 0; r < 4; ++r) { + if (r == col) continue; + const double f = aug[r][col]; + if (f == 0.0) continue; + for (int j = 0; j < 8; ++j) { + aug[r][j] -= f * aug[col][j]; + } + } + } + + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + out[i][j] = aug[i][j + 4]; + } + } + return true; +} + +} // namespace + +void KalmanBoxFilter::Initiate(const BoxCXCYWH& z) { + // §4.3 初始化：状态 = [cx, cy, w, h, 0, 0, 0, 0]（速度置零） + x_ = {{z.cx, z.cy, z.w, z.h, 0.0, 0.0, 0.0, 0.0}}; + + // §4.3 协方差：P = diag(2,2,2,2,100,100,100,100) + // （位置置信度中等，速度置信度低，允许前几帧快速收敛速度估计） + for (auto& row : P_) row.fill(0.0); + for (int i = 0; i < 4; ++i) { + P_[i][i] = kIniPos; // INI_POS = 2.0（§4.2） + P_[i + 4][i + 4] = kIniVel; // INI_VEL = 100.0（§4.2） + } + initialized_ = true; +} + +std::optional KalmanBoxFilter::Predict() { + // §4.4：未初始化返回空 + if (!initialized_) { + return std::nullopt; + } + + // §4.2 动态过程噪声：Q = diag(σ_pos², σ_vel²)，σ 按状态中 w/h 缩放 + // σ_pos = [w/50, h/50, w/50, h/50] + // σ_vel = [w/200, h/200, w/200, h/200] + // （Q 直接使用状态值，不做 abs，与规格 §4.2 注记保持一致） + const double w = x_[2]; + const double h = x_[3]; + const double std_pos[4] = {w * kStdPos, h * kStdPos, w * kStdPos, h * kStdPos}; + const double std_vel[4] = {w * kStdVel, h * kStdVel, w * kStdVel, h * kStdVel}; + std::array, 8> q{}; + for (int i = 0; i < 4; ++i) { + q[i][i] = std_pos[i] * std_pos[i]; // 前 4 对角元来自 σ_pos（各分量先平方） + q[i + 4][i + 4] = std_vel[i] * std_vel[i]; // 后 4 对角元来自 σ_vel + } + + // §4.4 状态转移：x ← F·x（恒定速度模型：位置/尺寸各自加上对应速度） + std::array x_new{}; + for (int i = 0; i < 8; ++i) { + double sum = 0.0; + for (int j = 0; j < 8; ++j) { + sum += kF[i][j] * x_[j]; + } + x_new[i] = sum; + } + x_ = x_new; + + // §4.4 协方差传播：P ← F·P·Fᵀ + Q + P_ = MatMul8(MatMul8(kF, P_), MatTranspose8(kF)); + for (int i = 0; i < 8; ++i) { + for (int j = 0; j < 8; ++j) { + P_[i][j] += q[i][j]; + } + } + + // §4.4 返回预测的位置/尺寸（状态向量前 4 维的副本） + return BoxCXCYWH{x_[0], x_[1], x_[2], x_[3]}; +} + +void KalmanBoxFilter::Update(const BoxCXCYWH& z) { + // §4.5：未初始化直接返回 + if (!initialized_) { + return; + } + + // §4.2 动态观测噪声：R = diag(2(w/50)², 2(h/50)², 2(w/50)², 2(h/50)²) + // （R 使用 abs(w)/abs(h) 防御负尺寸，并额外乘 2 增强平滑； + // 该差异与规格 §4.2 注记保持一致） + const double w = std::abs(x_[2]); + const double h = std::abs(x_[3]); + std::array, 4> r{}; + const double rw = kRScale * (w * kStdPos) * (w * kStdPos); + const double rh = kRScale * (h * kStdPos) * (h * kStdPos); + r[0][0] = rw; + r[1][1] = rh; + r[2][2] = rw; + r[3][3] = rh; + + // §4.5 新息协方差：S = H·P·Hᵀ + R（4×4） + // H = [I4 | 0]，故 H·P·Hᵀ 即 P 的左上 4×4 子块 + std::array, 4> s{}; + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + s[i][j] = P_[i][j] + r[i][j]; + } + } + + // §4.5 / §9.3：S⁻¹（4×4 高斯-约当求逆 + 对角加 1e-9 正则化防奇异） + std::array, 4> s_inv{}; + if (!Invert4x4(s, s_inv)) { + return; // 正则化后理论不可达；防御性跳过本次更新 + } + + // §4.5 卡尔曼增益：K = P·Hᵀ·S⁻¹（8×4） + // H = [I4 | 0]，故 P·Hᵀ 即 P 的前 4 列（8×4） + std::array, 8> k{}; + for (int i = 0; i < 8; ++i) { + for (int j = 0; j < 4; ++j) { + double sum = 0.0; + for (int l = 0; l < 4; ++l) { + sum += P_[i][l] * s_inv[l][j]; + } + k[i][j] = sum; + } + } + + // §4.5 残差（innovation）：y = z − H·x（H·x 即状态前 4 维） + const double z_vec[4] = {z.cx, z.cy, z.w, z.h}; + double y[4]; + for (int i = 0; i < 4; ++i) { + y[i] = z_vec[i] - x_[i]; + } + + // §4.5 状态更新：x ← x + K·y + for (int i = 0; i < 8; ++i) { + double sum = 0.0; + for (int j = 0; j < 4; ++j) { + sum += k[i][j] * y[j]; + } + x_[i] += sum; + } + + // §4.5 协方差更新：P ← (I8 − K·H)·P + // K·H（8×8）= K 的前 4 列铺到前 4 列上（H 选通前 4 维） + std::array, 8> ikh{}; + for (int i = 0; i < 8; ++i) { + for (int j = 0; j < 8; ++j) { + ikh[i][j] = (i == j ? 1.0 : 0.0) - (j < 4 ? k[i][j] : 0.0); + } + } + P_ = MatMul8(ikh, P_); +} + +} // namespace tracker +} // namespace drone diff --git a/reference-c++/target_tracker/src/target_tracker.cpp b/reference-c++/target_tracker/src/target_tracker.cpp new file mode 100644 index 0000000..91881cd --- /dev/null +++ b/reference-c++/target_tracker/src/target_tracker.cpp @@ -0,0 +1,312 @@ +// ============================================================================= +// target_tracker.cpp —— 多目标追踪过滤器（TargetTracker）实现 +// +// 对应规格：docs/视觉追踪技术路线.md +// §5 检测-轨迹关联（贪婪最邻近，§5.3） +// §6 轨迹生命周期与 ID 管理（主目标锁定与选举，§6.3） +// §7 主流程六步（UPDATE 伪代码） +// §9.5 线程模型（单线程写 Update，仅 last_result 缓存加锁） +// ============================================================================= + +#include "target_tracker/target_tracker.h" + +#include +#include +#include + +namespace drone { +namespace tracker { + +TargetTracker::TargetTracker() : TargetTracker(Config{}) {} + +TargetTracker::TargetTracker(const Config& cfg) : config_(cfg) {} + +TrackResult TargetTracker::Update(const std::vector& detections, + std::optional frame_shape) { + // §7：frame_count += 1（仅统计用） + ++frame_count_; + + // ---- 入口防御：过滤坐标或置信度含 NaN/Inf 的检测 ---- + // 防御性增强：上游检测器理论上已过滤，但库自身必须免疫—— + // 非有限值一旦进入卡尔曼状态将永久污染轨道并传导到主目标输出 + std::vector valid_detections; + valid_detections.reserve(detections.size()); + for (const Detection& det : detections) { + if (std::isfinite(det.x1) && std::isfinite(det.y1) && + std::isfinite(det.x2) && std::isfinite(det.y2) && + std::isfinite(det.conf)) { + valid_detections.push_back(det); + } + } + + // ---- 步骤 1：所有存活轨道先行预测（推进一帧）---- + // （§7 步骤 1：t.predict() 更新 last_center / last_box、age += 1） + for (auto& kv : tracks_) { + kv.second.Predict(); + } + + // ---- 步骤 2：构造检测中心表并贪婪关联（§7 步骤 2 / §5.3） ---- + AssociationResult assoc; + if (!valid_detections.empty()) { + assoc = AssociateDetections(valid_detections); + } else { + // 无检测：全部轨道视为未匹配，全部进入 Coast 计数（§7 步骤 2 否则分支） + for (const auto& kv : tracks_) { + assoc.unmatched_track_ids.push_back(kv.first); + } + } + + // ---- 步骤 3：用匹配到的检测更新轨道（§7 步骤 3） ---- + for (const auto& match : assoc.matches) { + const int track_id = match.first; + const Detection& det = valid_detections[static_cast(match.second)]; + // §7 步骤 3：由检测重建原始框 raw_box = [cx−w/2, cy−h/2, cx+w/2, cy+h/2] + // （等价于检测框本身：cx=(x1+x2)/2、w=x2−x1 的逆运算，浮点上精确还原） + const BoxXYXY raw_box{det.x1, det.y1, det.x2, det.y2}; + tracks_.at(track_id).Update(raw_box, det.conf, det.cls_id); + } + + // ---- 步骤 4：未匹配轨道计丢失，死亡者回收（§7 步骤 4） ---- + for (const int track_id : assoc.unmatched_track_ids) { + auto it = tracks_.find(track_id); + if (it == tracks_.end()) { + continue; + } + it->second.MarkMiss(); // lost_count+1、age+1 + if (it->second.IsDead()) { // lost_count > max_lost（严格大于，§6.1） + // 锁定的唯一解除途径之一：锁定轨道死亡被回收时清锁（§6.3） + if (locked_primary_id_.has_value() && *locked_primary_id_ == track_id) { + locked_primary_id_.reset(); + } + tracks_.erase(it); + } + } + + // ---- 步骤 5：未匹配检测 → 创建新轨道（§7 步骤 5 / §6.2） ---- + for (const int det_idx : assoc.unmatched_det_indices) { + const Detection& det = valid_detections[static_cast(det_idx)]; + // §7 步骤 5：以 [cx,cy,w,h] 创建；构造函数内重建 last_box， + // 保证创建当帧即可参与输出；next_id 从 0 递增永不复用（§6.2） + tracks_.emplace(next_id_, + Tracklet(next_id_, ToCenterSize({det.x1, det.y1, det.x2, det.y2}), + det.conf, det.cls_id, config_.max_lost_frames)); + ++next_id_; + } + + // ---- 步骤 6：选取/延续主目标，组装输出并缓存（§7 步骤 6 / §6.3） ---- + TrackResult result = SelectPrimary(frame_shape); + { + std::lock_guard lock(last_result_mutex_); + last_result_ = result; // 缓存供跨线程读取（§9.5） + } + return result; +} + +TrackResult TargetTracker::LastResult() const { + // §9.5：LastResult() 返回受互斥锁保护的拷贝 + std::lock_guard lock(last_result_mutex_); + return last_result_; +} + +void TargetTracker::Reset() { + // §2.1：清空所有轨道、ID 计数器、锁定状态与缓存结果 + tracks_.clear(); + next_id_ = 0; // §6.2：reset() 时计数器归零 + frame_count_ = 0; + locked_primary_id_.reset(); // 锁定的唯一解除途径之二（§6.3） + std::lock_guard lock(last_result_mutex_); + last_result_ = TrackResult{}; +} + +int TargetTracker::ActiveCount() const { + return static_cast(tracks_.size()); +} + +std::unordered_map TargetTracker::ActiveTracks() const { + return tracks_; // 值拷贝只读快照（§2.1） +} + +TargetTracker::AssociationResult TargetTracker::AssociateDetections( + const std::vector& detections) { + AssociationResult result; + + // §5.3：按轨道 confidence 降序排序（复制 ID 到 vector 后 sort， + // 不直接排哈希表，§9.2） + std::vector sorted_ids; + sorted_ids.reserve(tracks_.size()); + for (const auto& kv : tracks_) { + sorted_ids.push_back(kv.first); + } + std::sort(sorted_ids.begin(), sorted_ids.end(), [this](int a, int b) { + const double raw_a = tracks_.at(a).confidence; + const double raw_b = tracks_.at(b).confidence; + // NaN 安全：非有限置信度按 −∞ 处理（排最后），避免 NaN 参与比较 + // 破坏 std::sort 要求的严格弱序（未定义行为）；映射后为全序， + // 等价传递性恒成立 + const double ca = std::isfinite(raw_a) + ? raw_a + : -std::numeric_limits::infinity(); + const double cb = std::isfinite(raw_b) + ? raw_b + : -std::numeric_limits::infinity(); + if (ca != cb) return ca > cb; // 置信度从高到低 + return a < b; // 平局按 ID 升序作次级键，保证确定性 + }); + + std::vector det_assigned(detections.size(), false); + + for (const int track_id : sorted_ids) { + const Tracklet& track = tracks_.at(track_id); + // §5.3：每条轨道最优代价初始化为 1.0（仅 cost 严格小于 1.0 才记录， + // 即 cost ≥ 1.0 判为不可匹配，§5.1 匹配有效性判据） + double best_cost = 1.0; + int best_det = -1; + for (size_t d = 0; d < detections.size(); ++d) { + if (det_assigned[d]) continue; // 跳过已分配检测 + const Detection& det = detections[d]; + // §5.1：中心点欧氏距离（像素） + const double det_cx = (det.x1 + det.x2) / 2.0; + const double det_cy = (det.y1 + det.y2) / 2.0; + const double dist = + std::hypot(track.last_center.first - det_cx, + track.last_center.second - det_cy); + // §5.1：归一化距离，取 max(Dmax, 1e-3) 防除零 + const double norm_dist = + dist / std::max(config_.max_association_dist, 1e-3); + // §5.2：预测框与检测框的 IoU + const double iou = track.last_box.has_value() + ? ComputeIoU(*track.last_box, + {det.x1, det.y1, det.x2, det.y2}) + : 0.0; + // §5.1：cost = w_dist·norm_dist + w_iou·(1−IoU) + const double cost = config_.dist_weight * norm_dist + + config_.iou_weight * (1.0 - iou); + if (cost < best_cost) { // 严格小于才更新（§5.1 / §5.3） + best_cost = cost; + best_det = static_cast(d); + } + } + if (best_det >= 0) { + result.matches.emplace_back(track_id, best_det); + det_assigned[static_cast(best_det)] = true; + } else { + result.unmatched_track_ids.push_back(track_id); + } + } + + // §5.3：未分配检测收集 + for (size_t d = 0; d < detections.size(); ++d) { + if (!det_assigned[d]) { + result.unmatched_det_indices.push_back(static_cast(d)); + } + } + return result; +} + +TrackResult TargetTracker::SelectPrimary(std::optional frame_shape) { + TrackResult result; + result.n_active = static_cast(tracks_.size()); + + // §6.3：轨道表为空 → 清除锁定，返回 tracked=false + if (tracks_.empty()) { + locked_primary_id_.reset(); + return result; + } + + // §6.3 ① 锁定延续：锁定轨道仍存活则直接输出（Coast 中照样输出， + // is_predicted=true；不被其他更高置信度目标抢占） + Tracklet* chosen = nullptr; + bool is_predicted = false; + if (locked_primary_id_.has_value()) { + auto it = tracks_.find(*locked_primary_id_); + if (it != tracks_.end()) { + chosen = &it->second; + is_predicted = it->second.IsCoasting(); + } + } + + // §6.3 ② 重新选举（无锁定 / 锁定目标已死亡被回收） + if (chosen == nullptr) { + // measured ← 所有 lost_count == 0 的轨道；coasting ← 其余存活轨道 + std::vector measured; + std::vector coasting; + for (auto& kv : tracks_) { + if (kv.second.IsCoasting()) { + coasting.push_back(&kv.second); + } else { + measured.push_back(&kv.second); + } + } + + if (!measured.empty()) { + // confirmed ← measured 中 hits ≥ min_hits 者；为空则退回 measured + // （min_hits 只影响候选池优先级，不阻止输出，§6.1 注记 1） + std::vector pool; + for (Tracklet* t : measured) { + if (t->hits >= config_.min_hits) { + pool.push_back(t); + } + } + if (pool.empty()) { + pool = measured; + } + // chosen ← pool 中 confidence 最大者；平局取 unordered_map 遍历序 + // 中首个最大者，不做确定性承诺（规格 §6.3 未规定平局规则） + chosen = pool.front(); + for (Tracklet* t : pool) { + if (t->confidence > chosen->confidence) { + chosen = t; + } + } + is_predicted = false; + } else if (!coasting.empty()) { + // 无实测轨道：选 coasting 中离画面中心最近者（欧氏距离平方排序） + // §2.2：frame_shape 缺省按 640×480 画面中心 (320, 240) + const double center_x = + (frame_shape.has_value() && frame_shape->width > 0) + ? frame_shape->width / 2.0 + : 320.0; + const double center_y = + (frame_shape.has_value() && frame_shape->height > 0) + ? frame_shape->height / 2.0 + : 240.0; + double best_dist_sq = std::numeric_limits::infinity(); + for (Tracklet* t : coasting) { + const double dx = t->last_center.first - center_x; + const double dy = t->last_center.second - center_y; + const double dist_sq = dx * dx + dy * dy; + if (dist_sq < best_dist_sq) { + best_dist_sq = dist_sq; + chosen = t; + } + } + if (chosen == nullptr) { + // NaN 残余的防御兜底：若所有 coasting 轨道中心均为 NaN， + // dist_sq 恒为 NaN 无人当选，取首个避免空指针解引用； + // 正常输入已在 Update 入口经有限性过滤，本分支理论上不可达 + chosen = coasting.front(); + } + is_predicted = true; // Coast 兜底：仅预测、无实测 + } else { + // 理论上不可达（轨道非空则 measured/coasting 必居其一）；防御清锁 + locked_primary_id_.reset(); + return result; + } + locked_primary_id_ = chosen->track_id; // 选举后锁定（§6.3） + } + + // ---- 组装输出（§2.3 九字段） ---- + result.tracked = true; + result.primary_id = chosen->track_id; + result.center = chosen->last_center; + result.box = chosen->last_box; + result.confidence = chosen->confidence; // Coast 期间继承最近实测置信度 + result.is_predicted = is_predicted; + result.lost_frames = chosen->lost_count; + // §2.3：raw = 关联到的原始检测框；Coast 时为空（锁定延续分支同样遵守） + result.raw = is_predicted ? std::nullopt : chosen->last_raw_box; + return result; +} + +} // namespace tracker +} // namespace drone diff --git a/reference-c++/target_tracker/src/tracklet.cpp b/reference-c++/target_tracker/src/tracklet.cpp new file mode 100644 index 0000000..1936b65 --- /dev/null +++ b/reference-c++/target_tracker/src/tracklet.cpp @@ -0,0 +1,101 @@ +// ============================================================================= +// tracklet.cpp —— 轨道（Tracklet）与基础几何工具实现 +// +// 对应规格：docs/视觉追踪技术路线.md +// §5.2 IoU 计算 +// §7 TRACKLET_PREDICT / TRACKLET_UPDATE 伪代码 +// §3.2 Tracklet 字段语义 +// ============================================================================= + +#include "target_tracker/tracklet.h" + +#include + +namespace drone { +namespace tracker { + +BoxCXCYWH ToCenterSize(const BoxXYXY& box) { + // §7 TRACKLET_UPDATE：cx=(x1+x2)/2；cy=(y1+y2)/2；w=x2−x1；h=y2−y1 + return BoxCXCYWH{(box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0, + box.x2 - box.x1, box.y2 - box.y1}; +} + +BoxXYXY ToXYXY(const BoxCXCYWH& box) { + // §7 TRACKLET_PREDICT：center_size → [cx−w/2, cy−h/2, cx+w/2, cy+h/2] + return BoxXYXY{box.cx - box.w / 2.0, box.cy - box.h / 2.0, + box.cx + box.w / 2.0, box.cy + box.h / 2.0}; +} + +double ComputeIoU(const BoxXYXY& a, const BoxXYXY& b) { + // §5.2：任一框为空（宽或高 ≤ 0）返回 0 + const double area_a = (a.x2 - a.x1) * (a.y2 - a.y1); + const double area_b = (b.x2 - b.x1) * (b.y2 - b.y1); + if (area_a <= 0.0 || area_b <= 0.0) { + return 0.0; + } + + // 交集宽高与 0 取最大值（§5.2） + const double inter_w = std::min(a.x2, b.x2) - std::max(a.x1, b.x1); + const double inter_h = std::min(a.y2, b.y2) - std::max(a.y1, b.y1); + if (inter_w <= 0.0 || inter_h <= 0.0) { + return 0.0; // 交集面积 ≤ 0 返回 0（§5.2） + } + + const double inter_area = inter_w * inter_h; + const double union_area = area_a + area_b - inter_area; + if (union_area <= 0.0) { + return 0.0; // 并集 ≤ 0 返回 0（§5.2） + } + // IoU = area(A∩B) / (area(A)+area(B)−area(A∩B))（§5.2） + return inter_area / union_area; +} + +Tracklet::Tracklet(int id, const BoxCXCYWH& measurement, double conf, int cls, + int max_lost_frames) + : track_id(id), + max_lost(max_lost_frames), + confidence(conf), + cls_id(cls) { + // §4.3：用首次观测初始化滤波器（速度置零、P 取默认对角） + kf.Initiate(measurement); + // §7 步骤 5：创建当帧即赋 last_center / last_box（保证当帧可参与输出） + last_center = {measurement.cx, measurement.cy}; + last_box = ToXYXY(measurement); + // 创建即源于实测检测：同步记录原始检测框（Coast 期间保留， + // 对外输出按 §2.3 在 Coast 时置空） + last_raw_box = last_box; + // age / hits / lost_count 由类内默认值保证（1 / 1 / 0，§3.2） +} + +void Tracklet::Predict() { + // §7 TRACKLET_PREDICT：卡尔曼预测推进一帧 + const auto pred = kf.Predict(); + if (pred.has_value()) { + // 预测成功：更新 last_center / last_box，age += 1 + last_center = {pred->cx, pred->cy}; + last_box = ToXYXY(*pred); + ++age; + } +} + +void Tracklet::Update(const BoxXYXY& box, double conf, int cls) { + // §7 TRACKLET_UPDATE：框 → [cx,cy,w,h] → kf.update + kf.Update(ToCenterSize(box)); + last_center = {(box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0}; + last_box = box; + last_raw_box = box; // 实测关联到的原始检测框（Coast 期间保留不更新） + confidence = conf; + cls_id = cls; + lost_count = 0; // 匹配成功：连续丢失计数清零 + ++hits; // 累计匹配次数 +1 + ++age; // 帧龄 +1 +} + +void Tracklet::MarkMiss() { + // §7 步骤 4：未匹配计丢失 lost_count+1、age+1 + ++lost_count; + ++age; +} + +} // namespace tracker +} // namespace drone diff --git a/reference-c++/target_tracker/target_tracker.md b/reference-c++/target_tracker/target_tracker.md new file mode 100644 index 0000000..2b87406 --- /dev/null +++ b/reference-c++/target_tracker/target_tracker.md @@ -0,0 +1,217 @@ +# target_tracker 视觉追踪库实现文档 + +> 独立动态库（`libtarget_tracker.so`），实现轻量级多目标追踪过滤器： +> 恒定速度卡尔曼滤波平滑 + Coast 抗短暂丢失 + ID 稳定与主目标锁定。 +> 算法行为的唯一权威规格为 `docs/视觉追踪技术路线.md`（下称"技术路线"）， +> 本文档不重复抄录算法全文，仅描述实现结构与对照关系。 + +--- + +## 一、功能职责与边界 + +**做什么**： + +- 每帧接收 YOLO 检测框列表，输出**一个主目标**的平滑中心点/边界框/状态标志 + （`TrackResult`），供下游速度闭环（PID）消费。 +- 平滑：8 维恒定速度卡尔曼滤波（位置/尺寸 + 速度），抑制检测框逐帧抖动。 +- Coast 预测：目标短暂消失时用滤波器预测值继续输出（`is_predicted=true`）， + 避免控制输出突变；连续丢失超过 `max_lost_frames`（严格大于）才判丢。 +- ID 稳定：贪婪最邻近关联（中心距离 + IoU 加权代价）维持轨道 ID； + 主目标锁定后不被高置信度新目标抢占，仅在轨道死亡或 `Reset()` 时解除。 + +**不做什么**（边界）： + +- 不做光流估计、不做激光/惯导等多源融合（属 perception 模块职责）； +- 不做 Topic 订阅/发布接线、不依赖 spdlog/Eigen/OpenCV，零第三方依赖； +- 不做画面边界钳制与负尺寸约束（与技术路线 §4.5 注记保持一致）； +- 不要求检测置信度下限过滤（上游检测器已完成阈值过滤）。 + +## 二、接口与数据流 + +命名空间 `drone::tracker`，头文件保护 `#pragma once`。 + +| 头文件 | 内容 | +|--------|------| +| `include/target_tracker/kalman_box_filter.h` | `BoxCXCYWH`、`KalmanBoxFilter` | +| `include/target_tracker/tracklet.h` | `BoxXYXY`、`ToCenterSize`/`ToXYXY`/`ComputeIoU`、`Tracklet` | +| `include/target_tracker/target_tracker.h` | `Detection`、`FrameShape`、`TrackResult`、`TargetTracker` | + +主要接口签名： + +```cpp +class KalmanBoxFilter { + void Initiate(const BoxCXCYWH& z); // 首次观测初始化（§4.3） + std::optional Predict(); // 未初始化返回 nullopt（§4.4） + void Update(const BoxCXCYWH& z); // 观测更新（§4.5） + bool Initialized() const; + const std::array& State() const; // [cx,cy,w,h,vx,vy,vw,vh] +}; + +struct Tracklet { + Tracklet(int id, const BoxCXCYWH& measurement, double conf, int cls, + int max_lost_frames); + bool IsDead() const; // lost_count > max_lost（严格大于） + bool IsCoasting() const; // lost_count > 0 + void Predict(); // kf.predict 非空时更新 last_center/last_box、age+1 + void Update(const BoxXYXY& box, double conf, int cls); // 匹配更新 + void MarkMiss(); // lost+1、age+1 + // 字段：track_id / kf / lost_count / max_lost / confidence / cls_id / + // age / hits / last_center / last_box / last_raw_box +}; + +class TargetTracker { + struct Config { // §8.1 默认值 + int max_lost_frames = 8; + double max_association_dist = 200.0; + int min_hits = 3; + double dist_weight = 0.5; + double iou_weight = 0.5; + }; + TargetTracker(); // 默认配置 + explicit TargetTracker(const Config& cfg); + TrackResult Update(const std::vector& detections, + std::optional frame_shape = std::nullopt); + TrackResult LastResult() const; // 线程安全拷贝 + void Reset(); // 清全部状态且 next_id 归零 + int ActiveCount() const; + std::unordered_map ActiveTracks() const; // 只读快照 +}; +``` + +数据流：调用方每帧将检测列表（像素坐标 `[x1,y1,x2,y2,conf,cls_id]`）传入 +`Update()`，得到 `TrackResult`（九字段：tracked / primary_id / center / box / +confidence / is_predicted / lost_frames / n_active / raw）。丢失时 +`tracked=false`、定位字段空、`lost_frames=0`（技术路线 §2.3）。 + +## 三、关键实现点 + +### 3.1 卡尔曼滤波（对照技术路线 §4） + +- 状态 `x[8]=[cx,cy,w,h,vx,vy,vw,vh]`，观测 `z[4]=[cx,cy,w,h]`，dt=1 帧。 +- 常量矩阵：`F`（8×8，I4 + 右上 I4）、`H`（4×8，[I4|0]）为 `constexpr`； + 矩阵乘法/转置用 `std::array` 定长手写，全程 `double`（§9.3）。 +- 常量：`kStdPos=1/50`、`kStdVel=1/200`、`kIniPos=2.0`、`kIniVel=100.0`、 + `kRScale=2.0`（§4.2/§8.2）。 +- **动态噪声**：`Predict()` 时 Q=diag(σ_pos²,σ_vel²)，σ 直接用状态中的 w/h + 缩放；`Update()` 时 R 用 `abs(w)/abs(h)` 缩放并乘 2——两处差异与规格 + §4.2 注记严格保持一致。 +- `Initiate()`：速度置零，P=diag(2,2,2,2,100,100,100,100)（§4.3）。 +- `Update()` 中 S=HPHᵀ+R 为 4×4，用**高斯-约当消元 + 部分主元**求逆， + 消元前对角加 1e-9 正则化防奇异（§9.3）；随后 K=PHᵀS⁻¹、 + x←x+K(z−Hx)、P←(I−KH)P（§4.5）。 + +### 3.2 贪婪关联（对照 §5） + +- `cost = dist_weight·dist/max(Dmax,1e-3) + iou_weight·(1−IoU)`， + 距离用 `std::hypot`；每条轨道 `best_cost` 初始化为 1.0，**仅 cost 严格 + 小于才匹配**（cost=1.0 恰好不匹配，§5.1）。 +- 按轨道 confidence 降序（平局按 ID 升序保证确定性）逐一贪心匹配； + 排序时复制 ID 到 `std::vector` 后 `std::sort`，不直接排哈希表（§9.2）。 + +### 3.3 主目标选举三级回退（对照 §6.3） + +1. **锁定延续优先**：`locked_primary_id` 存活则直接输出（Coast 中照样输出 + `is_predicted=true`），不被高置信度目标抢占； +2. **重新选举**（无锁定/锁定已死亡）：measured（lost==0）中 hits≥min_hits + 的已确认池取 confidence 最大；池空则退回全部 measured； +3. 无 measured 时从 coasting 中选离画面中心最近者（欧氏距离平方， + `frame_shape` 缺省按 640×480 中心 (320,240)），`is_predicted=true`； + 都没有则 `tracked=false`。 + +锁定仅在轨道死亡删除处或 `Reset()` 时解除。 + +### 3.4 主流程六步（对照 §7） + +`Update()` 严格按：①全体轨道 predict ②检测中心表 + 贪婪关联 ③匹配轨道 +update（raw_box 由检测重建）④未匹配轨道 mark_miss、死亡即删、若为锁定 +目标同时清锁 ⑤未匹配检测新建轨道（创建当帧赋 last_box/last_raw_box， +保证当帧可参与输出；next_id 从 0 递增永不复用）⑥主目标选举组装输出并 +缓存 last_result。 + +### 3.5 线程模型（对照 §9.5） + +单线程写 `Update()`（推理主循环）；库内仅对 `last_result` 缓存加一把 +`std::mutex`，`LastResult()` 返回受保护拷贝。内部轨道状态不加锁； +若未来改为多线程调用 `Update()` 需整体加锁。 + +### 3.6 符号导出说明 + +当前不加 `-fvisibility=hidden`、不使用导出宏，共享库默认导出全部公共符号。 +日后若启用隐藏可见性（如 `CXX_VISIBILITY_PRESET hidden`），需补 +`TARGET_TRACKER_API` 宏（导出/导入双态）并标注所有公共类与函数， +否则下游链接将缺失符号。 + +## 四、日志行为 + +本库为**纯算法层，不打任何日志**（不依赖 spdlog）。若未来接入主工程， +接线层（订阅/发布适配部件）按主工程日志纪律负责关键路径日志与节流， +本库内部不新增日志。 + +## 五、测试方式 + +构建环境：WSL2 Ubuntu（AGENTS.md 规定的开发机构建环境）。 + +```bash +cd /mnt/d/ProgramData/drone_test/drone_test +cmake -S target_tracker -B target_tracker/build +cmake --build target_tracker/build -j$(nproc) +ctest --test-dir target_tracker/build --output-on-failure +``` + +测试共 27 条（kalman_box_filter_test 5 条 / tracklet_test 12 条 / +target_tracker_test 10 条），其中 `target_tracker_test.cpp` 逐条覆盖 +技术路线 §9.4 六条验收用例： + +1. `EmptyFramesCoastThenLost`：空检测帧 Coast 持续 max_lost_frames 帧 + （is_predicted=true），第 max_lost_frames+1 帧 tracked=false； +2. `IdStrictlyIncreasingNoReuse`：ID 严格递增不复用，Reset 后归零； +3. `LockNotStolenAndSwitchAfterDeath`：Coast 期间高置信度新目标不抢占锁定， + 锁定目标死亡后切换； +4. `MinHitsFallbackAllowsNewTrack`：无已确认实测轨道时新轨道（hits max_lost`（严格大于）， + 默认允许 Coast 8 帧、第 9 帧判丢；改成 `>=` 属于行为变更。 +- **关联不上 / 乱匹配**：检查 `max_association_dist` 与目标帧间位移是否 + 匹配；cost 判据为严格小于 1.0；dist/iou 权重之和建议保持 1.0。 +- **滤波发散或跳变**：检查 Q/R 动态噪声是否按状态 w/h 缩放（R 用 abs 且 + 乘 2、Q 不用 abs，该差异是规格要求）；4×4 求逆已加 1e-9 正则化。 +- **测试链接失败（找不到 so）**：测试可执行文件通过 `CMAKE_BUILD_RPATH` + 定位库；若把测试挪到别处运行需设置 `LD_LIBRARY_PATH`。 +- 修改任何行为后必须同步更新本文档与 `docs/视觉追踪技术路线.md` 的对照注释。 + +## 七、未来接线指引（接入主工程视频链路） + +本库以独立动态库交付，主工程接入建议采用： + +1. **订阅解码帧主题作为每帧节拍**：接线部件订阅解码帧 Topic，以解码帧 + 到达为帧节拍驱动追踪（保证与推理节奏一致）； +2. **按 frame_sequence 聚合 DetectionResult**：YOLO 每目标发布一条 + DetectionResult（同帧共享 frame_sequence），接线部件按 frame_sequence + 聚合成 `std::vector` 后调用 `Update()`； +3. **主工程 `include/common/types.h` 新增 `TrackedTarget` 消息与 + `kTrackedTarget` 主题**：由接线部件把 `TrackResult` 转换为 + `TrackedTarget` 发布，供控制线程（20Hz PID）订阅消费；Coast + （is_predicted=true）时调用方清 PID 积分、tracked=false 时零速刹车 + （技术路线 §1.2 上游行为约定）。 + +主工程链接该 so 的 CMake 片段示例： + +```cmake +# 主工程 CMakeLists.txt 中引入视觉追踪库（独立子工程） +add_subdirectory(target_tracker) + +target_link_libraries(drone_control PRIVATE target_tracker) +# 部署时需将 libtarget_tracker.so.1 一并打包到可执行文件目录 +# （或设置 INSTALL_RPATH=$ORIGIN，与 docs/部署与打包.md 目录式部署约定一致） +``` diff --git a/reference-c++/target_tracker/tests/kalman_box_filter_test.cpp b/reference-c++/target_tracker/tests/kalman_box_filter_test.cpp new file mode 100644 index 0000000..95b121b --- /dev/null +++ b/reference-c++/target_tracker/tests/kalman_box_filter_test.cpp @@ -0,0 +1,124 @@ +// ============================================================================= +// kalman_box_filter_test.cpp —— KalmanBoxFilter 单元测试 +// +// 覆盖 docs/视觉追踪技术路线.md §4 行为： +// - 初始化状态正确（§4.3） +// - 静止目标多帧收敛到观测（§4.5） +// - 匀速目标速度收敛且预测超前一个 dt（§4.4） +// - 未初始化 predict 返回 nullopt（§4.4） +// ============================================================================= + +#include + +#include +#include + +#include "target_tracker/kalman_box_filter.h" + +using drone::tracker::BoxCXCYWH; +using drone::tracker::KalmanBoxFilter; + +namespace { + +// 未初始化时 predict 返回 nullopt（§4.4）；update 直接返回不改变状态（§4.5） +TEST(KalmanBoxFilterTest, UninitializedPredictReturnsNullopt) { + KalmanBoxFilter kf; + EXPECT_FALSE(kf.Initialized()); + EXPECT_FALSE(kf.Predict().has_value()); + + // 未初始化时 update 无副作用 + kf.Update({100.0, 100.0, 20.0, 20.0}); + EXPECT_FALSE(kf.Initialized()); + EXPECT_FALSE(kf.Predict().has_value()); + for (const double v : kf.State()) { + EXPECT_DOUBLE_EQ(v, 0.0); + } +} + +// 初始化状态正确（§4.3）：速度置零、位置/尺寸取首次观测 +TEST(KalmanBoxFilterTest, InitiateSetsStateCorrectly) { + KalmanBoxFilter kf; + kf.Initiate({320.0, 240.0, 40.0, 30.0}); + EXPECT_TRUE(kf.Initialized()); + + const std::array expected = { + {320.0, 240.0, 40.0, 30.0, 0.0, 0.0, 0.0, 0.0}}; + for (size_t i = 0; i < expected.size(); ++i) { + EXPECT_DOUBLE_EQ(kf.State()[i], expected[i]) << "index=" << i; + } + + // 初始化后 predict 返回状态前 4 维副本（速度为零 → 位置不变，§4.4） + const auto pred = kf.Predict(); + ASSERT_TRUE(pred.has_value()); + EXPECT_NEAR(pred->cx, 320.0, 1e-9); + EXPECT_NEAR(pred->cy, 240.0, 1e-9); + EXPECT_NEAR(pred->w, 40.0, 1e-9); + EXPECT_NEAR(pred->h, 30.0, 1e-9); +} + +// 静止目标：多帧重复观测同一框，滤波输出应收敛到观测值（§4.5） +TEST(KalmanBoxFilterTest, StaticTargetConvergesToObservation) { + KalmanBoxFilter kf; + const BoxCXCYWH z{100.0, 150.0, 40.0, 60.0}; + kf.Initiate({104.0, 156.0, 42.0, 62.0}); // 带初始偏差，验证收敛能力 + + for (int i = 0; i < 50; ++i) { + kf.Predict(); + kf.Update(z); + } + + EXPECT_NEAR(kf.State()[0], z.cx, 0.5); // cx + EXPECT_NEAR(kf.State()[1], z.cy, 0.5); // cy + EXPECT_NEAR(kf.State()[2], z.w, 0.5); // w + EXPECT_NEAR(kf.State()[3], z.h, 0.5); // h + // 静止目标速度应收敛到约 0 + EXPECT_NEAR(kf.State()[4], 0.0, 0.1); + EXPECT_NEAR(kf.State()[5], 0.0, 0.1); +} + +// 匀速目标：速度估计收敛到真实速度，且 predict 超前一个 dt（§4.4） +TEST(KalmanBoxFilterTest, ConstantVelocityConvergesAndPredictLeadsOneDt) { + KalmanBoxFilter kf; + const double vx = 3.0; // 像素/帧 + const double vy = 2.0; // 像素/帧 + + // 目标从 (100, 100) 出发匀速运动，每帧观测一次（predict → update） + BoxCXCYWH z{100.0, 100.0, 30.0, 30.0}; + kf.Initiate(z); + for (int i = 0; i < 60; ++i) { + kf.Predict(); + z.cx += vx; + z.cy += vy; + kf.Update(z); + } + + // 速度收敛到真实速度（像素/帧） + EXPECT_NEAR(kf.State()[4], vx, 0.3) << "vx 未收敛"; + EXPECT_NEAR(kf.State()[5], vy, 0.3) << "vy 未收敛"; + // 位置贴合当前观测 + EXPECT_NEAR(kf.State()[0], z.cx, 1.5); + EXPECT_NEAR(kf.State()[1], z.cy, 1.5); + + // 再预测一帧：应超前当前观测一个 dt（≈ 速度 × 1 帧） + const auto pred = kf.Predict(); + ASSERT_TRUE(pred.has_value()); + EXPECT_NEAR(pred->cx, z.cx + vx, 1.0) << "预测未超前一个 dt"; + EXPECT_NEAR(pred->cy, z.cy + vy, 1.0) << "预测未超前一个 dt"; +} + +// 动态噪声不退化：小尺寸目标多帧观测仍稳定收敛（§4.2 噪声与尺寸成比例） +TEST(KalmanBoxFilterTest, SmallTargetNoiseScalingStable) { + KalmanBoxFilter kf; + const BoxCXCYWH z{50.0, 60.0, 4.0, 4.0}; // 小目标（4×4 像素） + kf.Initiate(z); + for (int i = 0; i < 30; ++i) { + kf.Predict(); + kf.Update(z); + } + EXPECT_NEAR(kf.State()[0], z.cx, 0.5); + EXPECT_NEAR(kf.State()[1], z.cy, 0.5); + EXPECT_TRUE(std::isfinite(kf.State()[2])); + EXPECT_TRUE(std::isfinite(kf.State()[3])); +} + +} // namespace diff --git a/reference-c++/target_tracker/tests/target_tracker_test.cpp b/reference-c++/target_tracker/tests/target_tracker_test.cpp new file mode 100644 index 0000000..897f549 --- /dev/null +++ b/reference-c++/target_tracker/tests/target_tracker_test.cpp @@ -0,0 +1,402 @@ +// ============================================================================= +// target_tracker_test.cpp —— TargetTracker 单元测试 +// +// 逐条覆盖 docs/视觉追踪技术路线.md §9.4 行为一致性验收用例（硬验收标准）： +// 1. 空检测帧序列：Coast 持续 max_lost_frames 帧，第 max_lost_frames+1 帧判丢 +// 2. ID 严格递增不复用（含 Reset 后归零） +// 3. 锁定不被抢占；锁定目标死亡后切换到新目标 +// 4. min_hits 回退：无已确认实测轨道时新轨道也可当选主目标 +// 5. cost=1.0 恰好不构成匹配（严格小于） +// 6. 置信度优先：两轨道竞争同一检测，置信度高者获胜 +// ============================================================================= + +#include + +#include +#include +#include +#include + +#include "target_tracker/target_tracker.h" + +using drone::tracker::Detection; +using drone::tracker::FrameShape; +using drone::tracker::TargetTracker; +using drone::tracker::TrackResult; + +namespace { + +Detection MakeDet(double x1, double y1, double x2, double y2, double conf, + int cls = 0) { + return Detection{x1, y1, x2, y2, conf, cls}; +} + +// ============================================================================ +// 验收用例 1（§9.4-1）：空检测帧序列 +// 连续输入空检测，主目标输出 is_predicted=true 共 max_lost_frames 帧， +// 第 max_lost_frames + 1 帧 tracked=false（死亡判定 lost_count > max_lost）。 +// ============================================================================ +TEST(TargetTrackerAcceptanceTest, EmptyFramesCoastThenLost) { + TargetTracker tracker; // 默认配置 max_lost_frames = 8 + ASSERT_EQ(tracker.GetConfig().max_lost_frames, 8); + + // 第 1 帧：给出一个检测，建立轨道并锁定为主目标 + TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 140.0, 140.0, 0.9)}); + ASSERT_TRUE(r.tracked); + ASSERT_TRUE(r.primary_id.has_value()); + EXPECT_FALSE(r.is_predicted); // 当帧有实测 + EXPECT_EQ(r.lost_frames, 0); + EXPECT_EQ(r.n_active, 1); + ASSERT_TRUE(r.box.has_value()); + ASSERT_TRUE(r.raw.has_value()); // 当帧关联到原始检测框 + + // 后续空检测帧：应持续 Coast 共 max_lost_frames 帧（is_predicted=true） + for (int i = 1; i <= 8; ++i) { + r = tracker.Update({}); + ASSERT_TRUE(r.tracked) << "第 " << i << " 帧 Coast 期间不应判丢"; + EXPECT_TRUE(r.is_predicted) << "第 " << i << " 帧应为纯预测输出"; + EXPECT_EQ(r.lost_frames, i); + EXPECT_TRUE(r.primary_id.has_value()); + EXPECT_TRUE(r.box.has_value()); // Coast 期间继续输出预测框 + EXPECT_FALSE(r.raw.has_value()); // Coast 时 raw 为空（§2.3） + EXPECT_DOUBLE_EQ(r.confidence, 0.9); // 继承最近实测置信度（§2.3） + } + + // 第 max_lost_frames + 1 帧：轨道死亡回收 → tracked=false + r = tracker.Update({}); + EXPECT_FALSE(r.tracked); + EXPECT_FALSE(r.primary_id.has_value()); + EXPECT_FALSE(r.box.has_value()); + EXPECT_EQ(r.lost_frames, 0); // 丢失时 lost_frames=0（§2.3） + EXPECT_EQ(r.n_active, 0); +} + +// ============================================================================ +// 验收用例 2（§9.4-2）：ID 单调性 +// 多次进出画面的目标获得严格递增、不复用的 ID；Reset 后计数器归零。 +// ============================================================================ +TEST(TargetTrackerAcceptanceTest, IdStrictlyIncreasingNoReuse) { + TargetTracker::Config cfg; + cfg.max_lost_frames = 1; // 缩短 Coast 窗口，加速轨道死亡 + TargetTracker tracker(cfg); + + // 目标 A 出现 → id 0 + TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.9)}); + ASSERT_TRUE(r.primary_id.has_value()); + const int id_a = *r.primary_id; + EXPECT_EQ(id_a, 0); + + // 目标 A 消失至死亡（max_lost=1：第 2 帧 Coast、第 3 帧死亡回收） + tracker.Update({}); + r = tracker.Update({}); + EXPECT_FALSE(r.tracked); + EXPECT_EQ(tracker.ActiveCount(), 0); + + // 目标 B 出现（同一位置）：ID 不复用，严格递增 → id 1 + r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.9)}); + ASSERT_TRUE(r.primary_id.has_value()); + const int id_b = *r.primary_id; + EXPECT_EQ(id_b, 1); + EXPECT_GT(id_b, id_a); + + // 目标 C 同帧新建 → id 2（同帧多目标亦严格递增） + r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.9), + MakeDet(400.0, 300.0, 430.0, 330.0, 0.8)}); + EXPECT_EQ(tracker.ActiveCount(), 2); + int max_id = 0; + for (const auto& kv : tracker.ActiveTracks()) { + max_id = std::max(max_id, kv.first); + } + EXPECT_EQ(max_id, 2); + + // Reset 后计数器归零（§6.2）：新轨道重新从 id 0 开始 + tracker.Reset(); + EXPECT_EQ(tracker.ActiveCount(), 0); + EXPECT_FALSE(tracker.LastResult().tracked); + r = tracker.Update({MakeDet(50.0, 50.0, 70.0, 70.0, 0.9)}); + ASSERT_TRUE(r.primary_id.has_value()); + EXPECT_EQ(*r.primary_id, 0); +} + +// ============================================================================ +// 验收用例 3（§9.4-3）：锁定不被抢占 +// 锁定目标 Coast 期间出现更高置信度新目标，primary_id 不变； +// 锁定目标死亡后切换到新目标。 +// ============================================================================ +TEST(TargetTrackerAcceptanceTest, LockNotStolenAndSwitchAfterDeath) { + TargetTracker::Config cfg; + cfg.max_lost_frames = 3; + TargetTracker tracker(cfg); + + // 低置信度目标 A 建立并锁定 + TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 140.0, 140.0, 0.4)}); + ASSERT_TRUE(r.primary_id.has_value()); + const int id_a = *r.primary_id; + + // A 消失进入 Coast；期间出现更高置信度新目标 B（远离 A，不会被误关联） + r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)}); + ASSERT_TRUE(r.primary_id.has_value()); + EXPECT_EQ(*r.primary_id, id_a) << "Coast 期间锁定不应被高置信度目标抢占"; + EXPECT_TRUE(r.is_predicted); + EXPECT_EQ(tracker.ActiveCount(), 2); // A（Coast）与 B 并存 + + // 继续 Coast，锁定依旧延续 + r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)}); + EXPECT_EQ(*r.primary_id, id_a); + EXPECT_TRUE(r.is_predicted); + + // A 继续丢失直至死亡（max_lost=3：lost 达 4 帧时回收） + for (int i = 0; i < 2; ++i) { + r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)}); + } + // A 已死亡删除 → 锁定解除，重新选举切换到 B（此时 B 已确认且实测） + ASSERT_TRUE(r.tracked); + EXPECT_NE(*r.primary_id, id_a) << "锁定目标死亡后应切换到新目标"; + EXPECT_FALSE(r.is_predicted); + + // 后续 B 持续作为主目标 + r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)}); + ASSERT_TRUE(r.primary_id.has_value()); + EXPECT_NE(*r.primary_id, id_a); +} + +// ============================================================================ +// 验收用例 4（§9.4-4）：min_hits 回退 +// 新轨道（hits 未达 min_hits）在无其他已确认实测轨道时也可当选主目标。 +// ============================================================================ +TEST(TargetTrackerAcceptanceTest, MinHitsFallbackAllowsNewTrack) { + TargetTracker::Config cfg; + cfg.min_hits = 5; // 抬高确认门槛，保证测试期间轨道始终未确认 + TargetTracker tracker(cfg); + + // 首帧新建轨道 hits=1 < min_hits：候选池 confirmed 为空 → 退回 measured + TrackResult r = tracker.Update({MakeDet(200.0, 150.0, 240.0, 190.0, 0.6)}); + ASSERT_TRUE(r.tracked) << "无已确认轨道时新轨道应能当选主目标"; + ASSERT_TRUE(r.primary_id.has_value()); + EXPECT_EQ(*r.primary_id, 0); + EXPECT_FALSE(r.is_predicted); + + // 再跟几帧（hits 增长但仍未达 min_hits=5），依旧当选 + for (int i = 0; i < 3; ++i) { + r = tracker.Update({MakeDet(200.0, 150.0, 240.0, 190.0, 0.6)}); + ASSERT_TRUE(r.tracked); + EXPECT_EQ(*r.primary_id, 0); + } +} + +// ============================================================================ +// 验收用例 5（§9.4-5）：cost 边界 +// 恰好 cost = 1.0 不构成匹配（仅严格小于才匹配）。 +// 构造：w_dist=1、w_iou=0（纯距离项），中心距离恰等于 Dmax → cost=1.0； +// 同时给出略小于阈值的对照帧证明边界两侧行为正确。 +// ============================================================================ +TEST(TargetTrackerAcceptanceTest, CostExactlyOneDoesNotMatch) { + TargetTracker::Config cfg; + cfg.max_lost_frames = 1; + cfg.max_association_dist = 200.0; + cfg.dist_weight = 1.0; // 纯距离项，IoU 项权重为 0 且两框零交集 + cfg.iou_weight = 0.0; + TargetTracker tracker(cfg); + + // 建立轨道：中心 (100,100) + TrackResult r = tracker.Update({MakeDet(90.0, 90.0, 110.0, 110.0, 0.9)}); + ASSERT_TRUE(r.primary_id.has_value()); + const int id_a = *r.primary_id; + + // 下一帧检测中心 (100,300)：距离 = 200 = Dmax → cost = 1.0，恰好不匹配 + // （预测后轨道中心速度为 0，仍位于 (100,100)；两框零交集 IoU=0 无影响） + r = tracker.Update({MakeDet(90.0, 290.0, 110.0, 310.0, 0.9)}); + // 未匹配 → 原轨道 Coast、检测新建轨道；两个目标并存 + EXPECT_EQ(tracker.ActiveCount(), 2) << "cost=1.0 不应匹配，应新建轨道"; + // 原轨道（锁定）进入 Coast：lost_frames=1 + EXPECT_TRUE(r.tracked); + EXPECT_TRUE(r.is_predicted); + EXPECT_EQ(*r.primary_id, id_a); + EXPECT_EQ(r.lost_frames, 1); + + // 对照：距离略小于阈值（199px → cost=0.995 < 1.0）应匹配成功 + TargetTracker tracker2(cfg); + r = tracker2.Update({MakeDet(90.0, 90.0, 110.0, 110.0, 0.9)}); + const int id_b = *r.primary_id; + r = tracker2.Update({MakeDet(90.0, 289.0, 110.0, 309.0, 0.9)}); + EXPECT_EQ(tracker2.ActiveCount(), 1) << "cost<1.0 应匹配成功，不应新建轨道"; + ASSERT_TRUE(r.primary_id.has_value()); + EXPECT_EQ(*r.primary_id, id_b); + EXPECT_FALSE(r.is_predicted); // 实测命中 +} + +// ============================================================================ +// 验收用例 6（§9.4-6）：置信度优先 +// 两轨道竞争同一检测时，置信度高者获胜。 +// ============================================================================ +TEST(TargetTrackerAcceptanceTest, HigherConfidenceTrackWinsDetection) { + TargetTracker tracker; + + // 同帧建立两条轨道：A 置信度 0.5、B 置信度 0.9，相距 80px（不互相重叠） + TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.5), + MakeDet(180.0, 100.0, 200.0, 120.0, 0.9)}); + ASSERT_EQ(tracker.ActiveCount(), 2); + + // 找出两条轨道 ID（按置信度区分） + int id_low = -1; + int id_high = -1; + for (const auto& kv : tracker.ActiveTracks()) { + if (kv.second.confidence > 0.8) { + id_high = kv.first; + } else { + id_low = kv.first; + } + } + ASSERT_NE(id_low, -1); + ASSERT_NE(id_high, -1); + + // 下一帧只给一个检测，放在两轨道预测中心之间（各距 40px，对称）： + // 距离项对两者相同，贪婪按置信度降序 → 高置信度轨道 B 优先匹配 + r = tracker.Update({MakeDet(140.0, 100.0, 160.0, 120.0, 0.7)}); + + const auto tracks = tracker.ActiveTracks(); + ASSERT_EQ(tracks.size(), 2u); + // 获胜者：命中（lost=0、hits=2、置信度更新） + EXPECT_EQ(tracks.at(id_high).lost_count, 0) << "高置信度轨道应获胜"; + EXPECT_EQ(tracks.at(id_high).hits, 2); + EXPECT_DOUBLE_EQ(tracks.at(id_high).confidence, 0.7); + // 失败者：未匹配进入 Coast（lost=1、hits 不变） + EXPECT_EQ(tracks.at(id_low).lost_count, 1) << "低置信度轨道应落选"; + EXPECT_EQ(tracks.at(id_low).hits, 1); + EXPECT_DOUBLE_EQ(tracks.at(id_low).confidence, 0.5); +} + +// ============================================================================ +// NaN/Inf 防御（评审修复项）：坐标或置信度含非有限值的检测被丢弃， +// 不崩溃、不新建轨道、不污染主目标输出 +// ============================================================================ +TEST(TargetTrackerAcceptanceTest, NonFiniteDetectionsAreDiscarded) { + TargetTracker tracker; + const double nan = std::numeric_limits::quiet_NaN(); + const double inf = std::numeric_limits::infinity(); + + // 第 1 帧：建立正常轨道 + TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 140.0, 140.0, 0.9)}); + ASSERT_TRUE(r.tracked); + + // 第 2 帧：混入 NaN 坐标 / Inf 坐标 / NaN 置信度检测与一个正常检测： + // 非有限检测全部被丢弃，仅正常检测参与关联，不新建轨道 + r = tracker.Update({MakeDet(nan, 100.0, 140.0, 140.0, 0.9), + MakeDet(100.0, -inf, 140.0, 140.0, 0.9), + MakeDet(100.0, 100.0, nan, 140.0, 0.9), + MakeDet(102.0, 102.0, 142.0, 142.0, nan), + MakeDet(101.0, 101.0, 141.0, 141.0, 0.9)}); + EXPECT_EQ(tracker.ActiveCount(), 1) << "非有限检测不应新建轨道"; + ASSERT_TRUE(r.tracked); + EXPECT_FALSE(r.is_predicted); // 正常检测命中，实测输出 + // 主目标输出未被 NaN 污染 + EXPECT_TRUE(std::isfinite(r.center.first)); + EXPECT_TRUE(std::isfinite(r.center.second)); + ASSERT_TRUE(r.box.has_value()); + EXPECT_TRUE(std::isfinite(r.box->x1) && std::isfinite(r.box->y1)); + EXPECT_TRUE(std::isfinite(r.box->x2) && std::isfinite(r.box->y2)); + EXPECT_TRUE(std::isfinite(r.confidence)); + ASSERT_TRUE(r.raw.has_value()); // 实测帧 raw 有值 + + // 第 3 帧：仅含 NaN 检测 → 等价空帧，轨道 Coast 不崩溃 + r = tracker.Update({MakeDet(nan, nan, nan, nan, nan)}); + EXPECT_TRUE(r.tracked); + EXPECT_TRUE(r.is_predicted); + EXPECT_EQ(r.lost_frames, 1); + EXPECT_FALSE(r.raw.has_value()); // Coast 时 raw 为空（§2.3） + EXPECT_TRUE(std::isfinite(r.center.first)); + + // 持续 NaN 检测直至轨道死亡回收，全程不崩溃 + for (int i = 0; i < 10; ++i) { + r = tracker.Update({MakeDet(nan, 0.0, 10.0, 10.0, 0.9)}); + } + EXPECT_FALSE(r.tracked); + EXPECT_EQ(tracker.ActiveCount(), 0); +} + +// ============================================================================ +// 补充行为测试（§2.3 / §6.3 / §9.5） +// ============================================================================ + +// 丢失时返回值约定：tracked=false、定位字段空、lost_frames=0（§2.3） +TEST(TargetTrackerBehaviorTest, LostResultFieldsAreEmpty) { + TargetTracker tracker; + TrackResult r = tracker.Update({}); // 空帧序列起点：无任何轨道 + EXPECT_FALSE(r.tracked); + EXPECT_FALSE(r.primary_id.has_value()); + EXPECT_FALSE(r.box.has_value()); + EXPECT_FALSE(r.raw.has_value()); + EXPECT_EQ(r.lost_frames, 0); + EXPECT_EQ(r.n_active, 0); + EXPECT_DOUBLE_EQ(r.confidence, 0.0); + + // LastResult 返回同一快照（§9.5 跨线程读取入口） + const TrackResult cached = tracker.LastResult(); + EXPECT_FALSE(cached.tracked); +} + +// Coast 兜底选举：无实测轨道时选离画面中心最近的 Coast 轨道（§6.3）。 +// 注意锁定延续优先：需先让锁定轨道死亡解除锁定，才会进入重新选举分支， +// 故采用错帧创建 + 缩短 max_lost 的方式构造场景。 +TEST(TargetTrackerBehaviorTest, CoastingFallbackChoosesNearestToFrameCenter) { + TargetTracker::Config cfg; + cfg.max_lost_frames = 2; + TargetTracker tracker(cfg); + + // F1：角落目标（高置信度）先建立并被锁定 + tracker.Update({MakeDet(10.0, 10.0, 30.0, 30.0, 0.95)}); + // F2：画面中心附近目标建立；角落目标丢失(1) + tracker.Update({MakeDet(300.0, 220.0, 340.0, 260.0, 0.9)}); + // F3：远处目标建立；角落丢失(2)、中心目标丢失(1) + tracker.Update({MakeDet(600.0, 450.0, 620.0, 470.0, 0.85)}); + // F4：全部丢失：角落丢失(3) > max_lost 死亡解除锁定； + // 其余两条 Coast 存活 → 重新选举选离画面中心最近者 + TrackResult r = tracker.Update({}, FrameShape{480, 640}); + ASSERT_TRUE(r.tracked); + EXPECT_TRUE(r.is_predicted); + EXPECT_NEAR(r.center.first, 320.0, 25.0); // 靠近画面中心 (320,240) 者当选 + EXPECT_NEAR(r.center.second, 240.0, 25.0); + EXPECT_EQ(r.n_active, 2); // 角落轨道已回收 +} + +// 锁定延续优先于置信度选举：锁定轨道实测时即使置信度较低也保持主目标（§6.3 ①） +TEST(TargetTrackerBehaviorTest, LockedPrimaryKeptWhileMeasured) { + TargetTracker tracker; + TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.4)}); + ASSERT_TRUE(r.primary_id.has_value()); + const int locked_id = *r.primary_id; + + // 高置信度新目标出现并持续，锁定目标也持续实测 → primary_id 不变 + for (int i = 0; i < 5; ++i) { + r = tracker.Update({MakeDet(100.0 + i, 100.0, 120.0 + i, 120.0, 0.4), + MakeDet(400.0, 300.0, 430.0, 330.0, 0.95)}); + ASSERT_TRUE(r.primary_id.has_value()); + EXPECT_EQ(*r.primary_id, locked_id); + EXPECT_FALSE(r.is_predicted); + } +} + +// frame_shape 缺省按 640×480 画面中心 (320,240) 计算（§8.2 / §2.2）： +// 两条 Coast 轨道分别位于默认中心两侧，当选者应为更近的一侧 +TEST(TargetTrackerBehaviorTest, DefaultFrameCenterIs320x240) { + TargetTracker::Config cfg; + cfg.max_lost_frames = 2; + TargetTracker tracker(cfg); + + // F1：角落目标先建立并被锁定（高置信度） + tracker.Update({MakeDet(0.0, 0.0, 20.0, 20.0, 0.95)}); + // F2：左侧目标（中心 (280,240)，距默认中心 40px）建立 + tracker.Update({MakeDet(270.0, 230.0, 290.0, 250.0, 0.9)}); + // F3：右侧目标（中心 (500,240)，距默认中心 180px）建立 + tracker.Update({MakeDet(490.0, 230.0, 510.0, 250.0, 0.85)}); + // F4：全部丢失：锁定轨道死亡解除锁定，两条 Coast 轨道兜底选举 + // （不传 frame_shape → 按默认画面中心 (320,240)；两候选间距足够大， + // 速度估计漂移不影响最近者判定） + TrackResult r = tracker.Update({}); + ASSERT_TRUE(r.tracked); + EXPECT_TRUE(r.is_predicted); + EXPECT_NEAR(r.center.first, 280.0, 30.0); // 左侧更近，当选 + EXPECT_NEAR(r.center.second, 240.0, 30.0); +} + +} // namespace diff --git a/reference-c++/target_tracker/tests/tracklet_test.cpp b/reference-c++/target_tracker/tests/tracklet_test.cpp new file mode 100644 index 0000000..665bea6 --- /dev/null +++ b/reference-c++/target_tracker/tests/tracklet_test.cpp @@ -0,0 +1,177 @@ +// ============================================================================= +// tracklet_test.cpp —— Tracklet 与几何工具单元测试 +// +// 覆盖 docs/视觉追踪技术路线.md： +// - IoU 边界（空框 / 零交集 / 完全重合=1，§5.2） +// - IsDead / IsCoasting 判定（§3.2 派生状态，§6.1 严格大于） +// - Predict / Update / MarkMiss 计数语义（§7 末尾伪代码） +// - BoxXYXY ↔ BoxCXCYWH 互转（§7） +// ============================================================================= + +#include + +#include "target_tracker/tracklet.h" + +using drone::tracker::BoxCXCYWH; +using drone::tracker::BoxXYXY; +using drone::tracker::ComputeIoU; +using drone::tracker::ToCenterSize; +using drone::tracker::ToXYXY; +using drone::tracker::Tracklet; + +namespace { + +// ---- 框表示互转（§7） ---- + +TEST(BoxConversionTest, RoundTripPreservesValues) { + const BoxXYXY box{10.0, 20.0, 50.0, 80.0}; + const BoxCXCYWH c = ToCenterSize(box); + EXPECT_DOUBLE_EQ(c.cx, 30.0); // (10+50)/2 + EXPECT_DOUBLE_EQ(c.cy, 50.0); // (20+80)/2 + EXPECT_DOUBLE_EQ(c.w, 40.0); // 50−10 + EXPECT_DOUBLE_EQ(c.h, 60.0); // 80−20 + + const BoxXYXY back = ToXYXY(c); + EXPECT_DOUBLE_EQ(back.x1, box.x1); + EXPECT_DOUBLE_EQ(back.y1, box.y1); + EXPECT_DOUBLE_EQ(back.x2, box.x2); + EXPECT_DOUBLE_EQ(back.y2, box.y2); +} + +// ---- IoU 边界（§5.2） ---- + +TEST(IoUTest, IdenticalBoxesReturnOne) { + const BoxXYXY a{10.0, 10.0, 30.0, 30.0}; + EXPECT_NEAR(ComputeIoU(a, a), 1.0, 1e-12); // 完全重合 = 1 +} + +TEST(IoUTest, DisjointBoxesReturnZero) { + const BoxXYXY a{0.0, 0.0, 10.0, 10.0}; + const BoxXYXY b{20.0, 20.0, 30.0, 30.0}; + EXPECT_DOUBLE_EQ(ComputeIoU(a, b), 0.0); // 零交集 +} + +TEST(IoUTest, TouchingEdgesReturnZero) { + // 仅边/角相接：交集宽高为 0 → 交集面积 ≤ 0 返回 0（§5.2） + const BoxXYXY a{0.0, 0.0, 10.0, 10.0}; + const BoxXYXY b{10.0, 0.0, 20.0, 10.0}; // 右边缘相接 + EXPECT_DOUBLE_EQ(ComputeIoU(a, b), 0.0); + const BoxXYXY c{10.0, 10.0, 20.0, 20.0}; // 角点相接 + EXPECT_DOUBLE_EQ(ComputeIoU(a, c), 0.0); +} + +TEST(IoUTest, EmptyBoxReturnsZero) { + const BoxXYXY normal{0.0, 0.0, 10.0, 10.0}; + const BoxXYXY zero_area{5.0, 5.0, 5.0, 5.0}; // 零面积 + const BoxXYXY inverted{10.0, 10.0, 0.0, 0.0}; // 反向（负宽高） + EXPECT_DOUBLE_EQ(ComputeIoU(normal, zero_area), 0.0); + EXPECT_DOUBLE_EQ(ComputeIoU(zero_area, normal), 0.0); + EXPECT_DOUBLE_EQ(ComputeIoU(normal, inverted), 0.0); + EXPECT_DOUBLE_EQ(ComputeIoU(zero_area, zero_area), 0.0); +} + +TEST(IoUTest, PartialOverlapIsCorrect) { + // A = [0,0,10,10]（面积 100），B = [5,5,15,15]（面积 100），交集 25 + const BoxXYXY a{0.0, 0.0, 10.0, 10.0}; + const BoxXYXY b{5.0, 5.0, 15.0, 15.0}; + EXPECT_NEAR(ComputeIoU(a, b), 25.0 / 175.0, 1e-12); + EXPECT_NEAR(ComputeIoU(b, a), 25.0 / 175.0, 1e-12); // 对称性 +} + +// ---- 构造与派生状态（§3.2 / §6.1） ---- + +TEST(TrackletTest, ConstructionInitialFields) { + const BoxCXCYWH meas{100.0, 100.0, 20.0, 20.0}; + Tracklet t(7, meas, 0.9, 1, 8); + + EXPECT_EQ(t.track_id, 7); + EXPECT_EQ(t.lost_count, 0); + EXPECT_EQ(t.max_lost, 8); + EXPECT_DOUBLE_EQ(t.confidence, 0.9); + EXPECT_EQ(t.cls_id, 1); + EXPECT_EQ(t.age, 1); // 创建时为 1（§3.2） + EXPECT_EQ(t.hits, 1); // 创建时为 1（§3.2） + EXPECT_DOUBLE_EQ(t.last_center.first, 100.0); + EXPECT_DOUBLE_EQ(t.last_center.second, 100.0); + ASSERT_TRUE(t.last_box.has_value()); // 创建当帧即赋 last_box（§7 步骤 5） + EXPECT_DOUBLE_EQ(t.last_box->x1, 90.0); + EXPECT_DOUBLE_EQ(t.last_box->y1, 90.0); + EXPECT_DOUBLE_EQ(t.last_box->x2, 110.0); + EXPECT_DOUBLE_EQ(t.last_box->y2, 110.0); + EXPECT_FALSE(t.IsDead()); + EXPECT_FALSE(t.IsCoasting()); +} + +// IsDead 使用严格大于：lost_count == max_lost 仍存活（§6.1 注记 2） +TEST(TrackletTest, IsDeadUsesStrictlyGreater) { + Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8); + for (int i = 0; i < 8; ++i) { + t.MarkMiss(); + EXPECT_FALSE(t.IsDead()) << "lost_count=" << t.lost_count + << " 未超过 max_lost 不应判死"; + } + EXPECT_EQ(t.lost_count, 8); + t.MarkMiss(); // lost_count = 9 > 8 + EXPECT_TRUE(t.IsDead()); +} + +TEST(TrackletTest, IsCoastingWhenLostPositive) { + Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8); + EXPECT_FALSE(t.IsCoasting()); + t.MarkMiss(); + EXPECT_TRUE(t.IsCoasting()); // lost_count > 0 即 Coast（§3.2） +} + +// ---- 生命周期方法计数语义（§7） ---- + +TEST(TrackletTest, PredictAdvancesAgeAndUpdatesLastState) { + Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8); + const int age_before = t.age; + t.Predict(); // 速度为零：位置不变，但 age+1、last_box 更新为预测框 + EXPECT_EQ(t.age, age_before + 1); + ASSERT_TRUE(t.last_box.has_value()); + EXPECT_NEAR(t.last_center.first, 100.0, 1e-9); + EXPECT_NEAR(t.last_center.second, 100.0, 1e-9); + // hits / lost_count / confidence 不受预测影响 + EXPECT_EQ(t.hits, 1); + EXPECT_EQ(t.lost_count, 0); + EXPECT_DOUBLE_EQ(t.confidence, 0.9); +} + +TEST(TrackletTest, UpdateResetsLostAndIncrementsHits) { + Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8); + t.MarkMiss(); + t.MarkMiss(); + ASSERT_EQ(t.lost_count, 2); + ASSERT_TRUE(t.IsCoasting()); + + const int age_before = t.age; + const BoxXYXY box{150.0, 160.0, 170.0, 180.0}; + t.Update(box, 0.75, 2); + + EXPECT_EQ(t.lost_count, 0); // 匹配成功清零（§7） + EXPECT_EQ(t.hits, 2); // hits+1 + EXPECT_EQ(t.age, age_before + 1); // age+1 + EXPECT_DOUBLE_EQ(t.confidence, 0.75); + EXPECT_EQ(t.cls_id, 2); + EXPECT_DOUBLE_EQ(t.last_center.first, 160.0); // (150+170)/2 + EXPECT_DOUBLE_EQ(t.last_center.second, 170.0); // (160+180)/2 + ASSERT_TRUE(t.last_box.has_value()); + EXPECT_DOUBLE_EQ(t.last_box->x1, box.x1); + ASSERT_TRUE(t.last_raw_box.has_value()); // 实测框同步记录 + EXPECT_DOUBLE_EQ(t.last_raw_box->x2, box.x2); + EXPECT_FALSE(t.IsCoasting()); +} + +TEST(TrackletTest, MarkMissIncrementsLostAndAge) { + Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8); + const int age_before = t.age; + t.MarkMiss(); + EXPECT_EQ(t.lost_count, 1); + EXPECT_EQ(t.age, age_before + 1); + // MarkMiss 不影响 hits / confidence + EXPECT_EQ(t.hits, 1); + EXPECT_DOUBLE_EQ(t.confidence, 0.9); +} + +} // namespace diff --git a/reference-c++/visual_track_control/CMakeLists.txt b/reference-c++/visual_track_control/CMakeLists.txt new file mode 100644 index 0000000..2c2d9a2 --- /dev/null +++ b/reference-c++/visual_track_control/CMakeLists.txt @@ -0,0 +1,122 @@ +cmake_minimum_required(VERSION 3.16) + +# ============================================================================= +# visual_track_control —— 视觉跟踪控制律静态库（独立子工程） +# +# 实现 docs/物理追踪思路.md 的水平航向控制（§4）、垂直/高度双支路控制（§5）、 +# 距离 PID（§6）、全量限幅（§8）、Coast 清积分与判丢刹车（§6.4）、 +# 超龄降级（§7.1、§9.2）。 +# +# 构建约束： +# - 与主工程完全解耦：根 CMakeLists.txt 零改动，可独立构建； +# - 仅依赖 third_party：nlohmann（单头文件）、spdlog（源码）、 +# c_library_v2-master（MAVLink 纯头文件）； +# - target_tracker 为 SHARED 库且零源码修改，原样 add_subdirectory 复用， +# TrackResult 出现在本库公共头（vtc_types.h），故以 PUBLIC 链接； +# - MAVLink include 目录设为 SYSTEM PRIVATE：压告警噪音且不泄漏给下游， +# 全工程仅 src/mavlink_command_gateway.cpp 一个 TU 包含 mavlink.h +# （MAVLINK_HELPER 默认空宏，多 TU 包含会多重定义）。 +# ============================================================================= + +project(visual_track_control LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# target_tracker 是 SHARED 库：BUILD_RPATH 指向构建目录，示例无需 +# LD_LIBRARY_PATH 即可直接运行（部署时需随带 libtarget_tracker.so.1）。 +set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR};${CMAKE_BINARY_DIR}/target_tracker;$ORIGIN") + +# ---- 依赖：target_tracker（零改动复用；其测试块由 -DBUILD_TESTING=OFF 压制） ---- +add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../target_tracker + ${CMAKE_BINARY_DIR}/target_tracker) + +# ---- 依赖：spdlog（守卫引入，避免挂载进根构建时重复定义；关闭示例/测试/动态库/安装） ---- +if(NOT TARGET spdlog::spdlog) + set(SPDLOG_BUILD_EXAMPLE OFF CACHE BOOL "" FORCE) + set(SPDLOG_BUILD_EXAMPLE_HO OFF CACHE BOOL "" FORCE) + set(SPDLOG_BUILD_TESTS OFF CACHE BOOL "" FORCE) + set(SPDLOG_BUILD_TESTS_HO OFF CACHE BOOL "" FORCE) + set(SPDLOG_BUILD_SHARED OFF CACHE BOOL "" FORCE) + set(SPDLOG_INSTALL OFF CACHE BOOL "" FORCE) + add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../third_party/spdlog + ${CMAKE_BINARY_DIR}/spdlog) +endif() + +# ---- 静态库本体 ---- +add_library(visual_track_control STATIC + src/vtc_config.cpp + src/pid_controller.cpp + src/line_of_sight.cpp + src/tracking_control_law.cpp + src/mavlink_command_gateway.cpp) + +target_include_directories(visual_track_control PUBLIC + $) + +# nlohmann JSON：单头文件，仅配置加载器（启动期）使用，不进入热路径 +target_include_directories(visual_track_control PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../third_party/nlohmann) + +# MAVLink 纯头文件库：SYSTEM 压告警、PRIVATE 不泄漏（公共头无任何 mavlink_* 类型） +target_include_directories(visual_track_control SYSTEM PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../third_party/c_library_v2-master) + +target_link_libraries(visual_track_control + PUBLIC target_tracker + PRIVATE spdlog::spdlog) +target_compile_features(visual_track_control PUBLIC cxx_std_17) + +# 编译告警：与主工程一致 +if(MSVC) + target_compile_options(visual_track_control PRIVATE /W4 /permissive-) +else() + target_compile_options(visual_track_control PRIVATE -Wall -Wextra -Wpedantic) +endif() + +# ---- 示例可执行文件（合成检测 → 追踪 → 控制律 → MAVLink 字节流打印） ---- +add_executable(track_control_demo examples/track_control_demo.cpp) +target_link_libraries(track_control_demo PRIVATE visual_track_control spdlog::spdlog) +target_compile_definitions(track_control_demo PRIVATE + VTC_DEMO_DEFAULT_CONFIG="${CMAKE_CURRENT_SOURCE_DIR}/config/track_control.json") +if(MSVC) + target_compile_options(track_control_demo PRIVATE /W4 /permissive-) +else() + target_compile_options(track_control_demo PRIVATE -Wall -Wextra -Wpedantic) +endif() + +# ---- 单元测试（Google Test） ---- +# 独立选项：构建本子工程时需 -DBUILD_TESTING=OFF 压制 target_tracker 的测试， +# 故本库测试开关不复用 BUILD_TESTING，二者互不干扰。 +# 开发机未安装 GTest 时自动跳过（不影响库与示例构建）。 +option(VTC_BUILD_TESTS "构建 visual_track_control 单元测试" ON) +if(VTC_BUILD_TESTS) + find_package(GTest QUIET) + if(GTest_FOUND) + enable_testing() + include(GoogleTest) + add_executable(vtc_unit_tests + tests/rate_limiter_test.cpp + tests/staleness_checker_test.cpp + tests/frame_transform_test.cpp + tests/line_of_sight_test.cpp + tests/pid_controller_test.cpp + tests/tracking_control_law_test.cpp + tests/vtc_config_test.cpp + tests/mavlink_command_gateway_test.cpp) + target_link_libraries(vtc_unit_tests PRIVATE + visual_track_control GTest::gtest GTest::gtest_main) + target_compile_definitions(vtc_unit_tests PRIVATE + VTC_TEST_CONFIG="${CMAKE_CURRENT_SOURCE_DIR}/config/track_control.json" + VTC_TEST_TMP_DIR="${CMAKE_CURRENT_BINARY_DIR}") + if(MSVC) + target_compile_options(vtc_unit_tests PRIVATE /W4 /permissive-) + else() + target_compile_options(vtc_unit_tests PRIVATE -Wall -Wextra -Wpedantic) + endif() + gtest_discover_tests(vtc_unit_tests) + else() + message(STATUS "未找到 GTest：跳过 vtc_unit_tests 单元测试") + endif() +endif() diff --git a/reference-c++/visual_track_control/config/track_control.json b/reference-c++/visual_track_control/config/track_control.json new file mode 100644 index 0000000..7dcd26b --- /dev/null +++ b/reference-c++/visual_track_control/config/track_control.json @@ -0,0 +1,48 @@ +{ + "_comment": "视觉跟踪控制律配置。全部数值为占位示例（对应 docs/物理追踪思路.md 第 8 章待定项），键名后缀标注单位；标定完成前仅限回放验证，不得用于实飞。", + + "control": { + "frequency_hz": 20.0, + "visual_stale_ms": 200, + "radar_stale_ms": 500, + "attitude_stale_ms": 300 + }, + + "camera": { + "image_width": 1920, + "image_height": 1080, + "fov_h_deg": 60.0, + "fov_v_deg": 35.0 + }, + + "heading": { + "mode": "rate", + "gain": 1.0, + "yaw_rate_limit_dps": 30.0, + "yaw_slew_limit_dps2": 60.0 + }, + + "vertical": { + "vz_gain_mps_per_deg": 0.1, + "vz_limit_mps": 2.0 + }, + + "distance": { + "d_exp_m": 10.0, + "kp": 0.5, + "ki": 0.05, + "kd": 0.1, + "integral_limit": 2.0, + "derivative_filter_coef": 0.2, + "approach_velocity_limit_mps": 3.0, + "retreat_velocity_limit_mps": 1.0, + "no_distance_action": "hold", + "no_distance_approach_limit_mps": 0.5 + }, + + "accel_limit": { + "ax_mps2": 2.0, + "ay_mps2": 2.0, + "az_mps2": 1.5 + } +} diff --git a/reference-c++/visual_track_control/examples/track_control_demo.cpp b/reference-c++/visual_track_control/examples/track_control_demo.cpp new file mode 100644 index 0000000..3e1a4b6 --- /dev/null +++ b/reference-c++/visual_track_control/examples/track_control_demo.cpp @@ -0,0 +1,225 @@ +// ============================================================================= +// track_control_demo.cpp —— 视觉跟踪控制律演示程序 +// +// 数据流（模拟单线程整合的完整链路）： +// 合成运动检测框 → TargetTracker::Update()（模拟推理线程每帧调用） +// → LastResult() + 打戳 → ControlSnapshot → TrackingControlLaw::Update() +// → MavlinkCommandGateway → LoggingTransport（字节数 + 十六进制摘要） +// +// 依次演示三种分支（对应物理追踪思路 §6.4）： +// 1. 正常跟踪：持续喂入合成检测框，输出速度航向意图（垂直/距离通道 +// 统一为速度分量）； +// 2. Coast：停止喂检测 → is_predicted==true → 清积分，继续预测跟踪； +// 3. 判丢刹车：连续丢帧超过追踪库阈值 → tracked==false → 零速刹车。 +// +// 运行方式： +// track_control_demo [配置文件路径] # 缺省使用编译期内置路径 +// ============================================================================= + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "visual_track_control/i_command_transport.h" +#include "visual_track_control/mavlink_command_gateway.h" +#include "visual_track_control/tracking_control_law.h" +#include "visual_track_control/vtc_config.h" +#include "visual_track_control/vtc_types.h" + +#ifndef VTC_DEMO_DEFAULT_CONFIG +#define VTC_DEMO_DEFAULT_CONFIG "config/track_control.json" +#endif + +namespace { + +// ---- 演示时长（秒）：总时长有限，自动退出 ---- +constexpr int kNormalPhaseSeconds = 6; // 阶段1：正常跟踪 +constexpr int kCoastPhaseSeconds = 4; // 阶段2：Coast（停止喂检测） +constexpr int kLostPhaseSeconds = 3; // 阶段3：判丢刹车 + +// 追踪库判丢阈值放大：默认 8 帧在 20Hz 下 Coast 仅 0.4 秒，不便于观察； +// 演示专用放宽到 60 帧（约 3 秒），正式参数以追踪库规格为准。 +constexpr int kDemoMaxLostFrames = 60; + +const char* ModeName(drone::vtc::ControlMode mode) { + switch (mode) { + case drone::vtc::ControlMode::kVelocityHeading: return "速度航向"; + case drone::vtc::ControlMode::kBrakeHover: return "零速刹车"; + case drone::vtc::ControlMode::kDegradedHold: return "降级保持"; + } + return "未知"; +} + +/// 演示用传输实现：统计字节数并打印十六进制摘要（按秒级节流，防止刷屏） +class LoggingTransport : public drone::vtc::ICommandTransport { +public: + bool Send(const std::uint8_t* data, std::size_t len) override { + ++msg_count_; + total_bytes_ += len; + + const auto now_ms = + std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); + if (now_ms - last_print_ms_ >= 1000) { + // 十六进制摘要：取包头前 12 字节 + std::string hex; + char buf[4]; + const std::size_t n = len < 12 ? len : 12; + for (std::size_t i = 0; i < n; ++i) { + std::snprintf(buf, sizeof(buf), "%02X ", data[i]); + hex += buf; + } + spdlog::info("[Transport] 累计 {} 帧 / {} 字节; 本帧 {} 字节, 包头: {}", + msg_count_, total_bytes_, len, hex); + last_print_ms_ = now_ms; + } + return true; + } + +private: + std::int64_t last_print_ms_ = 0; + std::uint64_t msg_count_ = 0; + std::uint64_t total_bytes_ = 0; +}; + +/// 合成运动检测框：从画面右下向中心缓慢漂移（模拟目标被持续锁定） +std::vector SynthesizeDetections(double elapsed_s, + int width, int height) { + const double center_x = width * 0.5 + 160.0 - 20.0 * elapsed_s; + const double center_y = height * 0.5 + 80.0 - 10.0 * elapsed_s; + const double half_w = 70.0; + const double half_h = 50.0; + + drone::tracker::Detection det; + det.x1 = center_x - half_w; + det.y1 = center_y - half_h; + det.x2 = center_x + half_w; + det.y2 = center_y + half_h; + det.conf = 0.9; + det.cls_id = 0; + return {det}; +} + +} // namespace + +int main(int argc, char** argv) { + // ---- 启动期：一次性加载配置（缺字段/非法值直接抛异常终止） ---- + const std::string config_path = argc > 1 ? argv[1] : VTC_DEMO_DEFAULT_CONFIG; + drone::vtc::TrackingConfig cfg; + try { + cfg = drone::vtc::TrackingConfig::LoadFromJson(config_path); + } catch (const std::exception& e) { + spdlog::error("配置加载失败: {}", e.what()); + return 1; + } + spdlog::info("配置加载成功: {} (控制频率 {} Hz)", config_path, + cfg.control.frequency_hz); + + // ---- 构造：追踪库 / 控制律 / 传输 / 网关 ---- + drone::tracker::TargetTracker::Config tracker_cfg; + tracker_cfg.max_lost_frames = kDemoMaxLostFrames; // 演示专用判丢阈值 + drone::tracker::TargetTracker tracker(tracker_cfg); + + drone::vtc::TrackingControlLaw control_law(cfg); + LoggingTransport transport; + drone::vtc::MavlinkCommandGateway gateway(transport, /*sys=*/1, /*comp=*/1, + /*target_sys=*/1, /*target_comp=*/1); + + const int width = cfg.camera.image_width; + const int height = cfg.camera.image_height; + const int period_ms = static_cast(1000.0 / cfg.control.frequency_hz); + const int normal_ticks = kNormalPhaseSeconds * static_cast(cfg.control.frequency_hz); + const int coast_ticks = kCoastPhaseSeconds * static_cast(cfg.control.frequency_hz); + const int lost_ticks = kLostPhaseSeconds * static_cast(cfg.control.frequency_hz); + const int total_ticks = normal_ticks + coast_ticks + lost_ticks; + + spdlog::info("演示开始: 正常跟踪 {}s → Coast {}s → 判丢刹车 {}s (共 {} 拍, {} ms/拍)", + kNormalPhaseSeconds, kCoastPhaseSeconds, kLostPhaseSeconds, + total_ticks, period_ms); + + drone::tracker::FrameShape frame_shape; + frame_shape.height = height; + frame_shape.width = width; + + const char* current_phase = nullptr; + + // ---- 主循环：按配置频率固定节拍（§7.1） ---- + for (int tick = 0; tick < total_ticks; ++tick) { + const double elapsed_s = static_cast(tick) / cfg.control.frequency_hz; + // 模拟单调时钟：1 秒偏移保证取样时间戳有效 + const std::int64_t now_ms = 1000 + static_cast(tick) * period_ms; + + // ---- 阶段划分 ---- + const char* phase = tick < normal_ticks ? "正常跟踪" + : tick < normal_ticks + coast_ticks ? "Coast" + : "判丢刹车"; + if (phase != current_phase) { + spdlog::info("===== 阶段切换: {} =====", phase); + current_phase = phase; + } + + // ---- 模拟推理线程：仅正常跟踪阶段喂入检测框 ---- + // 阶段2/3 停止喂检测：先 Coast（is_predicted），后判丢（tracked==false） + std::vector detections; + if (tick < normal_ticks) { + detections = SynthesizeDetections(elapsed_s, width, height); + } + tracker.Update(detections, frame_shape); + + // ---- 控制线程：LastResult() + 打戳（TrackResult 无时间戳，§7.2） ---- + drone::vtc::ControlSnapshot snapshot; + snapshot.track = tracker.LastResult(); + snapshot.track_time_ms = now_ms; + + // 雷达：正常阶段关联成立；Coast/刹车阶段取消关联（演示 §6.3 降级路径） + snapshot.radar_distance_m = 40.0 - 3.0 * elapsed_s; + snapshot.associated_to_target = (tick < normal_ticks) && + snapshot.radar_distance_m > 0.0; + snapshot.radar_time_ms = now_ms; + + // 自机姿态：演示中始终新鲜有效（接入主工程时来自 FlightStateSnapshot） + snapshot.attitude_present = true; + snapshot.attitude_time_ms = now_ms; + + // ---- 控制律单周期计算 ---- + const drone::vtc::ControlOutput output = control_law.Update(snapshot, now_ms); + + // ---- 按模式经网关发送 ---- + const auto time_boot_ms = static_cast(now_ms); + switch (output.mode) { + case drone::vtc::ControlMode::kVelocityHeading: + gateway.SendVelocityHeading(output, time_boot_ms); + break; + case drone::vtc::ControlMode::kBrakeHover: + gateway.SendBrakeHover(time_boot_ms); + break; + case drone::vtc::ControlMode::kDegradedHold: + break; // 降级保持：不发送指令（仅告警，§7.1） + } + + // ---- 半秒一次的摘要打印（演示观测用，非生产热路径） ---- + if (tick % 10 == 0) { + spdlog::info( + "[t={:5.1f}s] 阶段={} 模式={}{} tracked={} vx={:+.2f} vy={:+.2f} " + "vz={:+.2f} yaw_rate={:+.1f}dps d={:5.1f}m 关联={}", + elapsed_s, phase, ModeName(output.mode), + output.coast_active ? "(Coast)" : "", snapshot.track.tracked, + output.vx_mps, output.vy_mps, output.vz_mps, output.yaw_rate_dps, + snapshot.radar_distance_m, snapshot.associated_to_target); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); + } + + control_law.Reset(); // 停止视觉闭环：清全部状态（§6.4） + spdlog::info("演示结束"); + return 0; +} diff --git a/reference-c++/visual_track_control/include/visual_track_control/frame_transform.h b/reference-c++/visual_track_control/include/visual_track_control/frame_transform.h new file mode 100644 index 0000000..1318f64 --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/frame_transform.h @@ -0,0 +1,44 @@ +#pragma once + +// ============================================================================= +// frame_transform.h —— 机体系（前/右/下）→ 局部 NED（北/东/地）坐标旋转 +// +// 对应规格：docs/物理追踪思路.md +// §2.3 控制律内部计算统一在机体坐标系；输出的速度意图若采用局部系， +// 必须随姿态完成旋转并显式标注坐标系，禁止混用未标注坐标系的数据。 +// +// 实现要点： +// - 速度设定值只随**航向角 ψ**（绕下轴旋转）旋转：水平速度分量的 +// 姿态耦合项由飞控内环处理，这是 PX4 offboard 速度设定值的标准做法； +// 滚转/俯仰不参与（对速度指令为二阶小量）； +// - 旋转为正交变换：合速度大小不变，因此"限幅在机体系完成、旋转在 +// 输出端执行"不破坏 §8 限幅约束； +// - 无状态纯函数（零分配、无虚函数、无锁），可独立单元测试。 +// +// 正方向验证（NED：x=北，y=东，z=地；航向 ψ 自北顺时针为正）： +// - 机体前向 (1,0) 在 ψ=90°（机头朝东）时 → (0, 1) = 东 ✓ +// - 机体右向 (0,1) 在 ψ=0°（机头朝北）时 → (0, 1) = 东 ✓ +// ============================================================================= + +#include + +namespace drone::vtc { + +/// 机体系速度（前/右/下）→ 局部 NED（北/东/地），绕下轴按航向角 ψ 旋转： +/// v_north = v_forward·cosψ − v_right·sinψ +/// v_east = v_forward·sinψ + v_right·cosψ +/// v_down = v_down（不变） +/// 参数以引用传入并就地写出旋转结果。 +inline void RotateBodyToNed(double& v_forward, double& v_right, double& v_down, + double yaw_rad) { + (void)v_down; // 垂直分量不受绕下轴旋转影响；保留参数以保持接口对称 + const double c = std::cos(yaw_rad); + const double s = std::sin(yaw_rad); + const double north = v_forward * c - v_right * s; + const double east = v_forward * s + v_right * c; + v_forward = north; + v_right = east; + // v_down 不变 +} + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/include/visual_track_control/i_command_transport.h b/reference-c++/visual_track_control/include/visual_track_control/i_command_transport.h new file mode 100644 index 0000000..b9cc6cc --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/i_command_transport.h @@ -0,0 +1,31 @@ +#pragma once + +// ============================================================================= +// i_command_transport.h —— 命令传输层抽象接口 +// +// 设计目标（双重解耦）： +// - 传输层只吃字节流：串口 / 电台 / 回环打印均可注入实现， +// 后续重写传输只换实现，不动打包与控制律； +// - 打包封装（MavlinkCommandGateway）与本接口组合，不直接依赖具体链路。 +// +// 控制律不直接写飞控（物理追踪思路 §2.2 控制单出口约束的独立库映射）： +// 网关打包后交由实现方决定如何送达。 +// ============================================================================= + +#include +#include + +namespace drone::vtc { + +/// 命令传输接口：只吃字节流 +class ICommandTransport { +public: + virtual ~ICommandTransport() = default; + + /// 发送一帧已打包的命令字节流。 + /// 返回 true = 已交付底层链路；false = 发送失败（调用方可计数/告警）。 + /// 实现方保证：不长期持有 data 指针（同步拷贝或同步写出）。 + virtual bool Send(const std::uint8_t* data, std::size_t len) = 0; +}; + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/include/visual_track_control/line_of_sight.h b/reference-c++/visual_track_control/include/visual_track_control/line_of_sight.h new file mode 100644 index 0000000..e0040e9 --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/line_of_sight.h @@ -0,0 +1,38 @@ +#pragma once + +// ============================================================================= +// line_of_sight.h —— 像素偏移 → 视线角（针孔模型） +// +// 对应规格：docs/物理追踪思路.md +// §4.1 水平视线角 β： +// 针孔模型 β = atan( x_offset · 2·tan(θ_h/2) / W ) +// （线性近似 β ≈ x_offset · ( θ_h / W ) 仅在 |x_offset| 很小时成立） +// §5.1 垂直视线角 α 同理（θ_v、H） +// +// 实现要点： +// - 全程使用精确针孔模型：规格中的线性近似与针孔式在切换边界两侧 +// 函数值不连续（θ=60°、边界比例 0.2 处约 8% 角度台阶），会向控制 +// 环路注入突变；atan 计算代价可忽略，故直接全程采用精确式， +// linear_ratio 配置项随之移除（§8 待定项收敛）； +// - 构造期预计算针孔系数，热路径仅一次乘加 + atan +// （零分配、无虚函数、无锁）。 +// ============================================================================= + +namespace drone::vtc { + +/// 像素偏移 → 视线角（弧度）转换器；水平/垂直各构造一个实例 +class PixelToAngle { +public: + /// size_px：画面尺寸（W 或 H，像素，必须 > 0）； + /// fov_rad：视场角（θ_h 或 θ_v，弧度，必须 ∈ (0, π)）。 + /// 非法参数抛 std::invalid_argument（构造期校验）。 + PixelToAngle(double size_px, double fov_rad); + + /// 偏移（像素，水平右正 / 垂直下正）→ 视线角（弧度，同向为正） + double Convert(double offset_px) const; + +private: + double pinhole_scale_; // 2·tan(θ/2) / size：针孔模型系数 +}; + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/include/visual_track_control/mavlink_command_gateway.h b/reference-c++/visual_track_control/include/visual_track_control/mavlink_command_gateway.h new file mode 100644 index 0000000..91dc44f --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/mavlink_command_gateway.h @@ -0,0 +1,65 @@ +#pragma once + +// ============================================================================= +// mavlink_command_gateway.h —— MAVLink 命令打包网关（对外声明） +// +// 对应规格：docs/物理追踪思路.md §2.2 +// - kVelocityHeading → SET_POSITION_TARGET_LOCAL_NED（ID 84，速度模式） +// - kBrakeHover → ID 84 全零速度 +// （原 kAttitudeTrim → ID 82 已随姿态微调支路一并移除，见 vtc_types.h） +// +// 关键构建约束（务必遵守）： +// - MAVLink 头（common/mavlink.h）只允许被本库的 +// src/mavlink_command_gateway.cpp 一个 TU 包含： +// MAVLINK_HELPER 默认空宏，多 TU 包含会多重定义； +// - 因此本头文件不出现任何 mavlink_* 类型，打包/发送缓冲用 +// 固定尺寸字节存储承载（尺寸在 .cpp 中 static_assert 校验）。 +// +// 线程与资源：无线程、无 IO、公共方法非虚；成员固定缓冲，热路径零堆分配。 +// ============================================================================= + +#include +#include + +#include "visual_track_control/i_command_transport.h" +#include "visual_track_control/vtc_types.h" + +namespace drone::vtc { + +/// MAVLink 命令打包网关：控制意图 → MAVLink 消息字节流 → 传输接口 +class MavlinkCommandGateway { +public: + /// transport：字节流传输实现（注入，生命周期由调用方保证）； + /// system_id/component_id：本机 MAVLink 身份； + /// target_system/target_component：飞控端身份。 + MavlinkCommandGateway(ICommandTransport& transport, std::uint8_t system_id, + std::uint8_t component_id, std::uint8_t target_system, + std::uint8_t target_component); + + /// 速度与航向意图 → SET_POSITION_TARGET_LOCAL_NED（ID 84，速度模式 type_mask）。 + /// 掩码选择：`yaw_rate_dps` 非零 → 角速率掩码；`yaw_deg` 非零 → 航向角掩码； + /// 两者均为零 → 角速率掩码配零角速率（保持当前航向，避免误发绝对航向 0）。 + bool SendVelocityHeading(const ControlOutput& output, std::uint32_t time_boot_ms); + + /// 零速刹车 → ID 84 全零速度（判丢等场景，§6.4）。 + bool SendBrakeHover(std::uint32_t time_boot_ms); + + // 固定缓冲尺寸（.cpp 中与实际类型大小做 static_assert 校验）： + static constexpr std::size_t kMsgBufferSize = 512; // ≥ sizeof(mavlink_message_t) + static constexpr std::size_t kTxBufferSize = 512; // ≥ MAVLINK_MAX_PACKET_LEN + +private: + ICommandTransport& transport_; // 字节流传输实现（注入） + + // MAVLink 身份 + std::uint8_t system_id_; + std::uint8_t component_id_; + std::uint8_t target_system_; + std::uint8_t target_component_; + + // 固定缓冲：热路径零堆分配（对齐 8 满足 mavlink_message_t 对齐要求） + alignas(8) std::uint8_t msg_storage_[kMsgBufferSize]; // mavlink_message_t 承载 + std::uint8_t tx_buffer_[kTxBufferSize]; // 发送字节缓冲 +}; + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/include/visual_track_control/pid_controller.h b/reference-c++/visual_track_control/include/visual_track_control/pid_controller.h new file mode 100644 index 0000000..d3a1415 --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/pid_controller.h @@ -0,0 +1,59 @@ +#pragma once + +// ============================================================================= +// pid_controller.h —— 位置式 PID 控制器（纯算法构件） +// +// 对应规格：docs/物理追踪思路.md +// §6.2 距离控制律：前向速度指令 = PID(Δd)，Δd = d − d_exp +// §8 PID 参数（Kp/Ki/Kd、积分限幅、微分滤波）全部待定/配置化 +// +// 特性（热路径零分配、无虚函数、无锁）： +// - 位置式输出：out = Kp·e + Ki·∫e + Kd·d(e)/dt； +// - 积分钳位：积分项限幅在 ±integral_limit； +// - anti-windup（条件积分）：输出饱和且本拍积分增量仍推深饱和时冻结积分， +// 误差反号后允许积分回卷，防止积分饱和与残留； +// - 微分一阶低通：derivative_filter_coef ∈ [0,1)，0 = 不滤波； +// - ResetIntegral() / Reset() 均 O(1)（Coast 清积分，§6.4）。 +// ============================================================================= + +namespace drone::vtc { + +/// PID 参数（全部来自配置，代码不写死任何数值） +struct PidParams { + double kp = 0.0; // 比例增益 + double ki = 0.0; // 积分增益 + double kd = 0.0; // 微分增益 + double integral_limit = 0.0; // 积分项限幅（<=0 表示不限幅） + double derivative_filter_coef = 0.0; // 微分一阶低通系数 ∈ [0,1)：越大滤波越强 + double output_limit = 0.0; // 输出限幅（<=0 表示不限幅） +}; + +/// 位置式 PID 控制器 +class PidController { +public: + explicit PidController(const PidParams& params); + + /// 单步更新：输入误差与采样间隔（秒），返回限幅后的输出。 + /// dt_s <= 0 时保持上次输出（异常节拍保护）。 + double Update(double error, double dt_s); + + /// 清空积分项（§6.4：Coast 时防止积分饱和）；保留微分历史与上次输出 + void ResetIntegral(); + + /// 清空全部内部状态（积分、微分历史、上次输出） + void Reset(); + + /// 上次输出（只读） + double LastOutput() const { return last_output_; } + +private: + PidParams params_; + + double integral_ = 0.0; // 积分累计（已钳位） + double prev_error_ = 0.0; // 上次误差（微分用） + double filtered_derivative_ = 0.0; // 低通后的微分值 + double last_output_ = 0.0; // 上次输出 + bool has_prev_error_ = false; // 是否存在有效微分历史 +}; + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/include/visual_track_control/rate_limiter.h b/reference-c++/visual_track_control/include/visual_track_control/rate_limiter.h new file mode 100644 index 0000000..0ae8c98 --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/rate_limiter.h @@ -0,0 +1,56 @@ +#pragma once + +// ============================================================================= +// rate_limiter.h —— 限幅与变化率限制（纯算法构件，header-only） +// +// 对应规格：docs/物理追踪思路.md +// §8 所有阈值集中配置：速度/加速度/方向变化率必须限幅（FR-033） +// S6 首条视觉速度设定值必须经过限幅，避免位置→速度切换产生突变 +// （状态机设计.md S6） +// +// 特性（热路径零分配、无虚函数、无锁）： +// - Clamp()：静态限幅； +// - SlewRateLimiter：保存上次输出，按配置变化率限幅；初值置 0 时 +// 天然实现"首条指令突变抑制"（S6）。 +// ============================================================================= + +#include + +namespace drone::vtc { + +/// 限幅：将 v 钳制在 [lo, hi]（调用方保证 lo <= hi） +inline double Clamp(double v, double lo, double hi) { + return std::min(std::max(v, lo), hi); +} + +/// 变化率限幅器：限制输出每秒最大变化量（单位由调用方决定， +/// 如 度/秒^2 用于航向、米/秒^2 用于速度） +class SlewRateLimiter { +public: + /// rate_limit：每秒最大变化量（<=0 表示冻结输出）；initial：初始输出值 + explicit SlewRateLimiter(double rate_limit, double initial = 0.0) + : rate_limit_(rate_limit), output_(initial) {} + + /// 朝 target 推进一个节拍：单步变化不超过 rate_limit·dt_s。 + /// dt_s <= 0 或 rate_limit <= 0 时保持当前输出（异常节拍保护）。 + double Limit(double target, double dt_s) { + if (dt_s <= 0.0 || rate_limit_ <= 0.0) { + return output_; + } + const double max_step = rate_limit_ * dt_s; + output_ = Clamp(target, output_ - max_step, output_ + max_step); + return output_; + } + + /// 复位输出值（刹车/重置时归零，保证恢复跟踪后指令从零渐变） + void Reset(double value = 0.0) { output_ = value; } + + /// 上次输出（只读） + double LastOutput() const { return output_; } + +private: + double rate_limit_; // 每秒最大变化量 + double output_; // 当前输出 +}; + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/include/visual_track_control/staleness_checker.h b/reference-c++/visual_track_control/include/visual_track_control/staleness_checker.h new file mode 100644 index 0000000..e6a6571 --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/staleness_checker.h @@ -0,0 +1,38 @@ +#pragma once + +// ============================================================================= +// staleness_checker.h —— 输入超龄判定（单调毫秒时钟） +// +// 对应规格：docs/物理追踪思路.md +// §7.1 超龄数据只能触发降级或告警，不能继续生成正常控制指令； +// 超龄判定使用取样时间 + valid_for_ms（MessageHeader 语义）。 +// +// 特性：无状态纯函数封装（零分配、无虚函数、无锁）。 +// ============================================================================= + +#include + +namespace drone::vtc { + +/// 输入超龄判定器（无状态，仅提供静态判定） +class StalenessChecker { +public: + /// sample_time_ms：取样单调时钟毫秒（<=0 视为从未取样，直接判超龄）； + /// now_ms：当前单调时钟毫秒； + /// valid_for_ms：有效期（毫秒，配置给定）。 + /// 返回 true = 已超龄。 + static bool IsStale(std::int64_t sample_time_ms, std::int64_t now_ms, + std::int64_t valid_for_ms) { + if (sample_time_ms <= 0) { + return true; // 从未取样：视为超龄（触发降级） + } + if (now_ms < sample_time_ms) { + // 时间戳不一致（时钟源错配/打戳错误）：年龄不可信。 + // 单一单调时钟下不应发生；宁可降级也不信任不可核实的取样。 + return true; + } + return (now_ms - sample_time_ms) > valid_for_ms; + } +}; + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/include/visual_track_control/tracking_control_law.h b/reference-c++/visual_track_control/include/visual_track_control/tracking_control_law.h new file mode 100644 index 0000000..1ccd940 --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/tracking_control_law.h @@ -0,0 +1,63 @@ +#pragma once + +// ============================================================================= +// tracking_control_law.h —— 视觉跟踪主控律（TrackingControlLaw） +// +// 对应规格：docs/物理追踪思路.md +// §4 水平航向控制（增益 → slew → 角速度限幅，位置式/速率式按配置） +// §5 垂直/高度控制（统一速度支路：α → 垂直速度，原双支路已合并） +// §6 距离控制（PID(Δd) → 前向速度；未关联按配置降级，§6.3） +// §6.4 Coast 清积分、判丢零速刹车 +// §7.1 超龄判定（姿态/视觉超龄 → 降级保持） +// §8 统一限幅 +// §2.3 输出端随机体航向把机体系速度旋转为局部 NED +// +// 线程模型：单线程调用（状态机与控制线程），成员无锁、无堆对象； +// 日志仅状态切换点（刹车进出、降级切换），高频热路径不打日志。 +// ============================================================================= + +#include + +#include "visual_track_control/line_of_sight.h" +#include "visual_track_control/pid_controller.h" +#include "visual_track_control/rate_limiter.h" +#include "visual_track_control/vtc_config.h" +#include "visual_track_control/vtc_types.h" + +namespace drone::vtc { + +/// 视觉跟踪主控律：消费控制快照，输出限幅后的控制意图 +class TrackingControlLaw { +public: + /// cfg：启动期加载的全量配置（值拷贝持有，运行期不再触碰配置源） + explicit TrackingControlLaw(const TrackingConfig& cfg); + + /// 单周期控制计算（§7.1 固定周期持续运行）。 + /// now_ms：当前单调时钟毫秒；内部据此推算 dt 并做超龄判定。 + /// 流程：①超龄判定 → ②判丢刹车 → ③Coast 清积分 → ④像素→视线角 + /// → ⑤水平航向 → ⑥垂直速度 → ⑦距离通道 → ⑧机体系→NED 旋转输出 + ControlOutput Update(const ControlSnapshot& snapshot, std::int64_t now_ms); + + /// 停止视觉闭环时清全部状态（积分、限幅器历史、节拍时钟，§6.4） + void Reset(); + +private: + TrackingConfig cfg_; // 配置值拷贝（运行期只读） + + // ---- 预计算构件（构造期完成，热路径零分配） ---- + PixelToAngle pixel_to_beta_; // 水平：x_offset → β（§4.1） + PixelToAngle pixel_to_alpha_; // 垂直：y_offset → α（§5.1） + PidController distance_pid_; // 距离通道 PID（§6.2） + + // ---- 变化率限幅器（§8 / S6 首条指令限幅） ---- + SlewRateLimiter yaw_deg_limiter_; // 位置式航向变化率（度/秒^2） + SlewRateLimiter yaw_rate_limiter_; // 速率式角速度变化率（度/秒^2） + SlewRateLimiter vx_limiter_; // 前向加速度限幅（米/秒^2） + SlewRateLimiter vz_limiter_; // 垂直加速度限幅（米/秒^2） + + // ---- 节拍与状态（仅打日志用，不参与数学） ---- + std::int64_t last_now_ms_ = 0; // 上次计算时刻 + ControlMode last_mode_ = ControlMode::kDegradedHold; // 上次输出模式（状态切换日志） +}; + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/include/visual_track_control/vtc_config.h b/reference-c++/visual_track_control/include/visual_track_control/vtc_config.h new file mode 100644 index 0000000..820a24c --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/vtc_config.h @@ -0,0 +1,113 @@ +#pragma once + +// ============================================================================= +// vtc_config.h —— 视觉跟踪控制律配置（TrackingConfig + LoadFromJson） +// +// 对应规格：docs/物理追踪思路.md +// §8 限幅、阈值与配置化（所有阈值集中配置，不得散落在代码中， +// 需求分析.md NFR-007） +// §4 水平航向控制（模式、增益、限幅） +// §5 垂直/高度控制（垂直速度增益与限幅；原双支路已统一为速度支路） +// §6 距离控制（射程、PID、限幅、无距离降级动作） +// §7 控制周期与时序（频率、各输入超龄阈值） +// +// 设计约束： +// - 全部数值待定：本结构默认值与示例配置均为占位，标定前仅限回放验证； +// - LoadFromJson 启动期一次性解析：缺字段/类型错/非法值抛 +// std::runtime_error 并携带字段名（初始化抛异常规范）； +// - 热路径绝不触碰 JSON 对象，不做热更新（调参重启即可）。 +// ============================================================================= + +#include +#include + +namespace drone::vtc { + +/// 航向通道模式（§4.2：位置式/速率式按 PX4 1.11.3 Offboard 实测选择） +/// +/// 默认速率式：yaw_rate 指令天然是相对量，无坐标系歧义。位置式输出为 +/// 绝对 NED 航向（当前航向 + 限幅修正量合成），依赖新鲜自机姿态； +/// 姿态接线未完成前不得用于实飞。 +enum class HeadingMode : std::uint8_t { + kRate, // 速率式（默认）：β → 偏航角速度，输出 yaw_rate_dps + kPosition, // 位置式：β → 修正量，与当前航向合成绝对航向输出 yaw_deg +}; + +/// 雷达距离不可用（未关联/超龄）时的距离通道降级动作（§6.3，具体动作待定） +enum class NoDistanceAction : std::uint8_t { + kHold, // 保持：前向速度指令清零（安全占位语义） + kSlowApproach, // 限速接近：按 no_distance_approach_limit_mps 缓速接近 + kExit, // 退出近距操作：输出降级保持（kDegradedHold） +}; + +/// control 组：控制周期与各输入超龄阈值（§7） +struct ControlGroupConfig { + double frequency_hz = 20.0; // 控制频率（Hz，§7.1 待定） + std::int64_t visual_stale_ms = 200; // 视觉追踪结果超龄阈值（毫秒） + std::int64_t radar_stale_ms = 500; // 雷达距离超龄阈值（毫秒） + std::int64_t attitude_stale_ms = 300; // 自机姿态超龄阈值（毫秒） +}; + +/// camera 组：画面与视场角（§3.2、§4.1、§5.1；内参/畸变未标定，占位） +struct CameraGroupConfig { + int image_width = 1920; // 画面宽度 W（像素） + int image_height = 1080; // 画面高度 H（像素） + double fov_h_deg = 60.0; // 水平视场角 θ_h（度，仅水平，§4.1） + double fov_v_deg = 35.0; // 垂直视场角 θ_v（度，§5.1） +}; + +/// heading 组：水平航向通道（§4） +struct HeadingGroupConfig { + HeadingMode mode = HeadingMode::kRate; // 默认速率式（§4.2，见 HeadingMode 注释） + double gain = 1.0; // 增益：视线角(度) → 航向指令（无量纲占位） + double yaw_rate_limit_dps = 30.0; // 偏航角速度上限（度/秒，FR-033） + double yaw_slew_limit_dps2 = 60.0; // 方向变化率上限（度/秒^2，FR-033、S6 首条限幅） +}; + +/// vertical 组：垂直/高度控制（§5） +/// +/// 规格 §5.2 原设"小误差姿态微调 / 大误差垂直速度"双支路，实测评审后 +/// 统一为速度支路（见 ControlMode 注释）：切换阈值与姿态微调参数随之移除。 +/// 小误差经 vz_gain 线性映射为小垂直速度，行为连续且无模式翻转。 +struct VerticalGroupConfig { + double vz_gain_mps_per_deg = 0.1; // 垂直速度增益：α(度) → 垂直速度(米/秒) + double vz_limit_mps = 2.0; // 垂直速度限幅（米/秒） +}; + +/// distance 组：距离通道（§6） +struct DistanceGroupConfig { + double d_exp_m = 10.0; // 射程 d_exp（米，由拦截设备接口给出，§6.1 待定） + double kp = 0.0; // PID 比例（§6.2 待定） + double ki = 0.0; // PID 积分 + double kd = 0.0; // PID 微分 + double integral_limit = 0.0; // 积分项限幅 + double derivative_filter_coef = 0.0; // 微分一阶低通系数 ∈ [0,1)：0=不滤波 + double approach_velocity_limit_mps = 3.0; // 接近速度限幅（米/秒，§8） + double retreat_velocity_limit_mps = 1.0; // 后退速度限幅（米/秒，§8） + NoDistanceAction no_distance_action = NoDistanceAction::kHold; // 无距离降级（§6.3） + double no_distance_approach_limit_mps = 0.5; // kSlowApproach 限速值（米/秒） +}; + +/// accel_limit 组：各轴加速度限幅（§8，待定） +struct AccelLimitConfig { + double ax_mps2 = 2.0; // 前向加速度限幅（米/秒^2） + double ay_mps2 = 2.0; // 侧向加速度限幅（米/秒^2，预留） + double az_mps2 = 1.5; // 垂直加速度限幅（米/秒^2） +}; + +/// 视觉跟踪控制律全量配置（§8：所有阈值集中于此） +struct TrackingConfig { + ControlGroupConfig control; + CameraGroupConfig camera; + HeadingGroupConfig heading; + VerticalGroupConfig vertical; + DistanceGroupConfig distance; + AccelLimitConfig accel_limit; + + /// 从 JSON 文件一次性加载配置（启动期调用）。 + /// 缺字段 / 类型错误 / 非法取值均抛 std::runtime_error， + /// 异常消息携带字段名（如 "heading.gain"）。 + static TrackingConfig LoadFromJson(const std::string& path); +}; + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/include/visual_track_control/vtc_types.h b/reference-c++/visual_track_control/include/visual_track_control/vtc_types.h new file mode 100644 index 0000000..a156d75 --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/vtc_types.h @@ -0,0 +1,102 @@ +#pragma once + +// ============================================================================= +// vtc_types.h —— 视觉跟踪控制律的中立输入/输出结构 +// +// 对应规格：docs/物理追踪思路.md +// §2 输入输出与数据流（控制意图枚举语义） +// §6.4 与追踪库的行为衔接（tracked / is_predicted 语义） +// §7.2 与 target_tracker 的衔接（TrackResult 无时间戳，由调用方在 +// LastResult() 瞬间打戳） +// +// 设计约束： +// - ControlSnapshot / ControlOutput 均为纯值类型（POD 风格）， +// 控制律热路径零堆分配； +// - 本库不依赖主工程 include/ 的任何类型，独立可编译； +// - 雷达距离必须带 associated_to_target 关联标志：未关联不得当作 +// 目标距离（§6.3、需求分析.md FR-041）。 +// ============================================================================= + +#include + +#include + +namespace drone::vtc { + +/// 控制输出模式（对应物理追踪思路 §2.2 的控制意图语义） +/// +/// 注意：规格 §5.2 原设"垂直小误差姿态微调支路"（kAttitudeTrim → +/// SET_ATTITUDE_TARGET），实测评审后取消——姿态微调支路会把整个输出切成 +/// 姿态消息，导致同周期的距离通道前向速度被丢弃（逼近停滞），且双消息 +/// 类型逐拍翻转会扰动 PX4 offboard。现垂直通道一律走速度支路（小误差 +/// 对应小 vz），全周期单一 offboard 消息。 +enum class ControlMode : std::uint8_t { + kVelocityHeading, // 速度与航向意图：速度分量 + yaw（§4.2、§5、§6.2 主输出） + kBrakeHover, // 零速刹车/悬停：判丢等场景（§6.4、§9.1） + kDegradedHold, // 降级保持：超龄/退出近距操作，禁止正常控制（§7.1、§9.2） +}; + +/// 降级原因（仅 kDegradedHold 模式有意义） +enum class DegradedReason : std::uint8_t { + kNone, // 未降级 + kAttitudeStale, // PX4 姿态过期：禁止空间转换和控制（§9.2） + kVisionStale, // 视觉追踪结果超龄：不得继续生成正常指令（§7.1） + kNoDistanceExit, // 无距离关联且配置为退出近距操作（§6.3） +}; + +/// 控制律输入快照：调用方在同一控制周期内采集并打戳 +/// +/// 注意：TrackResult 本身不含时间戳（§7.2），调用方必须在读取 +/// LastResult() 的瞬间用单调时钟打戳填入 track_time_ms； +/// 超龄判定一律使用"取样时间 + valid_for_ms"（§7.1）。 +struct ControlSnapshot { + // ---- 视觉追踪结果（控制律主输入） ---- + drone::tracker::TrackResult track; // 追踪库结果（值拷贝） + std::int64_t track_time_ms = 0; // 取样单调时钟（毫秒），<=0 视为从未取样 + + // ---- 雷达距离（§6.3：未关联仅为前向安全距离，不得当目标距离） ---- + double radar_distance_m = 0.0; // 机头前向单点距离（米） + bool associated_to_target = false; // 相机—雷达空间关联成立标志 + std::int64_t radar_time_ms = 0; // 取样单调时钟（毫秒） + + // ---- 自机姿态/速度 ---- + // 姿态过期 → 禁止控制（§9.2）。`yaw_rad` 参与输出端机体系→NED 旋转 + // （§2.3）与位置式绝对航向合成；其余字段预留，接入主工程时由 + // FlightStateSnapshot 填充（机体补偿见 §4.1）。 + bool attitude_present = false; // 姿态源当前是否可用 + std::int64_t attitude_time_ms = 0; // 姿态取样单调时钟（毫秒） + double roll_rad = 0.0; // 横滚（弧度，FRD） + double pitch_rad = 0.0; // 俯仰（弧度，FRD） + double yaw_rad = 0.0; // 航向（弧度，NED） + double yaw_rate_radps = 0.0; // 偏航角速度（弧度/秒，机体补偿预留，§4.1） + double velocity_x_mps = 0.0; // 自机速度分量（预留） + double velocity_y_mps = 0.0; + double velocity_z_mps = 0.0; +}; + +/// 控制律输出：限幅后的控制意图（§8：所有输出必须经过限幅） +/// +/// 坐标语义：速度分量在机体系（前/右/下，§2.3）内计算与限幅，输出前 +/// 已随自机航向（ControlSnapshot.yaw_rad）旋转为局部 NED（北/东/地）， +/// 与网关使用的 MAV_FRAME_LOCAL_NED 一致；因此旋转后 vy_mps 可能非零 +/// （机体系侧向恒为 0，旋转不改变合速度大小，限幅在旋转前完成）。 +struct ControlOutput { + ControlMode mode = ControlMode::kDegradedHold; // 输出模式 + bool valid = false; // 指令是否可用于发送 + + // ---- 速度分量（局部 NED：北/东/地，米/秒；由机体系随航向旋转得到） ---- + double vx_mps = 0.0; // 北向（距离通道输出经旋转，§6.2） + double vy_mps = 0.0; // 东向（机体系恒 0，航向旋转后可非零） + double vz_mps = 0.0; // 垂直（NED 下正，垂直速度支路输出，§5.2） + + // ---- 航向通道（§4.2：位置式输出 yaw_deg，速率式输出 yaw_rate_dps） ---- + double yaw_deg = 0.0; // 目标绝对航向（度，NED，右转为正；仅位置式， + // 由当前航向 + 限幅修正量合成并回绕 [-180,180)） + double yaw_rate_dps = 0.0; // 偏航角速度指令（度/秒；速率式为默认，天然相对量） + + // ---- 状态标注 ---- + DegradedReason degraded_reason = DegradedReason::kNone; // 降级原因 + bool coast_active = false; // 本周期处于 Coast（is_predicted==true，§6.4） +}; + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/src/line_of_sight.cpp b/reference-c++/visual_track_control/src/line_of_sight.cpp new file mode 100644 index 0000000..671a0bb --- /dev/null +++ b/reference-c++/visual_track_control/src/line_of_sight.cpp @@ -0,0 +1,42 @@ +// ============================================================================= +// line_of_sight.cpp —— 像素偏移 → 视线角实现（全程针孔模型） +// +// 对应规格：docs/物理追踪思路.md §4.1（水平 β）、§5.1（垂直 α）： +// 针孔模型：angle = atan( offset · 2·tan(θ/2) / size ) +// +// 原设计的线性近似双路切换已取消：线性式与针孔式在切换边界两侧不连续 +// （θ=60°、边界比例 0.2 处约 8% 角度台阶），会向控制环路注入突变； +// atan 代价可忽略，全程采用精确针孔式。 +// +// 构造期预计算针孔系数，Convert 热路径仅一次乘加 + atan。 +// ============================================================================= + +#include "visual_track_control/line_of_sight.h" + +#include +#include + +namespace drone::vtc { + +namespace { +constexpr double kPi = 3.14159265358979323846; +} + +PixelToAngle::PixelToAngle(double size_px, double fov_rad) { + // 构造期校验：非法参数直接抛异常（初始化抛异常规范） + if (!(size_px > 0.0)) { + throw std::invalid_argument("PixelToAngle: size_px 必须为正"); + } + if (!(fov_rad > 0.0 && fov_rad < kPi)) { + throw std::invalid_argument("PixelToAngle: fov_rad 必须在 (0, π) 内"); + } + + // 预计算针孔系数（§4.1 公式） + pinhole_scale_ = 2.0 * std::tan(fov_rad / 2.0) / size_px; // 2·tan(θ/2) / size +} + +double PixelToAngle::Convert(double offset_px) const { + return std::atan(offset_px * pinhole_scale_); // 针孔模型，全偏移范围精确 +} + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/src/mavlink_command_gateway.cpp b/reference-c++/visual_track_control/src/mavlink_command_gateway.cpp new file mode 100644 index 0000000..202287a --- /dev/null +++ b/reference-c++/visual_track_control/src/mavlink_command_gateway.cpp @@ -0,0 +1,119 @@ +// ============================================================================= +// mavlink_command_gateway.cpp —— MAVLink 命令打包网关实现 +// +// 对应规格：docs/物理追踪思路.md §2.2 +// kVelocityHeading → SET_POSITION_TARGET_LOCAL_NED（ID 84，速度模式） +// kBrakeHover → ID 84 全零速度 +// （原 kAttitudeTrim → ID 82 已随姿态微调支路一并移除，见 vtc_types.h） +// +// 关键构建约束：本文件是全工程唯一包含 common/mavlink.h 的 TU。 +// MAVLINK_HELPER 默认空宏（mavlink_helpers.h），多 TU 包含会多重定义。 +// +// 打包策略： +// - 使用 mavlink_msg_*_pack（无通道状态、纯静态内联、可重入）， +// 不使用 _pack_chan 通道 API； +// - mavlink_msg_to_send_buffer 输出字节流 → ICommandTransport::Send； +// - 消息/发送缓冲为成员固定持有，热路径零堆分配。 +// +// 坐标系与语义说明（待实测确认）： +// - coordinate_frame 取 MAV_FRAME_LOCAL_NED；速度分量由主控律在输出端 +// 随机体航向旋转为局部 NED（北/东/地，物理追踪思路 §2.3），本文件 +// 直接填充不再做坐标变换； +// - 速度模式 type_mask 需以 PX4 1.11.3 Offboard 实测为准 +// （物理追踪思路 §10.1）。 +// ============================================================================= + +#include "visual_track_control/mavlink_command_gateway.h" + +#include + +#include "common/mavlink.h" // 全工程唯一包含点（见文件头构建约束） + +namespace drone::vtc { +namespace { + +constexpr double kPi = 3.14159265358979323846; +constexpr double kDegToRad = kPi / 180.0; + +// ---- SET_POSITION_TARGET_LOCAL_NED（ID 84）type_mask 常量 ---- +// 掩码位语义：置位 = 忽略该字段。 +// 基础位：忽略位置 x/y/z（bit0-2）与加速度 afx/afy/afz（bit6-8）= 0x01C7。 +constexpr std::uint16_t kMaskVelocityBase = 0x01C7; +// 速度 + 航向角模式：忽略 yaw_rate（bit11） +constexpr std::uint16_t kMaskVelocityYaw = kMaskVelocityBase | 0x0800; +// 速度 + 偏航角速率模式：忽略 yaw（bit10） +constexpr std::uint16_t kMaskVelocityYawRate = kMaskVelocityBase | 0x0400; +// 刹车模式：速度全零，yaw 与 yaw_rate 均忽略（bit10、bit11） +constexpr std::uint16_t kMaskBrake = kMaskVelocityBase | 0x0C00; + +/// 固定存储解释为消息结构（打包函数只写入字节，不读取旧值） +inline mavlink_message_t* AsMessage(std::uint8_t* storage) { + return reinterpret_cast(storage); +} + +} // namespace + +// 固定缓冲尺寸校验：编译期保证存储足够（头文件不暴露 mavlink 类型） +static_assert(sizeof(mavlink_message_t) <= MavlinkCommandGateway::kMsgBufferSize, + "msg_storage_ 不足以容纳 mavlink_message_t"); +static_assert(MAVLINK_MAX_PACKET_LEN <= MavlinkCommandGateway::kTxBufferSize, + "tx_buffer_ 不足以容纳最大 MAVLink 数据包"); + +MavlinkCommandGateway::MavlinkCommandGateway(ICommandTransport& transport, + std::uint8_t system_id, + std::uint8_t component_id, + std::uint8_t target_system, + std::uint8_t target_component) + : transport_(transport), + system_id_(system_id), + component_id_(component_id), + target_system_(target_system), + target_component_(target_component) {} + +bool MavlinkCommandGateway::SendVelocityHeading(const ControlOutput& output, + std::uint32_t time_boot_ms) { + auto* msg = AsMessage(msg_storage_); + + // 掩码三分支（A2 修复）： + // 角速率非零 → 速率掩码（速率式控制律常态，§4.2）； + // 航向角非零 → 航向掩码（位置式控制律）； + // 两者均零 → 速率掩码配零角速率：保持当前航向。 + // 旧实现把"零角速率"落到航向掩码，等于误发绝对航向 0（指向正北）。 + const bool has_yaw_rate = std::fabs(output.yaw_rate_dps) > 1.0e-9; + const bool has_yaw = std::fabs(output.yaw_deg) > 1.0e-9; + const std::uint16_t type_mask = + (!has_yaw_rate && has_yaw) ? kMaskVelocityYaw : kMaskVelocityYawRate; + + mavlink_msg_set_position_target_local_ned_pack( + system_id_, component_id_, msg, + time_boot_ms, target_system_, target_component_, + MAV_FRAME_LOCAL_NED, type_mask, + 0.0F, 0.0F, 0.0F, // 位置字段：忽略 + static_cast(output.vx_mps), static_cast(output.vy_mps), + static_cast(output.vz_mps), + 0.0F, 0.0F, 0.0F, // 加速度字段：忽略 + static_cast(output.yaw_deg * kDegToRad), + static_cast(output.yaw_rate_dps * kDegToRad)); + + const std::uint16_t len = mavlink_msg_to_send_buffer(tx_buffer_, msg); + return transport_.Send(tx_buffer_, len); +} + +bool MavlinkCommandGateway::SendBrakeHover(std::uint32_t time_boot_ms) { + auto* msg = AsMessage(msg_storage_); + + // 判丢零速刹车（§6.4）：速度全零，位置/加速度/航向字段全部忽略 + mavlink_msg_set_position_target_local_ned_pack( + system_id_, component_id_, msg, + time_boot_ms, target_system_, target_component_, + MAV_FRAME_LOCAL_NED, kMaskBrake, + 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F); + + const std::uint16_t len = mavlink_msg_to_send_buffer(tx_buffer_, msg); + return transport_.Send(tx_buffer_, len); +} + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/src/pid_controller.cpp b/reference-c++/visual_track_control/src/pid_controller.cpp new file mode 100644 index 0000000..687e421 --- /dev/null +++ b/reference-c++/visual_track_control/src/pid_controller.cpp @@ -0,0 +1,81 @@ +// ============================================================================= +// pid_controller.cpp —— 位置式 PID 控制器实现 +// +// 对应规格：docs/物理追踪思路.md +// §6.2 前向速度指令 = PID(Δd) +// §6.4 Coast 时积分项清空（ResetIntegral） +// +// anti-windup 策略（条件积分）：输出达到饱和限幅且本拍积分增量仍在 +// 推深饱和时，冻结本步积分累加（不采纳候选积分值），防止积分饱和； +// 误差反号后增量反向、允许积分回卷，避免饱和残留。 +// ============================================================================= + +#include "visual_track_control/pid_controller.h" + +#include + +#include "visual_track_control/rate_limiter.h" + +namespace drone::vtc { + +PidController::PidController(const PidParams& params) : params_(params) {} + +double PidController::Update(double error, double dt_s) { + if (dt_s <= 0.0) { + return last_output_; // 异常节拍保护：保持上次输出 + } + + // ---- 积分：候选值先钳位（积分限幅） ---- + double integral_candidate = integral_ + error * dt_s; + if (params_.integral_limit > 0.0) { + integral_candidate = + Clamp(integral_candidate, -params_.integral_limit, params_.integral_limit); + } + + // ---- 微分：一阶低通滤波（系数 ∈ [0,1)，0 = 不滤波） ---- + double derivative = 0.0; + if (has_prev_error_) { + derivative = (error - prev_error_) / dt_s; + const double coef = params_.derivative_filter_coef; + derivative = coef * filtered_derivative_ + (1.0 - coef) * derivative; + } + filtered_derivative_ = derivative; + prev_error_ = error; + has_prev_error_ = true; + + // ---- 位置式合成 ---- + const double integral_term = params_.ki * integral_candidate; + const double raw = params_.kp * error + integral_term + params_.kd * derivative; + + // ---- anti-windup（条件积分，C1 修复） ---- + // 冻结判据基于"本拍积分增量方向"（error·dt）：输出饱和且本拍增量仍 + // 在推深饱和时才冻结。不采用旧实现"积分项与输出同向"判据（误差反号 + // 后无法回卷），也不用 raw·error（大 kp 项会掩盖饱和方向、拖延回卷）。 + const bool saturated = params_.output_limit > 0.0 && std::fabs(raw) > params_.output_limit; + const bool increment_pushes_deeper = (error * dt_s) * raw > 0.0; + if (!(saturated && increment_pushes_deeper)) { + integral_ = integral_candidate; + } + + // ---- 输出限幅 ---- + if (params_.output_limit > 0.0) { + last_output_ = Clamp(raw, -params_.output_limit, params_.output_limit); + } else { + last_output_ = raw; + } + return last_output_; +} + +void PidController::ResetIntegral() { + integral_ = 0.0; // §6.4：Coast 清积分，防积分饱和 +} + +void PidController::Reset() { + integral_ = 0.0; + prev_error_ = 0.0; + filtered_derivative_ = 0.0; + last_output_ = 0.0; + has_prev_error_ = false; +} + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/src/tracking_control_law.cpp b/reference-c++/visual_track_control/src/tracking_control_law.cpp new file mode 100644 index 0000000..c5890c9 --- /dev/null +++ b/reference-c++/visual_track_control/src/tracking_control_law.cpp @@ -0,0 +1,239 @@ +// ============================================================================= +// tracking_control_law.cpp —— 视觉跟踪主控律实现 +// +// 对应规格：docs/物理追踪思路.md +// §4.1/§4.2 水平航向控制：像素偏移 → β → 增益 → slew → 角速度限幅， +// 位置式输出 yaw_deg / 速率式输出 yaw_rate_dps（按配置） +// §5.1/§5.2 垂直控制：统一速度支路 α → 垂直速度设定值（原"小误差姿态 +// 微调/大误差垂直速度"双支路已合并：姿态微调会把输出切成姿态 +// 消息，丢弃同周期距离通道前向速度导致逼近停滞，详见 +// vtc_types.h ControlMode 注释） +// §6.2/§6.3 距离通道：Δd = d − d_exp → PID → 前向速度（限幅）； +// 未关联/超龄 → 按配置降级动作（hold / slow_approach / exit） +// §6.4 Coast（is_predicted）清航向/距离积分；判丢（!tracked）零速刹车 +// §7.1 超龄数据只触发降级：姿态过期 → 禁止控制；视觉过期 → 降级保持 +// §8 所有输出统一限幅；首条指令经变化率限幅（S6） +// §2.3 输出端随机体航向把机体系速度旋转为局部 NED +// +// 日志纪律：仅状态切换点（刹车进出、降级切换）打印；热路径零日志、零分配。 +// ============================================================================= + +#include "visual_track_control/tracking_control_law.h" + +#include +#include + +#include + +#include "visual_track_control/frame_transform.h" +#include "visual_track_control/staleness_checker.h" + +namespace drone::vtc { +namespace { + +constexpr double kPi = 3.14159265358979323846; +constexpr double kRadToDeg = 180.0 / kPi; +constexpr double kDegToRad = kPi / 180.0; +constexpr double kMaxDtS = 0.5; // 单步 dt 上限（秒）：异常节拍保护 + +/// 角度回绕到 [-180, 180)（位置式绝对航向合成用） +double WrapDeg180(double deg) { + double d = std::fmod(deg + 180.0, 360.0); + if (d < 0.0) { + d += 360.0; + } + return d - 180.0; +} + +/// 控制模式可读名（仅状态切换日志使用） +const char* ModeName(ControlMode mode) { + switch (mode) { + case ControlMode::kVelocityHeading: return "速度航向"; + case ControlMode::kBrakeHover: return "零速刹车"; + case ControlMode::kDegradedHold: return "降级保持"; + } + return "未知"; +} + +/// 由距离配置构造 PID 参数（§6.2）；输出限幅取接近/后退限幅的较大者 +PidParams BuildDistancePidParams(const TrackingConfig& cfg) { + PidParams p; + p.kp = cfg.distance.kp; + p.ki = cfg.distance.ki; + p.kd = cfg.distance.kd; + p.integral_limit = cfg.distance.integral_limit; + p.derivative_filter_coef = cfg.distance.derivative_filter_coef; + p.output_limit = std::max(cfg.distance.approach_velocity_limit_mps, + cfg.distance.retreat_velocity_limit_mps); + return p; +} + +} // namespace + +TrackingControlLaw::TrackingControlLaw(const TrackingConfig& cfg) + : cfg_(cfg), + pixel_to_beta_(cfg.camera.image_width, cfg.camera.fov_h_deg * kDegToRad), + pixel_to_alpha_(cfg.camera.image_height, cfg.camera.fov_v_deg * kDegToRad), + distance_pid_(BuildDistancePidParams(cfg)), + yaw_deg_limiter_(cfg.heading.yaw_slew_limit_dps2), + yaw_rate_limiter_(cfg.heading.yaw_slew_limit_dps2), + vx_limiter_(cfg.accel_limit.ax_mps2), + vz_limiter_(cfg.accel_limit.az_mps2) {} + +ControlOutput TrackingControlLaw::Update(const ControlSnapshot& snapshot, + std::int64_t now_ms) { + // ---- 节拍推算（§7.1 固定周期；异常节拍用配置频率兜底） ---- + // 时钟回拨或 now_ms 未前进（同值）时不信任差值，回退配置频率： + // 单拍误差由限幅器与超龄判定兜底，不引入额外状态。 + double dt_s = 1.0 / cfg_.control.frequency_hz; + if (last_now_ms_ > 0 && now_ms > last_now_ms_) { + dt_s = std::min(static_cast(now_ms - last_now_ms_) / 1000.0, kMaxDtS); + } + last_now_ms_ = now_ms; + + ControlOutput out; + out.mode = ControlMode::kVelocityHeading; // 正常路径默认模式，分支可覆盖 + + // 状态切换日志辅助：仅模式变化时打印（刹车进出、降级切换等） + auto FinishAndLog = [&](const char* reason) { + if (out.mode != last_mode_) { + spdlog::info("[ControlLaw] 状态切换: {} -> {} ({})", + ModeName(last_mode_), ModeName(out.mode), reason); + last_mode_ = out.mode; + } + return out; + }; + + // ---- ① 超龄判定（§7.1 / §9.2） ---- + // 姿态过期或缺失：禁止空间转换和控制，立即退出正常控制（§9.2） + if (!snapshot.attitude_present || + StalenessChecker::IsStale(snapshot.attitude_time_ms, now_ms, + cfg_.control.attitude_stale_ms)) { + out.mode = ControlMode::kDegradedHold; + out.degraded_reason = DegradedReason::kAttitudeStale; + return FinishAndLog("姿态过期/缺失，禁止控制"); + } + // 视觉追踪结果超龄：不得继续生成正常指令（§7.1） + if (StalenessChecker::IsStale(snapshot.track_time_ms, now_ms, + cfg_.control.visual_stale_ms)) { + out.mode = ControlMode::kDegradedHold; + out.degraded_reason = DegradedReason::kVisionStale; + return FinishAndLog("视觉追踪结果超龄"); + } + + // ---- ② 判丢 → 零速刹车（§6.4：tracked == false 执行 kBrakeHover） ---- + if (!snapshot.track.tracked) { + if (last_mode_ != ControlMode::kBrakeHover) { + // 进入刹车：清积分与限幅器历史，恢复跟踪后指令从零渐变（S6 首条限幅） + distance_pid_.Reset(); + yaw_deg_limiter_.Reset(); + yaw_rate_limiter_.Reset(); + vx_limiter_.Reset(); + vz_limiter_.Reset(); + } + out.mode = ControlMode::kBrakeHover; // 速度分量保持默认全零 + out.valid = true; + return FinishAndLog("判丢，零速刹车"); + } + + // ---- ③ Coast：is_predicted == true → 清积分防饱和（§6.4） ---- + if (snapshot.track.is_predicted) { + distance_pid_.ResetIntegral(); + // 航向通道为纯比例增益映射（§4.2），无积分项，无需清理 + out.coast_active = true; + } + + // ---- ④ 像素偏移 → 视线角（§4.1 水平 β / §5.1 垂直 α） ---- + // x_offset = center.first − W/2（右正），y_offset = center.second − H/2（下正） + const double x_offset = + snapshot.track.center.first - cfg_.camera.image_width * 0.5; + const double y_offset = + snapshot.track.center.second - cfg_.camera.image_height * 0.5; + const double beta_deg = pixel_to_beta_.Convert(x_offset) * kRadToDeg; + const double alpha_deg = pixel_to_alpha_.Convert(y_offset) * kRadToDeg; + + // ---- ⑤ 水平航向通道（§4.2：增益 → slew → 角速度限幅） ---- + if (cfg_.heading.mode == HeadingMode::kRate) { + // 速率式：β → 偏航角速度指令，先变化率限幅再角速度限幅（FR-033） + const double target_rate = cfg_.heading.gain * beta_deg; + const double slewed = yaw_rate_limiter_.Limit(target_rate, dt_s); + out.yaw_rate_dps = Clamp(slewed, -cfg_.heading.yaw_rate_limit_dps, + cfg_.heading.yaw_rate_limit_dps); + } else { + // 位置式：β → 航向修正量，变化率限幅（方向变化率上限，FR-033）后 + // 与当前航向合成为绝对 NED 航向——ID 84 的 yaw 字段是绝对值， + // 直接发修正量会让机体转向"修正角"方向（A2 修复）。姿态已在 ① + // 通过超龄校验，yaw_rad 可用。 + const double target_yaw = cfg_.heading.gain * beta_deg; + const double correction_deg = yaw_deg_limiter_.Limit(target_yaw, dt_s); + out.yaw_deg = WrapDeg180(snapshot.yaw_rad * kRadToDeg + correction_deg); + } + + // ---- ⑥ 垂直速度通道（§5：统一速度支路） ---- + // 符号约定：α 下正（目标偏低）→ NED 下正 → vz 为正（下降趋近目标高度）。 + // 小误差自然映射为小垂直速度：无需单独的切换阈值与姿态微调支路， + // 输出模式保持 kVelocityHeading，距离通道前向速度不会被丢弃（A1 修复）。 + { + const double vz_target = cfg_.vertical.vz_gain_mps_per_deg * alpha_deg; + const double slewed = vz_limiter_.Limit(vz_target, dt_s); // 加速度限幅（§8） + out.vz_mps = Clamp(slewed, -cfg_.vertical.vz_limit_mps, + cfg_.vertical.vz_limit_mps); + } + + // ---- ⑦ 距离通道（§6.2 / §6.3） ---- + // 雷达距离不得直接当目标距离：仅关联成立且未超龄才可用（§6.3、FR-041） + const bool distance_available = + snapshot.associated_to_target && snapshot.radar_distance_m > 0.0 && + !StalenessChecker::IsStale(snapshot.radar_time_ms, now_ms, + cfg_.control.radar_stale_ms); + if (distance_available) { + // Δd = d − d_exp：d > d_exp 前向接近，d < d_exp 减速/后退（§6.2） + const double delta_d = snapshot.radar_distance_m - cfg_.distance.d_exp_m; + const double vx_raw = distance_pid_.Update(delta_d, dt_s); + const double slewed = vx_limiter_.Limit(vx_raw, dt_s); // 加速度限幅（§8） + out.vx_mps = Clamp(slewed, -cfg_.distance.retreat_velocity_limit_mps, + cfg_.distance.approach_velocity_limit_mps); + } else { + // 无可用目标距离：清积分防饱和，按配置降级动作（§6.3） + distance_pid_.ResetIntegral(); + switch (cfg_.distance.no_distance_action) { + case NoDistanceAction::kHold: + // 保持：前向指令清零（安全占位语义；接入主工程后可改为保持当前速度） + out.vx_mps = 0.0; + vx_limiter_.Reset(0.0); + break; + case NoDistanceAction::kSlowApproach: + // 限速接近：缓速前向，同样经加速度限幅 + out.vx_mps = vx_limiter_.Limit( + cfg_.distance.no_distance_approach_limit_mps, dt_s); + break; + case NoDistanceAction::kExit: + // 退出近距操作：整体降级保持 + out.mode = ControlMode::kDegradedHold; + out.degraded_reason = DegradedReason::kNoDistanceExit; + return FinishAndLog("距离未关联，退出近距操作"); + } + } + + // ---- ⑧ 机体系 → 局部 NED 旋转（§2.3，A4 修复） ---- + // 到此为止各通道限幅均在机体系完成（旋转为正交变换，合速度不变， + // 限幅约束不被破坏）；输出端随快照航向旋转为北/东/地，与网关的 + // MAV_FRAME_LOCAL_NED 一致。姿态已在 ① 通过超龄校验，yaw_rad 可用。 + RotateBodyToNed(out.vx_mps, out.vy_mps, out.vz_mps, snapshot.yaw_rad); + + out.valid = true; + return FinishAndLog(out.coast_active ? "Coast 预测跟踪" : "正常跟踪"); +} + +void TrackingControlLaw::Reset() { + // 停止视觉闭环时清全部状态（§6.4） + distance_pid_.Reset(); + yaw_deg_limiter_.Reset(); + yaw_rate_limiter_.Reset(); + vx_limiter_.Reset(); + vz_limiter_.Reset(); + last_now_ms_ = 0; + last_mode_ = ControlMode::kDegradedHold; +} + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/src/vtc_config.cpp b/reference-c++/visual_track_control/src/vtc_config.cpp new file mode 100644 index 0000000..629bc64 --- /dev/null +++ b/reference-c++/visual_track_control/src/vtc_config.cpp @@ -0,0 +1,225 @@ +// ============================================================================= +// vtc_config.cpp —— 配置加载器实现（启动期一次性解析） +// +// 对应规格：docs/物理追踪思路.md §8（阈值集中配置化，NFR-007）。 +// +// 实现要点： +// - 仅在启动期调用一次：缺字段 / 类型错误 / 非法取值抛 +// std::runtime_error，异常消息携带完整字段名（如 "heading.gain"）； +// - 热路径绝不触碰 JSON：解析结果拷贝进 TrackingConfig 后即释放； +// - 本文件是全库唯一包含 nlohmann/json 的 TU。 +// ============================================================================= + +#include "visual_track_control/vtc_config.h" + +#include +#include +#include + +#include + +namespace drone::vtc { +namespace { + +using Json = nlohmann::json; + +/// 读取并解析 JSON 文件；失败抛异常并携带路径 +Json ParseConfigFile(const std::string& path) { + std::ifstream ifs(path); + if (!ifs) { + throw std::runtime_error("配置文件打开失败: " + path); + } + try { + return Json::parse(ifs); + } catch (const std::exception& e) { + throw std::runtime_error("配置文件解析失败: " + path + " (" + e.what() + ")"); + } +} + +/// 取必需的对象分组；缺失或类型错抛异常 +const Json& RequireSection(const Json& root, const char* section) { + const auto it = root.find(section); + if (it == root.end() || !it->is_object()) { + throw std::runtime_error(std::string("配置缺失分组或类型错误: ") + section); + } + return *it; +} + +/// 取必需的数值字段；缺失/类型错抛异常并携带字段名 +double RequireNumber(const Json& section, const char* section_name, const char* key) { + const auto it = section.find(key); + if (it == section.end() || !it->is_number()) { + throw std::runtime_error(std::string("配置缺失字段或类型错误: ") + + section_name + "." + key); + } + return it->get(); +} + +/// 取必需的整数字段 +std::int64_t RequireInteger(const Json& section, const char* section_name, + const char* key) { + const auto it = section.find(key); + if (it == section.end() || !it->is_number_integer()) { + throw std::runtime_error(std::string("配置缺失字段或类型错误(需整数): ") + + section_name + "." + key); + } + return it->get(); +} + +/// 取必需的字符串字段 +std::string RequireString(const Json& section, const char* section_name, + const char* key) { + const auto it = section.find(key); + if (it == section.end() || !it->is_string()) { + throw std::runtime_error(std::string("配置缺失字段或类型错误(需字符串): ") + + section_name + "." + key); + } + return it->get(); +} + +/// 校验字段为正数；非法抛异常并携带字段名 +double RequirePositive(double value, const char* field_name) { + if (!(value > 0.0)) { + throw std::runtime_error(std::string("配置非法(必须为正): ") + field_name); + } + return value; +} + +/// 校验字段为非负数 +double RequireNonNegative(double value, const char* field_name) { + if (value < 0.0) { + throw std::runtime_error(std::string("配置非法(不得为负): ") + field_name); + } + return value; +} + +/// 校验字段在开区间 (lo, hi) +double RequireRange(double value, double lo, double hi, const char* field_name) { + if (!(value > lo && value < hi)) { + throw std::runtime_error(std::string("配置非法(超出范围): ") + field_name); + } + return value; +} + +ControlGroupConfig LoadControl(const Json& root) { + const Json& s = RequireSection(root, "control"); + ControlGroupConfig c; + c.frequency_hz = RequirePositive(RequireNumber(s, "control", "frequency_hz"), + "control.frequency_hz"); + c.visual_stale_ms = RequireInteger(s, "control", "visual_stale_ms"); + c.radar_stale_ms = RequireInteger(s, "control", "radar_stale_ms"); + c.attitude_stale_ms = RequireInteger(s, "control", "attitude_stale_ms"); + if (c.visual_stale_ms <= 0 || c.radar_stale_ms <= 0 || c.attitude_stale_ms <= 0) { + throw std::runtime_error("配置非法: control.*_stale_ms 必须为正"); + } + return c; +} + +CameraGroupConfig LoadCamera(const Json& root) { + const Json& s = RequireSection(root, "camera"); + CameraGroupConfig c; + c.image_width = static_cast( + RequirePositive(RequireInteger(s, "camera", "image_width"), "camera.image_width")); + c.image_height = static_cast( + RequirePositive(RequireInteger(s, "camera", "image_height"), "camera.image_height")); + c.fov_h_deg = RequireRange(RequireNumber(s, "camera", "fov_h_deg"), 0.0, 180.0, + "camera.fov_h_deg"); + c.fov_v_deg = RequireRange(RequireNumber(s, "camera", "fov_v_deg"), 0.0, 180.0, + "camera.fov_v_deg"); + return c; +} + +HeadingGroupConfig LoadHeading(const Json& root) { + const Json& s = RequireSection(root, "heading"); + HeadingGroupConfig h; + const std::string mode = RequireString(s, "heading", "mode"); + if (mode == "position") { + h.mode = HeadingMode::kPosition; + } else if (mode == "rate") { + h.mode = HeadingMode::kRate; + } else { + throw std::runtime_error("配置非法: heading.mode 仅支持 position / rate"); + } + h.gain = RequireNonNegative(RequireNumber(s, "heading", "gain"), "heading.gain"); + h.yaw_rate_limit_dps = RequirePositive( + RequireNumber(s, "heading", "yaw_rate_limit_dps"), "heading.yaw_rate_limit_dps"); + h.yaw_slew_limit_dps2 = RequirePositive( + RequireNumber(s, "heading", "yaw_slew_limit_dps2"), "heading.yaw_slew_limit_dps2"); + return h; +} + +VerticalGroupConfig LoadVertical(const Json& root) { + const Json& s = RequireSection(root, "vertical"); + VerticalGroupConfig v; + v.vz_gain_mps_per_deg = RequireNonNegative( + RequireNumber(s, "vertical", "vz_gain_mps_per_deg"), "vertical.vz_gain_mps_per_deg"); + v.vz_limit_mps = RequirePositive(RequireNumber(s, "vertical", "vz_limit_mps"), + "vertical.vz_limit_mps"); + return v; +} + +DistanceGroupConfig LoadDistance(const Json& root) { + const Json& s = RequireSection(root, "distance"); + DistanceGroupConfig d; + d.d_exp_m = RequirePositive(RequireNumber(s, "distance", "d_exp_m"), "distance.d_exp_m"); + d.kp = RequireNonNegative(RequireNumber(s, "distance", "kp"), "distance.kp"); + d.ki = RequireNonNegative(RequireNumber(s, "distance", "ki"), "distance.ki"); + d.kd = RequireNonNegative(RequireNumber(s, "distance", "kd"), "distance.kd"); + d.integral_limit = RequireNonNegative(RequireNumber(s, "distance", "integral_limit"), + "distance.integral_limit"); + d.derivative_filter_coef = RequireRange( + RequireNumber(s, "distance", "derivative_filter_coef"), -0.000001, 1.0, + "distance.derivative_filter_coef"); + if (d.derivative_filter_coef < 0.0) { + d.derivative_filter_coef = 0.0; // 容许 -0.0 + } + d.approach_velocity_limit_mps = RequirePositive( + RequireNumber(s, "distance", "approach_velocity_limit_mps"), + "distance.approach_velocity_limit_mps"); + d.retreat_velocity_limit_mps = RequirePositive( + RequireNumber(s, "distance", "retreat_velocity_limit_mps"), + "distance.retreat_velocity_limit_mps"); + const std::string action = RequireString(s, "distance", "no_distance_action"); + if (action == "hold") { + d.no_distance_action = NoDistanceAction::kHold; + } else if (action == "slow_approach") { + d.no_distance_action = NoDistanceAction::kSlowApproach; + } else if (action == "exit") { + d.no_distance_action = NoDistanceAction::kExit; + } else { + throw std::runtime_error( + "配置非法: distance.no_distance_action 仅支持 hold / slow_approach / exit"); + } + d.no_distance_approach_limit_mps = RequireNonNegative( + RequireNumber(s, "distance", "no_distance_approach_limit_mps"), + "distance.no_distance_approach_limit_mps"); + return d; +} + +AccelLimitConfig LoadAccelLimit(const Json& root) { + const Json& s = RequireSection(root, "accel_limit"); + AccelLimitConfig a; + a.ax_mps2 = RequirePositive(RequireNumber(s, "accel_limit", "ax_mps2"), + "accel_limit.ax_mps2"); + a.ay_mps2 = RequirePositive(RequireNumber(s, "accel_limit", "ay_mps2"), + "accel_limit.ay_mps2"); + a.az_mps2 = RequirePositive(RequireNumber(s, "accel_limit", "az_mps2"), + "accel_limit.az_mps2"); + return a; +} + +} // namespace + +TrackingConfig TrackingConfig::LoadFromJson(const std::string& path) { + const Json root = ParseConfigFile(path); + TrackingConfig cfg; + cfg.control = LoadControl(root); + cfg.camera = LoadCamera(root); + cfg.heading = LoadHeading(root); + cfg.vertical = LoadVertical(root); + cfg.distance = LoadDistance(root); + cfg.accel_limit = LoadAccelLimit(root); + return cfg; +} + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/tests/frame_transform_test.cpp b/reference-c++/visual_track_control/tests/frame_transform_test.cpp new file mode 100644 index 0000000..9812fec --- /dev/null +++ b/reference-c++/visual_track_control/tests/frame_transform_test.cpp @@ -0,0 +1,56 @@ +// ============================================================================= +// frame_transform_test.cpp —— RotateBodyToNed 单元测试 +// +// 覆盖（物理追踪思路 §2.3，A4）： +// 零航向恒等、朝东时机体前向 → 东向、垂直分量不变、模长守恒。 +// ============================================================================= + +#include + +#include + +#include "visual_track_control/frame_transform.h" + +namespace { + +using drone::vtc::RotateBodyToNed; + +constexpr double kPi = 3.14159265358979323846; + +TEST(FrameTransformTest, ZeroYawIsIdentity) { + double x = 1.0, y = 0.5, z = -2.0; + RotateBodyToNed(x, y, z, 0.0); + EXPECT_NEAR(x, 1.0, 1e-12); // 北 = 前 + EXPECT_NEAR(y, 0.5, 1e-12); // 东 = 右 + EXPECT_NEAR(z, -2.0, 1e-12); +} + +TEST(FrameTransformTest, YawEast90MapsForwardToEast) { + double x = 2.0, y = 0.0, z = 0.0; + RotateBodyToNed(x, y, z, kPi / 2.0); // 机头朝东 + EXPECT_NEAR(x, 0.0, 1e-9); // 北向 ≈ 0 + EXPECT_NEAR(y, 2.0, 1e-9); // 东向 = 前向速度 +} + +TEST(FrameTransformTest, RightIsEastWhenHeadingNorth) { + double x = 0.0, y = 1.0, z = 0.0; + RotateBodyToNed(x, y, z, 0.0); + EXPECT_NEAR(x, 0.0, 1e-12); + EXPECT_NEAR(y, 1.0, 1e-12); +} + +TEST(FrameTransformTest, DownComponentUnchanged) { + double x = 1.0, y = 1.0, z = 3.5; + RotateBodyToNed(x, y, z, kPi / 3.0); + EXPECT_DOUBLE_EQ(z, 3.5); +} + +TEST(FrameTransformTest, PreservesHorizontalNorm) { + // 旋转为正交变换：水平合速度不变（限幅约束不被破坏的前提） + double x = 3.0, y = -4.0, z = 1.0; + const double norm_before = std::hypot(x, y); + RotateBodyToNed(x, y, z, 0.7); + EXPECT_NEAR(std::hypot(x, y), norm_before, 1e-9); +} + +} // namespace diff --git a/reference-c++/visual_track_control/tests/line_of_sight_test.cpp b/reference-c++/visual_track_control/tests/line_of_sight_test.cpp new file mode 100644 index 0000000..5762ec8 --- /dev/null +++ b/reference-c++/visual_track_control/tests/line_of_sight_test.cpp @@ -0,0 +1,69 @@ +// ============================================================================= +// line_of_sight_test.cpp —— PixelToAngle 单元测试 +// +// 覆盖（物理追踪思路 §4.1 / §5.1，C2）： +// 零偏移、画面边缘偏移 = 半视场角、奇对称、单调性、 +// 与线性近似的偏离（全程针孔模型）、构造期非法参数抛异常。 +// ============================================================================= + +#include + +#include + +#include "visual_track_control/line_of_sight.h" + +namespace { + +using drone::vtc::PixelToAngle; + +constexpr double kPi = 3.14159265358979323846; +constexpr double kRadToDeg = 180.0 / kPi; + +TEST(PixelToAngleTest, ZeroOffsetGivesZeroAngle) { + PixelToAngle conv(1000.0, 90.0 * kPi / 180.0); + EXPECT_DOUBLE_EQ(conv.Convert(0.0), 0.0); +} + +TEST(PixelToAngleTest, HalfSizeOffsetGivesHalfFov) { + // 针孔模型：偏移 = size/2 时 angle = atan(tan(θ/2)) = θ/2 + PixelToAngle conv(1000.0, 90.0 * kPi / 180.0); + EXPECT_NEAR(conv.Convert(500.0), 45.0 * kPi / 180.0, 1e-12); + EXPECT_NEAR(conv.Convert(-500.0), -45.0 * kPi / 180.0, 1e-12); +} + +TEST(PixelToAngleTest, OddSymmetry) { + PixelToAngle conv(1920.0, 60.0 * kPi / 180.0); + for (const double offset : {1.0, 100.0, 730.5}) { + EXPECT_NEAR(conv.Convert(-offset), -conv.Convert(offset), 1e-15); + } +} + +TEST(PixelToAngleTest, MonotonicIncreasing) { + PixelToAngle conv(1080.0, 35.0 * kPi / 180.0); + double prev = conv.Convert(-600.0); + for (double x = -500.0; x <= 600.0; x += 100.0) { + const double cur = conv.Convert(x); + EXPECT_GT(cur, prev); + prev = cur; + } +} + +TEST(PixelToAngleTest, DeviatesFromLinearApproximation) { + // C2：全程针孔模型。θ=90°、偏移比例 0.25 处线性近似误差约 4°， + // 针孔结果必须与之明显偏离（证明未走线性支路）。 + PixelToAngle conv(1000.0, 90.0 * kPi / 180.0); + const double pinhole_deg = conv.Convert(250.0) * kRadToDeg; // atan(250·2·tan45°/1000) + const double linear_deg = 250.0 * (90.0 / 1000.0); // θ·offset/size = 22.5° + EXPECT_NEAR(pinhole_deg, std::atan(0.5) * kRadToDeg, 1e-9); // 26.565° + EXPECT_GT(std::fabs(pinhole_deg - linear_deg), 2.0); +} + +TEST(PixelToAngleTest, ConstructorRejectsInvalidArgs) { + EXPECT_THROW(PixelToAngle(0.0, 1.0), std::invalid_argument); + EXPECT_THROW(PixelToAngle(-10.0, 1.0), std::invalid_argument); + EXPECT_THROW(PixelToAngle(1000.0, 0.0), std::invalid_argument); + EXPECT_THROW(PixelToAngle(1000.0, kPi), std::invalid_argument); + EXPECT_THROW(PixelToAngle(1000.0, kPi + 0.1), std::invalid_argument); +} + +} // namespace diff --git a/reference-c++/visual_track_control/tests/mavlink_command_gateway_test.cpp b/reference-c++/visual_track_control/tests/mavlink_command_gateway_test.cpp new file mode 100644 index 0000000..3e4fadc --- /dev/null +++ b/reference-c++/visual_track_control/tests/mavlink_command_gateway_test.cpp @@ -0,0 +1,181 @@ +// ============================================================================= +// mavlink_command_gateway_test.cpp —— MavlinkCommandGateway 单元测试 +// +// 覆盖（物理追踪思路 §2.2、§4.2，A2 掩码三分支）： +// ID 84 速度模式的 type_mask 选择（角速率非零 → 速率掩码；航向角非零 → +// 航向掩码；两者均零 → 速率掩码配零角速率，保持航向）、 +// 速度/航向字段编码、刹车消息、坐标基准、传输失败传播。 +// +// 构建约束：本文件**不得**包含 common/mavlink.h（MAVLINK_HELPER 默认空宏， +// 多 TU 包含会多重定义），故按 MAVLink v2 线格式手工解码： +// 帧头 10 字节（STX=0xFD, len, flags×2, seq, sys, comp, msgid×3 LE）， +// 载荷按字段尺寸降序排列（ID 84：time,x,y,z,vx,vy,vz,afx,afy,afz, +// yaw,yaw_rate 均 4 字节在前，随后 u16 type_mask，再 target_system、 +// target_component、coordinate_frame 三个 u8）。 +// ============================================================================= + +#include +#include +#include + +#include + +#include "visual_track_control/i_command_transport.h" +#include "visual_track_control/mavlink_command_gateway.h" +#include "visual_track_control/vtc_types.h" + +namespace { + +using drone::vtc::ControlOutput; +using drone::vtc::ICommandTransport; +using drone::vtc::MavlinkCommandGateway; + +// ---- MAVLink v2 帧布局常量（见文件头说明） ---- +constexpr std::uint8_t kMavlinkV2Stx = 0xFD; +constexpr std::size_t kHeaderLen = 10; +constexpr std::size_t kMsgId84PayloadLen = 53; + +// ---- ID 84 载荷字段偏移（相对载荷起点） ---- +constexpr std::size_t kOffVx = 16; +constexpr std::size_t kOffVy = 20; +constexpr std::size_t kOffVz = 24; +constexpr std::size_t kOffYaw = 40; +constexpr std::size_t kOffYawRate = 44; +constexpr std::size_t kOffTypeMask = 48; +// u8 三字段线序（生成头 mavlink_msg_set_position_target_local_ned.h 为准） +constexpr std::size_t kOffTargetSystem = 50; +constexpr std::size_t kOffTargetComponent = 51; +constexpr std::size_t kOffCoordFrame = 52; + +// ---- 预期掩码值（与实现中常量一致：置位 = 忽略） ---- +// 基础 0x01C7（忽略位置与加速度）| 航向相关位 +constexpr std::uint16_t kExpectMaskYaw = 0x01C7 | 0x0800; // 忽略 yaw_rate +constexpr std::uint16_t kExpectMaskYawRate = 0x01C7 | 0x0400; // 忽略 yaw +constexpr std::uint16_t kExpectMaskBrake = 0x01C7 | 0x0C00; // 两者均忽略 + +/// 捕获式传输：记录最后一次发送的字节流，可注入失败 +class CaptureTransport : public ICommandTransport { +public: + bool Send(const std::uint8_t* data, std::size_t len) override { + ++send_count_; + last_.assign(data, data + len); + return succeed_; + } + + void SetSucceed(bool ok) { succeed_ = ok; } + int send_count() const { return send_count_; } + const std::vector& last() const { return last_; } + +private: + bool succeed_ = true; + int send_count_ = 0; + std::vector last_; +}; + +float ReadFloat(const std::vector& buf, std::size_t payload_off) { + float v; + std::memcpy(&v, buf.data() + kHeaderLen + payload_off, sizeof(v)); + return v; +} + +std::uint16_t ReadU16(const std::vector& buf, std::size_t payload_off) { + return static_cast(buf[kHeaderLen + payload_off]) | + (static_cast(buf[kHeaderLen + payload_off + 1]) << 8); +} + +void ExpectId84Header(const std::vector& buf) { + ASSERT_GE(buf.size(), kHeaderLen + kMsgId84PayloadLen + 2); // +CRC + EXPECT_EQ(buf[0], kMavlinkV2Stx); + EXPECT_EQ(buf[1], kMsgId84PayloadLen); + // msgid 24 位小端（字节 7-9）= 84 + EXPECT_EQ(buf[7], 84); + EXPECT_EQ(buf[8], 0); + EXPECT_EQ(buf[9], 0); +} + +constexpr double kDegToRad = 3.14159265358979323846 / 180.0; + +TEST(MavlinkGatewayTest, YawRateModeSelectsRateMask) { + CaptureTransport transport; + MavlinkCommandGateway gateway(transport, 1, 2, 3, 4); + + ControlOutput out; + out.mode = drone::vtc::ControlMode::kVelocityHeading; + out.vx_mps = 1.5; + out.vy_mps = -0.5; + out.vz_mps = 0.25; + out.yaw_rate_dps = 5.0; + out.yaw_deg = 0.0; + ASSERT_TRUE(gateway.SendVelocityHeading(out, 12345)); + + const auto& buf = transport.last(); + ExpectId84Header(buf); + EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskYawRate); + EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVx), 1.5F); + EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVy), -0.5F); + EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVz), 0.25F); + EXPECT_FLOAT_EQ(ReadFloat(buf, kOffYawRate), static_cast(5.0 * kDegToRad)); + EXPECT_EQ(buf[kHeaderLen + kOffCoordFrame], 1); // MAV_FRAME_LOCAL_NED + EXPECT_EQ(buf[kHeaderLen + kOffTargetSystem], 3); // target_system + EXPECT_EQ(buf[kHeaderLen + kOffTargetComponent], 4); // target_component +} + +TEST(MavlinkGatewayTest, YawOnlySelectsYawMask) { + CaptureTransport transport; + MavlinkCommandGateway gateway(transport, 1, 2, 3, 4); + + ControlOutput out; + out.mode = drone::vtc::ControlMode::kVelocityHeading; + out.yaw_deg = 10.0; + out.yaw_rate_dps = 0.0; + ASSERT_TRUE(gateway.SendVelocityHeading(out, 1)); + + const auto& buf = transport.last(); + ExpectId84Header(buf); + EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskYaw); + EXPECT_FLOAT_EQ(ReadFloat(buf, kOffYaw), static_cast(10.0 * kDegToRad)); +} + +TEST(MavlinkGatewayTest, BothZeroKeepsHeadingViaRateMask) { + // A2 回归：旧实现在零角速率时落到航向掩码，等于误发绝对航向 0（正北）。 + // 正确行为：速率掩码 + 零角速率 = 保持当前航向。 + CaptureTransport transport; + MavlinkCommandGateway gateway(transport, 1, 2, 3, 4); + + ControlOutput out; + out.mode = drone::vtc::ControlMode::kVelocityHeading; + out.vx_mps = 1.0; + ASSERT_TRUE(gateway.SendVelocityHeading(out, 1)); + + const auto& buf = transport.last(); + ExpectId84Header(buf); + EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskYawRate); + EXPECT_FLOAT_EQ(ReadFloat(buf, kOffYawRate), 0.0F); +} + +TEST(MavlinkGatewayTest, BrakeHoverSendsZeroVelocityAllIgnored) { + CaptureTransport transport; + MavlinkCommandGateway gateway(transport, 1, 2, 3, 4); + ASSERT_TRUE(gateway.SendBrakeHover(777)); + + const auto& buf = transport.last(); + ExpectId84Header(buf); + EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskBrake); + EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVx), 0.0F); + EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVy), 0.0F); + EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVz), 0.0F); +} + +TEST(MavlinkGatewayTest, TransportFailurePropagates) { + CaptureTransport transport; + transport.SetSucceed(false); + MavlinkCommandGateway gateway(transport, 1, 2, 3, 4); + + ControlOutput out; + out.mode = drone::vtc::ControlMode::kVelocityHeading; + EXPECT_FALSE(gateway.SendVelocityHeading(out, 1)); + EXPECT_FALSE(gateway.SendBrakeHover(1)); + EXPECT_EQ(transport.send_count(), 2); +} + +} // namespace diff --git a/reference-c++/visual_track_control/tests/pid_controller_test.cpp b/reference-c++/visual_track_control/tests/pid_controller_test.cpp new file mode 100644 index 0000000..5e50123 --- /dev/null +++ b/reference-c++/visual_track_control/tests/pid_controller_test.cpp @@ -0,0 +1,123 @@ +// ============================================================================= +// pid_controller_test.cpp —— PidController 单元测试 +// +// 覆盖（物理追踪思路 §6.2 / §6.4）： +// 位置式 P/I/D 三项、积分钳位、输出限幅、异常节拍保护、 +// anti-windup 条件积分（C1 修复回归：饱和冻结 + 误差反号回卷）、 +// ResetIntegral / Reset 行为。 +// ============================================================================= + +#include + +#include "visual_track_control/pid_controller.h" + +namespace { + +using drone::vtc::PidController; +using drone::vtc::PidParams; + +PidParams MakeParams(double kp, double ki, double kd, double integral_limit, + double output_limit, double filter_coef = 0.0) { + PidParams p; + p.kp = kp; + p.ki = ki; + p.kd = kd; + p.integral_limit = integral_limit; + p.derivative_filter_coef = filter_coef; + p.output_limit = output_limit; + return p; +} + +TEST(PidControllerTest, ProportionalOnly) { + PidController pid(MakeParams(2.0, 0.0, 0.0, 0.0, 0.0)); + EXPECT_DOUBLE_EQ(pid.Update(3.0, 0.1), 6.0); + EXPECT_DOUBLE_EQ(pid.Update(-1.5, 0.1), -3.0); +} + +TEST(PidControllerTest, IntegralAccumulates) { + PidController pid(MakeParams(0.0, 1.0, 0.0, 0.0, 0.0)); + // 两拍 error=2、dt=0.5：积分累计 1.0 → 2.0 + EXPECT_DOUBLE_EQ(pid.Update(2.0, 0.5), 1.0); + EXPECT_DOUBLE_EQ(pid.Update(2.0, 0.5), 2.0); +} + +TEST(PidControllerTest, IntegralClampedToLimit) { + PidController pid(MakeParams(0.0, 1.0, 0.0, /*integral_limit=*/1.0, 0.0)); + for (int i = 0; i < 5; ++i) { + pid.Update(10.0, 1.0); + } + // 积分候选值被钳位在 1.0 → 输出恒为 1.0 + EXPECT_DOUBLE_EQ(pid.LastOutput(), 1.0); +} + +TEST(PidControllerTest, OutputClampedToLimit) { + PidController pid(MakeParams(10.0, 0.0, 0.0, 0.0, /*output_limit=*/3.0)); + EXPECT_DOUBLE_EQ(pid.Update(1.0, 0.1), 3.0); + EXPECT_DOUBLE_EQ(pid.Update(-1.0, 0.1), -3.0); +} + +TEST(PidControllerTest, AntiWindupFreezeWhileSaturatedSameDirection) { + // kp=0、ki=2、output_limit=5：error=+1 持续饱和时积分冻结， + // 输出恒钉在 +5（10 拍累计 10 拍也不会让积分继续增长）。 + PidController pid(MakeParams(0.0, 2.0, 0.0, 0.0, 5.0)); + for (int i = 0; i < 10; ++i) { + EXPECT_DOUBLE_EQ(pid.Update(10.0, 1.0), 5.0); + } +} + +TEST(PidControllerTest, AntiWindupUnwindsAfterErrorFlips) { + // C1 回归：误差反号后积分必须立即回卷（旧判据"积分与输出同向"会 + // 冻结回卷、输出一直钉在 +5）。 + // kp=0、ki=2、limit=5：+10 饱和 3 拍、积分冻结在 0； + // error=-1 首拍 → 输出 2×(-1) = -2（立即回卷）； + // 持续负误差使输出到达负饱和 -5（积分冻结在 -2，输出 = clamp(-6)）。 + PidController pid(MakeParams(0.0, 2.0, 0.0, 0.0, 5.0)); + for (int i = 0; i < 3; ++i) { + EXPECT_DOUBLE_EQ(pid.Update(10.0, 1.0), 5.0); + } + EXPECT_DOUBLE_EQ(pid.Update(-1.0, 1.0), -2.0); // 首拍立即回卷 + double out = -2.0; + for (int i = 1; i < 12; ++i) { + out = pid.Update(-1.0, 1.0); + } + EXPECT_NEAR(out, -5.0, 1e-9); // 旧判据此处仍为 +5.0 +} + +TEST(PidControllerTest, DerivativeFirstStepIsZero) { + PidController pid(MakeParams(0.0, 0.0, 1.0, 0.0, 0.0)); + // 首拍无微分历史：微分项为 0 + EXPECT_DOUBLE_EQ(pid.Update(5.0, 1.0), 0.0); +} + +TEST(PidControllerTest, DerivativeWithLowPassFilter) { + PidController pid(MakeParams(0.0, 0.0, 1.0, 0.0, 0.0, /*filter=*/0.5)); + pid.Update(0.0, 1.0); // 建立微分历史（导数 0） + // 第二拍原始导数 1.0，低通后 0.5×0 + 0.5×1.0 = 0.5 + EXPECT_DOUBLE_EQ(pid.Update(1.0, 1.0), 0.5); +} + +TEST(PidControllerTest, ZeroOrNegativeDtHoldsLastOutput) { + PidController pid(MakeParams(1.0, 0.0, 0.0, 0.0, 0.0)); + EXPECT_DOUBLE_EQ(pid.Update(2.0, 0.1), 2.0); + EXPECT_DOUBLE_EQ(pid.Update(99.0, 0.0), 2.0); + EXPECT_DOUBLE_EQ(pid.Update(99.0, -1.0), 2.0); +} + +TEST(PidControllerTest, ResetIntegralClearsOnlyIntegral) { + PidController pid(MakeParams(0.0, 1.0, 1.0, 0.0, 0.0)); + pid.Update(2.0, 1.0); // 积分 = 2 + pid.ResetIntegral(); + // 积分清零、微分历史保留：输出应为 kd×(0−2)/1 = −2 + EXPECT_DOUBLE_EQ(pid.Update(0.0, 1.0), -2.0); +} + +TEST(PidControllerTest, ResetClearsEverything) { + PidController pid(MakeParams(1.0, 1.0, 1.0, 0.0, 0.0)); + pid.Update(5.0, 1.0); + pid.Reset(); + EXPECT_DOUBLE_EQ(pid.LastOutput(), 0.0); + // 全清后等同全新控制器：首拍无微分历史 + EXPECT_DOUBLE_EQ(pid.Update(3.0, 1.0), 3.0 + 3.0); // kp·e + ki·∫e +} + +} // namespace diff --git a/reference-c++/visual_track_control/tests/rate_limiter_test.cpp b/reference-c++/visual_track_control/tests/rate_limiter_test.cpp new file mode 100644 index 0000000..45c7673 --- /dev/null +++ b/reference-c++/visual_track_control/tests/rate_limiter_test.cpp @@ -0,0 +1,76 @@ +// ============================================================================= +// rate_limiter_test.cpp —— Clamp / SlewRateLimiter 单元测试 +// +// 覆盖（物理追踪思路 §8、状态机设计 S6）： +// 静态限幅、变化率限幅步进、首条指令从零渐变（S6）、 +// 异常节拍保护、Reset 归零。 +// ============================================================================= + +#include + +#include "visual_track_control/rate_limiter.h" + +namespace { + +using drone::vtc::Clamp; +using drone::vtc::SlewRateLimiter; + +TEST(ClampTest, ClampsValue) { + EXPECT_DOUBLE_EQ(Clamp(5.0, -2.0, 2.0), 2.0); + EXPECT_DOUBLE_EQ(Clamp(-5.0, -2.0, 2.0), -2.0); + EXPECT_DOUBLE_EQ(Clamp(1.5, -2.0, 2.0), 1.5); + // 边界值：落在区间内 + EXPECT_DOUBLE_EQ(Clamp(2.0, -2.0, 2.0), 2.0); + EXPECT_DOUBLE_EQ(Clamp(-2.0, -2.0, 2.0), -2.0); +} + +TEST(SlewRateLimiterTest, FirstCommandRampsFromZero) { + // S6：初始输出为 0，首条大指令只能按变化率渐变 + SlewRateLimiter lim(/*rate_limit=*/2.0); + EXPECT_DOUBLE_EQ(lim.Limit(10.0, 0.5), 1.0); // 0 + 2×0.5 + EXPECT_DOUBLE_EQ(lim.Limit(10.0, 0.5), 2.0); // 继续渐变 + EXPECT_DOUBLE_EQ(lim.Limit(10.0, 10.0), 10.0); // 步长足够 → 到达目标 +} + +TEST(SlewRateLimiterTest, TracksTargetWithinStep) { + SlewRateLimiter lim(10.0); + // 步长 10×1=10，目标 3 在步长内 → 直接到达 + EXPECT_DOUBLE_EQ(lim.Limit(3.0, 1.0), 3.0); + EXPECT_DOUBLE_EQ(lim.Limit(3.0, 1.0), 3.0); // 已在目标：保持 +} + +TEST(SlewRateLimiterTest, NegativeDirection) { + SlewRateLimiter lim(2.0); + EXPECT_DOUBLE_EQ(lim.Limit(-10.0, 0.5), -1.0); + EXPECT_DOUBLE_EQ(lim.Limit(-10.0, 0.5), -2.0); +} + +TEST(SlewRateLimiterTest, ZeroOrNegativeDtHolds) { + SlewRateLimiter lim(2.0); + lim.Limit(10.0, 1.0); // 输出 2.0 + EXPECT_DOUBLE_EQ(lim.Limit(10.0, 0.0), 2.0); + EXPECT_DOUBLE_EQ(lim.Limit(10.0, -1.0), 2.0); +} + +TEST(SlewRateLimiterTest, ZeroRateLimitFreezesOutput) { + SlewRateLimiter lim(0.0); + EXPECT_DOUBLE_EQ(lim.Limit(5.0, 1.0), 0.0); +} + +TEST(SlewRateLimiterTest, ResetToValue) { + SlewRateLimiter lim(2.0); + lim.Limit(10.0, 1.0); + lim.Reset(); + EXPECT_DOUBLE_EQ(lim.LastOutput(), 0.0); + lim.Reset(3.5); + EXPECT_DOUBLE_EQ(lim.LastOutput(), 3.5); + // 复位后从新值继续渐变 + EXPECT_DOUBLE_EQ(lim.Limit(10.0, 1.0), 5.5); // 3.5 + 2×1 +} + +TEST(SlewRateLimiterTest, CustomInitialValue) { + SlewRateLimiter lim(1.0, /*initial=*/5.0); + EXPECT_DOUBLE_EQ(lim.Limit(10.0, 1.0), 6.0); +} + +} // namespace diff --git a/reference-c++/visual_track_control/tests/staleness_checker_test.cpp b/reference-c++/visual_track_control/tests/staleness_checker_test.cpp new file mode 100644 index 0000000..a4f1309 --- /dev/null +++ b/reference-c++/visual_track_control/tests/staleness_checker_test.cpp @@ -0,0 +1,41 @@ +// ============================================================================= +// staleness_checker_test.cpp —— StalenessChecker 单元测试 +// +// 覆盖（物理追踪思路 §7.1）： +// 从未取样判超龄、有效期内新鲜、超出有效期超龄、 +// 边界恰好在有效期上（不算超龄）、时间戳不一致判超龄（C4）。 +// ============================================================================= + +#include + +#include "visual_track_control/staleness_checker.h" + +namespace { + +using drone::vtc::StalenessChecker; + +TEST(StalenessCheckerTest, NeverSampledIsStale) { + EXPECT_TRUE(StalenessChecker::IsStale(0, 1000, 200)); + EXPECT_TRUE(StalenessChecker::IsStale(-5, 1000, 200)); +} + +TEST(StalenessCheckerTest, WithinValidityIsFresh) { + EXPECT_FALSE(StalenessChecker::IsStale(900, 1000, 200)); + EXPECT_FALSE(StalenessChecker::IsStale(1000, 1000, 200)); // 刚取样 +} + +TEST(StalenessCheckerTest, BeyondValidityIsStale) { + EXPECT_TRUE(StalenessChecker::IsStale(799, 1000, 200)); // 201 > 200 +} + +TEST(StalenessCheckerTest, ExactlyAtBoundaryIsFresh) { + // 超龄判定为严格大于：恰好等于有效期仍算新鲜 + EXPECT_FALSE(StalenessChecker::IsStale(800, 1000, 200)); +} + +TEST(StalenessCheckerTest, InconsistentTimestampIsStale) { + // C4：now < sample 时间戳不一致 → 年龄不可信 → 判超龄（宁降级不轻信） + EXPECT_TRUE(StalenessChecker::IsStale(2000, 1000, 200)); +} + +} // namespace diff --git a/reference-c++/visual_track_control/tests/tracking_control_law_test.cpp b/reference-c++/visual_track_control/tests/tracking_control_law_test.cpp new file mode 100644 index 0000000..4ec1bce --- /dev/null +++ b/reference-c++/visual_track_control/tests/tracking_control_law_test.cpp @@ -0,0 +1,349 @@ +// ============================================================================= +// tracking_control_law_test.cpp —— TrackingControlLaw 主控律单元测试 +// +// 覆盖（物理追踪思路 §2.3 / §4 / §5 / §6 / §7 / §9）： +// 超龄降级（姿态缺失/过期、视觉超龄、降级优先级）、判丢零速刹车、 +// Coast 清积分与标志、速率式/位置式航向通道（含绝对航向合成与回绕， +// A2）、垂直统一速度支路（A1）、距离通道三种无距离降级动作、 +// 首拍变化率限幅（S6）、机体系→NED 输出旋转（A4）。 +// ============================================================================= + +#include + +#include + +#include "visual_track_control/tracking_control_law.h" +#include "visual_track_control/vtc_config.h" +#include "visual_track_control/vtc_types.h" + +namespace { + +using namespace drone::vtc; + +constexpr double kPi = 3.14159265358979323846; +constexpr double kDegToRad = kPi / 180.0; +constexpr double kRadToDeg = 180.0 / kPi; + +/// 测试基准配置：大限幅使限幅器近似透明，便于独立验证各通道数学。 +/// 画面 1000×1000、视场角 90° → 针孔系数 2·tan(45°)/1000 = 0.002， +/// 偏移 100 像素 → 视线角 atan(0.2) ≈ 11.3099°。 +TrackingConfig MakeConfig() { + TrackingConfig cfg; + cfg.control.frequency_hz = 20.0; + cfg.control.visual_stale_ms = 200; + cfg.control.radar_stale_ms = 500; + cfg.control.attitude_stale_ms = 300; + cfg.camera.image_width = 1000; + cfg.camera.image_height = 1000; + cfg.camera.fov_h_deg = 90.0; + cfg.camera.fov_v_deg = 90.0; + cfg.heading.mode = HeadingMode::kRate; + cfg.heading.gain = 1.0; + cfg.heading.yaw_rate_limit_dps = 30.0; + cfg.heading.yaw_slew_limit_dps2 = 100000.0; + cfg.vertical.vz_gain_mps_per_deg = 0.1; + cfg.vertical.vz_limit_mps = 2.0; + cfg.distance.d_exp_m = 10.0; + cfg.distance.kp = 0.5; + cfg.distance.ki = 0.0; + cfg.distance.kd = 0.0; + cfg.distance.integral_limit = 0.0; + cfg.distance.derivative_filter_coef = 0.0; + cfg.distance.approach_velocity_limit_mps = 3.0; + cfg.distance.retreat_velocity_limit_mps = 1.0; + cfg.distance.no_distance_action = NoDistanceAction::kHold; + cfg.distance.no_distance_approach_limit_mps = 0.5; + cfg.accel_limit.ax_mps2 = 1000.0; + cfg.accel_limit.ay_mps2 = 1000.0; + cfg.accel_limit.az_mps2 = 1000.0; + return cfg; +} + +/// 新鲜快照：目标在画面中心，姿态/追踪/雷达均新鲜 +ControlSnapshot MakeSnapshot(std::int64_t now_ms) { + ControlSnapshot s; + s.track.tracked = true; + s.track.is_predicted = false; + s.track.center = {500.0, 500.0}; + s.track_time_ms = now_ms; + s.attitude_present = true; + s.attitude_time_ms = now_ms; + return s; +} + +// ---- 超龄降级（§7.1 / §9.2） ---- + +TEST(TrackingControlLawTest, AttitudeMissingDegrades) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.attitude_present = false; + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.mode, ControlMode::kDegradedHold); + EXPECT_EQ(out.degraded_reason, DegradedReason::kAttitudeStale); + EXPECT_FALSE(out.valid); +} + +TEST(TrackingControlLawTest, AttitudeStaleDegrades) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.attitude_time_ms = 600; // 400 ms 前 > 300 ms 阈值 + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.mode, ControlMode::kDegradedHold); + EXPECT_EQ(out.degraded_reason, DegradedReason::kAttitudeStale); +} + +TEST(TrackingControlLawTest, VisionStaleDegrades) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.track_time_ms = 700; // 300 ms 前 > 200 ms 阈值 + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.mode, ControlMode::kDegradedHold); + EXPECT_EQ(out.degraded_reason, DegradedReason::kVisionStale); +} + +TEST(TrackingControlLawTest, NeverSampledTrackDegrades) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.track_time_ms = 0; // 从未取样 + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.mode, ControlMode::kDegradedHold); + EXPECT_EQ(out.degraded_reason, DegradedReason::kVisionStale); +} + +TEST(TrackingControlLawTest, AttitudeStaleBeatsVisionStale) { + // 降级优先级：姿态校验在前（§9.2 姿态过期禁止一切空间转换） + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.attitude_present = false; + snap.track_time_ms = 0; + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.degraded_reason, DegradedReason::kAttitudeStale); +} + +// ---- 判丢刹车（§6.4） ---- + +TEST(TrackingControlLawTest, LostTargetBrakes) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.track.tracked = false; + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.mode, ControlMode::kBrakeHover); + EXPECT_TRUE(out.valid); + EXPECT_DOUBLE_EQ(out.vx_mps, 0.0); + EXPECT_DOUBLE_EQ(out.vy_mps, 0.0); + EXPECT_DOUBLE_EQ(out.vz_mps, 0.0); +} + +// ---- Coast（§6.4） ---- + +TEST(TrackingControlLawTest, CoastSetsFlagAndKeepsTracking) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.track.is_predicted = true; + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.mode, ControlMode::kVelocityHeading); + EXPECT_TRUE(out.coast_active); + EXPECT_TRUE(out.valid); +} + +// ---- 水平航向通道（§4.2，A2） ---- + +TEST(TrackingControlLawTest, RateModeMapsBetaToYawRate) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.track.center = {600.0, 500.0}; // 目标偏右 100 像素 + const auto out = law.Update(snap, 1000); + const double beta_deg = std::atan(0.2) * kRadToDeg; // ≈ 11.31° + EXPECT_NEAR(out.yaw_rate_dps, beta_deg, 1e-6); // gain=1，右转正 + EXPECT_DOUBLE_EQ(out.yaw_deg, 0.0); +} + +TEST(TrackingControlLawTest, RateModeClampsToYawRateLimit) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.track.center = {950.0, 500.0}; // 大偏移：β≈43.5° > 30°/s 上限 + const auto out = law.Update(snap, 1000); + EXPECT_NEAR(out.yaw_rate_dps, 30.0, 1e-9); +} + +TEST(TrackingControlLawTest, PositionModeComposesAbsoluteHeading) { + // A2：位置式输出为绝对 NED 航向 = 当前航向 + 限幅修正量 + auto cfg = MakeConfig(); + cfg.heading.mode = HeadingMode::kPosition; + TrackingControlLaw law(cfg); + auto snap = MakeSnapshot(1000); + snap.track.center = {600.0, 500.0}; + snap.yaw_rad = 45.0 * kDegToRad; + const auto out = law.Update(snap, 1000); + const double beta_deg = std::atan(0.2) * kRadToDeg; + EXPECT_NEAR(out.yaw_deg, 45.0 + beta_deg, 1e-6); + EXPECT_DOUBLE_EQ(out.yaw_rate_dps, 0.0); +} + +TEST(TrackingControlLawTest, PositionModeWrapsAround180) { + auto cfg = MakeConfig(); + cfg.heading.mode = HeadingMode::kPosition; + TrackingControlLaw law(cfg); + auto snap = MakeSnapshot(1000); + snap.track.center = {600.0, 500.0}; + snap.yaw_rad = 175.0 * kDegToRad; + const auto out = law.Update(snap, 1000); + const double beta_deg = std::atan(0.2) * kRadToDeg; + // 175 + 11.31 = 186.31 → 回绕到 ≈ -173.69 + EXPECT_NEAR(out.yaw_deg, 175.0 + beta_deg - 360.0, 1e-6); + EXPECT_GT(out.yaw_deg, -180.0); + EXPECT_LT(out.yaw_deg, 180.0); +} + +// ---- 垂直统一速度支路（§5，A1） ---- + +TEST(TrackingControlLawTest, VerticalOffsetDrivesVz) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.track.center = {500.0, 600.0}; // 目标偏低 100 像素 → α>0 → 下降 + const auto out = law.Update(snap, 1000); + const double alpha_deg = std::atan(0.2) * kRadToDeg; + EXPECT_NEAR(out.vz_mps, 0.1 * alpha_deg, 1e-6); + EXPECT_EQ(out.mode, ControlMode::kVelocityHeading); // A1：不切姿态微调模式 +} + +TEST(TrackingControlLawTest, VerticalVelocityClamped) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.track.center = {500.0, 990.0}; // α≈44.5° × 0.1 = 4.45 > 2 上限 + const auto out = law.Update(snap, 1000); + EXPECT_NEAR(out.vz_mps, 2.0, 1e-9); +} + +// ---- 距离通道（§6.2 / §6.3） ---- + +TEST(TrackingControlLawTest, DistanceApproachWhenFar) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.radar_distance_m = 30.0; // Δd = +20 → kp 输出 10 → 限幅 3 + snap.associated_to_target = true; + snap.radar_time_ms = 1000; + const auto out = law.Update(snap, 1000); + EXPECT_NEAR(out.vx_mps, 3.0, 1e-9); +} + +TEST(TrackingControlLawTest, DistanceRetreatClampedWhenTooClose) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.radar_distance_m = 5.0; // Δd = -5 → kp 输出 -2.5 → 后退限幅 -1 + snap.associated_to_target = true; + snap.radar_time_ms = 1000; + const auto out = law.Update(snap, 1000); + EXPECT_NEAR(out.vx_mps, -1.0, 1e-9); +} + +TEST(TrackingControlLawTest, NoDistanceHoldZerosVx) { + TrackingControlLaw law(MakeConfig()); // 缺省 kHold + auto snap = MakeSnapshot(1000); + snap.associated_to_target = false; + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.mode, ControlMode::kVelocityHeading); + EXPECT_DOUBLE_EQ(out.vx_mps, 0.0); +} + +TEST(TrackingControlLawTest, NoDistanceSlowApproach) { + auto cfg = MakeConfig(); + cfg.distance.no_distance_action = NoDistanceAction::kSlowApproach; + TrackingControlLaw law(cfg); + auto snap = MakeSnapshot(1000); + snap.associated_to_target = false; + const auto out = law.Update(snap, 1000); + EXPECT_NEAR(out.vx_mps, 0.5, 1e-9); +} + +TEST(TrackingControlLawTest, NoDistanceExitDegrades) { + auto cfg = MakeConfig(); + cfg.distance.no_distance_action = NoDistanceAction::kExit; + TrackingControlLaw law(cfg); + auto snap = MakeSnapshot(1000); + snap.associated_to_target = false; + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.mode, ControlMode::kDegradedHold); + EXPECT_EQ(out.degraded_reason, DegradedReason::kNoDistanceExit); + EXPECT_FALSE(out.valid); +} + +TEST(TrackingControlLawTest, StaleRadarFallsBackToNoDistance) { + TrackingControlLaw law(MakeConfig()); // kHold + auto snap = MakeSnapshot(1000); + snap.radar_distance_m = 30.0; + snap.associated_to_target = true; + snap.radar_time_ms = 400; // 600 ms 前 > 500 ms 阈值 → 距离不可用 + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.mode, ControlMode::kVelocityHeading); + EXPECT_DOUBLE_EQ(out.vx_mps, 0.0); +} + +// ---- 首拍/限幅（§8、S6） ---- + +TEST(TrackingControlLawTest, FirstTickVxSlewsFromZero) { + // S6：首条速度指令必须经变化率限幅，从 0 渐变而非阶跃 + auto cfg = MakeConfig(); + cfg.accel_limit.ax_mps2 = 2.0; + TrackingControlLaw law(cfg); + auto snap = MakeSnapshot(1000); + snap.radar_distance_m = 30.0; // 通道输出本应为 3.0 + snap.associated_to_target = true; + snap.radar_time_ms = 1000; + const auto out = law.Update(snap, 1000); // 首拍：dt 按 1/20 Hz 兜底 + EXPECT_NEAR(out.vx_mps, 2.0 * (1.0 / 20.0), 1e-9); // 0.1 +} + +// ---- 机体系 → NED 旋转（§2.3，A4） ---- + +TEST(TrackingControlLawTest, OutputRotatedToNedByYaw) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.yaw_rad = kPi / 2.0; // 机头朝东 + snap.radar_distance_m = 30.0; + snap.associated_to_target = true; + snap.radar_time_ms = 1000; + const auto out = law.Update(snap, 1000); + // 机体前向 3.0 → 东向 3.0，北向 ≈ 0 + EXPECT_NEAR(out.vx_mps, 0.0, 1e-9); + EXPECT_NEAR(out.vy_mps, 3.0, 1e-9); +} + +TEST(TrackingControlLawTest, ZeroYawKeepsForwardAsNorth) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.yaw_rad = 0.0; + snap.radar_distance_m = 30.0; + snap.associated_to_target = true; + snap.radar_time_ms = 1000; + const auto out = law.Update(snap, 1000); + EXPECT_NEAR(out.vx_mps, 3.0, 1e-9); + EXPECT_NEAR(out.vy_mps, 0.0, 1e-9); +} + +// ---- Reset（§6.4） ---- + +TEST(TrackingControlLawTest, ResetRestoresSlewFromZero) { + auto cfg = MakeConfig(); + cfg.accel_limit.ax_mps2 = 2.0; + TrackingControlLaw law(cfg); + auto snap = MakeSnapshot(1000); + snap.radar_distance_m = 30.0; + snap.associated_to_target = true; + snap.radar_time_ms = 1000; + for (std::int64_t t = 1000; t < 3000; t += 50) { + snap.track_time_ms = t; + snap.radar_time_ms = t; + snap.attitude_time_ms = t; + law.Update(snap, t); // 若干拍后 vx 已爬升到限幅 3.0 + } + law.Reset(); + snap.track_time_ms = 3000; + snap.radar_time_ms = 3000; + snap.attitude_time_ms = 3000; + const auto out = law.Update(snap, 3000); + // Reset 后限幅器归零：又从 0.1 开始渐变 + EXPECT_NEAR(out.vx_mps, 0.1, 1e-9); +} + +} // namespace diff --git a/reference-c++/visual_track_control/tests/vtc_config_test.cpp b/reference-c++/visual_track_control/tests/vtc_config_test.cpp new file mode 100644 index 0000000..8a749c6 --- /dev/null +++ b/reference-c++/visual_track_control/tests/vtc_config_test.cpp @@ -0,0 +1,224 @@ +// ============================================================================= +// vtc_config_test.cpp —— TrackingConfig::LoadFromJson 单元测试 +// +// 覆盖（物理追踪思路 §8、需求分析 NFR-007）： +// 真实配置文件加载成功与取值正确（含 A1/A2 修改后的字段结构）、 +// 缺字段 / 缺分组 / 非法枚举 / 非法数值 / 文件缺失均抛带字段名的异常。 +// +// 依赖编译期宏： +// VTC_TEST_CONFIG —— config/track_control.json 绝对路径 +// VTC_TEST_TMP_DIR —— 测试临时文件输出目录（构建目录） +// ============================================================================= + +#include +#include +#include +#include + +#include + +#include "visual_track_control/vtc_config.h" + +namespace { + +using drone::vtc::HeadingMode; +using drone::vtc::NoDistanceAction; +using drone::vtc::TrackingConfig; + +/// 与真实配置同构的完整合法 JSON（用于派生各非法变体） +const char* kFullJson = R"({ + "control": { + "frequency_hz": 20.0, + "visual_stale_ms": 200, + "radar_stale_ms": 500, + "attitude_stale_ms": 300 + }, + "camera": { + "image_width": 1920, + "image_height": 1080, + "fov_h_deg": 60.0, + "fov_v_deg": 35.0 + }, + "heading": { + "mode": "rate", + "gain": 1.0, + "yaw_rate_limit_dps": 30.0, + "yaw_slew_limit_dps2": 60.0 + }, + "vertical": { + "vz_gain_mps_per_deg": 0.1, + "vz_limit_mps": 2.0 + }, + "distance": { + "d_exp_m": 10.0, + "kp": 0.5, + "ki": 0.05, + "kd": 0.1, + "integral_limit": 2.0, + "derivative_filter_coef": 0.2, + "approach_velocity_limit_mps": 3.0, + "retreat_velocity_limit_mps": 1.0, + "no_distance_action": "hold", + "no_distance_approach_limit_mps": 0.5 + }, + "accel_limit": { + "ax_mps2": 2.0, + "ay_mps2": 2.0, + "az_mps2": 1.5 + } +})"; + +/// 在 json 中把首个 from 替换为 to（未找到即测试失败） +void ReplaceOnce(std::string& json, const std::string& from, const std::string& to) { + const auto pos = json.find(from); + ASSERT_NE(pos, std::string::npos) << "未找到待替换片段: " << from; + json.replace(pos, from.size(), to); +} + +/// 把 JSON 文本写入临时文件并返回路径（调用方用完后删除） +std::string WriteTempJson(const std::string& content, const char* name) { + const std::string path = std::string(VTC_TEST_TMP_DIR) + "/" + name; + std::ofstream ofs(path); + EXPECT_TRUE(static_cast(ofs)) << "无法写入临时配置: " << path; + ofs << content; + return path; +} + +TEST(VtcConfigTest, LoadRealConfigSucceeds) { + const TrackingConfig cfg = TrackingConfig::LoadFromJson(VTC_TEST_CONFIG); + EXPECT_DOUBLE_EQ(cfg.control.frequency_hz, 20.0); + EXPECT_EQ(cfg.control.visual_stale_ms, 200); + EXPECT_EQ(cfg.camera.image_width, 1920); + EXPECT_EQ(cfg.camera.image_height, 1080); + EXPECT_DOUBLE_EQ(cfg.camera.fov_h_deg, 60.0); + EXPECT_EQ(cfg.heading.mode, HeadingMode::kRate); // A2：默认速率式 + EXPECT_DOUBLE_EQ(cfg.heading.yaw_rate_limit_dps, 30.0); + EXPECT_DOUBLE_EQ(cfg.vertical.vz_gain_mps_per_deg, 0.1); + EXPECT_DOUBLE_EQ(cfg.vertical.vz_limit_mps, 2.0); + EXPECT_DOUBLE_EQ(cfg.distance.d_exp_m, 10.0); + EXPECT_EQ(cfg.distance.no_distance_action, NoDistanceAction::kHold); + EXPECT_DOUBLE_EQ(cfg.accel_limit.ax_mps2, 2.0); +} + +TEST(VtcConfigTest, FullJsonRoundTripMatchesExpected) { + // 内嵌合法 JSON 与真实配置文件行为一致 + const std::string path = WriteTempJson(kFullJson, "vtc_cfg_full.json"); + const TrackingConfig cfg = TrackingConfig::LoadFromJson(path); + EXPECT_EQ(cfg.heading.mode, HeadingMode::kRate); + EXPECT_DOUBLE_EQ(cfg.distance.kp, 0.5); + std::remove(path.c_str()); +} + +TEST(VtcConfigTest, PositionModeAccepted) { + std::string json = kFullJson; + ReplaceOnce(json, "\"mode\": \"rate\"", "\"mode\": \"position\""); + const std::string path = WriteTempJson(json, "vtc_cfg_position.json"); + const TrackingConfig cfg = TrackingConfig::LoadFromJson(path); + EXPECT_EQ(cfg.heading.mode, HeadingMode::kPosition); + std::remove(path.c_str()); +} + +TEST(VtcConfigTest, MissingFileThrowsWithPath) { + try { + TrackingConfig::LoadFromJson("/nonexistent/vtc_no_such_file.json"); + FAIL() << "应当抛出异常"; + } catch (const std::runtime_error& e) { + EXPECT_NE(std::string(e.what()).find("vtc_no_such_file.json"), + std::string::npos); + } +} + +TEST(VtcConfigTest, MalformedJsonThrows) { + const std::string path = WriteTempJson("{ this is not json", "vtc_cfg_bad.json"); + EXPECT_THROW(TrackingConfig::LoadFromJson(path), std::runtime_error); + std::remove(path.c_str()); +} + +TEST(VtcConfigTest, MissingFieldThrowsWithFieldName) { + std::string json = kFullJson; + ReplaceOnce(json, "\"gain\": 1.0,\n", ""); + const std::string path = WriteTempJson(json, "vtc_cfg_missing.json"); + try { + TrackingConfig::LoadFromJson(path); + FAIL() << "应当抛出异常"; + } catch (const std::runtime_error& e) { + EXPECT_NE(std::string(e.what()).find("heading.gain"), std::string::npos); + } + std::remove(path.c_str()); +} + +TEST(VtcConfigTest, MissingSectionThrows) { + std::string json = kFullJson; + ReplaceOnce(json, + ",\n \"accel_limit\": {\n" + " \"ax_mps2\": 2.0,\n" + " \"ay_mps2\": 2.0,\n" + " \"az_mps2\": 1.5\n" + " }", + ""); + const std::string path = WriteTempJson(json, "vtc_cfg_nosection.json"); + try { + TrackingConfig::LoadFromJson(path); + FAIL() << "应当抛出异常"; + } catch (const std::runtime_error& e) { + EXPECT_NE(std::string(e.what()).find("accel_limit"), std::string::npos); + } + std::remove(path.c_str()); +} + +TEST(VtcConfigTest, InvalidHeadingModeThrows) { + std::string json = kFullJson; + ReplaceOnce(json, "\"mode\": \"rate\"", "\"mode\": \"magic\""); + const std::string path = WriteTempJson(json, "vtc_cfg_badmode.json"); + try { + TrackingConfig::LoadFromJson(path); + FAIL() << "应当抛出异常"; + } catch (const std::runtime_error& e) { + EXPECT_NE(std::string(e.what()).find("heading.mode"), std::string::npos); + } + std::remove(path.c_str()); +} + +TEST(VtcConfigTest, InvalidNoDistanceActionThrows) { + std::string json = kFullJson; + ReplaceOnce(json, "\"no_distance_action\": \"hold\"", + "\"no_distance_action\": \"zoom\""); + const std::string path = WriteTempJson(json, "vtc_cfg_badaction.json"); + try { + TrackingConfig::LoadFromJson(path); + FAIL() << "应当抛出异常"; + } catch (const std::runtime_error& e) { + EXPECT_NE(std::string(e.what()).find("no_distance_action"), + std::string::npos); + } + std::remove(path.c_str()); +} + +TEST(VtcConfigTest, NegativeFrequencyThrows) { + std::string json = kFullJson; + ReplaceOnce(json, "\"frequency_hz\": 20.0", "\"frequency_hz\": -1.0"); + const std::string path = WriteTempJson(json, "vtc_cfg_negfreq.json"); + try { + TrackingConfig::LoadFromJson(path); + FAIL() << "应当抛出异常"; + } catch (const std::runtime_error& e) { + EXPECT_NE(std::string(e.what()).find("control.frequency_hz"), + std::string::npos); + } + std::remove(path.c_str()); +} + +TEST(VtcConfigTest, FovOutOfRangeThrows) { + std::string json = kFullJson; + ReplaceOnce(json, "\"fov_h_deg\": 60.0", "\"fov_h_deg\": 200.0"); + const std::string path = WriteTempJson(json, "vtc_cfg_badfov.json"); + try { + TrackingConfig::LoadFromJson(path); + FAIL() << "应当抛出异常"; + } catch (const std::runtime_error& e) { + EXPECT_NE(std::string(e.what()).find("camera.fov_h_deg"), std::string::npos); + } + std::remove(path.c_str()); +} + +} // namespace diff --git a/reference-c++/visual_track_control/视觉跟踪控制律实现文档.md b/reference-c++/visual_track_control/视觉跟踪控制律实现文档.md new file mode 100644 index 0000000..a114a33 --- /dev/null +++ b/reference-c++/visual_track_control/视觉跟踪控制律实现文档.md @@ -0,0 +1,443 @@ +# 视觉跟踪控制律实现文档（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）。 +- **统一限幅**（规格 §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 视为从未取样，直接判超龄** | +| `radar_distance_m` | 机头前向单点距离（米） | +| `associated_to_target` | 相机—雷达空间关联成立标志；**未关联不得当目标距离**（规格 §6.3、需求分析 FR-041） | +| `radar_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`、 +`radar_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`）、`accel_limit` +（`ax_mps2`、`ay_mps2`、`az_mps2`）。JSON 键名与字段一一对应，见 +`config/track_control.json`。`heading.mode` 默认速率式（`"rate"`）； +相机组原 `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`，后台解析新配置成功后原子换指针， +控制线程每拍读取当前指针快照使用（当前未实现，不引入额外复杂度）。 + +### 3.8 主控律单周期流程 + +`Update()` 按固定流程执行（`last_now_ms_` 推算 dt，首拍与异常节拍 +（时钟回拨/`now_ms` 未前进）用 `1.0 / control.frequency_hz` 兜底， +dt 上限 `kMaxDtS = 0.5` 秒）： + +① 超龄判定（姿态过期/缺失 → `kDegradedHold`；视觉超龄 → `kDegradedHold`） +→ ② 判丢刹车（清积分与限幅器历史）→ ③ Coast 清距离积分 +→ ④ 像素偏移 → β/α（针孔模型）→ ⑤ 水平航向（按 `HeadingMode` 分支） +→ ⑥ 垂直统一速度支路（α → `vz_gain·α` → 加速度限幅 → 速度限幅） +→ ⑦ 距离通道（关联成立且未超龄才可用；否则清积分并按 +`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(当前航向 + 限幅修正量)`；速率式 + （默认）输出角速度指令，天然相对量、无此问题。 + +### 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 无距离降级路径（hold： + 前向清零）； +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 标志、速率/位置两种航向模式（含绝对航向回绕）、垂直支路与限幅、距离通道四种降级动作、雷达超龄回落、首拍变化率限幅（S6）、NED 旋转、`Reset` 后从零渐变 | +| `vtc_config_test.cpp` | 真实配置加载、position 模式可接受、缺文件/缺字段/缺分组/类型错/枚举非法/数值越界均抛异常且消息携带字段名 | +| `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. **雷达关联标志来源**：`associated_to_target` 必须由感知融合层按 + 规格 §6.3 / 需求分析 FR-041 的判据给出，控制律不自行关联。 + +### 6.4 红线声明 + +- `config/track_control.json` 与 `TrackingConfig` 默认值中**所有数值均为 + 占位值**（对应规格第 8 章待定项与需求分析 §7 未标定前提）：相机内参/ + 畸变/外参未标定、PID 与限幅参数未整定。标定与实测整定完成前，本库 + **仅限 SITL/台架录制数据回放验证数学链路，不得用于实飞**。 +- 构建环境差异：`../AGENTS.md` 规定 WSL2 Ubuntu 24.04，当前开发机仅 + 装有 Ubuntu 22.04，验证在 22.04 完成；安装 24.04 后需复核构建与测试。 diff --git a/utils/README.md b/utils/README.md new file mode 100644 index 0000000..074af5b --- /dev/null +++ b/utils/README.md @@ -0,0 +1,58 @@ +# utils —— reference-c++ 两个子项目的 Python 移植 + +将 `reference-c++/` 下的 `target_tracker`（多目标追踪过滤器）与 +`visual_track_control`（视觉跟踪控制律）移植为 Python，**两包互不依赖** +（唯一的数据转换点在胶水层 demo）。 + +## 目录结构 + +``` +utils/ +├── target_tracker/ # 包 1：卡尔曼滤波多目标追踪（无第三方硬依赖，仅 numpy） +│ └── README.md # 接口清单 + C++ 差异 +├── visual_track_control/ # 包 2：三通道控制律 + MAVSDK offboard 网关 +│ └── README.md # 接口清单 + C++ 差异 +├── demo_track_control.py # 胶水层集成演示（模拟检测 → 追踪 → 控制律 → 发送） +├── config/track_control.json # 控制律配置（拷贝自 reference-c++） +├── tests/ # pytest 测试（对应 C++ 全部 13 个测试文件） +│ ├── target_tracker/ # 27 条 +│ └── visual_track_control/ # 78 条 +├── conftest.py # 保证可从任意工作目录导入两包 +└── pytest.ini # 收集 *_test.py 命名（与 C++ 测试文件名对齐） +``` + +## 环境 + +conda env `pytorch`（`D:\program file\conda\envs\pytorch`），Python 3.12， +依赖：numpy、mavsdk、pytest。 + +## 运行测试 + +```bash +cd utils +"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q +``` + +105 条测试全部通过（含 C++ 验收用例的逐条移植：§9.4 六条硬验收、A1/A2/A4 +回归、PID anti-windup C1、超龄 C4、首拍限幅 S6）。 + +## 运行演示 + +```bash +# mock 传输（本地记录指令，无实机） +python utils/demo_track_control.py --speedup 10 + +# MAVSDK 实发（需飞控/SITL 在 udp://:14540） +python utils/demo_track_control.py --transport mavsdk +``` + +时间线 13 s（20 Hz）：正常跟踪 6 s（vx 从零渐变 → 逼近限速 3 m/s）→ +目标消失 4 s（Coast 预测，60 帧后轨道判亡）→ 判零速刹车 3 s。 + +## 移植约定 + +- MAVLink 部分改为 **MAVSDK 语义映射**（offboard API，非字节透传），三分支 + 映射保留 C++ A2 回归语义，详见包 2 README。 +- 测试逐条移植 C++ gtest 断言（`EXPECT_DOUBLE_EQ` → 精确 `==`， + `EXPECT_NEAR` → `pytest.approx`）；MAVSDK 部分因无 SITL/实机，仅 mock 验证。 +- 行为修复项（C1/C2/C4、A1/A2/A4、S6）在测试文件头注释中标注对应章节。 diff --git a/utils/__pycache__/conftest.cpython-312-pytest-9.1.1.pyc b/utils/__pycache__/conftest.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..a8a06ab Binary files /dev/null and b/utils/__pycache__/conftest.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/config/track_control.json b/utils/config/track_control.json new file mode 100644 index 0000000..7dcd26b --- /dev/null +++ b/utils/config/track_control.json @@ -0,0 +1,48 @@ +{ + "_comment": "视觉跟踪控制律配置。全部数值为占位示例（对应 docs/物理追踪思路.md 第 8 章待定项），键名后缀标注单位；标定完成前仅限回放验证，不得用于实飞。", + + "control": { + "frequency_hz": 20.0, + "visual_stale_ms": 200, + "radar_stale_ms": 500, + "attitude_stale_ms": 300 + }, + + "camera": { + "image_width": 1920, + "image_height": 1080, + "fov_h_deg": 60.0, + "fov_v_deg": 35.0 + }, + + "heading": { + "mode": "rate", + "gain": 1.0, + "yaw_rate_limit_dps": 30.0, + "yaw_slew_limit_dps2": 60.0 + }, + + "vertical": { + "vz_gain_mps_per_deg": 0.1, + "vz_limit_mps": 2.0 + }, + + "distance": { + "d_exp_m": 10.0, + "kp": 0.5, + "ki": 0.05, + "kd": 0.1, + "integral_limit": 2.0, + "derivative_filter_coef": 0.2, + "approach_velocity_limit_mps": 3.0, + "retreat_velocity_limit_mps": 1.0, + "no_distance_action": "hold", + "no_distance_approach_limit_mps": 0.5 + }, + + "accel_limit": { + "ax_mps2": 2.0, + "ay_mps2": 2.0, + "az_mps2": 1.5 + } +} diff --git a/utils/conftest.py b/utils/conftest.py new file mode 100644 index 0000000..e86fb96 --- /dev/null +++ b/utils/conftest.py @@ -0,0 +1,8 @@ +"""pytest 根配置：保证无论从哪个工作目录启动 pytest，均可导入 utils/ 下的两个包。""" + +import sys +from pathlib import Path + +_UTILS_DIR = Path(__file__).resolve().parent +if str(_UTILS_DIR) not in sys.path: + sys.path.insert(0, str(_UTILS_DIR)) diff --git a/utils/demo_track_control.py b/utils/demo_track_control.py new file mode 100644 index 0000000..213e0e6 --- /dev/null +++ b/utils/demo_track_control.py @@ -0,0 +1,207 @@ +"""demo_track_control.py —— 视觉跟踪控制链路集成演示（胶水层）。 + +对应 C++ 参考实现 reference-c++/visual_track_control/examples/track_control_demo.cpp。 + +串联三个组件（target_tracker 与 visual_track_control 互不 import， +本脚本是唯一的转换点）： + 模拟检测序列 → TargetTracker（8 维恒速卡尔曼多目标追踪） + → TrackResult 转 TrackState（解耦边界） + → TrackingControlLaw（三通道控制律 + 限幅 + 状态机） + → CommandSender（mock 记录 / MAVSDK offboard 实发） + +场景时间线（20 Hz，--speedup 可加速）： + 0~ 6s 正常跟踪：目标匀速横移 + 雷达距离 30→10 m 前向逼近 + 6~10s 目标消失：追踪器 Coast 预测（60 帧 = 3 s 后轨道判丢， + 控制律自动切换零速刹车） + 10~13s 持续判丢：维持零速刹车 + +用法： + python utils/demo_track_control.py # mock 传输 + python utils/demo_track_control.py --transport mavsdk # MAVSDK（需飞控/SITL） + python utils/demo_track_control.py --speedup 4 # 4 倍速 +""" + +import argparse +import asyncio +import logging +from pathlib import Path + +from target_tracker.target_tracker import Detection, TargetTracker, TrackerConfig +from visual_track_control.config import load_from_json +from visual_track_control.mavsdk_gateway import MavsdkCommandGateway +from visual_track_control.tracking_control_law import TrackingControlLaw +from visual_track_control.types import (ControlMode, ControlSnapshot, + ControlOutput, TrackState) + +logger = logging.getLogger("demo") + +_DEMO_DIR = Path(__file__).resolve().parent +_DEFAULT_CONFIG = _DEMO_DIR / "config" / "track_control.json" + +# 模拟场景参数 +_NORMAL_SECS = 6.0 # 正常跟踪阶段 +_COAST_SECS = 4.0 # 目标消失 → Coast（含轨道判亡） +_LOST_SECS = 3.0 # 判丢后持续刹车阶段 +_DEMO_MAX_LOST_FRAMES = 60 # 追踪器 Coast 窗口（20 Hz 下 = 3 s） +_STATUS_EVERY_TICKS = 10 # 每 0.5 s 打印一行状态 + + +def track_result_to_state(result) -> TrackState: + """两包之间的唯一转换点：TrackResult（box 为对象）→ TrackState（plain tuple）。""" + box = result.box + raw = result.raw + return TrackState( + tracked=result.tracked, + primary_id=result.primary_id, + center=result.center, + box=(box.x1, box.y1, box.x2, box.y2) if box is not None else None, + confidence=result.confidence, + is_predicted=result.is_predicted, + lost_frames=result.lost_frames, + n_active=result.n_active, + raw=(raw.x1, raw.y1, raw.x2, raw.y2) if raw is not None else None, + ) + + +def simulate_detections(tick: int, normal_ticks: int, period_s: float): + """场景检测源：正常阶段目标匀速横移，之后消失。""" + if tick >= normal_ticks: + return [] + cx = 960.0 + 3.0 * tick # 3 px/帧 匀速右移 + cy = 540.0 + w, h = 120.0, 160.0 + return [Detection(cx - w / 2.0, cy - h / 2.0, cx + w / 2.0, cy + h / 2.0, 0.9)] + + +def simulate_radar(tick: int, normal_ticks: int, period_s: float): + """场景雷达源：正常阶段关联成立、距离 30→10 m；之后未关联。""" + if tick >= normal_ticks: + return 0.0, False + distance = max(10.0, 30.0 - tick * period_s * 3.0) + return distance, True + + +class MockCommandSender: + """CommandSender 的 mock 实现：记录最近一条指令描述（无实机验证能力）。""" + + def __init__(self) -> None: + self.last_desc = "-" + + async def start(self) -> bool: + logger.info("[MockTransport] offboard 启动（模拟）") + return True + + async def send_velocity_heading(self, output: ControlOutput, + time_boot_ms=None) -> bool: + self.last_desc = (f"vx={output.vx_mps:+.2f} vy={output.vy_mps:+.2f} " + f"vz={output.vz_mps:+.2f} " + f"yaw_rate={output.yaw_rate_dps:+.2f}°/s " + f"yaw={output.yaw_deg:+.2f}°") + return True + + async def send_brake_hover(self, time_boot_ms=None) -> bool: + self.last_desc = "零速刹车（全零机体速度设定点）" + return True + + async def stop(self) -> None: + logger.info("[MockTransport] offboard 停止（模拟）") + + +async def make_sender(args) -> object: + if args.transport == "mock": + return MockCommandSender() + from mavsdk import System + drone = System() + logger.info("连接飞控 %s ...", args.conn) + await drone.connect(system_address=args.conn) + async for state in drone.core.connection_state(): + if state.is_connected: + logger.info("飞控已连接") + break + return MavsdkCommandGateway(drone) + + +async def dispatch(sender, out: ControlOutput, now_ms: int): + """按模式分发指令；DEGRADED_HOLD 不发送（禁止正常控制）。""" + if out.mode is ControlMode.VELOCITY_HEADING and out.valid: + return await sender.send_velocity_heading(out, now_ms) + if out.mode is ControlMode.BRAKE_HOVER and out.valid: + return await sender.send_brake_hover(now_ms) + return None + + +async def run_demo(args) -> None: + cfg = load_from_json(args.config) + period_s = 1.0 / cfg.control.frequency_hz + period_ms = int(round(period_s * 1000.0)) + + tracker = TargetTracker(TrackerConfig(max_lost_frames=_DEMO_MAX_LOST_FRAMES)) + law = TrackingControlLaw(cfg) + sender = await make_sender(args) + + normal_ticks = int(round(_NORMAL_SECS / period_s)) + coast_ticks = int(round(_COAST_SECS / period_s)) + lost_ticks = int(round(_LOST_SECS / period_s)) + total_ticks = normal_ticks + coast_ticks + lost_ticks + + if not await sender.start(): + logger.error("offboard 启动失败，退出") + return + + logger.info("开始 %d 拍（%.1f s，%.1f Hz）：正常 %.1fs → Coast %.1fs → 判丢 %.1fs", + total_ticks, total_ticks * period_s, cfg.control.frequency_hz, + _NORMAL_SECS, _COAST_SECS, _LOST_SECS) + try: + for tick in range(total_ticks): + now_ms = 1000 + tick * period_ms + detections = simulate_detections(tick, normal_ticks, period_s) + radar_m, associated = simulate_radar(tick, normal_ticks, period_s) + + result = tracker.update(detections) + snap = ControlSnapshot( + track=track_result_to_state(result), + track_time_ms=now_ms, # 每拍取样即打戳（视觉始终新鲜） + radar_distance_m=radar_m, + associated_to_target=associated, + radar_time_ms=now_ms if associated else 0, + attitude_present=True, + attitude_time_ms=now_ms, # 模拟姿态源持续可用 + yaw_rad=0.0, # 机头朝北（NED 旋转恒等） + ) + out = law.update(snap, now_ms) + await dispatch(sender, out, now_ms) + + if tick % _STATUS_EVERY_TICKS == 0: + logger.info( + "t=%5.2fs 检测=%d 追踪[trk=%d pred=%d lost=%d] | " + "[%s] vx=%+.2f vy=%+.2f vz=%+.2f yaw_rate=%+.2f coast=%d | 发送: %s", + (tick * period_s), len(detections), result.tracked, + result.is_predicted, result.lost_frames, out.mode.name, + out.vx_mps, out.vy_mps, out.vz_mps, out.yaw_rate_dps, + out.coast_active, sender.last_desc) + await asyncio.sleep(period_s / args.speedup) + finally: + await sender.stop() + logger.info("演示结束") + + +def main() -> None: + parser = argparse.ArgumentParser(description="视觉跟踪控制链路集成演示") + parser.add_argument("--transport", choices=["mock", "mavsdk"], default="mock", + help="指令发送方式：mock=本地记录，mavsdk=offboard 实发") + parser.add_argument("--conn", default="udp://:14540", + help="MAVSDK 连接串（仅 --transport mavsdk 生效）") + parser.add_argument("--speedup", type=float, default=1.0, + help="时间倍速（>1 加速演示）") + parser.add_argument("--config", default=str(_DEFAULT_CONFIG), + help="控制律配置文件路径") + args = parser.parse_args() + + logging.basicConfig(level=logging.INFO, + format="%(asctime)s.%(msecs)03d %(levelname)s %(message)s", + datefmt="%H:%M:%S") + asyncio.run(run_demo(args)) + + +if __name__ == "__main__": + main() diff --git a/utils/pytest.ini b/utils/pytest.ini new file mode 100644 index 0000000..eb6a8ff --- /dev/null +++ b/utils/pytest.ini @@ -0,0 +1,3 @@ +[pytest] +testpaths = tests +python_files = *_test.py test_*.py diff --git a/utils/target_tracker/README.md b/utils/target_tracker/README.md new file mode 100644 index 0000000..9344d6e --- /dev/null +++ b/utils/target_tracker/README.md @@ -0,0 +1,29 @@ +# target_tracker（Python 版） + +多目标追踪过滤器：8 维恒速卡尔曼滤波 + 贪婪关联 + 主目标锁定选举。 +移植自 `reference-c++/target_tracker/`（规格：docs/视觉追踪技术路线.md）。 + +## 接口清单 + +| 名称 | 说明 | +| --- | --- | +| `Detection(x1, y1, x2, y2, conf, cls_id=0)` | 单帧检测框输入 | +| `FrameShape(height, width)` | 画面尺寸（仅 Coast 兜底选举取画面中心用） | +| `TrackResult` | 每帧输出九字段：tracked / primary_id / center / box / confidence / is_predicted / lost_frames / n_active / raw | +| `TrackerConfig(max_lost_frames=8, max_association_dist=200.0, min_hits=3, dist_weight=0.5, iou_weight=0.5)` | 追踪器配置 | +| `TargetTracker(config=None)` | 追踪器主类 | +| `.update(detections, frame_shape=None) -> TrackResult` | 单帧主流程（§7 六步） | +| `.last_result() -> TrackResult` | 跨线程读取最近一帧结果（加锁拷贝） | +| `.reset()` / `.active_count()` / `.active_tracks()` / `.get_config()` | 生命周期与查询 | +| `KalmanBoxFilter` / `BoxCXCYWH` | 单目标滤波器（可独立使用：initiate / predict / update） | +| `Tracklet` / `BoxXYXY` / `to_center_size` / `to_xyxy` / `compute_iou` | 轨道与几何工具 | + +## 与 C++ 版的差异 + +- **依赖**：矩阵运算用 numpy（C++ 手写定长运算避免 Eigen）；关联/选举路径用纯标量，与 C++ 逐位同序。 +- **Optional 语义**：C++ `std::optional` → Python `None`；`State()` → `state` 属性（返回拷贝）。 +- **线程模型**：与 C++ 一致——仅 `last_result()` 加 `threading.Lock`，`update()` 单线程调用。 +- **字段命名**：成员函数蛇形命名（`IsDead` → `is_dead` 属性，`MarkMiss` → `mark_miss`）。 +- 行为语义（死亡判定 `lost_count > max_lost` 严格大于、best_cost=1.0 严格小于、 + ID 严格递增不复用、Q 用状态原值 / R 用 abs×2 的刻意不对称、S 求逆前 1e-9 正则化） + 与 C++ 完全一致，由 27 条移植测试锁定。 diff --git a/utils/target_tracker/__init__.py b/utils/target_tracker/__init__.py new file mode 100644 index 0000000..1f020b8 --- /dev/null +++ b/utils/target_tracker/__init__.py @@ -0,0 +1,15 @@ +"""target_tracker —— 轻量级多目标追踪过滤器（C++ 版 reference-c++/target_tracker 的 Python 移植）。 + +纯算法包，零第三方运行依赖之外的约束：仅依赖 numpy，不依赖 visual_track_control。 +""" + +from .kalman_box_filter import STD_POS, STD_VEL, INI_POS, INI_VEL, R_SCALE, BoxCXCYWH, KalmanBoxFilter +from .target_tracker import Detection, FrameShape, TargetTracker, TrackerConfig, TrackResult +from .tracklet import BoxXYXY, Tracklet, compute_iou, to_center_size, to_xyxy + +__all__ = [ + "STD_POS", "STD_VEL", "INI_POS", "INI_VEL", "R_SCALE", + "BoxCXCYWH", "BoxXYXY", "KalmanBoxFilter", "Tracklet", + "Detection", "FrameShape", "TargetTracker", "TrackerConfig", "TrackResult", + "compute_iou", "to_center_size", "to_xyxy", +] diff --git a/utils/target_tracker/__pycache__/__init__.cpython-312.pyc b/utils/target_tracker/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 0000000..92aaace Binary files /dev/null and b/utils/target_tracker/__pycache__/__init__.cpython-312.pyc differ diff --git a/utils/target_tracker/__pycache__/kalman_box_filter.cpython-312.pyc b/utils/target_tracker/__pycache__/kalman_box_filter.cpython-312.pyc new file mode 100644 index 0000000..f01aa75 Binary files /dev/null and b/utils/target_tracker/__pycache__/kalman_box_filter.cpython-312.pyc differ diff --git a/utils/target_tracker/__pycache__/target_tracker.cpython-312.pyc b/utils/target_tracker/__pycache__/target_tracker.cpython-312.pyc new file mode 100644 index 0000000..72411a1 Binary files /dev/null and b/utils/target_tracker/__pycache__/target_tracker.cpython-312.pyc differ diff --git a/utils/target_tracker/__pycache__/tracklet.cpython-312.pyc b/utils/target_tracker/__pycache__/tracklet.cpython-312.pyc new file mode 100644 index 0000000..f15a0b9 Binary files /dev/null and b/utils/target_tracker/__pycache__/tracklet.cpython-312.pyc differ diff --git a/utils/target_tracker/kalman_box_filter.py b/utils/target_tracker/kalman_box_filter.py new file mode 100644 index 0000000..3a9be8b --- /dev/null +++ b/utils/target_tracker/kalman_box_filter.py @@ -0,0 +1,111 @@ +"""单目标 8 维恒定速度卡尔曼滤波器。 + +对应 C++ 参考实现 reference-c++/target_tracker（规格：docs/视觉追踪技术路线.md §4）： +状态 x[8] = [cx, cy, w, h, vx, vy, vw, vh]，观测 z[4] = [cx, cy, w, h]，dt = 1 帧。 +矩阵运算用 numpy 表达（C++ 版为避免 Eigen 手写定长运算，此处等价）。 +""" + +from dataclasses import dataclass + +import numpy as np + +# §4.2 / §8.2 内部常量（经验值，针对 640×480 画面调优） +STD_POS = 1.0 / 50.0 # 位置噪声比例系数 +STD_VEL = 1.0 / 200.0 # 速度噪声比例系数 +INI_POS = 2.0 # 初始位置协方差 +INI_VEL = 100.0 # 初始速度协方差 +R_SCALE = 2.0 # R 额外缩放因子（增强平滑） +_S_REG = 1e-9 # S 求逆前的对角正则化（§9.3） + +# 状态转移矩阵 F（§4.1）：I4 + 右上 I4 +_F = np.eye(8, dtype=float) +_F[0:4, 4:8] = np.eye(4) + +# 观测矩阵 H（§4.1）：[I4 | 0]，直接观测位置与尺寸、不观测速度 +_H_COLS = slice(0, 4) + + +@dataclass +class BoxCXCYWH: + """轴对齐边界框（中心点 + 宽高表示，卡尔曼观测空间）。""" + + cx: float = 0.0 + cy: float = 0.0 + w: float = 0.0 + h: float = 0.0 + + +class KalmanBoxFilter: + def __init__(self) -> None: + self._x = np.zeros(8, dtype=float) + self._p = np.zeros((8, 8), dtype=float) + self._initialized = False + + @property + def initialized(self) -> bool: + return self._initialized + + @property + def state(self) -> np.ndarray: + """状态向量 [cx, cy, w, h, vx, vy, vw, vh]（拷贝）。""" + return self._x.copy() + + def initiate(self, z: BoxCXCYWH) -> None: + # §4.3：速度置零；P = diag(2,2,2,2,100,100,100,100) + self._x = np.array([z.cx, z.cy, z.w, z.h, 0.0, 0.0, 0.0, 0.0]) + self._p = np.diag([INI_POS] * 4 + [INI_VEL] * 4) + self._initialized = True + + def predict(self) -> BoxCXCYWH | None: + # §4.4：未初始化返回 None + if not self._initialized: + return None + + # §4.2 动态过程噪声：Q = diag(σ_pos², σ_vel²)，σ 按状态中 w/h 缩放。 + # Q 直接使用状态值、不做 abs，与 R 的 abs 处理刻意不同（规格 §4.2 注记）。 + w = self._x[2] + h = self._x[3] + std_pos = np.array([w * STD_POS, h * STD_POS, w * STD_POS, h * STD_POS]) + std_vel = np.array([w * STD_VEL, h * STD_VEL, w * STD_VEL, h * STD_VEL]) + q = np.zeros((8, 8), dtype=float) + q[np.arange(4), np.arange(4)] = std_pos * std_pos + q[np.arange(4) + 4, np.arange(4) + 4] = std_vel * std_vel + + # §4.4：x ← F·x；P ← F·P·Fᵀ + Q + self._x = _F @ self._x + self._p = _F @ self._p @ _F.T + q + return BoxCXCYWH(float(self._x[0]), float(self._x[1]), + float(self._x[2]), float(self._x[3])) + + def update(self, z: BoxCXCYWH) -> None: + # §4.5：未初始化直接返回 + if not self._initialized: + return + + # §4.2 动态观测噪声：R 用 abs(w)/abs(h) 缩放并额外乘 R_SCALE + w = abs(self._x[2]) + h = abs(self._x[3]) + rw = R_SCALE * (w * STD_POS) * (w * STD_POS) + rh = R_SCALE * (h * STD_POS) * (h * STD_POS) + r = np.diag([rw, rh, rw, rh]) + + # §4.5：S = H·P·Hᵀ + R。H = [I4|0]，故 H·P·Hᵀ 即 P 左上 4×4 子块 + s = self._p[:4, :4] + r + try: + s_inv = np.linalg.inv(s + _S_REG * np.eye(4)) + except np.linalg.LinAlgError: + return # 求逆失败：防御性跳过本次更新（C++ 版同样跳过） + + # §4.5：K = P·Hᵀ·S⁻¹，P·Hᵀ 即 P 的前 4 列（8×4） + k = self._p[:, _H_COLS] @ s_inv + + # 残差 y = z − H·x（H·x 即状态前 4 维） + y = np.array([z.cx, z.cy, z.w, z.h]) - self._x[:4] + + # x ← x + K·y + self._x = self._x + k @ y + + # P ← (I8 − K·H)·P：K·H 是 K 的前 4 列铺在前 4 列上 + kh = np.zeros((8, 8), dtype=float) + kh[:, :4] = k + self._p = (np.eye(8) - kh) @ self._p diff --git a/utils/target_tracker/target_tracker.py b/utils/target_tracker/target_tracker.py new file mode 100644 index 0000000..340e3fd --- /dev/null +++ b/utils/target_tracker/target_tracker.py @@ -0,0 +1,269 @@ +"""多目标追踪过滤器（TargetTracker）。 + +对应 C++ 参考实现 reference-c++/target_tracker/src/target_tracker.cpp +（规格：docs/视觉追踪技术路线.md §5 关联 / §6 生命周期与锁定 / §7 六步主流程）。 + +线程模型（§9.5）：update() 由单线程（推理主循环）调用；仅对 last_result +缓存加锁，last_result() 返回受保护拷贝。 +""" + +import math +import threading +from dataclasses import dataclass, field +from typing import Optional, Sequence + +from .kalman_box_filter import BoxCXCYWH +from .tracklet import BoxXYXY, Tracklet, compute_iou, to_center_size + + +@dataclass +class Detection: + """单个检测框（§2.2 输入元素：[x1,y1,x2,y2,conf,cls_id]）。""" + + x1: float = 0.0 + y1: float = 0.0 + x2: float = 0.0 + y2: float = 0.0 + conf: float = 0.0 + cls_id: int = 0 + + +@dataclass +class FrameShape: + """画面尺寸（height, width）：仅 Coast 兜底选举时用于取画面中心。""" + + height: int = 0 + width: int = 0 + + +@dataclass +class TrackResult: + """每帧追踪输出（§2.3 九字段）。""" + + tracked: bool = False + primary_id: Optional[int] = None + center: tuple[float, float] = (0.0, 0.0) + box: Optional[BoxXYXY] = None + confidence: float = 0.0 + is_predicted: bool = False + lost_frames: int = 0 + n_active: int = 0 + raw: Optional[BoxXYXY] = None + + +@dataclass +class TrackerConfig: + """追踪器配置参数（§8.1 默认值取自规格）。""" + + max_lost_frames: int = 8 + max_association_dist: float = 200.0 + min_hits: int = 3 + dist_weight: float = 0.5 + iou_weight: float = 0.5 + + +@dataclass +class _AssociationResult: + matches: list[tuple[int, int]] = field(default_factory=list) + unmatched_track_ids: list[int] = field(default_factory=list) + unmatched_det_indices: list[int] = field(default_factory=list) + + +class TargetTracker: + def __init__(self, config: Optional[TrackerConfig] = None) -> None: + self._config = config if config is not None else TrackerConfig() + self._next_id = 0 + self._tracks: dict[int, Tracklet] = {} + self._frame_count = 0 + self._locked_primary_id: Optional[int] = None + self._last_result = TrackResult() + self._last_result_mutex = threading.Lock() + + @property + def config(self) -> TrackerConfig: + return self._config + + def get_config(self) -> TrackerConfig: + return self._config + + def update(self, detections: Sequence[Detection], + frame_shape: Optional[FrameShape] = None) -> TrackResult: + # §7 六步主流程 + self._frame_count += 1 + + # 入口防御：过滤坐标或置信度含 NaN/Inf 的检测（非有限值一旦进入 + # 卡尔曼状态将永久污染轨道并传导到主目标输出） + valid_detections = [ + det for det in detections + if all(math.isfinite(v) for v in (det.x1, det.y1, det.x2, det.y2, det.conf)) + ] + + # 步骤 1：所有存活轨道先行预测（推进一帧） + for track in self._tracks.values(): + track.predict() + + # 步骤 2：贪婪关联（无检测时全部轨道进入 Coast 计数） + if valid_detections: + assoc = self._associate_detections(valid_detections) + else: + assoc = _AssociationResult(unmatched_track_ids=list(self._tracks.keys())) + + # 步骤 3：用匹配到的检测更新轨道 + for track_id, det_idx in assoc.matches: + det = valid_detections[det_idx] + raw_box = BoxXYXY(det.x1, det.y1, det.x2, det.y2) + self._tracks[track_id].update(raw_box, det.conf, det.cls_id) + + # 步骤 4：未匹配轨道计丢失，死亡者回收（锁定唯一解除途径之一） + for track_id in assoc.unmatched_track_ids: + track = self._tracks.get(track_id) + if track is None: + continue + track.mark_miss() + if track.is_dead: + if self._locked_primary_id == track_id: + self._locked_primary_id = None + del self._tracks[track_id] + + # 步骤 5：未匹配检测 → 创建新轨道（next_id 从 0 递增永不复用） + for det_idx in assoc.unmatched_det_indices: + det = valid_detections[det_idx] + measurement = to_center_size(BoxXYXY(det.x1, det.y1, det.x2, det.y2)) + self._tracks[self._next_id] = Tracklet( + self._next_id, measurement, det.conf, det.cls_id, + self._config.max_lost_frames) + self._next_id += 1 + + # 步骤 6：选取/延续主目标，组装输出并缓存 + result = self._select_primary(frame_shape) + with self._last_result_mutex: + self._last_result = result + return result + + def last_result(self) -> TrackResult: + with self._last_result_mutex: + return self._last_result + + def reset(self) -> None: + # §2.1：清空所有轨道、ID 计数器、锁定状态与缓存结果（next_id 归零） + self._tracks.clear() + self._next_id = 0 + self._frame_count = 0 + self._locked_primary_id = None + with self._last_result_mutex: + self._last_result = TrackResult() + + def active_count(self) -> int: + return len(self._tracks) + + def active_tracks(self) -> dict[int, Tracklet]: + return dict(self._tracks) + + def _associate_detections(self, detections: Sequence[Detection]) -> _AssociationResult: + result = _AssociationResult() + + # §5.3：按轨道 confidence 降序（平局按 ID 升序保证确定性）。 + # 非有限置信度按 −∞ 处理（排最后），避免 NaN 破坏全序。 + def sort_key(track_id: int) -> tuple[float, int]: + raw = self._tracks[track_id].confidence + conf = raw if math.isfinite(raw) else -math.inf + return (-conf, track_id) + + sorted_ids = sorted(self._tracks.keys(), key=sort_key) + det_assigned = [False] * len(detections) + + for track_id in sorted_ids: + track = self._tracks[track_id] + # §5.3：最优代价初始 1.0，仅 cost 严格小于才匹配（cost ≥ 1.0 判为不可匹配） + best_cost = 1.0 + best_det = -1 + for d, det in enumerate(detections): + if det_assigned[d]: + continue + det_cx = (det.x1 + det.x2) / 2.0 + det_cy = (det.y1 + det.y2) / 2.0 + dist = math.hypot(track.last_center[0] - det_cx, + track.last_center[1] - det_cy) + norm_dist = dist / max(self._config.max_association_dist, 1e-3) + iou = (compute_iou(track.last_box, + BoxXYXY(det.x1, det.y1, det.x2, det.y2)) + if track.last_box is not None else 0.0) + cost = (self._config.dist_weight * norm_dist + + self._config.iou_weight * (1.0 - iou)) + if cost < best_cost: + best_cost = cost + best_det = d + if best_det >= 0: + result.matches.append((track_id, best_det)) + det_assigned[best_det] = True + else: + result.unmatched_track_ids.append(track_id) + + result.unmatched_det_indices = [ + d for d, assigned in enumerate(det_assigned) if not assigned + ] + return result + + def _select_primary(self, frame_shape: Optional[FrameShape]) -> TrackResult: + result = TrackResult(n_active=len(self._tracks)) + + # §6.3：轨道表为空 → 清除锁定，返回 tracked=false + if not self._tracks: + self._locked_primary_id = None + return result + + # §6.3 ① 锁定延续：锁定轨道存活则直接输出（Coast 中照样输出， + # 不被高置信度目标抢占） + chosen: Optional[Tracklet] = None + is_predicted = False + if self._locked_primary_id is not None: + track = self._tracks.get(self._locked_primary_id) + if track is not None: + chosen = track + is_predicted = track.is_coasting + + # §6.3 ② 重新选举（无锁定 / 锁定目标已死亡被回收） + if chosen is None: + measured = [t for t in self._tracks.values() if not t.is_coasting] + coasting = [t for t in self._tracks.values() if t.is_coasting] + + if measured: + # confirmed ← measured 中 hits ≥ min_hits 者；为空则退回 measured + # （min_hits 只影响候选池优先级，不阻止输出） + pool = [t for t in measured if t.hits >= self._config.min_hits] + if not pool: + pool = measured + chosen = pool[0] + for t in pool: + if t.confidence > chosen.confidence: + chosen = t + is_predicted = False + elif coasting: + # 无实测轨道：选离画面中心最近者（缺省按 640×480 中心 (320,240)） + center_x = frame_shape.width / 2.0 if frame_shape and frame_shape.width > 0 else 320.0 + center_y = frame_shape.height / 2.0 if frame_shape and frame_shape.height > 0 else 240.0 + best_dist_sq = math.inf + for t in coasting: + dx = t.last_center[0] - center_x + dy = t.last_center[1] - center_y + dist_sq = dx * dx + dy * dy + if dist_sq < best_dist_sq: + best_dist_sq = dist_sq + chosen = t + is_predicted = True + else: + # 理论上不可达（轨道非空则 measured/coasting 必居其一）；防御清锁 + self._locked_primary_id = None + return result + self._locked_primary_id = chosen.track_id # 选举后锁定 + + # 组装输出（§2.3 九字段）；raw 仅实测时有值，Coast 时为空 + result.tracked = True + result.primary_id = chosen.track_id + result.center = chosen.last_center + result.box = chosen.last_box + result.confidence = chosen.confidence + result.is_predicted = is_predicted + result.lost_frames = chosen.lost_count + result.raw = None if is_predicted else chosen.last_raw_box + return result diff --git a/utils/target_tracker/tracklet.py b/utils/target_tracker/tracklet.py new file mode 100644 index 0000000..87c6b0f --- /dev/null +++ b/utils/target_tracker/tracklet.py @@ -0,0 +1,110 @@ +"""轨道（Tracklet）与基础几何工具。 + +对应 C++ 参考实现 reference-c++/target_tracker/include/target_tracker/tracklet.h +（规格：docs/视觉追踪技术路线.md §3.2 / §5.2 / §7）。 +""" + +from dataclasses import dataclass +from typing import Optional + +from .kalman_box_filter import BoxCXCYWH, KalmanBoxFilter + + +@dataclass +class BoxXYXY: + """轴对齐边界框（像素坐标，左上/右下角表示）。""" + + x1: float = 0.0 + y1: float = 0.0 + x2: float = 0.0 + y2: float = 0.0 + + +def to_center_size(box: BoxXYXY) -> BoxCXCYWH: + # §7 TRACKLET_UPDATE：cx=(x1+x2)/2；cy=(y1+y2)/2；w=x2−x1；h=y2−y1 + return BoxCXCYWH((box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0, + box.x2 - box.x1, box.y2 - box.y1) + + +def to_xyxy(box: BoxCXCYWH) -> BoxXYXY: + # §7 TRACKLET_PREDICT 的 center_size 逆变换 + return BoxXYXY(box.cx - box.w / 2.0, box.cy - box.h / 2.0, + box.cx + box.w / 2.0, box.cy + box.h / 2.0) + + +def compute_iou(a: BoxXYXY, b: BoxXYXY) -> float: + """标准轴对齐 IoU（§5.2）：任一框为空、交集 ≤ 0 或并集 ≤ 0 时返回 0。""" + area_a = (a.x2 - a.x1) * (a.y2 - a.y1) + area_b = (b.x2 - b.x1) * (b.y2 - b.y1) + if area_a <= 0.0 or area_b <= 0.0: + return 0.0 + + inter_w = min(a.x2, b.x2) - max(a.x1, b.x1) + inter_h = min(a.y2, b.y2) - max(a.y1, b.y1) + if inter_w <= 0.0 or inter_h <= 0.0: + return 0.0 + + inter_area = inter_w * inter_h + union_area = area_a + area_b - inter_area + if union_area <= 0.0: + return 0.0 + return inter_area / union_area + + +class Tracklet: + """单条轨道：生命周期计数 + 辅助状态 + 卡尔曼滤波器（§3.2）。 + + 构造即创建轨道（§7 步骤 5）：用首次观测初始化滤波器，age=1、hits=1、 + lost_count=0，last_center / last_box 立即由观测重建（创建当帧即可参与输出）。 + """ + + def __init__(self, track_id: int, measurement: BoxCXCYWH, conf: float, + cls_id: int, max_lost_frames: int) -> None: + self.track_id = track_id + self.kf = KalmanBoxFilter() + self.lost_count = 0 + self.max_lost = max_lost_frames + self.confidence = conf + self.cls_id = cls_id + self.age = 1 + self.hits = 1 + self.last_center: tuple[float, float] = (measurement.cx, measurement.cy) + self.last_box: Optional[BoxXYXY] = to_xyxy(measurement) + # 创建即源于实测检测：同步记录原始检测框（Coast 期间保留不更新） + self.last_raw_box: Optional[BoxXYXY] = self.last_box + self.kf.initiate(measurement) + + @property + def is_dead(self) -> bool: + # §6.1 注记 2：lost_count > max_lost（严格大于） + return self.lost_count > self.max_lost + + @property + def is_coasting(self) -> bool: + # §3.2：lost_count > 0 即处于纯预测状态 + return self.lost_count > 0 + + def predict(self) -> None: + # §7 TRACKLET_PREDICT：预测成功时更新 last_center / last_box 且 age+1 + pred = self.kf.predict() + if pred is not None: + self.last_center = (pred.cx, pred.cy) + self.last_box = to_xyxy(pred) + self.age += 1 + + def update(self, box: BoxXYXY, conf: float, cls_id: int) -> None: + # §7 TRACKLET_UPDATE：框 → [cx,cy,w,h] → kf.update + self.kf.update(to_center_size(box)) + self.last_center = ((box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0) + self.last_box = box + self.last_raw_box = box + self.confidence = conf + self.cls_id = cls_id + self.lost_count = 0 + self.hits += 1 + self.age += 1 + + def mark_miss(self) -> None: + # §7 步骤 4：未匹配计丢失 + self.lost_count += 1 + self.age += 1 diff --git a/utils/tests/target_tracker/__pycache__/kalman_box_filter_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/target_tracker/__pycache__/kalman_box_filter_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..0d7262b Binary files /dev/null and b/utils/tests/target_tracker/__pycache__/kalman_box_filter_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/target_tracker/__pycache__/target_tracker_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/target_tracker/__pycache__/target_tracker_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..2a62dd2 Binary files /dev/null and b/utils/tests/target_tracker/__pycache__/target_tracker_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/target_tracker/__pycache__/tracklet_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/target_tracker/__pycache__/tracklet_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..78c56a8 Binary files /dev/null and b/utils/tests/target_tracker/__pycache__/tracklet_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/target_tracker/kalman_box_filter_test.py b/utils/tests/target_tracker/kalman_box_filter_test.py new file mode 100644 index 0000000..1e9160b --- /dev/null +++ b/utils/tests/target_tracker/kalman_box_filter_test.py @@ -0,0 +1,107 @@ +"""kalman_box_filter_test.py —— KalmanBoxFilter 单元测试。 + +移植自 reference-c++/target_tracker/tests/kalman_box_filter_test.cpp。 +覆盖（视觉追踪技术路线 §4）：初始化状态、静止目标收敛、匀速目标速度收敛 +且预测超前一个 dt、未初始化 predict 返回 None。 +""" + +import math + +import pytest + +from target_tracker.kalman_box_filter import BoxCXCYWH, KalmanBoxFilter + + +def test_uninitialized_predict_returns_none(): + kf = KalmanBoxFilter() + assert not kf.initialized + assert kf.predict() is None + + # 未初始化时 update 无副作用 + kf.update(BoxCXCYWH(100.0, 100.0, 20.0, 20.0)) + assert not kf.initialized + assert kf.predict() is None + for v in kf.state: + assert v == 0.0 + + +def test_initiate_sets_state_correctly(): + kf = KalmanBoxFilter() + kf.initiate(BoxCXCYWH(320.0, 240.0, 40.0, 30.0)) + assert kf.initialized + + expected = [320.0, 240.0, 40.0, 30.0, 0.0, 0.0, 0.0, 0.0] + state = kf.state + for i, want in enumerate(expected): + assert state[i] == want, f"index={i}" + + # 初始化后 predict 返回状态前 4 维（速度为零 → 位置不变，§4.4） + pred = kf.predict() + assert pred is not None + assert pred.cx == pytest.approx(320.0, abs=1e-9) + assert pred.cy == pytest.approx(240.0, abs=1e-9) + assert pred.w == pytest.approx(40.0, abs=1e-9) + assert pred.h == pytest.approx(30.0, abs=1e-9) + + +def test_static_target_converges_to_observation(): + kf = KalmanBoxFilter() + z = BoxCXCYWH(100.0, 150.0, 40.0, 60.0) + kf.initiate(BoxCXCYWH(104.0, 156.0, 42.0, 62.0)) # 带初始偏差，验证收敛能力 + + for _ in range(50): + kf.predict() + kf.update(z) + + state = kf.state + assert state[0] == pytest.approx(z.cx, abs=0.5) # cx + assert state[1] == pytest.approx(z.cy, abs=0.5) # cy + assert state[2] == pytest.approx(z.w, abs=0.5) # w + assert state[3] == pytest.approx(z.h, abs=0.5) # h + # 静止目标速度应收敛到约 0 + assert state[4] == pytest.approx(0.0, abs=0.1) + assert state[5] == pytest.approx(0.0, abs=0.1) + + +def test_constant_velocity_converges_and_predict_leads_one_dt(): + kf = KalmanBoxFilter() + vx = 3.0 # 像素/帧 + vy = 2.0 # 像素/帧 + + # 目标从 (100, 100) 出发匀速运动，每帧观测一次（predict → update） + z = BoxCXCYWH(100.0, 100.0, 30.0, 30.0) + kf.initiate(z) + for _ in range(60): + kf.predict() + z.cx += vx + z.cy += vy + kf.update(z) + + state = kf.state + # 速度收敛到真实速度（像素/帧） + assert state[4] == pytest.approx(vx, abs=0.3), "vx 未收敛" + assert state[5] == pytest.approx(vy, abs=0.3), "vy 未收敛" + # 位置贴合当前观测 + assert state[0] == pytest.approx(z.cx, abs=1.5) + assert state[1] == pytest.approx(z.cy, abs=1.5) + + # 再预测一帧：应超前当前观测一个 dt（≈ 速度 × 1 帧） + pred = kf.predict() + assert pred is not None + assert pred.cx == pytest.approx(z.cx + vx, abs=1.0), "预测未超前一个 dt" + assert pred.cy == pytest.approx(z.cy + vy, abs=1.0), "预测未超前一个 dt" + + +def test_small_target_noise_scaling_stable(): + # 动态噪声不退化：小尺寸目标多帧观测仍稳定收敛（§4.2 噪声与尺寸成比例） + kf = KalmanBoxFilter() + z = BoxCXCYWH(50.0, 60.0, 4.0, 4.0) # 小目标（4×4 像素） + kf.initiate(z) + for _ in range(30): + kf.predict() + kf.update(z) + state = kf.state + assert state[0] == pytest.approx(z.cx, abs=0.5) + assert state[1] == pytest.approx(z.cy, abs=0.5) + assert math.isfinite(state[2]) + assert math.isfinite(state[3]) diff --git a/utils/tests/target_tracker/target_tracker_test.py b/utils/tests/target_tracker/target_tracker_test.py new file mode 100644 index 0000000..e10b6bf --- /dev/null +++ b/utils/tests/target_tracker/target_tracker_test.py @@ -0,0 +1,364 @@ +"""target_tracker_test.py —— TargetTracker 单元测试。 + +移植自 reference-c++/target_tracker/tests/target_tracker_test.cpp。 +逐条覆盖 docs/视觉追踪技术路线.md §9.4 行为一致性验收用例（硬验收标准）： + 1. 空检测帧序列：Coast 持续 max_lost_frames 帧，第 max_lost_frames+1 帧判丢 + 2. ID 严格递增不复用（含 Reset 后归零） + 3. 锁定不被抢占；锁定目标死亡后切换到新目标 + 4. min_hits 回退：无已确认实测轨道时新轨道也可当选主目标 + 5. cost=1.0 恰好不构成匹配（严格小于） + 6. 置信度优先：两轨道竞争同一检测，置信度高者获胜 +另含 NaN/Inf 防御与 §2.3/§6.3/§9.5 行为测试。 +""" + +import math + +import pytest + +from target_tracker.target_tracker import (Detection, FrameShape, TargetTracker, + TrackerConfig) + + +def make_det(x1, y1, x2, y2, conf, cls=0): + return Detection(x1, y1, x2, y2, conf, cls) + + +# ============================================================================ +# 验收用例 1（§9.4-1）：空检测帧序列 +# ============================================================================ +def test_empty_frames_coast_then_lost(): + tracker = TargetTracker() # 默认配置 max_lost_frames = 8 + assert tracker.get_config().max_lost_frames == 8 + + # 第 1 帧：给出一个检测，建立轨道并锁定为主目标 + r = tracker.update([make_det(100.0, 100.0, 140.0, 140.0, 0.9)]) + assert r.tracked + assert r.primary_id is not None + assert not r.is_predicted # 当帧有实测 + assert r.lost_frames == 0 + assert r.n_active == 1 + assert r.box is not None + assert r.raw is not None # 当帧关联到原始检测框 + + # 后续空检测帧：应持续 Coast 共 max_lost_frames 帧（is_predicted=True） + for i in range(1, 9): + r = tracker.update([]) + assert r.tracked, f"第 {i} 帧 Coast 期间不应判丢" + assert r.is_predicted, f"第 {i} 帧应为纯预测输出" + assert r.lost_frames == i + assert r.primary_id is not None + assert r.box is not None # Coast 期间继续输出预测框 + assert r.raw is None # Coast 时 raw 为空（§2.3） + assert r.confidence == pytest.approx(0.9) # 继承最近实测置信度（§2.3） + + # 第 max_lost_frames + 1 帧：轨道死亡回收 → tracked=False + r = tracker.update([]) + assert not r.tracked + assert r.primary_id is None + assert r.box is None + assert r.lost_frames == 0 # 丢失时 lost_frames=0（§2.3） + assert r.n_active == 0 + + +# ============================================================================ +# 验收用例 2（§9.4-2）：ID 单调性 +# ============================================================================ +def test_id_strictly_increasing_no_reuse(): + cfg = TrackerConfig() + cfg.max_lost_frames = 1 # 缩短 Coast 窗口，加速轨道死亡 + tracker = TargetTracker(cfg) + + # 目标 A 出现 → id 0 + r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.9)]) + assert r.primary_id is not None + id_a = r.primary_id + assert id_a == 0 + + # 目标 A 消失至死亡（max_lost=1：第 2 帧 Coast、第 3 帧死亡回收） + tracker.update([]) + r = tracker.update([]) + assert not r.tracked + assert tracker.active_count() == 0 + + # 目标 B 出现（同一位置）：ID 不复用，严格递增 → id 1 + r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.9)]) + assert r.primary_id is not None + id_b = r.primary_id + assert id_b == 1 + assert id_b > id_a + + # 目标 C 同帧新建 → id 2（同帧多目标亦严格递增） + r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.9), + make_det(400.0, 300.0, 430.0, 330.0, 0.8)]) + assert tracker.active_count() == 2 + max_id = max(tracker.active_tracks().keys()) + assert max_id == 2 + + # Reset 后计数器归零（§6.2）：新轨道重新从 id 0 开始 + tracker.reset() + assert tracker.active_count() == 0 + assert not tracker.last_result().tracked + r = tracker.update([make_det(50.0, 50.0, 70.0, 70.0, 0.9)]) + assert r.primary_id is not None + assert r.primary_id == 0 + + +# ============================================================================ +# 验收用例 3（§9.4-3）：锁定不被抢占 +# ============================================================================ +def test_lock_not_stolen_and_switch_after_death(): + cfg = TrackerConfig() + cfg.max_lost_frames = 3 + tracker = TargetTracker(cfg) + + # 低置信度目标 A 建立并锁定 + r = tracker.update([make_det(100.0, 100.0, 140.0, 140.0, 0.4)]) + assert r.primary_id is not None + id_a = r.primary_id + + # A 消失进入 Coast；期间出现更高置信度新目标 B（远离 A，不会被误关联） + r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)]) + assert r.primary_id is not None + assert r.primary_id == id_a, "Coast 期间锁定不应被高置信度目标抢占" + assert r.is_predicted + assert tracker.active_count() == 2 # A（Coast）与 B 并存 + + # 继续 Coast，锁定依旧延续 + r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)]) + assert r.primary_id == id_a + assert r.is_predicted + + # A 继续丢失直至死亡（max_lost=3：lost 达 4 帧时回收） + for _ in range(2): + r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)]) + # A 已死亡删除 → 锁定解除，重新选举切换到 B（此时 B 已确认且实测） + assert r.tracked + assert r.primary_id != id_a, "锁定目标死亡后应切换到新目标" + assert not r.is_predicted + + # 后续 B 持续作为主目标 + r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)]) + assert r.primary_id is not None + assert r.primary_id != id_a + + +# ============================================================================ +# 验收用例 4（§9.4-4）：min_hits 回退 +# ============================================================================ +def test_min_hits_fallback_allows_new_track(): + cfg = TrackerConfig() + cfg.min_hits = 5 # 抬高确认门槛，保证测试期间轨道始终未确认 + tracker = TargetTracker(cfg) + + # 首帧新建轨道 hits=1 < min_hits：候选池 confirmed 为空 → 退回 measured + r = tracker.update([make_det(200.0, 150.0, 240.0, 190.0, 0.6)]) + assert r.tracked, "无已确认轨道时新轨道应能当选主目标" + assert r.primary_id is not None + assert r.primary_id == 0 + assert not r.is_predicted + + # 再跟几帧（hits 增长但仍未达 min_hits=5），依旧当选 + for _ in range(3): + r = tracker.update([make_det(200.0, 150.0, 240.0, 190.0, 0.6)]) + assert r.tracked + assert r.primary_id == 0 + + +# ============================================================================ +# 验收用例 5（§9.4-5）：cost 边界（cost = 1.0 恰好不匹配） +# ============================================================================ +def test_cost_exactly_one_does_not_match(): + cfg = TrackerConfig() + cfg.max_lost_frames = 1 + cfg.max_association_dist = 200.0 + cfg.dist_weight = 1.0 # 纯距离项，IoU 项权重为 0 且两框零交集 + cfg.iou_weight = 0.0 + tracker = TargetTracker(cfg) + + # 建立轨道：中心 (100,100) + r = tracker.update([make_det(90.0, 90.0, 110.0, 110.0, 0.9)]) + assert r.primary_id is not None + id_a = r.primary_id + + # 下一帧检测中心 (100,300)：距离 = 200 = Dmax → cost = 1.0，恰好不匹配 + r = tracker.update([make_det(90.0, 290.0, 110.0, 310.0, 0.9)]) + # 未匹配 → 原轨道 Coast、检测新建轨道；两个目标并存 + assert tracker.active_count() == 2, "cost=1.0 不应匹配，应新建轨道" + # 原轨道（锁定）进入 Coast：lost_frames=1 + assert r.tracked + assert r.is_predicted + assert r.primary_id == id_a + assert r.lost_frames == 1 + + # 对照：距离略小于阈值（199px → cost=0.995 < 1.0）应匹配成功 + tracker2 = TargetTracker(cfg) + r = tracker2.update([make_det(90.0, 90.0, 110.0, 110.0, 0.9)]) + id_b = r.primary_id + r = tracker2.update([make_det(90.0, 289.0, 110.0, 309.0, 0.9)]) + assert tracker2.active_count() == 1, "cost<1.0 应匹配成功，不应新建轨道" + assert r.primary_id is not None + assert r.primary_id == id_b + assert not r.is_predicted # 实测命中 + + +# ============================================================================ +# 验收用例 6（§9.4-6）：置信度优先 +# ============================================================================ +def test_higher_confidence_track_wins_detection(): + tracker = TargetTracker() + + # 同帧建立两条轨道：A 置信度 0.5、B 置信度 0.9，相距 80px（不互相重叠） + r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.5), + make_det(180.0, 100.0, 200.0, 120.0, 0.9)]) + assert tracker.active_count() == 2 + + # 找出两条轨道 ID（按置信度区分） + id_low = None + id_high = None + for track_id, track in tracker.active_tracks().items(): + if track.confidence > 0.8: + id_high = track_id + else: + id_low = track_id + assert id_low is not None + assert id_high is not None + + # 下一帧只给一个检测，放在两轨道预测中心之间（各距 40px，对称）： + # 距离项对两者相同，贪婪按置信度降序 → 高置信度轨道 B 优先匹配 + r = tracker.update([make_det(140.0, 100.0, 160.0, 120.0, 0.7)]) + + tracks = tracker.active_tracks() + assert len(tracks) == 2 + # 获胜者：命中（lost=0、hits=2、置信度更新） + assert tracks[id_high].lost_count == 0, "高置信度轨道应获胜" + assert tracks[id_high].hits == 2 + assert tracks[id_high].confidence == pytest.approx(0.7) + # 失败者：未匹配进入 Coast（lost=1、hits 不变） + assert tracks[id_low].lost_count == 1, "低置信度轨道应落选" + assert tracks[id_low].hits == 1 + assert tracks[id_low].confidence == pytest.approx(0.5) + + +# ============================================================================ +# NaN/Inf 防御（评审修复项） +# ============================================================================ +def test_non_finite_detections_are_discarded(): + tracker = TargetTracker() + nan = math.nan + inf = math.inf + + # 第 1 帧：建立正常轨道 + r = tracker.update([make_det(100.0, 100.0, 140.0, 140.0, 0.9)]) + assert r.tracked + + # 第 2 帧：混入 NaN 坐标 / Inf 坐标 / NaN 置信度检测与一个正常检测： + # 非有限检测全部被丢弃，仅正常检测参与关联，不新建轨道 + r = tracker.update([make_det(nan, 100.0, 140.0, 140.0, 0.9), + make_det(100.0, -inf, 140.0, 140.0, 0.9), + make_det(100.0, 100.0, nan, 140.0, 0.9), + make_det(102.0, 102.0, 142.0, 142.0, nan), + make_det(101.0, 101.0, 141.0, 141.0, 0.9)]) + assert tracker.active_count() == 1, "非有限检测不应新建轨道" + assert r.tracked + assert not r.is_predicted # 正常检测命中，实测输出 + # 主目标输出未被 NaN 污染 + assert math.isfinite(r.center[0]) + assert math.isfinite(r.center[1]) + assert r.box is not None + assert math.isfinite(r.box.x1) and math.isfinite(r.box.y1) + assert math.isfinite(r.box.x2) and math.isfinite(r.box.y2) + assert math.isfinite(r.confidence) + assert r.raw is not None # 实测帧 raw 有值 + + # 第 3 帧：仅含 NaN 检测 → 等价空帧，轨道 Coast 不崩溃 + r = tracker.update([make_det(nan, nan, nan, nan, nan)]) + assert r.tracked + assert r.is_predicted + assert r.lost_frames == 1 + assert r.raw is None # Coast 时 raw 为空（§2.3） + assert math.isfinite(r.center[0]) + + # 持续 NaN 检测直至轨道死亡回收，全程不崩溃 + for _ in range(10): + r = tracker.update([make_det(nan, 0.0, 10.0, 10.0, 0.9)]) + assert not r.tracked + assert tracker.active_count() == 0 + + +# ============================================================================ +# 补充行为测试（§2.3 / §6.3 / §9.5） +# ============================================================================ + +def test_lost_result_fields_are_empty(): + tracker = TargetTracker() + r = tracker.update([]) # 空帧序列起点：无任何轨道 + assert not r.tracked + assert r.primary_id is None + assert r.box is None + assert r.raw is None + assert r.lost_frames == 0 + assert r.n_active == 0 + assert r.confidence == 0.0 + + # LastResult 返回同一快照（§9.5 跨线程读取入口） + cached = tracker.last_result() + assert not cached.tracked + + +def test_coasting_fallback_chooses_nearest_to_frame_center(): + # Coast 兜底选举：无实测轨道时选离画面中心最近的 Coast 轨道（§6.3）。 + # 锁定延续优先：需先让锁定轨道死亡解除锁定，才会进入重新选举分支。 + cfg = TrackerConfig() + cfg.max_lost_frames = 2 + tracker = TargetTracker(cfg) + + # F1：角落目标（高置信度）先建立并被锁定 + tracker.update([make_det(10.0, 10.0, 30.0, 30.0, 0.95)]) + # F2：画面中心附近目标建立；角落目标丢失(1) + tracker.update([make_det(300.0, 220.0, 340.0, 260.0, 0.9)]) + # F3：远处目标建立；角落丢失(2)、中心目标丢失(1) + tracker.update([make_det(600.0, 450.0, 620.0, 470.0, 0.85)]) + # F4：全部丢失：角落丢失(3) > max_lost 死亡解除锁定； + # 其余两条 Coast 存活 → 重新选举选离画面中心最近者 + r = tracker.update([], FrameShape(480, 640)) + assert r.tracked + assert r.is_predicted + assert r.center[0] == pytest.approx(320.0, abs=25.0) # 靠近画面中心 (320,240) + assert r.center[1] == pytest.approx(240.0, abs=25.0) + assert r.n_active == 2 # 角落轨道已回收 + + +def test_locked_primary_kept_while_measured(): + # 锁定延续优先于置信度选举（§6.3 ①） + tracker = TargetTracker() + r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.4)]) + assert r.primary_id is not None + locked_id = r.primary_id + + # 高置信度新目标出现并持续，锁定目标也持续实测 → primary_id 不变 + for i in range(5): + r = tracker.update([make_det(100.0 + i, 100.0, 120.0 + i, 120.0, 0.4), + make_det(400.0, 300.0, 430.0, 330.0, 0.95)]) + assert r.primary_id is not None + assert r.primary_id == locked_id + assert not r.is_predicted + + +def test_default_frame_center_is_320x240(): + # frame_shape 缺省按 640×480 画面中心 (320,240) 计算（§8.2 / §2.2） + cfg = TrackerConfig() + cfg.max_lost_frames = 2 + tracker = TargetTracker(cfg) + + # F1：角落目标先建立并被锁定（高置信度） + tracker.update([make_det(0.0, 0.0, 20.0, 20.0, 0.95)]) + # F2：左侧目标（中心 (280,240)，距默认中心 40px）建立 + tracker.update([make_det(270.0, 230.0, 290.0, 250.0, 0.9)]) + # F3：右侧目标（中心 (500,240)，距默认中心 180px）建立 + tracker.update([make_det(490.0, 230.0, 510.0, 250.0, 0.85)]) + # F4：全部丢失：锁定轨道死亡解除锁定，两条 Coast 轨道兜底选举 + r = tracker.update([]) + assert r.tracked + assert r.is_predicted + assert r.center[0] == pytest.approx(280.0, abs=30.0) # 左侧更近，当选 + assert r.center[1] == pytest.approx(240.0, abs=30.0) diff --git a/utils/tests/target_tracker/tracklet_test.py b/utils/tests/target_tracker/tracklet_test.py new file mode 100644 index 0000000..a94517f --- /dev/null +++ b/utils/tests/target_tracker/tracklet_test.py @@ -0,0 +1,159 @@ +"""tracklet_test.py —— Tracklet 与几何工具单元测试。 + +移植自 reference-c++/target_tracker/tests/tracklet_test.cpp。 +覆盖（视觉追踪技术路线 §3.2/§5.2/§6.1/§7）：框互转、IoU 边界、 +IsDead/IsCoasting 判定（严格大于）、Predict/Update/MarkMiss 计数语义。 +""" + +import pytest + +from target_tracker.tracklet import (BoxXYXY, BoxCXCYWH, Tracklet, compute_iou, + to_center_size, to_xyxy) + + +# ---- 框表示互转（§7） ---- + +def test_round_trip_preserves_values(): + box = BoxXYXY(10.0, 20.0, 50.0, 80.0) + c = to_center_size(box) + assert c.cx == 30.0 # (10+50)/2 + assert c.cy == 50.0 # (20+80)/2 + assert c.w == 40.0 # 50−10 + assert c.h == 60.0 # 80−20 + + back = to_xyxy(c) + assert back.x1 == box.x1 + assert back.y1 == box.y1 + assert back.x2 == box.x2 + assert back.y2 == box.y2 + + +# ---- IoU 边界（§5.2） ---- + +def test_identical_boxes_return_one(): + a = BoxXYXY(10.0, 10.0, 30.0, 30.0) + assert compute_iou(a, a) == pytest.approx(1.0, abs=1e-12) # 完全重合 = 1 + + +def test_disjoint_boxes_return_zero(): + a = BoxXYXY(0.0, 0.0, 10.0, 10.0) + b = BoxXYXY(20.0, 20.0, 30.0, 30.0) + assert compute_iou(a, b) == 0.0 # 零交集 + + +def test_touching_edges_return_zero(): + # 仅边/角相接：交集宽高为 0 → 交集面积 ≤ 0 返回 0（§5.2） + a = BoxXYXY(0.0, 0.0, 10.0, 10.0) + b = BoxXYXY(10.0, 0.0, 20.0, 10.0) # 右边缘相接 + assert compute_iou(a, b) == 0.0 + c = BoxXYXY(10.0, 10.0, 20.0, 20.0) # 角点相接 + assert compute_iou(a, c) == 0.0 + + +def test_empty_box_returns_zero(): + normal = BoxXYXY(0.0, 0.0, 10.0, 10.0) + zero_area = BoxXYXY(5.0, 5.0, 5.0, 5.0) # 零面积 + inverted = BoxXYXY(10.0, 10.0, 0.0, 0.0) # 反向（负宽高） + assert compute_iou(normal, zero_area) == 0.0 + assert compute_iou(zero_area, normal) == 0.0 + assert compute_iou(normal, inverted) == 0.0 + assert compute_iou(zero_area, zero_area) == 0.0 + + +def test_partial_overlap_is_correct(): + # A = [0,0,10,10]（面积 100），B = [5,5,15,15]（面积 100），交集 25 + a = BoxXYXY(0.0, 0.0, 10.0, 10.0) + b = BoxXYXY(5.0, 5.0, 15.0, 15.0) + assert compute_iou(a, b) == pytest.approx(25.0 / 175.0, abs=1e-12) + assert compute_iou(b, a) == pytest.approx(25.0 / 175.0, abs=1e-12) # 对称性 + + +# ---- 构造与派生状态（§3.2 / §6.1） ---- + +def test_construction_initial_fields(): + t = Tracklet(7, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 1, 8) + + assert t.track_id == 7 + assert t.lost_count == 0 + assert t.max_lost == 8 + assert t.confidence == 0.9 + assert t.cls_id == 1 + assert t.age == 1 # 创建时为 1（§3.2） + assert t.hits == 1 # 创建时为 1（§3.2） + assert t.last_center == (100.0, 100.0) + assert t.last_box is not None # 创建当帧即赋 last_box（§7 步骤 5） + assert t.last_box.x1 == 90.0 + assert t.last_box.y1 == 90.0 + assert t.last_box.x2 == 110.0 + assert t.last_box.y2 == 110.0 + assert not t.is_dead + assert not t.is_coasting + + +def test_is_dead_uses_strictly_greater(): + # IsDead 使用严格大于：lost_count == max_lost 仍存活（§6.1 注记 2） + t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8) + for _ in range(8): + t.mark_miss() + assert not t.is_dead, f"lost_count={t.lost_count} 未超过 max_lost 不应判死" + assert t.lost_count == 8 + t.mark_miss() # lost_count = 9 > 8 + assert t.is_dead + + +def test_is_coasting_when_lost_positive(): + t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8) + assert not t.is_coasting + t.mark_miss() + assert t.is_coasting # lost_count > 0 即 Coast（§3.2） + + +# ---- 生命周期方法计数语义（§7） ---- + +def test_predict_advances_age_and_updates_last_state(): + t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8) + age_before = t.age + t.predict() # 速度为零：位置不变，但 age+1、last_box 更新为预测框 + assert t.age == age_before + 1 + assert t.last_box is not None + assert t.last_center[0] == pytest.approx(100.0, abs=1e-9) + assert t.last_center[1] == pytest.approx(100.0, abs=1e-9) + # hits / lost_count / confidence 不受预测影响 + assert t.hits == 1 + assert t.lost_count == 0 + assert t.confidence == 0.9 + + +def test_update_resets_lost_and_increments_hits(): + t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8) + t.mark_miss() + t.mark_miss() + assert t.lost_count == 2 + assert t.is_coasting + + age_before = t.age + box = BoxXYXY(150.0, 160.0, 170.0, 180.0) + t.update(box, 0.75, 2) + + assert t.lost_count == 0 # 匹配成功清零（§7） + assert t.hits == 2 # hits+1 + assert t.age == age_before + 1 # age+1 + assert t.confidence == 0.75 + assert t.cls_id == 2 + assert t.last_center == (160.0, 170.0) # ((150+170)/2, (160+180)/2) + assert t.last_box is not None + assert t.last_box.x1 == box.x1 + assert t.last_raw_box is not None # 实测框同步记录 + assert t.last_raw_box.x2 == box.x2 + assert not t.is_coasting + + +def test_mark_miss_increments_lost_and_age(): + t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8) + age_before = t.age + t.mark_miss() + assert t.lost_count == 1 + assert t.age == age_before + 1 + # MarkMiss 不影响 hits / confidence + assert t.hits == 1 + assert t.confidence == 0.9 diff --git a/utils/tests/visual_track_control/__pycache__/frame_transform_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/frame_transform_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..1b9b9a8 Binary files /dev/null and b/utils/tests/visual_track_control/__pycache__/frame_transform_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/visual_track_control/__pycache__/line_of_sight_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/line_of_sight_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..27138b3 Binary files /dev/null and b/utils/tests/visual_track_control/__pycache__/line_of_sight_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/visual_track_control/__pycache__/mavsdk_gateway_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/mavsdk_gateway_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..0351912 Binary files /dev/null and b/utils/tests/visual_track_control/__pycache__/mavsdk_gateway_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/visual_track_control/__pycache__/pid_controller_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/pid_controller_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..ab6d52b Binary files /dev/null and b/utils/tests/visual_track_control/__pycache__/pid_controller_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/visual_track_control/__pycache__/rate_limiter_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/rate_limiter_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..c204b9f Binary files /dev/null and b/utils/tests/visual_track_control/__pycache__/rate_limiter_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/visual_track_control/__pycache__/staleness_checker_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/staleness_checker_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..228501e Binary files /dev/null and b/utils/tests/visual_track_control/__pycache__/staleness_checker_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/visual_track_control/__pycache__/tracking_control_law_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/tracking_control_law_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..e8f9888 Binary files /dev/null and b/utils/tests/visual_track_control/__pycache__/tracking_control_law_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/visual_track_control/__pycache__/vtc_config_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/vtc_config_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..3b76ce2 Binary files /dev/null and b/utils/tests/visual_track_control/__pycache__/vtc_config_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/visual_track_control/frame_transform_test.py b/utils/tests/visual_track_control/frame_transform_test.py new file mode 100644 index 0000000..ef3f938 --- /dev/null +++ b/utils/tests/visual_track_control/frame_transform_test.py @@ -0,0 +1,43 @@ +"""frame_transform_test.py —— rotate_body_to_ned 单元测试。 + +移植自 reference-c++/visual_track_control/tests/frame_transform_test.cpp。 +覆盖（物理追踪思路 §2.3，A4）：零航向恒等、朝东时机体前向 → 东向、 +垂直分量不变、模长守恒。 +""" + +import math + +import pytest + +from visual_track_control.frame_transform import rotate_body_to_ned + + +def test_zero_yaw_is_identity(): + north, east, down = rotate_body_to_ned(1.0, 0.5, -2.0, 0.0) + assert north == pytest.approx(1.0, abs=1e-12) # 北 = 前 + assert east == pytest.approx(0.5, abs=1e-12) # 东 = 右 + assert down == pytest.approx(-2.0, abs=1e-12) + + +def test_yaw_east_90_maps_forward_to_east(): + north, east, _ = rotate_body_to_ned(2.0, 0.0, 0.0, math.pi / 2.0) # 机头朝东 + assert north == pytest.approx(0.0, abs=1e-9) # 北向 ≈ 0 + assert east == pytest.approx(2.0, abs=1e-9) # 东向 = 前向速度 + + +def test_right_is_east_when_heading_north(): + north, east, _ = rotate_body_to_ned(0.0, 1.0, 0.0, 0.0) + assert north == pytest.approx(0.0, abs=1e-12) + assert east == pytest.approx(1.0, abs=1e-12) + + +def test_down_component_unchanged(): + _, _, down = rotate_body_to_ned(1.0, 1.0, 3.5, math.pi / 3.0) + assert down == 3.5 + + +def test_preserves_horizontal_norm(): + # 旋转为正交变换：水平合速度不变（限幅约束不被破坏的前提） + north, east, _ = rotate_body_to_ned(3.0, -4.0, 1.0, 0.7) + assert math.hypot(north, east) == pytest.approx(math.hypot(3.0, -4.0), + abs=1e-9) diff --git a/utils/tests/visual_track_control/line_of_sight_test.py b/utils/tests/visual_track_control/line_of_sight_test.py new file mode 100644 index 0000000..34a8aab --- /dev/null +++ b/utils/tests/visual_track_control/line_of_sight_test.py @@ -0,0 +1,63 @@ +"""line_of_sight_test.py —— PixelToAngle 单元测试。 + +移植自 reference-c++/visual_track_control/tests/line_of_sight_test.cpp。 +覆盖（物理追踪思路 §4.1/§5.1，C2）：零偏移、画面边缘偏移 = 半视场角、 +奇对称、单调性、与线性近似的偏离（全程针孔模型）、构造期非法参数抛异常。 +""" + +import math + +import pytest + +from visual_track_control.line_of_sight import PixelToAngle + + +def test_zero_offset_gives_zero_angle(): + conv = PixelToAngle(1000.0, math.radians(90.0)) + assert conv.convert(0.0) == 0.0 + + +def test_half_size_offset_gives_half_fov(): + # 针孔模型：偏移 = size/2 时 angle = atan(tan(θ/2)) = θ/2 + conv = PixelToAngle(1000.0, math.radians(90.0)) + assert conv.convert(500.0) == pytest.approx(math.radians(45.0), abs=1e-12) + assert conv.convert(-500.0) == pytest.approx(-math.radians(45.0), abs=1e-12) + + +def test_odd_symmetry(): + conv = PixelToAngle(1920.0, math.radians(60.0)) + for offset in (1.0, 100.0, 730.5): + assert conv.convert(-offset) == pytest.approx(-conv.convert(offset), + abs=1e-15) + + +def test_monotonic_increasing(): + conv = PixelToAngle(1080.0, math.radians(35.0)) + prev = conv.convert(-600.0) + for x in range(-500, 601, 100): + cur = conv.convert(float(x)) + assert cur > prev + prev = cur + + +def test_deviates_from_linear_approximation(): + # C2：全程针孔模型。θ=90°、偏移比例 0.25 处线性近似误差约 4°， + # 针孔结果必须与之明显偏离（证明未走线性支路）。 + conv = PixelToAngle(1000.0, math.radians(90.0)) + pinhole_deg = math.degrees(conv.convert(250.0)) # atan(250·2·tan45°/1000) + linear_deg = 250.0 * (90.0 / 1000.0) # θ·offset/size = 22.5° + assert pinhole_deg == pytest.approx(math.degrees(math.atan(0.5)), abs=1e-9) + assert abs(pinhole_deg - linear_deg) > 2.0 + + +def test_constructor_rejects_invalid_args(): + with pytest.raises(ValueError): + PixelToAngle(0.0, 1.0) + with pytest.raises(ValueError): + PixelToAngle(-10.0, 1.0) + with pytest.raises(ValueError): + PixelToAngle(1000.0, 0.0) + with pytest.raises(ValueError): + PixelToAngle(1000.0, math.pi) + with pytest.raises(ValueError): + PixelToAngle(1000.0, math.pi + 0.1) diff --git a/utils/tests/visual_track_control/mavsdk_gateway_test.py b/utils/tests/visual_track_control/mavsdk_gateway_test.py new file mode 100644 index 0000000..1221f4b --- /dev/null +++ b/utils/tests/visual_track_control/mavsdk_gateway_test.py @@ -0,0 +1,176 @@ +"""mavsdk_gateway_test.py —— MavsdkCommandGateway 单元测试（mock offboard）。 + +移植自 reference-c++/visual_track_control/tests/mavlink_command_gateway_test.cpp。 +C++ 版校验 MAVLink ID 84 线格式字节流；Python 版采用 MAVSDK 语义映射， +改为以 FakeOffboard 捕获设定点对象，校验三分支映射（物理追踪思路 §2.2、 +A2 掩码三分支的语义等价）： + + 角速率非零 → 机体速度设定点 + 非零 yawspeed； + 航向角非零（角速率为零）→ NED 速度设定点 + 绝对 yaw； + 两者均零 → 机体速度设定点 + 零 yawspeed（保持航向，A2 回归语义）； + 刹车 → 全零机体速度设定点。 + 传输失败（异常）→ 返回 False 且不向外抛。 +""" + +import asyncio + +import pytest + +from visual_track_control.mavsdk_gateway import MavsdkCommandGateway +from visual_track_control.types import ControlOutput + + +class FakeOffboard: + """捕获式 offboard mock：记录每次调用的设定点，可注入失败。""" + + def __init__(self): + self.body_setpoints = [] + self.ned_setpoints = [] + self.attempts = 0 + self.start_count = 0 + self.stop_count = 0 + self.succeed = True + + async def set_velocity_body(self, setpoint): + self.attempts += 1 # 与 C++ CaptureTransport 一致：失败也计入发送次数 + if not self.succeed: + raise RuntimeError("mock transport failure") + self.body_setpoints.append(setpoint) + + async def set_velocity_ned(self, setpoint): + self.attempts += 1 + if not self.succeed: + raise RuntimeError("mock transport failure") + self.ned_setpoints.append(setpoint) + + async def start(self): + if not self.succeed: + raise RuntimeError("mock start failure") + self.start_count += 1 + + async def stop(self): + self.stop_count += 1 + + @property + def send_count(self): + return self.attempts + + +class FakeDrone: + def __init__(self): + self.offboard = FakeOffboard() + + +def run(coro): + return asyncio.run(coro) + + +def make_gateway(): + drone = FakeDrone() + return MavsdkCommandGateway(drone), drone.offboard + + +def test_start_sends_initial_setpoint_then_starts(): + gateway, offboard = make_gateway() + assert run(gateway.start()) + assert offboard.start_count == 1 + # PX4 要求先流式发送首条设定点（全零）再请求启动 + assert len(offboard.body_setpoints) == 1 + sp = offboard.body_setpoints[0] + assert sp.forward_m_s == 0.0 + assert sp.right_m_s == 0.0 + assert sp.down_m_s == 0.0 + assert sp.yawspeed_deg_s == 0.0 + + +def test_start_failure_returns_false(): + gateway, offboard = make_gateway() + offboard.succeed = False + assert not run(gateway.start()) + + +def _make_velocity_heading_output(): + from visual_track_control.types import ControlMode + out = ControlOutput() + out.mode = ControlMode.VELOCITY_HEADING + return out + + +def test_yaw_rate_branch_uses_body_frame_with_yawspeed(): + from visual_track_control.types import ControlMode + gateway, offboard = make_gateway() + out = _make_velocity_heading_output() + out.vx_body_mps = 1.5 + out.vz_mps = 0.25 + out.yaw_rate_dps = 5.0 + assert run(gateway.send_velocity_heading(out, 12345)) + assert offboard.send_count == 1 + assert len(offboard.body_setpoints) == 1 + sp = offboard.body_setpoints[0] + assert sp.forward_m_s == pytest.approx(1.5) + assert sp.right_m_s == 0.0 + assert sp.down_m_s == pytest.approx(0.25) + assert sp.yawspeed_deg_s == pytest.approx(5.0) + assert offboard.ned_setpoints == [] + + +def test_yaw_only_branch_uses_ned_frame_with_absolute_yaw(): + # 航向角非零且角速率为零 → 航向掩码语义：NED 速度 + 绝对航向 + gateway, offboard = make_gateway() + out = _make_velocity_heading_output() + out.vx_mps = 2.0 + out.vy_mps = -1.0 + out.vz_mps = 0.5 + out.yaw_deg = 10.0 + out.yaw_rate_dps = 0.0 + assert run(gateway.send_velocity_heading(out, 1)) + assert offboard.send_count == 1 + assert len(offboard.ned_setpoints) == 1 + sp = offboard.ned_setpoints[0] + assert sp.north_m_s == pytest.approx(2.0) + assert sp.east_m_s == pytest.approx(-1.0) + assert sp.down_m_s == pytest.approx(0.5) + assert sp.yaw_deg == pytest.approx(10.0) + assert offboard.body_setpoints == [] + + +def test_both_zero_keeps_heading_via_body_zero_yawspeed(): + # A2 回归：两者均零时机体速度设定点配零角速率 = 保持当前航向， + # 不得误发绝对航向 0（正北）。 + gateway, offboard = make_gateway() + out = _make_velocity_heading_output() + out.vx_body_mps = 1.0 + out.yaw_rate_dps = 0.0 + out.yaw_deg = 0.0 + assert run(gateway.send_velocity_heading(out, 1)) + assert len(offboard.body_setpoints) == 1 + sp = offboard.body_setpoints[0] + assert sp.forward_m_s == pytest.approx(1.0) + assert sp.yawspeed_deg_s == 0.0 + assert offboard.ned_setpoints == [] # 不得走绝对航向支路 + + +def test_brake_hover_sends_zero_velocity_body(): + gateway, offboard = make_gateway() + assert run(gateway.send_brake_hover(777)) + assert offboard.send_count == 1 + sp = offboard.body_setpoints[0] + assert sp.forward_m_s == 0.0 + assert sp.right_m_s == 0.0 + assert sp.down_m_s == 0.0 + assert sp.yawspeed_deg_s == 0.0 + + +def test_transport_failure_propagates_as_false(): + gateway, offboard = make_gateway() + offboard.succeed = False + out = _make_velocity_heading_output() + assert not run(gateway.send_velocity_heading(out, 1)) + assert not run(gateway.send_brake_hover(1)) + assert offboard.send_count == 2 + + +def test_stop_calls_offboard_stop(): + gateway, offboard = make_gateway() + run(gateway.stop()) + assert offboard.stop_count == 1 diff --git a/utils/tests/visual_track_control/pid_controller_test.py b/utils/tests/visual_track_control/pid_controller_test.py new file mode 100644 index 0000000..adb2f4d --- /dev/null +++ b/utils/tests/visual_track_control/pid_controller_test.py @@ -0,0 +1,103 @@ +"""pid_controller_test.py —— PidController 单元测试。 + +移植自 reference-c++/visual_track_control/tests/pid_controller_test.cpp。 +覆盖（物理追踪思路 §6.2/§6.4）：位置式 P/I/D 三项、积分钳位、输出限幅、 +异常节拍保护、anti-windup 条件积分（C1 修复回归）、ResetIntegral / Reset。 +""" + +import pytest + +from visual_track_control.pid_controller import PidController, PidParams + + +def make_params(kp, ki, kd, integral_limit, output_limit, filter_coef=0.0): + return PidParams(kp=kp, ki=ki, kd=kd, integral_limit=integral_limit, + derivative_filter_coef=filter_coef, output_limit=output_limit) + + +def test_proportional_only(): + pid = PidController(make_params(2.0, 0.0, 0.0, 0.0, 0.0)) + assert pid.update(3.0, 0.1) == 6.0 + assert pid.update(-1.5, 0.1) == -3.0 + + +def test_integral_accumulates(): + pid = PidController(make_params(0.0, 1.0, 0.0, 0.0, 0.0)) + # 两拍 error=2、dt=0.5：积分累计 1.0 → 2.0 + assert pid.update(2.0, 0.5) == 1.0 + assert pid.update(2.0, 0.5) == 2.0 + + +def test_integral_clamped_to_limit(): + pid = PidController(make_params(0.0, 1.0, 0.0, integral_limit=1.0, output_limit=0.0)) + for _ in range(5): + pid.update(10.0, 1.0) + # 积分候选值被钳位在 1.0 → 输出恒为 1.0 + assert pid.last_output == 1.0 + + +def test_output_clamped_to_limit(): + pid = PidController(make_params(10.0, 0.0, 0.0, 0.0, output_limit=3.0)) + assert pid.update(1.0, 0.1) == 3.0 + assert pid.update(-1.0, 0.1) == -3.0 + + +def test_anti_windup_freeze_while_saturated_same_direction(): + # kp=0、ki=2、output_limit=5：error=+1 持续饱和时积分冻结， + # 输出恒钉在 +5（10 拍累计也不会让积分继续增长）。 + pid = PidController(make_params(0.0, 2.0, 0.0, 0.0, 5.0)) + for _ in range(10): + assert pid.update(10.0, 1.0) == 5.0 + + +def test_anti_windup_unwinds_after_error_flips(): + # C1 回归：误差反号后积分必须立即回卷（旧判据"积分与输出同向"会 + # 冻结回卷、输出一直钉在 +5）。 + # kp=0、ki=2、limit=5：+10 饱和 3 拍、积分冻结在 0； + # error=-1 首拍 → 输出 2×(-1) = -2（立即回卷）； + # 持续负误差使输出到达负饱和 -5（积分冻结在 -2，输出 = clamp(-6)）。 + pid = PidController(make_params(0.0, 2.0, 0.0, 0.0, 5.0)) + for _ in range(3): + assert pid.update(10.0, 1.0) == 5.0 + assert pid.update(-1.0, 1.0) == -2.0 # 首拍立即回卷 + out = -2.0 + for _ in range(11): + out = pid.update(-1.0, 1.0) + assert out == pytest.approx(-5.0, abs=1e-9) # 旧判据此处仍为 +5.0 + + +def test_derivative_first_step_is_zero(): + pid = PidController(make_params(0.0, 0.0, 1.0, 0.0, 0.0)) + # 首拍无微分历史：微分项为 0 + assert pid.update(5.0, 1.0) == 0.0 + + +def test_derivative_with_low_pass_filter(): + pid = PidController(make_params(0.0, 0.0, 1.0, 0.0, 0.0, filter_coef=0.5)) + pid.update(0.0, 1.0) # 建立微分历史（导数 0） + # 第二拍原始导数 1.0，低通后 0.5×0 + 0.5×1.0 = 0.5 + assert pid.update(1.0, 1.0) == 0.5 + + +def test_zero_or_negative_dt_holds_last_output(): + pid = PidController(make_params(1.0, 0.0, 0.0, 0.0, 0.0)) + assert pid.update(2.0, 0.1) == 2.0 + assert pid.update(99.0, 0.0) == 2.0 + assert pid.update(99.0, -1.0) == 2.0 + + +def test_reset_integral_clears_only_integral(): + pid = PidController(make_params(0.0, 1.0, 1.0, 0.0, 0.0)) + pid.update(2.0, 1.0) # 积分 = 2 + pid.reset_integral() + # 积分清零、微分历史保留：输出应为 kd×(0−2)/1 = −2 + assert pid.update(0.0, 1.0) == -2.0 + + +def test_reset_clears_everything(): + pid = PidController(make_params(1.0, 1.0, 1.0, 0.0, 0.0)) + pid.update(5.0, 1.0) + pid.reset() + assert pid.last_output == 0.0 + # 全清后等同全新控制器：首拍无微分历史 + assert pid.update(3.0, 1.0) == 3.0 + 3.0 # kp·e + ki·∫e diff --git a/utils/tests/visual_track_control/rate_limiter_test.py b/utils/tests/visual_track_control/rate_limiter_test.py new file mode 100644 index 0000000..4540d9f --- /dev/null +++ b/utils/tests/visual_track_control/rate_limiter_test.py @@ -0,0 +1,68 @@ +"""rate_limiter_test.py —— Clamp / SlewRateLimiter 单元测试。 + +移植自 reference-c++/visual_track_control/tests/rate_limiter_test.cpp。 +覆盖（物理追踪思路 §8、状态机设计 S6）：静态限幅、变化率限幅步进、 +首条指令从零渐变、异常节拍保护、Reset 归零。 +""" + +import pytest + +from visual_track_control.rate_limiter import SlewRateLimiter, clamp + + +def test_clamp_clamps_value(): + assert clamp(5.0, -2.0, 2.0) == 2.0 + assert clamp(-5.0, -2.0, 2.0) == -2.0 + assert clamp(1.5, -2.0, 2.0) == 1.5 + # 边界值：落在区间内 + assert clamp(2.0, -2.0, 2.0) == 2.0 + assert clamp(-2.0, -2.0, 2.0) == -2.0 + + +def test_first_command_ramps_from_zero(): + # S6：初始输出为 0，首条大指令只能按变化率渐变 + lim = SlewRateLimiter(2.0) + assert lim.limit(10.0, 0.5) == 1.0 # 0 + 2×0.5 + assert lim.limit(10.0, 0.5) == 2.0 # 继续渐变 + assert lim.limit(10.0, 10.0) == 10.0 # 步长足够 → 到达目标 + + +def test_tracks_target_within_step(): + lim = SlewRateLimiter(10.0) + # 步长 10×1=10，目标 3 在步长内 → 直接到达 + assert lim.limit(3.0, 1.0) == 3.0 + assert lim.limit(3.0, 1.0) == 3.0 # 已在目标：保持 + + +def test_negative_direction(): + lim = SlewRateLimiter(2.0) + assert lim.limit(-10.0, 0.5) == -1.0 + assert lim.limit(-10.0, 0.5) == -2.0 + + +def test_zero_or_negative_dt_holds(): + lim = SlewRateLimiter(2.0) + lim.limit(10.0, 1.0) # 输出 2.0 + assert lim.limit(10.0, 0.0) == 2.0 + assert lim.limit(10.0, -1.0) == 2.0 + + +def test_zero_rate_limit_freezes_output(): + lim = SlewRateLimiter(0.0) + assert lim.limit(5.0, 1.0) == 0.0 + + +def test_reset_to_value(): + lim = SlewRateLimiter(2.0) + lim.limit(10.0, 1.0) + lim.reset() + assert lim.last_output == 0.0 + lim.reset(3.5) + assert lim.last_output == 3.5 + # 复位后从新值继续渐变 + assert lim.limit(10.0, 1.0) == 5.5 # 3.5 + 2×1 + + +def test_custom_initial_value(): + lim = SlewRateLimiter(1.0, initial=5.0) + assert lim.limit(10.0, 1.0) == 6.0 diff --git a/utils/tests/visual_track_control/staleness_checker_test.py b/utils/tests/visual_track_control/staleness_checker_test.py new file mode 100644 index 0000000..856b16e --- /dev/null +++ b/utils/tests/visual_track_control/staleness_checker_test.py @@ -0,0 +1,32 @@ +"""staleness_checker_test.py —— is_stale 单元测试。 + +移植自 reference-c++/visual_track_control/tests/staleness_checker_test.cpp。 +覆盖（物理追踪思路 §7.1）：从未取样判超龄、有效期内新鲜、超出有效期超龄、 +边界恰好在有效期上（不算超龄）、时间戳不一致判超龄（C4）。 +""" + +from visual_track_control.staleness_checker import is_stale + + +def test_never_sampled_is_stale(): + assert is_stale(0, 1000, 200) + assert is_stale(-5, 1000, 200) + + +def test_within_validity_is_fresh(): + assert not is_stale(900, 1000, 200) + assert not is_stale(1000, 1000, 200) # 刚取样 + + +def test_beyond_validity_is_stale(): + assert is_stale(799, 1000, 200) # 201 > 200 + + +def test_exactly_at_boundary_is_fresh(): + # 超龄判定为严格大于：恰好等于有效期仍算新鲜 + assert not is_stale(800, 1000, 200) + + +def test_inconsistent_timestamp_is_stale(): + # C4：now < sample 时间戳不一致 → 年龄不可信 → 判超龄（宁降级不轻信） + assert is_stale(2000, 1000, 200) diff --git a/utils/tests/visual_track_control/tracking_control_law_test.py b/utils/tests/visual_track_control/tracking_control_law_test.py new file mode 100644 index 0000000..55704f2 --- /dev/null +++ b/utils/tests/visual_track_control/tracking_control_law_test.py @@ -0,0 +1,338 @@ +"""tracking_control_law_test.py —— TrackingControlLaw 主控律单元测试。 + +移植自 reference-c++/visual_track_control/tests/tracking_control_law_test.cpp。 +覆盖（物理追踪思路 §2.3/§4/§5/§6/§7/§9）：超龄降级（姿态缺失/过期、视觉 +超龄、降级优先级）、判丢零速刹车、Coast 清积分与标志、速率式/位置式航向 +通道（含绝对航向合成与回绕，A2）、垂直统一速度支路（A1）、距离通道三种 +无距离降级动作、首拍变化率限幅（S6）、机体系→NED 输出旋转（A4）。 +""" + +import math + +import pytest + +from visual_track_control.config import (HeadingMode, NoDistanceAction, + TrackingConfig) +from visual_track_control.tracking_control_law import TrackingControlLaw +from visual_track_control.types import (ControlMode, ControlSnapshot, + DegradedReason, TrackState) + + +def make_config() -> TrackingConfig: + """测试基准配置：大限幅使限幅器近似透明，便于独立验证各通道数学。 + 画面 1000×1000、视场角 90° → 针孔系数 2·tan(45°)/1000 = 0.002， + 偏移 100 像素 → 视线角 atan(0.2) ≈ 11.3099°。 + """ + cfg = TrackingConfig() + cfg.control.frequency_hz = 20.0 + cfg.control.visual_stale_ms = 200 + cfg.control.radar_stale_ms = 500 + cfg.control.attitude_stale_ms = 300 + cfg.camera.image_width = 1000 + cfg.camera.image_height = 1000 + cfg.camera.fov_h_deg = 90.0 + cfg.camera.fov_v_deg = 90.0 + cfg.heading.mode = HeadingMode.RATE + cfg.heading.gain = 1.0 + cfg.heading.yaw_rate_limit_dps = 30.0 + cfg.heading.yaw_slew_limit_dps2 = 100000.0 + cfg.vertical.vz_gain_mps_per_deg = 0.1 + cfg.vertical.vz_limit_mps = 2.0 + cfg.distance.d_exp_m = 10.0 + cfg.distance.kp = 0.5 + cfg.distance.ki = 0.0 + cfg.distance.kd = 0.0 + cfg.distance.integral_limit = 0.0 + cfg.distance.derivative_filter_coef = 0.0 + cfg.distance.approach_velocity_limit_mps = 3.0 + cfg.distance.retreat_velocity_limit_mps = 1.0 + cfg.distance.no_distance_action = NoDistanceAction.HOLD + cfg.distance.no_distance_approach_limit_mps = 0.5 + cfg.accel_limit.ax_mps2 = 1000.0 + cfg.accel_limit.ay_mps2 = 1000.0 + cfg.accel_limit.az_mps2 = 1000.0 + return cfg + + +def make_snapshot(now_ms: int) -> ControlSnapshot: + """新鲜快照：目标在画面中心，姿态/追踪/雷达均新鲜。""" + return ControlSnapshot( + track=TrackState(tracked=True, is_predicted=False, + center=(500.0, 500.0)), + track_time_ms=now_ms, + attitude_present=True, + attitude_time_ms=now_ms, + ) + + +# ---- 超龄降级（§7.1 / §9.2） ---- + +def test_attitude_missing_degrades(): + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + snap.attitude_present = False + out = law.update(snap, 1000) + assert out.mode is ControlMode.DEGRADED_HOLD + assert out.degraded_reason is DegradedReason.ATTITUDE_STALE + assert not out.valid + + +def test_attitude_stale_degrades(): + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + snap.attitude_time_ms = 600 # 400 ms 前 > 300 ms 阈值 + out = law.update(snap, 1000) + assert out.mode is ControlMode.DEGRADED_HOLD + assert out.degraded_reason is DegradedReason.ATTITUDE_STALE + + +def test_vision_stale_degrades(): + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + snap.track_time_ms = 700 # 300 ms 前 > 200 ms 阈值 + out = law.update(snap, 1000) + assert out.mode is ControlMode.DEGRADED_HOLD + assert out.degraded_reason is DegradedReason.VISION_STALE + + +def test_never_sampled_track_degrades(): + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + snap.track_time_ms = 0 # 从未取样 + out = law.update(snap, 1000) + assert out.mode is ControlMode.DEGRADED_HOLD + assert out.degraded_reason is DegradedReason.VISION_STALE + + +def test_attitude_stale_beats_vision_stale(): + # 降级优先级：姿态校验在前（§9.2 姿态过期禁止一切空间转换） + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + snap.attitude_present = False + snap.track_time_ms = 0 + out = law.update(snap, 1000) + assert out.degraded_reason is DegradedReason.ATTITUDE_STALE + + +# ---- 判丢刹车（§6.4） ---- + +def test_lost_target_brakes(): + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + snap.track.tracked = False + out = law.update(snap, 1000) + assert out.mode is ControlMode.BRAKE_HOVER + assert out.valid + assert out.vx_mps == 0.0 + assert out.vy_mps == 0.0 + assert out.vz_mps == 0.0 + + +# ---- Coast（§6.4） ---- + +def test_coast_sets_flag_and_keeps_tracking(): + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + snap.track.is_predicted = True + out = law.update(snap, 1000) + assert out.mode is ControlMode.VELOCITY_HEADING + assert out.coast_active + assert out.valid + + +# ---- 水平航向通道（§4.2，A2） ---- + +def test_rate_mode_maps_beta_to_yaw_rate(): + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + snap.track.center = (600.0, 500.0) # 目标偏右 100 像素 + out = law.update(snap, 1000) + beta_deg = math.degrees(math.atan(0.2)) # ≈ 11.31° + assert out.yaw_rate_dps == pytest.approx(beta_deg, abs=1e-6) # gain=1，右转正 + assert out.yaw_deg == 0.0 + + +def test_rate_mode_clamps_to_yaw_rate_limit(): + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + snap.track.center = (950.0, 500.0) # 大偏移：β≈43.5° > 30°/s 上限 + out = law.update(snap, 1000) + assert out.yaw_rate_dps == pytest.approx(30.0, abs=1e-9) + + +def test_position_mode_composes_absolute_heading(): + # A2：位置式输出为绝对 NED 航向 = 当前航向 + 限幅修正量 + cfg = make_config() + cfg.heading.mode = HeadingMode.POSITION + law = TrackingControlLaw(cfg) + snap = make_snapshot(1000) + snap.track.center = (600.0, 500.0) + snap.yaw_rad = math.radians(45.0) + out = law.update(snap, 1000) + beta_deg = math.degrees(math.atan(0.2)) + assert out.yaw_deg == pytest.approx(45.0 + beta_deg, abs=1e-6) + assert out.yaw_rate_dps == 0.0 + + +def test_position_mode_wraps_around_180(): + cfg = make_config() + cfg.heading.mode = HeadingMode.POSITION + law = TrackingControlLaw(cfg) + snap = make_snapshot(1000) + snap.track.center = (600.0, 500.0) + snap.yaw_rad = math.radians(175.0) + out = law.update(snap, 1000) + beta_deg = math.degrees(math.atan(0.2)) + # 175 + 11.31 = 186.31 → 回绕到 ≈ -173.69 + assert out.yaw_deg == pytest.approx(175.0 + beta_deg - 360.0, abs=1e-6) + assert -180.0 < out.yaw_deg < 180.0 + + +# ---- 垂直统一速度支路（§5，A1） ---- + +def test_vertical_offset_drives_vz(): + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + snap.track.center = (500.0, 600.0) # 目标偏低 100 像素 → α>0 → 下降 + out = law.update(snap, 1000) + alpha_deg = math.degrees(math.atan(0.2)) + assert out.vz_mps == pytest.approx(0.1 * alpha_deg, abs=1e-6) + assert out.mode is ControlMode.VELOCITY_HEADING # A1：不切姿态微调模式 + + +def test_vertical_velocity_clamped(): + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + snap.track.center = (500.0, 990.0) # α≈44.5° × 0.1 = 4.45 > 2 上限 + out = law.update(snap, 1000) + assert out.vz_mps == pytest.approx(2.0, abs=1e-9) + + +# ---- 距离通道（§6.2 / §6.3） ---- + +def test_distance_approach_when_far(): + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + snap.radar_distance_m = 30.0 # Δd = +20 → kp 输出 10 → 限幅 3 + snap.associated_to_target = True + snap.radar_time_ms = 1000 + out = law.update(snap, 1000) + assert out.vx_mps == pytest.approx(3.0, abs=1e-9) + + +def test_distance_retreat_clamped_when_too_close(): + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + snap.radar_distance_m = 5.0 # Δd = -5 → kp 输出 -2.5 → 后退限幅 -1 + snap.associated_to_target = True + snap.radar_time_ms = 1000 + out = law.update(snap, 1000) + assert out.vx_mps == pytest.approx(-1.0, abs=1e-9) + + +def test_no_distance_hold_zeros_vx(): + law = TrackingControlLaw(make_config()) # 缺省 kHold + snap = make_snapshot(1000) + snap.associated_to_target = False + out = law.update(snap, 1000) + assert out.mode is ControlMode.VELOCITY_HEADING + assert out.vx_mps == 0.0 + + +def test_no_distance_slow_approach(): + cfg = make_config() + cfg.distance.no_distance_action = NoDistanceAction.SLOW_APPROACH + law = TrackingControlLaw(cfg) + snap = make_snapshot(1000) + snap.associated_to_target = False + out = law.update(snap, 1000) + assert out.vx_mps == pytest.approx(0.5, abs=1e-9) + + +def test_no_distance_exit_degrades(): + cfg = make_config() + cfg.distance.no_distance_action = NoDistanceAction.EXIT + law = TrackingControlLaw(cfg) + snap = make_snapshot(1000) + snap.associated_to_target = False + out = law.update(snap, 1000) + assert out.mode is ControlMode.DEGRADED_HOLD + assert out.degraded_reason is DegradedReason.NO_DISTANCE_EXIT + assert not out.valid + + +def test_stale_radar_falls_back_to_no_distance(): + law = TrackingControlLaw(make_config()) # kHold + snap = make_snapshot(1000) + snap.radar_distance_m = 30.0 + snap.associated_to_target = True + snap.radar_time_ms = 400 # 600 ms 前 > 500 ms 阈值 → 距离不可用 + out = law.update(snap, 1000) + assert out.mode is ControlMode.VELOCITY_HEADING + assert out.vx_mps == 0.0 + + +# ---- 首拍/限幅（§8、S6） ---- + +def test_first_tick_vx_slews_from_zero(): + # S6：首条速度指令必须经变化率限幅，从 0 渐变而非阶跃 + cfg = make_config() + cfg.accel_limit.ax_mps2 = 2.0 + law = TrackingControlLaw(cfg) + snap = make_snapshot(1000) + snap.radar_distance_m = 30.0 # 通道输出本应为 3.0 + snap.associated_to_target = True + snap.radar_time_ms = 1000 + out = law.update(snap, 1000) # 首拍：dt 按 1/20 Hz 兜底 + assert out.vx_mps == pytest.approx(2.0 * (1.0 / 20.0), abs=1e-9) # 0.1 + + +# ---- 机体系 → NED 旋转（§2.3，A4） ---- + +def test_output_rotated_to_ned_by_yaw(): + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + snap.yaw_rad = math.pi / 2.0 # 机头朝东 + snap.radar_distance_m = 30.0 + snap.associated_to_target = True + snap.radar_time_ms = 1000 + out = law.update(snap, 1000) + # 机体前向 3.0 → 东向 3.0，北向 ≈ 0 + assert out.vx_mps == pytest.approx(0.0, abs=1e-9) + assert out.vy_mps == pytest.approx(3.0, abs=1e-9) + + +def test_zero_yaw_keeps_forward_as_north(): + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + snap.yaw_rad = 0.0 + snap.radar_distance_m = 30.0 + snap.associated_to_target = True + snap.radar_time_ms = 1000 + out = law.update(snap, 1000) + assert out.vx_mps == pytest.approx(3.0, abs=1e-9) + assert out.vy_mps == pytest.approx(0.0, abs=1e-9) + + +# ---- Reset（§6.4） ---- + +def test_reset_restores_slew_from_zero(): + cfg = make_config() + cfg.accel_limit.ax_mps2 = 2.0 + law = TrackingControlLaw(cfg) + snap = make_snapshot(1000) + snap.radar_distance_m = 30.0 + snap.associated_to_target = True + snap.radar_time_ms = 1000 + for t in range(1000, 3000, 50): + snap.track_time_ms = t + snap.radar_time_ms = t + snap.attitude_time_ms = t + law.update(snap, t) # 若干拍后 vx 已爬升到限幅 3.0 + law.reset() + snap.track_time_ms = 3000 + snap.radar_time_ms = 3000 + snap.attitude_time_ms = 3000 + out = law.update(snap, 3000) + # Reset 后限幅器归零：又从 0.1 开始渐变 + assert out.vx_mps == pytest.approx(0.1, abs=1e-9) diff --git a/utils/tests/visual_track_control/vtc_config_test.py b/utils/tests/visual_track_control/vtc_config_test.py new file mode 100644 index 0000000..4335252 --- /dev/null +++ b/utils/tests/visual_track_control/vtc_config_test.py @@ -0,0 +1,171 @@ +"""vtc_config_test.py —— TrackingConfig::load_from_json 单元测试。 + +移植自 reference-c++/visual_track_control/tests/vtc_config_test.cpp。 +覆盖（物理追踪思路 §8、需求分析 NFR-007）：真实配置文件加载成功与取值 +正确（含 A1/A2 修改后的字段结构）、缺字段 / 缺分组 / 非法枚举 / 非法数值 / +文件缺失均抛带字段名的 ValueError。 + +C++ 版用字符串替换构造非法变体；Python 版改为 dict 变异（更稳健）， +临时文件经 pytest 的 tmp_path 提供。 +""" + +import json +from pathlib import Path + +import pytest + +from visual_track_control.config import (HeadingMode, NoDistanceAction, + load_from_json) + +_REPO_ROOT = Path(__file__).resolve().parents[3] +REAL_CONFIG = _REPO_ROOT / "reference-c++" / "visual_track_control" / "config" / "track_control.json" + + +def full_config() -> dict: + """与真实配置同构的完整合法配置（用于派生各非法变体）。""" + return { + "control": { + "frequency_hz": 20.0, + "visual_stale_ms": 200, + "radar_stale_ms": 500, + "attitude_stale_ms": 300, + }, + "camera": { + "image_width": 1920, + "image_height": 1080, + "fov_h_deg": 60.0, + "fov_v_deg": 35.0, + }, + "heading": { + "mode": "rate", + "gain": 1.0, + "yaw_rate_limit_dps": 30.0, + "yaw_slew_limit_dps2": 60.0, + }, + "vertical": { + "vz_gain_mps_per_deg": 0.1, + "vz_limit_mps": 2.0, + }, + "distance": { + "d_exp_m": 10.0, + "kp": 0.5, + "ki": 0.05, + "kd": 0.1, + "integral_limit": 2.0, + "derivative_filter_coef": 0.2, + "approach_velocity_limit_mps": 3.0, + "retreat_velocity_limit_mps": 1.0, + "no_distance_action": "hold", + "no_distance_approach_limit_mps": 0.5, + }, + "accel_limit": { + "ax_mps2": 2.0, + "ay_mps2": 2.0, + "az_mps2": 1.5, + }, + } + + +def write_temp_json(tmp_path: Path, config: dict, name: str) -> str: + path = tmp_path / name + path.write_text(json.dumps(config), encoding="utf-8") + return str(path) + + +def test_load_real_config_succeeds(): + cfg = load_from_json(str(REAL_CONFIG)) + assert cfg.control.frequency_hz == 20.0 + assert cfg.control.visual_stale_ms == 200 + assert cfg.camera.image_width == 1920 + assert cfg.camera.image_height == 1080 + assert cfg.camera.fov_h_deg == 60.0 + assert cfg.heading.mode is HeadingMode.RATE # A2：默认速率式 + assert cfg.heading.yaw_rate_limit_dps == 30.0 + assert cfg.vertical.vz_gain_mps_per_deg == 0.1 + assert cfg.vertical.vz_limit_mps == 2.0 + assert cfg.distance.d_exp_m == 10.0 + assert cfg.distance.no_distance_action is NoDistanceAction.HOLD + assert cfg.accel_limit.ax_mps2 == 2.0 + + +def test_full_json_round_trip_matches_expected(tmp_path): + # 内嵌合法配置与真实配置文件行为一致 + path = write_temp_json(tmp_path, full_config(), "vtc_cfg_full.json") + cfg = load_from_json(path) + assert cfg.heading.mode is HeadingMode.RATE + assert cfg.distance.kp == 0.5 + + +def test_position_mode_accepted(tmp_path): + config = full_config() + config["heading"]["mode"] = "position" + path = write_temp_json(tmp_path, config, "vtc_cfg_position.json") + cfg = load_from_json(path) + assert cfg.heading.mode is HeadingMode.POSITION + + +def test_missing_file_throws_with_path(): + with pytest.raises(ValueError) as exc_info: + load_from_json("/nonexistent/vtc_no_such_file.json") + assert "vtc_no_such_file.json" in str(exc_info.value) + + +def test_malformed_json_throws(tmp_path): + path = tmp_path / "vtc_cfg_bad.json" + path.write_text("{ this is not json", encoding="utf-8") + with pytest.raises(ValueError): + load_from_json(str(path)) + + +def test_missing_field_throws_with_field_name(tmp_path): + config = full_config() + del config["heading"]["gain"] + path = write_temp_json(tmp_path, config, "vtc_cfg_missing.json") + with pytest.raises(ValueError) as exc_info: + load_from_json(path) + assert "heading.gain" in str(exc_info.value) + + +def test_missing_section_throws(tmp_path): + config = full_config() + del config["accel_limit"] + path = write_temp_json(tmp_path, config, "vtc_cfg_nosection.json") + with pytest.raises(ValueError) as exc_info: + load_from_json(path) + assert "accel_limit" in str(exc_info.value) + + +def test_invalid_heading_mode_throws(tmp_path): + config = full_config() + config["heading"]["mode"] = "magic" + path = write_temp_json(tmp_path, config, "vtc_cfg_badmode.json") + with pytest.raises(ValueError) as exc_info: + load_from_json(path) + assert "heading.mode" in str(exc_info.value) + + +def test_invalid_no_distance_action_throws(tmp_path): + config = full_config() + config["distance"]["no_distance_action"] = "zoom" + path = write_temp_json(tmp_path, config, "vtc_cfg_badaction.json") + with pytest.raises(ValueError) as exc_info: + load_from_json(path) + assert "no_distance_action" in str(exc_info.value) + + +def test_negative_frequency_throws(tmp_path): + config = full_config() + config["control"]["frequency_hz"] = -1.0 + path = write_temp_json(tmp_path, config, "vtc_cfg_negfreq.json") + with pytest.raises(ValueError) as exc_info: + load_from_json(path) + assert "control.frequency_hz" in str(exc_info.value) + + +def test_fov_out_of_range_throws(tmp_path): + config = full_config() + config["camera"]["fov_h_deg"] = 200.0 + path = write_temp_json(tmp_path, config, "vtc_cfg_badfov.json") + with pytest.raises(ValueError) as exc_info: + load_from_json(path) + assert "camera.fov_h_deg" in str(exc_info.value) diff --git a/utils/visual_track_control/README.md b/utils/visual_track_control/README.md new file mode 100644 index 0000000..25979a3 --- /dev/null +++ b/utils/visual_track_control/README.md @@ -0,0 +1,41 @@ +# visual_track_control（Python 版） + +视觉跟踪控制律：像素视线角 → 水平航向 / 垂直速度 / 距离 PID 三通道 + 统一限幅 + +状态机（正常 / Coast / 判丢刹车 / 降级保持）+ MAVSDK offboard 指令输出。 +移植自 `reference-c++/visual_track_control/`（规格：docs/物理追踪思路.md）。 + +## 接口清单 + +| 名称 | 说明 | +| --- | --- | +| `TrackingConfig` / `load_from_json(path)` | 配置加载（启动期校验，错误携带字段名，抛 `ValueError`） | +| `HeadingMode` / `NoDistanceAction` | 配置枚举（rate/position；hold/slow_approach/exit） | +| `TrackingControlLaw(cfg)` | 主控律 | +| `.update(snapshot, now_ms) -> ControlOutput` | 单周期八步流程（姿态/视觉超龄 → 判丢 → Coast → 三通道 → NED 旋转） | +| `.reset()` | 停止闭环时清全部内部状态 | +| `ControlSnapshot` | 输入快照（自带 `TrackState`，track_time_ms 由调用方打戳） | +| `ControlOutput` | 输出：mode / valid / vx·vy·vz（NED）/ vx_body_mps（机体系前向）/ yaw_deg / yaw_rate_dps / degraded_reason / coast_active | +| `PidController` / `PidParams` | 距离 PID（anti-windup 条件积分） | +| `SlewRateLimiter` / `clamp` | 变化率限幅（初值 0 → 首条指令渐变，S6） | +| `PixelToAngle` / `is_stale` / `rotate_body_to_ned` | 针孔视线角 / 超龄判定 / 机体→NED 旋转 | +| `CommandSender`（Protocol） | 指令输出抽象（供注入 mock） | +| `MavsdkCommandGateway(drone)` | MAVSDK offboard 网关（async：start / send_velocity_heading / send_brake_hover / stop） | + +## 与 C++ 版的差异 + +- **MAVLink → MAVSDK 语义映射**（替代 C++ 的 ID 84 手工编码）： + - 角速率非零 → `set_velocity_body(forward, 0, down, yawspeed)`； + - 航向角非零（角速率为零）→ `set_velocity_ned(north, east, down, yaw)`； + - 两者均零 → `set_velocity_body` 配零角速率（保持航向，A2 回归语义）； + - 刹车 → `set_velocity_body(0,0,0,0)`（MAVSDK 无刹车掩码的行为等效适配）； + - 降级保持不发送（由调用方判断模式）。 +- **异步**：MAVSDK Python 为 asyncio 接口，网关方法均为协程；传输异常捕获后返回 + `False`（与 C++ 传输失败传播一致）。 +- **解耦**：C++ 的 ControlSnapshot 内嵌 `target_tracker::TrackResult` 是唯一耦合点； + Python 版自带字段对齐的 `TrackState`（box/raw 为 plain tuple），两包互不 import， + 转换在胶水层（`utils/demo_track_control.py` 的 `track_result_to_state`）。 +- **新增字段** `ControlOutput.vx_body_mps`：旋转前的机体系前向速度，供速率式 + 机体设定点直接使用（免反旋）。 +- **日志**：spdlog → 标准 `logging`，仅状态切换点打印（热路径零日志）不变。 +- 行为语义（姿态超龄优先于视觉、判丢进刹车清限幅器、Coast 清距离积分、 + dt 兜底 1/frequency_hz 上限 0.5s、WrapDeg180 回绕）与 C++ 一致，由移植测试锁定。 diff --git a/utils/visual_track_control/__init__.py b/utils/visual_track_control/__init__.py new file mode 100644 index 0000000..59b1654 --- /dev/null +++ b/utils/visual_track_control/__init__.py @@ -0,0 +1,55 @@ +"""视觉跟踪控制包（visual_track_control Python 版）。 + +对应 C++ 参考实现 reference-c++/visual_track_control/。本包与 target_tracker +包互不依赖（TrackState 为字段对齐的独立值类型，胶水层负责转换）。 + +分层： + 纯算法构件 rate_limiter / staleness_checker / line_of_sight / + frame_transform / pid_controller + 配置 config（TrackingConfig + load_from_json） + 主控律 tracking_control_law（TrackingControlLaw） + 指令输出 command_sender（抽象 Protocol）+ mavsdk_gateway（MAVSDK offboard） +""" + +from .command_sender import CommandSender +from .config import (AccelLimitConfig, CameraGroupConfig, ControlGroupConfig, + DistanceGroupConfig, HeadingGroupConfig, HeadingMode, + NoDistanceAction, TrackingConfig, VerticalGroupConfig, + load_from_json) +from .frame_transform import rotate_body_to_ned +from .line_of_sight import PixelToAngle +from .mavsdk_gateway import MavsdkCommandGateway +from .pid_controller import PidController, PidParams +from .rate_limiter import SlewRateLimiter, clamp +from .staleness_checker import is_stale +from .tracking_control_law import TrackingControlLaw +from .types import (ControlMode, ControlOutput, ControlSnapshot, DegradedReason, + TrackState) + +__all__ = [ + "CommandSender", + "MavsdkCommandGateway", + "TrackingControlLaw", + "TrackingConfig", + "load_from_json", + "HeadingMode", + "NoDistanceAction", + "ControlGroupConfig", + "CameraGroupConfig", + "HeadingGroupConfig", + "VerticalGroupConfig", + "DistanceGroupConfig", + "AccelLimitConfig", + "ControlMode", + "DegradedReason", + "TrackState", + "ControlSnapshot", + "ControlOutput", + "PidController", + "PidParams", + "SlewRateLimiter", + "clamp", + "is_stale", + "PixelToAngle", + "rotate_body_to_ned", +] diff --git a/utils/visual_track_control/__pycache__/__init__.cpython-312.pyc b/utils/visual_track_control/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 0000000..f4b882a Binary files /dev/null and b/utils/visual_track_control/__pycache__/__init__.cpython-312.pyc differ diff --git a/utils/visual_track_control/__pycache__/command_sender.cpython-312.pyc b/utils/visual_track_control/__pycache__/command_sender.cpython-312.pyc new file mode 100644 index 0000000..bb2691a Binary files /dev/null and b/utils/visual_track_control/__pycache__/command_sender.cpython-312.pyc differ diff --git a/utils/visual_track_control/__pycache__/config.cpython-312.pyc b/utils/visual_track_control/__pycache__/config.cpython-312.pyc new file mode 100644 index 0000000..c320817 Binary files /dev/null and b/utils/visual_track_control/__pycache__/config.cpython-312.pyc differ diff --git a/utils/visual_track_control/__pycache__/frame_transform.cpython-312.pyc b/utils/visual_track_control/__pycache__/frame_transform.cpython-312.pyc new file mode 100644 index 0000000..6352063 Binary files /dev/null and b/utils/visual_track_control/__pycache__/frame_transform.cpython-312.pyc differ diff --git a/utils/visual_track_control/__pycache__/line_of_sight.cpython-312.pyc b/utils/visual_track_control/__pycache__/line_of_sight.cpython-312.pyc new file mode 100644 index 0000000..3553ad1 Binary files /dev/null and b/utils/visual_track_control/__pycache__/line_of_sight.cpython-312.pyc differ diff --git a/utils/visual_track_control/__pycache__/mavsdk_gateway.cpython-312.pyc b/utils/visual_track_control/__pycache__/mavsdk_gateway.cpython-312.pyc new file mode 100644 index 0000000..5ae297f Binary files /dev/null and b/utils/visual_track_control/__pycache__/mavsdk_gateway.cpython-312.pyc differ diff --git a/utils/visual_track_control/__pycache__/pid_controller.cpython-312.pyc b/utils/visual_track_control/__pycache__/pid_controller.cpython-312.pyc new file mode 100644 index 0000000..7fb683d Binary files /dev/null and b/utils/visual_track_control/__pycache__/pid_controller.cpython-312.pyc differ diff --git a/utils/visual_track_control/__pycache__/rate_limiter.cpython-312.pyc b/utils/visual_track_control/__pycache__/rate_limiter.cpython-312.pyc new file mode 100644 index 0000000..2a401df Binary files /dev/null and b/utils/visual_track_control/__pycache__/rate_limiter.cpython-312.pyc differ diff --git a/utils/visual_track_control/__pycache__/staleness_checker.cpython-312.pyc b/utils/visual_track_control/__pycache__/staleness_checker.cpython-312.pyc new file mode 100644 index 0000000..e4535a7 Binary files /dev/null and b/utils/visual_track_control/__pycache__/staleness_checker.cpython-312.pyc differ diff --git a/utils/visual_track_control/__pycache__/tracking_control_law.cpython-312.pyc b/utils/visual_track_control/__pycache__/tracking_control_law.cpython-312.pyc new file mode 100644 index 0000000..20b3895 Binary files /dev/null and b/utils/visual_track_control/__pycache__/tracking_control_law.cpython-312.pyc differ diff --git a/utils/visual_track_control/__pycache__/types.cpython-312.pyc b/utils/visual_track_control/__pycache__/types.cpython-312.pyc new file mode 100644 index 0000000..528ab34 Binary files /dev/null and b/utils/visual_track_control/__pycache__/types.cpython-312.pyc differ diff --git a/utils/visual_track_control/command_sender.py b/utils/visual_track_control/command_sender.py new file mode 100644 index 0000000..61052bf --- /dev/null +++ b/utils/visual_track_control/command_sender.py @@ -0,0 +1,32 @@ +"""控制指令发送抽象（语义接口，对应 C++ ICommandTransport + 网关门面）。 + +C++ 版的 ICommandTransport 是字节流传输抽象（Send(bytes)），MAVLink 编码 +在网关内完成；Python 版采用 MAVSDK 语义映射后，"传输"被抽象为设定点级 +指令发送。本 Protocol 仅声明门面接口，供胶水层依赖注入与 mock 测试。 + +异常约定：实现不得向外抛传输层异常，失败一律返回 False（网关已捕获）。 +""" + +from typing import Optional, Protocol, runtime_checkable + +from .types import ControlOutput + + +@runtime_checkable +class CommandSender(Protocol): + async def start(self) -> bool: + """进入 offboard 模式。失败返回 False。""" + ... + + async def send_velocity_heading(self, output: ControlOutput, + time_boot_ms: Optional[int] = None) -> bool: + """发送速度+航向意图（ControlMode.VELOCITY_HEADING）。失败返回 False。""" + ... + + async def send_brake_hover(self, time_boot_ms: Optional[int] = None) -> bool: + """发送零速刹车/悬停意图（ControlMode.BRAKE_HOVER）。失败返回 False。""" + ... + + async def stop(self) -> None: + """退出 offboard 模式（结束时调用）。""" + ... diff --git a/utils/visual_track_control/config.py b/utils/visual_track_control/config.py new file mode 100644 index 0000000..95117cc --- /dev/null +++ b/utils/visual_track_control/config.py @@ -0,0 +1,278 @@ +"""视觉跟踪控制律配置（TrackingConfig + load_from_json）。 + +对应 C++ 参考实现 reference-c++/visual_track_control/src/vtc_config.cpp +（规格：docs/物理追踪思路.md §8：全部阈值集中配置，NFR-007）。 + +启动期一次性解析：缺字段 / 类型错误 / 非法取值抛 ValueError 并携带完整 +字段名（如 "heading.gain"）；解析结果与 JSON 脱钩，热路径不做热更新。 +""" + +import json +from dataclasses import dataclass, field +from enum import Enum +from typing import Any + +from .types import ControlMode # noqa: F401 (re-export 便利性) + + +class HeadingMode(Enum): + """航向通道模式（§4.2）。默认速率式：yaw_rate 天然是相对量，无坐标系歧义。""" + + RATE = "rate" # β → 偏航角速度，输出 yaw_rate_dps + POSITION = "position" # β → 修正量，与当前航向合成绝对航向输出 yaw_deg + + +class NoDistanceAction(Enum): + """雷达距离不可用（未关联/超龄）时的距离通道降级动作（§6.3）。""" + + HOLD = "hold" # 保持：前向速度指令清零 + SLOW_APPROACH = "slow_approach" # 限速接近：按 no_distance_approach_limit_mps 缓速 + EXIT = "exit" # 退出近距操作：输出降级保持 + + +@dataclass +class ControlGroupConfig: + frequency_hz: float = 20.0 + visual_stale_ms: int = 200 + radar_stale_ms: int = 500 + attitude_stale_ms: int = 300 + + +@dataclass +class CameraGroupConfig: + image_width: int = 1920 + image_height: int = 1080 + fov_h_deg: float = 60.0 + fov_v_deg: float = 35.0 + + +@dataclass +class HeadingGroupConfig: + mode: HeadingMode = HeadingMode.RATE + gain: float = 1.0 + yaw_rate_limit_dps: float = 30.0 + yaw_slew_limit_dps2: float = 60.0 + + +@dataclass +class VerticalGroupConfig: + vz_gain_mps_per_deg: float = 0.1 + vz_limit_mps: float = 2.0 + + +@dataclass +class DistanceGroupConfig: + d_exp_m: float = 10.0 + kp: float = 0.0 + ki: float = 0.0 + kd: float = 0.0 + integral_limit: float = 0.0 + derivative_filter_coef: float = 0.0 + approach_velocity_limit_mps: float = 3.0 + retreat_velocity_limit_mps: float = 1.0 + no_distance_action: NoDistanceAction = NoDistanceAction.HOLD + no_distance_approach_limit_mps: float = 0.5 + + +@dataclass +class AccelLimitConfig: + ax_mps2: float = 2.0 + ay_mps2: float = 2.0 + az_mps2: float = 1.5 + + +@dataclass +class TrackingConfig: + control: ControlGroupConfig = field(default_factory=ControlGroupConfig) + camera: CameraGroupConfig = field(default_factory=CameraGroupConfig) + heading: HeadingGroupConfig = field(default_factory=HeadingGroupConfig) + vertical: VerticalGroupConfig = field(default_factory=VerticalGroupConfig) + distance: DistanceGroupConfig = field(default_factory=DistanceGroupConfig) + accel_limit: AccelLimitConfig = field(default_factory=AccelLimitConfig) + + +# --------------------------------------------------------------------------- +# JSON 解析与校验（错误消息携带完整字段名，与 C++ 版文本一致） +# --------------------------------------------------------------------------- + +def _require_section(root: dict, section: str) -> dict: + value = root.get(section) + if not isinstance(value, dict): + raise ValueError(f"配置缺失分组或类型错误: {section}") + return value + + +def _is_number(value: Any) -> bool: + return isinstance(value, (int, float)) and not isinstance(value, bool) + + +def _is_integer(value: Any) -> bool: + return isinstance(value, int) and not isinstance(value, bool) + + +def _require_number(section: dict, section_name: str, key: str) -> float: + value = section.get(key) + if not _is_number(value): + raise ValueError(f"配置缺失字段或类型错误: {section_name}.{key}") + return float(value) + + +def _require_integer(section: dict, section_name: str, key: str) -> int: + value = section.get(key) + if not _is_integer(value): + raise ValueError(f"配置缺失字段或类型错误(需整数): {section_name}.{key}") + return int(value) + + +def _require_string(section: dict, section_name: str, key: str) -> str: + value = section.get(key) + if not isinstance(value, str): + raise ValueError(f"配置缺失字段或类型错误(需字符串): {section_name}.{key}") + return value + + +def _require_positive(value: float, field_name: str) -> float: + if not (value > 0.0): + raise ValueError(f"配置非法(必须为正): {field_name}") + return value + + +def _require_non_negative(value: float, field_name: str) -> float: + if value < 0.0: + raise ValueError(f"配置非法(不得为负): {field_name}") + return value + + +def _require_range(value: float, lo: float, hi: float, field_name: str) -> float: + if not (lo < value < hi): + raise ValueError(f"配置非法(超出范围): {field_name}") + return value + + +def _load_control(root: dict) -> ControlGroupConfig: + s = _require_section(root, "control") + c = ControlGroupConfig() + c.frequency_hz = _require_positive( + _require_number(s, "control", "frequency_hz"), "control.frequency_hz") + c.visual_stale_ms = _require_integer(s, "control", "visual_stale_ms") + c.radar_stale_ms = _require_integer(s, "control", "radar_stale_ms") + c.attitude_stale_ms = _require_integer(s, "control", "attitude_stale_ms") + if c.visual_stale_ms <= 0 or c.radar_stale_ms <= 0 or c.attitude_stale_ms <= 0: + raise ValueError("配置非法: control.*_stale_ms 必须为正") + return c + + +def _load_camera(root: dict) -> CameraGroupConfig: + s = _require_section(root, "camera") + c = CameraGroupConfig() + c.image_width = int(_require_positive( + float(_require_integer(s, "camera", "image_width")), "camera.image_width")) + c.image_height = int(_require_positive( + float(_require_integer(s, "camera", "image_height")), "camera.image_height")) + c.fov_h_deg = _require_range(_require_number(s, "camera", "fov_h_deg"), + 0.0, 180.0, "camera.fov_h_deg") + c.fov_v_deg = _require_range(_require_number(s, "camera", "fov_v_deg"), + 0.0, 180.0, "camera.fov_v_deg") + return c + + +def _load_heading(root: dict) -> HeadingGroupConfig: + s = _require_section(root, "heading") + h = HeadingGroupConfig() + mode = _require_string(s, "heading", "mode") + if mode == "position": + h.mode = HeadingMode.POSITION + elif mode == "rate": + h.mode = HeadingMode.RATE + else: + raise ValueError("配置非法: heading.mode 仅支持 position / rate") + h.gain = _require_non_negative(_require_number(s, "heading", "gain"), "heading.gain") + h.yaw_rate_limit_dps = _require_positive( + _require_number(s, "heading", "yaw_rate_limit_dps"), "heading.yaw_rate_limit_dps") + h.yaw_slew_limit_dps2 = _require_positive( + _require_number(s, "heading", "yaw_slew_limit_dps2"), "heading.yaw_slew_limit_dps2") + return h + + +def _load_vertical(root: dict) -> VerticalGroupConfig: + s = _require_section(root, "vertical") + v = VerticalGroupConfig() + v.vz_gain_mps_per_deg = _require_non_negative( + _require_number(s, "vertical", "vz_gain_mps_per_deg"), + "vertical.vz_gain_mps_per_deg") + v.vz_limit_mps = _require_positive( + _require_number(s, "vertical", "vz_limit_mps"), "vertical.vz_limit_mps") + return v + + +def _load_distance(root: dict) -> DistanceGroupConfig: + s = _require_section(root, "distance") + d = DistanceGroupConfig() + d.d_exp_m = _require_positive(_require_number(s, "distance", "d_exp_m"), + "distance.d_exp_m") + d.kp = _require_non_negative(_require_number(s, "distance", "kp"), "distance.kp") + d.ki = _require_non_negative(_require_number(s, "distance", "ki"), "distance.ki") + d.kd = _require_non_negative(_require_number(s, "distance", "kd"), "distance.kd") + d.integral_limit = _require_non_negative( + _require_number(s, "distance", "integral_limit"), "distance.integral_limit") + # 容许 -0.0：范围下界略低于 0，读入后归一化为 0 + d.derivative_filter_coef = _require_range( + _require_number(s, "distance", "derivative_filter_coef"), + -0.000001, 1.0, "distance.derivative_filter_coef") + if d.derivative_filter_coef < 0.0: + d.derivative_filter_coef = 0.0 + d.approach_velocity_limit_mps = _require_positive( + _require_number(s, "distance", "approach_velocity_limit_mps"), + "distance.approach_velocity_limit_mps") + d.retreat_velocity_limit_mps = _require_positive( + _require_number(s, "distance", "retreat_velocity_limit_mps"), + "distance.retreat_velocity_limit_mps") + action = _require_string(s, "distance", "no_distance_action") + if action == "hold": + d.no_distance_action = NoDistanceAction.HOLD + elif action == "slow_approach": + d.no_distance_action = NoDistanceAction.SLOW_APPROACH + elif action == "exit": + d.no_distance_action = NoDistanceAction.EXIT + else: + raise ValueError( + "配置非法: distance.no_distance_action 仅支持 hold / slow_approach / exit") + d.no_distance_approach_limit_mps = _require_non_negative( + _require_number(s, "distance", "no_distance_approach_limit_mps"), + "distance.no_distance_approach_limit_mps") + return d + + +def _load_accel_limit(root: dict) -> AccelLimitConfig: + s = _require_section(root, "accel_limit") + a = AccelLimitConfig() + a.ax_mps2 = _require_positive(_require_number(s, "accel_limit", "ax_mps2"), + "accel_limit.ax_mps2") + a.ay_mps2 = _require_positive(_require_number(s, "accel_limit", "ay_mps2"), + "accel_limit.ay_mps2") + a.az_mps2 = _require_positive(_require_number(s, "accel_limit", "az_mps2"), + "accel_limit.az_mps2") + return a + + +def load_from_json(path: str) -> TrackingConfig: + """从 JSON 文件一次性加载配置（启动期调用，调参重启生效）。""" + try: + with open(path, "r", encoding="utf-8") as fp: + root = json.load(fp) + except OSError: + raise ValueError(f"配置文件打开失败: {path}") from None + except json.JSONDecodeError as exc: + raise ValueError(f"配置文件解析失败: {path} ({exc})") from None + + if not isinstance(root, dict): + raise ValueError(f"配置文件解析失败: {path} (根节点必须为对象)") + + cfg = TrackingConfig() + cfg.control = _load_control(root) + cfg.camera = _load_camera(root) + cfg.heading = _load_heading(root) + cfg.vertical = _load_vertical(root) + cfg.distance = _load_distance(root) + cfg.accel_limit = _load_accel_limit(root) + return cfg diff --git a/utils/visual_track_control/frame_transform.py b/utils/visual_track_control/frame_transform.py new file mode 100644 index 0000000..af65a08 --- /dev/null +++ b/utils/visual_track_control/frame_transform.py @@ -0,0 +1,25 @@ +"""机体系（前/右/下）→ 局部 NED（北/东/地）坐标旋转。 + +对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/frame_transform.h +（规格：docs/物理追踪思路.md §2.3）。 + +速度设定值只随航向角 ψ（绕下轴旋转）旋转：滚转/俯仰对速度指令为二阶 +小量，由飞控内环处理。旋转为正交变换：合速度大小不变，因此"限幅在机 +体系完成、旋转在输出端执行"不破坏 §8 限幅约束。 + +正方向验证（NED：x=北，y=东，z=地；航向 ψ 自北顺时针为正）： + 机体前向 (1,0) 在 ψ=90°（机头朝东）时 → (0, 1) = 东； + 机体右向 (0,1) 在 ψ=0°（机头朝北）时 → (0, 1) = 东。 +""" + +import math + + +def rotate_body_to_ned(v_forward: float, v_right: float, v_down: float, + yaw_rad: float) -> tuple[float, float, float]: + """绕下轴按航向角 ψ 旋转，返回 (北, 东, 地)；垂直分量不变。""" + c = math.cos(yaw_rad) + s = math.sin(yaw_rad) + north = v_forward * c - v_right * s + east = v_forward * s + v_right * c + return north, east, v_down diff --git a/utils/visual_track_control/line_of_sight.py b/utils/visual_track_control/line_of_sight.py new file mode 100644 index 0000000..69efa52 --- /dev/null +++ b/utils/visual_track_control/line_of_sight.py @@ -0,0 +1,25 @@ +"""像素偏移 → 视线角（针孔模型）。 + +对应 C++ 参考实现 reference-c++/visual_track_control/src/line_of_sight.cpp +（规格：docs/物理追踪思路.md §4.1 水平 β / §5.1 垂直 α）。 + +全程使用精确针孔模型 angle = atan(offset · 2·tan(θ/2) / size)：原线性近似 +双路切换在边界两侧不连续（θ=60°、边界比例 0.2 处约 8% 台阶），会向控制 +环路注入突变；atan 代价可忽略。构造期预计算针孔系数，Convert 仅一次乘加。 +""" + +import math + + +class PixelToAngle: + def __init__(self, size_px: float, fov_rad: float) -> None: + # 构造期校验：非法参数直接抛异常（初始化抛异常规范） + if not (size_px > 0.0): + raise ValueError("PixelToAngle: size_px 必须为正") + if not (0.0 < fov_rad < math.pi): + raise ValueError("PixelToAngle: fov_rad 必须在 (0, π) 内") + self._pinhole_scale = 2.0 * math.tan(fov_rad / 2.0) / size_px + + def convert(self, offset_px: float) -> float: + """偏移（像素，水平右正 / 垂直下正）→ 视线角（弧度，同向为正）。""" + return math.atan(offset_px * self._pinhole_scale) diff --git a/utils/visual_track_control/mavsdk_gateway.py b/utils/visual_track_control/mavsdk_gateway.py new file mode 100644 index 0000000..6cf7bea --- /dev/null +++ b/utils/visual_track_control/mavsdk_gateway.py @@ -0,0 +1,98 @@ +"""MAVSDK offboard 网关 —— 控制意图 → MAVSDK 设定点（语义映射）。 + +对应 C++ 参考实现 reference-c++/visual_track_control/src/mavlink_command_gateway.cpp +（C++ 版手工编码 MAVLink ID 84 字节流；Python 版改用 MAVSDK offboard API， +按 A2 语义映射三分支）： + + 速率式（yaw_rate_dps ≠ 0）→ set_velocity_body(机体前向, 0, vz, yaw_rate) + 位置式（yaw_deg ≠ 0 且角速率为零）→ set_velocity_ned(NED 速度 + 绝对 yaw) + 两者均零 → set_velocity_body 配零角速率（保持当前航向，A2 回归语义） + 刹车 → set_velocity_body(0, 0, 0, 0)（MAVSDK 无刹车掩码的行为等效适配） + +降级保持（DEGRADED_HOLD）不发送任何指令，由调用方（胶水层）判断模式。 +MAVSDK Python API 为 asyncio 异步接口，本网关方法均为协程；传输异常 +捕获后返回 False（与 C++ 传输失败传播一致），不向外抛异常。 +""" + +import logging +from typing import Any, Optional + +from .types import ControlOutput + +logger = logging.getLogger(__name__) + + +class MavsdkCommandGateway: + """drone：mavsdk System（仅取 .offboard 属性，duck-typed 便于注入 mock）。""" + + def __init__(self, drone: Any) -> None: + self._offboard = drone.offboard + self._started = False + + async def start(self) -> bool: + """进入 offboard：PX4 要求先流式发送首条设定点再请求启动。""" + try: + await self._offboard.set_velocity_body( + self._velocity_body(0.0, 0.0, 0.0, 0.0)) + await self._offboard.start() + except Exception as exc: # OffboardError / 连接异常统一视为启动失败 + logger.warning("[MavsdkGateway] offboard 启动失败: %s", exc) + return False + self._started = True + logger.info("[MavsdkGateway] offboard 已启动") + return True + + async def send_velocity_heading(self, output: ControlOutput, + time_boot_ms: Optional[int] = None) -> bool: + try: + if output.yaw_rate_dps != 0.0: + # 速率式：机体系速度 + 偏航角速度（NED 分量不再使用） + await self._offboard.set_velocity_body( + self._velocity_body(output.vx_body_mps, 0.0, output.vz_mps, + output.yaw_rate_dps)) + elif output.yaw_deg != 0.0: + # 位置式：NED 速度 + 绝对航向（A2：绝对航向合成在控制律完成） + await self._offboard.set_velocity_ned( + self._velocity_ned(output.vx_mps, output.vy_mps, + output.vz_mps, output.yaw_deg)) + else: + # 两者均零：机体系速度 + 零角速率 = 保持当前航向（A2 回归语义） + await self._offboard.set_velocity_body( + self._velocity_body(output.vx_body_mps, 0.0, output.vz_mps, + 0.0)) + except Exception as exc: + logger.warning("[MavsdkGateway] 速度航向指令发送失败: %s", exc) + return False + return True + + async def send_brake_hover(self, time_boot_ms: Optional[int] = None) -> bool: + try: + await self._offboard.set_velocity_body( + self._velocity_body(0.0, 0.0, 0.0, 0.0)) + except Exception as exc: + logger.warning("[MavsdkGateway] 刹车指令发送失败: %s", exc) + return False + return True + + async def stop(self) -> None: + try: + await self._offboard.stop() + except Exception as exc: + logger.warning("[MavsdkGateway] offboard 停止失败: %s", exc) + self._started = False + logger.info("[MavsdkGateway] offboard 已停止") + + @staticmethod + def _velocity_body(forward: float, right: float, down: float, + yawspeed: float) -> Any: + from mavsdk import offboard as _mavsdk_offboard + return _mavsdk_offboard.VelocityBodyYawspeed( + forward_m_s=forward, right_m_s=right, down_m_s=down, + yawspeed_deg_s=yawspeed) + + @staticmethod + def _velocity_ned(north: float, east: float, down: float, + yaw: float) -> Any: + from mavsdk import offboard as _mavsdk_offboard + return _mavsdk_offboard.VelocityNedYaw( + north_m_s=north, east_m_s=east, down_m_s=down, yaw_deg=yaw) diff --git a/utils/visual_track_control/pid_controller.py b/utils/visual_track_control/pid_controller.py new file mode 100644 index 0000000..c537eec --- /dev/null +++ b/utils/visual_track_control/pid_controller.py @@ -0,0 +1,95 @@ +"""位置式 PID 控制器（纯算法构件）。 + +对应 C++ 参考实现 reference-c++/visual_track_control/src/pid_controller.cpp +（规格：docs/物理追踪思路.md §6.2 距离控制律、§6.4 Coast 清积分）。 + +anti-windup（条件积分）：输出饱和且本拍积分增量（error·dt）仍在推深饱和 +时冻结本步积分累加；判据刻意基于"增量方向"而非"积分项与输出同向"—— +后者在误差反号后仍会冻结回卷，导致饱和残留。 +""" + +from dataclasses import dataclass + +from .rate_limiter import clamp + + +@dataclass +class PidParams: + """PID 参数（全部来自配置，代码不写死任何数值）。""" + + kp: float = 0.0 + ki: float = 0.0 + kd: float = 0.0 + integral_limit: float = 0.0 # 积分项限幅（<=0 表示不限幅） + derivative_filter_coef: float = 0.0 # 微分一阶低通系数 ∈ [0,1)：0 = 不滤波 + output_limit: float = 0.0 # 输出限幅（<=0 表示不限幅） + + +class PidController: + def __init__(self, params: PidParams) -> None: + self._params = params + self._integral = 0.0 + self._prev_error = 0.0 + self._filtered_derivative = 0.0 + self._last_output = 0.0 + self._has_prev_error = False + + def update(self, error: float, dt_s: float) -> float: + """单步更新：输入误差与采样间隔（秒），返回限幅后的输出。 + + dt_s <= 0 时保持上次输出（异常节拍保护）。 + """ + if dt_s <= 0.0: + return self._last_output + + # 积分：候选值先钳位（积分限幅） + integral_candidate = self._integral + error * dt_s + if self._params.integral_limit > 0.0: + integral_candidate = clamp(integral_candidate, + -self._params.integral_limit, + self._params.integral_limit) + + # 微分：一阶低通滤波（系数 ∈ [0,1)，0 = 不滤波） + derivative = 0.0 + if self._has_prev_error: + derivative = (error - self._prev_error) / dt_s + coef = self._params.derivative_filter_coef + derivative = coef * self._filtered_derivative + (1.0 - coef) * derivative + self._filtered_derivative = derivative + self._prev_error = error + self._has_prev_error = True + + # 位置式合成 + integral_term = self._params.ki * integral_candidate + raw = self._params.kp * error + integral_term + self._params.kd * derivative + + # anti-windup（条件积分）：输出饱和且本拍积分增量仍在推深饱和时冻结 + saturated = (self._params.output_limit > 0.0 + and abs(raw) > self._params.output_limit) + increment_pushes_deeper = (error * dt_s) * raw > 0.0 + if not (saturated and increment_pushes_deeper): + self._integral = integral_candidate + + # 输出限幅 + if self._params.output_limit > 0.0: + self._last_output = clamp(raw, -self._params.output_limit, + self._params.output_limit) + else: + self._last_output = raw + return self._last_output + + def reset_integral(self) -> None: + """清空积分项（§6.4：Coast 时防止积分饱和）；保留微分历史与上次输出。""" + self._integral = 0.0 + + def reset(self) -> None: + """清空全部内部状态（积分、微分历史、上次输出）。""" + self._integral = 0.0 + self._prev_error = 0.0 + self._filtered_derivative = 0.0 + self._last_output = 0.0 + self._has_prev_error = False + + @property + def last_output(self) -> float: + return self._last_output diff --git a/utils/visual_track_control/rate_limiter.py b/utils/visual_track_control/rate_limiter.py new file mode 100644 index 0000000..8beb90b --- /dev/null +++ b/utils/visual_track_control/rate_limiter.py @@ -0,0 +1,37 @@ +"""限幅与变化率限制（纯算法构件）。 + +对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/rate_limiter.h +（规格：docs/物理追踪思路.md §8、状态机设计 S6 首条指令限幅）。 +""" + +def clamp(v: float, lo: float, hi: float) -> float: + """静态限幅：将 v 钳制在 [lo, hi]（调用方保证 lo <= hi）。""" + return min(max(v, lo), hi) + + +class SlewRateLimiter: + """变化率限幅器：限制输出每秒最大变化量（单位由调用方决定）。 + + 初值置 0 时天然实现"首条指令突变抑制"（S6）：切换进入视觉跟踪后 + 首条指令从零按加速度限幅渐变。 + """ + + def __init__(self, rate_limit: float, initial: float = 0.0) -> None: + self._rate_limit = rate_limit + self._output = initial + + def limit(self, target: float, dt_s: float) -> float: + """朝 target 推进一个节拍；dt_s <= 0 或 rate_limit <= 0 时保持当前输出。""" + if dt_s <= 0.0 or self._rate_limit <= 0.0: + return self._output + max_step = self._rate_limit * dt_s + self._output = clamp(target, self._output - max_step, self._output + max_step) + return self._output + + def reset(self, value: float = 0.0) -> None: + """复位输出值（刹车/重置时归零，保证恢复跟踪后指令从零渐变）。""" + self._output = value + + @property + def last_output(self) -> float: + return self._output diff --git a/utils/visual_track_control/staleness_checker.py b/utils/visual_track_control/staleness_checker.py new file mode 100644 index 0000000..5bfca18 --- /dev/null +++ b/utils/visual_track_control/staleness_checker.py @@ -0,0 +1,19 @@ +"""输入超龄判定（单调毫秒时钟）。 + +对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/staleness_checker.h +（规格：docs/物理追踪思路.md §7.1：超龄数据只能触发降级或告警）。 +""" + + +def is_stale(sample_time_ms: int, now_ms: int, valid_for_ms: int) -> bool: + """输入超龄判定。 + + sample_time_ms <= 0 视为从未取样，直接判超龄；now_ms < sample_time_ms + （时间戳不一致：时钟源错配/打戳错误）同样判超龄——单一单调时钟下不应 + 发生，宁可降级也不信任不可核实的取样。 + """ + if sample_time_ms <= 0: + return True + if now_ms < sample_time_ms: + return True + return (now_ms - sample_time_ms) > valid_for_ms diff --git a/utils/visual_track_control/tracking_control_law.py b/utils/visual_track_control/tracking_control_law.py new file mode 100644 index 0000000..fc9dff7 --- /dev/null +++ b/utils/visual_track_control/tracking_control_law.py @@ -0,0 +1,220 @@ +"""视觉跟踪主控律（TrackingControlLaw）。 + +对应 C++ 参考实现 reference-c++/visual_track_control/src/tracking_control_law.cpp +（规格：docs/物理追踪思路.md §2.3/§4/§5/§6/§7/§8/§9）。 + +单周期八步流程： + ① 超龄判定（姿态优先于视觉，超龄 → 降级保持） + ② 判丢 → 零速刹车（进入时清积分与限幅器历史） + ③ Coast（is_predicted）→ 清距离积分防饱和 + ④ 像素偏移 → 视线角 β/α（全程针孔模型） + ⑤ 水平航向（速率式/位置式，A2：位置式合成绝对 NED 航向） + ⑥ 垂直统一速度支路（A1：不切姿态微调模式） + ⑦ 距离通道 PID → 前向速度；无距离按配置降级（hold/slow_approach/exit） + ⑧ 机体系 → 局部 NED 旋转（A4） + +日志纪律与 C++ 版一致：仅状态切换点打印，热路径零日志。 +Python 扩展：ControlOutput.vx_body_mps 记录旋转前的机体系前向分量， +供 MAVSDK 机体速度设定点使用。 +""" + +import logging +import math + +from .config import HeadingMode, NoDistanceAction, TrackingConfig +from .frame_transform import rotate_body_to_ned +from .line_of_sight import PixelToAngle +from .pid_controller import PidController, PidParams +from .rate_limiter import SlewRateLimiter, clamp +from .staleness_checker import is_stale +from .types import ControlMode, ControlOutput, ControlSnapshot, DegradedReason + +logger = logging.getLogger(__name__) + +_K_PI = 3.14159265358979323846 +_K_RAD_TO_DEG = 180.0 / _K_PI +_K_DEG_TO_RAD = _K_PI / 180.0 +_K_MAX_DT_S = 0.5 # 单步 dt 上限（秒）：异常节拍保护 + +_MODE_NAMES = { + ControlMode.VELOCITY_HEADING: "速度航向", + ControlMode.BRAKE_HOVER: "零速刹车", + ControlMode.DEGRADED_HOLD: "降级保持", +} + + +def _wrap_deg_180(deg: float) -> float: + """角度回绕到 [-180, 180)（位置式绝对航向合成用）。""" + d = math.fmod(deg + 180.0, 360.0) + if d < 0.0: + d += 360.0 + return d - 180.0 + + +def _build_distance_pid_params(cfg: TrackingConfig) -> PidParams: + """由距离配置构造 PID 参数（§6.2）；输出限幅取接近/后退限幅的较大者。""" + return PidParams( + kp=cfg.distance.kp, + ki=cfg.distance.ki, + kd=cfg.distance.kd, + integral_limit=cfg.distance.integral_limit, + derivative_filter_coef=cfg.distance.derivative_filter_coef, + output_limit=max(cfg.distance.approach_velocity_limit_mps, + cfg.distance.retreat_velocity_limit_mps), + ) + + +class TrackingControlLaw: + def __init__(self, cfg: TrackingConfig) -> None: + self._cfg = cfg + self._pixel_to_beta = PixelToAngle(cfg.camera.image_width, + cfg.camera.fov_h_deg * _K_DEG_TO_RAD) + self._pixel_to_alpha = PixelToAngle(cfg.camera.image_height, + cfg.camera.fov_v_deg * _K_DEG_TO_RAD) + self._distance_pid = PidController(_build_distance_pid_params(cfg)) + self._yaw_deg_limiter = SlewRateLimiter(cfg.heading.yaw_slew_limit_dps2) + self._yaw_rate_limiter = SlewRateLimiter(cfg.heading.yaw_slew_limit_dps2) + self._vx_limiter = SlewRateLimiter(cfg.accel_limit.ax_mps2) + self._vz_limiter = SlewRateLimiter(cfg.accel_limit.az_mps2) + self._last_now_ms = 0 + self._last_mode = ControlMode.DEGRADED_HOLD + + def update(self, snapshot: ControlSnapshot, now_ms: int) -> ControlOutput: + # ---- 节拍推算（§7.1 固定周期；异常节拍用配置频率兜底） ---- + # 时钟回拨或 now_ms 未前进（同值）时不信任差值，回退配置频率。 + dt_s = 1.0 / self._cfg.control.frequency_hz + if self._last_now_ms > 0 and now_ms > self._last_now_ms: + dt_s = min((now_ms - self._last_now_ms) / 1000.0, _K_MAX_DT_S) + self._last_now_ms = now_ms + + out = ControlOutput() + out.mode = ControlMode.VELOCITY_HEADING # 正常路径默认模式，分支可覆盖 + + # ---- ① 超龄判定（§7.1 / §9.2） ---- + # 姿态过期或缺失：禁止空间转换和控制，立即退出正常控制（§9.2） + if (not snapshot.attitude_present + or is_stale(snapshot.attitude_time_ms, now_ms, + self._cfg.control.attitude_stale_ms)): + out.mode = ControlMode.DEGRADED_HOLD + out.degraded_reason = DegradedReason.ATTITUDE_STALE + return self._finish_and_log(out, "姿态过期/缺失，禁止控制") + # 视觉追踪结果超龄：不得继续生成正常指令（§7.1） + if is_stale(snapshot.track_time_ms, now_ms, + self._cfg.control.visual_stale_ms): + out.mode = ControlMode.DEGRADED_HOLD + out.degraded_reason = DegradedReason.VISION_STALE + return self._finish_and_log(out, "视觉追踪结果超龄") + + # ---- ② 判丢 → 零速刹车（§6.4：tracked == false 执行 BRAKE_HOVER） ---- + if not snapshot.track.tracked: + if self._last_mode is not ControlMode.BRAKE_HOVER: + # 进入刹车：清积分与限幅器历史，恢复跟踪后指令从零渐变（S6 首条限幅） + self._distance_pid.reset() + self._yaw_deg_limiter.reset() + self._yaw_rate_limiter.reset() + self._vx_limiter.reset() + self._vz_limiter.reset() + out.mode = ControlMode.BRAKE_HOVER # 速度分量保持默认全零 + out.valid = True + return self._finish_and_log(out, "判丢，零速刹车") + + # ---- ③ Coast：is_predicted → 清积分防饱和（§6.4） ---- + if snapshot.track.is_predicted: + self._distance_pid.reset_integral() + # 航向通道为纯比例增益映射（§4.2），无积分项，无需清理 + out.coast_active = True + + # ---- ④ 像素偏移 → 视线角（§4.1 水平 β / §5.1 垂直 α） ---- + # x_offset = cx − W/2（右正），y_offset = cy − H/2（下正） + x_offset = snapshot.track.center[0] - self._cfg.camera.image_width * 0.5 + y_offset = snapshot.track.center[1] - self._cfg.camera.image_height * 0.5 + beta_deg = self._pixel_to_beta.convert(x_offset) * _K_RAD_TO_DEG + alpha_deg = self._pixel_to_alpha.convert(y_offset) * _K_RAD_TO_DEG + + # ---- ⑤ 水平航向通道（§4.2：增益 → slew → 角速度限幅） ---- + if self._cfg.heading.mode is HeadingMode.RATE: + # 速率式：β → 偏航角速度指令，先变化率限幅再角速度限幅（FR-033） + target_rate = self._cfg.heading.gain * beta_deg + slewed = self._yaw_rate_limiter.limit(target_rate, dt_s) + out.yaw_rate_dps = clamp(slewed, -self._cfg.heading.yaw_rate_limit_dps, + self._cfg.heading.yaw_rate_limit_dps) + else: + # 位置式：β → 航向修正量，变化率限幅（FR-033）后与当前航向合成 + # 绝对 NED 航向——ID 84 的 yaw 字段是绝对值，直接发修正量会让 + # 机体转向"修正角"方向（A2 修复）。姿态已在 ① 通过超龄校验。 + target_yaw = self._cfg.heading.gain * beta_deg + correction_deg = self._yaw_deg_limiter.limit(target_yaw, dt_s) + out.yaw_deg = _wrap_deg_180(snapshot.yaw_rad * _K_RAD_TO_DEG + + correction_deg) + + # ---- ⑥ 垂直速度通道（§5：统一速度支路） ---- + # 符号约定：α 下正（目标偏低）→ NED 下正 → vz 为正（下降趋近目标高度）。 + # 输出模式保持 VELOCITY_HEADING，距离通道前向速度不会被丢弃（A1 修复）。 + vz_target = self._cfg.vertical.vz_gain_mps_per_deg * alpha_deg + slewed = self._vz_limiter.limit(vz_target, dt_s) # 加速度限幅（§8） + out.vz_mps = clamp(slewed, -self._cfg.vertical.vz_limit_mps, + self._cfg.vertical.vz_limit_mps) + + # ---- ⑦ 距离通道（§6.2 / §6.3） ---- + # 雷达距离不得直接当目标距离：仅关联成立且未超龄才可用（§6.3、FR-041） + distance_available = (snapshot.associated_to_target + and snapshot.radar_distance_m > 0.0 + and not is_stale(snapshot.radar_time_ms, now_ms, + self._cfg.control.radar_stale_ms)) + if distance_available: + # Δd = d − d_exp：d > d_exp 前向接近，d < d_exp 减速/后退（§6.2） + delta_d = snapshot.radar_distance_m - self._cfg.distance.d_exp_m + vx_raw = self._distance_pid.update(delta_d, dt_s) + slewed = self._vx_limiter.limit(vx_raw, dt_s) # 加速度限幅（§8） + out.vx_mps = clamp(slewed, + -self._cfg.distance.retreat_velocity_limit_mps, + self._cfg.distance.approach_velocity_limit_mps) + else: + # 无可用目标距离：清积分防饱和，按配置降级动作（§6.3） + self._distance_pid.reset_integral() + action = self._cfg.distance.no_distance_action + if action is NoDistanceAction.HOLD: + # 保持：前向指令清零（安全占位语义） + out.vx_mps = 0.0 + self._vx_limiter.reset(0.0) + elif action is NoDistanceAction.SLOW_APPROACH: + # 限速接近：缓速前向，同样经加速度限幅 + out.vx_mps = self._vx_limiter.limit( + self._cfg.distance.no_distance_approach_limit_mps, dt_s) + else: # NoDistanceAction.EXIT + # 退出近距操作：整体降级保持 + out.mode = ControlMode.DEGRADED_HOLD + out.degraded_reason = DegradedReason.NO_DISTANCE_EXIT + return self._finish_and_log(out, "距离未关联，退出近距操作") + + # ---- ⑧ 机体系 → 局部 NED 旋转（§2.3，A4 修复） ---- + # 各通道限幅均在机体系完成（旋转为正交变换，合速度不变，限幅约束 + # 不被破坏）；输出端随快照航向旋转为北/东/地。旋转前记录机体系前向 + # 分量，供 MAVSDK 机体速度设定点直接使用（免反旋）。 + out.vx_body_mps = out.vx_mps + out.vx_mps, out.vy_mps, out.vz_mps = rotate_body_to_ned( + out.vx_mps, out.vy_mps, out.vz_mps, snapshot.yaw_rad) + + out.valid = True + return self._finish_and_log(out, + "Coast 预测跟踪" if out.coast_active + else "正常跟踪") + + def reset(self) -> None: + """停止视觉闭环时清全部状态（§6.4）。""" + self._distance_pid.reset() + self._yaw_deg_limiter.reset() + self._yaw_rate_limiter.reset() + self._vx_limiter.reset() + self._vz_limiter.reset() + self._last_now_ms = 0 + self._last_mode = ControlMode.DEGRADED_HOLD + + def _finish_and_log(self, out: ControlOutput, reason: str) -> ControlOutput: + """仅模式变化时打印状态切换日志（刹车进出、降级切换等）。""" + if out.mode is not self._last_mode: + logger.info("[ControlLaw] 状态切换: %s -> %s (%s)", + _MODE_NAMES[self._last_mode], _MODE_NAMES[out.mode], + reason) + self._last_mode = out.mode + return out diff --git a/utils/visual_track_control/types.py b/utils/visual_track_control/types.py new file mode 100644 index 0000000..01353f3 --- /dev/null +++ b/utils/visual_track_control/types.py @@ -0,0 +1,95 @@ +"""视觉跟踪控制律的中立输入/输出结构。 + +对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/vtc_types.h。 + +解耦说明：C++ 版的 ControlSnapshot 直接内嵌 target_tracker::TrackResult；本包为保持 +与 target_tracker 包互不依赖，自带字段对齐的 TrackState 值类型。调用方（胶水层） +负责把 target_tracker.TrackResult 转换为 TrackState（字段均为 plain tuple/标量）。 +""" + +from dataclasses import dataclass, field +from enum import Enum, auto +from typing import Optional + + +class ControlMode(Enum): + """控制输出模式（§2.2 控制意图语义）。""" + + VELOCITY_HEADING = auto() # 速度与航向意图 + BRAKE_HOVER = auto() # 零速刹车/悬停：判丢等场景 + DEGRADED_HOLD = auto() # 降级保持：超龄/退出近距操作，禁止正常控制 + + +class DegradedReason(Enum): + """降级原因（仅 DEGRADED_HOLD 模式有意义）。""" + + NONE = auto() + ATTITUDE_STALE = auto() # PX4 姿态过期：禁止空间转换和控制（§9.2） + VISION_STALE = auto() # 视觉追踪结果超龄（§7.1） + NO_DISTANCE_EXIT = auto() # 无距离关联且配置为退出近距操作（§6.3） + + +@dataclass +class TrackState: + """追踪结果视图（字段语义与 target_tracker.TrackResult 对齐，但独立定义）。""" + + tracked: bool = False + primary_id: Optional[int] = None + center: tuple[float, float] = (0.0, 0.0) + box: Optional[tuple[float, float, float, float]] = None + confidence: float = 0.0 + is_predicted: bool = False + lost_frames: int = 0 + n_active: int = 0 + raw: Optional[tuple[float, float, float, float]] = None + + +@dataclass +class ControlSnapshot: + """控制律输入快照：调用方在同一控制周期内采集并打戳。 + + TrackState 本身不含时间戳（§7.2），调用方必须在取样瞬间用单调时钟 + 打戳填入 track_time_ms；超龄判定一律使用"取样时间 + valid_for_ms"。 + """ + + track: TrackState = field(default_factory=TrackState) + track_time_ms: int = 0 # 取样单调时钟（毫秒），<=0 视为从未取样 + + radar_distance_m: float = 0.0 # 机头前向单点距离（米） + associated_to_target: bool = False # 相机—雷达空间关联成立标志（未关联不得当目标距离） + radar_time_ms: int = 0 + + attitude_present: bool = False # 姿态源当前是否可用（过期 → 禁止控制） + attitude_time_ms: int = 0 + yaw_rad: float = 0.0 # 航向（弧度，NED）；参与 NED 旋转与位置式航向合成 + roll_rad: float = 0.0 # 以下为预留字段（机体补偿未实现，§4.1） + pitch_rad: float = 0.0 + yaw_rate_radps: float = 0.0 + velocity_x_mps: float = 0.0 + velocity_y_mps: float = 0.0 + velocity_z_mps: float = 0.0 + + +@dataclass +class ControlOutput: + """控制律输出：限幅后的控制意图。 + + 坐标语义：vx/vy/vz 为局部 NED（北/东/地）分量——各通道在机体系内计算 + 并限幅后，随 snapshot.yaw_rad 旋转得到（旋转为正交变换，合速度不变）。 + vx_body_mps 为旋转前的机体系前向分量，供 MAVSDK 速度机体设定点使用 + （机体系侧向恒为 0，故不另设右向字段；vz 两系相同）。 + """ + + mode: ControlMode = ControlMode.DEGRADED_HOLD + valid: bool = False + + vx_mps: float = 0.0 # 北向（§6.2） + vy_mps: float = 0.0 # 东向（机体系恒 0，航向旋转后可非零） + vz_mps: float = 0.0 # 垂直（NED 下正，§5.2） + vx_body_mps: float = 0.0 # 机体系前向分量（旋转前，MAVSDK 机体设定点用） + + yaw_deg: float = 0.0 # 位置式目标绝对航向（度，NED，右转为正，回绕 [-180,180)） + yaw_rate_dps: float = 0.0 # 速率式偏航角速度指令（度/秒，右转为正） + + degraded_reason: DegradedReason = DegradedReason.NONE + coast_active: bool = False