JP2006520459A - 未知の障害物の中の予測不可能な目標物の実時間追跡 - Google Patents

Abstract

本発明の実施例は、環境内の障害物の配置についての事前の知識を必要とせず、ロボットが追跡する目標物の軌跡も知らずに、障害物のある環境内で動作する移動ロボットの運動を計算するための計画を提供する。本発明の実施例は、目標物の位置と環境内の障害物の位置の測定結果に基づいて、監視ロボットの運動を支配するアルゴリズムを提供する。アルゴリズムは目標物と障害物によって作られた監視者の視野領域との間の幾何学的配置を計算によって記述し、この記述を用いて連続的な制御則を計算する。本発明の実施例は、逃走経路ツリーデータ構造を使用して目標物の監視ロボット検出器からの逃走について起こり得るモードを分類し、逃走経路ツリーを用いて目標物の最短逃走経路を決定する。

Description

本発明は、目標物の追跡能力を持つロボットシステムに関する。より具体的には、本発明は、未知の障害物の中に置かれた予測不可能な目標物の実時間追跡等を提供する目標物追跡アルゴリズムに関する。

移動目標物の追跡、特に移動監視ロボットを用いた移動目標物の追跡は多くの難問を提起している。移動目標物の軌跡が未知で、移動監視ロボットが目標物及びロボット自身が移動するワークスペースに関して事前の知識をもたない場合は、これらの難問の解決はますます難しくなる。

図１はワークスペース１００の中で移動目標物１０１を追跡する監視ロボット１０３を示す。監視ロボット１０３はワークスペース１００の局所化された視野領域１０９をロボットに与える検出器を含む。監視ロボット１０３はワークスペース１００および目標物の軌跡について事前の知識を持たない。従って、監視ロボット１０３にとって、目標物の移動の変化を追跡するために目標物１０１を視野領域１０９の内部にとどめることが望ましい。監視ロボットの作業を複雑化するのはさまざまな障害物１０５、１１１及びこれらの障害物に起因するオクルージョンである。目標物１０１が目標物１０１自身と監視ロボット１０３の間に、これらの障害物１０５及び１１１の１つ、またはオクルージョンを挟むと、監視ロボット１０３は視野領域１０９内で目標物１０３を識別できず、目標物１０１を完全に見失うことになる。図１でワークスペース１００は囲まれた領域として示されているが、理論的にワークスペース１００は囲まれていない。

図２は監視ロボット１０３が使用する追跡アルゴリズムが目標物１０１を追跡するために視覚サーボだけを使用する場合の問題を示す。監視ロボットの検出装置は視野領域１０９ａ−１０９ｃを生成する。監視ロボット１０３は視野領域１０９ａ−１０９ｃのそれぞれで目標物１０１を識別し、目標物が位置１０１ａから位置１０１ｃまで移動するに従って位置１０３ａから位置１０３ｃまで漸次移動する。しかしながら、目標物１０１が障害物１１１の端部に近づくと、目標物の軌跡は端部の後ろ側の位置１０１ｄへと進む。監視ロボット１０３は障害物１１１の端部が障害となって視野領域１０９ｄから目標物１０１を識別できない。従って、監視ロボットの視覚サーボ制御は視覚ロボット１０３にいかなる操舵コマンドを与えるべきか分からない。このように、挟まれている障害物１１１を単純に通過するように目標物の軌跡が取られなければ、監視ロボット１０３は目標物１０１を見失うであろう。

図２に示された追跡問題は、大型の牽引トラックが回転内側の縁石に乗り上げることなくコーナーを曲がろうとする時に直面する問題に幾分似ている。しかしながら図２に示す例においては、監視ロボット１０３は障害物１１１を見通せないし、通過することもできない。従って、純粋な視覚サーボはワークスペース１００のようなワークスペースに対して適切な解を与えない。

他のさまざまな分野において追跡問題が持ち上がっている。しかしながら、オクルージョンを回避することは追跡問題の一般的な懸念事項ではない。例えば、ミサイル制御システムでは、オクルージョン問題を解決する必要は一般には無い。同様に、既知の環境で障害物中の目標物を追跡することは一般にさしたる問題ではない。もし監視ロボットがワークスペースに関して先験的に知識を持てば、ロボットは目標物が選んだ軌跡に反応できる。例えば、先行技術には図１に示したものと類似のワークスペース（美術館）の中を未知の軌跡で目標物が移動する「美術館の警備」として知られている問題の解決方法が含まれる。この問題では移動目標物を追跡するためにワークスペース内のさまざまな場所に固定された検出器が使用される。この問題の定式化において固定の検出器はワークスペースの知識と他の検出器の位置に関する知識を持っている。同様に、もし監視ロボットが目標物の軌跡について事前に知識を持てば、監視ロボットは目標物の移動を計画し、検出器データへの依存を低減する。事前の知識によってオフラインで進める解決策と、さらに小型のロボットで使用可能なものよりもっと能力の高い計算装置を監視ロボットの外部で使用することが可能になる。

同様に、オンラインのゲーム理論計画法は目標物の行動の確率的モデルの作成と、一般にワークスペースについての事前知識を含む。しかしながら、そのような解決策は一般に計算過程としては集約的で、ロボットへの応用には適さないことが多い。

従って、移動監視ロボットによって未知のワークスペースの中を未知の軌跡を持つ目標物を追跡するという課題に対して解決策を見つけ出すことが必要である。その解決策は移動監視ロボットの計算機リソースに及ぼす影響を最小化するのに十分なほど洗練されている必要がある。

発明の要約
本発明の実施例は、未知の環境で目標物の意図についても環境内の障害物の配置についても事前の知識を持たずに、目標物を追跡する監視ロボットの運動を計算するための計画を提示する。本発明の実施例は、環境内の目標物の位置と障害物の位置の計測に基づいて監視ロボットの運動を決定するアルゴリズムを提供する。アルゴリズムによって、障害物によって生成される監視者の視野領域と目標物との間の幾何学的な配置が計算されて記述され、この記述を用いて連続の制御則が計算される。

本発明の他の実施例では、ワークスペース内で複数の障害物の中を移動する目標物を追跡するための方法が提供される。この方法は複数の障害物から障害物の位置を識別する視野領域の作成を必要とし、検出器から受信したデータを用いて目標物の位置を識別する。目標物に対する複数の逃走経路は視野領域を用いて計算され、それぞれの逃走経路は目標物を検出器による検出から遮蔽する経路を表している。複数の逃走経路の中から、長さが最小の逃走経路となる１つの逃走経路セットが識別される。目標物を追跡する監視ロボットに対して操舵コマンドを作成するために使用することができる逃走経路セットに対して逃走リスク勾配が計算される。

本発明の他の実施例では、ワークスペース内で複数の障害物の中を移動する目標物を追跡するシステムが提供される。システムには検出器から受信されたデータを使用して複数の障害物から障害物の位置を識別する視野領域を作成するように構成された視野取得モジュールが含まれる。目標物捕捉モジュールは視野領域内で目標物の位置を決めるように構成されている。逃走経路ツリー構築モジュールは視野領域を用いて目標物に対して複数の逃走経路を識別し、それぞれの逃走経路は目標物を検出器による更なる検出から遮蔽する経路を表している。最短逃走経路ツリー計算モジュールは、最短距離となるように複数の逃走経路の中から１つの逃走経路セットを識別するように構成されている。リスク勾配計算モジュールは、目標物を追跡する監視ロボットに提供可能な逃走経路セットに対して逃走リスク勾配を計算する。

本発明の他の実施例では、ワークスペース内で目標物を追跡する方法が提供される。この方法は目標物と少なくとも１つの障害物を識別する視野領域を得るためにワークスペースを検出することが必要である。逃走リスクは視野領域から計算され、逃走リスクは目標物の軌跡が監視ロボットによる更なる検出から逃れるリスクを表している。監視ロボットからの逃走には、視野領域外への移動と少なくとも１つの障害物によって生成されるオクルージョン内への移動とが含まれる。監視ロボットに対する操舵コマンドは逃走リスクを用いて作成される。操舵コマンドによって目標物が監視ロボットによる検出から逃れる能力が低減される。

本発明の他の実施例では、ワークスペース内で目標物を追跡するシステムが提供される。検出器がワークスペースを記述するデータを取得する。視野領域取得モジュールは検出器から受信したデータを使用してワークスペース内で複数の障害物を識別する視野領域を生成する。目標物捕捉モジュールはワークスペース内で目標物を識別するように構成されている。リスク割付けモジュールは視野領域を用いて、目標物が監視ロボットの検出から逃れる逃走リスクを決める。監視ロボットからの逃走には、監視領域外への目標物の移動と複数の障害物の内の少なくとも１つの障害物によって生成されるオクルージョン内への目標物の移動とが含まれる。運動コマンド計算モジュールは目標物が監視ロボットによる検出から逃れる能力を低減する操舵コマンドを監視ロボットに対して作成する。

