JP2006201092A - ６自由度移動体の位置・姿勢計測方法および位置・姿勢計測装置 - Google Patents

Abstract

【課題】 複数のセンサを用いて簡単かつ正確に６自由度移動体の位置・姿勢を計測する。
【解決手段】 固定子と、固定子に対して６自由度変位する移動子と、複数のセンサとを備えた６自由度移動体の位置・姿勢計測装置において、適当な移動座標系と固定座標系を設定し、一定順番の回転座標変換を含む座標変換を行い、複数のギャップセンサ１、２、３、及び二次元センサ５、６を適切な場所に設置し、複数のセンサの検出情報および与えられた初期位置情報に基づいて可動子が変位した後の位置および姿勢を計算する。
【選択図】 図１

Description

本発明は、複数センサを用いて６自由度移動体の位置・姿勢を計測する計測装置に関する。

従来の６自由度移動体の位置・姿勢計測装置は、ｘ、ｙ、ｚの３方向に全部で６つ変位センサを設置し計測装置を構成している（例えば、特許文献１参照）。図９は従来技術の６自由度移動体の位置・姿勢計測装置の構成を示すブロック図である。
図９において、２１は移動子であり、アクチュエータ（不図示）に駆動されて６自由度の位置・姿勢変位する。３１はｘ方向変位センサであり、ｘ方向の変位量を計測する。３２と３３はｙ方向変位センサであり、ｙ方向の変位量およびｚ軸まわりの回転角を計測する。また、３４、３５および３６はｚ方向変位センサであり、ｚ方向の変位量およびｘ、ｙ軸のまわりの回転角度を計測する。
各軸のまわりの回転運動量が小さい場合は、回転が並進変位量の計測に与える影響が小さいためそれを無視でき、移動子が変位した後の位置・姿勢を簡単に求められる。
このように、従来技術の６自由度移動体の位置・姿勢計測方法は、ｘ、ｙ、ｚの３方向向きに設置した６つの変位センサ３１〜３６からの変位情報を用いて移動子２１が変位した後の位置・姿勢を計算するものである。
特開平１０−１９４４５０号公報

しかしながら、従来の６自由度移動体の位置・姿勢計測方法は、回転運動量が大きい又は計測精度が高く要求される場合に、移動子が変位した後の位置・姿勢を正確に計測することが困難であるため、要求された仕様を満足できないという問題があった。
そこで、本発明は、このような問題点に鑑みてなされたものであり、各種の適宜なセンサを適切な位置に設置し、適宜な座標変換を行うことで、簡単かつ正確に６自由度移動体の位置・姿勢を計測することができて、回転運動量が大きい場合や高い計測制度が要求される場合にも移動子が変位した後の位置・姿勢の正確な計測が可能な６自由度移動体の位置・姿勢計測方法および位置・姿勢計測装置を提供することを目的とする。

上記問題を解決するため、本発明は、次のように構成したのである。
請求項１に記載の発明は、固定子と、前記固定子に対して６自由度変位する移動子と、複数のセンサとを備えた６自由度移動体の位置・姿勢計測方法において、移動座標系と固定座標系に基づいて前記固定子および前記移動子に複数のセンサを設置し、該複数のセンサの検出情報および与えられた初期位置情報に基づいて前記移動子が変位した後の該移動子の前記固定子に対する位置および姿勢を計算して前記移動子上の任意点の位置を求める際に、固定座標系ｘｙｚｏを前記固定子に固定し、移動座標系ＸＹＺＯを前記移動子に固定し、前記移動子が変位する前に移動座標系と固定座標系を重ねるように移動座標系と固定座標系を設定し、ｘ軸、ｙ軸およびｚ軸のまわりの前記移動子の回転角度をそれぞれα、βおよびγとし、移動座標系の原点の絶対座標を（ｘ_o，ｙ_o，ｚ_o）とし、前記移動子上の任意固定点の絶対座標をこの任意固定点の移動座標と移動座標系の原点の絶対座標（ｘ_o，ｙ_o，ｚ_o）および回転角度α、βおよびγで表すように座標変換して計算するという手順を特徴としている。
また、請求項２に記載の発明は、前記複数のセンサは、ギャップセンサのヘッドとギャップセンサのターゲット平面との距離を計る第１のギャップセンサ、第２のギャップセンサおよび第３のギャップセンサを含み、回転座標変換を固定座標系のｘ軸、ｙ軸、ｚ軸の順で行い、前記第１のギャップセンサ、前記第２のギャップセンサおよび前記第３のギャップセンサに対して、すべてのヘッドが前記固定子に固定されて平面ｙｏｚ又は平面ｘｏｚに対して対称であり、すべてのターゲット平面が前記移動子に固定されて平面ＸＯＹに平行であるように前記第１のギャップセンサ、前記第２のギャップセンサおよび前記第３のギャップセンサを設置するという手順を特徴としている。

また、請求項３に記載の発明は、前記複数のセンサは、直交２方向の変位量を検出する第１の二次元センサおよび第２の二次元センサを含み、前記第１の二次元センサと前記第２の二次元センサに対して、すべてのヘッドが前記固定子に固定され、すべてのターゲットが前記移動子に固定され、すべてのターゲット側における初期測定点のＺ移動座標が同じであり、前記第１の二次元センサの２つ測定方向と前記第２の二次元センサの２つ測定方向が共にｘ軸又はｙ軸に平行であるように前記第１の二次元センサと前記第２の二次元センサを設置するという手順を特徴としている。
また、請求項４に記載の発明は、前記複数のセンサは、直線方向の変位量を検出する第１の一次元センサ、第２の一次元センサおよび第３の一次元センサを含み、前記第１の一次元センサ、前記第２の一次元センサおよび前記第３の一次元センサに対して、前記第１の一次元センサと前記第２の一次元センサの測定方向が共にｙ方向で、前記第３の一次元センサの測定方向がｘ方向であり、前記第１の一次元センサのターゲット側の初期測定点と前記第２の一次元センサのターゲット側の初期測定点とを通る直線がｘ軸に平行であるように前記第１の一次元センサ、前記第２の一次元センサおよび前記第３の一次元センサを設置するという手順を特徴としている。
また、請求項５に記載の発明は、前記複数のセンサは、直線方向の変位量を検出する第１の一次元センサ、第２の一次元センサおよび第３の一次元センサを含み、前記第１の一次元センサ、前記第２の一次元センサおよび前記第３の一次元センサに対して、前記第１の一次元センサの測定方向がｙ方向で、前記第２の一次元センサの測定方向と前記第３の一次元センサの測定方向が共にｘ方向であり、前記第２の一次元センサのターゲット側の初期測定点と前記第３の一次元センサのターゲット側の初期測定点とを通る直線がｙ軸に平行であるように前記第１の一次元センサ、前記第２の一次元センサおよび前記第３の一次元センサを設置するという手順を特徴としている。

また、請求項６に記載の発明は、前記複数のセンサは、ギャップセンサのヘッドとギャップセンサのターゲット平面との距離を計る第１のギャップセンサ、第２のギャップセンサおよび第３のギャップセンサを含み、前記第１のギャップセンサ、前記第２のギャップセンサおよび前記第３のギャップセンサに対して、すべてのヘッドが前記移動子に固定されて平面ＹＯＺ又は平面ＸＯＺに対して対称であり、すべてのターゲット平面が前記固定子に固定されて平面ｘｏｙに平行であるように前記第１のギャップセンサ、前記第２のギャップセンサおよび前記第３のギャップセンサを設置するという手順を特徴としている。
また、請求項７に記載の発明は、請求項２又は請求項６のいずれかに記載の６自由度移動体の位置・姿勢計測装置に第４のギャップセンサを追加した６自由度移動体の位置・姿勢計測装置において、
前記第１のギャップセンサ、前記第２のギャップセンサ、前記第３のギャップセンサおよび前記第４のギャップセンサが座標系に対して対称になるように前記第４のギャップセンサを設置し、前記第１のギャップセンサ、前記第２のギャップセンサ、前記第３のギャップセンサおよび前記第４のギャップセンサから任意の３つギャップセンサの検出情報および与えられた初期位置情報に基づいて前記移動子が変位した後の前記移動子の前記固定子に対する位置および姿勢を計算し、このように得られた４組のデータの平均値を取るという手順を特徴としている。
また、請求項８に記載の発明は、６自由度変位する移動子と、１つ方向の変位量および２つの回転角度を検出する第１の三次元センサと、直交２つの方向の変位量および１つの回転角度を検出する第２の三次元センサとを備えた６自由度移動体の位置・姿勢計測装置において、固定座標系ｘｙｚｏを大地に固定し、移動座標系ＸＹＺＯを移動子に固定し、前記移動子が変位する前に移動座標系と固定座標系を重ねるように移動座標系と固定座標系を設定し、ｘ軸、ｙ軸およびｚ軸のまわりの前記移動子の回転角度をそれぞれα、βおよびγとし、移動座標系の原点の絶対座標を（ｘ_o，ｙ_o，ｚ_o）とし、前記移動子上の任意固定点の絶対座標をこの任意固定点の移動座標と移動座標系の原点の絶対座標（ｘ_o，ｙ_o，ｚ_o）および回転角度α、βおよびγで表すように座標変換し、前記第１の三次元センサがｚ軸に平行である固定直線と移動子に固定された前記第１の三次元センサのターゲット平面との交点の移動距離およびｘとｙ軸のまわりの前記移動子の回転角度αとβを検出し、前記第２の三次元センサが移動子に固定されたターゲット測定点におけるｘ方向の変位量およびｙ方向の変位量とｚ軸のまわりの前記移動子の回転角度γを検出するように前記第１の三次元センサと前記第２の三次元センサを設置し、前記第１の三次元センサと前記第２の三次元センサとの検出情報および与えられた初期位置情報に基づいて前記移動子が変位した後の前記移動子の前記固定子に対する位置および姿勢を計算するという手順を特徴としている。

