画像処理を用いたエアホッケーロボット

今回は、画像処理についての記事です。

pythonで書いてます。動画もあるヨ！

なお、ここからは口調が変えて真面目に書きます。

エアホッケーロボットとは

エアホッケーロボットとは、人間と対戦して勝利することを想定したロボットである。自作のエアホッケー台を用意し、これにロボットを設置した。

制御について

ロボットの制御としては、まずカメラでとらえた映像を私のノートパソコンに送り、そこで画像処理をして、パック（エアホッケーで使われる丸い白い円盤）が到達するであろう座標を計算して、マイコンにその計算結果をシリアル通信で送信し、マイコンでエンコーダーを用いたPID制御でロボットを動かした。

開発環境

　使用したマイコンはSTM32F446REである。マイコン側の開発環境はSystem Workbench for STM32(通称SW4STM32)で、CubeMXというマイコンの回路などの設定をするツールで設定して、それをEclipseに吐き出してEclipse上でプログラミングをした。これらの開発環境は総称してHALと呼ばれるものである。用いた言語はCである。

　一方、ノートパソコンの方では、開発環境はWinpythonで、VScode上でプログラミングした。使用した言語はpythonである。用いたライブラリはserialとnumpyとopencvである。以下、画像処理のプログラムについて詳しく述べる。

動画

画像処理を用いたエアホッケーロボット

ソースコード

以下に実際に使用したソースコードを載せる。

動けばいいや、と思って書いたので、幼稚なプログラムで恐縮です。
皆さんは、参考程度にしてください。関数の名前などは以下の文章と対応しているので、適宜見てほしい。

コメントアウトは特に意味はないので、無視してほしい。

import cv2
import numpy as np
import math
import time
import serial
# mode change1
n=0
t=0
count = 0

def morph_and_blur2(img):
kernel = np.ones((8,8),np.uint8)
m = cv2.GaussianBlur(img,(3,3),0)
m = cv2.erode(m,kernel,iterations = 1)
# m = cv2.morphologyEx(m,cv2.MORPH_OPEN,kernel,iterations=2)
m = cv2.GaussianBlur(m,(5,5),0)
return m

def binary_threshold2(frame):
grayed = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)
morph = morph_and_blur2(grayed)
under_thresh = 245
upper_thresh = 180
maxvalue = 255
th,drop_back = cv2.threshold(morph,under_thresh,maxvalue,cv2.THRESH_BINARY)
th,clarify_born = cv2.threshold(morph,upper_thresh,maxvalue,cv2.THRESH_BINARY_INV)
merged = np.maximum(drop_back,clarify_born)
return merged

 


def padding_position(x,y,w,h,p):
return x-p,y-p,w+p*2,h+p*2


def detect_contour(path, min_size):
# contoured = cv2.cvtColor(path,cv2.COLOR_BGR2GRAY)
contoured = binary_threshold2(path)
forcrop = path
global centerx
global centery
centerx = 0
centery = 0 
birds = binary_threshold2(path)
birds = cv2.bitwise_not(birds)

im2,contours,hierarchy = cv2.findContours(birds,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)

crops = []

for c in contours:
if cv2.contourArea(c) < min_size:
continue 
x,y,w,h = cv2.boundingRect(c)
x,y,w,h = padding_position(x,y,w,h,5)

cropped = forcrop[y:(y+h),x:(x+w)]
crops.append(cropped)

cv2.drawContours(contoured,c,-1,(0,0,255),3)
cv2.rectangle(contoured,(x,y),(x+w,y+h),(0,255,0),3)

centerx = int(math.floor(x+w/2))
centery = int(math.ceil(y+h/2))
return contoured, crops,centerx,centery

class point:
def __init__(self,x,y):
self.x = float(x)
self.y = float(y)

def get_line_intersection(A1,A2,B1,B2):
a = A2.y - A1.y
b = -B2.y + B1.y
c = A2.x - A1.x
d = -B2.x + B1.x
C1 = B1.y - A1.y
C2 = B1.x - A1.x

tmp = a*d - b*c

