以下、本発明の実施形態に係る自律移動装置について、図表を参照して説明する。なお、図中同一又は相当部分には同一符号を付す。
本発明の実施形態に係る自律移動装置は、地図(環境地図)を作成しながら、用途に応じて自律的に移動する装置である。この用途とは、例えば、人とコミュニケーションを行う事、警備監視、屋内掃除等である。また、本実施形態に係る自律移動装置は、後述する動作モードに応じて移動する。
自律移動装置100は、図1に示すように、外観上、帰還信号受信部31(31a,31b)、駆動部32(32a,32b)、撮像部33、充電接続部35、を備える。なお、図1には示されていないが、自律移動装置100は、周囲に存在する物体(障害物)を検出する障害物センサを備えている。また、自律移動装置100の電池を充電するための充電設備である充電器200は、図2に示すように、外観上、帰還信号送信部51(51a,51b)、電力供給部52を備える。
自律移動装置100の充電接続部35と充電器200の電力供給部52とが接続することにより、充電器200から電力の供給を受けて、自律移動装置100は内蔵している充電池を充電することができる。なお、充電接続部35及び電力供給部52は、それぞれがお互いと接続するための接続端子である。これらは、駆動部32により自律移動装置100が充電器200の上に移動することによって、接続される。この接続は、充電接続部35と電力供給部52とが接触することによって接続してもよいし、充電接続部35と電力供給部52とが近接することによって電磁誘導等により接続してもよい。
撮像部33は、自律移動装置100の前方から上部までの広範囲を撮像できる広角レンズを備える。このため、撮像部33は天井の電灯が点灯しているか否かを判定可能な画像を撮像することができる。そして、自律移動装置100は、撮像部33が撮像した画像を利用して単眼SLAM(Simultaneous Localization And Mapping)処理を行うことができる。
自律移動装置100の帰還信号受信部31は、充電器200から送信される帰還信号(赤外線ビーコン)を受信するためのデバイスである。自律移動装置100の正面に向かって左に帰還信号受信部31a、右に帰還信号受信部31bと、合計2つの帰還信号受信部31が備えられている。また、充電器200の帰還信号送信部51は、自律移動装置100に帰還信号を送信するためのデバイスである。充電器200の正面に向かって右に帰還信号送信部51aが、左に帰還信号送信部51bが、それぞれ備えられている。そして、帰還信号送信部51aから送信される帰還信号と、帰還信号送信部51bから送信される帰還信号とは、異なる信号になっている。したがって、帰還信号受信部31は、各々左右どちらの帰還信号送信部51からの帰還信号を受信したのかを判別することができる。
充電器200の帰還信号送信部51から送信される帰還信号の左右それぞれの受信可能範囲53(53a,53b)の一例を図3に示す。帰還信号送信部51aから送信される帰還信号は、受信可能範囲53aの内部に自律移動装置100の帰還信号受信部31が入ると受信できる。そして、帰還信号送信部51bから送信される帰還信号は、受信可能範囲53bの内部に自律移動装置100の帰還信号受信部31が入ると受信できる。したがって、自律移動装置100は、受信可能範囲53の内部に入ると充電器200の存在する方向を把握することができる。そして、自律移動装置100は、帰還信号送信部51aからの帰還信号を帰還信号受信部31aが受信し、帰還信号送信部51bからの帰還信号を帰還信号受信部31bが受信するように向きを調整しながら進むことによって、充電器200の上に移動することができる。自律移動装置100は、充電器200の上に移動すると、充電接続部35と電力供給部52とが接続し、自律移動装置100に内蔵されている電池を充電することができる。
駆動部32は、独立2輪駆動型であって、車輪とモータとを備える移動手段である。自律移動装置100は、2つの車輪の同一方向駆動により前後の平行移動(並進移動)を、2つの車輪の逆方向駆動によりその場での回転(向き変更)を、2つの車輪のそれぞれ速度を変えた駆動により旋回移動(並進+回転(向き変更)移動)を、行うことができる。また、各々の車輪にはロータリエンコーダが備えられており、ロータリエンコーダで車輪の回転数を計測し、車輪の直径や車輪間の距離等の幾何学的関係を利用することで並進移動量及び回転量を計算できる。例えば、車輪の直径をD、回転数をR(ロータリエンコーダにより測定)とすると、その車輪の接地部分での並進移動量はπ・D・Rとなる。また、車輪の直径をD、車輪間の距離をI、右車輪の回転数をRR、左車輪の回転数をRLとすると、向き変更の回転量は(右回転を正とすると)360°×D×(RL-RR)/(2×I)となる。この並進移動量及び回転量を逐次足し合わせていくことで、駆動部32は、いわゆるオドメトリ(メカオドメトリ)として機能し、自機位置(移動開始時の位置及び向きを基準とした自律移動装置100の位置及び向き)を計測することができる。駆動部32は、撮像部33によって物体の境界等が写るように画像を撮像できる自機の位置及び向きに自律移動装置100を移動させる。
オドメトリから得られる自機の位置及び向きの精度は、車輪の摩耗やスリップ等により、低い精度になってしまうことが多い。特に誤差が累積していく関係上、時間経過と共に精度が悪化する。ただし、オドメトリの回転成分(向きの情報)に関しては、後述する角速度センサ情報を用いることで精度を向上させることができる。また、地磁気を検出して方位を特定する方位センサ(図示せず)を用いることで、オドメトリからの取得値とは関係なく、地磁気を利用した絶対的な向きの情報を取得することもできる。
なお、車輪の代わりにクローラを備えるようにしても良いし、複数(例えば二本)の足を備えて足で歩行することによって移動を行うようにしても良い。これらの場合も、二つのクローラの動きや、足の動きに基づいて、車輪の場合と同様に自機の位置や向きの計測が可能である。
図4に示すように、自律移動装置100は、帰還信号受信部31(31a,31b)、駆動部32(32a,32b)、撮像部33、充電接続部35、に加えて、制御部10、記憶部20、通信部34、センサ部40、を備える。
制御部10は、CPU(Central Processing Unit)等で構成され、記憶部20に記憶されたプログラムを実行することにより、後述する各部(位置計測部11、地図作成部12、位置推定部13、判定部14、地図編集部15、画像情報取扱部16)の機能を実現する。
記憶部20は、ROM(Read Only Memory)、RAM(Random Access Memory)等で構成され、画像記憶部21や地図記憶部22を含む。ROMには制御部10のCPUが実行するプログラム(例えば、後述するSLAM法の演算や自律移動制御処理に係るプログラム)や、プログラムを実行する上で予め必要なデータが記憶されている。RAMには、プログラム実行中に作成されたり変更されたりするデータが記憶される。
画像記憶部21には、撮像部33が撮影した画像が記憶される。ただし、記憶容量の節約のために、撮影したすべての画像を記憶しなくても良く、また画像自体ではなく、画像の特徴量を記憶するようにしても良い。重要な画像(後述するキーフレーム)については、画像の情報と共に、上述したオドメトリで計測した自機位置(自機の位置及び向き)の情報並びに後述するSLAM法で推定した自機位置(自機の位置及び向き)の情報が記憶される。
地図記憶部22には、後述するSLAM法や障害物センサ43からの情報に基づいて地図作成部12が作成した地図(特徴点や障害物の3次元位置の情報)が記憶される。
情報記憶部23には、画像先鋭度値データベースDB,DB2、画像先鋭度フラグデータベースDBG、顔領域先鋭度値データベースDB3、顔領域先鋭度フラグデータベースDB3H、等があり、画像情報取扱部16が生成した後述する情報G(図5(a)),G1(図5(b)),H(図5(c)),H1(図5(d))等が記憶される。
センサ部40として、加速度センサ41、角速度センサ42、障害物センサ43を備える。加速度センサ41は、XYZ方向(3軸)の加速度を測定するセンサである。角速度センサ42は角速度(単位時間あたりの角度移動量)を測定するセンサである。車輪の回転数に基づいて自機の向きを求めるよりも、角速度センサ42を用いて向きを求める方が精度が向上することが知られている。障害物センサ(距離測定素子)43は、走行中の障害物を検知するセンサであって、例えば、赤外線センサ、超音波センサである。なお、独立した障害物センサを搭載せずに、撮像部33を用いて障害物を検知するようにしても良い。また、他の物体に衝突したことを検知するバンパーセンサー(図示せず)を備えても良い。また、地磁気を検出して方位を特定する方位センサ(図示せず)を備えても良い。方位センサを用いることで、オドメトリからの取得値とは関係なく、地磁気を利用した絶対的な向きの情報を取得することができる。
通信部34は、充電器200等の外部装置と通信するためのモジュールであり、外部装置と無線通信する場合にはアンテナを含む無線モジュールである。例えば、通信部34は、Bluetooth(登録商標)に基づく近距離無線通信を行うための無線モジュールである。通信部34を用いることにより、自律移動装置100は、外部装置とデータ受け渡し等を行うことができる。
次に、制御部10の機能について説明する。制御部10は、位置計測部11、地図作成部12、位置推定部13、判定部14、地図編集部15、画像情報取扱部16を含み、後述するSLAM法の演算、自律移動装置100の移動指示や画像先鋭度値の算定等を行う。また、制御部10は、マルチスレッド機能に対応しており、複数のスレッド(異なる処理の流れ)を並行して進めることができる。
位置計測部11は、駆動部32の車輪とモータの動きに基づき、自機位置を計測する。具体的には、地面に高低差がなく、車輪がスリップしないという前提のもとで、各車輪の直径(D)及び回転数(R:ロータリエンコーダにより測定)に基づき、その車輪の接地部分での移動距離はπ・D・Rとして求められるので、これらと車輪間の距離とから並進移動量及び並進方向並びに向きの変化量(回転角度)を求めることができる。これを逐次加算していくことで、オドメトリとして、自機の位置及び向きを計測できる。地面に高低差がある場合は高さ方向を考慮した並進移動量を求める必要があるが、これは、加速度センサ41により高度の変化量を把握することにより求められる。また車輪のスリップに関しては、駆動用の車輪以外に移動量計測用の車輪も搭載しておくことで誤差を小さくすることが可能である。
地図作成部12は、画像記憶部21に記憶されている画像の情報並びに該画像撮影時の自機の位置及び向きの情報に基づいてSLAM法を用いて推定された特徴点の3次元位置(Map点)や、障害物センサ43で障害物を検知したときの自機の位置及び向きの情報に基づいて得られた該障害物の3次元位置等を、地図の情報として地図記憶部22に記憶する。
位置推定部13は、後述するSLAM法に基づいて、ビジュアルオドメトリとして、自機の位置及び向きを推定する。
判定部14は、位置計測部11で計測した位置及び向きと、位置推定部13で推定した位置及び向きとの差が所定誤差内か否かを判定する。オドメトリ(位置計測部11)で正しい位置及び向きが計測でき、しかも、SLAM法(ビジュアルオドメトリ:位置推定部13)で正しい位置及び向きを推定できているなら、これらはほぼ一致するので、その場合は地図記憶部22に記憶されている地図の情報も正しいと推測できる。しかし、位置及び向きの計測や推定に誤差が含まれていると、誤差の累積が生じ、計測値と推定値とは一致しなくなる。そのような場合は、地図記憶部22に記憶されている地図の情報にも誤差が累積されてしまっていると推測できる。したがって、これら両者の差が所定誤差内か否かを判定することで、地図の情報が正しいかどうかを判定することができる。
地図編集部15は、判定部14によって地図記憶部22に記憶されている地図の情報が正しくないと判定された場合は、該地図を編集する。地図の編集の最も簡単な処理は、該地図を消去する処理である。
画像情報取扱部16は、撮像部33によって撮像された画像(物体の境界等)に対してラプラシアンオペレータの畳み込み処理を行い、その後分散を取ることにより、画像(又は後述する画像内に検出された顔等の物体の検出領域)の画像先鋭度値(シャープネス)を算出する。画像先鋭度値は、画像全体又は画像内に検出された物体の物体検出領域全体に対して算出された1つの値(代表先鋭度値)である。画像先鋭度値は、画像又は物体検出領域を構成する構造の細かさを示す値である。ラプラシアンオペレータは、隣接する上下左右の4画素(4近傍)についての2次微分を行うものを使用するが、斜め方向に隣接する8画素(8近傍)について2次微分を行うものとしても良い。その他、画像から取得した後述の情報の各種算定等を行う。
以上、自律移動装置100の機能構成について説明した。次に、自律移動装置100で起動される各種処理について説明する。自律移動装置100は、電源オフ時は充電器200に接続されて充電されており、電源が投入されると、充電器200に接続された位置で、後述する起動時処理が実行され、各種アプリ及び各種スレッドが並行して実行される。では、自律移動装置100の起動時の処理について、図6を参照して説明する。図6は、自律移動装置100が起動した時に実行される起動時処理のフローチャートである。
まず、自律移動装置100の制御部10は、SLAM処理用の各種スレッドを起動する(ステップS101)。SLAM処理用の各種スレッドとは、自律移動装置100の姿勢(位置及び向き)を推定する自機位置推定処理及び環境地図を生成する処理を行う自機位置推定、環境地図生成スレッド、ループクロージング処理を行うループクロージングスレッド等である。なお、ループクロージング処理とは、自律移動装置100が、以前来たことのある同じ場所に戻ってきたことを認識した場合に、以前この同じ場所にいた時の自律移動装置100の姿勢の値と現在の姿勢の値とのずれを用いて、以前来た時から今までの移動軌跡中のキーフレーム(の3D姿勢)や、関連するMapPoint(の3D座標)を修正する処理をいう。自機位置推定、環境地図作成スレッドの詳細については後述する。
次に、制御部10は、移動指示アプリ及び行動計画アプリを起動する(ステップS102)。行動計画アプリ(図示せず)は、自律移動装置100の行動を制御するアプリである。行動計画アプリでは、自律移動装置100の動作モード(充電器帰還モード、お散歩モード、対話モード)の設定を行うことができる。また、行動計画アプリは、環境地図内での自律移動装置100の位置を理解し、自律移動装置100の移動先を移動スレッドに指示することができる。さらに、行動計画アプリは、撮像部33のレンズに汚れが有るとの通知を受けた際に、自律移動装置100にユーザに対して撮像部33のレンズの清掃を促す行動を指示する。移動指示アプリは、行動計画アプリの設定に応じた動作モード(例えば充電器帰還モード)に基づいて、次のステップで起動される自機位置推定、環境地図生成スレッドから自律移動装置100の現在の環境地図内における姿勢(位置及び向き)の情報を受けた行動計画アプリの指示する移動先への移動を実行するアプリである。なお、移動指示アプリの詳細については後述する。
次に、制御部10は、移動スレッド、顔検出スレッドを起動し(ステップS103)、起動時処理を終了する。移動スレッドは、移動指示アプリから移動命令を受けて、制御部10が駆動部32を制御して自律移動装置100を移動させる処理を行うスレッドである。そして、起動時処理を終了した後は、自律移動装置100は、この起動時処理によって起動された各アプリ及びスレッドにより制御される。なお、移動スレッド、顔検出スレッドの詳細については後述する。
SLAM処理用のスレッドのうち、自機位置推定、環境地図生成スレッドについて、図7を参照して説明する。自機位置推定、環境地図生成スレッドは、撮像部33により撮像して取得した画像を利用したVisual SLAM法による環境地図を用いて追跡処理(自機位置推定処理)を行うスレッドである。さらに、自機位置推定、環境地図生成のために取得した画像を利用して、撮像部33のレンズの汚れの有無を判定するスレッドでもある。
まず、制御部10は、SLAM法に基づいて自機位置推定、環境地図の生成を行う(ステップS201)。これにより、地図の情報及びビジュアルオドメトリ(地図と画像とを用いて推定した自機位置の情報)が生成される。なお、自機位置推定処理及び環境地図作成処理の詳細については後述する。
次に、制御部10は、生成した環境地図と推定した自機位置とを行動アプリに送信する(ステップS202)。これにより、行動計画アプリは、自律移動装置100の周囲の環境を理解し、自律移動装置100の移動先を指示することができる。
そして、制御部10は、画像情報取扱部16に、ステップS201で取得した画像の画像先鋭度値を算出させることにより、画像先鋭度データベースDBを生成(DBを生成1)する(ステップS203)。画像先鋭度データベースDBを生成する処理の詳細については後述する。
そして、制御部10は、画像の画像先鋭度値を用いて撮像部33のレンズの汚れを判定(レンズ汚れ判定1)する(ステップS204)。撮像部33のレンズの汚れを判定する処理については後述する。
撮像部33のレンズに汚れがあると判定された場合(ステップS205;Yes)、制御部10は、その旨を行動計画アプリに通知する(ステップS206)。行動アプリは、自律移動装置100に対して、ユーザに自律移動装置100の撮像部33のレンズの清掃を促す行動、例えば、レンズの清掃を促す音声又は警告音を発する、を実行するように指示し、ステップS207に進む。撮像部33のレンズに汚れが有ると判定されなかった場合(ステップS205;No)、レンズの汚れは不明であると判定し、ステップS207に進む。
ステップS207では、制御部10は、各種アプリやユーザから終了指示があるか否かを判定する。終了指示があるなら(ステップS207;Yes)、自機位置推定、環境地図生成スレッドを終了する。終了指示がないなら(ステップS207;No)、ステップS201に戻る。
以上、自機位置推定、環境地図生成スレッドの処理について説明した。次に、自機位置推定、環境地図生成スレッド(図7)のステップS201で実行される自機位置推定処理について、図8を参照して説明する。この処理は、位置推定部13が、最初に初期化処理を行い、その後自機位置推定(撮像部33で取得した画像を用いてビジュアルオドメトリにより自機位置を推定する)を続ける処理である。
これにより、地図の情報に基づいて自律的に移動を行いつつ、適宜地図の情報を更新することができる。
典型的な例としては、自律移動装置100は最初、充電器200に置いてある状態で電源を投入すると、障害物センサ43を頼りとして、家の各部屋をくまなく移動し、障害物センサ43によって壁等の障害物位置を特定し、障害物位置を含む地図の情報を作成することができる。地図がある程度作成されると、地図の情報がまだないが移動可能と考えられる領域を知ることができ、その領域に自律的に移動する等して、より広範囲の地図の作成を促すこともできるようになる。そして、移動可能なほぼ全域の地図の情報が作成されれば、地図の情報を利用した効率的な移動動作が可能になる。例えば部屋のどの位置からでも最短経路で充電器200に戻ったり、効率的に部屋の掃除をしたりすることが可能になる。
位置推定部13は、まず、動作終了かを判定する(ステップS301)。動作終了なら(ステップS301;Yes)終了し、動作終了でないなら(ステップS301;No)、初期化済であるか判定する(ステップS302)。初期化済なら(ステップS302;Yes)ステップS321以降の自機位置推定処理を行い、初期化済でないなら(ステップS302;No)ステップS303に進んで初期化処理を行う。まず初期化処理について説明する。
初期化処理では位置推定部13はまずフレームカウンタNに-1をセットし(ステップS303)、撮像部33で画像を取得する(ステップS304)。画像は例えば30fpsで取得することができる(取得した画像はフレームとも呼ばれる)。次に、取得した画像内から2D特徴点を取得する(ステップS305)。2D特徴点とは画像中のエッジ部分など、画像内の特徴的な部分であり、SIFT(Scale-Invariant Future Transform)やSURF(Speed-Up Robust Features)等のアルゴリズムを用いて取得することができる。なお、2D特徴点を取得するのに他のアルゴリズムを使っても良い。
取得した2D特徴点の個数が少ないと、後述するTwo-view Structure from Motion法での計算ができないため、位置推定部13はステップS306において、2D特徴点の取得数と基準値(例えば10個)とを比較し、基準値未満だった場合(ステップS306;No)はステップS304に戻り、基準値以上の2D特徴点数が得られるまで、画像の取得と2D特徴点の取得とを繰り返す。なお、この時点ではまだ地図の情報は作成できていないが、例えば上述した典型的な例では、障害物センサ43を頼りとして、家の各部屋をくまなく移動し始めているため、この初期化処理で画像取得と2D特徴点取得を繰り返していれば、移動しながら画像取得を繰り返すことになるので、様々な画像が取得でき、いずれは2D特徴点数が多い画像を取得できることが期待できる。
2D特徴点の取得数が基準値以上だった場合は(ステップS306;Yes)、位置推定部13はフレームカウンタNをインクリメントする(ステップS307)。そして、フレームカウンタNが0かどうかを判定する(ステップS308)。フレームカウンタNが0なら(ステップS308;Yes)画像をまだ一つしか取得していないということになるので、2枚目の画像を取得するためにステップS304に戻る。なお図8のフローチャートでは記載していないが、1枚目の画像を取得した時の自律移動装置100の位置と2枚目の画像を取得する時の自律移動装置100の位置とがある程度離れていた方が、これ以降の処理で推定する姿勢の精度が向上する。そこで、ここでステップS308からステップS304に戻る際に、オドメトリによる並進距離が所定の距離(例えば1m)以上となるまで待つ処理を追加しても良い。
フレームカウンタNが0でないなら(ステップS308;No)二つの画像を取得したということがわかるので、位置推定部13は、これら二つの画像間で2D特徴点の対応(実環境上の同一の点がそれぞれの画像中に存在し、その対応が取れるもの)を取得する(ステップS309)。ここで特徴点の対応数が5未満であれば、後述する二つの画像間の姿勢の推定ができないため、位置推定部13は特徴点の対応数が5未満かどうかを判定する(ステップS310)。5未満であれば(ステップS310;Yes)初期画像を取得し直すために、ステップS303に戻る。特徴点の対応数が5点以上であれば(ステップS310;No)、Two-view Structure from Motion法を用いることで、二つの画像間の姿勢(それぞれの画像を取得した位置の差分(並進ベクトルt)及び向きの差分(回転行列R))を推定することができる(ステップS311)。
この推定は具体的には、対応する特徴点から基礎行列Eを求め、基礎行列Eを並進ベクトルtと回転行列Rとに分解することによって得られるが、詳細はここでは割愛する。なお、ここで得られる並進ベクトルt(3次元空間内で移動することを想定すると、最初の画像を取得した位置を原点として、X,Y,Zの3要素を持つ)の各要素の値は実環境上での値とは異なる(Two-view Structure from Motion法では実環境上のスケール自体を得ることはできず、実環境と相似する空間上での値を得ることになる。)ため、これらをSLAM空間上での値とみなし、以下ではSLAM空間上での座標(SLAM座標)を用いて説明する。
二つの画像間の姿勢(並進ベクトルt及び回転行列R)が求まると、その値は、最初の画像を基準(最初の画像を取得した位置をSLAM座標の原点、並進ベクトルは0ベクトル、回転行列は単位行列Iとする。)にした場合の、二枚目の画像の姿勢(二つ目の画像を取得した時の自機の位置(並進ベクトルt)及び向き(回転行列R))となる。ここで、二つの画像それぞれの姿勢(該画像(フレーム)撮影時の自機の位置(並進ベクトルt)及び向き(回転行列R)で、フレーム姿勢とも言う。)が求まっている場合、その二つの画像間で対応が取れている2D特徴点(対応特徴点)のSLAM座標での3D位置を、以下の考え方に基づき、地図作成部12が求める(ステップS312)。
2D特徴点の画像中の座標(フレーム座標:既知)を(u,v)とし、その2D特徴点のSLAM座標での3D位置(未知)を(X,Y,Z)とすると、これらを同次座標で表したときのこれらの関係は、透視投影行列Pを用いて下記の式(1)で表される。ここで、「~」記号は「非零の定数倍を除いて等しい」(つまり、等しいか又は定数(非零)倍になっている)ことを表し、「’」記号は「転置」を表す。
(u v 1)’~P(X Y Z 1)’…(1)
上記の式(1)において、Pは3×4の行列で、カメラの内部パラメータを示す3×3の行列Aと、その画像の姿勢(フレーム姿勢)を示す外部パラメータR及びtから以下の式(2)で表される。ここで、(R|t)は、回転行列Rの右に並進列ベクトルtを並べた行列を表す。
P=A(R|t)…(2)
上記の式(2)において、R及びtは上述したようにそのフレーム姿勢として求められている。また、カメラの内部パラメータAは、焦点距離と撮像素子サイズにより決まるので、撮像部33を決めておけば定数となる。
二つの画像間で対応が取れている2D特徴点のうちの一つが、一つ目の画像のフレーム座標(u1,v1)と、二つ目の画像のフレーム座標(u2,v2)に写っているとすると、以下の式(3)及び式(4)ができる。ここで、Iは単位行列、0はゼロベクトル、(L|r)は、行列Lの右に列ベクトルrを並べた行列を表す。
(u1 v1 1)’~A(I|0)(X Y Z 1)’…(3)
(u2 v2 1)’~A(R|t)(X Y Z 1)’…(4)
上記の式(3)及び式(4)において、u1,v1,u2,v2それぞれについての式ができるため、式は4つできるが、未知数はX,Y,Zの3つなので、X,Y,Zを求めることができ、これがその2D特徴点のSLAM座標における3D位置となる。なお、式の個数の方が未知数の個数よりも多いため、例えばu1,v1,u2で求めたX,Y,Zとu1,v1,v2で求めたX,Y,Zとが異なる場合がありうる。このような場合は、過剰条件の連立一次方程式となり、一般には解が存在しないが、地図作成部12は、最小二乗法を用いて、最も確からしいX,Y,Zを求める。
2D特徴点のSLAM座標における3D位置(X,Y,Z)が求まったら、それをMap点として、地図作成部12がMap点データベース(Map点DBとも言い、地図記憶部22に格納される)に登録する(ステップS313)。Map点データベースに登録する要素としては、少なくとも、「2D特徴点のSLAM座標における3D位置であるX,Y,Z」と、「その2D特徴点の特徴量」(例えばSIFT等で得た特徴量)が必要である。「タイムスタンプ」(後述するキーフレームカウンタNKF(現在のキーフレーム番号を表す変数)のMap点データベースへの登録時点での値等)をMap点データベースへの登録要素に追加しておくと、Map点データベースを編集(過去の状態に戻す等)する際に便利である。
そして、地図作成部12は、二つの画像間で対応が取れている2D特徴点(対応特徴点)の全てをMap点データベースに登録したかを判定し(ステップS314)、まだ全ての登録ができていなかったら(ステップS314;No)ステップS312に戻り、全て登録できたら(ステップS314;Yes)ステップS315に進む。
次に位置推定部13は、キーフレーム(後に続くスレッドでの処理対象となる画像を指す)のカウンタNKFを0に初期化し(ステップS315)、二つ目の画像をキーフレームとしてフレームデータベース(フレームDBとも言い、画像記憶部21に格納される)に登録する(ステップS316)。
フレームデータベースに登録する要素は、「キーフレーム番号」(登録時点でのキーフレームカウンタNKFの値)、「姿勢」(その画像撮影時の自機のSLAM座標内での位置(並進ベクトルt)及び向き(回転行列R))、「オドメトリで計測した実環境上での姿勢」(実環境での駆動部32による移動距離に基づいて求められる自機の位置及び向き)、「抽出した全ての2D特徴点」、「全ての2D特徴点の中でMap点として3D位置が既知の点」、「キーフレーム自体の特徴」、である。
上記中、「オドメトリで計測した実環境上での姿勢」は、並進ベクトルtと回転行列Rとで表すこともできるが、通常、本自律移動装置100は2次元平面上を動くので、2次元データに単純化して、移動開始時の位置(原点)及び向きを基準にした2次元座標(X,Y)及び向きφとして表しても良い。また、「キーフレーム自体の特徴」とは、キーフレーム間の画像類似度を求める処理を効率化するためのデータであり、通常は画像中の2D特徴点のヒストグラム等を用いるのが良いが、画像自体を「キーフレーム自体の特徴」としても良い。
次に、位置推定部13は、キーフレームが生成された事を地図作成スレッドに知らせるために、地図作成スレッドのキーフレームキュー(キュー(Queue)は、先入れ先出しのデータ構造になっている)に、キーフレームカウンタNKFをセットする(ステップS317)。
以上で自機位置推定スレッドの初期化処理が完了したので、位置推定部13は、初期化済フラグをセットする(ステップS318)。
そして、位置推定部13は、SLAM座標と実環境座標とのスケール対応を得るために、オドメトリによる並進距離(実環境での座標で求められる)を、上記の処理で推定したSLAM座標での並進距離dで除することによって、スケールSを求める(ステップS319)。
次に、位置推定部13は、ステップS301、ステップS302を経由して、初期化済の場合の処理であるステップS321へ進む。
初期化済の場合の処理を説明する。この処理が、自機位置推定スレッドの通常時の処理であり、逐次現在の自律移動装置100の位置及び向き(SLAM座標内での並進ベクトルtと回転行列R)を推定する処理である。
位置推定部13は、撮像部33で画像を撮影し(ステップS321)、フレームカウンタNをインクリメントする(ステップS322)。そして、撮影した画像に含まれている2D特徴点を取得する(ステップS323)。次に、フレームデータベースに登録されている以前のキーフレーム(例えばキーフレーム番号がNKFである画像)の情報から、その画像の情報に含まれている2D特徴点のうち、3D位置が既知である(Map点データベースに登録されているMap点になっている)2D特徴点を取得し、今撮影した画像との間で対応が取れる2D特徴点(対応特徴点)を抽出する(ステップS324)。
そして、位置推定部13は、対応特徴点の個数が基準対応特徴点数(例えば10)未満かどうかを判定し(ステップS325)、基準対応特徴点数未満の場合(ステップS325;Yes)はSLAM法で推定する姿勢の精度が悪くなる(Visual SLAM制御の失敗となる)ので、位置の推定は行わずにステップS321での画像の取得に戻る。ここで、すぐにステップS321に戻るのではなく、ステップS324に戻って、フレームデータベースに登録されているキーフレームの中から対応特徴点の個数が基準対応特徴点数以上のものを検索するようにしても良い。この場合は、フレームデータベースに登録されているキーフレームの中に対応特徴点の個数が基準対応特徴点数以上のものが見つからなかった場合にステップS321に戻る。
位置推定部13は、基準対応特徴点数以上の対応特徴点が抽出できたら(ステップS325;No)、対応特徴点それぞれの3D位置(Xi,Yi,Zi)をMap点データベースから取得する(ステップS326)。今撮影した画像に含まれている対応特徴点のフレーム座標を(ui,vi)とし、その対応特徴点の3D位置を(Xi,Yi,Zi)とする(iは1から対応特徴点数までの値を取る)と、各対応特徴点の3D位置(Xi,Yi,Zi)を以下の式(5)によってフレーム座標系に投影した値(uxi,vxi)とフレーム座標(ui,vi)とは理想的には一致するはずである。
(uxi vxi 1)’~A(R|t)(Xi Yi Zi 1)’…(5)
実際には(Xi,Yi,Zi)にも(ui,vi)にも誤差が含まれているため、(uxi,vxi)と(ui,vi)とが一致することはめったにない。そして、未知数はRとt(3次元空間ではそれぞれ3次元となり、3+3=6が未知数の個数である)だけなのに、数式は対応特徴点の個数の2倍存在する(対応特徴点一つに対して、フレーム座標のu,vそれぞれに対する式が存在するため)ことになるため、過剰条件の連立一次方程式になり、上述したように最小二乗法で求めることになる。具体的には、位置推定部13は、以下の式(6)のコスト関数E1を最小化する姿勢(並進ベクトルt及び回転行列R)を求めることになる。これがSLAM法で求めたSLAM座標での自律移動装置100の姿勢(並進ベクトルt及び回転行列Rで表される自律移動装置100の位置及び向き)となる。このようにして、位置推定部13は自律移動装置100の姿勢を推定する(ステップS327)。
SLAM座標での現在の自律移動装置100の姿勢(並進ベクトルt及び回転行列R)が求められたので、位置推定部13は、これにスケールSを乗算することで、VO(ビジュアルオドメトリ)を求める(ステップS328)。VOは実環境での自律移動装置100の位置及び向きとして利用できる。
次に、位置推定部13は、フレームDBに登録されている直前のキーフレーム(キーフレーム番号がNKFである画像)を撮影した時の自律移動装置100の位置から基準並進距離(例えば1m)以上移動しているかを判定し(ステップS329)、移動しているなら(ステップS329;Yes)キーフレームカウンタNKFをインクリメントしてから(ステップS330)、現フレームをキーフレームとしてフレームDBに登録する(ステップS331)。基準並進距離未満しか移動していないなら(ステップS329;No)ステップS301に戻る。
ここで、基準並進距離と比較する自律移動装置100の移動距離は、直前のキーフレームから現フレームまでの並進距離(両フレームの並進ベクトルの差のベクトルの絶対値(要素の二乗和の平方根))をオドメトリから取得しても良いし、上述したVO(ビジュアルオドメトリ)から求めても良い。フレームDBに登録する内容は上述したように、「キーフレーム番号」、「姿勢」、「オドメトリで計測した実環境上での姿勢」、「抽出した全ての2D特徴点」、「全ての2D特徴点の中でMap点として3D位置が既知の点」、「キーフレーム自体の特徴」、である。
そして、位置推定部13は、新たなキーフレームが発生したことを地図作成スレッドに知らせるために、地図作成スレッドのキーフレームキューにキーフレームカウンタNKFをセットする(ステップS332)。そして、ステップS301に戻る。なお、キーフレームカウンタNKF、スケールS、Map点DB、フレームDBはスレッドをまたいで値を参照することができるように記憶部20に記憶されている。
なお、基準並進距離を自律移動装置100が並進する間に、ステップS325のVisual SLAM制御の失敗回数を、ステップS325に到達した回数で割ることによりVisual SLAM制御の失敗率を算定し、この失敗率が基準失敗率(例えば50パーセント)より高い場合、後述する撮像部33のレンズが汚れている、と判定しても良い。レンズが汚れていることにより、対応特徴点を正しく取得できていないと考えられるからである。
以上、自機位置推定処理について説明した。次に、自機位置推定、環境地図生成スレッド(図7)のステップS201で実行される環境地図生成処理について、図9を用いて説明する。この処理では地図作成部12が、キーフレーム中の対応特徴点の3D位置を計算して、地図の情報(Map点DB)を作成している。
まず、地図作成部12は、動作終了かを判定する(ステップS401)。動作終了なら(ステップS401;Yes)終了し、動作終了でないなら(ステップS401;No)、キーフレームキューが空かどうかを判定する(ステップS402)。キーフレームキューが空だったら(ステップS402;Yes)ステップS401に戻り、空でなければ(ステップS402;No)、キーフレームキューからデータを取り出してMKF(地図作成スレッドで処理するキーフレームのキーフレーム番号を表す変数)にセットする(ステップS403)。地図作成部12は、MKFが0より大きいか判定し(ステップS404)、MKFが0である場合は(ステップS404;No)ステップS401に戻ってキーフレームキューにデータが入るのを待つ。MKFが1以上の場合は(ステップS404;Yes)、以下の処理に進む。
地図作成部12は、フレームDBを参照し、前キーフレーム(キーフレーム番号がMKF-1のキーフレーム)の2D特徴点と現キーフレーム(キーフレーム番号がMKFのキーフレーム)の2D特徴点とで対応が取れる2D特徴点(対応特徴点)を抽出する(ステップS405)。フレームDBにはそれぞれのキーフレームの姿勢(並進ベクトルtと回転行列R)も登録されているので、自己位置推定処理の初期化時の処理の時と同様の方法で対応特徴点の3D位置を計算できる。地図作成部12は、3D位置が計算できた対応特徴点をMap点としてMap点DBに登録する(ステップS406)。地図作成部12は、フレームDBに対しても今回3D位置を計算できた2D特徴点に対して3D位置を登録する(ステップS407)。
なお、地図作成部12が抽出した対応特徴点がすでにMap点DBに登録済だった場合は、3D位置計算をスキップして次の対応特徴点(Map点DBに未登録のもの)に対する処理に進んでも良いし、改めて3D位置計算を行って、Map点DBに登録済の3D位置や、フレームDB中の対応特徴点に対する3D位置を更新するようにしても良い。
次に、地図作成部12は、キーフレームキューが空かどうかを判定する(ステップS408)。空であれば(ステップS408;Yes)、全キーフレームの姿勢と全Map点の3D位置に対して、バンドルアジャストメント処理を行って、精度向上を図ってから(ステップS409)ステップS401に戻る。キーフレームキューが空でなければ(ステップS408;No)ステップS401に戻る。
なお、バンドルアジャストメント処理とは、カメラ姿勢(キーフレーム姿勢)とMap点の3D位置とを同時に推定する非線形最適化法であり、Map点をキーフレーム上に投影させたときに発生する誤差が最小になるような最適化を行うものである。
このバンドルアジャストメントの処理を行うことで、キーフレーム姿勢とMap点の3D位置の精度向上を図ることができる。しかし、この処理を行わなくても精度向上が図れないだけで特別問題が発生するわけではない。したがって、他の処理がない状態のとき(例えば、キーフレームキューが空の状態)であっても、この処理を毎回行う必要はない。
また、バンドルアジャストメントの処理を行うと、キーフレーム上に投影させたときの誤差が所定の値よりも大きいMap点が見つかることがある。このような誤差の大きなMap点については、SLAM推定に悪影響を及ぼすため、Map点DB及びフレームDBから削除したり、誤差の大きな注意を要するMap点であることを識別するためのフラグを立てておいたりしても良い。なお、バンドルアジャストメントの処理は本実施形態においてはオプション的な扱いになるため、ここでは処理の詳細については省略する。
次に、自機位置推定、環境地図生成スレッド(図7)のステップS203で実行されるDBを生成1の処理について、図10を用いて説明する。この処理では撮像部33で撮像した物体の境界等が写る画像から、画像情報取扱部16が、画像先鋭度値を計算して、画像を撮像した時の自律移動装置100の姿勢(3次元の位置及び3次元の回転)と、画像を撮像した時刻とともに1つの情報Gとして生成し、情報記憶部23に保存されている情報Gの姿勢に近い姿勢で撮像された画像から得られた情報G0の画像先鋭度値と情報Gの画像先鋭度値とを比較し、画像先鋭度値が高い情報を情報記憶部23に保存する。
まず、画像情報取扱部16は、行動計画アプリ等から情報記憶部23内にある画像先鋭度値データベースDBをクリアするように指示があるか否かを判定する(ステップS501)。クリアするように指示があるなら(ステップS501;Yes)画像先鋭度値データベースDBをクリアし(ステップS502)、クリアするように指示がないなら(ステップS501;No)、ステップS503に進む。画像先鋭度値データベースDBをクリアするのは、撮像部33のレンズが清掃されたと推定される時である。画像先鋭度値データベースDBをクリアする指示は、例えば、自律移動装置100に備えられたスイッチ(図示せず)をレンズ清掃後にユーザが押すことであっても良いし、自律移動装置100がユーザと音声対話等によってレンズ清掃の確認ができたことであっても良い。
ステップS503では、画像情報取扱部16は、ステップS202で自機位置推定処理時等に取得した画像に対して以下の式(7)に示す4近傍のラプラシアンオペレータの畳み込み処理を行った後、分散を取り、画像先鋭度値とする。
次に、画像情報取扱部16は、ステップS202で自機位置推定処理時に計算した自律移動装置100の姿勢を、画像先鋭度値が算出された画像が撮像された時の姿勢とし、この姿勢と、この画像が撮像された時刻と、画像先鋭度値とをひとまとめにして、情報Gとする(ステップS504)。
画像情報取扱部16は、画像先鋭度値データベースDB内に、情報Gの姿勢と所定の誤差TH0の範囲内の姿勢である情報があるかどうかを検索する(ステップS505)。所定の誤差TH0の範囲内の姿勢である情報があった場合(ステップS505;Yes)、その情報を情報G0として取り出し、無かった場合(ステップS505;No)、ステップS508へ進む。なお、所定の誤差TH0の範囲内の姿勢の情報が複数ある場合には、1番画像先鋭度値が大きいものを選ぶ。姿勢間の距離は、ユークリッド距離を用いる。回転の3次元は、Exponential Map Quaternionを用いることによって、ユークリッド空間で扱うことができる。
次に、画像情報取扱部16は、情報Gの画像先鋭度値と情報G0の画像先鋭度値を比較する(ステップS506)。情報Gの画像先鋭度値が情報G0の画像先鋭度値よりも大きい場合(ステップS506;Yes)、画像先鋭度値データベースDB内の情報G0を削除する(ステップS507)。情報Gの画像先鋭度値が情報G0の画像先鋭度値よりも大きくない場合(ステップS506;No)、そのまま終了する。
ステップS508では、画像情報取扱部16は、画像先鋭度値データベースDBに情報Gを登録し、終了する。
次に、自機位置推定、環境地図生成スレッド(図7)のステップS204で実行されるレンズ汚れ判定1の処理について、図11を用いて説明する。この処理では、DBを生成1の処理で算定した情報Gの画像先鋭度値と、現時刻から所定の時間T0以前に撮像された過去の画像であって情報Gが撮像された姿勢から所定の誤差TH1内で情報Gの姿勢に1番近い姿勢である情報G1の画像先鋭度値に所定の係数を掛けたものと、を比較することにより、自律移動装置100の撮像部33のレンズの汚れを判定する。
まず、画像情報取扱部16は、画像先鋭度値データベースDB内に、現時刻より所定の時間T0以前に撮像された物体の境界等が写る画像の情報であって、情報Gの画像が撮像された姿勢からの姿勢距離が所定の誤差(範囲)TH1以下で1番近い姿勢である画像の情報(以下、この情報を情報G1とする)を検索する(ステップS601)。画像先鋭度値データベースDB内に、情報G1が存在する場合(ステップS601;Yes)、ステップS602に進む。画像先鋭度値データベースDB内に、情報G1が存在しない場合(ステップS601;No)、ステップS610に進む。
ステップS602では、情報G1の画像先鋭度値に1未満の係数THG(先鋭度値が落ちたとみなすためのマージン)を掛けたものと情報Gの画像先鋭度値とを比較する。情報G1の画像先鋭度値に係数THGを掛けたものより情報Gの画像先鋭度値が大きい場合(ステップS602;Yes)、画像先鋭度フラグを1とし、現時刻、情報Gの姿勢とともに画像先鋭度フラグデータベースDBG内に保存し(ステップS603)、ステップS604に進む。情報G1の画像先鋭度値に係数THGを掛けたものより情報Gの画像先鋭度値が大きくない場合(ステップS602;No)、画像先鋭度フラグを0とし、現時刻、情報Gの姿勢とともに画像先鋭度フラグデータベースDBG内に保存し(ステップS609)、ステップS604に進む。
次に、画像情報取扱部16は、画像先鋭度フラグデータベースDBG内の、現時刻より所定の時間T1以上古い情報を削除する(ステップS604)。
画像情報取扱部16は、画像先鋭度フラグデータベースDBG内に保存されている情報の全回転姿勢がTH3方向をカバーしているかを判定する(ステップS605)。ここで、全回転姿勢がTH3方向をカバーしている、について説明する。例えば、TH3=6であるとする。本来、回転姿勢は3次元で持っているが、通常、自律移動装置100は水平な床面上を移動するので、回転姿勢の移動はYaw角だけの1次元とみなす。TH3=6としているので、360度/6=60度となる。従って、画像先鋭度フラグデータベースDBG内に保存されている情報の回転方向が、60度毎に異なる6方向をカバーしている必要がある。これにより、太陽光や照明の方向によるレンズ汚れの検出精度への影響を小さくすることができる。画像先鋭度フラグデータベースDBG内に保存されている情報の全回転姿勢がTH3方向をカバーしている場合(ステップS605;Yes)、ステップS606に進む。画像先鋭度フラグデータベースDBG内に保存されている情報の全回転姿勢がTH3方向をカバーしてない場合(ステップS605;No)、ステップS610に進む。
ステップS606では、画像情報取扱部16は、画像先鋭度フラグデータベースDBG内の、画像先鋭度フラグが0である情報の割合Xを算出する。
算出した割合Xが所定の割合TH2より大きい場合(ステップS607;Yes)、自律移動装置100の撮像部33のレンズの汚れがあると判定する(ステップS608)。算出した割合Xが所定の割合TH2より大きくない場合(ステップS607;No)、自律移動装置100の撮像部33のレンズの汚れが不明であると判定する(ステップS610)。なお、ステップS601でNoの場合、ステップS605でNoの場合も、自律移動装置100の撮像部33のレンズの汚れが不明であると判定する(ステップS610)。
次に自律移動装置100の起動時処理(図6)のステップS102で起動される移動指示アプリについて、図12を用いて説明する。移動指示アプリは、行動計画アプリに設定された動作モードに応じて、自律移動装置100の移動先を移動スレッドに通知する。動作モードには、自律移動装置100を充電器200に移動させる充電器帰還モード、自律移動装置100をランダムに移動させるお散歩モード及び人とコミュニケーションを取る対話モードがある。他の動作モードを設定しても良い。
まず、制御部10は、動作終了かを判定する(ステップS701)。動作終了なら(ステップS701;Yes)終了する。動作終了でないなら(ステップS701;No)動作モードを確認する(ステップS702)。行動計画アプリによって設定された動作モードが、充電器帰還モードならステップS703に進み、お散歩モードならステップS706に進み、対話モードならステップS708に進む。
動作モードが充電器帰還モードの場合、制御部10は、充電器200を自律移動装置100の目的地に設定するように移動スレッドに通知する(ステップS703)。充電器帰還モードでは、充電器200からの赤外線ビーコンである帰還信号を頼りに自律移動装置100を充電器200に移動させる。ステップS704に進み、制御部10は、レンズ汚れ判定2を行う。レンズ汚れ判定2の詳細については後述する。
その後、制御部10は、自律移動装置100が目的地である充電器200に到着したかどうかを判定する(ステップS705)。自律移動装置100が充電器200に到着していれば(ステップS705;Yes)、ステップS701に戻る。自律移動装置100が充電器200に到着していなければ(ステップS705;No)、ステップS704に戻る。
動作モードがお散歩モードの場合、制御部10は、ランダムに目的地を移動スレッドに通知する。なお、目的地は、環境地図内の空いている空間や他のスレッドからの移動要求(例えば、スマホからのマニュアル動作等)等から決定しても良い(ステップS706)。
所定の時間t経過後、ステップS707に進み、後述する移動スレッドが終了していれば(ステップS707;Yes)、ステップS701に戻る。移動スレッドが終了していなければ(ステップS707;No)、所定の時間t経過後、ステップS707に戻る。
動作モードが対話モードの場合、制御部10は、最新の画像内に顔領域を検出したかどうかを判定する(ステップS708)。画像内に顔領域を検出できれば(ステップS708;Yes)、制御部10は、顔(検出)領域が撮像画像全体に行き渡るような方向に自律移動装置100を移動させるように移動指示を移動スレッドに通知する(ステップS709)。画像内に顔領域を検出できない場合(ステップS708;No)、制御部10は、ランダムに目的地を移動スレッドに通知する、或いは、環境地図内の空いている空間や他のスレッドからの移動要求(例えば、スマホからのマニュアル動作等)等から目的地を決定しても良い(ステップS711)。
ステップS709では、所定の時間t経過後、ステップS710に進み、移動スレッドが終了していれば(ステップS710;Yes)、ステップS701に戻る。移動スレッドが終了していなければ(ステップS710;No)、所定の時間t経過後、ステップS710に戻る。
ステップS711では、所定の時間t経過後、ステップS712に進み、移動スレッドが終了していれば(ステップS712;Yes)、ステップS701に戻る。移動スレッドが終了していなければ(ステップS712;No)、所定の時間t経過後、ステップS712に戻る。
次にステップS704で実行されるレンズ汚れ判定処理2について図13を用いて説明する。この処理では、充電器200に戻る途中、充電器200まで所定の距離DISTの地点付近で撮像した画像の画像先鋭度値と、画像先鋭度値データベースDB2に保存されている充電器200まで所定の距離DISTの地点付近で撮像した画像の画像先鋭度値とを比較することにより撮像部33の汚れがあるかを判定する。
まず、画像情報取扱部16は、行動計画アプリ等から情報記憶部23内にある画像先鋭度値データベースDB2をクリアするように指示があるか否かを判定する(ステップS801)。クリアするように指示がある場合(ステップS801;Yes)、画像先鋭度値データベースDB2をクリアし(ステップS802)、クリアするように指示がない場合(ステップS801;No)、ステップS803に進む。画像先鋭度値データベースDB2をクリアするのは、撮像部33のレンズが清掃されたと推定される時である。画像先鋭度値データベースDB2をクリアする指示は、例えば、自律移動装置100に備えられたスイッチをレンズ清掃後にユーザが押すことであっても良いし、自律移動装置100がユーザと音声対話してレンズ清掃の確認ができたことであっても良い。
ステップS803では、制御部10は、自律移動装置100が充電器200から帰還信号を受信中であるか及び、自律移動装置100が充電器200からの距離(或いは自律移動装置100の走行距離)が所定の距離DISTから所定の誤差E0の範囲内であるかを障害物センサ43よって判断する。自律移動装置100が充電器200から帰還信号を受信中であり、かつ、自律移動装置100が充電器200からの距離が所定の距離DISTから所定の誤差E0の範囲内である場合(ステップS803;Yes)、制御部10は、撮像部33に物体の境界等が写る画像を撮像させ、画像情報取扱部16に、撮像により取得した画像に対して4近傍のラプラシアンオペレータの畳み込み処理を行わせた後、分散を取らせ、画像先鋭度値を算定させる(ステップS804)。自律移動装置100が充電器200から帰還信号を受信中ではない場合、或いは、自律移動装置100が充電器200からの距離が所定の距離DISTから所定の誤差E0の範囲内でない場合(ステップS803;No)、戻る。
ステップS805では、画像情報取扱部16は、画像先鋭度値データベースDB2に、過去にステップS804で算出された画像先鋭度値が保存されているかどうかを判定する。画像先鋭度値データベースDB2に、過去にステップS804で算定された画像先鋭度値が保存されている場合(ステップS805;Yes)、画像情報取扱部16は、保存されている画像先鋭度値に1未満の所定の係数THH3を掛けたものとステップS804で算出された画像先鋭度値とを比較する(ステップS806)。画像先鋭度値データベースDB2に、過去にステップS804で算定された画像先鋭度値が保存されていない場合(ステップS805;No)、画像情報取扱部16は、ステップS804で算出された画像先鋭度値を画像先鋭度値データベースDB2に保存し(ステップS809)、戻る。
保存されている画像先鋭度値に所定の係数THH3を掛けたものよりステップS804で算出された画像先鋭度値が大きくない場合(ステップS806;No)、画像情報取扱部16は、レンズの汚れがあると判定する。このとき、制御部10は、レンズの汚れがあると行動計画アプリに通知する(ステップS810)。行動計画アプリは、自律移動装置100に対して、ユーザに自律移動装置100の撮像部33のレンズの清掃を促す行動、例えば、レンズの清掃を促す音声又は警告音を発する、を実行するように指示し、戻る。保存されている画像先鋭度値に所定の係数THH3を掛けたものよりステップS804で算出された画像先鋭度値が大きい場合(ステップS806;Yes)、画像情報取扱部16は、保存されている画像先鋭度値よりステップS804で算出された画像先鋭度値が大きいか否かを判定する(ステップS807)。
保存されている画像先鋭度値よりステップS804で算出された画像先鋭度値が大きい場合(ステップS807;Yes)、画像情報取扱部16は、保存されている画像先鋭度値をステップS804で算出された画像先鋭度値に入れ替えて画像先鋭度値データベースDB2に保存し(ステップS808)、戻る。保存されている画像先鋭度値よりステップS804で算出された画像先鋭度値が大きくない場合(ステップS807;No)、戻る。
以上、レンズ汚れ判定2の処理について説明した。次に、自律移動装置100の起動時処理(図6)のステップS103で起動される移動スレッドについて、図14を用いて説明する。このスレッドでは、移動指示アプリ等による移動指示に従って、自律移動装置100を移動指示のあった方向や目的地まで移動させる。
制御部10は、まず動作終了かどうかを判定し(ステップS901)、移動指示アプリ等から指示された方向や目的地に自律移動装置100が移動済み等で動作終了なら(ステップS901;Yes)終了し、動作終了でないなら(ステップS901;No)、移動指示アプリ等から移動指示があるかを判定する(ステップS902)。
移動指示アプリ等から移動指示がある場合(ステップS902;Yes)、制御部10は、移動指示アプリ等から指示された方向や目的地に自律移動装置100を移動させた後(ステップS903)、ステップS901に戻る。移動指示アプリ等から移動指示が無い場合(ステップS902;No)、ステップS901に戻る。
以上、移動スレッドについて説明した。次に、自律移動装置100の起動時処理(図6)のステップS103で起動される顔検出スレッドについて、図15を用いて説明する。このスレッドでは、取得した画像に顔領域を検出した場合、移動指示アプリに通知し、撮像部33のレンズの汚れ判定を行う。
制御部10は、まず動作終了かどうかを判定し(ステップS1001)、動作終了なら(ステップS1001;Yes)終了し、動作終了でないなら(ステップS1001;No)、撮像部33に自律移動装置100の前方を撮像させ、画像を取得する(ステップS1002)。
ステップS1003では、制御部10は、画像内に人の顔の輪郭、目、鼻及び口等の形状に類似する形状を有する顔領域を検出したかを判定する。画像内に顔領域を検出した場合(ステップS1003;Yes)、顔検出領域を移動指示アプリに通知する(ステップS1004)。画像内に顔領域を検出しなかった場合(ステップS1003;No)、ステップS1001に戻る。
ステップS1005では、制御部10は、画像情報取扱部16に、ステップS1002で取得した画像の顔(検出)領域について画像先鋭度値を算出させることにより、顔領域先鋭度値データベースDB3を生成(DBを生成3)する。顔領域先鋭度値データベースDB3の生成する処理の詳細については後述する。
そして、制御部10は、画像先鋭度値を用いて撮像部33のレンズの汚れを判定(レンズ汚れ判定3)する(ステップS1006)。撮像部33のレンズの汚れを判定する処理については後述する。
撮像部33のレンズに汚れが有ると判定された場合(ステップS1007;Yes)、制御部10は、その旨を行動計画アプリに通知する(ステップS1008)。行動計画アプリは、自律移動装置100に対して、ユーザに自律移動装置100の撮像部33のレンズの清掃を促す行動、例えば、レンズの清掃を促す音声又は警告音を発する、を実行するように指示し、ステップS1001に戻る。撮像部33に汚れが有ると判定されなかった場合(ステップS1007;No)、ステップS1001に戻る。
次に、顔検出スレッド(図15)のステップS1005で実行されるDBを生成3の処理について、図16を用いて説明する。この処理では撮像部33で撮像した顔領域が検出された顔検出画像から、画像情報取扱部16が、画像先鋭度値を計算して、顔検出領域と、画像を撮像した時刻とともに1つの情報Hとして生成し、情報記憶部23に保存されている情報Hの顔検出領域に近い顔検出領域である画像から得られた情報H0の画像先鋭度値と情報Hの画像先鋭度値とを比較し、画像先鋭度値が高い情報を情報記憶部23に保存する。
まず、画像情報取扱部16は、行動計画アプリ等から情報記憶部23内にある顔領域先鋭度値データベースDB3をクリアするように指示があるか否かを判定する(ステップS1101)。クリアするように指示があるなら(ステップS1101;Yes)顔領域先鋭度値データベースDB3をクリアし(ステップS1102)、クリアするように指示がないなら(ステップS1101;No)、ステップS1103に進む。顔領域先鋭度値データベースDB3をクリアするのは、撮像部33のレンズが清掃されたと推定される時である。顔領域先鋭度値データベースDB3をクリアする指示は、画像先鋭度値データベースDBをクリアするときと同様に、自律移動装置100に備えられたスイッチをレンズ清掃後にユーザが押すことであっても良いし、自律移動装置100がユーザと音声対話してレンズ清掃の確認ができたことであっても良い。
ステップS1103では、画像情報取扱部16は、顔検出スレッドのステップS1002で取得した画像に対して4近傍のラプラシアンオペレータの畳み込み処理を行った後、分散を取り、画像先鋭度値とする。
次に、画像情報取扱部16は、ステップS1002で取得した顔検出領域と、この画像が撮像された時刻と、画像先鋭度値とをひとまとめにして、情報Hとする(ステップS1104)。
画像情報取扱部16は、顔領域先鋭度値データベースDB3内に情報Hの顔検出領域と所定の誤差TH20の範囲内(基準領域内)の顔検出領域である過去の情報があるかどうかを検索する(ステップS1105)。所定の誤差TH20の範囲内の顔検出領域である情報があった場合(ステップS1105;Yes)、その情報を情報H0として取り出し、無かった場合(ステップS1105;No)、ステップS1108へ進む。なお、所定の誤差TH20の範囲内の顔検出領域の情報が複数ある場合には、1番顔検出領域が近いものを選ぶ。領域間の距離は、ユークリッド距離を用いる。領域の開始点(x、y)と、領域の大きさ(w、h)の4次元空間のユークリッド距離とすることができる。
次に、画像情報取扱部16は、情報Hの画像先鋭度値と情報H0の画像先鋭度値を比較する(ステップS1106)。情報Hの画像先鋭度値が情報H0の画像先鋭度値よりも大きい場合(ステップS1106;Yes)、顔領域先鋭度値データベースDB3内の情報H0を削除する(ステップS1107)。情報Hの画像先鋭度値が情報H0の画像先鋭度値よりも大きくない場合(ステップS1106;No)、そのまま終了する。
ステップS1108では、画像情報取扱部16は、顔領域先鋭度値データベースDB3に情報Hを登録し、終了する。
次に、顔検出スレッド(図15)のステップS1006で実行されるレンズ汚れ判定3の処理について、図17を用いて説明する。この処理では、DBを生成3の処理で算定した情報Hの先鋭度値と、顔領域先鋭度値データベースDB3内の情報の画像先鋭度値の平均値HM等を使用することにより、自律移動装置100の撮像部33のレンズの汚れを判定する。
まず、画像情報取扱部16は、顔領域先鋭度値データベースDB3内に保存されている情報の数が所定の値N以上であるかを判定する(ステップS1201)。顔領域先鋭度値データベースDB3内に保存されている情報の数がN以上である場合(ステップS1201;Yes)、ステップS1202に進む。顔領域先鋭度値データベースDB3内に保存されている情報の数がN以上でない場合(ステップS1201;No)、ステップS1211に進む。
ステップS1202では、画像情報取扱部16は、顔領域先鋭度値データベースDB3内に保存されている情報の画像先鋭度値の平均値HMを算出する。
画像情報取扱部16は、顔領域先鋭度値データベースDB3内に保存されている情報の画像先鋭度値の平均値HMに1未満の所定の係数THH2を掛けたものより情報Hの画像先鋭度値が大きいかを判定する(ステップS1203)。平均値HMにTHH2を掛けたものより情報Hの画像先鋭度値が大きい場合(ステップS1203;Yes)、画像先鋭度フラグを1とし、現時刻、顔検出領域とともに顔領域先鋭度フラグデータベースDB3H内に保存し(ステップS1204)、ステップS1206に進む。平均値HMにTHH2を掛けたものより情報Hの先鋭度値が大きくない場合(ステップS1203;No)、画像先鋭度フラグを0とし、現時刻、顔検出領域とともに顔領域先鋭度フラグデータベースDB3H内に保存し(ステップS1205)、ステップS1206に進む。
次に、画像情報取扱部16は、顔領域先鋭度フラグデータベースDB3H内の、現時刻より所定の時間T11以上古い情報を削除する(ステップS1206)。
画像情報取扱部16は、顔領域先鋭度フラグデータベースDB3H内に保存されている情報の数が所定の値N2以上であり、かつ、顔領域先鋭度フラグデータベースDB3H内の全情報で顔検出領域全体のうちの所定の領域TH13以上をカバーしているかを判定する(ステップS1207)。顔領域先鋭度フラグデータベースDB3H内に保存されている情報の数がN2以上であり、かつ、顔領域先鋭度フラグデータベースDB3H内の全情報で顔検出領域全体のうちの所定の領域TH13以上をカバーしている場合(ステップS1207;Yes)、ステップS1208に進む。顔領域先鋭度フラグデータベースDB2H内に保存されている情報の数が所定の値N2以上でない、又は、顔領域先鋭度フラグデータベースDB3H内の全情報で顔検出領域全体のうちの所定の領域TH13以上をカバーしていない場合(ステップS1207;No)、ステップS1211に進む。
ステップS1208では、画像情報取扱部16は、顔領域先鋭度フラグデータベースDB3H内の、先鋭度フラグが0である情報の割合X2を算出する。
算出した割合X2が所定の割合TH12より大きい場合(ステップS1209;Yes)、自律移動装置100の撮像部33のレンズの汚れがあると判定し(ステップS1210)、終了する。算出した割合X2が所定の割合TH12より大きくない場合(ステップS1209;Yes)、自律移動装置100の撮像部33のレンズの汚れが不明であると判定し(ステップS1211)、終了する。また、ステップS1201でNoの場合、ステップS1207でNoの場合も、自律移動装置100の撮像部33のレンズの汚れが不明であると判定し(ステップS1211)、終了する。
以上説明したように、自律移動装置100は、DBを生成1・3の処理及びレンズ汚れ判定1~3の処理において、画像先鋭度値をラプラシアンオペレータの畳み込み処理を行った後、分散を取るので、画像全体又は顔画像領域内のエッジ及び濃度を算定でき、撮像部のレンズの一部に汚れがあった場合でもレンズの汚れを好適に検出することができる。
そして、検出したレンズの汚れをユーザに知らせることにより、好ましいタイミングで、ユーザが汚れたレンズを清掃することができ、画像処理による自機位置推定及び環境地図生成のロバスト性と精度を維持することができる。
上記実施形態では、画像先鋭度値の算出にラプラシアンオペレータと分散を使用したが、離散フーリエ変換(Discrete Fourier Transform)や高速フーリエ変換(Fast Fourier Transform)をはじめとするフーリエ変換等の周波数解析(法)を用いても良い。フーリエ変換を用いた場合、例えばフーリエ変換後のパワースペクトルのうち最もパワー(周波数成分の振幅の2乗)が大きい周波数を画像先鋭度値(代表先鋭度値)とする。ラプラシアンオペレータの代わりに、ロバーツ、プリビットやソーベル等のエッジ検出オペレータでエッジ検出しても良い。
自律移動装置100は、入力部として、自律移動装置100を操作するための操作ボタンを備えていても良い。操作ボタンは、例えば、電源ボタン、モード切替ボタン(掃除モード、ペットモード等を切り替える)、初期化ボタン(地図の作成をやり直しさせる)などを含む。入力部として、音の入力を行うマイク(図示せず)と、自律移動装置100への操作指示の音声を認識する音声認識部を備えても良い。
DBを生成1及びレンズ汚れ判定1の処理では、撮像画像全体を使用して画像の判定を行っているが、自律移動装置100の下方(床近辺)は物の配置の変化が起こりやすく画像が変わりやすいので、物の配置の変化等が起こりにくい一定の高さ以上の部分の画像を使用して、判定を行っても良い。
レンズ汚れ判定1の処理では、回転姿勢を条件に用いたが、位置を用いても良い。或いは、環境地図をQ個のブロックに分け、そのうちのいくつかのブロック内の画像を有していることを条件としても良いし、位置及び回転の両方を条件としても良い。条件を満たしていない場合、条件を満たすような移動指示を発するようにしても良い。
顔検出スレッド、DBを生成3及びレンズ汚れ判定3では、顔を検出対象としていたが、顔以外の物体、例えば、衣服や眼鏡、ペット等を検出対象としても良く、特定の個人の顔のみを検出対象としても良く、TV検出としても良い。
上記実施形態では、カメラを用いるVisual SLAMを用いたが、カメラの代わりにLRF(Laser Range Finder)を用いたSLAMであっても良い。この場合、物体認識用のカメラについての汚れ検出を行う。
行動計画アプリの動作モードは、ユーザからのマニュアル操作で選択しても良く、所定のアルゴリズム、例えば、時間や人を認識した数などにより自動でモード選択をしても良い。
1つの画像先鋭度値は、1回の撮像により得られた画像又は物体検出領域から算定されるが、2回以上の撮像により得られた画像から1つの画像先鋭度値を算定しても良い。また、画像先鋭度値は、1つの画像又は物体検出領域から代表先鋭度値が1つ算定されるが、1つの画像又は物体検出領域のうちの一定の範囲毎に1つづつ算定されるようにしても良い。
汚れ検査用のマーカーシートを撮像することにより得られる画像から画像先鋭度値を算定しても良い。マーカーシートは、自律移動装置100の移動範囲内に、必要な時に配置すれば良い。マーカーシートは、汚れ検出が容易な模様、例えば市松模様等がデザインされていると良い。
上記実施形態では、撮像部33が単眼である単眼SLAMの例を示したが、複数の撮像部を用いた複眼SLAMでも同様の構成を採ることができる。例えば自律移動装置100が二台の撮像部33を備えている場合は、移動しなくても同じ位置から二つの画像を取得できるため、自機位置推定処理の初期化においてはステップS304の一回の動作で二つの画像が取得できる。それら二つの画像それぞれに基準値以上の特徴点が含まれていれば(ステップS306で判定する)、ステップS307では「N=N+2」を行い、ステップS308は常に「No」に進むことになる。そして、二つの画像の一方を前フレーム、もう一方を現フレームとして処理を行う場合は、ステップS319でスケールSを計算する際の「オドメトリ並進距離」は、二つの撮像部間の距離を用いれば良い。これによりオドメトリからの取得する位置の精度が低い場合でも安定して初期化を行うことができる。
ただし、この場合の二つの撮像部間の距離は、通常用いられる基準並進距離(例えば1m)よりも短い距離になってしまうことが多く、オドメトリから得られる並進距離の精度が高い場合は、むしろ単眼SLAMと同様の初期化処理(例えば複数ある撮像部のうちの第一撮像部のみを使った初期化処理)を行った方が高い精度が得られることもある。そこで、単眼SLAMと同様の初期化処理と、前段落で記載した二つの撮像部による初期化処理とを両方とも行って、誤差の小さいと思われる情報を採用するようにしても良い。
この発明の自律移動装置100の各機能は、通常のPC(Personal Computer)等のコンピュータによっても実施することができる。具体的には、上記実施形態では、自律移動装置100が行う自律移動制御処理のプログラムが、記憶部20のROMに予め記憶されているものとして説明した。しかし、プログラムを、フレキシブルディスク、CD-ROM(Compact Disc Read Only Memory)、DVD(Digital Versatile Disc)及びMO(Magneto-Optical Disc)等のコンピュータ読み取り可能な記録媒体に格納して配布し、そのプログラムをコンピュータに読み込んでインストールすることにより、上述の各機能を実現することができるコンピュータを構成してもよい。
以上、本発明の好ましい実施形態について説明したが、本発明は係る特定の実施形態に限定されるものではなく、本発明には、特許請求の範囲に記載された発明とその均等の範囲が含まれる。以下に、本願出願の当初の特許請求の範囲に記載された発明を付記する。
(付記1)
自機を移動及び回転させて自機位置及び自機の向きを変化させる駆動部と、
レンズを介して撮像する撮像部と、
撮像された画像を記憶する画像記憶部と、
前記駆動部、前記撮像部及び前記画像記憶部を制御し、前記画像又は前記画像に検出された物体の物体検出領域を構成する構造の細かさを示す画像先鋭度値を算定する制御部と、を備え、
前記制御部は、
前記画像記憶部に記憶された第1の画像の第1の画像先鋭度値と、
前記第1の画像が撮像された時の前記自機位置及び前記自機の向きから所定の範囲内の自機位置及び自機の向きで撮像された第2の画像の第2の画像先鋭度値とを、
又は前記第1の画像により検出された物体の第1の物体検出領域から基準領域内にある前記物体の第2の物体検出領域を含んだ第2の画像の第2の画像先鋭度値とを、算定し、前記第2の画像先鋭度値が前記第1の画像先鋭度値より低い場合、前記レンズが汚れていると判定する、
自律移動装置。
(付記2)
前記画像先鋭度値は、前記画像全体又は前記物体検出領域全体に対して算出された代表先鋭度値である、
付記1に記載の自律移動装置。
(付記3)
前記制御部は、前記画像又は前記検出領域をエッジ検出オペレータで処理し分散を取る、又は前記画像又は前記検出領域に対して周波数解析を行うことで得たパワースペクトルから最大のパワーの周波数を取得する、ことにより前記代表先鋭度値を算定する、
付記2に記載の自律移動装置。
(付記4)
前記撮像部は、物体の境界が写るように前記画像を撮像し、
前記制御部は、前記画像に写る前記物体の境界を前記エッジ検出オペレータで処理し分散を取る、又は前記周波数解析を行うことで得たパワースペクトルから最大のパワーの周波数を取得する、
付記3に記載の自律移動装置。
(付記5)
前記制御部は、前記撮像部によって前記物体の境界が写るように前記画像を撮像できる自機位置及び自機の向きに移動するように前記駆動部を制御する、
付記4に記載の自律移動装置。
(付記6)
前記物体は、顔である、
付記1から5のいずれか1つに記載の自律移動装置。
(付記7)
前記第1の画像及び前記第2の画像は、それぞれ2回以上の撮像により取得される、
付記1から6のいずれか1つに記載の自律移動装置。
(付記8)
前記第2の画像は、充電設備に向かう際に撮像されることにより取得される、
付記1から7のいずれか1つに記載の自律移動装置。
(付記9)
距離測定素子をさらに備え、
前記自機の走行距離が所定の距離に達したと前記距離測定素子によって測定された時に前記第2の画像を撮像する、
付記1から8のいずれか1つに記載の自律移動装置。
(付記10)
前記充電設備に向かう時に前記レンズの汚れを確認する、
付記8に記載の自律移動装置。
(付記11)
自機を移動及び回転させて自機位置及び自機の向きを変化させる駆動部と、レンズを介して撮像する撮像部と、前記撮像された画像を記憶する画像記憶部と、を備えた自律移動装置のレンズの汚れ検出方法であって、
第1の画像が撮像された時の自機位置及び自機の向きを記憶しておき、自機を移動及び回転させて前記第1の画像を撮影した時の自機位置及び自機の向きと所定の範囲内にある時に、第2の画像を撮像させ、前記画像記憶部に記憶された過去の画像である第1の画像と、現在の画像である前記第2の画像とを比較して、前記レンズの汚れを検出する、自律移動装置のレンズの汚れ検出方法。
(付記12)
自機を移動及び回転させて自機位置及び自機の向きを変化させる駆動部と、レンズを介して撮像する撮像部と、撮像された画像を記憶する画像記憶部と、を備えた自律移動装置のレンズの汚れ検出方法であって、
自機を移動及び回転させて、前記画像記憶部に記憶されている第1の画像により検出された物体の物体検出領域から基準領域内にある前記物体の物体検出領域を含んだ第2の画像を撮像させ、前記画像記憶部に記憶された過去の画像である前記第1の画像と、現在の画像である前記第2の画像とを比較して、前記レンズの汚れを検出する、自律移動装置のレンズの汚れ検出方法。
(付記13)
画像記憶部に記憶された第1の画像が撮像された時の自機位置及び自機の向きから所定の範囲内の自機位置及び自機の向き、又は前記第1の画像により検出された物体の第1の物体検出領域から基準領域内にある前記物体の第2の物体検出領域を含んだ第2の画像を撮像する自機位置及び自機の向きに駆動部によって自機を移動及び回転させることにより、撮像部に第2の画像を撮像させる撮像ステップと、
前記駆動部、前記撮像部及び前記画像記憶部を制御する制御部が、前記第1の画像の第1の画像先鋭度値と前記第2の画像の第2の画像先鋭度値とを算定する算定ステップと、
前記第2の画像先鋭度値が前記第1の画像先鋭度値より低い場合、前記制御部は、前記撮像部のレンズが汚れていると判定する判定ステップと、
を備える自律移動装置のレンズの汚れ検出方法。
(付記14)
自機を移動及び回転させて自機位置及び自機の向きを変化させる駆動部と、
レンズを介して画像を撮像する撮像部と、
撮像された前記画像を記憶する画像記憶部と、
前記駆動部、前記撮像部及び前記画像記憶部を制御してVisual SLAM制御を行なう制御部と、
を備えた自律移動装置のレンズの汚れ検出方法であって、
前記画像記憶部に記憶された第1の画像の特徴点と対応する前記撮像部で撮像した第2の画像の対応特徴点の数が、基準対応特徴点数未満である場合をVisual SLAM制御の失敗とし、
基準並進距離を並進する間における前記Visual SLAM制御の失敗率が基準失敗率より高い場合、レンズが汚れていると判定する、自律移動装置のレンズの汚れ検出方法。
(付記15)
コンピュータに、
画像記憶部に記憶された第1の画像が撮像された時の自機位置及び自機の向きから所定の範囲内の自機位置及び自機の向き、又は前記第1の画像により検出された物体の第1の物体検出領域から基準領域内にある前記物体の第2の物体検出領域を含んだ第2の画像を撮像する自機位置及び自機の向きに駆動部によって自機を移動及び回転させることにより、撮像部に第2の画像を撮像させる撮像ステップと、
前記駆動部、前記撮像部及び前記画像記憶部を制御する制御部が、前記第1の画像の第1の画像先鋭度値と前記第2の画像の第2の画像先鋭度値とを算定する算定ステップと、
前記第2の画像先鋭度値が前記第1の画像先鋭度値より低い場合、前記制御部は、前記撮像部のレンズが汚れていると判定する判定ステップと、
を実行させるためのプログラム。