JP6288060B2 - 自律移動装置、自律移動方法及びプログラム - Google Patents

Description

本発明は、自律移動装置、自律移動方法及びプログラムに関する。

用途に応じて自律的に移動する自律移動装置が普及してきている。例えば、屋内の掃除のために自律的に移動する自律移動装置が知られている。一般的に、このような自律移動装置は、実空間の地図の作成と、実空間内での自機位置の推定を行う必要がある。

実空間の地図を作成するための手法としては、例えばＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）法が知られている。単眼カメラを用いたＳＬＡＭ技術の基本的な原理は、非特許文献１に記載されており、カメラの撮影する動画像の複数フレームから、同一の特徴点を追跡することで、自機の３次元位置（カメラ位置）と特徴点の３次元位置（これが集まって地図の情報を構成する）とを交互に推定する処理を行っている。また、実空間内で環境地図を作成して自律移動を行う自律移動装置がある（例えば、特許文献１参照）。

Ａｎｄｒｅｗ Ｊ．Ｄａｖｉｓｏｎ， "Ｒｅａｌ−Ｔｉｍｅ Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ ｗｉｔｈ ａ Ｓｉｎｇｌｅ Ｃａｍｅｒａ"， Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ ９ｔｈ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ Ｖｏｌｕｍｅ ２， ２００３， ｐｐ．１４０３−１４１０ Ｒｉｃｈａｒｄ Ｈａｒｔｌｅｙ， Ａｎｄｒｅｗ Ｚｉｓｓｅｒｍａｎ， "Ｍｕｌｔｉｐｌｅ Ｖｉｅｗ Ｇｅｏｍｅｔｒｙ ｉｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ"， Ｓｅｃｏｎｄ Ｅｄｉｔｉｏｎ， Ｃａｍｂｒｉｄｇｅ． Ｕｎｉｖｅｒｓｉｔｙ Ｐｒｅｓｓ， Ｍａｒｃｈ ２００４， ｃｈａｐｔｅｒ ９

特開２００９−１６９８４５号公報

上記の方法では、累積誤差が発生してしまう。更には、一旦ノイズ等により推定精度が低下してしまうと、その後の累積誤差は実用上無視できない程に大きくなってしまう。しかしながら、誤差が存在していても、ＳＬＡＭ法により矛盾なく位置の推定ができている間は、自機位置や地図に誤差が含まれているかどうかの判断ができないという問題があった。また、特許文献１に記載の技術では誤差が含まれているかどうかをオペレータに判断させているが、自律移動装置が自ら判断することはできなかった。

そこで、本発明は、上記問題を解決するためになされたものであり、自機位置や地図に誤差が含まれているかどうかの判断を行うことができる自律移動装置等を提供することを目的とする。

上記目的を達成するため、本発明の自律移動装置は、
移動手段による移動量に基づいて自機位置を計測する位置計測部と、
撮像部が撮影した複数の画像の情報を用いて作成した地図を記憶する地図記憶部と、
前記撮像部が撮影した複数の画像の情報と前記地図記憶部に記憶されている地図の情報とに基づいて自機位置を推定する位置推定部と、
前記位置計測部が計測した自機位置と前記位置推定部が推定した自機位置との差が所定誤差内か否かを判定する判定部と、
前記判定部が前記所定誤差内でないと判定した場合、前記地図記憶部に記憶されている地図を編集する地図編集部と、
自機の向きの変化量を取得する向き変化量取得部と、
を備え、
前記判定部は、前記向き変化量取得部が取得した向きの変化量が基準向き変化量よりも小さいときに判定を行う、
ことを特徴とする。

本発明によれば、自機位置や地図に誤差が含まれているかどうかの判断を行うことができる。

実施形態に係る自律移動装置の外観を示す図である。 実施形態に係る自律移動装置の構成を示す図である。 実施形態に係る自律移動制御処理全体のフローチャートを示す図である。 実施形態に係る自律移動制御処理の中の自機位置推定スレッドの処理のフローチャートを示す図である。 実施形態に係る自律移動制御処理の中の地図作成スレッドの処理のフローチャートを示す図である。 実施形態に係る自律移動制御処理の中のループクロージングスレッドの処理のフローチャートを示す図である。 実施形態に係る自律移動制御処理の中の誤差判定スレッドの処理のフローチャートを示す図である。

以下、図１を参照しながら、本発明の実施形態に係る自律移動装置について説明する。自律移動装置１００は、用途に応じて自律的に移動する。この用途とは、例えば、警備監視用、屋内掃除用、ペット用、玩具用などである。

自律移動装置１００は、外観上、撮像部４１、駆動部４２を備える。

撮像部４１は、単眼の撮像装置（カメラ）を備える。撮像部４１は、例えば、３０ｆｐｓで画像（フレーム）を取得する。自律移動装置１００は、撮像部４１が逐次取得した画像に基づいて、自機位置と周囲環境とをリアルタイムに認識しながら、自律移動を行う。

駆動部４２は、独立２輪駆動型であって、車輪とモータとを備える移動手段である。自律移動装置１００は、２つの車輪の同一方向駆動により前後の平行移動（並進移動）を、２つの車輪の逆方向駆動によりその場での回転（向き変更）を、２つの車輪のそれぞれ速度を変えた駆動により旋回移動（並進＋回転（向き変更）移動）を、行うことができる。また、各々の車輪にはロータリエンコーダが備えられており、ロータリエンコーダで車輪の回転数を計測し、車輪の直径や車輪間の距離等の幾何学的関係を利用することで並進移動量及び回転量を計算できる。例えば、車輪の直径をＤ、回転数をＲ（ロータリエンコーダにより測定）とすると、その車輪の接地部分での並進移動量はπ・Ｄ・Ｒとなる。また、車輪の直径をＤ、車輪間の距離をＩ、右車輪の回転数をＲ_Ｒ、左車輪の回転数をＲ_Ｌとすると、向き変更の回転量は（右回転を正とすると）３６０°×Ｄ×（Ｒ_Ｌ−Ｒ_Ｒ）／（２×Ｉ）となる。この並進移動量や回転量を逐次足し合わせていくことで、駆動部４２は、いわゆるオドメトリとして機能し、自機位置（移動開始時の位置及び向きを基準とした位置及び向き）を計測することができる。

オドメトリから得られる自機の位置及び向きの精度は、車輪の摩耗やスリップ等により、低い精度になってしまうことが多い。特に誤差が累積していく関係上、時間経過と共に精度が悪化する。ただし、オドメトリの回転成分（向きの情報）に関しては、後述する角速度センサ情報を用いることで精度を向上させることができる。また、地磁気を検出して方位を特定する方位センサ（図示せず）を用いることで、オドメトリからの取得値とは関係なく、地磁気を利用した絶対的な向きの情報を取得することもできる。

なお、車輪の代わりにクローラを備えるようにしても良いし、複数（例えば二本）の足を備えて足で歩行することによって移動を行うようにしても良い。これらの場合も、二つのクローラの動きや、足の動きに基づいて、車輪の場合と同様に自機の位置や向きの計測が可能である。

図２に示すように、自律移動装置１００は、撮像部４１、駆動部４２に加えて、制御部１０、記憶部２０、センサ部３０、入力部４３、通信部４４、電源４５、を備える。

制御部１０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）等で構成され、記憶部２０に記憶されたプログラムを実行することにより、後述する各部（位置計測部１１、地図作成部１２、位置推定部１３、判定部１４、地図編集部１５）の機能を実現する。

記憶部２０は、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ)、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）等で構成され、画像記憶部２１や地図記憶部２２を含む。ＲＯＭには制御部１０のＣＰＵが実行するプログラム（例えば、後述するＳＬＡＭ法の演算や自律移動制御処理に係るプログラム）や、プログラムを実行する上で予め必要なデータが記憶されている。ＲＡＭには、プログラム実行中に作成されたり変更されたりするデータが記憶される。

画像記憶部２１には、撮像部４１が撮影した画像が記憶される。ただし、記憶容量の節約のために、撮影したすべての画像を記憶しなくてもよく、また画像自体ではなく、画像の特徴量を記憶するようにしても良い。重要な画像（後述するキーフレーム）については、画像の情報と共に、上述したオドメトリで計測した自機位置（自機の位置及び向き）の情報並びに後述するＳＬＡＭ法で推定した自機位置（自機の位置及び向き）の情報が記憶される。

地図記憶部２２には、後述するＳＬＡＭ法や障害物センサ３３からの情報に基づいて地図作成部１２が作成した地図（特徴点や障害物の３次元位置の情報）が記憶される。

センサ部３０として、加速度センサ３１、角速度センサ３２、障害物センサ３３を備える。加速度センサ３１は、ＸＹＺ方向（３軸）の加速度を測定するセンサである。角速度センサ３２は角速度（単位時間あたりの角度移動量）を測定するセンサである。車輪の回転数に基づいて自機の向きを求めるよりも、角速度センサ３２を用いて向きを求める方が精度が向上することが知られている。障害物センサ３３は、走行中の障害物を検知するセンサであって、例えば、赤外線センサ、超音波センサである。なお、独立した障害物センサ３３を搭載せずに、撮像部４１を用いて障害物を検知するようにしても良い。また、他の物体に衝突したことを検知するバンパーセンサー（図示せず）を備えても良い。また、地磁気を検出して方位を特定する方位センサ（図示せず）を備えても良い。方位センサを用いることで、オドメトリからの取得値とは関係なく、地磁気を利用した絶対的な向きの情報を取得することができる。

入力部４３として、自律移動装置１００を操作するための操作ボタンを備える。操作ボタンは、例えば、電源ボタン、モード切替ボタン（掃除モード、ペットモード等を切り替える）、初期化ボタン（地図の作成をやり直しさせる）などを含む。入力部４３として、音の入力を行うマイク（図示せず）と、自律移動装置１００への操作指示の音声を認識する音声認識部を備えても良い。

