JP6849330B2 - 地図生成方法、自己位置推定方法、ロボットシステム、およびロボット - Google Patents

Description

本開示は、移動ロボットの自己位置推定・環境地図作成を行う技術に関し、特に環境に設置された環境センサと該環境センサと連携する移動ロボットのシステムに関する。

人間やその他の機械が存在する環境空間を移動ロボットが自律的に移動するために、移動ロボット自身が自己の位置・姿勢、周辺環境を正しく認識を行うことが必要となる。従来、このような認識を行うための手法として、環境の地図を作成しながら同時に自己位置の推定を行うＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）技術が知られている。

ＳＬＡＭ技術における移動ロボットは、移動ロボットの内部の状態を知るためのセンサである内界センサと、外界の状態を知るためのセンサである外界センサを備えている。

ＳＬＡＭ技術では、まず内界センサの情報から現在の位置・姿勢を推定する。次に、推定した位置・姿勢において既に保持している周辺環境の地図情報から、移動ロボットの（ｉ）位置（ｉｉ）姿勢（ｉｉｉ）位置の不確実性（ｉｖ）姿勢の不確実性、の予測を行う。次に、外界センサによって得られた観測情報と、内界センサの情報から予測した情報を比較する。次に、比較によって算出された、それぞれの情報の尤もらしさから、内界センサと外界センサの情報の重みを決定する。そして、内界センサと外界センサのデータの重みの情報を用いて、現在の位置・姿勢と地図情報を更新する処理を行う。

特許文献１には複数の異なるセンサの出力に基づいて自己位置推定を行う複数の推定器の信頼度を評価し、信頼度に応じて複数の推定器による自己位置推定情報を統合する、ロボットシステムが開示されている。

特許文献２には環境センサと連携して、人の行動を邪魔することなく、人や障害物との衝突を回避しながら移動を行う移動ロボットが開示されている。環境センサは環境内に存在する全ての人の位置を検出するように複数配置される。

特開２０１２−２４８０３２号公報 特許５６１７５６２号公報

特許文献１や特許文献２などの従来の方法では、環境変化が大きい空間においても、高精度かつロバストに、地図生成と自己位置推定とを行うことについては検討されていなかった。

本開示の一態様によれば、
少なくとも１つ以上の環境センサノードを用いて地図生成を行う移動ロボットの地図生成方法であって、
前記移動ロボット周辺の情報を含む予め作成された第１の地図情報を取得し、
前記移動ロボットに搭載された外界センサによって前記移動ロボット周辺の情報を含む第２の地図情報を取得し、
前記環境センサノードから前記移動ロボット周辺の情報を含む第３の地図情報を受け取り、
（ｉ）前記第３の地図情報に時間的変化量が所定の閾値以上の場所の情報が存在する場合、
前記第３の地図情報における時間的変化量が前記所定の閾値以上の場所に対応する、前記第１の地図情報における場所の情報と前記第２の地図情報における場所の情報とを除去する除去処理を行う、または、
前記第３の地図情報における時間的変化量が前記所定閾値以上の場所に対応する、前記第１の地図情報における場所の情報と前記第２の地図情報における場所の情報それぞれの分散を増加する分散増加処理を行い、
（ｉｉ）前記除去処理または前記分散増加処理が行われた後の第１の地図情報と第２の地図情報とをマッチングし、そのマッチング結果に基づいて地図情報を生成し、
（ｉｉｉ）マッチング結果に基づいて生成された地図情報に、前記第１の地図情報を更新する、地図生成方法が提供される。

また、本開示の別態様によれば、
少なくとも１つのセンサノードを用いて自己位置推定を行う移動ロボットの自己位置推定方法であって、
前記移動ロボット周辺の情報を含む予め作成された第１の地図情報を取得し、
前記移動ロボットに搭載された外界センサによって前記移動ロボット周辺の情報を含む第２の地図情報を取得し、
環境センサノードから前記移動ロボット周辺の情報を含む第３の地図情報を受け取り、
（ｉ）前記第３の地図情報に時間的変化量が所定の閾値以上の場所の情報が存在する場合、
前記第３の地図情報における時間的変化量が前記所定の閾値以上の場所に対応する、前記第１の地図情報における場所の情報と前記第２の地図情報における場所の情報とを除去する除去処理を行う、または、
前記第３の地図情報における時間的変化量が前記所定閾値以上の場所に対応する、前記第１の地図情報における場所の情報と前記第２の地図情報における場所の情報それぞれの分散を増加する分散増加処理を行い、
（ｉｉ）前記除去処理または前記分散増加処理が行われた後の第１の地図情報と第２の地図情報とをマッチングし、そのマッチング結果に基づいて地図情報を生成し、
（ｉｉｉ）マッチング結果に基づいて生成された地図情報に、前記第１の地図情報を更新し、
（ｉｖ）更新済みの第１の地図情報と、前記移動ロボットの位置および姿勢の少なくとも一方を検出する内界センサの検出結果とに基づいて、前記更新済みの第１の地図情報上での前記ロボットの自己位置を推定する、自己位置推定方法が提供される。

さらに、本開示の異なる態様によれば、
少なくとも１つの環境センサノードと移動ロボットとを含むロボットシステムであって、
前記環境センサノードが、前記移動ロボット周辺の情報を含む第３の地図情報を取得し、
前記移動ロボットが、
前記移動ロボット周辺の情報を含む予め作成された第１の地図情報を記録しているデータベースと、
前記移動ロボット周辺の情報を含む第２の地図情報を取得する外界センサと、
前記環境センサノードと通信して前記第３の地図情報を取得する通信部と、
（ｉ）前記第３の地図情報に時間的変化量が所定の閾値以上の場所の情報が存在する場合、
前記第３の地図情報における時間的変化量が前記所定の閾値以上の場所に対応する、前記第１の地図情報における場所の情報と前記第２の地図情報における場所の情報とを除去する除去処理を行う、または、
前記第３の地図情報における時間的変化量が前記所定閾値以上の場所に対応する、前記第１の地図情報における場所の情報と前記第２の地図情報における場所の情報それぞれの分散を増加する分散増加処理を行い、
（ｉｉ）前記除去処理または前記分散増加処理が行われた後の第１の地図情報と第２の地図情報とをマッチングし、そのマッチング結果に基づいて地図情報を生成し、
（ｉｉｉ）マッチング結果に基づいて生成された地図情報に、前記予め記録していた第１の地図情報を更新する情報処理部と、を備える、ロボットシステムが提供される。

さらにまた、本開示のさらに異なる態様によれば、
移動ロボットであって、
前記移動ロボット周辺の情報を含む予め作成された第１の地図情報を記録しているデータベースと、
前記移動ロボット周辺の情報を含む第２の地図情報を取得する外界センサと、
前記移動ロボットの外部にあって且つ前記移動ロボット周辺の情報を含む第３の地図情報を取得する少なくとも１つの環境センサノードと通信して前記第３の地図情報を取得する通信部と、
（ｉ）前記第３の地図情報に時間的変化量が所定の閾値以上の場所の情報が存在する場合、
前記第３の地図情報における時間的変化量が前記所定の閾値以上の場所に対応する、前記第１の地図情報における場所の情報と前記第２の地図情報における場所の情報とを除去する除去処理を行う、または、
前記第３の地図情報における時間的変化量が前記所定閾値以上の場所に対応する、前記第１の地図情報における場所の情報と前記第２の地図情報における場所の情報それぞれの分散を増加する分散増加処理を行い、
（ｉｉ）前記除去処理または前記分散増加処理が行われた後の第１の地図情報と第２の地図情報とをマッチングし、そのマッチング結果に基づいて地図情報を生成し、
（ｉｉｉ）マッチング結果に基づいて生成された地図情報に、前記予め記録していた第１の地図情報を更新する情報処理部と、を備える、移動ロボットが提供される。

本開示によれば、環境変化が大きい空間においても、高精度かつロバストに、地図生成と自己位置推定とを行うことができる。

本開示の実施の形態１にかかるロボットシステムの全体構成ブロック図 環境センサノードおよびロボットが存在する一例の環境を示す概略図 ロボットシステムの処理フロー図 動体移動・環境変化情報統合部の処理フロー図 時刻Ｔにおける移動ロボットを含む環境の一例を示す図解図 時刻Ｔ＋αにおける図４Ａの環境を示す図解図 時刻Ｔにおけるセンサデータの一例を示す図解図 時刻Ｔ＋αにおけるセンサデータの一例を示す図解図 地図生成・自己位置推定処理時のデータマッチングの一例を示す図解図 マッチング結果例を示す図解図 マッチング評価値例を示す図解図 データの除去処理の一例を示す図解図 除去処理後のデータのマッチング結果例を示す図解図 除去処理後のデータのマッチング評価値例を示す図解図 データの分散増加処理の一例を示す図解図 分散増加処理後のデータのマッチング結果例を示す図解図 分散増加処理後のデータのマッチング評価値例を示す図解図 分散増加処理を説明するための図解図 分散増加処理を説明するための別の図解図 分散増加処理の結果を示す図解図 実施の形態２に係るロボットシステムの処理フロー図

