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

Description

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

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

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

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

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

このような画像を用いた自機位置推定では、カメラの撮影する方向が大きく変わった場合や撮影した画像中に特徴点が少ない場合は、過去の画像との共通特徴点が得られず、自機位置の推定に失敗するという問題があった。
また、特許文献１には、移動量に基づいて自己位置の候補点を推定し、地図上に複数の測定点を定め、距離マップに基づき前記測定点から壁までの距離を評価距離とし、第１評価工程において、評価距離に基づいて一致する度合いを評価し、第２の評価工程において、外界センサから測定点を向く方向の単位ベクトルである測定方向ベクトルと、測定点の壁方向ベクトルとの内積を評価関数として評価し、第１の評価工程において一致が大きく、かつ前記第２の評価工程において評価関数が大きい候補点を、自己位置として推定する移動ロボットの自己位置推定方法が開示されている。
また、特許文献２には、起動されたときに、停止要因記憶手段に記憶されている停止要因に基づいて、現在の自己位置から継続して自律移動が可能であるか否かを判断し、自律移動が可能であると判断した場合には、自己位置記憶手段に記憶されている自己位置に基づいて自律移動を開始し、自律移動が不可能であると判断した場合には、停止状態を保持することを特徴とする自律移動式無人搬送車が開示されている。
しかしながら、特許文献１に記載の技術では、自己位置推定の精度を上げることはできるが、自機位置の推定に失敗した場合、復帰ができないという課題があった。
また特許文献２に記載の技術では、やはり自律移動が不可能であると判断した場合、復帰ができないという課題があった。

そこで、本発明は、上記問題を解決するためになされたものであり、自機位置推定が不可能になった状態から素早く復帰することのできる自律移動装置等を提供することを目的とする。

上記目的を達成するため、本発明の自律移動装置は、
撮像部が撮影した複数の画像の情報に基づいて自律的に移動するように処理する移動処理部と、
前記撮像部が撮影した複数の画像の情報を用いて自機位置を推定する位置推定部と、
前記位置推定部における自機位置の推定結果が一つに定まるか否かを判定する推定判定部と、
前記推定判定部が、前記推定結果が一つに定まらないと判定した場合には、前記位置推定部における自機位置の推定結果が一つに定まる位置に移動するように前記移動処理部を制御する移動処理制御部と、
前記移動処理部により自機の位置を変化させるたびにその変化量を位置変化履歴記憶部に登録する位置変化履歴登録部と、
前記推定判定部が前記推定結果が一つに定まると判定した場合に、その時点での位置の情報を時刻情報と共に位置履歴記憶部に登録する位置履歴登録部と、
を備え、
前記移動処理制御部は、前記推定判定部が前記推定結果が一つに定まらないと判定した場合には、前記位置履歴記憶部に登録されている位置に自機がいた時点まで遡りながら、前記位置変化履歴記憶部に登録されている位置の変化量とは逆方向に移動するように前記移動処理部を制御することを特徴とする。

本発明によれば、自機位置推定が不可能になった状態から素早く復帰することができる。

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

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

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

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

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

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

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

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

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

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

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

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

位置変化履歴記憶部２３には、移動処理部１１によって自機の位置がどのように変化したかの履歴が記憶される。

位置履歴記憶部２４には、位置推定部１３によって推定された自機位置の履歴が記憶される。

センサ部３０として、加速度センサ３１、角速度センサ３２、障害物センサ３３を備える。加速度センサ３１は、ＸＹＺ方向（３軸）の加速度を測定するセンサである。これらの値を時間積分することでＸＹＺの各方向の速度を求めることができ、さらにこれら速度を時間積分することでＸＹＺの各方向の位置の変化量（移動距離）を求めることができる（ただしセンサ値を二回積分するので、累積誤差はかなり大きくなる）。

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

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

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

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

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

移動処理部１１は、駆動部４２を動かして自機を移動させる処理を行う。

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

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

推定判定部１４は、現在取得できている情報に基づいて位置推定部１３が自機位置を推定することが可能かどうかを判定する。より詳しくは、自機位置を推定するのに必要な情報が取得できており、それらの情報に基づいた自機位置の推定結果が一つに定まるなら推定可能と判定する。逆に、自機位置を推定するのに必要な情報が取得できていなかったり、取得できていてもそれらの情報に基づいた自機位置の推定結果が不定（複数の推定候補が生じてしまう等）になってしまったりしてしまう場合は推定不可能と判定する。

移動処理制御部１５は、推定判定部１４が推定不可能と判定した場合に、自機位置を推定可能な位置に自律的に移動するように移動処理部１１を制御する。

図３は、自律移動装置１００のメインフローである。制御部１０は、自機位置推定スレッド（ステップＳ１０１）、地図作成スレッド（ステップＳ１０２）、ループクロージングスレッド（ステップＳ１０３）の各スレッドを起動する。自機位置推定スレッドや地図作成スレッドが動作することで、ＳＬＡＭ法に基づいて、地図の情報及びビジュアルオドメトリ（地図と画像とを用いて推定した自機位置の情報）の生成が開始される。そして、制御部１０は、位置変化履歴カウンタＤＣ（位置変化履歴記憶部２３への位置変化履歴の登録数を示す変数）を０クリアする（ステップＳ１０４）。その後、制御部１０は、動作終了かどうかを判定し（ステップＳ１０５）、動作終了なら（ステップＳ１０５；Ｙｅｓ）動作を終了し、動作終了でなければ（ステップＳ１０５；Ｎｏ）地図作成部１２が地図の情報の作成・更新を行う（ステップＳ１０６）。

次に制御部１０は、推定状態変数ＥＳ（位置推定が可能な状態なのか否かを表す変数）の値が「位置推定不可能」であるかを判定する（ステップＳ１０７）。位置推定不可能なら（ステップＳ１０７；Ｙｅｓ）、位置推定可能な位置に自律的に移動するための処理である「移動処理制御」を行ってから（ステップＳ１１０）ステップＳ１０５に戻る。この「移動処理制御」が本実施形態の特徴的な機能を実現しており、詳細については後述する。

