特許 6724439 自律移動装置、自律移動システム、自律移動方法及びプログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6724439

(24)【登録日】2020年6月29日

(45)【発行日】2020年7月15日

(54)【発明の名称】自律移動装置、自律移動システム、自律移動方法及びプログラム

(51)【国際特許分類】

   G05D 1/02 20200101AFI20200706BHJP

   H04W 16/18 20090101ALI20200706BHJP

【ＦＩ】

   G05D1/02 H

   H04W16/18 110

【請求項の数】6

【全頁数】27

(21)【出願番号】特願2016-49544(P2016-49544)

(22)【出願日】2016年3月14日

(65)【公開番号】特開2017-167625(P2017-167625A)

(43)【公開日】2017年9月21日

【審査請求日】2019年2月1日

(73)【特許権者】

【識別番号】000001443

【氏名又は名称】カシオ計算機株式会社

(74)【代理人】

【識別番号】100095407

【弁理士】

【氏名又は名称】木村 満

(72)【発明者】

【氏名】浜田 玲

【審査官】 藤崎 詔夫

(56)【参考文献】

【文献】 特開２００８−０８７１０２（ＪＰ，Ａ）

【文献】 特開２０１２−２４７２９０（ＪＰ，Ａ）

【文献】 特開２００８−０９０５７６（ＪＰ，Ａ）

【文献】 米国特許出願公開第２０１５／０３１２７７４（ＵＳ，Ａ１）

【文献】 特開２００３−００５８３３（ＪＰ，Ａ）

【文献】 特開平１１−３２５９４１（ＪＰ，Ａ）

【文献】 特開２００９−１６２４１３（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０５Ｄ １／０２

Ｈ０４Ｗ １６／１８

(57)【特許請求の範囲】

【請求項1】

自律移動装置であって、
撮像部が撮影した複数の画像の情報を用いて地図を作成する地図作成部と、
前記撮像部が撮影した複数の画像の情報を用いて前記自律移動装置の位置を推定する位置推定部と、
外部の通信機と通信する際の通信品質を測定する通信品質測定部と、
前記位置推定部が推定した前記自律移動装置の位置と前記通信品質測定部が測定した通信品質とを用いて通信品質地図を作成する通信品質地図作成部と、
前記地図作成部が作成した地図と、前記通信品質地図作成部が作成した通信品質地図とを用いて、目的地までの経路を探索する経路探索部と、
前記経路探索部が探索した経路において通信品質が所定の閾値より低い領域である通信不良領域を通過するのに必要な時間である通信不良時間を取得する通信不良時間取得部と、を備え、
前記経路探索部は、前記通信不良時間が許容レイテンシ以下の場合には前記通信不良領域を通る経路を探索し、
さらに、前記自律移動装置の速度を設定する速度設定部を備え、
前記通信不良時間取得部は、前記自律移動装置が現在の速度よりも高い最大速度で移動した場合の前記通信不良時間を取得し、
前記自律移動装置が前記現在の速度で移動した場合の前記取得された前記通信不良時間が前記許容レイテンシを超え、かつ、前記自律移動装置が前記最大速度で移動した場合の前記取得された前記通信不良時間が前記許容レイテンシ以下の場合には、前記経路探索部は前記通信不良領域を通る経路を探索し、前記速度設定部は前記自律移動装置の速度を前記最大速度に設定し、
前記通信不良時間取得部が取得した通信不良時間、及び、前記自律移動装置が前記最大速度で移動した場合の前記取得された前記通信不良時間がいずれも前記許容レイテンシを超える場合には、前記経路探索部は、前記通信不良領域を通らない経路を探索する、
自律移動装置。

【請求項2】

さらに、前記自律移動装置のタスク内容に基づいて前記許容レイテンシを設定する許容レイテンシ設定部を備える、
請求項１に記載の自律移動装置。

【請求項3】

前記経路探索部は、前記目的地が設定されていない場合でも、前記通信品質地図作成部が作成した通信品質地図において、前記通信不良領域を避ける経路を設定する、
請求項１又は２に記載の自律移動装置。

【請求項4】

請求項１から３のいずれか１項に記載の自律移動装置と、
前記自律移動装置と通信する外部の通信機と、
を備える自律移動システム。

【請求項5】

自律移動装置の自律移動方法であって、
撮像部が撮影した複数の画像の情報を用いて地図を作成する地図作成ステップと、
前記撮像部が撮影した複数の画像の情報を用いて前記自律移動装置の位置を推定する位置推定ステップと、
外部の通信機と通信する際の通信品質を測定する通信品質測定ステップと、
前記位置推定ステップで推定した前記自律移動装置の位置と前記通信品質測定ステップで測定した通信品質とを用いて通信品質地図を作成する通信品質地図作成ステップと、
前記地図作成ステップで作成した地図と、前記通信品質地図作成ステップで作成した通信品質地図とを用いて、目的地までの経路を探索する経路探索ステップと、
前記経路探索ステップで探索した経路において通信品質が所定の閾値より低い領域である通信不良領域を通過するのに必要な時間である通信不良時間を取得する通信不良時間取得ステップと、を備え、
前記経路探索ステップは、前記通信不良時間が許容レイテンシ以下の場合には前記通信不良領域を通る経路を探索し、
さらに、前記自律移動装置の速度を設定する速度設定ステップを備え、
前記通信不良時間取得ステップは、前記自律移動装置が現在の速度よりも高い最大速度で移動した場合の前記通信不良時間を取得し、
前記自律移動装置が前記現在の速度で移動した場合の前記取得された前記通信不良時間が前記許容レイテンシを超え、かつ、前記自律移動装置が前記最大速度で移動した場合の前記取得された前記通信不良時間が前記許容レイテンシ以下の場合には、前記経路探索ステップは前記通信不良領域を通る経路を探索し、前記速度設定ステップは前記自律移動装置の速度を前記最大速度に設定し、
前記通信不良時間取得ステップが取得した通信不良時間、及び、前記自律移動装置が前記最大速度で移動した場合の前記取得された前記通信不良時間がいずれも前記許容レイテンシを超える場合には、前記経路探索ステップは、前記通信不良領域を通らない経路を探索する、
自律移動方法。

【請求項6】

自律移動装置のコンピュータに、
撮像部が撮影した複数の画像の情報を用いて地図を作成する地図作成ステップ、
前記撮像部が撮影した複数の画像の情報を用いて前記自律移動装置の位置を推定する位置推定ステップ、
外部の通信機と通信する際の通信品質を測定する通信品質測定ステップ、
前記位置推定ステップで推定した前記自律移動装置の位置と前記通信品質測定ステップで測定した通信品質とを用いて通信品質地図を作成する通信品質地図作成ステップ、
前記地図作成ステップで作成した地図と、前記通信品質地図作成ステップで作成した通信品質地図とを用いて、目的地までの経路を探索する経路探索ステップ、
前記経路探索ステップで探索した経路において通信品質が所定の閾値より低い領域である通信不良領域を通過するのに必要な時間である通信不良時間を取得する通信不良時間取得ステップ、を実行させ、
前記経路探索ステップは、前記通信不良時間が許容レイテンシ以下の場合には前記通信不良領域を通る経路を探索し、
さらに、前記自律移動装置の速度を設定する速度設定ステップを前記コンピュータに実行させ、
前記通信不良時間取得ステップは、前記自律移動装置が現在の速度よりも高い最大速度で移動した場合の前記通信不良時間を取得し、
前記自律移動装置が前記現在の速度で移動した場合の前記取得された前記通信不良時間が前記許容レイテンシを超え、かつ、前記自律移動装置が前記最大速度で移動した場合の前記取得された前記通信不良時間が前記許容レイテンシ以下の場合には、前記経路探索ステップは前記通信不良領域を通る経路を探索し、前記速度設定ステップは前記自律移動装置の速度を前記最大速度に設定し、
前記通信不良時間取得ステップが取得した通信不良時間、及び、前記自律移動装置が前記最大速度で移動した場合の前記取得された前記通信不良時間がいずれも前記許容レイテンシを超える場合には、前記経路探索ステップは、前記通信不良領域を通らない経路を探索する、
プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

