特許 6705972 姿勢推定装置、姿勢推定方法、制御プログラム、および記録媒体

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6705972

(24)【登録日】2020年5月19日

(45)【発行日】2020年6月3日

(54)【発明の名称】姿勢推定装置、姿勢推定方法、制御プログラム、および記録媒体

(51)【国際特許分類】

   G01C 15/00 20060101AFI20200525BHJP

   G01C 21/16 20060101ALI20200525BHJP

   G01C 21/26 20060101ALI20200525BHJP

【ＦＩ】

   G01C15/00 101

   G01C21/16

   G01C21/26 P

【請求項の数】7

【全頁数】15

(21)【出願番号】特願2016-101925(P2016-101925)

(22)【出願日】2016年5月20日

(65)【公開番号】特開2017-207456(P2017-207456A)

(43)【公開日】2017年11月24日

【審査請求日】2019年5月15日

(73)【特許権者】

【識別番号】512165628

【氏名又は名称】サイトセンシング株式会社

(73)【特許権者】

【識別番号】506132522

【氏名又は名称】株式会社ＮＳＤ

(74)【代理人】

【識別番号】110000338

【氏名又は名称】特許業務法人ＨＡＲＡＫＥＮＺＯ ＷＯＲＬＤ ＰＡＴＥＮＴ ＆ ＴＲＡＤＥＭＡＲＫ

(72)【発明者】

【氏名】興梠 正克

【審査官】 櫻井 仁

(56)【参考文献】

【文献】 国際公開第２０１０／００１９７０（ＷＯ，Ａ１）

【文献】 特開２００４−２６４０２８（ＪＰ，Ａ）

【文献】 米国特許出願公開第２００５／０２４０３４７（ＵＳ，Ａ１）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０１Ｃ １５／００

Ｇ０１Ｃ ２１／００−２１／３６

(57)【特許請求の範囲】

【請求項1】

加速度センサ、角速度センサ、および磁気センサの出力を用いてセンサ座標系における移動体の状態ベクトルを逐次更新し、該状態ベクトルを用いて上記移動体の姿勢をトラッキングする姿勢推定装置であって、
上記角速度センサの出力を用いて上記状態ベクトルの成分である重力方向ベクトルの予測値を算出し、該予測値を上記加速度センサの出力を観測として用いて更新する重力方向推定部と、
上記角速度センサの出力を用いて上記状態ベクトルの成分である水平方向ベクトルの予測値を算出し、該予測値を上記磁気センサの出力を観測として用いて更新する水平方向推定部と、
更新後の上記重力方向ベクトルと更新後の上記水平方向ベクトルとを用いて上記移動体の姿勢情報を生成する姿勢推定部と、を備えていることを特徴とする姿勢推定装置。

【請求項2】

上記水平方向推定部は、上記磁気センサの出力が上記水平方向ベクトルの更新に用いるのに適切な値となっているか否かを検査し、適切な値となっていない場合には該磁気センサの出力を観測として用いた更新を行わないことを特徴とする請求項１に記載の姿勢推定装置。

【請求項3】

上記水平方向推定部は、上記重力方向推定部が更新した上記重力方向ベクトルを用いて上記検査を行うことを特徴とする請求項２に記載の姿勢推定装置。

【請求項4】

上記水平方向推定部は、更新後の上記水平方向ベクトルを、更新後の上記重力方向ベクトルと直交するように補正することを特徴とする請求項１から３の何れか１項に記載の姿勢推定装置。

【請求項5】

加速度センサ、角速度センサ、および磁気センサの出力を用いてセンサ座標系における移動体の状態ベクトルを逐次更新し、該状態ベクトルを用いて上記移動体の姿勢をトラッキングする姿勢推定装置による姿勢推定方法であって、
上記角速度センサの出力を用いて上記状態ベクトルの成分である重力方向ベクトルの予測値を算出し、該予測値を上記加速度センサの出力を観測として用いて更新する重力方向推定ステップと、
上記角速度センサの出力を用いて上記状態ベクトルの成分である水平方向ベクトルの予測値を算出し、該予測値を上記磁気センサの出力を観測として用いて更新する水平方向推定ステップと、
更新後の上記重力方向ベクトルと更新後の上記水平方向ベクトルとを用いて上記移動体の姿勢情報を生成する姿勢推定ステップと、を含むことを特徴とする姿勢推定方法。

【請求項6】

請求項１から４の何れか１項に記載の姿勢推定装置としてコンピュータを機能させるための制御プログラムであって、上記重力方向推定部、上記水平方向推定部、および上記姿勢推定部としてコンピュータを機能させるための制御プログラム。

【請求項7】

請求項６に記載の制御プログラムを記録したコンピュータ読み取り可能な記録媒体。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、移動体に保持された加速度センサ、角速度センサ、および磁気センサの出力を用いて移動体の姿勢をトラッキングする装置等に関する。