本発明によれば、各種の適宜な変位センサを適切な位置に設置し、適宜な座標系を設定することで、各センサの設置位置の情報および各センサが検出した信号に基づいて移動子に固定されている移動座標系の原点位置および姿勢を求めることができる。
また、移動子上の任意点の移動座標と移動座標系の変位後の原点位置および姿勢を用いて正確に移動子が変位した後この点の位置を計算することができるので、回転運動量が大きい場合や高い計測制度が要求されるような場合にも移動子が変位した後の位置・姿勢を正確に計測することが可能になると言う効果がある。

以下、本発明の実施の形態について図を参照して説明する。

図１は、本発明の実施例１における６自由度移動体の位置・姿勢計測装置の構成を示す図である。
図１において、２１は移動子であり、アクチュエータ（不図示）に駆動されて６自由度の位置・姿勢変位する。２２は固定子である。１、２および３はギャップセンサ（光学式、静電式等がある）であり、ヘッドが固定子に固定され、ターゲットが移動子に固定され、ヘッドとターゲット平面との距離を検出する。そして、５と６は二次元センサ（面検出可能）であり、垂直２方向の変位量を検出する。
図１に示した本実施例の６自由度移動体の位置・姿勢計測装置が図９に示した特許文献１の６自由度移動体の位置・姿勢計測装置と異なる部分は、ｘ、ｙ軸方向向きに設置した３つ１自由度の変位センサの代わりに２つの二次元センサ５、６を備えた部分である。その他の構成は同一である。
以下、本実施例の計測原理について説明する。
ここで、位置・姿勢を厳密かつ明確に計算するため、固定子に固定される固定座標系ｘｙｚｏと移動子に固定されて移動子と一緒に変位する移動座標系ＸＹＺＯとを導入する。そして、固定座標系ｘｙｚｏが初期（移動子の変位が０である時）の移動座標系ＸＹＺＯと重なるように移動座標系と固定座標系を設定する。 移動子が変位した後に、移動座標系の原点Ｏの絶対座標を（ｘ_O，ｙ_O，ｚ_O）とし、移動座標系に対して固定座標系のｘ軸、ｙ軸、ｚ軸の順で回転座標変換を行う場合の姿勢角度を（α，β，γ）とし、座標変換行列をＧとすると、移動座標が（Ｘ，Ｙ，Ｚ）である点の絶対座標（ｘ，ｙ，ｚ）を次式で表すことができる。

ただし、Ｇ₁₁＝ｃｏｓβｃｏｓγ，
Ｇ₁₂＝ｃｏｓβｓｉｎγ，
Ｇ₁₃＝−ｓｉｎβ，
Ｇ₂₁＝ｓｉｎαｓｉｎβｃｏｓγ−ｃｏｓαｓｉｎγ，
Ｇ₂₂＝ｓｉｎαｓｉｎβｓｉｎγ＋ｃｏｓαｃｏｓγ，
Ｇ₂₃＝ｓｉｎαｃｏｓβ，
Ｇ₃₁＝ｃｏｓαｓｉｎβｃｏｓγ＋ｓｉｎαｓｉｎγ，
Ｇ₃₂＝ｃｏｓαｓｉｎβｓｉｎγ−ｓｉｎαｃｏｓγ，
Ｇ₃₃＝ｃｏｓαｃｏｓβ．
上式により、移動座標系の原点位置（ｘ_O，ｙ_O，ｚ_O）および姿勢（α，β，γ）を求めれば、移動子上の任意点の位置を求めることができる。

以上の座標系に基づいて各センサの設置方法について詳細に説明する。
ギャップセンサ１、ギャップセンサ２およびギャップセンサ３のヘッド側の測定点をそれぞれＨ₁、Ｈ₂およびＨ₃とし、ターゲット側の測定点をそれぞれＴ₁、Ｔ₂およびＴ₃とする。この３つのギャップセンサは、すべてのターゲット平面が平面ＸＯＹに平行であり、点Ｈ₁と点Ｈ₂とが平面ｙｏｚに対して対称であり、点Ｈ₂と点Ｈ₃とが平面ｘｏｚに対して対称であり、そして移動子の変位がない時直線Ｈ₁Ｔ₁、Ｈ₂Ｔ₂およびＨ₃Ｔ₃が全てｚ軸に平行であるように設置される。また、二次元センサ５と二次元センサ６のターゲット側の測定点をそれぞれＰとＱとする。この２つの二次元センサは、直線ＰＱが平面ＸＯＹに平行であり、そしてセンサの２つの測定方向がｘ軸又はｙ軸に平行であるように設置される。
従って、点Ｈ₁、Ｈ₂およびＨ₃の絶対座標をそれぞれ（ｘ_H1，ｙ_H1，ｚ_H1）、（ｘ_H2，ｙ_H2，ｚ_H2）および（ｘ_H3，ｙ_H3，ｚ_H3）とし、点Ｐおよび点Ｑの移動座標をそれぞれ（Ｘ_P，Ｙ_P，Ｚ_P）および（Ｘ_Q，Ｙ_Q，Ｚ_Q）とすると、
ｘ_H2＝ｘ_H3 … （２）
ｙ_H1＝ｙ_H2 … （３）
および
ｚ_H1＝ｚ_H2＝ｚ_H3， Ｚ_P＝Ｚ_Q … （４）
が成り立つ。
以下、各センサからの検出情報および与えられた各センサの初期位置に基づいて移動子が変位した後の移動座標系の原点位置（ｘ_O，ｙ_O，ｚ_O）および姿勢（α，β，γ）を求める。
まず、３つのギャップセンサの検出情報および初期位置情報に基づいてαおよびβを求める。
ギャップセンサ１のターゲット側の測定点Ｔ₁の初期ｚ絶対座標をｚ_T10とすると、ギャップセンサ１が測定した初期ギャップは
ｄ₁₀＝ｚ_T10−ｚ_H1 … （５）
となる。
ギャップセンサのターゲット平面が常に平面ＸＯＹに平行であるので、変位後ギャップセンサのターゲット平面の法線ベクトルは

となる。また、ギャップセンサ１のターゲット平面とＺ軸との交点Ｃ１の移動座標が（０，０，ｚ_T10）であるので、変位した後の絶対座標は

