パラレルリンク機構と視覚情報処理を使ったロボット

パラレルリンク機構を使ったテーブルホッケーロボットを作りました。

色々改良して、もう少し速い球が返せるようになりました。
位置計測は30fps出るように高速化しましたが、それでも遅延があるので約0.1秒後の位置を予測するようにしました。このため斜めに入ってきても芯に当たります。
他、サーボの通信と画像認識の別スレッド化、稼働範囲制限、その他バグの修正。 pic.twitter.com/4CCCtbFhbN

— 山口辰久 (@qzy13700) December 4, 2021

元々は大きなホワイトボードに字や絵を描くロボットを構想していたのですが、その前段階として要素技術の検証のために小型のものを作りました。要素技術とは、

• パラレルリンク機構
• 画像センサーを用いたキャリブレーション
• ある程度高速な動作
• ロボット用サーボモーターの速度制御機能を使う

このうちメカと画像センサーの統合までできました。

メカニズム

ABSで滑り軸受を作りました。FDM方式3Dプリンタによる成形です。

根本の2個のモーターで駆動します。KONDOのロボット用サーボモーター5000シリーズを使ってみました。これはラジコン用サーボモーターと違い、出力軸がセレーションではなくＩカット形状になっていて、フレームを直接取り付けることができます。

エンドエフェクタ部分はある程度簡単に取り替えられるようにしました。なお、真ん中部分の白い点に見えるのはキャリブレーション時にマーカーを貼り付けるためのソフト粘着剤（商品名: ひっつき虫）です。

電装系

ROBO-剣用ロボットと同じく、PCからKONDO USB DUALアダプターを使用して直接ロボット用サーボモーターにコマンドを送ります。電源は12V ACアダプタまたは10.8(NiMH)または11.1V(LiPo)のバッテリーを使用。

ソフトウェア

カメラキャリブレーション

まずこれがなくては始まりません。

平面パターンを使ってカメラのレンズパラメータと地面平面に対するカメラ位置姿勢の推定を行います。

opencv.jpのサンプルプログラムをもとにしたキャリブレーションツールを自作して使っています。GitHubに、このサンプルプログラムをOpenCV2のAPIで書き換えたものを公開しています。

• yamaguchi-am/opencv_calibration

ロボット用サーボモーターとの通信

KONDOのUSB-DUALアダプターのICSモードを使い、仮想シリアルポートから直接サーボモーターと通信します。このライブラリはGitHubで公開しています。

• yamaguchi-am/kondo-ics

物体検出

オレンジ色のピンポン玉を使ったので、次のような処理としました。

1. 歪補正
2. 処理時間短縮のため画像を1/2に縮小
3. 色相と彩度で２値化
4. 連続領域ごとにわけ、輪郭抽出
5. 周長^2と面積の比が最も真円に近いものを選択
6. 楕円のあてはめにより中心を求める

動画作成時の環境では0.015秒程度で処理を完了できています。

また、この後地面平面との三角測量により３次元の物理座標を求めています。今回実験ではたまたまほぼ真上から見た映像のため紛らわしいのですが、カメラで得た画像内での座標を直接処理に使っているわけではありません。遠近法の効果が出るような斜め向きにカメラを設置してもプログラムに変更は必要ありません。カメラ位置を変更した際にはカメラキャリブレーションの部分で地面平面の位置と座標軸の原点＋方向を与え、この情報を使います。

ハンドアイ較正(hand-eye calibration)

カメラで計測したボールの位置に合わせてアームを動かすには、アームがどのような形状をしているのか、カメラとの相対的な位置関係はどうなっているのかの情報が必要です。事前に計測・推定しておく方法を取りました。

パラレルリンク機構の形状、リンクの繋がり方は予め与えますが、関節間の距離などは自動的に推定するようにします。

• 根本の固定関節の位置（２×２自由度）
• 各リンクの長さ（４）
• エンドエフェクタのリンク内相対位置（２）
• サーボトリム（２）

以上の１２変数を推定する非線形最適化問題となります。トルク０の状態にしたサーボモーターのポテンショメータを使い、複数の点で（２つの関節角度）と（実際のエンドエフェクタ位置）の組を取得します。上記１２パラメータを仮に与えれば、関節角度から順運動学計算によりエンドエフェクタ位置が計算できます。この２つの誤差を最小にするような解を求めます。cminpackによるLM法の実装を用いました。

なお、１２自由度としていますが、エンドエフェクタの位置を決めるものとしては冗長で複数の解が等価となるため、これらをあてはめた表示は必ずしも実際のロボットの形状と一致しません。

移動予測 

USBカメラからの画像取得について、OpenCVのcv::VideoCpatureを使用して通常のビデオデバイスとしてキャプチャしています。使用環境においてこの方法ではおよそ100ms程度の遅延があることがわかりました。仮に機械の動作が十分速いとしても、認識処理に最低これだけの遅延が発生することになります。転がってくるボールをうまく打ち返せるようにするため、等速直線運動を仮定して未来の位置を予測し、これをもとに動かすようにしました。

白色い丸印が現在（ここでは、今取得できた画像の意味）のボール位置を、黄色い丸印が予測位置を示しています。

移動予測のために、まず直前0.2秒間程度、時間と位置のペアを記憶しておきます（上の画像上で米印で表示）。これらの点に最小二乗法による時間関数の当てはめを行います（x,y,z各座標について独立）。そして関数の外挿（要するに、未来時刻を代入して関数の値を計算）を行い、予測位置とします。※この図では時間方向の情報が入っていないため不親切ですが、この軌道(x-y平面)に対して直線を当てはめているわけではありません。

ここで使用する関数を１次関数とすれば等速直線運動、２次関数とすれば等加速度運動を仮定することになります。平面上を転がるボールの運動で、しかも0.1秒程度の短時間の予測なので、１次関数としました。（なお、こちらは上記のハンドアイ系と異なり、線形の最適化問題なので疑似逆行列の計算１回でできます。）

しかしこれだけでは、ボールが壁で跳ね返ることを反映できません。予測位置が容易にフィールド外になってしまいます。そのためまず、予測位置が範囲外となる場合は予め設定した壁のところでY座標を折り返した位置を予測位置とします。

更に、関数当てはめに使用する過去データが壁での反発の前後両方を含む場合は、そのままでは最小二乗法によるフィッティングができません。これについてはまだ対処を行っておらず、妥当ではない予測値が出ています。但し、この状態は一瞬だけなのですぐ復帰します。

動作アルゴリズム

ボールを打ち返す動作実現のために、最低限単純なルールを設定しました。X軸は前後方向、Y軸は左右方向です。

• a) ボールが見つかった時
• 上記の方法により0.1秒後の位置を予測する。できた場合は以降「ボール位置」をそれで置き換える。
• a1) X座標(手前→奥の軸)が一定値より遠い側であれば、X座標を待機位置、Y座標はボール位置と同じ地点に最速で移動する。
• a2) 近い側であれば、ボール位置からラケット半径分手前側の位置へ最速で移動する。
• b) ボールが見つからない時
• 待機位置にゆっくり移動する

a2になるのは通常a1の直後を想定しています。a1によりボールを追いかけ、a2によりラケットを前へ急激に出して接触することで、ボールを打ち返す動作となります。インパクト時のラケットの速度は制御しておらず、これはまだ誤魔化しているところになります。

次は速度の制御をもう少し緻密にしてみたいと思います。