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

Description

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

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

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

この方法は、自機位置と特徴点の３次元位置とを交互に推定していくため、累積誤差が発生し、ループ状の移動等を行って元の位置に戻ってきた場合でも、元の位置に戻ってきたことを認識できない場合がある。このような累積誤差を修正する方法として、ループクロージング処理（ループ解決処理）という技術があり、特許文献１等で用いられている。自機がループを検出すると（撮影したフレーム中に、過去同じ位置から撮影した同一の特徴点が含まれていることが検出されると）、過去のその位置を正しい位置とみなして、そこから現在位置に至るまでの経路中の位置を修正するループクロージング処理が行われる。

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

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

従来技術では、過去に通った位置に戻ってきた場合でも、カメラでの撮影方向が大きく異なっていると、同一の特徴点を検出することができず、ループクロージング処理が行われない。このため、累積誤差が修正されず、同一点に戻ってきていても同一点に戻ってきたことを認識できないという問題があった。

本発明は、上記問題を解決するためになされたものであり、ループクロージング処理を行う頻度を増加させることで、自機位置等の累積誤差を減らすことができる自律移動装置等を提供することを目的する。

上記目的を達成するため、本発明の自律移動装置は、
回転動作を行いながら画像を撮影する回転撮影部と、
撮影した画像の情報を、撮影時の自機の地図座標内での位置を示す並進ベクトルｔと回転行列Ｒの値と共に記憶する画像記憶部と、
前記画像記憶部に記憶されている複数の画像の情報を用いて作成した地図を記憶する地図記憶部と、
撮影した画像との類似度が所定の類似度以上になる画像を前記画像記憶部から検索する類似画像検索部と、
前記類似画像検索部が、前記所定の類似度以上になる画像を発見した場合、以前来たことのある同じ場所に戻ってきたことを認識したと判断し、前記発見した画像に対応して前記画像記憶部に記憶されている自機の地図座標内での位置を示す並進ベクトルｔと回転行列Ｒの値と、現在の位置の並進ベクトルｔと回転行列Ｒの値と、を用いて前記地図記憶部に記憶されている地図を補正する地図補正部と、
を備えることを特徴とする。

本発明によれば、ループクロージング処理を行う頻度が増加し、自機位置等の累積誤差を減らすことができる。

第一の実施形態に係る自律移動装置の外観を示す図である。 第一の実施形態に係る自律移動装置の構成を示す図である。 第一の実施形態に係る自律移動制御処理全体のフローチャートを示す図である。 第一の実施形態に係る自律移動制御処理の中の自機位置推定スレッドの処理のフローチャートを示す図である。 第一の実施形態に係る自律移動制御処理の中の地図作成スレッドの処理のフローチャートを示す図である。 第一の実施形態に係る自律移動制御処理の中のループクロージングスレッドの処理のフローチャートを示す図である。 第一の実施形態に係る自律移動制御処理の中の回転撮影処理のフローチャートを示す図である。 第二の実施形態に係る自律移動装置の構成を示す図である。 第二の実施形態に係る自律移動制御処理の中の自機位置推定スレッドの処理のフローチャートを示す図である。 コビジビリティーグラフを説明する図である。 第四の実施形態に係る自律移動制御処理の中の自機位置推定スレッドの処理のフローチャートを示す図である。 第四の実施形態に係る自律移動制御処理の中のループクロージングスレッドの処理のフローチャートを示す図である。

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

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

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

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

オドメトリから得られる自機の位置及び向きの精度は、車輪の摩耗やスリップ等により、低い精度になってしまうことが多い。特に誤差が累積していく関係上、時間経過と共に精度が悪化する。

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

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

制御部１０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）等で構成され、記憶部２０に記憶されたプログラムを実行することにより、後述する各部（回転撮影部１１、地図作成部１２、位置推定部１３、類似画像検索部１４、地図補正部１５）の機能を実現する。

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

画像記憶部２１には、撮像部４１が撮影した画像が記憶される。ただし、記憶容量の節約のために、撮影したすべての画像を記憶しなくてもよく、また画像自体ではなく、画像の特徴量を記憶するようにしても良い。重要な画像（後述するキーフレーム）については、画像の情報と共に、その画像を撮影した時の自機位置（自機の位置及び向き）の情報が記憶される。

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

センサ部３０として、障害物センサ３１を備える。障害物センサ３１は、走行中の障害物を検知するセンサであって、例えば、赤外線センサ、超音波センサである。なお、独立した障害物センサ３１を搭載せずに、撮像部４１を用いて障害物を検知するようにしても良い。また、他の物体に衝突したことを検知するバンパーセンサー（図示せず）を備えても良い。

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

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

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

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

回転撮影部１１は、所定の条件に基づき、駆動部４２で自機を回転させつつ、撮像部４１で画像を撮影し、撮影した画像は後述する位置推定部１３が推定した自機の位置及び向きの情報と共に画像記憶部２１に記憶させる。自機の位置及び向きについては、位置推定部１３が推定した値ではなく、上述したオドメトリから取得した値を用いても良い。

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

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

類似画像検索部１４は、撮像部４１が撮影した画像の特徴量と、画像記憶部２１に記憶されている画像の特徴量とを比較し、類似度が所定の類似度（基準類似度）以上のものを検索する。

地図補正部１５は、類似画像検索部１４によって類似度が基準類似度以上の画像が発見された時に、その画像が撮影された位置及び向きの情報に基づいて、地図記憶部２２に記憶されている地図の情報を補正する。

