JP4612804B2 - 位置姿勢計測方法および情報処理装置 - Google Patents

Description

本発明は、物体の位置及び姿勢を計測するものに関する。

物体の位置及び姿勢を計測する方法として、環境に磁場発生装置を固定して設置し、物体に装着した磁場検出装置によってその磁場を計測することで物体の位置及び姿勢を計測する磁気式センサが広く用いられている。しかし、磁気式センサは周辺の磁場環境の影響を大きく受けるため、鉄骨に囲まれたオフィスビル内等の環境においては、満足な計測精度を得ることができなかった（特許文献１）。

一方、本出願人は、特願２００３−００４９４５号において、物体上に３点以上の指標を設置し、天井等に固定して配置した撮像装置（以下、客観視点カメラと呼ぶ）によってこれを撮影し、得られた画像から指標の画像座標を検出することで、物体の位置及び姿勢を算出することを提案している。
特開平１１−１３６７０６号公報

この方法は環境に対する制約が少ないという利点を有しているが、画像上における指標間の距離（あるいは指標によって構成される凸包の面積）が不十分な場合に、期待するような性能を得ることができなかった。特に姿勢に関しては精度及び安定性の両面で非常に性能が悪く、その改善のためには、画像上における指標がなす凸包が十分に大きくなるように工夫する必要があった。しかし、指標が大写しになるように画角の狭いカメラを用いて物体を撮影すると、移動に対する計測範囲が極度に限定されてしまうという問題が発生する。また、物体上における指標の配置間隔を広く設けることである程度の改善も期待できるが、物体の大きさによってはそれを行うことが困難であった。

物体の位置姿勢を算出する情報処理装置であって、前記物体に配置された指標を、固定された位置が既知である客観視点位置姿勢から、撮像した撮像画像を入力する画像入力手段と、前記物体の姿勢計測値を計測する姿勢計測手段と、前記姿勢計測手段の方位ドリフト誤差補正値、前記指標の前記物体に対する位置、および前記客観視点位置を保持する保持手段と、前記姿勢計測値および前記方位ドリフト誤差補正値に基づいて、前記物体の姿勢予測値を計算する姿勢予測手段と、前記撮像画像に撮像された指標の画像座標を検出する指標検出手段と、前記姿勢予測値、前記検出された指標の画像座標、該指標の前記物体に対する位置、および前記客観視点位置姿勢に基づいて、前記物体の位置姿勢を算出する位置姿勢算出手段と、前記算出された位置姿勢に基づいて、前記保持された方位ドリフト誤差補正値を更新する更新手段と、を有し、前記位置姿勢算出手段が、前記物体の位置姿勢の設定値を入力する設定値入力手段と、前記検出した指標が２点以上あるか否かを判定する指標判定手段と、前記検出した指標が２点以上あると判定された場合に、前記姿勢予測値、前記設定値、前記指標の前記物体に対する位置、および前記客観視点位置姿勢に基づいて、該指標の画像座標の推定値を算出する推定値算出手段と、前記検出された指標の画像座標と当該指標の画像座標の推定値との誤差を算出する誤差算出手段と、前記算出された誤差に基づいて、前記物体の位置姿勢の補正値を算出する補正値算出手段と、前記算出された補正値に基づいて、前記設定値を補正する補正手段と、を有することを特徴とする。

本発明に係る位置姿勢計測装置によれば、画像上における指標のなす凸包が小さい場合であっても、物体の位置及び姿勢の計測を安定かつ高精度に行うことが可能となる。すなわち、同じ配置の指標を用いる場合であれば、従来手法と比べて安定した位置及び姿勢を得ることが可能であるし、指標の配置の制約が少ないので、従来手法よりも多種の物体を計測することができる。また、より広範囲をカバーする広角の客観視点カメラを用いることができるので、移動に対する計測範囲を広く保つことが可能となる。

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

［第１の実施形態］
本実施形態に係る位置姿勢計測装置は、任意の計測対象物体の位置及び姿勢の計測を行う。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。

図１は、本実施形態における位置姿勢計測装置の構成を示している。同図に示したように、本実施形態における位置姿勢計測装置１００は、客観視点カメラ１８０（１８０ａ，１８０ｂ，１８０ｃ，１８０ｄ）、画像入力部１６０、データ記憶部１７０、指標検出部１１０、姿勢センサ１４０、姿勢予測部１５０、及び位置姿勢算出部１２０によって構成されており、計測対象物体１３０に接続されている。

姿勢センサ１４０上の複数の位置には、客観視点カメラ１８０によって撮影するための指標として、物体座標系（計測対象物体１３０上の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）上における位置ｘ_Ｃ ^Ｐｋが既知である指標Ｐ_ｋ（ｋ＝１，，，Ｋ）が設定されている。

これらの指標は、位置及び姿勢を計測しようとする計測対象範囲内の各々の地点に計測対象物体１３０を位置させた時に、客観視点カメラ１８０によって取得される客観視点画像上において観測される指標の総数が常に少なくとも２個以上となるように設置されていることが望ましい。図１の例は、２個の指標Ｐ_１，Ｐ_２が設定されており、そのうちの指標Ｐ_１が客観視点カメラ１８０ｃの視野内に、指標Ｐ_２が客観視点カメラ１８０ｃ及び１８０ｄの視野内に含まれている状況を示している。

なお、指標Ｐ_ｋは、例えば、それぞれが異なる色を有する円形状のマーカによって構成してもよいし、それぞれが異なるテクスチャ特徴を有する自然特徴等の特徴点によって構成してもよい。撮影画像上における投影像の画像座標が検出可能であって、かついずれの指標であるかが識別可能であるような指標であれば、何れの形態であってもよい。また指標は故意に設定されたものであっても良いし、故意に設定されたものではない、自然形状のものを用いても良い。

客観視点カメラ１８０（１８０ａ，１８０ｂ，１８０ｃ，１８０ｄ）は、計測対象範囲内に計測対象物体１３０が位置するときに、そのいずれかが計測対象物体１３０を撮像可能な位置に固定して配置されている。ここで、各客観視点カメラ１８０の世界座標系における位置及び姿勢は、データ記憶部１７０に既知の値として予め保持されているものとする。客観視点カメラ１８０が出力する画像（以下、これを客観視点画像と呼ぶ）は、画像入力部１６０に入力される。

画像入力部１６０は、客観視点画像の夫々をデジタルデータに変換し、データ記憶部１７０に保存する。

姿勢センサ１４０は、計測対象物体１３０に装着されており、姿勢センサ１４０自身の現在の姿勢を計測して姿勢予測部１５０へと出力する。姿勢センサ１４０は、ジャイロセンサをベースとしたセンサユニットであり、例えば、株式会社トキメックのＴＩＳＳ−５−４０や、米国ＩｎｔｅｒＳｅｎｓｅ社のＩｎｅｒｔｉａＣｕｂｅ２等によって構成される。これらのセンサによって計測される姿勢計測値は、真の姿勢とは異なった、誤差を有する姿勢である。ただし、これらの姿勢センサは、地球の重力方向を観測するセンサを構成要素として有しており、傾斜角方向のドリフト誤差の蓄積をキャンセルする機能を有しているので、傾斜角方向（ピッチ角及びロール角）に関してはドリフト誤差を発生しないという性質を有している。言い換えれば、方位角方向（ヨー角方向）に関しては時間の経過に伴って蓄積されるドリフト誤差を有している。

姿勢予測部１５０は、方位ドリフト誤差補正値φをデータ記憶部１７０から入力し、姿勢センサ１４０から入力した姿勢計測値を補正して計測対象物体１３０の姿勢を予測し、データ記憶部１７０へと出力する。

指標検出部１１０は、データ記憶部１７０より客観視点画像を入力し、入力した画像中に撮影されている指標の画像座標を検出する。例えば、指標の各々が異なる色を有するマーカによって構成されている場合には、客観視点画像上から各々のマーカ色に対応する領域を検出し、その重心位置を指標の検出座標とする。また、指標の各々が異なるテクスチャ特徴を有する特徴点によって構成されている場合には、既知の情報として予め保持している各々の指標のテンプレート画像によるテンプレートマッチングを客観視点画像上に施すことにより、指標の位置を検出する。なお、計測対象物体１３０の位置の算出値（位置姿勢算出部１２０の出力）及び姿勢の予測値（姿勢予測部１５０の出力）をデータ記憶部１７０からさらに入力し、これらの値に基づいて画像上における指標の位置を予測し探索範囲を限定することにより、指標検出処理の計算負荷を軽減したり、指標の誤検出や誤同定を低減させたりすることが可能である。

指標検出部１１０は、さらに、検出された指標の画像座標とその指標の識別子をデータ記憶部１７０へと出力する。以下では、客観視点カメラ１８０（ａ，ｂ，ｃ，ｄ）の撮影画像に対して指標検出部１１０によって検出された指標を、カメラの識別子ｘ（ｘ＝ａ，ｂ，ｃ，ｄ）と、検出された指標の夫々に付けられた識別子ｍ（ｍ＝１，，，Ｍ_ｘ）を用いて、Ｐ_ｋｘｍと表記する。ここで、Ｍ_ｘは夫々の客観視点画像に対して検出された指標の数を表している。また、検出された指標Ｐ_ｋｘｍの画像座標を、その画像を撮影した客観視点カメラの識別子に応じて、各々ｕ_ａ ^Ｐｋａｍ，ｕ_ｂ ^Ｐｋｂｍ，ｕ_ｃ ^Ｐｋｃｍ，ｕ_ｄ ^Ｐｋｄｍと表記する。また、Ｍを、各画像上で検出された指標の総和とする。例えば図１の場合には、Ｍ_ａ＝０，Ｍ_ｂ＝０，Ｍ_ｃ＝２，Ｍ_ｄ＝１，Ｍ＝３であり、指標の識別子ｋ_ｃ１＝１，ｋ_ｃ２＝２，ｋ_ｄ１＝２と、これらを撮影したカメラの識別子と、これらに対応する画像座標ｕ_ｃ ^Ｐｋｃ１，ｕ_ｃ ^Ｐｋｃ２，ｕ_ｄ ^Ｐｋｄ１が出力される。

位置姿勢算出部１２０は、計測対象物体１３０の姿勢の予測値と、指標検出部１１０によって検出された各々の指標の画像座標ｕ_ａ ^Ｐｋａｍ，ｕ_ｂ ^Ｐｋｂｍ，ｕ_ｃ ^Ｐｋｃｍ，ｕ_ｄ ^Ｐｋｄｍとそれに対応する物体座標（物体座標系における座標値）ｘ_Ｃ ^Ｐｋａｍ，ｘ_Ｃ ^Ｐｋｂｍ，ｘ_Ｃ ^Ｐｋｃｍ，ｘ_Ｃ ^Ｐｋｄｍの組とをデータ記憶部１７０から入力し、これらの情報に基づいて計測対象物体１３０の位置及び姿勢を算出し、Ｉ／Ｆを介して外部へ出力する。また、算出した計測対象物体１３０の位置をデータ記憶部１７０に出力し、さらに、位置及び姿勢の算出工程で導出される姿勢センサ１４０の方位ドリフト誤差補正値の更新値によって、データ記憶部１７０が保持する方位ドリフト誤差補正値を更新する。

データ記憶部１７０は、方位ドリフト誤差補正値、画像入力部１６０から入力される画像、姿勢予測部１５０から入力される姿勢の予測値、位置姿勢算出部１２０から入力される位置の算出値、指標検出部１１０から入力される指標の画像座標及び識別子、さらに、既知の値である指標の物体座標（物体座標系における座標値）等のデータを保持し、必要に応じてこれらを入出力する。

なお、図１に示した画像入力部１６０、データ記憶部１７０、指標検出部１１０、姿勢予測部１５０、及び位置姿勢算出部１２０の夫々は、独立した装置として扱っても良いし、夫々ソフトウェアとして１つもしくは複数のコンピュータにインストールし、夫々のコンピュータのＣＰＵにより実行することで、その機能を実現するようにしても良い。本実施形態では、各部（画像入力部１６０、データ記憶部１７０、指標検出部１１０、姿勢予測部１５０、及び位置姿勢算出部１２０）は１台のコンピュータ内で実行対象となるソフトウェアとして扱う。

図２は画像入力部１６０、データ記憶部１７０、指標検出部１１０、姿勢予測部１５０、及び位置姿勢算出部１２０の夫々をソフトウェアとして実行するコンピュータの基本構成を示す図である。

１００１はＣＰＵで、ＲＡＭ１００２やＲＯＭ１００３に格納されたプログラムやデータを用いてコンピュータ全体の制御を行うと共に、画像入力部１６０、指標検出部１１０、姿勢予測部１５０、及び位置姿勢算出部１２０の夫々のソフトウェアの実行を制御して、各部の機能を実現する。

１００２はＲＡＭで、外部記憶装置１００７や記憶媒体ドライブ１００８からロードされたプログラムやデータを一時的に記憶するエリアを備えると共に、ＣＰＵ１００１が各種の処理を行うために必要とするワークエリアを備える。データ記憶部１７０の機能は、ＲＡＭ１００２によって実現される。

１００３はＲＯＭで、一般にコンピュータの記憶プログラムや設定データなどが格納されている。１００４、１００５は夫々キーボード、マウスで、操作者は夫々を用いて、各種の指示をＣＰＵ１００１に入力することができる。

１００６は表示部で、ＣＲＴや液晶画面などにより構成されており、例えば、計測対象物体１３０の位置姿勢計測のために表示すべきメッセージ等を表示することができる。

１００７は外部記憶装置で、ハードディスクなどの大容量情報記憶装置として機能する装置であって、ここにＯＳ（オペレーティングシステム）やソフトウェアのプログラム等を保存する。また本実施形態の説明において、既知であると説明する情報はここに保存されており、必要に応じてＲＡＭ１００２にロードされる。

１００８は記憶媒体ドライブで、ＣＤ−ＲＯＭやＤＶＤ−ＲＯＭなどの記憶媒体に記憶されているプログラムやデータをＣＰＵ１００１からの指示に従って読み出して、ＲＡＭ１００２や外部記憶装置１００７に出力する。

１００９はＩ／Ｆで、計測対象物体１３０を接続するためのアナログビデオポートあるいはＩＥＥＥ１３９４等のデジタル入出力ポート、姿勢センサ１４０を接続するためのＲＳ２３２ＣあるいはＵＳＢシリアルポート、また、算出した計測対象物体１３０の位置及び姿勢を外部へ出力するためのイーサネット（登録商標）ポート等によって構成される。夫々が入力したデータはＩ／Ｆ１００９を介してＲＡＭ１００２に取り込まれる。画像入力部１６０の機能の一部は、Ｉ／Ｆ１００９によって実現される。

１０１０は上述の各部を繋ぐバスである。

図３は、姿勢予測部１５０の処理手順を示すフローチャートであり、ＣＰＵ１００１が姿勢予測部１５０のソフトウェアのプログラムを実行することで行われる。なお、以下の処理を行う前段で、同フローチャートに従ったプログラムコードは、ＲＡＭ１００２に既にロードされているものとする。

なお、姿勢を表現する方法には様々なものが存在するが、ここでは、３×３の回転行列Ｒによって表現されているものとする。

ステップＳ３０００において、姿勢予測部１５０は、姿勢センサ１４０から姿勢計測値Ｒ^＃（ ^＃ はセンサによる計測値を表す記号）を入力する。

ステップＳ３０１０において、姿勢予測部１５０は、データ記憶部１７０から方位ドリフト誤差補正値φ^＊を入力する。

ステップＳ３０２０において、姿勢予測部１５０は、姿勢計測値Ｒ^＃（姿勢センサ１４０の姿勢を表している）に、姿勢センサ１４０から計測対象物体１３０への姿勢変換と、方位ドリフト誤差補正値φ^＊によるドリフト誤差補正を反映させ、方位ドリフト誤差補正後の計測対象物体１３０の姿勢を算出し、これを、姿勢の予測値Ｒ^＊とする。

ここで、△Ｒ（φ）は、方位方向にφだけの回転を加える回転行列であり、φの関数として次式によって定義される。

ここで、ｌ＝（ｌ_１，ｌ_２，ｌ_３）は、世界座標系における鉛直上方向（地球の重力の反対方向）を表す既知のベクトルを表している。

また、Ｒ_ＳＣは、物体座標系（計測対象物体１３０の位置及び姿勢を表す座標系）からセンサ座標系（姿勢センサ１４０の位置及び姿勢を表す座標系）へ姿勢を変換する３×３の行列であり、固定値である姿勢センサ１４０と計測対象物体１３０の相対姿勢に基づいて、既知の値として予め設定されている。

ステップＳ３０３０において、姿勢予測部１５０は、姿勢の予測値Ｒ^＊をデータ記憶部１７０へと出力する。

ステップＳ３０４０において、姿勢予測部１５０は、処理を終了するか否かの判定を行い、処理を終了しない場合には、処理をステップＳ３０００へと進める。

図４は、計測対象物体１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートであり、ＣＰＵ１００１が位置姿勢算出部１２０のソフトウェアのプログラムを実行することで行われる。なお、以下の処理を行う前段で、同フローチャートに従ったプログラムコードは、ＲＡＭ１００２に既にロードされているものとする。

位置姿勢算出部１２０は、計測対象物体１３０の位置ｔ＝［ｘｙｚ］^Ｔと、姿勢センサ１４０の方位ドリフト誤差補正値の更新値φの合計４パラメータを、算出すべき未知パラメータとして取り扱う。すなわち、本実施形態では、姿勢についてはその全てを未知とするのではなく、姿勢の予測値Ｒ^＊には方位方向のドリフト誤差のみが含まれており、その方位ドリフト誤差補正値の更新値φのみを求めることで、計測対象物体１３０の姿勢が得られるというモデルを適用する。以下では、求めるべき未知パラメータを４値の状態ベクトルｓ＝［ｘｙｚφ］^Ｔによって記述する。

ステップＳ４０００において、位置姿勢算出部１２０は、計測対象物体１３０の姿勢の予測値Ｒ^＊（姿勢予測部１５０の出力）をデータ記憶部１７０から入力する。

ステップＳ４０１０において、位置姿勢算出部１２０は、状態ベクトルｓの初期値を、ｓ＝［ｘ_τ−１ｙ_τ−１ｚ_τ−１０］^Ｔと設定する。ここで、ｘ_τ−１，ｙ_τ−１，ｚ_τ−１は、１ループ前の処理（時刻τ−１とおく）におけるステップＳ４１１０において算出された計測対象物体１３０の位置を表している。

ステップＳ４０２０において、位置姿勢算出部１２０は、指標検出部１１０によって検出された指標の画像座標とその世界座標の組を、データ記憶部１７０から入力する。例えば図１の場合には、画像座標ｕ_ｃ ^Ｐ１，ｕ_ｃ ^Ｐ２，ｕ_ｄ ^Ｐ２及びそれらに対応する物体座標（物体座標系における座標値）ｘ_Ｃ ^Ｐ１，ｘ_Ｃ ^Ｐ２が入力される。

ステップＳ４０３０において、位置姿勢算出部１２０は、入力された指標の情報が位置及び姿勢の推定に十分な情報を含むか否かを判定し、それに応じて処理の分岐を行う。具体的には、入力した指標の実体の総数が２点以上の場合はステップＳ４０４０に処理を進め、２点未満の場合はステップＳ４１００に処理を進める。例えば図１の場合には、２つの指標（投影像は３点であるが、指標の実体は２つ）が検出されているので，ステップＳ４０４０へと処理が進められる。

ステップＳ４０４０において、位置姿勢算出部１２０は、各々の指標Ｐ_ｋｍに対して、その画像座標の推定値ｕ^Ｐｋｍ＊を算出する。ｕ^Ｐｋｍ＊の算出は、既知な情報として予め保持している指標Ｐ_ｋｍ各々の物体座標（物体座標系における座標値）ｘ_Ｃ ^Ｐｋｍと、現在の状態ベクトルｓの関数：

に基づいて行われる。具体的には、関数Ｆ_Ｂ（）は、世界座標系上における当該指標の座標ｘ_Ｗ ^Ｐｋｍをｘ_Ｃ ^Ｐｋｍとｓから求める次式、

及び、世界座標系ｘ_Ｗ ^Ｐｋｍから客観視点カメラ座標（客観視点カメラ座標系（客観視点カメラ１８０上の１点を原点として定義し、更に互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系）上における当該指標の座標）ｘ_Ｂ ^Ｐｋｍを求める次式、

及び、客観視点カメラ座標ｘ_Ｂ ^Ｐｋｍから画像座標ｕ^Ｐｋｍ＊を求める次式、

によって構成されている。ここで、Ｒ^＊は、ステップＳ４０００で入力した姿勢の予測値を表している。また、△Ｒ（φ）は，方位方向にφだけの回転を加える回転行列を表しており、式２によって定義される。また、ｆ^Ｂ _ｘ及びｆ^Ｂ _ｙは夫々の客観視点カメラ１８０のｘ軸方向及びｙ軸方向における焦点距離、Ｒ_ＷＢは夫々の客観視点カメラ１８０の世界座標系における姿勢を表す３ｘ３行列、ｔ_ＷＢは夫々の客観視点カメラ１８０の世界座標系における位置を表す３次元ベクトルであり、客観視点カメラ１８０の夫々に対して既知の値として予め保持されている。

ステップＳ４０５０において、位置姿勢算出部１２０は、各々の指標Ｐ_ｋｍに対して、画像座標の推定値ｕ^Ｐｋｍ＊と実測値ｕ^Ｐｋｍとの誤差Δｕ^Ｐｋｍを、式７に基づいて算出する。

ステップＳ４０６０において、位置姿勢算出部１２０は、各々の指標Ｐ_ｋｍに対して、状態ベクトルｓに関する画像ヤコビアン（すなわち、式３の関数Ｆｂ（ ）を状態ベクトルｓの各要素で偏微分した解を各要素に持つ２行×４列のヤコビ行列）Ｊ_ｕｓ ^Ｐｋｍ（＝∂ｕ／∂ｓ）を算出する。具体的には、式６の右辺を客観視点カメラ座標ｘ_Ｂ ^Ｐｋｍの各要素で偏微分した解を各要素に持つ２行×３列のヤコビ行列Ｊ_ｕｘＢ ^Ｐｋｍ（＝∂ｕ／∂ｘ_Ｂ）と、式５の右辺を世界座標ｘ_Ｗ ^Ｐｋｍの各要素で偏微分した解を各要素に持つ３行×３列のヤコビ行列Ｊ_ｘＢｘＷ ^Ｐｋｍ（＝∂ｘ_Ｂ／∂ｘ_Ｗ）と、式４の右辺を状態ベクトルｓの各要素で偏微分した解を各要素に持つ３行×４列のヤコビ行列Ｊ_ｘＷｓ ^Ｐｋｍ（＝∂ｘ_Ｗ／∂ｓ）を算出し、次式によってＪ_ｕｓ ^Ｐｋｍを算出する。

ステップＳ４０７０において、位置姿勢算出部１２０は、ステップＳ４０５０及びステップＳ４０６０において算出した誤差Δｕ^Ｐｋｍ及び画像ヤコビアンＪ_ｕｓ ^Ｐｋｍに基づいて、状態ベクトルｓの補正値Δｓを算出する。具体的には、各々の指標Ｐ_ｋｍに対して求めた誤差Δｕ^Ｐｋｍを垂直に並べた２Ｍ次元の誤差ベクトルＵ及び、各々の指標Ｐ_ｋｍに対して求めた画像ヤコビアンＪ_ｕｓ ^Ｐｋｍを垂直に並べた２Ｍ行×４列の行列θを作成し、θの擬似逆行列θ’を用いて、