用途に応じて自律的に移動する自律移動装置が普及してきている。例えば、屋内の掃除のために自律的に移動する自律移動装置が知られている。一般的に、このような自律移動装置は、実空間の地図の作成と、実空間内での自機位置の推定を行う必要がある。実空間の地図を作成するための手法としては、例えばＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）法が知られている。単眼カメラを用いたＳＬＡＭ技術の基本的な原理は、非特許文献１及び非特許文献２に記載されており、カメラの撮影する動画像の複数フレームから、同一の特徴点を追跡することで、自機の３次元位置（カメラ位置）と特徴点の３次元位置（これが集まって地図の情報を構成する）とを交互または同時に推定する処理を行っている。

【0003】

この自律移動装置の画像認識・音声認識等の計算処理を、ネットワーク接続した外部の計算装置で処理させると、ネットワーク通信機能が必要となる代わりに、自律移動装置側の計算パワーや記憶容量の節約（それらに付随して冷却機構の省略やバッテリ等の軽量化）、データベースの大規模化とリアルタイム共有化、アップデートの容易さ等、多くのメリットが得られる。

【0004】

このような無線通信を行う移動ロボットの先行技術として、特許文献１には、無線通信が切断される場所に移動した場合であっても、自律的に無線通信を復旧できる場所に移動することが可能な移動ロボットが開示されている。

【先行技術文献】

【特許文献】

【0005】

【特許文献1】特開２００８−８７１０２号公報

【非特許文献】

【0006】

【非特許文献1】Ａｎｄｒｅｗ Ｊ．Ｄａｖｉｓｏｎ， “Ｒｅａｌ−Ｔｉｍｅ Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ ｗｉｔｈ ａ Ｓｉｎｇｌｅ Ｃａｍｅｒａ”， Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ ９ｔｈ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ Ｖｏｌｕｍｅ ２， ２００３， ｐｐ．１４０３−１４１０

【非特許文献2】Ｒｉｃｈａｒｄ Ｈａｒｔｌｅｙ， Ａｎｄｒｅｗ Ｚｉｓｓｅｒｍａｎ， “Ｍｕｌｔｉｐｌｅ Ｖｉｅｗ Ｇｅｏｍｅｔｒｙ ｉｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ”， Ｓｅｃｏｎｄ Ｅｄｉｔｉｏｎ， Ｃａｍｂｒｉｄｇｅ． Ｕｎｉｖｅｒｓｉｔｙ Ｐｒｅｓｓ， Ｍａｒｃｈ ２００４， ｃｈａｐｔｅｒ ９

【発明の概要】

【発明が解決しようとする課題】

【0007】

しかし、特許文献１に開示された技術では、通信が切断されると、本来の目的地は無視して通信可能な場所に移動するため、通信が切断されないように目的地に移動することはできなかった。

【0008】

本発明は、上記問題を解決するためになされたものであり、通信が切断されないように目的地に移動することができる自律移動装置等を提供することを目的する。

【課題を解決するための手段】

【0009】

上記目的を達成するため、本発明の自律移動装置は、
撮像部が撮影した複数の画像の情報を用いて地図を作成する地図作成部と、
前記撮像部が撮影した複数の画像の情報を用いて前記自律移動装置の位置を推定する位置推定部と、
外部の通信機と通信する際の通信品質を測定する通信品質測定部と、
前記位置推定部が推定した前記自律移動装置の位置と前記通信品質測定部が測定した通信品質とを用いて通信品質地図を作成する通信品質地図作成部と、
前記地図作成部が作成した地図と、前記通信品質地図作成部が作成した通信品質地図とを用いて、目的地までの経路を探索する経路探索部と、
前記経路探索部が探索した経路において通信品質が所定の閾値より低い領域である通信不良領域を通過するのに必要な時間である通信不良時間を取得する通信不良時間取得部と、を備え、
前記経路探索部は、前記通信不良時間が許容レイテンシ以下の場合には前記通信不良領域を通る経路を探索し、
さらに、前記自律移動装置の速度を設定する速度設定部を備え、
前記通信不良時間取得部は、前記自律移動装置が現在の速度よりも高い最大速度で移動した場合の前記通信不良時間を取得し、
前記自律移動装置が前記現在の速度で移動した場合の前記取得された前記通信不良時間が前記許容レイテンシを超え、かつ、前記自律移動装置が前記最大速度で移動した場合の前記取得された前記通信不良時間が前記許容レイテンシ以下の場合には、前記経路探索部は前記通信不良領域を通る経路を探索し、前記速度設定部は前記自律移動装置の速度を前記最大速度に設定し、
前記通信不良時間取得部が取得した通信不良時間、及び、前記自律移動装置が前記最大速度で移動した場合の前記取得された前記通信不良時間がいずれも前記許容レイテンシを超える場合には、前記経路探索部は、前記通信不良領域を通らない経路を探索する、
ことを特徴とする。

【発明の効果】

【0010】

本発明によれば、通信が切断されないように目的地に移動することができる。

【図面の簡単な説明】

【0011】

【図1】実施形態に係る自律移動装置の外観を示す図である。

【図2】実施形態に係る自律移動装置の構成を示す図である。

【図3】（ａ）実施形態に係るシステム構成のうちネットワーク経由の構成を示す図である。（ｂ）実施形態に係るシステム構成のうち直接通信での構成を示す図である。

【図4】実施形態に係る自律移動制御処理全体のフローチャートを示す図である。

【図5】実施形態に係る自律移動制御処理の中の自機位置推定スレッドの処理のフローチャートを示す図である。

【図6】実施形態に係る自律移動制御処理の中の地図作成スレッドの処理のフローチャートを示す図である。

【図7】地図兼経路コストマップＳ（ｘ，ｙ）の具体例を示す図である。

【図8】通信品質コストマップＣ（ｘ，ｙ）の具体例を示す図である。

【図9】実施形態に係る自律移動制御処理の中のループクロージングスレッドの処理のフローチャートを示す図である。

【図10】第１の実施形態に係る目的地への経路計画のフローチャートを示す図である。

【図11】（ａ）第１の実施形態に係る目的地への経路計画の具体例を地図兼経路コストマップＳ（ｘ，ｙ）で説明する図である。（ｂ）第１の実施形態に係る目的地への経路計画の具体例を経路探索用コストマップＴ（ｘ，ｙ）で説明する図である。

【図12】第２の実施形態に係る目的地への経路計画のフローチャートを示す図である。

【図13】（ａ）第２の実施形態に係る目的地への経路計画の具体例を経路探索用コストマップＴ（ｘ，ｙ）で説明する図である。（ｂ）第２の実施形態に係る目的地への経路計画の具体例を通信品質コストマップＣ（ｘ，ｙ）で説明する図である。