となる。よって、移動子が変位した後にギャップセンサ１のターゲット平面の方程式は
ｘ−ｘ_O−Ｇ₁₃ｚ_T10）Ｇ₁₃＋（ｙ−ｙ_O−Ｇ₂₃ｚ_T10）Ｇ₂₃
＋（ｚ−ｚ_O−Ｇ₃₃ｚ_T10）Ｇ₃₃＝０ … （８）
となる。
また、Ｇ₁₃ ²＋Ｇ₂₃ ²＋Ｇ₃₃ ²＝１を考慮して上式を整理すると、
ｘ_G13＋ｙ_G23＋ｚ_G33＝ｄ_O＋ｚ_T10 … （９）
となる。ただし、
ｄ_O＝ｘ_OＧ₁₃＋ｙ_OＧ₂₃＋ｚ_OＧ₃₃ … （１０）
である。
従って、点（ｘ_H1，ｙ_H1，ｚ_H1）とギャップセンサ１のターゲット平面との距離、すなわち、移動子が変位した後にギャップセンサ１が測定したギャップは
ｄ₁＝ｄ_O＋ｚ_T10−ｘ_H1Ｇ₁₃−ｙ_H1Ｇ₂₃−ｚ_H1Ｇ₃₃ … （１１）
となる。
式（５）および式（１１）によると、移動子が変位する前後第1ギャップセンサが検出した第1ギャップの変化量は
Δｄ₁≡ｄ₁−ｄ₁₀＝ｄ_O−ｘ_H1Ｇ₁₃−ｙ_H1Ｇ₂₃−ｚ_H1Ｇ₃₃−ｚ_H1 … （１２）
を満たす。
そして、同じ理由で移動子が変位する前後第２と第３ギャップセンサがそれぞれ検出した第２と第３ギャップの変化量Δｄ₂とΔｄ₃は
Δｄ₂＝ｄ_O−ｘ_H2Ｇ₁₃−ｙ_H2Ｇ₂₃−ｚ_H2Ｇ₃₃−ｚ_H2 … （１３）
および
Δｄ₃＝ｄ_O−ｘ_H3Ｇ₁₃−ｙ_H3Ｇ₂₃−ｚ_H3Ｇ₃₃−ｚ_H3 … （１４）
を満たす。
式（２）〜式（４）および式（１２）〜式（１４）によると、
Δｄ₁−Δｄ₂＝（ｘ_H2−ｘ_H1）Ｇ₁₃ …（１５）
および
Δｄ₂−Δｄ₃＝（ｙ_H3−ｙ_H2）Ｇ₂₃ … （１６）
となる。
Ｇ₁₃＝−ｓｉｎβおよびＧ₂₃＝ｓｉｎαｃｏｓβを上の２式に代入すると、αおよびβは次のように与えられる。
β＝ａｒｃｓｉｎ｛（Δｄ₁−Δｄ₂）／（ｘ_H1−ｘ_H2）｝ … （１７）
α＝ａｒｃｓｉｎ｛（Δｄ₂−Δｄ₃）／（ｙ_H3−ｙ_H2）／ｃｏｓβ｝ … （１８）
Ｈ₁、Ｈ₂、Ｈ₃は固定点なので、ｘ_H1、ｘ_H2、ｙ_H2およびｙ_H3を予め測定しておけば上の２式からαおよびβを求めることができる。
次に、求めたα、βの値、２つの二次元センサの検出情報および初期位置情報に基づいてγ、ｘ_Oおよびｙ_Oを求める。
点Ｐおよび点Ｑの絶対座標をそれぞれ（ｘ_P，ｙ_P，ｚ_P）および（ｘ_Q，ｙ_Q，ｚ_Q）とし、式（１）によると、

および

となる。
Ｚ_P＝Ｚ_Qを考慮して式（１９）から式（２０）を引くと、

となる。
また、ｘ_PQ＝ｘ_P−ｘ_Q，Ｘ_PQ＝Ｘ_P−Ｘ_Q，ｙ_PQ＝ｙ_P−ｙ_Q
および、Ｙ_PQ＝Ｙ_P−Ｙ_Qとし、
Ｇ₁₁＝ｃｏｓβｃｏｓγ，
Ｇ₁₂＝ｃｏｓβｓｉｎγ，
Ｇ₂₁＝ｓｉｎαｓｉｎβｃｏｓγ−ｃｏｓαｓｉｎγ、
および
Ｇ₂₂＝ｓｉｎαｓｉｎβｓｉｎγ＋ｃｏｓαｃｏｓγ
を上の２式に代入し、整理すると、

となる。この方程式を解くと、

となる。従って、γは次式のように与えられる。

上式中、ｘ_PQはｘ_Pとｘ_Qとの差、ｙ_PQはｙ_Pとｙ_Qとの差である。
また、（ｘ_P，ｙ_P）と（ｘ_Q，ｙ_Q）はそれぞれ点Ｐと点Ｑの変位後の絶対座標であり、直接センサから得られない。
しかし、（Ｘ_P，Ｙ_P）と（Ｘ_Q，Ｙ_Q）は既知であり、二次元センサ５が測定した点Ｐの変位量を（Ｓ_Px，Ｓ_Py）とし、二次元センサ６が測定した点Ｑの変位量を（Ｓ_Qx，Ｓ_Qy）とすると、点Ｐと点Ｑの変位後の絶対座標は次のように与えられる。
（ｘ_P，ｙ_P）＝（Ｘ_P，Ｙ_P）＋（Ｓ_Px，Ｓ_Py） … （２５）
（ｘ_Q，ｙ_Q）＝（Ｘ_Q，Ｙ_Q）＋（Ｓ_Qx，Ｓ_Qy） … （２６）
以上のようにα、β、γの値が決められると、式（１）より、回転座標変換行列Ｇの全ての要素Ｇ_ij（ｉ＝１，２，３、ｊ＝１，２，３）は決められる。点Ｐの移動座標（Ｘ_P，Ｙ_P，Ｚ_P）および変位後の点Ｐのｘｙ絶対座標（ｘ_P，ｙ_P）を式（１）に代入し変形すると、変位後移動座標の原点のｘｙ絶対座標は次のように与えられる。
ｘ_O＝ｘ_P−（Ｇ₁₁Ｘ_P＋Ｇ₁₂Ｙ_P＋Ｇ₁₃Ｚ_P） …（２７）
ｙ_O＝ｙ_P−（Ｇ₂₁Ｘ_P＋Ｇ₂₂Ｙ_P＋Ｇ₂₃Ｚ_P） …（２８）
また、同じ理由で変位前後の点Ｑの情報を用いて変位後移動座標の原点のｘｙ絶対座標を次のように与えられる。
ｘ_O＝ｘ_Q−（Ｇ₁₁Ｘ_Q＋Ｇ₁₂Ｙ_Q＋Ｇ₁₃Ｚ_Q） …（２９）
ｙ_O＝ｙ_Q−（Ｇ₂₁Ｘ_Q＋Ｇ₂₂Ｙ_Q＋Ｇ₂₃Ｚ_Q） …（３０）
式（２７）、（２８）又は式（２９）、（３０）を用いて得た２通りデータの平均値を変位後移動座標の原点のｘｙ絶対座標とすることで計測精度を上げることができる。また、この２通りのデータを用いて検出エラーの判断を行うことも可能である。

最後に、ｚ_Oを求める。
式（１０）と式（１１）によると、
ｄ₁＝ｚ_T10＋（ｘ_O−ｘ_H1）Ｇ₁₃＋（ｙ_O−ｙ_H1）Ｇ₂₃＋（ｚ_O−ｚ_H1）Ｇ₃₃ …（３１）
となる。
上式により、ｚ_Oは次式のように与えられる。
ｚ_O＝ｚ_H1＋｛ｄ₁−ｚ_T10−（ｘ_O−ｘ_H1）Ｇ₁₃−（ｙ_O−ｙ_H1）Ｇ₂₃｝／Ｇ₃₃ …（３２）
以上では、ギャップセンサ１の情報を用いてｚ_Oを計算しているが、同じ理由でギャップセンサ２或いはギャップセンサ３の情報を用いて次のようにｚ_Oを計算することができる。
ｚ_O＝ｚ_H2＋｛ｄ₂−ｚ_T20−（ｘ_O−ｘ_H2）Ｇ₁₃−（ｙ_O−ｙ_H2）Ｇ₂₃｝／Ｇ₃₃ …（３３）
或いは
ｚ_O＝ｚ_H3＋｛ｄ₃−ｚ_T30−（ｘ_O−ｘ_H3）Ｇ₁₃−（ｙ_O−ｙ_H3）Ｇ₂₃｝／Ｇ₃₃ …（３４）
式（３２）又は式（３３）又は式（３４）を用いて得た３通りデータの平均値を変位後移動座標の原点のｚ絶対座標とすることで計測精度を上げることができる。また、この３通りのデータを用いて検出エラーの判断を行うことも可能である。

このように、３つのギャップセンサは、すべてのヘッドが固定子に固定されて平面ｙｏｚ又は平面ｘｏｚに対して対称であり、すべてのターゲット平面が移動子に固定されてｘｏｙ平面に平行であるように設置され、また、２つの二次元センサは、すべてのヘッドが前記固定子に固定され、すべてのターゲットが前記移動子に固定され、すべてのターゲット側における初期測定点のＺ移動座標が同じであるように設置されると、各センサの設置位置の情報および各センサが検出した信号に基づいて移動子に固定されている移動座標系の原点位置および姿勢を高精度に求めることができる。
また、移動子上の任意点の移動座標と移動子が変位した後の移動座標系の原点位置および姿勢を用いて正確に移動子が変位した後この点の位置を計算することができる。
これによって、特許文献1の場合のような移動子の変位量の制御が近似計算により装置稼働前の採取データと比較する方式に対して、より正確な演算による制御が可能となり、例えば、最近の大型化・長尺化する液晶デバイス、半導体デバイスの基板の製造等において回転運動量が大きくなっても、高精度な位置制御により対応することで、製品の劣化を防止し歩留まりを改善できる。

