JP2022045269A - 移動体の姿勢推定方法 - Google Patents

Abstract

【課題】Madgwickフィルタの手法による移動体の姿勢の推定値の確からしさ（信頼度）の程度を適切に把握し得る状態量を得ることができる方法を提供する。【解決手段】角速度の検出値と、加速度及び地磁気のうちの少なくとも一方の検出対象量の検出値とからMadgwickフィルタにより、検出対象量の検出値と基準値との偏差の大きさを表す損失関数を最小化するように、移動体の姿勢の推定値を表す姿勢指標値を逐次求める第１ステップと、損失関数の姿勢指標値に対する２階偏微分値を成分として構成されるヘッセ行列を求め、該ヘッセ行列の固有値を姿勢指標値の確からしさに関連する状態量として生成する第２ステップとを備える。【選択図】図２

Description

本発明は、移動体の姿勢を推定する方法に関する。

移動体の姿勢を推定する手法は、従来より様々な手法が知られている。また、例えば特許文献１には、複数のアルゴリズムのそれぞれにより移動体の位置及び姿勢を推定し、これらの位置及び姿勢を、それぞれの推定値の確からしさに関する状態量から学習処理部により求めた重み係数により合成することで、移動体の位置及び姿勢を特定する技術が提案されている。

国際公開第２０１９／２０２８０６号

ところで、移動体の姿勢を推定する1つの手法として、例えばMadgwickフィルタの手法を採用し得る。この場合、Madgwickフィルタの手法による移動体の推定値に応じて移動体の移動を制御する場合、あるいは、Madgwickフィルタの手法による移動体の推定値を他の手法による姿勢の推定値と統合する場合等に、Madgwickフィルタの手法による移動体の姿勢の推定値の確からしさ（信頼度）の程度を把握し得ることが望まれる。

本発明はかかる背景に鑑みてなされたものであり、Madgwickフィルタの手法による移動体の姿勢の推定値の確からしさ（信頼度）の程度を適切に把握し得る状態量を得ることができる方法を提供することを目的とする。

さらに、Madgwickフィルタの手法による移動体の姿勢の推定値と、他の手法による移動体の姿勢の推定値とから上記状態量を利用して適切に移動体の姿勢を特定することが可能となる方法を提供することを目的とする。

本発明の移動体の姿勢推定方法は、上記の目的を達成するために、移動体に発生する角速度の検出値と、該移動体に発生する加速度及び該移動体の存在位置での地磁気のうちの少なくとも一方からなる検出対象量の検出値とを用いて該移動体の姿勢を推定する方法であって、
前記角速度の検出値と前記検出対象量の検出値とからMadgwickフィルタにより、前記検出対象量の検出値と該検出対象量の基準値との偏差の大きさを表す所定の損失関数を最小化するように、前記移動体の姿勢の推定値を表す姿勢指標値を逐次求める第１ステップと、
前記損失関数の前記姿勢指標値に対する２階偏微分値を成分として構成されるヘッセ行列を求め、当該求めたヘッセ行列の固有値を前記姿勢指標値の確からしさに関連する状態量として生成する第２ステップとを備えることを特徴とする（第１発明）。

ここで、Madgwickフォルタにより移動体の姿勢を推定する処理を実行した場合、上記のヘッセ行列の固有値が、前記移動体の姿勢の推定値を表す姿勢指標値の確からしさと相関性を有することが、本願発明者による様々な検討により判明した。

そこで、第１発明を上記の如く構成した。これにより、第１発明によれば、Madgwickフィルタの手法による移動体の姿勢の推定値の確からしさ（信頼度）の程度を適切に把握し得る状態量を得ることができる。

上記第１発明では、前記検出対象量は、前記加速度及び前記地磁気の両方から成る場合には、前記ヘッセ行列は、前記加速度に関連する第１ヘッセ行列と前記地磁気に関連する第２ヘッセ行列との和として表される行列である。この場合、前記第2ステップで生成される状態量は、前記第１ヘッセ行列の固有値と前記第２ヘッセ行列の固有値とのうちの少なくとも一方を含むという態様を採用し得る（第２発明）。

このように前記検出対象量が、加速度及び地磁気の両方から成る場合には、上記第１ヘセ行列の固有値及び第２ヘッセ行列の固有値のいずれをも、姿勢指標値の確からしさに関する適切な状態量として使用し得る。

上記第１発明又は第２発明では、前記角速度及び前記検出対象量をそれぞれ検出可能なセンサを含む複数のセンサのうちの1つ以上のセンサの検出値を用いて、前記Madgwickフィルタと異なる1つ以上のアルゴリズムにより前記移動体の姿勢を逐次推定する処理を各アルゴリズ毎に実行する第３ステップと、前記各アルゴリズムにより得られた前記移動体の姿勢の推定値の確からしさに関連する状態量を当該各アルゴリズム毎に生成する第４ステップと、前記Madgwickフィルタに対応して前記第２ステップで生成された状態量と前記各アルゴリズムに対応して前記第４ステップで生成された状態量とを学習済のニューラルネットワークに入力し、当該入力された状態量から該ニューラルネットワークにより前記Madgwickフィルタ及び前記各アルゴリズムのそれぞれ毎の重み係数を決定する第５ステップと、前記姿勢指標値により示される姿勢の推定値と、前記各アルゴリズムのそれぞれにより得られた姿勢の推定値とを、前記決定された重み係数により合成して得られる姿勢を、前記移動体の姿勢として特定する第６ステップとをさらに備えるという態様を採用し得る（第３発明）。

これによれば、Madgwickフィルタ及び他の各アルゴリズムのそれぞれ毎に生成される状態量から、学習済のニューラルネットワークによって、Madgwickフィルタ及び他の各アルゴリズムのそれぞれに対応する重み係数を適切に決定できる。このため、Madgwickフィルタにより得られた姿勢指標値により示される姿勢の推定値と、他の各アルゴリズムのそれぞれにより得られた姿勢の推定値とを上記重み係数により適切に合成して、移動体の姿勢を特定することができる。このため、信頼性の高い姿勢の特定値を得ることを高いロバスト性で実現することが可能となる。

よって、第３発明によれば、Madgwickフィルタの手法による移動体の姿勢の推定値と、他の手法による移動体の姿勢の推定値とから上記状態量を利用して適切に移動体の姿勢を特定することが可能となる。

本発明の実施形態における移動体を示す図。 実施形態の姿勢推定処理装置の機能的構成を示すブロック図。 移動体のヨー方向の姿勢角と第１ヘッセ行列の固有値との経時変化の実測例を示すグラフ。 移動体のヨー方向の姿勢角と第２ヘッセ行列の固有値との経時変化の実測例を示すグラフ。

