JP7166446B2 - ロボットの姿勢を推定するシステムおよび方法、ロボット、並びに記憶媒体 - Google Patents

Description

本発明は、包括的には、モバイルロボットに関し、より詳細には、確率論的複数のロボットによる測位に関する。

一団のロボットのタスク実行を成功させるための最も不可欠な前提条件の１つは、ロボットの姿勢を正確に推定し追跡することである。一般的に、ロボットによる測位に関する問題は、オドメトリ等のロボットの動きを示すセンサからの測定値、視覚カメラ、奥行きカメラ、ｌｉｄａｒ等の環境測定値を提供するセンサからの測定値、および、場合によっては何らかの外部情報に基づいてロボットの姿勢を推定することである。ロボットによる測位は、ほとんど全てのロボット応用の中核をなすものである。

単一のロボットによる測位は、多数のシステム、アルゴリズムおよび使用事例において大きな注目を集めてきた。近年、複数のロボットが協力して１つのタスクを実行するシナリオが関心を集めている。複数のロボットのシステムにおけるロボットによる測位は有利である。複数のロボットによる測位を行う素直な手法は、各ロボットに、単一のロボットによる測位を実行させることである。しかしながら、ロボットは、或るタスクを協力して実行するという目的のために、情報を交換できるように既になっているので、ロボットに、測位についても、同じように協力して測位を実行させることについては疑問の余地はない。すなわち、各ロボットは、自身のオドメトリ測定値および環境測定値だけではなく、他のロボットから受信された情報も用いて、自己の測位を行うことができる。この手法の利点は、より正確な測位ができること、推定を収束が早いこと、およびロバスト性が高いことである。

複数のロボットによる測位におけるいくつかの方法は、ロボットの協調を用いてオドメトリ誤差を低減させることを目標とする。一般的な枠組みとしては、一方のグループのロボットが他方のグループのロボットの動きを測定し、オドメトリ測定値の精度を高めるために、一方のグループのロボットが静止し、それと同時に残りのロボットが移動するように仕向けることである。しかしながら、それらの方法は、オドメトリ測定値が環境測定値と融合しないため、制約となる可能性がある。加えて、高精度化されたオドメトリ推定値における誤差は、経時的に蓄積し、最終的には、誤った姿勢推定値に至ってしまう。

他の協調的な測位の方法では、ロボットは、ロボット同士の２回の連続したランデブー間に収集した全ての測定値を交換する。それらの測定値は、通常、オドメトリおよびレーザスキャンによるものである。この手法の１つの欠点は、莫大な量のデータの交換をサポートするために必要となる膨大な通信帯域である。加えて、ロボットは、大量のデータを記憶する必要があるため、大容量のメモリが必要となる。

いくつかの方法、例えば、非特許文献１においてFoxらによって記述されている方法では、ロボットは、自身のオドメトリ測定値および環境測定値を、相対姿勢測定値および他のロボットの姿勢パーティクル（particle：粒子）と融合することによって自身の姿勢を確率論的に推測する。ロボット同士は、ランダムランデブーにおいて、姿勢パーティクルを交換し、相対的な姿勢測定を実行する。情報融合は、ベイズ推定（Bayesian inference）フレームワーク内で行われ、密度ツリー（density trees）を用いてロボットのパーティクルからロボットの姿勢を推定する。しかしながら、この方法の複雑度は、単一のロボットによる測位の複雑度よりも高い。

密度ツリーベースの位置推定の複雑度を低減するために、他の方法では、密度ツリーを用いる代わりに、異なるパーティクル更新スキームを用いる。例えば、非特許文献２においてProrokらによって記述されている方法では、ランデブーに関与する２つのロボットからのパーティクルの全てのペアについて、相対姿勢分布の評価を必要とするパーティクルの重み計算を用いる。しかしながら、この方法の計算複雑度は、Ｏ（Ｋ^２）である。ここで、Ｋは、パーティクルの数である。非特許文献３においてProrokらによって記述されている別の方法では、Ｋ個のパーティクルをＳ個のクラスターにクラスタリングすることによって、計算複雑度をＯ（ＫＳ）に低減する。しかしながら、この計算複雑度は、依然として望ましい数値よりも高い。加えて、この方法では、確率論的姿勢推定の性能を保証できない可能性がある。

Fox、他「A Probabilistic Approach to Collaborative Multi-Robot Localization」、Autonomous Robots Vol. 8(3):325-344 (2000) Prorok、他「A reciprocal sampling algorithm for lightweight distributed multi-robot localization」、2011 IEEE/RSJ International Conference on Intelligent Robots and Systems Prorok、他「Low-cost collaborative localization for large-scale multi-robot systems」、2012 IEEE International Conference on Robotics and Automation

いくつかの実施の形態の目的は、パーティクルフィルタ（particle filter：粒子フィルタ）を用いてロボットの姿勢を確率論的に推定することである。パーティクルフィルタは、パーティクルの集合を用いてロボットの姿勢を表す。ロボットの姿勢は、ロボットのロケーションおよび向きのうちの一方又は組み合わせを含む。各パーティクルは、ロボットの現在の姿勢の値の確率を定義する。

いくつかの実施の形態の別の目的は、ロボット同士がランデブー時に各自のパーティクルを交換する確率論的複数のロボットによる測位システムおよび方法を開示することである。とりわけ、この手法は、各ロボットのセンサの未加工測定値が記憶されることも、ランデブー時に交換されることも必要としない。有利な点としては、各ロボットは、対応するセンサの測定値を取得すると即座に、姿勢を表す更新されたパーティクルに融合することができる。ランデブー時にロボットはパーティクルを交換するので、通信帯域幅への要求は比較的緩和されたものとなる。加えて、この手法では、ロボットが接近したとき、例えば、互いの視界に入ったときにのみ接続が確立することができる。これにより、Ｂｌｕｅｔｏｏｔｈ規格のような低電力送受信機を用いて達成できる通信カバレッジの要件を削減することができる。

いくつかの実施の形態の別の目的は、計算複雑度を低減し、一方で、確率論的推定によって提供される性能保証を確保しつつ、そのような確率論的複数のロボットによる測位方法を提供することである。

いくつかの実施の形態は、パーティクルフィルタがベイズ推定問題を解き、それゆえ、複数のロボットの姿勢融合もパーティクルフィルタの性能を保証するためにベイズ推定の原理に従うべきであるという理解に基づいている。しかしながら、ロボット間のパーティクルの比較は、ペアワイズである。すなわち、パーティクルはペア単位でグループ化され、各ペアの２つのパーティクルが互いに比較されて、ロボットの姿勢が更新される。しかしながら、パーティクルをペアリングすることが決定論的であると、そのようなペアリングは、ベイズ推定が提供する性能保証を損なう可能性がある。

例えば、１つのロボットの全てのパーティクルは、別のロボットの全てのパーティクルと比較することができる。そのような網羅的なペアリングは決定論的であるが、仮説の全ての情報が確率推定のために利用可能である場合、偶然にもベイズ推定の原理に従っている。しかしながら、そのような比較は、結果として計算複雑度はＯ（Ｋ^２）となる。ここで、Ｋは、パーティクルの数である。網羅的なペアリングから逸脱する決定論的な試みは、ベイズ推定の原理に反する。例えば、アドホッククラスタリングおよび各クラスターのパーティクルの比較は、計算複雑度を低減することができるが、ベイズ推定の原理には反する。なぜならば、クラスタリングは、決定論的であるためである。

いくつかの実施の形態は、確率論的複数のロボットによる測位中の異なるロボットのパーティクルのランダムペアリングは、計算複雑度をＯ（Ｋ）に低減することができるという理解に基づいている。ここで、Ｋは、パーティクルフィルタの確率論的保証を確実にしながらランダムに選択されたパーティクルのペア数である。実際には、ランダムペアリングは、確率論的であり、パーティクルフィルタの性能を保証するためにベイズ推定の原理に従う。また加えて、ランダムペアリングは単純であり、計算的に効率的であり、並列計算に適している。

そのため、いくつかの実施の形態では、ロボットは、別の近傍ロボットとランデブーすると、恣意的にペアリングされたロボットのパーティクルを比較して、パーティクルの重みを更新し、最終的にロボットの姿勢を更新する。本明細書では、恣意的にペアリングされたパーティクルは、恣意的に、すなわち、決定論的にではなくサンプリングされる。そのような恣意的なペアリングは、任意の数が集合から選択される可能性が等しくなるように、等しい確率で数字の集合から数字を選択するように構成された任意のランダムおよび／又は擬似ランダム生成器を用いて実施することができる。別の実施態様において、パーティクルは、パーティクルの重みに従って、一様にランダムに又は非一様にランダムにサンプリングされる。この場合、より大きい重みを有するパーティクルは、より小さい重みを有するパーティクルよりもサンプリングされる可能性が高い。

したがって、１つの実施の形態は、ロボットの姿勢を推定するシステムであって、姿勢は、ロボットのロケーションおよびロボットの向きのうちの一方又は組み合わせを含む、システムを開示する。システムは、このロボットの現在の姿勢と近傍ロボットの現在の姿勢との間の相対姿勢の値を示すデータ、および近傍ロボットのパーティクルの値を示すデータを受信するように構成された入力インターフェースであって、近傍ロボットの各パーティクルは、近傍ロボットの現在の姿勢の値の確率を定義する、入力インターフェースと、ロボットのパーティクルの値を記憶するように構成されたメモリであって、ロボットの各パーティクルは、ロボットの現在の姿勢の値の確率を定義し、メモリは、パーティクルの集合を用いてロボットの姿勢を追跡するように構成されたパーティクルフィルタであって、各パーティクルは、ロボットの現在の姿勢の値の確率を定義する、パーティクルフィルタと、パーティクルチューナであって、ロボットの恣意的にサンプリングされたパーティクルを、近傍ロボットの恣意的にサンプリングされたパーティクルとペアリングし、ペアリングされたパーティクルによって定義される相対姿勢と、ロボットと近傍ロボットとの間の相対姿勢との間の誤差に反比例するようにペアリングされたパーティクルの重みを決定し、対応するペアリングされたパーティクルの重みに従ってロボットのパーティクルを更新するように構成される、パーティクルチューナとを含む実行可能な構成要素を記憶するように構成された、メモリと、ロボットの姿勢の変化に応答して、パーティクルフィルタを用いてロボットの姿勢を追跡するとともに、データを受信することに応答して、パーティクルチューナを用いてパーティクルフィルタのパーティクルを更新するように構成されたプロセッサと、ロボットの現在の姿勢を出力するように構成された出力インターフェースとを備える。

