コップ運びロボット
今回は前から作っていたドリンクサーバー（夏休みの自由研究）のコップ運びロボットを紹介します。
まず初めにロボットの外観を紹介します。
作ったロボットはこちらです。
本当はraspberrypiを使って作りたいのですがraspberrypiで作ると動かなくなったときにハードウェアに問題があるかソフトウェアに問題があるかがわかりにくいのでまず LEGOのEV3で作ることにしました。
EV3にはPythonでプログラミングができるようev3devというosをインストールします。
ev3devのインストールにはmicrosdカードが必要です。
ev3devの公式サイト(https://www.ev3dev.org/)からosをタウンロードしてEtcher(https://www.balena.io/etcher/)などを使いsdカードに書き込みます。
仕組みはテーブルの上につけたカメラの映像をパソコンで画像処理してロボットに無線で送りロボットが指示どおりに動きます。
カメラにはiphone 5sを使いました。
iphoneをwebカメラにできるivcamというアプリを使ってiphoneの映像を取得しています。
プログラミングにはPythonを使い、画像処理にはopencvを使いました。
今回は処理を軽くするために必ずコップをピンクの折り紙の上に置く必要があります。
ロボットとパソコンの通信にはsocket通信を使いました。
ロボットには超音波センサー、ジャイロセンサーがついています。
超音波センサーは超音波を使い距離を測定するセンサー、 ジャイロセンサーは回転を検知するセンサーです
画像処理のプログラム
import cv2 import numpy as np import time import socket import math LOW_COLOR_BLUE = np.array([100, 75, 75]) HIGH_COLOR_BLUE = np.array([140, 255, 255]) LOW_COLOR_GREEN = np.array([30, 64, 0]) HIGH_COLOR_GREEN = np.array([90,255,255]) LOW_COLOR_PINK = np.array([150, 100, 100]) HIGH_COLOR_PINK = np.array([188,255,255]) AREA_RATIO_THRESHOLD = 0.005 def find_specific_color(frame,AREA_RATIO_THRESHOLD,LOW_COLOR,HIGH_COLOR): """ 指定した範囲の色の物体の座標を取得する関数 frame: 画像 AREA_RATIO_THRESHOLD: area_ratio未満の塊は無視する LOW_COLOR: 抽出する色の下限(h,s,v) HIGH_COLOR: 抽出する色の上限(h,s,v) """ # 高さ，幅，チャンネル数 h,w,c = frame.shape # hsv色空間に変換 hsv = cv2.cvtColor(frame,cv2.COLOR_BGR2HSV) # 色を抽出する ex_img = cv2.inRange(hsv,LOW_COLOR,HIGH_COLOR) # 輪郭抽出 contours,hierarchy = cv2.findContours(ex_img,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE) # 面積を計算 areas = np.array(list(map(cv2.contourArea,contours))) if len(areas) == 0 or np.max(areas) / (h*w) < AREA_RATIO_THRESHOLD: # 見つからなかったらNoneを返す return None else: # 面積が最大の塊の重心を計算し返す max_idx = np.argmax(areas) max_area = areas[max_idx] result = cv2.moments(contours[max_idx]) x = int(result["m10"]/result["m00"]) y = int(result["m01"]/result["m00"]) return (x,y) #二つのベクトルの角度を出す def calcAngle(V1,V2): V1size = np.sqrt(V1[0] ** 2 + V1[1] ** 2) V2size = np.sqrt(V2[0] ** 2 + V2[1] ** 2) if V1size == 0 or V2size == 0: #0ベクトルエラー return None #内積を求める inner = (V1[0] * V2[0]) + (V1[1] * V2[1]) cos = inner / (V1size * V2size) rad = math.acos(cos) return rad def calc_sin(V1,V2): V1size = np.sqrt(V1[0] ** 2 + V1[1] ** 2) V2size = np.sqrt(V2[0] ** 2 + V2[1] ** 2) if V1size == 0 or V2size == 0: return None inner = np.cross(V1,V2) sin = inner / (V1size * V2size) rad = math.asin(sin) return rad def communication(data): with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: # サーバを指定 s.connect(('ロボットのIPアドレスを指定', 50007)) s.sendall(data.encode()) # webカメラを扱うオブジェクトを取得 cap = cv2.VideoCapture(1) c = 1 t = 0 purpose = 0 fname_img2='t4.jpg' temp = cv2.imread(fname_img2, 0) while True: ret,frame = cap.read() if ret is False: print("cannot read image") continue # 位置を抽出 pos_blue = find_specific_color( frame, AREA_RATIO_THRESHOLD, LOW_COLOR_BLUE, HIGH_COLOR_BLUE ) pos_green = find_specific_color( frame, AREA_RATIO_THRESHOLD, LOW_COLOR_GREEN, HIGH_COLOR_GREEN ) pos_pink = find_specific_color( frame, AREA_RATIO_THRESHOLD, LOW_COLOR_PINK, HIGH_COLOR_PINK ) gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) #マッチングテンプレートを実行 match_result = cv2.matchTemplate(gray, temp, cv2.TM_CCOEFF_NORMED) threshold = 0.5 #検出結果から検出領域の位置を取得 loc=np.where(match_result >= threshold) w, h = temp.shape[::-1] for top_left in zip(*loc[::-1]): bottom_right = (top_left[0] + w, top_left[1] + h) try: centerX = int((top_left[0] + bottom_right[0]) / 2) centerY = int((top_left[1] + bottom_right[1]) / 2) pos_img = (centerX,centerY) except: pos_img = None if purpose == 0: if pos_blue and pos_green and pos_pink is not None: c = 1 # 抽出した座標に丸を描く cv2.circle(frame,pos_blue,10,(0,0,255),-1) cv2.circle(frame,pos_green,10,(0,0,255),-1) x = int(((pos_green[0] - pos_blue[0]) / 2) + pos_blue[0]) y = int(((pos_green[1] - pos_blue[1]) / 2) + pos_blue[1]) cv2.circle(frame,(x,y),10,(0,0,255),-1) cv2.line(frame, pos_blue, pos_green, (0, 255, 0)) cv2.circle(frame,pos_pink,10,(0,0,255),-1) cv2.line(frame, pos_pink, (x,y), (0, 255, 0)) #点 O = [x,y] P = list(pos_blue) Q = list(pos_pink) #ベクトル V1 = [P[0]-O[0],P[1]-O[1]] V2 = [Q[0]-O[0],Q[1]-O[1]] #角度の計算 rad = calcAngle(V1,V2) sin = calc_sin(V1,V2) #ラジアンを角度に変換 degree = math.degrees(rad) if sin > 0: degree = 360 - degree degree = degree - 90 if degree < 0: r = "r " degree = degree * -1 else: r = "l " if (time.time() - t) >= 4: communication("4") communication(r + str(int(degree))) t = time.time() time.sleep(2) communication("0") a = (x - pos_pink[0]) ** 2 b = (y - pos_pink[1]) ** 2 if (a + b) < 20000: communication("4") communication("8") time.sleep(3) communication("5") time.sleep(6) purpose = 1 else: if c == 1: communication("4") c = 0 if purpose == 1: if pos_blue and pos_green and pos_img is not None: c = 1 # 抽出した座標に丸を描く cv2.circle(frame,pos_blue,10,(0,0,255),-1) cv2.circle(frame,pos_green,10,(0,0,255),-1) x = int(((pos_green[0] - pos_blue[0]) / 2) + pos_blue[0]) y = int(((pos_green[1] - pos_blue[1]) / 2) + pos_blue[1]) cv2.circle(frame,(x,y),10,(0,0,255),-1) cv2.line(frame, pos_blue, pos_green, (0, 255, 0)) cv2.circle(frame,pos_img,10,(0,0,255),-1) cv2.line(frame, pos_img, (x,y), (0, 255, 0)) #点 O = [x,y] P = list(pos_blue) Q = list(pos_img) #ベクトル V1 = [P[0]-O[0],P[1]-O[1]] V2 = [Q[0]-O[0],Q[1]-O[1]] #角度の計算 rad = calcAngle(V1,V2) sin = calc_sin(V1,V2) #ラジアンを角度に変換 degree = math.degrees(rad) if sin > 0: degree = 360 - degree degree = 90 - degree if degree < 0: r = "l " degree = degree * -1 else: r = "r " if (time.time() - t) >= 5: communication("4") communication(r + str(int(degree))) t = time.time() time.sleep(3) communication("0") a = (x - pos_img[0]) ** 2 b = (y - pos_img[1]) ** 2 if (a + b) < 16000: communication("4") communication("6") time.sleep(4) communication("1") time.sleep(1) communication("4") break else: if c == 1: communication("4") c = 0 # 画面に表示する cv2.imshow('frame',frame) # キーボード入力待ち key = cv2.waitKey(1) & 0xFF # qが押された場合は終了する if key == ord('q'): break cv2.destroyAllWindows()
ロボットのプログラム
import socket import ev3dev.ev3 as ev3 import time m_l = ev3.LargeMotor('outB') m_r = ev3.LargeMotor('outC') m_m = ev3.MediumMotor('outA') gs = ev3.GyroSensor() us = ev3.UltrasonicSensor() with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: s.bind(('ロボットのIPアドレスを指定', 50007)) s.listen(1) while True: # 誰かがアクセスしてきたら、コネクションとアドレスを入れる conn, addr = s.accept() with conn: data = conn.recv(1024) data = data.decode() print(data) if data == "0": m_l.run_forever(speed_sp=100) m_r.run_forever(speed_sp=100) elif data == "1": m_l.run_forever(speed_sp=-100) m_r.run_forever(speed_sp=-100) elif data == "4": m_l.stop() m_r.stop() elif data == "5": m_m.run_to_rel_pos(position_sp=1440, speed_sp=300, stop_action='hold') elif data == "6": m_m.run_to_rel_pos(position_sp=-1440, speed_sp=500, stop_action='hold') elif data == "8": m_l.run_forever(speed_sp=100) m_r.run_forever(speed_sp=100) while us.value() >= 60: time.sleep(0.1) m_l.stop() m_r.stop() elif "l " in data: try: angle = int(data[2:]) * -1 v = gs.value() m_l.run_forever(speed_sp=-100) m_r.run_forever(speed_sp=100) while (gs.value() - v) > angle: time.sleep(0.1) m_l.stop() m_r.stop() except: pass elif "r " in data: try: angle = int(data[2:]) v = gs.value() m_l.run_forever(speed_sp=100) m_r.run_forever(speed_sp=-100) while (gs.value() - v) < angle: time.sleep(0.1) m_l.stop() m_r.stop() except: pass
画像処理のプログラムではロボットについている緑と青の折り紙とコップの位置を表すピンクの折り紙を色で検出しすべて検出できた場合には緑と青の折り紙の間に線を引きその線の中心を求めます。
線の中心からピンクの折り紙の中心まで線を引き青の折り紙からピンクの折り紙の角度を求めます。
この時ピンクの折り紙がロボットの後ろにあるか前にあるかを判断して360度の角度を求めます。
そしてロボットに何度回ればいいかを伝えてロボットが指示どおりに動きます。
この後、前に進むプログラムを入れて次にロボットに命令が来るまで前に進みます。
これを2秒に一度実行します。
ロボットとコップの位置が近くなったらロボットが超音波センサーを使ってコップの距離を測定し、6㎝以下になったら、モーターを動かしてコップを持ち上げます。
ロボットが壊れるので持ち上げられるのは紙コップとプラコップのみです。
持ち上げた後は上のように角度を調節しながら走るプログラムのピンクの折り紙の座標をテンプレートマッチングで取得したカタツムリの絵の座標に入れ替えてカタツムリの絵のある場所まで持ってきてくれます。
ロボットのほうのプログラムはsocket通信を使って受け取った信号どおりに制御するプログラムです。
これを実際に動かすと
画面上に点と線が表示されてロボットが動きます。
実際に動いている写真を載せておきます。
