JP2008071352A - 移動ロボットの姿勢推定装置および方法 - Google Patents

Abstract

【課題】 移動ロボットに適用されるＳＬＡＭアルゴリズムの演算複雑性を減少させる方法および装置を提供する。
【解決手段】 移動ロボットの姿勢変化量を感知するセンサと、以前パーティクルに前記感知された姿勢変化量を適用して、現在パーティクルの姿勢および加重値を求めるパーティクルフィルタ部と、前記求めた姿勢および加重値に進化アルゴリズムを適用して、前記現在パーティクルの姿勢および加重値を更新する進化適用部とを含む複数のパーティクルを用いるパーティクルフィルタ基盤の移動ロボットの姿勢推定装置。
【選択図】 図１

Description

本発明は、移動ロボットに関し、より詳しくは、移動ロボットに適用されるＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）アルゴリズムの演算複雑性（ｃｏｍｐｕｔａｔｉｏｎａｌ ｃｏｍｐｌｅｘｉｔｙ）を減少させる方法および装置に関するものである。

一般的に移動ロボットは、産業用として開発されて工場自動化の一環として用いられたり、人間が耐えられない極限の環境で人間の代わりに作業を行う時に用いられたりしてきた。このような移動ロボット工学分野は、近来に入って最先端の宇宙開発産業に使われながら発展を繰り返して、最近人間親和的な家庭用移動ロボットの開発にまで至った。加えて、移動ロボットは医療用機器に代えて人間生体内に投入されることによって、既存の医療用機器では治療が不可能であった微細な人間生体組織の治療にまで用いられる。このような目覚ましい移動ロボット工学の発展は、インターネットによる情報革命と、これに引き続く生命工学分野とに代えて、新しく登場する最先端分野として注目を集めている。特に、最近では掃除用移動ロボットなどの家庭用移動ロボットのように人間の生活環境周辺で容易に接することができる形態の移動ロボットが大衆化している。

移動ロボットの移動において必須といえるローカライゼーション技術は、移動ロボットが周辺環境を基準にして自身の位置を決定する過程である。例えば、従来のローカライゼーションシステムで、移動ロボットはその周辺環境のマップを受ける。このような従来のローカライゼーションシステムは、手動でマップを生成することが相対的に難しく、専門的な作業を要するという側面で不利な点がある。さらに、一般的に多様な環境というものは固定的ではない。例えば、部屋で家具を移動させる時は既存のマップが役に立たない。結局、従来のローカライゼーションシステムで、正確な周辺環境を示すために、マップは相対的に頻繁に、また、高い費用で更新されなければならない。

したがって、移動ロボットが周辺の空間的情報が全くない地域で自身の相対的位置移動値と周辺環境の特徴点とを用いて、自身の空間上の絶対位置を予測して移動経路を計画して動的にマップを作成する技術が要求される。

このように、移動ロボットが相手位置値と周辺環境とを用いて自身の空間上の絶対位置を予測するために用いられる最も代表的な方法として、ＳＬＡＭアルゴリズムが知られている。これによれば、移動ロボットは、周辺の空間的情報、すなわちマップ情報を事前に知っていなくても、周辺情報を超音波、レーザーなどを用いて得た距離情報やカメラを用いて得た映像情報を活用して、特徴点を抽出してマップ情報を構成する。そして、移動ロボットは、前記マップ情報を用いて自身の位置を同時に知ることができる。

ロボットの現在位置で特徴点を用いて自身の位置を予測するために、一般的にカルマンフィルタ（Ｋａｌｍａｎ Ｆｉｌｔｅｒ）が多く用いられる。しかし、カルマンフィルタを用いる方法は、予想位置誤差の分布がガウス分布に従う場合にだけ有効であるという短所がある。これを克服するために、誤差の分布がガウス分布に従わない場合、位置予測値のサンプルを抽出して各サンプルの値が真である確率を用いて最適位置を予測する方法、すなわちパーティクルフィルタ技法も知られている。

パーティクルフィルタ技法と関連した従来技術としてエボリューション・ロボティックス（Ｅｖｏｌｕｔｉｏｎ Ｒｏｂｏｔｉｃｓ）社の特許文献１がある。前記特許文献１は、映像情報を用いたＳＬＡＭ技術による移動体の位置を連続的に計算するシステムおよび方法に関するものである。特に、前記特許文献１は、パーティクルフィルタを用いたＳＬＡＭ方式であって、２つのパーティクルセットを用いて複数のセンサ（オドメトリ（ｏｄｏｍｅｔｒｙ）とカメラ）を接合（ｆｕｓｉｏｎ）する技術を開示する。

