JP7327596B2 - 自律移動装置、自律移動装置のレンズの汚れ検出方法及びプログラム - Google Patents

Description

本発明は、自律移動装置、自律移動装置のレンズの汚れ検出方法及びプログラムに関する。

用途に応じて自律的に移動する自律移動装置が普及してきている。例えば、自律的に移動して人とコミュニケーションを行う自律移動装置が知られている。一般的に、このような自律移動装置は、実空間の地図の作成と、実空間内での自機位置の推定を行い、人の検出や認識を行う必要がある。

実空間の地図を作成するための手法としては、例えばＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）法が知られている。ＳＬＡＭ法では、カメラを用いて自機の周囲の画像を撮影して環境地図を作成しながら自己位置を推定し、移動制御を行う。また、人の検出や認識を行うためにもカメラを使用する。コミュニケーションを行う自律移動装置は、人と接触する可能性が少なくないため、搭載しているカメラのレンズをユーザに指等で触られてレンズが汚れることにより、自己位置推定、人の検出や認識に支障を来すおそれがある。さらに、このような装置では、カメラで取得する画像をユーザが確認することが無いので、ユーザがレンズの汚れに気付くことができない。

特許文献１では、撮像部により撮像された画像データの明るさの変化の急峻度のうち、検出した最大の急峻度が所定の急峻度未満である場合又は所定の値以上の急峻度が検出できない場合には、撮像部のレンズが汚れている可能性があると判定する装置が開示されている。

特開２００１－１１９６１４号公報

しかしながら、上記の装置では、撮影位置や撮影方向は関係なくレンズの汚れを判断するものであって、レンズの一部に汚れがあったとしても、レンズの汚れの無い部分で検出された急峻度が所定の急峻度を満たしていれば、レンズに汚れがあると判定されないこととなる。

そこで、本発明は、上記問題を解決するためになされたものであり、撮像部のレンズの汚れを好適に検出する自律移動装置、撮像部のレンズの汚れを好適に検出する方法及びプログラムを提供することを目的とする。

上記目的を達成するため、本発明の自律移動装置は、
自機を移動及び回転させて自機位置及び自機の向きを変化させる駆動部と、
レンズを介して画像を撮像する撮像部と、
撮像された前記画像を記憶する画像記憶部と、
前記駆動部、前記撮像部及び前記画像記憶部を制御してＶｉｓｕａｌ ＳＬＡＭ制御を行なう制御部と、を備え、
前記制御部は、
前記画像記憶部に記憶された第１の画像の特徴点と対応する、基準並進距離を並進する間の前記撮像部で撮像した第２の画像の対応特徴点の数が、基準対応特徴点数未満である場合をＶｉｓｕａｌ ＳＬＡＭ制御の失敗として自機の位置又は向きの少なくとも何れかの推定を行わず、
前記基準並進距離を並進する間における前記Ｖｉｓｕａｌ ＳＬＡＭ制御の失敗回数と前記第２の画像の対応特徴点が基準対応特徴点数未満かどうかを判定した回数とに基づく失敗率が基準失敗率より高い場合、前記レンズが汚れていると判定する、
ことを特徴とする。

本発明によれば、撮像部のレンズの汚れを好適に検出することができる。

本実施形態に係る自律移動装置の外観を示す図である。 本実施形態に係る充電器の外観を示す図である。 本実施形態に係る充電器が送信する帰還信号を説明する図である。 本実施形態に係る自律移動装置の機能構成を示す図である。 （ａ）～（ｄ）は、本実施形態に係る自律移動装置が生成する情報のデータ構造を示す図である。 本実施形態に係る自律移動装置の起動時処理のフローチャートである。 本実施形態に係る自律移動装置の自機位置推定、環境地図生成スレッドのフローチャートである。 本実施形態に係る自律移動装置の自機位置推定処理のフローチャートである。 本実施形態に係る自律移動制装置の環境地図生成処理のフローチャートである。 本実施形態に係る自律移動装置のＤＢを生成１の処理のフローチャートである。 本実施形態に係る自律移動装置のレンズ汚れ判定１の処理のフローチャートである。 本実施形態に係る自律移動装置の移動指示アプリのフローチャートである。 本実施形態に係る自律移動装置のレンズ汚れ判定２の処理のフローチャートである。 本実施形態に係る自律移動装置の移動スレッドのフローチャートである。 本実施形態に係る自律移動装置の顔検出スレッドのフローチャートである。 本実施形態に係る自律移動装置のＤＢを生成３の処理のフローチャートである。 本実施形態に係る自律移動装置のレンズ汚れ判定３の処理のフローチャートである。

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

本発明の実施形態に係る自律移動装置は、地図（環境地図）を作成しながら、用途に応じて自律的に移動する装置である。この用途とは、例えば、人とコミュニケーションを行う事、警備監視、屋内掃除等である。また、本実施形態に係る自律移動装置は、後述する動作モードに応じて移動する。

自律移動装置１００は、図１に示すように、外観上、帰還信号受信部３１（３１ａ，３１ｂ）、駆動部３２（３２ａ，３２ｂ）、撮像部３３、充電接続部３５、を備える。なお、図１には示されていないが、自律移動装置１００は、周囲に存在する物体（障害物）を検出する障害物センサを備えている。また、自律移動装置１００の電池を充電するための充電設備である充電器２００は、図２に示すように、外観上、帰還信号送信部５１（５１ａ，５１ｂ）、電力供給部５２を備える。

自律移動装置１００の充電接続部３５と充電器２００の電力供給部５２とが接続することにより、充電器２００から電力の供給を受けて、自律移動装置１００は内蔵している充電池を充電することができる。なお、充電接続部３５及び電力供給部５２は、それぞれがお互いと接続するための接続端子である。これらは、駆動部３２により自律移動装置１００が充電器２００の上に移動することによって、接続される。この接続は、充電接続部３５と電力供給部５２とが接触することによって接続してもよいし、充電接続部３５と電力供給部５２とが近接することによって電磁誘導等により接続してもよい。

撮像部３３は、自律移動装置１００の前方から上部までの広範囲を撮像できる広角レンズを備える。このため、撮像部３３は天井の電灯が点灯しているか否かを判定可能な画像を撮像することができる。そして、自律移動装置１００は、撮像部３３が撮像した画像を利用して単眼ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）処理を行うことができる。

自律移動装置１００の帰還信号受信部３１は、充電器２００から送信される帰還信号（赤外線ビーコン）を受信するためのデバイスである。自律移動装置１００の正面に向かって左に帰還信号受信部３１ａ、右に帰還信号受信部３１ｂと、合計２つの帰還信号受信部３１が備えられている。また、充電器２００の帰還信号送信部５１は、自律移動装置１００に帰還信号を送信するためのデバイスである。充電器２００の正面に向かって右に帰還信号送信部５１ａが、左に帰還信号送信部５１ｂが、それぞれ備えられている。そして、帰還信号送信部５１ａから送信される帰還信号と、帰還信号送信部５１ｂから送信される帰還信号とは、異なる信号になっている。したがって、帰還信号受信部３１は、各々左右どちらの帰還信号送信部５１からの帰還信号を受信したのかを判別することができる。

