树莓派应用--AI项目实战篇来啦-12.OpenCV摄像头云台物体追踪

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.建立电路

下图显示了电路图连接：

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）。

        上图显示了几何示例。
        想想平移摄像头将如何操作。请注意，摄像头实际图像和显示的图像正好处于一个镜像状态，这意味着如果您将对象移到“您的左侧”，则在与摄像机相对时，它将在屏幕上以“您的右侧”移动。