光流法与硬件连接

Optoelectronics/transform_app.py

@@ -239,7 +239,7 @@ def calculate_target_gps(ptz_lat, ptz_lon, ptz_alt, pan, tilt, distance):
 def main():
     sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
     # 实际测试时请修改为光电外设的正确 IP
-    sock.connect(('192.168.1.64', 39020))
+    sock.connect(('10.10.61.64', 39020))
 
     # 【修复 Bug 2】：传入文档推测的默认密码 'Abc.12345'
     token = login_to_ipc(sock, 'admin', 'Abc.12345')

config.json

@@ -17,6 +17,7 @@
         "sub_endpoint": "tcp://127.0.0.1:5556",
         "takeoff_alt": 5.0,
         "control_hz": 20,
+        "cruise_speed":1.0,
         "target_tracker": {
             "max_lost_frames": 8,
             "max_asscociation_dist": 200.0,
@@ -39,11 +40,22 @@
         "return_to_home": false,
         "target_lost_timeout": null
     },
+    "optical_flow":{
+        "grid_step":25,
+        "reset_threshold":30
+    },
     "log": {
     "enable_flight_log": true,
     "flight_log_interval": 20,
     "log_dir": "./log"
     },
+    "pid_x":{
+        "kp": 1.2,
+        "ki": 0.1,
+        "kd": 0.05,
+        "max_out": 0.8,
+        "min_out": -0.8
+    },
     "pid_y": {
         "kp": 1.0,
         "ki": 0.02,
@@ -57,5 +69,12 @@
         "kd": 0.3,
         "max_out": 0.4,
         "min_out": -0.4
+    },
+    "pid_yaw":{
+        "kp":0.8,
+        "ki":0.05,
+        "kd":0.0,
+        "max_out":1.0,
+        "min_out":0.0
     }
 }

drone_controller/optical_flow_controller.py