本発明の他の実施例では、ワークスペース内で移動する目標物を追跡するための方法が提供される。この方法は目標物をヘッドノードとする逃走経路ツリーの作成を必要とする。複数の逃走経路が目標物に対して識別され、それぞれの逃走経路は目標物を少なくとも１つの検出器から遮蔽するワークスペース経由の経路を表している。複数の逃走経路のそれぞれの逃走経路は逃走経路ツリーにおいてチャイルドノードとして配置され、それぞれの逃走経路は、長さの短い逃走経路が長さの長い逃走経路よりも逃走経路ツリーで高位置を占めるように整理される。より長さの短い逃走経路の１セットが逃走経路ツリーから選ばれる。この逃走経路セットを用いて目標物に対する逃走リスクが計算される。

本発明の他の実施例では、ワークスペース内を移動する目標物を追跡するシステムが提供される。逃走経路ツリー構築モジュールは目標物をヘッドノードとする逃走経路ツリーを作成し、目標物に対して複数の逃走経路を識別するように構成されている。それぞれの逃走経路は目標物を少なくとも１つの検出器から遮蔽するワークスペース経由の経路を表す。また構築モジュールは、複数の逃走経路のそれぞれの逃走経路を逃走経路ツリーのチャイルドノードに配置し、長さの短い逃走経路が長さの長い逃走経路よりも高位置に配置されるように逃走経路がツリーで整理されるように構成されている。最短逃走経路ツリー計算モジュールは逃走経路ツリーからより長さの短い逃走経路の１セットを選択する。リスク割付けモジュールはこの選択された逃走経路のセットを用いて目標物に対して逃走リスクを計算する。

本発明の他の実施例では、逃走経路ツリーデータ構造、目標物に対する位置データ及び目標物を追跡する監視ロボットに対する位置データからなる計算機が読取可能な媒体が提供される。逃走経路ツリーデータ構造は、目標物が検出器による検出から逃れるために使用可能な逃走経路を表すように構成されている。

実施形態の詳細な説明
多くの適用例は制御可能な検出システムへアクセスできる監視ロボットによる移動目標物の連続的な監視を必要とする。目標物を追跡することは目新しいことではないが、従来の技術では障害物の存在が無視され、画像化および目標物の認識問題に焦点が合わされている。障害物の中を移動する目標物に対しては、追跡を達成するためには複雑な移動問題が避けられない。即ち、制御可能な監視ロボットは目標物が障害物によって遮蔽されることを予想して、そのような事態の発生を防止するために移動することが必要である。本発明の実施例は目標物の意図に関する知識も環境内の障害物の配置に関する知識も無い状態で動作する移動ロボットの運動を計算するための手順を提供する。本発明の実施例はまた目標物の位置測定と環境内の障害物の配置測定に基づいて監視ロボットの運動を決定するアルゴリズムを提供する。このアルゴリズムは障害物によってできる監視者の視野領域と目標物との間の幾何学的配置を計算して記述し、この記述を用いて連続的な制御則を計算する。

図３は移動目標物３０１が障害物１１１の回りを移動する時に、追跡アルゴリズムを使用して移動目標物３０１を追跡する本発明の実施例の監視ロボット３０３の動きを説明する。目標物３０１はどのような移動物の形も取ることが可能で、ロボットを始めとして、機械装置、人間のような動物まで可能である。監視ロボット３０３は制御可能な検出器からデータを受信する。例えば、監視ロボット３０３には、追跡アルゴリズムを実行するソフトウェアを始めとして、ロボットに搭載された制御可能な視覚検出器が含まれる。監視ロボット３０３は、追跡アルゴリズムが監視ロボット３０３に適切な指示を出せるように事前に目標物の軌跡を知る必要が無い。監視ロボット３０３はまた事前に準備されたワークスペース３０５のモデル、地図を必要とせず、障害物１１１のような障害物の位置も知らない。従って、追跡アルゴリズムは、監視ロボット３０３と移動目標物３０１の間で検出（例えば視覚的な）の喪失を避けることを目指している。本願の実施例による追跡アルゴリズムは、監視ロボットの視野を妨害するワークスペース３０５内のいかなる障害物も監視ロボット３０３と目標物３０１の両者の運動を制約すると仮定し、逆もまた同様に仮定する（例えば透明な物体および煙幕は除外する）。

それぞれの検出器の掃引から、監視ロボット３０３はワ−クスペース３０５に対して視野領域３０９ａ−３０９ｅ（例えば局所地図）を決定する。監視ロボット３０３は目標物３０１を追跡しながら、自分自身の軌跡を決定するために、計測に基づいて追跡中に計算され、本発明の実施例のロボットの検出器によって生成された視野領域３０９ａ−３０９ｅに依存する。例えば、目標物の軌跡が進むに連れて、監視ロボットの追跡アルゴリズムは目標物３０１が障害物１１１の周りを移動するかを決定し、その後監視ロボット３０３は目標物３０１と検出器のコンタクトを失う。従って、図２の監視ロボットの動きとは対照的に、監視ロボット３０３は目標物３０１の追跡に応じて障害物１１１から少し離れるように移動する。このようにして、目標物３０１が障害物１１１を回り込む時に、視野領域３０９ｅに示されている検出器の掃引によって、監視ロボット３０３は目標物３０１と検出器コンタクトを失わない。

簡単化すると、監視ロボットの追跡アルゴリズムは１平面内だけで光を照射する懐中電灯を使用して未知の環境内を移動する目標物を追跡する人の行動に類似している。さらに、その人の懐中電灯が不規則に明滅して、目標物を見失ったか、目標物を追いかけて再度位置特定ができるか予想できないケースに類似している。追跡問題と解法の詳細な数学的説明は図１３を参照して後述する。

ワークスペースの視野領域
追跡アルゴリズムはワークスペース３０５の形状を解析する必要がある。ロボットの移動の妨害に加えて、障害物は監視ロボット３０３が見ることができるワークスペース３０５の範囲を制限する。例えば、監視ロボット３０３は障害物１１１のコーナーを回った先を見ることができない。このことによって、追跡アルゴリズムは監視ロボットの動きを位置３０３ａから３０３ｃにかけて障害物１１１から少し離れて移動させ、目標物３０１と検出器のコンタクトを維持させると言う監視ロボットの目標を実現している。

監視ロボットの装備配置によってその検出器の視野が決定される。監視ロボットの視野は固定の円錐（サイズは選び得る）に制限されるか、検出器レンジの下限及び／又は上限によって制約される。例えば、図３に示された視野領域は３０９ａ−３０９ｅで示された扇形からなる。追跡アルゴリズムは、人為的モデル又は検出器による測定から視野領域を計算できる。前者の場合、光線掃引アルゴリズムを用いて従来の多角形モデルに対して視野領域を計算できる。後者の場合は、視野領域は従来の手法を用いてレーザーレンジファインダーで測定が可能である。好ましい光線掃引アルゴリズムについて詳細な数学的な説明が図１３を参照してなされる。

追跡計画
目標物追跡は、目標物３０１が監視ロボット３０３の視野に常に存在するような、関数（追跡計画）の計算からなる。さらに、追跡計画は、監視ロボット３０３が通過するトータル距離、監視ロボット３０３から目標物３０１までの距離、又は検知器情報の質の程度のような他の判断基準を最適化することも可能である。いずれかの実施例において、目標物３０３を見失うことは不可避的であろうが、この場合には最適追跡計画は、監視ロボット３０３が最初に目標物３０１を見失う時刻、「逃走時刻」または「逃走予定時刻」（ｔｓｅｃ）を最大化することからなるだろう。例えば、図２に示されるように、逃走時刻は、目標物の移動によって、障害物１１１が目標物３０１と監視ロボット３０３との間に挟まれる瞬時の時刻である。

もし、目標物の行動が全ての時刻にわたって事前に知られていれば、目標物３０１は「予測可能」と呼ばれる。そのような場合には、監視ロボット３０３が目標物３０１の追跡を開始する前に、最適追跡計画は一般にオフラインで計算される。全ての時刻にわたって目標物３０１の位置が知られているので、目標物３０１が見失われた場合には、監視ロボット３０３は目標物３０１を再捕捉することができる。目標物３０１の常時追跡が不可能な場合には、目標物３０１が監視ロボット３０３によって視認可能なトータル時間、「露出」の最大化を逃走時刻の最大化に代えれば代替案となる。例えば、図２に示すように、監視ロボット１０３の目標物１０１に対する露出は監視ロボットの位置１０３ａ−１０３ｃに概略対応する。

追跡計画がある特定の時刻における目標物の状態の関数として計算される場合は、計画は閉ループ内で動作する。さもなければ、追跡計画は開ループ内で処理される。目標物の運動モデルと位置測定が正確と言う保証が無ければ、予測可能な場合も含めて閉ループの計画のほうが開ループのものより一般的に好まれる。

目標物の行動が知られていない場合、目標物３０１は「予測不可能」と呼ばれ、問題の解決法はもっと複雑になる。図２及び図３はいずれも予測不可能な目標物の移動を示す。予測不可能な場合は２つの方法で解析が可能である。目標物の行動が「非決定論的不確定性」としてモデル化される場合、目標物が取り得る行動のセットは分かっているが、特定の時刻において取られる特定の行動については分からないと仮定する。このように、行動のセットは分かっているが、いずれの時刻においても目標物３０１が選択した特定の行動は分かっていない。この場合、それぞれの与えられた時刻に対して最悪ケースの選択がされた場合に、最善の実行をする追跡計画が設計できる。代わりに確率的不確定性モデルが利用可能場合は（例えば確率密度関数が知られている場合）、予測の意味合いで最善の運動計画を計算することができる。