通信部４４は、外部装置と通信するためのモジュールであり、外部装置と無線通信する場合にはアンテナを含む無線モジュールである。例えば、通信部４４は、Ｂｌｕｅｔｏｏｔｈ（登録商標）に基づく近距離無線通信を行うための無線モジュールである。通信部４４では、自律移動装置１００と外部とのデータ受け渡し等を行う。

電源４５は、自律移動装置１００を動作させる電源であり、一般的には内蔵された充電池であるが、太陽電池であっても良いし、床面から無線で電力供給されるシステムであっても良い。電源４５が充電池の場合は、充電ステーション（ホームベース）に自律移動装置１００がドッキングすることで充電される。

次に、制御部１０の機能について説明する。制御部１０は、位置計測部１１、地図作成部１２、位置推定部１３、判定部１４、地図編集部１５を含み、後述するＳＬＡＭ法の演算や自律移動装置１００の移動指示等を行う。また、制御部１０は、マルチスレッド機能に対応しており、複数のスレッド（異なる処理の流れ）を並行して進めることができる。

位置計測部１１は、駆動部４２の車輪とモータの動きに基づき、自機位置を計測する。具体的には、地面に高低差がなく、車輪がスリップしないという前提のもとで、各車輪の直径（Ｄ）及び回転数（Ｒ：ロータリエンコーダにより測定）に基づき、その車輪の接地部分での移動距離はπ・Ｄ・Ｒとして求められるので、これらと車輪間の距離とから並進移動量及び並進方向並びに向きの変化量（回転角度）を求めることができる。これを逐次加算していくことで、オドメトリとして、自機の位置及び向きを計測できる。地面に高低差がある場合は高さ方向を考慮した並進移動量を求める必要があるが、これは、加速度センサ３１により高度の変化量を把握することにより求められる。また車輪のスリップに関しては、駆動用の車輪以外に移動量計測用の車輪も搭載しておくことで誤差を小さくすることが可能である。

地図作成部１２は、画像記憶部２１に記憶されている画像の情報並びに該画像撮影時の自機の位置及び向きの情報に基づいてＳＬＡＭ法を用いて推定された特徴点の３次元位置（Ｍａｐ点）や、障害物センサ３３で障害物を検知したときの自機の位置及び向きの情報に基づいて得られた該障害物の３次元位置等を、地図の情報として地図記憶部２２に記憶する。

位置推定部１３は、後述するＳＬＡＭ法に基づいて、ビジュアルオドメトリとして、自機の位置及び向きを推定する。

判定部１４は、位置計測部１１で計測した位置及び向きと、位置推定部１３で推定した位置及び向きとの差が所定誤差内か否かを判定する。オドメトリ（位置計測部１１）で正しい位置及び向きが計測でき、しかも、ＳＬＡＭ法（ビジュアルオドメトリ：位置推定部１３）で正しい位置及び向きを推定できているなら、これらはほぼ一致するので、その場合は地図記憶部２２に記憶されている地図の情報も正しいと推測できる。しかし、位置及び向きの計測や推定に誤差が含まれていると、誤差の累積が生じ、計測値と推定値とは一致しなくなる。そのような場合は、地図記憶部２２に記憶されている地図の情報にも誤差が累積されてしまっていると推測できる。したがって、これら両者の差が所定誤差内か否かを判定することで、地図の情報が正しいかどうかを判定することができる。

地図編集部１５は、判定部１４によって地図記憶部２２に記憶されている地図の情報が正しくないと判定された場合は、該地図を編集する。地図の編集の最も簡単な処理は、該地図を消去する処理である。

図３は、自律移動装置１００のメインフローである。制御部１０は、最初に初期化済フラグをクリアしてから自機位置推定スレッドを起動し（ステップＳ１０１）、次に地図作成スレッド（ステップＳ１０２）、ループクロージングスレッド（ステップＳ１０３）、誤差判定スレッド（ステップＳ１０４）の各スレッドを起動する。自機位置推定スレッドや地図作成スレッドが動作することで、ＳＬＡＭ法に基づいて、地図の情報及びビジュアルオドメトリ（地図と画像とを用いて推定した自機位置の情報）の生成が開始される。誤差判定スレッドが、本実施形態の特徴的な機能を実現しており、詳細については後述する。

その後、制御部１０は、動作終了かどうかを判定し（ステップＳ１０５）、動作終了なら（ステップＳ１０５；Ｙｅｓ）動作を終了し、動作終了でなければ（ステップＳ１０５；Ｎｏ）地図作成部１２が地図の情報の作成・更新を行う（ステップＳ１０６）。次に制御部１０は自律移動するために駆動部４２に所望の動作を指示し（ステップＳ１０７）、ステップＳ１０５に戻る。これにより、地図の情報に基づいて自律的に移動を行いつつ、適宜地図の情報を更新することができる。

典型的な例としては、自律移動装置１００は最初、充電ステーションに置いてある状態で電源を投入すると、障害物センサ３３を頼りとして、家の各部屋をくまなく移動し、障害物センサ３３によって壁等の障害物位置を特定し、障害物位置を含む地図の情報を作成することができる。地図がある程度作成されると、地図の情報がまだないが移動可能と考えられる領域を知ることができ、その領域に自律的に移動する等して、より広範囲の地図の作成を促すこともできるようになる。そして、移動可能なほぼ全域の地図の情報が作成されれば、地図の情報を利用した効率的な移動動作が可能になる。例えば部屋のどの位置からでも最短経路で充電ステーションに戻ったり、効率的に部屋の掃除をしたりすることが可能になる。

自律移動装置１００のメインフロー（図３）のステップＳ１０１で起動される自機位置推定スレッドについて、図４を用いて説明する。このスレッドは、位置推定部１３が、最初に初期化処理を行い、その後自機位置推定（撮像部４１で取得した画像を用いてビジュアルオドメトリにより自機位置を推定する）を続ける処理である。

位置推定部１３は、まず、動作終了かを判定する（ステップＳ２０１）。動作終了なら（ステップＳ２０１；Ｙｅｓ）終了し、動作終了でないなら（ステップＳ２０１；Ｎｏ）、初期化済であるか判定する（ステップＳ２０２）。初期化済なら（ステップＳ２０２；Ｙｅｓ）ステップＳ２２１以降の自機位置推定処理を行い、初期化済でないなら（ステップ２０２；Ｎｏ）ステップＳ２０３に進んで初期化処理を行う。まず初期化処理について説明する。

初期化処理では位置推定部１３はまずフレームカウンタＮに−１をセットし（ステップＳ２０３）、撮像部４１で画像を取得する（ステップＳ２０４）。画像は例えば３０ｆｐｓで取得することができる（取得した画像はフレームとも呼ばれる）。次に、取得した画像内から２Ｄ特徴点を取得する（ステップＳ２０５）。２Ｄ特徴点とは画像中のエッジ部分など、画像内の特徴的な部分であり、ＳＩＦＴ（Ｓｃａｌｅ−Ｉｎｖａｒｉａｎｔ Ｆｕｔｕｒｅ Ｔｒａｎｓｆｏｒｍ）やＳＵＲＦ（Ｓｐｅｅｄ−Ｕｐ Ｒｏｂｕｓｔ Ｆｅａｔｕｒｅｓ）等のアルゴリズムを用いて取得することができる。なお、２Ｄ特徴点を取得するのに他のアルゴリズムを使っても良い。

取得した２Ｄ特徴点の個数が少ないと、後述するＴｗｏ−ｖｉｅｗ Ｓｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎ法での計算ができないため、位置推定部１３はステップＳ２０６において、２Ｄ特徴点の取得数と基準値（例えば１０個）とを比較し、基準値未満だった場合（ステップＳ２０６；Ｎｏ）はステップＳ２０４に戻り、基準値以上の２Ｄ特徴点数が得られるまで、画像の取得と２Ｄ特徴点の取得とを繰り返す。なお、この時点ではまだ地図の情報は作成できていないが、例えば上述した典型的な例では、障害物センサ３３を頼りとして、家の各部屋をくまなく移動し始めているため、この初期化処理で画像取得と２Ｄ特徴点取得を繰り返していれば、移動しながら画像取得を繰り返すことになるので、様々な画像が取得でき、いずれは２Ｄ特徴点数が多い画像を取得できることが期待できる。

２Ｄ特徴点の取得数が基準値以上だった場合は（ステップＳ２０６；Ｙｅｓ）、位置推定部１３はフレームカウンタＮをインクリメントする（ステップＳ２０７）。そして、フレームカウンタＮが０かどうかを判定する（ステップＳ２０８）。フレームカウンタＮが０なら（ステップＳ２０８；Ｙｅｓ）画像をまだ一つしか取得していないということになるので、２枚目の画像を取得するためにステップＳ２０４に戻る。なお図４のフローチャートでは記載していないが、１枚目の画像を取得した時の自機の位置と２枚目の画像を取得する時の自機の位置とがある程度離れていた方が、これ以降の処理で推定する姿勢の精度が向上する。そこで、ここでステップＳ２０８からステップＳ２０４に戻る際に、メインフロー（図３）のステップＳ１０７での駆動部４２への動作指示によりオドメトリによる並進距離が所定の距離（例えば１ｍ）以上となるまで待つ処理を追加しても良い。

