JP2020181059A - 撮像装置及びその制御方法、姿勢角算出装置、プログラム、記憶媒体 - Google Patents

Abstract

【課題】角速度センサ及び加速度センサの出力値から姿勢を推定する場合に、地磁気センサの出力値が得られない状況でも、高精度に姿勢を推定することができる撮像装置を提供する。【解決手段】撮像装置であって、撮像装置の動きの角速度を検出する角速度検出部と、撮像装置の動きの加速度を検出する加速度検出部と、撮像装置で撮像された画像に基づいて動きベクトルを検出する動きベクトル検出部と、角速度検出部の検出値と、加速度検出部の検出値と、動きベクトル検出部の検出値とに基づいて撮像装置の姿勢角を算出する算出部とを備え、算出部は、動きベクトル検出部により得られた検出値を用いて角速度検出部の出力を補正し、補正した値に基づいて撮像装置の姿勢角を算出する。【選択図】 図１

Description

本発明は、姿勢情報を要する撮像装置において、複数のセンサを用いて撮像装置の姿勢を推定する技術に関するものである。

従来より、角速度センサ、加速度センサ、地磁気センサが搭載されたセンサデバイスを用いて、運動中の物体の姿勢を推定する方法が知られている。角速度センサの検出結果から、相対的な姿勢変化が算出できる。したがって、この姿勢変化を積算することにより、姿勢を推定できる。

一方で、角速度センサの出力値には、誤差が含まれているため、角速度センサの出力値を積算し続けると、算出される姿勢は徐々に真値から外れていく。このため、角速度センサの出力値を地磁気センサの出力を用いて補正する方法が提案されている。

例えば、特許文献１には、角速度センサ、加速度センサ、地磁気センサが搭載されたセンサデバイスの出力値に基づき、カルマンフィルタを用いて、現在の姿勢を表すクォータニオンを推定し出力する小型姿勢センサが開示されている。

特開２０１３−２００１６２号公報

しかしながら、特許文献１に開示されている技術では以下のような問題がある。特許文献１では、移動物体の独立な３軸上の角速度を計測する角速度センサ、加速度センサ、地磁気センサの出力値に基づいてクォータニオンの推定値を算出する必要がある。しかし、金属製の物体を近づける、地磁気が得られない場所へ移動する、地磁気センサがキャリブレーションされていないなど、地磁気センサの出力値を正確に得られない状況では、クォータニオンを推定できない。そのため、特許文献１に開示されている技術では、精度良く姿勢を推定できない場合がある。

本発明は上述した課題に鑑みてなされたものであり、その目的は、角速度センサ及び加速度センサの出力値から姿勢を推定する場合に、地磁気センサの出力値が得られない状況でも、高精度に姿勢を推定することができる撮像装置を提供することである。

本発明に係わる撮像装置は、撮像装置であって、前記撮像装置の動きの角速度を検出する角速度検出手段と、前記撮像装置の動きの加速度を検出する加速度検出手段と、前記撮像装置で撮像された画像に基づいて動きベクトルを検出する動きベクトル検出手段と、前記角速度検出手段の検出値と、前記加速度検出手段の検出値と、前記動きベクトル検出手段の検出値とに基づいて前記撮像装置の姿勢角を算出する算出手段と、を備え、前記算出手段は、前記動きベクトル検出手段により得られた検出値を用いて前記角速度検出手段の出力を補正し、補正した値に基づいて前記撮像装置の姿勢角を算出することを特徴とする。

また、本発明に係わる姿勢角算出装置は、撮像装置の動きの角速度情報を取得する角速度取得手段と、前記撮像装置の動きの加速度情報を取得する加速度取得手段と、前記撮像装置で撮像された画像に基づく動きベクトル情報を取得する動きベクトル取得手段と、前記角速度取得手段により取得した角速度情報と、前記加速度取得手段により取得した加速度情報と、前記動きベクトル取得手段により取得した動きベクトル情報とに基づいて前記撮像装置の姿勢角を算出する算出手段と、を備え、前記算出手段は、前記動きベクトル検出手段により取得した動きベクトル情報を用いて前記角速度検出手段により取得した角速度情報を補正し、補正した情報に基づいて前記撮像装置の姿勢角を算出することを特徴とする。

本発明によれば、角速度センサ及び加速度センサの出力値から姿勢を推定する場合に、地磁気センサの出力値が得られない状況でも、高精度に姿勢を推定することが可能となる。

本発明の第１の実施形態に係わる撮像システムの構成を示すブロック図。 第１及び第２の実施形態における座標系を示す図。 第１の実施形態におけるドリフト量算出動作を示すフローチャート。 第１の実施形態における姿勢演算部を示すブロック図。 第１の実施形態における地磁気の信頼性を判定する動作を示すフローチャート。 第１の実施形態における姿勢演算動作を示すフローチャート。 第２の実施形態の撮像システムの構成を示すブロック図。 第２の実施形態における姿勢演算部を示すブロック図。 第２の実施形態におけるドリフト量算出動作を示すフローチャート。

以下、添付図面を参照して実施形態を詳しく説明する。尚、以下の実施形態は特許請求の範囲に係る発明を限定するものではない。実施形態には複数の特徴が記載されているが、これらの複数の特徴の全てが発明に必須のものとは限らず、また、複数の特徴は任意に組み合わせられてもよい。さらに、添付図面においては、同一若しくは同様の構成に同一の参照番号を付し、重複した説明は省略する。なお、以下の実施形態の説明においては、撮像装置に加えられる動きは、図２に示すように、Ｙａｗ角、Ｐｉｔｃｈ角、Ｒｏｌｌ角の３種類とする。

（第１の実施形態）
図１は、本発明の第１の実施形態に係わる撮像システム１００の構成を示すブロック図である。撮像システム１００は、主に静止画像と動画像の撮影を行うためのレンズ交換式あるいはレンズ括り付けのデジタルカメラである。ただし、本発明の適用範囲はデジタルカメラに限定されるものではなく、各種の撮像システムに適用することが可能である。