充電器２００の帰還信号送信部５１から送信される帰還信号の左右それぞれの受信可能範囲５３（５３ａ，５３ｂ）の一例を図３に示す。帰還信号送信部５１ａから送信される帰還信号は、受信可能範囲５３ａの内部に自律移動装置１００の帰還信号受信部３１が入ると受信できる。そして、帰還信号送信部５１ｂから送信される帰還信号は、受信可能範囲５３ｂの内部に自律移動装置１００の帰還信号受信部３１が入ると受信できる。したがって、自律移動装置１００は、受信可能範囲５３の内部に入ると充電器２００の存在する方向を把握することができる。そして、自律移動装置１００は、帰還信号送信部５１ａからの帰還信号を帰還信号受信部３１ａが受信し、帰還信号送信部５１ｂからの帰還信号を帰還信号受信部３１ｂが受信するように向きを調整しながら進むことによって、充電器２００の上に移動することができる。自律移動装置１００は、充電器２００の上に移動すると、充電接続部３５と電力供給部５２とが接続し、自律移動装置１００に内蔵されている電池を充電することができる。

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

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

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

図４に示すように、自律移動装置１００は、帰還信号受信部３１（３１ａ，３１ｂ）、駆動部３２（３２ａ，３２ｂ）、撮像部３３、充電接続部３５、に加えて、制御部１０、記憶部２０、通信部３４、センサ部４０、を備える。

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

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

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

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

情報記憶部２３には、画像先鋭度値データベースＤＢ，ＤＢ２、画像先鋭度フラグデータベースＤＢＧ、顔領域先鋭度値データベースＤＢ３、顔領域先鋭度フラグデータベースＤＢ３Ｈ、等があり、画像情報取扱部１６が生成した後述する情報Ｇ（図５（ａ）），Ｇ１（図５（ｂ）），Ｈ（図５（ｃ）），Ｈ１（図５（ｄ））等が記憶される。

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

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

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

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

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

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

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

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

画像情報取扱部１６は、撮像部３３によって撮像された画像（物体の境界等）に対してラプラシアンオペレータの畳み込み処理を行い、その後分散を取ることにより、画像（又は後述する画像内に検出された顔等の物体の検出領域）の画像先鋭度値（シャープネス）を算出する。画像先鋭度値は、画像全体又は画像内に検出された物体の物体検出領域全体に対して算出された１つの値（代表先鋭度値）である。画像先鋭度値は、画像又は物体検出領域を構成する構造の細かさを示す値である。ラプラシアンオペレータは、隣接する上下左右の４画素（４近傍）についての２次微分を行うものを使用するが、斜め方向に隣接する８画素（８近傍）について２次微分を行うものとしても良い。その他、画像から取得した後述の情報の各種算定等を行う。

以上、自律移動装置１００の機能構成について説明した。次に、自律移動装置１００で起動される各種処理について説明する。自律移動装置１００は、電源オフ時は充電器２００に接続されて充電されており、電源が投入されると、充電器２００に接続された位置で、後述する起動時処理が実行され、各種アプリ及び各種スレッドが並行して実行される。では、自律移動装置１００の起動時の処理について、図６を参照して説明する。図６は、自律移動装置１００が起動した時に実行される起動時処理のフローチャートである。

まず、自律移動装置１００の制御部１０は、ＳＬＡＭ処理用の各種スレッドを起動する（ステップＳ１０１）。ＳＬＡＭ処理用の各種スレッドとは、自律移動装置１００の姿勢（位置及び向き）を推定する自機位置推定処理及び環境地図を生成する処理を行う自機位置推定、環境地図生成スレッド、ループクロージング処理を行うループクロージングスレッド等である。なお、ループクロージング処理とは、自律移動装置１００が、以前来たことのある同じ場所に戻ってきたことを認識した場合に、以前この同じ場所にいた時の自律移動装置１００の姿勢の値と現在の姿勢の値とのずれを用いて、以前来た時から今までの移動軌跡中のキーフレーム（の３Ｄ姿勢）や、関連するＭａｐＰｏｉｎｔ（の３Ｄ座標）を修正する処理をいう。自機位置推定、環境地図作成スレッドの詳細については後述する。

次に、制御部１０は、移動指示アプリ及び行動計画アプリを起動する（ステップＳ１０２）。行動計画アプリ（図示せず）は、自律移動装置１００の行動を制御するアプリである。行動計画アプリでは、自律移動装置１００の動作モード（充電器帰還モード、お散歩モード、対話モード）の設定を行うことができる。また、行動計画アプリは、環境地図内での自律移動装置１００の位置を理解し、自律移動装置１００の移動先を移動スレッドに指示することができる。さらに、行動計画アプリは、撮像部３３のレンズに汚れが有るとの通知を受けた際に、自律移動装置１００にユーザに対して撮像部３３のレンズの清掃を促す行動を指示する。移動指示アプリは、行動計画アプリの設定に応じた動作モード（例えば充電器帰還モード）に基づいて、次のステップで起動される自機位置推定、環境地図生成スレッドから自律移動装置１００の現在の環境地図内における姿勢（位置及び向き）の情報を受けた行動計画アプリの指示する移動先への移動を実行するアプリである。なお、移動指示アプリの詳細については後述する。

次に、制御部１０は、移動スレッド、顔検出スレッドを起動し（ステップＳ１０３）、起動時処理を終了する。移動スレッドは、移動指示アプリから移動命令を受けて、制御部１０が駆動部３２を制御して自律移動装置１００を移動させる処理を行うスレッドである。そして、起動時処理を終了した後は、自律移動装置１００は、この起動時処理によって起動された各アプリ及びスレッドにより制御される。なお、移動スレッド、顔検出スレッドの詳細については後述する。

ＳＬＡＭ処理用のスレッドのうち、自機位置推定、環境地図生成スレッドについて、図７を参照して説明する。自機位置推定、環境地図生成スレッドは、撮像部３３により撮像して取得した画像を利用したＶｉｓｕａｌ ＳＬＡＭ法による環境地図を用いて追跡処理（自機位置推定処理）を行うスレッドである。さらに、自機位置推定、環境地図生成のために取得した画像を利用して、撮像部３３のレンズの汚れの有無を判定するスレッドでもある。

まず、制御部１０は、ＳＬＡＭ法に基づいて自機位置推定、環境地図の生成を行う（ステップＳ２０１）。これにより、地図の情報及びビジュアルオドメトリ（地図と画像とを用いて推定した自機位置の情報）が生成される。なお、自機位置推定処理及び環境地図作成処理の詳細については後述する。

次に、制御部１０は、生成した環境地図と推定した自機位置とを行動アプリに送信する（ステップＳ２０２）。これにより、行動計画アプリは、自律移動装置１００の周囲の環境を理解し、自律移動装置１００の移動先を指示することができる。

そして、制御部１０は、画像情報取扱部１６に、ステップＳ２０１で取得した画像の画像先鋭度値を算出させることにより、画像先鋭度データベースＤＢを生成（ＤＢを生成１）する（ステップＳ２０３）。画像先鋭度データベースＤＢを生成する処理の詳細については後述する。

そして、制御部１０は、画像の画像先鋭度値を用いて撮像部３３のレンズの汚れを判定（レンズ汚れ判定１）する（ステップＳ２０４）。撮像部３３のレンズの汚れを判定する処理については後述する。

撮像部３３のレンズに汚れがあると判定された場合（ステップＳ２０５；Ｙｅｓ）、制御部１０は、その旨を行動計画アプリに通知する（ステップＳ２０６）。行動アプリは、自律移動装置１００に対して、ユーザに自律移動装置１００の撮像部３３のレンズの清掃を促す行動、例えば、レンズの清掃を促す音声又は警告音を発する、を実行するように指示し、ステップＳ２０７に進む。撮像部３３のレンズに汚れが有ると判定されなかった場合（ステップＳ２０５；Ｎｏ）、レンズの汚れは不明であると判定し、ステップＳ２０７に進む。

ステップＳ２０７では、制御部１０は、各種アプリやユーザから終了指示があるか否かを判定する。終了指示があるなら（ステップＳ２０７；Ｙｅｓ）、自機位置推定、環境地図生成スレッドを終了する。終了指示がないなら（ステップＳ２０７；Ｎｏ）、ステップＳ２０１に戻る。

