树莓派,opencv,Picamera2利用舵机云台追踪特定颜色对象(PID控制)

一、需要准备的硬件

  1. Raspiberry 4b
  2. 两个SG90 180度舵机(注意舵机的角度,最好是180度且带限位的,切勿选360度舵机)
  3. 二自由度舵机云台(如下图)
  4. Raspiberry CSI 摄像头
    组装后的效果:

二、项目目标

追踪特定颜色的物体:

当物体移动时,摄像头通过控制两个伺服电机(分别是偏航和俯仰)把该物体放到视界的中心位置,我在这里追踪的是一支黄色的铅笔。

三、具体步骤

3.1 获得被追踪对象的颜色参数

  1. 提前准备一张图片(如下图),可以直接用树莓派的CSI摄像头拍摄并保存,具体方法可以在我之前的文章里找到
  1. 利用下面的代码并通过调整滑块(Trackbar)获得红色铅笔的HSV颜色参数,为接下来的颜色追踪做准备
python 复制代码
import cv2
import json
path='crop_img.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)
    data={
        "hueLow":hueLow,
        "hueHigh":hueHigh,
        "satLow":satLow,
        "satHigh":satHigh,
        "valLow":valLow,
        "valHigh":valHigh,
    }
    mask_json=json.dumps(data)
    #按q键保存并退出
    if cv2.waitKey(1)==ord('q'):
        #将设置的参数保存到mask.json文件中
        with open('mask.json','w') as f:
            f.write(mask_json)
        break
cv2.destroyAllWindows() 
  1. 运行color_detection.py,并调整滑块(TrackBar)如下图,当然你的被追踪物体的颜色不同,参数也必然不同。

  2. 这时你会发现,红色铅笔被显示出来,其它部分被掩膜遮挡,当你在frame窗口按下"q"键后,会自动生成mask.json文件保存相应参数设置

3.2 目标追踪代码

  1. 新建color_tracking_pid.py文件,一级(pan)舵机的信号脚接在GPIO的19脚,二级(tilt)舵机的信号脚接在GPIO的16脚,在运行时可以通过调整main函数里的PID参数,代码如下:
python 复制代码
# -*- coding: UTF-8 -*-
# 调用必需库
# color_tracking_pid.py
from multiprocessing import Manager, Process
from pid import PID
from colorcenter import Colorcenter
from servo import Servo
import time
import signal
import sys
import cv2
from picamera2 import Picamera2
import json


# 定义舵机
pan = Servo(pin=19)
tilt = Servo(pin=16)

# 定义图像尺寸
dispW = 1280
dispH = 720
# 读取掩模配置文件
with open('mask.json') as f:
    mask = json.load(f)


def nothing(x):
    pass

# 键盘终止函数


def signal_handler(sig, frame):
    # 输出状态信息
    print("[INFO] You pressed `ctrl + c`! Exiting...")

    # 关闭舵机
    pan.stop()
    tilt.stop()

    # 退出
    sys.exit()


def color_center(objX, objY, centerX, centerY):
    # ctrl+c退出进程
    signal.signal(signal.SIGINT, signal_handler)

    # 启动视频流并缓冲
    print("[INFO] waiting for camera to warm up...")
    cv2.startWindowThread()
    picam2 = Picamera2()
    preview_config = picam2.create_preview_configuration(main={"size": (dispW, dispH),"format":"RGB888"})
    picam2.configure(preview_config)
    picam2.start()
    time.sleep(2.0)

    # 初始化色块探测器
    obj = Colorcenter(mask['hueLow'], mask['satLow'], mask['valLow'],
                      mask['hueHigh'], mask['satHigh'], mask['valHigh'])

    # 进入循环
    while True:

        # 从视频流抓取图像并旋转
        frame = picam2.capture_array()
        frame = cv2.flip(frame, 1)

        # 找到图像中心
        (H, W) = frame.shape[:2]
        centerX.value = W // 2
        centerY.value = H // 2

        # 画出图像中心点
        cv2.circle(frame, (centerX.value, centerY.value), 5, (0, 0, 255), -1)

        # 找到色块
        objectLoc = obj.update(frame, (centerX.value, centerY.value))
        ((objX.value, objY.value), rect) = objectLoc

        # 绘制色块外界矩形
        if rect is not None:
            (x, y, w, h) = rect
            cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 0, 255), 3)
            fX = int(x + (w / 2.0))
            fY = int(y + (h / 2.0))
            cv2.circle(frame, (fX, fY), 5, (0, 0, 255), -1)
            cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 0, 255), 3)
            # 在色块中心和视窗中心画的条连线
            cv2.line(frame, (centerX.value, centerY.value),
                     (fX, fY), (0, 255, 0), 2)
            # 显示图像
        cv2.imshow("Pan-Tilt Face Tracking", frame)
        cv2.waitKey(1)