図１において、撮像システム１００は、交換レンズとカメラ本体部とからなるレンズ交換式のカメラ、あるいはレンズ括り付けカメラで構成され、交換レンズはカメラ本体部に装着して使用される。

撮像レンズ１０１は、変倍を行うズームレンズ１０２、像ブレ補正を行うシフトレンズ等の像ブレ補正レンズ１０３、焦点調節を行うフォーカスレンズ１０４を含む。これらの構成により、撮像レンズ１０１は、ズーミング、フォーカシング、像ブレ補正等の動作を行い、被写体像を撮像素子１０５上に結像させる。なお、ズーミング、フォーカシング、像ブレ補正のうちの複数の動作を同一のレンズを制御して実行する構成でもよい。

撮像素子１０５は、例えばＸＹアドレス方式のＣＭＯＳ（Ｃｏｍｐｌｅｍｅｎｔａｒｙ Ｍｅｔａｌ Ｏｘｉｄｅ Ｓｅｍｉｃｏｎｄｕｃｔｏｒ）イメージセンサ等で構成される。そして、撮像レンズ１０１が形成する光学像を光電変換して電荷を蓄積し、その電荷を読み出すことにより複数の画素からなる画像信号を信号処理部１０６へ供給する。

信号処理部１０６は、撮像素子１０５から出力された画像信号にホワイトバランス調整処理やガンマ補正処理などの信号処理を施し、その結果生成されたフレーム画像を画像メモリ１０７に格納する。

動きベクトル検出部（ベクトル取得部）１０８は、信号処理部１０６からの画像信号及び画像メモリ１０７に格納された画像信号に基づいて、光軸に直交する一平面上で互いに直交した水平方向と、垂直方向のそれぞれ２方向の動きベクトルを検出する。動きベクトル検出部１０８の詳細については後述する。

動きベクトル変換部１０９は、動きベクトル検出部１０８から出力される動きベクトルを、焦点距離を用いて回転角度に変換する。光軸に直交する一平面上で互いに直交した軸をなすように、垂直方向の回転軸Ｙ（Ｙａｗ軸）と水平方向の回転軸Ｘ（Ｐｉｔｃｈ軸）を定め、それぞれの軸回りの回転角度であるＹａｗ角、Ｐｉｔｃｈ角を、ドリフト推定部１１３へ出力する。動きベクトル変換部１０９の詳細については後述する。

角速度センサ（角速度検出部、角速度取得部）１１０は、撮像装置１００に加わる角速度の変化を検出するためのジャイロセンサなどのセンサである。検出した角速度信号は、姿勢演算部２００へ出力される。角速度センサ１１０では、光軸に直交する一平面上で互いに直交した検出軸をなすように、垂直方向の回転軸Ｙ（Ｙａｗ軸）、水平方向の回転軸Ｘ（Ｐｉｔｃｈ軸）、光軸方向の回転軸Ｚ（Ｒｏｌｌ軸）の３軸が定められる。そして、この３軸回りの角速度を検出できるように、３つの角速度センサが配置されている。なお、３つの角速度センサはユニット化されていてもよいし、別々のセンサであってもよい。

加速度センサ（加速度検出部、加速度取得部）１１１は、撮像装置１００に加わる加速度の変化を検出するセンサである。一例として、ここでは地球の重力が上から下へ垂直に働いていることを利用し、重力加速度を検出する。検出した加速度信号は、姿勢演算部２００へ出力される。加速度センサ１１１では、光軸に直交する一平面上で互いに直交した検出軸をなすように、垂直方向の回転軸Ｙ（Ｙａｗ軸）、水平方向の回転軸Ｘ（Ｐｉｔｃｈ軸）、光軸方向の回転軸Ｚ（Ｒｏｌｌ軸）の３軸が定められる。そして、この３軸方向の加速度を検出できるように、３つの加速度センサが配置されている。なお、３つの加速度センサはユニット化されていてもよいし、別々のセンサであってもよい。

地磁気センサ（地磁気検出部）１１２は、撮像装置１００に加わる磁気の変化を検出するセンサである。一例として、ここでは地球の磁力線が南から北へ向いていることを利用し、地球が持つ磁気を検出する。また、地磁気センサ１１２の各軸が南から北へ向いているとき最大の磁束密度を検出し、最大磁束密度となる方角は、真北ではなく磁北である。検出した地磁気信号は、姿勢演算部２００へ出力される。地磁気センサ１１２では、光軸に直交する一平面上で互いに直交した検出軸をなすように、垂直方向の回転軸Ｙ（Ｙａｗ軸）、水平方向の回転軸Ｘ（Ｐｉｔｃｈ軸）、光軸方向の回転軸Ｚ（Ｒｏｌｌ軸）の３軸が定められる。そして、この３軸方向の地磁気を検出できるように３つの地磁気センサが配置されている。なお、３つの地磁気センサはユニット化されていてもよいし、別々のセンサであってもよい。

ドリフト推定部１１３は、動きベクトル変換部１０９の出力と、後述する姿勢演算部２００の出力とから、姿勢演算部２００で姿勢の演算を行う際に生じる誤差（算出誤差）であるドリフト量を推定する。ドリフト推定部１１３の詳細については後述する。

表示制御部１１４は、画像メモリ１０７から供給された映像信号を出力して表示デバイス１１５に画像を表示させる。また、後述する地磁気判定部２０４（図４参照）の出力Ｍ＿ＣａｌｉｂｒａｔｉｏｎがＦＡＬＳＥの場合は、地磁気が信頼できない旨を表示デバイス１１５に表示させる。表示制御部１１４は表示デバイス１１５を駆動し、表示デバイス１１５は液晶表示素子（ＬＣＤ）等により画像を表示する。

記録制御部１１６は、記録開始や終了の指示に用いる操作部（不図示）によって映像信号の記録が指示された場合、画像メモリ１０７から供給された映像信号と、後述する姿勢演算部２００の出力値とを同期させて記録媒体１１７へ出力し、記録させる。記録媒体１１７は、半導体メモリ等の情報記録媒体やハードディスク等の磁気記録媒体である。

