JP6659317B2 - 位置姿勢推定装置、位置姿勢推定プログラムおよび電気掃除機システム - Google Patents

Description

実施形態は、位置姿勢の推定に関する。

カメラによって取得した画像に基づいて当該カメラの位置姿勢を推定する技術が、コンピュータビジョンおよびロボティクスなどの分野において活用されている。この技術は、例えば、自律移動ロボットの自己位置推定、ナビゲーションシステム、ＡＲ（拡張現実）技術などに応用されている。

具体的には、カメラの位置姿勢と周辺の被写体の３次元構造とを同時に推定する技術として、ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）、ＳｆＭ（Ｓｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎ）などが研究されてきた。

位置姿勢推定技術には、単眼カメラが用いられることもあるし、ステレオカメラが用いられることもある。特に、ステレオカメラを用いてＳＬＡＭを実行すれば、当該ステレオカメラの周辺の３次元構造の絶対スケールを推定することができる。

ステレオカメラを用いたＳＬＡＭは、ある時刻（ｔ）のステレオ画像に基づいて特徴点の３次元点を復元し、別の時刻（ｔ＋１）にこの３次元点をこのステレオカメラに投影した場合の再投影誤差を最小化するように、別の時刻（ｔ＋１）におけるステレオカメラの位置姿勢を推定する。

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

実施形態は、２個のカメラを備える装置の位置姿勢を推定することを目的とする。

実施形態によれば、位置姿勢推定装置は、第１の撮像部と、第２の撮像部と、推定部とを含む。第１の撮像部は、第１の時刻に撮像された第１の基準画像と第２の時刻に撮像された第２の基準画像とを生成する。第２の撮像部は、第１の時刻に関連付けられる第１の参照画像と第２の時刻に関連付けられる第２の参照画像とを生成する。推定部は、第１の基準画像と第２の基準画像と第１の参照画像と第２の参照画像とに基づいて、第２の時刻における第１の撮像部の位置姿勢を基準とした第１の時刻における第１の撮像部の位置姿勢と、第１の時刻における第２の撮像部の推定位置に対する第２の撮像部が第１の参照画像を撮像した位置のずれとを推定する。

第１の実施形態に係る位置姿勢推定装置を例示するブロック図。 図１の位置姿勢推定装置の動作を例示するフローチャート。 第１の撮像部および第２の撮像部がそれぞれ撮像を行う第１の周期および第２の周期が同期している場合の両者の撮像タイミングを例示するタイミングチャート。 第１の周期および第２の周期が同期していない場合の両者の撮像タイミングを例示するタイミングチャート。 図１の位置姿勢推定装置によって行われる推定処理の説明図。 図１の位置姿勢推定装置のハードウェア構成を例示するブロック図。 第２の実施形態に係る電気掃除機を例示するブロック図。 第２の実施形態に係る電気掃除機を例示する斜視図。 第２の実施形態に係る電気掃除機を例示する底面図。 第２の実施形態に係る電気掃除機を含むネットワークを例示する図。

位置姿勢推定技術の精度は、３次元点の復元および再投影の精度に影響される。仮に、２個のカメラの撮像タイミングがずれている場合には、一方のカメラが撮像を行ってから他方のカメラが撮像を行うまでの間にステレオカメラは移動するかもしれず、他方のカメラは理想的な撮像位置からずれた位置で撮像を行うことになるおそれがある。故に、このような撮像タイミングが同期していないステレオカメラを用いると、３次元点の正確な復元および再投影は困難となる。換言すれば、この位置姿勢推定技術は、ステレオカメラの撮像タイミングが同期していることを前提とする。

しかしながら、撮像タイミングを同期させる同期回路を備えた専用のステレオカメラは高価である。故に、比較的安価な汎用カメラ２台を備えるステレオカメラを用いて位置姿勢を高精度に推定する技術が求められている。

以下、図面を参照しながら実施形態の説明が述べられる。尚、以降、説明済みの要素と同一または類似の要素には同一または類似の符号が付され、重複する説明は基本的に省略される。

（第１の実施形態）
図１に例示されるように、第１の実施形態に係る位置姿勢推定装置１０は、第１の撮像部１０１、第２の撮像部１０２および推定部１０３を含む。なお、便宜的に、後述される位置姿勢の推定処理は、第１の撮像部１０１の撮像タイミングに応じて実行されることとし、第１の撮像部１０１および第２の撮像部１０２はそれぞれ基準カメラおよび参照カメラとも呼ばれることとする。

第１の撮像部１０１は、撮像により基準画像１１０を生成する。第１の撮像部１０１は、複数回撮像を行う。第１の撮像部１０１は、例えば、第１の周期で繰り返し撮像を行う。第１の撮像部１０１は、複数の基準画像１１０を推定部１０３に与える。

第１の撮像部１０１は、複数のレンズを含む光学系とＣＭＯＳ（Ｃｏｍｐｌｅｍｅｎｔａｒｙ Ｍｅｔａｌ Ｏｘｉｄｅ Ｓｅｍｉｃｏｎｄｕｃｔｏｒ）、ＣＣＤ（Ｃｈａｒｇｅ Ｃｏｕｐｌｅｄ Ｄｅｖｉｃｅ）などのイメージセンサとを備えたディジタルカメラを用いて実装することができる。第１の撮像部１０１によって撮像される第１の撮像領域は、第１の撮像部１０１の外界の一部である。第１の撮像部１０１の撮像できる範囲は、当該第１の撮像部１０１の画角、イメージセンサのサイズなどに依存して決まる。第１の周期は、例えば一般的なディジタルカメラを用いて第１の撮像部１０１を実装する場合には、３０ｆｐｓ（ｆｒａｍｅｓ ｐｅｒ ｓｅｃｏｎｄ）または６０ｆｐｓ程度である。

第２の撮像部１０２は、第１の撮像領域と一部重複する第２の撮像領域を撮像することで参照画像１１１を生成する。第２の撮像部１０２は、複数回撮像を行う。第２の撮像部１０２は、例えば、第２の周期で繰り返し撮像を行う。第２の撮像部１０２は、複数の参照画像１１１を推定部１０３に与える。なお、第２の撮像部１０２の位置姿勢は、基準カメラとしての第１の撮像部１０１の位置姿勢に基づいて得ることができる。第１の撮像部１０１の位置姿勢を基にした第２の撮像部１０２の位置姿勢は、後述する推定部１０３または図示されない記憶部等に記憶されていてもよい。

第２の撮像部１０２は、第１の撮像部１０１と同様に、ディジタルカメラを用いて実装することができる。第２の撮像部１０２の内部パラメータ（例えば、解像度、レンズの焦点距離など）は、第１の撮像部１０１と同じでもよいし、異なっていてもよい。以降の説明では、両者の内部パラメータは同一であることとする。また、第２の周期は、第１の周期と同期していてもよいし同期していなくてもよい。すなわち、位置姿勢推定装置１０は、第１の撮像部１０１と第２の撮像部１０２の撮像を同期する同期回路を有していなくてもよい。