図３は、自律移動装置１００のメインフローである。制御部１０は、最初に初期化済フラグ（自機位置推定のための初期化処理が完了しているかどうかを示す変数）及び回転撮影フラグ（回転撮影処理中か否かを示す変数。以下「ＲＦ」ともいう。）をクリアしてから自機位置推定スレッドを起動し（ステップＳ１０１）、次に地図作成スレッド（ステップＳ１０２）、ループクロージングスレッド（ステップＳ１０３）の各スレッドを起動する。自機位置推定スレッドや地図作成スレッドが動作することで、ＳＬＡＭ法に基づいて、地図の情報及びビジュアルオドメトリ（地図と画像とを用いて推定した自機位置の情報）の生成が開始される。その後、制御部１０は、動作終了かどうかを判定し（ステップＳ１０４）、動作終了なら（ステップＳ１０４；Ｙｅｓ）動作を終了し、動作終了でなければ（ステップＳ１０４；Ｎｏ）地図作成部１２が地図の情報の作成・更新を行う（ステップＳ１０５）。

次に制御部１０は、回転撮影フラグＲＦが０かどうかを判定する（ステップＳ１０６）。回転撮影フラグＲＦは、本実施形態の特徴的な機能である回転撮影処理の状態を示すフラグであり、０は「回転撮影処理中ではない」、１は「回転撮影処理中（一回転する前の段階）」、２は「回転撮影処理の最終段階」を示している。ＲＦが０なら（ステップＳ１０６；Ｙｅｓ）、ステップＳ１０７へ行き、自律移動するために駆動部４２に所望の動作を指示し（ステップＳ１０７）、ステップＳ１０４に戻る。ＲＦが０でないなら（ステップＳ１０６；Ｎｏ）、現在回転撮影処理中ということなので、移動処理を回転撮影処理に任せるために、メインフローでの移動処理は行わずにステップＳ１０４に戻る。なお、回転撮影処理の詳細については後述する。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

以上で自機位置推定スレッドの初期化処理が完了したので、位置推定部１３は、初期化済フラグをセットし、ＳＬＡＭ座標と実環境座標とのスケール対応を得るために、オドメトリによる並進距離（実環境での座標で求められる）を、上記の処理で推定したＳＬＡＭ座標での並進距離ｄで除することによって、スケールＳを求める（ステップＳ２１８）。

そして、制御部１０は、定期的に回転撮影部１１を動作させるために、現在時刻をタイマー（図示せず）から取得してＲＴＩＭＥ（回転撮影処理を開始する時刻を設定するための変数）にセットする（ステップＳ２１９）。そして、ステップＳ２０１、ステップＳ２０２を経由して、初期化済の場合の処理であるステップＳ２２０へ進む。

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

まず、制御部１０は、現在時刻がＲＴＩＭＥから所定の時間（ＤＴＩＭＥ（回転撮影処理を行う時間間隔が設定されている変数）：例えば１０分）経過していないか（回転撮影を行うための条件を満たしていないか）を判定する（ステップＳ２２０）。経過していなければ（ステップＳ２２０；Ｙｅｓ）ステップＳ２２１へ進む。経過していたら（ステップＳ２２０；Ｎｏ）回転撮影処理（ステップＳ２３３）を行った後にステップＳ２０１に戻る。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

そして制御部１０は、ステップＳ４０１に戻る。

次に自律移動装置１００のメインフロー（図３）のステップＳ１０１で起動される自機位置推定スレッドのフローチャート（図４）のステップＳ２３３の処理内容である回転撮影処理について図７を用いて説明する。これは、駆動部４２で自機を回転させつつ、撮像部４１で画像を撮影し、撮影した画像を自機の位置及び向きの情報と共に画像記憶部２１に記憶する処理である。

まず、回転撮影処理の初期化処理として、回転撮影部１１は回転撮影フラグＲＦに１をセットし（ステップＳ５０１）、累積回転角φ_Ｓをクリアする（ステップＳ５０２）。

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

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

回転撮影部１１は、基準対応特徴点数以上の対応特徴点が抽出できたら（ステップＳ５０７；Ｎｏ）、対応特徴点それぞれの３Ｄ位置（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ）をＭａｐ点データベースから取得し（ステップＳ５０８）、自機位置推定スレッドのステップＳ２２７と同様の計算を行ってＳＬＡＭ座標での自機の姿勢（並進ベクトルｔ及び回転行列Ｒで表される自機の位置及び向き）を求める（ステップＳ５０９）。

そして、回転撮影部１１は、これにスケールＳを乗算することで、ＶＯ（ビジュアルオドメトリ）を求め（ステップＳ５１０）、キーフレームカウンタＮＫＦをインクリメントしてから（ステップＳ５１１）、現フレームをキーフレームとしてフレームＤＢに登録する（ステップＳ５１２）。そして、回転撮影部１１は、新たなキーフレームが発生したことを地図作成スレッドに知らせるために、地図作成スレッドのキーフレームキューにキーフレームカウンタＮＫＦをセットする（ステップＳ５１３）。

次に回転撮影部１１は、回転撮影フラグＲＦに２がセットされているかを判定する（ステップＳ５１４）。２がセットされているなら（ステップＳ５１４；Ｙｅｓ）ＲＴＩＭＥに現在時刻をセットし（ステップＳ５２０）、回転撮影フラグＲＦをクリアしてから（ステップＳ５２１）、自機位置推定スレッドのＳ２０１に戻る。ＲＦが２でないなら（ステップＳ５１４；Ｎｏ）ステップＳ５１５へ進み、累積回転角φ_Ｓ＋回転角φ_１が３６０°未満であるかを判定する（ステップＳ５１５）。ここで、回転角φ_１は回転撮影処理の回転動作一回分の回転角であり、例えば３０°である。φ_１は撮像部４１の視野角の半分以下の角度を設定するのが望ましい。