次に本発明の実施例２について図を参照して説明する。
図２は、本発明の実施例２における６自由度移動体の位置・姿勢計測装置の構成を示す図である。
図２に示した本実施例の６自由度移動体の位置・姿勢計測装置が図１に示した実施例１の６自由度移動体の位置・姿勢計測装置と異なる部分は、２つ二次元センサの代わりに３つ変位センサを備えた部分である。その他の構成は同一である。
図２において、７、８および９は変位センサ（光学式、静電式、磁気式等）であり、移動子に固定されているターゲット測定点における１方向の変位量を検出する。また、変位センサ７と変位センサ８との測定方向はｙ方向で、変位センサ９の測定方向はｘ方向であり、変位センサ７のターゲット測定点Ｄと変位センサ８のターゲット測定点Ｅとを通る直線がｘ軸に平行であるようにこの３つセンサを設置する。変位センサ７、変位センサ８および変位センサ９のターゲット測定点の移動座標をそれぞれＤ（Ｘ_D，Ｙ_D，Ｚ_D）、Ｅ（Ｘ_E，Ｙ_E，Ｚ_E）およびＦ（Ｘ_F，Ｙ_F，Ｚ_F）とすると、
Ｙ_D＝Ｙ_E， Ｚ_D＝Ｚ_E … （３５）
が成り立つ。
以下、各センサからの情報および各センサの初期位置に基づいて移動子が変位した後の移動座標系の原点位置（ｘ_O，ｙ_O，ｚ_O）および姿勢（α，β，γ）を求める。

まず、実施例１と同じように３つのギャップセンサ１、２、３の検出情報および初期位置情報に基づいてαおよびβを求める。
次に、求めたα、βの値、３つの変位センサ７、８、９の検出情報および初期位置情報に基づいてγ、ｘ_Oおよびｙ_Oを求める。
変位センサ７および変位センサ８が検出した変位量をそれぞれＳＤおよびＳＥとすると、移動子が変位した後点Ｄおよび点Ｅのｙ絶対座標はそれぞれ
ｙ_D＝Ｙ_D＋Ｓ_D， ｙ_E＝Ｙ_E＋Ｓ_E …（３６）
で求められる。また、式（１）によると、
ｙ_D＝ｙ_O＋（Ｇ₂₁Ｘ_D＋Ｇ₂₂Ｙ_D＋Ｇ₂₃Ｚ_D） …（３７）
および
ｙ_E＝ｙ_O＋（Ｇ₂₁Ｘ_E＋Ｇ₂₂Ｙ_E＋Ｇ₂₃Ｚ_E） …（３８）
が成り立つ。式（３５）を考慮して式（３７）から式（３８）を引くと、
ｙ_D−ｙ_E＝Ｇ₂₁（Ｘ_D−Ｘ_E） …（３９）
となる。Ｇ₂₁＝ｓｉｎαｓｉｎβｃｏｓγ−ｃｏｓαｓｉｎγを上式に代入してγを求めると、
γ＝Ａ−ａｒｃｓｉｎ｛ｋ（ｙ_D−ｙ_E）／（Ｘ_D−Ｘ_E）｝ … （４０）
となる。
ただし、Ａ＝ａｒｃｓｉｎ（ｋｓｉｎαｓｉｎβ），
ｋ＝１／√（ｓｉｎ２αｓｉｎ２β＋ｃｏｓ２α）．
また、変位センサ９が検出した変位量をＳＦとすると、移動子が変位した後点Ｆのｘ絶対座標は
ｘ_F＝Ｘ_F＋Ｓ_F …（４１）
で求められる。
式（１）により、ｘ_Oは次式のように与えられる。
ｘ_O＝ｘ_F−（Ｇ₁₁Ｘ_F＋Ｇ₁₂Ｙ_F＋Ｇ₁₃Ｚ_F） …（４２）
また、式（３７）により、ｙ_Oは次式のように与えられる。
ｙ_O＝ｙ_D−（Ｇ₂₁Ｘ_D+Ｇ₂₂Ｙ_D＋Ｇ₂₃Ｚ_D） …（４３）
最後に、実施例１と同じようにｚ_Oを計算する。
このように、３つギャップセンサと３つ変位センサを用いることで、正確に移動子の変位した後の位置・姿勢を計測することができる。
本実施例は実施例１と較べ、センサが１つ増えるので設置場所を大きく取る必要になるが、変位センサが二次元センサより廉価であるためコストを下げることができる。

次に、本発明の実施例３について図を参照して説明する。
図３は、本発明の実施例３における６自由度移動体の位置・姿勢計測装置の構成を示す図である。
図３に示した本実施例の６自由度移動体の位置・姿勢計測装置が図２に示した実施例２の６自由度移動体の位置・姿勢計測装置と異なる部分は、３つ変位センサの設置方法が違う部分である。その他の構成は同一である。
図３において、変位センサ７の測定方向はｙ方向で、変位センサ８と変位センサ９との測定方向はｘ方向であり、変位センサ８のターゲット測定点Ｅと変位センサ９のターゲット測定点Ｆとを通る直線がｙ軸に平行であるように３つの一次元センサを設置する。
変位センサ７、変位センサ８および変位センサ９のターゲット測定点の移動座標をそれぞれＤ（Ｘ_D，Ｙ_D，Ｚ_D）、Ｅ（Ｘ_E，Ｙ_E，Ｚ_E）およびＦ（Ｘ_F，Ｙ_F，Ｚ_F）とすると、
Ｘ_E＝Ｘ_F， Ｚ_E＝Ｚ_F … （４４）
が成り立つ。
以下、各センサからの情報および各センサの初期位置に基づいて移動子が変位した後の移動座標系の原点位置（ｘ_O，ｙ_O，ｚ_O）および姿勢（α，β，γ）を求める。

まず、実施例１と同じように３つのギャップセンサ１、２、３の検出情報および初期位置情報に基づいてαおよびβを求める。
次に、求めたα、βの値、３つの変位センサ７、８、９の検出情報および初期位置情報に基づいてγ、ｘ_Oおよびｙ_Oを求める。
変位センサ８および変位センサ９が検出した変位量をそれぞれＳ_EおよびＳ_Fとすると、変位後の点Ｅおよび点Ｆのｘ絶対座標はそれぞれ
ｘ_E＝Ｘ_E＋Ｓ_E， ｘ_F＝Ｘ_F＋Ｓ_F …（４５）
で求められる。
式（１）によると、
ｘ_E＝ｘ_O＋（Ｇ₁₁Ｘ_E＋Ｇ₁₂Ｙ_E＋Ｇ₁₃Ｚ_E） …（４６）
および
ｘ_F＝ｘ_O＋（Ｇ₁₁Ｘ_F＋Ｇ₁₂Ｙ_F＋Ｇ₁₃Ｚ_F） …（４７）
となる。
式（４４）を考慮して式（４６）から式（４７）を引くと、
ｘ_E−ｘ_F＝Ｇ₁₂（Ｙ_E−Ｙ_F） … （４８）
となる。
Ｇ₁₂＝ｃｏｓβｓｉｎγを上式に代入し、γを次式のように与えられる。
γ＝ａｒｃｓｉｎ｛（ｘ_E−ｘ_F）／（Ｙ_E−Ｙ_F）／ｃｏｓβ｝…（４９）
式（４６）により、ｘ_Oを次式のように求めることができる。
ｘ_O＝ｘ_E−（Ｇ₁₁Ｘ_E＋Ｇ₁₂Ｙ_E＋Ｇ₁₃Ｚ_E） …（５０）
また、変位センサ７が検出した変位量をＳＤとすると、移動子が変位した後点Ｄのｙ絶対座標は
ｙ_D＝Ｙ_D＋Ｓ_D …（５１）
で求められる。
式（１）により、ｙ_Oを次式のように求めることができる。
ｙ_O＝ｙ_D−（Ｇ₁₁Ｘ_D＋Ｇ₁₂Ｙ_D＋Ｇ₁₃Ｚ_D） …（５２）
最後に、実施例１と同じようにｚ_Oを計算する。

このように、３つギャップセンサと３つ変位センサを用いることで、簡単かつ正確に移動子変位後位置・姿勢を計測することができる。本実施例は実施例２と較べ、センサの数と種類が全く同じであるが、ｘ方向の移動範囲が大きい場合に実施例２の方が計測に使われるスペースが小さく、ｙ方向の移動範囲が大きい場合に本実施例の方が計測に使われるスペースが小さい。

