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

Description

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

周囲の環境の地図を作成して、作成した地図に基づいて自律的に移動する自律移動装置（自律移動ロボット）が知られている。また、このような自律移動ロボットに、画像をパターン認識する処理を組み込むことも知られている。これらの装置において、地図の作成処理及び画像の認識処理は、どちらも処理量が非常に多いため、処理バランスの改善を図る技術開発が進められている。例えば、特許文献１には、地図の作成状況と人の検出状況とに基づいて、地図作成部の処理量と顔認識部の処理量を調整する自律移動装置が記載されている。

特開２０１８－５５１６７号公報

特許文献１に記載の自律移動装置では、地図の作成処理及び顔の認識処理は、どちらも処理量が非常に多いため、作成済みの地図の大きさや、周囲に人が存在するか否かによって、地図作成部の処理量と顔認識部の処理量とを調整して、処理バランスの改善を図っている。その際、該自律移動装置は、処理負荷を軽減するために、地図の作成頻度を減らしたり、地図の作成を停止したりする場合があるが、そのようなことをすると現在位置をロスト（追跡失敗）する可能性が増大してしまう。しかし、該自律移動装置では、現在位置をロストする可能性を低減するための処理は用意されていない。したがって、従来の自律移動装置では、現在位置をロストする可能性を低減するための対応に改善の余地がある。

本発明は、上記実情に鑑みてなされたものであり、処理負荷を低減しつつロストする可能性を低減することができる自律移動装置、自律移動方法及びプログラムを提供することを目的とする。

上記目的を達成するため、本発明の自律移動装置は、
画像を用いて地図の作成及び自己位置の推定を行って移動する自律移動装置であって、
画像を逐次撮影する撮像部と、
制御部と、
を備え、
前記制御部は、
前記自律移動装置が移動する速度を取得し、
前記逐次撮影された画像から所定の間引き数の間引き画像を間引いた画像の情報を記憶部に保存し、
前記撮像部で撮影された画像に含まれる特徴点を抽出し、
前記記憶部に保存された画像の情報に含まれる特徴点と、前記撮影された画像から抽出した特徴点と、の対応を取得して自己位置を推定し、
前記推定の際に取得した前記対応する特徴点の数である対応特徴点数に応じて前記間引き数を調整する際、
前記速度が第１速度閾値より小さければ前記間引き数を増加させ、
前記速度が第２速度閾値より大きければ前記間引き数を減少させる。
また、上記目的を達成するため、本発明の自律移動装置は、
画像を用いて地図の作成及び自己位置の推定を行って移動する自律移動装置であって、
画像を逐次撮影する撮像部と、
制御部と、
を備え、
前記制御部は、
前記自律移動装置が移動する速度を取得し、
前記逐次撮影された画像から所定の間引き数の間引き画像を間引いた画像の情報を記憶部に保存し、
前記撮像部で撮影された画像に含まれる特徴点を抽出し、
前記記憶部に保存された画像の情報に含まれる特徴点と、前記撮影された画像から抽出した特徴点と、の対応を取得して自己位置を推定し、
前記推定の際に取得した前記対応する特徴点の数である対応特徴点数に応じて前記間引き数を調整する際、
予め前記記憶部に記憶された前記速度の範囲に対応する間引き数が設定された速度間引き数テーブルを参照し、前記間引き数を前記速度に対応する間引き数に設定する。
また、上記目的を達成するため、本発明の自律移動装置は、
画像を用いて地図の作成及び自己位置の推定を行って移動する自律移動装置であって、
画像を逐次撮影する撮像部と、
制御部と、
を備え、
前記制御部は、
前記逐次撮影された画像から所定の間引き数の間引き画像の情報と、前記逐次撮影された画像から前記間引き画像を間引いた画像の情報と、を記憶部に保存し、
前記撮像部で撮影された画像に含まれる特徴点を抽出し、
前記記憶部に保存された画像の情報に含まれる特徴点と、前記撮影された画像から抽出した特徴点と、の対応を取得して自己位置を推定し、
前記推定の際に取得した前記対応する特徴点の数である対応特徴点数に応じて前記間引き数を調整し、
前記自己位置の推定に失敗したら前記記憶部に保存された前記間引き画像の情報に含まれる特徴点の情報を用いて自己位置を推定する。

本発明によれば、状況に応じて間引き数を調整することにより、処理負荷を低減しつつロストする可能性を低減することができる。

実施形態１に係る自律移動装置の機能構成を示す図である。 実施形態１に係るＳＬＡＭ処理のフローチャートである。 実施形態１に係るトラッキングスレッドのフローチャートの前半部分である。 実施形態１に係るトラッキングスレッドのフローチャートの後半部分である。 実施形態１に係るマッピングスレッドのフローチャートである。 実施形態１に係るループクロージングスレッドのフローチャートである。 位置（姿勢）推定に成功した場合に間引き数が変化する様子を説明する図である。 位置（姿勢）推定に失敗した場合に間引きデータキューに保存されているデータが使われる様子を説明する図である。 変形例１に係る間引き数テーブルの一例を示す図である。 変形例２に係るトラッキングスレッドのフローチャートの後半部分である。 変形例３に係る速度間引き数テーブルの一例を示す図である。

以下、本発明の実施形態に係る自律移動装置について、図表を参照して説明する。なお、図中同一又は相当部分には同一符号を付す。

（実施形態１）
本発明の実施形態１に係る自律移動装置は、周囲の地図を作成しながら、用途に応じて自律的に移動する装置である。この用途とは、例えば、警備監視用、屋内掃除用、ペット用、玩具用等である。

図１に示すように、本発明の実施形態１に係る自律移動装置１００は、制御部１０、記憶部２０、撮像部３１、駆動部３２、通信部３３、を備える。

制御部１０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）等で構成され、記憶部２０に記憶されたプログラムを実行することにより、後述する各部（ＳＬＡＭ処理部１１、間引き数調整部１２、間引きデータ破棄部１３、移動制御部１４）の機能を実現する。また、制御部１０は、タイマー（図示せず）を備え、経過時間をカウントすることができる。

記憶部２０は、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）等で構成され、機能的に、フレームＤＢ（ＤａｔａＢａｓｅ）２１、マップ点ＤＢ（ＤａｔａＢａｓｅ）２２及び間引きデータキュー２３を含む。ＲＯＭには制御部１０のＣＰＵが実行するプログラム及びプログラムを実行する上で予め必要なデータが記憶されている。ＲＡＭには、プログラム実行中に作成されたり変更されたりするデータが記憶される。

フレームＤＢ２１には、撮像部３１により撮影された画像（フレーム）のうち、間引きの対象にならずに残った画像（間引かれた画像）が記憶される。ただし、記憶容量を節約するために、記憶対象となる画像（間引かれた画像）の一部のみ（例えば後述するキーフレームのみ）を記憶するようにしてもよいし、画像自体ではなく画像に含まれる特徴点の情報を記憶するようにしてもよい。なお、特徴点とは画像中の、エッジ部分やコーナー部分等、画像内の特徴的な部分の点のことを言う。特徴点は、ＳＩＦＴ（Ｓｃａｌｅ－Ｉｎｖａｒｉａｎｔ Ｆｅａｔｕｒｅ Ｔｒａｎｓｆｏｒｍ）やＳＵＲＦ（Ｓｐｅｅｄｅｄ Ｕｐ Ｒｏｂｕｓｔ Ｆｅａｔｕｒｅｓ）等のアルゴリズムを用いて取得することができる。