回転撮影部１１は、累積回転角φ_Ｓ＋回転角φ_１が３６０°未満である場合は（ステップＳ５１５；Ｙｅｓ）、駆動部４２に回転角φ_１の回転動作をするように指示を出し（ステップＳ５１６）、累積回転角φ_Ｓに回転角φ_１を加算（ステップＳ５１７）してからステップＳ５０３に戻る。ここでの回転動作は、駆動部４２の二つの車輪を逆方向に等速度で回転させる等によって、位置を移動せずに回転できるのであれば、位置を移動しない回転動作をするのが望ましい。二つの車輪を同じ方向にしか回転させられない場合には、できるだけ回転半径の小さい回転動作をするのが望ましい。

回転撮影部１１は、累積回転角φ_Ｓ＋回転角φ_１が３６０°以上である場合は（ステップＳ５１５；Ｎｏ）、駆動部４２に回転角（３６０°−φ_Ｓ）の回転動作をするように指示を出し（ステップＳ５１８）、回転撮影フラグＲＦに２をセットしてから（ステップＳ５１９）ステップＳ５０３に戻る。

なお、もし駆動部４２により、位置を移動しない回転動作を行う場合は、ステップＳ５０９では、並進ベクトルｔが不変であるという前提で、回転行列Ｒを求めることができる。

以上の第一の実施形態では、回転撮影部１１は、「所定の時間（ＤＴＩＭＥ）が経過したら」動作する処理になっていたが、変形例として「所定の距離（例えば基準並進距離）以上移動したら」動作するようにしても良い。この場合は、ＲＴＩＭＥの代わりにＲｔ（前回回転撮影処理を開始した位置（並進ベクトル）を設定するための変数）を導入し、図４のステップＳ２１９は「Ｒｔ＝現在の並進ベクトルｔ」とし、ステップＳ２２０は「現在の並進ベクトルｔとＲｔとの距離＜所定の距離」とし、図７のステップＳ５２０は「Ｒｔ＝キーフレーム番号ＮＫＦの並進ベクトルｔ」とすれば、所定の距離移動するたびに回転撮影部１１が動作するようになる。

以上、第一の実施形態においては、所定の時間が経過する度に、定期的に、回転撮影部１１が回転動作を行いながら画像を撮影し、第一の実施形態の変形例においては、所定の距離を移動する度に、定期的に、回転撮影部１１が回転動作を行いながら画像を撮影する。この回転撮影部１１の動作により、類似画像検索部１４での検索対象となる画像記憶部２１内の画像の情報を増加させることができる。

第二の実施形態として、過去に訪れた位置とユークリッド距離的に近い位置を訪れた場合に回転撮影動作を行う実施形態を説明する。本実施形態に係る自律移動装置１００’は図８に示すように、第一の実施形態の制御部１０に距離判定部１６が追加され、記憶部２０に位置履歴記憶部２３が追加された構成になっている。距離判定部１６は、自律移動装置１００’の現在位置と過去の位置との距離が所定の距離以上かどうかを判定する。位置履歴記憶部２３には、位置推定部１３が推定した位置（並進ベクトルｔ）がタイムスタンプ（キーフレームカウンタ）と共に記憶される。なお、独立した位置履歴記憶部２３を追加しなくても、画像記憶部２１に位置履歴記憶部２３が含まれていると考えることもできる（画像記憶部２１内のフレームＤＢに記憶されている「姿勢」の情報の中には位置履歴記憶部２３に記憶されている「位置」（位置履歴記憶時の並進ベクトルｔ）の情報が含まれているため）。

第二の実施形態の自律移動処理全体のフローチャートは、第一の実施形態と共通で、図３に示される。第二の実施形態の自機位置推定スレッドのフローチャートは図９に示される。これは第一の実施形態の自機位置推定スレッドのフローチャートである図４とほとんどの部分が共通なので、異なる部分のみを説明する。

ステップＳ２１９２では、制御部１０はＲｔに現在の並進ベクトルｔをセットする。ステップＳ２２０２では、距離判定部１６は、現在の並進ベクトルｔとＲｔとの距離が所定の距離Ｄ（所定の距離（基準距離）を表す変数。例えば基準並進距離を設定する。）以上であり、かつ、位置履歴記憶部２３に現在の並進ベクトルｔと所定の類似度（以下「基準位置類似度」という。例えば０．９。）以上一致する位置（位置履歴記憶時の並進ベクトルｔ）が記憶されているかを判定する（位置の類似度は比較する二つの並進ベクトルの絶対値（要素の二乗和の平方根）を１に正規化したもの同士の内積を計算することで求めることができる）。なお、独立した位置履歴記憶部２３を追加せずに画像記憶部２１内のフレームＤＢを位置履歴記憶部２３としても利用する場合は、フレームＤＢ内のキーフレームの各姿勢の中に現在の並進ベクトルｔと基準位置類似度以上一致する位置（キーフレーム登録時の並進ベクトルｔ）が記憶されているかを判定することになる。

