树莓派4B+sg90舵机实现红色小球追踪

引言：

        这次我利用树莓派4B做一个对红色小球追踪实验的简单版，后面可能还会有终版。下面我会一一讲解一下我的具体思路以及实现流程，所涉及到的硬件或者软件的知识在这里面我可能不会谈得很多，如果大家有不理解或者我说的有错误的地方也可以互相交流。

硬件环境：

        树莓派4B。

        16路PCA9685PWM波驱动板。

        一个USB摄像头。

        两个sg90舵机，一个充当X轴，一个充当Y轴。

软件环境：

        编译软件MobaXterm

        下载OpenCV环境到树莓派

        安装PCA9685驱动包到树莓派（注意这里下载的时候一定要用pip3下载，将环境下载的与python环境同目录，不然会因为找不到相关文件而报错。具体细节也可以看一下其它博主的文章）

实现过程：

        一、图像处理

                1.通过摄像头，利用OpenCV实时获取视频流并能够对视频流进行处理，得到我们想要的画面与数据。

                2.由于我是要追踪红色的小球，所以可以利用HSV颜色空间利用确定红色阈值对视频进行二值化处理，将红色部分作为白色也就是像素为255非红色部分作为黑色也就是像素为0。

                    HSV个颜色阈值给各位放下面了：                

                3.二值化完毕后可以遍历整个视频流（整个视频流可以看做是一个640*480的二维矩阵）通过堆像素的索引值得到全部的小球像素点，再利用他们在视频流中的位置折算成坐标。最后利用质心算法求出小球的质心，确定小球的中心坐标记为x,y。

代码如下：

import cv2
import numpy as np
from time import sleep

cap = cv2.VideoCapture(0)
while cap.isOpened():
    ret,frame = cap.read()
    img_hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    # 确定红色阈值
    low_rea = np.array([156, 43, 46])
    upper_red = np.array([180, 255, 255])
    img_mask = cv2.inRange(img_hsv, low_rea, upper_red)   # 建立掩膜
    height,width = img_mask.shape[:2]   # 得到视频流矩阵宽度与高度
    sum_x = 0
    sum_y = 0
    count = 0
    # 求质心，非常简单且暴力的一个算法
    for y in range(width):
        for x in range(height):
            if img_mask[x,y] > 0:
                sum_x += x
                sum_y += y
                count += 1
    cX,cY = 0,0
    # 找出质心坐标
    if count > 0:
        cX = int(sum_x/count)
        cY = int(sum_y/count)
    # 在原图中画一个圆圈出质心
    cv2.circle(frame,(cX,cY),100,(20,100,150),2)
    # cv2.imshow("img_mask",img_mask)
    cv2.imshow("fram",frame)
    # time.sleep(1)
    if cv2.waitKey(5) & 0x00FF == ord('q'):
        cv2.imwrite('tt', frame)
        break
cap.release()
cv2.destroyAllWindows()

        二、控制舵机跟随小球的移动而转动

                1.由于以及求出了小球的质心坐标，并且分别将横纵坐标记为了x,y。所以我们可以将两个坐标轴对应的值，分别折算成角度，分别给两个舵机，一个控制x轴的方向，一个控制y轴的方向。那么如何将视频流的值折算成角度呢？？？？个人认为，因为OpenCV的视频流为640*480的矩阵，并且sg90舵机的角度是从0~180，所以我们可以利用比例来求角度。比如：X轴对应的是看即为640，所以它对应的角度为（x/640）*180。同理可得y轴对应的角度为y/480*180。这样便也就实现了坐标到角度的转换。

                2.将角度得到后，由于sg90是用的PWM波来控制转动角度，所以还需要利用公式将角度计算成PWM波。

                代码如下：

# -*- coding: utf-8 -*-
# Import the PCA9685 module.
from __future__ import division
import time
import Adafruit_PCA9685


def set_servo_angle(channel, angle):  # 使用时将角度传入，调用该函数
    date = int(4096 * ((angle * 11) + 500) / 20000)  # 将角度计算成pwm
    pwm.set_pwm(channel, 0, date)
    #print(f"pca{f}")
    return date

# PCA9685的通道
channel1 = 0
channel2 = 1

pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(50)

        三、优化

                1.由于一个Opencv视频流矩阵是640*480的为矩阵。这也就意味着，在对视频实时处理的时候，直接通过粗暴的去便利所有像素值去求质心它的时间复杂度为O（N*M）本身数据量并不是很大，但是由于是一直在处理，加上还要将数据通过USB实时传输，本身还是比较慢的。如果直接用上面的代码的话，运行你就会发现视频流很卡，舵机的相应也不是很及时。对此我们要对其进行优化，我有想到了两张优化方案。

                第一种：在对视频流的数据进行处理的时候，可以考虑多进程，同时开两个进程对视频的数据流进行处理，得到质心坐标。当然，这样的方法对于这样的小项目而言本身肯定是可以的，但是进程本身也是对CPU资源的占用，所以一般情况下个人认为还是要从其他方面下手。

                第二种：优化算法。由于小球有颜色，有形状。本身我们也只需要二值化过后有颜色的像素，对此我们可以对他进行轮廓检测+边缘处理直接得到小球在视频流中的像素，直接遍历他们这样时间复杂度会大大减小，如果小球不大，或者小球距离摄像头很远，那么时间复杂度将会小于O（N）。就算是最坏的情况也会比之前的O（N*M）小很多。

