人脸追踪-舵机云台比例控制-1Z实验室

2018-08-16 本文已影响753人 1Z实验室阿凯

出品：1Z实验室（1ZLAB： Make Things Easy）

概要

视频链接：【OpenCV基础教程】3.人脸追踪-舵机云台比例控制（1Z实验室）

幻灯片1.PNG

本节课程讲解人脸追踪舵机云台比例控制算法。最开始阿凯简介了PID控制算法，然后通过二自由度云台人脸追踪的项目，讲解了比例控制中的目标值Target，实例值Real，比例系数Kp的调参过程，以及舵机云台的执行效果演示。

效果展示

幻灯片2.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比例系数的调参问题

幻灯片10.PNG

那又来了一个新的问题: 如何确定Kp的取值？

第一步靠猜 Guess

根据offset的取值范围还有舵机角度的范围，估计出一个大致的比例系数Kp。

**注意1：如果人脸在左边，通过比例调节舵机追踪转向了右边，正好相反。这个时候你应该设定Kp= -Kp **

注意2：这里的猜不是瞎猜，是有理有据的猜，根据工程经验，得到Kp的大致取值范围。

第二步靠试 Try

如果舵机摆动幅度过大，来回摆动，就说明比例系数太大，需要调小Kp.

如果舵机摆动缓慢，赶不上人移动的速度，那就需要调大Kp。

通过不断的尝试，最终得到一个合适的Kp.

注：不同的Kp对应效果见视频教程

设定死区范围

[图片上传失败...(image-dad86e-1534419405654)]

如果人脸在画面中间附近，可能会存在这么一个问题，就是舵机来回小幅摆动。解决在目标值附近抖动的问题，可以这样解决：如果偏移量(offset)小于一定的数值舵机就不动。

幻灯片12.PNG

注：设定死区代码操作见视频教程

结合云台上臂

再考虑进舵机云台上面的控制，与人脸中心举例画面中心的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） 通过串口通信发送给单片机执行对应的操作。

以上就是本项目的核心部分：比例控制算法。

比例控制核心代码

完整的人脸追踪代码见1Z实验室的代码工程库： 1ZLAB_Face_Track_Robot

幻灯片13.PNG

比例控制部分放在了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)

推广