(57)【要約】【課題】  ２つの物体の相対位置及び相対姿勢を正確に測定可能な三次元配置状態測定方法を提供する。【解決手段】  ２つの物体１０、２０の相対位置及び相対姿勢を測定する三次元配置状態測定方法であって、一方の物体１０に、互いに交差する３軸方向に磁界を送信可能な磁界送信機１１と、特定の送信座標軸の傾斜角を測定可能な送信機側角度検出器１２とを併設し、他方の物体２０に、互いに交差する３軸方向の磁界を受信可能な磁界受信機２１と、特定の受信座標軸の傾斜角を測定可能な受信機側角度検出器２２とを併設し、傾斜角に関する特定の送信座標軸及び特定の受信座標軸の特徴要素が共に基準状態であるときの磁界送信機１１から送信された送信磁界を磁界受信機２１で受信したときの受信信号に基づいて、２つの物体１０、２０の相対位置及び相対姿勢を測定する。【選択図】        図１
2005283284.tif 000002
【特許請求の範囲】
【請求項１】
２つの物体の相対位置及び相対姿勢を測定する三次元配置状態測定方法であって、
一方の物体に、互いに交差する３軸方向に磁界を送信可能な磁界送信機と、前記磁界送信機から送信される磁界の方向を特徴付ける３つの送信座標軸の内の少なくとも１つの特定の送信座標軸の傾斜角を測定可能な送信機側角度検出器とを併設し、
他方の物体に、互いに交差する３軸方向の磁界を受信可能な磁界受信機と、前記磁界受信機で受信される磁界の方向を特徴付ける３つの受信座標軸の内の少なくとも１つの特定の受信座標軸の傾斜角を測定可能な受信機側角度検出器とを併設し、
傾斜角に関する前記特定の送信座標軸及び前記特定の受信座標軸の特徴要素が共に基準状態であるときの前記磁界送信機から送信された送信磁界を前記磁界受信機で受信したときの受信信号に基づいて、前記２つの物体の相対位置及び相対姿勢を測定する三次元配置状態測定方法。
【請求項２】
傾斜角に関する前記特定の送信座標軸の特徴要素が前記基準状態となったときに前記磁界送信機から前記送信磁界の送信を行う請求項１記載の三次元配置状態測定方法。
【請求項３】
前記磁界送信機にて、傾斜角に関する前記特定の送信座標軸の特徴要素の前記基準状態からの逸脱量に応じて、前記特定の送信座標軸の特徴要素が前記基準状態から逸脱していないときの送信磁界に換算して送信して、見かけ上、傾斜角に関する前記特定の送信座標軸の特徴要素が前記基準状態であるとする請求項１記載の三次元配置状態測定方法。
【請求項４】
傾斜角に関する前記特定の受信座標軸の特徴要素が前記基準状態となったときに前記磁界受信機が前記送信磁界の受信を行う請求項１〜３のいずれか１項に記載の三次元配置状態測定方法。
【請求項５】
前記磁界受信機にて、傾斜角に関する前記特定の受信座標軸の特徴要素の前記基準状態からの逸脱量に応じて、前記特定の受信座標軸の特徴要素が前記基準状態から逸脱していないときの受信信号に換算して受信して、見かけ上、傾斜角に関する前記特定の受信座標軸の特徴要素が前記基準状態であるとする請求項１〜３のいずれか１項に記載の三次元配置状態測定方法。
【請求項６】
前記磁界送信機及び前記磁界受信機の少なくとも一方に、互いに直交する２軸の、鉛直方向に対する傾斜角を夫々測定可能な傾斜計、及び、地磁気を利用して方位角を測定可能な方位計が設けられている請求項１〜５のいずれか１項に記載の三次元配置状態測定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、２つの物体の相対位置及び相対姿勢を測定する三次元配置状態測定方法に関する。
【背景技術】
【０００２】
かかる２つの物体間の相対位置と相対姿勢とを測定する技術として、磁界送信機と磁界受信機とを利用したトラッキングシステムがある。このトラッキングシステムは、互いに直交する３軸方向に磁界を送信する磁界送信機と互いに直交する３軸方向から磁界を受信する磁界受信機とを用い、磁界送信機の送信磁界と磁界受信機の受信磁界とから、磁界送信機の２軸の回転角と磁界受信機の３軸の回転角、及び磁界送信機と磁界受信機の間の相対距離という６つの未知数を導出することで、磁界受信機との相対位置及び相対姿勢を知ろうとしている（例えば、非特許文献１参照）。
【０００３】
【非特許文献１】フレデリック・エイチ・ラブ（Ｆｒｅｄｅｒｉｃｋ  Ｈ．  Ｒａｂｂ）著，「マグネティック  ポジション  アンド  オリエンテーション  トラッキング  システム（Ｍａｇｎｅｔｉｃ  Ｐｏｓｉｔｉｏｎ  ａｎｄ  Ｏｒｉｅｎｔａｔｉｏｎ  Ｔｒａｃｋｉｎｇ  Ｓｙｓｔｅｍ）」，（米国），  アイトリプルイー  トランザクション  オン  エアロスペース  アンド  エレクトロニック  システム（ＩＥＥＥ  Ｔｒａｎｓａｃｔｉｏｎ  ｏｎ  Ａｅｒｏｓｐａｃｅ  ａｎｄ  Ｅｌｅｃｔｒｏｎｉｃ  Ｓｙｓｔｅｍｓ），  ＡＥＳ−１５（５），１９７９年９月，ｐ７０９−７１８
【発明の開示】
【発明が解決しようとする課題】
【０００４】
従来の三次元配置状態測定方法において、磁界送信機と磁界受信機とを利用したトラッキングシステムを用いた場合、未知数が多いなどの理由から、常時変化する磁界送信機と磁界受信機との相対位置及び相対姿勢を、それらの変化量が小さいという仮定条件のもとでの差分逐次更新式により随時更新しているため、値が安定せず誤差が大きい上、測定前に相対位置と相対姿勢の初期値を設定するキャリブレーション作業が必要であった。つまり、相対位置と相対姿勢とを求めるためには、送信磁界と受信磁界とを複数回測定することが必要であり、且つ、差分逐次更新式を用いているために測定誤差が発生すると、以降に求められる相対位置と相対姿勢にもその測定誤差の影響が出てしまうという問題がある。
【０００５】
本発明は、上記の課題に鑑みてなされたものであり、その目的は、１回の送信磁界と受信磁界との測定値に基づいて２つの物体の相対位置及び相対姿勢を正確に測定可能な三次元配置状態測定方法を提供する点にある。
【課題を解決するための手段】
【０００６】
上記目的を達成するための本発明に係る三次元配置状態測定方法の第１特徴構成は、２つの物体の相対位置及び相対姿勢を測定する三次元配置状態測定方法であって、一方の物体に、互いに交差する３軸方向に磁界を送信可能な磁界送信機と、前記磁界送信機から送信される磁界の方向を特徴付ける３つの送信座標軸の内の少なくとも１つの特定の送信座標軸の傾斜角を測定可能な送信機側角度検出器とを併設し、他方の物体に、互いに交差する３軸方向の磁界を受信可能な磁界受信機と、前記磁界受信機で受信される磁界の方向を特徴付ける３つの受信座標軸の内の少なくとも１つの特定の受信座標軸の傾斜角を測定可能な受信機側角度検出器とを併設し、傾斜角に関する前記特定の送信座標軸及び前記特定の受信座標軸の特徴要素が共に基準状態であるときの前記磁界送信機から送信された送信磁界を前記磁界受信機で受信したときの受信信号に基づいて、前記２つの物体の相対位置及び相対姿勢を測定する点にある。
【０００７】
上記第１特徴構成によれば、傾斜角に関する上記特定の送信座標軸の特徴要素と傾斜角に関する上記特定の受信座標軸の特徴要素とが共に基準状態にあるとき、例えば、三次元方向への回転角の少なくとも１つが同一であるときには、磁界送信機と磁界受信機との間の相対位置と相対姿勢とを測定するための未知数が、三次元方向への２つずつの回転角と相対距離との５変数に減少されることになる。従って、差分逐次更新式でない理論式に基づいて未知数を導出することが可能となるため、測定前に相対位置と相対姿勢の初期値を設定するキャリブレーション作業が不要になるといった利点や、例え測定誤差が発生したとしても以降に求められる相対位置と相対姿勢にその測定誤差の影響が出ないという利点がある。
従って、１回の送信磁界と受信磁界との測定値に基づいて２つの物体の相対位置及び相対姿勢を正確に測定可能な三次元配置状態測定方法が提供されることになる。
【０００８】
本発明に係る三次元配置状態測定方法の第２特徴構成は、上記第１特徴構成に加えて、傾斜角に関する前記特定の送信座標軸の特徴要素が前記基準状態となったときに前記磁界送信機から前記送信磁界の送信を行う点にある。
【０００９】
上記第２特徴構成によれば、傾斜角に関する上記特定の送信座標軸の特徴要素が基準状態であることを送信機側角度検出器が検出したときに磁界送信機から送信磁界の送信を行うので、磁界受信機が受信した送信磁界が、上記特定の送信座標軸の特徴要素が基準状態にあるときに送信された送信磁界を起源とするものであることが担保されるので、導出された相対位置と相対姿勢の精度を高い状態で保つことができる。
【００１０】
本発明に係る三次元配置状態測定方法の第３特徴構成は、上記第１特徴構成に加えて、前記磁界送信機にて、傾斜角に関する前記特定の送信座標軸の特徴要素の前記基準状態からの逸脱量に応じて、前記特定の送信座標軸の特徴要素が前記基準状態から逸脱していないときの送信磁界に換算して送信して、見かけ上、傾斜角に関する前記特定の送信座標軸の特徴要素が前記基準状態であるとする点にある。
【００１１】
上記第３特徴構成によれば、傾斜角に関する上記特定の送信座標軸の特徴要素が基準状態では無いとしても、傾斜角に関する上記特定の送信座標軸の特徴要素の基準状態からの逸脱量に応じて、上記特定の送信座標軸の特徴要素が基準状態から逸脱していないときの送信磁界に換算して送信されるので、見かけ上は、傾斜角に関する上記特定の送信座標軸の特徴要素が基準状態となっているものとして、常時相対位置と相対姿勢とを導出することができる。
【００１２】
本発明に係る三次元配置状態測定方法の第４特徴構成は、上記第１から第３のいずれかの特徴構成に加えて、傾斜角に関する前記特定の受信座標軸の特徴要素が前記基準状態となったときに前記磁界受信機が前記送信磁界の受信を行う点にある。
【００１３】
上記第４特徴構成によれば、傾斜角に関する上記特定の受信座標軸の特徴要素が基準状態であることを受信機側角度検出器が検出したときに磁界受信機が送信磁界の受信を行うので、上記特定の受信座標軸の特徴要素が基準状態にあるときに磁界受信機が受信した送信磁界を用いた測定が行われていることが担保されるので、導出された相対位置と相対姿勢の精度を高い状態で保つことができる。
【００１４】
本発明に係る三次元配置状態測定方法の第５特徴構成は、上記第１から第３のいずれかの特徴構成に加えて、前記磁界受信機にて、傾斜角に関する前記特定の受信座標軸の特徴要素の前記基準状態からの逸脱量に応じて、前記特定の受信座標軸の特徴要素が前記基準状態から逸脱していないときの受信信号に換算して受信して、見かけ上、傾斜角に関する前記特定の受信座標軸の特徴要素が前記基準状態であるとする点にある。
【００１５】
上記第５特徴構成によれば、傾斜角に関する上記特定の受信座標軸の特徴要素が基準状態では無いとしても、傾斜角に関する上記特定の受信座標軸の特徴要素の基準状態からの逸脱量に応じて、上記特定の受信座標軸の特徴要素が基準状態から逸脱していないときの受信信号に換算して受信されるので、見かけ上は、傾斜角に関する上記特定の受信座標軸の特徴要素が基準状態となっているものとして、常時相対位置と相対姿勢とを正確に求めることができる。
【００１６】
本発明に係る三次元配置状態測定方法の第６特徴構成は、上記第１から第５のいずれかの特徴構成に加えて、前記磁界送信機及び前記磁界受信機の少なくとも一方に、互いに直交する２軸の、鉛直方向に対する傾斜角を夫々測定可能な傾斜計、及び、地磁気を利用して方位角を測定可能な方位計が設けられている点にある。
【００１７】
上記第６特徴構成によれば、上記特徴構成により導出された相対位置及び相対姿勢を、重力と地磁気を基準とした地図座標に変換することができる。
【発明を実施するための最良の形態】
【００１８】
以下に図面を参照して本発明に係る三次元配置状態測定方法について説明する。
本発明に係る三次元配置状態測定方法は、図１に例示するような三次元空間内に配置された２つの物体１０、２０の相対位置及び相対姿勢を測定することに利用でき、この測定を実現するために、一方の物体１０には、互いに交差する３軸方向に磁界を送信可能な磁界送信機１１と、その磁界送信機から送信される磁界の方向を特徴付ける３つの送信座標軸の内の少なくとも１つの特定の送信座標軸の傾斜角を測定可能な送信機側角度検出器１２とを併設し、他方の物体２０には、互いに交差する３軸方向の磁界を受信可能な磁界受信機２１と、その磁界受信機で受信する磁界の方向を特徴付ける３つの受信座標軸の内の少なくとも１つの特定の受信座標軸の傾斜角を測定可能な受信機側角度検出器２２とを併設している。
【００１９】
この磁界送信機１１は３つの磁気双極子によりモデル化され、夫々の磁気双極子により送信される磁界の方向は、図１に示す例では、互いに直交して交差するｘ軸、ｙ軸及びｚ軸の３つの送信座標軸によって特徴付けられている。また、磁界受信機２１は３つの磁気センサによりモデル化され、夫々の磁気センサにより受信される磁界の方向は、図１に示す例では、互いに直交して交差するｘ’軸、ｙ’軸及びｚ’軸の３つの受信座標軸によって特徴付けられている。
【００２０】
送信機側角度検出器１２は、例えば、ｙ軸に平行な傾斜角を測定可能な傾斜計を用いて構成することができる。また、受信機側角度検出器２２も同様に、ｙ’軸に平行な傾斜角を測定可能な傾斜計を用いて構成することができる。
【００２１】
次に、傾斜角に関する特定の送信座標軸及び特定の受信座標軸の特徴要素が共に基準状態であるときの磁界送信機１１から送信された送信磁界を磁界受信機２１で受信したときの受信信号に基づいて、２つの物体１０、２０の相対位置及び相対姿勢を測定する方法について具体例を挙げて説明する。
【００２２】
図２は、磁界送信機１１を構成する１つの磁気双極子によって生じる磁界（送信磁界）を示す図であり、（１）式及び（２）式で記載することができる。尚、Ｍは磁気双極子の強さであり、ρは磁気双極子と磁気センサとの間の距離、つまり磁界送信機１１と磁界受信機２１との間の相対距離である。また、ζは磁気双極子の向きを基準とした角度である。
【００２３】
【数１】
2005283284.tif 000003
【００２４】
そして、図３のように互いに交差する３軸方向に磁界を送信可能な磁界送信機１１（以下、磁気双極子と呼ぶ）と互いに交差する３軸方向の磁界を受信可能な磁界受信機２１（以下、磁気センサと呼ぶ）とが配置されていたとすると、各磁気双極子による各磁気センサの受信信号は次式で表される。尚、ｍ＝Ｍ／（２π）である。
【００２５】
【数２】
2005283284.tif 000004
【００２６】
従って、任意の方向ベクトルの磁気双極子による、図３の３軸の磁気センサの受信信号は、一般的に次式となる。
【００２７】
【数３】
2005283284.tif 000005
【００２８】
続いて、図４に示すように、３軸の磁気双極子がｚ軸まわりにおけるｘ軸からｙ軸への回転（ヨー、−π≦φ＜π）、次に新しいｙ軸まわりにおけるｚ軸からｘ軸への回転（ピッチ、−π／２≦θ≦π／２）で回転した姿勢にある場合を考える。尚、新しいｘ軸まわりにおけるｙ軸からｚ軸への回転（ロール）はないものとしている。つまり、傾斜角に関する特定の送信座標軸の特徴要素（ｘ軸まわりにおけるｙ軸からｚ軸への回転（ロール））が基準状態（ロール角＝０°）にあるとする。尚、この仮定条件の成立の可否は、ｙ軸に平行な傾斜計により、確認することができる。そして、ｘ軸方向の磁気双極子を考えると次式が成立する。
【００２９】
【数４】
2005283284.tif 000006
【００３０】
そして、（５）式より、次式が得られる。
【００３１】
【数５】
2005283284.tif 000007
【００３２】
同様に、ｙ軸方向の磁気双極子とｚ軸方向の磁気双極子の回転後のベクトルは次式となる。
【００３３】
【数６】
2005283284.tif 000008
【００３４】
そして（４）式より、回転後の３軸の磁気双極子による図４の３軸の磁気センサの受信信号は次式となる。
【００３５】
【数７】
2005283284.tif 000009
【００３６】
次に、図５に示すように、３軸の磁気センサがｚ’軸まわりにおけるｘ’軸からｙ’軸への回転（ヨー、−π≦φ’＜π）、次に新しいｙ’軸まわりにおけるｚ’軸からｘ’軸への回転（ピッチ、−π／２≦θ’≦π／２）で、回転した場合を考える。尚、新しいｘ’軸まわりにおけるｙ’軸からｚ’軸への回転（ロール）はないものとする。つまり、傾斜角に関する特定の受信座標軸の特徴要素（ｘ’軸まわりにおけるｙ’軸からｚ’軸への回転（ロール））が基準状態（ロール角＝０°）にあるとする。尚、この仮定条件の成立の可否は、ｙ’軸に平行な傾斜計により、確認することができる。そして、各磁気双極子のベクトル成分が（９）〜（１１）式で分かっているので、回転後の磁気センサのベクトルとの内積により、各磁気センサの受信信号を求めることが可能となる。まず、ｘ’軸方向とｙ’軸方向とｚ’軸方向の各磁気センサの回転後のベクトルは、（６）〜（８）式と同様に次式となる。
【００３７】
【数８】
2005283284.tif 000010
【００３８】
従って、各磁気双極子の送信磁界による各軸の磁気センサの受信信号は、（９）〜（１１）式と（１２）〜（１４）式の３×３＝９通りの組合せの内積により次式で表される。ｆijはｉ軸方向（ｉ＝ｘ，ｙ，ｚ）の磁界送信機１１の送信磁界をｊ軸方向（ｉ＝ｘ’，ｙ’，ｚ’）の磁界として磁界受信機２１で測定した値であり、３つの磁気双極子を、時間をずらして磁界送信させる方法や、交流磁界の周波数を異ならせて磁界送信させることにより、ｆijを夫々独立して測定することができる。
【００３９】
【数９】
2005283284.tif 000011
【００４０】
まず、以下の手順によりρを求める。（１５）式と（１７）式及び（２１）式と（２３）式より、次の（２４）式及び（２５）式が得られる。
【００４１】
【数１０】
2005283284.tif 000012
【００４２】
そして、（２４）式と（２５）式より、次式が得られる。
【００４３】
【数１１】
2005283284.tif 000013
【００４４】
また、（１６）式と（２２）式より、次式が得られる。
【００４５】
【数１２】
2005283284.tif 000014
【００４６】
同様に、（１８）式と（２０）式より、次式が得られる。
【００４７】
【数１３】
2005283284.tif 000015
【００４８】
さらに、（１９）式より、次式が得られる。
【００４９】
【数１４】
2005283284.tif 000016
【００５０】
よって、（２６）式と（２８）式より、次式が成り立つ。
【００５１】
【数１５】
2005283284.tif 000017
【００５２】
同様に、（２７）式と（２９）式より次式が成り立つ。
【００５３】
【数１６】
2005283284.tif 000018
【００５４】
従って、（３０）式と（３１）式より次式が得られるので、ρを求めることができる。
【００５５】
【数１７】
2005283284.tif 000019
【００５６】
次に、求められたρを用いてθとθ’を求める。まず、（２７）式と（２８）式（つまり、（１６）式、（２２）式、（１８）式、（２０）式のφとφ’の項）が、同時に零となる場合、次式が成り立つ。
【００５７】
【数１８】
2005283284.tif 000020
【００５８】
（３３）式と（３４）式より、ｃｏｓφｓｉｎφ’＝１／４ｃｏｓφｓｉｎφ’、ｓｉｎφｃｏｓφ’＝１／４ｓｉｎφｃｏｓφ’となり、−π／２＜φ’＜π／２と仮定するとｃｏｓφ’≠０、ｓｉｎφ＝０となるので、ｃｏｓφ＝±１、ｓｉｎφ’＝０よりφ’＝０。従って、以下の２通りの解が得られる。
【００５９】
【数１９】
2005283284.tif 000021
【００６０】
そして（３５）式の解が得られるとき、つまり、（２７）式と（２８）式が同時に零となるとき、（１９）式より（３６）式と（３７）式が成立する。
【００６１】
【数２０】
2005283284.tif 000022
【００６２】
続いて、以上の２通りの解が得られる各々の場合についてθとθ’を求める。まず、（３６）式の場合、（１５）式、（１７）式、（２１）式、（２３）式より、次式が得られる。
【００６３】
【数２１】
2005283284.tif 000023
【００６４】
よって、（４０）式と（４１）式より次式が成立する。
【００６５】
【数２２】
2005283284.tif 000024
【００６６】
そして、−π／２≦θ≦π／２よりｃｏｓθ≧０となるので、ｃｏｓθの値を求めることができる。
また同様に、（３９）式と（４１）式より次式が成立する。
【００６７】
【数２３】
2005283284.tif 000025
【００６８】
そして、−π／２＜θ’＜π／２と仮定するとｃｏｓθ’＞０となるので、ｃｏｓθ’の値を求めることができる。そして、求められたｃｏｓθとｃｏｓθ’≠０の値を（３９）式と（４０）式に代入すると、ｓｉｎθとｓｉｎθ’の２元連立方程式が得られる。この２元連立方程式を解くことによって、ｓｉｎθとｓｉｎθ’の値が求められ、θとθ’の値を一意に求めることができる。
尚、２元連立方程式が不定となる条件ｃｏｓθｃｏｓθ’＝０では、ｃｏｓθ’≠０より、ｃｏｓθ＝０となる。このとき、ｃｏｓθ’の値は既知なので、（３９）式より、ｓｉｎθ（＝±１）、即ちθ（＝±π／２）の値を求めることができる。更に、ｃｏｓθ＝０と、求められたｓｉｎθ（＝±１）の値とを（３８）式に代入して、ｓｉｎθ’、即ちθ’の値を求めることができる。
【００６９】
次に、（３７）式の場合、（１５）式、（１７）式、（２１）式、（２３）式より、次式が得られる。
【００７０】
【数２４】
2005283284.tif 000026
【００７１】
よって、（４６）式と（４７）式より次式が得られる。
【００７２】
【数２５】
2005283284.tif 000027
【００７３】
そして、−π／２≦θ≦π／２よりｃｏｓθ≧０となるので、ｃｏｓθの値を求めることができる。
また同様に、（４５）式と（４７）式より次式が得られる。
【００７４】
【数２６】
2005283284.tif 000028
【００７５】
そして、−π／２＜θ’＜π／２と仮定するとｃｏｓθ’＞０となるので、ｃｏｓθ’の値を求めることができる。そして、求められたｃｏｓθとｃｏｓθ’≠０の値を（４５）式と（４６）式に代入すると、ｓｉｎθとｓｉｎθ’の２元連立方程式が得られる。この２元連立方程式を解くことによって、ｓｉｎθとｓｉｎθ’の値が求められ、θとθ’の値を一意に求めることができる。
尚、ｓｉｎθとｓｉｎθ’の２元連立方程式が不定となる条件ｃｏｓθｃｏｓθ’＝０では、ｃｏｓθ’≠０より、ｃｏｓθ＝０となる。このとき、ｃｏｓθ’の値は既知なので、（４５）式より、ｓｉｎθ（＝±１）、即ちθ（＝±π／２）の値を求めることができる。更に、ｃｏｓθ＝０と、求められたｓｉｎθ（＝±１）の値とを（４４）式に代入して、ｓｉｎθ’、即ちθ’の値を求めることができる。
以上、（２７）式と（２８）式（つまり、（１６）式、（２２）式、（１８）式、（２０）式のφとφ’の項）が、同時に零となる場合のθとθ’を求める方法を示したので、続いて、（２７）式と（２８）式が、同時には零とならない場合について説明する。（２７）式と（２８）式が同時には零とならないとき、（１６）式と（２２）式、若しくは（１８）式と（２０）式の比の値から、ｔａｎθ若しくはｔａｎθ’よりθ若しくはθ’を求めることができる。
【００７６】
よって、θの値のみが求められたとして、（１８）式と（２０）式を用いずに、θ’の値を導出する。まず、（１５）式と（２１）式、及び（１７）式と（２３）式より、次式が得られる。
【００７７】
【数２７】
2005283284.tif 000029
【００７８】
よって、（５０）式と（５１）式より、θ’の値を一意に求めることができる。
同様に、θ’の値のみが求められたとして、（１６）式と（２２）式を用いずに、θの値を導出する。まず、（１５）式と（１７）式、及び、（２１）式と（２３）式より、次式が得られる。
【００７９】
【数２８】
2005283284.tif 000030
【００８０】
よって、（５２）式と（５３）式より、θの値を一意に求めることができる。
最後に、求められたθとθ’とρを用いてφとφ’を求める。まず、（１５）式〜（２３）式のφとφ’の項を次式で定義する。
【００８１】
【数２９】
2005283284.tif 000031
【００８２】
すると、（１５）式〜（２３）式は以下のように簡単化される。
【００８３】
【数３０】
2005283284.tif 000032
【００８４】
これら（１５）’式〜（２３）’式より、定義されたＡ〜Ｄの値は次式により計算することができる。
【００８５】
【数３１】
2005283284.tif 000033
【００８６】
尚、正弦と余弦の値（例えば、ｓｉｎθとｃｏｓθの値）は同時には零とならないので、上式の値は必ず求めることが可能である。
そして、（５４）式〜（５７）式より次式が成立する。
【００８７】
【数３２】
2005283284.tif 000034
【００８８】
そして、（６２）式と（６５）式、および（６３）式と（６４）式より、ｃｏｓφとｓｉｎφを消去すると、次の２元連立方程式が得られる。
【００８９】
【数３３】
2005283284.tif 000035
【００９０】
最初に、（６６）式のｓｉｎφ’の係数が零となる場合を考える。そのとき、Ｃ＝Ｂ／２よりｓｉｎφｃｏｓφ’＝１／４ｓｉｎφｃｏｓφ’となり、−π／２＜φ’＜π／２という仮定条件よりｃｏｓφ’≠０となるので、ｓｉｎφ＝０及びｃｏｓφ＝±１が得られる。そして、ｓｉｎφ＝０とｃｏｓφ’＞０及び（１９）式の符号より、ｃｏｓφの符号が決まるので、ｃｏｓφ＝±１よりφの値を一意に決定することができる。また、φの値が決まれば、（５４）式〜（５７）式より、次式が得られるので、φ’の値も求めることができる。
【００９１】
【数３４】
2005283284.tif 000036
【００９２】
以上より、（６６）式のｓｉｎφ’の係数が零とならないとすると、（６６）式と（６７）式の２元連立方程式の係数はすべて零となることはない。従って、解が不定となるのは、（６６）式＝（６７）式の場合のみである。この場合には、（６６）式とｃｏｓ2φ’＋ｓｉｎ2φ’＝１からｓｉｎφ’を消去してｃｏｓφ’（＞０）を求め、（６６）式にｃｏｓφ’を代入してｓｉｎφ’を求めればφ’の値を求めることができる。尚、（６６）式のｃｏｓφ’の係数が零となっても、（６６）式のｓｉｎφ’の係数は零とならないので、ｓｉｎφ’の値、つまりφ’を簡単に決定することができる。また、φ’の値が決まれば、（６２）式〜（６５）式より、φの値も求めることができる。
以上のように、−π／２＜φ’＜π／２と−π／２＜θ’＜π／２という仮定条件の下で、すべての値を求めることができた。そして、−π／２＜φ＜π／２と−π／２＜θ＜π／２という仮定条件の下でも、全ての値を同様にして求めることができる。従って、物体１０に設けられた３軸の磁界送信機からの送信磁界を物体２０に設けられた３軸の磁界受信機で受信したときの受信信号から、これら２つの物体１０、２０間の相対距離（ρ）及び相対姿勢（θ、φ、θ’、φ’）とを測定できることが示された。
【００９３】
＜別実施形態＞
＜１＞
上記実施形態では、傾斜角に関する特定の送信座標軸の特徴要素が基準状態（ｘ軸まわりにおけるｙ軸からｚ軸への回転が無い（ロール角＝０°）状態）であり、且つ、傾斜角に関する特定の受信座標軸の特徴要素が基準状態（ｘ’軸まわりにおけるｙ’軸からｚ’軸への回転が無い（ロール角＝０°）状態）であるときに、磁界送信機から送信された送信磁界を磁界受信機で受信したときの受信信号に基づいて、２つの物体１０、２０の相対位置及び相対姿勢を測定することについて説明したが、必ずしも傾斜角に関する特定の送信座標軸の特徴要素が基準状態であり、且つ、傾斜角に関する特定の受信座標軸の特徴要素が基準状態である必要は無い。
【００９４】
例えば、磁界送信機が、傾斜角に関する特定の送信座標軸の特徴要素の上記基準状態からの逸脱量に応じて、特定の送信座標軸の特徴要素が上記基準状態から逸脱していないときの送信磁界に換算して送信して、見かけ上は、傾斜角に関する特定の送信座標軸の特徴要素が上記基準状態であるとすることができる。また、磁界受信機が、傾斜角に関する特定の受信座標軸の特徴要素の上記基準状態からの逸脱量に応じて、特定の受信座標軸の特徴要素が上記基準状態から逸脱していないときの受信信号に換算して受信して、見かけ上は、傾斜角に関する特定の受信座標軸の特徴要素が上記基準状態であるとすることができる。
【００９５】
具体例を挙げて説明すると、図６に示すように、ｙ軸に平行な磁気双極子Ｈyとｚ軸に平行な磁気双極子Ｈzが、ｘ軸まわりにおいてｙ軸からｚ軸へ回転（ロール、０≦ψ＜２π）したとすると次式が成立する。
【００９６】
【数３５】
2005283284.tif 000037
【００９７】
そして、回転前の座標系のｙ軸とｚ軸におけるベクトル成分は、次式で表される。
【００９８】
【数３６】
2005283284.tif 000038
【００９９】
従って、必要な磁界の強さをＨとすると、傾斜角に関する特定の送信座標軸の特徴要素の上記基準状態からの逸脱量（ｘ軸まわりにおけるｙ軸からｚ軸へのロール角：ψ）に応じて回転前の座標系のｙ軸に平行な送信磁界Ｈを合成することで、見かけ上は、傾斜角に関する特定の送信座標軸の特徴要素が上記基準状態にある（ｘ軸まわりにおけるｙ軸からｚ軸へのロール角＝０°）とするためには、（７３）式＝Ｈ、（７４）式＝０が成立すればよいので、次式で表される送信磁界を送信すればよい。
【０１００】
【数３７】
2005283284.tif 000039
【０１０１】
同様に、回転前の座標系のｚ軸に平行な送信磁界Ｈを合成するには、（７３）式＝０、（７４）式＝Ｈが成立すればよいので、次式で表される送信磁界を送信すればよい。
【０１０２】
【数３８】
2005283284.tif 000040
【０１０３】
尚、磁界受信機側の磁界センサの場合も、（７３）式と（７４）式より全く同様に求めることができる。
【０１０４】
＜２＞
上記実施形態では、２つの物体１０、２０の相対位置及び相対姿勢を求めたが、その相対座標を、重力と地磁気を基準とした地図座標に変換することもできる。例えば、磁界送信機１１に併設されている送信機側角度検出器１２及び磁界受信機２１に併設されている受信機側角度検出器２２の少なくとも一方に、互いに直交する２軸の、鉛直方向に対する傾斜角を夫々測定可能な傾斜計、及び、地磁気を利用して方位角を測定可能な方位計を設けることで、上記実施形態で求められた相対座標を重力と地磁気を基準とした地図座標に変換することができる。
【０１０５】
＜３＞
上記実施形態では、２つの物体１０、２０に磁界送信機１１と磁界受信機２１とを設けることで２つの物体１０、２０の相対位置及び相対姿勢を求めたが、同一の物体上の２つの部位に磁界送信機１１と磁界受信機２１とを設け、その２つの部位の相対位置と相対姿勢とを求めるような利用方法も可能である。例えば、たわむように配置されている配管の２つの部位に磁界送信機１１と磁界受信機２１とを設けることで、２つの部位間の相対位置及び相対姿勢で表される配管のたわみ具合を求めることができる。
【図面の簡単な説明】
【０１０６】
【図１】２つの物体の配置状態を示す図
【図２】１軸の磁気双極子による磁界を示す説明図
【図３】３軸の磁気双極子と３軸の磁気センサの配置関係を表す座標系を示す説明図
【図４】回転後の３軸の磁気双極子と回転前の３軸の磁気センサの配置関係を表す座標系を示す説明図
【図５】回転後の３軸の磁気双極子と回転後の３軸の磁気センサの配置関係を表す座標系を示す説明図
【図６】傾斜角が水平でない時の座標系の回転を示す説明図
【符号の説明】
【０１０７】
１０  物体
１１  磁界送信機
１２  送信機側角度検出器
２０  物体
２１  磁界受信機
２２  受信機側角度検出器
【図１】
2005283284.tif 000041
【図２】
2005283284.tif 000042
【図３】
2005283284.tif 000043
【図４】
2005283284.tif 000044
【図５】
2005283284.tif 000045
【図６】
2005283284.tif 000046