JP2008033696A

JP2008033696A - 自律移動体

Info

Publication number: JP2008033696A
Application number: JP2006207329A
Authority: JP
Inventors: Akira Chin; 彬陳
Original assignee: Fujitsu Ltd
Current assignee: Fujitsu Ltd
Priority date: 2006-07-31
Filing date: 2006-07-31
Publication date: 2008-02-14
Anticipated expiration: 2026-07-31
Also published as: JP4664251B2

Abstract

【課題】本発明は、上位制御機構と下位制御機構とで構成される自律移動体の行動を制御すべくパーティクルフィルタを適用する際に、自律移動体の現在位置の推定精度を高めることを実現する新たな技術の提供を目的とする。
【解決手段】下位センサの検出値に基づいて推定した自律移動体の状態変化量を処理対象として、その状態変化量の信頼度分布の上下限を設定する手段と、環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度に基づいて、自律移動体の状態変化量の信頼度分布の上下限を設定する手段と、自律移動体の状態毎に設定した上下限を統合する手段と、統合した上下限に基づいて自律移動体の状態変化量の信頼度分布を算出する手段と、環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度を、算出した信頼度分布に基づいて修正する手段とを備えるように構成する。
【選択図】図２

Description

本発明は、自律的に移動する自律移動体に関し、特に、パーティクルフィルタを用いて自律移動体の状態を推定してその移動を制御するという構成を採るときにあって、下位センサによる検出値に基づいてパーティクルフィルタに依らずに直接的に自律移動体を制御するという機構が組み込まれるときにも、パーティクルフィルタによる自律移動体の状態推定を高精度に実行できるようにする自律移動体に関する。

複雑な実環境下で自律移動体が安定的なナビゲーションを実現するためには、図１３に示すように、外界情報を感知する分散センサネットワークと、各々のセンサ入力に応じて単純的な基本行動を選択する機構（下位の制御機構）と、各センサの情報を統合して処理することによって最適な行動を選択する機構（上位の制御機構）という３つが必要である。

ここで、このような下位制御機構と上位制御機構とを統合したアーキテクチャはハイブリッドアーキテクチャと呼ばれている。

自律移動体における従来の研究では、エンコーダや慣性センサやビジョンセンサなどの複数のセンサの情報をカルマンフィルタを用いて統合することが一般的であるが、この方法はハイブリッドアーキテクチャ構成の自律移動体には現実には適用しにくいという問題がある。

すなわち、カルマンフィルタによる方法では、自律移動体の構造モデルと運動モデルとに従い、カルマンフィルタが推定した過去の時間での自律移動体の運動量に基づいて、ある決まった時間サンプリングで次の時間での運動状態を予測することになるが、この時間サンプリングは処理データが多い場合に長くなる。

一方、下位制御機構は常に短い時間サンプリングで自律移動体と障害物との接近状態を確認しており、自律移動体が障害物や壁などに接近し過ぎるとその動きはスタックされ、下位制御機構は、減速、方向変更などの単純行動を選択して、スタックから脱出することを試みることになるが、この際、カルマンフィルタが予測した自律移動体の運動状態は実際の状況とは異なるものとなる。

このようなことから、カルマンフィルタを用いてハイブリッドアーキテクチャ構成の自律移動体の最適な行動を制御するという方法は現実には適用しにくいという問題がある。

つまり、上位制御機構に実装しているカルマンフィルタが、実環境の状況変化を素早く把握して反応する下位制御機構の挙動を予測できないことから、カルマンフィルタを用いてハイブリッドアーキテクチャ構成の自律移動体の最適な行動を制御するという方法は現実には適用しにくいのである。

このようなことを背景にして、近年、パーティクルフィルタを用いてハイブリッドアーキテクチャ構成の自律移動体の最適な行動を制御するという方法が検討されている（例えば、非特許文献１参照）。
M.Isard and A.Blake,"Condensation-conditional density propagation for visual tracking," IJCV, Vol.29, No.1, pp.5-28, 1998.

パーティクルフィルタもまた、カルマンフィルタと同様に、パーティクルフィルタが推定した現在の自律移動体の運動状態に基づいて次の時間での運動状態を予測するという処理を行う。

これから、カルマンフィルタによる方法と同じ理由により、パーティクルフィルタによる方法も現実には適用しにくい。

ただし、予測される運動の量として、オドメトリ（エンコーダやジャイロによる自己位置推定）を用いるならば、パーティクルフィルタを適用することは可能である。

しかしながら、オドメトリには不確かさ（曖昧さ）が含まれており、この不確かさを考慮せずにパーティクルフィルタを適用するならば、推定した自己位置の精度が著しく低くなることから、安定した自己位置推定は困難なものとなる。

