JP2005125478A - 複数ロボット間の相対位置計測方法 - Google Patents

Abstract

【課題】ロボットのツール先端同士を突き合わせる必要がないとともに、測定装置のパラメータの同定が不要で、さらに測定装置のコストが小さい複数ロボット間の相対位置計測システムを提供する。
【解決手段】複数のロボットの可動部の特徴点を少なくとも３本の直線３０１上で動作させて、各直線３０１上の少なくとも２点３０２ａ，３０２ｂにおける前記特徴点のロボット座標値３０３ａ，３０３ｂを測定するとともに、各直線３０１上の特定点０cから前記特徴点までの距離または距離と相関関係を持つ物理量を測定し、各直線・各ロボットについて得られた測定値を基に複数ロボット間の相対位置関係を計算するものである。
【選択図】図４

Description

本発明は、複数台のロボットが協調して作業する際に、ロボット間の相対位置関係を計測する方法に関するものである。

２台のロボットを向い合せに配置して、一方のロボットでワークを把持して、他方のロボットにアーク溶接用トーチを持たせて、前記ワークに対してアーク溶接作業を行う装置が知られている。また、重量物のハンドリングや自動車ボディのスポット溶接など、複数台のロボットが協調して作業する事例も増加している。
このように複数台のロボットが協調作業する際には、ロボット間の相対位置関係（ロボットベース座標同士の位置と姿勢）を予め取得し、ロボットの制御装置にその相対位置関係を設定しておく必要がある。このための相対位置を求める方法が各種提案されている。
例えば、特許文献１には、一方のロボットの回転部上の１点に対して他方のロボットのツール先端を突き合わせ、その時の教示データを求めることを、前記回転部の角度を違えて３回繰り返して得られた、３通りの教示データに基づいて、前記他方のロボットの座標系からみた前記一方のロボットの座標系の相対位置を求める方法が開示されている。
また、特許文献２には、一方のロボットの手首フランジに力覚センサと面状ツールを取り付け、他方のロボットの手首フランジに針状ツールを取り付け、直線上にない３点において面状ツールと針状ツールを接触させて、一方のロボットのベース座標系における各接触点の位置と、他方のロボットのベース座標系における針状ツールの位置により、ロボット間の相対位置を求める方法が開示されている。
また、非特許文献１には、２台のロボットのツール先端にマーカを取り付け、適当な場所に２台のカメラを配置し、各々のロボットを動かしたときの複数の異なるマーカ位置を２台のカメラで測定することによって、ロボット間の相対位置関係を計算する方式が提案されている。この方式は、基本的にはカメラのステレオ視の原理（非特許文献２の第４章参照）を利用している。
特開平６−２７８０６３号公報（第９頁、図１、図３、図５） 特開平５−１１１８９７号公報（第６頁、図１） 菊地ほか，「"Plug & Produce"のためのロボット間キャリブレーション」，第１９回日本ロボット学会学術講演会予稿集，pp.1065-1066，2001 出口光一郎，「ロボットビジョンの基礎」，コロナ社，2000年7月12日

しかしながら、特許文献１あるいは特許文献２の方法では、ロボットのツール同士を突き合わせているので、ツールやロボットを破損させる危険性があり、安全上の問題があった。また、ロボットを破損させないように動作させるため、作業者にストレスを与えるという問題もあった。
非特許文献１の方式は、カメラによりマーカを観測するのでロボットのツール先端同士を突き合わせる必要がないという利点があるが、ステレオ視の原理に基づいているので２台のカメラが必要になり、測定装置のコストが増加するという問題がある。また、カメラパラメータの同定（キャリブレーション）が必要になるという問題もある。
本発明はこのような問題点に鑑みてなされたものであり、ロボットのツール先端同士を突き合わせる必要がないとともに、測定装置のパラメータの同定が不要で、さらに測定装置のコストが小さい複数ロボット間の相対位置計測システムを提供することを目的とする。

上記問題を解決するために、請求項１の発明は、複数のロボットの可動部の特徴点を少なくとも３本の直線上で動作させて、前記各直線上の少なくとも２点における前記特徴点のロボット座標値を測定するとともに、前記各直線上の特定点から前記特徴点までの距離または距離と相関関係を持つ物理量を測定し、各直線・各ロボットについて得られた測定値を基に複数ロボット間の相対位置関係を計算するものである。

また、請求項２の発明は、前記特徴点を撮像装置で撮像して、その画像を画像表示装置に表示して、前記画像表示装置上の前記特徴点の画像が、前記画像表示装置上に定義された基準点と一致するように前記ロボットを操作して、前記特徴点を直線上で動作させるものである。

また、請求項３の発明は、前記ロボットの可動部にターゲット物体を設け、前記ターゲット物体に前記特徴点を定義し、前記画像表示装置上に表示された前記ターゲット物体のイメージの面積または明るさを前記距離と相関関係を持つ物理量とすることを特徴とするものである。

また、請求項４の発明は、前記ロボットの可動部にターゲット物体を設け、前記ターゲット物体に前記特徴点を定義するとともに、前記撮像装置にズームレンズを取り付け、前記画像表示装置に映し出された前記ターゲット物体のイメージの面積が一定となるように前記ズームレンズを移動させ、そのときの前記ズームレンズの移動量を前記距離と相関関係を持つ物理量とするものである。

また、請求項５の発明は、前記ターゲット物体を発光させるものである。

また、請求項６の発明は、大きさまたは明るさの異なる複数のターゲット物体をロボットの可動部に設け、前記撮像装置からの距離に応じて前記ターゲット物体を切り替えるものである。

また、請求項７の発明は、レーザ発振装置から発振するレーザ光に沿って、前記ロボットの可動部の特徴点を直線動作させるものである。

また、請求項８の発明は、ロボットの可動部にターゲット物体を設け、前記ターゲット物体に前記特徴点を定義し、前記レーザ光を前記ターゲット物体に照射して、前記レーザ光の光点が前記特徴点に一致するように前記ロボットを操作して、前記特徴点を直線動作させるものである。

また、請求項９の発明は、撮像装置の光軸とレーザ発振装置のレーザ光軸が平行となるように、前記撮像装置と前記レーザ発信装置を一体にした装置を使用するものである。

また、請求項１０の発明は、細長い棒状ジグに沿って、前記ロボットの可動部の特徴点を直線動作させるものである。

