(57)【要約】      （修正有）【課題】移動体の位置を簡単な構成で精度良く検出することができる姿勢検出方法と装置を提供する。【解決手段】姿勢検出装置１は、多関節ロボットのハンドに取り付けられたターゲット部材１０と、カメラ１３と、円形のリング状光源６１とを有する。ターゲット部材１０は、凸面ミラーと、凸面ミラーを囲むリング状拡散反射面を有する。リング状拡散反射面は、楕円形状を有する。リング状光源６１からの光は、凸面ミラーと、リング状拡散反射面のそれぞれで反射して、カメラ１３に入力する。カメラ１３で撮像した画像を処理することによって、光軸に平行なＺ軸と、Ｚ軸に垂直なＸ軸及びＹ軸の３軸方向の位置と、これら３軸回りの回転角度を検出する。【選択図】図１５
2014089095.tif 000002
【特許請求の範囲】
【請求項１】
光源からの光を移動体に照射し、前記移動体に取り付けられた凸面ミラーにおける反射光を撮像装置で撮像し、前記光源の光の第１の反射像を含む画像を作成する工程と、
前記凸面ミラー上の反射点と前記第１の反射像を通り、前記撮像装置のレンズの主点を通る仮想線と前記撮像装置の光軸とがなす第１の角度を算出する工程と、
前記第１の反射点と凸面ミラーの中心とを結ぶ仮想線と前記撮像装置の光軸とがなす第２の角度と、前記光源と前記撮像装置の光軸との間の距離と、前記第１の角度とを用い、前記光源から光の前記第１の反射点における入射角と反射角とが一致するときの前記撮像装置から前記凸面ミラーの前記反射点までの距離を算出する工程と、
を含むことを特徴とする移動体の姿勢検出方法。
【請求項２】
複数の前記第１の反射像のそれぞれを用いて算出した、前記撮像装置から前記凸面ミラーの前記反射点までの距離の平均値を算出する工程を含むことを特徴とする請求項１に記載の移動体の姿勢検出方法。
【請求項３】
移動体に取り付けられた凸面ミラーと、
前記凸面ミラーで反射した光を受光する撮像装置と、
前記撮像装置の光軸を囲むように配置され、前記凸面ミラー及び前記拡散反射部材を照明する光源と、
前記凸面ミラー上の反射点と前記第１の反射像を通り、前記撮像装置のレンズの主点を通る仮想線と前記撮像装置の光軸とがなす第１の角度を算出し、前記第１の反射点と凸面ミラーの中心とを結ぶ仮想線と前記撮像装置の光軸とがなす第２の角度と、前記光源と前記撮像装置の光軸との間の距離とを用い、前記光源から光の前記第１の反射点における入射角と反射角とが一致するときの前記撮像装置から前記凸面ミラーの前記反射点までの距離を算出する処理部と、
を含むことを特徴とする移動体の姿勢検出装置。
【請求項４】
前記光源は、円環形状を有する請求項３に記載の移動体の姿勢検出装置。
【請求項５】
請求項３又は請求項４に記載の姿勢検出装置と、
先端にハンドが設けられた多関節ロボットと、
を含み、
前記移動体は、多関節ロボットの先端に取り付けられた部品組立用の前記ハンドであることを特徴とする部品組立装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の姿勢検出方法及び移動体の姿勢検出装置並びに部品組立装置に関する。
【背景技術】
【０００２】
部品の組立工程や加工工程では、多関節ロボットを用いて部品を他の部品に組み付けたり、加工したりする。部品のサイズが極めて小さい場合や、部品に精度の高い加工をする場合には、部品や工具を把持するハンドの位置を高精度、例えば数十μｍ以下の精度で制御しなければならないことがある。このために、多関節ロボットに姿勢検出装置を設け、ハンドの姿勢を検出しながらフィードバック制御してハンドを所望の位置に移動させている。
【０００３】
ハンドの姿勢を検出する方法としては、例えば、多関節ロボットのハンドの先端部をカメラで撮像し、ハンドの先端部の稜線などの特徴からハンドで把持した部品の姿勢を検出することがある。また、別の例では、ハンドにターゲットマークを取り付け、ターゲットマークの位置を、ハンドの位置として検出していた。
【０００４】
ここで、ハンドに取り付けたターゲットマークを利用してハンドの位置を認識する場合、ターゲットには、凹面鏡と、凹面鏡の周囲に配置された４つのマークとが使用される。４つのマークは、カメラの光軸に垂直で、かつ凹面鏡の曲率中心から光軸方向にオフセットさせた仮想平面上に配置されている。このタイプの姿勢検出装置では、ターゲットマークを照明する光源が１つ設けられている。
【０００５】
１つの光源から出力された光は、凹面鏡で反射される。光源からの光のうち、凹面鏡の曲率中心を通って凹面鏡に入射する光は、凹面鏡に反射され、再び曲率中心を通ってカメラに入射する。凹面鏡における反射光は、曲率中心を通過した光のみがカメラで撮像されるので、カメラの画像には凹面鏡に起因して撮像された輝点が含まれる。このために、輝点の位置から凹面鏡の曲率中心の位置を特定できる。このとき、凹面鏡の曲率中心の位置を基準座標とする。さらに、凹面鏡の曲率中心の位置と、マークの配置は既知であるため、画像中でマークに起因する輝点の配置から、基準座標に対するそれぞれの輝点の相対的な位置を特定できる。これによって、カメラの光軸をＺ軸と、Ｚ軸に直交するＸ軸及びＹ軸で表される３次元空間内での位置を検出できる。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開平３−２８２２０３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
ここで、Ｚ軸方向の位置の変化を検出するためには、撮像画像の輝点の位置の変化量を算出する必要があるが、凹面鏡で反射させた光を撮像して得られる輝点の位置の変化は、実際の距離の変化に比べて縮小される。従って、Ｚ軸方向の位置を精度良く検出することが困難であった。
【０００８】
また、凹面鏡を用いたロボットハンドの姿勢検出処理では、３次元空間における位置と、Ｚ軸回りの回転角度であるθｚと、Ｘ軸回りの回転角度であるθｘと、Ｙ軸回りの回転角度であるθｙの６種類のデータを精度良く取得することは困難であった。例えば、６種類のデータを取得するためには、角度が異なる位置にカメラを２つ配置し、２つのカメラで取得した２つの画像を用いて画像処理を行う必要がある。このために、装置構成が複雑化、及び大型化すると共に、コストアップの原因となっていた。
この発明は、このような事情に鑑みてなされたものであり、移動体の位置を簡単な構成で精度良く検出できるようにすることを目的とする。
【課題を解決するための手段】
【０００９】
実施形態の一観点によれば、光源からの光を移動体に照射し、前記移動体に取り付けられた凸面ミラーにおける反射光を撮像装置で撮像し、前記光源の光の第１の反射像を含む画像を作成する工程と、前記凸面ミラー上の反射点と前記第１の反射像を通り、前記撮像装置のレンズの主点を通る仮想線と前記撮像装置の光軸とがなす第１の角度を算出する工程と、前記第１の反射点と凸面ミラーの中心とを結ぶ仮想線と前記撮像装置の光軸とがなす第２の角度と、前記光源と前記撮像装置の光軸との間の距離と、前記第１の角度とを用い、前記光源から光の前記第１の反射点における入射角と反射角とが一致するときの前記撮像装置から前記凸面ミラーの前記反射点までの距離を算出する工程と、を含むことを特徴とする移動体の姿勢検出方法が提供される。
【００１０】
また、実施形態の別の観点によれば、移動体に取り付けられた凸面ミラーと、前記凸面ミラーで反射した光を受光する撮像装置と、前記撮像装置の光軸を囲むように配置され、前記凸面ミラー及び前記拡散反射部材を照明する光源と、前記凸面ミラー上の反射点と前記第１の反射像を通り、前記撮像装置のレンズの主点を通る仮想線と前記撮像装置の光軸とがなす第１の角度を算出し、前記第１の反射点と凸面ミラーの中心とを結ぶ仮想線と前記撮像装置の光軸とがなす第２の角度と、前記光源と前記撮像装置の光軸との間の距離とを用い、前記光源から光の前記第１の反射点における入射角と反射角とが一致するときの前記撮像装置から前記凸面ミラーの前記反射点までの距離を算出する処理部と、を含むことを特徴とする移動体の姿勢検出装置が提供される。
【発明の効果】
【００１１】
撮像装置と移動体との間の距離を光源の光を凸面ミラーで反射させることによって測定するようにしたので、距離の変化に伴って反射像の位置の変化が大きくなり易い。このために、反射像の位置を利用した距離の計算の精度が向上する。
【図面の簡単な説明】
【００１２】
【図１】図１は、本発明の実施の形態に係る姿勢検出装置の構成の一例を説明する図である。
【図２】図２は、本発明の実施の形態に係る姿勢検出装置における光路の一例を示す図である。
【図３】図３は、本発明の実施の形態に係る姿勢検出装置で撮像される画像の一例を示す図である。
【図４】図４は、本発明の実施の形態に係る姿勢検出装置における初期化処理の一例を示すフローチャートである。
【図５】図５は、本発明の実施の形態に係る姿勢検出装置におけるＺ軸位置検出処理の一例を示すフローチャートである。
【図６Ａ】図６Ａは、本発明の実施の形態に係る姿勢検出装置において撮像される画像の一例を示し、画像処理の具体例を説明するための図である。
【図６Ｂ】図６Ｂは、本発明の実施の形態に係る姿勢検出装置において撮像される移動後の画像の一例を示し、画像処理の具体例を説明するための図である。
【図７】図７は、本発明の実施の形態に係る姿勢検出装置においてＺ位置値を算出するための光路の一例を示す図である。
【図８】図８は、本発明の実施の形態に係る姿勢検出装置における姿勢制御処理の一例を示すフローチャートである。
【図９】図９は、本発明の実施の形態に係る姿勢検出装置における姿勢位置検出処理の一例を示すフローチャートである。
【図１０】図１０は、本発明の実施の形態に係る姿勢検出装置においてＺ位置値や倍率を算出するための光路の一例を示す図である。
【図１１Ａ】図１１Ａは、本発明の実施の形態に係る姿勢検出装置において移動時のＸ軸回りの回転角度の算出方法の一例を模式的に説明する図である。
【図１１Ｂ】図１１Ｂは、本発明の実施の形態に係る姿勢検出装置において移動時のＹ軸回りの回転角度の算出方法の一例を模式的に説明する図である。
【図１２】図１２は、本発明の実施の形態に係る姿勢検出装置においてＺ軸方向に移動したときの反射像間の距離を説明する図であって、（ａ）は移動前、（ｂ）は移動後の配置を示す図である。
【図１３】図１３は、本発明の実施の形態の比較例に係る姿勢検出装置において、Ｚ軸方向に移動したときの反射像間の距離を説明する図であって、（ａ）は移動前、（ｂ）は移動後の配置を示す図である。
【図１４】図１４は、本発明の実施の形態に係る姿勢検出装置においてターゲット部材に凸面ミラーを用いることによる効果の一例を説明する図である。
【図１５】図１５は、本発明の実施例に係る姿勢検出装置を搭載した部品組立装置の概略構成の一例を示す図である。
【図１６】図１６は、本発明の実施例に係る部品組立装置に搭載された姿勢検出装置の構成の一例を説明する図である。
【図１７】図１７は、本発明の実施例に係る部品組立装置に搭載された姿勢検出装置における光路の一例を示す図である。
【図１８Ａ】図１８Ａは、本発明の実施例に係る部品組立装置で撮像される画像の一例を示す図である。
【図１８Ｂ】図１８Ｂは、本発明の実施例に係る姿勢検出装置において撮像される画像の一例を示し、画像処理の具体例を説明するための図である。
【図１９】図１９は、本発明の実施例に係る部品組立装置における初期化処理の一例を示すフローチャートである。
【図２０】図２０は、本発明の実施例に係る部品組立装置における姿勢位置検出処理の一例を示すフローチャートである。
【図２１】図２１は、本発明の実施例に係る部品組立装置で撮像される移動後の画像の一例を示し、画像処理の具体例を説明するための図である。
【発明を実施するための形態】
【００１３】
発明の目的及び利点は、請求の範囲に具体的に記載された構成要素及び組み合わせによって実現され達成される。
前述の一般的な説明及び以下の詳細な説明は、典型例及び説明のためのものであって、本発明を限定するためのものではない。
【００１４】
最初に、実施形態の原理について説明する。
図１及び図２に姿勢検出系の概略構成を示すように、姿勢検出装置１は、姿勢検出の目的対象となる移動体に取り付けられるターゲット部材１０を有する。移動体は、例えば、多関節ロボットのハンドなどの移動体の先端部分である。ターゲット部材１０自体が移動体であっても良い。また、姿勢とは、ターゲット部材１０の３次元空間における位置と、直交する３軸のそれぞれの軸回り回転角度を含むものとする。
【００１５】
ターゲット部材１０は、中央に第１のターゲットマークとなる凸面ミラー１１を有する。凸面ミラー１１は、曲率中心がｍ、曲率半径がＲである。さらに、ターゲット部材１０は、凸面ミラー１１の周囲に第２のターゲットマークとなる４つの拡散反射面１２Ａ，１２Ｂ，１２Ｃ，１２Ｄ（拡散反射部材）が凸面ミラー１１の外周に沿って配置されている。各拡散反射面１２Ａ〜１２Ｄは、四角形で、凸面ミラー１１の外周と同心の仮想円の円周方向に等間隔に配置されている。
【００１６】
また、姿勢検出装置１は、ターゲット部材１０から所定距離だけ上方に離れた位置に、カメラ１３（撮像装置）と、４つの点光源ＬＳ１，ＬＳ２，ＬＳ３，ＬＳ４を有する。カメラ１３は、光軸Ｃ１が凸面ミラー１１の曲率中心ｍを通り、かつ凸面ミラー１１及び４つの拡散反射面１２Ａ〜１２Ｄを撮像可能な向きに配置されている。
【００１７】
４つの点光源ＬＳ１〜ＬＳ４は、カメラ１３の光軸Ｃ１を中心とする仮想円の円周上に等間隔で配置されている。カメラ１３の光軸Ｃ１を挟んで配置される２つの点光源ＬＳ１，ＬＳ３を結ぶ仮想線の中心は、カメラ１３の光軸Ｃ１に一致するように配置されている。同様に、カメラ１３の光軸Ｃ１を挟んで配置される２つの点光源ＬＳ２，ＬＳ４を結ぶ仮想線の中心は、カメラ１３の光軸Ｃ１に一致するように配置されている。このような点光源ＬＳ１〜ＬＳ４の配置の一例として、各点光源ＬＳ１〜ＬＳ４は、ターゲット部材１０の各拡散反射面１２Ａ〜１２Ｄの上方に１つずつ配置される。点光源ＬＳ１〜ＬＳ４と、拡散反射面１２Ａ〜１２Ｄの数と配置は、同じ数及び光軸Ｃ１回りに同じ配置であることが好ましいが、両者の数又は配置を異ならせることもできる。
【００１８】
ここで、ターゲット部材１０は、例えば、点光源ＬＳ１〜ＬＳ４の光を反射しない材料から製造され、凸面ミラー１１及び４つの拡散反射面１２Ａ〜１２Ｄは、点光源ＬＳ１〜ＬＳ４の光を反射する材料、例えばガラスや石英を用いて製造されている。
【００１９】
図２に示すように、カメラ１３は、撮像レンズ２１と、撮像面２２を有する。撮像面２２は、例えば、ＣＣＤ（Charge Coupled Device）が用いられる。カメラ１３で撮像したターゲット部材１０の画像の一例を図３に示す。画像２５には、画像の中心で直交する２つの直線を有し、これら直線がＸ軸と、Ｙ軸になっている。Ｘ軸及びＹ軸に直交するＺ軸であり、図２においてＺ軸はカメラ１３の光軸と一致している。
【００２０】
画像２５には、円形の４つの第１の反射像ｑ１，ｑ２，ｑ３，ｑ４と、四角形の第２の反射像ｓ１，ｓ２，ｓ３，ｓ４が含まれる。４つの第１の反射像ｑ１〜ｑ４は、４つの点光源ＬＳ１〜ＬＳ４が凸面ミラー１１で反射されることによって形成された像であり、互いの距離が狭い。４つの第２の反射像ｓ１〜ｓ４、点光源ＬＳ１〜ＬＳ４の光が反射することによって形成されることから、典型的には円形になる。
【００２１】
また、４つの第２の反射像ｓ１〜ｓ４は、４つの拡散反射面１２Ａ〜１２Ｄで反射されることによって形成された像であり、互いの距離が広い。４つの第２の反射像ｓ１〜ｓ４は、拡散反射面１２Ａ〜１２Ｄの形状に従って、典型的に四角形状になる。
【００２２】
次に、姿勢検出方法について説明する。姿勢検出の際には、最初に初期化処理を行う。さらに、移動体の姿勢を変化させながら、ターゲット部材１０を用いて姿勢検出を行い、検出結果を利用してフィードバック制御することによって、ターゲット部材１０が取り付けられた移動体を基準位置に導く。以下に、各処理の詳細について説明する。
【００２３】
最初に、図４のフローチャートを参照して、姿勢位置検出処理の初期化処理について説明する。
ステップＳ１０１で、ターゲット部材１０を初期化位置に移動させる。ターゲット部材１０が多関節ロボットのロボットハンドに取り付けられている場合には、ロボットハンドを初期位置に移動させる。
次に、ステップＳ１０２では、カメラ１３でターゲットマークを撮像する。具体的には、４つの点光源ＬＳ１〜ＬＳ４を点灯させてターゲット部材１０に光を照射し、凸面ミラー１１と４つの拡散反射面１２Ａ〜１２Ｄのそれぞれにおける各点光源ＬＳ１〜ＬＳ４の反射光をカメラ１３で撮像する。
【００２４】
この後、ステップＳ１０３では、Ｚ軸位置検出処理を行い、Ｚ軸上の位置を示すＺ軸位置値を算出する。Ｚ軸位置検出処理の詳細は、後に説明する。続いて、ステップＳ１０４で、カメラ１３の倍率βｓを算出する。図２に示す例において倍率βｓは、Ｆ／Ｚで算出できる。ここで、Ｆは、カメラ１３の撮像レンズ２１の主点ｎから撮像面２２までの距離であり、既知の値である。Ｚは、ステップＳ１０３で算出される値が用いられる。そして、ステップＳ１０５で、拡散反射面１２Ｂと拡散反射面１２Ｄの間の距離である拡散反射面間距離Ｒｘを２つの第２の反射像ｓ２，ｓ４の間の距離として算出する。また、拡散反射面１２Ａと拡散反射面１２Ｃの間の距離である拡散反射面間距離Ｒｙを２つの第２の反射像ｓ１，ｓ３の間の距離として算出する。図３の画像２５に示すように、対向する２点間の距離Ｒｘ、Ｒｙは、
【００２５】
【数１】
2014089095.tif 000003
【００２６】
【数２】
2014089095.tif 000004
【００２７】
で算出される。ここまでの処理が実行したら、各データを不図示の記憶装置に記憶し、初期化処理を終了する。
【００２８】
次に、図４のステップＳ１０３のＺ軸位置検出処理の詳細について、図５のフローチャートを参照して説明する。Ｚ軸位置検出処理は、カメラ１３が凸面ミラー１１における反射光を撮像したときに得られる４つの第１の反射像ｑ１〜ｑ４の１つ１つに着目して実施される。
最初のステップＳ２０１では、図３の画像２５内における４つの第１の反射像ｑ１〜ｑ４の第１の中心ｇ１（ｘ，ｙ）を算出する。第１の中心ｇ１（ｘ，ｙ）は、４つの第１の反射像ｑ１〜ｑ４の重心でもある。
【００２９】
具体的には、図６Ａ示す第１の反射像ｑ１と第１の反射像ｑ３を結ぶ仮想線Ｌ１３の式を求める。仮想線Ｌ１３は、
Ｙ１＝（ｑ１（ｙ）−ｑ３（ｙ））／（ｑ１（ｘ）−ｑ３（ｘ））×Ｘ１＋ｂ１
になる。ここで、Ｘ１とＹ１は、それぞれＸ軸上の座標と、Ｙ軸上の座標を示す。ｂ１は、仮想線Ｌ１３がＹ軸と交差する点のＹ座標に相当する。
【００３０】
同様に、第１の反射像ｑ２と第１の反射像ｑ４を結ぶ仮想線Ｌ２４の式を求める。仮想線Ｌ２４は、
Ｙ２＝（ｑ２（ｙ）−ｑ４（ｙ））／（ｑ２（ｘ）−ｑ４（ｘ））×Ｘ２＋ｂ２
になる。ここで、Ｘ２とＹ２は、それぞれＸ軸上の座標と、Ｙ軸上の座標を示す。ｂ２は、仮想線Ｌ２４がＹ軸と交差する点のＹ座標に相当する。
【００３１】
ここで、仮想線Ｌ１３と仮想線Ｌ２４とは、交差、より具体的には直交しており、これら２つの仮想線Ｌ１３、Ｌ２４の交点が４つの第１の反射像ｑ１〜ｑ４の第１の中心ｇ１（ｘ，ｙ）になる。ここで、カメラ１３の撮像系の座標と点光源ＬＳ１〜ＬＳ４との間にＺθ方向の角度オフセットを設けない場合には、仮想線Ｌ１３がＸ軸と平行なり、仮想線Ｌ２４がＹ軸と平行になるので、第１の中心ｇ１（ｑ２（ｘ），ｑ１（ｙ））になる。
【００３２】
続く図５のステップＳ２０２からステップＳ２０４は、４つの第１の反射像ｑ１〜ｑ４を１ずつ順番に抽出することによって実施される。まず、ステップＳ２０２で、角度αｎ（第１の角度）を算出する。ここで、図７に示すように、αｎは、カメラ１３の撮像レンズ２１の主点ｎを通過し、凸面ミラー１１の反射点ｐｎを結ぶ線分と、カメラ１３の光軸とのなす角度である。例えば、ｎ＝１のときは、点光源ＬＳ１の光が凸面ミラー１１で反射されたポイント、即ち反射点ｐ１と、反射点ｐ１における反射光が撮像レンズ２１を通って撮像面２２で撮像された第１の反射像ｑ１とを用いて計算される。
【００３３】
具体的には、図７を参照し、レンズの主点ｎを通り、凸面ミラー１１の反射点ｐｎを結ぶ線分とカメラ１３の光軸をなす角度αｎを算出する。角度αｎは、
αｎ＝ｔａｎ−１（（ｅｎ＋ｊｎ）／Ｆ）
と表せる。ここで、（ｅｎ＋ｊｎ）は、凸面ミラー１１からの第１の反射像ｑｎとカメラ１３の光軸の中心からの距離である。その内訳は、ｅｎが第１の中心ｇ１から第１の反射像ｑｎまでの距離であり、ｊｎが光軸ｃ１と凸面ミラー１１の中心とのずれ量である。例えば、点光源ＬＳ１又は点光源ＬＳ３を用いて角度α１又は角度α３を算出する場合には、（ｅｎ＋ｊｎ）は、ｘ軸の値が用いられる。一方、点光源ＬＳ２又は点光源ＬＳ４を用いて角度α２又は角度α４を算出する場合には、（ｅｎ＋ｊｎ）は、ｙ軸の値が用いられる。また、Ｆは、撮像レンズ２１の主点ｎから撮像面２２までの距離である。
【００３４】
さらに、図５のステップＳ２０３で、距離Ｚｎを算出する。距離Ｚｎは、撮像レンズ２１の主点ｎから凸面ミラー１１の反射点までの距離であって、光軸に沿った長さである。例えば、ｎ＝１のときは、点光源ＬＳ１の反射点ｐ１、反射像ｑ１を用いて距離Ｚ１が算出される。
【００３５】
具体的には、最初に、図７に示す点光源ＬＳｎから反射点ｐｎに入射する光の入射角φ´を求める。入射角φ´は、
φ´＝ｔａｎ−１（（Ｌ−ｈ）／Ｚｎ）―γ      （１）
で表せる。ここで、Ｌは、カメラ１３の光軸から点光源ＬＳｎまでの距離である。ｈは、カメラ１３の光軸から反射点ｐ１までの距離で、
ｈ＝Ｚｎ×ｔａｎ（α）      （２）
である。また、γは、凸面ミラー１１の曲率中心ｍと反射点ｐｎを結ぶ直線と、カメラ１３の光軸とがなす角度（第２の角度）である。
【００３６】
ここで、角度γは、
γ＝ｔａｎ−１（ｈ−ｈ０）／Ｒ）      （３）
で表せる。ｈ０は、カメラ１３の光軸からの凸面ミラー１１の曲率中心ｍまでのオフセット量である。オフセット量は、点光源ＬＳ１又は点光源ＬＳ３を用いて角度γを算出する場合には、Ｘ軸に沿った長さが用いられる。一方、点光源ＬＳ２又は点光源ＬＳ４を用いて角度γを算出する場合には、Ｙ軸に沿った長さが用いられる。
【００３７】
続いて、点光源ＬＳｎからの光が反射点ｐｎにおいて反射したときに生じる反射光の出射角φを求める。出射角φは、
φ＝α＋γ      （４）
で表せる。ここで、入射角度φ´と出射角φは、一致するので、（１）〜（４）の式を用いることによって、Ｚ１を算出できる。
【００３８】
この後、図５のステップＳ２０４の判定処理を実行することによって、ステップＳ２０２からステップＳ２０４を繰り返し、残りの３つの点光源ＬＳ２〜ＬＳ４の反射点ｐ２，ｐ３，ｐ４に対するそれぞれの距離Ｚ２、Ｚ３、Ｚ４を算出する。そして、４つの点光源ＬＳ１〜ＬＳ４について算出した４つの距離Ｚ１〜Ｚ４は、本来であれば同一の値であるべきなので、ステップＳ２０５で、４つの距離Ｚ１〜Ｚ４の平均値を算出し、これによって得られた値をＺ軸位置とする。具体的には、ｎ＝２のとき、点光源ＬＳ２におけるＺ２が算出される。さらに、ｎ＝３，４のとき、それぞれ点光源ＬＳ３、ＬＳ４に対するＺ３、Ｚ４が算出される。そして、Ｚｎを４つの点光源ＬＳ１〜ＬＳ４及びその反射点ｐ１〜ｐ４、第１の反射像ｑ１〜ｑ４を用いて算出したら、Ｚ軸位置値を求める。Ｚ軸位置値は、Ｚ＝（Ｚ１＋Ｚ２＋Ｚ３＋Ｚ４）／４の式によって求められる。
【００３９】
ここまでの処理で、初期状態のデータが得られたので、次に、図８に示すフローチャートに従って、移動体の姿勢を変化させながら、移動体をフィードバック制御する。
まず、ステップＳ３０１で、ターゲット部材１０の位置を移動させたときの姿勢位置検出処理を実行する。姿勢位置検出処理の詳細は、後に説明する。続いて、ステップＳ３０２で姿勢偏差検出処理を行い、目標となる基準位置情報と現在の姿勢位置情報との偏差を算出する。そして、ステップＳ３０３の判定処理で、基準位置情報と現在の姿勢位置情報の偏差がゼロであれば、姿勢制御処理を終了する。その一方で、偏差がゼロでなければ、誤差分だけ姿勢移動制御を実施して移動体の姿勢を変化させた後、ステップＳ３０１に戻る。
【００４０】
さらに、ステップＳ３０１の姿勢位置検出処理の詳細について、図９のフローチャートを参照して説明する。最初に、ステップＳ４０１で、ターゲットマークを撮像する。具体的には、４つの点光源ＬＳ１〜ＬＳ４を点灯させ、凸面ミラー１１と４つの拡散反射面１２Ａ〜１２Ｄのそれぞれにおける反射光をカメラ１３で撮像する。
【００４１】
続くステップＳ４０２で、Ｚ軸位置検出処理を実行する。ここでの処理は、図５と同様であり、移動後のターゲット部材１０のＺ軸位置値が算出される。さらに、ステップＳＳ４０３で、ＸＹ軸位置検出処理を実行し、移動後のターゲット部材１０のＸ軸上の位置値と、Ｙ軸上の位置値を算出する。また、４つの拡散反射面１２Ａ〜１４Ｄに対する各第２の反射像ｓ１〜ｓ４の第２の中心（重心）ｇ２´（ｘ，ｙ）、及び移動後の倍率βｍを算出する。この後、ステップＳ４０４において、Ｚ−θ軸位置検出処理を実行し、Ｚ−θ軸位置値を算出する。Ｚ−θ軸位置値とは、Ｚ軸回りの回転角度である。そして、ステップＳ４０５で、Ｘ−θ軸、Ｙ−θ軸の位置検出処理を実行し、Ｘ−θ軸位置値と、Ｙ−θ軸位置値とを算出する。Ｘ−θ軸位置値と、Ｙ−θ軸位置値は、それぞれＸ軸回りの回転角度と、Ｙ軸回りの回転角度である。
【００４２】
以下に、ステップＳ４０３のＸ軸位置値とＹ軸位置値の具体的な算出処理の具体例について説明する。
まず、図６Ｂに、移動後のターゲット部材１０からの反射光を撮像した画像２５Ａを示す。画像２５Ａ中において、ｑ１´〜ｑ４´は、点光源ＬＳ１〜ＬＳ４からの光が移動後の凸面ミラー１１にそれぞれ反射されることによって形成された反射像である。また、ｓ１´〜ｓ４´は、点光源ＬＳ１〜ＬＳ４からの光が移動後の移動後の拡散反射面１２Ａ〜１２Ｄにそれぞれ反射されることによって形成された反射像である。
【００４３】
最初に、図６Ｂに示す第２の反射像ｓ１´と第２の反射像ｓ３´を結ぶ仮想線Ｊ１３の式を求める。仮想線Ｊ１３は、
Ｙ３＝（ｓ１´（ｙ）−ｓ３´（ｙ））／（ｓ１´（ｘ）−ｓ３´（ｘ））×Ｘ１＋ｂ３
になる。ここで、Ｘ３とＹ３は、それぞれＸ軸上の座標と、Ｙ軸上の座標を示す。ｂ３は、仮想線Ｊ１３がＹ軸と交差する点のＹ座標に相当する。
【００４４】
同様に、第２の反射像ｓ２´と第２の反射像ｓ４´を結ぶ仮想線Ｊ２４の式を求める。仮想線Ｊ２４は、
Ｙ４＝（ｓ２´（ｙ）−ｓ４´（ｙ））／（ｓ２´（ｘ）−ｓ４´（ｘ））×Ｘ２＋ｂ４
と表せる。ここで、Ｘ４とＹ４は、それぞれＸ軸上の座標と、Ｙ軸上の座標を示す。ｂ４は、仮想線Ｊ２４がＹ軸と交差する点のＹ座標に相当する。仮想線Ｊ１３と仮想線Ｊ２４とは交差しており、これら２つの仮想線Ｊ１３、Ｊ２４の交点が４つの第２の反射像ｓ１´〜ｓ４´の第２の中心ｇ２´（ｘ，ｙ）になる。
【００４５】
また、図１０に示すように、倍率βｍは、βｍ＝Ｍ／Ｎ＝Ｆ／Ｚから算出できる。この結果、ターゲット部材１０上の実際の中心ｇｒ´２のＸ軸位置値は、ｇ２´（ｘ）／βｍで算出できる。同様に、中心のＹ軸位置値は、ｇ２´（ｙ）／βｍで算出できる。そして、座標（ｇ２´（ｘ）／βｍ，ｇ２´（ｙ）／βｍ）がターゲット部材１０のＸＹ平面における実位置となる。
【００４６】
次に、図９のステップＳ４０４のＺ−θ軸の位置検出処理の具体例について説明する。
まず、第２の反射像ｓ１´と第２の反射像ｓ３´を結ぶ仮想線Ｊ１３と、カメラ１３のＸ軸のなす角度をＺθ１´とする。同様に、第２の反射像ｓ２´と第２の反射像ｓ４´を結ぶ仮想線Ｊ２４と、カメラ１３のＹ軸のなす角度をＺθ２´とする。このとき、
Ｚθ１´＝ｔａｎ−1（（ｓ３´（ｙ）−ｓ１´（ｙ））／（ｓ３´（ｘ）−ｓ１´（ｘ）））
Ｚθ２´＝ｔａｎ−1（（ｓ４´（ｙ）−ｓ２´（ｙ））／（ｓ４´（ｘ）−ｓ２´（ｘ）））
と表せる。そして、Ｚθ１´とＺθ２´の平均値、即ち（Ｚθ１´＋Ｚθ２´）／２を算出すれば、Ｚ軸回りの回転角度であるＺ−θ値Ｚθ´が得られる。
【００４７】
次に、図９のステップＳ４０５のターゲット部材１０のＸ−θ軸と、Ｙ−θ軸の検出処理の具体例について説明する。
最初に、初期状態の倍率βｓを基準にして、ターゲット部材１０の姿勢が変化したときの拡散反射面１２Ａ，１２Ｃの第２の反射像ｓ１，ｓ３の２点間の距離Ｒｙ´と、拡散反射面１２Ｂ，１２Ｄの第２の反射像ｓ２，ｓ４の２点間の距離Ｒｘ´をそれぞれ算出する。そして、回転方向は、凸面ミラー１１の第１の反射像ｑ１´〜ｑ４´の第１の中心ｇ１´と、拡散反射面１２Ａ〜１２Ｄの第２の反射像ｓ１´〜ｓ４´の第２の中心ｇ２´の位置関係から決定する。
【００４８】
ターゲット部材１０の初期状態における拡散反射面１２Ａ〜１２Ｄの第２の反射像ｓ１〜ｓ４の２点間の距離Ｒｘ，Ｒｙは、既に図４のステップＳ１０５の処理で算出されている。また、初期状態における倍率βｓもステップＳ１０４において算出されている。このために、これらの値は、既に算出したものを用いる。
次に、ターゲット部材１０の姿勢を変化させた後の拡散反射面１２Ａ〜１２Ｄの第２の反射像ｓ１´〜ｓ４´の２点間の距離は、
【００４９】
【数３】
2014089095.tif 000005
【００５０】
【数４】
2014089095.tif 000006
【００５１】
で算出できる。また、移動後の倍率βｍは、Ｆ／Ｚｍで算出される。ここで、Ｚｍは、図５と同様の処理によって算出される。
【００５２】
次に、Ｘ軸回りの回転角度であるＸ−θ値Ｘθ´と、Ｙ軸回りの回転角度であるＹ−θ値Ｙθ´を算出する。図１１Ａに模式的に示すように、初期状態の拡散反射面１２Ｂ，１２Ｄが、移動後には、拡散反射面１２Ｂ´，１２Ｄ´になるので、
Ｘθ´＝ｃｏｓ−１(Ｒｘ´／βｍ)／（Ｒｘ／βｓ））
となる。ここで、Ｒｘ／βｓは、画像２５上の第２の反射像ｓ２，ｓ４の２点間の距離Ｒｘを倍率βｓで割り算したもので、初期位置における拡散反射面１２Ｂ，１２Ｄ間の実距離である。同様に、Ｒｘ´／βｍは、画像２５上の第２の反射像ｓ２´，ｓ４´の２点間の距離Ｒｘ´を倍率βｍで割り算したもので、移動後の拡散反射面１２Ｂ，１２Ｄ間の実距離である。
【００５３】
同様に、図１１Ｂに模式的に示すように、初期状態の拡散反射面１２Ａ，１２Ｃが、移動後には、拡散反射面１２Ａ´，１２Ｃ´になるので、
Ｙθ＝ｃｏｓ−１(Ｒｙ´／βｍ)／（Ｒｙ／βｓ））
となる。ここで、Ｒｙ／βｓは、画像２５上の第２の反射像ｓ１，ｓ３の２点間の距離Ｒｙを倍率βｓで割り算したもので、初期位置における拡散反射面１２Ａ，１２Ｃ間の実距離である。同様に、Ｒｙ´／βｍは、画像２５上の第２の反射像ｓ１´，ｓ３´の２点間の距離Ｒｙ´を倍率βｍで割り算したもので、移動後の拡散反射面１２Ａ，１２Ｃ間の実距離である。
【００５４】
さらに、ターゲット部材１０の回転方向を判定する。ターゲット部材１０の回転方向は、Ｘ−θ軸においては、ｇ１´（ｙ）＞ｇ２´（ｙ）のときは、拡散反射面１２Ｄ側がカメラ１３より遠くなる方向に回転している判定する。また、ｇ１´（ｙ）＜ｇ２´（ｙ）のときは、拡散反射面１２Ｂ側がカメラ１３より遠くなる方向に回転している判定する。そして、ｇ１´（ｙ）＝ｇ２´（ｙ）のときは、Ｘ−θ軸の傾きはゼロであると判定する。
【００５５】
同様に、Ｙ−θ軸においては、ｇ１´（ｘ）＞ｇ２´（ｘ）のときは、拡散反射面１２Ｃ側がカメラ１３より遠くなる方向に回転している判定する。また、ｇ１´（ｘ）＜ｇ２´（ｘ）のときは、拡散反射面１２Ａ側がカメラ１３より遠くなる方向に回転している判定する。そして、ｇ１´（ｘ）＝ｇ２´（ｘ）のときは、Ｙ−θ軸の傾きはゼロであると判定する。
【００５６】
このようにして、姿勢移動後のＸＹＺの３軸方向の位置値と、各軸回りの回転角度を算出することによって、ターゲット部材１０の現在の姿勢、即ちＸＹＺの３次元空間における位置値と、Ｚ−θ軸の回転角度Ｚθと、Ｘ−θ軸の回転角度Ｘθ´と、Ｙ−θ軸の回転角度Ｙθ´を特定することができる。カメラ１３の位置は予め決められているので、カメラ１３を用いて検出したターゲット部材１０の現在の姿勢が、所望の位置及び回転角度を示す基準位置に一致するようにフィードバック制御すれば、ターゲット部材１０の位置を精度良く制御できる。
【００５７】
ここで、ターゲット部材１０に凸面ミラー１１を用いることによる効果について、図１２及び図１３を参照して説明する。図１２は、凸面ミラー１１を用いた場合の撮像面２２上での反射像ｑ１，ｑ３間の距離の変化を示している。一方、図１３は、平板形の反射板を用いた場合の反射像ｑ１，ｑ３間の距離の変化を示している。図１２と図１３において、撮像装置１３の構成と、撮像装置１３に対する点光源ＬＳ１，ＬＳ３の配置は、同一になっている。
【００５８】
図１２（ａ）に示す姿勢形地装置１では、凸面ミラー１１と、撮像装置１３の撮像レンズ２１との間の距離（Ｚ軸位置）がＺａ１のとき、撮像面２２上で光軸Ｃ１を通る反射像ｑ１，ｑ３間の距離はＷ１になっている。ここで、点光源ＬＳ１，ＬＳ３と、それぞれの反射点ｓ１，ｓ３を結ぶ仮想線と、反射点ｓ１，ｓ３とレンズ２１の主点ｎを結ぶ仮想線とがなす角度はθ１とする。
【００５９】
これに対して、図１２（ｂ）に示すように、凸面ミラー１１と、撮像装置１３の撮像レンズ２１との間の距離をＺａ２まで近づけると、凸面ミラー１１上の反射点ｐ１，ｐ３が離れ、撮像面２２上での反射像ｑ１，ｑ３間の距離は、Ｗ１より大きいＷ２となる。
【００６０】
一方、図１３（ａ）に示すように、比較例に係る姿勢検出装置１０１では、反射板１０２と、撮像装置１３の撮像レンズ２１との間の距離（Ｚ軸位置）がＺａ１のときに、撮像面２２上で光軸Ｃ１を通る反射像ｑ１，ｑ３間の距離がｗ１になるように構成されている。ここで、点光源ＬＳ１，ＬＳ３と、それぞれの反射板１０２上の反射点ｓ１１，ｓ３３を結ぶ仮想線と、反射点ｓ１１，ｓ３３とレンズ２１の主点ｎを結ぶ仮想線とがなす角度はθ２とする。
【００６１】
これに対して、図１３（ｂ）に示すように、反射板１０２と、撮像装置１３の撮像レンズ２１との間の距離をＺａ２まで近づけると、撮像面２２上での反射像ｑ１，ｑ３間の距離は、ｗ１より大きいｗ３になる。これは、反射板１０２上での反射点ｐ１，ｐ３の位置は変化しないが、反射板１０２から撮像面２２までの距離が短くなるためである。
【００６２】
ここで、比較例における距離ｗ３は、図１２（ｂ）に示す姿勢検出装置１における反射像ｑ１，ｑ３間の距離ｗ２より小さい。これは、図１２に示す姿勢検出装置１は、凸面ミラー１１を用いているので、角度θ１がＺ軸位置の変化に応じて変化し易く、その変化量が比較例における角度φのＺ軸位置の変化に対する変化量より大きくなるためである。このことから、姿勢検出装置１は、Ｚ方向に同じ距離だけ移動したときの反射点ｐ１，ｐ３間の距離（又は光軸Ｃ１から反射点ｐ１，ｐ３までの距離）の変化量を大きくすることができる。
【００６３】
さらに、このようなＺ軸位置の変化に伴う反射像間の距離の変化の一例を図１４に示す。
図１４は、横軸がＺ軸位置、即ちターゲット部材１０とカメラ１３の撮像レンズ２１の間の距離を示す。横軸は、反射点ｐ１と反射点ｐ３の間の距離又は反射点ｐ２と反射点ｐ４の間の距離である。さらに、横軸は、撮像面２２における第１の反射像ｑ１と第１の反射像ｑ３の間の距離又は第１の反射像ｑ２と第１の反射像ｑ４の間の距離も示している。
【００６４】
ラインＥ１は、ミラーに平面鏡を用いた比較例のターゲット部材とカメラ１３の撮像レンズ２１の間の距離の変化を示している。比較例のターゲット部材とカメラ１３の撮像レンズ２１の間の距離を変化させても、平面ミラー上の反射点ｓ１〜ｓ４間の距離はあまり変化しない。これは、比較例のターゲット部材のミラーが平面であるためである。これに伴って、ラインＥ２に示すように、撮像面２２における第１の反射像ｑ１〜ｑ４の変化も小さくなる。
【００６５】
これに対して、ラインＥ３に示すように、実施形態のターゲット部材１０では凸面ミラー１１を用いたので、ターゲット部材１０とカメラ１３の撮像レンズ２１の間の距離の変化に応じて、凸面ミラー１１上の反射点間の距離が大きく変化する。これに伴って、ラインＥ４に示すように、撮像面２２における第１の反射像ｑ１〜ｑ４の位置の変化が比較例に比べて大きくなる。
【００６６】
従って、凸面ミラー１１を使用して取得した反射像ｑ１，ｑ３の位置変化からＺ軸位置を算出することによって、比較例に比べてＺ軸位置の算出精度が向上する。
【００６７】
ここで、カメラ１３を基準として、ターゲット部材１０の位置と距離が予め分かっている場合には、点光源ＬＳ１〜ＬＳ４を最小で２個にすることができる。また、ターゲットマークが方向を規定できるような形状を有する場合、ターゲット部材１０の位置と距離が予め分かっていれば、拡散反射面１２Ａ〜１２Ｄを最小で２個にすることができる。
【００６８】
（実施例）
以下に、実施例として、姿勢検出装置１を多関節ロボットに搭載し、かつ点光源の変わりにリング状光源を用い、４つの拡散反射面の代わりにリング状拡散反射面を用いた部品組立装置について説明する。
図１５に示すように、部品組立装置５１は、部品を把持して搬送する多関節ロボット５２と、多関節ロボット５２のハンド５３の姿勢を検出する姿勢検出装置１を有する。
多関節ロボット５２は、ロボット制御部５５からの信号を受けて、ハンド５３の姿勢を制御するように構成されている。ハンド５３の姿勢は、ＸＹＺの３軸に平行な方向及び、３軸回りに回転可能である。
【００６９】
姿勢検出装置１は、ハンド５３にターゲット部材１０が取り付けられており、リング状光源６１を制御する照明制御部６２と、カメラ１３を制御するカメラ制御部６３と、カメラ１３で撮像した画像を処理して姿勢検出する検知処理部６４と、ハンド５３の姿勢と基準位置情報６５との偏差を算出する偏差算出部６６とを有する。偏差算出部６６は、偏差の計算結果をロボット制御部５５に出力するように構成されている。
【００７０】
図１６及び図１７に示すように、姿勢検出装置１は、ハンド５３の所定位置に取り付けられたターゲット部材１０を有する。ターゲット部材１０の中央には、第１のターゲットマークとなる凸面ミラー１１を有し、凸面ミラー１１の周囲に第２のターゲットマークとなる楕円形状を有するリング状拡散反射面７１（リング状拡散反射部材）が配置されている。リング状拡散反射面７１は、リング状光源６１の光を反射する材料、例えばガラスや石英を用いて製造されている。
【００７１】
ここで、リング状拡散反射面７１の短軸ａの長さは、例えば、ａ＝（凸面ミラー１１の外径）＋（リング状拡散反射面７１のリング幅の１／２）である。また、リング状拡散反射面７１の長径ｂは、ｂ≧ｃｏｓ（θｘｙ）×ａ＋（リング状拡散反射面７１のリング幅の１／２）である。ここで、θｘｙは、姿勢検出装置１におけるＸ−θ軸又はＹ−θ軸の最大検出角度である。
【００７２】
また、ターゲット部材１０から離れた所定の位置には、カメラ１３と、リング状光源６１とが配置されている。カメラ１３は、撮像レンズ２１がターゲット部材１０に向かうように、かつ光軸が凸面ミラー１１の曲率中心ｍに一致するように配置されている。リング状光源６１は、全周にわたって発光可能に構成されており、中心がカメラ１３の光軸と一致する。
【００７３】
ここで、図１８Ａにカメラ１３で撮像したターゲット部材１０の画像の一例を示す。画像７５には、直交するラインで示されるＸＹ軸に加えて、リング状光源６１の光が凸面ミラー１１で反射されることによって形成された円形の反射像８１と、リング状拡散反射面７１における楕円形の反射像８２とが含まれる。
【００７４】
次に、実施例における姿勢検出方法について説明する。姿勢検出の際には、最初に初期化処理を行う。さらに、移動体であるハンド５３の姿勢を変化させながら、ターゲット部材１０を用いて姿勢検出を行い、検出結果を利用して多関節ロボット５２をフィードバック制御することによって、ハンド５３を基準位置に導く。以下に、各処理の詳細について説明する。
【００７５】
最初に、図１９のフローチャートを参照して、部品組付装置５１の姿勢位置検出処理の初期化処理について説明する。
ステップＳ１０１では、ロボット制御部５５が、ターゲット部材１０が初期化位置に配置されるように多関節ロボット５２を駆動させる。
続くステップＳ１０２では、カメラ１３でターゲットマークを撮像する。具体的には、照明制御部６２がリング状光源６１を点灯させ、ターゲット部材１０に光を照射する。さらに、カメラ制御部６３の指令に基づいて、凸面ミラー１１とリング状拡散反射面７１のそれぞれにおけるリング状光源６１の反射光をカメラ１３で撮像する。カメラ１３で撮像した画像は、検知処理部６４に送信される。
【００７６】
続く、ステップＳ１０２Ａでは、検知処理部６４が反射像８１，８２の画像処理を行い、画像７５に含まれる反射像８１からデータ解析用の４点の第１の反射像ｑ１〜ｑ４を抽出する。データ解析用の第１の反射像ｑ１〜ｑ４は、反射像８１の周方向の等間隔に配置される４つの点を抽出することが好ましい。同様に、画像７５の反射像８２からデータ解析用の４点の第２の反射像ｓ１〜Ｓ４を抽出する。データ解析用の第２の反射像ｓ１〜ｓ４は、反射像８２の周方向の等間隔に配置される４つの点を抽出することが好ましい。さらに、画像７５中の反射像８１の第１の中心ｇ１（ｘ，ｙ）と、反射像８２の第２の中心ｇ２（ｘ，ｙ）と、Ｚ−θ軸位置とをそれぞれ算出する。
【００７７】
具体的には、図１８Ａに示す凸面ミラー１１の反射像８１は、円形状になっているので、
（Ｘｉ−ｇ１（ｘ））２＋（Ｙｉ−ｇ１（ｙ））２＝ｒ２      （５）
と表すことができる。この式に、反射像８１を構成するｎ個（ｉ＝１〜ｎ）の座標値（Ｘｉ，Ｙｉ）から、円の近似値を例えば最小二乗法を用いて算出し、第１の中心ｇ１（ｘ，ｙ）と、円の半径ｒを求める。さらに、図１８Ｂに示すように、反射像８１を構成する画素から特徴的な４点を抽出し、データ解析用の４つの第１の反射像ｑ１〜ｑ４を決定する。
【００７８】
また、図１８Ａに示すように、リング状拡散反射面７１の反射像８２は、楕円形であるので、
【００７９】
【数５】
2014089095.tif 000007
【００８０】
と表すことができる。この式に、反射像８２を構成するｎ個（ｉ＝１〜ｎ）の座標値（Ｘｉ，Ｙｉ）から、円の近似値を、例えば最小二乗法を用いて算出し、第２の中心ｇ２（ｘ，ｙ）と、Ｘｒ，Ｙｒ，θを求める。さらに、図１８Ｂに示すように、反射像８２を構成する画素から特徴的な４点を抽出し、データ解析用の４つの第２の反射像ｓ１〜ｓ４を決定する。ここで、距離Ｒｘと距離Ｒｙは、楕円の長軸又は短軸に相当するので、ＸｒとＹｒをそれぞれ２倍した値になる。
【００８１】
また、Ｚ―θ軸位置の算出処理では、第２の中心ｇ２を基準にして対向して配置される２つの第２の反射像ｓ１，ｓ３を結ぶ仮想線Ｊ１３のＸ軸に対する傾きであるＺθ１を算出する。さらに、対向して配置される２つの第２の反射像ｓ２，ｓ４を結ぶ仮想線Ｊ２４のＹ軸に対する傾きであるＺθ２を算出する。そして、Ｚθ１とＺθ２の平均を算出すると、Ｚ−θ軸位置値が得られる。
【００８２】
また、図１９のステップＳ１０３で、Ｚ軸位置検出処理を行い、Ｚ軸上の位置を示すＺ軸位置値を算出する。Ｚ軸位置検出処理は、図５と同様である。即ち、反射像８１の第１の中心ｇ１（ｘ，ｙ）を算出し、第１の反射像ｑ１〜ｑ４のそれぞれについて角度αｎを算出し、式（１）〜（４）を用いて、Ｚ１〜Ｚ４を算出する。さらに、Ｚ１〜Ｚ４の平均値を計算し、これをＺ軸位置値とする。
【００８３】
続いて、ステップＳ１０４で、カメラ１３の倍率βｓを算出する。倍率βｓは、図１７に示すＦ／Ｚで算出できる。
そして、ステップＳ１０５で、リング状拡散反射面７１によって形成された楕円形の反射像８２の距離Ｒｘと距離Ｒｙをそれぞれ反射像８２の長軸と短軸である。ここまでの処理で、初期状態のデータが得られた。初期状態のデータは、検知処理部の不図示のメモリに記憶される。
【００８４】
次に、図８に示すフローチャートに従って、ロボットハンドの姿勢制御を開始する。即ち、ステップＳ３０１で、ロボット制御部５５の指令に基づいてターゲット部材１０の位置を移動させたときの姿勢位置検出処理を姿勢検出装置１で実行する。続いて、ステップＳ３０２で偏差算出部６６が姿勢偏差検出処理を行い、目標となる基準位置情報６５と現在の姿勢位置情報とを比較する。基準位置情報６５とは、例えば、カメラ１３の位置を基準として、ハンド５３が所望する位置に精度良く配置された状態におけるターゲット部材１０の姿勢を特定する情報である。そして、ステップＳ３０３の判定処理で、偏差算出部６６が基準位置情報と現在の姿勢位置情報との偏差がゼロになったと判定すれば、姿勢制御処理を終了する。その一方で、偏差がゼロでなければ、ロボット制御部５５が偏差を打ち消すような指令値を算出して多関節ロボット５２を駆動させ、ステップＳ３０１に戻る。
【００８５】
さらに、ステップＳ３０１の姿勢位置検出処理の詳細について図２０のフローチャートを主に参照して説明する。
最初に、ステップＳ４０１で、ターゲットマークを撮像する。具体的には、リング状光源６１を点灯させ、凸面ミラー１１とリング状拡散反射面７１のそれぞれにおける反射光をカメラ１３で撮像する。
続くステップＳ４０１Ａで、検知処理部６４が反射像８１，８２を画像処理する。ここでの処理は、図１９のステップＳ１０２Ａと同様である。また、ステップＳ４０２で、検知処理部６４がＺ軸位置検出処理を実行する。ここでの処理は、図５と同様の処理である。
【００８６】
さらに、ステップＳ４０３で、ＸＹ軸位置検出処理を実行し、ターゲット部材１０のＸ軸上の位置と、Ｙ軸上の位置を算出する。また、各第２の反射像ｓ１〜ｓ４の中心（重心）の座標ｇ２´（ｘ，ｙ）、及び倍率βｍを算出する。この後、ステップＳ４０４において、Ｚ−θ軸位置検出処理を実行し、Ｚ−θ軸位置値を算出する。そして、ステップＳ４０５で、Ｘ−θ軸、Ｙ−θ軸の位置検出処理を実行し、Ｘ−θ軸位置値と、Ｙ−θ軸位置値とを算出する。
【００８７】
ここで、移動後に図２１に示すような画像８５が得られた場合、リング状光源６１の凸面ミラー１１における反射像８１からデータ解析用の第１の反射像ｑ１´〜ｑ４´が抽出される。同様に、リング状光源６１のリング状拡散反射面７１における反射像８２からデータ解析用の第２の反射像ｓ１´〜ｓ４´が抽出される。そして、移動後の中心ｇ１´、ｇ２´は、式（５）、（６）から算出される。ターゲット部材１０のＸＹ座標は、（ｇ２´（ｘ）／βｍ，ｇ２´（ｙ）／βｍ）になる。
【００８８】
Ｚ軸位置は、Ｚ＝（Ｚ１＋Ｚ２＋Ｚ３＋Ｚ４）／４で算出される。Ｚ１〜Ｚ４は、式（１）〜式（４）で算出できる。Ｚ−θ軸の回転角度は、（Ｚθ１＋Ｚθ２）／２であり、Ｚθ１．Ｚθ２は、
Ｚθ１＝ｔａｎ−1（（ｓ３´（ｙ）−ｓ１´（ｙ））／（ｓ３´（ｘ）−ｓ１´（ｘ）））
Ｚθ２＝ｔａｎ−1（（ｓ４´（ｙ）−ｓ２´（ｙ））／（ｓ４´（ｘ）−ｓ２´（ｘ）））
から算出される。
【００８９】
図１１Ａに示すように、Ｘθは、Ｙθ＝ｃｏｓ−１(Ｒｘ´／βｍ)／（Ｒｘ／βｓ））になる。図１１Ｂに示すように、Ｙθは、Ｙθ＝ｃｏｓ−１(Ｒｙ´／βｍ)／（Ｒｙ／βｓ））になる。Ｘ−θ軸の回転方向と、Ｙ−θ軸の回転方向は、前記と同様に、ｇ１´とｇ２´の位置関係から決定する。
【００９０】
この実施例では、リング状光源６１からの光を凸面ミラー１１とリンク状拡散反射面７１で反射するようにしたので、カメラ１３で２つのリング状の反射像８１，８２が得られる。例えば、リング状光源６１やターゲット部材１０に汚れがあったり、姿勢検出装置１の光路上に障害物があったりした場合で、反射像８１，８２の１／３程度が欠落した場合であっても、近似計算によって円又は楕円を補完することができるので、ターゲット部材１０と、ターゲット部材１０が取り付けられたハンド５３の姿勢を、精度を低下させることなく検出できる。従って、図１５に示す部品組付装置５１において、ハンド５３で把持した部品９１を高い位置精度で他の部品９２に組み付けることができる。また、ハンド５３に工具を取り付けて他の部品９２を加工する場合には、高い精度の加工が可能になる。姿勢検出装置１の用途は、部品組付装置５１に限定されず、部品の搬送装置などに用いても良い。
【００９１】
ここで、リング状光源６１を円形にし、リング状拡散反射面７１を楕円形状にしたので、図１８Ａや図２０に示す反射像８１と反射像８２の区別が容易に付けられる。しかしながら、反射像８１より反射像８２が必ず大きくなるように配置される場合には、リング状光源６１とリング状拡散反射面７１とを同じ形状にしても良い。また、リング状光源６１、リング状拡散反射面７１の少なくとも一方は、一部が切り欠かれたＣ形状にすることができる。さらに、リング状光源６１、リング状拡散反射面７１の少なくとも一方は、２つ以上に分割された円弧又は楕円弧でも良い。
【００９２】
ここで挙げた全ての例及び条件的表現は、発明者が技術促進に貢献した発明及び概念を読者が理解するのを助けるためのものであり、ここで具体的に挙げたそのような例及び条件に限定することなく解釈するものであり、また、明細書におけるそのような例の編成は本発明の優劣を示すこととは関係ない。本発明の実施形態を詳細に説明したが、本発明の精神及び範囲から逸脱することなく、それに対して種々の変更、置換及び変形を施すことができる。
【００９３】
以下に、前記の実施の形態の特徴を付記する。
（付記１）  光源からの光を移動体に照射し、前記移動体に取り付けられた凸面ミラーにおける反射光を撮像装置で撮像し、前記光源の光の第１の反射像を含む画像を作成する工程と、前記凸面ミラー上の反射点と前記第１の反射像を通り、前記撮像装置のレンズの主点を通る仮想線と前記撮像装置の光軸とがなす第１の角度を算出する工程と、前記第１の反射点と凸面ミラーの中心とを結ぶ仮想線と前記撮像装置の光軸とがなす第２の角度と、前記光源と前記撮像装置の光軸との間の距離と、前記第１の角度とを用い、前記光源から光の前記第１の反射点における入射角と反射角とが一致するときの前記撮像装置から前記凸面ミラーの前記反射点までの距離を算出する工程と、を含むことを特徴とする移動体の姿勢検出方法。
（付記２）  複数の前記第１の反射像のそれぞれを用いて算出した、前記撮像装置から前記凸面ミラーの前記反射点までの距離の平均値を算出する工程を含むことを特徴とする付記１に記載の移動体の姿勢検出方法。
（付記３）  前記光源からの光を移動体の前記凸面ミラーの周囲に配置された複数の拡散反射部材に照射し、それぞれの反射光を前記撮像装置で撮像し、複数の第２の反射像を取得し、前記複数の第２の反射像の中心から、前記複数の前記第２の反射像の中心である第２の中心を算出する工程と、前記第２の中心を挟んで配置される２つの前記第２の反射像を結ぶ仮想線と、前記撮像装置の光軸に垂直な軸とがなす角度から前記撮像装置の光軸に平行な軸回りの前記移動体の回転角度を算出する工程と、を含むことを特徴とする付記１又は付記２に記載の移動体の姿勢検出方法。
（付記４）  前記移動体を移動させた前後のそれぞれのタイミングで、前記第２の中心を通り、所定の２つの前記第２の反射像を結ぶ仮想線の長さを算出し、前記移動体の移動前後での前記仮想線の長さ変化から、前記撮像装置の光軸に直交する軸線回りの回転角度を算出する工程をさらに有する付記３に記載の移動体の姿勢検出装置。
（付記５）  前記移動体は、多関節ロボットの先端に取り付けられたハンドであり、前記ハンドの姿勢の現在値と、予め定められた前記ハンドの基準位置との偏差を算出し、偏差がゼロになるように前記多関節ロボットを駆動させる工程を有することを特徴とする付記１乃至付記４のいずれか一項に記載の部品組立方法。
（付記６）  移動体に取り付けられた凸面ミラーと、前記凸面ミラーで反射した光を受光する撮像装置と、前記撮像装置の光軸を囲むように配置され、前記凸面ミラー及び前記拡散反射部材を照明する光源と、前記凸面ミラー上の反射点と前記第１の反射像を通り、前記撮像装置のレンズの主点を通る仮想線と前記撮像装置の光軸とがなす第１の角度を算出し、前記第１の反射点と凸面ミラーの中心とを結ぶ仮想線と前記撮像装置の光軸とがなす第２の角度と、前記光源と前記撮像装置の光軸との間の距離とを用い、前記光源から光の前記第１の反射点における入射角と反射角とが一致するときの前記撮像装置から前記凸面ミラーの前記反射点までの距離を算出する処理部と、を含むことを特徴とする移動体の姿勢検出装置。
（付記７）  前記光源は、円環形状を有する付記６に記載の移動体の姿勢検出装置。
（付記８）  前記光源は、４つ以上の点光源である付記１に記載の移動体の姿勢検出装置。
（付記９）  付記６乃至付記８のいずれか一項に記載の姿勢検出装置と、先端にハンドが設けられた多関節ロボットと、を含み、前記移動体は、多関節ロボットの先端に取り付けられた部品組立用の前記ハンドであることを特徴とする部品組立装置。
（付記１０）  前記処理部は、前記姿勢検出装置で検出した前記ハンドの姿勢と、予め定められた前記ハンドの基準位置との偏差を算出する偏差算出部を有し、前記偏差がゼロになるように前記ロボットを駆動させるように構成したことを特徴とする付記９に記載の部品組立装置。
【符号の説明】
【００９４】
１  姿勢検出装置
１１  凸面ミラー
１２Ａ〜１２Ｄ  拡散反射面（拡散反射部材）
１３  撮像装置
５１  部品組付装置
５２  多関節ロボット
５３  ハンド（移動体）
６１  リング状光源
６４  検知処理部（処理部）
７１  リング状拡散反射面（拡散反射部材）
Ｃ１  光軸
ＬＳ１〜ＬＳ４  光源
ｑ１〜ｑ４  第１の反射像
ｓ１〜ｓ４  第２の反射像
ｇ１  第１の中心
ｇ２  第２の中心
Ｘθ，Ｙθ，Ｚθ  回転角度
α  第１の角度
γ  第２の角度
【図１】
2014089095.tif 000008
【図２】
2014089095.tif 000009
【図３】
2014089095.tif 000010
【図４】
2014089095.tif 000011
【図５】
2014089095.tif 000012
【図６Ａ】
2014089095.tif 000013
【図６Ｂ】
2014089095.tif 000014
【図７】
2014089095.tif 000015
【図８】
2014089095.tif 000016
【図９】
2014089095.tif 000017
【図１０】
2014089095.tif 000018
【図１１Ａ】
2014089095.tif 000019
【図１１Ｂ】
2014089095.tif 000020
【図１２】
2014089095.tif 000021
【図１３】
2014089095.tif 000022
【図１４】
2014089095.tif 000023
【図１５】
2014089095.tif 000024
【図１６】
2014089095.tif 000025
【図１７】
2014089095.tif 000026
【図１８Ａ】
2014089095.tif 000027
【図１８Ｂ】
2014089095.tif 000028
【図１９】
2014089095.tif 000029
【図２０】
2014089095.tif 000030
【図２１】
2014089095.tif 000031