次に、本発明の実施例３について図を参照して説明する。
本実施例は、実施例１において、ギャップセンサの設置方法を変えたものである。
図４は、本発明の実施例４における６自由度移動体の位置・姿勢計測装置の構成を示す図である。
図４において、移動座標系と固定座標系との設置方法は実施例１と同じになっている。３つのギャップセンサ１、２、３は、すべてのヘッドが移動子に固定され、すべてのターゲットが固定子に固定されるようになっている。
また、ターゲット平面が全て平面ｘｏｙに平行であり、点Ｈ₁と点Ｈ₂とが平面ＹＯＺに対して対称であり、点Ｈ₂と点Ｈ₃とが平面ＸＯＺに対して対称である。なお、ベクトルＨ₁Ｔ₁、Ｈ₂Ｔ₂およびＨ₃Ｔ₃が全てｚ軸の負方向と一致する。従って、点Ｈ₁、Ｈ₂およびＨ₃の移動座標をそれぞれ（Ｘ_H1，Ｙ_H1，Ｚ_H1）、（Ｘ_H2，Ｙ_H2，Ｚ_H2）および（Ｘ_H3，Ｙ_H3，Ｚ_H3）とすると、
Ｘ_H2＝Ｘ_H3 …（５３）
Ｙ_H1＝Ｙ_H2 …（５４）
および
Ｚ_H1＝Ｚ_H2＝Ｚ_H3 …（５５）
が成り立つ。
固定座標系のｚ軸、ｙ軸、ｘ軸の順で回転座標変換行い、対応する移動子の回転角度を（γ，β，α）とし、座標変換行列をＧとし、また、移動子が変位した後の移動座標系の原点位置（ｘ_O，ｙ_O，ｚ_O）とすると、移動座標が（Ｘ，Ｙ，Ｚ）である点の絶対座標（ｘ，ｙ，ｚ）を次式で表すことができる。

ただし、Ｈ₁₁＝ｃｏｓβｃｏｓγ，
Ｈ₁₂＝ｓｉｎαｓｉｎβｃｏｓγ−ｃｏｓαｓｉｎγ，
Ｈ₁₃＝ｃｏｓαｓｉｎβｃｏｓγ＋ｓｉｎαｓｉｎγ，
Ｈ₂₁＝ｃｏｓβｓｉｎγ，
Ｈ₂₂＝ｓｉｎαｓｉｎβｓｉｎγ＋ｃｏｓαｃｏｓγ，
Ｈ₂₃＝ｃｏｓαｓｉｎβｓｉｎγ−ｓｉｎαｃｏｓγ，
Ｈ₃₁＝−ｓｉｎβ，
Ｈ₃₂＝ｓｉｎαｃｏｓβ，
Ｈ₃₃＝ｃｏｓαｃｏｓβ．
以下、各センサからの検出情報および各センサの初期位置に基づいて移動子が変位した後の移動座標系の原点位置（ｘ_O，ｙ_O，ｚ_O）および姿勢（α，β，γ）を求める。
まず、３つのギャップセンサの検出情報および初期位置情報に基づいてαおよびβを求める。
ギャップセンサ１、ギャップセンサ２およびギャップセンサ３のターゲット平面の方程式をそれぞれ
ｚ＝ｚ_T1， ｚ＝ｚ_T2， ｚ＝ｚ_T3 …（５７）
とし、ギャップセンサ１、ギャップセンサ２およびギャップセンサ３が検出したギャップ信号をそれぞれｄ₁、ｄ₂およびｄ₃とすると、変位後点Ｈ₁、Ｈ₂およびＨ₃のｚ絶対座標はそれぞれ
ｚ_H1＝ｚ_T1＋ｄ₁， ｚ_H2＝ｚ_T2＋ｄ₂， ｚ_H3＝ｚ_T3＋ｄ₃ …（５８）
となる。
一方、式（５６）により、
ｚ_H1＝ｚ_O＋Ｘ_H1Ｈ₃₁＋Ｙ_H1Ｈ₃₂＋Ｚ_H1Ｈ₃₃ …（５９）
ｚ_H2＝ｚ_O＋Ｘ_H2Ｈ₃₁＋Ｙ_H2Ｈ₃₂＋Ｚ_H2Ｈ₃₃ …（６０）
および
ｚ_H3＝ｚ_O＋Ｘ_H3Ｈ₃₁＋Ｙ_H3Ｈ₃₂＋Ｚ_H3Ｈ₃₃ …（６１）
が成り立つ。
式（５３）〜式（５５）を考慮して式（５９）から式（６０）を引き、式（６０）から式（６１）を引くと、
ｚ_H1−ｚ_H2＝（Ｘ_H1−Ｘ_H2）Ｈ₃₁ …（６２）
および
ｚ_H2−ｚ_H3＝（Ｙ_H2−Ｙ_H3）Ｈ₃₂ …（６３）
となる。
Ｈ３１＝−ｓｉｎβおよびＨ₃₂＝ｓｉｎαｃｏｓβを上の２式に代入すると、αおよびβは次のように与えられる。
β＝ａｒｃｓｉｎ｛（ｚ_H2−ｚ_H1）／（Ｘ_H1−Ｘ_H2）｝ …（６４）
α＝ａｒｃｓｉｎ｛（ｚ_H2−ｚ_H3）／（Ｙ_H2−Ｙ_H3）／ｃｏｓβ｝…（６５）
また、ギャップセンサ１、ギャップセンサ２およびギャップセンサ３のターゲット平面がすべて同じ平面にあるようになっていれば、
ｚ_T1＝ｚ_T2＝ｚ_T3
が成り立つので、式（５８）により、
β＝ａｒｃｓｉｎ｛（ｄ₂−ｄ₁）／（Ｘ_H1−Ｘ_H2）｝ …（６６）
α＝ａｒｃｓｉｎ｛（ｄ₂−ｄ₃）／（Ｙ_H2−Ｙ_H3）／ｃｏｓβ｝…（６７）
となる。

ｚ_T1、ｚ_T2、ｚ_T3、Ｘ_H1、Ｘ_H2およびＸ_H3は一定なので予め測定しておけば、式（５８）および式（６４）〜（６７）により、ギャップセンサ１、ギャップセンサ２およびギャップセンサ３が検出したギャップ信号ｄ₁、ｄ₂およびｄ₃に基づいてαおよびβを求めることができる。
次に、求めたα、βの値、２つの二次元センサの検出情報および初期位置情報に基づいてγ、ｘ_Oおよびｙ_Oを求める。
点Ｐおよび点Ｑの絶対座標をそれぞれ（ｘ_P，ｙ_P，ｚ_P）および（ｘ_Q，ｙ_Q，ｚ_Q）とし、式（５６）によると、

および

となる。
Ｚ_P＝Ｚ_Qを考慮して式（６８）から式（６９）を引くと、

となる。
また、ｘ_PQ＝ｘ_P−ｘ_Q，Ｘ_PQ＝Ｘ_P−Ｘ_Q，ｙ_PQ＝ｙ_P−ｙ_Q
および、Ｙ_PQ＝Ｙ_P−Ｙ_Qとし、
Ｈ₁₁＝ｃｏｓβｃｏｓγ，
Ｈ₁₂＝ｓｉｎαｓｉｎβｃｏｓγ−ｃｏｓαｓｉｎγ，
Ｈ₂₁＝ｃｏｓβｓｉｎγ、
および、Ｈ₂₂＝ｓｉｎαｓｉｎβｓｉｎγ＋ｃｏｓαｃｏｓγを
上の２式に代入し、整理すると、

となる。この方程式を解くと、

となる。従って、γを次式のように与えられる。

上式中、ｘ_PQはｘ_Pとｘ_Qとの差、ｙ_PQはｙ_Pとｙ_Qとの差である。
また、（ｘ_P，ｙ_P）と（ｘ_Q，ｙ_Q）はそれぞれ点Ｐと点Ｑの変位後の絶対座標であり、直接センサから得られない。
しかし、（Ｘ_P，Ｙ_P）と（Ｘ_Q，Ｙ_Q）は既知であり、二次元センサ５が測定した点Ｐの変位量を（Ｓ_Px，Ｓ_Py）とし、二次元センサ６が測定した点Ｑの変位量を（Ｓ_Qx，Ｓ_Qy）とすると、点Ｐと点Ｑの変位後の絶対座標は次のように与えられる。
（ｘ_P，ｙ_P）＝（Ｘ_P，Ｙ_P）＋（Ｓ_Px，Ｓ_Py） …（７４）
（ｘ_Q，ｙ_Q）＝（Ｘ_Q，Ｙ_Q）＋（Ｓ_Qx，Ｓ_Qy） …（７５）
以上のようにα、β、γの値が決められると、式（５６）より、回転座標変換行列Ｈの全ての要素Ｈ_ij（ｉ＝１，２，３、ｊ＝１，２，３）は決められる。
点Ｐの移動座標（Ｘ_P，Ｙ_P，Ｚ_P）および変位後の点Ｐのｘｙ絶対座標（ｘ_P，ｙ_P）を式（５６）に代入し変形すると、変位後移動座標の原点のｘｙ絶対座標は次のように与えられる。
ｘ_O＝ｘ_P−（Ｈ₁₁Ｘ_P＋Ｈ₁₂Ｙ_P＋Ｈ₁₃Ｚ_P） …（７６）
ｙ_O＝ｙ_P−（Ｈ₂₁Ｘ_P＋Ｈ₂₂Ｙ_P＋Ｈ₂₃Ｚ_P） …（７７）
最後に、式（５９）により、ｚ_Oを次式のように求めることができる。
ｚ_O＝ｚ_H1−（Ｘ_H1Ｈ₃₁＋Ｙ_H1Ｈ₃₂＋Ｚ_H1Ｈ₃₃） …（７８）