本発明はかかる事情に鑑みてなされたものであって、ハイブリッドアーキテクチャ構成の自律移動体の行動を制御すべくパーティクルフィルタを適用する際に、オドメトリの情報を直接利用して自律移動体の移動の予測量とすることで、パーティクルフィルタの適用を可能にする場合にあって、オドメトリの情報の不確かさを考慮して、複数センサからの情報を統合することにより、自律移動体の現在位置の推定精度を高めることを実現する新たな技術の提供を目的とする。

この目的を達成するために、本発明の自律移動体は、上位センサによる環境測定を行い、パーティクルフィルタを用いて自律移動体の状態を推定してその移動を制御する上位制御機構と、下位センサによる検出値に基づいて自律移動体の状態を推定してその移動を制御する下位制御機構とを備えるという構成を採るときにあって、（１）下位センサの検出値に基づいて推定した自律移動体の状態変化量を処理対象として、その状態変化量の信頼度分布の上限及び下限を設定する第１の設定手段と、（２）環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度に基づいて、自律移動体の状態変化量の信頼度分布の上限及び下限を設定する第２の設定手段と、（３）自律移動体の状態毎に、第１及び第２の設定手段の設定した上限を統合するとともに、第１及び第２の設定手段の設定した下限を統合する統合手段と、（４）統合手段の統合した上限及び下限に基づいて、自律移動体の状態変化量の信頼度分布を算出する算出手段と、（５）環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度を、算出手段の算出した信頼度分布に基づいて修正する修正手段とを備えるように構成する。

ここで、以上の各処理手段はコンピュータプログラムでも実現できるものであり、このコンピュータプログラムは、適当なコンピュータ読み取り可能な記録媒体に記録して提供されたり、ネットワークを介して提供され、本発明を実施する際に本発明の自律移動体が搭載するコンピュータにインストールされてＣＰＵなどの制御手段上で動作することにより本発明を実現することになる。

このように構成される本発明の自律移動体では、第１の設定手段は、下位センサの検出値に基づいて推定した自律移動体の状態変化量を処理対象として、その状態変化量の信頼度分布の上限及び下限を設定する。例えば、下位センサの検出値を中心にして、その近傍領域に規定の分布を示す状態変化量の集合の信頼度を設定して、その集合の示す信頼度に基づいて上限及び下限を設定するのである。

そして、第２の設定手段は、環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度に基づいて、自律移動体の状態変化量の信頼度分布の上限及び下限を設定する。例えば、各パーティクルの位置する状態変化量空間を中心にして、その近傍領域に含まれるパーティクルの持つ状態変化量の信頼度の平均値を算出することで、各パーティクルについて区間信頼度を設定して、それらの区間信頼度の示す信頼度に基づいて上限及び下限を設定するのである。

このようにして、第１及び第２の設定手段が自律移動体の状態変化量の信頼度分布の上限及び下限を設定すると、統合手段は、自律移動体の状態毎に、第１及び第２の設定手段の設定した上限を統合するとともに、第１及び第２の設定手段の設定した下限を統合する。例えば、第１及び第２の設定手段の設定した上限を重み平均することでそれらの上限を統合するとともに、第１及び第２の設定手段の設定した下限を重み平均することでそれらの下限を統合するのである。

続いて、算出手段は、統合手段の統合した上限及び下限に基づいて、自律移動体の状態変化量の信頼度分布を算出する。例えば、統合手段の統合した上限及び下限に対してDempster-Shafer 確率モデルを適用することで、自律移動体の状態変化量の信頼度分布を算出するのである。

これを受けて、修正手段は、環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度を、算出手段の算出した信頼度分布に基づいて修正する。例えば、環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度と、算出手段の算出した信頼度分布の持つ該当する信頼度とを乗算することで、環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度を修正するのである。

このようにして、本発明では、ハイブリッドアーキテクチャ構成の自律移動体の行動を制御すべくパーティクルフィルタを適用する際に、オドメトリの情報を直接利用して自律移動体の移動の予測量とすることで、パーティクルフィルタの適用を可能にする場合にあって、オドメトリの情報の不確かさを考慮して、複数センサからの情報を統合するように処理する。

この構成に従って、本発明によれば、下位センサによる検出値に基づいてパーティクルフィルタに依らずに直接的に自律移動体を制御するという機構が組み込まれるときにも、パーティクルフィルタによる自律移動体の状態推定を高精度に実行することができるようになる。

以下、実施の形態に従って本発明を詳細に説明する。

図１に、本発明を具備するハイブリッドアーキテクチャ構成の自律移動体に搭載されることになる上位制御機構１及び下位制御機構２の装置構成例を図示する。