自律移動装置１００は、フレームＤＢ２１に記憶された複数の画像（に含まれる特徴点の情報）を用いて、ＳＬＡＭ処理により、ＳＬＡＭ処理用のデータ（後述するマップ点のデータ）の生成及び自己位置の推定を行う。自己位置の推定に用いた画像のうち重要なものはキーフレームと呼ばれ、フレームＤＢ２１には、キーフレームの画像の情報（例えばその画像に含まれる特徴点の情報）が、そのキーフレームのキーフレーム番号（キーフレームが登録される度に１ずつ増えていく番号）、そのキーフレームを撮影した時の自律移動装置１００の姿勢（これをキーフレームの姿勢ともいい、位置（並進ベクトルｔ）及び向き（回転行列Ｒ）の情報を持つ）の情報等と紐付けられて記憶される。

マップ点ＤＢ２２には、フレームＤＢ２１に記憶されたキーフレームに含まれている特徴点のうち、３次元位置（Ｘ，Ｙ，Ｚ）が求められた特徴点の実空間上（３次元空間上）の点（マップ点と言う）の情報が記憶される。マップ点ＤＢ２２には、マップ点の情報として、その３次元位置と、そのマップ点の特徴量（例えばＳＩＦＴ等で得た特徴量）と、が紐付けられて記憶される。

ＳＬＡＭ処理では、フレームＤＢ２１に記憶されている複数のキーフレーム間で、同一の特徴点の対応を取得し、取得した対応特徴点の３次元位置をマップ点ＤＢ２２から取得することによって、自己位置の推定を行う。

間引きデータキュー２３には、撮像部３１により撮影された画像のうち、間引き画像（間引きの対象となった画像）が記憶される。ただし、フレームＤＢ２１と同様、記憶容量を節約するために、画像自体ではなく画像に含まれる特徴点の情報を記憶するようにしてもよい。また、間引きデータキュー２３のサイズは可変であり、初期化時のサイズ０である。そして、このサイズは、間引き数調整部１２により、０以上、最大キューサイズ（例えば１０）以下の範囲で調整される。また、間引きデータキュー２３は、先入れ先出し（ＦＩＦＯ：Ｆｉｒｓｔ Ｉｎ Ｆｉｒｓｔ Ｏｕｔ）でデータの保存と取り出しができるようになっている。

なお、図示しないが、自律移動装置１００は、周囲に存在する物体（障害物）を検出する障害物センサを備えてもよい。障害物センサは、例えば、赤外線距離センサ、超音波センサ等、障害物までの距離を測定することができる距離センサである。また、独立した障害物センサを搭載せずに、撮像部３１を用いて障害物を検出するようにしてもよい。また、障害物センサとして、距離センサではなく、他の物体に衝突したことを検知するバンパーセンサを備えてもよい。そして、自律移動装置１００は、障害物センサからの情報に基づいて、障害物の位置を記録した環境地図を記憶部２０に記憶するようにしてもよい。

撮像部３１は、単眼の撮像装置（カメラ）を備える。撮像部３１は、例えば、３０ｆｐｓ（ｆｒａｍｅｓ ｐｅｒ ｓｅｃｏｎｄ）で画像（フレーム）を撮影して取得する。自律移動装置１００は、撮像部３１が逐次撮影して取得した画像に基づいて、ＳＬＡＭ処理により、自己位置と周囲環境とをリアルタイムに認識しながら、自律移動を行う。

駆動部３２は、独立２輪駆動の車輪とモータとを備え、制御部１０からの指示（制御）により自律移動装置１００を移動させる。自律移動装置１００は、２つの車輪の同一方向駆動により前後の平行移動（並進移動）を、２つの車輪の逆方向駆動によりその場での回転（向き変更）を、２つの車輪のそれぞれ速度を変えた駆動により旋回移動（並進＋回転（向き変更）移動）を、行うことができる。また、各々の車輪にはロータリエンコーダが備えられており、ロータリエンコーダで車輪の回転数を計測し、車輪の直径、車輪間の距離等の幾何学的関係を利用することによって並進移動量及び回転量を計算できる。

例えば、車輪の直径をＤ、回転数をＣとすると、その車輪の接地部分での並進移動量はπ・Ｄ・Ｃとなる。ここで、回転数Ｃは、駆動部に備えられたロータリエンコーダにより測定可能である。また、車輪の直径をＤ、車輪間の距離をＩ、右車輪の回転数をＣ_Ｒ、左車輪の回転数をＣ_Ｌとすると、向き変更の回転量は（右回転を正とすると）３６０°×Ｄ×（Ｃ_Ｌ－Ｃ_Ｒ）／（２×Ｉ）となる。この並進移動量及び回転量をそれぞれ逐次足し合わせていくことで、駆動部３２は、メカオドメトリとして機能し、自律移動装置１００の位置（移動開始時の位置及び向きを基準とした現在の位置及び向き）を計測することができる。駆動部に備えられたロータリエンコーダは、距離計測部として機能する。また、時間当たりの移動距離を求めることにより、自律移動装置１００の速度を取得することができる。

なお、車輪の代わりにクローラを備えるようにしても良いし、複数（例えば二本）の足を備えて足で歩行することによって移動を行うようにしても良い。これらの場合も、二つのクローラの動き、足の動き等に基づいて、車輪の場合と同様に自律移動装置１００の位置（移動開始時の位置及び向きを基準とした現在の位置及び向き）の計測や速度の取得が可能である。

通信部３３は、外部装置と通信するためのモジュールであり、外部装置と無線通信する場合にはアンテナを含む無線モジュールである。例えば、通信部３３は、Ｂｌｕｅｔｏｏｔｈ（登録商標）に基づく近距離無線通信を行うための無線モジュールである。通信部３３を用いることにより、自律移動装置１００は、外部とデータの受け渡し等を行うことができる。また、自律移動装置１００は、外部のサーバ（図示せず）と通信部３３で通信することによって、制御部１０の機能の一部を外部のサーバで実行させるようにしてもよいし、記憶部２０で記憶すべき情報の一部を外部のサーバに記憶させるようにしてもよい。

次に、自律移動装置１００の制御部１０の機能的構成について説明する。制御部１０は、ＳＬＡＭ処理部１１、間引き数調整部１２、間引きデータ破棄部１３、移動制御部１４、の機能を実現し、自律移動装置１００の移動制御等を行う。また、制御部１０は、マルチスレッド機能に対応しており、複数のスレッド（異なる処理の流れ）を並行して実行することができる。

ＳＬＡＭ処理部１１は、撮像部３１が撮影しフレームＤＢ２１に記憶された画像を複数用いて、それらの画像から得られる特徴点の情報に基づき、ＳＬＡＭ処理により自律移動装置１００の姿勢（位置及び向き）を推定する。このＳＬＡＭ処理を行う際には、ＳＬＡＭ処理部１１は、画像に含まれる特徴点を抽出し、フレームＤＢ２１に該特徴点の情報を保存するとともに、３次元位置を計算できた特徴点（マップ点）については、マップ点ＤＢ２２に該マップ点の情報を保存する。

間引き数調整部１２は、位置（姿勢）推定に用いる画像（フレームＤＢ２１に保存する画像）の間引き数を調整する。間引き数の初期値は０で、この場合は、ＳＬＡＭ処理部１１は、画像を間引きせずに毎回位置（姿勢）推定を行う（そして、キーフレームをフレームＤＢ２１に保存する）。間引き数は、間引きデータキューのサイズと等しく、間引き数調整部１２は、間引きデータキューのサイズをｎに設定することにより、間引き数をｎにする。ＳＬＡＭ処理部１１は、ある画像で位置（姿勢）を推定した後、次のｎ枚の画像（間引き画像）では位置（姿勢）推定せずに、これらのｎ枚の画像（間引き画像）のデータを間引きデータキュー２３に保存する。

間引きデータ破棄部１３は、間引きデータキュー２３に保存されている画像データ（間引き画像データ）を破棄し、間引きデータキュー２３をクリアする。