第２の周期が第１の周期と同期しているならば、図３に例示されるように、第１の撮像部１０１および第２の撮像部１０２は間隔Δｔ毎に同一のタイミングでそれぞれ撮像を開始および終了することができる。この場合には、撮像タイミングが同期しているステレオカメラを用いた従来のＳＬＡＭを実行することで、第１の撮像部１０１（基準カメラ）の位置姿勢を高精度に推定することができる。

他方、第２の周期が第１の周期と同期していないならば、図４に例示されるように、第１の撮像部１０１および第２の撮像部１０２は異なるタイミングで撮像を行う。具体的には、第１の撮像部１０１がある時刻（ｔ）に撮像を行うと、それから遅延時間（δ_１）を経た時刻（ｔ＋δ_１）に第２の撮像部１０２が撮像を行う。同様に、第１の撮像部１０１が別の時刻（ｔ＋Δｔ）に撮像を行うと、それから遅延時間（δ_２）を経た時刻（ｔ＋Δｔ＋δ_２）に第２の撮像部１０２が撮像を行う。なお、遅延時間（δ_１）および遅延時間（δ_２）は、同じであってもよいし異なっていてもよい。

これらの遅延時間（δ_１またはδ_２）の間に第２の撮像部１０２が移動すると、第２の撮像部１０２が撮像を行う位置はこの移動に応じて理想的な撮像位置（時刻（ｔまたはｔ＋Δｔ）における第２の撮像部１０２の位置）からずれてしまう。すなわち、時刻（ｔ＋Δｔ）における第２の撮像部１０２の推定位置に対する第２の撮像部１０２が参照画像を撮像した位置のずれが生じる。第２の撮像部１０２の推定位置は、予め与えられた、第１の撮像部１０１の位置姿勢を基にした第２の撮像部１０２の位置姿勢から得ることができる。

しかしながら、以下に説明されるように、推定部１０３は、第１の撮像部１０１および第２の撮像部１０２の撮像タイミングが同期していないことにより上記ずれが発生したとしても、第１の撮像部１０１および第２の撮像部１０２の位置姿勢を高精度に推定することができる。

推定部１０３は、第１の撮像部１０１から基準画像１１０を受け取り、第２の撮像部１０２から参照画像１１１を受け取る。推定部１０３は、第１の時刻（ｔ＋１）に関連付けられる基準画像１１０−１および参照画像１１１−１と、当該第１の時刻（ｔ＋１）よりも過去の第２の時刻（ｔ）に関連付けられる基準画像１１０−２および参照画像１１１−２とに基づいて、推定処理を行う。そして、推定部１０３は、第１の時刻（ｔ＋１）における第１の撮像部１０１の（第２の時刻（ｔ）の位置姿勢を基準とした）相対的な位置姿勢と、第２の撮像部１０２が参照画像１１１−１および１１１−２を撮像した位置の理想的な撮像位置（すなわち、第１の時刻（ｔ＋１）および第２の時刻（ｔ）における第２の撮像部１０２の位置）からのずれとを推定する。第１の時刻（ｔ＋１）における第１の撮像部１０１の相対的な位置姿勢と、第２の撮像部１０２が参照画像１１１−１および１１１−２を撮像した位置の理想的な撮像位置からのずれは、同時に推定されても良い。推定部１０３は、推定結果を示す推定データ１１２を外部出力する。

図１の位置姿勢推定装置１０は、例えば図２に示されるように動作する。図２の動作は、基準カメラとしての第１の撮像部１０１が第１の時刻（ｔ＋１）に基準画像１１０−１を取得することで開始する（ステップＳ２０１）。他方、参照カメラとしての第２の撮像部１０２も参照画像１１１−１を撮像する（ステップＳ２０２）。なお、図２の例では、ステップＳ２０２は、ステップＳ２０１よりも後に実行されているが、実際にはこれらの順序は逆であってもよいし同時であってもよい。

推定部１０３は、ステップＳ２０１およびステップＳ２０２においてそれぞれ取得された基準画像１１０−１および参照画像１１１−１と、ステップＳ２０１およびステップＳ２０２の前回の実行時に取得された基準画像１１０−２および参照画像１１１−２とに基づいて、第１の撮像部１０１（基準カメラ）の位置姿勢の推定を行う（ステップＳ２０３）。

具体的には、推定部１０３は、まず、基準画像１１０−１および参照画像１１１−１にＲｅｃｔｉｆｉｃａｔｉｏｎを施す。Ｒｅｃｔｉｆｉｃａｔｉｏｎは、基準画像１１０−１および参照画像１１１−１を完全な平行ステレオカメラによって撮像されたようなステレオペア画像へと変換する。なお、レンズ歪除去をＲｅｃｔｉｆｉｃａｔｉｏｎに予め合成すれば、２つの変換を一度に行うことも可能である。変換パラメータは、キャリブレーションにより求まった第１の撮像部１０１および第２の撮像部１０２の外部パラメータおよびレンズ歪パラメータに基づいて計算することができる。Ｒｅｃｔｉｆｉｃａｔｉｏｎを施すことにより、後述される画像間での特徴点の対応の探索範囲が画像の横１ライン上に限定されるので、容易かつロバストな探索が可能となる。加えて、三角測量を利用した３次元点の復元の計算を簡単化することができる。

推定部１０３は、次に、第１の時刻（ｔ＋１）のステレオペア画像と、第２の時刻（ｔ）のステレオペア画像（これはステップＳ２０３の前回の実行時に生成されているかもしれない）との合計４枚の画像から特徴点を抽出し、全ての対応を探索する。特徴点の抽出および対応の探索は、例えば特徴量に基づいて行われる。特徴量は、例えば、ＳＩＦＴ（Ｓｃａｌｅ−Ｉｎｖａｒｉａｎｔ Ｆｅａｔｕｒｅ Ｔｒａｎｓｆｏｒｍ）、ＳＵＲＦ（Ｓｐｅｅｄｅｄ Ｕｐ Ｒｏｂｕｓｔ Ｆｅａｔｕｒｅｓ）、ＯＲＢ（Ｏｒｉｅｎｔｅｄ ＦＡＳＴ ａｎｄ Ｒｏｔａｔｅｄ ＢＲＩＥＦ）、ＫＡＺＥ（ＫＡＺＥ Ｆｅａｔｕｒｅｓ）、ＡＫＡＺＥ（Ａｃｃｅｌｅｒａｔｅｄ ＫＡＺＥ）などの手法を用いて計算することができる。

推定部１０３は、次に、第１の時刻（ｔ＋１）における第１の撮像部１０１の相対的な位置姿勢と、第２の撮像部１０２が参照画像１１１−１および１１１−２を撮像した位置の理想的な撮像位置からのずれとを同時に推定する。