別の実施の形態は、ロボットの姿勢を推定する方法であって、姿勢は、ロボットのロケーションおよびロボットの向きのうちの一方又は組み合わせを含み、方法は、方法を実施する記憶された命令と結合されたプロセッサを使用し、命令は、プロセッサによって実行されると、方法のステップを実行し、方法は、ロボットの現在の姿勢と近傍ロボットの現在の姿勢との間の相対姿勢の値を示すデータ、および近傍ロボットのパーティクルの値を示すデータを受信することであって、近傍ロボットの各パーティクルは、近傍ロボットの現在の姿勢の値の確率を定義することと、パーティクルの集合を用いてロボットの姿勢を追跡することであって、各パーティクルは、ロボットの現在の姿勢の値の確率を定義することと、ロボットの恣意的にサンプリングされたパーティクルを、近傍ロボットの恣意的にサンプリングされたパーティクルとペアリングすることと、ペアリングされたパーティクルによって定義される相対姿勢と、ロボットと近傍ロボットとの間の相対姿勢との間の誤差に反比例するようにペアリングされたパーティクルの重みを決定することと、対応するペアリングされたパーティクルの重みに従ってロボットのパーティクルを更新することとを含む、方法を開示する。

更に別の実施の形態は、方法を実行するプロセッサによって実行可能なプログラムがその上に具体化された非一時的コンピュータ可読記憶媒体であって、方法は、ロボットの現在の姿勢と近傍ロボットの現在の姿勢との間の相対姿勢の値を示すデータ、および近傍ロボットのパーティクルの値を示すデータを受信することであって、近傍ロボットの各パーティクルは、近傍ロボットの現在の姿勢の値の確率を定義することと、パーティクルの集合を用いてロボットの姿勢を追跡することであって、各パーティクルは、ロボットの現在の姿勢の値の確率を定義することと、ロボットの恣意的にサンプリングされたパーティクルを、近傍ロボットの恣意的にサンプリングされたパーティクルとペアリングすることと、ペアリングされたパーティクルによって定義される相対姿勢と、ロボットと近傍ロボットとの間の相対姿勢との間の誤差に反比例するようにペアリングされたパーティクルの重みを決定することと、対応するペアリングされたパーティクルの重みに従ってロボットのパーティクルを更新することとを含む、非一時的コンピュータ可読記憶媒体を開示する。

いくつかの実施形態による、複数のロボットが屋内エリア内を移動して、複数のロボットによる測位を実行するシナリオを示す概略図である。 いくつかの実施形態の原理を用いて協調的ロボット位置特定を利用するロボットの概略図である。 図１Ｂのロボットの種々の構成要素の概略図である。 いくつかの実施形態による、ロボットの姿勢を推定するシステム１００のブロック図である。 いくつかの実施形態によって用いられる基準系におけるロボットの姿勢の図である。 いくつかの実施形態によって用いられる占有グリッドマップの一例の図である。 いくつかの実施形態によって用いられる単一のロボット姿勢推定のブロック図である。 図２Ａの基準系におけるパーティクル群（particle cloud）の図である。 いくつかの実施形態による、或るエリアにおけるパーティクル群の図である。 １つの実施形態における２つのロボットのランデブーを示す概略図である。 いくつかの実施形態によって用いられる２つのロボット間の相対姿勢の概略図である。 いくつかの実施形態によって用いられる協調的位置特定パイプラインの原理的なブロック図である。 いくつかの実施形態による、パーティクルチューナによって実行される情報融合の原理的なブロック図である。 １つの実施形態によって用いられる協調的測位の擬似コードである。 いくつかの実施形態による、重み付けされたパーティクルペアを計算する方法のブロック図である。 図８Ａに示されたように生成された姿勢パーティクルの重み付けされたペアに基づいてパーティクルを更新するいくつかの実施形態によって用いられる方法のブロック図である。 所与の離散分布ｚについてのパーティクルサンプリングを実施するためにいくつかの実施形態によって用いられる擬似ランダムパーティクルサンプリングのブロック図である。 １つの実施形態によって用いられる擬似乱数生成のブロック図である。 いくつかの実施形態による、ロボットによる測位システムのブロック図である。

図１Ａは、いくつかの実施形態による、複数のロボットが屋内エリア内を移動して、複数のロボットによる測位を実行するシナリオを示す概略図を示している。１つの実施形態では、２Ｄ占有グリッドマップは、ロボットに既知であり、および／又はロボットに利用可能である。また、いくつかの実施形態では、未知の環境のマップを構築する又は更新する一方で、それと同時にロボットの姿勢を追跡することによってＳＬＡＭ（simultaneous localization and mapping）を実行する。

図１Ａに示される一例では、３つのロボット１２１、１２２および１２３が共通のエリア１０９内にあり、協調的に或る特定のタスクを実行する。タスク実行の成功のための主な前提条件のうちの１つは、ロボットに、エリア内で自身を位置特定する能力を与えることである。素朴な手法は、各ロボットに、従来技術からの多くの単一のロボット位置特定アルゴリズムのうちの１つを用いて自己を位置特定させることである。しかしながら、ロボットは、より正確なロケーション推定値を得るという潜在的な利益のために、位置特定を実行している間に協調することができる。そのために、いくつかの実施形態は、協調的ロボット位置特定のための複数のロボットによる測位を開示する。

図１Ｂは、いくつかの実施形態の原理を用いて協調的ロボット位置特定を利用するロボットの概略図を示している。図１Ｃは、図１Ｂのロボットの種々の構成要素の概略図を示している。ロボットは、ロボットが或るエリア内を自律的に移動することができるようにモータ１０８と、多様な測定値を取得するセンサ１０２、１０３、１０４、および１０５の集合と、測定値および他の物理量を記憶するメモリ１４０と、多様なアルゴリズムを実行するプロセッサ１２０と、他のロボットおよびデバイスと通信する送受信機１２６、すなわち、受信機および送信機と、電力をロボットおよび全てのそのデバイスに提供するバッテリー１２９とを含む完全に又は部分的に自律的なエンティティである。ロボットは、ロボットの種々のタスクを実行する機器１２８、例えば、グリッパーも備えることができる。

例えば、ロボットは、低コストの構造化光センサ等の環境検知のためのセンサと、オドメトリセンサ等のロボットの動きを記録するセンサとを備える。協調を可能にするために、ロボットは、他のロボットの存在を検出することができる必要がある。これは、複数の方法で達成することができる。例えば、１つのシナリオでは、或るロボットの前面には可視マーキングを施し、一方、他のロボットには、視覚カメラを備えることで、チェッカーボードの存在を検出するとともに、ロボット間の相対姿勢（すなわち、相対距離、方位角および進行方向）を測定する。

ロボットは、ランダム走行によって、又はいくつかの命令に従って、エリア内を移動し、時折互いに相対的に接近するように構成される。ランデブーが一方又は双方のロボットによって検出されると、ロボットは、ワイヤレス通信リンクを介して比較的少量の情報を交換する。各ロボットは、自身の計算リソースを用いて、受信情報を自身の情報とローカルに融合する。換言すれば、情報融合は、各ロボットにおいて独立して行われるので、中央サーバは、必要としない。

より具体的には、エリア内を移動する各ロボットは、環境および動き検知からの測定値を用いて、そのエリアのマップ内で自らの位置を特定する。ロケーション推定値は、従来の単一のロボットＰＦベースによる測位を用いて得られ、或る特定の数の姿勢パーティクルを用いて表される。ランデブー時に、２つのロボットは、各自のパーティクルを交換する。これは、ロボットが２つの連続したランデブーイベントの間で収集された全ての環境測定値および動きの測定値を交換する、他の複数のロボットのＳＬＡＭとは対照的である。パーティクルに加えて、ロボット間の相対姿勢を測定するロボットは、その測定値を他方のロボットに送信する。その後、各ロボットは、ランデブーの前の自身の姿勢を表すパーティクルを、他方のロボットから受信されたパーティクル、および２つのロボット間の相対姿勢測定値と融合する。各ロボット上でローカルに行われた情報融合の結果は、更新された姿勢パーティクルの集合である。その後、ロボットは、エリア内での移動を継続し、これらのロボットが再び遭遇するまで、姿勢推定を自身で実行する。

いくつかの実施形態は、ロボットは、別の近傍ロボットとランデブーすると、恣意的にペアリングされたロボットのパーティクルを比較して、パーティクルの重みを更新し、最終的にロボットの姿勢を更新する。本明細書では、恣意的にペアリングされたパーティクルは、恣意的に、すなわち、決定論的にではなくサンプリングされる。そのような恣意的なペアリングは、任意の数が集合から選択される可能性が等しくなるように、等しい確率で数字の集合から数字を選択するように構成された任意のランダムおよび／又は擬似ランダム生成器を用いて実施することができる。別の実施態様において、パーティクルは、パーティクルの重みに従って、一様にランダムに又は非一様にランダムにサンプリングされる。この場合、より大きい重みを有するパーティクルは、より小さい重みを有するパーティクルよりもサンプリングされる可能性が高い。

図１Ｄは、いくつかの実施形態による、ロボットの姿勢を推定するシステム１００のブロック図を示している。本明細書では、姿勢は、ロボットのロケーションおよびロボットの向きのうちの一方又は組み合わせを含む。システム１００は、計算複雑度を低減し、一方で、確率論的推定によって与えられる性能保証を確保する確率論的複数のロボットによる測位方法を実施する。

システム１００は、記憶された命令を実行するように構成されたプロセッサ１２０と、このプロセッサによって実行可能である命令を記憶するメモリ１４０とを備える。プロセッサ１２０は、シングルコアプロセッサ、マルチコアプロセッサ、コンピューティングクラスター、又は任意の数の他の構成とすることができる。メモリ１４０は、ランダムアクセスメモリ（ＲＡＭ）、リードオンリーメモリ（ＲＯＭ）、フラッシュメモリ、又は他の任意の適したメモリシステムを含むことができる。プロセッサ１２０は、バス１０６を通じて、１つ以上の入出力デバイスに接続される。これらの命令は、確率論的複数のロボットによる測位方法を実施する。

そのため、システム１００は、パーティクルの集合を用いてロボットの姿勢を追跡するように構成されたパーティクルフィルタ１３１を備える。各パーティクルは、ロボットの現在の姿勢の値の確率を定義する。パーティクルは、ロボットのセンサの測定値を用いて決定および／又は更新することができる。パーティクルは、センサの測定値に対する姿勢を表している。これによって、ロボットが、センサの未加工測定値を記憶することを回避し、パーティクル１３５のみを記憶することが可能になる。これは、メモリ効率が良い。例えば、記憶デバイス１３０は、パーティクル１３５および／又は他の近傍ロボットのパーティクルを記憶するように適応させることができる。記憶デバイス１３０は、ハードドライブ、光ドライブ、サムドライブ、ドライブのアレイ、又はそれらの任意の組み合わせを用いて実装することができる。

いくつかの実施態様では、パーティクルフィルタ１３１は、ロボットのセンサの測定値のみを用いてロボットの姿勢を更新することができる。しかしながら、システム１００は、外部情報、すなわち、他の情報を用いて、および／又はロボットのセンサ測定値に加えて、パーティクル１３５を更新するように構成されたパーティクルチューナ１３３も備える。具体的には、パーティクルチューナ１３３は、ロボットのパーティクルがロボットの現在の姿勢を正しく表すとともに近傍ロボットのパーティクルが近傍ロボットの現在の姿勢を正しく表す場合、各ロボットからのパーティクルのペアは、ロボットの相対姿勢を正しく表すはずであるという認識に基づいている。パーティクルのペアによって決定されるそのような相対姿勢は、他の手段によって決定される相対姿勢と比較することができ、比較の結果は、現在の姿勢の表現におけるパーティクルの正確性を示している。例えば、パーティクルのペアによって決定される相対姿勢と他の手段によって決定される相対姿勢との間の差が大きくなるほど、パーティクルがロボットの姿勢を正しく表す確率が低くなることが示される。

