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++ 一致,由移植测试锁定。