また、請求項１１の発明は、前記棒状ジグに距離目盛を付し、その距離目盛を使って、前記特徴点までの距離を測定するものである。

また、請求項１２の発明は、前記ロボットそれぞれ対して２つ以上の平行な平面を定義し、前記特徴点を前記各平面上に拘束させて前記各直線と前記各平面との交点に前記特徴点を位置決めし、前記各交点における前記特徴点のロボット座標値を測定するものである。

また、請求項１３の発明は、前記各交点における前記特徴点のロボット座標値をもとに、前記距離と相関関係を持つ物理量を測定するものである。

また、請求項１４の発明は、前記２つ以上の平面のうち、ある平面上の前記交点間の距離と他の平面上の前記交点間の距離の比を、前記距離と相関関係を持つ物理量とするものである。

請求項１〜６に記載の発明によると、カメラ１台の撮像平面の基準点にロボットツール上ターゲットの特徴点が一致するようにロボットを動かすことによって特徴点を直線上に拘束することができ、また、撮像平面上でのターゲットイメージの面積または明るさを測定することによってターゲットまでの距離を推定することができるので、低コストでロボットツールの先端同士を突き合わせることなくロボット間の相対位置関係を計測できる。さらに、カメラパラメータを同定することなくロボット間の相対位置関係を計測できる。
また、請求項７〜９に記載の発明によると、レーザ距離センサのレーザ光線がロボットツール上ターゲットの特徴点に照射されるようにロボットを動かすことによって特徴点を直線上に拘束することができ、また、レーザ距離センサを利用して直接ターゲットまでの距離を測定することができるので、ロボットツールの先端同士を突き合わせることなくロボット間の相対位置関係を正確に計測できる。
また、請求項１０〜１１に記載の発明によると、細長い棒状ジグを利用してロボットツールの特徴点を直線上に拘束するので、ロボットツールの先端同士を突き合わせることなくロボット間の相対位置関係を手軽に計測できる。
また、請求項１２〜１４に記載の発明によると、カメラ１台の撮像平面の基準点にロボットツール上マーカの特徴点が一致するようにロボットを動かし、さらに予め設定した平面上に特徴点が拘束されるようにロボットを動かすことによって、各平面と各直線との交点に特徴点を位置決めし、そこでの特徴点のロボット座標値を測定することによって、ロボット座標値だけを使ってターゲット（特徴点）までの距離を推定することができるので、低コストでロボットツールの先端同士を突き合わせることなくロボット間の相対位置関係を正確に計測できる。さらに、カメラパラメータを同定することなくロボット間の相対位置関係を計測できる。

以下、本発明の具体的実施例を図に基づいて説明する。

図１は、本発明の第1実施例を示す相対位置計測装置の構成図である。図において、１０１ａおよび１０１ｂは相対位置計測の対象の第1のロボットおよび第２のロボットである。１０２ａ，１０２ｂは第１および第２のロボットに固定されたベース座標系である。１０３ａ，１０３ｂは第１および第２のロボットの制御装置であり、１０４ａ，１０４ｂは第１および第２のロボットの操作インターフェイスである。また、１０５ａ，１０５ｂは第１および第２のロボットのツール先端に取り付けた球状ターゲットである。ここで、球状ターゲット１０５ａ，１０５ｂの幾何学的重心（球の中心）をこのターゲットの特徴点と定義する。
１０６は撮像装置（ＣＣＤカメラ）である。１０７は撮像装置１０６が出力するビデオ信号を処理する画像処理装置であり、１０８は画像処理装置１０７が出力した画像データを作業者に提示（表示）する画像表示装置である。１０９は、第１および第２のロボットの制御装置１０２ａ，１０２ｂと画像処理装置１０７から得られるデータをもとにロボット間の相対位置関係（ベース座標系１０２ａと１０２ｂの位置関係）を演算する相対位置演算装置である。また、１１０は撮像装置１０６の支持部である。
なお、撮像装置１０６の設置場所は、第１のロボット１０１ａおよび第２のロボット１０１ｂの球状ターゲット１０５ａ，１０５ｂを共に撮影できる場所であれば何処でもよく、その位置を測定する必要はなく（未知でよい）、撮像装置１０６について予め何らかのキャリブレーションをする必要もない。

図２は、本発明の第1実施例を示す画像表示の説明図である。図において、２０１は画像表示装置１０８に表示される撮像平面であり、２０２は撮像平面２０１の座標系ΣI（U軸、V軸、W軸）であり、２０３は撮像装置１０６のカメラ座標系Σc（Xc軸、Yc軸、Zc軸）である。カメラ座標系２０３のZc軸は撮像装置１０６の光軸に一致し、撮像面２０１に対して垂直になっている（W軸とZc軸は一致する）。ここで、カメラ座標系２０３の原点から撮像平面２０１までの距離fはレンズの焦点距離に一致する。２０４は撮像平面２０１に結像された球状ターゲット１０５ａのイメージ（像）である。２０５はイメージ２０４の重心であり、画像処理装置１０７によってその座標値が計算され、イメージ２０４上にオーバレイされる。例えば、画像処理装置１０７は、撮像装置１０６からのビデオ信号を２値化処理して円状領域を抽出し、抽出された領域の重心位置を計算する。２０６は、作業者が指定する撮像平面上の基準点であり、画像処理装置１０７によって基準点２０６のマーカを撮像平面２０１上にオーバレイする。ここで、撮像平面２０１における基準点２０６の座標値を(ua,va)と表す。
作業者は、画像表示装置１０８により撮像平面２０１の様子を目視しながら、イメージ２０４の重心２０５が基準点２０６に一致するように、操作インターフェイス１０４ａを用いて第１のロボット１０１ａの球状ターゲット１０５ａを移動させる。あるいは、画像ベースの視覚サーボ技術（非特許文献２の第６章参照）を適用し、画像処理装置１０７において基準点２０６と重心２０５間の撮像平面２０１上での位置偏差を画像ヤコビアン行列によりロボット座標系での微小位置変化量に変換し、その変化量を第１のロボット１０１ａの制御装置１０３ａに与えて位置フィードバックを構成することにより、基準点２０６と重心２０５が一致するように第１のロボット１０１ａの球状ターゲット１０５ａを自動で移動させることもできる。

