Geek专栏：基于 face_recognition 和PID 的舵机云台人脸识别和跟踪

今天Geek专栏为大家带来
乐聚机器人王松博士的
“基于face_recognition 和 PID 的舵机云台人脸识别和跟踪”

如何让机器人头部摄像头跟随识别到人脸位置变化而转动？一帧图像是 2 维的，人脸位置用坐标 (x,y) 表示，要实现跟踪人脸则需要两个轴 pan, tilt

• pan: 水平左右方向转头
• tilt: 竖直上下方向转头
  

人脸识别

首先需要在每一帧图像中识别到人脸，face_recognition（https://github.com/ageitgey/face_recognition）一个简单易用的人脸识别开源项目，并且配备了完整的开发文档和示例代码，还特别兼容了树莓派。face_recognition 基于 C++ 开源库 dlib 的深度学习模型，使用 Labeled Faces in the Wild（http://vis-www.cs.umass.edu/lfw/）人脸数据集进行测试，有高达99.38%的准确率。

安装 face_recognition

安装步骤请参考:face_recognition#installation
（https://github.com/ageitgey/face_recognition#installation）

识别人脸 Python 代码

1. import face_recognition
   
2. def face_location(frame, frame_center):
   
3.     face_locations = face_recognition.face_locations(frame)
   
4.     if len(face_locations) > 0:
   
5.         y0, x0, y1, x1 = face_locations[0]
   
6.         face_x = int((x0 + x1) / 2)
   
7.         face_y = int((y0 + y1) / 2)
   
8.         return face_x, face_y
   
9.     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 代码实现


在理解概念和公式后，就不难代码实现

1. # https://www.pyimagesearch.com/2019/04/01/pan-tilt-face-tracking-with-a-raspberry-pi-and-opencv/
   
2. import time
   
3. class PID:
   
4.     def __init__(self, kP=1, kI=0, kD=0):
   
5.         # initialize gains
   
6.         self.kP = kP
   
7.         self.kI = kI
   
8.         self.kD = kD
   
9.     def initialize(self):
   
10.         # intialize the current and previous time
    
11.         self.currTime = time.time()
    
12.         self.prevTime = self.currTime
    
13.         # initialize the previous error
    
14.         self.prevError = 0
    
15.         # initialize the term result variables
    
16.         self.cP = 0
    
17.         self.cI = 0
    
18.         self.cD = 0
    
19.     def update(self, error, sleep=0.2):
    
20.         # pause for a bit
    
21.         time.sleep(sleep)
    
22.         # grab the current time and calculate delta time
    
23.         self.currTime = time.time()
    
24.         deltaTime = self.currTime - self.prevTime
    
25.         # delta error
    
26.         deltaError = error - self.prevError
    
27.         # proportional term
    
28.         self.cP = error
    
29.         # integral term
    
30.         self.cI += error * deltaTime
    
31.         # derivative term and prevent divide by zero
    
32.         self.cD = (deltaError / deltaTime) if deltaTime > 0 else 0
    
33.         # save previous time and error for the next update
    
34.         self.prevTime = self.currTime
    
35.         self.prevError = error
    
36.         # sum the terms and return
    
37.         return sum([
    
38.             self.kP * self.cP,
    
39.             self.kI * self.cI,
    
40.             self.kD * self.cD])
    

复制代码

代码设计

由于云台有 2 个自由度（pan, tilt），所以需要用到 2 个 PID 控制器，来输出对应的角度

• x 轴偏差 对应 水平左右转动 pan
• y 轴偏差 对应 垂直上下转动 tilt
  

在代码设计时，需要考虑如下几点限制因素

• 受限于设备的性能，使用 face_recognition 识别一帧图像里的人脸可能会非常耗时
• 舵机控制时，不同的舵机响应时间不同
• PID 在代码中是通过循环累加来计算的；
  

1. def thread_face_center():
   
2.     print('face_center ..')
   
3.     process_this_frame = 0
   
4.     while True:
   
5.         time.sleep(0.01)
   
6.         if not QUEUE_IMG.empty():
   
7.             frame = QUEUE_IMG.get()
   
8.         else:
   
9.             continue
   
10. 
    
11.         (h, w) = frame.shape[:2]
    
12.         HEAD.center_x = w // 2
    
13.         HEAD.center_y = h // 2
    
14. 
    
15.         if process_this_frame > 8:
    