ここで、参照画像１１１−１の理想的な撮像位置は、例えば、第１の撮像部１０１と第２の撮像部１０２との位置関係に応じて、第１の時刻（ｔ＋１）における第１の撮像部１０１の位置をシフトさせることで算出することができる。第１の撮像部１０１と第２の撮像部１０２との位置関係は、例えばキャリブレーション時に導出することができる。

第２の撮像部１０２の理想的な撮像位置からのずれは、並進運動によるずれおよび回転運動によるずれに分解することができる。第２の撮像部１０２がその光学中心から所与の特徴点までを結ぶ直線に対して直交する方向に並進運動を行ったとすれば、当該特徴点の参照画像１１１−１における投影点のずれの大きさは、第１の時刻（ｔ＋１）における第２の撮像部１０２の光学中心から当該特徴点までの距離に反比例して小さくなる。他方、第２の撮像部１０２が回転運動を行ったとすれば、所与の特徴点の参照画像１１１−１における投影点のずれの大きさは、第１の時刻（ｔ＋１）における上記光学中心から当該特徴点までの距離に依存しない。故に、第２の撮像部１０２が特徴点からある程度離れているならば、当該特徴点の参照画像１１１における投影点のずれの大きさは主に回転運動によるずれに依存する。従って、以降の説明では、例えば、第２の撮像部１０２の理想的な撮像位置からのずれは、回転運動によるずれとみなすこととする。

推定部１０３は、第１の時刻（ｔ＋１）における第１の撮像部１０１の相対的な位置姿勢を３次元の並進ベクトルｔおよび３行×３列の回転行列Ｒで表す。また、推定部１０３は、第２の時刻（ｔ）および第１の時刻（ｔ＋１）に関する第２の撮像部１０２の理想的な撮像位置からのずれを３行×３列の回転行列Ｒ_１およびＲ_２で表す。そして、推定部１０３は、これらのパラメータｔ、Ｒ、Ｒ_１およびＲ_２を変数とする下記数式（１）に示される評価関数を最小化するように、これらのパラメータの値を推定する。

数式（１）において、ｗ_ｉ ^ＣＬおよびｗ_ｉ ^ＣＲは、第１の時刻（ｔ＋１）の左画像および右画像（前述の変換後の基準画像１１０−１および参照画像１１１−２）それぞれにおける第ｉ番目の特徴点の位置を表す。さらに、

は、第２の時刻（ｔ）の左画像および右画像の第ｉ番目の特徴点に基づいて復元した位置
を第１の時刻（ｔ＋１）における第１の撮像部１０１および第２の撮像部１０２それぞれに投影した再投影点の位置を表す。なお、特徴点および再投影点は同次座標で表される。復元した位置は、例えば、時刻（ｔ＋Δｔ）における基準位置に基づいて２次元または３次元で表わされる。

数式（１）は、特徴点の位置に対する再投影点の位置の誤差（以降、再投影誤差と称される）を評価するので、再投影誤差関数と呼ぶこともできる。

再投影点の位置は、下記数式（２）により算出することができる。

数式（２）のうちＡは第１の撮像部１０１（或いは第２の撮像部１０２）の内部パラメータを表す３行×３列の行列である。なお、前述のように、第２の撮像部１０２の内部パラメータは、第１の撮像部１０１と同じである。ｔ_Ｂ＝［−ｂ ０ ０］であって、ｂは第１の撮像部１０１および第２の撮像部１０２の間の眼間距離を表す。ｂは、例えばキャリブレーション時に導出される。また、ｐ_ｉは復元された３次元点の位置を表しており、下記数式（３）で導出することができる。

数式（３）において、ｔｒｉ（ｚ，ｚ’）は、三角測量の原理に基づいて３次元点を復元する関数であり、ｚ＝［ｚ_ｘ ｚ_ｙ １］^Ｔおよびｚ＝［ｚ_ｘ’ ｚ_ｙ’ １］^Ｔの場合には下記数式（４）で表すことができる。

数式（４）において、ｃ_ｘ、ｃ_ｙおよびｆは、第１の撮像部１０１（或いは第２の撮像部１０２）の光学中心の中点のｘ座標およびｙ座標ならびに第１の撮像部１０１の焦点距離を表す。なお、前述のように、第２の撮像部１０２の内部パラメータ（ｃ_ｘ、ｃ_ｙおよびｆを含む）は、第１の撮像部１０１と同じである。ｃ_ｘ、ｃ_ｙおよびｆは、キャリブレーション時に導出することができる。

ここで説明された種々の点、ベクトルおよび行列の関係は、図５に模式的に例示されている。また、推定部１０３は、再投影誤差を最小化するために、ニュートン法、準ニュートン法、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法などを用いることができる。また、特徴点間の対応の探索では、誤対応が得られることもある。故に、推定部１０３は、例えばＲＡＮＳＡＣ（Ｒａｎｄｏｍ ｓａｍｐｌｅ ｃｏｎｓｅｎｓｕｓ）法を用いて推定処理をロバスト化してもよい。

上記例では、推定部１０３は、第１の撮像部１０１の相対的な位置姿勢を３次元の並進ベクトルｔおよび回転行列Ｒで表し、第２の撮像部１０２の理想的な撮像位置からのずれを回転行列Ｒ_１およびＲ_２で表している。しかしなら、推定部１０３は、第１の撮像部１０１の相対的な位置姿勢および第２の撮像部１０２の理想的な撮像位置からのずれを異なるパラメータとして表すこともできる。

具体的には、第１の撮像部１０１および第２の撮像部１０２が２次元平面を３自由度（ｘ並進成分、ｙ並進成分、回転成分）で移動すると仮定することができるならば、第１の撮像部１０１の相対的な位置姿勢は、ｘ並進成分ｔ_ｘ、ｙ並進成分ｔ_ｙおよび回転成分（角度）θで表すことができる。同様に、第１の撮像部１０１の相対的な位置姿勢および第２の撮像部１０２の理想的な撮像位置からのずれは、回転成分θ_１およびθ_２で表すことができる。従って、上記数式（１）の再投影誤差関数は、下記数式（５）に置き換えることができる。

また、上記例では、推定部１０３は、並進ベクトルｔならびに回転行列Ｒ、Ｒ_１およびＲ_２のパラメータセット、または、ｘ並進成分ｔ_ｘ、ｙ並進成分ｔ_ｙならびに回転成分θ，θ_１およびθ_２のパラメータセットを全て同時に推定する。しかしながら、推定部１０３は、過去に、第２の時刻（ｔ）に関する第２の撮像部１０２の理想的な撮像位置からのずれ（すなわち、回転行列Ｒ_１または回転成分θ_１）を推定している可能性がある。このような場合には、推定部１０３は、回転行列Ｒ_１または回転成分θ_１を過去の推定値に設定して、上記数式（１）または数式（７）に示す再投影誤差関数を最小化してもよい。しかしながら、推定処理を繰り返すにつれ、回転行列Ｒ_１または回転成分θ_１の再利用に起因する誤差が累積するので、高精度な推定を実現するためには全てのパラメータを同時に推定することが好ましい。