以上の操作により、撮像平面２０１上において重心２０５と基準点２０６が一致した状態を図３に示す。この状態では、第１のロボットの球状ターゲット１０５ａの特徴点（球の中心）は、カメラ座標系２０３の原点Ｏcと基準点２０６を通る直線３０１上の位置３０２ａにある（ターゲットの形状、レンズの結像原理、カメラの撮像原理から明らか）。
この状態で、作業者は、第１のロボットのベース座標系１０２ａを基準にした特徴点３０２ａへの位置ベクトル３０３ａ（ ^r1 P_{t1_a1} ）を測定する。一般的な産業用ロボットでは、第１のロボット１０１ａの各関節軸に組み込まれたエンコーダ（内界センサ）値から順キネマティクス演算によりＴＣＰ位置が制御装置１０３ａで計算され、その結果を操作インターフェイス１０４ａ上で確認することができる。この一般的な機能を利用して位置ベクトル３０３ａを測定すればよい。また、画像処理装置１０７と画像表示装置１０８を利用して、撮像平面２０１上のイメージ２０４の面積（ S _{t1_a1} ）を測定する。画像処理装置１０７内で抽出された円状領域の面積を自動計算しても良いし、画像平面２０１上に手動カーソルをオーバレイさせて画像表示装置１０８上でカーソルから面積を読み取っても良い。

つぎに、図４に示すように、作業者は第１のロボット１０１ａの操作インターフェイス１０４ａを用いて球状ターゲット１０５ａを撮像平面２０１に対して奥行き方向（Zc軸方向）に移動させた上で、再度、イメージ２０４の重心２０５が基準点２０６に一致するように球状ターゲット１０５ａを移動させる。このとき球状ターゲット１０５ａの特徴点は、直線３０１上ではあるが図３に示した３０２ａとは異なる点にある。図４における特徴点位置を３０２ｂで表す。図３のときと同様に、作業者は、第１のロボットの座標系１０２ａを基準にした特徴点３０２ｂへの位置ベクトル３０３ｂ（ ^r1 P_{t1_a2} ）とイメージ２０４の面積（ S _{t1_a2} ）を測定する。
つぎに、撮像平面２０１上で基準点２０６とは異なる第２の基準点および第３の基準点を定義し、基準点２０６の場合と同様に、カメラ座標系２０３の原点０cと前記第２および第３の基準点を結ぶ各直線上に球状ターゲット１０５ａの特徴点を位置決めして、特徴点の位置ベクトルおよび球状ターゲット１０５ａの画像の面積を求める。すなわち、前記第２の基準点について、位置ベクトル ^r1 P_{t1_b1} および ^r1 P_{t1_b2} 、面積 S_{t1_b1}および S _{t1_b2} を、前記第３の基準点について、位置ベクトル ^r1 P_{t1_c1} および ^r1 P_{t1_c2} 、面積 S_{t1_c1} および S _{t1_c2} を求める。
そして、第２のロボット１０１ｂについても、同様の測定処理を行う（ロボットが３台以上ある場合についても同様）。
なお、各ロボットの各直線上に対する上述した位置ベクトルとターゲット面積の測定順序は、上記の説明にとらわれるものではなく、任意の順序で測定してもよい。

次に、上述した測定データに基づいて、第１のロボット１０１ａと第２のロボット１０１ｂ間の相対位置を求める手順を説明する。
直線３０１（図３、図４参照）上に第１のロボット１０１ａの球状ターゲット１０５ａの中心（特徴点）を置いたときに、次式が成り立つ。

^c P_OcOr1 ＋ ^c R_r1 ・^r1 P_{t1_a1}＝ w _{t1_a1} ( ua, va, f )^T (1)

^c P_OcOr1＋ ^c R _r1・^r1 P_{t1_a2} ＝ w _{t1_a2}( ua, va, f ) ^T (2)

ここで、^c P_OcOr1 は、カメラ座標系２０３の原点Ocから第１のロボット１０１ａのベース座標系１０２ａの原点Or1までの位置ベクトル（サイズ３×１）をカメラ座標系２０３を基準にして表したものである（左上の添え字は基準とする座標系を表す）。^c R _r1 は、第１のロボット１０１ａのベース座標系１０２ａからカメラ座標系２０３への回転行列（サイズ３×３）である。( ua, va, f ) ^T は、カメラ座標系２０３の原点Ocから撮像平面２０１上の基準点２０６までの位置ベクトルである（カメラ座標系２０３基準）。また、w_{t1_a1}とw_{t1_a2}は透視変換におけるスケールファクタであり、それぞれ、カメラ座標系２０３の原点Ocから特徴点３０２ａ，３０２ｂまでの距離に比例する正数になる。^r1 P_{t1_a1} と^r1 P_{t1_a2} は、それぞれ、第１のロボット１０１ａのベース座標系１０２ａの原点Or1から特徴点３０２ａ，３０２ｂまでの位置ベクトル（ベース座標系１０２ａ）であり、上述した測定プロセスで直接得られるデータである。

式(2)の両辺から式(1)の両辺を引くと次式が成り立つ。

^c R _r1 ・^r1 ΔP_{t1_a}＝ Δw _{t1_a} ( ua, va, f ) ^T (3)

ただし、式(3)において、

^r1 ΔP_{t1_a} ＝ ^r1 P_{t1_a2}− ^r1 P_{t1_a1} (4)

Δw_{t1_a} ＝ w _{t1_a2} − w _{t1_a1} (5)

である。また、式(3)には、つぎの拘束条件がつく。

|^r1 ΔP_{t1_a}| ＝ |Δw_{t1_a}| sqrt(ua ² ＋va ² ＋f ² ) (6)

第２のロボット１０１ｂについても、同様にして次式が成り立つ。

^c R _r2 ・^r2 ΔP_{t2_a}＝ Δw _{t2_a} ( ua, va, f ) ^T (7)

|^r2 ΔP_{t2_a}| ＝ |Δw_{t2_a}| sqrt(ua ² ＋va ² ＋f ² ) (8)

なお、^r2 ΔP_{t2_a}を求めるために測定される位置ベクトル^r2 P_{t2_a1}および^r2 P_{t2_a2}は、第１のロボット１０１ａに対して測定された特徴点３０２ａ，３０２ｂの位置ベクトル^r1 P_{t1_a1}および^r1 P_{t1_a2}に一致していなくても良い。
式(3)(7)より、( ua, va, f ) ^T を消去し、かつ式(6)(8)の拘束を利用して次式を得る。