（本発明の基礎となった知見）
ＳＬＡＭ技術では、地図生成または自己位置推定の際に該移動ロボットの外界センサの検知範囲内の場所に環境変化があった場合、地図生成・自己位置推定に誤りが生じ、正しく推定を行うことができなくなるという課題がある。オフィスや家庭内などの人やその他の動体による環境変化が大きい空間や、駅やショッピングモールなどの多くの人が行き交う空間では大きな外乱すなわち大きな環境変化が頻繁に発生するためＳＬＡＭ技術によって地図生成を行うことが困難であり、結果としてＳＬＡＭ技術によって人や障害物を回避する経路を計画することが難しくなる。

さらに、上で述べたように環境変化が大きい空間では、地図生成・自己位置推定の処理において外界センサによって得られた情報と予測した情報とを比較する処理部分の最小二乗法による推定値の収束性が悪化し、計算量が増加する。

本開示では、環境変化が大きい空間においても、高精度かつロバストに、地図生成と自己位置推定とを行うことを目的とする。また、そのために必要な計算量の増加を抑制することも目的とする。

以下で説明する実施の形態は、いずれも本開示の一例を示すものである。以下の実施の形態で示される数値、形状、構成要素、ステップ、ステップの順序などは、一例であり、本開示を限定する主旨ではない。また、以下の実施の形態における構成要素のうち、最上位概念を示す独立請求項に記載されていない構成要素については、任意の構成要素として説明される。また全ての実施の形態において、各々の内容を組み合わせることもできる。

以下、実施の形態について図面を参照しながら説明する。

（実施の形態１）
図１には、本実施の形態１におけるロボットシステムの構成を示す。本ロボットシステムは、移動ロボット１００と、環境センサノード１２０とを含む。

移動ロボット１００は、外界センサ１０１と、内界センサ１０２と、情報処理部（動体・環境変化情報統合部１０３、地図生成・自己位置推定部１０４、地図情報データベース１０５、および経路計画部１０６）と、制御部１０７と、アクチュエータ１０８と、通信部１０９とを含む。

また、移動ロボット１００は、移動ロボット１００を移動させる手段として、例えば、２つもしくは４つの車輪、もしくは、２脚以上の脚を含んでいる。それら車輪または脚はアクチュエータ１０８からの動力により動作し、それにより移動ロボット１００を移動させることが可能である。前述した移動ロボット１００の脚は、１つ以上の関節を備えることが望ましい。さらに、移動ロボット１００を移動させる手段として、センサもしくは任意の物体を装着または把持するための１つ以上の腕が含まれることが望ましい。前述した移動ロボット１００の腕は、１つ以上の関節を備えることが望ましい。

また、移動ロボット１００は、１つ以上のＣＰＵ、１つ以上の主記憶、１つ以上の補助記憶を備える（図示せず）。

前述した移動ロボット１００のＣＰＵは、以下に説明する、動体・環境変化情報統合部１０３、地図生成・自己位置推定部１０４、経路計画部１０６、制御部１０７などが行う、様々な演算処理を行う。

前述した移動ロボット１００の主記憶は、前述したＣＰＵが直接アクセスすることのできる記憶装置であり、ＤＲＡＭ、ＳＲＡＭ、フラッシュメモリ、ＲｅＲＡＭ、ＦｅＲＡＭ、ＭＲＡＭ、ＳＴＴ−ＲＡＭ、ＰＣＲＡＭ、等のメモリによって構成され得る。

前述した移動ロボット１００の補助記憶は、前述した主記憶の内容を長期保存、コピーもしくはバックアップを行う記憶装置であり、ＨＤＤ、フラッシュメモリ、光ディスク、磁気ディスク、等によって構成され得る。

外界センサ１０１は、移動ロボット１００の周辺環境についての情報を検出するセンサであり、周辺環境の２次元の形状もしくは３次元の形状および色・材質を検出する。外界センサ１０１は、例えばＬＲＦ（Ｌａｓｅｒ Ｒａｎｇｅ Ｆｉｎｄｅｒ）、ＬＩＤＡＲ（Ｌａｓｅｒ Ｉｍａｇｉｎｇ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ）、カメラ、深度カメラ、ステレオカメラ、ソナー、ＲＡＤＡＲなどのセンサもしくはそれぞれの全ての組合せによって構成され得る。ここで、外界センサ１０１の取得する周辺環境の情報には、座標に関する情報（座標情報）が含まれる。

内界センサ１０２は、移動ロボット１００の位置・姿勢を検出するセンサである。内界センサ１０２は移動ロボット１００の移動距離、速度、加速度、角速度、姿勢それぞれの３方向成分（例えば、Ｘ−Ｙ−Ｚ直交座標系のＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分）の絶対値の少なくとも１つを検出することが可能である。また、移動ロボット１００の脚、腕の各関節について、速度、加速度、角速度、姿勢、トルクの絶対値を検出することが可能であってもよい。内界センサ１０２は、例えば加速度センサ、角速度センサ、エンコーダ、地磁気センサ、気圧・水圧センサ、トルクセンサなどのセンサもしくはそれぞれの組合せによって構成され得る。

動体・環境変化情報統合部１０３は、移動ロボット１００に搭載される外界センサ１０１が検出した情報（第２の地図情報）を取得する。また、環境センサノード１２０において抽出された動体移動・環境変化情報（第３の地図情報）を、通信部１０９を介して取得する。また、環境センサノード１２０によって抽出された動体移動・環境変化情報を用いて、外界センサ１０１が検出した情報における時系列で動的に変化する場所についての情報を除去する処理（除去処理）、あるいはその場所についての情報の信用度を下げる処理（分散増加処理）を行う。除去処理と分散増加処理の具体的な内容に関しては後述する。

地図生成・自己位置推定部１０４は、内界センサ１０２より得られた移動ロボット１００の移動距離、速度、加速度、角速度、姿勢のそれぞれの３方向成分の絶対値の少なくとも１つを取得する。また、地図生成・自己位置推定部１０４は、動体・環境変化情報統合部１０３によって処理された（除去処理済みまたは分散増加処理済みの）外界センサ１０１による移動ロボット１００の周辺環境についての情報を取得する。そして地図生成・自己位置推定部１０４は、取得した情報から移動ロボット１００の自己位置の推定と地図情報の生成を、フィルタリング処理によって同時に行う。なお、自己位置の推定と地図情報の生成は同時に行わなくてもよい。フィルタリング処理は、カルマンフィルタ、拡張カルマンフィルタ、Ｕｎｓｅｎｔｅｄカルマンフィルタ、パーティクルフィルタ、Ｒａｏ−Ｂｌａｃｋｗｅｌｌｉｚｅｄパーティクルフィルタの各手法によって構成され得る。また、地図生成・自己位置推定部１０４は生成した地図を、地図情報データベース１０５へ保存する。既に該当する環境の地図が存在する場合は、地図情報データベース１０５に保存されている地図情報を合わせて用いて、地図情報の更新を行い、地図情報データベース１０５への保存を行う。

地図情報データベース１０５は、地図生成・自己位置推定部１０４によって生成・更新された地図情報（第１の地図情報）を保存する。保存されている地図情報は地図生成・自己位置推定部１０４、経路計画部１０６によってそれぞれ使用される。地図情報データベース１０５は移動ロボット１００の主記憶または二次記憶に配置される。

経路計画部１０６は、地図情報データベース１０５に保存されている地図情報と、地図生成・自己位置推定部１０４によって推定された移動ロボット１００の自己位置の情報を用いて、現在の自己位置から地図上を移動する経路を計画する。計画する経路は、経路のコストを最小化する経路である。経路のコストは、総移動時間、総移動距離、移動に使われる総エネルギ、経路の混雑度の総和もしくはそれぞれの全ての組合せによって表現され得る。

制御部１０７は、実際に移動ロボットが動作する環境において、経路計画部１０６によって計画された経路について、移動ロボット１００の移動動作を実行するための制御を行う。

アクチュエータ１０８は、制御部１０７からの制御命令に基づいて、例えば車輪などを駆動し、それにより実際に移動ロボット１００を移動させる。

