JP2008234350A - 自律移動体および移動方法 - Google Patents

Abstract

【課題】自律移動体の現在位置の推定精度を向上させること。
【解決手段】自律移動体１０は、カルマンフィルタを用いて下位センサの検出値から自律移動体１０の期待値および分散を推定し、自律移動体１０の状態変化量の確率分布の上限および下限を設定し、環境測定により推定した各パーティクルにおける自律移動体１０の状態変化の信頼度に基づいて、自律移動体１０の状態変化の確率分布の上限および下限を算出し、自律移動体１０の状態毎に各上限および下限を統合し、統合した上限および下限に基づいて、自律移動体１０の状態変化量の確率分布を算出し、環境計測により推定した各パーティクルにおける自律移動体１０の状態変化の信頼度を、算出した確率分布に基づいて修正する。
【選択図】 図１

Description

この発明は、上位センサによる環境測定を行い、パーティクルフィルタを用いて自律移動体の状態を推定して当該自律移動体の移動を制御する上位制御機構と、下位センサによる検出値から自律移動体の状態を推定して当該自律移動体の移動を制御する下位制御機構とを備える自律移動体およびその移動方法に関し、特に、自律移動体の現在位置の推定精度を向上させることができる自律移動装置および移動方法に関するものである。

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

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

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

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

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

また、パーティクルフィルタ（Particle Filter）を用いてハイブリッドアーキテクチャ構成の自律移動体の最適な行動を制御するという方法が検討されている（例えば、非特許文献１参照）。パーティクルフィルタもまた、カルマンフィルタと同様にパーティクルフィルタが推定した現在の自律移動体の運動状態に基づいて次の時間での運動状態を予測するという処理を行う。これらの理由から、カルマンフィルタによる方法と同じ理由により、パーティクルフィルタによる方法も適用しにくい。

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

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

M.Isard and A.Blake,"Condensation-conditional density propagation for visual tracking," IJCV, Vol.29, No.1, pp.5-28, 1998.

しかしながら、上述した従来の技術では、複数のセンサからの情報を統合し、自律移動体の現在位置の推定精度を高める場合に、各センサにおける状態推定値の確率分布に対して上限、下限（確率分布の上限と下限とがセンサの推定値の不確実さを意味する）をもうけ、パーティクルフィルタの状態推定値の確率分布を収束させているが、かかる上限と下限とを事前の評価データに基づいて設定しているため、センサの推定値の不確実さをうまく捕らえることができず、自律移動体の現在位置の推定精度が低下するという問題があった。

この発明は、上述した従来技術による問題点を解消するためになされたものであり、自律移動体の現在位置の推定精度を向上させることができる自律移動装置および自律移動方法を提供することを目的とする。

上述した課題を解決し、目的を達成するため、本発明は、上位センサによる環境測定を行い、パーティクルフィルタを用いて自律移動体の状態を推定して当該自律移動体の移動を制御する上位制御機構と、下位センサによる検出値から自律移動体の状態を推定して当該自律移動体の移動を制御する下位制御機構とを備える自律移動体であって、前記下位制御機構は、自律移動体の運動状態を予測するカルマンフィルタを備え、前記カルマンフィルタを用いて前記下位センサの検出値から自律移動体の期待値および分散を推定し、自律移動体の状態変化量の確率分布の上限および下限を設定する第１の設定手段と、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化の信頼度に基づいて、自律移動体の状態変化の確率分布の上限および下限を設定する第２の設定手段と、自律移動体の状態毎に、前記第１および第２の設定手段の設定した上限を統合すると共に、前記第１および第２の設定手段の設定した下限を統合する統合手段と、前記統合手段の統合した上限および下限に基づいて、自律移動体の状態変化量の確率分布を算出する算出手段と、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化の信頼度を、前記算出手段の算出した確率分布に基づいて修正する修正手段と、を備えることを特徴とする。

また、本発明は、上記発明において、前記統合手段は、前記第１および第２の設定手段の設定した上限を重み平均することで各上限を統合すると共に、前記第１および第２の設定手段の設定した下限を重み平均することで各下限を統合することを特徴とする。

また、本発明は、上記発明において、前記算出手段によって算出された確率分布に基づいて、前記下位制御機構に設定されている自律移動体の位置および姿勢の情報を修正する情報修正手段をさらに備えたことを特徴とする。