移動制御部１４は、上位アプリケーションプログラムからの指示にしたがって、駆動部３２を制御して、自律移動装置１００を移動させる。上位アプリケーションプログラムとは、自律移動装置１００の用途に応じて決まるソフトウェアであり、例えば屋内掃除用アプリケーションプログラムである。上位アプリケーションプログラムは、用途に応じて様々な処理を行うことが想定されるが、一般的には、自律移動装置１００が状況に応じて、地図を作成するために移動したり、掃除等の目的を達成するために移動したりするためのプログラムである。

以上、自律移動装置１００の機能構成について説明した。次に、自律移動装置１００のＳＬＡＭ処理について、図２を参照して説明する。自律移動装置１００は、電源オフ時は充電器（充電ステーション）に接続して充電されており、電源が投入されると、充電器に接続されていた位置をスタート地点としてＳＬＡＭ処理が開始される。なお、自律移動装置１００は電源が投入されると、このＳＬＡＭ処理以外に、用途に応じた上位アプリケーションプログラムが別途（別スレッドで）起動し、制御部１０は、この上位アプリケーションプログラムから移動等の指示を受ける。

まず、自律移動装置１００のＳＬＡＭ処理部１１は、記憶部２０に記憶されているＳＬＡＭ処理用の各種データ（フレームＤＢ２１、マップ点ＤＢ２２等）を初期化する（ステップＳ１０１）。

次に、ＳＬＡＭ処理部１１は、トラッキングスレッドを起動する（ステップＳ１０２）。トラッキングスレッドは、自律移動装置１００の現在位置を推定するスレッドである。

そして、ＳＬＡＭ処理部１１は、マッピングスレッドを起動する（ステップＳ１０３）。マッピングスレッドは、ＳＬＡＭ用の地図のデータ（具体的にはフレームＤＢ２１及びマップ点ＤＢ２２に格納される各種データ）を生成するスレッドである。

次に、ＳＬＡＭ処理部１１は、ループクロージングスレッドを起動する（ステップＳ１０４）。ループクロージングスレッドは、ループクロージング処理が可能か否かをチェックし続け、ループクロージング処理が可能な場合にはループクロージング処理を行うスレッドである。なお、ループクロージング処理とは、以前来たことのある同じ場所に戻ってきたことを認識した場合に、以前この同じ場所にいた時の姿勢の値と現在の姿勢の値とのずれを用いて、以前来た時から今までの移動の軌跡中のキーフレームや、関連するマップ点の３次元位置を修正することを言う。

これらのスレッド（トラッキングスレッド、マッピングスレッド、ループクロージングスレッド）が並行して動作することにより、ＳＬＡＭ処理部１１は、撮像部３１が撮影した画像から特徴点を抽出して、自己位置の推定と地図の作成を行う。これらの各スレッドの処理の詳細は後述する。

次に、ＳＬＡＭ処理部１１は、ユーザ、上位アプリケーションプログラム等から終了の指示があるか否かを判定する（ステップＳ１０５）。終了の指示がなければ（ステップＳ１０５；Ｎｏ）、ステップＳ１０５に戻って終了の指示を待つ。終了の指示があれば（ステップＳ１０５；Ｙｅｓ）、ＳＬＡＭ処理のための各スレッド（トラッキングスレッド、マッピングスレッド、ループクロージングスレッド）を停止させ（ステップＳ１０６）、ＳＬＡＭ処理を終了する。

以上がＳＬＡＭ処理の流れである。次に、ＳＬＡＭ処理で起動される各種スレッド（トラッキングスレッド、マッピングスレッド、ループクロージングスレッド）について、順に説明する。

まず、トラッキングスレッドについて図３及び図４を参照して説明する。このスレッドでは、ＳＬＡＭ処理部１１が、最初にトラッキング（位置推定）の初期化処理を行い、その後トラッキング処理（撮像部３１で取得した画像を用いてビジュアルオドメトリにより自律移動装置１００の姿勢（位置及び向き）を推定する処理）を行い続ける。そして、その際、トラッキング処理に支障が出ないと想定される場合に、撮像部３１で取得した画像を間引きする処理を行う。

まず、間引き数調整部１２は、間引きデータキュー２３のサイズを０に初期化する（ステップＳ２０１）。これにより、間引き数は０に初期化される。そして、ＳＬＡＭ処理部１１は、ＳＬＡＭ処理の状態を初期状態に初期化する（ステップＳ２０２）。なお、ＳＬＡＭ処理の状態には、初期状態、追跡状態、ロスト状態の３種類がある。初期状態は、トラッキング（位置推定）のための初期化がまだ完了していない状態である。追跡状態は、トラッキングが成功している状態である。ロスト状態は、トラッキングが失敗して、位置推定ができない状態である。

次に、ＳＬＡＭ処理部１１は、撮像部３１で画像（現在のフレーム）を取得（撮影）し、取得した画像内から特徴点を抽出する（ステップＳ２０３）。次に、ＳＬＡＭ処理部１１は、ＳＬＡＭ処理の状態が初期状態であるか否かを判定する（ステップＳ２０４）。初期状態なら（ステップＳ２０４；Ｙｅｓ）、ＳＬＡＭの初期化処理を行う（ステップＳ２１０）。ＳＬＡＭの初期化処理とは、具体的には、前回取得したフレームと今回取得したフレームとで対応が取れる特徴点（対応特徴点）を抽出し、自律移動装置１００の相対姿勢（前回取得したフレームを基準とした、今回の取得したフレームの相対的な姿勢（位置及び向き））を求めてフレームＤＢ２１に登録するとともに、この時抽出した対応特徴点の３次元位置を求めてマップ点ＤＢ２２に登録することを言う。

ＳＬＡＭの初期化処理において抽出した対応特徴点の数が５以上なら、Ｔｗｏ－ｖｉｅｗ Ｓｔｒｕｃｔｕｒｅ ｆｒｏｍ Ｍｏｔｉｏｎ法でこの２つのフレーム間の姿勢（それぞれのフレームを取得した位置の差分（並進ベクトルｔ）及び向きの差分（回転行列Ｒ））を推定することができる。すると、この推定した姿勢は、前回取得したフレームを基準とした、今回取得したフレームの相対的な姿勢ということができる。この相対的な姿勢を現在の自律移動装置１００の位置（姿勢）としてフレームＤＢ２１に登録し、対応特徴点の（現在の自律移動装置１００の位置（姿勢）を基準とした）３次元位置をマップ点ＤＢ２２に登録することになる。

そして、ＳＬＡＭ処理部１１は、初期化処理が成功したか否かを判定する（ステップＳ２１１）。例えば、前回取得したフレームと今回取得したフレームとで対応が取れる特徴点の数が５点未満だと相対姿勢を求められないので、初期化処理失敗となる。また、対応特徴点の数が５点以上あっても、エラー（本当は対応していないのに対応が取れると判断した場合等）が含まれていること等により、相対姿勢が求められない場合は、初期化処理失敗となる。初期化処理が失敗したら（ステップＳ２１１；Ｎｏ）、ステップＳ２０３に戻る。初期化処理が成功したら（ステップＳ２１１；Ｙｅｓ）、ＳＬＡＭ処理部１１は、ＳＬＡＭの状態を追跡状態に設定して（ステップＳ２１２）、ステップＳ２０３に戻る。

一方、ステップＳ２０４で、初期状態でないなら（ステップＳ２０４；Ｎｏ）、ＳＬＡＭ処理部１１は、ＳＬＡＭ処理の状態がロスト状態であるか否かを判定する（ステップＳ２０５）。ロスト状態なら（ステップＳ２０５；Ｙｅｓ）、ＳＬＡＭ処理部１１は、ロスト状態からの復帰処理を行う（ステップＳ２１３）。ロスト状態からの復帰処理とは、具体的には、フレームＤＢ２１に記憶されているフレームの中から、現在の（今回取得した）フレームと類似する（又は一致する）フレームをＢａｇ－ｏｆ－Ｆｅａｔｕｒｅｓ等により検索し、見つかったら、その見つかったフレームの情報を用いて現在位置（姿勢）を推定することを言う。