通信部１０９は、有線もしくは無線によって、１対多型、多対多型の通信を行う機能を備える。通信部１０９は、環境センサノード１２０に対して、指定した場所の動体移動・環境変化情報が存在するか否かを問い合わせる。

環境センサノード１２０は、環境センサ１２１と、動体移動・環境変化情報抽出部１２２と、環境センサ情報時系列データベース１２３と、動体移動・環境変化情報データベース１２４と、通信部１２５と、を含む。

環境センサ１２１は、環境センサノード１２０の周辺環境についての情報を検出するセンサである。周辺環境の情報には、周辺環境の２次元の形状もしくは３次元の形状及び色・材質に関する情報が含まれる。

図２は、環境センサノード１２０、環境センサノード１２０と同様の複数の環境センサノード３１２〜３１９、および移動ロボット１００が存在する環境の一例を概略的に示す図である。

なお、図１と図２に示す構成で符号が異なるが名称が同一であれば同一の構成であるものとする。

環境センサノード１２０、３１２〜３１９はそれぞれ環境センサ１２１、３２２〜３２９を備える。環境センサノード１２０、３１２の検出範囲はそれぞれ３３１、３３２である。環境センサノード３１３〜３１９の検出範囲は図の簡略化のため図示しないが、各環境センサノードによって、移動ロボットの移動可能な空間を全て検出可能とするように環境センサノードおよび環境センサを設置している。

また、環境センサノード１２０、３１２〜３１９の環境センサ１２１、３２２〜３２９は、例えばＬＲＦ（Ｌａｓｅｒ Ｒａｎｇｅ Ｆｉｎｄｅｒ）、ＬＩＤＡＲ（Ｌａｓｅｒ Ｉｍａｇｉｎｇ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ）、カメラ、深度カメラ、ステレオカメラ、ソナー、ＲＡＤＡＲ、焦電型赤外線センサ、赤外線ＴｏＦ（Ｔｉｍｅ ｏｆ Ｆｌｉｇｈｔ）センサなどのセンサもしくはそれぞれの全ての組合せによって構成され得る。

図２に示すように、移動ロボット１００は外界センサ１０１を備え、移動方向３０３に移動している。

図２に示すタイミングにおいて、外界センサ１０１の検出範囲は３０４である。

また、図２には、人／動体の現在の位置３４１、人／動体の過去の位置３４２、人／動体の現在の移動方向３４３を示している。人／動体が位置３４２から位置３４１へ移動したことによる時系列の環境変化は、環境センサノード１２０、３１２，３１３，３１４によって、詳細は後述するが、動体移動・環境変化情報として抽出される。

この環境センサ１２１の取得する周辺環境の情報には、座標に関する情報（座標情報）が含まれる。

動体移動・環境変化情報抽出部１２２は、環境センサ１２１が検出した時系列の周辺環境の情報を、検出情報と検出タイミングの組の形で環境センサ情報時系列データベース１２３へ保存する。例えば、検出タイミングは、時刻で表される。動体移動・環境変化情報抽出部１２２はまた、環境センサ情報時系列データベース１２３に保存されている時系列に沿った検出タイミングと検出情報の複数の組に基づいて、周辺環境の変化場所、変化形状、変化の大きさ及び変化にかかった時間を算出する。そして動体移動・環境変化情報抽出部１２２は、周辺環境の変化場所、その変化形状、その変化の大きさ、その変化にかかった時間を動体移動・環境変化情報として動体移動・環境変化情報データベース１２４に保存する。なお動体移動・環境変化情報は、周辺環境の変化場所、変化形状、変化の大きさ、変化にかかった時間、を全て含まなくてもよい。これらの情報のうち一部の情報のみを含んでいてもよいし、他の情報を含んでもよい。

環境センサ情報時系列データベース１２３は、動体移動・環境変化情報抽出部１２２によって取得した、環境センサ１２１が検出した時系列の周辺環境の情報を、検出情報と検出タイミングの組で保存している。保存された情報は、動体移動・環境変化情報抽出部１２２によって、動体移動・環境変化情報を抽出するために使用される。

動体移動・環境変化情報データベース１２４は、動体移動・環境変化情報抽出部１２２が算出した動体移動・環境変化情報を保存する。保存されている動体移動・環境変化情報は、移動ロボット１００の通信部１０９からの問い合わせを受けた通信部１２５によって、移動ロボット１００に対して送信される。

通信部１２５は、移動ロボット１００の通信部１０９からの動体移動・環境変化情報が存在するか否かの問い合わせを受けて、動体移動・環境変化情報データベース１２４内の検索を行い、動体移動・環境変化情報の移動ロボット１００への送信を無線または有線通信によって行う。

次に、図２、図３Ａ、および図３Ｂを参照しながら、本ロボットシステムの処理の流れについて説明する。Ｔ２０１は移動ロボット１００の処理フロー、Ｔ２２１Ａ、Ｔ２２１Ｂは環境センサノード１２０の処理フローを表す。

まず、図３ＡにおけるステップＳ２０１において、内界センサ１０２は、情報を取得する。ここで、取得する情報は移動ロボット１００の移動距離、速度、加速度、角速度、姿勢それぞれの３方向成分の絶対値である。

次に、ステップＳ２０２において、外界センサ１０１は移動ロボット１００の周辺環境についての情報を取得する。図２に示すタイミングにおける周辺環境の情報は検出範囲３０４について外界センサ１０１によって検出される情報である。ここで、取得する情報には、周辺環境の、２次元もしくは３次元の形状および色・材質に関する情報が含まれる。

次に、ステップＳ２０３において、移動ロボット１００は通信部１０９を介して環境センサノード１２０、３１２〜３１９に対して、指定した２次元もしくは３次元の任意の大きさを持つ場所内に、指定した時間帯に動体移動・環境変化情報が存在するか否かの問合せを行い、その問い合わせに対する返答を所定の時間待つ。ここで、場所や時間は、予め移動ロボット１００を管理するユーザによって任意の（固定の）大きさの場所や、任意の長さの時間に指定されていてもよい。また移動ロボットが状態に応じて、場所の大きさや時間の長さを変更し、指定してもよい。

また、移動ロボット１００からの問合せは、通信可能な他の移動ロボット、他の環境センサノード全てに対して行われてもよい。移動ロボット１００は全ての移動ロボット、環境センサノードからの、返答および動体移動・環境変化情報を受信することができる。また、移動ロボット１００からの問い合わせは、複数の環境センサノード（複数の他の移動ロボット）に対して、同時に行ってもよいし、順番に行ってもよい。

次に、ステップＳ２０４において、通信部１０９は、ステップＳ２０３での問合せに対して、環境センサノード１２０、３１２〜３１９からの返答があるか否かを判定する。一つ以上の返答があった場合はステップＳ２０５へ進む、返答がなかった場合はステップＳ２０８へ進む。

次に、ステップＳ２０５において、通信部１０９は環境センサノード１２０、３１２〜３１９および、その他の全ての環境センサノードと、その他の全ての移動ロボット、から動体移動・環境変化情報を受信する。なお、環境センサノード１２０、３１２〜３１９の中、ある特定の環境センサノードのみから動体移動・環境変化情報を受信してもよい。ステップＳ２０３で指定した場所に関して、環境変化情報を有する環境センサノードのみから情報を取得してもよい。

ステップＳ２０６において、動体・環境変化情報統合部１０３は、次のステップＳ２０７にて用いる、ステップＳ２０５で受信した動体移動・環境変化情報が存在する場所に対応する場所の情報を含む地図情報（第１の地図情報）を地図情報データベース１０５から取得する。

ここで、図３Ｂを参照しながら、ステップＳ２０７の具体的な内容、すなわちその詳細であるステップＳ２０７０１からステップＳ２０７０６について説明する。

ステップＳ２０７において、動体・環境変化情報統合部１０３は、ステップＳ２０２で外界センサ１０１が取得した情報と、ステップＳ２０６で取得した地図情報に対して、環境センサノード１２０、３１２〜３１９において抽出された一つ以上の動体移動・環境変化情報を統合する処理を行う。

まず、ステップＳ２０７０１において、動体・環境変化情報統合部１０３は、ステップＳ２０２で外界センサ１０１が取得した周辺環境の情報の座標系と、環境センサノード１２０、３１２〜３１９から送信された動体移動・環境変化情報の座標系を、ステップＳ２０６で取得した地図情報の座標系に変換する座標変換を行う。それぞれの座標系を、共通の座標系にする。

次に、ステップＳ２０７０２において、動体・環境変化情報統合部１０３は、動体移動・環境変化情報の形状に関する異なるタイミング間での時間的変化量の大きさが、閾値Ｔｈ＿ｃｈより大きい場所が存在するか否かの判定を行う。

本実施の形態の場合、「時間的変化量」は、例えば、２次元空間上もしくは３次元空間上における各点に物体が存在する確率である各点の存在確率（Ｅｘｉａｔｅｎｃｅ Ｐｒｏｂａｂｉｌｉｔｙ）に関する。具体的には、各点について第１のタイミングでの存在確率と第１のタイミングより時間的に後である第２のタイミングでの存在確率との間の差分の絶対値を求め、その各点それぞれの差分の絶対値を合計した値（Ｓｕｍ ｏｆ Ａｂｓｏｌｕｔｅ Ｄｉｆｆｅｒｅｎｃｅ）である。すなわち、第１のタイミングと第２のタイミングとの間の時間に変化した存在確率の変化量である。また、第１のタイミングおよび第２のタイミングは時刻であってもよいし、ロボットシステムが稼動し始めてからの経過時間であってもよい。

また、形状に関する時間的変化量は、例えば、各点に対するＳＳＤ（Ｓｕｍ ｏｆ Ｓｑｕａｒｅｄ Ｄｉｆｆｅｒｅｎｃｅ）、ＳＡＴＤ（Ｓｕｍ ｏｆ Ａｂｓｏｌｕｔｅ Ｔｒａｎｓｆｏｒｍｅｄ Ｄｉｆｆｅｒｅｎｃｅ）、ＭＡＤ（Ｍｅａｎ Ａｂｓｏｌｕｔｅ Ｄｉｆｆｅｒｅｎｃｅ）、ＭＳＤ（Ｍｅａｎ Ｓｑｕａｒｅｄ Ｄｉｆｆｅｒｅｎｃｅ）、もしくは画像特徴量であるＳＩＦＴ、ＳＵＲＦ、ＨｏＧ等の特徴ベクトルの差分であってもよい。また形状に関する時間的変化量は例えば、それらの特徴ベクトルを用いて生成したＢｏＦ（Ｂａｇ ｏｆ Ｆｅａｔｕｒｅ）特徴ベクトルの差分であってもよい。

閾値Ｔｈ＿ｃｈの具体的な値の設定は、用いる変化量の基準や状況によって異なる。一例としては、１０ｍ×１０ｍ×１０ｍの空間において、１００ｍｍ×１００ｍｍ×１００ｍｍの物体が２０ｍｍ移動した際の変化量を閾値Ｔｈ＿ｃｈ、として設定することが考えられる。ただし、閾値Ｔｈ＿ｃｈの具体値の設定は上記例に限定されるものではない。

異なるタイミング間の時間的変化量の大きさが閾値Ｔｈ＿ｃｈより大きい場所が存在する場合はステップＳ２０７０３に進む。そうでない場合はステップＳ２０７０７へ進む。

次に、ステップＳ２０７０３において、動体・環境変化情報統合部１０３は、動体移動・環境変化情報の形状に関する時間的変化量が閾値Ｔｈ＿ｃｈより大きいタイミング間の時間差が閾値Ｔｈ＿ｔｕ（第１の時間）より大きいか否かの判定を行う。閾値Ｔｈ＿ｔｕより大きい場合は、ステップＳ２０７０７へ進む。これは、時間差が所定の閾値Ｔｈ＿ｔｕより大きい場合は、当該の動体移動・環境変化情報は外乱による一時的な環境変化ではなく、静物体の移動等による半永続的又は永続的な環境の変化であるとみなし、環境の変化した部分を新たな地図として扱うことを意味する。例えば、室内で家具などを動かした際のレイアウト変更がそれに該当し、レイアウト変更のような時間差の大きい変化は新たな地図として扱う。時間差が閾値Ｔｈ＿ｔｕより小さいもしくは等しい場合は、ステップＳ２０７０４に進む。

次に、ステップＳ２０７０４において、動体・環境変化情報統合部１０３は、動体移動・環境変化情報の形状に関する時間的変化量が閾値Ｔｈ＿ｃｈより大きいタイミング間の時間差が閾値Ｔｈ＿ｔｂ（第２の時間）より小さいか否かの判定を行う。

閾値Ｔｈ＿ｔｂより小さい場合、ステップＳ２０７０５において、動体・環境変化情報統合部１０３は、動体移動・環境変化情報（第３の地図情報）における時間的変化量が閾値Ｔｈ＿ｃｈより大きい場所に対応する、移動ロボット１００の外界センサ１０１が取得した周辺環境の情報（第２の地図情報）における場所の情報とステップＳ２０６で取得した地図情報（第１の地図情報）における場所の情報を除去する除去処理を実行する。その情報の除去処理についての詳細は後述する。

一方、閾値Ｔｈ＿ｔｂより大きいもしくは等しい場合、ステップＳ２０７０６において、動体・環境変化情報統合部１０３は、動体移動・環境変化情報（第３の地図情報）における時間的変化量が閾値Ｔｈ＿ｃｈより大きい場所に対応する、移動ロボット１００の外界センサ１０１が取得した周辺環境の情報（第２の地図情報）における場所の情報とステップＳ２０６で取得した地図情報（第１の地図情報）における場所の情報それぞれの分散を増加変更する分散増加処理を実行する。なお、閾値Ｔｈ＿ｔｂは、閾値Ｔｈ＿ｔｕよりも大きい値である。例えば、分散の増加変更は分散Ｖに対して、１より大きい定数ａを乗じる形（ａ×Ｖ）で計算される。

ここで言う「情報の分散」とは情報のばらつき・不確からしさを表す指標を言う。周辺環境の情報に関して言えば、分散が小さい場合には周辺環境の空間上において期待値周辺に検出対象が存在する確率が高いことを意味する。一方、分散が大きい場合には期待値周辺に検出対象が存在する確率が低いことを意味し、検出対象の存在しうる確率の分布が空間上に大きく広がる。

また、情報の「分散増加処理」は、情報のばらつき・不確からしさを増加させることを言い、すなわち情報の信用度を下げることを言う。この情報の分散増加処理についての詳細は後述する。

なお、ステップＳ２０７０４で、動体移動・環境変化情報の形状に関する時間的変化量が閾値Ｔｈ＿ｃｈより大きいタイミング間の時間差が閾値Ｔｈ＿ｔｂより小さいか否かを判定したがこれに限られない。例えば、動体移動・環境変化情報の形状に関する時間的変化量の大きさが、閾値Ｔｈ＿ｃｈとは異なる新たな閾値より大きいか否かを判定してもよい。すなわち動体移動・環境変化情報の形状に関する時間的変化量の大きさが、新たな閾値より大きい場合（Ｙｅｓ）ステップＳ２０７０５に進み、そうでない場合（Ｎｏ）ステップＳ２０７０６に進む。ここで新たな閾値は閾値Ｔｈ＿ｃｈより高い値である。すなわちステップＳ２０７０４でＹｅｓの場合はそうでない場合（Ｎｏ）よりも変化量が大きい場合なので、動体移動・環境変化情報を外界センサ情報・地図情報から除去する処理が有効となる。

次に、ステップＳ２０７０７において、動体・環境変化情報統合部１０３は、受信した一つ以上の場所に関する動体移動・環境変化情報について、動体・環境変化情報統合部が処理していない、すなわち未処理の動体移動・環境変化情報が存在するか否かを判定する。未処理の動体移動・環境変化情報が存在する場合はステップＳ２０７０１に戻り、未処理の動体移動・環境変化情報がなくなるまで、処理を継続する。未処理の動体移動・環境変化情報が存在しない場合は、ステップＳ２０８に進む。

図３Ａに戻り、ステップＳ２０８において、地図情報の生成と移動ロボット１００の自己位置の推定（マッチング処理を含む）を行う。ステップＳ２０４で環境センサノード１２０からの返答があった場合は、ステップＳ２０７で動体移動・環境変化情報が統合された外界センサ１０１が取得した情報と、内界センサ１０２がステップＳ２０１で取得した情報とを用いて、地図生成・自己位置推定部１０４が、地図情報の生成と移動ロボット１００の自己位置の推定を行う。ここで、自己位置の推定の際には、地図情報と、外界センサが取得したセンサデータとを照らし合わせて（マッチング処理）、類似度の高い変換を探索し、最も類似度の高い変換をもって移動ロボット１００の移動距離および移動方向、すなわち現在の自己位置を推定する。ステップＳ２０４において環境センサノード１２０からの返答がなかった場合は、外界センサ１０１がステップＳ２０２にて取得した情報と、内界センサ１０２がステップＳ２０１にて取得した情報とを用いて、地図生成・自己位置推定部１０４は、地図情報の生成と移動ロボット１００の自己位置の推定を行う。地図情報の生成と自己位置の推定は地図生成・自己位置推定部１０４におけるフィルタリング処理によって同時に行われる。

