Pi视觉追踪（五）：完整集成与激光控制

本文收录两个可直接参考的完整追踪项目，以及激光控制的独立模块。从气球追踪的成熟方案起步，逐步替换模块逼近蚊子场景。

项目一：Autonomous Object Tracking Turret

来源：Cornell ECE5725 Spring 2018, Xitang Zhao & Francis Yu

架构：OpenCV HSV蓝色检测 → 多进程(1主+3worker) → PD控制 → pigpio硬件PWM双轴舵机 → GPIO激光开关

延迟：单进程120ms → 多进程约50ms

关键设计要点

项目二：蚊子追踪模板

思路：MOG2运动检测 + 卡尔曼预测 + PD控制 + pigpio双轴

这是基于前几个模块组合出的模板，未实测，需要按实际硬件调参。

蚊子场景与气球场景的核心差异

完整代码：蚊子追踪模板

"""
mosquito_tracker_template.py — 蚊子追踪模板（基于本资料夹模块组合）
说明：未实测的模板，需按你实际硬件调参。组合MOG2检测+卡尔曼预测+PD控制+pigpio双轴。

依赖：
  pip install opencv-python numpy pigpio RPi.GPIO
  sudo pigpiod

运行：
  python mosquito_tracker_template.py
按键：
  q退出 / l激光开关 / r重置卡尔曼
"""
import cv2
import numpy as np
import time
import pigpio
import RPi.GPIO as GPIO

# === 视觉配置 ===
CAM_INDEX = 0
RES_W, RES_H = 320, 240
HISTORY = 500
VAR_THRESHOLD = 40
DETECT_SHADOWS = False
MIN_AREA = 5
MAX_AREA = 500
MORPH_KERNEL_SIZE = 3
DETECT_FRAMES_CONFIRM = 3      # 连续3帧检测到才触发
LOST_FRAMES_RESET = 15         # 连续15帧未检测到则重置卡尔曼

# === 控制配置 ===
PAN_PIN = 12
TILT_PIN = 13
FREQ = 50
PAN_MIN, PAN_MAX = 35000, 130000
TILT_MIN, TILT_MAX = 45000, 110000
KP_PAN, KD_PAN = 0.4, 0.005
KP_TILT, KD_TILT = 0.4, 0.005
DEAD_ZONE_PX = 8               # 死区像素
SERVO_STEP_MAX = 5             # 单帧最大步进档位

# === 激光配置 ===
LASER_PIN = 6
GPIO.setmode(GPIO.BCM)
GPIO.setup(LASER_PIN, GPIO.OUT)
GPIO.output(LASER_PIN, GPIO.LOW)


# === 卡尔曼追踪器 ===
class KalmanTracker:
    def __init__(self, init_x=0, init_y=0, q=0.05, r=0.2):
        self.kf = cv2.KalmanFilter(4, 2)
        self.kf.transitionMatrix = np.array(
            [[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32)
        self.kf.measurementMatrix = np.array(
            [[1, 0, 0, 0], [0, 1, 0, 0]], np.float32)
        self.kf.processNoiseCov = np.eye(4, np.float32) * q
        self.kf.measurementNoiseCov = np.eye(2, np.float32) * r
        self.kf.statePre = np.array([[init_x], [init_y], [0], [0]], np.float32)
        self.kf.statePost = np.array([[init_x], [init_y], [0], [0]], np.float32)
        self.initialized = False

    def update(self, cx, cy):
        if not self.initialized:
            self.kf.statePre = np.array([[cx], [cy], [0], [0]], np.float32)
            self.kf.statePost = np.array([[cx], [cy], [0], [0]], np.float32)
            self.initialized = True
        self.kf.correct(np.array([[np.float32(cx)], [np.float32(cy)]]))

    def predict(self):
        p = self.kf.predict()
        return float(p[0]), float(p[1])

    def reset(self):
        self.initialized = False


# === 初始化 ===
pi_hw = pigpio.pi()
if not pi_hw.connected:
    raise RuntimeError("pigpiod未启动")
cur_pan = (PAN_MIN + PAN_MAX) // 2
cur_tilt = (TILT_MIN + TILT_MAX) // 2
pi_hw.hardware_PWM(PAN_PIN, FREQ, cur_pan)
pi_hw.hardware_PWM(TILT_PIN, FREQ, cur_tilt)

cap = cv2.VideoCapture(CAM_INDEX)
cap.set(3, RES_W)
cap.set(4, RES_H)

fgbg = cv2.createBackgroundSubtractorMOG2(
    history=HISTORY, varThreshold=VAR_THRESHOLD, detectShadows=DETECT_SHADOWS)
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (MORPH_KERNEL_SIZE, MORPH_KERNEL_SIZE))

kf = KalmanTracker(q=0.05, r=0.2)
center_x, center_y = RES_W // 2, RES_H // 2
detect_streak = 0
lost_streak = 0
laser_on = False
prev_t = time.time()


def move_servo(pin, cur, delta, lo, hi):
    new = max(lo, min(hi, cur + delta))
    pi_hw.hardware_PWM(pin, FREQ, int(new))
    return new


# === 主循环 ===
print("预热背景模型3秒...")
for _ in range(60):
    ret, _ = cap.read()

try:
    while True:
        ret, frame = cap.read()
        if not ret:
            break
        now = time.time()
        dt = now - prev_t
        prev_t = now

        # 1. MOG2检测
        fgmask = fgbg.apply(frame)
        fgmask = cv2.morphologyEx(fgmask, cv2.MORPH_OPEN, kernel)
        contours, _ = cv2.findContours(fgmask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        valid = [c for c in contours if MIN_AREA <= cv2.contourArea(c) <= MAX_AREA]

        detected = False
        if valid:
            largest = max(valid, key=cv2.contourArea)
            M = cv2.moments(largest)
            if M["m00"] > 0:
                cx = int(M["m10"] / M["m00"])
                cy = int(M["m01"] / M["m00"])
                detected = True

        # 2. 连续帧确认/丢失重置
        if detected:
            detect_streak += 1
            lost_streak = 0
            if detect_streak >= DETECT_FRAMES_CONFIRM:
                kf.update(cx, cy)
        else:
            detect_streak = 0
            lost_streak += 1
            if lost_streak > LOST_FRAMES_RESET:
                kf.reset()

        # 3. 预测位置（驱动舵机到预测点而非测量点）
        pred_x, pred_y = kf.predict()

        # 4. PD控制
        if kf.initialized:
            err_x = pred_x - center_x
            err_y = pred_y - center_y
            prop_x = err_x / (RES_W / 2.0)
            prop_y = err_y / (RES_H / 2.0)
            der_x = -err_x / dt if dt > 0 else 0
            der_y = -err_y / dt if dt > 0 else 0
            step_x = (KP_PAN * prop_x - KD_PAN * der_x) * 20
            step_y = (KP_TILT * prop_y - KD_TILT * der_y) * 20
            step_x = max(-SERVO_STEP_MAX, min(SERVO_STEP_MAX, step_x))
            step_y = max(-SERVO_STEP_MAX, min(SERVO_STEP_MAX, step_y))

            if abs(err_x) > DEAD_ZONE_PX:
                cur_pan = move_servo(PAN_PIN, cur_pan, step_x, PAN_MIN, PAN_MAX)
            if abs(err_y) > DEAD_ZONE_PX:
                cur_tilt = move_servo(TILT_PIN, cur_tilt, step_y, TILT_MIN, TILT_MAX)

        # 5. 显示
        if detected and detect_streak >= DETECT_FRAMES_CONFIRM:
            cv2.circle(frame, (cx, cy), 5, (0, 255, 0), -1)
            cv2.putText(frame, "DETECTED", (10, 20),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
        cv2.circle(frame, (int(pred_x), int(pred_y)), 3, (0, 0, 255), -1)
        cv2.drawMarker(frame, (center_x, center_y), (255, 0, 0),
                       cv2.MARKER_CROSS, 10, 1)
        cv2.imshow("mosquito_tracker", frame)

        k = cv2.waitKey(1) & 0xFF
        if k == ord('q'):
            break
        elif k == ord('l'):
            laser_on = not laser_on
            GPIO.output(LASER_PIN, GPIO.HIGH if laser_on else GPIO.LOW)
        elif k == ord('r'):
            kf.reset()
            print("Kalman reset")

except KeyboardInterrupt:
    pass
finally:
    pi_hw.hardware_PWM(PAN_PIN, FREQ, 0)
    pi_hw.hardware_PWM(TILT_PIN, FREQ, 0)
    pi_hw.stop()
    GPIO.output(LASER_PIN, GPIO.LOW)
    GPIO.cleanup()
    cap.release()
    cv2.destroyAllWindows()

激光控制独立模块

激光笔的软件开关是一个独立小模块，用GPIO控制MOSFET/三极管来通断激光笔电源：

"""
laser_control.py — 激光笔GPIO开关控制（独立小模块）
依赖：RPi.GPIO

接线：
  Pi5 GPIO6 (Pin 31) -> NPN/MOSFET栅极 -> 激光笔电源回路
  或：直接驱动KY-008激光模块的S信号脚

注意：
  - 激光笔即使低功率也应避免直射人眼
  - 程序中应有"对准后才点亮"的硬保护逻辑
  - 建议加限流电阻和MOSFET/三极管隔离
"""
import RPi.GPIO as GPIO
import time

LASER_PIN = 6

def setup():
    GPIO.setmode(GPIO.BCM)
    GPIO.setup(LASER_PIN, GPIO.OUT, initial=GPIO.LOW)

def on():
    GPIO.output(LASER_PIN, GPIO.HIGH)

def off():
    GPIO.output(LASER_PIN, GPIO.LOW)

def cleanup():
    GPIO.output(LASER_PIN, GPIO.LOW)
    GPIO.cleanup()

if __name__ == "__main__":
    setup()
    try:
        print("激光1秒开1秒关，按Ctrl+C退出")
        while True:
            on()
            time.sleep(1)
            off()
            time.sleep(1)
    except KeyboardInterrupt:
        pass
    finally:
        cleanup()

建议的调试顺序

1. 先用AutoTurret完整代码改成追气球（HSV颜色阈值不变），跑通端到端。
2. 把检测模块从HSV替换为MOG2，观察能否追上蚊子。
3. 在控制层加入卡尔曼滤波，把舵机目标从measurement改为prediction。
4. 实测发现滞后时调大 process_noise，让卡尔曼更信任预测。
5. 若SG90跟不上，要么换MG90S金属齿舵机，要么把舵机移动范围切小并提升预测权重。

关键参数（建议起步值）