所需硬體

• 智慧小車
• 樹莓派3B
• Pi Camera

環境配置

• 安裝opencv

sudo apt-get update
sudo apt-get upgrade
sudo apt-get install python-opencv

• 安裝PiCamera來控制攝像頭

sudo apt-get install python-pip 
sudo apt-get install python-dev 
sudo pip install picamera

程式碼

import cv2
import cv2.cv as cv
import cv
import picamera
import
 
 picamera.array
import math
import numpy as np
import zmq
import time
import serial, time, sys
from PIL import Image


# For OpenCV2 image display
IMAGE_WINDOW_NAME = 'YelloBarTracker'
CONTROL_WINDOW_NAME = 'Control'
MASK_WINDOW_NAME = 'Mask'

# For socket communication
port = '5556'
context = zmq.Context()
socket = context.socket(zmq.PUB)

# Setting the initial mask threshold 
 

# 根據環境除錯資料以便準確的捕捉小球
iLowH = 5
iHighH = 22

iLowS = 219
iHighS = 255

iLowV = 149
iHighV = 255

# 是否抓到球
getball = 0

# connect arduino
# 將arduino接到樹莓派上,輸入ls /dev檢視,我的是ttyUSB0
def connect_arduino():
    arduino = serial.Serial('/dev/ttyUSB0',9600,timeout=1)
    arduino.close()
    arduino.open()
    return arduino

# Require by cv2.createTrackbar. we have nothing to do with nothing method
 

def nothing(var):
    pass

def connect():
    print('Getting data from camera...')
    socket.bind('tcp://*:%s' % port)

# Create trackbars for easier adjustment of the HSV threshold  方便手動調節區間
def make_hsv_adjustment():
    cv2.namedWindow(CONTROL_WINDOW_NAME)
    cv2.createTrackbar('LowH', CONTROL_WINDOW_NAME, iLowH, 255, nothing); #Hue (0 - 179)
    cv2.createTrackbar('HighH', CONTROL_WINDOW_NAME, iHighH, 255, nothing);

    cv2.createTrackbar('LowS', CONTROL_WINDOW_NAME, iLowS, 255, nothing); #Saturation (0 - 255)
    cv2.createTrackbar('HighS', CONTROL_WINDOW_NAME, iHighS, 255, nothing);

    cv2.createTrackbar('LowV', CONTROL_WINDOW_NAME, iLowV, 255, nothing); #Value (0 - 255)
    cv2.createTrackbar('HighV', CONTROL_WINDOW_NAME, iHighV, 255, nothing);

def track(image):

    '''Accepts BGR image as Numpy array
       Returns: (x,y) coordinates of centroid if found
                (-1,-1) if no centroid was found
                None if user hit ESC
    '''

    # Blur the image to reduce noise
    blur = cv2.GaussianBlur(image, (5,5),0)

    # Convert BGR to HSV
    hsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV)

    # Get the treshold from the trackbars
    iLowH = cv2.getTrackbarPos('LowH', CONTROL_WINDOW_NAME)
    iHighH = cv2.getTrackbarPos('HighH', CONTROL_WINDOW_NAME)
    iLowS = cv2.getTrackbarPos('LowS', CONTROL_WINDOW_NAME)
    iHighS = cv2.getTrackbarPos('HighS', CONTROL_WINDOW_NAME)
    iLowV = cv2.getTrackbarPos('LowV', CONTROL_WINDOW_NAME)
    iHighV = cv2.getTrackbarPos('HighV', CONTROL_WINDOW_NAME)

    # Threshold the HSV image for only green colors
    lower_yellow = np.array([iLowH,iLowS,iLowV])
    upper_yellow = np.array([iHighH,iHighS,iHighV])

    # Threshold the HSV image to get only yellow colors
    mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
    cv2.imshow(MASK_WINDOW_NAME, mask)

    # Blur the mask
    bmask = cv2.GaussianBlur(mask, (5,5),0)

    # Take the moments to get the centroid
    moments = cv2.moments(bmask)
    m00 = moments['m00']
    centroid_x, centroid_y, radius = None, None, None
    if m00 != 0:
        centroid_x = int(moments['m10']/m00)
        centroid_y = int(moments['m01']/m00)
        radius = int(math.sqrt(m00 / 255 / 3.14159265358979323846))

    # Assume no centroid
    ball = (-1,-1, 0)

    # Use centroid if it exists
    if centroid_x != None and centroid_y != None and radius != None:

        ball = (centroid_x, centroid_y, radius) # 中心座標及半徑

        # Put red circle in at centroid in image
        cv2.circle(image, (centroid_x, centroid_y), 4, (255,0,0)) # center
        cv2.circle(image, (centroid_x, centroid_y), radius, (0,255,0))

    # Display full-color image
    cv2.imshow(IMAGE_WINDOW_NAME, image)

    # Force image display, setting centroid to None on ESC key input
    if cv2.waitKey(1) & 0xFF == 27:
        ball = None

    # Return coordinates of ball
    return ball

if __name__ == '__main__':
    connect()
    make_hsv_adjustment();
    arduino = connect_arduino();
    with picamera.PiCamera() as camera:
        with picamera.array.PiRGBArray(camera) as stream:
            camera.resolution = (320, 240)

            while True:
                camera.capture(stream, 'bgr', use_video_port=True)
                # stream.array now contains the image data in BGR order
                image = stream.array
                ball = track(image)
                if not ball:
                    break
                if cv2.waitKey(1) & 0xFF == 27:
                    break

                msg = '%d %d %d' % ball
                print(msg)
                socket.send(msg)

                # try
                # 未抓到球時發現距離較近,嘗試抓取 
                if ball[2] > 55 and getball == 0:
                  # 前進一步
                  arduino.write('1')
                  # 夾取
                  arduino.write('s')
                  # 假設夾取到
                  getball = 1
                # 已成功夾取到
                elif ball[2] > 55 and getball == 1:
                    # 小車停止
                    arduino.write('9')
                    break;
                # 未成功抓取 釋放夾手
                else:
                  arduino.write('r')
                  getball = 0

                if getball == 0:
                    # 前方未發現有小球
                    if ball[2] <= 2:
                        # 原地右轉
                        arduino.write('8')
                        arduino.write('9')
                    else:
                        # 小球在前方偏右 (攝像頭安裝時是倒的)
                        if ball[0] < 130 and ball[2] <= 20:
                            # 原地右轉
                            arduino.write('8')
                            arduino.write('9')
                        # 小球在前方偏左
                        elif ball[0] > 190 and ball[2] <= 20:
                            # 原地左轉
                            arduino.write('7')
                            arduino.write('9')
                        else:
                            arduino.write('1')

                # reset the stream before the next capture
                stream.seek(0)
                stream.truncate()

                time.sleep(1);

            # 完成夾取小球的任務後可以通過在控制檯敲入指令控制小車
            while True:
                cmd = sys.stdin.readline()
                arduino.write(cmd)

            cv2.destroyAllWindows()

（實訓時做的,水平有限，易受環境光照影響，小車找尋與抓取速度也較慢，僅供參考）