视觉跟随无人机工作原理

系统概述

我们的无人机结合了计算机视觉、机器学习和自适应控制算法，创建了一个智能跟随系统。该系统运行在树莓派上，采用混合架构，平衡了速度、准确性和安全性。

视觉系统

计算机视觉流水线

使用YOLOv8和OpenCV进行实时图像处理，用于人体检测和跟踪。

YOLOv8目标检测（ONNX Runtime）
CSRT/KCF视觉跟踪器
基于像素宽度的距离估算
帧稳定和跳跃检测

控制系统

混合自适应控制

PID + 前馈控制器，能够适应人类运动模式。

基于距离的PID控制
速度匹配前馈控制
自适应增益调度
紧急后退协议

安全系统

多层安全机制

用于自主操作的全面安全措施。

最低高度强制执行
紧急距离阈值
状态机恢复机制
受控下降算法

平台集成

DroneKit + MAVLink

与飞控系统的稳健通信。

DroneKit Python API
MAVLink速度指令
速率限制通信
引导模式自动驾驶

工作原理：分步流程

摄像头捕获与帧处理

树莓派摄像头以640x480分辨率、30 FPS捕获视频。每帧立即传递到视觉流水线。

关键洞察：系统使用最小缓冲区大小（1帧）以减少延迟。帧被缩放到160x160进行YOLO推理，同时保持宽高比以确保准确的距离计算。

人体检测（YOLOv8）

每帧由通过ONNX Runtime运行的YOLOv8神经网络处理。模型以0.50的置信度阈值检测"人"类别。

检测逻辑：当检测到多个人时，系统使用欧几里得距离计算选择最接近画面中心的人。

视觉跟踪与距离估算

一旦检测到，初始化CSRT跟踪器以在帧之间跟踪人，减少计算负载。

距离计算：距离 = （人体宽度 × 焦距）÷ 像素宽度。假设在1.5米参考距离下，平均人体宽度为0.45米。

自适应速度估算

系统通过分析随时间变化的距离，使用5个样本的移动窗口来计算人的移动速度。

创新点：实时估算人的速度：远离时为正，接近时为负。这实现了预测性跟随。

混合控制计算

结合用于距离维持的PID控制器和用于速度匹配的前馈项。

Python: 控制算法HybridController.calculate_velocity()

# 用于距离维持的PID项

error = current_distance - DESIRED_DISTANCE_M # 1.0米目标

P = 0.8 * error # 比例项

I = 0.25 * integral # 积分项（带抗饱和）

D = 0.1 * derivative # 微分项用于阻尼

# 前馈：匹配人的速度

feedforward = 0.7 * person_speed_est # 70%速度匹配

# 组合控制信号

raw_velocity = feedforward + (P + I + D)

# 安全考虑的不对称限制
if raw_velocity >0: # 向前

  velocity = clamp(raw_velocity, 0.1, 0.8)
else: # 向后（更保守）

  velocity = clamp(raw_velocity * 1.3, -0.5, -0.1)

横向控制与居中

在维持距离的同时，无人机还通过计算水平偏移并应用比例控制来将人置于画面中心。

死区：12%的死区可防止人几乎居中时产生不必要的左右振荡。

MAVLink指令执行

速度指令通过DroneKit以20Hz的最大速率发送到飞控（速率限制以防止数据洪泛）。

消息格式：SET_POSITION_TARGET_LOCAL_NED消息，带有机体坐标系速度，忽略位置和加速度以实现平滑跟随。

状态恢复与安全监控

持续监控跟踪丢失、跳跃和安全违规，并自动执行恢复协议。

恢复逻辑：如果跟踪器失败，系统每10帧尝试重新检测，最多30帧，然后进入安全停止状态。

核心算法详解

PID

自适应PID控制

PID控制器维持所需的1.0米跟随距离：

• 比例项 (0.8): 对距离误差的即时响应
• 积分项 (0.25): 消除稳态误差（抗饱和±1.5）
• 微分项 (0.1): 阻尼以防止超调

