(57)【要約】【課題】  複数のセンサを用いて簡単かつ正確に６自由度移動体の位置・姿勢を計測する。【解決手段】  固定子と、固定子に対して６自由度変位する移動子と、複数のセンサとを備えた６自由度移動体の位置・姿勢計測装置において、適当な移動座標系と固定座標系を設定し、一定順番の回転座標変換を含む座標変換を行い、複数のギャップセンサ１、２、３、及び二次元センサ５、６を適切な場所に設置し、複数のセンサの検出情報および与えられた初期位置情報に基づいて可動子が変位した後の位置および姿勢を計算する。【選択図】  図１
2006201092.tif 000002
【特許請求の範囲】
【請求項１】
固定子と、前記固定子に対して６自由度変位する移動子と、複数のセンサとを備えた６自由度移動体の位置・姿勢計測方法において、
移動座標系と固定座標系に基づいて前記固定子および前記移動子に複数のセンサを設置し、該複数のセンサの検出情報および与えられた初期位置情報に基づいて前記移動子が変位した後の該移動子の前記固定子に対する位置および姿勢を計算して前記移動子上の任意点の位置を求める際に、固定座標系ｘｙｚｏを前記固定子に固定し、移動座標系ＸＹＺＯを前記移動子に固定し、前記移動子が変位する前に移動座標系と固定座標系を重ねるように移動座標系と固定座標系を設定し、
ｘ軸、ｙ軸およびｚ軸のまわりの前記移動子の回転角度をそれぞれα、βおよびγとし、移動座標系の原点の絶対座標を（ｘo，ｙo，ｚo）とし、前記移動子上の任意固定点の絶対座標をこの任意固定点の移動座標と移動座標系の原点の絶対座標（ｘo，ｙo，ｚo）および回転角度α、βおよびγで表すように座標変換して計算する、という手順で処理することを特徴とする６自由度移動体の位置・姿勢計測方法。
【請求項２】
前記複数のセンサは、ギャップセンサのヘッドと該ギャップセンサのターゲット平面との距離を計る第１のギャップセンサ、第２のギャップセンサおよび第３のギャップセンサを含み、回転座標変換を固定座標系のｘ軸、ｙ軸、ｚ軸の順で行い、前記第１のギャップセンサ、前記第２のギャップセンサおよび前記第３のギャップセンサに対して、すべてのヘッドが前記固定子に固定されて平面ｙｏｚ又は平面ｘｏｚに対して対称であり、すべてのターゲット平面が前記移動子に固定されて平面ＸＯＹに平行であるように前記第１のギャップセンサ、前記第２のギャップセンサ２および前記第３のギャップセンサ３を設置することを特徴とする請求項１記載の６自由度移動体の位置・姿勢計測方法。
【請求項３】
前記複数のセンサは、直交２方向の変位量を検出する第１の二次元センサおよび第２の二次元センサを含み、前記第１の二次元センサと前記第２の二次元センサに対して、すべてのヘッドが前記固定子に固定され、すべてのターゲットが前記移動子に固定され、すべてのターゲット側における初期測定点のＺ移動座標が同じであり、前記第１の二次元センサの２つの測定方向と前記第２の二次元センサの２つの測定方向が共にｘ軸又はｙ軸に平行であるように前記第１の二次元センサと前記第２の二次元センサを設置することを特徴とする請求項１記載の６自由度移動体の位置・姿勢計測方法。
【請求項４】
前記複数のセンサは、直線方向の変位量を検出する第１の一次元センサ、第２の一次元センサおよび第３の一次元センサを含み、前記第１の一次元センサ、前記第２の一次元センサおよび前記第３の一次元センサに対して、前記第１の一次元センサと前記第２の一次元センサの測定方向が共にｙ方向で、前記第３の一次元センサの測定方向がｘ方向であり、前記第１の一次元センサのターゲット側の初期測定点と前記第２の一次元センサのターゲット側の初期測定点とを通る直線がｘ軸に平行であるように前記第１の一次元センサ、前記第２の一次元センサおよび前記第３の一次元センサを設置することを特徴とする請求項１記載の６自由度移動体の位置・姿勢計測方法。
【請求項５】
前記複数のセンサは、直線方向の変位量を検出する第１の一次元センサ、第２の一次元センサおよび第３の一次元センサを含み、前記第１の一次元センサ、前記第２の一次元センサおよび前記第３の一次元センサに対して、前記第１の一次元センサの測定方向がｙ方向で、前記第２の一次元センサの測定方向と前記第３の一次元センサの測定方向が共にｘ方向であり、前記第２の一次元センサのターゲット側の初期測定点と前記第３の一次元センサのターゲット側の初期測定点とを通る直線がｙ軸に平行であるように前記第１の一次元センサ、前記第２の一次元センサおよび前記第３の一次元センサを設置することを特徴とする請求項１記載の６自由度移動体の位置・姿勢計測方法。
【請求項６】
前記複数のセンサは、ギャップセンサのヘッドとギャップセンサのターゲット平面との距離を計る第１のギャップセンサ、第２のギャップセンサおよび第３のギャップセンサ３を含み、前記第１のギャップセンサ、前記第２のギャップセンサおよび前記第３のギャップセンサに対して、すべてのヘッドが前記移動子に固定されて平面ＹＯＺ又は平面ＸＯＺに対して対称であり、すべてのターゲット平面が前記固定子に固定されて平面ｘｏｙに平行であるように前記第１のギャップセンサ、前記第２のギャップセンサおよび前記第３のギャップセンサを設置することを特徴とする請求項１記載の６自由度移動体の位置・姿勢計測方法。
【請求項７】
請求項２又は請求項６のいずれかに記載の６自由度移動体の位置・姿勢計測装置に、第４のギャップセンサを追加した６自由度移動体の位置・姿勢計測方法において、
前記第１のギャップセンサ、前記第２のギャップセンサ、前記第３のギャップセンサおよび前記第４のギャップセンサが座標系に対して対称になるように前記第４のギャップセンサを設置し、前記第１のギャップセンサ、前記第２のギャップセンサ、前記第３のギャップセンサおよび前記第４のギャップセンサから任意の３つギャップセンサの検出情報および与えられた初期位置情報に基づいて前記移動子が変位した後の該移動子の前記固定子に対する位置および姿勢を計算し、このように得られた４組のデータの平均値を取ることを特徴とする６自由度移動体の位置・姿勢計測方法。
【請求項８】
６自由度変位する移動子と、１つの方向の変位量および２つの回転角度を検出する第１の三次元センサと、直交２つの方向の変位量および１つの回転角度を検出する第２の三次元センサとを備えた６自由度移動体の位置・姿勢計測方法において、
固定座標系ｘｙｚｏを大地に固定し、移動座標系ＸＹＺＯを移動子に固定し、前記移動子が変位する前に移動座標系と固定座標系を重ねるように移動座標系と固定座標系を設定し、ｘ軸、ｙ軸およびｚ軸のまわりの前記移動子の回転角度をそれぞれα、βおよびγとし、移動座標系の原点の絶対座標を（ｘo，ｙo，ｚo）とし、前記移動子上の任意固定点の絶対座標をこの任意固定点の移動座標と移動座標系の原点の絶対座標（ｘo，ｙo，ｚo）および回転角度α、βおよびγで表すように座標変換し、前記第１の三次元センサがｚ軸に平行である固定直線と移動子に固定された前記第１の三次元センサのターゲット平面との交点の移動距離およびｘとｙ軸のまわりの前記移動子の回転角度αとβを検出し、前記第２の三次元センサが移動子に固定されたターゲット測定点におけるｘ方向の変位量およびｙ方向の変位量とｚ軸のまわりの前記移動子の回転角度γを検出するように前記第１の三次元センサと前記第２の三次元センサを設置し、前記第１の三次元センサと前記第２の三次元センサとの検出情報および与えられた初期位置情報に基づいて前記移動子が変位した後の前記移動子の前記固定子に対する位置および姿勢を計算するという手順で処理することを特徴とする６自由度移動体の位置・姿勢計測方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数センサを用いて６自由度移動体の位置・姿勢を計測する計測装置に関する。
【背景技術】
【０００２】
従来の６自由度移動体の位置・姿勢計測装置は、ｘ、ｙ、ｚの３方向に全部で６つ変位センサを設置し計測装置を構成している（例えば、特許文献１参照）。図９は従来技術の６自由度移動体の位置・姿勢計測装置の構成を示すブロック図である。
図９において、２１は移動子であり、アクチュエータ（不図示）に駆動されて６自由度の位置・姿勢変位する。３１はｘ方向変位センサであり、ｘ方向の変位量を計測する。３２と３３はｙ方向変位センサであり、ｙ方向の変位量およびｚ軸まわりの回転角を計測する。また、３４、３５および３６はｚ方向変位センサであり、ｚ方向の変位量およびｘ、ｙ軸のまわりの回転角度を計測する。
各軸のまわりの回転運動量が小さい場合は、回転が並進変位量の計測に与える影響が小さいためそれを無視でき、移動子が変位した後の位置・姿勢を簡単に求められる。
このように、従来技術の６自由度移動体の位置・姿勢計測方法は、ｘ、ｙ、ｚの３方向向きに設置した６つの変位センサ３１〜３６からの変位情報を用いて移動子２１が変位した後の位置・姿勢を計算するものである。
【特許文献１】特開平１０−１９４４５０号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、従来の６自由度移動体の位置・姿勢計測方法は、回転運動量が大きい又は計測精度が高く要求される場合に、移動子が変位した後の位置・姿勢を正確に計測することが困難であるため、要求された仕様を満足できないという問題があった。
そこで、本発明は、このような問題点に鑑みてなされたものであり、各種の適宜なセンサを適切な位置に設置し、適宜な座標変換を行うことで、簡単かつ正確に６自由度移動体の位置・姿勢を計測することができて、回転運動量が大きい場合や高い計測制度が要求される場合にも移動子が変位した後の位置・姿勢の正確な計測が可能な６自由度移動体の位置・姿勢計測方法および位置・姿勢計測装置を提供することを目的とする。
【課題を解決するための手段】
【０００４】
上記問題を解決するため、本発明は、次のように構成したのである。
請求項１に記載の発明は、固定子と、前記固定子に対して６自由度変位する移動子と、複数のセンサとを備えた６自由度移動体の位置・姿勢計測方法において、移動座標系と固定座標系に基づいて前記固定子および前記移動子に複数のセンサを設置し、該複数のセンサの検出情報および与えられた初期位置情報に基づいて前記移動子が変位した後の該移動子の前記固定子に対する位置および姿勢を計算して前記移動子上の任意点の位置を求める際に、固定座標系ｘｙｚｏを前記固定子に固定し、移動座標系ＸＹＺＯを前記移動子に固定し、前記移動子が変位する前に移動座標系と固定座標系を重ねるように移動座標系と固定座標系を設定し、ｘ軸、ｙ軸およびｚ軸のまわりの前記移動子の回転角度をそれぞれα、βおよびγとし、移動座標系の原点の絶対座標を（ｘo，ｙo，ｚo）とし、前記移動子上の任意固定点の絶対座標をこの任意固定点の移動座標と移動座標系の原点の絶対座標（ｘo，ｙo，ｚo）および回転角度α、βおよびγで表すように座標変換して計算するという手順を特徴としている。
また、請求項２に記載の発明は、前記複数のセンサは、ギャップセンサのヘッドとギャップセンサのターゲット平面との距離を計る第１のギャップセンサ、第２のギャップセンサおよび第３のギャップセンサを含み、回転座標変換を固定座標系のｘ軸、ｙ軸、ｚ軸の順で行い、前記第１のギャップセンサ、前記第２のギャップセンサおよび前記第３のギャップセンサに対して、すべてのヘッドが前記固定子に固定されて平面ｙｏｚ又は平面ｘｏｚに対して対称であり、すべてのターゲット平面が前記移動子に固定されて平面ＸＯＹに平行であるように前記第１のギャップセンサ、前記第２のギャップセンサおよび前記第３のギャップセンサを設置するという手順を特徴としている。
【０００５】
また、請求項３に記載の発明は、前記複数のセンサは、直交２方向の変位量を検出する第１の二次元センサおよび第２の二次元センサを含み、前記第１の二次元センサと前記第２の二次元センサに対して、すべてのヘッドが前記固定子に固定され、すべてのターゲットが前記移動子に固定され、すべてのターゲット側における初期測定点のＺ移動座標が同じであり、前記第１の二次元センサの２つ測定方向と前記第２の二次元センサの２つ測定方向が共にｘ軸又はｙ軸に平行であるように前記第１の二次元センサと前記第２の二次元センサを設置するという手順を特徴としている。
また、請求項４に記載の発明は、前記複数のセンサは、直線方向の変位量を検出する第１の一次元センサ、第２の一次元センサおよび第３の一次元センサを含み、前記第１の一次元センサ、前記第２の一次元センサおよび前記第３の一次元センサに対して、前記第１の一次元センサと前記第２の一次元センサの測定方向が共にｙ方向で、前記第３の一次元センサの測定方向がｘ方向であり、前記第１の一次元センサのターゲット側の初期測定点と前記第２の一次元センサのターゲット側の初期測定点とを通る直線がｘ軸に平行であるように前記第１の一次元センサ、前記第２の一次元センサおよび前記第３の一次元センサを設置するという手順を特徴としている。
また、請求項５に記載の発明は、前記複数のセンサは、直線方向の変位量を検出する第１の一次元センサ、第２の一次元センサおよび第３の一次元センサを含み、前記第１の一次元センサ、前記第２の一次元センサおよび前記第３の一次元センサに対して、前記第１の一次元センサの測定方向がｙ方向で、前記第２の一次元センサの測定方向と前記第３の一次元センサの測定方向が共にｘ方向であり、前記第２の一次元センサのターゲット側の初期測定点と前記第３の一次元センサのターゲット側の初期測定点とを通る直線がｙ軸に平行であるように前記第１の一次元センサ、前記第２の一次元センサおよび前記第３の一次元センサを設置するという手順を特徴としている。
【０００６】
また、請求項６に記載の発明は、前記複数のセンサは、ギャップセンサのヘッドとギャップセンサのターゲット平面との距離を計る第１のギャップセンサ、第２のギャップセンサおよび第３のギャップセンサを含み、前記第１のギャップセンサ、前記第２のギャップセンサおよび前記第３のギャップセンサに対して、すべてのヘッドが前記移動子に固定されて平面ＹＯＺ又は平面ＸＯＺに対して対称であり、すべてのターゲット平面が前記固定子に固定されて平面ｘｏｙに平行であるように前記第１のギャップセンサ、前記第２のギャップセンサおよび前記第３のギャップセンサを設置するという手順を特徴としている。
また、請求項７に記載の発明は、請求項２又は請求項６のいずれかに記載の６自由度移動体の位置・姿勢計測装置に第４のギャップセンサを追加した６自由度移動体の位置・姿勢計測装置において、
前記第１のギャップセンサ、前記第２のギャップセンサ、前記第３のギャップセンサおよび前記第４のギャップセンサが座標系に対して対称になるように前記第４のギャップセンサを設置し、前記第１のギャップセンサ、前記第２のギャップセンサ、前記第３のギャップセンサおよび前記第４のギャップセンサから任意の３つギャップセンサの検出情報および与えられた初期位置情報に基づいて前記移動子が変位した後の前記移動子の前記固定子に対する位置および姿勢を計算し、このように得られた４組のデータの平均値を取るという手順を特徴としている。
また、請求項８に記載の発明は、６自由度変位する移動子と、１つ方向の変位量および２つの回転角度を検出する第１の三次元センサと、直交２つの方向の変位量および１つの回転角度を検出する第２の三次元センサとを備えた６自由度移動体の位置・姿勢計測装置において、固定座標系ｘｙｚｏを大地に固定し、移動座標系ＸＹＺＯを移動子に固定し、前記移動子が変位する前に移動座標系と固定座標系を重ねるように移動座標系と固定座標系を設定し、ｘ軸、ｙ軸およびｚ軸のまわりの前記移動子の回転角度をそれぞれα、βおよびγとし、移動座標系の原点の絶対座標を（ｘo，ｙo，ｚo）とし、前記移動子上の任意固定点の絶対座標をこの任意固定点の移動座標と移動座標系の原点の絶対座標（ｘo，ｙo，ｚo）および回転角度α、βおよびγで表すように座標変換し、前記第１の三次元センサがｚ軸に平行である固定直線と移動子に固定された前記第１の三次元センサのターゲット平面との交点の移動距離およびｘとｙ軸のまわりの前記移動子の回転角度αとβを検出し、前記第２の三次元センサが移動子に固定されたターゲット測定点におけるｘ方向の変位量およびｙ方向の変位量とｚ軸のまわりの前記移動子の回転角度γを検出するように前記第１の三次元センサと前記第２の三次元センサを設置し、前記第１の三次元センサと前記第２の三次元センサとの検出情報および与えられた初期位置情報に基づいて前記移動子が変位した後の前記移動子の前記固定子に対する位置および姿勢を計算するという手順を特徴としている。
【発明の効果】
【０００７】
本発明によれば、各種の適宜な変位センサを適切な位置に設置し、適宜な座標系を設定することで、各センサの設置位置の情報および各センサが検出した信号に基づいて移動子に固定されている移動座標系の原点位置および姿勢を求めることができる。
また、移動子上の任意点の移動座標と移動座標系の変位後の原点位置および姿勢を用いて正確に移動子が変位した後この点の位置を計算することができるので、回転運動量が大きい場合や高い計測制度が要求されるような場合にも移動子が変位した後の位置・姿勢を正確に計測することが可能になると言う効果がある。
【発明を実施するための最良の形態】
【０００８】
以下、本発明の実施の形態について図を参照して説明する。
【実施例１】
【０００９】
図１は、本発明の実施例１における６自由度移動体の位置・姿勢計測装置の構成を示す図である。
図１において、２１は移動子であり、アクチュエータ（不図示）に駆動されて６自由度の位置・姿勢変位する。２２は固定子である。１、２および３はギャップセンサ（光学式、静電式等がある）であり、ヘッドが固定子に固定され、ターゲットが移動子に固定され、ヘッドとターゲット平面との距離を検出する。そして、５と６は二次元センサ（面検出可能）であり、垂直２方向の変位量を検出する。
図１に示した本実施例の６自由度移動体の位置・姿勢計測装置が図９に示した特許文献１の６自由度移動体の位置・姿勢計測装置と異なる部分は、ｘ、ｙ軸方向向きに設置した３つ１自由度の変位センサの代わりに２つの二次元センサ５、６を備えた部分である。その他の構成は同一である。
以下、本実施例の計測原理について説明する。
ここで、位置・姿勢を厳密かつ明確に計算するため、固定子に固定される固定座標系ｘｙｚｏと移動子に固定されて移動子と一緒に変位する移動座標系ＸＹＺＯとを導入する。そして、固定座標系ｘｙｚｏが初期（移動子の変位が０である時）の移動座標系ＸＹＺＯと重なるように移動座標系と固定座標系を設定する。  移動子が変位した後に、移動座標系の原点Ｏの絶対座標を（ｘO，ｙO，ｚO）とし、移動座標系に対して固定座標系のｘ軸、ｙ軸、ｚ軸の順で回転座標変換を行う場合の姿勢角度を（α，β，γ）とし、座標変換行列をＧとすると、移動座標が（Ｘ，Ｙ，Ｚ）である点の絶対座標（ｘ，ｙ，ｚ）を次式で表すことができる。
【００１０】
【数１】
2006201092.tif 000003
【００１１】
ただし、Ｇ11＝ｃｏｓβｃｏｓγ，
Ｇ12＝ｃｏｓβｓｉｎγ，
Ｇ13＝−ｓｉｎβ，
Ｇ21＝ｓｉｎαｓｉｎβｃｏｓγ−ｃｏｓαｓｉｎγ，
Ｇ22＝ｓｉｎαｓｉｎβｓｉｎγ＋ｃｏｓαｃｏｓγ，
Ｇ23＝ｓｉｎαｃｏｓβ，
Ｇ31＝ｃｏｓαｓｉｎβｃｏｓγ＋ｓｉｎαｓｉｎγ，
Ｇ32＝ｃｏｓαｓｉｎβｓｉｎγ−ｓｉｎαｃｏｓγ，
Ｇ33＝ｃｏｓαｃｏｓβ．
上式により、移動座標系の原点位置（ｘO，ｙO，ｚO）および姿勢（α，β，γ）を求めれば、移動子上の任意点の位置を求めることができる。
【００１２】
以上の座標系に基づいて各センサの設置方法について詳細に説明する。
ギャップセンサ１、ギャップセンサ２およびギャップセンサ３のヘッド側の測定点をそれぞれＨ1、Ｈ2およびＨ3とし、ターゲット側の測定点をそれぞれＴ1、Ｔ2およびＴ3とする。この３つのギャップセンサは、すべてのターゲット平面が平面ＸＯＹに平行であり、点Ｈ1と点Ｈ2とが平面ｙｏｚに対して対称であり、点Ｈ2と点Ｈ3とが平面ｘｏｚに対して対称であり、そして移動子の変位がない時直線Ｈ1Ｔ1、Ｈ2Ｔ2およびＨ3Ｔ3が全てｚ軸に平行であるように設置される。また、二次元センサ５と二次元センサ６のターゲット側の測定点をそれぞれＰとＱとする。この２つの二次元センサは、直線ＰＱが平面ＸＯＹに平行であり、そしてセンサの２つの測定方向がｘ軸又はｙ軸に平行であるように設置される。
従って、点Ｈ1、Ｈ2およびＨ3の絶対座標をそれぞれ（ｘ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）および姿勢（α，β，γ）を求める。
まず、３つのギャップセンサの検出情報および初期位置情報に基づいてαおよびβを求める。
ギャップセンサ１のターゲット側の測定点Ｔ1の初期ｚ絶対座標をｚT10とすると、ギャップセンサ１が測定した初期ギャップは
ｄ10＝ｚT10−ｚH1                    … （５）
となる。
ギャップセンサのターゲット平面が常に平面ＸＯＹに平行であるので、変位後ギャップセンサのターゲット平面の法線ベクトルは
【００１３】
【数２】
2006201092.tif 000004
【００１４】
となる。また、ギャップセンサ１のターゲット平面とＺ軸との交点Ｃ１の移動座標が（０，０，ｚT10）であるので、変位した後の絶対座標は
【００１５】
【数３】
2006201092.tif 000005
【００１６】
となる。よって、移動子が変位した後にギャップセンサ１のターゲット平面の方程式は
ｘ−ｘO−Ｇ13ｚT10）Ｇ13＋（ｙ−ｙO−Ｇ23ｚT10）Ｇ23
＋（ｚ−ｚO−Ｇ33ｚT10）Ｇ33＝０    … （８）
となる。
また、Ｇ132＋Ｇ232＋Ｇ332＝１を考慮して上式を整理すると、
ｘG13＋ｙG23＋ｚG33＝ｄO＋ｚT10    … （９）
となる。ただし、
ｄO＝ｘOＧ13＋ｙOＧ23＋ｚOＧ33        … （１０）
である。
従って、点（ｘH1，ｙH1，ｚH1）とギャップセンサ１のターゲット平面との距離、すなわち、移動子が変位した後にギャップセンサ１が測定したギャップは
ｄ1＝ｄO＋ｚT10−ｘH1Ｇ13−ｙH1Ｇ23−ｚH1Ｇ33    … （１１）
となる。
式（５）および式（１１）によると、移動子が変位する前後第1ギャップセンサが検出した第1ギャップの変化量は
Δｄ1≡ｄ1−ｄ10＝ｄO−ｘH1Ｇ13−ｙH1Ｇ23−ｚH1Ｇ33−ｚH1  … （１２）
を満たす。
そして、同じ理由で移動子が変位する前後第２と第３ギャップセンサがそれぞれ検出した第２と第３ギャップの変化量Δｄ2とΔｄ3は
Δｄ2＝ｄO−ｘH2Ｇ13−ｙH2Ｇ23−ｚH2Ｇ33−ｚH2    … （１３）
および
Δｄ3＝ｄO−ｘH3Ｇ13−ｙH3Ｇ23−ｚH3Ｇ33−ｚH3   … （１４）
を満たす。
式（２）〜式（４）および式（１２）〜式（１４）によると、
Δｄ1−Δｄ2＝（ｘH2−ｘH1）Ｇ13        …（１５）
および
Δｄ2−Δｄ3＝（ｙH3−ｙH2）Ｇ23      … （１６）
となる。
Ｇ13＝−ｓｉｎβおよびＧ23＝ｓｉｎαｃｏｓβを上の２式に代入すると、αおよびβは次のように与えられる。
β＝ａｒｃｓｉｎ｛（Δｄ1−Δｄ2）／（ｘH1−ｘH2）｝  … （１７）
α＝ａｒｃｓｉｎ｛（Δｄ2−Δｄ3）／（ｙH3−ｙH2）／ｃｏｓβ｝ … （１８）
Ｈ1、Ｈ2、Ｈ3は固定点なので、ｘH1、ｘH2、ｙH2およびｙH3を予め測定しておけば上の２式からαおよびβを求めることができる。
次に、求めたα、βの値、２つの二次元センサの検出情報および初期位置情報に基づいてγ、ｘOおよびｙOを求める。
点Ｐおよび点Ｑの絶対座標をそれぞれ（ｘP，ｙP，ｚP）および（ｘQ，ｙQ，ｚQ）とし、式（１）によると、
【００１７】
【数４】
2006201092.tif 000006
【００１８】
および
【００１９】
【数５】
2006201092.tif 000007
【００２０】
となる。
ＺP＝ＺQを考慮して式（１９）から式（２０）を引くと、
【００２１】
【数６】
2006201092.tif 000008
【００２２】
となる。
また、ｘPQ＝ｘP−ｘQ，ＸPQ＝ＸP−ＸQ，ｙPQ＝ｙP−ｙQ
および、ＹPQ＝ＹP−ＹQとし、
Ｇ11＝ｃｏｓβｃｏｓγ，
Ｇ12＝ｃｏｓβｓｉｎγ，
Ｇ21＝ｓｉｎαｓｉｎβｃｏｓγ−ｃｏｓαｓｉｎγ、
および
Ｇ22＝ｓｉｎαｓｉｎβｓｉｎγ＋ｃｏｓαｃｏｓγ
を上の２式に代入し、整理すると、
【００２３】
【数７】
2006201092.tif 000009
【００２４】
となる。この方程式を解くと、
【００２５】
【数８】
2006201092.tif 000010
【００２６】
となる。従って、γは次式のように与えられる。
【００２７】
【数９】
2006201092.tif 000011
【００２８】
上式中、ｘ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−（Ｇ11ＸP＋Ｇ12ＹP＋Ｇ13ＺP）      …（２７）
ｙO＝ｙP−（Ｇ21ＸP＋Ｇ22ＹP＋Ｇ23ＺP）      …（２８）
また、同じ理由で変位前後の点Ｑの情報を用いて変位後移動座標の原点のｘｙ絶対座標を次のように与えられる。
ｘO＝ｘQ−（Ｇ11ＸQ＋Ｇ12ＹQ＋Ｇ13ＺQ）      …（２９）
ｙO＝ｙQ−（Ｇ21ＸQ＋Ｇ22ＹQ＋Ｇ23ＺQ）      …（３０）
式（２７）、（２８）又は式（２９）、（３０）を用いて得た２通りデータの平均値を変位後移動座標の原点のｘｙ絶対座標とすることで計測精度を上げることができる。また、この２通りのデータを用いて検出エラーの判断を行うことも可能である。
【００２９】
最後に、ｚOを求める。
式（１０）と式（１１）によると、
ｄ1＝ｚT10＋（ｘO−ｘH1）Ｇ13＋（ｙO−ｙH1）Ｇ23＋（ｚO−ｚH1）Ｇ33                                                        …（３１）
となる。
上式により、ｚOは次式のように与えられる。
ｚO＝ｚH1＋｛ｄ1−ｚT10−（ｘO−ｘH1）Ｇ13−（ｙO−ｙH1）Ｇ23｝／Ｇ33                                                      …（３２）
以上では、ギャップセンサ１の情報を用いてｚOを計算しているが、同じ理由でギャップセンサ２或いはギャップセンサ３の情報を用いて次のようにｚOを計算することができる。
ｚO＝ｚH2＋｛ｄ2−ｚT20−（ｘO−ｘH2）Ｇ13−（ｙO−ｙH2）Ｇ23｝／Ｇ33                                                            …（３３）
或いは
ｚO＝ｚH3＋｛ｄ3−ｚT30−（ｘO−ｘH3）Ｇ13−（ｙO−ｙH3）Ｇ23｝／Ｇ33                                                          …（３４）
式（３２）又は式（３３）又は式（３４）を用いて得た３通りデータの平均値を変位後移動座標の原点のｚ絶対座標とすることで計測精度を上げることができる。また、この３通りのデータを用いて検出エラーの判断を行うことも可能である。
【００３０】
このように、３つのギャップセンサは、すべてのヘッドが固定子に固定されて平面ｙｏｚ又は平面ｘｏｚに対して対称であり、すべてのターゲット平面が移動子に固定されてｘｏｙ平面に平行であるように設置され、また、２つの二次元センサは、すべてのヘッドが前記固定子に固定され、すべてのターゲットが前記移動子に固定され、すべてのターゲット側における初期測定点のＺ移動座標が同じであるように設置されると、各センサの設置位置の情報および各センサが検出した信号に基づいて移動子に固定されている移動座標系の原点位置および姿勢を高精度に求めることができる。
また、移動子上の任意点の移動座標と移動子が変位した後の移動座標系の原点位置および姿勢を用いて正確に移動子が変位した後この点の位置を計算することができる。
これによって、特許文献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＋（Ｇ21ＸD＋Ｇ22ＹD＋Ｇ23ＺD）  …（３７）
および
ｙE＝ｙO＋（Ｇ21ＸE＋Ｇ22ＹE＋Ｇ23ＺE）  …（３８）
が成り立つ。式（３５）を考慮して式（３７）から式（３８）を引くと、
ｙD−ｙE＝Ｇ21（ＸD−ＸE）               …（３９）
となる。Ｇ21＝ｓｉｎαｓｉｎβｃｏｓγ−ｃｏｓαｓｉｎγを上式に代入してγを求めると、
γ＝Ａ−ａｒｃｓｉｎ｛ｋ（ｙD−ｙE）／（ＸD−ＸE）｝  … （４０）
となる。
ただし、Ａ＝ａｒｃｓｉｎ（ｋｓｉｎαｓｉｎβ），
ｋ＝１／√（ｓｉｎ２αｓｉｎ２β＋ｃｏｓ２α）．
また、変位センサ９が検出した変位量をＳＦとすると、移動子が変位した後点Ｆのｘ絶対座標は
ｘF＝ＸF＋ＳF                        …（４１）
で求められる。
式（１）により、ｘOは次式のように与えられる。
ｘO＝ｘF−（Ｇ11ＸF＋Ｇ12ＹF＋Ｇ13ＺF）  …（４２）
また、式（３７）により、ｙOは次式のように与えられる。
ｙO＝ｙD−（Ｇ21ＸD+Ｇ22ＹD＋Ｇ23Ｚ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＋（Ｇ11ＸE＋Ｇ12ＹE＋Ｇ13ＺE）  …（４６）
および
ｘF＝ｘO＋（Ｇ11ＸF＋Ｇ12ＹF＋Ｇ13ＺF）  …（４７）
となる。
式（４４）を考慮して式（４６）から式（４７）を引くと、
ｘE−ｘF＝Ｇ12（ＹE−ＹF）                  … （４８）
となる。
Ｇ12＝ｃｏｓβｓｉｎγを上式に代入し、γを次式のように与えられる。
γ＝ａｒｃｓｉｎ｛（ｘE−ｘF）／（ＹE−ＹF）／ｃｏｓβ｝…（４９）
式（４６）により、ｘOを次式のように求めることができる。
ｘO＝ｘE−（Ｇ11ＸE＋Ｇ12ＹE＋Ｇ13ＺE）    …（５０）
また、変位センサ７が検出した変位量をＳＤとすると、移動子が変位した後点Ｄのｙ絶対座標は
ｙD＝ＹD＋ＳD                        …（５１）
で求められる。
式（１）により、ｙOを次式のように求めることができる。
ｙO＝ｙD−（Ｇ11ＸD＋Ｇ12ＹD＋Ｇ13ＺD）    …（５２）
最後に、実施例１と同じようにｚOを計算する。
【００３５】
このように、３つギャップセンサと３つ変位センサを用いることで、簡単かつ正確に移動子変位後位置・姿勢を計測することができる。本実施例は実施例２と較べ、センサの数と種類が全く同じであるが、ｘ方向の移動範囲が大きい場合に実施例２の方が計測に使われるスペースが小さく、ｙ方向の移動範囲が大きい場合に本実施例の方が計測に使われるスペースが小さい。
【実施例４】
【００３６】
次に、本発明の実施例３について図を参照して説明する。
本実施例は、実施例１において、ギャップセンサの設置方法を変えたものである。
図４は、本発明の実施例４における６自由度移動体の位置・姿勢計測装置の構成を示す図である。
図４において、移動座標系と固定座標系との設置方法は実施例１と同じになっている。３つのギャップセンサ１、２、３は、すべてのヘッドが移動子に固定され、すべてのターゲットが固定子に固定されるようになっている。
また、ターゲット平面が全て平面ｘｏｙに平行であり、点Ｈ1と点Ｈ2とが平面ＹＯＺに対して対称であり、点Ｈ2と点Ｈ3とが平面ＸＯＺに対して対称である。なお、ベクトルＨ1Ｔ1、Ｈ2Ｔ2およびＨ3Ｔ3が全てｚ軸の負方向と一致する。従って、点Ｈ1、Ｈ2およびＨ3の移動座標をそれぞれ（ＸH1，ＹH1，ＺH1）、（ＸH2，ＹH2，ＺH2）および（ＸH3，ＹH3，ＺH3）とすると、
ＸH2＝ＸH3                    …（５３）
ＹH1＝ＹH2                    …（５４）
および
ＺH1＝ＺH2＝ＺH3            …（５５）
が成り立つ。
固定座標系のｚ軸、ｙ軸、ｘ軸の順で回転座標変換行い、対応する移動子の回転角度を（γ，β，α）とし、座標変換行列をＧとし、また、移動子が変位した後の移動座標系の原点位置（ｘO，ｙO，ｚO）とすると、移動座標が（Ｘ，Ｙ，Ｚ）である点の絶対座標（ｘ，ｙ，ｚ）を次式で表すことができる。
【００３７】
【数１０】
2006201092.tif 000012
【００３８】
ただし、Ｈ11＝ｃｏｓβｃｏｓγ，
Ｈ12＝ｓｉｎαｓｉｎβｃｏｓγ−ｃｏｓαｓｉｎγ，
Ｈ13＝ｃｏｓαｓｉｎβｃｏｓγ＋ｓｉｎαｓｉｎγ，
Ｈ21＝ｃｏｓβｓｉｎγ，
Ｈ22＝ｓｉｎαｓｉｎβｓｉｎγ＋ｃｏｓαｃｏｓγ，
Ｈ23＝ｃｏｓαｓｉｎβｓｉｎγ−ｓｉｎαｃｏｓγ，
Ｈ31＝−ｓｉｎβ，
Ｈ32＝ｓｉｎαｃｏｓβ，
Ｈ33＝ｃｏｓαｃｏｓβ．
以下、各センサからの検出情報および各センサの初期位置に基づいて移動子が変位した後の移動座標系の原点位置（ｘO，ｙO，ｚO）および姿勢（α，β，γ）を求める。
まず、３つのギャップセンサの検出情報および初期位置情報に基づいてαおよびβを求める。
ギャップセンサ１、ギャップセンサ２およびギャップセンサ３のターゲット平面の方程式をそれぞれ
ｚ＝ｚT1，  ｚ＝ｚT2，      ｚ＝ｚT3        …（５７）
とし、ギャップセンサ１、ギャップセンサ２およびギャップセンサ３が検出したギャップ信号をそれぞれｄ1、ｄ2およびｄ3とすると、変位後点Ｈ1、Ｈ2およびＨ3のｚ絶対座標はそれぞれ
ｚH1＝ｚT1＋ｄ1，  ｚH2＝ｚT2＋ｄ2，  ｚH3＝ｚT3＋ｄ3  …（５８）
となる。
一方、式（５６）により、
ｚH1＝ｚO＋ＸH1Ｈ31＋ＹH1Ｈ32＋ＺH1Ｈ33       …（５９）
ｚH2＝ｚO＋ＸH2Ｈ31＋ＹH2Ｈ32＋ＺH2Ｈ33       …（６０）
および
ｚH3＝ｚO＋ＸH3Ｈ31＋ＹH3Ｈ32＋ＺH3Ｈ33       …（６１）
が成り立つ。
式（５３）〜式（５５）を考慮して式（５９）から式（６０）を引き、式（６０）から式（６１）を引くと、
ｚH1−ｚH2＝（ＸH1−ＸH2）Ｈ31    …（６２）
および
ｚH2−ｚH3＝（ＹH2−ＹH3）Ｈ32    …（６３）
となる。
Ｈ３１＝−ｓｉｎβおよびＨ32＝ｓｉｎαｃｏｓβを上の２式に代入すると、αおよびβは次のように与えられる。
β＝ａｒｃｓｉｎ｛（ｚH2−ｚH1）／（ＸH1−ＸH2）｝            …（６４）
α＝ａｒｃｓｉｎ｛（ｚH2−ｚH3）／（ＹH2−ＹH3）／ｃｏｓβ｝…（６５）
また、ギャップセンサ１、ギャップセンサ２およびギャップセンサ３のターゲット平面がすべて同じ平面にあるようになっていれば、
ｚT1＝ｚT2＝ｚT3
が成り立つので、式（５８）により、
β＝ａｒｃｓｉｎ｛（ｄ2−ｄ1）／（ＸH1−ＸH2）｝     …（６６）
α＝ａｒｃｓｉｎ｛（ｄ2−ｄ3）／（ＹH2−ＹH3）／ｃｏｓβ｝…（６７）
となる。
【００３９】
ｚT1、ｚT2、ｚT3、ＸH1、ＸH2およびＸH3は一定なので予め測定しておけば、式（５８）および式（６４）〜（６７）により、ギャップセンサ１、ギャップセンサ２およびギャップセンサ３が検出したギャップ信号ｄ1、ｄ2およびｄ3に基づいてαおよびβを求めることができる。
次に、求めたα、βの値、２つの二次元センサの検出情報および初期位置情報に基づいてγ、ｘOおよびｙOを求める。
点Ｐおよび点Ｑの絶対座標をそれぞれ（ｘP，ｙP，ｚP）および（ｘQ，ｙQ，ｚQ）とし、式（５６）によると、
【００４０】
【数１１】
2006201092.tif 000013
【００４１】
および
【００４２】
【数１２】
2006201092.tif 000014
【００４３】
となる。
ＺP＝ＺQを考慮して式（６８）から式（６９）を引くと、
【００４４】
【数１３】
2006201092.tif 000015
【００４５】
となる。
また、ｘPQ＝ｘP−ｘQ，ＸPQ＝ＸP−ＸQ，ｙPQ＝ｙP−ｙQ
および、ＹPQ＝ＹP−ＹQとし、
Ｈ11＝ｃｏｓβｃｏｓγ，
Ｈ12＝ｓｉｎαｓｉｎβｃｏｓγ−ｃｏｓαｓｉｎγ，
Ｈ21＝ｃｏｓβｓｉｎγ、
および、Ｈ22＝ｓｉｎαｓｉｎβｓｉｎγ＋ｃｏｓαｃｏｓγを
上の２式に代入し、整理すると、
【００４６】
【数１４】
2006201092.tif 000016
【００４７】
となる。この方程式を解くと、
【００４８】
【数１５】
2006201092.tif 000017
【００４９】
となる。従って、γを次式のように与えられる。
【００５０】
【数１６】
2006201092.tif 000018
【００５１】
上式中、ｘ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−（Ｈ11ＸP＋Ｈ12ＹP＋Ｈ13ＺP）     …（７６）
ｙO＝ｙP−（Ｈ21ＸP＋Ｈ22ＹP＋Ｈ23ＺP）    …（７７）
最後に、式（５９）により、ｚOを次式のように求めることができる。
ｚO＝ｚH1−（ＸH1Ｈ31＋ＹH1Ｈ32＋ＺH1Ｈ33）  …（７８）
【００５２】
このように、実施例１と較べ、ギャップセンサのヘッドが移動子上に固定されているため、信号検出ための配線が移動子の運動の妨げになるが、計算は簡単になる。
【実施例５】
【００５３】
次に、本発明の実施例５について図を参照して説明する。
本実施例は、実施例２において、ギャップセンサの設置方法を変えたものである。
図５は、本発明の実施例５における６自由度移動体の位置・姿勢計測装置の構成を示す図である。
図５において、移動座標系と固定座標系との設置方法およびギャップセンサの設置方法は実施例４と同じようになっている。
一方、３つの変位センサの設置方法は実施例２と同じようになっている。また、座標変換は実施例４と同じように行う。
以下、各センサからの情報および各センサの初期位置に基づいて移動子が変位した後の移動座標系の原点位置（ｘO，ｙO，ｚO）および姿勢（α，β，γ）を求める。
まず、実施例４と同じように３つのギャップセンサ１、２、３の検出情報および初期位置情報に基づいてαおよびβを求める。
次に、求めたα、βの値、３つの変位センサの検出情報および初期位置情報に基づいてγ、ｘOおよびｙOを求める。
【００５４】
変位センサ７および変位センサ８が検出した変位量をそれぞれＳDおよびＳEとすると、移動子が変位した後点Ｄおよび点Ｅのｙ絶対座標はそれぞれ
ｙD＝ＹD＋ＳD，  ｙE＝ＹE＋ＳE            …（７９）
で求められる。また、式（５６）によると、
ｙD＝ｙO＋（Ｈ21ＸD＋Ｈ22ＹD＋Ｈ23ＺD）   …（８０）
および
ｙE＝ｙO＋（Ｈ21ＸE＋Ｈ22ＹE＋Ｈ23ＺE）  …（８１）
が成り立つ。式（３５）を考慮して式（８０）から式（８１）を引くと、
ｙD−ｙE＝Ｈ21（ＸD−ＸE）                …（８２）
となる。Ｈ21＝ｃｏｓβｓｉｎγを上式に代入してγを求めると、
γ＝ａｒｃｓｉｎ｛（ｙD−ｙE）／（ＸD−ＸE）／ｃｏｓβ｝ …（８３）
となる。
また、変位センサ９が検出した変位量をＳＦとすると、移動子が変位した後点Ｆのｘ絶対座標は
ｘF＝ＸF＋ＳF                            …（８４）
で求められる。式（５６）により、ｘOは次式のように与えられる。
ｘO＝ｘF−（Ｈ11ＸF＋Ｈ12ＹF＋Ｈ13ＺF） … （８５）
また、式（８０）により、ｙOは次式のように与えられる。
ｙO＝ｙD−（Ｈ21ＸD＋Ｈ22ＹD＋Ｈ23ＺD）  …（８６）
最後に、実施例４と同じようにｚOを計算する。
【００５５】
このように、実施例２と較べ、ギャップセンサのヘッドが移動子上に固定されているため、信号検出ための配線が移動子の運動の妨げになるが、計算は簡単になる。また、実施例４と較べ、センサが１つ増えるので設置場所を大きく取る必要になるが、変位センサが二次元センサより廉価であるためコストを下げることができる。
【実施例６】
【００５６】
次に、本発明の実施例６について図を参照して説明する。
実施例６は、実施例３において、ギャップセンサの設置方法を変えたものである。
図６は、本発明の実施例６における６自由度移動体の位置・姿勢計測装置の構成を示す図である。
図６において、移動座標系と固定座標系との設置方法およびギャップセンサの設置方法は実施例４と同じようになっている。
一方、３つの変位センサの設置方法は実施例３と同じようになっている。また、座標変換は実施例４と同じように行う。
以下、各センサからの情報および各センサの初期位置に基づいて移動子が変位した後の移動座標系の原点位置（ｘO，ｙO，ｚO）および姿勢（α，β，γ）を求める。
まず、実施例４と同じように３つのギャップセンサ１、２、３の検出情報および初期位置情報に基づいてαおよびβを求める。
次に、求めたα、βの値、３つの変位センサの検出情報および初期位置情報に基づいてγ、ｘOおよびｙOを求める。
変位センサ８および変位センサ９が検出した変位量をそれぞれＳEおよびＳFとすると、変位後の点Ｅおよび点Ｆのｘ絶対座標はそれぞれ
ｘE＝ＸE＋ＳE，  ｘF＝ＸF＋ＳF            …（８７）
で求められる。式（５６）によると、
ｘE＝ｘO＋（Ｈ11ＸE＋Ｈ12ＹE＋Ｈ13ＺE）   … （８８）
および
ｘF＝ｘO＋（Ｈ11ＸF＋Ｈ12ＹF＋Ｈ13ＺF）    …（８９）
となる。式（４４）を考慮して式（８８）から式（８９）を引くと、
ｘE−ｘF＝Ｈ12（ＹE−ＹF）            …（９０）
となる。
Ｈ12＝ｓｉｎαｓｉｎβｃｏｓγ−ｃｏｓαｓｉｎγ
を上式に代入してγを求めると、
γ＝Ａ−ａｒｃｓｉｎ｛ｋ（ｙE−ｙF）／（ＸE−ＸF）｝  …（９１）
となる。
ただし、Ａ＝ａｒｃｓｉｎ（ｋｓｉｎαｓｉｎβ），
ｋ＝１／√（ｓｉｎ２αｓｉｎ２β＋ｃｏｓ２α）．
【００５７】
式（８８）により、ｘOを次式のように求めることができる。
ｘO＝ｘE−（Ｈ11ＸE＋Ｈ12ＹE＋Ｈ13ＺE）   …（９２）
また、変位センサ７が検出した変位量をＳＤとすると、移動子が変位した後点Ｄのｙ絶対座標は
ｙD＝ＹD＋ＳD                             …（９３）
で求められる。
式（５６）により、ｙOを次式のように求めることができる。
ｙO＝ｙD−（Ｈ11ＸD＋Ｈ12ＹD＋Ｈ13Ｚ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−（Ｇ11ＸN＋Ｇ12ＹN＋Ｇ13ＺN）    …（９６）
ｙO＝ｙN−（Ｇ21ＸN＋Ｇ22ＹN＋Ｇ23ＺN）     …（９７）
次に、ｚOを求める。
三次元センサ１０の測定点Ｍの初期絶対座標を（ｘM0，ｙM0，ｚM0）とすると、変位前三次元センサ１０のターゲット平面の方程式は
ｚ＝ｚM0                                …（９８）
となる。
また、点Ｍを通る三次元センサ１０の測定線ＭＬの方程式は
ｘ＝ｘM0，  ｙ＝ｙM0                  …（９９）
となる。
また、実施例１におけるギャップセンサ１のターゲット平面の変位した後の方程式を求めることと同じように三次元センサ１０のターゲット平面の変位した後の方程式は次式のように与えられる。
ｘＧ13＋ｙＧ23＋ｚＧ33＝ｄO＋ｚM0    …（１００）
となる。ただし、
ｄO＝ｘOＧ13＋ｙOＧ23＋ｚOＧ33      …（１０１）
である。従って、移動子が変位した後の点Ｍのｚ絶対座標は、
ｚM＝（ｄO＋ｚM0−ｘM0Ｇ13−ｙM0Ｇ23）／Ｇ33    …（１０２）
となる。
よって、三次元センサ１０が検出した点Ｍのｚ方向の変位量は
ＳMz≡ｚM−ｚM0
＝（ｄO＋ｚM0−ｘM0Ｇ13−ｙM0Ｇ23）／Ｇ33−ｚM0 …（１０３）
となる。
式（１０１）を式（１０３）に代入してｚOを求めると、
ｚO＝ＳMz＋ｚM0―｛ｚM0＋（ｘ0−ｘM0）Ｇ13＋（ｙ0−ｙM0）Ｇ23｝／Ｇ33                                                        …（１０４）
となる。
【００６３】
このように、２つ三次元センサの検出情報および初期位置情報に基づいて正確に移動子が変位した後の位置・姿勢を計測することができる。
【実施例９】
【００６４】
次に、本発明の実施例９について図を参照して説明する。
実施例９は、実施例８と同様の図８の構成において、座標変換に式（１）でなく式（５６）を用いた場合の実施例である。姿勢（α，β，γ）および移動子が変位した後の移動座標系の原点位置（ｘO，ｙO）は実施例８と同様にして求める。このときのｚOは以下のようにして求める。
移動子が変位した後の三次元センサ１０の測定点Ｍの絶対座標を（ｘM，ｙM，ｚM）、測定点Ｍの移動座標を（ＸM，ＹM，ＺM）とする。
ｘM＝ｘM0，ｙM＝ｙM0であるから、式（５６）より以下の式が成り立つ。
ただし、ＺMは既知である。
【００６５】
【数１７】
2006201092.tif 000019
【００６６】
式（１０５）の上の2行より
【００６７】
【数１８】
2006201092.tif 000020
【００６８】
となる。これを解くと
【００６９】
【数１９】
2006201092.tif 000021
【００７０】
となる。得られたＸM，ＹMよりｚOを求める。
ｚO＝ＳMz＋ｚM0―Ｈ31ＸM―Ｈ32ＹM―Ｈ33ＺM     …（１０８）
【実施例１０】
【００７１】
次に、本発明の実施例１０について図を参照して説明する。
実施例１０は、実施例３と同様の図３の構成において、座標変換に式（１）でなく式（５６）を用いた場合の実施例である。
６自由度移動体の位置・姿勢計測装置の構成、移動座標系と固定座標系との設置方法、ギャップセンサ１〜３、および３つの変位センサ７〜９の設置方法は実施例３と同じである。
以下、各センサからの情報および各センサの初期位置に基づいて移動子が変位した後の移動座標系の原点位置（ｘO，ｙO，ｚO）および姿勢（α，β，γ）を求める。
まず、実施例６と同じ方法でα、β、γ、ｘOおよびｙOを求める。
このときのｚOは以下のようにして求める。
ギャップセンサ1のヘッド位置の絶対座標を（ｘH1，ｙH1，ｚH1）、移動子が変位した後のギャップセンサ１の測定点Ｔ１の絶対座標を（ｘT1，ｙT1，ｚT1）、測定点Ｔ1の移動座標を（ＸT1，ＹT1，ＺT1）とする。
図３の構成では、以下の式が成り立つ。
ｘT1＝ｘH1                      …（１０９）
ｙT1＝ｙH1                      …（１１０）
ｚT1＝ｚH1＋ｄ1                …（１１１）
ただし、ｘH1、ｙH1、ＺT1は移動しない既知の座標である。
式（５６）より、以下の式が成り立つ。
【００７２】
【数２０】
2006201092.tif 000022
【００７３】
式（１１２）の上の2行より
【００７４】
【数２１】
2006201092.tif 000023
【００７５】
となる。これを解くと、
【００７６】
【数２２】
2006201092.tif 000024
【００７７】
となる。
得られたＸT1，ＹT1を用いて、以下の式によりｚOを求める。
ｚO＝ｚH1＋ｄ1―Ｈ31ＸT1―Ｈ32ＹT1―Ｈ33ＺT1    …（１１５）
【産業上の利用可能性】
【００７８】
本発明の技術は、複数センサの検出情報および初期位置情報に基づいて６自由度変位する移動子の位置・姿勢を正確に計測することができるので、多自由度かつ高精度な位置・姿勢制御を行う制御装置のフィードバック信号を計測するという用途にも適応できる。
【図面の簡単な説明】
【００７９】
【図１】本発明の実施例１における６自由度移動体の位置・姿勢計測装置の構成を示す図である。
【図２】本発明の実施例２における６自由度移動体の位置・姿勢計測装置の構成を示す図である。
【図３】本発明の実施例３における６自由度移動体の位置・姿勢計測装置の構成を示す図である。
【図４】本発明の実施例４における６自由度移動体の位置・姿勢計測装置の構成を示す図である。
【図５】本発明の実施例５における６自由度移動体の位置・姿勢計測装置の構成を示す図である。
【図６】本発明の実施例６における６自由度移動体の位置・姿勢計測装置の構成を示す図である。
【図７】本発明の実施例７における６自由度移動体の位置・姿勢計測装置のギャップセンサの配置を示す図である。
【図８】本発明の実施例８における６自由度移動体の位置・姿勢計測装置の構成を示す図である。
【図９】従来技術の６自由度移動体の位置・姿勢計測装置の構成を示す図である。
【符号の説明】
【００８０】
１、２、３、４  ギャップセンサ
５、６  二次元センサ
７、８、９  変位センサ
１０、１１  三次元センサ
２１  移動子
２２  固定子
【図１】
2006201092.tif 000025
【図２】
2006201092.tif 000026
【図３】
2006201092.tif 000027
【図４】
2006201092.tif 000028
【図５】
2006201092.tif 000029
【図６】
2006201092.tif 000030
【図７】
2006201092.tif 000031
【図８】
2006201092.tif 000032
【図９】
2006201092.tif 000033