このように、実施例１と較べ、ギャップセンサのヘッドが移動子上に固定されているため、信号検出ための配線が移動子の運動の妨げになるが、計算は簡単になる。

次に、本発明の実施例５について図を参照して説明する。
本実施例は、実施例２において、ギャップセンサの設置方法を変えたものである。
図５は、本発明の実施例５における６自由度移動体の位置・姿勢計測装置の構成を示す図である。
図５において、移動座標系と固定座標系との設置方法およびギャップセンサの設置方法は実施例４と同じようになっている。
一方、３つの変位センサの設置方法は実施例２と同じようになっている。また、座標変換は実施例４と同じように行う。
以下、各センサからの情報および各センサの初期位置に基づいて移動子が変位した後の移動座標系の原点位置（ｘ_O，ｙ_O，ｚ_O）および姿勢（α，β，γ）を求める。
まず、実施例４と同じように３つのギャップセンサ１、２、３の検出情報および初期位置情報に基づいてαおよびβを求める。
次に、求めたα、βの値、３つの変位センサの検出情報および初期位置情報に基づいてγ、ｘ_Oおよびｙ_Oを求める。

変位センサ７および変位センサ８が検出した変位量をそれぞれＳ_DおよびＳ_Eとすると、移動子が変位した後点Ｄおよび点Ｅのｙ絶対座標はそれぞれ
ｙ_D＝Ｙ_D＋Ｓ_D， ｙ_E＝Ｙ_E＋Ｓ_E …（７９）
で求められる。また、式（５６）によると、
ｙ_D＝ｙ_O＋（Ｈ₂₁Ｘ_D＋Ｈ₂₂Ｙ_D＋Ｈ₂₃Ｚ_D） …（８０）
および
ｙ_E＝ｙ_O＋（Ｈ₂₁Ｘ_E＋Ｈ₂₂Ｙ_E＋Ｈ₂₃Ｚ_E） …（８１）
が成り立つ。式（３５）を考慮して式（８０）から式（８１）を引くと、
ｙ_D−ｙ_E＝Ｈ₂₁（Ｘ_D−Ｘ_E） …（８２）
となる。Ｈ₂₁＝ｃｏｓβｓｉｎγを上式に代入してγを求めると、
γ＝ａｒｃｓｉｎ｛（ｙ_D−ｙ_E）／（Ｘ_D−Ｘ_E）／ｃｏｓβ｝ …（８３）
となる。
また、変位センサ９が検出した変位量をＳＦとすると、移動子が変位した後点Ｆのｘ絶対座標は
ｘ_F＝Ｘ_F＋Ｓ_F …（８４）
で求められる。式（５６）により、ｘ_Oは次式のように与えられる。
ｘ_O＝ｘ_F−（Ｈ₁₁Ｘ_F＋Ｈ₁₂Ｙ_F＋Ｈ₁₃Ｚ_F） … （８５）
また、式（８０）により、ｙ_Oは次式のように与えられる。
ｙ_O＝ｙ_D−（Ｈ₂₁Ｘ_D＋Ｈ₂₂Ｙ_D＋Ｈ₂₃Ｚ_D） …（８６）
最後に、実施例４と同じようにｚ_Oを計算する。

このように、実施例２と較べ、ギャップセンサのヘッドが移動子上に固定されているため、信号検出ための配線が移動子の運動の妨げになるが、計算は簡単になる。また、実施例４と較べ、センサが１つ増えるので設置場所を大きく取る必要になるが、変位センサが二次元センサより廉価であるためコストを下げることができる。

次に、本発明の実施例６について図を参照して説明する。
実施例６は、実施例３において、ギャップセンサの設置方法を変えたものである。
図６は、本発明の実施例６における６自由度移動体の位置・姿勢計測装置の構成を示す図である。
図６において、移動座標系と固定座標系との設置方法およびギャップセンサの設置方法は実施例４と同じようになっている。
一方、３つの変位センサの設置方法は実施例３と同じようになっている。また、座標変換は実施例４と同じように行う。
以下、各センサからの情報および各センサの初期位置に基づいて移動子が変位した後の移動座標系の原点位置（ｘ_O，ｙ_O，ｚ_O）および姿勢（α，β，γ）を求める。
まず、実施例４と同じように３つのギャップセンサ１、２、３の検出情報および初期位置情報に基づいてαおよびβを求める。
次に、求めたα、βの値、３つの変位センサの検出情報および初期位置情報に基づいてγ、ｘ_Oおよびｙ_Oを求める。
変位センサ８および変位センサ９が検出した変位量をそれぞれＳ_EおよびＳ_Fとすると、変位後の点Ｅおよび点Ｆのｘ絶対座標はそれぞれ
ｘ_E＝Ｘ_E＋Ｓ_E， ｘ_F＝Ｘ_F＋Ｓ_F …（８７）
で求められる。式（５６）によると、
ｘ_E＝ｘ_O＋（Ｈ₁₁Ｘ_E＋Ｈ₁₂Ｙ_E＋Ｈ₁₃Ｚ_E） … （８８）
および
ｘ_F＝ｘ_O＋（Ｈ₁₁Ｘ_F＋Ｈ₁₂Ｙ_F＋Ｈ₁₃Ｚ_F） …（８９）
となる。式（４４）を考慮して式（８８）から式（８９）を引くと、
ｘ_E−ｘ_F＝Ｈ₁₂（Ｙ_E−Ｙ_F） …（９０）
となる。
Ｈ₁₂＝ｓｉｎαｓｉｎβｃｏｓγ−ｃｏｓαｓｉｎγ
を上式に代入してγを求めると、
γ＝Ａ−ａｒｃｓｉｎ｛ｋ（ｙ_E−ｙ_F）／（Ｘ_E−Ｘ_F）｝ …（９１）
となる。
ただし、Ａ＝ａｒｃｓｉｎ（ｋｓｉｎαｓｉｎβ），
ｋ＝１／√（ｓｉｎ２αｓｉｎ２β＋ｃｏｓ２α）．

式（８８）により、ｘ_Oを次式のように求めることができる。
ｘ_O＝ｘ_E−（Ｈ₁₁Ｘ_E＋Ｈ₁₂Ｙ_E＋Ｈ₁₃Ｚ_E） …（９２）
また、変位センサ７が検出した変位量をＳＤとすると、移動子が変位した後点Ｄのｙ絶対座標は
ｙ_D＝Ｙ_D＋Ｓ_D …（９３）
で求められる。
式（５６）により、ｙ_Oを次式のように求めることができる。
ｙ_O＝ｙ_D−（Ｈ₁₁Ｘ_D＋Ｈ₁₂Ｙ_D＋Ｈ₁₃Ｚ_D） …（９４）
最後に、実施例４と同じようにｚ_Oを計算する。

このように、３つのギャップセンサと３つの変位センサを用いることで、簡単かつ正確に移動子変位後位置・姿勢を計測することができる。
本実施例は実施例５と較べ、センサの数と種類が全く同じであるが、ｘ方向の移動範囲が大きい場合に実施例５の方が計測に使われるスペースが小さく、ｙ方向の移動範囲が大きい場合に本実施例の方が計測に使われるスペースが小さい。

次に、本発明の実施例７について図を参照して説明する。
実施例７は、実施例１又は実施例２又は実施例３又は実施例４又は実施例５又は実施例６のいずれかにおいて、１つのギャップセンサを追加した例である。
図７は、本発明の実施例７における６自由度移動体の位置・姿勢計測装置の構成を示す図である。
図７において、ギャップセンサ１、ギャップセンサ２およびギャップセンサ３は実施例１又は実施例２又は実施例３又は実施例４又は実施例５又は実施例６のいずれかと同じように設置され、ギャップセンサ４はギャップセンサ１、ギャップセンサ２およびギャップセンサ３と座標系に対して対称であるように設置されている。
実施例１又は実施例２又は実施例３又は実施例４又は実施例５又は実施例６では、３つのギャップセンサの情報に基づいてαおよびβを求めた。ここでは、４つのギャップセンサの中から任意の３つの情報に基づいて実施例１と全く同じ方法でαおよびβを求める。 これで、αおよびβに関する４通りのデータを得ることができる。また、４つのギャップセンサとすべてのアクチュエータは移動子の構造に対して対称であれば、４通りのデータを平均化することで移動子の曲げモードによる機械振動成分を打ち消すことができる。

