(57)【要約】【課題】従来、回転体の姿勢角は機構的に拘束された軸となるジンバル軸回りの角度を検出することにより行っている。しかしながら、回転中心要素が球面軸受け等のピボット要素の場合は、ジンバル軸に相当する機構的に拘束された軸が存在しないため、従来の方法をそのまま適用できない。これに対応するため、ＬＥＤ、ＰＳＤなどの位置検出装置を回転の自由度の数だけ増設する方法も考えられるが、装置量が増大しスペース効率およびコストの面で好ましくない。【解決手段】回転体に連動して回転する二つの任意角で交差するレーザ光線と、レーザ光線を受光する機構的に拘束された二つの２次元位置検出装置を用い、二つの２次元位置検出装置で検出した受光位置情報を基に回転体の姿勢角を算出することにより、回転中心要素が球面軸受け等のピボット要素の場合であっても回転体の姿勢角を効率的に検出できる姿勢角検出方法および検出装置を提供する。【選択図】図１
2007225365.tif 000002
【特許請求の範囲】
【請求項１】
回転体がピボット要素を介して支持機構に取り付けられ、
レーザ光源が、前記レーザ光源を源とする二つのレーザ光線の光軸が前記回転体の回転中心要素で所定の交差角で交わるように前記回転体に固定され、
前記各レーザ光線に対応して前記レーザ光線を受光して受光位置を検出する２次元位置検出装置を設け、
前記２次元位置検出装置は、前記２次元位置検出装置の位置検出面の中心で受光する前記レーザ光線の光軸が前記位置検出面に対して直交するように前記支持機構に固定され、
前記２次元位置検出装置で検出される受光位置情報を基に、前記回転体が前記回転中心要素を中心として回転する際の姿勢角を算出する、
ことを特徴とする姿勢角検出方法。
【請求項２】
請求項１に記載の姿勢角検出方法において、
前記２次元位置検出装置の位置検出面の中心を原点とする２次元座標系を前記位置検出面上に定義して位置検出２次元座標系とし、
前記回転体の回転中心要素を始点として前記位置検出２次元座標系の原点へ向かうベクトルをｘ軸とし、回転中心要素を始点として前記位置検出２次元座標系で定義される２軸に平行となるベクトルをそれぞれｙ軸およびｚ軸とする３次元座標系を定義して位置検出３次元座標系とし、
前記位置検出２次元座標系で検出される前記レーザ光線の受光位置情報を基に、前記位置検出３次元座標系おける前記回転体の姿勢を示す単位光線ベクトル情報を生成し、
前記各２次元位置検出装置に対応して生成される二つの前記単位光線ベクトル情報を基に前記回転体の姿勢角を算出する、
ことを特徴とする姿勢角検出方法。
【請求項３】
請求項２に記載の姿勢角検出方法において、
前記位置検出３次元座標系において、前記回転体の回転中心要素から前記位置検出２次元座標の原点までの距離をｘとし、前記位置検出２次元座標系において前記レーザ光線を受光した際の受光位置を示す二つの座標値をそれぞれｙおよびｚとし、
前記単位光線ベクトル情報は、前記位置検出３次元座標系の原点を始点とし３次元座標（ｘ，ｙ，ｚ）を終点とするベクトルを単位ベクトルへ正規化したベクトル情報とする、
ことを特徴とする姿勢角検出方法。
【請求項４】
請求項１ないし請求項３のいずれかに記載の姿勢角検出方法において、
前記支持機構に固定されるｘ軸、ｙ軸、ｚ軸からなる３次元座標系を定義して支持機構固定座標系とし、
前記回転体の姿勢角を、前記支持機構固定座標系において、ｘ軸の正方向の右回りの回転を示すロール角、ｚ軸の正方向の右回りの回転を示すヨー角、および、ｙ軸の正方向の右回りの回転を示すピッチ角、の順に３軸回転する３軸回転角として表現する、
ことを特徴とする姿勢角検出方法。
【請求項５】
支持機構にピボット要素を介して取り付けられる回転体と、
前記回転体に固定されるレーザ光源と、
前記レーザ光源を源とする二つのレーザ光線の光軸を前記回転体の回転中心要素で所定の交差角で交わるように配置する手段と、
前記各レーザ光線に対応して設けられ、前記レーザ光線を受光して受光位置情報を検出する２次元位置検出装置と、
前記２次元位置検出装置を、前記２次元位置検出装置が位置検出面の中心で受光する前記レーザ光線の光軸が前記位置検出面に対して直交するように前記支持機構に固定する手段と、
前記２次元位置検出装置で検出される受光位置情報を基に、前記回転体が前記回転中心要素を中心として回転する際の姿勢角を算出する手段を備える、
ことを特徴とする姿勢角検出装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、回転体の姿勢角検出方法および姿勢角検出装置に係り、特に、空間安定化機構等のサーボ系においてロール、ヨー、ピッチの３軸回りの姿勢角を検出する際に、ロール、ヨー、ピッチの各軸が機構的拘束のないピボット要素等を回転中心要素とする回転体の姿勢角検出方法および姿勢角検出装置に関する。
【背景技術】
【０００２】
空間安定化機構のペイロード等の回転体は、ロール、ヨー、ピッチそれぞれ機構的に拘束された独立した３軸を構成し、それぞれの軸に取り付けられたエンコーダ、レゾルバ等の角度検出器によって姿勢角を検出するのが一般的であるが、小型軽量化等の理由により、ロール、ヨー、ピッチそれぞれ独立した軸を設けるのではなく、回転中心として球面軸受け等のピボット要素を用いた構成とするのが有効である。この場合、ロール、ヨー、ピッチ各軸が機構的に存在しないため、従来の角度検出器では姿勢角の検出は不可能であった。
空間安定化機構のペイロード等の回転体は、ロール（ｘ軸回り）、ヨー（ｚ軸回り）、ピッチ（ｙ軸回り）の回転動作は機構的に拘束されて独立したそれぞれの軸で行われ、各軸に取り付けられたエンコーダ、レゾルバ等の角度検出器によって姿勢角を検出する方法が一般的に用いられている。しかしながら、小型軽量化、スペース制約等のためには、ロール、ヨー、ピッチそれぞれ独立した軸を設けるのではなく、回転中心要素として球面軸受け等のピボット要素を用いた構成とするのが有効である。この場合、ロール、ヨー、ピッチ各軸が機構的に存在しないため、従来の機構的に拘束された軸に取り付けられた角度検出器では姿勢角の検出は不可能であった。
従来のロール、ヨー、ピッチ各軸が存在しない場合の姿勢角を検出する特許事例としては、例えば特許文献１がある。そこでは、直交する二つのレーザ光線を用いて姿勢角を検出する方法が示されている。
【特許文献１】特開２００５−２１４８６７号公報（第５〜１１頁、第１、３、４図）
【発明の開示】
【発明が解決しようとする課題】
【０００３】
上記の特許文献１では、回転体の姿勢角は機構的に拘束された軸となるジンバル軸回りの角度を検出するのではなく、回転中心要素がピボット要素で、ロール、ヨー、ピッチの各回転軸が機構的に拘束されない場合でも、２次元位置検出素子を用いて回転体の姿勢角を効率的に検出できる姿勢角検出方法を開示している。
しかしながら、特許文献１の発明では、姿勢角算出に必要とされるレーザ光線(ベクトルＰ，Ｑで表現されている)が直交することが前提であるため、２次元位置検出装置(ＰＳＤ)及びレーザ光源の配置に制約が生じ、レーザ光のケラレ等を回避するために他の部品の配置に苦慮する等、装置実現上の機構設計上の制約が課題となっていた。
【課題を解決するための手段】
【０００４】
第一の発明は、回転体がピボット要素を介して支持機構に取り付けられ、レーザ光源が、前記レーザ光源を源とする二つのレーザ光線の光軸が前記回転体の回転中心要素で所定の交差角で交わるように前記回転体に固定され、前記各レーザ光線に対応して前記レーザ光線を受光して受光位置を検出する２次元位置検出装置を設け、前記２次元位置検出装置は、前記２次元位置検出装置の位置検出面の中心で受光する前記レーザ光線の光軸が前記位置検出面に対して直交するように前記支持機構に固定され、前記２次元位置検出装置で検出される受光位置情報を基に、前記回転体が前記回転中心要素を中心として回転する際の姿勢角を算出する、ように構成した。
第一の発明によれば、レーザ光線に対応して設けられる２次元位置検出装置を用いて、回転体の回転と連動して回転するレーザ光線の受光位置情報を検出することにより、ピボット要素を介して３次元空間内を回転する回転体についても、その姿勢角を効率的に検出できる。
第二の発明は、上記第一の発明において、前記２次元位置検出装置の位置検出面の中心を原点とする２次元座標系を前記位置検出面上に定義して位置検出２次元座標系とし、前記回転体の回転中心要素を始点として前記位置検出２次元座標系の原点へ向かうベクトルをｘ軸とし、回転中心要素を始点として前記位置検出２次元座標系で定義される２軸に平行となるベクトルをそれぞれｙ軸およびｚ軸とする３次元座標系を定義して位置検出３次元座標系とし、前記位置検出２次元座標系で検出される前記レーザ光線の受光位置情報を基に、前記位置検出３次元座標系おける前記回転体の姿勢を示す単位光線ベクトル情報を生成し、前記各２次元位置検出装置に対応して生成される二つの前記単位光線ベクトル情報を基に前記回転体の姿勢角を算出する、ように構成した。
第二の発明によれば、支持機構に固定された二つの２次元位置検出装置の位置検出面に定義される２次元座標系を基に、受光した二つのレーザ光線の受光位置情報を検出し、それを、３次元空間で回転する回転体の姿勢角を算出し易いようにするために３次元座標系の単位光線ベクトル情報に変換するもので、二つの２次元座標系での受光位置情報を測定するだけで３次元座標系での回転体の姿勢を効率よく算出できるようになる。
第三の発明は、上記第二の発明に関して、前記位置検出３次元座標系において、前記回転体の回転中心要素から前記位置検出２次元座標の原点までの距離をｘとし、前記位置検出２次元座標系において前記レーザ光線を受光した際の受光位置を示す二つの座標値をそれぞれｙおよびｚとし、前記単位光線ベクトル情報は、前記位置検出３次元座標系の原点を始点とし３次元座標（ｘ，ｙ，ｚ）を終点とするベクトルを単位ベクトルへ正規化したベクトル情報とする、ように構成した。
第三の発明によれば、３次元空間内の回転体の姿勢角の算出を、互いに直交する二つの単位ベクトルの座標情報の計算に単純化することができ、効率的な姿勢角算出が可能になる。
第四の発明は、上記の発明に関して、前記支持機構に固定されるｘ軸、ｙ軸、ｚ軸からなる３次元座標系を定義して支持機構固定座標系とし、前記回転体の姿勢角を、前記支持機構固定座標系において、ｘ軸の正方向の右回りの回転を示すロール角、ｚ軸の正方向の右回りの回転を示すヨー角、および、ｙ軸の正方向の右回りの回転を示すピッチ角、の順に３軸回転する３軸回転角として表現する、ように構成した。
第四の発明によれば、３次元空間内の回転体の姿勢を、ロール角、ヨー角、ピッチ角の３軸回りの回転角の組み合わせとして表現することができ、姿勢角の算出過程がより分かりやすく表現できるようになり、より効率的な姿勢角算出が可能となる。
【発明の効果】
【０００５】
本発明によれば、機構的に軸拘束のない回転中心が球面軸受け等のピボット要素の場合、取り付け角等の制限がなく従来手法に比して機構部品間の干渉等、機構設計上の制約を緩和して自由に配置可能なレーザと２つの位置検出素子(ＰＳＤ、ＣＣＤ素子)を用いて姿勢角を検出することができる。
【発明を実施するための最良の形態】
【０００６】
（実施例）
図１は本発明の姿勢角検出装置の構成（１）で、本発明による姿勢角検出装置の構成例を示している。
支持機構に固定される支持機構固定座標系ｘｙｚを、ｘ軸２０１、ｙ軸２０２（紙面に垂直に立っている）、ｚ軸２０３により構成する。
レーザ光源４、５は、ピボット要素である回転中心要素１を中心に回転する回転体に固定され、それが発するレーザ光線の光軸は回転中心要素１で交わるように配置される。ここでは、レーザ光線の光軸が所定の交差角７５°で交わっている例を示しているが、この所定の交差角はレーザ光源４、５の配置により自由に変えることができる。
２次元位置検出装置６、７は支持機構に固定され、レーザ光源４、５のそれぞれから発光されるレーザ光線をその位置情報検出面で受光し、その受光点の位置情報を検出して、ロール角（ｘ軸回り）、ヨー角（ｚ軸回り）、ピッチ角（ｙ軸回り）の３軸回りの角度で表現される回転体の姿勢角を算出する。ここで、ロール角は支持機構に固定された座標系のｘ軸２０１の正の向きを向いて右回りの回転角とし、ヨー角は支持機構に固定された座標系のｚ軸２０３の正の向きを向いて右回りの回転角とし、ピッチ角は支持機構に固定された座標系のｙ軸２０２の正の向きを向いて右回りの回転角とする。図１では、ｙ軸２０２は紙面に垂直に立っており、その垂直に立っている方向が正の向きとなる。なお、２次元位置検出素子としては、ＰＳＤまたはＣＣＤ(Charge Coupled Device：電荷結合素子)デバイスを利用することが可能である。
回転体と共に回転するレーザ光源４、５が発するレーザ光線の光軸は、回転中心要素１で交わり、二つのレーザ光線の光軸は、初期状態として、それぞれ、２次元位置検出装置６、７の位置検出面の中心に垂直に交わっている。ベクトル２、３は、それぞれ、回転中心要素１を始点としてレーザ光線の光軸に沿ってレーザ光源４、５に向かうベクトルであり、回転体の回転運動に連動して、図１に示す初期状態から移動する。ただし、ベクトルの始点と二つのベクトルが交わる角度は維持される。また回転中心要素１から２次元位置検出装置６、７の位置検出面の中心までの距離ｄも一定に維持される。
図１では、回転体に固定されるレーザ光源４、５が初期状態にある場合を示しており、２次元位置検出装置６、７およびレーザ光源４、５は３次元空間内のｘｚ平面上に配置されている。２次元位置検出装置６、７は、回転中心要素１から２次元位置検出装置６、７の位置検出面の中心に向かう二つのベクトル（図１の初期状態ではベクトル２、３と一致している）が、ｘｚ平面上で所定の交差角度（ここでは75[deg]の例を示している）で交わるように支持機構に固定して配置されている。ここで、記号[deg]は角度を示す単位で、円周を360に等分した場合の１単位の円周の中心角を示す（以下同様）。
【０００７】
レーザ光源４、５は回転体に固定されており、その光軸が回転中心要素１で所定の交差角度で交わった状態のまま、回転中心要素１を中心にして３次元の回転を行うが、回転体は、二つのレーザ光線の各光軸が２次元位置検出装置６または７の位置検出面に交差する範囲内で回転するものとする。
【０００８】
図２は本発明の姿勢角検出装置の構成（２）で、図１に示したレーザ光源４、５の構成例を示すもので、レーザ発光素子８から発せられたレーザ光１１が、集光レンズ９により集光点１０に集光される様子を示している。
上記の図１の構成では、わかり易く表現するために、初期状態として二つのレーザ光線の光軸が支持機構固定座標系におけるｘｚ座標面上にあり、しかも回転中心要素１から二つの２次元位置検出装置の位置検出面の中心に向かう二つのベクトル２、３は所定の交差角75[deg]で交差している場合を示したが、本発明はこれに限定されるものではない。以降の説明では、初期状態としては、二つのレーザ光線の光軸は、回転中心要素１で任意に選定した所定の交差角で交わった状態で３次元空間内の任意の位置にある場合を前提にした場合の回転体の姿勢角算出原理を示す。
図３は本発明の座標系定義の概念図で、２次元位置検出装置の位置検出面に定義される位置検出２次元座標系と、それを基に定義される位置検出３次元座標系AｘAｙAｚを示している。
軸１０１、軸１０２は、２次元位置検出装置６の位置検出面上に定義される位置検出２次元座標系の座標軸である。
また、Aｘ軸１０３は、回転中心要素１を原点とし、２次元位置検出装置６の位置検出面の中心に向かうベクトルとして定義され、該位置検出面に対して垂直に交差している。Aｙ 軸１０４、Aｚ軸１０５は、回転中心要素１を始点とし、それぞれ、軸１０１、軸１０２と同じ向きとなるベクトルとして定義する。上記のようにして定義されるAｘ軸１０３、Aｙ軸１０４、Aｚ 軸１０５から構成される座標系を位置検出３次元座標系AｘAｙAｚと定義する。
【０００９】
回転体が３軸回転することにより、レーザ光源４から発せられるレーザ光線の光軸、つまり、回転中心要素１から２次元位置検出装置６の位置検出面へ向かうベクトル２が変位し、その変位は、２次元位置検出装置６の位置検出面に定義される位置検出２次元座標系での受光位置の変位として検出される。
【００１０】
この受光位置の変位は、位置検出２次元座標系での受光位置の座標を、１０１軸上の値をｖ1ｙ、１０２軸上の値をｖ1ｚで表すと、位置検出２次元座標系では原点（０，０）から（ｖ1ｙ，ｖ1ｚ）への変位として検出され、これを位置検出３次元座標系AｘAｙAｚ上で見ると、回転中心要素１から２次元位置検出装置６の位置検出面の中心までの距離をｄとして、座標（ｄ，０，０）から座標（ｄ,ｖ1ｙ,ｖ1ｚ）への変位として表現できる。
【００１１】
位置検出３次元座標系AｘAｙAｚにおいて、原点（0,０,０）から上記の座標（ｄ,ｖ1ｙ,ｖ1ｚ）へのベクトル２を単位ベクトルに正規化した単位光線ベクトルsｖ1（図３のベクトル２と同じ向きで長さが１）を、
【００１２】
【数１】
2007225365.tif 000003
【００１３】
と定義する。ここで、|Sｖ1|＝1で、Sｖ1xは、ベクトル２を回転中心要素１から２次元位置検出装置６の位置検出面の中心までの距離ｄが単位ベクトルのAx成分になるように正規化したものであり、Sｖ1yおよびSｖ1zは、それぞれ、２次元位置検出装置6の位置検出面での位置検出量が単位ベクトルのAｙ成分およびAｚ成分になるように正規化したものである。
もう一つの２次元位置検出装置７についても上記と同様にして位置検出３次元座標系BｘBｙBｚ を定義し、座標系BｘBｙBｚにおいて、図１に示した回転中心要素１を通るレーザ光源 5から発するレーザ光線の光軸上、つまり、回転中心要素１から２次元位置検出装置７の位置検出面上の位置検出点に向かうベクトルを単位ベクトルに正規化した単位光線ベクトルsｖ2を、
【００１４】
【数２】
2007225365.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ｚ から支持機構固定座標系 ｘｙｚへの変換行列は、それぞれ、
【００１６】
【数３】
2007225365.tif 000005
【００１７】
となり、位置検出３次元座標系で表した単位光線ベクトルＳｖ1、 Sｖ2を支持機構固定座標系ｘｙｚで表した単位光線ベクトルｖ1、ｖ2は、
【００１８】
【数４】
2007225365.tif 000006
【００１９】
となる。これにより、位置検出３次元座標系AｘAｙAｚおよび位置検出３次元座標系
BｘBｙBｚ による座標軸成分から、支持機構固定座標系 ｘｙｚによる座標軸成分を求めることができる。
【００２０】
図４は本発明の座標系変換の概念図で、上記の回転中心要素１を始点としてレーザ光線の光軸に沿ってレーザ光源４、５に向かうベクトル２および３と、位置検出３次元座標系AｘAｙAｚ、位置検出３次元座標系BｘBｙBｚ 、および支持機構固定座標系 ｘｙｚとの関係の概要を示したものである。
【００２１】
支持機構固定座標系 ｘｙｚは、ｘ軸２０１、ｙ軸２０２、ｚ軸２０３により構成される。
【００２２】
２次元位置検出装置６に対しては、回転中心要素１を始点としてその位置検出面の中心に直交するベクトルをAｘ軸１０３とし、その位置検出面に定義される２次元座標系の座標軸１０１および１０２と平行な、回転中心要素１を始点とするベクトルをそれぞれAｙ軸１０４、および、Aｚ 軸１０５として、位置検出３次元座標系AｘAｙAｚを構成する。２次元位置検出装置７に対しても、同様にして、回転中心要素１を始点としてその位置検出面の中心に直交するベクトルをBｘ軸４０３とし、その位置検出面に定義される２次元座標系の座標軸４０１および４０２と平行な、回転中心要素１を始点とするベクトルをそれぞれBｙ軸４０４、および、Bｚ 軸４０５として、位置検出３次元座標系BｘBｙBｚを構成する。
【００２３】
図５は本発明の正規直交基底の構成方法（１）で、所定の交差角で交わる単位ベクトルが張るベクトル部分空間の正規直交基底を求める方法を示している。
つまり、上記の回転中心要素１を始点としてレーザ光線の光軸に沿って２次元位置検出装置６，７に向かうベクトル２および３を正規化した単位光線ベクトルＳｖ1  ２１、 Sｖ2 ２２が張る平面２０を考え、単位光線ベクトルＳｖ1 ２１、 Sｖ2 ２２がなすベクトル部分空間の正規直交基底Ｐ、Ｑを構成する。  
【００２４】
まず、正規直交基底の一方Ｐを、
【００２５】
【数５】
2007225365.tif 000007
【００２６】
として構成する。
【００２７】
次に、ベクトルＰに直交しベクトルSｖ2 ２２と同じ終点を有するベクトル２４と、ベクトルSｖ2 ２２と同じ始点を有し上記ベクトル２４の始点をその終点とするベクトル２３を考えると、ベクトルSｖ2   ２２はベクトル２３とベクトル２４をベクトル加算したものと考えることができる。
ここで、ベクトルSｖ2 及びベクトルＰは単位ベクトルであるから、ベクトル２３の長さは ベクトル内積 (Sv2・Ｐ)となり、ベクトル２３は、
【００２８】
【数６】
2007225365.tif 000008
【００２９】
と表せる。従って、ベクトル２４は、
【００３０】
【数７】
2007225365.tif 000009
【００３１】
となり、このベクトル２４を正規化して単位ベクトル化すると、
【００３２】
【数８】
2007225365.tif 000010
【００３３】
が得られる。
図６は、本発明の正規直交基底の構成方法（２）で、単位ベクトルSｖ1とSｖ2との間のなす角度θの定義を示している。
つまり、ベクトルSｖ1  から時計回り方向に測ったベクトルSｖ2までの角度をθとする。      
【００３４】
θを上記のように定義した場合に、ベクトルＰ＝Sｖ1  に直交する単位ベクトルＱを
【００３５】
【数９】
2007225365.tif 000011
【００３６】
と定義すると、図５に示すように、ベクトルＰ ２１、ベクトルＱ２５はベクトルSｖ1、ベクトルSｖ2  がなすベクトル部分空間の正規直交基底となる。
上記のようにして構成したベクトルＰ、Ｑを、前記特許文献１（特開2005-214867「姿勢角検出方法および姿勢角検出装置」）に開示されている互いに直交する単位光線ベクトルＰ、Ｑと見なすことにより、姿勢角を算出することができる。
まず、互いに直交する単位光線ベクトルＰ、Ｑの姿勢角を定義する。支持機構固定座標系ｘｙｚ系において、直交する２つの単位ベクトル[1 0 0]Tおよび[0 0 1]Tが、その直交関係を維持した状態で３軸回転して、直交する単位光線ベクトルＰ、Ｑ  になったとした場合のロール角（ｘ軸回りの回転角）α、ヨー角（ｚ軸回りの回転角）β、ピッチ角（ｙ軸回りの回転角）γの組を、直交する単位光線ベクトルＰ、Ｑの姿勢角と定義し、以降
”(α,β,γ)”と表記する。
このとき、ｘ 軸回りのロール角αの回転行列をＲx(α)、ｚ 軸回りのヨー角βの回転行列を Ｒz(β)、ｙ 軸回りのピッチ角γの回転行列をＲy(γ)と表記すると、３軸回転時の回転行列は次のように表される。
【００３７】
【数１０】
2007225365.tif 000012
【００３８】
上記の式（４）と、姿勢角の定義より直交する単位ベクトル[1 0 0]T、[0 0 1]Tが、それぞれ、ロール角α、ヨー角β、ピッチ角γの順に３軸回転して、直交する単位光線ベクトルＰ、Ｑになることから、
【００３９】
【数１１】
2007225365.tif 000013
【００４０】
が得られ、これを基にして、単位光線ベクトルＰ、Ｑに関して、支持機構固定座標系
ｘｙｚの座標軸成分と、姿勢角(α,β,γ)の成分についての次の関係式が得られる。
【００４１】
【数１２】
2007225365.tif 000014
【００４２】
上記の式（４）により、単位光線ベクトルＰ、Ｑの支持機構固定座標系ｘｙｚの座標軸成分から、単位光線ベクトルＰ、Ｑの姿勢角(α,β,γ)を求めることができる。
ここで、arctan2(Ｙ，Ｘ) はロボット工学等で用いられる関数で、tan-1（Ｙ／Ｘ）を拡張したものである。角度をラジアンで表示した場合、tan-1（Ｙ／Ｘ）の値域が
【００４３】
【数１３】
2007225365.tif 000015
【００４４】
であり、Ｘ＝０の場合に対応できないのに対し、arctan2(Ｙ，Ｘ)は、引数Ｙ、Ｘの符号から象限を特定し、
【００４５】
【数１４】
2007225365.tif 000016
【００４６】
の値域を持ち、Ｘ＝０ の場合にもＹ の符号に応じて、
【００４７】
【数１５】
2007225365.tif 000017
【００４８】
を出力する。
【００４９】
次に、回転中心要素１を始点として、支持機構に固定された２次元位置検出装置６，７の位置検出面の中心方向を指す所定の交差角で交わる単位ベクトル、つまり、初期状態の単位光線ベクトルSｖ10、Sv20が、回転体の３軸回転により上記と同じ所定の交差角で交わる単位光線ベクトルSv1、Sｖ2になった場合を考える。例えば、図１に示した所定の交差角７５．０°で交わるベクトル２、３を正規化したものが、それぞれ、初期状態の単位光線ベクトルSｖ10、Sｖ20に相当する。
この初期状態の単位光線ベクトルSｖ10、Sｖ20を基に、上記の方法で単位光線ベクトルSｖ10、Sｖ20が張るベクトル部分空間の正規直交基底Ｐ0、Ｑ0を求める。
そして、この求めた正規直交基底Ｐ0、Ｑ0、つまり、初期状態の直交する単位光線ベクトルＰ0、Ｑ0の姿勢角を（α0，β0，γ0）とする。同様に、所定の交差角で交わる単位光線ベクトルSv1、Sｖ2について求めた正規直交基底Ｐ、Ｑ、つまり、直交する単位光線ベクトルＰ、Ｑの姿勢角を（α，β，γ）とする。そして、回転体が３軸回転した際の姿勢角の各成分、つまり、ロール角、ヨー角、ピッチ角の変化量を、それぞれ、Δα、Δβ、Δγと表記し、この姿勢角変化分の回転を行う回転行列をΔＲ(Δα,Δβ,Δγ)と表記すると、
【００５０】
【数１６】
2007225365.tif 000018
【００５１】
であることから、
【００５２】
【数１７】
2007225365.tif 000019
【００５３】
となり、さらに、回転行列の特性として、その逆行列は転置行列に等しいことから、
【００５４】
【数１８】
2007225365.tif 000020
【００５５】
となり、回転体の互いに直交する単位光線ベクトルの初期姿勢角（α0，β0，γ0）が分かれば、上記式（７）で求めた回転後の回転体の互いに直交する単位光線ベクトルの姿勢角（α，β，γ）を基に、回転行列ΔＲ(Δα,Δβ,Δγ)を求めることができる。
次に、上記で求めた回転行列ΔＲ(Δα,Δβ,Δγ)を基に、ロール角Δα、ヨー角Δβ、ピッチ角Δγを算出するためには、直交する単位ベクトル[1 0 0]T、[0 0 1]Tが、それぞれ、ロール角Δα、ヨー角Δβ、ピッチ角Δγだけ３軸回転してできる直交する単位ベクトル
ｅ1、ｅ2の姿勢角（Δα，Δβ，Δγ）を、式(６）を用いて求めればよい。
つまり、まず、支持機構固定座標系ｘｙｚ上でのｘ軸方向の単位ベクトルｅx=[1 0 0]T、および、ｚ軸方向の単位ベクトルｅz=[0 0 1]T  を回転行列ΔＲ(Δα,Δβ,Δγ)で変換して、
【００５６】
【数１９】
2007225365.tif 000021
【００５７】
を得、次に、この直交する単位ベクトルｅ1=[ｅ1x,ｅ1y,ｅ1z]Tおよびｅ２=[ｅ2x,ｅ2y,ｅ2z]Tと、その姿勢角（Δα，Δβ，Δγ）の成分に対して式(４）を適用すると、
【００５８】
【数２０】
2007225365.tif 000022
【００５９】
となり、これを基にして最終的な３軸姿勢角の変位量Δα、Δβ、Δγを算出できる。
【００６０】
以上が、本発明による姿勢角算出の原理である。
【００６１】
次に、上記の式（１）〜（８）を用いて、２次元位置検出装置の位置検出面での受光位置の検出データから回転体の姿勢角の変化量を求める場合の、算出手順の実施例を示す。      
【００６２】
以下の実施例は、上記の姿勢角算出原理を用いた算出手順の妥当性を示すことが目的であり、算出した結果は計算過程で丸め誤差が混入していることから若干の誤差が含まれているが、本発明の本質には影響しない。
図７は本発明の実施例を示している。
単一のレーザ光源３０１、ハーフ・ミラー３０２、および、ミラー３０３〜３０７は、回転中心要素１(球面軸受け)を中心に回転する回転体に固定され、ＰＳＤ３１０、３１１は支持機構３１２に固定されている。レーザ光源３０１から発光されるレーザ光線はハーフ・ミラー３０２及びミラー３０３〜３０７によって２つのレーザ光線３０８、３０９に分割され、ＰＳＤ３１０、３１１で受光される。ここで、レーザ光線３２１および３２２の光軸は回転中心要素１において所定の交差角で交差している。図７では、この所定の交差角が１２０°の実施例を示しているが、この交差角は任意に設定可能である。
支持機構３１２については、ｘ軸２０１、ｙ軸２０２（紙面に垂直に立っている）、ｚ軸２０３の３軸から成る支持機構固定座標系ｘｙｚが定義され、ＰＳＤ３１０については位置検出３次元座標系AｘAｙAｚが定義され、ＰＳＤ３１１については位置検出３次元座標系
BｘBｙBｚが定義される。
レーザ光線３２１、３２２は回転体と連動して回転するが、図７では、その初期状態が示されている。つまり、レーザ光線３２１を示すベクトルは、回転中心要素１からＰＳＤ３１０の中心方向に向かうベクトルを初期ベクトルとし、レーザ光線３２２を示すベクトルは、回転中心要素１からＰＳＤ３１１の中心に向かうベクトルを初期ベクトルとし、回転体に固定された状態で、互いに所定の交差角で交わった状態を維持したまま回転する。
図７に示す初期状態で回転中心要素１からＰＳＤ３１０、３１１の中心に向かう単位ベクトルを、それぞれSｖ10、Sｖ20  で表すと、それらは120[deg]の交差角で交差している。そして、前記図５の説明で示した方法により、このベクトルｖ10、ｖ20  が張る部分ベクトル空間の正規直交基底Ｐ0、Ｑ0を求める。
まず式（２）、式（３）においてθ=120[deg]であることから、
【００６３】
【数２１】
2007225365.tif 000023
【００６４】
【数２２】
2007225365.tif 000024
【００６５】
となる。ここで、
【００６６】
【数２３】
2007225365.tif 000025
【００６７】
であることから、
【００６８】
【数２４】
2007225365.tif 000026
【００６９】
が得られる。
Ｐ0、Ｑ0は互いに直交しており、支持機構固定座標系ｘｙｚを基準としたロール角（ｘ軸回り）、ヨー角（ｚ軸回り）、ピッチ角（ｙ軸回り）で示される姿勢角を“（ロール角,ヨー角,ピッチ角）”形式で表現すると図７から、ベクトルＰ0、Ｑ0は、支持機構固定座標系ｘｙｚにおいて、互いに直交する単位ベクトル[1 0 0]Tおよび[0 0 1]Tを、ｙ軸回り（ｙ軸の正方向に向かって右回り）に-30[deg]回転させたものと同じであり、姿勢角の定義から、互いに直交する単位ベクトルＰ0、Ｑ0の姿勢角は（0[deg],0[deg],-30[deg]）となる。
また、図７より、ＰＤＳ３１０に定義される位置検出３次元座標系AｘAｙAｚは、支持機構固定座標系ｘｙｚと一致していた座標系がｙ軸回りに-30[deg]度回転してできたものとして定義され、ＰＤＳ３１１に定義される位置検出３次元座標系BｘBｙBｚは、支持機構固定座標系ｘｙｚと一致していた座標系がｙ軸回りに-150[deg]回転してできたものとして定義される。このとき、位置検出３次元座標系AｘAｙAｚおよび位置検出３次元座標系BｘBｙBｚにそれぞれ対応する座標変換行列AＴ、BＴは次のようになる。
【００７０】
【数２５】
2007225365.tif 000027
【００７１】
【数２６】
2007225365.tif 000028
【００７２】
ＰＳＤ３１０の位置検出面で測定されるレーザ光線３２１の位置検出値は、前記位置検出３次元座標系AｘAｙAｚにおけるｙ軸方向およびｚ軸方向の変位として検出され、ＰＳＤ３１１の位置検出面で測定されるレーザ光線３２２の位置検出値は、前記位置検出３次元座標系BｘBｙBｚにおけるｙ軸方向およびｚ軸方向の変位として検出される。また、位置検出３次元座標系AｘAｙAｚおよび位置検出３次元座標系BｘBｙBｚのいずれにおいても、ｘ軸方向の位置はｄ＝40[mm]に固定されている。
上記の実施例の条件で、回転体に定義される直交する単位光線ベクトルが初期姿勢角(0[deg],0[deg],-30[deg]）から姿勢角(α,β,γ)へ回転した際の変位姿勢角
(Δα,Δβ,Δγ)を本発明による姿勢角算出原理を用いて算出する際のシミュレーション実施例を、以下の（１）および（２）に示す。
（１）  変位姿勢角算出シミュレーション１
回転体がピッチ軸（ｙ軸２０２）回りに回転した結果のＰＳＤの位置検出面における位置検出値が次の通りであったとする。
・ＰＳＤ３１０：(0.0[mm], +3.4995[mm])
・ＰＳＤ３１１：(0.0[mm]，+3.4995[mm])
回転中心要素１(球面軸受け)を中心に位置検出３次元座標系AｘAｙAｚを基にしたＰＳＤ３１０の位置検出面における位置検出点のベクトルは(+40[mm]0.00[mm] +3.4995[mm])Tとなり、これを単位ベクトルに正規化した単位光線ベクトルSｖ1は、
【００７３】
【数２７】
2007225365.tif 000029
【００７４】
となる。同様にして、ＰＳＤ３１１についての位置検出３次元座標系BｘBｙBｚにおける単位光線ベクトルSｖ2は、
【００７５】
【数２８】
2007225365.tif 000030
【００７６】
となる。
そして、式（１１）（１２）より、位置検出３次元座標系AｘAｙAｚ、および、位置検出３次元座標系BｘBｙBｚ から、支持機構固定座標系 ｘｙｚへの変換行列AＴおよびBＴは、
【００７７】
【数２９】
2007225365.tif 000031
【００７８】
【数３０】
2007225365.tif 000032
【００７９】
であることから、式（１)より、支持機構固定座標系ｘｙｚにおける単位光線ベクトル
Sｖ1、及び単位光線ベクトルSｖ2をそれぞれｖ1、ｖ2とすると
【００８０】
【数３１】
2007225365.tif 000033
【００８１】
【数３２】
2007225365.tif 000034
【００８２】
となる。
【００８３】
ここで、ベクトルｖ1、ｖ2 により張られる部分ベクトル空間の正規直交基底Ｐ、Ｑを式（９）（１０）を用いて求めると、
【００８４】
【数３３】
2007225365.tif 000035
【００８５】
【数３４】
2007225365.tif 000036
【００８６】
上記の結果を式（６）に適用すると、直交する単位光線ベクトルＰおよびＱの姿勢角（α,β,γ）は、
【００８７】
【数３５】
2007225365.tif 000037
【００８８】
となり、これから
【００８９】
【数３６】
2007225365.tif 000038
【００９０】
が得られる。
上記の結果より、回転体に固定されて回転する直交する単位光線ベクトルＰおよびＱの３軸回りの回転行列Ｒx(α)、Ｒz(β)、Ｒy(γ)は、
【００９１】
【数３７】
2007225365.tif 000039
【００９２】
となる。
次に、互いに直交する単位光線ベクトルＰおよびＱの初期状態の姿勢角
（α0,β0,γ0）は（0[deg],0[deg],-30[deg]）であることから、
【００９３】
【数３８】
2007225365.tif 000040
【００９４】
が得られ、初期姿勢角（α0,β0,γ0）の３軸回りの回転行列Ｒx(α0)、Ｒz(β0)、Ｒy(γ0)は、
【００９５】
【数３９】
2007225365.tif 000041
【００９６】
となる。
以上の結果を、式（７）に適用することにより、直交する単位光線ベクトルＰおよびＱの姿勢角変位分の回転を行う回転行列ΔＲ(Δα,Δβ,Δγ)は、
【００９７】
【数４０】
2007225365.tif 000042
【００９８】
となる。一方、直交する単位ベクトル[1 0 0]Tおよび[0 0 1]Tが、姿勢角(Δα,Δβ,Δγ)だけ回転して直交ベクトルｅ1およびｅ2になったとすると、
【００９９】
【数４１】
2007225365.tif 000043
【０１００】
【数４２】
2007225365.tif 000044
【０１０１】
となり、これを式（８）に適用すると、
【０１０２】
【数４３】
2007225365.tif 000045
【０１０３】
が得られる。
従って、直交する単位光線ベクトルＰおよびＱの変位姿勢角(Δα,Δβ,Δγ)を少数点以下２桁で求めると、(0.0[deg],0.0[deg], -5.00[deg])となる。
（２）変位姿勢角算出シミュレーション２
図７に示す本発明の実施例において、回転体が初期状態から回転した結果のＰＳＤにおける位置検出値が次の通りであったとする。
ＰＳＤ３１０：(0.7689[mm]， -2.1238[mm])  
ＰＳＤ３１１：(-2.8696[mm]， -2.1238[mm])
回転中心要素１(球面軸受け)を中心に位置検出３次元座標系AｘAｙAｚを基にしたＰＳＤ３１０における検出値が示すベクトルは[+40[mm] 0.7689[mm]，-2.1238[mm]]Tとなり、これを単位ベクトルに正規化した単位光線ベクトルSｖ1は、
【０１０４】
【数４４】
2007225365.tif 000046
【０１０５】
となる。同様にして、ＰＳＤ３１１についての位置検出３次元座標系BｘBｙBｚの単位光線ベクトルSｖ2は、
【０１０６】
【数４５】
2007225365.tif 000047
【０１０７】
となる。
また、位置検出３次元座標系AｘAｙAｚ、および、位置検出３次元座標系BｘBｙBｚ 系から、支持機構固定座標系 ｘｙｚ 系への変換行列AＴおよびBＴは、上記の変位姿勢角算出シミュレーション１と同様にして、
【０１０８】
【数４６】
2007225365.tif 000048
【０１０９】
【数４７】
2007225365.tif 000049
【０１１０】
となり、前記式（１)より、支持機構固定座標系ｘｙｚにおける単位光線ベクトルSｖ1、及び単位光線ベクトルSｖ2をそれぞれｖ1、ｖ2とすると、
【０１１１】
【数４８】
2007225365.tif 000050
【０１１２】
【数４９】
2007225365.tif 000051
【０１１３】
となる。
【０１１４】
ここで、ベクトルｖ1、ｖ2 により張られる部分ベクトル空間の正規直交基底Ｐ、Ｑを式（９）（１０）を用いて求めると、
【０１１５】
【数５０】
2007225365.tif 000052
【０１１６】
【数５１】
2007225365.tif 000053
【０１１７】
上記の結果を式（６）に適用すると、直交する単位光線ベクトルＰおよびＱの姿勢角（α,β,γ）は、
【０１１８】
【数５２】
2007225365.tif 000054
【０１１９】
となり、これから
【０１２０】
【数５３】
2007225365.tif 000055
【０１２１】
が得られる。
上記の結果より、回転体３０５と連動して回転する直交する単位光線ベクトルＰおよびＱの３軸周りの回転行列Ｒx(α)、Ｒz(β)、Ｒy(γ)は、
【０１２２】
【数５４】
2007225365.tif 000056
【０１２３】
となる。次に、直交する単位光線ベクトルＰおよびＱの初期状態の姿勢角
（α0,β0,γ0）は、(0[deg],0[deg],-30[deg])であることから、
【０１２４】
【数５５】
2007225365.tif 000057
【０１２５】
が得られ、初期姿勢角の３軸回りの回転行列Ｒx(α0)、Ｒz(β0)、Ｒy(γ0)は、
【０１２６】
【数５６】
2007225365.tif 000058
【０１２７】
となる。
以上の結果を式（７）に適用することにより、直交する単位光線ベクトルＰおよびＱの姿勢角変位分の回転を行う回転行列ΔＲ(Δα,Δβ,Δγ)は、
【０１２８】
【数５７】
2007225365.tif 000059
【０１２９】
となる。一方、直交する単位ベクトル[1 0 0]Tおよび[0 0 1]Tが、姿勢角(Δα,Δβ,Δγ)だけ回転して直交ベクトルｅ1およびｅ2になったとすると、
【０１３０】
【数５８】
2007225365.tif 000060
【０１３１】
【数５９】
2007225365.tif 000061
【０１３２】
となり、これを式（８）に適用すると、
【０１３３】
【数６０】
2007225365.tif 000062
【０１３４】
となる。
従って、回転体と連動して回転する直交する単位光線ベクトルＰおよびＱが、その初期姿勢角（0[deg],0[deg],-30[deg]）の姿勢から回転した場合の初期姿勢角からの変位姿勢角（Δα,Δβ,Δγ）を小数点以下２桁まで求めると、
(+3.00[deg],3.00[deg],3.00[deg])となる。
以上に詳述した本発明の実施の形態では、レーザ光線とそれを受光して位置を検出する２次元位置検出装置を実施例として示したが、直進性のある光線とそれを受光して位置を検出できる２次元位置検出装置であれば、レーザ光線に限定されずに本発明は適用可能である。
また、説明をわかり易くするために、変位姿勢角算出のシミュレーション例では、支持機構固定座標系ｘｙｚのｘｚ平面に二つの２次元位置検出装置の位置検出面の中心と回転中心要素が含まれる場合で説明したが、本発明の姿勢角算出の原理はこれに限定されずに適用できる。つまり、回転体の初期姿勢を示す単位光線ベクトルSｖ10、Sｖ20は回転中心要素において所定の交差角で交わりさえすれば、３次元空間内の任意位置に設定することができる。
上記の実施例では、２次元位置検出装置として、ＰＳＤを用いた例を示したが、これに限定されることはなく、ＣＣＤ素子などの他の手段でも可能である。
また、上記の実施例では、一つのレーザ光源をミラーを介して二つのレーザ光線とする構成を示したが、本発明はこれに限定されない。
以上詳細に述べた本発明の実施の態様は、以下の付記の通りである。
（付記１）回転体がピボット要素を介して支持機構に取り付けられ、
レーザ光源が、前記レーザ光源を源とする二つのレーザ光線の光軸が前記回転体の回転中心要素で所定の交差角で交わるように前記回転体に固定され、
前記各レーザ光線に対応して前記レーザ光線を受光して受光位置を検出する２次元位置検出装置を設け、
前記２次元位置検出装置は、前記２次元位置検出装置の位置検出面の中心で受光する前記レーザ光線の光軸が前記位置検出面に対して直交するように前記支持機構に固定され、
前記２次元位置検出装置で検出される受光位置情報を基に、前記回転体が前記回転中心要素を中心として回転する際の姿勢角を算出する、
ことを特徴とする姿勢角検出方法。
（付記２）付記１に記載の姿勢角検出方法において、
前記２次元位置検出装置の位置検出面の中心を原点とする２次元座標系を前記位置検出面上に定義して位置検出２次元座標系とし、
前記回転体の回転中心要素を始点として前記位置検出２次元座標系の原点へ向かうベクトルをｘ軸とし、回転中心要素を始点として前記位置検出２次元座標系で定義される２軸に平行となるベクトルをそれぞれｙ軸およびｚ軸とする３次元座標系を定義して位置検出３次元座標系とし、
前記位置検出２次元座標系で検出される前記レーザ光線の受光位置情報を基に、前記位置検出３次元座標系おける前記回転体の姿勢を示す単位光線ベクトル情報を生成し、
前記各２次元位置検出装置に対応して生成される二つの前記単位光線ベクトル情報を基に前記回転体の姿勢角を算出する、
ことを特徴とする姿勢角検出方法。
（付記３）付記２に記載の姿勢角検出方法において、
前記位置検出３次元座標系において、前記回転体の回転中心要素から前記位置検出２次元座標の原点までの距離をｘとし、前記位置検出２次元座標系において前記レーザ光線を受光した際の受光位置を示す二つの座標値をそれぞれｙおよびｚとし、
前記単位光線ベクトル情報は、前記位置検出３次元座標系の原点を始点とし３次元座標（ｘ，ｙ，ｚ）を終点とするベクトルを単位ベクトルへ正規化したベクトル情報とする、
ことを特徴とする姿勢角検出方法。
（付記４）付記１ないし付記３のいずれかに記載の姿勢角検出方法において、
前記支持機構に固定されるｘ軸、ｙ軸、ｚ軸からなる３次元座標系を定義して支持機構固定座標系とし、
前記回転体の姿勢角を、前記支持機構固定座標系において、ｘ軸の正方向の右回りの回転を示すロール角、ｚ軸の正方向の右回りの回転を示すヨー角、および、ｙ軸の正方向の右回りの回転を示すピッチ角、の順に３軸回転する３軸回転角として表現する、
ことを特徴とする姿勢角検出方法。
（付記５）付記１に記載の姿勢角検出方法において、
前記回転体に固定されるレーザ光源は二個のレーザ光源から成り、
前記二個のレーザ光源が発する二個のレーザ光線は、その光軸が前記回転体の回転中心要素で所定の交差角で交わるように配置される、
ことを特徴とする姿勢角検出方法。
（付記６）付記１に記載の姿勢角検出方法において、
前記回転体に固定されるレーザ光源は一個のレーザ光源から成り、
前記一個のレーザ光源が発するレーザ光線は、ハーフミラーおよびミラーを介して二個のレーザ光線に分岐され、前記分岐された二個のレーザ光線の光軸が前記回転体の回転中心要素で所定の交差角で交わるように配置される、
ことを特徴とする姿勢角検出方法。
（付記７）支持機構にピボット要素を介して取り付けられる回転体と、
前記回転体に固定されるレーザ光源と、
前記レーザ光源を源とする二つのレーザ光線の光軸を前記回転体の回転中心要素で所定の交差角で交わるように配置する手段と、
前記各レーザ光線に対応して設けられ、前記レーザ光線を受光して受光位置情報を検出する２次元位置検出装置と、
前記２次元位置検出装置を、前記２次元位置検出装置が位置検出面の中心で受光する前記レーザ光線の光軸が前記位置検出面に対して直交するように前記支持機構に固定する手段と、
前記２次元位置検出装置で検出される受光位置情報を基に、前記回転体が前記回転中心要素を中心として回転する際の姿勢角を算出する手段を備える、
ことを特徴とする姿勢角検出装置。
（付記８）付記７に記載の姿勢角検出装置において、
前記２次元位置検出装置の位置検出面の中心を原点として前記位置検出面に定義される２次元座標系である位置検出２次元座標系と、
前記回転体の回転中心要素を始点として前記位置検出２次元座標系の原点へ向かうベクトルをｘ軸とし、回転中心要素を始点として前記位置検出２次元座標系で定義される２軸に平行となるベクトルをそれぞれｙ軸およびｚ軸として定義される３次元座標系である位置検出３次元座標系と、
前記位置検出３次元座標系で検出される前記レーザ光線の受光位置情報を基に、前記位置検出３次元座標系における前記回転体の姿勢を示す単位光線ベクトル情報を生成する手段と、
前記各２次元位置検出装置に対応して生成される二つの前記単位光線ベクトル情報を基に前記回転体の姿勢角を算出する手段を備える、
ことを特徴とする姿勢角検出装置。
（付記９）付記８に記載の姿勢角検出装置において、
前記位置検出３次元座標系において、前記回転体の回転中心要素から前記位置検出２次元座標の原点までの距離をｘとし、前記位置検出２次元座標系において前記レーザ光線を受光した際の受光位置を示す二つの座標値をそれぞれｙおよびｚとし、前記位置検出３次元座標系の原点を始点とし３次元座標（ｘ，ｙ，ｚ）を終点とするベクトルを単位ベクトルへ正規化したベクトル情報を前記単位光線ベクトル情報とする手段を備える、
ことを特徴とする姿勢角検出装置。
（付記１０）付記７ないし付記９のいずれかに記載の姿勢角検出装置において、
前記支持機構に固定して定義されるｘ軸、ｙ軸、ｚ軸からなる３次元座標系である支持機構固定座標系と、
前記回転体の姿勢角を、前記支持機構固定座標系において、ｘ軸の正方向の右回りの回転を示すロール角、ｚ軸の正方向の右回りの回転を示すヨー角、および、ｙ軸の正方向の右回りの回転を示すピッチ角、の順に３軸回転する３軸回転角で表現する手段を備える、
ことを特徴とする姿勢角検出装置。
（付記１１）付記７に記載の姿勢角検出装置において、
前記回転体に固定されるレーザ光源は二個のレーザ光源から成り、
前記二個のレーザ光源が発する二個のレーザ光線は、その光軸が前記回転体の回転中心要素で所定の交差角で交わるように配置される、
ことを特徴とする姿勢角検出装置。
（付記１２）付記７に記載の姿勢角検出装置において、
前記回転体に固定されるレーザ光源は一個のレーザ光源から成り、
前記一個のレーザ光源が発するレーザ光線は、ハーフミラーおよびミラーを介して二個のレーザ光線に分岐され、前記分岐された二個のレーザ光線の光軸が前記回転体の回転中心要素で所定の交差角で交わるように配置される、
ことを特徴とする姿勢角検出装置。
（付記１３）付記７に記載の姿勢角検出装置において、
前記２次元位置検出装置は、半導体位置検出装置（ＰＳＤ）で構成される、
ことを特徴とする姿勢角検出装置。
（付記１４）付記７に記載の姿勢角検出方法において、
前記３次元位置検出装置は、電荷結合素子（ＣＣＤ）で構成される、
ことを特徴とする姿勢角検出装置。
【図面の簡単な説明】
【０１３５】
【図１】本発明の姿勢角検出装置の構成（１）
【図２】本発明の姿勢角検出装置の構成（２）
【図３】本発明の座標系定義の概念図
【図４】本発明の座標系変換の概念図
【図５】本発明の正規直交基底の構成方法（１）
【図６】本発明の正規直交基底の構成方法（２）
【図７】本発明の実施例
【符号の説明】
【０１３６】
１    回転中心要素（ピボット要素）
２    回転中心要素１から２次元位置検出装置６の位置検出点へ向かうべクトル
３    回転中心要素１から２次元位置検出装置７の位置検出点へ向かうべクトル
４、５    レーザ光源
６、７    ２次元位置検出装置
８      レーザ発光素子
９      集光用レンズ
１０    集光点  
１１    レーザ光
２０    ２次元ベクトル空間
２１、２２、２３、２４、２５  ベクトル
１０１、１０２    ２次元位置検出装置６の位置検出面上の座標軸
１０３    位置検出３次元座標系AｘAｙAｚのAｘ軸
１０４    位置検出３次元座標系AｘAｙAｚのAｙ軸
１０５    位置検出３次元座標系AｘAｙAｚのAｚ軸
２０１    支持機構固定座標系ｘｙｚのｘ軸
２０２    支持機構固定座標系ｘｙｚのｙ軸
２０３    支持機構固定座標系ｘｙｚのｚ軸
３０１    レーザ光源
３０２    ハーフミラー
３０３、３０４、３０５、３０６、３０７    ミラー
３１０、３１１    ＰＳＤ
３１２    支持機構
３２１、３２２    レーザ光線    
４０１、４０２    ２次元位置検出装置７の位置検出面上の座標軸
４０３    位置検出３次元座標系BｘBｙBｚのBｘ軸
４０４    位置検出３次元座標系BｘBｙBｚのBｙ軸
４０５    位置検出３次元座標系BｘBｙBｚのBｚ軸
【図１】
2007225365.tif 000063
【図２】
2007225365.tif 000064
【図３】
2007225365.tif 000065
【図４】
2007225365.tif 000066
【図５】
2007225365.tif 000067
【図６】
2007225365.tif 000068
【図７】
2007225365.tif 000069