また、本発明は、上位センサによる環境測定を行い、パーティクルフィルタを用いて自律移動体の状態を推定して当該自律移動体の移動を制御する上位制御機構と、下位センサによる検出値から自律移動体の状態を推定して当該自律移動体の移動を制御する下位制御機構とを備える自律移動体の移動方法であって、前記下位制御機構は、自律移動体の運動状態を予測するカルマンフィルタを備え、前記カルマンフィルタを用いて前記下位センサの検出値から自律移動体の期待値および分散を推定し、自律移動体の状態変化量の確率分布の上限および下限を設定する第１の設定工程と、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化の信頼度に基づいて、自律移動体の状態変化の確率分布の上限および下限を設定する第２の設定工程と、自律移動体の状態毎に、前記第１および第２の設定工程により設定した上限を統合すると共に、前記第１および第２の設定工程により設定した下限を統合する統合工程と、前記統合工程において統合した上限および下限に基づいて、自律移動体の状態変化量の確率分布を算出する算出工程と、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化の信頼度を、前記算出工程により算出した確率分布に基づいて修正する修正工程と、を含んだことを特徴とする。

また、本発明は、上記発明において、前記統合工程は、前記第１および第２の設定工程において設定した上限を重み平均することで各上限を統合すると共に、前記第１および第２の設定工程において設定した下限を重み平均することで各下限を統合することを特徴とする。

また、本発明は、上記発明において、前記算出工程によって算出された確率分布に基づいて、前記下位制御機構に設定されている自律移動体の位置および姿勢の情報を修正する情報修正工程をさらに含んだことを特徴とする。

本発明によれば、カルマンフィルタを用いて下位センサの検出値から自律移動体の期待値および分散を推定し、自律移動体の状態変化量の確率分布の上限および下限を設定し、環境測定により推定した各パーティクルにおける自律移動体の状態変化の信頼度に基づいて、自律移動体の状態変化の確率分布の上限および下限を算出し、自律移動体の状態毎に各上限および下限を統合し、統合した上限および下限に基づいて、自律移動体の状態変化量の確率分布を算出し、環境計測により推定した各パーティクルにおける自律移動体の状態変化の信頼度を、算出した確率分布に基づいて修正するので、自律移動体の現在位置の推定精度を高めることができる。

本発明によれば、第１および第２の設定手段の設定した上限を重み平均することで各上限を統合すると共に、第１および第２の設定手段の設定した下限を重み平均することで各下限を統合するので、確率分布の上限および下限を精度よく設定することができる。

本発明によれば、確率分布に基づいて、下位制御機構に設定されている自律移動体の位置および姿勢の情報を修正するので、自律移動体の移動制御を精度よく実施することができる。

以下に添付図面を参照して、この発明に係る自律移動体および移動方法の好適な実施の形態を詳細に説明する。

以下において、本実施例にかかる自律移動体（ロボット）について詳細に説明する。図１は、本実施例にかかる自律移動体の構成の一例を示す図である。同図に示すように、この自律移動体１０は、車輪１１と、モータ１２と、エンコーダ１３と、ＣＰＵ１４と、移動量算出部１５と、上位制御機構１００と、下位制御機構２００とを備えて構成される。

このうち、モータ１２は、下位制御機構２００からの制御信号に応答して車輪１１を回転させるモータであり、エンコーダ１３は、車輪の回転数等から自律移動体１０の移動量を測定する手段である。

ＣＰＵ１４は、各種の処理手順を規定したプログラムや制御データを格納するための内部メモリを有し、これらによって種々の処理を実行する制御手段であり、特に、本発明に密接に関連するものとしては、環境のグローバル地図の情報（以下、地図情報と表記する）を保持しており、上位制御機構１００からの要求に応答して地図情報を上位制御機構１００に出力する。

移動量算出部１５は、エンコーダ１３、ジャイロ（ジャイロスコープ；図示略）、距離センサ（図示略）などから入力される自律移動体の移動量、自律移動体の向きなどに関する情報を基にして、自律移動体１０の移動量、移動方向の変化量などを算出し、算出した情報を上位制御機構１００、下位制御機構２００に出力する手段である。ステレオカメラ１６は、複数のカメラから構成され、各カメラによって撮影した自律移動体１０の周囲の環境を撮影する。ステレオカメラ１６は、撮影した映像の情報（以下、映像情報と表記する）を上位制御機構１００に出力する。

上位制御機構１００は、ステレオカメラ１６から入力される環境情報、下位制御機構２００から入力される下位センサ（エンコーダ、ジャイロ、距離センサ）の情報を基にして、自律移動体の経路計画などを行う手段である。下位制御機構２００は、上位制御機構１００から出力される制御情報に基づいて、自律移動体１０の移動制御などを行う手段である。