【図14】（ａ）第２の実施形態に係る目的地への経路計画の別の具体例を経路探索用コストマップＴ（ｘ，ｙ）で説明する図である。（ｂ）第２の実施形態に係る目的地への経路計画の別の具体例を更新された経路探索用コストマップＴ（ｘ，ｙ）で説明する図である。

【発明を実施するための形態】

【0012】

以下、図１を参照しながら、本発明の実施形態に係る自律移動装置について説明する。自律移動装置１００は、用途に応じて自律的に移動する。この用途とは、例えば、警備監視用、屋内掃除用、ペット用、玩具用などである。

【0013】

自律移動装置１００は、外観上、撮像部４１、駆動部４２を備える。

【0014】

撮像部４１は、単眼の撮像装置（カメラ）を備える。撮像部４１は、例えば、３０ｆｐｓで画像（フレーム）を取得する。自律移動装置１００は、撮像部４１が逐次取得した画像に基づいて、自機位置と周囲環境とをリアルタイムに認識しながら、自律移動を行う。

【0015】

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

【0016】

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

【0017】

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

【0018】

図２に示すように、自律移動装置１００は、撮像部４１、駆動部４２に加えて、制御部１０、記憶部２０、センサ部３０、入力部４３、通信部４４、電源４５、を備える。

【0019】

制御部１０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）等で構成され、記憶部２０に記憶されたプログラムを実行することにより、後述する各部（地図作成部１１、位置推定部１２、通信品質測定部１３、通信品質地図作成部１４、経路探索部１５）の機能を実現する。

【0020】

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

【0021】

画像記憶部２１には、撮像部４１が撮影した画像が記憶される。ただし、記憶容量の節約のために、撮影したすべての画像を記憶しなくてもよく、また画像自体ではなく、画像の特徴量を記憶するようにしても良い。

【0022】

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

【0023】

通信品質地図記憶部２３には、後述する通信品質地図作成部１４が作成した通信品質地図が記憶される。

【0024】

経路探索用地図記憶部２４には、地図記憶部２２と通信品質地図記憶部２３の情報から作成した、通信品質を考慮した経路を探索するために用いる経路探索用地図が記憶される。

【0025】

センサ部３０として、距離センサ３１を備える。距離センサ３１は、周囲の壁や障害物までの距離を測定するセンサであって、例えば、赤外線や超音波を用いたセンサである。なお、独立した距離センサ３１を搭載せずに、撮像部４１を用いて壁や障害物までの距離を測定するようにしても良い。また、他の物体に衝突したことを検知するバンパーセンサー（図示せず）を備えても良い。

【0026】

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

【0027】

通信部４４は、外部の通信機２００と無線通信するための無線モジュールである。一般的には電波を用いた無線ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）モジュールであるが、赤外線や可視光を用いたものであっても良い。なお、外部の通信機２００との通信においては、図３（ａ）に示すように、ネットワーク経由で通信しても良いし、図３（ｂ）に示すように直接通信しても良い。図３に示すように、自律移動装置１００だけでなく通信機２００も通信部４４を備える。

【0028】

電源４５は、自律移動装置１００を動作させる電源であり、一般的には内蔵された充電池であるが、太陽電池であっても良いし、床面から無線で電力供給されるシステムであっても良い。電源４５が充電池の場合は、充電ステーション（ホームベース）に自律移動装置１００がドッキングすることで充電される。

【0029】

次に、制御部１０の機能について説明する。制御部１０は、地図作成部１１、位置推定部１２、通信品質測定部１３、通信品質地図作成部１４、経路探索部１５を含み、後述する通信品質地図の作成や自律移動装置１００の移動制御等を行う。また、制御部１０は、マルチスレッド機能に対応しており、複数のスレッド（異なる処理の流れ）を並行して進めることができる。

【0030】

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

【0031】

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

【0032】

通信品質測定部１３は、通信部４４で外部の通信機２００と通信する際の通信品質を測定する。

【0033】

通信品質地図作成部１４は、自律移動装置１００の周囲の各地点において、通信品質測定部１３で外部の通信機２００と通信する際の通信品質を測定し、通信品質地図（各地点での通信品質が記録された地図）を作成する。

【0034】

経路探索部１５は、経路探索用地図記憶部２４に記憶された地図の情報に基づいて経路を探索する。

【0035】

自律移動装置１００の自律移動制御処理のメインフローを図４を参照して説明する。制御部１０は、まず、目的地を示す変数（目的地）に初期値である「未設定」をセットする（ステップＳ１０１）。次に、制御部１０は、自機位置推定スレッドを起動する（ステップＳ１０２）。そして、地図作成スレッドを起動する（ステップＳ１０３）。次に、ループクロージングスレッドを起動する（ステップＳ１０４）。自機位置推定スレッドや地図作成スレッドが動作することで、ＳＬＡＭ法に基づいて、地図の情報及びビジュアルオドメトリ（地図と画像とを用いて推定した自機位置の情報）の生成が開始される。その後、制御部１０は、動作終了かどうかを判定し（ステップＳ１０５）、動作終了なら（ステップＳ１０５；Ｙｅｓ）動作を終了し、動作終了でなければ（ステップＳ１０５；Ｎｏ）、距離センサ３１とビジュアルオドメトリを利用して地図の更新を行う（ステップＳ１０６）。

【0036】

次に制御部１０は、目的地への移動指示があるか否かを判定する（ステップＳ１０７）。移動指示があれば（ステップＳ１０７；Ｙｅｓ）、指示された目的地を目的地を示す変数（目的地）に設定し（ステップＳ１１０）、目的地への経路計画を行う（ステップＳ１１１）。目的地への経路計画の詳細は後述する。そして、制御部１０は、経路計画により設定された経路に沿って移動を行うために駆動部４２に動作指示を出し（ステップＳ１１２）、ステップＳ１０５に戻る。

【0037】

目的地への移動指示がなければ（ステップＳ１０７；Ｎｏ）、制御部１０は、目的地が未設定であるか否かを判定する（ステップＳ１０８）。目的地が未設定の場合は（ステップＳ１０８；Ｙｅｓ）、自由な自律移動を行い（ステップＳ１０９）、ステップＳ１０５に戻る。自由な自律移動とは、ランダムに動き回るような移動でも良いし、地図を作成するために部屋の中をくまなく動き回るような移動でも良い。

【0038】

目的地が設定されている場合は（ステップＳ１０８；Ｎｏ）、経路計画により経路が設定されているので、その経路に沿って移動を行うために制御部１０は駆動部４２に動作指示を出し（ステップＳ１１２）、ステップＳ１０５に戻る。なお、目的地を示す変数（目的地）はスレッドをまたいで値を参照することができるように記憶部２０に記憶されている。

【0039】

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

【0040】

次に、自律移動装置１００のメインフロー（図４）のステップＳ１０２で起動される自機位置推定スレッドについて、図５を参照して説明する。このスレッドは、位置推定部１２が、最初に初期化処理を行い、その後自機位置推定（撮像部４１で取得した画像を用いてビジュアルオドメトリにより自機位置を推定する）を続ける処理である。

【0041】

