JP7009107B2 - 撮像装置およびその制御方法 - Google Patents

Description

本発明は、ビデオカメラ、デジタルスチルカメラやそれらの交換レンズ等の光学機器に関し、特に自己位置姿勢推定機能を有する撮像装置およびその制御方法に関するものである。

手振れ等により発生する被写体の像振れを補正する機能を有する撮像装置がある。像振れ補正を行うためには、撮像装置の本体部に加えられた振動および姿勢変化を慣性センサにより検出する必要がある。また、被写体（動体または静止体）に対して撮影者が動きながら、撮影画角内に被写体を収める撮影では、撮影者の意図した動き（以降、カメラワークという）による位置変化が生じる。撮像装置はその姿勢変化とは別にカメラワークによる位置変化を検出することが必要となる。

移動体の位置および姿勢の情報を取得する方法には、慣性センサによる慣性航法データとＧＰＳ（Global Positioning System）による測位データを用いる方法がある。特許文献１では慣性センサによって推定した位置姿勢推定情報の推定誤差を画像による動き情報で補正する方法が開示されている。また、撮像装置の姿勢および位置を検出するための自己位置推定方法として、ＳＦＭ(Structure from Motion)と慣性センサを用いた位置姿勢推定(Visual and Inertial Sensor Fusion)技術がある。この技術を応用して、実空間内に存在する物体の３次元位置と撮像装置の位置姿勢を推定する方法が知られている。

米国特許第８４９４２２５号明細書

従来の技術では、撮影環境の変動によって慣性センサのバイアス誤差の変動が非常に大きい場合や、急激なカメラワークの変化により慣性センサの出力が大きく変動した場合等に、画像情報による補正が正しく機能しない可能性がある。あるいはバイアス誤差が正しく補正されるまでに長い時間がかかると、補正が終了するまでの間、正しい位置姿勢推定が行われない等の問題があった。
本発明の目的は、振れ情報を取得して、より精度の高い位置姿勢推定を行うことが可能な撮像装置およびその制御方法を提供することである。

本発明の一実施形態に係る装置は、撮像手段により画像信号を取得する撮像装置であって、振れ検出手段によって検出される、前記撮像装置の振れを示す第１の振れ情報を取得する第１の取得手段と、前記撮像手段による画像信号から検出される、被写体の動きを示す第２の振れ情報を取得する第２の取得手段と、前記第１および第２の振れ情報を用いて前記撮像装置の位置または姿勢を推定する推定手段と、を備える。前記推定手段は、前記第１の振れ情報の周波数帯域を制限する帯域制限手段と、前記帯域制限手段の出力により前記撮像装置の位置または姿勢の推定値を算出する算出手段と、前記第２の振れ情報を用いて前記推定値に対する補正値を算出する補正手段を有し、前記推定値を前記補正値によって補正する。

本発明によれば、振れ情報を取得して、より精度の高い位置姿勢推定を行うことが可能な撮像装置およびその制御方法を提供できる。

本発明の実施形態に係る撮像装置の構成例を示す図である。 本発明の実施形態に係る特徴座標マップおよび位置姿勢推定部の構成例を示す図である。 本発明の第１実施形態に係る位置姿勢推定部の内部構成の説明図である。 動画フレームでの撮像装置の位置姿勢と３次元特徴点座標と撮影画像上の特徴点座標との関係を表す図である。 対象物のワールド座標での座標位置とカメラ座標での座標位置との関係図である。 仮想的な撮像面をレンズの前方に設定した透視射影のモデルを表す図である。 位置姿勢補正部の詳細な内部構成の説明図である。 振動センサノイズの大きさと制御パラメータとの関係を表す図である。 第１実施形態に係る位置姿勢推定処理のフローチャートである。 本発明の第２実施形態に係る位置姿勢推定部の内部構成の説明図である。 第２実施形態に係る位置姿勢推定処理のフローチャートである。

本発明の各実施形態について、添付図面を参照して説明する。各実施形態では位置姿勢推定機能を有する撮像装置を例示する。
［第１実施形態］
図１は、本実施形態に係る撮像装置の構成例を示すブロック図である。撮像装置１００は、例えばデジタルスチルカメラであり、動画撮影機能を有する。

撮像装置１００はズームユニット１０１を備える。ズームユニット１０１は結像光学系を構成し、撮影倍率を変更するズームレンズを含む。ズーム駆動部１０２は、制御部１１９からの制御信号に従ってズームユニット１０１を駆動する。像振れ補正レンズ（以下、補正レンズという）１０３は、像振れを補正する可動光学部材である。補正レンズ１０３は、撮像光学系の光軸方向に対して直交する方向に移動可能である。像振れ補正レンズ駆動部１０４は、制御部１１９からの制御信号に従って補正レンズ１０３の駆動を制御する。絞り・シャッタユニット１０５は、絞り機能を有するメカニカルシャッタを備える。絞り・シャッタ駆動部１０６は、制御部１１９からの制御信号に従って絞り・シャッタユニット１０５を駆動する。フォーカスレンズ１０７は焦点調節に使用する可動レンズであり、撮像光学系の光軸に沿って位置の変更が可能である。フォーカス駆動部１０８は、制御部１１９からの制御信号に従ってフォーカスレンズ１０７を駆動する。

