JP2023513841A - 産業ロボット用の衝突回避モーション計画方法 - Google Patents

Abstract

最悪状態サーチ及び最適化を使用するロボット衝突回避モーション計画技法である。モーション計画技法は、障害物、開始及びゴールポイント、及び疎らに離隔し得るウェイポイントの初期の組の幾何学的定義によって始まる。線形又はスプラインなどのポイント間補間方法が付与された場合に、連続的軌跡をウェイポイント及び弧長パラメータの関数として記述できる。次いで、ロボット及びツールのすべての部分を考慮することにより、障害物までの距離の最悪状態を有するウェイポイントのそれぞれの隣接するペアの間の場所を見出す最悪状態サーチが実行される。衝突回避制約が最悪状態場所を使用して定義され、且つ、最悪状態を改善するために、すべての衝突が除去され且つ障害物回避最小距離基準が充足される時点まで、ウェイポイント場所の最適化が実行される。

Description

関連出願の相互参照
本出願は、２０２０年２月１９日付けで出願された「ＣＯＬＬＩＳＩＯＮ ＡＶＯＩＤＡＮＣＥ ＭＯＴＩＯＮ ＰＬＡＮＮＩＮＧ ＭＥＴＨＯＤ ＦＯＲ ＩＮＤＵＳＴＲＩＡＬ ＲＯＢＯＴ」という名称の米国仮特許出願第６２／９７８，６５４号の優先日の利益を主張するものである。

本開示は、産業ロボットのモーション制御の分野に関し、且つ、更に詳しくは、障害物及び疎らに離隔し得るウェイポイントの初期の組の定義によって始まり、且つ、障害物までの距離の最悪状態を有するウェイポイントのそれぞれの隣接するペアの間の場所を見出し、且つ、次いで、最小障害物回避距離基準が充足される時点まで最悪状態を改善するためにウェイポイント場所の最適化を実行するロボット衝突回避モーション計画技法に関する。

様々な製造、組立、及び材料移動動作を実行するための産業ロボットの使用が周知である。多くのロボット作業空間環境においては、障害物が存在しており、これらは、ロボットのモーションの経路内に位置し得る。障害物は、機械及び備品などの永久的な構造物であってもよい。また、障害物は、操作されつつあるワークピース又は部品が配置されつつあるコンテナであってもよく、この場合に、ロボットは、動作を実行しつつ、ワークピース又はコンテナをその内外において操作しなければならない。ロボットと任意の障害物の間の衝突は、絶対に回避しれなければならない。

ロボット衝突回避モーション計画用の従来技術の技法は、通常、軌跡に沿ってウェイポイントの組を定義するステップと、ウェイポイントのそれぞれによって表されている別個の状態において衝突又は過剰に小さなクリアランスについてチェックするステップと、を伴っている。これらの技法においては、予め定義されたウェイポイントの間に細い障害物がたまたま存在している場合に、細い障害物との間の衝突がモーション計画において見逃されうる可能性がある。細い障害物が見逃されないことを保証するための唯一の確実な方法は、高密度のウェイポイントの組を定義するというものであり、この場合には、ウェイポイントの間の間隔は、細い障害物による占有を許容するほどに十分大きなものではない。

そして、従来技術の技法において必要とされている高密度のウェイポイント間隔の使用は、その他の問題を生成する。高密度のウェイポイント間隔に伴う１つの問題は、逆運動学的なロボット構成演算及び衝突回避最適化計算をそれぞれのウェイポイントごとに実行しなければならないことから、モーション計画演算時間が増大するという点にある。高密度のウェイポイント間隔に伴う別の問題は、曲率ブレンディング用の減速がしばしばそれぞれのウェイポイントの近傍において必要とされることから、不均等な又はぎくしゃくしたロボットモーションがしばしば生成されるという点にある。

上述の状況に鑑み、高密度のウェイポイント間隔を必要としないが依然としてロボットの軌跡に沿って任意の衝突又は最小限距離閾値違反を確実に識別し且つ自動的に解決する改善されたロボットモーション最適化技法に対するニーズが存在している。

本開示の教示内容によれば、最悪状態サーチ及び最適化を使用したロボット衝突回避モーション計画技法が開示されている。モーション計画技法は、障害物、開始及びゴールポイント、並びに、疎らに離隔し得るウェイポイントの初期の組の幾何学的定義によって始まっている。線形又はスプラインなどのポイント間補間方法が付与された状態において、連続的軌跡をウェイポイント及び弧長パラメータの関数として記述することができる。ロボット及びツールのすべての部分を考慮することにより、障害物までの距離の最悪状態を有するウェイポイントのそれぞれの隣接するペアの間の場所を見出す最悪状態サーチが実行されている。最悪状態場所を使用することにより、衝突回避制約が定義され、且つ、すべての衝突が除去され、且つ、障害物回避最小距離基準が充足される時点まで最悪状態を改善するためにウェイポイント場所の最適化が実行されている。

この開示されている装置及び方法の更なる特徴については、添付図面との関連において提供される以下の説明及び添付の請求項から明らかとなろう。

図１は、障害物の近傍において動作している産業ロボットの図であり、この場合に、従来の衝突回避モーション計画技法は、十分高密度なウェイポイント間隔が使用されない場合には、障害物衝突の識別に失敗し得るであろう。 図２は、車両ボディ構造の近傍において動作している産業ロボットの図であり、この場合には、車両ボディワークピース自体が、回避されなければならない障害物を表しており、且つ、従来の衝突回避モーション計画技法は、この場合にも、高密度のウェイポイント間隔を使用しなければならない。 図３は、本開示の一実施形態による障害物環境を通じたウェイポイントのシーケンスの図であり、この場合に、最悪状態サーチは、障害物との関係において最悪状態距離を有するウェイポイントのそれぞれのペアの間の軌跡上の場所を識別している。 図４は、本開示の一実施形態による図３の初期ウェイポイント及び軌跡の図であり、この場合に、最悪状態最適化は、衝突回避基準が充足される時点まで最悪状態を改善するためにウェイポイント場所を移動させている。 図５は、本開示の一実施形態による最悪状態に基づいて最悪状態サーチ及びウェイポイント最適化を使用するロボット衝突回避モーション計画用の方法のフローチャート図である。 図６は、図５の方法を使用するように構成されたロボット衝突回避モーション計画システムの図である。

