JP2008126401A - パーティクルフィルター基盤の移動ロボットの姿勢推定方法、装置及び媒体 - Google Patents

Abstract

【課題】 パーティクルフィルター基盤の移動ロボットの姿勢推定方法、装置及び媒体を提供する。
【解決手段】 移動ロボットの現在姿勢で現在パーティクルをサンプリングする段階、移動ロボットの姿勢変化量を感知して以前パーティクルに感知された姿勢変化量を適用して現在パーティクルの姿勢を求める段階、センサーから獲得したレンジデータとマップ情報から現在パーティクルの姿勢確率予測及び加重値を求める段階、加重値に基づいて現在パーティクルをリサンプリングする段階及びセンサーの誤差を考慮して加重値を調整する段階を含むパーティクルフィルター基盤の移動ロボットの姿勢推定方法。
【選択図】図１

Description

本発明は、パーティクルフィルター基盤の移動ロボットの姿勢推定方法及び装置において、さらに詳細には、パーティクルフィルターを用いた格子基盤（Ｇｒｉｄ−ｂａｓｅｄ）ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐ Ｂｕｉｌｄｉｎｇ）アルゴリズムにセンサーの誤差を反映し、加重値を調整して移動ロボットの姿勢を相対的に正確に推定する方法及び装置に関する。

最近、産業体で用いられる産業用ロボットだけでなく、一般家庭や事務室などで家事や事務補助用としてロボットが実用化されている。これに該当する代表的な例として掃除用ロボット、案内ロボット、防犯ロボットなどを挙げられる。

このようなロボットは、基本的に与えられた空間内で移動しつつ、ロボット自身の固有な機能を行う。ロボットが与えられた環境内で走行と機能を行うためには、周辺空間に対するマップの作成とロボットの位置及び方向角（Ｈｅａｄｉｎｇ ａｎｇｌｅ）についての情報を必要とする。このようなマップ作成と共に自身の位置を認識する方法としてＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）アルゴリズムが用いられる。

ＳＬＡＭアルゴリズムは、ある位置で周辺環境のマップを作成し、作成されたマップに基づいて再び動いたロボットの位置を把握する反復的な過程を通じてロボットの位置と周辺環境のマップを同時に推定しうる。

ＳＬＡＭアルゴリズムでは、ロボット自身の位置を推定するためにパーティクルフィルター技法が利用されうる。パーティクルフィルター技法とは、ロボットの位置及び方向角に対する予測値を有する複数個のサンプルを抽出し、各サンプルが実際ロボットの位置及び方向角である確率を用いてロボットの最適の姿勢を推定する方法である。本発明で言及する「姿勢（ｐｏｓｅ）」は、移動ロボットの平面上の２次元座標系位置（ｐｏｓｉｔｉｏｎ）及び方向角を意味する。

レンジデータ（ｒａｎｇｅ ｄａｔａ）を用いた格子基盤（Ｇｒｉｄ−ｂａｓｅｄ）ＳＬＡＭでは、特徴点（Ｆｅａｔｕｒｅ ｐｏｉｎｔｓ）を抽出せずとも、あらゆるレンジデータを用いてマップを作成することができる。しかし、従来には、レンジデータを得るために、正確度の優秀な距離測定器（Ｒａｎｇｅ ｆｉｎｄｅｒ）（以下、センサーと称する）を使用して初めて良質のマップが得られるので、センサーの正確度に大きく依存する問題があった。

一般的に商用化される掃除用ロボット、防犯ロボットのような事務用または家庭用ロボットにおいては、安価でありつつも、センサーの正確度の相対的に落ちる場合にも、マップ作成とロボットの位置とを正確に予測することが必要である。
日本公開特許第２００３−３２６４８４号公報

本発明は、前記問題点を考慮して案出されたものであって、マップ一貫性（Ｍａｐ ｃｏｎｓｉｓｔｅｎｃｙ）検査を通じてパーティクルフィルターで求めたパーティクルの加重値を調整することによって、移動ロボットの姿勢推定の正確度を高めることを目的とする。

これと共に、ロボットの方向角によるヒルクライミング（Ｈｉｌｌ−ｃｌｉｍｂｉｎｇ）を行うことで、効率的に移動ロボットの姿勢を推定することを目的とする。
本発明の目的は、前述した目的によって制限されず、言及していないさらなる目的は、下の記載から当業者に明確に理解されうる。

