1. 项目概述与硬件准备

你是不是曾经想过让机器自动追踪移动的物体？比如让摄像头始终对准一个彩色的球，或者跟踪房间里走动的人？今天我就来分享一个超实用的项目：用树莓派+USB摄像头+两个舵机搭建一个颜色追踪系统。这个项目不仅好玩，而且能学到很多实用的技术，包括图像处理、PID控制算法等。

你需要准备以下硬件：

• 树莓派（推荐4B型号，性能足够）
• USB摄像头（普通30万像素以上的就可以）
• 两个舵机（建议180度带限位的SG90）
• 二维舵机云台（可以自己3D打印或者购买现成的）
• 杜邦线若干

我最初尝试这个项目时，用的是树莓派3B+，发现处理速度有点跟不上，后来换成4B就流畅多了。USB摄像头建议选择免驱的，插上就能用，省去很多麻烦。舵机一定要注意选择180度的，千万别买成360度连续旋转的，那种不适合位置控制。

软件环境准备： 首先确保你的树莓派系统是最新的，建议使用Raspbian或Ubuntu系统。然后安装必要的库：

sudo apt update
sudo apt install python3-opencv python3-pigpio

pigpio库是用来控制舵机的，比RPi.GPIO更好用，支持硬件PWM。OpenCV当然是做图像处理的核心库了。

2. 硬件连接与舵机控制

硬件连接其实很简单，但有几个细节需要注意。USB摄像头直接插到树莓派的USB接口上，舵机连接到树莓派的GPIO引脚。我建议使用GPIO 12和13这两个引脚，因为它们支持硬件PWM，控制起来更平滑。

舵机接线注意事项：

• 棕色线：接地（GND）
• 红色线：接5V电源
• 橙色线：接信号引脚（GPIO）

为了避免树莓派供电不足，建议给舵机使用外部电源。我最初直接使用树莓派的5V引脚供电，当两个舵机同时转动时，树莓派会突然重启，这就是典型的供电不足现象。

舵机控制代码： 创建一个servo.py文件，实现舵机控制类：

import pigpio
from time import sleep
import subprocess

class Servo():
    MAX_PW = 1250  # 0.5ms / 20ms * 10000
    MIN_PW = 250   # 2.5ms / 20ms * 10000
    _freq = 50     # 50Hz, 20ms周期
    
    def __init__(self, pin, min_angle=-90, max_angle=90):
        # 启动pigpiod守护进程
        result = None
        status = 1
        for x in range(3):
            p = subprocess.Popen('sudo pigpiod', shell=True, 
                                stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
            result = p.stdout.read().decode('utf-8')
            status = p.poll()
            if status == 0:
                break
            sleep(0.2)
        
        if status != 0:
            print(status, result)
        
        self.pi = pigpio.pi()
        self.pin = pin
        self.pi.set_PWM_frequency(self.pin, self._freq)
        self.pi.set_PWM_range(self.pin, 10000)
        self.angle = 0
        self.max_angle = max_angle
        self.min_angle = min_angle
        self.pi.set_PWM_dutycycle(self.pin, 0)
    
    def set_angle(self, angle):
        if angle > self.max_angle:
            angle = self.max_angle
        elif angle < self.min_angle:
            angle = self.min_angle
        
        self.angle = angle
        duty = self._map(angle, -90, 90, 250, 1250)
        self.pi.set_PWM_dutycycle(self.pin, duty)
    
    def get_angle(self):
        return self.angle
    
    def stop(self):
        self.pi.set_PWM_dutycycle(self.pin, 0)
        self.pi.stop()
    
    def _map(self, x, in_min, in_max, out_min, out_max):
        return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

这个类封装了舵机的控制逻辑，包括角度限制和PWM信号生成。我在这里踩过一个坑：最初没有正确计算PWM占空比，导致舵机要么不动，要么乱转。后来才发现是微秒到占空比的转换公式错了。

3. OpenCV颜色识别原理

颜色识别是这个项目的核心功能。OpenCV提供了强大的图像处理能力，让我们能够很容易地识别特定颜色的物体。

HSV颜色空间的优势： 我建议使用HSV颜色空间而不是RGB，因为HSV对光照变化不那么敏感。HSV三个分量分别代表：

• H（色相）：颜色的类型
• S（饱和度）：颜色的纯度
• V（明度）：颜色的亮度

颜色阈值设置技巧： 创建一个动态调整颜色的窗口非常有用，可以实时看到识别效果：

import cv2
import numpy as np

def nothing(x):
    pass

cv2.namedWindow('Color Threshold')
cv2.createTrackbar('H Min', 'Color Threshold', 0, 179, nothing)
cv2.createTrackbar('H Max', 'Color Threshold', 179, 179, nothing)
cv2.createTrackbar('S Min', 'Color Threshold', 0, 255, nothing)
cv2.createTrackbar('S Max', 'Color Threshold', 255, 255, nothing)
cv2.createTrackbar('V Min', 'Color Threshold', 0, 255, nothing)
cv2.createTrackbar('V Max', 'Color Threshold', 255, 255, nothing)

在实际使用时，我先把要追踪的物体放在摄像头前，然后调节这些滑条，直到只有目标物体是白色的，其他区域都是黑色。记下这些阈值，然后在代码中使用。

图像处理流程：

1. 将图像从BGR转换到HSV颜色空间
2. 根据颜色阈值创建掩码
3. 进行形态学操作（开运算、闭运算）去除噪声
4. 查找轮廓并选择最大的一个
5. 计算轮廓的中心坐标

frameHSV = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
lower_bound = np.array([h_min, s_min, v_min])
upper_bound = np.array([h_max, s_max, v_max])
mask = cv2.inRange(frameHSV, lower_bound, upper_bound)

# 形态学操作
kernel = np.ones((5, 5), np.uint8)
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)

