(57)【要約】【課題】作成した地図を用いる技術を改善する。【解決手段】自律移動装置１００は、撮像部４１と、地図作成部１１と、天井推定部１３と、判定部１４と、判定結果処理部と、を備える。地図作成部１１は、撮像部４１が撮影した複数の画像の情報を用いて地図の情報を作成する。天井推定部１３は、撮像部４１が撮影した複数の画像の情報を用いて天井平面を推定する。判定部１４は、天井推定部１３が推定した天井平面を用いて地図の情報に誤差が基準以上多く含まれているか否かを判定する。判定結果処理部は、判定部１４が判定した結果に応じて所定の処理を行う。【選択図】図３
2018156538.tif 000002
【特許請求の範囲】
【請求項１】
撮像部と、
前記撮像部が撮影した複数の画像の情報を用いて地図の情報を作成する地図作成部と、
前記撮像部が撮影した複数の画像の情報を用いて天井平面を推定する天井推定部と、
前記天井推定部が推定した天井平面を用いて、前記地図の情報に誤差が基準以上多く含まれているか否かを判定する判定部と、
前記判定部が判定した結果に応じて所定の処理を行う判定結果処理部と、
を備える自律移動装置。
【請求項２】
前記地図作成部が作成した地図の情報を記憶する地図記憶部を備え、
前記判定部が前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていると判定した場合、前記判定結果処理部は前記地図記憶部に記憶されている地図の情報を編集する、
請求項１に記載の自律移動装置。
【請求項３】
前記判定結果処理部は、前記判定部が前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていると判定した場合、前記地図記憶部に記憶されている地図の情報を消去する、
請求項２に記載の自律移動装置。
【請求項４】
前記判定部が前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていないと判定した場合に前記地図記憶部に記憶されている地図の情報を保存する地図保存部を備え、
前記判定結果処理部は、前記判定部が前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていると判定した場合、前記地図保存部に保存されている地図の情報を前記地図記憶部に記憶させる、
請求項２又は３に記載の自律移動装置。
【請求項５】
前記判定部が前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていないと判定した時刻の情報を記憶する時刻記憶部を備え、
前記地図記憶部は、地図を作成した時刻の情報と共に地図の情報を記憶し、
前記判定結果処理部は、前記判定部が前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていると判定した場合、前記地図記憶部に記憶されている地図の情報のうち、前記時刻記憶部に記憶されている時刻よりも後に作成された地図の情報を消去する、
請求項２から４のいずれか１項に記載の自律移動装置。
【請求項６】
撮影した画像の情報を、撮影した時刻の情報と共に記憶する画像記憶部を備え、
前記判定結果処理部は、前記判定部が前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていると判定した場合、前記画像記憶部に記憶されている画像の情報のうち、前記時刻記憶部に記憶されている時刻よりも後に撮影された画像の情報を消去する、
請求項５に記載の自律移動装置。
【請求項７】
前記撮像部が撮影した画像の中から該画像に含まれている特徴的な部分である特徴点の３次元位置を取得する３次元位置取得部を備え、
前記判定部は、前記３次元位置取得部が取得した３次元位置のうち、前記天井推定部が推定した天井平面との距離が基準天井距離以下の３次元位置の個数が基準個数未満の場合に、前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていると判定する、
請求項２から６のいずれか１項に記載の自律移動装置。
【請求項８】
前記判定部は、前記天井推定部が推定した天井平面の傾きと、自律移動装置が移動している床面の傾きと、の角度の差が基準角度以上なら、前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていると判定する、
請求項２から７のいずれか１項に記載の自律移動装置。
【請求項９】
前記判定部は、前記天井推定部が推定した第１の天井平面の傾きと、前記天井推定部が前記第１の天井平面よりも前に推定した第２の天井平面の傾きと、の角度の差が基準角度以上なら、前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていると判定する、
請求項２から８のいずれか１項に記載の自律移動装置。
【請求項１０】
前記判定部は、自機と前方の壁との距離を取得し、該距離が前方距離基準値未満のときには、前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていると判定しない、
請求項２から９のいずれか１項に記載の自律移動装置。
【請求項１１】
前記天井推定部は、３次元ハフ変換によって天井平面を推定する、
請求項１から１０のいずれか１項に記載の自律移動装置。
【請求項１２】
前記撮像部は、単眼の撮像部であり、
前記地図作成部は前記単眼の撮像部が撮影した複数の画像の情報に基づいて地図の情報を作成する、
請求項１から１１のいずれか１項に記載の自律移動装置。
【請求項１３】
床と接しているか否かを検出する床面センサを備え、
前記床面センサが、床と接していると検出している時のみ、前記地図作成部は地図の情報を作成する、
請求項１から１２のいずれか１項に記載の自律移動装置。
【請求項１４】
撮影した複数の画像の情報を用いて地図の情報を作成するとともに天井平面を推定し、
前記推定した天井平面を用いて前記作成した地図の情報に誤差が基準以上多く含まれているか否かを判定し、
前記判定した結果に応じて所定の処理を行う、
画像処理方法。
【請求項１５】
撮影ステップと、
前記撮影ステップで撮影した複数の画像の情報を用いて地図の情報を作成する地図作成ステップと、
前記撮影ステップで撮影した複数の画像の情報を用いて天井平面を推定する天井推定ステップと、
前記天井推定ステップで推定した天井平面を用いて前記地図作成ステップで作成した地図の情報に誤差が基準以上多く含まれているか否かを判定する判定ステップと、
前記判定ステップで判定した結果に応じて所定の処理を行う判定結果処理ステップと、
を備える画像処理方法。
【請求項１６】
コンピュータに、
撮影ステップ、
前記撮影ステップで撮影した複数の画像の情報を用いて地図の情報を作成する地図作成ステップ、
前記撮影ステップで撮影した複数の画像の情報を用いて天井平面を推定する天井推定ステップ、
前記天井推定ステップで推定した天井平面を用いて前記地図作成ステップで作成した地図の情報に誤差が基準以上多く含まれているか否かを判定する判定ステップ、及び、
前記判定ステップで判定した結果に応じて所定の処理を行う判定結果処理ステップ、
を実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自律移動装置、画像処理方法及びプログラムに関する。
【背景技術】
【０００２】
用途に応じて自律的に移動する自律移動装置が普及してきている。例えば、屋内の掃除のために自律的に移動する自律移動装置が知られている。一般的に、このような自律移動装置は、実空間の地図の作成と、実空間内での自機位置の推定とを、両方とも行う必要がある。
【０００３】
実空間の地図の作成と実空間内での自機位置の推定とを同時に行う手法として、ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ  Ｌｏｃａｌｉｚａｔｉｏｎ  Ａｎｄ  Ｍａｐｐｉｎｇ）と呼ばれる手法が知られている。ＳＬＡＭ法では、カメラの撮影する動画像の各フレームから特徴点を抽出し、異なるフレームにおいて同一の特徴点を追跡することによって、自機の３次元位置（カメラ位置）と特徴点の３次元位置（これが集まって地図の情報を構成する）とを交互に推定する処理を行う。例えば特許文献１には、ＳＬＡＭ法を用いて周囲の地図を作成しながら移動することができる移動式ロボットが開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１６−５２５１５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
特許文献１に開示されている移動式ロボットは、ＳＬＡＭ法を用いることによって、地図を作成しながら移動することができる。しかし、前記の移動式ロボットでは作成した地図を用いる技術に改善の余地がある。
【０００６】
本発明は、作成した地図を用いる技術を改善することを目的とする。
【課題を解決するための手段】
【０００７】
本発明の自律移動装置は、
撮像部と、
前記撮像部が撮影した複数の画像の情報を用いて地図の情報を作成する地図作成部と、
前記撮像部が撮影した複数の画像の情報を用いて天井平面を推定する天井推定部と、
前記天井推定部が推定した天井平面を用いて、前記地図の情報に誤差が基準以上多く含まれているか否かを判定する判定部と、
前記判定部が判定した結果に応じて所定の処理を行う判定結果処理部と、
を備える。
【発明の効果】
【０００８】
本発明によれば、作成した地図を用いる技術を改善した自律移動装置、画像処理方法及びプログラムを提供することができる。
【図面の簡単な説明】
【０００９】
【図１】本発明の実施形態１に係る自律移動装置の外観を示す図である。
【図２】実施形態１に係る自律移動装置の備える撮像部が取得する画像の一例を示す図である。
【図３】実施形態１に係る自律移動装置の構成を示す図である。
【図４】実施形態１に係る自律移動制御処理全体のフローチャートを示す図である。
【図５】実施形態１に係る自律移動制御処理の中の自機位置推定スレッドの初期化処理のフローチャートを示す図である。
【図６】実施形態１に係る自律移動制御処理の中の自機位置推定スレッドの初期化処理後の自機位置推定処理のフローチャートを示す図である。
【図７】実施形態１に係る自律移動制御処理の中の地図作成スレッドの処理のフローチャートを示す図である。
【図８】実施形態１に係る自律移動制御処理の中のループクロージングスレッドの処理のフローチャートを示す図である。
【図９】実施形態１に係る自律移動制御処理の中の誤差判定スレッドの処理のフローチャートを示す図である。
【図１０Ａ】３次元ハフ変換における実空間を説明する図である。
【図１０Ｂ】３次元ハフ変換におけるハフ空間を説明する図である。
【図１１】前方の壁までの距離が小さくなると取得できる天井の画像が小さくなることを説明する図である。
【図１２】本発明の実施形態２に係る誤差判定スレッドの処理のフローチャートを示す図である。
【図１３】本発明の実施形態３に係る誤差判定スレッドの処理のフローチャートを示す図である。
【図１４】制御部等の一部を外部のサーバーに設ける構成の一例を示す図である。
【発明を実施するための形態】
【００１０】
以下、本発明の実施形態に係る自律移動装置、自律移動方法及びプログラムについて、図表を参照して説明する。なお、図中同一又は相当部分には同一符号を付す。
【００１１】
（実施形態１）
本発明の実施形態に係る自律移動装置は、周囲の地図を作成しながら、用途に応じて屋内を自律的に移動する装置である。この用途とは、例えば、警備監視用、屋内掃除用、ペット用、玩具用等である。
【００１２】
自律移動装置１００は、図１に示すように、外観上、撮像部４１、駆動部４２を備える。
【００１３】
撮像部４１は、単眼の撮像装置（カメラ）を備える。撮像部４１は、例えば、３０ｆｐｓで画像（フレーム）を取得する。自律移動装置１００は、撮像部４１が逐次取得した画像に基づいて、自機位置と周囲環境とをリアルタイムに認識しながら、自律移動を行う。撮像部４１は、天井方向の画像を取得できるように広角レンズが使用されており、図２に示すような、天井を含む画像を取得する。
【００１４】
駆動部４２は、独立２輪駆動型であって、車輪とモータとを備える移動手段である。自律移動装置１００は、２つの車輪の同一方向駆動により前後の平行移動（並進移動）を、２つの車輪の逆方向駆動によりその場での回転（向き変更）を、２つの車輪のそれぞれ速度を変えた駆動により旋回移動（並進＋回転（向き変更）移動）を、行うことができる。また、各々の車輪にはロータリエンコーダが備えられており、ロータリエンコーダで車輪の回転数を計測し、車輪の直径や車輪間の距離等の幾何学的関係を利用することで並進移動量及び回転量を計算できる。例えば、車輪の直径をＤ、回転数をＲ（ロータリエンコーダにより測定）とすると、その車輪の接地部分での並進移動量はπ・Ｄ・Ｒとなる。また、車輪の直径をＤ、車輪間の距離をＩ、右車輪の回転数をＲＲ、左車輪の回転数をＲＬとすると、向き変更の回転量は（右回転を正とすると）３６０°×Ｄ×（ＲＬ−ＲＲ）／（２×Ｉ）となる。この並進移動量や回転量を逐次足し合わせていくことで、駆動部４２は、いわゆるオドメトリとして機能し、自機位置（移動開始時の位置及び向きを基準とした位置及び向き）を計測することができる。
【００１５】
オドメトリから得られる自機の位置及び向きの精度は、車輪の摩耗やスリップ等により、低い精度になってしまうことが多い。特に誤差が累積していく関係上、時間経過と共に精度が悪化する。ただし、オドメトリの回転成分（向きの情報）に関しては、角速度（単位時間あたりの角度移動量）を測定する角速度センサを用いることにより精度を向上させることができる。また、地磁気を検出して方位を特定する方位センサ（図示せず）を用いることにより、オドメトリからの取得値とは関係なく、地磁気を利用した絶対的な向きの情報を取得することもできる。
【００１６】
なお、車輪の代わりにクローラを備えるようにしても良いし、複数（例えば二本）の足を備えて足で歩行することによって移動を行うようにしても良い。これらの場合も、二つのクローラの動きや、足の動きに基づいて、車輪の場合と同様に自機の位置や向きの計測が可能である。
【００１７】
図３に示すように、自律移動装置１００は、撮像部４１、駆動部４２に加えて、制御部１０、記憶部２０、センサ部３０、入力部４３、通信部４４、電源４５、を備える。
【００１８】
制御部１０は、ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）等で構成され、記憶部２０に記憶されたプログラムを実行することにより、後述する各部（地図作成部１１、位置推定部１２、天井推定部１３、判定部１４、地図編集部１５）の機能を実現する。
【００１９】
記憶部２０は、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）等で構成され、画像記憶部２１、地図記憶部２２及び天井推定値記憶部２３を含む。ＲＯＭには制御部１０のＣＰＵが実行するプログラム（例えば、後述するＳＬＡＭ法の演算や自律移動制御処理に係るプログラム）や、プログラムを実行する上で予め必要なデータが記憶されている。ＲＡＭには、プログラム実行中に作成されたり変更されたりするデータが記憶される。
【００２０】
画像記憶部２１には、撮像部４１が撮影した画像が記憶される。ただし、記憶容量の節約のため、画像記憶部２１は、撮像部４１が撮影したすべての画像を記憶しなくてもよく、また画像自体ではなく、画像の特徴量を記憶するようにしても良い。重要な画像（後述するキーフレーム）については、位置推定部１２が後述するＳＬＡＭ法で推定した、該画像を撮影した時の自機位置（自機の位置及び向き）の情報も、該画像の情報と共に（該画像に紐付けられて）画像記憶部２１に記憶される。
【００２１】
地図記憶部２２には、後述するＳＬＡＭ法や障害物センサ３１からの情報に基づいて地図作成部１１が作成した地図の情報（特徴点や障害物の３次元位置の情報）が記憶される。
【００２２】
天井推定値記憶部２３には、天井推定部１３が推定した、天井の平面を表す方程式の係数を記憶する。天井の平面を表す方程式については後述する。
【００２３】
センサ部３０として、障害物センサ３１及び床面センサ３２を備える。障害物センサ３１は、周囲に存在する物体（障害物）を検出し、該物体（障害物）までの距離を測定することができる距離センサであって、例えば、赤外線センサ、超音波センサである。なお、独立した障害物センサ３１を搭載せずに、撮像部４１を用いて障害物を検知するようにしても良い。また、他の物体に衝突したことを検知するバンパーセンサー（図示せず）を備えても良い。床面センサ３２は、自律移動装置１００が床面に接しているか否かを検知するセンサである。自律移動装置１００が床面の上を移動している間は、床面センサ３２は自律移動装置１００が床面に接していることを検知する。例えば、人が自律移動装置１００を持ち上げると、床面センサ３２は自律移動装置１００が床面から離れたことを検知する。
【００２４】
入力部４３として、自律移動装置１００を操作するための操作ボタンを備える。操作ボタンは、例えば、電源ボタン、モード切替ボタン（掃除モード、ペットモード等を切り替える）、初期化ボタン（地図の作成をやり直しさせる）などを含む。入力部４３として、音の入力を行うマイク（図示せず）と、自律移動装置１００への操作指示の音声を認識する音声認識部を備えても良い。
【００２５】
通信部４４は、外部装置と通信するためのモジュールであり、外部装置と無線通信する場合にはアンテナを含む無線モジュールである。例えば、通信部４４は、Ｂｌｕｅｔｏｏｔｈ（登録商標）に基づく近距離無線通信を行うための無線モジュールである。通信部４４では、自律移動装置１００と外部とのデータ受け渡し等を行う。
【００２６】
電源４５は、自律移動装置１００を動作させる電源であり、一般的には内蔵された充電池であるが、太陽電池であっても良いし、床面から無線で電力供給されるシステムであっても良い。電源４５が充電池の場合は、充電器（充電ステーション）に自律移動装置１００が接続することで充電される。
【００２７】
次に、自律移動装置１００の制御部１０の機能的構成について説明する。制御部１０は、地図作成部１１、位置推定部１２、天井推定部１３、判定部１４、地図編集部１５、の機能を実現し、後述するＳＬＡＭ法の演算や自律移動装置１００の移動制御等を行う。また、制御部１０は、マルチスレッド機能に対応しており、複数のスレッド（異なる処理の流れ）を並行して進めることができる。
【００２８】
地図作成部１１は、画像記憶部２１に記憶されている画像の情報並びに該画像撮影時の自機の位置及び向きの情報に基づいてＳＬＡＭ法を用いて後述する特徴点の３次元位置を推定したり、障害物センサ３１で障害物を検出したときの自機の位置及び向きの情報に基づいて該障害物の３次元位置を取得したりすることによって、地図の情報を作成する。そして、この推定された特徴点の３次元位置、取得された該障害物の３次元位置等を、地図の情報として地図記憶部２２に記憶する。なお、画像上の２次元空間に存在する特徴点を、後述するＳＬＡＭ座標の３次元空間上に対応させたものをＭａｐ点と言う。１つのＭａｐ点は複数の画像から観測されることもある。つまり、１つのＭａｐ点は複数の（画像上の）特徴点と対応づけられることもある。
【００２９】
位置推定部１２は、後述するＳＬＡＭ法に基づいて、ビジュアルオドメトリとして、自機の位置及び向きを推定する。
【００３０】
天井推定部１３は、後述する３次元ハフ（Ｈｏｕｇｈ）変換により、地図記憶部２２に記憶されている特徴点の中から天井に存在する特徴点（天井特徴点）を抽出し、天井の平面（天井平面）を推定する。そして、推定した天井平面を表す方程式の係数（天井係数）を天井推定値記憶部２３に記憶する。
【００３１】
判定部１４は、後述するキーフレーム中に含まれる特徴点に対応したＭａｐ点のうち、天井推定値記憶部２３に記憶されている天井係数で表される天井平面との距離が基準距離（例えば１０ｃｍ）以下であるＭａｐ点の個数を求め、該個数が基準個数（例えば２個）未満であるなら、地図記憶部２２に記憶されている地図の情報に誤差が基準以上多く含まれていると判定する。
【００３２】
この判定部１４の判定について、補足説明する。撮像部４１が取得する画像は、図２に示すように、天井の部分を多く含んでいるため、通常は、キーフレーム内の特徴点に対応したＭａｐ点に天井に存在するものが多く含まれる。そして、これら天井に存在するＭａｐ点と天井平面との距離は０である。したがって、天井推定値記憶部２３に記憶されている天井係数で表される天井平面が正しい天井平面であるなら、キーフレーム中に含まれる特徴点に対応したＭａｐ点のうち、天井推定値記憶部２３に記憶されている天井係数で表される天井平面との距離がほぼ０（つまり基準距離以下）であるＭａｐ点は多く（基準個数以上）含まれる。
【００３３】
つまり、キーフレーム中に含まれる特徴点に対応したＭａｐ点のうち、天井推定値記憶部２３に記憶されている天井係数で表される天井平面との距離が基準距離以下であるＭａｐ点の個数が基準個数未満ということは（該天井係数がほぼ正しいと仮定するなら）、Ｍａｐ点の３次元位置が誤っているということを意味する。Ｍａｐ点の３次元位置の情報は、地図記憶部２２に記憶されている地図の情報の一部であるため、Ｍａｐ点の３次元位置が誤っているということは、地図記憶部２２に記憶されている地図の情報にも誤差が多く含まれていると推測することができる。したがって、上記判定によって、地図記憶部２２に記憶されている地図の情報に誤差が基準以上多く含まれているか否かを判定することができる。
【００３４】
地図編集部１５は、判定部１４によって地図記憶部２２に記憶されている地図の情報に含まれる誤差が多いと判定された場合は、該地図を編集する。地図の編集の最も簡単な処理は、該地図を消去する処理である。
【００３５】
以上で、自律移動装置１００の機能構成を説明した。次に自律移動装置１００の自律移動制御処理について、図４を参照して説明する。図４は、自律移動装置１００の自律移動制御処理全体のフローチャートである。自律移動装置１００は、電源が投入されると、この自律移動制御処理が開始される。まず、自律移動装置１００の制御部１０は、自機位置推定スレッド（ステップＳ１０１）、地図作成スレッド（ステップＳ１０２）、ループクロージングスレッド（ステップＳ１０３）、誤差判定スレッド（ステップＳ１０４）の各スレッドを起動する。これら各スレッドの詳細については後述する。
【００３６】
その後、制御部１０は、自律移動制御処理による動作を終了するか否かを判定し（ステップＳ１０５）、動作終了なら（ステップＳ１０５；Ｙｅｓ）動作を終了し、動作終了でなければ（ステップＳ１０５；Ｎｏ）、地図作成部１１が地図の情報の作成及び更新を行う（ステップＳ１０６）。次に制御部１０は自律移動するために駆動部４２に所望の動作を指示し（ステップＳ１０７）、ステップＳ１０５に戻る。この自律移動制御処理により、地図の情報に基づいて自律的に移動を行いつつ、適宜地図の情報を更新することができる。
【００３７】
自律移動装置１００による自律移動制御処理の典型的な例を紹介する。自律移動装置１００は最初、充電器に置いてある状態で電源４５を投入すると、障害物センサ３１を頼りとして、家の各部屋をくまなく移動し、障害物センサ３１によって壁等の障害物位置を特定し、障害物位置を含む地図の情報を作成することができる。地図がある程度作成されると、地図の情報がまだないが移動可能と考えられる領域を知ることができ、その領域に自律的に移動する等して、より広範囲の地図の作成を促すこともできるようになる。そして、移動可能なほぼ全域の地図の情報が作成されれば、地図の情報を利用した効率的な移動動作が可能になる。例えば部屋のどの位置からでも最短経路で充電器に戻ったり、効率的に部屋の掃除をしたりすることが可能になる。
【００３８】
次に、自律移動制御処理の全体フローチャート（図４）のステップＳ１０１で起動される自機位置推定スレッドについて、図５を参照して説明する。このスレッドは、位置推定部１２が、最初に初期化処理を行い、その後自機位置推定（撮像部４１で取得した画像を用いてビジュアルオドメトリにより自機位置を推定する）を続ける処理である。
【００３９】
位置推定部１２は、まず、初期化済フラグをクリアする（ステップＳ２０１）。初期化済フラグとは、位置推定部１２が、自機位置を推定するために最初に行う初期化処理が完了しているか否かを示すフラグ変数である。初期化処理が完了すると初期化済フラグには１がセットされる。次に、自律移動制御処理による動作を終了するか否かを判定する（ステップＳ２０２）。動作終了なら（ステップＳ２０２；Ｙｅｓ）終了し、動作終了でないなら（ステップＳ２０２；Ｎｏ）、初期化済であるか否かを判定する（ステップＳ２０３）。この判定は、初期化済フラグが１（初期化済）か０（初期化済でない）かにより判定する。初期化済なら（ステップＳ２０３；Ｙｅｓ）、図６に示すステップＳ２３１以降の自機位置推定処理を行い、初期化済でないなら（ステップＳ２０３；Ｎｏ）ステップＳ２０４に進んで初期化処理を行う。まず初期化処理について説明する。
【００４０】
初期化処理では、位置推定部１２はまずフレームカウンタＮに−１をセットする（ステップＳ２０４）。そして、位置推定部１２は、床面センサ３２により自律移動装置１００が床面に接していることが検知されていれば撮像部４１で画像を取得する（ステップＳ２０５）。ステップＳ２０５は撮影ステップとも呼ばれる。床面センサ３２により床面に接していない（持ち上げられている）ことが検知されている間は、床面に接したことが検知されるまで位置推定部１２はステップＳ２０５以降の処理を行わずに待ち続ける。画像は例えば３０ｆｐｓで取得することができる。なお、撮像部４１が取得した画像はフレームとも呼ばれる。次に、取得した画像内から特徴点を取得する（ステップＳ２０６）。特徴点とは画像中のコーナー部分など、画像内の特徴的な部分のことを言う。特徴点は、ＳＩＦＴ（Ｓｃａｌｅ−Ｉｎｖａｒｉａｎｔ  Ｆｕｔｕｒｅ  Ｔｒａｎｓｆｏｒｍ）やＳＵＲＦ（Ｓｐｅｅｄ−Ｕｐ  Ｒｏｂｕｓｔ  Ｆｅａｔｕｒｅｓ）等のアルゴリズムを用いて取得することができる。なお、特徴点を取得するのにこれら以外のアルゴリズムを使っても良い。ステップＳ２０６において、位置推定部１２は、特徴点取得部として機能する。
【００４１】
取得した特徴点の個数が少ないと、後述するＴｗｏ−ｖｉｅｗ  Ｓｔｒｕｃｔｕｒｅ  ｆｒｏｍ  Ｍｏｔｉｏｎ法による姿勢の計算ができない。そのため、位置推定部１２は、特徴点の取得数が基準値（例えば１０個）以上か否かを判定する（ステップＳ２０７）。特徴点の取得数が基準値未満だった場合（ステップＳ２０７；Ｎｏ）はステップＳ２０５に戻り、基準値以上の特徴点数が得られるまで、画像の取得と特徴点の取得とを繰り返す。なお、この時点ではまだ地図の情報は作成できていないが、例えば上述した典型的な例では、障害物センサ３１を頼りとして、家の各部屋をくまなく移動し始めているため、この初期化処理で画像取得と特徴点取得とを繰り返していれば、移動しながら画像取得を繰り返すことになるので、様々な画像が取得でき、いずれは特徴点の数が多い画像を取得できることが期待できる。
【００４２】
特徴点の取得数が基準値以上だった場合は（ステップＳ２０７；Ｙｅｓ）、位置推定部１２はフレームカウンタＮをインクリメントする（ステップＳ２０８）。そして、フレームカウンタＮが０かどうかを判定する（ステップＳ２０９）。フレームカウンタＮが０なら（ステップＳ２０９；Ｙｅｓ）、画像をまだ一つしか取得していないということなので、２枚目の画像を取得するためにステップＳ２０５に戻る。なお、図５に示すフローチャートには記載していないが、１枚目の画像を取得した時の自機の位置と２枚目の画像を取得する時の自機の位置とがある程度離れていた方が、これ以降の処理で推定する姿勢の精度が向上する。そこで、ステップＳ２０９からステップＳ２０５に戻る際に、全体フローチャート（図４）のステップＳ１０７での駆動部４２への動作指示により、オドメトリによる並進距離が所定の距離（例えば１ｍ）以上となるまで待つ処理を追加しても良い。
【００４３】
フレームカウンタＮが０でないなら（ステップＳ２０９；Ｎｏ）、二つの画像を取得したということなので、位置推定部１２は、これら二つの画像間で特徴点の対応を取得する（ステップＳ２１０）。ここで、この特徴点の対応について、補足説明する。実環境上の同一の点が、これら二つの画像それぞれの中に特徴点として存在する場合に、この二つの特徴点の組を特徴点の対応と呼ぶ。そして、このような特徴点の組を取得することを特徴点の対応の取得と呼び、取得できた特徴点の組の数を特徴点の対応数と呼ぶ。
【００４４】
そして、特徴点の対応数が５未満であれば、後述する二つの画像間の相対姿勢の推定ができないため、位置推定部１２は特徴点の対応数が５未満か否かを判定する（ステップＳ２１１）。特徴点の対応数が５点未満であれば（ステップＳ２１１；Ｙｅｓ）、初期画像を取得し直すために、ステップＳ２０４に戻る。特徴点の対応数が５点以上であれば（ステップＳ２１１；Ｎｏ）、位置推定部１２は、Ｔｗｏ−ｖｉｅｗ  Ｓｔｒｕｃｔｕｒｅ  ｆｒｏｍ  Ｍｏｔｉｏｎ法を用いることにより、二つの画像間の相対姿勢（それぞれの画像を取得した位置の差分（並進ベクトルｔ）及び向きの差分（回転行列Ｒ））を計算する（ステップＳ２１２）。
【００４５】
この二つの画像間の相対姿勢の計算は、具体的には、対応する特徴点から基礎行列Ｅを求め、基礎行列Ｅを並進ベクトルｔと回転行列Ｒとに分解することによって行う。なお、この計算で得られる並進ベクトルｔ（３次元空間内で移動することを想定すると、最初の画像を取得した位置を原点として、Ｘ，Ｙ，Ｚの３要素を持つ）の各要素の値は実環境上での値とは異なる（Ｔｗｏ−ｖｉｅｗ  Ｓｔｒｕｃｔｕｒｅ  ｆｒｏｍ  Ｍｏｔｉｏｎ法では実環境上の値自体を得ることはできず、実環境と相似する空間上での値を得ることになる。）ため、これらをＳＬＡＭ空間上での値とみなし、以下ではＳＬＡＭ空間上での座標（ＳＬＡＭ座標）を用いて説明する。
【００４６】
二つの画像間の相対姿勢（並進ベクトルｔ及び回転行列Ｒ）が求まると、その値は、最初の画像を基準（最初の画像を取得した位置をＳＬＡＭ座標の原点、並進ベクトルは０ベクトル、回転行列は単位行列Ｉとする。）にした場合の、二枚目の画像の姿勢（二つ目の画像を取得した時の自機の位置（並進ベクトルｔ）及び向き（回転行列Ｒ））となる。ここで、二つの画像それぞれの姿勢（該画像（フレーム）撮影時の自機の位置（並進ベクトルｔ）及び向き（回転行列Ｒ）で、フレーム姿勢とも言う。）が求まっている場合、その二つの画像間で対応が取れている特徴点（対応特徴点）のＳＬＡＭ座標での３次元位置を、以下の考え方に基づき、地図作成部１１が求める（ステップＳ２１３）。この処理において、地図作成部１１は、３次元位置取得部として機能する。
【００４７】
特徴点の画像中の座標（フレーム座標：既知）を（ｕ，ｖ）とし、その特徴点のＳＬＡＭ座標での３次元位置（未知）を（Ｘ，Ｙ，Ｚ）とすると、これらを同次座標で表したときのこれらの関係は、透視投影行列Ｐを用いて下記の式（１）で表される。ここで、「〜」の記号は「非零の定数倍を除いて等しい」（つまり、等しいか又は定数（非零）倍になっている）ことを表し、「’」の記号は「転置」を表す。
（ｕ  ｖ  １）’〜Ｐ（Ｘ  Ｙ  Ｚ  １）’…（１）
【００４８】
上記の式（１）において、Ｐは３×４の行列で、カメラの内部パラメータを示す３×３の行列Ａと、その画像の姿勢（フレーム姿勢）を示す外部パラメータＲ及びｔから以下の式（２）で表される。ここで、（Ｒ｜ｔ）は、回転行列Ｒの右に並進列ベクトルｔを並べた行列を表す。
Ｐ＝Ａ（Ｒ｜ｔ）…（２）
【００４９】
上記の式（２）において、Ｒ及びｔは上述したようにそのフレーム姿勢として求められている。また、カメラの内部パラメータＡは、焦点距離と撮像素子サイズにより決まるので、撮像部４１を決めておけば定数となる。
【００５０】
二つの画像間で対応が取れている特徴点のうちの一つが、一つ目の画像のフレーム座標（ｕ１，ｖ１）と、二つ目の画像のフレーム座標（ｕ２，ｖ２）に写っているとすると、以下の式（３）及び式（４）ができる。ここで、Ｉは単位行列、０はゼロベクトル、（Ｌ｜ｒ）は、行列Ｌの右に列ベクトルｒを並べた行列を表す。
（ｕ１  ｖ１  １）’〜Ａ（Ｉ｜０）（Ｘ  Ｙ  Ｚ  １）’…（３）
（ｕ２  ｖ２  １）’〜Ａ（Ｒ｜ｔ）（Ｘ  Ｙ  Ｚ  １）’…（４）
【００５１】
上記の式（３）及び式（４）において、ｕ１，ｖ１，ｕ２，ｖ２それぞれについての式ができるため、式は４つできるが、未知数はＸ，Ｙ，Ｚの３つなので、Ｘ，Ｙ，Ｚを求めることができ、これがその特徴点のＳＬＡＭ座標における３次元位置となる。なお、式の個数の方が未知数の個数よりも多いため、例えばｕ１，ｖ１，ｕ２で求めたＸ，Ｙ，Ｚとｕ１，ｖ１，ｖ２で求めたＸ，Ｙ，Ｚとが異なる場合がありうる。このような場合は、過剰条件の連立一次方程式となり、一般には解が存在しないが、地図作成部１１は、最小二乗法を用いて、最も確からしいＸ，Ｙ，Ｚを求める。
【００５２】
特徴点のＳＬＡＭ座標における３次元位置（Ｘ，Ｙ，Ｚ）が求まったら、それをＭａｐ点として、地図作成部１１がＭａｐ点データベース（Ｍａｐ点ＤＢ（Ｄａｔａｂａｓｅ）とも言い、地図記憶部２２に格納される）に登録する（ステップＳ２１４）。Ｍａｐ点データベースに登録する要素としては、少なくとも、「特徴点のＳＬＡＭ座標における３次元位置であるＸ，Ｙ，Ｚ」と、「その特徴点の特徴量」（例えばＳＩＦＴ等で得た特徴量）が必要である。また、「タイムスタンプ」（後述するキーフレームカウンタＮＫＦ（現在のキーフレーム番号を表す変数）のＭａｐ点データベースへの登録時点での値等）をＭａｐ点データベースへの登録要素に追加しておくと、Ｍａｐ点データベースを編集（過去の状態に戻す等）する際に便利である。
【００５３】
そして、地図作成部１１は、二つの画像間で対応が取れている特徴点（対応特徴点）の全てをＭａｐ点データベースに登録したかを判定する（ステップＳ２１５）。まだ全てを登録していないなら（ステップＳ２１５；Ｎｏ）、ステップＳ２１３に戻り、全て登録したら（ステップＳ２１５；Ｙｅｓ）、ステップＳ２１６に進む。
【００５４】
次に位置推定部１２は、ＮＫＦ（キーフレーム（後に続くスレッドでの処理対象となる画像を指す）のカウンタを表す変数）を０に初期化し（ステップＳ２１６）、二つ目の画像をキーフレームとしてフレームデータベース（フレームＤＢ（Ｄａｔａｂａｓｅ）とも言い、画像記憶部２１に格納される）に登録する（ステップＳ２１７）。
【００５５】
フレームデータベースに登録する要素は、「キーフレーム番号」（登録時点でのキーフレームカウンタＮＫＦの値）、「姿勢」（その画像撮影時の自機のＳＬＡＭ座標内での位置（並進ベクトルｔ）及び向き（回転行列Ｒ））、「抽出した全ての特徴点」、「全ての特徴点の中でＭａｐ点として３次元位置が既知の点」、「キーフレーム自体の特徴」、である。
【００５６】
上記中、「キーフレーム自体の特徴」とは、キーフレーム間の画像類似度を求める処理を効率化するためのデータであり、通常は画像中の特徴点のヒストグラム等を用いるのが良いが、画像自体を「キーフレーム自体の特徴」としても良い。
【００５７】
次に、位置推定部１２は、キーフレームが生成されたことを地図作成スレッドに知らせるために、地図作成スレッドのキーフレームキュー（キュー（Ｑｕｅｕｅ）は、先入れ先出しのデータ構造になっている）に、キーフレームカウンタＮＫＦをセットする（ステップＳ２１８）。
【００５８】
以上で自機位置推定スレッドの初期化処理が完了したので、位置推定部１２は、初期化済フラグに１をセットする（ステップＳ２１９）。
【００５９】
そして、位置推定部１２は、ＳＬＡＭ座標と実環境座標とのスケール対応を得るために、オドメトリによる並進距離（実環境での座標で求められる）を、上記の処理で推定したＳＬＡＭ座標での並進距離ｓｄで除することによって、スケールＳｃを求める（ステップＳ２２０）。なお、実際には、スケールＳｃは、撮像範囲、撮像画素数等によって定まる値なので、撮像部４１を決めておけば定数となる。そこで、予め実環境での並進距離とＳＬＡＭ座標での並進距離との比を実験的に求め、それをスケールＳｃとして予め設定しておいてもよい。スケールＳｃを予め設定しておくことにより、駆動部４２がオドメトリの機能を有していなくても、後述するようにビジュアルオドメトリをオドメトリ代わりに利用できるようになる。
【００６０】
そして、制御部１０は、後述する誤差判定スレッドで用いられる変数ｐａ，ｐｂ，ｐｃ，ｐｄ（これらは天井推定値記憶部２３に記憶されている）をクリアする（ステップＳ２２１）。そして、ステップＳ２０２及びステップＳ２０３を経由して、初期化済の場合の処理である図６に示すステップＳ２３１へ進む。
【００６１】
では、図６を参照して、初期化済の場合の処理を説明する。この処理が、自機位置推定スレッドの通常時の処理であり、位置推定部１２が、逐次、現在の自機の位置及び向き（ＳＬＡＭ座標内での並進ベクトルｔと回転行列Ｒ）を推定する処理である。
【００６２】
位置推定部１２は、床面センサ３２により自律移動装置１００が床面に接していることが検知されていれば撮像部４１で画像を取得する（ステップＳ２３１）。ステップＳ２３１は、撮影ステップとも呼ばれる。床面センサ３２により床面に接していない（持ち上げられている）ことが検知されている間は、床面に接したことが検知されるまで位置推定部１２はステップＳ２３１以降の処理を行わずに待ち続ける。床面に接している場合は、その後、位置推定部１２は、フレームカウンタＮをインクリメントし（ステップＳ２３２）、撮影した画像に含まれている特徴点を取得する（ステップＳ２３３）。ステップＳ２３３において、位置推定部１２は特徴点取得部として機能する。次に、フレームデータベースに登録されている以前のキーフレーム（例えばキーフレーム番号がＮＫＦである画像）の情報から、その画像の情報に含まれている特徴点のうち、３次元位置が既知である（Ｍａｐ点データベースに登録されているＭａｐ点になっている）特徴点を取得し、今撮影した画像との間で対応が取れる特徴点（対応特徴点）を抽出する（ステップＳ２３４）。
【００６３】
そして、位置推定部１２は、対応特徴点の個数が所定の個数（例えば１０。以下「基準対応特徴点数」という。）未満かどうかを判定し（ステップＳ２３５）、基準対応特徴点数未満の場合（ステップＳ２３５；Ｙｅｓ）はＳＬＡＭ法で推定する姿勢の精度が悪くなるので、位置の推定は行わずにステップＳ２３１での画像の取得に戻る。ここで、すぐにステップＳ２３１に戻るのではなく、ステップＳ２３４に戻って、フレームデータベースに登録されているキーフレームの中から対応特徴点の個数が基準対応特徴点数以上のものを検索するようにしても良い。この場合は、フレームデータベースに登録されているキーフレームの中に対応特徴点の個数が基準対応特徴点数以上のものが見つからなかった場合にステップＳ２３１に戻る。
【００６４】
位置推定部１２は、基準対応特徴点数以上の対応特徴点が抽出できたら（ステップＳ２３５；Ｎｏ）、対応特徴点それぞれの３次元位置（Ｘｉ，Ｙｉ，Ｚｉ）をＭａｐ点データベースから取得する（ステップＳ２３６）。今撮影した画像に含まれている対応特徴点のフレーム座標を（ｕｉ，ｖｉ）とし、その対応特徴点の３次元位置を（Ｘｉ，Ｙｉ，Ｚｉ）とする（ｉは１から対応特徴点数までの値を取る）と、各対応特徴点の３次元位置（Ｘｉ，Ｙｉ，Ｚｉ）を以下の式（５）によってフレーム座標系に投影した値（ｕｘｉ，ｖｘｉ）とフレーム座標（ｕｉ，ｖｉ）とは理想的には一致するはずである。
（ｕｘｉ  ｖｘｉ  １）’〜Ａ（Ｒ｜ｔ）（Ｘｉ  Ｙｉ  Ｚｉ  １）’…（５）
【００６５】
実際には（Ｘｉ，Ｙｉ，Ｚｉ）にも（ｕｉ，ｖｉ）にも誤差が含まれているため、（ｕｘｉ，ｖｘｉ）と（ｕｉ，ｖｉ）とが一致することはめったにない。そして、未知数はＲとｔ（３次元空間ではそれぞれ３次元となり、３＋３＝６が未知数の個数である）だけなのに、数式は対応特徴点の個数の２倍存在する（対応特徴点一つに対して、フレーム座標のｕ，ｖそれぞれに対する式が存在するため）ことになるため、過剰条件の連立一次方程式になり、上述したように最小二乗法で求めることになる。具体的には、位置推定部１２は、以下の式（６）のコスト関数Ｅ１を最小化する姿勢（並進ベクトルｔ及び回転行列Ｒ）を求めることになる。これがＳＬＡＭ法で求めたＳＬＡＭ座標での自機の姿勢（並進ベクトルｔ及び回転行列Ｒで表される自機の位置及び向き）となる。このようにして、位置推定部１２は自機の姿勢を推定する（ステップＳ２３７）。
【数１】
2018156538.tif 000003
【００６６】
ＳＬＡＭ座標での現在の自機の姿勢（並進ベクトルｔ及び回転行列Ｒ）が求められたので、位置推定部１２は、これにスケールＳｃを乗算することで、ＶＯ（ビジュアルオドメトリ）を求める（ステップＳ２３８）。ＶＯは実環境での自機の位置及び向きとして利用できる。
【００６７】
次に、位置推定部１２は、フレームＤＢに登録されている直前のキーフレーム（キーフレーム番号がＮＫＦである画像）を撮影した時の自機の位置から所定の距離（例えば１ｍ。以下「基準並進距離」という。）以上移動しているかを判定する（ステップＳ２３９）。なお、基準並進距離と比較する自機の移動距離は、直前のキーフレームから現フレームまでの並進距離（両フレームの並進ベクトルの差のベクトルの絶対値（要素の二乗和の平方根））をオドメトリから取得しても良いし、上述したＶＯ（ビジュアルオドメトリ）から求めても良い。フレームＤＢに登録する内容は上述したように、「キーフレーム番号」、「姿勢」、「抽出した全ての特徴点」、「全ての特徴点の中でＭａｐ点として３次元位置が既知の点」、「キーフレーム自体の特徴」、である。
【００６８】
基準並進距離未満しか移動していないなら（ステップＳ２３９；Ｎｏ）、図５のステップＳ２０２に戻る。基準並進距離以上移動しているなら（ステップＳ２３９；Ｙｅｓ）キーフレームカウンタＮＫＦをインクリメントしてから（ステップＳ２４０）、現フレームをキーフレームとしてフレームＤＢに登録する（ステップＳ２４１）。
【００６９】
そして、位置推定部１２は、新たなキーフレームが発生したことを地図作成スレッドに知らせるために、地図作成スレッドのキーフレームキューにキーフレームカウンタＮＫＦをセットする（ステップＳ２４２）。そして、図５のステップＳ２０２に戻る。なお、キーフレームカウンタＮＫＦ、スケールＳｃ、Ｍａｐ点ＤＢ、フレームＤＢはスレッドをまたいで値を参照することができるように記憶部２０に記憶されている。
【００７０】
次に、自律移動制御処理の全体フローチャート（図４）のステップＳ１０２で起動される地図作成スレッドについて、図７を参照して説明する。このスレッドは地図作成部１１が、キーフレーム中の対応特徴点の３次元位置を計算して、地図の情報（Ｍａｐ点ＤＢ）を作成している。
【００７１】
まず、地図作成部１１は、自律移動制御処理による動作を終了するか否かを判定する（ステップＳ３０１）。動作終了なら（ステップＳ３０１；Ｙｅｓ）終了し、動作終了でないなら（ステップＳ３０１；Ｎｏ）、キーフレームキューが空かどうかを判定する（ステップＳ３０２）。キーフレームキューが空だったら（ステップＳ３０２；Ｙｅｓ）ステップＳ３０１に戻り、空でなければ（ステップＳ３０２；Ｎｏ）、キーフレームキューからデータを取り出してＭＫＦ（地図作成スレッドで処理するキーフレームのキーフレーム番号を表す変数）にセットする（ステップＳ３０３）。地図作成部１１は、ＭＫＦが０より大きいか判定し（ステップＳ３０４）、ＭＫＦが０である場合は（ステップＳ３０４；Ｎｏ）ステップＳ３０１に戻ってキーフレームキューにデータが入るのを待つ。ＭＫＦが１以上の場合は（ステップＳ３０４；Ｙｅｓ）、以下の処理に進む。
【００７２】
地図作成部１１は、フレームＤＢを参照し、前キーフレーム（キーフレーム番号がＭＫＦ−１のキーフレーム）の特徴点と現キーフレーム（キーフレーム番号がＭＫＦのキーフレーム）の特徴点とで対応が取れる特徴点（対応特徴点）を抽出する（ステップＳ３０５）。フレームＤＢにはそれぞれのキーフレームの姿勢（並進ベクトルｔと回転行列Ｒ）も登録されているので、自機位置推定スレッドの初期化時の処理の時と同様の方法で対応特徴点の３次元位置を計算できる。地図作成部１１は、３次元位置が計算できた対応特徴点の３次元位置をＭａｐ点としてＭａｐ点ＤＢに登録する（ステップＳ３０６）。ステップＳ３０６は、地図作成ステップとも呼ばれる。地図作成部１１は、フレームＤＢに対しても今回３次元位置を計算できた特徴点に対して３次元位置を登録する（ステップＳ３０７）。
【００７３】
なお、地図作成部１１が抽出した対応特徴点がすでにＭａｐ点ＤＢに登録済だった場合は、３次元位置計算をスキップして次の対応特徴点（Ｍａｐ点ＤＢに未登録のもの）に対する処理に進んでも良いし、改めて３次元位置計算を行って、Ｍａｐ点ＤＢに登録済の３次元位置や、フレームＤＢ中の対応特徴点に対する３次元位置を更新するようにしても良い。
【００７４】
次に、地図作成部１１は、キーフレームキューが空かどうかを判定する（ステップＳ３０８）。空であれば（ステップＳ３０８；Ｙｅｓ）、全キーフレームの姿勢と全Ｍａｐ点の３次元位置に対して、バンドルアジャストメント処理を行って、精度向上を図ってから（ステップＳ３０９）ステップＳ３１０に進む。キーフレームキューが空でなければ（ステップＳ３０８；Ｎｏ）ステップＳ３１０に進む。次に地図作成部１１は、ループクロージングスレッドのキーフレームキューにＭＫＦをセットして（ステップＳ３１０）、ステップＳ３０１に戻る。
【００７５】
なお、バンドルアジャストメント処理とは、カメラ姿勢（キーフレーム姿勢）とＭａｐ点の３次元位置とを同時に推定する非線形最適化法であり、Ｍａｐ点をキーフレーム上に投影させたときに発生する誤差が最小になるような最適化を行うものである。
【００７６】
このバンドルアジャストメントの処理を行うことで、キーフレーム姿勢とＭａｐ点の３次元位置の精度向上を図ることができる。しかし、この処理を行わなくても精度向上が図れないだけで特別問題が発生するわけではない。したがって、他の処理がない状態のとき（例えば、キーフレームキューが空の状態）であっても、この処理を毎回行う必要はない。
【００７７】
また、バンドルアジャストメントの処理を行うと、キーフレーム上に投影させたときの誤差が所定の値よりも大きいＭａｐ点が見つかることがある。このような誤差の大きなＭａｐ点については、ＳＬＡＭ推定に悪影響を及ぼすため、Ｍａｐ点ＤＢ及びフレームＤＢから削除したり、誤差の大きな注意を要するＭａｐ点であることを識別するためのフラグを立てておいたりしても良い。なお、バンドルアジャストメントの処理は本実施形態においてはオプション的な扱いになるため、ここでは処理の詳細については省略する。
【００７８】
次に、自律移動制御処理の全体フローチャート（図４）のステップＳ１０３で起動されるループクロージングスレッドについて、図８を参照して説明する。このスレッドでは制御部１０は、ループクロージング処理ができるかどうかをチェックし続け、できる場合にはループクロージング処理を行っている。なお、ループクロージング処理とは、以前来たことのある同じ場所に戻ってきたことを認識した場合に、以前この同じ場所にいた時の姿勢の値と現在の姿勢の値とのずれを用いて、以前来た時から今までの軌跡中のキーフレームや、関連するＭａｐ点の３次元位置を修正することを言う。
【００７９】
まず、制御部１０は、自律移動制御処理による動作を終了するか否かを判定する（ステップＳ４０１）。動作終了なら（ステップＳ４０１；Ｙｅｓ）終了する。動作終了でないなら（ステップＳ４０１；Ｎｏ）キーフレームキューが空かどうかを判定する（ステップＳ４０２）。キーフレームキューが空なら（ステップＳ４０２；Ｙｅｓ）ステップＳ４０１に戻り、キーフレームキューが空でないなら（ステップＳ４０２；Ｎｏ）、キーフレームキューからデータを取り出してＬＫＦ（ループクロージングスレッドで処理するキーフレームのキーフレーム番号を表す変数）にセットする（ステップＳ４０３）。次に制御部１０は、ＬＫＦが１より大きいかを判定する（ステップＳ４０４）。ＬＫＦが０又は１である場合（ステップＳ４０４；Ｎｏ）はステップＳ４０１に戻ってキーフレームキューにデータが入るのを待つ。そして、ＬＫＦが２以上の場合（ステップＳ４０４；Ｙｅｓ）は、以下の処理を行う。
【００８０】
制御部１０は、フレームＤＢを参照し、現キーフレーム（キーフレーム番号がＬＫＦのキーフレーム）と「キーフレーム自体の特徴」の類似度が所定の類似度（例えば０．９。以下「基準画像類似度」という。）以上になるキーフレームをフレームＤＢから検索する（ステップＳ４０５）。ここで、この類似度は、画像（キーフレーム）の特徴を特徴ベクトルで表している場合は、二つの画像の特徴ベクトルの内積の絶対値を１に正規化したものや、二つの画像の特徴ベクトルの絶対値（要素の二乗和の平方根）を１に正規化したもの同士の内積を、その二つの画像の類似度とすることができる。また、二つの画像の特徴ベクトル（絶対値を１に正規化したもの）の距離（各要素の差の二乗和の平方根）の逆数を類似度としても良い。
【００８１】
制御部１０は、「キーフレーム自体の特徴」の類似度が基準画像類似度以上になるキーフレームが発見されたかどうかを判定し（ステップＳ４０６）、発見されなければ（ステップＳ４０６；Ｎｏ）ステップＳ４０８へ行き、発見されたら（ステップＳ４０６；Ｙｅｓ）、発見されたキーフレームから現キーフレームまでの軌跡中のキーフレームの姿勢と、軌跡中のキーフレームに含まれるＭａｐ点の３次元位置を修正する（ステップＳ４０７）。例えば、制御部１０は、現キーフレームの姿勢を発見されたキーフレームの姿勢と同じ姿勢として修正する。そして、発見されたキーフレームの姿勢と現キーフレームの姿勢との差分を用いて、発見されたキーフレームから現キーフレームまでの軌跡中の各キーフレームの姿勢に線形的に補正を加える。さらにこれらの各キーフレームに含まれるＭａｐ点の３次元位置についても各キーフレームの姿勢の補正量に応じて修正する。
【００８２】
次に制御部１０は、誤差判定スレッドのキーフレームキューにＬＫＦをセットしてステップＳ４０１に戻る（ステップＳ４０８）。
【００８３】
次に、自律移動制御処理の全体フローチャート（図４）のステップＳ１０４で起動される誤差判定スレッドについて説明する。この誤差判定スレッドは、撮像部４１が取得する画像には天井の一部が含まれていることが多いということを前提として、撮像部４１が取得した画像中に天井付近に存在する特徴点が含まれていない状態が続くようであれば、判定部１４が地図の情報に含まれている誤差が大きい（地図の情報に誤差が基準以上多く含まれている）と判定する。そして、判定部１４が地図の情報に含まれている誤差が大きい（地図の情報に誤差が基準以上多く含まれている）と判定したら、地図編集部１５が地図の情報を消去する処理を行う。具体的には、天井推定部１３が３次元ハフ変換により天井平面を推定し、判定部１４が該天井平面との距離が基準天井距離ｎｃｄ（例えば１０ｃｍ）以下のＭａｐ点の個数Ｋが基準個数（例えば２個）未満なら、地図の情報に含まれている誤差が大きい（地図の情報に誤差が基準以上多く含まれている）と判定する。
【００８４】
通常部屋の中は、床にはテーブル、椅子等の様々な物が置いてあることが多いが、天井には照明以外は特に何も存在しないことが多い。したがって、撮像部４１が取得する画像に含まれる特徴点のうち、天井の平面上に存在する特徴点は安定的に取得できることが想定できる。誤差判定スレッドはこの想定を前提として、地図の情報に含まれている誤差が大きいか否か（地図の情報に誤差が基準以上多く含まれているか否か）を判定している。
【００８５】
では、図９を参照して、誤差判定スレッドの処理を説明する。まず、制御部１０が、自律移動制御処理による動作を終了するか否かどうかを判定し（ステップＳ５０１）、動作終了なら（ステップＳ５０１；Ｙｅｓ）終了し、動作終了でないなら（ステップＳ５０１；Ｎｏ）、誤差判定スレッドのキーフレームキューが空か否かを判定する（ステップＳ５０２）。キーフレームキューが空なら（ステップＳ５０２；Ｙｅｓ）ステップＳ５０１に戻って、キーフレームキューにデータが入るのを待つ。
【００８６】
キーフレームキューが空でなければ（ステップＳ５０２；Ｎｏ）、制御部１０はキーフレームキューからデータを取り出してＩＫＦ（誤差判定スレッドで処理するキーフレームのキーフレーム番号を表す変数）にセットする（ステップＳ５０３）。次に、制御部１０は、天井推定値記憶部２３に記憶されている変数ｐａ，ｐｂ，ｐｃ，ｐｄが全て０であるか否かを判定する（ステップＳ５０４）。変数ｐａ，ｐｂ，ｐｃ，ｐｄは、後述するように、天井推定部１３が推定した天井平面を表す方程式の係数の値がセットされるが、上述したように、図５のステップＳ２２１で初期化され、全て０にクリアされている。したがって、変数ｐａ，ｐｂ，ｐｃ，ｐｄが全て０か否かにより、天井推定部１３による天井平面の推定がまだ行われていないか否かを判定できる。
【００８７】
変数ｐａ，ｐｂ，ｐｃ，ｐｄが全て０であるなら（ステップＳ５０４；Ｙｅｓ）、天井推定部１３は、現キーフレーム（キーフレーム番号がＩＫＦのキーフレーム）の天井方向に存在するＭａｐ点を用いて、３次元ハフ変換により、天井平面を推定する（ステップＳ５０５）。自律移動装置１００は、図５に示す自機位置推定スレッドの初期化処理で床面を水平に移動してＳＬＡＭ座標の初期化をしているので、ＳＬＡＭ座標に基づき天井方向に存在するＭａｐ点をＭａｐ点ＤＢから抽出することができる。簡単には、画像中の上半分の領域に含まれるＭａｐ点を「天井方向に存在するＭａｐ点」としてもよい。
【００８８】
ステップＳ５０５における天井平面の推定は、天井方向に存在するＭａｐ点群をＭａｐ点ＤＢから取得し、通常の（２次元の）ハフ変換の拡張である３次元ハフ変換を用いることによって行う。そこで、３次元ハフ変換について説明する。実空間上の点（Ｘｐ，Ｙｐ，Ｚｐ）を含む平面Ｓは、図１０Ａに示すように、原点からの距離ρｓと２つの角度（方位角θｓ及び仰角φｓ）で表すことができる。この時、これらの関係は以下の式で表すことができる。
Ｘｐ＝ρｓ・ｃｏｓφｓ・ｃｏｓθｓ  …（７）
Ｙｐ＝ρｓ・ｃｏｓφｓ・ｓｉｎθｓ  …（８）
Ｚｐ＝ρｓ・ｓｉｎφｓ              …（９）
【数２】
2018156538.tif 000004
【数３】
2018156538.tif 000005
【数４】
2018156538.tif 000006
【００８９】
すると、ＸＹＺ座標系の実空間における平面Ｓ上の任意の点ｐ（Ｘｐ，Ｙｐ，Ｚｐ）は、図１０Ｂに示すようにρφθ座標系のハフ空間における１つの点ｓ（ρｓ，φｓ，θｓ）として表される。つまり、実空間における平面Ｓ上の任意の点ｐ（Ｘｐ，Ｙｐ，Ｚｐ）は、上記３次元ハフ変換を行うことによって、ハフ空間上の１つの点ｓ（ρｓ，φｓ，θｓ）に変換される。
【００９０】
したがって、各Ｍａｐ点の（Ｘ，Ｙ，Ｚ）座標を３次元ハフ変換により（ρ，φ，θ）座標に変換すると、実空間上の平面に対応するハフ空間上の点に、より多くのＭａｐ点が変換されることになる。そこで、ρφθ空間を格子状の領域（グリッド）に分割し、各Ｍａｐ点の（Ｘ，Ｙ，Ｚ）座標を３次元ハフ変換により（ρ，φ，θ）座標に変換した場合に、最も多くのＭａｐ点が変換されたグリッドに着目し、このグリッドを最多変換グリッドと呼ぶことにする。するとその最多変換グリッドに対応するハフ空間上の座標（例えば最多変換グリッドの中心点のρφθ座標）に対応する実空間上の平面を天井平面と推定することができる。ステップＳ５０５では、天井推定部１３は、この最多変換グリッドに変換されたＭａｐ点がいくつあるかを求める。このＭａｐ点の個数は、その最多変換グリッドに変換された平面をサポートする点の個数ということになる。
【００９１】
そして、天井推定部１３は、最多変換グリッドに変換されたＭａｐ点の個数（平面をサポートする点の個数）が基準変換個数（例えば１０個）以下か否かを判定する（ステップＳ５０６）。もし、最多変換グリッドに変換されたＭａｐ点が基準変換個数以下なら（ステップＳ５０６；Ｎｏ）、天井平面の推定を行うための情報が少ないため、推定失敗として、ステップＳ５０１に戻る。
【００９２】
最多変換グリッドに変換されたＭａｐ点が基準変換個数（例えば１０個）よりも多ければ（ステップＳ５０６；Ｙｅｓ）、天井推定部１３は、平面方程式の係数を推定し、推定した係数を変数ｃａ，ｃｂ，ｃｃ，ｃｄにセットする（ステップＳ５０７）。ステップＳ５０７は、天井推定ステップとも呼ばれる。この平面方程式の係数の推定について説明する。まず、実空間上の平面は、以下の平面方程式で表すことができる。
Ａ×ｘ＋Ｂ×ｙ＋Ｃ×ｚ＋Ｄ＝０  …（１３）
式（１３）で、Ａ，Ｂ，Ｃ，Ｄが平面方程式の係数である。そして、上記最多変換グリッドに変換された各Ｍａｐ点のＸＹＺ座標を式（１３）に代入すると、Ａ，Ｂ，Ｃ，Ｄを未知数とする方程式になる。未知数はＡ，Ｂ，Ｃ，Ｄの４個であるが、式（１３）に代入するＭａｐ点の個数は１０個よりも多いため、平面方程式の係数Ａ，Ｂ，Ｃ，Ｄは、過剰条件の連立一次方程式として、最小二乗法で解くことができる。
【００９３】
そして、求められたＡ，Ｂ，Ｃ，Ｄを、それぞれ変数ｃａ，ｃｂ，ｃｃ，ｃｄにセットする。これらの変数は、平面方程式の係数を一時的に保存する変数である。この時、推定した天井平面の法線ベクトルは（ｃａ，ｃｂ，ｃｃ）で表されるが、この法線ベクトルの傾きは、理想的には床面（ＸＹ平面）と垂直のはずである（なお、床面とＸＹ平面が異なる場合には、座標を変換して床面がＸＹ平面になるように座標を事前に変換しておく。）。つまり、ｃｃの値は、ｃａに比べて十分大きく、かつ、ｃｂと比べても十分大きい値になっているはずである（例えば、ｃｃ＞１０×ｃａかつｃｃ＞１０×ｃｂ）。そこで、天井推定部１３は、法線ベクトル（ｃａ，ｃｂ，ｃｃ）の傾きが異常な値（床面と垂直に近い値になっていない）であるか否かを判定する（ステップＳ５０８）。法線ベクトルの傾きが異常なら（ステップＳ５０８；Ｙｅｓ）、ステップＳ５０１に戻る。
【００９４】
法線ベクトルの傾きが異常でないなら（ステップＳ５０８；Ｎｏ）、変数ｃａ，ｃｂ，ｃｃ，ｃｄをそれぞれ、変数ｐａ，ｐｂ，ｐｃ，ｐｄにセットする（ステップＳ５０９）。そして、誤差判定用のカウンタ変数ＥＣＴをクリアし（ステップＳ５１０）、ステップＳ５０１に戻る。
【００９５】
一方、上記ステップＳ５０４で、変数ｐａ，ｐｂ，ｐｃ，ｐｄの少なくともいずれかが０でないなら（ステップＳ５０４；Ｎｏ）、天井平面の推定は既に行われているということなので、天井推定部１３は、現キーフレーム（キーフレーム番号がＩＫＦのキーフレーム）の天井方向に存在するＭａｐ点のうち、係数ｐａ，ｐｂ，ｐｃ，ｐｄで表される平面方程式との距離が基準天井距離ｎｃｄ（例えば１０ｃｍ）以下のＭａｐ点の個数を変数Ｋにセットする（ステップＳ５１１）。
【００９６】
そして、判定部１４は、変数Ｋの値が基準個数（例えば２個）未満か否かを判定する（ステップＳ５１２）。ステップＳ５１２は、判定ステップとも呼ばれる。変数Ｋの値が基準個数以上なら（ステップＳ５１２；Ｎｏ）、ステップＳ５１０へ進む。変数Ｋの値が基準個数未満なら（ステップＳ５１２；Ｙｅｓ）、誤差判定用のカウンタ変数ＥＣＴをインクリメントする（ステップＳ５１３）。
【００９７】
そして、判定部１４は、カウンタ変数ＥＣＴが基準カウンタ値（例えば５）以下か否かを判定する（ステップＳ５１４）。カウンタ変数ＥＣＴが基準カウンタ値以下なら（ステップＳ５１４；Ｎｏ）、ステップＳ５０１に戻る。
【００９８】
カウンタ変数ＥＣＴが基準カウンタ値よりも大きければ（ステップＳ５１４；Ｙｅｓ）、作成した地図の情報（Ｍａｐ点ＤＢの情報や、フレームＤＢの情報）に誤差が基準以上多く含まれているということなので、地図編集部１５は初期化済フラグ、Ｍａｐ点ＤＢ、フレームＤＢ、各スレッドのキーフレームキューを全てクリアする（ステップＳ５１５）。ステップＳ５１５において、地図編集部１５は、判定部１４が判定した結果に応じて所定の処理を行う判定結果処理部として機能する。また、ステップＳ５１５は、判定結果処理ステップとも呼ばれる。そして制御部１０は、ステップＳ５０１に戻って最初からやり直す。
【００９９】
以上説明した誤差判定スレッドの処理により、地図の情報に誤差が基準以上多く含まれる場合には、初期化済フラグ、Ｍａｐ点ＤＢ、フレームＤＢ、各スレッドのキーフレームキューが全てクリアされるので、全てのスレッドが最初からやり直されることになる。このため、作成した地図に誤差が基準以上多く含まれた状態で自律移動装置１００が動き続けてしまうことを防ぐことができる。
【０１００】
煩雑になるため図９には示していないが、ステップＳ５０８で天井推定部１３が推定した天井平面の放線ベクトルの傾きが異常であると判定した場合に、すぐにステップＳ５０１に戻るのではなく、傾き異常カウンタ変数ＬＡＣをインクリメントするようにし、このカウンタ変数の値が規定回数（例えば１０回）以上になったら、やはり地図の情報に誤差が基準以上多く含まれているとして、ステップＳ５１５の処理に進むようにしても良い。
【０１０１】
なお、ステップＳ５１２で変数Ｋの値が基準個数以上（ステップＳ５１２；Ｎｏ）なら、その時点における地図の情報にはまだそれほど誤差が含まれていなかった可能性もある。そこで、ステップＳ５１２の判定がＮｏだった場合は、地図編集部１５がその時点の地図の情報（Ｍａｐ点ＤＢ）を「確からしい地図」として地図保存部（図示せず）に保存してからステップＳ５１０に進み、ステップＳ５１５では地図編集部１５は初期化済フラグ、フレームＤＢ、各スレッドのキーフレームキューをクリアすることなく、Ｍａｐ点ＤＢを保存してある以前のＭａｐ点ＤＢの状態に戻すようにしても良い。このようにすることによって、過去に作成した地図の情報を有効活用できる。
【０１０２】
Ｍａｐ点ＤＢを以前の状態に戻す方法としては、Ｍａｐ点ＤＢ全体の情報を「確からしい地図」として保存しておく方法以外に、Ｍａｐ点ＤＢの各要素にタイムスタンプ（時刻情報を使っても良いが、キーフレームカウンタＮＫＦの値を使うのが合理的である）の情報も一緒に格納しておく方法もある。この方法では、ステップＳ５１２での判定がＮｏだった場合（地図の情報に誤差が基準以上多く含まれていない場合）は、ステップＳ５１０に進む前に、地図編集部１５がその時点のタイムスタンプ（時刻又はＮＫＦ）を時刻記憶部である変数ＴＳにセットしておき（このＴＳ自体もスタック状にして、次々に過去に戻れるようにしておいても良い）、ステップＳ５１５では地図編集部１５は、Ｍａｐ点ＤＢ内の情報のうち、タイムスタンプがＴＳより後になっているものを消去し、ＴＳ以前の情報はそのまま残す処理を行う。このような処理を行えば、過去の確からしいＭａｐ点の情報は保存したまま、最近の（誤差が大きい（誤差を多く含む）と考えられる）Ｍａｐ点の情報のみを消去できる。
【０１０３】
この考え方はフレームＤＢにも適用でき、地図編集部１５は、ステップＳ５１５で、フレームＤＢ内の情報のうち、キーフレーム番号（そのフレームが撮影された時刻を表す）が変数ＴＳ（地図の情報に誤差が基準以上多く含まれていない時点の時刻を表す）よりも大きいものを消去し、ＴＳ以前の情報はそのまま残すようにしても良い。このようにすることで、過去の確からしいキーフレームの情報は保存したまま、最近の（誤差が大きい（誤差を多く含む）と考えられる）キーフレームの情報のみを消去できる。
【０１０４】
（実施形態２）
実施形態１では、現キーフレーム（キーフレーム番号がＩＫＦのキーフレーム）の天井平面に特徴点が複数存在することを前提に誤差判定スレッドの処理を行っている。自律移動装置１００が部屋の中を移動する際、図１１に示すように、前方の壁までの距離が近くなるにつれて、撮像部４１が取得する画像に含まれる天井部分の面積が小さくなる。したがって、前方の壁までの距離が前方距離基準値（例えば１ｍ）を下回った状態では、撮像部４１が取得する画像には十分な面積の天井部分が含まれておらず、判定部１４は十分な精度で誤差判定をすることはできないと考えられる。そこで、前方の壁までの距離が前方距離基準値未満の場合には、誤差判定をしないようにした実施形態２について説明する。実施形態２は実施形態１と比較すると誤差判定スレッドの一部が異なるだけで、他は実施形態１同じである。そこで、誤差判定スレッドの変更部分について、図１２を参照して説明する。
【０１０５】
実施形態２に係る誤差判定スレッドにおける変更部分は、図９のステップＳ５０３とステップＳ５０４の間にステップＳ５２１の処理を追加しただけであり、それ以外は実施形態１と同じである。そこで、追加した処理について説明する。
【０１０６】
ステップＳ５２１では、判定部１４は、現キーフレーム（キーフレーム番号がＩＫＦのキーフレーム）を取得した時の自機位置と前方の壁との間の距離が前方距離基準値（例えば１ｍ）未満か否かを判定する。判定部１４は、該自機位置をフレームＤＢから取得できるので、前方の壁の位置を障害物センサ３１によって取得できていれば、この距離を算出できる。まだ前方の壁の位置を取得できていない場合は、判定部１４は、この距離を前方距離基準値以上とみなす。そして、この距離が前方距離基準値未満なら（ステップＳ５２１；Ｙｅｓ）、ステップＳ５０１に戻り、前方距離基準値以上なら（ステップＳ５２１；Ｎｏ）、ステップＳ５０４に進む。
【０１０７】
上記ステップＳ５２１が追加されることにより、実施形態２に係る自律移動装置１００は、キーフレームに十分な面積の天井部分が含まれていると想定される時のみ、天井平面の推定及び誤差判定を行うようになる。したがって、キーフレームに十分な面積の天井部分が含まれないことに起因して誤差判定に失敗する可能性を小さくすることができる。
【０１０８】
（実施形態３）
なお、上記の実施形態１及び実施形態２では、ステップＳ５１１では、天井平面との距離が基準天井距離ｎｃｄ以下のＭａｐ点の個数を変数Ｋにセットし、ステップＳ５１２で変数Ｋが基準個数未満か否かを判定することによって、判定部１４は地図情報の誤差が大きいか否かを判定している。しかし、天井推定部１３がキーフレーム番号ＩＫＦのキーフレーム中のＭａｐ点により天井平面（第１の天井平面）を推定し、この天井平面とステップＳ５０５で推定した天井平面（第２の天井平面）との距離が基準天井距離以下か否かを判定することによって地図情報の誤差が大きいか否かを判定部１４が判定してもよい。このような判定を行う実施形態３について説明する。実施形態３は実施形態１と比較すると誤差判定スレッドの一部が異なるだけで、他は実施形態１同じである。そこで、誤差判定スレッドの変更部分について、図１３を参照して説明する。
【０１０９】
実施形態３に係る誤差判定スレッドにおける変更部分は、図９のステップＳ５１１からステップＳ５１２までの処理を、図１３のステップＳ５３１からステップＳ５３４までの処理に置き換えただけであり、それ以外は実施形態１と同じである。そこで、置き換えた部分について説明する。
【０１１０】
ステップＳ５３１では、ステップＳ５０５と同様に、天井推定部１３は、現キーフレーム（キーフレーム番号がＩＫＦのキーフレーム）の天井方向に存在するＭａｐ点を用いて、３次元ハフ変換により、天井平面を推定する。具体的には、上述した最多変換グリッドに変換されたＭａｐ点がいくつあるかを求める。そして、ステップＳ５３２では、ステップＳ５０６と同様に、天井推定部１３は、最多変換グリッドに変換されたＭａｐ点の個数（平面をサポートする点の個数）が基準変換個数（例えば１０個）以下か否かを判定する。もし、最多変換グリッドに変換されたＭａｐ点が基準変換個数以下なら（ステップＳ５３２；Ｎｏ）、天井平面の推定を行うための情報が少ないため、誤差判定不可能として、ステップＳ５０１に戻る。
【０１１１】
最多変換グリッドに変換されたＭａｐ点が基準変換個数（例えば１０個）よりも多ければ（ステップＳ５３２；Ｙｅｓ）、ステップＳ５０７と同様に、天井推定部１３は、平面方程式の係数を推定する（ステップＳ５３３）。ステップＳ５３３は、天井推定ステップとも呼ばれる。そして、判定部１４は、ステップＳ５３３で推定した係数で表される平面と、変数ｐａ，ｐｂ，ｐｃ，ｐｄで表される平面との距離が基準天井距離ｎｃｄ以下か否かを判定する（ステップＳ５３４）。ステップＳ５３４は、判定ステップとも呼ばれる。
【０１１２】
この平面同士の距離の判定においては、判定部１４は、まず法線ベクトル同士を比較し、２つの法線ベクトルの角度の差が基準角度（例えば１０度）未満であるか否かを判定する。基準角度以上の差があるなら、判定部１４は、地図情報の誤差が基準以上多く含まれていると判定し、天井平面同士の距離が基準天井距離ｎｃｄより大きいとみなして（ステップＳ５３４；Ｎｏ）、ステップＳ５１３に進む。
【０１１３】
２つの法線ベクトルの角度の差が基準角度未満であるなら、それぞれの平面の原点からの距離の差を２つの平面の距離として、基準天井距離ｎｃｄと比較する。そして、各平面の原点からの距離の差が基準天井距離ｎｃｄ以下なら（ステップＳ５３４；Ｙｅｓ）、ステップＳ５１０に進み、基準天井距離ｎｃｄよりも大きければ（ステップＳ５３４；Ｎｏ）、ステップＳ５１３に進む。
【０１１４】
上記実施形態３に係る誤差判定スレッドでは、キーフレーム中に含まれるＭａｐ点数が多ければ、実施形態１に係る誤差判定スレッドよりも精度の高い誤差判定を行うことができる。
【０１１５】
上記、各実施形態は任意に組み合わせることが可能である。例えば、実施形態２に係る誤差判定スレッドの処理（ステップＳ５２１）と、実施形態３に係る誤差判定スレッドの処理（ステップＳ５３１からステップＳ５３４）とを両方とも行うことが可能である。また、実施形態１に係る誤差判定スレッドの処理と、実施形態３に係る誤差判定スレッドの処理とを組み合わせてもよい。例えば、キーフレーム中に含まれるＭａｐ点の個数が所定の個数未満の場合は、実施形態１に係る誤差判定スレッドの処理を行い、所定の個数以上の場合は、実施形態３に係る誤差判定スレッドの処理を行うようにしてもよい。また、例えば、ステップＳ５１１からステップＳ５１２までの処理による判定と、ステップＳ５３１からステップＳ５３４までの処理による判定とを両方とも行って、両方の判定が「地図情報に誤差が多い」だった場合に、カウンタ変数ＥＣＴをインクリメントするようにしても良い。
【０１１６】
なお、上記実施形態では、天井平面の推定を３次元ハフ変換によって行う例を示したが、他の方法（例えばＲＡＮＳＡＣ（ＲＡＮｄｏｍ  ＳＡｍｐｌｅ  Ｃｏｎｓｅｎｓｕｓ）アルゴリズム等）を用いて天井平面の推定を行ってもよい。
また、上記実施形態では、天井推定部１３が推定した天井平面を用いて、地図記憶部２２に記憶されている地図の情報に誤差が基準以上多く含まれているか否かを判定部１４が判定した結果を用いて、地図編集部１５が地図記憶部２２に記憶されている地図の情報の編集を行った。しかし、これに限ることなく、判定部１４が地図記憶部２２に記憶されている地図の情報に誤差が基準以上多く含まれていることを判定したら、制御部１０（判定結果処理部）は、地図に誤差が多いことをユーザに知らせるため何らかの報知を行うようにしてもよいし、移動動作を一時的に停止するようにしてもよい。
【０１１７】
また、上記実施形態では、撮像部４１が単眼である単眼ＳＬＡＭの例を示したが、複数の撮像部を用いた複眼ＳＬＡＭでも同様の構成を採ることができる。例えば自律移動装置１００が二台の撮像部４１を備えている場合は、移動しなくても同じ位置から二つの画像を取得できるため、自機位置推定スレッドの初期化処理においてはステップＳ２０５の一回の動作で二つの画像が取得できる。それら二つの画像それぞれに基準値以上の特徴点が含まれていれば（ステップＳ２０７で判定する）、ステップＳ２０８では「Ｎ＝Ｎ＋２」を行い、ステップＳ２０９は常に「Ｎｏ」に進むことになる。そして、二つの画像の一方を前フレーム、もう一方を現フレームとして処理を行う場合は、ステップＳ２２０でスケールＳｃを計算する際の「オドメトリ並進距離」は、二つの撮像部間の距離を用いれば良い。これによりオドメトリからの取得する位置の精度が低い場合でも安定して初期化を行うことができる。
【０１１８】
ただし、二つの撮像部間の距離が、特徴点までの距離に対して小さい場合には充分な精度がとれない。（例えば、二つの撮像部間の距離が３０ｍｍで、５ｍ先の特徴点までの距離を測定するときなど。）つまり、オドメトリから得られる並進距離の精度が高い場合は、むしろ単眼ＳＬＡＭと同様の初期化処理（例えば複数ある撮像部のうちの第一撮像部のみを使った初期化処理）を行った方が高い精度が得られることもある。そこで、単眼ＳＬＡＭと同様の初期化処理と、前段落で記載した二つの撮像部による初期化処理とを両方とも行って、それぞれで地図の情報（Ｍａｐ点ＤＢやフレームＤＢ）を作成し始め、途中で誤差判定スレッドと同様の処理を行ってそれぞれの誤差を判定し、その後は誤差の小さい方の地図の情報を採用するようにしても良い。
【０１１９】
なお、上述の各実施形態では、基準並進距離の値を「例えば１ｍ」と設定したが、自律移動装置自体の大きさや速度、自律移動する環境や移動範囲等に応じて、最適な基準並進距離は変わるので、基準並進距離の設定方法について補足しておく。
【０１２０】
基準並進距離の設定方法として、キーフレーム上の全てのＭａｐ点（３次元位置が既知の特徴点）の３次元位置から自機位置までの距離を平均した値（＝キーフレームで観察される全てのＭａｐ点までの奥行きの平均距離）と並進距離との比で設定しても良い（例えば、Ｍａｐ点までの奥行きの平均距離の５％の距離を基準並進距離とする等）。ここで用いる「全てのＭａｐ点」は制御部１０がＭａｐ点ＤＢを参照することで抽出できる。また、実環境上での並進距離の大きさに応じて、例えばテーブルの上を動くなら１０ｃｍ、部屋を動き回るなら１ｍ、屋外の広場等を動き回るなら１０ｍ等を基準並進距離に設定しても良いし、駆動部４２の車輪の直径に応じた値（例えば車輪の直径の１０倍）を基準並進距離に設定しても良い。
【０１２１】
基準並進距離として実環境上の距離で設定する場合は、ＳＬＡＭ座標での並進距離にはスケールＳｃを乗算して実環境上の距離に変換してから比較を行う（この逆も同様であり、基準並進距離としてＳＬＡＭ座標での距離を設定する場合は、実環境上の距離はスケールＳｃで除算してＳＬＡＭ座標での距離に変換してから比較を行う）。
【０１２２】
なお、自律移動装置１００の各機能は、通常のＰＣ（Ｐｅｒｓｏｎａｌ  Ｃｏｍｐｕｔｅｒ）等のコンピュータによっても実施することができる。具体的には、上記実施形態では、自律移動装置１００が行う自律移動制御処理のプログラムが、記憶部２０のＲＯＭに予め記憶されているものとして説明した。しかし、プログラムを、フレキシブルディスク、ＣＤ−ＲＯＭ（Ｃｏｍｐａｃｔ  Ｄｉｓｃ  Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、ＤＶＤ（Ｄｉｇｉｔａｌ  Ｖｅｒｓａｔｉｌｅ  Ｄｉｓｃ）及びＭＯ（Ｍａｇｎｅｔｏ−Ｏｐｔｉｃａｌ  Ｄｉｓｃ）等のコンピュータ読み取り可能な記録媒体に格納して配布し、そのプログラムをコンピュータに読み込んでインストールすることにより、上述の各機能を実現することができるコンピュータを構成してもよい。
【０１２３】
また、このコンピュータは自律移動装置１００に内蔵してもよいし、自律移動装置１００とは別個に存在してもよい。すなわち、図１４に示すように、制御部５０や記憶部６０を外部のサーバー２００に設けてもよい。クラウドコンピューティング技術により、自律移動装置１００の制御部１０は、撮像部４１やセンサ部３０等で取得したデータを通信部４４を介して外部のサーバー２００に送信し、演算処理を該サーバー２００にさせた後に、サーバー２００が行った演算結果を通信部４４を介して受信し、駆動部４２等を制御してもよい。図１４では、サーバー２００の制御部５０が地図作成部１１、位置推定部１２、天井推定部１３、判定部１４及び地図編集部１５を備える構成となっているが、サーバー２００の制御部５０と自律移動装置１００の制御部１０とのどちらかに各部が備わっていればよい。各部をサーバー２００の制御部５０が備えるか自律移動装置１００の制御部１０が備えるかについては、任意である。
また、情報を消去するとは、記憶媒体上から情報を物理的に無くしてもよいし、消去フラグを立てる等して論理的に消去したことにしてもよい。
【０１２４】
以上、本発明の好ましい実施形態について説明したが、本発明は係る特定の実施形態に限定されるものではなく、本発明には、特許請求の範囲に記載された発明とその均等の範囲が含まれる。以下に、本願出願の当初の特許請求の範囲に記載された発明を付記する。
【０１２５】
（付記１）
撮像部と、
前記撮像部が撮影した複数の画像の情報を用いて地図の情報を作成する地図作成部と、
前記撮像部が撮影した複数の画像の情報を用いて天井平面を推定する天井推定部と、
前記天井推定部が推定した天井平面を用いて、前記地図の情報に誤差が基準以上多く含まれているか否かを判定する判定部と、
前記判定部が判定した結果に応じて所定の処理を行う判定結果処理部と、
を備える自律移動装置。
【０１２６】
（付記２）
前記地図作成部が作成した地図の情報を記憶する地図記憶部を備え、
前記判定部が前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていると判定した場合、前記判定結果処理部は前記地図記憶部に記憶されている地図の情報を編集する、
付記１に記載の自律移動装置。
【０１２７】
（付記３）
前記判定結果処理部は、前記判定部が前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていると判定した場合、前記地図記憶部に記憶されている地図の情報を消去する、
付記２に記載の自律移動装置。
【０１２８】
（付記４）
前記判定部が前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていないと判定した場合に前記地図記憶部に記憶されている地図の情報を保存する地図保存部を備え、
前記判定結果処理部は、前記判定部が前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていると判定した場合、前記地図保存部に保存されている地図の情報を前記地図記憶部に記憶させる、
付記２又は３に記載の自律移動装置。
【０１２９】
（付記５）
前記判定部が前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていないと判定した時刻の情報を記憶する時刻記憶部を備え、
前記地図記憶部は、地図を作成した時刻の情報と共に地図の情報を記憶し、
前記判定結果処理部は、前記判定部が前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていると判定した場合、前記地図記憶部に記憶されている地図の情報のうち、前記時刻記憶部に記憶されている時刻よりも後に作成された地図の情報を消去する、
付記２から４のいずれか１つに記載の自律移動装置。
【０１３０】
（付記６）
撮影した画像の情報を、撮影した時刻の情報と共に記憶する画像記憶部を備え、
前記判定結果処理部は、前記判定部が前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていると判定した場合、前記画像記憶部に記憶されている画像の情報のうち、前記時刻記憶部に記憶されている時刻よりも後に撮影された画像の情報を消去する、
付記５に記載の自律移動装置。
【０１３１】
（付記７）
前記撮像部が撮影した画像の中から該画像に含まれている特徴的な部分である特徴点の３次元位置を取得する３次元位置取得部を備え、
前記判定部は、前記３次元位置取得部が取得した３次元位置のうち、前記天井推定部が推定した天井平面との距離が基準天井距離以下の３次元位置の個数が基準個数未満の場合に、前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていると判定する、
付記２から６のいずれか１つに記載の自律移動装置。
【０１３２】
（付記８）
前記判定部は、前記天井推定部が推定した天井平面の傾きと、自律移動装置が移動している床面の傾きと、の角度の差が基準角度以上なら、前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていると判定する、
付記２から７のいずれか１つに記載の自律移動装置。
【０１３３】
（付記９）
前記判定部は、前記天井推定部が推定した第１の天井平面の傾きと、前記天井推定部が前記第１の天井平面よりも前に推定した第２の天井平面の傾きと、の角度の差が基準角度以上なら、前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていると判定する、
付記２から８のいずれか１つに記載の自律移動装置。
【０１３４】
（付記１０）
前記判定部は、自機と前方の壁との距離を取得し、該距離が前方距離基準値未満のときには、前記地図記憶部に記憶されている地図の情報に誤差が基準以上多く含まれていると判定しない、
付記２から９のいずれか１つに記載の自律移動装置。
【０１３５】
（付記１１）
前記天井推定部は、３次元ハフ変換によって天井平面を推定する、
付記１から１０のいずれか１つに記載の自律移動装置。
【０１３６】
（付記１２）
前記撮像部は、単眼の撮像部であり、
前記地図作成部は前記単眼の撮像部が撮影した複数の画像の情報に基づいて地図の情報を作成する、
付記１から１１のいずれか１つに記載の自律移動装置。
【０１３７】
（付記１３）
床と接しているか否かを検出する床面センサを備え、
前記床面センサが、床と接していると検出している時のみ、前記地図作成部は地図の情報を作成する、
付記１から１２のいずれか１つに記載の自律移動装置。
【０１３８】
（付記１４）
撮影した複数の画像の情報を用いて地図の情報を作成するとともに天井平面を推定し、
前記推定した天井平面を用いて前記作成した地図の情報に誤差が基準以上多く含まれているか否かを判定し、
前記判定した結果に応じて所定の処理を行う、
画像処理方法。
【０１３９】
（付記１５）
撮影ステップと、
前記撮影ステップで撮影した複数の画像の情報を用いて地図の情報を作成する地図作成ステップと、
前記撮影ステップで撮影した複数の画像の情報を用いて天井平面を推定する天井推定ステップと、
前記天井推定ステップで推定した天井平面を用いて前記地図作成ステップで作成した地図の情報に誤差が基準以上多く含まれているか否かを判定する判定ステップと、
前記判定ステップで判定した結果に応じて所定の処理を行う判定結果処理ステップと、
を備える画像処理方法。
【０１４０】
（付記１６）
コンピュータに、
撮影ステップ、
前記撮影ステップで撮影した複数の画像の情報を用いて地図の情報を作成する地図作成ステップ、
前記撮影ステップで撮影した複数の画像の情報を用いて天井平面を推定する天井推定ステップ、
前記天井推定ステップで推定した天井平面を用いて前記地図作成ステップで作成した地図の情報に誤差が基準以上多く含まれているか否かを判定する判定ステップ、及び、
前記判定ステップで判定した結果に応じて所定の処理を行う判定結果処理ステップ、
を実行させるためのプログラム。
【符号の説明】
【０１４１】
１０，５０…制御部、１１…地図作成部、１２…位置推定部、１３…天井推定部、１４…判定部、１５…地図編集部、２０，６０…記憶部、２１…画像記憶部、２２…地図記憶部、２３…天井推定値記憶部、３０…センサ部、３１…障害物センサ、３２…床面センサ、４１…撮像部、４２…駆動部、４３…入力部、４４，７１…通信部、４５…電源、１００…自律移動装置、２００…サーバー
【図１】
2018156538.tif 000007
【図２】
2018156538.tif 000008
【図３】
2018156538.tif 000009
【図４】
2018156538.tif 000010
【図５】
2018156538.tif 000011
【図６】
2018156538.tif 000012
【図７】
2018156538.tif 000013
【図８】
2018156538.tif 000014
【図９】
2018156538.tif 000015
【図１０Ａ】
2018156538.tif 000016
【図１０Ｂ】
2018156538.tif 000017
【図１１】
2018156538.tif 000018
【図１２】
2018156538.tif 000019
【図１３】
2018156538.tif 000020
【図１４】
2018156538.tif 000021