【背景技術】

【0002】

人の腰部やつま先などに装着された自蔵センサ（加速度・ジャイロ・磁気・気圧センサなど）を用いて、歩行動作に特化して、その位置・方位を計測する技術は、歩行者デッドレコニング（PDR：Pedestrian Dead Reckoning）と呼ばれている。下記非特許文献１に記載されているように、このような技術については従来から研究が進められている。そして、このような技術においては、カルマンフィルタを用いて移動体の状態ベクトルを逐次更新することにより、リアルタイムのトラッキングが実現されている。

【先行技術文献】

【非特許文献】

【0003】

【非特許文献1】興梠正克、大隈隆史、蔵田武志、「歩行者ナビのための自蔵センサモジュールを用いた屋内測位システムとその評価」、シンポジウム「モバイル 08」予稿集、pp.151-156,2008

【非特許文献2】Paul Zarchan, Howard Musoff, "Fundamentals of Kalman Filtering: A Practical Approach - Second Edition", AIAA, 2004.

【発明の概要】

【発明が解決しようとする課題】

【0004】

しかしながら、上述のような従来技術には、カルマンフィルタによる演算のコストが大きいという問題がある。すなわち、上記非特許文献１の技術では、重力方向と水平基準方向のトラッキングをカルマンフィルタにより実現している。そして、このカルマンフィルタの状態ベクトルは、真北方向成分（３軸）、鉛直方向成分（３軸）、角速度成分（３軸）の計９つの成分を持つ９×１行列である。このため、状態ベクトルの更新には、９×９の行列の乗算と逆行列の演算が必要となり、この演算のコストは大きいものであった。なお、これは、歩行者デッドレコニングに限られず、任意の移動体の姿勢のトラッキング時に共通して生じる問題である。

【0005】

本発明は、前記の問題点に鑑みてなされたものであり、その目的は、姿勢の推定精度を低下させることなく状態ベクトルの更新に係る演算コストを低減することのできる姿勢推定装置等を実現することにある。

【課題を解決するための手段】

【0006】

上記の課題を解決するために、本発明の姿勢推定装置は、加速度センサ、角速度センサ、および磁気センサの出力を用いてセンサ座標系における移動体の状態ベクトルを逐次更新し、該状態ベクトルを用いて上記移動体の姿勢をトラッキングする姿勢推定装置であって、上記角速度センサの出力を用いて上記状態ベクトルの成分である重力方向ベクトルの予測値を算出し、該予測値を上記加速度センサの出力を観測として用いて更新する重力方向推定部と、上記角速度センサの出力を用いて上記状態ベクトルの成分である水平方向ベクトルの予測値を算出し、該予測値を上記磁気センサの出力を観測として用いて更新する水平方向推定部と、更新後の上記重力方向ベクトルと更新後の上記水平方向ベクトルとを用いて上記移動体の姿勢情報を生成する姿勢推定部と、を備えている。

【0007】

上記の構成によれば、重力方向ベクトルと水平方向ベクトルとをそれぞれ個別に算出および更新し、更新後の重力方向ベクトルと更新後の水平方向ベクトルとを用いて移動体の姿勢情報を生成する。これにより、重力方向ベクトルと水平方向ベクトルとを含む状態ベクトルを一括で更新する場合と同等の推定精度を維持しつつ、該場合と比べて更新に係る演算コストを低減することができる。

【0008】

また、上記水平方向推定部は、上記磁気センサの出力が上記水平方向ベクトルの更新に用いるのに適切な値となっているか否かを検査し、適切な値となっていない場合には該磁気センサの出力を観測として用いた更新を行わなくてもよい。

【0009】

上記の構成によれば、検査を行った結果、磁気センサの出力が水平方向ベクトルの更新に用いるのに適切な値となっていない場合には、気センサの出力を観測として用いた更新を行わない。よって、不適切な値の磁気センサの出力を用いた更新が行われて、姿勢の推定精度が低下することを防ぐことができる。

【0010】

また、上記水平方向推定部は、上記重力方向推定部が更新した上記重力方向ベクトルを用いて上記検査を行ってもよい。

【0011】

上記の構成によれば、重力方向推定部が更新した重力方向ベクトルを用いて上記検査を行う。この重力方向ベクトルは、角速度センサの出力を用いた予測により算出され、加速度センサの出力を観測として用いて更新されたものであるから、重力方向を正しく示している可能性が高い。また、磁気センサの出力である磁気ベクトルが外乱や歪みを含んでいなければ、該磁気ベクトルと重力方向との間には所定の関係が成り立つ。よって、該関係が成り立つ場合には磁気センサの出力が適切な値であり、成り立たない場合には、不適切な値であると言える。したがって、上記の構成によれば、重力方向推定部が更新した重力方向ベクトルを利用して、高精度な検査を行うことが可能になる。