前述した目的を達成するための本発明の一実施例によるパーティクルフィルター基盤の移動ロボットの姿勢推定方法は、（ａ）移動ロボットの現在姿勢で現在パーティクルをサンプリングする段階と、（ｂ）前記移動ロボットの姿勢変化量を感知して以前パーティクルに前記感知された姿勢変化量を適用して前記現在パーティクルの姿勢を求める段階と、（ｃ）センサーから獲得したレンジデータとマップ情報から前記現在パーティクルの姿勢確率予測及び加重値を求める段階と、（ｄ）前記加重値に基づいて前記現在パーティクルをリサンプリングする段階と、（ｅ）前記センサーの誤差を考慮して前記加重値を調整する段階と、を含む。

前記目的を達成するための本発明の一実施例によるパーティクルフィルター基盤の移動ロボットの姿勢推定装置は、移動ロボットの姿勢変化量を感知する走行計と、以前パーティクルに前記感知された姿勢変化量を適用して現在パーティクルの姿勢を求めるパーティクル姿勢計算部と、レンジデータを獲得するセンサーと、前記レンジデータとマップ情報から前記現在パーティクルの姿勢確率予測及び加重値を求めるパーティクルフィルター部と、前記センサーの誤差を考慮して前記加重値を調整する加重値調整部と、を備える。

前記目的を達成するための本発明の一実施例によるパーティクルフィルター基盤の移動ロボットの姿勢推定方法は、（ａ）移動ロボットの姿勢変化量を感知して以前パーティクルに前記感知された姿勢変化量を適用して現在パーティクルの姿勢を求める段階と、（ｂ）センサーから獲得したレンジデータとマップ情報とから前記現在パーティクルの姿勢確率予測及び加重値を求める段階と、（ｃ）前記センサーの誤差を考慮して前記移動ロボットの姿勢を予測するために前記加重値を調整する段階と、を含む。

前記目的を達成するために前記実施例の方法を行うためのコンピュータで判読可能な命令語を保存するコンピュータ判読可能な媒体が提供される。

本発明のパーティクルフィルター基盤の移動ロボットの姿勢推定方法及び装置によれば、次のような効果が１つあるいはそれ以上ある。

第１に、比較的正確度の落ちる安価型センサーを使用しても、パーティクルフィルターを用いた格子基盤（Ｇｒｉｄ−ｂａｓｅｄ）ＳＬＡＭアルゴリズムにより高い正確度で移動ロボットの姿勢を推定できるという長所がある。

第２に、ロボットの方向角を考慮してヒルクライミング技法を行うことによって、計算時間と位置誤差とを減少させうる長所もある。

その他の実施例の具体的な事項は詳細な説明及び図面に含まれている。本発明の利点及び特徴、そしてそれらを達成する方法は、添付される図面と共に詳細に後述される実施例を参照すれば明確になる。しかし、本発明は、以下に開示される実施例に限定されるものではなく、相異なる多様な形態に具現されることができ、単に本実施例は、本発明の開示を完全にし、本発明が属する技術分野で当業者に発明の範ちゅうを完全に知らせるために提供されるものであり、本発明は、請求項の範ちゅうにより定義されるだけである。明細書全体に亙って同じ参照符号は、同じ構成要素を示す。

以下、本発明の望ましい実施例について添付された図面を参照してさらに詳細に説明する。

図１は、本発明の一実施例によるパーティクルフィルター基盤の移動ロボットの姿勢推定方法を示すフローチャートである。

本発明の一実施例によるパーティクルフィルター基盤の移動ロボットの姿勢推定方法は、移動ロボットの姿勢変化量を感知し、以前パーティクルに感知された姿勢変化量を適用して、現在パーティクルの姿勢を求める段階（Ｓ１００）、ヒルクライミング（ｈｉｌｌ−ｃｌｉｍｂｉｎｇ）技法によりパーティクルの姿勢を更新する段階（Ｓ１１０）、センサー６２０から獲得したレンジデータとマップ情報とから現在パーティクルの姿勢確率予測及び加重値を求める段階（Ｓ１２０）、加重値に基づいて現在パーティクルをリサンプリングする段階（Ｓ１３０）及びセンサー６２０の誤差を考慮して加重値を調整する段階（Ｓ１４０）を含む。

本発明の一実施例によるパーティクルフィルター基盤の移動ロボットの姿勢推定方法を実施するために移動ロボットが走行しつつ、マップを作成している状況を説明する。

まず、移動ロボットの出発地点の位置及び移動ロボットの進行方向である方向角が与えられる。または、移動ロボットの出発位置及び出発時の方向角を基準値に設定しうる。「姿勢」は、移動ロボットの２次元平面上での位置及び方向角を意味するので、移動ロボットの姿勢を推定するということは、移動ロボットの位置及び方向角を推定することを意味する。

