【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、一般に、多数の移動物体が互いにそして他の障害物と衝突するのを回避するようにそれらの移動物体を制御することに係る。 より詳細には、本発明は、多数のロボットマニピュレータの移動計画及び制御に係る。 ロボットマニピュレータは、
部品の組立、分解、加工、修理、保守及び検査のような作業を遂行するために種々の工業及び商業用途に使用されている。 これらの用途は、これらの機能の逐次の組合せ又は同時の組合せを含んでもよく、そして多数のマニピュレータの整合した運動を伴ってもよい。 基本的に、
ロボットマニピュレータは、継手により互いに接続された多数の機械的なリンクによって三次元空間内の所望の点へ移動できるツールを備えている。 「エンド・エフェクタ」とも称するツールは、把持機構、溶接トーチ、切削装置、電磁石又は他の装置である。

【０００２】

【従来の技術】マニピュレータは、その最も一般的な形態においては、ツールが一連の堅固な機械的なリンクを経てベース又はフレームに接続され、これらのリンクが、回転継手とも称する簡単なピボット継手である継手又はジンバルを経て１つのリンクから次のリンクへ接続されるものを思い浮かべることができる。 又、マニピュレータは、スライド及び回転変位の組合せを伴う他の形式の継手を含んでもよい。 リンクの位置は、多数の継手角度又はジンバル角度によって定義される。 所与の１組のジンバル角度がツールの特定の位置を定義する。 １組のジンバル角度からツールの位置を導出することは、簡単な問題であり、通常は、順方向運動を伴うと称される。 反対即ち逆方向運動を伴う非常に困難な問題は、多数の組のジンバル角度によって定められる所与のツール位置から適当なジンバル角度を決定することである。

【０００３】本発明は、ロボット経路を計画すること、
即ちロボットマニピュレータツールを三次元空間においてある点から別の点へと移動することに関する。 ツールの各位置は、一般に、多数の組のジンバル角度のいずれかによって定義されるので、ある位置から別の位置へのツールの移動は、多数の経路のいずれかを経て行うことができる。 ロボット機構の経路計画のための１つの技術は、カルテシアン空間ではなく、継手空間とも称される配列空間を使用して、ツールの位置を定めるものである。 ここでは、配列空間（即ちｃ空間）という用語が使用される。 ｃ空間におけるツールの位置は、ジンバル即ち継手角度によって定義される。 ｎ個のジンバルがある場合には、ｃ空間は、ｎ次元をもつことになる。 ｃ空間における経路計画は、多数のマニピュレータを使用する場合に生じる多数の問題に対する解決策として提案されている。 例えば、ロザノ−ペレツ著の「空間計画：配列空間解決策（Ｓｐａｔｉａｌ Ｐｌａｎｎｉｎｇ： Ａ
Ｃｏｎｆｉｇｕｒａｔｉｏｎ Ｓｐａｃｅ Ａｐｐｒ
ｏａｃｈ）」、ＩＥＥＥトランザクションズ・オン・コンピュータズ、第３２巻、第２号、第１０８−１２０ページ、１９８３年２月を参照されたい。 固定の障害物をｃ空間に「マップ」させ、理論的に、それらを回避すると共に、マニピュレータの部品間の衝突を回避するように経路を計画することができる。

【０００４】ロボット経路の計画は、次のような多数の理由で本来的に困難な問題である。 １）全体的な計画を遂行し、即ち開始から終了までの完全な移動又は移動シーケンスを計画する必要がある。 より局部的な計画は、しばしば、正しい経路に対する「ハンチング」、デッド端経路、及び異なる経路を計画するための後方追跡を必要とする競合といった問題を招く。 ２）マニピュレータ、ツールが作用する被加工片、及び作業環境の間で偶発的な衝突を回避する必要がある。 衝突は、マニピュレータ、被加工片及び作業環境にダメージを及ぼし、経済的な損失又は受け入れられない危険性を招く。 ３）各ジンバルは、限定された角度運動範囲しか与えないストッパを有するので、各マニピュレータの各軸に対して限定された移動範囲内で機能する必要がある。 ジンバルのストッパに当たると、現在の作業が停止するだけでなく、物理的なダメージを招くことがある。 作業を再開するには、手動操作の介在が必要とされる。 ４）所望のツール位置及び方向を得るためにマニピュレータのジンバル角度を特定する際に固有のあいまいさ、
即ち逆運動問題を解消する必要がある。 ５）動的な運動限界、例えば、割合、加速度及び急な動きの限界内で機能する必要がある。 動的な限界は、安全性の問題及びハードウェア部品の制約の両方から生じる。

【０００５】