このように、本実施例は実施例１又は実施例２又は実施例３又は実施例４又は実施例５又は実施例６と較べ、１つセンサが増えるが、４通りのデータを平均化することでαおよびβの計測精度をあげることができる。また、この４通りのデータを用いて検出エラーの判断を行うことも可能である。

次に、本発明の実施例８について図を参照して説明する。
図８は、本発明の実施例８における６自由度移動体の位置・姿勢計測装置の構成を示す図である。
図８において、２１は移動子であり、アクチュエータ（不図示）に駆動されて６自由度の位置・姿勢変位する。１０は三次元センサ（第１）であり、１つの方向の変位量と２つの回転角度を検出する。また、１１は三次元センサ（第２）であり、直交２つの方向の変位量と１つの回転角度を検出する。
実施例１と同じように絶対座標系と固定座標系を設定し、座標変換を行う。以下、２つの三次元センサ１０、１１の設置方法および６自由度移動体の位置・姿勢計測方法について詳細に説明する。
三次元センサ１０は、ターゲット平面が平面ＸＯＹに平行であるようにターゲットが移動子に固定され、常にｚ軸に平行である１つの固定直線（ここで測定線と呼ぶ）とターゲット平面との交点Ｍのｚ方向の変位量Ｓ_Mz、ｘ軸のまわりの移動子の回転角度αおよびｙ軸のまわりの移動子の回転角度βを検出する。
また、三次元センサ１１は、ターゲットが移動子に固定され、ターゲット測定点Ｎのｘ方向変位量Ｓ_Nxとｙ方向変位量Ｓ_Nyおよびｚ軸のまわりの移動子の回転角度γを検出する。
以上のように、姿勢（α，β，γ）の情報は直接２つの三次元センサ１０、１１から得られた。
以下、移動子が変位した後の移動座標系の原点位置（ｘ_O，ｙ_O，ｚ_O）を求める。

まず、ｘ_Oとｙ_Oを求める。
点Ｎのｘ方向とｙ方向との変位量（Ｓ_Nx，Ｓ_Ny）および点Ｎのｘ方向とｙ方向との初期絶対座標（Ｘ_N，Ｙ_N）から、変位後点Ｎの絶対座標を次のように与えられる。
（ｘ_N，ｙ_N）＝（Ｘ_N，Ｙ_N）＋（Ｓ_Nx，Ｓ_Ny） …（９５）
また、α、β、γの値が検出されると、式（１）より、回転座標変換行列Ｇの全ての要素Ｇ_ij（ｉ＝１，２，３、ｊ＝１，２，３）が決定され、そして、点Ｎの移動座標（Ｘ_N，Ｙ_N，Ｚ_N）が既知なので、式（１）により、変位後移動座標の原点Ｏのｘｙ絶対座標は次のように与えられる。
ｘ_O＝ｘ_N−（Ｇ₁₁Ｘ_N＋Ｇ₁₂Ｙ_N＋Ｇ₁₃Ｚ_N） …（９６）
ｙ_O＝ｙ_N−（Ｇ₂₁Ｘ_N＋Ｇ₂₂Ｙ_N＋Ｇ₂₃Ｚ_N） …（９７）
次に、ｚ_Oを求める。
三次元センサ１０の測定点Ｍの初期絶対座標を（ｘ_M0，ｙ_M0，ｚ_M0）とすると、変位前三次元センサ１０のターゲット平面の方程式は
ｚ＝ｚ_M0 …（９８）
となる。
また、点Ｍを通る三次元センサ１０の測定線ＭＬの方程式は
ｘ＝ｘ_M0， ｙ＝ｙ_M0 …（９９）
となる。
また、実施例１におけるギャップセンサ１のターゲット平面の変位した後の方程式を求めることと同じように三次元センサ１０のターゲット平面の変位した後の方程式は次式のように与えられる。
ｘＧ₁₃＋ｙＧ₂₃＋ｚＧ₃₃＝ｄ_O＋ｚ_M0 …（１００）
となる。ただし、
ｄ_O＝ｘ_OＧ₁₃＋ｙ_OＧ₂₃＋ｚ_OＧ₃₃ …（１０１）
である。従って、移動子が変位した後の点Ｍのｚ絶対座標は、
ｚ_M＝（ｄ_O＋ｚ_M0−ｘ_M0Ｇ₁₃−ｙ_M0Ｇ₂₃）／Ｇ₃₃ …（１０２）
となる。
よって、三次元センサ１０が検出した点Ｍのｚ方向の変位量は
Ｓ_Mz≡ｚ_M−ｚ_M0
＝（ｄ_O＋ｚ_M0−ｘ_M0Ｇ₁₃−ｙ_M0Ｇ₂₃）／Ｇ₃₃−ｚ_M0 …（１０３）
となる。
式（１０１）を式（１０３）に代入してｚ_Oを求めると、
ｚ_O＝Ｓ_Mz＋ｚ_M0―｛ｚ_M0＋（ｘ₀−ｘ_M0）Ｇ₁₃＋（ｙ₀−ｙ_M0）Ｇ₂₃｝／Ｇ₃₃ …（１０４）
となる。

このように、２つ三次元センサの検出情報および初期位置情報に基づいて正確に移動子が変位した後の位置・姿勢を計測することができる。

次に、本発明の実施例９について図を参照して説明する。
実施例９は、実施例８と同様の図８の構成において、座標変換に式（１）でなく式（５６）を用いた場合の実施例である。姿勢（α，β，γ）および移動子が変位した後の移動座標系の原点位置（ｘ_O，ｙ_O）は実施例８と同様にして求める。このときのｚ_Oは以下のようにして求める。
移動子が変位した後の三次元センサ１０の測定点Ｍの絶対座標を（ｘ_M，ｙ_M，ｚ_M）、測定点Ｍの移動座標を（Ｘ_M，Ｙ_M，Ｚ_M）とする。
ｘ_M＝ｘ_M0，ｙ_M＝ｙ_M0であるから、式（５６）より以下の式が成り立つ。
ただし、Ｚ_Mは既知である。

式（１０５）の上の2行より

となる。これを解くと

となる。得られたＸ_M，Ｙ_Mよりｚ_Oを求める。
ｚ_O＝Ｓ_Mz＋ｚ_M0―Ｈ₃₁Ｘ_M―Ｈ₃₂Ｙ_M―Ｈ₃₃Ｚ_M …（１０８）

次に、本発明の実施例１０について図を参照して説明する。
実施例１０は、実施例３と同様の図３の構成において、座標変換に式（１）でなく式（５６）を用いた場合の実施例である。
６自由度移動体の位置・姿勢計測装置の構成、移動座標系と固定座標系との設置方法、ギャップセンサ１〜３、および３つの変位センサ７〜９の設置方法は実施例３と同じである。
以下、各センサからの情報および各センサの初期位置に基づいて移動子が変位した後の移動座標系の原点位置（ｘ_O，ｙ_O，ｚ_O）および姿勢（α，β，γ）を求める。
まず、実施例６と同じ方法でα、β、γ、ｘ_Oおよびｙ_Oを求める。
このときのｚ_Oは以下のようにして求める。
ギャップセンサ1のヘッド位置の絶対座標を（ｘ_H1，ｙ_H1，ｚ_H1）、移動子が変位した後のギャップセンサ１の測定点Ｔ１の絶対座標を（ｘ_T1，ｙ_T1，ｚ_T1）、測定点Ｔ₁の移動座標を（Ｘ_T1，Ｙ_T1，Ｚ_T1）とする。
図３の構成では、以下の式が成り立つ。
ｘ_T1＝ｘ_H1 …（１０９）
ｙ_T1＝ｙ_H1 …（１１０）
ｚ_T1＝ｚ_H1＋ｄ₁ …（１１１）
ただし、ｘ_H1、ｙ_H1、Ｚ_T1は移動しない既知の座標である。
式（５６）より、以下の式が成り立つ。

式（１１２）の上の2行より

となる。これを解くと、

となる。
得られたＸ_T1，Ｙ_T1を用いて、以下の式によりｚ_Oを求める。
ｚ_O＝ｚ_H1＋ｄ₁―Ｈ₃₁Ｘ_T1―Ｈ₃₂Ｙ_T1―Ｈ₃₃Ｚ_T1 …（１１５）