本発明の一実施形態を図１～図４を参照して説明する。図１を参照して、本実施形態での移動体１は、例えば車輪型の移動体であり、その移動環境の床面上に、複数の車輪２を介して支持される機体３を有する。そして、移動体１は、図示を省略する電動モータ等のアクチュエータにより、一つ以上の車輪２を回転駆動することにより、あるいは、一つ以上の車輪２の回転駆動と操舵（方向転換）とを行うことにより、床面上を移動する。

この移動体１には、該移動体１の姿勢を推定するために使用する複数のセンサが搭載されている。例えば、一つ以上の車輪２の回転角を検出する角度センサ等のオドメトリ用センサ５と、移動体１の周囲に存在する外界物までの距離を測定する測距センサとしてのレーザ・レンジ・ファインダ６（以降、ＬＲＦ６という）と、移動体１の上方に存在する天井を撮像するカメラ７と、移動体１の存在位置での地磁気、移動体１に発生する加速度及び移動体１に発生する角速度をそれぞれ検出する磁気センサ１１、加速度センサ１２及び角速度センサ１３を含むセンサユニット１０とが移動体１に搭載されている。

なお、オドメトリ用センサ５は、車輪２の回転角を検出する角度センサの他、車輪２のうちの操舵輪の操舵角を検出するセンサを含み得る。また、カメラ７は、本実施形態では、ステレオカメラである。また、磁気センサ１１、加速度センサ１２および角速度センサ１３は、３軸方向の地磁気、３軸方向の加速度、３軸方向の角速度を各々検出可能である。また、加速度センサ１２が検出する加速度には、重力加速度が含まれる。

移動体１には、これらのセンサの検出情報を用いて移動体１の姿勢（向き）の推定を行う姿勢推定処理装置２０が搭載されている。なお、移動体１の姿勢は、より詳しくは、移動体１の移動環境に対して任意に設定されたグローバル座標系（地表座標系）で

見た機体３の姿勢である。なお、姿勢推定処理装置２０により推定する姿勢は、移動体１の空間的な（３次元の）姿勢に限らず、例えば所定の１軸周りの姿勢（姿勢角）であってもよい。例えば、該姿勢は、ヨー方向（上下方向の軸周りの方向）の姿勢角であってもよい。

姿勢推定処理装置２０は、例えば、マイコン、メモリ、インターフェース回路等を含む一つ以上の電子回路ユニットにより構成される。そして、姿勢推定処理装置２０は、実装されたハードウェア構成及びプログラム（ソウフトウェア構成）の両方又は一方により実現される機能として、図２のブロック図で示す如く、第１姿勢推定処理部２１と、第２姿勢推定処理部２２と、第３姿勢推定処理部２３と、重み係数学習処理部２４と、姿勢特定処理部２５とを含む。なお、姿勢推定処理装置２０は、移動体１の姿勢を推定する機能に限らず、例えば移動体１の位置（グローバル座標系で見た位置）を推定する機能、移動体１の移動制御を行う機能等も含み得る、

第１姿勢推定処理部２１は、ＬＲＦ６の検出情報とオドメトリ用センサ５の検出情報とを用いて移動体１の姿勢を推定する処理を実行する機能部、第２姿勢推定処理部２２は、カメラ７の検出情報を用いて移動体１の姿勢を推定する処理を逐次実行する機能部、第３姿勢推定処理部２３は、磁気センサ１１、加速度センサ１２及び角速度センサ１３の検出情報を用いて移動体１の姿勢を推定する処理を逐次実行する機能部である。以降の説明では、第１～第３姿勢推定処理部２１～２３によりそれぞれ推定される移動体１の姿勢を第１推定姿勢、第２推定姿勢、第３推定姿勢ということがある。

また、重み係数学習処理部２４は、第１～第３姿勢推定処理部２１～２３のそれぞれから入力される後述の状態量Ａ，Ｂ，Ｃに応じて、第１～第３姿勢推定処理部２１～２３のそれぞれに対応する重み係数（詳しくは、第１～第３姿勢推定処理部２１～２３のそれぞれから出力される移動体１の第１推定姿勢、第２推定姿勢、第３推定姿勢のそれぞれに対する重み係数）を出力する機能部、姿勢特定処理部２５は、第１～第３姿勢推定処理部２１～２３のそれぞれから出力される移動体１の推定位置姿勢を、重み係数学習処理部２４から出力される重み係数に応じて合成することによって、移動体１の姿勢を特定する（移動体１の推定姿勢を確定する）機能部である。

以下に、上記各機能部の詳細と併せて、姿勢推定処理装置２０の全体の処理を説明する。第１～第３姿勢推定処理部２１～２３の処理が以下に説明する如く実行される。第１姿勢推定処理部２１には、ＬＲＦ６及びオドメトリ用センサ５のそれぞれの検出情報が入力されると共に、移動体１の移動環境のマップデータ（例えば、２次元の占有格子地図等）とが入力される。この場合、マップデータは、姿勢推定処理装置２０のメモリにあらかじめ記憶保持され、あるいは、外部のサーバ等から随時、ダウンロードされる。

そして、第１姿勢推定処理部２１は、ＬＲＦ６の検出情報（移動体１の周囲の外界物の測距データ）と、オドメトリ用センサ５の検出情報と、マップデータとから、姿勢推定用の所定のアルゴリズムにより、移動体１の姿勢を逐次推定する（推定姿勢を逐次決定する）。

本実施形態では、第１姿勢推定処理部２１は、姿勢推定用のアルゴリズムとして、移動体１の姿勢と併せて、移動体１の位置（自己位置）をも推定し得るアルゴリズムを使用する。具体的には、第１姿勢推定処理部２１では、粒子フィルタを用いて移動体１の位置姿勢（位置及び姿勢）を推定し得るアルゴリズム、例えば、AMCL（Adaptive Monte Carlo Localization）という公知の手法のアルゴリズムが使用される。

このAMCLのアルゴリズムでは、次のような処理によって、移動体１の位置及び姿勢が逐次推定される。まず、移動体１の初期位置及び初期姿勢に応じて、移動体１の近辺に、粒子フィルタにおける複数の粒子（パーティクル）が生成される。そして、オドメトリ用センサ５の検出情報から推定される移動体１の移動速度（並進速度及び角速度）に応じて、各粒子の新たな移動位置及び姿勢が推定される。このとき、各粒子の新たな移動位置及び姿勢は、オドメトリ用センサ５の検出情報の誤差（正規分布状の誤差等）を確率的に反映させて推定される。

次いで、ＬＲＦ６による移動体１の周囲の外界物の測距データを取得し、この測距データと、マップデータとに基づいて、各粒子の新たな移動位置及び姿勢の尤度が算出される。そして、リサンプリング処理によって、尤度が低い粒子が消滅され、さらに、尤度の高い粒子の近辺に新たな粒子が生成される。