そして、ＳＬＡＭ処理部１１は、ロスト状態からの復帰処理が成功したか否かを判定する（ステップＳ２１４）。成功したなら（ステップＳ２１４；Ｙｅｓ）、ステップＳ２１２に進み、失敗したなら（ステップＳ２１４；Ｎｏ）、ステップＳ２０３に戻る。

一方、ステップＳ２０５でロスト状態でないなら（ステップＳ２０５；Ｎｏ）、ＳＬＡＭ処理部１１は、間引きデータキュー２３に空きがあるか否かを判定する（ステップＳ２０６）。間引きデータキュー２３に空きがあるなら（ステップＳ２０６；Ｙｅｓ）、間引きデータキュー２３に、ステップＳ２０３で抽出した現在のフレームの特徴点の情報を保存し（ステップＳ２０７）、ステップＳ２０３に戻る。

間引きデータキュー２３に空きがないなら（ステップＳ２０６；Ｎｏ）、ＳＬＡＭ処理部１１は、現在の（今回取得した）フレームの特徴点、前回の（前回取得した）フレームの特徴点及び近傍のマップ点に基づき、現在位置（姿勢）を推定する（ステップＳ２０８）。そして、位置（姿勢）の推定が成功したか否かを判定する（ステップＳ２０９）。

位置（姿勢）の推定が失敗したら（ステップＳ２０９；Ｎｏ）、ＳＬＡＭ処理部１１は、間引きデータキュー２３が空か否かを判定する（ステップＳ２１５）。間引きデータキュー２３が空なら（ステップＳ２１５；Ｙｅｓ）、間引き数調整部１２は、間引きデータキュー２３のサイズを０にする（ステップＳ２１６）。そして、ＳＬＡＭ処理部１１は、ＳＬＡＭの状態をロスト状態にして（ステップＳ２１７）、ステップＳ２０３に戻る。

間引きデータキュー２３が空でないなら（ステップＳ２１５；Ｎｏ）、ＳＬＡＭ処理部１１は、間引きデータキュー２３から特徴点の情報（間引き画像の特徴点の情報）を取り出す（ステップＳ２３０）。そして、ＳＬＡＭ処理部１１は、取り出した特徴点、前回のフレームの特徴点及び近傍のマップ点に基づき、現在位置（姿勢）を推定する（ステップＳ２３１）。そして、位置（姿勢）の推定が成功したか否かを判定する（ステップＳ２３２）。

位置（姿勢）の推定が失敗したなら（ステップＳ２３２；Ｎｏ）、ＳＬＡＭ処理部１１は、ロスト状態からの復帰処理を行う（ステップＳ２３５）。そして、ロスト状態からの復帰処理が成功したか否かを判定する（ステップＳ２３６）。成功したなら（ステップＳ２３６；Ｙｅｓ）、ステップＳ２３３に戻り、失敗したなら（ステップＳ２３６；Ｎｏ）、ステップＳ２１５に戻る。

一方、ステップＳ２３２で位置（姿勢）の推定に成功したと判定したなら（ステップＳ２３２；Ｙｅｓ）、ＳＬＡＭ処理部１１は、間引きデータキュー２３が空か否かを判定する（ステップＳ２３３）。空でなければ（ステップＳ２３３；Ｎｏ）、ステップＳ２３０に戻る。空なら（ステップＳ２３３；Ｙｅｓ）、間引き数調整部１２は、間引きデータキュー２３のサイズを０にし（ステップＳ２３４）、ステップＳ２２６に進む。

また、ステップＳ２０９で位置（姿勢）の推定に成功したと判定したなら（ステップＳ２０９；Ｙｅｓ）、間引きデータ破棄部１３は、間引きデータキュー２３をクリアする（ステップＳ２２１）。そして、ステップＳ２０８で位置（姿勢）を推定した際の対応特徴点数が、第１閾値（例えば３００）より大きいか否かを、間引き数調整部１２が判定する（ステップＳ２２２）。

該対応特徴点数が第１閾値より大きければ（ステップＳ２２２；Ｙｅｓ）、間引き数調整部１２は、間引きデータキュー２３のサイズを１増やし（ステップＳ２２３）、ステップＳ２２６に進む。間引きデータキュー２３のサイズが１増えることにより、間引き数も１増える。ただし、サイズを増やすと、間引きデータキュー２３の最大キューサイズを超える場合は、間引きデータキュー２３のサイズは最大キューサイズに設定される。

該対応特徴点数が第１閾値以下なら（ステップＳ２２２；Ｎｏ）、間引き数調整部１２は、該対応特徴点数が第２閾値（例えば２００）未満か否かを判定する（ステップＳ２２４）。該対応特徴点数が第２閾値以上なら（ステップＳ２２４；Ｎｏ）、ステップＳ２２６に進む。

該対応特徴点数が第２閾値未満なら（ステップＳ２２４；Ｙｅｓ）、間引き数調整部１２は、間引きデータキュー２３のサイズを１減らし（ステップＳ２２５）、ステップＳ２２６に進む。間引きデータキュー２３のサイズが１減ることにより、間引き数も１減る。ただし、サイズを減らすと、サイズが０未満になる場合は、間引きデータキュー２３のサイズは０に設定される。

ステップＳ２２６では、ＳＬＡＭ処理部１１は、ＳＬＡＭ処理の状態を追跡状態に設定する。そして、ＳＬＡＭ処理部１１は、今回のフレームをキーフレーム（マッピングスレッドやループクロージングスレッドでの処理対象となるフレーム）とするか否かを判定する（ステップＳ２２７）。今回のフレームをキーフレームとするか否かの判定方法は任意の方法を使用可能だが、例えば、前回のキーフレームの位置から現在位置が基準距離以上離れているなら、今回のフレームをキーフレームとし、基準距離未満ならキーフレームとしないといった判定方法が考えられる。

今回のフレームをキーフレームとしないなら（ステップＳ２２７；Ｎｏ）、ステップＳ２０３に戻る。今回のフレームをキーフレームとするなら（ステップＳ２２７；Ｙｅｓ）、ＳＬＡＭ処理部１１は、今回のフレーム（キーフレーム）の情報をフレームＤＢ２１に登録し（ステップＳ２２８）、マッピングスレッドに今回のキーフレームを通知する（ステップＳ２２９）。そして、ステップＳ２０３に戻る。

以上、トラッキングスレッドの処理内容について説明した。なお、第２閾値は、第１閾値以下の任意の値である。これらの閾値は任意の値だが、ＳＬＡＭ処理部１１が取得する画像の解像度や、特徴点の抽出方法等により、適切な値に設定するのが好ましい。例えば、これ以上の値ならＳＬＡＭ処理部１１がほぼ確実に（例えば９０％以上の確率で）自己位置の推定に成功すると考えられる対応特徴点数を第１閾値に設定し、これ以下の値だと自己位置の推定に成功する確率が低い（例えば４０％以下）と考えられる対応特徴点数を第２閾値に設定する。次に、マッピングスレッドについて、図５を参照して説明する。このスレッドでは、ＳＬＡＭ処理部１１が、トラッキングスレッドによってフレームＤＢ２１に登録されたキーフレームの中の対応特徴点の３次元位置を計算して、マップ点ＤＢ２２に登録する処理を行う。

まず、ＳＬＡＭ処理部１１は、トラッキングスレッドから通知されたキーフレーム（現キーフレーム）を取得する（ステップＳ３０１）。そして、ＳＬＡＭ処理部１１は、現キーフレームと前キーフレームとで対応が取れる特徴点（対応特徴点）を抽出する（ステップＳ３０２）。なお、前キーフレームは、フレームＤＢ２１を参照すれば、キーフレーム番号が現キーフレームより１つ小さいキーフレームとして取得できる。