フレームカウンタＮが０でないなら（ステップＳ２０８；Ｎｏ）二つの画像を取得したということがわかるので、位置推定部１３は、これら二つの画像間で２Ｄ特徴点の対応（実環境上の同一の点がそれぞれの画像中に存在し、その対応が取れるもの）を取得する（ステップＳ２０９）。ここで特徴点の対応数が５未満であれば、後述する二つの画像間の姿勢の推定ができないため、位置推定部１３は特徴点の対応数が５未満かどうかを判定する（ステップＳ２１０）。５未満であれば（ステップＳ２１０；Ｙｅｓ）初期画像を取得し直すために、ステップＳ２０３に戻る。特徴点の対応数が５点以上であれば（ステップＳ２１０；Ｎｏ）、Ｔｗｏ−ｖｉｅｗ Ｓｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎ法を用いることで、二つの画像間の姿勢（それぞれの画像を取得した位置の差分（並進ベクトルｔ）及び向きの差分（回転行列Ｒ））を推定することができる（ステップＳ２１１）。

この推定は具体的には、対応する特徴点から基礎行列Ｅを求め、基礎行列Ｅを並進ベクトルｔと回転行列Ｒとに分解することによって得られるが、詳細は非特許文献２で説明されているため、ここでは割愛する。なお、ここで得られる並進ベクトルｔ（３次元空間内で移動することを想定すると、最初の画像を取得した位置を原点として、Ｘ，Ｙ，Ｚの３要素を持つ）の各要素の値は実環境上での値とは異なる（Ｔｗｏ−ｖｉｅｗ Ｓｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎ法では実環境上の値自体を得ることはできず、実環境と相似する空間上での値を得ることになる。）ため、これらをＳＬＡＭ空間上での値とみなし、以下ではＳＬＡＭ空間上での座標（ＳＬＡＭ座標）を用いて説明する。

二つの画像間の姿勢（並進ベクトルｔ及び回転行列Ｒ）が求まると、その値は、最初の画像を基準（最初の画像を取得した位置をＳＬＡＭ座標の原点、並進ベクトルは０ベクトル、回転行列は単位行列Ｉとする。）にした場合の、二枚目の画像の姿勢（二つ目の画像を取得した時の自機の位置（並進ベクトルｔ）及び向き（回転行列Ｒ））となる。ここで、二つの画像それぞれの姿勢（該画像（フレーム）撮影時の自機の位置（並進ベクトルｔ）及び向き（回転行列Ｒ）で、フレーム姿勢とも言う。）が求まっている場合、その二つの画像間で対応が取れている２Ｄ特徴点（対応特徴点）のＳＬＡＭ座標での３Ｄ位置を、以下の考え方に基づき、地図作成部１２が求める（ステップＳ２１２）。

２Ｄ特徴点の画像中の座標（フレーム座標：既知）を（ｕ，ｖ）とし、その２Ｄ特徴点のＳＬＡＭ座標での３Ｄ位置（未知）を（Ｘ，Ｙ，Ｚ）とすると、これらを同次座標で表したときのこれらの関係は、透視投影行列Ｐを用いて下記の式（１）で表される。ここで、「〜」記号は「非零の定数倍を除いて等しい」（つまり、等しいか又は定数（非零）倍になっている）ことを表し、「’」記号は「転置」を表す。
（ｕ ｖ １）’〜Ｐ（Ｘ Ｙ Ｚ １）’…（１）

上記の式（１）において、Ｐは３×４の行列で、カメラの内部パラメータを示す３×３の行列Ａと、その画像の姿勢（フレーム姿勢）を示す外部パラメータＲ及びｔから以下の式（２）で表される。ここで、（Ｒ｜ｔ）は、回転行列Ｒの右に並進列ベクトルｔを並べた行列を表す。
Ｐ＝Ａ（Ｒ｜ｔ）…（２）

上記の式（２）において、Ｒ及びｔは上述したようにそのフレーム姿勢として求められている。また、カメラの内部パラメータＡは、焦点距離と撮像素子サイズにより決まるので、撮像部４１を決めておけば定数となる。

二つの画像間で対応が取れている２Ｄ特徴点のうちの一つが、一つ目の画像のフレーム座標（ｕ_１，ｖ_１）と、二つ目の画像のフレーム座標（ｕ_２，ｖ_２）に写っているとすると、以下の式（３）及び式（４）ができる。ここで、Ｉは単位行列、０はゼロベクトル、（Ｌ｜ｒ）は、行列Ｌの右に列ベクトルｒを並べた行列を表す。
（ｕ_１ ｖ_１ １）’〜Ａ（Ｉ｜０）（Ｘ Ｙ Ｚ １）’…（３）
（ｕ_２ ｖ_２ １）’〜Ａ（Ｒ｜ｔ）（Ｘ Ｙ Ｚ １）’…（４）

上記の式（３）及び式（４）において、ｕ_１，ｖ_１，ｕ_２，ｖ_２それぞれについての式ができるため、式は４つできるが、未知数はＸ，Ｙ，Ｚの３つなので、Ｘ，Ｙ，Ｚを求めることができ、これがその２Ｄ特徴点のＳＬＡＭ座標における３Ｄ位置となる。なお、式の個数の方が未知数の個数よりも多いため、例えばｕ_１，ｖ_１，ｕ_２で求めたＸ，Ｙ，Ｚとｕ_１，ｖ_１，ｖ_２で求めたＸ，Ｙ，Ｚとが異なる場合がありうる。このような場合は、過剰条件の連立一次方程式となり、一般には解が存在しないが、地図作成部１２は、最小二乗法を用いて、最も確からしいＸ，Ｙ，Ｚを求める。

２Ｄ特徴点のＳＬＡＭ座標における３Ｄ位置（Ｘ，Ｙ，Ｚ）が求まったら、それをＭａｐ点として、地図作成部１２がＭａｐ点データベース（Ｍａｐ点ＤＢとも言い、地図記憶部２２に格納される）に登録する（ステップＳ２１３）。Ｍａｐ点データベースに登録する要素としては、少なくとも、「２Ｄ特徴点のＳＬＡＭ座標における３Ｄ位置であるＸ，Ｙ，Ｚ」と、「その２Ｄ特徴点の特徴量」（例えばＳＩＦＴ等で得た特徴量）が必要である。「タイムスタンプ」（後述するキーフレームカウンタＮＫＦ（現在のキーフレーム番号を表す変数）のＭａｐ点データベースへの登録時点での値等）をＭａｐ点データベースへの登録要素に追加しておくと、Ｍａｐ点データベースを編集（過去の状態に戻す等）する際に便利である。

そして、地図作成部１２は、二つの画像間で対応が取れている２Ｄ特徴点（対応特徴点）の全てをＭａｐ点データベースに登録したかを判定し（ステップＳ２１４）、まだ全ての登録ができていなかったら（ステップＳ２１４；Ｎｏ）ステップＳ２１２に戻り、全て登録できたら（ステップＳ２１４；Ｙｅｓ）ステップＳ２１５に進む。

次に位置推定部１３は、キーフレーム（後に続くスレッドでの処理対象となる画像を指す）のカウンタＮＫＦを０に初期化し（ステップＳ２１５）、二つ目の画像をキーフレームとしてフレームデータベース（フレームＤＢとも言い、画像記憶部２１に格納される）に登録する（ステップＳ２１６）。

フレームデータベースに登録する要素は、「キーフレーム番号」（登録時点でのキーフレームカウンタＮＫＦの値）、「姿勢」（その画像撮影時の自機のＳＬＡＭ座標内での位置（並進ベクトルｔ）及び向き（回転行列Ｒ））、「オドメトリで計測した実環境上での姿勢」（実環境での駆動部４２による移動距離に基づいて求められる自機の位置及び向き）、「抽出した全ての２Ｄ特徴点」、「全ての２Ｄ特徴点の中でＭａｐ点として３Ｄ位置が既知の点」、「キーフレーム自体の特徴」、である。

上記中、「オドメトリで計測した実環境上での姿勢」は、並進ベクトルｔと回転行列Ｒとで表すこともできるが、通常、本自律移動装置１００は２次元平面上を動くので、２次元データに単純化して、移動開始時の位置（原点）及び向きを基準にした２次元座標（Ｘ，Ｙ）及び向きφとして表しても良い。また、「キーフレーム自体の特徴」とは、キーフレーム間の画像類似度を求める処理を効率化するためのデータであり、通常は画像中の２Ｄ特徴点のヒストグラム等を用いるのが良いが、画像自体を「キーフレーム自体の特徴」としても良い。

次に、位置推定部１３は、キーフレームが生成された事を地図作成スレッドに知らせるために、地図作成スレッドのキーフレームキュー（キュー（Ｑｕｅｕｅ）は、先入れ先出しのデータ構造になっている）に、キーフレームカウンタＮＫＦをセットする（ステップＳ２１７）。

以上で自機位置推定スレッドの初期化処理が完了したので、位置推定部１３は、初期化済フラグをセットする（ステップＳ２１８）。

そして、位置推定部１３は、ＳＬＡＭ座標と実環境座標とのスケール対応を得るために、オドメトリによる並進距離（実環境での座標で求められる）を、上記の処理で推定したＳＬＡＭ座標での並進距離ｄで除することによって、スケールＳを求める（ステップＳ２１９）。

次に、位置推定部１３は、後述する誤差判定スレッドで用いられるカウンタＮＩをクリアし（ステップＳ２２０）、ステップＳ２０１、ステップＳ２０２を経由して、初期化済の場合の処理であるステップＳ２２１へ進む。

初期化済の場合の処理を説明する。この処理が、自機位置推定スレッドの通常時の処理であり、逐次現在の自機の位置及び向き（ＳＬＡＭ座標内での並進ベクトルｔと回転行列Ｒ）を推定する処理である。

位置推定部１３は、撮像部４１で画像を撮影し（ステップＳ２２１）、フレームカウンタＮをインクリメントする（ステップＳ２２２）。そして、撮影した画像に含まれている２Ｄ特徴点を取得する（ステップＳ２２３）。次に、フレームデータベースに登録されている以前のキーフレーム（例えばキーフレーム番号がＮＫＦである画像）の情報から、その画像の情報に含まれている２Ｄ特徴点のうち、３Ｄ位置が既知である（Ｍａｐ点データベースに登録されているＭａｐ点になっている）２Ｄ特徴点を取得し、今撮影した画像との間で対応が取れる２Ｄ特徴点（対応特徴点）を抽出する（ステップＳ２２４）。