推定状態変数ＥＳの値が「位置推定不可能」でないなら（ステップＳ１０７；Ｎｏ）位置変化履歴カウンタＤＣをインクリメントし（ステップＳ１０８）、自律移動するために移動処理部１１が駆動部４２に所望の動作を指示するとともに、その動作にともなう位置の変化量をタイムスタンプ（現在時刻など、位置履歴記憶部２４のタイムスタンプと比較可能なものを用いる。）と共に位置変化履歴記憶部２３のＤＣ番目の項目に登録し（ステップＳ１０９）、ステップＳ１０５に戻る。このステップＳ１０９での位置の変化量の登録処理において、移動処理部１１は位置変化履歴登録部に相当する。

ここで、位置の変化量は、ステップＳ１０９で移動処理部１１が駆動部４２に指示を出す前の位置から、駆動部４２が指示に従って動作して自律移動装置１００が移動した後の位置までの（ステップＳ１０９での移動処理の動作一回分の）変化量である。この変化量はオドメトリ情報を用いて取得しても良いし、加速度センサ３１や角速度センサ３２の値を用いて取得しても良い。通常はオドメトリ情報を用いた方が精度の高い位置変化量を取得できるが、人の手によって移動させられた場合等はオドメトリ情報は役に立たない（この場合でも加速度センサ３１や角速度センサ３２の値は役に立つ）。そこで、オドメトリ情報とセンサ情報を両方とも用いることで、車輪がスリップしたり、人の手によって移動させられてしまったりした場合等によるオドメトリ情報の誤差をセンサ情報によって補償するようにするのが望ましい。

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

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

まず図４を用いて自機位置推定スレッドの初期化処理を説明する。位置推定部１３は、推定状態変数ＥＳに「初期化中」をセットし、位置履歴登録カウンタＨＣ（位置履歴記憶部２４への位置履歴の登録数を示す変数）と位置履歴検索カウンタＳＣ（位置履歴記憶部２４から位置履歴を取得する際に用いる変数）をクリアする（ステップＳ２０１）。次にフレームカウンタＮ（撮像部４１での画像の取得数を示す変数）に−１をセットする（ステップＳ２０２）。次に位置推定部１３は、動作終了かを判定する（ステップＳ２０３）。動作終了なら（ステップＳ２０３；Ｙｅｓ）終了し、動作終了でないなら（ステップＳ２０３；Ｎｏ）、撮像部４１で画像を取得する（ステップＳ２０４）。画像は例えば３０ｆｐｓで取得することができる（取得した画像はフレームとも呼ばれる）。

次に、取得した画像内から２Ｄ特徴点を取得する（ステップＳ２０５）。２Ｄ特徴点とは画像中のエッジ部分など、画像内の特徴的な部分であり、ＳＩＦＴ（Ｓｃａｌｅ−Ｉｎｖａｒｉａｎｔ Ｆｕｔｕｒｅ Ｔｒａｎｓｆｏｒｍ）やＳＵＲＦ（Ｓｐｅｅｄ−Ｕｐ Ｒｏｂｕｓｔ Ｆｅａｔｕｒｅｓ）等のアルゴリズムを用いて取得することができる。なお、２Ｄ特徴点を取得するのに他のアルゴリズムを使っても良い。

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

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

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

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

なお、特徴点の対応数が５点以上あっても、二つの画像間の姿勢の推定値が一つに定まらない場合がある（条件が足らずに不定となってしまう場合や、最小二乗法で一つに定めようとしても確からしい推定値が一つに定まらずに複数になってしまう場合等がある）。位置推定部１３が二つの画像間の姿勢（それぞれの画像を取得した位置の差分（並進ベクトルｔ）及び向きの差分（回転行列Ｒ））を推定する際に、推定値が一つに定まる場合は推定判定部１４は推定可能と判定し、推定値が一つに定まらない場合は推定判定部１４は推定不可能と判定する（ステップＳ２１２）。

推定判定部１４が推定不可能と判定した場合は（ステップＳ２１２；Ｎｏ）、ステップＳ２０２に戻って初期画像の取得からやり直す。推定判定部１４が推定可能と判定した場合は（ステップＳ２１２；Ｙｅｓ）、二つの画像間の姿勢（並進ベクトルｔ及び回転行列Ｒ）が求まったということになるので、ステップＳ２１３へ進む。

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

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

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

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

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

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

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

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

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

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

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

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

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

以上で自機位置推定スレッドの初期化処理が完了したので、図５の自機位置推定（メイン処理）に進む（ステップＳ２２０）。

次に図５を用いて自機位置推定スレッドのメイン処理を説明する。まず、位置推定部１３は、推定状態変数ＥＳに「位置推定可能」をセットする（ステップＳ２３１）。初期化処理において、自機位置の推定ができており、この時点では位置推定可能な状態であると言えるからである。次に位置推定部１３は、動作終了かを判定する（ステップＳ２３２）。動作終了なら（ステップＳ２３２；Ｙｅｓ）終了し、動作終了でないなら（ステップＳ２３２；Ｎｏ）、撮像部４１で画像を取得し（ステップＳ２３３）、フレームカウンタＮをインクリメントする（ステップＳ２３４）。

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

そして、推定判定部１４は、対応特徴点の個数が所定の値（例えば５）未満かどうかを判定し（ステップＳ２３７）、所定の値未満の場合（ステップＳ２３７；Ｙｅｓ）は確からしい自機位置の推定が不可能なので、ステップＳ２４７に進む。ここで、すぐにステップＳ２４７に進むのではなく、ステップＳ２３６に戻って、フレームデータベースに登録されているキーフレームの中から対応特徴点の個数が所定の値以上のものを検索するようにしても良い。この場合は、フレームデータベースに登録されているキーフレームの中に対応特徴点の個数が所定の値以上のものが見つからなかった場合にステップＳ２４７に進む。

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

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