図２は、姿勢を表す絶対座標系の３軸と各軸の絶対回転角を示す図である。絶対座標系とは、一度設定すると原点の位置や座標軸の方向が変わらない固定座標系であり、本実施形態では、撮像システム１００が地球に対し水平に設置されている場合の座標系とする。慣性センサのように移動や回転が可能な座標系はセンサ座標系である。地球の重力方向を指す軸をＹ軸とし、Ｙ軸回りの水平回転角をＹａｗ角とする。また、撮像システム１００が地球に対し水平に設置されている場合の光軸をＺ軸とし、Ｚ軸回りの回転角をＲｏｌｌ角とする。残りの１軸をＸ軸とし、Ｘ軸回りの回転角をＰｉｔｃｈ角とする。絶対座標系におけるＹａｗ角、Ｐｉｔｃｈ角、Ｒｏｌｌ角はそれぞれオイラー角である。

図１の動きベクトル検出部１０８について説明する。動きベクトル検出部１０８は、光軸に直交する一平面上で互いに直交した水平方向と、垂直方向のそれぞれ２方向の動きベクトルを検出する。具体的には、まず、動きベクトル検出法として相関法やブロックマッチング法等がある。ここでは、その一例として、ブロックマッチング法を動きベクトル検出部１０８に採用するものとする。

このブロックマッチング法とは、まず入力画像信号を複数の適当な大きさのブロック（例えば、１６画素×１６画素のブロック）に分割し、ブロック単位に前のフィールド又はフレームの一定範囲の画素との差を計算する。そして、この差の絶対値の和が最小となる前のフィールド又はフレームのブロックを検索し、当該ブロックの相対的なずれをそのブロックの動きベクトルとして検出する。結果として、画素単位での垂直方向及び水平方向各々の移動量（即ち動きベクトル）が求められる。この動きベクトルは、撮像タイミングが異なる２フィールドまたは２フレームの撮像画像の移動量、すなわち比較する２フィールドまたは２フレームの撮像画像を得る間の期間における撮像装置の移動量を示すものである。そして、この動きベクトルを、比較する２フィールドまたは２フレームの撮像画像の撮像タイミング（例えばフレームレート）を用いて演算すると単位時間当たりの撮像装置の移動量を示す情報を得ることができる。また動きベクトルがうまく検出できない場合は、動きベクトルエラー判定フラグＶ＿ｆｌａｇをＦＡＬＳＥにし、それ以外ではＴＲＵＥとする。動きベクトルのエラー判定方法の一例として、輝度信号が小さい、検出値がピーク値である等の条件が考えられる。

図１の動きベクトル変換部１０９について説明する。動きベクトル変換部１０９は、動きベクトル検出部１０８から出力される動きベクトルを、焦点距離を用いて回転角度に変換する。なお、動きベクトル変換部１０９の処理は、任意の所定の周期で繰り返し実行される。具体的には、検出された動きベクトルを（Ｙｖ,Ｘｖ)とし、焦点距離をｆとし、動きベクトルから求めたＹａｗ角をψｖ、Ｐｉｔｃｈ角をθｖとすると、（式１）（式２）のように記述できる。

ψｖ＝ｔａｎ^-1（Ｘｖ／ｆ） …（式１）
θｖ＝ｔａｎ^-1（Ｙｖ／ｆ） …（式２）
また、（式１）、（式２）で算出したＹａｗ角ψｖ、Ｐｉｔｃｈ角θｖを、処理される周期ごとに積算し続ける。積算後のＹａｗ角をψｖ＿ｉｎｔ、Ｐｉｔｃｈ角をθｖ＿ｉｎｔとし、ドリフト推定部１１３へ出力する。

図１のドリフト推定部１１３について説明する。なお、Ｙａｗ角とＰｉｔｃｈ角の処理は同じとなるので、いずれか一方の制御に関してのみ説明を行う。ドリフト推定部１１３は、動きベクトル変換部１０９の出力と、後述する姿勢演算部２００の出力とから、姿勢角誤差をドリフト量として推定する。演算誤差が生じる要因として、角速度センサ１１０の検出誤差が挙げられる。

推定方法の一例を図３のフローチャートを参照しながら説明する。なお、図３に示される処理は、角速度センサ１１０の検出周期など、任意の所定の周期で繰り返し実行される。また任意のドリフト推定周期をＥｓｔ＿ｔｉｍｅとする。

まず、ステップＳ１０１において、ドリフト推定部１１３は、動きベクトル変換部１０９が出力する角度Ｓ１（一例としてＹａｗ角ψｖ＿ｉｎｔ）を取得し、ステップＳ１０２へ進む。ステップＳ１０２において、ドリフト推定部１１３は、後述する姿勢演算部２００が出力する角度Ｓ２（一例としてＹａｗ角ψＧ＿ｉｎｔ）を取得し、ステップＳ１０３へ進む。

ステップＳ１０３において、ドリフト推定部１１３は、動きベクトルエラー判定フラグＶ＿ｆｌａｇがＴＲＵＥであるかＦＡＬＳＥであるかを判定する。そしてＦＡＬＳＥと判定された場合は、ステップＳ１０４へ進み、ドリフト推定部１１３は、ベクトルエラーカウントＶ＿ｅｒｒｏｒ＿ｃｏｕｎｔをインクリメントする。ＴＲＵＥと判定された場合は、ステップＳ１０５へ進む。

ステップＳ１０５において、ドリフト推定部１１３は、処理されるタイミングが任意のドリフト推定周期Ｅｓｔ＿ｔｉｍｅであるか判定する。そして任意のドリフト推定周期でない場合は、ステップＳ１１８へ進み、任意のドリフト推定周期であればステップＳ１０６へ進む。

ステップＳ１０６において、ドリフト推定部１１３は、角度Ｓ１の絶対値を所定のリミット値Ｓ＿Ｔｈ１と比較して、リミット値Ｓ＿Ｔｈ１を超えているか否かを判定する。そして、角度Ｓ１の絶対値がリミット値Ｓ＿Ｔｈ１を超えていると判定された場合は、ステップＳ１０８へ進み、ドリフト推定部１１３は、ベクトルエラーカウントＶ＿ｅｒｒｏｒ＿ｃｏｕｎｔをインクリメントする。ステップＳ１０６の判定において、角度Ｓ１の絶対値がリミット値Ｓ＿Ｔｈ１を超えていないと判定された場合はステップＳ１０７へ進む。

