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：

• target_tracker Python 版
• visual_track_control Python 版

许可证

本项目遵循开源许可证协议。

贡献指南

欢迎提交 Issue 和 Pull Request 改进本项目。