机器视觉（6）-- 云台人脸追踪
Table of Contents
概要
硬件清单：
解决方案：
目录
效果展示
PID控制简介
舵机云台旋转方向
像素偏移量
Kp比例系数的调参问题
结合云台上臂
比例控制核心代码
特别鸣谢：1Z实验室阿凯
概要
硬件清单：
- Arduino 数量01 或 STM32 或其他单片机系统
- 舵机DS3218或3518或其他 数量02
- 二自由度舵机支架 数量01套
- USB摄像头或手机IP摄像头
- 树莓派及显示器（可选）
- 连接线若干
解决方案：
首先采集摄像头的图像，原文“使用一款名字叫做IP摄像头的APP”，这里使用电脑的USB摄像头。使用OpenCV的人脸检测的API获取人脸在画面中的位置，根据人脸位置距离画面中心的x轴与y轴的偏移量(offset) ，通过P比例控制(PID控制中最简单的一种)控制二自由度云台上臂与下臂的旋转角度，将角度信息通过串口通信发送给Arduino单片机(不限于Arduino，STM32,ESP32都可以)解析执行对应的操作，从而使得人脸尽可能处在画面的正中间。
视频链接： 【OpenCV基础教程】3.人脸追踪-舵机云台比例控制（1Z实验室）
GitHub源码01：https://github.com/1zlab/1ZLAB_Face_Track_Robot 01 Robot
GitHub源码02：https://github.com/1zlab/1ZLAB_OpenCV_Face_Detection 02 OpenCV face detection
本节课程讲解人脸追踪舵机云台比例控制算法。最开始阿凯简介了PID控制算法，然后通过二自由度云台人脸追踪的项目，讲解了比例控制中的 目标值Target，实例值Real，比例系数Kp的调参过程，以及舵机云台的执行效果演示。
目录
幻灯片3.PNG
效果展示
PID控制简介
幻灯片4.PNG
PID控制算法是自控里面最经典而且使用最广泛的算法， PID的全称为比例-积分-微分控制器。
- P 比例 proportion
- I 积分 Integral
- D 微分 Derivative
本节教程讲解了比例控制，比例控制是最简单的PID控制。结合二自由度人脸追踪的项目，让你对比例控制的理解更加深入。
舵机云台旋转方向
让我们切换一下视角，将云台与摄像头作为第一视角观察这个控制问题。
幻灯片5.PNG
假如我是舵机云台，而且为了简化问题，我们先只关注控制水平方向的舵机，**假设舵机云台现在只能水平旋转 **。
如果发现这个人在画面中偏左的位置，那么为了实现让人脸尽可能处在画面的正中间 的控制目标，我(舵机云台)是不是应该在水平方向上向左旋转才是？如果这个人脸在画面中偏右的位置， 舵机云台是不是应该向右旋转
幻灯片6.PNG
像素偏移量
幻灯片7.PNG
我们现在知道舵机往哪个角度旋转，那旋转的幅度呢？
这个时候我们就需要参照人脸中心在X轴方向的偏移量 offset, 在介绍偏移量offset之前我们需要先介绍目标值(Target) 还有实际值(Real)。
在我们的这个应用里面，人脸中心的X轴坐标的目标值(Target)是整个画面宽度的一半， 如果画面分辨率为800×600 ，那么对于x轴来讲x_target = 800/2 = 400 .
实际值(Real) 指的就是人脸矩形区域中心在画面中坐标的X轴取值。
什么是偏移量(Offset)？即偏移量是指的人脸中心偏移画面中心的值。
Offset = Real - Target
偏移量 = 实际值 - 目标值
举个例子，如果人脸中心的X坐标是200, 画面中心的X坐标是400（画面分辨率为 800×600）, 那么这里的x_offset 就是
x_offset = 200 - 400 = -200
幻灯片8.PNG
注意： 这里考虑到不同分辨率的因素， 将偏移量整体放缩到[-1,1] 。
根据x_offset 的正负可以得到舵机角度的旋转方向（角度增大还是减小）。
这里我们仅仅知道偏移量还不够，我们最终的控制量是舵机角度的增量(变化幅度)， 所以需要在偏移量与舵机角度的增量之间建立某种联系。
当偏移量分别为 1，10， 100 的时候，显然他们的舵机角度增量是不一样，数值越大那么增量也就越大，如果用数学模型来描述它这种关系就是线性关系y=k*x。这里我们定义一个比例系数(Kp)来描述这段关系：
幻灯片9.PNG
delta_degree = Kp * x_offset
新的舵机角度公式为：
新的舵机角度 = 旧的舵机角度 + 角度增量
new_degree = old_degree + delta_degree
Kp比例系数的调参问题
那又来了一个新的问题: 如何确定Kp的取值？
第一步靠猜 Guess
根据offset的取值范围还有舵机角度的范围，估计出一个大致的比例系数Kp。
**注意1：如果人脸在左边，通过比例调节舵机追踪转向了右边，正好相反。 这个时候你应该设定Kp= -Kp **
注意2： 这里的猜不是瞎猜，是有理有据的猜，根据工程经验，得到Kp的大致取值范围。
第二步靠试 Try
如果舵机摆动幅度过大，来回摆动，就说明比例系数太大，需要调小Kp.
如果舵机摆动缓慢，赶不上人移动的速度， 那就需要调大Kp。
通过不断的尝试，最终得到一个合适的Kp.
注： 不同的Kp对应效果见视频教程
设定死区范围
如果人脸在画面中间附近，可能会存在这么一个问题， 就是舵机来回小幅摆动。 解决在目标值附近抖动的问题， 可以这样解决：如果偏移量(offset)小于一定的数值舵机就不动。
注：设定死区代码操作见视频教程
结合云台上臂
再考虑进舵机云台上面的控制，与人脸中心举例画面中心的Y轴偏移量有关系。
X轴与Y轴这两个其实是两个相对独立的比例控制。X轴方向是水平旋转，Y轴方向是纵向旋转。
人脸区域矩形中心举例画面中心有一个Y轴的偏移量y_offset
Y轴的偏移量 = 人脸矩形中心Y轴坐标 - 整个画面高度/2
y_offset = cy - height/2
同样，也有另外一个Kp， 我们称之为Kp2
上臂舵机的角度增量 = 比例系数2 × Y轴偏移量，比例系数越大，步幅越大，失稳的可能性越大
delta_degree2 = Kp2 * y_offset
新的舵机角度 = 旧的舵机角度 + 角度增量
new_degree2 = old_degree2 + delta_degree2
最终将两个角度值（new_degree, new_degree2）分别对应X方向、Y方向的偏移角度值， 通过串口通信发送给单片机执行对应的操作。
以上就是本项目的核心部分：比例控制算法。
比例控制核心代码
完整的人脸追踪代码见1Z实验室的代码工程库： 1ZLAB_Face_Track_Robot
比例控制部分放在了src/pc/ipcam-face-track.py 里面， 下面是核心代码片段的截取：
定义一些常量
btm_kp = 5 # 底部舵机的Kp系数
top_kp = 5 # 顶部舵机的Kp系数
offset_dead_block = 0.1 # 设置偏移量的死区
底部舵机的比例控制代码
def btm_servo_control(offset_x):
'''
底部舵机的比例控制
这里舵机使用开环控制
'''
global offset_dead_block # 偏移量死区大小
global btm_kp # 控制舵机旋转的比例系数
global last_btm_degree # 上一次底部舵机的角度
# 设置最小阈值
if abs(offset_x) < offset_dead_block:
offset_x = 0
# offset范围在-50到50左右
delta_degree = offset_x * btm_kp
# 计算得到新的底部舵机角度
next_btm_degree = last_btm_degree + delta_degree
# 添加边界检测
if next_btm_degree < 0:
next_btm_degree = 0
elif next_btm_degree > 180:
next_btm_degree = 180
return int(next_btm_degree)
顶部舵机的比例控制代码
顶部舵机的比例控制代码与底部舵机的比例控制代码基本一致。
def top_servo_control(offset_y):
'''
顶部舵机的比例控制
这里舵机使用开环控制
'''
global offset_dead_block
global top_kp # 控制舵机旋转的比例系数
global last_top_degree # 上一次顶部舵机的角度
# 如果偏移量小于阈值就不相应
if abs(offset_y) < offset_dead_block:
offset_y = 0
# offset_y *= -1
# offset范围在-50到50左右
delta_degree = offset_y * top_kp
# 新的顶部舵机角度
next_top_degree = last_top_degree + delta_degree
# 添加边界检测
if next_top_degree < 0:
next_top_degree = 0
elif next_top_degree > 180:
next_top_degree = 180
return int(next_top_degree)