そして、位置推定部１３は、対応特徴点の個数が基準対応特徴点数（例えば１０）未満かどうかを判定し（ステップＳ２２５）、基準対応特徴点数未満の場合（ステップＳ２２５；Ｙｅｓ）はＳＬＡＭ法で推定する姿勢の精度が悪くなるので、位置の推定は行わずにステップＳ２２１での画像の取得に戻る。ここで、すぐにステップＳ２２１に戻るのではなく、ステップＳ２２４に戻って、フレームデータベースに登録されているキーフレームの中から対応特徴点の個数が基準対応特徴点数以上のものを検索するようにしても良い。この場合は、フレームデータベースに登録されているキーフレームの中に対応特徴点の個数が基準対応特徴点数以上のものが見つからなかった場合にステップＳ２２１に戻る。

位置推定部１３は、基準対応特徴点数以上の対応特徴点が抽出できたら（ステップＳ２２５；Ｎｏ）、対応特徴点それぞれの３Ｄ位置（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ）をＭａｐ点データベースから取得する（ステップＳ２２６）。今撮影した画像に含まれている対応特徴点のフレーム座標を（ｕ_ｉ，ｖ_ｉ）とし、その対応特徴点の３Ｄ位置を（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ）とする（ｉは１から対応特徴点数までの値を取る）と、各対応特徴点の３Ｄ位置（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ）を以下の式（５）によってフレーム座標系に投影した値（ｕｘ_ｉ，ｖｘ_ｉ）とフレーム座標（ｕ_ｉ，ｖ_ｉ）とは理想的には一致するはずである。
（ｕｘ_ｉ ｖｘ_ｉ １）’〜Ａ（Ｒ｜ｔ）（Ｘ_ｉ Ｙ_ｉ Ｚ_ｉ １）’…（５）

実際には（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ）にも（ｕ_ｉ，ｖ_ｉ）にも誤差が含まれているため、（ｕｘ_ｉ，ｖｘ_ｉ）と（ｕ_ｉ，ｖ_ｉ）とが一致することはめったにない。そして、未知数はＲとｔ（３次元空間ではそれぞれ３次元となり、３＋３＝６が未知数の個数である）だけなのに、数式は対応特徴点の個数の２倍存在する（対応特徴点一つに対して、フレーム座標のｕ，ｖそれぞれに対する式が存在するため）ことになるため、過剰条件の連立一次方程式になり、上述したように最小二乗法で求めることになる。具体的には、位置推定部１３は、以下の式（６）のコスト関数Ｅ１を最小化する姿勢（並進ベクトルｔ及び回転行列Ｒ）を求めることになる。これがＳＬＡＭ法で求めたＳＬＡＭ座標での自機の姿勢（並進ベクトルｔ及び回転行列Ｒで表される自機の位置及び向き）となる。このようにして、位置推定部１３は自機の姿勢を推定する（ステップＳ２２７）。

ＳＬＡＭ座標での現在の自機の姿勢（並進ベクトルｔ及び回転行列Ｒ）が求められたので、位置推定部１３は、これにスケールＳを乗算することで、ＶＯ（ビジュアルオドメトリ）を求める（ステップＳ２２８）。ＶＯは実環境での自機の位置及び向きとして利用できる。

次に、位置推定部１３は、フレームＤＢに登録されている直前のキーフレーム（キーフレーム番号がＮＫＦである画像）を撮影した時の自機の位置から基準並進距離（例えば１ｍ）以上移動しているかを判定し（ステップＳ２２９）、移動しているなら（ステップＳ２２９；Ｙｅｓ）キーフレームカウンタＮＫＦをインクリメントしてから（ステップＳ２３０）、現フレームをキーフレームとしてフレームＤＢに登録する（ステップＳ２３１）。基準並進距離未満しか移動していないなら（ステップＳ２２９；Ｎｏ）ステップＳ２０１に戻る。

ここで、基準並進距離と比較する自機の移動距離は、直前のキーフレームから現フレームまでの並進距離（両フレームの並進ベクトルの差のベクトルの絶対値（要素の二乗和の平方根））をオドメトリから取得しても良いし、上述したＶＯ（ビジュアルオドメトリ）から求めても良い。フレームＤＢに登録する内容は上述したように、「キーフレーム番号」、「姿勢」、「オドメトリで計測した実環境上での姿勢」、「抽出した全ての２Ｄ特徴点」、「全ての２Ｄ特徴点の中でＭａｐ点として３Ｄ位置が既知の点」、「キーフレーム自体の特徴」、である。

そして、位置推定部１３は、新たなキーフレームが発生したことを地図作成スレッドに知らせるために、地図作成スレッドのキーフレームキューにキーフレームカウンタＮＫＦをセットする（ステップＳ２３２）。そして、ステップＳ２０１に戻る。なお、キーフレームカウンタＮＫＦ、カウンタＮＩ、スケールＳ、Ｍａｐ点ＤＢ、フレームＤＢはスレッドをまたいで値を参照することができるように記憶部２０に記憶されている。

次に、自律移動装置１００のメインフロー（図３）のステップＳ１０２で起動される地図作成スレッドについて、図５を用いて説明する。このスレッドは地図作成部１２が、キーフレーム中の対応特徴点の３Ｄ位置を計算して、地図の情報（Ｍａｐ点ＤＢ）を作成している。

まず、地図作成部１２は、動作終了かを判定する（ステップＳ３０１）。動作終了なら（ステップＳ３０１；Ｙｅｓ）終了し、動作終了でないなら（ステップＳ３０１；Ｎｏ）、キーフレームキューが空かどうかを判定する（ステップＳ３０２）。キーフレームキューが空だったら（ステップＳ３０２；Ｙｅｓ）ステップ３０１に戻り、空でなければ（ステップＳ３０２；Ｎｏ）、キーフレームキューからデータを取り出してＭＫＦ（地図作成スレッドで処理するキーフレームのキーフレーム番号を表す変数）にセットする（ステップＳ３０３）。地図作成部１２は、ＭＫＦが０より大きいか判定し（ステップＳ３０４）、ＭＫＦが０である場合は（ステップＳ３０４；Ｎｏ）ステップＳ３０１に戻ってキーフレームキューにデータが入るのを待つ。ＭＫＦが１以上の場合は（ステップＳ３０４；Ｙｅｓ）、以下の処理に進む。

地図作成部１２は、フレームＤＢを参照し、前キーフレーム（キーフレーム番号がＭＫＦ−１のキーフレーム）の２Ｄ特徴点と現キーフレーム（キーフレーム番号がＭＫＦのキーフレーム）の２Ｄ特徴点とで対応が取れる２Ｄ特徴点（対応特徴点）を抽出する（ステップＳ３０５）。フレームＤＢにはそれぞれのキーフレームの姿勢（並進ベクトルｔと回転行列Ｒ）も登録されているので、自機位置推定スレッドの初期化時の処理の時と同様の方法で対応特徴点の３Ｄ位置を計算できる。地図作成部１２は、３Ｄ位置が計算できた対応特徴点をＭａｐ点としてＭａｐ点ＤＢに登録する（ステップＳ３０６）。地図作成部１２は、フレームＤＢに対しても今回３Ｄ位置を計算できた２Ｄ特徴点に対して３Ｄ位置を登録する（ステップＳ３０７）。

なお、地図作成部１２が抽出した対応特徴点がすでにＭａｐ点ＤＢに登録済だった場合は、３Ｄ位置計算をスキップして次の対応特徴点（Ｍａｐ点ＤＢに未登録のもの）に対する処理に進んでも良いし、改めて３Ｄ位置計算を行って、Ｍａｐ点ＤＢに登録済の３Ｄ位置や、フレームＤＢ中の対応特徴点に対する３Ｄ位置を更新するようにしても良い。

次に、地図作成部１２は、キーフレームキューが空かどうかを判定する（ステップＳ３０８）。空であれば（ステップＳ３０８；Ｙｅｓ）、全キーフレームの姿勢と全Ｍａｐ点の３Ｄ位置に対して、バンドルアジャストメント処理を行って、精度向上を図ってから（ステップＳ３０９）ステップＳ３１０に進む。キーフレームキューが空でなければ（ステップＳ３０８；Ｎｏ）ステップ３１０に進む。次に地図作成部１２は、ループクロージングスレッドのキーフレームキューにＭＫＦをセットして（ステップＳ３１０）、ステップＳ３０１に戻る。

なお、バンドルアジャストメント処理とは、カメラ姿勢（キーフレーム姿勢）とＭａｐ点の３Ｄ位置とを同時に推定する非線形最適化法であり、Ｍａｐ点をキーフレーム上に投影させたときに発生する誤差が最小になるような最適化を行うものである。

このバンドルアジャストメントの処理を行うことで、キーフレーム姿勢とＭａｐ点の３Ｄ位置の精度向上を図ることができる。しかし、この処理を行わなくても精度向上が図れないだけで特別問題が発生するわけではない。したがって、他の処理がない状態のとき（例えば、キーフレームキューが空の状態）であっても、この処理を毎回行う必要はない。

また、バンドルアジャストメントの処理を行うと、キーフレーム上に投影させたときの誤差が所定の値よりも大きいＭａｐ点が見つかることがある。このような誤差の大きなＭａｐ点については、ＳＬＡＭ推定に悪影響を及ぼすため、Ｍａｐ点ＤＢ及びフレームＤＢから削除したり、誤差の大きな注意を要するＭａｐ点であることを識別するためのフラグを立てておいたりしても良い。なお、バンドルアジャストメントの処理は本実施形態においてはオプション的な扱いになるため、ここでは処理の詳細については省略する。