if tmp:
invMa = d /tmp
invMb = -b /tmp
invMc = -c /tmp
invMd = a /tmp

m = invMa*C1 + invMb*C2
n = invMc*C1 + invMd*C2
secx = int(A1.x + m*(A2.x - A1.x))
secy = int(A1.y + m*(A2.y - A1.y)) 
secx = abs(secx)
secy = abs(480-secy)
return secx,secy

else:
secx=205
secy=50
return secx,secy


capture = cv2.VideoCapture(0)
median = np.full(5,0,dtype="uint16")
mediany = np.full(9,0,dtype="uint16")
triming1x = 200
triming2x = 30
h = 480
w = 640
widthx = w - (triming1x + triming2x)
heighty = 450
tyusin = int(widthx/2)
encx = 2000
ency = 1800
ceny = int(h/2)
cenx = int(w/2)
scale = 1.0
angle = 0.0

 

ser = serial.Serial('COM7',115200,timeout=1)
while(1):
#time.sleep(1)
start = time.time()
ret,frame = capture.read()
rotation_matrix = cv2.getRotationMatrix2D((cenx,ceny),angle,scale)
img_rot = cv2.warpAffine(frame,rotation_matrix,(w,h),flags=cv2.INTER_CUBIC)

dst = img_rot[0:h,0+triming1x:w-triming2x]
contoured1, crops1,centerx1,centery1 = detect_contour(dst,1000)
ret, frame = capture.read()
img_rot = cv2.warpAffine(frame,rotation_matrix,(w,h),flags=cv2.INTER_CUBIC)
dst = img_rot[0:h,0+triming1x:w-triming2x]

contoured2, crops2, centerx2, centery2 = detect_contour(dst,1000)
contoured2 = cv2.circle(contoured2,(centerx1,centery1),10,(0,0,0),-1)
contoured2 = cv2.circle(contoured2,(centerx2,centery2),10,(0,0,0),-1)

A1 = point(0,h-1)
A2 = point(w-1,h-1)
B1 = point(centerx1,centery1)
B2 = point(centerx2,centery2)

 


cv2.line(contoured2,(centerx1,centery1),(centerx2,centery2),(255,0,0),1)

secx,secy = get_line_intersection(A1,A2,B1,B2)
print('centery2:'+str(centery2))
#print('secy:'+str(secy))

if centery2>200 and int(t)<60:
secx = int(centerx2 / widthx*encx)
secy = int(abs(h-centery2) / heighty*ency)
secx_str = str(int(secx))
secy_str = str(int(secy))
secx_zero = secx_str.zfill(4)
secy_zero = secy_str.zfill(4)

value = secx_zero + secy_zero + "s"
print("yyyyyyyyyyyyyyyyy")
print(int(t))
print(value)
print(w,h)

print('portstr',ser.portstr)
value2=value.encode('utf-8')
ser.write(value2)
contoured2 = cv2.circle(contoured2, (secx,secy),10,(255,0,0),-1)
cv2.line(contoured2,(centerx2,centery2),(secx,secy),(255,0,0),1)
cv2.namedWindow('image',cv2.WINDOW_NORMAL)
cv2.imshow('image',contoured2)
cv2.imshow('image2',dst)
elapsed_time = time.time() - start
t = t + elapsed_time
print("elapsed_time:{0}".format(t)+"[sec]")
count = 0
if cv2.waitKey(1) & 0xFF == ord('q'):
break 
continue

secx = abs(secx)
while(secx>(widthx-1)):
a = secx - (widthx-1)
secx = abs((widthx-1)-a)
median = np.append(median,int(secx))
median = np.delete(median,0)
m = np.median(median)
m = int(m/widthx*encx)

mediany = np.append(mediany,int(secy))
mediany = np.delete(mediany,0)
my = np.median(mediany)
my = int(my/heighty*ency)

 

 


secy = int(secy/heighty*ency)
if n == m:
if count == 0:

secx = int(tyusin/ widthx*encx)
secy = int( 80/ heighty*ency)
secx_str = str(int(secx))
secy_str = str(int(secy))
secx_zero = secx_str.zfill(4)
secy_zero = secy_str.zfill(4)