ここで、上位制御機構１００および下位制御機構２００の構成について説明する。図２は、上位制御機構１００および下位制御機構２００の構成を示す機能ブロック図である。同図の左側に示すように、上位制御機構１００は、下位センサ情報取得モジュール１１０と、ステレオカメラ１２０（図１に示したステレオカメラ１６に対応）と、ステレオカメラ入力部１３０と、３次元環境計測部１４０と、自己位置推定モジュール１５０と、経路計画モジュール１６０と、を備えて構成される。

このうち、下位センサ情報取得モジュール１１０は、下位制御機構２００から送られてくる下位センサ情報（下位センサ系２１０が検出した各検出値、カルマンフィルタ２７０に推定された自律移動体１０の期待値と分散の情報等）を取得する手段である。ステレオカメラ入力部１３０は、ステレオカメラ１２０が撮影した映像情報を３次元環境計測部１４０に入力する手段である。

３次元環境計測部１４０は、ステレオカメラ入力部１３０から入力された映像情報に基づいて３次元環境計測（ビジョンの計測）を実行する手段である。例えば、３次元環境計測部１４０は、映像情報から特徴点（床や壁など）およびステレオカメラの位置関係から三角法を用いて自律移動体１０から各特徴点までの距離を算出し、ビジョンを計測する。

自己位置推定モジュール１５０は、３次元環境計測部１４０の測定結果を使用し、パーティクルフィルタ（Particle Filter）の手法に基づいて自律移動体の自律位置を推定する手段である。経路計画モジュール１６０は、自己位置推定モジュール１５０の自己位置推定結果に基づいて自律移動体の経路を計画する手段である。

一方、図２の右側に示すように、下位制御機構２００は、下位センサ系２１０と、センサデータ記憶モジュール２２０と、行動情報受付モジュール２３０と、単純反応処理モジュール２４０と、行動選択機構２５０と、自律移動体駆動機構２６０と、カルマンフィルタ２７０とを備えて構成される。

下位制御機構２００の備える下位センサ系２１０は、自律移動体１０のＸ、Ｙ方向の移動量と向きの変化θを検出するエンコーダ２１１，２１２（図１のエンコーダ１３に対応する）と、自律移動体１０の移動方向を検出するジャイロ２１３と、自律移動体１０の距離を測定する距離センサ２１４などのセンサで構成されている。

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

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

自律移動体駆動機構２６０は、行動選択機構２５０から出力される行動情報を取得した場合に、取得した行動情報に従って自律移動体１０を駆動させるべく、モータ１２（図１参照）に制御信号を出力する。

カルマンフィルタ２７０は、予め設定された自律移動体１０の構造モデルと運動モデルとに従い、推定した過去の時間での自律移動体１０の運動量に基づいて、ある決まった時間サンプリングで次の時間での運動状態を予測する。特に、本実施例では、カルマンフィルタ２７０は、エンコーダ２１１，２１２およびジャイロ２１３の検出値から自律移動体１０の期待値（自律移動体１０の移動量ｕ、ｖおよび変化量θの期待値）および分散（自律移動体１０の移動量ｕ、ｖおよび変化量θの分散）を推定して、上位制御機構１００に出力する。

また、カルマンフィルタ２７０は、エンコーダ２１１，２１２の検出値（ジャイロ２１３の検出値を含まない）から自律移動体の期待値（自律移動体１０の移動量ｕ、ｖおよび変化量θの期待値）および分散（自律移動体１０の移動量ｕ、ｖおよび変化量θの分散）を推定して上位制御機構１００に出力する。本実施例において利用されるカルマンフィルタは、周知技術のカルマンフィルタと同様である。

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

続いて、本発明を実現すべく備えられる自己位置推定モジュール１５０の構成例について説明する。図３は、本実施例にかかる自己位置推定モジュール１５０の構成例を示す図である。同図に示すように、この自己位置推定モジュール１５０は、パーティクルフィルタ３００と、エンコーダ・ジャイロ推定確率分布処理部３０１と、エンコーダ推定確率分布処理部３０２と、ビジョン推定確率分布処理部３０３と、上限重み平均算出部３０４と、下限重み平均算出部３０５と、Ｐｌｓ分布生成部３０６と、統合処理部３０７とを備えて構成される。

このうち、パーティクルフィルタ３００は、パーティクル（自律移動体１０のＸ軸方向の移動量ｕ、自律移動体１０のＹ軸方向の移動量ｖ、自律移動体１０の移動方向の変化量θ）のリサンプリング（予測処理）、サンプリングの評価（更新処理）を行い、ビジョンによる信頼度の分布であるビジョン確率分布を得て、このビジョン確率分布に基づいて、パーティクルのリサンプリングとサンプリングの評価を行うことを繰り返すことで、自律移動体の自己位置を推定する処理を行う。

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