位置推定部１２は、まず、推定状態変数に「初期化中」をセットする（ステップＳ２０１）。推定状態変数は、その時点の自機位置推定処理の状態を示し、「初期化中」「追跡成功」「追跡失敗」の三つの値を取る。次に、動作終了か否かを判定する（ステップＳ２０２）。動作終了なら（ステップＳ２０２；Ｙｅｓ）終了し、動作終了でないなら（ステップＳ２０２；Ｎｏ）、推定状態変数が「初期化中」であるか否かを判定する（ステップＳ２０３）。初期化中でないなら（ステップＳ２０３；Ｎｏ）ステップＳ２２１以降の自機位置推定処理を行い、初期化中なら（ステップＳ２０３；Ｙｅｓ）ステップＳ２０４に進んで初期化処理を行う。まず初期化処理について説明する。

【0042】

初期化処理では位置推定部１２はまずフレームカウンタＮに−１をセットし（ステップＳ２０４）、撮像部４１で画像を取得する（ステップＳ２０５）。画像は例えば３０ｆｐｓで取得することができる（取得した画像はフレームとも呼ばれる）。次に、位置推定部１２は、撮像部４１で取得した画像に含まれている２Ｄ特徴点を取得する（ステップＳ２０６）。

【0043】

２Ｄ特徴点とは、画像中のエッジ部分など、画像内の特徴的な部分であり、ＳＩＦＴ（Ｓｃａｌｅ−Ｉｎｖａｒｉａｎｔ Ｆｕｔｕｒｅ Ｔｒａｎｓｆｏｒｍ）やＳＵＲＦ（Ｓｐｅｅｄ−Ｕｐ Ｒｏｂｕｓｔ Ｆｅａｔｕｒｅｓ）等のアルゴリズムを用いて取得することができる。

【0044】

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

【0045】

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

【0046】

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

【0047】

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

【0048】

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

【0049】

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

【0050】

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

【0051】

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

【0052】

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

【0053】

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

【0054】

２Ｄ特徴点のＳＬＡＭ座標における３Ｄ位置（Ｘ，Ｙ，Ｚ）が求まったら、それをＭａｐ点として、地図作成部１１がＭａｐ点データベース（Ｍａｐ点ＤＢ（Ｄａｔａｂａｓｅ）とも言い、地図記憶部２２に格納される）に登録する（ステップＳ２１４）。Ｍａｐ点データベースに登録する要素としては、少なくとも、「２Ｄ特徴点のＳＬＡＭ座標における３Ｄ位置であるＸ，Ｙ，Ｚ」と、「その２Ｄ特徴点の特徴量」（例えばＳＩＦＴ等で得た特徴量）が必要である。

【0055】

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

【0056】

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

【0057】

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

【0058】

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

【0059】

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

【0060】

以上で自機位置推定スレッドの初期化処理が完了したので、位置推定部１２は、推定状態変数に「追跡成功」をセットし（ステップＳ２１９）、ＳＬＡＭ座標と実環境座標とのスケール対応を得るために、オドメトリによる並進距離（実環境での座標で求められる）を、上記の処理で推定したＳＬＡＭ座標での並進距離ｄで除することによって、スケールＳを求める（ステップＳ２２０）。

【0061】

そして、ステップＳ２０２、ステップＳ２０３を経由して、初期化済の場合の処理であるステップＳ２２１へ進む。

【0062】

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

【0063】

まず、位置推定部１２は、撮像部４１で画像を撮影し（ステップＳ２２１）、フレームカウンタＮをインクリメントする（ステップＳ２２２）。そして、特徴点取得部１１は、撮像部４１で撮影した画像に含まれている２Ｄ特徴点を取得する（ステップＳ２２３）。

【0064】

次に、位置推定部１２は、推定状態変数が「追跡成功」か否かを判定する（ステップＳ２２４）。「追跡成功」なら（ステップＳ２２４；Ｙｅｓ）、位置推定部１２は、フレームデータベースに登録されている一つ前のキーフレーム（キーフレーム番号がＮＫＦである画像）の情報から、その画像の情報に含まれている２Ｄ特徴点のうち、３Ｄ位置が既知である（Ｍａｐ点データベースに登録されているＭａｐ点になっている）２Ｄ特徴点を取得して、その中でステップＳ２２３で取得した特徴点と対応が取れる２Ｄ特徴点（対応特徴点）の個数（特徴点対応数）を取得する（ステップＳ２２５）。

【0065】

推定状態変数の値が「追跡成功」でないなら（ステップＳ２２４；Ｎｏ）、位置推定部１２は、フレームデータベースに登録されている二つ以上前のキーフレーム（キーフレーム番号がＮＫＦ未満である画像）の中から、Ｍａｐ点とステップＳ２２３で取得した特徴点との対応数が最大となるキーフレームと、その特徴点対応数を取得する（ステップＳ２２６）。

【0066】

そして、位置推定部１２は、特徴点対応数が閾値（例えば３０。以下「基準対応特徴点数」という。）より大きいか否かを判定し（ステップＳ２２７）、基準対応特徴点数以下の場合（ステップＳ２２７；Ｎｏ）はＳＬＡＭ法で推定する姿勢の精度が悪くなるので、推定状態変数に「追跡失敗」をセットし（ステップＳ２３０）、位置の推定は行わずにステップＳ２０２に戻る。

【0067】

特徴点対応数が基準対応特徴点数より大きい場合は（ステップＳ２２７；Ｙｅｓ）、ステップＳ２２５又はステップＳ２２６において特徴点対応数を取得したキーフレームの近傍のキーフレーム（特徴点対応数を取得したキーフレームと、所定の割合以上（例えば３０％以上）特徴点の重なりが存在するキーフレーム）に含まれるＭａｐ点と対応が取れるステップＳ２２３で取得した特徴点（対応特徴点）の個数（特徴点対応数）を位置推定部１２が取得する（ステップＳ２２８）。

【0068】

そして、位置推定部１２は、特徴点対応数が閾値（例えば５０。以下「第２の基準対応特徴点数」という。）より大きいか否かを判定し（ステップＳ２２９）、第２の基準対応特徴点数以下なら（ステップＳ２２９；Ｎｏ）、推定状態変数に「追跡失敗」をセットし（ステップＳ２３０）、位置の推定は行わずにステップＳ２０２に戻る。特徴点対応数が第２の基準対応特徴点数よりも大きければ（ステップＳ２２９；Ｙｅｓ）、ステップＳ２３１に進む。

【0069】

ステップＳ２３１では、位置推定部１２は、推定状態変数に追跡成功をセットする（ステップＳ２３１）。位置推定部１２は、ステップＳ２２８で取得した対応特徴点それぞれの３Ｄ位置（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ）をＭａｐ点データベースから取得し、ステップＳ２２１で取得した画像に含まれている対応特徴点との対応関係を用いて、現在の自機の姿勢（並進ベクトルｔ及び回転行列Ｒで表される自機の位置及び向き）を推定する（ステップＳ２３２）。

【0070】

この自機の姿勢の推定方法について、補足説明する。ステップＳ２２１で取得した画像に含まれている対応特徴点のフレーム座標を（ｕ_ｉ，ｖ_ｉ）とし、その対応特徴点の３Ｄ位置を（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ）とする（ｉは１から特徴点対応数までの値を取る）と、各対応特徴点の３Ｄ位置（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ）を以下の式（５）によってフレーム座標系に投影した値（ｕｘ_ｉ，ｖｘ_ｉ）とフレーム座標（ｕ_ｉ，ｖ_ｉ）とは理想的には一致するはずである。
（ｕｘ_ｉ ｖｘ_ｉ １）’〜Ａ（Ｒ｜ｔ）（Ｘ_ｉ Ｙ_ｉ Ｚ_ｉ １）’…（５）

