JP2004050356A - Position and attitude sensor of movable structure - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a position and posture sensor of a movable body which precisely detects the reference point of the position and posture set at the movable body. <P>SOLUTION: The sensor is provided with: a target unit 11 which has three targets not aligned on the same straight line and is mounted on a tool 30; a plurality of cameras 13 which images the target unit from a camera position where a absolute position is grasped in a robot coordinate system and creates an image data showing the image of the target unit; an image processing section 14 which finds the absolute posture of the reference point and the absolute of the tool set on the tool in the robot coordinate system based on the image data from the cameras; a gyro sensor 15 and an acceleration sensor 16 which are mounted on the tool and detect the relative angular displacement and relative slide displacement of the tool; and an operation section 18 which finds the position and posture of the reference point on the tool based on the relative slide displacement and relative angular displacement of the tool and the absolute position and absolute posture of the tool. <P>COPYRIGHT: (C)2004,JPO

Description

【００３１】
前式（１）において、ｈ_１は、同次座標表現における媒介変数であり、右辺における左側の行列は、予め定められる第１カメラ１３Ａのカメラパラメータ行列である。前式（２）において、ｈ_２は、同次座標表現における媒介変数であり、右辺における左側の行列は、予め定められる第２カメラ１３Ｂのカメラパラメータ行列である。
【００３２】
画像処理部１４は、第１カメラ１３Ａからの画像データに基づいて、点Ｑ１の座標（Ｘ_Ｃ１，Ｙ_Ｃ１）、および第２カメラ１３Ｂからの画像データに基づいて、点Ｑ２の座標（Ｘ_Ｃ２，Ｙ_Ｃ２）を求めて、前式（１）および式（２）を連立させて、各媒介変数ｈ_１，ｈ_２を消去して、点Ｐのロボット座標系における座標（Ｘ，Ｙ，Ｚ）に関して整理すると、次式（３）が得られる。
Ｂ＝Ａ・Ｖ　　　　　　　　　　　　　　　　　　　　　　…（３）
【００３３】
前式（３）の左辺の行列Ｂは次式（５）で表され、右辺における左側の行列Ａは次式（４）で表され、右辺における右側の行列Ｖは次式（６）で表される。
【００３４】
【数２】

【００３５】
したがって、点Ｐのロボット座標系における座標（Ｘ，Ｙ，Ｚ）は、次式（７）のように求められる。
Ｖ＝（Ａ^Ｔ・Ａ）^−１・Ａ^Ｔ・Ｂ　　　　　　　　　　　　　…（７）
【００３６】
前式（７）において、Ｔは行列の転置を示す。
前述の点Ｐのロボット座標系における座標（Ｘ，Ｙ，Ｚ）の求め方は、第１カメラ１３Ａおよび第２カメラ１３Ｂの２台のカメラ１３を用いた場合であるが、カメラ１３を３台以上用いる場合であっても同様にして点Ｐのロボット座標系における座標（Ｘ，Ｙ，Ｚ）を求めることができる。このようにしてロボット座標系におけるターゲットユニット１１の第１ターゲット３１の重心Ｌ１の座標（Ｘ_Ｌ１，Ｙ_Ｌ１，Ｚ_Ｌ１）を求めることによって、ツール３０に設定された基準点の絶対位置を求めることができる。
【００３７】
また、このようにして画像処理部１４は、カメラ１３からの画像データに基づいて、ターゲットユニット１１の第１〜第４ターゲット３１〜３４の重心の座標が得られる。ロボット座標系における第１ターゲット３１の重心Ｌ１の座標（Ｘ_Ｌ１，Ｙ_Ｌ１，Ｚ_Ｌ１）、第２ターゲット３２の重心Ｌ２の座標（Ｘ_Ｌ２，Ｙ_Ｌ２，Ｚ_Ｌ２）、第３ターゲット３３の重心Ｌ３の座標（Ｘ_Ｌ３，Ｙ_Ｌ３，Ｚ_Ｌ３）、第４ターゲット３４の重心Ｌ４の座標（Ｘ_Ｌ４，Ｙ_Ｌ４，Ｚ_Ｌ４）とすると、第１ターゲット３１から第２ターゲット３２に向かう第１基底ベクトルＶｘ、第１ターゲット３１から第３ターゲット３３に向かう第２基底ベクトルＶｙおよび第１ターゲット３１から第４ターゲット３４に向かう第３基底ベクトルＶｚは、それぞれ次式（８）、式（９）および式（１０）で表される。
【００３８】
【数３】