ステップＳ１０７において、ドリフト推定部１１３は、角度Ｓ１の絶対値から角度Ｓ２の絶対値を引き、さらにその絶対値（以下、差の絶対値）を所定のリミット値Ｓ＿Ｔｈ２と比較して、リミット値Ｓ＿Ｔｈ２を超えているか否かを判定する。そして、差の絶対値がリミット値Ｓ＿Ｔｈ２を超えていると判定された場合は、ステップＳ１０８へ進み、ドリフト推定部１１３は、ベクトルエラーカウントＶ＿ｅｒｒｏｒ＿ｃｏｕｎｔをインクリメントする。ステップＳ１０７の判定において、差の絶対値がリミット値Ｓ＿Ｔｈ２を超えていないと判定された場合はステップＳ１０９へ進む。

ステップＳ１０９において、ドリフト推定部１１３は、角度Ｓ１から前回のドリフト推定周期で取得した動きベクトル変換部１０９の出力値Ｓ１＿ｏｌｄを減算し、差分Ｓ１＿ｄｉｆｆを算出する。ステップＳ１１０において、ドリフト推定部１１３は、角度Ｓ２から前回のドリフト推定周期で取得した姿勢演算部２００の出力値Ｓ２＿ｏｌｄを減算し、差分Ｓ２＿ｄｉｆｆを算出する。

ステップＳ１１１において、ドリフト推定部１１３は、差分Ｓ２＿ｄｉｆｆから差分Ｓ１＿ｄｉｆｆを減算し、ドリフト推定周期で除算してドリフト推定微分値Ｄｒｉｆｔ＿ｄｉｆｆを算出する。

ステップＳ１１２において、ドリフト推定部１１３は、ベクトルエラーカウントＶ＿ｅｒｒｏｒ＿ｃｏｕｎｔが０であるか否かを判定する。そして０であると判定された場合にはステップＳ１１３へ進み、０以外と判定された場合はステップＳ１１５へ進む。

ステップＳ１１３において、ドリフト推定部１１３は、前回のドリフト推定周期で算出したドリフト推定値Ｄｒｉｆｔ＿ｏｌｄと、ドリフト推定微分値Ｄｒｉｆｔ＿ｄｉｆｆを加算し、ドリフト推定値Ｄｒｉｆｔを算出する。ステップＳ１１４において、ドリフト推定部１１３は、前回のドリフト推定周期で算出したドリフト推定値Ｄｒｉｆｔ＿ｏｌｄを、今回のドリフト推定周期で算出した、ドリフト推定値Ｄｒｉｆｔへ更新する。

ステップＳ１１５において、ドリフト推定部１１３は、角度Ｓ１＿ｏｌｄを、今回のドリフト推定周期で取得した角度Ｓ１に更新する。ステップＳ１１６において、ドリフト推定部１１３は、角度Ｓ２＿ｏｌｄを、今回のドリフト推定周期で取得した角度Ｓ２に更新する。

ステップＳ１１７において、ドリフト推定部１１３は、ベクトルエラーカウントＶ＿ｅｒｒｏｒ＿ｃｏｕｎｔを初期値０に更新する。ステップＳ１１８において、ドリフト推定部１１３は、今回のドリフト推定周期で算出した、ドリフト推定値Ｄｒｉｆｔを出力する。

次に、図４のブロック図を用いて、姿勢演算部２００について説明する。角速度角度変換部２０１は、角速度センサ１１０から出力される角速度データを、回転角度へ変換する。まず、角速度センサ１１０から出力される角速度データ（ωｙ，ωｘ，ωｚ）はセンサ座標系であるため、（式３）を用いて、絶対座標系（オイラー角）の角速度（回転角度の微分値）（ψ^・Ｇ，θ^・Ｇ，φ^・Ｇ）へ変換する。その後、所定の積分時間をtとし、（式３）の結果をそれぞれ積分することによりＹ軸回りの絶対回転角度Ｙａｗ角、Ｘ軸回りの絶対回転角度Ｐｉｔｃｈ角、Ｚ軸回りの絶対回転角度Ｒｏｌｌ角（ψＧ，θＧ，φＧ）を算出する（式４、式５、式６）。算出したそれぞれの絶対角度（ψＧ，θＧ，φＧ）は、後述する姿勢角演算部２０６へ出力される。

加速度角度変換部２０２は、加速度センサ１１１から出力される加速度データを回転角度へ変換する。静止時において、加速度センサは重力加速度のみを検出することから、重力加速度をｇ（静止時の垂直方向加速度）、Ｙ軸回りの絶対回転角度Ｙａｗ角、Ｘ軸回りの絶対回転角度Ｐｉｔｃｈ角、Ｚ軸回りの絶対回転角度Ｒｏｌｌ角をそれぞれ（ψ，θ，φ）とし、加速度センサ１１１から取得したＹ軸方向、Ｘ軸方向、Ｚ軸方向の加速度データをそれぞれ（Ａｙ，Ａｘ，Ａｚ）とすると、（式７）（式８）が成り立つ。

（式８）は、センサ座標系から絶対座標系へ変換する回転行列である。（式７）を展開し、加速度データから求めたＸ軸回り、Ｚ軸回りの回転角をそれぞれ（θＡ，φＡ）とすると、（式９）（式１０）（式１１）の関係が得られる。算出したそれぞれの絶対角度（θＡ，φＡ）は、後述する姿勢角演算部２０６へ出力される。

地磁気校正部２０３は、地磁気センサ１１２から出力される地磁気データから、地磁気センサの校正を行う。地磁気センサの校正はキャリブレーションと呼ばれ、地磁気センサのキャリブレーション方法の一例として、地磁気センサを水平及び垂直に回転させ、磁気歪みとオフセットを除去する方法がある。この方法では、地磁気センサの出力から感度を求めることにより磁気歪みを除去し、原点を求めることによりオフセットを除去できる。また、キャリブレーションを行ってもその後、強い磁場の影響を受けて撮像装置内の着磁状態が変化する。詳しいキャリブレーション方法については、公知の技術であるため説明は省略する。

