From 47fbfe69ca449fc9f5b5e759271653fa3397dd24 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E4=B8=80=E5=96=B5?= Date: Fri, 12 Jun 2026 10:20:22 +0800 Subject: [PATCH] =?UTF-8?q?2026.6.12=E5=B0=8F=E6=9B=B4=E6=96=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- drone_controller/pid_counter.py | 11 +- drone_controller/target_tracker.py | 621 +++++++++++++++++++++++++++++ infer_camera_modular.py | 193 +++++++-- 3 files changed, 782 insertions(+), 43 deletions(-) create mode 100644 drone_controller/target_tracker.py diff --git a/drone_controller/pid_counter.py b/drone_controller/pid_counter.py index 05807a4..15c357d 100644 --- a/drone_controller/pid_counter.py +++ b/drone_controller/pid_counter.py @@ -55,4 +55,13 @@ class PID: """ self.last_err = 0.0 self.integral = 0.0 - self.last_time = time.time() \ No newline at end of file + self.last_time = time.time() + + def reset_integral(self): + """ + 仅清空积分项（I term），保留上一帧误差以维持微分（D term）连续性 + + 用于卡尔曼 coast（滑行预测）期间： + 只靠比例 (P) + 微分 (D) 稳住，积分项清零防止过冲。 + """ + self.integral = 0.0 \ No newline at end of file diff --git a/drone_controller/target_tracker.py b/drone_controller/target_tracker.py new file mode 100644 index 0000000..34aaf6b --- /dev/null +++ b/drone_controller/target_tracker.py @@ -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}])") diff --git a/infer_camera_modular.py b/infer_camera_modular.py index 29a1db1..2079d66 100644 --- a/infer_camera_modular.py +++ b/infer_camera_modular.py @@ -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: