Pi视觉追踪（四）：控制算法与卡尔曼

控制算法负责把视觉检测到的目标位置转化为舵机运动。从纯比例到卡尔曼预测，层层递进。

控制层级

画面检测到质心(cx, cy)
  → 计算与画面中心偏差 error = (cx - frame_cx, cy - frame_cy)
  → 控制器计算舵机移动量
  → 驱动舵机

控制器公式

纯比例

strength = Kp * error

简单但有滞后和震荡。

PD（AutoTurret用法，推荐起步）

proportional = error / half_frame
derivative = (prev_error - error) / dt
strength = Kp * proportional - Kd * derivative

AutoTurret实测值：Kp=3，Kd=0.005。

注意：derivative 前面是减号，因为目的是抑制变化（防超调）。

PID（完整版）

integral += error * dt
output = Kp*error + Ki*integral + Kd*derivative

蚊子轨迹跳跃不规则，积分项易累积误差引发windup，慎用I项。

卡尔曼滤波预测（蚊子场景几乎必备）

蚊子小而快，舵机响应有滞后，必须预测目标下一位置再驱动舵机到预测点。

状态向量设计（2D位置+速度，4维）

state = [x, y, dx, dy]
measurement = [x, y]   （OpenCV能测到的只有位置）

OpenCV KalmanFilter(4, 2) 关键矩阵

调用流程

每帧：
  if 检测到：
    kf.correct(measurement)   # 用测量修正状态
  prediction = kf.predict()    # 总是预测下一帧位置
  # 用 prediction 驱动舵机，不是用 measurement

完整代码：卡尔曼追踪器

"""
kalman_tracker.py — OpenCV 2D KalmanFilter 目标追踪器
来源：综合 myronabotanel/motion-tracking-opencv 与 OpenCV官方示例
依赖：opencv-python numpy

状态向量: [x, y, dx, dy]  (4维，位置+速度)
测量向量: [x, y]          (2维，仅位置可测)

用法：
  from kalman_tracker import KalmanTracker
  kf = KalmanTracker()
  if detected:
      kf.update(cx, cy)
  pred_x, pred_y = kf.predict()  # 用预测点驱动舵机
"""
import cv2
import numpy as np


class KalmanTracker:
    def __init__(self, init_x=0, init_y=0, process_noise=0.03, measurement_noise=0.1):
        """
        process_noise: 运动模型不确定度，蚊子轨迹不规则可调大到0.1
        measurement_noise: 测量噪声，C270小目标建议0.1~0.5
        """
        self.kf = cv2.KalmanFilter(4, 2)

        # 状态转移矩阵 F（恒速模型）
        self.kf.transitionMatrix = np.array(
            [[1, 0, 1, 0],
             [0, 1, 0, 1],
             [0, 0, 1, 0],
             [0, 0, 0, 1]], dtype=np.float32)

        # 测量矩阵 H（仅测位置）
        self.kf.measurementMatrix = np.array(
            [[1, 0, 0, 0],
             [0, 1, 0, 0]], dtype=np.float32)

        # 过程噪声协方差 Q
        self.kf.processNoiseCov = np.eye(4, dtype=np.float32) * process_noise

        # 测量噪声协方差 R
        self.kf.measurementNoiseCov = np.eye(2, dtype=np.float32) * measurement_noise

        # 初始状态
        self.kf.statePre = np.array(
            [[init_x], [init_y], [0], [0]], dtype=np.float32)
        self.kf.statePost = np.array(
            [[init_x], [init_y], [0], [0]], dtype=np.float32)

        self.initialized = False

    def update(self, cx, cy):
        """喂入测量，修正状态"""
        measurement = np.array([[np.float32(cx)], [np.float32(cy)]])
        if not self.initialized:
            self.kf.statePre = np.array(
                [[cx], [cy], [0], [0]], dtype=np.float32)
            self.kf.statePost = np.array(
                [[cx], [cy], [0], [0]], dtype=np.float32)
            self.initialized = True
        self.kf.correct(measurement)

    def predict(self):
        """返回预测位置(x, y)与速度(vx, vy)"""
        pred = self.kf.predict()
        x, y = float(pred[0]), float(pred[1])
        vx, vy = float(pred[2]), float(pred[3])
        return x, y, vx, vy

    def get_velocity(self):
        """当前估计速度"""
        return float(self.kf.statePost[2]), float(self.kf.statePost[3])


if __name__ == "__main__":
    # 模拟轨迹测试
    kf = KalmanTracker(process_noise=0.05, measurement_noise=0.2)
    # 模拟目标以匀速移动+噪声
    true_traj = [(50 + 5 * t, 50 + 3 * t) for t in range(20)]
    noisy = [(x + np.random.randn() * 3, y + np.random.randn() * 3) for x, y in true_traj]
    print("t  true_x  true_y  meas_x  meas_y  pred_x  pred_y")
    for t, (mx, my) in enumerate(noisy):
        kf.update(mx, my)
        px, py, _, _ = kf.predict()
        tx, ty = true_traj[t]
        print(f"{t:2d}  {tx:6.1f} {ty:6.1f}  {mx:6.1f} {my:6.1f}  {px:6.1f} {py:6.1f}")

调试建议

1. 先用比例项调到能跟住（不要求稳）。
2. 加微分项消除抖动。
3. 测量噪声大时（蚊子几像素）启用卡尔曼预测，把舵机目标设为预测点。
4. 积分项仅在有明显稳态偏差时启用，且加anti-windup限幅。
5. Kp过大导致超调震荡；Kd过大导致高频抖动；从Kp=0.2、Kd=0.01起步。