以上、自機位置推定、環境地図生成スレッドの処理について説明した。次に、自機位置推定、環境地図生成スレッド（図７）のステップＳ２０１で実行される自機位置推定処理について、図８を参照して説明する。この処理は、位置推定部１３が、最初に初期化処理を行い、その後自機位置推定（撮像部３３で取得した画像を用いてビジュアルオドメトリにより自機位置を推定する）を続ける処理である。

これにより、地図の情報に基づいて自律的に移動を行いつつ、適宜地図の情報を更新することができる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

次に、位置推定部１３は、ステップＳ３０１、ステップＳ３０２を経由して、初期化済の場合の処理であるステップＳ３２１へ進む。

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

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

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

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

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

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

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

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

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

なお、基準並進距離を自律移動装置１００が並進する間に、ステップＳ３２５のＶｉｓｕａｌ ＳＬＡＭ制御の失敗回数を、ステップＳ３２５に到達した回数で割ることによりＶｉｓｕａｌ ＳＬＡＭ制御の失敗率を算定し、この失敗率が基準失敗率（例えば５０パーセント）より高い場合、後述する撮像部３３のレンズが汚れている、と判定しても良い。レンズが汚れていることにより、対応特徴点を正しく取得できていないと考えられるからである。

以上、自機位置推定処理について説明した。次に、自機位置推定、環境地図生成スレッド（図７）のステップＳ２０１で実行される環境地図生成処理について、図９を用いて説明する。この処理では地図作成部１２が、キーフレーム中の対応特徴点の３Ｄ位置を計算して、地図の情報（Ｍａｐ点ＤＢ）を作成している。

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

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

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

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

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

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

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

次に、自機位置推定、環境地図生成スレッド（図７）のステップＳ２０３で実行されるＤＢを生成１の処理について、図１０を用いて説明する。この処理では撮像部３３で撮像した物体の境界等が写る画像から、画像情報取扱部１６が、画像先鋭度値を計算して、画像を撮像した時の自律移動装置１００の姿勢（３次元の位置及び３次元の回転）と、画像を撮像した時刻とともに１つの情報Ｇとして生成し、情報記憶部２３に保存されている情報Ｇの姿勢に近い姿勢で撮像された画像から得られた情報Ｇ０の画像先鋭度値と情報Ｇの画像先鋭度値とを比較し、画像先鋭度値が高い情報を情報記憶部２３に保存する。

まず、画像情報取扱部１６は、行動計画アプリ等から情報記憶部２３内にある画像先鋭度値データベースＤＢをクリアするように指示があるか否かを判定する（ステップＳ５０１）。クリアするように指示があるなら（ステップＳ５０１；Ｙｅｓ）画像先鋭度値データベースＤＢをクリアし（ステップＳ５０２）、クリアするように指示がないなら（ステップＳ５０１；Ｎｏ）、ステップＳ５０３に進む。画像先鋭度値データベースＤＢをクリアするのは、撮像部３３のレンズが清掃されたと推定される時である。画像先鋭度値データベースＤＢをクリアする指示は、例えば、自律移動装置１００に備えられたスイッチ（図示せず）をレンズ清掃後にユーザが押すことであっても良いし、自律移動装置１００がユーザと音声対話等によってレンズ清掃の確認ができたことであっても良い。

ステップＳ５０３では、画像情報取扱部１６は、ステップＳ２０２で自機位置推定処理時等に取得した画像に対して以下の式（７）に示す４近傍のラプラシアンオペレータの畳み込み処理を行った後、分散を取り、画像先鋭度値とする。

次に、画像情報取扱部１６は、ステップＳ２０２で自機位置推定処理時に計算した自律移動装置１００の姿勢を、画像先鋭度値が算出された画像が撮像された時の姿勢とし、この姿勢と、この画像が撮像された時刻と、画像先鋭度値とをひとまとめにして、情報Ｇとする（ステップＳ５０４）。

画像情報取扱部１６は、画像先鋭度値データベースＤＢ内に、情報Ｇの姿勢と所定の誤差ＴＨ０の範囲内の姿勢である情報があるかどうかを検索する（ステップＳ５０５）。所定の誤差ＴＨ０の範囲内の姿勢である情報があった場合（ステップＳ５０５；Ｙｅｓ）、その情報を情報Ｇ０として取り出し、無かった場合（ステップＳ５０５；Ｎｏ）、ステップＳ５０８へ進む。なお、所定の誤差ＴＨ０の範囲内の姿勢の情報が複数ある場合には、１番画像先鋭度値が大きいものを選ぶ。姿勢間の距離は、ユークリッド距離を用いる。回転の３次元は、Ｅｘｐｏｎｅｎｔｉａｌ Ｍａｐ Ｑｕａｔｅｒｎｉｏｎを用いることによって、ユークリッド空間で扱うことができる。

次に、画像情報取扱部１６は、情報Ｇの画像先鋭度値と情報Ｇ０の画像先鋭度値を比較する（ステップＳ５０６）。情報Ｇの画像先鋭度値が情報Ｇ０の画像先鋭度値よりも大きい場合（ステップＳ５０６；Ｙｅｓ）、画像先鋭度値データベースＤＢ内の情報Ｇ０を削除する（ステップＳ５０７）。情報Ｇの画像先鋭度値が情報Ｇ０の画像先鋭度値よりも大きくない場合（ステップＳ５０６；Ｎｏ）、そのまま終了する。

ステップＳ５０８では、画像情報取扱部１６は、画像先鋭度値データベースＤＢに情報Ｇを登録し、終了する。

次に、自機位置推定、環境地図生成スレッド（図７）のステップＳ２０４で実行されるレンズ汚れ判定１の処理について、図１１を用いて説明する。この処理では、ＤＢを生成１の処理で算定した情報Ｇの画像先鋭度値と、現時刻から所定の時間Ｔ０以前に撮像された過去の画像であって情報Ｇが撮像された姿勢から所定の誤差ＴＨ１内で情報Ｇの姿勢に１番近い姿勢である情報Ｇ１の画像先鋭度値に所定の係数を掛けたものと、を比較することにより、自律移動装置１００の撮像部３３のレンズの汚れを判定する。

まず、画像情報取扱部１６は、画像先鋭度値データベースＤＢ内に、現時刻より所定の時間Ｔ０以前に撮像された物体の境界等が写る画像の情報であって、情報Ｇの画像が撮像された姿勢からの姿勢距離が所定の誤差（範囲）ＴＨ１以下で１番近い姿勢である画像の情報（以下、この情報を情報Ｇ１とする）を検索する（ステップＳ６０１）。画像先鋭度値データベースＤＢ内に、情報Ｇ１が存在する場合（ステップＳ６０１；Ｙｅｓ）、ステップＳ６０２に進む。画像先鋭度値データベースＤＢ内に、情報Ｇ１が存在しない場合（ステップＳ６０１；Ｎｏ）、ステップＳ６１０に進む。

ステップＳ６０２では、情報Ｇ１の画像先鋭度値に１未満の係数ＴＨＧ（先鋭度値が落ちたとみなすためのマージン）を掛けたものと情報Ｇの画像先鋭度値とを比較する。情報Ｇ１の画像先鋭度値に係数ＴＨＧを掛けたものより情報Ｇの画像先鋭度値が大きい場合（ステップＳ６０２；Ｙｅｓ）、画像先鋭度フラグを１とし、現時刻、情報Ｇの姿勢とともに画像先鋭度フラグデータベースＤＢＧ内に保存し（ステップＳ６０３）、ステップＳ６０４に進む。情報Ｇ１の画像先鋭度値に係数ＴＨＧを掛けたものより情報Ｇの画像先鋭度値が大きくない場合（ステップＳ６０２；Ｎｏ）、画像先鋭度フラグを０とし、現時刻、情報Ｇの姿勢とともに画像先鋭度フラグデータベースＤＢＧ内に保存し（ステップＳ６０９）、ステップＳ６０４に進む。