例えば、いくつかの実施態様では、システム１００は、任意選択で、センサ情報を用いて、２つのロボット間の相対姿勢を決定することができる相対姿勢推定構成要素１３７を備えることができる。センサ情報は、種々の形式を有するとともに、高信頼度の相対姿勢の推定を提供することができる。例えば、１つのシナリオでは、近傍ロボットの前面には、可視マーキングを施し、一方、ロボットには、視覚カメラを備えることで、可視マーキング、例えば、チェッカーボードの存在を検出するとともに、ロボット間の相対姿勢（すなわち、相対距離、方位角および進行方向）を測定する。また、システム１００は、近傍ロボットから相対姿勢推定値を受信することもできる。

いくつかの実施形態は、パーティクルフィルタがベイズ推定問題を解き、それゆえ、複数のロボットの姿勢融合もパーティクルフィルタの性能を保証するためにベイズ推定の原理に従うべきであるという理解に基づいている。しかしながら、ロボット間のパーティクルの比較は、ペアワイズである。すなわち、パーティクルはペア単位でグループ化され、各ペアの２つのパーティクルが互いに比較されて、ロボットの姿勢が更新される。しかしながら、パーティクルをペアリングすることが決定論的であると、そのようなペアリングは、ベイズ推定が提供する性能保証を損なう可能性がある。

いくつかの実施形態は、確率論的複数のロボットによる測位中の異なるロボットのパーティクルのランダムペアリングは、計算複雑度をＯ（Ｋ）に低減することができるという理解に基づいている。ここで、Ｋは、パーティクルフィルタの確率論的保証を確実にしながらランダムに選択されたパーティクルのペア数である。実際には、ランダムペアリングは、確率論的であり、パーティクルフィルタの性能を保証するためにベイズ推定の原理に従う。また加えて、ランダムペアリングは単純であり、計算的に効率的であり、並列計算に適している。

そのため、パーティクルチューナは、ロボットの恣意的にサンプリングされたパーティクルを、近傍ロボットの恣意的にサンプリングされたパーティクルとペアリングし、ペアリングされたパーティクルによって定義される相対姿勢と、ロボットと近傍ロボットとの間の相対姿勢との間の誤差に反比例するようにペアリングされたパーティクルの重みを決定し、対応するペアリングされたパーティクルの重みに従ってロボットのパーティクルを更新するように構成される。本明細書では、恣意的にペアリングされたパーティクルは、恣意的に、すなわち、決定論的にではなくサンプリングされる。そのような恣意的なペアリングは、任意の数が集合から選択される可能性が等しくなるように、等しい確率で数字の集合から数字を選択するように構成された任意のランダムおよび／又は擬似ランダム生成器を用いて実施することができる。別の実施態様において、パーティクルは、パーティクルの重みに従って、一様にランダムに又は非一様にランダムにサンプリングされる。この場合、より大きい重みを有するパーティクルは、より小さい重みを有するパーティクルよりもサンプリングされる可能性が高い。

ネットワークインターフェースコントローラ（NIC）１５０は、バス１０６を通じて、システム１００をネットワーク１９０に接続するように適応される。ネットワーク１９０を通じて、送受信機１２６を用いて、システムは、ロボットのパーティクルを近傍ロボットに送信し、および／又は、近傍ロボットに、近傍ロボット自身のパーティクルを送信するように要求することができる。加えて、システム１００は、ロボットのセンサに測定値を要求し、および／又は、近傍ロボットに近傍ロボット自身のパーティクルを送信するように要求するために、ロボットが近傍ロボットに近接しているときに近傍ロボットとのランデブーを検出するセンサインターフェース１８０を備える。

いくつかの実施態様では、システム１００内のヒューマンマシンインターフェース（HMI）１１０は、システムを、キーボード１１１およびポインティングデバイス１１２に接続する。ポインティングデバイス１１２は、マウス、トラックボール、タッチパッド、ジョイスティック、ポインティングスティック、スタイラス、又はタッチスクリーンを含む。システム１００は、ロボットの現在の姿勢を出力するように構成された出力インターフェースを備える。例えば、出力インターフェースは、ロボットの姿勢をレンダリングするメモリ、および／又は、ロボットの推定された姿勢から利益を得るシステムへの種々のインターフェースを備えることができる。例えば、システム１００は、バス１０６を通じて、システム１００をディスプレイデバイス１６５に接続するように適応されたディスプレイインターフェース１６０にリンクすることができる。ディスプレイデバイス１６５は、コンピュータモニタ、カメラ、テレビジョン、プロジェクタ、又はモバイルデバイスを含む。システム１００は、システムを、ロボットの位置に基づいてロボット固有タスクを実行する機器１２８に接続するように適応されたアプリケーションインターフェース１７０に接続することもできる。

（パーティクルフィルタ）
種々の実施形態において、パーティクルフィルタ１３１は、ロボットの姿勢を推定するように構成される。以下の記載によって、複数のロボットのデータ融合とともにいくつかの実施形態によって用いられるパーティクルフィルタ（ＰＦ）ベースによる測位アルゴリズムの簡潔な概説を提供する。

図２Ａは、いくつかの実施形態によって用いられる基準系におけるロボットの姿勢の図である。２Ｄ環境におけるロボット２００の姿勢２１０を推定することは、環境に関連付けられた座標系を基準とするロボットの２Ｄ座標（ｘ，ｙ）２１１および２１２並びに向きθ ２１３を推定することに関係する。時点ｔにおける２Ｄ座標および向きは、姿勢ベクトルｘ_ｔ＝［ｘ_ｔ ｙ_ｔ θ_ｔ］^Ｔにまとめられる。ここで、演算子Ｔは、ベクトル／行列の転置を示している。

図２Ｂは、いくつかの実施形態によって用いられる占有グリッドマップの一例である。環境に関する知識は、占有グリッドマップｍ∈｛０，１｝^Ｎを用いて表される。これは、エリアをＮ個のビンに分割することによって得られ、第ｉのビンが占有される場合にはｍ_ｉ＝１であり、占有されない場合にはｍ_ｉ＝０である。この例では、未知のエリア２２０は、８つのビンによって８つに分割される。ここで、ビンは、占有される（２２２）か又は占有されない（２２１）かのいずれかである。例えば、占有される複数の連続ビンは、壁部２２３の存在を示す。

いくつかの実施態様では、少なくとも２つのタイプの測定値、すなわち、環境測定値および動き測定値が、ロボットが環境を通して移動するにつれてセンサ１０２～１０５によって収集される。環境測定値は、例えば、ロボットから、その視野内で視認される障害物までの距離を表す距離測定値を、出力する時点ｔにおけるレーザスキャンによって生成することができる。これらの測定値は、ベクトルｚ_ｔにまとめられる。距離測定値ｚ_ｔは、所与の姿勢ｘ_ｔおよびマップｍについてのｚ_ｔの実現値を得る確率を表すｐ（ｚ_ｔ｜ｘ_ｔ，ｍ）を用いて確率論的にモデル化される。直観的に、この目標は、所与のロボット姿勢およびマップと整合した測定値に大きい確率質量を与えることである。

ロボットの動きに関連付けられた測定値は、時間区間（ｔ－１，ｔ］内のオドメトリセンサ測定値を表わすベクトルｕ_ｔにまとめられる。オドメトリセンサは、ホイールエンコーダとすることができる。その場合、本発明者らは真の測定値、又は、姿勢推定のために測定値として扱われる、ロボットに送出される制御入力に関して言及している。ロボットの動きモデルは、時点ｔ－１におけるロボットの姿勢ｘ_ｔ－１およびオドメトリ測定値ｕ_ｔを所与とすると、何らかの姿勢実現値ｘ_ｔにあるロボットを発見する確率を表すｐ（ｘ_ｔ｜ｘ_ｔ－１，ｕ_ｔ）を用いて確率論的に記述される。直観的に、時点ｔ－１における姿勢およびオドメトリ測定値と整合しないｘ_ｔの姿勢実現値は、比較的小さい確率質量を受け取る。

図３は、いくつかの実施形態によって用いられる単一のロボットの姿勢推定３２０のブロック図を示している。姿勢推定３２０は、入力として、オドメトリモデルおよび測定値３２２、距離モデルおよび測定値３２３、並びに環境のマップ３２１を取り込み、ロボットの姿勢の推定値３３０を生成する。

いくつかの実施形態では、姿勢推定３２０は、ベイズ推定フレームワークを用いてパーティクルフィルタ１３１によって確率論的に実行される。ベイズ推定フレームワーク内のロボット姿勢推定の目標は、マップ並びに時点ｔまでの全ての距離測定値およびオドメトリ測定値を所与とすると、時点ｔにおけるロボット姿勢ｘ_ｔの確率分布を推測することである。すなわち、目的は、

を発見することである。

加えて、（１）の推測は順次行う必要がある。すなわち、時点ｔにおけるロボットの姿勢に関する信念は、時点ｔ－１におけるその姿勢の信念から更新されるべきである。これは、概念的には、動きｐ（ｘ_ｔ｜ｘ_ｔ－１，ｕ_ｔ）および距離距離モデルｐ（ｚ_ｔ｜ｘ_ｔ，ｍ）を与えられたベイズフィルタを利用することによって行われる。ベイズフィルタは、線形およびガウスモーションおよび距離モデルの事例ではカルマンフィルタになる。他の事例では、これらのモデルの線形化時に、ベイズフィルタは、拡張カルマンフィルタになる。より一般的には、モデル線形化を回避するために、ベイズフィルタは、いくつかの実施形態によれば、パーティクルフィルタ（ＰＦ）によって実施される。ＰＦは、パーティクルと呼ばれる、或る特定の数の可能な姿勢実現値を用いた確率分布（１）を表している。すなわち、姿勢推定３２０からの出力３３０は、姿勢パーティクルの集合である。

図４Ａは、図２Ａの基準系におけるパーティクル群の図である。ロボットの姿勢が、パーティクル４０１、４０２、４０３、４０４、４０５を用いて表されている。各パーティクルは、姿勢を定義する自身のｘ座標４１０、ｙ座標４１１、および向き４１２を有する。集合的に、パーティクル４０１、４０２、４０３、４０４、４０５は、姿勢の確率分布を表している。

図４Ｂは、いくつかの実施形態による、或るエリアにおけるパーティクル群の図である。典型的には、ロボットの姿勢を確率論的に表すのに多くのパーティクルが用いられる。図４Ｂの例では、或る時点ｔにおいて、マップ４２０内のロボット４２１の姿勢は、パーティクル群４２２を用いて表される。

パーティクルフィルタは、以下のように、時点ｔにおけるロボットの姿勢の信念を近似するものとして考えることができる。