として算出する。図１の例ではＭ＝３であるので、Ｕは６次元ベクトル、θは６行×４列の行列となる。

ステップＳ４０８０において、位置姿勢算出部１２０は、ステップＳ４０７０において算出した補正値Δｓを用いて、式１０に従って状態ベクトルｓを補正し、得られた値をｓの新たな推定値とする。

ステップＳ４０９０において、位置姿勢算出部１２０は、誤差ベクトルＵが予め定めた閾値より小さいかどうか、あるいは、補正値Δｓが予め定めた閾値より小さいかどうかといった何らかの判定基準を用いて、計算が収束しているか否かの判定を行う。収束してない場合には、補正後の状態ベクトルｓを用いて、再度ステップＳ４０４０以降の処理を行う。

ステップＳ４０９０において計算が収束したと判定されると、ステップＳ４１００において、位置姿勢算出部１２０は、得られた状態ベクトルｓから、計測対象物体１３０の姿勢を算出する。具体的には、前ステップまでで得られた状態ベクトルｓから方位ドリフト誤差補正値の更新値φを得て、次式：

によって、計測対象物体１３０の姿勢Ｒを算出する。

ステップＳ４１１０において、位置姿勢算出部１２０は、得られた計測対象物体１３０の位置及び姿勢の情報をＩ／Ｆ１００９を介して外部へ出力する。また、計測対象物体１３０の位置ｔをデータ記憶部１７０へと出力する。なお、位置及び姿勢の出力形態は例えば姿勢を表す３×３行列Ｒと位置を表す３次元ベクトルｔの組であってもよいし、姿勢成分をオイラー角に変換したものでもよいし、位置及び姿勢から算出したモデリング変換行列でもよいし、他のいずれの位置姿勢記述方法によるものでもよい。

ステップＳ４１２０において、位置姿勢算出部１２０は、以上の算出工程で得られた方位ドリフト誤差補正値の更新値φを用いて、データ記憶部１７０が保持している方位ドリフト誤差補正値φ^＊を次式によって更新する。

ステップＳ４１３０において、位置姿勢算出部１２０は、処理を終了するか否かの判定を行い、終了しない場合には再びステップＳ４０００へと進み、次フレーム以降の入力データに対して同様の処理を実行する。

以上によって、計測対象物体の位置及び姿勢が計測される。

なお、上記の実施形態では複数台の客観視点カメラ１８０を用いていたが、必ずしも複数台の客観視点カメラが必要ではなく、客観視点カメラが１台の場合であっても本実施形と同様の効果が得られることはいうまでもない。

［第２の実施形態］
上記の実施形態では、姿勢センサの方位ドリフト誤差補正値の更新値φを未知数として求めていたが、姿勢センサの精度が良い場合や、使用時間が短時間である場合や、あるいは、方位ドリフト誤差補正値の更新値の入力を手作業で行うことが可能な場合には、位置姿勢算出部１２０で求めるべきパラメータを計測対象物体１３０の位置のみに限定してもよい。本実施形態に係る位置姿勢計測装置は、任意の計測対象物体の位置及び姿勢を計測することを目的とした位置姿勢計測装置であって、第１の実施形態における位置姿勢計測装置の位置姿勢算出部１２０の機能を変更した構成となっている。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。

本実施形態では、第１の実施形態におけるφをすべて０とおく。すなわち、本実施形態における位置姿勢算出部１２０では、求めるべき未知パラメータを３値の状態ベクトルｓ’＝［ｘｙｚ］^Ｔによって記述する。また、本実施形態における位置姿勢算出部１２０では、第１の実施形態における位置姿勢算出部１２０の各処理工程（各ヤコビ行列や式４等）からφに関する項を取り除いたものを用いればよい。例えば式４は次式に変更する。

本実施形態に係る位置姿勢計測装置によれば、求めるべき未知パラメータ数が少なくなるため、得られる解（計測対象物体１３０の位置及び姿勢）の安定性のさらなる向上が期待できる。

なお、手作業で方位ドリフト誤差補正値の更新値を入力するためには、例えば、図１の構成に補正値更新手段を更に追加すればよい。補正値更新手段は、オペレータの入力に従って方位ドリフト誤差補正値の更新値φを得て、式１２にしたがってデータ記憶部１７０に保存された方位ドリフト誤差補正値φ^＊を更新する。補正値更新手段は、例えば、キーボード１００４の特定のキーをインターフェイスとして用いることで実現される。例えば、［＋］キーにより＋０．１度を、［−］キーにより−０．１度を更新値として設定するように構成すればよい。なお、第１の実施形態のような、画像情報に基づいて方位ドリフト誤差補正値の更新値を導出する形態においても、手入力による補正値更新手段を併用することが可能なことはいうまでもない。

［第３の実施形態］
上記の実施形態の各々では、未知の値として求めるパラメータを、位置及び方位ドリフト誤差補正値の更新値φか、位置のみかのいずれかに固定していた。しかし、何れのパラメータを未知とするかは必ずしも固定である必要はなく、夫々のパラメータの特性に応じて未知とするパラメータを適宜変更することにより、より好適な位置姿勢の推定を行うことができる。本実施形態に係る位置姿勢計測装置は、任意の計測対象物体の位置及び姿勢を計測することを目的とした位置姿勢計測装置であり、第１の実施形態における位置姿勢計測装置の位置姿勢算出部１２０の機能を変更した構成となっている。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。

本実施形態における位置姿勢算出部は、第１の実施形態における位置姿勢算出部と第２の実施形態における位置姿勢算出部の夫々の機能を併せ持っており、通常は位置のみを未知のパラメータとする第２の実施形態における位置姿勢算出部の処理を行い、一定時間間隔（例えば１０秒（３００フレーム）に１回）毎に、位置と方位ドリフト誤差補正値の更新値を未知のパラメータとする第１の実施形態における位置姿勢算出部の処理を実行する。なお、方位ドリフト誤差補正値を更新する時間間隔は、姿勢センサ１４０のドリフト特性に応じて設定するのが好ましく、オペレータによる対話的操作によって設定可能であることが好ましい。

