JP4914019B2 - 位置姿勢計測方法及び装置 - Google Patents

Description

本発明は、傾斜角センサの計測値に基づき、撮像装置または物体の位置と姿勢を直接的に算出するものに関する。

近年、複合現実感（Ｍｉｘｅｄ Ｒｅａｌｉｔｙ；ＭＲ）技術の研究が盛んである。ＭＲ技術は、現実空間とコンピュータによって作られる仮想空間を継ぎ目なく融合する技術である。ＭＲ技術の中でも、現実空間に仮想空間を重ね合わせて表示するＡｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ（ＡＲ、拡張現実感、増強現実感とも呼ばれる）技術が特に注目を集めている。

ＡＲの画像表示装置は、ビデオカメラなどの撮像装置によって撮影された現実空間の画像に、該撮像装置の位置及び姿勢に応じて生成された仮想空間（コンピュータグラフィクスにより描画された仮想物体や文字情報）の画像を重畳描画した合成画像を表示するビデオシースルー方式か、あるいは観察者の頭部に装着された光学シースルー型ディスプレイに、観察者の視点の位置及び姿勢に応じて生成された仮想空間の画像を表示する光学シースルー方式によって実現される。

ＡＲ技術は、患者の体表面に体内の様子を重畳表示する手術支援や、空き地に仮想のビルを重畳表示する建築シミュレーション、組立作業時に作業手順を重畳表示する組み立て支援など様々な分野への応用が期待される。

ＡＲ技術において最も重要な課題の一つは、現実空間と仮想空間の間の位置合わせをいかに正確に行うかということであり、従来より多くの取り組みが行われてきた。ＡＲにおける位置合わせの問題は、ビデオシースルー方式の場合はシーン中における（すなわちシーン中で規定された基準座標系における）撮像装置の位置及び姿勢を求める問題に帰結する。同様に、光学シースルー方式の場合は、シーン中における観察者の視点あるいはディスプレイの位置及び姿勢を求める問題となる。

前者の問題を解決する方法として、シーン中に複数の指標を配置あるいは設定し、撮像装置が撮影した画像上での指標の投影像の位置を検出することで、シーン中における該撮像装置の位置及び姿勢を求めることが一般的に行われている。また、後者の問題を解決する方法として、計測対象物（すなわち観察者の頭部あるいはディスプレイ）に撮像装置を装着し、前者と同様な方法によって該撮像装置の位置及び姿勢を求め、それに基づいて計測対象物の位置及び姿勢を求めることが一般的に行われている。

画像上の指標の投影像とその指標の三次元位置との対応をもとに撮像装置の位置及び姿勢を求める方法は、写真測量の分野において古くから提案されている（例えば、非特許文献１を参照）。また、前述の画像上の指標の投影像をもとに求めた撮像装置の位置及び姿勢を初期値とし、画像上での指標の投影像の実際の観察位置と、投影像の計算位置、すなわち指標の三次元位置と撮像装置の位置及び姿勢から計算される投影像の位置との誤差が最小になるように反復演算によって撮像装置の位置及び姿勢を最適化することが行われている（例えば、非特許文献２を参照）。

一方、撮像装置によって撮影された画像だけを用いるのではなく、ジャイロセンサ（厳密には、３軸方向の角速度を計測するための複数のジャイロセンサと、３軸方向の加速度を計測するための複数の加速度センサの組合せによって構成される３軸姿勢センサであるが、本明細では便宜上これをジャイロセンサと呼ぶことにする）を撮像装置に装着し、画像情報とジャイロセンサの情報を併用して撮像装置の位置及び姿勢を求めることも行われている。

特許文献１では、ジャイロセンサの出力する姿勢の計測値をそのまま撮像装置の姿勢として用い、該撮像装置の姿勢と画像上の指標の投影像及びその指標の三次元位置との対応をもとに撮像装置の位置を線形連立方程式を解くことによって求めている。また、非特許文献３では、ジャイロセンサの出力する姿勢の計測値のうち傾斜角成分をそのまま撮像装置の姿勢の傾斜角成分とし、前述した画像上での指標の投影像の観察位置と計算位置との誤差が最小化されるように、撮像装置の位置と姿勢の方位角成分を反復演算によって最適化することが行われている。
解析写真測量 改訂版，社団法人日本写真測量学会，１９８９． 加藤，Ｍ．Ｂｉｌｌｉｎｇｈｕｒｓｔ，浅野，橘："マーカー追跡に基づく拡張現実感システムとそのキャリブレーション"，日本バーチャルリアリティ学会論文誌，ｖｏｌ．４，ｎｏ．４，ｐｐ．６０７−６１６，１９９９． Ｋ．Ｓａｔｏｈ，Ｓ．Ｕｃｈｉｙａｍａ，ａｎｄ Ｈ．Ｙａｍａｍｏｔｏ："Ａ ｈｅａｄ ｔｒａｃｋｉｎｇ ｍｅｔｈｏｄ ｕｓｉｎｇ ｂｉｒｄ’ｓ−ｅｙｅ ｖｉｅｗ ｃａｍｅｒａ ａｎｄ ｇｙｒｏｓｃｏｐｅ，"Ｐｒｏｃ．３ｒｄ ＩＥＥＥ／ＡＣＭ Ｉｎｔ’ｌ Ｓｙｍｐ．ｏｎ Ｍｉｘｅｄ ａｎｄ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ（ＩＳＭＡＲ２００４），ｐｐ．８９−９８，２００４． 特開２００２−２５９９９２号公報

前述した、撮像装置によって撮影された画像上での指標の投影像のみを利用した手法では、得られる撮像装置の位置及び姿勢が、指標の画像上での検出誤差や、画像の解像度が有限であることが原因となって、間違った位置及び姿勢が得られたり、ジッタを生じることがあった。