ここで、δ（）は、ディラックのデルタ関数（Dirac’s delta function）であり、Ｋは、パーティクルの全体数であり、ｘ_ｔ，ｋは、パーティクルである。パーティクル平均は、ロボットの姿勢の最終点推定値として機能するが、それらの拡散（すなわち、分散）は、姿勢推定値において有することができる信頼度を示唆している。したがって、極めて範囲が限られたパーティクルは、高い信頼度を有するが、このことは、正しい姿勢推定値を必ずしも暗示しない。

いくつかの実施態様では、パーティクルフィルタは、ロボットが存在するエリアから一様にサンプリングされたＫ個のパーティクルを有するように初期化される。時点ｔ－１におけるロボットの姿勢を表すパーティクルの集合｛ｘ_{ｔ－１，ｋ}｝_ｋ＝１ ^Ｋおよび時点ｔにおいて得られるオドメトリ測定値ｕ_ｔを所与とすると、ロボットの動きモデルｐ（ｘ_ｔ｜ｘ_ｔ－１，ｕ_ｔ）は、（仮の）パーティクル｛ｘ^’ _ｋ｝_ｋ＝１ ^Ｋをサンプリングするのに用いられる。各仮のパーティクルｘ^’ _ｋは、距離モデルおよび測定値ｚ_ｔから計算される以下の重みｗ_ｋに関連付けられ、

また、Σ_ｋ＝１ ^Ｋ ｗ_ｋ＝１であるように正規化される。仮のパーティクルは、時点ｔにおけるロボットの姿勢を表すパーティクル集合｛ｘ_ｔ，ｋ｝_ｋ＝１ ^Ｋを生成するために｛ｗ_ｋ｝_ｋ＝１ ^Ｋに従ってサンプリングされる。

（ロボットランデブー）
いくつかの実施形態では、ロボットは、時折遭遇し、何らかの情報を交換して協調的姿勢推定を支援する。いくつかの実施態様では、ランデブーは、ランダムに起こるか、又は、ロボットの適切な制御によって構成される。２つのロボットのランデブー時、少なくとも一方のロボットは、他方のロボットの存在を検出できるようになっていなければならない。２つのロボットのシナリオでは、この要件を達成する１つの方法は、一方のロボットに視覚タグを備え、他方のロボットにＲＧＢカメラを備えることである。その場合、コンピュータビジョンアルゴリズムがＲＧＢカメラフィード内でタグを探索し、ランデブー時に、他方のロボットの存在を検出する。この要件を達成する他の手法も可能である。例えば、いくつかの実施態様では、各ロボットが、シグネチャ超音波信号を放出する。それゆえ、２つのロボットが互いに接近した場合、例えば、１０メートル未満の距離にある場合、これらのロボットは、互いに「ヒアリング（hear）」し、受信されたシグネチャ超音波信号から互いに識別情報を検出することができる。

ロボット同士が互いの近い位置にあることを自身が検出した後、一方（又は双方のロボット）が、他方のロボットの進行方向に対する一方のロボットの進行方向がφであるように、相対距離ｒ並びに相対方位角および進行方向を測定する。１つの実施形態では、方位角φは、他方のロボットの識別情報タグを含むＲＧＢカメラフレームから取得され、一方で、距離ｒは、方位角測定値および測距スキャンから得られる。

図５Ａは、１つの実施形態における２つのロボットのランデブーを示す概略図である。この例では、ロボットＡ ５１０は、自身の視覚タグとして機能するチェッカーボード５１１を備える。その場合、ロボットＡ ５１０およびロボットＢ ５１２は、近接しているとともに、ロボットＢのカメラ５１３がロボットＡのチェッカーボードを向くような向きにあるとき、ロボットＢは、ロボットＡの存在を検出し、それらの相対姿勢５１５を測定する。

例えば、１つの実施形態では、ロボットＡの検出時に、ロボットＢは、２つのステップにおいて相対姿勢測定を実行する。まず、ＲＧＢＤセンサによって提供される奥行き測定値から生成される点群から、チェッカーボードパターンの中心のロケーション（ｘ，ｙ，ｚ）が取得される。その場合、相対姿勢推定は、チェッカーボードコーナーの点群を抽出することと、ＯｐｅｎＰＣＬ法を用いて面法線を推定することとによって実行される。

図５Ｂは、いくつかの実施形態によって用いられる２つのロボット間の相対姿勢の概略図である。ベイズフレームワーク内で情報融合を実行するために、いくつかの実施形態は、ロボットの姿勢を条件として、相対姿勢測定値の確率論的モデルを指定する。形式的には、このモデルは、ｐ（ｒ，φ｜ｘ_ｔ ^Ａ，ｘ_ｔ ^Ｂ）を用いて捕捉され、ここで、図５Ｂを参照すると、ｘ_ｔ ^Ａ ５２１およびｘ_ｔ ^Ｂ ５２２は、ランデブー時点ｔにおける２つのロボットの姿勢を示す。そのために、相対距離ｒおよび方位角－進行方向測定値φは、独立であり、それぞれが、０平均およびそれぞれ分散σ_ｒ ^２およびσ_φ ^２のガウス分布誤差を有する。すなわち、

であり、ここで、

は、平均μおよび分散σ^２のガウス分布変数ｘであり、一方、ｄ ５２３およびρ ５２４は、姿勢ｘ_ｔ ^Ａおよびｘ_ｔ ^Ｂに対応する相対距離および方位角－進行方向である。一般に、分散σ_ｒ ^２およびσ_φ ^２は、測定データから較正することもできるし、測定方法の精度およびマップｍの分解能に基づいて評価することもできる。当業者であれば認識するであろうように、以下で説明される情報融合は、ｐ（ｒ，φ｜ｘ_ｔ ^Ａ，ｘ_ｔ ^Ｂ）の選択によって制限されない。

（パーティクルチューナ）
図６Ａは、いくつかの実施形態によって用いられる協調的位置特定パイプラインの原理的なブロック図である。最初に、ロボットは、単独動作状態（on its own）であり、環境を通して移動し、パーティクルフィルタ１３１を用いて単一のロボットによる測位６０１を実行する。このロボットが他の何らかのロボットに接近した場合、一方又は双方のロボットがロボット検出６０５を実行する。ロボット検出の結果、２つのロボットが互いに近づいていることを各ロボットが認識し、一方又は双方のロボットが、例えば、姿勢推定構成要素１３７を用いてロボット間の相対姿勢６０６を測定する。一方のロボットのみがこの相対姿勢を測定する場合には、そのロボットは、相対姿勢測定値を他方のロボットに送信することができる。このステージの際に、ロボットは、それらの姿勢パーティクルを交換する。各ロボットは、その後、協調的測位６００を実行する。これは、６０１から得られた自身の姿勢パーティクル、他方のロボットの姿勢パーティクル６１０および相対姿勢測定値６０６を融合する。ロボットによって実行された協調的測位６００の結果、そのロボットに対応する更新された姿勢パーティクル６１５が得られ、単一のロボットによる測位６０１に返される。その後、各ロボットは、再び単独動作状態になり、他のロボットに遭遇するまで単一のロボットによる測位アルゴリズムの実行を継続する。

より詳細に説明すると、２つのロボットＡおよびＢは、時点ｔにおいて遭遇し、それらの姿勢パーティクルを交換する。加えて、ロボット間の相対姿勢を測定するロボットは、他方のロボットに測定値を送信する。１つの実施形態は、ロボットＡに対する情報融合アルゴリズムを開示する。しかしながら、同等の融合を、ロボットＢによって実行することができる。

図６Ｂは、いくつかの実施形態による、パーティクルチューナ１３３によって実行される情報融合の原理的なブロック図である。時点ｔにおけるロボットＡの姿勢を表すパーティクルの集合６２１、ロボットＢから受信された、時点ｔにおけるロボットＢの姿勢を表すパーティクルの集合６２２は、６００において実施される本発明者らのアルゴリズムに従って相対姿勢測定値６０６とともに融合される。このブロックの出力６１５は、ロボットのランデブー時のロボットＡの姿勢を表す、結果として得られる更新されたパーティクルの集合である。

一般性を失うことなく、ロボットが初めて遭遇すると仮定すると、時点ｔまでロボットＡによって行われる全ての距離測定値およびオドメトリ測定値は、集合的に、Ｄ_ｔ ^Ａ＝｛ｕ_１：ｔ ^Ａ，Ｚ_１：ｔ ^Ａ｝として示される。ｐ（ｘ_ｔ ^Ａ｜Ｄ_ｔ ^Ａ，ｍ）を用いて記述される、時点ｔにおけるロボットＡの姿勢は、本質的には、ランデブー時点ｔにおけるロボットＡの姿勢に関する事前分布である。

ロボットＡは、ロボットＢから、パーティクルＸ_ｔ ^Ｂ＝｛ｘ_ｔ，ｋ ^Ｂ｝_ｋ＝１ ^Ｋを受信し、これを、ロボットＡは以下のように扱う。

とりわけ、ロボットＡがアクセスを有するのはロボットＢのパーティクルのみであるために、（５）において、時点ｔまでロボットＢによって収集される距離測定値およびオドメトリ測定値を条件とすることが回避され、ロボットＡは、その後、ロボットＢの姿勢の確率論的表現を構築するためにロボットＢのパーティクルを使用する。ロボットＢのパーティクルに加えて、ロボットＡは、自身の測定値から直接、又はロボットＢから受信することによって間接的に、相対距離ｒおよび方位角－進行方向φの測定値へのアクセスを有する。

ロボットＡがロボットＢのパーティクルを受信し、ｒおよびφの測定値を取得した後、ロボットＡは、それらを、自身の姿勢ｘ_ｔ ^Ａの現在の推定値と融合する。形式的には、これは、ロボットＡに利用可能な全ての情報を条件として、姿勢ｘ_ｔ ^Ａの事後分布を計算することを目標とするベイズ推定によって行われる。表記が混乱したものになることを回避するために、全ての式における時間インデックスｔおよびマップｍに対する条件付けを組み入れているものとして、ロボットＡの姿勢の事後分布は、以下によって与えられる。

ここで、

は、ｘ^Ｂに依存せず、

は、ベイズの定理、並びにｘ^Ａおよびｘ^Ｂを条件としたときのＤ^ＡおよびＸに対するｒおよびφの条件付き独立性から導かれる。（６）に（５）を代入することによって、以下が得られる。

一般に、ロボットＡの姿勢は、ランデブーの前の自身の姿勢分布ｐ（ｘ^Ａ｜Ｄ^Ａ）が既知であることを所与とすると、（７）を用いて更新される。本発明者らは、ランデブー時のロボットＡの姿勢の事後分布を表すサンプルを導出するために重点サンプリング（importance sampling）を用いる。重点サンプリングの術語に従って、事後分布（７）は、目標分布（target distribution）である。サンプルを導出するために用いられる提案分布（proposal distribution）は、ｐ（ｘ^Ａ｜Ｄ^Ａ）である。それゆえ、重点サンプリングは、以下から開始される。