def pid_process(output, p, i, d, objCoord, centerCoord):
    # ctrl+c退出进程
    signal.signal(signal.SIGINT, signal_handler)

    # 创建一个PID类的对象并初始化
    p = PID(p.value, i.value, d.value)
    p.initialize()

    # 进入循环
    while True:
        # 计算误差
        error = centerCoord.value - objCoord.value

        # 更新输出值,当error小于50时,误差设为0,以避免云台不停运行。
        if abs(error) < 50:
            error = 0
        output.value = p.update(error)


def set_servos(panAngle, tiltAngle):
    # ctrl+c退出进程
    signal.signal(signal.SIGINT, signal_handler)

    # 进入循环
    while True:
        # 偏角变号
        yaw = -1 * panAngle.value
        pitch = -1 * tiltAngle.value

        # 设置舵机角度。
        pan.set_angle(yaw)
        tilt.set_angle(pitch)


# 启动主程序
if __name__ == "__main__":

    # 启动多进程变量管理
    with Manager() as manager:  # 相当于manager=Manager(),with as 语句操作上下文管理器(context manager),它能够帮助我们自动分配并且释放资源。
        # 舵机角度置零
        pan.set_angle(0)
        tilt.set_angle(0)

        # 为图像中心坐标赋初值
        centerX = manager.Value("i", 0)  # "i"即为整型integer
        centerY = manager.Value("i", 0)

        # 为人脸中心坐标赋初值
        objX = manager.Value("i", 0)
        objY = manager.Value("i", 0)

        # panAngle和tiltAngle分别是两个舵机的PID控制输出量
        panAngle = manager.Value("i", 0)
        tiltAngle = manager.Value("i", 0)

       # 设置一级舵机的PID参数
        panP = manager.Value("f", 0.015)  # "f"即为浮点型float
        panI = manager.Value("f", 0.01)
        panD = manager.Value("f", 0.0008)

        # 设置二级舵机的PID参数
        tiltP = manager.Value("f", 0.025)
        tiltI = manager.Value("f", 0.01)
        tiltD = manager.Value("f", 0.008)

        # 创建4个独立进程
        # 1. objectCenter  - 探测人脸
        # 2. panning       - 对一级舵机进行PID控制,控制偏航角
        # 3. tilting       - 对二级舵机进行PID控制,控制俯仰角
        # 4. setServos     - 根据PID控制的输出驱动舵机

        processObjectCenter = Process(
            target=color_center, args=(objX, objY, centerX, centerY))
        processPanning = Process(target=pid_process, args=(
            panAngle, panP, panI, panD, objX, centerX))
        processTilting = Process(target=pid_process, args=(
            tiltAngle, tiltP, tiltI, tiltD, objY, centerY))
        processSetServos = Process(
            target=set_servos, args=(panAngle, tiltAngle))

        # 开启4个进程
        processObjectCenter.start()
        processPanning.start()
        processTilting.start()
        processSetServos.start()

        # 添加4个进程
        processObjectCenter.join()
        processPanning.join()
        processTilting.join()
        processSetServos.join()
  1. 上述代码中的from servo import Servo导入servo,这个库是没有的,我们要手动创建这个库,在object_tracking.py所在的目录下新建servo.py文件,复制下面的代码到文件中
python 复制代码
#!/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)

. 在树莓派中运行该文件,运行前确认

  1. 运行color_tracking_pid.py,移动黄色铅笔,摄像头就会自动追踪该对象
相关推荐
子燕若水1 小时前
Unreal Engine 5中的AI知识
人工智能
极限实验室2 小时前
Coco AI 实战(一):Coco Server Linux 平台部署
人工智能
杨过过儿3 小时前
【学习笔记】4.1 什么是 LLM
人工智能
巴伦是只猫3 小时前
【机器学习笔记Ⅰ】13 正则化代价函数
人工智能·笔记·机器学习
大千AI助手3 小时前
DTW模版匹配:弹性对齐的时间序列相似度度量算法
人工智能·算法·机器学习·数据挖掘·模版匹配·dtw模版匹配
AI生存日记3 小时前
百度文心大模型 4.5 系列全面开源 英特尔同步支持端侧部署
人工智能·百度·开源·open ai大模型
LCG元4 小时前
自动驾驶感知模块的多模态数据融合:时序同步与空间对齐的框架解析
人工智能·机器学习·自动驾驶
why技术4 小时前
Stack Overflow,轰然倒下!
前端·人工智能·后端
彭祥.4 小时前
Jetson边缘计算主板:Ubuntu 环境配置 CUDA 与 cudNN 推理环境 + OpenCV 与 C++ 进行目标分类
c++·opencv·分类