与えられた出発点から移動ロボットが所定の距離を移動しうる。ロボットに装着されているロボットの移動量及び回転角を感知しうる走行計（ｏｄｏｍｅｔｒｙ）６００により移動ロボットの動きによる姿勢変化量を感知しうる。走行計６００の例として、エンコーダ及びジャイロを挙げられる。例えば、エンコーダによって移動距離及び方向を積分することによって、現在移動ロボットの姿勢が分かる。もし、積分誤差がなければ、エンコーダだけでも、ロボットの姿勢推定が可能であるが、エンコーダは、短い区間では、比較的正確であるが、数字を積分し続けることによって、誤差が累積される問題がある。また、移動ロボットは、移動中に滑るか障害物に合うことがあるので、エンコーダ値によるロボットの位置と実際位置との間には、誤差が発生しうる。

したがって、ロボットの姿勢をさらに正確に推定するために、パーティクルフィルターを用いたＳＬＡＭアルゴリズムを用いる。

サンプリング部６４０は、最初にＮ個のパーティクルをランダムにサンプリングする。移動ロボットの初期位置の近傍にＮ個の任意の地点、すなわち、次の数学式のようにＮ個のパーティクルを選択する。

ここで、ｘ_０ ^（ｊ）及びｗ_０ ^（ｊ）は、特定インデックスｊのパーティクルに対して移動ロボットの初期位置及び初期加重値を各々示す。前記ｗ_０ ^（ｊ）は、インデックスｊと関係なく、いずれも１／Ｎに同一に設定しうる。加重値は、現在パーティクルの位置が移動ロボットの位置となる確率を意味する。

ロボットが所定距離を移動した時には、所定時間が経ったので、経時的なパーティクルの姿勢を更新しうる。
パーティクル姿勢計算部６１０は、前述した移動ロボットに備えられた走行計６００により感知された姿勢変化量をパーティクルに適用してパーティクルの姿勢を求める（Ｓ１００）。

この際、パーティクル姿勢計算部６１０は、パーティクル確率の正確度を高めるために、ヒルクライミング方式を導入して各パーティクルの姿勢を更新しうる（Ｓ１１０）。

図２は、絶対座標系を基準にステップサイズの同一な従来のヒルクライミング方法（ａ）と、ロボットの方向角による相対座標系を基準にステップサイズの異なる修正されたヒルクライミング方法（ｂ）との座標軸を示す図面であり、図３は、図２の（ａ）、（ｂ）方法によって各パーティクルに対してヒルクライミングを行うことを示す図面である。図２（ａ）のように従来のヒルクライミング方式は、絶対座標軸に基づいて４方向の直線移動による変化Δｘ、−Δｘ、Δｙ、−Δｙと２方向の回転移動による変化Δθ、−Δθを前記求めたパーティクルの姿勢に各々加えた。言い換えれば、パーティクルが有しているロボットの姿勢に、前記６個の変化を各々さらに加えて、パーティクルの姿勢を若干変化させてロボットの位置確率が増加する方向にロボットの位置を変化させる方式である。従来のヒルクライミング方式は、Ｇ． Ｇｒｉｓｅｔｔｉ， Ｃ． Ｓｔａｃｈｎｉｓｓ， ａｎｄ Ｗ． Ｂｕｒｇａｒｄ， “Ｉｍｐｒｏｖｉｎｇ ｇｒｉｄ−ｂａｓｅｄ ｓｌａｍ ｗｉｔｈ Ｒａｏ−Ｂｌａｃｋｗｅｌｌｉｚｅｄ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒｓ ｂｙ ａｄａｐｔｉｖｅ ｐｒｏｐｏｓａｌｓ ａｎｄ ｓｅｌｅｃｔｉｖｅ ｒｅｓａｍｐｌｉｎｇ，” Ｉｎ Ｐｒｏｃ． ｏｆ ｔｈｅ ＩＥＥＥ Ｉｎｔ． Ｃｏｎｆ． ｏｎ Ｒｏｂｏｔｉｃｓ ＆ Ａｕｔｏｍａｔｉｏｎ （ＩＣＲＡ）， ｐｐ．２４４３−２４４８， Ｂａｒｃｅｌｏｎａ， Ｓｐａｉｎ， ２００５とＤ． Ｈaｈｎｅｌ， Ｄ． Ｓｃｈｕｌｚ， ａｎｄ Ｗ． Ｂｕｒｇａｒｄ， “Ｍａｐ ｂｕｉｌｄｉｎｇ ｗｉｔｈ ｍｏｂｉｌｅ ｒｏｂｏｔｓ ｉｎ ｐｏｐｕｌａｔｅｄ ｅｎｖｉｒｏｎｍｅｎｔｓ，” Ｉｎ Ｐｒｏｃ． ｏｆ ｔｈｅ ＩＥＥＥ／ＲＳＪ Ｉｎｔ． Ｃｏｎｆ． ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ（ＩＲＯＳ）， ２００２に詳述されているので、詳細な説明は省略する。そして、ロボット位置の確率を計算する過程は、後述する。