ロボットＡの事前分布ｐ（ｘ^Ａ｜Ｄ^Ａ）は、パーティクルＸ^Ａを用いて表されると仮定すると、（８）におけるパーティクルは、Ｘ^Ａから一様にランダムにサンプリングすることによって有効に取得される。結果として得られるパーティクルに関連付けられた重みは、以下のように目標分布と提案分布との間の比から計算され、

また、Σ_ｌ＝１ ^Ｌ ｗ_ｌ＝１となるように正規化される。最後に、パーティクルの集合｛ｘ_ｌ ^Ａ｝_ｌ＝１ ^Ｌは、更新されて（６１５）、例えば｛ｗ_ｌ｝_ｌ＝１ ^Ｌに従って再サンプリングされて、一様重みを有するパーティクルの最終集合が得られ、これは、ランデブー後のロボットＡの姿勢事後分布を表す。代替的に、パーティクルの更新された重みは、非一様にサンプリングされたパーティクルを生成するように保持することができる。

図７は、１つの実施形態によって用いられる協調的測位の擬似コードである。この実施形態は、説明される方法の計算的なボトルネックは、（繰り返しがないことを仮定して）パーティクルｘ_ｌ ^Ａごとに相対姿勢確率分布のＫ回の評価を要求する（９）から生じるという認識に基づいている。複雑度を低減するために、この実施形態は、（９）における総和を、検討されるｘ_ｌ ^ＡおよびＸ^Ｂからの一様にランダムにサンプリングされたパーティクルにおける相対姿勢分布の単一の評価に置き換える。この結果、ロボットＡのパーティクルの数における線形複雑度のパーティクル更新スキームが得られる。アルゴリズムからの出力は、ロボットＢから受信された情報および相対姿勢測定を融合した際のロボットＡの姿勢事後分布を表すパーティクルの集合

である。姿勢事後分布更新についての全ての計算は、ロボットＡのプラットフォーム上でローカルに行われる。

類似的に、時点ｔにおけるランデブーの前のロボットＡ自身の姿勢を表すロボットＡのパーティクル、および相対姿勢測定値を受信した際にロボットＢの事後分布を更新するために、ロボットＢのパーティクルチューナによって同じ情報融合が用いられる。情報融合は、ロボットＢのパーティクルチューナによってロボットＢのプラットフォーム上でローカルに実行され、出力は、ランデブー時のロボットＢの姿勢事後分布を表すパーティクルの集合

である。

ランデブー後、各ロボットは、自身のパーティクルフィルタに、更新されたパーティクル集合

を与え、同じロボット又は他の何らかのロボットとの次のランデブーまで、単一のロボットモードにおいて単独動作でエリアを通した移動を継続する。いくつかの実施形態は、ロボットが短い時間区間内での同じロボットとの複数回のランデブーに基づいて自身の姿勢パーティクルを更新しないように注意を払う。これにより、有効に、同じ又は同様の情報を複数回融合することに至るとともに、過度に高信頼度（over-confident：自信過剰）でかつ潜在的に誤った推定値を生成する結果となる可能性がある。例えば、いくつかの実施形態では、データ融合が実行され、例えば、ロボットＡが、ロボットＡおよびロボットＢのランデブーと以前のランデブーとの間の距離が閾値を超えたときにのみ、ランデブーを検出すると、ロボットＢに、ロボットＢ自身のパーティクルを送信するように要求する。例えば、異なる実施形態において、距離は、以前のランデブーから経過した時間、および以前のランデブーからロボットによってカバーされた距離のうちの一方又は組み合わせである。

とりわけ、ロボットＢの姿勢が既知でありかつ相対姿勢測定値が正しい場合、ロボットＡの姿勢は、相対姿勢測定値に従ってロボットＢの姿勢を変換することから即座に導かれる。それゆえ、１つの実施形態は、ランデブーの前のロボットＢのパーティクルがロボットＢの真の姿勢の周囲の小空間内の範囲に限られており、一方、ロボットＡのパーティクルはエリア中にわたって拡散されている事例を検討する。それゆえ、Ｘ^Ｂからサンプリングされた各パーティクルｘ_ｌ ^Ｂは、そのような範囲が限られた空間内にあり、一方で、一様にサンプリングされたパーティクルｘ_ｌ ^Ａは、エリア内の至る所に存在する。したがって、ｘ_ｌ ^Ａについて計算される重みｗ_ｌは、ｘ_ｌ ^Ａおよびｘ_ｌ ^Ｂが相対姿勢測定値に従っている場合には比較的大きく、そうではない場合には比較的小さい。したがって、低下していく品質の相対姿勢測定値を仮定すると、ロボットＡの真の姿勢の周囲に存在する

からの全てのパーティクルｘ_ｌ ^Ａは、再サンプリングステップ後にも残存し、最終出力

を生成する。これは、所望の結果である。

（パーティクルチューナの例示の実施態様）
図８Ａは、いくつかの実施形態による、重み付けされたパーティクルペアを計算する方法のブロック図である。方法は、単一の姿勢パーティクルペアおよびその重みを生成する。方法は、ロボットＡのパーティクルチューナによって実施される。方法は、ランデブーの直前のロボットＡの姿勢に対応する姿勢パーティクルの集合６２１から姿勢パーティクルをランダムにサンプリングする（８１１）。同様に、ランデブーの直前のロボットＢの姿勢に対応する姿勢パーティクルの集合６２２からの姿勢パーティクルもランダムにサンプリングされる（８１２）。それゆえ、ブロック８１１および８１２は、姿勢パーティクルのペアを得る。パーティクルのペアは、相対姿勢測定値６０６および相対姿勢モデル、例えば（９）によって示唆されるような相対姿勢モデルに従って重み付けされる（８１５）。したがって、プロセス８００全体は、姿勢パーティクルの重み付けされたペア８２０を出力する。

１つの実施形態では、パーティクルチューナは、ロボットのパーティクルを一様にランダムにサンプリングし、各ロボットのパーティクルの集合からの任意のパーティクルがサンプリングされる、すなわち、集合から選択される可能性は等しい。この実施形態は、全てのパーティクルが等しい重みを有するときに有利である。別の実施形態では、パーティクルチューナは、パーティクルの重みに従って非一様にランダムにロボットのパーティクルをサンプリングし、より大きい重みを有するパーティクルは、より小さい重みを有するパーティクルよりもサンプリングされる可能性が高いようになっている。この実施形態は、異なるパーティクルが異なる重みを有し得るときに有利である。

図８Ｂは、図８Ａに示されたように生成された姿勢パーティクルの重み付けされたペアに基づいてパーティクルを更新するいくつかの実施形態によって用いられる方法のブロック図である。例えば、姿勢パーティクルの複数の重み付けされたペアが並列に生成される（８００）。並列した実装によって、全体として、ロボットのランデブー時の姿勢パーティクルを迅速に更新することができる。各並列分岐は、姿勢パーティクルの１つの重み付けされたペアを出力する。全ての並列分岐の出力が収集され、それらの重みは、それらが総和して１になるように正規化される（８３０）。そのため、いくつかの実施形態では、パーティクルチューナは、並列に、パーティクルをペアリングし、ペアリングされたパーティクルを重み付けするように構成される。例えば、プロセッサは、複数のＣＰＵを有する並列プロセッサである。

いくつかの実施態様では、パーティクルの正規化された重みは、パーティクルの重み付けされた集合を結果としてもたらすように保持される。また、いくつかの実施形態では、重み付けされたパーティクルは、それらの重みに従って再サンプリングされて、その結果、パーティクルの等しく重み付けされた集合がもたらされる。例えば、姿勢パーティクルペアは、より大きい重みを有するペアがより高い確率でサンプリングされるように正規化された重みに従って再サンプリングされる（８３５）。このステップの結果は、姿勢パーティクルペアの集合である。方法がロボットＡ上で実行される場合、姿勢パーティクルのペアにおけるロボットＡに対応する姿勢パーティクルが抽出され、それらの集合は、更新された姿勢パーティクルの集合６１５である。同様に、ロボットＢに対応する姿勢パーティクルは、姿勢パーティクルペアの集合から抽出され、それらの集合は、更新された姿勢パーティクルの集合６１５である。

（擬似ランダムサンプリング）
種々の実施形態は、何らかの離散分布に従って、ランダムサンプリングに、姿勢パーティクルの繰り返しを組み込む。Ｌ個のパーティクルを仮定すると、第ｌのエントリｚ_ｌが第ｌのパーティクルを選択する確率を示すようにこの分布をベクトルｚで示す。本明細書において用いられる場合、Σ_ｌ＝１ ^Ｌ Ｚ_ｌ＝１および繰り返しを用いるサンプリングは、同じパーティクルを複数回サンプリングすることができることを意味する。開示される方法において用いられる分布ｚの１つの例は、一様分布であり、ここで、ｚ_ｌ＝１／Ｌ、ｌ＝１，．．．，Ｌであり、この一様分布は、姿勢パーティクルのペア８１１、８１２を生成する際に用いられる。別の例では、ｚ、ｚ_ｌ＝ｗ_ｌ／Σ_ｌ＝１ ^Ｌ ｗ_ｌは、姿勢パーティクルのペアを再サンプリングする（８３５）ために用いられる。

図９Ａは、所与の離散分布ｚについてのパーティクルサンプリングを実施するためにいくつかの実施形態によって用いられる擬似ランダムパーティクルサンプリングのブロック図である。一般性を失うことなく、図９Ａの例では、所望の離散分布９０３がエントリｚ_１、ｚ_２、ｚ_３、ｚ_４を有するようにＬ＝４である。擬似乱数生成器９０１は、その範囲内の各数が選択される等しい尤度を有するように、０～１の数を一様にランダムにサンプリングする。サンプリングされるパーティクルのインデックスは、所望の離散分布９０３および擬似乱数９０２に基づいて決定される（９０５）。これを行うことで、０～１の範囲は、所望の離散分布に従ってＬ＝４個の長さセグメントｚ_１、ｚ_２、ｚ_３、ｚ_４に分割され、サンプリングされるインデックスは、擬似乱数９０２が到達する（９０７）セグメントのインデックスである。図９Ａに示すように、擬似乱数９０２は、ｚ_１＋ｚ_２とｚ_１＋ｚ_２＋ｚ_３との間にあり、したがって、９０５からの出力９０８は、インデックス３である。一般に、出力９０８は、１～Ｌの整数であり、この出力は、姿勢パーティクルルックアップ９０９に進む。そのようなインデックスを有するパーティクルは、サンプリングされる姿勢パーティクル９１０として出力される。説明されるパーティクルサンプリング実施態様は、ｚ_ｌ＝１／Ｌ、ｌ＝１，．．．，Ｌを用いる図８Ａの８１１および８１２において用いられる。また、説明されるパーティクルサンプリング実施態様は、複数のパーティクルが、収集された重み付けされたパーティクルペア８３０からのｚ_ｌ＝ｗ_ｌ／Σ_ｌ＝１ ^Ｌ ｗ_ｌを用いてサンプリングされる図８Ｂの再サンプリング８３５において用いられる。図９Ａからのパーティクルサンプリングルーティーンが８３５において呼び出される回数は、更新されるパーティクルの所望の数に等しい。