次に、画像情報取扱部１６は、画像先鋭度フラグデータベースＤＢＧ内の、現時刻より所定の時間Ｔ１以上古い情報を削除する（ステップＳ６０４）。

画像情報取扱部１６は、画像先鋭度フラグデータベースＤＢＧ内に保存されている情報の全回転姿勢がＴＨ３方向をカバーしているかを判定する（ステップＳ６０５）。ここで、全回転姿勢がＴＨ３方向をカバーしている、について説明する。例えば、ＴＨ３＝６であるとする。本来、回転姿勢は３次元で持っているが、通常、自律移動装置１００は水平な床面上を移動するので、回転姿勢の移動はＹａｗ角だけの１次元とみなす。ＴＨ３＝６としているので、３６０度／６＝６０度となる。従って、画像先鋭度フラグデータベースＤＢＧ内に保存されている情報の回転方向が、６０度毎に異なる６方向をカバーしている必要がある。これにより、太陽光や照明の方向によるレンズ汚れの検出精度への影響を小さくすることができる。画像先鋭度フラグデータベースＤＢＧ内に保存されている情報の全回転姿勢がＴＨ３方向をカバーしている場合（ステップＳ６０５；Ｙｅｓ）、ステップＳ６０６に進む。画像先鋭度フラグデータベースＤＢＧ内に保存されている情報の全回転姿勢がＴＨ３方向をカバーしてない場合（ステップＳ６０５；Ｎｏ）、ステップＳ６１０に進む。

ステップＳ６０６では、画像情報取扱部１６は、画像先鋭度フラグデータベースＤＢＧ内の、画像先鋭度フラグが０である情報の割合Ｘを算出する。

算出した割合Ｘが所定の割合ＴＨ２より大きい場合（ステップＳ６０７；Ｙｅｓ）、自律移動装置１００の撮像部３３のレンズの汚れがあると判定する（ステップＳ６０８）。算出した割合Ｘが所定の割合ＴＨ２より大きくない場合（ステップＳ６０７；Ｎｏ）、自律移動装置１００の撮像部３３のレンズの汚れが不明であると判定する（ステップＳ６１０）。なお、ステップＳ６０１でＮｏの場合、ステップＳ６０５でＮｏの場合も、自律移動装置１００の撮像部３３のレンズの汚れが不明であると判定する（ステップＳ６１０）。

次に自律移動装置１００の起動時処理（図６）のステップＳ１０２で起動される移動指示アプリについて、図１２を用いて説明する。移動指示アプリは、行動計画アプリに設定された動作モードに応じて、自律移動装置１００の移動先を移動スレッドに通知する。動作モードには、自律移動装置１００を充電器２００に移動させる充電器帰還モード、自律移動装置１００をランダムに移動させるお散歩モード及び人とコミュニケーションを取る対話モードがある。他の動作モードを設定しても良い。

まず、制御部１０は、動作終了かを判定する（ステップＳ７０１）。動作終了なら（ステップＳ７０１；Ｙｅｓ）終了する。動作終了でないなら（ステップＳ７０１；Ｎｏ）動作モードを確認する（ステップＳ７０２）。行動計画アプリによって設定された動作モードが、充電器帰還モードならステップＳ７０３に進み、お散歩モードならステップＳ７０６に進み、対話モードならステップＳ７０８に進む。

動作モードが充電器帰還モードの場合、制御部１０は、充電器２００を自律移動装置１００の目的地に設定するように移動スレッドに通知する（ステップＳ７０３）。充電器帰還モードでは、充電器２００からの赤外線ビーコンである帰還信号を頼りに自律移動装置１００を充電器２００に移動させる。ステップＳ７０４に進み、制御部１０は、レンズ汚れ判定２を行う。レンズ汚れ判定２の詳細については後述する。

その後、制御部１０は、自律移動装置１００が目的地である充電器２００に到着したかどうかを判定する（ステップＳ７０５）。自律移動装置１００が充電器２００に到着していれば（ステップＳ７０５；Ｙｅｓ）、ステップＳ７０１に戻る。自律移動装置１００が充電器２００に到着していなければ（ステップＳ７０５；Ｎｏ）、ステップＳ７０４に戻る。

動作モードがお散歩モードの場合、制御部１０は、ランダムに目的地を移動スレッドに通知する。なお、目的地は、環境地図内の空いている空間や他のスレッドからの移動要求（例えば、スマホからのマニュアル動作等）等から決定しても良い（ステップＳ７０６）。

所定の時間ｔ経過後、ステップＳ７０７に進み、後述する移動スレッドが終了していれば（ステップＳ７０７；Ｙｅｓ）、ステップＳ７０１に戻る。移動スレッドが終了していなければ（ステップＳ７０７；Ｎｏ）、所定の時間ｔ経過後、ステップＳ７０７に戻る。

動作モードが対話モードの場合、制御部１０は、最新の画像内に顔領域を検出したかどうかを判定する（ステップＳ７０８）。画像内に顔領域を検出できれば（ステップＳ７０８；Ｙｅｓ）、制御部１０は、顔（検出）領域が撮像画像全体に行き渡るような方向に自律移動装置１００を移動させるように移動指示を移動スレッドに通知する（ステップＳ７０９）。画像内に顔領域を検出できない場合（ステップＳ７０８；Ｎｏ）、制御部１０は、ランダムに目的地を移動スレッドに通知する、或いは、環境地図内の空いている空間や他のスレッドからの移動要求（例えば、スマホからのマニュアル動作等）等から目的地を決定しても良い（ステップＳ７１１）。

ステップＳ７０９では、所定の時間ｔ経過後、ステップＳ７１０に進み、移動スレッドが終了していれば（ステップＳ７１０；Ｙｅｓ）、ステップＳ７０１に戻る。移動スレッドが終了していなければ（ステップＳ７１０；Ｎｏ）、所定の時間ｔ経過後、ステップＳ７１０に戻る。

ステップＳ７１１では、所定の時間ｔ経過後、ステップＳ７１２に進み、移動スレッドが終了していれば（ステップＳ７１２；Ｙｅｓ）、ステップＳ７０１に戻る。移動スレッドが終了していなければ（ステップＳ７１２；Ｎｏ）、所定の時間ｔ経過後、ステップＳ７１２に戻る。

次にステップＳ７０４で実行されるレンズ汚れ判定処理２について図１３を用いて説明する。この処理では、充電器２００に戻る途中、充電器２００まで所定の距離ＤＩＳＴの地点付近で撮像した画像の画像先鋭度値と、画像先鋭度値データベースＤＢ２に保存されている充電器２００まで所定の距離ＤＩＳＴの地点付近で撮像した画像の画像先鋭度値とを比較することにより撮像部３３の汚れがあるかを判定する。

まず、画像情報取扱部１６は、行動計画アプリ等から情報記憶部２３内にある画像先鋭度値データベースＤＢ２をクリアするように指示があるか否かを判定する（ステップＳ８０１）。クリアするように指示がある場合（ステップＳ８０１；Ｙｅｓ）、画像先鋭度値データベースＤＢ２をクリアし（ステップＳ８０２）、クリアするように指示がない場合（ステップＳ８０１；Ｎｏ）、ステップＳ８０３に進む。画像先鋭度値データベースＤＢ２をクリアするのは、撮像部３３のレンズが清掃されたと推定される時である。画像先鋭度値データベースＤＢ２をクリアする指示は、例えば、自律移動装置１００に備えられたスイッチをレンズ清掃後にユーザが押すことであっても良いし、自律移動装置１００がユーザと音声対話してレンズ清掃の確認ができたことであっても良い。

