UAV_Tracking_Planner_V2 · 无人机目标跟踪与规划

📋 概述

uav_tracking_planner_v2 是为自主无人机设计的模块化跟踪规划包。它接收移动目标的位置，预测其未来轨迹，在环境地图中规划避障路径，并通过PID+MAVROS控制无人机实时跟踪。主要模块：

📈 轨迹预测

加权最小二乘 + 五阶贝塞尔曲线，满足速度/加速度约束。

🎲 贝叶斯预测

基于地图和目的地概率，蒙特卡洛采样轨迹。

🧭 混合A*规划器

动力学约束的状态扩展，离散加速度采样，碰撞检测。

🔌 MAVROS接口

与PX4/ArduPilot通信，实现位置/速度控制。

🎛️ PID控制器

抗积分饱和PID，实时计算期望速度。

👁️ 可视化

RViz支持轨迹、路径、目的地概率显示。

✨ 核心特性

完整论文实现 — 第四章全部公式：加权贝塞尔(4-4~4-7)、贝叶斯意图(4-9~4-13)、混合A*(4-14)、前瞻跟踪(4-16)。
双模式预测 — 基于历史的贝塞尔预测 + 基于目的地意图的贝叶斯预测。
动力学感知规划 — 混合A*考虑最大速度/加速度，采样离散加速度输入。
实时闭环控制 — PID控制器 + MAVROS接口，支持位置/速度控制模式。
全可视化 — 预测轨迹、规划路径、目的地概率球、调试标记。
模块化设计 — 每个组件可独立编译测试，易于替换算法。

⚡ 快速开始

最小启动示例 (使用模拟目标)

# 1. 编译
cd ~/catkin_ws
catkin_make
source devel/setup.bash

# 2. 启动规划节点 (需先启动roscore)
roslaunch uav_tracking_planner_v2 tracking_planner.launch

# 3. 起飞
rosservice call /tracking_planner_v2/takeoff "{}"

# 4. 发布目标位置 (模拟)
rostopic pub /target_position geometry_msgs/PointStamped "
header: {frame_id: 'world'}
pose: {x: 5.0, y: 2.0, z: 3.0}"

# 5. 查看RViz中的预测轨迹和规划路径

📦 编译与依赖

Ubuntu 20.04 / 22.04 (ROS Noetic)

sudo apt install ros-noetic-desktop-full
sudo apt install libeigen3-dev libosqp-dev  # OSQP 用于QP求解器

# 编译g2o (可选, 用于后端优化, 当前未用)
cd ~
git clone https://github.com/RainerKuemmerle/g2o.git
cd g2o && mkdir build && cd build
cmake .. && make -j4 && sudo make install

# 创建工作空间并编译
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
git clone https://github.com/your/uav_tracking_planner_v2.git
cd ..
catkin_make -DCMAKE_BUILD_TYPE=Release
source devel/setup.bash

💡 如果缺少OSQP，可以源码编译： git clone https://github.com/osqp/osqp.git; cd osqp; mkdir build; cd build; cmake ..; make -j4; sudo make install

🏗️ 系统架构

主节点 tracking_planner_node_v2 聚合所有模块，数据流如下：

目标位置 (/target_position) 
    → TrajectoryPredictor (历史+预测)
    → BayesianPredictor (目的地概率)
    → 前瞻跟踪点生成 (论文4-16)
    → HybridAStarPlanner (规划避障路径)
    → PIDController + MAVROSInterface (控制无人机)
    → 可视化发布 (/predicted_trajectory, /planned_path, /destination_markers)

所有核心类位于命名空间 uav_tracking_planner_v2 中。

📈 轨迹预测 (TrajectoryPredictor)

使用加权最小二乘拟合五阶贝塞尔曲线，权重与时间成反比 (论文公式4-4)。支持速度和加速度约束。

关键方法

void addObservation(const Eigen::Vector3d& pos, double timestamp)

添加目标观测点，自动更新权重。

bool predict(double horizon, std::vector<Eigen::Vector3d>& trajectory)