本発明で新たに提案する修正されたヒルクライミング方式は、ロボットの方向角を主軸として各パーティクルの姿勢を更新しうる。したがって、図２（ｂ）に示されたように、絶対座標系によるヒルクライミングでない、ロボットの方向角による相対座標系のｘ’軸とそれに垂直なｙ’軸を二軸としてヒルクライミングを行う。

さらに、ヒルクライミングステップサイズ（従来のΔｘ、Δｙに該当）を定数とせず、走行計６００モデルの誤差共分散楕円（Ｅｒｒｏｒ ｃｏｖａｒｉａｎｃｅ ｅｌｌｉｐｓｅ）によって適応的に変換可能にしうる。これは、ロボットの移動方向に走行計６００の誤差がさらに大きく発生する可能性が高いということを反映したものであり、ヒルクライミング隨行時間とパーティクルの位置誤差を減少させうる。これは、下記数学式（２）のように定義されうる。

ここで、（ｘ，ｙ）は、絶対座標系を基準とするヒルクライミング方式の軸であり、（ｘ’，ｙ’）は、ロボットの方向角による相対座標系を基準とするヒルクライミング方式の軸を示す。

図３の（ａ）と（ｂ）は、前述した２つのヒルクライミング方法により各パーティクル２１０の姿勢を更新することを示す図面であるが、図３（ａ）では、絶対座標軸を基準にロボット２００の方向角と関係なく、±Δｘ、±Δｙ、±Δθほどパーティクル２１０の位置を移しつつ、位置確率の増加方向に位置を移る。一方、図３（ｂ）では、ロボット２００の移動方向に該当する方向角による相対座標軸を基準に±Δｘ’，±Δｙ’，±Δθほどパーティクル２１０の位置を移しつつ、ヒルクライミング技法を行う。この際、ステップの間隔は、ロボット２００の進行方向である±Δｘ’方向には、±Δｙ’に比べて大きいということが分かる。

したがって、ｘ軸移動（±Δｘ）とｙ（±Δｙ）軸移動及び回転移動（±Δθ）（望ましくは、相対座標系によるｘ‘軸移動（±Δｘ’）とｙ‘（±Δｙ’）軸移動及び回転移動（±Δθ））の６つの場合に対してヒルクライミングを適用して各々に対して後述する数５によりスコア（ｓｃｏｒｅ）を計算しうる。スコアは、ヒルクライミングにより算出される加重値であって、ヒルクライミングの終了後に、最もスコアの高い値が該当パーティクルの加重値となり、スコアの最も高い値の位置及び方向角が該当パーティクルの姿勢となる。

前記のヒルクライミングは、数回に亙って行われる。言い換えれば、６つの場合についてヒルクライミングを行い、最も高いスコアに該当する値を該当パーティクルの姿勢として再び同じ過程を反復して最も高いスコアの値を探す試みをする。そのように試みた場合のうち、最も高いスコアを有する値を該当パーティクルの加重値で更新し、その時のロボットの位置及び方向角が該当パーティクルの姿勢に更新する。

移動ロボットが所定距離を移動した後、センサー６２０により新たな観察情報のレンジデータを得て、このようなレンジデータと既存のマップ情報によりパーティクルフィルター部６３０は、パーティクルの姿勢確率を予測して加重値を計算しうる（Ｓ１２０）。

加重値は、パーティクルの姿勢が推定される移動ロボットの現在姿勢と類似しているほど大きくなり、よって、加重値が高いほど、移動ロボットが、そのような加重値を有するパーティクルの姿勢を有する可能性が高いと言える。このように、観察情報は、移動ロボットに備えられた周囲情報、特に、周辺障害物との距離を認識しうるセンサー６２０により得られる。このようなセンサー６２０は、超音波または赤外線センサー６２を用いたり構造光（Ｓｔｒｕｃｔｕｒｅｄ ｌｉｇｈｔ）モジュールを用いてロボットの周囲の障害物との距離を感知しうる。

パーティクルフィルターを用いてパーティクルの姿勢確率を予測して加重値を計算する方法について説明する。

移動ロボットの位置を確率分布として示し、このような確率分布（ｐ）は、以前ステップ（時間ｔ−１）と現在ステップ（時間ｔ）とのデータを用いる次の数学式（３）により示しうる。

ここで、ｘ_ｔ−１は、時間ｔ−１での姿勢であり、ｕ_ｔは、時間ｔ（現在）での走行計６００により感知されたデータであり、ｘ_ｔは、時間ｔでの姿勢である。ｍ_ｔ−１は、時間ｔ−１でのマップ上のデータであり、ｚ_ｔは、時間ｔでのセンサー６２０によって得られたデータである。前記確率分布は、ロボットが動的ノイズやシステムに存在する他の不確実な因子を有したまま

