[ロボ実験記録] ロボットアームの先にusbカメラをつけてarマーカーで自己位置推定 (メモ)

Kan Hatakeyama

概要

ARマーカーを参照しながらピペットを動かすシステムを作った際のメモです｡
最終系は以下の感じです｡

構成

ロボットアーム(dobot magician)に3Dプリンタで作製したホルダを装着し､そこに電動ピペット(pipetty pro)とusbカメラを搭載します｡
カメラでARマーカを認識しながら､そこで得た座標をもとに試薬瓶を目指します｡

下準備

このあたりの関連記事を参照

流れ

実空間での作業を伴い､全てを説明すると非常に長くなるので､流れだけを記載します｡

キャリブレーション用のARマーカーの準備

6x8個のARマーカーを並べた画像を印刷します｡
以下のプログラムで､画像を出力します｡

出てきた画像をwordなどに貼り付けて､印刷します｡

ARマーカーのID=0が､ロボットアーム上での座標(300,-100)になるように紙を設置します｡

こんな感じです

色々と計算して､各ARマーカーのidとロボットアーム座標系での対応を紐づけた辞書を作ります｡

calibrator_dict=
{0: array([ 300., -100.]),
 1: array([ 276.55053406, -100.        ]),
 2: array([ 253.10106812, -100.        ]),
 3: array([ 229.65160217, -100.        ]),
 4: array([ 206.20213623, -100.        ]),
 5: array([ 182.75267029, -100.        ]),
 6: array([300.        , -77.86073957]),
 7: array([276.55053406, -77.86073957]),
...


ロボットアームの動作

関連コードはこちら｡
0805arm_camera_class.ipynbが実際の実装コードになります｡

はじめに､USBカメラのキャリブレーションファイルを作製しておきます｡

初期化関連
ラッパークラスを初期化していきます

from cv2 import aruco
from DobotDriver.DobotWrapper import DobotWrapper
import joblib

#dobotのラッパークラス
dobot=DobotWrapper("COM3")
dobot.initiate()

#USBカメラのラッパークラス
from utils.camera.USBCamera import USBCamera
from utils.camera.ArucoDetect import ArucoDetect
import cv2
import numpy as np
import time
camera=USBCamera(0)


#ARマーカーの位置データの読み込み
calibrator_dict=joblib.load("dat/calibrator_dict.pkl")
tip_offset=(-11,6,-20)
calibrator_dict={k:(v[0]+tip_offset[0],v[1]+tip_offset[1],
tip_offset[2]) for k,v in calibrator_dict.items()}

#試しにアームを動かしてみる
dobot.move_arm(*calibrator_dict[47],wait=True)
print(dobot.get_position())

#dobot + USBカメラの諸々の処理をまとめたクラス
from utils.camera.CameraHand import CameraHand

arm_camera=CameraHand(camera,dobot,
marker_length=1.5/2,mtx_path="dat/mtx.npy",dist_path="dat/dist.npy")
arm_camera.set_calibrator_dict(calibrator_dict)

最初のキャリブレーション
適当にアームを動かしていきます｡
その際のアーム座標 + arマーカーの相対座標を記録していきます｡
arm_camera.update()を実行すると､記録etcの処理が自動でなされます｡

#キャリブレーション用に適当に座標を設定
dobot_set_pos_list=[
(200,0,80),
(220,50,80),
(200,-50,0),
(250,-50,50),
]

#点を補完
interp_points = []
num_interp = 5
for i in range(len(dobot_set_pos_list) - 1):
    segment_points = []
    for j in range(3):   
        segment_points.append(np.linspace(dobot_set_pos_list[i][j], dobot_set_pos_list[i+1][j], num_interp).tolist())
    segment_points = np.transpose(segment_points)
    interp_points += segment_points.tolist()

#アーム移動して情報を取得
for set_pos in interp_points:
    dobot.move_arm(*set_pos,wait=True)
    time.sleep(1)
    arm_camera.update()


実行の様子

位置推定モデルの構築

カメラの画像から推定した座標をもとに､実際のARマーカーの座標を推定するモデルを作ります｡

arm_camera.fit_model(draw=True)

ここでの処理について､簡単に解説します｡

cv2のestimatePoseSingleMarkersという関数を用いると､usbカメラからARマーカーまでの並進ベクトルtvecを推定してくれます｡

以下の記事が参考になります｡

しかしながら､この推定には､なんだかんだで誤差が含まれます｡また､カメラの座標系とアーム座標系で座標軸が異なります｡
そこで､機械学習によって諸々を一気に補正する関数fMLを作ります｡

タスクは､
(実際の並進ベクトル)=(既知のカメラ座標pos_armとARマーカーの座標pos_ARの差分)=fML(t_vec,pos_arm)を満たすfMLを得ることです｡
(fMLの説明変数は､理論的にはt_vecだけで十分ですが､補正項としてpos_armも入れてみました)

理論的には､この変換は線形なので､一次の回帰式でOKなのですが､実際に回帰してみると､微妙に誤差が生じることがわかります｡
場所によっては､数mm程度の誤差が出てしまいます｡細かな作業をするには､しんどいレベルです｡

相対位置の推定結果: xは実測,yは予測. 色の違いはロボットアームのx,y,z軸の予測に対応

データを増やした状態で､回帰モデルにMLPを使ったりすると､少しは精度が向上しました｡

更にキャリブレーション
実際にARマーカーまでアームを動かしながら､キャリブレーションデータを増やします｡

import time
import random


arm_camera.calibrate = True

while True:

    #マーカーをランダムに選択
    target_id = random.randint(0,47)

    #アームを少し上に上げておく
    try:
        dobot.lift_arm(100)
    except:
        pass
    #アームを上げて､全体を俯瞰
    dobot.move_arm(200,0,150)

    print(target_id)
    time.sleep(0.1)
    arm_camera.update()

    #指定したARマーカーに移動
    arm_camera.move_to_target_arco(target_id,z_limit=-20)

    time.sleep(0.1)

    #モデル更新
    arm_camera.fit_model(draw=False)

本番

最後は校正用のARシート(座標既知)を外して､実際に追従させたいARマーカー(座標未知)のみを使います｡

#キャリブレーションはoff
arm_camera.calibrate = False

while True:

    #ID=0を追従させる
    target_id =0

    #アームを少し上に上げておく
    try:
        dobot.lift_arm(100)
    except:
        pass
    #アームを上げて､全体を俯瞰
    dobot.move_arm(200,0,150)

    print(target_id)
    time.sleep(0.1)
    arm_camera.update()

    #指定したARマーカーに移動
    arm_camera.move_to_target_arco(target_id,z_limit=-20)

    time.sleep(0.1)

    #モデル更新はしない
    #arm_camera.fit_model(draw=False)

注意点

動画ではピペットの先端がサンプル瓶の中に入っています｡
しかし､これは偶然です｡
本来は､ARマーカーの右上を狙うようにプログラムされているのですが､種々の誤差が積み重なり､たまたま､ARマーカーではなくサンプル瓶の中にピペットの先端が入りました｡

実際､サンプル瓶の先端の直径は10 mm程度であるのに対し､今回のプログラムの位置推定では10 mmを超える予測誤差が出てしまっています｡

(再掲) 相対位置の推定結果: xは実測,yは予測.

誤差要因として､ARマーカーの認識精度､校正に使ったAR用紙のたわみ､カメラのガタツキ､キャリブレーション時のARマーカーの高さのバリエーションが少ない､などが考えられます｡

誤差を減らすのは結構､難しそうです｡
ARマーカーは､ざっくりとした位置推定に使うにとどめ､
その上で､画像認識による微修正を加えた方が良いかもしれません｡

カメラ画像を参照しながら､穴の中にピペットを入れるアルゴリズムが必要そうです


そのためには､サンプル瓶の上部を検出する必要があります｡
ただ､瓶がガラスで輪郭がぼやけていることもあり､わりと難易度が高そうです｡普通の輪郭抽出は期待薄でした｡
yoloなどのdeep learningが必要かもしれません｡

適当に輪郭抽出した結果｡赤い波線部を検出したい｡

まとめ

ARマーカーで位置推定をしながら､ピペットを動かしました｡
ただし､ミリメートルレベルの制御には､少し工夫が必要そうです｡

いいなと思ったら応援しよう！