【発明が解決しようとする課題】原理的に、ｃ空間技術の使用は、これらの問題の最初の４つに対して一応の解決策を与える。 しかしながら、実際には、ｃ空間解決策を直接適用すると、過剰な計算負担を生じる。 これについては、キャニー、ジョーンＣ著の「ロボット運動計画の複雑さ（Ｔｈｅ Ｃｏｍｐｌｅｘｉｔｙ ｏｆ Ｒｏ
ｂｏｔ Ｍｏｔｉｏｎ Ｐｌａｎｎｉｎｇ）」、Ｐｈ．
Ｄ． 論文、デパートメント・オブ・エレクトリカル・エンジニアリング・アンド・コンピュータサイエンス、Ｍ
ＩＴ、マサチューセッツ州、ケンブリッジ、１９８７年５月（本としても出版されている）に説明されている。
例えば、３つの３軸マニピュレータのシステムに対し純粋なｃ空間セル解決策を使用する場合に、各軸が３６０
Ｅ範囲を有しそして適度な分解能が０．１Ｅと仮定すると、（３６０／０．１） ^{３＋３＋３} ＝３６００ ^９即ち約１０ ^３２のセルの計算及び記憶が必要となる。 このような規模の計算を実行する技術は現在存在せず、又計画もされていない。

【０００６】ロボットマニピュレータにおいて衝突を回避する別の解決策は、人為的なポテンシャル又は力フィールドの類推を使用して、マニピュレータの部品を衝突から守ることである。 簡単に述べると、この解決策は、
マニピュレータの相対的な接近度に基づいてマニピュレータの「反発」力を発生する。 ２つのマニピュレータが互いに接近するほど、反発力が大きくなる。 基本的に、
各マニピュレータは、一次経路に沿ってその意図された方向に移動するように制御されるが、その経路がマニピュレータを別のマニピュレータに接近させ過ぎるか又は固定の障害物に接近させ過ぎるときには、発生される反発力によりその経路からそらされる。

【０００７】人為的なポテンシャルフィールドの使用は、従来のｃ空間解決策の計算上の複雑さを回避するが、他の問題を引き起こす。 特に、一般のマニピュレータシステムに対するポテンシャルフィールドは、局部的な最小値が、非周期的振動及び無秩序な運動を含むハンチング、デッドエンド、所望の目標位置以外の平衡位置での停止、及びシステムが目標位置に到達せずに限界サイクルへと安定化するような競合状態を招くことがある。

【０００８】以上のことから、衝突を回避するよう多数のマニピュレータの経路を計画し、しかも、ｃ空間経路計画及び人為的ポテンシャル衝突回避に関連した問題を最小限に抑えるような技術が依然として要望されていることが明らかである。 本発明は、この要望を満足するものである。

【０００９】

【課題を解決するための手段】本発明は、共通の作用空間において多数の移動物体の経路を計画しそして移動を実行する装置及びそれに対応する方法であって、各物体の位置が、その物体のｎ個の自由度に対応する複数ｎの配列設定により定義される複数の相互接続されたリンクの配列に依存するような装置及び方法に係る。 簡単にそして一般的に述べると、本発明の装置は、経路計画手段と、経路実行モジュールとを含む。 経路計画手段は、各移動物体の一連の移動を前もって計画し、比較的おおまかなスケールの移動計画を与えるように働く。 経路計画手段は、三次元空間内の各物体の位置を定義するｎ個の配列設定に対応するｎ個の直交軸によって定義された配列空間において経路を決定する。 経路実行モジュールは、経路計画手段により与えられた比較的おおまかなスケールの計画に基づいて物体を移動するよう動作し、そして細かいスケールの人為的力フィールド衝突回避サブシステムを備え、これは、一部分は経路計画手段によりそして一部分は衝突回避サブシステムにより決定された経路に沿って移動物体が移動されるよう移動物体に制御信号を与える。 従って、物体は、互いに又は固定の障害物と衝突せずに、スタート点から所望の終了点まで移動される。

【００１０】より詳細には、経路計画手段は、作用空間を多数のｎ次元セルとして定義する配列空間データベースと；該配列空間データベースの各セルに対して衝突状態を決定するためのデータベース発生ロジックであって、衝突状態は、セル内の配列設定の全ての組合せに対して衝突の可能性があることを指示する「フル」であるか、又はセル内に衝突の可能性がないことを指示する「フリー」であるか、又はセル内の配列設定のある組合せに対して衝突の可能性があることを指示する「ミックス」であるようなデータベース発生ロジックと；配列空間データベースの各セルに関する情報を使用して、衝突が回避されるように配列空間のある点から別の点への経路を計画するための経路計画実行モジュールとを備えている。 更に、経路計画手段は、第１の試行経路を選択し、そしてその経路がフル又はミックスセルを通るかどうか決定するための手段と；以前に選択された試行経路が衝突を受ける場合には第２の他の試行経路を系統的に選択し、最終的に衝突のない経路が選択されるようにする手段とを備えている。 第２の他の試行経路を系統的に選択する上記手段は、好ましくは、フル又はミックスセルの衝突領域を通過する試行経路の部分の中点を検出する手段と；上記中点を通る線上において上記中点を通過する試行経路に直交するよう配置された試行中間点を選択する手段とを備えている。 この試行中間点は、そてが衝突領域にない場合には、計画された経路上にあるものとして選択される。 試行中間点が衝突領域にある場合には、別のものが選択され、というようにして、最終的に衝突のない経路が計画される。

