目录
零、简介
在这一部分,我们将探讨如何实现一个基于树莓派的安保巡逻机器人,使用增量式PID控制算法进行二维云台追踪。该系统利用摄像头捕捉周围环境,并对检测到的人脸进行实时追踪。以下是实现该功能的详细代码及其说明。
在这个基于 PID 控制的机器人系统中,有两个核心部分协同工作:PID 控制线程和视频帧的实时处理。这两部分各司其职,分别通过单独的线程运行,协同完成对人脸的追踪。以下是这两部分的详细解释。
一、PCA9685 控制板
PCA9685 是一款来自 NXP(恩智浦)的 16 通道 PWM 控制芯片,广泛用于控制多个舵机和 LED 等设备。它可以通过 I²C 接口与控制板(如树莓派)连接,提供稳定的脉宽调制信号。
相比直接使用树莓派的 GPIO 控制,PCA9685 产生的 PWM 信号更为稳定且更安全,有助于提升舵机的响应速度和控制精度。
pip3 install Adafruit_PCA9685
安装PCA9685驱动库:Adafruit PCA9685库
如有需要对树莓派进行IIC线拓展:GitHub - JJSlabbert/Raspberry_PI_i2C_conficts: Use multiple i2c devices with same i2c address. No extra hardware required.
二、人脸检测与追踪
结合 Mediapipe
进行人脸检测,并采用增量式 PID 控制算法对二维云台进行调节,以实现对人脸的精准追踪。通过 PWM 控制舵机的角度变化,机器人可以实时调整摄像头方向,实现自动锁定和跟踪目标人脸。
人脸检测相关内容查看这个:基于树莓派的安保巡逻机器人--(一、快速人脸录入与精准人脸识别)-CSDN博客
主要代码结构及其核心模块
import threading
import mediapipe as mp
import cv2 as cv
import time
import smbus
import Adafruit_PCA9685
PCA9685 控制板设置 PWM 频率为 60Hz,通过 set_pwmss
函数来控制舵机的角度。舵机的初始位置 X_P
和 Y_P
分别为 425 和 1200。
pwm = Adafruit_PCA9685.PCA9685(busnum=4)
pwm.set_pwm_freq(60)
def set_pwmss(chann, duty):
dutyss = 4095 / 4095 * duty
pwm.set_pwm(chann, 0, int(dutyss))
三、增量式 PID 控制
我们在 FaceDetector
类中实现 PID 控制。先检测人脸位置,计算误差,再根据增量式 PID 公式计算舵机需要的角度变化。在 FaceDetector
类中,我们使用 增量式 PID 控制 算法根据人脸的偏移位置来调整舵机的角度,从而使摄像头实时跟随人脸的移动。增量式 PID 控制是一种常用的控制算法,它通过当前位置和目标位置的误差来逐步调整控制量,使得目标接近设定值。
增量式 PID 控制主要由三个部分组成:比例 (P)、积分 (I) 和 微分 (D)。
公式如下:
ΔU=Kp×Δe+Ki×e+Kd×(e−elast)
其中:
Δe :当前误差(目标位置与当前位置的偏差)。
Kp 、Ki 、Kd :比例、积分和微分系数。
e :累计的误差。
elaste :上一次误差。
四、在项目中使用增量式 PID 控制步骤
1、定义目标位置和误差
我们希望摄像头的画面中心能够对准人脸的位置,因此摄像头画面中心 (320, 240) 被定义为目标点。而人脸的中心点 (cx, cy) 则被实时计算出,与目标点的距离就是当前的误差(偏移量)
2、计算当前误差 thisError_x
和 thisError_y
cx, cy = x + w // 2, y + h // 2 # 人脸矩形框的中心点
thisError_x = cx - 320 # X 轴上的偏差,目标中心位置为 320
thisError_y = 240 - cy # Y 轴上的偏差,目标中心位置为 240
这里,thisError_x
和 thisError_y
表示人脸相对于画面中心在 X 和 Y 轴上的偏移值,这步是最重要的,只要理解好这一步,这个跟踪功能也就基本完成了。
3、计算增量式 PID 输出
根据增量式 PID 公式,我们计算舵机在 X 和 Y 轴上的调整量。
pwm_x = thisError_x * 5 + 1 * (thisError_x - lastError_x)
pwm_y = thisError_y * 5 + 1 * (thisError_y - lastError_y)
P(比例)控制:thisError_x * 5
和 thisError_y * 5
。这部分直接与当前误差成比例,用于迅速将人脸移动到目标位置。比例系数为 5,增大该系数可以让舵机响应更快。
D(微分)控制:(thisError_x - lastError_x)
和 (thisError_y - lastError_y)
。微分控制对误差的变化率进行调节,避免快速移动或振荡。系数为 1。
积分控制在此例中并未使用,因为在云台追踪中,积分控制通常容易导致系统震荡,而 P 和 D 足以让摄像头平稳跟随人脸
4、将 PID 调节后的值应用到舵机位置 X_P
和 Y_P
上:
XP = pwm_x / 100
YP = pwm_y / 100
X_P = X_P + int(XP)
Y_P = Y_P + int(YP)
这里的 XP
和 YP
是调节后的舵机角度变化。由于舵机的控制精度较高,我们将变化量控制在较小范围内。
XP = pwm_x / 100
YP = pwm_y / 100
X_P = X_P + int(XP)
Y_P = Y_P + int(YP)
5、限制舵机的活动范围
防止舵机的角度超出有效的移动范围(防止撞杆或造成系统损坏),设置上下限:
if X_P > 1200: X_P = 1200
if X_P < 210: X_P = 210
if Y_P > 1500: Y_P = 1500
if Y_P < 1000: Y_P = 1000
代码示例:增量式 PID 控制的完整流程
def fancy_draw(self, frame, bbox, l=30, t=10):
global X_P, Y_P, lastError_x, lastError_y
x, y, w, h = bbox
cx, cy = x + w // 2, y + h // 2 # 计算人脸的中心点
thisError_x = cx - 320
thisError_y = 240 - cy
# 计算增量式 PID 调整量
pwm_x = thisError_x * 5 + 1 * (thisError_x - lastError_x)
pwm_y = thisError_y * 5 + 1 * (thisError_y - lastError_y)
# 更新误差
lastError_x = thisError_x
lastError_y = thisError_y
# 将 PID 调整量转换为舵机位置
XP = pwm_x / 100
YP = pwm_y / 100
X_P += int(XP)
Y_P += int(YP)
# 限制舵机活动范围
if X_P > 1200: X_P = 1200
if X_P < 210: X_P = 210
if Y_P > 1500: Y_P = 1500
if Y_P < 1000: Y_P = 1000
return frame
增量式 PID 控制算法在此实现了对二维云台的稳定控制,通过检测人脸位置的偏移量,实时调整摄像头角度,使其保持在人脸的正前方。而
五、启动 PID 控制线程
PID 控制算法的调节过程是通过一个独立的线程 servo_control
函数持续执行的。这样设计可以让 PID 控制在后台实时运作,而不会干扰到主线程中的图像处理和人脸检测工作。
通过单独线程运行 servo_control
函数,程序可以在后台同时进行摄像头图像处理和 PWM 输出控制。这避免了因为图像处理开销过大而影响舵机控制的实时性。
# 在主程序中启动 PID 控制线程
thread_servo_control = threading.Thread(target=servo_control)
thread_servo_control.start()
打开摄像头,并对每帧图像进行人脸检测。当检测到人脸时,PID 控制算法计算出摄像头需要调整的角度,从而实时跟随人脸移动。
附上完整代码:
import threading
import mediapipe as mp
import cv2 as cv
import time
import os
import smbus
import Adafruit_PCA9685
pwm = Adafruit_PCA9685.PCA9685(busnum=1)
pwm.set_pwm_freq(60)
def set_pwmss(chann,duty):
dutyss=4095 /4095*duty
pwm.set_pwm(chann, 0,int(dutyss))
# 舵机初始位置
X_P = 425
Y_P = 1200
lastError_x = 0
lastError_y = 0
# 人脸检测类
class FaceDetector:
def __init__(self, min_detection_confidence=0.5):
self.mpFaceDetection = mp.solutions.face_detection
self.face_detection = self.mpFaceDetection.FaceDetection(min_detection_confidence=min_detection_confidence)
def find_faces(self, frame):
img_RGB = cv.cvtColor(frame, cv.COLOR_BGR2RGB)
results = self.face_detection.process(img_RGB)
bboxs = []
if results.detections:
for id, detection in enumerate(results.detections):
bboxC = detection.location_data.relative_bounding_box
ih, iw, ic = frame.shape
bbox = int(bboxC.xmin * iw), int(bboxC.ymin * ih), \
int(bboxC.width * iw), int(bboxC.height * ih)
bboxs.append([id, bbox, detection.score])
frame = self.fancy_draw(frame, bbox)
return frame, bboxs
def fancy_draw(self, frame, bbox, l=30, t=10):
global X_P, Y_P, lastError_x, lastError_y
x, y, w, h = bbox
x1, y1 = x + w, y + h
cv.rectangle(frame, (x, y), (x + w, y + h), (255, 0, 255), 1)# 绘制矩形框
cx, cy = x + w // 2, y + h // 2 # 计算中心点
thisError_x = cx - 320
thisError_y = 240 -cy
pwm_x = thisError_x * 5 + 1 * (thisError_x - lastError_x)
pwm_y = thisError_y * 5 + 1 * (thisError_y - lastError_y)
lastError_x = thisError_x
lastError_y = thisError_y
XP = pwm_x / 100
YP = pwm_y / 100
X_P = X_P + int(XP)
Y_P = Y_P + int(YP)
max_movement = 10
if X_P > 1200:
X_P = 1200
if X_P < 210:
X_P = 210
if Y_P > 1500:
Y_P = 1500
if Y_P < 1000:
Y_P = 1000
return frame
def servo_control():
global X_P, Y_P
Kp = 0.45
while True:
set_pwmss(11, min(1023, max(0, X_P * Kp)))
set_pwmss(12, min(1023, max(0, Y_P *+ Kp)))
time.sleep(0.065)
if __name__ == '__main__':
capture = cv.VideoCapture(-1)
capture.set(cv.CAP_PROP_FRAME_WIDTH, 640)
capture.set(cv.CAP_PROP_FRAME_HEIGHT, 480)
print("capture get FPS : ", capture.get(cv.CAP_PROP_FPS))
pTime, cTime = 0, 0
face_detector = FaceDetector(0.75)
thread_servo_control = threading.Thread(target=servo_control)
thread_servo_control.start()
while capture.isOpened():
ret, frame = capture.read()
frame = cv.flip(frame, 1)
frame, bboxs = face_detector.find_faces(frame)
if cv.waitKey(1) & 0xFF == ord('q'):
break
cTime = time.time()
fps = 1 / (cTime - pTime)
pTime = cTime
text = "FPS : " + str(int(fps))
cv.putText(frame, text, (20, 30), cv.FONT_HERSHEY_SIMPLEX, 0.9, (0, 0, 255), 1)
cv.imshow('frame', frame)
capture.release()
cv.destroyAllWindows()
#############################################################
#########下面的代码为舵机调试代码##############################
# import sys
# import termios
# import tty
# def get_key():
# """获取单个键的输入"""
# fd = sys.stdin.fileno()
# old_settings = termios.tcgetattr(fd)
# try:
# tty.setraw(fd)
# ch = sys.stdin.read(1)
# finally:
# termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
# return ch
# def update_values():
# global X_P, Y_P
# Kp = 0.45w
# print("按 'q' 增加 X_P, 按 'p' 增加 Y_P, 按 'esc' 退出。")
# print("按 'w 减小 X_P, 按 's' 减小 Y_P, 按 'esc' 退出。")
# while True:
# key = get_key()
# if key == 'q':
# X_P += 1
# print(f"X_P: {X_P}")
# set_pwmss(11, min(1023, max(0, int(X_P * Kp))))
# elif key == 'w':
# X_P -= 1
# print(f"X_P: {X_P}")
# set_pwmss(11, min(1023, max(0, int(X_P * Kp))))
# elif key == 'a':
# Y_P += 1
# print(f"Y_P: {Y_P}")
# set_pwmss(12, min(1023, max(0, int(Y_P * Kp))))
# elif key == 's':
# Y_P -= 1
# print(f"Y_P: {Y_P}")
# set_pwmss(12, min(1023, max(0, int(Y_P * Kp))))
# elif key == '\x1b': # Esc 键
# print("程序退出")
# break
# if __name__ == '__main__':
# update_values()
六、对陌生人识别并跟踪拍摄
而进行对陌生人识别并跟踪拍摄只需将人脸检测更改成人脸识别即可,具体人脸识别代码可参考:
基于树莓派的安保巡逻机器人--(一、快速人脸录入与精准人脸识别)-CSDN博客
具体跟踪操作与原理是一样的故直接上代码:
import threading
import mediapipe as mp
import cv2 as cv
import time
import numpy as np
import smbus
import Adafruit_PCA9685
pwm = Adafruit_PCA9685.PCA9685(busnum=1)
pwm.set_pwm_freq(60)
def set_pwmss(chann,duty):
dutyss=4095 /4095*duty
pwm.set_pwm(chann, 0,int(dutyss))
# 设置舵机的初始位置和误差
X_P = 425
Y_P = 1200
lastError_x = 0
lastError_y = 0
# 初始化人脸识别器
recognizer = cv.face.LBPHFaceRecognizer_create()
recognizer.read('trainer/trainer.yml')
cascadePath = "haarcascade_frontalface_default.xml"
faceCascade = cv.CascadeClassifier(cascadePath)
font = cv.FONT_HERSHEY_SIMPLEX
# 初始化摄像头
capture = cv.VideoCapture(-1)
capture.set(3, 640)
capture.set(4, 480)
minW = 0.1 * capture.get(3)
minH = 0.1 * capture.get(4)
class FaceDetector:
def __init__(self, min_detection_confidence=0.5):
self.mpFaceDetection = mp.solutions.face_detection
self.face_detection = self.mpFaceDetection.FaceDetection(min_detection_confidence=min_detection_confidence)
def find_faces(self, frame):
global tracking_enabled
gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
faces = faceCascade.detectMultiScale(gray, scaleFactor=1.2, minNeighbors=5, minSize=(int(minW), int(minH)))
bboxs = []
tracking_enabled = False
for (x, y, w, h) in faces:
bbox = x, y, w, h
bboxs.append([0, bbox, 0])
id, confidence = recognizer.predict(gray[y:y + h, x:x + w])
# 判断是否是陌生人
if confidence >= 70: # 如果是陌生人
id_text = "Unknown"
tracking_enabled = True # 开启追踪
else:
id_text = f"ID={id}" # 如果不是陌生人,显示ID信息
frame = self.fancy_draw(frame, bbox)
cv.putText(frame, id_text, (x + 5, y - 5), font, 1, (255, 255, 255), 2)
return frame, bboxs
def fancy_draw(self, frame, bbox, l=30, t=10):
global X_P, Y_P, lastError_x, lastError_y,tracking_enabled
x, y, w, h = bbox
x1, y1 = x + w, y + h
cv.rectangle(frame, (x, y), (x + w, y + h), (255, 0, 255), 1)# 绘制矩形框
if tracking_enabled :
cx, cy = x + w // 2, y + h // 2 # 计算中心点
thisError_x = cx - 320
thisError_y = 240 -cy
pwm_x = thisError_x * 5 + 1 * (thisError_x - lastError_x)
pwm_y = thisError_y * 5 + 1 * (thisError_y - lastError_y)
lastError_x = thisError_x
lastError_y = thisError_y
XP = pwm_x / 100
YP = pwm_y / 100
X_P = X_P + int(XP)
Y_P = Y_P + int(YP)
if X_P > 1200:
X_P = 1200
if X_P < 210:
X_P = 210
if Y_P > 1500:
Y_P = 1500
if Y_P < 1000:
Y_P = 1000
return frame
def servo_control():
global X_P, Y_P
Kp = 0.45
while True:
set_pwmss(11, min(1023, max(0, X_P * Kp)))
set_pwmss(12, min(1023, max(0, Y_P *+ Kp)))
time.sleep(0.065)
if __name__ == '__main__':
capture.set(6, cv.VideoWriter.fourcc('M', 'J', 'P', 'G'))
capture.set(cv.CAP_PROP_FRAME_WIDTH, 640)
capture.set(cv.CAP_PROP_FRAME_HEIGHT, 480)
print("Capture FPS: ", capture.get(cv.CAP_PROP_FPS))
pTime = 0
face_detector = FaceDetector(0.75)
# 启动舵机控制线程
thread_servo_control = threading.Thread(target=servo_control)
thread_servo_control.start()
while capture.isOpened():
ret, frame = capture.read()
frame = cv.flip(frame, 1)
frame, bboxs = face_detector.find_faces(frame)
if cv.waitKey(1) & 0xFF == ord('q'): break
cTime = time.time()
fps = 1 / (cTime - pTime)
pTime = cTime
cv.putText(frame, f"FPS: {int(fps)}", (20, 30), cv.FONT_HERSHEY_SIMPLEX, 0.9, (0, 0, 255), 1)
cv.imshow('frame', frame)
capture.release()
cv.destroyAllWindows()