最悪状態サーチ及び最悪状態最適化を使用するロボット衝突回避モーション計画技法を対象とした本開示の実施形態に関する以下の説明は、その特性が例示を目的としたものに過ぎず、且つ、決して、開示されている装置及び技法或いはその用途又は使用を限定することを意図したものではない。

様々な製造、組立、及び材料移動作業のために産業ロボットを使用することが周知である。多くのロボット作業空間環境においては、障害物が存在しており、これらは、ロボットのモーションの経路内に位置し得る―即ち、障害物は、ロボットが現時点において位置決めされている場所とロボットの目的地位置の間に配置され得る。障害物は、機械、備品、及びテーブルなどの構造物であってもよく、且つ、ロボットは、動作を実行しつつワークピース又はコンテナをその内外において操作しなければならないことから、ロボットによってその内外において操作されているワークピース又はコンテナは、それ自体が障害物となり得る。当技術分野においては、ツールが任意の障害物との間のロボットの衝突を回避しつつ経路を辿るように、ロボットモーションを演算するための技術が開発されている。

図１は、障害物の近傍において動作している産業ロボットの図であり、この場合に、従来の衝突回避モーション計画技法は、十分高密度のウェイポイント間隔が使用されていない場合には障害物衝突の識別に失敗し得るであろう。ロボット１００は、第１コンテナ１１０から第２コンテナ１２０にワークピースを移動させるほどに簡単なものであり得る動作を実行しつつある。ディバイダ１３０がコンテナ１１０及び１２０の間に配置されている。コンテナ１１０及び１２０の側壁並びにディバイダ１３０は、いずれも、ロボット１００のすべての部分によって回避されなければならない障害物を提示しており、これには、ロボットアーム、ツール（フィンガタイプのグラスパ又はサクションカップグリッパであり得るであろう）、及び移動させられているワークピースそれ自体のすべてが含まれている。

開始ポイント１４０及びゴールポイント１４２は、ワークピースが移動する経路上において、それぞれ、開始及び終了ポイントを表している。上述のタイプのワークピースのピックアップ及び配置動作においては、ビジョンシステム（カメラ又はその他のセンサ―図示せず）が、移動させられる次のワークピースのコンテナ１１０内における場所を識別することが一般的である。また、コンテナ１２０内のコンパートメントが個々に充填されるのに伴って、ワークピースの目的地場所が１つのものから次のものへと異なっていることも一般的である。従って、開始ポイント１４０及びゴールポイント１４２の両方は、移動させられるそれぞれのワークピースごとに一意に識別されており、これは、それぞれのワークピースごとに一意の経路を演算しなければならないことを意味している。開始ポイント１４０及びゴールポイント１４２は、障害物（コンテナ１１０及び１２０並びにディバイダ１３０）の幾何学的定義と共に、モーション計画計算用の入力として提供されている。

ワークピースの経路に沿っていくつかのウェイポイントを定義することが知られており、この場合に、ウェイポイントの初期基準場所は、例えば、開始ポイント１４０からゴールポイント１４２までの直線に沿って均等に分散させることができる。従来技術のモーション計画システムにおいては、ロボット状態が、それぞれのウェイポイントにおいて演算され、且つ、衝突回避演算が、それぞれのウェイポイント状態において実行されている。但し、ウェイポイントが疎らに離隔している場合には、従来技術のモーション計画システムは、ウェイポイントの間に存在している干渉状態の識別に失敗し得る可能性がある。以下の説明においては、図１を更に参照しつつ、この状況について示すこととする。

軌跡１５０は、開始ポイント１４０とゴールポイント１４２の間において直線として定義されている。開始ポイント１４０及びゴールポイント１４２に加えて、ウェイポイント１５２及びウェイポイント１５４という２つのウェイポイントが定義されている。従来の最適化に基づいたモーション計画技法においては、初期基準経路が定義され、ロボット状態及び干渉が演算され、且つ、新しい経路が定義されており、これが、衝突が経路に沿った任意のウェイポイントにおいて存在していない経路が見出される時点まで繰り返し反復されている。但し、軌跡１５０のケースにおいては、衝突がウェイポイント１５２又はウェイポイント１５４において発生しないと判定される場合があり、且つ、モーション計画システムは、直線軌跡１５０が開始ポイント１４０からゴールポイント１４２までの適切な経路であると結論付けることになろう。ロボット外側アーム及び／又はツールが、コンテナ１１０の側壁、ディバイダ１３０、及びコンテナ１２０の側壁と衝突することになることから、これは、明らかに受け入れ可能な解決策ではない。

上述の状況が発生する理由は、従来技術のモーション計画システムが、ウェイポイントによって表された別個の状態のみを考慮しており、且つ、ウェイポイントの間において発生し得る衝突についてチェックしていないからである。上述の誤った解決策を回避するために、従来技術のモーション計画システムのユーザーは、高密度のウェイポイントの組を定義することが一般的である。例えば、軌跡１５０において使用されている４つのポイント（開始点１４０、ゴールポイント１４２、及び２つの中間ウェイポイント）の代わりに、軌跡１６０は、合計で１０個のポイント（開始ポイント１４０、ゴールポイント１４２、及び８つの中間ウェイポイント）を使用して定義されている。中間ウェイポイント（１６２、１６４、１６６、など）が、当初、開始ポイント１４０とゴールポイント１４２の間において直線状に配置されている場合にも、衝突が別個のウェイポイント状態のいくつかにおいて識別されることになり、且つ、最適化ルーチンは、開始ポイント１４０とゴールポイント１４２の間のすべての障害物をアーチ状に回避する軌跡１６０に収束する時点まで、ウェイポイントが上方に運動するようにすることになる。

