树莓派，opencv，Picamera2利用舵机云台追踪特定颜色对象
一、需要准备的硬件
- Raspiberry 4b
- 两个SG90 180度舵机（注意舵机的角度，最好是180度且带限位的，切勿选360度舵机）
- 二自由度舵机云台（如下图）
- Raspiberry CSI 摄像头
组装后的效果：
二、项目目标
追踪特定颜色的物体：
当物体移动时，摄像头通过控制两个伺服电机（分别是偏航和俯仰）把该物体放到视界的中心位置，我在这里追踪的是一支红色的铅笔。
三、具体步骤
3.1 获得被追踪对象的颜色参数
-
提前准备一张图片（如下图），可以直接用树莓派的CSI摄像头拍摄并保存，具体方法可以在我之前的文章里找到
-
利用下面的代码并通过调整滑块（Trackbar）获得红色铅笔的HSV颜色参数，为接下来的颜色追踪做准备
***color_detection.py***
import cv2
path='test_full.jpg'
cv2.namedWindow("TrackBar")
def nothing(x):
pass
#创建滑块控件
cv2.createTrackbar("Hue Min","TrackBar",0,179,nothing)
cv2.createTrackbar("Hue Max","TrackBar",179,179,nothing)
cv2.createTrackbar("Sat Min","TrackBar",0,255,nothing)
cv2.createTrackbar("Sat Max","TrackBar",255,255,nothing)
cv2.createTrackbar("Val Min","TrackBar",0,255,nothing)
cv2.createTrackbar("Val Max","TrackBar",255,255,nothing)
while True:
#读取目标图片
image=cv2.imread(path)
image=cv2.resize(image,(640,480))
imgHSV=cv2.cvtColor(image,cv2.COLOR_BGR2HSV)
hueLow=cv2.getTrackbarPos("Hue Min","TrackBar")
hueHigh=cv2.getTrackbarPos("Hue Max","TrackBar")
satLow=cv2.getTrackbarPos("Sat Min","TrackBar")
satHigh=cv2.getTrackbarPos("Sat Max","TrackBar")
valLow=cv2.getTrackbarPos("Val Min","TrackBar")
valHigh=cv2.getTrackbarPos("Val Max","TrackBar")
print(hueLow,hueHigh,satLow,satHigh,valLow,valHigh)
#创建掩膜
mask=cv2.inRange(imgHSV,(hueLow,satLow,valLow),(hueHigh,satHigh,valHigh))
image=cv2.bitwise_and(image,image,mask=mask)
#显示图像
cv2.imshow('Origial',image)
cv2.imshow('HSV',imgHSV)
#按q键退出
if cv2.waitKey(1)==ord('q'):
break
cv2.destroyAllWindows()
- 运行color_detection.py，并调整滑块（TrackBar)如下图，当然你的被追踪物体的颜色不同，参数也必然不同。
这时你会发现，红色铅笔被显示出来，其它部分被掩膜遮挡，记下Hue Min, Hui Max, Sat Min, Sat Max, Val Min, Val Max这六个数值在接下来的代码中会用到。
3.2 目标追踪代码
- 输入color_detection.py里得到的六个参数到相应位置，注释里已经注明。
***color_tracking.py***
import cv2
from picamera2 import Picamera2
import time
import numpy as np
from servo import Servo
picam2 = Picamera2()
#偏航伺服电机连接上GPIO19脚，俯仰伺服电机信号线连接到GPIO16脚上
pan=Servo(pin=19)
tilt=Servo(pin=16)
panAngle=0
tiltAngle=0
pan.set_angle(panAngle)
tilt.set_angle(tiltAngle)
#初始化pi camera
dispW=1280
dispH=720
picam2.preview_configuration.main.size = (dispW,dispH)
picam2.preview_configuration.main.format = "RGB888"
picam2.preview_configuration.controls.FrameRate=30
picam2.preview_configuration.align()
picam2.configure("preview")
picam2.start()
fps=0
pos=(30,60)
font=cv2.FONT_HERSHEY_SIMPLEX
height=1.5
weight=3
myColor=(0,0,255)
def nothing(x):
pass
cv2.namedWindow('myTracker')
#输入color_detection.py里得到的六个参数到xxx位置，比如cv2.createTrackbar('Hue Low','myTracker',xxx,179,nothing)
cv2.createTrackbar('Hue Low','myTracker',56,179,nothing)
cv2.createTrackbar('Hue High','myTracker',179,179,nothing)
cv2.createTrackbar('Sat Low','myTracker',165,255,nothing)
cv2.createTrackbar('Sat High','myTracker',255,255,nothing)
cv2.createTrackbar('Val Low','myTracker',77,255,nothing)
cv2.createTrackbar('Val High','myTracker',255,255,nothing)
while True:
tStart=time.time()
#获取取摄像头图片
frame= picam2.capture_array()
frame=cv2.flip(frame,1)
frameHSV=cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)
cv2.putText(frame,str(int(fps))+' FPS',pos,font,height,myColor,weight)
hueLow=cv2.getTrackbarPos('Hue Low','myTracker')
satLow=cv2.getTrackbarPos('Sat Low','myTracker')
valLow=cv2.getTrackbarPos('Val Low','myTracker')
hueHigh=cv2.getTrackbarPos('Hue High','myTracker')
satHigh=cv2.getTrackbarPos('Sat High','myTracker')
valHigh=cv2.getTrackbarPos('Val High','myTracker')
lowerBound=np.array([hueLow,satLow,valLow])
upperBound=np.array([hueHigh,satHigh,valHigh])
myMask=cv2.inRange(frameHSV,lowerBound,upperBound)
myMaskSmall=cv2.resize(myMask,(int(dispW/2),int(dispH/2)))
myObject=cv2.bitwise_and(frame,frame, mask=myMask)
myObjectSmall=cv2.resize(myObject,(int(dispW/2),int(dispH/2)))
contours,junk=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)
#cv2.drawContours(frame,contours,-1,(255,0,0),3)
contour=contours[0]
x,y,w,h=cv2.boundingRect(contour)
cv2.rectangle(frame,(x,y),(x+w,y+h),(0,0,255),3)
#偏航电机纠偏X轴方向上的偏差，大于30度，偏航角度减小，小于-30度，偏航角度增加
errorX=dispW/2-(x+w/2)
if errorX>30:
panAngle=panAngle-1
if panAngle<-90:
panAngle=-90
pan.set_angle(panAngle)
if errorX<-30:
panAngle=panAngle+1
if panAngle>90:
panAngle=90
pan.set_angle(panAngle)
#俯仰电机纠偏Y轴方向上的偏差，大于30度，俯仰角度减小，小于-30度，俯仰角度增加
errorY=dispH/2-(y+h/2)
if errorY>30:
tiltAngle=tiltAngle-1
if tiltAngle<-90:
tiltAngle=-90
tilt.set_angle(tiltAngle)
if errorY<-30:
tiltAngle=tiltAngle+1
if tiltAngle>90:
tiltAngle=90
tilt.set_angle(tiltAngle)
cv2.imshow('Camera',frame)
cv2.imshow('Mask',myMaskSmall)
cv2.imshow('My Object',myObjectSmall)
#按q键退出
if cv2.waitKey(1)==ord('q'):
pan.stop()
tilt.stop()
picam2.stop()
break
tEnd=time.time()
loopTime=tEnd-tStart
fps=.9*fps + .1*(1/loopTime)
cv2.destroyAllWindows()
- 上述代码中的from servo import Servo导入servo，这个库是没有的，我们要手动创建这个库，在object_tracking.py所在的目录下新建servo.py文件，复制下面的代码到文件中
#!/usr/bin/env python3
import pigpio
from time import sleep
# Start the pigpiod daemon
import subprocess
result = None
status = 1
for x in range(3):
p = subprocess.Popen('sudo pigpiod', shell=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
result = p.stdout.read().decode('utf-8')
status = p.poll()
if status == 0:
break
sleep(0.2)
if status != 0:
print(status, result)
'''
> Use the DMA PWM of the pigpio library to drive the servo
> Map the servo angle (0 ~ 180 degree) to (-90 ~ 90 degree)
'''
class Servo():
MAX_PW = 1250 # 0.5/20*100
MIN_PW = 250 # 2.5/20*100
_freq = 50 # 50 Hz, 20ms
def __init__(self, pin, min_angle=-90, max_angle=90):
self.pi = pigpio.pi()
self.pin = pin
self.pi.set_PWM_frequency(self.pin, self._freq)
self.pi.set_PWM_range(self.pin, 10000)
self.angle = 0
self.max_angle = max_angle
self.min_angle = min_angle
self.pi.set_PWM_dutycycle(self.pin, 0)
def set_angle(self, angle):
if angle > self.max_angle:
angle = self.max_angle
elif angle < self.min_angle:
angle = self.min_angle
self.angle = angle
duty = self.map(angle, -90, 90, 250, 1250)
self.pi.set_PWM_dutycycle(self.pin, duty)
def get_angle(self):
return self.angle
def stop(self):
self.pi.set_PWM_dutycycle(self.pin, 0)
self.pi.stop()
# will be called automatically when the object is deleted
# def __del__(self):
# pass
def map(self, x, in_min, in_max, out_min, out_max):
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
if __name__ =='__main__':
from vilib import Vilib
# Vilib.camera_start(vflip=True,hflip=True)
# Vilib.display(local=True,web=True)
pan = Servo(pin=13, max_angle=90, min_angle=-90)
tilt = Servo(pin=12, max_angle=30, min_angle=-90)
panAngle = 0
tiltAngle = 0
pan.set_angle(panAngle)
tilt.set_angle(tiltAngle)
sleep(1)
while True:
for angle in range(0, 90, 1):
pan.set_angle(angle)
tilt.set_angle(angle)
sleep(.01)
sleep(.5)
for angle in range(90, -90, -1):
pan.set_angle(angle)
tilt.set_angle(angle)
sleep(.01)
sleep(.5)
for angle in range(-90, 0, 1):
pan.set_angle(angle)
tilt.set_angle(angle)
sleep(.01)
sleep(.5)
- 运行object_tracking.py，移动红色铅笔，摄像头就会自动追踪该对象