ステップＳ８０３では、制御部１０は、自律移動装置１００が充電器２００から帰還信号を受信中であるか及び、自律移動装置１００が充電器２００からの距離（或いは自律移動装置１００の走行距離）が所定の距離ＤＩＳＴから所定の誤差Ｅ０の範囲内であるかを障害物センサ４３よって判断する。自律移動装置１００が充電器２００から帰還信号を受信中であり、かつ、自律移動装置１００が充電器２００からの距離が所定の距離ＤＩＳＴから所定の誤差Ｅ０の範囲内である場合（ステップＳ８０３；Ｙｅｓ）、制御部１０は、撮像部３３に物体の境界等が写る画像を撮像させ、画像情報取扱部１６に、撮像により取得した画像に対して４近傍のラプラシアンオペレータの畳み込み処理を行わせた後、分散を取らせ、画像先鋭度値を算定させる（ステップＳ８０４）。自律移動装置１００が充電器２００から帰還信号を受信中ではない場合、或いは、自律移動装置１００が充電器２００からの距離が所定の距離ＤＩＳＴから所定の誤差Ｅ０の範囲内でない場合（ステップＳ８０３；Ｎｏ）、戻る。

ステップＳ８０５では、画像情報取扱部１６は、画像先鋭度値データベースＤＢ２に、過去にステップＳ８０４で算出された画像先鋭度値が保存されているかどうかを判定する。画像先鋭度値データベースＤＢ２に、過去にステップＳ８０４で算定された画像先鋭度値が保存されている場合（ステップＳ８０５；Ｙｅｓ）、画像情報取扱部１６は、保存されている画像先鋭度値に１未満の所定の係数ＴＨＨ３を掛けたものとステップＳ８０４で算出された画像先鋭度値とを比較する（ステップＳ８０６）。画像先鋭度値データベースＤＢ２に、過去にステップＳ８０４で算定された画像先鋭度値が保存されていない場合（ステップＳ８０５；Ｎｏ）、画像情報取扱部１６は、ステップＳ８０４で算出された画像先鋭度値を画像先鋭度値データベースＤＢ２に保存し（ステップＳ８０９）、戻る。

保存されている画像先鋭度値に所定の係数ＴＨＨ３を掛けたものよりステップＳ８０４で算出された画像先鋭度値が大きくない場合（ステップＳ８０６；Ｎｏ）、画像情報取扱部１６は、レンズの汚れがあると判定する。このとき、制御部１０は、レンズの汚れがあると行動計画アプリに通知する（ステップＳ８１０）。行動計画アプリは、自律移動装置１００に対して、ユーザに自律移動装置１００の撮像部３３のレンズの清掃を促す行動、例えば、レンズの清掃を促す音声又は警告音を発する、を実行するように指示し、戻る。保存されている画像先鋭度値に所定の係数ＴＨＨ３を掛けたものよりステップＳ８０４で算出された画像先鋭度値が大きい場合（ステップＳ８０６；Ｙｅｓ）、画像情報取扱部１６は、保存されている画像先鋭度値よりステップＳ８０４で算出された画像先鋭度値が大きいか否かを判定する（ステップＳ８０７）。

保存されている画像先鋭度値よりステップＳ８０４で算出された画像先鋭度値が大きい場合（ステップＳ８０７；Ｙｅｓ）、画像情報取扱部１６は、保存されている画像先鋭度値をステップＳ８０４で算出された画像先鋭度値に入れ替えて画像先鋭度値データベースＤＢ２に保存し（ステップＳ８０８）、戻る。保存されている画像先鋭度値よりステップＳ８０４で算出された画像先鋭度値が大きくない場合（ステップＳ８０７；Ｎｏ）、戻る。

以上、レンズ汚れ判定２の処理について説明した。次に、自律移動装置１００の起動時処理（図６）のステップＳ１０３で起動される移動スレッドについて、図１４を用いて説明する。このスレッドでは、移動指示アプリ等による移動指示に従って、自律移動装置１００を移動指示のあった方向や目的地まで移動させる。

制御部１０は、まず動作終了かどうかを判定し（ステップＳ９０１）、移動指示アプリ等から指示された方向や目的地に自律移動装置１００が移動済み等で動作終了なら（ステップＳ９０１；Ｙｅｓ）終了し、動作終了でないなら（ステップＳ９０１；Ｎｏ）、移動指示アプリ等から移動指示があるかを判定する（ステップＳ９０２）。

移動指示アプリ等から移動指示がある場合（ステップＳ９０２；Ｙｅｓ）、制御部１０は、移動指示アプリ等から指示された方向や目的地に自律移動装置１００を移動させた後（ステップＳ９０３）、ステップＳ９０１に戻る。移動指示アプリ等から移動指示が無い場合（ステップＳ９０２；Ｎｏ）、ステップＳ９０１に戻る。

以上、移動スレッドについて説明した。次に、自律移動装置１００の起動時処理（図６）のステップＳ１０３で起動される顔検出スレッドについて、図１５を用いて説明する。このスレッドでは、取得した画像に顔領域を検出した場合、移動指示アプリに通知し、撮像部３３のレンズの汚れ判定を行う。

制御部１０は、まず動作終了かどうかを判定し（ステップＳ１００１）、動作終了なら（ステップＳ１００１；Ｙｅｓ）終了し、動作終了でないなら（ステップＳ１００１；Ｎｏ）、撮像部３３に自律移動装置１００の前方を撮像させ、画像を取得する（ステップＳ１００２）。

ステップＳ１００３では、制御部１０は、画像内に人の顔の輪郭、目、鼻及び口等の形状に類似する形状を有する顔領域を検出したかを判定する。画像内に顔領域を検出した場合（ステップＳ１００３；Ｙｅｓ）、顔検出領域を移動指示アプリに通知する（ステップＳ１００４）。画像内に顔領域を検出しなかった場合（ステップＳ１００３；Ｎｏ）、ステップＳ１００１に戻る。

ステップＳ１００５では、制御部１０は、画像情報取扱部１６に、ステップＳ１００２で取得した画像の顔（検出）領域について画像先鋭度値を算出させることにより、顔領域先鋭度値データベースＤＢ３を生成（ＤＢを生成３）する。顔領域先鋭度値データベースＤＢ３の生成する処理の詳細については後述する。

そして、制御部１０は、画像先鋭度値を用いて撮像部３３のレンズの汚れを判定（レンズ汚れ判定３）する（ステップＳ１００６）。撮像部３３のレンズの汚れを判定する処理については後述する。

撮像部３３のレンズに汚れが有ると判定された場合（ステップＳ１００７；Ｙｅｓ）、制御部１０は、その旨を行動計画アプリに通知する（ステップＳ１００８）。行動計画アプリは、自律移動装置１００に対して、ユーザに自律移動装置１００の撮像部３３のレンズの清掃を促す行動、例えば、レンズの清掃を促す音声又は警告音を発する、を実行するように指示し、ステップＳ１００１に戻る。撮像部３３に汚れが有ると判定されなかった場合（ステップＳ１００７；Ｎｏ）、ステップＳ１００１に戻る。