AMCLのアルゴリズムでは、以上の如き処理を逐次実行することで得られる尤度の高い粒子の位置及び姿勢の時系列に基づいて、移動体１の位置及び姿勢が逐次推定される（移動体１の位置及び姿勢の推定値としての推定位置姿勢が逐次決定される）。また、このアルゴリズムでは、当該推定処理の実行過程で、移動体１の推定位置姿勢の確からしさに関連する状態量Ａとしての共分散（詳しくは分散共分散）が生成される。

そして、第１姿勢推定処理部２１は、上記の如くAMCLのアルゴリズムにより推定した移動体１の位置及び姿勢のうちの姿勢を第１推定姿勢として逐次出力すると共に、該第１推定姿勢の推定時の時刻である推定時刻を該第１推定姿勢と共に出力する。さらに、第１姿勢推定処理部２１は、第１推定姿勢の確からしさ（信頼度）に関連する状態量Ａとして、上記共分散（分散共分散）を該第１推定姿勢と共に出力する。

第２姿勢推定処理部２２には、カメラ７の検出情報としての撮像画像（移動体１の上方の天井の撮像画像）が入力される。なお、本実施形態では、カメラ７はステレオカメラであるので、第２姿勢推定処理部２２に入力されるカメラ７の検出情報（撮像画像）はステレオ画像（一対の撮像画像）である。ただし、カメラ７は、ステレオカメラに限らず、単眼カメラであってもよい。

そして、第２姿勢推定処理部２２は、入力された撮像画像から、姿勢推定用の所定のアルゴリズムにより、移動体１の姿勢を逐次推定する。本実施形態では、第２姿勢推定処理部２２は、姿勢推定用のアルゴリズムとして、移動体１の姿勢と併せて、移動体１の位置（自己位置）をも推定し得るアルゴリズムを使用する。具体的には、第２姿勢推定処理部２２では、入力された撮像画像からの特徴点を検出し、該特徴点と、過去に入力された撮像画像から検出された特徴点とのマッチングとを行うアルゴリズム、例えばORB_SLAMという公知の手法のアルゴリズムが使用される。

このORB_SLAMのアルゴリズムでは、次のような処理によって、移動体１の位置及び姿勢が逐次推定される。すなわち、移動体１の初期位置及び初期姿勢を設定した上で、カメラ７による撮像画像（ステレオ画像）が第２姿勢推定処理部２２に逐次入力される。そして、入力された撮像画像毎に、該撮像画像からＯＲＢ特徴点を検出する処理が逐次実行される。なお、移動体１の初期位置及び初期姿勢は、例えば、ＬＲＦ６の検出情報に基づいて設定され得る。

そして、新たに入力された撮像画像（時刻ｔの撮像画像）から検出されたＯＲＢ特徴点と、一つ前に入力された過去の撮像画像（時刻ｔ－Δｔの撮像画像）から検出されたＯＲＢ特徴点とが比較され、ＯＲＢ特徴点同士のマッチングが行われる。このマッチングでは、特徴量の差の大きさ（絶対値）が所定の閾値以下となるＯＲＢ特徴点同士が互いに同じＯＲＢ特徴点（互いに対応するＯＲＢ特徴点）であると判定される。

そして、当該マッチングに基づいて、上記二つの時刻ｔ，ｔ－Δｔの間の期間での移動体１の移動量及び移動方向が推定される。さらに、この移動量及び移動方向の推定値と、時刻ｔ－Δｔでの移動体１の位置及び姿勢の推定値とから、時刻ｔでの移動体１の位置及び姿勢が決定される。なお、ORB_SLAMのアルゴリズムにより求める移動体１の位置及び姿勢の推定値は、例えば、第２姿勢推定処理部２２に入力される撮像画像のフレーム数が所定数に達する毎に、ＬＲＦ６の検出情報に基づいて認識される移動体１の位置及び姿勢にリセットしてもよい。

ORB_SLAMのアルゴリズムでは、以上の如き処理を逐次実行することで、移動体１の位置及び姿勢が逐次推定される（移動体１の位置及び姿勢の推定値としての推定位置姿勢が逐次決定される）。また、このアルゴリズムでは、当該推定処理の実行過程で、推定位置姿勢の確からしさに関連する状態量Ｂとして、移動体１の推定速度・角速度（並進速度及び角速度の推定値）と、検出されたＯＲＢ特徴点の個数（総数）及びマッチング個数（前記マッチングにより互いに同じであると判定されたＯＲＢ特徴点の個数）と、誤差関数の誤差値（前記マッチングにおける特徴量の差）と、推定状態（移動体１の推定速度・角速度の推定精度の高低度合い）とが生成される。

そして、第２姿勢推定処理部２２は、上記の如くORB_SLAMのアルゴリズムにより推定した移動体１の位置及び姿勢のうちの姿勢を第２推定姿勢として逐次出力すると共に、該第２推定姿勢の推定時の時刻である推定時刻を逐次出力する。さらに第２姿勢推定処理部２２は、第２推定姿勢の確からしさ（信頼度）に関連する状態量Ｂとして、移動体１の推定速度・角速度（並進速度及び角速度の推定値）と、検出されたＯＲＢ特徴点の個数及びマッチング個数と、誤差関数の誤差値（前記マッチングにおける特徴量の差）と、推定状態（移動体１の推定速度・角速度の推定精度の高低度合い）とを推定姿勢と共に出力する。なお、ＯＲＢ特徴点の個数及びマッチング個数の代わりに、ＯＲＢ特徴点の個数（総数）に対するマッチング個数の割合を出力するようにしてもよい。

第３姿勢推定処理部２３には、磁気センサ１１、加速度センサ１２及び角速度センサ１３の検出情報が入力される。そして、第３姿勢推定処理部２３は、入力された検出情報（地磁気、加速度及び角速度の検出情報）から、姿勢推定用の所定のアルゴリズムにより、移動体１の姿勢を逐次推定する（移動体１の姿勢の推定値としての推定姿勢を逐次決定する）。

本実施形態では、第３姿勢推定処理部２３は、姿勢推定用のアルゴリズムとして、Madgwickフィルタという公知の手法のアルゴリズムを使用する。このMadgwickフィルタでは、磁気センサ１１により検出される地磁気と、加速度センサ１２により検出される加速度（並進加速度）と、該地磁気及び加速度のそれぞれの基準値と、移動体１の姿勢の推定値との関数として表される所定の損失関数Ｌの値を最小化し得るように、移動体１の姿勢が勾配法によって逐次推定される。この場合、Madgwickフィルタの処理では、移動体１の姿勢は、クオータニオンの形態で表される。該クオータニオンが本発明における姿勢指標値に相当するものである。