本実施形態に係る位置姿勢計測装置によれば、姿勢センサ１４０として、短時間であれば方位ドリフト誤差を無視できる程度の精度が得られる姿勢センサを用いる場合に、方位ドリフト誤差を補正しつつ、得られる解の安定性を向上させることが期待できる。

［第４の実施形態］
上記の実施形態の各々では、ある１時刻の画像情報から方位ドリフト誤差補正値の更新値を求めていた。しかし、方位ドリフト誤差の値はフレーム間での相関性が高いので、複数フレームの情報を利用することで、より高精度に求めることができる。本実施形態に係る位置姿勢計測装置は、任意の計測対象物体の位置及び姿勢を計測することを目的とした位置姿勢計測装置であり、第１の実施形態における位置姿勢計測装置の位置姿勢算出部１２０の機能を変更した構成となっている。以下、本実施形態に係る位置姿勢計測装置及び位置姿勢計測方法について説明する。

本実施形態における位置姿勢算出部５２０は、第１の実施形態における位置姿勢算出部と第２の実施形態における位置姿勢算出部の夫々の機能を併せ持っており、夫々のパラメータ推定処理を共に実行する。図５は、計測対象物体１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートであり、ＣＰＵ１００１が位置姿勢算出部５２０のソフトウェアのプログラムを実行することで行われる。なお、以下の処理を行う前段で、同フローチャートに従ったプログラムコードは、ＲＡＭ１００２に既にロードされているものとする。

ステップＳ５０００において、位置姿勢算出部５２０は、第１の実施形態におけるステップＳ４０００と同様に、計測対象物体１３０の姿勢の予測値Ｒ^＊（姿勢予測部１５０の出力）をデータ記憶部１７０から入力する。

ステップＳ５０１０において、位置姿勢算出部５２０は、第１の実施形態におけるステップＳ４０２０と同様に、指標検出部１１０によって検出された指標の画像座標とその世界座標を、データ記憶部１７０から入力する。

ステップＳ５０２０において、位置姿勢算出部５２０は、計測対象物体１３０の位置ｔ＝［ｘｙｚ］^Ｔと姿勢センサ１４０の方位ドリフト誤差補正値の更新値φを未知のパラメータとして、第１の実施形態におけるステップＳ４０１０及びステップＳ４０３０からステップＳ４０９０と同様な処理によってこれらを推定する。

ステップＳ５０３０において、位置姿勢算出部５２０は、ステップＳ５０２０で算出された方位ドリフト誤差補正値の更新値φを積算し、積算値φ_ＳＵＭを求める。

ステップＳ５０４０において、位置姿勢算出部５２０は、予め定めたフレーム数（例えば３０フレーム）の積算処理を行ったか否かの判定を行い、行っている場合は処理をステップＳ５０５０に進め、行っていない場合は処理をステップＳ５０８０へと進める。

ステップＳ５０５０において、位置姿勢算出部５２０は、ステップＳ５０３０で求めた積算値φ_ＳＵＭを処理フレーム数で除算することで、各フレームで得られた方位ドリフト誤差補正値の更新値の平均値を算出し、これを新たに、方位ドリフト誤差補正値の更新値φとする。このあと、積算値φ_ＳＵＭは０にクリアされる。

ステップＳ５０６０において、位置姿勢算出部５２０は、第１の実施形態におけるステップＳ４１２０と同様に、ステップＳ５０５０で得られた方位ドリフト誤差補正値の更新値φを用いて、データ記憶部１７０が保持している方位ドリフト誤差補正値φ^＊を式１２によって更新する。

ステップＳ５０７０において、位置姿勢算出部５２０は、ステップＳ５０５０で得られた方位ドリフト誤差補正値の更新値φを用いて、式１１によって計測対象物体１３０の姿勢を算出し、得られた姿勢を新たな姿勢の予測値とする。

ステップＳ５０８０において、位置姿勢算出部５２０は、計測対象物体１３０の位置ｔ＝［ｘｙｚ］^Ｔを未知のパラメータとして、第２の実施形態と同様な処理によってこれを推定する。

ステップＳ５０９０において、位置姿勢算出部５２０は、第１の実施形態におけるステップＳ４１１０と同様に、得られた計測対象物体１３０の位置及び姿勢の情報を出力する。

ステップＳ５１００において、位置姿勢算出部５２０は、処理を終了するか否かの判定を行い、終了しない場合には再びステップＳ５０００へと進み、次フレーム（時刻τ＋１）以降の入力データに対して同様の処理を実行する。

以上によって、複数フレームの情報を用いることによる、方位ドリフト誤差の補正値の更新値の精度向上が実現される。なお、本実施形態では各フレームで得られる更新値の平均値を用いていたが、各フレームで得られる更新値の中央値を用いても良いし、他のいずれのローパスフィルタを用いても良い。また、第３の実施形態における位置とドリフト誤差補正値の更新値とを未知パラメータとする位置姿勢算出処理手段として、本実施形態における位置姿勢算出部を用いることも、もちろん可能である。

（変形例１）
上記の実施形態において、誤差ベクトルＵと行列θに基づく状態ベクトルの補正値Δｓの算出に式９で表現される最急降下法を用いているが、補正値Δｓの算出は必ずしも最急降下法によって行わなくてもよい。例えば公知の非線形方程式の反復解法であるＬＭ法（Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法）を用いて求めてもよいし、公知のロバスト推定手法であるＭ推定等の統計的手法を組み合わせてもよいし、他の何れの数値計算手法を適用しても構わない。