【００１１】経路実行モジュールは、更に特定に述べると、経路計画手段により与えられる経路データから微細分解能経路を発生するための中間点補間手段と；配列空間を通る微細分解能経路と、物体の予想される配列設定とに基づいて制御信号を発生するための制御法則モジュールと；作用空間における物体の相互の接近度に基づいて衝突回避制御信号を発生するための衝突回避力モデルと；この衝突回避制御信号と、上記制御法則モジュールからの制御信号とを合成するための手段であって、配列空間を通して計画された経路を修正することによって微細なスケールで衝突回避を達成する手段とを備えている。 経路実行モジュールは、更に、制御法則モジュール及び衝突回避力モデルにより使用するための物体配列設定の推定値を与える物体動的モデルを備えている。

【００１２】新規な方法については、本発明は、経路を計画し、経路の実行を制御し、そして移動物体に制御信号を付与するという段階を備えている。 計画段階は、各移動物体の一連の移動を含む経路を前もって計画し、比較的おおまかなスケールの移動計画を形成する段階を含む。 計画段階は、三次元空間における各物体の位置を定義するｎ個の配列設定に対応するｎ個の直交軸により定義された配列空間において経路を決定する。 制御段階においては、計画された経路の実行は、主として計画段階によって形成された比較的おおまかなスケールの計画に基づいて物体を移動するように制御され、そして人為的なカフィールドモデルに基づいて衝突回避物体制御信号を発生する付加的な段階を含む。 移動物体に制御信号を付与する段階は、一部分は経路計画段階によりそして一部分は経路の実行を制御する段階により決定された経路セグメントに沿って物体を移動する。 それ故、物体は、
互いに又は固定の障害物と衝突することなく開始点から所望の終了点へと移動される。

【００１３】より詳細には、経路計画段階は、作用空間を多数のｎ次元セルとして定義する配列空間データベースを設定し；該配列空間データベースの各セルに対して衝突状態を決定し、衝突状態は、セル内の配列設定の全ての組合せに対して衝突の可能性があることを指示する「フル」であるか、又はセル内に衝突の可能性がないことを指示する「フリー」であるか、又はセル内の配列設定のある組合せに対して衝突の可能性があることを指示する「ミックス」であり；そして配列空間データベースの各セルに関する情報を使用して、おおまかな経路計画を発生し、衝突が回避されるように配列空間のある点から別の点への経路において移動を与えることを含む。

【００１４】おおまかな経路計画を発生する上記段階は、更に、第１の試行経路を選択し、そしてその経路がフル又はミックスセルを通るかどうかを決定し；そして以前に選択された試行経路が衝突を受ける場合には第２
の他の試行経路を系統的に選択し、最終的に衝突のない経路が選択されるようにすることを含む。 第２の他の試行経路を系統的に選択する上記段階は、フル又はミックスセルの衝突領域を通過ずる試行経路の部分の中点を検出し；そして上記中点を通る線上において上記中点を通過する試行経路に直交するよう配置された試行中間点を選択し、この試行中間点は、それが衝突領域にない場合には、計画された経路上にあるものとして選択される。

【００１５】本発明の好ましい実施形態においては、経路実行移動を制御する上記段階は、おおまかな経路計画の中間点間を補間して、微細分解能計画経路を発生し；
配列空間を通る微細分解能経路と、物体の予想される配列設定とに基づいて制御信号を発生し；作用空間における物体の相互の接近度に基づいて衝突回避制御信号を発生し；そしてこの衝突回避制御信号と、上記微細分解能計画経路に基づく制御信号とを合成することを含む。 衝突回避は、配列空間を通して計画された経路を修正することにより達成される。 経路の実行を制御する上記段階は、物体の移動を動的にモデリングすることを更に備え、微細分解能経路に基づいて制御信号を発生する上記段階及び衝突回避制御信号を発生する上記段階に使用するための物体配列設定の推定値を与える。

【００１６】本発明の好ましい実施形態においては、移動物体は、共通の作用空間において動作するロボットマニピュレータである。 配列設定は、マニピュレータの位置を集合的に定義するマニピュレータ継手角度及び位置である。

【００１７】以上の説明から明らかなように、本発明は、共通の作用空間で動作する多数の移動装置、例えばロボットマニピュレータに対する経路計画の分野に著しい進歩をもたらす。 本発明以前の経路計画は、配列が簡単であることを除くと計算が甚だしいものであり、そして衝突回避のための力フィールドモデリングは常に満足な解決策を与えるものではない。 本発明は、比較的おおまかなスケールの配列空間経路計画を移動実行段階中に微細なスケールの衝突回避と結合して、多数のマニピュレータの問題に対して便利で且つ実用的な解決策をもたらす。 本発明の他の特徴及び効果は、添付図面を参照した以下の詳細な説明から明らかとなろう。

【００１８】

【発明の実施の形態】説明の目的で添付図面に示されたように、本発明は、共通の作用空間を共用するロボットマニピュレータのような多数の移動物体の衝突のない運動を計画しそして実行するシステムに関する。 この一般的な目的のために他のシステムも提案されているが、完全に満足なものは皆無である。