しかし、一般的にパーティクルフィルタを用いたＳＬＡＭ技法は、一般的に多くのサンプルが要求されるため演算量が増加して、移動ロボットが移動中にリアルタイムで自身の位置を予測することが難しくなる。したがって、パーティクルフィルタを用いたＳＬＡＭアルゴリズムで、リアルタイムでロボット自身の位置を予測できるように演算量を減少させられる技術が要求される。
米国特許第７０１５８３１号公報 米国公開第２００６−１２４９３号公報

本発明が達成しようとする技術的課題は、パーティクルフィルタに基づいたＳＬＡＭ技法を用いる移動ロボットにおいて、演算複雑性を低くすることによってリアルタイムローカライゼーションおよびマップ作成を可能にする方法および装置を提供することにある。

本発明の技術的課題は、以上で言及した技術的課題に制限されず、言及しないまた他の技術的課題は下記記載によって当業者に明確に理解できるものである。

本発明の一態様に係わる姿勢推定装置は、複数のパーティクルを用いるパーティクルフィルタ基盤の移動ロボットの姿勢推定装置であって、移動ロボットの姿勢変化量を感知するセンサと、以前パーティクルに前記感知された姿勢変化量を適用して、現在パーティクルの姿勢および加重値を求めるパーティクルフィルタ部と、前記求めた姿勢および加重値に進化アルゴリズムを適用して、前記現在パーティクルの姿勢および加重値を更新する進化適用部とを含む。

本発明の他の態様に係わる姿勢推定方法は、複数のパーティクルを用いるパーティクルフィルタ基盤の移動ロボットの姿勢推定方法であって、移動ロボットの姿勢変化量を感知するステップと、以前パーティクルに前記感知された姿勢変化量を適用して、現在パーティクルの姿勢および加重値を求めるステップと、前記求めた姿勢および加重値に進化アルゴリズムを適用して、前記現在パーティクルの姿勢および加重値を更新するステップとを含む。

前記本発明によれば、移動ロボットの姿勢を推定するためのパーティクルフィルタ技法に進化アルゴリズムを適用することによって、要求されるパーティクルの数を減少させられる。
これによって、ＳＬＡＭ実行のための演算速度が向上して必要なメモリの大きさが減少する効果がある。

以下、添付した図面を参照して、本発明の一実施形態を詳細に説明する。

本発明は、既存のカルマンフィルタ基盤のＳＬＡＭ技法から発生する様々な問題、例えば、非ガウス分布に対処することができない問題、多重仮説（Ｍｕｌｔｉｐｌｅ ｈｙｐｏｔｈｅｓｉｓ）による問題などを解決するために基本的にはパーティクルフィルタアルゴリズムを採択する。前記多重仮説による問題とは、いくつかの特徴点マッチング仮説が発生する時、一度誤った仮説を採用すれば再びこの問題を解決できなくなることを意味する。

また、本発明は、前記パーティクルフィルタで少ない数のパーティクルを用いて予測された特徴点の位置と観測された特徴点とのマッチングによって移動ロボットの最適の姿勢を推定するが、前記最適推定のために遺伝的アルゴリズム（Ｇｅｎｅｔｉｃ Ａｌｇｏｒｉｔｈｍ）のような進化アルゴリズム（Ｅｖｏｌｕｔｉｏｎａｒｙ Ａｌｇｏｒｉｔｈｍ）が用いられる。前記推定された最適位置および特徴点マッチングによって特徴点の最近の最適位置が計算される。

上述したように、本発明の目的は、既存のパーティクルフィルタ技法にてより少ない数のパーティクルを用いて演算複雑性を減少させながらも、既存と同一であるか、またはより優れた性能を出すことにある。ここで、優れた性能とは、位置誤差の減少を意味する。パーティクルフィルタに前記遺伝的アルゴリズムを追加して適用することに伴って演算複雑性が多少増加するようになるが、パーティクルの数の減少に伴う演算複雑性の減少の大きさがより大きいならば、全体的な演算複雑性は減少できる。

図１は、本発明の一実施形態に係る移動ロボット１００および姿勢推定部２００の構成を示すブロック図である。移動ロボット１００は、ロボット制御部１１０、走行部１２０、エンコーダ１３０、カメラ１４０、および姿勢推定部２００を含む。

走行部１２０は、移動ロボット１００が移動することができるように動力を提供する。走行部１２０は、一般的に複数のホイールと方向制御装置などを含むが、移動ロボット１００の移動ができる限り、他の公知の走行手段で成されても構わない。