# 查找轮廓
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if len(contours) > 0:
    largest_contour = max(contours, key=cv2.contourArea)
    x, y, w, h = cv2.boundingRect(largest_contour)
    center_x = x + w // 2
    center_y = y + h // 2

4. PID控制算法原理

PID控制器是让追踪系统平滑运行的关键。如果没有PID，舵机会剧烈抖动，根本无法稳定追踪目标。

PID三个分量的作用：

• 比例项（P）：与当前误差成正比，提供快速响应
• 积分项（I）：累积历史误差，消除稳态误差
• 微分项（D）：基于误差变化率，抑制 overshoot

我刚开始只用了P控制，结果舵机总是在目标位置来回振荡。加上D项后，系统就稳定多了。I项要小心使用，积分饱和会导致系统响应过慢。

PID实现代码： 创建一个pid.py文件：

import time

class PID:
    def __init__(self, kP=1, kI=0, kD=0):
        self.kP = kP
        self.kI = kI
        self.kD = kD
        self.initialized = False
    
    def initialize(self):
        self.curr_time = time.time()
        self.prev_time = self.curr_time
        self.prev_error = 0
        self.cP = 0
        self.cI = 0
        self.cD = 0
        self.initialized = True
    
    def update(self, error, sleep=0):
        if not self.initialized:
            self.initialize()
        
        time.sleep(sleep)
        self.curr_time = time.time()
        delta_time = self.curr_time - self.prev_time
        
        # 比例项
        self.cP = error
        
        # 积分项
        self.cI += error * delta_time
        
        # 微分项
        self.cD = (error - self.prev_error) / delta_time if delta_time > 0 else 0
        
        self.prev_time = self.curr_time
        self.prev_error = error
        
        return self.kP * self.cP + self.kI * self.cI + self.kD * self.cD
    
    def set_kp(self, kP):
        self.kP = kP
    
    def set_ki(self, kI):
        self.kI = kI
    
    def set_kd(self, kD):
        self.kD = kD

这个PID实现包含了时间delta的计算，确保在不同帧率下都能稳定工作。我最初忽略了时间因素，结果在高帧率和低帧率下表现完全不一样。

5. 系统集成与实时调试

现在我们把所有部件集成在一起。关键是创建一个流畅的视频流类，确保图像获取不会成为性能瓶颈。

视频流类实现： 使用多线程来读取视频帧，避免I/O阻塞：

from threading import Thread

class VideoStream:
    def __init__(self, src=0):
        self.stream = cv2.VideoCapture(src)
        self.stream.set(cv2.CAP_PROP_FRAME_WIDTH, 320)
        self.stream.set(cv2.CAP_PROP_FRAME_HEIGHT, 240)
        self.stream.set(cv2.CAP_PROP_FPS, 30)
        self.grabbed, self.frame = self.stream.read()
        self.stopped = False
    
    def start(self):
        Thread(target=self.update, args=()).start()
        return self
    
    def update(self):
        while not self.stopped:
            self.grabbed, self.frame = self.stream.read()
    
    def read(self):
        return self.frame
    
    def stop(self):
        self.stopped = True
        self.stream.release()

主循环结构： 主程序需要完成以下任务：

1. 读取视频帧
2. 进行颜色识别
3. 计算位置误差
4. 使用PID计算舵机调整量
5. 更新舵机位置
6. 显示调试信息

# 初始化PID控制器
pan_pid = PID(0.05, 0.01, 0.001)
tilt_pid = PID(0.05, 0.01, 0.001)
pan_pid.initialize()
tilt_pid.initialize()

# 初始化舵机
pan_servo = Servo(pin=12)
tilt_servo = Servo(pin=13)

# 启动视频流
vs = VideoStream(src=0).start()