增益是不对称的：后退运动获得30%更高的增益，以便在人接近时更快响应。

前馈

前馈速度匹配

匹配人行走速度的预测性组件：

• 70%速度匹配: 无人机尝试匹配人估算速度的70%
• 5样本移动窗口: 基于最近5个距离测量值计算速度
• 0.05米/秒阈值: 忽略低于此阈值的噪声

这创造了"预测性跟随"——无人机预测运动而不仅仅是反应。

安全与恢复算法

多层安全系统：

• 紧急后退 (0.6米): 如果距离小于0.6米，强制以0.4米/秒后退
• 跳跃检测 (100像素): 如果中心在帧间移动超过100像素，重新初始化
• 最低高度 (0.3米): 如果低于最低安全高度，禁用控制
• 重新获取 (30次尝试): 在停止前尝试恢复30帧

系统架构与代码结构

关键设计决策

混合架构: 结合v3状态机的稳定性和自适应速度匹配，实现自然的跟随行为。

无头操作: 为没有显示的树莓派设计，使用高效算法和最少的依赖。

不对称控制: 基于安全考虑，向前（0.8米/秒）和向后（0.5米/秒）运动有不同的限制。

Python: 主控制循环track_me_hybrid.py（简化版）

whileTrue:

  # 1. 获取摄像头帧

  ret, frame = camera.read_frame()

  if follow_state == FollowState.FOLLOWING:

    # 2. 更新跟踪器

    ok, box = tracker.update(frame)

    if ok:

      # 3. 计算距离和速度

      distance = estimate_distance_from_bbox_width(box_width)

      person_speed = estimate_person_speed(distance, current_time)

      # 4. 混合控制计算

      vel_x = controller.calculate_velocity(distance, person_speed)

      vel_y = calculate_lateral_control(box_center, frame_center)

      # 5. 如果安全则发送给无人机

      if altitude > MIN_ALT_FOR_CONTROL:

        send_ned_velocity(vel_x, vel_y, 0.0)

    else:

      # 6. 进入恢复模式

      follow_state = FollowState.REACQUIRE

      reacquire_left = REACQUIRE_MAX_FRAMES

  elif follow_state == FollowState.REACQUIRE:

    # 7. 尝试重新检测人

    boxes, _ = model.predict(frame, REDTECT_CONF)

    if boxes:

      # 重新初始化跟踪器并恢复跟随

      tracker.init(frame, best_box)

      follow_state = FollowState.FOLLOWING

性能特性

1.0 米

目标跟随距离

0.8 米/秒

最大前进速度

30 FPS

处理帧率

0.3 米

最低安全高度

实际性能说明

延迟: 总系统延迟（摄像头到电机指令）在树莓派4上约为100-150毫秒。

功耗: 完整系统在5V下消耗约3A，YOLO推理是计算最密集的组件。

范围: 有效跟随范围为0.6米到8米，受限于摄像头分辨率和人检测精度。

安全与故障模式

全面安全协议

1. 跟踪丢失

进入重新获取模式（以较低置信度尝试30次），如果不成功则进入安全停止。

2. 距离过近（＜0.6米）

无论控制信号如何，强制以0.4米/秒后退。

3. 高度过低（＜0.3米）

禁用水平控制，仅允许垂直运动用于降落。

4. 跳跃检测（＞100像素）

假设跟踪器故障，使用新检测重新初始化。

硬件要求与部署

最低硬件要求

• 处理器: 树莓派4 Model B（4GB RAM或更高）
• 摄像头: 树莓派摄像头模块 V2（支持640x480 @ 30FPS）
• 飞控: Pixhawk系列或兼容MAVLink的飞控
• 电源: 5V 3A稳压电源
• 存储: 32GB microSD卡（Class 10或更高）

文件结构

ubuntu@raspberrypi:~
├── Lab/
│ └── yolo/
│ └── track_me_hybrid.py# 主程序
├── test/
│ └── dronekit_wrapper.py# 无人机连接封装
└── yolov8n.onnx# YOLOv8模型文件