@@ -0,0 +1,482 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+光流法障碍检测 - 网格点追踪版（复位点不参与统计）
+特性：
+1. 全图均匀网格点，覆盖无盲区
+2. 点漂移后硬复位（当前帧和上一帧均置为初始位置）
+3. 复位点在该帧不参与任何统计（散度、平均光流等）
+4. 分区（左/中/右）风险统计，仅基于有效运动点
+5. 与 PID 和 MAVLink 无缝集成
+"""
+
+import cv2
+import numpy as np
+import time
+from collections import deque
+from typing import Optional, Dict, Any
+
+# ==================== PID 部分 =====================
+from dataclasses import dataclass
+
+
+@dataclass
+class PIDConfig:
+    """PID 参数配置"""
+    kp: float
+    ki: float
+    kd: float
+    max_out: float
+    min_out: float
+    dt_default: float = 0.033
+    alpha: float = 0.2  # 微分滤波系数 (0 < alpha <= 1)
+
+
+class ImprovedPID:
+    def __init__(self, config: PIDConfig):
+        # 参数校验
+        if config.max_out <= config.min_out:
+            raise ValueError("max_out must be greater than min_out")
+        if not (0 < config.alpha <= 1):
+            raise ValueError("alpha must be in (0, 1]")
+
+        self.cfg = config
+
+        self.last_err: float = 0.0
+        self.last_fb: float = 0.0
+        self.integral: float = 0.0
+        self.last_d: float = 0.0
+        self.last_time: float = time.perf_counter()
+        self.first_run: bool = True   # 标记首次调用
+
+    def update(self, error: float, feedback: Optional[float] = None, dt: Optional[float] = None) -> float:
+        current_time = time.perf_counter()
+
+        # --- 首次调用处理：初始化状态，避免微分冲击 ---
+        if self.first_run:
+            self.first_run = False
+            self.last_err = error
+            if feedback is not None:
+                self.last_fb = feedback
+            self.last_time = current_time
+            # 首次仅返回比例项，微分和积分暂不参与（或也可返回0，视应用而定）
+            # 通常直接返回 P 项能让系统平滑起步
+            return self.cfg.kp * error
+
+        # 1. 计算或使用传入的 dt
+        if dt is None:
+            dt = current_time - self.last_time
+            if dt <= 0:
+                dt = self.cfg.dt_default
+
+        # 2. 比例项 (P)
+        p_out = self.cfg.kp * error
+
+        # 3. 微分项 (D) —— 微分先行 + 低通滤波
+        if feedback is not None:
+            # 对反馈微分，设定值变化不引入冲击
+            d_raw = (self.last_fb - feedback) / dt
+            self.last_fb = feedback
+        else:
+            d_raw = (error - self.last_err) / dt
+
+        self.last_d = self.cfg.alpha * d_raw + (1.0 - self.cfg.alpha) * self.last_d
+        d_out = self.cfg.kd * self.last_d
+
+        # 4. 积分项 (I) —— 先计算潜在值
+        potential_integral = self.integral + error * dt
+        i_out = self.cfg.ki * potential_integral
+
+        # 5. 总输出与抗积分饱和（基于积分方向判断）
+        output = p_out + i_out + d_out
+
+        if output > self.cfg.max_out:
+            output = self.cfg.max_out
+            # 如果积分项仍在推高输出（i_out > 0），则冻结积分
+            if i_out > 0:
+                pass   # 不更新 self.integral
+            else:
+                self.integral = potential_integral
+        elif output < self.cfg.min_out:
+            output = self.cfg.min_out
+            # 如果积分项仍在拉低输出（i_out < 0），则冻结积分
+            if i_out < 0:
+                pass
+            else:
+                self.integral = potential_integral
+        else:
+            # 未饱和，正常更新积分
+            self.integral = potential_integral
+
+        # 6. 更新状态变量
+        self.last_err = error
+        self.last_time = current_time
+
+        return output
+
+    def reset(self) -> None:
+        """重置所有状态，恢复到首次运行前的状态"""
+        self.last_err = 0.0
+        self.last_fb = 0.0
+        self.integral = 0.0
+        self.last_d = 0.0
+        self.last_time = time.perf_counter()
+        self.first_run = True
+
+# =================================================
+
+
+class OpticalFlowObstacleDetector:
+    """网格点光流检测器，复位点不参与统计"""
+    def __init__(self, grid_step: int = 25, reset_threshold: float = 30.0):
+        """
+        Args:
+            grid_step: 网格点间距（像素），值越小点越多
+            reset_threshold: 点偏离初始位置超过此值时复位（像素）
+        """
+        self.grid_step = grid_step
+        self.reset_threshold = reset_threshold
+
+        # 光流跟踪参数
+        self.lk_params = dict(
+            winSize=(21, 21),
+            maxLevel=3,
+            criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 30, 0.01)
+        )
+
+        # 状态变量
+        self.old_gray = None
+        self.p0 = None               # 当前点坐标 (N,1,2)
+        self.grid_initial = None     # 初始网格坐标 (N,2)
+        self.frame_count = 0
+
+        # 分区统计历史（用于兜底）
+        self.last_zone_stats = None
+        self.flow_history = deque(maxlen=10)
+
+    def _generate_grid(self, img_w: int, img_h: int) -> np.ndarray:
+        """生成网格点，返回 (N,2) 坐标数组"""
+        x = np.arange(self.grid_step // 2, img_w, self.grid_step, dtype=np.float32)
+        y = np.arange(self.grid_step // 2, img_h, self.grid_step, dtype=np.float32)
+        xv, yv = np.meshgrid(x, y)
+        points = np.stack((xv.ravel(), yv.ravel()), axis=1)
+        return points
+
+    def process_frame(self, frame):
+        """
+        处理单帧，返回 (is_collision, flow_data)
+        更新 self.last_zone_stats（仅包含有效点）
+        """
+        self.frame_count += 1
+        if frame is None:
+            return False, None
+
+        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+        h, w = gray.shape
+
+        # --- 1. 初始化第一帧 ---
+        if self.old_gray is None:
+            self.old_gray = gray
+            grid_pts = self._generate_grid(w, h)
+            self.grid_initial = grid_pts.copy()
+            self.p0 = grid_pts.reshape(-1, 1, 2).astype(np.float32)
+            self.last_zone_stats = None
+            return False, None
+
+        # --- 2. 光流跟踪 ---
+        p1, st, err = cv2.calcOpticalFlowPyrLK(
+            self.old_gray, gray, self.p0, None, **self.lk_params
+        )
+
+        if p1 is None:
+            p1 = self.p0.copy()
+            st = np.ones(len(self.p0), dtype=np.uint8)
+
+        # 将跟踪结果展开为 (N,2)
+        p1_flat = p1.reshape(-1, 2)
+        p0_flat = self.p0.reshape(-1, 2)
+        st_flat = st.flatten()
+
+        # 初始化新的点数组（默认保持旧位置）
+        new_points = p0_flat.copy()
+        old_points = p0_flat.copy()   # 上一帧位置，用于计算光流
+
+        # 成功跟踪的点，先更新位置
+        success_indices = np.where(st_flat == 1)[0]
+        if len(success_indices) > 0:
+            new_points[success_indices] = p1_flat[success_indices]
+            old_points[success_indices] = p0_flat[success_indices]
+
+        # --- 3. 复位逻辑与有效点掩码生成 ---
+        # 复位掩码：True 表示该点需要复位（跟踪失败或漂移过大）
+        reset_mask = np.zeros(len(new_points), dtype=bool)
+
+        # 3.1 跟踪失败的点直接复位
+        failed_indices = np.where(st_flat == 0)[0]
+        reset_mask[failed_indices] = True
+
+        # 3.2 检查成功点是否漂移过大
+        for idx in success_indices:
+            if np.linalg.norm(new_points[idx] - self.grid_initial[idx]) > self.reset_threshold:
+                reset_mask[idx] = True
+
+        # 执行复位（将当前和上一帧位置均设为初始位置）
+        if np.any(reset_mask):
+            new_points[reset_mask] = self.grid_initial[reset_mask]
+            old_points[reset_mask] = self.grid_initial[reset_mask]
+
+        # --- 4. 生成有效点掩码（未复位且跟踪成功） ---
+        # 有效点：跟踪成功且未复位
+        valid_mask = np.ones(len(new_points), dtype=bool)
+        valid_mask[reset_mask] = False
+
+        # --- 5. 更新状态 ---
+        self.p0 = new_points.reshape(-1, 1, 2).astype(np.float32)
+        self.old_gray = gray
+
+        # 如果有效点太少，不进行统计（但保留状态）
+        if np.sum(valid_mask) < 5:
+            # 保留上一帧统计值，或返回空
+            return False, None
+
+        # --- 6. 计算光流向量（仅针对有效点） ---
+        valid_old = old_points[valid_mask]
+        valid_new = new_points[valid_mask]
+        flow_vectors = valid_new - valid_old   # (M,2)
+        mean_mag = np.mean(np.linalg.norm(flow_vectors, axis=1))
+
+        # --- 7. 分区统计（仅使用有效点） ---
+        zone_stats = self._compute_zone_stats(valid_old, flow_vectors, w, h)
+        self.last_zone_stats = zone_stats
+
+        # 整体散度（用于碰撞警告）
+        divergence = self._compute_divergence(valid_old, flow_vectors, w, h)
+
+        # 碰撞判断（保留阈值）
+        is_collision = False
+        if divergence > 0.8 and mean_mag > 1.0:
+            is_collision = True
+        elif mean_mag > 3.0 and len(valid_old) > 50:
+            if self._check_roi_expansion(valid_old, flow_vectors):
+                is_collision = True
+
+        # 历史记录
+        self.flow_history.append({
+            'mean_flow': np.mean(flow_vectors, axis=0),
+            'mean_mag': mean_mag,
+            'divergence': divergence,
+            'points': np.sum(valid_mask)
+        })
+
+        return is_collision, {
+            'points': np.sum(valid_mask),
+            'mean_mag': mean_mag,
+            'divergence': divergence,
+        }
+
+    # ---------- 辅助方法（与之前一致，但传入的点已过滤） ----------
+    def _compute_divergence(self, old_points, flow_vectors, img_w, img_h):
+        if len(old_points) < 5:
+            return 0.0
+        center = np.array([img_w / 2, img_h / 2])
+        dir_to_center = center - old_points
+        dir_to_center /= (np.linalg.norm(dir_to_center, axis=1, keepdims=True) + 1e-6)
+        radial_flow = np.sum(flow_vectors * dir_to_center, axis=1)
+        return np.mean(radial_flow)
+
+    def _compute_zone_stats(self, old_pts, flow_vecs, img_w, img_h):
+        left_bound = img_w / 3
+        right_bound = 2 * img_w / 3
+        zones = {'left': [], 'center': [], 'right': []}
+        for pt, vec in zip(old_pts, flow_vecs):
+            x = pt[0]
+            if x < left_bound:
+                zones['left'].append((pt, vec))
+            elif x < right_bound:
+                zones['center'].append((pt, vec))
+            else:
+                zones['right'].append((pt, vec))
+
+        stats = {}
+        for name, pts_vecs in zones.items():
+            if len(pts_vecs) < 3:  # 阈值降低，因为点可能较少
+                stats[name] = {'mean_vec': np.zeros(2), 'divergence': 0.0, 'count': 0, 'risk': 0.0}
+                continue
+            old_pts_zone = np.array([p for p, v in pts_vecs])
+            flow_zone = np.array([v for p, v in pts_vecs])
+            div = self._compute_divergence(old_pts_zone, flow_zone, img_w, img_h)
+            stats[name] = {
+                'mean_vec': np.mean(flow_zone, axis=0),
+                'divergence': div,
+                'count': len(pts_vecs),
+                'risk': div if div > 0 else 0.0
+            }
+        return stats
+
+    def _check_roi_expansion(self, old_points, flow_vectors):
+        h, w = 480, 640
+        roi_center = np.array([w / 2, h / 2])
+        roi_mask = (np.abs(old_points[:, 0] - w / 2) < w / 4) & (np.abs(old_points[:, 1] - h / 2) < h / 4)
+        roi_points = old_points[roi_mask]
+        roi_flow = flow_vectors[roi_mask]
+        if len(roi_points) < 5:
+            return False
+        dir_vec = roi_points - roi_center
+        dir_unit = dir_vec / (np.linalg.norm(dir_vec, axis=1, keepdims=True) + 1e-6)
+        radial = np.sum(roi_flow * dir_unit, axis=1)
+        return np.mean(radial) > 0.5
+
+    def get_zone_decision(self, zone_stats=None, threshold_div=0.8):
+        """根据分区风险生成控制建议（返回 yaw_rate, speed_scale）"""
+        if zone_stats is None:
+            zone_stats = self.last_zone_stats
+        if zone_stats is None:
+            return 0.0, 1.0
+
+        left = zone_stats['left']['risk']
+        center = zone_stats['center']['risk']
+        right = zone_stats['right']['risk']
+        max_r = max(left, center, right)
+
+        if max_r < 0.3:
+            return 0.0, 1.0
+
+        if center >= left and center >= right and center > threshold_div:
+            if max(left, right) > threshold_div * 0.6:
+                return 0.0, 0.0
+            else:
+                return 0.0, 0.3
+
+        if left > right + 0.2:
+            yaw = min(0.8, 0.3 + 0.5 * (left - right))
+            return yaw, 0.7
+        elif right > left + 0.2:
+            yaw = -min(0.8, 0.3 + 0.5 * (right - left))
+            return yaw, 0.7
+        else:
+            return 0.0, 0.5
+
+
+# ==================== 控制器（与之前相同） ====================
+class ObstacleAvoidanceController:
+    """避障控制器，集成网格点光流检测器、PID 和 MAVLink 发送"""
+    def __init__(self,
+                 camera_id: int = 0,
+                 frame_width: int = 640,
+                 frame_height: int = 480,
+                 cruise_speed: float = 2.0,
+                 grid_step: int = 25,
+                 reset_threshold: float = 30.0,
+                 yaw_pid_cfg: Optional[PIDConfig] = None,
+                 speed_pid_cfg: Optional[PIDConfig] = None):
+        self.cap = cv2.VideoCapture(camera_id)
+        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, frame_width)
+        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, frame_height)
+        self.cap.set(cv2.CAP_PROP_FPS, 30)
+        if not self.cap.isOpened():
+            raise RuntimeError("无法打开摄像头")
+
+        self.detector = OpticalFlowObstacleDetector(grid_step=grid_step, reset_threshold=reset_threshold)
+        self.cruise_speed = cruise_speed
+
+        if yaw_pid_cfg is None:
+            yaw_pid_cfg = PIDConfig(kp=1.2, ki=0.1, kd=0.05, max_out=0.8, min_out=-0.8)
+        if speed_pid_cfg is None:
+            speed_pid_cfg = PIDConfig(kp=0.8, ki=0.05, kd=0.0, max_out=1.0, min_out=0.0)
+
+        self.yaw_pid = ImprovedPID(yaw_pid_cfg)
+        self.speed_pid = ImprovedPID(speed_pid_cfg)
+
+        self.current_yaw_rate = 0.0
+        self.current_speed = 0.0
+        self.running = False
+
+    def update_feedback(self, yaw_rate: float, speed: float):
+        self.current_yaw_rate = yaw_rate
+        self.current_speed = speed
+
+    def run_iteration(self, mav_proxy, enable_pid: bool = True) -> Dict[str, Any]:
+        ret, frame = self.cap.read()
+        if not ret:
+            return {'success': False, 'error': '摄像头读取失败'}
+
+        is_collision, flow_data = self.detector.process_frame(frame)
+        yaw_setpoint, speed_scale = self.detector.get_zone_decision()
+        speed_setpoint = speed_scale * self.cruise_speed
+
+        if enable_pid:
+            yaw_error = yaw_setpoint - self.current_yaw_rate
+            speed_error = speed_setpoint - self.current_speed
+            yaw_output = self.yaw_pid.update(yaw_error, feedback=self.current_yaw_rate)
+            speed_output = self.speed_pid.update(speed_error, feedback=self.current_speed)
+        else:
+            yaw_output = yaw_setpoint
+            speed_output = speed_setpoint
+
+        # 调用您的 MAVLink 代理的 send_setpoint 方法
+        success, ack = mav_proxy.send_setpoint(
+            control_mode="VELOCITY",
+            vx=speed_output,
+            vy=0.0,
+            vz=0.0,
+            yaw_rate=yaw_output
+        )
+
+        return {
+            'success': success,
+            'ack': ack,
+            'is_collision': is_collision,
+            'yaw_setpoint': yaw_setpoint,
+            'speed_setpoint': speed_setpoint,
+            'yaw_output': yaw_output,
+            'speed_output': speed_output,
+            'points': flow_data['points'] if flow_data else 0,
+        }
+
+    def run_forever(self, mav_proxy, enable_pid: bool = True):
+        self.running = True
+        print("🔄 网格点光流避障（复位点不统计）已启动，按 'q' 退出")
+        while self.running:
+            result = self.run_iteration(mav_proxy, enable_pid)
+            if not result.get('success', False):
+                print("⚠️ 迭代失败:", result.get('error', ''))
+                continue
+            if result['is_collision']:
+                print("🚨 碰撞警告!")
+
+            # 显示画面（方便调试）
+            ret, frame = self.cap.read()
+            if ret:
+                cv2.imshow('Grid Optical Flow', frame)
+            if cv2.waitKey(1) & 0xFF == ord('q'):
+                self.stop()
+                break
+
+    def stop(self):
+        self.running = False
+        self.cap.release()
+        cv2.destroyAllWindows()
+
+
+# ==================== 测试入口 ====================
+if __name__ == "__main__":
+    # 模拟 MAVLink 代理（实际应替换为真实对象）
+    class MockMavProxy:
+        def send_setpoint(self, control_mode="VELOCITY", **kwargs):
+            print(f"📤 发送指令: {control_mode} {kwargs}")
+            return True, {"result": "ok"}
+
+    mav = MockMavProxy()
+    controller = ObstacleAvoidanceController(
+        camera_id=0,
+        grid_step=25,
+        reset_threshold=30.0,
+        cruise_speed=2.0
+    )
+    controller.update_feedback(0.0, 0.0)  # 模拟初始反馈
+
+    try:
+        controller.run_forever(mav, enable_pid=True)
+    except KeyboardInterrupt:
+        controller.stop()
+        print("🛑 已退出")

drone_controller/pid_counter.py

@@ -15,6 +15,8 @@ class PID:
         self.last_err = 0.0
         self.integral = 0.0
         self.last_time = time.time()
+        # 积分冻结标志位，默认为不冻结（False）
+        self.integral_frozen = False
 
     def update(self, error):
         """