第１の実施形態に係る位置姿勢推定装置は、例えば図６に例示されるコンピュータ６０を用いて実装することができる。コンピュータ６０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）６０１と、入力装置６０２と、表示装置６０３と、通信装置６０４と、記憶装置６０５とを含んでおり、これらはバス６０６を介して相互に接続されている。

ＣＰＵ６０１は、種々のプログラムを実行することで、コンピュータ６０の制御装置および演算装置として機能する電子回路である。ＣＰＵ６０１は、例えば、入力装置６０２、通信装置６０４または記憶装置６０５からデータを入力して演算処理を行う。そして、ＣＰＵ６０１は、演算結果または当該演算結果に基づく制御信号を例えば、表示装置６０３、通信装置６０４、記憶装置６０５へと出力する。

具体的には、ＣＰＵ６０１は、コンピュータ６０のＯＳ（オペレーティングシステム）に加えて、コンピュータ６０を第１の実施形態に係る位置姿勢推定装置として機能させるための位置姿勢推定プログラム（画像処理プログラムと呼ぶこともできる）を実行することにより、バス６０６を介して接続された各装置を制御する。

この位置姿勢推定プログラムは、一時的でない有形のコンピュータ読み取り可能な記録媒体に記録される。記録媒体は、例えば、光ディスク、光磁気ディスク、磁気ディスク、磁気テープ、フラッシュメモリまたは半導体メモリであってもよいが、これらに限られない。位置姿勢推定プログラムは、記憶装置６０５に予め保存されていてもよいし、記憶装置６０５以外の記録媒体に保存されていてもよいし、ネットワーク（例えばインターネット）上にアップロードされていてもよい。いずれにせよ、位置姿勢推定プログラムをコンピュータ６０にインストールし、ＣＰＵ６０１が当該位置姿勢推定プログラムを実行することによって、コンピュータ６０は第１の実施形態に係る位置姿勢推定装置として機能する。

入力装置６０２は、コンピュータ６０への入力情報を受付る。入力装置６０２は、第１の撮像部１０１および第２の撮像部１０２としてのディジタルカメラを含むことができる。入力装置６０２は、例えば、キーボード、マウス、タッチパネルなどをさらに含むこともできる。

表示装置６０３は、静止画像または動画像を表示する。表示装置６０３は、例えば、ＬＣＤ（Ｌｉｑｕｉｄ Ｃｒｙｓｔａｌ Ｄｉｓｐｌａｙ）、ＣＲＴ（Ｃａｔｈｏｄｅ Ｒａｙ Ｔｕｂｅ）、ＰＤＰ（Ｐｌａｓｍａ Ｄｉｓｐｌａｙ Ｐａｎｅｌ）などであってもよいが、これらに限られない。表示装置６０３は、例えば基準画像１１０および参照画像１１１を表示してもよいし、当該基準画像１１０および参照画像１１１に含まれる特徴点を表示してもよい。また、表示装置６０３は、位置姿勢推定プログラムの実行結果に基づいて作成されたマップ画像を表示してもよい。さらに、表示装置６０３は、第１の撮像部１０１（基準カメラ）の現在の位置姿勢を重畳したマップ画像を表示してもよいし、第１の撮像部１０１（基準カメラ）の位置姿勢の軌跡を重畳したマップ画像を表示してもよい。

通信装置６０４は、外部装置に対して無線または有線通信を行う。通信装置６０４は、例えば、モデム、ハブ、ルータなどであってもよいが、これらに限られない。なお、第１の撮像部１０１および第２の撮像部１０２は、コンピュータ６０ではなく外部装置に備え付けられていてもよい。この場合には、ＣＰＵ６０１は、通信装置６０４によって受信された基準画像１１０および参照画像１１１に基づいて位置姿勢の推定処理を行う。

記憶装置６０５は、種々のプログラム（例えば、コンピュータ６０のＯＳ、位置姿勢推定プログラムなど）、当該プログラムの実行に必要なデータ、当該プログラムの実行により生成されたデータなどを記憶する。

記憶装置６０５は、主記憶装置と外部記憶装置とを含む。主記憶装置は、例えば、ＤＲＡＭ（Ｄｙｎａｍｉｃ Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＳＲＡＭ（Ｓｔａｔｉｃ ＲＡＭ）などのＲＡＭであるが、これに限られない。外部記憶装置は、例えば、ハードディスク、光ディスク、フラッシュメモリ、磁気テープなどであるが、これらに限られない。

なお、ＣＰＵ６０１、入力装置６０２、表示装置６０３、通信装置６０４および記憶装置６０５は、それぞれ複数の装置を含むことができる。また、図６には示されていない周辺機器（例えば、プリンタ、スキャナ）がコンピュータ６０に接続されてもよい。

さらに、単独のコンピュータ６０ではなく相互に接続されたコンピュータ６０（すなわち、コンピュータシステム）を用いて、第１の実施形態に係る位置姿勢推定装置を実装することもできる。

以上説明したように、第１の実施形態に係る位置姿勢推定装置は、第１の時刻における第１の撮像部の第２の時刻を基準とした相対的な位置姿勢と、第２の撮像部が第１の参照画像および第２の参照画像を撮像した位置の理想的な撮像位置からのずれとを同時に推定する。従って、この位置姿勢推定装置によれば、第１の撮像部および第２の撮像部の撮像タイミングが同期していなくても、基準カメラとしての第１の撮像部の位置姿勢を推定することができる。すなわち、この位置姿勢推定装置は、比較的安価な汎用カメラ２台を備えるステレオカメラを用いて実装することができる。

なお、この位置姿勢推定装置は、第１の時刻または第２の時刻と第２の撮像部が第１の参照画像または第２の参照画像を撮像した時刻との間の時間差が未知であっても、当該第１の時刻または第２の時刻に関する第２の撮像部の理想的な撮像位置からのずれを推定できる。従って、この位置姿勢推定装置は、例えば上記時間差を計測するためのタイムスタンプを取得できない状況下であっても使用可能である。

さらに、この位置姿勢推定装置は、第１の時刻または第２の時刻から第１の参照画像または第２の参照画像が撮像されるまでの第２の撮像部の動きを例えば等速直線運動などと仮定することによる単純化をしない。故に、この位置姿勢推定装置によれば、この動きが回転運動または加速度運動を含んでいたとしても、第１の時刻または第２の時刻に関する第２の撮像部の理想的な撮像位置からのずれを高精度に推定することができる。

本実施形態の位置姿勢推定装置によると、移動体が、現在位置や目的位置を取得したり、現在位置から目的位置まで効率よく移動したりすることができる。移動体は、位置姿勢推定装置を備えていてもよいし、位置姿勢推定装置から遠隔に移動の指示を受けてもよい。