次に、ステップＳ２０９において、経路計画部１０６は、地図情報データベース１０５に保存されている、ステップＳ２０８で生成された地図情報を用いて、現在の自己位置から移動する経路を計画する。経路計画の方法については上述の通りである。

次に、ステップＳ２１０において、制御部１０７は、ステップＳ２０９において計画された経路について、移動ロボット１００の移動動作を行うための制御命令を生成する。

次に、ステップＳ２１１において、アクチュエータ１０８は、ステップＳ２１０にて生成された制御命令を受けて実際に移動ロボット１００を動かす。

ここからは、環境センサノード１２０における処理である、ステップＳ２２１からＳ２２７について説明する。

処理フローＴ２２１ＡのステップＳ２２１において、環境センサノード１２０の通信部１２５は移動ロボット１００からの問合せを受け付ける。

ステップＳ２２３において、移動ロボット１００からの問合せ内容である、指定した場所、指定した時間帯という条件に当てはまる動体移動・環境変化情報が存在するか否か、動体移動・環境変化情報データベース１２４を用いて確認する。確認の結果、条件に当てはまる動体移動・環境変化情報が存在した場合は、ステップＳ２２５の処理に進む。条件に当てはまる動体移動・環境変化情報が存在しない場合は、ステップＳ２２１に戻る。

次に、ステップＳ２２５において、通信部１２５は移動ロボット１００に対して、ステップＳ２０３での指定した場所、時間帯に動体移動・環境変化情報が存在するか否かの問合せに対する返答（指定した場所の動体移動・環境変化情報が存在するか否かの情報）を送信する。

次に、ステップＳ２２６において、通信部１２５は、ステップＳ２２１で受け付けた問合せを行った移動ロボット１００に対して、動体移動・環境変化情報を送信する。送信を行った後はステップＳ２２１に戻る。

処理フロー２２１ＢのステップＳ２２２において、動体移動・環境変化情報抽出部１２２は、環境センサ１２１が検出した環境センサノード１２０の周辺の情報を取得し、環境センサ情報時系列データベース１２３に、検出時間と検出情報の組の形で時系列順に保存する。

ステップＳ２２４において、動体移動・環境変化情報抽出部１２２は、環境センサ情報時系列データベース１２３に保存されている、環境センサノード１２０の周辺に関する検出時間と検出情報の複数の組を時系列に沿って参照し、環境センサノード１２０の周辺の環境が変化しているか否かを判定する。

ステップＳ２２７にて、動体移動・環境変化情報抽出部１２２は、動体移動・環境変化情報抽出部１２２は、環境センサ情報時系列データベース１２３に保存されている検出時間と検出情報の時系列に沿った複数の組に基づいて、周辺環境の変化場所、変化形状、変化の大きさ及び変化にかかった時間を算出する。前記の変化場所、変化の大きさ、変化にかかった時間を動体移動・環境変化情報として動体移動・環境変化情報データベース１２４に保存する。

抽出された動体移動・環境変化情報は、図２に示すように、環境センサノードに搭載される通信手段３５１と移動ロボット１００に搭載される通信手段３５２によって通信経路３５３を通って環境センサノードから移動ロボットに送られる。移動ロボットに送られた動体移動・環境変化情報は、動体移動・環境変化情報統合部によって、人／動体による環境変化情報が存在する場所に対応する、移動ロボット１００の外界センサ１０１が検出した情報（第２の地図情報）における場所の情報を除去するために使用される。これにより、外乱である動的な環境変化を移動ロボット１００の外界センサ１０１が検出した情報から除去することが可能となる。

図４Ａは、例えば、任意の時刻Ｔにおいて移動ロボット１００が環境４００に存在していることを示す。図４Ｃに、時刻Ｔにおいて移動ロボット１００の外界センサ１０１が取得した情報（センサデータ）４３０を示す。図４Ｂに、時刻Ｔから任意の時間間隔αが経過した時刻Ｔ＋αにおいて移動ロボット１００が環境４００に存在していることを示す。図４Ｄに、時刻Ｔから任意の時間間隔αが経過した時刻Ｔ＋αにおける移動ロボット１００の外界センサ１０１が取得した情報（センサデータ）４３１を示す。

図４Ａに示すように、環境４００には、移動ロボット１００と、位置４０１に存在する静物体Ａと、位置４０２に存在する動体Ｈと、環境センサノード１２０が存在している。移動ロボット１００は、進行方向４０５に向かって移動している。動体Ｈは、進行方向４０４に向かって位置４０２から移動している。

図４Ｂに示すように、環境４００には、移動ロボット１００と、位置４０１に存在する静物体Ａと、位置４０３に存在する動体Ｈと、環境センサノード１２０が存在している。

図４Ａと図４Ｂを比較すると、移動ロボット１００の位置が変化している。また、動体Ｈの位置も、位置４０２から位置４０３に変化している。

移動ロボット１００に搭載される外界センサ１０１は、環境４００における移動ロボット１００の周辺の情報を取得する。

時刻Ｔにおいて、環境４００に対して、外界センサ１０１が取得したセンサデータ４３０（太実線）を、図４Ｃは示している。センサデータ４３０には、環境４００における、位置４０１に存在する静物体Ａと、位置４０２に存在する動体Ｈと、壁Ｗそれぞれの形状および外界センサ１０１からの距離が含まれている。

時刻Ｔから任意の時間間隔αが経過した時刻Ｔ＋αにおいて、環境４００に対して、外界センサ１０１が取得したセンサデータ４３１（太破線）を、図４Ｄは示している。センサデータ４３１には、環境４００における、位置４０１に存在する静物体Ａと、位置４０３に存在する動体Ｈと、壁Ｗそれぞれの形状および外界センサ１０１からの距離が含まれている。

センサデータ４３０とセンサデータ４３１は、２次元の距離データ、３次元の距離データ、２次元の形状データ、３次元の形状データ、画像特徴点、などのデータ形式もしくはそれぞれの全ての組合せを含み得る。すなわち、外界センサ１０１が取得する情報には座標情報が含まれる。環境センサノード１２０は、環境センサ１２１によって、動体Ｈが時刻Ｔから時刻Ｔ＋αの間に、位置４０２から位置４０３に移動したことによる環境変化を動体移動・環境変化情報として抽出する。図５Ａ〜図５Ｃに、時刻Ｔにおける地図データと時刻Ｔ＋αにおけるセンサデータとのマッチング処理と、その結果例を示す。

センサデータのマッチング処理は、上述したステップＳ２０８にて、地図生成・自己位置推定部１０４によって実行される。マッチング処理は、地図情報（データ）と、外界センサが取得したセンサデータとを照らし合わせて、類似度の高い変換を探索し、最も類似度の高い変換をもって、移動ロボット１００の、移動距離および移動方向、すなわち現在の自己位置を推定する。

一例として、センサデータ４３０をもとに作られた時刻Ｔにおける地図データ５０１（すなわち地図情報データベース１０５内の地図情報）と、時刻Ｔ＋αにおいて移動ロボット１００の外界センサ１０１によって取得されたセンサデータ４３１とのマッチング処理について説明する。

マッチング処理は、複数のデータを照らし合わせて、相違度が低い、すなわち類似度が高い複数のデータ間の変換を探索する処理である。マッチング処理は、相違度の勾配を計算し、相違度が極小となる変換を探索する。

しかし、探索する対象の形状が変形している場合または、データに外乱によるノイズが入っている場合に、あらかじめ定められた相違度の閾値を下回らず、探索が収束しないという問題が発生する。さらに、そのような場合に、本来の正解であるデータ間の対応とは違う誤った対応を最適な対応と判定し、変換の探索の精度が低下する問題が発生する。

マッチング処理による、地図データ５０１とセンサデータ４３１のマッチング結果は、図５Ｂに示す結果例５０３、結果例５０４のようになり得る。

結果例５０４は、地図データ５０１とセンサデータ４３１の対応としての正解（Ｇｒｏｕｎｄ ｔｒｕｔｈ）に非常に近い例を示している。

結果例５０４は、位置４０１から移動しない静物体Ａと壁の形状によるデータの部分（左側部分）が高い精度で対応しており、正解と高い精度で対応している。一方、動体Ｈが位置４０２から位置４０３へ移動したことによるセンサデータの形状が変化した部分（右側部分）の対応がうまくとれておらず、その結果として、センサデータ全体の類似度が低下している。