もし、現在の並進ベクトルｔがＲｔよりもＤ以上離れていて、かつ、位置履歴記憶部２３（又はフレームＤＢ）に基準位置類似度以上一致する位置が記憶されているなら（ステップＳ２２０２；Ｙｅｓ）ステップＳ２３３へ進んで回転撮影処理を行った後にステップＳ２０１に戻る。Ｒｔとの距離が基準距離Ｄ未満であるか、又は、位置履歴記憶部２３（若しくはフレームＤＢ）に基準位置類似度以上一致する位置が記憶されていないなら（ステップＳ２２０２；Ｎｏ）ステップＳ２２１へ進む。

ステップＳ２３４は、位置推定部１３が、位置履歴記憶部２３に現在の位置（並進ベクトルｔ）をタイムスタンプと共に登録する。登録する際に今までに記憶されている位置を確認して、同じ位置（又は基準位置類似度以上一致する位置）が記憶されている場合は登録しないようにしても良い。このようにすることで、位置履歴記憶部２３の記憶領域を節約することができる。また、独立した位置履歴記憶部２３を追加せずに画像記憶部２１内のフレームＤＢを位置履歴記憶部２３としても利用する場合は、ステップＳ２３４は省略できる。

第二の実施形態の地図作成スレッドは、第一の実施形態と共通であり、図５に示される。第二の実施形態のループクロージングスレッドも第一の実施形態と共通であり、図６に示される。第二の実施形態の回転撮影処理は図７に示される第一の実施形態とほとんど同じであるが、図７のステップＳ５２０の処理が「ＲＴＩＭＥ＝現在時刻」ではなく、「Ｒｔにキーフレーム番号ＮＫＦの並進ベクトルｔをセットする」となる点だけが異なる。

第二の実施形態は、このような処理にすることによって、過去に訪れた位置とユークリッド距離的に近い位置に来た時だけ回転撮影動作を行うことになり、ループクロージング処理を行う可能性が高い時を選んで効率的に回転撮影動作を行うことができる。なお、上記ではステップＳ２２０２の処理で距離判定部１６は「位置履歴記憶部２３に現在の並進ベクトルｔと基準位置類似度以上一致する位置が記憶されているかを判定」していたが、「位置履歴記憶部２３に現在の並進ベクトルｔとの距離が所定の距離（例えば「５ｃｍ」や「基準並進距離の１／１０」など、Ｄに比べてかなり小さい値を設定する。）未満の位置が記憶されているかを判定」しても良い（以下に述べる変形例においても同様）。

さらに第二の実施形態の変形例として、図９のステップＳ２２０２の条件を「現在の並進ベクトルｔとＲｔとの距離が基準距離Ｄ以上であり、かつ、位置履歴記憶部２３に現在の並進ベクトルｔと基準位置類似度以上一致する位置が記憶されており、かつ、現キーフレームと基準画像類似度以上に類似するキーフレームがフレームＤＢに存在しない」というように、３番目の条件を追加することが考えられる。

現キーフレームと基準画像類似度以上に類似するキーフレームがフレームＤＢに存在する場合、回転撮影処理をしなくても、ループクロージングスレッドにおいてループクロージング処理が行われる可能性が高い。したがって、この変形例では、このような場合に回転撮影処理を行わないことで、第二の実施形態に比較してさらに回転撮影処理を行う回数を減らすことができる。

第三の実施形態として、過去に訪れた位置とユークリッド距離的に近く、コビジビリティーグラフ的距離が遠い位置に訪れた場合に回転撮影動作を行う実施形態を説明する。まず、コビジビリティーグラフについて説明する。コビジビリティーグラフとは、図１０に示すように、「共通するＭａｐ点（３次元位置を取得済みの特徴点）を複数含んでいる等、直接的に相対位置関係が計算可能であるキーフレーム同士」を線で結んだグラフのことである。二つのキーフレーム間でこの線を最低ｎ回たどらないとたどり着けない場合、これらのキーフレーム間のコビジビリティーグラフ的距離はｎとなる。コビジビリティーグラフ的距離が大きいキーフレーム同士では、ＳＬＡＭ推定においてこの距離分の誤差が累積するため、相対位置の誤差を多く含むことになる。

第三の実施形態は、第二の実施形態と共通する点が多い。図９のステップＳ２２０２の条件にＡＮＤ条件で「基準位置類似度以上一致する位置とのコビジビリティーグラフ的距離が所定値ｎ以上」を加えた点だけが異なり、それ以外の処理や構成は第二の実施形態と共通である。

この場合、図９のステップＳ２２０２の条件は「現在の並進ベクトルｔとＲｔとの距離が基準距離Ｄ以上であり、かつ、位置履歴記憶部２３に現在の並進ベクトルｔと基準位置類似度以上一致する位置が記憶されており、かつ、基準位置類似度以上一致する位置とのコビジビリティーグラフ的距離が所定値以上」となる。

コビジビリティーグラフ的距離の求め方を説明する。まず、キーフレームＫと直接的に相対位置関係が計算可能なキーフレームの集合（簡単には「キーフレームＫとの画像類似度が基準画像類似度以上のキーフレームの集合」とすれば良い）をフレームＤＢを参照して抽出すれば、これが「キーフレームＫとのコビジビリティーグラフ的距離が１のキーフレーム集合」となる。そして、「キーフレームＫとのコビジビリティーグラフ的距離がｉのキーフレーム集合」に含まれるキーフレーム一つ一つについて、そのキーフレームと直接的に相対位置関係が計算可能なキーフレームの集合（簡単には「そのキーフレームとの画像類似度が基準画像類似度以上のキーフレームの集合」とすれば良い）をフレームＤＢを参照して抽出すれば、これが「キーフレームＫとのコビジビリティーグラフ的距離がｉ＋１のキーフレーム集合」となる。これらのキーフレーム集合の抽出処理においては、類似画像検索部１４が相対位置計算可能判定部に相当する処理を行う。