エンコーダ・ジャイロ推定確率分布処理部３０１は、エンコーダ２１１，２１２、およびジャイロ２１３により検出された検出値に基づいてカルマンフィルタ２７０に推定された自律移動体１０の移動量ｕ、移動量ｖ、変化量θの期待値および分散を取得し、取得した移動量ｕ、移動量ｖ、変化量θの期待値についての推定確率分布を生成し、移動量ｕ、移動量ｖ、変化量θの分散に基づいて、推定確率分布の上限および下限を設定する。

エンコーダ・ジャイロ推定確率分布処理部３０１は、カルマンフィルタ２７０によって推定された移動量ｕについての分散から、移動量ｕについての推定確率分布の上限・下限を算出する（移動量ｕについての分散に対して予め設定された値からガウス分布を求め、このガウス分布を積分することによって、推定確率分布の上限・下限を算出する）。移動量ｖおよび変化量θについても同様に、各分散からガウス分布を求め、かかるガウス分布を積分することによって、推定確率分布の上限・下限（移動量ｖに対応する推定確率分布の上限・下限、変化量θに対応する推定確率分布の上限・下限）を算出する。

エンコーダ推定確率分布処理部３０２は、エンコーダ２１１，２１２により検出された検出値に基づいてカルマンフィルタ２７０により推定された自律移動体１０の移動方向の変化量θの期待値および分散を取得し、取得した移動方向の変化量θの期待値についての推定確率分布を生成し、変化量θの分散に基づいて、推定確率分布の上限および下限を設定する。

また、エンコーダ推定確率分布処理部３０２は、カルマンフィルタ２７０によって推定された変化量θについての分散から、変化量θについての推定確率分布の上限・下限を算出する（変化量θについての分散に対して予め設定された値からガウス分布を求め、このガウス分布を積分することによって、推定確率分布の上限・下限を算出する）。

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

上限重み平均算出部３０４は、自律移動体１０の移動量ｕについて、エンコーダ・ジャイロ推定確率分布処理部３０１の設定した推定確率分布の上限と、ビジョン推定確率分布処理部３０３の設定した推定信頼度分布の上限とを重み平均することで統合する。

また、上限重み平均算出部３０４は、自律移動体１０の移動量ｖについて、エンコーダ・ジャイロ推定確率分布処理部３０１の設定した推定確率分布の上限と、ビジョン推定確率分布処理部３０３の設定した推定確率分布の上限とを重み平均することで統合する。

また、上限重み平均算出部３０４は、自律移動体１０の移動方向の変化量θについて、エンコーダ・ジャイロ推定確率分布処理部３０１の設定した推定確率分布の上限と、エンコーダ推定確率分布処理部３０２の設定した推定確率分布の上限と、ビジョン推定確率分布処理部３０３が設定した推定確率分布の上限とを重み平均することで統合する。

下限重み平均算出部３０５は、自律移動体１０の移動量ｕについて、エンコーダ・ジャイロ推定確率分布処理部３０１の設定した推定確率分布の下限と、ビジョン推定確率分布処理部３０３の設定した推定確率分布の下限とを重み平均することで統合する。

また、下限重み平均算出部３０５は、自律移動体１０の移動量ｖについて、エンコーダ・ジャイロ推定確率分布処理部３０１の設定した推定確率分布の下限と、ビジョン推定確率分布処理部３０３の設定した推定確率分布の下限とを重み平均することで統合する。

また、下限重み平均算出部３０５は、自律移動体１０の移動方向の変化量θについて、エンコーダ・ジャイロ推定確率分布処理部３０１の設定した推定確率分布の下限と、エンコーダ推定確率分布処理部３０２の設定した推定確率分布の下限と、ビジョン推定確率分布処理部３０３の設定した推定確率分布の下限とを重み平均することで統合する。

Ｐｌｓ分布生成部３０６は、各パーティクルｓ_i＝（ｕ_i，ｖ_i，θ_i）に対して、下記処理を行い、信頼度（Ｐｌｓ）_iを計算することによって、Ｐｌｓ分布を生成する。自律移動体１０の移動量ｕについて、上限重み平均算出部３０４の統合した上限と下限重み平均算出部３０５の統合した下限とに対してDempster-Shafer確率モデルを適用することで、自律移動体１０の真の移動量ｕが区間[ｕ_i−ｕ’，ｕ_i＋ｕ’]に属する信頼度を示す

を計算する。

また、Ｐｌｓ分布生成部３０６は、自律移動体１０の移動量ｖについて、上限重み平均算出部３０４の統合した上限と下限重み平均算出部３０５の統合した下限とに対してDempster-Shafer確率モデルを適用することで、自律移動体１０の真の移動量ｖが区間[ｖ_i−ｖ’，ｖ_i＋ｖ’]に属する信頼度を示す