また、上記の実施形態では、各入力画像に対して最適な（誤差を最小化する）解を求める非線形最適化手法を用いていたが、画像上での指標の誤差に基づいてその誤差を解消するような解を求める手法はこれに限られるものではなく、客観視点カメラによって撮影される画像情報から、計測対象物体の位置及び姿勢センサの方位ドリフト誤差補正値とを算出することにより、計測対象物体の位置及び姿勢を安定かつ高精度に得るという本発明の本質は、他の手法を用いた場合であっても損なわれるものではない。例えば、画像上での指標の誤差に基づいてその誤差を解消するような解を求める手法として知られている、拡張カルマンフィルタ（Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）や反復拡張カルマンフィルタ（ｉｔｅｒａｔｉｖｅＥｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）（これらについては”Ｊ．Ｐａｒｋ，Ｂ．Ｊｉａｎｇ，ａｎｄ Ｕ．Ｎｅｕｍａｎｎ，Ｖｉｓｉｏｎ−ｂａｓｅｄ ｐｏｓｅ ｃｏｍｐｕｔａｔｉｏｎ：ｒｏｂｕｓｔ ａｎｄ ａｃｃｕｒａｔｅ ａｕｇｍｅｎｔｅｄ ｒｅａｌｉｔｙ ｔｒａｃｋｉｎｇ，Ｐｒｏｃ．Ｓｅｃｏｎｄ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｗｏｒｋｓｈｏｐ ｏｎ Ａｕｇｍｅｎｔｅｄ Ｒｅａｌｉｔｙ （ＩＷＡＲ’９９），ｐｐ．３−１２，１９９９．”に詳細が述べられている）を用いた場合、上記の実施形態におけるｓを状態ベクトルと定義し、式３を観測方程式と定義することで、本発明の効果を有したフィルタを構成することができる。

（変形例２）
上記の実施形態において、指標Ｐとして一つの指標が一つの座標を表すような指標（以下、これを点指標と呼ぶ）を用いていた。しかし、他の指標の種類を適用することも可能である。

例えば、公知の位置姿勢計測装置（例えば、“高橋，石井，牧野，中静，ＶＲインターフェースのための単眼による長方形マーカ位置・姿勢の高精度実時間推定法，３次元画像コンファレンス９６公演論文集，ｐｐ．１６７−１７２，１９９６．”を参照）に用いられているような、特定の幾何形状をもつマーカを指標として用いることも出来る。例えば、四角形のマーカを指標として用いる場合、四角形の各頂点の世界座標を既知の値として保持しておき（あるいは、マーカの位置、向き、サイズからこれらの値を演算し）、画像から各頂点の画像座標を検出することで、上記実施形態における４点の指標と同様の効果を得ることができる。特に、ＩＤ情報を有する一つの四角形マーカ（例えば、“加藤，Ｍ．ＢｉｌｌｉｎｇＨｕｒｓｔ，浅野，橘，マーカー追跡に基づく拡張現実感システムとそのキャリブレーション，日本バーチャルリアリティ学会論文誌，ｖｏｌ．４，ｎｏ．４，ｐｐ．６０７−６１６，１９９９．”を参照）を計測対象物体に設置する構成（あるいは、姿勢センサに予め一つの四角形マーカを設置しておく構成）は、画像からのマーカの検出精度及び同定精度の良さが期待できるため、特に好適な形態といえる。

また、別の公知の位置姿勢計測装置（例えば、“Ｄ．Ｇ．Ｌｏｗｅ：Ｆｉｔｔｉｎｇ ｐａｒａｍｅｔｅｒｉｚｅｄ ｔｈｒｅｅ−ｄｉｍｅｎｓｉｏｎａｌ ｍｏｄｅｌｓ ｔｏ ｉｍａｇｅｓ， ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ ＰＡＭＩ，ｖｏｌ．１３，ｎｏ．５，ｐｐ．４４１−４５０，１９９１．”を参照）に用いられているような、線特徴によって構成される指標（以下、これを線指標と呼ぶ）を用いてもよい。例えば、直線の原点からの距離を評価のための基準として、画像からの検出値ｄと状態ベクトルｓからの推定値ｄ^＊から算出する誤差Δｄによって誤差ベクトルＵを構成し、ｄ^＊の算出式を状態ベクトルｓの各要素で偏微分した解を各要素に持つ１行×４列のヤコビ行列Ｊ_ｄｓ（＝∂ｄ／∂ｓ）によって行列θを構成することで、上記実施形態と同様な枠組みによって位置及び姿勢の計測を行うことが出来る。また、線指標と点指標、その他の指標から得られる誤差及び画像ヤコビアンを積み重ねることによって、それらの特徴を併用することが可能である。

（変形例３）
上記の実施形態では、方位ドリフト誤差を有する姿勢センサを用いていたが、姿勢センサとして方位方向にのみ顕著な誤差を有する他の姿勢センサを用いることも可能である。例えば、加速度センサによって傾斜方向の角度を計測し、地磁気センサによって方位方向の角度を計測するタイプの姿勢センサにおいても、位置と方位誤差補正値の更新値を未知パラメータとして、上記実施形態と同様な処理によって計測対象の位置と姿勢の計測を行うことができる。ただし、この場合、方位ドリフト誤差とは誤差の性質が異なるため、第３の実施形態や、第４の実施形態のような形態に用いるには適していない。また、傾斜方向のみを計測する姿勢センサを用いる場合でも、方位方向の計測値が常に０となる３軸の姿勢センサと仮定すると、同様な処理によって計測対象の位置と姿勢の計測を行うことができる。

［他の実施形態］
本発明の目的は、前述した実施形態の機能を実現するソフトウェアのプログラムコードを記録した記憶媒体（または記録媒体）を、システムあるいは装置に供給し、そのシステムあるいは装置のコンピュータ（またはＣＰＵやＭＰＵ）が記憶媒体に格納されたプログラムコードを読み出し実行することによっても、達成される。この場合、記憶媒体から読み出されたプログラムコード自体が前述した実施形態の機能を実現することになり、そのプログラムコードを記憶した記憶媒体は本発明を構成することになる。また、コンピュータが読み出したプログラムコードを実行することにより、前述した実施形態の機能が実現されるだけでなく、そのプログラムコードの指示に基づき、コンピュータ上で稼働しているオペレーティングシステム（ＯＳ）などが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。