@@ -28,8 +30,11 @@ class PID:
         # 1. 比例项
         p_out = self.kp * error
 
-        # 2. 积分项（增加累加计算，并对积分做出局部限幅防止初段积分饱和）
+        # 2. 积分项
+        # 增加策略：只有在未冻结时，才进行积分累加
+        if not self.integral_frozen:
             self.integral += error * dt
+            
         i_out = self.ki * self.integral
         # 将积分上限限制在总输出幅值的 20%
         i_out = max(min(i_out, self.max_out * 0.2), self.min_out * 0.2)
@@ -56,12 +61,41 @@ class PID:
         self.last_err = 0.0
         self.integral = 0.0
         self.last_time = time.time()
+        self.integral_frozen = False  # 重置时默认恢复积分功能
 
     def reset_integral(self):
         """
         仅清空积分项（I term），保留上一帧误差以维持微分（D term）连续性
-
             用于卡尔曼 coast（滑行预测）期间：
             只靠比例 (P) + 微分 (D) 稳住，积分项清零防止过冲。
         """
         self.integral = 0.0
+
+    # ================== 新增方法 ==================
+
+    def freeze_integral(self):
+        """
+        冻结积分项：
+        保持当前的积分值（保持之前的成果），但不再随时间累加新的误差。
+        
+        适用场景：执行机构饱和（如马达已满载）、或者进入特定暂态过程。
+        """
+        self.integral_frozen = True
+
+    def unfreeze_integral(self, mode="maintain"):
+        """
+        配套解冻策略：
+        
+        :param mode: 解冻模式选择
+            - "maintain": 保持冻结期间的值，直接从该点继续累加（最常用，平滑过渡）。
+            - "clear": 解冻的同时清空历史积分，完全重新开始。
+            - "fade": 仅保持原有积分的 50%（衰减），防止突变过冲。
+        """
+        self.integral_frozen = False
+        
+        if mode == "clear":
+            self.integral = 0.0
+        elif mode == "fade":
+            self.integral *= 0.5
+        elif mode == "maintain":
+            pass # 保持原样，直接继续累加