を計算する。

また、Ｐｌｓ分布生成部３０６は、自律移動体１０の移動方向の変化量θについて、上限重み平均算出部３０４の統合した上限と下限重み平均算出部３０５の統合した下限とに対してDempster-Shafer確率モデルを適用することで、自律移動体１０の真の移動方向の変化量θが区間[θ_i−θ’，θ_i＋θ’]に属する信頼度を示す

を計算する。

ただし、ｕ’，ｖ’，θ’は、事前に決めたパラメータである。
ここで、

とする。

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

つぎに、図３のように構成される自己位置推定モジュール１５０の実行する処理について説明する。パーティクルフィルタ３００は、パーティクルのリサンプリングとサンプリングの評価を行い、ビジョンによる信頼度の分布であるビジョン確率分布３０８を生成する。

このビジョン確率分布３０８の生成を受けて、エンコーダ・ジャイロ推定確率分布処理部３０１は、エンコーダ２１１，２１２により検出された移動量ｕ、ｖ（移動量の期待値ｕ、ｖ）についての推定確率分布を生成し、移動量の分散から推定確率分布の上限、下限を設定する。また、エンコーダ・ジャイロ推定確率分布処理部３０１は、移動方向の変化量θ（変化量の期待値θ）についての推定確率分布を生成し、変化量の分散から推定確率分布の上限、下限を設定する。

図４は、推定確率分布の生成を説明するための図であり、図５は、上限・下限の設定を説明するための図である。図４に示すように、エンコーダ２１１，２１２により検出された自律移動体１０の移動量ｕの近傍に三角分布で定義される確率分布の集合を想定することで、その移動量ｕについての推定確率分布を生成する。

また、図５に示すように、分散が±αである場合には、期待値（移動量ｕ）から−αの位置におけるガウス分布を求め、期待値から＋αの位置におけるガウス分布を求める。各位置におけるガウス分布は予め設定されているものとする。そして、各ガウス分布を積分することによって、推定確率分布の上限および下限を算出することができる。

一方、ビジョン確率分布３０８の生成を受けて、エンコーダ推定確率分布処理部３０２は、エンコーダ・ジャイロ推定確率分布処理部３０１と同様の処理に従って、エンコーダ２１１、２１２により検出された自律移動体１０の移動方向の変化量θについての推定確率分布を生成すると共に、分散に対応するガウス分布を積分することによって、推定確率分布の上限および下限を算出する。

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

例えば、移動量ｕについての推定確率分布の上限および下限の設定処理で説明するならば、（ｕ_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," Tecnical report, Sandia National Laboratories, 2003

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

まず、自律移動体１０の移動量ｕについて説明する。図７は、重み平均演算を説明するための図である。図７に示すような重み平均演算を行うことで、エンコーダ・ジャイロ推定確率分布処理部３０１が自律移動体１０の移動量ｕについて設定した推定確率分布の上限と、ビジョン推定確率分布処理部３０３が自律移動体１０の移動量ｕについて設定した推定確率分布の上限とを統合する。

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

図８は、移動量ｕの上限および下限の統合を説明する図である。同図に示すように、自己位置推定モジュール１５０は、エンコーダ２１１，２１２、ジャイロ２１３により検出された自律移動体１０の移動量ｕについての推定確率分布（図中に示すエラーモデル）を生成して、その生成した推定確率分布の上限および下限を設定し、さらに、ビジョン確率分布３０８に基づいて、ビジョンにより検出された自律移動体１０の移動量ｕについての推定確率分布を生成して、その生成した推定確率分布の上限および下限を設定して、それらの上限を重み平均により統合すると共に、それらの下限を重み平均により統合するように処理する。

図９は、移動量ｖの上限および下限の統合を説明する図である。同図に示すように、自己位置推定モジュール１５０は、エンコーダ２１１，２１２、ジャイロ２１３により検出された自律移動体１０の移動量ｖについての推定確率分布を生成して、その生成した推定確率分布の上限および下限を設定し、さらに、ビジョン確率分布３０８に基づいて、ビジョンにより検出された自律移動体１０の移動量ｖについての推定確率分布を生成して、その生成した推定確率分布の上限および下限を設定して、それらの上限を重み平均で統合すると共に、それらの下限を重み平均により統合するように処理する。