（第２の実施形態）
前述の第１の実施形態に係る位置姿勢推定装置は、例えば、図７乃至図９に例示される電気掃除機（電気掃除システム）１１に組み込むことができる。電気掃除機１１は、いわゆる自走式のロボットクリーナ（掃除ロボットとも呼ばれる）であって、掃除対象領域（例えば建物内の床面）上を自律走行しながら掃除を行う。

電気掃除機１１は、例えば図１０に示される充電装置１２を基地として、一連の掃除動作を行うことができる。すなわち、電気掃除機１１は、充電装置１２を出発して走行しながら掃除を行い、最終的に当該充電装置１２に帰還して次の掃除動作指示を待機する。充電装置１２は、電気掃除機１１と機械的および電気的に接続した状態で、当該電気掃除機１１に内蔵された二次電池２８を充電する。

電気掃除機１１は、図７および図１０に例示されるように、中継装置としてのホームゲートウェイ１４（ルータ、アクセスポイントなどであってもよい）と有線通信または無線通信（例えば、Ｗｉ−Ｆｉ（登録商標）、Ｂｌｕｅｔｏｏｔｈ（登録商標）など）を行ってもよい。ホームゲートウェイ１４は、インターネットなどのネットワーク１５に例えば有線で接続されており、当該ネットワーク１５を介して外部装置としてのサーバ１６、スマートフォン１７、ＰＣ１８（タブレットであってもよい）などの通信機器と通信可能である。すなわち、電気掃除機１１は、ホームゲートウェイ１４およびネットワーク１５を介して、サーバ１６、スマートフォン１７、ＰＣ１８などと通信することができる。

サーバ１６は、ネットワーク１５に接続されたコンピュータ（クラウドサーバ）であって、電気掃除機１１に関わる種々のデータ（例えば、電気掃除機１１から送信された画像）を保存することができる。サーバ１６は、保存しているデータを、スマートフォン１７またはＰＣ１８からの要求に応じて送信してもよい。

スマートフォン１７またはＰＣ１８は、ホームゲートウェイ１４が設置された建物の内部では例えば当該ホームゲートウェイ１４を介して電気掃除機１１と有線通信または無線通信を行うことができる。また、スマートフォン１７またはＰＣ１８は、建物の外部からもネットワーク１５を介して電気掃除機１１と有線通信または無線通信を行うことができる。スマートフォン１７またはＰＣ１８は、いずれも画像を表示する表示装置を含む。

このようなネットワークにより、例えば、電気掃除機１１のユーザは、スマートフォン１７またはＰＣ１８を操作して電気掃除機１１に動作を指示したり、電気掃除機１１の掃除作業の結果（例えば走行履歴）を画像としてスマートフォン１７またはＰＣ１８に備え付けられた表示装置を介して閲覧したりすることができる。この画像は、例えば、サーバ１６からダウンロードされる。

電気掃除機１１は、本体ケース２０を含む。この本体ケース２０は、例えば合成樹脂などにより略円柱（厚みのある円盤）状に形成される。すなわち、本体ケース２０は、それぞれ略円形の上面部２０ｂおよび下面部２０ｃと、これらを接続する略円筒状の側面部２０ａとを含む。また、本体ケース２０の内部は中空であって、種々の部品（例えば、制御部２７、二次電池２８、電動送風機４１、集塵部４６など）を内蔵する。

以降の説明では、電気掃除機１１（本体ケース２０）を走行させる駆動輪３４の回転軸方向に略平行な方向を左右方向（幅方向）とし、当該幅方向に略直交する電気掃除機１１の走行方向を前後方向（図８および図９のＦＲおよびＲＲ）とする。図８および図９では、理解を助けるために、前後方向に平行に中心線Ｌが描かれている。

本体ケース２０の側面部２０ａの前方両側には、図８に例示されるように、撮像部２５（より具体的には、左カメラ５１−Ｌおよび右カメラ５１−Ｒ）などが取り付けられている。左カメラ５１−Ｌおよび右カメラ５１−Ｒは、両者（の例えば光学中心）を結ぶ直線が例えば幅方向（すなわち、駆動輪３４の回転軸方向）に略平行になるように取り付けられる。左カメラ５１−Ｌおよび右カメラ５１−Ｒは、例えば可視光領域の画像を取得する。可視光領域の画像は、例えば赤外領域の画像に比べて画質が良好であるので、複雑な画像処理を施すことなくユーザに視認可能に表示することができる。

さらに、左カメラ５１−Ｌおよび右カメラ５１−Ｒの周囲には図示されないランプ（例えば、ＬＥＤ（Ｌｉｇｈｔ Ｅｍｉｔｔｉｎｇ Ｄｉｏｄｅ））が取り付けられてもよい。このランプは、例えば周囲の明るさが所定以下の場合に点灯し、それ以外の場合には点灯しないように例えば後述される制御部２７によって制御される。ランプは、点灯時に可視光領域を含む光を発生することで、左カメラ５１−Ｌおよび右カメラ５１−Ｒのための照明として機能し、物陰のような暗い場所や夜間などでも適切な画像を取得できるようにする。

なお、左カメラ５１−Ｌおよび右カメラ５１−Ｒは、赤外領域の画像を取得する赤外線カメラであってもよい。この場合には、図示されないランプによって赤外領域を含む光を発生することで、周囲の明るさを問わず安定的に適切な画像を取得することができる。さらに、このランプによって発生する光が可視光領域を含まないならば、当該ランプを物陰のような暗い場所や夜間などで点灯させたとしても、周囲の人間に明るく感じさせることなく撮像作業を遂行することができる。

本体ケース２０の下面部２０ｃは、走行時に掃除対象領域と向かい合う面であって、図９に例示されるように、電気掃除機１１を走行させるための駆動輪３４および旋回輪３６が露出している。また、下面部２０ｃには、集塵口としての吸込み口３１と、排気口３２とが開口されている。吸込み口３１付近には塵埃を当該吸込み口３１へと掻き揚げる回転清掃体としての回転ブラシ４２が回転可能に取り付けられており、下面部２０ｃの前方両側には塵埃を掻き集める補助掃除部としてのサイドブラシ４４が回転可能に取り付けられている。さらに、下面部２０ｃの後方両側には、本体ケース２０に内蔵された二次電池２８を充電装置１２に電気的および機械的に接続させて充電を可能とする充電端子７１が取り付けられている。

この電気掃除機１１は、図７に例示されるように、走行部２１と、掃除部２２と、通信部２３と、撮像部２５と、センサ部２６と、制御部２７と、二次電池２８とを含む。

二次電池２８は、電気掃除機１１の動力源であって、走行部２１、掃除部２２、通信部２３、撮像部２５、センサ部２６および制御部２７への給電を行う。なお、二次電池２８は、前述の充電端子７１と電気的に接続されており、当該充電端子７１を介して充電装置１２に電気的および機械的に接続することで充電が可能となる。