キャリブレーションが完了したら、地磁気判定結果Ｍ＿ＣａｌｉｂｒａｔｉｏｎをＴＲＵＥ、否であればＦＬＡＳＥとし、また磁気歪み補正後及びオフセット補正後の地磁気データを（Ｍｙ，Ｍｘ，Ｍｚ）として地磁気判定部２０４へ出力する。

地磁気判定部２０４は、地磁気校正部２０３の出力から地磁気データの信頼性判定を行う。信頼性判定方法の一例を図５のフローチャートを参照しながら説明する。なお、図５に示される処理は、地磁気センサ１１２の検出周期など、任意の所定の周期で繰り返し実行される。

まず、ステップＳ２０１において、地磁気判定部２０４は、地磁気校正部２０３が出力する各軸の地磁気データ（Ｍｙ，Ｍｘ，Ｍｚ）を取得し、ステップＳ２０２へ進む。ステップＳ２０２において、地磁気判定部２０４は、地磁気判定結果Ｍ＿Ｃａｌｉｂｒａｔｉｏｎを取得し、ステップＳ２０３へ進む。

ステップＳ２０３において、地磁気判定部２０４は、地磁気判定結果Ｍ＿ＣａｌｉｂｒａｔｉｏｎがＴｒｕｅであるかＦＡＬＳＥであるかを判定し、そしてＦＡＬＳＥと判定された場合は、ステップＳ２０５へ進む。ステップＳ２０３の判定において、ＴＲＵＥと判定された場合は、ステップＳ２０４へ進む。

ステップＳ２０４において、地磁気判定部２０４は、各軸の地磁気データ（Ｍｙ，Ｍｘ，Ｍｚ）を三平方の定理を用いて合成し、合成したデータ（以下、合成データ）を所定のリミット値Ｍ＿Ｔｈ１と比較して、Ｍ＿Ｔｈ１を超えているか否かを判定する。そして、合成データがリミット値Ｍ＿Ｔｈ１を超えている（信頼性が所定値よりも低い）と判定された場合は、ステップＳ２０５へ進む。ステップＳ２０４の判定において、合成データがリミット値Ｍ＿Ｔｈ１を超えていない（信頼性が所定値以上）と判定された場合はステップＳ２０６へ進む。

ステップＳ２０５において、地磁気判定部２０４は、地磁気判定結果Ｍ＿ＣａｌｉｂｒａｔｉｏｎをＦＡＬＳＥとする。また、地磁気センサ１１２の検出値の信頼性が低いことを、表示デバイス１１５などを用いてユーザーに通知する。ステップＳ２０６において、地磁気判定部２０４は、地磁気校正部２０３が出力する各軸の地磁気データ（Ｍｙ，Ｍｘ，Ｍｚ）と、地磁気判定結果Ｍ＿Ｃａｌｉｂｒａｔｉｏｎとを後述する姿勢角演算部２０６へ出力する。

図４の説明に戻って、地磁気角度変換部２０５は、地磁気判定部２０４から出力される地磁気データ（Ｍｙ，Ｍｘ，Ｍｚ）を、後述する姿勢角演算部２０６の姿勢角出力（θ，φ）を用いて、回転角度へ変換する。地磁気センサは、センサが水平である場合に方位角、つまり垂直方向の回転軸Ｙ（Ｙａｗ軸）回りの絶対角度を算出できる。しかしセンサが水平でない場合には磁場を正確に検出できないため誤差が生じる。この水平でないことにより生じる誤差は、絶対角Ｐｉｔｃｈ角θとＲｏｌｌ角φを用いて修正できる。地磁気判定部２０４から取得したＹ軸方向、Ｘ軸方向、Ｚ軸方向の地磁気データをそれぞれ（Ｍｙ，Ｍｘ，Ｍｚ）、誤差を補正したＹ軸方向、Ｘ軸方向、Ｚ軸方向の地磁気データをそれぞれ（Ｍｙ＿ｃｏｍ，Ｍｘ＿ｃｏｍ，Ｍｚ＿ｃｏｍ）、回転角度へ変換したＹａｗ角をψＭとすると、（式１２）（式１３）の関係が得られる。算出した回転角度ψＭは、後述する姿勢角演算部２０６へ出力される。

姿勢角演算部２０６は、角速度角度変換部２０１、加速度角度変換部２０２、地磁気角度変換部２０５から出力されるデータから、カルマンフィルタを用いてセンサフュージョンを行い、姿勢角を算出する。今回、カルマンフィルタは、推定したい値を状態値とし、センサ等から得られる情報を入力値と観測値とし、システム行列を設定する。なお、カルマンフィルタは、状態方程式（式１４）及び観測方程式（式１５）からリアルタイムに最適な状態値を推定し続けることができるフィルタであり、詳しい内容は公知であるため説明を省略する。今回は、ｘを状態値、ｙを観測値、ｕを入力値、システム行列をＡ、Ｂ、Ｃとする。なお、添え字ｔは時点を表す。

ｘ(t+1)＝Ａ(t)ｘ(t)＋Ｂ(t)ｕ(t)＋ｍ(t) …（式１４）
ｙ(t)＝Ｃ(t)ｘ(t)＋ｎ(t) …（式１５）
姿勢角演算方法の一例を図６のフローチャートを参照しながら説明する。なお、図６に示される処理は、各センサの検出周期など、任意の所定の周期で繰り返し実行される。

まず、ステップＳ３０１において、姿勢角演算部２０６は、角速度センサ１１０から出力される角速度データからドリフト推定値Ｄｒｉｆｔを減算した角速度（ωｙ，ωｘ，ωｚ）と、角速度角度変換部２０１から出力されるＹ軸回り、Ｘ軸回り、Ｚ軸回りそれぞれの回転角（ψＧ，θＧ，φＧ）を取得し、ステップＳ３０２へ進む。