図１０は、変化量θの上限および下限の統合を説明する図である。同図に示すように、自己位置推定モジュール１５０は、エンコーダ２１１，２１２，ジャイロ２１３により検出された自律移動体の移動方向の変化量θについての推定確率分布を生成して、その生成した推定確率分布の上限および下限を設定、さらに、エンコーダ２１１，２１２により検出された自律移動体１０の移動方向の変化量θについての推定確率分布を生成して、その生成した推定確率分布の上限および下限を設定し、さらに、ビジョンにより検出された自律移動体１０の移動方向の変化量θについての推定確率分布を生成して、その生成した推定確率分布の上限および下限を設定してそれらの上限を重み平均により統合すると共に、それらの下限を重み平均により統合するように処理する。

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

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

このＰｌｓ分布生成部３０６によりＰｌｓ分布の生成を受けて、統合処理部３０７は、パーティクルフィルタ３００により生成されたビジョン確率分布３０８と、Ｐｌｓ分布生成部３０６により生成されたＰｌｓ分布とを統合（乗算）することで、パーティクルフィルタ３００により生成された自律移動体１０の移動量（ｕ，ｖ，θ）についてビジョン確率分布３０８を修正する。

統合処理部３０７は、統合した結果得られるビジョン確率分布３０８を経路設計モジュール１６０に出力する。経路設計モジュールは、ビジョン確率分布３０８に基づいて、自律移動体１０の位置および姿勢などを算出する。そして、経路計画モジュール１６０は、算出した位置および姿勢から自律移動体１０を目標起軌道に戻すための制御経路情報を生成し、生成した制御経路情報とビジョン確率分布３０８の情報を下位制御機構２００に出力する。

下位制御機構２００は、上位制御機構１００から出力される制御経路情報に従って自律移動体駆動機構２６０が自律移動体を移動させる。また、下位制御機構２００は、ビジョン確率分布３０８から自律移動体１０の期待値および分散を求め、下位制御機構２００に含まれるカルマンフィルタの誤差を調整する。例えば、カルマンフィルタ２７０が、既に設定されてある自律移動体１０の位置および姿勢を、上位制御機構１００から入力される確率分布から推定される自律移動体１０の位置および姿勢に修正する。

つぎに、本実施例にかかる自律移動体１０の処理手順について説明する。図１２および図１３は、本実施例にかかる自律移動体の処理手順を示すフローチャートである。同図に示すように、上位制御機構１００は、ＫＦ（Kalman Filter；エンコーダ＋ジャイロ）により推定された自律移動体１０の現在状態の期待値（移動量ｕ、ｖ、変化量θの期待値）および分散（移動量ｕ、ｖ、変化量θの分散）を下位制御機構２００から取得し（ステップＳ１０１）、ＫＦ（エンコーダ）により推定された自律移動体１０の現在状態の期待値（変化量θの期待値）および分散（変化量θの分散）を取得する（ステップＳ１０２）。

そして、上位制御機構１００は、ＫＦによって推定された自律移動体１０の現在状態の周辺で、一様ランダムサンプリングを行うと共に、サンプルセットを初期化し（ステップＳ１０３）、自律移動体１０の現在状態の確率分布（ビジョン確率分布）を生成する（ステップＳ１０４）。

また、上位制御機構１００は、確率分布に対してランダムサンプリングを行い、サンプルセットを生成し（ステップＳ１０５）、ＫＦ（エンコーダ＋ジャイロ）により推定された自律移動体１０の現在状態の期待値および分散を下位制御機構２００から取得し（ステップＳ１０６）、ＫＦ（エンコーダ）により推定された自律移動体１０の現在状態の期待値および分散を下位制御機構２００から取得し（ステップＳ１０７）、自律移動体の状態変化を計算する（ステップＳ１０８）。

そして、上位制御機構１００は、自律移動体１０の状態変化に基づいてサンプルの状態を遷移し（ステップＳ１０９）、各サンプルに対して、ステレオカメラ１２０によって観測した環境情報と地図情報とを照合し、尤度を計算する（ステップＳ１１０）。

続いて、上位制御機構１００は、ビジョンによる確率分布を生成し（ステップＳ１１１）、ビジョンによる確率分布から自律移動体１０の状態の期待値と分散とを生成し（ステップＳ１１２）、自律移動体１０の期待値および分散に基づいて、現在位置姿勢から目標軌道に戻す制御経路を生成し、下位制御機構に出力する（ステップＳ１１３）。

また、上位制御機構１００は、自律移動体の期待値および分散を下位制御機構２００に出力し、下位制御機構２００に保持されている期待値および分散（あるいは、自律移動体１０の位置および姿勢）を修正する（ステップＳ１１４）。