したがって、いくつかの実施形態は、姿勢パーティクルをサンプリングするために乱数生成器を用いる。一般に、乱数生成器の出力は、０～１の純粋な乱数であるべきであり、ここで、この範囲からの各数は、サンプリングされる等しい尤度を有する。プロセッサ内の乱数生成器の実施態様は決定論的であるので、このブロックからの出力は、純粋に乱数ではあり得ない。実際には、乱数生成器からの出力は、決定論的である。しかしながら、乱数生成器は、長期において、乱数生成器が生成するシーケンスが乱数のシーケンスに類似するように設計される。それらの理由で、乱数生成器は、擬似乱数生成器、例えば、擬似乱数生成器９０１と一般に称される。

本明細書において用いられる場合、「類似する」という表現によって、擬似乱数生成器９０１が生成したシーケンスが、乱数生成器から出力される純粋にランダムで無限長のシーケンスであれば満たすであろう２つの特性を近似的に満たすことが意味される。第１の特性は、生成される数に一様分布を遵守させることに関し、これは、生成された数のヒストグラムが平坦であることを意味する。第２の特性は、生成される数を互いに無関係にすることに関し、これは、シーケンスの正規化された自己相関をデルタ関数（０ラグにおいて１に等しく、かつ全ての非０ラグにおいて０に等しい）にすることになる。

図９Ｂは、１つの実施形態によって用いられる擬似乱数生成のブロック図である。擬似乱数生成器９０１は、以下に従って擬似乱数整数のシーケンスを生成する線形合同法（ＬＣＧ：linear congruential generator）９２２を含み、

である。ここで、ｙ_ｎは、シーケンス内の第ｎの数であり、ａ、ｂおよびｍは、固定スカラーである。一方、ｍｏｄは、ａｙ_ｎ－１＋ｂのｍによる除算の剰余を得るモジュロ演算（modulus operation：剰余演算）を示す。それゆえ、ｙ_ｎは、０～ｍ－１である任意の整数とすることができる。共通して、ｍは、プロセッサに記憶することができる最大の整数である。

シーケンス内の最初のエントリｙ_１は、事前に固定されたシード９２１によって提供されるｙ_０に基づいて生成される。ｎ＞１の場合、ｙ_ｎは、ｙ_ｎ－１に基づいて生成され、ｙ_ｎ－１は記憶されてフィードバックされる（９２３）必要がある。擬似乱数整数のシーケンス９２４内の各エントリは、ｍによって除算されて（９２５）、０～１の擬似乱数のシーケンス９０２が得られる。擬似乱数のシーケンスを生成するのに他の方法を用いることができる。例えば、合同関係を用いてｙ_ｎを生成するためにより多くの出力ｙ_ｎ－ｋ、ｋ＞０をフィードバックすることができる。

（いくつかの実施形態の効果）
情報融合のための他の方法は、２つのロボットのランデブーの前のそれらの２つのロボットの姿勢パーティクルおよび相対姿勢測定値に基づいてロボットの姿勢パーティクルを更新する。したがって、１つの方法は、密度ツリーを用いて、融合前にロボットのパーティクルからそのロボットの姿勢を推定する。その方法と比較して、本明細書において開示される方法は、密度ツリーを利用する必要がなく、それゆえ、追加の計算コストを課さない。他の方法は、異なるロボットから由来する姿勢パーティクルの全てのペアを検討し、各ペアが、その姿勢パーティクルが相対姿勢測定値とどの程度一致するのかに基づいて重み付けされる。各ロボットのパーティクル群がＬ個のパーティクルを含むと仮定すると、全ての可能なパーティクルペアの重みを計算する全体計算コストは、オーダーＬ^２である。対照的に、本明細書において開示される方法は、姿勢パーティクル更新の計算複雑度がオーダーＬであるようにパーティクルのＬ個のペアを生成する。更に別の方法では、１つのロボットの姿勢パーティクルは、Ｓ個のクラスターにクラスタリングされる。その後、Ｓ個のクラスターのそれぞれを表すＳ個のパーティクルと他のロボットからのＬ個の姿勢パーティクルとの間の全てのペアごとの組み合わせが形成され、それらの重みが、相対姿勢測定値との一致に基づいて計算される。したがって、この方法における重み計算の複雑度は、オーダーＫＳである。

より低い計算複雑度に加えて、本明細書において開示される方法は、他の方法に対して追加の利益を有する。特に、パーティクルのペアは互いに独立しているので、パーティクルペアの生成および重み計算は、並列して行うことができ、これによって、ランデブー時の姿勢パーティクルの更新に必要な時間の潜在的に大幅な節約が得られる。換言すれば、本発明者らによる方法において全てのパーティクルペアを生成および計算するのに必要な時間量は、単一のパーティクルペアの重みを生成および計算するのに必要な時間量に等しい。対照的に、クラスターを用いる方法は、パーティクルペアの重みを単に計算するのにＫＳ倍長い期間と、それに加えてクラスタリングを実行するための追加の期間とを要求する。

本明細書において開示される方法は、外見上では統計的推定原理を遵守することによって導出され、ベイズ推定フレームワークにおいて最適であり、上述された利益は、性能劣化を犠牲にすることはない。むしろ、本発明者らによる方法の性能保証は、ベイジアンの意味でのその最適性から起因しており、従来のパーティクルフィルタと同じである。

（非一様パーティクル重み）
種々の実施形態のいくつかの実施態様は、双方のロボットのランデブーの直前のそれらのロボットの姿勢パーティクルが一様重みを有すると仮定する。これは、Ｌ個のパーティクルを含む姿勢パーティクルの集合内の各パーティクルが重み１／Ｌを有することを意味する。また、説明される方法は、再サンプリング８３５後のランデブー時に更新された姿勢パーティクルの集合を出力し、これは、更新された姿勢パーティクルが一様重みを有することを意味する。全体として、入力および出力の姿勢パーティクルは、一様重みを有すると仮定される。

他のいくつかの実施態様は、姿勢パーティクル重みのための他の選択肢を用いる。したがって、図８Ｂを参照すると、開示される方法の１つの実施形態は、姿勢パーティクルペアの再サンプリング８３５をスキップし、その代わりに、正規化された重みを有する検討されるロボットに対応する姿勢パーティクルを直接出力することができる。同様に、ロボットＡからの姿勢パーティクルの集合６２１および／又はロボットＢからの姿勢パーティクルの集合６２２は、重み付けされたパーティクルを含むことができる。その場合、重みは、総和して１になるように正規化されることが通常である。そうではない場合、重みは、まず正規化されるべきである。その場合、図８Ａを参照すると、姿勢パーティクルのランダムサンプリング８１１および／又は８１２は一様ではなく、パーティクル重みに従い、より大きい重みを有するパーティクルが選択される尤度がより高いようになっている。

（相対姿勢の複数の測定値）
いくつかの実施形態では、ロボットは、ロボット同士が遭遇したときにそれらの相対姿勢を測定する。相対姿勢測定値は、一方のみのロボットによって作成され、かつ、他方に通信される単一の測定値とすることができる。代替的に、各ロボットは、相対姿勢を測定し、測定値を他方のロボットと共有することができる。更に別の代替形態およびより一般的な事例として、各ロボットは、複数の相対姿勢測定を実行し、それらの測定値を他のロボットと交換することができる。結果として、各ロボットは、Ｍ個の相対姿勢測定値ｒ_１、ｒ_２、．．．、ｒ_Ｍおよびφ_１、φ_２、．．．、φ_Ｍを有することができる。その事例では、Ｍ個の測定値が取得されている間にロボットが移動しないと仮定すると、重みｗ_ｌは、以下のように計算される。

複数の姿勢測定値が所与のロボットの姿勢ｘ_ｌ ^Ａおよびｘ_ｌ ^Ｂについて条件付き独立であると仮定すると、重みｗ_ｌは以下の式によって与えられる。

ここで、ｐ（ｒ_ｍ，φ_ｍ｜ｘ_ｌ ^Ａ，ｘ_ｌ ^Ｂ）は、（４）におけるモデル等の相対姿勢測定モデルから評価される。

（２つのロボットの複数のランデブー）
２つのロボットは、ランデブー時に、それらのロボットが実行しているタスクに応じて上位レベル命令に従って単独動作状態を継続する。ロボット同士が再び遭遇し得ることを仮定すると、それらの後続のランデブーにおいて姿勢パーティクルをいかに更新するかについての自然な疑問が生じる。一般に、同じ方法が適用され得る。しかしながら、２つのロボットが比較的短い時間区間内に２回以上遭遇する場合、説明された方法を用いることは、複数回同じ情報を融合することになり得る。その場合、結果として得られる姿勢パーティクルは、必要以上に縮小し、過度に高信頼度の姿勢推定値が結果としてもたらされる。これが起こらないことを確実にするために、いくつかの実施態様では、ロボットは、一方又は双方が少なくとも或る特定のアプリケーション依存の距離をトラバースしていない限り、情報融合を実行しない。ロボットが或る特定の距離を移動した後にのみ情報を交換および融合することができるようにすることによって、本発明者らは、ロボットが最後のランデブー以来に収集した新たな情報量が十分大きくなり、極めて過度に高信頼度の姿勢推定値を取得しないようにする。

（複数のロボットのランデブー）
開示される方法は、３つ以上のロボットの協調位置特定に拡張可能である。すなわち、ロボットＡがまずロボットＢに遭遇し、少し移動して、その後、ロボットＣに遭遇すると仮定する。そのようなシナリオにおいて、ロボットＡは、まず、ロボットＢから受信する情報に基づいてロボットＡ自身の姿勢パーティクルを更新し、ロボットＣに遭遇するまで、単一のロボットによる測位を実行する。その後、ロボットＡは、ロボットＣから受信する情報を用いてロボットＡ自身の最新の姿勢パーティクルを更新する。

同様に、ロボットＡ、ＢおよびＣが同時に遭遇すると仮定する。そのような事例では、ロボットＡは、まず、ロボットのうちの１つ、例えば、ロボットＢから受信する情報に基づいてロボットＡ自身の姿勢パーティクルを更新する。その後、ロボットＡは、他のロボット、例えばロボットＣから受信する情報を用いてロボットＡ自身の最新の姿勢パーティクルを更新する。他のロボットは、同様の方法において各自の姿勢パーティクル更新を進める。

図１０は、いくつかの実施形態による、ロボットによる測位システム１０００のブロック図を示している。システム１０００は、ロボットＡおよび／又はＢの内部に実装することができる。また、システム１０００は、ロボットＡおよび／又はＢに通信可能に接続することができる。

システム１０００は、カメラ１０１０、慣性測定ユニット（ＩＭＵ）１０３０、プロセッサ１０５０、メモリ１０６０、送受信機１０７０、およびディスプレイ／スクリーン１０８０のうちの１つ又は組み合わせを備えることができる。これらは、接続部１０２０を通じて他の構成要素に動作的に結合することができる。接続部１０２０は、バス、ライン、ファイバー、リンク又はそれらの組み合わせを含むことができる。