また、ジャイロセンサにはドリフト誤差があるため、時間経過に伴って徐々に方位方向の計測値に誤差が生じてしまう。そのため、ジャイロセンサが出力する姿勢計測値をそのまま撮像装置の姿勢とした場合には、間違った撮像装置の位置が得られることがあった。

さらに、反復演算によって撮像装置の位置及び姿勢、または撮像装置の位置及び姿勢の方位角成分を求める場合には初期値が必要である。前フレームの結果を初期値として用いる場合に、前フレームで撮像装置の位置及び姿勢が求められなければ（例えば、前フレームで指標が撮影されなかった場合）初期値を得ることができなかった。また、撮影画像だけから算出した撮像装置の位置及び姿勢を初期値として用いる場合には、前述のように初期値そのものが誤ったものになる可能性があるため、その後の反復演算でも最適化できないことがあった。

本発明は以上の問題に鑑みてなされたものであり、本願請求項１記載の発明は、傾斜角センサが装着された撮像装置または物体の位置と姿勢を、反復演算を行うことなく直接的に求めることを目的とする。

また、本願請求項２記載の発明は、傾斜角センサが装着された撮像装置と傾斜角が装着された物体との相対的な位置と姿勢を反復演算を行うことなく直接的に求めることを目的とする。

本発明の目的を達成するために、本発明における位置姿勢計測装置は次の構成を備える。

本願請求項１記載の位置姿勢計測方法は、物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値入力工程と、前記撮像装置から撮影画像を入力する画像入力工程と、前記撮影画像から前記物体上の指標を検出する指標検出工程と、前記物体または前記撮像装置の姿勢の方位角成分を未知としてから、前記傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記傾斜角センサが装着された物体または撮像装置の位置と姿勢を直接的に算出する位置姿勢算出工程とを備えることを特徴とする。

本願請求項２記載の位置姿勢計測方法は、物体に装着された第１傾斜角センサの第１傾斜角計測値を入力する第１傾斜角計測値入力工程と、撮像装置に装着された第２傾斜角センサの第２傾斜角計測値を入力する第２傾斜角計測値入力工程と、前記撮像装置から撮影画像を入力する画像入力工程と、前記撮影画像から前記物体上の指標を検出する指標検出工程と、前記物体および前記撮像装置の姿勢の方位角成分を未知としてから、前記第１傾斜角計測値、前記第２傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記物体と前記撮像装置との相対的な位置と姿勢を直接的に算出する位置姿勢算出工程とを備えることを特徴とする。

本願請求項１記載の発明によれば、傾斜角センサが装着された撮像装置または物体の位置と姿勢を、反復演算を行うことなく直接的に求めることができる。

また、本願請求項２記載の発明は、傾斜角センサが装着された撮像装置と傾斜角が装着された物体との相対的な位置と姿勢を反復演算を行うことなく直接的に求めることができる。

以下、添付図面を参照して本発明の好適な実施形態について詳細に説明する。

［第１の実施形態］
本実施形態では、本発明の位置姿勢計測装置及びその方法を、ジャイロセンサを装着した撮像装置の位置及び姿勢の計測に適用した場合について説明する。なお、本実施形態におけるジャイロセンサとは、前述したように３軸方向の角速度を計測するための複数のジャイロセンサと、３軸方向の加速度を計測するための複数の加速度センサの組合せによって構成される３軸姿勢センサであり、傾斜角成分はある程度信頼できる計測値を出力するが、方位角成分はドリフト誤差を含んでいる。そのため、本実施形態はジャイロセンサにのみ適用されるものではなく、姿勢の傾斜角成分を計測可能な任意の傾斜角センサについて適用できるものである。

図１は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１００は、指標検出部１１０、姿勢入力部１２０、位置姿勢算出部１３０によって構成されており、撮像装置１４０に接続されている。また、ジャイロセンサ１５０は撮像装置１４０上に固定され、姿勢入力部１２０に接続されている。

図２は、本実施形態における座標系の関係を示している。基準座標系には、位置が既知の指標が配置または設定されているものとする。本実施形態に係る位置姿勢計測装置は、基準座標系に対する撮像装置の位置及び姿勢を計測する。本実施形態では、基準座標系のｙ_ｗ軸を鉛直上方向にとる。

ジャイロセンサ１５０は、撮像装置の基準座標系に対する姿勢を出力する。基準座標系に対する撮像装置の姿勢を表す３×３回転変換行列をＲ_ｗｃ、基準座標系に対する撮像装置の位置を表す３次元ベクトルをｔ_ｗｃと表す。Ｒ_ｗｃは、基準座標系のｚ_ｗ軸まわりの回転変換

軸まわりの回転変換

軸まわりの回転変換

を順に組み合わせることで表現可能であり、（式１）のように表すことができる

ここで、

はＲ_ｗｃの傾斜角成分、

はＲ_ｗｃの方位角成分であるため、

とおくと、
（式１）は（式２）のように記述できる（ここでｉはｉｎｃｌｉｎａｔｉｏｎ（傾斜角）、ａはａｚｉｍｕｔｈ（方位角）を表す）。

よって、基準座標系から撮像装置の座標系（以下、撮像装置座標系）への座標変換は、

及び撮像装置座標系における基準座標系の原点の位置ｔ_ｃｗを用いて（式３）のように表せる。

但し、ｘ_ｗ、ｘ_ｃはそれぞれ基準座標系と撮像装置座標系における点の座標を表している。

ここで、

を（式４）のように表す。

また、方位角をθとすると、

は（式５）のように表される。