【0012】

また、上記水平方向推定部は、更新後の上記水平方向ベクトルを、更新後の上記重力方向ベクトルと直交するように補正してもよい。

【0013】

ここで、水平方向ベクトルと重力方向ベクトルは、何れも同じセンサ座標系におけるベクトルであるから、理論上は直交する。しかし、実際に演算にて更新した水平方向ベクトルと重力方向ベクトルは直交しない場合があり、直交しない状態のまま姿勢の推定を行うと、その推定精度が低下してしまう。そこで、上記の構成によれば、更新後の水平方向ベクトルを、更新後の重力方向ベクトルと直交するように補正する。これにより、姿勢の推定精度が低下することを防ぐことができる。

【0014】

また、本発明の姿勢推定方法は、上記の課題を解決するために、加速度センサ、角速度センサ、および磁気センサの出力を用いてセンサ座標系における移動体の状態ベクトルを逐次更新し、該状態ベクトルを用いて上記移動体の姿勢をトラッキングする姿勢推定装置による姿勢推定方法であって、上記角速度センサの出力を用いて上記状態ベクトルの成分である重力方向ベクトルの予測値を算出し、該予測値を上記加速度センサの出力を観測として用いて更新する重力方向推定ステップと、上記角速度センサの出力を用いて上記状態ベクトルの成分である水平方向ベクトルの予測値を算出し、該予測値を上記磁気センサの出力を観測として用いて更新する水平方向推定ステップと、更新後の上記重力方向ベクトルと更新後の上記水平方向ベクトルとを用いて上記移動体の姿勢情報を生成する姿勢推定ステップと、を含む。該方法によれば、上記姿勢推定装置と同様の作用効果を奏する。

【0015】

本発明の各態様に係る姿勢推定装置は、コンピュータによって実現してもよく、この場合には、コンピュータを上記姿勢推定装置が備える各部（ソフトウェア要素）として動作させることにより上記姿勢推定装置をコンピュータにて実現させる姿勢推定装置の制御プログラム、およびそれを記録したコンピュータ読み取り可能な記録媒体も、本発明の範疇に入る。

【発明の効果】

【0016】

本発明によれば、姿勢の推定精度を低下させることなく状態ベクトルの更新に係る演算コストを低下させることができるという効果を奏する。

【図面の簡単な説明】

【0017】

【図1】本発明の実施形態１に係る姿勢推定装置の要部構成の一例を示すブロック図である。

【図2】重力方向推定部が実行する処理の一例を示すフローチャートである。

【図3】水平方向推定部が実行する処理の一例を示すフローチャートである。

【発明を実施するための形態】

【0018】

〔実施形態１〕
以下、本発明の一実施の形態について図１から図３に基づいて説明する。図１は、本実施形態の姿勢推定装置１の要部構成の一例を示すブロック図である。姿勢推定装置１は、姿勢の推定対象となる移動体に保持され、該移動体の姿勢をトラッキングする装置である。図示のように、姿勢推定装置１は、制御部１０、記憶部１１、出力部１２、加速度センサ１３、タイミングデバイス１４、角速度センサ１５、および磁気センサ１６を備えている。

【0019】

制御部１０は、姿勢推定装置１の各部を統括して制御するものであり、重力方向推定部１００、水平方向推定部１０１、および姿勢推定部１０２を含む。また、記憶部１１は、姿勢推定装置１が使用する各種データを記憶するものであり、出力部１２は姿勢推定部１０２が推定した姿勢を示す姿勢情報を出力するものである。出力部１２の出力態様は特に限定されず、表示出力であってもよい。この場合、出力部１２は表示装置ということになる。また、他の装置（例えば姿勢情報を用いて移動体の位置を算出する測位装置）への出力であってもよく、この場合、出力部１２は通信装置あるいは通信インターフェースということになる。

【0020】

加速度センサ１３は、センサ座標系における３軸方向の加速度ベクトルを計測して制御部１０に入力するセンサデバイスである。加速度センサ１３が制御部１０に入力する加速度ベクトル（加速度センサ１３の出力）は下記のように表される。

【0021】

【数1】

【0022】

なお、上記センサ座標系とは、加速度センサ１３の出力する加速度ベクトルの３軸方向で規定される座標系である。ここでは、加速度センサ１３、角速度センサ１５、および磁気センサ１６は、同一のセンサ座標系でセンサデータを計測できるか、あるいは計測できるとみなすことができるものとする。また、仮に各センサが異なる座標系上に配置されていたとしても、一つの仮想的なセンサ座標系での計測結果が制御部１０に出力されることを前提とする。

【0023】