次に、顔検出スレッド（図１５）のステップＳ１００５で実行されるＤＢを生成３の処理について、図１６を用いて説明する。この処理では撮像部３３で撮像した顔領域が検出された顔検出画像から、画像情報取扱部１６が、画像先鋭度値を計算して、顔検出領域と、画像を撮像した時刻とともに１つの情報Ｈとして生成し、情報記憶部２３に保存されている情報Ｈの顔検出領域に近い顔検出領域である画像から得られた情報Ｈ０の画像先鋭度値と情報Ｈの画像先鋭度値とを比較し、画像先鋭度値が高い情報を情報記憶部２３に保存する。

まず、画像情報取扱部１６は、行動計画アプリ等から情報記憶部２３内にある顔領域先鋭度値データベースＤＢ３をクリアするように指示があるか否かを判定する（ステップＳ１１０１）。クリアするように指示があるなら（ステップＳ１１０１；Ｙｅｓ）顔領域先鋭度値データベースＤＢ３をクリアし（ステップＳ１１０２）、クリアするように指示がないなら（ステップＳ１１０１；Ｎｏ）、ステップＳ１１０３に進む。顔領域先鋭度値データベースＤＢ３をクリアするのは、撮像部３３のレンズが清掃されたと推定される時である。顔領域先鋭度値データベースＤＢ３をクリアする指示は、画像先鋭度値データベースＤＢをクリアするときと同様に、自律移動装置１００に備えられたスイッチをレンズ清掃後にユーザが押すことであっても良いし、自律移動装置１００がユーザと音声対話してレンズ清掃の確認ができたことであっても良い。

ステップＳ１１０３では、画像情報取扱部１６は、顔検出スレッドのステップＳ１００２で取得した画像に対して４近傍のラプラシアンオペレータの畳み込み処理を行った後、分散を取り、画像先鋭度値とする。

次に、画像情報取扱部１６は、ステップＳ１００２で取得した顔検出領域と、この画像が撮像された時刻と、画像先鋭度値とをひとまとめにして、情報Ｈとする（ステップＳ１１０４）。

画像情報取扱部１６は、顔領域先鋭度値データベースＤＢ３内に情報Ｈの顔検出領域と所定の誤差ＴＨ２０の範囲内（基準領域内）の顔検出領域である過去の情報があるかどうかを検索する（ステップＳ１１０５）。所定の誤差ＴＨ２０の範囲内の顔検出領域である情報があった場合（ステップＳ１１０５；Ｙｅｓ）、その情報を情報Ｈ０として取り出し、無かった場合（ステップＳ１１０５；Ｎｏ）、ステップＳ１１０８へ進む。なお、所定の誤差ＴＨ２０の範囲内の顔検出領域の情報が複数ある場合には、１番顔検出領域が近いものを選ぶ。領域間の距離は、ユークリッド距離を用いる。領域の開始点（ｘ、ｙ）と、領域の大きさ（ｗ、ｈ）の４次元空間のユークリッド距離とすることができる。

次に、画像情報取扱部１６は、情報Ｈの画像先鋭度値と情報Ｈ０の画像先鋭度値を比較する（ステップＳ１１０６）。情報Ｈの画像先鋭度値が情報Ｈ０の画像先鋭度値よりも大きい場合（ステップＳ１１０６；Ｙｅｓ）、顔領域先鋭度値データベースＤＢ３内の情報Ｈ０を削除する（ステップＳ１１０７）。情報Ｈの画像先鋭度値が情報Ｈ０の画像先鋭度値よりも大きくない場合（ステップＳ１１０６；Ｎｏ）、そのまま終了する。

ステップＳ１１０８では、画像情報取扱部１６は、顔領域先鋭度値データベースＤＢ３に情報Ｈを登録し、終了する。

次に、顔検出スレッド（図１５）のステップＳ１００６で実行されるレンズ汚れ判定３の処理について、図１７を用いて説明する。この処理では、ＤＢを生成３の処理で算定した情報Ｈの先鋭度値と、顔領域先鋭度値データベースＤＢ３内の情報の画像先鋭度値の平均値ＨＭ等を使用することにより、自律移動装置１００の撮像部３３のレンズの汚れを判定する。

まず、画像情報取扱部１６は、顔領域先鋭度値データベースＤＢ３内に保存されている情報の数が所定の値Ｎ以上であるかを判定する（ステップＳ１２０１）。顔領域先鋭度値データベースＤＢ３内に保存されている情報の数がＮ以上である場合（ステップＳ１２０１；Ｙｅｓ）、ステップＳ１２０２に進む。顔領域先鋭度値データベースＤＢ３内に保存されている情報の数がＮ以上でない場合（ステップＳ１２０１；Ｎｏ）、ステップＳ１２１１に進む。

ステップＳ１２０２では、画像情報取扱部１６は、顔領域先鋭度値データベースＤＢ３内に保存されている情報の画像先鋭度値の平均値ＨＭを算出する。

画像情報取扱部１６は、顔領域先鋭度値データベースＤＢ３内に保存されている情報の画像先鋭度値の平均値ＨＭに１未満の所定の係数ＴＨＨ２を掛けたものより情報Ｈの画像先鋭度値が大きいかを判定する（ステップＳ１２０３）。平均値ＨＭにＴＨＨ２を掛けたものより情報Ｈの画像先鋭度値が大きい場合（ステップＳ１２０３；Ｙｅｓ）、画像先鋭度フラグを１とし、現時刻、顔検出領域とともに顔領域先鋭度フラグデータベースＤＢ３Ｈ内に保存し（ステップＳ１２０４）、ステップＳ１２０６に進む。平均値ＨＭにＴＨＨ２を掛けたものより情報Ｈの先鋭度値が大きくない場合（ステップＳ１２０３；Ｎｏ）、画像先鋭度フラグを０とし、現時刻、顔検出領域とともに顔領域先鋭度フラグデータベースＤＢ３Ｈ内に保存し（ステップＳ１２０５）、ステップＳ１２０６に進む。

次に、画像情報取扱部１６は、顔領域先鋭度フラグデータベースＤＢ３Ｈ内の、現時刻より所定の時間Ｔ１１以上古い情報を削除する（ステップＳ１２０６）。

画像情報取扱部１６は、顔領域先鋭度フラグデータベースＤＢ３Ｈ内に保存されている情報の数が所定の値Ｎ２以上であり、かつ、顔領域先鋭度フラグデータベースＤＢ３Ｈ内の全情報で顔検出領域全体のうちの所定の領域ＴＨ１３以上をカバーしているかを判定する（ステップＳ１２０７）。顔領域先鋭度フラグデータベースＤＢ３Ｈ内に保存されている情報の数がＮ２以上であり、かつ、顔領域先鋭度フラグデータベースＤＢ３Ｈ内の全情報で顔検出領域全体のうちの所定の領域ＴＨ１３以上をカバーしている場合（ステップＳ１２０７；Ｙｅｓ）、ステップＳ１２０８に進む。顔領域先鋭度フラグデータベースＤＢ２Ｈ内に保存されている情報の数が所定の値Ｎ２以上でない、又は、顔領域先鋭度フラグデータベースＤＢ３Ｈ内の全情報で顔検出領域全体のうちの所定の領域ＴＨ１３以上をカバーしていない場合（ステップＳ１２０７；Ｎｏ）、ステップＳ１２１１に進む。

ステップＳ１２０８では、画像情報取扱部１６は、顔領域先鋭度フラグデータベースＤＢ３Ｈ内の、先鋭度フラグが０である情報の割合Ｘ２を算出する。

算出した割合Ｘ２が所定の割合ＴＨ１２より大きい場合（ステップＳ１２０９；Ｙｅｓ）、自律移動装置１００の撮像部３３のレンズの汚れがあると判定し（ステップＳ１２１０）、終了する。算出した割合Ｘ２が所定の割合ＴＨ１２より大きくない場合（ステップＳ１２０９；Ｙｅｓ）、自律移動装置１００の撮像部３３のレンズの汚れが不明であると判定し（ステップＳ１２１１）、終了する。また、ステップＳ１２０１でＮｏの場合、ステップＳ１２０７でＮｏの場合も、自律移動装置１００の撮像部３３のレンズの汚れが不明であると判定し（ステップＳ１２１１）、終了する。