図２は、車両ボディ構造の近傍において動作している産業ロボットの図であり、この場合には、車両ボディワークピース自体が、回避されなければならない障害物を表しており、且つ、従来の衝突回避モーション計画技法は、この場合にも、高密度のウェイポイント間隔を使用しなければならない。ロボット２００は、車両ボディ構造２１０上においてスポット溶接などの動作を実行しつつある。開始ポイント２２０は、ロボット２００用のホーム場所であり、且つ、ゴールポイント２２２は、（例えば、ボディ構造２１０の内側に配置された部分上の）溶接が実行される場所である。ボディ構造２１０のルーフレール２１２は、開始ポイント２２０とゴールポイント２２２の間の場所を直接的に占有している。

上述の図１の説明に基づいて、直線軌跡２３０が衝突を伴わないものであると判定され得る方式を想定することは容易である。軌跡２３０が、開始ポイント２２０とゴールポイント２２２の間において（５つなどの）適度な数の中間ウェイポイントによって定義されている場合にも、従来技術のモーション計画システムは、衝突の検出に失敗することになる可能性が非常に高く、その理由は、衝突回避計算が別個のウェイポイント状態においてのみ実行され、且つ、細いルーフレール２１２がウェイポイントの間を通過し得るからである。

十分に大きな数のウェイポイントが定義された場合には、従来技術のモーション計画システムは、ルーフレール２１２との間の潜在的な衝突を識別することになる。衝突回避制約は、最適化ルーチンがウェイポイント用の異なる場所を試みるようにすることになり、且つ、ウェイポイントは、最終的に、（ロボット２００のベースとボディ構造２１０の間において）ボディ構造２１０の外側を通過する且つゴールポイント２２２に曲がって戻る軌跡２４０に収束することになる。

従来技術のモーション計画システムにおいて確実な衝突回避のために必要とされる高密度のウェイポイント間隔は、２つの理由から問題である。第１に、最適化演算（逆運動学的ロボット構成及び干渉チェック計算を含む）は、ウェイポイントの数が増大するのに伴って、益々複雑化し且つ時間を所要することになる。第２に、高密度のウェイポイント間隔によってロボットツールのスムーズなモーションを実現することは、困難又は不可能であり、その理由は、それぞれのウェイポイントにおける経路セグメント遷移が、通常、遷移において必要とされる曲率に起因してツール中心点の減速を生成するからである。

以下は、高密度のウェイポイント間隔を必要としないが、連続的軌跡に沿ったウェイポイントの間の衝突を確実に識別し、且つ、次いで、衝突を伴っておらず且つ障害物回避最小距離要件を充足している軌跡を提供するためにウェイポイント場所を最適化する衝突回避モーション計画方法の説明である。本開示の技法によれば、この方法は、最悪干渉状態を有する連続的軌跡上の場所を識別するための２ステップ最悪状態サーチを含み、これには、衝突回避要件が充足される時点まで最悪状態を改善するためにウェイポイントを移動させる２ステップ最悪状態最適化が後続している。

図３は、本開示の一実施形態による障害物環境を通じたウェイポイントのシーケンスの図であり、この場合に、最悪状態サーチは、障害物との関係において最悪状態距離を有するウェイポイントのそれぞれのペアの間の軌跡上の場所を識別している。図３は、後述する図４と共に、わかりやすさを目的として単純に２次元（２Ｄ）として示されている。実際の一実装形態においては、障害物、並びに、開始及びゴールポイント、ウェイポイント、及び最悪状態ポイントの座標は、いずれも３次元において定義されており、且つ、計算のすべては、相応して処理されている。また、ロボット及びワークピースは、簡潔性及びわかりやすさを目的として図３及び図４から省略されている。図３及び図４の軌跡及びウェイポイントは、最終的に、動作を実行するためにロボットによって使用されており、この場合に、最終的な軌跡は、図１及び図２において示されている方式でロボットのツール中心点が辿る経路であることを理解されたい。

図３において、障害物３１０、３１２及び３１４は、ロボット及びワークピースによって回避されなければならない任意のタイプの物体を表している。図１及び図２との関係において上述したように、障害物３１０／３１２／３１４は、構造物、ツール、備品、又は大きなワークピース自体でありうるであろう。また、ロボット作業空間内の人間操作者も、障害物として定義された安全ゾーン内において配置されうるであろう。

開始ポイント３２０及びゴールポイント３２２が定義されている。図３及び図４に示されているそれぞれのポイントの座標は、ロボットの作業セル座標フレームなどの共通固定座標フレーム内のデカルト空間内において又は更に後述するロボットの関節位置の観点において定義されている。開始ポイント３２０及びゴールポイント３２２の座標は、一部分がその間において移動される位置などの動作が実行される要件を表しており、且つ、従って、開始ポイント３２０及びゴールポイント３２２の場所を変更することはできない。従って、開始ポイント３２０及びゴールポイント３２２の場所は、後述する最適化計算において等式制約として取り扱われている。ウェイポイントの初期の組（３３２、３３４、及び３３６）は、一般的には、開始ポイント３２０とゴールポイント３２２の間の場所において定義されている。ウェイポイント３３２／３３４／３３６の初期場所は、任意の適切な方式で定義されていてもよく、これには、教示ペンダントなどの装置を使用した人間操作者による定義、類似の開始及びゴールポイントを有する予め演算された経路に基づいたコンピュータによる自動選択、などが含まれている。

図３は、状態パラメータ化及び最悪状態サーチという開示されている方法の２つのステップを描いている。以下、これら２つのステップのそれぞれについて説明する。状態パラメータ化は、開始ポイント３２０、ゴールポイント３２２、及びウェイポイント３３２／３３４／３３６の関数として連続的軌跡３３０を定義するステップを伴っている。いくつかの専門用語が以下のように確立されている。軌跡３３０が通過するすべてのポイントの組全体は、ｑ^ｒとして定義されている。即ち、ｑ^ｒは、開始及びゴールポイント３２０／３２２及びウェイポイント３３２／３３４／３３６を含む。換言すれば、ｑ^ｒ＝｛ｑ_０，ｑ_１，ｑ_２，ｑ_３，ｑ_４｝であり、この場合に、開始ポイント３２０は、ｑ_０であり、ゴールポイント３２２は、ｑ_４であり、且つ、ウェイポイント３３２／３３４／３３６は、それぞれ、ｑ_１、ｑ_２、ｑ_３である。ｑ_１などのポイントのそれぞれは、デカルト空間（ｘ／ｙ／ｚ座標及びロール／ピッチ／ヨー）内において又は関節空間（Ｊ_１、Ｊ_２、．．．Ｊ_ｎｒであり、この場合に、ｎ_ｒは、ロボットの自由度である）内において定義されている。