次に自律移動装置１００のメインフロー（図３）のステップＳ１０３で起動されるループクロージングスレッドについて、図６を用いて説明する。このスレッドでは制御部１０は、ループクロージング処理ができるかどうかをチェックし続け、できる場合にはループクロージング処理を行っている。なお、ループクロージング処理とは、以前来たことのある同じ場所に戻ってきたことを認識した場合に、以前この同じ場所にいた時の姿勢の値と現在の姿勢の値とのずれを用いて、以前来た時から今までの軌跡中のキーフレームや、関連するＭａｐ点の３Ｄ位置を修正することを言う。

まず、制御部１０は、動作終了かを判定する（ステップＳ４０１）。動作終了なら（ステップＳ４０１；Ｙｅｓ）終了する。動作終了でないなら（ステップＳ４０１；Ｎｏ）キーフレームキューが空かどうかを判定する（ステップＳ４０２）。キーフレームキューが空なら（ステップＳ４０２；Ｙｅｓ）ステップＳ４０１に戻り、キーフレームキューが空でないなら（ステップＳ４０２；Ｎｏ）、キーフレームキューからデータを取り出してＬＫＦ（ループクロージングスレッドで処理するキーフレームのキーフレーム番号を表す変数）にセットする（ステップＳ４０３）。次に制御部１０は、ＬＫＦが１より大きいかを判定する（ステップＳ４０４）。ＬＫＦが０又は１である場合（ステップＳ４０４；Ｎｏ）はステップＳ４０１に戻ってキーフレームキューにデータが入るのを待つ。そして、ＬＫＦが２以上の場合（ステップＳ４０４；Ｙｅｓ）は、以下の処理を行う。

制御部１０は、フレームＤＢを参照し、現キーフレーム（キーフレーム番号がＬＫＦのキーフレーム）と「キーフレーム自体の特徴」の類似度が基準画像類似度以上になるキーフレームをフレームＤＢから検索する（ステップＳ４０５）。ここで、この類似度は、画像（キーフレーム）の特徴を特徴ベクトルで表している場合は、二つの画像の特徴ベクトルの内積の絶対値を１に正規化したものを、その二つの画像の類似度とすることができる。また、二つの画像の特徴ベクトルの距離（各要素の差の二乗和の平方根）の逆数を類似度としても良い。

制御部１０は、「キーフレーム自体の特徴」の類似度が基準画像類似度以上になるキーフレームが発見されたかどうかを判定し（ステップＳ４０６）、発見されなければ（ステップＳ４０６；Ｎｏ）ステップＳ４０８へ行き、発見されたら（ステップＳ４０６；Ｙｅｓ）、発見されたキーフレームから現キーフレームまでの軌跡中のキーフレームの姿勢と、軌跡中のキーフレームに含まれるＭａｐ点の３Ｄ位置を修正する（ステップＳ４０７）。例えば、制御部１０は、現キーフレームの姿勢を発見されたキーフレームの姿勢と同じ姿勢として修正する。そして、発見されたキーフレームの姿勢と現キーフレームの姿勢との差分を用いて、発見されたキーフレームから現キーフレームまでの軌跡中の各キーフレームの姿勢に線形的に補正を加える。さらにこれらの各キーフレームに含まれるＭａｐ点の３Ｄ位置についても各キーフレームの姿勢の補正量に応じて修正する。

次に制御部１０は、誤差判定スレッドのキーフレームキューにＬＫＦをセットしてステップＳ４０１に戻る（ステップＳ４０８）。

次に自律移動装置１００のメインフロー（図３）のステップＳ１０４で起動される誤差判定スレッドについて図７を用いて説明する。このスレッドは、位置推定部１３がＳＬＡＭで推定した位置（ビジュアルオドメトリでの位置）と、位置測定部１１がオドメトリで求めた位置との差を判定部１４が判定し、この差が所定誤差内でなければ、作成した地図の情報に含まれている誤差が大きいということになるので、地図編集部１５が地図を消去する処理を行っている。

このスレッドでは制御部１０がまず動作終了かどうかを判定し（ステップＳ５０１）、動作終了なら（ステップ５０１；Ｙｅｓ）終了し、動作終了でないなら（ステップ５０１；Ｎｏ）、誤差判定スレッドのキーフレームキューが空かを判定する（ステップＳ５０２）。キーフレームキューが空なら（ステップＳ５０２；Ｙｅｓ）ステップＳ５０１に戻って、キーフレームキューにデータが入るのを待つ。

キーフレームキューが空でなければ（ステップＳ５０２；Ｎｏ）、制御部１０はキーフレームキューからデータを取り出してＩＫＦ（誤差判定スレッドで処理するキーフレームのキーフレーム番号を表す変数）にセットする（ステップＳ５０３）。次にＩＫＦが０より大きいかを判定し（ステップＳ５０４）、ＩＫＦが０である場合は（ステップ５０４；Ｎｏ）、ステップＳ５０１に戻ってキーフレームキューにデータが入るのを待つ。ＩＫＦが１以上の場合は（ステップＳ５０４；Ｙｅｓ）、以下の処理を行う。

制御部１０は、フレームＤＢを参照し、前キーフレーム（キーフレーム番号がＩＫＦ−１のキーフレーム）のオドメトリから取得した回転角φ_１と、現キーフレーム（キーフレーム番号がＩＫＦのキーフレーム）のオドメトリから取得した回転角φ_２の差分の絶対値φ_ｄ（＝｜φ_１−φ_２｜、自機の向きの変化量）と、基準回転角φ_ｒ（例えば５°）とを比較する（ステップＳ５０５）。

制御部１０は、φ_ｄがφ_ｒ以上であれば（ステップＳ５０５；Ｎｏ）、カウンタＮＩをクリアして（ステップＳ５０６）、ステップＳ５０１に戻り、次のキーフレームを待つ。そして、φ_ｄがφ_ｒ未満であるなら（ステップ５０５；Ｙｅｓ）、カウンタＮＩをインクリメントし（ステップＳ５０７）、カウンタＮＩが基準蓄積数Ｃ（例えば１０）未満かを判定する（ステップ５０８）。カウンタＮＩが基準蓄積数Ｃ未満であれば（ステップ５０８；Ｙｅｓ）、ステップＳ５０１に戻って引き続き次のキーフレームを待つ。

カウンタＮＩが基準蓄積数Ｃ以上であれば（ステップ５０８；Ｎｏ）、判定部１４は、Ｃ回前のキーフレーム（キーフレーム番号がＩＫＦ−Ｃのキーフレーム）から現キーフレーム（キーフレーム番号がＩＫＦのキーフレーム）までの姿勢変化に基づきＳＬＡＭ座標での並進距離を求め、Ｔ１とする（ステップＳ５０９）。また、判定部１４は、Ｃ回前のキーフレームから現キーフレームまでのオドメトリによる並進距離を求め、Ｔ２とする（ステップＳ５１０）。Ｔ１やＴ２の求め方をより詳しく説明すると以下のようになる。

フレームＤＢに各キーフレームの「姿勢」（その画像撮影時の自機のＳＬＡＭ座標内での位置（並進ベクトルｔ）及び向き（回転行列Ｒ））、「オドメトリから取得した実環境上での姿勢」（実環境の距離で求められる位置及び向き）」が格納されているので、簡単には、Ｃ回前のキーフレームの並進ベクトルｔ_１と現キーフレームの並進ベクトルｔ_２との差の大きさ（｜ｔ_１−ｔ_２｜）をＴ１（ＳＬＡＭ座標の並進ベクトルｔを用いて計算）やＴ２（オドメトリから取得した値を用いて計算）とすれば良い。或いは、Ｃ回前のキーフレームから現キーフレームまでのＣ＋１個のキーフレームについて、隣り合うキーフレームの並進ベクトルの差の大きさをＣ個加算したもの（Σ｜ｔ_ｉ−ｔ_ｉ＋１｜）をＴ１やＴ２としてもよい。

そして、判定部１４は、カウンタＮＩをクリアする（ステップＳ５１１）。また、Ｔ１はＳＬＡＭ座標での値なので、実環境の距離に変換するためにスケールＳを乗じたものをＴ３とする（ステップＳ５１２）。そして、判定部１４は、Ｔ２とＴ３の違いが所定誤差範囲内（フローチャートの例では１．５倍以内）に入っているかを判定する（ステップＳ５１３及びステップＳ５１４）。所定誤差範囲内（Ｔ２が、Ｔ３×１．５未満（ステップＳ５１３；Ｙｅｓ）かつＴ３÷１．５より大きい（ステップＳ５１４；Ｙｅｓ）場合）であれば、地図の情報（Ｍａｐ点ＤＢの情報や、フレームＤＢの情報）はそれほど不正確ではないので、ステップＳ５０１に戻って引き続き次のキーフレームを待つ。

Ｔ２とＴ３の違いが所定誤差範囲を超える場合（Ｔ２が、Ｔ３×１．５以上（ステップＳ５１３；Ｎｏ）又はＴ３÷１．５以下（ステップＳ５１４；Ｎｏ）の場合）は、作成した地図の情報（Ｍａｐ点ＤＢの情報や、フレームＤＢの情報）は不正確であるので、地図編集部１５は初期化済フラグ、Ｍａｐ点ＤＢ、フレームＤＢ、各スレッドのキーフレームキューを全てクリアする（ステップＳ５１５）。そして制御部１０は、ステップＳ５０１に戻って最初からやり直す。初期化済フラグやキーフレームキューがクリアされるので、全てのスレッドが最初からやり直されることになる。このようにすることにより、作成した地図に誤差が含まれた状態で自律移動装置１００が動き続けてしまうことを防ぐことができる。