そのため、探索方法や閾値によっては、正解である結果例５０４へ収束するまでに、マッチング演算の繰り返し回数が増加するという問題が発生する。

図５Ｃに、マッチングにおける評価値例５０５を示す。評価値例５０５として、横軸にマッチングを行う２つのデータ５０１、４３１間のＸ軸方向の距離、縦軸に評価値をとりプロットした例を示す。評価値は、データ間の相違度を表し、データの形状が一致しているほど低い値となる。

ここで、理解を容易にするために、評価値例５０５は、地図データ５０１に対してセンサデータ４３１をＸ軸方向のみに並進させた場合の例を示している。なお、実際には、変換を探索するために、各軸に対するデータの並進、回転が行われる。並進とは、一方向に平行移動する運動を示す。

図５Ｃに示すように、マッチングにおける評価値例５０５には、結果例５０４に対するマッチング演算の評価値５０７が示されている。評価値５０７の付近では、評価値の変化がなだらかであり、勾配法等の手法を用いて結果を求める場合、結果へ収束するまでにマッチング演算の繰り返し回数が多数必要となる。

また、図５Ｂに示す結果例５０３は、地図データ５０１とセンサデータ４３１の対応としての正解（Ｇｒｏｕｎｄ ｔｒｕｔｈ）との対応度が低い例を示している。図５Ｃに示すように、マッチングにおける評価値例５０５には、結果例５０３に対するマッチング演算の評価値５０６が示されている。評価値５０６は、評価値例５０５において、極小値となっている。そのため、勾配法等の手法を用いて結果を求める場合、評価値５０６に結果が収束し、正解との対応度が低い対応である結果例５０３を誤って最適な結果と判定する。このように結果例５０３に収束し、正解との対応度が低い対応を最適と誤って判定する問題も発生し得る。

それにより、データ間の変換の探索精度が低下し、結果として、地図の生成精度とその地図に基づく移動ロボット１００の自己位置の推定精度とが低下する。

このように、動体の移動によるセンサデータの形状の変化が生じると、マッチング演算の繰り返し回数が増加し、正解との対応度が低い対応を最適と誤判断することによって地図の生成精度と移動ロボット１００の自己位置の推定精度が低下する問題が発生する。以下、この問題に対処する２つの処理について、図６Ａ−図６Ｃ、図７Ａ−図７Ｃを参照しながら説明する。

図６Ａには、１つ目の処理によって、すなわち動体Ｈの移動を原因として動体・環境変化情報が存在する場所（領域Ｃ）のデータを除去する処理によって、対象の場所のデータを除去する処理が行われた後の時刻Ｔの地図データ６０１と時刻Ｔ＋αのセンサデータ６０２とが示されている。

なお、図６Ａに示す処理は、ステップＳ２０７０５と同一の処理である。すなわち、図６Ａに示す領域Ｃは、時間的変化量が閾値Ｔｈ＿ｃｈより大きい場所に対応する。

図６Ｂと図６Ｃは、地図データ６０１とセンサデータ６０２のマッチング結果例とマッチング評価値例を示す。ここでは、理解を容易にするために、マッチング評価値例は、地図データ６０１に対してセンサデータ６０２をＸ軸方向のみに並進させた場合の例を示している。すなわち図６Ｃのグラフにおいて、横軸は２つのデータ６０１、６０２間のＸ軸方向の距離を示し、縦軸は評価値（２つのデータ６０１、６０２の相違度）を示している。

図６Ｃに示されるように、図６Ａに示す除去処理によれば、動体の移動を原因とするセンサデータの形状の変化による影響が除去されているので、評価値の変化が険しくなる。よって勾配法等の手法を用いて結果を求める場合、図５Ｃに示す評価値例５０５と比較して、図６Ｃに示す評価値例では、評価値５０６の点に対応する極小値が存在しない。すなわち、図６Ｃに示す評価値例では、評価値６０７の点に対応する最小値のみが極小値と存在する。この評価値例によれば、誤判断を回避し、正解への高精度な対応が可能である。さらに、評価値６０７の周辺の評価値の変化が、図５Ｃに示す評価値５０７と比較して急峻となっており、それによりマッチング演算の繰り返し回数が図５Ｃに示す評価値例５０５を用いる場合に比べて削減される。

以上により、地図生成・自己位置推定におけるデータ間のマッチング処理において、マッチング対象の２つのデータそれぞれにおける動体が存在する領域（図６Ａに示す領域Ｃ）の情報をマッチング処理実行前に除去することにより、マッチング処理の高精度化・演算量の低減が図れる。

次に、２つ目の処理について説明する。図７Ａには、動体Ｈの移動を原因として動体・環境変化情報が存在する場所（領域Ｃ）のデータの分散を増加させる処理によって、対象の場所のデータの分散を増加させる処理が行われた後の時刻Ｔの地図データ６０３と、時刻Ｔ＋αのセンサデータ６０４とが示されている。

ここで、情報の分散の増加について、図８−図１０を参照しながら補足する。

図８は、時刻Ｔにおける地図データ５０１を示している。図８に示すように、地図データ５０１には、移動ロボット１００の外界センサ１０１によって実測された実測点（実測座標）が含まれている。

その地図データ５０１の情報の分散は、実測点の分散とみなすことができ、その実測点の不確からしさ（その座標情報の信用度）を表している。図９に示すように、実測点の分散は、具体的には実測点をピークとする存在確率の分布の幅で定義することができる。したがって、情報の分散の増加は、実測点をピークとする存在確率の分布の幅の増加を意味する。

一例として、図１０には、地図データ５０１の一部分（右側部分）において、情報の分散の増加が実行された、すなわち実測点の存在確率の分布の幅が増加した様子が示されている。

なお、図７Ａに示す処理は、ステップＳ２０７０６と同一の処理である。すなわち、図７Ａに示す領域Ｃは、時間的変化量が閾値Ｔｈ＿ｃｈより大きい場所に対応する。

図７Ｂと図７Ｃに、地図データ６０３とセンサデータ６０４のマッチング結果例とマッチング評価値例を示す。ここでは、理解を容易にするために、マッチング評価値例は、地図データ６０３に対してセンサデータ６０４をＸ軸方向のみに並進させた場合の例を示している。

図７Ｃに示されるように、図７Ａに示す分散増加処理によれば、動体の移動を原因とするセンサデータの形状の変化による影響を緩和しているので評価値の変化が険しくなる。よって勾配法等の手法を用いて結果を求める場合、図５Ｃに示す評価値例５０５の評価値５０６の点に対応する極小値が評価値６０８の点に比べて充分に大きい値であるため、図５Ｃの評価値例５０５と比較して、評価値６０８へ収束しやすい。この評価例によれば、誤判断を回避しやすくなり、正解への高精度な対応が可能である。さらに、評価値６０８の周辺の評価値の変化が、図５Ｃに示す評価値５０７と比較して急峻となっており、それによりマッチング演算の繰り返し回数が図５Ｃに示す評価値例５０５を用いる場合に比べて削減される。

以上により、地図生成・自己位置推定におけるデータ間のマッチング処理において、マッチング対象の２つのデータそれぞれにおける動体が存在する領域（例えば図７Ａに示す領域Ｃ）の情報をマッチング処理実行前に分散増加することにより、マッチング処理の高精度化・演算量の低減が図れる。

図６Ａに示す情報の除去処理と図７Ａに示す情報の分散増加処理は、図３Ｂに示すように、条件に応じて使い分けられる。具体的には、第１のタイミングと第２のタイミングとの間の時間的変化量が閾値Ｔｈ＿ｃｈより大きい場所が存在する場合（そのような動体移動・環境変化情報が存在する場所がある場合）、且つ、その第１のタイミングと第２のタイミングとの間の時間差が閾値Ｔｈ＿ｔｂ（第２の時間）より小さい場合、すなわち動体の移動・環境の変化による影響が相対的に大きいと判断される場合に、ステップＳ２０７０５にて除去処理が実行される。一方、第１のタイミングと第２のタイミングとの間の時間差が閾値Ｔｈ＿ｔｂより大きい場合、すなわち動体の移動・環境の変化による影響が相対的に小さいと判断される場合に、ステップＳ２０７０６にて分散増加処理が実行される。

除去処理は、分散増加処理に比べて、動体の移動・環境の変化による影響をより小さくなるように抑制する処理である。このように、本実施の形態では、動体の移動・環境の変化による影響の程度によって除去処理と分散増加処理とを使い分け、それにより最適な手法を用いたデータ間のマッチング処理を実現している。

