JP2005214867A - 姿勢角検出方法および姿勢角検出装置 - Google Patents

Abstract

【課題】従来、回転体の姿勢角は機構的に拘束された軸となるジンバル軸回りの角度を検出することにより行っている。しかしながら、回転中心要素が球面軸受け等のピボット要素の場合は、ジンバル軸に相当する機構的に拘束された軸が存在しないため、従来の方法をそのまま適用できない。これに対応するため、ＬＥＤ、ＰＳＤなどの位置検出装置を回転の自由度の数だけ増設する方法も考えられるが、装置量が増大しスペース効率およびコストの面で好ましくない。
【解決手段】回転体に連動して回転する二つの直交するレーザ光線と、レーザ光線を受光する機構的に拘束された二つの２次元位置検出装置を用い、二つの２次元位置検出装置で検出した受光位置情報を基に回転体の姿勢角を算出することにより、回転中心要素が球面軸受け等のピボット要素の場合であっても回転体の姿勢角を効率的に検出できる姿勢角検出方法および検出装置を提供する。
【選択図】図１

Description

本発明は、回転体の姿勢角検出方法および姿勢角検出装置に係り、特に、空間安定化機構等のサーボ系においてロール、ヨー、ピッチの３軸回りの姿勢角を検出する際に、ロール、ヨー、ピッチの各軸が機構的拘束のないピボット要素等を回転中心要素とする回転体の姿勢角検出方法および姿勢角検出装置に関する。