予測不可能な場合は一般に実時間のオンラインで解決される必要がある。コンタクトが失われ目標物３０１の再捕捉の機能が無い場合、監視ロボット３０３に設けられた優れた追跡アルゴリズムは露出の最大化とは対照的に逃走時刻の最大化を試みる。最悪ケースのシナリオに対して設計された計画は未来の時間範囲で目標物の最も不都合な行動を予期し、計算された追跡計画の小さな（場合によっては差分の）初期部分を実行した後、再度全体の手順を繰り返す。一方、目標物の予想行動を予想するように設計された追跡計画は、監視ロボット３０３が検知器によって目標物３０１を視野に捕る今後の可能性の最大化を目指す。

逃走予定時刻の最大化に基づく最悪ケースの追跡は、監視ロボットを積極的に回避しようとする敵対的な目標物に特に適している。特に小型ロボットの場合に当てはまるのだが、この追跡問題の解決には高額の計算処理費用を要する。本発明の実施例では逃走リスクの考えを逃走時刻の代理関数として使用する。後述のように、逃走リスク勾配は解析的に計算され、高速の追跡アルゴリズムとなる。

図４は本発明の実施例による追跡アルゴリズムが実行するステップを示すフローチャートである。それぞれのステップにおいて、追跡アルゴリズムは、監視ロボット(例えば図３に示す監視ロボット３０３)に対して、目標物（例えば図３に示す目標物３０１）が監視ロボットの視野領域から逃れるリスクを最小化する。追跡アルゴリズムはワークスペース(例えば図３に示すワークスペース３０５)の事前に準備されたモデルを必要としない。追跡アルゴリズムは未知の軌跡を持つ一時的な目標物に対しても対応できる。後述するように、追跡アルゴリズムは監視ロボットが目標物とのコンタクトを維持する能力を協調して改善する反応項と予測項の両者を組み合わせる。高度に予測不可能な軌跡を持つ活動的な目標物を用いて、追跡アルゴリズムの良好な動作が実験で示された。

追跡アルゴリズムはステップ４０１−４１１を繰返し実行する。追跡アルゴリズムは２つの幾何学的計算を用いてそれぞれの時間ステップで目標物の逃走リスク（φ）における変化を評価する。第１の計算では、追跡アルゴリズムはワークスペースの視野領域(例えば図３に示す視野領域３０９ａ)を取得する４０１。ステップ４０１ではワークスペース (例えば図３に示すワークスペース３０５) 内の監視ロボットの視野領域（例えば領域３０９ａ）が出力される。目標物の位置及び視野領域に含まれる視野妨害障害物と同様に、検出器の特性（例えば最小及び最大レンジ）が視野領域を制限する。この視野データは他の方法（例えばワークスペース内の他の検出器から）で追跡アルゴリズムに提供することもできる。視野データを持つと、追跡アルゴリズムは視野領域内で目標物（例えば図３の目標物３０１）の位置を決めることができる４０３。障害物と目標物の検出方法の具体的な選択は一般に個々の適用ケースに基づいて行われる。

第２の幾何学的計算４０５では、目標物が監視者の視野領域から逃れるために選択する可能性のある最悪ケースの経路を含むデータ構造、即ち逃走経路ツリー（ＥＰＴ）を生成する。目標物の位置を用いて追跡アルゴリズムは目標物に対して最短の逃走経路を計算する。追跡アルゴリズムは次にＥＰＴの情報を用いて目標物の最短経路に逃走リスクを割付ける４０７。ＥＰＴの計算と適用の詳細について以下に述べる。

追跡アルゴリズムは次にリスク勾配を計算する４０９。監視ロボット位置の差分変化は一般にＥＰＴの差分変化を生み、ＥＰＴの差分変化はリスクφの差分変化を生む。追跡アルゴリズムはこの差分変化を解析的に計算するので、φの勾配は監視ロボットの運動を指示するために使用できる。この方法は差分計画（例えばフィードバック制御器）と同じように移動ロボットを制御するためのオンライン計画を提供する。

追跡アルゴリズムは監視ロボット（例えば図３に示す監視ロボット３０３）に対して運動コマンドをリスク勾配の再帰平均として計算する４１１。

追跡アルゴリズムのステップ４０１−４１１は明確化の目的で図４では連続的に示されている。しかしながら、実行に際してこれらのステップを混ぜれば一般により効率的である。ステップ４０１−４０３及び４１１は実行形態に依存する。位置及び目標物のデータ取得は配置される検出器に依存し、ロボットの操舵制御は実際に使用されるロボットの設計仕様に依存する。ステップ４０７は逃走方程式（後述）の勾配を逃走経路に対して計算することからなる。ステップ４０９はステップ４０５で生成されたＥＰＴのさまざまな逃走経路のリスクの再帰平均の計算からなる。

従って、ステップ４０５が実行される際の速さと効率が追跡アルゴリズムの全体性能に影響を与える。実時間で動作する追跡アルゴリズムに対しては、ＥＰＴの計算は効率的に実行されるべきである。本発明の実施例によれば、光線掃引アルゴリズムはＥＰＴの性能を効率化するのに適した方法である。光線掃引アルゴリズムの特徴について以下に述べる。

本発明の実施例によれば、追跡アルゴリズムは目標物がその視野から逃れるリスクの程度φの差分変化に基づいて１秒当たり数回の頻度で監視ロボット（例えば図３に示す監視ロボット３０３）の向きを再変更する。追跡アルゴリズムは、目標物がいかに早く逃走できるかと、監視ロボットがそのような出来事に対していかに簡単に反応できるかの関数として決定論的にφを計算する。例えば、目標物がコーナー（例えば図３に示す障害物１１１）を回って移動することにより逃走できる場合、φは監視ロボットとコーナーの間の距離に応じて増加する。この方法では、監視ロボットがオクルージョンを起こす端部から離れている場合、オクルージョンを取り除くことは難しいと認識している。

図５−８は、視野領域の取得、目標物の位置取得、ＥＰＴの構築及び最短逃走経路の決定に関する追加情報を示す。

図５は本発明の実施例による検出器データ５０９を示す。検出器データ５０９は検出器５０３によって追跡アルゴリズムに送られ、視野領域（例えば図３に示す視野領域３０９ａ）の決定および目標物の位置決めに使用できる。図示の検出器データ５０９はワークスペースについての単一平面の情報を表す。追跡アルゴリズムは、障害物５０１ａ−５０１ｂと目標物３０１を識別して監視ロボットの視野領域を決定するために検出器データ５０９を使用する。図５に示すように、追跡アルゴリズムは、目標物３０１を囲む滑らかな円によって示されるように、目標物を既に識別している。実施例によっては、目標物の何らかの特徴が追跡アルゴリズムに知らされている場合、目標物の識別は簡単化される可能性がある。例えば、追跡アルゴリズムが目標物は監視ロボットの検出器に対して半径ｒの円イメージで現れると知っていれば、追跡アルゴリズムはそのような情報が無い場合より簡単に目標物を検出できる可能性がある。障害物５０１ａ−５０１ｂを示すぎざぎざマークは追跡アルゴリズムがまだこの検出器データを視野領域の「障害物」として識別していないことを表している。

図６は本発明の実施例による追跡アルゴリズムによって識別された障害物６０１ａ−６０１ｅを含んだ視野領域６０９を表す。前述のように、障害物の１つが監視者と目標物の間に挟まれると、障害物６０１ａ−６０１ｅは監視ロボットが目標物との検出器コンタクトを喪失する逃走リスクの構成要素となる。視野領域６０９は瞬時のワークスペース地図（少なくとも監視ロボットの検出器によって認識可能なワークスペースの一部分）を追跡アルゴリズムに与える。

図７は本発明の実施例による、視野領域６０９の逃走経路ツリー（ＥＰＴ）への移行における第１のステップを表す。視野領域６０９で障害物（例えば障害物６０１ａ−６０１ｅ）の位置を決定しているので、追跡アルゴリズムは次にそれらの障害物によって現出される可能性のあるオクルージョン７０３ａ−７０３ｆを決定できる。例えば、目標物３０１が監視ロボットの位置に基づいてオクルージョン７０３ａ内へ移動すると、監視ロボット３０３は障害物６０１ｂによって目標物３０１の追跡を喪失する。個々のオクルージョン７０３ａ−７０３ｆによって表された領域は目標物に逃走の機会を提供する。換言すれば、監視ロボット３０３から逃走するために、目標物３０１は１つの障害物（例えば障害物６０１ｂ）の横をすり抜ける必要は無く、代わりに目標物３０１は障害物のオクルージョン領域（例えばオクルージョン領域７０３ａの任意の場所）に入り込みさえすればよい。従って、目標物の最短逃走経路は目標物のオクルージョン領域の端部までの距離に基づいて決定できる。