キーフレームＫとＬとが与えられたときに、ＫとＬとの間のコビジビリティーグラフ的距離を求めるには、上述したやり方で、まず「キーフレームＫとのコビジビリティーグラフ的距離が１のキーフレーム集合」を生成する。そして、「キーフレームＫとのコビジビリティーグラフ的距離がｉのキーフレーム集合」の中にＬが含まれていなければ、「キーフレームＫとのコビジビリティーグラフ的距離がｉ＋１のキーフレーム集合」を生成する処理を、「キーフレームＫとのコビジビリティーグラフ的距離がｉ＋１のキーフレーム集合」にＬが含まれるまで繰り返す。Ｌが含まれるキーフレーム集合が得られたら、その時の「ｉ＋１」がＫとＬとの間のコビジビリティグラフ的距離となる。このコビジビリティグラフ的距離算出処理においては、制御部１０がコビジビリティグラフ的距離算出部に相当する処理を行う。

ステップＳ２２０２では、二つ目の判定条件として「位置履歴記憶部２３に現在の並進ベクトルｔと基準位置類似度以上一致する位置が記憶されているか」という条件があるが、この条件が満たされた場合、その位置に対応するキーフレーム（位置履歴記憶部２３に位置を登録するときのタイムスタンプ（キーフレームカウンタ）の値のキーフレーム番号のキーフレームが、その位置に対応するキーフレームとなる）をフレームＤＢを参照して抽出してキーフレームＫとすると、「基準位置類似度以上一致する位置とのコビジビリティーグラフ的距離が所定値ｎ以上」の判定は、上記のようにして、「現キーフレームとのコビジビリティーグラフ的距離が１のキーフレーム集合」〜「現キーフレームとのコビジビリティーグラフ的距離がｎ−１のキーフレーム集合」を生成し、これらのキーフレーム集合の中に、キーフレームＫが含まれていなければ「基準位置類似度以上一致する位置とのコビジビリティーグラフ的距離が所定値ｎ以上」と判定できる。

コビジビリティーグラフ的距離が近いキーフレームが存在する場合は、誤差はあまり累積していないと考えられるので、コビジビリティーグラフ的距離が所定値以上の場合だけ回転撮影処理を行うことで、誤差が所定値以上累積した場合だけ回転撮影処理を行うことになり、回転撮影処理を行う回数を減らすことができる。

今まで説明した実施形態では、この回転撮影処理により、様々な向きから撮影した画像がキーフレームとしてフレームＤＢに登録されるため、それ以後のループクロージングスレッドにおいて、類似キーフレームを発見しやすくなり、その結果、地図補正が行われる頻度が増加し、自機位置等の累積誤差を減らすことができる。

ここまでは、フレームＤＢに登録するキーフレームを増加させることによって、ループクロージング処理を行う頻度を増加させたが、別のやり方として、ループクロージングスレッドでのステップＳ４０５で類似画像検索部１４が類似画像を検索する際に、基準画像類似度を小さくする（類似度がそれほど大きくなくてもループクロージング処理を行うようにする）という方法もある。

ただし、単に基準画像類似度を小さくしただけでは間違った位置（過去に訪れた場所とは関係ない位置）でもループクロージング処理を行ってしまう可能性が高くなる。そこで、位置推定部１３によって推定された現在の自機位置が、類似画像検索部１４での比較対象となるキーフレームの位置と基準距離以下になっている場合だけ基準画像類似度を小さくする処理を行う。これを第四の実施形態として説明する。

第四の実施形態の構成は第二の実施形態と共通であり、図８に示される。第四の実施形態の自律移動処理全体のフローチャートは、第一の実施形態と共通で、図３に示される。第四の実施形態の自機位置推定スレッドのフローチャートは図１１に示される。これは第一の実施形態の自機位置推定スレッドのフローチャートである図４とほとんどの部分が共通なので、異なる部分のみを説明する。

ステップＳ２１９３は、回転撮影処理の所定条件の初期化を行う。例えば、制御部１０はＲｔに現在の並進ベクトルｔをセットする。第四の実施形態では回転撮影処理は必須ではないので、回転撮影処理を行わない場合は、このステップＳ２１９３は省略可能である。

ステップＳ２２０３では、制御部１０は、回転撮影処理の所定条件が満たされているかを判定する。もし満たされているなら（ステップＳ２２０３；Ｙｅｓ）ステップＳ２３３へ進んで回転撮影処理を行った後にステップＳ２０１に戻る。満たされていないなら（ステップＳ２２０３；Ｎｏ）ステップＳ２２１へ進む。第四の実施形態において、回転撮影処理を行わない場合は、このステップＳ２２０３も省略可能で、この場合は条件判定をおこなうことなく、ステップＳ２２１へ進む。

ステップＳ２３４は、第二の実施形態の自機位置推定スレッドのフローチャートである図９のステップＳ２３４と同じなので、説明を省略する。

第四の実施形態の地図作成スレッドは、第一の実施形態と共通であり、図５に示される。第四の実施形態の回転撮影処理は図７に示される第一の実施形態とほとんど同じであるが、図７のステップＳ５２０の処理が「ＲＴＩＭＥ＝現在時刻」ではなく、「回転撮影処理の所定条件の初期化」となる点だけが異なる。なお、第四の実施形態で回転撮影処理を行わない場合は、図７の処理全体が省略できる。