動いた時、時間ｔでのロボット姿勢に対する確率分布を意味する。

従来のＧｒｉｄ−ｂａｓｅｄ ＳＬＡＭアルゴリズムは、式（３）を近似化させて計算した。センサー６２０の正確度の走行計６００に比べて非常に高い場合に、例えば、図４（ａ）のように走行計６００とセンサー６２０との正確度の標準偏差比が３０：１である場合、観察領域

では、センサー６２０によるロボットの姿勢確率値

が急変するが、走行計６００によるロボットの姿勢確率値

は、定数（ｋ）で近似化されうる。したがって、式（３）は、次のように数学式（４）によって近似化されうる。

現在パーティクルの加重値は、次の数学式（５）により近似化させて計算しうる。

前述した数学式（３）、（４）、（５）によるパーティクルの姿勢を予測して加重値を求める過程は、 Ｇ． Ｇｒｉｓｅｔｔｉ， Ｃ． Ｓｔａｃｈｎｉｓｓ， ａｎｄ Ｗ． Ｂｕｒｇａｒｄ， “Ｉｍｐｒｏｖｉｎｇ ｇｒｉｄ−ｂａｓｅｄ ｓｌａｍ ｗｉｔｈ Ｒａｏ−Ｂｌａｃｋｗｅｌｌｉｚｅｄ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒｓ ｂｙ ａｄａｐｔｉｖｅ ｐｒｏｐｏｓａｌｓ ａｎｄ ｓｅｌｅｃｔｉｖｅ ｒｅｓａｍｐｌｉｎｇ，” Ｉｎ Ｐｒｏｃ． ｏｆ ｔｈｅ ＩＥＥＥ Ｉｎｔ． Ｃｏｎｆ． ｏｎ Ｒｏｂｏｔｉｃｓ ＆ Ａｕｔｏｍａｔｉｏｎ （ＩＣＲＡ）， ｐｐ．２４４３−２４４８， Ｂａｒｃｅｌｏｎａ， Ｓｐａｉｎ， ２００５に詳述されている。

パーティクルフィルター部６３０が前記数学式（３）、（４）、（５）によってパーティクルの姿勢確率を予測して加重値を計算した後、サンプリング部６４０は、加重値に基づいて現在パーティクルをリサンプリングする（Ｓ１３０）。初期にＮ個のパーティクルから出発して時間カウントｔが１増加したとき、前記Ｎ個のパーティクルのうち、加重値の低いパーティクルを脱落させ、加重値の高いパーティクルを中心にサンプリングして、再びＮ個のパーティクルを保持させる。これをリサンプリングと称する。

次いで、マップ評価（ｍａｐ ｅｓｔｉｍａｔｉｏｎ）を通じたグリッドマップ（ｇｒｉｄ ｍａｐ）を更新する時、加重値調整部６５０では、センサー６２０の誤差を考慮して加重値を調整する（Ｓ１４０）。

本発明の一実施例では、センサー６２０の正確度が優秀ではないので、走行計６００によるロボットの姿勢確率値

を定数として適用せず、加重値を計算する方法を提示する。すなわち、前述した方法で求めた加重値をリサンプリングしてから後述する方法で調整を行う。

センサー６２０の正確度が優秀ではない場合には、走行計６００による姿勢確率値を定数として近似化できない。すなわち、図４（ｂ）のように走行計６００とセンサー６２０との正確度の標準偏差比が１０：１である場合には、観察領域

で走行計６００によるロボットの姿勢確率値

は、定数ｋとして近似化されられない。

図５は、走行計６００とセンサー６２０との正確度標準偏差が、３０：１である場合（ａ）と、１０：１である場合（ｂ）とに数学式（３）による正確な式による姿勢確率値と数学式（４）による近似化した式による姿勢確率値を示し、正確な式と近似化した式の誤差比（ｒ（ｘ））を示す図面である。

３０：１である場合には、走行計６００によるロボットの姿勢確率値を定数としても、実際に積分を行う区間であるロボットの推定位置付近（図５（ａ）のＡ部分）では、誤差がほとんどないので、誤差比ｒ（ｘ）が１に近いことが分かる。一方、１０：１である場合には、センサー６２０の正確度が相対的に高くないために、誤差が大きくなる。これは、図５（ｂ）に分布される誤差比ｒ（ｘ）が、ロボット位置と推定される領域（図５（ｂ）のＡ領域）で相対的に大きく変化することから分かる。

したがって、図５（ｂ）に示される誤差比ｒ（ｘ）を、確率の計算に考慮することによって、走行計６００によるロボットの姿勢確率値を定数とせず、変化量で計算しうる。
誤差比率ｒ（ｘ）を考慮すれば、次のような加重値調整式が得られる。