エンコーダ１３０は、走行部１２０に含まれる走行輪の回転速度を感知する。これによって、移動ロボット１００の以前位置と現在位置との間の位置変化および方向変化を測定する。エンコーダ１３０は、一般的にロボットの位置移動または方向変化の指令を下し、ロボットが指令に追従して動く時、その動きを制御する目的で設けられる。エンコーダ１３０を利用すれば動いた距離および方向を積分して現在移動ロボットの位置が分かる。積分誤差がなければ、エンコーダ１３０だけでもロボットのローカライゼーションが可能であるが、エンコーダは短い期間には比較的正確であるがサンプリングするごとに誤差が累積する短所がある。移動ロボット１００の動きを感知（ｓｅｎｓｉｎｇ）するセンサとしては、前記エンコーダ１３０の代わりにジャイロ（Ｇｙｒｏｓｃｏｐｅ）などの慣性センサが用いられても構わない。

カメラ１４０は、外部のイメージを捉えてこれをデジタル信号に変換する装置である。一般的にカメラ１４０は、ＣＣＤモジュールやＣＭＯＳモジュールを含んで構成される。

ロボット制御部１１０は、移動ロボット１００の走行部１２０、カメラ１４０などの機能を制御する。すなわち、ロボット制御部１１０は、移動ロボット１００がユーザの命令した位置に移動するように制御したり、カメラ１４０の映像捕捉動作を制御したりすることができる。

姿勢推定部２００は、パーティクルフィルタに進化アルゴリズムを適用して、移動ロボット１００の位置推定を行う装置である。併せて、姿勢推定部２００は、前記位置推定と共にマップ作成動作を行うこともできる。

姿勢推定部２００は、パーティクルフィルタ部２１０、特徴点処理部２２０、進化適用部２３０、およびメモリ２４０を含んで構成することができる。

特徴点処理部２２０は、カメラ１４０に捉えられた映像を処理して特徴点を抽出し、抽出された特徴点に基づいてメモリ２４０に既に格納された特徴点を更新（変更または追加）する。

パーティクルフィルタ部２１０はパーティクルフィルタを適用する。すなわち、複数のパーティクルを選択して移動ロボット１００の各移動段階毎に前記選択されたパーティクルの加重値を変化させ、前記変化した加重値に基づいて前記選択されたパーティクルを再サンプリングする。

進化適用部２３０は、前記選択された各々のパーティクルに対して進化アルゴリズムを適用して進化したパーティクルおよび前記進化したパーティクルに対する加重値（進化した加重値）を求める。

メモリ２４０は、現在まで捉えられた複数の特徴点を格納しており、このような特徴点を利用して１つの完成されたマップを構成することができる。

図２は、姿勢推定部２００の詳細構成を示すブロック図である。
先ず、特徴点処理部２２０は、特徴点抽出部２２１、特徴点変換部２２２、および特徴点マッチング部２２３を含んで構成することができる。

特徴点抽出部２２１は、カメラ１４０によって捉えられたイメージからスケールが不変（ｓｃａｌｅ−ｉｎｖａｒｉａｎｔ）し、かつ回転が不変（ｒｏｔａｔｉｏｎ−ｉｎｖａｒｉａｎｔ）する特徴点を抽出する。ＳＬＡＭの過程で、このような特徴点はマップ生成の基礎になるが、いわゆるランドマークとも呼ばれる。ロボットが移動する時、特徴点は大きくなったり小さくなったりもし、既存の特徴点に対して回転したりもする。このような種類の変化が発生した後に特徴点を抽出するためには、スケールおよび回転に不変する特徴点を選んでランドマークとしなければならない。特徴点を抽出する過程は、次の（ａ）〜（ｅ）ステップの細部段階で行われる。

（ａ）原イメージを得るステップ
原イメージはＣＣＤカメラからサンプリングされた映像からデジタル化された映像を意味する。

（ｂ）ガウスぼかし（Ｇａｕｓｓｉａｎ ｂｌｕｒｒｅｄ）イメージを計算するステップ
Ｎ回のガウスぼかしを原イメージに適用してＮガウスぼかしイメージを得る。このようなぼかし過程によって互いに異なるスケールのイメージを得ることができる。より一層ぼかすことは観測が遠ざかるということを意味する。これは遠い観察距離はより一層ぼかした場面に対応するという知覚的特性に基づくことである。

（ｃ）ガウスイメージの差を計算するステップ
隣接したガウスぼかしイメージ間の差（ＤｏＧ）を計算する。２つのぼかしイメージＩ_１およびＩ_２があると仮定すれば、これらの距離Ｄ_１は次の一般式（１）により求められる。ここで、ｘ、ｙはイメージのｘ座標成分およびｙ座標成分を各々意味する。

したがって、Ｎ個のガウスぼかしイメージからはＮ−１個のＤｏＧが計算される。