以降の説明では、任意の変数名Ｑの先頭に「↑」を付した参照符号↑Ｑは、複数の成分から成るベクトル（縦ベクトル）を表し、任意の変数名Ｑの先頭に「#」を付した参照符号#Ｑは行列を表す。また、任意のベクトル↑Ｑ又は行列#Ｑの変数名の右上に添え字「Ｔ」を付したもの（↑Ｑ^T又は#Ｑ^T）は、該ベクトル↑Ｑ又は行列#Ｑの転置を表す。

また、４成分のベクトルとして表される任意のクオータニオン↑Ｑに対して、添え字「_hat」を付加したもの（↑Ｑ_hat）は、↑Ｑを正規化したクオータニオン（↑Ｑの各成分を↑Ｑの絶対値｜↑Ｑ｜で除算してなるクオータニオン）を意味する。また、任意のクオータニオン↑Ｑの右上に添え字「*」を付したもの（↑Ｑ^*）は、↑Ｑの共役クオータニオンを意味する。

また、スカラーと、スカラー又はベクトル又は行列との乗算、あるいは、ベクトル同士のスカラー積（内積）、あるいは、ベクトルと行列との乗算、あるいは、行列同士の乗算を表す算術記号として「・」を使用し、クオータニオン同士の乗算を表す算術記号として「＊」を使用する。

第３姿勢推定処理部２３が、Madgwickフィルタの処理で用いる損失関数Ｌは、以下に示す式（１ａ）～（１ｃ）により定義される関数↑ｆ、↑ｆg、↑ｆbを用いて、式（１）により定義される関数、すなわち、関数↑ｆ（＝[↑ｆb^T, ↑ｆb^T]^T）のノルム（絶対値）の2乗値（換言すれば、↑ｆのスカラー積（＝↑ｆ^T・↑ｆ））として定義される関数である。

Ｌ＝｜↑ｆ｜²＝↑ｆ^T・↑ｆ ……（１）
ただし、
↑ｆ＝[↑ｆg^T, ↑ｆb^T]^T ……（１ａ）
↑ｆg＝↑ｑ(S/E)_hat^*＊↑ｇ(E)＊↑ｑ(S/E)_hat－↑ａ(S)
……（１ｂ）
↑ｆb＝↑ｑ(S/E)_hat^*＊↑b(E)＊↑ｑ(S/E)_hat－↑ｍ(S)
……（１ｃ）

ここで、式（１ｂ），（１ｃ）に関して説明すると、↑ｑ(S/E)_hatは、移動体１の移動環境に設定されたグローバル座標系（地表座標系）Ｅから、センサユニット１０に対して設定されたセンサ座標系Ｓへの姿勢の回転を表す正規化されたクオータニオン（グローバル座標系Ｅで見たセンサ座標系Ｓの姿勢を表す正規化されたクオータニオン）、↑ｑ(S/E)_hat^*は、↑ｑ(S/E)_hatの共役クオータニオンである。該クオータニオン↑ｑ(S/E)_hatは、換言すれば、グローバル座標系Ｅで見た移動体１の姿勢を表す姿勢指標値としてのクオータニオンであり、以降、姿勢クオータニオン↑ｑ(S/E)_hatという。

この場合、本実施形態では、上記グローバル座標系Ｅは、例えば南北方向をＸ軸方向、東西方向をＹ軸方向、上下方向（鉛直方向）をＺ軸方向とする３軸直交座標系であり、上記センサ座標系Ｓは、Ｘ軸、Ｙ軸及びＺ軸の各座標軸の方向が磁気センサ１１、加速度センサ１２及び角速度センサ１３に対して固定された方向を有する３軸直交座標系である。そして、磁気センサ１１、加速度センサ１２及び角速度センサ１３によりそれぞれ検出される地磁気、加速度及び角速度はセンサ座標系Ｓで見た検出値である。

また、式（１ｂ）における↑ｇ(E)_hatは、グローバル座標系Ｅで見た重力加速度を表す正規化されたクオータニオン（＝[0, gx, gy, gz]^T）、↑ａ(S)_hatは、センサ座標系Ｓで見た加速度の検出値（加速度センサ１２による検出値）を表す正規化されたクオータニオン（＝[0, ax, ay, az]^T）である。

ここで、↑ｇ(E)_hatの第２成分（＝gx）、第３成分（＝gy）、及び第４成分（＝gz）は、それぞれ、グローバル座標系Ｅで見た重力加速度（正規化された重力加速度）のＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分を表し、↑ａ(S)_hatの第２成分（＝ax）、第３成分（＝ay）、及び第４成分（＝az）は、それぞれ、センサ座標系Ｓで見た加速度の検出値（加速度センサ１２による検出値）を正規化したもののＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分を表している。この場合、上記の如く各座標軸の方向が設定されたグローバル座標系Ｅで見た重力加速度は、Ｘ軸方向の成分とＹ軸方向の成分とがゼロになるベクトルであるので、↑ｇ(E)_hat＝[0, 0, 0, 1]^Tである。

また、式（１ｃ）における↑ｂ(E)_hatは、グローバル座標系Ｅで見た地磁気（一定値）を表す正規化されたクオータニオン（＝[0, bx, by, bz]^T）、↑ｍ(S)_hatは、センサ座標系Ｓで見た地磁気の検出値（磁気センサ１１による検出値）を表す正規化されたクオータニオン（＝[0, mx, my, mz]^T）である。

ここで、↑ｂ(E)_hatの第２成分（＝bx）、第３成分（＝by）、及び第４成分（＝bz）は、それぞれ、グローバル座標系Ｅで見た地磁気（正規化された地磁気）のＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分を表し、↑ｍ(S)_hatの第２成分（＝mx）、第３成分（＝my）、及び第４成分（＝mz）は、それぞれ、センサ座標系Ｓで見た地磁気の検出値（磁気センサ１１による検出値）を正規化したもののＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分を表している。この場合、上記の如く各座標軸の方向が設定されたグローバル座標系Ｅで見た地磁気は、Ｙ軸方向の成分がゼロとなるベクトルであるので、↑ｂ(E)_hat＝[0, bx, 0, bz]^Tである。

式（１ｂ）において、右辺の第１項は、グローバル座標系Ｅで見た重力加速度のベクトルをセンサ座標系Ｓに変換したベクトルを表すクオータニオン（正規化したクオータニオン）に相当する。そして、該クオータニオンは、移動体１に発生する加速度の定常状態での基準値（センサ座標系Ｓで見た基準値）としての意味を持つ。従って、式（１ｂ）により定義される関数↑ｆgは、移動体１に発生する加速度の基準値と検出値との偏差（基準値に対する検出値の残差）を表すクオータニオンである。

