(57)【要約】  【課題】従来、回転体の姿勢角は機構的に拘束された軸となるジンバル軸回りの角度を検出することにより行っている。しかしながら、回転中心要素が球面軸受け等のピボット要素の場合は、ジンバル軸に相当する機構的に拘束された軸が存在しないため、従来の方法をそのまま適用できない。これに対応するため、ＬＥＤ、ＰＳＤなどの位置検出装置を回転の自由度の数だけ増設する方法も考えられるが、装置量が増大しスペース効率およびコストの面で好ましくない。  【解決手段】回転体に連動して回転する二つの直交するレーザ光線と、レーザ光線を受光する機構的に拘束された二つの２次元位置検出装置を用い、二つの２次元位置検出装置で検出した受光位置情報を基に回転体の姿勢角を算出することにより、回転中心要素が球面軸受け等のピボット要素の場合であっても回転体の姿勢角を効率的に検出できる姿勢角検出方法および検出装置を提供する。【選択図】図１
2005214867.tif 000002
【特許請求の範囲】
【請求項１】
回転体がピボット要素を介して支持機構に取り付けられ、
レーザ光源が、前記レーザ光源を源とする二つのレーザ光線の光軸が前記回転体の回転中心要素で直交するように前記回転体に固定され、
前記各レーザ光線に対応して前記レーザ光線を受光して受光位置を検出する２次元位置検出装置を設け、
前記２次元位置検出装置は、前記２次元位置検出装置の位置検出面の中心で受光する前記レーザ光線の光軸が前記位置検出面に対して直交するように前記支持機構に固定され、
前記２次元位置検出装置で検出される受光位置情報を基に、前記回転体が前記回転中心要素を中心として回転する際の姿勢角を算出する、
ことを特徴とする姿勢角検出方法。
【請求項２】
請求項１に記載の姿勢角検出方法において、
前記２次元位置検出装置の位置検出面の中心を原点とする２次元座標系を前記位置検出面上に定義して位置検出２次元座標系とし、
前記回転体の回転中心要素を始点として前記位置検出２次元座標系の原点へ向かうベクトルをｘ軸とし、回転中心要素を始点として前記位置検出２次元座標系で定義される２軸に平行となるベクトルをそれぞれｙ軸およびｚ軸とする３次元座標系を定義して位置検出３次元座標系とし、
前記位置検出２次元座標系で検出される前記レーザ光線の受光位置情報を基に、前記位置検出３次元座標系おける前記回転体の姿勢を示す単位光線ベクトル情報を生成し、
前記各２次元位置検出装置に対応して生成される二つの前記単位光線ベクトル情報を基に前記回転体の姿勢角を算出する、
ことを特徴とする姿勢角検出方法。
【請求項３】
請求項２に記載の姿勢角検出方法において、
前記位置検出３次元座標系において、前記回転体の回転中心要素から前記位置検出２次元座標の原点までの距離をｘとし、前記位置検出２次元座標系において前記レーザ光線を受光した際の受光位置を示す二つの座標値をそれぞれｙおよびｚとし、
前記単位光線ベクトル情報は、前記位置検出３次元座標系の原点を始点とし３次元座標（ｘ，ｙ，ｚ）を終点とするベクトルを単位ベクトルへ正規化したベクトル情報とする、
ことを特徴とする姿勢角検出方法。
【請求項４】
請求項１ないし請求項３のいずれかに記載の姿勢角検出方法において、
前記支持機構に固定されるｘ軸、ｙ軸、ｚ軸からなる３次元座標系を定義して支持機構固定座標系とし、
前記回転体の姿勢角を、前記支持機構固定座標系において、ｘ軸の正方向の右回りの回転を示すロール角、ｚ軸の正方向の右回りの回転を示すヨー角、および、ｙ軸の正方向の右回りの回転を示すピッチ角、の順に３軸回転する３軸回転角として表現する、
ことを特徴とする姿勢角検出方法。
【請求項５】
支持機構にピボット要素を介して取り付けられる回転体と、
前記回転体に固定されるレーザ光源と、
前記レーザ光源を源とする二つのレーザ光線の光軸を前記回転体の回転中心要素で直交  するように配置する手段と、
前記各レーザ光線に対応して設けられ、前記レーザ光線を受光して受光位置情報を検出する２次元位置検出装置と、
前記２次元位置検出装置を、前記２次元位置検出装置が位置検出面の中心で受光する前記レーザ光線の光軸が前記位置検出面に対して直交するように前記支持機構に固定する手段と、
前記２次元位置検出装置で検出される受光位置情報を基に、前記回転体が前記回転中心要素を中心として回転する際の姿勢角を算出する手段を備える、
ことを特徴とする姿勢角検出装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、回転体の姿勢角検出方法および姿勢角検出装置に係り、特に、空間安定化機構等のサーボ系においてロール、ヨー、ピッチの３軸回りの姿勢角を検出する際に、ロール、ヨー、ピッチの各軸が機構的拘束のないピボット要素等を回転中心要素とする回転体の姿勢角検出方法および姿勢角検出装置に関する。
【背景技術】
【０００２】
空間安定化機構のペイロード等の回転体は、ロール（ｘ軸回り）、ヨー（ｚ軸回り）、ピッチ（ｙ軸回り）の回転動作は機構的に拘束されて独立したそれぞれの軸で行われ、各軸に取り付けられたエンコーダ、レゾルバ等の角度検出器によって姿勢角を検出する方法が一般的に用いられている。しかしながら、小型軽量化、スペース制約等のためには、ロール、ヨー、ピッチそれぞれ独立した軸を設けるのではなく、回転中心要素として球面軸受け等のピボット要素を用いた構成とするのが有効である。この場合、ロール、ヨー、ピッチ各軸が機構的に存在しないため、従来の機構的に拘束された軸に取り付けられた角度検出器では姿勢角の検出は不可能であった。例えば、特許文献１には、ＰＳＤ(Position Sensitive Device：半導体位置検出装置)を用いた姿勢制御装置が示されている。これは、小型化が要求される装置にも好適しつつ、小型化に伴うモータの出力不測を解消するもので、撮像部の一方の端部に配置され角度検出器で検出された角度に応じて選択的に発光するＬＥＤ(Light-Emitting Diode：発光ダイオード)と、このＬＥＤから発光される光を受光しその受光位置に応じた出力を発生するＰＳＤとの間の相対関係により、撮像部の姿勢角度を検出するようにしている。ここでは、ＬＥＤ及びＰＳＤは、機構的に拘束されたジンバル軸に取り付けられた角度検出器の補助的な役目をしている。
【特許文献１】特開２００１−９９６５２号公報（第６〜７頁、第２〜３図）
【発明の開示】
【発明が解決しようとする課題】
【０００３】
上記のように、従来、回転体の姿勢角は機構的に拘束された軸となるジンバル軸回りの角度を検出することにより行っている。しかしながら、回転中心要素が球面軸受け等のピボット要素の場合は、上記のジンバル軸に相当する機構的に拘束された軸が存在しないため、従来の方法はそのまま適用できない。これに対応するため、ＬＥＤ、ＰＳＤなどの位置検出装置を回転の自由度の数だけ増設する方法も考えられるが、装置量が増大しスペース効率およびコストの面で好ましくない。また、ＬＥＤはコヒーレント光ではなく、直進性に問題を生じるため位置検出精度において好ましくない。
【０００４】
本発明は、回転体が３次元空間で回転する際の姿勢角検出方法および姿勢角検出装置において、回転中心要素がピボット要素で、ロール、ヨー、ピッチの各回転軸が機構的に拘束されない場合でも、２次元位置検出素子を用いて回転体の姿勢角を効率的に検出できる姿勢角検出方法および姿勢角検出装置を提供することを目的とする。
【課題を解決するための手段】
【０００５】
第一の発明は、回転体がピボット要素を介して支持機構に取り付けられ、レーザ光源が、前記レーザ光源を源とする二つのレーザ光線の光軸が前記回転体の回転中心要素で直交するように前記回転体に固定され、前記各レーザ光線に対応して前記レーザ光線を受光して受光位置を検出する２次元位置検出装置を設け、前記２次元位置検出装置は、前記２次元位置検出装置の位置検出面の中心で受光する前記レーザ光線の光軸が前記位置検出面に対して直交するように前記支持機構に固定され、前記２次元位置検出装置で検出される受光位置情報を基に、前記回転体が前記回転中心要素を中心として回転する際の姿勢角を算出する、ように構成した。
【０００６】
第一の発明によれば、レーザ光線に対応して設けられる２次元位置検出装置を用いて、回転体の回転と連動して回転するレーザ光線の受光位置情報を検出することにより、ピボット要素を介して３次元空間内を回転する回転体についても、その姿勢角を効率的に検出できる。
【０００７】
第二の発明は、上記第一の発明において、前記２次元位置検出装置の位置検出面の中心を原点とする２次元座標系を前記位置検出面上に定義して位置検出２次元座標系とし、  前記回転体の回転中心要素を始点として前記位置検出２次元座標系の原点へ向かうベクトルをｘ軸とし、回転中心要素を始点として前記位置検出２次元座標系で定義される２軸に平行となるベクトルをそれぞれｙ軸およびｚ軸とする３次元座標系を定義して位置検出３次元座標系とし、前記位置検出２次元座標系で検出される前記レーザ光線の受光位置情報を基に、前記位置検出３次元座標系おける前記回転体の姿勢を示す単位光線ベクトル情報を生成し、前記各２次元位置検出装置に対応して生成される二つの前記単位光線ベクトル情報を基に前記回転体の姿勢角を算出する、ように構成した。
【０００８】
第二の発明によれば、支持機構に固定された二つの２次元位置検出装置の位置検出面に定義される２次元座標系を基に、受光した二つのレーザ光線の受光位置情報を検出し、それを、３次元空間で回転する回転体の姿勢角を算出し易いようにするために３次元座標系の単位光線ベクトル情報に変換するもので、二つの２次元座標系での受光位置情報を測定するだけで３次元座標系での回転体の姿勢を効率よく算出できるようになる。
【０００９】
第三の発明は、上記第二の発明に関して、前記位置検出３次元座標系において、前記回転体の回転中心要素から前記位置検出２次元座標の原点までの距離をｘとし、前記位置検出２次元座標系において前記レーザ光線を受光した際の受光位置を示す二つの座標値をそれぞれｙおよびｚとし、前記単位光線ベクトル情報は、前記位置検出３次元座標系の原点を始点とし３次元座標（ｘ，ｙ，ｚ）を終点とするベクトルを単位ベクトルへ正規化したベクトル情報とする、ように構成した。
【００１０】
第三の発明によれば、３次元空間内の回転体の姿勢角の算出を、互いに直交する二つの単位ベクトルの座標情報の計算に単純化することができ、効率的な姿勢角算出が可能になる。
【００１１】
第四の発明は、上記の発明に関して、前記支持機構に固定されるｘ軸、ｙ軸、ｚ軸からなる３次元座標系を定義して支持機構固定座標系とし、前記回転体の姿勢角を、前記支持機構固定座標系において、ｘ軸の正方向の右回りの回転を示すロール角、ｚ軸の正方向の右回りの回転を示すヨー角、および、ｙ軸の正方向の右回りの回転を示すピッチ角、の順に３軸回転する３軸回転角として表現する、ように構成した。
【００１２】
第四の発明によれば、３次元空間内の回転体の姿勢を、ロール角、ヨー角、ピッチ角の３軸回りの回転角の組み合わせとして表現することができ、姿勢角の算出過程がより分かりやすく表現できるようになり、より効率的な姿勢角算出が可能となる。
【発明の効果】
【００１３】
本発明によれば、機構的に軸拘束のない回転中心要素が球面軸受け等のピボット要素の場合でも、二つの２次元位置検出装置での受光位置情報の測定により、３次元空間における回転体の姿勢角を検出することができる。また、姿勢角の算出は、二つの２次元位置検出装置での受光位置情報を基にして、直交する二つの単位ベクトルとなる単位光線ベクトル、および、ロール角、ヨー角、ピッチ角の３軸回転角に関する計算に単純化され、効率的な姿勢角算出が可能となる。
【発明を実施するための最良の形態】
【００１４】
【実施例】
【００１５】
図１は、本発明の姿勢角検出装置の構成（１）である。
【００１６】
支持機構に固定される支持機構固定座標系ｘｙｚを、ｘ軸２０１、ｙ軸２０２（紙面に垂直に立っている）、ｚ軸２０３により構成する。
【００１７】
レーザ光源４、５は、ピボット要素である回転中心要素１を中心に回転する回転体に固定され、それが発するレーザ光線の光軸は回転中心要素１で直交するように配置される。
【００１８】
２次元位置検出装置６、７は支持機構に固定され、レーザ光源４、５それぞれから発光されるレーザ光線をその位置情報検出面で受光し、その受光点の位置情報を検出して、ロール角（ｘ軸回り）、ヨー角（ｚ軸回り）、ピッチ角（ｙ軸回り）の３軸回りの角度で表現される回転体の姿勢角を算出する。ここで、ロール角は支持機構に固定された座標系のｘ軸２０１の正の向きを向いて右回りの回転角とし、ヨー角は支持機構に固定された座標系のｚ軸２０３の正の向きを向いて右回りの回転角とし、ピッチ角は支持機構に固定された座標系のｙ軸２０２の正の向きを向いて右回りの回転角とする。図１では、ｙ軸２０２は紙面に垂直に立っており、その垂直に立っている方向が正の向きとなる。なお、２次元位置検出素子としては、ＰＳＤまたはＣＣＤ(Charge Coupled Device：電荷結合素子)デバイスを利用することが可能である。
【００１９】
回転体と共に回転するレーザ光源４、５が発するレーザ光線の光軸は、回転中心要素１で互いに直交し、二つのレーザ光線の光軸は、初期状態として、それぞれ、２次元位置検出装置６、７の位置検出面の中心に垂直に交わっている。ベクトル２、３は、それぞれ、回転中心要素１を始点としてレーザ光線の光軸に沿ってレーザ光源４、５に向かうベクトルであり、回転体の回転運動に連動して、図１に示す初期状態から移動する。ただし、ベクトルの始点と二つのベクトルが直交した状態は維持される。
【００２０】
図１では、回転体に固定されるレーザ光源４，５が初期状態にある場合を示しており、２次元位置検出装置６、７およびレーザ光源４、５は３次元空間内のｘｚ平面上に配置されている。２次元位置検出装置６、７は、回転中心要素１から２次元位置検出装置６、７の位置検出面の中心に向かう二つのベクトル（図１の初期状態ではベクトル２、３と一致している）が、それぞれ、ｘｚ平面上でｘ軸の正方向に対して+45[deg]および+135[deg]の角度になるように支持機構に固定して配置されている。ここで、記号[deg]は角度を示す単位で、円周を360に等分した場合の１単位の円周の中心角を示す（以下同様）。
【００２１】
レーザ光源４、５は回転体に固定されており、その光軸が回転中心要素１で互いに直交した状態のまま、回転中心要素１を中心にして３次元の回転を行うが、回転体は、二つのレーザ光線の各光軸が２次元位置検出装置６または７の位置検出面に交差する範囲内で回転するものとする。
【００２２】
図２は、本発明の姿勢角検出装置の構成（２）で、図１に示したレーザ光源４、５の構成例を示すもので、レーザ発光素子８から発せられたレーザ光が、集光レンズ９で集光される様子を示している。
【００２３】
上記の図１の構成では、わかり易く表現するために、初期状態として二つのレーザ光線の光軸が支持機構固定座標系におけるｘｚ座標面上にあり、二つの２次元位置検出装置はｘ軸の正方向に対して+45[deg]および +135[deg]の角度に支持機構に固定的に配置されている場合を示したが、本発明はこれに限定されるものではない。以降の説明では、初期状態としては、回転中心要素１で互いに直交する光軸が３次元空間内の任意の位置にある場合を前提にして、回転体の姿勢角算出原理を示す。
【００２４】
図３は、座標系定義の概念図であり、２次元位置検出装置の位置検出面に定義される位置検出２次元座標系と、それを基に定義される位置検出３次元座標系AｘAｙAｚを示している。
【００２５】
軸１０１、軸１０２は、２次元位置検出装置６の位置検出面上に定義される位置検出２次元座標系の座標軸である。
【００２６】
また、Aｘ軸は、回転中心要素１を原点とし、２次元位置検出装置６の位置検出面の中心に向かうベクトルとして定義され、該位置検出面に対して垂直に交差している。Aｙ 軸、Aｚ軸は、回転中心要素１を始点とし、それぞれ、軸１０１、軸１０２と同じ向きとなるベクトルとして定義する。上記のようにして定義されるAｘ軸、Aｙ軸、Aｚ 軸から構成される座標系を位置検出３次元座標系AｘAｙAｚと定義する。
【００２７】
回転体が３軸回転することにより、レーザ光源４から発せられるレーザ光線の光軸、つまり、回転中心要素１から２次元位置検出装置６の位置検出面へ向かうベクトル２が変位し、その変位は、２次元位置検出装置６の位置検出面に定義される位置検出２次元座標系での受光位置の変位として検出される。
【００２８】
位置検出２次元座標系での受光位置の座標を、１０１軸上の値をＰy、１０２軸上の値をＰzで表すと、位置検出２次元座標系では原点（０，０）から（Ｐy，Ｐz）への変位として検出され、これを位置検出３次元座標系AｘAｙAｚ上で見ると、回転中心要素１から２次元位置検出装置６の位置検出面の中心までの距離をｄとして、座標（ｄ，０，０）から座標（ｄ,Ｐy,Ｐz）への変位として表現できる。
位置検出３次元座標系AｘAｙAｚにおいて、原点（0,０,０）から上記の座標（ｄ,Ｐy,Ｐz）へのベクトルを単位ベクトルに正規化した単位光線ベクトルsＰ（図３のベクトル２に相当）を、
【００２９】
【数１】
2005214867.tif 000003
【００３０】
と定義する。ここで、|SＰ|＝1で、SＰxは、回転中心要素１から２次元位置検出装置６の位置検出面の中心までの距離ｄを単位ベクトルのAｘ成分になるように正規化したものであり、SＰyおよびSＰzは、それぞれ、２次元位置検出装置6の位置検出面での位置検出量を単位ベクトルのAｙ成分およびAｚ成分になるように正規化したものである。
【００３１】
もう一つの２次元位置検出装置７についても同様にして位置検出３次元座標系BｘBｙBｚ を定義し、座標系BｘBｙBｚにおいて、図１に示した回転中心要素１を通るレーザ光源 5から発するレーザ光線の光軸上、つまり、回転中心要素１から２次元位置検出装置７の位置検出面に向かうベクトルを単位ベクトルに正規化した単位光線ベクトルsＱを、
【００３２】
【数２】
2005214867.tif 000004
【００３３】
と定義する。
【００３４】
次に、位置検出３次元座標系AｘAｙAｚのAｘ軸、Aｙ軸、Aｚ 軸の支持機構固定座標系ｘｙｚにおける単位ベクトルを、それぞれ、Aｅx、Aｅy 、Aｅz とし、位置検出３次元座標系
BｘBｙBｚ のBx軸、By軸、Bz軸の支持機構固定座標系ｘｙｚにおける単位ベクトルを、それぞれ、Bex、Bey、Bｅz とすると、位置検出３次元座標系AｘAｙAｚおよび位置検出３次元座標系BｘBｙBｚ から、支持機構固定座標系 ｘｙｚへの変換行列は、それぞれ、
【００３５】
【数３】
2005214867.tif 000005
【００３６】
となり、位置検出３次元座標系で表した単位光線ベクトルSＰ、 SＱを支持機構固定座標系ｘｙｚで表した単位光線ベクトルＰ、Ｑは、
【００３７】
【数４】
2005214867.tif 000006
【００３８】
となり、これにより、位置検出３次元座標系AｘAｙAｚおよび位置検出３次元座標系
BｘBｙBｚ による座標軸成分から、支持機構固定座標系 ｘｙｚによる座標軸成分を求めることができる。
【００３９】
図４は座標系変換の概念図で、上記の単位光線ベクトルＰおよびＱと、位置検出３次元座標系AｘAｙAｚ、位置検出３次元座標系BｘBｙBｚ 、および支持機構固定座標系 ｘｙｚとの関係の概要を示したものである。
【００４０】
２次元位置検出装置６に対しては、回転中心要素１を始点としてその位置検出面の中心に直交するベクトルをAｘ軸とし、その位置検出面に定義される２次元座標系の座標軸１０１および１０２と平行な、回転中心要素１を始点とするベクトルをそれぞれAｙ軸、および、Aｚ 軸として、位置検出３次元座標系AｘAｙAｚを構成する。２次元位置検出装置７に対しても、同様にして、位置検出３次元座標系BｘBｙBｚを構成する。
【００４１】
次に、互いに直交する単位光線ベクトルＰ、Ｑの姿勢角を定義する。支持機構固定座標系ｘｙｚ系において、直交する２つの単位ベクトル[1 0 0]Tおよび[0 0 1]Tが、その直交関係を維持した状態で３軸回転して、直交する単位光線ベクトルＰ、Ｑ  になったとした場合のロール角（ｘ軸回りの回転角）α、ヨー角（ｚ軸回りの回転角）β、ピッチ角（ｙ軸回りの回転角）γの組を、直交する単位光線ベクトルＰ、Ｑの姿勢角と定義し、以降
"(α,β,γ)"と表記する。
【００４２】
このとき、ｘ 軸回りのロール角αの回転行列をＲx(α)、ｚ 軸回りのヨー角βの回転行列を Ｒz(β)、ｙ 軸回りのピッチ角γの回転行列をＲy(γ)と表記すると、３軸回転時の回転行列は次のように表される。
【００４３】
【数５】
2005214867.tif 000007
【００４４】
上記の式（２）と、姿勢角の定義より直交する単位ベクトル[1 0 0]T、[0 0 1]T  が、それぞれ、ロール角α、ヨー角β、ピッチ角γの順に３軸回転して、直交する単位  光線ベクトルＰ、Ｑになることから、
【００４５】
【数６】
2005214867.tif 000008
【００４６】
が得られ、これを基にして、単位光線ベクトルＰ、Ｑに関して、支持機構固定座標系
ｘｙｚの座標軸成分と、姿勢角(α,β,γ)の成分についての次の関係式が得られる  。
【００４７】
【数７】
2005214867.tif 000009
【００４８】
上記の式（４）により、単位光線ベクトルＰ、Ｑの支持機構固定座標系ｘｙｚの座標軸成分から、単位光線ベクトルＰ、Ｑの姿勢角(α,β,γ)を求めることができる。
【００４９】
ここで、arctan2(Ｙ，Ｘ) はロボット工学等で用いられる関数で、tan-1（Ｙ／Ｘ）を拡張したものである。角度をラジアンで表示した場合、tan-1（Ｙ／Ｘ）の値域が
【００５０】
【数８】
2005214867.tif 000010
【００５１】
であり、Ｘ＝０の場合に対応できないのに対し、arctan2(Ｙ，Ｘ)は、引数Ｙ、Ｘの符号から象限を特定し、
【００５２】
【数９】
2005214867.tif 000011
【００５３】
の値域を持ち、Ｘ＝０ の場合にもＹ の符号に応じて、
【００５４】
【数１０】
2005214867.tif 000012
【００５５】
を出力する。
【００５６】
次に、回転中心要素１を始点として、支持機構に固定された２次元位置検出装置６，７の位置検出面の中心方向を指す互いに直交する単位ベクトル、つまり、初期状態の単位光線ベクトルＰ0、Ｑ0が、回転体の３軸回転により直交する単位光線ベクトルＰ、Ｑになった場合を考える。例えば、図１に示した互いに直交するベクトル２、３が、それぞれ、初期状態の単位光線ベクトルＰ0、Ｑ0に相当する。
【００５７】
初期状態の直交する単位光線ベクトルＰ0、Ｑ0の姿勢角を（α0，β0，γ0）とし、直交する単位光線ベクトルＰ、Ｑの姿勢角を（α，β，γ）とする。そして、回転体が３軸回転した際の姿勢角の各成分、つまり、ロール角、ヨー角、ピッチ角の変化量を、それぞれ、Δα、Δβ、Δγと表記し、この姿勢角変化分の回転を行う回転行列をΔＲ(Δα,Δβ,Δγ)と表記すると、
【００５８】
【数１１】
2005214867.tif 000013
【００５９】
であることから、
【００６０】
【数１２】
2005214867.tif 000014
【００６１】
となり、さらに、回転行列の特性として、その逆行列は転置行列に等しいことから、
【００６２】
【数１３】
2005214867.tif 000015
【００６３】
となり、回転体の互いに直交する単位光線ベクトルの初期姿勢角（α0，β0，γ0）が分かれば、上記式（４）で求めた回転後の回転体の互いに直交する単位光線ベクトルの姿勢角（α，β，γ）を基に、回転行列ΔＲ(Δα,Δβ,Δγ)を求めることができる。
【００６４】
次に、上記で求めた回転行列ΔＲ(Δα,Δβ,Δγ)を基に、ロール角Δα、ヨー角Δβ、ピッチ角Δγを算出するためには、直交する単位ベクトル[1 0 0]T、[0 0 1]Tが、それぞれ、ロール角Δα、ヨー角Δβ、ピッチ角Δγだけ３軸回転してできる直交する単位ベクトル
ｅ1、ｅ2の姿勢角（Δα，Δβ，Δγ）を、式(４）を用いて求めればよい。
【００６５】
つまり、まず、支持機構固定座標系ｘｙｚ上でのｘ軸方向の単位ベクトルｅx=[1 0 0]T、および、ｚ軸方向の単位ベクトルｅz=[0 0 1]T を回転行列ΔＲ(Δα,Δβ,Δγ)で変換して、
【００６６】
【数１４】
2005214867.tif 000016
【００６７】
を得、次に、この直交する単位ベクトルｅ1=[ｅ1x,ｅ1y,ｅ1z]Tおよびｅ2=[ｅ2x,ｅ2y,ｅ2z]Tと、その姿勢角（Δα，Δβ，Δγ）の成分に対して式(４）を適用すると、
【００６８】
【数１５】
2005214867.tif 000017
【００６９】
となり、これを基にして最終的な３軸姿勢角の変位量Δα、Δβ、Δγを算出できる。
【００７０】
以上が、本発明による姿勢角算出の原理である。
【００７１】
次に、上記の式（１）〜（６）を用いて、２次元位置検出装置の位置検出面での受光位置の検出データから回転体の姿勢角の変化量を求める場合の、算出手順の実施例を示す。
【００７２】
以下の実施例は、上記の姿勢角算出原理を用いた算出手順の過程を示すことが目的であり、算出した結果は計算過程で丸め誤差が混入していることから若干の誤差が含まれているが、本発明の本質には影響しない。
【００７３】
図５は本発明の実施例（１）の全体図であり、図６は、図５に示す本発明の実施例（１）の中の主要部分を拡大表示したものである。
【００７４】
レーザ光源３０１、３０２は、それが発するレーザ光線の光軸が、回転中心要素１(球面軸受け)で互いに直交する形で回転体３０５に固定されている。ここでは、２次元位置検出装置としてＰＳＤを用いた例を示しており、ＰＳＤ３０４、３０３は、回転中心要素１からそれらの位置検出面の中心への垂線が互いに直交する形で支持機構３０６に固定されている。支持機構３０６については前記支持機構固定座標系ｘｙｚが定義され、ＰＳＤ３０４については前記位置検出３次元座標系AｘAｙAｚが定義され、ＰＳＤ３０３については前記位置検出３次元座標系BｘBｙBｚが定義される。
【００７５】
図５、６に示す実施例（１）では、回転中心要素１から、ＰＳＤ３０４、３０３の位置検出面の中心に向かう単位ベクトル、つまり、互いに直交する単位光線ベクトルをそれぞれＰ0、Ｑ0とすると、その姿勢角は（0[deg],0[deg],+45[deg]）になっている。つまり、初期状態の直交する単位光線ベクトルＰ0、Ｑ0は、支持機構固定座標系ｘｙｚにおいて、互いに直交する単位ベクトル[1 0 0]Tおよび[0 0 1]Tを、ｙ軸回り（ｙ軸の正方向に向かって右回り）に+45[deg]回転させたものと同じである。
【００７６】
一方、ＰＤＳ３０４の位置検出３次元座標系AｘAｙAｚは、支持機構固定座標系ｘｙｚと一致していた座標系がｙ軸回りに+45[deg]度回転してできたものとして定義し、また、ＰＤＳ３０３の位置検出３次元座標系BｘBｙBｚは、支持機構固定座標系ｘｙｚと一致していた座標系がｙ軸回りに-45[deg]回転してできたものとして定義する。
【００７７】
このとき、位置検出３次元座標系AｘAｙAｚおよび位置検出３次元座標系BｘBｙBｚにそれぞれ対応する座標変換行列AＴ、BＴは次のようになる。
【００７８】
【数１６】
2005214867.tif 000018
【００７９】
【数１７】
2005214867.tif 000019
【００８０】
また、ＰＳＤ３０４の位置検出２次元座標系で測定される位置検出値は、定義より、前記位置検出３次元座標系AｘAｙAｚにおけるｙ軸方向およびｚ軸方向の変位として表され、ＰＳＤ３０３の位置検出２次元座標系で測定される位置検出値は、前記位置検出３次元座標系BｘBｙBｚにおけるｙ軸方向およびｚ軸方向の変位として表される。
【００８１】
また、図５、６に示す実施例（１）では、位置検出３次元座標系AｘAｙAｚおよび位置検出３次元座標系BｘBｙBｚのいずれにおいても、ｘ軸の値、つまり、回転中心要素１からＰＳＤ３０４およびＰＳＤ３０３の位置検出面までの距離は、ｄ＝40[mm]に固定されている。ここで、[mm]は、長さをミリメートルの単位で表示することを示す（以下同様）。
【００８２】
上記の実施例（１）の条件を基に、回転体３０５の直交する単位光線ベクトルが初期姿勢角(0[deg],0[deg],+45[deg]）から姿勢角(α,β,γ)へ回転した際の変位姿勢角(Δα,Δβ,Δγ)を本発明による姿勢角算出原理を用いて算出する際のシミュレーション実施例を、以下の（１）および（２）に示す。
(1)変位姿勢角算出シミュレーション１
回転体３０５がピッチ軸（ｙ軸２０２）回りに回転した結果のＰＳＤの位置検出面に定義される位置検出２次元座標系での位置検出値が次の通りであったとする。ここで、位置検出値は、"（位置検出３次元座標系のｙ軸方向の値, 位置検出３次元座標系のｚ軸方向の値）"の形式で表記する。
【００８３】
・ＰＳＤ３０４：  (0.00[mm],+3.50[mm])
・ＰＳＤ３０３：  (0.00[mm],+3.50[mm])
ここで、位置検出３次元座標系におけるｘ軸の値、つまり、回転中心要素１からＰＳＤ３０４およびＰＳＤ３０３の位置検出面までの距離は、ｄ＝40[mm]に固定されていることから、位置検出３次元座標系AｘAｙAｚにおいて、ＰＳＤ３０４の位置検出面における検出点を示すベクトルは(+40[mm] 0.00[mm] +3.50[mm])Tとなり、これを単位ベクトルに正規化した位置検出３次元座標系座標系AｘAｙAｚにおける単位光線ベクトルSＰは、
【００８４】
【数１８】
2005214867.tif 000020
【００８５】
となる。
【００８６】
同様にして、位置検出３次元座標系座標系BｘBｙBｚにおける、ＰＳＤ３０３の位置検出面における検出点を示すベクトルは(+40[mm] 0.00[mm] +3.50[mm])Tとなり、これを単位ベクトルに正規化した位置検出３次元座標系座標系BｘBｙBｚにおける単位光線ベクトルSＱは、
【００８７】
【数１９】
2005214867.tif 000021
【００８８】
となる。
【００８９】
前記式(１)〜（６）および実施例（１）に適用される式（７）より、直交する単位光線ベクトルSＰおよびSＱを、支持機構固定座標系ｘｙｚで表現した互いに直交する単位光線ベクトルＰおよびＱは、
【００９０】
【数２０】
2005214867.tif 000022
【００９１】
【数２１】
2005214867.tif 000023
【００９２】
となる。この結果を前記式（４）に適用すると、回転体３０５に連動して回転する互いに直交する単位光線ベクトルＰおよびＱの姿勢角(α,β,γ)は、
【００９３】
【数２２】
2005214867.tif 000024
【００９４】
となり、これから
【００９５】
【数２３】
2005214867.tif 000025
【００９６】
が得られる。
【００９７】
上記の結果より、回転体３０５に連動して回転する直交する単位光線ベクトルＰおよびＱの３軸回りの回転行列Ｒx(α)、Ｒz(β)、Ｒy(γ)は、
【００９８】
【数２４】
2005214867.tif 000026
【００９９】
となる。
【０１００】
同様にして、直交する単位光線ベクトルＰおよびＱの初期状態の姿勢角(α0,β0,γ0)は
（0[deg],0[deg],+45[deg]）であることから、初期姿勢角の３軸回りの回転行列Ｒx(α0)、Ｒz(β0)、Ｒy(γ0)は、
【０１０１】
【数２５】
2005214867.tif 000027
【０１０２】
となる。
【０１０３】
以上の結果を前記式（５）に適用することにより、直交する単位光線ベクトルＰおよびＱの姿勢角の変化分の回転に対応する回転行列ΔＲ(Δα,Δβ,Δγ)は、
【０１０４】
【数２６】
2005214867.tif 000028
【０１０５】
となる。
【０１０６】
一方、直交する単位ベクトル[1 0 0]Tおよび[0 0 1]Tが、姿勢角(Δα,Δβ,Δγ)だけ回転して直交するベクトルｅ1およびｅ2になったとすると、
【０１０７】
【数２７】
2005214867.tif 000029
【０１０８】
となり、これを前記式（６）に適用すると、
【０１０９】
【数２８】
2005214867.tif 000030
【０１１０】
が得られる。
【０１１１】
以上より、回転体３０５の回転に連動して、回転体３０５の直交する単位光線ベクトルが、初期姿勢角（α0,β0,γ0）=(0[deg],0[deg],+45[deg])から回転した際のＰＳＤ３０４およびＰＳＤ３０３での位置検出値が
・ＰＳＤ３０４：  (0.00[mm],+3.50[mm])
・ＰＳＤ３０３：  (0.00[mm],+3.50[mm])
であった場合は、回転による単位光線ベクトルの変位姿勢角(Δα,Δβ,Δγ) は、(0.0[deg],0.0[deg], -5.003[deg])と算出できる。
【０１１２】
次に、実施例（１）における二番目の変位姿勢角算出例を示す。
（２）変位姿勢角算出シミュレーション２
上記の実施例（１）において、回転体３０５がピッチ軸（ｙ軸２０２）回りに回転した結果のＰＳＤでの位置検出値が次の通りであったとする。
【０１１３】
・ＰＳＤ３０４：  (+2.97[mm],-2.15[mm])
・ＰＳＤ３０３：  (0.002[mm],-2.15[mm])
ＰＳＤ３０４に定義された位置検出３次元座標系AｘAｙAｚにおけるＰＳＤ３０４の位置検出面での検出点を示すベクトルは、上記の変位姿勢角シミュレーション１と同様にして、(40[mm], +2.97[mm], -2.15[mm])Tとなり、これを単位ベクトルに正規化した位置検出３次元座標系AｘAｙAｚの単位光線ベクトルSＰは、
【０１１４】
【数２９】
2005214867.tif 000031
【０１１５】
となる。同様にして、ＰＳＤ３０３に定義された位置検出３次元座標系BｘBｙBｚを基にしたＰＳＤ３０３の位置検出面での検出点を示すベクトルは(+40[mm], +0.002[mm], -2.15[mm])Tとなり、これを単位ベクトルに正規化した位置検出３次元座標系BｘBｙBｚの単位光線ベクトルSＱは、
【０１１６】
【数３０】
2005214867.tif 000032
【０１１７】
となる。
【０１１８】
また、位置検出３次元座標系AｘAｙAｚ、および、位置検出３次元座標系BｘBｙBｚ から、支持機構固定座標系 ｘｙｚ 系への回転行列AＴおよびBＴは、上記変位姿勢角算出シミュレーション１と同様で、
【０１１９】
【数３１】
2005214867.tif 000033
【０１２０】
【数３２】
2005214867.tif 000034
【０１２１】
であり、前記式(１)より、支持機構固定座標系 ｘｙｚ 系での直交する単位光線ベクトルＰおよびＱは、
【０１２２】
【数３３】
2005214867.tif 000035
【０１２３】
【数３４】
2005214867.tif 000036
【０１２４】
となる。上記の結果を前記式(４）に適用すると、直交する単位光線ベクトルＰおよびＱの姿勢角（α,β,γ）は、
【０１２５】
【数３５】
2005214867.tif 000037
【０１２６】
となり、これから
【０１２７】
【数３６】
2005214867.tif 000038
【０１２８】
が得られる。
【０１２９】
上記の結果より、互いに直交する単位光線ベクトルＰおよびＱの３軸周りの回転行列
Ｒx(α)、Ｒz(β)、Ｒy(γ)は、
【０１３０】
【数３７】
2005214867.tif 000039
【０１３１】
となる。同様にして、互いに直交する単位光線ベクトルＰおよびＱの初期状態の姿勢角
（α0,β0,γ0）は、(0[deg],0[deg],+45[deg])であることから、
【０１３２】
【数３８】
2005214867.tif 000040
【０１３３】
が得られ、初期姿勢角の３軸回りの回転行列Ｒx(α0)、Ｒz(β0)、Ｒy(γ0)は、
【０１３４】
【数３９】
2005214867.tif 000041
【０１３５】
となる。
【０１３６】
以上の結果を前記式(５）に適用することにより、直交する単位光線ベクトルＰおよびＱが、その初期姿勢角(α0,β0,γ0) =(0[deg],0[deg],+45[deg])の位置から、姿勢角の変化分(Δα,Δβ,Δγ)の回転を行う際の回転行列ΔＲ(Δα,Δβ,Δγ)は、
【０１３７】
【数４０】
2005214867.tif 000042
【０１３８】
となる。一方、直交する単位ベクトル[1 0 0]Tおよび[0 0 1]Tが、姿勢角
(Δα,Δβ,Δγ)だけ回転して直交ベクトルｅ1およびｅ2になったとすると、
【０１３９】
【数４１】
2005214867.tif 000043
【０１４０】
【数４２】
2005214867.tif 000044
【０１４１】
となり、これを前記式(６）に適用すると、
【０１４２】
【数４３】
2005214867.tif 000045
【０１４３】
が得られる。
【０１４４】
以上より、回転体３０５と連動して回転する直交する単位光線ベクトルが初期姿勢角
（α0,β0,γ0）=(0[deg],0[deg],+45[deg])から回転して現在の姿勢角(α,β,γ)へ変位した際の変位姿勢角(Δα,Δβ,Δγ) は、(+3.0037[deg],+2.9979[deg],+2.9916[deg])と算出できる。
【０１４５】
以上が、図５、６に示した本発明の実施例（１）についての変位姿勢角算出シミュレーションの実施例である。
【０１４６】
次に、もう一つの実施例である実施例（２）について説明する。
【０１４７】
図７は本発明の実施例（２）である。
【０１４８】
単一のレーザ光源４０１、ハーフ・ミラー４０２、および、ミラー４０３〜４０７は、回転中心要素１(球面軸受け)を中心に回転する回転体に固定され、ＰＳＤ４１０，４１１は支持機構４１２に固定されている。この実施例（２）では、図５、６に示した実施例（１）と比較すると、レーザ光源を単一にでき小型ミラーを適用することにより省スペース化が可能な構成にできる利点がある。レーザ光源４０１から発光されるレーザ光線はハーフ・ミラー４０２及びミラー４０３〜４０７によって２つのレーザ光線４０８、４０９に分割され、ＰＳＤ４１０、４１１で受光される。ここで、レーザ光線４０８および４０９の光軸は回転中心要素１において互いに直交している。
【０１４９】
支持機構４１２については、ｘ軸２０１、ｙ軸２０２（紙面に垂直に立っている）、ｚ軸２０３の３軸から成る支持機構固定座標系ｘｙｚが定義され、ＰＳＤ４１１については位置検出３次元座標系AｘAｙAｚが定義され、ＰＳＤ４１０については位置検出３次元座標系
BｘBｙBｚが定義される。
【０１５０】
レーザ光線４０８、４０９は回転体と連動して回転するが、図７では、その初期状態が示されている。つまり、レーザ光線４０８を示すベクトルは、回転中心要素１からＰＳＤ４１１の中心方向に向かうベクトルを初期ベクトルとし、レーザ光線４０９を示すベクトルは、回転中心要素１からＰＳＤ４１０の中心に向かうベクトルを初期ベクトルとし、回転体に固定された状態で、互いに直交した関係を維持したまま回転する。
【０１５１】
回転中心要素１からＰＳＤ４１１、４１０の中心に向かう単位ベクトルを、それぞれＰ0、Ｑ0で表すと、それらは互いに直交しており、支持機構固定座標系ｘｙｚを基準としたロール角（ｘ軸回り）、ヨー角（ｚ軸回り）、ピッチ角（ｙ軸回り）で示される姿勢角を“（ロール角,ヨー角,ピッチ角）”形式で表現すると、図７より、ベクトルＰ0、Ｑ0は、支持機構固定座標系ｘｙｚにおいて、互いに直交する単位ベクトル[1 0 0]Tおよび[0 0 1]Tを、ｙ軸回り（ｙ軸の正方向に向かって右回り）に-45[deg]回転させたものと同じであり、姿勢角の定義から、互いに直交する単位ベクトルＰ0、Ｑ0の姿勢角は（0[deg],0[deg],-45[deg]）とみなすことができる。
【０１５２】
また、図７より、ＰＤＳ４１１に定義される位置検出３次元座標系AｘAｙAｚは、支持機構固定座標系ｘｙｚと一致していた座標系がｙ軸回りに-45[deg]度回転してできたものとして定義され、ＰＤＳ３０３に定義される位置検出３次元座標系BｘBｙBｚは、支持機構固定座標系ｘｙｚと一致していた座標系がｙ軸回りに-135[deg]回転してできたもの定義される。このとき、位置検出３次元座標系AｘAｙAｚおよび位置検出３次元座標系BｘBｙBｚにそれぞれ対応する座標変換行列AＴ、BＴは次のようになる。
【０１５３】
【数４４】
2005214867.tif 000046
【０１５４】
【数４５】
2005214867.tif 000047
【０１５５】
ＰＳＤ４１１の位置検出面で測定されるレーザ光線４０８の位置検出値は、前記位置検出３次元座標系AｘAｙAｚにおけるｙ軸方向およびｚ軸方向の変位として検出され、ＰＳＤ４１０の位置検出面で測定されるレーザ光線４０９の位置検出値は、前記位置検出３次元座標系BｘBｙBｚにおけるｙ軸方向およびｚ軸方向の変位として検出される。また、位置検出３次元座標系AｘAｙAｚおよび位置検出３次元座標系BｘBｙBｚのいずれにおいても、ｘ軸方向の位置はｄ＝40[mm]に固定されている。
【０１５６】
上記の実施例（２）の条件で、回転体に定義される直交する単位光線ベクトルが初期姿勢角(0[deg],0[deg],+45[deg]）から姿勢角(α,β,γ)へ回転した際の変位姿勢角
(Δα,Δβ,Δγ)を本発明による姿勢角算出原理を用いて算出する際のシミュレーション実施例を、以下の（３）および（４）に示す。
（３）変位姿勢角算出シミュレーション３
回転体がピッチ軸（ｙ軸２０２）回りに回転した結果のＰＳＤの位置検出面における位置検出値が次の通りであったとする。
【０１５７】
・ＰＳＤ４１１：(0.0[mm], +3.50[mm])
・ＰＳＤ４１０：(0.0[mm]，+3.50[mm])
回転中心要素１(球面軸受け)を中心に位置検出３次元座標系AｘAｙAｚを基にしたＰＳＤ４１１の位置検出面における位置検出点のベクトルは(+40[mm] 0.00[mm] +3.50[mm])Tとなり、これを単位ベクトルに正規化した単位光線ベクトルSＰは、
【０１５８】
【数４６】
2005214867.tif 000048
【０１５９】
となる。同様にして、ＰＳＤ４１０についての位置検出３次元座標系BｘBｙBｚにおける単位光線ベクトルSＱは、
【０１６０】
【数４７】
2005214867.tif 000049
【０１６１】
となる。
【０１６２】
また、前記式（８）より、位置検出３次元座標系AｘAｙAｚ、および、位置検出３次元座標系BｘBｙBｚ から、支持機構固定座標系 ｘｙｚへの変換行列AＴおよびBＴは、
【０１６３】
【数４８】
2005214867.tif 000050
【０１６４】
【数４９】
2005214867.tif 000051
【０１６５】
であることから、前記式（１)より、支持機構固定座標系 ｘｙｚにおける直交する単位光線ベクトルＰおよびＱは、
【０１６６】
【数５０】
2005214867.tif 000052
【０１６７】
【数５１】
2005214867.tif 000053
【０１６８】
となる。上記の結果を前記式（４）に適用すると、直交する単位光線ベクトルＰおよびＱの姿勢角（α,β,γ）は、
【０１６９】
【数５２】
2005214867.tif 000054
【０１７０】
となり、これから
【０１７１】
【数５３】
2005214867.tif 000055
【０１７２】
が得られる。
【０１７３】
上記の結果より、回転体に固定されて回転する直交する単位光線ベクトルＰおよびＱの３軸回りの回転行列Ｒx(α)、Ｒz(β)、Ｒy(γ)は、
【０１７４】
【数５４】
2005214867.tif 000056
【０１７５】
となる。同様にして、互いに直交する単位光線ベクトルＰおよびＱの初期状態の姿勢角
（α0,β0,γ0）は（0[deg],0[deg],-45[deg]）であることから、
【０１７６】
【数５５】
2005214867.tif 000057
【０１７７】
が得られ、初期姿勢角（α0,β0,γ0）の３軸回りの回転行列Ｒx(α0)、Ｒz(β0)、Ｒy(γ0)は、
【０１７８】
【数５６】
2005214867.tif 000058
【０１７９】
となる。
【０１８０】
以上の結果を、前記式（５）に適用することにより、直交する単位光線ベクトルＰおよびＱの姿勢角変位分の回転を行う回転行列ΔＲ(Δα,Δβ,Δγ)は、
【０１８１】
【数５７】
2005214867.tif 000059
【０１８２】
となる。一方、直交する単位ベクトル[1 0 0]Tおよび[0 0 1]Tが、姿勢角(Δα,Δβ,Δγ)だけ回転して直交ベクトルｅ1およびｅ2になったとすると、
【０１８３】
【数５８】
2005214867.tif 000060
【０１８４】
【数５９】
2005214867.tif 000061
【０１８５】
となり、これを前記式（６）に適用すると、
【０１８６】
【数６０】
2005214867.tif 000062
【０１８７】
が得られる。つまり、直交する単位光線ベクトルＰおよびＱの変位姿勢角(Δα,Δβ,Δγ)は、(0.0[deg],0.0[deg], -5.0035[deg])となる。
（４）変位姿勢角算出シミュレーション４
図７に示す本発明の実施例（２）において、回転体が初期状態から回転した結果のＰＳＤにおける位置検出値が次の通りであったとする。
【０１８８】
ＰＳＤ４１１：(0.002[mm]， -2.15 [mm])
ＰＳＤ４１０：(-2.97[mm]， -2.15 [mm])
回転中心要素１(球面軸受け)を中心に位置検出３次元座標系AｘAｙAｚを基にしたＰＳＤ４１１における検出値が示すベクトルは[+40[mm] +0.002[mm] -2.15[mm]]Tとなり、これを単位ベクトルに正規化した単位光線ベクトルSＰは、
【０１８９】
【数６１】
2005214867.tif 000063
【０１９０】
となる。同様にして、ＰＳＤ４１０についての位置検出３次元座標系BｘBｙBｚの単位光線ベクトルSＱは、
【０１９１】
【数６２】
2005214867.tif 000064
【０１９２】
となる。
【０１９３】
また、位置検出３次元座標系AｘAｙAｚ、および、位置検出３次元座標系BｘBｙBｚ 系から、支持機構固定座標系 ｘｙｚ 系への変換行列AＴおよびBＴは、上記の変位姿勢角算出シミュレーション１、２と同様にして、
【０１９４】
【数６３】
2005214867.tif 000065
【０１９５】
【数６４】
2005214867.tif 000066
【０１９６】
となり、前記式（１)より、支持機構固定座標系ｘｙｚにおける直交する単位光線ベクトルＰおよびＱは、
【０１９７】
【数６５】
2005214867.tif 000067
【０１９８】
【数６６】
2005214867.tif 000068
【０１９９】
となる。上記の結果を前記式（４）に適用すると、直交する単位光線ベクトルＰおよびＱの姿勢角（α,β,γ）は、
【０２００】
【数６７】
2005214867.tif 000069
【０２０１】
となり、これから
【０２０２】
【数６８】
2005214867.tif 000070
【０２０３】
が得られる。
【０２０４】
上記の結果より、回転体３０５と連動して回転する直交する単位光線ベクトルＰおよびＱの３軸周りの回転行列Ｒx(α)、Ｒz(β)、Ｒy(γ)は、
【０２０５】
【数６９】
2005214867.tif 000071
【０２０６】
となる。同様にして、直交する単位光線ベクトルＰおよびＱの初期状態の姿勢角
（α0,β0,γ0）は、(0[deg],0[deg],-45[deg])であることから、
【０２０７】
【数７０】
2005214867.tif 000072
【０２０８】
が得られ、初期姿勢角の３軸回りの回転行列Ｒx(α0)、Ｒz(β0)、Ｒy(γ0)は、
【０２０９】
【数７１】
2005214867.tif 000073
【０２１０】
となる。
【０２１１】
以上の結果を前記式（５）に適用することにより、直交する単位光線ベクトルＰおよびＱの姿勢角変位分の回転を行う回転行列ΔＲ(Δα,Δβ,Δγ)は、
【０２１２】
【数７２】
2005214867.tif 000074
【０２１３】
となる。一方、直交する単位ベクトル[1 0 0]Tおよび[0 0 1]Tが、姿勢角(Δα,Δβ,Δγ)だけ回転して直交ベクトルｅ1およびｅ2になったとすると、
【０２１４】
【数７３】
2005214867.tif 000075
【０２１５】
【数７４】
2005214867.tif 000076
【０２１６】
となり、これを前記式（６）に適用すると、
【０２１７】
【数７５】
2005214867.tif 000077
【０２１８】
となり、回転体と連動して回転する直交する単位光線ベクトルＰおよびＱが、その初期姿勢角（0[deg],0[deg],-45[deg]）の姿勢から回転した場合の、初期姿勢角からの変位姿勢角として、(+3.000[deg],+2.996[deg],+3.003[deg])が得られる。
【０２１９】
以上に詳述した本発明の実施の形態では、レーザ光線とそれを受光して位置を検出する２次元位置検出装置を実施例として示したが、直進性のある光線とそれを受光して位置を検出できる２次元位置検出装置であれば、レーザ光線に限定されずに本発明は適用可能である。
【０２２０】
また、説明をわかり易くするために、変位姿勢角算出のシミュレーション例では、支持機構固定座標系ｘｙｚのｘｚ平面に二つの２次元位置検出装置の位置検出面の中心と回転中心要素が含まれる場合で説明したが、本発明の姿勢角算出の原理はこれに限定されずに適用できる。つまり、回転体の初期姿勢を示す直交する単位光線ベクトルＰ0、Ｑ0は、任意に設定することができる。
【０２２１】
上記の実施例では、２次元位置検出装置として、ＰＳＤを用いた例を示したが、これに限定されることはなく、ＣＣＤ素子などの他の手段でも可能である。
【０２２２】
以上詳細に述べた本発明の実施の態様は、以下の付記の通りである。
（付記１）回転体がピボット要素を介して支持機構に取り付けられ、
レーザ光源が、前記レーザ光源を源とする二つのレーザ光線の光軸が前記回転体の回転中心要素で直交するように前記回転体に固定され、
前記各レーザ光線に対応して前記レーザ光線を受光して受光位置を検出する２次元位置検出装置を設け、
前記２次元位置検出装置は、前記２次元位置検出装置の位置検出面の中心で受光する前記レーザ光線の光軸が前記位置検出面に対して直交するように前記支持機構に固定され、
前記２次元位置検出装置で検出される受光位置情報を基に、前記回転体が前記回転中心要素を中心として回転する際の姿勢角を算出する、
ことを特徴とする姿勢角検出方法。
（付記２）付記１に記載の姿勢角検出方法において、
前記２次元位置検出装置の位置検出面の中心を原点とする２次元座標系を前記位置検出面上に定義して位置検出２次元座標系とし、
前記回転体の回転中心要素を始点として前記位置検出２次元座標系の原点へ向かうベクトルをｘ軸とし、回転中心要素を始点として前記位置検出２次元座標系で定義される２軸に平行となるベクトルをそれぞれｙ軸およびｚ軸とする３次元座標系を定義して位置検出３次元座標系とし、
前記位置検出２次元座標系で検出される前記レーザ光線の受光位置情報を基に、前記位置検出３次元座標系おける前記回転体の姿勢を示す単位光線ベクトル情報を生成し、
前記各２次元位置検出装置に対応して生成される二つの前記単位光線ベクトル情報を基に前記回転体の姿勢角を算出する、
ことを特徴とする姿勢角検出方法。
（付記３）付記２に記載の姿勢角検出方法において、
前記位置検出３次元座標系において、前記回転体の回転中心要素から前記位置検出２次元座標の原点までの距離をｘとし、前記位置検出２次元座標系において前記レーザ光線を受光した際の受光位置を示す二つの座標値をそれぞれｙおよびｚとし、
前記単位光線ベクトル情報は、前記位置検出３次元座標系の原点を始点とし３次元座標（ｘ，ｙ，ｚ）を終点とするベクトルを単位ベクトルへ正規化したベクトル情報とする、
ことを特徴とする姿勢角検出方法。
（付記４）付記１ないし付記３のいずれかに記載の姿勢角検出方法において、
前記支持機構に固定されるｘ軸、ｙ軸、ｚ軸からなる３次元座標系を定義して支持機構固定座標系とし、
前記回転体の姿勢角を、前記支持機構固定座標系において、ｘ軸の正方向の右回りの回転を示すロール角、ｚ軸の正方向の右回りの回転を示すヨー角、および、ｙ軸の正方向の右回りの回転を示すピッチ角、の順に３軸回転する３軸回転角として表現する、
ことを特徴とする姿勢角検出方法。
（付記５）付記１に記載の姿勢角検出方法において、
前記回転体に固定されるレーザ光源は二個のレーザ光源から成り、
前記二個のレーザ光源が発する二個のレーザ光線は、その光軸が前記回転体の回転中心要素で互いに直交するように配置される、
ことを特徴とする姿勢角検出方法。
（付記６）付記１に記載の姿勢角検出方法において、
前記回転体に固定されるレーザ光源は一個のレーザ光源から成り、
前記一個のレーザ光源が発するレーザ光線は、ハーフミラーおよびミラーを介して二個のレーザ光線に分岐され、前記分岐された二個のレーザ光線の光軸が前記回転体の回転中心要素で互いに直交するように配置される、
ことを特徴とする姿勢角検出方法。
（付記７）支持機構にピボット要素を介して取り付けられる回転体と、
前記回転体に固定されるレーザ光源と、
前記レーザ光源を源とする二つのレーザ光線の光軸を前記回転体の回転中心要素で直交  するように配置する手段と、
前記各レーザ光線に対応して設けられ、前記レーザ光線を受光して受光位置情報を検出する２次元位置検出装置と、
前記２次元位置検出装置を、前記２次元位置検出装置が位置検出面の中心で受光する前記レーザ光線の光軸が前記位置検出面に対して直交するように前記支持機構に固定する手段と、
前記２次元位置検出装置で検出される受光位置情報を基に、前記回転体が前記回転中心要素を中心として回転する際の姿勢角を算出する手段を備える、
ことを特徴とする姿勢角検出装置。
（付記８）付記７に記載の姿勢角検出装置において、
前記２次元位置検出装置の位置検出面の中心を原点として前記位置検出面に定義される２次元座標系である位置検出２次元座標系と、
前記回転体の回転中心要素を始点として前記位置検出２次元座標系の原点へ向かうベクトルをｘ軸とし、回転中心要素を始点として前記位置検出２次元座標系で定義される２軸に平行となるベクトルをそれぞれｙ軸およびｚ軸として定義される３次元座標系である位置検出３次元座標系と、
前記位置検出３次元座標系で検出される前記レーザ光線の受光位置情報を基に、前記位置検出３次元座標系における前記回転体の姿勢を示す単位光線ベクトル情報を生成する手段と、
前記各２次元位置検出装置に対応して生成される二つの前記単位光線ベクトル情報を基に前記回転体の姿勢角を算出する手段を備える、
ことを特徴とする姿勢角検出装置。
（付記９）付記８に記載の姿勢角検出装置において、
前記位置検出３次元座標系において、前記回転体の回転中心要素から前記位置検出２次元座標の原点までの距離をｘとし、前記位置検出２次元座標系において前記レーザ光線を受光した際の受光位置を示す二つの座標値をそれぞれｙおよびｚとし、前記位置検出３次元座標系の原点を始点とし３次元座標（ｘ，ｙ，ｚ）を終点とするベクトルを単位ベクトルへ正規化したベクトル情報を前記単位光線ベクトル情報とする手段を備える、
ことを特徴とする姿勢角検出装置。
（付記１０）付記７ないし付記９のいずれかに記載の姿勢角検出装置において、
前記支持機構に固定して定義されるｘ軸、ｙ軸、ｚ軸からなる３次元座標系である支持機構固定座標系と、
前記回転体の姿勢角を、前記支持機構固定座標系において、ｘ軸の正方向の右回りの回転を示すロール角、ｚ軸の正方向の右回りの回転を示すヨー角、および、ｙ軸の正方向の右回りの回転を示すピッチ角、の順に３軸回転する３軸回転角で表現する手段を備える、
ことを特徴とする姿勢角検出装置。
（付記１１）付記７に記載の姿勢角検出装置において、
前記回転体に固定されるレーザ光源は二個のレーザ光源から成り、
前記二個のレーザ光源が発する二個のレーザ光線は、その光軸が前記回転体の回転中心要素で互いに直交するように配置される、
ことを特徴とする姿勢角検出装置。
（付記１２）付記７に記載の姿勢角検出装置において、
前記回転体に固定されるレーザ光源は一個のレーザ光源から成り、
前記一個のレーザ光源が発するレーザ光線は、ハーフミラーおよびミラーを介して二個のレーザ光線に分岐され、前記分岐された二個のレーザ光線の光軸が前記回転体の回転中心要素で互いに直交するように配置される、
ことを特徴とする姿勢角検出装置。
（付記１３）付記７に記載の姿勢角検出装置において、
前記２次元位置検出装置は、半導体位置検出装置（ＰＳＤ）で構成される、
ことを特徴とする姿勢角検出装置。
（付記１４）付記７に記載の姿勢角検出方法において、
前記３次元位置検出装置は、電荷結合素子（ＣＣＤ）で構成される、
ことを特徴とする姿勢角検出装置。
【図面の簡単な説明】
【０２２３】
【図１】本発明の姿勢角検出装置の構成（１）
【図２】本発明の姿勢角検出装置の構成（２）
【図３】座標系定義の概念図
【図４】座標系変換の概念図
【図５】本発明の実施例（１）の全体図
【図６】本発明の実施例（１）の拡大図
【図７】本発明の実施例（２）
【符号の説明】
【０２２４】
1    回転中心要素（ピボット要素）
2,3    ベクトル
4,5,    レーザ
6,7    ２次元位置検出装置
8    レーザ発光素子
9    集光用レンズ
101,102    ２次元位置検出装置の位置検出面上の座標軸
103    位置検出３次元座標系AｘAｙAｚのAｘ軸
104    位置検出３次元座標系AｘAｙAｚのAｙ軸
105    位置検出３次元座標系AｘAｙAｚのAｚ軸
201    支持機構固定座標系ｘｙｚのｘ軸
202    支持機構固定座標系ｘｙｚのｙ軸
203    支持機構固定座標系ｘｙｚのｚ軸
301,302    レーザ光源
303,304    ＰＳＤ
401    レーザ光源
402    ハーフ・ミラー
403,404,405,406,407    ミラー
408,409    レーザ光線
410,411    ＰＳＤ
412    支持機構
【図１】
2005214867.tif 000078
【図２】
2005214867.tif 000079
【図３】
2005214867.tif 000080
【図４】
2005214867.tif 000081
【図５】
2005214867.tif 000082
【図６】
2005214867.tif 000083
【図７】
2005214867.tif 000084