第四の実施形態のループクロージングスレッドは図６に示す第一の実施形態のループクロージングスレッドと一部異なり、図１２に示すように、ステップＳ４０４とステップＳ４０５の間にステップＳ４０８〜ステップＳ４１０の各ステップが追加される。ステップＳ４０８は、制御部１０が位置履歴記憶部２３に現キーフレーム（キーフレーム番号がＬＫＦのキーフレーム）の位置との一致度が基準位置類似度以上になるもの（現キーフレームを撮影した時の自機の位置と、位置履歴記憶部２３に記憶されている位置との距離が所定の値以下になる位置）が格納されているかを判定する。

位置履歴記憶部２３に格納されているなら（ステップＳ４０８；Ｙｅｓ）基準画像類似度の値としてＤ１を採用し（ステップＳ４０９）、格納されていないなら（ステップＳ４０８；Ｎｏ）基準画像類似度の値としてＤ２を採用する（ステップＳ４１０）。ここで、Ｄ１とＤ２は、Ｄ１＜Ｄ２という関係になっている。つまり、以前訪れた位置に近い位置に来ている場合（ステップＳ４０８がＹｅｓとなる場合）は、基準画像類似度を通常値（Ｄ２）よりも小さな値に設定する。このようにすることによって、ステップＳ４０５での検索において類似度が若干小さくても（Ｄ１でも）類似画像を発見できるようにして、ステップＳ４０７でのループクロージング処理を行わせるようにする。

ステップＳ４０８でＹｅｓと判定される場合は、以前訪れた位置に近い位置にいることが多い。この状況でフレームＤＢから類似画像が発見できないということは、以前とは撮影している向きが異なっていると考えられる。このような場合に、基準画像類似度を通常値よりも小さな値に設定することで、撮影した向きが多少異なっていても類似画像を発見できることになり、ループクロージング処理を行わせることができるようになる。

この第四の実施形態は、第一〜第三の実施形態とは異なり、回転撮影処理がなくても実現できる。そのため、上記では回転撮影処理を省略しても良いことも説明した。ただし、第四の実施形態を回転撮影処理と併せて実行すると、より確実にループクロージング処理を実行できる。

回転撮影処理を行わない第四の実施形態においては、撮像部４１として視野角の広いものを採用するのが望ましい。特に、視野角が１８０°以上の撮像部４１（最も望ましいのは視野角３６０°の魚眼レンズを採用した撮像部４１）を用いれば、以前訪れた位置に来たら、どちらの方向を向いていても、共通の特徴点を撮影できていることが多いため、画像の類似度の判定の閾値を下げてもループクロージング処理を行えることが多い。

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

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

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

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

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

（付記１）
回転動作を行いながら画像を撮影する回転撮影部と、
撮影した画像の情報を、撮影時の自機の位置の情報と共に記憶する画像記憶部と、
前記画像記憶部に記憶されている複数の画像の情報を用いて作成した地図を記憶する地図記憶部と、
撮影した画像との類似度が所定の類似度以上になる画像を前記画像記憶部から検索する類似画像検索部と、
前記類似画像検索部が、前記所定の類似度以上になる画像を発見した場合、該画像に対応する位置の情報に基づいて前記地図記憶部に記憶されている地図を補正する地図補正部と、
を備える自律移動装置。

（付記２）
前記回転撮影部は、定期的に、回転動作を行いながら画像を撮影する、
付記１に記載の自律移動装置。

（付記３）
前記回転撮影部は、所定の時間が経過する度に、回転動作を行いながら画像を撮影する、
付記２に記載の自律移動装置。

（付記４）
前記回転撮影部は、自律移動装置が所定の距離を移動する度に、回転動作を行いながら画像を撮影する、
付記２に記載の自律移動装置。

（付記５）
前記自律移動装置は、さらに、自機の位置を推定する位置推定部と、
前記位置推定部が推定した位置の履歴を記憶する位置履歴記憶部と、
現在の位置との距離が所定の距離以下になる過去の位置が前記位置履歴記憶部に記憶されているかを判定する距離判定部と、
を備え、
前記回転撮影部は、前記距離判定部が前記過去の位置が前記位置履歴記憶部に記憶されていると判定した場合に、回転動作を行いながら画像を撮影する、
付記１に記載の自律移動装置。

（付記６）
前記回転撮影部は、前記距離判定部が前記過去の位置が前記位置履歴記憶部に記憶されていると判定した場合、前記類似画像検索部が前記所定の類似度以上になる画像を発見できない場合にのみ、回転動作を行いながら画像を撮影する、
付記５に記載の自律移動装置。

（付記７）
前記自律移動装置は、さらに、二つの撮影位置の相対位置関係を直接計算可能かを判定する相対位置計算可能判定部と、
前記画像記憶部に記憶されている画像を最低何枚用いれば与えられた位置と現在位置との相対位置関係を計算できるかを前記相対位置計算可能判定部により算出することによってコビジビリティグラフ的距離を算出するコビジビリティグラフ的距離算出部と、
を備え、
前記回転撮影部は、前記距離判定部が前記過去の位置が前記位置履歴記憶部に記憶されていると判定した場合、前記コビジビリティグラフ的距離算出部が算出した前記過去の位置と現在位置との間のコビジビリティグラフ的距離が所定の値以上の場合にのみ、回転動作を行いながら画像を撮影する、
付記５に記載の自律移動装置。

