1. 介绍
本项目主要是实现OpenCV识别物体，找出中心位置，根据中心位置的偏离情况来修正二维云台，让物体的中心位置始终处于图像的中心位置，要保证追踪的流畅性，这里引入了 PID算法来抑制云台的抖动。
2. PID算法
在实际使用舵机追踪小球时，由于小球的中心位置在图像中会不停的抖动，导致舵机云台在追踪的效果中，需要进行误差的处理，追踪的原理其实就是移动舵机云台的x方向和y方向，始终保持小车处于图像的中心位置，在实际测试中，结合树莓派的性能，这个采用的窗口大小为（320,240）像素的窗口大小，那么中心位置则为（160,120）位置上，误差的处理我们这里需要使用到PID算法。
2.1 什么是PID 控制器
常见的闭环控制算法是所谓的PID及比例-积分-微分控制器。
PID通常用于自动化，使得机械装置可以快速且准确地达到最佳值（由反馈传感器读取）。
它们用于制造，自动化过程控制，机器人等。
PID 控制器计算误差项（所需设定点和传感器读数之间的差值）并且具有补偿误差的目
标。
PID 计算输出一个值，该值用作“过程”的输入（机电过程，而不是计算机科学/软件工程师类型认为的“计算机过程”）。
传感器输出称为“过程变量”，并作为等式的输入。在整个反馈回路中，捕获定时并将其输入到等式中。
图：比例积分微分（PID）控制回路将用于我们的每个平移和倾斜过程
注意输出如何循环回输入。还要注意如何计算和求和比例，积分和微分值。
该图可以用等式形式写成：
P（比例）：如果当前误差很大，输出将成比例增大，以引起显着的校正。
I（积分）：误差的历史值随时间积分。进行微弱的校正以减少误差。如果错误被消除，该项将不会增大。
D（微分）：这个微分项预示着未来。实际上，它是一种阻尼方法。如果P或|将导致值过冲（即舵机转过了一个物体或方向盘转得太多），D将在到达输出之前抑制效果。
简单来说：
P-比例，直接调节幅度
I-积分，修正累计误差
D-微分，阻尼（预测未来）
本节将以追踪黄色物体为例进行实验，通过滑条可以随时调节HSV的各种分量的值。
2.2总线舵机零点位置校准
通过 python 程序来调节舵机位置，使得初始位置力中间位置，也就是说，两个舵机的角度都为90度的时候，为零点状态，这样子舵机的左右角度和倾斜角度是等分的。
import cv2
import numpy as np
from adafruit_servokit import ServoKit
import time
kit = ServoKit(channels=16)
# 舵机调零
pan = 90
tilt = 90
＃初始化位置
kit.servo[0].angle=pan
kit.servo[1].angle=tilt“Pan”舵机将“水平”移动摄像头（“方位角”），而“Tilt” 舵机将其“垂直”移动（仰角）。
在我们的开发过程中，我们不会进行“极限” 操作，而只能使用30至150度的 “平移/倾斜”机制。此范围足以用于摄像头。
当平移/倾斜角度旋转到90度位置后，而实际上舵机云台不在中间位置，这个时候，我们就需要重新安装一下舵机云台，使舵机云台的零点位置处于正确位置上即可。
3.建立电路
| 树莓派 | T型转接板 | PCA9685 舵机驱动模块 |
| SCL | SCL | SCL |
| SDA | SDA | SDA |
| 3.3V | 3.3V | VCC |
| 5V | 5V | V+ |
| GND | GND | GND |
| 舵机 | PCA9685 舵机驱动模块 |
| 水平方向舵机(pan) | PWM0 |
| 倾斜舵机(tilt) | PWM1 |
下图显示了电路图连接：
4.源程序代码
# 载入必要的库
import cv2
import numpy as np
from adafruit_servokit import ServoKit
import time
kit = ServoKit(channels=16)
# 舵机调零
pan = 90
tilt = 90
# 初始化位置
kit.servo[0].angle=pan
kit.servo[1].angle=tilt
def bgr8_to_jpeg(value, quality=75):
return bytes(cv2.imencode('.jpg', value)[1])
import traitlets
import ipywidgets.widgets as widgets
from IPython.display import display
FGmaskComp_img = widgets.Image(format='jpeg', width=320, height=240)
frame_img = widgets.Image(format='jpeg', width=320, height=240)
dispaly_img = widgets.HBox([FGmaskComp_img,frame_img])
display(dispaly_img)
# 线程函数操作库
import threading # 线程
import ctypes
import inspect
# 线程结束代码
def _async_raise(tid, exctype):
tid = ctypes.c_long(tid)
if not inspect.isclass(exctype):
exctype = type(exctype)
res = ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, ctypes.py_object(exctype))
if res == 0:
raise ValueError("invalid thread id")
elif res != 1:
ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, None)
raise SystemError("PyThreadState_SetAsyncExc failed")
def stop_thread(thread):
_async_raise(thread.ident, SystemExit)
import libcamera
from picamera2 import Picamera2
picamera = Picamera2()
config = picamera.create_preview_configuration(main={"format": 'RGB888', "size": (320, 240)},
raw={"format": "SRGGB12", "size": (1920, 1080)})
config["transform"] = libcamera.Transform(hflip=0, vflip=1)
picamera.configure(config)
picamera.start()
width = 320
height = 240
hueLower = widgets.IntSlider(min=3,max=179,step=1,description='hueLower：',value=3)
hueUpper = widgets.IntSlider(min=23,max=179,step=1,description='hueUpper：',value=23)
hue2Lower = widgets.IntSlider(min=3,max=179,step=1,description='hue2Lower：',value=3)
hue2Upper = widgets.IntSlider(min=0,max=179,step=1,description='hue2Upper：',value=0)
satLow = widgets.IntSlider(min=100,max=255,step=1,description='satLow：',value=100)
satHigh = widgets.IntSlider(min=255,max=255,step=1,description='satHigh：',value=255)
valLow = widgets.IntSlider(min=100,max=255,step=1,description='valLow：',value=100)
valHigh = widgets.IntSlider(min=255,max=255,step=1,description='valHigh：',value=255)
slider_img=widgets.VBox([
widgets.HBox([hueLower,hueUpper]),
widgets.HBox([hue2Lower,hue2Upper]),
widgets.HBox([satLow,satHigh]),
widgets.HBox([valLow,valHigh])
])
display(slider_img)
def Video_display():
global pan
global tilt
while True:
frame = picamera.capture_array()
hsv=cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)
hueLow=hueLower.value
hueUp=hueUpper.value
hue2Low=hue2Lower.value
hue2Up=hue2Upper.value
Ls= satLow.value
Us = satHigh.value
Lv=valLow.value
Uv=valHigh.value
l_b=np.array([hueLow,Ls,Lv])
u_b=np.array([hueUp,Us,Uv])
l_b2=np.array([hue2Low,Ls,Lv])
u_b2=np.array([hue2Up,Us,Uv])
FGmask=cv2.inRange(hsv,l_b,u_b)
FGmask2=cv2.inRange(hsv,l_b2,u_b2)
FGmaskComp=cv2.add(FGmask,FGmask2)
FGmaskComp_img.value = bgr8_to_jpeg(FGmaskComp)
contours,_=cv2.findContours(FGmaskComp,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
contours=sorted(contours,key=lambda x:cv2.contourArea(x),reverse=True)
for cnt in contours:
area=cv2.contourArea(cnt)
(x,y,w,h)=cv2.boundingRect(cnt)
if area>=50:
cv2.rectangle(frame,(x,y),(x+w,y+h),(255,0,0),3)
objX=x+w/2
objY=y+h/2
errorPan=objX-width/2
errorTilt=objY-height/2
if abs(errorPan)>15:
pan=pan-errorPan/75
if abs(errorTilt)>15:
tilt=tilt-errorTilt/75
if pan>180:
pan=180
print("Pan Out of Range")
if pan<0:
pan=0
print("Pan Out of Range")
if tilt>180:
tilt=180
print("Tilt Out of Range")
if tilt<0:
tilt=0
print("Tilt Out of Range")
kit.servo[0].angle=180-pan
kit.servo[1].angle=180-tilt
break
frame_img.value = bgr8_to_jpeg(frame)
cam.release()
t = threading.Thread(target=Video_display)
t.setDaemon(True)
t.start()
# 结束线程
stop_thread(t)可以通过滑块调节具体的HSV值 ：
基于（x,y）坐标，使用kit.servo[0].angle=pan和kit. servo[1].angle=tilt函数生成舵机位置命令。例如，假设y位置“50”，这意味着我们的对象几乎在屏幕的顶部，可以将其转换为“摄像头视线”为“低”（假设倾斜角度为120度）因此，我们必须“减小”倾斜角度（假设为100度），这样摄像头的视线将“向上”并且对象在屏幕上将“向下”（y会增加到 190）。
上图显示了几何示例。
想想平移摄像头将如何操作。请注意，摄像头实际图像和显示的图像正好处于一个镜像状态，这意味着如果您将对象移到“您的左侧”，则在与摄像机相对时，它将在屏幕上以“您的右侧”移动。