また、式（１ｃ）において、右辺の第１項は、グローバル座標系Ｅで見た地磁気（一定値の地磁気）のベクトルをセンサ座標系Ｓに変換したベクトルを表すクオータニオン（正規化したクオータニオン）に相当する。そして、該クオータニオンは、移動体１の存在位置での定常状態での地磁気の基準値（センサ座標系Ｓで見た基準値）としての意味を持つ。従って、式（１ｃ）により定義される関数↑ｆbは、移動体１の存在位置での地磁気の基準値と検出値との偏差（基準値に対する検出値の残差）を表すクオータニオンである。

本実施形態では、第３姿勢推定処理部２３でのMdgwickフィルタの処理では、上記関数↑ｆg、↑ｆbから式（１ａ）及び（１）により規定される損失関数Ｌを用い、この損失関数Ｌの値を最小化し得るように、前記姿勢クオータニオンｑ(S/E)_hatが推定される。このため、加速度センサ１２で検出される加速度の基準値に対する残差と、磁気センサ１１で検出される地磁気の基準値に対する残差との両方を最小化し得るように、姿勢クオータニオンｑ(S/E)_hatが推定される。

補足すると、関数↑ｆgは、加速度の検出値を表すクオータニオン↑ａ(S)をグローバル座標系Ｅで見たクオータニオンに変換したものと、グローバル座標系Ｅで見た重力加速度を示すクオータニオンとの偏差を表す関数として定義してもよい。同様に、関数↑ｆbは、地磁気の検出値を表すクオータニオン↑ｍ(S)をグローバル座標系Ｅで見たクオータニオンに変換したものと、グローバル座標系Ｅで見た一定値の地磁気との偏差を表す関数として定義してもよい。

第３姿勢推定処理部２３によるMadwickフィルタの処理では、上記損失関数Ｌを最小化し得るように姿勢クオータニオン↑ｑ(E/S)_hatを推定するための手法として、勾配法の手法が用いられる。この場合、第３姿勢推定処理部２３は、以下に示す式（２－１）の逐次演算により、正規化前の姿勢クオータニオンｑ(E/S)を逐次推定し、この姿勢クオータニオンｑ(E/S)から、式（２－２）により正規化された姿勢クオータニオンｑ(E/S)_hatの推定値を算出する。

↑ｑ(S/E) (t)＝↑ｑ(S/E) (t-1)＋↑ｑ(S/E) _dot(t)・Δt
……（２－１）
↑ｑ(S/E)_hat(t)＝↑ｑ(S/E)(t)／｜↑ｑ(S/E)(t)｜
……（２－２）
ただし、
↑ｑ(S/E)_ dot(t)＝↑ｑ(S/E)_ω_dot(t)－β・((∇Ｌ)^T／｜∇Ｌ｜)
……（２ａ）
↑ｑ(S/E)_ω_dot(t)＝(１／２)・↑ｑ(S/E) (t-1)・ω(S)(t)
……（２ｂ）

ここで、上記の式において、姿勢クオータニオンｑ(E/S)等の変数に付加した (t)と(t-1)とは、ｑ(E/S)の推定値を算出する各時刻（離散時間系での時刻）とその1つ前の時刻とを表し、Δｔはｑ(E/S)の推定を行う演算処理周期（時刻ｔ，ｔ－１の間の時間幅）を表している。

また、式（２－１）の右辺の↑ｑ(S/E)_dotは、姿勢クオータニオン↑ｑ(S/E)の微分値（時間的変化率）である。従って、式（２－１）は、姿勢クオータニオン↑ｑ(S/E)の微分値↑ｑ(S/E) _dot（以降、姿勢変化クオータニオン↑ｑ(S/E)_dotという）を逐次積分することで、姿勢クオータニオン↑ｑ(S/E)を逐次算出する演算を表している。

上記姿勢変化クオータニオン↑ｑ(S/E)_dotは、上記の式（２ａ）により算出される。この場合、式（２ａ）における↑ｑ(S/E)_ω_dotは、角速度センサ１３により検出される角速度から推定される姿勢変化クオータニオンを表している。そして、各時刻ｔでの姿勢変化クオータニオン↑ｑ(S/E)_ω_dot(t)は、該時刻ｔでの角速度の検出値を表すクオータニオン↑ω(s)(t)（＝[0, ωx, ωy, ωz]^T）と、時刻ｔ－１で求められた姿勢クオータニオン↑ｑ(S/E)_dotの推定値とから、上記の式（２ｂ）により算出される。なお、↑ω(s)(t)の第２成分（＝ωx）、第３成分（＝ωy）及び第４成分（＝ωz）は、それぞれ、センサ座標系Ｓで見た角速度の検出値のＸ軸周り方向の成分、Ｙ軸周り方向の成分、Ｚ軸周り方向の成分である。

また、式（２ａ）におけるβは、所定値の係数、∇Ｌは、正規化された姿勢クオータニオン↑ｑ(S/E)_hatに対する損失関数Ｌの勾配、すなわち、次式（３）により定義されるクオータニオンである。なお、ｑ1，ｑ2，ｑ3，ｑ4は、それぞれ、姿勢クオータニオン↑ｑ(S/E)_hatの第１成分、第２成分、第３成分、第４成分を表す変数である。以降、∇Ｌを勾配クオータニオンということがある。なお、本実施形態の説明では、∇Ｌは行ベクトルでる。

∇Ｌ＝[∂Ｌ／∂ｑ1，∂Ｌ／∂ｑ2，∂Ｌ／∂ｑ3，∂Ｌ／∂ｑ4]
……（３）

ここで、前記式（１）、（１ａ）～（１ｃ）により、∇Ｌは、次式（４）に書き換えることができる。

∇Ｌ＝２・（↑ｆg^T・#Ｊg＋↑ｆb^T・#Ｊb）……（４）
ただし、
#Ｊg＝∇↑ｆg
＝[∂↑ｆg／∂ｑ1, ∂↑ｆg／∂ｑ2, ∂↑ｆg／∂ｑ3, ∂↑ｆg／∂ｑ4]
……（４ａ）
#Ｊb＝∇↑ｆb
＝[∂↑ｆb／∂ｑ1, ∂↑ｆb／∂ｑ2, ∂↑ｆb／∂ｑ3, ∂↑ｆb／∂ｑ4]
……（４ｂ）

上記の式（４）における#Ｊg及び#Ｊbは、それぞれ、上記の式（４ａ），（４ｂ）により定義される行列である。これらの行列#Ｊg及び#Ｊbは、所謂、ヤコビアンである。なお、式（４ａ）における∂↑ｆg／∂ｑi（ｉ＝１，２，３，４）は、関数↑ｆgの各成分の偏微分値（姿勢クオータニオン↑ｑ(S/E)_hatの第ｉ成分ｑiに対する偏微分値）を並べた縦ベクトルである。同様に、式（４ｂ）における∂↑ｆb／∂ｑi（ｉ＝１，２，３，４）は、関数↑ｆbの各成分の偏微分値（姿勢クオータニオン↑ｑ(S/E)_hatの第ｉ成分に対する偏微分値）を並べた縦ベクトルである。