16.             HEAD.obj_x, HEAD.obj_y = face_location(frame, (HEAD.center_x, HEAD.center_y))
    
17.             print(HEAD.obj_x, HEAD.obj_y)
    
18.             process_this_frame = 0
    
19.         process_this_frame += 1
    
20. 
    
21. 
    
22. def thread_pid_pan():
    
23.     p, i, d = 0.09, 0.08, 0.002
    
24. 
    
25.     pid = PID(p, i, d)
    
26.     pid.initialize()
    
27.     while True:
    
28.         error = HEAD.center_x - HEAD.obj_x
    
29.         HEAD.pan = pid.update(error)
    
30. 
    
31. 
    
32. def thread_pid_tlt():
    
33.     p, i, d = 0.11, 0.10, 0.002
    
34. 
    
35.     pid = PID(p, i, d)
    
36.     pid.initialize()
    
37.     while True:
    
38.         error = HEAD.center_y - HEAD.obj_y
    
39.         HEAD.tlt = pid.update(error)
    
40. 
    
41. def thread_set_servos():
    
42.     set_head_servo([0, 90])
    
43.     while True:
    
44.         time.sleep(0.01)
    
45.         pan_angle = HEAD.pan + 0
    
46.         tlt_angle = 90 - HEAD.tlt
    
47.         print('[pan_angle, tlt_angle] = ', pan_angle, tlt_angle)
    
48.         set_head_servo([pan_angle, tlt_angle])
    

复制代码

需要将人脸识别、PID过程、角度控制放到单独的线程处理。

1. async_do_job(thread_face_center)
   
2. async_do_job(thread_pid_pan)
   
3. async_do_job(thread_pid_tlt)
   
4. async_do_job(thread_set_servos)
   

复制代码

参考链接

• Wikipedia: PID 控制器
  

（https://zh.wikipedia.org/wiki/PID%E6%8E%A7%E5%88%B6%E5%99%A8）

• The world's simplest facial recognition api for Python and the command line
  

（https://github.com/ageitgey/face_recognition）

• Pan/tilt face tracking with a Raspberry Pi and OpenCV
  

（https://www.pyimagesearch.com/20 ... erry-pi-and-opencv/）

Geek专栏：基于 face_recognition 和PID 的舵机云台人脸识别和跟踪

今天Geek专栏为大家带来
乐聚机器人王松博士的
“基于face_recognition 和 PID 的舵机云台人脸识别和跟踪”

如何让机器人头部摄像头跟随识别到人脸位置变化而转动？一帧图像是 2 维的，人脸位置用坐标 (x,y) 表示，要实现跟踪人脸则需要两个轴 pan, tilt

• pan: 水平左右方向转头
• tilt: 竖直上下方向转头
  

人脸识别

首先需要在每一帧图像中识别到人脸，face_recognition（https://github.com/ageitgey/face_recognition）一个简单易用的人脸识别开源项目，并且配备了完整的开发文档和示例代码，还特别兼容了树莓派。face_recognition 基于 C++ 开源库 dlib 的深度学习模型，使用 Labeled Faces in the Wild（http://vis-www.cs.umass.edu/lfw/）人脸数据集进行测试，有高达99.38%的准确率。

安装 face_recognition

安装步骤请参考:face_recognition#installation
（https://github.com/ageitgey/face_recognition#installation）

识别人脸 Python 代码

1. import face_recognition
   
2. def face_location(frame, frame_center):
   
3.     face_locations = face_recognition.face_locations(frame)
   
4.     if len(face_locations) > 0:
   
5.         y0, x0, y1, x1 = face_locations[0]
   
6.         face_x = int((x0 + x1) / 2)
   
7.         face_y = int((y0 + y1) / 2)
   
8.         return face_x, face_y
   
9.     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 代码实现


在理解概念和公式后，就不难代码实现

1. # https://www.pyimagesearch.com/2019/04/01/pan-tilt-face-tracking-with-a-raspberry-pi-and-opencv/
   
2. import time
   
3. class PID:
   
4.     def __init__(self, kP=1, kI=0, kD=0):
   
5.         # initialize gains
   
6.         self.kP = kP
   
7.         self.kI = kI
   
8.         self.kD = kD
   
9.     def initialize(self):
   
10.         # intialize the current and previous time
    
11.         self.currTime = time.time()
    
12.         self.prevTime = self.currTime
    