この自機位置の推定において、自機の姿勢の推定値が一つに定まらない場合がある（条件が足らずに不定となってしまう場合（例えば二つの画像での対応特徴点数が５点未満の場合）や、最小二乗法で一つに定めようとしても確からしい推定値が一つに定まらずに複数になってしまう場合（例えば式（６）で、最小値が所定の値よりも大きい場合や、極小値が複数存在し最小値（最も小さい極小値）を２番目に小さい極小値で割った値が所定の値よりも大きい場合）等がある）。位置推定部１３が自機の姿勢（並進ベクトルｔ及び回転行列Ｒ）を推定する際に、推定値が一つに定まる場合は推定判定部１４は推定可能と判定し、推定値が一つに定まらない場合は推定判定部１４は推定不可能と判定する（ステップＳ２４０）。

推定判定部１４が推定不可能と判定した場合は（ステップＳ２４０；Ｎｏ）、ステップＳ２４７に進む。推定判定部１４が推定可能と判定した場合は（ステップＳ２４０；Ｙｅｓ）、自機の姿勢（並進ベクトルｔ及び回転行列Ｒ）が求まったということになるので、ステップＳ２４１へ進む。

ＳＬＡＭ座標での現在の自機の姿勢（並進ベクトルｔ及び回転行列Ｒ）が求められたので、制御部１０は、位置履歴登録カウンタＨＣをインクリメントし、位置履歴記憶部２４のＨＣ番目の項目に、現在の自機の姿勢（並進ベクトルｔ及び回転行列Ｒ）をタイムスタンプ（現在時刻など、位置変化履歴記憶部２３のタイムスタンプと比較可能なものを用いる）と共に登録し、位置履歴検索カウンタＳＣに位置履歴登録カウンタＨＣの値をセットする（ステップＳ２４１）。このステップＳ２４１において、制御部１０は位置履歴登録部に相当する。そして、位置推定部１３は、現在の自機の姿勢にスケールＳを乗算することで、ＶＯ（ビジュアルオドメトリ）を求める（ステップＳ２４２）。ＶＯは実環境での自機の位置及び向きとして利用できる。

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

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

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

次にステップＳ２４７以降を説明する。これは自機位置の推定ができなくなったときの処理である。まず、位置推定部１３は、推定状態変数ＥＳに「位置推定不可能」をセットする（ステップＳ２４７）。次に位置推定部１３は、動作終了かを判定する（ステップＳ２４８）。動作終了なら（ステップＳ２４８；Ｙｅｓ）終了し、動作終了でないなら（ステップＳ２４８；Ｎｏ）、撮像部４１で画像を取得する（ステップＳ２４９）。

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

制御部１０は、取得画像との類似度が基準画像類似度以上になるキーフレームをフレームＤＢから発見できたかを判定する（ステップＳ２５１）。発見できなかったら（ステップＳ２５１；Ｎｏ）ステップＳ２４８に戻る。発見できたら（ステップＳ２５１；Ｙｅｓ）取得画像の２Ｄ特徴点と、発見した類似画像の２Ｄ特徴点とをそれぞれ取得して（ステップＳ２５２）、これら二つの画像間で２Ｄ特徴点の対応を取得する（ステップＳ２５３）。

そして、ステップＳ２３９と同様の方法で自機の姿勢を推定する（ステップＳ２５４）。この時、推定判定部１４がステップＳ２４０と同様にして推定可能かを判定し（ステップＳ２５５）、推定不可能なら（ステップＳ２５５；Ｎｏ）ステップＳ２４８に戻り、推定可能なら（ステップＳ２５５；Ｙｅｓ）自機位置の推定ができるようになったということになるので、ステップＳ２３１に進む。

このように、自機位置推定スレッドの初期化終了後は、推定状態変数ＥＳに「位置推定可能」か「位置推定不可能」かのどちらかがセットされるので、図３に示すメインフローにおいて、ＥＳの値が「位置推定可能」であれば自律移動装置１００は所望の動作を行い、ＥＳの値が「位置推定不可能」であれば位置推定可能になるような位置を求めて移動処理制御を行うことができるようになる。

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

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

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

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

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

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

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

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

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

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

制御部１０は、フレームＤＢを参照し、現キーフレーム（キーフレーム番号がＬＫＦのキーフレーム）と「キーフレーム自体の特徴」の類似度が基準画像類似度以上になるキーフレームをフレームＤＢから検索する（ステップＳ４０５）。

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

次に自律移動装置１００のメインフロー（図３）のステップＳ１１０の処理である移動処理制御について、図８を用いて説明する。まず、制御部１０は、位置履歴検索カウンタＳＣの値が０かどうかを判定する（ステップＳ５０１）。ＳＣが０なら、位置履歴記憶部２４に記憶されている位置全てに対して戻る処理を試みたが位置推定可能状態にならなかったことを意味しており、この場合は（ステップＳ５０１；Ｙｅｓ）位置の推定ができていない状態で自律動作を行わせるためにメインフロー（図３）のステップＳ１０８、Ｓ１０９と同様の動作を行い（ステップＳ５０２、Ｓ５０３）、メインフロー（図３）のステップＳ１０５に戻る。

ＳＣが０でないなら（ステップＳ５０１；Ｎｏ）、位置履歴記憶部２４に記憶されているＳＣ番目の情報（自機の姿勢（位置）の情報及びその時の時刻）を取得する（ステップＳ５０４）。取得した情報のうち、自機の姿勢（位置）の情報はＨＰ（位置履歴記憶部２４から取得した自機の位置の情報を格納する変数）に、時刻はＨＴ（位置履歴記憶部２４から取得した時刻を格納する変数）に、それぞれ格納する。そして、位置変化履歴カウンタＤＣの値が０かどうかを判定する（ステップＳ５０５）。ＤＣが０なら位置変化履歴記憶部２３に記憶されている位置の変化を全て試みたが位置推定可能状態にならなかったことを意味しており、この場合は（ステップＳ５０５；Ｙｅｓ）位置の推定ができていない状態で自律動作を行わせるためにメインフロー（図３）のステップＳ１０８、Ｓ１０９と同様の動作を行い（ステップＳ５０２、Ｓ５０３）、メインフロー（図３）のステップＳ１０５に戻る。