フレームＤＢ２１にはそれぞれのキーフレームの姿勢（並進ベクトルｔと回転行列Ｒ）も登録されているので、トラッキングスレッドの初期化処理の時と同様の方法で対応特徴点の３次元位置を計算できる。ＳＬＡＭ処理部１１は、３次元位置が計算できた対応特徴点をマップ点としてマップ点ＤＢ２２に登録する（ステップＳ３０３）。

次に、ＳＬＡＭ処理部１１は、全キーフレームの位置（姿勢）と全マップ点の３次元位置に対して、バンドルアジャストメント処理を行って、精度向上を図る（ステップＳ３０４）。バンドルアジャストメント処理とは、カメラ姿勢（キーフレーム姿勢）とマップ点の３次元位置とを同時に推定する非線形最適化法であり、マップ点をキーフレーム上に投影させたときに発生する誤差が最小になるような最適化を行うものである。このバンドルアジャストメントの処理を行うことで、キーフレーム姿勢とマップ点の３次元位置の精度向上を図ることができる。

次に、ＳＬＡＭ処理部１１、複数のキーフレームの姿勢と、そのキーフレーム上のマップ点と、そのマップ点の３次元位置と、に基づいてマップ点の３次元位置の誤差を求め、誤差が基準誤差よりも大きなマップ点の情報をマップ点ＤＢ２２から削除する（ステップＳ３０５）。姿勢とマップ点とを固定させると、上述のバンドルアジャストメント処理等により該マップ点の３次元位置を推定できるので、推定した３次元位置と、マップ点ＤＢ２２に登録されている３次元位置とに基づいて、誤差を計算できる。なお、ステップＳ３０５は必ずしもステップＳ３０４と別個に行う必要はなく、ステップＳ３０４で行われるバンドルアジャストメント処理の中で、誤差の大きいマップ点について、そのマップ点の情報をマップ点ＤＢ２２から削除するようにしてもよい。

そして、ＳＬＡＭ処理部１１は、現キーフレームをループクロージングスレッドに通知し（ステップＳ３０６）、ステップＳ３０１に戻る。

以上、マッピングスレッドの処理内容について説明した。次に、ループクロージングスレッドについて、図６を参照して説明する。このスレッドでは、ＳＬＡＭ処理部１１が、ループクロージング処理が可能か否かをチェックし続け、ループクロージング処理が可能な場合にはループクロージング処理を行う。

まず、ＳＬＡＭ処理部１１は、マッピングスレッドから通知されたキーフレーム（現キーフレーム）を取得する（ステップＳ４０１）。

そして、ＳＬＡＭ処理部１１は、現キーフレームと「キーフレーム自体の特徴」の類似度が基準画像類似度以上になるキーフレームをフレームＤＢ２１から検索する（ステップＳ４０２）。ここで、この類似度としてどのような値を用いるかは任意だが、例えば、画像（キーフレーム）の特徴を特徴ベクトルで表している場合は、比較対象の２つの画像の特徴ベクトルのノルムを１に正規化したもの同士の内積を、その２つの画像の類似度とすることができる。また、２つの画像の特徴ベクトルの距離（各要素の差の二乗和の平方根）の逆数を、その２つの画像の類似度としてもよい。

そして、ＳＬＡＭ処理部１１は、「キーフレーム自体の特徴」の類似度が基準画像類似度以上になるキーフレームが発見されたか否かを判定する（ステップＳ４０３）。発見されなければ（ステップＳ４０３；Ｎｏ）、ステップＳ４０１に戻る。発見されたら（ステップＳ４０３；Ｙｅｓ）、発見されたキーフレームと現キーフレームとが同じ位置（姿勢）になるように、フレームＤＢ２１内のキーフレームの情報やマップ点ＤＢ２２内のマップ点の情報を修正し（ステップＳ４０４）、ステップＳ４０１に戻る。

ステップＳ４０４では、例えば、ＳＬＡＭ処理部１１は、現キーフレームの姿勢を、発見されたキーフレームの姿勢と同じ姿勢に修正する。そして、発見されたキーフレームの姿勢と現キーフレームの姿勢との差分を用いて、発見されたキーフレームから現キーフレームまでの軌跡中の各キーフレームの姿勢に線形的に補正を加える。さらにこれらの各キーフレームに含まれるマップ点の３次元位置についても各キーフレームの姿勢の補正量に応じて修正する。

以上、ループクロージングスレッドの処理内容について説明した。次に、間引き数が変化する様子について、図７を参照して説明する。

この図では、間引きデータキュー２３のサイズ（間引き数）は０で、ＳＬＡＭ処理の状態が追跡状態であるところから始まっている。ＳＬＡＭ処理部１１が１番目の画像で位置（姿勢）推定に成功し、この時の対応特徴点数は第１閾値より小さく、第２閾値より大きかったとする。この場合、間引きデータキュー２３のサイズ（間引き数）は変化せず０のままなので、次は２番目の画像で位置（姿勢）の推定が行われる。なお、ＳＬＡＭ処理部１１が位置（姿勢）推定に成功したので、間引きデータ破棄部１３は間引きデータキュー２３をクリアするが、間引きデータキュー２３のサイズが０の場合は、値が書き込まれないので、常にクリアされているのと同じである。したがって、これ以降の説明でも間引きデータキュー２３のサイズが０の場合は、間引きデータキュー２３のクリア処理についての記載は省略する。

そして、ＳＬＡＭ処理部１１は、２番目の画像で位置（姿勢）推定に成功し、この時の対応特徴点数が第１閾値より大きかったとする。すると、間引き数調整部１２が間引きデータキュー２３のサイズ（間引き数）を増やして１にするので、３番目の画像は間引きデータキュー２３に保存され、４番目の画像で位置（姿勢）の推定が行われる。

そして、ＳＬＡＭ処理部１１は、４番目の画像で位置（姿勢）推定に成功し、この時の対応特徴点数も第１閾値より大きかったとする。すると、間引きデータ破棄部１３は間引きデータキュー２３をクリアし、間引き数調整部１２が間引きデータキュー２３のサイズ（間引き数）を増やして２にするので、５番目の画像と６番目の画像は間引きデータキュー２３に保存され、７番目の画像で位置（姿勢）の推定が行われる。

そして、ＳＬＡＭ処理部１１は、７番目の画像で位置（姿勢）推定に成功し、この時の対応特徴点数は第２閾値より小さかったとする。すると、間引きデータ破棄部１３は間引きデータキュー２３をクリアし、間引き数調整部１２が間引きデータキュー２３のサイズ（間引き数）を減らして１にするので、８番目の画像は間引きデータキュー２３に保存され、９番目の画像で位置（姿勢）の推定が行われる。

そして、ＳＬＡＭ処理部１１は、９番目の画像で位置（姿勢）推定に成功し、この時の対応特徴点数は第１閾値より小さく、第２閾値より大きかったとする。すると間引きデータ破棄部１３は間引きデータキュー２３をクリアし、間引き数調整部１２は間引きデータキュー２３のサイズ（間引き数）を変化させず１のままにするので、１０番目の画像は間引きデータキュー２３に保存され、１１番目の画像で位置（姿勢）の推定が行われる。

このように、自律移動装置１００は、対応特徴点数の大きさに応じて間引き数を動的に変化させることにより、位置（姿勢）推定をできるだけ失敗させないようにしつつ、位置（姿勢）推定の処理を間引くことができる。位置（姿勢）推定の処理は負荷が大きいため、間引いて処理を行うことができれば、それだけ処理負荷を低減でき、能力の低いハードウェアでも問題無く実行できるようになる。

次に、位置（姿勢）推定に失敗した場合に、間引きデータキュー２３に保存された画像データが使われる様子について、図８を参照して説明する。