以上説明したように、自律移動装置１００は、ＤＢを生成１・３の処理及びレンズ汚れ判定１～３の処理において、画像先鋭度値をラプラシアンオペレータの畳み込み処理を行った後、分散を取るので、画像全体又は顔画像領域内のエッジ及び濃度を算定でき、撮像部のレンズの一部に汚れがあった場合でもレンズの汚れを好適に検出することができる。

そして、検出したレンズの汚れをユーザに知らせることにより、好ましいタイミングで、ユーザが汚れたレンズを清掃することができ、画像処理による自機位置推定及び環境地図生成のロバスト性と精度を維持することができる。

上記実施形態では、画像先鋭度値の算出にラプラシアンオペレータと分散を使用したが、離散フーリエ変換（Ｄｉｓｃｒｅｔｅ Ｆｏｕｒｉｅｒ Ｔｒａｎｓｆｏｒｍ）や高速フーリエ変換（Ｆａｓｔ Ｆｏｕｒｉｅｒ Ｔｒａｎｓｆｏｒｍ）をはじめとするフーリエ変換等の周波数解析（法）を用いても良い。フーリエ変換を用いた場合、例えばフーリエ変換後のパワースペクトルのうち最もパワー（周波数成分の振幅の２乗）が大きい周波数を画像先鋭度値（代表先鋭度値）とする。ラプラシアンオペレータの代わりに、ロバーツ、プリビットやソーベル等のエッジ検出オペレータでエッジ検出しても良い。

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

ＤＢを生成１及びレンズ汚れ判定１の処理では、撮像画像全体を使用して画像の判定を行っているが、自律移動装置１００の下方（床近辺）は物の配置の変化が起こりやすく画像が変わりやすいので、物の配置の変化等が起こりにくい一定の高さ以上の部分の画像を使用して、判定を行っても良い。

レンズ汚れ判定１の処理では、回転姿勢を条件に用いたが、位置を用いても良い。或いは、環境地図をＱ個のブロックに分け、そのうちのいくつかのブロック内の画像を有していることを条件としても良いし、位置及び回転の両方を条件としても良い。条件を満たしていない場合、条件を満たすような移動指示を発するようにしても良い。

顔検出スレッド、ＤＢを生成３及びレンズ汚れ判定３では、顔を検出対象としていたが、顔以外の物体、例えば、衣服や眼鏡、ペット等を検出対象としても良く、特定の個人の顔のみを検出対象としても良く、ＴＶ検出としても良い。

上記実施形態では、カメラを用いるＶｉｓｕａｌ ＳＬＡＭを用いたが、カメラの代わりにＬＲＦ（Ｌａｓｅｒ Ｒａｎｇｅ Ｆｉｎｄｅｒ）を用いたＳＬＡＭであっても良い。この場合、物体認識用のカメラについての汚れ検出を行う。

行動計画アプリの動作モードは、ユーザからのマニュアル操作で選択しても良く、所定のアルゴリズム、例えば、時間や人を認識した数などにより自動でモード選択をしても良い。

１つの画像先鋭度値は、１回の撮像により得られた画像又は物体検出領域から算定されるが、２回以上の撮像により得られた画像から１つの画像先鋭度値を算定しても良い。また、画像先鋭度値は、１つの画像又は物体検出領域から代表先鋭度値が１つ算定されるが、１つの画像又は物体検出領域のうちの一定の範囲毎に１つづつ算定されるようにしても良い。

汚れ検査用のマーカーシートを撮像することにより得られる画像から画像先鋭度値を算定しても良い。マーカーシートは、自律移動装置１００の移動範囲内に、必要な時に配置すれば良い。マーカーシートは、汚れ検出が容易な模様、例えば市松模様等がデザインされていると良い。

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

ただし、この場合の二つの撮像部間の距離は、通常用いられる基準並進距離（例えば１ｍ）よりも短い距離になってしまうことが多く、オドメトリから得られる並進距離の精度が高い場合は、むしろ単眼ＳＬＡＭと同様の初期化処理（例えば複数ある撮像部のうちの第一撮像部のみを使った初期化処理）を行った方が高い精度が得られることもある。そこで、単眼ＳＬＡＭと同様の初期化処理と、前段落で記載した二つの撮像部による初期化処理とを両方とも行って、誤差の小さいと思われる情報を採用するようにしても良い。

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

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

（付記１）
自機を移動及び回転させて自機位置及び自機の向きを変化させる駆動部と、
レンズを介して撮像する撮像部と、
撮像された画像を記憶する画像記憶部と、
前記駆動部、前記撮像部及び前記画像記憶部を制御し、前記画像又は前記画像に検出された物体の物体検出領域を構成する構造の細かさを示す画像先鋭度値を算定する制御部と、を備え、
前記制御部は、
前記画像記憶部に記憶された第１の画像の第１の画像先鋭度値と、
前記第１の画像が撮像された時の前記自機位置及び前記自機の向きから所定の範囲内の自機位置及び自機の向きで撮像された第２の画像の第２の画像先鋭度値とを、
又は前記第１の画像により検出された物体の第１の物体検出領域から基準領域内にある前記物体の第２の物体検出領域を含んだ第２の画像の第２の画像先鋭度値とを、算定し、前記第２の画像先鋭度値が前記第１の画像先鋭度値より低い場合、前記レンズが汚れていると判定する、
自律移動装置。

（付記２）
前記画像先鋭度値は、前記画像全体又は前記物体検出領域全体に対して算出された代表先鋭度値である、
付記１に記載の自律移動装置。

（付記３）
前記制御部は、前記画像又は前記検出領域をエッジ検出オペレータで処理し分散を取る、又は前記画像又は前記検出領域に対して周波数解析を行うことで得たパワースペクトルから最大のパワーの周波数を取得する、ことにより前記代表先鋭度値を算定する、
付記２に記載の自律移動装置。

（付記４）
前記撮像部は、物体の境界が写るように前記画像を撮像し、
前記制御部は、前記画像に写る前記物体の境界を前記エッジ検出オペレータで処理し分散を取る、又は前記周波数解析を行うことで得たパワースペクトルから最大のパワーの周波数を取得する、
付記３に記載の自律移動装置。

（付記５）
前記制御部は、前記撮像部によって前記物体の境界が写るように前記画像を撮像できる自機位置及び自機の向きに移動するように前記駆動部を制御する、
付記４に記載の自律移動装置。

（付記６）
前記物体は、顔である、
付記１から５のいずれか１つに記載の自律移動装置。

（付記７）
前記第１の画像及び前記第２の画像は、それぞれ２回以上の撮像により取得される、
付記１から６のいずれか１つに記載の自律移動装置。

（付記８）
前記第２の画像は、充電設備に向かう際に撮像されることにより取得される、
付記１から７のいずれか１つに記載の自律移動装置。

（付記９）
距離測定素子をさらに備え、
前記自機の走行距離が所定の距離に達したと前記距離測定素子によって測定された時に前記第２の画像を撮像する、
付記１から８のいずれか１つに記載の自律移動装置。

（付記１０）
前記充電設備に向かう時に前記レンズの汚れを確認する、
付記８に記載の自律移動装置。

