基于PID的树莓派控制二自由度舵机人脸追踪云台设计

人脸追踪云台的设计

一、舵机控制概述

1. 脉冲宽度与舵机转角：
   在脉冲信号频率50Hz的条件下输入的脉冲信号宽度和舵机转角的对应关系图以如图1.所示，该转角的计算公式如下。树莓派端通过占空比信号对舵机进行角度控制，将该占空比信号通过树莓派端传输给PCA9685芯片来控制舵机，实现追踪人脸的动作。
   

图1. 脉冲宽度与舵机转角的关系

脉冲数与转动角度的计算公式为：
$((Angle\ast 11)+500)/20000=Puls{e}_n/4096$

2. PID算法概述：
   本设计的舵机云台控制采用PID控制算法，其是自动控制原理中比较经典的控制算法，控制流程图如下：
   

图2. PID控制算法流程图

本设计采用省去积分环节$K_i$(即令$K_i$=0) 的位置式PID来进行控制，其公式如下：
$\begin{array}{rl}\text{ Output }& =K_p\ast e_k+K_i\sum _{j=0}^k{e}_j+K_d\left(e_k-e_{k-1}\right)\\ & =K_p\ast e_k+K_d\left(e_k-e_{k-1}\right)\end{array}$

---

二、人脸追踪控制方法

1. 位置坐标更新：
   开机后，将摄像头捕捉的视频帧通过Haar级联分类器进行人脸定位，得到人脸左上角在整体图像中的位置坐标，再通过该坐标来计算出人脸中心点坐标并记录。
2. PID计算程序：
   将程序上一循环记录的人脸中心点坐标与当前最新的坐标进行求差，分别得到x轴、y轴的坐标误差量thisError_x、thisError_y并记录，结束完本次运算（公式如下图）后再当作程序下轮循环的前次误差值lastError_x、lastError_y，最后将计算结果传给舵机执行，本设计中$K_p$、$K_d$的值均设置为7。
   

图3. 舵机动作的PID计算程序

3. 舵机的GPIO控制：
   PCA9685中A0-A5为片选地址端，当串联芯片时用来选择确定控制哪一片。将输出使能端OE接地，表示使能控制芯片；EXTCLK为外部时钟信号端口，可以连接树莓派芯片并保持与其时钟同步；将I2C总线的时钟信号线（SCL）和数据线（SDA）连接树莓派端的对应IO口，在PCA9685的控制寄存器06h～45h地址上写入低8位、高8位共16位的数据来控制输出16路PWM信号（LED0-LED15）开关。各个输出端口都配有一个精度为12位的计数器，可将每个计数周期划分为$2^{12}$=4096份；最终再通过控制两个通、断寄存器获得想要的脉冲宽度从而控制舵机转动角度来追踪人脸。

图4. PCA9685舵机控制器电路