【0071】

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

【数1】

【0072】

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

【0073】

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

【0074】

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

【0075】

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

【0076】

次に、自律移動装置１００のメインフロー（図４）のステップＳ１０３で起動される地図作成スレッドについて、図６を参照して説明する。このスレッドは地図作成部１１が、キーフレーム中の対応特徴点の３Ｄ位置を計算して取得するＭａｐ点の情報と、距離センサ３１を用いて取得する壁や障害物までの距離の情報と、から地図の情報を作成し、地図記憶部２２に記憶する。そして、通信品質地図作成部１４が、通信品質測定部１３から取得する通信品質の情報を用いて通信品質地図を作成し、通信品質地図記憶部２３に記憶する。

【0077】

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

【0078】

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

【0079】

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

【0080】

次に地図作成部１１は、自機位置推定スレッドの推定状態変数が「追跡成功」か否かを判定する（ステップＳ３０８）。「追跡成功」でなければ（ステップＳ３０８；Ｎｏ）、自機位置を推定できていないということなので、障害物や通信品質の測定をスキップしてステップＳ３１１へ進む。

【0081】

推定状態変数が「追跡成功」であれば（ステップＳ３０８；Ｙｅｓ）、地図作成部１１は、距離センサ３１で壁や障害物までの距離を測定し、ＶＯで示される自機の位置及び向きと測定した距離とに基づき、壁や障害物の位置や、自機位置から壁や障害物までの距離を地図記憶部２２に登録する（ステップＳ３０９）。

【0082】

そして、通信品質地図作成部１４は、通信品質測定部１３により通信品質を測定し、測定して得られた通信品質の値を、ＶＯで示される自機位置での通信品質の値として、通信品質地図記憶部２３に登録する（ステップＳ３１０）。

【0083】

通信品質は、通信部４４で観測できる電界強度や受信電力を用いたり、実際に行っている通信のＲＴＴ（Ｒｏｕｎｄ−Ｔｒｉｐ Ｔｉｍｅ）や、実効速度、エラー率、遅延を用いたりして取得できる。これら様々な評価値を複合した評価値を用いても良い。また、赤外線や可視光の通信リンクを用いる場合などは、信号が到達したか否か（間に遮蔽物があるか否か）という二値の情報を用いても良い。既にその位置で通信品質を測定済みだった場合は、それまでの測定結果の平均値やメディアンを採用したり、最悪値を採用したりしても良い。また、平均値、メディアン、最良値、最悪値等を別々に保存しておき、自律移動装置１００の移動目的に応じて使い分けても良い。

【0084】

通信品質地図記憶部２３に登録する値は、通信品質の良いほど小さい正の数（コスト値）になるように変換してから登録する。例えば、電界強度や実効速度は通信品質が良いほど大きくなるので、逆数を登録する等する。

【0085】

次に、地図作成部１１は、キーフレームキューが空かどうかを判定する（ステップＳ３１１）。空であれば（ステップＳ３１１；Ｙｅｓ）、全キーフレームの姿勢と全Ｍａｐ点の３Ｄ位置に対して、バンドルアジャストメント処理を行って、精度向上を図る（ステップＳ３１２）。そして、バンドルアジャストメント処理を行った結果、誤差が大きいＭａｐ点が見つかったら、それをＭａｐ点ＤＢから削除し（ステップＳ３１３）、ステップＳ３１４に進む。キーフレームキューが空でなければ（ステップＳ３１１；Ｎｏ）、何もせずにステップＳ３１４に進む。

【0086】

ステップＳ３１４では、地図作成部１１は、地図記憶部２２に記憶されている情報に基づき、地図兼経路コストマップを配列Ｓ（ｘ，ｙ）に格納する（ステップＳ３１４）。地図兼経路コストマップとは、座標（ｘ，ｙ）が移動不能領域（壁や障害物）なら「Ｓ（ｘ，ｙ）＝∞」、移動不能領域でなければ「Ｓ（ｘ，ｙ）＝座標（ｘ，ｙ）から移動不能領域までの最短距離が大きいほど小さい正数」となるように値が格納された配列である。例えば、座標（ｘ，ｙ）が移動不能領域でない場合、座標（ｘ，ｙ）から移動不能領域までの最短距離の逆数をＳ（ｘ，ｙ）に格納することができる。なお、配列Ｓは、地図記憶部２２に記憶される。

【0087】

地図兼経路コストマップの配列Ｓ（ｘ，ｙ）の具体例を図７に示す。図７で、黒の領域は移動不能領域（壁や障害物）を示しており、この領域内はＳ（ｘ，ｙ）＝∞である。また白い領域は移動不能領域までの最短距離が大きい領域を示しており、この領域内のＳ（ｘ，ｙ）の値は小さい（例えば１未満）。そして、灰色の領域は移動不能領域までの最短距離が小さい領域を示しており、この領域内のＳ（ｘ，ｙ）の値は大きい（例えば１以上）。

【0088】

次に、通信品質地図作成部１４は、通信品質地図記憶部２３に記憶されている情報に基づき、通信品質コストマップを配列Ｃ（ｘ，ｙ）に格納する（ステップＳ３１５）。通信品質コストマップとは、座標（ｘ，ｙ）での通信品質が良いほど小さい正数がＣ（ｘ，ｙ）に格納された配列である。例えば、座標（ｘ，ｙ）での電界強度や実効速度の逆数をＣ（ｘ，ｙ）に格納することができる。なお、配列Ｃは通信品質地図記憶部２３に記憶される。

【0089】

通信品質コストマップを配列Ｃ（ｘ，ｙ）の具体例を図８に示す。図８の左端に外部の通信機２００（図示せず）が存在している。図８で、白の領域は通信品質が良好な領域を示しており、この領域内のＣ（ｘ，ｙ）は小さい（例えば１未満）。そして、薄い灰色の領域は通信品質が悪い領域を示しており、この領域内のＣ（ｘ，ｙ）の値は比較的大きい（例えば１以上３未満）。濃い灰色の領域は通信できないほど通信品質が悪い領域を示しており、この領域内のＣ（ｘ，ｙ）の値はとても大きい（例えば３以上）。

【0090】

自律移動装置１００が通過する座標（ｘ，ｙ）は一般に疎なので、Ｓ（ｘ，ｙ）もＣ（ｘ，ｙ）も、まばらにしか値が設定されていない状態になることが多い。そこで、内挿・外挿を行って、各座標（ｘ，ｙ）に密に値を設定する。

【0091】

次に、地図作成部１１は、ループクロージングスレッドのキーフレームキューにＭＫＦをセットして（ステップＳ３１６）、ステップＳ３０１に戻る。

【0092】

以上の処理で作成した地図兼経路コストマップの配列Ｓ（ｘ，ｙ）及び通信品質コストマップの配列Ｃ（ｘ，ｙ）は、いずれもスレッドをまたいで値を参照することができるように記憶部２０に記憶されている。

【0093】

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

【0094】