【００１９】本発明によれば、衝突回避は、おおまかなスケールでの全体的な経路計画と、ロボットマニピュレータ運動の細かいスケールでの実行の組合せで達成される。 ｃ空間と称する配列空間におけるロボット運動の全体的な経路計画は、現在ロボット位置と所望のロボット位置との間の公称運動軌道を与える。 ｃ空間とは、単に、ロボットマニピュレータの各自由度ごとに１つづつの１組のカルテシアン軸により定められた空間に過ぎない。 各軸のスケールは、ロボットマニピュレータの１つの継手のジンバル角度又は継手位置を測定する。 例えば、第１のアームがベースにヒンジ固定されそして第２
のアームが別のヒンジ継手によって第１のアームに接続されたマニピュレータは、２つの自由度を有し、ｃ空間は、二次元となる。 ｃ空間内の各点は、マニピュレータの特定の位置を定義し、そしてｃ空間の軸に対する点の座標は、マニピュレータのその位置に対応するジンバル角度を測定する。 障害物がｃ空間へと「マップ」され、
従って、マニピュレータの所望の運動は、障害物を考慮して、ある位置から所望の位置へマニピュレータを移動する一連の段階を形成するように計画することができる。 一般に、ある位置から別の位置へ移動する最も速い方法は、ｃ空間内の直線により表されるが、固定の又は動的な障害物が存在するためにこの経路は常に可能ではない。

【００２０】しかしながら、不都合なことに、ｃ空間技術のみに基づく経路計画は、計算が甚だしく、多数のマニピュレータのほとんどのシステムにとって実現不可能な解決策である。 本発明は、比較的おおまかなスケールでの経路計画のためのｃ空間技術と、細かいスケールでの力フィールド衝突回避モデルとを組み合わせるものである。 それにより形成されるシステムは、これら技術の各々を個々に用いたときの欠点を解消し、そしてロボットマニピュレータのような多数の移動物体の運動を計画しそして制御するための実際的なシステムを提供する。

【００２１】 システムの概要 ：図１に示すように、本発明のシステムは、４つの主たる要素、即ちスーパーバイザー１０と、おおまかなスケールの経路計画モジュール１２と、細かいスケールの経路実行モジュール１４と、
ロボットマニピュレータシステム１６とを備えている。
おおまかなスケールの経路計画モジュール１２及び細かいスケールの経路実行モジュール１４は、一緒に、経路計画及び実行システム１８と称する。

【００２２】スーパーバイザー１０は、有用な作業を実施する一連のマニピュレータ移動を特定する。 スーパーバイザー１０は、ライン２０で示された移動要求を経路計画モジュール１２に送り、マニピュレータ位置及び方向の次に要求される組合せを特定する。 又、スーパーバイザー１０は、マニピュレータシステム１６からのライン２２で示すように、マニピュレータのジンバル角度及び他のパラメータ全部の初期及び現在状態を受け取る。
既に述べたように、マニピュレータシステム１６は、継手によって互いに接続された堅固なリンクで構成され、
継手の多くは、１つの自由度を有する簡単なヒンジのような回転継手である。 他の形式の継手としては、スライド運動はできるが回転できないプリズム型継手、三次元において角度運動を行える球状継手、回転及びスライド運動の両方を行える円筒状継手、及び回転によりリンクを延長できるスクリュー型継手が含まれる。 マニピュレータシステム１６における継手の形式の組合せに関わりなく、いかなる瞬間におけるその配列も、１組の継手角度及び位置によって完全に定義される。

【００２３】経路計画モジュール１２は、スーパーバイザー１０から移動要求を受け取り、そして時間タグ付きのｃ空間「中間点」のリストを形成し、これは、ライン２４で示すように、経路実行モジュール１４へ送られる。 この中間点リストは、特定の時間における継手の角度／位置の１組の組合せである。

【００２４】経路実行モジュール１４は、経路計画モジュール１２から中間点リストを受け取り、この中間点リストを処理して、この中間点リストにより特定されたおおまかなスケールでの移動をたどりながら、微細なスケールで衝突を回避するに必要なマニピュレータの移動（角度／位置及び時間の両方の）を決定し、そして時間サンプルされる継手コマンドをマニピュレータシステム１６に送る。 又、経路実行モジュール１４は、継手軸の全ての角度及び位置を含むマニピュレータ１６の初期及び更新状態をライン２２を経て受け取る。

【００２５】ロボットマニピュレータシステム１６は、
経路実行モジュール１６からライン２６を経てマニピュレータ継手コマンドを受け取り、そしてマニピュレータ継手コントローラ及びセンサのような従来型のロボットマニピュレータ部品（図示せず）を用いてこれらのコマンドを実行し、現在継手角度又は位置を決定する。 マニピュレータシステム１６は、又、部品挿入、切削、研磨等の精密な作業のために位置及び力の複合制御を実行する能力を組み込むこともできる。 従来のロボットシステムは、スーパーバイザー１０及びマニピュレータシステム１６を備えており、従って、これらの要素については、ここでは詳細に説明しない。