なお、Ｔ２とＴ３の違いが所定範囲を超える場合は、その時点の地図の情報が不正確であることがわかるが、それ以前の誤差判定スレッドの処理で、Ｔ２とＴ３の違いが所定範囲内であった時点では、地図の情報はそれほど不正確ではなかった可能性もある。そこで、ステップＳ５１４がＹｅｓだった場合は、ステップＳ５０１に戻る前に、地図編集部１５がその時点の地図の情報（Ｍａｐ点ＤＢ）を「確からしい地図」として保存するようにし、ステップＳ５１５では地図編集部１５は初期化済フラグ、フレームＤＢ、各スレッドのキーフレームキューをクリアすることなく、Ｍａｐ点ＤＢを保存してある以前のＭａｐ点ＤＢの状態に戻すようにしても良い。このようにすることによって、過去に作成した地図の情報を有効活用できる。

Ｍａｐ点ＤＢを以前の状態に戻す方法としては、Ｍａｐ点ＤＢ全体の情報を「確からしい地図」として保存しておく方法以外に、Ｍａｐ点ＤＢの各要素にタイムスタンプ（時刻情報を使っても良いが、キーフレームカウンタＮＫＦの値を使うのが合理的である）の情報も一緒に格納しておく方法もある。この方法では、ステップＳ５１４がＹｅｓだった場合は、ステップＳ５０１に戻る前に、地図編集部１５はその時点のタイムスタンプ（時刻又はＮＫＦ）をＴＳにセットしておき（このＴＳ自体もスタック状にして、次々に過去に戻れるようにしておいても良い）、ステップＳ５１５では地図編集部１５は、Ｍａｐ点ＤＢ内の情報のうち、タイムスタンプがＴＳより後になっているものを消去し、ＴＳ以前の情報はそのまま残す処理を行う。このような処理を行えば、過去の確からしいＭａｐ点の情報は保存したまま、最近の（誤差が大きいと考えられる）Ｍａｐ点の情報のみを消去できる。

この考え方はフレームＤＢにも適用でき、地図編集部１５は、ステップＳ５１５で、フレームＤＢ内の情報のうち、キーフレーム番号がＴＳよりも大きいものを消去し、ＴＳ以前の情報はそのまま残すようにしても良い。このようにすることで、過去の確からしいキーフレームの情報は保存したまま、最近の（誤差が大きいと考えられる）キーフレームの情報のみを消去できる。

なお、上記実施形態では、ステップＳ５０５での比較対象となる回転角φ_ｄはオドメトリから取得した回転角を用いて算出していたが、ビジュアルオドメトリから取得した回転角（フレームＤＢに格納されている各キーフレームの「姿勢（ＳＬＡＭ座標内での並進ベクトルｔと回転行列Ｒとで表される位置及び向き）」のＲから得られる回転角）を用いて算出しても良い。具体的には、前キーフレームの姿勢の回転行列をＲ１、現フレームの姿勢の回転行列をＲ２とすると、Ｒ３＝Ｒ２×Ｒ１^−１を計算することで、前キーフレームから現キーフレームの間の回転を表す回転行列Ｒ３が得られる（Ｒ１^−１はＲ１の逆行列を表す（回転行列には必ず逆行列が存在する））ので、ここで得られた回転行列Ｒ３の各要素に基づいて、各軸の回転角を求めることができる。回転行列が３×３の場合は、回転行列から各軸の回転角は一意に求まらない場合があるが、実際には自律移動装置１００が移動するのは地面等の平面上であることが多いため、２×２の回転行列に近似することで、回転角を一意に定めることができる。このようにビジュアルオドメトリを用いることによって、駆動部４２の経年劣化やスリップ等の影響を受けずに回転角φ_ｄを取得することができる。

駆動部４２の経年劣化やスリップ等の影響を考慮し、制御部１０は、経年劣化が少なくかつスリップ等の影響がない路面を移動している場合にはオドメトリから取得した回転角を用い、そうでない場合はビジュアルオドメトリから取得した回転角を用いるようにしても良い。

さらに、自律移動装置１００が方位センサも備えている場合には、フレームＤＢに各キーフレーム取得時の方位センサの値も格納するようにしておき、ステップＳ５０５では、制御部１０は、方位センサで得た方位に基づく回転角φ_ｄが基準回転角φ_ｒ未満かを判定するようにしても良い。

また、図７のフローチャートでは、回転角φ_ｄを、前キーフレームからの回転角としたが、カウンタＮＩ＝０の時点でのキーフレームからの回転角が基準回転角φ_ｒ未満であるかどうかをステップＳ５０５で判定するようにしても良い。これにより、少しずつ同じ方向に回転していくように移動した場合を誤差判定スレッドでの判定対象から外すことができる。

また、図７のフローチャートでは、カウンタＮＩの閾値である基準蓄積数Ｃを１０、Ｔ２とＴ３の違いを判定する際の所定誤差範囲を１．５倍としているが、Ｃの値は１０でなくても良いし、所定誤差範囲は１．５倍でなくても良い。例えば、Ｃを５として所定誤差範囲を１．３倍としたり、Ｃを２０として所定誤差範囲を２倍としたりしても良い。所定誤差範囲を１より大きな値の範囲で１に近づければ近づけるほど、判定条件が厳しくなり、地図の情報（Ｍａｐ点ＤＢ等の情報）に少しの誤差が含まれただけでも地図編集部１５は地図の情報をクリアするようになる。

また、図７のステップＳ５０７〜ステップＳ５０８等において、カウンタＮＩの値を用いるのではなく、オドメトリから取得した並進距離を閾値として判定しても良い。例えば「オドメトリの並進距離が基準並進距離以上になるまで」としても良い。これは、自機位置推定スレッドのステップＳ２２０並びに誤差判定スレッドのステップＳ５０６及びステップＳ５１１において「ＮＩ＝０」とする代わりに「オドメトリから取得した並進ｔをｔＩ１にセット」とし、ステップＳ５０７では「オドメトリから取得した並進ｔをｔＩ２にセット」とし、ステップＳ５０８では｜ｔＩ１−ｔＩ２｜（位置の変化量）が基準並進距離未満かの判定をするようにすれば良い。

またさらに、カウンタＮＩを使わないそれ以外の方法として、キーフレーム間にて、オドメトリから取得した回転角がずっと０の状態で（つまり全く回転せずに直進して）並進距離Ｄを検出したときに、オドメトリ並進距離Ｔ２をＤとして判定しても良い。これは、自機位置推定スレッドのステップＳ２２０並びに誤差判定スレッドのステップＳ５０６及びステップＳ５１１において「ＮＩ＝０」とする代わりに「オドメトリから取得した回転ＲをＲＩにセットし、オドメトリから取得した並進ｔをｔＩ１にセット」とし、ステップＳ５０５では「オドメトリから取得した回転ＲをＲＩと比較し、ＲＩと同じままなら（回転していないので）ステップＳ５０７へ進み、ＲＩと異なる値になっていたら（回転したことになるので）ステップＳ５０６に進む」とし、ステップＳ５０７では「オドメトリから取得した並進ｔをｔＩ２にセット」とし、ステップＳ５０８では「｜ｔＩ１−ｔＩ２｜＜Ｄ」での判定をするようにすれば良い。このときはステップＳ５１０は「Ｔ２にＤをセット」となる。

さらに、搭載されている加速度センサ３１により、自律移動装置１００の自律移動とは明らかに異なる加速度が検出された場合は、例えば人が移動体を持ち上げて移動させた等が考えられるため、この場合は制御部１０がカウンタＮＩを０にクリアする。具体的には図７のステップＳ５０５の直前（又は直後）に、「加速度センサ３１の値とオドメトリから算出される加速度とを比較して、所定範囲を超える差が発生したらステップＳ５０６へ進む」処理を加えれば良い。このようにすることによって、誤差判定スレッドでの判定動作がリセットされるため、不用意に地図の情報（Ｍａｐ点ＤＢ等）がクリアされてしまうことを防ぐことができる。なお、加速度は３次元のベクトル値として得られるので、加速度の差も３次元のベクトル値となるが、上記の所定範囲を超える差が発生したかどうかの判定においては、差のベクトルの絶対値（要素の二乗和の平方根）が所定値を超えるかどうかで判定すれば良い。

また、車輪が大きくスリップした場合、極端な場合には空転した場合等には、車輪を駆動するモータの負荷が急に軽くなるため、モータに流れる電流が変化する。したがって、オドメトリの測定結果とモータ電流の関係を確認することで、大きなスリップや空転が発生したことを検出できる。大きなスリップや空転を検出したら、制御部１０がカウンタＮＩを０にクリアすることで、誤差判定スレッドでの判定動作がリセットされ、判定対象から除くことができる。これも具体的には図７のステップＳ５０５の直前（又は直後。前述した加速度センサの判定とこのスリップ判定のどちらを先にやっても良い）に、「モータ電流の実際の値とオドメトリから推定されるモータ電流とを比較して、所定範囲を超える差が発生したらステップＳ５０６へ進む」処理を加えれば良い。このようにすることによって、大きなスリップや空転等が発生していない状況のみを利用して判定することができ、不用意に地図の情報（Ｍａｐ点ＤＢ等）がクリアされてしまうことを防ぐことができる。

また、駆動部４２は経年劣化して、オドメトリの精度が保てなくなるため、制御部１０は累積移動距離の増大に応じて、回転角φ_ｄの閾値φ_ｒを大きくしても良い。これにより、オドメトリの精度が落ちる回転動作を伴う並進移動の際も判定を行うことになり、オドメトリの精度が悪い時の地図の情報（Ｍａｐ点ＤＢ等）をクリアすることができる。

さらに、利用環境によって閾値φ_ｒを変えても良い。例えば、制御部１０が反射センサ（図示せず）を用いて走行面を測定し、鏡面反射があればスリップしやすい走行面だと想定して閾値φ_ｒを大きくし、拡散反射があればスリップしにくい走行面だと想定して閾値φ_ｒを小さくしても良い。これにより、走行面の状態に応じて、スリップしやすい走行面なら回転動作が比較的多い場合も判定を行わせ、スリップしにくい走行面なら回転動作がほとんどない場合にのみ判定を行わせるということが可能になる。