基準座標系における位置がｘ_ｗ＝［ｘ_ｗｙ_ｗｚ_ｗ］^ｔである指標が画像上に投影される位置（ｕ_ｘ，ｕ_ｙ）は、焦点距離をｆ、α＝ｃｏｓθ、β＝ｓｉｎθ、ｘ_ｃ＝［ｘ_ｃｙ_ｃｚ_ｃ］^ｔ、ｔ_ｃｗ＝［ｔ_ｘｔ_ｙｔ_ｚ］^ｔとして（式６）のように表される。

次に、撮像装置が撮影した画像上で検出される指標と、ジャイロセンサが出力する姿勢の傾斜角成分を用いて撮像装置の位置及び姿勢を算出する方法について説明する。図３は、本実施形態の処理手順を示すフローチャートである。

まずステップＳ１０１０において、指標検出部１１０は撮像装置１４０から画像を入力し、姿勢入力部１２０は撮影時のジャイロセンサの計測値を入力する。姿勢入力部１２０はジャイロセンサの計測値Ｒ_ｗｃを傾斜角成分

と方位角成分

に分解し、（式４）からｒ_１１，ｒ_１２，ｒ_１３，ｒ_２１，ｒ_２２，ｒ_２３，ｒ_３１，ｒ_３２，ｒ_３３を得て、位置姿勢算出部１３０へと出力する。次に、ステップＳ１０２０において、指標検出部１１０は撮影画像中から指標を検出して、位置姿勢算出部１３０へと出力する。ここでは、画像中で検出される指標の数をｎとし、検出指標ｉ（ｉ＝１，２，…，ｎ）の画像上での検出位置を

とする。

次に、ステップＳ１０３０において、位置姿勢算出部１３０は、すべての検出した指標が同一水平面上（本実施形態における水平面はｙ_ｗ＝定数で表される）にあるかどうかを判定する。同一水平面上にあるかどうかの判定は例えば次のように行う。検出指標ｉ（ｉ＝１，２，…，ｎ）の基準座標系における位置を

として、（式７）より検出指標のｙ_ｗ座標の分散σ^２を求める。

但し、

は指標のｙ_ｗ座標の平均値であり、（式８）から求められる。

この分散σ^２が一定値以下であればすべての検出された指標が同一水平面上にあると判定する。ステップＳ１０３０において、検出指標が同一水平面上にあると判定された場合には、ステップＳ１０５０に進み、同一水平面上にないと判定された場合にはステップＳ１０４０に進む。

ステップＳ１０４０においては、位置姿勢算出部１３０は検出指標の数が３個以上であるか否かの判定を行う。ステップＳ１０４０において検出指標の数が３個以上であればステップＳ１０６０に進み、３個未満であれば終了する。また、ステップＳ１０５０においては、検出指標の数が２個以上であるか否かの判定を行う。ステップＳ１０５０において検出指標の数が２個以上であればステップＳ１０７０に進み、２個未満であれば終了する。

ステップＳ１０６０では、位置姿勢算出部１３０は手法（１Ａ）を用いて撮像装置の位置姿勢を算出し、これを出力する。ここで手法（１Ａ）について説明する。

（式６）の分母を払い、

とおくことによって、（式９）に示すα、β、ｔ_ｘ、ｔ_ｙ、ｔ_ｚについての線形方程式が得られる。

入力した夫々の（ｎ点の）指標について、

とおいて、各指標について（式９）の線形方程式を立てると、（式１０）に示す連立方程式が得られる。

（式１０）を解くことによって、α、β、ｔ_ｘ、ｔ_ｙ、ｔ_ｚを得る。この連立方程式において、未知数の数は５個であるため、最低３個の検出指標が必要となる。（式１０）を（式１１）に示すように簡略化して表す。
Ａ・ｘ＝ｂ （式１１）
ここでｘ＝［αβｔ_ｘｔ_ｙｔ_ｚ］^ｔである。また、Ａは行数（２×ｎ）、列数５の行列となり、ｂは（２×ｎ）次元ベクトルとなる。最小二乗法によってｘを（式１２）のように求める。
ｘ＝（Ａ^ｔ・Ａ）^−１・Ａ^ｔ・ｂ （式１２）
なお、（式１２）では、Ａの一般化逆行列を（Ａ^ｔ・Ａ）^−１としているが，他の一般化逆行列を用いても本発明の本質が失われることがないのは言うまでもない。さらに、（式１２）より得られたα、βより、（式１３）によって方位角θを算出する。

得られたθと、センサ計測値の傾斜角成分

をもとに、基準座標系に対する撮像装置の姿勢Ｒ_ｗｃを（式１４）によって算出する。

また、基準座標系における撮像装置の位置ｔ_ｗｃを、（式１５）によって算出する。
ｔ_ｗｃ＝−Ｒ_ｗｃ・ｔ_ｃｗ （式１５）
以上述べた方法で、ステップＳ１０６０において撮像装置の位置姿勢を算出し、これを出力する。

ステップＳ１０７０では、位置姿勢算出部１３０は手法（１Ｂ）を用いて撮像装置の位置姿勢を算出し、これを出力する。ここで手法（１Ｂ）について説明する。

指標の基準座標系におけるｙ_ｗ座標をｙ_ａｖｇとする。（式６）においてｙ_ｗ＝０として、

とおいて分母を払い、さらに

とおくと（式１６）が得られる。

入力した夫々の（ｎ点の）指標について同様に

とおいて、各指標について（式１６）の線形方程式を立てると、（式１７）に示す連立方程式が得られる。

（式１７）を（式１２）に示した方法と同じように解くことによって、α’、β’、ｔ’_ｘ、ｔ’_ｙを得る。この連立方程式において、未知数の数は４個であるため、最低２個の検出指標が必要となる。

（式１７）を解くことによって得られたα’、β’より、方位角θを（式１８）を用いて算出する。

得られたθと、センサ計測値の傾斜角成分