【００２６】 経路計画モジュール１２ ：おおまかなスケールの経路計画を形成するための経路計画モジュール１
２が図２に詳細に示されている。 このモジュールは、５
つの基本的な要素、即ち経路計画実行部３０と、ｃ空間データベース３２と、ｃ空間データベース発生モジュール３４と、経路セグメントタイミングモジュール３６
と、マニピュレータシステムモデル３８とを備えている。

【００２７】経路計画実行部３０は、ライン２０を経て移動要求を受け取り、そしてｃ空間データベース３２の比較的おおまかな分解能内で衝突を回避する一連のｃ空間経路セグメントを決定する。 各経路セグメントは、ｃ
空間内の直線である。 又、実行部３０は、経路セグメントタイミングモジュール３６に問い合わせすることにより各経路セグメントのタイミングを決定し、そして最終的に、経路セグメントの終了点と経路セグメントのタイミングとを結合して、時間タグ付けされた中間点のリストをライン２４に出力する。

【００２８】ｃ空間データベース３２は、実行部３０がｃ空間内の各セルのセル形式を決定できるようにする。
セルとは、一般的に述べると、継手変数の個々の値によって各継手軸において境界定めされたｃ空間内の平行六面体である。 ｃ空間は、マニピュレータシステム１６
（図１）の自由度と同程度の次元を有することを想起されたい。 例えば、自由度が２つしかない場合には、ｃ空間は二次元であり、各セルは、個別の継手角度により境界定めされるこの二次元空間の長方形要素に過ぎない。
２つの自由度が回転継手に対応する場合は、セルは、例えば、１度（角度測定値）ｘ１度であってもよいし、又は他の選択された分解能であってもよい。 ｃ空間データベースの重要な特徴は、セルが「フリー」であるか、
「フル」であるか、又は「ミックス」であるかについての各セルの指示を含むことである。 フリーのセルは、その体積内のどこの継手変数のいかなる組合せについても衝突がない。 即ち、ロボットマニピュレータシステム１
６は、ｃ空間の同じセルを占有する固定又は動的な障害物に衝突することなくセル内のいかなる点へも移動することができる。 フルのセルは、その体積内の継手変数の全ての組合せに対し潜在的な衝突を指示する。 ミックスのセルは、フリーでもフルでもないセルである。 又、データベース３２は、マニピュレータ継手の各々に対して継手限界を組み込んでいる。 各継手は、通常、角度又は並進移動の制限範囲を有する。 指定の限界を越える継手変数を含むセルは、データベース３２において「フル」
とタグ付けされる。

【００２９】ｃ空間データベース発生モジュール３４
は、継手変数分解能の指定のレベルに対し各ｃ空間セルのセル形式を決定するのに必要なロジックを含んでいる。 このモジュールは、オフライン計算を含んでもよいし、オンライン計算を含んでもよいし、又はその両方の組合せを含んでもよい。 特に、オンライン計算は、おおまかな分解能レベルにおいてミックス形式であると決定されたセルのサブ領域のセル形式を決定するのに使用できる。 このプロセスは、「セルのエクスプロージョン（ｅｘｐｌｏｓｉｏｎ）」と称する。

【００３０】マニピュレータシステムモデル３８は、マニピュレータシステム１６の物理的な構造、このシステムが作用する被加工片、及び作業環境の数学的記述を含んでいる。 このモデルは、データベースのサイズを減少するか、又は潜在的な衝突から部品を保護するための安全余裕を与えるために、マニピュレータシステムの選択された部品の物理的な寸法に対する幾つかの近似値を組み込んでもよい。 モデル３８は、ｃ空間データベース発生モジュール３４により、ｃ空間データベース３２を発生及び更新するのに使用される。

【００３１】経路セグメントタイミングモジュール３６
は、継手変数の動的な制約を組み込む１群のパラメータ方程式に基づいて経路セグメントのタイミングどりを実行する。 上記制約は、割合、加速度、急な動き、又は継手変数の高次の時間導関数の組合せである。 モジュール３６は、動的な制約のもとでマニピュレータ継手変数を指定の量だけ変更するために必要とされる時間に関連した１組の数学方程式を組み込んでもよいし、又は等価データを含むルックアップテーブルを組み込んでもよい。
このモジュール３６は、経路計画実行部３０により問合せを受け、そして経路セグメントの時間データを実行部に返送する。

【００３２】 経路実行モジュール１４ ：経路実行モジュール１４が図３に詳細に示されている。 このモジュールは、次の要素、即ち中間点補間手段４０と、制御法則モジュール４２と、マニピュレータ動的モデル４４と、衝突回避力モデル４６と、マニピュレータシステムモデル４８と、サンプリング手段５０とを備えている。

【００３３】中間点補間手段４０は、時間タグ付けされた中間点リストを経路計画モジュール１２からライン２
４を経て受け取り、そして経路セグメントタイミングモジュール３６において経路セグメントのタイミングを決定するのに使用した同じ１組のパラメータ方程式を用いて、中間点リストよりも微細な時間分解能で継手角度／
位置、割合又は加速度（又は高次の時間導関数）の必要な組合せを決定する。 補間手段４０と経路セグメントタイミングモジュール３６との整合は、公称マニピュレータ移動において動的な制約を実施する能力を与える。 補間手段４０は、ライン５２で示されたように、制御法則モジュール４２に補間された中間点を与える。

