読者です 読者をやめる 読者になる 読者になる

raspberry pi2 + カメラ付きサーボで特定の色を追っかけるカメラを作る

概要

raspberry pi2 を使ってカメラ付きサーボモータを制御した。 python+OpenCVで特定の色の中心を追っかけるようにした。 かくかくしてるけどだいたい良い。

はじめに

学生の頃、ロボットに触る機会はよくあったが、だいたい「これが見えたらあっちに歩け」ぐらいの抽象度で、モータの角度を何度にするとか、電気回路がどうとかはさっぱりわからない。 仮にも機械工学科目を修めておいてこれではいけないなと思って、とりあえず部屋に転がっていたサーボモータを動かしてみることにした。

使用したもの

  • 秋月のwebカメラ付きサーボ
  • raspberry pi2
  • ブレッドボードなど

画像中の特定の色の中心を求める

OpenCVを使って何も考えずに書くと以下のようになる。 今回は青色を追っかけるようにした。

import cv2
import numpy as np

capture = cv2.VideoCapture(0)
capture.set(3, 320)
capture.set(4, 240)

dst = np.zeros((320, 240), dtype=np.uint8)

while True:
    _, img = capture.read()
    dst = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    conditions = (dst[:,:,0]<150)*1 * ((dst[:,:,0]>90)*1) * (dst[:,:,1]>100)

    sum_x = 0
    sum_y = 0
    sum_item = 0
    for x,row in enumerate(conditions):
        for y,item in enumerate(row):
            sum_x += x * item
            sum_y += y * item
            sum_item += item
    mean_x = sum_x / sum_item
    mean_y = sum_y / sum_item

    cv2.circle(img, (mean_y, mean_x),10, (0,0,255), -1)
    cv2.imshow("camera", img)
    cv2.waitKey(1)

capture.release()
cv2.destroyAllWindows()

pythonを使っている人にはわかると思うが、このプログラムはとんでもなく遅い。 実際にラズパイで実行してみると体感2fpsぐらいだった。 というわけで、以下のように書き換えた。

import cv2
import numpy as np

capture = cv2.VideoCapture(0)
capture.set(3, 320)
capture.set(4, 240)

while True:
    _, img = capture.read()
    dst = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    conditions = (dst[:,:,0]<150)*1 * ((dst[:,:,0]>90)*1) * (dst[:,:,1]>100)

    sum_item = np.sum(conditions)
    t_conditions = np.transpose(conditions)

    temp = range(len(conditions))
    mean_x = np.sum([temp * t_condition for t_condition in t_conditions])/sum_item
    temp = range(len(t_conditions))
    mean_y = np.sum([temp * condition for condition in conditions])/sum_item

    cv2.circle(img, (mean_y, mean_x),10, (0,0,255), -1)
    cv2.imshow("camera", img)
    cv2.waitKey(1)

capture.release()
cv2.destroyAllWindows()

これでまあ満足できる程度の早さになる。 実行するとこのようになる。

f:id:ksknw:20160228102336p:plain

サーボモータを制御する

回路を適当に組む。 サーボの動作電圧が4.8Vだったので、電池を3つつなげて4.5Vで使用した。 サーボの信号線をラズパイのPWMができるピンに接続する。

f:id:ksknw:20160228003450j:plain

ラズパイのピンのレイアウトはこちらを参考にした。 PWMで制御するために今回はGPIO13と18を使う。

こちら を参考にしてモータを動かしてみる。

import wiringpi2

PWM_PIN = 13
#PWM_PIN = 18
DUTY_MAX = 123 # 90°
DUTY_MIN = 26  # -90°
DUTY_HOME = 74 # 0°
duty = 0

wiringpi2.wiringPiSetupGpio()
wiringpi2.pinMode(PWM_PIN, wiringpi2.GPIO.PWM_OUTPUT)
wiringpi2.pwmSetMode(wiringpi2.GPIO.PWM_MODE_MS)
wiringpi2.pwmSetClock(375)

wiringpi2.pwmWrite(PWM_PIN, DUTY_HOME)
wiringpi2.delay(100)

def move(degree):
    duty = int((DUTY_MAX-DUTY_MIN)/180.0 * degree + DUTY_HOME)
    wiringpi2.pwmWrite(PWM_PIN, duty)

for degree in [0,45,90,0,-45,-90,0,30,-30,0]:
    move(degree)
    wiringpi2.delay(500)

実行するとこんな感じで動く。

f:id:ksknw:20160228103343g:plain

青色を追っかける

2つのプログラムを組み合わせる。

import wiringpi2
import cv2
import numpy as np

PIN_X = 13
PIN_Y = 18
DUTY_MAX = 123 # 90°
DUTY_MIN = 26  # -90°
DUTY_HOME = 74 # 0°
duty = 0

wiringpi2.wiringPiSetupGpio()
wiringpi2.pinMode(PIN_X, wiringpi2.GPIO.PWM_OUTPUT)
wiringpi2.pinMode(PIN_Y, wiringpi2.GPIO.PWM_OUTPUT)
wiringpi2.pwmSetMode(wiringpi2.GPIO.PWM_MODE_MS)
wiringpi2.pwmSetClock(375)

wiringpi2.pwmWrite(PIN_X, DUTY_HOME)
wiringpi2.pwmWrite(PIN_Y, DUTY_HOME)
wiringpi2.delay(100)

def move(degree_x, degree_y):
    duty_x = int((DUTY_MAX-DUTY_MIN)/180.0 * degree_x + DUTY_HOME)
    duty_y = int((DUTY_MAX-DUTY_MIN)/180.0 * degree_y + DUTY_HOME)
    wiringpi2.pwmWrite(PIN_X, duty_x)
    wiringpi2.pwmWrite(PIN_Y, duty_y)
    wiringpi2.delay(100)

def detect_blue_center(img):
    dst = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    conditions = (dst[:,:,0]<150)*1 * ((dst[:,:,0]>90)*1) * (dst[:,:,1]>100)
    sum_item = np.sum(conditions)
    t_conditions = np.transpose(conditions)

    temp = range(len(conditions))
    mean_x = np.sum([temp * t_condition for t_condition in t_conditions])/sum_item
    temp = range(len(t_conditions))
    mean_y = np.sum([temp * condition for condition in conditions])/sum_item
    return mean_x, mean_y

capture = cv2.VideoCapture(0)
capture.set(3, 320)
capture.set(4, 240)

now_angle_x = 0
now_angle_y = 0

while True:
    _, img = capture.read()
    mean_x, mean_y = detect_blue_center(img)
    cv2.circle(img, (mean_y, mean_x),10, (0,0,255), -1)

    angle_x = (mean_x - 160)/160.0*7
    angle_y = -(mean_y - 120)/120.0*7
    move(now_angle_y + angle_y, now_angle_x + angle_x)
    now_angle_y = now_angle_y + angle_y
    now_angle_x = now_angle_x + angle_x

    cv2.imshow("camera", img)
    cv2.waitKey(1)

capture.release()
cv2.destroyAllWindows()

実行するとこんな感じ。 かくかくしてるし、ラグがあって微妙だけど動くからいいかな。 f:id:ksknw:20160228142524g:plain

おわりに

かくかくしてるのとかラグとか直すべきところはいっぱいあるけど、特に目標があって始めたわけでもないからやる気がでない。

追記(3月4日)

なんとなくdelayを入れていたけど、とってみるとカクカクしなくなった。金麦飲んでたせいで気づかなかったわー金麦のせいだわー。 ついでに画像サイズ小さくしたらかなりよくなった。

参考