(57)【要約】      （修正有）【課題】模型の位置および姿勢の検出精度を向上させること。【解決手段】磁力によって空中に浮揚させられている模型の、Ｘ、Ｙ、Ｚ座標値で定義される模型の位置、およびＸ軸周りのロール角度、Ｙ軸周りのピッチ角度、Ｚ軸周りのヨー角度で定義される模型の姿勢を推定するために、模型に記された位置姿勢検出用の第１のマークをＺ軸方向から観察する第１の観察装置と、模型に記された位置姿勢検出用の第２のマークをＹ軸方向から観察する第２の観察装置とを設け、第１の観察装置および第２の観察装置によって取得された第１のマークおよび第２のマークの検出データを用いて模型のＸ，Ｙ，Ｚ座標値の変位量およびピッチ角度およびヨー角度を推定する第１の過程と、第１の過程にて推定された模型のＸ，Ｙ，Ｚ座標値の変位量およびピッチ角度およびヨー角度と、予め登録されているロール角度補正式とを用いて、模型のロール角度を推定する第２の過程とを具備する。【選択図】図２
2008281418.tif 000002
【特許請求の範囲】
【請求項１】
磁力によって空中に浮揚させられている模型に対して位置検出の基準となるＸＹＺ直交座標系を定義し、Ｘ、Ｙ、Ｚ座標値で定義される前記模型の位置、およびＸ軸周りのロール角度、Ｙ軸周りのピッチ角度、Ｚ軸周りのヨー角度で定義される前記模型の姿勢を推定する位置姿勢推定方法であって、
前記模型に記された位置姿勢検出用の第１のマークをＺ軸方向から観察する第１の観察装置と、前記模型に記された位置姿勢検出用の第２のマークをＹ軸方向から観察する第２の観察装置とを設け、前記第１の観察装置および前記第２の観察装置によって取得された前記第１のマークおよび前記第２のマークの検出データを用いて前記模型のＸ，Ｙ，Ｚ座標値の変位量およびピッチ角度およびヨー角度を推定する第１の過程と、
前記第１の過程にて推定された前記模型のＸ，Ｙ，Ｚ座標値の変位量およびピッチ角度およびヨー角度と、予め登録されているロール角度補正式とに基づいて、前記模型のロール角度を推定する第２の過程と
を具備する位置姿勢推定方法。
【請求項２】
前記ロール角度補正式は、前記ＸＹＺ直交座標系において基準位置姿勢にある模型の位置および姿勢を変位させたときの変位前後の位置姿勢の関係を表す第１の関係式、並びに、前記模型を前記Ｙ軸方向に水平移動させたときに前記第２の観察装置により取得される検出データの前記第２のマークの解像度に関する第２の関係式を用いて導出される請求項１に記載の位置姿勢推定方法。
【請求項３】
前記ロール角度補正式は、以下の（８）式で表される請求項１または請求項２に記載の位置姿勢推定方法。
【数１】
2008281418.tif 000011
【請求項４】
少なくともＸ、Ｙ、Ｚ座標値からなる３つのパラメータの設定値を入力することにより、前記模型を所望の位置に移動させることが可能なステージに模型を設置し、
ロール角を基準角度で一定に保ったまま、各パラメータの設定値をそれぞれ独立して所定間隔で変化させることで、前記模型の位置を所定間隔でそれぞれ個別に変位させ、
変位後の各位置における前記模型を前記第１の観察装置および前記第２の観察装置により観察し、
前記第１の観察装置および前記第２の観察装置の検出データと、前記ロール角度補正式とから前記模型のロール角度をそれぞれ算出し、
算出した前記模型のロール角度と前記基準角度との誤算を算出し、
該誤差が予め設定されている許容範囲を超えていた場合に、前記ロール角度補正式を補正する請求項１から請求項３のいずれかに記載の位置姿勢推定方法。
【請求項５】
前記第１の観察装置により前記第２のマークを検出し、この検出データと前記ロール角度補正式とを用いて、ロール角度を推定し、
該ロール角度が前記第２の観察装置による検出データを用いて推定されたロール角度よりも小さい場合には、前記第１の観察装置の検出データを用いて推定されたロール角度を採用する請求項１から請求項４のいずれかに記載の位置姿勢推定方法。
【請求項６】
磁力によって空中に浮揚させられている模型に対して位置検出の基準となるＸＹＺ直交座標系を定義し、Ｘ、Ｙ、Ｚ座標値で定義される前記模型の位置、およびＸ軸周りのロール角度、Ｙ軸周りのピッチ角度、Ｚ軸周りのヨー角度で定義される前記模型の姿勢を推定する位置姿勢推定装置であって、
前記模型に記された位置姿勢検出用の第１のマークをＺ軸方向から観察する第１の観察装置と、
前記模型に記された位置姿勢検出用の第２のマークをＹ軸方向から観察する第２の観察装置と、
前記第１の観察装置および前記第２の観察装置によって取得された前記第１のマークおよび前記第２のマークの検出データが入力される処理装置と
を備え、
前記処理装置は、
前記第１のマークおよび前記第２のマークの検出データを用いて前記模型のＸＹＺ座標値の変位量およびピッチ角度およびヨー角度を推定する第１の処理と、
前記第１の処理にて推定された前記模型のＸＹＺ座標値の変位量およびピッチ角度およびヨー角度と予め登録されているロール角度補正式とを用いて前記模型のロール角度を推定する第２の処理と
を実行する位置姿勢推定装置。
【請求項７】
請求項６に記載の位置姿勢推定装置を備える磁力支持天秤装置。
【請求項８】
磁力によって空中に浮揚させられている模型に対して位置検出の基準となるＸＹＺ直交座標系を定義し、Ｘ、Ｙ、Ｚ座標値で定義される前記模型の位置、およびＸ軸周りのロール角度、Ｙ軸周りのピッチ角度、Ｚ軸周りのヨー角度で定義される前記模型の姿勢を推定する位置姿勢推定方法であって、
前記模型に記された位置姿勢検出用の第１のマークをＺ軸方向から観察する第１の観察装置と、前記模型に記された位置姿勢検出用の第２のマークをＹ軸方向から観察する第２の観察装置と、前記模型に記されたロール角検出用の第３のマークをＸ軸方向から観察する第３の観察装置とを設け、前記第１および前記第２の観察装置によって取得された前記第１のマークおよび前記第２のマークの検出データを用いて前記模型の位置およびピッチ角度およびヨー角度を推定する第１の過程と、
前記第３の観察装置によって取得された前記第３のマークの検出データを用いて前記模型のロール角度を推定する第２の過程と
を具備する位置姿勢推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、空中に磁力によって支持される模型の位置および姿勢を推定する位置姿勢推定方法に関するものである。
【背景技術】
【０００２】
空洞内において、模型を気流中に支持する装置として、磁力支持天秤装置（Magnetic Suspension and Balance System）が知られている（例えば、特許文献１参照）。
この磁力支持天秤装置は、磁力により模型を空中で静止させるものである。磁力制御のパラメータは、模型の現在の位置および姿勢をもとに決定されるため、より安定した磁力支持を行うためには、できるだけ模型の位置や姿勢を高精度で推定することが重要となる。
【０００３】
模型の位置・姿勢の検出方法としては、例えば、非特許文献１に開示されるものがある。非特許文献１には、ＸＹＺ直交座標系に配置された模型をＺ軸方向（上方）から観察する上方カメラと、Ｙ軸方向から観察する水平カメラとを設け、上方カメラから模型の編揺角（Ｚ軸回りの角度）、Ｙ軸方向の位置、Ｘ軸方向の位置を測定し、水平カメラでＺ軸方向の位置と縦揺角（Ｙ軸回りの角度）を測定することにより、模型の位置および姿勢を推定することが開示されている。
【特許文献１】特開２００３−３４４２１５号公報
【非特許文献１】澤田  秀夫、外２名、”高亜音速用小型磁力支持天秤装置の予備的試作検討結果について”、２００１年１０月２９−３１日、第３９回飛行機シンポジウム講演集
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら、上述したような非特許文献１に開示されている位置姿勢の推定方法では、Ｘ軸回りの角度（ロール角度）については測定しておらず、模型の位置・姿勢を高精度で推定することができないという問題があった。
【０００５】
本発明は、上記問題を解決するためになされたもので、模型の位置および姿勢の検出精度を向上させることのできる位置姿勢推定方法を提供することを目的とする。
【課題を解決するための手段】
【０００６】
上記課題を解決するために、本発明は以下の手段を採用する。
本発明は、磁力によって空中に浮揚させられている模型に対して位置検出の基準となるＸＹＺ直交座標系を定義し、Ｘ、Ｙ、Ｚ座標値で定義される前記模型の位置、およびＸ軸周りのロール角度、Ｙ軸周りのピッチ角度、Ｚ軸周りのヨー角度で定義される前記模型の姿勢を推定する位置姿勢推定方法であって、前記模型に記された位置姿勢検出用の第１のマークをＺ軸方向から観察する第１の観察装置と、前記模型に記された位置姿勢検出用の第２のマークをＹ軸方向から観察する第２の観察装置とを設け、前記第１の観察装置および前記第２の観察装置によって取得された前記第１のマークおよび前記第２のマークの検出データを用いて前記模型のＸ，Ｙ，Ｚ座標値の変位量およびピッチ角度およびヨー角度を推定する第１の過程と、前記第１の過程にて推定された前記模型のＸ，Ｙ，Ｚ座標値の変位量およびピッチ角度およびヨー角度と、予め登録されているロール角度補正式とに基づいて、前記模型のロール角度を推定する第２の過程とを具備する位置姿勢推定方法を提供する。
【０００７】
模型のロール角度を推定する場合、例えば、模型の軸線方向に沿って記された第２のマークを第２の観察装置によって検出し、この第２のマークの検出位置に基づいてロール角度を推定することが考えられる。しかしながら、模型の軸線方向に沿って記された第２のマークの位置は、ロール角度が変化したときだけでなく、例えば、模型がＺ軸方向に水平移動した場合や、Ｙ軸方向に水平移動した場合においても変化することから、この第２のマークを検出するだけではロール角度を正確に検出することが不可能である。つまり、ロール角度を推定するためには、模型のＹ座標値やＺ座標値がどのような位置にあるかを把握し、この位置を反映させる必要が生ずる。
一方、ロール角度以外のパラメータ、つまり、Ｘ，Ｙ，Ｚ座標値およびピッチ角度、ヨー角度については、他のパラメータに依存していないため、第１の観察装置および第２の観察装置によって取得された検出データを用いてそれぞれ独立に処理することで、高精度な位置・姿勢推定を行うことが可能である。
このような事項を考慮して、本発明では、他のパラメータに依存するロール角度を推定する前段階として、Ｘ，Ｙ，Ｚ座標値の変位量およびピッチ角度、ヨー角度からなる５つのパラメータを第１の観察装置および第２の観察装置の検出データを用いて推定し、この推定した５つのパラメータに基づいてロール角度を推定することとした。これにより、推定されたロール角度は、ロール角度に影響を及ぼすパラメータが考慮された値となる。これにより、ロール角度を単に推定するだけでなく、更に、その推定精度の向上を図ることが可能となる。
【０００８】
上記位置姿勢推定方法において、前記ロール角度補正式は、前記ＸＹＺ直交座標系において基準位置姿勢にある模型の位置および姿勢を変位させたときの変位前後の位置姿勢の関係を表す第１の関係式、並びに、前記模型を前記Ｙ軸方向に水平移動させたときに前記第２の観察装置により取得される検出データの前記第２のマークの解像度に関する第２の関係式を用いて導出されることとしてもよい。
【０００９】
上記位置姿勢推定方法において、前記ロール角度補正式は、例えば、以下の（８）式で表される。
【００１０】
【数２】
2008281418.tif 000003
【００１１】
上記位置姿勢推定方法において、少なくともＸ、Ｙ、Ｚ座標値からなる３つのパラメータの設定値を入力することにより、前記模型を所望の位置に移動させることが可能なステージに模型を設置し、ロール角を基準角度で一定に保ったまま、各パラメータの設定値をそれぞれ独立して所定間隔で変化させることで、前記模型の位置を所定間隔でそれぞれ個別に変位させ、変位後の各位置における前記模型を前記第１の観察装置および前記第２の観察装置により観察し、前記第１の観察装置および前記第２の観察装置の検出データと、前記ロール角度補正式とから前記模型のロール角度をそれぞれ算出し、算出した前記模型のロール角度と前記基準角度との誤算を算出し、該誤差が予め設定されている許容範囲を超えていた場合に、前記ロール角度補正式を補正することとしてもよい。
【００１２】
このように、ロール角度補正式を補正するので、ロール角度補正式に用いられている各パラメータの誤差を低減させることが可能となる。これにより、ロール角度の推定精度をさらに向上させることが可能となる。
【００１３】
上記位置姿勢推定方法において、前記第１の観察装置により前記第２のマークを検出し、この検出データと前記ロール角度補正式とを用いて、ロール角度を推定し、該ロール角度が前記第２の観察装置による検出データを用いて推定されたロール角度よりも小さい場合には、前記第１の観察装置の検出データを用いて推定されたロール角度を採用することとしてもよい。
【００１４】
例えば、ロール角度が所定値以上になってしまうと、第２の観察装置による第２のマークの検出精度が低下するおそれがある。一方、ロール角度が所定値以上になった状態では、第２の観察装置よりも第１の観察装置による検出精度が上がる可能性がある。そこで、２つの観察装置によってそれぞれ第２のマークを検出し、この検出データを用いてそれぞれロール角度を推定し、推定したロール角度を比較して好ましい方を採用することにより、第２の観察装置による検出データのみを用いてロール角度を推定するときよりも、推定精度を向上させることが可能となる。
【００１５】
本発明は、磁力によって空中に浮揚させられている模型に対して位置検出の基準となるＸＹＺ直交座標系を定義し、Ｘ、Ｙ、Ｚ座標値で定義される前記模型の位置、およびＸ軸周りのロール角度、Ｙ軸周りのピッチ角度、Ｚ軸周りのヨー角度で定義される前記模型の姿勢を推定する位置姿勢推定装置であって、前記模型に記された位置姿勢検出用の第１のマークをＺ軸方向から観察する第１の観察装置と、前記模型に記された位置姿勢検出用の第２のマークをＹ軸方向から観察する第２の観察装置と、前記第１の観察装置および前記第２の観察装置によって取得された前記第１のマークおよび前記第２のマークの検出データが入力される処理装置とを備え、前記処理装置は、前記第１のマークおよび前記第２のマークの検出データを用いて前記模型のＸＹＺ座標値の変位量およびピッチ角度およびヨー角度を推定する第１の処理と、前記第１の処理にて推定された前記模型のＸＹＺ座標値の変位量およびピッチ角度およびヨー角度と予め登録されているロール角度補正式とを用いて前記模型のロール角度を推定する第２の処理とを実行する位置姿勢推定装置を提供する。
【００１６】
上記位置姿勢推定装置は、例えば、磁力支持天秤装置に利用されて好適なものであり、この位置姿勢推定装置を採用することにより、磁力支持のためのコイルへの電流制御の精度を向上させることが可能となり、所望の位置姿勢で模型を支持することが可能となる。
【００１７】
本発明は、磁力によって空中に浮揚させられている模型に対して位置検出の基準となるＸＹＺ直交座標系を定義し、Ｘ、Ｙ、Ｚ座標値で定義される前記模型の位置、およびＸ軸周りのロール角度、Ｙ軸周りのピッチ角度、Ｚ軸周りのヨー角度で定義される前記模型の姿勢を推定する位置姿勢推定方法であって、前記模型に記された位置姿勢検出用の第１のマークをＺ軸方向から観察する第１の観察装置と、前記模型に記された位置姿勢検出用の第２のマークをＹ軸方向から観察する第２の観察装置と、前記模型に記されたロール角検出用の第３のマークをＸ軸方向から観察する第３の観察装置とを設け、前記第１および前記第２の観察装置によって取得された前記第１のマークおよび前記第２のマークの検出データを用いて前記模型の位置およびピッチ角度およびヨー角度を推定する第１の過程と、前記第３の観察装置によって取得された前記第３のマークの検出データを用いて前記模型のロール角度を推定する第２の過程とを具備する位置姿勢推定方法を提供する。
【００１８】
このように、ロール角度検出用の第３のマークをＸ軸方向から観察する第３の観察装置を設けたので、第３の観察装置によって検出されたデータを用いれば容易にロール角度を推定することが可能となる。
【発明の効果】
【００１９】
本発明によれば、模型の位置・姿勢の検出精度の向上を図ることができるという効果を奏する。
【発明を実施するための最良の形態】
【００２０】
以下に、本発明に係る位置姿勢推定方法および位置姿勢推定装置ならびに位置姿勢推定装置を備える磁力支持天秤装置の一実施形態について、図面を参照して説明する。
【００２１】
〔第１の実施形態〕
図１は、本実施形態に係る磁力支持天秤装置の斜視図である。
図１に示されるように、磁力支持天秤装置１は、風洞計測部の外周に配置される複数のコイル２と、これらのコイル２に電流等を供給する電源装置（図示略）、コイル２に供給される電流等を制御することにより、風洞計測部内に収容される模型Ａの位置および姿勢を制御する制御装置３と、風洞計測部内の模型Ａの位置および姿勢を検出する位置姿勢推定装置（図示略）とを備えている。
風洞計測の対象となる模型Ａは、例えば、アルミ等の非磁性材料で所望の形状に成形されている。模型Ａの内部には、磁化された物質、超伝導コイルのような電流を流し続けるコイル、或いは、永久磁石等の磁石が収容されている。
【００２２】
このような磁力支持天秤装置１では、コイル２に電流を流すことにより、模型Ａの周辺に磁場を発生させる。模型Ａに収容されている永久磁石は、この磁場と干渉して磁力を発生し、この磁力が模型Ａに作用することにより、模型Ａが風洞計測部内において浮揚支持される。コイルに流す電流は制御装置３によって制御されることにより、模型Ａの位置、姿勢を自由に変化させることができるようになっている。
【００２３】
次に、上記位置姿勢推定装置について図２を参照して説明する。
まず、模型Ａの位置検出の基準となるＸＹＺ直交座標系（基準座標）を定義する。図１に示されるように、本実施形態では、基準位置姿勢における模型Ａの軸線方向にＸ軸を定義し、Ｘ軸と直交し、鉛直方向へ向かう軸線をＺ軸と定義し、Ｘ軸とＺ軸との両方に直交する軸線をＹ軸と定義する。また、模型Ａが基準位置姿勢にあるときの模型Ａの中心は、ＸＹＺ直交座標軸の原点Ｏと一致している。また、Ｘ軸回りをロール、Ｙ軸周りをピッチ、Ｚ軸回りをヨーと定義し、基準位置姿勢からの変位量をそれぞれロール角度（φ）、ピッチ角度（θ）、ヨー角度（ψ）で示す。
【００２４】
図２は、位置姿勢推定装置１０に関する主な構成要素を示した斜視図である。図３は、模型Ａに記された軸線マーク（第２のマーク）と該軸線マークを検出する第２の観察装置との位置関係を示した図である。
図２および図３に示されるように、位置姿勢推定装置１０は、模型Ａの周方向に沿って描かれた周マーカ（第１のマーク）をＺ軸方向から観察する第１の観察装置１１と、模型Ａの軸線方向に沿って描かれた前マーカおよび後マーカからなる軸線マーカ（第２のマーク）を検出する第２の観察装置１２と、第１の観察装置１１および第２の観察装置１２によってそれぞれ取得された周マーカ、軸線マーカの検出データが入力される処理装置１３とを備えている。
【００２５】
上記第１の観察装置１１および第２の観察装置１３は、上記風洞計測部に対して移動可能に設置されているが、模型Ａの位置検出を行うために配置位置が一旦決定されると、以降、この位置で固定とされ、基準位置姿勢における模型Ａとの位置関係は不動のものとなる。
処理装置１３は、例えば、コンピュータにより構成されている。
【００２６】
図３に示されるように、軸線マーカは、２つの線分からなり、模型Ａにおいて前側に記されている線分を前マーカ、後側に記されている線分を後マーカとする。前マーカと後マーカとの間には、周マーカが記されている。
第１の観察装置１１および第２の観察装置１２は、Ｈ型に配置された３つのラインセンサによってそれぞれ構成されている。つまり、位置姿勢推定装置１０は、合計６つのラインセンサを備えていることとなる。ラインセンサは、例えば、ＣＣＤカメラ等の撮像素子によって構成されている。
【００２７】
図３において、模型Ａの上面（第１の観察装置１１が配置されている側）に描かれているＨ型の破線は、第１の観察装置１１による撮影ラインであり、模型Ａの側面（第２の観察装置が配置されている側）に描かれているＨ型の破線は、第２の観察装置１２による撮影ラインである。
【００２８】
本実施形態において、模型Ａは、例えば、各マーカが検出されやすいように白に着色されている。各マーカは検出されやすい色、例えば、黒で描かれている。また、図２に示されるように、これらマーカを検出しやすいように、模型Ａの上方および側方には照明１５が設けられている。
【００２９】
図４に、第２の観察装置１２によって取得される検出データの一例を示す。模型Ａは明るい色で着色されており、前マーカおよび後マーカは暗い色で着色されているので、輝度データのエッジを検出することにより、マーカを検出することが可能となる。
ここで、エッジはマーカの上側と下側とで合計２箇所存在するが、いずれか一方のエッジを採用する。本実施形態では、上側エッジＥを採用することとする。なお、図４の検出データにおいて、エッジｅは、いずれも模型と周囲との輝度差を検出したエッジである。
【００３０】
次に、上記構成からなる位置姿勢推定装置１０による位置、姿勢の検出方法について説明する。
ここでは、まず、位置、姿勢を検出する準備段階として行われるＸ，Ｙ，Ｚ座標値およびピッチ角度ならびにヨー角度をそれぞれ推定するのに使用される各補正式の作成過程について図５を参照して説明する。
【００３１】
まず、模型Ａを校正用６軸ステージに設置する（ステップＳＡ１）。この校正用６軸ステージは、Ｘ、Ｙ、Ｚの各座標値およびロール角度φ、ピッチ角度θ、ヨー角度ψの６つのパラメータの設定値を入力することにより、模型Ａを所望の位置および姿勢に変位させることが可能なステージである。
次に、Ｘ，Ｙ，Ｚ，θ，ψからなる５つのパラメータのうち、動かすパラメータを１つ決定する（ステップＳＡ２）。続いて、決定したパラメータを所定量変位させる。これにより、ステージは入力された各パラメータの値に応じて移動・回転することとなる（ステップＳＡ３）。移動後の位置または姿勢における模型Ａが第１の観察装置１１および第２の観察装置１２により撮影され、このときの検出データが処理装置１３に送られる。処理装置１３は、第１の観察装置１１および第２の観察装置１２からの検出データから周マーカのエッジ、前マーカの上側エッジ、および後マーカの上側エッジＥをそれぞれ検出し、撮影画像内におけるこれらエッジＥの位置と、該検出データが取得されたときのパラメータの設定入力値とを対応付けて記憶装置に記録する（ステップＳＡ４）。なお、記憶装置は、処理装置１３に内蔵されているものでもよく、外部端子を介して着脱可能に接続される記憶媒体でもよい。
【００３２】
続いて、指定範囲内のデータが取得されたか否かを判定する（ステップＳＡ５）。例えば、Ｘ，Ｙ，Ｚのパラメータであれば、原点Ｏに対してプラス側およびマイナス側に所定距離移動させたときのデータが取得できたか否かを、また、θ、ψであれば、原点を中心に所定角度回転させたか否かを判定する。この結果、所定範囲にわたるデータが取得できていなければ（ステップＳＡ５において「ＮＯ」）、ステップＳＡ３に戻りデータ取得を引き続き行い、データが取得できていれば、ステップＳＡ６に進み、５つのパラメータについてデータ取得が終了したかを判定する。この結果、５つのパラメータ全てについてデータが取得できていなければ（ステップＳＡ６において「ＮＯ」）、ステップＳＡ２に戻り、データ取得が終了していないパラメータの１つを設定し、このパラメータに関して上述したステップＳＡ３からステップＳＡ５を実行することにより、データ取得を行う。
【００３３】
一方、５つのパラメータ全てについてデータが取得できていた場合には、取得したデータに基づいて各パラメータの補正式を作成する（ステップＳＡ７）。この補正式は、上述した第１の観察装置１１及び第２の観察装置１２によって撮影された各撮影画像内におけるエッジＥの位置から模型Ａの位置および姿勢を推定するための変換式に相当する。具体的には、Ｘであれば、Ｘを変位させたときの第１の観察装置１１による検出データを取得し、この検出データにおける各撮影画像内における周マーカのエッジの位置と、Ｘ変位量（ΔＸ）との関係から補正式を求める。Ｙ，ψについても同様に、第１の観察装置１１によって取得された検出データを用いて補正式をそれぞれ独立に求める。また、Ｚ、θについては、第２の観察装置１２によって取得された検出データを用いて、補正式をそれぞれ独立に求める。
【００３４】
ΔＸ，ΔＹ，ΔＺ，θ，ψからなる５つのパラメータについて、それぞれの補正式が作成されると、処理装置１３はこれらの補正式を記憶装置に記録し、当該準備処理を終了する。このように、補正式が記憶装置に記録されることで、実際の位置・姿勢の推定処理においては、第１の観察装置１１および第２の観察装置１２からの検出データと補正式とからΔＸ，ΔＹ，ΔＺ，θ、ψの５つの位置・姿勢の推定が可能となる。
【００３５】
一方、上述したように、ロール角度φについては、上記５つのパラメータの影響により値が変化するために、上述した５つのパラメータと同様の手順による補正式の作成方法では対応することができない。そこで、以下に示すような方法によって、ロール角度補正式を求めることとする。
ここでは、まず、ロール角度補正式の導出について概念を説明する。
【００３６】
例えば、今、図６に示されるように、基準位置姿勢の模型Ａにおいて、後マーカの上側エッジ上に任意の点（Ｘ1，Ｙ1，Ｚ1）を定義する。続いて、基準位置姿勢にある模型ＡをＸ軸方向、Ｙ軸方向、Ｚ軸方向に所定量ΔX，ΔY，ΔZ平行移動させ、更に、Ｘ軸回り、Ｙ軸回り、Ｚ軸周りに所定角度量θ，φ，ψそれぞれ回転させる。ここで、移動・回転後の模型Ａにおける該点（Ｘ1，Ｙ1，Ｚ1）に相当する座標を点（Ｘ2，Ｙ2，Ｚ2）とする。移動前の点（Ｘ1，Ｙ1，Ｚ1）と移動後の点（Ｘ2，Ｙ2，Ｚ2）との間には、以下の（１）式に示される関係が成り立つ。
以下の（１）式において、ロール角度φが今、求めたいパラメータである。
【００３７】
【数３】
2008281418.tif 000004
【００３８】
また、上記（１）式を展開すると、以下の（２）式から（４）式が得られる。
【００３９】
【数４】
2008281418.tif 000005
【００４０】
ここで、上記（２）式から（４）式までの３つの式には、１３個のパラメータＸ1，Ｙ1，Ｚ1，Ｘ2，Ｙ2，Ｚ2，ΔＸ，ΔＹ，ΔＺ，θ，ψ，φが含まれているが、このうち未知のパラメータは以下の表１に示されるように４つ存在する。つまり、パラメータＹ1，Ｚ1，Ｘ2，ΔＸ，ΔＹ，ΔＺ，θ，ψについては、既知パラメータであり、パラメータＸ1，Ｙ2，Ｚ2，φについては、未知パラメータである。
以下、この理由について簡単に説明する。
【００４１】
【表１】
2008281418.tif 000006
【００４２】
まず、模型Ａが基準位置姿勢にあるときに、第２の観察装置１２によって軸線マーカを検出する場合、軸線マーカは模型Ａの軸線方向に沿って記されているので、第２の観察装置１２によって検出されるデータＸ1，Ｙ1，Ｚ1のうち、Ｙ1とＺ1の値は一定となる。一方、Ｘ1については、軸線に沿って記されているマークのうち、どの位置を第２の観察装置１２によって検出するかにより、Ｘ1の値は変化してしまうため、値を特定することができない。したがって、Ｘ1は未知パラメータ、Ｙ1，Ｚ1は既知パラメータとなる。
【００４３】
次に、移動後の模型Ａを考えると、第２の観察装置１２によって検出されるデータＸ2，Ｙ2，Ｚ2のうち、Ｘ2については、Ｘ軸方向における原点Ｏから第２の観察装置１２のスキャン位置までの距離とすることができる。一方、Ｙ2については、Ｙ軸は第２の観察装置１２からみて奥行方向に相当するため、計測は不可能となる。
【００４４】
また、Ｚ2については、通常、第２の観察装置１２によって取得された撮影画像において、画像中心点から後マーカの上側エッジＥまでの画素数に基づいて距離が決定される。ここで、模型ＡがＹ軸方向において第２の観察装置１２の近くにあるときと、離れた位置にあるときとでは、撮影画像における後マークの解像度が変化する。つまり、Ｚ軸方向における模型Ａの位置を固定した状態で、模型ＡをＹ軸方向に水平移動させた場合、Ｙ軸の位置に応じて撮影画像中の後マーカの大きさが変化することとなる。このため、撮影画像の中心から後マーカの上側エッジＥまでの画素数はＹ座標値に応じて変化することとなり、Ｚ軸方向における位置が変化していないにもかかわらず、検出されるＺ座標値は異なる値を示すこととなる。このように、Ｚ2はＹ2に依存するパラメータであるため、上述のようにＹ2が未知パラメータである以上、Ｚ2も未知パラメータとなる。
また、ロール角φは、この値を求めたいのであるから、当然ながら未知パラメータである。
また、変位量ΔＸ，ΔＹ，ΔＺ，θ，ψについては、上述した各パラメータに対応する補正式を用いて演算により求めることができ、既知である。
【００４５】
上述のように、３つの関係式の中に４つの未知パラメータが存在するため、このままでは未知のパラメータＸ1，Ｙ2，Ｚ2，φを解くことができない。そこで、４つめの関係式を用意する。
【００４６】
今、図７に示すように、移動後の模型Ａ（図６の下段参照）に対して、ＸＹＺ直交座標系の原点Ｏを通りＸ−Ｚ平面に平行な基準面Ｂを仮定する。ここで、基準面ＢのＺ方向の長さをＳB、第２の観察装置１２と基準面Ｂとの距離をＤSとする。また、第２の観察装置１２により検出される後マーカの上側エッジＥの座標は、当然（Ｘ2，Ｙ2，Ｚ2）となる。ここで、基準面Ｂを底辺として、その両端から第２の観察装置の中心までを直線で結んだ場合に形成される三角形を考えたとき、後マーカの上側エッジＥにおける相似三角形の底辺を観察面Ｃとする。
この関係から、三角形の相似を利用して、以下の（５）式が得られる。
【００４７】
ＳB：ＳC＝ＤS：ＤS−Ｙ2            （５）
ここで、ＳB，ＳCはそれぞれ基準面Ｂ、観測面Ｃの長さを表す。
また、基準面Ｂの画像上での長さ（画素数）をＰHとした場合、観測面ＣもＰHの画素数で撮影されており、観察面Ｃでの分解能は上記（５）式を用いて以下の（６）式で表される。
【００４８】
【数５】
2008281418.tif 000007
【００４９】
このことから、撮影画像における後マーカの上側エッジＥの画素位置、並びに上記（６）式で表される観測面Ｃにおける分解能から、後マーカの上側エッジＥのＺ軸方向の座標値Ｚ2は、次の（７）式で表すことができる。
【００５０】
【数６】
2008281418.tif 000008
【００５１】
この（７）式を上記（２）から（４）式に加え、ロール角度φについて解くと、以下の（８）式が導出される。
【００５２】
【数７】
2008281418.tif 000009
【００５３】
次に、上述した概念に基づく実際の処理について説明する。
図８は、ロール角度の推定処理に用いられる補正式の作成手順を示したフローチャートである。この工程は、例えば、図５に示したロール角度以外のパラメータ、即ち、Ｘ，Ｙ，Ｚ，θ、ψに関する補正式の算出処理の後に行われる。なお、上述した（８）式については、予め処理装置１３の記憶装置に登録されているものとする。以下に示す処理手順は、（８）式に用いられる既知のパラメータを取得するためのものである。
【００５４】
まず、模型Ａを基準位置姿勢とし、模型Ａの原点Ｏから第２の観察装置１２による後マークのスキャン位置までのＸ軸方向における距離を計測する。処理装置１３は、この距離をパラメータＸ2として記憶装置に記録する（ステップＳＢ１）。
続いて、第２の観察装置１２により基準位置姿勢にある模型Ａの後マーカを撮影し、検出データを取得する。処理装置１３は、この検出データから後マーカの上側エッジＥのＹ座標値およびＺ座標値を取得し、これらそれぞれの座標値をそれぞれパラメータＹ1，Ｚ1として記憶装置に記録する（ステップＳＢ２）。
【００５５】
次に、模型Ａの原点Ｏから第２の観察装置１２の設置位置までのＹ軸方向における距離を計測する。処理装置１３は、この距離をパラメータＤSとして記憶装置に記録する（ステップＳＢ３）。続いて、原点Ｏを通り、Ｘ−Ｚ平面に平行な基準面Ｂを設定する。このとき、基準面ＢのＺ軸方向の長さはＳBとする（ステップＳＢ４）。
次に、この基準面Ｂを第２の観察装置１２により撮影し、このときの検出データからＳBの画素数ＰHを第２の観察装置１２の画角から計算する（ステップＳＢ５）。
これにより、上記（８）式として表されたロール角度φの補正式において、必要となる既知のパラメータ、つまり、Ｘ2，Ｙ1，Ｚ1，ＤS，ＰH，ＳBの全てが取得されることとなる。
【００５６】
次に、図５に示した処理によって作成された各パラメータの補正式および上述したロール角度補正式を用いて実際に模型Ａの位置姿勢を推定する場合について図９を参照して説明する。
まず、第１の観察装置１１および第２の観察装置１２によって模型Ａが撮影されることにより、周マーカおよび軸線マーカが検出され、この検出データが処理装置１３に送られる。処理装置１３は、各検出データから撮影画像中における周マーカおよび軸線マーカの所定のエッジＥの位置を検出し、Ｘ、Ｙ、Ｚ座標値の変位量ΔＸ，ΔＹ，ΔＺおよびピッチ角度およびロー角度を推定するために必要となるデータを取得する（ステップＳＣ１）。
次に、処理装置１３は、ステップＳＣ１において取得したデータと予め記憶装置に登録されている各パラメータに対応する補正式とを用いて、ロール角度以外のパラメータ、即ち、ΔＸ，ΔＹ，ΔＺ，θ、ψの値を推定する（ステップＳＣ２）。
次に、ステップＳＣ２において推定した５つの位置・姿勢パラメータΔＸ，ΔＹ，ΔＺ，θ、ψを（８）式で示されるロール角度補正式に代入し、ロール角度φを推定する（ステップＳＣ３）。
【００５７】
以上説明してきたように、本実施形態に係る位置姿勢推定方法、位置姿勢推定装置、並びに磁力支持天秤装置によれば、ロール角度を検出することができる上に、更に、ロール角度の検出に影響を及ぼす各パラメータΔＸ，ΔＹ，ΔＺ，θ、ψを用いて、ロール角度φを推定するので、ロール角度の推定精度の向上を図ることが可能となる。
【００５８】
ところで、上述した第１の実施形態においては、模型の原点から第２の観察装置１２までの距離Ｘ２等を作業員等が巻尺等を用いて計測するなどして、ロール角度推定に必要となる既知のパラメータを取得している。このため、取得されたパラメータの中には誤差が含まれ、ロール角度の推定精度を低下させるおそれがある。
そこで、上述した（８）式に用いられる各既知のパラメータに含まれる誤差を低減するための修正過程を更に設けることとしてもよい。
以下、修正過程について図１０および図１１を参照して説明する。
【００５９】
まず、Ｘ，Ｙ，Ｚのパラメータのうちのいずれかを選択する（ステップＳＤ１）。続いて、選択したパラメータを所定間隔で変化させたときの検出データを記憶装置から抽出する（ステップＳＤ２）。この検出データは、図５に示した過程を実施することにより取得された検出データである。
次に、抽出した複数の検出データのいずれか一つを選択し（ステップＳＤ３）、この検出データと各補正式とを用いて各パラメータΔＸ，ΔＹ，ΔＺ，θ、ψを推定し（ステップＳＤ４）、推定したこれらパラメータΔＸ，ΔＹ，ΔＺ，θ、ψを（８）式のロール角度補正式に代入することで、ロール角度を推定する（ステップＳＤ５）。
【００６０】
次に、推定したロール角度と上記検出データが得られたときの真のロール角度との誤差を算出する（ステップＳＤ６）。なお、上記検出データは、基準位置姿勢にある模型ＡをＸ軸方向にのみ、或いは、Ｙ軸方向にのみ、或いは、Ｚ軸方向にのみ、移動させたときに得られた検出データであることから、ここでのロール角度の真の値はいずれもゼロとなる。
【００６１】
続いて、上記誤差が予め設定されている許容範囲内であるか否かを判定する（図１１のステップＳＤ７）。この結果、誤差が許容範囲を超えていた場合には（ステップＳＤ７において「YES」）、上述のステップＳＤ１において選択したパラメータがいずれであるかを判定し、Ｘが選択したパラメータであった場合にはＸ2を修正し（ステップＳＤ８，９）、Ｙが選択したパラメータであった場合にはＹ1、ＤSを修正し（ステップＳＤ１０，１１）、Ｚが選択したパラメータであった場合にはＺ1を修正する（ステップＳＤ１２，１３）。
【００６２】
次に、ステップＳＤ１４に進み、上記ステップＳＤ２において抽出した複数の検出データの中から、他の検出データを選択し、この検出データを用いて、上述と同様に各パラメータΔＸ，ΔＹ，ΔＺ，θ、ψを推定し、推定したこれらパラメータを修正後のロール角度補正式に代入することで、ロール角度を推定する（ステップＳＤ１５）。
【００６３】
続いて、ステップＳＤ６に戻り、ステップＳＤ１５において推定したロール角度と真のロール角度との誤差を算出し、続くステップＳＤ７において、この誤差が許容範囲内であるか否かを判定する。この結果、誤差が許容範囲内でなければ（ステップＳＤ７において「YES」）、ステップＳＤ８以降の処理を行い、ロール角度補正式の修正を引き続き行う。
【００６４】
一方、ステップＳＤ７において、誤差が許容範囲内であると判断された場合には（ステップＳＤ７において「NO」）、ステップＳＤ１６に進み、上述したステップＳＤ１において、Ｘ，Ｙ，Ｚのうち、全てのパラメータを選択したか否かを判定する。この結果、全てのパラメータを選択していなかった場合には、上述したステップＳＤ１に戻り、他のパラメータを選択し、ステップＳＤ２以降の処理を引き続き行う。一方、全てのパラメータについて選択していた場合には、記憶装置に記録されているロール角度補正式を修正後のロール角度補正式に更新し、当該補正処理を終了する。
【００６５】
このように、ロール角度補正式を用いて推定したロール角度と実際のロール角度とを比較して、ロール角度補正式を修正するので、ロール角度補正式に含まれている各パラメータの誤差を低減することができ、ロール角度の推定精度を向上させることが可能となる。
【００６６】
〔第２の実施形態〕
次に、本発明の第２の実施形態について説明する。
上述した第１の実施形態では、Ｚ軸方向から模型Ａを観察する第１の観察装置１１によって模型Ａの周マーカを検出し、また、Ｙ軸方向から模型Ａを観察する第２の観察装置１２により軸線マーカを検出し、これら検出データを用いて、位置および姿勢の推定を行っていた。ここで、ロール角度φが４５°を超えて変位してしまうと、ロール角度φの変位量に対する軸線マーカの検出位置の変化の割合が小さくなってしまう。つまり、角度分解能が悪化する。
そこで、本実施形態においては、Ｚ軸方向から模型Ａを観察する第１の観察装置によっても軸線マークを検出することとし、第１の観察装置によって検出された軸線マークの情報も用いて模型Ａの位置および姿勢を推定する。
【００６７】
具体的には、処理装置１３は、第２の観察装置１２によって検出された軸線マーカの情報を用いたロール角度の推定処理と、第１の観察装置１１によって検出された軸線マーカの情報を用いたロール角度の推定処理とを平行して行い、推定された２つのロール角度φのうち、小さい方を採用する。
【００６８】
以上説明してきたように、第１の観察装置１１によっても軸線マーカを検出し、この軸線マーカの検出データを用いてロール角度の推定を行うことにより、ロール角度の変位が大きい場合でも検出精度の低下を抑制し、一定値以上の精度を維持することが可能となる。
【００６９】
〔第３の実施形態〕
次に、本発明の第３の実施形態について説明する。
上述した第１の実施形態では、Ｙ軸方向から模型Ａを観察する第２の観察装置１２からの検出データに基づいて模型Ａのロール角φを推定していた。本実施形態では、第２の観察装置１２に代えて、Ｘ軸方向から模型Ａを観察する第３の観察装置を設け、この第３の観察装置によるマークの検出データに基づいてロール角φの推定を行う。
【００７０】
具体的には、図１２および図１３に示されるように、模型Ａの前面または後面にロール角度φを検出するためのロール角度用マーカを記す。このロール角度用マーカは、例えば、模型Ａの軸線と交わり、かつ、軸線に直交する線分である。模型Ａの周辺には、Ｘ軸方向から模型Ａを観察する第３の観察装置１５が設けられている。
【００７１】
第３の観察装置１５は、図１２に示されるように、ロール角度用マークを２箇所Ｐ１、Ｐ２で検出するために、２つのラインセンサを備えている。２つのラインセンサは、Ｚ軸方向に沿って模型Ａを観察するようになっている。
【００７２】
ロール角度の推定を行う場合には、２つのラインセンサによってロール角度用マークが撮影され、その検出データが処理装置に送られる。処理装置は、各ラインセンサによって取得された検出データにおいて、ロール角度用マーカの検出位置Ｐ１およびＰ２を特定し、このＰ１とＰ２との位置関係からロール角φを推定する。各ラインセンサにおけるロール角度用マーカの上側エッジの検出位置をＰ１，Ｐ２とした場合、ロール角度φは以下の（９）式にて与えられる。
【００７３】
【数８】
2008281418.tif 000010
【００７４】
上記（９）式において、ｄはラインセンサ間の距離である。
【００７５】
このように、Ｘ軸方向から模型Ａを観察する第３の観察装置１５を設け、この第３の観察装置１５によって検出された検出データを用いてロール角度φを直接的に検出することにより、ロール角度の検出精度を更に向上させることが可能となる。また、直接的に検出することから、処理負担の軽減、処理時間の短縮等を図ることが可能となる。
【００７６】
以上、本発明の実施形態について図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、本発明の要旨を逸脱しない範囲の設計変更等も含まれる。
【図面の簡単な説明】
【００７７】
【図１】本発明の第１の実施形態に係る磁力支持天秤装置の斜視図である。
【図２】本発明の第１の実施形態に係る位置姿勢推定装置に関する主な構成要素を示した斜視図である。
【図３】模型Ａに記されたマークと該マークを検出するラインセンサとの位置関係を示した図である。
【図４】第２の観察装置によって取得される検出データの一例を示す。
【図５】位置、姿勢を検出する準備段階として行われる補正式の作成過程について示したフローチャートである。
【図６】基準位置姿勢の模型と移動後の模型との位置関係を説明するための図である。
【図７】ロール角度補正式を作成するために用いられる関係式について説明に用いられる図である。
【図８】ロール角度の推定処理に用いられる補正式の作成手順を示したフローチャートである。
【図９】図５に示した過程において作成された補正式等を用いて実際に模型の位置姿勢を検出する場合の処理手順を示したフローチャートである。
【図１０】ロール角度補正式を修正するための修正過程の手順を示したフローチャートである。
【図１１】ロール角度補正式を修正するための修正過程の手順を示したフローチャートである。
【図１２】本発明の第３の実施形態に係る位置姿勢推定装置が備える第３の観察装置およびロール角度用マークを示した図である。
【図１３】第３の観察装置により検出される検出データおよびロール角度の推定方法を説明するために用いられる図である。
【符号の説明】
【００７８】
１  磁力支持天秤装置
２  コイル
３  制御装置
１０  位置姿勢推定装置
１１  第１の観察装置
１２  第２の観察装置
１３  処理装置
１５  第３の観察装置
【図１】
2008281418.tif 000012
【図２】
2008281418.tif 000013
【図３】
2008281418.tif 000014
【図４】
2008281418.tif 000015
【図５】
2008281418.tif 000016
【図６】
2008281418.tif 000017
【図７】
2008281418.tif 000018
【図８】
2008281418.tif 000019
【図９】
2008281418.tif 000020
【図１０】
2008281418.tif 000021
【図１１】
2008281418.tif 000022
【図１２】
2008281418.tif 000023
【図１３】
2008281418.tif 000024