また、定期的に制御部１０が自律移動装置１００を直進移動させ、オドメトリとビジュアルオドメトリの差の判定を判定部１４に強制的に行わせても良い。これにより、地図の情報（Ｍａｐ点ＤＢ等）に誤差を含んだまま自律移動をし続けるのを防ぐことができる。

上記実施形態では、撮像部４１が単眼である単眼ＳＬＡＭの例を示したが、複数の撮像部を用いた複眼ＳＬＡＭでも同様の構成を採ることができる。例えば自律移動装置１００が二台の撮像部４１を備えている場合は、移動しなくても同じ位置から二つの画像を取得できるため、自機位置推定スレッドの初期化処理においてはステップＳ２０４の一回の動作で二つの画像が取得できる。それら二つの画像それぞれに基準値以上の特徴点が含まれていれば（ステップＳ２０６で判定する）、ステップＳ２０７では「Ｎ＝Ｎ＋２」を行い、ステップＳ２０８は常に「Ｎｏ」に進むことになる。そして、二つの画像の一方を前フレーム、もう一方を現フレームとして処理を行う場合は、ステップＳ２１９でスケールＳを計算する際の「オドメトリ並進距離」は、二つの撮像部間の距離を用いれば良い。これによりオドメトリからの取得する位置の精度が低い場合でも安定して初期化を行うことができる。

ただし、この場合の二つの撮像部間の距離は、通常用いられる基準並進距離（例えば１ｍ）よりも短い距離になってしまうことが多く、オドメトリから得られる並進距離の精度が高い場合は、むしろ単眼ＳＬＡＭと同様の初期化処理（例えば複数ある撮像部のうちの第一撮像部のみを使った初期化処理）を行った方が高い精度が得られることもある。そこで、単眼ＳＬＡＭと同様の初期化処理と、前段落で記載した二つの撮像部による初期化処理とを両方とも行って、それぞれで地図の情報（Ｍａｐ点ＤＢやフレームＤＢ）を作成し始め、途中で誤差判定スレッドと同様の処理を行ってそれぞれの誤差を判定し、その後は誤差の小さい方の地図の情報を採用するようにしても良い。

なお、上述の各実施形態では、基準並進距離は「例えば１ｍ」としたが、自律移動装置自体の大きさや速度、自律移動する環境や移動範囲等に応じて、最適な基準並進距離は変わるので、基準並進距離の設定方法について補足しておく。

基準並進距離の設定方法として、キーフレーム上の全てのＭａｐ点（３Ｄ位置が既知の２Ｄ特徴点）の３Ｄ位置から自機位置までの距離を平均した値（＝キーフレームで観察される全てのＭａｐ点までの奥行きの平均距離）と並進距離との比で設定しても良い（例えば、Ｍａｐ点までの奥行きの平均距離の５％の距離を基準並進距離とする等）。また、実環境上での並進距離の大きさに応じて、例えばテーブルの上を動くなら１０ｃｍ、部屋を動き回るなら１ｍ、屋外の広場等を動き回るなら１０ｍ等を基準並進距離に設定しても良いし、駆動部４２の車輪の直径に応じた値（例えば車輪の直径の１０倍）を基準並進距離に設定しても良い。

基準並進距離として実環境上の距離で設定する場合は、ＳＬＡＭ座標での並進距離にはスケールＳを乗算して実環境上の距離に変換してから比較を行う（この逆も同様であり、基準並進距離としてＳＬＡＭ座標での距離を設定する場合は、実環境上の距離はスケールＳで除算してＳＬＡＭ座標での距離に変換してから比較を行う）。

この発明の自律移動装置１００の各機能は、通常のＰＣ（Ｐｅｒｓｏｎａｌ Ｃｏｍｐｕｔｅｒ）等のコンピュータによっても実施することができる。具体的には、上記実施形態では、自律移動装置１００が行う自律移動制御処理のプログラムが、記憶部２０のＲＯＭに予め記憶されているものとして説明した。しかし、プログラムを、フレキシブルディスク、ＣＤ−ＲＯＭ（Ｃｏｍｐａｃｔ Ｄｉｓｃ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）及びＭＯ（Ｍａｇｎｅｔｏ−Ｏｐｔｉｃａｌ Ｄｉｓｃ）等のコンピュータ読み取り可能な記録媒体に格納して配布し、そのプログラムをコンピュータに読み込んでインストールすることにより、上述の各機能を実現することができるコンピュータを構成してもよい。

以上、本発明の好ましい実施形態について説明したが、本発明は係る特定の実施形態に限定されるものではなく、本発明には、特許請求の範囲に記載された発明とその均等の範囲が含まれる。以下に、本願出願の当初の特許請求の範囲に記載された発明を付記する。

（付記１）
移動手段による移動量に基づいて自機位置を計測する位置計測部と、
撮像部が撮影した複数の画像の情報を用いて作成した地図を記憶する地図記憶部と、
前記撮像部が撮影した複数の画像の情報と前記地図記憶部に記憶されている地図の情報とに基づいて自機位置を推定する位置推定部と、
前記位置計測部が計測した自機位置と前記位置推定部が推定した自機位置との差が所定誤差内か否かを判定する判定部と、
前記判定部が前記所定誤差内でないと判定した場合、前記地図記憶部に記憶されている地図を編集する地図編集部と、
を備える自律移動装置。

（付記２）
前記自律移動装置は、さらに、自機の向きの変化量を取得する向き変化量取得部を備え、
前記判定部は、前記向き変化量取得部が取得した向きの変化量が基準向き変化量よりも小さいときに判定を行う、
付記１に記載の自律移動装置。

（付記３）
前記自律移動装置は、さらに、自機の位置の変化量を取得する位置変化量取得部を備え、
前記判定部は、前記位置変化量取得部が取得した位置の変化量が基準位置変化量よりも大きくかつ前記向き変化量取得部が取得した向きの変化量が基準向き変化量よりも小さい時に判定を行う、
付記２に記載の自律移動装置。

（付記４）
前記地図編集部は、前記判定部が前記所定誤差内でないと判定した場合、前記地図記憶部に記憶されている地図を消去する、
付記１から３のいずれか一つに記載の自律移動装置。

（付記５）
前記自律移動装置は、さらに、前記判定部が前記所定誤差内であると判定した場合に前記地図記憶部に記憶されている地図を保存する地図保存部を備え、
前記地図編集部は、前記判定部が前記所定誤差内でないと判定した場合、前記地図保存部に保存されている地図を前記地図記憶部に記憶させる、
付記１から３のいずれか一つに記載の自律移動装置。

（付記６）
前記自律移動装置は、さらに、前記判定部が前記所定誤差内であると判定した時刻の情報を記憶する時刻記憶部を備え、
前記地図記憶部は、地図を作成した時刻の情報と共に地図の情報を記憶し、
前記地図編集部は、前記判定部が前記所定誤差内でないと判定した場合、前記地図記憶部に記憶されている地図の情報のうち、前記時刻記憶部に記憶されている時刻よりも後に作成された地図の情報を消去する、
付記１から３のいずれか一つに記載の自律移動装置。

（付記７）
前記自律移動装置は、さらに、撮影した画像の情報を、撮影した時刻の情報と共に記憶する画像記憶部を備え、
前記地図記憶部には、前記画像記憶部に記憶されている複数の画像の情報を用いて作成した地図が記憶され、
前記地図編集部は、前記判定部が前記所定誤差内でないと判定した場合、前記画像記憶部に記憶されている情報のうち、前記時刻記憶部に記憶されている時刻よりも後に撮影された情報を消去する、
付記６に記載の自律移動装置。

（付記８）
前記撮像部は、単眼の撮像部であり、
前記位置推定部は前記撮像部が撮影した複数の画像の情報に基づいて自機位置を推定する、
付記１から７のいずれか一つに記載の自律移動装置。

（付記９）
前記自律移動装置は、さらに、加速度センサによって加速度を取得する加速度取得部と、
移動手段による移動量に基づいて自機の加速度を算出する加速度算出部と、
を備え、
前記地図編集部は、前記判定部が前記所定誤差内でないと判定した場合であっても、前記加速度取得部が取得した加速度と前記加速度算出部が算出した加速度との差が所定の値を超える場合は、前記地図記憶部に記憶されている地図を編集しない、
付記１から８のいずれか一つに記載の自律移動装置。

（付記１０）
前記自律移動装置は、さらに、移動手段に備えられたモータに流れる電流の値を取得する電流取得部と、
前記移動手段による移動量に基づいてモータに流れる電流の値を推定する電流推定部と、
を備え、
前記地図編集部は、前記判定部が前記所定誤差内でないと判定した場合であっても、前記電流取得部が取得した電流の値と前記電流推定部が推定した電流の値との差が所定の値を超える場合は、前記地図記憶部に記憶されている地図を編集しない、
付記１から９のいずれか一つに記載の自律移動装置。

（付記１１）
移動手段による移動量に基づいて自機位置を計測する位置計測ステップと、
撮像部が撮影した複数の画像の情報を用いて地図を作成する地図作成ステップと、
前記撮像部が撮影した複数の画像の情報と前記作成した地図の情報とに基づいて自機位置を推定する位置推定ステップと、
前記位置計測ステップで計測された自機位置と前記位置推定ステップで推定された自機位置との差が所定誤差内か否かを判定する判定ステップと、
前記判定ステップで前記所定誤差内でないと判定された場合、前記作成した地図を編集する地図編集ステップと、
を備える自律移動方法。