例えば、目標物３０１が監視ロボット３０３によって視認されていると仮定する。目標物３０３に対する「逃走経路」は目標物３０３と監視ロボットの視野領域外（例えばオクルージョン７０３ｃ）の点を結んだ任意の衝突なしの経路である。この経路の逃走点はその経路が監視ロボットの視野領域の境界と交差する点に一致し、自由端に沿って起きる。この実施例では目標物３０１が障害物を貫通できないと仮定しているので、オクルージョン領域の自由端は一般に、障害物（例えば障害物６０１ａ）には該当しないオクルージョン（例えばオクルージョン７０３ｃ）の一部分を表す。当然のことながら、目標物３０１と監視ロボット３０３の特定の構成に対して、無限数の逃走経路が存在する。しかしながら、特定の逃走点に対しては、逃走点と目標物間の長さが最低の経路が存在する。さらに、任意の自由端ｅに対して、端部に沿った全ての逃走点の中で長さが最低の逃走経路が存在する。そのような経路を目標物に対する自由端経由の最短逃走経路（ＳＥＰ）と呼ぶ。ＳＥＰの長さは自由端ｅを経由する最短逃走距離（ＳＤＥ）である。目標物３０１が逃走経路を通過する最短時刻が逃走経路の「逃走時刻」である。任意の自由端ｅと与えられた目標物位置に対して、逃走時刻が最小の経路が存在するが、この経路は一般にはＳＥＰと同じではない。

図８は本発明の実施例による、視野領域６０９に組み合わされた逃走経路ツリー８１１を示す。前述のように、ＥＰＴ８１１は一般に監視ロボットのメモリーに保存されるデータ構造を表す。しかし、読者のＥＰＴの理解と、ＥＰＴと追跡アルゴリズムによって受信される検知器データ間の関係の理解を助けるために、ＥＰＴ８１１を視野領域６０９に組み合して示した。

ある構成に対して全ての逃走経路が計算されれば、ツリー構造、即ち「逃走経路ツリー」が形成される。このツリーの根元には目標物（例えば目標物３０１）があり、ツリーのそれぞれの枝は自由端で終わっている。ＥＰＴの各々のノード（例えばノード８０７）が視野領域の頂点であるので、このツリー複合体は線形である。

ＥＰＴ８１１は目標物３０１が監視ロボット３０３の視野から逃れるために取ることができる経路を地図で示している。経路はオクルージョンの端部又は障害物の端部へ引くことができる。ＥＰＴ８１１は逃走経路間の距離の関係をも示している。例えば、ＥＰＴ８１１は逃走経路８０１ａが逃走経路８０１ｄの親であることを示している。換言すると、目標物３０１は逃走経路８０１ａに沿って移動し逃走経路８０１ｄへ到達する。ＥＰＴ８１１を見れば、目標物３０１に対する最短の２つの逃走経路は逃走経路８０１ｂと８０１ｃであることが分かる。

最悪ケースシナリオの予想に基づく追跡計画は目標物が最も速い経路（例えば逃走経路８０１ｂ）を取って逃走すると仮定しているので、逃走経路の計算は重要である。そのような計画の１つは局所的にＳＤＥを最小化する位置へ監視ロボット３０３を移動させることである。ここでは追跡アルゴリズムは最短の追跡経路を計算できると仮定する。ＳＤＥは目標物が監視ロボットの視野領域を外れる可能性の最悪ケースの程度である。ＳＤＥが大きくなればそれだけ目標物が監視ロボットの視野内に留まる可能性が高くなる。代替案としては、全ての経路ＳＥＰにわたり平均長さを最小化するか、全ての個々の経路にわたって動作する類似の関数を最適化することが上げられる。

図９−図１２は追跡アルゴリズムに取り込まれ、監視ロボットが目標物（例えば目標物３０１）とコンタクトを保つ能力を改善する反応項と予測項を示す。

図９は本発明の実施例による、追跡アルゴリズムのリスク基準の計画に対する初期の考慮事項を示す。前述のように、目標物３０１がオクルージョンに向かって移動するにつれて、監視ロボット３０３はオクルージョンから離れるように移動して反応する。直線９０１は、障害物１０５の端部によって現出するオクルージョン内へ入った目標物の経路に基づいて、監視ロボットが目標物３０１と検出器コンタクトを喪失した状態を示す。このように目標物３０１が障害物１０５によるオクルージョンに向かって逃走経路９０３に沿って移動すると、監視ロボット３０３は経路９０５に沿って移動して反応する。目標物３０１が監視ロボットから逃走するリスクは、最短逃走経路（例えば経路９０３）の長さの逆数の単調増加関数で表すことができる。

図１０は本発明の実施例による、障害物１０５によって作られるオクルージョンに向かってさらに移動する目標物３０１と反応コンポーネントの役割を示す。監視ロボット３０３が経路９０５に沿って移動すると、目標物３０１の最短逃走経路はもはや経路９０３ではなくて、経路１００３が代替する。従って、障害物１０５によって現出されたオクルージョン内へ目標物の経路が移動することを基に、監視ロボットの目標物３０１に対する検出器コンタクトの喪失が直線１００１によって表される。このようにして、この新しい逃走経路が監視ロボット３０３に対して別の位置を決定するために使用される。

図９及び図１０は本発明の実施例による、追跡アルゴリズムの反応コンポーネントを示す。この反応コンポーネントは、最短逃走経路に割付けられたリスクの最小化を試みて目標物３０１の移動に反応する追跡アルゴリズムの一部分である。しかしながら、このリスクを最小化することだけが追跡アルゴリズムで考慮されるべき事柄ではない。例えば、目標物が障害物１０５によるオクルージョンに向かう軌跡を取れば、追跡アルゴリズム（予測コンポーネントの無い状態）は目標物３０１とのコンタクト喪失を遅らせるために障害物１０５から離れるように移動を継続する。不幸にして監視ロボット３０３が反応コンポーネント軌跡に従い続ける場合は、目標物３０１と障害物１０５の間の距離は増加しつづける。この距離の増加は、目標物３０１が取る今後の軌跡において、監視ロボット３０３に対する問題を引起す可能性がある。

図１１は本発明の実施例による、反応コンポーネント９０５と予測コンポーネント１１０３との組合せ、および監視ロボットに対する軌跡１１０７を示す。図１２は、目標物３０１が監視ロボットと同様に障害物１０５と同じ側にある例を示すことによって、予測コンポーネント１１０３の役割を説明する。この例では、目標物の最短逃走経路１２０１は障害物１０５へ向かう。従って、監視ロボットの軌跡は（図示の時間内で）監視ロボットのオクルージョンライン１２０３に沿った移動になる。予測コンポーネントは監視ロボットとオクルージョン点との間の距離の２乗で表される。

図１１に戻ると、監視ロボットの経路は反応コンポーネント９０５の軌跡にも予測コンポーネント１１０３の軌跡にも従わない。追跡アルゴリズムは監視ロボットが軌跡１１０７に沿って移動するように反応コンポーネント９０５と予測コンポーネント１１０３を合成する。

前述のように、逃走予定時刻の代理として追跡アルゴリズムで使用される関数が「逃走リスク」で、全ての自由端ｅに対して次のように定義される。

ここでｈ＝ＳＤＥ（ｅ、ｑ^t）は目標物と端部ｅの間の最短距離を、ｒ_oは監視ロボット（ｑ^o）からｅでオクルージョンを発生するコーナー迄の距離を、ｄは目標物（ｑ^t）とこのコーナーの間の距離を、ｃは正の定数を、ｆ（１／ｈ）は１／ｈの単調増加関数を表す。ｃｒ_o ²の最小化は監視ロボットの今後の追跡能力を増加させるので、この項が予測コンポーネントである。ｆ（１／ｈ）の項の最小化は目標物が現在の視野領域から逃走する可能性を減少させるので、この項が反応コンポーネントである。具体的なｆ（１／ｈ）の実施例としてｌｏｇ（１／ｈ²＋１）および（１／ｈ）^m+2が上げられる。ここでｍは負でない整数である。

クローズドフォームで監視者の位置に関する追跡方程式の勾配を計算することが可能である。逃走経路ＳＥＰ（ｅ、ｑ^t）がＶ（ｑ^o）（例えば頂点Ｖ（ｑ^o）またはコーナー経由で）を退去する仕方により、勾配は異なる式で表される。一般にリスク関数の勾配は全ての端部ｅで次の形を取る。

ここでｆ'（１／ｈ）はｆ（１／ｈ）の微分で、▽ｈはｈの勾配である。▽ｈは端部ｅに関連した逃走経路ＳＥＰ（ｅ、ｑ^t）の幾何学に従って計算される。目標物が単一点の場合に勾配の計算は悪化するが、物理的な寸法のある目標物に対しては勾配がうまく定まる。▽φ^eは解析的に計算するので、追跡アルゴリズムは現在の検出器情報しか必要としない。例えば本発明の実施例では関数ｆ（１／ｈ）は（１／ｈ）²に等しい。図１１のシナリオに対してリスク勾配は次式で表される。

図１２に示すシナリオに対してリスク勾配は次式で表される。

このように追跡計画は監視ロボット（例えば図３の監視ロボット３０３）をＶ（ｑ^o）内の全ての自由端ｅにわたる▽φ^eの平均値とは逆の方向へ移動させることからなる。

しかしながら、個々のリスクを集める他の方法も可能である。