走行部２１は、電気掃除機１１の本体ケース２０を掃除対象領域上で走行させる。具体的には、走行部２１は、一対の駆動輪３４と、一対のモータ３５と、旋回輪３６とを含む。

各駆動輪３４は、対応するモータ３５から伝達される動力によって回転駆動され、電気掃除機１１を掃除対象領域上で前後方向に走行させる。各駆動輪３４は、その回転軸が前述のように本体ケース２０の幅方向に略平行であり、図９に例示されるように本体ケース２０の前後方向を軸として略対称に配置される。なお、駆動輪３４は、それぞれ対応するモータ３５によって独立に駆動される。

各モータ３５は、後述される走行制御部６６からの制御に従って、対応する駆動輪３４に動力を伝達する。旋回輪３６は、掃除対象領域上で旋回可能な従動輪であって、図９に例示されるように本体ケース２０の前方中央付近に配置される。

掃除部２２は、掃除対象領域上にある塵埃を掃除する。具体的には、掃除部２２は、電動送風機４１と、回転ブラシ４２と、ブラシモータ４３と、一対のサイドブラシ４４と、一対のサイドブラシモータ４５と、集塵部４６とを含む。なお、ここで例示された一部の要素を省略したとしても、掃除対象領域上の塵埃を掃除することは可能である。

電動送風機４１は、本体ケース２０に内蔵されており、後述される掃除制御部６７からの制御に従って動作する。電動送風機４１は、吸込み口３１から空気とともに塵埃を吸込み、吸い込んだ空気を排気口３２から排気する。電動送風機４１によって吸い込まれた塵埃は集塵部４６へと導かれる。

ブラシモータ４３は、掃除制御部６７からの制御に従って、前述の回転ブラシ４２を回転駆動する。サイドブラシモータ４５は、掃除制御部６７からの制御に従って、前述のサイドブラシ４４を個別に回転駆動する。集塵部４６は、吸込み口３１に連通して配置されており、当該吸込み口３１を介して吸い込まれた塵埃を集積する。

通信部２３は、充電装置１２またはホームゲートウェイ１４などの外部装置と通信をする。具体的には、通信部２３は、無線ＬＡＮデバイス４７を含む。通信部２３は、図６の通信装置６０４に相当する。

無線ＬＡＮデバイス４７は、ホームゲートウェイ１４およびネットワーク１５を介して外部装置（例えば、サーバ１６、スマートフォン１７、ＰＣ１８など）へと種々の情報を送信したり、逆に外部装置から種々の情報を受信したりする。無線ＬＡＮデバイス４７は、例えば本体ケース２０に内蔵することができる。

なお、通信部２３は、図示されない送信部および受信部を含むことができる。この送信部は、例えば充電装置１２へ赤外線信号（無線信号）を送信する赤外線発光素子であってよい。また、この受信部は、充電装置１２または図示されないリモートコントローラから送信された赤外線信号（無線信号）を受信するフォトトランジスタであってよい。

撮像部２５は、電気掃除機１１の周囲の画像を取得する。前述のように、撮像部２５は、左カメラ５１−Ｌおよび右カメラ５１−Ｒを含み、図示されないランプを含むこともできる。左カメラ５１−Ｌおよび右カメラ５１−Ｒは、前述の第１の撮像部１０１および第２の撮像部１０２にそれぞれ相当する。すなわち、左カメラ５１−Ｌおよび右カメラ５１−Ｒは、後述される撮像制御部６８からの制御に従って、個別の撮像タイミングで画像を取得する。なお、左カメラ５１−Ｌおよび右カメラ５１−Ｒによって取得された画像は、図示されない画像処理回路によって所定のデータ形式へと圧縮されてもよい。

センサ部２６は、例えば各駆動輪３４または各モータ３５の回転数を測定する回転数センサ５５を含む。回転数センサ５５は、例えば光エンコーダであってよい。なお、回転数センサ５５は省略可能である。

制御部２７は、走行部２１、掃除部２２、通信部２３および撮像部２５などを制御する。具体的には、制御部２７は、メモリ６１と、位置姿勢推定部６２と、走行制御部６６と、掃除制御部６７と、撮像制御部６８と、マップ作成部７０とを含む。制御部２７は、例えばマイクロコントローラであってよい。制御部２７は、図６のＣＰＵ６０１および記憶装置６０５に相当する。

なお、図７の機能分割は一例に過ぎない。例えば、位置姿勢推定部６２、走行制御部６６、掃除制御部６７、撮像制御部６８およびマップ作成部７０の一部または全部が、制御部２７とは独立して設けられてもよいし、これらのうちの２つ以上を任意に組み合わせることもできる。

メモリ６１には、例えば、左カメラ５１−Ｌおよび右カメラ５１−Ｒによって取得された画像などの種々の制御または演算に必要とされるデータが保存される。なお、メモリ６１に保存されたデータは、電気掃除機１１の電源状態に関わらず維持されることが好ましい。故に、メモリ６１は、例えばフラッシュメモリなどの不揮発性記録媒体である。

位置姿勢推定部６２は、図１の推定部１０３に相当する。すなわち、位置姿勢推定部６２は、左カメラ５１−Ｌおよび右カメラ５１−Ｒによって取得された画像（それぞれ基準画像１１０および参照画像１１１に対応）をメモリ６１から読み出して、前述の推定処理を行う。位置姿勢推定部６２は、基準カメラとしての左カメラ５１−Ｌの位置姿勢を示す推定データを生成する。なお、位置姿勢推定部６２は、推定した左カメラ５１−Ｌの位置姿勢を本体ケース２０の中心の位置姿勢へと変換してもよい。この推定データは、例えば走行制御部６６によって自律走行の制御（例えば、速度または方向の決定）に利用されたり、マップ作成部７０によってマップ作成に利用されたりする。

走行制御部６６は、各モータ３５に流れる電流の大きさおよび向きをそれぞれ制御することで、当該モータ３５を正転または逆転させる。各駆動輪３４は、対応するモータ３５の回転に連動し、電気掃除機１１を所望の方向へと走行させる。

掃除制御部６７は、電動送風機４１、ブラシモータ４３およびサイドブラシモータ４５を個別に導通角制御する。なお、掃除制御部６７は、電動送風機４１、ブラシモータ４３およびサイドブラシモータ４５のそれぞれにつき１つずつ設けられてもよい。撮像制御部６８は、左カメラ５１−Ｌおよび右カメラ５１−Ｒの撮像開始および撮像終了を個別制御する。

マップ作成部７０は、位置姿勢推定部６２によって生成された推定データに基づいてマップ画像を作成する。マップ作成部７０は、電気掃除機１１の外界だけでなく電気掃除機１１の現在の位置姿勢を重畳したマップ画像を作成してもよいし、電気掃除機１１の位置姿勢の軌跡（すなわち、走行履歴）を重畳したマップ画像を作成してもよい。

