opencv+树莓派 人脸追踪(增量式PID算法控制双舵机)

1、用python+opencv打开树莓派的摄像头

   这里先放一个打开摄像头并实时显示图像的例程

import cv2

cap = cv2.VideoCapture(0)        //将摄像头数据传入cap
cap.set(3, 320)                  //设置分辨率为320*240
cap.set(4, 240)

while True:
    
    ret,frame = cap.read()        //ret的值为True或False，frame存入每一帧的数据
    cv2.imshow("capture", frame)
    if cv2.waitKey(1)>0:        //键盘按下退出循环
        break
cap.release()
cv2.destroyAllWindows()

 2、人脸检测

接下来我们往这个例程继续添加几行代码，里面的地址需要对应你自己的opencv文件夹；

face_cascade = cv2.CascadeClassifier( '/home/pi/opencv-3.4.1/data/lbpcascades/lbpcascade_frontalface.xml' )

将图像转为灰度图像

gray = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)

得到人脸信息并存入faces中

faces = face_cascade.detectMultiScale( gray )

得到人脸坐标并存入x,y变量中，画出人脸范围

    if len(faces)>0:
        #print('face found!')
        for (x,y,w,h) in faces:
            cv2.rectangle(frame,(x,y),(x+h,y+w),(0,255,0),2)
            #max_face=w*h
            result = (x,y,w,h)
            x=result[0]
            y = result[1]
            #print (x)

至此，将代码加入最开始的例程内，我们就可以识别人脸了，完整代码如下

import cv2

cap = cv2.VideoCapture(0)        //将摄像头数据传入cap
cap.set(3, 320)                  //设置分辨率为320*240
cap.set(4, 240)
face_cascade = cv2.CascadeClassifier( '/home/pi/opencv-3.4.1/data/lbpcascades/lbpcascade_frontalface.xml' )

while True:
    
    ret,frame = cap.read()        //ret的值为True或False，frame存入每一帧的数据
    gray = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)
    faces = face_cascade.detectMultiScale( gray )
    if len(faces)>0:
        for (x,y,w,h) in faces:
            cv2.rectangle(frame,(x,y),(x+h,y+w),(0,255,0),2)
            #max_face=w*h
            result = (x,y,w,h)
            x=result[0]
            y = result[1]
            #print (x)
    cv2.imshow("capture", frame)
    if cv2.waitKey(1)>0:        //键盘按下退出循环
        break
cap.release()
cv2.destroyAllWindows()

3、舵机的控制

这里我们要借助树莓派内置的RPi.GPIO库来控制IO口，总共有两个舵机，每个舵机都采用增量式的PID算法进行控制

3.1增量式PID算法的设计

以y轴舵机为例，首先设置四个个变量

pwm_y=0
Y_P = 7
thisError_y=0
lastError_y=0

在读取每一帧图像时，也就是在while循环内，算出人脸位置坐标和屏幕中心的坐标的差值，由于图像分辨率为320*240，图像中心坐标就为(160,120），由此可以得到这一帧图像的thisError_y的值

thisError_y=y-120       

设置P值为0.001，D值为0.001(这里P,D值根据实际情况调节，使得舵机稳定就行)，得出误差值pwm_y，将这一帧的thisError存入lastError，将pwm_y累加至Y_P中，这里pwm_y可正可负，Y_P的初始值为7，这是舵机转90度的值

        pwm_y = thisError_y*0.001+0.001*(thisError_y-lastError_y)
        lastError_y = thisError_y
        Y_P+= pwm_y

最后再设置舵机转动的阈值(2.5,12)

        if Y_P>12:
            Y_P=11
        elif Y_P<2.5:
            Y_P=2.5

算法部分完成

3.2舵机的控制

在最初的例程里加入一下代码

import RPi.GPIO as GPIO  
import time  
import signal  
import atexit

atexit.register(GPIO.cleanup)    
  
servopin = 12
servo_y = 20
GPIO.setmode(GPIO.BCM)                //设置IO口为BCM码模式，可以对照下图找到BCM码对应的io口
GPIO.setup(servopin, GPIO.OUT, initial=False)      //初始化io口
GPIO.setup(servo_y, GPIO.OUT, initial=False) 
p = GPIO.PWM(servopin,50) #50HZ                    //设置为模拟pwm输出
Y = GPIO.PWM(servo_y,50) #50HZ
p.start(0)                                          //启动pwm输出
Y.start(0)
time.sleep(2)

这里附上树莓派的IO口图

在while循环内，加入代码

    Y.ChangeDutyCycle(Y_P)
    time.sleep(0.02)
    Y.ChangeDutyCycle(0)

这里我建议开启一个线程专门控制舵机，实际上python开线程也很简单，这里就不贴代码了

最后在程序的最后一行(cv2.destroyAllWindows()后)不要忘了加上

GPIO.cleanup()

不然下次再用io口的时候会有警告

至此，简单的双舵机人脸追踪云台就完成了

完整代码：https://github.com/ZhuTou409/little_project

csdn好像不能上传视频，我截个图好了