図１３は本発明の実施例による追跡アルゴリズムを実行するように構成された監視ロボット１３０１のブロック図である。追跡アルゴリズムを実行する時、ロボット１３０１のＣＰＵ１３０５は追跡アルゴリズムのさまざまなステップ（例えば図４に示すステップ４０１−４１１）を実行に移すための指示を含んでいる。従って、ＣＰＵ１３０５は、視野領域取得モジュール１３０３、目標物捕捉モジュール１３０７、逃走経路ツリー構築モジュール１３０９、最短ＥＰＴ計算モジュール１３１１、リスク割付けモジュール１３１３、リスク勾配計算モジュール１３２１および運動コマンド計算モジュール１３２３のようなソフトウェアモジュールを含んでいる。

視野領域取得モジュール１３０３は追跡アルゴリズムが使用する視野領域（例えば図４のステップ４０１に示す視野領域）を検出器データから生成する。目標物捕捉モジュール１３０７は視野領域の目標物を識別するステップ（例えば図４に示す目標物捕捉ステップ４０５）を実行する。逃走経路ツリー構築モジュール１３０９は視野領域に含まれているデータからＥＰＴを構築する。最短ＥＰＴ計算モジュール１３０７はモジュール１３０９が生成したＥＰＴ内の目標物に対して最短の逃走経路を決定する。リスク割付けモジュール１３１３はモジュール１３０７が決めた最短逃走経路にリスクを割付けるステップ（例えば図４のステップ４０７）を実行する。リスク勾配計算モジュール１３２１はリスク勾配を計算するステップ（例えば図４のステップ４０９）を実行する。運動コマンド計算モジュール１３２３はモジュール１３２１が出力するリスク勾配計算結果を用いてロボット１３０１に運動コマンドを送る。

説明の明確化の目的で、追跡アルゴリズム全てが図１３のＣＰＵ１３０５の内部にあるものとして示されている。実施に際しては、１つのモジュール（場合によっては１つのモジュールの一部分）が、指示を実行に移すことが必要な瞬間にＣＰＵ１３０５内部にありさえすれば良い。同様に、追跡アルゴリズムはさまざまなソフトウェアモジュールからなるとして説明して来たが、追跡アルゴリズムは特定用途向け集積回路（ＡＳＩＣ）または適当なハードウェアの形で物理的なハードウェアとして実現することが可能である。従って、追跡アルゴリズムはさまざまなロボットプラットフォームに展開することができる。

ロボット１３０１は必要に応じてＣＰＵ１３０５にコピーされる追跡ソフトウェアの不揮発性コピーを収納するデータ格納庫１３１５も含む。データ格納庫１３１５は、追跡アルゴリズムのさまざまなモジュールによって生成されたデータとともに、ＥＰＴモジュール１３０９が生成したＥＰＴも収納できる。検出器１３１９は、視野領域を形成するためと、目標物捕捉モジュール１３０７が視野領域の目標物を識別するために使用する検出器の生データを視野領域取得モジュール１３０３へ送る。アクチュエータ１３１７は運動コマンド計算モジュール１３２３によって生成された運動コマンドを受信して、ロボット１３０１を目標物（例えば図３の目標物３０１）との検出器コンタクトを維持するように移動させる。

監視ロボット１３０１は検出器１３１９が取得するデータ量が多いにもかかわらず、かなりすばやい移動目標物を成功裏に捕捉できる。実施例の一例として示すと、ＥＰＴモジュール１３０９とリスク勾配計算モジュールの制御レートは約１０Ｈｚである。従って、ロボット１３０１のアクチュエータ１３１７はかなり速いベースで新規のコマンドを受信している。

追跡プロセスの改善は追跡アルゴリズムをより高速のプロセッサまたは専用の回路に埋め込むことによって実現が可能である。目標物の捕捉と障害物の識別のプロセスを補助する他の検出器（例えば他のカメラ）の追加とともに、検出器１３１９の速さを改善することによっても追跡プロセスの向上が図られる。

追跡問題はロボットの自己位置同定問題と相互に関連していることが多い。このことは、追跡アルゴリズムが監視ロボットの行動を計算するために事前に準備された環境の地図を用いる場合に起きる。自己位置同定は一般に目印を用いる。従来技術のシステムのあるものでは、目印はワークスペース全体に分散した人工的な天井目印からなる。目印が見えれば監視ロボットは良い精度で自己の位置を同定する。そうでない場合は、監視ロボットは推測航法で操縦され、監視者の位置の不確定性は次の目印の視認まで増加する。

ある先行技術は自己位置同定のためにロボットが目印を見ることを明示的には要求していない。またそれに関連したアルゴリズムは、最良の追跡運動を実行すべきか、又は目印を発見してより良い自己位置同定を実現するために最良の追跡運動から外れることが好ましいのかをそれぞれの段階で決定する。当然のことながら、監視ロボットが事前に用意される地図を必要とせず、検出器の入力を使用する局所地図計算機に基づいて全ての決定をする場合は、自己位置同定問題は潜在的に解決され、他の自己位置同定機構を必要としない。

追跡アルゴリズムはここまで単一平面に対する検出器データに基づいた動作として説明して来た。追加の平面に対するデータが与えられれば、追跡アルゴリズムは類似の仕方で動作する。例えば、この追加の情報は目標物と監視ロボットが通過できない障害物をより正確に識別するために使用される。さらに、追跡アルゴリズムは、穴、階段および目標物が上り下りに利用できる柱のような物を障害物としてモデル化することができる。

数式を用いた詳細な説明
次のセクションでは、図３−図１３に示した追跡アルゴリズムについてさらに詳しく数式を用いて説明する。

監視ロボット３０３と目標物３０１は剛体と仮定され、それぞれの自由配置空間（自由コンフィギュレーション空間）はＣ^oとＣ^tで表される。χが問題の状態空間で、監視ロボット３０３と目標物３０１の両者のそれぞれの状態空間の直積演算であるとする。直積演算Ｃ^o×Ｃ^tはダイナミクスが無いときはχに等しい。一般にＣ^o×Ｃ^tは状態空間のサブスペースである。

関数ｆ^oは監視者のダイナミクスをモデル化し、非ホロノミック制約又は他の制約の符号化が可能である。例えば、関数ｆ^oは図３の３０３ａから３０３ｂへの監視ロボットの移動を表す。

しかしながら、前述のようにここで説明する追跡アルゴリズムは目標物の動きに関して事前の知識を必要とはしない。

ワークスペース内の視野領域
監視者のコンフィギュレーションは検出器（例えば図５の検出器５０３）の視野を決める。

セットＶ（ｑ^o）は監視者の位置ｑ^oにおける視野領域であり、いくつかの仕方で定義が可能である。例えば、監視者は３６０度の視野を持ち、目標物はＷ内の１つの点である。この場合、監視者の視線が妨害されない場合のみ目標物は視認されると言える。他の例では、固定の円錐に限定されるか、検出器レンジの下限及び／又は上限によって制限される。例えば、図７に示す視野領域は目標物３０１を含む半円の陰をつけた部分からなる。追跡アルゴリズムは人為的モデルまたは検出器の測定結果から視野領域を計算できる。前者の場合、光線掃引アルゴリズムが従来の多角形モデルに対して視野領域を計算するために使用することができる。後者の場合、視野領域は従来の技術を用いレーザーレンジファインダーで計測可能である。

追跡計画

さらに、追跡計画は、監視ロボット３０３が通過するトータル距離、監視ロボット３０３から目標物３０１までの距離、あるいは視覚情報の品質の程度のような他の判断基準を最適化しても良い。いずれかの実施例においては、目標物３０１の追跡喪失を避けられない。その場合には、最適追跡計画は監視ロボット３０３が初めて目標物３０１を見失う時刻、「逃走時刻」又は「逃走予定時刻」（ｔ_esc）の最大化を含む。例えば、図２に示すように、逃走時刻は目標物１０１が障害物１１１に沿って曲がる瞬間である。

このような場合、最適追跡計画は監視ロボット３０３が目標物３０１を追跡する前にオフラインで計算できる。目標物３０１の位置が全てのｔに対して知られているので、監視ロボット３０３は目標物３０１を見失ったときに目標物３０１を再捕捉できる。

例えば、図２に示すように監視ロボット１０３の目標物１０１に対する露出はほぼ監視ロボットの位置１０３ａ−１０３ｃに対応している。

もし追跡計画ｕ^*（ｔ）が状態ｘ（ｔ）の関数として計算されれば、計画は閉ループで動作する。そうでなければ、追跡計画は開ループで動作する。目標物の運動モデルと位置測定が正確でなければ、予測可能な場合も含めて閉ループの追跡計画が開ループのものより一般に好まれる。

目標物の行動が知られていない場合、目標物３０１は「予測不可能」と呼ばれ、問題の解法はもっと複雑化する。図２及び３は予測不可能な目標物の移動を示す。予測不可能な場合は２つの方法で解析できる。

このように行動セットは既知であるが任意の時刻における目標物３０１が選択する具体的な行動は分からない。この場合、θ（ｔ）に対する最悪ケースの選択が与えられれば、追跡計画は最善策を実行するように設計できる。代替案として、確率的不確定性モデルが利用可能であれば（例えば確率密度関数ｐ（θ（ｔ）が既知）、予測の意味合いで最適の運動計画を計算できる。