この図では、間引きデータキュー２３のサイズ（間引き数）は３で、ＳＬＡＭ処理の状態が追跡状態であるところから始まっている。ＳＬＡＭ処理部１１が１番目の画像で位置（姿勢）推定に成功し、この時の対応特徴点数は第１閾値より小さく、第２閾値より大きかったとする。すると、間引きデータ破棄部１３は間引きデータキュー２３をクリアし、間引き数調整部１２は、間引きデータキュー２３のサイズ（間引き数）を３のままにするので、２番目から４番目までの画像は間引きデータキュー２３に保存され、５番目の画像で位置（姿勢）の推定が行われる。

そして、ＳＬＡＭ処理部１１は、５番目の画像で位置（姿勢）推定に失敗したとする。すると、ＳＬＡＭ処理部１１は、間引きデータキュー２３から２番目の画像を取り出し、位置（姿勢）の推定を行う。そして、位置（姿勢）の推定に成功したら、間引きデータキュー２３から３番目の画像を取り出し、位置（姿勢）の推定を行う。（位置（姿勢）の推定に失敗したら、ロスト状態からの復帰処理をした後に、間引きデータキュー２３から３番目の画像を取り出し、位置（姿勢）の推定を行う。）

そして、また位置（姿勢）の推定に成功したら、間引きデータキュー２３から４番目の画像を取り出し、位置（姿勢）の推定を行う。（位置（姿勢）の推定に失敗したら、ロスト状態からの復帰処理をした後に、間引きデータキュー２３から４番目の画像を取り出し、位置（姿勢）の推定を行う。）

そして、また位置（姿勢）の推定に成功したら、２番目から４番目までの画像は既に取り出し済みで、間引きデータキュー２３は空になっているので、間引きデータキュー２３のサイズ（間引き数）を０にして、ＳＬＡＭ処理の状態を追跡状態にし、６番目の画像から再度、間引き処理しながら位置（姿勢）推定する処理に戻る。（位置（姿勢）の推定に失敗し、ロスト状態からの復帰にも失敗したら、間引きデータキュー２３のサイズ（間引き数）を０にして、ＳＬＡＭ処理の状態をロスト状態にし、ロスト状態からの復帰処理に成功するまで、ロスト状態からの復帰処理を繰り返す。）

以上説明したように、対応特徴点数が多い場合には、間引き数を大きくしても位置（姿勢）推定に成功する可能性が高いため、処理負荷を低減するために、間引き数調整部１２は間引き数を大きくしている。逆に、対応特徴点数が少ない場合には、位置（姿勢）推定に失敗する可能性が高まるため、間引き数調整部１２は間引き数を小さくして、できるだけ位置（姿勢）推定が失敗しないようにしている。このようにすることにより自律移動装置１００は、対応特徴点数に応じて間引き数を適切な値に調整することができる。

また、位置（姿勢）推定に失敗したら、ＳＬＡＭ処理部１１は、間引きデータキュー２３に保存されている画像データ（特徴点の情報）を用いて再度位置（姿勢）推定をすることにより、ロスト状態になることを回避し、精度を落とさずに位置（姿勢）推定を行うことができる。さらに、位置（姿勢）推定に成功したら、間引きデータ破棄部１３が間引きデータキュー２３をクリアすることにより、必要以上に記憶容量が圧迫されることを防いでいる。

なお、第１閾値よりも小さい第３閾値（例えば１００）を設定しておいて、ＳＬＡＭ処理部１１がトラッキングスレッド（図４）のステップＳ２２４の直前（又はステップＳ２２５の直後）等で対応点数を第３閾値と比較し、対応点数が第３閾値未満で、しかも間引きデータキュー２３が空で無ければ、ステップＳ２３０に飛ぶようにしてもよい。このようにすると、自己位置をロストした後だけでなく、ロストしそうな場合にも間引きデータキュー２３に保存された情報に基づいて自己位置を推定することができるようになる。

（変形例１）
実施形態１では、対応特徴点数が第１閾値より大きければ間引き数を大きくし、対応特徴点数が第２閾値より小さければ間引き数を小さくすることによって、対応特徴点数に応じて間引き数を調整している。しかし、間引き数の調整方法はこれに限られない。例えば、変形例１として、各間引き数に対応した閾値を設定する方法も考えられる。このような変形例１に係る自律移動装置１００は、図９に示すような、各間引き数に対応した対応特徴点数の範囲が設定されたテーブル（間引き数テーブル）を記憶部２０に記憶させておく。そして、間引き数調整部１２は、間引き数テーブルを参照して、対応特徴点数がある閾値の範囲内であれば、その範囲に対応する間引き数を、間引きデータキュー２３のサイズとして設定する。

変形例１では、間引きデータキュー２３のサイズが、対応特徴点数の大きさに対応した間引き数に素早く変更されるため、対応特徴点数が急激に大きくなったり小さくなったりする環境でも、できるだけ位置（姿勢）推定が失敗しないようにしつつ、間引き数を調整することができる。

（変形例２）
また、実施形態１や変形例１では、対応特徴点数に応じて間引き数を調整しているが、位置（姿勢）推定が成功する可能性の指標として、対応特徴点数以外にも、例えば、自律移動装置１００の速度が考えられる。速度が遅ければ、間引き数を大きくしても位置（姿勢）推定に成功する可能性は高いと考えられるが、速度が速ければ間引き数を小さくしないと位置（姿勢）推定に失敗する可能性が高いと考えられるからである。

そこで、変形例２として、自律移動装置１００の速度に応じて間引き数を調整する方法も考えられる。変形例２に係る自律移動装置１００では、例えば速度の閾値として第１速度閾値（例えば０．５ｋｍ／時）と第２速度閾値（例えば３ｋｍ／時）を設定する。そして、実施形態１のトラッキングスレッド（図４）の一部（ステップＳ２２２及びステップＳ２２４）を、図１０に示すステップＳ２５１及びステップＳ２５２に変更する。

ステップＳ２５１では速度が第１速度閾値未満か否かを判定する。速度が第１速度閾値未満なら（ステップＳ２５１；Ｙｅｓ）、間引き数調整部１２は、間引きデータキュー２３のサイズを１増やす（ステップＳ２２３）。また、ステップＳ２５２では速度が第２速度閾値より大きいか否かを判定する。速度が第２速度閾値より大きければ（ステップＳ２５２；Ｙｅｓ）、間引き数調整部１２は、間引きデータキュー２３のサイズを１減らす（ステップＳ２２５）。なお、速度は、上述したように駆動部３２から取得することができる。

変形例２に係る自律移動装置１００は、このようにすることにより、速度に応じて間引き数を適切な値に調整することができる。この場合、速度が遅ければ取得画像の変化が小さいため、対応特徴点数は多くなると考えられ、速度が速ければ取得画像の変化が大きいため、対応特徴点数は少なくなると考えられる。したがって、直接的には速度を見て間引き数を調整しているが、間接的には変形例２も対応特徴点数に応じて間引き数を調整していると言える。

変形例２においても、第１速度閾値よりも大きい第３速度閾値（例えば１０ｋｍ／時）を設定しておいて、ＳＬＡＭ処理部１１がトラッキングスレッド（図１０）のステップＳ２５２の直前（又はステップＳ２２５の直後）等で速度を第３速度閾値と比較し、速度が第３速度閾値より大きく、しかも間引きデータキュー２３が空で無ければ、ステップＳ２３０に飛ぶようにしてもよい。このようにすると、自己位置をロストした後だけでなく、ロストしそうな場合にも間引きデータキュー２３に保存された情報に基づいて自己位置を推定することができるようになる。

（変形例３）
また、速度に関しても、変形例１のように、各間引き数に対応した閾値を設定する方法が考えられる。このような変形例３に係る自律移動装置１００は、図１１に示すような、各間引き数に対応した速度の範囲が設定されたテーブル（速度間引き数テーブル）を記憶部２０に記憶させておく。そして、間引き数調整部１２は、速度間引き数テーブルを参照して、速度がある閾値の範囲内であれば、その範囲に対応する間引き数を、間引きデータキュー２３のサイズとして設定する。