^r1 R _r2 ・^r2 ΔP_{t2_a}＝ (Δw _{t2_a} / Δw_{t1_a}) ^r1 ΔP_{t1_a} (9)

|Δw_{t2_a}| / |Δw _{t1_a}| ＝ |^r2 ΔP_{t2_a}| / |^r1 ΔP_{t1_a}| (10)

ここで、Δw _{t2_a}とΔw _{t1_a}の符号は、直線３０１においてロボットのTCPを撮像装置に対して手前に動かしたか、奥に動かしたかによって（スケールファクタの大小が決まるので）判断できる。よって、式(9)右辺の(Δw_{t2_a}/ Δw _{t1_a})は、式(10)とそれぞれの符号から計算できる。
カメラ座標系２０３の原点０cと前記第２および第３の基準点を結ぶ各直線上についても同様にして、式(9)(10)に該当する式を導出することによって、つぎの連立方程式が得られる。

^r1 R _r2・( ^r2 ΔP_{t2_a}, ^r2 ΔP_{t2_b}, ^r2 ΔP_{t2_c} ) ＝
( α ^r1 ΔP_{t1_a}, β ^r1 ΔP_{t1_b} , γ ^r1 ΔP_{t1_c} ) (11)

ただし、係数α,β,γは

α ＝ Δw_{t2_a} / Δw _{t1_a} (12a)
β ＝ Δw _{t2_b} / Δw _{t1_b} (12b)
γ ＝ Δw _{t2_c} / Δw _{t1_c} (12c)

である。

式(11)の両辺に、右から( ^r2 ΔP_{t2_a} , ^r2 ΔP_{t2_b}, ^r2 ΔP_{t2_c} )の逆行列をかける（連立方程式を解く）ことによって、第１のロボット１０１ａのベース座標系１０２ａと第２のロボット１０１ｂのベース座標系１０２ｂ間の姿勢を表す回転行列^r1 R _r2が計算できる。( ^r2 ΔP_{t2_a} , ^r2 ΔP_{t2_b}, ^r2 ΔP_{t2_c} )の逆行列は、各直線（基準点）が独立であれば必ず存在する。また、計測を行う直線（基準点）の数を４本以上にして、連立方程式(11)を冗長にすることにより、測定誤差によるバラツキの最も小さい回転行列を求めることもできる（最小二乗法に等価）。

つぎに、第１のロボット１０１ａのベース座標系１０２ａの原点Or1と第２のロボット１０１ｂのベース座標系１０２ｂの原点Or2間の位置ベクトルについて、その導出方法を説明する。
式(1)(2)の両辺に左から^r1 R _cをかけると次式が成り立つ。

^r1 P_OcOr1 ＋ ^r1 P_{t1_a1}＝ w _{t1_a1} ^r1 R _c ( ua, va, f ) ^T (13)

^r1 P_OcOr1 ＋ ^r1 P_{t1_a2}＝ w _{t1_a2} ^r1 R _c ( ua, va, f ) ^T (14)

式(13)(14)より、^r1 R _c ( ua, va, f ) ^Tを消去すると次式が成り立つ。

^r1 P_OcOr1 ＋ ^r1 P_{t1_a1}＝ (w _{t1_a1}/ w _{t1_a2}) ( ^r1 P_OcOr1 ＋ ^r1 P_{t1_a2}) (15)

式(15)は次式のように変形できる。
^r1 P_OcOr1 ＝ (w _{t1_a1} / w _{t1_a2})/(1 − w_{t1_a1} / w _{t1_a2}) ^r1 P_{t1_a2}
− 1 / (1 − w _{t1_a1}/ w_{t1_a2}) ^r1 P_{t1_a1} (16)

ここで、w _{t1_a1} / w _{t1_a2} はスケールファクタの比であるから、カメラ座標系２０３の原点Ocから特徴点３０２ａ（^r1 P_{t1_a1}の終点）および３０２ｂ（^r1 P_{t1_a2}の終点）までの距離比に一致する。透視変換の幾何学から、撮像平面２０１上のイメージ２０４の面積は、カメラ座標系２０３の原点Ocから球状ターゲット１０５ａの特徴点（球の中心）までの距離の二乗に反比例する。したがって、以下の近似が成り立つ。

w_{t1_a1} / w _{t1_a2} ≒ sqrt( S _{t1_a2} / S _{t1_a1}) (17)

第２のロボット１０１ｂについても同様にして、次式が成り立つ。

^r2 P_OcOr2＝ (w _{t2_a1} / w _{t2_a2})/(1 − w _{t2_a1} / w_{t2_a2}) ^r2 P_{t2_a2}
− 1 / (1 − w_{t2_a1} / w _{t2_a2}) ^r2 P_{t2_a1} (18)

w _{t2_a1} / w_{t2_a2} ≒ sqrt( S _{t2_a2} / S _{t2_a1} ) (19)

周囲の照明状態によっては、ターゲットイメージの面積を正確に測定できなくなる場合がある。そのような場合には、ターゲットを直接あるいは間接的に発光させ、周囲の照明状態に依存せずに面積が測定できるようにすればよい。このとき、ある特定の波長の光で発光させ、撮像装置の方でその波長の光以外をフィルタリングするようにすれば一層効果的である。
結局、式(16)(18)で計算される位置ベクトル^r1 P_OcOr1と^r2 P_OcOr2 、および回転行列^r1 R_r2から、第１のロボット１０１ａのベース座標系１０２ａの原点Or1と第２のロボット１０１ｂのベース座標系１０２bの原点Or2間の位置ベクトルは、次式により計算される。

^r1 P_Or1Or2＝ ^r1 R_r2・^r2 P_OcOr2 − ^r1 P_OcOr1 (20)

また、前記第２の基準点、第３の基準点に対しても同様の手順で^r1 P_Or1Or2を求め、それらの平均（重心）を最終的な位置ベクトルとしても良い。
また、奥行き方向に球状ターゲット１０５aを移動させたときのイメージ２０４の面積変化を測定してターゲットまでの距離比（スケールファクタの比）を求める（式(17)(19)）代わりに、撮像装置１０６にズームレンズを取り付け、イメージ２０４の面積が一定となるようにズームレンズを移動させ、その移動量からターゲットまでの距離比を求めても良い。
また、撮像平面上でターゲットイメージの面積を測定する代わりに、ターゲットイメージの明るさ（光量）を測定することによって、式(17)(19)の距離比を推定しても良い。