光線掃引アルゴリズムを用いた逃走経路の計算
Ｖ（ｑ^o）は目標物（例えば図３の目標物３０１）が監視ロボット（例えば図３の監視ロボット３０３）によって視認される全ての位置のセットであることを思い起こそう。Ｖ（ｑ^o）は反時計回りに整理された頂点のリストによって表されると仮定する。リストの内容は、Ｌ_lとＬ_rに分けられ、Ｌ_lはｌ（ｑ^o、ｑ^t）の左側のＶ（ｑ^o）の全ての頂点のリストで、Ｌ_rはｌ（ｑ^o、ｑ^t）の右側の全ての頂点のリストである。Ｌ_rの順番はその内容を時計回りに整理されるように逆にすることができる。従って、光線掃引アルゴリズムは、Ｌ_lの連続掃引に引き続き、Ｌ_rの類似の掃引を実行することにより、ｑ^tからＶ（ｑ^o）内の全ての頂点までの最短経路を計算することからなる。

ここでＬ_lに対する代表的な掃引について述べる。

アルゴリズムによる更新は次のように行われる。

ピボットリスト更新：
（π_i）のサイズ＜３またはステップ２が不成功に終わるまで以下を繰り返す。
１．ｕ_r+1＝ｖ_iで、ｕ_r-1、ｕ_rおよびｕ_r+1をπ_iの最後の３エレメントとする
２．ｕ_r+1が直線（ｕ_r-1、ｕ_r）の右側にあればπ_iからｕ_rを取り除く

光線掃引アルゴリズムではひとつの頂点が一旦逃走経路でなくなると再び逃走経路にはならないという事実を利用している。これは以下で説明する定理１から派生する結果である。

最短逃走経路ＳＥＰ（ｑ^t、ｅ）は現在取り上げている頂点ｖ_iでピボットリストπ_iから計算される。ｖ_iに対して互いに排他的な３ケースがあり、それぞれのケースでアルゴリズムは異なる実行をする。
１．ｖ_iが自由端になければπ_iは逃走経路ではない。
２．ｖ_iが自由端の終点でセグメント（ｖ_i-1、ｖ_i）が障害物の端部であれば、π_iは新しい逃走経路ＳＥＰ（ｑ^t、ｅ）である。
３．ｖ_iが自由端の終点であるが、セグメント（ｖ_i-1、ｖ_i）が自由空間内にあれば、逃走点をｖ_iの前の自由端に沿ってずらすことにより、新規に発見された逃走経路を短縮できる可能性がある。この計算は一定時間内で簡単に実行できる。

実行時間解析
本発明の実施例によれば、効率的な動作を実現するためにＬ_lとＬ_r内の個々の頂点は厳密に一回だけピボットリストに加えられる。同様に、取り除かれた頂点はいずれもリストに再度入れられることは無い。このようにして、Ｖ（ｑ^o）を表す入力リストが事前に分類されると、光線掃引アルゴリズムの計算費用はＬ_lとＬ_r内に保存された頂点の数に比例する。逃走経路を全て計算するための費用はＯ（ｎ）である。ツリー内の個々のノードはＶ（ｑ^o）内の頂点であるので、この費用は逃走経路ツリーの計算に必要な費用でもある。

逃走経路ツリー
視線視野モデルでは多角形のワークスペース内の領域Ｖ（ｑ^o）も多角形である（実際には、多角形は星型の多角形である）。この視野多角形は線形複雑度を有し、その境界は固定および自由端で構成される。固定端はワークスペースの監視されたセクション（例えば物理的障害物の部分）を表す。

目標物３０１が監視ロボット３０３に視認される場合を想定する。

ｑ^tに位置する目標物３０１に対する「逃走経路」はｑ^tとＶ（ｑ^o）の外部のＷにある点を結ぶ任意の衝突のない経路である。例えば目標物３０１に対する逃走経路は、障害物１１１または障害物１１１によって作られるオクルージョンを、目標物３０１自身と監視ロボット３０３との間に配置する。逃走経路の「逃走点」は逃走経路がＶ（ｑ^o）の境界を横切る点であり、常に自由端に沿って発生する。当然、目標物３０１と監視ロボット３０３のある特定のコンフィギュレーションに対して無限数の逃走経路が存在する。しかしながら、ある特定の逃走点に対して、逃走点と目標物間の距離が最小となる経路が存在する。さらに、任意の自由端に対して、この自由端に沿う全ての逃走点の中で距離が最小となる逃走経路が存在する。そのような経路ＳＥＰ（ｑ^t、ｅ）は自由端ｅ経由の目標物の最短逃走経路と呼ばれる。ＳＥＰ（ｑ^t、ｅ）の長さはｅ（ＳＤＥ（ｑ^t、ｅ））を経由して逃れる最短距離である。

目標物３０１が逃走経路を通過する最短時間がその経路に対する「逃走時刻」である。任意の自由端ｅおよび目標物の位置ｑ^tに対して逃走時刻が最小となる経路が存在し、一般にはＳＥＰ（ｑ^t、ｅ）に一致しない。これらの経路は目標物がホロノーム系で慣性が無視できる場合に限って一致する。
記号ｔ_esc（ｑ^t、ｑ^o）はｑ^tを起点としてＶ（ｑ^o）から出る全ての逃走経路を移動する場合の最小時間を示す。

このようにＶ（ｑ^o）がｎ_fの自由端によって境を限られればｎ_fの最短逃走経路が存在する。Ｖ（ｑ^o）の境を限定するｅ全体にわたる最短のＳＥＰ（ｑ^t，ｅ）がコンフィギュレーション（ｑ^t、ｑ^o）に対する最短逃走経路ＳＥＰ（ｑ^t、ｑ^o）になる。その長さはＳＤＥ（ｑ^t、ｑ^o）である。目標物がホロノーム系で慣性が無視できれば、ＳＤＥ（ｑ^t、ｑ^o）はｔ_esc（ｑ^t、ｑ^o）に目標物の最大スピードを乗じたものと等しくなる。

逃走経路の特性

目標物が点と仮定すると、多角形のワークスペースに対して経路ＳＥＰ（ｑ^t、ｅ）は次の基本的な特性を満たす。

特性１：ＳＥＰ（ｑ^t、ｅ）はｑ^tとＶ（ｑ^o）を限定する自由端ｅの点を結ぶ多角形の直線である。この多角形の直線の頂点がある場合は、個々の頂点はＶ（ｑ^o）の頂点になる。
特性２：経路ＳＥＰ（ｑ^t、ｅ）は放射状の直線ｌ（ｑ^t、ｑ^o）を厳密には横切ることはできない。この経路は全てがｌ（ｑ^t、ｑ^o）の一方の側（右又は左）にあるか、ｌ（ｑ^t、ｑ^o）に含まれるかのいずれかである。

Ｌ_lをｌ（ｑ^t、ｑ^o）の左側にあり反時計回りに分類されるＶ（ｑ^o）の頂点のリストとする。同様に、Ｌ_rをｌ（ｑ^t、ｑ^o）の右側にあり時計回りに分類されるＶ（ｑ^o）の頂点のリストとする。ｌ（ｑ^t、ｑ^o）がＶ（ｑ^o）の頂点を通過すれば、この頂点はＬ_lとＬ_rの両者に含まれるとする。

次の定理が上述の光線掃引アルゴリズムの基礎となっている。
定理１：ｑ^tからＬ_l（又はＬ_r）に含まれる障害物の頂点ｖまでの最短経路が、Ｌ_l（又はＬ_r）に含まれる前の頂点ｕを通過しない場合は、ｑ^tからＬ_l（又はＬ_r）に含まれる頂点ｖの後に現れる任意の頂点ｗまでの最短経路が頂点ｕを通過しない。
証明：ｑ^tからｗまでの最短経路がｕを通過すれば、ｑ^tからｖまでの最短経路はｑ^t以外の点でｑ^tからｗまでの最短経路を通過することになる。従って、２つの経路の内の１つがより短くなる。

定理１はＳＥＰ（ｑ^t、ｅ）が上述のような光線掃引アルゴリズムを用いて漸増的に構築できることを示唆している。Ｌ_l（又はＬ_r）の掃引時に、ｑ^tから直前に処理した障害物の頂点までの最短経路を記憶しておくだけで充分である。

逃走経路ツリー
逃走経路ツリーを見ると多くの経路が同一の初期構造を共有していることが分かる。この特性は、全ての経路ＳＥＰ（ｑ^t、ｅ）に対する平均距離を最小化する計画にとって基本的な問題を提示している。同じ枝に沿う逃走経路は孤立の逃走経路を犠牲にした全体平均を取ることにより過剰に表される。椅子、テーブルの脚、類似の小さな障害物が同一の枝に沿う多くの逃走経路を生み出す実際の適用時に、この問題が起きる。この問題は、逃走経路ツリーの子供から親ノードへ逆行させて再帰平均を計算することにより軽減される。同じノードから分岐する子供はまず子供同士の平均が計算され、その結果が前のノードへ逆伝播される。

逃走リスクの最小化による追跡
目標物追跡問題の解法は小型のロボットでは特に計算費用が高額になる。充分高速のアルゴリズムを提供するために、小さな時間領域Δｔを対象として計画する方法が実際上有効であることがある。従って、適切なアルゴリズムを実現するためには、小さな時間刻みΔｔの段階毎の問題に離散化することがその一方法として含まれる。与えられた段階ｋに対してアルゴリズムは次の方程式を解いて監視ロボットに対する制御アクションｕ_kを見つける。