変形例３では、間引きデータキュー２３のサイズが、速度の大きさに対応した間引き数に素早く変更されるため、速度が急激に早くなったり遅くなったりした場合でも、できるだけ位置（姿勢）推定が失敗しないようにしつつ、間引き数を調整することができる。

また、上述の実施形態１及び各変形例は適宜組み合わせることができる。例えば、実施形態１と変形例１を組み合わせると、間引き数調整部１２は、次のように間引き数を調整することができる。
‐対応特徴点数が間引き数テーブルに設定されている対応特徴点数の範囲のいずれかに含まれる場合には、その範囲に対応する間引き数を、間引きデータキュー２３のサイズとして設定する。
‐対応特徴点数が間引き数テーブルに設定されている対応特徴点数の範囲のいずれにも含まれない場合には、第１閾値よりも対応特徴点数が大きければ間引きデータキュー２３のサイズを１ずつ増やし（ただし最大キューサイズまで）、第２閾値よりも対応特徴点数が小さければ間引きデータキュー２３のサイズを１ずつ減らす（ただし０まで）。

また、実施形態１と変形例２を組み合わせると、間引き数調整部１２による間引き数の調整方法は、例えば少なくとも以下の２通りの方法が考えられる。

１つ目の方法は間引き数をできるだけ変化させる調整方法である。この場合、トラッキングスレッド（図４）のステップＳ２２２では、対応特徴点数が第１閾値より大きいか否かの判定と速度が第１速度閾値未満か否かの判定の論理和を判定する。対応特徴点数が第１閾値より大きいか又は速度が第１速度閾値より小さければ（ステップＳ２２２；Ｙｅｓ）、間引き数調整部１２は、間引きデータキュー２３のサイズを１増やす（ステップＳ２２３）。また、トラッキングスレッド（図４）のステップＳ２２４では対応特徴点数が第２閾値未満か否かの判定と速度が第２速度閾値より大きいか否かの判定の論理和を判定する。対応特徴点数が第２閾値未満か又は速度が第２速度閾値より大きければ（ステップＳ２２４；Ｙｅｓ）、間引き数調整部１２は、間引きデータキュー２３のサイズを１減らす（ステップＳ２２５）。

２つ目の方法は間引き数をできるだけ変化させない調整方法である。この場合、トラッキングスレッド（図４）のステップＳ２２２では、対応特徴点数が第１閾値より大きいか否かの判定と速度が第１速度閾値未満か否かの判定の論理積を判定する。対応特徴点数が第１閾値より大きくかつ速度が第１速度閾値より小さければ（ステップＳ２２２；Ｙｅｓ）、間引き数調整部１２は、間引きデータキュー２３のサイズを１増やす（ステップＳ２２３）。また、トラッキングスレッド（図４）のステップＳ２２４では対応特徴点数が第２閾値未満か否かの判定と速度が第２速度閾値より大きいか否かの判定の論理積を判定する。対応特徴点数が第２閾値未満でかつ速度が第２速度閾値より大きければ（ステップＳ２２４；Ｙｅｓ）、間引き数調整部１２は、間引きデータキュー２３のサイズを１減らす（ステップＳ２２５）。

同様に、実施形態１と変形例３を組み合わせることもできるし、変形例１と変形例３を組み合わせることもできる。対応特徴点数だけに頼って間引き数を調整すると、特徴点の対応が間違っていた場合に問題が生じる可能性があるが、上述のように変形例２や変形例３を組み合わせて速度の情報も用いることで、状況に応じて間引き数の調整をさらに適切に行うことできるようになる。

なお、上述の実施形態１及び変形例２の説明において、間引き数調整部１２は、間引きデータキュー２３のサイズを１ずつ増減していたが、増減する値は任意である。例えば２ずつ増減してもよいし、増やす時は２ずつ増やし減らす時は３ずつ減らす等にしてもよい。また、間引き数テーブルのような形で、間引き数の値の代わりに増減値を設定し、対応特徴点数の大きさに応じて増減値を変化させてもよい。

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

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

（付記１）
画像を用いて地図の作成及び自己位置の推定を行って移動する自律移動装置であって、
画像を逐次撮影する撮像部と、
制御部と、
を備え、
前記制御部は、
前記逐次撮影された画像から所定の間引き数の間引き画像を間引いた画像の情報を記憶部に保存し、
前記撮像部で撮影された画像に含まれる特徴点を抽出し、
前記記憶部に保存された画像の情報に含まれる特徴点と、前記撮影された画像から抽出した特徴点と、の対応を取得して自己位置を推定し、
前記推定の際に取得した前記対応する特徴点の数である対応特徴点数に応じて前記間引き数を調整する、
自律移動装置。

（付記２）
前記制御部は、
前記対応特徴点数が第１閾値より大きければ前記間引き数を増加させ、
前記対応特徴点数が第２閾値より小さければ前記間引き数を減少させる、
付記１に記載の自律移動装置。

（付記３）
前記記憶部には前記対応特徴点数の大きさの範囲に対応する間引き数が設定された間引き数テーブルが記憶されており、
前記制御部は、前記間引き数テーブルを参照し、前記間引き数を前記対応特徴点数の大きさに対応する間引き数に設定する、
付記１又は２に記載の自律移動装置。

（付記４）
前記制御部は、
自律移動装置が移動する速度を取得し、
前記速度が第１速度閾値より小さければ前記間引き数を増加させ、
前記速度が第２速度閾値より大きければ前記間引き数を減少させる、
付記１から３のいずれか１つに記載の自律移動装置。

（付記５）
前記制御部は、
自律移動装置が移動する速度を取得し、
前記記憶部に記憶された前記速度の範囲に対応する間引き数が設定された速度間引き数テーブルを参照し、前記間引き数を前記速度に対応する間引き数に設定する、
付記１から４のいずれか１つに記載の自律移動装置。

（付記６）
前記制御部は、
前記間引き画像の情報も前記記憶部に保存し、
前記自己位置の推定に失敗したら前記記憶部に保存された前記間引き画像の情報に含まれる特徴点の情報を用いて自己位置を推定する、
付記１から５のいずれか１つに記載の自律移動装置。

（付記７）
前記制御部は、
前記間引き画像の情報に含まれる特徴点の情報を用いて自己位置を推定することに成功したら、前記記憶部に保存された前記間引き画像の情報を破棄する、
付記６に記載の自律移動装置。

（付記８）
逐次撮影した画像を用いて地図の作成及び自己位置の推定を行って移動する自律移動方法であって、
前記逐次撮影した画像から所定の間引き数の間引き画像を間引いた画像の情報を用いて自己位置を推定し、
状況に応じて前記間引き数を調整する、
自律移動方法。

（付記９）
逐次撮影した画像を用いて地図の作成及び自己位置の推定を行って移動する自律移動装置のコンピュータに、
前記逐次撮影した画像から所定の間引き数の間引き画像を間引いた間引かれた画像と前記間引き画像とをそれぞれ記憶部に保存し、
前記間引かれた画像の情報を用いて自己位置を推定し、
状況に応じて前記間引き数を調整し、
前記自己位置の推定に失敗したら前記記憶部に保存された前記間引き画像の情報を用いて自己位置を推定する、
処理を実行させるためのプログラム。

１０…制御部、１１…ＳＬＡＭ処理部、１２…間引き数調整部、１３…間引きデータ破棄部、１４…移動制御部、２０…記憶部、２１…フレームＤＢ、２２…マップ点ＤＢ、２３…間引きデータキュー、３１…撮像部、３２…駆動部、３３…通信部、１００…自律移動装置

Claims (10)