制御部２７の動作状態は、電気掃除機１１に自律走行しつつ掃除を行わせる掃除モードと、電気掃除機１１を充電装置１２に電気的および機械的に接続させて二次電池２８の充電を行う充電モードと、次の掃除モードが開始するまで電気掃除機１１を例えば充電装置１２に接続した状態で待機させる待機モードとに大別することができる。このうち充電モードでは、例えば充電装置１２に内蔵された定電流回路などの充電回路を用いた既知の手法が適用可能である。なお、制御部２７は、スマートフォン１７、ＰＣ１８または図示されないリモートコントローラからの指示に従って、左カメラ５１−Ｌおよび右カメラ５１−Ｒのうち少なくとも一方に例えば所定の対象物を撮像させてもよい。

例えば予め設定されている掃除開始時刻が到来したり、スマートフォン１７、ＰＣ１８または図示されないリモートコントローラから送信された掃除開始指令信号を受信したりすると、制御部２７の動作状態は待機モードまたは充電モードから掃除モードへと遷移する。掃除モードが開始すると、制御部２７の走行制御部６６は各モータ３５を駆動させ、電気掃除機１１は充電装置１２から所定距離離脱する。

それから、電気掃除機１１は、掃除対象領域上を自律走行しつつ掃除をする。電気掃除機１１の走行中に、制御部２７の撮像制御部６８は、左カメラ５１−Ｌおよび右カメラ５１−Ｒにそれぞれ周期的に撮像を行わせ、本体ケース２０の前方正面のステレオ画像（すなわち、基準画像１１０および参照画像１１１）を取得する。取得されたステレオ画像は、メモリ６１に保存される。

制御部２７の位置姿勢推定部６２は、メモリ６１に保存されているステレオ画像を読み出して、前述の推定処理を行う。位置姿勢推定部６２は、基準カメラとしての左カメラ５１−Ｌ（若しくは本体ケース２０の中心）の位置姿勢を示す推定データ１１２を生成する。そして、制御部２７の走行制御部６６は、この推定データ１１２に基づいて、電気掃除機１１の自律走行を制御する。

なお、メモリ６１に保存された画像は、所定のタイミングでホームゲートウェイ１４およびネットワーク１５を介してサーバ１６へと送信（アップロード）されてもよい。具体的には、画像は、電気掃除機１１が充電装置１２に帰還した時にアップロードされてもよいし、掃除作業中に定期的に若しくは不定期にアップロードされてもよいし、スマートフォン１７またはＰＣ１８などの外部装置からの要求に応じてアップロードされてもよい。送信済みの画像はメモリ６１から消去されてもよいし、当該画像の記憶領域が新たなデータを上書きするために利用されてもよい。これにより、メモリ６１の容量を効率よく使用することができる。なお、サーバ１６の代わりに、メモリ６１、スマートフォン１７またはＰＣ１８がデータを蓄積することも可能である。

制御部２７の掃除制御部６７は、電気掃除機１１の走行中に、電動送風機４１、ブラシモータ４３およびサイドブラシモータ４５を駆動する。これにより、掃除対象領域上にある塵埃は、吸込み口３１から吸い込まれて集塵部４６に集積される。

掃除対象領域の掃除作業が完了すると、制御部２７の走行制御部６６は電気掃除機１１を充電装置１２へと帰還させる。なお、予定された掃除作業を完了させるのに二次電池２８の容量が十分でない場合には、最終的に二次電池２８の容量が枯渇して電気掃除機１１は充電装置１２へ帰還できないかもしれない。故に、掃除作業の途中であっても二次電池２８の容量が所定値まで低下している（例えば、二次電池２８の電圧が放電終止電圧近傍まで低下している）場合には、制御部２７の走行制御部６６は電気掃除機１１を充電装置１２へと帰還させてもよい。充電端子７１が充電装置１２に電気的および機械的に接続されると、制御部２７の動作状態は掃除モードから充電モードに遷移する。充電完了後に、制御部２７の動作状態は待機モードへと遷移してもよいし、再び掃除モードに復帰してもよい。

以上説明したように、第２の実施形態に係る電気掃除機は、前述の第１の実施形態で記述されたように自己の位置姿勢を推定し、推定結果に基づいて自律走行する。故に、この電気掃除機によれば、自己の位置姿勢と周囲のマップとを考慮して走行することで、掃除作業を効率的に遂行することができる。また、この電気掃除機は、撮像タイミングの同期していないステレオカメラによって取得された画像に基づいて自己の位置姿勢を推定できるので、コストを削減することができる。

なお、スマートフォン１７またはＰＣ１８などの外部装置に表示される画像は、例えば制御部２７によって当該外部装置に表示できるように処理されてもよいし、当該外部装置にインストールされた専用のプログラム（アプリケーション）によって当該外部装置に表示できるように処理されてもよい。或いは、制御部２７またはサーバ１６が画像に対して予め処理を行うことで、外部装置はブラウザなどの汎用のプログラムを用いて画像を表示することもできる。すなわち、画像の表示制御は、制御部２７、サーバ１６または外部装置にインストールされたプログラムによって実現することができる。

また、左カメラ５１−Ｌおよび右カメラ５１−Ｒによって取得された画像は、例えば電気掃除機１１に備え付けられた表示部（例えば、図６の表示装置６０３に相当）に直接的に表示されてもよい。この場合には、電気掃除機１１がホームゲートウェイ１４およびネットワーク１５を介して画像を送信できなくても、ユーザは当該画像を閲覧することができる。すなわち、ネットワーク設備が貧弱な環境下での画像閲覧が可能となるとともに電気掃除機１１の構成および制御を簡略化することもできる。

さらに、例えば、左カメラ５１−Ｌおよび右カメラ５１−Ｒの視野外にある障害物を検出する接触センサなどのセンサが本体ケース２０の後部などに設けられてもよいし、掃除対象領域にある段差を検出する段差検出部（例えば、赤外線センサ）が本体ケース２０の下面部２０ｃなどに設けられてもよい。

電気掃除機（電気掃除システム）１１の位置姿勢推定部６２は、撮像部２５が収められた本体ケースの外に設けられていても良い。すなわち、例えば、制御部２７が、撮像部２５および掃除部２２と別に設けられていて、これらが無線または有線で通信してもよい。

本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。

