特定の色を検出して追従するロボット メモ

以前 カメラの画像処理によるライントレースカーを作ったことがあり、その応用で「前方の特定の色を検出して追従する車」が作れるのではないかと思い、実際に作ってみました。

pic.twitter.com/yzy1QrbtIs September 9, 2020

一旦ソースコードだけですが公開しておきます。 どういう考え方で制御しているかなど、また気が向いたら追記していきます。

import cv2
import numpy as np
import time
import RPi.GPIO as GPIO
import pigpio

# 正転・逆転制御用GPIO
R_ctrl = 17
L_ctrl = 27

# PWM制御用GPIO
R_PWM = 12
L_PWM = 13

# GPIO設定
GPIO.setwarnings(False) 
GPIO.setmode(GPIO.BCM)
GPIO.setup(R_ctrl, GPIO.OUT)
GPIO.setup(L_ctrl, GPIO.OUT)

# PWM設定
pi = pigpio.pi()
pi.set_mode(R_PWM, pigpio.OUTPUT)
pi.set_mode(L_PWM, pigpio.OUTPUT)

duty_S = 4 #直進PWM duty比
duty_T = 4 #旋回PWM duty比
freq = 700 #PWM周波数
sleep_time_S = 0.05 #モーター動作待ち 直進
sleep_time_T = 0.15 #モーター動作待ち 旋回

# ↓画像処理関係設定↓
camera = cv2.VideoCapture(0)   # カメラCh.(ここでは0)を指定

# カメラ画素数（実環境に合わせて修正）
camera_H = 480 #幅
camera_W = 640 #高さ

B_width = 150 #ブロックエリア幅

T_th = 1000  #曲がり判定しきい値
N_th = 60000 #近づき判定しきい値
F_th = 50000 #離れ判定しきい値

# 二値化設定
th = 50 #閾値
i_max = 255 #閾値超えたら置き換える値

# 青色検出関数
def detect_blue_color(img):
    # HSV色空間に変換
    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

    # 青色のHSVの値域1
    hsv_min = np.array([90, 64, 0])
    hsv_max = np.array([150,255,255])

    # 青色領域のマスク
    mask = cv2.inRange(hsv, hsv_min, hsv_max)

    # マスキング処理
    masked_img = cv2.bitwise_and(img, img, mask=mask)

    return masked_img

# HSV画像のグレースケール化関数
def hsv2gray(img):
    
    (h,s,v) = cv2.split(img) #h,s,v に分割
    s[:] = 0 #彩度を0に
    merge = cv2.merge((h, s, v))
    rgb = cv2.cvtColor(merge, cv2.COLOR_HSV2RGB) #hsv→rgb変換
    gray = cv2.cvtColor(rgb, cv2.COLOR_RGB2GRAY) #rgb→グレースケール化
    
    return gray

# モーター制御関数
def motorctrl(R, L, frequency ,duty ,sleep_time):

    #正転・逆転指示
    GPIO.output(R_ctrl, R)
    GPIO.output(L_ctrl, L)

    #指定時間だけモーター回転
    pi.hardware_PWM(R_PWM, frequency, duty*100000)
    pi.hardware_PWM(L_PWM, frequency, duty*100000)
    time.sleep(sleep_time)

    #停止
    pi.hardware_PWM(R_PWM, freq, 000000)
    pi.hardware_PWM(L_PWM, freq, 000000)


while True:
    ret, frame = camera.read()    #フレームを取得
    blue_frame = detect_blue_color(frame) #青色検出
    gray_frame = hsv2gray(blue_frame) #青色をグレースケール化
    ret, bi_frame = cv2.threshold(gray_frame, th, i_max, cv2.THRESH_BINARY) #グレースケールを二値化

    #↓ブロックエリア描画設定（描画させないならコメントアウトでOK）
    #線は青色で描画している（二値化画像では白で表示される）
    cv2.rectangle(bi_frame,(0,camera_H),(B_width,camera_H),(255,0,0),1) #左ブロックエリア
    cv2.rectangle(bi_frame,(camera_W - B_width,0),(camera_W,camera_H),(255,0,0),1) #右ブロックエリア

    #↓エリア設定（領域計算用）[縦,横]
    LB = bi_frame[0:camera_H, 0:B_width] #左ブロックエリア
    RB = bi_frame[0:camera_H, camera_W - B_width:camera_W] #右ブロックエリア
    CA = bi_frame[0:camera_H, 0:camera_W] #画面全体

    #↓各領域の面積計算
    Det_LB = cv2.countNonZero(LB) #左ブロックエリアの白ピクセルカウント
    Det_RB = cv2.countNonZero(RB) #右ブロックエリアの白ピクセルカウント
    Det_CA = cv2.countNonZero(CA) #画面全体の白ピクセルカウント
    
    print("Det_LB: " +str(Det_LB) + "  Det_CA: " +str(Det_CA) +"  Det_RB: " +str(Det_RB))
    
    #↓画像を表示（表示不要であればコメントアウト。表示しないほうが処理早い）
    cv2.imshow('original', frame) #元画像を表示
    #cv2.imshow('detect_blue', blue_frame) #青色検出画像を表示
    cv2.imshow('blue_white', bi_frame) #青色→グレースケール→二値化画像を表示

    #↓制御部分
    #ちょうどいい距離の時は止まっておく
    if (N_th -5000) > Det_CA > (F_th + 5000):
        motorctrl(1,1,freq,0,sleep_time_S)

    #離れすぎたら近づく
    elif Det_CA < F_th:
        print("離れすぎを検出しました")
        motorctrl(1,1,freq,duty_S,sleep_time_S)

    #近すぎたら離れる
    elif Det_CA > N_th:
        print("近すぎを検出しました")
        motorctrl(0, 0, freq, duty_S, sleep_time_S)

    #左にずれたら右旋回
    elif Det_LB > T_th:
        print("左ブロックエリア侵入を検出しました")
        motorctrl(1, 0, freq, duty_T, sleep_time_T)

    #右にずれたら左旋回
    elif Det_RB > T_th:
        print("右ブロックエリア侵入を検出しました")
        motorctrl(0, 1, freq, duty_T, sleep_time_T)

    if cv2.waitKey(1) & 0xFF == ord('q'):
        print("エスケープが押されました")
        break

# 撮影用オブジェクトとウィンドウの解放
camera.release()
cv2.destroyAllWindows()

やってみて思ったのは、とりあえず原理的にできることはわかりましたが、あまり実用的ではないのかなと思いました。
理由
・検出対象と同系色の物体も検出してしまう
・光の当たり方で色の見え方が変わる
等、ノイズに弱いから

フィルタかけたりで工夫はできるのかもしれませんが、どこまでできるのかな、という感じです。
やっぱりもっと形とか、対象の特徴を捉えて追従させるのがいいんだと思います。
単純なロジックなので、処理的には軽いのかもしれません（他の手法をやったことないので比較できていません）