（付記８）
前記自律移動装置は、さらに、自機の位置を推定する位置推定部と、
前記位置推定部が推定した位置の履歴を記憶する位置履歴記憶部と、
現在の位置との距離が所定の距離以下になる過去の位置が前記位置履歴記憶部に記憶されているかを判定する距離判定部と、
を備え、
前記類似画像検索部は、前記距離判定部が前記過去の位置が前記位置履歴記憶部に記憶されていると判定した場合に、前記所定の類似度を所定の値だけ小さくする、
付記１から７のいずれか一つに記載の自律移動装置。

（付記９）
前記画像記憶部は、自律移動装置が基準並進距離を移動する度に、撮影した画像の情報を記憶する、
付記１から８のいずれか一つに記載の自律移動装置。

（付記１０）
駆動部の車輪の直径に所定の値を乗算した値を前記基準並進距離に設定する、
付記９に記載の自律移動装置。

（付記１１）
前記自律移動装置は、さらに、複数の画像から位置を推定可能な特徴点を抽出する位置推定可能特徴点抽出部を備え、
前記位置推定可能特徴点抽出部が抽出した全ての特徴点と自機位置との距離の平均値を所定の値で除算した値を前記基準並進距離に設定する、
付記９に記載の自律移動装置。

（付記１２）
撮像部が撮影した複数の画像の情報に基づいて地図を作成して自律的に移動する自律移動方法であって、
回転動作を行いながら前記撮像部が画像を撮影する回転撮影ステップと、
前記撮像部が撮影した画像との類似度が所定の類似度以上になる画像が過去に撮影されていた場合、該過去に撮影した画像に対応する位置の情報に基づいて、前記地図を補正する地図補正ステップと、
を備える自律移動方法。

（付記１３）
回転動作を行いながら画像を撮影する回転撮影ステップと、
撮影した画像の情報を、撮影時の自機の位置の情報と共に画像記憶部に記憶させる画像記憶ステップと、
前記画像記憶部に記憶されている複数の画像の情報を用いて地図を作成する地図作成ステップと、
撮影した画像との類似度が所定の類似度以上になる画像を前記画像記憶部から検索する類似画像検索ステップと、
前記類似画像検索ステップで、前記所定の類似度以上になる画像を発見した場合、該画像に対応する位置の情報に基づいて作成されている地図を補正する地図補正ステップと、
を備える自律移動方法。

（付記１４）
コンピュータに、
回転動作を行いながら画像を撮影する回転撮影ステップ、
撮影した画像の情報を、撮影時の自機の位置の情報と共に画像記憶部に記憶させる画像記憶ステップ、
前記画像記憶部に記憶されている複数の画像の情報を用いて地図を作成する地図作成ステップ、
撮影した画像との類似度が所定の類似度以上になる画像を前記画像記憶部から検索する類似画像検索ステップ、
前記類似画像検索ステップで、前記所定の類似度以上になる画像を発見した場合、該画像に対応する位置の情報に基づいて作成されている地図を補正する地図補正ステップ、
を実行させるためのプログラム。

１００，１００’…自律移動装置、１０…制御部、１１…回転撮影部、１２…地図作成部、１３…位置推定部、１４…類似画像検索部、１５…地図補正部、１６…距離判定部、２０…記憶部、２１…画像記憶部、２２…地図記憶部、２３…位置履歴記憶部、３０…センサ部、３１…障害物センサ、４１…撮像部、４２…駆動部、４３…入力部、４４…通信部、４５…電源

Claims (15)

1. 回転動作を行いながら画像を撮影する回転撮影部と、
   撮影した画像の情報を、撮影時の自機の地図座標内での位置を示す並進ベクトルｔと回転行列Ｒの値と共に記憶する画像記憶部と、
   前記画像記憶部に記憶されている複数の画像の情報を用いて作成した地図を記憶する地図記憶部と、
   撮影した画像との類似度が所定の類似度以上になる画像を前記画像記憶部から検索する類似画像検索部と、
   前記類似画像検索部が、前記所定の類似度以上になる画像を発見した場合、以前来たことのある同じ場所に戻ってきたことを認識したと判断し、前記発見した画像に対応して前記画像記憶部に記憶されている自機の地図座標内での位置を示す並進ベクトルｔと回転行列Ｒの値と、現在の位置の並進ベクトルｔと回転行列Ｒの値と、を用いて前記地図記憶部に記憶されている地図を補正する地図補正部と、
   を備える自律移動装置。
2. 前記回転撮影部は、定期的に、回転動作を行いながら画像を撮影する、
   請求項１に記載の自律移動装置。
3. 前記回転撮影部は、所定の時間が経過する度に、回転動作を行いながら画像を撮影する、
   請求項２に記載の自律移動装置。
4. 前記回転撮影部は、自律移動装置が所定の距離を移動する度に、回転動作を行いながら画像を撮影する、
   請求項２に記載の自律移動装置。
5. 前記自律移動装置は、さらに、自機の位置を推定する位置推定部と、
   前記位置推定部が推定した位置の履歴を記憶する位置履歴記憶部と、
   現在の位置との距離が所定の距離以下になる過去の位置が前記位置履歴記憶部に記憶されているかを判定する距離判定部と、
   を備え、
   前記回転撮影部は、前記距離判定部が前記過去の位置が前記位置履歴記憶部に記憶されていると判定した場合に、回転動作を行いながら画像を撮影する、
   請求項１に記載の自律移動装置。