１０・・・位置姿勢推定装置
１１・・・電気掃除機
１２・・・充電装置
１４・・・ホームゲートウェイ
１５・・・ネットワーク
１６・・・サーバ
１７・・・スマートフォン
１８・・・ＰＣ
２０・・・本体ケース
２０ａ・・・側面部
２０ｂ・・・上面部
２０ｃ・・・下面部
２１・・・走行部
２２・・・掃除部
２３・・・通信部
２５・・・撮像部
２６・・・センサ部
２７・・・制御部
２８・・・二次電池
３１・・・吸込み口
３２・・・排気口
３４・・・駆動輪
３５・・・モータ
３６・・・旋回輪
４１・・・電動送風機
４２・・・回転ブラシ
４３・・・ブラシモータ
４４・・・サイドブラシ
４５・・・サイドブラシモータ
４６・・・集塵部
４７・・・無線ＬＡＮデバイス
５１−Ｌ・・・左カメラ
５１−Ｒ・・・右カメラ
５５・・・回転数センサ
６０・・・コンピュータ
６１・・・メモリ
６２・・・位置姿勢推定部
６６・・・走行制御部
６７・・・掃除制御部
６８・・・撮像制御部
７０・・・マップ作成部
７１・・・充電端子
１０１・・・第１の撮像部
１０２・・・第２の撮像部
１０３・・・推定部
１１０・・・基準画像
１１１・・・参照画像
１１２・・・推定データ
６０１・・・ＣＰＵ
６０２・・・入力装置
６０３・・・表示装置
６０４・・・通信装置
６０５・・・記憶装置
Ｌ・・・中心線

Claims (7)

1. 撮像タイミングが互いに同期していない第１の撮像部と第２の撮像部とを有するステレオカメラの第１の撮像部の位置姿勢を推定する位置姿勢推定装置であって、
   第１の時刻に撮像された第１の基準画像と第２の時刻に撮像された第２の基準画像とを生成する前記第１の撮像部と、
   前記第１の時刻に関連付けられる第１の参照画像および前記第２の時刻に関連付けられる第２の参照画像を生成する前記第２の撮像部と、
   前記第１の基準画像と前記第２の基準画像と前記第１の参照画像と前記第２の参照画像とに基づいて、前記第２の時刻における前記第１の撮像部の位置姿勢を基準とした前記第１の時刻における前記第１の撮像部の位置姿勢と、前記第１の時刻における前記第２の撮像部の理想的な推定位置に対する前記第２の撮像部が前記第１の参照画像を撮像した位置のずれとを推定する推定部と
   を具備し、
   前記推定部は、前記第１の時刻における前記第２の撮像部の理想的な推定位置を、前記第１の撮像部と前記第２の撮像部との位置関係に応じて前記第１の時刻における前記第１の撮像部の位置をシフトさせることによって算出し、
   前記推定部は、前記第１の基準画像と前記第２の基準画像と前記第１の参照画像と前記第２の参照画像との間で対応する特徴点を探索し、当該特徴点の位置を前記第２の基準画像および前記第２の参照画像に基づいて復元し、復元した位置を前記第１の時刻において前記第１の撮像部および前記第２の撮像部に投影することで得られる再投影点と、前記第１の基準画像および前記第１の参照画像に含まれる前記特徴点との間の誤差が最小となるように、前記第２の時刻における前記第１の撮像部の位置姿勢を基準とした前記第１の時刻における前記第１の撮像部の位置姿勢と、前記第１の時刻における前記第２の撮像部の理想的な推定位置に対する前記第２の撮像部が前記第１の参照画像を撮像した位置のずれとを推定する、位置姿勢推定装置。
2. 前記推定部は、前記第１の時刻における前記第２の撮像部の理想的な推定位置に対する前記第２の撮像部が前記第１の参照画像を撮像した位置のずれを回転運動とみなす、請求項１に記載の位置姿勢推定装置。
3. 前記推定部は、さらに前記第２の撮像部が前記第２の参照画像を撮像した位置の前記第２の時刻における前記第２の撮像部の理想的な推定位置からのずれを推定し、
   前記推定部は、前記第２の撮像部が前記第２の参照画像を撮像した位置の前記第２の時刻における前記第２の撮像部の理想的な推定位置を、前記第１の撮像部と前記第２の撮像部との位置関係に応じて前記第２の時刻における前記第１の撮像部の位置をシフトさせることによって算出する、請求項１に記載の位置姿勢推定装置。
4. 前記推定部は、前記第２の時刻における前記第１の撮像部の位置姿勢を基準とした前記第１の時刻における前記第１の撮像部の位置姿勢を２自由度の並進成分および１自由度の回転成分として推定し、前記第１の時刻における前記第２の撮像部の理想的な推定位置に対する前記第２の撮像部が前記第１の参照画像を撮像した位置のずれを１自由度の回転成分として推定する、請求項１に記載の位置姿勢推定装置。
5. 請求項１乃至請求項４のいずれか１項に記載の位置姿勢推定装置を具備する、電気掃除機システム。
6. 前記電気掃除機を走行させる駆動輪をさらに具備し、
   前記駆動輪の回転軸は、前記第１の撮像部および前記第２の撮像部を結ぶ直線に対して略平行である、
   請求項５記載の電気掃除機システム。
7. コンピュータを
   撮像タイミングが互いに同期していない第１の撮像部と第２の撮像部とを有するステレオカメラの第１の撮像部の位置姿勢を推定する位置姿勢推定装置における、
   第１の時刻に撮像された第１の基準画像と第２の時刻に撮像された第２の基準画像とを生成する前記第１の撮像部の制御手段、
   前記第１の時刻に関連付けられる第１の参照画像と前記第２の時刻に関連付けられる第２の参照画像とを生成する前記第２の撮像部の制御手段、
   前記第１の基準画像と前記第２の基準画像と前記第１の参照画像と前記第２の参照画像とに基づいて、前記第２の時刻における前記第１の撮像部の位置姿勢を基準とした前記第１の時刻における前記第１の撮像部の位置姿勢と、前記第１の時刻における前記第２の撮像部の理想的な推定位置に対する前記第２の撮像部が前記第１の参照画像を撮像した位置のずれとを推定する推定手段、
   として機能させ、
   前記推定手段は、前記第１の時刻における前記第２の撮像部の理想的な推定位置を、前記第１の撮像部と前記第２の撮像部との位置関係に応じて前記第１の時刻における前記第１の撮像部の位置をシフトさせることによって算出し、
   前記推定手段は、前記第１の基準画像と前記第２の基準画像と前記第１の参照画像と前記第２の参照画像との間で対応する特徴点を探索し、当該特徴点の位置を前記第２の基準画像および前記第２の参照画像に基づいて復元し、復元した位置を前記第１の時刻において前記第１の撮像部および前記第２の撮像部に投影することで得られる再投影点と、前記第１の基準画像および前記第１の参照画像に含まれる前記特徴点との間の誤差が最小となるように、前記第２の時刻における前記第１の撮像部の位置姿勢を基準とした前記第１の時刻における前記第１の撮像部の位置姿勢と、前記第１の時刻における前記第２の撮像部の理想的な推定位置に対する前記第２の撮像部が前記第１の参照画像を撮像した位置のずれとを推定する、
   位置姿勢推定プログラム。