図５は本発明の第１実施例の変形例を示すロボットの説明図である。この変形例は第１のロボット１０１ａのツール先端に大小２個の球状ターゲット１０５ａを取り付けたことを特徴とするものである。球状ターゲットの大きさを変える代わりに、ターゲットの発光の明るさを異ならせても良い。
図５に示すようにロボットのツール部分に大きさ（あるいは明るさ）の異なる複数のターゲット物体を取りつけると、撮像装置１０６からの距離が遠くなるほど大きい（明るい）ターゲットに切り替えることよって、距離比の推定精度を高めることができる。ただし、式(17)(19)によって距離比を計算する際に、ターゲットの大きさ（明るさ）の相違を考慮する必要がある。例えば、式(17)の S_{t1_a2} を測定するときに、S_{t1_a1} を測定するときに使用したターゲットのk倍の大きさのターゲットを使用したとすると、次式によって距離比を推定する。

w _{t1_a1} / w_{t1_a2} ≒ sqrt( S _{t1_a2} / k²×S _{t1_a1} ) (21)

以上説明したように、第１実施例によれば、カメラ１台の撮像平面の基準点にロボットツールの特徴点が一致するようにロボットを動かすことによって特徴点を直線上に拘束することができ、また、撮像平面上でのターゲットの面積比（明るさの比）を測定することによってターゲットまでの距離比を推定することができるので、低コストでロボットツールの先端同士を突き合わせることなくロボット間の相対位置関係を計測できるのである。また、カメラパラメータを同定することなく、ロボット間の相対位置関係を計測できるのである。ターゲット物体を発光させることによってターゲット面積を周囲の照明状態に依存せずに測定でき、さらに、撮像装置からの距離が遠くなるほど大きい（明るい）ターゲットに切り替えることによって距離比の推定精度を高めることができるので、精度よくロボット間の相対位置関係を計測できるのである。

図６は、本発明の第２実施例を示す相対位置計測装置の構成図である。図において、５０１はレーザ距離センサのレーザ発振装置である。５０２はレーザ発振装置の支持部であり、２自由度（パンとチルト）の回転機構を有する。５０３はレーザ距離センサの距離演算部である。なお、第１実施例と共通する構成要素には同一の符号を付したので説明を省略する。
前述した第１実施例では、カメラの撮像平面２０１においてイメージ２０４の重心２０５を撮像平面上の基準点２０６に一致させることによって、ターゲットの特徴点を直線上に拘束している。それに対して、第２実施例では、レーザ発振装置５０１から発振されるレーザ光線を利用して、ターゲットの特徴点を直線上に拘束する。すなわち、直進するレーザ光線そのものを、ターゲットの特徴点を拘束すべき直線とするものである。
第２実施例では、カメラ（撮像装置１０６）でロボットツール先端の球状ターゲットの近傍を撮影し、画像表示装置１０８に提示される撮像平面を目視しながらターゲットイメージの重心とレーザ光スポットが一致するように、作業者は操作インターフェイス１０４ａ，１０４ｂを利用してロボットのターゲットを移動させる（実施例１と同様にしてこの処理を自動化しても良い）。このようにすることによって、ターゲットの特徴点（球の中心）を直線（レーザ光線）上に拘束できる。

図６では、撮像装置１０６（カメラ）とレーザ発振装置５０１を別々に設置しているが、両者をジグで一体化させても良い。図７は撮像装置とレーザ発振装置を一体にまとめた装置の説明図である。この装置は撮像装置１０６の光軸とレーザ発振装置５０１のレーザ光軸が平行になるように、上下に重ねて支持部５０２に固定することによって、ロボットツール先端の球状ターゲットへのレーザ光照射が容易にできる。すなわち、カメラのズームを調節しながら、撮像平面２０１の中心付近にターゲットが映し出されるようにロボットを移動させればよい。なお、図中の６０１は撮像平面２０１上のレーザ光の画像である。

第２実施例においても、第１実施例と同様に、ある直線上の２点において、ロボット座標系原点から球状ターゲットの特徴点までの位置ベクトルをロボットの内界センサにより測定する。同時にレーザ距離センサの距離演算部５０３により、直線に沿った特徴点までの距離を測定する（第１の実施例では、直接距離は測定せず、距離と相関を持つターゲットイメージの面積を測定した）。
３本以上の直線に対して、各ロボットについて、同様にして位置ベクトルと距離を測定する。直線の向きを変更するには、支持部５０２のパン軸とチルト軸を回転させればよい。

以上の測定データからロボット間の相対位置関係を導出する原理は、第１の実施例とほとんど同じであるが、媒介パラメータの記述が若干異なる。
３本の直線を直線A, B, Cと表し、直線A上に拘束されたロボット１のターゲット特徴点までの位置ベクトルを同様に^r1 P_{t1_a1}および^r1 P_{t1_a2}と表す。ここで、直線Aの（単位）方向ベクトルをLa、レーザ距離センサで測定した^r1 P_{t1_a1}の終点および^r1 P_{t1_a2}の終点までの距離をそれぞれd _{t1_a1}, d _{t1_a2}と表すと、式(1)〜(21)の所々で記述されている ( ua, va, f ) ^T をLaに、w _{t1_a1}をd _{t1_a1}に、w_{t1_a2}をd_{t1_a2}に置き換えて同様に相対位置を導出すればよい。ただし、第２実施例では、式(6)(8)(10)(17)(19)(21)は不要になる。なぜなら、w_{t1_a1}やw_{t1_a2}と異なり、d _{t1_a1}やd _{t1_a2}はレーザ距離センサで直接正確に測定可能な物理量であるからである。

以上説明したように、第２実施例によれば、レーザ距離センサのレーザ光線がロボットツールの特徴点に照射されるようにロボットを動かすことによって特徴点を直線上に拘束することができ、また、レーザ距離センサを利用して直接ターゲットまでの距離を正確に測定することができるので、ロボットツールの先端同士を突き合わせることなくロボット間の相対位置関係を正確に計測できるのである。