预测未来horizon秒的轨迹点 (时间步长dt_)。

实现细节

设计矩阵：伯恩斯坦基函数在归一化时间上的值。
加权法方程：(Bᵀ W B) C = Bᵀ W Y，添加平滑正则项。
速度/加速度约束采样点强制满足动力学。

🎲 贝叶斯预测 (BayesianPredictor)

基于环境占据栅格和可能的目的地，使用贝叶斯更新目的地概率 (论文4-11)，然后通过蒙特卡洛采样预测轨迹。

核心公式

P(dest|obs) ∝ P(obs|dest) * P(dest)
P(obs|dest) = exp(-α * (dist(next, dest) - dist(current, dest)))  // (4-9)

void setDestinations(const std::vector<Eigen::Vector2d>& dests)

设置可能的目的地，初始概率均匀。

bool predictTrajectory(double horizon, std::vector<Eigen::Vector3d>& traj)

使用当前最可能目的地，通过A*规划部分路径作为预测。

💡 内部使用A*搜索计算最短路径距离，需要提前设置地图。

🧭 混合A*规划器 (HybridAStarPlanner)

考虑无人机运动学，状态节点包含位置和速度。扩展时采样离散加速度 (论文4-14)。

状态转移

vₖ = vₖ₋₁ + a·Δt
pₖ = pₖ₋₁ + vₖ₋₁·Δt + 0.5·a·Δt²

bool plan(const Eigen::Vector3d& start_pos, const Eigen::Vector3d& start_vel, const Eigen::Vector3d& goal_pos, const Eigen::Vector3d& goal_vel, std::vector<geometry_msgs::PoseStamped>& path)

返回规划路径（离散点）。

启发式

使用时间最优启发式：max(距离/最大速度, 速度差/最大加速度)。

碰撞检测基于占据栅格和机器人半径。

📐 贝塞尔曲线 (BezierCurve)

五阶贝塞尔曲线实现，提供伯恩斯坦基及其导数。

Eigen::Vector3d evaluate(double t) const

计算曲线上的点 (t归一化0~1)。

Eigen::Vector3d evaluateVelocity(double t)

一阶导 (速度)。

Eigen::Vector3d evaluateAcceleration(double t)

二阶导 (加速度)。

内部使用组合数公式，支持任意阶数 (默认为5)。

⚙️ 二次规划 (QPSolver)

封装OSQP求解器，用于论文公式(4-7)的约束优化。当前用于拟合时施加约束，也可扩展为MPC。

void setProblem(const Eigen::MatrixXd& Q, const Eigen::VectorXd& c, const Eigen::MatrixXd& A, const Eigen::VectorXd& b, const Eigen::MatrixXd& C, const Eigen::VectorXd& lb, const Eigen::VectorXd& ub)

设置QP问题：min 0.5 xᵀQx + cᵀx, s.t. A x = b, lb ≤ C x ≤ ub。

bool solve(Eigen::VectorXd& solution)

求解并返回最优解。

📌 需要预先安装OSQP (libosqp-dev)。

🛠️ 工具集 (Utils)

静态工具函数，涵盖坐标转换、可视化、插值等。

函数	说明
pointToEigen / eigenToPoint	ROS点与Eigen转换
createPathMarker	创建Path的可视化Marker
createTrajectoryMarker	创建轨迹Marker
createDestinationMarkers	创建目的地概率球
lerp / clamp	线性插值与限幅

📡 主节点 (tracking_planner_node_v2)

ROS节点，整合所有模块，提供服务和话题接口。

订阅话题

/target_position (PointStamped) — 目标当前位置
/map (OccupancyGrid) — 环境地图
/destinations (PoseArray) — 可能的目的地集合

发布话题

/planned_path (Path) — 规划路径
/predicted_trajectory (Marker) — 贝塞尔预测轨迹
/bayesian_trajectory (Marker) — 贝叶斯预测轨迹
/destination_markers (MarkerArray) — 目的地概率
/debug_marker (Marker) — 调试路径

提供服务

