树莓派4B_OpenCv学习笔记19：OpenCV舵机云台物体追踪

今日继续学习树莓派4B 4G：（Raspberry Pi，简称RPi或RasPi）

本人所用树莓派4B 装载的系统与版本如下:

版本可用命令 (lsb_release -a) 查询:

Opencv 版本是4.5.1：

Python 版本3.7.3：

今日学习：OpenCV舵机云台物体追踪代码是创乐博的，本文只作解释分析......

前置学习条件如下：

IIC驱动_PCA9685(16路舵机驱动模块) 文章网址如下：

树莓派学习笔记18:IIC驱动_PCA9685(16路舵机驱动模块)-CSDN博客

Python多线程编程文章网址如下：

树莓派4B学习笔记14：Python多线程编程_线程间的同步通信_(锁‘threading.Lock’)_树莓派多线程-CSDN博客

文章提供测试代码讲解，整体代码贴出、测试效果图

目录

测试效果视频：

代码贴出：

例程测试步骤：

例程测试步骤相关工程下载：

网上查阅资料网址：

测试效果视频：

OpenCV舵机云台物体追踪

代码贴出：

# -*- coding: utf-8 -*-

from __future__ import division

import time
import cv2
import numpy as np
import Adafruit_PCA9685
import RPi.GPIO as GPIO
import threading


#初始化PCA9685和舵机
servo_pwm = Adafruit_PCA9685.PCA9685()  # 实例化舵机云台

# 设置舵机初始值，可以根据自己的要求调试
servo_pwm.set_pwm_freq(60)  # 设置频率为60HZ
servo_pwm.set_pwm(5,0,325)  # 底座舵机
servo_pwm.set_pwm(4,0,325)  # 倾斜舵机
time.sleep(1)

#初始化摄像头并设置阙值
usb_cap = cv2.VideoCapture(0)

# 设置球体追踪的HSV值，上下限值
ball_yellow_lower=np.array([171,161,186])
ball_yellow_upper=np.array([178,188,255])

# 设置显示的分辨率，设置为320×240 px
usb_cap.set(3, 320)
usb_cap.set(4, 240)

#舵机云台的每个自由度需要4个变量
pid_thisError_x=500       #当前误差值
pid_lastError_x=100       #上一次误差值
pid_thisError_y=500
pid_lastError_y=100

pid_x=0
pid_y=0

# 舵机的转动角度
pid_Y_P = 325
pid_X_P = 325           #转动角度
pid_flag=0


# initialize LED GPIO
redLed = 18    # LED灯
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup(redLed, GPIO.OUT)

# 舵机旋转
def Robot_servo(X_P,Y_P):
    servo_pwm.set_pwm(5,0,650-pid_X_P)
    servo_pwm.set_pwm(4,0,650-pid_Y_P)


# 关闭LED
GPIO.output(redLed, GPIO.LOW)
ledOn = False


# loop over the frames from the video stream
while True:
    ret,frame = usb_cap.read()
    
    #高斯模糊处理
    frame=cv2.GaussianBlur(frame,(5,5),0)
    hsv= cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)
    #ROI及找到形态学找到小球进行处理
    mask=cv2.inRange(hsv,ball_yellow_lower,ball_yellow_upper) # 掩膜处理
    mask=cv2.erode(mask,None,iterations=2)
    mask=cv2.dilate(mask,None,iterations=2)
    mask=cv2.GaussianBlur(mask,(3,3),0)
    res=cv2.bitwise_and(frame,frame,mask=mask)
    cnts=cv2.findContours(mask.copy(),cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)[-2]  #发现小球
    
    # only proceed if at least one contour was found
    if len(cnts) > 0:
        
        cap_cnt=max(cnts,key=cv2.contourArea)
        (pid_x,pid_y),radius=cv2.minEnclosingCircle(cap_cnt)
        cv2.circle(frame,(int(pid_x),int(pid_y)),int(radius),(255,0,255),2)       
	    # 误差值处理
        pid_thisError_x=pid_x-160
        pid_thisError_y=pid_y-120

		#PID控制参数
        pwm_x = pid_thisError_x*3+1*(pid_thisError_x-pid_lastError_x)
        pwm_y = pid_thisError_y*3+1*(pid_thisError_y-pid_lastError_y)

		#迭代误差值操作
        pid_lastError_x = pid_thisError_x
        pid_lastError_y = pid_thisError_y
        
        pid_XP=pwm_x/100
        pid_YP=pwm_y/100

        # pid_X_P pid_Y_P 为最终PID值
        pid_X_P=pid_X_P+int(pid_XP)
        pid_Y_P=pid_Y_P+int(pid_YP)
        
        GPIO.output(redLed, GPIO.HIGH)
        
        #限值舵机在一定的范围之内
        if pid_X_P>650:
            pid_X_P=650
        if pid_X_P<0:
            pid_X_P=0
        if pid_Y_P>650:
            pid_Y_P=650
        if pid_X_P<0:
            pid_Y_p=0

    # 如果没有检测到球，关闭LED灯
    else:
        GPIO.output(redLed, GPIO.LOW)
              

    servo_tid=threading.Thread(target=Robot_servo,args=(pid_X_P,pid_Y_P))  # 多线程
    servo_tid.setDaemon(True)
    servo_tid.start()   # 开启线程
    
    cv2.imshow("MAKEROBO Robot", frame)  # 显示图像
    # 等待键盘输入，如果按下'q'则退出循环
    key = cv2.waitKey(1) & 0xFF
    if key == ord('q'):
        break    

# do a bit of cleanup
print("\n [INFO] Exiting Program and cleanup stuff \n")
GPIO.cleanup()
cv2.destroyAllWindows()
usb_cap.stop()

例程测试步骤：

用到的程序会统一打包在文后下载

1、先拍摄一张照片，使用PS软件获取其BGR色域：78 54 208：

2、再转入HSV色域：（这是大致色域）

{165,100,100}

{185,255,255}

3、放入HSV程序进行微调：得到比较稳定色域：

{145，161，189}

{179，196，246}

4、将HSV色域填入程序：