図８は、本発明の第３実施例を示す相対位置計測装置の構成図である。図において、７０１は棒状ジグであり、その表面には距離目盛がつけられている。７０２ａおよび７０２ｂは棒状ジグ７０１の支持部である。支持部７０２ａ，７０２ｂには、直動ジョイント７０３ａ，７０３ｂおよびユニバーサルジョイント７０４ａ，７０４ｂが付けられており、棒状ジグ７０１の方向を自由に変更して、任意の方向に固定できる。
この第３実施例では、ロボットツールとして球状ターゲットではなく、針状ツール７０５ａ，７０５ｂを取り付ける。そして、針状ツール７０５ａ，７０５ｂの先端を棒状ジグ７０１上に拘束する（突き合わせる）ことによって、ロボットツールの特徴点（先端）を直線上に拘束し、そのときのツール先端位置ベクトルをロボットの内界センサにより計測する。また、針状ツール７０５ａ，７０５ｂの先端を棒状ジグ７０１上に拘束した状態で距離目盛を読み取ることにより、棒状ジグ７０１の端点からの距離を測定する。
３本以上の直線（棒状ジグの配置を変える）に対して、各ロボットについて、同様にして位置ベクトルと距離を測定する。また、測定したデータからロボット間相対位置を導出原理は、第２実施例で説明した通りである。
以上説明したように、第３実施例では、細長い棒状ジグ７０１を利用してロボットツールの特徴点を直線上に拘束するので、ロボットツールの先端同士を突き合わせることなく、高価な装置を使用しなくてもロボット間の相対位置関係を手軽に計測できるのである。

図９は、本発明の第４実施例の相対位置計測方式の原理を示している。同図において、９０１は第１のロボット１０１ａのツール先端に取り付けられたマーカ（目印）である。実施例１および実施例２では、ロボットのツール先端に球状ターゲット（１０５ａ，１０５ｂ）を取り付けたが、実施例４におけるマーカ９０１は球状ターゲットほどの大きさを必要としない。蛍光塗料が塗られた反射型マーカ、ＬＥＤマーカ、赤外線マーカなどを使用する。実施例４において相対位置計測に用いる装置の構成は、マーカ以外は図１と同じである（図１の装置構成において球状ターゲットをマーカに置き換える）。
図９において、９０２はロボット１０１ａのベース座標系１０２ａの原点Or1，９０３はカメラ座標系２０３の原点Ocである。９０４ａ，９０４ｂはロボット１０１ａに対して設定された平行な２つの平面であり、それぞれ平面G，平面Hと表す。２０６は、撮像平面２０１上に３つ以上設定した基準点であり、それらをI₁ , I₂ , …, I_N と表す（Nは３以上の整数）。３０１は、カメラ座標原点Ocと基準点I₁ , I₂ , …, I_N を通る直線であり、それぞれ直線１，直線２…直線Nと表す。９０５は、直線１，直線２…直線Nと平面Gとの交点であり、それぞれG₁ , G₂ , …, G_N と表す。９０６は、直線１，直線２…直線Nと平面Hとの交点であり、それぞれH₁ , H₂ , …, H_N と表す。９０７は撮像平面２０１に写ったマーカ９０１のイメージ（像）である。

ここで、マーカ９０１の中心を特徴点とし、特徴点を平面Gに拘束する制御をロボット１０１ａに施して、特徴点が平面G上で動作するようにする。この状態（特徴点を平面Gに拘束した状態）で、撮像平面２０１において、マーカイメージ９０７の重心が基準点I₁ に一致するようにロボット１０１ａを操作することによって、特徴点を交点G₁ に位置決めすることができる。特徴点を交点G₁ に位置決めした後、ロボット座標原点Or1（９０２）から交点G₁（特徴点）までの位置ベクトル９０８を、ロボット１０１ａの内界センサ（各軸のエンコーダなど）値からロボット座標値（ベース座標系１０２ａの座標値）として計測する。同様にして、特徴点を交点G₂ , …, G_N に位置決めし、各交点（特徴点）までの位置ベクトルを計測する。このようにして測定した交点G₁ , G₂ , …, G_N までの位置ベクトルを^r1 P_{g_1} , ^r1 P_{g_2} , …, ^r1 P_{g_N} と表す。

つぎに、ロボット１０１ａの特徴点（マーカの中心）を平面Hに拘束した状態で、同様にして、マーカイメージ９０７の重心がI₁ , I₂ , …, I_N に一致するようにロボット１０１ａを操作して特徴点を平面H上の交点H₁ , H₂ , …, H_N に位置決めし、ロボット座標原点Or1から各交点（特徴点）までの位置ベクトル９０９を測定する。このようにして測定した交点H₁ , H₂ , …, H_N までの位置ベクトルを^r1 P_{h_1} , ^r1 P_{h_2} , …, ^r1 P_{h_N} と表す。
撮像平面２０１において、マーカイメージ９０７の重心を基準点２０６に一致させるロボットの操作は、実施例１で説明したように画像ベースの視覚サーボ技術（非特許文献２の第６章参照）を適用して自動化することもできる。
また、位置ベクトルの測定順序は上記によらず任意に変更して構わない。さらには、ロボットの特徴点を直線３０１に拘束させた状態で、特徴点が各平面上に乗るようにロボットを動かしても良い。
以上が第１のロボット１０１ａに対して行う測定処理である。

つぎに、第２のロボット１０１ｂのツール先端にも同様のマーカ（９０１）を取り付ける。また、ロボット１０１ｂに対しても２つの平行な平面を設定する。それらを平面Sと平面Tと表す。ここで、平面S（平面T）は平面G（平面H）と同じである必要はない（同じか異なるかについては拘らない）。直線１，直線２…直線Nと平面Sとの交点をそれぞれS₁ , S₂ , …, S_N と表し、直線１，直線２…直線Nと平面Tとの交点をそれぞれT₁ , T₂ , …, T_N と表す。
ロボット１０１ａの測定処理で使用した同一の基準点I₁ , I₂ , …, I_N を使って、ロボット１０１ｂに対しても同様の測定処理を行う。すなわち、ロボット１０１ｂのマーカ中心（特徴点）を平面S（平面T）に拘束した状態で、撮像平面２０１においてマーカイメージ９０７の重心が各基準点I₁ , I₂ , …, I_N に一致するようにロボット１０１ｂを操作することによって、その特徴点を各交点S₁ , S₂ , …, S_N （交点T₁ , T₂ , …, T_N ）に位置決めし、各交点までのベース座標系１０２ｂを基準にした位置ベクトル^r2 P_{s_1}, ^r2 P_{s_2} , …, ^r2 P_{s_N} （^r2 P_{t_1}, ^r2 P_{t_2} , …, ^r2 P_{t_N} ）を測定する。