【００３９】
このようにして第１ターゲット３１の重心Ｌ１を座標原点とし、互いに直交する第１〜第３基底ベクトルＶｘ，Ｖｙ，Ｖｚによって構成されるターゲット座標系が求められる。これによってターゲット座標系のロボット座標系に対する傾斜がわかるので、ツール３０の絶対姿勢を求めることができる。
【００４０】
ここで万一、ターゲットユニット１１の第２〜第４ターゲット３２〜３４のいずれか１つがカメラ１３によって撮影できなくても、第１ターゲット３１を含む３つの残余のターゲットの重心の座標を求めることによって、前記３つの残余のターゲットの重心を含む平面が求められるので、この平面の法線を求めることができる。これによってターゲット座標系を求めることができる。
【００４１】
このようなターゲットユニット１１をカメラ１３で撮影して、その画像データに基づいてターゲット座標系を求めて、ツール３０に設定された基準点の位置およびツール３０の姿勢を求めるには、多少の時間、たとえば０．１秒程度を要するので、画像処理部１４によってツール３０の基準点の位置および姿勢が求められてから、次に画像処理部１４によってツール３０の基準点の位置および姿勢が求められるまでの間は、ジャイロセンサ１５および加速度センサ１６によって検出される相対角変位および相対スライド変位に基づいて、演算部１８は、ツール３０の基準点の位置および姿勢を求める。
【００４２】
ジャイロセンサ１５および加速度センサ１６によって検出される相対角変位および相対スライド変位に基づくツール３０の基準点の位置および姿勢は、画像処理部１４によって求められるツール３０の基準点の位置および姿勢に比べて精度が低いけれども、相対角変位および相対スライド変位の誤差が蓄積されるまでには、新たに画像処理部１４によってツール３０の基準点の位置および姿勢が求められるので、連続して精度の高いツール３０の基準点の位置および姿勢を求めることができる。
【００４３】
図５は、ロボット制御部１９によるツール３０のスライド変位および角変位を制御する手順を示すフローチャートである。図６は、ロボット制御部１９によるツール３０のスライド変位および角変位を制御する方法を説明するための図である。ロボット制御部１９がロボット２０に制御信号を与える時間間隔である指令サイクルをｔ秒とする。ステップｓ０で、制御手順が開始されて、ステップｓ１に進む。
【００４４】
ステップｓ１では、演算部１８は、ある時刻Ｔ_ｉにおいて求めたツール３０の基準点および姿勢を示す実測値Ｒ_ｉと、予め設定される時刻Ｔ_ｉにおけるツール３０の基準点および姿勢を示す制御指令値Ｈ_ｉとの誤差ベクトルΔ_ｉを求めて、ステップｓ２に進む。ここで添字ｉは整数である。
【００４５】
ステップｓ２では、演算部１８は、時刻Ｔ_ｉからｔ秒後の時刻Ｔ_ｉ＋１における予め設定されているロボット２０のツール３０の基準点および姿勢を示す制御指令値Ｈ_ｉ＋１に、誤差ベクトルの総和ΣΔ_ｉ−１と、ステップｓ１で求めた誤差ベクトルΔ_ｉとを加算して、時刻Ｔ_ｉ＋１における修正制御指令値Ｎ_ｉ＋１を求めて、ステップｓ３に進む。誤差ベクトルの総和ΣΔ_ｉ−１とは、時刻Ｔ_ｉのｔ秒前の時刻Ｔ_ｉ−１までに累積されている演算部１８によって求められたツール３０の基準点および姿勢を示す実測値と、予め設定されるロボット２０のツール３０の基準点および姿勢を示す制御指令値との誤差ベクトルの総和である。
【００４６】
ステップｓ３では、演算部１８は、時刻Ｔ_ｉ＋１において、ステップｓ２で求めた修正制御指令値Ｎ_ｉ＋１を含む制御信号をロボット制御部１９に与えて、ステップｓ４に進む。ステップｓ４では、添字ｉをｉ＋１に置換えて、ステップｓ１に戻る。
【００４７】
このような手順で制御を行うことによって、図６に示すように、演算部１８によって求められたツール３０の基準点および姿勢を示す実測値の軌跡Ｌ１１は、予め設定されるツール３０の基準点および姿勢を示す制御指令値の軌跡Ｌ１０に近づいていく。したがって演算部１８からの高精度のツール３０の基準点の位置および姿勢に基づく修正制御指令値を含む制御信号をロボット制御部１９に与えることによって、ツール３０のスライド変位および角変位を高精度に制御することができる。
【００４８】
以上のように本実施の形態の位置姿勢検出装置１０によれば、複数のカメラ１３によって、予め定めるロボット座標系における絶対位置が把握されている撮像位置から、ツール３０に設けられるターゲットユニット１１が撮影され、ターゲットユニット１１の画像を表す画像データが生成される。カメラ１３が複数、たとえば２個あれば、ターゲットユニット１１を両眼立体視することができる。またカメラ１３を３個以上にすることによって、ツール３０のスライド変位および角変位によって、各ターゲット３１〜３４が各カメラ１３の死角領域に入ってしまうことを可及的に防止することができる。
【００４９】
画像処理部１４によって、カメラ１３からの画像データに基づいて、ロボット座標系におけるツール３０に設定された基準点の絶対位置およびツール３０の絶対姿勢が求められる。このときターゲットユニット１１において、４個のターゲット３１〜３４のうち、いずれの３個のターゲットは同一直線上に並ばないので、３個のターゲットを含む平面を容易に求めて、その平面の法線を求めて、ターゲットを基準とするターゲット座標系を求めることができる。したがってロボット座標系とターゲット座標系とに基づいて、ロボット座標系におけるツール３０に設定された基準点の絶対位置およびツール３０の絶対姿勢を容易かつ正確に求めることができる。さらにツール３０に設けられるジャイロセンサ１５および加速度センサ１６によって、ツール３０の相対スライド変位および相対角変位が検出される。
【００５０】
演算部１８によって、画像処理部１４からのツール３０の基準点の絶対位置および絶対姿勢と、ジャイロセンサ１５および加速度センサ１６からのツール３０の相対スライド変位および相対角変位とに基づいて、ツール３０の基準点の位置および姿勢が求められる。これによって、たとえば画像処理部１４によって求められるツール３０の基準点の絶対位置および絶対姿勢が、ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対スライド変位および相対角変位よりも高精度であるけれども、画像処理部１４はツール３０の基準点の絶対位置および絶対姿勢を連続的に求めることができない場合、または、万一、ターゲットユニット１１が各カメラ１３の死角に入ってしまった場合、演算部１８は、画像処理部１４によってツール３０の基準点の絶対位置および絶対姿勢が求められるまでは、ジャイロセンサ１５および加速度センサ１６によって求められるツール３０の相対スライド変位および相対角変位に基づいて、ツール３０の基準点の位置および姿勢を求めることができる。このように演算部１８は、画像処理部１４ならびにジャイロセンサ１５および加速度センサ１６からのツール３０の基準点の絶対位置および絶対姿勢、ならびに相対スライド変位および相対角変位に基づいて、極めて高精度なツール３０の基準点の位置および姿勢を連続的に求めることができる。
【００５１】
また本実施の形態の位置姿勢検出装置１０によれば、演算部１８からの高精度のツール３０の基準点の位置および姿勢に基づいて、ツール３０のスライド変位および角変位を高精度に制御することができる。
【００５２】
本実施の形態の位置姿勢検出装置１０において、ターゲットユニット１１の各ターゲット３１〜３４は、同形状の球状に形成され、ＬＥＤによって互いに異なる色で発光するとしたけれども、各ターゲット３１〜３４は、画像処理部１４が、各ターゲット３１〜３４を区別可能であればよい。たとえば、各ターゲット３１〜３４を互いに異なる形状、具体的な一例としては、互いに異なる大きさの球にしてもよい。
【００５３】
また本実施の形態の位置姿勢検出装置１０において、ターゲットユニット１１は、ツール３０に設けられ、第１ターゲット３１は、第１ターゲット３１の重心がツール３０に設定される基準点となるようにして配置されるとしたけれども、これに限ることはない。たとえば、ターゲットユニット１１を２個用いてもよい。この場合、８つのターゲットは互いに異なる色で発光するようにして、各ターゲットユニット１１の第１ターゲット３１を、たとえば手首２４の軸線に関して対称となる位置に配置して、ツール３０の基準点をツール３０の重心として、各第１ターゲット３１とツール３０の基準点との位置関係を把握しておくようにする。ターゲットユニット１１を２個用いることによって、２つのターゲットユニット１１がともに、全てのカメラ１３の死角領域に入る可能性が極めて低くなるので、ほぼ確実にターゲット座標系を求めることができ、したがってツール３０の基準点の位置および姿勢を確実に求めることができる。
【００５４】
また各カメラ１３の視野を狭くし、かつ画像処理部１４によって求められる第１ターゲット３１の重心の座標に基づいて、ターゲットユニット１１を自動的に追尾する機能を有する雲台に連結して、各カメラ１３をスライド変位および角変位させるようにしてもよい。これによって、画像処理部１４によって求められるツール３０の基準点の位置および姿勢の精度を向上することができる。
【００５５】
図７は、ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対角変位および相対スライド変位を主にして、ツール３０の基準点の位置および姿勢を制御する場合の、ツール３０の基準点の位置および姿勢の実測値の真値からの誤差と時間との関係を示すグラフである。前述の本実施の形態では、画像処理部１４によって求められるツール３０の基準点の絶対位置および絶対姿勢を主とし、ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対角変位および相対スライド変位を副として、ツール３０の基準点の位置および姿勢を制御したが、これとは逆に、ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対角変位および相対スライド変位を主とし、画像処理部１４によって求められるツール３０の基準点の絶対位置および絶対姿勢を副として、ツール３０の基準点の位置および姿勢を制御するようにしてもよい。
【００５６】
この場合、演算部１８は、ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対角変位および相対スライド変位に基づく修正制御指令値を含む制御信号をロボット制御部１９に与え、所定の時間間隔となる時刻Ｕ１，Ｕ２，Ｕ３，…，Ｕｎにおいて、画像処理部１４によって求められるツール３０の基準点の絶対位置および絶対姿勢に基づく修正制御指令値を含む制御信号をロボット制御部１９に与えることによって、図７の破線Ｌ１２に示すように、ジャイロセンサ１５および加速度センサ１６によって検出される相対角変位および相対スライド変位に基づいて修正制御指令値に含まれる誤差を可及的に小さくすることができる。
【００５７】
また、たとえばロボット２０の動作領域内に複数の予め定める待機点を設定しておくようにしてもよい。この場合、ロボット２０が待機点に配置されたときに、画像処理部１４によって求められるツール３０の基準点の絶対位置および絶対姿勢に基づく修正制御指令値を含む制御信号をロボット制御部１９に与えることによって、図７の実線Ｌ１３に示すように、ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対角変位および相対スライド変位に基づく修正制御指令値に含まれる誤差をほぼ零にすることができる。
【００５８】
またロボット２０の繰返し精度が高いことを利用して、複数の待機点におけるツール３０の基準点の位置および姿勢を予め測定しておき、ツール３０が待機点に配置されたときに、ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対角変位および相対スライド変位に基づくツール３０の基準点の位置および姿勢を、予め測定しておいた数値に置きかえてもよい。
【００５９】
また本実施の形態の位置姿勢検出装置１０において、カメラ１３によってターゲットユニット１１を撮影し、その画像データに基づいて、ロボット座標系におけるツール３０の基準点の絶対位置および絶対姿勢を求め、ロボット座標系におけるツール３０の基準点の絶対位置および絶対姿勢を求めるとしたけれども、これに限ることはない。たとえば前述の待機点近傍に距離計測センサを設置して、ツール３０の基準点の位置および姿勢を測定するようにしてもよい。
【００６０】
またロボット２０が繰返し作業を行う場合は、ドライラン時にツール３０の基準点の位置および姿勢を測定し、作業前に自動的に制御指令値を修正しておくようにしてもよい。さらにセンサをツール３０として用いて、ワークの形状などを計測する計測ロボットとして利用する場合、ワークの計測終了後にツール３０の基準点および姿勢の誤差による計測データへの影響を可及的に小さくすることができる。
【００６１】
図８は、本発明の第２の実施形態の位置および姿勢検出装置５０の構成を示すブロック図である。図９は、ロボット２０およびレーザ干渉計５３を模式的に示す図である。位置および姿勢検出装置（以後、単に「位置姿勢検出装置」と表記することがある。）５０は、産業用ロボットなどのロボット２０の手首２４に連結されるツール３０などの可動体に設定された基準点の位置およびツール３０の姿勢を検出して、ツール３０のスライド変位および角変位を制御する。ロボット２０は、第１の実施形態におけるロボット２０と同様の軸垂直多関節ロボットであるので、同一の参照符号を付して、詳細な説明は省略する。本実施の形態において、ロボット座標系とは、ロボット２０のベース部２１が固定される床に平行に延び、互いに直交するｘ軸およびｙ軸、ならびに鉛直上向きに延びるｚ軸で構成される直交座標系である。
【００６２】
位置姿勢検出装置５０は、ターゲット５１、回転駆動部５２、レーザ干渉計５３、測距処理部５４、ジャイロセンサ１５、加速度センサ１６、信号処理部１７、演算部５８およびロボット制御部１９を含んで構成される。
【００６３】
ターゲット５１は、ロボット２０の上部アーム２３の先端部、ツールチェンジャーおよびツール３０のいずれかに設けられる。本実施の形態では、ターゲットユニット１１は、ツール３０に設けられ、ツール３０とともにスライド変位および角変位可能である。ターゲット５１は、たとえばコーナーキューブプリズムで実現され、レーザ干渉計５３からターゲット５１に入射したレーザ光を、レーザ光の入射方向とは平行な方向に反射する。駆動手段である回転駆動部５２は、たとえばステッピングモータ、またはインダクションモータとエンコーダとで実現され、演算部５８からの回転制御信号に基づいて、ターゲット５１を予め定める軸線である付加軸線Ｌ５２まわりに回転駆動するとともに、ターゲット５１の角度位置を検出可能である。このとき基準点は、ターゲット５１の回転中心となるように、付加軸線Ｌ５２を配置する。本実施の形態において、回転とは、３６０度未満の角変位および３６０度以上の回転を含むものとする。
【００６４】
ターゲット距離測定手段であるレーザ干渉計５３は、予め定める座標系であるロボット座標系における絶対位置が把握されている測距位置に配置され、レーザ光を射出して、ターゲット５１にレーザ光を入射して、その反射光と、射出したレーザ光と同位相のレーザ光とを干渉させて、干渉強度の最大値および最小値の変化の回数を計数することによって、前記側距位置からターゲット５１までの距離を測定する。レーザ干渉計５３には、ターゲット５１を追尾する機能を有し、ターゲット５１にレーザ光を常に入射させることができるとともに、レーザ光の射出方向を検出可能である。
【００６５】
第１検出手段である測距処理部５４は、たとえばマイクロコンピュータで実現され、演算部５８からの計測指令信号が与えられると、レーザ干渉計５３によって測定された距離および検出されたレーザ光の射出方向に基づいて、ロボット座標系におけるツール３０に設定された基準点の絶対位置およびツール３０の絶対姿勢を求め、求めたツール３０の基準点の絶対位置および絶対姿勢を含む絶対位置姿勢検出信号を演算部５８に与える。測距処理部５４は、詳細に述べると、レーザ干渉計５３によって測定された距離および検出されたレーザ光の射出方向に基づいて、レーザ干渉計５３の座標系である干渉計座標系におけるツール３０の基準点の絶対位置および絶対姿勢を求めてから、ロボット座標系におけるツール３０の基準点の絶対位置および絶対姿勢に変換する。
【００６６】
第２検出手段であるジャイロセンサ１５および加速度センサ１６は、第１の実施形態の位置姿勢検出装置１０におけるジャイロセンサ１５および加速度センサ１６と同様であるので、同一の参照符号を付して、詳細な説明は省略する。信号処理部１７は、第１の実施形態の位置姿勢検出装置１０における信号処理部１７と同様であるので、同一の参照符号を付して、詳細な説明は省略する。
【００６７】
演算手段である演算部５８は、たとえばＣＰＵなどの演算処理装置、ならびにランダムアクセスメモリ、リードオンリーメモリおよびハードディスクドライブなどの記憶装置を含んで実現され、位置姿勢検出装置５０を統括的に制御する。演算部５８は、ターゲット５１の付加軸線Ｌ５２まわりの回転駆動を示す回転制御信号を回転駆動部５２与える。また演算部１８は、レーザ干渉計５３によって測定された距離および検出されたレーザ光の射出方向に基づいて、ロボット座標系におけるツール３０に設定されたツール３０の基準点の絶対位置および絶対姿勢を求めることを示す計測指令信号を、測距処理部５４に与える。さらに演算部５８は、ロボット制御部１９から指令要請信号が与えられると、測距処理部５４からの絶対位置姿勢検出信号に含まれるロボット座標系におけるツール３０の基準点の絶対位置および絶対姿勢と、信号処理部１７からの相対変位検出信号に含まれるツール３０の相対角変位および相対スライド変位とに基づいて、ツール３０の基準点の位置および姿勢を求めて、求めた位置および姿勢に基づく修正制御指令値を含む制御信号をロボット制御部１９に与える。
【００６８】
制御手段であるロボット制御部１９は、第１の実施の形態の位置姿勢検出装置１０におけるロボット制御部１９と同様であるので、同一の参照符号を付して、詳細な説明は省略する。
【００６９】
図１０は、レーザ干渉計５３およびロボット２０における干渉計座標系、付加軸座標系およびロボット座標系を模式的に示す図である。図１１は、ツール３０がスライド変位および角変位していない静止状態で、ツール３０の基準点の位置および姿勢を検出する方法を説明するための図である。ツール３０がスライド変位および角変位していない静止状態で、ツール３０の基準点の位置および姿勢を検出する方法を以下に説明する。
【００７０】
演算部５８は、予め定める第１静止角度位置Ｍ１にターゲット５１を配置することを示す回転制御信号を回転駆動部５２に与える。回転駆動部５２は、前記回転制御信号に基づいて、ターゲット５１を付加軸線Ｌ５２まわりに回転させて第１静止角度位置Ｍ１に配置する。このときレーザ干渉計５３は、第１静止角度位置Ｍ１に配置されるターゲット５１と測距位置との距離を測定するとともに、レーザ光の射出方向を検出する。測距処理部５４は、測定された距離および検出された射出方向に基づいて、第１静止角度位置Ｍ１に配置されるターゲット５１の干渉計座標系の第１静止位置の干渉計座標系における座標を求める。
【００７１】
続いて演算部５８は、第１静止角度位置Ｍ１から予め定める回転方向に９０度角変位した角度位置である第２静止角度位置Ｍ２にターゲット５１を配置することを示す回転制御信号を回転駆動部５２に与える。回転駆動部５２は、前記回転制御信号に基づいて、ターゲット５１を付加軸線Ｌ５２まわりに回転させて第２静止角度位置Ｍ２に配置する。このときレーザ干渉計５３は、第２静止角度位置Ｍ２に配置されるターゲット５１と測距位置との距離を測定するとともに、レーザ光の射出方向を検出する。測距処理部５４は、測定された距離および検出された射出方向に基づいて、第２静止角度位置Ｍ２に配置されるターゲット５１の干渉計座標系の第２静止位置の干渉計座標系における座標を求める。
【００７２】
さらに演算部５８は、第１静止角度位置Ｍ１から前記回転方向に２７０度角変位した角度位置である第３静止角度位置Ｍ３にターゲット５１を配置することを示す回転制御信号を回転駆動部５２に与える。回転駆動部５２は、前記回転制御信号に基づいて、ターゲット５１を付加軸線Ｌ５２まわりに回転させて第３静止角度位置Ｍ３に配置する。このときレーザ干渉計５３は、第３静止角度位置Ｍ３に配置されるターゲット５１と測距位置との距離を測定するとともに、レーザ光の射出方向を検出する。測距処理部５４は、測定された距離および検出された射出方向に基づいて、第３静止角度位置Ｍ３に配置されるターゲット５１の干渉計座標系の第３静止位置の干渉計座標系における座標を求める。
【００７３】
このようにターゲット５１の３つの静止角度位置Ｍ１〜Ｍ３における干渉計座標系の同一直線上に並ばない３つの静止位置が求められたので、測距処理部５４は、３つの第１〜第３静止位置を含む平面を求めるとともに、これら３つの静止位置およびターゲット５１の回転半径から回転中心を決定して、前記回転中心から第１静止位置に向かう第１基底ベクトルＶｘ、前記回転中心から第２静止位置に向かう第２基底ベクトルＶｙ、および前記回転中心から前記平面に垂直な方向に向かう第３基底ベクトルＶｚを求める。このようにして前記回転中心すなわち基準点を座標原点とし、互いに直交する第１〜第３基底ベクトルＶｘ，Ｖｙ，Ｖｚによって構成されるターゲット座標系が求められる。これによってターゲット座標系の干渉計座標系に対する傾斜がわかるので、また、ロボット座標系と干渉計座標系との関係も既知であるので、ツール３０の絶対姿勢を求めることができる。
【００７４】
ここで万一、レーザ干渉計５３が、前記第１〜第３角度位置Ｍ１〜Ｍ３に配置されるターゲット５１にレーザ光を入射させることができなくても、前記第１〜第３角度位置Ｍ１〜Ｍ３以外の同一直線上に無い３つの角度位置にターゲット５１を配置して、これらの角度位置に配置されるターゲット５１の干渉計座標系における第１〜第３位置を求めることができる。これによって測距処理部５４は、この干渉計座標系における第１〜第３位置である３つの位置を含む平面を求めるとともに、これら３つの位置およびターゲット５１の回転半径から回転中心を求めて、前記回転中心を座標原点とし、互いに直交する第１〜第３基底ベクトルＶｘ，Ｖｙ，Ｖｚを求めて、ターゲット座標系を求めることができる。
【００７５】
図１２は、ツール３０がスライド変位および角変位している動作状態で、ツール３０の基準点の位置および姿勢を検出する方法を説明するための図である。ツール３０がスライド変位および角変位している動作状態で、ツール３０の基準点の位置および姿勢を検出する方法を以下に説明する。
【００７６】
演算部５８は、ロボット２０のツール３０のスライド速度よりも充分に速い回転速度でターゲット５１を回転することを示す回転制御信号を回転駆動部５２に与える。回転駆動部５２は、前記回転制御信号に基づいて、ツール３０のスライド速度よりも充分に速い回転速度でターゲット５１を付加軸線Ｌ５２まわりに回転させる。ターゲット５１が、ツール３０のスライド速度よりも充分に速い回転速度で付加軸線Ｌ５２まわりに回転することによって、所定の時間間隔となる複数、本実施の形態では３つの時刻におけるターゲット５１の３つの位置が、近似的に、３つの時刻のうちの最も最近の時刻における付加軸線Ｌ５２に垂直な平面上に存在する。図１２は、理解を容易にするために、３つの角度位置Ｄ１〜Ｄ３が重ならないように、それぞれ異なる平面（付加軸線Ｌ５２に垂直な平面）上に存在しているように表記しているが、実際には、矢符Ｃの方向には、ほとんど間隔をあけていない。
【００７７】
レーザ干渉計５３は、前述の所定の時間間隔で、回転しているターゲット５１と測距位置との距離を測定するとともに、レーザ光の射出方向を検出する。そして測距処理部５４は、測定された距離および検出された射出方向に基づいて、測定された各時刻におけるターゲット５１の干渉計座標系における座標を求める。
【００７８】
ある測定時刻ｔ_ｉにおけるツール３０の基準点の絶対位置および絶対姿勢を求めるためには、前記測定時刻ｔ_ｉを含む３つの時刻におけるターゲット５１の干渉計座標における座標が必要である。本実施の形態では、測定時刻ｔ_ｉと、前記測定時刻ｔ_ｉよりも所定の時間間隔だけ過去の第１過去時刻ｔ_ｉ−１と、前記第１過去時刻ｔ_ｉ−１よりも所定の時間間隔だけ過去の第２過去時刻ｔ_ｉ−２とにおけるターゲット５１の干渉計座標における座標を用いる。
【００７９】
測定時刻ｔ_ｉにおいて、ターゲット５１は、予め定める基準角度位置Ｗから前記回転方向に第１角度θ_ｉだけ角変位した第１回転角度位置Ｄ１にあるとする。また第１過去時刻ｔ_ｉ−１において、ターゲット５１は、基準角度位置Ｗから前記回転方向に第２角度θ_ｉ−１だけ角変位した第２回転角度位置Ｄ２にあるとする。さらに第２過去時刻ｔ_ｉ−２において、ターゲット５１は、基準角度位置Ｗから前記回転方向に第３角度θ_ｉ−２だけ角変位した第３回転角度位置Ｄ３にあるとする。
【００８０】
レーザ干渉計５３および測距処理部５４によって、各時刻ｔ_ｉ−２，ｔ_ｉ−１，ｔ_ｉにおけるターゲット５１の各回転角度位置Ｄ１〜Ｄ３の干渉計座標系における座標が求められる。前述のように、これらの３つの座標は、近似的に、測定時刻ｔ_ｉにおける付加軸線Ｌ５２に垂直な平面上に存在する。したがって、これらの３つの座標は、同一直線上に並ばないので、測距処理部５４は、これら３つの座標を含む円周を有する円の中心の干渉計座標系における座標を求める。この円の中心の座標が、ターゲット５１の回転中心であり、すなわちツール３０の基準点の干渉計座標系の座標である。ロボット座標系と干渉計座標系との関係は既知であるので、ツール３０の基準点の絶対位置を求めることができる。
【００８１】
また測距処理部５４は、前述の３つの座標を含む平面を求めるとともに、前記３つの座標と、ターゲット５１の回転速度と、所定の時間間隔と、第１〜第３角度θ_ｉ〜θ_ｉ−２とに基づいて、基準角度位置Ｗの干渉計座標系における座標を求める。さらに測距処理部５４は、前記回転中心から基準角度位置Ｗに向かう第１基底ベクトルＶｘ、前記回転中心から前記平面に平行であって、前記第１基底ベクトルＶｘに回転方向に角度９０度を成す第２基底ベクトルＶｙ、および前記回転中心から前記平面に垂直な方向に向かう第３基底ベクトルＶｚを求める。このようにして前記回転中心すなわち基準点を座標原点とし、互いに直交する第１〜第３基底ベクトルＶｘ，Ｖｙ，Ｖｚによって構成されるターゲット座標系が求められる。これによってターゲット座標系の干渉計座標系に対する傾斜がわかるので、また、ロボット座標系と干渉計座標系との関係も既知であるので、ツール３０の絶対姿勢を求めることができる。
【００８２】
このようにロボット２０のツール３０のスライド速度よりも充分に速い回転速度でターゲット５１を回転させることによって、たとえば、ターゲット５１と回転中心との距離を１００ミリメートルとし、ロボット２０のツール３０が１ミリメートルだけスライド変位する間に、ターゲット５１が角度９０度だけ回転するようにすれば、ｔａｎ^−１（１／１００）≒０．５度の精度で、ツール３０の絶対姿勢を求めることができる。
【００８３】
以上のように本実施の形態の位置姿勢検出装置５０によれば、レーザ干渉計５３によって、ロボット座標系における絶対位置が把握されている測距位置から、ツール３０に設けられ、回転駆動部５２によって付加軸線Ｌ５２まわりに回転するターゲット５１までの距離が測定されるとともにレーザ光の射出方向が検出される。測距処理部５４によって、レーザ干渉計５３によって測定された距離および検出されたレーザ光の射出方向に基づいて、ロボット座標系におけるツール３０に設定された基準点の絶対位置およびツール３０の絶対姿勢が求められる。ターゲット５１が角変位することによって、レーザ干渉計５３は、ロボット座標系における絶対位置が把握されている測距位置から、互いに異なる３つの角度位置に配置されるターゲット５１までの距離を測定するとともに、レーザ光の射出方向を検出することができる。このとき、たとえばターゲット５１の互いに異なる３つの位置が同一直線上に並ばない場合、この３つの位置を含む平面を容易に求めて、その平面の法線を決定して、ターゲット５１を基準とするターゲット座標系を求めることができる。したがってロボット座標系、干渉計座標系およびターゲット座標系に基づいて、ロボット座標系におけるツール３０に設定された基準点の絶対位置およびツール３０の絶対姿勢を容易かつ正確に求めることができる。さらにツール３０に設けられるジャイロセンサ１５および加速度センサ１６によって、ツール３０の相対スライド変位および相対角変位が検出される。
【００８４】
演算部５８によって、測距処理部５４からのツール３０の基準点の絶対位置および絶対姿勢と、ジャイロセンサ１５および加速度センサ１６からのツール３０の相対スライド変位および相対角変位とに基づいて、ツール３０の基準点の位置および姿勢が求められる。これによって、たとえば測距処理部５４によって求められるツール３０の基準点の絶対位置および絶対姿勢が、ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対スライド変位および相対角変位よりも高精度であるけれども、測距処理部５４はツール３０の基準点の絶対位置および絶対姿勢を連続的に求めることができない場合、または、万一、ターゲット５１がレーザ干渉計５３の死角領域に入ってしまった場合、演算部５８は、測距処理部５４によってツール３０の基準点の絶対位置および絶対姿勢が求められるまでは、ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対スライド変位および相対角変位に基づいて、ツール３０の基準点の位置および姿勢を求めることができる。このように測距処理部５４ならびにジャイロセンサ１５および加速度センサ１６からのツール３０の基準点の絶対位置および絶対姿勢、ならびにツール３０の相対スライド変位および相対角変位に基づいて、極めて高精度なツール３０の基準点の位置および姿勢を連続的に求めることができる。
【００８５】
また本実施の形態のロボットの位置および姿勢検出装置５０によれば、演算部５８からの高精度のツール３０の基準点の位置および姿勢に基づいて、ツール３０のスライド変位および角変位を高精度に制御することができる。
【００８６】
本実施の形態のロボットの位置および姿勢検出装置５０において、ロボット２０の動作中においてツール３０の姿勢がほとんど変化しない場合、ロボット２０の動作領域内に複数の予め定める待機点を設定して、各待機点においてロボット２０の動作が停止しているときに、ターゲット５１を回転させてツール３０の姿勢を求めるようにしてもよい。
【００８７】
本実施の形態のロボットの位置および姿勢検出装置５０において、ターゲット距離測定手段はレーザ干渉計５１としたけれども、これに限ることなく、たとえばＣＣＤを有する２台のカメラでもよい。
【００８８】
【発明の効果】
請求項１記載の本発明によれば、たとえば第１検出手段によって求められる可動体の基準点の絶対位置および絶対姿勢が、第２検出手段によって検出される可動体の相対スライド変位および相対角変位よりも高精度であるけれども、第１検出手段は可動体の基準点の絶対位置および絶対姿勢を連続的に求めることができない場合、または、万一、ターゲットユニットが各撮影手段の死角領域に入ってしまった場合、演算手段は、第１検出手段によって可動体の基準点の絶対位置および絶対姿勢が求められるまでは、第２検出手段によって検出される可動体の相対スライド変位および相対角変位に基づいて、可動体の基準点の位置および姿勢を求めることができる。このように演算手段は、２つの検出手段からの可動体の基準点の絶対位置および絶対姿勢、ならびに可動体の相対スライド変位および相対角変位に基づいて、極めて高精度な可動体の基準点の位置および姿勢を連続的に求めることができる。
【００８９】
請求項２記載の本発明によれば、たとえば第１検出手段によって求められる可動体の基準点の絶対位置および絶対姿勢が、第２検出手段によって検出される可動体の相対スライド変位および相対角変位よりも高精度であるけれども、第１検出手段は可動体の基準点の絶対位置および絶対姿勢を連続的に求めることができない場合、または、万一、ターゲットがターゲット距離測定手段の死角領域に入ってしまった場合、演算手段は、第１検出手段によって可動体の基準点の絶対位置および絶対姿勢が求められるまでは、第２検出手段によって検出される可動体の相対スライド変位および相対角変位に基づいて、可動体の基準点の位置および姿勢を求めることができる。このように演算手段は、２つの検出手段からの可動体の基準点の絶対位置および絶対姿勢、ならびに可動体の相対スライド変位および相対角変位に基づいて、極めて高精度な可動体の基準点の位置および姿勢を連続的に求めることができる。
【００９０】
請求項３記載の本発明によれば、演算手段からの高精度の可動体の基準点の位置および可動体の姿勢に基づいて、可動体のスライド変位および角変位を高精度に制御することができる。
【図面の簡単な説明】
【図１】
本発明の第１の実施形態の位置および姿勢検出装置１０の構成を示すブロック図である。
【図２】ロボット２０および複数のカメラ１３Ａ，１３Ｂ，１３Ｃを模式的に示す図である。
【図３】ターゲットユニット１１を示す斜視図である。
【図４】カメラ１３を用いて、ある点Ｐのロボット座標系における座標（Ｘ，Ｙ，Ｚ）を求める方法を説明するための図である。
【図５】ロボット制御部１９によるツール３０のスライド変位および角変位を制御する手順を示すフローチャートである。
【図６】ロボット制御部１９によるツール３０のスライド変位および角変位を制御する方法を説明するための図である。
【図７】ジャイロセンサ１５および加速度センサ１６によって検出されるツール３０の相対角変位および相対スライド変位を主にして、ツール３０の基準点の位置および姿勢を制御する場合の、ツール３０の基準点の位置および姿勢の実測値の真値からの誤差と時間との関係を示すグラフである。
【図８】本発明の第２の実施形態の位置および姿勢検出装置５０の構成を示すブロック図である。
【図９】ロボット２０およびレーザ干渉計５３を模式的に示す図である。
【図１０】レーザ干渉計５３およびロボット２０における干渉計座標系、付加軸座標系およびロボット座標系を模式的に示す図である。
【図１１】ツール３０がスライド変位および角変位していない静止状態で、ツール３０の基準点の位置および姿勢を検出する方法を説明するための図である。
【図１２】ツール３０がスライド変位および角変位している動作状態で、ツール３０の基準点の位置および姿勢を検出する方法を説明するための図である。
【符号の説明】
１０，５０　位置姿勢検出装置
１１　ターゲットユニット
３１，３２，３３，３４；５１　ターゲット
１３　カメラ
１４　画像処理部
１５　ジャイロセンサ
１６　加速度センサ
１８，５８　演算部
１９　ロボット制御部
３０　ツール
５２　回転駆動部
５３　レーザ干渉計
５４　測距処理部[0001]
TECHNICAL FIELD OF THE INVENTION
The present invention relates to a position and posture detection device for a movable body that detects a position and a posture of a tip of a movable body such as a tool connected to a wrist of an industrial robot.
[0002]
[Prior art]
There are various conventional techniques for detecting and controlling the slide displacement and angular displacement of a movable body such as a tool connected to a wrist of an industrial robot. As a prior art, there is a posture detection device in which a gyro is provided at the tip of an arm of an articulated robot. In this attitude detection device, the relative angular displacement of the tip of the arm is detected by a gyro, and the attitude of a tool provided at the tip of the arm is obtained based on the relative angular displacement.
[0003]
As another conventional technique, an ultrasonic transmitter for generating an ultrasonic pulse is provided at the tip of the arm of the robot, and a receiver fixed at a predetermined position receives the ultrasonic pulse from the ultrasonic transmitter to generate an ultrasonic wave. There is a measuring device that obtains a distance between a tip of an arm and a predetermined position based on a time from when a pulse is generated to when it is received, and measures a position and a posture of a tool provided at the tip of the arm.
[0004]
Further, as another conventional technique, when performing teach-back of the articulated robot, a sensor is provided at the end of the arm of the articulated robot, and a tool provided at the end of the arm of the articulated robot by the sensor is provided at a predetermined position. There is a method of measuring a distance from a tip of a calibration jig to be arranged, correcting a change in arm length due to a temperature change, and calibrating each parameter necessary for controlling the robot.
[0005]
[Problems to be solved by the invention]
Industrial robots have high repetition accuracy when repeatedly positioning at a certain position, for example, about 50 micrometers, but absolute accuracy when positioning at a certain predetermined position, especially for a large robot, Since it is as low as about 3 to 4 mm, it is almost always taught in advance by a teach playback method before using the robot. The causes of low absolute accuracy are the geometrical manufacturing error of the robot, the bending of the arm when gripping heavy objects, the expansion and contraction of the arm due to temperature, and the play of the reducer, etc., from each encoder provided at each joint of the robot. This is because the position and orientation of the reference point set on the tool provided at the tip of the arm of the robot based on the detection signal has an error with respect to the command value. This means that the computer generates operation data offline and downloads the operation data to the robot to perform work such as welding, or to install a sensor at the end of the arm and use the robot as a measuring device. It is an obstacle to future development of robots.
[0006]
To solve such a problem, a method of continuously measuring a position and an attitude using an inertial sensor such as a gyro sensor for detecting a relative angular displacement and an acceleration sensor for detecting a relative slide displacement as in the above-described related art. Is a commonly known method used in inertial navigation. If this method can be used to determine the position and posture of the robot arm tip, it is possible to measure the position and posture of the robot continuously from a position remote from the robot, even if it is temporally continuous. There is an advantage in that even if a blind spot occurs depending on the position and posture of the robot, the measurement does not affect the measurement.
[0007]
However, the gyro sensor does not actually detect the relative angular displacement directly, but detects the relative angular displacement by measuring the relative angular velocity and integrating the relative angular velocity with respect to time. Similarly, the acceleration sensor detects the relative slide displacement by measuring the relative acceleration and integrating the relative acceleration twice with respect to time. Therefore, the detection error and the drift accumulate as the displacement amount of the arm tip of the robot increases or as time elapses. Therefore, in such an inertial sensor, the position and posture of the reference point of the tool provided at the tip of the robot arm are used. Cannot be measured with high accuracy. For example, commercially available general-purpose optical fiber gyros and ring laser gyros have a relative acceleration accuracy of at most about 0.01 to 0.03 gravitational acceleration. With such an accuracy, if the robot operates for one second, In the worst case, an error of 5 to 15 cm is accumulated.
[0008]
Also, as in the above-described conventional technique, in measuring the position and orientation of a tool provided at the tip of an arm by ultrasonic waves, a part of the ultrasonic pulse from the ultrasonic transmitter is installed around the robot and the robot. The reflected wave is reflected by a device or the like, and the reflected wave is received by a receiver fixed at a predetermined position, and the distance between the arm tip and the predetermined position cannot be accurately obtained. Also, since the robot operates at high speed, the distance between the tip of the arm and the predetermined position cannot be accurately obtained due to the Doppler effect.
[0009]
Therefore, an object of the present invention is to provide a position and orientation detection device for a movable body that can detect the position of a reference point set on the movable body and the orientation of the movable body with high accuracy.
[0010]
[Means for Solving the Problems]
The present invention according to claim 1 has three targets that are not arranged on the same straight line, and a target unit provided on a movable body;
A plurality of photographing means for photographing a target unit from an imaging position where an absolute position in a predetermined coordinate system is grasped, and generating image data representing an image of the target unit;
First detection means for obtaining an absolute position of a reference point and an absolute posture of the movable body in the coordinate system based on image data from the imaging means;
Second detection means provided on the movable body, for detecting relative slide displacement and relative angular displacement of the movable body,
Arithmetic means for determining the position of the reference point and the attitude of the movable body based on the absolute position and absolute attitude of the reference point from the first detection means and the relative sliding displacement and relative angular displacement of the movable body from the second detection means And a position and orientation detection device for the movable body.
[0011]
According to the present invention, a target unit provided on a movable body has three targets that are not aligned on the same straight line from an imaging position where an absolute position in a predetermined coordinate system is grasped by a plurality of imaging units. The image is captured and image data representing the image of the target unit is generated. If there are a plurality of photographing means, for example, two, the target unit can be binocularly stereoscopically viewed. Further, by using three or more photographing means, it is possible to prevent as much as possible each target from entering the blind spot area of the photographing means due to sliding displacement and angular displacement of the movable body.
[0012]
An absolute position of a reference point and an absolute posture of the movable body in the coordinate system are obtained by the first detecting means based on image data from the photographing means. At this time, in the target unit, since the three targets are not aligned on the same straight line, a plane including the three targets is easily obtained, a normal to the plane is obtained, and a target coordinate system based on the target is determined. Can be requested. Therefore, based on the predetermined coordinate system and the target coordinate system, the absolute position of the reference point and the absolute posture of the movable body in the predetermined coordinate system can be easily and accurately obtained. Further, the second sliding means and the relative angular displacement of the movable body are detected by the second detection means provided on the movable body.
[0013]
The calculating means calculates the reference point of the movable body based on the absolute position and the absolute attitude of the movable body from the first detecting means and the relative sliding displacement and relative angular displacement of the movable body from the second detecting means. The position and attitude of the movable body are required. Thereby, for example, although the absolute position and the absolute posture of the reference point of the movable body determined by the first detecting means are more accurate than the relative sliding displacement and the relative angular displacement of the movable body detected by the second detecting means, If the first detection means cannot continuously determine the absolute position and absolute posture of the reference point of the movable body, or if the target unit enters the blind spot area of each imaging means, the calculation means Until the absolute position and absolute position of the reference point of the movable body are determined by the first detection means, the reference point of the movable body is determined based on the relative sliding displacement and the relative angular displacement of the movable body detected by the second detection means. Can be obtained. In this way, the calculating means determines the reference point of the movable body with extremely high accuracy based on the absolute position and absolute posture of the movable body reference point from the two detecting means, and the relative sliding displacement and relative angular displacement of the movable body. The position and orientation can be determined continuously.
[0014]
According to a second aspect of the present invention, there is provided a target provided on a movable body,
Driving means for driving the target to be angularly displaced about a predetermined axis;
Target distance measurement means for measuring the distance from the distance measurement position at which the absolute position in the predetermined coordinate system is known to the target,
First detection means for obtaining an absolute position of a reference point and an absolute attitude of the movable body in the coordinate system based on the distance measured by the target distance measurement means;
Second detection means provided on the movable body, for detecting relative slide displacement and relative angular displacement of the movable body,
Arithmetic means for determining the position of the reference point and the attitude of the movable body based on the absolute position and absolute attitude of the reference point from the first detection means and the relative sliding displacement and relative angular displacement of the movable body from the second detection means And a position and orientation detection device for the movable body.
[0015]
According to the present invention, the distance from the distance measurement position where the absolute position in the predetermined coordinate system is grasped by the target distance measurement means to the target provided on the movable body and angularly displaced around the axis predetermined by the drive means. Is measured. The first detecting means obtains the absolute position of the reference point and the absolute posture of the movable body in the coordinate system based on the distance measured by the target distance measuring means. When the target is angularly displaced, the target distance measuring means can measure a distance from a distance measuring position at which an absolute position in a predetermined coordinate system is grasped to targets arranged at different angular positions. . At this time, for example, if three angular positions that are not aligned on the same straight line of the target are set, a plane including the three angular positions is easily obtained, and a normal line of the plane is obtained, and the target based on the target is determined. A coordinate system can be determined. Therefore, based on the predetermined coordinate system and the target coordinate system, the absolute position of the reference point and the absolute posture of the movable body in the predetermined coordinate system can be easily and accurately obtained. Further, the second sliding means and the relative angular displacement of the movable body are detected by the second detection means provided on the movable body.
[0016]
The calculating means calculates the reference point of the movable body based on the absolute position and the absolute attitude of the movable body from the first detecting means and the relative sliding displacement and relative angular displacement of the movable body from the second detecting means. The position and attitude of the movable body are required. Thereby, for example, although the absolute position and the absolute posture of the reference point of the movable body determined by the first detecting means are more accurate than the relative sliding displacement and the relative angular displacement of the movable body detected by the second detecting means, If the first detecting means cannot continuously determine the absolute position and absolute attitude of the reference point of the movable body, or if the target enters the blind spot area of the target distance measuring means, the calculating means Until the absolute position and absolute position of the reference point of the movable body are determined by the first detection means, the reference point of the movable body is determined based on the relative sliding displacement and the relative angular displacement of the movable body detected by the second detection means. Can be obtained. In this way, the calculating means determines the reference point of the movable body with extremely high accuracy based on the absolute position and absolute posture of the movable body reference point from the two detecting means, and the relative sliding displacement and relative angular displacement of the movable body. The position and orientation can be determined continuously.
[0017]
According to a third aspect of the present invention, there is further provided a control means for controlling the sliding displacement and the angular displacement of the movable body based on the position of the reference point of the movable body and the attitude of the movable body from the arithmetic means. .
[0018]
According to the present invention, the slide displacement and the angular displacement of the movable body can be controlled with high accuracy based on the position of the highly accurate reference point of the movable body and the attitude of the movable body from the arithmetic means.
[0019]
BEST MODE FOR CARRYING OUT THE INVENTION
FIG. 1 is a block diagram showing a configuration of a position and orientation detection device 10 according to the first embodiment of the present invention. FIG. 2 is a diagram schematically illustrating the robot 20 and the plurality of cameras 13A, 13B, and 13C. The position and orientation detection device (hereinafter, sometimes simply referred to as “position and orientation detection device”) 10 is set on a movable body such as a tool 30 connected to a wrist 24 of a robot 20 such as an industrial robot. The position of the reference point and the posture of the tool 30 are detected, and the sliding displacement and the angular displacement of the tool 30 are controlled.
[0020]
The robot 20 is, for example, a 6-axis vertical articulated robot. The robot 20 has a base 21, a lower arm 22, an upper arm 23, and a wrist 24. The base portion 21 is pivotable about a first axis J1 extending in the vertical direction. The lower arm 22 is connected to the base 21 so as to be angularly displaceable about a second axis J2 extending in the horizontal direction. The upper arm 23 is connected to the upper end of the lower arm 22 such that the base end thereof can be angularly displaced around a third axis J3 extending in the horizontal direction. The wrist 24 is angularly displaceable about a fourth axis J4 extending parallel to the axis of the upper arm 23, and is angularly displaceable about a fifth axis J5 perpendicular to the fourth axis J4. Be linked. The wrist 24 has a flange 25 that can be angularly displaced around a sixth axis J6 perpendicular to the fifth axis J5. The tool 30 such as a hand, a welding torch and a sensor is connected to the flange 25 via a tool changer (not shown) provided on the flange 25.
[0021]
The position and orientation detection device 10 includes a target unit 11, an LED drive unit 12, a camera 13, an image processing unit 14, a gyro sensor 15, an acceleration sensor 16, a signal processing unit 17, a calculation unit 18, and a robot control unit 19. You.
[0022]
FIG. 3 is a perspective view showing the target unit 11. The target unit 11 is provided on either the tool changer or the tool 30. The target unit 11 may be provided at the tip of the upper arm 23 of the robot 20 when the error of the wrist portion is smaller than the error of the entire robot 20. In the present embodiment, the target unit 11 is provided on the tool 30, and is capable of sliding displacement and angular displacement together with the tool 30. The target unit 11 has three targets that are not aligned on the same straight line. In the present embodiment, the target unit 11 has four targets of a first target 31, a second target 32, a third target 33, and a fourth target.
[0023]
The first target 31 to the fourth target 34 are formed in a spherical shape having the same shape, have a light-transmitting property, and have light-emitting diodes (Light Emitting Diodes; abbreviations: LEDs) that emit light of different colors at their centers of gravity. The second target 32 is connected to the first target 32 via a cylindrical first link 35. The third target 33 is connected to the first target 32 via a cylindrical second link 36. The fourth target 34 is connected to the first target 32 via a third link 37 having a cylindrical shape. The first to third links 35 to 37 have exactly the same shape, and have a distance from the center of gravity of the first target 31 to the center of gravity of the second target 32 and a distance from the center of gravity of the first target 31 to the center of gravity of the third target 33. The distance and the distance from the first target 31 to the fourth target 34 are the same. The first link 35, the second link 36, and the third link 37 are arranged such that their axes are orthogonal to each other. The first target 31 of the target unit 11 is arranged such that the center of gravity of the first target 31 becomes a reference point set on the tool 30. The LED drive unit 12 supplies and does not supply drive power required to cause the LEDs provided on the targets 31 to 34 of the target unit 11 to emit light based on the LED control signal from the arithmetic unit 18.
[0024]
The camera 13 serving as an imaging unit is arranged at an imaging position where an absolute position in a robot coordinate system which is a predetermined coordinate system is grasped, and images the target unit 11 from the imaging position to represent an image of the target unit 11. A plurality of image data is generated and provided. In the present embodiment, the robot coordinate system refers to a rectangular coordinate system that includes an x-axis and a y-axis that extend parallel to a floor to which the base unit 21 of the robot 20 is fixed and are orthogonal to each other, and a z-axis that extends vertically upward. System. In the present embodiment, the camera 13 is realized by a CCD camera provided with a solid-state imaging device (Charge Coupled Device; abbreviated as CCD). In the present embodiment, the camera 13 is configured by three cameras, that is, a first camera 13A, a second camera 13B, and a third camera 13C, and is located at different imaging positions at which absolute positions in the robot coordinate system are grasped. Be placed. The imaging position where each of the cameras 13A to 13C is arranged is, in detail, a position outside the operation area of the robot 20 and at which the entire operation area of the robot 20 can be photographed by all the cameras 13A to 13C. Hereinafter, when the first to third cameras 13A to 13C are not distinguished, they may be simply referred to as the camera 13.
[0025]
The image processing unit 14 serving as the first detection unit is realized by, for example, a microcomputer. When a measurement command signal is supplied from the calculation unit 18, the image processing unit 14 performs image processing based on image data from the camera 13 and performs processing in the robot coordinate system. The absolute position of the reference point set in the tool 30 and the absolute attitude of the tool 30 are obtained, and an absolute position / posture detection signal including the obtained absolute position and absolute attitude of the reference point of the tool 30 is provided to the arithmetic unit 18.
[0026]
The gyro sensor 15 and the acceleration sensor 16, which are the second detection means, are provided on either the tool 30 or the tool changer, and are slidable and angularly displaceable together with the tool 30. The gyro sensor 15 detects a relative angular displacement of the tool 30 by detecting an angular velocity of the tool 30, and supplies a relative angular displacement detection signal including the relative angular displacement of the tool 30 to the signal processing unit 17. The acceleration sensor 16 detects a relative slide displacement of the tool 30 by detecting a slide acceleration of the tool 30, and provides a relative slide displacement detection signal including the relative slide displacement of the tool 30 to the signal processing unit 17. In the present embodiment, the second detection means includes a gyro sensor 15 and an acceleration sensor 16. The signal processing unit 17 performs signal processing for converting the relative angular displacement detection signal and the relative slide displacement detection signal from the gyro sensor 15 and the acceleration sensor 16 into a signal that can be processed by the calculation unit 18, A relative displacement detection signal including an angular displacement and a relative slide displacement is given to the arithmetic unit 18.
[0027]
The arithmetic unit 18 as arithmetic means is, for example, a central processing unit (Central
Processing unit (abbreviation: CPU) and the like, and a storage device such as a random access memory, a read-only memory, and a hard disk drive are realized, and generally control the position and orientation detection device 10. The arithmetic unit 18 provides the LED drive unit 12 with an LED control signal indicating supply and non-supply of drive power to each of the targets 31 to 34 of the target unit 11. The arithmetic unit 18 also provides the image processing unit 14 with a measurement command signal indicating that the absolute position of the reference point set on the tool 30 in the robot coordinate system and the absolute posture of the tool 30 are to be obtained. Further, when the command request signal is given from the robot control unit 19, the calculation unit 18 determines the absolute position of the reference point set in the tool 30 in the robot coordinate system included in the absolute position / posture detection signal from the image processing unit 14 and the tool. 30 based on the absolute attitude of the tool 30 and the relative angular displacement of the tool 30 and the relative sliding displacement of the reference point of the tool 30 included in the relative displacement detection signal from the signal processing unit 17. Is obtained, and a control signal including a correction control command value based on the obtained position and posture is given to the robot controller 19.
[0028]
A robot control unit (hereinafter, may be simply referred to as a “control unit”) 19 as a control unit gives a control signal including a preset control command value and a correction command value obtained by the arithmetic unit 18. Command request signal indicating that such a request is made to the arithmetic unit 18. Further, the control unit 19 controls the slide displacement and the angular displacement of the tool 30 based on the control command value and the corrected control command value included in the control signal given from the calculation unit 18. The lower arm 22, the upper arm 23, the wrist 24 and the flange 25 of the robot 20 are mounted on the robot 20 and controlled by a plurality of servomotors (not shown) controlled by the control unit 19. It is angularly displaced around six axes J1 to J6. Thus, the control unit 19 controls the slide displacement and the angular displacement of the tool 30.
[0029]
FIG. 4 is a diagram for explaining a method of obtaining the coordinates (X, Y, Z) of a certain point P in the robot coordinate system using the camera 13. Here, in order to facilitate understanding, a method of obtaining the coordinates (X, Y, Z) of a certain point P in the robot coordinate system when the first camera 13A and the second camera 13B are used will be described. When the point P captured by the first camera 13A is captured as a point Q1 on the imaging surface A1 of the first camera 13A, the coordinates of the point Q1 in the coordinate system of the imaging surface A1 of the first camera 13A are represented by (X _C1 , Y _C1 ). When the point P captured by the second camera 13B is captured as a point Q2 on the imaging surface A2 of the second camera 13B, the coordinates of the point Q2 in the coordinate system of the imaging surface A2 of the second camera 13B are represented by (X _C2 , Y _C2 ). At this time, the relation between the point P and the point Q1 is represented by the following equation (1), and the relation between the point P and the point Q2 is represented by the following equation (2).
[0030]
(Equation 1)

[0031]
In the above equation (1), h ₁ Is a parameter in the homogeneous coordinate expression, and the left matrix on the right side is a predetermined camera parameter matrix of the first camera 13A. In the above equation (2), h ₂ Is a parameter in the homogeneous coordinate expression, and the left matrix on the right side is a predetermined camera parameter matrix of the second camera 13B.
[0032]
The image processing unit 14 determines the coordinates (X) of the point Q1 based on the image data from the first camera 13A. _C1 , Y _C1 ) And the image data from the second camera 13B, the coordinates (X _C2 , Y _C2 ) Is calculated, and the above equations (1) and (2) are made simultaneous to obtain each parameter h. ₁ , H ₂ Is deleted and the coordinates of the point P in the robot coordinate system (X, Y, Z) are rearranged to obtain the following equation (3).
B = AV (3)
[0033]
The matrix B on the left side of the previous equation (3) is expressed by the following equation (5), the matrix A on the left side on the right side is expressed by the following equation (4), and the matrix V on the right side on the right side is expressed by the following equation (6) Is done.
[0034]
(Equation 2)

