目錄

效果展示

基礎理論（人臉識別）

1、基于特征的演算法

2、基于影像的演算法

3、Haar特征

4、Adaboost級聯決策器

API

基礎理論（PID演算法）

1、作用

應用場景

2、PID原理

1、P（比例）

2、D（微分）

3、I（積分）

3、PID公式

1、位置式演算法

2、增量式演算法

一、初始化

二、人臉識別

主程式

1、創建人臉分類器

2、打開攝像頭

3、轉灰度圖

4、人臉檢測

5、獲取人臉坐標、在影像上框出人臉

代碼

三、PID處理

主程式

函式前部

1、獲取誤差（x、y方向）

2、PID控制引數

3、保存本次誤差

4、得到最終的PID值（P分量）

5、限值

對比：不用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 = frame

3、轉灰度圖

# 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處理

主程式

# 識別到人臉
        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:240

2、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 + 上次的誤差增量*1

3、保存本次誤差

# 3 保存本次誤差，以便下一次運算
    last_error_x = error_x
    last_error_y = error_y

4、得到最終的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()

可能有一些不正確或者理解有誤的地方，還請不吝賜教，（也可以互相交流一下想法）