そして、上位制御機構１００は、各サンプルに対して、自律移動体１０の真の状態がこのサンプル周辺に含まれている信念度を生成し、真の状態の仮説と、この仮説に対する信念度のペアの集合を生成し（ステップＳ１１５）、各サンプルに対して、自律移動体１０の真の状態がこのサンプルの周辺に含まれている信念度を生成し、真の状態の仮説と、この仮説に対する信念度のペアとなる仮説集合を生成する（ステップＳ１１６）。

上位制御機構１００は、仮説集合に対して積分を行い、自律移動体１０の状態の確率分布の第１の上限と下限とを生成し（ステップＳ１１７）、ＫＦ（エンコーダ＋ジャイロ）により推定された期待値と分散から自律移動体１０の状態の確率分布の第２の上限と下限とを生成し（ステップＳ１１８）、ＫＦ（エンコーダ）により推定された期待値と分散から自律移動体１０の状態の確率分布の第３の上限と下限とを生成し（ステップＳ１１９）、各上限と下限のペアを統合する（ステップＳ１２０）。

そして、上位制御機構１００は、統合した上限と下限に対して微分処理を行い、自律移動体１０の真の状態の属する数値空間、およびこの区間に属する信念度を生成（Ｐｌｓ分布を生成）し（ステップＳ１２１）、ビジョンによる確率分布とＰｌｓ分布とを乗算した確率分布を生成し（ステップＳ１２２）、かかる確率分布によって確率分布を更新（代入）し（ステップＳ１２３）、ステップＳ１０５に移行する。

上述してきたように、本実施例にかかる自律移動体１０は、カルマンフィルタを用いて下位センサの検出値から自律移動体１０の期待値および分散を推定し、自律移動体１０の状態変化量の確率分布の上限および下限を設定し、環境測定により推定した各パーティクルにおける自律移動体１０の状態変化の信頼度に基づいて、自律移動体１０の状態変化の確率分布の上限および下限を算出し、自律移動体１０の状態毎に各上限および下限を統合し、統合した上限および下限に基づいて、自律移動体１０の状態変化量の確率分布を算出し、環境計測により推定した各パーティクルにおける自律移動体１０の状態変化の信頼度を、算出した確率分布に基づいて修正するので、自律移動体１０の現在位置の推定精度を高めることができる。

また、本実施例にかかる自律移動体１０は、カルマンフィルタによって推定される自律移動体１０の期待値および分散から確率分布の上限・下限を設定しているので、確率分布の上限・下限にかかる制約条件が追加され、自律移動体１０の現在位置推定を効率よく実施することができる。

さて、これまで本発明の実施例について説明したが、本発明は上述した実施例以外にも、特許請求の範囲に記載した技術的思想の範囲内において種々の異なる実施例にて実施されてもよいものである。

また、本実施例において説明した各処理のうち、自動的におこなわれるものとして説明した処理の全部または一部を手動的におこなうこともでき、あるいは、手動的におこなわれるものとして説明した処理の全部または一部を公知の方法で自動的におこなうこともできる。

この他、上記文書中や図面中で示した処理手順、制御手順、具体的名称、各種のデータやパラメータを含む情報については、特記する場合を除いて任意に変更することができる。

また、図示した各装置の各構成要素は機能概念的なものであり、必ずしも物理的に図示のように構成されていることを要しない。すなわち、各装置の分散・統合の具体的形態は図示のものに限られず、その全部または一部を、各種の負荷や使用状況などに応じて、任意の単位で機能的または物理的に分散・統合して構成することができる。

以上のように、本発明にかかる自律移動体および移動方法は、分散センサネットワークおよびハイブリッドアーキテクチャの制御方式を用いる自律移動体に有用であり、特に、自律移動体の自己位置を高精度に推定する必要がある場合に適している。

本実施例にかかる自律移動体の構成の一例を示す図である。 上位制御機構および下位制御機構の構成を示す機能ブロック図である。 本実施例にかかる自己位置推定モジュールの構成例を示す図である。 推定確率分布の生成を説明するための図である。 上限・下限の設定を説明するための図である。 ビジョンにより検出された自律移動体の移動量ｕについての推定確率分布の上限・下限の設定手法を説明するための図である。 重み平均演算を説明するための図である。 移動量ｕの上限および下限の統合を説明する図である。 移動量ｖの上限および下限の統合を説明する図である。 変化量θの上限および下限の統合を説明する図である。 Ｐｌｓ分布の生成を説明する図である。 本実施例にかかる自律移動体の処理手順を示すフローチャート（１）である。 本実施例にかかる自律移動体の処理手順を示すフローチャート（２）である。 従来の技術を説明するための図である。

符号の説明