この図に示すように、本発明を実現すべく備えられる上位制御機構１は、下位制御機構２から送られてくる下位センサ情報を取得する下位センサ情報取得モジュール１０と、自律移動体の周囲の環境を撮影するステレオカメラ１１と、ステレオカメラ１１の撮影した映像情報を入力するステレオカメラ入力部１２と、ステレオカメラ入力部１２の入力した映像情報に基づいて３次元環境計測（ビジョンの計測）を実行する３次元環境計測部１３と、３次元環境計測部１３の計測結果を使い、パーティクルフィルタの手法に基づいて自律移動体の自己位置を推定する自己位置推定モジュール１４と、自己位置推定モジュール１４の自己位置推定結果に基づいて自律移動体の経路を計画する経路計画モジュール１５とを備える。

一方、本発明を実現すべく備えられる下位制御機構２は、複数のセンサで構成される下位センサ系２０と、下位センサ系２０のセンサにより検出されたデータを記憶するセンサデータ記憶モジュール２１と、上位制御機構１の経路計画モジュール１５から送られてくる自律移動体の行動情報（自律移動体をどのように動かすのかを指示する情報）を受け取る行動情報受付モジュール２２と、下位センサ系２０のセンサにより検出されたデータに基づいて行動する自律移動体の行動情報（自律移動体をどのように動かすのかを指示する情報）を出力する単純反応処理モジュール２３と、行動情報受付モジュール２２の受け付けた行動情報か単純反応処理モジュール２３の出力する行動情報のいずれか一方を選択する行動選択機構２４と、行動選択機構２４の選択した行動情報に基づいて自律移動体を駆動する自律移動体駆動機構２５とを備える。

下位制御機構２の備える下位センサ系２０は、自律移動体のＸ方向の移動量を検出するエンコーダ２００と、自律移動体のＹ方向の移動量を検出するエンコーダ２０１と、自律移動体の移動方向を検出するジャイロ２０２と、自律移動体と障害物との間の距離を測定する距離センサ２０３などのセンサで構成されている。

ここで、本発明では、ジャイロ２０２を使って自律移動体の移動方向を検出するという方法を用いる他に、エンコーダ２００の検出する移動量とエンコーダ２０１の検出する移動量とに基づいて自律移動体の移動方向を検出するという方法を用いている。

また、行動選択機構２４は、距離センサ２０３が自律移動体と障害物との間の距離が近いことを検出する場合には、単純反応処理モジュール２３の出力する行動情報を選択して自律移動体駆動機構２５に渡し、距離センサ２０３が自律移動体と障害物との間の距離が遠いことを検出する場合には、行動情報受付モジュール２２の受け付けた行動情報を選択して自律移動体駆動機構２５に渡すように処理する。

このように構成される上位制御機構１及び下位制御機構２に従って、自律移動体は、障害物との間の距離が近い場合には、下位センサ系２０のセンサデータに基づいて決定された行動情報に従って行動し、障害物との間の距離が遠い場合には、パーティクルフィルタによる自己位置推定結果に基づいて決定された行動情報に従って行動することになる。

図２に、本発明を実現すべく備えられる自己位置推定モジュール１４の装置構成例を図示する。

この図に示すように、自己位置推定モジュール１４は、パーティクルフィルタ１４０と、エンコーダ推定信頼度分布処理部１４１と、ジャイロ推定信頼度分布処理部１４２と、ビジョン推定信頼度分布処理部１４３と、上限重み平均算出部１４４と、下限重み平均算出部１４５と、Ｐｌｓ分布生成部１４６と、統合処理部１４７とを備える。

このパーティクルフィルタ１４０は、パーティクルのリサンプリングとサンプルの評価を行い、ビジョンによる信頼度の分布であるビジョン信頼度分布１４８を得て、それに基づいて、パーティクルのリサンプリングとサンプルの評価を行うことを繰り返すことで、自律移動体の自己位置を推定する処理を行う。

ここで、ビジョン信頼度分布１４８は、横軸がパーティクルの位置を示し、縦軸が信頼度を示すものであって、パーティクルフィルタ１４０により、自律移動体のＸ方向の移動量ｕ（前回処理時の位置からの移動量）、自律移動体のＹ方向の移動量ｖ（前回処理時の位置からの移動量）、自律移動体の移動方向の変化量θ（前回処理時の移動方向からの変化量）のそれぞれについて生成されることになる。

エンコーダ推定信頼度分布処理部１４１は、エンコーダ２００，２０１により検出された自律移動体の移動量ｕ，ｖを入力として、その入力した移動量ｕ，ｖについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定し、さらに、入力した自律移動体の移動量ｕ，ｖに基づいて自律移動体の移動方向の変化量θを算出し、その算出した移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定する。

ジャイロ推定信頼度分布処理部１４２は、ジャイロ２０２により検出された自律移動体の移動方向の変化量θを入力として、その入力した移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定する。