ＤＣが０でないなら（ステップＳ５０５；Ｎｏ）、位置変化履歴記憶部２３に記憶されているＤＣ番目の情報（位置変化量及びその時の時刻）を取得する（ステップＳ５０６）。取得した情報のうち、位置変化量はＤＰ（位置変化履歴記憶部２３から取得した位置変化量を格納する変数）に、時刻はＤＴ（位置変化履歴記憶部２３から取得した時刻を格納する変数）に、それぞれ格納する。そして、位置履歴記憶部２４から取得した時刻ＨＴの方が位置変化履歴記憶部２３から取得した時刻ＤＴよりも過去の時刻か（小さい値か）を判定する（ステップＳ５０７）。

ＨＴの方がＤＴよりも過去なのであれば（ステップＳ５０７；Ｙｅｓ）、まだ位置履歴記憶部２４から取得した過去のＨＰの位置まで自機は戻りきっていないので、ステップＳ５０８に進む。ＨＴがＤＴと同時刻又はＤＴの方が過去なのであれば（ステップＳ５０７；Ｎｏ）移動処理制御での最後の移動処理を行うためにステップＳ５１０に進む。

ステップＳ５０８では、位置変化履歴記憶部２３に登録されていた位置変化量ＤＰに基づき、その変化量を打ち消す移動処理を移動処理制御部１５が移動処理部１１に行わせる（ステップＳ５０８）。例えばＤＰが「前方に１０ｃｍ進んだ」であれば、ここでの移動処理は「後方に１０ｃｍ戻る」処理となる。そして、位置変化履歴カウンタＤＣをデクリメントして（ステップＳ５０９）ステップＳ５０５に戻る。

ステップＳ５１０もステップＳ５０８と同様に、位置変化履歴記憶部２３に登録されていた位置変化量ＤＰに基づき、その変化量を打ち消す移動処理を移動処理制御部１５が移動処理部１１に行わせる（ステップＳ５１０）。この移動処理により、（もし位置変化履歴記憶部２３に記憶されている情報や移動処理部１１の移動距離に誤差がなければ）自律移動装置１００は過去に位置推定ができていた位置（ＨＰで示される位置）に戻ることになるはずなので、自機位置推定スレッドでの自機位置推定が正しく行われるように、この最後の移動処理は速度を落として行うのが望ましい。特にステップＳ５０７での判定の際にＨＴとＤＴの値が等しかったのであれば、この移動によってＨＰで示される位置に戻るはずなので、ステップＳ５１０とステップＳ５１１との間に（自機位置推定スレッドでの自機位置推定が行われるのを待つために）所定の時間移動せずに停止する処理を追加しても良い。

次に、位置変化履歴カウンタＤＣをデクリメントし（ステップＳ５１１）、位置履歴検索カウンタＳＣをデクリメントして（ステップＳ５１２）、メインフローのステップＳ１０５に戻る。位置履歴検索カウンタＳＣをデクリメントするのは、今回の移動処理制御によっても自機位置推定スレッドで自機位置推定が可能にならなかった場合は、次回の移動処理制御では、さらに過去の位置に戻って自機位置推定を試みるためである。ただし、次回までの間に自機位置推定に成功すれば、図５のステップＳ２４１で位置履歴検索カウンタＳＣには位置履歴登録カウンタＨＣの値がセットされる。このような移動処理制御を行うことで、過去に自機位置推定ができていた位置に戻って、自機位置推定を可能にすることができる。

次に、上記とは異なる移動処理制御として、図９のフローチャートに示される移動処理制御を行う第二の実施形態について説明する。第二の実施形態は、移動処理制御のフローチャート以外は第一の実施形態と同じである。

まず、制御部１０は、位置履歴検索カウンタＳＣの値が０かどうかを判定する（ステップＳ６０１）。ＳＣが０なら、位置履歴記憶部２４に記憶されている位置全てに対して戻る処理を試みたが位置推定可能状態にならなかったことを意味しており、この場合は（ステップＳ６０１；Ｙｅｓ）位置の推定ができていない状態で自律動作を行わせるためにメインフロー（図３）のステップＳ１０８、Ｓ１０９と同様の動作を行い（ステップＳ６０２、Ｓ６０３）、メインフロー（図３）のステップＳ１０５に戻る。

ＳＣが０でないなら（ステップＳ６０１；Ｎｏ）、位置履歴記憶部２４に記憶されているＳＣ番目の情報（自機の姿勢（並進ベクトルｔ及び回転行列Ｒ）の情報ＨＰ及びその時の時刻ＨＴ）を取得する（ステップＳ６０４）。そして、位置変化履歴カウンタＤＣの値を位置変化履歴ワークカウンタＷＣ（位置変化履歴記憶部２３から位置変化の履歴を取得する際に用いる変数）にセットし、位置変化履歴スタック（位置変化量を格納するスタック）を初期化する（ステップＳ６０５）。なお、スタックは、先入れ後出しのデータ構造になっている。

そして、制御部１０は、位置変化履歴ワークカウンタＷＣが０かどうかを判定する（ステップＳ６０６）。ＷＣが０なら位置変化履歴記憶部２３に記憶されている位置の変化を全て試みたが位置推定可能状態にならなかったことを意味しており、この場合は（ステップＳ６０６；Ｙｅｓ）位置の推定ができていない状態で自律動作を行わせるためにメインフロー（図３）のステップＳ１０８、Ｓ１０９と同様の動作を行い（ステップＳ６０２、Ｓ６０３）、メインフロー（図３）のステップＳ１０５に戻る。