このバンドルアジャストメントの処理を行うことで、キーフレーム姿勢とＭａｐ点の３Ｄ位置の精度向上を図ることができる。しかし、この処理は計算量が多いことと、この処理を行わなくても精度向上が図れないだけで特別問題が発生するわけではない。したがって、ここでは、キーフレームが空の状態の時（他の処理が比較的軽い状態の時）のみバンドルアジャストメント処理を行うようにしている。通信品質が良い場合は、自律移動装置１００自身でバンドルアジャストメント処理をするのではなく、画像記憶部２１や地図記憶部２２の情報を通信部４４で外部の通信機２００に送信し、外部の通信機２００でバンドルアジャストメント処理した結果を自律移動装置１００に送り返してもらうようにしても良い。

【0095】

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

【0096】

次に自律移動装置１００のメインフロー（図４）のステップＳ１０４で起動されるループクロージングスレッドについて、図９を参照して説明する。このスレッドでは制御部１０は、ループクロージング処理ができるかどうかをチェックし続け、ループクロージング処理ができる場合にはループクロージング処理を行っている。なお、ループクロージング処理とは、以前来たことのある同じ場所に戻ってきたことを認識した場合に、以前この同じ場所にいた時の姿勢の値と現在の姿勢の値とのずれを用いて、以前来た時から今までの軌跡中のキーフレームや、関連するＭａｐ点の３Ｄ位置を修正することをいう。

【0097】

まず、制御部１０は、動作終了かを判定する（ステップＳ４０１）。動作終了なら（ステップＳ４０１；Ｙｅｓ）終了する。動作終了でないなら（ステップＳ４０１；Ｎｏ）キーフレームキューが空かどうかを判定する（ステップＳ４０２）。キーフレームキューが空なら（ステップＳ４０２；Ｙｅｓ）ステップＳ４０１に戻り、キーフレームキューが空でないなら（ステップＳ４０２；Ｎｏ）、キーフレームキューからデータを取り出してＬＫＦ（ループクロージングスレッドで処理するキーフレームのキーフレーム番号を表す変数）にセットする（ステップＳ４０３）。次に制御部１０は、ＬＫＦが１より大きいかを判定する（ステップＳ４０４）。ＬＫＦが０又は１である場合（ステップＳ４０４；Ｎｏ）はステップＳ４０１に戻ってキーフレームキューにデータが入るのを待つ。そして、ＬＫＦが２以上の場合（ステップＳ４０４；Ｙｅｓ）は、以下の処理を行う。

【0098】

制御部１０は、フレームＤＢを参照し、現キーフレーム（キーフレーム番号がＬＫＦのキーフレーム）と「キーフレーム自体の特徴」の類似度が所定の類似度（例えば０．９。以下「基準画像類似度」という。）以上になるキーフレームをフレームＤＢから検索する（ステップＳ４０５）。ここで、この類似度は、画像（キーフレーム）の特徴を特徴ベクトルで表している場合は、二つの画像の特徴ベクトルの絶対値（要素の二乗和の平方根）を１に正規化したもの同士の内積を、その二つの画像の類似度とすることができる。また、二つの画像の特徴ベクトル（絶対値を１に正規化したもの）の距離（各要素の差の二乗和の平方根）の逆数を類似度としても良い。

【0099】

制御部１０は、「キーフレーム自体の特徴」の類似度が基準画像類似度以上になるキーフレームが発見されたかどうかを判定し（ステップＳ４０６）、発見されなければ（ステップＳ４０６；Ｎｏ）ステップＳ４０１へ戻り、発見されたら（ステップＳ４０６；Ｙｅｓ）、発見されたキーフレームから現キーフレームまでの軌跡中のキーフレームの姿勢と、軌跡中のキーフレームに含まれるＭａｐ点の３Ｄ位置を修正する（ステップＳ４０７）。例えば、制御部１０は、現キーフレームの姿勢を、発見されたキーフレームの姿勢と同じ姿勢として修正する。そして、発見されたキーフレームの姿勢と現キーフレームの姿勢との差分を用いて、発見されたキーフレームから現キーフレームまでの軌跡中の各キーフレームの姿勢に線形的に補正を加える。さらにこれらの各キーフレームに含まれるＭａｐ点の３Ｄ位置についても各キーフレームの姿勢の補正量に応じて修正する。そしてステップＳ４０１に戻る。

【0100】

以上のループクロージング処理は、計算量が多いため、通信品質が良い場合は、自律移動装置１００自身でループクロージング処理をするのではなく、画像記憶部２１や地図記憶部２２の情報を通信部４４で外部の通信機２００に送信し、外部の通信機２００でループクロージング処理した結果を自律移動装置１００に送り返してもらうようにしても良い。

【0101】

次に自律移動装置１００のメインフロー（図４）のステップＳ１１１の処理である目的地への経路計画について説明する。従来の経路計画は、地図兼経路コストマップである配列Ｓ（ｘ，ｙ）、現在位置及び目的地位置を与えると、現在位置から目的地位置への最小累積コスト経路を選択するものである。具体的には、各座標をノード、隣接関係をエッジとし、ノード乃至エッジに配列Ｓ（ｘ，ｙ）に従った重みをつけたグラフを構成すれば、Ｄｉｊｋｓｔｒａアルゴリズムなどグラフ理論の最短経路探索のアルゴリズムを用いて効率良く最小累積コスト経路を選択することができる。本発明では、配列Ｓ（ｘ，ｙ）だけでなく、通信品質マップである配列Ｃ（ｘ，ｙ）も用いることで、通信品質を保つような経路を選択できる。

【0102】

（第１の実施形態）
では、目的地への経路計画の第１の実施形態について、図１０を参照して説明する。まず経路探索部１５は、閾値の値を通信品質が良好と考えられる場合の値に設定する（ステップＳ５０１）。次に、各配列の全ての座標について値を更新するために、ｘとｙを０に初期化する（ステップＳ５０２）。

【0103】

次に、経路探索部１５は、地図兼経路コストマップである配列Ｓの（ｘ，ｙ）の値を、経路探索用コストマップである配列Ｔ（ｘ，ｙ）に代入する（ステップＳ５０３）。配列Ｔは、経路を探索する際に、従来の地図兼経路コストマップＳの代わりに用いる配列で、経路探索用地図記憶部２４に記憶される。

【0104】

次に経路探索部１５は、通信品質マップである配列Ｃの（ｘ，ｙ）の値が閾値より大きいか否かを判定する（ステップＳ５０４）。閾値よりも大きければ（ステップＳ５０４；Ｙｅｓ）、座標（ｘ，ｙ）での通信品質が良好でないことを意味するので、その場所を通らないようにするために、Ｔ（ｘ，ｙ）に∞を設定し（ステップＳ５０５）、ステップＳ５０６に進む。閾値以下なら（ステップＳ５０４；Ｎｏ）、その場所（ｘ，ｙ）での通信品質は良好ということになるので、Ｔ（ｘ，ｙ）の値はＳ（ｘ，ｙ）のまま変更せずにステップＳ５０６へ進む。

【0105】

ステップＳ５０６では、経路探索部１５は、全てのｘ，ｙについて、ステップＳ５０３〜ステップＳ５０５の処理が完了したか否かを判定する（ステップＳ５０６）。完了していなければ（ステップＳ５０６；Ｎｏ）、ステップＳ５０３に戻る。

【0106】

全てのｘ，ｙについて、ステップＳ５０３〜ステップＳ５０５の処理が完了したら（ステップＳ５０６；Ｙｅｓ）、経路探索用コストマップである配列Ｔ（ｘ，ｙ）の設定が完了したことになるので、経路探索部１５は、Ｔに従来の経路計画のアルゴリズムを適用して、経路探索を行う（ステップＳ５０７）。

