地図処理方法及びプログラム、並びにロボットシステム
【課題】地図処理方法及びプログラム、並びにロボットシステムにおいて、複数の推定器が推定した位置姿勢を高精度に統合することを目的とする。
【解決手段】互いに異なるセンサからの入力に基づいて自己位置を推定する複数の推定器の信頼度をこれらの推定器に対応する信頼性評価モデルを用いて評価する評価処理と、これらの複数の推定器からの推定結果に対して信頼性に応じた異なる統合処理を行う結合処理をコンピュータに実行させるように構成する。
【解決手段】互いに異なるセンサからの入力に基づいて自己位置を推定する複数の推定器の信頼度をこれらの推定器に対応する信頼性評価モデルを用いて評価する評価処理と、これらの複数の推定器からの推定結果に対して信頼性に応じた異なる統合処理を行う結合処理をコンピュータに実行させるように構成する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、地図処理方法及びプログラム、並びにロボットシステムに関する。本発明は、地図処理プログラムを格納したコンピュータ読み取り可能な記憶媒体にも関する。
【背景技術】
【０００２】
自律移動型ロボットは、実環境内で障害物、壁などと衝突すること無く自由に移動できるように、例えばロボットに搭載されたセンサの計測情報と環境地図との照合に基づいて自己位置の推定を行う。ロボットが実環境下で自己位置を確実、且つ、正確に推定するために、複数の異なる地図を併用する方式がある。また、環境地図を自動的に作成する技術としては、例えばＳＬＡＭ（Simultaneous Localization And Mapping）が知られている。
【０００３】
複数の地図を自動的に作成する方式は、一般的には以下の２つの方式に分類できる。第１の方式では、ある種類のセンサの計測情報に基づいて最初の地図を作成する。その後、他の種類のセンサの計測情報を最初の地図に足し加えて２番目の地図を作成し、以下同様にして３番目以降の地図を作成する。一方、第２の方式では、複数の異なる種類のセンサの計測情報を推定器に同時観測情報として入力して、複数の地図を同時に、且つ、自動的に作成する。
【０００４】
第１の方式の場合、２番目以降に作成される地図が最初に作成された地図に依存する。このため、２番目以降に作成された地図の精度は、最初に作成された地図の精度に依存してしまう。一方、第２の方式の場合、センサの種類が多くなると、推定器の観測空間が大きくなるため、推定器の計算量が多くなると共に計算時間がかかる。また、異なる種類のセンサは異なる計測レートを用いるため、使用するセンサの計測レートを最も遅い計測レートに合わせる必要がある。
【０００５】
しかし、複数の地図を同時に生成する場合、異なる種類のセンサの異なる誤差モデルを反映しながら各センサ独自の計測レートを用いることが望ましい。このように、独立して作成された複数の地図では、各地図の座標系が異なると共に、推定誤差によって夫々の地図が異なる歪みを含んでいる。このため、ロボットが自己位置の推定を行う段階では、各センサの計測情報と対応する地図とのマッチングにより推定された自己位置が、異なる座標系で表現されたものであるため、例えばロボットの移動経路を計画あるいは決定する経路計画（または、経路計画）などを行うためには推定した自己位置の座標系を統合（または、統一）する必要がある。
【０００６】
自律移動型ロボットのナビゲーションシステムにおいて、複数の自己位置推定器が推定した位置姿勢を統合する従来の統合方式では、例えば各自己位置推定器の推定結果を１つのベクトルにまとめ、統合処理の観測データとして入力することで確率的フィルタにより統合する。しかし、従来の統合方式では、以下のようなデメリットが生じてしまう。
【０００７】
第１に、確率的フィルタによる統合を行うために、各自己位置推定器の推定精度を表す推定結果の共分散行列をある同一の基準に合わせる必要がある。しかし、各自己位置推定器が独立に開発されているため、自己位置推定器間では共分散行列の計算方法や設定パラメータなどに違いがある。従って、このような共分散行列に基づく統合処理では、複数の自己位置推定器が独立して推定した自己位置の座標系を高精度に統合することは難しい。
【０００８】
第２に、例えばある自己位置推定器の推定精度が低下すると（例えば、推定処理が破綻した場合も含む）、統合結果が推定精度の低下した自己位置推定器の推定結果の影響を受けて統合性能が低下してしまう。
【先行技術文献】
【特許文献】
【０００９】
【特許文献１】特開２００７−７２５９２号公報
【発明の概要】
【発明が解決しようとする課題】
【００１０】
従来の統合方式では、複数の推定器が推定した位置姿勢を高精度に統合することは難しいという問題があった。
【００１１】
そこで、本発明は、複数の推定器が推定した位置姿勢を高精度に統合することのできる地図処理方法及びプログラム、並びにロボットシステムを提供することを目的とする。
【課題を解決するための手段】
【００１２】
本発明の一観点によれば、互いに異なるセンサからの入力に基づいて自己位置を推定する複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価処理と、前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合処理をコンピュータに実行させることを特徴とする地図処理方法が提供される。
【００１３】
本発明の一観点によれば、互いに異なるセンサからの入力に基づいて自己位置を推定する複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価手順と、前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合手順をコンピュータに実行させることを特徴とするプログラムが提供される。
【００１４】
本発明の一観点によれば、自律移動型ロボットに設けられた互いに異なるセンサと、前記異なるセンサからの入力に基づいて自己位置を推定する複数の推定器と、前記複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価部と、前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合部を備えたことを特徴とするロボットシステムが提供される。
【発明の効果】
【００１５】
開示の地図処理方法及びプログラム、並びにロボットシステムによれば、複数の推定器が推定した位置姿勢を高精度に統合することが可能になる。
【図面の簡単な説明】
【００１６】
【図１】ロボットの一例を示す斜視図である。
【図２】ロボットを含むロボットシステムの一例を示すブロック図である。
【図３】推定器側からの逐次的入力により統合ツリーが生長していく様子を説明する図である。
【図４】推定器側からの逐次的入力により統合ツリーが生長していく様子を説明する図である。
【図５】統合ツリーの規模を一定以下に抑えるための枝刈を説明する図である。
【図６】統合器の動作の概要を説明するフローチャートである。
【図７】統合ツリーの規模の増大を説明する図である。
【図８】統合ツリーの最新レイヤーから不良ノードを削除する処理を説明する図である。
【図９】統合ツリーのメンテナンスを説明する図である。
【図１０】統合器の動作をより詳細に説明するフローチャートである。
【図１１】経路評価を説明する図である。
【図１２】オドメトリからロボットの移動により生じた位置姿勢の不確実さを算出するフィルタ処理を説明する図である。
【図１３】隠れマルコフモデルを説明する図である。
【図１４】統合ツリーの一例を示す図である。
【図１５】各ノードに対する経路評価で得られる評価スコアの一例を示す図である。
【図１６】比較例における統合結果の一例を説明する図である。
【図１７】実施例における統合結果の一例を説明する図である。
【発明を実施するための形態】
【００１７】
開示の地図処理方法及びプログラム、並びにロボットシステムでは、統合器が各推定器からの入力情報の信頼性を評価して、異なる信頼性のレベルに応じた異なる方式で統合履歴と関連付ける。この関連付けにより、統合履歴を表した統合ツリーが時間的に更新され、規模が増大していく。信頼性のレベルとは、信頼性を数量化した信頼度をさらに例えば「高」、「中」、「低」の３段階に分けたものを言う。
【００１８】
また、統合ツリーの最新ノードから過去に辿って得られた推定経路を評価して下位のノードを削除することで、ツリー全体のメンテナンスを行いツリー規模を一定以下に維持するようにしても良い。
【００１９】
以下に、開示の地図処理方法及びプログラム、並びにロボットシステムの各実施例を図面と共に説明する。
【実施例】
【００２０】
図１は、ロボットの一例を示す斜視図である。図１に示すロボット１００は、例えばモータなどにより駆動される台車１０１、車輪１０２、各種内的センサ及び外的センサ、及び例えばＣＰＵ（Central Processing Unit）などのプロセッサにより形成された制御部を有する周知の基本構成を有する自律移動型ロボットで形成可能である。内的センサには、レーザレンジファインダ（ＬＲＦ：Laser Range Finder）２，３、及びカメラ３が含まれる。ＬＲＦ２とＬＲＦ３は、例えば異なる種類のＬＲＦであっても良い。異なる種類のＬＲＦには、計測範囲の異なるＬＲＦも含まれる。
【００２１】
図２は、ロボットを含むロボットシステムの一例を示すブロック図である。この例では、ロボットシステム２００は、自律移動型ロボット１００のナビゲーションシステムとしても機能する。ここでは説明の便宜上、推定器が３つの場合を説明するが、推定器は少なくとも２つあれば良い。
【００２２】
推定器４は、上記ＣＰＵで形成可能であり、ＬＲＦ１と記憶部５に格納された地図１Ａに基づいてロボット１００の自己位置を推定するソフトウェアモジュール（推定モジュール）を形成する。推定器６は、上記ＣＰＵで形成可能であり、ＬＲＦ２と記憶部７に格納された地図１Ｂに基づいてロボット１００の自己位置を推定するソフトウェアモジュールを形成する。推定器８は、上記ＣＰＵで形成可能であり、カメラ３と記憶部９に格納された地図１Ｃに基づいてロボット１００の自己位置を推定するソフトウェアモジュールを形成する。
【００２３】
統合器１０は、上記ＣＰＵで形成可能であり、推定器４，６，８の自己位置推定結果を統合するソフトウェアモジュール（統合モジュール）を形成する。統合器１０において、推定器信頼性評価部１１は、推定器４，６，８からの入力の信頼性を評価して、例えば「高」＞「中」＞「低」の３段階の信頼性のレベルに分ける。信頼性のレベルとは、信頼性を数量化した信頼度をさらに例えば「高」、「中」、「低」の３段階に分けたものを言う。各信頼性のレベルによって、後述する統合ツリーへの結合が異なる。結合部１２は、信頼性のレベルが「高」（以下、説明の便宜上、信頼度が「高」とも言う）の場合、推定器からの入力を統合するためのフィルタの観測値としてフィルタ状態の更新処理を行い、新しいノードを作成して統合ツリーに追加する、推定器データの統合ツリーへの結合処理を行う。信頼性のレベルが「低」の場合、オドメトリのみでフィルタ状態の更新処理を行い、新しいノードを作成して統合ツリーに追加する。信頼性のレベルが「中」の場合、「高」と「低」の場合で行った処理を全て行う。これにより、統合ツリーの規模が増大していく。
【００２４】
統合器１０において、結合良さの評価部１３は、統合ツリーの規模を一定以下にするための枝刈を必要に応じて行うために、統合ツリーへの結合の良さを評価する。この評価は、統合器１０において推定器からの入力を統合するフィルタが推定した位置姿勢からロボット１００の動きを算出して、算出した動きとオドメトリ（Odometry）部２０で計測した動きと比較して動きの尤度を算出することで行うことができる。オドメトリ部２０は、ロボット１００の移動量を、モータ１８または台車１０１の車輪１０２の回転を検出するエンコーダ、または、ロボット１００に設けられたジャイロセンサ（または、ジャイロスコープ）などの内的センサからの検出信号に基づいて計算する周知の構成を有する。図２では、エンコーダ及びジャイロセンサがエンコーダ・ジャイロ部１９に含まれる。
【００２５】
統合ツリーのメンテナンス部１４は、推定経路中の評価対象で下位のノードを統合ツリーから削除し、次に統合ツリー全体をスキャンして、子を持っていないノードを削除する。さらに１つの子だけを持っている親ノードを子ノードと併合する。統合ツリーは、記憶部１５に格納される。
【００２６】
統合器１０は、例えば、推定器４で推定された地図Ａの座標系の自己位置（または、推定器８で推定された地図Ｃの座標系の自己位置）と、推定器６で推定された地図Ｂの座標系の自己位置を統合する統合処理を行い、統合結果を例えばロボット１００の移動経路を計画あるいは決定する移動経路計画部１６に供給する。この例では、移動経路計画部１６が決定した移動経路は、上記ＣＰＵで形成可能な台車移動コントローラ１７に供給される。台車移動コントローラ１７は、移動経路に応じてモータ１８を駆動して車輪１０２を回線することで台車１０１を例えば図１の矢印方向へ移動させる。オドメトリ部２０で計算されたロボット１００の移動量は、各推定器４，６，８に供給される。
【００２７】
統合器１０は、例えば、推定器４で推定された地図Ａの座標系の自己位置と、推定器６で推定された地図Ｂの座標系の自己位置と、推定器８で推定された地図Ｃの座標系の自己位置を統合する統合処理を行っても良いことは言うまでもない。
【００２８】
ロボットシステム２００がロボット１００で形成されている場合、図２に示す各部はロボット１００に設けられる。一方、ロボットシステム２００がロボット１００及び当該ロボット１００を制御する当該ロボット１００とは別体の情報処理装置（すなわち、サーバなどのコンピュータ）で形成されている場合、ＬＲＦ１，２及びカメラ３以外の部分を情報処理装置に設けても、統合器１０を情報処理装置に設けても、統合器１０及び移動経路計画部１６を情報処理装置に設けても良い。情報処理装置は、ロボット１００と通信可能な通信部（または、インタフェース）、ＣＰＵなどのプロセッサ、及びプロセッサが実行するプログラム及び各種データを記憶する記憶部を有する周知の構成を有する汎用のコンピュータで形成可能である。
【００２９】
少なくとも統合器１０が行う統合処理を実行するプログラムを記憶する記憶部には、半導体記憶装置、磁気、光、光磁気記録媒体などを含む各種周知のコンピュータ読み取り可能な記憶媒体を使用可能である。
【００３０】
図３及び図４は、推定器側からの逐次的入力により統合ツリーが生長していく様子を説明する図である。図３は任意の時刻ｔ１，ｔ２での処理、図４は任意の時刻ｔ３，ｔ４での処理を夫々示し、ｔ１＜ｔ２＜ｔ３＜ｔ４である。つまり、時刻ｔ１が最も早く、時刻ｔ４が最も遅い時刻である。図３及び図４において、左側が推定器側の処理を示し、右側が統合器側の処理を示す。図３及び図４では説明の便宜上、推定器４で推定された地図Ａの座標系の自己位置と、推定器６で推定された地図Ｂの座標系の自己位置を統合する統合処理を行うものとする。また、推定器４，６を夫々推定器Ａ，Ｂと図示し、推定器４のＩＤはＡ、推定器６のＩＤはＢで示す。
【００３１】
図３に示す時刻ｔ１では、統合器１０は、ステップＳＴ１で推定器Ａ，Ｂから入力した推定値をツリーの持つ情報のうち観測値へコピーし、ノードの持つの情報のうち状態ベクトルへコピーする。また、ステップＳＴ１は、推定器Ａ，Ｂから入力したオドメトリをツリーの持つ情報のうちオドメトリへコピーし、図３にハッチング付きの○印で表すルートノード（Root Node）を生成する。さらに、ステップＳＴ１は、ノードの持つ情報のうち推定器Ａ，Ｂの隠れマルコフモデル（ＨＭＭ：Hidden Markov Model）を初期化する。以下の説明では、ルートノードはハッチング付きの○印で表し、子ノードはハッチング無しの○印で表す。
【００３２】
時刻ｔ２では、統合器１０は、ステップＳＴ２で推定器Ａ，Ｂから入力した推定値及びオドメトリをツリーの持つ情報の観測値及びオドメトリへコピーする。これにより、統合器１０は、ステップＳＴ３でオドメトリ及び状態ベクトルに基づきＨＭＭを用いて推定器Ｂへの信頼度を計算する。統合部１０は、ステップＳＴ４で計算された推定器Ｂへの信頼度が「高」、「中」、「低」であるか（この例では「中」）に応じて子ノードを追加し、各子ノードの状態ベクトルを更新すると共に、親ノード（この場合、ルートノード）のＨＭＭを各子ノードへコピーする。
【００３３】
図４に示す時刻ｔ３では、統合器１０は、ステップＳＴ５で推定器Ａ，Ｂから入力した推定値及びオドメトリをツリーの持つ情報の観測値及びオドメトリへコピーする。これにより、統合器１０は、ステップＳＴ６で各ノードについて、上記ステップＳＴ３と同様にＨＭＭを用いて推定器Ａへの信頼度を計算し、上記ステップＳＴ４と同様に信頼度によって各ノードに子ノードを追加し、子ノードの状態ベクトルを更新すると共に、親ノードのＨＭＭを各子ノードへコピーする。
【００３４】
時刻ｔ４では、統合器１０は、ステップＳＴ７で上記ステップＳＴ５と同様の処理を行い、ステップＳＴ８で上記ステップＳＴ６と同様の処理を行う。
【００３５】
このようにして、種類の異なる複数の内的センサからの入力（計測情報）に基づいて自己位置を推定する複数の推定器の信頼度をＨＭＭを用いて計算する。各推定器に対応するＨＭＭの計算に用いるパラメータは、当該推定器からの入力（推定値及びオドメトリ）に含まれる。また、信頼度に応じてルートノードに子ノードを追加することで、統合ツリーを作成する。従って、記憶部１５に格納される統合ツリーの規模が成長して増大していく。この統合ツリーに含まれる最新のノード（最新レイヤーのノード）から過去のノードを辿ってルートノードまでの経路を評価して、削除対象するべきノードを検出して削除することで、統合ツリーの規模を一定以下に抑えることもできる。
【００３６】
図５は、統合ツリーの規模を一定以下に抑えるための枝刈を説明する図である。図５中、ノードは図３及び図４と同様に図示されており、さらに、削除対象のノードを●印で表す。図５中、破線で囲まれたノードは、最新レイヤーのノードを示す。図５（ａ）に示す統合ツリーの推定経路の評価の結果、例えば図５（ｂ）に●印で表す不良ノードが検出されると、図５（ｃ）に示すようにこれらの不良ノードを削除する。次に、子ノードを持っていない図５（ｄ）に●印で表すノードを抽出する。この場合、子ノードを持っていないノードのうち、最新レイヤーに存在するノードは抽出の対象外とする。次に、図５（ｅ）に示すように、抽出された、子ノードを持っていないノードを削除する。さらに、図５（ｆ）に破線で囲んで示すように、１つの子だけを持つノードを抽出する。この場合も、１つの子だけを持つノードのうち、最新レイヤーに存在するノードは抽出の対象外とする。そして、図５（ｇ）に示すように、図５（ｆ）に破線で囲んで示す子ノードと親ノードを１つのノードにマージする。このようにして、この例では図５（ａ）に示す統合ツリーの規模を一定以下に抑えるための枝刈を行った結果、統合ツリーの規模は図５（ｇ）に示す如き規模となる。なお、統合ツリーの規模を一定以下に抑えるための枝刈の詳細については後述する。
【００３７】
図６は、統合器の動作の概要を説明するフローチャートである。図６において、統合器１０は一点鎖線で囲まれた処理を実行し、（１）〜（２８）は図６の各処理でやり取りされる情報の流れの一例を示す。ステップＳ１０４〜Ｓ１２０は、統合部１０を形成するＣＰＵの各機能が実行する処理（または、手順）に相当する。推定器４，６，８の計測値と地図Ａ，Ｂ，Ｃとに基づいて推定されたロボット１００の位置姿勢は、（１），（２），（３）で示すように統合器１０に入力される。
【００３８】
統合器１０は、初めて推定器４からの入力を受け取った時、ｔ＝０として統合ツリーを初期化する。具体的には、図５に示したようなルートノードを１つ生成して、推定器４からの表１に示す如き入力をルートノードにコピーすると共に、表２に如きデータセットを持たせる。表２中、Ｎはロボットシステム２００が有する推定器に任意に割り当てられた「１」から開始する連番を示し、この例では推定器のＩＤ番号とは異なる。この例では、３つの推定器４，６，８を用いているので、表２において「推定器[1]」は推定器４、「推定器[2]」は推定器６、「推定器[N]」は推定器８を表す。
【００３９】
【表１】
【００４０】
【表２】
【００４１】
（１） 推定器４は、表１の如きデータを統合器１０に入力する。
【００４２】
（２） 推定器６は、表１の如きデータを統合器１０に入力する。
【００４３】
（３） 推定器８は、表１の如きデータを統合器１０に入力する。
【００４４】
（４） ステップＳ１０４では、統合器１０が推定器４，６，８からの最新入力を待ち合わせる入力待ち状態となり、新しい入力があるか否かを示すフラグｆ１を管理する。上記の如く、統合器１０への入力は表１に示す如き内容のデータである。ステップＳ１０５は、フラグｆ１が推定器からの新しい入力があることを示すと、新たな入力値を共有メモリに書き込む。共有メモリは、統合器１０内の記憶部で形成可能である。
【００４５】
（５） ステップＳ１０６では、統合器１０が共有メモリから入力値を読み出し、統合ツリーを初期化するか否かを判定する。統合器１０が初めて推定器４，６，８からの入力を受け取った場合、判定結果はＹＥＳであり上記の如く統合ツリーの初期化を行う。
【００４６】
（６） 統合器１０は、初期化を行うか否かを示すフラグｆ２と共有メモリから読み出した推定器からの入力値を後述するステップＳ１０７に入力する。
【００４７】
（８） 統合器１０は、推定器からの入力に含まれているオドメトリ成分を後述するステップＳ１１０に入力する。
【００４８】
（９） 統合器１０は、推定器からの入力に含まれているロボット１００の位置姿勢の推定結果成分を後述するステップＳ１１１に入力する。
【００４９】
（１０） 統合器１０は、推定器からの入力に含まれている推定器のＩＤ、及びロボット１００の位置姿勢の推定結果成分を後述するステップＳ１１２に入力する。
【００５０】
ステップＳ１０７では、統合器１０が上記の如く統合ツリーを初期化する。
【００５１】
（７） ステップＳ１０８では、統合器１０が統合ツリーを統合器１０内の記憶部で形成可能な共有メモリに保持する。
【００５２】
（１１） 統合器１０は、共有メモリに保持された統合ツリーをステップＳ１０９に入力する。
【００５３】
（１９） 統合器１０は、共有メモリに保持された統合ツリーを後述するステップＳ１１５に入力する。
【００５４】
（１２） ステップＳ１０９では、統合器１０が統合ツリーの最新レイヤーの全ノードを取得する。統合ツリーが既に初期化されていれば、最新レイヤーにある全てのノードをリストとして取得する。統合器１０は、最新レイヤーから抽出したノードのリストをステップＳ１１０に入力する。ステップＳ１１０では、統合器１０がレイヤー上の全ノードの状態をオドメトリによる予測に基づいて更新する。具体的には、ステップＳ１０９で取得したノードのリスト（ノードリスト）の全てのノード（メンバー）に対して、図１０と共に後述する式（１１）、式（１２）に基づいて予測更新を行う。
【００５５】
（１３） 統合器１０は、図１０と共に後述する式（２０）を計算するための予測更新したフィルタ状態、及び予測した観測値の共分散行列をステップＳ１１１に入力する。ステップＳ１１１は、各ノードの持つＨＭＭによる入力値の信頼性評価を図１０と共に後述する式（２１）〜（２４）に基づいて行い、推定器から入力したロボット１００の位置姿勢の推定結果の信頼性のレベルを計算する。
【００５６】
（１４） 位置姿勢の推定結果の信頼性のレベルが「高」であると、統合器１０は、信頼性のレベル、図１０と共に後述する式（１８），（１９）を計算するための予測更新したフィルタ状態、予測した観測値、及び予測した観測値の共分散行列をステップＳ１１２に入力する。信頼性のレベルが「高」であると、ステップＳ１１２では、統合器１０が信頼性のレベルが「高」と評価されたノードについてノードの状態を図１０と共に後述する式（１８），（１９）に基づいて更新する。
【００５７】
（１５） 位置姿勢の推定結果の信頼性のレベルが「低」であると、統合器１０は、信頼性のレベル、及び予測更新したフィルタ状態を後述するステップＳ１１３に入力する。
【００５８】
（１５−１） 位置姿勢の推定結果の信頼性のレベルが「中」であると、統合器１０は、信頼性のレベル、図１０と共に後述する式（１８），（１９）を計算するための予測更新したフィルタ状態、予測した観測値、及び予測した観測値の共分散行列をステップＳ１１２に入力する。信頼性のレベルが「中」であると、ステップＳ１１２では、統合器１０が信頼性のレベルが「中」と評価されたノードについてノードの状態を図１０と共に後述する式（１８），（１９）に基づいて更新する。
【００５９】
（１５−２） 位置姿勢の推定結果の信頼性のレベルが「中」であると、統合器１０はさらに、信頼性のレベル、及び予測更新したフィルタ状態を後述するステップＳ１１３に入力する。
【００６０】
（１６） 統合器１０は、観測更新したフィルタ状態をステップＳ１１３に入力する。ステップＳ１１３は、親ノードに追加する子ノードを生成する。
【００６１】
（１７） 統合器１０は、新しいフィルタ状態を持った子ノードのリストをステップＳ１１４に入力する。これにより、ステップＳ１１４では、統合器１０が生成した子ノードを各親ノードに追加する。信頼性のレベルが「高」と評価されたノードには１つだけの子ノードを追加する。信頼性のレベルが「低」と評価されたノードにも１つだけの子ノードを追加する。信頼性のレベルが「中」と評価されたノードには２つの子ノードを追加する。
【００６２】
図７は、統合ツリーの規模の増大を説明する図である。図７中、ＨＬ，ＭＬ，ＬＬは、夫々信頼性のレベルが「高」、「中」、「低」のノードを示す。例えば時刻ｔ＋１に推定器からの最新の入力があった時に、統合ツリーから時刻ｔに作成したノードを全て抽出する。ただし、ｔ＋１＝１の場合、抽出されるノードはルートノードのみとなる。次に、各ノードに対して以下の処理を行う。ここで、新しいノードΦを１つ作成しておき、現在処理しているノードをΨで表すものとする。ノードΨからオドメトリを抽出して、入力された最新のオドメトリと合わせて、図１０と共に後述する式（１）〜（６）のようにノードΨに保持している推定状態を更新する。更新した推定情報は、ノードΦに保存し、これと同時に、最新のオドメトリもノードΦに保存する。信頼性のレベルは、図１０と共に供述する式（２１）〜（２４）に基づいて計算することができる。
【００６３】
信頼性のレベルが「高（ＨＬ）」の場合、図１０と共に後述する式（１６）〜（１９）に基づいて新しいノードΦのフィルタ状態の観測更新を行って、ノードΨに図７中時刻ｔ＋１の左側に示すように子ノードとして追加する。信頼性のレベルが「低（ＬＬ）」の場合、新しいノードΦのフィルタ状態の観測更新は行わず、ノードΨに図７中時刻ｔ＋１の中央に示すように子ノードとして直接追加する。信頼性のレベルが「中（ＭＬ）」の場合、図７中時刻ｔ＋１の右側に示すように新しいノードΦをコピーして、もう１つのノードΦ’を作成する。ノードΦ’のフィルタ状態の観測更新を行い、ノードΨに子ノードとして追加する。また、ノードΦのフィルタ状態の観測更新は行わず、ノードΨに２番目の子ノードとして直接追加する。
【００６４】
（１８） 統合器１０は、子ノードを追加した統合ツリーをステップＳ１０８に入力する。ステップＳ１０８では、統合器１０が統合ツリーを保持するメモリを更新する。統合ツリーは、統合器１０内の記憶部で形成可能である。
【００６５】
（１９） 統合器１０は、メモリから統合ツリーを読み出してステップＳ１１５に入力する。ステップＳ１１５は、統合ツリーに含まれている最新レイヤー上にある全てのノードをリストとして取得する。ステップＳ１１５で取得されるノードは、ステップＳ１１４で統合ツリーに新たに追加した子ノードである。
【００６６】
（２０） 統合器１０は、最新ノード、すなわち、ステップＳ１１４で新たに追加した子ノードを、ステップＳ１１６に入力する。ステップＳ１１６では、統合器１０がリスト内の各ノードに対して、ルートノードまで辿って得られた全経路を抽出する。
【００６７】
（２１） 統合器１０は、抽出した全経路のリストをステップＳ１１７に入力する。ステップＳ１１７では、統合器１０が抽出した各経路に対して、オドメトリで測定した運動パターンとフィルタで推定した運動パターンを比較して、図１０と共に後述する式（２５）に基づいてリスト中の経路を評価する経路評価を行う。
【００６８】
（２２） 統合器１０は、経路評価により得られた経路の良さを表す評価スコアのうち、評価スコアが閾値より低い下位のノードのリストをステップＳ１１８入力する。ステップＳ１１８では、統合器１０が下位のノードを統合ツリーから削除する。
【００６９】
図８は、統合ツリーの最新レイヤーから不良ノードを削除する処理を説明する図である。最新レイヤーにＫ個のノードが含まれているとすると、Ｋが事前に設定した閾値Ｋthreshより大きくなるとノードの削除処理を行う。この場合、Ｋ−Ｋthresh個のノードを削除することになる。各ノードの対応する経路の評価スコアは、例えばΩ（Ｐ（Φｋ）），ｋ＝０，１，．．．，ｋ，．．．Ｋで表される。そこで、ノードを評価スコアΩ（Ｐ（Φｋ））の昇順でソートし、下位のＫ−Ｋthresh個のノードを最新レイヤーから削除することで、統合ツリーは図８（ａ）に示す状態から図８（ｂ）に示す状態に変化する。図８（ａ）では、削除対象の不良ノードを●印で表す。
【００７０】
（２３） 統合器１０は、ステップＳ１１７の経路評価により得られた経路の良さを表す評価スコアをステップＳ１２０に入力する。ステップＳ１２０では、統合器１０が統合結果と信頼性の評価結果を出力する。具体的には、経路の評価スコアのトップノードが持っているフィルタ推定値を統合ツリーの処理結果とし、後述するアプリケーションに出力する。また、これと同時に、この処理結果と推定した信頼性のレベルを推定モジュールを形成する推定器にフィードバックする。
【００７１】
（２４） 統合器１０は、ステップＳ１１８により下位ノードが削除された統合ツリーをステップＳ１０８に入力して、統合ツリーを保持するメモリを更新する。
【００７２】
なお、ステップＳ１１９では、統合器１０が統合ツリーのメンテナンスを行う。図９は、統合ツリーのメンテナンスを説明する図である。統合ツリーのメンテナンスでは、図９（ａ）に示すように、最新レイヤーを除き、子ノードを持っていないノードを統合ツリーから抽出し、図９（ｂ）に示すように抽出した子ノードを統合ツリーから削除する。また、図９（ｃ）に破線で囲んで示すように、最新レイヤーを除き、１つの子だけを持つノードを統合ツリーから抽出する。そして、図９（ｄ）に示すように、図９（ｃ）に破線で囲んで示す子ノードと親ノードを１つの新しいノードにマージすることで、子ノードの保持しているデータを新しいノードのデータとする。このような統合ツリーのメンテナンスは、図５（ｄ）〜図５（ｆ）と共に説明した統合ツリーの規模を一定以下に抑えるための枝刈と同様である。
【００７３】
（２５） 統合器１０は、メンテナンスを行った統合ツリーをステップＳ１０８に入力してメモリに保持する。
【００７４】
（２６） 統合器１０は、経路の評価スコアのトップノードが持っているフィルタ推定値を統合ツリーの処理結果として、例えばアプリケーションを実行するステップＳ１２１に入力する。これにより、アプリケーションには、統合ツリー方式で推定したロボット１００の位置姿勢の情報を入力できる。アプリケーションは、例えば統合ツリーから推定したロボット１００の位置姿勢を用いてロボット１００の移動経路を計画あるいは決定する移動経路計画部１６が実行するものである。
【００７５】
（２７） 統合器１０は、上記の一連の統合処理が終了したことを示すフラグｆ３をステップＳ１０４に入力する。
【００７６】
（２８） 統合器１０は、統合ツリー方式で推定したロボット１００の位置姿勢、推定した信頼性のレベルを、推定モジュールへの診断結果として、推定モジュールを形成する推定器にフィードバックする。
【００７７】
図１０は、統合器の動作をより詳細に説明するフローチャートである。図１０において、 (1)〜(48)は図１０の各ステップでやり取りされる情報の流れの一例を示す。ステップＳ２０１〜Ｓ２３４は、統合部１０を形成するＣＰＵの各機能が実行する処理（または、手順）に相当する。ステップＳ２０１は、ロボット１００またはロボット１００とは別体の情報処理装置から入力される統合処理開始コマンドに応答して統合処理を開始する。
【００７８】
(1) 統合処理の開始を示す開始コマンドは、ステップＳ２０２に入力され、ステップＳ２０２は、推定器４，６，８からの入力待ち状態となり、推定器４，６，８から新しい入力があるか否かをチェックする。
【００７９】
(2) 新しい入力があると、新しい入力があることを示すフラグｆ１をステップＳ２０３に入力する。ステップＳ２０３は、入力されたフラグが終了フラグであるか否かを判定する。ステップＳ２０３の判定結果がＮＯであると、フラグをステップＳ２０５に入力する。他方、ステップＳ２０３の判定結果がＹＥＳであると、ステップＳ２０４で統合処理は終了する。
【００８０】
ステップＳ２０５は、入力されたフラグが新しい入力を示すフラグｆ１であるか否かを判定する。ステップＳ２０５の判定結果がＹＥＳであると処理はステップＳ２０６へ進み、判定結果がＮＯであると処理はステップＳ２０２へ戻る。
【００８１】
(5) ステップＳ２０５の判定結果がＮＯであると、新しい入力の待ちコマンドをステップＳ２０２に入力する。
【００８２】
(6) ステップＳ２０５の判定結果がＹＥＳであると、推定器からの入力データを例えば上記の表１の如き形式でステップＳ２０６に入力する。ステップＳ２０６は、このような推定器からの入力データから推定器の推定情報（すなわち、推定結果）だけを抽出する。
【００８３】
(7) ステップＳ２０６の後、データの抽出完了フラグｆ４、及び推定器の入力データをステップＳ２０７に入力する。ステップＳ２０７は、例えば上記の表１の如き形式の推定器の入力データからオドメトリを抽出する。
【００８４】
(8) ステップＳ２０６の後、推定器が推定したロボット１００の位置姿勢、及び推定器の推定精度を表す推定結果の共分散行列を後述するステップＳ２２３に入力する。
【００８５】
(10) ステップＳ２０６の後、上記(8)と同様に、推定器が推定したロボット１００の位置姿勢、及び推定器の推定精度を表す推定結果の共分散行列を後述するステップＳ２１８に入力する。
【００８６】
(9) ステップＳ２０７の後、オドメトリの抽出が完了したことを示すフラグｆ５、及び推定器の入力データをステップＳ２０８に入力する。ステップＳ２０８は、入力元の推定器のＩＤ番号を識別して抽出する。
【００８７】
(21) ステップＳ２０７の後、オドメトリを後述するステップＳ２１６に入力する。
【００８８】
(11) ステップＳ２０８の後、推定器のＩＤ番号の抽出が完了したことを示すフラグｆ６をステップＳ２０９に入力する。ステップＳ２０９は、統合ツリーの最新レイヤーにあるノードを全て抽出して、抽出したノードのノードリストを作成する。ここで、最新レイヤーとは、前回の処理で統合ツリーに追加した新しいノードを含むレイヤーを言う。
【００８９】
(12) ステップＳ２０８の後、抽出した推定器のＩＤ番号を後述するステップＳ２１４に入力する。
【００９０】
(13) 抽出された、最新レイヤーにあるノードの数（ノード数）をステップＳ２１０に入力する。ステップＳ２１０は、ノード数がゼロ（０）であるか否かを判定し、判定結果がＹＥＳであれば統合ツリーが構築されていないことが分かるので処理はステップＳ２１１へ進み、判定結果がＮＯであれば処理はステップＳ２１２へ進む。
【００９１】
(14) ステップＳ２１０の判定結果がＹＥＳであると、統合ツリーの初期化コマンドをステップＳ２１１に入力する。ステップＳ２１１は、初期化コマンドに応答して統合ツリーを上記の如く初期化する。
【００９２】
(15) ステップＳ２１１の後、入力待ちコマンドをステップＳ２０２に入力する。
【００９３】
(16) ステップＳ２１０の判定結果がＮＯであると、作成したノードリストをステップＳ２１２に入力する。ステップＳ２１２は、ノードリストからノードを１つ抽出して以下に説明するステップＳ２１３，Ｓ２１４，Ｓ２１５の処理を開始し、抽出したノードをＮＤとする処理を、ノードリスト中の未処理の各ノードに対して行う。
【００９４】
(17) ステップＳ２１２の後、上記の表２の如きデータセットを持つノードＮＤをステップＳ２１３に入力する。
【００９５】
(18) ステップＳ２１２の後、上記(17)と同様にして、上記の表２の如きデータセットを持つノードＮＤをステップＳ２１４に入力する。
【００９６】
(19) ステップＳ２１２の後、上記(17)と同様にして、上記の表２の如きデータセットを持つノードＮＤをステップＳ２１５に入力する。
【００９７】
ステップＳ２１３は、ノードＮＤの持つフィルタデータからこれまでにフィルタが推定したロボット１００の位置姿勢、共分散行列、及びオドメトリを抽出する。
【００９８】
(20) ステップＳ２１３の後、フィルタがこれまでに推定したロボット１００の位置姿勢、及びオドメトリをステップＳ２１６に入力する。
【００９９】
ステップＳ２１４は、ステップＳ２０８の後(12)で入力した推定器のＩＤ番号を使ってノードＮＤから推定器ＩＤに対応したＨＭＭを抽出する。ノードＮＤは、推定器数分のＨＭＭを持っているが、この処理では、統合器１０にデータを出力した推定器の対応するＨＭＭだけを抽出する。
【０１００】
(25) ステップＳ２１４の後、ＨＭＭデータ、すなわち、これまで計算した状態「１」にある確率、状態「２」にある確率をステップＳ２１８に入力する。ＨＭＭデータについては、図１３と共に供述する。
【０１０１】
ステップＳ２１５は、ノードからこれまでの経路の評価スコアを抽出する。具体的には、以下の経路評価処理を行うために、ノードＮＤに保存されているルートノードからの経路に対する評価スコアを抽出する。
【０１０２】
【数１】
【０１０３】
(48) ステップＳ２１５の後、ノードＮＤまでの経路評価スコアを後述するステップＳ２２９に入力する。
【０１０４】
ステップＳ２１６は、オドメトリで位置姿勢を予測更新する。具体的には、(21)で入力した新しいオドメトリと、(20)でノードＮＤに保存されているオドメトリを使って、以下に説明する式（１５）までの処理を行う。
【０１０５】
【数２】
【０１０６】
【数３】
【０１０７】
【数４】
【０１０８】
【数５】
【０１０９】
【数６】
【０１１０】
図１３は、隠れマルコフモデル（ＨＭＭ）を説明する図である。ＨＭＭは、各推定器の性能に合わせて構築される。図１３に示すようなHMMでは、状態「１」は推定器の入力が信頼できる状態、状態「２」推定器の入力が信頼できない状態を表す。観測シンボル「０」は、推定器の入力がフィルタの予測ゲート外にあることを示し、観測シンボル「１」は推定器の入力がフィルタの予測ゲート内にあることを示す。
【０１１１】
【数７】
【０１１２】
次に、ＨＭＭのパラメータの設定について説明する。ＨＭＭの状態遷移確率というパラメータは、ＨＭＭを使って評価する対象の推定器の性能によって決める。例えば、ＬＲＦによる自己位置推定と、カメラによる（例えば、画像ランドマークを用いた）自己位置推定を行う２つの推定器（図１の推定器４または６と、推定器８に相当）があるものとする。この場合、２つの推定器は以下のような性質ｃ１，ｃ２を有する。
【０１１３】
ｃ１：ＬＲＦの計測情報を使って自己位置推定を行う際、前回のロボット１００の推定位置の近傍で今回の新しい位置を探す。この場合、前回の推定位置が間違っていたら、今回探した結果も間違いである可能性が高い。
【０１１４】
ｃ２：逆に、画像ランドマークを使う場合、前回のロボット１００の推定位置が間違っていたとしても、今回推定した結果は真の位置に近づいていく。
【０１１５】
このため、状態遷移マトリクスでは、例えば
ＬＲＦの場合： a00=0.90, a01=0.10, a10=0.01,a11=0.99;
カメラの場合： a00=0.95, a01=0.05, a10=0.10,a11=0.90;
に設定される。ここで、
a00： 前回は信頼でき、今回も信頼できる確率を示し、ＬＲＦとカメラの特性からカメラの方はより高く設定されている。
a01： 前回は信頼でき、今回は信頼できない確率を示し、a00+a01=1となるように設定している。
a10： 前回は信頼できず、今回は信頼できる確率（リカバリー性能）を示し、ＬＲＦの場合はリカバリー機能が弱いため、カメラに比べで低く設定されている。
a11： 前回は信頼できず、今回も信頼できない確率を示し、a10+a11=1となるように設定される。
図１０のステップＳ２１６以降の処理の説明に戻る。
【０１１６】
(22) ステップＳ２１６の後、推定器の推定値に対しての予測値、及び予測ゲート（上記の式（１４）と式（１５）の計算結果）のデータをステップＳ２１８に入力する。
【０１１７】
(23) ステップＳ２１６の後、上記の式（１１）と式（１２）の計算結果のデータをステップＳ２１７に入力する。ステップＳ２１７は、新しいノードを１つ作成し、そのノードに(23)で入力されるデータを保存する。
【０１１８】
(24) ステップＳ２１７の後、新しいノードのデータをステップＳ２２２に入力する。
【０１１９】
ステップＳ２１８は、上記の式（２０）に基づいて入力した推定器の推定値の尤度を計算し、尤度を上記の式（２２）のように２値化することで、観測シンボルを生成する。
【０１２０】
(26) ステップＳ２１８の後、２値化の結果である観測シンボルをステップＳ２１９に入力する。ステップＳ２１９は、上記の式（２１）に基づいてＨＭＭの各状態を更新する。
【０１２１】
(27) ステップＳ２１９の後、上記の式（２１）の計算結果から得られたＨＭＭの各状態にある尤度をステップＳ２２０に入力する。ステップＳ２２０は、以下の式（２３）に基づいて信頼性を算出し、以下の式（２４）に基づいて信頼性のレベルを算出する。
【０１２２】
【数８】
【０１２３】
(28) ステップＳ２２０の後、式（２４）に基づいて算出された信頼性のレベルεｔをステップＳ２２１に入力する。ステップＳ２２１は、信頼性のレベルεｔがＨＬ（高）、ＭＬ（中）、ＬＬ（低）のいずれかであるかを判定する。ステップＳ２２１の判定結果がＨＬであると、処理はステップＳ２２２へ進む。ステップＳ２２１の判定結果がＬＬであると、処理はステップＳ２２４へ進む。また、ステップＳ２２１の判定結果がＭＬであると、処理はステップＳ２２２及びステップＳ２２４へ進む。
【０１２４】
(29) ステップＳ２２１の判定結果がＨＬであると、ＨＬを示す処理フラグをステップＳ２２２に入力する。
【０１２５】
(30) ステップＳ２２１の判定結果がＬＬであると、ＬＬを示す処理フラグをステップＳ２２４に入力する。
【０１２６】
(31) ステップＳ２２１の判定結果がＭＬであると、ＭＬを示す処理フラグをステップＳ２２２及びステップＳ２２４に入力する。
【０１２７】
ステップＳ２２２は、新しいノードを１つ作成して(24)で入力されたデータをそのノードに保存することで、新しいノードを複製する。
【０１２８】
(32) ステップＳ２２２の後、複製したノードのデータをステップＳ２２３に入力する。ステップＳ２２３は、上記の式（１６）〜式（１９）に基づいて複製したノードの状態を更新すると共に、式（１８）及び式（１９）の計算結果のデータを複製したノードに保存する。
【０１２９】
(33) ステップＳ２２３の後、状態を更新した複製ノードのデータをステップＳ２２５に入力する。
【０１３０】
一方、ステップＳ２２４は、新しいノードを１つ作成して(24)で入力されたデータをそのノードに保存することで、新しいノードを複製する。
【０１３１】
(34) ステップＳ２２４の後、新しいノードに保存したデータをステップＳ２２５に入力する。ステップＳ２２５は、図７と共に説明したように、ＨＬの場合はステップＳ２２３で処理したノード１つ、ＬＬの場合はステップＳ２２４で複製したノード１つを親ノードＮＤに子ノードとして追加し、ＭＬの場合がステップＳ２２３及びステップＳ２２３で処理した２つのノードを親ノードＮＤの子ノードとして追加する、ノードを子ノードとして親ノードに追加する処理を行う。
【０１３２】
(35) ステップＳ２２５の後、処理したノードＮＤのデータをステップＳ２２６に入力する。ステップＳ２２６は、全てのノードの処理が完了したか否かを判定し、判定結果がＮＯであると(36)で処理はステップＳ２１２へ戻り、判定結果がＹＥＳであると処理はステップＳ２２７へ進む。具体的には、ステップＳ２２６は、(16)で入力されたノードリストからノードＮＤを削除し、ノードリスト中の残りのノード数がゼロ（０）であるか否かを判定する。残りのノード数がゼロであれば判定結果がＹＥＳであり、残りのノード数がゼロでなければ判定結果がＮＯである。
【０１３３】
(37) ステップＳ２２６の判定結果がＹＥＳであると、全てのノードの処理が完了したことを示す処理完了フラグをステップＳ２２７に入力する。ステップＳ２２７は、統合ツリーから最新レイヤーの全てのノードを抽出してノードリストを作成する。この場合、最新ノードは、ステップＳ２２５で追加したばかりの子ノードである。
【０１３４】
(38) ステップＳ２２７の後、ノードリストをステップＳ２２８に入力する。ステップＳ２２８は、ノードリストから未処理のノードを１つを抽出する。
【０１３５】
(39) ステップＳ２２８の後、抽出されたノードをステップＳ２２９に入力する。ステップＳ２２９は、上記の式（１２）に基づいて、抽出されたノードからルートノードまで辿った経路の良さを表す評価スコアを更新し、式（１２）の計算結果をそのノードに保存する。
【０１３６】
(40) ステップＳ２２９の後、処理されたノードのデータをステップＳ２３０に入力する。ステップＳ２３０は、全てのノードの処理が完了したか否かを判定し、判定結果がＮＯであると処理はステップＳ２２８へ戻り、判定結果がＹＥＳであると処理はステップＳ２３１へ進む。具体的には、ステップＳ２３０は、(38)で入力されたノードリストから、(40)で入力された処理されたノードを削除し、ノードリスト中の残りのノード数がゼロ（０）であるか否かを判定する。残りのノード数がゼロであれば判定結果がＹＥＳであり、残りのノード数がゼロでなければ判定結果がＮＯである。
【０１３７】
(41) ステップＳ２３０の判定結果がＹＥＳであると、処理完了フラグ、各経路とその経路への評価スコアをステップＳ２３１に入力する。
【０１３８】
(42) ステップＳ２３０の判定結果がＮＯであると、ノードリスト中の残りのノードのデータをステップＳ２２８に入力する。
【０１３９】
ステップＳ２３１は、各経路への評価スコアを昇順でソートすると共に、評価スコアが低い下位の経路に含まれている子ノードを抽出してリストを作成する。
【０１４０】
(43) ステップＳ２３１の後、削除対象となる、評価スコアが低い下位の経路に含まれている子ノードのリストをステップＳ２３２に入力する。ステップＳ２３２は、図８と共に説明した処理を行うことで、統合ツリーから(43)で入力されたリストに含まれている子ノードを削除する、子ノード削除処理を行う。
【０１４１】
(44) ステップＳ２３２の後、子ノードを削除した統合ツリーのデータをステップＳ２３３に入力する。ステップＳ２３３は、図９と共に説明したように、統合ツリーのメンテナンス処理を行う。具体的には、統合ツリーの最新レイヤーからルートノードまでを辿って（スキャンして）、子ノードを持っていないノードを削除する。また、子が１つだけの親ノードを子ノードとマージする。
【０１４２】
(45) ステップＳ２３３の後、メンテナンスを行った統合ツリーのデータをステップＳ２３４に入力する。ステップＳ２３４は、信頼性評価結果及び統合結果出力処理を行う。具体的には、ステップＳ２２０で計算して(28)で入力されたデータ、及び経路の評価スコアが最大値のノードにあるロボット１００の位置姿勢の状態を共有メモリに書き込み、アプリケーションと共有する。アプリケーションは、例えば統合ツリーから推定したロボット１００の位置姿勢を用いてロボット１００の移動経路を計画あるいは決定する移動経路計画部１６が実行するものである。
【０１４３】
(46) ステップＳ２３４の後、入力待ちコマンドをステップＳ２０２に入力する。
【０１４４】
(47) ステップＳ２３４の後、推定器への信頼性のレベル、及び統合ツリーから推定したロボットの位置姿勢の結果をステップＳ２３５に入力する。ステップＳ２３５は、統合器１０の外部、例えば、図２に示す移動経路計画部１６により実行される。ステップＳ２３５は、推定器への信頼性のレベル、及び統合ツリーから推定したロボットの位置姿勢の結果に基づいて、例えば移動経路計画などを実行する。
【０１４５】
図１４は、統合ツリーの一例を示す図である。図１４中、ノードは○印で表し、最新ノードは◎印で表すと共に、◎印の中央部分が濃い程経路の評価スコアが高いことを示す。また、隣り合う２本の水平線で囲まれた帯状の領域は１つのレイヤーを示し、ノードの横に示されている数字は経路番号を示す。
【０１４６】
図１５は、図１４に示す統合ツリーの各ノードに対する経路評価で得られる評価スコアの一例を示す図である。図１５では、１行に１０個の異なるノード番号の評価スコアが示されている。ノードのノード番号は、例えば統合ツリーの最上位レイヤーから最下位レイヤーまで、各レイヤーにおいて図１４中左から右の順番で各ノードに割り当てられる。
【０１４７】
図１６は、比較例における統合結果の一例を説明する図である。図１６及び後述する図１７は、ロボット１００が部屋の中を自律移動した場合に自己位置の推定結果から得られる移動経路の平面図を示す。図１６及び図１７において、部屋の中にある５個の四辺形は、例えば机である。
【０１４８】
図１６中、（ａ）はカメラ３を用いる推定器８によるロボット１００の自己位置の推定結果に基づく経路を一点鎖線Ｌ１で示し、（ｂ）は５．０ｍの計測範囲を有するＬＲＦ２を用いる推定器６によるロボット１００の自己位置の推定結果に基づく経路を二点鎖線Ｌ２で示し、（ｃ）は従来のカルマンフィルタを用いた（すなわち、統合ツリーを用いない）統合結果に基づく経路を実線Ｌ３で示す。また、破線で囲まれた箇所は、図１６（ａ）では経路が精度良く推定されているものの、図１６（ｂ）では経路の精度が著しく低下しているため、図１６（ａ），（ｂ）が統合された図１６（ｃ）に示す経路では統合結果が図１６（ｂ）の精度が著しく低下した経路の悪影響を受けて精度が著しく低下している。このように、比較例では、統合結果の特に破線で囲まれた箇所での経路の精度が低いことが確認された。
【０１４９】
図１７は、実施例における統合結果の一例を説明する図である。図１７中、（ａ）はカメラ３を用いる推定器８によるロボット１００の自己位置の推定結果に基づく経路を一点鎖線Ｌ１で示し、（ｂ）は５．０ｍの計測範囲を有するＬＲＦ２を用いる推定器６によるロボット１００の自己位置の推定結果に基づく経路を二点鎖線Ｌ２で示し、（ｃ）は上記実施例の統合ツリーを用いた統合結果に基づく経路を実線Ｌ３０で示す。また、破線で囲まれた箇所は、図１７（ａ）では経路が精度良く推定されているものの、図１７（ｂ）では経路の精度が著しく低下している箇所を示す。しかし、本実施例によれば、図１７（ｃ）に示すように、統合ツリーを用いるため、統合結果の破線で囲まれた箇所においても、経路の良い精度が維持されることが確認された。つまり、複数の推定器が推定した位置姿勢を高精度に統合することが可能であることが確認された。
【０１５０】
上記の比較例の場合、統合結果が例えば破綻した推定器の推定結果の影響を受けて、最悪の場合は統合結果が使えない程推定経路の精度が低下してしまう。一方、上記実施例のように統合ツリーを用いることで、統合ツリーから例えば破綻した推定器を特定して隔離することができる。このため、統合結果が例えば破綻した推定器の推定結果の影響を受けないように処理を行うことができ、ロボットシステムまたはナビゲーションシステムの安定性を大きく向上することができる。
【０１５１】
次に、上記実施例における入力情報の信頼性評価、信頼性に基づき異なる統合処理、統合ツリーの枝刈、及び診断機能のフィードバックについて説明する。
【０１５２】
入力情報の信頼性評価：
入力情報の信頼性評価にＨＭＭを用いることで、推定器からの入力情報に対する信頼性を評価する。統合器が各推定器からの位置、姿勢、位置姿勢共分散行列といった入力情報に対して、信頼性という状態量を追加して、入力情報を位置、姿勢、位置姿勢共分散行列、信頼性に拡張する。信頼性については、統合器が過去の推定履歴に基づいて推定する。また、各推定器が上記の推定情報と共に、推定を行った地点でのオドメトリ位置も入力する。
【０１５３】
統合器は、推定器数分の信頼性評価モデルを用意する。信頼性評価モデルは、ＨＭＭに基づいて構築する。ＨＭＭは２つの状態を持ち、各状態において２つのシンボルが観測できる。このシンボルは実際の入力値が統合器の統合履歴から予測した新しい入力値の近傍に入るかどうかを表す「０」と「１」の２値である。このシンボルが観測されると、ＨＭＭ内部の状態を更新する。この２つの状態の確率の比から信頼性を算出する。
【０１５４】
ＨＭＭのパラメータは、各推定器の性能に合わせて設定される。例えばカメラを用いる推定器のエラーリカバリー機能がＬＲＦを用いる推定器より高く、信頼できない状態から信頼できる状態への遷移確率が高く設定されている。
【０１５５】
信頼性に基づき異なる統合処理：
信頼性に基づき異なる統合処理は、異なる信頼性の入力情報に対して、統合ツリーを利用して異なる統合処理を行う。統合器が推定した信頼性を、例えば「高」、「中」、「低」の３つのレベルに離散化し、各レベルによって異なる統合処理を行う。信頼性の各レベルに対して異なる処理を行うために統合ツリーを導入する。統合ツリーの深さ方向は時間方向に対応する。統合ツリーの最も低いレイヤーが最新時間に対応する。統合ツリーの各ノードがそのノードの対応する時刻での統合結果であり、統合して得られたロボットの現在位置が含まれている。また、ノードは推定器から新しい入力がくる度に作成され、その時入力されたオドメトリの情報も含まれている。推定器から新しい入力がきた時、現在のノードに新しい子ノードを追加して、親ノードの統合結果を更新して子ノードに保存する。
【０１５６】
子ノードの追加は、例えば以下のように行う。先ず、現在最新のレイヤーにある全てのノードに対して、推定器からの新しい入力の信頼性のレベルが「高」であると判断した場合、親ノードに１つのみの子ノードを追加する。入力情報を観測情報として、親ノードの状態を更新して子ノードに保存する。さらに、新しい入力に含まれているオドメトリの情報も子ノードに保存する。
【０１５７】
信頼性のレベルが「低」であると判断した場合、親ノードに１つのみの子ノードを追加する。入力情報を観測情報とせず、親ノードの状態をオドメトリの情報のみを利用して更新して子ノードに保存する。
【０１５８】
信頼性のレベルが「中」であると判断した場合、信頼性のレベルが「高」と「低」と判断した場合の処理を夫々行い、その結果親ノードが２つの子を持つようになる。
【０１５９】
このようにして、新しい入力が逐時に入ることによって統合ツリーのノード数と深さが増大していき、ツリーの規模が大きくなる。
【０１６０】
統合ツリーの枝刈：
統合ツリーの枝刈は、推定した経路の速度パターンによる評価に基づき、統合ツリーの規模を一定以下に抑えるために行う。この統合ツリーの枝刈を行うため、ツリーの最新ノードからルートノードまでの経路の良さを評価する。評価スコアが低い経路の最新ノードは削除する。なお、経路とは最新レイヤーにあるノードからツリーのルートノードまで辿って抽出した時間順に並べた複数のノードのシーケンスである。経路上の各ノードに保存されているオドメトリから速度パターンを求め、これと各ノードで統合処理によって得られたロボットの位置姿勢から求めた速度パターンとの類似度を計算して評価スコアとする。各経路に対して評価を行って得られた評価スコアで各経路を昇順にソートして、下位の経路に対して最新ノードを統合ツリーから削除する。その後、統合ツリーの全体をスキャンして、子を持っていないノードを削除する。１つの子だけを持っている親ノードを子ノードと併合して、新しいノードを生成する。新しいノードでは親ノードの情報がなくなり、子ノードの情報のみを保存する。
【０１６１】
診断機能のフィードバック：
診断機能のフィードバックでは、統合器が各推定器に対して行う信頼性の評価結果を推定器への診断結果として統合結果と共に出力する。これにより、推定器が診断結果をチェックして推定器のリセットなどを行うこともできる。
【０１６２】
上記の如く、開示の地図処理方法及びプログラム、並びにロボットシステムでは、統合器が各推定器からの入力情報の信頼性を評価して、異なる信頼性のレベルに応じた異なる方式で統合履歴と関連付ける。この関連付けにより、統合履歴を表した統合ツリーが時間的に更新され、規模が増大していく。また、統合ツリーの最新ノードから過去に辿って得られた推定経路を評価して下位のノードを削除することで、ツリー全体のメンテナンスを行いツリー規模を一定以下に維持することもできる。さらに、例えば推定精度の低下した推定器の推定結果を統合処理から隔離できるため、統合結果が推定精度の低下した推定器の推定結果に影響されることなく、例えばナビゲーションの安定性を向上することができる。
【０１６３】
以上の実施例を含む実施形態に関し、さらに以下の付記を開示する。
（付記１）
互いに異なるセンサからの入力に基づいて自己位置を推定する複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価処理と、
前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合処理
をコンピュータに実行させることを特徴とする、地図処理方法。
（付記２）
前記信頼性評価モデルは、パラメータが各推定器の性能に合わせて設定される隠れマルコフモデルで構築され、前記パラメータは各推定器から入力されることを特徴とする、付記１記載の地図処理方法。
（付記３）
前記評価処理は、前記信頼性を３つのレベルに離散化し、各レベルによって異なる統合処理を行うために複数のノードを含む統合ツリーを作成し、
前記統合ツリーの深さ方向は時間方向に対応し、最も低いレイヤーが最新時間に対応し、各ノードが当該ノードの対応する時刻での統合結果であり統合処理の結果得られる位置を含むことを特徴とする、付記１または２記載の地図処理方法。
（付記４）
前記結合処理は、
前記統合ツリーの最新のレイヤーにある全てのノードに対して、１つの推定器からの新しい入力の信頼性のレベルが「高」であると判断した場合、親ノードに１つのみの子ノードを追加すると共に、当該推定器からの入力を観測情報として前記親ノードの状態を更新して子ノードに保存し、前記新しい入力に含まれているオドメトリの情報を子ノードに保存し、
前記信頼性のレベルが「低」であると判断した場合、前記親ノードに１つのみの子ノードを追加すると共に、当該推定器からの入力を観測情報とせず、前記親ノードの状態をオドメトリの情報のみを利用して更新して子ノードに保存し、
前記信頼性のレベルが「中」であると判断した場合、前記信頼性のレベルが「高」と「低」と判断した場合の処理を夫々行い、その結果前記親ノードに２つの子を持たせることを特徴とする、付記３記載の地図処理方法。
（付記５）
前記統合ツリーの最新ノードからルートノードまで辿って抽出した時間順に並べた複数のノードのシーケンスで形成された経路の良さを評価する評価スコアを、前記経路上の各ノードに保存されているオドメトリから求めた速度パターンと、前記経路上の各ノードで統合処理によって求めた速度パターンとの類似度から計算する処理と、
前記評価スコアが閾値より低い前記経路上の最新ノードを削除するメンテナンス処理
をさらに前記コンピュータに実行させることを特徴とする、付記３または４記載の地図処理方法。
（付記６）
前記メンテナンス処理は、前記経路上の最新ノードを削除した後、前記統合ツリー全体をスキャンして子を持っていないノードを削除し、さらに１つの子だけを持っている親ノードを子ノードと併合することを特徴とする、付記５記載の地図処理方法。
（付記７）
互いに異なるセンサからの入力に基づいて自己位置を推定する複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価手順と、
前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合手順
をコンピュータに実行させることを特徴とする、プログラム。
（付記８）
前記信頼性評価モデルは、パラメータが各推定器の性能に合わせて設定される隠れマルコフモデルで構築され、前記パラメータは各推定器から入力されることを特徴とする、付記７記載のプログラム。
（付記９）
前記評価手順は、前記信頼性を３つのレベルに離散化し、各レベルによって異なる統合処理を行うために複数のノードを含む統合ツリーを作成し、
前記統合ツリーの深さ方向は時間方向に対応し、最も低いレイヤーが最新時間に対応し、各ノードが当該ノードの対応する時刻での統合結果であり統合処理の結果得られる位置を含むことを特徴とする、付記７または８記載のプログラム。
（付記１０）
前記結合手順は、
前記統合ツリーの最新のレイヤーにある全てのノードに対して、１つの推定器からの新しい入力の信頼性のレベルが「高」であると判断した場合、親ノードに１つのみの子ノードを追加すると共に、当該推定器からの入力を観測情報として前記親ノードの状態を更新して子ノードに保存し、前記新しい入力に含まれているオドメトリの情報を子ノードに保存し、
前記信頼性のレベルが「低」であると判断した場合、前記親ノードに１つのみの子ノードを追加すると共に、当該推定器からの入力を観測情報とせず、前記親ノードの状態をオドメトリの情報のみを利用して更新して子ノードに保存し、
前記信頼性のレベルが「中」であると判断した場合、前記信頼性のレベルが「高」と「低」と判断した場合の処理を夫々行い、その結果前記親ノードに２つの子を持たせることを特徴とする、付記９記載のプログラム。
（付記１１）
前記統合ツリーの最新ノードからルートノードまで辿って抽出した時間順に並べた複数のノードのシーケンスで形成された経路の良さを評価する評価スコアを、前記経路上の各ノードに保存されているオドメトリから求めた速度パターンと、前記経路上の各ノードで統合処理によって求めた速度パターンとの類似度から計算する手順と、
前記評価スコアが閾値より低い前記経路上の最新ノードを削除するメンテナンス手順
をさらに前記コンピュータに実行させることを特徴とする、付記９または１０記載のプログラム。
（付記１２）
前記メンテナンス手順は、前記経路上の最新ノードを削除した後、前記統合ツリー全体をスキャンして子を持っていないノードを削除し、さらに１つの子だけを持っている親ノードを子ノードと併合することを特徴とする、付記１１記載のプログラム。
（付記１３）
自律移動型ロボットに設けられた互いに異なるセンサと、
前記異なるセンサからの入力に基づいて自己位置を推定する複数の推定器と、
前記複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価部と、
前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合部
を備えたことを特徴とする、ロボットシステム。
（付記１４）
前記信頼性評価モデルは、パラメータが各推定器の性能に合わせて設定される隠れマルコフモデルで構築され、前記パラメータは各推定器から入力されることを特徴とする、付記１３記載のロボットシステム。
（付記１５）
前記評価部は、前記信頼性を３つのレベルに離散化し、各レベルによって異なる統合処理を行うために複数のノードを含む統合ツリーを作成し、
前記統合ツリーの深さ方向は時間方向に対応し、最も低いレイヤーが最新時間に対応し、各ノードが当該ノードの対応する時刻での統合結果であり統合処理の結果得られる位置を含むことを特徴とする、付記１３または１４記載のロボットシステム。
（付記１６）
前記結合部は、
前記統合ツリーの最新のレイヤーにある全てのノードに対して、１つの推定器からの新しい入力の信頼性のレベルが「高」であると判断した場合、親ノードに１つのみの子ノードを追加すると共に、当該推定器からの入力を観測情報として前記親ノードの状態を更新して子ノードに保存し、前記新しい入力に含まれているオドメトリの情報を子ノードに保存し、
前記信頼性のレベルが「低」であると判断した場合、前記親ノードに１つのみの子ノードを追加すると共に、当該推定器からの入力を観測情報とせず、前記親ノードの状態をオドメトリの情報のみを利用して更新して子ノードに保存し、
前記信頼性のレベルが「中」であると判断した場合、前記信頼性のレベルが「高」と「低」と判断した場合の処理を夫々行い、その結果前記親ノードに２つの子を持たせることを特徴とする、付記１５記載のロボットシステム。
（付記１７）
前記統合ツリーの最新ノードからルートノードまで辿って抽出した時間順に並べた複数のノードのシーケンスで形成された経路の良さを評価する評価スコアを、前記経路上の各ノードに保存されているオドメトリから求めた速度パターンと、前記経路上の各ノードで統合処理によって求めた速度パターンとの類似度から計算する評価部と、
前記評価スコアが閾値より低い前記経路上の最新ノードを削除するメンテナンス部
をさらに備えたことを特徴とする、付記１５または１６記載のロボットシステム。
（付記１８）
前記メンテナンス部は、前記経路上の最新ノードを削除した後、前記統合ツリー全体をスキャンして子を持っていないノードを削除し、さらに１つの子だけを持っている親ノードを子ノードと併合することを特徴とする、付記１７記載のロボットシステム。
【０１６４】
以上、開示の地図処理方法及びプログラム、並びにロボットシステムを実施例により説明したが、本発明は上記実施例に限定されるものではなく、本発明の範囲内で種々の変形及び改良が可能であることは言うまでもない。
【符号の説明】
【０１６５】
４，６，８ 推定器
５，７，９ 記憶部
１０ 統合器
１１ 推定器信頼性評価部
１２ 結合部
１３ 結合の良さの評価部
１４ 統合ツリーのメンテナンス部
１５ 記憶部（統合ツリー）
２０ オドメトリ部
【技術分野】
【０００１】
本発明は、地図処理方法及びプログラム、並びにロボットシステムに関する。本発明は、地図処理プログラムを格納したコンピュータ読み取り可能な記憶媒体にも関する。
【背景技術】
【０００２】
自律移動型ロボットは、実環境内で障害物、壁などと衝突すること無く自由に移動できるように、例えばロボットに搭載されたセンサの計測情報と環境地図との照合に基づいて自己位置の推定を行う。ロボットが実環境下で自己位置を確実、且つ、正確に推定するために、複数の異なる地図を併用する方式がある。また、環境地図を自動的に作成する技術としては、例えばＳＬＡＭ（Simultaneous Localization And Mapping）が知られている。
【０００３】
複数の地図を自動的に作成する方式は、一般的には以下の２つの方式に分類できる。第１の方式では、ある種類のセンサの計測情報に基づいて最初の地図を作成する。その後、他の種類のセンサの計測情報を最初の地図に足し加えて２番目の地図を作成し、以下同様にして３番目以降の地図を作成する。一方、第２の方式では、複数の異なる種類のセンサの計測情報を推定器に同時観測情報として入力して、複数の地図を同時に、且つ、自動的に作成する。
【０００４】
第１の方式の場合、２番目以降に作成される地図が最初に作成された地図に依存する。このため、２番目以降に作成された地図の精度は、最初に作成された地図の精度に依存してしまう。一方、第２の方式の場合、センサの種類が多くなると、推定器の観測空間が大きくなるため、推定器の計算量が多くなると共に計算時間がかかる。また、異なる種類のセンサは異なる計測レートを用いるため、使用するセンサの計測レートを最も遅い計測レートに合わせる必要がある。
【０００５】
しかし、複数の地図を同時に生成する場合、異なる種類のセンサの異なる誤差モデルを反映しながら各センサ独自の計測レートを用いることが望ましい。このように、独立して作成された複数の地図では、各地図の座標系が異なると共に、推定誤差によって夫々の地図が異なる歪みを含んでいる。このため、ロボットが自己位置の推定を行う段階では、各センサの計測情報と対応する地図とのマッチングにより推定された自己位置が、異なる座標系で表現されたものであるため、例えばロボットの移動経路を計画あるいは決定する経路計画（または、経路計画）などを行うためには推定した自己位置の座標系を統合（または、統一）する必要がある。
【０００６】
自律移動型ロボットのナビゲーションシステムにおいて、複数の自己位置推定器が推定した位置姿勢を統合する従来の統合方式では、例えば各自己位置推定器の推定結果を１つのベクトルにまとめ、統合処理の観測データとして入力することで確率的フィルタにより統合する。しかし、従来の統合方式では、以下のようなデメリットが生じてしまう。
【０００７】
第１に、確率的フィルタによる統合を行うために、各自己位置推定器の推定精度を表す推定結果の共分散行列をある同一の基準に合わせる必要がある。しかし、各自己位置推定器が独立に開発されているため、自己位置推定器間では共分散行列の計算方法や設定パラメータなどに違いがある。従って、このような共分散行列に基づく統合処理では、複数の自己位置推定器が独立して推定した自己位置の座標系を高精度に統合することは難しい。
【０００８】
第２に、例えばある自己位置推定器の推定精度が低下すると（例えば、推定処理が破綻した場合も含む）、統合結果が推定精度の低下した自己位置推定器の推定結果の影響を受けて統合性能が低下してしまう。
【先行技術文献】
【特許文献】
【０００９】
【特許文献１】特開２００７−７２５９２号公報
【発明の概要】
【発明が解決しようとする課題】
【００１０】
従来の統合方式では、複数の推定器が推定した位置姿勢を高精度に統合することは難しいという問題があった。
【００１１】
そこで、本発明は、複数の推定器が推定した位置姿勢を高精度に統合することのできる地図処理方法及びプログラム、並びにロボットシステムを提供することを目的とする。
【課題を解決するための手段】
【００１２】
本発明の一観点によれば、互いに異なるセンサからの入力に基づいて自己位置を推定する複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価処理と、前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合処理をコンピュータに実行させることを特徴とする地図処理方法が提供される。
【００１３】
本発明の一観点によれば、互いに異なるセンサからの入力に基づいて自己位置を推定する複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価手順と、前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合手順をコンピュータに実行させることを特徴とするプログラムが提供される。
【００１４】
本発明の一観点によれば、自律移動型ロボットに設けられた互いに異なるセンサと、前記異なるセンサからの入力に基づいて自己位置を推定する複数の推定器と、前記複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価部と、前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合部を備えたことを特徴とするロボットシステムが提供される。
【発明の効果】
【００１５】
開示の地図処理方法及びプログラム、並びにロボットシステムによれば、複数の推定器が推定した位置姿勢を高精度に統合することが可能になる。
【図面の簡単な説明】
【００１６】
【図１】ロボットの一例を示す斜視図である。
【図２】ロボットを含むロボットシステムの一例を示すブロック図である。
【図３】推定器側からの逐次的入力により統合ツリーが生長していく様子を説明する図である。
【図４】推定器側からの逐次的入力により統合ツリーが生長していく様子を説明する図である。
【図５】統合ツリーの規模を一定以下に抑えるための枝刈を説明する図である。
【図６】統合器の動作の概要を説明するフローチャートである。
【図７】統合ツリーの規模の増大を説明する図である。
【図８】統合ツリーの最新レイヤーから不良ノードを削除する処理を説明する図である。
【図９】統合ツリーのメンテナンスを説明する図である。
【図１０】統合器の動作をより詳細に説明するフローチャートである。
【図１１】経路評価を説明する図である。
【図１２】オドメトリからロボットの移動により生じた位置姿勢の不確実さを算出するフィルタ処理を説明する図である。
【図１３】隠れマルコフモデルを説明する図である。
【図１４】統合ツリーの一例を示す図である。
【図１５】各ノードに対する経路評価で得られる評価スコアの一例を示す図である。
【図１６】比較例における統合結果の一例を説明する図である。
【図１７】実施例における統合結果の一例を説明する図である。
【発明を実施するための形態】
【００１７】
開示の地図処理方法及びプログラム、並びにロボットシステムでは、統合器が各推定器からの入力情報の信頼性を評価して、異なる信頼性のレベルに応じた異なる方式で統合履歴と関連付ける。この関連付けにより、統合履歴を表した統合ツリーが時間的に更新され、規模が増大していく。信頼性のレベルとは、信頼性を数量化した信頼度をさらに例えば「高」、「中」、「低」の３段階に分けたものを言う。
【００１８】
また、統合ツリーの最新ノードから過去に辿って得られた推定経路を評価して下位のノードを削除することで、ツリー全体のメンテナンスを行いツリー規模を一定以下に維持するようにしても良い。
【００１９】
以下に、開示の地図処理方法及びプログラム、並びにロボットシステムの各実施例を図面と共に説明する。
【実施例】
【００２０】
図１は、ロボットの一例を示す斜視図である。図１に示すロボット１００は、例えばモータなどにより駆動される台車１０１、車輪１０２、各種内的センサ及び外的センサ、及び例えばＣＰＵ（Central Processing Unit）などのプロセッサにより形成された制御部を有する周知の基本構成を有する自律移動型ロボットで形成可能である。内的センサには、レーザレンジファインダ（ＬＲＦ：Laser Range Finder）２，３、及びカメラ３が含まれる。ＬＲＦ２とＬＲＦ３は、例えば異なる種類のＬＲＦであっても良い。異なる種類のＬＲＦには、計測範囲の異なるＬＲＦも含まれる。
【００２１】
図２は、ロボットを含むロボットシステムの一例を示すブロック図である。この例では、ロボットシステム２００は、自律移動型ロボット１００のナビゲーションシステムとしても機能する。ここでは説明の便宜上、推定器が３つの場合を説明するが、推定器は少なくとも２つあれば良い。
【００２２】
推定器４は、上記ＣＰＵで形成可能であり、ＬＲＦ１と記憶部５に格納された地図１Ａに基づいてロボット１００の自己位置を推定するソフトウェアモジュール（推定モジュール）を形成する。推定器６は、上記ＣＰＵで形成可能であり、ＬＲＦ２と記憶部７に格納された地図１Ｂに基づいてロボット１００の自己位置を推定するソフトウェアモジュールを形成する。推定器８は、上記ＣＰＵで形成可能であり、カメラ３と記憶部９に格納された地図１Ｃに基づいてロボット１００の自己位置を推定するソフトウェアモジュールを形成する。
【００２３】
統合器１０は、上記ＣＰＵで形成可能であり、推定器４，６，８の自己位置推定結果を統合するソフトウェアモジュール（統合モジュール）を形成する。統合器１０において、推定器信頼性評価部１１は、推定器４，６，８からの入力の信頼性を評価して、例えば「高」＞「中」＞「低」の３段階の信頼性のレベルに分ける。信頼性のレベルとは、信頼性を数量化した信頼度をさらに例えば「高」、「中」、「低」の３段階に分けたものを言う。各信頼性のレベルによって、後述する統合ツリーへの結合が異なる。結合部１２は、信頼性のレベルが「高」（以下、説明の便宜上、信頼度が「高」とも言う）の場合、推定器からの入力を統合するためのフィルタの観測値としてフィルタ状態の更新処理を行い、新しいノードを作成して統合ツリーに追加する、推定器データの統合ツリーへの結合処理を行う。信頼性のレベルが「低」の場合、オドメトリのみでフィルタ状態の更新処理を行い、新しいノードを作成して統合ツリーに追加する。信頼性のレベルが「中」の場合、「高」と「低」の場合で行った処理を全て行う。これにより、統合ツリーの規模が増大していく。
【００２４】
統合器１０において、結合良さの評価部１３は、統合ツリーの規模を一定以下にするための枝刈を必要に応じて行うために、統合ツリーへの結合の良さを評価する。この評価は、統合器１０において推定器からの入力を統合するフィルタが推定した位置姿勢からロボット１００の動きを算出して、算出した動きとオドメトリ（Odometry）部２０で計測した動きと比較して動きの尤度を算出することで行うことができる。オドメトリ部２０は、ロボット１００の移動量を、モータ１８または台車１０１の車輪１０２の回転を検出するエンコーダ、または、ロボット１００に設けられたジャイロセンサ（または、ジャイロスコープ）などの内的センサからの検出信号に基づいて計算する周知の構成を有する。図２では、エンコーダ及びジャイロセンサがエンコーダ・ジャイロ部１９に含まれる。
【００２５】
統合ツリーのメンテナンス部１４は、推定経路中の評価対象で下位のノードを統合ツリーから削除し、次に統合ツリー全体をスキャンして、子を持っていないノードを削除する。さらに１つの子だけを持っている親ノードを子ノードと併合する。統合ツリーは、記憶部１５に格納される。
【００２６】
統合器１０は、例えば、推定器４で推定された地図Ａの座標系の自己位置（または、推定器８で推定された地図Ｃの座標系の自己位置）と、推定器６で推定された地図Ｂの座標系の自己位置を統合する統合処理を行い、統合結果を例えばロボット１００の移動経路を計画あるいは決定する移動経路計画部１６に供給する。この例では、移動経路計画部１６が決定した移動経路は、上記ＣＰＵで形成可能な台車移動コントローラ１７に供給される。台車移動コントローラ１７は、移動経路に応じてモータ１８を駆動して車輪１０２を回線することで台車１０１を例えば図１の矢印方向へ移動させる。オドメトリ部２０で計算されたロボット１００の移動量は、各推定器４，６，８に供給される。
【００２７】
統合器１０は、例えば、推定器４で推定された地図Ａの座標系の自己位置と、推定器６で推定された地図Ｂの座標系の自己位置と、推定器８で推定された地図Ｃの座標系の自己位置を統合する統合処理を行っても良いことは言うまでもない。
【００２８】
ロボットシステム２００がロボット１００で形成されている場合、図２に示す各部はロボット１００に設けられる。一方、ロボットシステム２００がロボット１００及び当該ロボット１００を制御する当該ロボット１００とは別体の情報処理装置（すなわち、サーバなどのコンピュータ）で形成されている場合、ＬＲＦ１，２及びカメラ３以外の部分を情報処理装置に設けても、統合器１０を情報処理装置に設けても、統合器１０及び移動経路計画部１６を情報処理装置に設けても良い。情報処理装置は、ロボット１００と通信可能な通信部（または、インタフェース）、ＣＰＵなどのプロセッサ、及びプロセッサが実行するプログラム及び各種データを記憶する記憶部を有する周知の構成を有する汎用のコンピュータで形成可能である。
【００２９】
少なくとも統合器１０が行う統合処理を実行するプログラムを記憶する記憶部には、半導体記憶装置、磁気、光、光磁気記録媒体などを含む各種周知のコンピュータ読み取り可能な記憶媒体を使用可能である。
【００３０】
図３及び図４は、推定器側からの逐次的入力により統合ツリーが生長していく様子を説明する図である。図３は任意の時刻ｔ１，ｔ２での処理、図４は任意の時刻ｔ３，ｔ４での処理を夫々示し、ｔ１＜ｔ２＜ｔ３＜ｔ４である。つまり、時刻ｔ１が最も早く、時刻ｔ４が最も遅い時刻である。図３及び図４において、左側が推定器側の処理を示し、右側が統合器側の処理を示す。図３及び図４では説明の便宜上、推定器４で推定された地図Ａの座標系の自己位置と、推定器６で推定された地図Ｂの座標系の自己位置を統合する統合処理を行うものとする。また、推定器４，６を夫々推定器Ａ，Ｂと図示し、推定器４のＩＤはＡ、推定器６のＩＤはＢで示す。
【００３１】
図３に示す時刻ｔ１では、統合器１０は、ステップＳＴ１で推定器Ａ，Ｂから入力した推定値をツリーの持つ情報のうち観測値へコピーし、ノードの持つの情報のうち状態ベクトルへコピーする。また、ステップＳＴ１は、推定器Ａ，Ｂから入力したオドメトリをツリーの持つ情報のうちオドメトリへコピーし、図３にハッチング付きの○印で表すルートノード（Root Node）を生成する。さらに、ステップＳＴ１は、ノードの持つ情報のうち推定器Ａ，Ｂの隠れマルコフモデル（ＨＭＭ：Hidden Markov Model）を初期化する。以下の説明では、ルートノードはハッチング付きの○印で表し、子ノードはハッチング無しの○印で表す。
【００３２】
時刻ｔ２では、統合器１０は、ステップＳＴ２で推定器Ａ，Ｂから入力した推定値及びオドメトリをツリーの持つ情報の観測値及びオドメトリへコピーする。これにより、統合器１０は、ステップＳＴ３でオドメトリ及び状態ベクトルに基づきＨＭＭを用いて推定器Ｂへの信頼度を計算する。統合部１０は、ステップＳＴ４で計算された推定器Ｂへの信頼度が「高」、「中」、「低」であるか（この例では「中」）に応じて子ノードを追加し、各子ノードの状態ベクトルを更新すると共に、親ノード（この場合、ルートノード）のＨＭＭを各子ノードへコピーする。
【００３３】
図４に示す時刻ｔ３では、統合器１０は、ステップＳＴ５で推定器Ａ，Ｂから入力した推定値及びオドメトリをツリーの持つ情報の観測値及びオドメトリへコピーする。これにより、統合器１０は、ステップＳＴ６で各ノードについて、上記ステップＳＴ３と同様にＨＭＭを用いて推定器Ａへの信頼度を計算し、上記ステップＳＴ４と同様に信頼度によって各ノードに子ノードを追加し、子ノードの状態ベクトルを更新すると共に、親ノードのＨＭＭを各子ノードへコピーする。
【００３４】
時刻ｔ４では、統合器１０は、ステップＳＴ７で上記ステップＳＴ５と同様の処理を行い、ステップＳＴ８で上記ステップＳＴ６と同様の処理を行う。
【００３５】
このようにして、種類の異なる複数の内的センサからの入力（計測情報）に基づいて自己位置を推定する複数の推定器の信頼度をＨＭＭを用いて計算する。各推定器に対応するＨＭＭの計算に用いるパラメータは、当該推定器からの入力（推定値及びオドメトリ）に含まれる。また、信頼度に応じてルートノードに子ノードを追加することで、統合ツリーを作成する。従って、記憶部１５に格納される統合ツリーの規模が成長して増大していく。この統合ツリーに含まれる最新のノード（最新レイヤーのノード）から過去のノードを辿ってルートノードまでの経路を評価して、削除対象するべきノードを検出して削除することで、統合ツリーの規模を一定以下に抑えることもできる。
【００３６】
図５は、統合ツリーの規模を一定以下に抑えるための枝刈を説明する図である。図５中、ノードは図３及び図４と同様に図示されており、さらに、削除対象のノードを●印で表す。図５中、破線で囲まれたノードは、最新レイヤーのノードを示す。図５（ａ）に示す統合ツリーの推定経路の評価の結果、例えば図５（ｂ）に●印で表す不良ノードが検出されると、図５（ｃ）に示すようにこれらの不良ノードを削除する。次に、子ノードを持っていない図５（ｄ）に●印で表すノードを抽出する。この場合、子ノードを持っていないノードのうち、最新レイヤーに存在するノードは抽出の対象外とする。次に、図５（ｅ）に示すように、抽出された、子ノードを持っていないノードを削除する。さらに、図５（ｆ）に破線で囲んで示すように、１つの子だけを持つノードを抽出する。この場合も、１つの子だけを持つノードのうち、最新レイヤーに存在するノードは抽出の対象外とする。そして、図５（ｇ）に示すように、図５（ｆ）に破線で囲んで示す子ノードと親ノードを１つのノードにマージする。このようにして、この例では図５（ａ）に示す統合ツリーの規模を一定以下に抑えるための枝刈を行った結果、統合ツリーの規模は図５（ｇ）に示す如き規模となる。なお、統合ツリーの規模を一定以下に抑えるための枝刈の詳細については後述する。
【００３７】
図６は、統合器の動作の概要を説明するフローチャートである。図６において、統合器１０は一点鎖線で囲まれた処理を実行し、（１）〜（２８）は図６の各処理でやり取りされる情報の流れの一例を示す。ステップＳ１０４〜Ｓ１２０は、統合部１０を形成するＣＰＵの各機能が実行する処理（または、手順）に相当する。推定器４，６，８の計測値と地図Ａ，Ｂ，Ｃとに基づいて推定されたロボット１００の位置姿勢は、（１），（２），（３）で示すように統合器１０に入力される。
【００３８】
統合器１０は、初めて推定器４からの入力を受け取った時、ｔ＝０として統合ツリーを初期化する。具体的には、図５に示したようなルートノードを１つ生成して、推定器４からの表１に示す如き入力をルートノードにコピーすると共に、表２に如きデータセットを持たせる。表２中、Ｎはロボットシステム２００が有する推定器に任意に割り当てられた「１」から開始する連番を示し、この例では推定器のＩＤ番号とは異なる。この例では、３つの推定器４，６，８を用いているので、表２において「推定器[1]」は推定器４、「推定器[2]」は推定器６、「推定器[N]」は推定器８を表す。
【００３９】
【表１】
【００４０】
【表２】
【００４１】
（１） 推定器４は、表１の如きデータを統合器１０に入力する。
【００４２】
（２） 推定器６は、表１の如きデータを統合器１０に入力する。
【００４３】
（３） 推定器８は、表１の如きデータを統合器１０に入力する。
【００４４】
（４） ステップＳ１０４では、統合器１０が推定器４，６，８からの最新入力を待ち合わせる入力待ち状態となり、新しい入力があるか否かを示すフラグｆ１を管理する。上記の如く、統合器１０への入力は表１に示す如き内容のデータである。ステップＳ１０５は、フラグｆ１が推定器からの新しい入力があることを示すと、新たな入力値を共有メモリに書き込む。共有メモリは、統合器１０内の記憶部で形成可能である。
【００４５】
（５） ステップＳ１０６では、統合器１０が共有メモリから入力値を読み出し、統合ツリーを初期化するか否かを判定する。統合器１０が初めて推定器４，６，８からの入力を受け取った場合、判定結果はＹＥＳであり上記の如く統合ツリーの初期化を行う。
【００４６】
（６） 統合器１０は、初期化を行うか否かを示すフラグｆ２と共有メモリから読み出した推定器からの入力値を後述するステップＳ１０７に入力する。
【００４７】
（８） 統合器１０は、推定器からの入力に含まれているオドメトリ成分を後述するステップＳ１１０に入力する。
【００４８】
（９） 統合器１０は、推定器からの入力に含まれているロボット１００の位置姿勢の推定結果成分を後述するステップＳ１１１に入力する。
【００４９】
（１０） 統合器１０は、推定器からの入力に含まれている推定器のＩＤ、及びロボット１００の位置姿勢の推定結果成分を後述するステップＳ１１２に入力する。
【００５０】
ステップＳ１０７では、統合器１０が上記の如く統合ツリーを初期化する。
【００５１】
（７） ステップＳ１０８では、統合器１０が統合ツリーを統合器１０内の記憶部で形成可能な共有メモリに保持する。
【００５２】
（１１） 統合器１０は、共有メモリに保持された統合ツリーをステップＳ１０９に入力する。
【００５３】
（１９） 統合器１０は、共有メモリに保持された統合ツリーを後述するステップＳ１１５に入力する。
【００５４】
（１２） ステップＳ１０９では、統合器１０が統合ツリーの最新レイヤーの全ノードを取得する。統合ツリーが既に初期化されていれば、最新レイヤーにある全てのノードをリストとして取得する。統合器１０は、最新レイヤーから抽出したノードのリストをステップＳ１１０に入力する。ステップＳ１１０では、統合器１０がレイヤー上の全ノードの状態をオドメトリによる予測に基づいて更新する。具体的には、ステップＳ１０９で取得したノードのリスト（ノードリスト）の全てのノード（メンバー）に対して、図１０と共に後述する式（１１）、式（１２）に基づいて予測更新を行う。
【００５５】
（１３） 統合器１０は、図１０と共に後述する式（２０）を計算するための予測更新したフィルタ状態、及び予測した観測値の共分散行列をステップＳ１１１に入力する。ステップＳ１１１は、各ノードの持つＨＭＭによる入力値の信頼性評価を図１０と共に後述する式（２１）〜（２４）に基づいて行い、推定器から入力したロボット１００の位置姿勢の推定結果の信頼性のレベルを計算する。
【００５６】
（１４） 位置姿勢の推定結果の信頼性のレベルが「高」であると、統合器１０は、信頼性のレベル、図１０と共に後述する式（１８），（１９）を計算するための予測更新したフィルタ状態、予測した観測値、及び予測した観測値の共分散行列をステップＳ１１２に入力する。信頼性のレベルが「高」であると、ステップＳ１１２では、統合器１０が信頼性のレベルが「高」と評価されたノードについてノードの状態を図１０と共に後述する式（１８），（１９）に基づいて更新する。
【００５７】
（１５） 位置姿勢の推定結果の信頼性のレベルが「低」であると、統合器１０は、信頼性のレベル、及び予測更新したフィルタ状態を後述するステップＳ１１３に入力する。
【００５８】
（１５−１） 位置姿勢の推定結果の信頼性のレベルが「中」であると、統合器１０は、信頼性のレベル、図１０と共に後述する式（１８），（１９）を計算するための予測更新したフィルタ状態、予測した観測値、及び予測した観測値の共分散行列をステップＳ１１２に入力する。信頼性のレベルが「中」であると、ステップＳ１１２では、統合器１０が信頼性のレベルが「中」と評価されたノードについてノードの状態を図１０と共に後述する式（１８），（１９）に基づいて更新する。
【００５９】
（１５−２） 位置姿勢の推定結果の信頼性のレベルが「中」であると、統合器１０はさらに、信頼性のレベル、及び予測更新したフィルタ状態を後述するステップＳ１１３に入力する。
【００６０】
（１６） 統合器１０は、観測更新したフィルタ状態をステップＳ１１３に入力する。ステップＳ１１３は、親ノードに追加する子ノードを生成する。
【００６１】
（１７） 統合器１０は、新しいフィルタ状態を持った子ノードのリストをステップＳ１１４に入力する。これにより、ステップＳ１１４では、統合器１０が生成した子ノードを各親ノードに追加する。信頼性のレベルが「高」と評価されたノードには１つだけの子ノードを追加する。信頼性のレベルが「低」と評価されたノードにも１つだけの子ノードを追加する。信頼性のレベルが「中」と評価されたノードには２つの子ノードを追加する。
【００６２】
図７は、統合ツリーの規模の増大を説明する図である。図７中、ＨＬ，ＭＬ，ＬＬは、夫々信頼性のレベルが「高」、「中」、「低」のノードを示す。例えば時刻ｔ＋１に推定器からの最新の入力があった時に、統合ツリーから時刻ｔに作成したノードを全て抽出する。ただし、ｔ＋１＝１の場合、抽出されるノードはルートノードのみとなる。次に、各ノードに対して以下の処理を行う。ここで、新しいノードΦを１つ作成しておき、現在処理しているノードをΨで表すものとする。ノードΨからオドメトリを抽出して、入力された最新のオドメトリと合わせて、図１０と共に後述する式（１）〜（６）のようにノードΨに保持している推定状態を更新する。更新した推定情報は、ノードΦに保存し、これと同時に、最新のオドメトリもノードΦに保存する。信頼性のレベルは、図１０と共に供述する式（２１）〜（２４）に基づいて計算することができる。
【００６３】
信頼性のレベルが「高（ＨＬ）」の場合、図１０と共に後述する式（１６）〜（１９）に基づいて新しいノードΦのフィルタ状態の観測更新を行って、ノードΨに図７中時刻ｔ＋１の左側に示すように子ノードとして追加する。信頼性のレベルが「低（ＬＬ）」の場合、新しいノードΦのフィルタ状態の観測更新は行わず、ノードΨに図７中時刻ｔ＋１の中央に示すように子ノードとして直接追加する。信頼性のレベルが「中（ＭＬ）」の場合、図７中時刻ｔ＋１の右側に示すように新しいノードΦをコピーして、もう１つのノードΦ’を作成する。ノードΦ’のフィルタ状態の観測更新を行い、ノードΨに子ノードとして追加する。また、ノードΦのフィルタ状態の観測更新は行わず、ノードΨに２番目の子ノードとして直接追加する。
【００６４】
（１８） 統合器１０は、子ノードを追加した統合ツリーをステップＳ１０８に入力する。ステップＳ１０８では、統合器１０が統合ツリーを保持するメモリを更新する。統合ツリーは、統合器１０内の記憶部で形成可能である。
【００６５】
（１９） 統合器１０は、メモリから統合ツリーを読み出してステップＳ１１５に入力する。ステップＳ１１５は、統合ツリーに含まれている最新レイヤー上にある全てのノードをリストとして取得する。ステップＳ１１５で取得されるノードは、ステップＳ１１４で統合ツリーに新たに追加した子ノードである。
【００６６】
（２０） 統合器１０は、最新ノード、すなわち、ステップＳ１１４で新たに追加した子ノードを、ステップＳ１１６に入力する。ステップＳ１１６では、統合器１０がリスト内の各ノードに対して、ルートノードまで辿って得られた全経路を抽出する。
【００６７】
（２１） 統合器１０は、抽出した全経路のリストをステップＳ１１７に入力する。ステップＳ１１７では、統合器１０が抽出した各経路に対して、オドメトリで測定した運動パターンとフィルタで推定した運動パターンを比較して、図１０と共に後述する式（２５）に基づいてリスト中の経路を評価する経路評価を行う。
【００６８】
（２２） 統合器１０は、経路評価により得られた経路の良さを表す評価スコアのうち、評価スコアが閾値より低い下位のノードのリストをステップＳ１１８入力する。ステップＳ１１８では、統合器１０が下位のノードを統合ツリーから削除する。
【００６９】
図８は、統合ツリーの最新レイヤーから不良ノードを削除する処理を説明する図である。最新レイヤーにＫ個のノードが含まれているとすると、Ｋが事前に設定した閾値Ｋthreshより大きくなるとノードの削除処理を行う。この場合、Ｋ−Ｋthresh個のノードを削除することになる。各ノードの対応する経路の評価スコアは、例えばΩ（Ｐ（Φｋ）），ｋ＝０，１，．．．，ｋ，．．．Ｋで表される。そこで、ノードを評価スコアΩ（Ｐ（Φｋ））の昇順でソートし、下位のＫ−Ｋthresh個のノードを最新レイヤーから削除することで、統合ツリーは図８（ａ）に示す状態から図８（ｂ）に示す状態に変化する。図８（ａ）では、削除対象の不良ノードを●印で表す。
【００７０】
（２３） 統合器１０は、ステップＳ１１７の経路評価により得られた経路の良さを表す評価スコアをステップＳ１２０に入力する。ステップＳ１２０では、統合器１０が統合結果と信頼性の評価結果を出力する。具体的には、経路の評価スコアのトップノードが持っているフィルタ推定値を統合ツリーの処理結果とし、後述するアプリケーションに出力する。また、これと同時に、この処理結果と推定した信頼性のレベルを推定モジュールを形成する推定器にフィードバックする。
【００７１】
（２４） 統合器１０は、ステップＳ１１８により下位ノードが削除された統合ツリーをステップＳ１０８に入力して、統合ツリーを保持するメモリを更新する。
【００７２】
なお、ステップＳ１１９では、統合器１０が統合ツリーのメンテナンスを行う。図９は、統合ツリーのメンテナンスを説明する図である。統合ツリーのメンテナンスでは、図９（ａ）に示すように、最新レイヤーを除き、子ノードを持っていないノードを統合ツリーから抽出し、図９（ｂ）に示すように抽出した子ノードを統合ツリーから削除する。また、図９（ｃ）に破線で囲んで示すように、最新レイヤーを除き、１つの子だけを持つノードを統合ツリーから抽出する。そして、図９（ｄ）に示すように、図９（ｃ）に破線で囲んで示す子ノードと親ノードを１つの新しいノードにマージすることで、子ノードの保持しているデータを新しいノードのデータとする。このような統合ツリーのメンテナンスは、図５（ｄ）〜図５（ｆ）と共に説明した統合ツリーの規模を一定以下に抑えるための枝刈と同様である。
【００７３】
（２５） 統合器１０は、メンテナンスを行った統合ツリーをステップＳ１０８に入力してメモリに保持する。
【００７４】
（２６） 統合器１０は、経路の評価スコアのトップノードが持っているフィルタ推定値を統合ツリーの処理結果として、例えばアプリケーションを実行するステップＳ１２１に入力する。これにより、アプリケーションには、統合ツリー方式で推定したロボット１００の位置姿勢の情報を入力できる。アプリケーションは、例えば統合ツリーから推定したロボット１００の位置姿勢を用いてロボット１００の移動経路を計画あるいは決定する移動経路計画部１６が実行するものである。
【００７５】
（２７） 統合器１０は、上記の一連の統合処理が終了したことを示すフラグｆ３をステップＳ１０４に入力する。
【００７６】
（２８） 統合器１０は、統合ツリー方式で推定したロボット１００の位置姿勢、推定した信頼性のレベルを、推定モジュールへの診断結果として、推定モジュールを形成する推定器にフィードバックする。
【００７７】
図１０は、統合器の動作をより詳細に説明するフローチャートである。図１０において、 (1)〜(48)は図１０の各ステップでやり取りされる情報の流れの一例を示す。ステップＳ２０１〜Ｓ２３４は、統合部１０を形成するＣＰＵの各機能が実行する処理（または、手順）に相当する。ステップＳ２０１は、ロボット１００またはロボット１００とは別体の情報処理装置から入力される統合処理開始コマンドに応答して統合処理を開始する。
【００７８】
(1) 統合処理の開始を示す開始コマンドは、ステップＳ２０２に入力され、ステップＳ２０２は、推定器４，６，８からの入力待ち状態となり、推定器４，６，８から新しい入力があるか否かをチェックする。
【００７９】
(2) 新しい入力があると、新しい入力があることを示すフラグｆ１をステップＳ２０３に入力する。ステップＳ２０３は、入力されたフラグが終了フラグであるか否かを判定する。ステップＳ２０３の判定結果がＮＯであると、フラグをステップＳ２０５に入力する。他方、ステップＳ２０３の判定結果がＹＥＳであると、ステップＳ２０４で統合処理は終了する。
【００８０】
ステップＳ２０５は、入力されたフラグが新しい入力を示すフラグｆ１であるか否かを判定する。ステップＳ２０５の判定結果がＹＥＳであると処理はステップＳ２０６へ進み、判定結果がＮＯであると処理はステップＳ２０２へ戻る。
【００８１】
(5) ステップＳ２０５の判定結果がＮＯであると、新しい入力の待ちコマンドをステップＳ２０２に入力する。
【００８２】
(6) ステップＳ２０５の判定結果がＹＥＳであると、推定器からの入力データを例えば上記の表１の如き形式でステップＳ２０６に入力する。ステップＳ２０６は、このような推定器からの入力データから推定器の推定情報（すなわち、推定結果）だけを抽出する。
【００８３】
(7) ステップＳ２０６の後、データの抽出完了フラグｆ４、及び推定器の入力データをステップＳ２０７に入力する。ステップＳ２０７は、例えば上記の表１の如き形式の推定器の入力データからオドメトリを抽出する。
【００８４】
(8) ステップＳ２０６の後、推定器が推定したロボット１００の位置姿勢、及び推定器の推定精度を表す推定結果の共分散行列を後述するステップＳ２２３に入力する。
【００８５】
(10) ステップＳ２０６の後、上記(8)と同様に、推定器が推定したロボット１００の位置姿勢、及び推定器の推定精度を表す推定結果の共分散行列を後述するステップＳ２１８に入力する。
【００８６】
(9) ステップＳ２０７の後、オドメトリの抽出が完了したことを示すフラグｆ５、及び推定器の入力データをステップＳ２０８に入力する。ステップＳ２０８は、入力元の推定器のＩＤ番号を識別して抽出する。
【００８７】
(21) ステップＳ２０７の後、オドメトリを後述するステップＳ２１６に入力する。
【００８８】
(11) ステップＳ２０８の後、推定器のＩＤ番号の抽出が完了したことを示すフラグｆ６をステップＳ２０９に入力する。ステップＳ２０９は、統合ツリーの最新レイヤーにあるノードを全て抽出して、抽出したノードのノードリストを作成する。ここで、最新レイヤーとは、前回の処理で統合ツリーに追加した新しいノードを含むレイヤーを言う。
【００８９】
(12) ステップＳ２０８の後、抽出した推定器のＩＤ番号を後述するステップＳ２１４に入力する。
【００９０】
(13) 抽出された、最新レイヤーにあるノードの数（ノード数）をステップＳ２１０に入力する。ステップＳ２１０は、ノード数がゼロ（０）であるか否かを判定し、判定結果がＹＥＳであれば統合ツリーが構築されていないことが分かるので処理はステップＳ２１１へ進み、判定結果がＮＯであれば処理はステップＳ２１２へ進む。
【００９１】
(14) ステップＳ２１０の判定結果がＹＥＳであると、統合ツリーの初期化コマンドをステップＳ２１１に入力する。ステップＳ２１１は、初期化コマンドに応答して統合ツリーを上記の如く初期化する。
【００９２】
(15) ステップＳ２１１の後、入力待ちコマンドをステップＳ２０２に入力する。
【００９３】
(16) ステップＳ２１０の判定結果がＮＯであると、作成したノードリストをステップＳ２１２に入力する。ステップＳ２１２は、ノードリストからノードを１つ抽出して以下に説明するステップＳ２１３，Ｓ２１４，Ｓ２１５の処理を開始し、抽出したノードをＮＤとする処理を、ノードリスト中の未処理の各ノードに対して行う。
【００９４】
(17) ステップＳ２１２の後、上記の表２の如きデータセットを持つノードＮＤをステップＳ２１３に入力する。
【００９５】
(18) ステップＳ２１２の後、上記(17)と同様にして、上記の表２の如きデータセットを持つノードＮＤをステップＳ２１４に入力する。
【００９６】
(19) ステップＳ２１２の後、上記(17)と同様にして、上記の表２の如きデータセットを持つノードＮＤをステップＳ２１５に入力する。
【００９７】
ステップＳ２１３は、ノードＮＤの持つフィルタデータからこれまでにフィルタが推定したロボット１００の位置姿勢、共分散行列、及びオドメトリを抽出する。
【００９８】
(20) ステップＳ２１３の後、フィルタがこれまでに推定したロボット１００の位置姿勢、及びオドメトリをステップＳ２１６に入力する。
【００９９】
ステップＳ２１４は、ステップＳ２０８の後(12)で入力した推定器のＩＤ番号を使ってノードＮＤから推定器ＩＤに対応したＨＭＭを抽出する。ノードＮＤは、推定器数分のＨＭＭを持っているが、この処理では、統合器１０にデータを出力した推定器の対応するＨＭＭだけを抽出する。
【０１００】
(25) ステップＳ２１４の後、ＨＭＭデータ、すなわち、これまで計算した状態「１」にある確率、状態「２」にある確率をステップＳ２１８に入力する。ＨＭＭデータについては、図１３と共に供述する。
【０１０１】
ステップＳ２１５は、ノードからこれまでの経路の評価スコアを抽出する。具体的には、以下の経路評価処理を行うために、ノードＮＤに保存されているルートノードからの経路に対する評価スコアを抽出する。
【０１０２】
【数１】
【０１０３】
(48) ステップＳ２１５の後、ノードＮＤまでの経路評価スコアを後述するステップＳ２２９に入力する。
【０１０４】
ステップＳ２１６は、オドメトリで位置姿勢を予測更新する。具体的には、(21)で入力した新しいオドメトリと、(20)でノードＮＤに保存されているオドメトリを使って、以下に説明する式（１５）までの処理を行う。
【０１０５】
【数２】
【０１０６】
【数３】
【０１０７】
【数４】
【０１０８】
【数５】
【０１０９】
【数６】
【０１１０】
図１３は、隠れマルコフモデル（ＨＭＭ）を説明する図である。ＨＭＭは、各推定器の性能に合わせて構築される。図１３に示すようなHMMでは、状態「１」は推定器の入力が信頼できる状態、状態「２」推定器の入力が信頼できない状態を表す。観測シンボル「０」は、推定器の入力がフィルタの予測ゲート外にあることを示し、観測シンボル「１」は推定器の入力がフィルタの予測ゲート内にあることを示す。
【０１１１】
【数７】
【０１１２】
次に、ＨＭＭのパラメータの設定について説明する。ＨＭＭの状態遷移確率というパラメータは、ＨＭＭを使って評価する対象の推定器の性能によって決める。例えば、ＬＲＦによる自己位置推定と、カメラによる（例えば、画像ランドマークを用いた）自己位置推定を行う２つの推定器（図１の推定器４または６と、推定器８に相当）があるものとする。この場合、２つの推定器は以下のような性質ｃ１，ｃ２を有する。
【０１１３】
ｃ１：ＬＲＦの計測情報を使って自己位置推定を行う際、前回のロボット１００の推定位置の近傍で今回の新しい位置を探す。この場合、前回の推定位置が間違っていたら、今回探した結果も間違いである可能性が高い。
【０１１４】
ｃ２：逆に、画像ランドマークを使う場合、前回のロボット１００の推定位置が間違っていたとしても、今回推定した結果は真の位置に近づいていく。
【０１１５】
このため、状態遷移マトリクスでは、例えば
ＬＲＦの場合： a00=0.90, a01=0.10, a10=0.01,a11=0.99;
カメラの場合： a00=0.95, a01=0.05, a10=0.10,a11=0.90;
に設定される。ここで、
a00： 前回は信頼でき、今回も信頼できる確率を示し、ＬＲＦとカメラの特性からカメラの方はより高く設定されている。
a01： 前回は信頼でき、今回は信頼できない確率を示し、a00+a01=1となるように設定している。
a10： 前回は信頼できず、今回は信頼できる確率（リカバリー性能）を示し、ＬＲＦの場合はリカバリー機能が弱いため、カメラに比べで低く設定されている。
a11： 前回は信頼できず、今回も信頼できない確率を示し、a10+a11=1となるように設定される。
図１０のステップＳ２１６以降の処理の説明に戻る。
【０１１６】
(22) ステップＳ２１６の後、推定器の推定値に対しての予測値、及び予測ゲート（上記の式（１４）と式（１５）の計算結果）のデータをステップＳ２１８に入力する。
【０１１７】
(23) ステップＳ２１６の後、上記の式（１１）と式（１２）の計算結果のデータをステップＳ２１７に入力する。ステップＳ２１７は、新しいノードを１つ作成し、そのノードに(23)で入力されるデータを保存する。
【０１１８】
(24) ステップＳ２１７の後、新しいノードのデータをステップＳ２２２に入力する。
【０１１９】
ステップＳ２１８は、上記の式（２０）に基づいて入力した推定器の推定値の尤度を計算し、尤度を上記の式（２２）のように２値化することで、観測シンボルを生成する。
【０１２０】
(26) ステップＳ２１８の後、２値化の結果である観測シンボルをステップＳ２１９に入力する。ステップＳ２１９は、上記の式（２１）に基づいてＨＭＭの各状態を更新する。
【０１２１】
(27) ステップＳ２１９の後、上記の式（２１）の計算結果から得られたＨＭＭの各状態にある尤度をステップＳ２２０に入力する。ステップＳ２２０は、以下の式（２３）に基づいて信頼性を算出し、以下の式（２４）に基づいて信頼性のレベルを算出する。
【０１２２】
【数８】
【０１２３】
(28) ステップＳ２２０の後、式（２４）に基づいて算出された信頼性のレベルεｔをステップＳ２２１に入力する。ステップＳ２２１は、信頼性のレベルεｔがＨＬ（高）、ＭＬ（中）、ＬＬ（低）のいずれかであるかを判定する。ステップＳ２２１の判定結果がＨＬであると、処理はステップＳ２２２へ進む。ステップＳ２２１の判定結果がＬＬであると、処理はステップＳ２２４へ進む。また、ステップＳ２２１の判定結果がＭＬであると、処理はステップＳ２２２及びステップＳ２２４へ進む。
【０１２４】
(29) ステップＳ２２１の判定結果がＨＬであると、ＨＬを示す処理フラグをステップＳ２２２に入力する。
【０１２５】
(30) ステップＳ２２１の判定結果がＬＬであると、ＬＬを示す処理フラグをステップＳ２２４に入力する。
【０１２６】
(31) ステップＳ２２１の判定結果がＭＬであると、ＭＬを示す処理フラグをステップＳ２２２及びステップＳ２２４に入力する。
【０１２７】
ステップＳ２２２は、新しいノードを１つ作成して(24)で入力されたデータをそのノードに保存することで、新しいノードを複製する。
【０１２８】
(32) ステップＳ２２２の後、複製したノードのデータをステップＳ２２３に入力する。ステップＳ２２３は、上記の式（１６）〜式（１９）に基づいて複製したノードの状態を更新すると共に、式（１８）及び式（１９）の計算結果のデータを複製したノードに保存する。
【０１２９】
(33) ステップＳ２２３の後、状態を更新した複製ノードのデータをステップＳ２２５に入力する。
【０１３０】
一方、ステップＳ２２４は、新しいノードを１つ作成して(24)で入力されたデータをそのノードに保存することで、新しいノードを複製する。
【０１３１】
(34) ステップＳ２２４の後、新しいノードに保存したデータをステップＳ２２５に入力する。ステップＳ２２５は、図７と共に説明したように、ＨＬの場合はステップＳ２２３で処理したノード１つ、ＬＬの場合はステップＳ２２４で複製したノード１つを親ノードＮＤに子ノードとして追加し、ＭＬの場合がステップＳ２２３及びステップＳ２２３で処理した２つのノードを親ノードＮＤの子ノードとして追加する、ノードを子ノードとして親ノードに追加する処理を行う。
【０１３２】
(35) ステップＳ２２５の後、処理したノードＮＤのデータをステップＳ２２６に入力する。ステップＳ２２６は、全てのノードの処理が完了したか否かを判定し、判定結果がＮＯであると(36)で処理はステップＳ２１２へ戻り、判定結果がＹＥＳであると処理はステップＳ２２７へ進む。具体的には、ステップＳ２２６は、(16)で入力されたノードリストからノードＮＤを削除し、ノードリスト中の残りのノード数がゼロ（０）であるか否かを判定する。残りのノード数がゼロであれば判定結果がＹＥＳであり、残りのノード数がゼロでなければ判定結果がＮＯである。
【０１３３】
(37) ステップＳ２２６の判定結果がＹＥＳであると、全てのノードの処理が完了したことを示す処理完了フラグをステップＳ２２７に入力する。ステップＳ２２７は、統合ツリーから最新レイヤーの全てのノードを抽出してノードリストを作成する。この場合、最新ノードは、ステップＳ２２５で追加したばかりの子ノードである。
【０１３４】
(38) ステップＳ２２７の後、ノードリストをステップＳ２２８に入力する。ステップＳ２２８は、ノードリストから未処理のノードを１つを抽出する。
【０１３５】
(39) ステップＳ２２８の後、抽出されたノードをステップＳ２２９に入力する。ステップＳ２２９は、上記の式（１２）に基づいて、抽出されたノードからルートノードまで辿った経路の良さを表す評価スコアを更新し、式（１２）の計算結果をそのノードに保存する。
【０１３６】
(40) ステップＳ２２９の後、処理されたノードのデータをステップＳ２３０に入力する。ステップＳ２３０は、全てのノードの処理が完了したか否かを判定し、判定結果がＮＯであると処理はステップＳ２２８へ戻り、判定結果がＹＥＳであると処理はステップＳ２３１へ進む。具体的には、ステップＳ２３０は、(38)で入力されたノードリストから、(40)で入力された処理されたノードを削除し、ノードリスト中の残りのノード数がゼロ（０）であるか否かを判定する。残りのノード数がゼロであれば判定結果がＹＥＳであり、残りのノード数がゼロでなければ判定結果がＮＯである。
【０１３７】
(41) ステップＳ２３０の判定結果がＹＥＳであると、処理完了フラグ、各経路とその経路への評価スコアをステップＳ２３１に入力する。
【０１３８】
(42) ステップＳ２３０の判定結果がＮＯであると、ノードリスト中の残りのノードのデータをステップＳ２２８に入力する。
【０１３９】
ステップＳ２３１は、各経路への評価スコアを昇順でソートすると共に、評価スコアが低い下位の経路に含まれている子ノードを抽出してリストを作成する。
【０１４０】
(43) ステップＳ２３１の後、削除対象となる、評価スコアが低い下位の経路に含まれている子ノードのリストをステップＳ２３２に入力する。ステップＳ２３２は、図８と共に説明した処理を行うことで、統合ツリーから(43)で入力されたリストに含まれている子ノードを削除する、子ノード削除処理を行う。
【０１４１】
(44) ステップＳ２３２の後、子ノードを削除した統合ツリーのデータをステップＳ２３３に入力する。ステップＳ２３３は、図９と共に説明したように、統合ツリーのメンテナンス処理を行う。具体的には、統合ツリーの最新レイヤーからルートノードまでを辿って（スキャンして）、子ノードを持っていないノードを削除する。また、子が１つだけの親ノードを子ノードとマージする。
【０１４２】
(45) ステップＳ２３３の後、メンテナンスを行った統合ツリーのデータをステップＳ２３４に入力する。ステップＳ２３４は、信頼性評価結果及び統合結果出力処理を行う。具体的には、ステップＳ２２０で計算して(28)で入力されたデータ、及び経路の評価スコアが最大値のノードにあるロボット１００の位置姿勢の状態を共有メモリに書き込み、アプリケーションと共有する。アプリケーションは、例えば統合ツリーから推定したロボット１００の位置姿勢を用いてロボット１００の移動経路を計画あるいは決定する移動経路計画部１６が実行するものである。
【０１４３】
(46) ステップＳ２３４の後、入力待ちコマンドをステップＳ２０２に入力する。
【０１４４】
(47) ステップＳ２３４の後、推定器への信頼性のレベル、及び統合ツリーから推定したロボットの位置姿勢の結果をステップＳ２３５に入力する。ステップＳ２３５は、統合器１０の外部、例えば、図２に示す移動経路計画部１６により実行される。ステップＳ２３５は、推定器への信頼性のレベル、及び統合ツリーから推定したロボットの位置姿勢の結果に基づいて、例えば移動経路計画などを実行する。
【０１４５】
図１４は、統合ツリーの一例を示す図である。図１４中、ノードは○印で表し、最新ノードは◎印で表すと共に、◎印の中央部分が濃い程経路の評価スコアが高いことを示す。また、隣り合う２本の水平線で囲まれた帯状の領域は１つのレイヤーを示し、ノードの横に示されている数字は経路番号を示す。
【０１４６】
図１５は、図１４に示す統合ツリーの各ノードに対する経路評価で得られる評価スコアの一例を示す図である。図１５では、１行に１０個の異なるノード番号の評価スコアが示されている。ノードのノード番号は、例えば統合ツリーの最上位レイヤーから最下位レイヤーまで、各レイヤーにおいて図１４中左から右の順番で各ノードに割り当てられる。
【０１４７】
図１６は、比較例における統合結果の一例を説明する図である。図１６及び後述する図１７は、ロボット１００が部屋の中を自律移動した場合に自己位置の推定結果から得られる移動経路の平面図を示す。図１６及び図１７において、部屋の中にある５個の四辺形は、例えば机である。
【０１４８】
図１６中、（ａ）はカメラ３を用いる推定器８によるロボット１００の自己位置の推定結果に基づく経路を一点鎖線Ｌ１で示し、（ｂ）は５．０ｍの計測範囲を有するＬＲＦ２を用いる推定器６によるロボット１００の自己位置の推定結果に基づく経路を二点鎖線Ｌ２で示し、（ｃ）は従来のカルマンフィルタを用いた（すなわち、統合ツリーを用いない）統合結果に基づく経路を実線Ｌ３で示す。また、破線で囲まれた箇所は、図１６（ａ）では経路が精度良く推定されているものの、図１６（ｂ）では経路の精度が著しく低下しているため、図１６（ａ），（ｂ）が統合された図１６（ｃ）に示す経路では統合結果が図１６（ｂ）の精度が著しく低下した経路の悪影響を受けて精度が著しく低下している。このように、比較例では、統合結果の特に破線で囲まれた箇所での経路の精度が低いことが確認された。
【０１４９】
図１７は、実施例における統合結果の一例を説明する図である。図１７中、（ａ）はカメラ３を用いる推定器８によるロボット１００の自己位置の推定結果に基づく経路を一点鎖線Ｌ１で示し、（ｂ）は５．０ｍの計測範囲を有するＬＲＦ２を用いる推定器６によるロボット１００の自己位置の推定結果に基づく経路を二点鎖線Ｌ２で示し、（ｃ）は上記実施例の統合ツリーを用いた統合結果に基づく経路を実線Ｌ３０で示す。また、破線で囲まれた箇所は、図１７（ａ）では経路が精度良く推定されているものの、図１７（ｂ）では経路の精度が著しく低下している箇所を示す。しかし、本実施例によれば、図１７（ｃ）に示すように、統合ツリーを用いるため、統合結果の破線で囲まれた箇所においても、経路の良い精度が維持されることが確認された。つまり、複数の推定器が推定した位置姿勢を高精度に統合することが可能であることが確認された。
【０１５０】
上記の比較例の場合、統合結果が例えば破綻した推定器の推定結果の影響を受けて、最悪の場合は統合結果が使えない程推定経路の精度が低下してしまう。一方、上記実施例のように統合ツリーを用いることで、統合ツリーから例えば破綻した推定器を特定して隔離することができる。このため、統合結果が例えば破綻した推定器の推定結果の影響を受けないように処理を行うことができ、ロボットシステムまたはナビゲーションシステムの安定性を大きく向上することができる。
【０１５１】
次に、上記実施例における入力情報の信頼性評価、信頼性に基づき異なる統合処理、統合ツリーの枝刈、及び診断機能のフィードバックについて説明する。
【０１５２】
入力情報の信頼性評価：
入力情報の信頼性評価にＨＭＭを用いることで、推定器からの入力情報に対する信頼性を評価する。統合器が各推定器からの位置、姿勢、位置姿勢共分散行列といった入力情報に対して、信頼性という状態量を追加して、入力情報を位置、姿勢、位置姿勢共分散行列、信頼性に拡張する。信頼性については、統合器が過去の推定履歴に基づいて推定する。また、各推定器が上記の推定情報と共に、推定を行った地点でのオドメトリ位置も入力する。
【０１５３】
統合器は、推定器数分の信頼性評価モデルを用意する。信頼性評価モデルは、ＨＭＭに基づいて構築する。ＨＭＭは２つの状態を持ち、各状態において２つのシンボルが観測できる。このシンボルは実際の入力値が統合器の統合履歴から予測した新しい入力値の近傍に入るかどうかを表す「０」と「１」の２値である。このシンボルが観測されると、ＨＭＭ内部の状態を更新する。この２つの状態の確率の比から信頼性を算出する。
【０１５４】
ＨＭＭのパラメータは、各推定器の性能に合わせて設定される。例えばカメラを用いる推定器のエラーリカバリー機能がＬＲＦを用いる推定器より高く、信頼できない状態から信頼できる状態への遷移確率が高く設定されている。
【０１５５】
信頼性に基づき異なる統合処理：
信頼性に基づき異なる統合処理は、異なる信頼性の入力情報に対して、統合ツリーを利用して異なる統合処理を行う。統合器が推定した信頼性を、例えば「高」、「中」、「低」の３つのレベルに離散化し、各レベルによって異なる統合処理を行う。信頼性の各レベルに対して異なる処理を行うために統合ツリーを導入する。統合ツリーの深さ方向は時間方向に対応する。統合ツリーの最も低いレイヤーが最新時間に対応する。統合ツリーの各ノードがそのノードの対応する時刻での統合結果であり、統合して得られたロボットの現在位置が含まれている。また、ノードは推定器から新しい入力がくる度に作成され、その時入力されたオドメトリの情報も含まれている。推定器から新しい入力がきた時、現在のノードに新しい子ノードを追加して、親ノードの統合結果を更新して子ノードに保存する。
【０１５６】
子ノードの追加は、例えば以下のように行う。先ず、現在最新のレイヤーにある全てのノードに対して、推定器からの新しい入力の信頼性のレベルが「高」であると判断した場合、親ノードに１つのみの子ノードを追加する。入力情報を観測情報として、親ノードの状態を更新して子ノードに保存する。さらに、新しい入力に含まれているオドメトリの情報も子ノードに保存する。
【０１５７】
信頼性のレベルが「低」であると判断した場合、親ノードに１つのみの子ノードを追加する。入力情報を観測情報とせず、親ノードの状態をオドメトリの情報のみを利用して更新して子ノードに保存する。
【０１５８】
信頼性のレベルが「中」であると判断した場合、信頼性のレベルが「高」と「低」と判断した場合の処理を夫々行い、その結果親ノードが２つの子を持つようになる。
【０１５９】
このようにして、新しい入力が逐時に入ることによって統合ツリーのノード数と深さが増大していき、ツリーの規模が大きくなる。
【０１６０】
統合ツリーの枝刈：
統合ツリーの枝刈は、推定した経路の速度パターンによる評価に基づき、統合ツリーの規模を一定以下に抑えるために行う。この統合ツリーの枝刈を行うため、ツリーの最新ノードからルートノードまでの経路の良さを評価する。評価スコアが低い経路の最新ノードは削除する。なお、経路とは最新レイヤーにあるノードからツリーのルートノードまで辿って抽出した時間順に並べた複数のノードのシーケンスである。経路上の各ノードに保存されているオドメトリから速度パターンを求め、これと各ノードで統合処理によって得られたロボットの位置姿勢から求めた速度パターンとの類似度を計算して評価スコアとする。各経路に対して評価を行って得られた評価スコアで各経路を昇順にソートして、下位の経路に対して最新ノードを統合ツリーから削除する。その後、統合ツリーの全体をスキャンして、子を持っていないノードを削除する。１つの子だけを持っている親ノードを子ノードと併合して、新しいノードを生成する。新しいノードでは親ノードの情報がなくなり、子ノードの情報のみを保存する。
【０１６１】
診断機能のフィードバック：
診断機能のフィードバックでは、統合器が各推定器に対して行う信頼性の評価結果を推定器への診断結果として統合結果と共に出力する。これにより、推定器が診断結果をチェックして推定器のリセットなどを行うこともできる。
【０１６２】
上記の如く、開示の地図処理方法及びプログラム、並びにロボットシステムでは、統合器が各推定器からの入力情報の信頼性を評価して、異なる信頼性のレベルに応じた異なる方式で統合履歴と関連付ける。この関連付けにより、統合履歴を表した統合ツリーが時間的に更新され、規模が増大していく。また、統合ツリーの最新ノードから過去に辿って得られた推定経路を評価して下位のノードを削除することで、ツリー全体のメンテナンスを行いツリー規模を一定以下に維持することもできる。さらに、例えば推定精度の低下した推定器の推定結果を統合処理から隔離できるため、統合結果が推定精度の低下した推定器の推定結果に影響されることなく、例えばナビゲーションの安定性を向上することができる。
【０１６３】
以上の実施例を含む実施形態に関し、さらに以下の付記を開示する。
（付記１）
互いに異なるセンサからの入力に基づいて自己位置を推定する複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価処理と、
前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合処理
をコンピュータに実行させることを特徴とする、地図処理方法。
（付記２）
前記信頼性評価モデルは、パラメータが各推定器の性能に合わせて設定される隠れマルコフモデルで構築され、前記パラメータは各推定器から入力されることを特徴とする、付記１記載の地図処理方法。
（付記３）
前記評価処理は、前記信頼性を３つのレベルに離散化し、各レベルによって異なる統合処理を行うために複数のノードを含む統合ツリーを作成し、
前記統合ツリーの深さ方向は時間方向に対応し、最も低いレイヤーが最新時間に対応し、各ノードが当該ノードの対応する時刻での統合結果であり統合処理の結果得られる位置を含むことを特徴とする、付記１または２記載の地図処理方法。
（付記４）
前記結合処理は、
前記統合ツリーの最新のレイヤーにある全てのノードに対して、１つの推定器からの新しい入力の信頼性のレベルが「高」であると判断した場合、親ノードに１つのみの子ノードを追加すると共に、当該推定器からの入力を観測情報として前記親ノードの状態を更新して子ノードに保存し、前記新しい入力に含まれているオドメトリの情報を子ノードに保存し、
前記信頼性のレベルが「低」であると判断した場合、前記親ノードに１つのみの子ノードを追加すると共に、当該推定器からの入力を観測情報とせず、前記親ノードの状態をオドメトリの情報のみを利用して更新して子ノードに保存し、
前記信頼性のレベルが「中」であると判断した場合、前記信頼性のレベルが「高」と「低」と判断した場合の処理を夫々行い、その結果前記親ノードに２つの子を持たせることを特徴とする、付記３記載の地図処理方法。
（付記５）
前記統合ツリーの最新ノードからルートノードまで辿って抽出した時間順に並べた複数のノードのシーケンスで形成された経路の良さを評価する評価スコアを、前記経路上の各ノードに保存されているオドメトリから求めた速度パターンと、前記経路上の各ノードで統合処理によって求めた速度パターンとの類似度から計算する処理と、
前記評価スコアが閾値より低い前記経路上の最新ノードを削除するメンテナンス処理
をさらに前記コンピュータに実行させることを特徴とする、付記３または４記載の地図処理方法。
（付記６）
前記メンテナンス処理は、前記経路上の最新ノードを削除した後、前記統合ツリー全体をスキャンして子を持っていないノードを削除し、さらに１つの子だけを持っている親ノードを子ノードと併合することを特徴とする、付記５記載の地図処理方法。
（付記７）
互いに異なるセンサからの入力に基づいて自己位置を推定する複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価手順と、
前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合手順
をコンピュータに実行させることを特徴とする、プログラム。
（付記８）
前記信頼性評価モデルは、パラメータが各推定器の性能に合わせて設定される隠れマルコフモデルで構築され、前記パラメータは各推定器から入力されることを特徴とする、付記７記載のプログラム。
（付記９）
前記評価手順は、前記信頼性を３つのレベルに離散化し、各レベルによって異なる統合処理を行うために複数のノードを含む統合ツリーを作成し、
前記統合ツリーの深さ方向は時間方向に対応し、最も低いレイヤーが最新時間に対応し、各ノードが当該ノードの対応する時刻での統合結果であり統合処理の結果得られる位置を含むことを特徴とする、付記７または８記載のプログラム。
（付記１０）
前記結合手順は、
前記統合ツリーの最新のレイヤーにある全てのノードに対して、１つの推定器からの新しい入力の信頼性のレベルが「高」であると判断した場合、親ノードに１つのみの子ノードを追加すると共に、当該推定器からの入力を観測情報として前記親ノードの状態を更新して子ノードに保存し、前記新しい入力に含まれているオドメトリの情報を子ノードに保存し、
前記信頼性のレベルが「低」であると判断した場合、前記親ノードに１つのみの子ノードを追加すると共に、当該推定器からの入力を観測情報とせず、前記親ノードの状態をオドメトリの情報のみを利用して更新して子ノードに保存し、
前記信頼性のレベルが「中」であると判断した場合、前記信頼性のレベルが「高」と「低」と判断した場合の処理を夫々行い、その結果前記親ノードに２つの子を持たせることを特徴とする、付記９記載のプログラム。
（付記１１）
前記統合ツリーの最新ノードからルートノードまで辿って抽出した時間順に並べた複数のノードのシーケンスで形成された経路の良さを評価する評価スコアを、前記経路上の各ノードに保存されているオドメトリから求めた速度パターンと、前記経路上の各ノードで統合処理によって求めた速度パターンとの類似度から計算する手順と、
前記評価スコアが閾値より低い前記経路上の最新ノードを削除するメンテナンス手順
をさらに前記コンピュータに実行させることを特徴とする、付記９または１０記載のプログラム。
（付記１２）
前記メンテナンス手順は、前記経路上の最新ノードを削除した後、前記統合ツリー全体をスキャンして子を持っていないノードを削除し、さらに１つの子だけを持っている親ノードを子ノードと併合することを特徴とする、付記１１記載のプログラム。
（付記１３）
自律移動型ロボットに設けられた互いに異なるセンサと、
前記異なるセンサからの入力に基づいて自己位置を推定する複数の推定器と、
前記複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価部と、
前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合部
を備えたことを特徴とする、ロボットシステム。
（付記１４）
前記信頼性評価モデルは、パラメータが各推定器の性能に合わせて設定される隠れマルコフモデルで構築され、前記パラメータは各推定器から入力されることを特徴とする、付記１３記載のロボットシステム。
（付記１５）
前記評価部は、前記信頼性を３つのレベルに離散化し、各レベルによって異なる統合処理を行うために複数のノードを含む統合ツリーを作成し、
前記統合ツリーの深さ方向は時間方向に対応し、最も低いレイヤーが最新時間に対応し、各ノードが当該ノードの対応する時刻での統合結果であり統合処理の結果得られる位置を含むことを特徴とする、付記１３または１４記載のロボットシステム。
（付記１６）
前記結合部は、
前記統合ツリーの最新のレイヤーにある全てのノードに対して、１つの推定器からの新しい入力の信頼性のレベルが「高」であると判断した場合、親ノードに１つのみの子ノードを追加すると共に、当該推定器からの入力を観測情報として前記親ノードの状態を更新して子ノードに保存し、前記新しい入力に含まれているオドメトリの情報を子ノードに保存し、
前記信頼性のレベルが「低」であると判断した場合、前記親ノードに１つのみの子ノードを追加すると共に、当該推定器からの入力を観測情報とせず、前記親ノードの状態をオドメトリの情報のみを利用して更新して子ノードに保存し、
前記信頼性のレベルが「中」であると判断した場合、前記信頼性のレベルが「高」と「低」と判断した場合の処理を夫々行い、その結果前記親ノードに２つの子を持たせることを特徴とする、付記１５記載のロボットシステム。
（付記１７）
前記統合ツリーの最新ノードからルートノードまで辿って抽出した時間順に並べた複数のノードのシーケンスで形成された経路の良さを評価する評価スコアを、前記経路上の各ノードに保存されているオドメトリから求めた速度パターンと、前記経路上の各ノードで統合処理によって求めた速度パターンとの類似度から計算する評価部と、
前記評価スコアが閾値より低い前記経路上の最新ノードを削除するメンテナンス部
をさらに備えたことを特徴とする、付記１５または１６記載のロボットシステム。
（付記１８）
前記メンテナンス部は、前記経路上の最新ノードを削除した後、前記統合ツリー全体をスキャンして子を持っていないノードを削除し、さらに１つの子だけを持っている親ノードを子ノードと併合することを特徴とする、付記１７記載のロボットシステム。
【０１６４】
以上、開示の地図処理方法及びプログラム、並びにロボットシステムを実施例により説明したが、本発明は上記実施例に限定されるものではなく、本発明の範囲内で種々の変形及び改良が可能であることは言うまでもない。
【符号の説明】
【０１６５】
４，６，８ 推定器
５，７，９ 記憶部
１０ 統合器
１１ 推定器信頼性評価部
１２ 結合部
１３ 結合の良さの評価部
１４ 統合ツリーのメンテナンス部
１５ 記憶部（統合ツリー）
２０ オドメトリ部
【特許請求の範囲】
【請求項１】
互いに異なるセンサからの入力に基づいて自己位置を推定する複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価手順と、
前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合手順
をコンピュータに実行させることを特徴とする、プログラム。
【請求項２】
前記信頼性評価モデルは、パラメータが各推定器の性能に合わせて設定される隠れマルコフモデルで構築され、前記パラメータは各推定器から入力されることを特徴とする、請求項１記載のプログラム。
【請求項３】
前記評価手順は、前記信頼性を３つのレベルに離散化し、各レベルによって異なる統合処理を行うために複数のノードを含む統合ツリーを作成し、
前記統合ツリーの深さ方向は時間方向に対応し、最も低いレイヤーが最新時間に対応し、各ノードが当該ノードの対応する時刻での統合結果であり統合処理の結果得られる位置を含むことを特徴とする、請求項１または２記載のプログラム。
【請求項４】
自律移動型ロボットに設けられた互いに異なるセンサと、
前記異なるセンサからの入力に基づいて自己位置を推定する複数の推定器と、
前記複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価部と、
前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合部
を備えたことを特徴とする、ロボットシステム。
【請求項５】
互いに異なるセンサからの入力に基づいて自己位置を推定する複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価処理と、
前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合処理
をコンピュータに実行させることを特徴とする、地図処理方法。
【請求項１】
互いに異なるセンサからの入力に基づいて自己位置を推定する複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価手順と、
前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合手順
をコンピュータに実行させることを特徴とする、プログラム。
【請求項２】
前記信頼性評価モデルは、パラメータが各推定器の性能に合わせて設定される隠れマルコフモデルで構築され、前記パラメータは各推定器から入力されることを特徴とする、請求項１記載のプログラム。
【請求項３】
前記評価手順は、前記信頼性を３つのレベルに離散化し、各レベルによって異なる統合処理を行うために複数のノードを含む統合ツリーを作成し、
前記統合ツリーの深さ方向は時間方向に対応し、最も低いレイヤーが最新時間に対応し、各ノードが当該ノードの対応する時刻での統合結果であり統合処理の結果得られる位置を含むことを特徴とする、請求項１または２記載のプログラム。
【請求項４】
自律移動型ロボットに設けられた互いに異なるセンサと、
前記異なるセンサからの入力に基づいて自己位置を推定する複数の推定器と、
前記複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価部と、
前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合部
を備えたことを特徴とする、ロボットシステム。
【請求項５】
互いに異なるセンサからの入力に基づいて自己位置を推定する複数の推定器の信頼度を前記複数の推定器に対応する信頼性評価モデルを用いて評価する評価処理と、
前記複数の推定器からの推定結果に対して前記信頼性に応じた異なる統合処理を行う結合処理
をコンピュータに実行させることを特徴とする、地図処理方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【公開番号】特開２０１２−２４８０３２（Ｐ２０１２−２４８０３２Ａ）
【公開日】平成２４年１２月１３日（２０１２．１２．１３）
【国際特許分類】
【出願番号】特願２０１１−１１９６７６（Ｐ２０１１−１１９６７６）
【出願日】平成２３年５月２７日（２０１１．５．２７）
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成２３年度、独立行政法人新エネルギー・産業技術総合開発機構、「次世代ロボット知能化技術開発プロジェクト 移動知能（サービス産業分野）の開発 動的視覚認識に基づく移動知能モジュール群の研究開発」委託研究、産業技術力強化法第１９条の適用を受ける特許出願
【出願人】（０００００５２２３）富士通株式会社 (25,993)
【Ｆターム（参考）】
【公開日】平成２４年１２月１３日（２０１２．１２．１３）
【国際特許分類】
【出願日】平成２３年５月２７日（２０１１．５．２７）
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成２３年度、独立行政法人新エネルギー・産業技術総合開発機構、「次世代ロボット知能化技術開発プロジェクト 移動知能（サービス産業分野）の開発 動的視覚認識に基づく移動知能モジュール群の研究開発」委託研究、産業技術力強化法第１９条の適用を受ける特許出願
【出願人】（０００００５２２３）富士通株式会社 (25,993)
【Ｆターム（参考）】
[ Back to top ]