【００３４】制御法則モジュール４２は、補間手段４０
から補間された中間点を受け取ると共に、マニピュレータ動的モデル４４からライン５３を経てマニピュレータ動的モデル状態データも受け取る。 制御法則モジュール４２は、２組の入力データを数学方程式により処理し、
ライン５４を経て動的モデル４４へ制御トルクコマンドを発生する。 制御法則モジュール４２は、フィードバック及びフィードフォワードの両方の制御項を組み込むことができる。 フィードバック制御項は、マニピュレータ動的モデル状態、又は補間された中間点とマニピュレータの動的モデル状態との間の差に基づく。 フィードフォワード制御項は、補間された中間点のみに基づく。 制御法則におけるフィードバック制御項の目的は、マニピュレータ動的モデル４４及び衝突回避力モデル４６の組合せの動的な振る舞いを安定化しそして変更することである。 制御法則におけるフィードバック項は、比例、割合、遅れ、積分又は他の形態の制御補償を含む。 制御法則におけるフィードフォワード項の目的は、衝突回避モデル４６が存在しない場合に、補間された中間点により暗示される軌道に沿ってマニピュレータシステムを移動する公称制御トルクを与えることである。 制御法則は、
非直線項又はモード切り換えを含んでもよく、例えば、
制御エラーが小さいときに積分項を追加してもよい。

【００３５】マニピュレータ動的モデル４４は、制御法則モジュール４２からライン５４を経て受け取った制御トルクと、衝突回避モデル４６からライン５６を経て受け取った力モデルトルクとの合成作用のもとで、マニピュレータシステムの移動の実時間解を生じる１組の数学方程式を備えている。 実際のマニピュレータシステム１
６（図１）からの測定されたマニピュレータ継手角度／
位置は、マニピュレータ動的モデル４４の状態を初期化又は更新するのに使用される。 継手変数（角度及び位置）の範囲は、数学方程式に含まれる。 動的モデル４４
は、継手角度／位置、割合及び加速度のようなマニピュレータの動的モデル状態をライン５３に出力する。 モデル４４に使用される動的な方程式の忠実度のレベルは、
必要とされる精度に基づいて、完全な非直線的結合方程式から簡単化及び理想化された方程式までの範囲である。

【００３６】衝突回避力モデル４６は、動的モデル４４
からのマニピュレータ動的モデル継手変数状態を作用空間におけるマニピュレータ部品の相対的な位置に関連付ける順方向運動方程式を組み込んでいる。 又、モデル４
６は、作用空間におけるマニピュレータ部品の相対的な位置をそれら部品間の推定反発力に関連付ける数学方程式と、それら部品間の推定反発力をマニピュレータシステムの合成力及びモーメントに関連付ける数学方程式も組み込んでいて、ライン５６を経て動的モデル４４へ返送する。 これらの数学方程式は、マニピュレータシステムモデル４８を組み込んでいる。

【００３７】推定反発力方程式は、有限の素子近似を組み込むことができる。 推定反発力方程式は、距離と共に指数関数的に減衰するか、距離と共にガウス減衰するか又は相対的な距離の単調に減衰する関数である項「１／
ｒ ^２ 」（ｒは、潜在的に衝突する部品間の距離である）
を含む。 推定反発力方程式は、選択されたマニピュレータ部品の反発力を一方的なものとして、衝突回避のために選択されたマニピュレータの経路を偏らせる優先順位を与えるためのオプションを組み込むことができる。 従って、低い優先順位のマニピュレータは、高い優先順位のマニピュレータに「譲歩」し、衝突を回避する。

【００３８】マニピュレータシステムモデル４８は、その内容及び範囲がシステムモデル３８（図２）に類似している。 しかしながら、このモデル４８は、微細なスケールの運動計画をサポートするので、少なくともモデル３８と同程度に正確且つ詳細でなければならない。

【００３９】 ハードウェア実施 ：図４は、本発明のハードウェア実施を例示するものである。 計算ハードウェア６０は、スーパーバイザー１０と、経路計画モジュール１２と、経路実行モジュール１４とを備えている。 又、
計算ハードウェアは、関連するデータ入力装置６２及び他の従来型の計算要素（図示せず）も備えている。 ここに示すロボットマニピュレータシステム１６は、第１のマニピュレータ６４と、第２のマニピュレータ６６とを備え、その各々は、塗りつぶした円で示された３つの継手と、太い直線で示された３つのリンクとを有する。 マニピュレータ６２及び６６は、ベース６８及び７０に各々取り付けられ、そしてロボットマニピュレータシステムにより組み立て中であるユニット７６に合体されるべき被加工片７２及び７４を保持するものとして示されている。 計算ハードウェア６０は、ライン２６を経て１組の継手コントローラ７８へマニピュレータコマンドを与え、次いで、このコントローラは、ライン８０を経てマニピュレータ６４及び６６へ制御信号を供給する。