をもとに、基準座標系に対する撮像装置の姿勢Ｒ_ｗｃを（式１４）によって算出する。また、ｔ_ｃｗ＝［ｔ_ｘｔ_ｙｔ_ｚ］^ｔを（式１９）から求める。

また、基準座標系における撮像装置の位置ｔ_ｗｃは、（式２０）によって算出する。

以上述べた方法で、ステップＳ１０７０において撮像装置の位置姿勢を算出し、これを出力する。

以上述べたように、本実施形態では、撮像装置にジャイロセンサを装着した場合に、撮像装置の位置姿勢をセンサ計測値の傾斜角成分と画像情報から反復演算を必要としない方法で直接的に算出することができる。

撮像装置にジャイロセンサを装着し、撮像装置によって撮影された画像とジャイロセンサの姿勢計測値のうち傾斜角成分だけを用いて、反復演算を行うことなく（言い換えれば、初期値を用いることなく）直接的に撮像装置の位置及び姿勢を求めることができる。したがって、反復演算を使用した場合に生じていた初期値に起因する位置姿勢算出精度の低下を防ぐことができる。つまり、撮影画像に指標が撮影されている場合は、高精度な位置姿勢を算出することができる。実施形態では、１台のカメラの画像情報を利用する場合の位置姿勢計測方法について述べた。ある種のＡＲアプリケーションでは、ステレオ表示が有効であるため、ステレオカメラをＨＭＤ（Ｈｅａｄ Ｍｏｕｎｔｅｄ Ｄｉｓｐｌａｙ）に搭載している場合がある。このようなＨＭＤに姿勢センサが装着されている場合、ステレオペアのカメラに対して１つの傾斜角センサが装着されていると考えると、１台の場合と同様にそれぞれのカメラの傾斜角が一意に定まる。そのため、本実施形態で説明した位置姿勢計測方法において、ステレオカメラのそれぞれのカメラから得られる指標を利用するようにしてもよい。また、カメラ間の相対位置姿勢がわかっていれば、カメラは２台に限られることはなく、任意の台数のカメラを利用することもできる。

［第２の実施形態］
第１の実施形態では、撮像装置にジャイロセンサを装着して、撮像装置の位置姿勢を求めた。第２の実施形態では、位置姿勢計測装置及びその方法を、基準座標系に固定された撮像装置によって撮影される移動物体の位置及び姿勢の計測に適用した場合について説明する。

図４は、第２の実施形態における位置姿勢計測装置の構成を示している。同図に示したように、第２の実施形態における位置姿勢計測装置２００は、指標検出部２１０、姿勢入力部２２０、位置姿勢算出部２３０によって構成されている。基準座標系に固定された撮像装置２４０は指標検出部２１０に接続されている。また移動物体２６０上に装着されたジャイロセンサ２５０は姿勢入力部２２０に接続されている。第２の実施形態では、撮像装置２４０によって撮影された画像中で検出される移動物体上の指標と、移動物体上に装着されたジャイロセンサの計測値の傾斜角成分を使って、基準座標系に対する移動物体の位置姿勢を計測する。

図５は、第２の実施形態における座標系の関係を示す図である。移動物体上には、移動物体座標系での位置が既知の指標が配置又は設置されているものとする。撮像装置は基準座標系に固定されており、撮像装置の基準座標系における位置を表す３次元ベクトルｔ_ｗｃ、姿勢を表す３×３回転行列Ｒ_ｗｃは既知とする。第２の実施形態では、基準座標系に対する移動物体の姿勢Ｒ_ｗｏ及び位置ｔ_ｗｏを求める。

基準座標系に対する移動物体の姿勢を表す３×３回転行列Ｒ_ｗｏは、（式２１）に示すように傾斜角成分

と方位角成分

に分解される。

移動物体座標系ｘ_ｏから撮像装置座標系ｘ_ｃへの座標変換は、

及び撮像装置座標系における移動物体の位置ｔ_ｃｏを用いて（式２２）のように表せる。

ここで、

を（式２３）のように表す。

また、方位角をθとすると、

は（式２４）のように表される。

とおいた場合に、移動物体座標がｘ_ｏ＝［ｘ_ｏｙ_ｏｚ_ｏ］^ｔの指標が画像上に投影される位置（ｕ_ｘ，ｕ_ｙ）は、焦点距離をｆ、α＝ｃｏｓθ、β＝ｓｉｎθ、ｘ_ｃ＝［ｘ_ｃｙ_ｃｚ_ｃ］^ｔ、ｔ_ｃｏ＝［ｔ_ｏｘｔ_ｏｙｔ_ｏｚ］^ｔとして（式２５）のように表される。

次に、撮像装置が撮影した画像上で検出される指標と、ジャイロセンサが出力する姿勢の傾斜角成分を用いて移動物体の位置及び姿勢を算出する方法について説明する。図６は、第２の実施形態の処理手順を示すフローチャートである。

まずステップＳ２０１０において、指標検出部２１０は撮像装置２４０から画像を入力する。姿勢入力部２２０は、撮影時の移動物体上のジャイロセンサの計測値Ｒ_ｗｏを入力し、傾斜角成分

と方位角成分

に分解し、（式２３）から既知の値であるＲ_１１，Ｒ_１２，Ｒ_１３，Ｒ_２１，Ｒ_２２，Ｒ_２３，Ｒ_３１，Ｒ_３２，Ｒ_３３を得て、位置姿勢算出部２３０へと出力する。次に、ステップＳ２０２０において、指標検出部２１０は撮影画像中から移動物体上の指標を検出して、位置姿勢算出部２３０へ出力する。ここでは、画像中で検出される指標の数をｎとし、検出指標ｉ（ｉ＝１，２，…，ｎ）の画像上での検出位置を