この結果、連続的軌跡３３０は、関数ｇ（ｑ^ｒ，α）として定義され、この場合に、α∈［０，１］は、弧長パラメータであり、ｑ^ｒは、上述のように、すべてのエンドポイント及びウェイポイントの組｛ｑ_０，ｑ_１，ｑ_２，ｑ_３，ｑ_４｝である。上付き文字ｒは、「基準」を意味しており、これは、後述する最適化計算における初期の又は以前の値を通知している。図３において示されるように、ｑ_１においてα＝１／４となり、ｑ_２においてα＝２／４となり、ｑ_３においてα＝３／４となり、且つ、ｑ_４においてα＝４／４となるように、ウェイポイントは、連続的軌跡３３０に沿って均等に離隔している。関数ｇを定義するためには、軌跡補間方法を選択しなければならない。例えば、軌跡３３０は、線形補間として定義することが可能であり、この場合に、軌跡は、ｑ_１からｑ_２までの直線、ｑ_２からｑ_３までの別の直線、などの形態を有する。ウェイポイントの組用の補間方法の選択については、ロボットモーションプログラミングの技術分野において既知である。

軌跡３３０の線形補間のケースにおいては、ｑ_１からｑ_２までのセクションにおける関数ｇは、次式によって評価され、
ｇ（ｑ^ｒ，α）＝（１－α）ｑ_１＋αｑ_２ （１）
ここで、すべての変数は、以上において定義されているとおりである。関数ｇは、軌跡３３０のすべてのその他のセクション（ｑ_２からｑ_３まで、など）において類似の方式で評価することができる。

使用され得る別の軌跡補間方法は、スプライン補間である。軌跡３３０のスプライン補間の一例においては、ｑ_１からｑ_２までのセクションにおける関数ｇは、次式によって評価されており、
ｇ（ｑ^ｒ，α）＝（ｋ_３α^３＋ｋ_２α^２＋ｋ_１α＋１）ｑ_１＋（ｍ_３α^３＋ｍ_２α^２＋ｍ_１α）ｑ_２ （２）
この場合に、ｋ及びｍは、スプライン軌跡の形状に影響を及ぼす定数であり、且つ、この場合にも、関数ｇは、軌跡３３０のすべてのその他のセクション（ｑ_２からｑ_３まで、など）において類似の方式で評価することができる。また、当業者によって理解されるように、線形及びスプライン以外のその他のタイプの軌跡補間を選択することもできる。

以上の説明は、開示されている方法の状態パラメータ化ステップを完成させている。要すれば、補間方法（線形、スプライン、など）及びウェイポイントｑ^ｒが付与されたら、連続的軌跡３３０を関数ｇ（ｑ^ｒ，α）によって記述することが可能であり、この場合に、αは、弧長パラメータである。

こちらも図３に示されているプロセスの次のステップは、最悪状態サーチである。最悪状態サーチは、障害物３１０／３１２／３１４の１つのものとの間の干渉の最悪量又は障害物３１０／３１２／３１４の１つまでの最小距離であり得る障害物との関係における最悪状態距離を有するウェイポイントｑ^ｒのそれぞれの隣接するペアの間における軌跡３３０に沿った場所を見出すステップを伴っている。軌跡３３０の場合に、最悪状態ポイント３５２、３５４、３５６、及び３５８は、それぞれ、軌跡３３０の第１、第２、第３、及び第４セクションの最悪状態である。最悪状態ポイントのそれぞれは、ｑ_{ｗｏｒｓｔ，ｉ}として表記され、この場合に、ｉ＝（１，．．．，４）である。

最悪状態を見出すためには、ウェイポイントｑ^ｒを固定し、且つ、１次元最小化問題を次式のように定式化することが可能であり、

ここで、α_ｉ ^＊は、最悪状態が発生するセクションｉにおける場所αであり、且つ、距離関数Ｄｉｓｔ（ｇ（ｑ^ｒ，α），Ｏｂｓ）は、関数ｇによって定義された軌跡３３０上の特定のポイントαについてロボットのすべての部分と障害物３１０／３１２／３１４の間の最小距離（即ち、最大干渉）を演算することにより、評価されており、この場合に、αは、セクションｉの下限（ｌｂ）からセクションｉの上限（ｕｂ）までの範囲をとることが許容されている。従って、最小化問題は、最悪状態が発生するセクションｉ内の場所α_ｉ ^＊に収束する。ロボットのすべての部分と障害物３１０／３１２／３１４の間の最小距離（即ち、最大干渉）は、ロボットアーム及びツールのそれぞれのものの周りにおいて幾何学的プリミティブを確立し、且つ、例えば、プリミティブから障害物までの距離を算出するなどのように、任意の適切な技法を使用して軌跡３３０上のそれぞれのポイントにおいて算出することができる。

式（３）において定義されている最小化問題は、軌跡セクションのそれぞれごとに解かれている。図３の軌跡３３０の場合には、α_ｉ ^＊は、セクションのそれぞれｉ＝（１，．．．，４）ごとに見出されている。図３において、関数ｇは、軌跡３３０の有意な曲率にみられるように、スプライン補間を使用している。

この結果、付与されたウェイポイントの初期の組ｑ_ｒにおける最悪状態は、次式のようにポイントα_ｉ ^＊において関数ｇを評価することにより、識別され、
ｑ_{ｗｏｒｓｔ，ｉ}＝ｇ（ｑ^ｒ，α_ｉ ^＊） （４）
ここで、ｑ_{ｗｏｒｓｔ，ｉ}は、セクションｉ内の最悪状態を有するポイントであり、且つ、式（４）のすべてのその他の変数は、上述のとおりである。