ビジョン推定信頼度分布処理部１４３は、パーティクルフィルタ１４０により生成されたビジョン信頼度分布１４８を入力として、その入力したビジョン信頼度分布１４８に基づいて、ビジョンにより検出された自律移動体の移動量ｕ，ｖについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定するとともに、ビジョンにより検出された自律移動体の移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定する。

上限重み平均算出部１４４は、（イ）自律移動体の移動量ｕについて、エンコーダ推定信頼度分布処理部１４１の設定した推定信頼度分布の上限と、ビジョン推定信頼度分布処理部１４３の設定した推定信頼度分布の上限とを重み平均することで統合し、（ロ）さらに、自律移動体の移動量ｖについて、エンコーダ推定信頼度分布処理部１４１の設定した推定信頼度分布の上限と、ビジョン推定信頼度分布処理部１４３の設定した推定信頼度分布の上限とを重み平均することで統合し、（ハ）さらに、自律移動体の移動方向の変化量θについて、エンコーダ推定信頼度分布処理部１４１の設定した推定信頼度分布の上限と、ジャイロ推定信頼度分布処理部１４２の設定した推定信頼度分布の上限と、ビジョン推定信頼度分布処理部１４３の設定した推定信頼度分布の上限とを重み平均することで統合する。

下限重み平均算出部１４５は、（イ）自律移動体の移動量ｕについて、エンコーダ推定信頼度分布処理部１４１の設定した推定信頼度分布の下限と、ビジョン推定信頼度分布処理部１４３の設定した推定信頼度分布の下限とを重み平均することで統合し、（ロ）さらに、自律移動体の移動量ｖについて、エンコーダ推定信頼度分布処理部１４１の設定した推定信頼度分布の下限と、ビジョン推定信頼度分布処理部１４３の設定した推定信頼度分布の下限とを重み平均することで統合し、（ハ）さらに、自律移動体の移動方向の変化量θについて、エンコーダ推定信頼度分布処理部１４１の設定した推定信頼度分布の下限と、ジャイロ推定信頼度分布処理部１４２の設定した推定信頼度分布の下限と、ビジョン推定信頼度分布処理部１４３の設定した推定信頼度分布の下限とを重み平均することで統合する。

Ｐｌｓ分布生成部１４６は、（イ）自律移動体の移動量ｕについて、上限重み平均算出部１４４の統合した上限と下限重み平均算出部１４５の統合した下限とに対してDempster-Shafer 確率モデルを適用することで、自律移動体の移動量ｕについての信頼度分布を示すＰｌｓ分布を生成し、（ロ）自律移動体の移動量ｖについて、上限重み平均算出部１４４の統合した上限と下限重み平均算出部１４５の統合した下限とに対してDempster-Shafer 確率モデルを適用することで、自律移動体の移動量ｖについての信頼度分布を示すＰｌｓ分布を生成し、（ハ）自律移動体の移動方向の変化量θについて、上限重み平均算出部１４４の統合した上限と下限重み平均算出部１４５の統合した下限とに対してDempster-Shafer 確率モデルを適用することで、自律移動体の移動方向の変化量θについての信頼度分布を示すＰｌｓ分布を生成する。

統合処理部１４７は、（イ）パーティクルフィルタ１４０により生成された自律移動体の移動量ｕについてのビジョン信頼度分布１４８と、Ｐｌｓ分布生成部１４６により生成された自律移動体の移動量ｕについてのＰｌｓ分布とを統合することで、パーティクルフィルタ１４０のリサンプリングに用いられる自律移動体の移動量ｕについてのビジョン信頼度分布（ビジョン信頼度分布１４８に代わるもの）を生成し、（ロ）さらに、パーティクルフィルタ１４０により生成された自律移動体の移動量ｖについてのビジョン信頼度分布１４８と、Ｐｌｓ分布生成部１４６により生成された自律移動体の移動量ｖについてのＰｌｓ分布とを統合することで、パーティクルフィルタ１４０のリサンプリングに用いられる自律移動体の移動量ｖについてのビジョン信頼度分布（ビジョン信頼度分布１４８に代わるもの）を生成し、（ハ）さらに、パーティクルフィルタ１４０により生成された自律移動体の移動方向の変化量θについてのビジョン信頼度分布１４８と、Ｐｌｓ分布生成部１４６により生成された自律移動体の移動方向の変化量θについてのＰｌｓ分布とを統合することで、パーティクルフィルタ１４０のリサンプリングに用いられる自律移動体の移動方向の変化量θについてのビジョン信頼度分布（ビジョン信頼度分布１４８に代わるもの）を生成する。

次に、図２のように構成される自己位置推定モジュール１４の実行する処理について説明する。

パーティクルフィルタ１４０は、パーティクルのリサンプリングとサンプルの評価を行い、ビジョンによる信頼度の分布であるビジョン信頼度分布１４８を生成する。