【0107】

そして、経路探索部１５は、ステップＳ５０７で経路を見つけることができたか否かを判定する（ステップＳ５０８）。経路が見つかったら（ステップＳ５０８；Ｙｅｓ）、終了する。経路が存在しない場合は（ステップＳ５０８；Ｎｏ）、経路探索部１５は閾値を上げて（ステップＳ５０９）、ステップＳ５０２に戻り、リトライを繰り返す。

【0108】

なお、ステップＳ５０９で閾値の値が、通信品質が「通信不可能」を示すほど大きな値になった場合は、経路探索をあきらめて終了する。フローチャートが煩雑になるため、図には示していないが、この場合は、メインフロー（図４）のステップＳ１１０での目的地の設定からやり直す必要がある。

【0109】

なお、この処理を行う前に、通信品質マップである配列Ｃ（ｘ，ｙ）が示す通信品質が悪い領域（図８で薄い灰色や濃い灰色の領域）を膨張、収縮等させておいてもよい。より確実に通信を行いたい場合は通信品質が悪い領域を膨張させておく。また、移動中の通信の瞬断を許容して可動域を広く取りたい場合は通信品質が悪い領域を収縮させておく。

【0110】

経路探索の具体例を図７、図８及び図１１を参照して説明する。例えば、図７で示される地図兼経路コストマップＳ（ｘ，ｙ）を用いて、図１１に示す現在位置から目的位置までの経路を従来の方法で探索すると、図１１（ａ）の矢印で示す経路が選択される。これに対し、図８で示される通信品質コストマップＣ（ｘ，ｙ）を上述の第１の実施形態に適用すると、図８の白い部分以外は通信品質が悪いので、図１１（ｂ）に示す経路探索用コストマップＴ（ｘ，ｙ）が得られ、この図で現在位置から目的位置までの経路を探索すれば、図１１（ｂ）の点線の矢印で示す経路が選択される。このように、通信品質を考慮した経路探索用コストマップを用いることで、通信が切断されない経路をたどって目的地に移動することができる。

【0111】

（第２の実施形態）
次に、一定時間（許容されるレイテンシ（ｌａｔｅｎｃｙ））通信ができない状態を許容する第２の実施形態について、説明する。例えば、自律移動装置１００が、その実行するタスクとして、人間との自然言語会話を行っていて、自然言語会話を行うために必要な音声認識処理を外部の通信機２００に処理させているとすると、許容されるレイテンシは、その自然言語会話中の沈黙期間の延長が許される時間である。例えば、この場合、レイテンシを２秒と設定することができる。また、自律移動装置１００が実行するタスクの内容によって、外部の通信機２００と通信するデータ量が変わるが、このデータ量の通信に必要な帯域を満たす通信品質を閾値に設定する。

【0112】

では、第２の実施形態について、図１２を参照して説明する。まず、経路探索部１５は、自律移動装置１００が実行するタスクの内容に基づき、許容レイテンシを変数ＴＬに設定する（ステップＳ６０１）。このステップにおいて、経路探索部１５は許容レイテンシ設定部に相当する。次に、経路探索部１５は、自律移動装置１００が実行するタスクの内容に基づき、通信品質の閾値を設定する（ステップＳ６０２）。そして、経路探索部１５は、地図兼経路コストマップである配列Ｓの（ｘ，ｙ）の値を、経路探索用コストマップである配列Ｔ（ｘ，ｙ）に代入する（ステップＳ６０３）。

【0113】

次に、経路探索部１５は、経路探索用コストマップＴに従来の経路計画のアルゴリズムを適用して経路探索を行い、得られた経路を仮経路Ｌとする（ステップＳ６０４）。この処理において、経路探索部１５は経路候補探索部に相当する。もしここで経路が得られなかった場合は、経路探索をあきらめて終了する。フローチャートが煩雑になるため、図には示していないが、この場合は、メインフロー（図４）のステップＳ１１０での目的地の設定からやり直す必要がある。

【0114】

次に、経路探索部１５は、仮経路Ｌを現在位置から目的位置まで順にたどって、Ｃ（ｘ，ｙ）＞閾値となる区間（仮経路Ｌをたどる際に通信品質が足りない区間）の長さに対して、その長さを通過するのに必要な通過所要時間（以下、ＴＴで表す）を算出し、ＴＬ＜ＴＴか否かを判定する（ステップＳ６０５）。ＴＴの算出は、簡単には、仮経路Ｌの経路を一つたどるたびに、その一つ分の経路の長さを自律移動装置１００の移動速度で割った値を足していけば良い。ステップＳ６０５において、経路探索部１５は通信不良時間取得部に相当する。

【0115】

仮経路Ｌを現在位置からたどった際に、ＴＬ＜ＴＴとなるような区間無しに目的位置までたどり着けたら（ステップ６０５；Ｎｏ）、仮経路Ｌを本経路に採用し（ステップＳ６０７）、終了する。

【0116】

仮経路Ｌをたどった際に、ＴＬ＜ＴＴとなるような区間が見つかったら（ステップ６０５；Ｙｅｓ）、仮経路Ｌは採用できないということになるので、仮経路Ｌを現在位置からＴＬ＜ＴＴとなる地点までたどる区間上の点の周辺（当該区間を所定のマージン幅だけ拡大した領域）のＴ（ｘ，ｙ）に∞を代入し（ステップＳ６０６）、ステップＳ６０４に戻る。当該区間の周辺領域のＴ（ｘ，ｙ）に∞を代入することで、次回以降の経路探索において、その区間を経路に選択しないようにしている。

【0117】

この第２の実施形態での経路探索の具体例を図７、図８、図１３及び図１４を参照して説明する。例えば、図７で示される地図兼経路コストマップＳ（ｘ，ｙ）と、図８で示される通信品質コストマップを用いて図１２で示される経路計画の処理を行うと、ステップＳ６０４において、図１３（ａ）の矢印で示される仮経路Ｌが得られる。そして、ステップＳ６０５で、図１３（ｂ）の薄い灰色部分を矢印に沿って通過する時間ＴＴが、許容レイテンシＴＬ以下であったなら、仮経路Ｌが本経路として採用される。

【0118】

図１３（ｂ）の薄い灰色部分を矢印に沿って通過する途中で時間ＴＴが、許容レイテンシＴＬを超えてしまったら、ステップＳ６０６で、図１４（ａ）に示されるように、仮経路Ｌの矢印の途中まで（ＴＴがＴＬを超えた位置まで）のＴ（ｘ，ｙ）の周辺の領域が∞（図では黒塗り）に設定される。そして、ステップＳ６０４に戻って、再度新たな仮経路Ｌを求める。この新たな仮経路Ｌは、図１４（ａ）の点線矢印で表している。

【0119】

この点線矢印の経路をたどっても、途中で時間ＴＴが、許容レイテンシＴＬを超えてしまうので、ステップＳ６０６で、図１４（ｂ）に示されるように、仮経路Ｌの点線矢印の途中まで（ＴＴがＴＬを超えた位置まで）のＴ（ｘ，ｙ）の周辺の領域が∞（図では黒塗り）に設定される。そして、ステップＳ６０４に戻って、再度新たな仮経路Ｌを求める。この新たな仮経路Ｌは、図１４（ｂ）の点線矢印で表している。

【0120】