ここで目標物は次段階の計算まで、例えば次の視野領域の取得まで同じ位置に留まると仮定する。上記の方程式の解法のかぎはｔ_escの計算である。これは計算機を用いた計算としては費用のかかる計算である。

キノダイナミックな制約条件下の任意の目標物に対しては、ｔ_escはＳＤＥに比例する因子によって上限が限定され、計算が格段に容易なる。次の方程式が上記の方程式の近似解を与える。

ここではＳＤＥを逃走時刻の代理関数として使用している。実際には、この方法は一般にシミュレーション実験とダイナミクスの無いホロノミック系の監視者を除いた全ての場合に不十分な結果をもたらす。上記の方程式には２つの問題がある。１つはＳＤＥ関数に特有な問題である。監視ロボットが動くと、新しいオクルージョンが形成され古いオクルージョンは消滅し、新しい逃走経路が最短逃走経路になる。

モデル化されていない監視ロボットのダイナミクスはシャタリング信号によって励起するので、監視ロボットに誤った予測不可能な運動を与える。第２の問題はＳＤＥがｔ_escの適切な代理関数ではないことである。ＳＤＥとｔ_escの関係は線形ではない。実際、大きなＳＤＥは目標物が逃走するのを格段に難しくする。これを理解するためには、ＳＤＥがだんだん大きくなる状況を想像してみれば良い。目標物が逃走するために長い距離を移動しなければならないばかりでなく、監視ロボットが今後の目標物の追跡の能力を改善する時間があることにもよって、追跡がより簡単になる。ｔ_escに対して次の代理関数を用いれば、第２の方程式に付随する両方の問題は解決することができる。この関数が「逃走リスク」で、すべての自由端ｅに次のように定義される。

ここでｈ＝ＳＤＥ（ｅ、ｑ^t）は目標物と端部ｅとの間の最短距離を、ｒ_oは監視ロボット（ｑ^o）からｅでオクルージョンを起こすコーナー迄の距離を、ｄは目標物（ｑ^t）からコーナーまでの距離を、ｃは正の定数を、ｆ（１／ｈ）は１／ｈの単調増加関数を表す。項ｃｒ_o ²はこの最小化によって監視ロボットが追跡する今後の能力が増加するので予測コンポーネントである。項ｆ（１／ｈ）はこの最小化によって目標物が現在の視野領域から逃走する可能性を減少させるので反応コンポーネントである。ｌｏｇ（１／ｈ²＋１）及び（１／ｈ）^m+2（ｍは負でない整数）がｆ（１／ｈ）の例である。

追跡アルゴリズムの適用
本発明の実施例を用いた移動検出器を持つロボットを適用することにより多くの恩恵を被る。本発明を利用したロボットは障害物が散乱する環境内を移動する予測不可能な目標物を自律的に監視できる。例えば、自立的監視者（ＡＯ）として知られているカメラを搭載されたロボットは地域に分布したチームがロボットのソフトウェアをデバッグするのを補助できる。ＡＯは独立のタスクを実行しながら第２のロボット（目標物）を連続的に追跡する。ＡＯによって取得された情報は、目標物とその環境の３次元グラフィック化によってプログラマーが目標物のソフトウェアのバグを発見し修正することが可能なリモートワークステーションへ送られる。

障害物のある環境内の目標物追跡技術は、デジタルアクターのグラフィックアニメーションで、アクターが環境内を移動するにつれて映し出される連続的な視点（仮想のカメラ位置）を選ぶことに用いられる。手術に用いられる制御可能なカメラは、予測不可能な人または器具による潜在的な妨害となる動きがあるにもかかわらず、患者の臓器または組織を連続的に観察できる。

空港の環境では、旅行者が荷物の搬送に利用できる移動ロボットは空港内を移動する際、自律的に旅行者の動きに従うことができる。軍事利用の分野では、障害物中の目標物の追跡にさまざまな形で利用できる。これらの適用例と従来の追跡例（例えばミサイル制御と純視覚的追跡）の差異のかぎは、検出器の視野を遮蔽し検出器と目標物の運動を妨害する障害物を導入した点にある。検出器は好ましくないオクルージョンの発生を防止するために移動する必要がある。

追跡アルゴリズムは他のロボットに対して新しい環境の地図を作成するためにも使用できる。例えば、追跡アルゴリズムは新しい環境の調査に当てられたロボットに使用できる。簡単な事務所の例では、追跡アルゴリズムを持つ監視ロボットがフロア全体に渡って目標物を追跡し、目標物を追跡しながらフロアの地図を作成するようにできる。この地図はフロアでさまざまなタスクを実行するために準備された監視業務を行うロボットに与えられる。監視ロボットは複数の検出器を具備することも可能である。例えば、ロボットの航法のための視野領域を与える低分解能の検出器と、監視ロボットがフロアを通過する際にフロア計画の詳細地図を作成するための高分解能の検出器が上げられる。詳細地図は別のアルゴリズムで制御される後継ロボットに与えることが可能である。

追跡アルゴリズムは本田のアシモロボットのような擬人ロボットを含むあらゆるタイプの移動ロボットに利用することが可能である。さらに、本発明の実施例によれば、追跡コマンドが監視ロボットに与えられれば（例えば送信されれば）、視野領域を最終的に与える検出器も、追跡アルゴリズムそのものも監視ロボット上に必ずしもある必要は無い。

上記の詳細説明の観点から、以上の変更を含めて本発明に変更を加えることができる。一般に、請求項において、使用されている用語が本発明を明細書に開示されている具体的な実施例に限定すると解釈されてはならず、請求項の下で動作する全てのシステムと方法とを含むと解釈されるべきである。

図１はワークスペース内で移動目標物を追跡する移動監視ロボットを示す。 図２は監視ロボット用の追跡アルゴリズムが目標物を追跡するために視覚サーボしか使わない場合に起きる問題を示す。 図３は本発明の一実施例による、監視ロボットが障害物の回りを移動する時に、移動目標物を追跡する追跡アルゴリズムを適用している視覚ロボットの動作を説明する。 図４は本発明の一実施例による追跡アルゴリズムが実行するステップを示すフローチャートである。 図５は本発明の一実施例による、検出器によって追跡アルゴリズムに送られ、視野領域の決定と目標物の位置決定に使用される検出器データを示す。 図６は本発明の一実施例による、追跡アルゴリズムによって識別された障害物を含む視野領域を示す。 図７は本発明の一実施例による、視野領域の逃走経路ツリー（ＥＰＴ）への遷移における第１のステップを示す。 図８は本発明の一実施例による、視野領域に対して設定された逃走経路ツリーを示す。 図９は本発明の一実施例による、追跡アルゴリズムのリスク基準の計画に対する初期の考慮事項を示す。 図１０は本発明の一実施例による、障害物によって作られたオクルージョンに向かってさらに移動する目標物と反応コンポーネントの役割を示す。 図１１は本発明の一実施例による反応コンポーネントと予測コンポーネントとの組合せ、および監視ロボットに対する軌跡を示す。 図１２は、目標物が監視ロボットと同様に障害物と同じ側にある例を示すことによって、予測コンポーネントの役割を説明する。 図１３は本発明の一実施例による、追跡アルゴリズムを実行するように構成された監視ロボットのブロック図である。

Claims (28)

1. ワークスペース内の複数の障害物の間を移動する目標物を追跡する方法であって、
   前記複数の障害物から障害物の位置を識別し、検出器から受信したデータを用いて前記目標物の位置を識別する視野領域を作成するステップと、
   前記視野領域を用いて複数の逃走経路を前記目標物に対して計算するステップと、ここでそれぞれの逃走経路は前記検出器による前記目標物の検出を遮蔽する経路を表す、
   前記複数の逃走経路の中で最短長さの経路を表すように１つの逃走経路セットを前記複数の逃走経路の中から識別するステップと、
   前記逃走経路セットに対して逃走リスクと逃走リスク勾配とを計算するステップとからなる方法。
2. 目標物を追尾する監視ロボットに対して前記逃走リスク勾配を用いて運動コマンドを作成するステップをさらに含む請求項１の方法。
3. 前記運動コマンドを作成するステップは前記逃走リスク勾配の再帰平均を計算するステップを更に含む請求項２の方法。
4. 複数の逃走経路を計算するステップが、
   オクルージョン領域は前記検出器が目標物を検知できない領域からなり、前記複数の障害物の妨害によって発生するオクルージョン領域を識別するステップをさらに含む請求項１の方法。
5. 前記視野領域を用いて複数の逃走経路を前記目標物に対して計算するステップが、
   前記逃走経路を識別するために計算機で実行する光線掃引アルゴリズムの適用をさらに含むことを特徴とする請求項１の方法。
6. ワークスペース内の目標物を追跡する方法であって、
   前記目標物と少なくとも１つの障害物を識別する視野領域を取得するために前記ワークスペースを検出するステップと、
   前記視野領域を用いて前記目標物の軌跡が監視ロボットによる検出から逃れる逃走リスク勾配を計算するステップと、ここで前記監視ロボットからの逃走は視野領域外の移動と少なくとも１つの障害物によってできるオクルージョン内への移動を含む、
   前記逃走リスク勾配を用いて、前記監視ロボットによる検出から逃れる前記目標物の能力を低減させる操舵コマンドを監視ロボットに対して作成するステップとからなる方法。