このビジョン信頼度分布１４８の生成を受けて、エンコーダ推定信頼度分布処理部１４１は、エンコーダ２００，２０１により検出された自律移動体の移動量ｕ，ｖについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定し、さらに、移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定する。

例えば、図３に示すように、エンコーダ２００により検出された自律移動体の移動量ｕの近傍に、三角分布で定義される信頼度分布の集合を想定することで、その移動量ｕについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定するのである。

そして、このビジョン信頼度分布１４８の生成を受けて、ジャイロ推定信頼度分布処理部１４２は、エンコーダ推定信頼度分布処理部１４１と同様の処理に従って、ジャイロ２０２により検出された自律移動体の移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定する。

一方、このビジョン信頼度分布１４８の生成を受けて、ビジョン推定信頼度分布処理部１４３は、ビジョン信頼度分布１４８に基づいて、ビジョンにより検出された自律移動体の移動量ｕ，ｖについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定するとともに、ビジョンにより検出された自律移動体の移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定する。

例えば、移動量ｕについての推定信頼度分布の上限及び下限の設定処理で説明するならば、（ｕ_i,ｖ_i,θ_i) という３次元空間値を持つパーティクルＳ_i（例えば、ｉ＝１〜５００）のそれぞれについて、パーティクルＳ_iを中心とする近傍空間を定義して、区間〔ｕ_i−ｕ’，ｕ_i＋ｕ’〕の中に入る全てのパーティクルについて、パーティクルＳ_iの近傍空間に入るのか否かをチェックして、近傍空間に入ると判断したパーティクルＳ_iの信頼度の平均値ｍ_iを算出することで、
｛〔ｕ₁−ｕ’，ｕ₁＋ｕ’〕，ｍ₁｝
｛〔ｕ₂−ｕ’，ｕ₂＋ｕ’〕，ｍ₂｝
・
・
｛〔ｕ₅₀₀−ｕ’，ｕ₅₀₀＋ｕ’〕，ｍ₅₀₀｝
という集合を生成して、
上限＝Σｍ_i ただし、ａ∩ｂ≠φの総和を算出する・・・・（１）式
下限＝Σｍ_i ただし、ａ⊆ｂ≠φの総和を算出する・・・・（２）式
という算出式に従って、図４に示すように、ビジョンにより検出された自律移動体の移動量ｕについての推定信頼度分布の上限及び下限を設定するのである。

ここで、（１）式及び（２）式はDempster-Shafer 確率モデルで使用されている式であり、Dempster-Shafer 確率モデルについて記載する下記の文献では、（１）式についてはＰｌｓ（ｂ）として記載し、（２）式についてはＢｅｌ（ｂ）と記載している。

〔Dempster-Shafer 確率モデルの文献〕
S.Ferson, V.Kreinovich, L.Ginzburg, D.Myers, and K.Sents, "Constructing probability boxes and DempsterShafer structures," Technical report, Sandia National Laboratories, 2003.
エンコーダ推定信頼度分布処理部１４１、ジャイロ推定信頼度分布処理部１４２及びビジョン推定信頼度分布処理部１４３が推定信頼度分布の上限を設定すると、上限重み平均算出部１４４は、自律移動体の移動量ｕについて、それらの設定された推定信頼度分布の上限を重み平均することで統合し、さらに、自律移動体の移動量ｖについて、それらの設定された推定信頼度分布の上限を重み平均することで統合し、さらに、自律移動体の移動方向の変化量θについて、それらの設定された推定信頼度分布の上限を重み平均することで統合する。

すなわち、自律移動体の移動量ｕについて説明するならば、図５に示すような重み平均演算を行うことで、エンコーダ推定信頼度分布処理部１４１が自律移動体の移動量ｕについて設定した推定信頼度分布の上限と、ビジョン推定信頼度分布処理部１４３が自律移動体の移動量ｕについて設定した推定信頼度分布の上限とを統合するのである。

そして、エンコーダ推定信頼度分布処理部１４１、ジャイロ推定信頼度分布処理部１４２及びビジョン推定信頼度分布処理部１４３が推定信頼度分布の下限を設定すると、下限重み平均算出部１４５は、上限重み平均算出部１４４と同様の重み平均演算を実行することで、自律移動体の移動量ｕについて、それらの設定された推定信頼度分布の下限を重み平均することで統合し、さらに、自律移動体の移動量ｖについて、それらの設定された推定信頼度分布の下限を重み平均することで統合し、さらに、自律移動体の移動方向の変化量θについて、それらの設定された推定信頼度分布の下限を重み平均することで統合する。