try:
    while True:
        frame = vs.read()
        if frame is None:
            break
        
        # 颜色识别和误差计算
        error_x, error_y = color_tracking(frame)
        
        # PID更新
        pan_adjustment = pan_pid.update(error_x)
        tilt_adjustment = tilt_pid.update(error_y)
        
        # 更新舵机位置
        pan_angle += pan_adjustment
        tilt_angle += tilt_adjustment
        
        # 限制角度范围
        pan_angle = max(-90, min(pan_angle, 90))
        tilt_angle = max(-90, min(tilt_angle, 90))
        
        pan_servo.set_angle(pan_angle)
        tilt_servo.set_angle(tilt_angle)
        
finally:
    vs.stop()
    cv2.destroyAllWindows()

6. PID参数调优指南

PID调优是个经验活，我花了很长时间才找到规律。下面分享我的调参经验：

手动调参步骤：

1. 先将KI和KD设为0，逐步增加KP直到系统开始振荡
2. 将KP设为振荡值的50%
3. 慢慢增加KD直到振荡消失
4. 最后增加KI来消除稳态误差

参数调试界面： 创建一个实时调整PID参数的窗口非常有用：

cv2.namedWindow('PID Tuner')
cv2.createTrackbar('Pan Kp', 'PID Tuner', int(pan_pid.kP * 100), 100, nothing)
cv2.createTrackbar('Pan Ki', 'PID Tuner', int(pan_pid.kI * 100), 100, nothing)
cv2.createTrackbar('Pan Kd', 'PID Tuner', int(pan_pid.kD * 100), 100, nothing)
cv2.createTrackbar('Tilt Kp', 'PID Tuner', int(tilt_pid.kP * 100), 100, nothing)
cv2.createTrackbar('Tilt Ki', 'PID Tuner', int(tilt_pid.kI * 100), 100, nothing)
cv2.createTrackbar('Tilt Kd', 'PID Tuner', int(tilt_pid.kD * 100), 100, nothing)

在主循环中实时获取这些参数：

pan_pid.kP = cv2.getTrackbarPos('Pan Kp', 'PID Tuner') / 100
pan_pid.kI = cv2.getTrackbarPos('Pan Ki', 'PID Tuner') / 100
pan_pid.kD = cv2.getTrackbarPos('Pan Kd', 'PID Tuner') / 100
tilt_pid.kP = cv2.getTrackbarPos('Tilt Kp', 'PID Tuner') / 100
tilt_pid.kI = cv2.getTrackbarPos('Tilt Ki', 'PID Tuner') / 100
tilt_pid.kD = cv2.getTrackbarPos('Tilt Kd', 'PID Tuner') / 100

常见问题与解决方案：

现象	原因	解决方案
舵机剧烈振荡	KP太大	减小KP值
响应太慢	KP太小	增加KP值
稳态误差	需要积分项	适当增加KI
Overshoot	需要微分项	增加KD

我在调试过程中发现，水平和垂直方向的PID参数可能需要不同的设置，因为两个舵机的机械特性可能不同。不要指望一套参数能适用于两个轴。

7. 性能优化与实战技巧

经过几次迭代，我总结出一些提升系统性能的实用技巧：

分辨率与帧率权衡： 较高的分辨率能提供更精确的位置信息，但会降低处理速度。我建议使用320x240分辨率，这个分辨率在树莓派4B上能达到30FPS，足够大多数应用场景。

减少处理延迟的技巧：

1. 使用多线程视频采集
2. 限制图像处理区域（ROI）
3. 优化OpenCV操作，避免不必要的复制
4. 使用C++重写性能关键代码（如果需要更高性能）

光照适应性改进： 颜色识别对光照很敏感，我增加了自动亮度调整：

def auto_adjust_brightness(frame):
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    mean_brightness = np.mean(gray)
    alpha = 128 / mean_brightness if mean_brightness > 0 else 1
    return cv2.convertScaleAbs(frame, alpha=alpha, beta=0)

机械结构优化： 舵机云台的机械结构对系统性能影响很大。我建议：

1. 确保摄像头重心在舵机旋转中心上
2. 减少运动部件的惯性
3. 使用高质量的舵机减少回差

实际测试结果： 经过优化后，我的系统能够稳定追踪以0.5m/s速度移动的网球，追踪距离达到2米。舵机运动平滑，没有明显的振荡或延迟。

这个项目最让我满意的地方是，它很好地演示了软件算法和硬件控制的结合。通过调整PID参数，你能亲眼看到系统响应特性的变化，这种直观的反馈是纯软件项目无法提供的。

记得在正式使用前，充分测试各种场景下的性能。不同的光照条件、不同的目标物体大小和速度都会影响追踪效果。好的工程师不是一次调参就完事，而是会根据实际应用场景不断优化调整。