１０ 自律移動体（ロボット）
１１ 車輪
１２ モータ
１３，２１１，２１２ エンコーダ
１４ ＣＰＵ
１５ 移動量算出部
１００ 上位制御機構
１１０ 下位センサ情報取得モジュール
１２０ ステレオカメラ
１３０ ステレオカメラ入力部
１４０ ３次元環境計測部
１５０ 自己位置推定モジュール
１６０ 経路計画モジュール
２００ 下位制御機構
２１０ 下位センサ系
２１３ ジャイロ
２１４ 距離センサ
２２０ センサデータ記憶モジュール
２３０ 行動情報受付モジュール
２４０ 単純反応処理モジュール
２５０ 行動選択機構
２６０ 自律移動体駆動機構
２７０ カルマンフィルタ
３００ パーティクルフィルタ
３０１ エンコーダ・ジャイロ推定確率分布処理部
３０２ エンコーダ推定確率分布処理部
３０３ ビジョン推定確率分布処理部
３０４ 上限重み平均算出部
３０５ 下限重み平均算出部
３０６ Ｐｌｓ分布生成部
３０７ 統合処理部

Claims (6)

1. 上位センサによる環境測定を行い、パーティクルフィルタを用いて自律移動体の状態を推定して当該自律移動体の移動を制御する上位制御機構と、下位センサによる検出値から自律移動体の状態を推定して当該自律移動体の移動を制御する下位制御機構とを備える自律移動体であって、
   前記下位制御機構は、自律移動体の運動状態を予測するカルマンフィルタを備え、
   前記カルマンフィルタを用いて前記下位センサの検出値から自律移動体の期待値および分散を推定し、自律移動体の状態変化量の確率分布の上限および下限を設定する第１の設定手段と、
   前記環境測定により推定した各パーティクルにおける自律移動体の状態変化の信頼度に基づいて、自律移動体の状態変化の確率分布の上限および下限を設定する第２の設定手段と、
   自律移動体の状態毎に、前記第１および第２の設定手段の設定した上限を統合すると共に、前記第１および第２の設定手段の設定した下限を統合する統合手段と、
   前記統合手段の統合した上限および下限に基づいて、自律移動体の状態変化量の確率分布を算出する算出手段と、
   前記環境測定により推定した各パーティクルにおける自律移動体の状態変化の信頼度を、前記算出手段の算出した確率分布に基づいて修正する修正手段と、
   を備えることを特徴とする自律移動体。
2. 前記統合手段は、前記第１および第２の設定手段の設定した上限を重み平均することで各上限を統合すると共に、前記第１および第２の設定手段の設定した下限を重み平均することで各下限を統合することを特徴とする請求項１に記載の自律移動体。
3. 前記算出手段によって算出された確率分布に基づいて、前記下位制御機構に設定されている自律移動体の位置および姿勢の情報を修正する情報修正手段をさらに備えたことを特徴とする請求項１または２に記載の自律移動体。
4. 上位センサによる環境測定を行い、パーティクルフィルタを用いて自律移動体の状態を推定して当該自律移動体の移動を制御する上位制御機構と、下位センサによる検出値から自律移動体の状態を推定して当該自律移動体の移動を制御する下位制御機構とを備える自律移動体の移動方法であって、
   前記下位制御機構は、自律移動体の運動状態を予測するカルマンフィルタを備え、
   前記カルマンフィルタを用いて前記下位センサの検出値から自律移動体の期待値および分散を推定し、自律移動体の状態変化量の確率分布の上限および下限を設定する第１の設定工程と、
   前記環境測定により推定した各パーティクルにおける自律移動体の状態変化の信頼度に基づいて、自律移動体の状態変化の確率分布の上限および下限を設定する第２の設定工程と、
   自律移動体の状態毎に、前記第１および第２の設定工程により設定した上限を統合すると共に、前記第１および第２の設定工程により設定した下限を統合する統合工程と、
   前記統合工程において統合した上限および下限に基づいて、自律移動体の状態変化量の確率分布を算出する算出工程と、
   前記環境測定により推定した各パーティクルにおける自律移動体の状態変化の信頼度を、前記算出工程により算出した確率分布に基づいて修正する修正工程と、
   を含んだことを特徴とする移動方法。
5. 前記統合工程は、前記第１および第２の設定工程において設定した上限を重み平均することで各上限を統合すると共に、前記第１および第２の設定工程において設定した下限を重み平均することで各下限を統合することを特徴とする請求項４に記載の移動方法。
6. 前記算出工程によって算出された確率分布に基づいて、前記下位制御機構に設定されている自律移動体の位置および姿勢の情報を修正する情報修正工程をさらに含んだことを特徴とする請求項４または５に記載の移動方法。

Images