树莓派，opencv，Picamera2利用舵机云台追踪人脸

一、需要准备的硬件

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

二、项目目标

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

三、具体步骤

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

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

3.2人脸追踪代码

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

import cv2
from picamera2 import Picamera2
import time
import numpy as np
from servo import Servo
from time import sleep
picam2 = Picamera2()#偏航伺服电机连接上GPIO19脚，俯仰伺服电机信号线连接到GPIO16脚上
pan=Servo(pin=19)
tilt=Servo(pin=16)panAngle=0
tiltAngle=0pan.set_angle(panAngle)
tilt.set_angle(tiltAngle)#初始化pi camera
dispW=1280
dispH=720
picam2.preview_configuration.main.size = (dispW,dispH)
picam2.preview_configuration.main.format = "RGB888"
picam2.preview_configuration.controls.FrameRate=30
picam2.preview_configuration.align()
picam2.configure("preview")
picam2.start()
fps=0
pos=(30,60)
font=cv2.FONT_HERSHEY_SIMPLEX
height=1.5
weight=3
myColor=(0,0,255)while True:tStart=time.time()#获取取摄像头图片frame= picam2.capture_array()frame=cv2.flip(frame,1)gray=cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)#设置人脸识别参数face_cascade=cv2.CascadeClassifier('haarcascade_frontalface_default.xml')rects = face_cascade.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]cv2.rectangle(frame,(x,y),(x+w,y+h),(0,0,255),3)errorX=dispW/2-(x+w/2)if errorX>30:panAngle=panAngle-3 #调整此代码中的数字可以提高追踪速度if panAngle<-90:panAngle=-90pan.set_angle(panAngle)if errorX<-30:panAngle=panAngle+3 #调整此代码中的数字可以提高追踪速度if panAngle>90:panAngle=90pan.set_angle(panAngle)#俯仰电机纠偏Y轴方向上的偏差，大于30度，俯仰角度减小，小于-30度，俯仰角度增加errorY=dispH/2-(y+h/2)if errorY>30:tiltAngle=tiltAngle-3 #调整此代码中的数字可以提高追踪速度if tiltAngle<-90:tiltAngle=-90tilt.set_angle(tiltAngle)if errorY<-30:tiltAngle=tiltAngle+3 #调整此代码中的数字可以提高追踪速度if tiltAngle>90:tiltAngle=90tilt.set_angle(tiltAngle)   cv2.imshow('Camera',frame)#按q键退出，释放伺服电机和摄像头if cv2.waitKey(1)==ord('q'):pan.stop()tilt.stop()picam2.stop()print('stop') sleep(1)        breaktEnd=time.time()loopTime=tEnd-tStartfps=.9*fps + .1*(1/loopTime)
cv2.destroyAllWindows()

上述代码中的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:breaksleep(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*100MIN_PW = 250 # 2.5/20*100_freq = 50 # 50 Hz, 20msdef __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 = 0self.max_angle = max_angleself.min_angle = min_angleself.pi.set_PWM_dutycycle(self.pin, 0)def set_angle(self, angle):if angle > self.max_angle:angle = self.max_angleelif angle < self.min_angle:angle = self.min_angleself.angle = angleduty = self.map(angle, -90, 90, 250, 1250)self.pi.set_PWM_dutycycle(self.pin, duty)def get_angle(self):return self.angledef 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):#     passdef map(self, x, in_min, in_max, out_min, out_max):return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_minif __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 = 0tiltAngle = 0pan.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)