タイミングデバイス１４は、センサデータの取得タイミングを計測するデバイスである。具体的には、タイミングデバイス１４は、加速度センサ１３による加速度ベクトルの取得、角速度センサ１５による角速度ベクトルの取得、および磁気センサ１６による磁気ベクトルの取得が行われたタイミングを検出する。そして、タイミングデバイス１４は、センサデータの取得時間間隔Δｔを示すタイミングデータを制御部１０に入力する。Δｔは、時間的に連続して取得されたサンプル（すなわちセンサデータ）間の離散時刻差であるとも言える。なお、タイミングデバイス１４は、上記Δｔを算出可能なタイミングデータを出力するものであればよく、例えば絶対時刻を計測・出力するデバイスであってもよい。なお、各センサにおいてセンサデータが同時に取得されることは必須ではない。

【0024】

角速度センサ１５は、センサ座標系における３軸方向の角速度ベクトルを計測して制御部１０に入力するセンサデバイスである。角速度センサ１５が制御部１０に入力する角速度ベクトル（角速度センサ１５の出力）は下記のように表される。

【0025】

【数2】

【0026】

磁気センサ１６は、センサ座標系における３軸方向の磁気ベクトルを計測して制御部１０に入力するセンサデバイスである。磁気センサ１６が制御部１０に入力する磁気ベクトル（磁気センサ１６の出力）は下記のように表される。

【0027】

【数3】

【0028】

重力方向推定部１００は、センサ座標系における移動体の状態ベクトルを逐次更新する。より詳細には、重力方向推定部１００は、センサ座標系における重力方向ベクトルを状態データ（状態ベクトル）として保持し、更新（推定）し、出力する。重力方向ベクトルは、鉛直方向ベクトルと表現することもできる。詳細は後述するが、重力方向推定部１００は、センサ座標系におけるセンサデータ（３軸加速度ベクトルおよび３軸角速度ベクトル）と、センサデータの取得時間間隔Δtを入力として重力方向ベクトルを推定する。

【0029】

重力方向推定部１００は、内部の状態データとして、少なくとも状態ベクトル（具体的には重力方向ベクトル）と、状態ベクトルの誤差分散共分散行列Ｐ_ｇを保持している。本例では重力方向ベクトルが下記のように３×１サイズの行列であるから、誤差分散共分散行列は、３×３サイズの正方行列となる。重力方向推定部１００は、カルマンフィルタの枠組みに従ってこれらの状態データを処理する。上記重力方向ベクトルは下記のように表される。

【0030】

【数4】

【0031】

水平方向推定部１０１は、センサ座標系における移動体の状態ベクトルを逐次更新する。より詳細には、水平方向推定部１０１は、センサ座標系における水平方向ベクトルを状態データ（状態ベクトル）として保持し、更新（推定）し、出力する。詳細は後述するが、水平方向推定部１０１は、センサ座標系におけるセンサデータ（３軸加速度ベクトル、３軸角速度ベクトル、および３軸磁気ベクトル）と、センサデータの取得時間間隔Δtを入力として水平方向ベクトルを推定する。

【0032】

水平方向推定部１０１は、内部の状態データとして、少なくとも状態ベクトル（具体的には水平方向ベクトル）と、状態ベクトルの誤差分散共分散行列Ｐ_ｈを保持している。本例では水平方向ベクトルが下記のように３×１サイズの行列であるから、誤差分散共分散行列は、３×３サイズの正方行列となる。水平方向推定部１０１は、カルマンフィルタの枠組みに従ってこれらの状態データを処理する。上記水平方向ベクトルは下記のように表される。このベクトルは、水平方向の基準を表す単位ベクトルである。なお、水平方向の基準とは、真北方向を指す水平方向ベクトルであり、このため、水平方向ベクトルは、真北方向ベクトルと表現することもできる。

【0033】

【数5】

【0034】

姿勢推定部１０２は、センサ座標系における重力方向ベクトルと水平方向ベクトルから移動体の姿勢を示す姿勢情報を生成する。具体的には、姿勢推定部１０２は、まず、重力方向推定部１００が保持する状態データである重力方向ベクトル（更新後）と、水平方向推定部１０１が保持する状態データである水平方向ベクトル（更新後）との外積ベクトル（下記）を算出する。

【0035】

【数6】

【0036】

そして、姿勢推定部１０２は、上記外積ベクトルを用いて、下記の数式（１）で示される回転行列Ｒ（姿勢情報）を生成する。この回転行列Ｒを用いることにより、センサ座標系から世界座標系に姿勢を変換することができる。また、この回転行列Ｒは、歩行者自律航法などにも利用可能である。なお、姿勢推定部１０２が生成する姿勢情報は、移動体の姿勢を示すものであればよく、下記の回転行列Ｒに限られない。例えば、姿勢推定部１０２は、重力方向ベクトルと水平方向ベクトルを用いてＤＣＭ（direction cosine matrix）を生成してもよい。