送受信機１０７０は、例えば、１つ以上のタイプの無線通信ネットワークを通じて１つ以上の信号を送信することが可能にされた送信機と、１つ以上のタイプの無線通信ネットワークを通じて送信された１つ以上の信号を受信する受信機とを備えることができる。送受信機１０７０は、様々な技術に基づいて無線ネットワークとの通信を可能にすることができる。これらの技術は、標準規格のＩＥＥＥ８０２．１１ファミリーに基づくことができるフェムトセル、Ｗｉ－Ｆｉネットワーク又は無線ローカルエリアネットワーク（ＷＬＡＮ）、標準規格のＩＥＥＥ８０２．１５ｘファミリーに基づくＢｌｕｅｔｏｏｔｈ、近距離場通信（ＮＦＣ）、ネットワーク等の無線パーソナルエリアネットワーク（ＷＰＡＮ）、および／又はＬＴＥ、ＷｉＭＡＸ等の無線ワイドエリアネットワーク（ＷＷＡＮ）等であるが、これらに限定されるものではない。システム１０００は、有線ネットワークを通じて通信するための１つ以上のポートを備えることもできる。

いくつかの実施形態では、システム１０００は、ＣＣＤセンサ若しくはＣＭＯＳセンサ、レーザおよび／又はカメラ等の画像センサ１０１０を備えることができる。この画像センサを、以下では「センサ１０１０」と呼ぶ。例えば、センサ１０１０は、光画像を電子画像又はデジタル画像に変換することができ、取得された画像をプロセッサ１０５０に送信することができる。また、センサ１０１０は、シーン内のターゲット物体から反射された光を検知し、捕捉された光の強度をプロセッサ１０５０にサブミットすることができる。

例えば、センサ１０１０は、「カラー情報」を提供するカラーカメラ又はグレースケールカメラを含むことができる。「カラー情報」という用語は、本明細書において用いられるとき、カラー情報および／又はグレースケール情報を指す。一般に、カラー画像又はカラー情報は、本明細書において用いられるとき、１～Ｎ個のチャネルを含むものとみなすことができる。ここで、Ｎは、画像を記憶するのに用いられている色空間に依存する或る整数である。例えば、ＲＧＢ画像は、３つのチャネルを含み、赤情報、青情報および緑情報についてそれぞれ１つのチャネルを有する。

例えば、センサ１０１０は、「深度情報」を提供する深度センサを含むことができる。深度情報は、深度センサを用いて様々な方法で取得することができる。「深度センサ」という用語は、深度情報を単独でおよび／又は他のいくつかのカメラと併せて取得するのに用いることができる機能ユニットを指すのに用いられる。例えば、いくつかの実施形態では、深度センサおよび光カメラは、センサ１０１０の一部分とすることができる。例えば、いくつかの実施形態では、センサ１０１０はＲＧＢＤカメラを備える。このＲＧＢＤカメラは、カラー（ＲＧＢ）画像に加えて、深度センサが有効にされているときはピクセルごとの深度（Ｄ）情報を捕捉することができる。

別の例として、いくつかの実施形態では、センサ１０１０は、３Ｄ飛行時間（３ＤＴＯＦ）カメラを含むことができる。３ＤＴＯＦカメラを用いた実施形態では、深度センサは、３ＤＴＯＦカメラに結合されたストロボライトの形態をとることができる。このストロボライトは、シーン内の物体を照明することができ、反射された光は、センサ１０１０内のＣＣＤ／ＣＭＯＳセンサが捕捉することができる。深度情報は、光パルスが物体に進んでセンサに戻って来るまでに要する時間を測定することによって取得することができる。

更なる例として、深度センサは、センサ１０１０に結合された光源の形態をとることができる。１つの実施形態では、この光源は、１つ以上の狭い光の帯を含むことができる構造化された光パターン又はテクスチャー付けされた光パターンをシーン内の物体に投射する。深度情報は、物体の表面形状によって引き起こされる投射パターンの幾何学的歪みを利用することによって取得される。１つの実施形態は、赤外線構造化光プロジェクタと、ＲＧＢカメラに位置合わせされた赤外線カメラとの組み合わせ等のステレオセンサから深度情報を求める。

いくつかの実施形態では、センサ１０１０は立体カメラを備える。例えば、深度センサは、２つ以上のカメラを用いてシーンの深度情報を取得することができる受動ステレオビジョンセンサの一部分を成すことができる。捕捉されたシーンにおける双方のカメラに共通の点のピクセル座標を、カメラ姿勢情報および／又は三角測量技法とともに用いて、ピクセルごとの深度情報を取得することができる。

いくつかの実施形態では、システム１０００は、デュアルフロントカメラおよび／又は前面カメラおよび背面カメラ等の複数のセンサ１０１０に動作的に接続することができ、これらの複数のセンサは、様々なセンサを組み込むこともできる。いくつかの実施形態では、センサ１０１０は、静止画像およびビデオ画像の双方を捕捉することができる。いくつかの実施形態では、センサ１０１０は、例えば、３０フレーム毎秒（ｆｐｓ）で画像を捕捉することが可能なＲＧＢＤ又は立体ビデオカメラを備えることができる。１つの実施形態では、センサ１０１０によって捕捉された画像は、生の未圧縮フォーマットとすることができ、処理および／又はメモリ１０６０への記憶の前に圧縮することができる。いくつかの実施形態では、画像圧縮は、プロセッサ１０５０によって可逆圧縮技法又は非可逆圧縮技法を用いて実行することができる。

いくつかの実施形態では、プロセッサ１０５０は、ＩＭＵ１０３０から入力を受信することもできる。他の実施形態では、ＩＭＵ１０３０は、３軸加速度計、３軸ジャイロスコープ、および／又は磁気計を備えることができる。ＩＭＵ１０３０は、速度、方位、および／又は他の位置関連情報をプロセッサ１０５０に提供することができる。いくつかの実施形態では、ＩＭＵ１０３０は、測定情報を、センサ１０１０による各画像フレームの捕捉と同期して出力することができる。いくつかの実施形態では、ＩＭＵ１０３０の出力は、プロセッサ１０５０がセンサ測定値を融合しおよび／又は融合された測定値を更に処理するのに部分的に用いられる。

また、システム１０００は、カラー画像および／又は深度画像等の画像をレンダリングするスクリーン又はディスプレイ１０８０を備えることができる。いくつかの実施形態では、ディスプレイ１０８０は、センサ１０１０によって捕捉されたライブ画像、融合画像、拡張現実（ＡＲ）画像、グラフィカルユーザインターフェース（ＧＵＩ）、および他のプログラム出力を表示するのに用いることができる。いくつかの実施形態では、ディスプレイ１０８０は、ユーザが、仮想キーボード、アイコン、メニュー、又は他のＧＵＩ、ユーザジェスチャおよび／又はスタイラスおよび他の筆記用具等の入力デバイスの或る組み合わせを介してデータを入力することを可能にするタッチスクリーンを備えることができおよび／又はこのようなタッチスクリーンとともに収容することができる。いくつかの実施形態では、ディスプレイ１０８０は、液晶ディスプレイ（ＬＣＤ）又は有機ＬＥＤ（ＯＬＥＤ）ディスプレイ等の発光ダイオード（ＬＥＤ）ディスプレイを用いて実施することができる。他の実施形態では、ディスプレイ１０８０は、ウェアラブルディスプレイとすることができる。いくつかの実施形態では、融合の結果をディスプレイ１０８０にレンダリングすることもできるし、システム１０００の内部又は外部に存在することができる異なるアプリケーションにサブミットすることもできる。

例示的なシステム１０００は、図示した機能ブロックのうちの１つ以上の追加、組み合わせ、又は省略等によって、本開示と整合性を有するように様々な方法で変更することもできる。例えば、いくつかの構成では、システム１０００は、ＩＭＵ１０３０又は送受信機１０７０を備えていない。さらに、いくつかの特定の例示の実施態様では、システム１０００は、周辺光センサ、マイクロフォン、音響センサ、超音波センサ、レーザレンジファインダー等の様々な他のセンサ（図示せず）を備える。いくつかの実施形態では、システム１０００のいくつかの部分は、１つ以上のチップセット等の形態をとる。

プロセッサ１０５０は、ハードウェア、ファームウェアおよびソフトウェアの組み合わせを用いて実現することができる。プロセッサ１０５０は、センサ融合および／又は融合した測定値を更に処理する方法に関連付けられる計算手順又はプロセスの少なくとも一部を実行するように構成可能な１つ以上の回路を表すことができる。プロセッサ１０５０は、メモリ１０６０から命令および／又はデータを引き出す。プロセッサ１０５０は、１つ以上の特定用途向け集積回路（ＡＳＩＣ）、中央および／又はグラフィカル処理ユニット（ＣＰＵおよび／又はＧＰＵ）、デジタルシグナルプロセッサ（ＤＳＰ）、デジタル信号処理デバイス（ＤＳＰＤ）、プログラマブル論理デバイス（ＰＬＤ）、フィールドプログラマブルゲートアレイ（ＦＰＧＡ）、コントローラ、マイクロコントローラ、マイクロプロセッサ、埋め込みプロセッサコア、電子デバイス、本明細書において記述される機能を実行するように設計された他の電子ユニット、又はその組み合わせを用いて実現することができる。

メモリ１０６０は、プロセッサ１０５０の内部に、および／又はプロセッサ１０５０の外部に実装することができる。本明細書において使用されるときに、「メモリ」という用語は、任意のタイプの長期、短期、揮発性、不揮発性又は他のメモリを指しており、任意の特定のタイプのメモリ若しくはメモリの数、又はメモリが記憶される物理媒体のタイプに制限されるべきではない。いくつかの実施形態では、メモリ１０６０は、確率論的複数のロボットによる測位１０５５を容易にするプログラムコードを保持する。

例えば、メモリ１０６０は、静止画像、深度情報、ビデオフレーム、プログラム結果、並びにＩＭＵ１０３０および他のセンサによって提供されるデータ等のセンサの測定値を記憶することができる。メモリ１０６０は、ロボットのジオメトリ、空間のマップ、ロボットの運動学的モデル、およびロボットの動的モデルを記憶するメモリを記憶することができる。一般に、メモリ１０６０は、任意のデータ記憶機構を表すことができる。メモリ１０６０は、例えば、一次メモリおよび／又は二次メモリを含むことができる。一次メモリは、例えば、ランダムアクセスメモリ、リードオンリーメモリ等を含むことができる。図１０においてプロセッサ１０５０とは別であるように示されるが、一次メモリの全て若しくは一部をプロセッサ１０５０内に設けることができるか、又はそうでなくても、プロセッサ１０５０と同一の場所に配置し、および／又はプロセッサ１０５０に結合することができることは理解されたい。

二次メモリは、例えば、一次メモリと同じ、又は類似のタイプのメモリ、および／又は例えば、フラッシュ／ＵＳＢメモリドライブ、メモリカードドライブ、ディスクドライブ、光ディスクドライブ、テープドライブ、ソリッドステートドライブ、ハイブリッドドライブ等の１つ以上のデータ記憶デバイス又はシステムを含むことができる。いくつかの特定の実施態様において、二次メモリは、取り外し可能な媒体ドライブ（図示せず）内の非一時的コンピュータ可読媒体に動作的に収容可能であるか、又は別の方法で、動作的に構成可能とすることができる。いくつかの実施形態において、非一時的コンピュータ可読媒体は、メモリ１０６０および／又はプロセッサ１０５０の一部を形成する。