本実施の形態１によれば、環境変化が大きい空間においても、高精度かつロバストに、地図生成とその地図を用いて移動する移動ロボットの自己位置推定とを行うことができる。

（実施の形態２）
上述の実施の形態１に係るロボットシステムは、１つの移動ロボットと、少なくとも１つの環境センサノードとを含んでいる。これに対して、本実施の形態２に係るロボットシステムは、複数の移動ロボットと、少なくとも１つの環境センサノードとを含んでいる。そして、その複数の移動ロボットそれぞれが、上述の実施の形態１における環境センサノードと同様の機能を備える。すなわち、地図生成や自己位置推定を行う１つの移動ロボットに対して、残りの移動ロボットが環境センサノードとして機能することができる。

具体的には、本実施の形態２の移動ロボットそれぞれは、環境センサを用いて最終的に動体移動・環境変化情報を取得する上述の実施の形態１の環境センサノードと同様に、外界センサを用いて最終的に動体移動・環境変化情報を取得するように構成されている。また、移動ロボットは、図１に示す環境センサノードの動体移動・環境変化情報抽出部、環境センサ情報時系列データベース、および動体移動・環境変化情報データベースと実質的に同一の構成要素を含んでいる。

図１１に、本実施の形態２に係るロボットシステムにおける、複数の移動ロボットの処理フローを示す。

図１１に示すように、本実施の形態２に係る移動ロボットは、メイン処理フローＴ７０１と、サブ処理フローＴ７０２Ａ、Ｔ７０２Ｂとにしたがって動作するように構成されている。また、図１１は、メイン処理フローを実行するロボット（上述の実施の形態１の移動ロボット１００に対応）とサブ処理フローＴ７０２Ａ、Ｔ７０２Ｂを実行する少なくとも１つの別の移動ロボット（上述の実施の形態１の環境センサノード１２０として機能するロボット）との情報のやりとりが示されている。

図１１に示す処理フローを行う本実施の形態２に係るロボットシステムと図３Ａ、図３Ｂに示す上述の実施の形態１に係るロボットシステムとの違いは、動体移動・環境変化情報が、処理フローＴ７０２Ａ、Ｔ７０２Ｂを実行して環境センサノードの役割をする移動ロボットによって抽出され、そのロボットから、メイン処理フローを実行して地図生成および自己位置推定を行う移動ロボットに対して送信されること、環境センサノードの役割をする移動ロボットが外界センサを用いてその周辺の情報を取得すること、である。

本実施の形態２によれば、上述の実施の形態１と同様に、環境変化が大きい空間においても、高精度かつロバストに、地図生成とその地図を用いて移動する移動ロボットの自己位置推定とを行うことができる。また、移動可能な移動ロボットが環境センサノードの役割をすることで、動体移動・環境変化を検出することができるエリアが拡大し、それにともなって動体移動・環境変化情報の情報量が増加する（固定の環境センサノードのロボットシステムに比べて）。その結果として、地図生成・自己位置推定の処理における推定値の精度劣化の抑制と、外乱による計算量の増加の抑制とがより図られる。

なお、本実施の形態２の場合、複数の移動ロボットのいずれかが環境センサノードの役割を果たすので、少なくとも１つの環境センサノード自体をロボットシステムから除外することも可能である。すなわち、この場合、ロボットシステムは、環境センサノードを含まず、環境センサノードの役割を果たすことができる少なくとも２つの移動ロボットを含んでいる。

なお、上記態様において説明された技術は、例えば、以下のロボットシステムの類型において実現されうる。しかし、上記態様において説明された技術が実現されるロボットシステムの類型はこれらに限られるものでない。

以上、実施の形態にて本開示の処理について説明したが、各処理が実施される主体や装置に関しては特に限定しない。ローカルに配置された特定の装置内に組み込まれたプロセッサーなど（以下に説明）によって処理されてもよい。またローカルの装置と異なる場所に配置されているクラウドサーバなどによって処理されてもよい。また、ローカルの装置とクラウドサーバ間で情報の連携を行うことで、本開示にて説明した各処理を分担してもよい。以下本開示の実施態様を説明する。

（１）上記の装置は、具体的には、マイクロプロセッサ、ＲＯＭ、ＲＡＭ、ハードディスクユニット、ディスプレイユニット、キーボード、マウスなどから構成されるコンピュータシステムである。前記ＲＡＭまたはハードディスクユニットには、コンピュータプログラムが記憶されている。前記マイクロプロセッサが、前記コンピュータプログラムにしたがって動作することにより、各装置は、その機能を達成する。ここでコンピュータプログラムは、所定の機能を達成するために、コンピュータに対する指令を示す命令コードが複数個組み合わされて構成されたものである。

（２）上記の装置を構成する構成要素の一部または全部は、１個のシステムＬＳＩ（Ｌａｒｇｅ Ｓｃａｌｅ Ｉｎｔｅｇｒａｔｉｏｎ：大規模集積回路）から構成されているとしてもよい。システムＬＳＩは、複数の構成部を１個のチップ上に集積して製造された超多機能ＬＳＩであり、具体的には、マイクロプロセッサ、ＲＯＭ、ＲＡＭなどを含んで構成されるコンピュータシステムである。前記ＲＡＭには、コンピュータプログラムが記憶されている。前記マイクロプロセッサが、前記コンピュータプログラムにしたがって動作することにより、システムＬＳＩは、その機能を達成する。

（３）上記の装置を構成する構成要素の一部または全部は、各装置に脱着可能なＩＣカードまたは単体のモジュールから構成されているとしてもよい。前記ＩＣカードまたは前記モジュールは、マイクロプロセッサ、ＲＯＭ、ＲＡＭなどから構成されるコンピュータシステムである。前記ＩＣカードまたは前記モジュールは、上記の超多機能ＬＳＩを含むとしてもよい。マイクロプロセッサが、コンピュータプログラムにしたがって動作することにより、前記ＩＣカードまたは前記モジュールは、その機能を達成する。このＩＣカードまたはこのモジュールは、耐タンパ性を有するとしてもよい。

（４）本開示は、上記に示す方法であるとしてもよい。また、これらの方法をコンピュータにより実現するコンピュータプログラムであるとしてもよいし、前記コンピュータプログラムからなるデジタル信号であるとしてもよい。

（５）また、本開示は、前記コンピュータプログラムまたは前記デジタル信号をコンピュータで読み取り可能な記録媒体、例えば、フレキシブルディスク、ハードディスク、ＣＤ−ＲＯＭ、ＭＯ、ＤＶＤ、ＤＶＤ−ＲＯＭ、ＤＶＤ−ＲＡＭ、ＢＤ（Ｂｌｕ−ｒａｙ（登録商標） Ｄｉｓｃ）、半導体メモリなどに記録したものとしてもよい。また、これらの記録媒体に記録されている前記デジタル信号であるとしてもよい。

また、本開示は、前記コンピュータプログラムまたは前記デジタル信号を、電気通信回線、無線または有線通信回線、インターネットを代表とするネットワーク、データ放送等を経由して伝送するものとしてもよい。

また、本開示は、マイクロプロセッサとメモリを備えたコンピュータシステムであって、前記メモリは、上記コンピュータプログラムを記憶しており、前記マイクロプロセッサは、前記コンピュータプログラムにしたがって動作するとしてもよい。

また、前記プログラムまたは前記デジタル信号を前記記録媒体に記録して移送することにより、または前記プログラムまたは前記デジタル信号を、前記ネットワーク等を経由して移送することにより、独立した他のコンピュータシステムにより実施するとしてもよい。

（６）上記実施の形態及びその変形例をそれぞれ組み合わせるとしてもよい。

最後に、移動ロボットおよび環境センサノードは、様々な形態が可能である。例えば、図２に示す移動ロボット１００や環境センサノード１２０（３１２−３１９）は屋内で使用されるが、これに限らない。例えば、移動ロボットが屋外の道路などを自動走行する車両の形態であってもよい。この場合、環境センサノードは、道路標識のように道路に沿って配置される、または、道路近傍の建物に設けられる。

１００ 移動ロボット
１０１ 外界センサ
１０２ 内界センサ
１０３ 動体・環境変化情報統合部
１０４ 地図生成・自己位置推定部
１０５ 地図情報データベース
１０６ 経路計画部
１０７ 制御部
１０８ アクチュエータ
１０９ 通信部
１２０ 環境センサノード
１２１ 環境センサ
１２２ 動体移動・環境変化情報抽出部
１２３ 環境センサ情報時系列データベース
１２４ 動体移動・環境変化情報データベース
１２５ 通信部

