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

概要

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マーカーで位置推定をしながら、ピペットを動かしました。
ただし、ミリメートルレベルの制御には、少し工夫が必要そうです。

この記事が気に入ったらサポートをしてみませんか?