6. 前記回転撮影部は、前記距離判定部が前記過去の位置が前記位置履歴記憶部に記憶されていると判定した場合、前記類似画像検索部が前記所定の類似度以上になる画像を発見できない場合にのみ、回転動作を行いながら画像を撮影する、
   請求項５に記載の自律移動装置。
7. 前記自律移動装置は、さらに、二つの撮影位置の相対位置関係を直接計算可能かを判定する相対位置計算可能判定部と、
   前記画像記憶部に記憶されている画像を最低何枚用いれば与えられた位置と現在位置との相対位置関係を計算できるかを前記相対位置計算可能判定部により算出することによってコビジビリティグラフ的距離を算出するコビジビリティグラフ的距離算出部と、
   を備え、
   前記回転撮影部は、前記距離判定部が前記過去の位置が前記位置履歴記憶部に記憶されていると判定した場合、前記コビジビリティグラフ的距離算出部が算出した前記過去の位置と現在位置との間のコビジビリティグラフ的距離が所定の値以上の場合にのみ、回転動作を行いながら画像を撮影する、
   請求項５に記載の自律移動装置。
8. 前記自律移動装置は、さらに、自機の位置を推定する位置推定部と、
   前記位置推定部が推定した位置の履歴を記憶する位置履歴記憶部と、
   現在の位置との距離が所定の距離以下になる過去の位置が前記位置履歴記憶部に記憶されているかを判定する距離判定部と、
   を備え、
   前記類似画像検索部は、前記距離判定部が前記過去の位置が前記位置履歴記憶部に記憶されていると判定した場合に、前記所定の類似度を所定の値だけ小さくする、
   請求項１から７のいずれか一項に記載の自律移動装置。
9. 前記画像記憶部は、自律移動装置が基準並進距離を移動する度に、撮影した画像の情報を記憶する、
   請求項１から８のいずれか一項に記載の自律移動装置。
10. 駆動部の車輪の直径に所定の値を乗算した値を前記基準並進距離に設定する、
    請求項９に記載の自律移動装置。
11. 前記自律移動装置は、さらに、複数の画像から位置を推定可能な特徴点を抽出する位置推定可能特徴点抽出部を備え、
    前記位置推定可能特徴点抽出部が抽出した全ての特徴点と自機位置との距離の平均値を所定の値で除算した値を前記基準並進距離に設定する、
    請求項９に記載の自律移動装置。
12. 回転動作を行いながら画像を撮影する回転撮影ステップと、
    撮影した画像の情報を、撮影時の自機の地図座標内での位置を示す並進ベクトルｔと回転行列Ｒの値と共に画像記憶部に記憶させる画像記憶ステップと、
    前記画像記憶部に記憶されている複数の画像の情報を用いて地図を作成する地図作成ステップと、
    撮影した画像との類似度が所定の類似度以上になる画像を前記画像記憶部から検索する類似画像検索ステップと、
    前記類似画像検索ステップで、前記所定の類似度以上になる画像を発見した場合、以前来たことのある同じ場所に戻ってきたことを認識したと判断し、前記発見した画像に対応して前記画像記憶部に記憶されている自機の地図座標内での位置を示す並進ベクトルｔと回転行列Ｒの値と、現在の位置の並進ベクトルｔと回転行列Ｒの値と、を用いて、作成されている地図を補正する地図補正ステップと、
    を備える自律移動方法。
13. コンピュータに、
    回転動作を行いながら画像を撮影する回転撮影ステップ、
    撮影した画像の情報を、撮影時の自機の地図座標内での位置を示す並進ベクトルｔと回転行列Ｒの値と共に画像記憶部に記憶させる画像記憶ステップ、
    前記画像記憶部に記憶されている複数の画像の情報を用いて地図を作成する地図作成ステップ、
    撮影した画像との類似度が所定の類似度以上になる画像を前記画像記憶部から検索する類似画像検索ステップ、
    前記類似画像検索ステップで、前記所定の類似度以上になる画像を発見した場合、以前来たことのある同じ場所に戻ってきたことを認識したと判断し、前記発見した画像に対応して前記画像記憶部に記憶されている自機の地図座標内での位置を示す並進ベクトルｔと回転行列Ｒの値と、現在の位置の並進ベクトルｔと回転行列Ｒの値と、を用いて、作成されている地図を補正する地図補正ステップ、
    を実行させるためのプログラム。
14. 撮像部と、駆動部と、制御部と、記憶部と、を備える自律移動装置であって、
    前記記憶部は、
    前記撮像部が撮影した画像の情報を、撮影時の自機の地図座標内での位置を示す並進ベクトルｔと回転行列Ｒの値と紐付けて記憶し、
    前記記憶した複数の画像の情報を用いて作成した地図を記憶し、
    前記制御部は、
    前記駆動部に自機を回転させる動作を行わせつつ、前記撮像部に画像を撮影させ、
    前記撮影させた画像との類似度が所定の類似度以上になる画像を前記記憶部から検索し、
    前記所定の類似度以上になる画像を発見した場合、以前来たことのある同じ場所に戻ってきたことを認識したと判断し、前記発見した画像に対応して前記記憶部に記憶されている画像の自機の地図座標内での位置を示す並進ベクトルｔと回転行列Ｒの値と、現在の位置の並進ベクトルｔと回転行列Ｒの値と、を用いて前記記憶部に記憶されている地図を補正する、
    自律移動装置。
15. 外部装置と通信する通信部をさらに備え、
    前記通信部は当該自律移動装置と前記外部装置とのデータ受け渡しを行う、
    請求項１４に記載の自律移動装置。

Images