Claims (11)

1. 少なくとも１つ以上の環境センサノードを用いて地図生成を行う移動ロボットの地図生成方法であって、
   前記移動ロボット周辺の情報を含む予め作成された第１の地図情報を取得し、
   前記移動ロボットに搭載された外界センサによって前記移動ロボット周辺の情報を含む第２の地図情報を取得し、
   前記環境センサノードから前記移動ロボット周辺の情報を含む第３の地図情報を受け取り、
   （ｉ）前記第３の地図情報に第１のタイミングと第２のタイミングとの間の時間における物体の存在確率の変化量、又は物体の特徴ベクトルの変化量である時間的変化量が所定の閾値以上の場所の情報が存在する場合、
   前記第３の地図情報における時間的変化量が前記所定の閾値以上の場所に対応する、前記第１の地図情報における場所の情報と前記第２の地図情報における場所の情報とを除去する除去処理を行う、または、
   前記第３の地図情報における時間的変化量が前記所定閾値以上の場所に対応する、前記第１の地図情報における場所の情報と前記第２の地図情報における場所の情報それぞれの分散を増加する分散増加処理を行い、
   （ｉｉ）前記除去処理または前記分散増加処理が行われた後の第１の地図情報と第２の地図情報とをマッチングし、そのマッチング結果に基づいて地図情報を生成し、
   （ｉｉｉ）マッチング結果に基づいて生成された地図情報に、前記第１の地図情報を更新する、地図生成方法。
   
2. 前記第３の地図情報に第１のタイミングと第２のタイミングとの間で前記時間的変化量が前記所定の閾値以上である場所の情報が存在する場合において、
   前記第１のタイミングと前記第２のタイミングとの間の時間差が第１の時間と前記第１の時間に比べて長い第２の時間との間の範囲内である場合には、前記除去処理を行い、
   前記第１のタイミングと前記第２のタイミングとの間の時間差が前記第２の時間以上である場合には、前記分散増加処理を行う、請求項１に記載の地図生成方法。
   
3. 前記第３の地図情報に前記第１のタイミングと前記第２のタイミングとの間で前記時間的変化量が前記所定の閾値以上である場所の情報が存在する場合において、
   前記第１のタイミングと前記第２のタイミングとの間の時間差が前記第１の時間以下である場合には、前記除去処理および前記分散増加処理を行わずに、前記第１の地図情報と前記第２の地図情報とをマッチングし、そのマッチング結果に基づいて地図情報を生成する、請求項２に記載の地図生成方法。
   
4. 前記第３の地図情報に前記時間的変化量が前記所定の閾値以上である場所の情報が存在しない場合、前記第１の地図情報と前記第２の地図情報とをマッチングし、そのマッチング結果に基づいて地図情報を生成する、請求項１から３のいずれか一項に記載の地図生成方法。
   
5. 前記分散増加処理が、前記第１の地図情報の対応場所と前記第２の地図情報の対応場所それぞれの情報の不確からしさを増加させる処理である、請求項１から４のいずれか一項に記載の地図生成方法。
   
6. 前記第３の地図情報が、前記環境センサノード周辺の物体の存在確率の情報を含み、
   前記時間的変化量が、前記存在確率の変化量である、請求項１から５のいずれか一項に記載の地図生成方法。
   
7. 前記第１の地図情報、前記第２の地図情報、および前記第３の地図情報が、二次元座標系における座標情報または三次元座標系における座標情報であって、
   前記マッチングを行う前に、前記第１の地図情報、前記第２の地図情報、および前記第３の地図情報それぞれの座標系を、共通の座標系に変換する座標変換処理を行う、請求項１から６のいずれか一項に記載の地図生成方法。
   
8. 少なくとも１つのセンサノードを用いて自己位置推定を行う移動ロボットの自己位置推定方法であって、
   前記移動ロボット周辺の情報を含む予め作成された第１の地図情報を取得し、
   前記移動ロボットに搭載された外界センサによって前記移動ロボット周辺の情報を含む第２の地図情報を取得し、
   環境センサノードから前記移動ロボット周辺の情報を含む第３の地図情報を受け取り、
   （ｉ）前記第３の地図情報に第１のタイミングと第２のタイミングとの間の時間における物体の存在確率の変化量、又は物体の特徴ベクトルの変化量である時間的変化量が所定の閾値以上の場所の情報が存在する場合、
   前記第３の地図情報における時間的変化量が前記所定の閾値以上の場所に対応する、前記第１の地図情報における場所の情報と前記第２の地図情報における場所の情報とを除去する除去処理を行う、または、
   前記第３の地図情報における時間的変化量が前記所定閾値以上の場所に対応する、前記第１の地図情報における場所の情報と前記第２の地図情報における場所の情報それぞれの分散を増加する分散増加処理を行い、
   （ｉｉ）前記除去処理または前記分散増加処理が行われた後の第１の地図情報と第２の地図情報とをマッチングし、そのマッチング結果に基づいて地図情報を生成し、
   （ｉｉｉ）マッチング結果に基づいて生成された地図情報に、前記第１の地図情報を更新し、
   （ｉｖ）更新済みの第１の地図情報と、前記移動ロボットの位置および姿勢の少なくとも一方を検出する内界センサの検出結果とに基づいて、前記更新済みの第１の地図情報上での前記移動ロボットの自己位置を推定する、自己位置推定方法。
   
9. 前記更新済みの第１の地図情報と推定された自己位置とに基づいて移動経路を算出し、
   前記移動経路に沿って前記移動ロボットを移動させる、請求項８に記載の自己位置推定方法。
   
10. 少なくとも１つの環境センサノードと移動ロボットとを含むロボットシステムであって、
    前記環境センサノードが、前記移動ロボット周辺の情報を含む第３の地図情報を取得し、
    前記移動ロボットが、
    前記移動ロボット周辺の情報を含む予め作成された第１の地図情報を記録しているデータベースと、
    前記移動ロボット周辺の情報を含む第２の地図情報を取得する外界センサと、
    前記環境センサノードと通信して前記第３の地図情報を取得する通信部と、
    （ｉ）前記第３の地図情報に第１のタイミングと第２のタイミングとの間の時間における物体の存在確率の変化量、又は物体の特徴ベクトルの変化量である時間的変化量が所定の閾値以上の場所の情報が存在する場合、
    前記第３の地図情報における時間的変化量が前記所定の閾値以上の場所に対応する、前記第１の地図情報における場所の情報と前記第２の地図情報における場所の情報とを除去する除去処理を行う、または、
    前記第３の地図情報における時間的変化量が前記所定閾値以上の場所に対応する、前記第１の地図情報における場所の情報と前記第２の地図情報における場所の情報それぞれの分散を増加する分散増加処理を行い、
    （ｉｉ）前記除去処理または前記分散増加処理が行われた後の第１の地図情報と第２の地図情報とをマッチングし、そのマッチング結果に基づいて地図情報を生成し、
    （ｉｉｉ）マッチング結果に基づいて生成された地図情報に、前記予め記録していた第１の地図情報を更新する情報処理部と、を備える、ロボットシステム。
    
11. 移動ロボットであって、
    前記移動ロボット周辺の情報を含む予め作成された第１の地図情報を記録しているデータベースと、
    前記移動ロボット周辺の情報を含む第２の地図情報を取得する外界センサと、
    前記移動ロボットの外部にあって且つ前記移動ロボット周辺の情報を含む第３の地図情報を取得する少なくとも１つの環境センサノードと通信して前記第３の地図情報を取得する通信部と、
    （ｉ）前記第３の地図情報に第１のタイミングと第２のタイミングとの間の時間における物体の存在確率の変化量、又は物体の特徴ベクトルの変化量である時間的変化量が所定の閾値以上の場所の情報が存在する場合、
    前記第３の地図情報における時間的変化量が前記所定の閾値以上の場所に対応する、前記第１の地図情報における場所の情報と前記第２の地図情報における場所の情報とを除去する除去処理を行う、または、
    前記第３の地図情報における時間的変化量が前記所定閾値以上の場所に対応する、前記第１の地図情報における場所の情報と前記第２の地図情報における場所の情報それぞれの分散を増加する分散増加処理を行い、
    （ｉｉ）前記除去処理または前記分散増加処理が行われた後の第１の地図情報と第２の地図情報とをマッチングし、そのマッチング結果に基づいて地図情報を生成し、
    （ｉｉｉ）マッチング結果に基づいて生成された地図情報に、前記予め記録していた第１の地図情報を更新する情報処理部と、を備える、移動ロボット。

Images