本発明の上記で説明した実施形態は、多数の方法のうちの任意のもので実施することができる。例えば、実施形態は、ハードウェア、ソフトウェア又はそれらの組み合わせを用いて実施することができる。ソフトウェアで実施される場合、ソフトウェアコードは、単一のコンピュータに設けられるのか又は複数のコンピュータ間に分散されるのかにかかわらず、任意の適したプロセッサ又はプロセッサの集合体において実行することができる。そのようなプロセッサは、１つ以上のプロセッサを集積回路部品に有する集積回路として実装することができる。ただし、プロセッサは、任意の適したフォーマットの回路類を用いて実装することができる。

また、本発明の実施形態は、例が提供された方法として実施することができる。この方法の一部として実行される動作は、任意の適切な方法で順序付けすることができる。したがって、動作が示したものと異なる順序で実行される実施形態を構築することができ、これには、例示の実施形態では一連の動作として示されたにもかかわらず、いくつかの動作を同時に実行することを含めることもできる。

請求項の要素を修飾する、特許請求の範囲における「第１」、「第２」等の序数の使用は、それ自体で、或る請求項の要素の別の請求項の要素に対する優先順位も、優位性も、順序も暗示するものでもなければ、方法の動作が実行される時間的な順序も暗示するものでもなく、請求項の要素を区別するために、単に、或る特定の名称を有する或る請求項の要素を、同じ（序数の用語の使用を除く）名称を有する別の要素と区別するラベルとして用いられているにすぎない。

Claims (20)

1. ロボットの姿勢を推定するロボットのシステムであって、前記姿勢は、前記ロボットのロケーションおよび前記ロボットの向きのうちの一方又は組み合わせを含み、前記システムは、
   前記ロボットの現在の姿勢と近傍ロボットの現在の姿勢との間の相対姿勢の値を示すデータ、および前記近傍ロボットのパーティクルの値を示すデータを受信するように構成された入力インターフェースであって、前記近傍ロボットの各パーティクルは、前記近傍ロボットの現在の姿勢の値の確率を定義する、入力インターフェースと、
   前記ロボットのパーティクルの値を記憶するように構成されたメモリであって、前記ロボットの各パーティクルは、前記ロボットの現在の姿勢の値の確率を定義し、前記メモリは、
   パーティクルの集合を用いて前記ロボットの前記姿勢を追跡するように構成されたパーティクルフィルタであって、各パーティクルは、前記ロボットの現在の姿勢の値の確率を定義する、パーティクルフィルタと、
   パーティクルチューナであって、
   前記ロボットの恣意的にサンプリングされたパーティクルを、前記近傍ロボットの恣意的にサンプリングされたパーティクルとペアリングし、
   ペアリングされたパーティクルによって定義される相対姿勢と、前記ロボットと前記近傍ロボットとの間の前記相対姿勢との間の誤差に反比例するように前記ペアリングされたパーティクルの重みを決定し、
   対応するペアリングされたパーティクルの重みに従って前記ロボットのパーティクルを更新する、
   ように構成される、パーティクルチューナと、
   を含む実行可能な構成要素を記憶するように構成された、メモリと、
   前記ロボットの前記姿勢の変化に応答して、前記パーティクルフィルタを用いて前記ロボットの前記姿勢を追跡するとともに、前記データを受信することに応答して、前記パーティクルチューナを用いて前記パーティクルフィルタのパーティクルを更新するように構成されたプロセッサと、
   前記ロボットの現在の姿勢を出力するように構成された出力インターフェースと、
   を備える、システム。
2. 前記パーティクルチューナは、前記ロボットのパーティクルをランダムにサンプリングして、前記恣意的にサンプリングされたパーティクルをペアリングするように構成されている、請求項１に記載のシステム。
3. 前記パーティクルチューナは、前記ロボットのパーティクルを一様にランダムにサンプリングするように構成されている、請求項２に記載のシステム。
4. 前記パーティクルチューナは、前記パーティクルの重みに従って前記ロボットの前記パーティクルを非一様にランダムにサンプリングするように構成されており、より大きい重みを有するパーティクルは、より小さい重みを有するパーティクルよりもサンプリングされる可能性が高い、請求項２に記載のシステム。
5. 前記パーティクルチューナは、前記ロボットのパーティクルを更新するために、等しい重みを有する前記ロボットの前記姿勢を表すパーティクルの集合を生成するためにそれらの重みに従って前記パーティクルを再サンプリングするように構成される、請求項１に記載のシステム。
6. 前記パーティクルチューナは、前記ロボットのパーティクルを更新するために、異なる重みを有する前記ロボットの前記姿勢を表すパーティクルの集合を生成するためにパーティクルの重みを正規化するように構成される、請求項１に記載のシステム。
7. 前記システムは、等しい確率を有する数字の集合から数字を選択するように構成された擬似ランダム生成器を更に備え、任意の数字は、前記集合から選択される可能性が等しく、前記パーティクルチューナは、前記擬似ランダム生成器を用いて数字を選択するととともに、前記数字に基づいて前記ロボットのパーティクルおよび前記近傍ロボットのパーティクルのうちの一方又は組み合わせを選択して前記ペアリングされたパーティクルを形成するように構成される、請求項１に記載のシステム。
8. 前記プロセッサは、今回のランデブーポイントと以前のランデブーポイントとの間の距離が閾値を超えたとき、前記近傍ロボットとのランデブーを検出すると、前記近傍ロボットに、前記近傍ロボットのパーティクルを送信するように要求するように構成されており、
   前記入力インターフェースは、前記近傍ロボットのパーティクルの値を示すデータを前記近傍ロボットから受信するようにさらに構成されている、請求項１に記載のシステム。
9. 前記距離は、前記以前のランデブーポイントから前記ロボットによってカバーされた距離である、請求項８に記載のシステム。
10. 前記パーティクルチューナは、並列に、パーティクルをペアリングし、ペアリングされたパーティクルを重み付けするように構成される、請求項１に記載のシステム。
11. 前記プロセッサは、並列プロセッサである、請求項１０に記載のシステム。
12. タスクを実行するロボットであって、
    前記ロボットの前記姿勢を変更するように構成されたモータと、
    前記ロボットの前記姿勢を追跡するように構成された請求項１に記載のシステムと、
    近傍ロボットのパーティクルを受信するように構成された受信機と、
    前記ロボットのパーティクルを前記近傍ロボットに送信するように構成された送信機と、
    前記ロボットが前記近傍ロボットに近接しているときに前記近傍ロボットとのランデブーを検出し、前記ロボットの前記姿勢を追跡するために前記パーティクルフィルタに環境測定値およびオドメトリ測定値を提供するように構成された少なくとも１つのセンサと、
    を備える、ロボット。
13. ロボットの姿勢を推定する方法であって、前記姿勢は、前記ロボットのロケーションおよび前記ロボットの向きのうちの一方又は組み合わせを含み、前記方法は、前記方法を実施する記憶された命令と結合された前記ロボットのプロセッサを使用し、前記命令は、前記プロセッサによって実行されると、前記方法のステップを実行し、前記方法は、
    前記ロボットの現在の姿勢と近傍ロボットの現在の姿勢との間の相対姿勢の値を示すデータ、および前記近傍ロボットのパーティクルの値を示すデータを受信することであって、前記近傍ロボットの各パーティクルは、前記近傍ロボットの現在の姿勢の値の確率を定義することと、
    パーティクルの集合を用いて前記ロボットの前記姿勢を追跡することであって、各パーティクルは、前記ロボットの現在の姿勢の値の確率を定義することと、
    前記ロボットの恣意的にサンプリングされたパーティクルを、前記近傍ロボットの恣意的にサンプリングされたパーティクルとペアリングすることと、
    ペアリングされたパーティクルによって定義される相対姿勢と、前記ロボットと前記近傍ロボットとの間の前記相対姿勢との間の誤差に反比例するように前記ペアリングされたパーティクルの重みを決定することと、
    対応するペアリングされたパーティクルの重みに従って前記ロボットのパーティクルを更新することと、
    前記ロボットの現在の姿勢を出力することと、
    を含む、方法。
14. 前記ロボットのパーティクルは、ランダムにサンプリングされて、前記恣意的にサンプリングされたパーティクルがペアリングされる、請求項１３に記載の方法。
15. 前記ロボットのパーティクルは、一様にランダムにサンプリングされる、請求項１４に記載の方法。
16. 前記ロボットのパーティクルは、前記パーティクルの重みに従って非一様にランダムにサンプリングされ、より大きい重みを有するパーティクルは、より小さい重みを有するパーティクルよりもサンプリングされる可能性が高い、請求項１４に記載の方法。
17. 前記方法は、等しい重みを有する前記ロボットの前記姿勢を表すパーティクルの集合を生成するためにそれらの重みに従って前記パーティクルを再サンプリングすることを更に含む、請求項１３に記載の方法。
18. 前記方法は、異なる重みを有する前記ロボットの前記姿勢を表すパーティクルの集合を生成するためにパーティクルの重みを正規化することを更に含む、請求項１３に記載の方法。
19. 前記方法は、今回のランデブーポイントと以前のランデブーポイントとの間の距離が閾値を超えたとき、前記近傍ロボットとのランデブーを検出すると、前記近傍ロボットに、前記近傍ロボットのパーティクルを送信するように要求することと、前記近傍ロボットのパーティクルの値を示すデータを前記近傍ロボットから受信することとを更に含み、前記距離は、前記以前のランデブーポイントから前記ロボットによってカバーされた距離である、請求項１３に記載の方法。
20. 方法を実行するロボットのプロセッサによって実行可能なプログラムが格納された非一時的コンピュータ可読記憶媒体であって、前記方法は、
    ロボットの現在の姿勢と近傍ロボットの現在の姿勢との間の相対姿勢の値を示すデータ、および前記近傍ロボットのパーティクルの値を示すデータを受信することであって、前記近傍ロボットの各パーティクルは、前記近傍ロボットの現在の姿勢の値の確率を定義することと、
    パーティクルの集合を用いて前記ロボットの前記姿勢を追跡することであって、各パーティクルは、前記ロボットの現在の姿勢の値の確率を定義することと、
    前記ロボットの恣意的にサンプリングされたパーティクルを、前記近傍ロボットの恣意的にサンプリングされたパーティクルとペアリングすることと、
    前記ペアリングされたパーティクルによって定義される相対姿勢と、前記ロボットと前記近傍ロボットとの間の前記相対姿勢との間の誤差に反比例するようにペアリングされたパーティクルの重みを決定することと、
    対応するペアリングされたパーティクルの重みに従って前記ロボットの前記パーティクルを更新することと、
    前記ロボットの現在の姿勢を出力することと、
    を含む、非一時的コンピュータ可読記憶媒体。

Images