ｉ＝（１，．．．，４）の場合の式（４）からの最悪状態ポイントｑ_{ｗｏｒｓｔ，ｉ}は、図３に示されている且つ上述した最悪状態ポイント３５２、３５４、３５６、及び３５８である。最悪状態ポイント３５２（ｑ_{ｗｏｒｓｔ，１}）は、障害物３１０との間の最大干渉を有する。最悪状態ポイント３５４は、障害物３１２までの最小距離を有する。最悪状態ポイント３５６及び３５８は、いずれも、障害物３１４までの最小距離を有する。

図３及び以上の説明は、以下のように要約される。状態パラメータ化ステップにおいて、補間方法（線形、スプライン、など）及びウェイポイントｑ^ｒが付与された場合に、連続的軌跡３３０は、関数ｇ（ｑ^ｒ，α）によって記述され、この場合に、αは弧長パラメータである。次いで、最悪状態サーチステップにおいて、連続的軌跡内の最悪状態は、式（３）及び式（４）を使用して見出される。

図４は、本開示の一実施形態による図３のウェイポイントｑ^ｒ及び軌跡３３０の図であり、この場合に、最悪状態最適化は、衝突回避基準が充足される時点まで最悪状態を改善するためにウェイポイント場所を移動させている。最悪状態最適化は、衝突回避制約とウェイポイント場所の間の関係を見出すステップと、これに後続する最悪状態最適化自体と、という２つのステップから構成されている。これらのステップは、いずれも、図４に描かれており、以下、これらについて説明する。

場所α_ｉ ^＊が固定された状態において、式（４）からのｑ_{ｗｏｒｓｔ，ｉ}の値が付与された場合に、最悪状態ポイントｑ_{ｗｒｏｓｔ，ｉ}における不等式制約は、次式として記述することが可能であり、
Ｄｉｓｔ（ｑ_{ｗｏｒｓｔ，ｉ}，Ｏｂｓ）＝Ｄｉｓｔ（ｇ（ｑ，α_ｉ ^＊），Ｏｂｓ）≧ｄ_ｓａｆｅ （５）
ここで、ｄ_ｓａｆｅは、予め定義された衝突回避閾値基準（５０ｍｍなどの最小安全クリアランス距離）であり、且つ、唯一の変数は、ウェイポイント場所の新しい組ｑである。

最悪状態最適化の目的は、その初期場所ｑ^ｒから場所の新しい組ｑにウェイポイントを移動させるというものである。最初及び最後のウェイポイントｑ_０及びｑ_４は、上述のように移動させることができない。最悪状態を改善するために移動させることができるのは、中間ウェイポイントのみである。最適化問題をセットアップするためには、衝突回避制約とウェイポイント場所ｑの間の関係を更に高度化しなければならない。最悪状態とすべてのウェイポイントの間の関係を判定するために、その他のウェイポイント上における最悪状態の偏導関数が算出されている。

連鎖律を有する１次テイラー展開を使用することにより、最悪状態のメトリック値をそれぞれのウェイポイントの線形組合せとして近似することができる。従って、最悪状態とウェイポイントのそれぞれのものの間の関係は、次式のように衝突メトリックの偏導関数によって近似され、

この場合に、Ｄｉｓｔ関数及び関数ｇは、上述のとおりであり、且つ、ｑの異なるインスタンスの概念も、上述のとおりであり、これには、以前の反復からの基準値を表記する上付き文字ｒが含まれている。

上述の式（５）は、場所α_ｉ ^＊及びウェイポイントの新しい組ｑにおいて見出される最悪状態の観点において衝突回避制約を確立している。式（６）は、最適化ルーチン内において数値解に適した形態において衝突回避制約を近似している。

最悪状態最適化ステップにおいては、目的関数及び１つ又は複数の制約を有する最適化問題が定式化されている。目的関数は、通常、最小距離及び／又は滑らかさなどの特定の軌跡特性を実現するために選択されている。目的関数の一例は、次式であり、

ここで、ｗ値は、重み付け係数であり、||ｑ－ｑ^ｒ||項は、新しいウェイポイントと基準ウェイポイントの間の距離の量を表し、||∇ｑ||項は、ウェイポイントを通じた経路長の近似を表し、且つ、||∇^２ｑ||項は、曲率又は滑らかさメトリックを表している。特定の用途に適するように、その他の目的関数を定義することができる。

いくつかの制約関数を最適化問題のために定義することができる。ロボットモーション最適化において使用されている１つの通常の制約は、ロボットの関節が、軌跡に沿ってツールを移動させている際に既知の関節位置限度内に留まらなければならないというものである。関節位置限度は、不等式制約として定義することができる。別の制約は、ポイント３２０及び３２２である軌跡の開始及びゴールポイントが、処方された場所において留まらなければならないというものである。開始及びゴールポイント場所は、等式制約として定義することができる。これに加えて、上述のように、式（６）において偏導関数の形態において表されている最悪状態衝突メトリックは、次式のように不等式衝突回避制約として定義されてもよく、

ここで、上述のテイラー級数展開において、ｊは、０～ｎの値を取得する一方で、ｉは、１～ｎの値を取得しており、その理由は、ｊがウェイポイントを表しており、且つ、ｉが軌跡セクションを表しているらである。例えば、図３及び図４においては、ｎ＝４であり、且つ、５つのウェイポイントと４つの軌跡セクションが存在している。

最悪状態最適化問題が目的関数及び制約の以上の説明に従って定義されている状態において、ウェイポイントの位置を更新するために、異なる最適化更新規則を適用することができる。可能な最適化更新規則は、ＳＱＰ（Ｓｅｑｕｅｎｔｉａｌ Ｑｕａｄｒａｔｉｃ Ｐｒｏｇｒａｍｍｉｎｇ）、ＳＱＰ信頼領域、又はＣＨＯＭＰ（Ｃｏｖａｒｉａｎｔ Ｈａｍｉｌｔｏｎｉａｎ Ｏｐｔｉｍｉｚａｔｉｏｎ ｆｏｒ Ｍｏｔｉｏｎ Ｐｌａｎｎｉｎｇ）などのアルゴリズムを含む。最適化計算が収束した際に、ウェイポイント場所の新しい組ｑが結果的に得られることになる。