補足すると、クオータニオンとしての関数↑ｆg、↑ｆbのそれぞれの第１成分はゼロであるので、行列#Ｊg，#Ｊbのそれぞれの第１行はゼロベクトルになる。従って、関数↑ｆg、↑ｆbのそれぞれを第２成分、第３成分及び第４成分だけを抽出してなる３成分のベクトルとして扱うと共に、行列#Ｊg，#Ｊbのそれぞれを第２行、第３行及び第４行だけを抽出してなる３行４列の行列として扱うことも可能である。

第３姿勢推定処理部２３は、上記の式（２－１），（２－２），（２ａ），（２ｂ），（４），（４ａ），（４ｂ）に基づいて、正規化された姿勢クオータニオン↑ｑ(S/E)_hatの推定値を算出する処理（Madgwickフィルタの処理）を所定の演算処理周期で逐次実行する。

この場合、各演算処理周期において、姿勢クオータニオン↑ｑ(S/E)_hatの推定値の前回値（今回の演算処理の時刻ｔの1つ前の演算処理の時刻ｔ－１で求めた推定値↑ｑ(S/E)_hat(t-1)）と、加速度センサ１２による加速度の検出値の今回値（今回の演算処理の時刻ｔでの検出値）を表すクオータニオン↑ａ(S)_hat(t)と、グローバル座標系Ｅでの重力加速度（加速度の基準値）を表すクオータニオン↑ｇ(E)_hatと、磁気センサ１１による地磁気の検出値の今回値を表すクオータニオン↑ｍ(S)_hat(t)と、グローバル座標系Ｅでの地磁気（地磁気の基準値）を表すクオータニオン↑b(E)_hatとから、姿勢クオータニオン↑ｑ(S/E)_hatに対する損失関数Ｌの勾配を表す勾配クオータニオン∇Ｌ（今回値）が算出される。

より具体的には、本実施形態では、前記したように↑ｇ(E)_hat＝[0, 0, 0, 1]^T、↑ｂ(E)_hat＝[0, bx, 0, bz]^Tであるので、姿勢クオータニオン↑ｑ(S/E)_hatを[ｑ1, ｑ2, ｑ3, ｑ4]^T、加速度の検出値を表すクオータニオン↑ａ(S)_hatを[0, ａx, ａy, ａz]^T、地磁気の検出値を表すクオータニオン↑ｍ(S)_hatを[0, ｍx, ｍy, ｍz]^Tと表記すると、前記関数↑ｆg，↑ｆbは、前記式（１ｂ）、（１ｃ）に基づいて、次式（５ａ），（５ｂ）により表される。さらに、前記行列#Ｊg，#Ｊbは、前記式（４ａ），（４ｂ）に基づいて、次式（６ａ），（６ｂ）により表される。

従って、姿勢クオータニオン↑ｑ(S/E)_hatの推定値（前回値）と、加速度の検出値（今回値）を表すクオータニオン↑ａ(S)_hatとから式（５ａ）に基づいて、関数↑ｆgの値を算出し得る。同様に、姿勢クオータニオン↑ｑ(S/E)_hatの推定値（前回値）と、地磁気の検出値（今回値）を表すクオータニオン↑ｍ(S)_hatと、グローバル座標系Ｅでの地磁気を表すクオータニオン↑ｂ(E)_hatとから式（５ｂ）に基づいて、関数↑ｆbの値を算出し得る。

さらに、姿勢クオータニオン↑ｑ(S/E)_hatの推定値（前回値）から式（６ａ）に基づいて、行列#Ｊgを算出し得る。また、姿勢クオータニオン↑ｑ(S/E)_hatの推定値（前回値）と、グローバル座標系Ｅでの地磁気を表すクオータニオン↑ｂ(E)_hatとから式（６ｂ）に基づいて、行列#Ｊbを算出し得る。

そして、上記の如く算出された↑ｆg、#Ｊg、↑ｆb、#Ｊbから、前記式（４）に基づいて、勾配クオータニオン∇Ｌを算出することができる。なお、式（４）により算出される勾配クオータニオン∇Ｌは、行ベクトルである。

また、角速度センサ１３による角速度の検出値（今回値）を表すクオータニオン↑ω(S)から前記式（２ｂ）に基づいて、前記式（２ａ）の演算に使用しる姿勢変化クオータニオン↑ｑ(S/E)_ω_dotが算出される。そして、この↑ｑ(S/E)_ω_dotと、上記の如く算出される勾配クオータニオン∇Ｌとから、前記式（２ａ）の演算により、前記式（２－１）の演算に使用する姿勢変化クオータニオン↑ｑ(S/E)_ω_dotが算出される。

さらに、この姿勢変化クオータニオン↑ｑ(S/E)_ω_dotと、姿勢クオータニオン↑ｑ(S/E)の推定値の前回値とから、前記式（２－１）の演算を行うことで算出される姿勢クオータニオン↑ｑ(S/E)の今回値を、前記式（２－２）により正規化することで、正規化された姿勢クオータニオン↑ｑ(S/E)_hatの推定値（今回値）が新たに算出される。

第３姿勢推定処理部２３は、以上の如く、Madgwickフィルタの演算処理を逐次行うことで、グローバル座標系Ｅで見た移動体１の姿勢を表す指標値としての姿勢クオータニオン↑ｑ(S/E)_hatを逐次推定する。そして、第３姿勢推定処理部２３は、該姿勢クオータニオン↑ｑ(S/E)_hatにより示される移動体１の姿勢の推定値を第３推定姿勢として逐次出力するとと共に、その推定時刻を逐次出力する。

また、第３姿勢推定処理部２３は、上記の如く姿勢クオータニオン↑ｑ(S/E)_hatを逐次推定することに加えて、その推定値（第３推定姿勢）の確からしさに関連する状態量として、前記損失関数Ｌの二階微分（換言すれば、損失関数Ｌの勾配クオータニオン∇Ｌの勾配）を表すヘッセ行列#Ｈの固有値を逐次算出する。

このヘッセ行列#Ｈは、次式（７）により表される。

なお、上記のただし書きに記載した通り、本明細書では、×^dpはベクトル同士の直積を表す算術記号であり、∇は上記の式（７ａ）により表される演算子ベクトル（ここでは行ベクトル）である。また、∇Ｌは、前記式（３）により定義されるベクトル（行ベクトル）である。補足すると、∇を転置した縦ベクトル（＝∇^T）を１列の行列、∇Ｌを1行の行列とみなしたとき、∇×^dp∇Ｌは、１列の行列∇^Tと、１行の行列∇Ｌとの積（＝∇^T・∇Ｌ）に一致する。