7. 逃走リスクの計算ステップは、
   複数の逃走経路に対して前記目標物の軌跡を補正する反応項を計算することと、前記監視ロボットと前記少なくとも１つの障害物間の距離を補正する予測項を計算することとによって、前記逃走リスクを最小化するステップを更に含むことを特徴とする請求項６の方法。
8. 前記視野領域のそれぞれの自由端に対する前記逃走リスク計算が、下記の式の実行によってなされることを特徴とする請求項６の方法。
   

   

   ここで、ｃは正の定数、ｈは前記目標物と、前記監視ロボット／ターゲット間のオクルージョンとの間の最短距離、ｒ_oは前記監視ロボットからオクルージョンを引起す障害物までの距離、ｆ（１／ｈ）は１／ｈの単調増加関数とする。
9. 前記視野領域に対して複数の逃走リスクの計算と、
   前記複数の逃走リスクに対して逃走リスク勾配の計算とをさらに含む請求項６の方法。
10. 前記逃走リスクを用いて前記監視ロボットに対して前記操舵コマンドを作成するステップは前記リスク勾配の再帰平均を計算するステップからなることを特徴とする請求項９の方法。
11. ワークスペース内を移動する目標物を追跡する方法であって、
    前記目標物をヘッドノードとして持つ逃走経路ツリーを作成するステップと、
    前記目標物に対して複数の逃走経路を識別するステップと、ここでそれぞれの逃走経路は前記目標物を少なくとも１つの検出器から遮蔽する前記ワークスペースを通る経路を表す、
    前記複数の逃走経路のそれぞれの逃走経路を前記逃走経路ツリーにおいてチャイルドノードとして設定し、長さの短い逃走経路が長さの長い逃走経路よりも前記逃走経路ツリーにおいてより高い位置にあるように前記逃走経路ツリー内のそれぞれの逃走経路を整理するステップと、
    前記逃走経路ツリーから、より長さの短い逃走経路からなる１セットを選択するステップと、
    前記逃走経路のセットに対して逃走リスクを計算するステップとからなる方法。
12. 前記目標物に対して前記複数の逃走経路を識別するステップは、前記逃走経路を識別するために計算機が実行する光線掃引アルゴリズムの適用をさらに含むことを特徴とする請求項１１の方法。
13. 前記視野領域の全ての自由端ｅに対する逃走リスクの計算が、下記の式の実行によってなされることを特徴とする請求項１１の方法。
    

    

    ここで、ｃは正の定数、ｈは前記目標物と前記監視ロボット／ターゲット間のオクルージョンとの間の最短距離、ｒ_oは前記監視ロボットからオクルージョンを引起す障害物までの距離、ｆ（１／ｈ）は１／ｈの単調増加関数である。
14. 複数の逃走リスクの計算と、
    前記複数の逃走リスクに対して逃走リスク勾配の計算とをさらに含む請求項１１の方法。
15. 前記目標物を追跡する監視ロボットに対して前記逃走リスク勾配を用いて運動コマンドを作成するステップをさらに含む請求項１４の方法。
16. ワークスペース内の複数の障害物の間を移動する目標物を追跡するシステムであって、
    検出器から受信するデータを用いて複数の障害物から障害物の位置を識別する視野領域を作成するように構成された視野取得モジュールと、
    前記視野領域内の前記目標物の位置決定をするように構成された目標物捕捉モジュールと、
    前記視野領域を用いて前記目標物に対して複数の逃走経路を識別するように構成された逃走経路ツリー構築モジュールと、ここでそれぞれの逃走経路は前記目標物を前記検出器の検知から遮蔽する経路を表す、
    前記複数の逃走経路の中で逃走経路セットが最短長さの経路となるように前記複数の逃走経路から１つの逃走経路セットを識別するように構成された最短逃走経路ツリー計算モジュールと、
    前記逃走経路セットに対して逃走リスク勾配を計算するように構成されたリスク勾配計算モジュールとからなるシステム。
17. 前記逃走リスク勾配を用いて前記監視ロボットに対して、前記目標物が前記検出器の検知から逃れる能力を低減する操舵コマンドを作成するように構成された運動コマンド計算モジュールをさらに含む請求項１６のシステム。
18. 前記リスク勾配計算モジュールは、前記視野領域における全ての端部ｅに関連させたリスク関数上で動作を実行することにより、前記逃走経路セットに対して前記逃走リスク勾配を計算するように構成され、前記動作は、下記の式によって与えられることを特徴とする請求項１６のシステム。
    

    

    ここで、ｆ'（１／ｈ）はｆ（１／ｈ）の導関数、ｈの勾配▽ｈは端部ｅに関連した逃走経路ＳＥＰ（ｅ、ｑ^t）の幾何学に従って計算され、ｃは正の定数、ｈは前記目標物とターゲットを追跡する監視ロボット／ターゲット間のオクルージョンとの間の最短距離、ｒ_oは前記監視ロボットからオクルージョンを引起す障害物までの距離である。
19. 前記逃走経路構築モジュールが、計算機が実行する光線掃引アルゴリズムを実行することによって、前記目標物に対して複数の逃走経路を識別することを特徴とする請求項１６のシステム。
20. ワークスペース内の目標物を追跡するシステムであって、
    前記ワークスペースを記述するデータを取得するように構成された検出器と、
    前記検出器から受信したデータを使用して、前記ワークスペース内の複数の障害物を識別する視野領域を生成するように構成された視野領域取得モジュールと、
    前記ワークスペース内の前記目標物を識別するように構成された目標物捕捉モジュールと、
    前記視野領域を使用して、前記目標物が監視ロボットの検出から逃れる逃走リスクを決定するように構成されたリスク割付けモジュールと、ここで前記監視ロボットからの逃走には、視野領域外での目標物の移動と前記複数の障害物の少なくとも１つの障害物が引き起こすオクルージョン内への目標物の移動が含まれ、
    前記逃走リスクを用いて、前記目標物の前記監視ロボットによる検出から逃走する能力を低減する操舵コマンドを、前記監視ロボットに対して作成するように構成された運動コマンド計算モジュールとからなるシステム。
21. 前記リスク割付けモジュールによって決定された前記逃走リスクを用いてリスク勾配を計算し、前記操舵コマンド作成のために前記逃走リスク勾配を前記運動コマンド計算モジュールに与えるように構成されたリスク勾配計算モジュールをさらに含む請求項２０のシステム。
22. 前記リスク勾配計算モジュールが、前記視野領域における全ての端部ｅに割付けたリスク関数上で動作を実行することにより前記逃走リスク勾配を計算するように構成され、前記動作は、下記の式によって与えられることを特徴とする請求項２１のシステム。
    

    

    ここで、ｆ'（１／ｈ）はｆ（１／ｈ）の導関数、ｈの勾配▽ｈは端部ｅに関連した逃走経路ＳＥＰ（ｅ、ｑ^t）の幾何学に従って計算され、ｃは正の定数、ｈは前記目標物と前記ターゲットを追跡する監視ロボット／ターゲット間のオクルージョンとの間の最短距離、ｒ_oは前記監視ロボットからオクルージョンを引起す障害物までの距離である。
23. 前記監視ロボットが検出器に取付けられたことを特徴とする請求項２０のシステム。
24. 前記監視ロボットが擬人的ロボットであって、前記運動コマンド計算モジュールが擬人的アクチュエータによる実行のために前記操舵コマンドを作成することを特徴とする請求項２０のシステム。
25. 前記目標物は哺乳動物で、前記検出器は前記ワークスペースを記述する熱関連のデータを取得するように構成されたことを特徴とする請求項２０のシステム。
26. ワークスペース内を移動する目標物を追跡するシステムであって、
    前記目標物をヘッドノードとする逃走経路ツリーを作成するように構成され、前記目標物に対して複数の逃走経路を識別するように構成され、ここでそれぞれの逃走経路は前記目標物を少なくとも１つの検出器から遮蔽する前記ワークスペースを通る経路を表す、前記複数の逃走経路のそれぞれの逃走経路を前記逃走経路ツリーにおいてチャイルドノードとして配置するように構成され、前記逃走経路ツリー内のそれぞれの逃走経路に対して、長さの短い逃走経路が長さの長い逃走経路よりも前記逃走経路ツリーの上位を占めるような整理をするように構成された逃走経路ツリー構築モジュールと、
    前記逃走経路ツリーからより長さの短い逃走経路のセットを選択するように構成された最短逃走経路ツリー計算モジュールと、
    前記逃走経路セットを用いて前記目標物に対して逃走リスクを計算するように構成されたリスク割付けモジュールとからなるシステム。
27. 前記逃走経路ツリーを保持するように構成されたデータ格納庫をさらに含む請求項２６のシステム。
28. ワークスペース内を移動する目標物を追跡するように構成された計算システムの中の計算機が判読できる媒体であって、
    前記目標物が検出器による検出を逃れるために使用可能な逃走経路を表すように構成された逃走経路ツリー構造と、
    前記目標物に対する位置データと、
    前記目標物を追跡する監視ロボットに対する位置データとからなる媒体。
    

Images