【００４０】 経路計画実行部３０の詳細 ：ｃ空間方法を用いる移動計画は、反復解決策を使用する。 各反復において、図５に示すように、経路が現在中間点９０から次の中間点９２へと走査され、そしてｃ空間においてこの直接経路をたどることにより横断される各セルのセル形式（即ち、フリーか、フルか又はミックスか）が決定される。 陰影付けされた領域９４は、フル又はミックスセルの領域を表し、直接経路によって横断されると、衝突を生じるおそれがあるものである。 この状態が検出されると、衝突を回避するために、１つ以上の付加的な中間点を見つけなければならない。 この手順の重要な段階は、経路計画プロセスの次の反復に対し、候補即ち試行中間点を発生することである。 ９６で示された試行中間点＃１は、領域９４の外部を通るので、衝突を回避する。 ９８で示された試行中間点＃２は、このようにならない。 衝突のない経路を効果的に見つける確率は、元の経路に直交し且つ種々の方向にある試行中間点を発生する方法に基づく。

【００４１】本発明以前には、幾何学的な論理付けが充分である二次元又は三次元のｃ空間のみに対して試行中間点発生アルゴリズムが存在した。 ほとんどの潜在的な用途は、それより高い次元のｃ空間、通常は、５ないし１４次元において経路計画を必要とするので、更に一般的な方法が必要とされる。 このような方法が図６に示されている。

【００４２】この方法は、元の経路セグメントに直交し且つ全ての使用可能な方向に及ぶ試

【００４３】ブロック１０２に示すように、次のステップは、経路セグメントを定義するベ

【００４４】

自然ベースであり、即ちｂ

_１ ＝ｎ

_１ ＝（１、０、０・・

・）、ｂ

_２ ＝ｎ

_２ ＝（０、１、０・・・）、及びｂ

_ｊ ＝

ｎ

_ｊ ＝（０、０、１・・・）と設定することによるものである。 上記第２のケースでは、自然ベースベクトルの１つが冗長であり、先へ進む前に削除されねばならない。 直交補数ベースベクトルを発生する手順が図７に示されており、図８及び９には更に詳細に示されている。

【００４５】試行中間点を決定するプロセスの次のステップは、ブロック１０６及び１０８に各々示すように、
カウンタｋを初期化及び増加することである。 次いで、
ブロック１１０に示すように、データベースをアクセスし、各試行中間点ｋに対する重みｗ _ｉｋが得られる。 重みは、種々の方法で選択することができる。 例えば、考えられる全ての２ ^ｊ−１の組合せにおいてｗ _ｉ ＝−１又は＋１とすると、超立

_ｉを選択すると、現在経路セグメントに直交するサブ空間において超球体の表面に試行中間点をもつことになる。 この場合に、重みは、試行中間点への方向の多様性を確保するために決定論的に又はランダムに選択することができる。

【００４６】次いで、ブロック１１２に示すように、各試行中間点は、次の式から計算される。

【００４７】

ブロック１２８及び更に図９に示すように、直交補数に対してｊ−１個のベースベクトルを計算することである。 最後に、ブロック１３０に示すように、次元数ｎｄ

ｉｍは、ブロック１２６における１ベクトルの削除を考慮してｊ−１にセットされる。 ｊ個の自然ベースが全て使用される場合には、ブロック１３２に示すように、ｎ

ｄｉｍがｊにセットされる。

【００４８】図８は、図７のブロック１２６を説明するものである。 冗長な自然ベースベクトルを削除する第１
のステップは、ブロック１３４に示すように、１組のベクト

において、手前のブロックで計算された最も大きなドット積のインデックスを決定することである。 ブロック１

３６内のフローチャートに詳細に示されたように、これは、単に、ドット積の次々の値を、ｄ

_ｉに初期化された現在最大値と比較するだけである。 ドット積が現在最大値より大きいと分かった場合には、新たな最大値が宣言される。 ｊ個のドット積が全てこのように比較されたときに、現在最大値が全てのドット積の最大値となり、その最大値に関連したインデックスｉ

_ｍａｘが分かる。 最後に、インデックスｉ

_ｍａｘをもつ自然ベースベクトルを除いて、ｊ−１個の自然ベースベクトルが選択される。 便宜上、ブロック１３８に示すように、ｊ−１個のベクトルの組がテーブルから選択される。

【００４９】図９は、ｊ−１個のベースベクトルが直交補数として計算された図７のブロック１２８を説明するものである。 このプロセスへの入力は、ｊ−１個の自然ベー

経路セグメントベクトル ットされ、そしてブロック１４４において１だけ増加される。 ｉの各値に対し、ブロック１４６において中間ベクトルｚが次のように計算される。 ベースベクトルは、ブロック１４８に示すように、次の式を用いて、ｚベクトルの長さを１に対して正規化することにより得られる。 ブロック１５０において、ｉの値がチェックされ、ｊ−

１個の全てのベースベクトルが得られたかどうか決定される。 もしそうであれば、ｉ＝１、２・・・ｊ−