ここで、上記の式（７）に、前記式（４）を適用すると、次式（８）が得られる。

従って、損失関数Ｌに関するヘッセ行列#Ｈは、加速度に関連する行列として上記の式（８ａ）により定義されるヘッセ行列#Ｈgと、地磁気に関連する行列として上記の式（８ｂ）により定義されるヘッセ行列#Ｈbとの総和の行列として表される。以降、#Ｈgを第１ヘッセ行列、#Ｈbを第２ヘッセ行列という。

そして、本実施形態では、第３姿勢推定処理部２３は、上記第１ヘッセ行列#Ｈgの固有値と、第２ヘッセ行列#Ｈｂの固有値とを前記したMadgwickフィルタの処理により逐次推定する姿勢クオータニオン↑ｑ(S?E)_hatの確からしさに関連する状態量Ｃとして逐次算出する。

この場合、第１ヘッセ行列#Ｈgは、姿勢クオータニオン↑ｑ(S/E)の推定値を算出する過程で前記した如く算出される関数↑ｆg及び行列#Ｊgの値と、前記式（６ａ）を用いて算出し得る∂(#Ｊg)／∂ｑi（ｉ＝１，２，３，４）の値（定数行列）とから、式（８ａ）に従って求めることができる。

同様に、第２ヘッセ行列#Ｈbは、姿勢クオータニオン↑ｑ(S/E)の推定値を算出する過程で前記した如く算出される関数↑ｆb及び行列#Ｊbの値と、前記式（６ｂ）を用いて算出し得る∂(#Ｊb)／∂ｑi（ｉ＝１，２，３，４）の値（定数行列）とから、式（８ｂ）に従って求めることができる。

そして、第３姿勢推定処理部２３は、このように求められる第１ヘッセ行列#Ｈg及び第２ヘッセ行列#Ｈbから、それらの固有値を算出し、該固有値を第３推定姿勢の確からしさに関連する状態量Ｃとして、移動体１の第３推定姿勢と共に逐次出力する。

ここで、前記第３姿勢推定処理部２３で求められる第３推定姿勢と、第１ヘッセ行列#Ｈg及び第２ヘッセ行列#Ｈbのそれぞれの固有値との関係に関する検証データに関して図３及び図４を参照して補足しておく。

図３の上段のグラフは、移動体１の第３推定姿勢のうちの例えばヨー方向（グローバル座標系ＥのＺ軸周りの方向）の姿勢角の推定値の誤差の経時変化の実測データを例示するグラフ、下段のグラフは、第１ヘッセ行列#Ｈgの固有値の経時変化の実測データを例示するグラフである。この場合、ヨー方向の姿勢角の誤差が増加しており、それに伴い、第１ヘッセ行列#Ｈgの固有値は、その変動が拡大していくように変化する傾向があることが判る。

また、図４の上段のグラフは、移動体１のヨー方向の姿勢角の推定値の誤差の経時変化の実測データを例示するグラフ、下段のグラフは、第２ヘッセ行列#Ｈbの固有値の経時変化の実測データを例示するグラフである。この場合、ヨー方向の姿勢角の推定値の誤差の変動が大きい状況で、第２ヘッセ行列#Ｈbの固有値の変動も大きくなる傾向があることが判る。

このように、第１ヘッセ行列#Ｈg及び第２ヘッセ行列#Ｈbのそれぞれの固有値は、第３姿勢推定処理部２３がMadgwickフィルタの処理を用いて推定する移動体１の姿勢の確からしさと相関性を有することが判る。

図２に戻って、第１～第３姿勢推定処理部２１～２３からそれぞれ出力される前記状態量Ａ，Ｂ，Ｃは、重み係数学習処理部２４に入力される。この重み係数学習処理部２４は、本実施形態では、ニューラルネットワークにより構成される。そして、このニューラルネットワークは、前記状態量Ａ，Ｂ，Ｃから構成される特徴ベクトルをマルチモーダル情報として用いるマルチモーダル学習があらかじめ実施されている。

このマルチモーダル学習は、より詳しくは、種々様々な状態量Ａ，Ｂ，Ｃから構成される特徴ベクトルから、第１～第３姿勢推定処理部２１～２３のそれぞれのアルゴリズムによる移動体１の姿勢の推定処理が、他のアルゴリズムに比してどの程度適切になされているかの度合いに相当する重み係数（第１～第３姿勢推定処理部２１～２３から出力される推定姿勢のそれぞれに対応する重み係数）を生成する学習である。

重み係数学習処理部２４のニューラルネットワークは、このようなマルチモーダル学習があらかじめ実施されている。このため、第１～第３姿勢推定処理部２１～２３のそれぞれから状態量Ａ，Ｂ，Ｃが入力された重み係数学習処理部２４は、該状態量Ａ，Ｂ，Ｃにより構成される特徴ベクトルから、第１～第３姿勢推定処理部２１～２３のそれぞれに対応する重み係数をニューラルネットワークにより生成して出力する。

この場合、当該ニューラルネットワークは、基本的には、第１～第３姿勢推定処理部２１～２３のそれぞれが出力する移動体１の推定姿勢のそれぞれの確からしさがより高いものほど、重み係数が大きな値となるように第１～第３姿勢推定処理部２１～２３のそれぞれに対応する重み係数を生成して出力する。

重み係数学習処理部２４から出力される第１～第３姿勢推定処理部２１～２３のそれぞれに対応する重み係数は、姿勢特定処理部２５に入力される。また、該姿勢特定処理部２５には、第１～第３姿勢推定処理部２１～２３のそれぞれから、移動体１の推定姿勢及び推定時刻が入力される。

そして、姿勢特定処理部２５は、第１～第３姿勢推定処理部２１～２３のそれぞれの互いに同一時刻での推定位置姿勢を、入力された重み係数に応じて合成してなる姿勢を、移動体１の姿勢として特定する。すなわち、第１～第３姿勢推定処理部２１～２３のそれぞれの推定姿勢に対しそれぞれに対応する重み係数を乗じたものを合成してなる姿勢が移動体１の姿勢として特定される。なお、このとき各重み係数の和は１である。

本実施形態では、姿勢推定処理装置２０は、以上説明した如く移動体１の姿勢を逐次特定する。この場合、第１～第３姿勢推定処理部２１～２３のそれぞれにより推定された姿勢を、重み係数学習処理部２４により第１～第３姿勢推定処理部２１～２３のそれぞれ毎の状態量Ａ，Ｂ，Ｃに応じて決定される重み係数により合成することによって移動体１の姿勢が特定される。

そして、この場合、重み係数学習処理部２４は、第１～第３姿勢推定処理部２１～２３のそれぞれ毎の状態量Ａ，Ｂ，Ｃを入力情報とするものであるので、移動体１の種々様々な移動環境で、状態量Ａ，Ｂ，Ｃに応じて出力すべき重み係数を事前に学習しておくことができる。そのため、姿勢推定処理装置２０は、移動体１の種々様々な移動環境で、移動体１の姿勢を高い信頼性で推定（特定）することができる。