【0037】

【数7】

【0038】

〔重力方向推定部が実行する処理の流れ〕
次に、重力方向推定部１００が実行する処理（姿勢推定方法）について図２に基づいて説明する。図２は、重力方向推定部１００が実行する処理の一例を示すフローチャートである。

【0039】

まず、重力方向推定部１００は、重力方向推定部１００が保持する状態データ、すなわち状態ベクトル（重力方向ベクトル）と、その誤差分散共分散行列を初期化する（Ｓ２００）。初期状態における姿勢（状態ベクトル）が未知であれば（多くの場合が未知である）、状態ベクトルの初期値は任意の値とすればよく、例えば(0,0,1)^Ｔとしてもよい。また、誤差分散共分散行列については、取り得る状態ベクトルの不確かさを勘案して設定すればよい。

【0040】

続いて、重力方向推定部１００は、角速度センサ１５から角速度ベクトルの入力を受け付ける（Ｓ２０１）と共に、タイミングデバイス１４からのタイミングデータΔｔの入力を受け付ける（Ｓ２０２）。なお、センサ間の同期が取れていない場合は、角速度ベクトルのセンサデータの取得時間間隔をΔｔとする。

【0041】

次に、重力方向推定部１００は、Ｓ２０１で入力された角速度ベクトルとＳ２０２で入力されたΔｔから状態データを更新する（Ｓ２０３）。この更新は予測に基づく更新であり、重力方向ベクトルの予測値が算出され、該予測値により重力方向推定部１００の保持する重力方向ベクトルが上書き更新される。具体的には、重力方向推定部１００は、下記の数式（２）により状態ベクトル（重力方向ベクトル）を更新する。

【0042】

【数8】

【0043】

この数式（２）において、

【0044】

【数9】

【0045】

は、更新後の状態ベクトル（重力方向ベクトルの予測値）である。また、ｅｘｐ（）は行列の指数関数を返す演算子である。そして、

【0046】

【数10】

【0047】

における角速度ベクトルに付された演算子は歪対称行列（skew symmetric matrix）を取得するための演算子である。

【0048】

また、重力方向推定部１００は、下記の数式（３）により誤差分散共分散行列を更新する。

【0049】

【数11】

【0050】

この数式（３）において、Ｍは更新後の誤差分散共分散行列であり、Φ（Δｔ）はｅｘｐ（ＦΔｔ）である。また、Ｆは、カルマンフィルタにおいて、予測更新式の線形要素となる行列である。なお、非線形の行列については、拡張カルマンフィルタの枠組みに従い、テイラー展開によって１次項までを用いて線形化する。そして、Ｑは、プロセス誤差分散共分散行列である。

【0051】

次に、重力方向推定部１００は、加速度センサ１３から加速度ベクトルの入力を受け付ける（Ｓ２０４）。そして、重力方向推定部１００は、上記加速度ベクトルを重力方向ベクトル（単位ベクトル）の観測として用いて、以下の数式（４）〜（６）により状態データ（重力方向ベクトルの予測値とその誤差分散共分散行列）を更新する（Ｓ２０５、重力方向推定ステップ）。なお、加速度ベクトルの単位系は［Ｇ］（地球の重力加速度を１とする単位）として扱う。

【0052】

【数12】

【0053】

【数13】

【0054】

【数14】

【0055】

上記数式（４）のＫはカルマンゲインであり、Ｒは観測誤差分散共分散行列である。また、上記数式（５）のＩは単位行列、Ｐ_ｇは更新後の誤差分散共分散行列である。

【0056】

観測誤差分散共分散行列Ｒは、より詳細には、観測として得られる加速度ベクトルによる重力方向の観測における誤差の分散共分散行列である。例えば、静置されているような状況下では、小さな値を要素とし、運動中（運動加速度が外乱として印可されているような状況下）では、大きな値を要素とするようにＲを設定する。また、例えば、姿勢推定装置１を歩行者が装着するという前提がある場合、歩行動作中と、非歩行動作中でＲの要素を切り替えてもよい。なお、この場合、加速度ベクトルの値等に基づいて歩行動作中であることを検出する歩行動作検出部を制御部１０の構成要素に追加する。そして、歩行動作の検出を契機にＲの要素を切り替え、その後歩行動作が検出されなくなったことを契機にＲの要素を戻せばよい。

【0057】

Ｓ２０５の終了後、処理はＳ２０１に戻る。つまり、Ｓ２０１〜Ｓ２０５の処理は、移動体の姿勢のトラッキング中において継続して繰り返し行われ、これにより移動体の状態データは移動体の最新の状態に応じたものに逐次更新される。

【0058】

