(57)【要約】【課題】可動体に設定される基準点の位置および可動体の姿勢を高精度で検出することができる可動体の位置および姿勢検出装置を提供する。【解決手段】同一直線上に並ばない３個のターゲットを有し、ツール３０に設けられるターゲットユニット１１と、ロボット座標系における絶対位置が把握されている撮像位置からターゲットユニットを撮影して、ターゲットユニットの画像を表す画像データを生成する複数のカメラ１３と、カメラからの画像データに基づいて、ロボット座標系におけるツールに設定された基準点の絶対位置およびツールの絶対姿勢を求める画像処理部１４と、ツールに設けられ、ツールの相対角変位および相対スライド変位を検出するジャイロセンサ１５および加速度センサ１６と、ツールの相対スライド変位および相対角変位と、ツールの基準点の絶対位置および絶対姿勢とに基づいて、ツールの基準点の位置および姿勢を求める演算部１８を備える。【選択図】　　　図１
2004050356.tif 000002
【特許請求の範囲】
【請求項１】
同一直線上に並ばない３個のターゲットを有し、可動体に設けられるターゲットユニットと、
予め定める座標系における絶対位置が把握されている撮像位置からターゲットユニットを撮影して、ターゲットユニットの画像を表す画像データを生成する複数の撮影手段と、
撮影手段からの画像データに基づいて、前記座標系における可動体に設定された基準点の絶対位置および可動体の絶対姿勢を求める第１検出手段と、
可動体に設けられ、可動体の相対スライド変位および相対角変位を検出する第２検出手段と、
第１検出手段からの基準点の絶対位置および絶対姿勢と、第２検出手段からの可動体の相対スライド変位および相対角変位とに基づいて、基準点の位置および可動体の姿勢を求める演算手段を備えることを特徴とする可動体の位置および姿勢検出装置。
【請求項２】
可動体に設けられるターゲットと、
ターゲットを予め定める軸線まわりに角変位駆動する駆動手段と、
予め定める座標系における絶対位置が把握されている測距位置からターゲットまでの距離を測定するターゲット距離測定手段と、
ターゲット距離測定手段によって測定された距離に基づいて、前記座標系における可動体に設定された基準点の絶対位置および可動体の絶対姿勢を求める第１検出手段と、
可動体に設けられ、可動体の相対スライド変位および相対角変位を検出する第２検出手段と、
第１検出手段からの基準点の絶対位置および絶対姿勢と、第２検出手段からの可動体の相対スライド変位および相対角変位とに基づいて、基準点の位置および可動体の姿勢を求める演算手段を備えることを特徴とする可動体の位置および姿勢検出装置。
【請求項３】
演算手段からの可動体の基準点の位置および可動体の姿勢に基づいて、可動体のスライド変位および角変位を制御する制御手段をさらに備えることを特徴とする請求項１または２記載の可動体の位置および姿勢検出装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、産業用ロボットの手首に連結されるツールなどの可動体の先端部の位置および姿勢を検出する可動体の位置および姿勢検出装置に関する。
【０００２】
【従来の技術】
産業用ロボットの手首に連結されるツールなどの可動体のスライド変位および角変位を検出して制御する種々の従来技術がある。従来技術として、多関節ロボットのアームの先端にジャイロを設けた姿勢検出装置がある。この姿勢検出装置は、ジャイロによって、アーム先端の相対角変位が検出されて、この相対角変位に基づいてアーム先端に設けられるツールの姿勢を求める。
【０００３】
他の従来技術として、ロボットのアーム先端に超音波パルスを発生する超音波発信器を設け、所定位置に固定される受信器によって、超音波発信器からの超音波パルスを受信して、超音波パルスが発生してから受信されるまでの時間に基づいて、アーム先端と所定位置との距離を求めて、アーム先端に設けられるツールの位置および姿勢を計測する計測装置がある。
【０００４】
さらに他の従来技術として、多関節ロボットのティーチプレイバックを行う際に、多関節ロボットのアーム先端にセンサを設けて、前記センサによって多関節ロボットのアーム先端に設けられるツールと、予め定める位置に配置される較正治具の先端との距離を測定して、温度変化によるアームの長さの変化を補正して、ロボットの制御に必要な各パラメータのキャリブレーションを行う方法がある。
【０００５】
【発明が解決しようとする課題】
産業用ロボットは、ある位置に繰り返して位置決めするときの繰り返し精度は、たとえば約５０マイクロメートルのように高いけれども、ある所定の位置に位置決めするときの絶対精度が、特に大型ロボットの場合は、たとえば約３〜４ミリメートルのように低いので、ロボットを用いる前に、ティーチプレイバック方式で予め教示しておくことがほとんどである。絶対精度の低い原因は、ロボットの幾何学的な製造誤差、重量物を把持したときのアームの撓み、温度によるアームの伸縮および減速機のガタ等によって、ロボットの各関節に設けられる各エンコーダからの検出信号に基づくロボットのアーム先端に設けられるツールに設定される基準点の位置および姿勢が、指令値に対する誤差があるためである。このことが計算機によってオフラインで動作データを生成して、その動作データをロボットにダウンロードして、溶接などの作業をさせたり、センサをアーム先端に設けて、ロボットを計測装置として利用するような、今後のロボットの新しい展開に対する障害となっている。
【０００６】
このような問題に対して、前述の従来技術のように、相対角変位を検出するジャイロセンサおよび相対スライド変位を検出する加速度センサなどの慣性センサを用いて位置および姿勢を連続的に測定する方法は、慣性航法で使われる一般に知られた方法である。この方法を利用してロボットのアーム先端部の位置および姿勢を求めることができれば、時間的に連続でリアルタイムな測定が可能であり、さらにロボットから離れた位置からロボットの位置および姿勢を測定する場合に比べて、ロボットの位置および姿勢によって死角が生じても、測定に影響しない等の利点がある。
【０００７】
しかしジャイロセンサは、実際には相対角変位を直接検出するのではなく、相対角速度を測定して、その相対角速度を時間に関して積分することで相対角変位を検出している。また同様に加速度センサは、相対加速度を測定して、その相対加速度を時間に関して２回積分することで相対スライド変位を検出している。したがってロボットのアーム先端の変位量が増えるにつれて、または時間が経過するにつれて、検出誤差およびドリフトが累積するので、このような慣性センサでは、ロボットのアーム先端に設けられるツールの基準点の位置および姿勢を高精度で測定することができない。たとえば市販されている汎用の光ファイバジャイロおよびリングレーザジャイロは、相対加速度の精度は、高々０．０１〜０．０３重力加速度程度であり、この程度の精度であると、ロボットが１秒間動作すると、最悪５〜１５センチメートルもの誤差が蓄積される。
【０００８】
また前述の従来技術のように、超音波によるアーム先端に設けられるツールの位置および姿勢を計測において、超音波発信器からの超音波パルスの一部が、ロボットおよびロボットの周辺に設置されている機器などに反射し、その反射波が所定位置に固定される受信器によって受信され、アーム先端と所定位置との距離を正確に求めることができない。またロボットは、高速で動作するので、ドップラー効果によって、アーム先端と所定位置との距離を正確に求めることができない。
【０００９】
したがって本発明の目的は、可動体に設定される基準点の位置および可動体の姿勢を高精度で検出することができる可動体の位置および姿勢検出装置を提供することである。
【００１０】
【課題を解決するための手段】
請求項１記載の本発明は、同一直線上に並ばない３個のターゲットを有し、可動体に設けられるターゲットユニットと、
予め定める座標系における絶対位置が把握されている撮像位置からターゲットユニットを撮影して、ターゲットユニットの画像を表す画像データを生成する複数の撮影手段と、
撮影手段からの画像データに基づいて、前記座標系における可動体に設定された基準点の絶対位置および可動体の絶対姿勢を求める第１検出手段と、
可動体に設けられ、可動体の相対スライド変位および相対角変位を検出する第２検出手段と、
第１検出手段からの基準点の絶対位置および絶対姿勢と、第２検出手段からの可動体の相対スライド変位および相対角変位とに基づいて、基準点の位置および可動体の姿勢を求める演算手段を備えることを特徴とする可動体の位置および姿勢検出装置である。
【００１１】
本発明に従えば、複数の撮影手段によって、予め定める座標系における絶対位置が把握されている撮像位置から、同一直線上に並ばない３個のターゲットを有し、可動体に設けられるターゲットユニットが撮影され、ターゲットユニットの画像を表す画像データが生成される。撮影手段が複数、たとえば２個あれば、ターゲットユニットを両眼立体視することができる。また撮影手段を、３個以上にすることによって、可動体のスライド変位および角変位によって、各ターゲットが撮影手段の死角領域に入ってしまうことを可及的に防止することができる。
【００１２】
第１検出手段によって、撮影手段からの画像データに基づいて、前記座標系における可動体に設定された基準点の絶対位置および可動体の絶対姿勢が求められる。このときターゲットユニットにおいて、３個のターゲットは同一直線上に並ばないので、この３個のターゲットを含む平面を容易に求めて、その平面の法線を求めて、ターゲットを基準とするターゲット座標系を求めることができる。したがって予め定める座標系とターゲット座標系とに基づいて、前記予め定められる座標系における可動体に設定された基準点の絶対位置および可動体の絶対姿勢を容易かつ正確に求めることができる。さらに可動体に設けられる第２検出手段によって、可動体の相対スライド変位および相対角変位が検出される。
【００１３】
演算手段によって、第１検出手段からの可動体の基準点の絶対位置および絶対姿勢と、第２検出手段からの可動体の相対スライド変位および相対角変位とに基づいて、可動体の基準点の位置および可動体の姿勢が求められる。これによって、たとえば第１検出手段によって求められる可動体の基準点の絶対位置および絶対姿勢が、第２検出手段によって検出される可動体の相対スライド変位および相対角変位よりも高精度であるけれども、第１検出手段は可動体の基準点の絶対位置および絶対姿勢を連続的に求めることができない場合、または、万一、ターゲットユニットが各撮影手段の死角領域に入ってしまった場合、演算手段は、第１検出手段によって可動体の基準点の絶対位置および絶対姿勢が求められるまでは、第２検出手段によって検出される可動体の相対スライド変位および相対角変位に基づいて、可動体の基準点の位置および姿勢を求めることができる。このように演算手段は、２つの検出手段からの可動体の基準点の絶対位置および絶対姿勢、ならびに可動体の相対スライド変位および相対角変位に基づいて、極めて高精度な可動体の基準点の位置および姿勢を連続的に求めることができる。
【００１４】
請求項２記載の本発明は、可動体に設けられるターゲットと、
ターゲットを予め定める軸線まわりに角変位駆動する駆動手段と、
予め定める座標系における絶対位置が把握されている測距位置からターゲットまでの距離を測定するターゲット距離測定手段と、
ターゲット距離測定手段によって測定された距離に基づいて、前記座標系における可動体に設定された基準点の絶対位置および可動体の絶対姿勢を求める第１検出手段と、
可動体に設けられ、可動体の相対スライド変位および相対角変位を検出する第２検出手段と、
第１検出手段からの基準点の絶対位置および絶対姿勢と、第２検出手段からの可動体の相対スライド変位および相対角変位とに基づいて、基準点の位置および可動体の姿勢を求める演算手段を備えることを特徴とする可動体の位置および姿勢検出装置である。
【００１５】
本発明に従えば、ターゲット距離測定手段によって、予め定める座標系における絶対位置が把握されている測距位置から、可動体に設けられ、駆動手段によって予め定める軸線まわりに角変位するターゲットまでの距離が測定される。第１検出手段によって、ターゲット距離測定手段によって測定された距離に基づいて、前記座標系における可動体に設定された基準点の絶対位置および可動体の絶対姿勢が求められる。ターゲットが角変位することによって、ターゲット距離測定手段は、予め定める座標系における絶対位置が把握されている測距位置から、互いに異なる角度位置に配置されるターゲットまでの距離を、測定することができる。このとき、たとえばターゲットの同一直線上に並ばない３つの角度位置を設定すれば、この３つの角度位置を含む平面を容易に求めて、その平面の法線を求めて、ターゲットを基準とするターゲット座標系を求めることができる。したがって予め定める座標系とターゲット座標系とに基づいて、前記予め定められる座標系における可動体に設定された基準点の絶対位置および可動体の絶対姿勢を容易かつ正確に求められる。さらに可動体に設けられる第２検出手段によって、可動体の相対スライド変位および相対角変位が検出される。
【００１６】
演算手段によって、第１検出手段からの可動体の基準点の絶対位置および絶対姿勢と、第２検出手段からの可動体の相対スライド変位および相対角変位とに基づいて、可動体の基準点の位置および可動体の姿勢が求められる。これによって、たとえば第１検出手段によって求められる可動体の基準点の絶対位置および絶対姿勢が、第２検出手段によって検出される可動体の相対スライド変位および相対角変位よりも高精度であるけれども、第１検出手段は可動体の基準点の絶対位置および絶対姿勢を連続的に求めることができない場合、または、万一、ターゲットがターゲット距離測定手段の死角領域に入ってしまった場合、演算手段は、第１検出手段によって可動体の基準点の絶対位置および絶対姿勢が求められるまでは、第２検出手段によって検出される可動体の相対スライド変位および相対角変位に基づいて、可動体の基準点の位置および姿勢を求めることができる。このように演算手段は、２つの検出手段からの可動体の基準点の絶対位置および絶対姿勢、ならびに可動体の相対スライド変位および相対角変位に基づいて、極めて高精度な可動体の基準点の位置および姿勢を連続的に求めることができる。
【００１７】
請求項３記載の本発明は、演算手段からの可動体の基準点の位置および可動体の姿勢に基づいて、可動体のスライド変位および角変位を制御する制御手段をさらに備えることを特徴とする。
【００１８】
本発明に従えば、演算手段からの高精度の可動体の基準点の位置および可動体の姿勢に基づいて、可動体のスライド変位および角変位を高精度に制御することができる。
【００１９】
【発明の実施の形態】
図１は、本発明の第１の実施形態の位置および姿勢検出装置１０の構成を示すブロック図である。図２は、ロボット２０および複数のカメラ１３Ａ，１３Ｂ，１３Ｃを模式的に示す図である。位置および姿勢検出装置（以後、単に「位置姿勢検出装置」と表記することがある。）１０は、産業用ロボットなどのロボット２０の手首２４に連結されるツール３０などの可動体に設定された基準点の位置およびツール３０の姿勢を検出して、ツール３０のスライド変位および角変位を制御する。
【００２０】
ロボット２０は、たとえば６軸垂直多関節ロボットである。ロボット２０は、ベース部２１、下部アーム２２、上部アーム２３および手首２４を有する。ベース部２１は、鉛直方向に延びる第１軸線Ｊ１まわりに旋回可能である。下部アーム２２は、水平方向に延びる第２軸線Ｊ２まわりに角変位可能にしてベース部２１に連結される。上部アーム２３は、その基端部が水平方向に延びる第３軸線Ｊ３まわりに角変位可能にして下部アーム２２の上端部に連結される。手首２４は、上部アーム２３の軸線に平行に延びる第４軸線Ｊ４まわりに角変位可能、かつ前記第４軸線Ｊ４に垂直な第５軸線Ｊ５まわりに角変位可能にして上部アーム２３の先端部に連結される。手首２４は、第５軸線Ｊ５に垂直な第６軸線Ｊ６まわりに角変位可能なフランジ２５を有する。ハンド、溶接トーチおよびセンサなどのツール３０は、フランジ２５に設けられるツールチェンジャー（図示せず）などを介してフランジ２５に連結される。
【００２１】
位置姿勢検出装置１０は、ターゲットユニット１１、ＬＥＤ駆動部１２、カメラ１３、画像処理部１４、ジャイロセンサ１５、加速度センサ１６、信号処理部１７、演算部１８およびロボット制御部１９を含んで構成される。
【００２２】
図３は、ターゲットユニット１１を示す斜視図である。ターゲットユニット１１は、ツールチェンジャーおよびツール３０のいずれかに設けられる。またターゲットユニット１１は、ロボット２０全体の誤差のうち手首部分の誤差が小さい場合、ロボット２０の上部アーム２３の先端部に設けてもよい。本実施の形態では、ターゲットユニット１１は、ツール３０に設けられ、ツール３０とともにスライド変位および角変位可能である。ターゲットユニット１１は、同一直線上に並ばない３個のターゲットを有する。本実施の形態では、ターゲットユニット１１は、第１ターゲット３１、第２ターゲット３２、第３ターゲット３３および第４ターゲット３４の４つのターゲットを有する。
【００２３】
第１ターゲット３１〜第４ターゲット３４は、同形状の球状に形成され、透光性を有し、その重心に互いに異なる色で発光する発光ダイオード（ＬｉｇｈｔＥｍｉｔｔｉｎｇ　Ｄｉｏｄｅ；略称：ＬＥＤ）を備える。第２ターゲット３２は、円柱状の第１リンク３５を介して第１ターゲット３２に連結される。第３ターゲット３３は、円柱状の第２リンク３６を介して第１ターゲット３２に連結される。第４ターゲット３４は、円柱状の第３リンク３７を介して第１ターゲット３２に連結される。第１〜第３リンク３５〜３７は、全く同じ形状であり、第１ターゲット３１の重心から第２ターゲット３２の重心までの距離と、第１ターゲット３１の重心から第３ターゲット３３の重心までの距離と、第１ターゲット３１から第４ターゲット３４までの距離とは、同じである。第１リンク３５、第２リンク３６および第３リンク３７は、それらの軸線が互いに直交するようにして配置される。ターゲットユニット１１の第１ターゲット３１は、第１ターゲット３１の重心がツール３０に設定される基準点となるようにして配置される。ＬＥＤ駆動部１２は、演算部１８からのＬＥＤ制御信号に基づいて、ターゲットユニット１１の各ターゲット３１〜３４に備えられるＬＥＤを発光させるために必要な駆動電力の供給および非供給を行う。
【００２４】
撮影手段であるカメラ１３は、予め定める座標系であるロボット座標系における絶対位置が把握されている撮像位置に配置され、前記撮像位置からターゲットユニット１１を撮影して、ターゲットユニット１１の画像を表す画像データを生成し、複数設けられる。本実施の形態において、ロボット座標系とは、ロボット２０のベース部２１が固定される床に平行に延び、互いに直交するｘ軸およびｙ軸、ならびに鉛直上向きに延びるｚ軸で構成される直交座標系である。本実施の形態において、カメラ１３は、固体撮像素子（Ｃｈａｒｇｅ　Ｃｏｕｐｌｅｄ　Ｄｅｖｉｃｅ　；略称：ＣＣＤ）を備えるＣＣＤカメラで実現される。またカメラ１３は、本実施の形態では、第１カメラ１３Ａ、第２カメラ１３Ｂおよび第３カメラ１３Ｃの３台のカメラで構成され、ロボット座標系における絶対位置が把握されている互いに異なる撮像位置に配置される。各カメラ１３Ａ〜１３Ｃが配置される撮像位置は、詳細に述べると、ロボット２０の動作領域外であって、全カメラ１３Ａ〜１３Ｃによって、ロボット２０の動作領域全体を撮影できる位置である。以後、第１〜第３カメラ１３Ａ〜１３Ｃを区別しない場合には、単に、カメラ１３と表記することがある。
【００２５】
第１検出手段である画像処理部１４は、たとえばマイクロコンピュータで実現され、演算部１８からの計測指令信号が与えられると、カメラ１３からの画像データに基づいて画像処理を行い、ロボット座標系におけるツール３０に設定された基準点の絶対位置およびツール３０の絶対姿勢を求め、求めたツール３０の基準点の絶対位置および絶対姿勢を含む絶対位置姿勢検出信号を演算部１８に与える。
【００２６】
第２検出手段であるジャイロセンサ１５および加速度センサ１６は、ツール３０およびツールチェンジャーのいずれかに設けられ、ツール３０とともにスライド変位および角変位可能である。ジャイロセンサ１５は、ツール３０の角速度を検出することによってツール３０の相対角変位を検出して、ツール３０の相対角変位を含む相対角変位検出信号を信号処理部１７に与える。加速度センサ１６は、ツール３０のスライド加速度を検出することによってツール３０の相対スライド変位を検出して、ツール３０の相対スライド変位を含む相対スライド変位検出信号を信号処理部１７に与える。本実施の形態において、第２検出手段は、ジャイロセンサ１５および加速度センサ１６によって構成される。信号処理部１７は、ジャイロセンサ１５および加速度センサ１６からの相対角変位検出信号および相対スライド変位検出信号を、演算部１８によって演算処理可能な信号に変換する信号処理を行って、ツール３０の相対角変位および相対スライド変位を含む相対変位検出信号を演算部１８に与える。
【００２７】
演算手段である演算部１８は、たとえば中央演算処理装置（Ｃｅｎｔｒａｌ
Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ；略称：ＣＰＵ）などの演算処理装置、ならびにランダムアクセスメモリ、リードオンリーメモリおよびハードディスクドライブなどの記憶装置を含んで実現され、位置姿勢検出装置１０を統括的に制御する。演算部１８は、ターゲットユニット１１の各ターゲット３１〜３４への駆動電力の供給および非供給を示すＬＥＤ制御信号を、ＬＥＤ駆動部１２に与える。また演算部１８は、ロボット座標系におけるツール３０に設定された基準点の絶対位置およびツール３０の絶対姿勢を求めることを示す計測指令信号を、画像処理部１４に与える。さらに演算部１８は、ロボット制御部１９から指令要請信号が与えられると、画像処理部１４からの絶対位置姿勢検出信号に含まれるロボット座標系におけるツール３０に設定された基準点の絶対位置およびツール３０の絶対姿勢と、信号処理部１７からの相対変位検出信号に含まれるツール３０の相対角変位およびツール３０の基準点の相対スライド変位とに基づいて、ツール３０の基準点の位置およびツール３０の姿勢を求めて、求めた位置および姿勢に基づく修正制御指令値を含む制御信号をロボット制御部１９に与える。
【００２８】
制御手段であるロボット制御部（以後、単に「制御部」と表記することがある。）１９は、予め設定されている制御指令値および演算部１８によって求められる修正指令値を含む制御信号を与えるように要請することを示す指令要請信号を、演算部１８に与える。また制御部１９は、演算部１８から与えられる制御信号に含まれる制御指令値および修正制御指令値に基づいて、ツール３０のスライド変位および角変位を制御する。ロボット２０の、下部アーム２２、上部アーム２３、手首２４およびフランジ２５は、ロボット２０に搭載され、制御部１９によって制御される複数のサーボモータ（図示せず）の駆動力によって、第１〜第６軸線Ｊ１〜Ｊ６まわりに角変位する。これによって制御部１９は、ツール３０のスライド変位および角変位を制御する。
【００２９】
図４は、カメラ１３を用いて、ある点Ｐのロボット座標系における座標（Ｘ，Ｙ，Ｚ）を求める方法を説明するための図である。ここでは理解を容易にするために、第１カメラ１３Ａおよび第２カメラ１３Ｂを用いる場合の、ある点Ｐのロボット座標系における座標（Ｘ，Ｙ，Ｚ）を求める方法を説明する。第１カメラ１３Ａが撮影した点Ｐが、第１カメラ１３Ａの撮像面Ａ１において点Ｑ１として撮影された場合、点Ｑ１の第１カメラ１３Ａの撮像面Ａ１座標系における座標を（ＸＣ１，ＹＣ１）とする。また第２カメラ１３Ｂが撮影した点Ｐが、第２カメラ１３Ｂの撮像面Ａ２において点Ｑ２として撮影された場合、点Ｑ２の第２カメラ１３Ｂの撮像面Ａ２座標系における座標を（ＸＣ２，ＹＣ２）とする。このとき点Ｐと点Ｑ１との関係は、次式（１）で表され、点Ｐと点Ｑ２との関係は、次式（２）で表される。
【００３０】
【数１】
2004050356.tif 000003
【００３１】
前式（１）において、ｈ１は、同次座標表現における媒介変数であり、右辺における左側の行列は、予め定められる第１カメラ１３Ａのカメラパラメータ行列である。前式（２）において、ｈ２は、同次座標表現における媒介変数であり、右辺における左側の行列は、予め定められる第２カメラ１３Ｂのカメラパラメータ行列である。
【００３２】
画像処理部１４は、第１カメラ１３Ａからの画像データに基づいて、点Ｑ１の座標（ＸＣ１，ＹＣ１）、および第２カメラ１３Ｂからの画像データに基づいて、点Ｑ２の座標（ＸＣ２，ＹＣ２）を求めて、前式（１）および式（２）を連立させて、各媒介変数ｈ１，ｈ２を消去して、点Ｐのロボット座標系における座標（Ｘ，Ｙ，Ｚ）に関して整理すると、次式（３）が得られる。
Ｂ＝Ａ・Ｖ　　　　　　　　　　　　　　　　　　　　　　…（３）
【００３３】
前式（３）の左辺の行列Ｂは次式（５）で表され、右辺における左側の行列Ａは次式（４）で表され、右辺における右側の行列Ｖは次式（６）で表される。
【００３４】
【数２】
2004050356.tif 000004
【００３５】
したがって、点Ｐのロボット座標系における座標（Ｘ，Ｙ，Ｚ）は、次式（７）のように求められる。
Ｖ＝（ＡＴ・Ａ）−１・ＡＴ・Ｂ　　　　　　　　　　　　　…（７）
【００３６】
前式（７）において、Ｔは行列の転置を示す。
前述の点Ｐのロボット座標系における座標（Ｘ，Ｙ，Ｚ）の求め方は、第１カメラ１３Ａおよび第２カメラ１３Ｂの２台のカメラ１３を用いた場合であるが、カメラ１３を３台以上用いる場合であっても同様にして点Ｐのロボット座標系における座標（Ｘ，Ｙ，Ｚ）を求めることができる。このようにしてロボット座標系におけるターゲットユニット１１の第１ターゲット３１の重心Ｌ１の座標（ＸＬ１，ＹＬ１，ＺＬ１）を求めることによって、ツール３０に設定された基準点の絶対位置を求めることができる。
【００３７】
また、このようにして画像処理部１４は、カメラ１３からの画像データに基づいて、ターゲットユニット１１の第１〜第４ターゲット３１〜３４の重心の座標が得られる。ロボット座標系における第１ターゲット３１の重心Ｌ１の座標（ＸＬ１，ＹＬ１，ＺＬ１）、第２ターゲット３２の重心Ｌ２の座標（ＸＬ２，ＹＬ２，ＺＬ２）、第３ターゲット３３の重心Ｌ３の座標（ＸＬ３，ＹＬ３，ＺＬ３）、第４ターゲット３４の重心Ｌ４の座標（ＸＬ４，ＹＬ４，ＺＬ４）とすると、第１ターゲット３１から第２ターゲット３２に向かう第１基底ベクトルＶｘ、第１ターゲット３１から第３ターゲット３３に向かう第２基底ベクトルＶｙおよび第１ターゲット３１から第４ターゲット３４に向かう第３基底ベクトルＶｚは、それぞれ次式（８）、式（９）および式（１０）で表される。
【００３８】
【数３】
2004050356.tif 000005
【００３９】
このようにして第１ターゲット３１の重心Ｌ１を座標原点とし、互いに直交する第１〜第３基底ベクトルＶｘ，Ｖｙ，Ｖｚによって構成されるターゲット座標系が求められる。これによってターゲット座標系のロボット座標系に対する傾斜がわかるので、ツール３０の絶対姿勢を求めることができる。
【００４０】
ここで万一、ターゲットユニット１１の第２〜第４ターゲット３２〜３４のいずれか１つがカメラ１３によって撮影できなくても、第１ターゲット３１を含む３つの残余のターゲットの重心の座標を求めることによって、前記３つの残余のターゲットの重心を含む平面が求められるので、この平面の法線を求めることができる。これによってターゲット座標系を求めることができる。
【００４１】
このようなターゲットユニット１１をカメラ１３で撮影して、その画像データに基づいてターゲット座標系を求めて、ツール３０に設定された基準点の位置およびツール３０の姿勢を求めるには、多少の時間、たとえば０．１秒程度を要するので、画像処理部１４によってツール３０の基準点の位置および姿勢が求められてから、次に画像処理部１４によってツール３０の基準点の位置および姿勢が求められるまでの間は、ジャイロセンサ１５および加速度センサ１６によって検出される相対角変位および相対スライド変位に基づいて、演算部１８は、ツール３０の基準点の位置および姿勢を求める。
【００４２】
ジャイロセンサ１５および加速度センサ１６によって検出される相対角変位および相対スライド変位に基づくツール３０の基準点の位置および姿勢は、画像処理部１４によって求められるツール３０の基準点の位置および姿勢に比べて精度が低いけれども、相対角変位および相対スライド変位の誤差が蓄積されるまでには、新たに画像処理部１４によってツール３０の基準点の位置および姿勢が求められるので、連続して精度の高いツール３０の基準点の位置および姿勢を求めることができる。
【００４３】
図５は、ロボット制御部１９によるツール３０のスライド変位および角変位を制御する手順を示すフローチャートである。図６は、ロボット制御部１９によるツール３０のスライド変位および角変位を制御する方法を説明するための図である。ロボット制御部１９がロボット２０に制御信号を与える時間間隔である指令サイクルをｔ秒とする。ステップｓ０で、制御手順が開始されて、ステップｓ１に進む。
【００４４】
ステップｓ１では、演算部１８は、ある時刻Ｔｉにおいて求めたツール３０の基準点および姿勢を示す実測値Ｒｉと、予め設定される時刻Ｔｉにおけるツール３０の基準点および姿勢を示す制御指令値Ｈｉとの誤差ベクトルΔｉを求めて、ステップｓ２に進む。ここで添字ｉは整数である。
【００４５】
ステップｓ２では、演算部１８は、時刻Ｔｉからｔ秒後の時刻Ｔｉ＋１における予め設定されているロボット２０のツール３０の基準点および姿勢を示す制御指令値Ｈｉ＋１に、誤差ベクトルの総和ΣΔｉ−１と、ステップｓ１で求めた誤差ベクトルΔｉとを加算して、時刻Ｔｉ＋１における修正制御指令値Ｎｉ＋１を求めて、ステップｓ３に進む。誤差ベクトルの総和ΣΔｉ−１とは、時刻Ｔｉのｔ秒前の時刻Ｔｉ−１までに累積されている演算部１８によって求められたツール３０の基準点および姿勢を示す実測値と、予め設定されるロボット２０のツール３０の基準点および姿勢を示す制御指令値との誤差ベクトルの総和である。
【００４６】
ステップｓ３では、演算部１８は、時刻Ｔｉ＋１において、ステップｓ２で求めた修正制御指令値Ｎｉ＋１を含む制御信号をロボット制御部１９に与えて、ステップｓ４に進む。ステップｓ４では、添字ｉをｉ＋１に置換えて、ステップｓ１に戻る。
【００４７】
このような手順で制御を行うことによって、図６に示すように、演算部１８によって求められたツール３０の基準点および姿勢を示す実測値の軌跡Ｌ１１は、予め設定されるツール３０の基準点および姿勢を示す制御指令値の軌跡Ｌ１０に近づいていく。したがって演算部１８からの高精度のツール３０の基準点の位置および姿勢に基づく修正制御指令値を含む制御信号をロボット制御部１９に与えることによって、ツール３０のスライド変位および角変位を高精度に制御することができる。
【００４８】
以上のように本実施の形態の位置姿勢検出装置１０によれば、複数のカメラ１３によって、予め定めるロボット座標系における絶対位置が把握されている撮像位置から、ツール３０に設けられるターゲットユニット１１が撮影され、ターゲットユニット１１の画像を表す画像データが生成される。カメラ１３が複数、たとえば２個あれば、ターゲットユニット１１を両眼立体視することができる。またカメラ１３を３個以上にすることによって、ツール３０のスライド変位および角変位によって、各ターゲット３１〜３４が各カメラ１３の死角領域に入ってしまうことを可及的に防止することができる。
【００４９】
画像処理部１４によって、カメラ１３からの画像データに基づいて、ロボット座標系におけるツール３０に設定された基準点の絶対位置およびツール３０の絶対姿勢が求められる。このときターゲットユニット１１において、４個のターゲット３１〜３４のうち、いずれの３個のターゲットは同一直線上に並ばないので、３個のターゲットを含む平面を容易に求めて、その平面の法線を求めて、ターゲットを基準とするターゲット座標系を求めることができる。したがってロボット座標系とターゲット座標系とに基づいて、ロボット座標系におけるツール３０に設定された基準点の絶対位置およびツール３０の絶対姿勢を容易かつ正確に求めることができる。さらにツール３０に設けられるジャイロセンサ１５および加速度センサ１６によって、ツール３０の相対スライド変位および相対角変位が検出される。
【００５０】
演算部１８によって、画像処理部１４からのツール３０の基準点の絶対位置および絶対姿勢と、ジャイロセンサ１５および加速度センサ１６からのツール３０の相対スライド変位および相対角変位とに基づいて、ツール３０の基準点の位置および姿勢が求められる。これによって、たとえば画像処理部１４によって求められるツール３０の基準点の絶対位置および絶対姿勢が、ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対スライド変位および相対角変位よりも高精度であるけれども、画像処理部１４はツール３０の基準点の絶対位置および絶対姿勢を連続的に求めることができない場合、または、万一、ターゲットユニット１１が各カメラ１３の死角に入ってしまった場合、演算部１８は、画像処理部１４によってツール３０の基準点の絶対位置および絶対姿勢が求められるまでは、ジャイロセンサ１５および加速度センサ１６によって求められるツール３０の相対スライド変位および相対角変位に基づいて、ツール３０の基準点の位置および姿勢を求めることができる。このように演算部１８は、画像処理部１４ならびにジャイロセンサ１５および加速度センサ１６からのツール３０の基準点の絶対位置および絶対姿勢、ならびに相対スライド変位および相対角変位に基づいて、極めて高精度なツール３０の基準点の位置および姿勢を連続的に求めることができる。
【００５１】
また本実施の形態の位置姿勢検出装置１０によれば、演算部１８からの高精度のツール３０の基準点の位置および姿勢に基づいて、ツール３０のスライド変位および角変位を高精度に制御することができる。
【００５２】
本実施の形態の位置姿勢検出装置１０において、ターゲットユニット１１の各ターゲット３１〜３４は、同形状の球状に形成され、ＬＥＤによって互いに異なる色で発光するとしたけれども、各ターゲット３１〜３４は、画像処理部１４が、各ターゲット３１〜３４を区別可能であればよい。たとえば、各ターゲット３１〜３４を互いに異なる形状、具体的な一例としては、互いに異なる大きさの球にしてもよい。
【００５３】
また本実施の形態の位置姿勢検出装置１０において、ターゲットユニット１１は、ツール３０に設けられ、第１ターゲット３１は、第１ターゲット３１の重心がツール３０に設定される基準点となるようにして配置されるとしたけれども、これに限ることはない。たとえば、ターゲットユニット１１を２個用いてもよい。この場合、８つのターゲットは互いに異なる色で発光するようにして、各ターゲットユニット１１の第１ターゲット３１を、たとえば手首２４の軸線に関して対称となる位置に配置して、ツール３０の基準点をツール３０の重心として、各第１ターゲット３１とツール３０の基準点との位置関係を把握しておくようにする。ターゲットユニット１１を２個用いることによって、２つのターゲットユニット１１がともに、全てのカメラ１３の死角領域に入る可能性が極めて低くなるので、ほぼ確実にターゲット座標系を求めることができ、したがってツール３０の基準点の位置および姿勢を確実に求めることができる。
【００５４】
また各カメラ１３の視野を狭くし、かつ画像処理部１４によって求められる第１ターゲット３１の重心の座標に基づいて、ターゲットユニット１１を自動的に追尾する機能を有する雲台に連結して、各カメラ１３をスライド変位および角変位させるようにしてもよい。これによって、画像処理部１４によって求められるツール３０の基準点の位置および姿勢の精度を向上することができる。
【００５５】
図７は、ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対角変位および相対スライド変位を主にして、ツール３０の基準点の位置および姿勢を制御する場合の、ツール３０の基準点の位置および姿勢の実測値の真値からの誤差と時間との関係を示すグラフである。前述の本実施の形態では、画像処理部１４によって求められるツール３０の基準点の絶対位置および絶対姿勢を主とし、ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対角変位および相対スライド変位を副として、ツール３０の基準点の位置および姿勢を制御したが、これとは逆に、ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対角変位および相対スライド変位を主とし、画像処理部１４によって求められるツール３０の基準点の絶対位置および絶対姿勢を副として、ツール３０の基準点の位置および姿勢を制御するようにしてもよい。
【００５６】
この場合、演算部１８は、ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対角変位および相対スライド変位に基づく修正制御指令値を含む制御信号をロボット制御部１９に与え、所定の時間間隔となる時刻Ｕ１，Ｕ２，Ｕ３，…，Ｕｎにおいて、画像処理部１４によって求められるツール３０の基準点の絶対位置および絶対姿勢に基づく修正制御指令値を含む制御信号をロボット制御部１９に与えることによって、図７の破線Ｌ１２に示すように、ジャイロセンサ１５および加速度センサ１６によって検出される相対角変位および相対スライド変位に基づいて修正制御指令値に含まれる誤差を可及的に小さくすることができる。
【００５７】
また、たとえばロボット２０の動作領域内に複数の予め定める待機点を設定しておくようにしてもよい。この場合、ロボット２０が待機点に配置されたときに、画像処理部１４によって求められるツール３０の基準点の絶対位置および絶対姿勢に基づく修正制御指令値を含む制御信号をロボット制御部１９に与えることによって、図７の実線Ｌ１３に示すように、ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対角変位および相対スライド変位に基づく修正制御指令値に含まれる誤差をほぼ零にすることができる。
【００５８】
またロボット２０の繰返し精度が高いことを利用して、複数の待機点におけるツール３０の基準点の位置および姿勢を予め測定しておき、ツール３０が待機点に配置されたときに、ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対角変位および相対スライド変位に基づくツール３０の基準点の位置および姿勢を、予め測定しておいた数値に置きかえてもよい。
【００５９】
また本実施の形態の位置姿勢検出装置１０において、カメラ１３によってターゲットユニット１１を撮影し、その画像データに基づいて、ロボット座標系におけるツール３０の基準点の絶対位置および絶対姿勢を求め、ロボット座標系におけるツール３０の基準点の絶対位置および絶対姿勢を求めるとしたけれども、これに限ることはない。たとえば前述の待機点近傍に距離計測センサを設置して、ツール３０の基準点の位置および姿勢を測定するようにしてもよい。
【００６０】
またロボット２０が繰返し作業を行う場合は、ドライラン時にツール３０の基準点の位置および姿勢を測定し、作業前に自動的に制御指令値を修正しておくようにしてもよい。さらにセンサをツール３０として用いて、ワークの形状などを計測する計測ロボットとして利用する場合、ワークの計測終了後にツール３０の基準点および姿勢の誤差による計測データへの影響を可及的に小さくすることができる。
【００６１】
図８は、本発明の第２の実施形態の位置および姿勢検出装置５０の構成を示すブロック図である。図９は、ロボット２０およびレーザ干渉計５３を模式的に示す図である。位置および姿勢検出装置（以後、単に「位置姿勢検出装置」と表記することがある。）５０は、産業用ロボットなどのロボット２０の手首２４に連結されるツール３０などの可動体に設定された基準点の位置およびツール３０の姿勢を検出して、ツール３０のスライド変位および角変位を制御する。ロボット２０は、第１の実施形態におけるロボット２０と同様の軸垂直多関節ロボットであるので、同一の参照符号を付して、詳細な説明は省略する。本実施の形態において、ロボット座標系とは、ロボット２０のベース部２１が固定される床に平行に延び、互いに直交するｘ軸およびｙ軸、ならびに鉛直上向きに延びるｚ軸で構成される直交座標系である。
【００６２】
位置姿勢検出装置５０は、ターゲット５１、回転駆動部５２、レーザ干渉計５３、測距処理部５４、ジャイロセンサ１５、加速度センサ１６、信号処理部１７、演算部５８およびロボット制御部１９を含んで構成される。
【００６３】
ターゲット５１は、ロボット２０の上部アーム２３の先端部、ツールチェンジャーおよびツール３０のいずれかに設けられる。本実施の形態では、ターゲットユニット１１は、ツール３０に設けられ、ツール３０とともにスライド変位および角変位可能である。ターゲット５１は、たとえばコーナーキューブプリズムで実現され、レーザ干渉計５３からターゲット５１に入射したレーザ光を、レーザ光の入射方向とは平行な方向に反射する。駆動手段である回転駆動部５２は、たとえばステッピングモータ、またはインダクションモータとエンコーダとで実現され、演算部５８からの回転制御信号に基づいて、ターゲット５１を予め定める軸線である付加軸線Ｌ５２まわりに回転駆動するとともに、ターゲット５１の角度位置を検出可能である。このとき基準点は、ターゲット５１の回転中心となるように、付加軸線Ｌ５２を配置する。本実施の形態において、回転とは、３６０度未満の角変位および３６０度以上の回転を含むものとする。
【００６４】
ターゲット距離測定手段であるレーザ干渉計５３は、予め定める座標系であるロボット座標系における絶対位置が把握されている測距位置に配置され、レーザ光を射出して、ターゲット５１にレーザ光を入射して、その反射光と、射出したレーザ光と同位相のレーザ光とを干渉させて、干渉強度の最大値および最小値の変化の回数を計数することによって、前記側距位置からターゲット５１までの距離を測定する。レーザ干渉計５３には、ターゲット５１を追尾する機能を有し、ターゲット５１にレーザ光を常に入射させることができるとともに、レーザ光の射出方向を検出可能である。
【００６５】
第１検出手段である測距処理部５４は、たとえばマイクロコンピュータで実現され、演算部５８からの計測指令信号が与えられると、レーザ干渉計５３によって測定された距離および検出されたレーザ光の射出方向に基づいて、ロボット座標系におけるツール３０に設定された基準点の絶対位置およびツール３０の絶対姿勢を求め、求めたツール３０の基準点の絶対位置および絶対姿勢を含む絶対位置姿勢検出信号を演算部５８に与える。測距処理部５４は、詳細に述べると、レーザ干渉計５３によって測定された距離および検出されたレーザ光の射出方向に基づいて、レーザ干渉計５３の座標系である干渉計座標系におけるツール３０の基準点の絶対位置および絶対姿勢を求めてから、ロボット座標系におけるツール３０の基準点の絶対位置および絶対姿勢に変換する。
【００６６】
第２検出手段であるジャイロセンサ１５および加速度センサ１６は、第１の実施形態の位置姿勢検出装置１０におけるジャイロセンサ１５および加速度センサ１６と同様であるので、同一の参照符号を付して、詳細な説明は省略する。信号処理部１７は、第１の実施形態の位置姿勢検出装置１０における信号処理部１７と同様であるので、同一の参照符号を付して、詳細な説明は省略する。
【００６７】
演算手段である演算部５８は、たとえばＣＰＵなどの演算処理装置、ならびにランダムアクセスメモリ、リードオンリーメモリおよびハードディスクドライブなどの記憶装置を含んで実現され、位置姿勢検出装置５０を統括的に制御する。演算部５８は、ターゲット５１の付加軸線Ｌ５２まわりの回転駆動を示す回転制御信号を回転駆動部５２与える。また演算部１８は、レーザ干渉計５３によって測定された距離および検出されたレーザ光の射出方向に基づいて、ロボット座標系におけるツール３０に設定されたツール３０の基準点の絶対位置および絶対姿勢を求めることを示す計測指令信号を、測距処理部５４に与える。さらに演算部５８は、ロボット制御部１９から指令要請信号が与えられると、測距処理部５４からの絶対位置姿勢検出信号に含まれるロボット座標系におけるツール３０の基準点の絶対位置および絶対姿勢と、信号処理部１７からの相対変位検出信号に含まれるツール３０の相対角変位および相対スライド変位とに基づいて、ツール３０の基準点の位置および姿勢を求めて、求めた位置および姿勢に基づく修正制御指令値を含む制御信号をロボット制御部１９に与える。
【００６８】
制御手段であるロボット制御部１９は、第１の実施の形態の位置姿勢検出装置１０におけるロボット制御部１９と同様であるので、同一の参照符号を付して、詳細な説明は省略する。
【００６９】
図１０は、レーザ干渉計５３およびロボット２０における干渉計座標系、付加軸座標系およびロボット座標系を模式的に示す図である。図１１は、ツール３０がスライド変位および角変位していない静止状態で、ツール３０の基準点の位置および姿勢を検出する方法を説明するための図である。ツール３０がスライド変位および角変位していない静止状態で、ツール３０の基準点の位置および姿勢を検出する方法を以下に説明する。
【００７０】
演算部５８は、予め定める第１静止角度位置Ｍ１にターゲット５１を配置することを示す回転制御信号を回転駆動部５２に与える。回転駆動部５２は、前記回転制御信号に基づいて、ターゲット５１を付加軸線Ｌ５２まわりに回転させて第１静止角度位置Ｍ１に配置する。このときレーザ干渉計５３は、第１静止角度位置Ｍ１に配置されるターゲット５１と測距位置との距離を測定するとともに、レーザ光の射出方向を検出する。測距処理部５４は、測定された距離および検出された射出方向に基づいて、第１静止角度位置Ｍ１に配置されるターゲット５１の干渉計座標系の第１静止位置の干渉計座標系における座標を求める。
【００７１】
続いて演算部５８は、第１静止角度位置Ｍ１から予め定める回転方向に９０度角変位した角度位置である第２静止角度位置Ｍ２にターゲット５１を配置することを示す回転制御信号を回転駆動部５２に与える。回転駆動部５２は、前記回転制御信号に基づいて、ターゲット５１を付加軸線Ｌ５２まわりに回転させて第２静止角度位置Ｍ２に配置する。このときレーザ干渉計５３は、第２静止角度位置Ｍ２に配置されるターゲット５１と測距位置との距離を測定するとともに、レーザ光の射出方向を検出する。測距処理部５４は、測定された距離および検出された射出方向に基づいて、第２静止角度位置Ｍ２に配置されるターゲット５１の干渉計座標系の第２静止位置の干渉計座標系における座標を求める。
【００７２】
さらに演算部５８は、第１静止角度位置Ｍ１から前記回転方向に２７０度角変位した角度位置である第３静止角度位置Ｍ３にターゲット５１を配置することを示す回転制御信号を回転駆動部５２に与える。回転駆動部５２は、前記回転制御信号に基づいて、ターゲット５１を付加軸線Ｌ５２まわりに回転させて第３静止角度位置Ｍ３に配置する。このときレーザ干渉計５３は、第３静止角度位置Ｍ３に配置されるターゲット５１と測距位置との距離を測定するとともに、レーザ光の射出方向を検出する。測距処理部５４は、測定された距離および検出された射出方向に基づいて、第３静止角度位置Ｍ３に配置されるターゲット５１の干渉計座標系の第３静止位置の干渉計座標系における座標を求める。
【００７３】
このようにターゲット５１の３つの静止角度位置Ｍ１〜Ｍ３における干渉計座標系の同一直線上に並ばない３つの静止位置が求められたので、測距処理部５４は、３つの第１〜第３静止位置を含む平面を求めるとともに、これら３つの静止位置およびターゲット５１の回転半径から回転中心を決定して、前記回転中心から第１静止位置に向かう第１基底ベクトルＶｘ、前記回転中心から第２静止位置に向かう第２基底ベクトルＶｙ、および前記回転中心から前記平面に垂直な方向に向かう第３基底ベクトルＶｚを求める。このようにして前記回転中心すなわち基準点を座標原点とし、互いに直交する第１〜第３基底ベクトルＶｘ，Ｖｙ，Ｖｚによって構成されるターゲット座標系が求められる。これによってターゲット座標系の干渉計座標系に対する傾斜がわかるので、また、ロボット座標系と干渉計座標系との関係も既知であるので、ツール３０の絶対姿勢を求めることができる。
【００７４】
ここで万一、レーザ干渉計５３が、前記第１〜第３角度位置Ｍ１〜Ｍ３に配置されるターゲット５１にレーザ光を入射させることができなくても、前記第１〜第３角度位置Ｍ１〜Ｍ３以外の同一直線上に無い３つの角度位置にターゲット５１を配置して、これらの角度位置に配置されるターゲット５１の干渉計座標系における第１〜第３位置を求めることができる。これによって測距処理部５４は、この干渉計座標系における第１〜第３位置である３つの位置を含む平面を求めるとともに、これら３つの位置およびターゲット５１の回転半径から回転中心を求めて、前記回転中心を座標原点とし、互いに直交する第１〜第３基底ベクトルＶｘ，Ｖｙ，Ｖｚを求めて、ターゲット座標系を求めることができる。
【００７５】
図１２は、ツール３０がスライド変位および角変位している動作状態で、ツール３０の基準点の位置および姿勢を検出する方法を説明するための図である。ツール３０がスライド変位および角変位している動作状態で、ツール３０の基準点の位置および姿勢を検出する方法を以下に説明する。
【００７６】
演算部５８は、ロボット２０のツール３０のスライド速度よりも充分に速い回転速度でターゲット５１を回転することを示す回転制御信号を回転駆動部５２に与える。回転駆動部５２は、前記回転制御信号に基づいて、ツール３０のスライド速度よりも充分に速い回転速度でターゲット５１を付加軸線Ｌ５２まわりに回転させる。ターゲット５１が、ツール３０のスライド速度よりも充分に速い回転速度で付加軸線Ｌ５２まわりに回転することによって、所定の時間間隔となる複数、本実施の形態では３つの時刻におけるターゲット５１の３つの位置が、近似的に、３つの時刻のうちの最も最近の時刻における付加軸線Ｌ５２に垂直な平面上に存在する。図１２は、理解を容易にするために、３つの角度位置Ｄ１〜Ｄ３が重ならないように、それぞれ異なる平面（付加軸線Ｌ５２に垂直な平面）上に存在しているように表記しているが、実際には、矢符Ｃの方向には、ほとんど間隔をあけていない。
【００７７】
レーザ干渉計５３は、前述の所定の時間間隔で、回転しているターゲット５１と測距位置との距離を測定するとともに、レーザ光の射出方向を検出する。そして測距処理部５４は、測定された距離および検出された射出方向に基づいて、測定された各時刻におけるターゲット５１の干渉計座標系における座標を求める。
【００７８】
ある測定時刻ｔｉにおけるツール３０の基準点の絶対位置および絶対姿勢を求めるためには、前記測定時刻ｔｉを含む３つの時刻におけるターゲット５１の干渉計座標における座標が必要である。本実施の形態では、測定時刻ｔｉと、前記測定時刻ｔｉよりも所定の時間間隔だけ過去の第１過去時刻ｔｉ−１と、前記第１過去時刻ｔｉ−１よりも所定の時間間隔だけ過去の第２過去時刻ｔｉ−２とにおけるターゲット５１の干渉計座標における座標を用いる。
【００７９】
測定時刻ｔｉにおいて、ターゲット５１は、予め定める基準角度位置Ｗから前記回転方向に第１角度θｉだけ角変位した第１回転角度位置Ｄ１にあるとする。また第１過去時刻ｔｉ−１において、ターゲット５１は、基準角度位置Ｗから前記回転方向に第２角度θｉ−１だけ角変位した第２回転角度位置Ｄ２にあるとする。さらに第２過去時刻ｔｉ−２において、ターゲット５１は、基準角度位置Ｗから前記回転方向に第３角度θｉ−２だけ角変位した第３回転角度位置Ｄ３にあるとする。
【００８０】
レーザ干渉計５３および測距処理部５４によって、各時刻ｔｉ−２，ｔｉ−１，ｔｉにおけるターゲット５１の各回転角度位置Ｄ１〜Ｄ３の干渉計座標系における座標が求められる。前述のように、これらの３つの座標は、近似的に、測定時刻ｔｉにおける付加軸線Ｌ５２に垂直な平面上に存在する。したがって、これらの３つの座標は、同一直線上に並ばないので、測距処理部５４は、これら３つの座標を含む円周を有する円の中心の干渉計座標系における座標を求める。この円の中心の座標が、ターゲット５１の回転中心であり、すなわちツール３０の基準点の干渉計座標系の座標である。ロボット座標系と干渉計座標系との関係は既知であるので、ツール３０の基準点の絶対位置を求めることができる。
【００８１】
また測距処理部５４は、前述の３つの座標を含む平面を求めるとともに、前記３つの座標と、ターゲット５１の回転速度と、所定の時間間隔と、第１〜第３角度θｉ〜θｉ−２とに基づいて、基準角度位置Ｗの干渉計座標系における座標を求める。さらに測距処理部５４は、前記回転中心から基準角度位置Ｗに向かう第１基底ベクトルＶｘ、前記回転中心から前記平面に平行であって、前記第１基底ベクトルＶｘに回転方向に角度９０度を成す第２基底ベクトルＶｙ、および前記回転中心から前記平面に垂直な方向に向かう第３基底ベクトルＶｚを求める。このようにして前記回転中心すなわち基準点を座標原点とし、互いに直交する第１〜第３基底ベクトルＶｘ，Ｖｙ，Ｖｚによって構成されるターゲット座標系が求められる。これによってターゲット座標系の干渉計座標系に対する傾斜がわかるので、また、ロボット座標系と干渉計座標系との関係も既知であるので、ツール３０の絶対姿勢を求めることができる。
【００８２】
このようにロボット２０のツール３０のスライド速度よりも充分に速い回転速度でターゲット５１を回転させることによって、たとえば、ターゲット５１と回転中心との距離を１００ミリメートルとし、ロボット２０のツール３０が１ミリメートルだけスライド変位する間に、ターゲット５１が角度９０度だけ回転するようにすれば、ｔａｎ−１（１／１００）≒０．５度の精度で、ツール３０の絶対姿勢を求めることができる。
【００８３】
以上のように本実施の形態の位置姿勢検出装置５０によれば、レーザ干渉計５３によって、ロボット座標系における絶対位置が把握されている測距位置から、ツール３０に設けられ、回転駆動部５２によって付加軸線Ｌ５２まわりに回転するターゲット５１までの距離が測定されるとともにレーザ光の射出方向が検出される。測距処理部５４によって、レーザ干渉計５３によって測定された距離および検出されたレーザ光の射出方向に基づいて、ロボット座標系におけるツール３０に設定された基準点の絶対位置およびツール３０の絶対姿勢が求められる。ターゲット５１が角変位することによって、レーザ干渉計５３は、ロボット座標系における絶対位置が把握されている測距位置から、互いに異なる３つの角度位置に配置されるターゲット５１までの距離を測定するとともに、レーザ光の射出方向を検出することができる。このとき、たとえばターゲット５１の互いに異なる３つの位置が同一直線上に並ばない場合、この３つの位置を含む平面を容易に求めて、その平面の法線を決定して、ターゲット５１を基準とするターゲット座標系を求めることができる。したがってロボット座標系、干渉計座標系およびターゲット座標系に基づいて、ロボット座標系におけるツール３０に設定された基準点の絶対位置およびツール３０の絶対姿勢を容易かつ正確に求めることができる。さらにツール３０に設けられるジャイロセンサ１５および加速度センサ１６によって、ツール３０の相対スライド変位および相対角変位が検出される。
【００８４】
演算部５８によって、測距処理部５４からのツール３０の基準点の絶対位置および絶対姿勢と、ジャイロセンサ１５および加速度センサ１６からのツール３０の相対スライド変位および相対角変位とに基づいて、ツール３０の基準点の位置および姿勢が求められる。これによって、たとえば測距処理部５４によって求められるツール３０の基準点の絶対位置および絶対姿勢が、ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対スライド変位および相対角変位よりも高精度であるけれども、測距処理部５４はツール３０の基準点の絶対位置および絶対姿勢を連続的に求めることができない場合、または、万一、ターゲット５１がレーザ干渉計５３の死角領域に入ってしまった場合、演算部５８は、測距処理部５４によってツール３０の基準点の絶対位置および絶対姿勢が求められるまでは、ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対スライド変位および相対角変位に基づいて、ツール３０の基準点の位置および姿勢を求めることができる。このように測距処理部５４ならびにジャイロセンサ１５および加速度センサ１６からのツール３０の基準点の絶対位置および絶対姿勢、ならびにツール３０の相対スライド変位および相対角変位に基づいて、極めて高精度なツール３０の基準点の位置および姿勢を連続的に求めることができる。
【００８５】
また本実施の形態のロボットの位置および姿勢検出装置５０によれば、演算部５８からの高精度のツール３０の基準点の位置および姿勢に基づいて、ツール３０のスライド変位および角変位を高精度に制御することができる。
【００８６】
本実施の形態のロボットの位置および姿勢検出装置５０において、ロボット２０の動作中においてツール３０の姿勢がほとんど変化しない場合、ロボット２０の動作領域内に複数の予め定める待機点を設定して、各待機点においてロボット２０の動作が停止しているときに、ターゲット５１を回転させてツール３０の姿勢を求めるようにしてもよい。
【００８７】
本実施の形態のロボットの位置および姿勢検出装置５０において、ターゲット距離測定手段はレーザ干渉計５１としたけれども、これに限ることなく、たとえばＣＣＤを有する２台のカメラでもよい。
【００８８】
【発明の効果】
請求項１記載の本発明によれば、たとえば第１検出手段によって求められる可動体の基準点の絶対位置および絶対姿勢が、第２検出手段によって検出される可動体の相対スライド変位および相対角変位よりも高精度であるけれども、第１検出手段は可動体の基準点の絶対位置および絶対姿勢を連続的に求めることができない場合、または、万一、ターゲットユニットが各撮影手段の死角領域に入ってしまった場合、演算手段は、第１検出手段によって可動体の基準点の絶対位置および絶対姿勢が求められるまでは、第２検出手段によって検出される可動体の相対スライド変位および相対角変位に基づいて、可動体の基準点の位置および姿勢を求めることができる。このように演算手段は、２つの検出手段からの可動体の基準点の絶対位置および絶対姿勢、ならびに可動体の相対スライド変位および相対角変位に基づいて、極めて高精度な可動体の基準点の位置および姿勢を連続的に求めることができる。
【００８９】
請求項２記載の本発明によれば、たとえば第１検出手段によって求められる可動体の基準点の絶対位置および絶対姿勢が、第２検出手段によって検出される可動体の相対スライド変位および相対角変位よりも高精度であるけれども、第１検出手段は可動体の基準点の絶対位置および絶対姿勢を連続的に求めることができない場合、または、万一、ターゲットがターゲット距離測定手段の死角領域に入ってしまった場合、演算手段は、第１検出手段によって可動体の基準点の絶対位置および絶対姿勢が求められるまでは、第２検出手段によって検出される可動体の相対スライド変位および相対角変位に基づいて、可動体の基準点の位置および姿勢を求めることができる。このように演算手段は、２つの検出手段からの可動体の基準点の絶対位置および絶対姿勢、ならびに可動体の相対スライド変位および相対角変位に基づいて、極めて高精度な可動体の基準点の位置および姿勢を連続的に求めることができる。
【００９０】
請求項３記載の本発明によれば、演算手段からの高精度の可動体の基準点の位置および可動体の姿勢に基づいて、可動体のスライド変位および角変位を高精度に制御することができる。
【図面の簡単な説明】
【図１】
本発明の第１の実施形態の位置および姿勢検出装置１０の構成を示すブロック図である。
【図２】ロボット２０および複数のカメラ１３Ａ，１３Ｂ，１３Ｃを模式的に示す図である。
【図３】ターゲットユニット１１を示す斜視図である。
【図４】カメラ１３を用いて、ある点Ｐのロボット座標系における座標（Ｘ，Ｙ，Ｚ）を求める方法を説明するための図である。
【図５】ロボット制御部１９によるツール３０のスライド変位および角変位を制御する手順を示すフローチャートである。
【図６】ロボット制御部１９によるツール３０のスライド変位および角変位を制御する方法を説明するための図である。
【図７】ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対角変位および相対スライド変位を主にして、ツール３０の基準点の位置および姿勢を制御する場合の、ツール３０の基準点の位置および姿勢の実測値の真値からの誤差と時間との関係を示すグラフである。
【図８】本発明の第２の実施形態の位置および姿勢検出装置５０の構成を示すブロック図である。
【図９】ロボット２０およびレーザ干渉計５３を模式的に示す図である。
【図１０】レーザ干渉計５３およびロボット２０における干渉計座標系、付加軸座標系およびロボット座標系を模式的に示す図である。
【図１１】ツール３０がスライド変位および角変位していない静止状態で、ツール３０の基準点の位置および姿勢を検出する方法を説明するための図である。
【図１２】ツール３０がスライド変位および角変位している動作状態で、ツール３０の基準点の位置および姿勢を検出する方法を説明するための図である。
【符号の説明】
１０，５０　位置姿勢検出装置
１１　ターゲットユニット
３１，３２，３３，３４；５１　ターゲット
１３　カメラ
１４　画像処理部
１５　ジャイロセンサ
１６　加速度センサ
１８，５８　演算部
１９　ロボット制御部
３０　ツール
５２　回転駆動部
５３　レーザ干渉計
５４　測距処理部
【図１】
2004050356.tif 000006
【図２】
2004050356.tif 000007
【図３】
2004050356.tif 000008
【図４】
2004050356.tif 000009
【図５】
2004050356.tif 000010
【図６】
2004050356.tif 000011
【図７】
2004050356.tif 000012
【図８】
2004050356.tif 000013
【図９】
2004050356.tif 000014
【図１０】
2004050356.tif 000015
【図１１】
2004050356.tif 000016
【図１２】
2004050356.tif 000017