最適化計算において含まれている衝突回避不等式制約は、図４の矢印によって示されているように、最適化ソルバが、障害物３１０／３１２／３１４から離れるように最悪の状態を「プッシュ」するようにしている。これは、新しいウェイポイント場所の組ｑの場所に対する結果的に得られる効果を有しており、これも、衝突メトリックを改善するために最悪状態の軽減を支援する方向において漸進的に移動している。

式（７）及び（８）において記述されている最適化計算は、基準ウェイポイントのために判定された最悪状態との関係において障害物回避状況を改善するウェイポイント場所の新しい組ｑをもたらしている。新しい軌跡は、以前の軌跡よりも障害物回避との関係において相対的に良好なものとなるが、新しい軌跡は、依然として衝突回避基準を充足し得ない場合がある。従って、プロセスは、ウェイポイント場所の新しい組ｑが、上述の軌跡補間及び最悪状態サーチステップのための基準ウェイポイント入力として使用されるように、開始時点にループして戻っている。このループは、衝突回避基準を充足するウェイポイント場所の組ｑが判定される、即ち、最悪状態距離のいずれもが最小衝突回避閾値ｄ_ｓａｆｅを超過する、時点まで反復されている。

図４には、衝突回避基準を充足するための最後の収束の後のウェイポイント場所の新しい組ｑが示されている。最終的軌跡３６０は、開始ポイント３２０において開始し、ゴールポイント３２２において終了し、且つ、新しいウェイポイント３６２、３６４、及び３６６を通過している。最終的軌跡３６０は、障害物３１０／３１２／３１４を回避して通過していることを観察することができる。この場合にも、上述の計算は、すべて３次元形状に基づいており（図３及び図４に示されているように２Ｄではない）、且つ、衝突回避計算は、ロボットのすべての部分（即ち、最終的軌跡３６０によって表されているツール中心ポイント経路のみならず、ロボットアーム及びツール）を考慮していることを強調しておきたい。

図５は、本開示の一実施形態による最悪状態に基づいた最悪状態サーチ及びウェイポイント最適化を使用するロボット衝突回避モーション計画用の方法のフローチャート図５００である。ボックス５０２において、初期化が実行されている。初期化は、ロボット、開始及びゴールポイント、及び障害物の位置を作業セル座標フレームなどの共通座標フレーム内において定義するステップを含む。また、初期化は、開始及びゴールポイント場所に基づいて逆運動学を使用してロボットの開始及びゴール構成を算出するステップをも含む。

ボックス５０４における補間ステップにおいて、開始ポイントからゴールポイントまでの基準ウェイポイントが提供されており、且つ、ロボット構成が逆運動学を使用してそれぞれのウェイポイントにおいて演算されている。プロセスの最初の時点においては、基準ウェイポイントが初期基準ポイントである（これは、例えば、開始ポイントからゴールポイントまでの直線、類似の開始及びゴールポイントを有する予め計画された経路からのウェイポイント、或いは、教示ペンダント装置を使用して操作者によって教示されたポイントであってよい）。

ボックス５０６において、状態パラメータ化が以上の説明に従って実行されている。状態パラメータ化は、開始ポイント、ゴールポイント、及び中間ウェイポイントの関数として連続的軌跡を定義するステップを含む。連続的軌跡は、補間方法（線形、スプライン、など）に基づいてウェイポイントを通じて定義されており、且つ、弧長パラメータαが軌跡の長さに沿って定義されている。ボックス５０８において、最悪状態サーチが以上の説明に従って実行されている。最悪状態サーチは、障害物との関係における最悪状態距離を有するウェイポイントのそれぞれの隣接するペアの間の軌跡に沿った場所を見出すステップを含む。最悪状態サーチは、式（３）を使用して最悪状態が発生するそれぞれの軌跡セクション内において場所αを見出し、且つ、式（４）を使用してそれぞれの軌跡セクション内において実際の最悪状態ポイントを見出している。

ボックス５０６の状態パラメータ及びボックス５０８の最悪状態サーチは、ボックス５１０内において含まれた且つ図３において描かれた全体最悪状態サーチプロセスの２つのステップを集合的に構成している。

ボックス５１２において、最悪状態を使用して衝突回避制約とウェイポイント場所の間の関係を見出すステップが以上の説明に従って実行されている。関係を見出すステップは、上述の式（５）及び式（６）を使用するステップを含む。ボックス５１４において、最悪状態最適化が以上の説明に従って実行されている。最悪状態最適化は、式（７）において示されている例などの目的関数及び１つ又は複数の制約を有する最適化問題を定式化するステップを含む。制約は、式（８）に示されているように、ウェイポイント場所に対する最悪状態の関係に基づいた衝突回避最小距離不等式制約を含む。最悪状態最適化は、衝突回避メトリックを改善するウェイポイントの新しい組をもたらしている。

ボックス５１２の関係及びボックス５１４の最悪状態最適化を見出すステップは、ボックス５１６内において含まれた且つ図４上において描かれた全体最悪状態最適化プロセスの２つのステップを集合的に構成している。

最悪状態最適化に後続し、判定ダイアモンド５１８において、ウェイポイントの最後の組を通じた軌跡が衝突回避最小距離基準を充足しているかどうかが判定されている。最小距離基準が充足されている場合には、ウェイポイントの最後の組を通じた軌跡がボックス５２０における最終的な又は最適な軌跡として使用される。最終的な又は最適な軌跡は、望ましい動作（例えば、ワークピースの移動）を実行するためにロボットの運動を制御するようにロボットコントローラによって使用される。最小距離基準が充足されていない場合には、プロセスは、判定ダイアモンド５１８から補間ボックス５０４にループして戻っており、ここでは、ウェイポイント最後の組が、状態パラメータ化を再度開始するための基準ウェイポイントとして使用されている。