（ｄ）ＤｏＧイメージの極点（ｅｘｔｒｅｍａ）を感知するステップ
前記極点とは、そのグレイ値が周辺の全てのグレイ値より大きいか小さいピクセルを意味する。前記ピクセルは、ぼかしてから鮮明に変化するピクセルである。このようなピクセルは、スケールまたは回転変化後にも非常に安定的（ｓｔａｂｌｅ）である。

（ｅ）特徴点を得るために極点を選択するステップ
ＤｏＧにある極点は候補キーポイントとも呼ばれる。このステップは、特徴点の安定性を向上させるためにエッジに沿って誤って選択された点などを除去しなければならないため必要である。

特徴点変換部２２２は、現在推定された位置から抽出された特徴点に対して正確な位置と方向および明るさの変化率の方向を求める。このために特徴点変換部２２２は、前記キーポイントの周辺にローカルイメージをサンプリングし、全ての特徴点の位置および方向を計算して、グレイ階調（ｇｒａｙ ｇｒａｄｉｅｎｔ）の観点で特徴点を記述するベクトルを計算する。特徴点変換部２２２は、細部的に次の２つのステップ（（ａ）および（ｂ））を行う。

（ａ）特徴点の正確な位置および方向を計算するステップ
特徴点変換部２２２は、キーポイントの現在位置がピクセル単位で表現される。精密度向上のために、前記ピクセル座標は補間される必要がある。特徴点変換部２２２は、キーポイント周辺の６４×６４ピクセルイメージ領域のグレイ階調によって、各々のキーポイントに固定された方向を付与することによって回転−不変性を得る。

（ｂ）特徴点のグレイ階調ベクトルを計算するステップ
図３を参照すれば、特徴点変換部２２２は各々の特徴点に対して４個の副領域を付与する。各々の副領域は８個のグレイ階調ベクトルに対応する。結局、図３に示すように、各々の特徴点に対して３２個のグレイ階調ベクトルが存在する。図３で各々の矢印はその方向へのグレイ階調ベクトルの大きさを示す。すなわち、矢印がさらに長いほどグレイ階調もより大きくなるということを意味する。

特徴点マッチング部２２３は、現在瞬間で感知された特徴点（現在特徴点）のグレイ階調のベクトルをマップメモリ２４０に格納された特徴点のグレイ階調ベクトルと比較して、互いに類似する特徴点を探す。２つの特徴点のベクトルの差が閾値より小さければ前記２つの特徴点は互いにマッチすることと見なされる。現在特徴点とマッチングされる特徴点がメモリ２４０に複数存在すれば、現在特徴点と最も近い特徴点がマッチすることと選択される。メモリ２４０内にマッチする特徴点がなければ現在特徴点は新しい特徴点としてメモリ２４０に格納される。

再び図２を参照すれば、パーティクルフィルタ部２１０は、サンプリング部２１１、姿勢予測部２１２、および加重値計算部２１３を含んで構成することができる。パーティクルフィルタ部２１０は、特徴点処理部２２０から提供された特徴点の特性を用いて推定可能なロボットの位置および方向、すなわち姿勢の標本値の加重値を計算し、これらから最適の姿勢を推定する。前記加重値は真である確率を意味する。

姿勢予測部２１２は、移動ロボット１００の移動により移動する移動ロボット１００の位置を確率分布で示す。前記確率分布は、条件付き確率ｐ（ｘ_ｔ｜ｘ_ｔ−１、ｕ_ｔ−１）で示すことができる。ここで、ｘ_ｔ−１は時間ｔ−１での姿勢であり、ｕ_ｔ−１はエンコーダで測定されたデータであり、ｘ_ｔは時間ｔ（現在）での姿勢である。前記姿勢は、２次元直交座標系における２つの座標値および移動ロボット１００の向かう方向角を含み、３つの成分からなる。前記確率分布は、ロボットが動的雑音やシステムに存在する他の不確実な因子を有したまま、ｘ_ｔ−１からｘ_ｔまで動いた時、時間ｔにおけるロボット姿勢に対する確率分布を意味する。

図４は、開始点Ｓから最終点Ｅまで各々の中間段階で移動ロボット１００の姿勢に対する確率分布を示す。ここで、開始点から遠ざかるほどパーティクルがより多く分散することが分かる。これはエンコーダのエラーがロボットの開始点から経路の距離に応じて大きくなるためである。

パーティクルフィルタにおけるパーティクル数をＮと仮定すれば、全てのパーティクルは（ｘ_ｔ ^（ｉ）、ｗ_ｔ ^（ｉ））として表現される。ここで、ｉは１〜Ｎまでの整数インデックスであり、ｘ_ｔ ^（ｉ）は時間ｔでの適した（ｆｅａｓｉｂｌｅ）姿勢であり、ｗ_ｔ ^（ｉ）は姿勢がｘ_ｔ ^（ｉ）になる確率または加重値（ｗｅｉｇｈｔ）である。本姿勢予測部２１２は、パーティクルごとに確率モデルｐ（ｘ_ｔ｜ｘ_ｔ−１、ｕ_ｔ−１）によってｘ_ｔ ^（ｉ）を計算する。