とする。次に、ステップＳ２０３０において、位置姿勢算出部２３０は、すべての検出した指標が同一水平面上にあるかどうかを判定する。同一水平面上にあるかどうかの判定は例えば次のように行う。検出指標ｉ（ｉ＝１，２，…，ｎ）の移動物体座標系での位置を

として、（式２６）より検出指標のｙ_ｗ座標の分散σ^２を求める。

但し、

は指標のｙ_ｗ座標の平均値であり、（式２７）から求められる。

この分散σ^２が一定値以下であればすべての検出された指標が同一水平面上にあると判定する。ステップＳ２０３０において、検出指標が同一水平面上にあると判定された場合には、ステップＳ２０５０に進み、同一水平面上にないと判定された場合にはステップＳ２０４０に進む。ステップＳ２０４０においては、位置姿勢算出部２３０は検出指標の数が３個以上であるか否かの判定を行う。ステップＳ２０４０において検出指標の数が３個以上であればステップＳ２０６０に進み、３個未満であれば終了する。また、ステップＳ２０５０においては、検出指標の数が２個以上であるか否かの判定を行う。ステップＳ２０５０において検出指標の数が２個以上であればステップＳ２０７０に進み、２個未満であれば終了する。

ステップＳ２０６０では、位置姿勢算出部２３０は手法（２Ａ）を用いて移動物体の位置姿勢を算出し、これを出力する。ここで手法（２Ａ）について説明する。（式２５）の分母を払い、

とおいて得られるα、β、ｔ_ｏｘ、ｔ_ｏｙ、ｔ_ｏｚについての線形方程式を検出された各指標について立てることにより、（式２８）に示す連立方程式が得られる。

（式２８）を解くことにより、α、β、ｔ_ｏｘ、ｔ_ｏｙ、ｔ_ｏｚを得る。この連立方程式において、未知数の数は５個であるため、最低３個の検出指標が必要となる。α、βより、（式１３）を用いて方位角θを算出する。得られたθと、センサ計測値の傾斜角成分

をもとに、基準座標系に対する移動物体の姿勢Ｒ_ｗｏを（式２９）によって算出する。

また移動物体の基準座標系における位置ｔ_ｗｏを（式３０）によって算出する。
ｔ_ｗｏ＝Ｒ_ｗｃ・ｔ_ｃｏ＋ｔ_ｗｃ （式３０）
以上述べた方法で、ステップＳ２０６０において移動物体の位置姿勢を算出し、これを出力する。

ステップＳ２０７０では、位置姿勢算出部２３０は手法（２Ｂ）を用いて移動物体の位置姿勢を算出し、これを出力する。ここで手法（２Ｂ）について説明する。

指標の基準座標系におけるｙ_ｗ座標をｙ_ａｖｇとする。（式２５）において

として、

とおいて分母を払い、

とおいて得られるα’、β’、ｔ’_ｏｘ、ｔ’_ｏｙについての線形方程式を検出された各指標について立てることにより、（式３１）に示す連立方程式が得られる。

この連立方程式において、未知数の数は４個であるため、最低２個の検出指標が必要となる。（式３１）を解くことによって得られたα’、β’から、方位角θを（式１８）によって算出する。得られたθと、センサ計測値の傾斜角成分

をもとに、基準座標系に対する移動物体の姿勢Ｒ_ｗｏを（式２９）によって算出する。また、ｔ_ｃｏ＝［ｔ_ｏｘｔ_ｏｙｔ_ｏｚ］^ｔを（式３２）によって算出する。

さらに移動物体の基準座標系における位置ｔ_ｗｏを（式３３）によって算出する。

以上述べた方法で、ステップＳ２０７０において移動物体の位置姿勢を算出し、これを出力する。

以上述べたように、本実施形態によれば、移動物体にジャイロセンサを装着した場合に、移動物体の位置姿勢をセンサ計測値の傾斜角成分と基準座標系に固定された撮像装置の画像情報から反復演算を必要としない方法で直接的に算出することができる。

本実施形態によれば、移動物体にジャイロセンサを装着し、シーンに固定した撮像装置によって撮影された移動物体の画像とジャイロセンサの姿勢計測値のうち傾斜角成分だけを用いて、反復演算を行うことなく（言い換えれば、初期値を用いることなく）直接的に移動物体の位置及び姿勢を求めることができる。したがって、反復演算を使用した場合に生じていた初期値に起因する位置姿勢算出精度の低下を防ぐことができる。つまり、撮影画像に指標が撮影されている場合は、高精度な位置姿勢を算出することができる。

本実施形態では、１台のカメラの画像情報を利用する場合の位置姿勢計測方法について述べた。本実施形態でのカメラは基準座標系での位置姿勢が既知な客観視点カメラである。客観視点カメラの基準座標系における位置をｔ_ｗｃ（＝［ｘ_ｗｃｙ_ｗｃｚ_ｗｃ］^ｔ）、姿勢を表す３×３回転行列をＲ_ｗｃとすると、移動物体座標系ｘ_ｏから撮像装置座標系ｘ_ｃへの座標変換は、移動物体の基準座標系における位置ｔ_ｗｏ（＝［ｘ_ｗｏｙ_ｗｏｚ_ｗｏ］^ｔ）を用いて、（式３４）で表せる。

、（式２３）、（式２４）、（式３４）より点

が画像上に投影される位置（ｕ_ｘ，ｕ_ｙ）は（式３５）のように表される。

式（３５）は、ｔ_ｗｏとα、βを未知パラメータとして、画像上で検出される各指標について成り立つ観測方程式である。この観測方程式を複数のカメラが撮影する画像上の指標について立てることにより、本実施形態で述べたのと同様な方法で、ｔ_ｗｏとα、βを求めることができる。すなわち、本実施形態は１台のカメラに限定されるものではなく、基準座標系における位置姿勢が既知な複数のカメラの画像情報を利用することができる。