図６は、図５の最悪状態サーチ及び最適化方法を使用するように構成されたロボット衝突回避モーション計画システムの図である。ロボット６００は、１つ又は複数の障害物６１０を含む作業空間６０２内において動作している。コントローラ６２０は、通常、図示のように、ハードウェアケーブル接続を介してロボット６００と通信している。コントローラ６２０は、当技術分野において既知のように、関節モーターコマンドをロボット６００に送信し且つロボット６００内の関節エンコーダから関節モータ位置データを受け取ることにより、ロボット６００のモーションを制御している。

コントローラ６２０との通信状態にある任意選択のコンピュータ６３０は、ＣＡＤソリッド又は表面モデルの形態において障害物形状データを提供することを含むいくつかの異なるタスクのために使用することができる。コンピュータ６３０は、使用されている場合には、任意の適切な無線又は有線ネットワーク接続を介してコントローラ６２０と通信している。障害物６１０を定義するためのＣＡＤデータの使用の一代替肢として、センサ６４０などの１つ又は複数のセンサを使用することができる。１つ又は複数のセンサ６４０は、作業空間６０２内の障害物６１０の３Ｄ形状を提供する能力を有するカメラ又は物体センサの任意のタイプであってよい。１つ又は複数のセンサ６４０は、１つ又は複数の３Ｄカメラであるか、又はそのデータが３Ｄ障害物データとして組み合わせられる複数の２Ｄカメラであり得るであろう。又、１つ又は複数のセンサ６４０は、レーダー、ＬｉＤＡＲ、及び／又は超音波などのその他のタイプの装置を含み得るであろう。また、１つ又は複数のセンサ６４０は、任意の適切な無線又は有線ネットワーク接続を介してコントローラ６２０及び／又はコンピュータ６３０と通信している。

一実施形態において、コントローラ６２０は、コンピュータ６３０又は１つ又は複数のセンサ６４０から障害物形状データを受け取っている。障害物形状データは、作業空間６０２内において存在しているすべての障害物６１０の３Ｄ形状を定義している。また、コントローラ６２０は、センサデータ又はその他のものから次の動作用の開始及びゴールポイントを判定している。次いで、コントローラ６２０は、フローチャート図５００のステップの残りの部分を実行するように進捗しており、これには、ウェイポイントの初期セット用のロボットモーションを補間するステップと、衝突回避基準が充足される時点まで最悪状態サーチ及び最悪状態最適化計算を実行するステップと、が含まれている。次いで、コントローラ６２０は、最後の軌跡用のモーションコマンドを演算し、且つ、このコマンドをロボット６００に提供している。

別の実施形態において、コンピュータ６３０は、演算の大部分を実行しており、これには、障害物データ及び開始／ゴールポイントを受け取るステップ、初期ウェイポイントを補間するステップ、及び衝突回避基準が充足される時点まで最悪状態サーチ及び最悪状態最適化計算を実行するステップが含まれている。この実施形態において、コンピュータ６３０は、最後の最適な軌跡及びウェイポイントをコントローラ６２０に提供しており、且つ、コントローラ６２０は、対応するモーションコマンドを演算し且つこのコマンドをロボット６００に提供している。演算の責任は、コンピュータ６３０とコントローラ６２０の間において任意の適切な方式で分割することができる。

以上の説明の全体を通じて、様々なコンピュータ及びコントローラが記述及び示唆されている。これらのコンピュータ及びコントローラのソフトウェアアプリケーション及びモジュールは、プロセッサ及びメモリモジュールを有する１つ又は複数の演算装置上において実行されることを理解されたい。具体的には、これは、上述の（使用されている場合に）ロボットコントローラ６２０及びコンピュータ６３０のそれぞれ内のプロセッサを含む。具体的には、（使用されている場合に）コントローラ６２０及び／又はコンピュータ６３０内のプロセッサは、以上の開示の全体を通じて記述されている方式で最悪状態サーチ及び最悪状態最適化を使用して衝突回避経路計画演算を実行するように構成されている。

以上において概説したように、最悪状態サーチ及び最悪状態最適化を使用するロボット衝突回避モーション最適化用の開示されている技法は、従来技術の方法との比較において大きな利点を提供している。開示されている最悪状態サーチ／最悪状態最適化技法は、ウェイポイントの疎らな配置を可能にすると共に、そうでありつつも依然として、ウェイポイントの間の潜在的な干渉―場合によっては、従来技術の方法によっては見逃され得る細い障害物との間の干渉―を見出し且つ除去している。

最悪状態サーチ及び最悪状態最適化を使用するロボット衝突回避モーション最適化技法のいくつかの例示用の態様及び実施形態について上述したが、当業者は、その変更、並べ替え、追加、及びサブ組合せについて認識することになろう。従って、以下の添付の請求項及びこの後に導入される請求項は、その真の精神及び範囲に含まれているすべてのこのような変更、並べ替え、追加、及びサブ組合せを含むものと解釈されたい。

Claims (20)

1. 産業ロボットの経路を計画する方法であって、
   前記ロボットの作業空間内の障害物を定義する障害物データを提供するステップと、
   前記経路用の開始ポイント及びゴールポイントを含むウェイポイントの組及び１つ又は複数の中間ウェイポイントを定義するステップと、
   定義された経路補間方法に基づいて前記ウェイポイントの組を通じて軌跡を演算するステップであって、軌跡セクションは、隣接するウェイポイントの間の前記軌跡の一部分として定義されている、ステップと、
   前記軌跡に沿った距離として弧長パラメータを定義するステップと、
   最悪衝突回避メトリックを有するそれぞれの軌跡セクション内の最悪状態ポイントを識別するために最悪状態サーチを実行するステップと、
   ウェイポイントの新しい組を判定するために、前記最悪状態ポイントを使用して最悪状態最適化を実行するステップと、
   前記衝突回避メトリックが予め定義された基準を充足していない際に前記ウェイポイントの新しい組を使用して軌跡を演算するステップに戻るステップと、
   前記衝突回避メトリックが前記基準を充足している際に前記ロボットのモーションを制御するためにロボットコントローラによって前記ウェイポイントの新しい組及び対応する最適な軌跡を使用するステップと、
   を有する方法。