value = secx_zero + secy_zero + "s"
print('portstr',ser.portstr)
value2=value.encode('utf-8')
ser.write(value2)
count = 1
contoured2 = cv2.circle(contoured2, (secx,secy),10,(255,0,0),-1)
cv2.line(contoured2,(centerx2,centery2),(secx,secy),(255,0,0),1)
cv2.namedWindow('image',cv2.WINDOW_NORMAL)
cv2.imshow('image',contoured2)
cv2.imshow('image2',dst)
elapsed_time = time.time() - start
t = t + elapsed_time
print("elapsed_time:{0}".format(t)+"[sec]")
if cv2.waitKey(1) & 0xFF == ord('q'):
break 
continue


contoured2 = cv2.circle(contoured2, (secx,secy),10,(255,0,0),-1)
cv2.line(contoured2,(centerx2,centery2),(secx,secy),(255,0,0),1)
cv2.namedWindow('image',cv2.WINDOW_NORMAL)
cv2.imshow('image',contoured2)
cv2.imshow('image2',dst)

count = 0

print('aaaaaaaaaaaaaaa')
elapsed_time = time.time() - start
t = t + elapsed_time
print("elapsed_time:{0}".format(t)+"[sec]")
if cv2.waitKey(1) & 0xFF == ord('q'):
break 
continue

secy = int(50/heighty*ency)
n = m
print('only x')
secx_str = str(int(m))
secy_str = str(int(secy))
secx_zero = secx_str.zfill(4)
secy_zero = secy_str.zfill(4)
value = secx_zero + secy_zero + "s"
print(value)

contoured2 = cv2.circle(contoured2, (m,secy),10,(255,0,0),-1)
cv2.line(contoured2,(centerx2,centery2),(m,secy),(255,0,0),1)
cv2.namedWindow('image',cv2.WINDOW_NORMAL)
cv2.imshow('image',contoured2)
cv2.imshow('image2',dst)
if cv2.waitKey(1) & 0xFF == ord('q'):
break 
print('portstr',ser.portstr)
value2=value.encode('utf-8')
ser.write(value2)
elapsed_time = time.time() - start
t = t + elapsed_time
print("elapsed_time:{0}".format(t)+"[sec]")
count = 0
# time.sleep(0.05)

ser.close()
capture.release()
cv2.destroyAllWindows()

 

本題の画像処理

　まず、今回のプログラムの仕組みについて簡単に述べる。まずカメラから入手したフレームを２枚用意し、前処理と白黒二値化した画像を用意して、パックかどうか判別する関数を用いてパックの現在地を判別する。

　そうして、二つのフレームにおける座標をもとに、直線を作成して、ロボット側の底辺と交わる座標を求める。この時、反射も考慮して、実際の座標に対応する座標を求める。

　これによって、近似的にパックの来るである座標を求めることができる。

前処理について

　まず生のフレームをカメラから入手したときに前処理をして、判別しやすいように画像データを変換する。

　まず、opencvのcv2.cvtColorを用いてRGB画像からGRAY画像に変換する。この理由は、画像をRGB画像として処理するとデータのサイズが大きくなってしまうので、今回のような速さが求められるプログラムでは白黒で処理することにした。

　次に、この画像データをcv2.GaussianBlur, cv2.erodeというopencvの関数を用いてぼやけさせたり、対象の周囲をnumpyで作成した配列分ピクセルだけ侵食することでノイズの影響を小さくする。この理由としては、今回はパックを白色、背景となるホッケー台を茶色で行ったが、ロボットを金属で作ったために光の反射などにより白色と誤認識することがあるため、この前処理は必須であった。

　次に、cv2.thresholdを用いることでGRAY画像を黒と白の完全な二値画像に変換した。手順としては、白として得たい、パックの部分の画像と、背景として得たいそれ以外の部分をcv2.thresholdで二つ作成しておき、これらをnp.maximumによって結合することで得られる。この時、境界となるGRAYの値を０～２５５のうち適切なものを選ぶことで光の反射による比較的明るい白と、パックの白をある程度までなら区別することができた。