1. 画像を用いて地図の作成及び自己位置の推定を行って移動する自律移動装置であって、
   画像を逐次撮影する撮像部と、
   制御部と、
   を備え、
   前記制御部は、
   前記自律移動装置が移動する速度を取得し、
   前記逐次撮影された画像から所定の間引き数の間引き画像を間引いた画像の情報を記憶部に保存し、
   前記撮像部で撮影された画像に含まれる特徴点を抽出し、
   前記記憶部に保存された画像の情報に含まれる特徴点と、前記撮影された画像から抽出した特徴点と、の対応を取得して自己位置を推定し、
   前記推定の際に取得した前記対応する特徴点の数である対応特徴点数に応じて前記間引き数を調整する際、
   前記速度が第１速度閾値より小さければ前記間引き数を増加させ、
   前記速度が第２速度閾値より大きければ前記間引き数を減少させる、
   ことを特徴とする自律移動装置。
2. 画像を用いて地図の作成及び自己位置の推定を行って移動する自律移動装置であって、
   画像を逐次撮影する撮像部と、
   制御部と、
   を備え、
   前記制御部は、
   前記自律移動装置が移動する速度を取得し、
   前記逐次撮影された画像から所定の間引き数の間引き画像を間引いた画像の情報を記憶部に保存し、
   前記撮像部で撮影された画像に含まれる特徴点を抽出し、
   前記記憶部に保存された画像の情報に含まれる特徴点と、前記撮影された画像から抽出した特徴点と、の対応を取得して自己位置を推定し、
   前記推定の際に取得した前記対応する特徴点の数である対応特徴点数に応じて前記間引き数を調整する際、
   予め前記記憶部に記憶された前記速度の範囲に対応する間引き数が設定された速度間引き数テーブルを参照し、前記間引き数を前記速度に対応する間引き数に設定する、
   ことを特徴とする自律移動装置。
3. 画像を用いて地図の作成及び自己位置の推定を行って移動する自律移動装置であって、
   画像を逐次撮影する撮像部と、
   制御部と、
   を備え、
   前記制御部は、
   前記逐次撮影された画像から所定の間引き数の間引き画像の情報と、前記逐次撮影された画像から前記間引き画像を間引いた画像の情報と、を記憶部に保存し、
   前記撮像部で撮影された画像に含まれる特徴点を抽出し、
   前記記憶部に保存された画像の情報に含まれる特徴点と、前記撮影された画像から抽出した特徴点と、の対応を取得して自己位置を推定し、
   前記推定の際に取得した前記対応する特徴点の数である対応特徴点数に応じて前記間引き数を調整し、
   前記自己位置の推定に失敗したら前記記憶部に保存された前記間引き画像の情報に含まれる特徴点の情報を用いて自己位置を推定する、
   ことを特徴とする自律移動装置。
4. 前記制御部は、
   前記間引き画像の情報に含まれる特徴点の情報を用いて自己位置を推定することに成功したら、前記記憶部に保存された前記間引き画像の情報を破棄する、
   ことを特徴とする請求項３に記載の自律移動装置。
5. 逐次撮影した画像を用いて地図の作成及び自己位置の推定を行って移動する自律移動装置が実行する自律移動方法であって、
   前記自律移動装置が移動する速度を取得し、
   前記逐次撮影された画像から所定の間引き数の間引き画像を間引いた画像の情報を記憶部に保存し、
   前記撮影された画像に含まれる特徴点を抽出し、
   前記記憶部に保存された画像の情報に含まれる特徴点と、前記撮影された画像から抽出した特徴点と、の対応を取得して自己位置を推定し、
   前記推定の際に取得した前記対応する特徴点の数である対応特徴点数に応じて前記間引き数を調整する際、
   前記速度が第１速度閾値より小さければ前記間引き数を増加させ、
   前記速度が第２速度閾値より大きければ前記間引き数を減少させる、
   ことを特徴とする自律移動方法。
6. 逐次撮影した画像を用いて地図の作成及び自己位置の推定を行って移動する自律移動装置が実行する自律移動方法であって、
   前記自律移動装置が移動する速度を取得し、
   前記逐次撮影された画像から所定の間引き数の間引き画像を間引いた画像の情報を記憶部に保存し、
   前記撮影された画像に含まれる特徴点を抽出し、
   前記記憶部に保存された画像の情報に含まれる特徴点と、前記撮影された画像から抽出した特徴点と、の対応を取得して自己位置を推定し、
   前記推定の際に取得した前記対応する特徴点の数である対応特徴点数に応じて前記間引き数を調整する際、
   予め前記記憶部に記憶された前記速度の範囲に対応する間引き数が設定された速度間引き数テーブルを参照し、前記間引き数を前記速度に対応する間引き数に設定する、
   ことを特徴とする自律移動方法。
7. 逐次撮影した画像を用いて地図の作成及び自己位置の推定を行って移動する自律移動装置が実行する自律移動方法であって、
   前記逐次撮影された画像から所定の間引き数の間引き画像の情報と、前記逐次撮影された画像から前記間引き画像を間引いた画像の情報と、を記憶部に保存し、
   前記撮影された画像に含まれる特徴点を抽出し、
   前記記憶部に保存された画像の情報に含まれる特徴点と、前記撮影された画像から抽出した特徴点と、の対応を取得して自己位置を推定し、
   前記推定の際に取得した前記対応する特徴点の数である対応特徴点数に応じて前記間引き数を調整し、
   前記自己位置の推定に失敗したら前記記憶部に保存された前記間引き画像の情報に含まれる特徴点の情報を用いて自己位置を推定する、
   ことを特徴とする自律移動方法。
8. 逐次撮影した画像を用いて地図の作成及び自己位置の推定を行って移動する自律移動装置のコンピュータに、
   前記自律移動装置が移動する速度を取得し、
   前記逐次撮影された画像から所定の間引き数の間引き画像を間引いた画像の情報を記憶部に保存し、
   前記撮影された画像に含まれる特徴点を抽出し、
   前記記憶部に保存された画像の情報に含まれる特徴点と、前記撮影された画像から抽出した特徴点と、の対応を取得して自己位置を推定し、
   前記推定の際に取得した前記対応する特徴点の数である対応特徴点数に応じて前記間引き数を調整する際、
   前記速度が第１速度閾値より小さければ前記間引き数を増加させ、
   前記速度が第２速度閾値より大きければ前記間引き数を減少させる、
   処理を実行させるためのプログラム。
9. 逐次撮影した画像を用いて地図の作成及び自己位置の推定を行って移動する自律移動装置のコンピュータに、
   前記自律移動装置が移動する速度を取得し、
   前記逐次撮影された画像から所定の間引き数の間引き画像を間引いた画像の情報を記憶部に保存し、
   前記撮影された画像に含まれる特徴点を抽出し、
   前記記憶部に保存された画像の情報に含まれる特徴点と、前記撮影された画像から抽出した特徴点と、の対応を取得して自己位置を推定し、
   前記推定の際に取得した前記対応する特徴点の数である対応特徴点数に応じて前記間引き数を調整する際、
   予め前記記憶部に記憶された前記速度の範囲に対応する間引き数が設定された速度間引き数テーブルを参照し、前記間引き数を前記速度に対応する間引き数に設定する、
   処理を実行させるためのプログラム。
10. 逐次撮影した画像を用いて地図の作成及び自己位置の推定を行って移動する自律移動装置のコンピュータに、
    前記逐次撮影された画像から所定の間引き数の間引き画像の情報と、前記逐次撮影された画像から前記間引き画像を間引いた画像の情報と、を記憶部に保存し、
    前記撮影された画像に含まれる特徴点を抽出し、
    前記記憶部に保存された画像の情報に含まれる特徴点と、前記撮影された画像から抽出した特徴点と、の対応を取得して自己位置を推定し、
    前記推定の際に取得した前記対応する特徴点の数である対応特徴点数に応じて前記間引き数を調整し、
    前記自己位置の推定に失敗したら前記記憶部に保存された前記間引き画像の情報に含まれる特徴点の情報を用いて自己位置を推定する、
    処理を実行させるためのプログラム。

Images