如图所示（边上不规则的地方是我的手指，轮廓提取和颜色识别相对于还是比较准确的）：

效果展示：         

        OpenCV视频效果：   

横向转动如下：

        

竖直转动如下：

两者并不是分开转动的，是一起转动的可以达到跟踪的目的。

最后缺陷分析：

         1.摄像头没有固定在舵机上面，固定在舵机上面摄像头会随着舵机的转动而转动，这样环境一改变，摄像头的抖动就会便严重。对此个人认为可以加入PID算法，提高精确度，减少抖动。

        2.由于是HSV颜色空间+边缘化轮廓处理，所以环境的颜色以及物体的形状对它还是有一定的干扰，对此也可以进行优化，可以考虑训练一个模型。

整合后代码：

        主文件：

import cv2
import numpy as np
import time
import Position_pid as Pid
import PCA as pca

cap = cv2.VideoCapture(0)
# PCA9685的通道
channel1 = 0
channel2 = 1
pca.set_servo_angle(channel1, 0)
pca.set_servo_angle(channel2, 0)
cx = [0,0]
cy = [0,0]
while cap.isOpened():
    ret, frame = cap.read()
    # 转化为颜色空间根据颜色空间的颜色阈值进行二值化
    img_hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    # 确定红色阈值
    low_rea = np.array([150, 43, 46])
    upper_red = np.array([180, 255, 255])
    # 创建掩膜
    img_mask = cv2.inRange(img_hsv, low_rea, upper_red)
    contours,hierarchy = cv2.findContours(img_mask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_NONE)  # 边缘检测，RETR_EXTERNAL:提取物体边缘轮廓。CHAIN_APPROX_NONE:存储所有轮廓点
    """
    contours_data = [[[int(x) for x in point[0]] for point in contour] for contour in contours]
    sum_x = 0
    sum_y = 0
    count = 0
    index = -1
    for i in range(len(contours)):
        n = len(contours[i])
        if index < n < len(contours):
            index = n
        for j in range(len(contours[i])):
            for x,y in contours[i][j]:
                sum_x += x
                sum_y += y
                count += 1
    cX, cY = 0, 0
    if count > 0:
        cX = int(sum_x / count)
        cY = int(sum_y / count)
    """
    area = []
    for contour in contours:
        a = cv2.contourArea(contour, True)
        area.append(abs(a))
    cX, cY = 0, 0
    index = -1
    if len(area) > 0:  # 如果提取到轮廓，并计算出面积
        if max(area) > 10:  # 设置一个阈值，避免干扰信息
            index = area.index(max(area))  # 获取最大面积的索引
            M = cv2.moments(contours[index])  # 最大轮廓 求矩
            cX = int(M['m10'] / M['m00'])  # 求轮廓的 x 坐标
            cY = int(M['m01'] / M['m00'])  # 求轮廓的 y 坐标
    cv2.circle(frame, (cX, cY), 40, (20, 100, 150), 2)
    x = (cX/640)*180
    y = (cY/480)*180
    cx[1],cx[0] = x,cx[1]
    cy[1],cy[0] = y,cy[1]
    cv2.circle(frame, (cX, cY), 40, (20, 100, 150), 2)
    #pid = Pid.PIDController(0.5,0.1,0.2,setpoint=640)
    #derct = pid.update((cX//640)*(1/18)+2.5)
    pca.set_servo_angle(channel2, y)
    pca.set_servo_angle(channel1, x)
    """
    if abs(cx[0]-cx[1]) >= 5:
      f = pca.set_servo_angle(channel1, x)
      print(f"1:{f}")
    if abs(cy[0]-cy[1]) >= 5:
      f = pca.set_servo_angle(channel2, y)
      print(f"2:{f}")
    """
    dst = cv2.drawContours(frame, contours, contourIdx=index, color=(0, 255, 255), thickness=-3, lineType=cv2.LINE_AA)
    if cv2.waitKey(5) & 0x00FF == ord('q'):
        break
    #cv2.imshow("frame",frame)
    cv2.imshow("mask",img_mask)
    #time.sleep(0.5)
cap.release()
cv2.destroyAllWindows()

        PWM驱动板文件：

# -*- coding: utf-8 -*-
# Import the PCA9685 module.
from __future__ import division
import time
import Adafruit_PCA9685


def set_servo_angle(channel, angle):  # ÊäÈë½Ç¶Èת»»³É12^¾«¶ÈµÄÊýÖµ
    date = int(4096 * ((angle * 11) + 500) / 20000)
    pwm.set_pwm(channel, 0, date)
    #print(f"pca{f}")
    return date

# PCA9685的通道
channel1 = 0
channel2 = 1

pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(50)