ＷＣが０でないなら（ステップＳ６０６；Ｎｏ）、制御部１０は位置変化履歴記憶部２３に記憶されているＷＣ番目の情報（位置変化量ＤＰ及びその時の時刻ＤＴ）を取得する（ステップＳ６０７）。そして、位置履歴記憶部２４から取得した時刻ＨＴの方が位置変化履歴記憶部２３から取得した時刻ＤＴよりも過去の時刻か（小さい値か）を判定する（ステップＳ６０８）。

ＨＴの方がＤＴよりも過去なのであれば（ステップＳ６０８；Ｙｅｓ）、まだ位置履歴記憶部２４から取得した過去のＨＰの位置まで自機は戻りきっていないので、ステップＳ６０９に進む。ＨＴがＤＴと同時刻又はＤＴの方が過去なのであれば（ステップＳ６０８；Ｎｏ）自機の位置を位置変化履歴から積算推定するためにステップＳ６１１に進む。

ステップＳ６０９では、制御部１０は、位置変化履歴スタックに位置変化量ＤＰの値をプッシュする。そして、位置変化履歴ワークカウンタＷＣをデクリメントして（ステップＳ６１０）ステップＳ６０６に戻る。これにより、位置変化履歴スタックにこれまでの位置変化が積まれていく。

ステップＳ６１１は、制御部１０は、位置履歴記憶部２４から取得した過去のＨＰの姿勢（並進ベクトルｔ及び回転行列Ｒ）を初期値としてＭＰ（自機位置の推定値を求めるために用いる変数）にセットし、位置変化履歴スタックからデータ（ＤＰ）を取り出しては、ＭＰに加算していく処理を、位置変化履歴スタックが空になるまで行う。これにより、ＭＰには過去の自機位置推定ができる状態だったときの位置ＨＰから、位置変化履歴ＤＰを積算することによって得られた現在の自機位置の推定値が得られる。この処理において、制御部１０は積算位置推定部に相当する。

次に、現在の自機位置の推定値ＭＰから、過去の自機位置ＨＰまで移動するように、移動処理制御部１５は、移動処理部１１を制御する（ステップＳ６１２）。具体的には、ＨＰの並進ベクトルからＭＰの並進ベクトルを減算した分だけ並進移動を行い、ＨＰの回転行列にＭＰの回転行列の逆行列を乗算して得られた回転行列で表される回転分だけ回転動作を行うように制御する。この移動処理制御により、（もし位置変化履歴記憶部２３に記憶されている情報や移動処理部１１の移動距離に誤差がなければ）自律移動装置１００は過去に位置推定ができていた位置（ＨＰで示される位置）に戻ることになるはずなので、ステップＳ６１２とステップＳ６１３との間に（自機位置推定スレッドでの自機位置推定が行われるのを待つために）所定の時間移動せずに停止する処理を追加しても良い。

次に、位置履歴検索カウンタＳＣをデクリメントして（ステップＳ６１３）、メインフローのステップＳ１０５に戻る。位置履歴検索カウンタＳＣをデクリメントするのは、今回の移動処理制御によっても自機位置推定スレッドで自機位置推定が可能にならなかった場合は、次回の移動処理制御では、さらに過去の位置に戻って自機位置推定を試みるためである。ただし、次回までの間に自機位置推定に成功すれば、図５のステップＳ２４１で位置履歴検索カウンタＳＣには位置履歴登録カウンタＨＣの値がセットされる。このような移動処理制御を行うことで、過去自機位置推定ができていた位置に戻って、自機位置推定を可能にすることができる。

第二の実施形態は、第一の実施形態と比較すると、位置変化履歴記憶部２３に記憶されている位置変化について一つ一つ移動処理するのではなく、まとめて移動処理するため、移動処理制御時の駆動部４２の誤差が蓄積しにくいという利点がある。

今まで説明した二つの実施形態は、いずれも過去に自機位置推定ができていた位置に戻る処理を行っていたが、これらは位置変化履歴記憶部２３に記憶されている情報や移動処理部１１による移動距離が正確でないと過去の正しい位置に戻れないという問題がある。

そこで、位置変化履歴記憶部２３や位置履歴記憶部２４を必要としない第三の実施形態について説明する。第三の実施形態の構成を図１０に示す、第一や第二の実施形態と比較すると、制御部１０の中に回転撮影部１６が存在する点と、記憶部２０の中に位置変化履歴記憶部２３や位置履歴記憶部２４は存在せず、特徴点記憶部２５が存在する点が異なる。

図３〜図７の各フローについては、位置変化履歴カウンタＤＣ、位置変化履歴記憶部２３、位置履歴登録カウンタＨＣ、位置履歴検索カウンタＳＣに関する処理を省略するだけで、第三の実施形態の処理フローと同じになる。移動処理制御の処理フローは第一や第二の実施形態と大きく異なるため、図１１を用いて以下に説明する。

まず、回転撮影部１６は、回転撮影フラグＲＦ（現在回転撮影の処理中かどうかを示す変数）に１をセットし（ステップＳ７０１）、累積回転角φ_Ｓを０クリアする（ステップＳ７０２）。次に、撮像部４１で画像を取得する（ステップＳ７０３）。次に、取得した画像内から２Ｄ特徴点を取得する（ステップＳ７０４）。そして、取得した２Ｄ特徴点の個数と累積回転角φ_Ｓとを特徴点記憶部２５に登録する（ステップＳ７０５）。

次に回転撮影部１６は、回転撮影フラグＲＦが２かどうかを判定する（ステップＳ７０６）。ＲＦが２なら（ステップＳ７０６；Ｙｅｓ）１回転分の撮影が完了したことを意味するので、ステップＳ７１２に進む。ＲＦが２でないなら（ステップＳ７０６；Ｎｏ）、累積回転角φ_Ｓ＋回転角φ_１が３６０°未満であるかを判定する（ステップＳ７０７）。ここで、回転角φ_１は回転撮影部１６による回転動作一回分の回転角であり、例えば３０°である。周囲の環境からできるだけ多くの特徴点を撮影することができるように、φ_１は撮像部４１の視野角の半分以下の角度を設定するのが望ましい。

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

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

