JP2024003460A - 姿勢算出装置、姿勢算出方法、姿勢算出装置を備える撮像装置とその制御方法 - Google Patents

Abstract

【課題】 複数のセンサを用いて姿勢角を算出する場合に、精度よく姿勢角を算出することが可能な姿勢算出装置を提供すること。
【解決手段】 姿勢算出手段は、移動装置の動きを検出する第１のセンサと、第２のセンサと、から検出信号を取得する取得手段と、第１のセンサの出力に基づいて移動装置の第１の姿勢情報を算出する第１の算出手段と、第１のセンサの出力と第２のセンサの出力とに基づいて、第１の算出手段と異なる方法で前記移動装置の第２の姿勢情報を算出する第２の算出手段と、移動装置の動きの状態を判定する判定手段と、判定結果に基づいて、前記移動装置の姿勢を決定する決定手段と、を備える。決定手段は、移動装置の動きが第１の状態であると判定された場合は第１の姿勢情報を前記移動装置の姿勢を示す情報とし、移動装置の動きが第１の状態よりも大きい第２の状態であると判定された場合は第２の姿勢情報を移動装置の姿勢を示す情報とする。
【選択図】 図４

Description

本発明は、動きを検出する複数のセンサを用いて姿勢を算出する技術に関するものである。

ビデオカメラ等の撮像装置には、搭載された角速度センサの出力から相対的な姿勢変化を算出する機能を有するものがある。撮像装置の姿勢は直交する３軸まわりの角度で表現され、角速度センサの出力である角速度を積算することによって運動による姿勢（角度）の変化量を算出できる。しかしながら、角速度センサは温度変化等によって特に低周波帯域での出力が変化する特性があるため、温度変化が生じると、角速度を積算して得られた姿勢が真値からずれてしまう。そのため、加速度センサや地磁気センサといった撮像装置の動きを検出可能な他の種類のセンサを併用することで、角速度センサの出力変化による姿勢の算出角度の誤差を低減する方法が提案されている。

特許文献１には、角速度センサの出力信号と加速度センサ及び地磁気センサの出力信号を合成し、撮像装置の角度を算出する姿勢角算出装置が記載されている。

特開２０２０－１８１０５９号公報

しかしながら、加速度センサ及び地磁気センサの出力は、低周波帯域の誤差の影響は角速度センサの出力よりも小さいものの、高周波ノイズが多く含まれている。よって、これらセンサの出力信号を常に使用すると、算出する姿勢角度の精度が低下してしまう場合がある。

本発明は上述の課題を鑑みてなされたものであり、複数のセンサの出力を用いて姿勢角を算出する場合に、精度よく姿勢角を算出することが可能な姿勢算出装置を提供することを目的とする。

本発明の一側面としての姿勢算出装置は、移動装置の動きを検出する第１のセンサと、前記第１のセンサが検出する動きと同じ軸における前記移動装置の動きを検出する第２のセンサと、から検出信号を取得する取得手段と、前記第１のセンサの出力に基づいて前記移動装置の第１の姿勢情報を算出する第１の算出手段と、前記第１のセンサの出力と前記第２のセンサの出力とに基づいて、前記第１の算出手段と異なる方法で前記移動装置の第２の姿勢情報を算出する第２の算出手段と、前記第２のセンサの出力に基づいて前記移動装置の動きの状態を判定する判定手段と、前記判定手段による判定結果に基づいて、前記移動装置の姿勢を決定する決定手段と、を備え、前記第１のセンサは、当該第１のセンサの出力が前記第２のセンサの出力よりも、高周波帯域におけるノイズが小さく、低周波帯域における誤差成分と誤差成分の変動との少なくともいずれかが大きいものであり、前記決定手段は、前記第２のセンサの出力に基づいて取得される前記移動装置の動きが第１の状態であると判定された場合は、前記第１の姿勢情報を前記移動装置の姿勢を示す情報とし、前記第２のセンサの出力に基づいて取得される前記移動装置の動きが前記第１の状態よりも大きい第２の状態であると判定された場合は、前記第２の姿勢情報を前記移動装置の姿勢を示す情報とすることを特徴とする。本発明のその他の側面については、以下で説明する実施の形態で明らかにする。

本発明によれば、複数のセンサの出力を用いて、精度よく姿勢角を算出することが可能な姿勢算出装置を提供することができる。

実施形態のカメラの構成図である。 絶対座標系を説明するための概念図である。 センサ座標系を説明するための概念図である。 実施形態の姿勢演算部の構成図である。 実施形態の姿勢演算部の処理を示すフローチャートである。

以下に、本発明の好ましい実施形態を添付の図面に基づいて詳細に説明する。なお、各図において、同一の部材については同一の参照番号を付し、重複する説明は省略する。尚、以下の実施形態は特許請求の範囲に係る発明を限定するものではない。実施形態には複数の特徴が記載されているが、これらの複数の特徴の全てが発明に必須のものとは限らず、また、複数の特徴は任意に組み合わせられてもよい。

本実施形態では、撮像装置であるカメラ１００（ビデオカメラを含む）内に姿勢算出装置を設けた形態について図面を参照して説明する。

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

撮像レンズ１０１は撮像光学系である。簡略化のため、図１にはレンズを１つしか記載していないが、変倍を行うズームレンズ、像ブレ補正を行うシフトレンズ等の像ブレ補正レンズ、焦点調節を行うフォーカスレンズ等を含むことができる。これらの構成により、被写体像を撮像素子１０２上に結像させる
撮像素子１０２は撮像レンズ１０１により結像された被写体像を撮像するものである。被写体像を光電変換して電荷を蓄積し、その電荷を読み出すことにより複数の画素からなる画像信号を信号処理部１０３へ出力する。撮像素子１０２は、例えばＣＭＯＳ（Ｃｏｍｐｌｅｍｅｎｔａｒｙ Ｍｅｔａｌ Ｏｘｉｄｅ Ｓｅｍｉｃｏｎｄｕｃｔｏｒ）イメージセンサ等で構成される。

信号処理部１０３は、撮像素子１０２から出力された画像信号にホワイトバランス調整やガンマ補正等の信号処理を施し、各フレームの画像信号からなる映像信号を生成して画像メモリ１０４に格納する。表示制御部１０５は画像メモリ１０４に格納された映像信号を読み出し、表示デバイス１０６に画像を表示させる。表示デバイス１０６は、画像を表示することができるデバイスであり、例えばＬＣＤ（Ｌｉｑｕｉｄ Ｃｒｙｓｔａｌ Ｄｉｓｐｌａｙ）パネルで構成される。

角速度センサ１０７は、カメラ１００に加わる動き（振れ）の角速度を検出するセンサであり、直交する３本の検出軸で各軸まわりの角速度を検出する角速度検出部として機能する。検出軸の詳細については後述する。角速度センサ１０７は例えばジャイロセンサである。本発明及び本明細書では、角速度センサ１０７のように、カメラ１００に加わる動きの角速度を検出するセンサであっても、カメラ１００の動きを検出するという。なお、角速度センサ１０７は、１つの検出軸の各軸回りの角速度を検出する３つの角速度センサが別々に設けられてもよいし、ユニット化されていてもよい。角速度センサ１０７は、検出信号である角速度信号を姿勢演算部２００に出力する。

加速度センサ１０８は、カメラ１００に加わる動き（振れ）の加速度を検出するセンサであり、直交する３本の検出軸で加速度を検出する加速度検出部として機能する。これにより、重力方向を検出することができる。本発明及び本明細書では、加速度センサ１０８のように、カメラ１００に加わる動きの加速度を検出するセンサであっても、カメラ１００の動きを検出するという。なお、加速度センサ１０８は、１つの検出軸の各軸回りの加速度を検出する３つの加速度センサが別々に設けられてもよいし、ユニット化されていてもよい。加速度センサ１０８は、検出信号である加速度信号を姿勢演算部２００に出力する。

地磁気センサ１０９は、カメラ１００が受ける磁気を検出するセンサであり、直交する３本の検出軸で磁気を検出することで地磁気の方向を検出する地磁気検出部として機能する。地磁気センサ１０９はカメラ１００の動きそのものを検出するセンサではないが、地磁気の方向の変化に基づいてカメラ１００の動きを検出することができる。よって、本発明及び本明細書では、地磁気センサ１０９のように、カメラ１００に加わる動きの地磁気の方向を検出するセンサであっても、カメラ１００の動きを検出するという。なお、地磁気センサ１０９は、１つの検出軸の各軸回りの地磁気を検出する３つの地磁気センサが別々に設けられてもよいし、ユニット化されていてもよい。地磁気センサ１０９は、検出信号である地磁気信号を姿勢演算部２００に出力する。

姿勢演算部２００は角速度センサ１０７から出力された角速度信号、加速度センサ１０８から出力された加速度信号、及び地磁気１０９センサから出力された地磁気信号を用いた演算処理を行う。これにより、カメラ１００の姿勢を示す情報（以下、姿勢情報と呼ぶことがある）として、基準角度に対する現在のカメラ１００の角度（以下、姿勢角と呼ぶことがある）を算出する。

姿勢演算部２００による詳細な姿勢算出方法は後述する。姿勢演算部２００は算出した姿勢情報を記録制御部１１１に出力する。

操作部１１０は、ユーザのカメラ操作による、映像（動画）記録開始及び停止の操作情報を取得する。操作部１１０は、例えばタッチパネルやスイッチ等で構成される。

記録制御部１１１は操作部１１０から映像の記録が指示された場合、画像メモリ１０４に格納された映像信号と姿勢演算部２００から出力された姿勢情報とを同期させて記録媒体１１２に出力し、記録させる。記録媒体１１２は、カメラ１００に備え付けられていてもよいし、可搬式であってもよく、例えば半導体メモリやハードディスク等で構成される。

図２は、カメラ１００の姿勢を表すための絶対座標系を示す図である。絶対座標系は、直交する３軸で構成される。本実施形態では絶対座標系の３軸をＸ_Ｏ、Ｙ_Ｏ、Ｚ_Ｏで表し、東方向をＸ_Ｏ軸、重力方向をＹ_Ｏ軸、北方向をＺ_Ｏ軸と定義する。また、絶対座標系のＸ_Ｏ軸まわりの回転角をＰｉｔｃｈ角（ピッチ角）、Ｙ_Ｏ軸まわりの回転角をＹａｗ角（ヨー角）、Ｚ_Ｏ軸まわりの回転角をＲｏｌｌ角（ロール角）とする。

図２（ａ）は、カメラ１００の姿勢が第１の姿勢である時のカメラ１００と絶対座標との関係を示す図である。図２（ｂ）は、カメラ１００の姿勢が第１の姿勢から変化した後のカメラ１００と絶対座標との関係を示す図である。絶対座標系の各軸の向きは、カメラ１００（搭載されたセンサを含む）の姿勢が変化しても変化しない。

本実施形態では、図２（ａ）に示すように、撮像レンズ１０１の光軸がＺ_Ｏ軸、カメラ１００水平方向がＸ_Ｏ軸と一致する第１の姿勢をとるときの姿勢角（Ｐｉｔｃｈ角、Ｙａｗ角、Ｒｏｌｌ角）を０角度であるとして説明をする。カメラ１００が所定の姿勢をとるときの、この０角度に対する変位角を初期角度とし、後述する加速度角度演算部２０１及び地磁気角度演算部２０２で算出する。以降は初期角度を基準とした相対的な角度をカメラの姿勢角として算出する。所定の姿勢は特に問わず、例えば、カメラの電源起動時の姿勢でもよいし、映像記録開始時の変位角でもよい。この場合、初期角度は、それぞれ、カメラの電源起動時の変位角または、映像記録開始時の変位角となる。

図３は、角速度センサ１０７の検出方向を表すためのセンサ座標系を示す図である。センサ座標系は、直交する３軸で構成される。本実施形態ではセンサ座標系の３軸をＸ、Ｙ、Ｚで表し、角速度センサ１０７は、Ｘ、Ｙ、Ｚ軸を中心とする回転角の速度を検出する。また、カメラ１００が上述の第１の姿勢をとるときはセンサ座標系の３軸をＸ、Ｙ、Ｚの向きが絶対座標系のＸ_Ｏ、Ｙ_Ｏ、Ｚ_Ｏの向きとそれぞれ一致しているものとする。

図３（ａ）は、角速度センサ１０７が固定されているカメラ１００が第１の姿勢をとるとき（映像記録開始時）のカメラ１００とセンサ座標との関係を示す図である。図３（ｂ）は、カメラ１００の姿勢が第１の姿勢から変化したときのカメラ１００とセンサ座標との関係を示す図である。センサ座標系は、カメラ１００の姿勢が変化すると、その変化に相当する回転角で回転し、各軸（Ｘ、Ｙ、Ｚ）の向きが変化する。そのため、角速度センサ１０７の出力からカメラ１００の姿勢を算出するためには、センサ座標系から絶対座標系への変換が必要である。この変換方法についての説明は後述する。

図３では、角速度センサ１０７のみについて説明したが、加速度センサ１０８及び地磁気センサ１０９の出力についても同様にセンサ座標系で表されるため、絶対座標系への変換が必要である。尚、本実施形態では、角速度センサ１０７と加速度センサ１０８と地磁気センサ１０９との検出軸はＸ、Ｙ、Ｚで一致しているものとするが、各センサの検出軸がズレている場合はその補正を行うことで同様に姿勢を検出することができる。

以下、姿勢演算部２００による姿勢情報の算出について説明する。姿勢演算部２００は、姿勢情報を算出する方法が異なる第１の算出手段と第２の算出手段とを有し、カメラ１００の動きの大きさに応じていずれの算出手段で算出された姿勢情報をカメラの姿勢情報とするかを決定して出力する。カメラ１００の動きが小さいときは、低周波帯域の誤差と誤差変化が加速度センサ１０８と地磁気センサ１０９よりも生じやすいが、高周波帯域のノイズ成分が少ない角速度センサ１０７の出力に基づいて姿勢情報を検出する第１の算出手段を選択する。カメラの動きが大きいときは、角速度センサ１０７の出力に加えて、加速度センサ１０８と地磁気センサ１０９の出力にも基づいて姿勢情報を算出する第２の算出手段を選択する。このように、角速度センサ１０７の出力のみに基づいて姿勢情報を算出すると誤差が生じやすい状態のときに加速度センサ１０８と地磁気センサ１０９も用いて姿勢を算出する。これにより、低周波帯域の誤差と高周波帯域のノイズ成分との影響を軽減した姿勢情報を算出することができる。

図４及び図５を用いて姿勢演算部２００による姿勢情報の算出について詳細に説明する。図４は、姿勢演算部２００の構成を示すブロック図である。また、図５は、姿勢演算部２００における処理の流れを示すフローチャート図である。図５に示す処理は、任意の所定の周期で繰り返し実行される。

姿勢演算部２００を構成する各ブロックについて説明をする。加速度角度演算部２０１は、加速度センサ１０８から出力されたセンサ座標系における加速度信号（Ａ_ｘ、Ａ_ｙ、Ａ_ｚ：Ｘ軸加速度、Ｙ軸加速度、Ｚ軸加速度）から絶対座標系におけるＰｉｔｃｈ角θ_Ａ及びＲｏｌｌ角φ_Ａを（式１）、（式２）によって算出する。算出結果は、静止判定部２０３及びカルマンフィルタ演算部２０７に出力する。

地磁気角度演算部２０２は、地磁気センサ１０９から出力されたセンサ座標系における地磁気信号（Ｍ_ｘ、Ｍ_ｙ、Ｍ_ｚ：Ｘ軸地磁気、Ｙ軸地磁気、Ｚ軸地磁気）から絶対座標系におけるＹａｗ角を算出する。まず、センサ座標系で表される地磁気信号（Ｍ_ｘ、Ｍ_ｙ、Ｍ_ｚ）と、後述する出力決定部２０８から出力される姿勢角（ψ、θ、φ：Ｙａｗ角、Ｐｉｔｃｈ角、Ｒｏｌｌ角）とを用いて、絶対座標系における地磁気信号（Ｍ_ｘ’、Ｍ_ｙ’_、Ｍ_ｚ’：Ｘ_０軸地磁気、Ｙ_０軸地磁気、Ｚ_０軸地磁気）を（式３）によって算出する。尚、出力決定部２０８から出力される姿勢角（ψ、θ、φ）を第３の姿勢情報と呼ぶことがある。

算出した絶対座標系における地磁気信号から（式４）によってＹａｗ角ψ_Ｍを算出し、算出結果を静止判定部２０３及びカルマンフィルタ演算部２０７に出力する。

静止判定部２０３は、加速度角度演算部２０１から出力されたＰｉｔｃｈ角θ_Ａ及びＲｏｌｌ角φ_Ａ、及び地磁気角度演算部２０２から出力されたＹａｗ角ψ_Ｍを用いて、カメラ１００に生じている動きの状態を判定する判定手段として機能する。本実施形態の静止判定部２０３は、カメラ１００の動きの状態として静止しているか否かを判定し、その判定結果（静止状態／非静止状態）をオフセット演算部２０４及び出力決定部２０８に出力する。静止判定部２０３は、例えば、Ｐｉｔｃｈ角θ_Ａ及びＲｏｌｌ角φ_Ａ、Ｙａｗ角ψ_Ｍの所定時間の出力変動幅が、それぞれ所定値以内であればカメラ１００が静止状態であると判定する。静止判定部２０３は、低周波帯域の誤差が角速度センサ１０７よりも小さい加速度センサ１０８と地磁気センサ１０９との出力に基づいてカメラ１００に生じている動きの状態を判定する。そのため、角速度センサ１０７の出力に基づいて動きの状態を判定するよりも、精度よく動きの状態を判定できる。

オフセット演算部２０４は、角速度センサ１０７から出力された角速度信号に含まれるオフセット値を算出して出力する。以下、オフセット演算部２０４で算出されたオフセット値を、オフセット算出値と呼ぶ。本実施形態では、静止判定部２０３でカメラ１００が静止状態であると判定された場合にのみ、オフセット値を新たに算出して出力することで、オフセット算出値を更新する。一方で、非静止状態であると判定された場合にはオフセット値の算出と更新はせず、前回のオフセット算出値を出力する。本実施形態では、オフセット算出値は、所定の期間に出力される角速度信号の平均値とする。尚、平均値の代わりに、所定の期間に出力される角速度信号に対してローパスフィルタ処理を施して抽出した低周波帯域の成分を、オフセット算出値としてもよい。

オフセット減算器２０５は、角速度センサ１０７から出力された角速度信号からオフセット演算部２０４から出力されたオフセット算出値を差し引いたオフセット補正後角速度信号を角速度角度演算部２０６に出力する。

角速度角度演算部２０６は、角速度に基づいて、カメラの姿勢情報を算出する第１の算出手段として機能する。角速度角度演算部２０６は、オフセット減算器２０５から出力されたオフセット補正後角速度信号及び後述する出力決定部２０８から出力された姿勢角に基づいて絶対座標系におけるカメラ１００の姿勢角を算出して出力する。角速度角度演算部２０６により算出される姿勢角（ψ_Ｇ，θ_Ｇ，φ_Ｇ：Ｙａｗ角、Ｐｉｔｃｈ角、Ｒｏｌｌ角）を以下、第１の姿勢情報と呼ぶことがある。

角速度角度演算部２０６による絶対座標系におけるカメラ１００の姿勢角の算出方法について説明をする。まず、（式５）に示すようにセンサ座標系で表されるオフセット補正後角速度信号（ω_ｙ，ω_ｘ，ω_ｚ：Ｙａｗ角速度、Ｐｉｔｃｈ角速度、Ｒｏｌｌ角速度）を、後述する出力決定部２０８から出力される姿勢角（ψ、θ、φ）を用いて絶対座標系における

に変換する。

次に、（式６）に示すように、算出した絶対座標系における

を積算することによって絶対座標系における角度（ψ_Ｇ，θ_Ｇ，φ_Ｇ）を算出する。角速度信号の積分器（ψ_{Ｇ（ｔ－１）}，θ_{Ｇ（ｔ－１）}，φ_{Ｇ（ｔ－１）}）は、映像記録開始時（ｔ＝０）では０であり、時間経過とともに角速度信号が検出され、角速度が積算されていく。

角速度角度演算部２０６は、算出した絶対座標系における角度（ψ_Ｇ，θ_Ｇ，φ_Ｇ）を、カルマンフィルタ演算部２０７及び出力決定部２０８に出力する。

カルマンフィルタ演算部２０７は、角速度角度演算部２０６から出力された姿勢角（ψ_Ｇ，θ_Ｇ，φ_Ｇ）に加え、加速度センサと地磁気センサとの検出結果も用いて第２の姿勢情報を算出する第２の算出手段として機能する。具体的には、角速度角度演算部２０６から出力された姿勢角（ψ_Ｇ，θ_Ｇ，φ_Ｇ）、加速度角度演算部２０１から出力されたＰｉｔｃｈ角θ_Ａ及びＲｏｌｌ角φ_Ａ、及び地磁気角度演算部２０２から出力されたＹａｗ角ψ_Ｍをカルマンフィルタに入力する。これにより、各出力に含まれる誤差を低減した第２の姿勢情報（ψ_Ｋ，θ_Ｋ，φ_Ｋ：Ｙａｗ角、Ｐｉｔｃｈ角、Ｒｏｌｌ角）を算出する。

カルマンフィルタ演算部２０７による第２の姿勢情報の算出方法について説明をする。カルマンフィルタは状態方程式及び観測方程式からリアルタイムに最適な状態値を推定するフィルタである。カルマンフィルタは公知の情報であるため、詳細な説明は省略する。（式７）に示すように、カルマンフィルタの状態値ｘ_（ｔ）を求めるべき姿勢角（ψ_Ｋ，θ_Ｋ，φ_Ｋ）とし、観測値ｙ_（ｔ）を加速度角度演算部２０１及び地磁気角度演算部２０２から出力された姿勢角（ψ_Ｍ、θ_Ａ、φ_Ａ）とする。さらに、入力値ｕ_（ｔ）を角速度角度演算部２０６から出力された姿勢角（ψ_Ｇ，θ_Ｇ，φ_Ｇ）とすると、状態方程式及び観測方程式は（式８）で示される。

なお、プロセスノイズｖ_（ｔ）、及び観測ノイズｗ_（ｔ）は、各センサ（角速度センサ１０７、加速度センサ１０８、地磁気センサ１０９）に含まれるノイズや発生する誤差を勘案して、公知の技術に則って、適切な値に設定する。

出力決定部２０８は、静止判定部２０３によるカメラ１００の静止判定結果に応じて、第１の姿勢情報と第２の姿勢情報とのいずれをカメラ１００の姿勢情報として出力するかを決定する。そして、決定された方の姿勢情報を第３の姿勢情報（ψ、θ、φ）として記録制御部１１１に出力する。出力決定部２０８は、静止判定結果が、カメラ１００が静止状態であることを示す場合には角速度角度演算部２０６から出力された第１の姿勢情報（ψ_Ｇ，θ_Ｇ，φ_Ｇ）を第３の姿勢情報（ψ、θ、φ）として出力する。一方、出力決定部２０８は、静止判定結果が、カメラが非静止状態であることを示す場合にはカルマンフィルタ演算部２０７から出力された第２の姿勢情報（ψ_Ｋ，θ_Ｋ，φ_Ｋ）を第３の姿勢情報（ψ、θ、φ）として出力する。

このように構成される姿勢演算部２００による姿勢情報の算出フローについて、図５のフローチャートを用いて説明をする。

まず、ステップＳ１０１において、加速度角度演算部２０１は、加速度センサ１０８から出力されたセンサ座標系における加速度信号（Ａ_ｘ、Ａ_ｙ、Ａ_ｚ）から絶対座標系におけるＰｉｔｃｈ角θ_Ａ及びＲｏｌｌ角φ_Ａを算出する。

次に、ステップＳ１０２において、地磁気角度演算部２０２は、地磁気センサ１０９から出力されたセンサ座標系における地磁気信号（Ｍ_ｘ、Ｍ_ｙ、Ｍ_ｚ）から絶対座標系におけるＹａｗ角を算出する。

次に、ステップＳ１０３において、静止判定部２０３は、ステップＳ１０１で算出したＰｉｔｃｈ角θ_Ａ、Ｒｏｌｌ角φ_Ａ、ステップＳ１０２で算出したＹａｗ角ψ_Ｍを用いて、カメラ１００が静止しているか否かを判定する。静止状態であると判定された場合は、ステップＳ１０４へ進み、静止状態でない（非静止状態である）と判定された場合は、ステップＳ１０５へ進む。

ステップＳ１０４において、オフセット演算部２０４は、角速度センサ１０７から出力された角速度信号に含まれるオフセット値を算出して、前回のステップＳ１０４で算出されたオフセット値を更新してステップＳ１０５へ進む。

ステップＳ１０５において、角速度角度演算部２０６は、オフセット補正後角速度信号（ω_ｙ，ω_ｘ，ω_ｚ）及び前回のフローで出力決定部２０８から出力された姿勢角（ψ，θ，φ）に基づいて、カメラの第１の姿勢情報（ψ_Ｇ，θ_Ｇ，φ_Ｇ）を算出する。

ステップＳ１０６では、カルマンフィルタ演算部２０７は、ステップＳ１０５で算出された第１の姿勢情報（ψ_Ｇ，θ_Ｇ，φ_Ｇ）、加速度角度演算部２０１と地磁気角度演算部２０２からから出力されたＰｉｔｃｈ角θ_Ａ、Ｒｏｌｌ角φ_Ａ、Ｙａｗ角ψ_Ｍに基づいて第２の姿勢情報（ψ_Ｋ，θ_Ｋ，φ_Ｋ）を算出する。

ステップＳ１０７では、ステップＳ１０３で静止判定部２０３が判定した結果を参照する。判定結果が静止状態であることを示す場合（つまり、ステップＳ１０４を経由してステップＳ１０７へ至った場合）はステップＳ１０８へ進む。判定結果が静止状態であることを示さない場合（つまり、ステップＳ１０４を経由せずにステップＳ１０７へ至った場合）はステップＳ１０９へ進む。

ステップＳ１０８では、角速度センサ１０７の出力に基づいて算出された第１の姿勢をカメラ１００の姿勢情報として出力する。ステップＳ１０８を実行する場合、カメラ１００が静止状態であるため、角速度センサ１０７の出力に含まれる低周波帯域の誤差成分の影響がオフセット算出値の減算により十分に低減できていることが推測される。そのため、高周波帯域のノイズの影響が第２の姿勢よりも小さい第１の姿勢をカメラ１００の姿勢情報として出力する。

一方、ステップＳ１０９では、角速度センサ１０７、加速度センサ１０８及び地磁気センサ１０９の出力に基づいて算出された第２の姿勢をカメラ１００の姿勢情報として出力する。ステップＳ１０９を実行する場合、カメラ１００が非静止状態であり、カメラ１００の動きに起因する低周波成分と誤差とを分離することが難しいため、加速度センサ１０８及び地磁気センサ１０９の検出結果にも基づいて姿勢情報を算出する。

以上説明してきたように、本実施形態によれば、加速度センサ及び地磁気センサに含まれる高周波ノイズによる影響を抑えつつ、温度等による角速度センサの低周波帯域の誤差または誤差変動の影響を精度よく補正し、カメラの姿勢角を算出することが可能となる。

［変形例］
上述の実施形態では、カメラ１００の姿勢は、撮像レンズ１０１の光軸がＺ_Ｏ軸、カメラ本体の水平方向がＸ_Ｏ軸と一致する状態を０角度とし、初期角度に対する相対的な角度として算出する方法について説明した。しかしながら、０角度として設定する姿勢は特に問わない。例えば、Ｐｉｔｃｈ角に対して傾いた状態が０角度であってもよい。

上述の実施形態では、角速度センサ１０７を第２のセンサよりも低周波帯域の誤差成分または誤差成分の変動が大きく、高周波帯域のノイズ成分が小さい第１のセンサとした。また、加速度センサ１０８及び地磁気センサ１０９を第１のセンサよりも低周波帯域の誤差成分または誤差成分の変動は小さく、高周波帯域のノイズ成分が大きい第２のセンサとした。しかしながら、使用するセンサはこれらに限定されない。角度情報が取得できる別のセンサを用いる場合、低周波帯域の誤差成分または誤差成分の変動が相対的に大きく高周波帯域のノイズ成分が相対的に小さいものを上述の実施形態における角速度センサに当てはめる。そして、低周波帯域の誤差成分または誤差成分の変動が相対的に小さく高周波帯域のノイズ成分が相対的に大きいものを上述の実施形態における加速度センサ及び地磁気センサに当てはめることで、上述の実施形態と同様に姿勢情報を算出することができる。

また、上述の実施形態では、静止しているか否かに応じて第１の算出手段と第２の算出手段のいずれで算出された姿勢情報をカメラの姿勢情報とするかを選択したが、本発明はこれに限定されない。例えば、静止しているとまでは言えなくても、角速度センサ１０７の出力に基づいて姿勢を算出したほうが算出される姿勢の精度が高いと判断される程度の小さな動きしか生じていない場合は、第１の姿勢情報をカメラ１００の姿勢情報とする形態でもよい。

また、上述の実施形態では、撮像装置であるカメラ１００が姿勢算出装置を備える形態について説明をしたが、姿勢算出装置はこれに限定されず、姿勢算出装置が姿勢を算出する対象である移動装置も撮像装置に限定されない。

例えば、上述の実施形態の機能を有し、車体の姿勢として傾斜角を算出するための傾斜角検出装置として機能する姿勢算出装置も本発明に含む。また、本発明及び本明細書において移動装置とは駆動部を備え、自力で移動できる移動体（自動車や電車などの車両、ドローンなどの航空機を含む）だけではなく、ユーザや移動体によって姿勢を変えることができる可搬装置を含む。具体的には、撮像装置のほか、携帯電話（スマートフォン）、タブレット、パーソナルコンピュータ、ヘッドマウントディスプレイ、スマートグラス等を含む。

また、姿勢算出装置は、必ずしも移動装置内に設けられていなくてもよい。移動装置内に設けられた各種センサから検出信号を受信できれば、姿勢算出装置は移動装置から離れた位置に設置され、移動装置と一体的に移動しない構成としてもよい。検出信号の受信は、有線または無線の通信により取得可能に構成されていれば良い。

また、上述の実施形態では、カメラが静止状態であってもカルマンフィルタ演算部２０７により第２の姿勢情報の算出を行った。しかしながら、カメラが静止状態であり、決定手段が角速度角度演算部２０６からの出力を姿勢算出手段２００からの出力とする場合は、カルマンフィルタ演算部２０７での第２の姿勢情報の算出を一時的に停止してもよい。例えば、第２の姿勢情報（ψ_Ｋ，θ_Ｋ，φ_Ｋ）の算出を行うステップＳ１０６を、ステップＳ１０７で静止状態でないと判定された後に行ってもよい。

上述の実施形態の機能を実現するソフトウェアのプログラムを、記録媒体から直接、或いは有線／無線通信を用いてプログラムを実行可能なコンピュータを有するシステム又は装置に供給し、そのプログラムを実行する場合も本発明に含む。

従って、本発明の機能処理をコンピュータで実現するために、該コンピュータに供給、インストールされるプログラムコード自体も本発明を実現するものである。つまり、本発明の機能処理を実現するためのコンピュータプログラム自体も本発明に含まれる。その場合、プログラムの機能を有していれば、オブジェクトコード、インタプリタにより実行されるプログラム、ＯＳに供給するスクリプトデータ等、プログラムの形態を問わない。プログラムを供給するための記録媒体としては、例えば、ハードディスク、磁気テープ等の磁気記録媒体、光／光磁気記憶媒体、不揮発性の半導体メモリでもよい。また、プログラムの供給方法としては、コンピュータネットワーク上のサーバに本発明を形成するコンピュータプログラムを記憶し、接続のあったクライアントコンピュータがコンピュータプログラムをダウンロードしてプログラムするような方法も考えられる。

以上、本発明をその好適な実施形態に基づいて詳述してきたが、本発明はこれら特定の実施形態に限られるものではなく、この発明の要旨を逸脱しない範囲の様々な形態も本発明に含まれる。また、上述の実施形態の一部を適宜組み合わせてもよい。

１００ カメラ
１０７ 角速度センサ
１０８ 加速度センサ
１０９ 地磁気センサ
２００ 姿勢演算部
２０１ 加速度角度演算部
２０２ 地磁気角度演算部
２０３ 静止判定部
２０４ オフセット演算部
２０５ オフセット減算器
２０６ 角速度角度演算部
２０７ カルマンフィルタ演算部
２０８ 出力決定部

Claims (13)

1. 移動装置の動きを検出する第１のセンサと、前記第１のセンサが検出する動きと同じ軸における前記移動装置の動きを検出する第２のセンサと、から検出信号を取得する取得手段と、
   前記第１のセンサの出力に基づいて前記移動装置の第１の姿勢情報を算出する第１の算出手段と、
   前記第１のセンサの出力と前記第２のセンサの出力とに基づいて、前記第１の算出手段と異なる方法で前記移動装置の第２の姿勢情報を算出する第２の算出手段と、
   前記第２のセンサの出力に基づいて前記移動装置の動きの状態を判定する判定手段と、
   前記判定手段による判定結果に基づいて、前記移動装置の姿勢を決定する決定手段と、を備え、
   前記第１のセンサは、当該第１のセンサの出力が前記第２のセンサの出力よりも、高周波帯域におけるノイズが小さく、低周波帯域における誤差成分と誤差成分の変動との少なくともいずれかが大きいものであり、
   前記決定手段は、
   前記第２のセンサの出力に基づいて取得される前記移動装置の動きが第１の状態であると判定された場合は、前記第１の姿勢情報を前記移動装置の姿勢を示す情報とし、
   前記第２のセンサの出力に基づいて取得される前記移動装置の動きが前記第１の状態よりも大きい第２の状態であると判定された場合は、前記第２の姿勢情報を前記移動装置の姿勢を示す情報とすることを特徴とする姿勢算出装置。
2. 前記第１の姿勢情報と前記第２の姿勢情報とのそれぞれは、所定の姿勢に対する前記移動装置のＹａｗ角とＰｉｔｃｈ角とＲｏｌｌ角との角度の情報を含むことを特徴とする請求項１に記載の姿勢算出装置。
3. 前記第１のセンサは、３軸における前記移動装置の動きを検出し、
   前記第２のセンサは、３軸における前記移動装置の動きを検出することを特徴とする請求項１に記載の姿勢算出装置。
4. 前記第１の算出手段は、
   前記第１のセンサの出力に対してローパスフィルタ処理を施した信号に基づいて前記第１の姿勢情報を算出することを特徴とする請求項１に記載の姿勢算出装置。
5. 前記第１の算出手段は、
   所定の期間の前記第１のセンサの出力の平均値を第１のセンサの出力から除した信号に基づいて前記第１の姿勢情報を算出することを特徴とする請求項１に記載の姿勢算出装置。
6. 前記第１のセンサは前記移動装置の動きの角速度を検出する角速度センサであり、
   前記第２のセンサは前記移動装置の動きの加速度を検出する加速度センサまたは前記移動装置の方向の変化を検出する地磁気センサの少なくともいずれかであることを特徴とする請求項１に記載の撮像装置。
7. 前記第１の姿勢情報と前記第２の姿勢情報とのそれぞれは、前記移動装置のＹａｗ角とＰｉｔｃｈ角とＲｏｌｌ角とであり、
   前記第２のセンサは、前記地磁気センサと前記加速度センサとを有し、
   前記第２の算出手段は、
   前記地磁気センサの出力と前記角速度センサの出力とに基づいて前記Ｙａｗ角を算出し、前記加速度センサの出力と前記角速度センサの出力とに基づいて前記Ｐｉｔｃｈ角と前記Ｒｏｌｌ角とを算出する請求項６に記載の姿勢算出装置。
8. 前記第１の状態は、前記移動装置が静止している状態であることを特徴とする請求項１に記載の姿勢算出装置。
9. 前記第２の算出手段は、
   前記第１のセンサの出力に基づく信号と前記第２のセンサの出力に基づく信号とをカルマンフィルタに入力することで前記第２の姿勢情報を算出することを特徴とする請求項１に記載の姿勢算出装置。
10. 撮像光学系により結像される被写体像を撮像する撮像素子を備える撮像装置であって、
    前記撮像素子が出力する画像信号の記録を制御する記録制御手段と、
    請求項１乃至９のいずれか１項に記載の姿勢算出装置と、を備え、
    前記姿勢算出装置が算出する前記移動装置の姿勢は、前記撮像装置の姿勢であり、
    前記記録制御手段は、前記画像信号と前記決定手段により決定された前記撮像装置の姿勢とを同期づけて記録することを特徴とする撮像装置。
11. 前記第１のセンサと前記第２のセンサとを備える請求項１０に記載の撮像装置。
12. 駆動部を備える移動体であって、
    請求項１乃至９のいずれか１項に記載の姿勢算出装置を備え、
    前記姿勢算出装置が算出する前記移動装置の姿勢は、前記移動体の姿勢であることを特徴とする移動体。
13. 移動装置の姿勢を示す情報を算出する姿勢算出装置の制御方法であって、
    前記移動装置の動きを検出する第１のセンサと、前記第１のセンサが検出する動きと同じ軸における前記移動装置の動きを検出する第２のセンサと、からの検出信号を取得する取得工程と、
    前記第１のセンサの出力に基づいて前記移動装置の第１の姿勢を示す情報を算出する第１の算出工程と、
    前記第１のセンサの出力と前記第２のセンサの出力とに基づいて、前記第１の算出工程と異なる方法で前記移動装置の第２の姿勢を示す情報を算出する第２の算出工程と、
    前記第２のセンサの出力に基づいて前記移動装置の動きの状態を判定する判定工程と、
    前記判定工程による判定結果に基づいて、前記移動装置の姿勢を決定する決定工程と、を有し、
    前記第１のセンサは、当該第１のセンサの出力が前記第２のセンサの出力よりも、高周波帯域におけるノイズが小さく、低周波帯域における誤差成分と誤差成分の変動との少なくともいずれかが大きいものであり、
    前記決定工程において、
    前記第２のセンサの出力に基づいて取得される前記移動装置の動きが第１の状態であると判定された場合は、前記第１の姿勢を示す情報を前記移動装置の姿勢を示す情報とし、
    前記第２のセンサの出力に基づいて取得される前記移動装置の動きが前記第１の状態よりも大きい第２の状態であると判定された場合は、前記第２の姿勢を示す情報を前記移動装置の姿勢を示す情報とすることを特徴とする姿勢算出装置の制御方法。

Images