树莓派4B-Python-使用PCA9685控制舵机云台+跟随人脸转动
系列文章
- 树莓派4B-Python-控制舵机
- 树莓派-Pico控制舵机
- 树莓派4B-Python-使用PCA9685控制舵机云台+跟随人脸转动（本文章）
目录
- 系列文章
- 前言
- 一、SG90s舵机是什么？
- 二、PCA9685与舵机信号线的接线图
- 三、控制SG90s云台（也可用来测试舵机转动的范围）
- 四、给树莓派注入灵魂（代码）
- 五、给电脑注入灵魂（代码）
- 总结
前言
先说明一下哈，本人用的是树莓派4B，Python的版本为3.7，OpenCV的版本为3.2.0
计划了好久的舵机云台终于有机会做出来了！原先一开始用的是塑料的云台+SG90舵机，但效果有点怪，云台会乱抽搐，于是换了金属的云台+SG90s舵机，嘿嘿，现在就没啥问题。
后来想了一下，还是觉得是以下的问题导致云台抽搐的：
- 舵机线靠近其他正在通电的电源线，受到电磁的干扰，导致舵机信号被干扰；
- PCA9685舵机控制板的问题。
1、与舵机的接线接触不良；
2、控制板与树莓派的接线接触不良。一般为通信线接触不良。
3、控制板的5V供电有问题。输出电压不够5V或输出电流小于1A。 - 云台顺滑度的问题。本人的就是云台的上下转动的舵机那里卡了，导致变换不了舵机的角度，最后舵机发热严重，排查了好久才发现这个问题。
一、SG90s舵机是什么？
简单的说，就是SG90舵机的齿轮换成了金属齿轮而已，扭力增加了而已。可以理解为SG90舵机的升级版吧。
还是不知道的可以参考之前写的关于舵机的内容：
树莓派4B-Python-控制舵机
树莓派-Pico控制舵机
二、PCA9685与舵机信号线的接线图
简单说明一下接线：
| PCA9685 | 树莓派4B |
|---|---|
| GND | GND |
| DE | 空着 |
| SCL | SCL |
| SDA | SDA |
| VCC | 3.3V |
| V+ | 5V（5V1A） |
一定要接V+到5V，不然板子达不到控制舵机的效果。
若想外接5V1A的电源，那最好是将那个电源的GND与树莓派的GND连上，也就是共地。
提示：使用下面的代码前，记得先测试好舵机的转动角度哦，若转动的角度会卡死的话，舵机轻则发热，重则爆炸，所以切记切记，一定要先确定好舵机转动的范围。
三、控制SG90s云台（也可用来测试舵机转动的范围）
import time # 引入time库
from adafruit_servokit import ServoKit # 引入刚刚安装的PCA9685库并将名字缩写成ServoKit方便后续调用
# from Adafruit_PCA9685 import ServoKit
kit = ServoKit(channels=16) # 明确PCA9685的舵机控制数
Servo1 = kit.servo[0] # 左右转向的舵机
Servo2 = kit.servo[1] # 上下移动的舵机
Servo3 = kit.servo[2] # 前后伸缩的舵机
Servo4 = kit.servo[3] # 夹子舵机
Servo5 = kit.servo[14]
Servo6 = kit.servo[15]
Servo = [Servo1, Servo2, Servo3, Servo4, Servo5, Servo6]
def servo_start(servo_number, start_angle, stop_angle, angle_sleep, time_sleep):
"""
servo_number:设置想要控制的舵机通道
start_angle:设置起始的角度
stop_angle:设置终止的角度
angle_sleep:设置角度变换的步长 一般设为10
time_sleep:设置角度变换的间隔 数字越小越快，但最好不要设置太快，舵机会反应不过来然后抽搐。
最快0.005s，但是到达指定角度位置后需要等待1s左右让程序反应过来，否则会抽搐。
"""
for speed in range(start_angle, stop_angle, angle_sleep):
servo_number.angle = speed
print(speed)
time.sleep(time_sleep)
def servo_dongzuo1():
"""
先让机械臂向后缩，然后向前伸，再向下移动，最后向上移动，这就回到了原点
"""
time_sleep = 0.01
servo_start(Servo3, 110, 41, -1, time_sleep)
servo_start(Servo3, 40, 111, 1, time_sleep)
time.sleep(0.1)
servo_start(Servo2, 130, 31, -1, time_sleep)
servo_start(Servo2, 30, 131, 1, time_sleep)
time.sleep(0.1)
def aa():
time_sleep = 0.01
servo_start(Servo3, 40, 181, 1, time_sleep)
servo_start(Servo3, 180, 41, -1, time_sleep)
servo_start(Servo1, 90, 0, -1, 0.01)
servo_start(Servo1, 0, 180, 1, 0.01)
servo_start(Servo1, 180, 0, -1, 0.01)
servo_start(Servo1, 0, 90, 1, 0.01)
servo_start(Servo2, 80, 170, 1, 0.01)
servo_start(Servo2, 170, 80, -1, 0.01)
四、给树莓派注入灵魂（代码）
注入的灵魂如下（示例）：
# -*- coding: utf-8 -*-
import cv2
import time # 引入time库
from adafruit_servokit import ServoKit # 引入刚刚安装的PCA9685库并将名字缩写成ServoKit方便后续调用
class Follow_the_face:
def __init__(self):
kit = ServoKit(channels=16) # 明确PCA9685的舵机控制数
self.Servo1 = kit.servo[0] # 左右转向的舵机
self.Servo2 = kit.servo[1] # 上下移动的舵机
self.Servo3 = kit.servo[2] # 前后伸缩的舵机
self.Servo4 = kit.servo[3] # 夹子舵机
self.Servo5 = kit.servo[14]
self.Servo6 = kit.servo[15]
self.Servo = [self.Servo1, self.Servo2, self.Servo3, self.Servo4, self.Servo5, self.Servo6]
self.l_r_number_of_degrees = 110 # 左右电机初始化的角度
self.u_d_number_of_degrees = 150 # 上下电机初始化的角度
self.range_value = 30 # 在人脸检测中，设置人脸框的中心坐标与屏幕中心坐标的可接受偏移的范围
def servo_start(self, servo_number, start_angle, stop_angle, angle_sleep, time_sleep):
"""
servo_number:设置想要控制的舵机通道
start_angle:设置起始的角度
stop_angle:设置终止的角度
angle_sleep:设置角度变换的步长 一般设为10
time_sleep:设置角度变换的间隔 数字越小越快，但最好不要设置太快，舵机会反应不过来然后抽搐。
最快0.005s，但是到达指定角度位置后需要等待1s左右让程序反应过来，否则会抽搐。
"""
for speed in range(start_angle, stop_angle, angle_sleep):
servo_number.angle = speed
print(speed)
time.sleep(time_sleep)
def Inittalize(self):
"""
初始化云台的各个位置，也算是一个启动前的角度自检和归位
"""
self.servo_start(self.Servo1, 111, 110, -1, 0.01) # 可转动的角度为：0～180
self.servo_start(self.Servo2, 180, 40, -1, 0.01) # 可转动的角度为：40～180.40度时为垂直向上看
self.servo_start(self.Servo2, 40, 150, 1, 0.01)
def Auto_Angle(self, direction, offset_number): # , auto_angle
"""
自动偏移指定度数
direction：选择方向
offset_number：偏移数
auto_angle：自动角度
"""
if direction == "左":
self.l_r_number_of_degrees -= offset_number
if 1 <= self.l_r_number_of_degrees <= 180:
self.servo_start(self.Servo1, self.l_r_number_of_degrees, self.l_r_number_of_degrees - 1, -1, 0.01)
else:
self.l_r_number_of_degrees = self.l_r_number_of_degrees
if direction == "右":
self.l_r_number_of_degrees += offset_number
if 1 <= self.l_r_number_of_degrees <= 180:
self.servo_start(self.Servo1, self.l_r_number_of_degrees, self.l_r_number_of_degrees - 1, -1, 0.01)
else:
self.l_r_number_of_degrees = self.l_r_number_of_degrees
if direction == "上":
self.u_d_number_of_degrees -= offset_number
print('上被触发', self.u_d_number_of_degrees)
if 41 <= self.u_d_number_of_degrees <= 179:
self.servo_start(self.Servo2, self.u_d_number_of_degrees, self.u_d_number_of_degrees - 1, -1, 0.01)
elif self.u_d_number_of_degrees > 179:
u_d_number_of_degrees = 179
self.servo_start(self.Servo2, u_d_number_of_degrees, u_d_number_of_degrees - 1, -1, 0.01)
elif self.u_d_number_of_degrees < 40:
self.u_d_number_of_degrees = 40
self.servo_start(self.Servo2, self.u_d_number_of_degrees, self.u_d_number_of_degrees - 1, -1, 0.01)
else:
print('上出现意外', self.u_d_number_of_degrees)
if direction == "下":
self.u_d_number_of_degrees += offset_number
print('下被触发', self.u_d_number_of_degrees)
if 41 <= self.u_d_number_of_degrees <= 179:
self.servo_start(self.Servo2, self.u_d_number_of_degrees, self.u_d_number_of_degrees - 1, -1, 0.01)
elif self.u_d_number_of_degrees > 179:
self.u_d_number_of_degrees = 179
self.servo_start(self.Servo2, self.u_d_number_of_degrees, self.u_d_number_of_degrees - 1, -1, 0.01)
elif self.u_d_number_of_degrees < 40:
self.u_d_number_of_degrees = 40
self.servo_start(self.Servo2, self.u_d_number_of_degrees, self.u_d_number_of_degrees - 1, -1, 0.01)
else:
print('下出现意外', self.u_d_number_of_degrees)
def determine_face_position(self, difference_value_x, difference_value_y):
"""
检测人脸位置
:param difference_value_x:x偏移的距离值
:param difference_value_y:y偏移的距离值
:return:
"""
if -self.range_value <= difference_value_x <= self.range_value and -self.range_value <= difference_value_y <= self.range_value:
print("x坐标、y坐标都不用移动")
elif difference_value_x > self.range_value:
print("摄像头向右转动一点")
self.Auto_Angle('右', 1)
if difference_value_y > self.range_value:
print("摄像头向下转动一点1")
self.Auto_Angle('下', 2)
if difference_value_y < -self.range_value:
print("摄像头向上转动一点1")
self.Auto_Angle('上', 2)
elif difference_value_x < -self.range_value:
print("摄像头向左转动一点")
self.Auto_Angle('左', 1)
if difference_value_y > self.range_value:
print("摄像头向下转动一点2")
self.Auto_Angle('下', 2)
if difference_value_y < -self.range_value:
print("摄像头向上转动一点2")
self.Auto_Angle('上', 2)
elif difference_value_y > self.range_value:
print("摄像头向下转动一点")
self.Auto_Angle('下', 2)
elif difference_value_y < -self.range_value:
print("摄像头向上转动一点")
self.Auto_Angle('上', 2)
def turn_on_face_detection(self):
"""
使用人脸检测
:return:
"""
# 加载预训练的人脸检测器。直接选择树莓派中人脸模型（haarcascade_frontalface_default.xml）的路径即可
face_cascade = cv2.CascadeClassifier('/home/pi/Desktop/12_29/haarcascade_frontalface_default.xml')
# face_cascade = cv2.CascadeClassifier(cv2.data.haarcascades + 'haarcascade_frontalface_default.xml')
# 创建用于显示相机画面的窗口
cv2.namedWindow("Camera Feed")
# 打开摄像头
cap = cv2.VideoCapture(0)
r, f = cap.read()
# 获取画面的宽度和高度
h, w, ch = f.shape
print("输出画面的高度和宽度", h, w)
center_x1 = int(w / 2)
center_y1 = int(h / 2)
print("输出中心坐标为：", center_x1, center_y1)
# 记录人脸中心坐标
f_x = f_y = f_w = f_h = None
while True:
# 读取相机画面
ret, frame = cap.read()
# 画面水平翻转
flipped_frame = cv2.flip(frame, 1) # 0时为垂直翻转
# 获取画面的宽度和高度
height, width, channels = flipped_frame.shape
# 计算中心点的坐标
center_x = int(width / 2)
center_y = int(height / 2)
# 正方形的边长
length = 120
# 左上角的xy坐标
left_up_x = int(center_x - (length / 2)) # 因为原点在正方形的中心，处于一半的位置，所以左上角的x坐标需要边长除以2
left_up_y = int(center_y - (length / 2))
# 画红色的正方形
cv2.rectangle(flipped_frame, (left_up_x, left_up_y), (left_up_x + length, left_up_y + length), (0, 0, 255),
3)
# 将帧转换为灰度图像
gray = cv2.cvtColor(flipped_frame, cv2.COLOR_BGR2GRAY)
# 进行人脸检测
# 检测设置，将图片放大1.1倍（一般设1.1倍，看效果而定）
# 重复检测的次数为6次（检测次数越多，速度越慢，检测也越严格，准确率可能有所提升）
# 最小的检测框为100*100的正方形，以上的会显示，以下的被屏蔽
faces = face_cascade.detectMultiScale(gray, scaleFactor=1.1, minNeighbors=6, minSize=(100, 100))
# 在帧中标记人脸
for (x, y, w, h) in faces:
f_x, f_y, f_w, f_h = x, y, w, h
cv2.rectangle(flipped_frame, (x, y), (x + w, y + h), (0, 255, 0), 3)
# 计算人脸正方形的中心点坐标
if len(faces) > 0: # 判断人脸的数量是否>0
f_z_x = int(f_x + (f_w / 2))
f_z_y = int(f_y + (f_h / 2))
# print("脸部中心x:", f_z_x, "脸部中心y:", f_z_y)
else:
f_z_x, f_z_y = center_x, center_y # 没有人脸时就默认人脸位置居中，防止后续使用舵机云台时乱动
# print("脸部中心x:", f_z_x, "脸部中心y:", f_z_y)
# 计算人脸中心与画面中心相差的坐标
difference_value_x = f_z_x - center_x
difference_value_y = f_z_y - center_y
# print("x坐标偏移量为：", difference_value_x, "y坐标偏移量为：", difference_value_y)
# 检测人脸位置
self.determine_face_position(difference_value_x, difference_value_y)
# 显示相机画面
cv2.imshow("Face following...", flipped_frame)
# 检测按键，如果按下q键则退出循环
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# 释放摄像头资源
cap.release()
# 关闭窗口
cv2.destroyAllWindows()
face = Follow_the_face()
face.Inittalize() # 初始化舵机位置
face.turn_on_face_detection() # 开启人脸检测
五、给电脑注入灵魂（代码）
import cv2
def detect_faces():
# 加载预训练的人脸检测器
face_cascade = cv2.CascadeClassifier(cv2.data.haarcascades + 'haarcascade_frontalface_default.xml')
# 打开默认摄像头
cap = cv2.VideoCapture(0)
r, f = cap.read()
# 获取画面的宽度和高度
h, w, ch = f.shape
print("输出画面的高度和宽度", h, w, ch)
center_x1 = int(w / 2)
center_y1 = int(h / 2)
print("输出中心坐标为：", center_x1, center_y1)
# 记录人脸中心坐标
f_x = None
f_y = None
f_w = None
f_h = None
while True:
# 读取帧
ret, frame = cap.read()
# 水平翻转帧
flipped_frame = cv2.flip(frame, 1) # 0时为垂直翻转
# 获取画面的宽度和高度
height, width, channels = flipped_frame.shape
# 计算中心点的坐标
center_x = int(width / 2)
center_y = int(height / 2)
# 在中心点画一个红色圆形
# radius = 20
# color = (0, 0, 255) # 红色
# thickness = 0 # 填充圆形
# cv2.circle(flipped_frame, (center_x, center_y), radius, color, thickness)
# 画个红色正方形
length = 120
# 左上
left_up_x = int(center_x - (length / 2)) # 因为原点在正方形的中心，处于一半的位置，所以左上角的x坐标需要边长除以2
left_up_y = int(center_y - (length / 2))
# # 左下
# left_down_x = int(center_x - (length / 2))
# left_down_y = int(center_y + (length / 2))
# # 右上
# right_up_x = int(center_x + (length / 2))
# right_up_y = int(center_y - (length / 2))
# # 右下
# right_down_x = int(center_x + (length / 2))
# right_down_y = int(center_y + (length / 2))
cv2.rectangle(flipped_frame, (left_up_x, left_up_y), (left_up_x + length, left_up_y + length), (0, 0, 255), 3)
# 将帧转换为灰度图像
gray = cv2.cvtColor(flipped_frame, cv2.COLOR_BGR2GRAY)
# 进行人脸检测
faces = face_cascade.detectMultiScale(gray, scaleFactor=1.1, minNeighbors=5, minSize=(30, 30))
# 在帧中标记人脸
for (x, y, w, h) in faces:
f_x, f_y, f_w, f_h = x, y, w, h
cv2.rectangle(flipped_frame, (x, y), (x + w, y + h), (0, 255, 0), 3)
# 计算人脸正方形的中心点坐标
if len(faces) > 0: # 判断人脸的数量是否>0
f_z_x = int(f_x + (f_w / 2))
f_z_y = int(f_y + (f_h / 2))
# print("脸部中心x:", f_z_x, "脸部中心y:", f_z_y)
else:
f_z_x, f_z_y = center_x, center_y # 没有人脸时就默认人脸位置居中，防止后续使用舵机云台时乱动
# print("脸部中心x:", f_z_x, "脸部中心y:", f_z_y)
# 计算人脸中心与画面中心相差的坐标
difference_value_x = f_z_x - center_x
difference_value_y = f_z_y - center_y
# print("x坐标偏移量为：", difference_value_x, "y坐标偏移量为：", difference_value_y)
range_value = 10 # 设置可接受偏移的范围
if -range_value <= difference_value_x <= range_value and -range_value <= difference_value_y <= range_value:
print("x坐标、y坐标都不用移动")
elif difference_value_x > range_value:
print("摄像头向右转动一点")
if difference_value_y > range_value:
print("摄像头向下转动一点")
if difference_value_y < -range_value:
print("摄像头向上转动一点")
elif difference_value_x < -range_value:
print("摄像头向左转动一点")
if difference_value_y > range_value:
print("摄像头向下转动一点")
if difference_value_y < -range_value:
print("摄像头向上转动一点")
elif difference_value_y > range_value:
print("摄像头向下转动一点")
elif difference_value_y < -range_value:
print("摄像头向上转动一点")
# 显示带有人脸标记的帧
cv2.imshow("Faces Detected", flipped_frame)
# 按下 'q' 键退出循环
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# 释放摄像头并关闭窗口
cap.release()
cv2.destroyAllWindows()
# 调用实时人脸检测函数
detect_faces()
总结
目前的云台呢确实是可以跟随人脸转动的，但以下问题点：
- 实时显示相机中的画面并进行人脸检测时，有些流畅度不足，因为本人是显示彩色画面的，若改成灰度图的话应该会更快一点；
- 检测人脸的准确度不足，这里使用的是OpenCV自带的人脸检测模型，存在较多的误判，这样会干扰云台的自动人脸定位，所以可以的话，自己训练一个更优的人脸检测模型来使用。或者是调整重复检测次数之类的增加一些精准度吧，但重复检测次数增加得越多，画面越卡顿；
- 目前的程序只能针对画面中有一个人脸的时候进行跟随，若出现多个人脸，云台就会乱套了，各位大佬可以根据自身需求优化或利用这个问题；
- 使用模拟信号的SG90s舵机会存在一些精度的问题，表现的样子就是定位有些不准，然后出现抖动。可以的话，将模拟舵机换成数字舵机，当然价格会贵上一倍。数字舵机的精准度会高上许多，之后本人就打算买回来试试；
该文章若对你有帮助，不妨点个赞噢~
2024.01.15写
有写得不好、写得不对的地方还请各位指出，谢谢！