このようにして、自己位置推定モジュール１４は、図６に示すように、エンコーダ２００により検出された自律移動体の移動量ｕについての推定信頼度分布（図中に示すエラーモデル）を生成して、その生成した推定信頼度分布の上限及び下限を設定し、さらに、ビジョン信頼度分布１４８に基づいて、ビジョンにより検出された自律移動体の移動量ｕについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定して、それらの上限を重み平均により統合するとともに、それらの下限を重み平均により統合するように処理するのである。

そして、自己位置推定モジュール１４は、図７に示すように、エンコーダ２０１により検出された自律移動体の移動量ｖについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定し、さらに、ビジョン信頼度分布１４８に基づいて、ビジョンにより検出された自律移動体の移動量ｖについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定して、それらの上限を重み平均により統合するとともに、それらの下限を重み平均により統合するように処理するのである。

そして、自己位置推定モジュール１４は、図８に示すように、エンコーダ２００，２０１により検出された自律移動体の移動量ｕ，ｖに基づいて自律移動体の移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定、さらに、ジャイロ２０２により検出された自律移動体の移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定し、さらに、ビジョンにより検出された自律移動体の移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定して、それらの上限を重み平均により統合するとともに、それらの下限を重み平均により統合するように処理するのである。

続いて、Ｐｌｓ分布生成部１４６は、上限重み平均算出部１４４の統合した上限と下限重み平均算出部１４５の統合した下限とに対して、上述の（１）式を適用することによりDempster-Shafer 確率モデルを適用することで、自律移動体の移動量ｕについての信頼度分布を示すＰｌｓ分布と、自律移動体の移動量ｖについての信頼度分布を示すＰｌｓ分布と、自律移動体の移動方向の変化量θについての信頼度分布を示すＰｌｓ分布とを生成する。

すなわち、上限重み平均算出部１４４の統合した上限と下限重み平均算出部１４５の統合した下限とに対して、上述の（１）式を適用することで、図９に示すようなＰｌｓ分布を生成することで、自律移動体の移動量ｕについての信頼度分布を示すＰｌｓ分布と、自律移動体の移動量ｖについての信頼度分布を示すＰｌｓ分布と、自律移動体の移動方向の変化量θについての信頼度分布を示すＰｌｓ分布とを生成するのである。

このＰｌｓ分布生成部１４６によるＰｌｓ分布の生成を受けて、統合処理部１４７は、パーティクルフィルタ１４０により生成されたビジョン信頼度分布１４８と、Ｐｌｓ分布生成部１４６により生成されたＰｌｓ分布とを統合することで、パーティクルフィルタ１４０により生成された自律移動体の移動量ｕについてのビジョン信頼度分布１４８と、パーティクルフィルタ１４０により生成された自律移動体の移動量ｖについてのビジョン信頼度分布１４８と、パーティクルフィルタ１４０により生成された自律移動体の移動方向の変化量θについてのビジョン信頼度分布１４８とを補正する。

すなわち、自律移動体の移動量ｕについて説明するならば、図１０に示すような統合演算を行うことで、パーティクルフィルタ１４０により生成された自律移動体の移動量ｕについてのビジョン信頼度分布１４８と、Ｐｌｓ分布生成部１４６により生成された自律移動体の移動量ｕについてのＰｌｓ分布とを統合することで、パーティクルフィルタ１４０により生成された自律移動体の移動量ｕについてのビジョン信頼度分布１４８を補正するのである。

このようにして、自己位置推定モジュール１４は、図１１の処理フローに示すように、ステップＳ１０で、パーティクルフィルタ１４０によりパーティクルのリサンプリングとサンプルの評価を行い、ビジョンによる信頼度の分布であるビジョン信頼度分布１４８を生成すると、続いて、ステップＳ１１で、エンコーダ２００，２０１の検出値に基づいて、自律移動体の移動量ｕ，ｖについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定し、さらに、自律移動体の移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定する。

続いて、ステップＳ１２で、ジャイロ２０２の検出値に基づいて、自律移動体の移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定する。

続いて、ステップＳ１３で、ビジョン信頼度分布１４８に基づいて、自律移動体の移動量ｕ，ｖについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定し、さらに、自律移動体の移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定する。

続いて、ステップＳ１４で、自律移動体の移動量ｕについて、設定した推定信頼度分布の上限を重み平均することで統合し、さらに、自律移動体の移動量ｖについて、設定した推定信頼度分布の上限を重み平均することで統合し、さらに、自律移動体の移動方向の変化量θについて、設定した推定信頼度分布の上限を重み平均することで統合する。

続いて、ステップＳ１５で、自律移動体の移動量ｕについて、設定した推定信頼度分布の下限を重み平均することで統合し、さらに、自律移動体の移動量ｖについて、設定した推定信頼度分布の下限を重み平均することで統合し、さらに、自律移動体の移動方向の変化量θについて、設定した推定信頼度分布の下限を重み平均することで統合する。

