JP2020040205A - マニピュレータの衝突回避のための動作計画方法および装置 - Google Patents

Abstract

【課題】マニピュレータの出発点から目的点まで衝突を回避する動作計画において、局所的な最適化に基づく従来の動作計画が極小値で動けなくなるという不利な点を回避し、変化する環境においてロボットのために最適化された動作計画を提供する。【解決手段】広域的な計画器(34)を用いて、目的点までのマニピュレータの目標軌道を繰り返し決定するステップと、広域的な計画器(34)の現在の目標軌道に基づいて、局所的な計画器(36)用いて、マニピュレータの動作を連続的に決定するステップと、前記広域的な計画器(34)および前記局所的な計画器(36)による前記決定と並行して、マニピュレータによる動作を実行するステップとを含む。【選択図】図２

Description

本発明は、出発点から目的点までのマニピュレータの衝突回避のための動作計画方法に関する。また本発明は、少なくとも１つのマニピュレータおよび前記方法を実行する制御ユニットを備えた装置に関する。

ロボット工学において、衝突回避のための動作計画は、動作している間、障壁もしくはそれ自体との衝突を避けながら、出発点から終点までロボットまたはマニピュレータの動作軌跡を発見する作業である。衝突回避のための動作計画を作成する方法は、例えば、特許文献１により公知である。

特許文献１は、生産プラント等において使われる、単一のロボットアーム（マニピュレータ）を有する産業用ロボットの衝突回避のための動作計画を開示する。産業用ロボットとは、作業、組立または工作物の処理のために働く機械である。

通常、産業用ロボットは、いくつかの動作軸を有する少なくとも１つのマニピュレータと、ロボットのドライブを制御することによって動作中のロボットの動作シーケンスを調整するプログラマブルコントローラとを備える。

人間の近くでロボットを使用することは、最小の遅延で衝突回避の動作を算出することができる動作計画方法を要求する。特に、動作計画方法は、可動障壁を考慮した常に変動する環境に適応することができなければならない。後者は、ロボットが動作する間、計画への適応または再計画を必要とする。

ロボットの行動を十分にもしくはまったく考慮することができない場合、動作計画の連続的な再作成は、公知の広域的な軌道計画方法（例えば、動的ロードマップ（ＤＲＭ））によっては、困難である。例えば、周知の広域的な計画器は、通常初めにロボットの動作を考慮することなく軌道計画を実行して、次の期間、ロボットの動作のみを考慮して、見つかった軌道の時間パラメーター化を行う。

この時間パラメータ化された軌道の実行は、動く障壁との衝突の可能性を避けるために、監視される。衝突の可能性があれば、ロボットは安全に停止される。安全の見地から、ロボットが、新しい条件に適合した軌道を続けることができるように、新しい再計画が実行される。環境にある動く障壁は、頻繁な停止につながり、ロボットの動作を遅くし、不自然にする。