［第３の実施形態］
第２の実施形態では、撮像装置を基準座標系に固定し、移動物体にジャイロセンサを装着したが、第３の実施形態では、位置姿勢計測装置及びその方法を、撮像装置、移動物体の両方にジャイロセンサを装着した場合の撮像装置に対する移動物体の位置及び姿勢の計測に用いた場合について説明する。

図７は、第３の実施形態における位置姿勢計測装置の構成を示している。同図に示したように、第３の実施形態における位置姿勢計測装置３００は、指標検出部３１０、姿勢入力部３２０、位置姿勢算出部３３０によって構成されている。撮像装置３４０は指標検出部３１０に接続されている。また、移動物体３６０にはジャイロセンサ３５５が、撮像装置３４０にはジャイロセンサ３５０が装着されており、ジャイロセンサ３５０、３５５は姿勢入力部３２０に接続されている。第３の実施形態では、撮像装置３４０によって撮影された画像中で検出される移動物体の指標と、移動物体及び撮像装置に装着された夫々のジャイロセンサの計測値の傾斜角成分を使って、撮像装置に対する移動物体の位置姿勢を求める。図８は第３の実施形態における座標系の関係を表す図である。第３の実施形態では、撮像装置座標系に対する移動物体の姿勢Ｒ_ｃｏ及び位置ｔ_ｃｏを求める。基準座標系における移動物体の姿勢を表す３×３回転行列をＲ_ｗｏ、撮像装置の姿勢を表す３×３回転行列をＲ_ｗｃとする。Ｒ_ｗｏは（式３６）に示すように、傾斜角成分

と方位角成分

に分解される。

またＲ_ｗｃは（式３７）に示すように、傾斜角成分

と方位角成分

に分解される。

移動物体座標系ｘ_ｏから撮像装置座標系ｘ_ｃへの変換は、

及び撮像装置座標系における移動物体の位置ｔ_ｃｏを用いて（式３８）のように表せる。

は方位角を表しているので、

としてまとめる。

ここで、

、Ｒ_ａをそれぞれ（式３９）、（式４０）のように表す。θは方位角である。

とおいた場合に、移動物体座標がｘ_ｏ＝［ｘ_ｏｙ_ｏｚ_ｏ］^ｔの指標が画像上に投影される位置（ｕ_ｘ，ｕ_ｙ）は、焦点距離をｆ、α＝ｃｏｓθ、β＝ｓｉｎθ、ｘ_ｃ＝［ｘ_ｃｙ_ｃｚ_ｃ］^ｔ、ｔ_ｃｏ＝［ｔ_ｏｘｔ_ｏｙｔ_ｏｚ］^ｔとして（式４１）のように表される。

次に、撮像装置が撮影した画像上で検出される指標と、ジャイロセンサが出力する姿勢の傾斜角成分を用いて、撮像装置に対する移動物体の位置姿勢を算出する方法について説明する。図９は、第３の実施形態の処理手順を示すフローチャートである。

まずステップＳ３０１０において、指標検出部３１０は、撮像装置から画像を入力する。また、姿勢入力部３２０は、撮影と同時に移動物体に装着されたジャイロセンサの計測値Ｒ_ｗｏ及び撮像装置に装着されたジャイロセンサの計測値Ｒ_ｗｃを入力し、それぞれ公知の手法を使って傾斜角成分

と方位角成分

に分解する。また、（式３９）から既知であるｒ_１１，ｒ_１２，ｒ_１３，ｒ_２１，ｒ_２２，ｒ_２３，ｒ_３１，ｒ_３２，ｒ_３３を得て、位置姿勢算出部３３０に出力する。次に、ステップＳ３０２０において、指標検出部３１０は、撮影画像中から移動物体上の指標を検出して位置姿勢算出部３３０に出力する。ここでは、画像中で検出される指標の数をｎとし、検出指標ｉ（ｉ＝１，２，…，ｎ）の画像上での検出位置を

とする。次に、ステップＳ３０３０において、位置姿勢算出部３３０は、すべての検出した指標が同一水平面上にあるかどうかを判定する。同一水平面上にあるかどうかの判定は例えば次のように行う。検出指標ｉ（ｉ＝１，２，…，ｎ）の移動物体座標系での位置を

として、（式４２）より検出指標のｙ_ｗ座標の分散σ^２を求める。

但し、

は指標のｙ_ｗ座標の平均値であり、（式４３）から求められる。

この分散σ^２が一定値以下であればすべての検出された指標が同一水平面上にあると判定する。ステップＳ３０３０において、検出指標が同一水平面上にあると判定された場合には、ステップＳ３０５０に進み、同一水平面上にないと判定された場合にはステップＳ３０４０に進む。

ステップＳ３０４０においては、位置姿勢算出部３３０は、検出指標の数が３個以上であるか否かの判定を行う。ステップＳ３０４０において検出指標の数が３個以上であればステップＳ３０６０に進み、３個未満であれば終了する。また、ステップＳ３０５０においては、検出指標の数が２個以上であるか否かの判定を行う。ステップＳ３０５０において検出指標の数が２個以上であればステップＳ３０７０に進み、２個未満であれば終了する。

ステップＳ３０６０では、位置姿勢算出部３３０は、手法（３Ａ）を用いて撮像装置に対する移動物体の位置姿勢を算出し、これを出力する。ここで手法（３Ａ）について説明する。（式４１）の分母を払い、

とおいて得られるα、β、ｔ_ｏｘ、ｔ_ｏｙ、ｔ_ｏｚについての線形方程式を検出された各指標について立てることにより、（式４４）に示す連立方程式が得られる。