（付記１１）
自機を移動及び回転させて自機位置及び自機の向きを変化させる駆動部と、レンズを介して撮像する撮像部と、前記撮像された画像を記憶する画像記憶部と、を備えた自律移動装置のレンズの汚れ検出方法であって、
第１の画像が撮像された時の自機位置及び自機の向きを記憶しておき、自機を移動及び回転させて前記第１の画像を撮影した時の自機位置及び自機の向きと所定の範囲内にある時に、第２の画像を撮像させ、前記画像記憶部に記憶された過去の画像である第１の画像と、現在の画像である前記第２の画像とを比較して、前記レンズの汚れを検出する、自律移動装置のレンズの汚れ検出方法。

（付記１２）
自機を移動及び回転させて自機位置及び自機の向きを変化させる駆動部と、レンズを介して撮像する撮像部と、撮像された画像を記憶する画像記憶部と、を備えた自律移動装置のレンズの汚れ検出方法であって、
自機を移動及び回転させて、前記画像記憶部に記憶されている第１の画像により検出された物体の物体検出領域から基準領域内にある前記物体の物体検出領域を含んだ第２の画像を撮像させ、前記画像記憶部に記憶された過去の画像である前記第１の画像と、現在の画像である前記第２の画像とを比較して、前記レンズの汚れを検出する、自律移動装置のレンズの汚れ検出方法。

（付記１３）
画像記憶部に記憶された第１の画像が撮像された時の自機位置及び自機の向きから所定の範囲内の自機位置及び自機の向き、又は前記第１の画像により検出された物体の第１の物体検出領域から基準領域内にある前記物体の第２の物体検出領域を含んだ第２の画像を撮像する自機位置及び自機の向きに駆動部によって自機を移動及び回転させることにより、撮像部に第２の画像を撮像させる撮像ステップと、
前記駆動部、前記撮像部及び前記画像記憶部を制御する制御部が、前記第１の画像の第１の画像先鋭度値と前記第２の画像の第２の画像先鋭度値とを算定する算定ステップと、
前記第２の画像先鋭度値が前記第１の画像先鋭度値より低い場合、前記制御部は、前記撮像部のレンズが汚れていると判定する判定ステップと、
を備える自律移動装置のレンズの汚れ検出方法。

（付記１４）
自機を移動及び回転させて自機位置及び自機の向きを変化させる駆動部と、
レンズを介して画像を撮像する撮像部と、
撮像された前記画像を記憶する画像記憶部と、
前記駆動部、前記撮像部及び前記画像記憶部を制御してＶｉｓｕａｌ ＳＬＡＭ制御を行なう制御部と、
を備えた自律移動装置のレンズの汚れ検出方法であって、
前記画像記憶部に記憶された第１の画像の特徴点と対応する前記撮像部で撮像した第２の画像の対応特徴点の数が、基準対応特徴点数未満である場合をＶｉｓｕａｌ ＳＬＡＭ制御の失敗とし、
基準並進距離を並進する間における前記Ｖｉｓｕａｌ ＳＬＡＭ制御の失敗率が基準失敗率より高い場合、レンズが汚れていると判定する、自律移動装置のレンズの汚れ検出方法。

（付記１５）
コンピュータに、
画像記憶部に記憶された第１の画像が撮像された時の自機位置及び自機の向きから所定の範囲内の自機位置及び自機の向き、又は前記第１の画像により検出された物体の第１の物体検出領域から基準領域内にある前記物体の第２の物体検出領域を含んだ第２の画像を撮像する自機位置及び自機の向きに駆動部によって自機を移動及び回転させることにより、撮像部に第２の画像を撮像させる撮像ステップと、
前記駆動部、前記撮像部及び前記画像記憶部を制御する制御部が、前記第１の画像の第１の画像先鋭度値と前記第２の画像の第２の画像先鋭度値とを算定する算定ステップと、
前記第２の画像先鋭度値が前記第１の画像先鋭度値より低い場合、前記制御部は、前記撮像部のレンズが汚れていると判定する判定ステップと、
を実行させるためのプログラム。

１０…制御部、１１…位置計測部、１２…地図作成部、１３…位置推定部、１４…判定部、１５…地図編集部、１６…画像情報取扱部、２０…記憶部、２１…画像記憶部、２２…地図記憶部、２３…情報記憶部、３１，３１ａ，３１ｂ…帰還信号受信部、３２，３２ａ，３２ｂ…駆動部、３３…撮像部、３４…通信部、３５…充電接続部、４０…センサ部、４１…加速度センサ、４２…角速度センサ、４３…障害物センサ、５１，５１ａ，５１ｂ…帰還信号送信部、５２…電力供給部、５３，５３ａ，５３ｂ…受信可能範囲、１００…自律移動装置、２００…充電器

Claims (3)

1. 自機を移動及び回転させて自機位置及び自機の向きを変化させる駆動部と、
   レンズを介して画像を撮像する撮像部と、
   撮像された前記画像を記憶する画像記憶部と、
   前記駆動部、前記撮像部及び前記画像記憶部を制御してＶｉｓｕａｌ ＳＬＡＭ制御を行なう制御部と、を備え、
   前記制御部は、
   前記画像記憶部に記憶された第１の画像の特徴点と対応する、基準並進距離を並進する間の前記撮像部で撮像した第２の画像の対応特徴点の数が、基準対応特徴点数未満である場合をＶｉｓｕａｌ ＳＬＡＭ制御の失敗として自機の位置又は向きの少なくとも何れかの推定を行わず、
   前記基準並進距離を並進する間における前記Ｖｉｓｕａｌ ＳＬＡＭ制御の失敗回数と前記第２の画像の対応特徴点が基準対応特徴点数未満かどうかを判定した回数とに基づく失敗率が基準失敗率より高い場合、前記レンズが汚れていると判定する、
   自律移動装置。
2. 自機を移動及び回転させて自機位置及び自機の向きを変化させる駆動部と、レンズを介して画像を撮像する撮像部と、撮像された前記画像を記憶する画像記憶部と、前記駆動部、前記撮像部及び前記画像記憶部を制御してＶｉｓｕａｌ ＳＬＡＭ制御を行なう制御部と、を備えた自律移動装置のレンズの汚れ検出方法であって、
   前記制御部は、前記画像記憶部に記憶された第１の画像の特徴点と対応する、基準並進距離を並進する間の前記撮像部で撮像した第２の画像の対応特徴点の数が、基準対応特徴点数未満である場合をＶｉｓｕａｌ ＳＬＡＭ制御の失敗として自機の位置又は向きの少なくとも何れかの推定を行わず、
   前記基準並進距離を並進する間における前記Ｖｉｓｕａｌ ＳＬＡＭ制御の失敗回数と前記第２の画像の対応特徴点が基準対応特徴点数未満かどうかを判定した回数とに基づく失敗率が基準失敗率より高い場合、前記レンズが汚れていると判定する、自律移動装置のレンズの汚れ検出方法。
3. 自機を移動及び回転させて自機位置及び自機の向きを変化させる駆動部と、レンズを介して画像を撮像する撮像部と、撮像された前記画像を記憶する画像記憶部と、を備えた自律移動装置が備えるコンピュータに、
   前記画像記憶部に記憶された第１の画像の特徴点と対応する、基準並進距離を並進する間の前記撮像部で撮像した第２の画像の対応特徴点の数が、基準対応特徴点数未満である場合をＶｉｓｕａｌ ＳＬＡＭ制御の失敗として自機の位置又は向きの少なくとも何れかの推定を行わず、
   前記基準並進距離を並進する間における前記Ｖｉｓｕａｌ ＳＬＡＭ制御の失敗回数と前記第２の画像の対応特徴点が基準対応特徴点数未満かどうかを判定した回数とに基づく失敗率が基準失敗率より高い場合、前記レンズが汚れていると判定する、判定ステップを実行させるためのプログラム。

Images