サンプリング部２１１は、最初Ｎ個のパーティクルをランダムサンプリングする。そして、時間カウントｔが１増加する時、前記Ｎ個のパーティクルのうち一部のパーティクル（加重値が低いパーティクル）を脱落させ、一部のパーティクル（加重値が高いパーティクル）を重複サンプリングして再びＮ個のパーティクルを維持する。これを再サンプリングという。すなわち、サンプリング部２１１は、最初１回のサンプリング過程とその以後繰り返される複数の再サンプリング過程を行う。

最初ランダムサンプリングは、移動ロボット１００の最初位置近くにＮ個の任意の地点、すなわちＮ個のパーティクルを選択する過程である。前記過程は次の一般式（２）のように表示される。

前記ｘ_０ ^（ｊ）およびｗ_０ ^（ｊ）は、特定インデックスｊのパーティクルに対して、移動ロボット１００の最初位置および最初加重値を各々示す。前記ｗ_０ ^（ｊ）は、インデックスｊに係わらず全て１／Ｎで同一に設定される。

一方、サンプリング部２１１は、時間カウントが増加する時、既存のパーティクルセット

からＮ個のランダムサンプルを再サンプリングする。これによって、新しいサンプルセットを加重値ｗ_ｔ−１ ^（ｊ）に基づいて求める。

加重値計算部２１３は、時間カウントｔが増加する度に、各々のパーティクルに対する加重値ｗ_ｔ ^（ｊ）を計算して前記計算された値で該当加重値を更新する。

時間ｔでのパーティクル情報（ｘ_ｔ ^（ｊ）、ｗ_ｔ ^（ｊ））のうち加重値ｗ_ｔ ^（ｊ）はロボットの姿勢がｘ_ｔ ^（ｊ）である確率を示す。
移動ロボット１００が新しい観察情報を得る時、加重値計算部２１３は前記新しい情報に基づいてパーティクルの加重値を再び計算する。ここで、加重値の計算とは確率を修正してロボットを再びローカライゼーションすることを意味する。エンコーダ１３０は、累積誤差が発生するため、特徴点マッチング部２２３での特徴点マッチング後にパーティクルの加重値を再計算することは大変重要である。

新しく更新される加重値ｗ_ｔ ^（ｊ）は次の一般式（３）により計算される。

ここで、ｙ_ｔは時間ｔで実際観測された移動ロボット１００の位置を意味する。一般式（３）によれば、結局ｗ_ｔ ^（ｊ）は各々のパーティクルの予測された位置ｘ_ｔ ^（ｊ）が実際観測された値と一致する確率を意味するものである。前記観測された値は、特徴点マッチング部２２３でのマッチングされた特徴点を基準にして計算される値である。

例えば、ｗ_ｔ ^（ｊ）は、図５のように、Ｎ個のパーティクルの予測値があるとした時、各々の予測値と観測値との間の距離（ｄ_１、ｄ_２、…、ｄ_Ｎ）の逆数に比例する値で計算される。すなわち、観測値との距離が近いほど高い加重値が付与される。

再び図２を参照すれば、進化適用部２３０は、初期化部２３１、交配適用部２３２、および突然変異適用部２３３を含む。進化適用部２３０は、進化アルゴリズムを用いて、状態パラメータｘ_ｔ ^（ｊ）およびｗ_ｔ ^（ｊ）を最適化する。前記ｘ_ｔ ^（ｊ）は、姿勢予測部２１２で計算される現在各パーティクルの予測位置を意味し、ｗ_ｔ ^（ｊ）は加重値計算部２１３で計算されて更新された加重値を意味する。

前記予測および加重値の計算後には、Ｎ個の可能な姿勢およびこれらの確率を得ることができるが、実際ロボットの姿勢はＮ個の可能な姿勢のうち１つに該当しない場合が多い。より一層正確な姿勢を得るためには、パーティクルフィルタのための多くのパーティクルが必要であるが、パーティクル数の増加は演算量および要求メモリの量を急激に増大させる。したがって、進化適用部２３０は、ロボットの姿勢が実際姿勢に近接するように最適化するのに用いられる。このような最適化の結果として、ロボットローカライゼーションのために非常に少ないパーティクルが所要できるようにする。

進化適用部２３０に適用される進化アルゴリズムとしては、交配（ｃｒｏｓｓｏｖｅｒ）、突然変異（ｍｕｔａｔｉｏｎ）その他知らされた進化多様なアルゴリズムまたはこれらの組合せが適用される。ただし、本発明ではその内で効果が良いと予測される交配および／または突然変異アルゴリズムを例に挙げて説明する。