ステップＳ７１２では、移動処理制御部１５は特徴点記憶部２５に登録された２Ｄ特徴点の個数が最も多い時の累積回転角φ_Ｓを特徴点記憶部２５から検索し、回転角φ_Ｓの回転動作をするように移動処理部１１を制御する（ステップＳ７１３）。その後、移動処理制御部１５は所定の距離（例えば基準並進距離の３倍）だけゆっくりと前方へ移動するように移動処理部１１を制御する（ステップＳ７１４）。ゆっくりと移動することにより、自機位置推定スレッドで自機位置が推定できるタイミングを増やすことができる。

その後、回転撮影部１６は回転撮影フラグＲＦを０クリアし（ステップＳ７１５）、メインフローのステップＳ１０５に戻る。このような動作を行うことで、２Ｄ特徴点の個数の多い方向に移動を行うので、自機位置の推定が成功する可能性が高まることになる。なお、この実施形態では２Ｄ特徴点の個数のみに着目したが、個数だけでなく質にも着目して、２Ｄ特徴点の個数と質とで評価関数を定義し、ステップＳ７１２ではこの２Ｄ特徴点の評価関数が最も高くなる回転角φ_Ｓを検索する処理を行っても良い。

また、第一、第二、第三の各実施形態の移動処理制御を適宜切り替えて行っても良い。例えば、最初に第一の実施形態の移動処理制御を行いそれでも自機位置推定が可能にならなければ第三の実施形態の移動処理制御を行う実施形態、最初に第二の実施形態の移動処理制御を行いそれでも自機位置推定が可能にならなければ第三の実施形態の移動処理制御を行う実施形態、最初に第一の実施形態の移動処理制御を行いそれでも自機位置推定が可能にならなければ第二の実施形態の移動処理制御を行いそれでも自機位置推定が可能にならなければ第三の実施形態の移動処理制御を行う実施形態等が考えられる。

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

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

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

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

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

（付記１）
撮像部が撮影した複数の画像の情報に基づいて自律的に移動するように処理する移動処理部と、
前記撮像部が撮影した複数の画像の情報を用いて自機位置を推定する位置推定部と、
前記位置推定部における自機位置の推定結果が一つに定まるか否かを判定する推定判定部と、
前記推定判定部が、前記推定結果が一つに定まらないと判定した場合には、前記位置推定部における自機位置の推定結果が一つに定まる位置に移動するように前記移動処理部を制御する移動処理制御部と、
を備える自律移動装置。

（付記２）
前記自律移動装置は、さらに、複数の画像から対応する特徴点を抽出する対応特徴点取得部を備え、
前記推定判定部は、前記位置推定部が用いる複数の画像から前記対応特徴点取得部が抽出した対応する特徴点の数が所定の数未満の場合に、前記推定結果が一つに定まらないと判定する、
付記１に記載の自律移動装置。

（付記３）
前記自律移動装置は、さらに、複数の画像から対応する特徴点を抽出する対応特徴点取得部を備え、
前記位置推定部は、前記対応特徴点取得部が抽出した特徴点の前記画像中の座標に基づいて定義したコスト関数を最小化することによって自機位置を推定し、
前記推定判定部は、前記コスト関数の最小値が所定の値よりも大きい場合に、前記推定結果が一つに定まらないと判定する、
付記１に記載の自律移動装置。

（付記４）
前記自律移動装置は、さらに、複数の画像から対応する特徴点を抽出する対応特徴点取得部を備え、
前記位置推定部は、前記対応特徴点取得部が抽出した特徴点の前記画像中の座標に基づいて定義したコスト関数を最小化することによって自機位置を推定し、
前記推定判定部は、前記コスト関数が複数の極小値を持ち、最小の極小値を２番目に小さい極小値で割った値が所定の値よりも大きい場合に、前記推定結果が一つに定まらないと判定する、
付記１に記載の自律移動装置。

（付記５）
前記自律移動装置は、さらに、前記移動処理部により自機の位置を変化させるたびにその変化量を位置変化履歴記憶部に登録する位置変化履歴登録部と、
前記推定判定部が前記推定結果が一つに定まると判定した場合に、その時点での位置の情報を時刻情報と共に位置履歴記憶部に登録する位置履歴登録部と、
を備え、
前記移動処理制御部は、前記推定判定部が前記推定結果が一つに定まらないと判定した場合には、前記位置履歴記憶部に登録されている位置に自機がいた時点まで遡りながら、前記位置変化履歴記憶部に登録されている位置の変化量とは逆方向に移動するように前記移動処理部を制御する、
付記１から４のいずれか一つに記載の自律移動装置。

（付記６）
前記自律移動装置は、さらに、前記移動処理部により自機の位置を変化させるたびにその変化量を位置変化履歴記憶部に登録する位置変化履歴登録部と、
前記推定判定部が前記推定結果が一つに定まると判定した場合に、その時点での位置の情報を時刻情報と共に位置履歴記憶部に登録する位置履歴登録部と、
前記推定判定部が前記推定結果が一つに定まらないと判定した場合に、前記位置履歴記憶部に登録されている位置に、該位置に対応する時刻から現在時刻まで前記位置変化履歴記憶部に登録されている位置の変化量を積算することで現在の自機の位置を推定する積算位置推定部と、
を備え、
前記移動処理制御部は、前記推定判定部が前記推定結果が一つに定まらないと判定した場合には、前記積算位置推定部が推定した位置から前記位置履歴記憶部に登録されている位置に向かって移動するように前記移動処理部を制御する、
付記１から４のいずれか一つに記載の自律移動装置。