この点線矢印の経路は、通信品質が良好な部分のみを通るので、この経路が本経路として採用される。

【0121】

以上、図１２のような処理を行うことで、通信品質の悪い領域があっても、そこを通過する時間が許容レイテンシ以下であれば、そこを通過する経路をたどることができるようになる。目的地への移動に、通信品質の悪い領域を通らざるを得ない場合、第１の実施形態では目的地への移動が不可能だったが、第２の実施形態では許容レイテンシ以下の時間でその領域を通過できる場合は、目的地の移動が可能になる。

【0122】

例えば、音声認識のもとになる音声データは、通信が途絶えている間はバッファしておき、通信品質が回復した後に外部機器に送信して処理を行うようにすれば、許容レイテンシを守って処理を継続できる。また、自律移動装置１００のタスクが自然言語会話の場合、自然言語会話において沈黙期間の延長が許される時間（例えば２秒）を許容レイテンシとして設定しておけば、通信品質の悪い領域を通過せざるを得ない場合でも自然言語会話を継続できる。

【0123】

なお、自律移動装置１００の速度を現在速度より上げることができる場合、ステップＳ６０５でＴＴ＜ＴＬとなった場合でも、すぐにはステップＳ６０６に進まず、その環境での最大速度で移動した場合のＴＴを計算し直し、計算し直したＴＴならＴＬ以下になる場合は（ステップＳ６０５；Ｎｏ）、経路探索部１５は、仮経路Ｌを本経路とし（ステップＳ６０７）、制御部１０は駆動部４２に最大速度を設定して移動を指示するようにしても良い。この処理において、制御部１０は、速度設定部に相当する。このような処理をすることにより、通常の速度では許容レイテンシ以下の時間で通信品質の悪い領域を通過できない場合でも、移動速度を上げて通過させることができる。したがって、通常速度では目的地に到達できない状況下でも、速度を上げることにより、許容レイテンシを守って目的地へ到達できるようになる。

【0124】

また、上述した実施形態では、目的地への経路計画を行う時のみ通信品質を考慮したが、メインフロー（図４）のステップＳ１０９での自由な自律移動の際も、制御部１０が通信品質コストマップＣ（ｘ，ｙ）を参照して、通信品質が悪いところには長時間滞在しないように移動制御をしても良い。このようにすることによって、目的地の位置や目的地の設定の有無によらず、外部の通信機との通信を良好に保つことができる。

【0125】

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

【0126】

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

【0127】

（付記１）
撮像部が撮影した複数の画像の情報を用いて地図を作成する地図作成部と、
前記撮像部が撮影した複数の画像の情報を用いて自機位置を推定する位置推定部と、
外部の通信機と通信する際の通信品質を測定する通信品質測定部と、
前記位置推定部が推定した自機位置と前記通信品質測定部が測定した通信品質とを用いて通信品質地図を作成する通信品質地図作成部と、
前記通信品質地図作成部が作成した通信品質地図を用いて、目的地までの経路を探索する経路探索部と、
を備える自律移動装置。

【0128】

（付記２）
前記経路探索部は、前記通信品質地図作成部が作成した通信品質地図において、通信品質が所定の閾値より低い領域である通信不良領域を通らない経路を探索する、
付記１に記載の自律移動装置。

【0129】

（付記３）
さらに、前記経路探索部が探索した経路において通信品質が所定の閾値より低い領域である通信不良領域を通過するのに必要な時間である通信不良時間を取得する通信不良時間取得部を備え、
前記経路探索部は、前記通信不良時間取得部が取得した通信不良時間が許容レイテンシを超える場合には前記通信不良領域を通らない経路を探索し、前記通信不良時間が前記許容レイテンシ以下の場合には前記通信不良領域を通る経路を探索する、
付記１に記載の自律移動装置。

【0130】

（付記４）
さらに、自機のタスク内容に基づいて前記許容レイテンシを設定する許容レイテンシ設定部を備える、
付記３に記載の自律移動装置。

【0131】

（付記５）
さらに、自機の速度を設定する速度設定部を備え、
前記通信不良時間取得部は、自律移動装置が最大速度で移動した場合の前記通信不良時間を取得し、
前記自律移動装置が最大速度で移動した場合の前記通信不良時間が前記許容レイテンシ以下の場合には、前記経路探索部は前記通信不良領域を通る経路を探索し、前記速度設定部は自機の速度を最大速度に設定する、
付記３又は４に記載の自律移動装置。

【0132】

（付記６）
前記経路探索部は、前記目的地が設定されていない場合でも、前記通信品質地図作成部が作成した通信品質地図において、通信品質が所定の閾値より低い領域である通信不良領域を避ける経路を設定する、
付記１に記載の自律移動装置。

【0133】

（付記７）
付記１から６のいずれか一つに記載の自律移動装置と、
前記自律移動装置と通信する外部の通信機と、
を備える自律移動システム。

【0134】

（付記８）
外部の通信機と通信を行いながら自律的に移動するための自律移動方法であって、
前記通信機と通信する際の通信品質に応じて経路を探索する、
自律移動方法。

【0135】

（付記９）
前記通信機と通信する際の通信品質が所定の閾値より低い領域である通信不良領域を通らない経路を探索する、
付記８に記載の自律移動方法。

【0136】

（付記１０）
前記通信機と通信する際の通信品質が所定の閾値より低い通信不良領域を許容レイテンシ以下の時間しか通らない経路を探索する、
付記８に記載の自律移動方法。

【0137】

（付記１１）
目的地が設定されていない場合でも、前記通信機と通信する際の通信品質が所定の閾値より低い領域である通信不良領域を避ける経路を設定する、
付記８に記載の自律移動方法。

【0138】

（付記１２）
撮像部が撮影した複数の画像の情報を用いて地図を作成する地図作成ステップと、
前記撮像部が撮影した複数の画像の情報を用いて自機位置を推定する位置推定ステップと、
外部の通信機と通信する際の通信品質を測定する通信品質測定ステップと、
前記位置推定ステップで推定した自機位置と前記通信品質測定ステップで測定した通信品質とを用いて通信品質地図を作成する通信品質地図作成ステップと、
前記通信品質地図作成ステップで作成した通信品質地図を用いて、目的地までの経路を探索する経路探索ステップと、
を備える自律移動方法。

【0139】

（付記１３）
コンピュータに、
撮像部が撮影した複数の画像の情報を用いて地図を作成する地図作成ステップ、
前記撮像部が撮影した複数の画像の情報を用いて自機位置を推定する位置推定ステップ、
外部の通信機と通信する際の通信品質を測定する通信品質測定ステップ、
前記位置推定ステップで推定した自機位置と前記通信品質測定ステップで測定した通信品質とを用いて通信品質地図を作成する通信品質地図作成ステップ、
前記通信品質地図作成ステップで作成した通信品質地図を用いて、目的地までの経路を探索する経路探索ステップ、
を実行させるためのプログラム。

【符号の説明】

【0140】

１００…自律移動装置、１０…制御部、１１…地図作成部、１２…位置推定部、１３…通信品質測定部、１４…通信品質地図作成部、１５…経路探索部、２０…記憶部、２１…画像記憶部、２２…地図記憶部、２３…通信品質地図記憶部、２４…経路探索用地図記憶部、３０…センサ部、３１…距離センサ、４１…撮像部、４２…駆動部、４３…入力部、４４…通信部、４５…電源、２００…通信機

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】