続いて、ステップＳ１６で、自律移動体の移動量ｕについて、統合した上限と統合した下限とに対してDempster-Shafer 確率モデルを適用することで、自律移動体の移動量ｕについての信頼度分布を示すＰｌｓ分布を生成し、さらに、自律移動体の移動量ｖについて、統合した上限と統合した下限とに対してDempster-Shafer 確率モデルを適用することで、自律移動体の移動量ｖについての信頼度分布を示すＰｌｓ分布を生成し、さらに、自律移動体の移動方向の変化量θについて、統合した上限と統合した下限とに対してDempster-Shafer 確率モデルを適用することで、自律移動体の移動方向の変化量θについての信頼度分布を示すＰｌｓ分布を生成する。

続いて、ステップＳ１７で、自律移動体の移動量ｕについて、パーティクルフィルタ１４０により生成したビジョン信頼度分布１４８をＰｌｓ分布で補正し、自律移動体の移動量ｖについて、パーティクルフィルタ１４０により生成したビジョン信頼度分布１４８をＰｌｓ分布で補正し、自律移動体の移動方向の変化量θについて、パーティクルフィルタ１４０により生成したビジョン信頼度分布１４８をＰｌｓ分布で補正して、パーティクルフィルタ１４０による次のリサンプリングにと処理を進めていくことになる。

図１２に、本発明の有効性を検証するために行った実験結果を図示する。

この実験結果では、ｕ，ｖ，θについてのＰｌｓ分布と、それらのＰｌｓ分布を統合する前のビジョン信頼度分布１４８と、それらのＰｌｓ分布を統合した後のビジョン信頼度分布１４８の一例を図示している。

この実験結果から分かるように、エンコーダ２００，２０１及びジャイロ２０２の検出結果をビジョン信頼度分布１４８に反映させることで、誤りのピークが抑制されることになるとともに、曖昧さが抑制されることになり、これにより本発明の有効性を検証できた。

本発明によれば、分散センサネットワーク及びハイブリッドアーキテクチャの制御方式を用いる自律移動体を対象として、自律移動体の自己位置を推定するために、内部センサによるデッドレコリングと外部センサによるデッドレコリングとを統合して、自律移動体の自己位置を高精度に推定することができるようになる。

（付記１）上位センサによる環境測定を行い、パーティクルフィルタを用いて自律移動体の状態を推定してその移動を制御する上位制御機構と、下位センサによる検出値に基づいて自律移動体の状態を推定してその移動を制御する下位制御機構とを備える自律移動体であって、前記下位センサの検出値に基づいて推定した自律移動体の状態変化量を処理対象として、その状態変化量の信頼度分布の上限及び下限を設定する第１の設定手段と、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度に基づいて、自律移動体の状態変化量の信頼度分布の上限及び下限を設定する第２の設定手段と、自律移動体の状態毎に、前記第１及び第２の設定手段の設定した上限を統合するとともに、前記第１及び第２の設定手段の設定した下限を統合する統合手段と、前記統合手段の統合した上限及び下限に基づいて、自律移動体の状態変化量の信頼度分布を算出する算出手段と、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度を、前記算出手段の算出した信頼度分布に基づいて修正する修正手段とを備えることを、特徴とする自律移動体。

（付記２）付記１に記載の自律移動体において、前記第１の設定手段は、前記下位センサの検出値を中心にして、その近傍領域に規定の分布を示す状態変化量の集合の信頼度を設定して、その集合の示す信頼度に基づいて、前記上限及び下限を設定することを、特徴とする自律移動体。

（付記３）付記１に記載の自律移動体において、前記第２の設定手段は、各パーティクルの位置する状態変化量空間を中心にして、その近傍領域に含まれるパーティクルの持つ状態変化量の信頼度の平均値を算出することで、各パーティクルについて区間信頼度を設定して、それらの区間信頼度の示す信頼度に基づいて、前記上限及び下限を設定することを、特徴とする自律移動体。

（付記４）付記１ないし３のいずれか１項に記載の自律移動体において、前記統合手段は、前記第１及び第２の設定手段の設定した上限を重み平均することでそれらの上限を統合するとともに、前記第１及び第２の設定手段の設定した下限を重み平均することでそれらの下限を統合することを、特徴とする自律移動体。

（付記５）付記１ないし４のいずれか１項に記載の自律移動体において、前記算出手段は、前記統合手段の統合した上限及び下限に対してDempster-Shafer 確率モデルを適用することで、自律移動体の状態変化量の信頼度分布を算出することを、特徴とする自律移動体。

（付記６）付記１ないし５のいずれか１項に記載の自律移動体において、前記修正手段は、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度と、前記算出手段の算出した信頼度分布の持つ該当する信頼度とを乗算することで、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度を修正することを、特徴とする自律移動体。

