树莓派视觉小车 — 人脸追踪（人脸识别、PID控制舵机运动）
目录
效果展示
基础理论（人脸识别）
人脸检测算法按照方法可以被分为两大类，基于特征的算法、基于图像的算法。
1、基于特征的算法
基于特征的算法就是通过提取图像中的特征和人脸特征进行匹配，如果匹配上了就说明是人脸，反之则不是。提取的特征是人为设计的特征，例如Haar，FHOG，特征提取完之后，再利用分类器去进行判断。通俗的说就是采用模板匹配，就是用人脸的模板图像与待检测的图像中的各个位置进行匹配，匹配的内容就是提取的特征，然后再利用分类器进行判断是否有人脸。
2、基于图像的算法
基于图像的算法，将图像分为很多小窗口，然后分别判断每个小窗是否有人脸。通常基于图像的方法依赖于统计分析和机器学习，通过统计分析或者学习的过程来找到人脸和非人脸之间的统计关系来进行人脸检测。最具代表性的就是CNN，CNN用来做人脸检测也是目前效果最好，速度最快的。
3、Haar特征
我们使用机器学习的方法完成人脸检测，首先需要大量的正样本图像（面部图像）和负样本图像〈不含面部的图像）来训练分类器。我们需要从其中提取特征。下图中的 Haar特征会被使用，就像我们的卷积核，每一个特征是一个值，这个值等于黑色矩形中的像素值之和减去白色矩形中的像素值之和。
Haar特征值反映了图像的灰度变化情况。例如︰脸部的一些特征能由矩形特征简单的描述，眼睛要比脸颊颜色要深，鼻梁两侧比鼻梁颜色要深，嘴巴比周围颜色要深等。
Haar特征可用于于图像任意位置，大小也可以任意改变，所以矩形特征值是矩形模版类别、矩形位置和矩形大小这三个因素的函数。故类别、大小和位置的变化，使得很小的检测窗口含有非常多的矩形特征。
4、Adaboost级联决策器
得到图像的特征后，训练一个决策树构建的adaboost级联决策器来识别是否为人脸。
人脸检测，把图像分成一个个小块，对每一个小块判断是否是人脸，假如一张图被分成了5000块，则速度非常慢。为了提高效率，OpenCV 提供 cascades 来避免这种情况。提供了一系列的xml文件。（cascades ：级联）
cascade 对于每个数据块，它都进行一个简单快速的检测。若过，会再进行一个更仔细的检测。该算法有 30 到 50 个这样的阶段，或者说 cascade。只有通过全部阶段，cascade才会判断检测到人脸。这样做的好处是：大多数小块都会在前几步就产生否定反馈，节约时间。
API
detectMultiScale ：
在灰度图上检测人脸，输出是人脸区域的外接矩形框。
faces = face_cascade.detectMultiScale(self, image: Any, scaleFactor: Any = None, minNeighbors: Any = None, flags: Any = None, minSize: Any = None, maxSize: Any = None) -> None参数：
1.image：表示的是要检测的输入图像
2.scaleFactor：表示每次图像尺寸减小的比例
3. minNeighbors：至少检测次数。若为3，表示每一个目标至少要被检测到3次才算是真的目标(因为周围的像素和不同的窗口大小都可以检测到人脸)
4.flags，要么使用默认值，要么使用CV_HAAR_DO_CANNY_PRUNING，如果设置为CV_HAAR_DO_CANNY_PRUNING，那么函数将会使用Canny边缘检测来排除边缘过多或过少的区域，因此这些区域通常不会是人脸所在区域
5.minSize为目标的最小尺寸
6.minSize为目标的最大尺寸
基础理论（PID算法）
1、作用
需要将某一个物理量“保持稳定”的场合（比如维持平衡、稳定温度、转速等），PID都会派上大用场。
黄色折线：控制指令。 绿色曲线：飞机运动轨迹
飞机具有惯性，所以不能随着我们的指令瞬间移动，所以飞机的运动轨迹是一条曲线。
我们的目标是让飞机快速准确的悬停在目标高度，两条曲线贴合越紧密，则说明控制效果越好。
应用场景
1、平衡车倾斜角度
2、穿越机旋转速度
3、对于反馈值向目标值的调节都适用PID控制
2、PID原理
1、P（比例）
测量无人机当前位置与目标位置的距离，距离越远，我们就用越大的力把物体推回去。
这个过程类似于弹簧（离平衡位置越远，回复力越大）：
P控制好，就相当于在目标点与飞机之间绑了一个弹簧，永远会把飞机往平衡位置拉。
缺陷：
但由于飞机本身具有一定的惯性，到达目标点时虽然没有受力，但还是会偏移一定的距离，P越大，则说明弹簧越“硬”，回复速度越快，同时震动的频率越高。如果只有P，那么飞机会反复处于矫正过度状态，无休止运动。
（越接近目标，P的作用越温柔）
所以要控制好飞机，不仅要知道飞机的位置，还要知道飞机的速度，我们引入（D）。
2、D（微分）
通过微分的方法来计算运动速度。
D越大，物体运动时的阻力越大（这个阻力和物体的运动方向相反）。
（场景模拟：把物体扔入液体，物体速度越大，阻力越大；液体密度越大，阻力越大）
D值适当：物体可以很快停留在目标位置。
D值过大：阻力很大，抵消回复力，让控制变得迟钝。
（D让速度趋于0）
在动态控制中，最需要调节的就是P和D两个参数， P和D就是为你要控制的系统模拟出合适的弹簧和缓冲液。
3、I（积分）
I的作用：减小静态情况下的误差，让受控物理量尽可能接近目标值。
只有在受到稳定的外界干扰，或是存在系统误差的情况下，I值才能派上用场。
飞机会不停检测位置是否存在偏差，偏差越大，持续时间越长，矫正的力越大。
举例：
以热水为例。假如把加热装置带到了非常冷的地方，开始烧水，需要烧到50℃：
在P的作用下，水温慢慢升高。直到升高到45℃时，他发现了一个不好的事情：天气太冷，水散热的速度，和P控制的加热的速度相等了。
P：我和目标已经很近了，只需要轻轻加热就可以了。
D：加热和散热相等，温度没有波动，我好像不用调整什么。于是，水温永远地停留在45℃，永远到不了50℃。
设置一个积分量I。
I：只要偏差存在，就不断地对偏差进行积分（累加），并反应在调节力度上。
这样一来，即使45℃和50℃相差不太大，但是随着时间的推移，只要没达到目标温度，这个积分量就不断增加。系统就会慢慢意识到：还没有到达目标温度，该增加功率啦！
到了目标温度后，假设温度没有波动，积分值就不会再变动。这时，加热功率仍然等于散热功率。但是，温度是稳稳的50℃。
P、I、D分别代表着：现在、过去、未来。
3、PID公式
Kp -> 控制器的比例系数
Ti -> 控制器的积分时间，也称积分系数
Td -> 控制器的微分时间，也称微分系数
1、位置式算法（较少使用）
其中T为采样时间，由于T之类的参数是常量，所以将Kp乘入公式中可以转换成另一种写法，这个公式叫位置式算法。
（位置式PID的输出与过去的所有状态有关，计算时要对e（每一次的控制误差）进行累加，这个计算量非常大，而明显没有必要。而且小车的PID控制器的输出并不是绝对数值，而是一个△，代表增多少，减多少。换句话说，通过增量PID算法，每次输出是PWM要增加多少或者减小多少，而不是PWM的实际值。所以明白增量式PID就行了）
2、增量式算法（常用）
由于要不断的累加ej，增加了计算量，所以这个公式又可以转换为增量式算法：
PID = Uk + KP*【E(k)-E(k-1)】+ KI*E(k) + KD*【E(k)-2E(k-1)+E(k-2)】
一、初始化
# 初始化PCA9685和舵机
def Servo_Init():
global servo_pwm
servo_pwm = Adafruit_PCA9685.PCA9685() # 实例话舵机云台
# 设置舵机初始值，可以根据自己的要求调试
servo_pwm.set_pwm_freq(60) # 设置频率为60HZ
servo_pwm.set_pwm(5, 0, 350) # 底座舵机
servo_pwm.set_pwm(4, 0, 370) # 倾斜舵机
time.sleep(1)
# 摄像头初始化
def Capture_Init():
global capture
# 初始化摄像头并设置阙值
capture = cv2.VideoCapture(0)
# 设置显示的分辨率，设置为320×240 px（即摄像头大小）
capture.set(3, 320)
capture.set(4, 240)二、人脸识别
主程序
while True:
# 1 识别人脸
(x, y) = Face_Detect()1、创建人脸分类器
# 1 实例化官方训练好的人脸识别器
face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_alt.xml')2、打开摄像头
# 2 获取每帧图像
ret,frame = capture.read()
cv2.imshow('frame', frame)
image = frame3、转灰度图
# 3 转灰度图
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
cv2.imshow('gray', gray)4、人脸检测
# 4 人脸检测
faces = face_cascade.detectMultiScale(gray, 1.3, 1)5、获取人脸坐标、在图像上框出人脸
# 5 获取人脸坐标并在图像上框出人脸
try:
x,y,w,h = faces[0]
cv2.rectangle(image, (x,y),(x+w,y+h), (255,0,255),3)
cv2.imshow('image',image)
return (x+w/2, y+h/2)
except:
return (0, 0)
代码
# 1 识别人脸
def Face_Detect():
# 1 实例化官方训练好的人脸识别器
face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_alt.xml')
# 2 获取每帧图像
ret,frame = capture.read()
cv2.imshow('frame', frame)
image = frame
# 3 转灰度图
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
cv2.imshow('gray', gray)
# 4 人脸检测
faces = face_cascade.detectMultiScale(gray, 1.3, 1)
# 5 获取人脸坐标并在图像上框出人脸
try:
x,y,w,h = faces[0]
cv2.rectangle(image, (x,y),(x+w,y+h), (255,0,255),3)
cv2.imshow('image',image)
return (x+w/2, y+h/2)
except:
return (0, 0)三、PID处理
这里只用到了PID中的P(比例)，基于误差进行计算，当误差大的时候，计算出来的pid值就大；误差小的时候，pid值近乎不变。（像弹簧一样，误差大，弹力大；否则静止）
主程序
# 识别到人脸
if not (x==0 and y==0):
# 2 PID舵机控制
PID_Servo_Control(x, y)函数前部
# 2 PID舵机控制（这里分别设置使用PID和不用PID的情况）
def PID_Servo_Control(x, y):
global error_x, error_y, last_error_x, last_error_y, pid_X_P, pid_Y_P
# 下面开始pid算法：
# pid总公式：PID = Uk + KP*【E(k)-E(k-1)】 + KI*E(k) + KD*【E(k)-2E(k-1)+E(k-2)】
# 这里只用到了p，所以公式为：P = Uk + KP*【E(k)-E(k-1)】
# uk:原值 E(k):当前误差 KP:比例系数 KI:积分系数 KD:微分系数
# 使用PID（可以发现舵机云台运动比较稳定）1、获取误差（x、y方向）
注：(x,y)是当前获得的图像中心坐标（前面有过处理的：(x+width/2, y+height/2））。
相当于是计算图像中心对于摄像头的x、y轴（水平、竖直中点线） 的偏移程度。
# 1 获取误差(x和y方向)（分别计算距离x、y轴中点的误差）
error_x = x - 160 # width:320
error_y = y - 120 # height:2402、PID控制参数
这里只用到了p，所以公式为：P = Uk + KP*【E(k)-E(k-1)】
计算PID的P分量：
# 2 PID控制参数
pwm_x = error_x*3 + (error_x - last_error_x)*1
pwm_y = error_y*3 + (error_y - last_error_y)*1
# 这里pwm（p分量） = 当前误差*3 + 上次的误差增量*13、保存本次误差
# 3 保存本次误差，以便下一次运算
last_error_x = error_x
last_error_y = error_y4、得到最终的PID值（P分量）
# 4 最终PID值（舵机旋转角度）
pid_X_P -= int(pwm_x/50)
pid_Y_P -= int(pwm_y/50)
# p(pid的p) = 原值 + p分量5、限值
# 5 限值(0~650)
if pid_X_P>650:
pid_X_P=650
if pid_X_P<0:
pid_X_P=0
if pid_Y_P>650:
pid_Y_P=650
if pid_X_P<0:
pid_Y_P=0对比：不用PID处理
这里做了一个不用PID处理的，和之前PID处理的做一个对比。
有PID处理：舵机移动平稳。
无PID处理：舵机移动不平稳（有比较明显的摇摇晃晃）。
# 不用PID（舵机云台上下左右乱晃）
if x<160:
pid_X_P += 2
elif x>=160:
pid_X_P -= 2
if y<120:
pid_Y_P += 2
elif y>=120:
pid_Y_P -= 2代码
# 2 PID舵机控制（这里分别设置使用PID和不用PID的情况）
def PID_Servo_Control(x, y):
global error_x, error_y, last_error_x, last_error_y, pid_X_P, pid_Y_P
# 下面开始pid算法：
# pid总公式：PID = Uk + KP*【E(k)-E(k-1)】 + KI*E(k) + KD*【E(k)-2E(k-1)+E(k-2)】
# 这里只用到了p，所以公式为：P = Uk + KP*【E(k)-E(k-1)】
# uk:原值 E(k):当前误差 KP:比例系数 KI:积分系数 KD:微分系数
# 使用PID（可以发现舵机云台运动比较稳定）
# 1 获取误差(x和y方向)（分别计算距离x、y轴中点的误差）
error_x = x - 160 # width:320
error_y = y - 120 # height:240
# 2 PID控制参数
pwm_x = error_x*3 + (error_x - last_error_x)*1
pwm_y = error_y*3 + (error_y - last_error_y)*1
# 这里pwm（p分量） = 当前误差*3 + 上次的误差增量*1
# 3 保存本次误差，以便下一次运算
last_error_x = error_x
last_error_y = error_y
# 4 最终PID值（舵机旋转角度）
pid_X_P -= int(pwm_x/50)
pid_Y_P -= int(pwm_y/50)
# p(pid的p) = 原值 + p分量
'''# 不用PID（舵机云台上下左右乱晃）
if x<160:
pid_X_P += 2
elif x>=160:
pid_X_P -= 2
if y<120:
pid_Y_P += 2
elif y>=120:
pid_Y_P -= 2'''
# 5 限值(0~650)
if pid_X_P>650:
pid_X_P=650
if pid_X_P<0:
pid_X_P=0
if pid_Y_P>650:
pid_Y_P=650
if pid_X_P<0:
pid_Y_P=0四、舵机运动
主程序（多线程舵机控制）
多线程调用舵机控制函数。
# 多线程处理（舵机控制）
servo_tid = threading.Thread(target=Robot_servo)
# 函数 参数
servo_tid.setDaemon(True) # 设置守护线程，防止程序无限挂起
servo_tid.start() # 开启线程舵机运动函数
# 舵机旋转
def Robot_servo():
servo_pwm.set_pwm(5,0,650 - pid_X_P)
servo_pwm.set_pwm(4,0,650 - pid_Y_P)总代码
#!/usr/bin/env python2
# -*- coding: utf-8 -*-
from __future__ import division
import sys
reload(sys)
sys.setdefaultencoding('utf8')
# 人脸跟踪（pid舵机云台）
import cv2
import time
import numpy as np
import Adafruit_PCA9685
import threading
#舵机云台的每个自由度需要4个变量
error_x=500 #当前误差值
last_error_x=100 #上一次误差值
error_y=500
last_error_y=100
# 舵机的转动角度(初始转动角度)
pid_Y_P = 280
pid_X_P = 300
# 初始化PCA9685和舵机
def Servo_Init():
global servo_pwm
servo_pwm = Adafruit_PCA9685.PCA9685() # 实例话舵机云台
# 设置舵机初始值，可以根据自己的要求调试
servo_pwm.set_pwm_freq(60) # 设置频率为60HZ
servo_pwm.set_pwm(5,0,350) # 底座舵机
servo_pwm.set_pwm(4,0,370) # 倾斜舵机
time.sleep(1)
# 摄像头初始化
def Capture_Init():
global capture
#初始化摄像头并设置阙值
capture = cv2.VideoCapture(0)
# 设置显示的分辨率，设置为320×240 px（即摄像头大小）
capture.set(3, 320)
capture.set(4, 240)
# 舵机旋转
def Robot_servo():
servo_pwm.set_pwm(5,0,650 - pid_X_P)
servo_pwm.set_pwm(4,0,650 - pid_Y_P)
# 1 识别人脸
def Face_Detect():
# 1 实例化官方训练好的人脸识别器
face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_alt.xml')
# 2 获取每帧图像
ret,frame = capture.read()
cv2.imshow('frame', frame)
image = frame
# 3 转灰度图
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
cv2.imshow('gray', gray)
# 4 人脸检测
faces = face_cascade.detectMultiScale(gray, 1.3, 1)
# 5 获取人脸坐标并在图像上框出人脸
try:
x,y,w,h = faces[0]
cv2.rectangle(image, (x,y),(x+w,y+h), (255,0,255),3)
cv2.imshow('image',image)
return (x+w/2, y+h/2)
except:
return (0, 0)
# 2 PID舵机控制（这里分别设置使用PID和不用PID的情况）
def PID_Servo_Control(x, y):
global error_x, error_y, last_error_x, last_error_y, pid_X_P, pid_Y_P
# 下面开始pid算法：
# pid总公式：PID = Uk + KP*【E(k)-E(k-1)】 + KI*E(k) + KD*【E(k)-2E(k-1)+E(k-2)】
# 这里只用到了p，所以公式为：P = Uk + KP*【E(k)-E(k-1)】
# uk:原值 E(k):当前误差 KP:比例系数 KI:积分系数 KD:微分系数
# 使用PID（可以发现舵机云台运动比较稳定）
# 1 获取误差(x和y方向)（分别计算距离x、y轴中点的误差）
error_x = x - 160 # width:320
error_y = y - 120 # height:240
# 2 PID控制参数
pwm_x = error_x*3 + (error_x - last_error_x)*1
pwm_y = error_y*3 + (error_y - last_error_y)*1
# 这里pwm（p分量） = 当前误差*3 + 上次的误差增量*1
# 3 保存本次误差，以便下一次运算
last_error_x = error_x
last_error_y = error_y
# 4 最终PID值（舵机旋转角度）
pid_X_P -= int(pwm_x/50)
pid_Y_P -= int(pwm_y/50)
# p(pid的p) = 原值 + p分量
'''# 不用PID（舵机云台上下左右乱晃）
if x<160:
pid_X_P += 2
elif x>=160:
pid_X_P -= 2
if y<120:
pid_Y_P += 2
elif y>=120:
pid_Y_P -= 2'''
# 5 限值(0~650)
if pid_X_P>650:
pid_X_P=650
if pid_X_P<0:
pid_X_P=0
if pid_Y_P>650:
pid_Y_P=650
if pid_X_P<0:
pid_Y_P=0
if __name__ == '__main__':
# 摄像头初始化
Capture_Init()
# 舵机初始化
Servo_Init()
while True:
# 1 识别人脸
(x, y) = Face_Detect()
# 识别到人脸
if not (x==0 and y==0):
# 2 PID舵机控制
PID_Servo_Control(x, y)
# 多线程处理（舵机控制）
servo_tid = threading.Thread(target=Robot_servo)
# 函数 参数
servo_tid.setDaemon(True) # 设置守护线程，防止程序无限挂起
servo_tid.start() # 开启线程
# Robot_servo(pid_X_P, pid_Y_P)
if cv2.waitKey(1)=='q':
break
capture.release()
cv2.destroyAllWindows()
可能有一些不正确或者理解有误的地方，还请不吝赐教。（也可以互相交流一下想法）