(57)【特許請求の範囲】
【請求項１】
移動体の加速度を含む観測情報を観測する内界センサと、
前記観測情報から、前記移動体の第１の移動量を導出する第１の導出部と、
外界センサで観測した前記移動体の周辺情報に基づいて、前記移動体の第２の移動量を導出する第２の導出部と、
前記第２の移動量の信頼度を算出する信頼度算出部と、
前記信頼度が特定の基準を満たす場合、前記第１の移動量および前記第２の移動量に基づいて前記移動体の現在の移動量を算出し、前記信頼度が前記基準を満たさない場合、前記第１の移動量を前記現在の移動量として算出する、移動量算出部と、
を備え、
前記周辺情報は、前記移動体の周辺の撮影画像および距離情報の少なくとも一方であり、
前記信頼度算出部は、前記第２の移動量の導出に用いた第１のタイミングで観測された前記周辺情報における対象点と、第２のタイミングで観測された前記周辺情報に基づいて前記第１のタイミングにおける前記対象点を推定した推定点と、の誤差に基づいて、前記信頼度を算出する、
情報処理装置。
【請求項２】
前記第２の導出部は、
前記第１のタイミングで観測された前記撮影画像における前記対象点、および、前記第２のタイミングで観測された前記撮影画像における前記対象点に対応する対応点を、前記外界センサを配置した仮想三次元空間に投影し、前記仮想三次元空間に投影された前記対応点を前記第１のタイミングで観測された前記撮影画像に再投影することによって前記推定点を求め、前記仮想三次元空間に配置された前記外界センサの姿勢および位置を変えながら、前記第１のタイミングで観測された前記撮影画像における前記対象点と対応する前記推定点との差である再投影誤差が設定値以下となるまで、前記投影および前記再投影を繰り返し、該再投影誤差が前記設定値以下となった時に設定していた前記第２のタイミングの前記外界センサの位置および姿勢に対する、前記第１のタイミングの前記外界センサの位置および姿勢の変化量を、前記第２の移動量として導出し、
前記信頼度算出部は、
前記設定値以下となったときの前記再投影誤差である前記誤差に基づいて、前記信頼度を算出する、
請求項１に記載の情報処理装置。
【請求項３】
前記第２の導出部は、
前記第１のタイミングで観測された前記距離情報における前記対象点を検出し、前記第２のタイミングで観測された前記距離情報における、前記対象点の各々に対応する対応点を探索し、前記第２のタイミングで観測された前記距離情報における前記対応点を、前記第１のタイミングで観測された前記距離情報上に配置し、配置した前記対応点と対応する前記対象点との差から該対象点の前記推定点を求め、該対象点と前記推定点との推定誤差を導出し、
前記信頼度算出部は、
前記推定誤差である前記誤差に基づいて、前記信頼度を算出する、
請求項１に記載の情報処理装置。
【請求項４】
前記信頼度算出部は、
複数の前記対象点と複数の前記推定点との複数の前記誤差の内、特定の条件を満たす前記誤差の数を、前記信頼度として算出する、請求項１に記載の情報処理装置。
【請求項５】
前記特定の条件は、前記誤差が基準値未満の値であることを示す、請求項４に記載の情報処理装置。
【請求項６】
前記信頼度算出部は、
前記対象点と対応する前記推定点との特徴量の差が基準値以下の前記対象点と対応する前記推定点との対の数を、前記信頼度として算出する、
請求項２に記載の情報処理装置。
【請求項７】
前記信頼度算出部は、
前記第２の移動量と前記第１の移動量との差に基づいて、前記信頼度を算出する、
請求項１に記載の情報処理装置。
【請求項８】
前記第２の導出部は、
前記外界センサから前記周辺情報を取得する毎に前記第２の移動量を導出し、
前記信頼度算出部は、
前記周辺情報の取得に前回失敗してから前記周辺情報を今回取得するまでに、前記周辺情報の取得を連続して成功した回数を、前記信頼度として算出する、
請求項１に記載の情報処理装置。
【請求項９】
前記移動量算出部は、
前記信頼度が閾値以上または閾値未満のときに、前記信頼度が前記特定の基準を満たすと判断する、請求項１に記載の情報処理装置。
【請求項１０】
前記移動体に搭載された、請求項１に記載の情報処理装置。
【請求項１１】
前記外界センサは、距離センサおよび撮影装置の少なくとも一方を含む、請求項１に記載の情報処理装置。
【請求項１２】
前記内界センサは、前記移動体の環境に非依存の前記観測情報を観測する、請求項１に記載の情報処理装置。
【請求項１３】
前記内界センサは、慣性計測装置である、請求項１に記載の情報処理装置。
【請求項１４】
前記現在の移動量に関する情報を含む出力情報を出力する出力制御部を備える、請求項１に記載の情報処理装置。
【請求項１５】
内界センサで観測された、移動体の加速度を含む観測情報から、前記移動体の第１の移動量を導出する第１の導出ステップと、
外界センサで観測した前記移動体の周辺情報に基づいて、前記移動体の第２の移動量を導出する第２の導出ステップと、
前記第２の移動量の信頼度を算出する信頼度算出ステップと、
前記信頼度が特定の基準を満たす場合、前記第１の移動量および前記第２の移動量に基づいて前記移動体の現在の移動量を算出し、前記信頼度が前記基準を満たさない場合、前記第１の移動量を前記現在の移動量として算出する移動量算出ステップと、
を含み、
前記周辺情報は、前記移動体の周辺の撮影画像および距離情報の少なくとも一方であり、
前記信頼度算出ステップは、
前記第２の移動量の導出に用いた第１のタイミングで観測された前記周辺情報における対象点と、第２のタイミングで観測された前記周辺情報に基づいて前記第１のタイミングにおける前記対象点を推定した推定点と、の誤差に基づいて、前記信頼度を算出する、
情報処理方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明の実施の形態は、情報処理装置、および、情報処理方法に関する。
【背景技術】
【０００２】
移動体の現在の移動量を算出する技術が知られている。例えば、特許文献１には、慣性センサにより提供される情報と、カメラにより得られる情報と、を用いて、進んだコースや進んだ距離を計算することが開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１４−２２２５５０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ここで、移動体の環境によって、カメラにより得られる情報の精度が低下する場合がある。しかし、従来では、この点を考慮した技術は開示されていなかった。すなわち、従来では、移動体の現在の移動量の算出精度が低下する場合があった。
【０００５】
本発明が解決しようとする課題は、移動体の移動量の算出精度の低下を抑制することができる、情報処理装置、および、情報処理方法を提供することである。
【課題を解決するための手段】
【０００６】
実施の形態の情報処理装置は、内界センサと、第１の導出部と、第２の導出部と、信頼度算出部と、移動量算出部と、を備える。内界センサは、移動体の加速度を含む観測情報を観測する。第１の導出部は、前記観測情報から、前記移動体の第１の移動量を導出する。第２の導出部は、外界センサで観測した前記移動体の周辺情報に基づいて、前記移動体の第２の移動量を導出する。信頼度算出部は、前記第２の移動量の信頼度を算出する。移動量算出部は、前記信頼度が特定の基準を満たす場合、前記第１の移動量および前記第２の移動量に基づいて前記移動体の現在の移動量を算出し、前記信頼度が前記基準を満たさない場合、前記第１の移動量を前記現在の移動量として算出する。前記周辺情報は、前記移動体の周辺の撮影画像および距離情報の少なくとも一方であり、前記信頼度算出部は、前記第２の移動量の導出に用いた第１のタイミングで観測された前記周辺情報における対象点と、第２のタイミングで観測された前記周辺情報に基づいて前記第１のタイミングにおける前記対象点を推定した推定点と、の誤差に基づいて、前記信頼度を算出する。
【図面の簡単な説明】
【０００７】
【図１】情報処理装置の一例を示す図。
【図２】情報処理装置の構成の一例を示すブロック図。
【図３】第２の移動量の導出の説明図。
【図４】第２の移動量の導出の説明図。
【図５】第２の移動量の導出の説明図。
【図６】出力情報の一例を示す模式図。
【図７】処理回路が実行する処理の手順の一例を示すフローチャート。
【図８】第２の移動量の導出処理の手順の一例を示すフローチャート。
【図９】ハードウェア構成図。
【発明を実施するための形態】
【０００８】
以下に添付図面を参照して、情報処理装置、および、情報処理方法を詳細に説明する。
【０００９】
図１は、本実施の形態の情報処理装置１０の一例を示す図である。情報処理装置１０は、移動体の現在の移動量を算出する。
【００１０】
本実施の形態では、情報処理装置１０が移動体１２に搭載されている場合を、一例として説明する。
【００１１】
移動体１２とは、移動可能な物である。図１に示す例では、移動体１２は、矢印Ｘ方向に走行する。移動体１２は、例えば、車両、台車、飛行可能な物体（有人飛行機、無人飛行機（例えば、ドローン））、ロボット、などである。本実施の形態では、移動体１２が車両である場合を一例として説明する。車両は、例えば、二輪自動車、三輪自動車、四輪自動車などである。本実施の形態では、車両が、四輪自動車である場合を一例として説明する。
【００１２】
移動体１２の移動量とは、移動体１２の、速度の変化量、加速度の変化量、姿勢の変化量、位置の変化量（移動距離）、の少なくとも１つを含む。位置の変化量は、例えば、並進ベクトルで表す。姿勢の変化量は、例えば、回転行列、ロール・ピッチ・ヨー角、回転ベクトル、オイラー角、クォータニオン、などで表すことができる。本実施の形態では、移動量を、移動体１２の位置の変化量と、移動体１２の姿勢の変化量と、で表す場合を、一例として説明する。
【００１３】
情報処理装置１０は、処理回路１０Ａと、出力回路１０Ｃと、外界センサ１０Ｇと、内界センサ１０Ｊと、を備える。
【００１４】
内界センサ１０Ｊは、観測情報を観測するセンサである。内界センサ１０Ｊは、移動量の算出対象の移動体１２に搭載されている。このため、内界センサ１０Ｊは、移動体１２の環境に非依存であり、環境変動に拘らず、安定して観測情報を観測する。
【００１５】
観測情報は、少なくとも移動体１２の加速度を含む情報である。具体的には、観測情報は、移動体１２の加速度、移動体１２の速度、移動体１２の角速度、の少なくとも１つを含む。
【００１６】
内界センサ１０Ｊは、例えば、慣性計測装置（ＩＭＵ：Ｉｎｅｒｔｉａｌ  Ｍｅａｓｕｒｅｍｅｎｔ  Ｕｎｉｔ）、加速度センサ、速度センサ、ロータリエンコーダ、などである。
【００１７】
本実施の形態では、内界センサ１０Ｊが、ＩＭＵである場合を一例として説明する。このため、本実施の形態では、内界センサ１０Ｊは、移動体１２の三軸加速度および三軸角速度を含む観測情報を観測する。
【００１８】
外界センサ１０Ｇは、移動体１２の周辺情報を観測する。外界センサ１０Ｇは、移動量の算出対象の移動体１２に搭載されていてもよいし、該移動体１２の外部（例えば、他の移動体や外部装置など）に搭載されていてもよい。
【００１９】
周辺情報は、移動量の算出対象の移動体１２の周辺の情報を含む、周辺の状況を示す情報である。移動量の算出対象の移動体１２の周辺とは、該移動体１２から予め定めた範囲内の領域である。この範囲は、外界センサ１０Ｇの観測可能な範囲である。この範囲は、予め設定すればよい。
【００２０】
具体的には、周辺情報は、移動体１２の周辺の撮影画像および距離情報の少なくとも一方である。なお、周辺情報は、移動体１２の位置情報を含んでいてもよい。撮影画像は、撮影によって得られる撮影画像データである（以下、単に、撮影画像と称する）。距離情報は、移動体１２から対象までの距離を示す情報である。対象は、外界における、外界センサ１０Ｇによって観測可能な箇所である。位置情報は、相対位置であってもよいし、絶対位置であってもよい。
【００２１】
外界センサ１０Ｇは、例えば、撮影によって撮影画像を得る撮影装置、距離センサ（ミリ波レーダ、レーザセンサ、距離画像センサ）、位置センサ（ＧＮＳＳ（Ｇｌｏｂａｌ  Ｎａｖｉｇａｔｉｏｎ  Ｓａｔｅｌｌｉｔｅ  Ｓｙｓｔｅｍ）、無線通信装置）などである。
【００２２】
本実施の形態では、外界センサ１０Ｇが、撮影画像を周辺情報として得る撮影装置である場合を一例として説明する。
【００２３】
なお、外界センサ１０Ｇは、所定タイミングごとに周辺情報を観測し、観測した周辺情報を、順次、処理回路１０Ａへ出力する。同様に、内界センサ１０Ｊは、所定のタイミングごとに、観測情報を観測し、観測した観測情報を、順次、処理回路１０Ａへ出力する。外界センサ１０Ｇが周辺情報を観測し処理回路１０Ａへ出力するタイミングと、内界センサ１０Ｊが観測情報を観測し処理回路１０Ａへ出力するタイミングとは、毎回同じであってもよいし、異なるタイミングを含むものであってもよい。なお、異なるタイミングを含む場合でも、少なくとも予め定めた間隔ごとに、同じタイミングで観測された周辺情報および観測情報が、処理回路１０Ａへ出力されればよい。
【００２４】
本実施の形態では、一例として、外界センサ１０Ｇおよび内界センサ１０Ｊの各々は、同じタイミングで周辺情報および観測情報を観測し、同じタイミングで処理回路１０Ａへ出力する場合を一例として説明する。
【００２５】
処理回路１０Ａは、外界センサ１０Ｇで観測した周辺情報（本実施の形態では撮影画像）と、内界センサ１０Ｊで観測した観測情報と、に基づいて、移動体１２の現在の移動量を算出する。処理回路１０Ａの詳細は後述する。
【００２６】
出力回路１０Ｃは、出力情報を出力する。出力情報は、移動体１２の現在の移動量に関する情報を含む。出力情報については、後述する。
【００２７】
出力回路１０Ｃは、出力情報を表示する表示機能、出力情報を示す音を出力する音出力機能、出力情報を送信する通信機能、などを備える。例えば、出力回路１０Ｃは、通信回路１０Ｄと、ディスプレイ１０Ｅと、スピーカ１０Ｆと、を含む。本実施の形態では、出力回路１０Ｃは、移動体１２のドライバに向かって出力情報を出力可能な位置に設けられていることが好ましい。
【００２８】
通信回路１０Ｄは、出力情報などの情報を他の装置へ送信する。例えば、通信回路１０Ｄは、移動体１２のドライバの有する端末装置へ、公知の通信回線を介して出力情報を送信する。ディスプレイ１０Ｅは、出力情報を表示する。ディスプレイ１０Ｅは、例えば、公知のＬＣＤ（ｌｉｑｕｉｄ  ｃｒｙｓｔａｌ  ｄｉｓｐｌａｙ）や投影装置やライトなどである。スピーカ１０Ｆは、出力情報を示す音を出力する。
【００２９】
情報処理装置１０の構成について詳細に説明する。図２は、情報処理装置１０の構成の一例を示すブロック図である。
【００３０】
情報処理装置１０は、例えば、専用または汎用コンピュータである。情報処理装置１０は、処理回路１０Ａと、記憶回路１０Ｂと、出力回路１０Ｃと、外界センサ１０Ｇと、内界センサ１０Ｊと、入力装置１０Ｈと、を備える。出力回路１０Ｃは、上述したように、通信回路１０Ｄと、ディスプレイ１０Ｅと、スピーカ１０Ｆと、を含む。
【００３１】
処理回路１０Ａ、記憶回路１０Ｂ、出力回路１０Ｃ、外界センサ１０Ｇ、内界センサ１０Ｊ、および、入力装置１０Ｈは、バス１０Ｉを介して接続されている。
【００３２】
なお、記憶回路１０Ｂ、出力回路１０Ｃ（通信回路１０Ｄ、ディスプレイ１０Ｅ、スピーカ１０Ｆ）、外界センサ１０Ｇ、内界センサ１０Ｊ、および、入力装置１０Ｈは、有線または無線で処理回路１０Ａに接続すればよい。また、記憶回路１０Ｂ、出力回路１０Ｃ（通信回路１０Ｄ、ディスプレイ１０Ｅ、スピーカ１０Ｆ）、外界センサ１０Ｇ、内界センサ１０Ｊ、および入力装置１０Ｈの少なくとも１つと、処理回路１０Ａと、を、ネットワークを介して接続してもよい。
【００３３】
なお、上述したように、少なくとも内界センサ１０Ｊは、移動量の算出対象の移動体１２に搭載されている必要がある。一方、内界センサ１０Ｊ以外の構成要素（処理回路１０Ａ、記憶回路１０Ｂ、出力回路１０Ｃ（通信回路１０Ｄ、ディスプレイ１０Ｅ、スピーカ１０Ｆ）、外界センサ１０Ｇ、および入力装置１０Ｈ）の少なくとも１つは、移動体１２の外部に搭載されていてもよい。
【００３４】
本実施の形態では、処理回路１０Ａ、記憶回路１０Ｂ、出力回路１０Ｃ、外界センサ１０Ｇ、内界センサ１０Ｊ、および入力装置１０Ｈが、移動体１２に搭載されている形態を一例として説明する。
【００３５】
入力装置１０Ｈは、ユーザからの各種指示や情報入力を受け付ける。入力装置１０Ｈは、例えば、マウスやトラックボール等のポインティングデバイス、あるいはキーボード等の入力デバイスである。
【００３６】
記憶回路１０Ｂは、各種データを記憶する。記憶回路１０Ｂは、例えば、ＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）、フラッシュメモリ等の半導体メモリ素子、ハードディスク、光ディスク等である。なお、記憶回路１０Ｂは、情報処理装置１０の外部に設けられた記憶装置であってもよい。また、記憶回路１０Ｂは、記憶媒体であってもよい。具体的には、記憶媒体は、プログラムや各種情報を、ＬＡＮ（Ｌｏｃａｌ  Ａｒｅａ  Ｎｅｔｗｏｒｋ）やインターネットなどを介してダウンロードして記憶または一時記憶したものであってもよい。また、記憶回路１０Ｂを、複数の記憶媒体から構成してもよい。
【００３７】
次に、処理回路１０Ａについて説明する。処理回路１０Ａは、導出機能１０Ｋと、導出機能１０Ｓと、信頼度算出機能１０Ｌと、移動量算出機能１０Ｍと、出力制御機能１０Ｎと、変更機能１０Ｑと、を備える。
【００３８】
処理回路１０Ａにおける各処理機能は、コンピュータによって実行可能なプログラムの形態で記憶回路１０Ｂへ記憶されている。処理回路１０Ａは、プログラムを記憶回路１０Ｂから読出、実行することで、各プログラムに対応する機能を実現するプロセッサである。
【００３９】
各プログラムを読み出した状態の処理回路１０Ａは、図２の処理回路１０Ａ内に示された各機能を有することになる。図２においては単一の処理回路１０Ａによって、導出機能１０Ｋ、導出機能１０Ｓ、信頼度算出機能１０Ｌ、移動量算出機能１０Ｍ、出力制御機能１０Ｎ、および、変更機能１０Ｑが実現されるものとして説明する。
【００４０】
なお、各機能の各々を実現するための独立した複数のプロセッサを組み合わせて処理回路１０Ａを構成してもよい。この場合、各プロセッサがプログラムを実行することにより各機能を実現する。また、各処理機能がプログラムとして構成され、１つの処理回路が各プログラムを実行する場合であってもよいし、特定の機能が専用の独立したプログラム実行回路に実装される場合であってもよい。
【００４１】
なお、本実施の形態において用いる「プロセッサ」との文言は、例えば、ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）、ＧＰＵ（Ｇｒａｐｈｉｃａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）或いは、特定用途向け集積回路（Ａｐｐｌｉｃａｔｉｏｎ  Ｓｐｅｃｉｆｉｃ  Ｉｎｔｅｇｒａｔｅｄ  Ｃｉｒｃｕｉｔ：ＡＳＩＣ）、プログラマブル論理デバイス（例えば、単純プログラマブル論理デバイス（Ｓｉｍｐｌｅ  Ｐｒｏｇｒａｍｍａｂｌｅ  Ｌｏｇｉｃ  Ｄｅｖｉｃｅ：ＳＰＬＤ）、複合プログラマブル論理デバイス（Ｃｏｍｐｌｅｘ  Ｐｒｏｇｒａｍｍａｂｌｅ  Ｌｏｇｉｃ  Ｄｅｖｉｃｅ：ＣＰＬＤ）、およびフィールドプログラマブルゲートアレイ（Ｆｉｅｌｄ  Ｐｒｏｇｒａｍｍａｂｌｅ  Ｇａｔｅ  Ａｒｒａｙ：ＦＰＧＡ））の回路を意味する。
【００４２】
プロセッサは、記憶回路１０Ｂに保存されたプログラムを読み出し実行することで機能を実現する。なお、記憶回路１０Ｂにプログラムを保存する代わりに、プロセッサの回路内にプログラムを直接組み込むよう構成しても構わない。この場合、プロセッサは回路内に組み込まれたプログラムを読み出し実行することで機能を実現する。
【００４３】
導出機能１０Ｓは、第１の導出部の一例である。導出機能１０Ｓは、内界センサ１０Ｊによって観測された観測情報に基づいて、移動体１２の移動量を導出する。なお、導出機能１０Ｓが導出する移動量を、第１の移動量と称して説明する。
【００４４】
第１の移動量とは、前回の第１の移動量の導出に用いた観測情報の観測のタイミングから、今回の第１の移動量の導出に用いた観測情報の観測のタイミングまでの期間の、移動体１２の移動量を示す。なお、第１の移動量は、今回の第１の移動量の導出に用いた観測情報の観測のタイミングにおける、単位時間あたりの移動体１２の移動量であってもよい。
【００４５】
本実施の形態では、導出機能１０Ｓは、内界センサ１０Ｊの観測情報から、移動体１２の位置の変化量と、移動体１２の姿勢の変化量と、を含む第１の移動量を導出する。
【００４６】
ここで、上述したように、本実施の形態では、内界センサ１０Ｊは、移動体１２の三軸加速度および三軸角速度を含む観測情報を観測する。観測情報に含まれる、ｘ軸，ｙ軸，ｚ軸の各々の加速度を、ａｘ，ａｙ，ａｚと定義する。また、観測情報に含まれる、ｘ軸，ｙ軸，ｚ軸の各々の角速度を、ωｘ，ωｙ，ωｚと定義する。そして、これらの３軸の加速度を示す加速度ベクトルａを、下記式（１）で表す。また、これらの３軸の角速度を示す角速度ベクトルωを、下記式（２）で表す。なお、式（１）および式（２）中、Ｔは、転置を示す。
【００４７】
【数１】
0006685847.tif 000002
【００４８】
また、３軸（ｘ軸，ｙ軸，ｚ軸）の速度を示す速度ベクトルｖを、ｖ＝（ｖｘ，ｖｙ，ｖｚ）Ｔとする。すると、移動体１２の位置の変化量ｔ２と、移動体１２の姿勢の変化量δｒ２と、移動体１２の速度の変化量δｖと、は、下記式(３)から式（５）の各々によって求めることができる。
【００４９】
【数２】
0006685847.tif 000003
【００５０】
式（３）〜式（５）中、τは時間を示し、ｇは重力加速度を意味する。
【００５１】
なお、τには、あらかじめ定めた単位時間を用いればよい。すなわち、第１の移動量として、単位時間あたりの移動体１２の移動量を用いる場合には、τは、該単位時間を用いればよい。また、第１の移動量として、前回の観測情報の観測のタイミングから今回の観測情報の観測のタイミングまでの期間を用いる場合には、この期間をτとして用いればよい。
【００５２】
導出機能１０Ｓは、式（３）〜式（５）を算出することによって、移動体１２の位置の変化量δｔ２と、移動体１２の姿勢の変化量δｒ２と、移動体１２の速度の変化量δｖ２と、を含む、第１の移動量を導出する。
【００５３】
次に、導出機能１０Ｋについて説明する。導出機能１０Ｋは、第２の導出部の一例である。導出機能１０Ｋは、外界センサ１０Ｇで観測した周辺情報に基づいて、移動体１２の移動量を導出する。導出機能１０Ｋが導出する移動体１２の移動量を、第２の移動量と称する。
【００５４】
第２の移動量とは、前回の第２の移動量の導出に用いた周辺情報の観測のタイミングから、今回の第２の移動量の導出に用いた周辺情報の観測のタイミングまでの期間の、移動体１２の移動量を示す。なお、この期間は、第１の移動量の示す期間と同じである。なお、第２の移動量は、今回の第２の移動量の導出に用いた周辺情報の観測のタイミングにおける、単位時間あたりの移動体１２の移動量であってもよい。
【００５５】
本実施の形態では、導出機能１０Ｋは、外界センサ１０Ｇで観測した周辺情報から、移動体１２の位置の変化量と、移動体１２の姿勢の変化量と、を含む第２の移動量を導出する。
【００５６】
導出機能１０Ｋは、外界センサ１０Ｇによって異なるタイミングで観測された２つの周辺情報を用いて、移動体１２の第２の移動量を導出する。
【００５７】
例えば、導出機能１０Ｋが、第１のタイミングにおける移動体１２の第２の移動量を導出すると仮定する。この場合、導出機能１０Ｋは、第１のタイミングで観測された周辺情報と、第２のタイミングで観測された周辺情報と、を用いて、第１のタイミングにおける移動体１２の第２の移動量を導出する。
【００５８】
第１のタイミングと第２のタイミングとは、異なるタイミングであればよい。具体的には、第２のタイミングは、第１のタイミングより前の（過去の）タイミングであってもよい。また、第２のタイミングは、第１のタイミングより後の（未来の）タイミングであってもよい。
【００５９】
第２のタイミングが第１のタイミングより後（未来の）のタイミングである場合、導出機能１０Ｋは、観測のタイミングの異なる複数の観測情報を予め記憶回路１０Ｂなどに記憶する。そして、導出機能１０Ｋは、複数の観測情報の中から、任意の２つの観測情報を読取ることで、一方を第１のタイミングの観測情報とし、他方を第２のタイミングの観測情報として用いればよい。
【００６０】
そして、導出機能１０Ｋは、これらの第１のタイミングで観測された周辺情報と、第２のタイミングで観測された周辺情報と、を用いて、第１のタイミングにおける移動体１２の第２の移動量を導出する。
【００６１】
詳細には、導出機能１０Ｋは、第１のタイミングで観測された周辺情報における対象点を検出する。対象点とは、周辺情報における外界センサ１０Ｇによって観測された点である。対象点は、例えば、特徴的な領域を示す特徴点を含む。特徴点は、輝度、色、距離、の少なくとも１つが所定値以上異なる領域間の境界を示す点である。特徴点は、例えば、エッジ（輝度が急激に変化する点）や、コーナー（線分の黒点やエッジが急激にまがった箇所）である。なお、対象点は、周辺情報における任意の箇所（点）であればよく、特徴点に限定されない。
【００６２】
図３は、第２の移動量の導出の説明図である。例えば、図３に示すように、外界センサ１０Ｇを搭載した移動体１２が、三次元空間Ｓにおける位置Ａから位置Ｂへと移動したと仮定する。そして、外界センサ１０Ｇが、位置Ｂのタイミング（第１のタイミング）と位置Ａのタイミング（第２のタイミング）の各々において、周辺情報３１を観測したと仮定する。そして、導出機能１０Ｋが、位置Ａから位置Ｂへの第２の移動量を導出する場合を説明する。
【００６３】
この場合、外界センサ１０Ｇは、実空間を観測することで、様々な対象点４０を含む周辺情報３１を観測する。対象点４０は、上述したように、例えば、エッジやコーナーなどの特徴点を含む。導出機能１０Ｋは、周辺情報３１に含まれる対象点４０を検出し、第２の移動量の導出に用いる。
【００６４】
導出機能１０Ｋは、第１のタイミングで観測された周辺情報３１における対象点４０と、第２のタイミングで観測された周辺情報３１に基づいて、第１のタイミングにおける対象点４０を推定した推定点と、の誤差に基づいて、第１のタイミングにおける移動体１２の第２の移動量を導出する。
【００６５】
詳細に説明する。図４は、周辺情報３１が撮影画像３０である場合の、第２の移動量の導出の説明図である。
【００６６】
図４（Ａ）は、外界センサ１０Ｇが観測した撮影画像３０の一例である。撮影画像３０Ｂは、位置Ｂ（図３参照）で第１のタイミングで観測された撮影画像３０の一例である。撮影画像３０Ａは、位置Ｂより前の位置Ａ（図３参照）で第２のタイミングで観測された撮影画像３０の一例である。
【００６７】
まず、導出機能１０Ｋは、第１のタイミングで観測された撮影画像３０Ｂにおける対象点４２ｂを検出する。
【００６８】
導出機能１０Ｋは、公知の画像処理方法によって、第１のタイミングで観測された撮影画像３０Ｂにおけるエッジやコーナーを示す場所を、対象点４２ｂとして検出する。例えば、導出機能１０Ｋは、Ｈａｒｒｉｓオペレータによるコーナー検出や、Ｋｉｒｓｃｈオペレータによるエッジ検出を利用して、対象点４２ｂを検出する。
【００６９】
次に、導出機能１０Ｋは、第１のタイミングとは異なる第２のタイミングで観測された撮影画像３０Ａにおける、上記対象点４２ｂの各々に対応する対応点４２ａを探索する（図４（Ａ）参照）。
【００７０】
撮影画像３０Ｂの対象点４２ｂの各々に対応する、撮影画像３０Ａにおける対応点４２ａの探索には、公知の方法を用いればよい。例えば、導出機能１０Ｋは、テンプレートマッチングによる画像パッチの対応付けを用いて、対応点４２ａを探索してもよい。また、導出機能１０Ｋは、ＳＵＲＦ特徴量、ＳＩＦＴ特徴量、ＡＫＡＺＥ特徴量などを用いて、撮影画像３０Ｂにおける対象点４２ｂの各々の特徴量と一致する（または特徴量の近い）場所を、対応点４２ａとして探索してもよい。
【００７１】
次に、導出機能１０Ｋは、第１のタイミングの撮影画像３０Ｂから検出した対象点４２ｂと、第２のタイミングの撮影画像３０Ａから探索した対応点４２ａと、を用いて、第２の移動量を導出する。
【００７２】
例えば、導出機能１０Ｋは、基本行列推定に基づく手法を用いて、第２の移動量を導出する。本実施の形態では、一例として、５点法を用いる場合を説明する。５点法を用いる場合、対象点４２ｂと対応点４２ａとの対を、５対以上必要とする。基本行列は、第１のタイミングにおける外界センサ１０Ｇと、第２のタイミングにおける外界センサ１０Ｇと、の間の位置と姿勢の関係を包含した行列である。すなわち、基本行列は、第１のタイミングで観測された撮影画像３０Ｂにおける対象点４２ｂの位置と、第２のタイミングで観測された撮影画像３０Ａにおける対応点４２ａの位置と、の関係を表す。
【００７３】
基本行列推定に基づく手法では、特異値分解を用いて、対象点４２ｂと対応点４２ａとの複数の対に適合する基本行列を推定し、推定の過程で得られる特異値分解の結果から、第１のタイミングの外界センサ１０Ｇの位置および姿勢の変化量を示す、並進ベクトルδｔ１と回転行列δＲ１を算出する。
【００７４】
図４を用いて詳細に説明する。次に、導出機能１０Ｋは、第１のタイミングの撮影画像３０Ｂから検出した対象点４２ｂと、第２のタイミングの撮影画像３０Ａから探索した対応点４２ａと、の各々を、外界センサ１０Ｇを配置した仮想三次元空間Ｓ’に投影する（図４（Ｂ）参照）。
【００７５】
このとき、導出機能１０Ｋは、仮想三次元空間Ｓ’における、所定の２つの位置に、所定の姿勢で、外界センサ１０Ｇを配置する。
【００７６】
そして、導出機能１０Ｋは、撮影画像３０Ｂの対象点４２ｂと、撮影画像３０Ａの対応点４２ａと、の各々に対し、仮想三次元空間Ｓ’に配置した２つの外界センサ１０Ｇの位置および姿勢を用いた三角測量によって、仮想三次元空間Ｓ’における三次元位置を計算する。
【００７７】
そして、導出機能１０Ｋは、仮想三次元空間Ｓ’における対応点４２ａを、撮影画像３０Ｂにおける対応する対象点４２ｂ’に再投影する。
【００７８】
これによって、導出機能１０Ｋは、第１のタイミングの撮影画像３０Ｂにおける対象点４２ｂと、第２のタイミングで観測された撮影画像３０Ａに基づいて該対象点４２ｂを推定した推定点４２ｂ’と、を求める。
【００７９】
さらに、導出機能１０Ｋは、仮想三次元空間Ｓ’における外界センサ１０Ｇの姿勢および位置を変えながら、撮影画像３０Ｂにおける対象点４２ｂと対応する推定点４２ｂ’との差である再投影誤差４２Ｄ’が、設定値以下となるまで、仮想三次元空間Ｓ’への投影および撮影画像３０Ｂへの再投影を繰返す（図５（Ｃ）参照）。
【００８０】
この設定値は、予め定めればよい。また、撮影画像３０Ａと撮影画像３０Ｂには、対象点４２ｂと対応する対応点４２ａとの対が複数存在する。このため、導出機能１０Ｋは、これらの複数の対応点４２ａと対象点４２ｂとの対ごとの再投影誤差４２Ｄ’の内、最も大きい再投影誤差４２Ｄ’が設定値以下となるまで、投影および再投影を繰返してもよい。また、導出機能１０Ｋは、これらの複数の対応点４２ａと対象点４２ｂとの対ごとの再投影誤差４２Ｄ’の平均値が設定値以下となるまで、投影および再投影を繰返してもよい。
【００８１】
そして、導出機能１０Ｋは、再投影誤差４２Ｄ’が設定値以下となったときに設定していた、第２のタイミングの外界センサ１０Ｇの位置および姿勢に対する、第１のタイミングの外界センサ１０Ｇの位置および姿勢の変化量を、第２の移動量として導出する。
【００８２】
なお、この設定値以下となったときの再投影誤差４２Ｄ’を、誤差４７と称して説明する。すなわち、誤差４７は、導出機能１０Ｋが第２の移動量の導出に用いた再投影誤差４２Ｄ’を示す。
【００８３】
なお、再投影誤差４２Ｄ’が設定値以下となったときに設定していた、第２のタイミングの外界センサ１０Ｇの位置および姿勢に対する、第１のタイミングの外界センサ１０Ｇの位置および姿勢の変化量は、並進ベクトルδｔ１と回転行列δＲ１で表される。このため、導出機能１０Ｋは、姿勢の変化量を示す回転行列δＲ１をロール・ピッチ・ヨー角δｒ１に変換する。そして、導出機能１０Ｋは、並進ベクトルによって表される位置の変化量δｔ１と、ロール・ピッチ・ヨー角で表される姿勢の変化量δｒ１と、を第２の移動量として導出する。
【００８４】
次に、外界センサ１０Ｇで観測した周辺情報３１が距離情報３２である場合の、第２の移動量の導出方法を説明する。
【００８５】
図５は、周辺情報３１が距離情報３２である場合の、第２の移動量の導出の説明図である。なお、図５には、距離情報３２が、画素ごとに外界センサ１０Ｇからの距離を規定した距離画像である場合を一例として示した。
【００８６】
図５（Ｂ）に示す距離情報３２Ｂは、位置Ｂ（図３参照）で第１のタイミングで観測された距離情報３２の一例である。図５（Ａ）に示す距離情報３２Ａは、位置Ｂより前の位置Ａ（図３参照）で第２のタイミングで観測された距離情報３２の一例である。
【００８７】
まず、導出機能１０Ｋは、第１のタイミングで観測された距離情報３２Ｂにおける対象点４４ｂを検出する（図５（Ｂ）参照）。対象点４４ｂの検出方法は、対象点４２ｂの検出方法と同様である。そして、導出機能１０Ｋは、第１のタイミングとは異なる第２のタイミングで観測された距離情報３２Ａにおける、上記対象点４４ｂの各々に対応する対応点４４ａを探索する（図５（Ａ）参照）。対応点４４ａの探索方法は、対応点４２ａの探索と同様の方法を用いればよい。
【００８８】
ここで、距離情報３２Ａおよび距離情報３２Ｂは、三次元空間における距離を示す距離情報である。このため、導出機能１０Ｋは、距離情報３２Ａにおける対応点４４ａを、距離情報３２Ｂ上に配置し、配置した対応点４４ａと対応する対象点４４ｂとの差から、該対象点４４ｂの推定点４４ｂ’を求める。
【００８９】
そして、導出機能１０Ｋは、対象点４４ｂと推定点４４ｂ’との推定誤差４４Ｄ’を導出する。すなわち、導出機能１０Ｋは、第１のタイミングで観測された距離情報３２Ｂにおける対象点４４ｂと、第２のタイミングで観測された距離情報３２Ａにおける、該対象点４４ｂに対応する対応点４４ａから推定した推定点４４ｂ’と、の誤差を導出する。
【００９０】
さらに、導出機能１０Ｋは、この推定誤差４４Ｄ’によって表される並進ベクトルδｔ１と回転行列δＲ１の内、回転行列δＲ１を、ロール・ピッチ・ヨー角δｒ１に変換する。そして、導出機能１０Ｋは、並進ベクトルによって表される位置の変化量δｔ１と、ロール・ピッチ・ヨー角で表される姿勢の変化量δｒ１と、を第２の移動量として導出する。
【００９１】
なお、距離情報３２Ａと距離情報３２Ｂとによって、対象点４４ｂと推定点４４ｂ’との対が複数構成される場合がある。この場合、導出機能１０Ｋは、複数の対の各々の推定誤差４４Ｄ’の内、最も大きい推定誤差４４Ｄ’を用いて、第２の移動量を導出すればよい。また、導出機能１０Ｋは、複数の対の推定誤差４４Ｄ’の平均値を用いて、第２の移動量を導出してもよい。
【００９２】
なお、導出機能１０Ｋが第２の移動量の導出に用いた推定誤差４４Ｄ’を、誤差４８と称して説明する。
【００９３】
また、導出機能１０Ｋが距離情報３２を用いた第２の移動量の導出に用いた推定誤差４４Ｄ’である誤差４８と、導出機能１０Ｋが撮影画像３０を用いた第２の移動量の導出に用いた再投影誤差４２Ｄ’である誤差４７と、を総称する場合には、単に、誤差４６と称して説明する場合がある。
【００９４】
図２に戻り説明を続ける。
【００９５】
ここで、上述したように、内界センサ１０Ｊが観測する観測情報は、移動体１２の環境に非依存の観測情報である。一方、外界センサ１０Ｇが観測する周辺情報３１は、移動体１２の環境に依存する。例えば、移動体１２がトンネル内などの外部との通信エラーの発生しやすい場所を走行中である場合、処理回路１０Ａは、外界センサ１０Ｇから周辺情報３１を得ることが困難となる場合がある。また、移動体１２が所定の明るさ未満の暗い環境を移動中である場合についても、対象点４０を精度良く探索可能な周辺情報３１が得られない場合がある。同様に、移動体１２が所定の明るさ以上の明るい環境を移動中である場合についても、対象点４０を精度良く探索可能な周辺情報３１が得られない場合がある。
【００９６】
このため、外界センサ１０Ｇで観測された周辺情報に基づいて導出された第２の移動量と、内界センサ１０Ｊで観測された観測情報に基づいて導出された第１の移動量と、の双方を常に用いて、移動体１２の現在の移動量を算出すると、算出精度が低下する場合がある。
【００９７】
そこで、信頼度算出機能１０Ｌは、第２の移動量の信頼度を算出する。すなわち、信頼度算出機能１０Ｌは、導出機能１０Ｋによって導出された第２の移動量の、信頼度を算出する。信頼度とは、第２の移動量の精度の高さを示す。信頼度が高いとは、第２の移動量の精度が高い（すなわち、移動体１２の実際の移動量に近い）ことを意味する。
【００９８】
信頼度算出機能１０Ｌは、上記誤差４６、第１の移動量と第２の移動量との差、および、周辺情報の取得を連続して成功した回数、の少なくとも１つに基づいて、第２の移動量の信頼度を算出する。
【００９９】
まず、誤差４６を用いて信頼度を算出する方法を説明する。
【０１００】
上述したように、誤差４６には、第２の移動量の導出に用いた再投影誤差４２Ｄ’（誤差４７、図４参照）や、第２の移動量の導出に用いた推定誤差４４Ｄ’（誤差４８、図５参照）が含まれる。
【０１０１】
例えば、信頼度算出機能１０Ｌは、第２の移動量の導出に用いた第１のタイミングの周辺情報３１（撮影画像３０、距離情報３２）における対象点４０（対象点４２ｂ、対象点４４ｂ）と、第２のタイミングで観測された周辺情報３１（撮影画像３０、距離情報３２）に基づいて推定した、対象点４０（対象点４２ｂ、対象点４４ｂ）を示す推定点（推定点４２ｂ’、推定点４４ｂ’）と、の誤差４６（誤差４７、誤差４８）に基づいて、信頼度を算出する。
【０１０２】
具体的には、信頼度算出機能１０Ｌは、第２の移動量の導出に用いた第１のタイミングの周辺情報３１（撮影画像３０、距離情報３２）における複数の対象点４０（対象点４２ｂ、対象点４４ｂ）と、対応する複数の推定点（推定点４２ｂ’、推定点４４ｂ’）と、の各々の複数の誤差４６を読取る。具体的には、信頼度算出機能１０Ｌは、複数の再投影誤差４２Ｄ’、または、複数の推定誤差４４Ｄ’を、複数の誤差４６として読取る。そして、信頼度算出機能１０Ｌは、これらの複数の誤差４６の内、特定の条件を満たす誤差４６の数を算出する。そして、信頼度算出機能１０Ｌは、算出した数を、第１のタイミングにおける第２の移動量の信頼度として算出する。
【０１０３】
この特定の条件は、予め定めればよい。例えば、特定の条件は、誤差４６（再投影誤差４２Ｄ’、推定誤差４４Ｄ’）が基準値未満の値であることを示す。この場合、信頼度算出機能１０Ｌは、対象点４０と対応する推定点との誤差４６の内、基準値未満の値の誤差４６の数を、信頼度として算出する。
【０１０４】
また、特定の条件は、対象点４０と対応する推定点との特徴量の差が基準値以下であることを示してもよい。この場合、信頼度算出機能１０Ｌは、対象点４０と対応する推定点の各々について、特徴量を導出する。そして、特徴量の差が基準値以下の対象点４０と推定点との対の数を、信頼度として算出すればよい。
【０１０５】
対象点４０および推定点の各々の特徴量は、対象点４０および推定点の各々の示すエッジやコーナーに応じた値を定めればよい。具体的には、信頼度算出機能１０Ｌは、対象点４０および推定点の各々の示すエッジが鋭角であるほど大きく、また、コーナーが大きいほど大きい特徴量を導出する。そして、信頼度算出機能１０Ｌは、これらの特徴量の差が基準値以下の、対象点４０と対応する推定点との対の数を、信頼度として算出すればよい。
【０１０６】
また、信頼度算出機能１０Ｌは、第１の移動量と第２の移動量との差に基づいて、信頼度を算出してもよい。例えば、信頼度算出機能１０Ｌは、第１の移動量と第２の移動量との差を、信頼度として算出する。
【０１０７】
なお、信頼度算出機能１０Ｌは、第１の移動量と第２の移動量との差の逆数を、信頼度として算出してもよい。この場合、信頼度算出機能１０Ｌは、下記式（６）を用いて、信頼度を算出すればよい。
【０１０８】
【数３】
0006685847.tif 000004
【０１０９】
式（６）中、ａは、信頼度を示す。なお、δｔ１、δｔ２、δｒ１、δｒ２は、上記と同様である。
【０１１０】
なお、信頼度算出機能１０Ｌは、第２の移動量と第１の移動量の各々に、あらかじめ定めた重みづけ値を乗算した値の差を、信頼度として算出してもよい。
【０１１１】
例えば、信頼度算出機能１０Ｌは、移動量に含まれる、位置の変位量、および姿勢の変位量の各々に、予め定めた重み付け値を設定する。そして、信頼度算出機能１０Ｌは、第２の移動量に含まれる位置の変位量および姿勢の変位量の各々に、予め定めた重み付け値を加算または乗算する。同様に、信頼度算出機能１０Ｌは、第１の移動量に含まれる位置の変位量および姿勢の変位量の各々に、予め定めた重み付け値を加算または乗算する。信頼度算出機能１０Ｌは、これらの重み付け値を加算または乗算した後の、位置の変位量および姿勢の変位量の各々の差を、信頼度として算出してもよい。
【０１１２】
また、信頼度算出機能１０Ｌは、第１の移動量および第２の移動量における、位置の変化量のみ、または、姿勢の変化量のみをもちいて、信頼度を算出してもよい。
【０１１３】
ここで、導出機能１０Ｋは、外界センサ１０Ｇから周辺情報を取得する毎に、移動体１２の第２の移動量を導出する。そして、信頼度算出機能１０Ｌは、導出機能１０Ｋが周辺情報の取得に前回失敗してから、周辺情報を今回取得するまでに、周辺情報の取得を連続して成功した回数を、信頼度として用いてもよい。
【０１１４】
次に、移動量算出機能１０Ｍについて説明する。移動量算出機能１０Ｍは、第２の移動量の信頼度が特定の基準を満たす場合、第２の移動量および第１の移動量に基づいて、移動体１２の現在の移動量を算出する。一方、移動量算出機能１０Ｍは、第２の移動量の信頼度が特定の基準を満たさない場合、第１の移動量を、移動体１２の現在の移動量として算出する。
【０１１５】
信頼度が特定の基準を満たすとは、導出機能１０Ｋで導出した第２の移動量に対する信頼度が、移動体１２の現在の移動量の算出に用いることができる程度に高いことを示す。
【０１１６】
この特定の基準は、信頼度の種類に応じて予め設定すればよい。すなわち、上述したように、信頼度算出機能１０Ｌは、様々な方法を用いて信頼度を算出する。このため、特定の基準は、信頼度の種類に応じて予め設定すればよい。
【０１１７】
例えば、信頼度の示す値が大きいほど、高い信頼度を示す場合、移動量算出機能１０Ｍは、信頼度が閾値以上であるときに、信頼度が特定の基準を満たすと判断する。一方、信頼度の示す値が小さいほど、高い信頼度を示す場合、移動量算出機能１０Ｍは、信頼度が閾値未満であるときに、信頼度が特定の基準を満たすと判断する。
【０１１８】
また、信頼度の示す値がある範囲内にあることが、高い信頼度を示す場合がある。この場合、移動量算出機能１０Ｍは、信頼度がこの範囲内であるときに、信頼度が特定の基準を満たすと判断すればよい。
【０１１９】
また、信頼度の示す値がある範囲外にあることが、高い信頼度を示す場合がある。この場合、移動量算出機能１０Ｍは、信頼度がこの範囲外であるときに、信頼度が特定の基準を満たすと判断すればよい。
【０１２０】
なお、この特定の基準に用いる閾値は、変更可能であってもよい。変更機能１０Ｑは、この閾値を変更する。変更機能１０Ｑは、ユーザによる入力装置１０Ｈの操作指示によって指示された値に、閾値を変更する。また、変更機能１０Ｑは、第２の移動量の導出に用いた誤差４６に応じて、閾値を変更してもよい。
【０１２１】
具体的には、変更機能１０Ｑは、第２の移動量の導出に用いた誤差４６の平均値や、該誤差４６の中央値に応じて、閾値を変更してもよい。また、変更機能１０Ｑは、該誤差４６の分散や該誤差４６の標準偏差に応じて、閾値を変更してもよい。また、変更機能１０Ｑは、該誤差４６の最大値または最小値に応じて、閾値を変更してもよい。
【０１２２】
移動量算出機能１０Ｍは、公知の方法を用いて、第２の移動量および第１の移動量から、移動体１２の現在の移動量を算出すればよい。例えば、移動量算出機能１０Ｍは、ガウス・ニュートン法、レーベンバーグ・マーカート法、共役勾配法などを用いて、移動体１２の現在の移動量を算出する。
【０１２３】
一例として、移動量算出機能１０Ｍが、ガウス・ニュートン法を用いて、移動体１２の現在の移動量を算出する方法を説明する。
【０１２４】
例えば、算出する現在の移動量ｘを、下記式（７）で表す。δｔ３は、位置の変化量を示す。δｒ３は、姿勢の変化量を示す。ガウス・ニュートン法では、下記式（８）により、移動量ｘを算出する。
【０１２５】
【数４】
0006685847.tif 000005
【０１２６】
【数５】
0006685847.tif 000006
【０１２７】
Δｘは、更新量を表す。移動量算出機能１０Ｍは、式（８）を用いて移動量ｘを更新する。そして、移動量算出機能１０Ｍは、移動量ｘの更新が収束するまで、更新量Δｘの更新を繰返す。
【０１２８】
具体的には、移動量算出機能１０Ｍは、更新量Δｘ、または、第２の移動量の導出に用いた上記誤差４６の大きさが特定値以下になったときに、収束と判定する。なお、この誤差４６を示す誤差ベクトルを、ｅと定義する。すると、この誤差ベクトルｅのヤコビ行列Ｊは、下記式（９）で表せる。
【０１２９】
【数６】
0006685847.tif 000007
【０１３０】
上記更新量Δｘは、上記式（９）に示すヤコビ行列Ｊにより、下記式（１０）で表すことができる。
【０１３１】
【数７】
0006685847.tif 000008
【０１３２】
一方、上記誤差ベクトルｅは、下記式（１１）および式（１２）で表すことができる。
【０１３３】
【数８】
0006685847.tif 000009
【０１３４】
上述したように、上記式（１１）中、δｔ２は、第１の移動量によって示される、移動体１２の位置の変化量を示す。上記式（１１）中、δｔ３は、移動体１２の現在の移動量によって示される、移動体１２の位置の変化量を示す。また、上記式（１２）中、δｒ２は、第１の移動量によって示される、移動体１２の姿勢の変化量を示す。また、上記式（１２）中、δｒ３は、移動体１２の現在の移動量によって示される、移動体１２の姿勢の変化量を示す。
【０１３５】
次に、移動量算出機能１０Ｍは、算出する現在の移動量ｘにおける、移動体１２の位置の変化量δｔ３、および、移動体１２の姿勢の変化量δｒ３の各々に対する、基本行列Ｅ３を、下記式（１３）を用いて算出する。
【０１３６】
【数９】
0006685847.tif 000010
【０１３７】
但し、算出する現在の移動量ｘにおける、移動体１２の位置の変化量δｔ３、および、移動体１２の姿勢の変化量δｒ３の各々は、下記式（１４）および下記式（１５）で表せる。
【０１３８】
【数１０】
0006685847.tif 000011
【０１３９】
そして、移動量算出機能１０Ｍは、下記式（１６）によって示される対象点４０（対象点４２ｂ、対象点４４ｂ）の各々に対応する、下記式（１７）によって示される対応点（対応点４２ａ、対応点４４ａ）の誤差４６を、式（１８）によって算出する。
【０１４０】
【数１１】
0006685847.tif 000012
【０１４１】
ここで、式（１６）〜式（１８）中、ｉは、対象点４０（対象点４２ｂ、対象点４４ｂ）、対応点（対応点４２ａ、対応点４４ａ）に対するインデックスである。また、第１のタイミングで観測された周辺情報３１（撮影画像３０または距離情報３２）に含まれる対象点４０の数を、ｎとする。
【０１４２】
すると、誤差ベクトルｅは、上記式（１１）〜式（１８）を用いることで、下記式（１９）で表せる。
【０１４３】
【数１２】
0006685847.tif 000013
【０１４４】
そして、移動量算出機能１０Ｍは、上記式（８）により、移動体１２の現在の移動量ｘを算出する。
【０１４５】
なお、移動量算出機能１０Ｍは、第２の移動量および第１の移動量に基づいて、移動体１２の現在の移動量を算出する場合、重み付けを加えた上で、算出処理を行ってもよい。
【０１４６】
例えば、移動量算出機能１０Ｍは、第２の移動量に用いる重み付け値と、第１の移動量に用いる重み付け値と、を予め定める。そして、移動量算出機能１０Ｍは、第２の移動量に、対応する予め定めた重み付け値を付加（加算または乗算）した値と、第１の移動量に、対応する予め定めた重み付け値を付加（加算または乗算）した値と、を用いて、上記と同様にして、移動体１２の現在の移動量を算出してもよい。
【０１４７】
また、移動量算出機能１０Ｍは、第２の移動量および第１の移動量に含まれる、位置の変動量、および姿勢の変動量の内の少なくとも一つについて、予め定めた重み付け値を付加した上で、移動体１２の現在の移動量を算出してもよい。
【０１４８】
なお、移動量算出機能１０Ｍは、算出した移動量を用いて、移動体１２の現在の位置、速度、加速度、移動方向、移動予定方向、および、移動軌跡の少なくとも１つを更に算出してもよい。これらの算出方法には、公知の方法を用いればよい。
【０１４９】
次に、出力制御機能１０Ｎについて説明する。出力制御機能１０Ｎは、出力制御部の一例である。出力制御機能１０Ｎは、出力情報を出力する。
【０１５０】
出力情報は、移動体１２の現在の移動量に関する情報を含む。出力情報は、移動体１２の現在の移動量のそのものを示す情報であってもよいし、該移動量から算出した各種値（例えば、移動体１２の現在の位置、現在進んでいる方向、現在の姿勢、など）を示す情報であってもよい。
【０１５１】
また、出力情報は、移動体１２の現在の移動量を用いて算出された、移動体１２の現在の位置、速度、加速度、移動方向、移動予定方向、および、移動軌跡の少なくとも１つを更に含んでいてもよい。
【０１５２】
また、出力情報は、該出力情報に含まれる、移動体１２の現在の移動量の算出方法を示す情報を含んでもよい。算出方法を示す情報は、例えば、内界センサ１０Ｊで観測された観測情報から導出された第１の移動量のみを用いて、現在の移動量を算出したか、を示す情報である。また、例えば、算出方法を示す情報は、内界センサ１０Ｊで観測された観測情報から導出された第１の移動量と、外界センサ１０Ｇで観測された周辺情報から導出された第２の移動量と、の双方を用いて、現在の移動量を算出したか、を示す情報である。
【０１５３】
出力制御機能１０Ｎは、移動量算出機能１０Ｍで算出された、移動体１２の現在の移動量と、該移動量算出機能１０Ｍによる移動量の算出方法を用いて、出力情報を生成する。
【０１５４】
そして、出力制御機能１０Ｎは、出力情報を出力するように出力回路１０Ｃを制御する。このため、出力回路１０Ｃは、出力情報を出力する。具体的には、出力回路１０Ｃにおける通信回路１０Ｄが、出力対象の移動体３０のドライバの携帯する携帯端末などへ出力情報を送信する。また、例えば、出力回路１０Ｃにおけるディスプレイ１０Ｅに出力情報が表示される。また、例えば、ディスプレイ１０Ｅが、出力情報に応じた色や強度の光を、出力情報に応じた点灯状態（点灯または点滅）で点灯する。
【０１５５】
また、出力制御機能１０Ｎは、出力情報に応じた音を出力するようにスピーカ１０Ｆを制御する。出力情報に応じた音は、出力情報を示す音声であってもよいし、出力情報に応じた警告音であってもよい。
【０１５６】
図６は、ディスプレイ１０Ｅに表示された出力情報５０の一例を示す模式図である。図６（Ａ）は、移動量算出機能１０Ｍが第１の移動量を移動体１２の現在の移動量として算出した場合の出力情報５０Ａの一例である。この場合、例えば、出力情報５０Ａは、ＩＭＵ（すなわち内界センサ１０Ｊ）のみを用いて、移動体１２の現在の移動量を算出したことを示す情報と、現在の移動量を示す情報と、を含む。
【０１５７】
図６（Ｂ）は、移動量算出機能１０Ｍが第２の移動量と第１の移動量に基づいて移動体１２の現在の移動量を算出した場合の、出力情報５０Ｂの一例である。この場合、例えば、出力情報５０Ｂは、ＩＭＵ（すなわち内界センサ１０Ｊ）と外界センサ１０Ｇとを用いて、移動体１２の現在の移動量を算出したことを示す情報と、現在の移動量を示す情報と、を含む。
【０１５８】
次に、処理回路１０Ａが実行する処理の手順を説明する。図７は、処理回路１０Ａが実行する処理の手順の一例を示す、フローチャートである。
【０１５９】
まず、導出機能１０Ｓが、内界センサ１０Ｊから観測情報を取得する（ステップＳ１００）。次に、導出機能１０Ｓは、ステップＳ１００で取得した観測情報から第１の移動量を導出する（ステップＳ１０２）。
【０１６０】
次に、導出機能１０Ｋが、ステップＳ１００と同じタイミングで観測された周辺情報を取得する（ステップＳ１０４）。次に、導出機能１０Ｋは、ステップＳ１０４で取得した周辺情報を用いて、第２の移動量を導出する、導出処理を実行する（ステップＳ１０６）（詳細後述）。
【０１６１】
次に、導出機能１０Ｋは、ステップＳ１０６で導出した第２の移動量の信頼度を算出する（ステップＳ１０８）。次に、移動量算出機能１０Ｍは、ステップＳ１０８で算出した信頼度が特定の基準を満たすか否かを判別する（ステップＳ１１０）。ステップＳ１１０で肯定判断すると（ステップＳ１１０：Ｙｅｓ）、ステップＳ１１２へ進む。
【０１６２】
ステップＳ１１２では、移動量算出機能１０Ｍが、ステップＳ１０２で導出した第１の移動量と、ステップＳ１０６で導出した第２の移動量と、に基づいて、移動体１２の現在の移動量を算出する（ステップＳ１１２）。そして、ステップＳ１１６へ進む。
【０１６３】
一方、ステップＳ１１０で否定判断すると（ステップＳ１１０：Ｎｏ）、ステップＳ１１４へ進む。ステップＳ１１４では、移動量算出機能１０Ｍは、ステップＳ１０２で導出した第１の移動量を、移動体１２の現在の移動量として算出する（ステップＳ１１４）。
【０１６４】
次に、出力制御機能１０Ｎは、ステップＳ１１２またはステップＳ１１４で算出した、移動体１２の現在の移動量に関する出力情報を、出力回路１０Ｃから出力する（ステップＳ１１６）。
【０１６５】
次に、処理回路１０Ａは、処理を終了するか否かを判断する（ステップＳ１１８）。例えば、処理回路１０Ａは、ユーザによる入力装置１０Ｈの操作指示によって処理終了を示す信号が入力されたか否かを判別することで、ステップＳ１１８の判断を行う。
【０１６６】
ステップＳ１１８で否定判断すると（ステップＳ１１８：Ｎｏ）、上記ステップＳ１００へ戻る。一方、ステップＳ１１８で肯定判断すると（ステップＳ１１８：Ｙｅｓ）、本ルーチンを終了する。
【０１６７】
次に、第２の移動量の導出処理の手順を説明する。図８は、ステップＳ１０６（図７参照）における、第２の移動量の導出処理の手順の一例を示す、フローチャートである。
【０１６８】
まず、導出機能１０Ｋは、第１のタイミングで観測された周辺情報３１を取得する（ステップＳ２００）。この第１のタイミングで観測された周辺情報３１は、ステップＳ１０４で取得した周辺情報３１である（図７参照）。
【０１６９】
次に、導出機能１０Ｋは、ステップＳ２００で取得した周辺情報３１における対象点４０を検出する（ステップＳ２０２）。
【０１７０】
次に、導出機能１０Ｋは、第１のタイミングとは異なる第２のタイミングで観測された周辺情報３１を取得する（ステップＳ２０４）。次に、導出機能１０Ｋは、ステップＳ２０４で取得した第２のタイミングの周辺情報３１における、上記対象点４０の各々に対応する対応点を探索する（ステップＳ２０６）。
【０１７１】
次に、導出機能１０Ｋは、ステップＳ２０２で検出した対象点４０と、ステップＳ２０６で探索した対応点と、に基づいて、第２のタイミングの外界センサ１０Ｇの位置および姿勢に対する、第１のタイミングの外界センサ１０Ｇの位置および姿勢の変化量を示す、並進ベクトルと回転行列を推定する（ステップＳ２０８）。
【０１７２】
そして、導出機能１０Ｋは、ステップＳ２０８で推定した、姿勢の変化量を示す回転行列を、ロール・ピッチ・ヨー角に変換する（ステップＳ２１０）。次に、導出機能１０Ｋは、ステップＳ２０８で推定した並進ベクトルによって表される位置の変化量と、ステップＳ２１０で導出したロール・ピッチ・ヨー角で表される姿勢の変化量と、を第２の移動量として導出する（ステップＳ２１２）。そして、本ルーチンを終了する。
【０１７３】
以上説明したように、本実施の形態の情報処理装置１０は、内界センサ１０Ｊと、導出機能１０Ｓ（第１の導出部）と、導出機能１０Ｋ（第２の導出部）と、信頼度算出機能１０Ｌ（信頼度算出部）と、移動量算出機能１０Ｍ（移動量算出部）と、を備える。
【０１７４】
内界センサ１０Ｊは、移動体１２の加速度を含む観測情報を観測する。導出機能１０Ｓは、観測情報から、移動体１２の第１の移動量を導出する。導出機能１０Ｋは、外界センサ１０Ｇで観測した移動体１２の周辺情報３１に基づいて、移動体１２の第２の移動量を導出する。信頼度算出機能１０Ｌは、第２の移動量の信頼度を算出する。移動量算出機能１０Ｍは、信頼度が特定の基準を満たす場合、第２の移動量および第１の移動量に基づいて移動体１２の現在の移動量を算出する。移動量算出機能１０Ｍは、信頼度が基準を満たさない場合、第１の移動量を移動体１２の現在の移動量として算出する。
【０１７５】
このように、本実施の形態の情報処理装置１０は、外界センサ１０Ｇで観測した周辺情報３１に基づいて導出した第２の移動量の信頼度が基準を満たす場合に、第２の移動量および第１の移動量に基づいて、移動体１２の現在の移動量を算出する。また、情報処理装置１０は、該第２の移動量の信頼度が基準を満たさない場合、内界センサ１０Ｊで観測した観測情報に基づいて導出した第１の移動量を、移動体１２の現在の移動量として用いる。
【０１７６】
このため、本実施の形態の情報処理装置１０では、外界センサ１０Ｇで観測した周辺情報の精度が低い場合であっても、移動体１２の移動量を精度良く安定して算出することができる。
【０１７７】
従って、本実施の形態の情報処理装置１０では、移動体１２の移動量の算出精度の低下を抑制することができる。
【０１７８】
次に、上記実施の形態の情報処理装置１０のハードウェア構成の一例を説明する。図９は、上記実施の形態の情報処理装置１０のハードウェア構成図の一例である。
【０１７９】
上記実施の形態の情報処理装置１０は、ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）８６などの制御装置と、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）８８やＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）９０やＨＤＤ（ハードディスクドライブ）９２などの記憶装置と、各種機器とのインターフェースであるＩ／Ｆ部８２と、出力情報などの各種情報を出力する出力部８０と、ユーザによる操作を受付ける入力部９４と、各部を接続するバス９６とを備えており、通常のコンピュータを利用したハードウェア構成となっている。
【０１８０】
上記実施の形態の情報処理装置１０では、ＣＰＵ８６が、ＲＯＭ８８からプログラムをＲＡＭ９０上に読み出して実行することにより、上記各機能がコンピュータ上で実現される。
【０１８１】
なお、上記実施の形態の情報処理装置１０で実行される上記各処理を実行するためのプログラムは、ＨＤＤ９２に記憶されていてもよい。また、上記実施の形態の情報処理装置１０で実行される上記各処理を実行するためのプログラムは、ＲＯＭ８８に予め組み込まれて提供されていてもよい。
【０１８２】
また、上記実施の形態の情報処理装置１０で実行される上記処理を実行するためのプログラムは、インストール可能な形式または実行可能な形式のファイルでＣＤ−ＲＯＭ、ＣＤ−Ｒ、メモリカード、ＤＶＤ（Ｄｉｇｉｔａｌ  Ｖｅｒｓａｔｉｌｅ  Ｄｉｓｋ）、フレキシブルディスク（ＦＤ）等のコンピュータで読み取り可能な記憶媒体に記憶されてコンピュータプログラムプロダクトとして提供されるようにしてもよい。また、上記実施の形態の情報処理装置１０で実行される上記処理を実行するためのプログラムを、インターネットなどのネットワークに接続されたコンピュータ上に格納し、ネットワーク経由でダウンロードさせることにより提供するようにしてもよい。また、上記実施の形態の情報処理装置１０で実行される上記処理を実行するためのプログラムを、インターネットなどのネットワーク経由で提供または配布するようにしてもよい。
【０１８３】
なお、上記には、本発明の実施の形態を説明したが、上記実施の形態は、例として提示したものであり、発明の範囲を限定することは意図していない。この新規な実施の形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。この実施の形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。
【符号の説明】
【０１８４】
１０  情報処理装置
１０Ｇ  外界センサ
１０Ｊ  内界センサ
１０Ｋ  導出機能
１０Ｌ  信頼度算出機能
１０Ｍ  移動量算出機能
１０Ｎ  出力制御機能
１０Ｓ  導出機能
【図１】
0006685847.tif 000014
【図２】
0006685847.tif 000015
【図３】
0006685847.tif 000016
【図４】
0006685847.tif 000017
【図５】
0006685847.tif 000018
【図６】
0006685847.tif 000019
【図７】
0006685847.tif 000020
【図８】
0006685847.tif 000021
【図９】
0006685847.tif 000022