初期化部２３１は、進化アルゴリズムに適用される進化レベルＧおよび進化サンプル数Ｍを決定する。前記進化レベルおよび進化サンプル数は、ユーザから入力された値であり得、姿勢推定部２００に予め設定されたデフォルト値であり得る。前記進化レベルを多く進行するほど正確度が向上できるが、演算量が多少増加され得るため、前記進化レベルとして適切な値が選択される必要がある。姿勢推定部２００のコンピュータパワによって変われるが、進化レベルは大概３〜５程度で決定することが適当である。

前記進化サンプル数Ｍも多いほど正確度が向上できるが、演算量が増加するため適切な数（例えば、１０個内外）が選択されなければならない。前記進化サンプル数は、パーティクル毎に選択されることもでき、全てのパーティクルに対して同じ数を使用することもできる。

交配適用部２３２は、１つのパーティクルに対して選択されたＭ個の進化サンプルのうち、ランダムに選択された２つのサンプルｘ_ｔ ^（Ａ）、ｘ_ｔ ^（Ｂ）を抽出して交配アルゴリズムを適用する。交配アルゴリズムは、次の一般式（４）のように表現される。すなわち、子サンプルｘ_ｔ ^（Ａｃ）、ｘ_ｔ ^（Ｂｃ）は親サンプルｘ_ｔ ^（Ａ）、ｘ_ｔ ^（Ｂ）の加重和として表現される。ここで、ξは０と１との間のランダムユニフォーム分布から選択される任意の実数である。そして、ｗｔ^（Ａｃ）およびｗｔ^（Ｂｃ）は前記子サンプルに対する加重値である。前記子サンプルに対する加重値は、該当サンプルの値が観測値と一致する確率を意味する。

交配適用部２３２は、図６に示すように、２つの親サンプルおよび２つの子サンプル、すなわち

のうち最も大きい加重値を有する２つのサンプルを選択して、

を前記選択された値に更新する。このような交配過程は前記Ｍ個の進化サンプルのうち任意の２つに対して適用される。

突然変異適用部２３３は、前記交配適用部２３２によって交配アルゴリズムが適用されたＭ個の進化サンプルのうち１つのサンプルｘ_ｔ ^（Ａ）を抽出して、突然変異アルゴリズムを適用する。但し、前記サンプルｘ_ｔ ^（Ａ）の加重値ｗｔ（Ａ）が所定の閾値以上の場合に限って、突然変異アルゴリズムを適用する。加重値が低いサンプルに対して突然変異を適用すれば誤差がさらに大きくなり得るためである。

突然変異アルゴリズムは、次の一般式（５）のように表現される。ここでτは、ランダムノイズであって、ガウス分布Ｎ（０、σ^２）に従う３次元ベクトルである。前記σは、前記ガウス分布の標準偏差である。前記ｘ_ｔ ^（Ａ）は親サンプルであり、ｘ_ｔ ^（Ａｍ）はこれより突然変異された子サンプルである。そして、ｗ_ｔ ^（Ａｍ）は前記子サンプルに対する加重値である。

突然変異適用部２３３は、図７に示すように、１つの親サンプルおよび１つの子サンプル、すなわちｘ_ｔ ^（Ａ）およびｘ_ｔ ^（Ａｍ）のうち大きい加重値を有するサンプルを選択して、ｘ_ｔ ^（Ａ）を前記選択された値に更新する。

Ｍ個の進化サンプルに対して１回の交配および突然変異を適用すれば進化レベルは１つ上昇する。このような過程は、進化レベルが初期化部２３１で定めた進化レベルＧと同一になるまで繰り返される。

以上では交配アルゴリズムを適用した後、突然変異アルゴリズムを適用することを例に挙げた。しかし、以上の交配アルゴリズムおよび突然変異アルゴリズムは逆順に行うこともでき、これらアルゴリズムのうち１つだけ適用できることは当業者であれば十分に理解できるものである。

前記進化適用部２３０によって最適化されたパーティクルに対する加重値は、次の一般式（６）によって正規化される。この場合、全体パーティクルの加重値の合計は１になる。

以上の過程を経て、最適化されたパラメータセット

は再びサンプリング部２１１に提供される。以後、時間カウントが１増加すれば、サンプリング部２１１は前記パラメータセットを有するＮ個のパーティクルに基づいて再サンプリングし、姿勢予測部２１２および加重値計算部２１３は前記再サンプリングＮ個のパーティクルに対して前述したように、位置予測および加重値計算を実行し、進化適用部２３０は進化アルゴリズムを適用する。このような過程は前記時間カウントが終了地点（図４のＥ）になるまで繰り返される。