（式４４）を解くことにより、α、β、ｔ_ｏｘ、ｔ_ｏｙ、ｔ_ｏｚを得る。この連立方程式において、未知数の数は５個であるため、最低３個の検出指標が必要となる。α、βより、（式１３）を用いて方位角θを算出する。移動物体の撮像装置に対する姿勢Ｒ_ｃｏは（式４５）によって算出する。

以上述べた方法で、ステップＳ３０６０において撮像装置に対する移動物体の位置姿勢を算出し、これを出力する。

ステップＳ３０７０では、位置姿勢算出部３３０は、手法（３Ｂ）を用いて撮像装置に対する移動物体の位置姿勢を算出し、これを出力する。ここで手法（３Ｂ）について説明する。

指標の基準座標系におけるｙ_ｗ座標をｙ_ａｖｇとする。（式４１）において

として、

とおいて分母を払い、

とおいて得られるα’、β’、ｔ’_ｏｘ、ｔ’_ｏｙについての線形方程式を検出された各指標について立てることにより、（式４６）に示す連立方程式が得られる。

この連立方程式において、未知数の数は４個であるため、最低２個の検出指標が必要となる。（式４６）を解くことによって得られたα’、β’より、方位角θは（式１８）によって算出する。移動物体の撮像装置に対する姿勢Ｒ_ｃｏは（式４５）によって算出する。また、ｔ_ｃｏ＝［ｔ_ｏｘｔ_ｏｙｔ_ｏｚ］^ｔは（式３２）から算出する。

以上述べた方法で、ステップＳ３０７０において撮像装置に対する移動物体の位置姿勢を算出し、これを出力する。

以上述べたように、本実施形態では、移動物体と撮像装置にジャイロセンサを装着した場合に、撮像装置に対する移動物体の位置姿勢（すなわち、撮像装置と移動物体の相対的な位置と姿勢）をセンサ計測値の傾斜角成分と撮像装置の画像情報から反復演算を必要としないで直接的に算出することができる。

本実施形態によれば、撮像装置と移動物体の両方にジャイロセンサを装着し、撮像装置によって撮影された移動物体の画像とジャイロセンサの姿勢計測値のうち傾斜角成分だけを用いて、反復演算を行うことなく（言い換えれば、初期値を用いることなく）直接的に撮像物体と移動物体の相対的な位置及び姿勢を求めることができる。したがって、反復演算を使用した場合に生じていた初期値に起因する位置姿勢算出精度の低下を防ぐことができる。つまり、撮影画像に指標が撮影されている場合は、高精度な位置姿勢を算出することができる。

［変形例１］
以上述べた実施形態では、ジャイロセンサを姿勢センサとして用いていたが、これに限るものではなく、姿勢の傾斜角成分を計測可能なセンサであればどのような傾斜角センサを用いてもよい。また、傾斜角は鉛直軸に対する傾斜角に限定されるものではなく、任意の軸に対する傾斜角であって、傾斜角センサは該軸に対する傾斜角を計測するものであればどのような傾斜角センサを用いてもよい。

［変形例２］
以上述べた実施形態では、画像上での指標の画像座標とその三次元位置及び姿勢センサから得られる姿勢の傾斜角成分をもとに、反復演算をしないで撮像装置の位置姿勢（第１の実施形態）、移動物体の位置姿勢（第２の実施形態）、撮像装置と物体との相対位置姿勢（第３の実施形態）を求めた。このようにして得られる位置姿勢を初期値として、別の手段によって位置姿勢を最適化してもよい。例えば、非特許文献２の手法を用いて、指標の画像座標とその三次元位置に基づき、６自由度の位置姿勢すべてを最適化してもよいし、非特許文献３の手法を用いて、指標の画像座標とその三次元位置および姿勢センサの計測値に基づき位置と姿勢の方位角成分を最適化してもよい。その際に反復演算を行うことがあっても、構わない。上記実施形態１〜３の方法により、例えば反復演算を用いる最適化の初期値にすることにより、初期値が不適切である（例えば前フレームで位置姿勢を求めることができない時）ことによる位置姿勢算出精度の低下を抑制することができる。

［変形例３］
以上述べた実施形態では、行列演算によって撮像装置と物体との相対位置姿勢を求めていたが、上記実施形態の本質は連立方程式を反復演算なしに直接的に解くことにあり、必ずしも行列演算によって求める必要がないのは言うまでもない。

［変形例４］
また、以上述べた実施形態では、（式１０）に表されるように、すべての検出指標の信頼度を同じ信頼度とみなして連立方程式を解いているが、検出指標ごとに信頼度を変えて、重みを付与して連立方程式を解いてもよい。

［変形例５］
さらに、以上述べた実施形態では、指標が同一水平面上にあるかどうかを判定して計算手法を切り替えていたが、指標が同一水平面上にあるかどうかが予め分かっている場合には、その判定ステップを省略して直接（式１０）または（式１７）（もしくは（式２８）または（式３１）、（式４４）または（式４６））を解いてもよい。

（他の実施形態）
前述した実施の形態の機能を実現する様に各種のデバイスを動作させる様に該各種デバイスと接続された装置あるいはシステム内のコンピュータに、前記実施の形態の機能を実現するためのソフトウエアのプログラムコードを供給し、そのシステムあるいは装置のコンピュータ（ＣＰＵあるいはＭＰＵ）を格納されたプログラムに従って前記各種デバイスを動作させることによって実施したものも本発明の範疇に含まれる。

この場合、前記ソフトウエアのプログラムコード自体が前述した実施の形態の機能を実現することになり、そのプログラムコード自体、及びそのプログラムコードをコンピュータに供給するための手段、例えばかかるプログラムコードを格納した記憶媒体は本発明を構成する。

かかるプログラムコードを格納する記憶媒体としては例えばフロッピー（登録商標）ディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、磁気テープ、不揮発性のメモリカード、ＲＯＭ等を用いることが出来る。