より効果的にロボットの動きを考慮する一つの方向は、「RRTx」(Real-time Motion Planning/Re-planning for Environments with unpredictable obstacles; 予測不可能な障壁のある環境におけるリアルタイムな動作計画）と称されるkinodynamicな動作計画である（Michael Otte）。

しかしながら、kinodynamicな動作計画は、見込みに基づく方法であり、それが決定的な軌道を生成せず、さらに、この種の方法のために必要な計算時間が制限され、しかし通常は予測可能でないという不利な点がある。DRMに基づいた方法のような決定論的な方法は、しかしkinodynamicな動作計画に適していない。

加えて、人工の場(artificial potential fields)のような局所反応方法(local reactive methods)が、環境を変えるための代替方法として公知である。

これらは、完全な軌道に代えて、ロボットの行動のための制御パルスを発生させる。制御パルスは、障壁から、好ましくは所定の目的地の方向にロボットを「押しのける」。

これらの方法は、計画と制御の区別なく、障壁に直ちに反応する。これは特に、動く障壁に、急速に反応することを可能にする。

独国特許出願公開10 2010 007 458 A1号明細書

しかしながら、問題点は、局所的な最適化に基づくこれらの方法が、局所における極小値で動けなくなることとなり、したがって、複雑な環境にある高次元のロボットの用途が限られてしまうということである。

したがって、本発明の目的は、上記の不利な点を回避し、変化する環境においてロボットのための最適化された動作計画を提供することである。

特に本発明の目的は、ロボットのダイナミックスを考慮した、停止または減速のない連続的な再計画を許容する動作計画を特定することである。

本発明の一態様によれば、本発明の目的は、第１のマニピュレータの出発点から目的点まで衝突回避のための動作計画方法により達成される。該方法は、広域的な計画器によって、第１のマニピュレータの目的点への目標軌道を繰り返し決定するステップと、局所的な計画器を用いて広域的な計画器の現在の目標軌道に基づいて、第１のマニピュレータの動作を連続的に決定するステップと、前記広域的な計画器および前記局所的な計画器によって、前記判定と並列に第１のマニピュレータによって、動作を実行するステップとを含む。

このように、広域的な計画器を、ロボットの動作計画のための連続的な局所計画方法と繰り返し結合することが、本発明の着想である。

「連続的な」とは、再計画が連続的に、好ましくは所定の間隔で起こることを言う。一方「繰り返し」とは、広域的な計画器が少なくとも一回、再計画をしなければならないことを言う。

目標軌道の反復決定は、最初、動機なしに開始される。すなわち現在の目標軌道がまだ効力を有するかまだ実行できるかどうかにかかわらず、目標軌道の新たな決定が行われる。

公知の方法とは対照的に、環境が局所的な計画器のために明らかに変化しなかった場合でも、再計画は意識的に実行される。このように、広域的な計画器による再計画は、局所的な計画器によって「起動される」必要はない。このようにして、例えば、障壁が前に計画された目標軌道から消滅する場合、広域的な計画器は、より短い目標軌道を見つけることができる。

したがって、本発明によれば、２つの異なる（局所的なおよび広域的な）計画器が並行して実行され、互いを補足し、特に、それぞれの計画において互いを考慮する。この明細書における「並行して」とは、時間的に並行して行われることを意味する。すなわち各計画器が、例えば各スレッドで、並行して処理される。

広域的な計画器は、広域的に計画された軌道をつくり、局所的な計画器（それは速い制御動作を生成できる）は、広域的に計画された軌道を考慮しながら障壁との衝突を回避する。

ロボットまたはマニピュレータは、このように広域的に計画された軌道に沿って導かれ、同時に障壁から「押しのけられる」。両計画器（広域および局所）は、ループで数回実行されて、好ましくは互いの結果を利用する。

両計画器の組合せは、一方では、局所的な計画器が広域的な計画器によって「案内され」るので極小値の課題を回避することができる効果がある。他方では、広域的な計画器は、局所的な計画器が目標軌道を局所的に最適化するので、より「動的」になる。

特に、広域的な計画器は基本的に、純粋に運動学的な計画であり続けることができ、通常の軌道平滑化および時間パラメーター化が、局所的な計画器を用いた軌道追従制御に置き換えられる。

局所的な計画器による連続計画および広域的な計画器による補完的な再計画によって、１つの方法の不利な点がその他の利点で相殺され、動作計画は複雑な環境に最適に適合することができる。

特に、ロボットの運動を計画段階の間に十分に考慮することができ、ロボットの停止や減速のない再計画を可能にし、変化する環境の中で継ぎ目のない動作を可能にする。

これは、動く障壁を避けることができ、障壁が取り除かれる場合、より短い軌道の採用を可能にする。本発明の目的は、このように完全に達成される。

好ましい改良において、広域的な計画器は、現在の目標軌道を連続的に決定する。広域的な計画器が、繰り返しでなく連続的な計画をすることによって、新しい目標軌道が常に決定され、複雑な動的環境を確実に考慮することができる。

連続的に決定する場合、計画器は、好ましくは、まず最初は動機なく、すなわち現在の目標軌道が妥当かどうかにかかわらず、再計画を実行する。

更なる構成において、広域的な計画器は、特に動的ロードマップ(DRM)に基づく決定論的な計画器である。

複数の決定論的な計画器は、見込みに基づく方法に比べて、それらが決定論的な軌道、すなわち所定の期間の中で唯一の再生可能な解を提供するという利点がある。

見込みに基づく方法においては、必要な演算時間は、一般に制限されることもあるが、これは通常予測不可能である。

局所的な計画器と広域的な計画器との組合せは、広域的な計画器の型式(type)に対していかなる要求もしないので、動的に変化する環境の中で衝突回避のための素早い決定論的な再計画のために、決定論的な計画器の利用が有利である。

動的ロードマップ(DRM)に基づいた広域的な計画器による目標軌道の決定は、特に好ましい。

DRMベースの計画器を使った広域的な計画方法は、実行時、素早いおよび決定論的な広域的な軌道計画を可能にするので、特に好ましい。

更なる改良において、広域的な計画器は、再計画の過程で局所的な計画器から結果を聞いて、目標軌道を決定するときにそれを考慮する。したがって、広域的な計画器は、各期間で、並行した局所計画の結果を考慮できる。特に、広域的な計画器は、局所的な計画器に、予測された、現在の開始状態を問い合わせることができて、再計画の際に新しい開始状態として、それを考慮することができる。したがって再計画は、通常の場合のように休止状態を想定する必要はない。換言すれば、再計画を実行するために最初停止しなくて済むように、ロボットの運動が考慮される。この改良は、ロボットのよりなめらかな、より自然な動作に、このように寄与する。

更なる改良において、局所的な計画器はモデル予測制御(MPC)に基づく。すなわち動作の決定は、動的最適化問題を解決することによる最適化に基づく。

最適化に基づく方法は、ロボットまたはそのマニピュレータの滑らかな自然な動作をさせる軌道を見つけるために、動作計画に最適である。軌道は決定論的で、したがって再現可能で、有限な演算時間において、決定されることができる。同時に、モデル予測制御に基づく計画器は、多数の変数パラメータ、したがって特にロボットの運動を包括的に考慮できる。

好ましい改良において、動的最適化問題は、目標軌道からのマニピュレータの変位を重み付けし、および目標軌道で特定された軌道パラメータからの現在の軌道パラメータの変位を重み付けする費用関数を含む。

動的最適化問題が、計画において、目標軌道からの変位および現在の軌道パラメータからの変位を考慮するという点で、局所的な計画器は有利な方法によって広域的な計画器にリンクされる。目標軌道は、このように動的最適化問題の整数部分であり、広域的な計画器は、より短いサイクルタイムにおいて、局所的な計画器を「案内」する。

更なる改良において、動的最適化問題は、軌道の動的化および軌道の入力制約を含む。それによって、軌道の動的化は、特に広域的な計画器の目標軌道に基づくことになる。局所的な計画器が軌道の動的化（特に目標軌道の動的化）を考慮することにより、ロボットの自然な動作を、更に改善することができる。

更なる改良において、動的最適化問題は、衝突までの距離のための少なくとも１つの不等式制約(inequality constraint)を含む。この追加的な不等式制約によって、障壁までの最小距離が定義される。最適化問題を解決するときに、それは考慮されなければならない。このようにして、障壁との衝突を回避できる。

同時にマニピュレータは、特定された目標軌道に分類上固執する必要がなく、例えばより素早くその目的地に到達するために、状況に応じてそれから逸脱できる。ロボットの動作はこのように、変化する環境に対してより自然になる。

更なる改良において、広域的な計画器による決定の実行は、所定の条件で調節可能である。したがって、広域的な計画器の実行は、各種条件に関連付けられ、それによって、好ましくは自動的に、計画は、広域的な計画器の特殊性および特定の場合に反応できる。このようにして、より強固な(robust)動作計画を達成できる。

好ましくは、広域的な計画器による決定の、無条件の実行、不実行または条件付きの実行は、所定の条件を経て設定される。無条件の実行、不実行または条件付きの実行の区別は、これによって、好ましくは動的に選択可能で、特定の場合に対する適切な反応が、広域的な計画において起こり得るので、有利であると判明した。

好ましくは、条件付き実行は、決定された目標軌道および／または決定された動作が所定の計画期間において有効かどうか、ならびに、現在の状態と目標とする状態との間の距離が、最後のサイクル・ステップの間、減じたかどうかにも依存している。有効な軌道が発見できないこと、または計画が進展しないことは、条件付き実行により考慮される特殊なケースである。これは、動作計画をよりロバストにする。

更なる改良において、広域的な計画器による計画が失敗した場合、第１のマニピュレータを動かすことは中断される。あるいは、広域的な計画器による計画が失敗した場合、第１のマニピュレータを動かすことは局所的な計画器に基づいてのみ続けられる。

広域的な計画器の特定のケースに対する反応として、計画を休止することまたは局所的な計画器だけで継続することは、本発明による２種類の計画器の組合せに適用されることができる２つの代替的なアプローチである。動作計画の堅固性は、このように更に増加できる。

更なる改良において、広域的な計画器および／または局所的な計画器は障害物の動きを部分的に予測する。また、広域的な計画器および／または局所的な計画器は障壁の表示を作為的に拡大する。

障壁を作為的に拡大するか、または障害物の動きを予測することによって、各計画器は、衝突を回避するために、時間前に再計画をすることが要求される。このようにして、特に障害の許容範囲が、例えば、推定の不確定性を適切に考慮することによって、増大する。

更なる改良において、本方法は少なくとも１つの第２のマニピュレータのための追加的な動作計画を更に含む。この追加的な動作計画は、第１のマニピュレータの衝突回避のための動作計画と並行に実行され調整される。

本方法によって、動作計画は、複数のマニピュレータ、例えば２つのアームを持つロボット、のために容易に実行されることができる。

例えば、広域的な計画器は、両方のマニピュレータのために調整された目標軌道を決定するために、マニピュレータごとに別々のDRMを有することができる。局所的な計画器は、そのとき、動作を実行するために、決定され調整された目標軌道に取り組む。このために同じアルゴリズムを基本的に用いることが可能であるので、複数のマニピュレータの動作を容易に設定することができる。

前述した各特徴および以下で説明される各特徴は、示された組合せだけで用いられるのではなく、本発明の範囲内で、他の組合せでも、または単独でも使用できることは言うまでもない。

本発明の実施形態は、図面に示されて、以下の説明において、更に詳細に説明される。

本発明の実施形態に係る装置を示す略図である。 本発明の実施形態に係る方法を示す略図である。 当該方法の望ましい実施形態を示す略図である。 局所計画の最適化問題の数式表現を示す図である。 可動障壁のある場合に、本発明の実施形態に係る方法に従った典型的な動作計画を示す図である。

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

図１に、本発明の実施形態に係る装置を示す。装置の全体を参照番号10で表す。

ここで、装置10は産業用ロボット12である。産業用ロボットは、少なくとも１つのマニピュレータ14を有し、工作物を取扱い組立てるための汎用かつプログラム可能な機械である。それは環境との物理的な相互作用を可能にして、ロボットの機械作業を実行する。マニピュレータ14は、駆動部品（例えば軸16の機械的結合のためのモーター、ギア、エンコーダ（図示せず）、継手18）を備えていてもよい。

駆動部品は、プログラム可能なコンピュータとして設計される制御ユニット20によって制御される。制御ユニット20はロボットおよびマニピュレータの動作を調整する。

図１に示される実施形態において、ロボット12は、基台22に固定されたロボットであり、基台22にマニピュレータ14が動作できるように載置されている。

マニピュレータ14は、例えば３本の軸16と、3本の継手18とを備え、マニピュレータ14のエッジ（枝）には、エンドエフェクタ24が配置されている。図では、それは把手として例示されている。

制御ユニット20は、ロボット12およびマニピュレータ14の動作を調整する。制御ユニット20はロボット12に内蔵されるか、または外部デバイスとしてそれに接続される。

以上の固定ロボットは、本発明による方法を実行することができるロボットの1つの可能な構成にすぎないことは、言うまでもない。

それ以外に、移動ロボット、いくつかの複数のマニピュレータを有するロボット、または複数のコンポーネントに分割されたモジュラー制御ユニットを有するロボットも、考えられる。同様に、この方法は、産業用ロボットに限られず、他の種類の自動機械にも適用することができる。

ドライブを制御することに加えて、ロボットを制御することは、衝突回避のための動作計画、すなわち出発点26から、それ自体とまたは障壁と衝突することなく終点28まで到達するための、ロボットまたはそのマニピュレータの動作軌跡を探索することを含む。

障壁は、静止している障壁30または動く障壁32でありえる。後者の場合、
最初に計画された軌跡が障壁のため無効になったときのために、動作計画は一回だけでなく、数回実行されることが必要である。

動的な環境における衝突回避のための動作計画は、本発明の主題である。図2を参照してさらに詳細に説明する。

図2は、本発明の実施形態に従った方法を説明するための略図である。

本方法は3つの工程を含む。それらは、好ましくは、並行的に実行できる。

第1の工程および第2の工程は、計画に係る工程である。第3の工程は、実際のロボットの制御を含む。この明細書において「並行」とは、同期しながら、または非同期で、複数の方法が並列的に実行されることを意味する。

第1の工程は、広域的な計画器34によって反復され得る動作計画である。第2の工程は局所的な計画器36による連続した動作計画である。第3の工程は、ロボットのドライブを制御することによって、第１および第２の計画34、36によって作成された計画を実行する。

この明細書における広域的な計画器34は、所定の出発点から所定の目的点までの目標軌道(target path)を決定する計画である。このためには、運動学的な軌道計画で充分である。換言すれば、広域的な計画器は、動作のための実際の軌道を決定するための軌道の平滑化もしくは時間のパラメーター化を必要としない。これは、局所的な計画器36を用いた軌道追従制御(path-following control)により達成される。

広域的な計画器による各実行の結果は、もし見つかれば、運動学の軌道計画すなわち目的点への可能な軌道である。望ましい実施形態において、出発点または出発状態は、局所的な計画器36の結果に基づいて、各実行において、変化してもよい。

本発明における局所的な計画器36は、反応(reactive)的に働く計画である。すなわち全軌道の代わりに、局所的な計画器36は、環境への反応として即時に制御パルスを発生させる。

このようにして局所的な計画器36は、衝突を回避するために障壁からロボットを「押しのける」ことができる動作を開始する。それによって、全体の動きは、好ましくは目的点への方向に「駆動される」。この方法は、期間(horizon)を超える動きの予測を含み、それは、制約(constraints)を考慮することを許容する。

本発明に従った組合せは、両方の計画が第3の工程（すなわちドライブの制御）に影響するように、設計されている。

好ましくは、したがって、広域的な計画器34および局所的な計画器36は、互いに独立に実行されるのでなく、相互交換で実行される。局所的な計画器36は広域的な計画器34の結果40を基にして設計される。そして、広域的な計画器34は再計画する際に局所的な計画器36の結果42を利用する。

相互交換は矢印44で示されて、それぞれの計画34、36の設置により実現される。したがって、相互交換44は、好ましくはそれぞれの計画の整数部分(integral part)である。

広域的な計画器34は、繰り返し計画される。すなわち、動作の間、少なくとも１つの新しい計画を実行する。局所的な計画器36は常に連続的に計画される。すなわち計画は、特に固定サイクルT₂において、連続的に行われる。

望ましい実施形態において、広域的な計画器はまた、好ましくは固定サイクルT₁において、連続的に行われる。ここでサイクルT₁は、サイクルT₂より、例えば10〜30倍長い期間である。20倍長い期間であってもよい。それは、基本的に個々のステップが、最悪予想される演算時間に依存する。

広域的な計画器34による新しい計画は、動機なく起こる。すなわち広域的な計画器34は、外部的なきっかけによらなくても、少なくとも1つの新しい計画を実行する。換言すれば、広域的な再計画は初めから、前の解がまだ効力を有するかどうかにかかわらず、独立している。

通常、本発明による方法は、それぞれの計画の具体的な設置(implementation)に限定されるわけではない。目標となる軌道を繰り返し生成できる他の広域的な計画器、または広域的な計画器の目標となる軌道を参照して使用できる他の局所的な計画器を、本発明の意味において結合することも、考えられる。

図3を参照して、好ましい広域的な計画器および好ましい局所的な計画器並びにそれらの組合せを、さらに詳細に説明する。同じ引用符号は、以前に出てきたのと同じ部品を意味する。

この方法は、広域的な計画器34および局所的な計画器36を含んでいる。この実施形態では、計画34、36はモデラー(modeler)38により補充される。モデラー38は、環境モデル46を含み、局所的な計画器36のために距離地図(distance map)47を含む。

図3の実施形態において、広域的な計画器34による広域的な動作計画は、動的ロードマップ(DRM)に基づいている。動的ロードマップとは、頻繁な再計画を必要とする変化する環境のための計画方法である。

DRMは、ロボットおよびマッピング規則の構成空間CにおけるグラフG = (V, E)を含む。マッピング規則は、ラスター化された工作物の領域Wを、グラフGの中のノード（節）Vおよびエッジ（枝）Eにマッピングする。それらは、計画段階において、特定の障壁のためグラフの有効な部分を速やかに識別するために用いられる。

DRMベースの計画方法は、主として、予備段階および計画段階という２つの段階を備える。

予備段階においては、ランダムな構成q(u)（u ∈ V）が、n個のノード（節）の各々に、最初に割り当てられる。この構成は自己衝突のためにのみ点検される。その理由は、この段階では、空の環境を想定しているからである。その後、グラフの各エッジ（枝）(u, v) ∈ Eを生成するために、各ノードのv ∈ Vに最も近いk_nnが、距離測定基準(distance metric)ｄ(ｕ, v) = d(q(u), q(v))を使用して、決定される。距離測定基準の選択は、ロードマップの品質に影響する重要な要素である。

各体積要素(voxel)w ∈ Wごとに、そのボクセルが障壁によって占められる場合、グラフGのどのノード（節）およびエッジ（枝）が無効であるかを、マッピングφ_v： W → Vおよびφ_e：Ｗ→ E が指し示す。

グラフにおけるノード（節）およびエッジ（枝）に対応するすべての構成および動作はラスタライズされ、逆マッピング規則φ_e ^-1：V → W およびφ_v ^-1：E → Wが生成される。この逆マッピング規則によって、DRMのマッピングは容易にできる。

計画段階において、DRMが、出発地構成q_Sと目的地構成q_Gとの間に有効な軌道を見つけるために用いられる。

第1に、出発地および目的地は、グラフのなかでそれらと最も近いk_nnに接続される。これは、計画段階の間に、衝突の点検を必要とする唯一のステップである。それから、障壁と交わっているすべてのボクセルが識別され、衝突に至るグラフのすべてのノード（節）およびエッジ（枝）を無効にするために、マッピング規則φ_vおよびφ_eが用いられる。

その結果、出発地と目的地との間に最短の有効軌道が検索される。その結果は、本発明で言う目標軌道(target path)に対応する。

DRMをベースにした計画では、100ミリ秒未満の計画時間が、標準的なハードウェアで遂行できる。さらにまた、ロードマップのサイズは、GPU上で並行する実行により百万個のノード（節）にまで拡大可能である。

DRMを使用した計画は繰り返される。すなわち、目標となる軌道は異なる時間ごとに複数回決定される。好ましくは、広域的な計画器も、固定された間隔で連続的に起こる。

図3において、例えば、広域的な再計画が、200ミリ秒毎に実行されている。広域的な計画器34はこのように連続的なループで実行されて、目的地に到達したこと、または、計画終了のいずれかまで、目標軌道を生成する。後者の計画終了は、もし予測された軌道が広域的なサンプリング時間 t ∈[0, Δt_global]の範囲内で無効になった場合、または、最大の実行時間が満了した場合である。

望ましい実施形態において、広域的な計画器による連続的な決定の実行は、所定の状態を経てセットされることができる。広域的な計画器34の実行は、１つまたはそれ以上のポリシー(policy)によって、このように影響される。広域的な計画器のポリシー48と失敗の場合のポリシー50との区別が、好ましくはなされる。

広域的な計画器を、現在の目標軌道を決定するためにいつ呼び出すかを特定するために、計画器のポリシー48を用いることができる。

広域的な計画器の、無条件の実行または条件付き実行のために、所定の条件が用意される。

無条件の実行は各サイクルで広域的な計画器を実行する。しかし、所定の条件が広域的な計画器の実行を許容しない場合、単に局所的な計画器だけが使われる。これに対して、条件付き実行においては、その計画が実行されることになっているか否か決定するために、さまざまな基準が使用できる。例えば、１つの基準は、予測された軌道が全ての期間(horizon)において有効でないかどうか、である。他の、または補完的な基準は、現在の状態と目標の状態との距離が最後のサイクルで減少したかどうか、すなわち局所的な計画器が目的地に向けて充分に前進したかどうか、である。

一方、失敗の場合のポリシー50は、解がないまたは動的ロードマップが不完全であるという理由で広域的な計画器が有効な軌道を見つけることができない場合に現れる。

この場合、対応する挙動を、失敗の場合のポリシー50を経て定義することが可能である。

ロボットは、例えば、現在の状態が目標軌道として定義される、待ちの状態に入ることができる。あるいは、以前の目標軌道を維持してもよい。

それから、局所的な計画器36は現在の位置を「保持」するか、あるいは前に有効であった軌道に沿って進む。

他の選択肢として、理論的で直接的な軌道を失敗の場合のポリシー50に基づいて新しい軌道として設定することもできる。その結果、局所的な計画器はいかなる障壁にもかかわらず、目的地に直接到達する。

この場合、局所的な計画器は、基本的に、そのものであり(on its own)、広域的な計画器の影響を受けない。

計画のポリシー48および失敗の場合のポリシー50を用いて、動作計画はこのように、それぞれのシナリオに柔軟に適応することができ、本発明の方法の応用の可能性を増加させる。

DRMベースの計画によってここで決定された目標軌道は、矢印52に示すように、局所的な計画器36のための入力として機能する。ここで示されるように、例えば、局所的な計画器36は10ミリ秒のサイクルタイムで連続的に動作する。

この例では、局所的な計画器36は、モデル予測制御に基づいており、それはModel Predictive Controller (MPC)とも呼ばれる。モデル予測制御は、マニピュレータの実際の制御を決定し、とりわけ、衝突回避および軌道追従の課題を持つ。この目的のために、各サイクルで最適化問題が解決され、そして、最適解の第１部分が制御入力として用いられる。目標軌道p(s)（s ∈ [0, s_G]）が広域的な計画器34によって生成され、局所的な計画器36のためのガイドとして役立ち、このように極小値の問題を回避する。

最適化問題は、図4に示すように公式化されることができる。

図4は、好ましい局所的な計画器36の最適化問題の数式表現を示す。

動的最適化問題54の出発点は、可動システムのダイナミックス56を記述するシステムモデルである。可動システムとは、例えば、状態58および制御変数60を伴うマニピュレータ14である。

ダイナミックス56は、状態x = qおよび制御変数

を伴う積分

として単純化することができる。または、状態

および制御変数

を伴う二重積分

として単純化することができる。

算出された軌道が直接実行されることになっている場合、最小および最大の加速が考慮できる二重積分が好まれる。

費用関数(cost function)62は、状態58および制御変数60の重み付けをする。最適化の課題は、通常無限の数の制御関数から、費用関数62を最小化する最適な制御関数を見つけることである。複数の状態58は複数の状態制限64に従い、複数の制御変数60は複数の制御変数制限66に従う。

局所的な計画器36を広域的な計画器34に結合するために、費用関数62は、現在の位置および／または現在の状態によって特定される現在の軌道のパラメータｓの、目標軌道により特定される軌道パラメータs_Gからの変位を重み付けするとともに、目標軌道からのマニピュレータ14の変位を追加的に重み付けする。

現在の軌道パラメータsと目標軌道パラメータs_Gとの間の変位は、二次曲線的に重み付けされないことが好ましい。その理由は、勾配の次元は目的地までの距離に依存するからである。その代わりに、費用関数62は絶対値を使用し、絶対値が所定の閾値Δsより小さい場合、二次曲線的な重みに変わる。このようにして、目的地の近くの振動を回避できる。

さらにまた、動的最適化問題54は、軌道動的化68および軌道入力の制約70を含むことができる。軌道動的化68は、広域的な計画器34の目標軌道をベースとしている。比喩的に云えば、広域的に計画された軌道の後に、局所的な計画が、ばねから懸架したように続く。

加えて、動的最適化問題54は、障壁を考慮することができる少なくとも１つの不等式制約(inequality constraints)72を含むことができる。不等式制約72は、自己衝突までの距離および／または障壁30、32との衝突までの距離を考慮する。

自己衝突までの距離を算出するために、中心および半径を有する球体によるロボットの近似を用いることができる。障壁との衝突までの距離を算出するために、環境を、ラスター地図によって、近似することができ、それによって必要とされる距離は、ラスター地図のユークリッド距離変換から生じる。最も短い距離は、自己衝突または障壁との衝突のいずれかまたは双方に際して考慮される。

障壁のラスター化およびユークリッド距離変換は、好ましくは前もって算出される（モデラー38）。よって、リアルタイム最適化能力は影響を受けない。

速く動く可動障壁を考慮する場合、障壁の動作の推定および短い予測は、計画において行うことができる。動的なロードマップおよび最適化ベースの計画が、工作物エリアWのラスター化された表示(representation)に基づくので、障害物がいつ特定の位置に来るかという情報がない。しかしながら、追加的な体積要素(voxels)が障壁の動作方向に沿ってブロックされることができる、および／または、計画が障壁に素早く反応できる ように、障壁の表示を作為的に拡大することができる。よって、このことにより、方法のロバスト性は増大する。

図5は、広域的な計画器および局所的な計画器を複合した軌道計画を使用した、考え得る動作の略図を示す。

図5は、無効な領域C_obsにより定義された障壁を含む二次元の構成空間Cを示す。広域的に計画された目標軌道p(s) 74が、局所的最適化ベースの計画のためのガイドとして使われる。それは軌道x(t) 76の形で実際の動作を決定する。

図5において、時間的に続く３つの線図が、左、中央、右に図示される。

第1の線図において、局所的な計画器36は、より速く進むために、目標軌道74aから変位する軌道76aを生成する。

第2の線図において、障壁が動いたあと、局所的な計画器36は障壁との衝突を回避するために軌道76bを生成する。

新しい広域的な軌道74bが算出されるとすぐに、最後の線図に示すように、局所的な計画器36は、動作を、新しい目標軌道74bに自動的に合わせる。

ここで示されるように、広域的な計画器34は、局所的な計画器36から予測された位置78の形での中間的な結果を要請することができて、マニピュレータ14の新しい目標軌道74bを決定するときに、それを考慮することができる。

ここで提示された各例は、言うまでもなく、本発明による方法と装置の実施例として理解されるものである。他の例も、本発明の要旨の範囲内に含まれる。通常、本願の保護の範囲は、請求の範囲によって与えられて、本明細書に説明されまたは図示された特徴により制限されない。

３４ 広域的な計画器
３６ 局所的な計画器
３８ モデラー
４０ 広域的な計画器34の結果
４２ 局所的な計画器36の結果
４４ 相互交換
４６ 環境モデル
４７ 距離地図
４８ 計画器のポリシー
５０ 失敗の場合のポリシー
５４ 動的最適化問題

Claims (16)

1. 第1のマニピュレータ(14)の出発点(26)から目的点(28)まで衝突を回避する動作計画のための方法であって、
   広域的な計画器(34)を用いて、目的点(28)までの前記第1のマニピュレータ(14)の目標軌道を繰り返し決定するステップと、
   広域的な計画器(34)の現在の目標軌道に基づいて、局所的な計画器(36)用いて、前記第1のマニピュレータ(14)の動作を連続的に決定するステップと、
   前記広域的な計画器(34)および前記局所的な計画器(36)による前記決定と並行して、前記第1のマニピュレータ(14)による動作を実行するステップとを含む、方法。
2. 前記広域的な計画器(34)は、現在の目標軌道を連続して決定する、請求項1に記載の方法。
3. 前記広域的な計画器(34)は、特に動的ロードマップ(DRM)に基づいた決定論的な計画器である、請求項1または請求項2に記載の方法。
4. 前記広域的な計画器(34)は、再計画する間、前記局所的な計画器(36)の結果を参照して、目標軌道を決定するときにそれを考慮する、請求項１〜請求項３のいずれか１項に記載の方法。
5. 前記局所的な計画器(36)はモデル予測制御(MPC)に基づいており、前記動作の決定は、動的最適化問題(54)を解決することによる最適化に基づく、請求項1〜請求項4のいずれか１項に記載の方法。
6. 前記動的最適化問題(54)は、前記マニピュレータ(14)の前記目標軌道からの変位と、現在の軌道パラメータの前記目標軌道によって特定された軌道パラメータからの変位と、を重み付けする費用関数(62)を含む、請求項５に記載の方法。
7. 前記動的最適化問題(54)は、軌道動的化(68)と軌道入力制約(70)とをさらに含み、前記軌道動的化(68)は、特に広域的な計画器(34)の目標軌道に基づく、請求項6に記載の方法。
8. 前記動的最適化問題(54)は、衝突までの距離の少なくとも１つの不等式制約(72)をさらに含む、請求項5〜請求項7のいずれか１項に記載の方法。
9. 前記広域的な計画器の実行は、所定の条件を経て調節可能である、請求項1〜請求項8のいずれか１項に記載の方法。
10. 前記広域的な計画器(34)による決定の、無条件の実行、不実行または条件付きの実行は、所定の条件を経て設定される、請求項9に記載の方法。
11. 前記条件付きの実行は、（ａ）決定された目標軌道および決定された動作のうちの一方または双方が、所定の計画期間(planning horizon)において有効かどうか、および、（ｂ）現在の状態と目標状態との間の距離が局所的な計画器(36)の最後の判定サイクルの期間で減じたかどうかに、依存する、 請求項10に記載の方法 。
12. 前記第1のマニピュレータ(14)の動作は、前記広域的な計画器(34)が失敗すれば休止される、請求項1〜請求項11のいずれか１項に記載の方法。
13. 前記第1のマニピュレータ(14)の動作は、前記広域的な計画器(34)による計画が失敗した場合、前記局所的な計画器(36)のみに基づいて続けられる、請求項1〜請求項11のいずれか１項に記載の方法。
14. 前記広域的な計画器(34)および前記局所的な計画器(36)のいずれかまたは双方は、障害物の動きを部分的に予測し、および／または障壁(30, 32)の表示を作為的に拡大する、請求項1〜請求項13のいずれか１項に記載の方法。
15. 少なくとも第２のマニピュレータのための追加的な動作計画をさらに備え、当該追加的な動作計画は、前記第1のマニピュレータ(14)の衝突を回避する動作計画と並行して、かつ協調して実行される、請求項1〜請求項14のいずれか１項に記載の方法。
16. 少なくとも１つのマニピュレータ(14)と、請求項1〜請求項15のいずれか１項に記載の方法を実行するための制御ユニット(20)、とを含む装置(10)。

Images