撮像部１０９は、撮像光学系により結像される光学像を、ＣＣＤイメージセンサやＣＭＯＳイメージセンサ等の撮像素子によって画素単位の電気信号に変換することで画像信号を取得する。ＣＣＤは、“Ｃｈａｒｇｅ Ｃｏｕｐｌｅｄ Ｄｅｖｉｃｅ”の略号である。ＣＭＯＳは、“Ｃｏｍｐｌｅｍｅｎｔａｒｙ Ｍｅｔａｌ－Ｏｘｉｄｅ Ｓｅｍｉｃｏｎｄｕｃｔｏｒ”の略号である。撮像信号処理部１１０は、撮像部１０９が出力する画像信号に対して、Ａ（Analog）／Ｄ（Digital）変換、相関二重サンプリング、ガンマ補正、ホワイトバランス補正、色補間処理等を行い、映像信号に変換する。

映像信号処理部１１１は、撮像信号処理部１１０から取得した画像信号を用途に応じて加工する。具体的には、映像信号処理部１１１は、表示用の画像信号を生成し、また記録用に符号化処理やデータファイル化の処理を行う。表示部１１２は、映像信号処理部１１１が出力する表示用の画像信号にしたがい、必要に応じて画像表示を行う。電源部１１５は撮像装置の各部へ、用途に応じて電源を供給する。外部入出力端子部１１６は、外部装置との間で通信信号および映像信号の入出力に使用される。操作部１１７は、ユーザが撮像装置に指示を与えるためのボタンやスイッチ等の操作部材を有する。例えば操作部１１７は、レリーズボタンの押し込み量に応じて第１スイッチ（ＳＷ１と記す）および第２スイッチ（ＳＷ２と記す）が順にオンするように構成されたレリーズスイッチを有する。また操作部１１７は、各種のモード設定用スイッチを有する。記憶部１１８は、映像情報等を含む様々なデータを記憶する。

制御部１１９は、例えばＣＰＵ、ＲＯＭ、ＲＡＭを有する。ＣＰＵは、“Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ”の略号である。ＲＯＭは、“Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ”の略号である。ＲＡＭは、“Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ”の略号である。ＣＰＵはＲＯＭに記憶された制御プログラムをＲＡＭに展開して実行することによって、撮像装置１００の各部を制御し、以下に説明する様々な動作を実現する。操作部１１７が備えるレリーズボタンの半押し操作でＳＷ１がオンすると、制御部１１９は、映像信号処理部１１１が表示部１１２に出力する表示用の映像信号に基づいてＡＦ（オートフォーカス）評価値を算出する。制御部１１９は、ＡＦ評価値に基づいてフォーカス駆動部１０８を制御することにより自動焦点検出および焦点調節制御を行う。また、制御部１１９は、映像信号の輝度情報および予め定められたプログラム線図に基づいて、適切な露光量を得るための絞り値およびシャッタ速度を決定するＡＥ（自動露出）処理を行う。さらに、レリーズボタンの全押し操作でＳＷ２がオンすると、制御部１１９は、決定した絞り値およびシャッタ速度で撮影処理を行い、撮像部１０９で得た画像データを記憶部１１８に記憶させるように各処理部を制御する。

操作部１１７は、像振れ補正（防振）モードの選択に使用する操作スイッチを有する。この操作スイッチの操作により像振れ補正モードが選択された場合、制御部１１９は、像振れ補正レンズ駆動部１０４に像振れ補正動作を指示する。像振れ補正レンズ駆動部１０４は制御部１１９の制御指令にしたがって、像振れ補正のオフ指示がなされるまでの間、像振れ補正動作を行う。また、操作部１１７は、静止画撮影モードと動画撮影モードのうちの一方を選択可能な撮影モード選択スイッチを有する。撮影モード選択スイッチのユーザ操作により撮影モードの選択処理が行われ、制御部１１９は、像振れ補正レンズ駆動部１０４の動作条件を変更する。像振れ補正レンズ駆動部１０４は、本実施形態の像振れ補正装置を構成する。また操作部１１７は、再生モードを選択するための再生モード選択スイッチを有する。ユーザが再生モード選択スイッチの操作によって再生モードを選択した場合、制御部１１９は像振れ補正動作を停止させる制御を行う。操作部１１７は、ズーム倍率変更の指示を行う倍率変更スイッチを有する。倍率変更スイッチのユーザ操作によってズーム倍率変更が指示された場合、制御部１１９からの指示を受けたズーム駆動部１０２はズームユニット１０１を駆動して、指示された位置にズームレンズを移動させる。

図２は、本実施形態に係る撮像装置の位置姿勢推定を実現するための構成を示すブロック図である。撮像装置１００は第１振動センサ２０１と第２振動センサ２０３を備える。第１振動センサ２０１は撮像装置１００の角速度を検出する角速度センサであり、第２振動センサ２０３は撮像装置１００の並進加速度を検出する加速度センサである。ＡＤ変換器２０２は第１振動センサ２０１が検出したアナログ信号をデジタルデータへ変換する。ＡＤ変換器２０４は第２振動センサ２０３が検出したアナログ信号をデジタルデータへ変換する。

動きベクトル検出部２１０は、撮像部１０９により取得される画像データを撮像信号処理部１１０が処理した信号から動きベクトルを検出する。特徴点追跡部２０９は動きベクトル検出部２１０から動きベクトルを取得し、撮影画像上の所定の特徴点について、動画撮影時の各フレームでの座標の移動位置を検出して追跡する。

特徴座標マップおよび位置姿勢推定部（以下、単に推定部という）２０５は、ＡＤ変換器２０２，２０４から、振動センサ２０１，２０３による各揺れ情報と、特徴点追跡部２０９からの画像情報による揺れ情報を取得する。推定部２０５は、これらの情報に基づき特徴座標マップを推定する。特徴座標マップは、撮像装置１００の位置姿勢および撮像装置に対する撮影被写体の奥行きを含めた位置関係を示す情報である。推定部２０５は、特徴座標マップ推定部２０６、位置姿勢推定部２０７、位置姿勢補正部２０８、振動センサノイズ判定部２１１により構成される。

次に図３を参照して、位置姿勢推定部２０７の構成および処理について説明する。図３は位置姿勢推定部２０７の構成例を示す図である。位置姿勢推定部２０７は姿勢推定部４０１と位置推定部４２０で構成される。姿勢推定部４０１は撮像装置１００の姿勢を推定し、位置推定部４２０は撮像装置１００の位置を推定する。

姿勢推定部４０１は、加算器４０２、４０５、４０６を備える。以下では減算を負値の加算として加算器が行うものとする。姿勢推定部４０１は、撮像装置１００の姿勢変化に係る演算を行う運動方程式演算部４０３と、ハイパスフィルタ４０４、遅延素子４０８、回転行列変換部４０７を備える。周波数帯域の帯域制限を行うハイパスフィルタ４０４はカットオフ周波数を変更可能なフィルタである。遅延素子４０８は、姿勢推定演算を所定周期で実行する際に演算周期の１サンプルだけ信号を遅延させる役目をもつ。回転行列変換部４０７は姿勢情報をクォータニオン（四元数）表現から回転行列表現に変換する。

また、位置推定部４２０は、加算器４０９、４１１、４１３、４１４、４１５、ワールド座標における加速度演算部４１０、ハイパスフィルタ４１２、４１７、遅延素子４１６、４１８を備える。ハイパスフィルタ４１２、４１７はいずれもカットオフ周波数を変更可能なフィルタである。

まず姿勢推定部４０１による姿勢推定を説明する。撮像装置１００の角速度と、後述する位置姿勢補正部２０８により算出された角速度バイアス推定値を以下のように定義する。

加算器４０２は、数式１によりバイアス補正を行い、姿勢変化に係る運動方程式演算部４０３に出力する。

姿勢推定値を下記数３のように定義する。遅延素子４０８は、遅延された１サンプル前の姿勢推定値を出力する。１サンプル前とは、所定のサンプリング周波数に対応する周期（１サンプリング期間）の長さだけ過去の時点であることを意味する。

運動方程式演算部４０３は１サンプル前の姿勢推定値と数式１のωに基づいて、数式２により姿勢を求める。

各記号の意味は以下の通りである。

ワールド座標は、図５で示すような撮像装置の位置とは無関係に対象物の座標を規定する固定座標である。本実施形態では説明を簡単にするため、第１振動センサ２０１および第２振動センサ２０３による座標系はカメラ座標と一致しているものとする。また、数式２中の

は数式３により計算できる。

運動方程式演算部４０３は演算結果である姿勢角変化量を、ハイパスフィルタ４０４に出力する。ハイパスフィルタ４０４は姿勢角変化量の帯域制限を行い、低周波成分を除去する。加算器４０５は、ハイパスフィルタ４０４の出力と遅延素子４０８の出力（１サンプル前の姿勢推定値）を加算し、次回のサンプルでの姿勢推定値を演算する。このように低周波成分を除去した後に積分が行われることで、角速度バイアス推定値

が正しく推定される前または推定誤差が大きい状態においても、角速度バイアス誤差による姿勢推定値のドリフト変動を抑制できる。
加算器４０５の出力は加算器４０６と遅延素子４０８に送られ、遅延素子４０８は遅延信号を運動方程式演算部４０３と回転行列変換部４０７に出力する。加算器４０５の出力である姿勢推定値は、加算器４０６において後述する姿勢推定補正値により補正された後に最終的な姿勢推定値として出力される。

次に、位置推定部４２０による位置推定を説明する。撮像装置１００の加速度と、後述する位置姿勢補正部２０８により算出された加速度バイアス推定値を以下のように定義する。

加算器４０９は、数式４によりバイアス補正を行い、ワールド座標における加速度演算部４１０に出力する。

速度推定値を、

と表記する。遅延素子４１６は、遅延された１サンプル前の速度推定値を出力する。加速度演算部４１０は、１サンプル前の速度推定値と加算器４０９の出力を取得する。加速度演算部４１０および加算器４１１は、ワールド座標での重力加速度ｇに基づき、数式５により撮像装置１００の速度を求めてハイパスフィルタ４１７へ出力する。

加速度センサである第２振動センサ２０３はカメラ座標系での加速度を検出する。本来推定したいワールド座標系での加速度情報とするためには座標系を変換する必要がある。そこで撮像装置の姿勢推定値である回転行列

によりカメラ座標系での加速度情報をワールド座標系での加速度情報に変換する処理が行われる。

第２振動センサ２０３は撮像装置１００の動きによる加速度の他に重力加速度を検出しているため、加算器４１１は数式５にて、加速度演算部４１０の出力から重力加速度ｇを減算することで重力加速度の影響を除去する。

さらに本実施形態では、加速度バイアス推定値

の推定誤差および、姿勢推定部４０１による姿勢推定誤差の影響への対策が講じられる。これらの誤差により、加速度を積分して算出した速度推定値

にドリフトが生じることが懸念される。そこで、ハイパスフィルタ４１２は加算器４１１の出力に対し、ワールド座標での加速度情報の低周波成分誤差を除去する。ハイパスフィルタ４１２の出力は加算器４１３が取得して、遅延素子４１６による１サンプル前の遅延信号と加算される。遅延素子４１６は加算器４１３の出力を取得し、遅延信号を加速度演算部４１０に出力する。

加算器４１３の出力する速度推定値（数１６）は、さらにハイパスフィルタ４１７によって低周波成分が除去される。加算器４１４は、ハイパスフィルタ４１７の出力と、遅延素子４１８による１サンプル前の位置推定値とを加算する。加算後の出力

は、加算器４１５と遅延素子４１８に送られる。加算器４１５は、加算器４１４の出力に対して後述する位置推定補正値を加算することにより補正を行い、最終的な位置推定値を出力する。

以上のように、位置推定においては姿勢推定演算に比べて、加速度の２階積分処理により位置が推定されるため、加速度および速度推定値にバイアス誤差がある場合、位置推定結果にドリフト誤差が発生する可能性がある。さらに加速度情報から重力加速度の影響を除去する際に姿勢推定部４０１による推定結果を用いるため、姿勢推定誤差によって大きなドリフトが発生する可能性がある。そこで本実施形態では、加速度バイアス推定値

が推定される前や姿勢推定部による姿勢推定値の誤差が大きい期間において、位置推定演算内でハイパスフィルタ処理が行われる。ハイパスフィルタ４１２，４１７により２回に亘って低周波成分を除去することで位置推定値のドリフトを抑制できる。

次に、特徴座標マップ推定部２０６における特徴座標マップ推定方法について説明する。特徴座標マップ推定部２０６は、特徴点の抽出方法としてharrisコーナー点、ＳＩＦＴ特徴子等を用いる。また特徴点追跡部２０９による特徴点の追跡方法に関しては、特徴点を中心として正方形のウィンドウを設け、動画像の新たなフレームが与えられると、フレーム間でウィンドウ内の残差が最も少なくなる点を求める方法が用いられる。このようにして求めた各フレームでの特徴点追跡情報を用いて、撮像装置の位置、姿勢と、撮像装置が撮像する実空間での特徴点の位置がＳＦＭ(structure from motion)により同時に推定される。

図４は、動画フレームでの撮像装置１００の位置姿勢と３次元特徴点座標と撮影画像上の特徴点座標との関係を表す模式図である。推定したい奥行き情報も含めた３次元空間上の特徴点座標マップの特徴座標を（Ｘ，Ｙ，Ｚ）と表記する。第１フレーム（Frame1）でのカメラ位置姿勢についてはカメラ位置をＯ（原点）とし、カメラ姿勢をＩ（単位行列）で表わす。その時の撮影画面上の特徴座標を

と表記する。

また、第２フレーム（Frame2）でのカメラ位置姿勢についてカメラ位置をＴとし、カメラ姿勢をＲで表わし、その時の撮影画面上の特徴座標を

と表記する。

図５から図７を参照して、位置姿勢補正部２０８の構成について説明する。図７の位置姿勢補正部２０８は撮影画像上特徴座標変換部３０１、加算器３０２、位置姿勢フィードバックゲイン部３０３、バイアス推定ゲイン部３０４で構成される。以下、撮影画像上特徴座標変換部を座標変換部と略称し、位置姿勢フィードバックゲイン部をＦＢゲイン部と略称する。ＦＢゲイン部３０３のゲイン値は位置姿勢補正用の補正倍率に相当する。またバイアス推定ゲイン部３０４のゲイン値はバイアス補正用の補正倍率に相当する。各ゲイン値は可変値である。

座標変換部３０１では、特徴座標マップ推定部２０６により推定された奥行き情報も含めた３次元空間上の特徴座標マップ情報と位置姿勢推定部２０７による位置姿勢推定値から、撮影画像上での特徴点座標への変換処理を行う。座標変換部３０１は、まず特徴座標マップ推定部２０６が推定したワールド座標系での３次元特徴座標を、カメラ座標系での３次元特徴座標へ変換する。

図５は、対象物のワールド座標での座標位置とカメラ座標での座標位置との関係図である。Ｔは、ワールド座標の原点ＯＷからカメラ座標の原点ＯＣへ向かうベクトルである。(rx，ry，rz)はワールド座標で見た場合のカメラ座標の各軸（ｘ，ｙ，ｚ）の方向を表す単位ベクトルを示す。カメラ座標系での点（ｘ，ｙ，ｚ）は、ワールド座標系で点（Ｘ，Ｙ，Ｚ）として表されるものとする。これらの座標の関係は、下記式のようになる。

数式６にて、Ｒは回転行列を表わし、Ｔは平行移動ベクトルを表している。ＲおよびＴは位置姿勢推定部２０７により算出される。

次に、数式６により変換されたカメラ座標系の３次元特徴座標から撮影画像上の座標への変換が、例えば透視変換により行われる。図６は、仮想的な撮像面をレンズの前方にて焦点距離ｆの位置に設定した場合の透視射影のモデルを表している。図６の点Ｏはカメラレンズの中心を表わし、Ｚ軸はカメラの光軸を表す。また点Ｏを原点とする座標系がカメラ座標系を示している。（Ｘ，Ｙ，Ｚ）は対象物のカメラ座標系での座標位置を示す。透視変換による対象物のカメラ座標（Ｘ，Ｙ，Ｚ）から射影した画像座標を（ｘ，ｙ）と表記する。（Ｘ，Ｙ，Ｚ）から（ｘ，ｙ）への変換式は下記式で表される。

数式７により、特徴座標マップ推定部２０６の推定した、被写体の奥行きを含めた３次元特徴座標マップを、撮影画像上の２次元特徴座標へ変換することができる。

図７の加算器３０２は特徴点追跡部２０９の出力から、座標変換部３０１の出力を減算する。特徴点追跡部２０９により実際に観測された特徴座標の位置座標と、対象被写体の特徴座標マップから変換された２次元特徴座標との誤差（位置座標誤差）が算出される。位置座標誤差に対してＦＢゲイン部３０３およびバイアス推定ゲイン部３０４はそれぞれゲイン値を乗算して、位置姿勢推定補正値およびバイアス補正値を算出する。

加算器３０２により算出される位置座標誤差は、実際に観測された撮影画面上の特徴点座標と３次元座標マップを撮影画面上に投影した特徴点座標との誤差である。よって、３次元座標マップ値と観測による特徴座標追跡値とが正しい場合、位置座標誤差は３次元座標マップを撮影画面上に投影した際の位置姿勢推定の誤差により生じていることになる。この座標誤差量がゼロとなるように位置姿勢推定補正値およびバイアス推定値（補正値）は、加算器４０６および４１５、４０２、４０９によって位置姿勢推定結果にフィードバックされることで、位置姿勢推定値が真値に近づくことになる。

本実施形態では、ＦＢゲイン部３０３およびバイアス推定ゲイン部３０４の各ゲイン値、および、位置姿勢補正の頻度が、振動センサノイズ判定部（以下、単にノイズ判定部という）２１１の判定結果により変更される。

図８を参照して、ノイズ判定部２１１によるノイズ判定と、位置姿勢推定値の補正との関係を説明する。位置姿勢推定部２０７による位置姿勢推定では、第１振動センサ２０１および第２振動センサ２０３の各出力を用いて演算が行われる。そのため、振動センサのバイアス値、ドリフト値が大きい場合や、振動が大きい状況においては推定値に誤差が生じ得る。推定値に誤差が生じる可能性がある場合、ノイズ判定部２１１は評価値を算出し、評価値に基づいて制御内容を変更する。これにより、位置姿勢推定の精度を向上させることができる。

図８（Ａ）は、ノイズ判定部２１１による判定内容と評価値との関係を示す模式図である。横軸は判定対象の大きさをそれぞれ表し、縦軸は評価値を表す。図８（Ｂ）は評価値と補正制御パラメータとの関係を示す。横軸は評価値を表し、縦軸は補正制御パラメータを表す。

ノイズ判定部２１１は、以下の条件によりノイズ評価値を判定する。
・第１振動センサ２０１、第２振動センサ２０３の出力が大きい場合
例えば、撮像装置１００のパンニングにより大きく画角が変更された場合には、急峻な位置姿勢変動が生じる。急峻な位置姿勢変化に対する位置姿勢推定結果に誤差が生じる可能性がある。そこでノイズ判定部２１１は、振動センサ出力が大きくなるにつれて評価値を高くする。
・バイアス補正後の振動センサ出力にドリフト誤差（変動量）が生じている場合
位置姿勢推定値にドリフト誤差が生じる可能性があるので、ノイズ判定部２１１はドリフト誤差が大きくなるにつれて評価値を高くする。
・再射影誤差が生じている場合
再射影誤差は、位置姿勢補正部２０８の加算器３０２により算出される特徴点の追跡座標と特徴座標マップを撮影画面上の特徴座標に変換した座標値との誤差である。ノイズ判定部２１１は再射影誤差が大きくなるにつれて評価値を高くする。
・駆動部の駆動量または駆動速度が大きい場合
撮像装置はズームユニット１０１、像振れ補正レンズ１０３、絞り・シャッタユニット１０５、フォーカスレンズ１０７等を駆動する機構部を備える。あるいは機構部は撮像装置の本体部に装着可能なレンズ装置が備える。これらの機構部に係る駆動部の駆動量や駆動速度が大きい時には、振動センサが機械的駆動による振動を検出してしまい、位置姿勢推定値に誤差が生じる可能性がある。そこで、ノイズ判定部２１１は駆動量や駆動速度が大きくなるにつれて評価値を高くする。
・バイアス補正後の振動センサ出力にバイアス補正誤差が生じている場合
バイアス補正誤差によりバイアス推定値が真値からずれると、位置姿勢推定値にドリフト誤差が生じる可能性があるので、ノイズ判定部２１１はバイアス補正誤差が大きくなるにつれて評価値を高くする。

上記条件の判定結果にしたがって、図８（Ｂ）に示すように制御内容が変更される。
・評価値が高くなるにつれて、ハイパスフィルタのカットオフ周波数を高くする制御。
評価値が高くなると、評価値が低い時に比べて、ハイパスフィルタ４０４，４１２，４１７のカットオフ周波数が高域側へ変更される。
・評価値が高くなるにつれて、ゲイン値（補正倍率）を大きくする制御。
評価値が高くなると、評価値が低い時に比べて、ＦＢゲイン部３０３とバイアス推定ゲイン部３０４の各ゲイン値が大きくなる。
・評価値が高くなるにつれて、補正頻度を大きくする制御。
評価値が高くなると、評価値が低い時に比べて、位置姿勢補正部２０８による位置姿勢補正の頻度、およびバイアス補正による位置姿勢推定値の補正の頻度が高く設定される。

以上のように、振動センサのノイズ評価値が高いと判定された場合、振動センサの検出ノイズによって、低周波ドリフト等の低周波成分の位置姿勢推定に誤差が生じることが懸念される。本実施形態では、ハイパスフィルタで低周波成分を除去する対策を講じる。また、ハイパスフィルタによる低周波成分の除去を行うと、位置姿勢推定部２０７による位置姿勢推定値の低周波の動きを正しく検出できなくなる可能性がある。本実施形態では、ＦＢゲイン部３０３のゲイン値を大きくすることにより、低周波域での推定誤差を補正することができる。位置姿勢補正のゲイン値およびバイアス推定のゲイン値を大きくする以外の対策もある。例えば、特徴座標マップ推定部２０６による特徴座標推定の推定周期、および、位置姿勢補正部２０８による位置姿勢推定の補正値およびバイアス補正値の演算周期を短くする方法がある。この場合、ノイズ判定部２１１は特徴座標マップ推定部２０６に対して推定周期を指示し、また位置姿勢補正部２０８に対して補正周期（補正量の演算周期）を指示する。また所定時間における加算器４０２、４０６、４０９、４１５による位置姿勢推定値とバイアスの補正回数を多くすること、つまり補正頻度を高めることで補正ゲイン値を大きくする方法と同等の効果が得られる。この場合、ノイズ判定部２１１は位置姿勢推定部２０７に対して所定期間あたりの補正回数を指示する。

ＦＢゲイン部３０３およびバイアス推定ゲイン部３０４の各ゲイン値を大きくすると、位置姿勢の補正速度が大きくなるので、推定値をより早く真値に近づけることが可能である。しかし、画像から検出される特徴点の追跡が不可能な場合や、特徴点の追跡座標にノイズ成分が重畳されている場合等では、補正した推定結果に誤差が生じる可能性がある。そのため、ゲイン値については、あまり大きくしすぎないことが望ましい。本実施形態では、補正ゲインおよびハイパスフィルタのカットオフ周波数を常に高く設定するのではなく、ノイズ判定部２１１による評価値の判定結果に基づいて制御内容が変更される。振動センサノイズ等が大きく、評価値が高い時に、状況に応じて制御内容が変更されるので、位置姿勢推定の精度を向上させることができる。

図９のフローチャートを参照して、撮像装置１００の位置姿勢推定処理について説明する。図９にてＳ１０１からＳ１０５の処理と、Ｓ１０８からＳ１１０の処理は並行処理として実行される。
まず、Ｓ１０８にて撮像部１０９は、被写体から撮像光学系を介して結像される光信号を電気信号に変換して、画像信号を取得する。次に撮像信号処理部１１０はアナログ信号からデジタル信号への変換と、所定の画像処理を行う。動きベクトル検出部２１０は撮像時刻の異なる複数の画像データから動きベクトルを算出する（Ｓ１０９）。動きベクトル検出部２１０は、メモリにあらかじめ記憶されている１フレーム前の画像を取得し（Ｓ１１１）、当該画像と現在のフレームの画像とを比較し、画像のずれから動きベクトルを算出する。動きベクトルの検出方法としては相関法やブロックマッチング法等がある。本発明において動きベクトルの算出方法には如何なる方法を用いても構わない。

特徴点追跡部２０９は、検出された画像の動きベクトル情報から、撮影画像上の所定の特徴点について、動画撮影時の各フレームでの座標の移動位置を検出して追跡する（Ｓ１１０）。特徴点の追跡技術に関しては、特徴点を中心として正方形のウィンドウを設け、対象映像の新たなフレームが与えられると、フレーム間でウィンドウ内の残差が最も少なくなる点を求める方法がある。この方法による処理は、動画撮影の画像取得のフレームレートに対応する周期で繰り返し実行される。Ｓ１１０の処理後、Ｓ１０６に進む。

次に図９のＳ１０１からＳ１０５を参照して、位置姿勢推定処理を説明する。
第１振動センサ２０１は撮像装置１００の角速度を検出し（Ｓ１０１）、第２振動センサ２０３は撮像装置１００の加速度を検出する（Ｓ１０２）。次に、前回のサンプリング周期での処理により位置姿勢補正部２０８が推定したバイアス補正値を用いて、第１振動センサ２０１および第２振動センサ２０３のバイアス補正が行われる（Ｓ１０３）。ノイズ判定部２１１により設定されたカットオフ周波数のハイパスフィルタ４０４、４１２、４１７は帯域制限した信号をそれぞれ出力する（Ｓ１０４）。前述した演算によって位置姿勢推定値が算出される（Ｓ１０５）。

Ｓ１１０で検出された特徴点の追跡座標と、Ｓ１０５で算出された位置姿勢推定値はひと組の情報として記憶部１１８に記憶される（Ｓ１０６）。次のＳ１０７では現行フレームの番号が、位置姿勢補正部２０８による位置姿勢推定補正を行う動画フレームの番号に相当するか否かについて判定される。位置姿勢推定補正を実行する動画フレームであるかどうかの判定については、特徴座標マップの推定に必要なフレーム数だけ特徴座標と位置姿勢推定値の組数が揃っているか否かによって行われる。例えば、図４で示す特徴座標マップの推定処理の場合、フレーム数が２以上の状態から推定が可能である。よって、２フレーム分の情報が取得された時点でＳ１１２の処理に進む。フレーム数が十分でない場合には処理を終了する。なお、ノイズ判定部２１１により、評価値が高くなるにつれて位置姿勢補正のゲイン値およびバイアス推定ゲイン値を大きくする方法とは別の対策として推定周期や演算周期を短くし、または補正頻度を高める方法について説明した。これらの場合には、Ｓ１０７にて判定される位置姿勢推定補正の実行フレーム数が、より少ないフレーム数に設定される。これは、位置姿勢推定補正値およびバイアス補正値の演算周期を短くし、所定時間における加算器４０２、４０６、４０９、４１５による位置姿勢推定値とバイアス値の補正回数を多くする制御が行われるためである。

Ｓ１１２で特徴座標マップの推定処理が実行された後、位置姿勢補正部２０８によりバイアス値が更新される（Ｓ１１３）。次に位置姿勢推定値の補正処理が行われ（Ｓ１１４）、一連の処理を終了する。Ｓ１０７で否定的判定が下された場合（ＮＯ）、バイアス値および位置姿勢推定値の補正は実施せずにそのまま処理を終了する。

本実施形態によれば、撮像装置の位置姿勢の推定と特徴座標マップ推定の処理および位置姿勢推定値の補正を、動画の各フレームにおいて繰り返し実行することで、正しい位置姿勢の推定値を得ることができる。

［第２実施形態］
次に本発明の第２実施形態を説明する。なお、本実施形態にて第１実施形態の場合と同様の構成要素については既に使用した符号を用いることにより、それらの詳細な説明を省略する。

図１０は、本実施形態の位置姿勢推定部２０７の構成例を示す図である。図３に示す構成例との相違点は以下のとおりである。
・周波数帯域の帯域制限を行うハイパスフィルタ４０４、４１２、４１７が削除されていること。
・第１実施形態の位置姿勢推定とは別に、特徴点座標推定用の位置姿勢推定の変化情報に対するハイパスフィルタ１００１、１００２が追加されていること。
ハイパスフィルタ１００１は加算器４０６の出力に対してフィルタ処理を行い、特徴点座標推定用の姿勢推定値を特徴座標マップ推定部２０６に出力する。ハイパスフィルタ１００２は加算器４１５の出力に対してフィルタ処理を行い、特徴点座標推定用の位置推定値を特徴座標マップ推定部２０６に出力する。ハイパスフィルタ１００１、１００２はいずれもカットオフ周波数を変更可能なフィルタである。

図１１は、本実施形態に係る位置姿勢推定処理を説明するフローチャートである。図９との相違点は以下のとおりである。
・図９のＳ１０４のハイパスフィルタ処理が存在せず、Ｓ１０３のバイアス補正処理の次にＳ１０５の位置姿勢推定処理に進むこと。
・Ｓ１０５とＳ１０６の間のＳ１１０１では、特徴座標マップ推定に用いる位置姿勢の変化情報（位置姿勢推定値の変化）に対しハイパスフィルタ１００１、１００２がそれぞれ処理を行うこと。

本実施形態は、位置姿勢推定部２０７での振動センサによる低周波の位置姿勢変化の推定値に対して、ハイパスフィルタによる帯域制限処理を行わない構成である。これにより、低周波でゆっくりとした位置姿勢変化の推定精度を向上させることができる。一方、特徴座標マップ推定部２０６による３次元座標推定は、位置姿勢推定の大きなずれ（特に位置のドリフト）の影響が推定精度に大きく影響することが判っている。そこで、位置推定値のドリフト誤差による３次元座標推定の誤差を低減して改善するために、３次元座標推定に用いる位置姿勢推定値の変化にのみハイパスフィルタ１００１、１００２による低周波成分の除去を実施する。

本実施形態によれば、３次元座標推定値の推定精度が向上し、位置姿勢推定値の補正により、位置姿勢推定の精度を改善することができる。振動センサ情報の誤差の変動が大きい状況であっても、より精度の高い位置姿勢推定結果を得ることが可能な撮像装置およびその制御方法を提供できる。

１０９ 撮像部
２０１ 第１振動センサ
２０３ 第２振動センサ
２０５ 特徴座標マップおよび位置姿勢推定部
２０６ 特徴座標マップ推定部
２０７ 位置姿勢推定部
２０８ 位置姿勢補正部
２０９ 特徴点追跡部
２１０ 動きベクトル検出部
２１１ 振動センサノイズ判定部

Claims (20)

1. 撮像手段により画像信号を取得する撮像装置であって、
   振れ検出手段によって検出される、前記撮像装置の振れを示す第１の振れ情報を取得する第１の取得手段と、
   前記撮像手段による画像信号から検出される、被写体の動きを示す第２の振れ情報を取得する第２の取得手段と、
   前記第１および第２の振れ情報を用いて前記撮像装置の位置または姿勢を推定する推定手段と、を備え、
   前記推定手段は、前記第１の振れ情報の周波数帯域を制限する帯域制限手段と、前記帯域制限手段の出力により前記撮像装置の位置または姿勢の推定値を算出する算出手段と、前記第２の振れ情報を用いて前記推定値に対する補正値を算出する補正手段を有し、前記推定値を前記補正値によって補正する
   ことを特徴とする撮像装置。
2. 撮像手段により画像信号を取得する撮像装置であって、
   振れ検出手段によって検出される、前記撮像装置の振れを示す第１の振れ情報を取得する第１の取得手段と、
   前記撮像手段による画像信号から検出される、被写体の動きを示す第２の振れ情報を取得する第２の取得手段と、
   前記撮像装置の位置または姿勢を推定する推定手段と、を備え、
   前記推定手段は、前記第１および第２の振れ情報により前記撮像装置の位置または姿勢の推定値を算出する算出手段と、前記第２の振れ情報を用いて前記推定値に対する補正値を算出する補正手段と、前記補正値によって補正された推定値に対して周波数帯域を制限する帯域制限手段を有する
   ことを特徴とする撮像装置。
3. 前記推定手段は推定誤差の評価値を算出する評価手段を備え、前記評価値にしたがって前記帯域制限手段により制限される周波数帯域を変更し、または前記推定値を前記補正値により補正する際の補正倍率を変更する
   ことを特徴とする請求項１または請求項２に記載の撮像装置。
4. 前記第２の振れ情報を用いて撮影画面上の被写体の座標値を算出して特徴点追跡を行う追跡手段を備え、
   前記推定手段は、前記第１の振れ情報および前記被写体の座標値から、前記撮像装置の位置もしくは姿勢、または位置および姿勢と、前記被写体と前記撮像装置との奥行きを含めた位置関係を表す特徴座標マップを生成する
   ことを特徴とする請求項１から３のいずれか１項に記載の撮像装置。
5. 前記補正手段は、前記特徴座標マップと前記撮像装置の位置または姿勢の推定値から前記被写体の撮影画面上の特徴座標を推定し、前記撮影画面上の特徴座標の推定値と前記追跡手段による特徴点の追跡座標から前記補正値を算出する
   ことを特徴とする請求項４に記載の撮像装置。
6. 前記補正手段は、
   前記撮像装置の位置または姿勢の推定値と前記特徴座標マップを取得して撮影画面上の特徴座標への変換を行う変換手段と、
   前記追跡手段による特徴点の追跡座標から前記変換手段の出力を減算する減算手段と、
   前記減算手段の出力に対してゲイン値を乗算して前記補正値を算出するゲイン手段を備える
   ことを特徴とする請求項５に記載の撮像装置。
7. 前記振れ検出手段は角速度センサおよび加速度センサである
   ことを特徴とする請求項１から６のいずれか１項に記載の撮像装置。
8. 前記第２の振れ情報として動きベクトルを検出する検出手段を備える
   ことを特徴とする請求項１から７のいずれか１項に記載の撮像装置。
9. 前記評価手段は前記評価値により前記振れ検出手段のノイズ判定を行う
   ことを特徴とする請求項３に記載の撮像装置。
10. 前記振れ検出手段の出力または該出力の変動量が大きい場合の前記評価値は、前記振れ検出手段の出力または該出力の変動量が小さい場合の前記評価値よりも高い
    ことを特徴とする請求項９に記載の撮像装置。
11. 前記第２の振れ情報を用いて撮影画面上の被写体の座標値を算出して特徴点追跡を行う追跡手段を備え、
    前記推定手段は、前記第１の振れ情報および前記被写体の座標値から、前記撮像装置の位置もしくは姿勢、または位置および姿勢と、前記被写体と前記撮像装置との奥行きを含めた位置関係を表す特徴座標マップを生成し、前記特徴点の追跡座標と、前記特徴座標マップを撮影画面上の特徴座標に変換した座標値との誤差である再射影誤差を算出する
    ことを特徴とする請求項３または請求項９に記載の撮像装置。
12. 前記再射影誤差が大きい場合の前記評価値は、前記再射影誤差が小さい場合の前記評価値よりも高い
    ことを特徴とする請求項１１に記載の撮像装置。
13. 前記撮像装置または当該撮像装置の本体部に装着されるレンズ装置が備える駆動手段の駆動量または駆動速度が大きい場合の前記評価値は、前記駆動手段の駆動量または駆動速度が小さい場合の前記評価値よりも高い
    ことを特徴とする請求項３または請求項９に記載の撮像装置。
14. 前記駆動手段は、撮像光学系を構成する、レンズまたは絞りまたはシャッタを駆動する
    ことを特徴とする請求項１３に記載の撮像装置。
15. 前記振れ検出手段のバイアス補正誤差が大きい場合の前記評価値は、前記振れ検出手段のバイアス補正誤差が小さい場合の前記評価値よりも高い
    ことを特徴とする請求項９に記載の撮像装置。
16. 前記推定手段は、前記評価値が高くなるにつれて前記帯域制限手段のカットオフ周波数を高くする
    ことを特徴とする請求項３、請求項９から１５のいずれか１項に記載の撮像装置。
17. 前記推定手段は、前記評価値が高くなるにつれて前記補正倍率または補正頻度を大きくする
    ことを特徴とする請求項３、請求項９から１６のいずれか１項に記載の撮像装置。
18. 前記推定手段は、前記評価値が高くなるにつれて推定の周期または前記補正値の演算を行う周期を短くする
    ことを特徴とする請求項３、請求項９から１７のいずれか１項に記載の撮像装置。
19. 撮像手段により画像信号を取得する撮像装置にて実行される制御方法であって、
    振れ検出手段によって検出される、前記撮像装置の振れを示す第１の振れ情報、および前記撮像手段による画像信号から検出される、被写体の動きを示す第２の振れ情報を取得する工程と、
    前記第１および第２の振れ情報を用いて前記撮像装置の位置または姿勢を推定する推定工程と、を有し、
    前記推定工程は、
    前記第１の振れ情報の周波数帯域を帯域制限手段によって制限する工程と、
    前記帯域制限手段の出力により前記撮像装置の位置または姿勢の推定値を算出する工程と、
    前記第２の振れ情報を用いて前記推定値を補正する工程を含む
    ことを特徴とする撮像装置の制御方法。
20. 撮像手段により画像信号を取得する撮像装置にて実行される制御方法であって、
    振れ検出手段によって検出される、前記撮像装置の振れを示す第１の振れ情報、および前記撮像手段による画像信号から検出される、被写体の動きを示す第２の振れ情報を取得する工程と、
    前記撮像装置の位置または姿勢を推定する推定工程と、を有し、
    前記推定工程は、
    前記第１および第２の振れ情報により前記撮像装置の位置または姿勢の推定値を算出する工程と、
    前記第２の振れ情報を用いて前記推定値に対する補正値を算出する工程と、
    前記補正値によって補正された推定値に対して周波数帯域を制限する工程を含む
    ことを特徴とする撮像装置の制御方法。
    

Images