さらに、記憶媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張カードやコンピュータに接続された機能拡張ユニットに備わるメモリに書込まれた後、そのプログラムコードの指示に基づき、その機能拡張カードや機能拡張ユニットに備わるＣＰＵなどが実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれることは言うまでもない。

本発明を上記記憶媒体に適用する場合、その記憶媒体には先に説明したフローチャートに対応するプログラムコードが格納されることになる。

第１の実施形態における位置姿勢計測装置の構成を示す図である。 コンピュータの基本構成を示す図である。 ＣＰＵ１００１が姿勢予測部１５０のソフトウェアのプログラムを実行することで行われる、計測対象物体１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートである。 ＣＰＵ１００１が位置姿勢算出部１２０のソフトウェアのプログラムを実行することで行われる、計測対象物体１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートである。 ＣＰＵ１００１が位置姿勢算出部５２０のソフトウェアのプログラムを実行することで行われる、計測対象物体１３０の位置及び姿勢を示すパラメータを算出する処理のフローチャートである。

Claims (5)

1. 物体の位置姿勢を算出する情報処理装置であって、
   前記物体に配置された指標を、固定された位置が既知である客観視点位置姿勢から、撮像した撮像画像を入力する画像入力手段と、
   前記物体の姿勢計測値を計測する姿勢計測手段と、
   前記姿勢計測手段の方位ドリフト誤差補正値、前記指標の前記物体に対する位置、および前記客観視点位置を保持する保持手段と、
   前記姿勢計測値および前記方位ドリフト誤差補正値に基づいて、前記物体の姿勢予測値を計算する姿勢予測手段と、
   前記撮像画像に撮像された指標の画像座標を検出する指標検出手段と、
   前記姿勢予測値、前記検出された指標の画像座標、該指標の前記物体に対する位置、および前記客観視点位置姿勢に基づいて、前記物体の位置姿勢を算出する位置姿勢算出手段と、
   前記算出された位置姿勢に基づいて、前記保持された方位ドリフト誤差補正値を更新する更新手段と、
   を有し、
   前記位置姿勢算出手段が、
   前記物体の位置姿勢の設定値を入力する設定値入力手段と、
   前記検出した指標が２点以上あるか否かを判定する指標判定手段と、
   前記検出した指標が２点以上あると判定された場合に、前記姿勢予測値、前記設定値、前記指標の前記物体に対する位置、および前記客観視点位置姿勢に基づいて、該指標の画像座標の推定値を算出する推定値算出手段と、
   前記検出された指標の画像座標と当該指標の画像座標の推定値との誤差を算出する誤差算出手段と、
   前記算出された誤差に基づいて、前記物体の位置姿勢の補正値を算出する補正値算出手段と、
   前記算出された補正値に基づいて、前記設定値を補正する補正手段と、
   を有することを特徴とする情報処理装置。
2. 前記位置姿勢算出手段が、前記算出された誤差もしくは前記算出された補正値が、予め保持された閾値よりも小さいか否かを判定する収束判定手段とを有し、
   前記設定値入力手段が、前記前記算出された誤差もしくは前記算出された補正値が、予め保持された閾値よりも小さくないと判定された場合に、前記算出された位置姿勢を前記設定値として再入力することを特徴とする請求項１に記載の情報処理装置。
3. 物体の位置姿勢を算出する情報処理装置が行う位置姿勢計測方法であって、
   前記情報処理装置が有する画像入力手段が、前記物体に配置されて該物体に対する位置が既知である指標を、固定された位置が既知である客観視点位置姿勢から、撮像した撮像画像を入力する画像入力工程と、
   前記情報処理装置が有する姿勢計測手段が、前記物体の姿勢計測値を計測する姿勢計測工程と、
   前記姿勢計測値および予め保持された前記姿勢計測手段の方位ドリフト誤差補正値に基づいて、前記物体の姿勢予測値を計算する姿勢予測工程と、
   前記情報処理装置が有する指標検出手段が、前記撮像画像に撮像された指標の画像座標を検出する指標検出工程と、
   前記情報処理装置が有する位置姿勢算出手段が、前記姿勢予測値、前記検出された指標の画像座標、該指標の前記物体に対する位置、および前記客観視点位置姿勢に基づいて、前記物体の位置姿勢を算出する位置姿勢算出工程と、
   前記情報処理装置が有する更新手段が、前記算出された位置姿勢に基づいて、前記保持された方位ドリフト誤差補正値を更新する更新工程と、
   を有し、
   前記位置姿勢算出工程では、
   前記位置姿勢算出手段が有する設定値入力手段が、前記物体の位置姿勢の設定値を入力する設定値入力工程と、
   前記位置姿勢算出手段が有する指標判定手段が、前記検出した指標が２点以上あるか否かを判定する指標判定工程と、
   前記位置姿勢算出手段が有する推定値算出手段が、前記検出した指標が２点以上あると判定された場合に、前記姿勢予測値、前記設定値、前記指標の前記物体に対する位置、および前記客観視点位置姿勢に基づいて、該指標の画像座標の推定値を算出する推定値算出工程と、
   前記位置姿勢算出手段が有する誤差算出手段が、前記検出された指標の画像座標と当該指標の画像座標の推定値との誤差を算出する誤差算出工程と、
   前記位置姿勢算出手段が有する補正値算出手段が、前記算出された誤差に基づいて、前記物体の位置姿勢の補正値を算出する補正値算出工程と、
   前記位置姿勢算出手段が有する補正手段が、前記算出された補正値に基づいて、前記設定値を補正する補正工程と、
   を有することを特徴とする位置姿勢計測方法。
4. 請求項３に記載の位置姿勢計測方法が有する各工程をコンピュータに実行させるためのプログラム。
5. 請求項４に記載のプログラムを格納するコンピュータ読み取り可能な記憶媒体。

Images