[0035]
Therefore, the coordinates (X, Y, Z) of the point P in the robot coordinate system are obtained as in the following equation (7).
V = (A ^T ・ A) ^-1 ・ A ^T ・ B… (7)
[0036]
In the above equation (7), T indicates transposition of a matrix.
The method of obtaining the coordinates (X, Y, Z) of the point P in the robot coordinate system is based on the case where two cameras 13 of the first camera 13A and the second camera 13B are used. Even in the above case, the coordinates (X, Y, Z) of the point P in the robot coordinate system can be obtained in the same manner. Thus, the coordinates (X) of the center of gravity L1 of the first target 31 of the target unit 11 in the robot coordinate system _L1 , Y _L1 , Z _L1 ), The absolute position of the reference point set on the tool 30 can be obtained.
[0037]
Further, in this way, the image processing unit 14 can obtain the coordinates of the center of gravity of the first to fourth targets 31 to 34 of the target unit 11 based on the image data from the camera 13. The coordinates of the center of gravity L1 of the first target 31 in the robot coordinate system (X _L1 , Y _L1 , Z _L1 ), The coordinates of the center of gravity L2 of the second target 32 (X _L2 , Y _L2 , Z _L2 ), The coordinates of the center of gravity L3 of the third target 33 (X _L3 , Y _L3 , Z _L3 ), The coordinates of the center of gravity L4 of the fourth target 34 (X _L4 , Y _L4 , Z _L4 ), The first base vector Vx from the first target 31 to the second target 32, the second base vector Vy from the first target 31 to the third target 33, and the second base vector Vy from the first target 31 to the fourth target 34. The three basis vectors Vz are expressed by the following equations (8), (9), and (10), respectively.
[0038]
[Equation 3]

