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 timekit = 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 timekit = ServoKit(channels=16)# 舵机调零
pan = 90
tilt = 90
# 初始化位置
kit.servo[0].angle=pan
kit.servo[1].angle=tiltdef bgr8_to_jpeg(value, quality=75):return bytes(cv2.imencode('.jpg', value)[1])import traitlets
import ipywidgets.widgets as widgets
from IPython.display import displayFGmaskComp_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 Picamera2picamera = 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 = 240hueLower = 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 panglobal tiltwhile True: frame = picamera.capture_array()hsv=cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)hueLow=hueLower.value hueUp=hueUpper.valuehue2Low=hue2Lower.valuehue2Up=hue2Upper.valueLs= satLow.valueUs = satHigh.valueLv=valLow.valueUv=valHigh.valuel_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/2objY=y+h/2errorPan=objX-width/2errorTilt=objY-height/2 if abs(errorPan)>15:pan=pan-errorPan/75 if abs(errorTilt)>15:tilt=tilt-errorTilt/75if pan>180:pan=180print("Pan Out of Range") if pan<0:pan=0print("Pan Out of Range") if tilt>180:tilt=180print("Tilt Out of Range") if tilt<0:tilt=0print("Tilt Out of Range")kit.servo[0].angle=180-pankit.servo[1].angle=180-tiltbreak 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)。
上图显示了几何示例。
想想平移摄像头将如何操作。请注意,摄像头实际图像和显示的图像正好处于一个镜像状态,这意味着如果您将对象移到“您的左侧”,则在与摄像机相对时,它将在屏幕上以“您的右侧”移动。