songhaoyu/opi-yolo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

2026.6.12小更新

Browse Source
This commit is contained in:
songhaoyu
2026-06-12 10:20:22 +08:00
parent 84f9e6b2ee
commit 47fbfe69ca
3 changed files with 782 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
drone_controller/pid_counter.py
+10 -1
View File
@@ -55,4 +55,13 @@ class PID:
"""
self.last_err = 0.0
self.integral = 0.0
self.last_time = time.time()
self.last_time = time.time()
def reset_integral(self):
"""
仅清空积分项（I term），保留上一帧误差以维持微分（D term）连续性
用于卡尔曼 coast（滑行预测）期间：
只靠比例 (P) + 微分 (D) 稳住，积分项清零防止过冲。
"""
self.integral = 0.0
drone_controller/target_tracker.py
+621
View File
@@ -0,0 +1,621 @@
"""
target_tracker.py — 轻量级多目标追踪过滤器
【解决的问题】
1. 目标短暂丢帧（2-5帧）导致无人机抽搐 → 卡尔曼滤波 coast/predict 平滑过渡
2. 多目标 ID 飘移导致甩头 → 最邻近欧氏距离关联锁定 ID 不变
【核心设计】
- 恒定速度卡尔曼滤波器（Kalman Filter × 8状态：cx, cy, w, h, vx, vy, vw, vh）
- 最邻近贪婪关联，阈值 200px 内视为同一目标
- 死区预测（Coast/Predict）：连续丢失 ≤ max_lost_frames 帧时用滤波器预测值
- 超过 max_lost_frames → 释放轨道，对外判丢失
【典型用法】
tracker = TargetTracker(max_lost_frames=8)
# 每帧将 YOLO 原始检测喂入
result = tracker.update(yolo_detections, frame_shape=(480, 640))
if result["tracked"]:
cx, cy = result["center"] # 平滑后的中心点（像素坐标）
pid.update(...) # 正常 PID 控制
else:
pid.reset(); send_zero_velocity() # 判丢 → 刹车
【依赖】
- numpy（与推理模块共用，无新增重量级依赖）
- 纯 Python，无 OpenCV / scipy 依赖
"""
import numpy as np
import time
import logging
import copy
logger = logging.getLogger(__name__)
# ================================================================
# 单目标卡尔曼滤波器
# ================================================================
class _KalmanBoxFilter:
"""
单目标恒定速度卡尔曼滤波器
状态向量 (8) : [cx, cy, w, h, vx, vy, vw, vh]
位置 + 尺寸 + 各自速度
观测向量 (4) : [cx, cy, w, h]
所有坐标使用原始像素值（而非归一化），
与 YOLO postprocess 输出的坐标空间一致。
"""
# ---- 噪声权重（经验调优值，适用于 640×480 画面） ----
_STD_POS = 1.0 / 50 # 位置过程噪声
_STD_VEL = 1.0 / 200 # 速度过程噪声
_INI_POS = 2.0 # 初始位置协方差系数
_INI_VEL = 100.0 # 初始速度协方差系数
def __init__(self):
dt = 1.0 # 帧间步长（帧率变化时仍用 dt=1，Q 已含尺度因子）
# ---- 状态转移矩阵 F ----
self.F = np.eye(8, dtype=float)
for i in range(4):
self.F[i, i + 4] = dt # 位置 += 速度 × dt
# ---- 观测矩阵 H ----
self.H = np.zeros((4, 8), dtype=float)
self.H[0, 0] = 1.0
self.H[1, 1] = 1.0
self.H[2, 2] = 1.0
self.H[3, 3] = 1.0
# ---- 协方差初始值 ----
self.P = np.eye(8, dtype=float) * self._INI_POS
for i in range(4, 8):
self.P[i, i] = self._INI_VEL
# ---- 状态向量 ----
self.x = np.zeros(8, dtype=float)
self._initialized = False
# ------------------------------------------------------------------
# 公开接口
# ------------------------------------------------------------------
def initiate(self, measurement):
"""
用首次观测初始化滤波器
Args:
measurement: [cx, cy, w, h] — 像素坐标
"""
cx, cy, w, h = measurement
self.x = np.array([cx, cy, w, h, 0, 0, 0, 0], dtype=float)
# 初始协方差：位置置信度中等，速度置信度低
self.P = np.eye(8, dtype=float) * self._INI_POS
for i in range(4, 8):
self.P[i, i] = self._INI_VEL
self._initialized = True
def predict(self):
"""
预测步骤
Returns:
[cx, cy, w, h] — 预测的像素坐标，未初始化时返回 None
"""
if not self._initialized:
return None
# ---- 动态过程噪声 Q（与当前目标尺寸相关） ----
std_pos = [self._STD_POS * self.x[2], # w 为宽度
self._STD_POS * self.x[3], # h 为高度
self._STD_POS * self.x[2],
self._STD_POS * self.x[3]]
std_vel = [self._STD_VEL * self.x[2],
self._STD_VEL * self.x[3],
self._STD_VEL * self.x[2],
self._STD_VEL * self.x[3]]
q_diag = [s * s for s in std_pos] + [s * s for s in std_vel]
Q = np.diag(q_diag)
# ---- 预测 ----
self.x = self.F @ self.x
self.P = self.F @ self.P @ self.F.T + Q
return self.x[:4].copy()
def update(self, measurement):
"""
更新步骤
Args:
measurement: [cx, cy, w, h] — 观测到的像素坐标
"""
if not self._initialized:
return
z = np.array(measurement, dtype=float)
# ---- 动态观测噪声 R（与目标尺寸相关） ----
std_pos = [self._STD_POS * abs(self.x[2]),
self._STD_POS * abs(self.x[3]),
self._STD_POS * abs(self.x[2]),
self._STD_POS * abs(self.x[3])]
R = np.diag([s * s * 2.0 for s in std_pos]) # ×2 增强平滑
# ---- 卡尔曼增益 ----
S = self.H @ self.P @ self.H.T + R
K = self.P @ self.H.T @ np.linalg.inv(S)
# ---- 后验更新 ----
y = z - self.H @ self.x # 残差（innovation）
self.x = self.x + K @ y
self.P = (np.eye(8) - K @ self.H) @ self.P
def get_prediction(self):
"""返回预测的观测值 [cx, cy, w, h]（未初始化时返回 None）"""
if not self._initialized:
return None
return self.x[:4].copy()
def get_state(self):
"""返回完整状态向量（未初始化时返回 None）"""
if not self._initialized:
return None
return self.x.copy()
@property
def initialized(self):
return self._initialized
# ================================================================
# 跟踪轨道（Tracklet）
# ================================================================
class _Tracklet:
"""
一条跟踪轨道，包装 _KalmanBoxFilter + 辅助状态
生命周期：
创建（首次检测到）→ 跟踪中（可能 coast）→ 超过丢失上限 → 回收
"""
__slots__ = (
"track_id", "kf", "lost_count", "max_lost",
"confidence", "cls_id",
"age", "hits", "last_center", "last_box",
)
def __init__(self, track_id, measurement, confidence, cls_id, max_lost=8):
"""
Args:
track_id: 全局唯一轨道ID
measurement: [cx, cy, w, h]
confidence: 检测置信度
cls_id: 类别ID
max_lost: 最大连续丢失帧数
"""
self.track_id = track_id
self.kf = _KalmanBoxFilter()
self.kf.initiate(measurement)
self.lost_count = 0
self.max_lost = max_lost
self.confidence = confidence
self.cls_id = cls_id
self.age = 1 # 已存在帧数
self.hits = 1 # 成功匹配次数
cx, cy, w, h = measurement
self.last_center = (float(cx), float(cy))
self.last_box = None # 首次调用 predict/update 后设置
def predict(self):
"""预测 -> 更新 last_center / last_box"""
pred = self.kf.predict()
if pred is not None:
cx, cy, w, h = pred
self.last_center = (float(cx), float(cy))
self.last_box = self._center_to_box(cx, cy, w, h)
self.age += 1
return self.last_center, self.last_box
def update(self, detected_box, confidence, cls_id):
"""
用检测结果更新轨道
Args:
detected_box: [x1, y1, x2, y2]
confidence: 检测置信度
cls_id: 类别ID
"""
cx = (detected_box[0] + detected_box[2]) / 2.0
cy = (detected_box[1] + detected_box[3]) / 2.0
w = detected_box[2] - detected_box[0]
h = detected_box[3] - detected_box[1]
self.kf.update([cx, cy, w, h])
self.last_center = (float(cx), float(cy))
self.last_box = detected_box
self.confidence = confidence
self.cls_id = cls_id
self.lost_count = 0
self.hits += 1
self.age += 1
def mark_miss(self):
"""标记当前帧未匹配到检测"""
self.lost_count += 1
self.age += 1
@property
def is_dead(self):
"""超过最大丢失帧数时标记为死亡，可回收"""
return self.lost_count > self.max_lost
@property
def is_coasting(self):
"""当前帧是否处在预测（无实测）状态"""
return self.lost_count > 0
@staticmethod
def _center_to_box(cx, cy, w, h):
"""[cx, cy, w, h] → [x1, y1, x2, y2]"""
half_w = w / 2.0
half_h = h / 2.0
return [cx - half_w, cy - half_h, cx + half_w, cy + half_h]
def __repr__(self):
return (f"_Tracklet(id={self.track_id}, "
f"lost={self.lost_count}/{self.max_lost}, "
f"age={self.age}, hits={self.hits})")
# ================================================================
# 多目标追踪器
# ================================================================
class TargetTracker:
"""
轻量级多目标追踪器
职责：
- 接收每帧原始 YOLO 检测列表
- 通过卡尔曼滤波 + 最邻近关联维持稳定目标ID
- 提供平滑后的目标框 / 中心点供 PID 控制使用
- 自动回收超过丢失上限的旧轨道
典型配置（建议在实例化时按需调整）：
| 参数 | 默认值 | 说明 |
|------|--------|------|
| max_lost_frames | 8 | 目标连续消失帧数上限（~250ms @ 30fps） |
| max_association_dist | 200 | 关联距离阈值（像素），超过视为新目标 |
| min_hits | 3 | 新轨道确认前需要连续匹配帧数 |
update() 返回值:
{
"tracked": bool — 是否存在有效主目标
"primary_id": int — 主目标 ID（None 如果无目标）
"center": (cx,cy) — 平滑后中心像素坐标
"box": [x1,y1,x2,y2] — 平滑后边界框
"confidence": float — 当前置信度（coast 期间从最近测量继承）
"is_predicted": bool — True = 当前帧 coasting（无实测）
"lost_frames": int — 连续未命中帧数
"n_active": int — 当前活跃轨道总数
"raw": [...] — 关联到的原始检测（coast 时为 None）
}
或丢失时:
{
"tracked": False,
"primary_id": None,
...
"lost_frames": > max_lost_frames
}
"""
def __init__(self, max_lost_frames=8, max_association_dist=200.0,
min_hits=3):
"""
Args:
max_lost_frames: 最大连续丢失帧数（默认 8 ≈ 250-300ms）
max_association_dist:关联距离阈值（像素，默认 200px）
min_hits: 新轨道确认前最少连续匹配帧数（默认 3）
"""
self.max_lost_frames = max_lost_frames
self.max_association_dist = max_association_dist
self.min_hits = min_hits
self._next_id = 0
self._tracks = {} # {track_id: _Tracklet}
self._frame_count = 0
# 缓存上一帧的 primary 结果（锁外创建副本供控制线程读取）
self._last_result = {
"tracked": False,
"primary_id": None,
"center": None,
"box": None,
"confidence": 0.0,
"is_predicted": False,
"lost_frames": 0,
"n_active": 0,
"raw": None,
}
# ------------------------------------------------------------------
# 主接口
# ------------------------------------------------------------------
def update(self, detections, frame_shape=None):
"""
每帧调用一次，更新追踪状态
Args:
detections: YOLO postprocess 输出的检测列表
[[x1, y1, x2, y2, conf, cls_id], ...]
允许为空列表 []（表示无检测）
frame_shape: (height, width) 用于边界钳制，可选
Returns:
dict — 主目标跟踪结果（格式见类文档）
"""
self._frame_count += 1
# ---- 步骤 1：所有活跃轨道先 predict ----
for tid in list(self._tracks.keys()):
track = self._tracks[tid]
track.predict()
# ---- 步骤 2：关联 — 提取检测中心 ----
if detections:
# 提取检测中心 + 尺寸
det_centers = [] # [(idx, cx, cy, w, h, conf, cls_id)]
for i, det in enumerate(detections):
x1, y1, x2, y2, conf, cls_id = det
cx = (x1 + x2) / 2.0
cy = (y1 + y2) / 2.0
w = x2 - x1
h = y2 - y1
det_centers.append((i, cx, cy, w, h, conf, cls_id))
# 关联
matches, unmatched_tids, unmatched_dids = self._associate(det_centers)
else:
matches = []
unmatched_tids = list(self._tracks.keys())
unmatched_dids = []
# ---- 步骤 3：更新匹配的轨道 ----
for tid, did in matches:
_, cx, cy, w, h, conf, cls_id = det_centers[did]
raw_box = [cx - w/2, cy - h/2, cx + w/2, cy + h/2]
self._tracks[tid].update(raw_box, conf, cls_id)
# ---- 步骤 4：未匹配的轨道 → mark_miss / 回收 ----
for tid in unmatched_tids:
if tid in self._tracks:
self._tracks[tid].mark_miss()
if self._tracks[tid].is_dead:
logger.debug("轨道 %d 已死亡 (lost=%d), 回收",
tid, self._tracks[tid].lost_count)
del self._tracks[tid]
# ---- 步骤 5：未匹配的检测 → 创建新轨道 ----
for did in unmatched_dids:
_, cx, cy, w, h, conf, cls_id = det_centers[did]
measurement = [cx, cy, w, h]
new_track = _Tracklet(
track_id=self._next_id,
measurement=measurement,
confidence=conf,
cls_id=cls_id,
max_lost=self.max_lost_frames,
)
# 新轨道直接使用测量值作为 last_box
new_track.last_box = [cx - w/2, cy - h/2, cx + w/2, cy + h/2]
self._tracks[self._next_id] = new_track
self._next_id += 1
# ---- 步骤 6：选取主目标（优先级：置信度 > 命中数 > 贴近画面中心） ----
result = self._select_primary(frame_shape)
self._last_result = result
return result
def get_last_result(self):
"""获取上一帧的跟踪结果（线程安全，外部可直接读取）"""
return self._last_result
def reset(self):
"""重置所有轨道（清空追踪状态）"""
self._tracks.clear()
self._next_id = 0
self._frame_count = 0
self._last_result = {
"tracked": False,
"primary_id": None,
"center": None,
"box": None,
"confidence": 0.0,
"is_predicted": False,
"lost_frames": 0,
"n_active": 0,
"raw": None,
}
logger.info("TargetTracker 已重置")
# ------------------------------------------------------------------
# 属性
# ------------------------------------------------------------------
@property
def active_tracks(self):
"""返回当前所有活跃轨道（dict: {track_id: _Tracklet} 的浅复制）"""
return dict(self._tracks)
@property
def active_count(self):
"""当前活跃轨道数"""
return len(self._tracks)
# ------------------------------------------------------------------
# 内部方法
# ------------------------------------------------------------------
def _associate(self, det_centers):
"""
最邻近贪婪关联
Args:
det_centers: [(idx, cx, cy, w, h, conf, cls_id), ...]
Returns:
(matches, unmatched_tids, unmatched_dids)
"""
if not self._tracks or not det_centers:
return [], list(self._tracks.keys()), list(range(len(det_centers)))
# 提取所有轨道的预测位置
track_preds = {} # {tid: (cx, cy)}
for tid, track in self._tracks.items():
cx, cy = track.last_center
track_preds[tid] = (cx, cy)
# 贪婪匹配：每条轨道找最近的未分配检测
matches = []
assigned_dets = set()
assigned_tracks = set()
# 先将轨道按置信度排序（更可靠的轨道优先匹配）
sorted_tracks = sorted(
self._tracks.items(),
key=lambda kv: kv[1].confidence,
reverse=True
)
for tid, track in sorted_tracks:
if tid in assigned_tracks:
continue
pred_cx, pred_cy = track_preds[tid]
best_dist = self.max_association_dist
best_did = None
for did, _, d_cx, d_cy, _, _, _ in det_centers:
if did in assigned_dets:
continue
dist = np.hypot(pred_cx - d_cx, pred_cy - d_cy)
if dist < best_dist:
best_dist = dist
best_did = did
if best_did is not None:
matches.append((tid, best_did))
assigned_dets.add(best_did)
assigned_tracks.add(tid)
unmatched_tids = [tid for tid in self._tracks if tid not in assigned_tracks]
unmatched_dids = [did for did in range(len(det_centers)) if did not in assigned_dets]
return matches, unmatched_tids, unmatched_dids
def _select_primary(self, frame_shape=None):
"""
从活跃轨道中选出主目标
优先级：
1. 含有实测（非 coasting）且确认（hits ≥ min_hits）
2. 实测轨道中置信度最高
3. 若无实测轨道，取 coast 中最接近画面中心的
Returns:
dict — 与 update() 返回值格式一致
"""
if not self._tracks:
return {
"tracked": False,
"primary_id": None,
"center": None,
"box": None,
"confidence": 0.0,
"is_predicted": False,
"lost_frames": 0,
"n_active": 0,
"raw": None,
}
# 分组：实测轨道 vs coasting 轨道
measured = []
coasting = []
for tid, track in self._tracks.items():
entry = (tid, track)
if track.lost_count == 0 and track.hits >= self.min_hits:
measured.append(entry)
elif track.lost_count == 0:
# hits < min_hits 尚未确认，视为暂态
measured.append(entry)
else:
coasting.append(entry)
# 优先从实测轨道选
chosen = None
is_predicted = False
if measured:
# 置信度最高的实测轨道
measured.sort(key=lambda kv: kv[1].confidence, reverse=True)
chosen = measured[0]
elif coasting:
# 无实测时选最靠近画面中心的
cx_frame = frame_shape[1] / 2.0 if frame_shape else 320.0
cy_frame = frame_shape[0] / 2.0 if frame_shape else 240.0
coasting.sort(key=lambda kv: (
(kv[1].last_center[0] - cx_frame) ** 2 +
(kv[1].last_center[1] - cy_frame) ** 2
))
chosen = coasting[0]
is_predicted = True
if chosen is None:
return {
"tracked": False,
"primary_id": None,
"center": None,
"box": None,
"confidence": 0.0,
"is_predicted": False,
"lost_frames": 0,
"n_active": len(self._tracks),
"raw": None,
}
tid, track = chosen
cx, cy = track.last_center
box = track.last_box
conf = track.confidence
lost = track.lost_count
return {
"tracked": True,
"primary_id": tid,
"center": (float(cx), float(cy)),
"box": box,
"confidence": float(conf),
"is_predicted": is_predicted,
"lost_frames": lost,
"n_active": len(self._tracks),
"raw": (None if is_predicted else box),
}
def __repr__(self):
tracks_info = ", ".join(str(t) for t in self._tracks.values())
return (f"TargetTracker(frame={self._frame_count}, "
f"active={len(self._tracks)}, "
f"tracks=[{tracks_info}])")
infer_camera_modular.py
+151 -42
View File
@@ -23,6 +23,7 @@ except ImportError:
# ================== ZMQ Router 通信代理 =====================
from drone_controller.router_proxy import RouterProxy
from drone_controller.pid_counter import PID
from drone_controller.target_tracker import TargetTracker
# ===========================================================
@@ -277,6 +278,17 @@ class UAVControlLoop:
self.pid_y = PID(**pid_y_cfg)
self.pid_z = PID(**pid_z_cfg)
# ---- 轻量级目标追踪过滤器 ----
# 平滑 YOLO 检测、稳定 ID、coast 预测
tracker_cfg = fc_cfg.get("target_tracker", {})
self.tracker = TargetTracker(
max_lost_frames=tracker_cfg.get("max_lost_frames", 8),
max_association_dist=tracker_cfg.get("max_association_dist", 200.0),
min_hits=tracker_cfg.get("min_hits", 3),
)
self._tracked_result = None # 跟踪结果（锁保护）
# ------------------------------------------------
# 控制参数（按协议改为 20Hz，§4.1 要求 ≥20Hz）
self.control_hz = 20.0
self.control_interval = 1.0 / self.control_hz
@@ -285,7 +297,7 @@ class UAVControlLoop:
self.running = False
self.control_thread = None
self.current_target = (0.0, 0.0, 0.0)
self.detections = [] # 最新检测结果（线程间共享）
self.detections = [] # 最新检测结果（仅用于渲染，线程间共享）
self.frame_shape = (480, 640) # 默认尺寸
# 锁保护共享数据
@@ -294,6 +306,9 @@ class UAVControlLoop:
# ---- 防断流戳：主线程每次 update_detections 时更新 ----
self._last_update_time = 0.0
# ---- 安全刹车请求标志（主线程判丢 → 控制线程执行刹车） ----
self._brake_requested = False
def start_uav(self):
"""连接 Router，执行标准起飞序列（协议 §3.3.1）
@@ -437,48 +452,95 @@ class UAVControlLoop:
def update_detections(self, detections, frame_shape):
"""由主循环调用，更新最新检测结果和图像尺寸
处理流程：
1. 用 YOLO 原始检测更新 TargetTracker（获取平滑/预测结果）
2. Coast 滑行期间 → PID 积分清空（只靠 PD 稳住，不充积分）
3. 判定彻底丢失 → 请求控制线程执行安全零速刹车
4. 将结果写入线程安全共享区，供控制线程消费
Args:
detections: postprocess 后的检测列表 [[x1,y1,x2,y2,conf,cls_id], ...]
frame_shape: 原始帧形状 (height, width)
Returns:
(tracked: bool, result: dict)
— tracked: 当前是否存在有效主目标
— result: 跟踪器完整输出（含 center, box, is_predicted, lost_frames 等）
"""
# ---- 跟踪过滤器：处理原始检测，输出平滑中心点 ----
# 计算在锁外进行，避免阻塞控制线程（Kalman 计算 < 0.1ms 量级）
result = self.tracker.update(detections, frame_shape)
tracked = result.get("tracked", False)
# ------------------------------------------------
# ---- ① Coast 滑行期间：PID 积分清空 ----
# 卡尔曼预测阶段目标可能漂移，积分项在此累积会导致
# 目标重获瞬间积分暴冲（I-term windup）
if tracked and result.get("is_predicted"):
self.pid_y.reset_integral()
self.pid_z.reset_integral()
# ---- ② 目标彻底丢失：请求控制线程执行刹车 ----
# 注意：不在此处直接调 proxy.send_setpoint 以避免 REQ-REP
# 并发冲突（控制线程也在使用同一 REQ socket），改用标志位
# 由控制线程在下一 20Hz 周期拾取并发送刹车指令
if not tracked:
self.pid_y.reset_integral()
self.pid_z.reset_integral()
with self.data_lock:
self._brake_requested = True
with self.data_lock:
self.detections = detections
self.frame_shape = frame_shape
self._last_update_time = time.time()
self._tracked_result = result
return tracked, result
# ---- 内部控制线程 ----
def _control_loop(self):
"""独立线程：以 20Hz 发送 SETPOINT，保证 OFFBOARD 保活
控制流：
1. 读取 TargetTracker 的平滑结果（免锁快照）
2. 防断流检测（主线程 500ms 无更新 → 强制判丢）
3. 跟踪器判丢 → PID 复位 → 零速刹车
4. 跟踪器锁定 → 用平滑中心点计算 PID → VELOCITY SETPOINT
无论是否有目标，每次迭代都发送 VELOCITY SETPOINT。
无目标时速度全零（悬停刹车），满足协议 ≥20Hz 要求。
"""
while self.running:
start = time.time()
# 获取最新的检测结果（加锁）
# ---- ① 读取跟踪结果和帧尺寸（加锁快照） ----
with self.data_lock:
dets = self.detections.copy()
tracked = self._tracked_result
h, w = self.frame_shape[:2]
last_update = self._last_update_time
# ---- 防断流检测：超过 500ms 无更新则强制零速 ----
# 覆盖主线程卡死、摄像头断流等场景
# ---- ② 防断流检测 ----
# 主线程超过 500ms 未更新 → 强制判丢（覆盖摄像头断流/主线程卡死）
if time.time() - last_update > 0.5:
if len(dets) > 0:
# 只在首次过期时打印，避免刷屏
pass # 保留静默刹车
dets = [] # 清空检测 → PID 复位 → 零速
self.pid_y.reset()
self.pid_z.reset()
# ------------------------------------------------
tracked = None
# 计算速度指令
vx, vy, vz = self._compute_velocity(dets, w, h)
# ---- ③ 解析跟踪结果 → 速度指令（使用跟踪器输出的平滑中心点） ----
vx, vy, vz = self._velocity_from_tracker(tracked, w, h)
# 通过 ZMQ 发送 VELOCITY SETPOINT（协议 §3.1 速度控制）
# 无论是否有目标，都持续发送以满足 20Hz 保活要求
# ---- ③' 安全刹车请求：主线程判丢时紧急抱闸 ----
# update_detections 检测到目标彻底丢失后设置此标志，
# 控制线程在此拾取并执行一次全零 VELOCITY 刹车
with self.data_lock:
if self._brake_requested:
self._brake_requested = False
vx, vy, vz = 0.0, 0.0, 0.0
# 同时清空 PID（确保 PD 也归零）
self.pid_y.reset()
self.pid_z.reset()
# ---- ④ 通过 ZMQ 发送 VELOCITY SETPOINT（协议 §3.1 速度控制） ----
ok, ack = self.proxy.send_setpoint(
vx=vx, vy=vy, vz=vz,
yaw_rate=0.0,
@@ -489,42 +551,49 @@ class UAVControlLoop:
# recv 超时或格式错误由 _send_req 内部恢复 REQ socket
pass
# 精确控制循环频率（20Hz → 每 50ms）
# ---- ⑤ 精确控制循环频率（20Hz → 每 50ms） ----
elapsed = time.time() - start
sleep_time = self.control_interval - elapsed
if sleep_time > 0:
time.sleep(sleep_time)
def _compute_velocity(self, detections, frame_w, frame_h):
"""根据检测结果计算机体速度 (vx, vy, vz)
def _velocity_from_tracker(self, tracked, frame_w, frame_h):
"""
根据跟踪器输出计算机体速度 (vx, vy, vz)
与原始逻辑相同：
- 无目标 → PID 复位 → 返回零速（悬停/刹车）
- 有目标 → 取置信度最高的 → 偏差归一化 → PID 输出
这是控制线程使用的唯一速度计算路径。
平滑后的中心点 → 归一化误差 → PID 输出。
Args:
tracked: tracker.update() 返回的 result dict（或 None）
frame_w: 画面宽度（像素）
frame_h: 画面高度（像素）
Returns:
(vx, vy, vz): NED 坐标系速度 (m/s)
"""
if not detections:
# 无目标：悬停，并重置 PID 积分
if tracked is None or not tracked.get("tracked"):
self.pid_y.reset()
self.pid_z.reset()
return 0.0, 0.0, 0.0
# 取置信度最高的目标
best = max(detections, key=lambda x: x[4])
x1, y1, x2, y2, _conf, _cls_id = best
target_x = (x1 + x2) / 2
target_y = (y1 + y2) / 2
img_center_x = frame_w / 2
img_center_y = frame_h / 2
cx, cy = tracked["center"]
img_cx = frame_w / 2.0
img_cy = frame_h / 2.0
err_x = (target_x - img_center_x) / img_center_x
err_y = (target_y - img_center_y) / img_center_y
err_x = (cx - img_cx) / img_cx
err_y = (cy - img_cy) / img_cy
vy = self.pid_y.update(err_x) # 左右移动
vz = self.pid_z.update(err_y) # 上下移动（NED 中正为下）
vx = 0.0 # 前后移动
vx = 0.0 # 前后维持不变
# ---- 滑行预测期间：强制清空积分项 ----
# 双重保险（update_detections 中也有一道）：
# 确保 coast 帧只靠 PD 出力，积分项绝不充能
if tracked.get("is_predicted"):
self.pid_y.reset_integral()
self.pid_z.reset_integral()
return vx, vy, vz
@@ -543,8 +612,11 @@ class UAVControlLoop:
)
time.sleep(0.2) # 确保指令发出
self.tracker.reset()
self._tracked_result = None
self.proxy.close()
print("控制已停止，ZMQ 代理已关闭")
print("控制已停止，ZMQ 代理已关闭，跟踪器已重置")
# ==================== 主程序入口 ======================
@@ -599,26 +671,63 @@ def main():
# 3. 坐标反求与控制计算
detections = detector.postprocess(outputs, ratio, dwdh, orig_shape)
controller.update_detections(detections, orig_shape)
tracked, track_res = controller.update_detections(detections, orig_shape)
# 4. 界面渲染
frame = detector.draw_boxes(frame, detections)
cv2.putText(frame, f"NPU Pure FPS: {fps_pure:.1f}", (20, 30),
# ================================================================
# 4. "所见即所得" 界面渲染
# 控制与渲染的坐标绝对值统一：绘制追踪器的平滑框，不绘制原始 YOLO 框
# ┌──────────┬──────────┬──────────┐
# │ 状态 │ 框颜色 │ 标签 │
# ├──────────┼──────────┼──────────┤
# │ TRACK │ 亮绿(0,255,0) │ ID:1 [MEAS] │
# │ COAST │ 黄(0,255,255) │ ID:1 [COAST]│
# │ LOST │ 不画框 │ 仅文字 │
# └──────────┴──────────┴──────────┘
# ================================================================
# 追踪器锁定 → 绘制平滑框
if tracked and track_res.get("box") is not None:
bx1, by1, bx2, by2 = track_res["box"]
is_coast = track_res.get("is_predicted", False)
box_color = (0, 255, 255) if is_coast else (0, 255, 0) # COAST=黄, MEAS=绿
label = f"ID:{track_res.get('primary_id','?')}"
label += " [COAST]" if is_coast else " [MEAS]"
cv2.rectangle(frame, (int(bx1), int(by1)), (int(bx2), int(by2)),
box_color, 3)
cv2.putText(frame, label, (int(bx1), int(by1) - 12),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, box_color, 2)
# 左上角状态面板
cv2.putText(frame, f"NPU FPS: {fps_pure:.1f}", (20, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
if track_res:
status_str = ("✅" if tracked else "❌")
lost = track_res.get("lost_frames", 0)
n_act = track_res.get("n_active", 0)
cv2.putText(frame, f"Tracker: {status_str} ID:{track_res.get('primary_id','-')} "
f"Lost:{lost}f Act:{n_act}",
(20, 55), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 1)
# 5. 推送至后台异步图传队列
streamer.push_to_stream(frame)
# 6. 周期性打印 Router 状态（每 100 帧）
# 6. 周期性打印 Router + Tracker 状态（每 100 帧）
if loop_count % 100 == 0:
state = controller.proxy.get_latest_state()
tracker_info = (f" TK:{'✅' if tracked else '❌'}"
f" ID:{track_res.get('primary_id','?')}"
f" Lost:{track_res.get('lost_frames',0)}f"
f" {'PRED' if track_res.get('is_predicted') else 'MEAS'}"
f" Act:{track_res.get('n_active',0)}")
if state:
d = state["drone"]
age_us, _ = controller.proxy.get_state_freshness(state)
age_ms = (age_us or 0) / 1000
print(f"📡 [Router] mode={d['mode']} armed={d['armed']} "
print(f"📡 Router: mode={d['mode']} armed={d['armed']} "
f"alt={d['alt_rel']:.1f}m spd={d['ground_speed']:.1f}m/s "
f"batt={d['battery']:.0f}% state_age={age_ms:.0f}ms",
f"batt={d['battery']:.0f}% age={age_ms:.0f}ms"
f"{tracker_info}",
end="" if loop_count % 500 != 0 else "\n")
except KeyboardInterrupt: