(57)【要約】 【課題】処理負荷を抑えつつ、姿勢を推定する。【解決手段】３方向の加速度を周期的に検出して検出結果を加速度データａとして順次出力する加速度センサ８０と、３方向の角速度を周期的に検出して検出結果を角速度データωとして順次出力する角速度センサ９０と、を備えた携帯機器１に設けられ、携帯機器１の姿勢ｑを推定する姿勢推定装置であって、角速度データωに基づいて、現時刻よりも過去の時刻における携帯機器１の姿勢ｑから、現時刻における携帯機器１の姿勢ｑを予測する姿勢予測部２１と、加速度データａに基づいて、姿勢予測部２１の予測結果を補正する姿勢補正部２２と、姿勢予測部２１の予測結果または姿勢補正部２２の補正結果のうち何れか一方を、携帯機器１の姿勢ｑの推定値として出力する推定姿勢出力部２３と、を備える、ことを特徴とする姿勢推定装置。【選択図】図４
2016109608.tif 000002
【特許請求の範囲】
【請求項１】
３方向の加速度を周期的に検出して検出結果を加速度データとして順次出力する加速度センサと、
３方向の角速度を周期的に検出して検出結果を角速度データとして順次出力する角速度センサと、
を備えた機器に設けられた姿勢推定装置であって、
前記角速度データに基づいて、現時刻よりも過去の時刻における前記機器の姿勢から、現時刻における前記機器の姿勢を予測する予測部と、
前記加速度データに基づいて、前記予測部の予測結果を補正する補正部と、
を備える、
ことを特徴とする姿勢推定装置。
【請求項２】
前記加速度データに基づいて前記機器の動きが安定しているか否かを判定する判定部を備え、
前記補正部は、
前記判定部の判定結果が肯定の場合に、前記予測部の予測結果を補正する、
ことを特徴とする、請求項１に記載の姿勢推定装置。
【請求項３】
前記補正部は、
現時刻に前記加速度センサが出力する加速度データの示す値と、前記機器が前記予測部の予測結果の示す姿勢である場合に前記加速度センサが出力する加速度データの示す値との、加重平均値に基づいて、前記予測部の予測結果を補正する、
ことを特徴とする、請求項１または２に記載の姿勢推定装置。
【請求項４】
３方向の加速度を周期的に検出して検出結果を加速度データとして順次出力する加速度センサと、
３方向の角速度を周期的に検出して検出結果を角速度データとして順次出力する角速度センサと、
コンピュータと、
を備えた機器に設けられた姿勢推定装置の制御プログラムであって、
前記コンピュータを、
前記角速度データに基づいて、現時刻よりも過去の時刻における前記機器の姿勢から、現時刻における前記機器の姿勢を予測する予測部と、
前記加速度データに基づいて、前記予測部の予測結果を補正する補正部と、
して機能させる、
ことを特徴とする姿勢推定装置の制御プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は姿勢推定装置及び姿勢推定装置の制御プログラムに関する。
【背景技術】
【０００２】
物体の姿勢等、動的システムの状態を推定する演算を実行する場合、１種類のセンサの出力結果に基づいて算出するよりも、複数のセンサの出力結果を統合して算出した方が、短時間に正確な推定をすることができる。
【０００３】
異種の物理量を測定する複数のセンサの出力を統合し動的システムの状態を推定する方法として、カルマンフィルタを用いる方法が知られている。例えば、特許文献１には、非線形カルマンフィルタの１種であるシグマポイントカルマンフィルタを用いて、３軸の角速度センサ及び３軸の加速度センサ等の複数のセンサからの出力を統合することで、姿勢を含む動的システムの状態を推定する推定装置が開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１２−１３２７１３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ところで、カルマンフィルタの演算では、姿勢を含むシステムの状態を周期的に推定する演算を実行するのに加えて、当該状態の推定値の推定誤差の共分散を周期的に演算することが必要となる。このため、カルマンフィルタを用いて姿勢を推定する場合には、処理負荷が大きくなるという問題が存在した。また、カルマンフィルタの演算では、状態モデルを用いて姿勢を推定するため、当該状態モデルが正確でない場合には、正確な姿勢の推定も困難になるという問題が存在した。
【０００６】
本発明は、上述した点に鑑みてなされたものであり、処理負荷を抑えつつ、姿勢を推定する技術を提供することを、解決課題の一つとする。
【課題を解決するための手段】
【０００７】
上述した課題を解決するため、本発明に係る姿勢推定装置は、３方向の加速度を周期的に検出して検出結果を加速度データとして順次出力する加速度センサと、３方向の角速度を周期的に検出して検出結果を角速度データとして順次出力する角速度センサと、を備えた機器に設けられた姿勢推定装置であって、前記角速度データに基づいて、現時刻よりも過去の時刻における前記機器の姿勢から、現時刻における前記機器の姿勢を予測する予測部と、前記加速度データに基づいて、前記予測部の予測結果を補正する補正部と、を備える、ことを特徴とする。
【０００８】
この発明に係る姿勢推定装置によれば、角速度データに基づいて機器の姿勢の予測値を算出し、加速度データに基づいて機器の姿勢の予測値を補正することで、機器の姿勢を推定する。すなわち、本発明にかかる姿勢推定装置では、機器の姿勢の推定値の推定誤差の共分散を算出することなく、機器の姿勢を推定する。よって、本発明にかかる姿勢推定装置では、カルマンフィルタ等のように、推定値の推定誤差の共分散を算出しつつ姿勢を推定する場合と比較して、計算負荷を軽減することができる。
また、この発明によれば、加速度データ及び角速度データの双方に基づいて機器の姿勢を推定することができるため、加速度データまたは角速度データのいずれか一方に基づいて機器の姿勢を推定する場合と比較して、機器の真の姿勢に近い推定値を算出することができる。
【０００９】
上述した姿勢推定装置は、前記加速度データに基づいて前記機器の動きが安定しているか否かを判定する判定部を備え、前記補正部は、前記判定部の判定結果が肯定の場合に、前記予測部の予測結果を補正する、ことを特徴とすることが好ましい。
この態様によれば、機器の動きが不安定であり、加速度データにノイズが重畳している可能性が高い場合に、加速度データを用いた補正の演算を実行しないことができる。このため、加速度データに重畳するノイズが、機器の姿勢の推定に対して及ぼす影響を小さくすることができる。すなわち、この態様によれば、判定部の判定結果に関わらず補正の演算を実行する場合と比較して、精度の高い姿勢の推定が可能となる。
【００１０】
上述した姿勢推定装置において、前記補正部は、現時刻に前記加速度センサが出力する加速度データの示す値と、前記機器が前記予測部の予測結果の示す姿勢である場合に前記加速度センサが出力する加速度データの示す値との、加重平均値に基づいて、前記予測部の予測結果を補正する、ことを特徴とすることが好ましい。
この態様によれば、加速度データ及び角速度データの双方に基づいて機器の姿勢を推定するため、加速度データまたは角速度データのいずれか一方に基づいて機器の姿勢を推定する場合と比較して、精度の高い姿勢の推定が可能となる。
【００１１】
また、本発明に係る姿勢推定装置の制御プログラムは、３方向の加速度を周期的に検出して検出結果を加速度データとして順次出力する加速度センサと、３方向の角速度を周期的に検出して検出結果を角速度データとして順次出力する角速度センサと、コンピュータと、を備えた機器に設けられた姿勢推定装置の制御プログラムであって、前記コンピュータを、前記角速度データに基づいて、現時刻よりも過去の時刻における前記機器の姿勢から、現時刻における前記機器の姿勢を予測する予測部と、前記加速度データに基づいて、前記予測部の予測結果を補正する補正部と、して機能させる、ことを特徴とする。
【００１２】
この発明によれば、カルマンフィルタ等のように推定値の推定誤差の共分散を算出しつつ姿勢を推定する場合と比較して、計算負荷を抑えつつ、機器の姿勢を推定することができる。また、この発明によれば、加速度データまたは角速度データのいずれか一方に基づいて機器の姿勢を推定する場合と比較して、機器の姿勢を精度良く推定することができる。
【図面の簡単な説明】
【００１３】
【図１】本発明の実施形態に係る携帯機器１の構成を示すブロック図である。
【図２】携帯機器１の外観を示す斜視図である。
【図３】姿勢推定部２を表すブロック図である。
【図４】演算部２０を表すブロック図である。
【図５】補正加速度ベクトルａＣを説明するための説明図である。
【発明を実施するための形態】
【００１４】
以下、本発明を実施するための形態について図面を参照して説明する。ただし、各図において、各部の寸法及び縮尺は、実際のものと適宜に異ならせてある。また、以下に述べる実施の形態は、本発明の好適な具体例であるから、技術的に好ましい種々の限定が付されているが、本発明の範囲は、以下の説明において特に本発明を限定する旨の記載がない限り、これらの形態に限られるものではない。
＜Ａ．実施形態＞
本発明の実施の形態について図面を参照して説明する。
【００１５】
＜１．  機器構成及びソフトウェア構成について＞
図１は、本発明の実施形態に係る携帯機器１（「機器」の一例）のブロック図であり、図２は携帯機器１の外観を示す斜視図である。携帯機器１は、携帯機器１の姿勢に応じて携帯機器１が備える画面（後述する表示部６２）に表示されている地図等の画像を回転させることによって、画像によって表されている方位を現実空間の方位に追従させる機能を備える。この機能は、各種センサの出力に基づいて、携帯機器１の姿勢を推定することによって実現される。
【００１６】
携帯機器１は、各種の構成要素とバスを介して接続され装置全体を制御するＣＰＵ１０、ＣＰＵ１０の作業領域として機能するＲＡＭ（Random Access Memory）５１、姿勢推定プログラム１００等の各種のプログラムやデータを記憶したＲＯＭ（Read Only Memory）５２、通信を実行する通信部６１、画像を表示する表示部６２、及び、ＧＰＳ部６３を備える。また、携帯機器１は、加速度を検出して加速度データａを出力する加速度センサ８０と、角速度を検出して角速度データωを出力する角速度センサ９０を備える。
【００１７】
表示部６２は、ＣＰＵ１０が姿勢推定プログラム１００を実行することにより推定した携帯機器１の姿勢ｑの推定値に基づいて、方位等を表す矢印等の画像を表示する。ＧＰＳ部６３は、ＧＰＳ衛星からの信号を受信して携帯機器１の位置情報（緯度、経度）を生成する。
【００１８】
加速度センサ８０は、Ｘ軸加速度センサ８１、Ｙ軸加速度センサ８２、及び、Ｚ軸加速度センサ８３を備える。加速度センサには、例えば、ピエゾ抵抗型、静電容量型、熱検知型等のような検知方式のセンサがある。加速度センサＩ／Ｆ８４は、各センサからの出力信号をＡＤ変換して加速度データａを単位時間毎に周期的に出力する。加速度データａは、加速度センサ８０と一体となって運動する携帯機器１に固定された座標系における慣性力と重力との合力を、ｘ軸、ｙ軸、及び、ｚ軸の３成分を有するベクトルとして示すデータである。より具体的には、加速度データａは、携帯機器１に固定された座標系において、Ｘ軸加速度センサ８１からの出力値をｘ軸成分で表し、Ｙ軸加速度センサ８２からの出力値をｙ軸成分で表し、Ｚ軸加速度センサ８３からの出力値をｚ軸成分で表す３次元のベクトルデータである。
従って、携帯機器１が静止状態または等速直線運動状態にあれば、加速度データａは、携帯機器１に固定された座標系において重力加速度の大きさと方向とを示す３次元のベクトルデータとなる。
【００１９】
角速度センサ９０は、Ｘ軸角速度センサ９１、Ｙ軸角速度センサ９２、及び、Ｚ軸角速度センサ９３を備える。角速度センサＩ／Ｆ９４は、各センサからの出力信号をＡＤ変換して角速度データωを単位時間毎に周期的に出力する。角速度データωは、携帯機器１に固定された座標系において、３方向に延在する３軸（Ｘ軸、Ｙ軸、Ｚ軸）の周りのそれぞれの角速度を示す３次元のベクトルデータである。
【００２０】
ＣＰＵ１０は、ＲＯＭ５２に格納されている姿勢推定プログラム１００を実行することによって、携帯機器１の姿勢ｑを単位時間毎に周期的に推定する。すなわち、携帯機器１に設けられたＣＰＵ１０は、姿勢推定プログラム１００を実行することにより、姿勢推定部２（「姿勢推定装置」の一例）として機能する。
【００２１】
＜２．  姿勢推定部について＞
図３は、姿勢推定部２の機能を表した機能ブロック図である。姿勢推定部２は、単位時間毎に出力される加速度データａ及び角速度データωに基づいて、単位時間毎に携帯機器１の姿勢ｑを推定する。以下、姿勢推定部２が推定した携帯機器１の姿勢を、推定姿勢ｑＯＵＴと称する。
【００２２】
本実施形態では、携帯機器１の姿勢ｑを、クォータニオンを用いて表現する。クォータニオンとは、物体の姿勢を表す４次元数である。例えば、携帯機器１に固定された座標系の各軸と、地上に固定された座標系の各軸とが一致する姿勢を基準姿勢と定義し、基準姿勢をｑ＝［０、０、０、１］Ｔと表現する。このとき、携帯機器１の任意の姿勢ｑは、地上に固定された座標系における３次元の単位ベクトルρの方向に延在する軸を回転軸として、基準姿勢である携帯機器１を角度ψだけ回転した姿勢として表現できる。すなわち、携帯機器１の姿勢ｑは、クォータニオンを用いて、以下の式（１）で表される。
【数１】
2016109608.tif 000003
【００２３】
図３に示すように、姿勢推定部２は、推定姿勢ｑＯＵＴを推定する演算を実行する演算部２０と、加速度データａに基づいて携帯機器１の動きが安定しているか否かを判定し、判定結果を示す判定情報ＪDを出力する判定部３０と、判定部３０の判定情報ＪDに基づいて、加速度データａを演算部２０に対して供給するか否かを制御するスイッチＳＷと、を備える。
【００２４】
判定部３０は、上述のとおり、加速度データａに基づいて、携帯機器１の動きが安定しているか否か、すなわち、携帯機器１に大きな振動が生じていないか否かを判定する。具体的には、判定部３０は、以下の式（２）に例示する条件、すなわち、「加速度データａの示すベクトルの大きさと重力加速度ベクトルｇの大きさとの差分値の絶対値が、所定の閾値εＡよりも小さい」という条件を充足するか否かを判定する。ここで、重力加速度ベクトルｇとは、地上に固定された座標系において重力加速度の向き及び大きさを示す３次元のベクトルである。
なお、各種変数、ベクトル、行列等を表す符号に付された添え字ｋは、０≦ｋを満たす自然数であり、当該変数、ベクトル、行列等が時刻Ｔｋにおける値であることを意味する。ここで、時刻Ｔｋ＋１は時刻Ｔｋよりも単位時間だけ後の時刻を示し、時刻Ｔｋ−１は時刻Ｔｋよりも単位時間だけ過去の時刻を示すこととする。また、以下では、時刻Ｔｋにおける加速度データａの示す３次元ベクトルを加速度ベクトルａｋと称し、時刻Ｔｋにおける角速度データωの示す３次元ベクトルを角速度ベクトルωｋと称する。また、加速度ベクトルａｋを、加速度センサ８０により観測される観測値と称し、角速度ベクトルωｋを、角速度センサ９０により観測される観測値と称する場合がある。
【数２】
2016109608.tif 000004
【００２５】
判定部３０は、式（２）に示す条件が充足される場合には、携帯機器１の動きが安定していると判定し、判定情報ＪDに対して、携帯機器１の動きが安定していることを示す値、例えば「１」を設定する。また、判定部３０は、式（２）に示す条件が充足されない場合には、携帯機器１の動きが不安定であると判定し、判定情報ＪDに対して、携帯機器１の動きが不安定であることを示す値、例えば「０」を設定する。
【００２６】
スイッチＳＷは、加速度センサ８０と演算部２０との間に設けられ、判定情報ＪDに基づいて、加速度データａを演算部２０に対して供給するか否かを制御する。
具体的には、スイッチＳＷは、判定部３０における判定結果が肯定である場合、すなわち、判定部３０が出力する判定情報ＪDが「１」を示す場合、オンする。この場合、演算部２０は、加速度センサ８０が出力する加速度データａと、角速度センサ９０が出力する角速度データωと、を用いて推定姿勢ｑＯＵＴを算出する。
一方、スイッチＳＷは、判定部３０における判定結果が否定である場合、すなわち、判定部３０が出力する判定情報ＪDが「０」を示す場合、オフする。この場合、演算部２０は、加速度センサ８０が出力する加速度データａを用いずに、角速度センサ９０が出力する角速度データωを用いて推定姿勢ｑＯＵＴを算出する。
以下、演算部２０の動作について詳述する。
【００２７】
＜３．  演算部について＞
図４は、演算部２０の機能ブロック図である。この図に示すように、演算部２０は、姿勢予測部２１（「予測部」の一例）と、姿勢補正部２２（「補正部」の一例）と、推定姿勢出力部２３（「出力部」の一例）と、を備える。
【００２８】
姿勢予測部２１は、時刻Ｔｋ−１において出力される角速度データωの示す角速度ベクトルωｋ−１に基づいて、姿勢推定部２により推定された時刻Ｔｋ−１における携帯機器１の推定姿勢ｑＯＵＴから、時刻Ｔｋにおける携帯機器１の姿勢ｑの予測値（以下、「予測姿勢ｑＧ」と称する）を算出する。
姿勢補正部２２は、時刻Ｔｋにおいて出力される加速度データａの示す加速度ベクトルａｋに基づいて予測姿勢ｑＧを補正することで、時刻Ｔｋにおける携帯機器１の姿勢ｑの予測値の補正値（以下、「補正姿勢ｑＡ」と称する）を算出する。
推定姿勢出力部２３は、姿勢予測部２１が算出した予測姿勢ｑＧ、または、姿勢補正部２２が算出した補正姿勢ｑＡを、時刻Ｔｋにおける携帯機器１の推定姿勢ｑＯＵＴとして出力する。
以下、これら姿勢予測部２１、姿勢補正部２２、及び、推定姿勢出力部２３について詳細に説明する。
【００２９】
上述のとおり、姿勢予測部２１は、角速度ベクトルωｋ−１に基づいて、時刻Ｔｋ−１における携帯機器１の推定姿勢ｑＯＵＴから、予測姿勢ｑＧを予測する演算を実行する。
なお、以下では、図４に示すように、時刻Ｔｋ−１における携帯機器１の推定姿勢ｑＯＵＴが、姿勢予測部２１による時刻Ｔｋにおける予測姿勢ｑＧの算出に用いられる場合には、説明の便宜上、当該推定姿勢ｑＯＵＴを予測前姿勢ｑＧ−ＩＮと称する場合がある。
【００３０】
姿勢予測部２１は、例えば、以下に示す回転運動の方程式（３）に従って、予測前姿勢ｑＧ−ＩＮから予測姿勢ｑＧを算出する。
ここで、式（３）の演算子Ωは、式（４）で定義される。また、式（４）の行列Ｉ３×３は、３行３列の単位行列である。また、式（４）の演算子Γは、３次元ベクトルＬ＝（Ｌ１，Ｌ２，Ｌ３）を用いて、式（５）で定義される。また、符号Δｔは、時刻Ｔｋ−１から時刻Ｔｋまでの単位時間を示す。また、式（４）の成分Ψｋは、式（６）で定義される。
【数３】
2016109608.tif 000005
【００３１】
上述のとおり、姿勢補正部２２は、加速度ベクトルａｋに基づいて、予測姿勢ｑＧを補正し、補正姿勢ｑＡを算出する。
この姿勢補正部２２は、図４に例示するように、予測観測値演算部２２１と、補正観測値演算部２２２と、補正量演算部２２３と、補正姿勢演算部２２４と、を備える。以下、これら姿勢補正部２２の各構成要素について説明する。
【００３２】
予測観測値演算部２２１は、時刻Ｔｋにおける携帯機器１の姿勢が予測姿勢ｑＧである場合に、時刻Ｔｋにおいて加速度センサ８０により観測される観測値である加速度ベクトルａｋを予測する演算を実行する。そして、予測観測値演算部２２１は、当該予測の結果を表す予測加速度ベクトルａＯを出力する。
具体的には、予測観測値演算部２２１は、以下に示す式（７）に従って、予測加速度ベクトルａＯを算出する。ここで、式（７）の行列Ｒ（ｑ）は、地上に固定された座標系から、姿勢ｑである携帯機器１に固定された座標系へと座標変換するための３行３列の行列であり、以下の式（８）で表される。
【数４】
2016109608.tif 000006
【００３３】
上述のとおり、演算部２０には、判定部３０における判定結果が肯定である場合、すなわち、携帯機器１の動きが安定している場合に、加速度ベクトルａｋが供給される。携帯機器１の動きが安定している場合、加速度センサ８０による観測値である加速度ベクトルａｋは、重力加速度を表していると看做すことができる。このため、予測観測値演算部２２１は、式（７）に示すように、時刻Ｔｋにおける携帯機器１の姿勢ｑを予測姿勢ｑＧとしたうえで、重力加速度ベクトルｇを携帯機器１に固定された座標系に座標変換し、当該座標変換により得られたベクトルを、加速度ベクトルａｋの予測値である予測加速度ベクトルａＯとする。
【００３４】
補正観測値演算部２２２は、予測観測値演算部２２１が算出した予測加速度ベクトルａＯと、加速度センサ８０が供給する加速度データａの示す加速度ベクトルａｋとを、加重平均することで、補正加速度ベクトルａＣを生成する。
具体的には、補正観測値演算部２２２は、以下の式（９）に基づいて、補正加速度ベクトルａＣを生成する。なお、式（９）において、係数αＡは、０＜αＡ＜１を満たす実数である。
【数５】
2016109608.tif 000007
【００３５】
図５は、加速度ベクトルａｋ、予測加速度ベクトルａＯ、及び、補正加速度ベクトルａＣの関係について説明するための説明図である。この図に示すように、補正加速度ベクトルａＣは、加速度ベクトルａｋ及び予測加速度ベクトルａＯに垂直な単位ベクトルｄＡに垂直なベクトルであって、加速度ベクトルａｋ及び予測加速度ベクトルａＯの間の方向を向いたベクトルとなる。以下、この図に示すように、予測加速度ベクトルａＯ及び補正加速度ベクトルａＣのなす角度を、角度θＡと称する。
【００３６】
補正量演算部２２３は、補正加速度ベクトルａＣと予測加速度ベクトルａＯとに基づいて、予測姿勢ｑＧを補正するためのクォータニオンである姿勢変化量ＰＡを算出する。具体的には、補正量演算部２２３は、以下に示す式（１０）に基づいて、姿勢変化量ＰＡを算出する。なお、単位ベクトルｄＡは、式（１１）により求めることができる。式（１１）において、「ａＯ×ａＣ」は、予測加速度ベクトルａＯと補正加速度ベクトルａＣとのクロス積を意味する。
【数６】
2016109608.tif 000008
【００３７】
式（１）からも明らかなように、クォータニオンは、姿勢そのものを表すことができる他に、姿勢の変化量を表すことができる。姿勢の変化が、ある軸を中心とした回転である場合に、当該姿勢の変化量（例えば、姿勢変化量ＰＡ）を、軸の延在する方向（例えば、単位ベクトルｄＡ）と、当該軸を中心とする回転の大きさ（例えば、角度θＡ）と、を示すクォータニオンを用いて表すことができる。
すなわち、クォータニオンである姿勢変化量ＰＡは、予測加速度ベクトルａＯを補正加速度ベクトルａＣへと姿勢変化させる場合の、姿勢の変化量を表す。換言すれば、姿勢変化量ＰＡは、携帯機器１の姿勢ｑを、加速度センサ８０の観測値が予測加速度ベクトルａＯとなる予測姿勢ｑＧから、加速度センサ８０の観測値が補正加速度ベクトルａＣとなる補正姿勢ｑＡへと、変化させる場合の、携帯機器１の姿勢の変化量を示すクォータニオンである。
【００３８】
補正姿勢演算部２２４は、予測姿勢ｑＧを、姿勢変化量ＰＡだけ変化させることで、補正姿勢ｑＡを算出する。
なお、以下では、図４に示すように、予測姿勢ｑＧが、補正姿勢演算部２２４による補正姿勢ｑＡの算出に用いられる場合には、説明の便宜上、当該予測姿勢ｑＧを補正前姿勢ｑＡ−ＩＮと称する場合がある。
【００３９】
補正姿勢演算部２２４は、以下の式（１２）に従って、補正前姿勢ｑＡ−ＩＮを姿勢変化量ＰＡだけ変化させる補正を実行することで、補正姿勢ｑＡを算出する。
ここで、演算子［×］は、クォータニオン積である。クォータニオン積とは、２つのクォータニオンのうち、一方のクォータニオンを姿勢と看做し、他方のクォータニオンを姿勢変化量と看做すことで、一方のクォータニオンとして表された姿勢を、他方のクォータニオンとして表された姿勢変化量だけ姿勢変化させる演算である。
【数７】
2016109608.tif 000009
【００４０】
以上において説明したように、姿勢補正部２２は、角速度センサ９０からの出力値である角速度ベクトルωｋ−１に応じて定められる予測加速度ベクトルａＯと、加速度センサ８０からの出力値である加速度ベクトルａｋとの、加重平均として補正加速度ベクトルａＣを算出する（式（９）参照）。そして、予測加速度ベクトルａＯを、予測加速度ベクトルａＯ及び加速度ベクトルａｋの間の方向を向く補正加速度ベクトルａＣへと姿勢変化させる場合の姿勢変化量ＰＡを求め、当該姿勢変化量ＰＡにより予測姿勢ｑＧを補正することで、補正姿勢ｑＡを算出する。すなわち、本実施形態において、補正姿勢ｑＡは、角速度センサ９０が出力する角速度データωと、加速度センサ８０が出力する加速度データａとの双方に基づいて定められる。
ところで、仮に、式（９）において、係数αＡを「１」とする場合、補正加速度ベクトルａＣは、加速度ベクトルａｋとなる。この場合、補正姿勢ｑＡは、角速度データωの影響を受けることなく、加速度データａのみに基づいて定められる。そして、この場合、補正姿勢ｑＡは、加速度データａに重畳するノイズや、加速度センサ８０における測定誤差の影響により、真の姿勢から乖離した姿勢となることがある。
また、仮に、式（９）において、係数αＡを「０」とする場合、補正加速度ベクトルａＣは、予測加速度ベクトルａＯとなる。この場合、補正姿勢ｑＡは、加速度ベクトルａｋの影響を受けることなく、角速度データωに基づいて定められる。そして、この場合、補正姿勢ｑＡは、角速度データωに重畳するノイズや、角速度センサ９０における測定誤差の影響により、真の姿勢から乖離した姿勢となる事がある。
これに対して本実施形態では、式（９）に示すように、補正加速度ベクトルａＣを、予測加速度ベクトルａＯと加速度ベクトルａｋとの加重平均として算出し、補正姿勢ｑＡを、加速度データａ及び角速度データωの双方に基づいて定める。このため、本実施形態では、補正姿勢ｑＡを、加速度データａまたは角速度データωの何れか一方に基づいて定める場合と比較して、加速度データａ及び角速度データωに重畳するノイズの影響や、加速度センサ８０及び角速度センサ９０における測定誤差の影響を小さく抑えることができる。これにより、本実施形態では、真の姿勢に近い正確な補正姿勢ｑＡの算出が可能となる。
【００４１】
推定姿勢出力部２３は、上述のとおり、予測姿勢ｑＧまたは補正姿勢ｑＡを、時刻Ｔｋにおける携帯機器１の推定姿勢ｑＯＵＴとして出力する。
より具体的には、推定姿勢出力部２３は、判定情報ＪDが「１」を示す場合、すなわち、判定部３０における判定結果が肯定である場合には、姿勢補正部２２が予測姿勢ｑＧを補正して補正姿勢ｑＡを算出するため、当該補正姿勢ｑＡを、時刻Ｔｋにおける携帯機器１の推定姿勢ｑＯＵＴとして出力する。一方、推定姿勢出力部２３は、判定情報ＪDが「０」を示す場合、すなわち、判定部３０における判定結果が否定である場合には、姿勢補正部２２による予測姿勢ｑＧに対する補正が実行されないため、姿勢予測部２１により算出された予測姿勢ｑＧを、時刻Ｔｋにおける携帯機器１の推定姿勢ｑＯＵＴとして出力する。
【００４２】
このように、姿勢推定部２は、単位時間毎に角速度センサ９０が出力する角速度データωに基づいて、携帯機器１の姿勢ｑの予測値である予測姿勢ｑＧを生成する。また、姿勢推定部２は、単位時間毎に加速度センサ８０が出力する加速度データａの示す値が安定している場合には、当該加速度データａに基づいて、予測姿勢ｑＧを補正することで補正姿勢ｑＡを算出する。そして、姿勢推定部２は、予測姿勢ｑＧが補正姿勢ｑＡへと補正された場合には、補正姿勢ｑＡを携帯機器１の推定姿勢ｑＯＵＴとして出力し、また、予測姿勢ｑＧが補正されない場合には、予測姿勢ｑＧを携帯機器１の推定姿勢ｑＯＵＴとして出力する。
このように、本実施形態に係る姿勢推定部２は、角速度センサ９０の出力する角速度データωと、加速度センサ８０の出力する加速度データａとの双方に基づいて、時刻Ｔｋ−１における推定姿勢ｑＯＵＴから、時刻Ｔｋにおける推定姿勢ｑＯＵＴを算出することで、携帯機器１の姿勢ｑを単位時間毎に推定する。
【００４３】
＜４．  実施形態の結論＞
以上において説明したように、本実施形態に係る姿勢推定部２は、角速度センサ９０が出力する角速度データωに基づいて算出された予測姿勢ｑＧを、加速度センサ８０が出力する加速度データａに基づいて補正することで、補正姿勢ｑＡを算出する。すなわち、本実施形態では、角速度データωと、加速度データａとの双方に基づいて、携帯機器１の姿勢ｑを推定する。
【００４４】
ところで、一般的に、加速度データａには、携帯機器１の姿勢ｑの推定のために加速度センサ８０が検出すべき重力加速度の他に、携帯機器１の振動に起因する加速度がノイズとして重畳する可能性が高い。他方、角速度データωは、一般的に、携帯機器１の姿勢ｑの推定のために角速度センサ９０が検出すべき携帯機器１の姿勢変化のみを検出するため、加速度データａと比較してノイズが重畳する可能性が低い。これらのことから、携帯機器１の姿勢ｑの推定において、信頼性の高いデータのみを使用する態様、すなわち、角速度データωに基づいて算出された予測姿勢ｑＧを推定姿勢ｑＯＵＴとして出力し、加速度データａに基づく予測姿勢ｑＧの補正を実行しない態様（以下、「対比例」と称する）も想定されうる。
【００４５】
しかし、時刻Ｔｋにおける携帯機器１の姿勢ｑの予測結果である予測姿勢ｑＧは、時刻Ｔｋ−１における携帯機器１の推定姿勢ｑＯＵＴを、時刻Ｔｋ−１から時刻Ｔｋまでの単位時間における携帯機器１の姿勢変化量だけ変化させた姿勢である（式（３）参照）。つまり、姿勢予測部２１が実行する携帯機器１の姿勢ｑを予測する演算（以下、「予測演算」と称する）は、単位時間における携帯機器１の姿勢ｑの相対的な変化量を求める演算に過ぎない。
よって、対比例においては、例えば、初期状態（時刻Ｔ０）における携帯機器１の推定姿勢ｑＯＵＴと、時刻Ｔ０における携帯機器１の真の姿勢ｑとの間に誤差が存在する場合、姿勢予測部２１による予測演算を繰り返し実行しても、当該誤差は解消されない。
また、対比例において、時刻Ｔｋにおける携帯機器１の予測姿勢ｑＧは、初期状態（時刻Ｔ０）から時刻Ｔｋまでの間に姿勢予測部２１が実行したｋ回の予測演算において得られるｋ個の姿勢変化量Ω（ω０）、Ω（ω１）、…、Ω（ωｋ−１）の合計値に基づいて算出される。このため、仮に、角速度センサ９０の信頼性が高く、角速度データωに重畳するノイズが小さい場合であっても、時刻Ｔ０と時刻Ｔｋとの時間長が長くなり、予測演算の実行回数が増加するに従って、角速度データωに重畳するノイズの予測姿勢ｑＧに対する影響も蓄積する。つまり、時刻Ｔ０と現時刻Ｔｋとの時間長が長くなるに従って、携帯機器１の予測姿勢ｑＧと携帯機器１の真の姿勢ｑとの誤差も蓄積し増大する。
【００４６】
これに対して、本実施形態に係る姿勢推定部２は、判定部３０における判定結果が肯定である場合、すなわち、加速度ベクトルａｋが重力加速度を表していると看做せる場合に、加速度ベクトルａｋにより予測姿勢ｑＧを補正する。
携帯機器１の姿勢ｑが変化している場合、時間の経過と共に、携帯機器１に固定された座標系の向き（Ｘ軸、Ｙ軸、Ｚ軸の向き）も変化する。この場合、時刻Ｔｋ−１における携帯機器１に固定された座標系は、時刻Ｔｋにおける携帯機器１に固定された座標系とは異なる座標系となる。よって、携帯機器１の姿勢ｑが変化している場合、ある時刻における携帯機器１に固定された座標系から見た、当該ある時刻における携帯機器１の姿勢と、当該ある時刻における携帯機器１に固定された座標系から見た、当該ある時刻とは異なる時刻における携帯機器１の姿勢とは、異なる姿勢となる場合がある。このような、ある時刻における携帯機器１に固定された座標系から見た、携帯機器１の姿勢は、角速度ベクトルωｋに基づいて表される。具体的には、角速度ベクトルωｋ−１は、時刻Ｔｋ−１における携帯機器１に固定された座標系から見た時刻Ｔｋにおける携帯機器１の謂わば相対的な姿勢を示す（式（３）参照）。
これに対して、加速度ベクトルａｋは、地上に固定された座標系から見た携帯機器１の謂わば絶対的な姿勢を示す。このため、本実施形態のように、加速度ベクトルａｋを用いて予測姿勢ｑＧに対する補正を実行することで、初期状態において生じていた推定姿勢ｑＯＵＴの誤差を小さくすることができ、また、予測演算を繰り返すことで蓄積される誤差を小さくすることができる。これにより、本実施形態にかかる姿勢推定部２は、携帯機器１の姿勢ｑを正確に推定することができる。
【００４７】
ところで、上述のとおり、演算部２０が備える姿勢補正部２２は、加速度ベクトルａｋと、重力加速度ベクトルｇを携帯機器１に固定された座標系において表したベクトルとが、略同じであることを前提として予測姿勢ｑＧを補正する。しかし、携帯機器１が振動する等、携帯機器１の動きが不安定な場合、加速度ベクトルａｋには、重力加速度以外の加速度がノイズとして重畳する。この場合、加速度ベクトルａｋと、重力加速度ベクトルｇを携帯機器１に固定された座標系において表したベクトルとを、略同じと看做すことはできない。よって、携帯機器１の動きが不安定である場合に、加速度ベクトルａｋを用いて予測姿勢ｑＧを補正して得られる補正姿勢ｑＡは、携帯機器１の真の姿勢から大きく乖離している可能性が高い。つまり、この場合、姿勢推定部２が、補正姿勢ｑＡを推定姿勢ｑＯＵＴとして出力すると、姿勢推定部２は、携帯機器１の真の姿勢から大きく乖離した推定姿勢ｑＯＵＴを出力することとなる。
これに対して、本実施形態に係る姿勢推定部２は、判定部３０における判定結果が肯定で、携帯機器１の動きが安定している場合にのみ、加速度データａを演算部２０に供給し、加速度ベクトルａｋによる予測姿勢ｑＧに対する補正を実行する。このため、本実施形態に係る姿勢推定部２では、携帯機器１の真の姿勢から大きく乖離した推定姿勢ｑＯＵＴが算出されることを防止することができる。
【００４８】
このように、本実施形態に係る姿勢推定部２は、角速度センサ９０の出力する角速度データωと、加速度センサ８０の出力する加速度データａとの双方に基づいて推定姿勢ｑＯＵＴを算出するため、角速度データωまたは加速度データａの一方のみに基づいて推定姿勢ｑＯＵＴを算出する場合と比較して、携帯機器１の真の姿勢により近い推定姿勢ｑＯＵＴを算出することができる。
更に、本実施形態にかかる姿勢推定部２は、カルマンフィルタによる演算のように、姿勢ｑの推定値の推定誤差の共分散を単位時間毎に演算する必要が無いため、カルマンフィルタを用いて携帯機器１の姿勢ｑを推定する場合と比較して、処理負荷を軽減することができる。
【００４９】
＜Ｂ．変形例＞
以上の実施形態は多様に変形され得る。具体的な変形の態様を以下に例示する。以下の例示から任意に選択された２以上の態様は、相互に矛盾しない範囲内で適宜に併合され得る。
なお、以下に例示する変形例において作用や機能が実施形態と同等である要素については、以上の説明で参照した符号を流用して各々の詳細な説明を適宜に省略する。
【００５０】
＜変形例１＞
上述した実施形態に係る判定部３０は、上述した式（２）に例示したように、加速度ベクトルａｋの大きさと重力加速度ベクトルｇの大きさとの差分が、閾値εＡよりも小さい否かを判定するが、本発明はこのような形態に限定されるものでは無く、判定部３０は、携帯機器１の動きが安定しているか否かを判定することができれば、どのような構成であってもよい。
例えば、判定部３０は、加速度データａの有する加速度ベクトルａｋの各成分について、ＨＰＦ（High-pass filter）を透過させた場合に、ＨＰＦの出力値が所定値よりも小さいか否かを判定するものであってもよい。この場合、携帯機器１に振動が生じていれば、ＨＰＦの出力値が所定値以上となるため、携帯機器１の動きが安定しているか否かを判定することができる。
【００５１】
＜変形例２＞
上述した実施形態及び変形例において、姿勢推定部２はスイッチＳＷを備えるが、本発明はこのような形態に限定されるものでは無く、姿勢推定部２はスイッチＳＷを備えない構成であってもよい。
【００５２】
姿勢推定部２がスイッチＳＷを備えない場合、判定部３０の判定結果に関わらず、加速度ベクトルａｋを演算部２０に供給してもよい。この場合、姿勢補正部２２は、判定部３０の判定結果に関わらず、予測姿勢ｑＧを加速度ベクトルａｋにより補正して補正姿勢ｑＡを算出してもよいし、また、推定姿勢出力部２３は、判定部３０の判定結果に関わらず、補正姿勢ｑＡを推定姿勢ｑＯＵＴとして出力してもよい。
【００５３】
なお、姿勢推定部２がスイッチＳＷを供えない場合、上述した式（９）に示す、補正加速度ベクトルａＣを算出する加重平均の演算において、加速度ベクトルａｋに付される重みである係数αＡを、判定部３０の判定結果に基づいて変化させてもよい。例えば、判定部３０の判定結果が肯定であり携帯機器１の動作が安定であると看做せる場合には、式（９）に示すとおり係数αＡを用いて補正加速度ベクトルａＣを算出し、判定部３０の判定結果が否定であり携帯機器１の動作が不安定である場合には、式（９）において、係数αＡの代わりに、０≦αＢ＜αＡ＜１を満たす実数である係数αＢを用いて、補正加速度ベクトルａＣを算出してもよい。
携帯機器１の動作が不安定であり、加速度ベクトルａｋが重力加速度を示す成分以外のノイズ成分を含む場合、加速度ベクトルａｋに基づいて算出される補正姿勢ｑＡも、加速度ベクトルａｋに含まれるノイズの影響を受ける。この場合、補正姿勢ｑＡが、携帯機器１の真の姿勢から大きく乖離したものとして算出される。
本変形例では、判定部３０の判定結果が否定である場合、式（９）に示す補正加速度ベクトルａＣを算出するための加重平均の演算において、加速度ベクトルａｋに対応する重みを、係数αＡの代わりに、係数αＡよりも小さい係数αＢとするため、加速度ベクトルａｋに対応する重みを係数αＡとする場合と比較して、加速度ベクトルａｋの有するノイズの補正姿勢ｑＡに対する影響を小さくできる。つまり、本変形例では、式（９）において加速度ベクトルａｋに対応する重みを係数αＡとする場合と比較して、予測加速度ベクトルａＯに対応する重みである係数（１−αＢ）を大きくすることができる。このため、携帯機器１の動作が不安定であっても、携帯機器１の真の姿勢に近い補正姿勢ｑＡを算出することができる。
【００５４】
また、上述した実施形態及び変形例において、姿勢推定部２は判定部３０を備えるが、本発明はこのような形態に限定されるものでは無く、姿勢推定部２は判定部３０を備えない構成であってもよい。この場合、姿勢推定部２はスイッチＳＷを備えない構成であってもよい。
姿勢推定部２が判定部３０を備えない場合、加速度センサ８０の出力する加速度ベクトルａｋが、常に演算部２０に供給されることとしてもよい。この場合、演算部２０は、加速度ベクトルａｋに基づいて予測姿勢ｑＧを補正して得られる補正姿勢ｑＡを、推定姿勢ｑＯＵＴとして出力すればよい。
なお、姿勢推定部２が判定部３０を供えない場合、式（９）における係数αＡの代わりに、０＜αＣ＜αＡ＜１を満たす実数である係数αＣを用いて、補正加速度ベクトルａＣを算出してもよい。この場合、予測加速度ベクトルａＯに対応する重みである係数（１−αＣ）を大きくすることができる。このため、携帯機器１の動作が不安定であっても、携帯機器１の真の姿勢に近い補正姿勢ｑＡを算出することができる。
【００５５】
＜変形例３＞
上述した実施形態及び変形例において、携帯機器１は、加速度センサ８０及び角速度センサ９０を備えるが、本発明はこのような形態に限定されるものでは無く、携帯機器１は、これらに加えて、３方向の磁気を周期的に検出して検出結果を出力する磁気センサを備えるものであってもよい。
この場合、姿勢推定部２は、上述した実施形態及び変形例のように、加速度センサ８０及び角速度センサ９０からの出力結果に基づいて、鉛直方向（重力加速度ベクトルｇの延在方向）からの携帯機器１の傾きを推定するのに加え、磁気センサ及び角速度センサ９０からの出力結果に基づいて、磁極北に向かう方向からの携帯機器１の傾きを推定するものであってもよい。
【００５６】
＜変形例４＞
上述した実施形態及び変形例において、加速度センサ８０及び角速度センサ９０等のセンサは、単位時間毎に周期的に検出した観測値を出力するが、本発明はこのような形態に限定されるものでは無く、任意のタイミングで観測値を出力するものであればよい。この場合、姿勢予測部２１は、角速度データωが出力されるタイミングで、予測姿勢ｑＧを生成するための演算を実行すればよく、姿勢補正部２２は、加速度データａが出力されるタイミングで、補正姿勢ｑＡを生成するための演算を実行すればよい。すなわち、演算部２０は、角速度データωに基づいて予測姿勢ｑＧを生成する姿勢予測部２１の処理と、加速度データａに基づいて補正姿勢ｑＡを生成する姿勢補正部２２の処理とを、独立して、非同期で実行してもよい。
更に、姿勢予測部２１と姿勢補正部２２とが非同期で動作する場合、姿勢補正部２２は、予測姿勢ｑＧの代わりに推定姿勢ｑＯＵＴに基づいて、補正姿勢ｑＡを生成してもよい。すなわち、演算部２０は、推定姿勢ｑＯＵＴを予測前姿勢ｑＧ−ＩＮとして当該予測前姿勢ｑＧ−ＩＮに基づいて予測姿勢ｑＧを生成する姿勢予測部２１における処理と、推定姿勢ｑＯＵＴを補正前姿勢ｑＡ−ＩＮとして当該補正前姿勢ｑＡ−ＩＮに基づいて補正姿勢ｑＡを生成する姿勢補正部２２における処理とを、互いに排他的なタイミングで且つ互いに独立して実行するものであってもよい。
【符号の説明】
【００５７】
１…携帯機器、２…姿勢推定部、１０…ＣＰＵ、２０…演算部、２１…姿勢予測部、２２…姿勢補正部、２３…推定姿勢出力部、２２１…予測観測値演算部、２２２…補正観測値演算部、２２３…補正量演算部、２２４…補正姿勢演算部、３０…判定部、８０…加速度センサ、９０…角速度センサ。
【図１】
2016109608.tif 000010
【図２】
2016109608.tif 000011
【図３】
2016109608.tif 000012
【図４】
2016109608.tif 000013
【図５】
2016109608.tif 000014