【００５０】 部分的に決定される移動への拡張 ：ある次元において既に計画された移動を変更せずに中間点を挿入することが望まれる場合には、ここに述べるように方法を変更することができる。 図６における試行中間点ｋ
の定義から、所定の動きをたどるように制限される各素子ｍに対して必要とされる制限式は、次のようになる。

【００５１】

_ｍｋ ＝０に設定するだけで満足することができる。 残りの重みは、前記のように選択することができる。

【００５２】

６次元のｃ空間であって 運動を中間点により変更すべきでない場合について考える。 この場合、 ｊ＝６ ｎｄｉｍ＝５ ｍ

_１ ＝１、ｍ

_２ ＝２、ｍ

_３ ＝３ である。 マトリクス形態では、３つの制限式が次のように変換される。 但し、ｂ

_ｉｊはベースベクトルｉの素子ｊである。 これは、５つの未知数（ｗ

_ｉｋ 、但し、ｉ＝１、２、３、

４、５）において３つの式（各行）を有する式のアンダーデターミンド（ｕｎｄｅｒ−ｄｅｔｅｒｍｉｎｅｄ）

システムであり、分割、移項及び逆転によりｗ

_ｉｋの３

つに対して他の２つで解くことができる。 これにより得られる解は、２つの独立した重みについて３つの従属する重みを与える。

【００５３】特に、１つの考えられる解は、次の通りである。

_４５ｋ （ｗ

_４ｋ及びｗ

_５ｋ ）の素子は、独立した重み（フリーパラメータ）であり、そしてＷ

_１２３Ｋ （ｗ

_１ｋ ，ｗ

_２ｋ及びｗ

_３ｋ ）の素子は、ｗ

_４ｋ及びｗ

_５ｋについて定められる。 ｇ

_ｉｊは、従属重みｗ

_ｉｋを独立重みｗ

_ｊｋに関連付けるファクタでありそして上記式の指示されたマトリクス演算により定められる。 もちろん、３ｘ３マトリクスＢ

_１２３は、可逆でなければならず、さもなくば、独立変数の別の選択を試みなければならない。 ベースベクトルは、直線的に独立しているので、独立重みの１０個の考えられる選択肢（以下の表に示す）の少なくとも１つが可逆のマトリクスを生じる。 ３ｘ３マトリクスの最大行列式を与える選択は、最良の数値的振る舞いを有する。 その結果、経路セグメントに直交しそして１、２又は３

軸のｃ空間に沿った所定の経路に対してずれを伴わないような試行中間点の２つのパラメータ群が得られる。 拡張形態で書き表すと、試行中間点ｋは、次のようになる。 ｗ

_４及びｗ

_５を変えることにより、経路ベクトルに垂直で、且つｃ空間の１、２又は３軸に沿った所定の経路に対してずれを伴わないような平面内のどこかに試行中間点を発生することができる。

【００５４】 結論 ：以上の説明から、本発明は、多数のロボット機構の制御に著しい進歩をもたらし、そして本発明の原理は、衝突を回避するように移動物体の経路を制御しなければならない他の分野にも適用できることが明らかであろう。 特に、本発明は、各物体の経路をおおまかに計画するｃ空間経路計画と、微細な分解能レベルでの人為的な力フィールド実行との新規な組合せを提供する。 この組合せは、本発明以前にこれら解決策の各々がもたらした問題を解消する。 本発明の他の特徴は、ｃ
空間経路計画の改良に係り、より詳細には、中程度の計算要求のみで作用空間を通る経路を計画するための反復ｃ空間技術に係る。 又、本発明の特定の実施形態を以上に説明したが、本発明の精神及び範囲から逸脱せずに、
種々の変更がなされ得ることも明らかであろう。 従って、本発明は、特許請求の範囲のみによって限定されるものとする。

【図面の簡単な説明】

【図１】本発明のトップレベルのシステム構成ブロック図である。

【図２】本発明においておおまかな経路計画に使用される経路計画手段のブロック図である。

【図３】微細なスケールを用いた経路実行を示すブロック図である。

【図４】例示的なロボットマニピュレータシステムに対する本発明のハードウェア実施を示すブロック図である。

【図５】二次元の配列空間における経路探索の反復を示す図である。

【図６】本発明の１つの特徴により任意に次元の配列空間に試行通路点を発生する方法を示すフローチャートである。

【図７】図６のフローチャートの１つのブロック１０４
を具現化するための方法を示すフローチャートである。

【図８】図７のフローチャートの１つのブロック１２６
を具現化するための方法を示すフローチャートである。

【図９】図７の別のブロック１２８を具現化するための方法を示すフローチャートである。

【符号の説明】

１０ スーパーバイザー １２ 経路計画モジュール １４ 経路実行モジュール １６ ロボットマニピュレータシステム ３０ 経路計画実行部 ３２ ｃ空間データベース ３４ ｃ空間データベース発生モジュール ３６ 経路セグメントタイミングモジュール ３８ マニピュレータシステムモデル ４０ 中間点補間手段 ４２ 制御法則モジュール ４４ マニピュレータ動的モデル ４６ 衝突回避力モデル ４８ マニピュレータシステムモデル ５０ サンプリング手段