空間安定化機構のペイロード等の回転体は、ロール（ｘ軸回り）、ヨー（ｚ軸回り）、ピッチ（ｙ軸回り）の回転動作は機構的に拘束されて独立したそれぞれの軸で行われ、各軸に取り付けられたエンコーダ、レゾルバ等の角度検出器によって姿勢角を検出する方法が一般的に用いられている。しかしながら、小型軽量化、スペース制約等のためには、ロール、ヨー、ピッチそれぞれ独立した軸を設けるのではなく、回転中心要素として球面軸受け等のピボット要素を用いた構成とするのが有効である。この場合、ロール、ヨー、ピッチ各軸が機構的に存在しないため、従来の機構的に拘束された軸に取り付けられた角度検出器では姿勢角の検出は不可能であった。例えば、特許文献１には、ＰＳＤ(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Ｐ（図３のベクトル２に相当）を、

と定義する。ここで、|^SＰ|＝1で、^SＰ_xは、回転中心要素１から２次元位置検出装置６の位置検出面の中心までの距離ｄを単位ベクトルの^Aｘ成分になるように正規化したものであり、^SＰ_yおよび^SＰ_zは、それぞれ、２次元位置検出装置6の位置検出面での位置検出量を単位ベクトルの^Aｙ成分および^Aｚ成分になるように正規化したものである。

もう一つの２次元位置検出装置７についても同様にして位置検出３次元座標系^Bｘ^Bｙ^Bｚ を定義し、座標系^Bｘ^Bｙ^Bｚにおいて、図１に示した回転中心要素１を通るレーザ光源 5から発するレーザ光線の光軸上、つまり、回転中心要素１から２次元位置検出装置７の位置検出面に向かうベクトルを単位ベクトルに正規化した単位光線ベクトル^sＱを、

と定義する。

次に、位置検出３次元座標系^Aｘ^Aｙ^Aｚの^Aｘ軸、^Aｙ軸、^Aｚ 軸の支持機構固定座標系ｘｙｚにおける単位ベクトルを、それぞれ、^Aｅ_x、^Aｅ_y 、^Aｅ_z とし、位置検出３次元座標系
^Bｘ^Bｙ^Bｚ の^Bx軸、^By軸、^Bz軸の支持機構固定座標系ｘｙｚにおける単位ベクトルを、それぞれ、^Be_x、^Be_y、^Bｅ_z とすると、位置検出３次元座標系^Aｘ^Aｙ^Aｚおよび位置検出３次元座標系^Bｘ^Bｙ^Bｚ から、支持機構固定座標系 ｘｙｚへの変換行列は、それぞれ、

となり、位置検出３次元座標系で表した単位光線ベクトル^SＰ、 ^SＱを支持機構固定座標系ｘｙｚで表した単位光線ベクトルＰ、Ｑは、

となり、これにより、位置検出３次元座標系^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(γ)と表記すると、３軸回転時の回転行列は次のように表される。

上記の式（２）と、姿勢角の定義より直交する単位ベクトル[1 0 0]^T、[0 0 1]^T が、それぞれ、ロール角α、ヨー角β、ピッチ角γの順に３軸回転して、直交する単位 光線ベクトルＰ、Ｑになることから、

が得られ、これを基にして、単位光線ベクトルＰ、Ｑに関して、支持機構固定座標系
ｘｙｚの座標軸成分と、姿勢角(α,β,γ)の成分についての次の関係式が得られる 。

上記の式（４）により、単位光線ベクトルＰ、Ｑの支持機構固定座標系ｘｙｚの座標軸成分から、単位光線ベクトルＰ、Ｑの姿勢角(α,β,γ)を求めることができる。

ここで、arctan2(Ｙ，Ｘ) はロボット工学等で用いられる関数で、tan^-1（Ｙ／Ｘ）を拡張したものである。角度をラジアンで表示した場合、tan^-1（Ｙ／Ｘ）の値域が

であり、Ｘ＝０の場合に対応できないのに対し、arctan2(Ｙ，Ｘ)は、引数Ｙ、Ｘの符号から象限を特定し、

の値域を持ち、Ｘ＝０ の場合にもＹ の符号に応じて、

を出力する。

次に、回転中心要素１を始点として、支持機構に固定された２次元位置検出装置６，７の位置検出面の中心方向を指す互いに直交する単位ベクトル、つまり、初期状態の単位光線ベクトルＰ₀、Ｑ₀が、回転体の３軸回転により直交する単位光線ベクトルＰ、Ｑになった場合を考える。例えば、図１に示した互いに直交するベクトル２、３が、それぞれ、初期状態の単位光線ベクトルＰ0、Ｑ₀に相当する。

初期状態の直交する単位光線ベクトルＰ0、Ｑ₀の姿勢角を（α₀，β₀，γ₀）とし、直交する単位光線ベクトルＰ、Ｑの姿勢角を（α，β，γ）とする。そして、回転体が３軸回転した際の姿勢角の各成分、つまり、ロール角、ヨー角、ピッチ角の変化量を、それぞれ、Δα、Δβ、Δγと表記し、この姿勢角変化分の回転を行う回転行列をΔＲ(Δα,Δβ,Δγ)と表記すると、

であることから、

となり、さらに、回転行列の特性として、その逆行列は転置行列に等しいことから、

となり、回転体の互いに直交する単位光線ベクトルの初期姿勢角（α₀，β₀，γ₀）が分かれば、上記式（４）で求めた回転後の回転体の互いに直交する単位光線ベクトルの姿勢角（α，β，γ）を基に、回転行列ΔＲ(Δα,Δβ,Δγ)を求めることができる。

次に、上記で求めた回転行列ΔＲ(Δα,Δβ,Δγ)を基に、ロール角Δα、ヨー角Δβ、ピッチ角Δγを算出するためには、直交する単位ベクトル[1 0 0]^T、[0 0 1]^Tが、それぞれ、ロール角Δα、ヨー角Δβ、ピッチ角Δγだけ３軸回転してできる直交する単位ベクトル
ｅ₁、ｅ₂の姿勢角（Δα，Δβ，Δγ）を、式(４）を用いて求めればよい。

つまり、まず、支持機構固定座標系ｘｙｚ上でのｘ軸方向の単位ベクトルｅ_x=[1 0 0]^T、および、ｚ軸方向の単位ベクトルｅ_z=[0 0 1]^T を回転行列ΔＲ(Δα,Δβ,Δγ)で変換して、

を得、次に、この直交する単位ベクトルｅ₁=[ｅ_1x,ｅ_1y,ｅ_1z]^Tおよびｅ₂=[ｅ_2x,ｅ_2y,ｅ_2z]^Tと、その姿勢角（Δα，Δβ，Δγ）の成分に対して式(４）を適用すると、

となり、これを基にして最終的な３軸姿勢角の変位量Δα、Δβ、Δγを算出できる。

以上が、本発明による姿勢角算出の原理である。

次に、上記の式（１）〜（６）を用いて、２次元位置検出装置の位置検出面での受光位置の検出データから回転体の姿勢角の変化量を求める場合の、算出手順の実施例を示す。

以下の実施例は、上記の姿勢角算出原理を用いた算出手順の過程を示すことが目的であり、算出した結果は計算過程で丸め誤差が混入していることから若干の誤差が含まれているが、本発明の本質には影響しない。

図５は本発明の実施例（１）の全体図であり、図６は、図５に示す本発明の実施例（１）の中の主要部分を拡大表示したものである。

レーザ光源３０１、３０２は、それが発するレーザ光線の光軸が、回転中心要素１(球面軸受け)で互いに直交する形で回転体３０５に固定されている。ここでは、２次元位置検出装置としてＰＳＤを用いた例を示しており、ＰＳＤ３０４、３０３は、回転中心要素１からそれらの位置検出面の中心への垂線が互いに直交する形で支持機構３０６に固定されている。支持機構３０６については前記支持機構固定座標系ｘｙｚが定義され、ＰＳＤ３０４については前記位置検出３次元座標系^Aｘ^Aｙ^Aｚが定義され、ＰＳＤ３０３については前記位置検出３次元座標系^Bｘ^Bｙ^Bｚが定義される。

図５、６に示す実施例（１）では、回転中心要素１から、ＰＳＤ３０４、３０３の位置検出面の中心に向かう単位ベクトル、つまり、互いに直交する単位光線ベクトルをそれぞれＰ₀、Ｑ₀とすると、その姿勢角は（0[deg],0[deg],+45[deg]）になっている。つまり、初期状態の直交する単位光線ベクトルＰ₀、Ｑ₀は、支持機構固定座標系ｘｙｚにおいて、互いに直交する単位ベクトル[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Ｔは次のようになる。

また、ＰＳＤ３０４の位置検出２次元座標系で測定される位置検出値は、定義より、前記位置検出３次元座標系^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Ｐは、

となる。

同様にして、位置検出３次元座標系座標系^Bｘ^Bｙ^Bｚにおける、ＰＳＤ３０３の位置検出面における検出点を示すベクトルは(+40[mm] 0.00[mm] +3.50[mm])^Tとなり、これを単位ベクトルに正規化した位置検出３次元座標系座標系^Bｘ^Bｙ^Bｚにおける単位光線ベクトル^SＱは、

となる。

前記式(１)〜（６）および実施例（１）に適用される式（７）より、直交する単位光線ベクトル^SＰおよび^SＱを、支持機構固定座標系ｘｙｚで表現した互いに直交する単位光線ベクトルＰおよびＱは、

となる。この結果を前記式（４）に適用すると、回転体３０５に連動して回転する互いに直交する単位光線ベクトルＰおよびＱの姿勢角(α,β,γ)は、

となり、これから

が得られる。

上記の結果より、回転体３０５に連動して回転する直交する単位光線ベクトルＰおよびＱの３軸回りの回転行列Ｒ_x(α)、Ｒ_z(β)、Ｒ_y(γ)は、

となる。

同様にして、直交する単位光線ベクトルＰおよびＱの初期状態の姿勢角(α₀,β₀,γ₀)は
（0[deg],0[deg],+45[deg]）であることから、初期姿勢角の３軸回りの回転行列Ｒ_x(α₀)、Ｒ_z(β₀)、Ｒ_y(γ₀)は、

となる。

以上の結果を前記式（５）に適用することにより、直交する単位光線ベクトルＰおよびＱの姿勢角の変化分の回転に対応する回転行列ΔＲ(Δα,Δβ,Δγ)は、

となる。

一方、直交する単位ベクトル[1 0 0]^Tおよび[0 0 1]^Tが、姿勢角(Δα,Δβ,Δγ)だけ回転して直交するベクトルｅ₁およびｅ₂になったとすると、

となり、これを前記式（６）に適用すると、

が得られる。

以上より、回転体３０５の回転に連動して、回転体３０５の直交する単位光線ベクトルが、初期姿勢角（α₀,β₀,γ₀）=(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Ｐは、

となる。同様にして、ＰＳＤ３０３に定義された位置検出３次元座標系^Bｘ^Bｙ^Bｚを基にしたＰＳＤ３０３の位置検出面での検出点を示すベクトルは(+40[mm], +0.002[mm], -2.15[mm])^Tとなり、これを単位ベクトルに正規化した位置検出３次元座標系^Bｘ^Bｙ^Bｚの単位光線ベクトル^SＱは、

となる。

また、位置検出３次元座標系^Aｘ^Aｙ^Aｚ、および、位置検出３次元座標系^Bｘ^Bｙ^Bｚ から、支持機構固定座標系 ｘｙｚ 系への回転行列^AＴおよび^BＴは、上記変位姿勢角算出シミュレーション１と同様で、

であり、前記式(１)より、支持機構固定座標系 ｘｙｚ 系での直交する単位光線ベクトルＰおよびＱは、

となる。上記の結果を前記式(４）に適用すると、直交する単位光線ベクトルＰおよびＱの姿勢角（α,β,γ）は、

となり、これから

が得られる。

上記の結果より、互いに直交する単位光線ベクトルＰおよびＱの３軸周りの回転行列
Ｒ_x(α)、Ｒ_z(β)、Ｒ_y(γ)は、

となる。同様にして、互いに直交する単位光線ベクトルＰおよびＱの初期状態の姿勢角
（α₀,β₀,γ₀）は、(0[deg],0[deg],+45[deg])であることから、

が得られ、初期姿勢角の３軸回りの回転行列Ｒ_x(α₀)、Ｒ_z(β₀)、Ｒ_y(γ₀)は、

となる。

以上の結果を前記式(５）に適用することにより、直交する単位光線ベクトルＰおよびＱが、その初期姿勢角(α₀,β₀,γ₀) =(0[deg],0[deg],+45[deg])の位置から、姿勢角の変化分(Δα,Δβ,Δγ)の回転を行う際の回転行列ΔＲ(Δα,Δβ,Δγ)は、

となる。一方、直交する単位ベクトル[1 0 0]^Tおよび[0 0 1]^Tが、姿勢角
(Δα,Δβ,Δγ)だけ回転して直交ベクトルｅ₁およびｅ₂になったとすると、

となり、これを前記式(６）に適用すると、

が得られる。

以上より、回転体３０５と連動して回転する直交する単位光線ベクトルが初期姿勢角
（α₀,β₀,γ₀）=(0[deg],0[deg],+45[deg])から回転して現在の姿勢角(α,β,γ)へ変位した際の変位姿勢角(Δα,Δβ,Δγ) は、(+3.0037[deg],+2.9979[deg],+2.9916[deg])と算出できる。

以上が、図５、６に示した本発明の実施例（１）についての変位姿勢角算出シミュレーションの実施例である。

次に、もう一つの実施例である実施例（２）について説明する。

図７は本発明の実施例（２）である。

単一のレーザ光源４０１、ハーフ・ミラー４０２、および、ミラー４０３〜４０７は、回転中心要素１(球面軸受け)を中心に回転する回転体に固定され、ＰＳＤ４１０，４１１は支持機構４１２に固定されている。この実施例（２）では、図５、６に示した実施例（１）と比較すると、レーザ光源を単一にでき小型ミラーを適用することにより省スペース化が可能な構成にできる利点がある。レーザ光源４０１から発光されるレーザ光線はハーフ・ミラー４０２及びミラー４０３〜４０７によって２つのレーザ光線４０８、４０９に分割され、ＰＳＤ４１０、４１１で受光される。ここで、レーザ光線４０８および４０９の光軸は回転中心要素１において互いに直交している。

支持機構４１２については、ｘ軸２０１、ｙ軸２０２（紙面に垂直に立っている）、ｚ軸２０３の３軸から成る支持機構固定座標系ｘｙｚが定義され、ＰＳＤ４１１については位置検出３次元座標系^Aｘ^Aｙ^Aｚが定義され、ＰＳＤ４１０については位置検出３次元座標系
^Bｘ^Bｙ^Bｚが定義される。

レーザ光線４０８、４０９は回転体と連動して回転するが、図７では、その初期状態が示されている。つまり、レーザ光線４０８を示すベクトルは、回転中心要素１からＰＳＤ４１１の中心方向に向かうベクトルを初期ベクトルとし、レーザ光線４０９を示すベクトルは、回転中心要素１からＰＳＤ４１０の中心に向かうベクトルを初期ベクトルとし、回転体に固定された状態で、互いに直交した関係を維持したまま回転する。

回転中心要素１からＰＳＤ４１１、４１０の中心に向かう単位ベクトルを、それぞれＰ₀、Ｑ₀で表すと、それらは互いに直交しており、支持機構固定座標系ｘｙｚを基準としたロール角（ｘ軸回り）、ヨー角（ｚ軸回り）、ピッチ角（ｙ軸回り）で示される姿勢角を“（ロール角,ヨー角,ピッチ角）”形式で表現すると、図７より、ベクトルＰ₀、Ｑ₀は、支持機構固定座標系ｘｙｚにおいて、互いに直交する単位ベクトル[1 0 0]^Tおよび[0 0 1]^Tを、ｙ軸回り（ｙ軸の正方向に向かって右回り）に-45[deg]回転させたものと同じであり、姿勢角の定義から、互いに直交する単位ベクトルＰ₀、Ｑ₀の姿勢角は（0[deg],0[deg],-45[deg]）とみなすことができる。

また、図７より、ＰＤＳ４１１に定義される位置検出３次元座標系^Aｘ^Aｙ^Aｚは、支持機構固定座標系ｘｙｚと一致していた座標系がｙ軸回りに-45[deg]度回転してできたものとして定義され、ＰＤＳ３０３に定義される位置検出３次元座標系^Bｘ^Bｙ^Bｚは、支持機構固定座標系ｘｙｚと一致していた座標系がｙ軸回りに-135[deg]回転してできたもの定義される。このとき、位置検出３次元座標系^Aｘ^Aｙ^Aｚおよび位置検出３次元座標系^Bｘ^Bｙ^Bｚにそれぞれ対応する座標変換行列^AＴ、^BＴは次のようになる。

ＰＳＤ４１１の位置検出面で測定されるレーザ光線４０８の位置検出値は、前記位置検出３次元座標系^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Ｐは、

となる。同様にして、ＰＳＤ４１０についての位置検出３次元座標系^Bｘ^Bｙ^Bｚにおける単位光線ベクトル^SＱは、

となる。

また、前記式（８）より、位置検出３次元座標系^Aｘ^Aｙ^Aｚ、および、位置検出３次元座標系^Bｘ^Bｙ^Bｚ から、支持機構固定座標系 ｘｙｚへの変換行列^AＴおよび^BＴは、

であることから、前記式（１)より、支持機構固定座標系 ｘｙｚにおける直交する単位光線ベクトルＰおよびＱは、

となる。上記の結果を前記式（４）に適用すると、直交する単位光線ベクトルＰおよびＱの姿勢角（α,β,γ）は、

となり、これから

が得られる。

上記の結果より、回転体に固定されて回転する直交する単位光線ベクトルＰおよびＱの３軸回りの回転行列Ｒ_x(α)、Ｒ_z(β)、Ｒ_y(γ)は、

となる。同様にして、互いに直交する単位光線ベクトルＰおよびＱの初期状態の姿勢角
（α₀,β₀,γ₀）は（0[deg],0[deg],-45[deg]）であることから、

が得られ、初期姿勢角（α₀,β₀,γ₀）の３軸回りの回転行列Ｒ_x(α₀)、Ｒ_z(β₀)、Ｒ_y(γ₀)は、

となる。

以上の結果を、前記式（５）に適用することにより、直交する単位光線ベクトルＰおよびＱの姿勢角変位分の回転を行う回転行列ΔＲ(Δα,Δβ,Δγ)は、

となる。一方、直交する単位ベクトル[1 0 0]^Tおよび[0 0 1]^Tが、姿勢角(Δα,Δβ,Δγ)だけ回転して直交ベクトルｅ₁およびｅ₂になったとすると、

となり、これを前記式（６）に適用すると、

が得られる。つまり、直交する単位光線ベクトルＰおよびＱの変位姿勢角(Δα,Δβ,Δγ)は、(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Ｐは、

となる。同様にして、ＰＳＤ４１０についての位置検出３次元座標系^Bｘ^Bｙ^Bｚの単位光線ベクトル^SＱは、

となる。

また、位置検出３次元座標系^Aｘ^Aｙ^Aｚ、および、位置検出３次元座標系^Bｘ^Bｙ^Bｚ 系から、支持機構固定座標系 ｘｙｚ 系への変換行列^AＴおよび^BＴは、上記の変位姿勢角算出シミュレーション１、２と同様にして、

となり、前記式（１)より、支持機構固定座標系ｘｙｚにおける直交する単位光線ベクトルＰおよびＱは、

となる。上記の結果を前記式（４）に適用すると、直交する単位光線ベクトルＰおよびＱの姿勢角（α,β,γ）は、

となり、これから

が得られる。

上記の結果より、回転体３０５と連動して回転する直交する単位光線ベクトルＰおよびＱの３軸周りの回転行列Ｒ_x(α)、Ｒ_z(β)、Ｒ_y(γ)は、

となる。同様にして、直交する単位光線ベクトルＰおよびＱの初期状態の姿勢角
（α₀,β₀,γ₀）は、(0[deg],0[deg],-45[deg])であることから、

が得られ、初期姿勢角の３軸回りの回転行列Ｒ_x(α₀)、Ｒ_z(β₀)、Ｒ_y(γ₀)は、

となる。

以上の結果を前記式（５）に適用することにより、直交する単位光線ベクトルＰおよびＱの姿勢角変位分の回転を行う回転行列ΔＲ(Δα,Δβ,Δγ)は、

となる。一方、直交する単位ベクトル[1 0 0]^Tおよび[0 0 1]^Tが、姿勢角(Δα,Δβ,Δγ)だけ回転して直交ベクトルｅ₁およびｅ₂になったとすると、

となり、これを前記式（６）に適用すると、

となり、回転体と連動して回転する直交する単位光線ベクトルＰおよびＱが、その初期姿勢角（0[deg],0[deg],-45[deg]）の姿勢から回転した場合の、初期姿勢角からの変位姿勢角として、(+3.000[deg],+2.996[deg],+3.003[deg])が得られる。

以上に詳述した本発明の実施の形態では、レーザ光線とそれを受光して位置を検出する２次元位置検出装置を実施例として示したが、直進性のある光線とそれを受光して位置を検出できる２次元位置検出装置であれば、レーザ光線に限定されずに本発明は適用可能である。

また、説明をわかり易くするために、変位姿勢角算出のシミュレーション例では、支持機構固定座標系ｘｙｚのｘｚ平面に二つの２次元位置検出装置の位置検出面の中心と回転中心要素が含まれる場合で説明したが、本発明の姿勢角算出の原理はこれに限定されずに適用できる。つまり、回転体の初期姿勢を示す直交する単位光線ベクトルＰ₀、Ｑ₀は、任意に設定することができる。

上記の実施例では、２次元位置検出装置として、ＰＳＤを用いた例を示したが、これに限定されることはなく、ＣＣＤ素子などの他の手段でも可能である。

以上詳細に述べた本発明の実施の態様は、以下の付記の通りである。
（付記１）回転体がピボット要素を介して支持機構に取り付けられ、
レーザ光源が、前記レーザ光源を源とする二つのレーザ光線の光軸が前記回転体の回転中心要素で直交するように前記回転体に固定され、
前記各レーザ光線に対応して前記レーザ光線を受光して受光位置を検出する２次元位置検出装置を設け、
前記２次元位置検出装置は、前記２次元位置検出装置の位置検出面の中心で受光する前記レーザ光線の光軸が前記位置検出面に対して直交するように前記支持機構に固定され、
前記２次元位置検出装置で検出される受光位置情報を基に、前記回転体が前記回転中心要素を中心として回転する際の姿勢角を算出する、
ことを特徴とする姿勢角検出方法。
（付記２）付記１に記載の姿勢角検出方法において、
前記２次元位置検出装置の位置検出面の中心を原点とする２次元座標系を前記位置検出面上に定義して位置検出２次元座標系とし、
前記回転体の回転中心要素を始点として前記位置検出２次元座標系の原点へ向かうベクトルをｘ軸とし、回転中心要素を始点として前記位置検出２次元座標系で定義される２軸に平行となるベクトルをそれぞれｙ軸およびｚ軸とする３次元座標系を定義して位置検出３次元座標系とし、
前記位置検出２次元座標系で検出される前記レーザ光線の受光位置情報を基に、前記位置検出３次元座標系おける前記回転体の姿勢を示す単位光線ベクトル情報を生成し、
前記各２次元位置検出装置に対応して生成される二つの前記単位光線ベクトル情報を基に前記回転体の姿勢角を算出する、
ことを特徴とする姿勢角検出方法。
（付記３）付記２に記載の姿勢角検出方法において、
前記位置検出３次元座標系において、前記回転体の回転中心要素から前記位置検出２次元座標の原点までの距離をｘとし、前記位置検出２次元座標系において前記レーザ光線を受光した際の受光位置を示す二つの座標値をそれぞれｙおよびｚとし、
前記単位光線ベクトル情報は、前記位置検出３次元座標系の原点を始点とし３次元座標（ｘ，ｙ，ｚ）を終点とするベクトルを単位ベクトルへ正規化したベクトル情報とする、
ことを特徴とする姿勢角検出方法。
（付記４）付記１ないし付記３のいずれかに記載の姿勢角検出方法において、
前記支持機構に固定されるｘ軸、ｙ軸、ｚ軸からなる３次元座標系を定義して支持機構固定座標系とし、
前記回転体の姿勢角を、前記支持機構固定座標系において、ｘ軸の正方向の右回りの回転を示すロール角、ｚ軸の正方向の右回りの回転を示すヨー角、および、ｙ軸の正方向の右回りの回転を示すピッチ角、の順に３軸回転する３軸回転角として表現する、
ことを特徴とする姿勢角検出方法。
（付記５）付記１に記載の姿勢角検出方法において、
前記回転体に固定されるレーザ光源は二個のレーザ光源から成り、
前記二個のレーザ光源が発する二個のレーザ光線は、その光軸が前記回転体の回転中心要素で互いに直交するように配置される、
ことを特徴とする姿勢角検出方法。
（付記６）付記１に記載の姿勢角検出方法において、
前記回転体に固定されるレーザ光源は一個のレーザ光源から成り、
前記一個のレーザ光源が発するレーザ光線は、ハーフミラーおよびミラーを介して二個のレーザ光線に分岐され、前記分岐された二個のレーザ光線の光軸が前記回転体の回転中心要素で互いに直交するように配置される、
ことを特徴とする姿勢角検出方法。
（付記７）支持機構にピボット要素を介して取り付けられる回転体と、
前記回転体に固定されるレーザ光源と、
前記レーザ光源を源とする二つのレーザ光線の光軸を前記回転体の回転中心要素で直交 するように配置する手段と、
前記各レーザ光線に対応して設けられ、前記レーザ光線を受光して受光位置情報を検出する２次元位置検出装置と、
前記２次元位置検出装置を、前記２次元位置検出装置が位置検出面の中心で受光する前記レーザ光線の光軸が前記位置検出面に対して直交するように前記支持機構に固定する手段と、
前記２次元位置検出装置で検出される受光位置情報を基に、前記回転体が前記回転中心要素を中心として回転する際の姿勢角を算出する手段を備える、
ことを特徴とする姿勢角検出装置。
（付記８）付記７に記載の姿勢角検出装置において、
前記２次元位置検出装置の位置検出面の中心を原点として前記位置検出面に定義される２次元座標系である位置検出２次元座標系と、
前記回転体の回転中心要素を始点として前記位置検出２次元座標系の原点へ向かうベクトルをｘ軸とし、回転中心要素を始点として前記位置検出２次元座標系で定義される２軸に平行となるベクトルをそれぞれｙ軸およびｚ軸として定義される３次元座標系である位置検出３次元座標系と、
前記位置検出３次元座標系で検出される前記レーザ光線の受光位置情報を基に、前記位置検出３次元座標系における前記回転体の姿勢を示す単位光線ベクトル情報を生成する手段と、
前記各２次元位置検出装置に対応して生成される二つの前記単位光線ベクトル情報を基に前記回転体の姿勢角を算出する手段を備える、
ことを特徴とする姿勢角検出装置。
（付記９）付記８に記載の姿勢角検出装置において、
前記位置検出３次元座標系において、前記回転体の回転中心要素から前記位置検出２次元座標の原点までの距離をｘとし、前記位置検出２次元座標系において前記レーザ光線を受光した際の受光位置を示す二つの座標値をそれぞれｙおよびｚとし、前記位置検出３次元座標系の原点を始点とし３次元座標（ｘ，ｙ，ｚ）を終点とするベクトルを単位ベクトルへ正規化したベクトル情報を前記単位光線ベクトル情報とする手段を備える、
ことを特徴とする姿勢角検出装置。
（付記１０）付記７ないし付記９のいずれかに記載の姿勢角検出装置において、
前記支持機構に固定して定義されるｘ軸、ｙ軸、ｚ軸からなる３次元座標系である支持機構固定座標系と、
前記回転体の姿勢角を、前記支持機構固定座標系において、ｘ軸の正方向の右回りの回転を示すロール角、ｚ軸の正方向の右回りの回転を示すヨー角、および、ｙ軸の正方向の右回りの回転を示すピッチ角、の順に３軸回転する３軸回転角で表現する手段を備える、
ことを特徴とする姿勢角検出装置。
（付記１１）付記７に記載の姿勢角検出装置において、
前記回転体に固定されるレーザ光源は二個のレーザ光源から成り、
前記二個のレーザ光源が発する二個のレーザ光線は、その光軸が前記回転体の回転中心要素で互いに直交するように配置される、
ことを特徴とする姿勢角検出装置。
（付記１２）付記７に記載の姿勢角検出装置において、
前記回転体に固定されるレーザ光源は一個のレーザ光源から成り、
前記一個のレーザ光源が発するレーザ光線は、ハーフミラーおよびミラーを介して二個のレーザ光線に分岐され、前記分岐された二個のレーザ光線の光軸が前記回転体の回転中心要素で互いに直交するように配置される、
ことを特徴とする姿勢角検出装置。
（付記１３）付記７に記載の姿勢角検出装置において、
前記２次元位置検出装置は、半導体位置検出装置（ＰＳＤ）で構成される、
ことを特徴とする姿勢角検出装置。
（付記１４）付記７に記載の姿勢角検出方法において、
前記３次元位置検出装置は、電荷結合素子（ＣＣＤ）で構成される、
ことを特徴とする姿勢角検出装置。

本発明の姿勢角検出装置の構成（１） 本発明の姿勢角検出装置の構成（２） 座標系定義の概念図 座標系変換の概念図 本発明の実施例（１）の全体図 本発明の実施例（１）の拡大図 本発明の実施例（２）

符号の説明

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 支持機構

Claims (5)

1. 回転体がピボット要素を介して支持機構に取り付けられ、
   レーザ光源が、前記レーザ光源を源とする二つのレーザ光線の光軸が前記回転体の回転中心要素で直交するように前記回転体に固定され、
   前記各レーザ光線に対応して前記レーザ光線を受光して受光位置を検出する２次元位置検出装置を設け、
   前記２次元位置検出装置は、前記２次元位置検出装置の位置検出面の中心で受光する前記レーザ光線の光軸が前記位置検出面に対して直交するように前記支持機構に固定され、
   前記２次元位置検出装置で検出される受光位置情報を基に、前記回転体が前記回転中心要素を中心として回転する際の姿勢角を算出する、
   ことを特徴とする姿勢角検出方法。
2. 請求項１に記載の姿勢角検出方法において、
   前記２次元位置検出装置の位置検出面の中心を原点とする２次元座標系を前記位置検出面上に定義して位置検出２次元座標系とし、
   前記回転体の回転中心要素を始点として前記位置検出２次元座標系の原点へ向かうベクトルをｘ軸とし、回転中心要素を始点として前記位置検出２次元座標系で定義される２軸に平行となるベクトルをそれぞれｙ軸およびｚ軸とする３次元座標系を定義して位置検出３次元座標系とし、
   前記位置検出２次元座標系で検出される前記レーザ光線の受光位置情報を基に、前記位置検出３次元座標系おける前記回転体の姿勢を示す単位光線ベクトル情報を生成し、
   前記各２次元位置検出装置に対応して生成される二つの前記単位光線ベクトル情報を基に前記回転体の姿勢角を算出する、
   ことを特徴とする姿勢角検出方法。
3. 請求項２に記載の姿勢角検出方法において、
   前記位置検出３次元座標系において、前記回転体の回転中心要素から前記位置検出２次元座標の原点までの距離をｘとし、前記位置検出２次元座標系において前記レーザ光線を受光した際の受光位置を示す二つの座標値をそれぞれｙおよびｚとし、
   前記単位光線ベクトル情報は、前記位置検出３次元座標系の原点を始点とし３次元座標（ｘ，ｙ，ｚ）を終点とするベクトルを単位ベクトルへ正規化したベクトル情報とする、
   ことを特徴とする姿勢角検出方法。
4. 請求項１ないし請求項３のいずれかに記載の姿勢角検出方法において、
   前記支持機構に固定されるｘ軸、ｙ軸、ｚ軸からなる３次元座標系を定義して支持機構固定座標系とし、
   前記回転体の姿勢角を、前記支持機構固定座標系において、ｘ軸の正方向の右回りの回転を示すロール角、ｚ軸の正方向の右回りの回転を示すヨー角、および、ｙ軸の正方向の右回りの回転を示すピッチ角、の順に３軸回転する３軸回転角として表現する、
   ことを特徴とする姿勢角検出方法。
5. 支持機構にピボット要素を介して取り付けられる回転体と、
   前記回転体に固定されるレーザ光源と、
   前記レーザ光源を源とする二つのレーザ光線の光軸を前記回転体の回転中心要素で直交 するように配置する手段と、
   前記各レーザ光線に対応して設けられ、前記レーザ光線を受光して受光位置情報を検出する２次元位置検出装置と、
   前記２次元位置検出装置を、前記２次元位置検出装置が位置検出面の中心で受光する前記レーザ光線の光軸が前記位置検出面に対して直交するように前記支持機構に固定する手段と、
   前記２次元位置検出装置で検出される受光位置情報を基に、前記回転体が前記回転中心要素を中心として回転する際の姿勢角を算出する手段を備える、
   ことを特徴とする姿勢角検出装置。
   

Images