使用树莓派+舵机+pca9685+usb摄像头制作二维云台,图像追踪
为什么使用pca9685驱动舵机,而不使用树莓派自带的引脚驱动舵机呢?
因为树莓派无法产生稳定的pwm波,容易造成舵机的抖动
我使用的是树莓派+ubuntu系统+pca9685
1.首先在ubuntu系统中安装相关依赖
sudo apt install whiptail parted lua5.1 alsa-utils psmisc
2.下载安装包
wget http://mirrors.ustc.edu.cn/archive.raspberrypi.org/debian/pool/main/r/raspi-config/raspi-config_20200601_all.deb
3.安装rasp-config
sudo dpkg -i raspi-config_20200601_all.deb
4.启用rasp-config
sudo raspi-config
5.打开IIC
6.安装PCA9685的库
下载依赖库
sudo apt-get install git build-essential python-dev
从github下载源码(这一步如果下载失败,可以直接复制链接到GitHub下载,然后移到工作空间下)
git clone https://github.com/adafruit/Adafruit_Python_PCA9685.git
安装库
cd Adafruit_Python_PCA9685
sudo python setup.py install
测试是否接好线
sudo apt-get install i2c-tools
sudo i2cdetect -y 1
测试舵机
python3 ~/Adafruit_Python_PCA9685/examples/simpletest.py
然后进行下面的图像追踪
import cv2
import time
import numpy as np
from threading import Thread
from pid import PID
import Adafruit_PCA9685
# 初始化PCA9685
pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(60)
# 舵机的极限位置
servo_min = 150 # 最小脉冲长度
servo_max = 600 # 最大脉冲长度
# 初始化伺服电机
panChannel = 0
tiltChannel = 1
panAngle = 90 # 中心位置
tiltAngle = 90 # 中心位置
def set_servo_angle(channel, angle):
pulse = (servo_min + (servo_max - servo_min) * angle / 180)
pwm.set_pwm(channel, 0, int(pulse))
set_servo_angle(panChannel, panAngle)
set_servo_angle(tiltChannel, tiltAngle)
# 定义视频流类
class VideoStream:
def __init__(self, src=0):
self.stream = cv2.VideoCapture(src)
self.stream.set(cv2.CAP_PROP_FRAME_WIDTH, 320)
self.stream.set(cv2.CAP_PROP_FRAME_HEIGHT, 240)
self.stream.set(cv2.CAP_PROP_FPS, 30)
self.grabbed, self.frame = self.stream.read()
self.stopped = False
def start(self):
Thread(target=self.update, args=()).start()
return self
def update(self):
while not self.stopped:
self.grabbed, self.frame = self.stream.read()
def read(self):
return self.frame
def stop(self):
self.stopped = True
self.stream.release()
# 启动视频流
vs = VideoStream(src=0).start()
# 设置 PID 控制器参数
pan_pid = PID(0.05, 0.01, 0.001)
tilt_pid = PID(0.05, 0.01, 0.001)
pan_pid.initialize()
tilt_pid.initialize()
# 计算帧率
fps = 0
pos = (10, 20)
font = cv2.FONT_HERSHEY_SIMPLEX
height = 0.5
weight = 1
myColor = (0, 0, 255)
def nothing(x):
pass
cv2.namedWindow('PID Tuner')
cv2.createTrackbar('Pan Kp', 'PID Tuner', int(pan_pid.kP * 100), 100, nothing)
cv2.createTrackbar('Pan Ki', 'PID Tuner', int(pan_pid.kI * 100), 100, nothing)
cv2.createTrackbar('Pan Kd', 'PID Tuner', int(pan_pid.kD * 100), 100, nothing)
cv2.createTrackbar('Tilt Kp', 'PID Tuner', int(tilt_pid.kP * 100), 100, nothing)
cv2.createTrackbar('Tilt Ki', 'PID Tuner', int(tilt_pid.kI * 100), 100, nothing)
cv2.createTrackbar('Tilt Kd', 'PID Tuner', int(tilt_pid.kD * 100), 100, nothing)
last_update = time.time()
update_interval = 0.1 # 控制更新频率
try:
while True:
tStart = time.time()
frame = vs.read()
if frame is None:
print("Failed to grab frame")
break
frame = cv2.flip(frame, 1)
frameHSV = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
cv2.putText(frame, str(int(fps)) + ' FPS', pos, font, height, myColor, weight)
lowerBound = np.array([0, 147, 114])
upperBound = np.array([88, 255, 255])
myMask = cv2.inRange(frameHSV, lowerBound, upperBound)
contours, _ = cv2.findContours(myMask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if len(contours) > 0:
contours = sorted(contours, key=lambda x: cv2.contourArea(x), reverse=True)
contour = contours[0]
x, y, w, h = cv2.boundingRect(contour)
cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 0, 255), 3)
# 计算误差
errorX = (x + w / 2) - (320 / 2)
errorY = (240 / 2) - (y + h / 2) # 反转误差方向
if time.time() - last_update > update_interval:
# 获取PID参数并更新
pan_pid.kP = cv2.getTrackbarPos('Pan Kp', 'PID Tuner') / 100
pan_pid.kI = cv2.getTrackbarPos('Pan Ki', 'PID Tuner') / 100
pan_pid.kD = cv2.getTrackbarPos('Pan Kd', 'PID Tuner') / 100
tilt_pid.kP = cv2.getTrackbarPos('Tilt Kp', 'PID Tuner') / 100
tilt_pid.kI = cv2.getTrackbarPos('Tilt Ki', 'PID Tuner') / 100
tilt_pid.kD = cv2.getTrackbarPos('Tilt Kd', 'PID Tuner') / 100
panAdjustment = pan_pid.update(errorX, sleep=0)
tiltAdjustment = tilt_pid.update(errorY, sleep=0)
panAngle += panAdjustment
tiltAngle += tiltAdjustment
# 限制角度范围
panAngle = max(0, min(panAngle, 180))
tiltAngle = max(0, min(tiltAngle, 180))
# 设置伺服电机角度
set_servo_angle(panChannel, panAngle)
set_servo_angle(tiltChannel, tiltAngle)
last_update = time.time()
# 仅在图形环境中显示图像窗口
try:
cv2.imshow('Camera', frame)
cv2.imshow('Mask', myMask)
except cv2.error as e:
print(f"OpenCV error: {e}")
if cv2.waitKey(1) == ord('q'):
break
tEnd = time.time()
loopTime = tEnd - tStart
fps = .9 * fps + .1 * (1 / loopTime)
finally:
vs.stop()
cv2.destroyAllWindows()
在同一文件目录下,创建pid.py文件
# pid.py
# -*- coding: UTF-8 -*-
# 调用必需库
import time
class PID:
def __init__(self, kP=1, kI=0, kD=0):
# 初始化参数
self.kP = kP
self.kI = kI
self.kD = kD
def initialize(self):
# 初始化当前时间和上一次计算的时间
self.currTime = time.time()
self.prevTime = self.currTime
# 初始化上一次计算的误差
self.prevError = 0
# 初始化误差的比例值,积分值和微分值
self.cP = 0
self.cI = 0
self.cD = 0
def update(self, error, sleep=0.2):
# 暂停
time.sleep(sleep)
# 获取当前时间并计算时间差
self.currTime = time.time()
deltaTime = self.currTime - self.prevTime
# 计算误差的微分
deltaError = error - self.prevError
# 比例项
self.cP = error
# 积分项
self.cI += error * deltaTime
# 微分项
self.cD = (deltaError / deltaTime) if deltaTime > 0 else 0
# 保存时间和误差为下次更新做准备
self.prevTime = self.currTime
self.prevError = error
# 返回输出值
return sum([
self.kP * self.cP,
self.kI * self.cI,
self.kD * self.cD])
def set_Kp(self, kP):
self.kP = kP
def set_Ki(self, kI):
self.kI = kI
def set_Kd(self, kD):
self.kD = kD