(57)【特許請求の範囲】
【請求項１】
少なくとも加速度を含む計測値を取得し、
前記計測値に含まれるドリフトを除去するために補正し、
第１の画像と前記第１の画像とは取得時が異なる第２の画像を取得し、
第１の画像及び第２の画像それぞれに含まれる特徴点を検出し、
前記第１の画像上の特徴点に対応する対応点を、前記第２の画像上の特徴点の中から探索し、
前記補正された計測値を用いて、移動体の位置および姿勢を計算し、
前記移動体の位置および姿勢を用いて推定される、前記第２の画像上の対応点を前記第１の画像上へ投影した場合の前記第１の画像上での位置と、実際の前記第１の画像上の特徴点の位置との差である再投影誤差を計算し、
前記移動体の位置および姿勢の尤度と、前記再投影誤差を計算するために用いられる前記第１の画像上の特徴点の３次元位置の尤度とを推定し、
前記移動体の位置および姿勢の尤度と前記第１の画像上の特徴点の３次元位置の尤度とから、前記移動体の位置および姿勢と前記第１の画像上の特徴点の３次元位置とを推定し、
推定された前記移動体の位置および姿勢と前記第１の画像上の特徴点の３次元位置とから、前記第１の画像の取得時と前記第２の画像の取得時との間の前記移動体の相対的な移動量を推定する、
処理回路を具備する情報処理装置。
【請求項２】
前記処理回路は、
前記推定された前記移動体の相対的な移動量と、前記補正された計測値と、前記計測値の補正に用いられたドリフト量とを用いて、次に取得される計測値の補正に用いられる、前記取得された計測値のドリフト量を推定する請求項１に記載の情報処理装置。
【請求項３】
前記処理回路は、前記計測値の補正に用いられたドリフト量と、前記推定されたドリフト量との差に基づいて、前記計測値を計測するセンサが故障しているか否かを判定する請求項１に記載の情報処理装置。
【請求項４】
前記処理回路は、前記計測値を計測するセンサが故障していると判定した場合、判定結果を出力する請求項３に記載の情報処理装置。
【請求項５】
少なくとも加速度を含む計測値を取得することと、
前記計測値に含まれるドリフトを除去するために補正することと、
第１の画像と前記第１の画像とは取得時が異なる第２の画像を取得することと、
第１の画像及び第２の画像それぞれに含まれる特徴点を検出することと、
前記第１の画像上の特徴点に対応する対応点を、前記第２の画像上の特徴点の中から探索することと、
前記補正された計測値を用いて、移動体の位置および姿勢を計算することと、
前記移動体の位置および姿勢を用いて推定される、前記第２の画像上の対応点を前記第１の画像上へ投影した場合の前記第１の画像上での位置と、実際の前記第１の画像上の特徴点の位置との差である再投影誤差を計算することと、
前記移動体の位置および姿勢の尤度と、前記再投影誤差を計算するために用いられる前記第１の画像上の特徴点の３次元位置の尤度とを推定することと、
前記移動体の位置および姿勢の尤度と前記第１の画像上の特徴点の３次元位置の尤度とから、前記移動体の位置および姿勢と前記第１の画像上の特徴点の３次元位置とを推定することと、
推定された前記移動体の位置および姿勢と前記第１の画像上の特徴点の３次元位置とから、前記第１の画像の取得時と前記第２の画像の取得時との間の前記移動体の相対的な移動量を推定することと、
を具備する方法。
【請求項６】
コンピュータに、
少なくとも加速度を含む計測値を取得することと、
前記計測値に含まれるドリフトを除去するために補正することと、
第１の画像と前記第１の画像とは取得時が異なる第２の画像を取得することと、
第１の画像及び第２の画像それぞれに含まれる特徴点を検出することと、
前記第１の画像上の特徴点に対応する対応点を、前記第２の画像上の特徴点の中から探索することと、
前記補正された計測値を用いて、移動体の位置および姿勢を計算することと、
前記移動体の位置および姿勢を用いて推定される、前記第２の画像上の対応点を前記第１の画像上へ投影した場合の前記第１の画像上での位置と、実際の前記第１の画像上の特徴点の位置との差である再投影誤差を計算することと、
前記移動体の位置および姿勢の尤度と、前記再投影誤差を計算するために用いられる前記第１の画像上の特徴点の３次元位置の尤度とを推定することと、
前記移動体の位置および姿勢の尤度と前記第１の画像上の特徴点の３次元位置の尤度とから、前記移動体の位置および姿勢と前記第１の画像上の特徴点の３次元位置とを推定することと、
推定された前記移動体の位置および姿勢と前記第１の画像上の特徴点の３次元位置とから、前記第１の画像の取得時と前記第２の画像の取得時との間の前記移動体の相対的な移動量を推定することと、
を実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明の実施形態は、情報処理装置および方法に関する。
【背景技術】
【０００２】
移動体の自己運動を推定する技術は、自己位置の推定を目的として、自動車、船舶、飛行機などに応用されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１４−２４０２６６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
移動体の移動量を精度よく求めることができる情報処理装置、方法およびプログラムを提供することである。
【課題を解決するための手段】
【０００５】
実施形態によれば、情報処理装置は、処理回路を具備する。処理回路は、少なくとも加速度を含む計測値を取得し、前記計測値に含まれるドリフトを除去するために補正し、第１の画像と前記第１の画像とは取得時が異なる第２の画像を取得し、第１の画像及び第２の画像それぞれに含まれる特徴点を検出し、前記第１の画像上の特徴点に対応する対応点を、前記第２の画像上の特徴点の中から探索し、前記補正された計測値を用いて、移動体の位置および姿勢を計算し、前記移動体の位置および姿勢を用いて推定される、前記第２の画像上の対応点を前記第１の画像上へ投影した場合の前記第１の画像上での位置と、実際の前記第１の画像上の特徴点の位置との差である再投影誤差を計算し、前記移動体の位置および姿勢の尤度と、前記再投影誤差を計算するために用いられる前記第１の画像上の特徴点の３次元位置の尤度とを推定し、前記移動体の位置および姿勢の尤度と前記第１の画像上の特徴点の３次元位置の尤度とから、前記移動体の位置および姿勢と前記第１の画像上の特徴点の３次元位置とを推定し、推定された前記移動体の位置および姿勢と前記第１の画像上の特徴点の３次元位置とから、前記第１の画像の取得時と前記第２の画像の取得時との間の前記移動体の相対的な移動量を推定する。
【図面の簡単な説明】
【０００６】
【図１】第１実施形態の情報処理装置の構成の一例を示すブロック図。
【図２】第１実施形態の情報処理装置が備える運動推定機能の構成の一例を示すブロック図。
【図３】第１実施形態の情報処理装置が実行する処理の流れの一例を示すフローチャート。
【図４】第１実施形態の情報処理装置が実行する処理の中の運動推定処理の流れの一例を示すフローチャート。
【図５】第１実施形態の情報処理装置により推定された運動推定誤差の評価の一例を示す図。
【図６】第２実施形態の情報処理装置の構成の一例を示すブロック図。
【図７】第２実施形態の情報処理装置が実行する処理の流れの一例を示すフローチャート。
【発明を実施するための形態】
【０００７】
以下、実施形態について図面を参照して説明する。
（第１実施形態）
図１は、本実施形態の情報処理装置１０の構成の一例を示すブロック図である。情報処理装置１０は、移動体１の自己位置を推定する運動推定装置として用いられる。自己位置とは、移動体１の位置および姿勢である。移動体１は、例えば車両（自動車、二輪車等）、船舶、飛行機、ドローンである。
【０００８】
移動体１には、慣性センサ２０と、撮像装置３０とが搭載されている。本実施形態では、慣性センサ２０と、撮像装置３０がそれぞれ１つずつ設けられている例を述べるが、それぞれ複数設けられてもよい。
慣性センサ２０としては、加速度センサ、角速度センサ、姿勢センサ、方位センサ、速度センサなどを用いてよい。本実施形態では、３軸加速度センサ（加速度センサ２０Ａ）と、３軸角速度センサ（角速度センサ２０Ｂ）を用いる例について説明を行う。以下、加速度センサ２０Ａおよび角速度センサ２０Ｂを慣性センサ２０と総称することがある。したがって、慣性センサ２０の計測値として、ｘ，ｙ，ｚ方向の加速度成分と、ｘ，ｙ，ｚ軸周りの回転速度成分とを取得する。なお、たとえば移動体１の移動が平面移動のみである場合などにおいては、加速度センサ２０Ａのみであってもよい。
【０００９】
また、撮像装置３０としては、可視光カメラ、赤外光カメラ、紫外光カメラなどを用いてよいが、ここでは、透視投影の可視光カメラを用いる例について説明を行う。
情報処理装置１０は、加速度センサ２０Ａおよび角速度センサ２０Ｂによる計測値と、撮像装置３０による撮像画像とを用いて、移動体１の自己運動を推定する。自己運動とは、移動体１の位置および姿勢の移動である。この自己運動を累積することで、移動体１の自己位置を推定することが可能である。
【００１０】
慣性センサ２０は、振動や熱などに起因する誤差によってドリフトが発生することが知られている。したがって、慣性センサ２０の計測値のみを用いて、移動体１の自己位置を推定する場合、微小なドリフトであっても、累積することで誤差が増大し、自己位置の誤差が大きくなってしまう。  
このような事象に対処するため、撮像装置３０による撮像画像を用いて、移動体１の自己運動を別途推定し、慣性センサ２０による計測値を用いて推定された移動体１の自己運動との比較により、慣性センサ２０のドリフト量を推定することも考えられ得る。しかしながら、撮像装置３０による撮像画像のみを用いた自己運動推定の結果は、常に信頼できるものではなく、撮像画像から特徴点が検出できない状況では、誤差が大きくなることが知られている。大きい誤差を含んだ自己運動推定結果を用いて慣性センサ２０のドリフト量を推定すると、ドリフト推定結果の誤差も大きくなる。たとえば、移動体１が自動車であって、トンネル内を走行中などといった、撮像装置３０による撮像画像を用いた自己運動推定が困難な状況（撮像画像から特徴点が検出しづらい状況）においては、前述の方策の場合、大きい誤差を含んで推定されたドリフト量を用いてドリフト補正を行うこととなるために、最終的な自己位置の誤差が大きくなってしまう。
【００１１】
そこで、情報処理装置１０は、慣性センサ２０による計測値と、撮像装置３０による撮像画像とを用いて、移動体１の自己運動を推定するとともに、慣性センサ２０のドリフト量を推定する。これにより、撮像装置３０による撮像画像を用いた自己運動推定が困難な状況下においても、ドリフト推定の誤差が増大することを抑え、自己位置推定の誤差を小さくすることができるようにした。以下、この点について詳述する。
【００１２】
情報処理装置１０は、たとえば、専用または汎用のコンピュータであって、処理回路１００、記憶回路２００、通信回路３００、各回路間を接続するバス４００を備える。
処理回路１００は、取得機能１１０、補正機能１２０、検出機能１３０、探索機能１４０、運動推定機能１５０、ドリフト推定機能１６０を備える。これらの各処理機能については後述する。これらの各処理機能は、コンピュータによって実行可能なプログラムの形態で記憶回路２００へ記憶されている。処理回路１００は、プログラムを記憶回路２００から読み出し、実行することで、各プログラムに対応する処理機能を実現するプロセッサである。各プログラムを読み出した状態の処理回路１００は、図１の処理回路１００内に示された各処理機能を有することになる。なお、処理回路１００は、単一のプロセッサで構成されるものであってもよいし、複数の独立したプロセッサを組み合わせて構成されるものであってもよい。たとえば、複数の独立したプロセッサを組み合わせて処理回路１００を構成し、各プロセッサがプログラムを実行することにより、図１の処理回路１００内に示された各処理機能を実現するものとしてもよい。また、たとえば、特定の処理機能が専用の独立したプロセッサに実装されてもよい。
【００１３】
ここで用いた「プロセッサ」という文言は、たとえば、ＣＰＵ（Central Processing Unit）、ＧＰＵ（Graphical Processing Unit）、特定用途向け集積回路（Application Specific Integrated Circuit：ＡＳＩＣ）、プログラマブル論理デバイス（たとえば、単純プログラマブル論理デバイス（Simple Programmable Logic Device：ＳＰＬＤ）、複合プログラマブル論理デバイス（Complex Programmable Logic Device：ＣＰＬＤ）、フィールドプログラマブルゲートアレイ（Field Programmable Gate Array：ＦＰＧＡ）など）などの回路を意味する。プロセッサは、記憶回路２００に保存されたプログラムを読み出し、実行することで、処理機能を実現する。なお、記憶回路２００にプログラムを保存する代わりに、プロセッサの回路内にプログラムを直接組み込むよう構成しても構わない。この場合、プロセッサは、回路内に組み込まれたプログラムを読み出し、実行することで、処理機能を実現する。
【００１４】
記憶回路２００は、処理回路１００が行う各処理機能に伴うデータ等を必要に応じて記憶する。たとえば、記憶回路２００は、ＲＡＭ（Random Access Memory）やフラッシュメモリなどの半導体メモリ素子、ハードディスク、光ディスクなどである。記憶回路２００は、ＬＡＮ（Local Area Network）やインターネットなどを介してダウンロードされたプログラムを記憶または一時記憶した記憶媒体であってもよい。また、記憶媒体は１つに限られず、複数の記憶媒体から、処理回路１００の各処理機能が実現される場合も、本実施形態における記憶媒体に含まれ、記憶媒体の構成は、いずれの構成であってもよい。
【００１５】
通信回路３００は、有線または無線で接続された外部装置と情報の入出力を行うインターフェースである。通信回路３００は、ネットワークに接続して通信を行ってもよい。
ここで、まず、処理回路１００が備える各処理機能について概略的に説明する。
取得機能１１０は、慣性センサ２０、すなわち、加速度センサ２０Ａおよび角速度センサ２０Ｂで計測されたドリフトを含む計測値と、撮像装置３０で撮像された画像とを取得する。
【００１６】
補正機能１２０は、取得機能１１０により取得された計測値を、その計測値に含まれるドリフトを除去するために補正する。この補正は、後述するドリフト推定機能１６０により推定されたドリフト量が存在する場合、そのドリフト量を用いて実行される。存在しない場合には、予め設定されたドリフト量を用いて実行される。
【００１７】
検出機能１３０は、取得機能１１０により取得された画像から特徴点を検出する。特徴点とは、エッジ点やコーナ点などであり、複数の画像間で対応が取りやすい箇所を意味する。基本的には、複数の特徴点が検出されることが想定されている。
探索機能１４０は、あるタイミングで取得された画像（第１の画像）から検出された特徴点に対応する対応点を、その画像が取得されたタイミングとは異なるタイミングで取得された画像（第２の画像）上において探索する。より詳細には、第１の画像上の特徴点に対応する対応点を、第２の画像から検出された特徴点の中から探索する。第１の画像が取得されるタイミングと第２の画像が取得されるタイミングとの間隔は、たとえば、撮像装置３０が画像を撮像する間隔である。第１の画像と第２の画像とが撮像されるタイミングはどちらが先であっても構わない。本実施形態では、第１の画像が先に撮像され、第２の画像が後に撮像される例について説明を行う。
【００１８】
運動推定機能１５０は、補正機能１２０により補正された計測値と、第１の画像上の特徴点と、第２の画像上の対応点とを用いて、第１の画像の取得時と第２の画像の取得時との間における移動体１の相対的な移動量を推定する。運動推定機能１５０については、その構成をさらに後述する。運動推定機能１５０により推定された移動量は、処理回路１００の処理結果として出力される（自己移動量）。
【００１９】
ドリフト推定機能１６０は、運動推定機能１５０により推定された移動量と、補正機能１２０により補正された計測値とを用いて、計測値に含まれるドリフト量を推定する。ドリフト推定機能１６０により推定されたドリフト量は、取得機能１１０により次に取得される計測値に対する補正機能１２０による補正に用いられる。
【００２０】
図２は、運動推定機能１５０の構成の一例を示すブロック図である。
運動推定機能１５０は、運動方程式計算機能１５１、投影誤差計算機能１５２、尤度推定機能１５３、位置・姿勢推定機能１５４を備える。ここでは、まず、運動推定機能１５０が備える各処理機能について概略的に説明する。
【００２１】
運動方程式計算機能１５１は、補正機能１２０により補正された計測値を運動方程式に代入することによって、移動体１の位置、姿勢、速度を計算する。位置は、移動体１の位置の移動量を示す。姿勢は、移動体１の姿勢の移動量を示す。移動体１の自己位置は、位置および姿勢の移動量を累積することで推定することが可能である。
投影誤差計算機能１５２は、運動方程式計算機能１５１により計算された位置および姿勢を用いて推定される、探索機能１４０により探索された第２の画像上の対応点を第１の画像上へ投影した場合の第１の画像上での位置（２次元位置）と、実際の第１の画像上の特徴点の位置（２次元位置）との差（再投影誤差）を計算する。
【００２２】
尤度推定機能１５３は、位置および姿勢と再投影誤差とのそれぞれに対して設定された誤差モデルを用いて、運動方程式計算機能１５１により計算された位置および姿勢の尤度と、投影誤差計算機能１５２により（再投影誤差を計算するために）計算された第１の画像上の特徴点の３次元位置の尤度とを推定する。
【００２３】
位置・姿勢推定機能１５４は、尤度推定機能１５３により推定される尤度が最大となるときの移動体１の位置および姿勢を推定する。
図３は、処理回路１００が実行する処理の流れの一例を示すフローチャートである。図３を参照しながら、処理回路１００が備える各処理機能について詳細に説明する。
【００２４】
取得機能１１０は、撮像装置３０で撮像された画像を取得する（ステップＡ１）。取得機能１１０は、たとえば画像の取得と並行して、加速度センサ２０Ａで計測された（ドリフトを含む）加速度と、角速度センサ２０Ｂで計測された（ドリフトを含む）角速度とを取得する（ステップＡ５）。図３では、ステップＡ１〜ステップＡ４とステップＡ５〜ステップＡ６とを並列処理として示したが、順次処理としてもよい。
【００２５】
検出機能１３０は、取得された画像から特徴点を検出する（ステップＡ２）。特徴点の検出には、たとえば、Ｈａｒｒｉｓオペレータによるコーナ検出、Ｋｉｒｓｃｈオペレータによるエッジ検出などが利用できる。
取得機能１１０により取得され、検出機能１３０により特徴点が検出された画像が１枚目の画像であった場合（ステップＡ３，ＹＥＳ）、ステップＡ１の取得機能１１０による画像の取得と、ステップＡ５の同じく取得機能１１０による加速度および角速度の取得とに戻る。言うまでもないが、ここでの説明に用いている第１の画像とは、１枚目の画像を含み得るが、１枚目の画像とは意味が異なる。
【００２６】
１枚目の画像でない場合（ステップＡ３，ＮＯ）、探索機能１４０は、最も新しく取得された画像（第２の画像）上において、その１つ前に取得された画像（第１の画像）から検出された特徴点に対応する対応点を探索する（ステップＡ４）。対応点の探索には、たとえば、テンプレートマッチングによる対応づけ、ＳＵＲＦ特徴量、ＳＩＦＴ特徴量、ＡＫＡＺＥ特徴量などの特徴量に基づく対応づけなどが利用できる。
【００２７】
また、補正機能１２０は、取得された加速度および角速度を、その加速度および角速度に含まれるドリフトを除去するために補正する（ステップＡ６）。より詳細には、補正機能１２０は、慣性センサ２０の計測値から、後述するステップＡ８でドリフト推定機能１６０により推定されたドリフト量を減算することによって、慣性センサ２０の計測値に含まれるドリフトを除去する。ステップＡ８のドリフト推定機能１６０によるドリフトの推定が１度も実行されていない状況下においては、補正機能１２０は、予め設定されたドリフト量を利用する。
【００２８】
運動推定機能１５０は、補正機能１２０により補正された加速度および角速度と、検出機能１３０により検出された第１の画像上の特徴点と、探索機能１４０により探索された第２の画像上の対応点とを用いて、第１の画像の取得時と第２の画像の取得時との間の移動体１の相対的な移動量を推定する（ステップＡ７）。
【００２９】
図４は、図３のステップＡ７の運動推定機能１５０による運動推定処理の流れの一例を示すフローチャートである。図４を参照しながら、図２に示した、運動推定機能１５０が備える各処理機能について詳細に説明する。
運動方程式計算機能１５１は、加速度と角速度とから、移動体１の位置、速度、姿勢を計算する（ステップＢ１）。第１の画像が取得されたときの移動体１の位置、速度、姿勢をそれぞれｔ（１）、ｖ（１）、Ｒ（１）とし、第２の画像が取得されたときの移動体１の位置、速度、姿勢をそれぞれｔ（２）、ｖ（２）、Ｒ（２）とする。ここで、ｔ（・）とｖ（・）とは３次元ベクトルであり、Ｒ（・）は３×３行列である。加速度をａ、角速度をωと定義する。ここで、ａとωとは３次元ベクトルである。ｔ（２）、ｖ（２）、Ｒ（２）は、式（１）〜式（３）によって計算できる。
【数１】
0006702543.tif 000002
【数２】
0006702543.tif 000003
【数３】
0006702543.tif 000004
ここで、ｇは重力加速度、τは第１の画像の取得時刻と第２の画像の取得時刻との間の時間、Ωは角速度ωを回転行列表現したものである。ｔ（１）、ｖ（１）、Ｒ（１）が未知の場合、ｔ（１）＝（０，０，０）Ｔ、ｖ（１）＝（０，０，０）Ｔ、Ｒ（１）＝Ｉ（単位行列）を用いる。ｔ（１）、ｖ（１）、Ｒ（１）が未知の場合とは、処理回路１００が処理を開始する際の初期状態の場合である。
【００３０】
投影誤差計算機能１５２は、ｔ（１）、Ｒ（１）、ｔ（２）、Ｒ（２）を用いて、第１の画像上の特徴点と第２の画像上の対応点とに関する再投影誤差を計算する（ステップＢ２）。第１の画像上における特徴点の位置を（ｘ１，ｙ１）、第２の画像上における対応点の位置を（ｘ２，ｙ２）とする。特徴点の３次元位置（Ｘ，Ｙ，Ｚ）が既知である場合、第２の画像上の対応点の第１の画像上への再投影位置（ｘ，ｙ）は、式（４）〜式（５）を用いて計算できる。
【数４】
0006702543.tif 000005
【数５】
0006702543.tif 000006
【００３１】
再投影位置を用いることで、再投影誤差が計算でき、たとえば、第１の画像上における特徴点の位置に対する再投影誤差（誤差ｅ）は、式（６）で計算できる。
【数６】
0006702543.tif 000007
【００３２】
特徴点の３次元位置が未知の場合、２枚の画像における特徴点位置と画像間の相対的な位置・姿勢が既知であるとき、特徴点の３次元位置は、三角測量によって推定することができる。特徴点の３次元位置が未知の場合とは、その特徴点が初めて出現した（検出された）場合である。２枚の画像間の相対的な位置、姿勢の変化量をｔ、Ｒとする。ここで、ｔ＝（ｔｘ，ｔｙ，ｔｚ）Ｔの３次元ベクトルであり、３×３行列Ｒの第１行をｒ１、第２行をｒ２、第３行をｒ３とする。特徴点の３次元位置（Ｘ，Ｙ，Ｚ）は、式（７）〜式（９）によって計算できる。
【数７】
0006702543.tif 000008
【数８】
0006702543.tif 000009
【数９】
0006702543.tif 000010
ここで、ｏ１＝（ｘ１，ｘｙ，１）Ｔである。
【００３３】
尤度推定機能１５３は、運動方程式計算機能１５１により計算された移動体１の位置および姿勢の尤度と、投影誤差計算機能１５２により計算された特徴点の３次元位置の尤度とを推定する（ステップＢ３）。
前述したように、再投影誤差（誤差ｅ）は、式（６）で計算できる。特徴点の３次元位置の尤度Ｌは、この誤差ｅを用いて、式（１０）で計算される。
【数１０】
0006702543.tif 000011
ここで、Σは特徴点の２次元位置の誤差分散である。移動体１の位置と姿勢とを１つの行列に纏めたものポーズと呼ぶ。ポーズは、式（１１）で定義される。
【数１１】
0006702543.tif 000012
【００３４】
移動体１の位置、姿勢に対する推定値をそれぞれｔ´、Ｒ´とし、式（１２）を定義する。
【数１２】
0006702543.tif 000013
【００３５】
これらを用いて、ポーズに対する誤差ｅｐは、式（１３）で計算できる。
【数１３】
0006702543.tif 000014
ここで、Ｒｅは姿勢の誤差、ｔｅは位置の誤差を意味する。姿勢誤差Ｒｅについて、回転行列表現から回転ベクトル表現にしたものをｒｅとする。これらを用いて、ポーズに対する尤度Ｌｐは、式（１４）で計算される。
【００３６】
【数１４】
0006702543.tif 000015
ここで、Σｔは位置の誤差共分散、Σｒは姿勢の誤差共分散である。
【００３７】
位置・姿勢推定機能１５４は、尤度推定機能１５３により推定される尤度が最大となるような移動体１の位置および姿勢を推定する（ステップＢ４）。
これには、ガウス・ニュートン法やレーベンバーグ・マーカート法が利用できる。ここでは、ガウス・ニュートン法を利用した場合について説明するが、レーベンバーグ・マーカート法や共役勾配法などを用いても良い。検出機能１３０では、Ｎ個の特徴点が検出される。１つの特徴点に対して式（１０）が１つ定義できるため、Ｎ個の特徴点に対してはＮ個の式（１０）が定義でき、それらを式（１５）のように表す。
【数１５】
0006702543.tif 000016
ここで、１≦ｉ≦Ｎである。このとき、式（１６）を最大化する問題は、式（１７）を最小化する問題と等価となり、ガウス・ニュートン法を適用できる。
【数１６】
0006702543.tif 000017
【数１７】
0006702543.tif 000018
【００３８】
移動体１の位置、姿勢に対する推定値ｔ´、Ｒ´と、特徴点の３次元位置に対する推定値（Ｘｉ´，Ｙｉ´，Ｚｉ´）（１≦ｉ≦Ｎ）を１組のパラメータｘとすると、ガウス・ニュートン法では、パラメータを、適切な更新量δｘを用いて、ｘからｘ＋δｘへと更新して、パラメータが収束するまで反復する。式（１７）において、ｔｅとｒｅとは３次元ベクトルであり、ｅｉは２次元ベクトルである。これを１つの組として、（６＋２＊Ｎ）次元ベクトルを構成し、これをｚとする。ｚのヤコビ行列は、式（１８）で計算できる。
【数１８】
0006702543.tif 000019
【００３９】
このヤコビ行列を用いて、前述のパラメータ更新量δｘを式（１９）で計算する。
【数１９】
0006702543.tif 000020
【００４０】
式（１９）で計算されたパラメータ更新量δｘを用いてパラメータを更新することで、移動体１の位置および姿勢と特徴点の３次元位置とを同時推定する。更新された移動体１の位置および姿勢と特徴点の３次元位置を用いて、式（１７）を再計算し、Ｅが設定値よりも小さくなったときに収束と判定する。収束していない場合は、ガウス・ニュートン法を再度実行する。収束の判定として、パラメータ更新量δｘの大きさを利用することもでき、その大きさが設定値よりも小さくなったときに収束と判定する。
【００４１】
再び、図３を参照する。
ドリフト推定機能１６０は、運動推定機能１５０により推定された移動体１の位置および姿勢と、補正機能１２０により補正された加速度および角速度とを用いて、第２のドリフト量を推定する（ステップＡ８）。式（１）または式（２）においてａ＝ａ´＋ｅａ、式（３）においてω＝ω´＋ｅωと定義する。ここで、ｅａとｅωとはそれぞれ加速度の誤差と角速度の誤差とであり、補正機能１２０において補正し切れなかった成分を意味する。式（１）または式（２）と、式（３）とを用いて、ｅａとｅωとを計算する。補正機能１２０において利用されたドリフト量に、このｅａ、ｅωを加えたものを第２のドリフト量とする。
【００４２】
ステップＡ７の運動推定機能１５０による推定と、ステップＡ８のドリフト推定機能１６０による推定とは、交互に繰り返すことによって精度が高まる。処理回路１００は、補正機能１２０によって補正された加速度、角速度からｅａ、ｅωだけを減算し、ステップＡ７を実施して、ステップＡ７で得られた移動体１の位置および姿勢を用いて、ステップＡ８にてドリフトの誤差を計算する。精度が高まると、繰り返しによる変化量が小さくなる。変化量が設定された閾値よりも小さくなったとき、処理回路１００は、推定が収束したと判定する（ステップＡ９，ＹＥＳ）。換言すると、変化量が設定された閾値よりも小さくなっていないときには（ステップＡ９，ＮＯ）、処理回路１００は、ステップＡ７へ戻る。
【００４３】
推定が収束したと判定した場合、処理回路１００は、処理の終了が指示されていなければ（ステップＡ１０，ＮＯ）、ステップＡ１の取得機能１１０による画像の取得と、ステップＡ５の同じく取得機能１１０による加速度および角速度の取得とに戻る。一方、処理の終了が指示されている場合（ステップＡ１０，ＹＥＳ）、処理回路１００は、処理を終了する。
【００４４】
情報処理装置１０による効果を図５に示す。図５は、ＩＭＵ（Inertial Measurement Unit）と撮像装置とを搭載した自動車の走行中に取得された画像列と加速度および角速度の計測値とを用いて、情報処理装置１０が運動推定した際の誤差の一例を表す。ＩＭＵは、慣性センサのユニットであって、加速度センサや角速度センサが入ったユニットの総称である。図５中、横軸は画像のフレーム番号、縦軸は位置誤差［メートル］である。フレーム番号０において、撮像装置による特徴点検出が不可能になり、その状態から撮像装置による特徴点検出が可能になるまでにｎフレーム経過していることを想定している。ｎは０、２０、４０、６０、８０として実験を行った。参考として、ＩＭＵのみを用いるデッドレコニング法と比較している。図５によると、ｎ≦６０であれば、デッドレコニング法よりも平均的に小さい誤差を維持することができている。なお、撮像装置のフレームレートは１０ｆｐｓであるため、ｎ＝６０の場合、実時間における６秒経過を意味する。
【００４５】
このように、情報処理装置１０は、慣性センサ２０による計測値と、撮像装置３０による撮像画像とを用いて、また、尤度に基づいて、移動体１の自己運動を推定するとともに、慣性センサ２０のドリフト量を推定することで、撮像装置３０により撮像された画像による運動推定が不可能な状況に陥った場合においても、ドリフト推定の誤差が増大することを抑え、自己位置推定の誤差を小さくすることを可能とする。
【００４６】
（第２実施形態）
次に、第２実施形態について説明する。なお、第１実施形態と同様の動作をする構成や処理には共通の符号を付して、重複する説明は省略する。
図６は、情報処理装置１０の構成の一例を示すブロック図である。前述した第１実施形態との違いは、本実施形態では、処理回路１００が、故障判定機能１７０をさらに備える点にある。
【００４７】
情報処理装置１０は、慣性センサ２０の故障により慣性センサ２０の計測値が異常となった場合、ドリフト推定機能１６０によるドリフト量の推定値に現れる点に着目し、ドリフト推定機能１６０によるドリフト量の推定値を慣性センサ２０の故障判定に利用する。すなわち、故障判定機能１７０は、ドリフト推定機能１６０によるドリフト量の推定値を用いて、慣性センサ２０の故障判定を行う。
【００４８】
図７は、処理回路１００が実行する処理の流れの一例を示すフローチャートである。図７を参照しながら、処理回路１００が備える故障判定機能１７０について説明する。
図７中、ステップＣ１〜ステップＣ９は、第１実施形態で説明した図３中のステップＡ１〜ステップＡ９に相当する処理であるので、説明を省略する。
【００４９】
ステップＣ７の運動推定機能１５０による推定と、ステップＣ８のドリフト推定機能１６０による推定とが収束した場合（ステップＣ９，ＹＥＳ）、故障判定機能１７０は、ステップＣ６で補正機能１２０により用いられたドリフト量と、ステップＣ８でドリフト推定機能１６０により推定されたドリフト量（第２のドリフト量）との差分を計算し（ステップＣ１０）、設定された閾値との比較を行う（ステップＣ１１）。
【００５０】
差分が閾値より大きい場合（ステップＣ１１，ＹＥＳ）、故障判定機能１７０は、慣性センサ２０が故障したと判定する（ステップＣ１２）。故障判定機能１７０よる慣性センサ２０の故障判定結果は、処理回路１００の処理結果として出力される（故障情報）。出力は、例えば、表示装置への表示を行う、または警告音を鳴らさせる等であってもよい。慣性センサ２０が故障したと判定された場合、処理回路１００は、処理を終了する。
【００５１】
差分が閾値以下の場合は（ステップＣ１１，ＮＯ）、故障判定機能１７０により慣性センサ２０が故障したと判定されることはない。この場合、処理回路１００は、処理の終了が指示されていなければ（ステップＣ１３，ＮＯ）、ステップＣ１の取得機能１１０による画像の取得と、ステップＣ５の同じく取得機能１１０による加速度および角速度の取得とに戻る。一方、処理の終了が指示されている場合（ステップＣ１３，ＹＥＳ）、処理回路１００は、処理を終了する。
【００５２】
このように、情報処理装置１０は、ドリフト推定機能１６０によるドリフト量の推定値を慣性センサ２０の故障判定に利用することで、慣性センサ２０の故障判定を適切に行うことを可能とする。
なお、実施形態におけるコンピュータは、記憶媒体に記憶されたプログラムに基づき、実施形態における各処理を実行するためのものであって、パーソナルコンピュータ、マイクロコンピュータなどの１つからなる装置、複数の装置がネットワーク接続されたシステムなどのいずれの構成であってもよい。
【００５３】
また、実施形態におけるコンピュータとは、パーソナルコンピュータ、情報処理機器に含まれる演算処理装置、マイクロコンピュータなど、プログラムによって実施形態における機能を実現することが可能な機器、装置を総称している。
本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。
【符号の説明】
【００５４】
１…移動体、１０…情報処理装置、２０…慣性センサ、２０Ａ…加速度センサ、２０Ｂ…角速度センサ、３０…撮像装置、１００…処理回路、１１０…取得機能、１２０…補正機能、１３０…検出機能、１４０…探索機能、１５０…運動推定機能、１５１…運動方程式計算機能、１５２…投影誤差計算機能、１５３…尤度推定機能、１５４…位置・姿勢推定機能、１６０…ドリフト推定機能、１７０…故障判定機能。
【図１】
0006702543.tif 000021
【図２】
0006702543.tif 000022
【図３】
0006702543.tif 000023
【図４】
0006702543.tif 000024
【図５】
0006702543.tif 000025
【図６】
0006702543.tif 000026
【図７】
0006702543.tif 000027