〔水平方向推定部が実行する処理の流れ〕
次に、水平方向推定部１０１が実行する処理（姿勢推定方法）について図３に基づいて説明する。図３は、水平方向推定部１０１が実行する処理の一例を示すフローチャートである。なお、本処理では、上述した図２の処理の計算結果を利用するので、本処理は図２の処理の終了後に行われる。

【0059】

まず、水平方向推定部１０１は、水平方向推定部１０１が保持する状態データ、すなわち状態ベクトル（水平方向ベクトル）と、その誤差分散共分散行列を初期化する（Ｓ３００）。初期状態における姿勢（状態ベクトル）が未知であれば（多くの場合が未知である）、状態ベクトルの初期値は任意の値とすればよく、例えば(1,0,0)^Ｔとしてもよい。また、誤差分散共分散行列については、取り得る状態ベクトルの不確かさを勘案して設定すればよい。

【0060】

続いて、水平方向推定部１０１は、角速度センサ１５から角速度ベクトルの入力を受け付ける（Ｓ３０１）と共に、タイミングデバイス１４からのタイミングデータΔｔの入力を受け付ける（Ｓ３０２）。なお、センサ間の同期が取れていない場合は、角速度ベクトルのセンサデータの取得時間間隔をΔｔとする。

【0061】

次に、水平方向推定部１０１は、Ｓ３０１で入力された角速度ベクトルとＳ３０２で入力されたΔｔから状態データを更新する（Ｓ３０３）。この更新は予測に基づく更新であり、水平方向ベクトルの予測値が算出され、該予測値により水平方向推定部１０１の保持する水平方向ベクトルが上書き更新される。具体的には、水平方向推定部１０１は、下記の数式（７）により状態ベクトル（水平方向ベクトル）を更新する。

【0062】

【数15】

【0063】

この数式（７）において、

【0064】

【数16】

【0065】

は、更新後の状態ベクトル（水平方向ベクトルの予測値）である。また、ｅｘｐ（）は行列の指数関数を返す演算子である。そして、

【0066】

【数17】

【0067】

における角速度ベクトルに付された演算子は歪対称行列を取得するための演算子である。

【0068】

なお、Ｓ３０３の更新に用いる行列は、Ｓ２０３で用いるものと計算方法が同一である。このため、同一の角速度ベクトルと同一のΔｔが入力されている場合は、水平方向推定部１０１は、重力方向推定部１００がＳ２０３で求めた行列を流用し、それらの行列の計算処理を省略してもよい。

【0069】

また、水平方向推定部１０１は、下記の数式（８）により誤差分散共分散行列を更新する。

【0070】

【数18】

【0071】

この数式（８）において、Ｍ_ｈは更新後の誤差分散共分散行列であり、Φ（Δｔ）はｅｘｐ（ＦΔｔ）である。また、Ｆは、カルマンフィルタにおいて、予測更新式の線形要素となる行列である。なお、非線形の行列については、拡張カルマンフィルタの枠組みに従い、テイラー展開によって１次項までを用いて線形化する。そして、Ｑ_ｈは、プロセス誤差分散共分散行列である。

【0072】

次に、水平方向推定部１０１は、磁気センサ１６から磁気ベクトルの入力を受け付ける（Ｓ３０４）と共に、重力方向推定部１００から、重力方向推定部１００が保持する重力方向ベクトルを取得する（Ｓ３０５）。なお、この重力方向ベクトルは、Ｓ２０５で更新された、センサ座標系における重力方向ベクトルである。

【0073】

続いて、水平方向推定部１０１は、Ｓ３０４で入力を受け付けた磁気ベクトルが、状態データの更新に用いるのに適切な値となっているか否かを検査する（Ｓ３０６）。具体的には、水平方向推定部１０１は、磁気ベクトルと重力方向ベクトルを用いて、以下の二つの検査を実行する。
（Ａ）磁気ベクトルの大きさの検査
（Ｂ）磁気ベクトルの伏角の検査
上記（Ａ）の検査では、水平方向推定部１０１は、Ｓ３０４で得られた磁気ベクトルの大きさが、この検査が実施されている周辺地域（移動体が存在する地域）の地磁気ベクトルの大きさに対して、所定の誤差範囲に収まっているか否かを判定する。

【0074】

上記（Ｂ）の検査では、水平方向推定部１０１は、Ｓ３０４で得られた磁気ベクトルとＳ３０５で得られた重力方向ベクトルを用いて磁気ベクトルの伏角を計算する。そして、水平方向推定部１０１は、計算した伏角が、この検査が実施されている周辺地域（移動体が存在する地域）の地磁気ベクトルの伏角に対して、所定の誤差範囲に収まっているか否かを判定する。

【0075】