なお、本発明は以上説明した実施形態に限定されるものではなく、他の実施形態を採用することもできる。以下に、他の実施形態をいくつか例示する。

前記実施形態では、Madgwickフィルタの処理により移動体１の姿勢を推定する第３姿勢推定処理部２３は、該姿勢の推定値としての第３推定姿勢の確からしさに関連する状態量Ｃの要素として、加速度に関連する第１ヘッセ行列#Ｈgの固有値と、地磁気に関連する第１ヘッセ行列#Ｈgの固有値との両方を出力するようにしたが、いずれか一方の固有値だけを状態量Ｃの要素として出力してもよい。

また、第３姿勢推定処理部２３の処理（Madgwickフィルタの処理）では、損失関数Ｌは、前記式（１）により定義される関数に限らず、例えば、加速度に関連する前記関数↑ｆgのノルム（絶対値）の２乗値として表される関数（＝｜↑ｆg｜²）、あるいは、地磁気に関連する前記関数↑ｆbgのノルム（絶対値）の２乗値として表される関数（＝｜↑ｆb｜²）を、損失関数Ｌとして用いてもよい。

損失関数Ｌとして｜↑ｆg｜²を使用する場合には、磁気センサ１１を省略できる。そして、この場合、姿勢クオータニオン↑ｑ(S/E)を逐次推定する処理では、損失関数Ｌの勾配クオータニオン∇Ｌは、前記式（４）の右辺の括弧内の第２項を除去した式に基づいて算出することができる。

また、損失関数Ｌとして｜↑ｆb｜²を使用する場合には、加速度センサ１２を省略できる。そして、この場合、姿勢クオータニオン↑ｑ(S/E)を逐次推定する処理では、損失関数Ｌの勾配クオータニオン∇Ｌは、前記式（４）の右辺の括弧内の第１項を除去した式に基づいて算出することができる。

また、第３姿勢推定処理部２３が求める第３推定姿勢の確からしさに関連する状態量Ｃは、第１ヘッセ行列#Ｈgの固有値と、第２ヘッセ行列#Ｈbの固有値との両方又はいずれか一方に加えて、他の要素、例えば第３推定姿勢の姿勢角の大きさ、あるいは、該姿勢角の時間的変化率（角速度）を含んでいてもよい。

また、前記実施形態では、第１～第３姿勢推定処理部２１～２３の３種類の姿勢推定処理部を備える姿勢推定処理装置２０を例示した。ただし、姿勢推定処理装置２０は、例えば、第１及び第２姿勢推定処理部２１，２２のうちのいずれか一方を備えないものであってもよい。あるいは、姿勢推定処理装置２０は、さらに多くの姿勢推定処理部を備えていてもよい。

また、前記実施形態では、重み係数学習処理部２４として、マルチモーダル学習を行うニューラルネットワークを例示した。ただし、重み係数学習処理部２４として、例えば、LSTM（Long short-term memory）等の再帰型ニューラルネットワーク（RNN）等を使用してもよい。

さらに、前記実施形態では、第３姿勢推定処理部２３を含む複数の姿勢推定処理部を含むシステムを例示した。ただし、例えば第１及び第２姿勢推定処理部２１，２２を備えずに、第３姿勢推定処理部２３から出力される第３推定姿勢と状態量Ｃとに応じて移動体１の移動制御を行うようにしてもよい。この場合、状態量Ｃにより示される第３推定姿勢の確からしさ（信頼度）が低い場合に、該確からしさが高い場合よりも、移動体１の移動を制限する等の制御態様を採用し得る。

また、前記実施形態では、移動体として、車輪型の移動体１を例示した。ただし、本発明の適用対象の移動体は、車輪型の移動体に限らず、例えば、脚式の移動体、倒立振子型の移動体等であってよい。

１…移動体、５…オドメトリ用センサ、６…レーザ・レンジ・ファインダ（センサ）、７…カメラ（センサ）、１１…磁気センサ、１２…加速度センサ、１３…角速度センサ。

Claims (3)

1. 移動体に発生する角速度の検出値と、該移動体に発生する加速度及び該移動体の存在位置での地磁気のうちの少なくとも一方からなる検出対象量の検出値と用いて該移動体の姿勢を推定する方法であって、
   前記角速度の検出値と前記検出対象量の検出値とからMadgwickフィルタにより、前記検出対象量の検出値と該検出対象量の基準値との偏差の大きさを表す所定の損失関数を最小化するように、前記移動体の姿勢の推定値を表す姿勢指標値を逐次求める第１ステップと、
   前記損失関数の前記姿勢指標値に対する２階偏微分値を成分として構成されるヘッセ行列を求め、当該求めたヘッセ行列の固有値を前記姿勢指標値の確からしさに関連する状態量として生成する第２ステップとを備えることを特徴とする移動体の姿勢推定方法。
2. 請求項１記載の移動体の姿勢推定方法において、
   前記検出対象量は、前記加速度及び前記地磁気の両方から成り、前記ヘッセ行列は、前記加速度に関連する第１ヘッセ行列と前記地磁気に関連する第２ヘッセ行列との和として表される行列であり、前記第2ステップで生成される状態量は、前記第１ヘッセ行列の固有値と前記第２ヘッセ行列の固有値とのうちの少なくとも一方を含むことを特徴とする移動体の姿勢推定方法。
3. 請求項１又は２記載の移動体の姿勢推定方法において、
   前記角速度及び前記検出対象量をそれぞれ検出可能なセンサを含む複数のセンサのうちの1つ以上のセンサの検出値を用いて、前記Madgwickフィルタと異なる1つ以上のアルゴリズムにより前記移動体の姿勢を逐次推定する処理を各アルゴリズ毎に実行する第３ステップと、
   前記各アルゴリズムにより得られた前記移動体の姿勢の推定値の確からしさに関連する状態量を当該各アルゴリズム毎に生成する第４ステップと、
   前記Madgwickフィルタに対応して前記第２ステップで生成された状態量と前記各アルゴリズムに対応して前記第４ステップで生成された状態量とを学習済のニューラルネットワークに入力し、当該入力された状態量から該ニューラルネットワークにより前記Madgwickフィルタ及び前記各アルゴリズムのそれぞれ毎の重み係数を決定する第５ステップと、
   前記姿勢指標値により示される姿勢の推定値と、前記各アルゴリズムのそれぞれにより得られた姿勢の推定値とを、前記決定された重み係数により合成して得られる姿勢を、前記移動体の姿勢として特定する第６ステップとを備えることを特徴とする移動体の姿勢推定方法。

Images