（付記７）上位センサによる環境測定を行い、パーティクルフィルタを用いて自律移動体の状態を推定してその移動を制御する上位制御機構と、下位センサによる検出値に基づいて自律移動体の状態を推定してその移動を制御する下位制御機構とを備える自律移動体を制御するために用いられる自律移動体制御用プログラムであって、コンピュータを、前記下位センサの検出値に基づいて推定した自律移動体の状態変化量を処理対象として、その状態変化量の信頼度分布の上限及び下限を設定する第１の設定手段と、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度に基づいて、自律移動体の状態変化量の信頼度分布の上限及び下限を設定する第２の設定手段と、自律移動体の状態毎に、前記第１及び第２の設定手段の設定した上限を統合するとともに、前記第１及び第２の設定手段の設定した下限を統合する統合手段と、前記統合手段の統合した上限及び下限に基づいて、自律移動体の状態変化量の信頼度分布を算出する算出手段と、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度を、前記算出手段の算出した信頼度分布に基づいて修正する修正手段として機能させるための自律移動体制御用プログラム。

本発明を具備するハイブリッドアーキテクチャ構成の自律移動体に搭載されることになる上位制御機構及び下位制御機構の装置構成例である。自己位置推定モジュールの装置構成例である。推定信頼度分布の上限及び下限の設定処理の説明図である。推定信頼度分布の上限及び下限の設定処理の説明図である。重み平均演算の説明図である。自己位置推定モジュールの実行する処理の説明図である。自己位置推定モジュールの実行する処理の説明図である。自己位置推定モジュールの実行する処理の説明図である。Ｐｌｓ分布の生成処理の説明図である。統合演算の説明図である。自己位置推定モジュールの実行する処理フローである。本発明の有効性を検証するために行った実験結果の説明図である。自律移動体の安定的なナビゲーションを実現するためのハイブリッドアーキテクチャ構成の説明図である。

符号の説明

１上位制御機構
２下位制御機構
１０下位センサ情報取得モジュール
１１ステレオカメラ
１２ステレオカメラ入力部
１３３次元環境計測部
１４自己位置推定モジュール
１５経路計画モジュール
２０下位センサ系
２１センサデータ記憶モジュール
２２行動情報受付モジュール
２３単純反応処理モジュール
２４行動選択機構
２５自律移動体駆動機構
２００エンコーダ
２０１エンコーダ
２０２ジャイロ
２０３距離センサ

Claims

上位センサによる環境測定を行い、パーティクルフィルタを用いて自律移動体の状態を推定してその移動を制御する上位制御機構と、下位センサによる検出値に基づいて自律移動体の状態を推定してその移動を制御する下位制御機構とを備える自律移動体であって、
前記下位センサの検出値に基づいて推定した自律移動体の状態変化量を処理対象として、その状態変化量の信頼度分布の上限及び下限を設定する第１の設定手段と、
前記環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度に基づいて、自律移動体の状態変化量の信頼度分布の上限及び下限を設定する第２の設定手段と、
自律移動体の状態毎に、前記第１及び第２の設定手段の設定した上限を統合するとともに、前記第１及び第２の設定手段の設定した下限を統合する統合手段と、
前記統合手段の統合した上限及び下限に基づいて、自律移動体の状態変化量の信頼度分布を算出する算出手段と、
前記環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度を、前記算出手段の算出した信頼度分布に基づいて修正する修正手段とを備えることを、
特徴とする自律移動体。
請求項１に記載の自律移動体において、
前記第１の設定手段は、前記下位センサの検出値を中心にして、その近傍領域に規定の分布を示す状態変化量の集合の信頼度を設定して、その集合の示す信頼度に基づいて、前記上限及び下限を設定することを、
特徴とする自律移動体。
請求項１に記載の自律移動体において、
前記第２の設定手段は、各パーティクルの位置する状態変化量空間を中心にして、その近傍領域に含まれるパーティクルの持つ状態変化量の信頼度の平均値を算出することで、各パーティクルについて区間信頼度を設定して、それらの区間信頼度の示す信頼度に基づいて、前記上限及び下限を設定することを、
特徴とする自律移動体。
請求項１ないし３のいずれか１項に記載の自律移動体において、
前記統合手段は、前記第１及び第２の設定手段の設定した上限を重み平均することでそれらの上限を統合するとともに、前記第１及び第２の設定手段の設定した下限を重み平均することでそれらの下限を統合することを、
特徴とする自律移動体。
請求項１ないし４のいずれか１項に記載の自律移動体において、
前記算出手段は、前記統合手段の統合した上限及び下限に対してDempster-Shafer 確率モデルを適用することで、自律移動体の状態変化量の信頼度分布を算出することを、
特徴とする自律移動体。