つぎに、上述した測定データに基づいて第１のロボット１０１ａと第２のロボット１０１ｂ間の相対位置を求める手順を説明する。
まず、ベース座標１０２ａとベース座標１０２ｂ間の回転行列^r1 R _r2 を求める。
ここで、直線kの方向ベクトルをn _k と表す（k=1, 2, …, N）。ただし、カメラ座標原点Ocからマーカ９０１に向かう方向を正とする。図９を見れば分かるように、直線kと平面Gおよび平面Hの交点G_k , H_k までの位置ベクトル^r1 P_{g_k} と ^r1 P_{h_k} の差をとって正規化することによって、ロボット１０１ａのベース座標１０２ａを基準にした方向ベクトル^r1 n _k を計算することができる。すなわち、

^r1 n _k ＝ δ(G,H)・( ^r1 P_{g_k} − ^r1 P_{h_k})／| ^r1 P_{g_k} − ^r1 P_{h_k} | (22)
(k=1, 2, …, N)

である。ここで、δ(G,H) は平面Gと平面Hの位置関係によって定まる符号係数（１または−1）である。
式(22)の方向ベクトルと、カメラ座標系２０３を基準にした方向ベクトル^C n _k はつぎのように関係付けられる。

^r1 R _C・^C n _k ＝ ^r1 n _k , (k=1, 2, …, N) (23)

ロボット１０１ｂについても、同様につぎの式が成り立つ。

^r2 n _k ＝ δ(S,T)・( ^r2 P_{s_k} − ^r2 P_{t_k})／| ^r2 P_{s_k} − ^r2 P_{t_k} | (24)
^r2 R _C・^C n _k ＝ ^r2 n _k, (k=1, 2, …, N) (25)

いま、カメラパラメータは未知なので（カメラ座標系２０３に対する撮像平面２０１の位置関係が分からないので）、方向ベクトル^C n _k の値（座標値）は分からないが、式(23)(25)から^C n _k を消去すれば、つぎのようなロボット１０１ａとロボット１０１ｂ間の直接の関係式が導出される。

^r1 R _r2・^r2 n _k ＝ ^r1 n _k, (k=1, 2, …, N) (26)

式(26)の連立方程式を解くことによって、回転行列^r1 R _r2 が求まる。位置ベクトルの測定誤差のない理想的な状態では、N=3（つまり基準点が３個、直線が３本）であれば十分であるが、実際には測定誤差が含まれるので、Nを３以上に増やして計算結果のバラツキ（標準偏差）を小さく抑える。

つぎに、ベース座標１０２ａとベース座標１０２ｂ間の位置ベクトル^r1 Po_r1o_r2 を求める。
図９において、平面Gと平面Hは平行であるから、線分G₁G₂と線分H₁H₂も平行である。したがって、三角形OcG₁G₂ と三角形OcH₁H₂ は相似になり、

G₁G₂： H₁H₂ ＝ OcG₁ ： OcH₁ (27)

が成り立つ。さらには、平面G上の交点G₁ , G₂ , …, G_N から任意に２つ選び出した交点G_i , G_j と、対応する平面H上の交点H_i , H_j に対して、

G_iG_j： H_iH_j ＝ OcG_k ： OcH_k (28)
OcG_k/ OcH_k ＝ const. (G_iG_j / H_iH_j＝ const.) (29)
( i=1,2,…,N, j=1,2,…,N, i≠j, k=1,2, …,N )

が成り立つ。OcG_k /OcH_k をL_g /L_h とおき、測定した位置ベクトルからつぎのように平均値を求める。

ここで_NC₂はＮ個の交点から２個を選び出す場合の組み合わせである。

カメラ座標原点Ocから交点G_k および交点H_k までの距離比が式(30)により求まるので、この比と位置ベクトル^r1 P_{g_k} および^r1 P_{h_k} を用いると、外分点を求める要領で、ロボット座標原点Or1からカメラ座標原点Ocまでの位置ベクトル^r1 Po_r1o_C（図９の９１０）をつぎのように求めることができる。

^r1 Po_r1o_C＝ (−L_h)/(L_g−L_h)・^r1 P_{g_k}＋ L_g/(L_g−L_h)・^r1 P_{h_k} (31)
(k=1, 2, …, N)

位置ベクトルには測定誤差が含まれるので、式(31)をもとにつぎのように平均値を求めればよい。

結局、位置ベクトルの測定値をもとに、式(30)(32)から^r1 Po_r1o_C が求まる。

つぎに、第２のロボット１０１ｂに対しても、全く同様にして、ベース座標系１０２ｂの原点Or2からカメラ座標系２０３の原点Ocまでの位置ベクトル^r2 Po_r2o_Cをつぎのように求める。

以上より、実施例１と同様にして、次式により位置ベクトル^r1 Po_r1o_r2 が求まる。

^r1 Po_r1o_r2＝ ^r1 R _r2・^r2 Po_Co_r2 − ^r1 Po_Co_r1
＝ −^r1 R _r2・^r2 Po_r2o_C ＋ ^r1 Po_r1o_C (35)

また、式(22)(24)におけるδ(G,H)とδ(S,T)は、L_g /L_h とL_s /L_t を用いてつぎのように計算できる。

δ(G,H) ＝ sgn( L_g / L_h − 1 ) (36)
δ(S,T) ＝ sgn( L_s / L_t − 1 ) (37)

ここでsgn(ｘ)は符号関数であり、ｘ≧0のとき１、ｘ＜0のとき-1である。

以上説明したように、第４実施例によれば、ロボットツールの特徴点を２つの平行な平面上で動作させ、かつ、カメラ１台の撮像平面の各基準点に特徴点が一致するようにロボットを動かすことによって、基準点を通る各直線と各平面との交点に特徴点を位置決めし、各交点における特徴点のロボット座標値を測定するので、撮像平面でのターゲットイメージの大きさや明るさを計測することなく、カメラ１台のみで奥行き方向の距離情報を正確に得ることができる。したがって、低コストでロボットツールの先端同士を突き合わせることなく、ロボット間の相対位置関係をより正確に計測できるのである。また、カメラパラメータを同定することなく、ロボット間の相対位置関係を手軽に計測できるのである。

本発明は、複数台のロボットが協調して作業する際に、ロボット間の相対位置関係を計測する方法として有用である。