加重値調整を効率的にするためにログ（ｌｏｇ）式に変換すれば、次の式（７）のようである。

数学式（５）をログ式に変換した後、数学式（７）と比較すれば、数学式（７）で、Ｒ（ｘ）の大きさを差し引く点で異なる。これは、センサー６２０の誤差が比較的大きい場合に誤差により生じる量であって、各センサー６２０の誤差特性によってその形態と大きさとが異なりうる。

この際、数学式（７）のＲ（ｘ）は、Ｐ（ｘ）と格子セル占有確率（Ｇｒｉｄ ｃｅｌｌ ｏｃｃｕｐａｎｃｙ）ｃ_ｊとの積で示しうる。下記数学式（８）によって現れるＰ（ｘ）の形態は、センサー６２０とセンサー６２０から感知された領域を連結する線であるグリッド線に沿ってほう物線形態に図６に示される。数学式（８）のＰ（ｘ）を示す図６のグラフは、センサー６２０によってその形態と値とが多様に異なるということは言うまでもない。

数学式（８）で、ｍは、センサー６２０の正確度によって左右される値であり、

は、グリッドセル占有確率値であって、０と１との間の値である。また、ｘは、各パーティクルの姿勢上で有しているマップ上の格子点（Ｇｒｉｄ ｐｏｉｎｔ）までの距離であり、Ｄは、現在移動ロボットのセンサー６２０により測定された格子点までの距離である。したがって、現在移動ロボットのセンサー６２０により測定された格子点データが正しく、各パーティクルの姿勢が現在移動ロボットの実際姿勢に近接していれば、ｘとＤとの値にほとんど違いがなく、結果的にＰ（ｘ）とＲ（ｘ）との値が小さくなる。しかし、逆に、パーティクルで有している姿勢が現在移動ロボットの実際姿勢と非類似であれば、ｘ値とＤ値との間に大差が発生して、Ｐ（ｘ）とＲ（ｘ）との値が大きくなる。これを、マップ一貫性検査（Ｍａｐ ｃｏｎｓｉｓｔｅｎｃｙ ｃｈｅｃｋ）と称する。

したがって、前述したようにあらゆるパーティクルに対してマップ一貫性検査によりＲ（ｘ）を検査して、各パーティクルの加重値を調整することができる。このような各パーティクルに対する加重値調整は、グリッドマップを更新する時に、行われるので、追加的な計算時間の増加はほとんどない。
ロボットは、前述した段階を反復しつつ、移動中のロボットの姿勢を推定しうる。

図７は、本発明の一実施例によるパーティクルフィルター基盤の移動ロボットの姿勢推定装置のブロック図である。

本発明の一実施例にパーティクルフィルター基盤による移動ロボットの姿勢推定装置は、走行計６００、パーティクル姿勢計算部６１０、センサー６２０、パーティクルフィルター部６３０、加重値調整部６５０、サンプリング部６４０、マップ生成部６６０を備えうる。

この際、本実施例で使われる「〜部」という用語は、ソフトウェアまたはＦＰＧＡまたはＡＳＩＣのようなハードウェア構成要素を意味し、「〜部」は所定の役割を行う。しかし、「〜部」は、ソフトウェアまたはハードウェアに限定される意味ではない。「〜部」は、アドレッシング可能な記録媒体に位置させてもよく、１つまたはそれ以上のプロセッサーを再現させるように構成されても良い。したがって、一例として、「〜部」は、ソフトウェアの構成要素、客体指向ソフトウェア構成要素、クラス構成要素及びタスク構成要素のような構成要素と、プロセス、関数、属性、プロシージャ、サブルーチン、プログラムコードのセグメント、ドライバー、ファームウェア、マイクロコード、回路、データ、データベース、データ構造、テーブル、アレイ、及び変数を含む。構成要素と「〜部」内で提供される機能は、さらに少数の構成要素及び「〜部」で結合されるか、追加的な構成要素と「ユニット」とにさらに分離されうる。

走行計６００は、移動ロボットの姿勢変化量を感知するものであって、走行計６００の一例としてエンコーダまたはジャイロを挙げられる。

パーティクル姿勢計算部６１０は、走行計６００により感知された姿勢変化量に基づいて各パーティクルの姿勢を求める。この際、従来のヒルクライミング技法またはロボットの方向角によって座標軸の方向とステップの大きさとを異ならせるヒルクライミング技法によりパーティクルの位置確率が増加する方向にパーティクルの姿勢を更新することが望ましい。

センサー６２０は、移動ロボットの外部に備えられて観察情報、特に、周辺障害物との距離を認識してレンジデータを獲得する。本発明においては、比較的センサー６２０の正確度の落ちる構造光センサーを使用しうる。