2. 障害物データを提供するステップは、コンピュータ支援設計（ＣＡＤ）システムから前記障害物データを提供するステップ又は前記作業空間内において前記障害物を検出するように構成された１つ又は複数のセンサから前記障害物データを提供するステップを含む請求項１に記載の方法。
3. 前記開始ポイント及び前記ゴールポイントは、ロボットツール中心の前記経路の開始及び終了を定義している請求項１に記載の方法。
4. 前記経路補間方法は、線形補間、スプライン補間、又は別のタイプの非線形補間の１つを含む請求項１に記載の方法。
5. 前記弧長パラメータは、前記開始ポイントにおいては、ゼロの値を有し、且つ、前記ゴールポイントにおいては、１の値を有する請求項１に記載の方法。
6. 前記衝突回避メトリックは、前記ロボットの任意の部分から前記障害物の１つまでの最小距離であり、且つ、前記基準は、前記メトリックが超過しなければならない最小安全距離である請求項１に記載の方法。
7. 最悪状態サーチを実行するステップは、前記ロボットの任意の部分から前記障害物の任意のものまでの距離が最小値であるそれぞれの軌跡セクションに沿った前記最悪状態ポイントを見出すステップを含む請求項１に記載の方法。
8. 前記最悪状態最適化を実行するステップは、前記最悪状態ポイントに基づいて前記衝突回避メトリックと前記ウェイポイントの場所の間の関係を定義するステップを含む請求項１に記載の方法。
9. 前記関係は、前記ウェイポイントのそれぞれのものの線形組合せとしての前記最悪状態ポイントの前記メトリックのテイラー級数展開として近似されており、前記級数内のそれぞれのウェイポイント項は、前記ウェイポイントの関数として定義された前記軌跡との関係における前記衝突メトリックの偏導関数を含む請求項８に記載の方法。
10. 前記最悪状態最適化を実行するステップは、極小化される目的関数を定義するステップと、衝突回避不等式制約を定義するために前記関係を使用するステップと、を含む請求項８に記載の方法。
11. 前記目的関数は、ウェイポイント変化の距離、軌跡の長さ、及び軌跡の形状の項の重み付けされた組合せを含み、前記重み付け係数の任意のものは、ゼロであってもよく、且つ、前記最適化は、前記開始及びゴールポイントを決定する等式制約を更に含む請求項１０に記載の方法。
12. 産業ロボットの経路を計画する方法であって、ウェイポイントの組を通じて軌跡を演算するステップであって、軌跡セクションは、隣接ウェイポイントの間の前記軌跡の一部分である、ステップと、前記軌跡に沿った距離として弧長パラメータを定義するステップと、最悪衝突回避メトリックを有するそれぞれの軌跡セクション内の最悪状態ポイントを識別するために最悪状態サーチを実行するステップと、ウェイポイントの新しい組を判定するために前記最悪状態ポイントを使用して最悪状態最適化を実行するステップと、前記衝突回避メトリックが予め定義されている基準を充足する時点まで前記ウェイポイントの新しい組を使用して軌跡を演算するステップに戻るステップと、を有する方法。
13. 産業ロボット用の経路計画システムであって、
    前記ロボットの作業空間内の障害物を定義する障害物データを提供する手段と、
    前記ロボット及び前記障害物データを提供する手段との通信状態にあるコンピュータであって、
    前記経路の開始ポイント及びゴールポイント並びに１つ又は複数の中間ウェイポイントを含むウェイポイントの組を定義し、
    経路補間関数を使用して前記ウェイポイントの組を通じて軌跡を演算し、軌跡セクションは、隣接するウェイポイントの間の前記軌跡の一部分として定義されており、
    前記軌跡に沿った距離として弧長パラメータを定義し、
    最悪衝突回避メトリックを有するそれぞれの軌跡セクション内の最悪状態ポイントを識別するために最悪状態サーチを実行し、
    ウェイポイントの新しい組を判定するために、前記最悪状態ポイントを使用して最悪状態最適化を実行し、
    前記衝突回避メトリックが予め定義されている基準を充足していない際に前記ウェイポイントの新しい組を使用して軌跡を演算するステップに戻り、且つ、
    前記衝突回避メトリックが前記基準を充足している際に前記ロボットのモーションを制御するために前記ウェイポイントの新しい組及び対応する最適な軌跡を使用する、
    ように構成されたプロセッサ及びメモリを有するコンピュータと、
    を有するシステム。
14. 前記障害物データを提供する手段は、前記作業空間内において前記障害物を検出するように構成されたコンピュータ支援設計（ＣＡＤ）システム又は１つ又は複数のセンサを含み、前記１つ又は複数のセンサは、１つ又は複数のカメラ、レーダーセンサ、ＬｉＤＡＲセンサ、超音波センサ、又は赤外線センサを含む請求項１３に記載のシステム。
15. 前記弧長パラメータは、前記開始ポイントにおいては、０の値を有し、且つ、前記ゴールポイントにおいては、１の値を有する請求項１３に記載のシステム。
16. 前記衝突回避メトリックは、前記ロボットの任意の部分から前記障害物の１つまでの最小距離であり、且つ、前記基準は、前記メトリックが超過しなければならない最小安全距離である請求項１３に記載のシステム。
17. 最悪状態サーチを実行するステップは、前記ロボットの任意の部分から前記障害物の任意のものまでの距離が最小値であるそれぞれの軌跡セクションに沿った前記最悪状態ポイントを見出すステップを含む請求項１３に記載のシステム。
18. 前記最悪状態最適化を実行するステップは、前記最悪状態ポイントに基づいて前記衝突回避メトリックと前記ウェイポイントの場所の間の関係を定義するステップを含む請求項１３に記載のシステム。
19. 前記関係は、前記ウェイポイントのそれぞれのものの線形組合せとしての前記最悪状態ポイントの前記メトリックのテイラー級数展開として近似されており、前記級数内のそれぞれのウェイポイント項は、前記ウェイポイントの関数として定義された前記軌跡との関係における前記衝突メトリックの偏導関数を含む請求項１８に記載のシステム。
20. 前記最悪状態最適化を実行するステップは、最小化される目的関数を定義するステップと、衝突回避不等式制約を定義するために前記関係を使用するステップと、を含む請求項１８に記載のシステム。

Images