（付記１２）
撮像部が撮影した複数の画像の情報を用いて地図を作成し、
前記撮像部が撮影した複数の画像の情報と前記作成した地図の情報とに基づいて自機位置を推定する自律移動方法であって、
移動手段による移動量に基づいて計測された自機位置と前記推定された自機位置との差が所定誤差内でないと判定された場合、前記作成した地図を編集する自律移動方法。

（付記１３）
コンピュータに、
移動手段による移動量に基づいて自機位置を計測する位置計測ステップ、
撮像部が撮影した複数の画像の情報を用いて地図を作成する地図作成ステップ、
前記撮像部が撮影した複数の画像の情報と前記作成した地図の情報とに基づいて自機位置を推定する位置推定ステップ、
前記位置計測ステップで計測された自機位置と前記位置推定ステップで推定された自機位置との差が所定誤差内か否かを判定する判定ステップ、
前記判定ステップで前記所定誤差内でないと判定された場合、前記作成した地図を編集する地図編集ステップ、
を実行させるためのプログラム。

１００…自律移動装置、１０…制御部、１１…位置計測部、１２…地図作成部、１３…位置推定部、１４…判定部、１５…地図編集部、２０…記憶部、２１…画像記憶部、２２…地図記憶部、３０…センサ部、３１…加速度センサ、３２…角速度センサ、３３…障害物センサ、４１…撮像部、４２…駆動部、４３…入力部、４４…通信部、４５…電源

Claims (13)

1. 移動手段による移動量に基づいて自機位置を計測する位置計測部と、
   撮像部が撮影した複数の画像の情報を用いて作成した地図を記憶する地図記憶部と、
   前記撮像部が撮影した複数の画像の情報と前記地図記憶部に記憶されている地図の情報とに基づいて自機位置を推定する位置推定部と、
   前記位置計測部が計測した自機位置と前記位置推定部が推定した自機位置との差が所定誤差内か否かを判定する判定部と、
   前記判定部が前記所定誤差内でないと判定した場合、前記地図記憶部に記憶されている地図を編集する地図編集部と、
   自機の向きの変化量を取得する向き変化量取得部と、
   を備え、
   前記判定部は、前記向き変化量取得部が取得した向きの変化量が基準向き変化量よりも小さいときに判定を行う、
   自律移動装置。
2. 移動手段による移動量に基づいて自機位置を計測する位置計測部と、
   撮像部が撮影した複数の画像の情報を用いて作成した地図を記憶する地図記憶部と、
   前記撮像部が撮影した複数の画像の情報と前記地図記憶部に記憶されている地図の情報とに基づいて自機位置を推定する位置推定部と、
   前記位置計測部が計測した自機位置と前記位置推定部が推定した自機位置との差が所定誤差内か否かを判定する判定部と、
   前記判定部が前記所定誤差内でないと判定した場合、前記地図記憶部に記憶されている地図を編集する地図編集部と、
   加速度センサによって加速度を取得する加速度取得部と、
   前記移動手段による移動量に基づいて自機の加速度を算出する加速度算出部と、
   を備え、
   前記地図編集部は、前記判定部が前記所定誤差内でないと判定した場合であっても、前記加速度取得部が取得した加速度と前記加速度算出部が算出した加速度との差が所定の値を超える場合は、前記地図記憶部に記憶されている地図を編集しない、
   自律移動装置。
3. 移動手段による移動量に基づいて自機位置を計測する位置計測部と、
   撮像部が撮影した複数の画像の情報を用いて作成した地図を記憶する地図記憶部と、
   前記撮像部が撮影した複数の画像の情報と前記地図記憶部に記憶されている地図の情報とに基づいて自機位置を推定する位置推定部と、
   前記位置計測部が計測した自機位置と前記位置推定部が推定した自機位置との差が所定誤差内か否かを判定する判定部と、
   前記判定部が前記所定誤差内でないと判定した場合、前記地図記憶部に記憶されている地図を編集する地図編集部と、
   前記移動手段に備えられたモータに流れる電流の値を取得する電流取得部と、
   前記移動手段による移動量に基づいてモータに流れる電流の値を推定する電流推定部と、
   を備え、
   前記地図編集部は、前記判定部が前記所定誤差内でないと判定した場合であっても、前記電流取得部が取得した電流の値と前記電流推定部が推定した電流の値との差が所定の値を超える場合は、前記地図記憶部に記憶されている地図を編集しない、
   自律移動装置。
4. 前記自律移動装置は、さらに、自機の位置の変化量を取得する位置変化量取得部を備え、
   前記判定部は、前記位置変化量取得部が取得した位置の変化量が基準位置変化量よりも大きくかつ前記向き変化量取得部が取得した向きの変化量が基準向き変化量よりも小さい時に判定を行う、
   請求項１から３のいずれか一項に記載の自律移動装置。
5. 前記地図編集部は、前記判定部が前記所定誤差内でないと判定した場合、前記地図記憶部に記憶されている地図を、前記地図の編集として消去する処理を行う、
   請求項１から４のいずれか一項に記載の自律移動装置。
6. 前記自律移動装置は、さらに、前記判定部が前記所定誤差内であると判定した場合に前記地図記憶部に記憶されている地図を保存する地図保存部を備え、
   前記地図編集部は、前記判定部が前記所定誤差内でないと判定した場合、前記地図保存部に保存されている地図を前記地図記憶部に記憶させる、
   請求項１から４のいずれか一項に記載の自律移動装置。
7. 前記自律移動装置は、さらに、前記判定部が前記所定誤差内であると判定した時刻の情報を記憶する時刻記憶部を備え、
   前記地図記憶部は、地図を作成した時刻の情報と共に地図の情報を記憶し、
   前記地図編集部は、前記判定部が前記所定誤差内でないと判定した場合、前記地図記憶部に記憶されている地図の情報のうち、前記時刻記憶部に記憶されている時刻よりも後に作成された地図の情報を、前記地図の編集として消去する処理を行う、
   請求項１から４のいずれか一項に記載の自律移動装置。
8. 前記自律移動装置は、さらに、撮影した画像の情報を、撮影した時刻の情報と共に記憶する画像記憶部を備え、
   前記地図記憶部には、前記画像記憶部に記憶されている複数の画像の情報を用いて作成した地図が記憶され、
   前記地図編集部は、前記判定部が前記所定誤差内でないと判定した場合、前記画像記憶部に記憶されている情報のうち、前記時刻記憶部に記憶されている時刻よりも後に撮影された情報を、前記地図の編集として消去する処理を行う、
   請求項７に記載の自律移動装置。
9. 前記撮像部は、単眼の撮像部であり、
   前記位置推定部は前記撮像部が撮影した複数の画像の情報に基づいて自機位置を推定する、
   請求項１から８のいずれか一項に記載の自律移動装置。
10. 移動手段による移動量に基づいて自機位置を計測する位置計測ステップと、
    撮像部が撮影した複数の画像の情報を用いて地図を作成する地図作成ステップと、
    前記撮像部が撮影した複数の画像の情報と前記作成した地図の情報とに基づいて自機位置を推定する位置推定ステップと、
    前記位置計測ステップで計測された自機位置と前記位置推定ステップで推定された自機位置との差が所定誤差内か否かを判定する判定ステップと、
    前記判定ステップで前記所定誤差内でないと判定された場合、前記作成した地図を編集する地図編集ステップと、
    自機の向きの変化量を取得する向き変化量取得ステップと、
    を備え、
    前記判定ステップでは、前記向き変化量取得ステップで取得された向きの変化量が基準向き変化量よりも小さいときに判定を行う、
    自律移動方法。
11. 移動手段による移動量に基づいて自機位置を計測する位置計測ステップと、
    撮像部が撮影した複数の画像の情報を用いて地図を作成する地図作成ステップと、
    前記撮像部が撮影した複数の画像の情報と前記作成した地図の情報とに基づいて自機位置を推定する位置推定ステップと、
    前記位置計測ステップで計測された自機位置と前記位置推定ステップで推定された自機位置との差が所定誤差内か否かを判定する判定ステップと、
    前記判定ステップで前記所定誤差内でないと判定された場合、前記作成した地図を編集する地図編集ステップと、
    加速度センサによって加速度を取得する加速度取得ステップと、
    前記移動手段による移動量に基づいて自機の加速度を算出する加速度算出ステップと、
    を備え、
    前記地図編集ステップでは、前記判定ステップで前記所定誤差内でないと判定された場合であっても、前記加速度取得ステップで取得された加速度と前記加速度算出ステップで算出された加速度との差が所定の値を超える場合は、前記地図作成ステップで作成された地図を編集しない、
    自律移動方法。
12. 移動手段による移動量に基づいて自機位置を計測する位置計測ステップと、
    撮像部が撮影した複数の画像の情報を用いて地図を作成する地図作成ステップと、
    前記撮像部が撮影した複数の画像の情報と前記作成した地図の情報とに基づいて自機位置を推定する位置推定ステップと、
    前記位置計測ステップで計測された自機位置と前記位置推定ステップで推定された自機位置との差が所定誤差内か否かを判定する判定ステップと、
    前記判定ステップで前記所定誤差内でないと判定された場合、前記作成した地図を編集する地図編集ステップと、
    前記移動手段に備えられたモータに流れる電流の値を取得する電流取得ステップと、
    前記移動手段による移動量に基づいてモータに流れる電流の値を推定する電流推定ステップと、
    を備え、
    前記地図編集ステップでは、前記判定ステップで前記所定誤差内でないと判定された場合であっても、前記電流取得ステップで取得された電流の値と前記電流推定ステップで推定された電流の値との差が所定の値を超える場合は、前記地図作成ステップで作成された地図を編集しない、
    自律移動方法。
13. コンビュータを請求項１から９のいずれか一項に記載の自律移動装置として機能させるための、あるいはコンピュータに請求項１０から１２のいずれか一項に記載の自律移動方法を実行させるためのプログラム。

Images