/tracking_planner_v2/takeoff (Trigger) — 起飞
/tracking_planner_v2/land (Trigger) — 降落
/tracking_planner_v2/emergency (Trigger) — 紧急停止

🔌 MAVROS 接口

与PX4/ArduPilot通信的核心类，封装了状态订阅、控制指令发布。

控制模式

位置控制 — 发布 mavros_msgs::PositionTarget，指定位置和偏航。
速度控制 — 发布速度指令 (通过PID输出)。

void setTargetPosition(const Eigen::Vector3d& position, double yaw)

发送位置设定点。

void setTargetVelocity(const Eigen::Vector3d& velocity, double yaw_rate)

发送速度设定点。

bool takeoff(double height); bool land(); void emergencyStop();

基础飞行管理。

💡 内部自动维护状态机 (IDLE, TAKEOFF, HOVER, TRACKING, LANDING, EMERGENCY)。

🎛️ PID控制器

带抗积分饱和的PID控制器，支持位置和速度控制模式。

Eigen::Vector3d computeControl()

根据当前位置和设定点计算期望速度，内部包括微分滤波和积分限幅。

void computeFullControl(Eigen::Vector3d& velocity_cmd, double& yaw_rate_cmd)

同时计算线速度和偏航角速度。

增益参数 (可在yaml中调整)

Kp_xy: 1.5      # 水平位置P
Ki_xy: 0.1      # 水平积分
Kd_xy: 0.3      # 水平微分
Kp_z: 2.0       # 高度P
Kp_vel: 2.0     # 速度P (用于偏航)

⚙️ 配置参数 (planner_params.yaml)

参数	默认值	说明
history_length	15	历史轨迹点数量
max_speed / max_accel	3.0 / 2.0	运动限制
prediction_horizon	3.0	预测时长(秒)
bayes_alpha	10.0	路径概率衰减系数
robot_radius	0.3	碰撞检测半径
control_rate	20.0	MAVROS控制频率(Hz)
takeoff_height	2.0	起飞高度(m)

💡 示例：自定义离线预测

#include "uav_tracking_planner_v2/trajectory_predictor.h"
using namespace uav_tracking_planner_v2;

int main() {
    TrajectoryPredictor::PredictionConfig cfg;
    cfg.history_length = 10;
    TrajectoryPredictor predictor(cfg);

    // 模拟历史轨迹
    for (int i = 0; i < 20; ++i) {
        double t = i * 0.1;
        Eigen::Vector3d pos(0.5 * t, sin(t), 1.0);
        predictor.addObservation(pos, t);
    }

    std::vector<Eigen::Vector3d> future;
    if (predictor.predict(2.0, future)) {
        for (const auto& p : future)
            std::cout << "pred: " << p.transpose() << std::endl;
    }
    return 0;
}

🔍 故障排除

MAVROS连接失败 检查 mavros 是否启动，Firmware是否已连接 (仿真用 roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557")。

规划失败/无路径 检查地图话题是否正确，起点是否在障碍物内，增大 goal_tolerance 或减小 robot_radius。

预测轨迹跳变 增大 history_length 或增大平滑项权重 weight_smoothness。

编译找不到OSQP 确认安装后，在CMakeLists.txt中添加 find_package(osqp REQUIRED) 并链接。

📊 性能调优

预测速度：减小 history_length 或使用更少的约束采样点。
规划速度：调整 num_accel_samples (当前为3，即7³=343种加速度，可降为2或1)。
控制频率：control_rate 可根据无人机响应能力调整 (通常10-50Hz)。
地图分辨率：使用0.2~0.5m栅格加速碰撞检测。
并行化：预测与规划目前在主线程顺序执行，可考虑多线程分离。

📝 更新日志

版本 2.0.1 (2026-03-20)

修复MAVROS接口中四元数转偏航的错误。
增加紧急停止服务。
优化PID积分抗饱和。

版本 2.0.0 (2026-03-19)

完整实现论文第四章所有模块。
添加贝叶斯预测器与混合A*规划器。
集成MAVROS实现真机闭环。
RViz可视化支持。

⚖️ 许可证

MIT License