树莓派，opencv，Picamera2利用舵机云台追踪人脸(PID控制）

一、需要准备的硬件

Raspiberry 4b
两个SG90 180度舵机（注意舵机的角度，最好是180度且带限位的，切勿选360度舵机）
二自由度舵机云台（如下图）
Raspiberry CSI 摄像头
组装后的效果：

二、项目目标

追踪人脸：
当人脸移动时，摄像头通过控制两个伺服电机（分别是偏航和俯仰）把该人脸放到视界的中心位置，本文采用了PID控制伺服电机

三、具体步骤

3.1 下载用于人脸识别的级联分类器

下载级联分类器“haarcascade_frontalface_default.xml”，下载地址：haarcascade_frontalface_default.xml
下载完成后将其与后面的所有文件放到同一目录中。

3.2人脸追踪代码

创建文件“face_tracking_PID.py” ，代码如下：

#face_tracking_PID.py
#-*- coding: UTF-8 -*-	
# 调用必需库
from multiprocessing import Manager
from multiprocessing import Process
from objcenter import ObjCenter
from pid import PID
from servo import Servo
import argparse
import signal
import time
import sys
import cv2
from picamera2 import Picamera2


# 定义舵机
pan=Servo(pin=19)
tilt=Servo(pin=16)

#定义图像尺寸
dispW=1280
dispH=720

      
# 键盘终止函数
def signal_handler(sig, frame):
    # 输出状态信息
    print("[INFO] You pressed `ctrl + c`! Exiting...")

    # 关闭舵机
    pan.stop()
    tilt.stop()

    # 退出
    sys.exit()

def obj_center(args, objX, objY, centerX, centerY):
    # ctrl+c退出进程
    signal.signal(signal.SIGINT, signal_handler)

    # 启动视频流并缓冲
    print("[INFO] waiting for camera to warm up...")
    cv2.startWindowThread()
    picam2 = Picamera2()
    picam2.preview_configuration.main.size = (dispW,dispH)
    picam2.preview_configuration.main.format = "RGB888"
    picam2.preview_configuration.controls.FrameRate=10
    picam2.preview_configuration.align()
    picam2.configure("preview")
    picam2.start()
    time.sleep(2.0)

    # 初始化人脸中心探测器
    obj = ObjCenter(args["cascade"])
    # 进入循环
    while True:

        # 从视频流抓取图像并旋转
        frame= picam2.capture_array()
        frame = cv2.flip(frame, 1)

        # 找到图像中心
        (H, W) = frame.shape[:2]
        centerX.value = W // 2
        centerY.value = H // 2

        #draw a point in the center of frame
        cv2.circle(frame, (centerX.value, centerY.value), 5, (0, 0, 255), -1)

        # 找到人脸中心
        objectLoc = obj.update(frame, (centerX.value, centerY.value))
        ((objX.value, objY.value), rect) = objectLoc
        print("objx.value", objX.value)
        print("objy.value", objY.value)

        # 绘制人脸外界矩形
        if rect is not None:
            (x, y, w, h) = rect
            cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2)
            fX = int(x + (w / 2.0))
            fY = int(y + (h / 2.0))
            cv2.circle(frame, (fX, fY), 5, (0, 0, 255), -1)
            # 在人脸中心和视窗中心画一条连线
            cv2.line(frame, (centerX.value, centerY.value),
                     (fX, fY), (0, 255, 0), 2)
            # 显示图像
        cv2.imshow("Pan-Tilt Face Tracking", frame)
        cv2.waitKey(1)

def pid_process(output, p, i, d, objCoord, centerCoord):
    # ctrl+c退出进程
    signal.signal(signal.SIGINT, signal_handler)
    
     # 创建一个PID类的对象并初始化
    p = PID(p.value, i.value, d.value)
    p.initialize()

    # 进入循环
    while True:
        # 计算误差
        error = centerCoord.value - objCoord.value
        # 更新输出值,当error小于50时，误差设为0，以避免云台不停运行。
        if abs(error) < 50:
            error = 0
        output.value = p.update(error)


def set_servos(panAngle, tiltAngle):
    # ctrl+c退出进程
    signal.signal(signal.SIGINT, signal_handler)

    #进入循环
    while True:
        # 偏角变号
        yaw = -1 * panAngle.value
        pitch = -1 * tiltAngle.value

        # 设置舵机角度。
        pan.set_angle(yaw)
        tilt.set_angle(pitch)
            

# 启动主程序
if __name__ == "__main__":
    # 建立语法分析器
    ap = argparse.ArgumentParser()
    ap.add_argument("-c", "--cascade", type=str, required=True,help="path to input Haar cascade for face detection")
    args = vars(ap.parse_args())

    # 启动多进程变量管理
    with Manager() as manager: #相当于manager=Manager(),with as 语句操作上下文管理器（context manager），它能够帮助我们自动分配并且释放资源。
        # 舵机角度置零
        pan.set_angle(0)
        tilt.set_angle(0)
        
        # 为图像中心坐标赋初值
        centerX = manager.Value("i", 0) #"i"即为整型integer
        centerY = manager.Value("i", 0)

        # 为人脸中心坐标赋初值
        objX = manager.Value("i", 0)
        objY = manager.Value("i", 0)

        # panAngle和tiltAngle分别是两个舵机的PID控制输出量	    
        panAngle = manager.Value("i", 0)
        tiltAngle = manager.Value("i", 0)

       # 设置一级舵机的PID参数
        panP = manager.Value("f", 0.015)  # "f"即为浮点型float
        panI = manager.Value("f", 0.01)
        panD = manager.Value("f", 0.0008)

        # 设置二级舵机的PID参数
        tiltP = manager.Value("f", 0.025)
        tiltI = manager.Value("f", 0.01)
        tiltD = manager.Value("f", 0.008)

        # 创建4个独立进程
        # 1. objectCenter  - 探测人脸
        # 2. panning       - 对一级舵机进行PID控制，控制偏航角
        # 3. tilting       - 对二级舵机进行PID控制，控制俯仰角
        # 4. setServos     - 根据PID控制的输出驱动舵机
                          
        processObjectCenter = Process(target=obj_center,args=(args, objX, objY, centerX, centerY))
        processPanning = Process(target=pid_process,args=(panAngle, panP, panI, panD, objX, centerX))
        processTilting = Process(target=pid_process,args=(tiltAngle, tiltP, tiltI, tiltD, objY, centerY))
        processSetServos = Process(target=set_servos, args=(panAngle, tiltAngle))

        # 开启4个进程
        processObjectCenter.start()
        processPanning.start()
        processTilting.start()
        processSetServos.start()

        # 添加4个进程
        processObjectCenter.join()
        processPanning.join()
        processTilting.join()
        processSetServos.join()