（付記７）
前記自律移動装置は、さらに、回転動作を行いながら画像を撮影する回転撮影部を備え、
前記移動処理制御部は、前記推定判定部が前記推定結果が一つに定まらないと判定した場合には、前記回転撮影部が撮影した画像の中で、前記位置推定部が推定に用いる情報が最も多い前記画像を撮影した時の方向に向かって移動するように前記移動処理部を制御する、
付記１から４のいずれか一つに記載の自律移動装置。

（付記８）
前記回転撮影部が行う回転動作の一回分の回転角は、前記撮像部の視野角の半分以下の角度である、
付記７に記載の自律移動装置。

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

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

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

（付記１２）
撮像部が撮影した複数の画像の情報に基づいて自律的に移動する自律移動方法であって、
前記撮像部が撮影した複数の画像の情報に基づく自機位置の推定結果が一つに定まらない場合には、前記推定結果が一つに定まる位置を探して移動する、
自律移動方法。

（付記１３）
撮像部が撮影した複数の画像の情報に基づいて自律的に移動するように処理する移動処理ステップと、
前記撮像部が撮影した複数の画像の情報に基づいて自機位置を推定する位置推定ステップと、
前記位置推定ステップにおける自機位置の推定結果が一つに定まるか否かを判定する推定判定ステップと、
前記推定判定ステップで、前記推定結果が一つに定まらないと判定された場合には、前記位置推定ステップにおける自機位置の推定結果が一つに定まる位置に移動するように前記移動処理ステップでの処理を制御する移動処理制御ステップと、
を備える自律移動方法。

（付記１４）
コンピュータに、
撮像部が撮影した複数の画像の情報に基づいて自律的に移動するように処理する移動処理ステップ、
前記撮像部が撮影した複数の画像の情報に基づいて自機位置を推定する位置推定ステップ、
前記位置推定ステップにおける自機位置の推定結果が一つに定まるか否かを判定する推定判定ステップ、
前記推定判定ステップで、前記推定結果が一つに定まらないと判定された場合には、前記位置推定ステップにおける自機位置の推定結果が一つに定まる位置に移動するように前記移動処理ステップでの処理を制御する移動処理制御ステップ、
を実行させるためのプログラム。

１００…自律移動装置、１０…制御部、１１…移動処理部、１２…地図作成部、１３…位置推定部、１４…推定判定部、１５…移動処理制御部、１６…回転撮影部、２０…記憶部、２１…画像記憶部、２２…地図記憶部、２３…位置変化履歴記憶部、２４…位置履歴記憶部、２５…特徴点記憶部、３０…センサ部、３１…加速度センサ、３２…角速度センサ、３３…障害物センサ、４１…撮像部、４２…駆動部、４３…入力部、４４…通信部、４５…電源

Claims (14)

1. 撮像部が撮影した複数の画像の情報に基づいて自律的に移動するように処理する移動処理部と、
   前記撮像部が撮影した複数の画像の情報を用いて自機位置を推定する位置推定部と、
   前記位置推定部における自機位置の推定結果が一つに定まるか否かを判定する推定判定部と、
   前記推定判定部が、前記推定結果が一つに定まらないと判定した場合には、前記位置推定部における自機位置の推定結果が一つに定まる位置に移動するように前記移動処理部を制御する移動処理制御部と、
   前記移動処理部により自機の位置を変化させるたびにその変化量を位置変化履歴記憶部に登録する位置変化履歴登録部と、
   前記推定判定部が前記推定結果が一つに定まると判定した場合に、その時点での位置の情報を時刻情報と共に位置履歴記憶部に登録する位置履歴登録部と、
   を備え、
   前記移動処理制御部は、前記推定判定部が前記推定結果が一つに定まらないと判定した場合には、前記位置履歴記憶部に登録されている位置に自機がいた時点まで遡りながら、前記位置変化履歴記憶部に登録されている位置の変化量とは逆方向に移動するように前記移動処理部を制御する、
   自律移動装置。
2. 撮像部が撮影した複数の画像の情報に基づいて自律的に移動するように処理する移動処理部と、
   前記撮像部が撮影した複数の画像の情報を用いて自機位置を推定する位置推定部と、
   前記位置推定部における自機位置の推定結果が一つに定まるか否かを判定する推定判定部と、
   前記推定判定部が、前記推定結果が一つに定まらないと判定した場合には、前記位置推定部における自機位置の推定結果が一つに定まる位置に移動するように前記移動処理部を制御する移動処理制御部と、
   前記移動処理部により自機の位置を変化させるたびにその変化量を位置変化履歴記憶部に登録する位置変化履歴登録部と、
   前記推定判定部が前記推定結果が一つに定まると判定した場合に、その時点での位置の情報を時刻情報と共に位置履歴記憶部に登録する位置履歴登録部と、
   前記推定判定部が前記推定結果が一つに定まらないと判定した場合に、前記位置履歴記憶部に登録されている位置に、該位置に対応する時刻から現在時刻まで前記位置変化履歴記憶部に登録されている位置の変化量を積算することで現在の自機の位置を推定する積算位置推定部と、
   を備え、
   前記移動処理制御部は、前記推定判定部が前記推定結果が一つに定まらないと判定した場合には、前記積算位置推定部が推定した位置から前記位置履歴記憶部に登録されている位置に向かって移動するように前記移動処理部を制御する、
   自律移動装置。
3. 前記自律移動装置は、さらに、複数の画像から対応する特徴点を抽出する対応特徴点取得部を備え、
   前記推定判定部は、前記位置推定部が用いる複数の画像から前記対応特徴点取得部が抽出した対応する特徴点の数が所定の数未満の場合に、前記推定結果が一つに定まらないと判定する、
   請求項１または２に記載の自律移動装置。
