(57)【要約】【課題】閉鎖運動について、自己衝突および障害物との衝突を回避しながら、再現可能性と計算時間とに関して確定的な軌道の決定を可能にするとともに、変化する環境に軌道が適応できる、動作計画のための方法を提供する。【解決手段】方法 (100) は、動的最適化問題 (12) を定義することと、数値的アプローチを使用して最適化問題 (12) を解くことと、最適化問題 (12) の解に基づいて第１マニピュレータ (28)のための第１移動経路を決定することとを含む。動的最適化問題 (12) は、第１マニピュレータ (28) の状態 (16) および制御変数 (18) に重み付けを行うコスト関数 (20) と、第１マニピュレータ (28) の状態 (16) および制御変数 (18) を時間の関数として定義する運動 (14) と、衝突までの距離に関する少なくとも１つの不等式制約 (24) とを含む。さらに、最適化問題 (12) は、閉鎖運動に関する少なくとも１つの等式制約 (22) を含む。【選択図】図２
2021506031.tif 000002
【特許請求の範囲】
【請求項１】
閉鎖運動を伴う第１マニピュレータ（２８）の衝突回避動作計画のための方法（１００）であって、
動的最適化問題（１２）を定義することと、
前記最適化問題（１２）を数値的アプローチで解くことと、
前記最適化問題（１２）の解に基づいて、前記第１マニピュレータ（２８）のための第１移動経路を決定することと、を含み、
前記動的最適化問題は、前記第１マニピュレータ（２８）の状態（１６）および制御変数（１８）に重み付けを行うコスト関数（２０）と、前記第１マニピュレータ（２８）の状態（１６）および制御変数（１８）を時間の関数として定義する運動（１４）と、衝突までの距離に関する少なくとも１つの不等式制約（２４）と、閉鎖運動に関する少なくとも１つの等式制約（２２）とを含む、方法。
【請求項２】
前記動的最適化問題（１２）は、開始条件および終了条件、具体的には前記第１マニピュレータ（２８）の開始位置および目標位置を定義する境界条件（２６）をさらに含む、請求項１に記載の方法。
【請求項３】
前記動的最適化問題（１２）は、前記制御変数の最小値および最大値、具体的には前記第１マニピュレータ（２８）の最低および最高の速度および／または加速度の値を定義する１つまたは複数の制御変数制約をさらに含む、請求項１または請求項２に記載の方法。
【請求項４】
前記動的最適化問題（１２）は、前記状態の限界、具体的には、前記第１マニピュレータ（２８）の最小および最大の関節の角度および／または関節の角速度を定義する１つまたは複数の状態制約をさらに含む、請求項１〜請求項３のいずれか１項に記載の方法。
【請求項５】
前記動的最適化問題（１２）は、移動の持続時間を表す所定の終了時間Ｔをさらに含む、請求項１〜請求項４のいずれか１項に記載の方法。
【請求項６】
前記終了時間Ｔは、決まった値とするか、または前記終了時間が前記コスト関数（２０）によって重み付けを行われるようにいずれか選択される、請求項５に記載の方法。
【請求項７】
前記動的最適化問題（１２）は、１つまたは複数の終了時間の制約、具体的には最小終了時間および最大終了時間の値をさらに含む、請求項５または請求項６に記載の方法。
【請求項８】
衝突までの距離に関する前記少なくとも１つの不等式制約（２４）は、衝突までの最小距離を定義する、請求項１〜請求項７のいずれか１項に記載の方法。
【請求項９】
前記閉鎖運動に関する前記少なくとも１つの等式制約（２２）は、前記第１マニピュレータ（２８）の第１の動的位置制約を定義する、請求項１〜請求項８のいずれか１項に記載の方法。
【請求項１０】
前記閉鎖運動に関する前記少なくとも１つの等式制約（２２）は、前記第１マニピュレータ（２８）の第１の動的向き制約を定義する、請求項１〜請求項９のいずれか１項に記載の方法。
【請求項１１】
前記数値的アプローチは、制約付きの最適化問題から制約なしの最適化問題への前記動的最適化問題の変換を含む、請求項１〜請求項１０のいずれか１項に記載の方法。
【請求項１２】
前記閉鎖運動に関する前記少なくとも１つの等式制約（２２）および衝突までの距離に関する前記少なくとも１つの不等式制約（２４）は、それぞれ乗数を掛けて、前記コスト関数（２０）に加算される、請求項１１に記載の方法。
【請求項１３】
閉鎖運動に関する前記少なくとも１つの等式制約（２２）および衝突までの距離に関する前記少なくとも１つの不等式制約（２４）は、ペナルティパラメータを二乗したものを掛けて、前記コスト関数（２０）に加算される、請求項１１〜請求項１２のいずれか１項に記載の方法。
【請求項１４】
前記乗数および前記ペナルティパラメータは、前記動的最適化問題（１２）を解くときに反復的に適応される、請求項１３に記載の方法。
【請求項１５】
前記数値的アプローチは、勾配アプローチを用いた前記制約なしの最適化問題を解くことを含む、請求項１１〜請求項１４のいずれか１項に記載の方法。
【請求項１６】
前記第１移動経路を実行することをさらに含み、
前記第１移動経路の実行は、前記最適化問題を前記解くことと並行して行われる、請求項１〜請求項１５のいずれか１項に記載の方法。
【請求項１７】
前記動的最適化問題（１２）は、前記移動の前記持続時間を表す終了時間Ｔを有し、前記数値的アプローチは、第１ステップで、前記移動の前記持続時間よりも短い少なくとも１つの第１期間に関する第１部分解を生成し、少なくとも１つの第２ステップで、第２期間に関する第２部分解を生成し、前記第２ステップについては、前記動的最適化問題（１２）に対応するとともに初期条件として前記第１期間の終了時の状態を考慮した、適応最適化問題を使用する、請求項１６に記載の方法。
【請求項１８】
第２マニピュレータ（３０）のための第２移動経路を決定することをさらに含み、
前記コスト関数（２０）は、前記第２マニピュレータ（３０）の前記状態（１６）および制御変数（１８）に追加で重み付けを行い、前記運動（１４）は、前記第２マニピュレータ（３０）の前記状態（１６）および制御変数（１８）を時間の関数として定義し、前記第１マニピュレータ（２８）と前記第２マニピュレータ（３０）とが閉鎖運動を形成する、請求項１〜請求項１７のいずれか１項に記載の方法。
【請求項１９】
前記閉鎖運動に関する前記少なくとも１つの等式制約（２２）は、前記第１マニピュレータ（２８）の第１の動的位置制約および／または向き制約、ならびに前記第２マニピュレータ（３０）の第２の動的位置制約および／または向き制約を含み、これらは互いに定義された決まった関係にあるため、前記閉鎖運動が実現される、請求項１８に記載の方法。
【請求項２０】
第１マニピュレータ（２８）と制御ユニット（４２）とを備える装置であって、前記制御ユニット（４２）は、請求項１〜請求項１９のいずれか１項に記載の方法を実行するように構成されている、装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、閉鎖運動を伴うマニピュレータの衝突回避動作計画のための方法、および対応する装置に関する。
【背景技術】
【０００２】
ロボット工学における衝突回避動作計画は、ロボットまたはそのマニピュレータについて、自身または障害物との衝突のない、始点から終点までの動作経路を探索することである。
【０００３】
閉鎖運動系とは、少なくとも１つの閉鎖運動連鎖（closed kinematic chain）を有するものとして定義される。これは、マニピュレータがその基部に２つの異なる形で運動学的に連結されている場合である。閉鎖運動連鎖は、たとえば、ロボットと床もしくは壁に可動に取り付けられている要素とによって、または、物体を共同で把持して動かすロボットの複数のマニピュレータによって形成される。そして、動作計画は、閉鎖運動が常時実現されるように実施されなければならない、すなわち、ロボットは、固定要素との、または共同で持ち運ぶ物体との接触を失わないように動かなければならない。したがって、動作計画にとって、閉鎖運動は、動きの自由度の追加的な制限をもたらす。
【０００４】
Probabilistic Roadmaps（ＰＲＭ：確率的ロードマップ法）またはRapidly-exploring Random Trees（ＲＲＴ：高速探索ランダムツリー）など一般的なサンプルに基づく動作計画器にとって、閉鎖運動は、ランダムコンフィギュレーションが追加の制約を満たす確率はゼロに近いため、大きな課題である。
【０００５】
単純な勾配降下法によりランダムサンプルから許容可能なコンフィギュレーションを生成するアプローチは公知であるが、これらは、問題解決に非常に多数のサンプルが必要な場合、多くの計算時間を伴う。加えて、確率的方法には、確定的な軌道が生成されない、すなわち、計算時間を制限できるが一般的に予測できない、という問題がある。
【０００６】
確率的方法の他に、遠回りせずにロボットまたはそのマニピュレータの滑らかで自然な移動を直接生成するように設計された、最適化に基づく方法が知られている。最適化に基づく方法には、再現可能かつ決定可能な確定的な軌道を有限の計算時間で提供できるという追加の利点がある。最適化に基づく方法の前提条件は、動作計画を十分に表す適切な最適化問題の定式化である。
【発明の概要】
【発明が解決しようとする課題】
【０００７】
そのため、本発明の目的は、閉鎖運動について、自己衝突および障害物との衝突を回避しながら、再現可能性と計算時間とに関して確定的な軌道の決定を可能にするとともに、変化する環境に軌道が適応できる、動作計画のための方法を提供することである。
【課題を解決するための手段】
【０００８】
本発明の一態様によれば、この方法は、閉鎖運動を伴う第１マニピュレータの衝突回避動作計画のための方法であって、
・動的最適化問題を定義するステップと、
・数値的アプローチにより最適化問題を解くステップと、
・最適化問題の解に基づいて第１マニピュレータの第１移動経路を決定するステップと、を含む。
【０００９】
動的最適化問題は、第１マニピュレータの状態および制御変数に重み付けを行うコスト関数と、第１マニピュレータの状態および制御変数を時間の関数として定義する運動(dynamic)と、衝突までの距離に関する少なくとも１つの不等式制約と、閉鎖運動に関する少なくとも１つの等式制約とを備える方法によって解決される。
【００１０】
さらに、前記目的は、第１マニピュレータと制御ユニットとを備える装置であって、制御ユニットが前記衝突回避動作計画のための方法を実行するように構成されている装置によって解決される。
【００１１】
このように、閉鎖運動を伴う衝突回避動作計画を最適化問題として定式化し、それを数値的に解くことが本発明者の着想である。
【００１２】
最適化問題は動的問題であり、マニピュレータの運動をその状態および制御変数に関して考慮し、コスト関数によってそれに重み付けを行う。運動は、経時的な状態および制御変数の進行を表す。状態は、関節角度によって示される空間内の位置であってもよく、制御変数は、関節の角速度（積分器）または関節の角加速度（二重積分器）であってもよい。
【００１３】
コスト関数は、状態および制御変数の重み付けを行う。コスト関数は、速度または加速度を最小にするために、重み付け行列で制御変数にペナルティを与えるだけでもよい。この定式化は、ロボットの開始位置と到達するべき目標位置との間の最短経路の探索に相当する。
【００１４】
閉鎖運動を考慮し、自己衝突または障害物との衝突を回避するために、動的最適化問題を等式制約と不等式制約とによって補足する。不等式制約は、自己衝突および障害物との衝突を排除するために観察しなければならない片側限界を定義するために使用することができる。他方で、等式制約は、満たさなければならないが、マニピュレータの移動で動的に変えることのできる特別な要求事項である。等式制約によって、閉鎖運動によって規定される条件を移動中に観察することを確保することができる。
【００１５】
少なくとも１つの不等式制約と少なくとも１つの等式制約とを含む動的最適化問題は、その後、数値的アプローチを用いて解かれる。等式制約と不等式制約とを含む動的最適化問題として定式化された動作計画は、公知の動的最適化問題と異ならないため、公知の数値解法を使用することが有利である。したがって、動的最適化問題を決定的なものにするために公知の手段を適用することができる。
【００１６】
まとめると、不等式制約と等式制約とを含む動的最適化問題として動作問題を定式化することで動作計画が単純化され、確定的な軌道を生成することができるとともに、閉鎖運動の条件でマニピュレータの衝突回避動作制御が可能になる。こうして、冒頭に述べた目的は完全に解決される。
【００１７】
さらなる改良形態において、動的最適化問題は、開始条件および終了条件、具体的には第１マニピュレータの開始位置および目標位置を定義する追加の境界条件を含む。
【００１８】
追加の境界条件を使用すると、動作計画で別のパラメータを直接考慮することができるので、たとえば、マニピュレータを初期位置に移さなくても、動作計画をすぐに開始することができる。
【００１９】
さらなる改良形態において、動的最適化問題は、制御変数の最小値および最大値、具体的には第１マニピュレータの最低および最高の速度および／または加速度の値を定義する１つまたは複数の制御変数制約をさらに備える。
【００２０】
追加の制御変数の制限を使用することで、動作計画を、第１マニピュレータの実現可能な移動のみに直接制限することが可能である。したがって、動的最適化問題の解において、マニピュレータの能力を直接考慮することができる。
【００２１】
さらなる改良形態において、動的最適化問題は、状態の限界、具体的には第１マニピュレータの最小および最大の関節角度および／または関節の角速度を定義する１つまたは複数の状態制約をさらに含む。
【００２２】
この改良形態では、マニピュレータの位置の制限を直接考慮することができる。たとえば、マニピュレータが理論上は到達することができるが、そのように想定されていない空間を定義することができる。たとえば、マニピュレータについて定義された作業空間を定義することができ、これはすぐに動作計画中に考慮される。
【００２３】
さらなる改良形態において、動的最適化問題は、移動の持続時間を表す定義された終了時間をさらに含む。そのため、計画するべき移動の持続時間は最適化問題の一部であり、動作計画中に考慮しなければならない。
【００２４】
具体的には、終了時間は、決めることができ、または、コスト関数によって重み付けを行うことができる。第１のケースでは、移動にかかってもよい時間の長さを決めるのに対して、第２のケースでは、別の変数として該時間を最適化する。後者の場合、終了時間は、コスト関数によって重み付けを行い、最小値および最大値を定義することが好ましい。
【００２５】
さらなる改良形態において、衝突までの距離に関する少なくとも１つの不等式制約は、衝突までの最小距離を定義する。したがって、不等式制約は、マニピュレータが障害物にどの程度の近さまで近づいてもよいか、またはマニピュレータがそれ自体からどのくらいの距離を保たなければならないかを決定する。
【００２６】
さらなる改良形態において、閉鎖運動に関する少なくとも１つの等式制約は、第１の動的位置制約および／または第１の動的向き制約を定義する。したがって、等式制約は、移動中のマニピュレータの位置または向きに関して動的制約を課す。このように、閉鎖運動の条件は、移動中、並進制限および回転制限によって維持することができる。
【００２７】
さらなる改良形態において、数値的アプローチは、制約付きの最適化問題から制約なしの最適化問題への動的最適化問題の変換を含む。動的最適化問題の制約なしの最適化問題への変換により、特に効率的に最適化問題を解くことが可能になり、制約なしの最適化問題を解くための公知の方法を使用することができる。
【００２８】
好適な改良形態において、閉鎖運動に関する少なくとも１つの等式制約および衝突までの距離に関する少なくとも１つの不等式制約は、乗数を掛けて、コスト関数に加算される。さらに、閉鎖運動に関する少なくとも１つの等式制約および衝突までの距離に関する少なくとも１つの不等式制約は、ペナルティパラメータを二乗したものを掛けて、コスト関数に加算することができる。したがって、数値的な解は、拡張ラグランジュ法に基づくことができる。乗数およびペナルティパラメータは、すべての制約について決定され、乗数およびペナルティパラメータ付きの制約が、コスト関数に加算される。この結果が、制御変数に関して基礎となる最小化を、乗数に関して付加的に最大化を図る、制約なしの最大最小最適化問題となる。
【００２９】
好ましくは、乗数およびペナルティパラメータは、動的最適化問題を解くときに反復的に調整され、制約なしの最適化問題は、勾配アプローチで解かれる。
【００３０】
さらなる改良形態において、本方法は、第１軌道の実行を含み、第１軌道の実行は、最適化問題を解くことと並行して行われる。このように、移動を始める前に、必要な計算時間を減らしながら、計画と実行とを並行して行うことができる。実行中に軌道をさらに最適化することにより、頻繁な再計画も回避することができる。
【００３１】
具体的には、並行的計画および実行の場合の動的最適化問題は、移動の持続時間を表す終了時間を有し、数値的アプローチは、第１ステップで、移動の持続時間よりも短い少なくとも１つの第１期間に関する第１部分解を生成し、数値的アプローチは、少なくとも１つの第２ステップで、第２期間に関する第２部分解を生成し、第２期間については、動的最適化問題に対応するとともに初期条件として第１期間の終了時の状態を考慮する、適応最適化問題を使用する。
【００３２】
このように、動作計画は、連続して行われる。閉鎖運動を伴う動作計画は、開放運動を伴うものよりも、はるかに難しく、計算に相当多くの時間を要するため、動作の開始前に、必要な計算時間を連続した並行的計画および実行によって減らすことができる。そのため、軌道を連続的にさらに最適化することが着想である。
【００３３】
動作は、まだ完全には衝突回避になっていなくても、すぐに実行することができる。変化した環境条件に対してさらなる最適化が適応されるため、局所的な回避挙動が自動的に生じる。したがって、移動ホライズンでの最適化で、変化する環境条件に対する軌道を解くことおよび適応させることの、反復的改善が可能である。準最適解での開始が可能になり、それにより、実行中に解がさらに改善される。
【００３４】
さらなる構成において、本方法は、第２マニピュレータのための第２移動経路の決定をさらに含み、コスト関数は、第２マニピュレータの状態および制御変数に追加で重み付けを行い、運動は、第２マニピュレータの状態および制御変数を時間の関数として定義し、第１マニピュレータと第２マニピュレータとが閉鎖運動を形成する。
【００３５】
この改良形態において、２つの軌道を動的最適化問題に基づいてこのように決定し、それに従って２つのマニピュレータは、マニピュレータが閉鎖運動連鎖を形成するように移動することができる。言い換えると、最適化問題は、たとえば物体を共同で動かすことによって互いに連結されている、双腕ロボットの制御を実現する。この改良形態は、有利なことに、双腕または多腕ロボットのコントローラの効率的な実施に寄与する。
【００３６】
特に好ましくは、閉鎖運動に関する少なくとも１つの等式制約は、この場合、第１マニピュレータの第１の動的位置制約および／または第１の動的向き制約、ならびに第２マニピュレータの第２の動的位置制約および／または第２の動的向き制約を備え、これらは互いに定義された決まった関係にあるため、閉鎖運動が実現される。したがって、最適化問題の等式制約は、第１および第２のマニピュレータの位置および／または向きに関して決まった関係によって定義される。
【００３７】
上述した特徴および以下に説明する特徴は、示された組み合わせだけでなく、本発明の範囲を離脱することなく他の組み合わせでまたは単独でも使用できることが理解される。
【００３８】
本発明の実施の形態を図面に表し、以下の説明でさらに詳細に説明する。
【図面の簡単な説明】
【００３９】
【図１】本発明に係る方法の実施の形態を示すフロー図である。
【図２】最適化問題の形式的な表現を示す。
【図３】並行的計画および実行の模式的な表現を示す。
【図４】本発明に係る装置の好適な実施の形態を示す。
【発明を実施するための形態】
【００４０】
図１では、本発明に係る方法の実施例が、全体として参照符号１００で示されている。
【００４１】
当該方法は、ステップ１０２で、動作計画に適切な最適化問題の定式化から始まる。最適化問題の基礎は、可能な状態と制御変数とに関する移動システムの運動を時間の関数として記述するシステムモデルである。さらに、最適化問題は、最適化の目標を規定するコスト関数を含む。コスト関数は、システムモデルのパラメータに重み付けを行う１つまたは複数の関数を含む。
【００４２】
最適化の目標は、コスト関数を最小化する独立変数の関数（通常は時間関数）を見つけることである。言い換えると、動作計画の場合、１つの目標は、マニピュレータを制御するために使用することのできる最適な入力軌跡を見つけることである。これは、最適制御問題ともいう。
【００４３】
この最適化問題は、空間内の自由な移動を制限する制約も含む。この制約は、動作を計画しているときに考慮しなければならない。この制約は、空間内の障害物を考慮する少なくとも１つの不等式制約と、運動制約を考慮する少なくとも１つの等式制約とを含む。
【００４４】
ステップ１０２の結果は、システム、その限界および所望の最適化の形式的な表現である。
【００４５】
次に、ステップ１０４で、最適化問題が解かれる、すなわち、コスト関数を最小化する関数を探す。これは、数値的アプローチを使用して行われ、好ましくはコンピュータ支援される。具体的な解の代わりに、数値的アプローチは、最適化問題の近似計算を可能にする。したがって、明確な解の提示が可能ではない場合、または妥当な期間で解を見つけることができない場合に、解を規定することもできる。
【００４６】
原則として最適化問題を解くことのできる、多数の異なる数値的な方法およびプログラムが知られている。どのアプローチが一番適しているかは、要求される正確さまたは利用できる計算時間を含む、様々な要因による。本発明が特定の数値解アプローチに限定されないことに留意すべきである。
【００４７】
ステップ１０４の結果は、最適解関数または十分な正確さでのこの最適関数の近似化である。
【００４８】
最適解関数またはその近似化に基づいて、ステップ１０６で、第１マニピュレータの具体的な動作経路が決定される。これは、数値的解に基づいて、マニピュレータの移動が定義されることを意味する。最も単純なケースでは、最適化問題の解は、最適入力軌道であり、これは、マニピュレータの動作制御に直接変換することができる。
【００４９】
このように、本発明による方法は、好ましくは、直接的かつ即座に所望の制御を提供し、これは、原則としてマニピュレータの滑らかで自然な移動をもたらす。
【００５０】
以下にさらに詳細に説明されるように、最適化問題は、少なくとも１つの不等式制約で障害物を、少なくとも１つの等式制約で閉鎖運動を考慮するため、本方法は、障害物の周りでの、閉鎖運動についての要求事項に違反することのない、全体的に滑らかで自然なマニピュレータの移動を生みだす。したがって、動作計画の様々な問題（障害物、運動）を十分に考慮することができるので、計画は、反復的に、特に数値的アプローチを通して決定的に行うことができる。計画時間は短く、かつ決定可能であり、動作計画の実時間の実施を可能にする。生じる移動は、自然で、衝突回避であり、閉鎖運動を考慮している。
【００５１】
図２は、動的最適化問題の形式的な表現を示し、ここでは、全体が参照符号１２で示されている。始点は、システムモデルであり、これは、移動システム（たとえばマニピュレータ）の運動１４を、その状態１６および制御変数１８を用いて記述する。運動１４は、状態ｘ＝ｑ、制御変数
【００５２】
【数１】
2021506031.tif 000003
【００５３】
としたときに積分器(integrator)
【００５４】
【数２】
2021506031.tif 000004
【００５５】
として単純化した形で仮定することができる。または、状態
【００５６】
【数３】
2021506031.tif 000005
【００５７】
、制御変数
【００５８】
【数４】
2021506031.tif 000006
【００５９】
としたときに二重積分器
【００６０】
【数５】
2021506031.tif 000007
【００６１】
として、仮定することができる。
【００６２】
時間のパラメータ化のない最適化を用いて１つだけの経路を計算しようとする場合には、簡単な前記積分器がシステムモデルとして十分である。他方、計算された軌道を直接実行しようとする場合には、ここでは最低および最高の加速度を考慮できるため、前記二重積分器を使用するべきである。
【００６３】
コスト関数２０は、状態１６および制御変数１８に重み付けを行う。最適化のタスクは、通常有限数の制御関数から、コスト関数２０を最小化する最適な制御関数を見つけることである。
【００６４】
少なくとも１つの等式制約２２により、計画中にシステムの閉鎖運動を考慮する。閉鎖運動系は、少なくとも１つの閉鎖運動連鎖がある場合に生じる。これは、たとえば、マニピュレータが、異なる２通りの形でその基部に運動学的に連結されている場合である。
【００６５】
閉鎖運動は、互いに連結されている複数のマニピュレータをロボットが有する場合、たとえば、マニピュレータがワークを共同で持ち上げて移動させる場合にも生じる。この場合、各マニピュレータのエンドエフェクタの姿勢は、常にそれぞれ他のマニピュレータのエンドエフェクタの姿勢に関連する。そのため、個々のマニピュレータが互いに取ることのできる姿勢には制約があり、これは、等式制約（Ｐ１（ｘ）Ｐrelativ＝Ｐ２（ｘ））として、２つ１組で表される。このように、等式制約は、動的最適化問題を解くときに考慮しなければならない動的限界を定義する。言い換えると、閉鎖運動の条件は、追加の制約として解くべき最適化問題に流入する。それにより、閉鎖運動は、動作計画で直接考慮され、動作計画自体の不可欠な成分である。
【００６６】
さらに、動的最適化問題１２は、少なくとも１つの不等式制約２４を含み、それにより空間内の障害物を考慮することができる。好ましくは、少なくとも１つの不等式制約は、空間内の障害物との衝突からの最小距離を定める。この衝突距離と、状態に従うその偏微分の計算とが、最適化問題の最も複雑な部分にあたる。
【００６７】
不等式制約２４を用いて、自己衝突までの距離および障害物との衝突までの距離を考慮することが好ましい。自己衝突の距離を計算するために、実施の形態では、ロボットの近似化は、中心と半径とを有する複数のボールを用いてロボットを近似化することとすることができる。障害物との衝突については、ラスタ地図によって環境の近似化を行うことができ、それにより、考慮するべき距離をラスタ地図のユークリッド距離変換から得る。自己衝突および／または障害物との衝突までのいずれかの最小距離を考慮する。
【００６８】
言うまでもなく、上述の制約に加えて、動的最適化問題１２で別の境界条件２６および制約を扱うこともできる。具体的には、可能な状態の上限と下限とを定義する別の不等式制約２４を想定してもよい。たとえば、最小および最大の関節角度の限界を、最適化問題によって定義し考慮することができる。さらに、制御変数の最小値および最大値、具体的には第１マニピュレータの最低および最高の速度および／または加速度を定義する制御変数が考えられる。
【００６９】
好適な実施の形態では、定義された終了時間Ｔによって定義される、移動の持続時間も考慮してもよい。終了時間Ｔは、開始状態と終了状態とによって定めることができ、または別の変数として最適化の成分とすることができる。後者の場合、コスト関数２０によって時間に重み付けを行い、好ましくは最小値と最大値とを定義する。最適化の決定的な挙動は、終了時間Ｔを考慮することによって達成することができる。
【００７０】
ここで提示する最適化問題１２を解くことは、異なる方法で、および異なる数値的アプローチによって達成することができる。好適な実施の形態において、最適化問題１２の数値解は、勾配アプローチおよび拡張ラグランジュ法によって達成される。ここで、数値的アプローチの核心は、制約付きの動的最適化問題を制約なしの最適化問題に変換することである。言い換えると、最適化問題は、制約なしの補助関数の最適化へと、単純な形に変換される。変換のために、制約は、因子（いわゆるラグランジュ乗数）を掛けて、目的関数に埋め込まれる。制約の違反に罰則を与えるために、正のラグランジュ乗数を使用する。
【００７１】
拡張ラグランジュ法は、二次の強化関数をさらに含み、これは、追加の乗数を使用して目的関数に埋め込まれる。
【００７２】
得られる補助関数は、拡張ラグランジュ関数とも呼ばれ、ラグランジュ乗数をμ、ペナルティパラメータをρとしたとき、以下のとおりである。
【００７３】
【数６】
2021506031.tif 000008
【００７４】
式の前の部分ｆ（ｘ）＋μｇ（ｘ）は、通常のラグランジュ関数に対応し、後の部分
【００７５】
【数７】
2021506031.tif 000009
【００７６】
は、二乗の強化関数に対応する。最適解ｘ＊，μ＊の強双対性と存在とを仮定すると、各ρ≧０について鞍点条件
Ｌ（ｘ＊，μ，ρ）≦Ｌ（ｘ＊，μ＊，ρ）≦Ｌ（ｘ，μ＊，ρ）
を満たさなければならないことが分かる。このことから、ラグランジュ関数は、変数ｘに関して最小化し、ラグランジュ乗数μについて最大化しなければならない、すなわち、新たな最適化問題
【００７７】
【数８】
2021506031.tif 000010
【００７８】
を解かなければならないことは明らかである。
【００７９】
この問題を解くために、まず、基礎となる最小化問題
【００８０】
【数９】
2021506031.tif 000011
【００８１】
を反復ｋにより定義し、これは、勾配アプローチにより解かれる。その後、ラグランジュ乗数は、ペナルティパラメータρ（ｋ）をステップサイズとして、最急上昇(steepest ascent)
μ（ｋ）＝μ（ｋ）＋ρ（ｋ）ｇ（ｘ（ｋ））
によって更新される。この手順の収束は、ペナルティパラメータを適応することにより大幅に改善することができる。因子βinc＞１，βdec＜１，γinc  および  γdec＜１βinc＞１，βdec＜１、絶対許容値をεｇとして、ヒューリスティクス
【００８２】
【数１０】
2021506031.tif 000012
【００８３】
を試行およびテストした。
【００８４】
最小化により制約の十分な改善が得られない場合、ペナルティパラメータをさらに増やす。制約が必要許容値と合致したらすぐに、ペナルティパラメータを再び減らすことができる。最大反復数ｋ＝Ｎmultに到達するまで、または最小化が収束してすべての制約が満足するまで、この手順を繰り返す。
【００８５】
不等式制約ｈ（ｘ）≦０は、スラック変数ｚ≧０を導入することにより、等式制約ｇ（ｘ）＝ｈ（ｘ）＋ｚ＝０に変換することができる。ｚに関する最小化は、明確に計算することができ、
【００８６】
【数１１】
2021506031.tif 000013
【００８７】
になる。ｇ（ｘ）に挿入すると、スラック変数のない、ラグランジュ関数に使用することのできる等価式が以下のように得られる。
【００８８】
【数１２】
2021506031.tif 000014
【００８９】
絶対量のない不等式制約について、ペナルティパラメータの適応規則が成立する。
【００９０】
等式制約および不等式制約がいくつかある場合、制約ごとに１つのラグランジュ乗数がある。ペナルティパラメータは、すべてに均一か、またはラグランジュ乗数として、各制約に別々かのいずれかを選択することができる。手順のロバストさを改善するために、最小化で十分な収束がまだ得られていない場合には、乗数の更新をスキップすることができる。さらに、ラグランジュ乗数およびペナルティパラメータは、最小値と最大値とによって制限することができる。
【００９１】
詳しくは、勾配アプローチは、初期制御変数ｕ（０）（ｔ）から始めて、各反復ｊで以下のステップを行う。
・初期条件から始まるシステムを順時間で積分して、ｘ（ｊ）（ｔ）を求める。
・終了条件から始まる随伴系を逆時間で積分して、λ（ｊ）（ｔ）を求める。
・基礎となる直線探索問題を解く。
【００９２】
【数１３】
2021506031.tif 000015
【００９３】
ここで、探索の方向は、以下のとおりである。
【００９４】
【数１４】
2021506031.tif 000016
【００９５】
制御変数制約は、許容可能な数［ｕ−，ｕ＋］にψを投影して考慮する。
・新たな制御変数ｕ（ｊ＋１）＝ψ（ｕ（ｊ）−α（ｊ）ｇ（ｊ））を計算して、ｊ＝ｊ＋１を設定する。
・ｊ＝Ｎgradで、または収束基準に達したときに、勾配手順を停止する。
【００９６】
ここに示す勾配アプローチは、比較的簡単に実施することができる。個々の反復には、わずかな計算時間しか必要なく、アプローチに必要なメモリも低い。特に手順の始めに、通常、コスト関数の大幅な簡約化があるので、準最適解がすぐに見つかる。しかし、高い正確さを得るためには、二次情報（ヘッセ行列；Hesse matrix）を使用する方法と比較して、多くの反復を必要とする。
【００９７】
好適な実施の形態において、計画は、実際の実行と並行して、すなわち、マニピュレータの実際の移動と並行して行われる。この並行的計画および実行の原理を図３に示す。
【００９８】
並行的計画および実行は、移動の開始前に必要な計算時間を減らすとともに、頻繁な再計画の必要性をなくすことができる。軌道は、実行中にさらに最適化される。したがって、軌道は、まだ完全には衝突回避になっていなくても、すぐに実行することができる。加えて、さらなる最適化によって、変化する環境条件に軌道を適応させることができるので、局所的な回避挙動が自動的に達成される。
【００９９】
第１の時間ステップで、ｑＳからｑＧまでの移動が計画される（Ａ）。第２の時間ステップで、この軌道の最初からｑ１＝ｑ（ΔＴ）までが実行される（Ｂ）と同時に、ｑ１からｑＧまでの移動がさらに最適化される（Ｃ）。ロボットが位置ｑ１に到達したとき、軌道の次の部分がｑ２＝ｑ（２ΔＴ）まですぐに実行され（Ｄ）、この地点から最適化が継続する（Ｅ）。ある時間ステップ内で目標地点ｑＧに到達するまでこのアプローチを繰り返す（Ｆ）ので、さらなる最適化は必要ない。
【０１００】
このように、実際の実施には、計画と実行との密接な相互作用を必要とする。各計画ステップ（Ａ，Ｃ，Ｄ，Ｆ）は、サンプリング時間Δｔ内にその計算を終了しなければならない。時間ステップｔｋ＝ｋΔｔで、ｑｋからｑＧまでの計算済みの軌道は、少なくとも次のサンプリング時間中は、許容可能でなければならない、すなわち、ｑｋ→ｑｋ＋１は、衝突回避であるとともに、閉鎖運動に適合しなければならない。しかし、実行（Ｂ，Ｄ）は、最後の移動ｑｋ−１→ｑｋが完了しなければ始まらないため、ロボットは、転移点でちょうど位置ｑｋになる。たとえば、転移点で速度に急激な変化がある場合，問題が生じるおそれがある。これは、フォローアップ制御を使用して補償しなければならない。
【０１０１】
予測的計画の場合、冒頭で提示した最適化問題は、時間ステップｔｋで以下のように調整される。
【０１０２】
【数１５】
2021506031.tif 000017
【０１０３】
ここで
【０１０４】
【数１６】
2021506031.tif 000018
【０１０５】
および
【０１０６】
【数１７】
2021506031.tif 000019
【０１０７】
は、実際の状態ｘ（ｔ）と区別するために、時間τ∈［０，Ｔ］に関するオプティマイザの内部変数を表す。
【０１０８】
時間ステップｔｋでは将来の状態ｘ（ｔｋ＋Δｔ）に基づいて計画しなければならないため、初期条件は、特に最適化問題に関して変化している。さらに、終了時間Ｔは、限界付きおよびコスト関数による重み付け付きの別の最適化変数として考慮することができる。
【０１０９】
目標位置ｑＧが結局ｔ→∞に漸近的に達しないことを確実にするために、最適化問題で確固とした最終的な制約ｑ（Ｔ）＝ｑＧを設定しなければならず、時間ホライズンＴは、あるステップで目標地点Ｔ＝Δｔにちょうど到達するまで、時間ホライズンＴを連続的に短縮しなければならない。このために、必要な時間の初期の見積りに基づいて、各時間ステップの後に、最終時間ＴをΔｔ分だけ短縮する、すなわち、Ｔ（ｋ＋１）＝Ｔ（ｋ）−Δｔ。時間最適挙動および変化した環境条件への適応を実現するために、自由最適化変数として最終時間Ｔを考慮しなければならない。
【０１１０】
特に閉鎖運動を伴う計画を行うときに、第１ステップで許容可能な軌道を計算するのにサンプリング時間Δｔが十分ではないということが起こる可能性がある。そのため、第１ステップを行う前に、より多くの時間を最適化に投じなければならない。このために、この事前計算の最大数の反復Ｎｏｃｐを制限することができ、または時間Ｔvalidate＜Ｔを予め定義することができる。この場合、軌道の対応する部分が許容されるときにのみ実行が開始する。たとえば、移動は、次の瞬間にすでに衝突回避でなければならないと要求することができる。
【０１１１】
図４は、対応する装置の好適な実施の形態を示す。ここでは、装置は全体として参照符号１０で示されており、この実施の形態では、第１マニピュレータ２８と第２マニピュレータ３０とを備える双腕ロボットとして図示されている。マニピュレータ２８，３０は、共通の基部３１で互いに連結されており、対称的に構成されている。各マニピュレータ２８，３０は、３つのリンク３２と３つの関節３４とを有し、各マニピュレータ２８，３０の端部には、エンドエフェクタ３６があり、この実施例では、これはグリッパである。２つのマニピュレータ２８，３０は、ワーク３８を共同で握って移動させることができる。ワーク３８を介した連結が閉鎖運動連鎖４０を作り、ここでは、これを点線で示す。
【０１１２】
２つのマニピュレータ２８，３０の移動中、したがってその移動計画中にも、この閉鎖運動を考慮しなければならない。そうでなければマニピュレータ２８，３０が、ワーク３８を落としてしまうからである。
【０１１３】
第１マニピュレータ２８および第２マニピュレータ３０の移動は、制御ユニット４２によって協調させられる。制御ユニット４２は、前述した方法を実行する。すなわち、制御ユニット４２は、閉鎖運動を考慮して、第１マニピュレータ２８および第２マニピュレータ３０の両方の協調した移動経路を決定する。したがって、制御ユニット４２は、基礎となる最適化問題を数値的に解くように構成される。
【０１１４】
好ましくは、制御ユニット４２は、計画および実行を並列して行う。これは、マニピュレータ２８，３０が、最適で完全な動作計画が起こる前であってもすでに移動することを意味する。
【０１１５】
閉鎖運動に加えて、制御器４２は、マニピュレータ２８，３０の移動範囲に位置している障害物４４も考慮する。特に、移動空間の静的な障害物だけでなく、移動する障害物も考慮する。
【０１１６】
連続的な最適化を含む並行的計画および実行により、有利なことに、変化する環境を考慮することができる。
【図１】
2021506031.tif 000020
【図２】
2021506031.tif 000021
【図３】
2021506031.tif 000022
【図４】
2021506031.tif 000023
【国際調査報告】
2021506031.tif 000024
2021506031.tif 000025
2021506031.tif 000026
2021506031.tif 000027
2021506031.tif 000028
2021506031.tif 000029