如何讓機器人頭部攝像頭跟隨識別到人臉位置變化而轉動？一幀圖像是 2 維的，人臉位置用坐標 (x,y) 表示，要實現跟蹤人臉則需要兩個軸 pan, tilt

  • pan: 水平左右方向轉頭
  • tilt: 豎直上下方向轉頭

人臉識別

首先需要在每一幀圖像中識別到人臉，face_recognition 一個簡單易用的人臉識別開源項目，並且配備了完整的開發文檔和示例代碼，還特別兼容了樹莓派。face_recognition 基於 C++ 開源庫 dlib 的深度學習模型，使用 Labeled Faces in the Wild 人臉數據集進行測試，有高達99.38%的準確率。

安裝 face_recognition

安裝步驟請參考: face_recognition#installation

識別人臉 Python 代碼

import face_recognition

def face_location(frame, frame_center):
face_locations = face_recognition.face_locations(frame)
if len(face_locations) > 0:
y0, x0, y1, x1 = face_locations[0]
face_x = int((x0 + x1) / 2)
face_y = int((y0 + y1) / 2)
return face_x, face_y
return frame_center

PID 控制

使用 face_recognition 可以很容易地用 Python 代碼實現人臉識別，計算出人臉在一幀圖像中的像素坐標 (x, y)，接下來就是需要控制舵機對人臉進行跟蹤。

控制目標：調整橫向和縱向兩個自由度的舵機，使得攝像頭中的人臉中心與圖像的中心重合。

這就需要引入 PID 控制了，先直接放公式

  • 時間 t，在這裡時間是離散的；
  • 偏差 e(t)，在人臉跟蹤中指的是圖像中心與人臉中心之間的距離（x 方向，y 方向）；
  • 系統輸出 u(t)，即輸出的舵機角度(分水平和垂直方向兩個舵機的角度)；

接下來需要理解，Kp?，Ki?，Kd? 三個參數的作用，先放一張動圖直觀感受下參數的作用

比例(P)

比例控制的輸出信號與輸入偏差成比例關係。偏差一旦產生，控制器立即產生控制作用以減小偏差，是最基本的控制規律。當僅有比例控制時系統輸出存在穩態誤差。

  • 當人臉與圖像中心相距較遠時，需要舵機大幅度運動對準人臉
  • 當人臉與圖像中心相距較近時，需要舵機小幅度靠近對準人臉

根據 kp 取值不同，攝像頭都會去對準人臉，只是 kp 大了到達的快，kp 小了到達的慢一些。

積分(I)

防止系統進入穩定後存在的穩定誤差，即有可能攝像頭穩定後停下來了，但是沒有對準人臉的中心。為了消除穩態誤差，必須引入積分控制。積分作用是對歷史的偏差進行積分，隨著時間的增加，積分輸出會增大，使穩態誤差進一步減小，直到偏差為零，纔不再繼續增加，最後系統穩定下來，纔可能正好中心對準人臉。

微分(D)

在微分控制中，控制器的輸出與輸入偏差信號的微分(即偏差的變化率)成正比關係。微分控制反映偏差的變化率，只有當偏差隨時間變化時，微分控制才會對系統起作用，而對無變化或緩慢變化的對象不起作用。通俗來說，是為了在人臉追蹤時，防止追過勁了，在中心對準人臉後可以及時地停止，防止震蕩。

PID 代碼實現

在理解概念和公式後，就不難代碼實現

# https://www.pyimagesearch.com/2019/04/01/pan-tilt-face-tracking-with-a-raspberry-pi-and-opencv/
import time

class PID:
def __init__(self, kP=1, kI=0, kD=0):
# initialize gains
self.kP = kP
self.kI = kI
self.kD = kD

def initialize(self):
# intialize the current and previous time
self.currTime = time.time()
self.prevTime = self.currTime

# initialize the previous error
self.prevError = 0

# initialize the term result variables
self.cP = 0
self.cI = 0
self.cD = 0

def update(self, error, sleep=0.2):
# pause for a bit
time.sleep(sleep)

# grab the current time and calculate delta time
self.currTime = time.time()
deltaTime = self.currTime - self.prevTime

# delta error
deltaError = error - self.prevError

# proportional term
self.cP = error

# integral term
self.cI += error * deltaTime

# derivative term and prevent divide by zero
self.cD = (deltaError / deltaTime) if deltaTime > 0 else 0

# save previous time and error for the next update
self.prevTime = self.currTime
self.prevError = error

# sum the terms and return
return sum([
self.kP * self.cP,
self.kI * self.cI,
self.kD * self.cD])

代碼設計

由於雲臺有 2 個自由度（pan, tilt），所以需要用到 2 個 PID 控制器，來輸出對應的角度

  • x 軸偏差 對應 水平左右轉動 pan
  • y 軸偏差 對應 垂直上下轉動 tilt

在代碼設計時，需要考慮如下幾點限制因素

  • 受限於設備的性能，使用 face_recognition 識別一幀圖像裏的人臉可能會非常耗時
  • 舵機控制時，不同的舵機響應時間不同
  • PID 在代碼中是通過循環累加來計算的；

def thread_face_center():
print(face_center ..)
process_this_frame = 0
while True:
time.sleep(0.01)
if not QUEUE_IMG.empty():
frame = QUEUE_IMG.get()
else:
continue

(h, w) = frame.shape[:2]
HEAD.center_x = w // 2
HEAD.center_y = h // 2

if process_this_frame > 8:
HEAD.obj_x, HEAD.obj_y = face_location(frame, (HEAD.center_x, HEAD.center_y))
print(HEAD.obj_x, HEAD.obj_y)
process_this_frame = 0
process_this_frame += 1

def thread_pid_pan():
p, i, d = 0.09, 0.08, 0.002

pid = PID(p, i, d)
pid.initialize()
while True:
error = HEAD.center_x - HEAD.obj_x
HEAD.pan = pid.update(error)

def thread_pid_tlt():
p, i, d = 0.11, 0.10, 0.002

pid = PID(p, i, d)
pid.initialize()
while True:
error = HEAD.center_y - HEAD.obj_y
HEAD.tlt = pid.update(error)

def thread_set_servos():
set_head_servo([0, 90])
while True:
time.sleep(0.01)
pan_angle = HEAD.pan + 0
tlt_angle = 90 - HEAD.tlt
print([pan_angle, tlt_angle] = , pan_angle, tlt_angle)
set_head_servo([pan_angle, tlt_angle])

需要將人臉識別、PID 過程、角度控制放到單獨的線程處理。

async_do_job(thread_face_center)
async_do_job(thread_pid_pan)
async_do_job(thread_pid_tlt)
async_do_job(thread_set_servos)

參考鏈接

  • Wikipedia: PID 控制器
  • The worlds simplest facial recognition api for Python and the command line
  • Pan/tilt face tracking with a Raspberry Pi and OpenCV

推薦閱讀：

相關文章