　次に、判別する関数の部分について述べる。ここではcv2.findContoursという関数を用いた。この関数は、白黒の二値画像を入力として、白の部分のピクセルを一つ見つけると、その周囲のピクセルについて白色のピクセルがあるかどうか探索し、あればそのピクセルに移動して再度探索をするというものである。つまり、白色で閉じている部分の面積を求めているのである。

　この関数の戻り値を、cv2.boundingRect関数の引数として計算すると、白で閉じている部分のピクセルのうちxy座標が最も小さい座標と、閉じている部分の縦と横の長さが得られる。これによって、パックをより正確に検出するために、ある値の面積を決めておき、それ以上ならパックである、また、縦と横の長さがある値からある値までの範囲をとっていればパックである、というような条件を付けることが可能である。このようにして、画像からあるものを検出することができる。

実際の制御について

　まず、前提として、今回のロボットはｘ、ｙ座標に動く直動機構のロボットである。それぞれのモータがラック上で回転し、それをエンコーダーでフィードバックすることで現在地を把握している。

　まず、基本的にロボットは、パックを入れるゴールの中心に待機することにした。そして、ロボット側から見て左右をｘ軸とすると、パックが来た時はまずｘ軸上の予想された座標にロボットを動かすことを優先する。

　そして、そこにたどり着いた後にｙ軸とｘ軸について現在のパックの座標にロボットを動かす。そうすることで、理論上は少なくてもパックに間に合いさえすれば、パックを打ち返せるはずである。

　また、ｙ軸について、ラックの長さ以上の座標にロボットを動かすことはできないため、if文で分岐するようにした。

　また、パックの予想座標をマイコンに送信するときに、常に値を送信していると、マイコンのバッファを破壊してしまうことになりかねないため、送信する値と前回の値の値にあまり変化がない場合は送信しないようにする、という機能を中央値を取ることで実装した。

結果（まとめ？）

　結果としては、画像処理を用いたロボットを自動で動かすことはできたが、反応がかなり遅く、人間に勝てるロボットは到底作れなかった。この遅さは異常で、プログラムが正しく動いていればもっと速く動いているはずであった。

　この原因としてはやはり、マイコンのバッファに値を一方的に大量に送り付けすぎていることがあげられる。実際に、値を一つだけ送る場合では問題なくすぐに動いた。

　また、なぜかロボットを動かしていると、エンコーダーの値がどんどんずれていくという現象が発生し、最大でも一分半程度しか連続して動かせなかった。それ以上動かすと、ロボットがラックから外れてストールしてしまった。この対策としては、動かす範囲の両端にリミットスイッチを作成し、エンコーダーの値を初期化することがあげられるが、これを実装するとロボットにおかしな挙動がみられたため、やむを得ずデバッグを断念した。

　また、画像処理の部分では、照明の明るさなどによってパックや背景の明るさは変化するため、朝と夜では閾値が異なり、定期的なパラメータ調整が必要であった。

　また、カメラ自身が揺れることでそもそもの画像にずれが生じてしまうこともあった。このため、画像のトリミングや回転などをその都度調整する必要があった。

改善案？

　改善点としては、まずマイコンのバッファの破壊を防ぐために、ノートパソコン側でPID制御も行い、必要な出力だけをマイコンに渡すようにすることである。このためには、エンコーダーの代わりに、ノートパソコン側でパックだけでなくロボットの認識も　行う必要がある。

　また、今回のパックの予測ではパックは直線で進み、反射するときは常に反射の法則が成り立つとしていたが、そんなわけはないので、線形回帰を用いた機械学習を導入することがあげられる。画像を特徴量とするのではなく、画像を処理して得られたパックの座標、二つのフレームでのパックの移動量（つまりベクトル）を特徴量として線形回帰を用いることで、パックの位置とタイミングをより正確に、より早く得られるはずである。

　また、そもそもpythonの処理が遅いため、C++を用いたopencvによるプログラムにするべきである。

　また、カメラの揺れを補正するために、エアホッケー台を自動で検出する機能も付けるべきである。

まとめ（２回目）

画像処理は奥が深いなあ