4. 前記自律移動装置は、さらに、複数の画像から対応する特徴点を抽出する対応特徴点取得部を備え、
   前記位置推定部は、前記対応特徴点取得部が抽出した特徴点の前記画像中の座標に基づいて定義したコスト関数を最小化することによって自機位置を推定し、
   前記推定判定部は、前記コスト関数の最小値が所定の値よりも大きい場合に、前記推定結果が一つに定まらないと判定する、
   請求項１または２に記載の自律移動装置。
5. 前記自律移動装置は、さらに、複数の画像から対応する特徴点を抽出する対応特徴点取得部を備え、
   前記位置推定部は、前記対応特徴点取得部が抽出した特徴点の前記画像中の座標に基づいて定義したコスト関数を最小化することによって自機位置を推定し、
   前記推定判定部は、前記コスト関数が複数の極小値を持ち、最小の極小値を２番目に小さい極小値で割った値が所定の値よりも大きい場合に、前記推定結果が一つに定まらないと判定する、
   請求項１または２に記載の自律移動装置。
6. 初期値からの累積回転角をφ_Ｓ、一回分の回転角をφ_１として回転動作を行いながら画像を撮影する回転撮影部と、
   前記回転撮影部が撮影した画像から特徴点を取得して、該特徴点の数と前記累積回転角（φ_Ｓ）とを合わせて記憶する特徴点記憶部と、
   を備え、
   前記移動処理制御部は、前記推定判定部が、前記推定結果が一つに定まらないと判定した場合には、前記特徴点記憶部に記憶された特徴点の数が最も多い時の前記累積回転角（φ_Ｓ）を検索し、前記回転撮影部に、前記検索した累積回転角（φ_Ｓ）に基づいた回転動作をさせる、
   請求項１または２に記載の自律移動装置。
7. 前記移動処理制御部は、前記回転撮影部に、前記累積回転角φ_Ｓ＋前記一回分の回転角φ_１が３６０°未満である場合は、前記一回分の回転角φ_１の回転動作をさせ、前記累積回転角φ_Ｓ＋前記一回分の回転角φ_１が３６０°以上である場合は、回転角（３６０°−φ_Ｓ）の回転動作をさせる、請求項６に記載の自律移動装置。
8. 前記回転撮影部が行う回転動作の一回分の回転角は、前記撮像部の視野角の半分以下の角度である、
   請求項６に記載の自律移動装置。
9. 前記自律移動装置は、さらに、基準並進距離を移動する度に、前記撮像部が撮影した画像の情報を記憶する画像記憶部を備え、
   前記位置推定部は、前記画像記憶部に記憶されている複数の画像の情報を用いて自機位置を推定する、
   請求項１から８のいずれか一項に記載の自律移動装置。
10. 駆動部の車輪の直径に所定の値を乗算した値を前記基準並進距離に設定する、
    請求項９に記載の自律移動装置。
11. 前記自律移動装置は、さらに、複数の画像から位置を推定可能な特徴点を抽出する位置推定可能特徴点抽出部を備え、
    前記位置推定可能特徴点抽出部が抽出した全ての特徴点と自機位置との距離の平均値を所定の値で除算した値を前記基準並進距離に設定する、
    請求項９に記載の自律移動装置。
12. 撮影した複数の画像の情報に基づいて自律的に移動するように処理する移動処理ステップと、
    前記撮影した複数の画像の情報に基づいて自機位置を推定する位置推定ステップと、
    前記位置推定ステップにおける自機位置の推定結果が一つに定まるか否かを判定する推定判定ステップと、
    前記推定判定ステップで、前記推定結果が一つに定まらないと判定された場合には、前記位置推定ステップにおける自機位置の推定結果が一つに定まる位置に移動するように前記移動処理ステップでの処理を制御する移動処理制御ステップと、
    前記移動処理ステップで自機の位置を変化させるたびにその変化量を位置変化履歴記憶部に登録する位置変化履歴登録ステップと、
    前記推定判定ステップで前記推定結果が一つに定まると判定された場合に、その時点での位置の情報を時刻情報と共に位置履歴記憶部に登録する位置履歴登録ステップと、
    を備え、
    前記移動処理制御ステップでは、前記推定判定ステップで前記推定結果が一つに定まらないと判定された場合には、前記位置履歴記憶部に登録されている位置に自機がいた時点まで遡りながら、前記位置変化履歴記憶部に登録されている位置の変化量とは逆方向に移動するように前記移動処理ステップでの処理を制御する、
    自律移動方法。
13. 撮影した複数の画像の情報に基づいて自律的に移動するように処理する移動処理ステップと、
    前記撮影した複数の画像の情報に基づいて自機位置を推定する位置推定ステップと、
    前記位置推定ステップにおける自機位置の推定結果が一つに定まるか否かを判定する推定判定ステップと、
    前記推定判定ステップで、前記推定結果が一つに定まらないと判定された場合には、前記位置推定ステップにおける自機位置の推定結果が一つに定まる位置に移動するように前記移動処理ステップでの処理を制御する移動処理制御ステップと、
    前記移動処理ステップで自機の位置を変化させるたびにその変化量を位置変化履歴記憶部に登録する位置変化履歴登録ステップと、
    前記推定判定ステップで前記推定結果が一つに定まると判定された場合に、その時点での位置の情報を時刻情報と共に位置履歴記憶部に登録する位置履歴登録ステップと、
    前記推定判定ステップで前記推定結果が一つに定まらないと判定された場合に、前記位置履歴記憶部に登録されている位置に、該位置に対応する時刻から現在時刻まで前記位置変化履歴記憶部に登録されている位置の変化量を積算することで現在の自機の位置を推定する積算位置推定ステップと、
    を備え、
    前記移動処理制御ステップでは、前記推定判定ステップで前記推定結果が一つに定まらないと判定された場合には、前記積算位置推定ステップで推定された位置から前記位置変化履歴記憶部に登録されている位置に向かって移動するように前記移動処理ステップでの処理を制御する、
    自律移動方法。
14. コンピュータに、請求項１２または１３に記載の自律移動方法を実行させるためのプログラム。

Images