本発明の第1実施例を示す相対位置計測装置の構成図である。 本発明の第1実施例を示す画像表示の説明図である。 本発明の第1実施例を示す画像表示の説明図である。 本発明の第1実施例を示す画像表示の説明図である。 本発明の第１実施例の変形例を示すロボットの説明図である。 本発明の第２実施例を示す相対位置計測装置の構成図である。 本発明の第２実施例で用いる撮像装置とレーザ発振装置を一体化した装置の説明図である。 本発明の第３実施例を示す相対位置計測装置の構成図である。 本発明の第４実施例における計測原理の説明図である。

符号の説明

１０１a 第１のロボット、１０１ｂ 第２のロボット、１０２ａ，１０２ｂ ベース座標系、１０３ａ，１０３ｂ 制御装置、１０４ａ，１０４ｂ 操作インターフェイス、１０５ａ，１０５ｂ 球状ターゲット、１０６ 撮像装置、１０７ 画像処理装置、１０８ 画像表示装置、１０９ 相対位置演算装置、１１０ 支持部、２０１ 撮像平面、２０２ 撮像平面の座標系、２０３ カメラ座標系、２０４ 球状ターゲットのイメージ、２０５ 球状ターゲットのイメージの重心、２０６ 基準点、３０１ 直線、３０２ａ，３０２ｂ 球状ターゲットの特徴点（球の中心）の位置、３０３ａ，３０３ｂ ロボット座標系原点から球状ターゲットの特徴点までの位置ベクトル、５０１ レーザ発振装置、５０２ 支持部、５０３ 距離計測装置、６０１ ターゲットに照射されたレーザ光線のスポット、７０１ 棒状ジグ、７０２ａ，７０２ｂ 棒状ジグの支持部、７０３ａ，７０３ｂ 直動ジョイント、７０４ａ，７０４ｂ ユニバーサルジョイント、７０５ａ，７０５ｂ 針状ツール、９０１ マーカ、９０２ ベース座標系１０２ａの原点、９０３ カメラ座標系２０３の原点、９０４ａ 平面G、９０４ｂ 平面H、９０５ 平面Gと直線３０１の交点、９０６ 平面Hと直線３０１の交点、９０７ マーカのイメージ、９０８ ロボット座標原点９０２から交点９０５までの位置ベクトル、９０９ ロボット座標原点９０２から交点９０６までの位置ベクトル、９１０ ロボット座標原点９０２からカメラ座標原点９０３までの位置ベクトル

Claims (14)

1. 複数のロボットの可動部の特徴点を少なくとも３本の直線上で動作させて、前記各直線上の少なくとも２点における前記特徴点のロボット座標値を測定するとともに、前記各直線上の特定点から前記特徴点までの距離または距離と相関関係を持つ物理量を測定し、各直線・各ロボットについて得られた測定値を基に複数ロボット間の相対位置関係を計算することを特徴とする複数ロボット間の相対位置計測方法。
2. 前記特徴点を撮像装置で撮像して、その画像を画像表示装置に表示して、前記画像表示装置上の前記特徴点の画像が、前記画像表示装置上に定義された基準点と一致するように前記ロボットを操作して、前記特徴点を直線上で動作させることを特徴とする請求項１記載の複数ロボット間の相対位置計測方法。
3. 前記ロボットの可動部にターゲット物体を設け、前記ターゲット物体に前記特徴点を定義し、前記画像表示装置上に表示された前記ターゲット物体のイメージの面積または明るさを前記距離と相関関係を持つ物理量とすることを特徴とする請求項２記載の複数ロボット間の相対位置計測方法。
4. 前記ロボットの可動部にターゲット物体を設け、前記ターゲット物体に前記特徴点を定義するとともに、前記撮像装置にズームレンズを取り付け、前記画像表示装置に映し出された前記ターゲット物体のイメージの面積が一定となるように前記ズームレンズを移動させ、そのときの前記ズームレンズの移動量を前記距離と相関関係を持つ物理量とすることを特徴とする請求項２記載の複数ロボット間の相対位置計測方法。
5. 前記ターゲット物体を発光させることを特徴とする請求項３または請求項４記載の複数ロボット間の相対位置計測方法。
6. 大きさまたは明るさの異なる複数のターゲット物体をロボットの可動部に設け、前記撮像装置からの距離に応じて前記ターゲット物体を切り替えることを特徴とする請求項３乃至５記載の複数ロボット間の相対位置計測方法。
7. レーザ発振装置から発振するレーザ光に沿って、前記ロボットの可動部の特徴点を直線動作させることを特徴とする請求項１記載の複数ロボット間の相対位置計測方法。
8. ロボットの可動部にターゲット物体を設け、前記ターゲット物体に前記特徴点を定義し、前記レーザ光を前記ターゲット物体に照射して、前記レーザ光の光点が前記特徴点に一致するように前記ロボットを操作して、前記特徴点を直線動作させることを特徴とする請求項７記載の複数ロボット間の相対位置計測方法。
9. 撮像装置の光軸とレーザ発振装置のレーザ光軸が平行となるように、前記撮像装置と前記レーザ発信装置を一体にした装置を使用することを特徴とする請求項８記載の複数ロボット間の相対位置計測装置。
10. 細長い棒状ジグに沿って、前記ロボットの可動部の特徴点を直線動作させることを特徴とする請求項１記載の複数ロボット間の相対位置計測方法。
11. 前記棒状ジグに距離目盛を付し、その距離目盛を使って、前記特徴点までの距離を測定することを特徴とする請求項１０記載の複数ロボット間の相対位置計測方法。
12. 前記ロボットそれぞれ対して２つ以上の平行な平面を定義し、前記特徴点を前記各平面上に拘束させて前記各直線と前記各平面との交点に前記特徴点を位置決めし、前記各交点における前記特徴点のロボット座標値を測定することを特徴とする請求項１または請求項２記載の複数ロボット間の相対位置計測方法。
13. 前記各交点における前記特徴点のロボット座標値をもとに、前記距離と相関関係を持つ物理量を測定することを特徴とする請求項１２記載の複数ロボット間の相対位置計測方法。
14. 前記２つ以上の平行な平面のうち、ある平面上の前記交点間の距離と他の平面上の前記交点間の距離の比を、前記距離と相関関係を持つ物理量とすることを特徴とする請求項１３記載の複数ロボット間の相対位置計測方法。

Images