進化適用部２３０で最適化されたパーティクルのうち加重値が最も高いパーティクルの姿勢を移動ロボット１００の現在姿勢と推定される。前記推定された姿勢はロボット制御部１１０に提供されるが、ロボット制御部１１０はこの姿勢情報を用いて移動ロボット１００を移動させて掃除などの作業を実行できる。

今まで図１および図２の各構成要素はメモリ上の所定領域で行われるタスク、クラス、サブルーチン、プロセス、オブジェクト、実行スレッド、プログラムのようなソフトウェアや、ＦＰＧＡ（フィールドプログラマブルゲートアレイ）やＡＳＩＣ（ａｐｐｌｉｃａｔｉｏｎ−ｓｐｅｃｉｆｉｃ ｉｎｔｅｇｒａｔｅｄ ｃｉｒｃｕｉｔ）のようなハードウェアによって具現でき、また、前記ソフトウェアおよびハードウェアの組合せで成されることもできる。前記構成要素は、コンピュータで読取可能な格納媒体に含まれていることもでき、複数のコンピュータにその一部が分散して分布することもできる。

図８Ａおよび図８Ｂは、従来技術と本発明の結果を比較するための図面である。図８Ａは従来のパーティクルフィルタ技法にともなうローカライゼーションおよびマップ生成（特徴点表示）結果を、図８Ｂは本発明に係るローカライゼーションおよびマップ生成結果を示す。図８Ａで用いられたパーティクルの数は１００個であり、図８Ｂで用いられたパーティクルの数は５０個（進化レベルは５）である。図８Ａおよび８Ｂを比較すると、本発明は、既存のパーティクルフィルタ技法に比べて、さらに小さいパーティクルの数でもより良いマップを生成することができることが分かる。

この場合、ＳＬＡＭ実行時間（演算に必要とされる時間）を比較すると、図８Ａの場合４７４．７ｍｓが所要されたが、図８Ｂの場合４４３．５ｍｓが所要された。すなわち、同等またはその以下の演算時間で非常に良い結果を示すことが分かる。これは言い換えれば、同一の結果を示すのに用いられる演算時間が非常に減少する可能性があることを意味する。

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

本発明の一実施形態に係る移動ロボットおよび姿勢推定部の構成を示すブロック図である。 姿勢推定部２００の詳細構成を示すブロック図である。 特徴点のグレイ階調ベクトルを計算する過程を示す図である。 開始点Ｓから最終点Ｅまで各々の中間段階で移動ロボットの姿勢に対する確率分布を示す図である。 観測値と複数の予測値との間の距離を示す図である。 交配アルゴリズムの概念を示す図である。 突然変異アルゴリズムの概念を示す図である。 従来のパーティクルフィルタ技法にともなうローカライゼーションおよびマップ生成結果を示す図である。 本発明に係るローカライゼーションおよびマップ生成結果を示す図である。

符号の説明

１００ 移動ロボット
１１０ ロボット制御部
１２０ 走行部
１３０ エンコーダ
１４０ カメラ
２００ 姿勢推定部
２１０ パーティクルフィルタ部
２１１ サンプリング部
２１２ 姿勢予測部
２１３ 加重値計算部
２２０ 特徴点処理部
２２１ 特徴点抽出部
２２２ 特徴点変換部
２２３ 特徴点マッチング部
２３０ 進化適用部
２３１ 初期化部
２３２ 交配適用部
２３３ 突然変異適用部
２４０ メモリ

Claims (22)

1. 複数のパーティクルを用いるパーティクルフィルタ基盤の移動ロボットの姿勢推定装置であって、
   移動ロボットの姿勢変化量を感知するセンサと、
   以前パーティクルに前記感知された姿勢変化量を適用して、現在パーティクルの姿勢および加重値を求めるパーティクルフィルタ部と、
   前記求めた姿勢および加重値に進化アルゴリズムを適用して、前記現在パーティクルの姿勢および加重値を更新する進化適用部とを含む姿勢推定装置。
2. 外部イメージを捉えるカメラと、
   前記捉えられたイメージから特徴点を抽出し、前記抽出された特徴点を加工してマップを生成する特徴点処理部とをさらに含む請求項１に記載の姿勢推定装置。
3. 前記センサは、
   エンコーダおよびジャイロのうち少なくとも１つを含む請求項１に記載の姿勢推定装置。
4. 前記姿勢は、
   平面上の２次元座標系の各座標値および方向角を含む請求項１に記載の姿勢推定装置。