またコンピュータが供給されたプログラムコードを実行することにより、前述の実施形態の機能が実現されるだけではなく、そのプログラムコードがコンピュータにおいて稼働しているＯＳ（オペレーティングシステム）、あるいは他のアプリケーションソフト等と共同して前述の実施形態の機能が実現される場合にもかかるプログラムコードは本発明の実施形態に含まれることは言うまでもない。

更に供給されたプログラムコードが、コンピュータの機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに格納された後そのプログラムコードの指示に基づいてその機能拡張ボードや機能格納ユニットに備わるＣＰＵ等が実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も本発明に含まれることは言うまでもない。

第一の実施形態の構成を示す図である。 第一の実施形態における座標系の関係を示す図である。 第一の実施形態の処理手順を示すフローチャートである。 第二の実施形態の構成を示す図である。 第二の実施形態における座標系の関係を示す図である。 第二の実施形態の処理手順を示すフローチャートである。 第三の実施形態の構成を示す図である。 第三の実施形態における座標系の関係を示す図である。 第三の実施形態の処理手順を示すフローチャートである。

Claims (15)

1. 物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値入力工程と、
   前記撮像装置から撮影画像を入力する画像入力工程と、
   前記撮影画像から前記物体上の指標を検出する指標検出工程と、
   前記物体または前記撮像装置の姿勢の方位角成分を未知としてから、前記傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記傾斜角センサが装着された物体または撮像装置の位置と姿勢を直接的に算出する位置姿勢算出工程とを備えることを特徴とする位置姿勢計測方法。
2. 物体に装着された第１傾斜角センサの第１傾斜角計測値を入力する第１傾斜角計測値入力工程と、
   撮像装置に装着された第２傾斜角センサの第２傾斜角計測値を入力する第２傾斜角計測値入力工程と、
   前記撮像装置から撮影画像を入力する画像入力工程と、
   前記撮影画像から前記物体上の指標を検出する指標検出工程と、
   前記物体および前記撮像装置の姿勢の方位角成分を未知としてから、前記第１傾斜角計測値、前記第２傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記物体と前記撮像装置との相対的な位置と姿勢を直接的に算出する位置姿勢算出工程とを備えることを特徴とする位置姿勢計測方法。
3. 前記位置姿勢算出工程では、前記物体および前記撮像装置の姿勢の方位角成分を未知としてから、前記第１傾斜角計測値、前記第２傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、該方位角成分を算出し、反復演算をせずに、前記物体と前記撮像装置との相対的な位置と姿勢を直接的に算出することを特徴とする請求項２に記載の位置姿勢計測方法。
4. 前記位置姿勢算出工程では、複数の位置姿勢算出手法を有し、前記複数の位置姿勢算出方法から前記検出された指標に応じた位置姿勢算出方法を選択することを特徴とする請求項１乃至３のいずれか１項に記載の位置姿勢計測方法。
5. ３個以上の検出された指標に基づき、前記位置と姿勢を算出することを特徴とする請求項１乃至４のいずれか１項に記載の位置姿勢計測方法。
6. 前記指標検出工程によって検出された指標が所定の軸に垂直な平面上にあることを制約条件として用いて、前記位置と姿勢を算出することを特徴とする請求項１乃至５のいずれか１項に記載の位置姿勢計測方法。
7. 前記指標検出工程によって検出された指標が水平な平面上にあることを制約条件として用いて、前記位置と姿勢を算出することを特徴とする請求項１乃至６のいずれか１項に記載の位置姿勢計測方法。
8. ２個以上の検出された指標に基づき、前記位置と姿勢を算出することを特徴とする請求項７に記載の位置姿勢計測方法。
9. 前記検出された各指標に対して線形方程式を立てることにより得られる連立方程式を解くことにより、前記位置と姿勢を算出することを特徴とする請求項１乃至８のいずれか１項に記載の位置姿勢計測方法。
10. 前記算出された位置と姿勢を初期値として、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、前記位置と姿勢を最適化することを特徴とする請求項１乃至９のいずれか１項に記載の位置姿勢計測方法。
11. 前記傾斜角センサは、時間経過に伴う方位ドリフト誤差を有することを特徴とする請求項１乃至１０の何れか１項に記載の位置姿勢計測方法。
12. 前記傾斜角センサは、ジャイロセンサであることを特徴とする請求項１１に記載の位置姿勢計測方法。
13. 請求項１乃至１１のいずれか１項に記載の位置姿勢計測方法をコンピュータにて実現するためのプログラム。
14. 物体または撮像装置に装着された傾斜角センサの傾斜角計測値を入力する傾斜角計測値入力手段と、
    前記撮像装置から撮影画像を入力する画像入力手段と、
    前記撮影画像から前記物体上の指標を検出する指標検出手段と、
    前記物体または前記撮像装置の姿勢の方位角成分を未知としてから、前記傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記傾斜角センサが装着された物体または撮像装置の位置と姿勢を直接的に算出する位置姿勢算出手段とを備えることを特徴とする位置姿勢計測装置。
15. 物体に装着された第１傾斜角センサの第１傾斜角計測値を入力する第１傾斜角計測値入力手段と、
    撮像装置に装着された第２傾斜角センサの第２傾斜角計測値を入力する第２傾斜角計測値入力手段と、
    前記撮像装置から撮影画像を入力する画像入力手段と、
    前記撮影画像から前記物体上の指標を検出する指標検出手段と、
    前記物体および前記撮像装置の姿勢の方位角成分を未知としてから、前記第１傾斜角計測値、前記第２傾斜角計測値、前記検出された指標の画像座標および前記検出された指標の既知の位置情報に基づき、反復演算をせずに、前記物体と前記撮像装置との相対的な位置と姿勢を直接的に算出する位置姿勢算出手段とを備えることを特徴とする位置姿勢計測装置。

Images