ステップＳ３０２において、姿勢角演算部２０６は、加速度角度変換部２０２から出力されるＸ軸回り、Ｚ軸回りそれぞれの回転角（θＡ，φＡ）を取得し、ステップＳ３０３へ進む。ステップＳ３０３において、姿勢角演算部２０６は、地磁気角度変換部２０５から出力されるＹａｗ角ψＭを取得し、ステップＳ３０４へ進む。

ステップＳ３０４において、姿勢角演算部２０６は、撮像システム１００が起動されてから初めて行う処理であるか否かを判定する。そして否であると判定された場合にはステップＳ３０６へ進み、初めて行う処理と判定された場合はステップＳ３０５へ進む。

ステップＳ３０５において、姿勢角演算部２０６は、カルマンフィルタの各初期パラメータをセットする。初期のカルマンフィルタ状態値にステップＳ３０１で取得した回転角（ψＧ，θＧ，φＧ）を設定する。観測値及び推定後の姿勢角（ψＥｓｔ，θＥｓｔ，φＥｓｔ）の初期値としてステップＳ３０２で取得した回転角（θＡ，φＡ）と、ステップＳ３０３で取得したＹａｗ角ψＭを設定し、入力値にステップＳ３０１で取得した角速度（ωｙ，ωｘ，ωｚ）を設定する。システム行列Ａ(t)、Ｂ(t)、Ｃ(t)を、（式１６）（式１７）のように設定する。その他、カルマンゲインＫの初期値やプロセスノイズｍ、観測ノイズｎ、誤差共分散行列Ｐについては、それぞれ構築したシステムに適するゲイン、白色ガウスノイズ、センサノイズ、推定誤差量からそれぞれ設定し、ステップＳ３０７へ進む。

ステップＳ３０６において、姿勢角演算部２０６は、カルマンフィルタのパラメータをセットする。カルマンフィルタ状態値にステップＳ３０１で取得した回転角（ψＧ，θＧ，φＧ）を設定する。観測値にステップＳ３０２で取得した回転角（θＡ，φＡ）と、ステップＳ３０３で取得したＹａｗ角ψＭを設定し、入力値にステップＳ３０１で取得した角速度（ωｙ，ωｘ，ωｚ）を設定する。システム行列Ａ(t)、Ｂ(t)、Ｃ(t)を（式１６）（式１７）のように設定し、（ψＥｓｔ(t)，θＥｓｔ(t)，φＥｓｔ(t)）は前回の周期で推定した姿勢角を用いる。その他、プロセスノイズｍ、観測ノイズｎについては、それぞれ構築したシステムに適する白色ガウスノイズ、センサノイズから設定し、ステップＳ３０７へ進む。

ステップＳ３０７において、姿勢角演算部２０６は、カルマンフィルタを用いて、姿勢角となる状態値（ψＥｓｔ，θＥｓｔ，φＥｓｔ）を推定する。カルマンフィルタは、現時点での推定値が得られており、その情報から離散状態方程式を用いて次のフェーズでの推定値を予測する予測フェーズと、予測フェーズの後に実際にセンサより得られた測定値から、予測していた推定値の補正を掛ける更新フェーズに分けられる。まず、予測フェーズでは、現時点での状態値から次の推定値を推定し（式１８）、予測誤差の共分散を算出する（式１９）。次に更新フェーズへ進み、カルマンゲインを更新し（式２０）、予測フェーズで推定した推定値を測定値で補正する（式２１）。次に状態予測誤差の共分散を算出し（式２２）、ステップＳ３０８へ進む。また、プロセスノイズｍに関する共分散行列をＱ、観測ノイズｎに関する共分散行列をＲとする。

上記の予測フェーズと更新フェーズを繰り返すことにより、常に姿勢角となる状態値を推定できる。

ステップＳ３０８において、姿勢角演算部２０６は、地磁気判定結果Ｍ＿ＣａｌｉｂｒａｔｉｏｎがＴｒｕｅであるかＦＡＬＳＥであるかを判定し、そしてＴＲＵＥと判定された場合は、地磁気センサ１１２の検出値を姿勢角の算出に用いることに決定し、ステップＳ３０９へ進む。ステップＳ３０８の判定において、ＦＡＬＳＥと判定された場合は、ステップＳ３１０へ進む。

ステップＳ３０９において、ステップＳ３０７で推定した姿勢角（ψＥｓｔ，θＥｓｔ，φＥｓｔ）を出力する。ステップＳ３１０において、ステップＳ３０１で取得したψＧとステップＳ３０７で推定した姿勢角（θＥｓｔ，φＥｓｔ）を出力する。

以上説明したように、本実施形態によれば、金属の物体を近づける、地磁気が得られない場所へ移動する、地磁気センサがキャリブレーションされていないなど、地磁気センサの出力値を正確に得られない状況でも、姿勢角誤差をドリフト量として推定することにより正しい姿勢角を算出することが可能となる。算出された姿勢角は、画像に関連付けて記録される。

なお、姿勢角誤差をドリフト量として推定し、除去する方法はこれに限らず、動きベクトルから相補フィルタやカルマンフィルタを用いて角速度を算出する方法で誤差を除去してもよく、角度としてドリフト量を推定し除去する方法に限定されるものではない。

（第２の実施形態）
図７は、本発明の第２の実施形態に係わる撮像システム１５０の構成を示すブロック図である。なお、図１と同様の構成には同じ符号を付し、説明は省略する。図７は、図１の構成に対し、ドリフト推定部１１３が削除され、ドリフト推定部１１３とは異なる制御のドリフト推定部１１８が追加された構成となっている。また、図１の姿勢演算部２００が内部構成の異なる姿勢演算部２５０となっている。以下、図８のブロック図を用いて、本実施形態における姿勢演算部２５０の構成と、その動作について具体的に説明する。なお図８において、図４と同様の構成には同じ符号を付し、説明は省略する。図８の姿勢演算部２５０の構成は、図４の構成に対し、角速度角度変換部２０１が削除され、クォータニオン変換部２０７が追加された構成となっている。

第１の実施形態では、角速度角度変換部２０１において、角速度センサ１１０から出力される角速度データから、回転角度へ変換する際に、オイラー角を用いて変換を行う方法について説明した。しかしオイラー角は、Ｐｉｔｃｈ角が±９０°である垂直となる場合に（式３）に含まれるｓｅｃθ及びｔａｎθの値が∞となり計算不能に陥り、また±９０°へ接近するほど誤差が大きくなる特異点問題が発生する。そのため、角度を持たないクォータニオンを使うことにより、特異点問題の影響を受けることなく撮像装置の姿勢演算を行う。クォータニオンは、４次元で３次元空間の回転を行い、（式２３）で示すように３つの虚数ｉ，ｊ，ｋを持つ複素数である。

ｑ＝ｑ1ｉ＋ｑ2ｊ＋ｑ3ｋ＋ｑ4 …（式２３）
図８において、クォータニオン変換部２０７は、角速度センサ１１０から出力される角速度データと、ドリフト推定部１１８から出力されるデータを、姿勢角演算部２０６から出力される姿勢角データから差し引いたデータを、回転角度へ変換する。まず、姿勢角演算部２０６から出力される姿勢角データからドリフト推定部１１８のデータを差し引いたデータを（ψＥｓｔ_q，θＥｓｔ_q，φＥｓｔ_q）とすると、（式２４）を用いてクォータニオンへ変換できる。その後、角速度センサ１１０から出力されるＹ軸回り、Ｘ軸回り、Ｚ軸回りの角速度データを（ωｙ，ωｘ，ωｚ）とし、（式２５）を用いて角速度からクォータニオンへ変換する。所定の積分時間をｔとし、（式２５）で求めたクォータニオンを積分した値を（式２６）の回転行列を用いてオイラー角（ψＧ，θＧ，φＧ）へ変換し、姿勢角演算部２０６へ出力する。

しかしながら、姿勢角演算部２０６で、クォータニオン変換部２０７、加速度角度変換部２０２、地磁気角度変換部２０５の出力から姿勢角を演算する際に、地磁気角度変換部２０５の出力が正確でない場合がある。したがって、地磁気角度変換部２０５の出力を使わない場合には（式２５）で求めたクォータニオンを積分する際の誤差が蓄積されることになる。

そこで、本実施形態の撮像システム１５０では、後述するドリフト推定部１１８において、動きベクトル変換部１０９の出力と、姿勢角演算部２０６の出力とから、演算した姿勢角のドリフト量を推定する。

ドリフト推定部１１８について説明する。なお、Ｙａｗ角とＰｉｔｃｈ角の処理は同じとなるので、いずれか一方の制御に関してのみ説明を行う。また、ドリフト推定部１１８は、動きベクトル変換部１０９の出力と、姿勢角演算部２０６の出力とから、姿勢角誤差をドリフト量として推定する。

演算誤差が生じる要因として、角速度センサの検出誤差が挙げられる。推定方法の一例を図９のフローチャートを参照しながら説明する。図９において、図３と同様の構成には同じ符号を付し、説明は省略する。なお、図９に示される処理は、角速度センサ１１０の検出周期など、任意の所定の周期で繰り返し実行される。また任意のドリフト推定周期をＥｓｔ＿ｔｉｍｅとする。

ステップＳ１０５において、ドリフト推定部１１８は、処理されるタイミングが任意のドリフト推定周期Ｅｓｔ＿ｔｉｍｅであるか否かを判定する。そして任意のドリフト推定周期でない場合は、ステップＳ１１９へ進み、任意のドリフト推定周期であればステップＳ１０６へ進む。

ステップＳ１０７において、ドリフト推定部１１８は、角度Ｓ１の絶対値から角度Ｓ２の絶対値を引き、さらにその絶対値（以下、差分絶対値）を所定のリミット値Ｓ＿Ｔｈ３と比較して、リミット値Ｓ＿Ｔｈ３を超えているか否かを判定する。そしてリミット値Ｓ＿Ｔｈ３を超えていると判定された場合は、ステップＳ１０８へ進み、ドリフト推定部１１８は、ベクトルエラーカウントＶ＿ｅｒｒｏｒ＿ｃｏｕｎｔをインクリメントする。

ステップＳ１０７の判定において、ドリフト推定部１１８は、差分絶対値がリミット値Ｓ＿Ｔｈ３を超えていないと判定された場合はステップＳ１１２へ進む。ステップＳ１１２において、ドリフト推定部１１８は、ベクトルエラーカウントＶ＿ｅｒｒｏｒ＿ｃｏｕｎｔが０であるか否かを判定する。そして０であると判定された場合にはステップＳ１２０へ進み、０以外と判定された場合はステップＳ１１９へ進む。

ステップＳ１１９において、ドリフト推定部１１８は、ドリフト推定値Ｄｒｉｆｔを０とする。ステップＳ１２０において、ドリフト推定部１１８は、角度Ｓ２から角度Ｓ１を減算し、ドリフト推定値Ｄｒｉｆｔを算出する。

ステップＳ１１７において、ドリフト推定部１１８は、ベクトルエラーカウントＶ＿ｅｒｒｏｒ＿ｃｏｕｎｔを初期値０に更新する。ステップＳ１１８において、今回のドリフト推定周期で算出した、ドリフト推定値Ｄｒｉｆｔを出力する。

以上説明したように、本実施形態によれば、金属の物体を近づける、地磁気が得られない場所へ移動する、地磁気センサがキャリブレーションされていないなど、地磁気センサの出力値を正確に得られない状況でも、姿勢角誤差をドリフト量として推定することにより正しい姿勢角を算出することが可能となる。

なお、本実施形態では、クォータニオンを用いて姿勢角を算出することにより、演算途中のオイラー角がわかりにくくなるものの、Ｐｉｔｃｈ角が±９０°の垂直な場合でも演算が可能となる。

なお、上記の各実施形態では、姿勢角演算手段の一例としてカルマンフィルタを用いる場合について説明したが、その他の演算手法を用いてもよい。例えば、センサの周波数特性に着目し、低周波特性がよいセンサと高周波特性がよいセンサを組み合わせて、センサ単体よりも周波数領域特性を向上させる相補フィルタを用いてもよい。また、次の状態の確率密度推定にモンテカルロ法を用いるパーティクルフィルタを用いてもよい。

（他の実施形態）
また本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現できる。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現できる。

例えば、撮像装置から角速度情報、加速度情報、動きベクトル情報を取得した姿勢角算出装置のＣＰＵなどの算出回路が撮像装置の姿勢角を算出し、算出した姿勢角情報を撮像装置に送信するようにしてもよい。すなわち、撮像装置の姿勢角を算出する算出回路を撮像装置とは異なる姿勢角算出装置が有していてもよい。

発明は上記実施形態に制限されるものではなく、発明の精神及び範囲から離脱することなく、様々な変更及び変形が可能である。従って、発明の範囲を公にするために請求項を添付する。

１００，１５０：撮像システム、１０１：撮像レンズ、１０４：フォーカスレンズ、１０５：撮像素子、１０８：動きベクトル検出部、１０９：動きベクトル変換部、１１０：角速度センサ、１１１：加速度センサ、１１２：地磁気センサ、１１３，１１８ドリフト推定部、２００，２５０：姿勢演算部

Claims (15)

1. 撮像装置であって、
   前記撮像装置の動きの角速度を検出する角速度検出手段と、
   前記撮像装置の動きの加速度を検出する加速度検出手段と、
   前記撮像装置で撮像された画像に基づいて動きベクトルを検出する動きベクトル検出手段と、
   前記角速度検出手段の検出値と、前記加速度検出手段の検出値と、前記動きベクトル検出手段の検出値とに基づいて前記撮像装置の姿勢角を算出する算出手段と、を備え、
   前記算出手段は、前記動きベクトル検出手段により得られた検出値を用いて前記角速度検出手段の出力を補正し、補正した値に基づいて前記撮像装置の姿勢角を算出することを特徴とする撮像装置。
2. 前記算出手段は、前記撮像装置の姿勢角を、加速度が変化しない軸に基づく座標系での姿勢角として算出することを特徴とする請求項１に記載の撮像装置。
3. 前記撮像装置に加わる地磁気を検出する地磁気検出手段をさらに備え、前記算出手段は、前記角速度検出手段の検出値と、前記加速度検出手段の検出値と、前記動きベクトル検出手段の検出値と、前記地磁気検出手段の検出値とに基づいて、前記撮像装置の姿勢角を算出することを特徴とする請求項１または２に記載の撮像装置。
4. 前記地磁気検出手段の検出値の信頼性を判定する判定手段をさらに備え、前記算出手段は、前記判定手段の判定結果に基づいて、前記地磁気検出手段の検出値を前記撮像装置の姿勢角の算出に用いるか否かを決定することを特徴とする請求項３に記載の撮像装置。
5. 前記算出手段は、前記判定手段により前記地磁気検出手段の信頼性が所定値以上と判定された場合に、前記地磁気検出手段の検出値を前記撮像装置の姿勢角の算出に用いることを特徴とする請求項４に記載の撮像装置。
6. 前記算出手段は、前記判定手段により前記地磁気検出手段の信頼性が所定値より低いと判定された場合に、前記地磁気検出手段の検出値を前記撮像装置の姿勢角の算出に用いないことを特徴とする請求項５に記載の撮像装置。
7. 前記判定手段により前記地磁気検出手段の信頼性が所定値より低いと判定された場合に、ユーザーにその旨を通知する通知手段をさらに備えることを特徴とする請求項４乃至６のいずれか１項に記載の撮像装置。
8. 前記算出手段から得られる前記撮像装置の姿勢角と、前記動きベクトル検出手段により得られる検出値とを用いて、前記算出手段の算出誤差を推定する推定手段をさらに備え、前記算出手段は、前記角速度検出手段の検出値と、前記加速度検出手段の検出値と、前記地磁気検出手段の検出値と、前記推定手段による推定値とに基づいて、前記撮像装置の姿勢角を算出することを特徴とする請求項４乃至７のいずれか１項に記載の撮像装置。
9. 前記算出手段により算出された姿勢角に基づく情報を、画像に関連付けて記録する記録手段をさらに備えることを特徴とする請求項１乃至８のいずれか１項に記載の撮像装置。
10. 前記算出手段は、オイラー角を用いて前記撮像装置の姿勢角を算出することを特徴とする請求項２に記載の撮像装置。
11. 前記算出手段は、クォータニオンを用いて前記撮像装置の姿勢角を算出することを特徴とする請求項３に記載の撮像装置。
12. 撮像装置の制御方法であって、
    前記撮像装置の動きの角速度を検出する角速度検出工程と、
    前記撮像装置の動きの加速度を検出する加速度検出工程と、
    前記撮像装置で撮像された画像に基づいて動きベクトルを検出する動きベクトル検出工程と、
    前記角速度検出工程の検出値と、前記加速度検出工程の検出値と、前記動きベクトル検出工程の検出値とに基づいて前記撮像装置の姿勢角を算出する算出工程と、を有し、
    前記算出工程では、前記動きベクトル検出工程において得られた検出値を用いて前記角速度検出工程の出力を補正し、補正した値に基づいて前記撮像装置の姿勢角を算出することを特徴とする撮像装置の制御方法。
13. 請求項１２に記載の制御方法の各工程をコンピュータに実行させるためのプログラム。
14. 請求項１２に記載の制御方法の各工程をコンピュータに実行させるためのプログラムを記憶したコンピュータが読み取り可能な記憶媒体。
15. 撮像装置の動きの角速度情報を取得する角速度取得手段と、
    前記撮像装置の動きの加速度情報を取得する加速度取得手段と、
    前記撮像装置で撮像された画像に基づく動きベクトル情報を取得する動きベクトル取得手段と、
    前記角速度取得手段により取得した角速度情報と、前記加速度取得手段により取得した加速度情報と、前記動きベクトル取得手段により取得した動きベクトル情報とに基づいて前記撮像装置の姿勢角を算出する算出手段と、を備え、
    前記算出手段は、前記動きベクトル検出手段により取得した動きベクトル情報を用いて前記角速度検出手段により取得した角速度情報を補正し、補正した情報に基づいて前記撮像装置の姿勢角を算出することを特徴とする姿勢角算出装置。

Images