[0039]
In this manner, a target coordinate system including the first to third base vectors Vx, Vy, and Vz orthogonal to each other with the center of gravity L1 of the first target 31 as the coordinate origin is obtained. Thus, since the inclination of the target coordinate system with respect to the robot coordinate system can be determined, the absolute posture of the tool 30 can be obtained.
[0040]
Here, even if any one of the second to fourth targets 32 to 34 of the target unit 11 cannot be photographed by the camera 13, the coordinates of the center of gravity of the three remaining targets including the first target 31 are determined. As a result, a plane including the centers of gravity of the three remaining targets is obtained, so that a normal to this plane can be obtained. Thus, the target coordinate system can be obtained.
[0041]
It takes some time to photograph such a target unit 11 with the camera 13, obtain a target coordinate system based on the image data, and obtain the position of the reference point set in the tool 30 and the posture of the tool 30. For example, since it takes about 0.1 second, the position and orientation of the reference point of the tool 30 are determined by the image processing unit 14, and then the position and orientation of the reference point of the tool 30 are determined by the image processing unit 14. Until the calculation, the calculation unit 18 obtains the position and orientation of the reference point of the tool 30 based on the relative angular displacement and the relative slide displacement detected by the gyro sensor 15 and the acceleration sensor 16.
[0042]
The position and orientation of the reference point of the tool 30 based on the relative angular displacement and relative slide displacement detected by the gyro sensor 15 and the acceleration sensor 16 are compared with the position and orientation of the reference point of the tool 30 obtained by the image processing unit 14. Although the accuracy is low, the position and orientation of the reference point of the tool 30 are newly obtained by the image processing unit 14 until the errors of the relative angular displacement and the relative slide displacement are accumulated. The positions and orientations of the 30 reference points can be obtained.
[0043]
FIG. 5 is a flowchart illustrating a procedure for controlling the slide displacement and the angular displacement of the tool 30 by the robot control unit 19. FIG. 6 is a diagram for explaining a method of controlling the slide displacement and the angular displacement of the tool 30 by the robot control unit 19. A command cycle which is a time interval at which the robot control unit 19 gives a control signal to the robot 20 is set to t seconds. In step s0, the control procedure is started, and proceeds to step s1.
[0044]
In step s1, the operation unit 18 determines that a certain time T _i Measured value R indicating the reference point and posture of the tool 30 obtained in _i And a preset time T _i Command value H indicating the reference point and attitude of tool 30 at _i And the error vector Δ _i And proceeds to step s2. Here, the subscript i is an integer.
[0045]
In step s2, the arithmetic unit 18 determines the time T _i Time T after t seconds from _{i + 1} The control command value H indicating the preset reference point and posture of the tool 30 of the robot 20 in FIG. _{i + 1} Is the sum of the error vectors ΣΔ _i-1 And the error vector Δ obtained in step s1 _i At the time T _{i + 1} Correction control command value N at _{i + 1} And proceeds to step s3. Sum of error vectors ΣΔ _i-1 Is the time T _i Time t before t seconds _i-1 The error vector between the actual measurement value indicating the reference point and the attitude of the tool 30 obtained by the arithmetic unit 18 and the control command value indicating the reference point and the attitude of the tool 30 of the robot 20 which has been preset is calculated. It is a sum.
[0046]
In step s3, the calculation unit 18 determines the time T _{i + 1} , The modified control command value N obtained in step s2 _{i + 1} Is given to the robot controller 19, and the process proceeds to step s4. In step s4, the subscript i is replaced with i + 1, and the process returns to step s1.
[0047]
By performing control in such a procedure, as shown in FIG. 6, the reference point L11 of the measured value indicating the reference point and the attitude of the tool 30 obtained by the calculation unit 18 is set to the reference point of the tool 30 set in advance. And a locus L10 of the control command value indicating the attitude. Therefore, by providing a control signal including a correction control command value based on the position and orientation of the high-precision reference point of the tool 30 from the calculation unit 18 to the robot control unit 19, the slide displacement and the angular displacement of the tool 30 can be accurately performed. Can be controlled.
[0048]
As described above, according to the position and orientation detection apparatus 10 of the present embodiment, the target unit 11 provided on the tool 30 is moved from the imaging position where the absolute position in the predetermined robot coordinate system is grasped by the plurality of cameras 13. The image data is captured and image data representing the image of the target unit 11 is generated. If there are a plurality of cameras 13, for example, two cameras, the target unit 11 can be binocularly stereoscopically viewed. By using three or more cameras 13, it is possible to prevent the targets 31 to 34 from entering the blind spot area of each camera 13 as much as possible due to the sliding displacement and the angular displacement of the tool 30.
[0049]
The image processing unit 14 obtains the absolute position of the reference point set in the tool 30 and the absolute attitude of the tool 30 in the robot coordinate system based on the image data from the camera 13. At this time, in the target unit 11, since any three targets among the four targets 31 to 34 are not aligned on the same straight line, a plane including the three targets is easily obtained, and a normal to the plane is obtained. , A target coordinate system based on the target can be obtained. Therefore, based on the robot coordinate system and the target coordinate system, the absolute position of the reference point set on the tool 30 and the absolute posture of the tool 30 in the robot coordinate system can be easily and accurately obtained. Further, the gyro sensor 15 and the acceleration sensor 16 provided on the tool 30 detect a relative sliding displacement and a relative angular displacement of the tool 30.
[0050]
The calculating unit 18 determines the tool 30 based on the absolute position and the absolute attitude of the reference point of the tool 30 from the image processing unit 14 and the relative sliding displacement and relative angular displacement of the tool 30 from the gyro sensor 15 and the acceleration sensor 16. The position and orientation of the reference point are determined. Thereby, for example, the absolute position and the absolute posture of the reference point of the tool 30 obtained by the image processing unit 14 are more accurate than the relative sliding displacement and the relative angular displacement of the tool 30 detected by the gyro sensor 15 and the acceleration sensor 16. However, if the image processing unit 14 cannot continuously determine the absolute position and the absolute posture of the reference point of the tool 30, or if the target unit 11 enters the blind spot of each camera 13, Until the image processing unit 14 determines the absolute position and the absolute posture of the reference point of the tool 30, the calculation unit 18 is based on the relative sliding displacement and the relative angular displacement of the tool 30 determined by the gyro sensor 15 and the acceleration sensor 16. , The position and orientation of the reference point of the tool 30 can be obtained. . As described above, the calculation unit 18 performs extremely high-precision based on the absolute position and the absolute posture of the reference point of the tool 30 from the image processing unit 14 and the gyro sensor 15 and the acceleration sensor 16, and the relative slide displacement and the relative angular displacement. The position and orientation of the reference point of the tool 30 can be continuously obtained.
[0051]
Further, according to the position and orientation detection device 10 of the present embodiment, the slide displacement and the angular displacement of tool 30 are controlled with high accuracy based on the position and orientation of the reference point of tool 30 with high accuracy from operation unit 18. be able to.
[0052]
In the position and orientation detection device 10 of the present embodiment, each of the targets 31 to 34 of the target unit 11 is formed in a spherical shape having the same shape, and emits light of different colors by LEDs. The processing unit 14 only needs to be able to distinguish the targets 31 to 34. For example, each of the targets 31 to 34 may have a shape different from each other, for example, spheres having different sizes from each other.
[0053]
In the position and orientation detection device 10 of the present embodiment, the target unit 11 is provided on the tool 30, and the first target 31 is set such that the center of gravity of the first target 31 is a reference point set on the tool 30. Although it is said that it is arranged, it is not limited to this. For example, two target units 11 may be used. In this case, the eight targets emit light in different colors, and the first target 31 of each target unit 11 is arranged, for example, at a position symmetrical with respect to the axis of the wrist 24, and the reference point of the tool 30 is As the center of gravity of 30, the positional relationship between each first target 31 and the reference point of the tool 30 is grasped. By using two target units 11, the possibility that both of the two target units 11 enter the blind spot area of all the cameras 13 is extremely low, so that the target coordinate system can be almost certainly determined, and therefore the tool 30 The position and orientation of the reference point can be reliably obtained.
[0054]
In addition, based on the coordinates of the center of gravity of the first target 31 obtained by the image processing unit 14, the field of view of each camera 13 is narrowed, and the camera 13 is connected to a camera platform having a function of automatically tracking the target unit 11. The camera 13 may be slid and angularly displaced. Thereby, the accuracy of the position and orientation of the reference point of the tool 30 obtained by the image processing unit 14 can be improved.
[0055]
FIG. 7 shows a reference point of the tool 30 when the position and orientation of the reference point of the tool 30 are controlled mainly based on the relative angular displacement and the relative slide displacement of the tool 30 detected by the gyro sensor 15 and the acceleration sensor 16. 6 is a graph showing a relationship between an error from a true value of an actual measured value of the position and the attitude of the subject and time. In the above-described embodiment, the absolute position and the absolute position of the reference point of the tool 30 obtained by the image processing unit 14 are mainly used, and the relative angular displacement and the relative slide of the tool 30 detected by the gyro sensor 15 and the acceleration sensor 16 are used. Although the position and orientation of the reference point of the tool 30 were controlled using the displacement as a subordinate, on the contrary, the relative angular displacement and the relative sliding displacement of the tool 30 detected by the gyro sensor 15 and the acceleration sensor 16 were mainly used, The position and orientation of the reference point of the tool 30 may be controlled using the absolute position and orientation of the reference point of the tool 30 obtained by the image processing unit 14 as a subordinate.
[0056]
In this case, the calculation unit 18 gives a control signal including a correction control command value based on the relative angular displacement and the relative slide displacement of the tool 30 detected by the gyro sensor 15 and the acceleration sensor 16 to the robot control unit 19 for a predetermined time. At time intervals U1, U2, U3,..., Un, control signals including a correction control command value based on the absolute position and absolute posture of the reference point of the tool 30 determined by the image processing unit 14 are given to the robot control unit 19. As a result, as shown by a broken line L12 in FIG. 7, the error included in the correction control command value based on the relative angular displacement and the relative slide displacement detected by the gyro sensor 15 and the acceleration sensor 16 is reduced as much as possible. Can be.
[0057]
Further, for example, a plurality of predetermined standby points may be set in the operation area of the robot 20. In this case, when the robot 20 is placed at the standby point, a control signal including a correction control command value based on the absolute position and the absolute posture of the reference point of the tool 30 obtained by the image processing unit 14 is given to the robot control unit 19. As a result, as shown by the solid line L13 in FIG. 7, the error included in the correction control command value based on the relative angular displacement and the relative slide displacement of the tool 30 detected by the gyro sensor 15 and the acceleration sensor 16 is made substantially zero. Can be.
[0058]
Also, utilizing the high repetition accuracy of the robot 20, the positions and orientations of the reference points of the tool 30 at a plurality of standby points are measured in advance, and when the tool 30 is placed at the standby point, the gyro sensor 15 The position and orientation of the reference point of the tool 30 based on the relative angular displacement and the relative sliding displacement of the tool 30 detected by the acceleration sensor 16 may be replaced with numerical values measured in advance.
[0059]
In the position and orientation detection device 10 of the present embodiment, the target unit 11 is photographed by the camera 13 and the absolute position and the absolute orientation of the reference point of the tool 30 in the robot coordinate system are determined based on the image data. Although the absolute position and the absolute attitude of the reference point of the tool 30 in the system are determined, the present invention is not limited to this. For example, a distance measurement sensor may be installed near the aforementioned standby point to measure the position and orientation of the reference point of the tool 30.
[0060]
When the robot 20 repeatedly performs the operation, the position and orientation of the reference point of the tool 30 may be measured during a dry run, and the control command value may be automatically corrected before the operation. Further, when the sensor is used as the tool 30 and used as a measurement robot for measuring the shape of the work, the influence on the measurement data due to errors in the reference point and the attitude of the tool 30 after the measurement of the work is minimized. be able to.
[0061]
FIG. 8 is a block diagram showing the configuration of the position and orientation detection device 50 according to the second embodiment of the present invention. FIG. 9 is a diagram schematically showing the robot 20 and the laser interferometer 53. The position and orientation detection device (hereinafter, sometimes simply referred to as “position and orientation detection device”) 50 is set on a movable body such as a tool 30 connected to the wrist 24 of the robot 20 such as an industrial robot. The position of the reference point and the posture of the tool 30 are detected, and the sliding displacement and the angular displacement of the tool 30 are controlled. Since the robot 20 is an axially vertical articulated robot similar to the robot 20 in the first embodiment, the same reference numerals are given and the detailed description is omitted. In the present embodiment, the robot coordinate system refers to a rectangular coordinate system that includes an x-axis and a y-axis that extend parallel to a floor to which the base unit 21 of the robot 20 is fixed and are orthogonal to each other, and a z-axis that extends vertically upward. System.
[0062]
The position and orientation detection device 50 includes a target 51, a rotation drive unit 52, a laser interferometer 53, a distance measurement processing unit 54, a gyro sensor 15, an acceleration sensor 16, a signal processing unit 17, a calculation unit 58, and a robot control unit 19. Be composed.
[0063]
The target 51 is provided at one of the tip of the upper arm 23 of the robot 20, the tool changer, and the tool 30. In the present embodiment, the target unit 11 is provided on the tool 30, and is capable of sliding displacement and angular displacement together with the tool 30. The target 51 is realized by, for example, a corner cube prism, and reflects laser light incident on the target 51 from the laser interferometer 53 in a direction parallel to the incident direction of the laser light. The rotation driving unit 52 as a driving unit is realized by, for example, a stepping motor or an induction motor and an encoder, and rotates the target 51 around an additional axis L52 which is a predetermined axis based on a rotation control signal from the arithmetic unit 58. While being driven, the angular position of the target 51 can be detected. At this time, the additional axis L52 is arranged so that the reference point becomes the rotation center of the target 51. In the present embodiment, the rotation includes an angular displacement of less than 360 degrees and a rotation of 360 degrees or more.
[0064]
The laser interferometer 53, which is a target distance measuring means, is arranged at a distance measurement position where the absolute position in the robot coordinate system, which is a predetermined coordinate system, is known, emits laser light, and enters the laser light into the target 51. Then, by causing the reflected light to interfere with the laser light having the same phase as the emitted laser light and counting the number of changes in the maximum value and the minimum value of the interference intensity, the distance from the side distance position to the target 51 is counted. Measure the distance. The laser interferometer 53 has a function of tracking the target 51. The laser interferometer 53 can always make the laser light incident on the target 51 and can detect the emission direction of the laser light.
[0065]
The distance measurement processing unit 54, which is the first detection unit, is implemented by, for example, a microcomputer. When a measurement command signal is given from the calculation unit 58, the distance measured by the laser interferometer 53 and the emission of the detected laser light are provided. Based on the direction, the absolute position of the reference point set in the tool 30 and the absolute posture of the tool 30 in the robot coordinate system are obtained, and the absolute position / posture detection signal including the obtained absolute position and the absolute position of the reference point of the tool 30 is obtained. This is given to the arithmetic unit 58. To be more specific, the distance measurement processing unit 54 determines the tool 30 in the interferometer coordinate system, which is the coordinate system of the laser interferometer 53, based on the distance measured by the laser interferometer 53 and the emission direction of the detected laser beam. , The absolute position and the absolute attitude of the reference point are obtained, and then converted into the absolute position and the absolute attitude of the reference point of the tool 30 in the robot coordinate system.
[0066]
The gyro sensor 15 and the acceleration sensor 16 as the second detecting means are the same as the gyro sensor 15 and the acceleration sensor 16 in the position and orientation detection device 10 of the first embodiment, and thus are denoted by the same reference numerals, and are described in detail. Detailed description is omitted. Since the signal processing unit 17 is the same as the signal processing unit 17 in the position and orientation detection device 10 of the first embodiment, the same reference numeral is assigned and the detailed description is omitted.
[0067]
The arithmetic unit 58, which is an arithmetic unit, is implemented by including an arithmetic processing device such as a CPU, and a storage device such as a random access memory, a read-only memory, and a hard disk drive, and controls the position and orientation detection device 50 as a whole. The arithmetic unit 58 gives a rotation control signal indicating the rotation of the target 51 about the additional axis L52 to the rotation drive unit 52. The calculating unit 18 also calculates the absolute position and the absolute posture of the reference point of the tool 30 set on the tool 30 in the robot coordinate system based on the distance measured by the laser interferometer 53 and the emission direction of the detected laser light. A measurement command signal indicating the determination is given to the distance measurement processing unit 54. Further, when the command request signal is given from the robot control unit 19, the calculation unit 58 calculates the absolute position and the absolute posture of the reference point of the tool 30 in the robot coordinate system included in the absolute position and posture detection signal from the distance measurement processing unit 54. The position and orientation of the reference point of the tool 30 are determined based on the relative angular displacement and relative slide displacement of the tool 30 included in the relative displacement detection signal from the signal processing unit 17, and correction based on the determined position and orientation. A control signal including a control command value is given to the robot controller 19.
[0068]
The robot control unit 19, which is a control unit, is the same as the robot control unit 19 in the position and orientation detection device 10 according to the first embodiment, so that the same reference numerals are given and the detailed description is omitted.
[0069]
FIG. 10 is a diagram schematically illustrating the interferometer coordinate system, the additional axis coordinate system, and the robot coordinate system in the laser interferometer 53 and the robot 20. FIG. 11 is a diagram for explaining a method of detecting the position and orientation of the reference point of the tool 30 in a stationary state where the tool 30 is not slid or angularly displaced. A method for detecting the position and orientation of the reference point of the tool 30 in a stationary state where the tool 30 does not undergo the sliding displacement and the angular displacement will be described below.
[0070]
The calculation unit 58 supplies a rotation control signal indicating that the target 51 is arranged at the predetermined first stationary angle position M1 to the rotation driving unit 52. The rotation drive unit 52 rotates the target 51 around the additional axis L52 based on the rotation control signal and arranges the target 51 at the first stationary angular position M1. At this time, the laser interferometer 53 measures the distance between the target 51 disposed at the first stationary angle position M1 and the distance measurement position, and detects the emission direction of the laser light. Based on the measured distance and the detected emission direction, the distance measurement processing unit 54 determines the coordinates of the first stationary position in the interferometer coordinate system of the interferometer coordinate system of the target 51 disposed at the first stationary angular position M1. Ask for.
[0071]
Subsequently, the arithmetic unit 58 outputs a rotation control signal indicating that the target 51 is to be arranged at the second stationary angle position M2 which is an angular position displaced by 90 degrees in a predetermined rotational direction from the first stationary angle position M1. Give to 52. The rotation drive unit 52 rotates the target 51 around the additional axis L52 based on the rotation control signal and arranges the target 51 at the second stationary angle position M2. At this time, the laser interferometer 53 measures the distance between the target 51 located at the second stationary angle position M2 and the distance measurement position, and detects the emission direction of the laser light. Based on the measured distance and the detected emission direction, the distance measurement processing unit 54 determines the coordinates of the second stationary position of the interferometer coordinate system of the interferometer coordinate system of the target 51 disposed at the second stationary angle position M2 in the interferometer coordinate system. Ask for.
[0072]
Further, the arithmetic unit 58 sends to the rotation drive unit 52 a rotation control signal indicating that the target 51 is located at the third stationary angle position M3 which is an angular position shifted by 270 degrees in the rotation direction from the first stationary angle position M1. give. The rotation drive unit 52 rotates the target 51 around the additional axis L52 based on the rotation control signal and arranges the target 51 at the third stationary angle position M3. At this time, the laser interferometer 53 measures the distance between the target 51 disposed at the third stationary angle position M3 and the distance measurement position, and detects the emission direction of the laser light. Based on the measured distance and the detected emission direction, the distance measurement processing unit 54 calculates the coordinates of the third stationary position in the interferometer coordinate system of the interferometer coordinate system of the target 51 disposed at the third stationary angular position M3. Ask for.
[0073]
As described above, since three stationary positions that are not aligned on the same straight line in the interferometer coordinate system at the three stationary angular positions M1 to M3 of the target 51 have been obtained, the distance measurement processing unit 54 sets the three first to third positions. A plane including the stationary position is obtained, and a rotation center is determined from the three stationary positions and the radius of rotation of the target 51. A first base vector Vx from the rotation center to the first stationary position and a second base vector Vx from the rotation center are determined. A second base vector Vy heading toward the stationary position and a third base vector Vz heading from the rotation center in a direction perpendicular to the plane are obtained. In this way, a target coordinate system composed of the first to third base vectors Vx, Vy, Vz orthogonal to each other with the rotation center, that is, the reference point as the coordinate origin is obtained. Thus, since the inclination of the target coordinate system with respect to the interferometer coordinate system is known, and the relationship between the robot coordinate system and the interferometer coordinate system is also known, the absolute attitude of the tool 30 can be obtained.
[0074]
Here, even if the laser interferometer 53 cannot make the laser beam incident on the target 51 disposed at the first to third angular positions M1 to M3, the first to third angular positions M1 By arranging the targets 51 at three angular positions that are not on the same straight line other than to M3, the first to third positions of the targets 51 located at these angular positions in the interferometer coordinate system can be obtained. Accordingly, the distance measurement processing unit 54 obtains a plane including three positions, which are the first to third positions in the interferometer coordinate system, and obtains a rotation center from these three positions and the rotation radius of the target 51. A target coordinate system can be obtained by obtaining first to third base vectors Vx, Vy, Vz orthogonal to each other with the rotation center as a coordinate origin.
[0075]
FIG. 12 is a diagram for explaining a method of detecting the position and orientation of the reference point of the tool 30 in an operation state in which the tool 30 is performing sliding displacement and angular displacement. A method for detecting the position and orientation of the reference point of the tool 30 in an operation state where the tool 30 is slid and angularly displaced will be described below.
[0076]
The calculation unit 58 supplies a rotation control signal indicating that the target 51 is rotated at a rotation speed sufficiently higher than the sliding speed of the tool 30 of the robot 20 to the rotation drive unit 52. The rotation drive unit 52 rotates the target 51 about the additional axis L52 at a rotation speed sufficiently higher than the sliding speed of the tool 30 based on the rotation control signal. By rotating the target 51 around the additional axis L52 at a rotational speed sufficiently higher than the sliding speed of the tool 30, a plurality of positions at predetermined time intervals, in this embodiment, three positions of the target 51 at three times are provided. Exists approximately on a plane perpendicular to the additional axis L52 at the latest time among the three times. In FIG. 12, in order to facilitate understanding, the three angular positions D1 to D3 are described as being present on different planes (planes perpendicular to the additional axis L52) so as not to overlap. Actually, there is little space in the direction of arrow C.
[0077]
The laser interferometer 53 measures the distance between the rotating target 51 and the distance measurement position at the above-described predetermined time interval, and detects the emission direction of the laser light. Then, the distance measurement processing unit 54 calculates the coordinates of the target 51 in the interferometer coordinate system at each measured time based on the measured distance and the detected emission direction.
[0078]
A certain measurement time t _i In order to obtain the absolute position and the absolute posture of the reference point of the tool 30 at the measurement time t _i Are required in the interferometer coordinates of the target 51 at three times including In the present embodiment, the measurement time t _i And the measurement time t _i The first past time t earlier by a predetermined time interval than _i-1 And the first past time t _i-1 Second past time t earlier by a predetermined time interval than _i-2 And the coordinates of the target 51 in the interferometer coordinates.
[0079]
Measurement time t _i , The target 51 moves from the predetermined reference angle position W to the first angle θ in the rotation direction. _i It is assumed that it is at the first rotation angle position D1 that has been angularly displaced by only. Also, the first past time t _i-1 , The target 51 moves from the reference angle position W to the rotation direction in the second angle θ. _i-1 It is assumed that it is at the second rotational angle position D2 that is angularly displaced by only Further, the second past time t _i-2 , The target 51 moves from the reference angle position W to the third angle θ in the rotation direction. _i-2 It is assumed that it is at the third rotational angle position D3 that is angularly displaced by only
[0080]
Each time t is calculated by the laser interferometer 53 and the distance measurement processing unit 54. _i-2 , T _i-1 , T _i Are obtained in the interferometer coordinate system of the respective rotational angle positions D1 to D3 of the target 51 in the above. As described above, these three coordinates are approximately equal to the measurement time t _i Exists on a plane perpendicular to the additional axis L52. Therefore, since these three coordinates are not arranged on the same straight line, the distance measurement processing unit 54 obtains the coordinates in the interferometer coordinate system of the center of the circle having the circumference including the three coordinates. The coordinates of the center of the circle are the rotation centers of the target 51, that is, the coordinates of the reference point of the tool 30 in the interferometer coordinate system. Since the relationship between the robot coordinate system and the interferometer coordinate system is known, the absolute position of the reference point of the tool 30 can be obtained.
[0081]
Further, the distance measurement processing unit 54 obtains a plane including the above-described three coordinates, and also obtains the three coordinates, the rotation speed of the target 51, a predetermined time interval, and the first to third angles θ. _i ~ Θ _i-2 , The coordinates of the reference angular position W in the interferometer coordinate system are determined. Further, the distance measurement processing unit 54 sets a first base vector Vx from the rotation center toward the reference angular position W parallel to the plane from the rotation center and an angle of 90 degrees in the rotation direction to the first base vector Vx. A second base vector Vy to be formed and a third base vector Vz from the rotation center in a direction perpendicular to the plane are obtained. In this way, a target coordinate system composed of the first to third base vectors Vx, Vy, Vz orthogonal to each other with the rotation center, that is, the reference point as the coordinate origin is obtained. Thus, since the inclination of the target coordinate system with respect to the interferometer coordinate system is known, and the relationship between the robot coordinate system and the interferometer coordinate system is also known, the absolute attitude of the tool 30 can be obtained.
[0082]
By rotating the target 51 at a rotation speed sufficiently higher than the sliding speed of the tool 30 of the robot 20, the distance between the target 51 and the rotation center is set to 100 mm, and the tool 30 of the robot 20 is set to 1 mm If the target 51 is rotated by 90 degrees during the sliding displacement, tan ^-1 The absolute posture of the tool 30 can be obtained with an accuracy of (1/100) ≒ 0.5 degrees.
[0083]
As described above, according to the position and orientation detection device 50 of the present embodiment, the laser interferometer 53 is provided on the tool 30 from the distance measurement position where the absolute position in the robot coordinate system is grasped, and the rotation drive unit 52 Thus, the distance to the target 51 rotating about the additional axis L52 is measured, and the emission direction of the laser light is detected. Based on the distance measured by the laser interferometer 53 and the detected emission direction of the laser beam by the distance measurement processing unit 54, the absolute position of the reference point set in the tool 30 and the absolute attitude of the tool 30 in the robot coordinate system Is required. When the target 51 is angularly displaced, the laser interferometer 53 measures the distance from the ranging position where the absolute position in the robot coordinate system is grasped to the targets 51 arranged at three different angular positions. , The direction of emission of the laser beam can be detected. At this time, for example, when three different positions of the target 51 are not aligned on the same straight line, a plane including the three positions is easily obtained, the normal of the plane is determined, and the target 51 is used as a reference. A target coordinate system can be determined. Therefore, based on the robot coordinate system, the interferometer coordinate system, and the target coordinate system, the absolute position of the reference point set on the tool 30 and the absolute posture of the tool 30 in the robot coordinate system can be easily and accurately obtained. Further, the gyro sensor 15 and the acceleration sensor 16 provided on the tool 30 detect a relative sliding displacement and a relative angular displacement of the tool 30.
[0084]
Based on the absolute position and absolute posture of the reference point of the tool 30 from the distance measurement processing unit 54 and the relative sliding displacement and relative angular displacement of the tool 30 from the gyro sensor 15 and the acceleration sensor 16, The positions and postures of 30 reference points are obtained. Thereby, for example, the absolute position and the absolute attitude of the reference point of the tool 30 obtained by the distance measurement processing unit 54 have higher accuracy than the relative sliding displacement and the relative angular displacement of the tool 30 detected by the gyro sensor 15 and the acceleration sensor 16. However, when the distance measurement processing unit 54 cannot continuously determine the absolute position and the absolute attitude of the reference point of the tool 30, or the target 51 enters the blind spot area of the laser interferometer 53. In this case, the calculation unit 58 determines the relative sliding displacement and relative displacement of the tool 30 detected by the gyro sensor 15 and the acceleration sensor 16 until the distance measurement processing unit 54 obtains the absolute position and the absolute attitude of the reference point of the tool 30. The position and orientation of the reference point of the tool 30 can be obtained based on the angular displacement. . As described above, based on the absolute position and absolute attitude of the reference point of the tool 30 from the distance measurement processing unit 54, the gyro sensor 15 and the acceleration sensor 16, and the relative sliding displacement and relative angular displacement of the tool 30, an extremely accurate tool The positions and orientations of the 30 reference points can be determined continuously.
[0085]
Further, according to the robot position and orientation detection device 50 of the present embodiment, the slide displacement and angular displacement of the tool 30 can be accurately determined based on the position and orientation of the high-precision reference point of the tool 30 from the calculation unit 58. Can be controlled.
[0086]
In the robot position and posture detection device 50 of the present embodiment, when the posture of the tool 30 hardly changes during the operation of the robot 20, a plurality of predetermined standby points are set in the operation area of the robot 20, When the operation of the robot 20 is stopped at the standby point, the posture of the tool 30 may be obtained by rotating the target 51.
[0087]
In the position and orientation detection device 50 of the robot of the present embodiment, the target distance measuring means is the laser interferometer 51. However, the present invention is not limited to this. For example, two cameras having CCDs may be used.
[0088]
【The invention's effect】
According to the first aspect of the present invention, for example, the absolute position and absolute position of the reference point of the movable body determined by the first detecting means are relative sliding displacement and relative angular displacement of the movable body detected by the second detecting means. Although the first detection means cannot obtain the absolute position and the absolute attitude of the reference point of the movable body continuously even though the accuracy is higher than that of the movable body, or the target unit enters the blind spot area of each photographing means. If it has, the calculating means calculates the relative sliding displacement and relative angular displacement of the movable body detected by the second detecting means until the first detecting means obtains the absolute position and the absolute position of the reference point of the movable body. Based on this, the position and orientation of the reference point of the movable body can be obtained. In this way, the calculating means determines the reference point of the movable body with extremely high accuracy based on the absolute position and absolute posture of the movable body reference point from the two detecting means, and the relative sliding displacement and relative angular displacement of the movable body. The position and orientation can be determined continuously.
[0089]
According to the second aspect of the present invention, for example, the absolute position and absolute position of the reference point of the movable body determined by the first detecting means are relative sliding displacement and relative angular displacement of the movable body detected by the second detecting means. Although the first detection means cannot obtain the absolute position and the absolute attitude of the reference point of the movable body continuously, or the target enters the blind spot area of the target distance measurement means although the accuracy is higher than that of the target distance measurement means. If it has, the calculating means calculates the relative sliding displacement and relative angular displacement of the movable body detected by the second detecting means until the first detecting means obtains the absolute position and the absolute position of the reference point of the movable body. Based on this, the position and orientation of the reference point of the movable body can be obtained. In this way, the calculating means determines the reference point of the movable body with extremely high accuracy based on the absolute position and absolute posture of the movable body reference point from the two detecting means, and the relative sliding displacement and relative angular displacement of the movable body. The position and orientation can be determined continuously.
[0090]
According to the third aspect of the present invention, it is possible to control the slide displacement and the angular displacement of the movable body with high accuracy based on the position of the highly accurate reference point of the movable body and the attitude of the movable body from the arithmetic means. it can.
[Brief description of the drawings]
FIG.
FIG. 1 is a block diagram illustrating a configuration of a position and orientation detection device 10 according to a first exemplary embodiment of the present invention.
FIG. 2 is a diagram schematically showing a robot 20 and a plurality of cameras 13A, 13B, 13C.
FIG. 3 is a perspective view showing a target unit 11;
FIG. 4 is a diagram for explaining a method of obtaining coordinates (X, Y, Z) of a certain point P in a robot coordinate system by using a camera 13;
FIG. 5 is a flowchart showing a procedure for controlling the slide displacement and the angular displacement of the tool 30 by the robot control unit 19;
FIG. 6 is a diagram for explaining a method of controlling the slide displacement and the angular displacement of the tool 30 by the robot control unit 19;
FIG. 7 shows a reference point of the tool 30 when the position and orientation of the reference point of the tool 30 are controlled mainly based on the relative angular displacement and the relative slide displacement of the tool 30 detected by the gyro sensor 15 and the acceleration sensor 16. 6 is a graph showing a relationship between an error from a true value of an actual measured value of the position and the attitude of the subject and time.
FIG. 8 is a block diagram illustrating a configuration of a position and orientation detection device 50 according to a second embodiment of the present invention.
FIG. 9 is a diagram schematically showing a robot 20 and a laser interferometer 53.
FIG. 10 is a diagram schematically showing an interferometer coordinate system, an additional axis coordinate system, and a robot coordinate system in the laser interferometer 53 and the robot 20.
FIG. 11 is a diagram for explaining a method of detecting the position and orientation of a reference point of the tool 30 in a stationary state where the tool 30 is not slid or angularly displaced.
FIG. 12 is a diagram for explaining a method of detecting a position and a posture of a reference point of the tool 30 in an operation state in which the tool 30 performs a sliding displacement and an angular displacement.
[Explanation of symbols]
10,50 Position and orientation detection device
11 Target unit
31, 32, 33, 34; 51 targets
13 Camera
14 Image processing unit
15 Gyro sensor
16 acceleration sensor
18,58 arithmetic unit
19 Robot controller
30 tools
52 rotation drive
53 laser interferometer
54 Distance measuring unit