パーティクルフィルター部６３０は、センサー６２０から獲得したレンジデータとマップ情報とから現在パーティクルの姿勢確率を予測して加重値を求める。

加重値調整部６５０は、センサー６２０の誤差を考慮してパーティクルフィルター部で求めた加重値を調整する。この際、現在パーティクル位置でマップ上の距離とセンサー６２０から測定した距離によって、センサー６２０によって既定の誤差特性値で加重値を調整することができる。

サンプリング部６４０は、移動ロボットの最初位置に基づいてパーティクルをサンプリングし、ロボットの移動後に、パーティクルフィルター部６３０で求めた加重値に基づいてパーティクルをリサンプリングする。この際、リサンプリングは、加重値の低いパーティクルは、脱落させ、加重値の高いパーティクルを中心にサンプリングを行える。

マップ生成部６６０は、ロボットが移動しつつセンサー６２０から獲得したレンジデータに基づいてマップを生成して更新する。

以上、添付された図面を参照して本発明の実施例を説明したが、本発明が属する技術分野で当業者ならば、本発明がその技術的思想や必須な特徴を変更せずとも、他の具体的な形態に実施されうるということを理解できるであろう。したがって、前述した実施例は、あらゆる面で例示的なものであり、限定的なものではないという点を理解せねばならない。

本発明によれば、パーティクルフィルター基盤の移動ロボットの姿勢推定方法、装置及び媒体関連の技術分野に好適に適用されうる。

本発明の一実施例によるパーティクルフィルター基盤の移動ロボットの姿勢推定方法を示すフローチャートである。 絶対座標系を基準にステップサイズの同じ従来のヒルクライミング方法（ａ）とロボットの方向角による相対座標系を基準にステップサイズの異なる修正されたヒルクライミング方法（ｂ）との座標軸を示す図である。 図２の（ａ）、（ｂ）方法によって各パーティクルに対してヒルクライミングを行う姿を示す図である。 走行計とセンサーの正確度標準偏差が３０：１である場合（ａ）と１０：１である場合（ｂ）とに走行計とセンサーによるロボットの姿勢確率値を示す図である。 走行計とセンサーとの正確度標準偏差が３０：１である場合（ａ）と１０：１である場合（ｂ）とに、数学式（３）による正確な式による姿勢確率値と数学式（４）による近似化した式による姿勢確率値の一例を示し、正確な式と近似した式との誤差比（ｒ（ｘ））を示す図である。 本発明の一実施例によるセンサーの誤差特性によって加重値を再調整する関数を示すＰ（ｘ）を示すグラフである。 本発明の一実施例によるパーティクルフィルター基盤の移動ロボットの姿勢推定装置のブロック図である。

符号の説明

６００ 走行計
６１０ パーティクル姿勢計算部
６２０ センサー
６３０ パーティクルフィルター部
６４０ サンプリング部
６５０ 加重値調整部
６６０ マップ生成部

Claims (23)

1. （ａ）移動ロボットの現在姿勢で現在パーティクルをサンプリングする段階と、
   （ｂ）前記移動ロボットの姿勢変化量を感知して以前パーティクルに前記感知された姿勢変化量を適用して前記現在パーティクルの姿勢を求める段階と、
   （ｃ）センサーから獲得したレンジデータ及びマップ情報から前記現在パーティクルの姿勢確率予測及び加重値を求める段階と、
   （ｄ）前記加重値に基づいて前記現在パーティクルをリサンプリングする段階と、
   （ｅ）前記センサーの誤差を考慮して前記加重値を調整する段階と、を含むパーティクルフィルター基盤の移動ロボットの姿勢推定方法。