なお、上記判定に用いる各データ（地磁気ベクトルの大きさすなわち全磁力と、地磁気ベクトルの伏角）は、予め記憶部１１等に記憶しておいてもよい。例えば、地域ごとの地磁気ベクトルの大きさと伏角を記録したデータベースを記憶しておけば、移動体の位置に応じたデータを取得可能である。また、これらのデータを演算にて算出してもよい。この他にも、これらデータは、該データを記憶しているか、あるいは該データを算出する機能を備えた他の装置から取得してもよい。後述のＳ３０８で用いる地磁気ベクトルの偏角についても同様である。

【0076】

続いて、水平方向推定部１０１は、Ｓ３０４で得られた磁気ベクトルが検査を通過するか否かを判定する（Ｓ３０７）。具体的には、水平方向推定部１０１は、上記（Ａ）（Ｂ）の検査の両方において所定の誤差範囲に収まっていると判定した場合に検査通過と判定し、それ以外の場合には検査不通過と判定する。

【0077】

Ｓ３０７で検査通過と判定した場合（Ｓ３０７でＹＥＳ）、水平方向推定部１０１は、Ｓ３０４で得られた磁気ベクトルを、基準となる水平方向の観測として用いて、状態データ（水平方向ベクトルの予測値とその誤差分散共分散行列）を更新する（Ｓ３０８、水平方向推定ステップ）。具体的には、水平方向推定部１０１は下記の数式（９）〜（１１）を用いて状態データを更新する。

【0078】

【数19】

【0079】

【数20】

【0080】

【数21】

【0081】

上記数式（９）のＲ_ｈは観測誤差分散共分散行列である。また、上記数式（１０）のＫはカルマンゲインであり、Ｉは単位行列、Ｐ_ｈは更新後の誤差分散共分散行列である。

【0082】

また、上記数式（１１）の

【0083】

【数22】

【0084】

は、磁気ベクトルを重力方向ベクトルと直交するように補正すると共に、地磁気ベクトルの偏角だけ補正した水平方位角ベクトル（単位ベクトル）である。このような補正後の磁気ベクトルを用いることにより、水平方向ベクトルを高精度に更新することができる。

【0085】

一方、Ｓ３０７で検査不通過と判定した場合（Ｓ３０７でＮＯ）、水平方向推定部１０１は、磁気ベクトルの観測は無効であるとみなし、予測のみから状態データを更新する（Ｓ３０９）。具体的には、水平方向推定部１０１は、Ｓ３０３で得られた状態データの値を用いて状態データを上書きする。すなわち、

【0086】

【数23】

【0087】

【数24】

【0088】

とする。この場合には、磁気ベクトルを用いた更新は行われず、実質的には状態データは更新されない。

【0089】

上記Ｓ３０８およびＳ３０９の何れの処理の後にもＳ３１０の処理が行われる。Ｓ３１０では、水平方向推定部１０１は、Ｓ３０５で得られた重力方向ベクトルに直交するように、Ｓ３０８またはＳ３０９の処理において更新された水平方向ベクトルを補正する。具体的には、水平方向推定部１０１は、下記の数式（１２）にて補正後の水平方向ベクトルを算出し、この水平方向ベクトルを状態ベクトルとして保持する。

【0090】

【数25】

【0091】

Ｓ３１０の終了後、処理はＳ３０１に戻る。つまり、Ｓ３０１〜Ｓ３１０の処理は、移動体の姿勢のトラッキング中において継続して繰り返し行われ、これにより移動体の状態データは移動体の最新の状態に応じたものに逐次更新される。

【0092】

〔姿勢の推定〕
以上説明したように、重力方向推定部１００は、重力方向ベクトルを含む状態データを逐次更新し、水平方向推定部１０１は、水平方向ベクトルを含む状態データを随時更新する。よって、姿勢推定部１０２は、任意のタイミングにおいて、重力方向推定部１００から重力方向ベクトルを取得すると共に、水平方向推定部１０１から水平方向ベクトルを取得して、上述の数式（１）により移動体の姿勢を示す姿勢情報を生成する（姿勢推定ステップ）。そして、姿勢の推定を逐次行うことにより、移動体の姿勢のトラッキングを行うことができる。

【0093】

〔演算量の低減効果〕
姿勢推定装置１で更新する重力方向ベクトルと水平方向ベクトルは何れも３×１行列であるから、これらの更新の際に発生する乗算は何れも３×３行列で済む。これに対し、非特許文献１のような従来技術では、９つの成分を持つ９×１行列で表される状態ベクトルを更新し、この更新の際には９×９行列の乗算が発生する。

【0094】