5. 前記パーティクルフィルタ部は、
   前記パーティクルに前記感知された姿勢変化量を適用して、前記現在パーティクルの姿勢を求める姿勢予測部と、
   前記抽出された特徴点に基づいて観測された値と前記求めた姿勢とを比較して、類似するパーティクルにさらに高い加重値を付与する加重値計算部とを含む請求項２に記載の姿勢推定装置。
6. 前記パーティクルフィルタ部は、
   前記複数のパーティクルを前記移動ロボットの最初の位置に基づいてサンプリングし、前記更新された加重値に基づいて前記複数のパーティクルを再サンプリングするサンプリング部をさらに含む請求項５に記載の姿勢推定装置。
7. 前記進化適用部は、
   前記パーティクルのうち１つのパーティクルからランダムサンプリングされる複数の進化サンプルのうち２つ以上の親サンプルを選択して、これらの加重和を計算し、前記加重和から生成される１つ以上の子サンプルと前記親サンプルとのうち一部のサンプルを選択する交配適用部を含む請求項２に記載の姿勢推定装置。
8. 前記一部のサンプルは、
   前記子サンプルと前記親サンプルとのうち加重値が大きいサンプルである請求項７に記載の姿勢推定装置。
9. 前記進化適用部は、
   前記パーティクルのうち１つのパーティクルからランダムサンプリングされる複数の進化サンプルのうち１つの親サンプルを選択して、これにランダムノイズを加えて子サンプルを生成し、前記親サンプルおよび子サンプルのうち加重値が大きいサンプルを選択する突然変異適用部を含む請求項２に記載の姿勢推定装置。
10. 前記親サンプルは、
    前記複数の進化サンプルのうちその加重値が所定の閾値を超えるサンプルの中から選択される請求項９に記載の姿勢推定装置。
11. 前記ランダムノイズは、
    ガウス分布に従う請求項９に記載の姿勢推定装置。
12. 複数のパーティクルを用いるパーティクルフィルタ基盤の移動ロボットの姿勢推定方法であって、
    移動ロボットの姿勢変化量を感知するステップと、
    以前パーティクルに前記感知された姿勢変化量を適用して、現在パーティクルの姿勢および加重値を求めるステップと、
    前記求めた姿勢および加重値に進化アルゴリズムを適用して、前記現在パーティクルの姿勢および加重値を更新するステップとを含む姿勢推定方法。
13. 外部イメージを捉えるステップと、
    前記捉えられたイメージから特徴点を抽出し、前記抽出された特徴点を加工してマップを生成するステップとをさらに含む請求項１２に記載の姿勢推定方法。
14. 前記感知するステップは、
    エンコーダおよびジャイロのうち少なくとも１つを用いて行われる請求項１２に記載の姿勢推定方法。
15. 前記姿勢は、
    平面上の２次元座標系の各座標値および方向角を含む請求項１２に記載の姿勢推定方法。
16. 前記姿勢および加重値を求めるステップは、
    前記パーティクルに前記感知された姿勢変化量を適用して、前記現在パーティクルの姿勢を求めるステップと、
    前記抽出された特徴点に基づいて観測された値と前記求めた姿勢とを比較して、類似するパーティクルにさらに高い加重値を付与するステップとを含む請求項１３に記載の姿勢推定方法。
17. 前記姿勢および加重値を求めるステップは、
    前記複数のパーティクルを前記移動ロボットの最初の位置に基づいてサンプリングするステップと、
    前記更新された加重値に基づいて前記複数のパーティクルを再サンプリングするステップとをさらに含む請求項１６に記載の姿勢推定方法。
18. 前記姿勢および加重値を更新するステップは、
    前記パーティクルのうち１つのパーティクルからランダムサンプリングされる複数の進化サンプルのうち２つ以上の親サンプルを選択するステップと、
    前記親サンプルの加重和を計算するステップと、
    前記加重和から生成される１つ以上の子サンプルと前記親サンプルとのうち一部のサンプルを選択するステップとを含む請求項１３に記載の姿勢推定方法。
19. 前記一部のサンプルは、
    前記子サンプルと前記親サンプルとのうち加重値が大きいサンプルである請求項１８に記載の姿勢推定方法。
20. 前記姿勢および加重値を更新するステップは、
    前記パーティクルのうち１つのパーティクルからランダムサンプリングされる複数の進化サンプルのうち１つの親サンプルを選択するステップと、
    前記親サンプルにランダムノイズを加えて子サンプルを生成するステップと、
    前記親サンプルおよび子サンプルのうち加重値が大きいサンプルを選択するステップとを含む請求項１３に記載の姿勢推定方法。
21. 前記親サンプルは、
    前記複数の進化サンプルのうちその加重値が所定の閾値を超えるサンプルの中から選択される請求項２０に記載の姿勢推定方法。
22. 前記ランダムノイズは、
    ガウス分布に従う請求項２０に記載の姿勢推定方法。