2. 前記（ｂ）段階において、ヒルクライミング方法を用いて位置確率が増加する方向に前記現在パーティクルの姿勢を更新する段階をさらに含む請求項１に記載のパーティクルフィルター基盤の移動ロボットの姿勢推定方法。
3. 前記（ｂ）段階において、前記移動ロボットの方向角を主軸とする相対座標系を基準にヒルクライミング方法を用いて位置確率が増加する方向に前記現在パーティクルの姿勢を更新する段階をさらに含む請求項１に記載のパーティクルフィルター基盤の移動ロボットの姿勢推定方法。
4. 前記ヒルクライミング方法は、ステップサイズを前記ロボットの進行方向に対して大きくなるように、前記姿勢変化量を感知する走行計モデルの誤差共分散楕円によって適応的に変える請求項３に記載のパーティクルフィルター基盤の移動ロボットの姿勢推定方法。
5. 前記（ｅ）段階において、前記加重値は、前記現在パーティクルの位置でマップ上の距離と前記センサーにより測定された距離とに基づいて前記センサーによって定義される誤差特性値に調整される請求項１に記載のパーティクルフィルター基盤の移動ロボットの姿勢推定方法。
6. 前記姿勢変化量は、エンコーダまたはジャイロにより感知されることを含む請求項１に記載のパーティクルフィルター基盤の移動ロボットの姿勢推定方法。
7. 前記姿勢は、平面上の２次元座標系の各座標値及び方向角を含む請求項１に記載のパーティクルフィルター基盤の移動ロボットの姿勢推定方法。
8. 前記センサーは、構造光センサーを含む請求項１に記載のパーティクルフィルター基盤の移動ロボットの姿勢推定方法。
9. 前記リサンプリングは、前記加重値の低いパーティクルは脱落させ、前記加重値の高いパーティクルを中心にサンプリングする請求項１に記載のパーティクルフィルター基盤の移動ロボットの姿勢推定方法。
10. 移動ロボットの姿勢変化量を感知する走行計と、
    以前パーティクルに前記感知された姿勢変化量を適用して現在パーティクルの姿勢を求めるパーティクル姿勢計算部と、
    レンジデータを獲得するセンサーと、
    前記レンジデータ及びマップ情報から前記現在パーティクルの姿勢確率予測及び加重値を求めるパーティクルフィルター部と、
    前記センサーの誤差を考慮して前記加重値を調整する加重値調整部と、を備えるパーティクルフィルター基盤の移動ロボットの姿勢推定装置。
11. 前記パーティクル姿勢計算部は、ヒルクライミング方法を用いて位置確率が増加する方向に前記現在パーティクルの姿勢を更新する請求項１０に記載のパーティクルフィルター基盤の移動ロボットの姿勢推定装置。
12. 前記パーティクル姿勢計算部は、前記移動ロボットの方向角を主軸とする相対座標系を基準にヒルクライミング方法を用いて位置確率が増加する方向に前記現在パーティクルの姿勢を更新する請求項１０に記載のパーティクルフィルター基盤の移動ロボットの姿勢推定装置。
13. 前記ヒルクライミング方法は、ステップサイズを前記ロボットの進行方向に対して大きくなるように、前記姿勢変化量を感知する走行計モデルの誤差共分散楕円によって適応的に変える請求項１２に記載のパーティクルフィルター基盤の移動ロボットの姿勢推定装置。
14. 前記センサーは、構造光センサーを含む請求項１０に記載のパーティクルフィルター基盤の移動ロボットの姿勢推定方法。
15. 前記加重値調整部は、前記現在パーティクルの位置でマップ上の距離と前記センサーにより測定された距離に基づいて前記センサーによって定義される誤差特性値で前記加重値を調整する請求項１０に記載のパーティクルフィルター基盤の移動ロボットの姿勢推定装置。
16. 前記走行計は、エンコーダまたはジャイロを含む請求項１０に記載のパーティクルフィルター基盤の移動ロボットの姿勢推定装置。
17. 前記姿勢は、平面上の２次元座標系の各座標値及び方向角を含む請求項１０に記載のパーティクルフィルター基盤の移動ロボットの姿勢推定装置。
18. 前記レンジデータから前記マップを生成するマップ生成部をさらに含む請求項１０に記載のパーティクルフィルター基盤の移動ロボットの姿勢推定装置。
19. 前記移動ロボットの最初位置に基づいて前記パーティクルをサンプリングし、前記パーティクルフィルター部で求めた加重値に基づいてパーティクルをリサンプリングするサンプリング部をさらに含む請求項１０に記載のパーティクルフィルター基盤の移動ロボットの姿勢推定装置。
20. 前記リサンプリングは、前記加重値の低いパーティクルは脱落させ、前記加重値の高いパーティクルを中心にサンプリングする請求項１９に記載のパーティクルフィルター基盤の移動ロボットの姿勢推定装置。
21. 請求項１に記載の方法を行うための少なくとも１つのプロセッサーを制御するコンピュータで判読可能な命令語を保存するコンピュータ判読可能媒体。
22. （ａ）移動ロボットの姿勢変化量を感知して以前パーティクルに前記感知された姿勢変化量を適用して現在パーティクルの姿勢を求める段階と、
    （ｂ）センサーから獲得したレンジデータ及びマップ情報から前記現在パーティクルの姿勢確率予測及び加重値を求める段階と、
    （ｃ）前記センサーの誤差を考慮して前記移動ロボットの姿勢を予測するために前記加重値を調整する段階と、を含むパーティクルフィルター基盤の移動ロボットの姿勢推定方法。
23. 請求項２２に記載の方法を行うための少なくとも１つのプロセッサーを制御するコンピュータで判読可能な命令語を保存するコンピュータ判読可能媒体。

Images