(57)【要約】【課題】環境の変化に対してロバストな自己位置推定を可能にする。【解決手段】自律移動装置１００は、撮像部３３で撮影された画像を用いて環境地図の作成及び位置の推定を行う。自律移動装置１００は、制御部１０と記憶部２０とを備える。制御部１０は、周囲の環境の変化に応じて複数の環境地図を作成し、作成した複数の環境地図を正規化して統一的に扱えるように記憶部２０に保存し、正規化された環境地図を用いて位置を推定する。【選択図】図４
2020021257.tif 000002
【特許請求の範囲】
【請求項１】
撮像部で撮影された画像を用いて環境地図の作成及び位置の推定を行う自律移動装置であって、
制御部と記憶部とを備え、
前記制御部は、
周囲の環境の変化に応じて複数の環境地図を作成し、
前記作成した複数の環境地図を正規化して統一的に扱えるように前記記憶部に保存し、
前記正規化された環境地図を用いて位置を推定する、
自律移動装置。
【請求項２】
前記制御部は、
前記複数の環境地図の中から前記正規化の基準に用いる環境地図である基準地図を選択し、
前記複数の環境地図のうち、前記基準地図で正規化する対象の環境地図である対象地図を前記基準地図に基づいて正規化する、
請求項１に記載の自律移動装置。
【請求項３】
前記環境地図は、前記撮像部で撮影された画像のうち位置の推定に用いる画像であるキーフレームの情報と、前記撮像部で撮影された画像に含まれる特徴的な点である特徴点のうち３次元位置の座標が推定できた特徴点であるＭａｐＰｏｉｎｔの情報と、を含み、さらに、前記キーフレームの情報として、前記キーフレームが撮影された時の前記自律移動装置の姿勢の情報と、前記キーフレームに含まれる前記特徴点の情報と、を含み、
前記制御部は、前記対象地図に含まれる前記姿勢の情報及び前記ＭａｐＰｏｉｎｔの情報を、前記基準地図の座標系における値に変換することにより、前記対象地図を正規化する、
請求項２に記載の自律移動装置。
【請求項４】
前記制御部は、
前記対象地図に含まれる前記キーフレームと、前記キーフレームに類似する前記基準地図に含まれるキーフレームと、の対応により推定した、前記キーフレームが撮影された時の前記自律移動装置の姿勢に基づいて、前記対象地図から前記基準地図への姿勢変換行列を算出し、
前記姿勢変換行列により、前記環境地図を正規化する、
請求項３に記載の自律移動装置。
【請求項５】
前記制御部は、
前記対象地図に含まれる複数の前記キーフレームそれぞれを用いて前記姿勢変換行列を算出することにより、複数の姿勢変換行列からなる姿勢変換行列群を算出し、
前記姿勢変換行列群に含まれる複数の姿勢変換行列を用いて誤差を少なくした１つの姿勢変換行列を算出する、
請求項４に記載の自律移動装置。
【請求項６】
前記制御部は、前記姿勢変換行列群に含まれる複数の姿勢変換行列の中央値を算出することにより前記１つの姿勢変換行列を算出する、
請求項５に記載の自律移動装置。
【請求項７】
前記制御部は、前記複数の姿勢変換行列の中央値を算出する際に、前記複数の姿勢変換行列のそれぞれから取り出した回転行列をクォータニオンに変換し、前記クォータニオンを線形空間に射影して線形空間内で中央値を算出する、
請求項６に記載の自律移動装置。
【請求項８】
前記制御部は、前記姿勢変換行列群に含まれる姿勢変換行列の数が所定の閾値以下ならば、前記環境地図に含まれる前記ＭａｐＰｏｉｎｔを用いて、前記１つの姿勢変換行列の誤差を少なくする、
請求項５から７のいずれか１項に記載の自律移動装置。
【請求項９】
前記制御部は、
前記対象地図から前記基準地図への姿勢変換行列を算出できない場合は、前記対象地図を削除して、新たな環境地図の作成を開始する、
請求項４から８のいずれか１項に記載の自律移動装置。
【請求項１０】
前記環境地図は、さらに、正規化の際の基準となる姿勢である基準姿勢の情報を含み、
前記制御部は、前記基準姿勢に基づいて、前記対象地図から前記基準地図への姿勢変換行列を算出し、
前記姿勢変換行列により、前記対象地図を正規化する、
請求項３に記載の自律移動装置。
【請求項１１】
前記制御部は、
前記対象地図を正規化する前に、前記対象地図に前記基準姿勢が登録されているか否かを判定し、前記基準姿勢が登録されていないなら、基準姿勢を登録する所定の場所まで前記自律移動装置を移動させて、前記所定の場所で前記基準姿勢を前記対象地図に登録する、
請求項１０に記載の自律移動装置。
【請求項１２】
前記基準姿勢を登録する所定の場所は、充電器の設置場所である、
請求項１１に記載の自律移動装置。
【請求項１３】
前記制御部は、
前記対象地図に含まれる姿勢の情報から取り出した位置ベクトルの各要素の標準偏差と、前記基準地図に含まれる姿勢の情報から取り出した位置ベクトルの各要素の標準偏差と、の比に基づいて、前記対象地図から前記基準地図への長さの比であるスケールを算出し、
前記スケールも用いて前記環境地図を正規化する、
請求項３から１２のいずれか１項に記載の自律移動装置。
【請求項１４】
前記周囲の環境の変化は、照明の変化である、
請求項１から１３のいずれか１項に記載の自律移動装置。
【請求項１５】
撮像部で撮影された画像を用いて環境地図の作成及び位置の推定を行う自律移動装置の自律移動方法であって、
周囲の環境の変化に応じて複数の環境地図を作成し、
前記作成した複数の環境地図を正規化して統一的に扱えるように記憶部に保存し、
前記正規化された環境地図を用いて位置を推定する、
自律移動方法。
【請求項１６】
撮像部で撮影された画像を用いて環境地図の作成及び位置の推定を行う自律移動装置のコンピュータに、
周囲の環境の変化に応じて複数の環境地図を作成し、
前記作成した複数の環境地図を正規化して統一的に扱えるように記憶部に保存し、
前記正規化された環境地図を用いて位置を推定する、
処理を実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自律移動装置、自律移動方法及びプログラムに関する。
【背景技術】
【０００２】
屋内を自動的に掃除する掃除機ロボットのような、環境地図を作成して自律的に移動する自律移動装置が普及してきている。このような自律移動装置は、カメラを使用して自己位置推定と環境地図作成とを同時に行うｖＳＬＡＭ（Ｖｉｓｕａｌ  Ｓｉｍｕｌｔａｎｅｏｕｓ  Ｌｏｃａｌｉｚａｔｉｏｎ  Ａｎｄ  Ｍａｐｐｉｎｇ）処理を行っていることが多い。ｖＳＬＡＭ処理では、カメラで撮影した画像に含まれる特徴点に基づいて自己位置推定と環境地図作成とが行われるので、自己位置推定の精度や作成される環境地図の内容は照明等の環境に大きく影響を受ける。そのため、環境地図を作成した際の照明等の環境と異なる環境において、その環境地図を利用して自己位置の推定を行うと、位置推定の性能が大幅に悪化してしまう。この問題を解決する技術として、特許文献１には、周囲環境のランドマーク配置をもとに自己位置を推定することにより、外乱による影響を抑制した自己位置推定を行うことができる自律移動装置が記載されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】国際公開第２０１６／０１６９５５号
【発明の概要】
【発明が解決しようとする課題】
【０００４】
特許文献１に記載されている自律移動装置は、照明変化等の外乱要因（環境の変化）に影響されないランドマークを選択し、選択したランドマークを追跡するようにカメラを制御し、そのように追跡制御されているカメラで撮影したランドマークの配置をもとに自己位置を推定するため、環境の変化に対してロバストな自己位置推定ができる。しかし、環境の変化に影響されないランドマークとは、床の白線等の明確な形状を持つランドマーク（マーカー）であり、周囲にこのような明確なランドマークがない環境では、自律移動装置の動作が不安定になる可能性があった。
【０００５】
照明環境が変わる度に別の環境地図を作成し、そのようにして作成した複数の環境地図を、その時々の照明環境に応じて使い分けるようにすると、上記の問題は解決する。しかし、ｖＳＬＡＭ処理で作成された複数の環境地図の座標系は、環境地図毎に異なる座標系になってしまい、統一的に扱うことができない。なぜなら、一般的にｖＳＬＡＭ処理で作成する環境地図の座標系においては、ｖＳＬＡＭ処理の初期化終了時の自律移動装置の位置が原点になり、ｖＳＬＡＭ処理の初期化終了時の自律移動装置の向きで座標軸が決まるからである。したがって、従来の自律移動装置では、環境の変化に応じて作成される複数の環境地図を統一的に扱うことができないので、環境の変化に対してロバストな自己位置推定を可能にする技術に改善の余地がある。
【０００６】
本発明は、上記問題を解決するためになされたものであり、複数の環境地図を統一的に扱うことができるようにすることにより、環境の変化に対してロバストな自己位置推定を可能にする自律移動装置、自律移動方法及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００７】
上記目的を達成するため、本発明の自律移動装置は、
撮像部で撮影された画像を用いて環境地図の作成及び位置の推定を行う自律移動装置であって、
制御部と記憶部とを備え、
前記制御部は、
周囲の環境の変化に応じて複数の環境地図を作成し、
前記作成した複数の環境地図を正規化して統一的に扱えるように前記記憶部に保存し、
前記正規化された環境地図を用いて位置を推定する。
【発明の効果】
【０００８】
本発明によれば、環境の変化に対してロバストな自己位置推定を可能にする。
【図面の簡単な説明】
【０００９】
【図１】本発明の実施形態１に係る自律移動装置の外観を示す図である。
【図２】実施形態１に係る充電器の外観を示す図である。
【図３】実施形態１に係る充電器が送信する帰還信号を説明する図である。
【図４】実施形態１に係る自律移動装置の機能構成を示す図である。
【図５】実施形態１に係る自律移動装置が作成する環境地図のデータ構造を示す図である。
【図６】実施形態１に係る自律移動装置の起動時処理のフローチャートである。
【図７】実施形態１に係る自律移動装置の自己位置推定スレッドのフローチャートである。
【図８】実施形態１に係る自律移動装置の環境地図抽出処理のフローチャートである。
【図９】実施形態１に係る自律移動装置のＲｅｌｏｃａｌｉｚａｔｉｏｎ処理のフローチャートである。
【図１０】実施形態１に係る自律移動装置の姿勢推定処理のフローチャートである。
【図１１】実施形態１に係る自律移動装置の環境地図保存処理のフローチャートである。
【図１２】実施形態１に係る自律移動装置の環境地図正規化処理のフローチャートである。
【図１３】実施形態１に係る自律移動装置の姿勢変換行列の誤差修正処理のフローチャートである。
【図１４】実施形態１に係る自律移動装置のＩＣＰ処理のフローチャートである。
【図１５】本発明の実施形態１の変形例１に係る自律移動装置の環境地図保存処理のフローチャートである。
【図１６】本発明の実施形態２に係る自律移動装置が作成する環境地図のデータ構造を示す図である。
【図１７】実施形態２に係る自律移動装置の環境地図保存処理のフローチャートである。
【図１８】実施形態２に係る自律移動装置の環境地図正規化処理のフローチャートである。
【発明を実施するための形態】
【００１０】
以下、本発明の実施形態に係る自律移動装置について、図表を参照して説明する。なお、図中同一又は相当部分には同一符号を付す。
【００１１】
（実施形態１）
本発明の実施形態に係る自律移動装置は、地図（環境地図）を作成しながら、用途に応じて自律的に移動する装置である。この用途とは、例えば、警備監視用、屋内掃除用、ペット用、玩具用等である。
【００１２】
実施形態１に係る自律移動装置１００は、図１に示すように、外観上、帰還信号受信部３１（３１ａ，３１ｂ）、駆動部３２（３２ａ，３２ｂ）、撮像部３３、充電接続部３５、を備える。なお、図１には示されていないが、自律移動装置１００は、周囲に存在する物体（障害物）を検出する障害物センサを備えてもよい。また、自律移動装置１００の電池を充電するための充電器２００は、図２に示すように、外観上、帰還信号送信部５１（５１ａ，５１ｂ）、電力供給部５２を備える。
【００１３】
自律移動装置１００の充電接続部３５と充電器２００の電力供給部５２とが接続することにより、充電器２００から電力の供給を受けて、自律移動装置１００は内蔵している電池を充電することができる。なお、充電接続部３５及び電力供給部５２は、それぞれがお互いと接続するための接続端子である。これらは、駆動部３２により自律移動装置１００が充電器２００の上に移動することによって、接続される。この接続は、充電接続部３５と電力供給部５２とが接触することによって接続してもよいし、充電接続部３５と電力供給部５２とが近接することによって電磁誘導等により接続してもよい。
【００１４】
撮像部３３は、自律移動装置１００の前方から上部までの広範囲を撮影できる広角レンズを備える。このため、撮像部３３は天井の電灯が点灯しているか否かを判別可能な画像を撮影することができる。そして、自律移動装置１００は、撮像部３３が撮影した画像を利用して単眼のＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ  Ｌｏｃａｌｉｚａｔｉｏｎ  Ａｎｄ  Ｍａｐｐｉｎｇ）処理を行うことができる。
【００１５】
自律移動装置１００の帰還信号受信部３１は、充電器２００から送信される帰還信号（赤外線ビーコン）を受信するためのデバイスである。自律移動装置１００の正面に向かって左に帰還信号受信部３１ａ、右に帰還信号受信部３１ｂと、合計２つの帰還信号受信部３１が備えられている。また、充電器２００の帰還信号送信部５１は、自律移動装置１００に帰還信号を送信するためのデバイスである。充電器２００の正面に向かって右に帰還信号送信部５１ａが、左に帰還信号送信部５１ｂが、それぞれ備えられている。そして、帰還信号送信部５１ａから送信される帰還信号と、帰還信号送信部５１ｂから送信される帰還信号とは、異なる信号になっている。したがって、帰還信号受信部３１は、各々左右どちらの帰還信号送信部５１からの帰還信号を受信したのかを判別することができる。
【００１６】
充電器２００の帰還信号送信部５１から送信される帰還信号の左右それぞれの受信可能範囲５３（５３ａ，５３ｂ）の一例を図３に示す。帰還信号送信部５１ａから送信される帰還信号は、受信可能範囲５３ａの内部に自律移動装置１００の帰還信号受信部３１が入ると受信できる。そして、帰還信号送信部５１ｂから送信される帰還信号は、受信可能範囲５３ｂの内部に自律移動装置１００の帰還信号受信部３１が入ると受信できる。したがって、自律移動装置１００は、受信可能範囲５３の内部に入ると充電器２００の存在する方向を把握することができる。そして、自律移動装置１００は、帰還信号送信部５１ａからの帰還信号を帰還信号受信部３１ａが受信し、帰還信号送信部５１ｂからの帰還信号を帰還信号受信部３１ｂが受信するように向きを調整しながら進むことによって、充電器２００の上に移動することができる。自律移動装置１００は、充電器２００の上に移動すると、充電接続部３５と電力供給部５２とが接続し、自律移動装置１００に内蔵されている電池を充電することができる。
【００１７】
駆動部３２は、独立２輪駆動型であって、車輪とモータとを備える移動手段である。自律移動装置１００は、２つの車輪の同一方向駆動により前後の平行移動（並進移動）を、２つの車輪の逆方向駆動によりその場での回転（向き変更）を、２つの車輪のそれぞれ速度を変えた駆動により旋回移動（並進＋回転（向き変更）移動）を、行うことができる。また、各々の車輪にはロータリエンコーダが備えられており、ロータリエンコーダで車輪の回転数を計測し、車輪の直径や車輪間の距離等の幾何学的関係を利用することで並進移動量及び回転量を計算できる。例えば、車輪の直径をＤ、回転数をＲ（ロータリエンコーダにより測定）とすると、その車輪の接地部分での並進移動量はπ・Ｄ・Ｒとなる。また、車輪の直径をＤ、車輪間の距離をＩ、右車輪の回転数をＲＲ、左車輪の回転数をＲＬとすると、向き変更の回転量は（右回転を正とすると）３６０°×Ｄ×（ＲＬ−ＲＲ）／（２×Ｉ）となる。この並進移動量及び回転量を逐次足し合わせていくことで、駆動部３２は、いわゆるオドメトリ（メカオドメトリ）として機能し、自己位置（移動開始時の位置及び向きを基準とした自律移動装置１００の位置及び向き）を計測することができる。
【００１８】
図４に示すように、自律移動装置１００は、帰還信号受信部３１（３１ａ，３１ｂ）、駆動部３２（３２ａ，３２ｂ）、撮像部３３、充電接続部３５、に加えて、制御部１０、記憶部２０、通信部３４、を備える。
【００１９】
制御部１０は、ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）等で構成され、記憶部２０に記憶されたプログラムを実行することにより、後述する各部（環境情報取得部１１、地図作成部１２、地図正規化部１３、自己位置推定部１４、行動計画部１５、移動制御部１６）の機能を実現する。また、制御部１０は、時計（図示せず）を備え、現在日時の取得や経過時間のカウントをすることができる。
【００２０】
記憶部２０は、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）等で構成され、ＲＯＭの一部又は全部は電気的に書き換え可能なメモリ（フラッシュメモリ等）で構成されている。記憶部２０は、機能的に、地図記憶部２１と、地図保存部２２と、を含む。ＲＯＭには制御部１０のＣＰＵが実行するプログラム及びプログラムを実行する上で予め必要なデータが記憶されている。ＲＡＭには、プログラム実行中に作成されたり変更されたりするデータが記憶される。
【００２１】
地図記憶部２１には、撮像部３３が撮影した画像の情報に基づいてＳＬＡＭ処理によって地図作成部１２が作成した環境地図が記憶される。環境地図は、図５に示すように、地図ＩＤ（Ｉｄｅｎｔｉｆｉｅｒ）、環境情報、キーフレーム情報群及びＭａｐＰｏｉｎｔ情報群を含む。地図ＩＤは、環境地図を一意に識別するためのＩＤである。環境情報は、周囲の明るさ等、ＳＬＡＭ処理による位置推定に影響を及ぼすと考えられる周囲の環境の情報である。ここでは、キーフレーム情報群及びＭａｐＰｏｉｎｔ情報群を取得した際の環境情報が記録される。
【００２２】
なお、キーフレームとは、ＳＬＡＭ処理において、撮像部３３が撮影した画像（フレーム）のうち３次元（３Ｄ）位置の推定に用いるフレームのことをいう。また、ＭａｐＰｏｉｎｔとは、ＳＬＡＭ処理によって、環境地図（３次元空間）内での３Ｄ位置の座標が推定できた特徴点の３Ｄ座標点（３次元空間内の点）のことをいう。
【００２３】
そして、図５に示すように、キーフレーム情報は、当該キーフレームを一意に識別するためのＩＤであるキーフレームＩＤと、キーフレームを撮影した時の撮像部３３（自律移動装置１００）の環境地図（３次元空間）内での３Ｄ姿勢（位置及び向き）と、キーフレーム中に含まれる特徴点の情報である特徴点情報（通常、１つのキーフレームには複数の特徴点を含むので、特徴点情報群となる。）を含む。
【００２４】
このうち３Ｄ姿勢は、回転や並進の計算を容易にするため、向きを表す（３×３の）回転行列と位置（３Ｄ座標）を表す（３×１の）位置行列（位置ベクトル）とを１つの行列で表した（４×４の）同次座標形式の姿勢行列で表す。このため、後述する姿勢変換行列も回転行列と位置行列とを含む（同次座標形式で表される４×４の）行列になる。なお、回転行列には、回転行列の右側に作用させるベクトルを記述する右手座標系と、回転行列の左側に作用させるベクトルを記述する左手座標系があるが、本実施形態においては、右手座標系の回転行列を用いる。
【００２５】
キーフレーム中に含まれる特徴点とは、キーフレーム（画像）中の、エッジ部分やコーナー部分等、画像内の特徴的な部分の点のことをいう。特徴点は、例えばＳＩＦＴ（Ｓｃａｌｅ−Ｉｎｖａｒｉａｎｔ  Ｆｅａｔｕｒｅ  Ｔｒａｎｓｆｏｒｍ）、ＳＵＲＦ（Ｓｐｅｅｄｅｄ  Ｕｐ  Ｒｏｂｕｓｔ  Ｆｅａｔｕｒｅｓ）、ＦＡＳＴ（Ｆｅａｔｕｒｅｓ  ｆｒｏｍ  Ａｃｃｅｌｅｒａｔｅｄ  Ｓｅｇｍｅｎｔ  Ｔｅｓｔ）等のアルゴリズムで検出することができる。そして、図５に示すように、特徴点情報として、当該特徴点のキーフレーム内における２Ｄ座標、当該特徴点の特徴量、そして、当該特徴点の環境地図内における３Ｄ座標点が推定済であるならその特徴点に対応するＭａｐＰｏｉｎｔのＩＤを含む。当該特徴点の３Ｄ座標がまだ推定されていないなら「対応するＭａｐＰｏｉｎｔのＩＤ」には、３Ｄ座標が未推定であることを示す特別なＩＤ（例えば０）が格納される。なお、特徴点の特徴量としては、例えばＯＲＢ（Ｏｒｉｅｎｔｅｄ  ＦＡＳＴ  ａｎｄ  Ｒｏｔａｔｅｄ  ＢＲＩＥＦ）特徴を用いることができる。
【００２６】
また、図５に示すように、ＭａｐＰｏｉｎｔ情報は当該ＭａｐＰｏｉｎｔを一意に識別するためのＩＤであるＭａｐＰｏｉｎｔ  ＩＤと、当該ＭａｐＰｏｉｎｔの環境地図内における３Ｄ座標が含まれる。したがって、特徴点情報に含まれる「対応するＭａｐＰｏｉｎｔのＩＤ」に基づいてＭａｐＰｏｉｎｔ情報を参照することにより、当該特徴点の環境地図内における３Ｄ座標を取得することができる。
【００２７】
また、環境情報は、電灯のＯＮ／ＯＦＦ、周囲の明るさ、時刻等、自律移動装置１００の位置推定に影響を及ぼすと考えられる周囲の環境の情報である。環境情報として重要なものは主に明るさに関係する情報（照明のＯＮ／ＯＦＦ、カーテンの開け閉め、窓からの日光の条件（朝と夕、天候）等）であるが、人の数、家具の配置等を環境情報に含めてもよい。また、気温、湿度、気圧等は位置推定には直接は影響しないが、これらによって部屋の配置や人の出入りが変化するのであれば、位置推定に影響を及ぼすため、これらの情報を環境情報に含めてもよい。なお、本実施例では、環境情報として、点灯している電灯の数を用いる。これは、例えば、天井を撮影した画像の中で、輝度が高い領域（所定の輝度基準値以上の明るさをもつ領域）の数として求めることができる。
【００２８】
地図保存部２２は、記憶部２０の電気的に書き換え可能なＲＯＭ（フラッシュメモリ等）内に存在し、地図記憶部２１に記憶されている環境地図を、自律移動装置１００の電源が落とされた後も消滅しないように保存する。
【００２９】
通信部３４は、充電器２００やその他の外部装置と無線通信するためのアンテナを含む無線モジュールである。例えば、通信部３４は、Ｂｌｕｅｔｏｏｔｈ（登録商標）に基づく近距離無線通信を行うための無線モジュールである。通信部３４を用いることにより、自律移動装置１００は、外部装置とデータの受け渡し等を行うことができる。
【００３０】
次に、自律移動装置１００の制御部１０の機能的構成について説明する。制御部１０は、環境情報取得部１１、地図作成部１２、地図正規化部１３、自己位置推定部１４、行動計画部１５、移動制御部１６、の機能を実現し、環境地図の作成、自機の姿勢（位置及び向き）の推定、移動制御等を行う。また、制御部１０は、マルチスレッド機能に対応しており、複数のスレッド（異なる処理の流れ）を並行して実行することができる。
【００３１】
環境情報取得部１１は、撮像部３３が撮影した画像データに基づき、周囲の環境を表す環境情報として、点灯している電灯の数を取得する。ただし、これは環境情報の一例にすぎず、他の情報を環境情報として用いたい場合には、環境情報取得部１１は、電灯のＯＮ／ＯＦＦ、明るさ情報等、環境情報として用いたい情報を取得する。また、用いたい環境情報に日時情報が含まれる場合は、環境情報取得部１１は、制御部１０が備える時計から現在日時を取得する。
【００３２】
地図作成部１２は、撮像部３３が撮影した画像データを用いてＳＬＡＭ処理により、図５に示すキーフレーム情報群とＭａｐＰｏｉｎｔ情報群とからなる環境地図のデータを作成し、地図記憶部２１に書き込む。
【００３３】
地図正規化部１３は、地図作成部１２が作成した環境地図のデータを、後述する地図正規化処理により正規化する。
【００３４】
自己位置推定部１４は、撮像部３３が撮影した画像データと地図記憶部２１に記憶されている環境地図のデータとを用いてＳＬＡＭ処理により、環境地図の座標系における自律移動装置１００の姿勢（位置及び向き）を推定する。なお、毎回「姿勢（位置及び向き）」と記載するのは煩雑であるため、本明細書及び特許請求の範囲では、単に「位置」と記載している場合でも、位置だけでなく向きも含む場合がある。つまり「姿勢（位置及び向き）」を表す言葉として「位置」が用いられる場合がある。特に「自己位置の推定」は、自機（自律移動装置１００）の姿勢（位置及び向き）の推定を意味するものとする。
【００３５】
行動計画部１５は、地図記憶部２１に記憶されている環境地図及び動作モードに基づき、移動先及び経路を設定する。なお、動作モードとは、自律移動装置１００の行動様式を定めるものである。自律移動装置１００は、例えば、ランダムに移動する「自由散歩モード」、地図の作成範囲を広げていく「地図作成モード」、後述するメインスレッド等から指定された場所に移動する「移動先指定モード」等の複数の動作モードを持つ。動作モードは、例えば、初期値は地図作成モードで、地図がある程度作成されたら（例えば地図作成モードで１０分経過したら）自由散歩モードになり、電池残量が少なくなったら充電器２００の位置を移動先に指定された移動先指定モードになるというように、予め変化する条件が設定されていてもよいし、外部（ユーザ、メインスレッド等）からの指示により、設定されてもよい。行動計画部１５が経路を設定する際には、地図作成部１２が作成した環境地図に基づき、自律移動装置１００の現在位置から移動先までの経路を設定する。
【００３６】
移動制御部１６は、行動計画部１５が設定した経路に沿って、自律移動装置１００を移動させるように駆動部３２を制御する。
【００３７】
以上、自律移動装置１００の機能構成について説明した。次に、自律移動装置１００で起動される各種処理について説明する。自律移動装置１００は、電源オフ時は充電器２００に接続されて充電されている。そして、電源が投入されると、充電器２００に接続された位置で、後述する起動時処理が実行され、メインスレッドをはじめとする各種スレッドが並行して実行開始され、自律移動装置１００の用途に応じた処理が行われていくことになる。自律移動装置１００の起動時の処理について、図６を参照して説明する。図６は、自律移動装置１００が起動した時に実行される起動時処理のフローチャートである。
【００３８】
まず、自律移動装置１００の制御部１０は、メインスレッドを起動する（ステップＳ１０１）。メインスレッドは、次のステップで起動される自己位置推定スレッドから自律移動装置１００の現在の環境地図内における姿勢（位置及び向き）の情報を受けて、用途に応じた処理（例えば屋内掃除用の処理）を行うスレッドである。
【００３９】
次に、制御部１０は、ＳＬＡＭ処理用の各種スレッドを起動する（ステップＳ１０２）。ＳＬＡＭ処理用の各種スレッドとは、自律移動装置１００の姿勢（位置及び向き）を推定する自己位置推定スレッド、環境地図を作成する地図作成スレッド、ループクロージング処理を行うループクロージングスレッド等である。なお、ループクロージング処理とは、自律移動装置１００が、以前来たことのある同じ場所に戻ってきたことを認識した場合に、以前この同じ場所にいた時の自律移動装置１００の姿勢と現在の姿勢とのずれを用いて、以前来た時から今までの移動軌跡中のキーフレーム（の３Ｄ姿勢）や、関連するＭａｐＰｏｉｎｔ（の３Ｄ座標）を修正する処理をいう。
【００４０】
次に、制御部１０は、移動スレッドを起動し（ステップＳ１０３）、起動時処理を終了する。移動スレッドは、メインスレッドから移動命令を受けて、移動制御部１６が駆動部３２を制御して自律移動装置１００を移動させる処理を行うスレッドである。そして、起動時処理を終了した後は、自律移動装置１００は、この起動時処理によって起動された各スレッドにより制御される。
【００４１】
ＳＬＡＭ処理用のスレッドのうち、自己位置推定スレッドについて、図７を参照して説明する。自己位置推定スレッドは、地図保存部２２に保存されている環境地図のうち現在の環境に適合した環境地図を選択し、その選択した環境地図を用いて追跡処理（自己位置推定処理）を行うスレッドである。
【００４２】
まず、制御部１０は、地図保存部２２に環境地図が保存されているか否かを判定する（ステップＳ２０１）。地図保存部２２に環境地図が保存されていなければ（ステップＳ２０１；Ｎｏ）、制御部１０は初期状態からＳＬＡＭ処理を開始し、変数ＭＯＤＥにＴＲＡＣＫＩＮＧをセットし（ステップＳ２０２）、ステップＳ２１２に進む。なお、変数ＭＯＤＥは、自律移動装置１００が現在、自己位置を推定可能な状態（ＴＲＡＣＫＩＮＧ状態）なのか、推定できない状態（ＬＯＳＴ状態）なのかを表す変数である。また、ステップＳ２０２での「初期状態からＳＬＡＭ処理を開始」とは、環境地図がクリアされた状態から移動しつつ撮像部３３で画像を２つ取得し、この２つの画像間での特徴点の対応に基づいて、自己位置の推定と環境地図作成を開始することを意味する。２つの画像間での特徴点の対応数が５点以上あれば、Ｔｗｏ−ｖｉｅｗ  Ｓｔｒｕｃｔｕｒｅ  ｆｒｏｍ  Ｍｏｔｉｏｎ法を用いることで、２つの画像間の姿勢（それぞれの画像を取得した位置の差分（並進ベクトルｔ）及び向きの差分（回転行列Ｒ））を推定することができるので、５点以上の特徴点の対応が取れるまで撮像部３３で画像を取得し続けることで、制御部１０はいずれは２つの画像間の姿勢を推定できる。そして、この推定した姿勢は、最初の画像を取得した位置を原点とした場合の、２つ目の画像を取得した時の自己位置（位置及び向き）であると考えることができ、制御部１０は、その後は、推定した姿勢や特徴点の情報を環境地図の情報として地図記憶部２１に書き込みつつ、環境地図に含まれる特徴点と撮像部３３で取得した画像中の特徴点とから自己位置の推定をすることができるようになる。これがＳＬＡＭ処理である。
【００４３】
地図保存部２２に環境地図が保存されていれば（ステップＳ２０１；Ｙｅｓ）、制御部１０は、地図保存部２２に保存されている環境地図の中から現在の環境に適合する可能性の高い環境地図を抽出する処理である環境地図抽出処理を行う（ステップＳ２０３）。環境地図抽出処理の詳細については後述する。
【００４４】
そして、制御部１０は、現在の自己位置が不明な状態において環境地図を利用して自己位置を推定する処理であるＲｅｌｏｃａｌｉｚａｔｉｏｎ処理を行う（ステップＳ２０４）。Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理の詳細については後述する。
【００４５】
そして、制御部１０は、Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理が成功したか否かを判定する（ステップＳ２０５）。Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理が失敗したら（ステップＳ２０５；Ｎｏ）、制御部１０はメインスレッドやユーザから終了指示があるか否かを判定する（ステップＳ２０６）。終了指示があるなら（ステップＳ２０６；Ｙｅｓ）、自己位置推定スレッドを終了する。終了指示がないなら（ステップＳ２０６；Ｎｏ）、移動スレッドに移動指示を出して移動する（ステップＳ２０７）。なお、ステップＳ２０７での移動は、Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理の最初に取得する画像を変化させるための移動であり、例えば並行動作している他のスレッドの処理により自律移動装置１００が移動している場合は、ステップＳ２０７で改めて移動する必要はない。
【００４６】
そして、制御部１０は、ステップＳ２０４でのＲｅｌｏｃａｌｉｚａｔｉｏｎ処理が所定時間以上失敗し続けているか否かを判定する（ステップＳ２０８）。この判定は、例えば、Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理の失敗を記録する変数ＲＦＣと、変数ＲＦＣが１になった時の時刻を記録する変数ＲＦＴと、を導入して以下のように行う。Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理が成功したら変数ＲＦＣ及び変数ＲＦＴを０に初期化し、失敗したら変数ＲＦＣに１を加算する。変数ＲＦＣが１になったら、その時の時刻を変数ＲＦＴに記録する。そして、ステップＳ２０８では、現在時刻が変数ＲＦＴに記録されている時刻から所定時間（例えば５分）経過しているか否かを判定する。
【００４７】
Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理が所定時間以上失敗し続けているなら（ステップＳ２０８；Ｙｅｓ）、ステップＳ２０２に行き、ＳＬＡＭ処理の初期化（及び地図記憶部２１のクリア）から処理をやり直すようにする。Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理が失敗してもまだ所定時間経過していないなら（ステップＳ２０８；Ｎｏ）、ステップＳ２０４に戻って再度Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理を行う。なお、ステップＳ２０８での判定として、所定時間の代わりに所定回数（例えば５回）以上失敗し続けているか否かを判定しても良い。この判定は、上述した変数ＲＦＣの値が所定回数以上になったか否かにより行うことができる。
【００４８】
一方、Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理が成功したら（ステップＳ２０５；Ｙｅｓ）、制御部１０は、Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理が成功した際に選択していた環境地図を地図記憶部２１に読み込んで、その後の自己位置推定に利用する（ステップＳ２０９）。そして、制御部１０は、変数ＭＯＤＥにＴＲＡＣＫＩＮＧをセットし（ステップＳ２１０）、ステップＳ２１２に進む。
【００４９】
ステップＳ２１２では、ＳＬＡＭによる追跡処理により、自己位置を推定する。この追跡処理は、まず、撮像部３３が撮影した画像データから特徴点を抽出し、抽出した特徴点と、環境地図に含まれる３Ｄ座標が推定済みであるキーフレームの特徴点との対応を、特徴量を利用して取得する。対応が取れた特徴点（対応特徴点）の数が追跡可能基準数（例えば１０）以上あれば、対応特徴点の画像中の２Ｄ座標と、環境地図内における３Ｄ座標との関係から、制御部１０は自己位置を推定することができる。この場合は追跡成功となる。対応特徴点の数が追跡可能基準数未満の場合は自己位置を推定しても誤差が大きくなってしまうため、制御部１０は、追跡失敗と判断して、自己位置の推定は行わない。
【００５０】
制御部１０は、追跡処理の後、追跡処理が成功したか否かを判定する（ステップＳ２１３）。追跡処理が成功したなら（ステップＳ２１３；Ｙｅｓ）、制御部１０は、追跡処理で取得した自己位置をメインスレッドに送信する（ステップＳ２１４）。そして、制御部１０は、所定時間（例えば１０秒）スリープする（ステップＳ２１５）。
【００５１】
一方、追跡処理が失敗したなら（ステップＳ２１３；Ｎｏ）、制御部１０は、変数ＭＯＤＥにＬＯＳＴをセットし（ステップＳ２２１）、メインスレッドに自己位置取得を失敗した旨を送信し（ステップＳ２２２）、ステップＳ２１５に進んで所定時間スリープする。
【００５２】
その後、制御部１０は、メインスレッドやユーザから終了指示があるか否かを判定する（ステップＳ２１６）。終了指示があるなら（ステップＳ２１６；Ｙｅｓ）、自己位置推定スレッドを終了する。終了指示がないなら（ステップＳ２１６；Ｎｏ）、環境地図を正規化して地図保存部２２に保存する処理である環境地図保存処理を行う（ステップＳ２１７）。環境地図保存処理の詳細については後述する。
【００５３】
次に、制御部１０は、変数ＭＯＤＥにセットされている値がＬＯＳＴであるか否かを判定する（ステップＳ２１１）。変数ＭＯＤＥにセットされている値がＬＯＳＴでないなら（ステップＳ２１１；Ｎｏ）、ＴＲＡＣＫＩＮＧ中であるということなので、ステップＳ２１２に進んで追跡処理を行う。
【００５４】
変数ＭＯＤＥにセットされている値がＬＯＳＴなら（ステップＳ２１１；Ｙｅｓ）、制御部１０は、現在利用している（地図記憶部２１に読み込まれている）環境地図を用いてＲｅｌｏｃａｌｉｚａｔｉｏｎ処理を行い（ステップＳ２１８）、そのＲｅｌｏｃａｌｉｚａｔｉｏｎ処理が成功したか否かを判定する（ステップＳ２１９）。Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理が成功したなら（ステップＳ２１９；Ｙｅｓ）、制御部１０は、変数ＭＯＤＥにＴＲＡＣＫＩＮＧをセットし（ステップＳ２２０）、ステップＳ２１４に進む。Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理が失敗したなら（ステップＳ２１９；Ｎｏ）、ステップＳ２２２に進む。
【００５５】
以上、自己位置推定スレッドの処理について説明した。次に、自己位置推定スレッド（図７）のステップＳ２０３で実行される環境地図抽出処理について、図８を参照して説明する。この処理は、地図保存部２２に保存されている複数の環境地図の中から、現在の環境に適合する可能性の高い環境地図を抽出するために、現在の環境情報と一致又は類似する環境情報を含んだ環境地図を抽出する処理である。
【００５６】
まず、制御部１０は、撮像部３３で画像を撮影する（ステップＳ３０１）。そして、画像中の輝度が高い領域（所定の輝度基準値以上の明るさをもつ領域）の数を求めることにより、点灯している電灯の数を検出する（ステップＳ３０２）。
【００５７】
そして、制御部１０は、地図保存部２２に保存されている複数の環境地図の中から、環境情報（点灯電灯数）が同じか又は類似する環境地図を所定の候補数（Ｎ個）抽出し（ステップＳ３０３）、環境地図抽出処理を終了する。環境地図を抽出する際は、環境地図に付加されている環境情報が現在の環境情報に類似する順にＮ個抽出する。抽出されたＮ個の環境地図は、今後利用される環境地図の候補となるので、候補環境地図と呼ばれる。なお、Ｎには例えば５等、任意の数を設定してよいが、地図保存部２２に保存されている環境地図の個数が少ない場合には、Ｎ未満の個数の候補環境地図しか抽出できない場合もありうる。
【００５８】
以上説明した環境地図抽出処理により、地図保存部２２に保存されている複数の環境地図の中から環境情報が現在の環境情報と一致又は類似する候補環境地図がＮ個抽出される。次に、自己位置推定スレッド（図７）のステップＳ２０４で実行されるＲｅｌｏｃａｌｉｚａｔｉｏｎ処理について、図９を参照して説明する。Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理とは、現在の自己位置が不明な状態において、環境地図を利用して自己位置を推定する処理である。
【００５９】
まず、制御部１０は、撮像部３３で撮影して画像を取得する（ステップＳ４０１）。そして、画像中の特徴点を検出し、検出した各特徴点の特徴量を算出する（ステップＳ４０２）。特徴点の検出方法や、用いる特徴量は任意だが、例えば、制御部１０は、特徴点の検出方法としてＦＡＳＴを用い、特徴点の特徴量としてＯＲＢを用いることができる。
【００６０】
次に、制御部１０は、先に行われていた環境地図抽出処理で抽出されたＮ個の候補環境地図全てについて、特徴点の対応を確認したか否かを判定する（ステップＳ４０３）。全ての候補環境地図について特徴点対応を確認したなら（ステップＳ４０３；Ｙｅｓ）、Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理は失敗とし（ステップＳ４０４）、Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理を終了する。
【００６１】
まだ特徴点対応を確認していない候補環境地図が残っているなら（ステップＳ４０３；Ｎｏ）、制御部１０は、その残っている候補環境地図の中から順に１つ選択する（ステップＳ４０５）。そして、制御部１０は、ステップＳ４０１で取得した画像と、ステップＳ４０５で選択した環境地図とに基づいて、姿勢推定処理を行う（ステップＳ４０６）。後述するように、姿勢推定処理は、推定する姿勢で撮影した画像Ａ、姿勢推定に用いる環境地図Ｂ及びＲｅｌｏｃａｔｉｏｎ処理か否かを示すフラグ変数ｉｓＲｅｌｏｃの３つの引数を取るサブルーチンになっており、ここではフラグ変数ｉｓＲｅｌｏｃにｔｒｕｅをセットして姿勢推定処理を呼び出す。姿勢推定処理の詳細は後述する。
【００６２】
そして、制御部１０は、姿勢推定処理が成功したか否かを判定する（ステップＳ４０７）。姿勢推定処理が失敗したら（ステップＳ４０７；Ｎｏ）、ステップＳ４０３に戻る。姿勢推定処理が成功したら（ステップＳ４０７；Ｙｅｓ）、Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理は成功とし（ステップＳ４０８）、Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理を終了する。
【００６３】
以上説明したＲｅｌｏｃａｌｉｚａｔｉｏｎ処理により、自律移動装置１００は、撮像部３３で撮影した画像に基づいて、自機の姿勢（位置及び向き）を推定できる環境地図を選択することができる。
【００６４】
次に、上述のＲｅｌｏｃａｌｉｚａｔｉｏｎ処理（図９）のステップＳ４０６で呼び出される姿勢推定処理について、図１０を参照して説明する。上述したように、この姿勢推定処理は３つの引数を取るので、それぞれ、画像Ａ、環境地図Ｂ、フラグ変数ｉｓＲｅｌｏｃとして以下説明する。
【００６５】
まず、制御部１０は、環境地図Ｂのキーフレーム情報群の中から、画像Ａに類似する類似キーフレームを検索する（ステップＳ５０１）。この類似キーフレームの検索方法は任意だが、例えば、環境地図Ｂ内の全キーフレームを特徴量のヒストグラムで分類し、画像Ａの特徴量のヒストグラムとヒストグラムの類似度によって類似検索をすると高速に検索することができる。
【００６６】
そして、制御部１０は、ステップＳ５０１で検索された類似キーフレームの特徴点のうち３Ｄ座標が推定済の特徴点と画像Ａの特徴点との間で特徴量により対応を取る。例えば、類似キーフレーム内のある特徴点（３Ｄ座標推定済）の特徴量と、画像Ａ内のある特徴点の特徴量と、の類似度が所定の基準類似度よりも高い場合、この２つの特徴点は対応が取れた特徴点（対応特徴点）であると判断することとする。そして、制御部１０は、この対応特徴点の数を求める（ステップＳ５０２）。
【００６７】
そして、制御部１０は、対応特徴点の数が３より大きいか否かを判定する（ステップＳ５０３）。対応特徴点の数が３以下であれば（ステップＳ５０３；Ｎｏ）、姿勢推定は失敗として（ステップＳ５０４）、姿勢推定処理を終了する。なお、ここですぐに姿勢推定を失敗とするのではなく、ステップＳ５０１に戻って、画像Ａに類似するキーフレームのうち、前回検索されなかった（つまり類似度の高さが２番以下の）類似キーフレームを検索してもよい。類似度が高くなくても、対応特徴点の数が多い場合はあり得るからである。このようにする場合、所定の回数（例えば３回）ステップＳ５０１に戻って別の類似キーフレームで対応特徴点の数を求めても、それが３以下なら（ステップＳ５０３；Ｎｏ）、姿勢推定は失敗として（ステップＳ５０４）、姿勢推定処理を終了する。
【００６８】
一方、対応特徴点の数が３より大きければ（ステップＳ５０３；Ｙｅｓ）、ステップＳ５０５に進む。画像Ａ内の特徴点のうち４点以上が類似キーフレーム内の特徴点（３Ｄ座標推定済）と対応が取れた場合、ＰｎＰ問題（Ｐｅｒｓｐｅｃｔｉｖｅ−ｎ−Ｐｏｉｎｔ  Ｐｒｏｂｌｅｍ）として、画像Ａを取得した時の自律移動装置１００の姿勢（位置及び向き）を推定することができる。
【００６９】
ステップＳ５０５では、制御部１０は、このＰｎＰ問題を解いて自律移動装置１００の姿勢を推定し、この推定した姿勢を用いて、類似キーフレーム内の特徴点（３Ｄ座標を推定済みであるか否かは問わない）の２Ｄ座標と、その特徴点に対応する画像Ａ内の特徴点の２Ｄ座標と、の間の誤差（対応誤差）を算出する。そして、この対応誤差が基準誤差Ｔ以下であれば、その特徴点の位置は整合すると判断することにして、制御部１０は、そのような整合する対応特徴点の数（整合数）を求める（ステップＳ５０５）。
【００７０】
そして、制御部１０は、フラグ変数ｉｓＲｅｌｏｃがｔｒｕｅであるか否かを判定する（ステップＳ５０６）。フラグ変数ｉｓＲｅｌｏｃがｔｒｕｅなら（ステップＳ５０６；Ｙｅｓ）、制御部１０は、整合数が整合基準値Ｋ（例えば５０）よりも大きいか否かを判定する（ステップＳ５０７）。整合数が整合基準値Ｋ以下なら（ステップＳ５０７；Ｎｏ）、姿勢推定は失敗として（ステップＳ５０４）、姿勢推定処理を終了する。整合数が整合基準値Ｋより大きければ（ステップＳ５０７；Ｙｅｓ）、制御部１０は、ステップＳ５０５で推定した姿勢を類似キーフレームの３Ｄ姿勢に加算することにより姿勢を推定し、推定した結果を行列Ｐａにセットする（ステップＳ５０８）。そして、姿勢推定は成功として（ステップＳ５０９）、姿勢推定処理を終了する。
【００７１】
一方、ステップＳ５０６で、フラグ変数ｉｓＲｅｌｏｃがｔｒｕｅでなければ（ステップＳ５０６；Ｎｏ）、制御部１０は、整合数が整合基準値Ｋ（例えば１０）×０．８よりも大きいか否かを判定する（ステップＳ５１０）。整合数が整合基準値Ｋ×０．８以下なら（ステップＳ５１０；Ｎｏ）、姿勢推定は失敗として（ステップＳ５０４）、姿勢推定処理を終了する。整合数が整合基準値Ｋ×０．８より大きければ（ステップＳ５１０；Ｙｅｓ）、制御部１０は、ステップＳ５０５で推定した姿勢を類似キーフレームの３Ｄ姿勢に加算することにより姿勢を推定し、推定した結果を行列Ｐａにセットする（ステップＳ５０８）。そして、姿勢推定は成功として（ステップＳ５０９）、姿勢推定処理を終了する。
【００７２】
以上説明した姿勢推定処理により、変数ｉｓＲｅｌｏｃ＝ｔｒｕｅの時は、自律移動装置１００は自機の姿勢（位置及び向き）を推定することができ、変数ｉｓＲｅｌｏｃ＝ｆａｌｓｅの時は、最初の引数（画像Ａ）を撮影した時の自律移動装置１００の姿勢を若干の誤差を許して推定することができる。なお、上記のステップＳ５１０では、整合数を整合基準値Ｋ×０．８と比較しているが、この０．８という数値は一例に過ぎない。ただし、この数値を小さくしすぎると誤差が増えてしまうので、１から０．５程度の間で設定するのが好ましい。
【００７３】
また、図９のＲｅｌｏｃａｌｉｚａｔｉｏｎ処理では、候補環境地図のうち最初に姿勢推定に成功した環境地図を最終的に選択する処理にしているが、この処理も一例に過ぎない。例えば、姿勢推定処理（図１０）において、ステップＳ５０５で求めた整合数を戻り値とするようにしておき、Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理では、全ての候補環境地図について整合数を求め、整合数が最も大きい環境地図を最終的に選択するようにしてもよい。また、姿勢推定処理（図１０）の、ステップＳ５０５で整合数を求める際に特徴点の座標位置の誤差も各候補環境地図について記憶するようにしておき、Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理では、全ての候補環境地図の中から最も誤差の小さい環境地図を最終的に選択するようにしてもよい。
【００７４】
次に、自己位置推定スレッド（図７）のステップＳ２１７で実行される環境地図保存処理について、図１１を参照して説明する。この処理は、所定の時間（例えば１時間）毎に、地図記憶部２１に記憶されている地図を地図保存部２２に保存する処理である。
【００７５】
まず制御部１０は、環境地図を前回、地図保存部２２に保存してから、所定時間（例えば１時間）経過したか否かを判定する（ステップＳ６０１）。所定時間経過していないなら（ステップＳ６０１；Ｎｏ）、環境地図保存処理を終了する。所定時間経過しているなら（ステップＳ６０１；Ｙｅｓ）、制御部１０は、撮像部３３で画像を撮影する（ステップＳ６０２）。そして、制御部１０は、画像中の輝度が高い領域の数を数えることにより、点灯している電灯の数を取得する（ステップＳ６０３）。なお、画像中の輝度が高い領域とは、具体的には所定の輝度基準値以上の明るさをもつ領域である。撮像部３３は前方から上部までの広範囲を撮影できる広角レンズを備えるので、天井が撮像範囲に含まれており、天井の電灯の数を判別可能な画像を撮影することができる。ステップＳ６０３において、制御部１０は、環境情報取得部１１として機能する。
【００７６】
そして、制御部１０は、取得した点灯電灯数を環境情報として、地図記憶部２１に書き込む（ステップＳ６０４）。次に、制御部は地図保存部２２に１つ以上の環境地図が保存されているか否かを判定する（ステップＳ６０５）。
【００７７】
地図保存部２２に１つ以上の環境地図が保存されているなら（ステップＳ６０５；Ｙｅｓ）、制御部１０は、地図保存部２２に保存されている環境地図の中から正規化の基準に用いる環境地図（以下、「基準地図」と言う。）を選択する（ステップＳ６０６）。例えば、最初に地図保存部２２に保存された環境地図を「基準地図」として選択する。そして、地図正規化部１３は、地図記憶部２１に記憶されている環境地図を、ステップＳ６０６で選択した「基準地図」を基準として正規化する環境地図正規化処理を行う（ステップＳ６０７）。ここで、正規化とは、環境地図の座標軸、原点及びスケールを、「基準地図」に合わせることを言う。後述するように、環境地図正規化処理は、正規化を行う環境地図（以下、「対象地図」と言う。）と、正規化の基準にする「基準地図」の２つの引数を取るサブルーチンになっている。環境地図正規化処理の詳細については後述する。
【００７８】
そして、制御部１０は、環境地図正規化処理が成功したか否かを判定する（ステップＳ６０８）。環境地図正規化処理が失敗したなら（ステップＳ６０８；Ｎｏ）、制御部１０は、地図記憶部２１をクリアして、初期状態からＳＬＡＭ処理を開始し、変数ＭＯＤＥにＴＲＡＣＫＩＮＧをセットして（ステップＳ６１０）、環境地図保存処理を終了する。環境地図正規化処理が成功したなら（ステップＳ６０８；Ｙｅｓ）、制御部１０は、地図記憶部２１に記憶されている正規化された環境地図（「対象地図」）を地図保存部２２に保存する（ステップＳ６０９）。そして、環境地図保存処理を終了する。
【００７９】
一方、地図保存部２２に環境地図が１つも保存されていなければ（ステップＳ６０５；Ｎｏ）、制御部１０は、地図記憶部２１に記憶されている環境地図をそのまま（正規化処理をする必要なく）地図保存部２２に保存する（ステップＳ６０９）。そして、環境地図保存処理を終了する。
【００８０】
次に、環境地図保存処理（図１１）のステップＳ６０７で呼び出される環境地図正規化処理について、図１２を参照して説明する。この環境地図正規化処理は、上述したように、正規化を行う環境地図及び正規化の基準にする環境地図の２つの引数を取るので、正規化を行う環境地図を「対象地図」、正規化の基準にする環境地図を「基準地図」として、以下説明する。
【００８１】
まず、制御部１０は、作業用変数ｎ及びｍに０を代入する（ステップＳ７０１）。変数ｍは、「対象地図」に含まれているキーフレームを順に１つずつ扱う時のインデックスとして使用される変数である。また、変数ｎは「対象地図」に含まれているキーフレームのうち、「基準地図」を用いて当該キーフレームの姿勢（当該キーフレームを撮影した時の自律移動装置１００の姿勢）を推定するのに成功した数をカウントするために使用される変数である。
【００８２】
次に、制御部１０は、変数ｍに１を加算する（ステップＳ７０２）。そして、制御部１０は、「対象地図」に含まれている全てのキーフレームについて、処理（後述するキーフレームの姿勢を推定する処理）が終了したか否かを判定する（ステップＳ７０３）。「対象地図」に含まれている全てのキーフレームについて処理が終了していなければ（ステップＳ７０３；Ｎｏ）、制御部１０は、「対象地図」のｍ番目のキーフレームについて、「基準地図」に基づいて姿勢推定処理を行う（ステップＳ７０４）。上述したように、姿勢推定処理は、画像Ａ、環境地図Ｂ及びフラグ変数ｉｓＲｅｌｏｃの３つの引数を取るサブルーチンになっているが、ここでは画像Ａに「対象地図」のｍ番目のキーフレームを、環境地図Ｂに「基準地図」を、フラグ変数ｉｓＲｅｌｏｃにｆａｌｓｅをそれぞれセットして、姿勢推定処理を呼び出す。
【００８３】
そして、制御部１０は、ステップＳ７０４で呼び出した姿勢推定処理が姿勢推定に成功したか否かを判定する（ステップＳ７０５）。姿勢推定に成功しなかったなら（ステップＳ７０５；Ｎｏ）、ステップＳ７０２に戻る。姿勢推定に成功したなら（ステップＳ７０５；Ｙｅｓ）、姿勢推定結果等を保存する（ステップＳ７０６）。具体的には、「対象地図」のｍ番目のキーフレームを「基準地図」に基づいて姿勢推定した結果である行列Ｐａを配列変数ＰＡ［ｎ］に代入し、「対象地図」のｍ番目のキーフレームの３Ｄ姿勢を配列変数ＰＸ［ｎ］に代入する。そして変数ｎに１を加算して（ステップＳ７０７）、ステップＳ７０２に戻る。
【００８４】
一方、「対象地図」に含まれている全てのキーフレームについて処理が終了したなら（ステップＳ７０３；Ｙｅｓ）、制御部１０は、変数ｎが０であるか否かを判定する（ステップＳ７０８）。変数ｎが０であるなら（ステップＳ７０８；Ｙｅｓ）、「対象地図」に含まれている全てのキーフレームについて「基準地図」に基づく姿勢推定に失敗したということであり、この場合は「対象地図」を「基準地図」で正規化することができないため、環境地図の正規化は失敗として（ステップＳ７０９）、環境地図正規化処理を終了する。この場合、制御部１０は「対象地図」を破棄し、以降利用しないようにする。
【００８５】
変数ｎが０でなければ（ステップＳ７０８；Ｎｏ）、制御部１０は、「対象地図」から見た「基準地図」のスケールＳ（「対象地図」から「基準地図」へのスケールＳ）を算出する（ステップＳ７１０）。具体的には、以下の式（１）でスケールＳを求める。
Ｓ＝ｓｅｌ（ｓｔｄ（ｐｏｓ（ＰＡ［］））／ｓｔｄ（ｐｏｓ（ＰＸ［］）））  …（１）
【００８６】
ここで、ｐｏｓ（）は、姿勢行列群に含まれる各姿勢行列から位置行列を抜き出して、抜き出した位置行列によって構成された位置行列群を返す関数である。位置行列群は、ｘ，ｙ，ｚの３要素からなる位置行列（列ベクトル）が複数（ｎ個）並んだ行列群である。また、ｓｔｄ（）は、引数の行列群に含まれるｎ個の位置行列の３要素であるｘ，ｙ，ｚそれぞれについて、ｎ個の値から求められる標準偏差を返す関数である。ｓｔｄ（ｐｏｓ（ＰＡ［］））／ｓｔｄ（ｐｏｓ（ＰＸ［］））により、ＰＡから求められる標準偏差とＰＸから求められる標準偏差との比が３要素（ｘ，ｙ，ｚ）それぞれについて求められる。そして、ｓｅｌ（）は、この３要素それぞれについて求められた３つの比の内の最大値を選択する関数である。つまり、スケールＳは、位置行列の３要素であるｘ，ｙ，ｚの各標準偏差の比の内、標準偏差の比が最も大きい値となる。
【００８７】
ただし、式（１）は、自律移動装置１００が平面上を走行するものと仮定した場合の式である。自律移動装置１００が３次元空間を自由に移動できるものである場合は、姿勢行列群ＰＡと姿勢行列群ＰＸとで向きを揃える必要があるので、例えば姿勢行列群ＰＡの回転行列部分ＲＡと、姿勢行列群ＰＸの回転行列部分ＲＸとを取り出し、以下の式（２）のようにしてスケールＳを求めることができる。ここで、ｔｒ（）は転置行列を取る関数である（回転行列においては、逆行列は転置行列であるため、式（２）では逆行列を求めるために転置行列を求めている）。また、ＲＡはｐｏｓ（ＰＡ［］）で取り出した位置行列に対応する回転行列であり、ＲＸはｐｏｓ（ＰＸ［］）で取り出した位置行列に対応する回転行列である。
Ｓ＝ｓｅｌ（ｓｔｄ（ｐｏｓ（ＰＡ［］））／
ｓｔｄ（ＲＡ・ｔｒ（ＲＸ）・ｐｏｓ（ＰＸ［］）））  …（２）
【００８８】
次に、制御部１０は、姿勢行列群ＰＸと姿勢行列群ＰＡとから姿勢変換行列群ＰＡ’を算出する（ステップＳ７１１）。具体的には、姿勢行列群ＰＡは、「基準地図」の座標内における、「対象地図」の各キーフレームの３Ｄ姿勢を表すものになっているため、「対象地図」の各キーフレームの３Ｄ姿勢（姿勢行列群ＰＸ）に対して、以下の式（３）の計算をすることにより、スケール修正を行った「対象地図」から「基準地図」への姿勢変換行列群ＰＡ’を求めることができる。なお、式（３）でｉｎｖ（）は逆行列を求める関数を表す。
ＰＡ’＝ＰＡ・ｉｎｖ（Ｓ・ＰＸ）  …（３）
【００８９】
姿勢行列群ＰＡ及び姿勢行列群ＰＸは、それぞれｎ個の姿勢行列を含んでいるので、各姿勢行列群に含まれているｎ個の各姿勢行列でそれぞれ式（３）の計算を行うことにより、ｎ個の姿勢変換行列を含む姿勢変換行列群ＰＡ’が得られる。ただし、姿勢変換行列群ＰＡ’に含まれる姿勢変換行列は、ステップＳ７０４でｉｓＲｅａｌｏｃ＝ｆａｌｓｅとして姿勢推定処理を呼び出しているため、姿勢推定処理（図１０）のステップＳ５１０の判定における閾値が低く設定されており、誤差を比較的多く含んでいると考えられる。
【００９０】
そこで、制御部１０は、姿勢変換行列の誤差修正処理を行う（ステップＳ７１２）。この誤差修正処理の詳細については後述する。そして、制御部１０は、誤差修正処理を行った姿勢変換行列ＰとスケールＳにより「対象地図」に含まれるキーフレーム情報群及びＭａｐＰｏｉｎｔ情報群を変換する（ステップＳ７１３）。具体的には、「対象地図」に含まれる各キーフレーム情報の３Ｄ姿勢ＰＸ０を以下の式（４）で正規化した３Ｄ姿勢ＰＳに変換し、各ＭａｐＰｏｉｎｔ情報の３Ｄ座標ＭＸ０を以下の式（５）で正規化した３Ｄ座標ＭＳに変換する。
ＰＳ＝Ｐ・Ｓ・ＰＸ０  …（４）
ＭＳ＝Ｐ・Ｓ・ＭＸ０  …（５）
【００９１】
そして、環境地図の正規化は成功として（ステップＳ７１４）、環境地図正規化処理を終了する。上述の処理ではスケールＳを計算により求めているが、メカオドメトリ等で絶対スケールが分かる場合には、ＳＬＡＭの初期化時や環境地図の保存時等にスケールを合わせておいてもよい。複数の環境地図の間でスケールが合っていれば、環境地図正規化処理（図１２）のステップＳ７１０でのスケールＳを算出する処理は不要で、スケールＳ＝１として処理すればよい。
【００９２】
次に、環境地図正規化処理（図１２）のステップＳ７１２で実行される姿勢変換行列の誤差修正処理について、図１３を参照して説明する。
【００９３】
まず、制御部１０は、姿勢変換行列群ＰＡ’に含まれているｎ個の姿勢変換行列の中央値を算出し、姿勢変換行列Ｐ０とする（ステップＳ７３１）。上述したように、姿勢変換行列は同次座標形式で表されるため、回転行列と位置行列から構成されている。姿勢変換行列の中央値を算出するには、回転行列の中央値と位置行列の中央値をそれぞれ求めればよい。位置行列は線形なので、位置行列の中央値は、位置行列の要素毎（ｘ，ｙ，ｚそれぞれ）の中央値を求めればよい。回転行列は非線形なので、容易に回転行列の中央値を算出するために、回転行列をクォータニオン（四元数）に変換して線形空間に射影し、線形空間内で中央値を求め、その後クォータニオンを回転行列に変換して元の非線形空間に戻すという処理を行う。
【００９４】
例えば、回転行列をＥｘｐｏｎｅｎｔｉａｌ  Ｍａｐで扱うことにより線形空間として扱うことができる。具体的には、まず、回転行列をクォータニオンｑに変換する。回転行列からクォータニオンへの変換及びクォータニオンから回転行列の変換については公知技術なので、ここでの説明は省略する。クォータニオンｑはｗ，ｘ，ｙ，ｚの４要素を持っている（ｑ＝ｗ＋ｘｉ＋ｙｊ＋ｚｋ）ので、この４要素を４軸とする４次元空間における、原点を中心とする半径１の超球（ｗ２＋ｘ２＋ｙ２＋ｚ２＝１）を考えることができる。この超球の接空間をＥｘｐｏｎｅｎｔｉａｌ  Ｍａｐと呼ぶ。Ｅｘｐｏｎｅｎｔｉａｌ  Ｍａｐは４次元空間上の超球に接する３次元空間であり、線形空間である。
【００９５】
クォータニオンｑをｑ．ｗ＋ｑ．ｘ・ｉ＋ｑ．ｙ・ｊ＋ｑ．ｚ・ｋで表すと、クォータニオンｑからＥｘｐｏｎｅｎｔｉａｌ  Ｍａｐ（ｅｘｐｍａｐ）への変換は、例えば次の式（６）及び式（７）で表すことができる。ここで、ａｃｏｓ（）は逆余弦関数を表し、ｓｉｎ（）は正弦関数を表す。
θ０＝ａｃｏｓ（ｑ．ｗ）  …（６）
ｅｘｐｍａｐ＝θ０／ｓｉｎ（θ０）・［ｑ．ｘ，ｑ．ｙ，ｑ．ｚ］  …（７）
【００９６】
姿勢変換行列群ＰＡ’に含まれているｎ個の姿勢変換行列の各回転行列部分をクォータニオンｑに変換したものから式（６）及び式（７）により、ｎ個のｅｘｐｍａｐを算出することができる。ここで算出したｅｘｐｍａｐは、ｘ，ｙ，ｚの３要素を持つので、この３要素それぞれで中央値を取り、ｅｘｐｍａｐ’とする。すると、以下の式（８）、式（９）及び式（１０）により、クォータニオンｑ’（＝ｑ’．ｗ＋ｑ’．ｘ・ｉ＋ｑ’．ｙ・ｊ＋ｑ’．ｚ・ｋ）に戻すことができる。ここでｎｏｒｍ（）はユークリッドノルムを返す関数を表し、ｃｏｓ（）は余弦関数を表す。
θ１＝ｎｏｒｍ（ｅｘｐｍａｐ’）  …（８）
ｑ’．ｗ＝ｃｏｓ（θ１）  …（９）
［ｑ’．ｘ，ｑ’．ｙ，ｑ’．ｚ］＝ｓｉｎ（θ１）・ｅｘｐｍａｐ’／θ１  …（１０）
【００９７】
そして、クォータニオンｑ’を回転行列に変換し、中央値を算出済みの位置行列と合わせて姿勢変換行列Ｐ０とする。なお、上述の例では、姿勢変換行列Ｐ０を算出する際に、ｎ個の姿勢変換行列の中央値を用いたが、中央値を用いるのは一例に過ぎず、例えば平均値を用いて姿勢変換行列Ｐ０を算出してもよい。あるいは、ＲＡＮＳＡＣ（ＲＡＮｄｏｍ  ＳＡｍｐｌｅ  Ｃｏｎｓｅｎｓｕｓ）を用いて、サポートするキーフレームが多いもの（姿勢が近いもの）を選択したり、サポートするＭａｐＰｏｉｎｔが多いもの（位置が近いもの）を選択したりしてもよい。
【００９８】
上記の「サポートするキーフレームが多いもの（姿勢が近いもの）を選択」とは、「姿勢変換行列群ＰＡ’に含まれるｎ個の姿勢変換行列のうち、「対象地図」のキーフレームの３Ｄ姿勢を変換した時に、「基準地図」のキーフレームの３Ｄ姿勢と一致する（又は所定の基準以上類似する）ようなキーフレームの数が所定の閾値よりも多くなる姿勢変換行列を選択」という意味である。ここで複数の姿勢変換行列が選択される場合は、選択された姿勢変換行列の中央値や平均値を取ってもよいし、上述のような（一致又は所定の基準以上類似する）キーフレームの数が最も多い姿勢変換行列を選択してもよい。
【００９９】
また、上記の「サポートするＭａｐＰｏｉｎｔが多いもの（位置が近いもの）を選択」とは、「姿勢変換行列群ＰＡ’に含まれるｎ個の姿勢変換行列のうち、「対象地図」のＭａｐＰｏｉｎｔの３Ｄ座標を変換した時に、「基準地図」のＭａｐＰｏｉｎｔの３Ｄ座標と一致する（又は２点間の距離が所定の基準距離以下になる）ようなＭａｐＰｏｉｎｔの数が所定の閾値よりも多くなる姿勢変換行列を選択」という意味である。ここで複数の姿勢変換行列が選択される場合は、選択された姿勢変換行列の中央値や平均値を取ってもよいし、上述のような（２点間の距離が所定の基準距離以下になる）ＭａｐＰｏｉｎｔの数が最も多い姿勢変換行列を選択してもよい。
【０１００】
そして、制御部１０は、ｎ（姿勢変換行列群ＰＡ’に含まれている姿勢変換行列の数）が所定の閾値Ｎ１（例えば、「対象地図」に含まれるキーフレームの数の３０％程度の値に設定する）より大きいか否かを判定する（ステップＳ７３２）。ｎがＮ１よりも大きければ（ステップＳ７３２；Ｙｅｓ）、姿勢変換行列Ｐ０に含まれる誤差は小さいと判断し、姿勢変換行列Ｐ０を姿勢変換行列Ｐとして（ステップＳ７３３）、誤差修正処理を終了し、環境地図正規化処理（図１２）のステップＳ７１３に進む。
【０１０１】
ｎがＮ１以下なら（ステップＳ７３２；Ｎｏ）、姿勢変換行列Ｐ０の誤差を小さくするためにＭａｐＰｏｉｎｔも利用する。このため、制御部１０は、「対象地図」に含まれる各ＭａｐＰｏｉｎｔの３Ｄ座標を姿勢変換行列Ｐ０及びスケールＳで「基準地図」の座標に変換した点群をＭ１とする（ステップＳ７３４）。具体的には、「対象地図」のＭａｐＰｏｉｎｔ群（の各３Ｄ座標）をＭ０で表すとすると、姿勢変換行列Ｐ０及びスケールＳとから、以下の式（１１）により、Ｍ１を算出する。Ｍ０は複数のＭａｐＰｏｉｎｔの３Ｄ座標を含むので、Ｍ１はＭ０と同数の点の３Ｄ座標を含む点群となる。
Ｍ１＝Ｐ０・Ｓ・Ｍ０  …（１１）
【０１０２】
そして、「基準地図」のＭａｐＰｏｉｎｔ群（の各３Ｄ座標）をＭＢで表すことにすると、制御部１０は、点群Ｍ１から点群ＭＢへの姿勢変化を示す姿勢変換行列Ｐ１をＩＣＰ（Ｉｔｅｒａｔｉｖｅ  Ｃｌｏｓｅｓｔ  Ｐｏｉｎｔ）処理を用いて算出する（ステップＳ７３５）。ＩＣＰ処理の概要については後述する。
【０１０３】
そして、制御部１０は、姿勢変換行列Ｐ０と姿勢変換行列Ｐ１とから、式（１２）により、姿勢変換行列Ｐを算出し（ステップＳ７３６）、誤差修正処理を終了し、環境地図正規化処理（図１２）のステップＳ７１３に進む。
Ｐ＝Ｐ１・Ｐ０  …（１２）
【０１０４】
次に、誤差修正処理（図１３）のステップＳ７３５で実行されるＩＣＰ処理について、図１４を参照して説明する。ＩＣＰ処理は、引数として２つの点群を取るので、それぞれ点群Ｔ０及び点群Ｔ１とする。誤差修正処理（図１３）のステップＳ７３５でＩＣＰ処理が呼び出されると、点群Ｔ０には点群Ｍ１が、点群Ｔ１には点群ＭＢが、それぞれ代入されて処理が開始される。なお、一般にＩＣＰ処理は初期値依存性が高いという問題があるが、上述のように、「対象地図」のＭａｐＰｏｉｎｔ群を姿勢変換行列Ｐ０で変換した点群Ｍ１を初期値として与えるようにしたため、初期値に含まれる誤差が小さくなり、初期値依存問題を回避することができる。
【０１０５】
まず、制御部１０は、作業用変数Ｌに最大ループ数（例えば１０）を、作業用変数ｃｔに０を、それぞれ代入し、求めるべき点群Ｔ０から点群Ｔ１への姿勢変換行列Ｐ０１の初期値として、回転行列部分が単位行列で位置行列部分が０ベクトルの同次座標形式の行列を代入する（ステップＳ７５１）。
【０１０６】
そして、制御部１０は、点群Ｔ０の各点にて、最短距離となる点を点群Ｔ１から求めて対応を取得する（ステップＳ７５２）。次に、制御部１０は、点群Ｔ０と点群Ｔ１との対応に基づき、点群Ｔ０から点群Ｔ１への姿勢変換行列を算出し、姿勢変換行列Ｐｔｍｐとする（ステップＳ７５３）。
【０１０７】
そして、制御部１０は、姿勢変換行列Ｐ０１及び点群Ｔ０を、それぞれ姿勢変換行列Ｐｔｍｐで変換したものに更新し、作業用変数ｃｔに１を加算する（ステップＳ７５４）。そして、制御部１０は、ステップＳ７５４における姿勢変換行列Ｐ０１の更新量（姿勢変換行列Ｐｔｍｐで姿勢変換行列Ｐ０１を変換する前後での差分）が所定の既定値以下か否かを判定する（ステップＳ７５５）。
【０１０８】
姿勢変換行列Ｐ０１の更新量が既定値以下なら（ステップＳ７５５；Ｙｅｓ）、姿勢変換行列Ｐ０１が収束したと判断し、制御部１０は、ＩＣＰ処理の戻り値としての姿勢変換行列Ｐ１に姿勢変換行列Ｐ０１を代入して（ステップＳ７５６）、ＩＣＰ処理を終了する。
【０１０９】
姿勢変換行列Ｐ０１の更新量が既定値を超えるなら（ステップＳ７５５；Ｎｏ）、制御部１０は、作業用変数ｃｔが最大ループ数Ｌ未満か否かを判定する（ステップＳ７５７）。そして、作業用変数ｃｔが最大ループ数Ｌ未満なら（ステップＳ７５７；Ｙｅｓ）、ステップＳ７５２に戻って、姿勢変換行列Ｐ０１の更新を繰り返す。
【０１１０】
作業用変数ｃｔが最大ループ数Ｌ以上なら（ステップＳ７５７；Ｎｏ）、これ以上更新を繰り返しても十分に収束しないと判断し、制御部１０は、ＩＣＰ処理の戻り値としての姿勢変換行列Ｐ１に姿勢変換行列Ｐ０１を代入して（ステップＳ７５６）、ＩＣＰ処理を終了する。
【０１１１】
以上説明した姿勢変換行列の誤差修正処理及びＩＣＰ処理により、姿勢変換行列Ｐの誤差を小さくすることができ、環境地図正規化処理は誤差の小さな姿勢変換行列Ｐを用いて「対象地図」を「基準地図」で正規化することができる。
【０１１２】
そして、上述の環境地図正規化処理により、地図保存部２２には所定時間毎に環境情報が付加された環境地図のデータが、「基準地図」で正規化されてから保存されていく。したがって、自律移動装置１００は、複数の環境地図を統一的に扱うことができるので、環境の変化に対してロバストな自己位置推定を行うことができる。
【０１１３】
なお、上述の姿勢変換行列の誤差修正処理（図１３）では、ステップＳ７３２で制御部１０は、ｎが所定の閾値Ｎ１より大きいか否かを判定し、ｎがＮ１以下ならＭａｐＰｏｉｎｔも利用して姿勢変換行列Ｐ０の誤差を小さくする処理を行っていたが、この処理を行わなくてもよい。つまり、ｎ個の姿勢変換行列を含む姿勢変換行列群ＰＡ’から、中央値等を求めることによって求めた姿勢変換行列Ｐ０を最終的な姿勢変換行列Ｐとしてもよい。
【０１１４】
（実施形態１の変形例１）
実施形態１では、環境地図保存処理（図１１）のステップＳ６０６において「基準地図」（正規化の基準に用いる環境地図）を選択する際に、最初に地図保存部２２に保存された環境地図を「基準地図」として選択する例を挙げた。しかし、「基準地図」の選択方法はこれに限られない。別の方法として、例えば、地図保存部２２に保存されている環境地図の中で、規模が大きな環境地図を「基準地図」として選択する方法も考えられる。ここで、規模が大きいとは、環境地図に含まれるキーフレームの数やＭａｐＰｏｉｎｔの数が多いことを意味する。このように規模の大きな環境地図を「基準地図」として選択する実施形態１の変形例１について説明する。
【０１１５】
実施形態１では、最初に地図保存部２２に保存された環境地図を「基準地図」として用いているので、新たに作成された環境地図も全て最初に保存された環境地図（「基準地図」）と座標軸、原点及びスケールが合わせられて保存されることになる。つまり「基準地図」は１つに定まっており、変化しない。これに対し、実施形態１の変形例１では、過去に保存された環境地図よりも規模の大きい環境地図が地図保存部２２に記憶されると、その後の環境地図保存処理で選択される「基準地図」は、それより前の「基準地図」とは異なる環境地図になる。つまり、「基準地図」が変化する可能性がある。
【０１１６】
したがって、実施形態１の変形例１の環境地図保存処理は、図１５に示すように、現在の「基準地図」の地図ＩＤを示す変数ＫＭＡＰを導入し、「基準地図」が変化したら、地図保存部２２に保存されている他の環境地図を新たな「基準地図」で正規化し直す処理を行う。なお、変数ＫＭＡＰを使用する事前準備として、起動時処理（図６）のステップＳ１０２の最初に変数ＫＭＡＰを初期化（存在しない地図ＩＤとして、例えば０をセット）する処理を行う。
【０１１７】
実施形態１の変形例１の環境地図保存処理（図１５）は、実施形態１の環境地図保存処理（図１１）に、ステップＳ６２１からステップＳ６２４までの処理が追加された内容になっているので、追加された各ステップについて説明する。
【０１１８】
ステップＳ６２１では、制御部１０は、地図保存部２２に保存されている環境地図の中から規模の大きな環境地図を「基準地図」として選択する。例えば、環境地図に含まれているキーフレームの数とＭａｐＰｏｉｎｔの数の和が最大の環境地図を「基準地図」として選択する。ここで単純な和の最大値により「基準地図」を選択するのではなく、キーフレームの数とＭａｐＰｏｉｎｔの数それぞれに重みを掛けてから和を取ったものが最大となる環境地図を「基準地図」として選択してもよい。
【０１１９】
ステップＳ６２２では、制御部１０は、変数ＫＭＡＰに格納されている値が、ステップＳ６２１で選択した「基準地図」の地図ＩＤと同じであるか否かを判定する。同じなら（ステップＳ６２２；Ｙｅｓ）、ステップＳ６０９に進む。この場合は、地図保存部２２に保存されている環境地図は既に「基準地図」で正規化されているからである。
【０１２０】
変数ＫＭＡＰに格納されている値が、「基準地図」の地図ＩＤと同じでないなら（ステップＳ６２２；Ｎｏ）、制御部１０は、地図保存部２２に保存されている環境地図（「基準地図」以外の全ての環境地図）を、それぞれ「基準地図」で正規化して地図保存部２２に保存し直す（ステップＳ６２３）。そして、制御部１０は、変数ＫＭＡＰに「基準地図」の地図ＩＤをセットし（ステップＳ６２４）、ステップＳ６０９に進む。
【０１２１】
以上説明した実施形態１の変形例１の環境地図保存処理では、規模の大きな環境地図が「基準地図」として選択されるので、「基準地図」には様々なキーフレームやＭａｐＰｏｉｎｔが含まれる可能性が高くなる。そうすると、例えば照明の影響を受けにくい場所（机の下等）のキーフレームやＭａｐＰｏｉｎｔが多く含まれる可能性が高くなり、環境地図の正規化に成功しやすくなることが期待できる。したがって、より多くの環境地図を正規化することができるので、環境の変化に対してよりロバストな自己位置推定が可能になる。
【０１２２】
（実施形態１の変形例２）
上述の実施形態１では、Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理が所定時間以上失敗し続けたらＳＬＡＭ処理の初期化（及び地図記憶部２１のクリア）を行っていた。しかし、ＳＬＡＭ処理の初期化（及び地図記憶部２１のクリア）のタイミングは、このタイミングに限定されるわけではない。例えば、ＳＬＡＭ処理の初期化（及び地図記憶部２１のクリア）を、環境地図保存処理（図１１）で地図記憶部２１に記憶されている環境地図を地図保存部に保存した後に行ってもよい。このような実施形態１の変形例２について説明する。
【０１２３】
実施形態１の変形例２では、環境地図保存処理（図１１）のステップＳ６０９の処理の後に、ステップＳ６１０に進み、ステップＳ６１０の処理の後に、環境地図保存処理を終了する。
【０１２４】
上述の処理を行うことにより、実施形態１の変形例２は、環境地図を地図保存部２２に保存する度に、初期状態から新しい環境地図を作成し直すので、直近の環境情報を反映した環境地図を地図保存部２２に保存することができる。したがって、地図保存部２２には、様々な環境情報に対する環境地図が保存されることになり、環境地図を選択し直す際に、現在の環境により適合した環境地図を選択できる可能性を高くすることができる。
【０１２５】
（実施形態１の変形例３）
さらに、ＳＬＡＭ処理の初期化（及び地図記憶部２１のクリア）のタイミングとしては、上記以外のタイミングも考えられる。例えば、自己位置推定スレッド（図７）で、現環境地図でのＲｅｌｏｃａｌｉｚａｔｉｏｎ処理（ステップＳ２１８）が一定時間経過しても成功しない場合にＳＬＡＭ処理の初期化（及び地図記憶部２１のクリア）を行ってもよい。このような実施形態１の変形例３について説明する。
【０１２６】
実施形態１の変形例３では、Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理の失敗をカウントする変数ＲＦＣ２を導入し、自己位置推定スレッド（図７）の現環境地図でのＲｅｌｏｃａｌｉｚａｔｉｏｎ処理（ステップＳ２１８）が成功すれば（ステップＳ２１９；Ｙｅｓ）、変数ＲＦＣ２を０にし、失敗すれば（ステップＳ２１９；Ｎｏ）、変数ＲＦＣ２に１を加算する。そして、変数ＲＦＣ２が所定の値（例えば５）を超えたら、ＳＬＡＭ処理の初期化（及び地図記憶部２１のクリア）を行ってステップＳ２０２から処理をやり直すようにする。あるいは、変数ＲＦＣ２が１になった時の時刻を変数ＲＦＴ２に記憶し、ＲＦＴ２から所定の時間（例えば５分）経過しても現環境地図でのＲｅｌｏｃａｌｉｚａｔｉｏｎ処理が失敗し続けているなら、ＳＬＡＭ処理の初期化（及び地図記憶部２１のクリア）を行ってステップＳ２０２から処理をやり直すようにする。
【０１２７】
上述の処理を行うことにより、実施形態１の変形例３は、現環境地図でのＲｅｌｏｃａｌｉｚａｔｉｏｎ処理がいつまでたっても成功しない状況に陥るのを防ぐことができる。
【０１２８】
（実施形態１の変形例４）
また、ＳＬＡＭ処理の初期化のタイミングとして、例えば、自己位置推定スレッド（図７）の開始時に地図保存部２２に記憶されている環境地図の数が所定の環境地図基準数以下の場合に、ＳＬＡＭ処理の初期化（及び地図記憶部２１のクリア）を行ってもよい。このような実施形態１の変形例４について説明する。
【０１２９】
実施形態１の変形例４では、自己位置推定スレッド（図７）のステップＳ２０１において、地図保存部２２に環境地図が保存されているか否かを判定するのではなく、環境地図が環境地図基準数（例えば５つ）以上保存されているか否かを判定する。そして、環境地図基準数未満しか保存されていなければ（ステップＳ２０１；Ｎｏ）、初期状態からＳＬＡＭを開始する。また、Ｒｅｌｏｃａｌｉｚａｔｉｏｎ（ステップＳ２０４）に失敗した場合に、ステップＳ２０４ではなく、ステップＳ２０３に戻って、複数の環境地図の中から現在の環境に適合する可能性の高い環境地図を抽出するようにしてもよい。さらに、現環境地図でのＲｅｌｏｃａｌｉｚａｔｉｏｎ（ステップＳ２１８）に何度も失敗した場合に、ステップＳ２０３に戻って、複数の環境地図の中から現在の環境に適合する可能性の高い環境地図を抽出するようにしてもよい。また、ステップＳ２０４やステップＳ２１８でＲｅｌｏｃａｌｉｚａｔｉｏｎに失敗した場合には、ステップＳ２０２に戻って、ＳＬＡＭ処理の初期化（及び地図記憶部２１のクリア）を行い、新たに環境地図を作成開始するところから処理を行ってもよい。
【０１３０】
上述の処理を行うことにより、実施形態１の変形例４は、ある程度の数、環境地図が地図保存部２２に保存されるまでは、異なる環境地図を作成するので、地図保存部２２に様々な環境の元での環境地図が保存される可能性が高まる。そして、Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理が失敗した場合に環境地図抽出（ステップＳ２０３）から処理をやり直すようにすると、現在の環境に適合する環境地図を抽出できる可能性が高まり、その結果、環境の変化に対してロバストな自己位置推定を行うことができる。
【０１３１】
（実施形態１の変形例５）
上述の実施形態では、環境地図保存処理（図１１）の中の環境地図正規化処理に失敗した場合（ステップＳ６０８；Ｎｏ）、地図記憶部２１に記憶されている環境地図を破棄し、以後使用しないようにしていた。しかし、例えば「基準地図」に異常値が多く含まれている場合には、環境地図正規化処理がほとんど失敗してしまうことも考えられる。そこで、正規化の失敗が続いた場合には、「基準地図」に異常があると判断し、制御部１０は、地図保存部２２から「基準地図」を削除してもよい。このように環境地図の正規化に成功しない場合に地図保存部２２から「基準地図」を削除する実施形態１の変形例５について説明する。
【０１３２】
実施形態１の変形例５においては、環境地図保存処理（図１１）で正規化が連続して失敗する回数をカウントするための変数ＦＣを導入する。そして、正規化に成功したら（ステップＳ６０８；Ｙｅｓ）、制御部１０は、変数ＦＣを０にしてステップＳ６０９に進む。一方、正規化に失敗したら（ステップＳ６０８；Ｎｏ）、制御部１０は、変数ＦＣに１を加算し、ＦＣの値が所定の値（例えば５）を超えたら、「基準地図」を地図保存部２２から削除する。そして、地図保存部２２に保存されている残りの環境地図の中から１つ（例えば、最も規模の大きい環境地図を）選択して「基準地図」とし、地図保存部２２に保存されている他の環境地図を正規化し直す。そして、新たな「基準地図」で地図記憶部２１に記憶されている環境地図の正規化を試みる。新たな「基準地図」で地図記憶部２１に記憶されている環境地図の正規化に成功したら、変数ＦＣを０にし、ステップＳ６０９に進む。
【０１３３】
新たな「基準地図」でもまた正規化に失敗した場合、「基準地図」ではなく、地図記憶部２１に記憶されている環境地図に問題がある可能性もあるため、変数ＦＣに１をセットして、ステップＳ６１０に進む。そして、その後も、環境地図の正規化に失敗し続け、ＦＣの値が所定の値（例えば５）を超えたら、「基準地図」を地図保存部２２から削除し、上述の処理を繰り返す。
【０１３４】
以上の処理を行うことにより、実施形態１の変形例５は、「基準地図」に問題がある場合に、いつまでも環境地図の正規化に成功しない状況に陥るのを防ぐことができる。
【０１３５】
（実施形態２）
実施形態１では、地図保存部２２に保存されている任意の環境地図を正規化の基準とする環境地図（「基準地図」）にすることができたが、正規化をする際の姿勢変換行列Ｐを算出するのに計算コストがかかった。そこで、各環境地図に正規化の際の基準となる姿勢（基準姿勢）を導入することにより、計算コストを抑えて正規化を行うことができる実施形態２について説明する。
【０１３６】
実施形態２に係る自律移動装置１０１は、実施形態１に係る自律移動装置１００と同一の外観及び構成を持つ。ただし、地図記憶部２１に記憶される環境地図は、図１６に示すように、実施形態１の環境地図に基準姿勢情報が追加されたデータ構造になっている。基準姿勢情報は、基準姿勢と変換済み行列とで構成される。基準姿勢とは、環境地図を正規化する際に基準とする姿勢であり、通常所定の場所（例えば充電器２００の設置場所）での自律移動装置１０１の姿勢が基準姿勢として登録される。また、変換済み行列とは、環境地図を正規化する際に用いた姿勢変換行列である。
【０１３７】
そして、基準姿勢と変換済み行列はいずれも、３×３の回転行列と３×１の位置行列とを含む、４×４の同次座標形式の姿勢行列で表される。環境地図生成開始時（初期化時）は、基準姿勢は要素が全て０の行列（０行列）に初期化され、変換済み行列は回転行列部分が単位行列、位置行列部分が０行列となるように初期化される。
【０１３８】
また、自律移動装置１０１は、駆動部３２をメカオドメトリとして機能させることができ、制御部１０は、メカオドメトリにより移動距離を取得できる。そして、地図作成部１２は、メカオドメトリを用いて、環境地図に含まれるキーフレームの３Ｄ姿勢、ＭａｐＰｏｉｎｔの３Ｄ座標等の単位（スケール）をメートル系で表すように環境地図を作成する。つまり、環境地図上での単位長さ１が、対応する実際の３Ｄ空間での１メートルになるように環境地図の長さの単位（スケール）を定める。このため、自己位置推定部１４が推定する自律移動装置１０１の姿勢の単位（スケール）もメートル系で表されることになる。
【０１３９】
自律移動装置１０１で起動される各種処理のうち、環境地図保存処理及び環境地図正規化処理は、自律移動装置１００で起動される当該処理と異なる部分があるため、これらの処理について説明する。まず、環境地図保存処理について、図１７を参照して説明する。ただし、自律移動装置１０１の環境地図保存処理は、自律移動装置１００の環境地図保存処理（図１１）と共通する部分が多いので、異なる部分を中心に説明する。
【０１４０】
自律移動装置１０１の環境地図保存処理では、ステップＳ６０４の次に、制御部１０は、地図記憶部２１に記憶されている環境地図に基準姿勢が登録されているか否かを判定する（ステップＳ６３１）。基準姿勢が０行列であれば、基準姿勢は登録されていないので（ステップＳ６３１；Ｎｏ）、制御部１０は、自律移動装置１０１を充電器２００の設置場所に戻す制御を行う（ステップＳ６３２）。
【０１４１】
そして、充電器２００の設置場所に戻ったら、制御部１０は、充電器２００の位置での自律移動装置１０１の姿勢を基準姿勢として地図記憶部２１に書き込む（ステップＳ６３３）。なお、充電器２００の位置での自律移動装置１０１の姿勢は、ＳＬＡＭ処理で算出してもよいし、メカオドメトリによって求めてもよい。
【０１４２】
その後の処理は、自律移動装置１００の環境地図保存処理（図１１）とほぼ同じである。ただし、後述するように自律移動装置１０１の環境地図正規化処理（図１８）は、正規化に失敗することがない（常に成功する）ため、自律移動装置１００の環境地図保存処理（図１１）のステップＳ６０８及びステップＳ６１０の処理は不要である。
【０１４３】
次に、自律移動装置１０１の環境地図正規化処理について、図１８を参照して説明する。この環境地図正規化処理は、実施形態１と同様に、正規化を行う環境地図及び正規化の基準にする環境地図の２つの引数を取るので、正規化を行う環境地図を「対象地図」、正規化の基準にする環境地図を「基準地図」として、以下説明する。
【０１４４】
まず、制御部１０は、「基準地図」の基準姿勢を、基準姿勢を格納する変数ＳＢに代入する（ステップＳ７７１）。次に、制御部１０は、「対象地図」の基準姿勢を、基準姿勢を格納する変数ＳＸに代入する（ステップＳ７７２）。次に、制御部１０は、「対象地図」の変換済み行列を、変換済み行列を格納する変数Ｐ０に代入する（ステップＳ７７３）。
【０１４５】
そして、制御部１０は、「基準地図」の基準姿勢ＳＢ、「対象地図」の基準姿勢ＳＸ及び「対象地図」の変換済み行列Ｐ０に基づき、「対象地図」から「基準地図」への姿勢変換行列Ｐを算出する（ステップＳ７７４）。具体的には以下の式（１３）により姿勢変換行列Ｐを求める。ここで、ｉｎｖ（）は逆行列を求める関数である。
Ｐ＝ＳＢ・ｉｎｖ（ＳＸ）・ｉｎｖ（Ｐ０）  …（１３）
【０１４６】
次に、制御部１０は、姿勢変換行列Ｐにより「対象地図」に含まれるキーフレーム情報群及びＭａｐＰｏｉｎｔ情報群を変換する（ステップＳ７７５）。具体的には、「対象地図」に含まれる各キーフレーム情報の３Ｄ姿勢ＰＸ０を以下の式（１４）で正規化した３Ｄ姿勢ＰＳに変換し、各ＭａｐＰｏｉｎｔ情報の３Ｄ座標ＭＸ０を以下の式（１５）で正規化した３Ｄ座標ＭＳに変換する。
ＰＳ＝Ｐ・ＰＸ０  …（１４）
ＭＳ＝Ｐ・ＭＸ０  …（１５）
【０１４７】
そして、制御部１０は、「対象地図」の変換済み行列として姿勢変換行列Ｐを地図記憶部２１に書き込み（ステップＳ７７６）、環境地図正規化処理を終了する。
【０１４８】
上述の処理は、自律移動装置１０１は、メカオドメトリにより全ての環境地図のスケールが合っているものとして、スケールＳの計算を省いた処理になっている。しかし、スケールが環境地図によって変動する可能性がある場合には、実施形態１と同様の処理により、スケールＳを計算で求めてもよい。ただしその場合、上述の式（１４）や式（１５）での計算時に、さらにスケールＳを掛ける必要がある。
【０１４９】
また、上述の処理では、充電器２００の位置での自律移動装置１０１の姿勢を基準姿勢としたが、これは基準姿勢の一例に過ぎない。例えば、テレビの正面でテレビから所定の距離離れた位置での自律移動装置１０１の姿勢を基準姿勢としてもよい。この場合、撮像部３３で撮影した画像から、一般の画像認識でテレビの正面を認識する。そして、例えば、部屋の中を任意に移動している時に、撮影した画像の中の特定の位置に特定の大きさのテレビが認識された時の自律移動装置１０１の姿勢を基準姿勢とすることができる。
【０１５０】
以上説明した実施形態２に係る自律移動装置１０１は、基準姿勢を利用して環境地図の正規化を行う（座標系を合わせる）ため、環境地図を保存する前に基準姿勢を取得する必要がある。しかし、実施形態２では、基準姿勢を用いて行列の演算をするだけで正規化のための姿勢変換行列を算出できるため、正規化に失敗するおそれはなく、計算コストを抑えて確実に正規化できるという特長がある。
【０１５１】
なお、上述した実施形態では、環境地図保存処理は、地図記憶部２１に記憶されている地図を、所定の時間（例えば１時間）毎に、地図保存部２２に保存していたが、この「所定の時間毎に」という判定は一例に過ぎない。例えば、「周囲の明るさが所定の量以上変化したら」というような判定を採用してもよい。この場合、環境地図保存処理（図１１，１５，１７）のステップＳ６０１での判定は、「前回地図保存時の環境情報から、所定の量以上、環境情報が変化したか否かの判定」となる。
【０１５２】
また、上述した実施形態では、点灯している電灯数を環境情報としたが、これは環境情報の一例に過ぎない。例えば、環境情報として、「電灯の点灯の有無」と「周囲の明るさ」とからなる２次元ベクトル値を採用してもよい。この場合、環境情報の２次元ベクトルの最初の値（天井の電灯のＯＮ又はＯＦＦ）は、電灯がＯＮなら１、ＯＦＦなら０の二値とし、環境情報の２次元ベクトルの２番目の値（周囲の明るさ）は、撮像部３３が撮影した画像に含まれる全ての画素値の平均値又は中央値をとすればよい。
【０１５３】
そして、環境地図抽出処理（図８）では、画像撮影して得た環境情報の２次元ベクトルと、地図保存部２２に保存されている環境地図それぞれに付加された環境情報の２次元ベクトルと、の間で類似度を求め、類似度の高い順にＮ個の候補環境地図を抽出すればよい。なお、２次元ベクトル間の類似度は、例えば、それぞれのベクトルのノルムを１に正規化してから内積を取ることにより得ることができる。
【０１５４】
また、環境情報として、さらに多くの情報を用いる場合も、環境情報を表すベクトルの次元数を増やすことにより対応できる。そして、ベクトルの次元数が増えても、環境情報の類似度は、類似度計算の対象となる２つのベクトルのノルムを１に正規化してから内積を取ることにより算出することができる。なお、上述の例では、環境情報をベクトルで表す例を示したが、環境情報はベクトルで表さなければならないわけではなく、他のデータ構造を採用してもよい。
【０１５５】
なお、上述の実施形態では、環境情報として照明変化（照明変動）の影響について説明したが、照明変動とは、照明方向、照明位置が変わることを特に指す。具体的には、電灯のＯＮ／ＯＦＦ、窓から入ってくる日差しが太陽の位置によって変わること、ブラインドの開け／閉め等である。
【０１５６】
照明変動が生じると撮影した画像の特徴量が変化し、自律移動装置１００，１０１の位置推定に影響を及ぼすと考えられるため、上述の実施形態では環境情報として、照明の変化を例にとって説明した。しかし、環境情報はこれに限られない。環境情報には、特徴点となる構造物の位置変化等を含めることもできる。例えば、倉庫で定期的に物の位置や量が大きく変わるような場合は、物の位置や量を環境情報に含めてもよい。
【０１５７】
この場合、制御部１０は、物の位置や量の情報を、撮像部３３で撮影した画像から、一般の画像認識によって取得してもよい。また、制御部１０は、倉庫の在庫管理を行っている外部の在庫管理システム等と通信部３４を介して通信して、該在庫管理システム等から物の位置や量の情報を取得してもよい。物の量を環境情報に含めれば、倉庫に一杯に物があるときと、倉庫に物が少ないときとで、環境情報が異なるようになり、制御部１０は、それぞれ別の環境地図を作成することになるので、物の量の変化に対してロバストな自己位置推定を行うことができる。
【０１５８】
また、上述の実施形態及び変形例は適宜組み合わせることができる。例えば、実施形態１の変形例２、変形例３及び変形例４を組み合わせることにより、地図保存部２２に様々な環境の元での環境地図が正規化されて保存され、Ｒｅｌｏｃａｌｉｚａｔｉｏｎ処理に失敗した際には、当該様々な環境地図の中から現在の環境に適合し、しかも正規化された環境地図を抽出することができるので、環境の変化に対してよりロバストな自己位置推定を行うことができる。
【０１５９】
なお、自律移動装置１００，１０１の各機能は、通常のＰＣ（Ｐｅｒｓｏｎａｌ  Ｃｏｍｐｕｔｅｒ）等のコンピュータによっても実施することができる。具体的には、上記実施形態では、自律移動装置１００，１０１が行う自律移動制御処理のプログラムが、記憶部２０のＲＯＭに予め記憶されているものとして説明した。しかし、プログラムを、フレキシブルディスク、ＣＤ−ＲＯＭ（Ｃｏｍｐａｃｔ  Ｄｉｓｃ  Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、ＤＶＤ（Ｄｉｇｉｔａｌ  Ｖｅｒｓａｔｉｌｅ  Ｄｉｓｃ）、ＭＯ（Ｍａｇｎｅｔｏ−Ｏｐｔｉｃａｌ  Ｄｉｓｃ）、メモリカード、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ  Ｓｅｒｉａｌ  Ｂｕｓ）メモリ等のコンピュータ読み取り可能な記録媒体に格納して配布し、そのプログラムをコンピュータに読み込んでインストールすることにより、上述の各機能を実現することができるコンピュータを構成してもよい。
【０１６０】
以上、本発明の好ましい実施形態について説明したが、本発明は係る特定の実施形態に限定されるものではなく、本発明には、特許請求の範囲に記載された発明とその均等の範囲が含まれる。以下に、本願出願の当初の特許請求の範囲に記載された発明を付記する。
【０１６１】
（付記１）
撮像部で撮影された画像を用いて環境地図の作成及び位置の推定を行う自律移動装置であって、
制御部と記憶部とを備え、
前記制御部は、
周囲の環境の変化に応じて複数の環境地図を作成し、
前記作成した複数の環境地図を正規化して統一的に扱えるように前記記憶部に保存し、
前記正規化された環境地図を用いて位置を推定する、
自律移動装置。
【０１６２】
（付記２）
前記制御部は、
前記複数の環境地図の中から前記正規化の基準に用いる環境地図である基準地図を選択し、
前記複数の環境地図のうち、前記基準地図で正規化する対象の環境地図である対象地図を前記基準地図に基づいて正規化する、
付記１に記載の自律移動装置。
【０１６３】
（付記３）
前記環境地図は、前記撮像部で撮影された画像のうち位置の推定に用いる画像であるキーフレームの情報と、前記撮像部で撮影された画像に含まれる特徴的な点である特徴点のうち３次元位置の座標が推定できた特徴点であるＭａｐＰｏｉｎｔの情報と、を含み、さらに、前記キーフレームの情報として、前記キーフレームが撮影された時の前記自律移動装置の姿勢の情報と、前記キーフレームに含まれる前記特徴点の情報と、を含み、
前記制御部は、前記対象地図に含まれる前記姿勢の情報及び前記ＭａｐＰｏｉｎｔの情報を、前記基準地図の座標系における値に変換することにより、前記対象地図を正規化する、
付記２に記載の自律移動装置。
【０１６４】
（付記４）
前記制御部は、
前記対象地図に含まれる前記キーフレームと、前記キーフレームに類似する前記基準地図に含まれるキーフレームと、の対応により推定した、前記キーフレームが撮影された時の前記自律移動装置の姿勢に基づいて、前記対象地図から前記基準地図への姿勢変換行列を算出し、
前記姿勢変換行列により、前記環境地図を正規化する、
付記３に記載の自律移動装置。
【０１６５】
（付記５）
前記制御部は、
前記対象地図に含まれる複数の前記キーフレームそれぞれを用いて前記姿勢変換行列を算出することにより、複数の姿勢変換行列からなる姿勢変換行列群を算出し、
前記姿勢変換行列群に含まれる複数の姿勢変換行列を用いて誤差を少なくした１つの姿勢変換行列を算出する、
付記４に記載の自律移動装置。
【０１６６】
（付記６）
前記制御部は、前記姿勢変換行列群に含まれる複数の姿勢変換行列の中央値を算出することにより前記１つの姿勢変換行列を算出する、
付記５に記載の自律移動装置。
【０１６７】
（付記７）
前記制御部は、前記複数の姿勢変換行列の中央値を算出する際に、前記複数の姿勢変換行列のそれぞれから取り出した回転行列をクォータニオンに変換し、前記クォータニオンを線形空間に射影して線形空間内で中央値を算出する、
付記６に記載の自律移動装置。
【０１６８】
（付記８）
前記制御部は、前記姿勢変換行列群に含まれる姿勢変換行列の数が所定の閾値以下ならば、前記環境地図に含まれる前記ＭａｐＰｏｉｎｔを用いて、前記１つの姿勢変換行列の誤差を少なくする、
付記５から７のいずれか１つに記載の自律移動装置。
【０１６９】
（付記９）
前記制御部は、
前記対象地図から前記基準地図への姿勢変換行列を算出できない場合は、前記対象地図を削除して、新たな環境地図の作成を開始する、
付記４から８のいずれか１つに記載の自律移動装置。
【０１７０】
（付記１０）
前記環境地図は、さらに、正規化の際の基準となる姿勢である基準姿勢の情報を含み、
前記制御部は、前記基準姿勢に基づいて、前記対象地図から前記基準地図への姿勢変換行列を算出し、
前記姿勢変換行列により、前記対象地図を正規化する、
付記３に記載の自律移動装置。
【０１７１】
（付記１１）
前記制御部は、
前記対象地図を正規化する前に、前記対象地図に前記基準姿勢が登録されているか否かを判定し、前記基準姿勢が登録されていないなら、基準姿勢を登録する所定の場所まで前記自律移動装置を移動させて、前記所定の場所で前記基準姿勢を前記対象地図に登録する、
付記１０に記載の自律移動装置。
【０１７２】
（付記１２）
前記基準姿勢を登録する所定の場所は、充電器の設置場所である、
付記１１に記載の自律移動装置。
【０１７３】
（付記１３）
前記制御部は、
前記対象地図に含まれる姿勢の情報から取り出した位置ベクトルの各要素の標準偏差と、前記基準地図に含まれる姿勢の情報から取り出した位置ベクトルの各要素の標準偏差と、の比に基づいて、前記対象地図から前記基準地図への長さの比であるスケールを算出し、
前記スケールも用いて前記環境地図を正規化する、
付記３から１２のいずれか１つに記載の自律移動装置。
【０１７４】
（付記１４）
前記周囲の環境の変化は、照明の変化である、
付記１から１３のいずれか１つに記載の自律移動装置。
【０１７５】
（付記１５）
撮像部で撮影された画像を用いて環境地図の作成及び位置の推定を行う自律移動装置の自律移動方法であって、
周囲の環境の変化に応じて複数の環境地図を作成し、
前記作成した複数の環境地図を正規化して統一的に扱えるように記憶部に保存し、
前記正規化された環境地図を用いて位置を推定する、
自律移動方法。
【０１７６】
（付記１６）
撮像部で撮影された画像を用いて環境地図の作成及び位置の推定を行う自律移動装置のコンピュータに、
周囲の環境の変化に応じて複数の環境地図を作成し、
前記作成した複数の環境地図を正規化して統一的に扱えるように記憶部に保存し、
前記正規化された環境地図を用いて位置を推定する、
処理を実行させるためのプログラム。
【符号の説明】
【０１７７】
１０…制御部、１１…環境情報取得部、１２…地図作成部、１３…地図正規化部、１４…自己位置推定部、１５…行動計画部、１６…移動制御部、２０…記憶部、２１…地図記憶部、２２…地図保存部、３１，３１ａ，３１ｂ…帰還信号受信部、３２，３２ａ，３２ｂ…駆動部、３３…撮像部、３４…通信部、３５…充電接続部、５１，５１ａ，５１ｂ…帰還信号送信部、５２…電力供給部、５３，５３ａ，５３ｂ…受信可能範囲、１００，１０１…自律移動装置、２００…充電器
【図１】
2020021257.tif 000003
【図２】
2020021257.tif 000004
【図３】
2020021257.tif 000005
【図４】
2020021257.tif 000006
【図５】
2020021257.tif 000007
【図６】
2020021257.tif 000008
【図７】
2020021257.tif 000009
【図８】
2020021257.tif 000010
【図９】
2020021257.tif 000011
【図１０】
2020021257.tif 000012
【図１１】
2020021257.tif 000013
【図１２】
2020021257.tif 000014
【図１３】
2020021257.tif 000015
【図１４】
2020021257.tif 000016
【図１５】
2020021257.tif 000017
【図１６】
2020021257.tif 000018
【図１７】
2020021257.tif 000019
【図１８】
2020021257.tif 000020