keita
b147925d58
核心结论: L1 不用新写控制律 —— TrackingControlLaw 第7步已内建无距离降级 (hold / slow_approach / exit), 复用其限幅/超龄/Coast/无距离/NED旋转(81测试保障)。 三个关键设计: 1. 航向信号用「框架角+脱靶量」而非只用脱靶量 —— 吊舱伺服会把目标保持居中, 脱靶量稳态≈0 没信号; 框架角才是目标相对机头的方位。且角度输入与 FOV 无关, 吊舱变焦不会失真(像素路径会)。 2. 接入路线 A1 推荐: ControlSnapshot 加可选 los_beta_deg/los_alpha_deg。 3. tracked 判据不能只信 bit2(实测不复位), 需第二判据。 含改动清单/配置外置/台架 S0-S5 步骤/风险表/L2 接口预留, 以及待拍板三项。
code-test-drone
本项目包含一个视觉目标追踪与控制系统的 C++ 参考实现及其 Python 移植版本，用于无人机对移动目标的追踪与控制。
项目结构
code-test-drone/
├── reference-c++/ # C++ 参考实现
│ ├── target_tracker/ # 目标追踪库（独立子工程）
│ │ ├── include/ # 头文件目录
│ │ ├── src/ # 源文件目录
│ │ └── tests/ # 单元测试
│ └── visual_track_control/# 视觉追踪控制库
│ ├── include/ # 头文件目录
│ ├── src/ # 源文件目录
│ └── tests/ # 单元测试
└── utils/ # Python 移植
├── target_tracker/ # 目标追踪（Python 版）
├── visual_track_control/# 追踪控制（Python 版）
└── tests/ # 单元测试
主要模块
target_tracker - 目标追踪库
提供基于卡尔曼滤波的目标追踪功能：
- KalmanBoxFilter: 8维卡尔曼滤波器（位置、速度）
- Tracklet: 目标轨迹管理与状态更新
- TargetTracker: 主追踪器，包含关联、选举逻辑
核心特性：
- 贪婪关联算法进行检测匹配
- 主目标选举三级回退机制
- 支持 Coast（纯预测）与 Lost（丢失）状态管理
visual_track_control - 视觉追踪控制库
基于追踪结果生成无人机控制指令：
- PIDController: 距离 PID 控制
- TrackingControlLaw: 追踪控制律
- MavlinkCommandGateway: MAVLink 指令网关
- PixelToAngle: 像素到角度转换
- StalenessChecker: 数据时效性检查
- SlewRateLimiter: 速率限制器
支持多种控制模式：
- Rate 模式: 角速率控制
- Position 模式: 绝对航向控制
- 距离控制: 接近/后退策略
环境要求
C++ 版本
- CMake 3.10+
- C++17 编译器
- MAVLink 库（MAVSDK）
Python 版本
- Python 3.12+
- 依赖：
numpy,pymavlink,mavsdk
安装
Python 环境
pip install -r requirements.txt
C++ 构建
cd reference-c++/target_tracker
mkdir build && cd build
cmake ..
make
cd ../../visual_track_control
mkdir build && cd build
cmake ..
make
测试
Python 测试
cd utils
pytest
C++ 测试
cd reference-c++/target_tracker/build
ctest
cd ../../visual_track_control/build
ctest
配置
C++ 与 Python 版本均使用 JSON 格式配置文件：
{
"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
},
"distance": {
"d_exp_m": 10.0,
"kp": 0.5,
"ki": 0.05,
"kd": 0.1,
"integral_limit": 2.0
}
}
使用示例
Python 追踪演示
cd utils
python demo_track_control.py
C++ 追踪演示
cd reference-c++/visual_track_control/build
./track_control_demo
移植说明
Python 版本与 C++ 版的接口差异请参考各子目录的 README：
许可证
本项目遵循开源许可证协议。
贡献指南
欢迎提交 Issue 和 Pull Request 改进本项目。