创建文件“objcenter.py”，代码如下：

#objcenter.py
#-*- coding: UTF-8 -*-
# 调用必需库
import cv2

class ObjCenter:
	def __init__(self, haarPath):
		# 加载人脸探测器
		self.detector = cv2.CascadeClassifier(haarPath)

	def update(self, frame, frameCenter):
		# 将图像转为灰度图
		gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

		# 探测图像中的所有人脸
		rects = self.detector.detectMultiScale(gray, scaleFactor=1.05,
			minNeighbors=9, minSize=(30, 30),
			flags=cv2.CASCADE_SCALE_IMAGE)

		# 是否检测到人脸
		if len(rects) > 0:
			# 获取矩形的参数
			# x,y为左上角点坐标，w,h为宽度和高度
	        # 计算图像中心
			(x, y, w, h) = rects[0]
			faceX = int(x + (w / 2.0))
			faceY = int(y + (h / 2.0))

			# 返回人脸中心
			return ((faceX, faceY), rects[0])

		# 如果没有识别到人脸，返回图像中心
		return (frameCenter, None)

创建“pid.py”，代码如下：

#pid.py
#-*- coding: UTF-8 -*-
# 调用必需库
import time

class PID:
	def __init__(self, kP=1, kI=0, kD=0):
		# 初始化参数
		self.kP = kP
		self.kI = kI
		self.kD = kD

	def initialize(self):
		# 初始化当前时间和上一次计算的时间
		self.currTime = time.time()
		self.prevTime = self.currTime

		# 初始化上一次计算的误差
		self.prevError = 0

		# 初始化误差的比例值，积分值和微分值
		self.cP = 0
		self.cI = 0
		self.cD = 0

	def update(self, error, sleep=0.2):
		# 暂停
		time.sleep(sleep)

		# 获取当前时间并计算时间差
		self.currTime = time.time()
		deltaTime = self.currTime - self.prevTime

		# 计算误差的微分
		deltaError = error - self.prevError

		# 比例项
		self.cP = error

		# 积分项
		self.cI += error * deltaTime

		# 微分项
		self.cD = (deltaError / deltaTime) if deltaTime > 0 else 0

		# 保存时间和误差为下次更新做准备
		self.prevTime = self.currTime
		self.prevError = error

		# 返回输出值
		return sum([
			self.kP * self.cP,
			self.kI * self.cI,
			self.kD * self.cD])

上述代码中的from servo import Servo导入servo，这个库是没有的，我们要手动创建这个库，在object_tracking.py所在的目录下新建servo.py文件，复制下面的代码到文件中

#!/usr/bin/env python3
import pigpio
from time import sleep
# Start the pigpiod daemon
import subprocess
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)
'''
> Use the DMA PWM of the pigpio library to drive the servo
> Map the servo angle (0 ~ 180 degree) to (-90 ~ 90 degree)

'''

class Servo():
    MAX_PW = 1250  # 0.5/20*100
    MIN_PW = 250 # 2.5/20*100
    _freq = 50 # 50 Hz, 20ms
 
    def __init__(self, pin, min_angle=-90, max_angle=90):

        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()

    # will be called automatically when the object is deleted
    # def __del__(self):
    #     pass

    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


if __name__ =='__main__':
    from vilib import Vilib
    # Vilib.camera_start(vflip=True,hflip=True) 
    # Vilib.display(local=True,web=True)

    pan = Servo(pin=13, max_angle=90, min_angle=-90)
    tilt = Servo(pin=12, max_angle=30, min_angle=-90)
    panAngle = 0
    tiltAngle = 0
    pan.set_angle(panAngle)
    tilt.set_angle(tiltAngle)
    sleep(1)

    while True:
        for angle in range(0, 90, 1):
            pan.set_angle(angle)
            tilt.set_angle(angle)
            sleep(.01)
        sleep(.5)
        for angle in range(90, -90, -1):
            pan.set_angle(angle)
            tilt.set_angle(angle)
            sleep(.01)
        sleep(.5)
        for angle in range(-90, 0, 1):
            pan.set_angle(angle)
            tilt.set_angle(angle)
            sleep(.01)
        sleep(.5)

在树莓派相应文件目录中输入`“python face_tracking_PID.py --cascade haarcascade_frontalface_default.xml"，即可实现对人脸对象自动追踪。相较之前的非PID控制而言，系统运行会更顺滑一些。在本例中采用的命令参数输入的方式，可以方便有多个人脸识别级联分类器时随时切换。
当运行时，可能会有摄像头随机摆动的现象出现，这是因为人脸识别级联分类器的识别过程中的误识别，对于普通用户我们还无能为力，只能是避开经常被误该识别的物体。