本発明の技術は、複数センサの検出情報および初期位置情報に基づいて６自由度変位する移動子の位置・姿勢を正確に計測することができるので、多自由度かつ高精度な位置・姿勢制御を行う制御装置のフィードバック信号を計測するという用途にも適応できる。

本発明の実施例１における６自由度移動体の位置・姿勢計測装置の構成を示す図である。 本発明の実施例２における６自由度移動体の位置・姿勢計測装置の構成を示す図である。 本発明の実施例３における６自由度移動体の位置・姿勢計測装置の構成を示す図である。 本発明の実施例４における６自由度移動体の位置・姿勢計測装置の構成を示す図である。 本発明の実施例５における６自由度移動体の位置・姿勢計測装置の構成を示す図である。 本発明の実施例６における６自由度移動体の位置・姿勢計測装置の構成を示す図である。 本発明の実施例７における６自由度移動体の位置・姿勢計測装置のギャップセンサの配置を示す図である。 本発明の実施例８における６自由度移動体の位置・姿勢計測装置の構成を示す図である。 従来技術の６自由度移動体の位置・姿勢計測装置の構成を示す図である。

符号の説明

１、２、３、４ ギャップセンサ
５、６ 二次元センサ
７、８、９ 変位センサ
１０、１１ 三次元センサ
２１ 移動子
２２ 固定子

Claims (8)

1. 固定子と、前記固定子に対して６自由度変位する移動子と、複数のセンサとを備えた６自由度移動体の位置・姿勢計測方法において、
   移動座標系と固定座標系に基づいて前記固定子および前記移動子に複数のセンサを設置し、該複数のセンサの検出情報および与えられた初期位置情報に基づいて前記移動子が変位した後の該移動子の前記固定子に対する位置および姿勢を計算して前記移動子上の任意点の位置を求める際に、固定座標系ｘｙｚｏを前記固定子に固定し、移動座標系ＸＹＺＯを前記移動子に固定し、前記移動子が変位する前に移動座標系と固定座標系を重ねるように移動座標系と固定座標系を設定し、
   ｘ軸、ｙ軸およびｚ軸のまわりの前記移動子の回転角度をそれぞれα、βおよびγとし、移動座標系の原点の絶対座標を（ｘ_o，ｙ_o，ｚ_o）とし、前記移動子上の任意固定点の絶対座標をこの任意固定点の移動座標と移動座標系の原点の絶対座標（ｘ_o，ｙ_o，ｚ_o）および回転角度α、βおよびγで表すように座標変換して計算する、という手順で処理することを特徴とする６自由度移動体の位置・姿勢計測方法。
2. 前記複数のセンサは、ギャップセンサのヘッドと該ギャップセンサのターゲット平面との距離を計る第１のギャップセンサ、第２のギャップセンサおよび第３のギャップセンサを含み、回転座標変換を固定座標系のｘ軸、ｙ軸、ｚ軸の順で行い、前記第１のギャップセンサ、前記第２のギャップセンサおよび前記第３のギャップセンサに対して、すべてのヘッドが前記固定子に固定されて平面ｙｏｚ又は平面ｘｏｚに対して対称であり、すべてのターゲット平面が前記移動子に固定されて平面ＸＯＹに平行であるように前記第１のギャップセンサ、前記第２のギャップセンサ２および前記第３のギャップセンサ３を設置することを特徴とする請求項１記載の６自由度移動体の位置・姿勢計測方法。
3. 前記複数のセンサは、直交２方向の変位量を検出する第１の二次元センサおよび第２の二次元センサを含み、前記第１の二次元センサと前記第２の二次元センサに対して、すべてのヘッドが前記固定子に固定され、すべてのターゲットが前記移動子に固定され、すべてのターゲット側における初期測定点のＺ移動座標が同じであり、前記第１の二次元センサの２つの測定方向と前記第２の二次元センサの２つの測定方向が共にｘ軸又はｙ軸に平行であるように前記第１の二次元センサと前記第２の二次元センサを設置することを特徴とする請求項１記載の６自由度移動体の位置・姿勢計測方法。
4. 前記複数のセンサは、直線方向の変位量を検出する第１の一次元センサ、第２の一次元センサおよび第３の一次元センサを含み、前記第１の一次元センサ、前記第２の一次元センサおよび前記第３の一次元センサに対して、前記第１の一次元センサと前記第２の一次元センサの測定方向が共にｙ方向で、前記第３の一次元センサの測定方向がｘ方向であり、前記第１の一次元センサのターゲット側の初期測定点と前記第２の一次元センサのターゲット側の初期測定点とを通る直線がｘ軸に平行であるように前記第１の一次元センサ、前記第２の一次元センサおよび前記第３の一次元センサを設置することを特徴とする請求項１記載の６自由度移動体の位置・姿勢計測方法。
5. 前記複数のセンサは、直線方向の変位量を検出する第１の一次元センサ、第２の一次元センサおよび第３の一次元センサを含み、前記第１の一次元センサ、前記第２の一次元センサおよび前記第３の一次元センサに対して、前記第１の一次元センサの測定方向がｙ方向で、前記第２の一次元センサの測定方向と前記第３の一次元センサの測定方向が共にｘ方向であり、前記第２の一次元センサのターゲット側の初期測定点と前記第３の一次元センサのターゲット側の初期測定点とを通る直線がｙ軸に平行であるように前記第１の一次元センサ、前記第２の一次元センサおよび前記第３の一次元センサを設置することを特徴とする請求項１記載の６自由度移動体の位置・姿勢計測方法。
6. 前記複数のセンサは、ギャップセンサのヘッドとギャップセンサのターゲット平面との距離を計る第１のギャップセンサ、第２のギャップセンサおよび第３のギャップセンサ３を含み、前記第１のギャップセンサ、前記第２のギャップセンサおよび前記第３のギャップセンサに対して、すべてのヘッドが前記移動子に固定されて平面ＹＯＺ又は平面ＸＯＺに対して対称であり、すべてのターゲット平面が前記固定子に固定されて平面ｘｏｙに平行であるように前記第１のギャップセンサ、前記第２のギャップセンサおよび前記第３のギャップセンサを設置することを特徴とする請求項１記載の６自由度移動体の位置・姿勢計測方法。
7. 請求項２又は請求項６のいずれかに記載の６自由度移動体の位置・姿勢計測装置に、第４のギャップセンサを追加した６自由度移動体の位置・姿勢計測方法において、
   前記第１のギャップセンサ、前記第２のギャップセンサ、前記第３のギャップセンサおよび前記第４のギャップセンサが座標系に対して対称になるように前記第４のギャップセンサを設置し、前記第１のギャップセンサ、前記第２のギャップセンサ、前記第３のギャップセンサおよび前記第４のギャップセンサから任意の３つギャップセンサの検出情報および与えられた初期位置情報に基づいて前記移動子が変位した後の該移動子の前記固定子に対する位置および姿勢を計算し、このように得られた４組のデータの平均値を取ることを特徴とする６自由度移動体の位置・姿勢計測方法。
8. ６自由度変位する移動子と、１つの方向の変位量および２つの回転角度を検出する第１の三次元センサと、直交２つの方向の変位量および１つの回転角度を検出する第２の三次元センサとを備えた６自由度移動体の位置・姿勢計測方法において、
   固定座標系ｘｙｚｏを大地に固定し、移動座標系ＸＹＺＯを移動子に固定し、前記移動子が変位する前に移動座標系と固定座標系を重ねるように移動座標系と固定座標系を設定し、ｘ軸、ｙ軸およびｚ軸のまわりの前記移動子の回転角度をそれぞれα、βおよびγとし、移動座標系の原点の絶対座標を（ｘ_o，ｙ_o，ｚ_o）とし、前記移動子上の任意固定点の絶対座標をこの任意固定点の移動座標と移動座標系の原点の絶対座標（ｘ_o，ｙ_o，ｚ_o）および回転角度α、βおよびγで表すように座標変換し、前記第１の三次元センサがｚ軸に平行である固定直線と移動子に固定された前記第１の三次元センサのターゲット平面との交点の移動距離およびｘとｙ軸のまわりの前記移動子の回転角度αとβを検出し、前記第２の三次元センサが移動子に固定されたターゲット測定点におけるｘ方向の変位量およびｙ方向の変位量とｚ軸のまわりの前記移動子の回転角度γを検出するように前記第１の三次元センサと前記第２の三次元センサを設置し、前記第１の三次元センサと前記第２の三次元センサとの検出情報および与えられた初期位置情報に基づいて前記移動子が変位した後の前記移動子の前記固定子に対する位置および姿勢を計算するという手順で処理することを特徴とする６自由度移動体の位置・姿勢計測方法。

Images