９×９行列の乗算に係る演算コストは、３×３行列の乗算に係る演算コストの３^３＝２７倍であるから、重力方向ベクトルと水平方向ベクトルのそれぞれについて更新を行う演算コストを考慮しても、従来技術の方が２７÷２＝１３．５倍も演算コストが大きい。よって、姿勢推定装置１によれば、従来技術に対して最大で１３．５倍も演算速度を高速化することが可能になる。また、この高速化は、従来のカルマンフィルタによる状態ベクトルの更新に冗長性が含まれている点に着目し、その冗長性が解消されるようにすることによって実現されているから、姿勢の推定精度は従来技術と遜色がない。

【0095】

〔実施形態２〕
上記実施形態では、加速度センサ１３、角速度センサ１５、および磁気センサ１６を備える姿勢推定装置１について説明したが、本発明の姿勢推定装置１は、これらのセンサの計測結果を取得できればよく、これらのセンサを必ずしも備えている必要はない。例えば、本発明の姿勢推定装置１は、移動体に装着された上述の各センサから無線通信にて計測結果を取得してもよい。また、リアルタイムで姿勢を推定する必要がない場合には、上述の各センサの計測結果を記録媒体に記録しておき、この記録媒体から計測結果を取得してもよい。同様に、本発明の姿勢推定装置１は、タイミングデバイス１４を必ずしも備えている必要はなく、姿勢推定装置１の外部のタイミングデバイス１４からタイミングデータを取得してもよい。

【0096】

〔実施形態３〕
上記実施形態では、姿勢情報を出力する姿勢推定装置１について説明したが、生成した姿勢情報を用いて姿勢情報以外のパラメータを算出・出力する装置も本発明の範疇に含まれる。例えば、上記実施形態の手法にて生成した姿勢情報と、移動体の速度（加速度センサ１３の計測結果から算出可能）とを用いて、その移動体の現在位置を算出する移動体の測位装置も本発明の範疇に含まれる。また、上記姿勢情報から移動体（例えば歩行者）がどのような動作を行っているかを検出する動作検出装置や、上記姿勢情報から移動体の移動速度を算出する移動速度算出装置も本発明の範疇に含まれる。

【0097】

〔実施形態４〕
上記各実施形態では、従来技術（例えば非特許文献１）におけるカルマンフィルタの状態ベクトルの９つの成分のうち、角速度成分は状態ベクトルとして使用していないが、角速度成分も状態ベクトルに含めてもよい。

【0098】

この場合、重力方向推定部１００の保持する状態ベクトルが、３軸の重力方向成分および３軸の角速度成分の計６つの成分を持つ６×１行列となる。同様に、水平方向推定部１０１の保持する状態ベクトルが、３軸の水平方向成分および３軸の角速度成分の計６つの成分を持つ６×１行列となる。このような構成であっても、９つの成分を有する状態ベクトルを更新する従来技術と比べて更新に係る演算量を減らすことができる。ただし、本願の発明者による検討の結果、角速度成分を状態ベクトルに含めなくとも、含めた場合と同等の精度で姿勢の推定が可能であることが分かっているので、演算量を減らすという観点からは角速度成分を状態ベクトルに含めないことが好ましい。

【0099】

〔ソフトウェアによる実現例〕
姿勢推定装置１の制御ブロック（特に制御部１０）は、集積回路（ＩＣチップ）等に形成された論理回路（ハードウェア）によって実現してもよいし、ＣＰＵ（Central Processing Unit）を用いてソフトウェアによって実現してもよい。

【0100】

後者の場合、姿勢推定装置１は、各機能を実現するソフトウェアであるプログラムの命令を実行するＣＰＵ、上記プログラムおよび各種データがコンピュータ（またはＣＰＵ）で読み取り可能に記録されたＲＯＭ（Read Only Memory）または記憶装置（これらを「記録媒体」と称する）、上記プログラムを展開するＲＡＭ（Random Access Memory）などを備えている。そして、コンピュータ（またはＣＰＵ）が上記プログラムを上記記録媒体から読み取って実行することにより、本発明の目的が達成される。上記記録媒体としては、「一時的でない有形の媒体」、例えば、テープ、ディスク、カード、半導体メモリ、プログラマブルな論理回路などを用いることができる。また、上記プログラムは、該プログラムを伝送可能な任意の伝送媒体（通信ネットワークや放送波等）を介して上記コンピュータに供給されてもよい。なお、本発明は、上記プログラムが電子的な伝送によって具現化された、搬送波に埋め込まれたデータ信号の形態でも実現され得る。

【0101】

本発明は上述した各実施形態に限定されるものではなく、請求項に示した範囲で種々の変更が可能であり、異なる実施形態にそれぞれ開示された技術的手段を適宜組み合わせて得られる実施形態についても本発明の技術的範囲に含まれる。

【符号の説明】

【0102】

１ 姿勢推定装置
１３ 加速度センサ
１５ 角速度センサ
１６ 磁気センサ
１００ 重力方向推定部
１０１ 水平方向推定部
１０２ 姿勢推定部

【図1】

【図2】

【図3】