infer_camera_modular.py

@@ -440,6 +440,14 @@ class UAVControlLoop:
         self._log_throttle = {}  # key → 上次打印时间戳
         self._brake_last_logged = False  # 刹车是否已输出过日志
 
+        # ---- 光流避障决策（外部注入） ----
+        self.obstacle_yaw_rate = 0.0
+        self.obstacle_speed_scale = 1.0
+        self.obstacle_active = False
+        self.cruise_speed = fc_cfg.get("cruise_speed", 1.0)   # 默认 1 m/s
+        # ---- 光流线程锁 ----
+        self._obstacle_lock = threading.Lock()
+
     def start_uav(self):
         """连接 Router，执行标准起飞序列（协议 §3.3.1）
 
@@ -648,6 +656,21 @@ class UAVControlLoop:
 
         return tracked, result
     
+    # ---- 光流避障决策 ----
+    def set_obstacle_decision(self, yaw_rate: float, speed_scale: float):
+        """
+        由外部注入光流避障决策。
+        当 yaw_rate != 0 或 speed_scale < 0.95 时视为激活。
+        """
+        with self._obstacle_lock:
+            self.obstacle_yaw_rate = yaw_rate
+            self.obstacle_speed_scale = speed_scale
+            self.obstacle_active = (abs(yaw_rate) > 0.02 or speed_scale < 0.95)
+            if self.obstacle_active:
+                self.logger.debug("光流避障激活: yaw=%.2f, scale=%.2f", yaw_rate, speed_scale)
+            else:
+                self.logger.debug("光流避障已清除")
+
     # ---- 内部控制线程 ----
 
     def _control_loop(self):
@@ -703,12 +726,9 @@ class UAVControlLoop:
                 else:
                     self._brake_last_logged = False
 
-            # ---- 记录当前速度指令（供外部日志读取） ----
-            self._last_vx, self._last_vy, self._last_vz = vx, vy, vz
             
             # ---- ④ 通过 ZMQ 发送 VELOCITY SETPOINT（协议 §3.1 速度控制） ----
-            self.logger.debug("SETPOINT: control_mode=VELOCITY "
+
-                              "vx=%.2f vy=%.2f vz=%.2f", vx, vy, vz)
             # ---- 防御性检查：飞控已切到 AUTO.LAND/AUTO.RTL 则不再发送 ----
             state = self.proxy.get_latest_state()
             if state:
@@ -716,9 +736,31 @@ class UAVControlLoop:
                 if mode in ("AUTO.LAND", "AUTO.RTL"):
                     # 静默跳过，不发送 SETPOINT
                     continue
+            # ---- 决策融合：光流避障是否覆盖 ----
+            with self._obstacle_lock:
+                if self.obstacle_active:
+                    # 避障模式：前向速度 × 缩放，偏航角速度由光流决策给出
+                    cmd_vx = self.cruise_speed * self.obstacle_speed_scale
+                    cmd_vy = 0.0
+                    cmd_vz = 0.0
+                    cmd_yaw_rate = self.obstacle_yaw_rate
+                else:
+                    # 正常追踪模式
+                    cmd_vx = vx       # 原逻辑 vx 始终为 0（可保留）
+                    cmd_vy = vy
+                    cmd_vz = vz
+                    cmd_yaw_rate = 0.0
+
+            # 记录当前速度指令（供外部日志读取）
+            self._last_vx, self._last_vy, self._last_vz = cmd_vx, cmd_vy, cmd_vz
+
+            # ---- ⑤ 通过 ZMQ 发送 VELOCITY SETPOINT ----
+            self.logger.debug("SETPOINT: control_mode=VELOCITY "
+                              "vx=%.2f vy=%.2f vz=%.2f yaw_rate=%.2f",
+                              cmd_vx, cmd_vy, cmd_vz, cmd_yaw_rate)
             ok, ack = self.proxy.send_setpoint(
-                vx=vx, vy=vy, vz=vz,
+                vx=cmd_vx, vy=cmd_vy, vz=cmd_vz,
-                yaw_rate=0.0,
+                yaw_rate=cmd_yaw_rate,
                 control_mode="VELOCITY"
             )
             if not ok and self.running:
@@ -726,7 +768,7 @@ class UAVControlLoop:
                 # recv 超时或格式错误由 _send_req 内部恢复 REQ socket
                 self.logger.warning("SETPOINT ACK 失败: %s", ack)
 
-            # ---- ⑤ 精确控制循环频率（20Hz → 每 50ms） ----
+            # ---- ⑥ 精确控制循环频率（20Hz → 每 50ms） ----
             elapsed = time.time() - start
             sleep_time = self.control_interval - elapsed
             if sleep_time > 0:

run_mission.py

@@ -61,6 +61,8 @@ from drone_controller.router_proxy import RouterProxy
 # run_mission.py 在外部协调其控制线程的启停。
 from infer_camera_modular import UAVControlLoop, VideoStreaming, YOLO26UAVInfer
 # 注：coord.latlon_to_ned 不再需要 — 航点直接存储原始经纬度
+# ---- 光流模块 ----
+from drone_controller.optical_flow_controller import OpticalFlowObstacleDetector
 
 logger = logging.getLogger("RunMission")
 
@@ -129,6 +131,15 @@ class MissionOrchestrator:
             return_to_home=missions.get("return_to_home", True),
         )
 
+        # ================================================================
+        # 💡 OpticalFlowObstacleDetector （光流计算）
+        # ================================================================
+        flow_cfg = cfg.get("optical_flow", {})
+        self.optical_flow = OpticalFlowObstacleDetector(
+            grid_step=flow_cfg.get("grid_step", 25),
+            reset_threshold=flow_cfg.get("reset_threshold", 30.0)
+        )
+
         # ================================================================
         #  🛸 UAVControlLoop（20Hz VELOCITY 闭环追踪）
         # ================================================================
@@ -555,6 +566,15 @@ class MissionOrchestrator:
                                          detections, orig_shape)
                 prev_state = current_state
 
+                # ---- 光流避障决策 ----
+                if self.mission.state == MissionState.VISUAL_TRACKING:
+                    _, _ = self.optical_flow.process_frame(frame)
+                    yaw_setpoint, speed_scale = self.optical_flow.get_zone_decision()
+                    self.controller.set_obstacle_decision(yaw_setpoint, speed_scale)
+                else:
+                    # 非追踪状态清除避障
+                    self.controller.set_obstacle_decision(0.0, 1.0)
+
                 # ---- ④ 状态机更新 ----
                 self.mission.update(target_detected=target_detected)