Claims (3)

A target unit having three targets that are not aligned on the same straight line and provided on the movable body;
A plurality of photographing means for photographing a target unit from an imaging position where an absolute position in a predetermined coordinate system is grasped, and generating image data representing an image of the target unit;
First detection means for obtaining an absolute position of a reference point and an absolute posture of the movable body in the coordinate system based on image data from the imaging means;
Second detection means provided on the movable body, for detecting relative slide displacement and relative angular displacement of the movable body,
Arithmetic means for determining the position of the reference point and the attitude of the movable body based on the absolute position and absolute attitude of the reference point from the first detection means and the relative sliding displacement and relative angular displacement of the movable body from the second detection means A position and orientation detection device for a movable body, comprising: A target provided on the movable body,
Driving means for driving the target to be angularly displaced about a predetermined axis;
Target distance measurement means for measuring the distance from the distance measurement position at which the absolute position in the predetermined coordinate system is known to the target,
First detection means for obtaining an absolute position of a reference point and an absolute attitude of the movable body in the coordinate system based on the distance measured by the target distance measurement means;
Second detection means provided on the movable body, for detecting relative slide displacement and relative angular displacement of the movable body,
Arithmetic means for determining the position of the reference point and the attitude of the movable body based on the absolute position and absolute attitude of the reference point from the first detection means and the relative sliding displacement and relative angular displacement of the movable body from the second detection means A position and orientation detection device for a movable body, comprising: 3. The movable body according to claim 1, further comprising control means for controlling a sliding displacement and an angular displacement of the movable body based on a position of a reference point of the movable body and a posture of the movable body from the arithmetic means. Position and attitude detection device.

Images