13.         # initialize the previous error
    
14.         self.prevError = 0
    
15.         # initialize the term result variables
    
16.         self.cP = 0
    
17.         self.cI = 0
    
18.         self.cD = 0
    
19.     def update(self, error, sleep=0.2):
    
20.         # pause for a bit
    
21.         time.sleep(sleep)
    
22.         # grab the current time and calculate delta time
    
23.         self.currTime = time.time()
    
24.         deltaTime = self.currTime - self.prevTime
    
25.         # delta error
    
26.         deltaError = error - self.prevError
    
27.         # proportional term
    
28.         self.cP = error
    
29.         # integral term
    
30.         self.cI += error * deltaTime
    
31.         # derivative term and prevent divide by zero
    
32.         self.cD = (deltaError / deltaTime) if deltaTime > 0 else 0
    
33.         # save previous time and error for the next update
    
34.         self.prevTime = self.currTime
    
35.         self.prevError = error
    
36.         # sum the terms and return
    
37.         return sum([
    
38.             self.kP * self.cP,
    
39.             self.kI * self.cI,
    
40.             self.kD * self.cD])
    

复制代码

代码设计

由于云台有 2 个自由度（pan, tilt），所以需要用到 2 个 PID 控制器，来输出对应的角度

• x 轴偏差 对应 水平左右转动 pan
• y 轴偏差 对应 垂直上下转动 tilt
  

在代码设计时，需要考虑如下几点限制因素

• 受限于设备的性能，使用 face_recognition 识别一帧图像里的人脸可能会非常耗时
• 舵机控制时，不同的舵机响应时间不同
• PID 在代码中是通过循环累加来计算的；
  

1. def thread_face_center():
   
2.     print('face_center ..')
   
3.     process_this_frame = 0
   
4.     while True:
   
5.         time.sleep(0.01)
   
6.         if not QUEUE_IMG.empty():
   
7.             frame = QUEUE_IMG.get()
   
8.         else:
   
9.             continue
   
10. 
    
11.         (h, w) = frame.shape[:2]
    
12.         HEAD.center_x = w // 2
    
13.         HEAD.center_y = h // 2
    
14. 
    
15.         if process_this_frame > 8:
    
16.             HEAD.obj_x, HEAD.obj_y = face_location(frame, (HEAD.center_x, HEAD.center_y))
    
17.             print(HEAD.obj_x, HEAD.obj_y)
    
18.             process_this_frame = 0
    
19.         process_this_frame += 1
    
20. 
    
21. 
    
22. def thread_pid_pan():
    
23.     p, i, d = 0.09, 0.08, 0.002
    
24. 
    
25.     pid = PID(p, i, d)
    
26.     pid.initialize()
    
27.     while True:
    
28.         error = HEAD.center_x - HEAD.obj_x
    
29.         HEAD.pan = pid.update(error)
    
30. 
    
31. 
    
32. def thread_pid_tlt():
    
33.     p, i, d = 0.11, 0.10, 0.002
    
34. 
    
35.     pid = PID(p, i, d)
    
36.     pid.initialize()
    
37.     while True:
    
38.         error = HEAD.center_y - HEAD.obj_y
    
39.         HEAD.tlt = pid.update(error)
    
40. 
    
41. def thread_set_servos():
    
42.     set_head_servo([0, 90])
    
43.     while True:
    
44.         time.sleep(0.01)
    
45.         pan_angle = HEAD.pan + 0
    
46.         tlt_angle = 90 - HEAD.tlt
    
47.         print('[pan_angle, tlt_angle] = ', pan_angle, tlt_angle)
    
48.         set_head_servo([pan_angle, tlt_angle])
    

复制代码

需要将人脸识别、PID过程、角度控制放到单独的线程处理。

1. async_do_job(thread_face_center)
   
2. async_do_job(thread_pid_pan)
   
3. async_do_job(thread_pid_tlt)
   
4. async_do_job(thread_set_servos)
   

复制代码

参考链接

• Wikipedia: PID 控制器
  

（https://zh.wikipedia.org/wiki/PID%E6%8E%A7%E5%88%B6%E5%99%A8）

• The world's simplest facial recognition api for Python and the command line
  

（https://github.com/ageitgey/face_recognition）

• Pan/tilt face tracking with a Raspberry Pi and OpenCV
  

（https://www.pyimagesearch.com/20 ... erry-pi-and-opencv/）