JP2008242859A - 物体の運動制御装置及び運動制御方法、並びにコンピュータ・プログラム - Google Patents

Abstract

【課題】制御対象となる移動体の衝突をあらかじめ検知又は予測し、衝突若しくは浸透を回避するための軌道を修正して、制御対象が持つ動作や行動目的（タスク）を効率的且つスムーズに遂行する。
【解決手段】空間上に存在する物体それぞれに衝突の可能性があることを予測するための反力作用領域を配置し、その領域を侵し合う物体間に、物体間の最短距離に応じた反力を発生させることで、物体の軌道を逐次修正し、未然に衝突を回避する。また、物体それぞれの軌道修正量を算出する際には、物体の動作目的のプライオリティを物体の移動プライオリティに反映させる。
【選択図】 図５

Description

本発明は、仮想空間若しくは現実空間上における制御対象となる物体の運動を制御する運動制御装置及び運動制御方法、並びにコンピュータ・プログラムに係り、特に、ともに制御対象となる物体同士の重なりを未然に防ぎながら制御対象となる物体の運動を制御する運動制御装置及び運動制御方法、並びにコンピュータ・プログラムに関する。

さらに詳しくは、本発明は、制御対象となる各物体に人工ポテンシャル場を配置して物体間に人工反力を作用させることで物体間の衝突を回避する運動制御装置及び運動制御方法、並びにコンピュータ・プログラムに係り、特に、制御対象となる移動体の衝突をあらかじめ検知又は予測し、衝突若しくは浸透を回避するための軌道を修正して、制御対象が持つ動作や行動目的（タスク）を効率的且つスムーズに遂行する運動制御装置及び運動制御方法、並びにコンピュータ・プログラムに関する。

力学シミュレーション、精巧なＣＧアニメーション、移動ロボットといった、仮想空間若しくは現実空間からなるシーン上で制御対象となる各物体の運動制御を行なうシステムにおいては、物体同士の重なりを未然に防ぐ必要がある。何故ならば、物体間の重なりは、仮想世界では非現実的な現象であり、現実世界では物体や環境の損傷を招来するからである。

空間上に存在する物体間の衝突検出を行ない、衝突が検出又は予測されたときに、その衝突検出情報を基に制御対象の動作を停止すれば、物体間の浸透（重なり）を未然に防ぐことができる。しかしながら、その制御対象が持つ動作や行動目的（タスク）を効率的且つスムーズに遂行するためには、ただ動作を停止するのではなく、あらかじめ衝突を検知又は予測し、衝突若しくは浸透を回避するための軌道を逐次修正する必要がある。

制御不可能な障害物（静的物体）と制御対象（マニピュレータ）との衝突回避制御として、Ｋｈａｔｉｂらは、人工ポテンシャル場に基づく人工反力生成手段を有したリアルタイム衝突回避制御手法を提案した（例えば、非特許文献１を参照のこと）。この手法によれば、物体間最短距離に基づく人工ポテンシャル場による物体間人工反力を制御物体に作用させ、リアルタイムに衝突回避を実現することができる。しかしながら、人工ポテンシャル場による反力とその方向を、制御対象のある特定の制御点にしか設定することができない。このため、エンドエフェクターの一点といった特定のポイントのみの衝突を考慮して、特定のタスクのみを実現することは可能であるが、任意のポイントに人工反力を発生させることはできないので、衝突予測や衝突回避の制御に厳密性を欠くとともに、複雑な動きを必要とするタスクや運動には不向きである。また、この手法は、制御不可能な障害物と制御対象との衝突回避制御を扱うものであり、制御対象となる移動体を制御不可能な障害物との衝突を回避するように制御することはできるが、ともに制御可能な物体間（例えば、１つのロボットの部位間）で衝突が検知又は予測された場合についての各物体の制御手法若しくは制御戦略に関しては論じられていない。

また、Ｊ．Ｋｕｆｆｎｅｒらは、ヒューマノイドロボットの自己干渉問題解決手法として、凸多面体における高速な距離判定法を用いた、自己干渉問題を考慮した実時間歩行計画手法を提案した（例えば、非特許文献２を参照のこと）。しかしながら、この手法は、あくまでもロボットの動作計画（プランニング）における手法であり、リアクティブな衝突回避制御を行なうためのものではない。

また、Ｆ．Ｓｅｔｏらは、人間と協調作業を行なうロボットにおける実時間での自己衝突回避制御の実現を目的とした，ロボットの弾性要素表現“ＲｏＢＥ（Ｒｅｐｒｅｓｅｎｔａｔｉｏｎ ｏｆ Ｂｏｄｙ ｂｙ Ｅｌａｓｔｉｃ ｅｌｅｍｅｎｔｓ”及びＲｏＢＥを用いたロボット単体での自己衝突回避制御手法（例えば、非特許文献３を参照のこと）、人間との協調作業中においても自己衝突回避を可能とする制御手法（例えば、非特許文献４を参照のこと）について提案し、後者の制御手法に関しては実機（ロボット）を用い有効性を確認している。

これらの手法は、ロボット形状をＲｏＢＥという弾性要素を用いて仮想的に表現し、このような弾性要素モデルを用いることでロボットが運動した際の弾性要素間の接触を検出し、接触した弾性要素間に仮想的な反力を発生させ、この仮想的な反力を用いて実際のロボットを制御することにより、自己衝突の回避を実現するものである。しかしながら、これらの手法では、計算負荷を軽減する目的から、弾性要素を球と円柱として用いていることから、制御対象は粗い物体形状近似として扱われるため、厳密な衝突回避動作を行なうのは困難である。また、仮想的な反力を作用させる方向においても、弾性要素の中心を結んだ直線上であると近似することで問題を簡潔にしているが、ここにおいても、厳密且つ緻密な動作計画を実行するためには問題が多い。

さらにＦ．Ｓｅｔｏらのグループは、さまざまに変化する作業状態や環境に応じ、制御対象にどのような回避運動を行なわせるべきかという問題を考慮し、作業拘束や環境から受ける拘束力に応じたプライオリティ付けに基づく自己干渉回避動作戦略を提案した（例えば、非特許文献５を参照のこと）。この制御戦略では、制御対象となるそれぞれの物体に生じる拘束力に基づいてプライオリティ（プライオリティを反映する変数）を決定するようになっている。しかしながら、ロボットの各部位に課される拘束力は、ロボットの動作計画若しくは行動目的を反映するものではない。言い換えれば、拘束力に基づいて各部位のプライオリティを決定して衝突回避動作を行なっても、ロボット制御の本質、すなわち、ロボットの運動目的（タスク遂行・実行・実現）を直感的且つ厳密に扱うことにはならない。

Ｏｕｓｓａｍａ Ｋｈａｔｉｂ"Ｒｅａｌ−ｔｉｍｅ ｏｂｓｔａｃｌｅ ａｖｏｉｄａｎｃｅ ｆｏｒ ｍａｎｉｐｕｌａｔｏｒｓ ａｎｄ ｍｏｂｉｌｅ ｒｏｂｏｔｓ"（Ｉｎｔ．Ｊ．ｏｆ Ｒｏｂｏｔｉｃｓ Ｒｅｓｅａｒｃｈ，５（１）：９０−９８，１９８６） Ｊ．Ｋｕｆｆｎｅｒ，Ｋ．Ｎｉｓｈｉｗａｋｉ，Ｓ．Ｋａｇａｍｉ，Ｙ．Ｋｕｎｉｙｏｓｈｉ，Ｍ．Ｉｎａｂａ，ａｎｄ Ｈ．Ｉｎｏｕｅ"Ｓｅｌｆ−ｃｏｌｌｉｓｉｏｎ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｐｒｅｖｅｎｔｉｏｎ ｆｏｒ Ｈｕｍａｎｏｉｄ Ｒｏｂｏｔ"（Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ，ｐｐ．２２６５−２２７０，２００２） Ｆ．Ｓｅｔｏ，Ｋ．Ｋｏｓｕｇｅ，ａｎｄ Ｙ．Ｈｉｒａｔａ"Ｒｅａｌ−ｔｉｍｅ Ｃｏｎｔｒｏｌ ｏｆ Ｓｅｌｆ−Ｃｏｌｌｉｓｉｏｎ Ａｖｏｉｄａｎｃｅ ｆｏｒ Ｒｏｂｏｔ ｕｓｉｎｇ ＲｏＢＥ"（Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ＩＥＥＥ−ＲＡＳ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｈｕｍａｎｏｉｄ Ｒｏｂｏｔｓ ２００３，２００３） 瀬戸、小菅、平田共著「ＲｏＢＥを用いたモバイルマニピュレータの実時間自己衝突回避制御−第３報 人間とのインタラクションを考慮した自己衝突回避−」（日本機会学会［Ｎｏ．０４−４］ロボティクス・メカトロニクス講演会‘０４講演論文集、２００４） 瀬戸、小菅、平田共著「ＲｏＢＥを用いたモバイルマニピュレータの実時間自己衝突回避制御−第５報 一般性を有する自己衝突回避動作に対する優先度の導入−」（第１０回ロボティクスシンポジア予稿集、２００５）

本発明の目的は、仮想空間若しくは現実空間上において、ともに制御対象となる物体同士の重なりを未然に防ぎながら制御対象となる物体の運動を好適に制御することができる、優れた運動制御装置及び運動制御方法、並びにコンピュータ・プログラムを提供することにある。

本発明のさらなる目的は、制御対象となる各物体に人工ポテンシャル場を配置して物体間に人工反力を作用させることで物体間の衝突を好適に回避することができる、優れた運動制御装置及び運動制御方法、並びにコンピュータ・プログラムを提供することにある。

本発明のさらなる目的は、制御対象となる移動体の衝突をあらかじめ検知又は予測し、衝突若しくは浸透を回避するための軌道を修正して、制御対象が持つ動作や行動目的（タスク）を効率的且つスムーズに遂行することができる、優れた運動制御装置及び運動制御方法、並びにコンピュータ・プログラムを提供することにある。

本発明は、上記課題を参酌してなされたものであり、その第１の側面は、空間上における物体の運動を制御する運動制御装置であって、
空間上に存在する各物体の位置及び姿勢に関する座標系情報と形状情報、物体間でポテンシャルが作用する距離境界値により定まる反力作用領域をシーンデータとして蓄積するシーンデータ蓄積部と、
空間上に存在する各物体の位置及び姿勢に関する座標系情報と形状情報に基づいて、物体間の最短距離とその最短距離を表す最近傍点対（衝突代表点）と、最近傍点対を結ぶ単位方向ベクトル（衝突代表点方向）の算出を行なう衝突検出部と、
前記衝突検出部により算出された最短距離に基づいて、障害物を回避するための人工反力を物体に発生させるべきか否かを判定する衝突予測部と、
前記衝突予測部において障害物を回避することが必要であると判定された物体に対して、衝突代表点方向への障害物との最短距離に基づく人工反力を算出する人工反力決定部と、
前記衝突予測部において障害物を回避することが必要であると判定された物体の組み合わせに関して、前記人工反力決定部で算出された人工反力を、前記衝突検出部により出力された衝突代表点対に作用させた場合における各物体の軌道修正量を算出する軌道修正量算出部と、
を具備することを特徴とする運動制御装置である。

力学シミュレーション、精巧なＣＧアニメーション、移動ロボットといった、仮想空間若しくは現実空間からなるシーン上で制御対象となる各物体の運動制御を行なうシステムにおいては、物体同士の重なりを未然に防ぐ必要がある。

例えば、人工ポテンシャル場に基づく人工反力生成手段を有したリアルタイム衝突回避制御手法によれば、物体間最短距離に基づく人工ポテンシャル場による物体間人工反力を制御物体に作用させて、リアルタイムに衝突回避を実現することができる。しかしながら、従来は、人工ポテンシャル場による反力の作用点とその方向を、制御対象物体のある特定の制御点にしか設定することができない。このため、任意のポイントに人工反力を発生させることはできないので、衝突予測や衝突回避の制御に厳密性を欠くとともに、複雑な動きを必要とするタスクや運動には不向きである。

また、従来の衝突回避制御手法の多くは、制御対象となる移動体を制御不可能な障害物との衝突を回避するような衝突回避制御を扱うものであり、ともに制御可能な物体間（例えば、１つのロボットの部位間）で衝突が検知又は予測された場合についての各物体の制御手法若しくは制御戦略に関しては十分には論じられていない。

これに対し、本発明に係る運動制御装置は、前記衝突検出部は、ＧＪＫ（Ｇｉｌｂｅｒｔ−Ｊｏｈｎｓｏｎ−Ｋｅｅｒｔｈｉｄｉｓｔａｎｃｅ ａｌｇｏｒｉｔｈｍ）アルゴリズムを用いて物体間の最短距離とその最短距離を表す最近傍点対（衝突代表点）と、最近傍点対を結ぶ単位方向ベクトル（衝突代表点方向）の算出を行なうようになっている。

したがって、前記人工反力決定部は、最近傍点対間を結ぶ単位ベクトル方向に人工ポテンシャル場を配置することによって、より厳密に人工反力を決定することができる。すなわち、ＧＪＫ衝突アルゴリズムを用いて算出された最近傍点対という任意のポイントに人工反力を発生させることで、衝突予測や衝突回避の制御に厳密性を確保するとともに、複雑な動きを必要とするタスクや運動に適応することができる。

また、本発明に係る運動制御装置は、前記衝突予測判定部において判定された物体及び障害物がともに運動制御が可能な制御対象物体である場合には、前記軌道修正量算出部は、前記人工反力決定部により算出された人工反力に基づいて、物体及び障害物それぞれに対する軌道修正量を算出するように構成されている。

したがって、本発明によれば、衝突が予測された双方の物体に対してそれぞれ方向が反対の人工反力を作用させて各物体の軌道を修正する、あるいは、片方の物体のみに人工反力を作用させてもう一方を固定するといった、制御戦略を定めることが可能となる。

このような制御戦略を実現するために、シーンに存在する制御対象物体毎のシーンデータとして、各物体の運動若しくは行動目的（タスク）に基づくプライオリティを、制御対象物体を構成するプリミティブ毎に設定し、シーンデータとしてシーンデータ蓄積部に保持する。そして、人工反力決定部で算出された人工反力を、シーン中のプリミティブとなるそれぞれの制御対象物体に分配するための手段、すなわち人工反力分配部を新たに配設する。したがって、軌道修正量算出部は、人工反力分配部によってそれぞれの制御対象物体に分配された人工反力に基づいて制御対象物体毎の軌道修正量を算出することができる。

人工反力分配部は、人工反力決定部で算出された人工反力を、運動若しくは行動目的（タスク）に設定されているプライオリティに基づいて、衝突が予測された各物体に分配する。高いプライオリティが付与された物体に分配される人工反力が弱まるので、タスクによる軌道が維持される。逆に、低いプライオリティが付与された物体に分配される人工反力は高まるので、タスクによる軌道が大きく修正されることになる。したがって、設定されたタスク遂行のプライオリティが衝突回避制御にそのまま反映されたことになる。

また、すべての制御対象物体対について衝突検出並びに衝突予測を行なうと、空間中の物体数が大幅に増加した場合には、その処理負荷は膨大になってしまう、という問題がある。

そこで、本発明に係る運動制御装置は、物体に衝突予測マージン（例えば、検出対象物体と球（マージン）とのＭｉｎｋｏｗｓｋｉ和）を設定し、衝突検出の対象となる物体を膨張させ、衝突検出対象物体間の衝突を事前に予測する膨張物体衝突予測部を備えていても良い。このような場合、衝突予測部では、膨張物体衝突予測部において衝突の危険性、すなわち衝突回避制御を行なう必要がある物体間のみ、より詳細な最短距離演算を行なうようして、物体間最短距離演算をより効率的に行なうことができる。

ＧＪＫアルゴリズムベースの衝突検出処理において、物体が浸透しているか否かの判定処理は、分離軸を見つけるだけの処理となるので、最短距離演算に比較すると、高速に行なうことができる。したがって、衝突予測マージンを利用した効率的な最短距離演算算出機構を構成することで、計算コストの大幅な削減につながることから、空間中に多くの物体が存在する場合においても、効率的に衝突回避制御を行なうことが可能となる。

また、上述した衝突予測マージンや反力作用領域を、タスクに応じて可変にすることで、より柔軟な運動指定を行なうことができる。

また、本発明の第２の側面は、空間上における物体の運動を制御するための処理をコンピュータ上で実行するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、前記コンピュータに対し、
各物体に設定されるローカル座標系のワールド座標系における位置及び姿勢情報を設定する座標系設定処理手順と、
凸形状に近似した物体間の最短距離、衝突代表点対、及び衝突代表点方向を算出する衝突検出処理手順と、
前記衝突検出処理手順の出力である凸形状物体間最短距離に基づいて、障害物を回避するための人工反力を物体に発生させるべきか否かを判定する衝突予測手順と、
前記衝突予測手順において人工反力を発生させるべきと判定した物体に対して、最近傍点対間を結ぶ単位ベクトル方向に人工ポテンシャル場を配置することにより、衝突代表点方向への物体間の最短距離に基づく人工反力を算出する人工反力算出処理手順と、
前記人工反力算出手順で算出された人工反力を前記衝突検出手順より出力された衝突代表点対に作用させた場合の物体の軌道修正量を算出する軌道修正量算出処理手順と、
を実行させることを特徴とするコンピュータ・プログラムである。

本発明の第２の側面に係るコンピュータ・プログラムは、コンピュータ上で所定の処理を実現するようにコンピュータ可読形式で記述されたコンピュータ・プログラムを定義したものである。換言すれば、本発明の第２の側面に係るコンピュータ・プログラムをコンピュータにインストールすることによって、コンピュータ上では協働的作用が発揮され、本発明の第１の側面に係る運動制御装置と同様の作用効果を得ることができる。

本発明によれば、仮想空間若しくは現実空間上において、ともに制御対象となる物体同士の重なりを未然に防ぎながら制御対象となる物体の運動を好適に制御することができる、優れた運動制御装置及び運動制御方法、並びにコンピュータ・プログラムを提供することができる。

本発明に係る運動制御装置は、制御対象となる各物体に人工ポテンシャル場を配置して物体間に人工反力を作用させることで物体間の衝突回避を実現するものであるが、制御対象となる移動体の衝突をあらかじめ検知又は予測し、衝突若しくは浸透を回避するための軌道を修正して、制御対象が持つ動作や行動目的（タスク）を効率的且つスムーズに遂行することができる。

本発明によれば、ＧＪＫアルゴリズムに基づく物体間最短距離及び最近傍点の算出処理によって得られた最近傍点対を、人工反力を作用させる制御点とすることで、決められた制御点をコントロールする従来の衝突回避手法と比較して、より厳密で柔軟、且つ精密な衝突回避制御を行なうことができる。

勿論、本発明は、複雑なリンク構成からなるオブジェクト、例えばヒューマノイドのような多リンク系オブジェクトにおける自己衝突回避にも適用することができる。

本発明に係る運動制御方法を、ロボットの運動制御にも適用する場合、ロボット制御の本質であるロボットの運動目的達成を考慮した、リアルタイム衝突検出機構を提供することができる。空間上に存在する物体それぞれに衝突の可能性があることを予測するための領域（すなわち反力作用領域）を配置し、その領域を侵し合う物体間に、物体間の最短距離に応じた反力を発生させることで、物体の軌道を逐次修正し、未然に衝突を回避する。

また、本発明によれば、タスクに基づいて物体を形成するプリミティブ毎にプライオリティ付けを行なうことで、物体それぞれの軌道修正量を算出する際には、物体の動作目的（タスク）のプライオリティを物体の移動プライオリティに反映させるとともに、より精密な衝突回避や自己干渉回避を実現することができる。また、制御系でのタスクのプライオリティに基づく動作実現にかかる計算コストを低減するとともに、より緻密且つ直感的な移動体の動作計画が可能となり、物体の動作目的（タスク）が効率的且つスムーズに実現される。

また、物体間の衝突検出を行なう際に、衝突予測マージンを導入することによって、最短距離算出処理を効率化して、計算コストを大幅に削減して、効率的な衝突・干渉回避制御を行なうことができる。

本発明によれば、計算コストを抑えることで、リアルタイムに衝突回避制御を行なうことができる。

本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。

以下、図面を参照しながら本発明の実施形態について詳解する。

まず、図１に示すような、３次元空間上に存在する、運動制御可能な物体（制御対象物体）と制御不可能な静的物体（障害物）との衝突回避問題を例にとって考察する。同図中の制御対象物体は、凹形状で表されており、その動作は制御可能なものとする。また、障害物は、操作対象ではなく、その動きや位置を制御することができない。同図中で、○で表されたポイントは、制御対象物体が最終的に目指す目標地点（ゴール）であり、制御対象の運動目的（タスク）を「このゴール地点に到達すること」に設定する。また、障害物と制御対象物体は同一空間を占有すること、すなわち浸透（重なり）は許されないものとする。

制御対象物体は、例えば自律型のヒューマノイドロボットのように、ゴールを目指す際に必要となる目標地点への移動計画を行なう軌道計画手段を具備しているものと仮定する。３次元空間上に障害物が存在しないときには、制御対象物体は、図２に示すような軌道でゴール地点を目指す。一方、図１に示すように、障害物が３次元空間中に存在するときには、制御対象をゴール地点に到達させるには、障害物を回避するための障害物回避手段、すなわち、逐次衝突回避手段が必要である。このような逐次衝突回避手段を実現するための主な技術課題として、（１）衝突検出若しくは衝突予測を行なう手段（すなわち物体間最短距離算出手段）と、（２）衝突を回避するための物体軌道を修正するための軌道修正手段が挙げられる。以下では、これら２つの技術課題について順次述べていく。

衝突検出・衝突予測手段：
本実施形態では、衝突を検出又は予測する制御対象物体の形状を、凸包を用いて近似する。図３には、図１に示した制御対象物体の凸包近似例を示している。凸包は、頂点集合を包含する最小の凸多角形頂点集合からなるが、球や円柱、四角柱などによる近似に比べると物体へのフィット率が高く、物体形状を比較的厳密に扱うことが可能である。また、物体形状に関する幾何演算を比較的安価に行なうことが可能である。

なお、頂点集合を包含する最小の凸多角形頂点集合を得るための凸包演算として、例えばＧｒａｈａｍ法を用いることができる。Ｇｒａｈａｍ法については、Ｒｏｎａｌｄ Ｇｒａｈａｍ，“Ａｎ Ｅｆｆｉｃｉｅｎｔ Ａｌｇｏｒｉｔｈｍ ｆｏｒ Ｄｅｔｅｒｍｉｎｉｎｇ ｔｈｅ Ｃｏｎｖｅｘ Ｈｕｌｌ ｏｆ ａ Ｆｉｎｉｔｅ Ｐｌａｎａｒ Ｓｅｔ”（Ｉｎｆｏ．Ｐｒｏｃ．Ｌｅｔｔｅｒｓ，１，ｐｐ．１３２−１３３，１９７２）を参照されたい。

本実施形態では、凸形状物体間衝突検出アルゴリズムとして、ＧＪＫ（Ｇｉｌｂｅｒｔ−Ｊｏｈｎｓｏｎ−Ｋｅｅｒｔｈｉ ｄｉｓｔａｎｃｅａｌｇｏｒｉｔｈｍ）を想定する。ＧＪＫアルゴリズムは、凸形図形間距離を算出するための反復的な方法であり、衝突代表点対（最近傍点対又は最浸透点対）を、干渉検出結果として出力する。図４には、ＧＪＫアルゴリズムの概略図を示している。図４Ａは物体間の最近傍点算出処理を表し、図４Ｂは物体間の最浸透点算出処理を表している。同図では、簡素化のため２次元空間表現を用いているが、３次元空間においても成立することを十分理解されたい。

なお、ＧＪＫアルゴリズムの詳細については、例えば、Ｇ．ｖａｎ ｄｅｎ Ｂｅｒｇｅｎ著“Ａ Ｆａｓｔ ａｎｄ Ｒｏｂｕｓｔ ＧＪＫ ｉｍｐｌｅｍｅｎｔａｔｉｏｎ ｆｏｒ Ｃｏｌｌｉｓｉｏｎ Ｄｅｔｅｃｔｉｏｎ ｏｆ Ｃｏｎｖｅｘ Ｏｂｊｅｃｔｓ”（Ｊｏｕｒｎａｌ ｏｆ Ｇｒａｐｈｉｃｓ Ｔｏｏｌｓ，４（２），ｐｐ．７−２５，２００１）を参照されたい。

以下の説明では、特に断りがない限り、最近傍点や最浸透点を算出するアルゴリズムをＧＪＫアルゴリズムと総称して取り扱うことにする。但し、実際には、最近傍点と最浸透点をそれぞれ算出するアルゴリズムやその使用目的は異なる。すなわち、物体同士が浸透していない場合には最近傍点算出アルゴリズムとしてＧＪＫ ｄｉｓｔａｎｃｅアルゴリズムが用いられ、物体同士が浸透している場合には、最浸透点算出アルゴリズムとしてＥＰＡ（Ｅｘｐａｎｄｉｎｇ Ｐｏｌｙｔｏｐｅ Ａｌｇｏｒｉｔｈｍ）が用いられる。

軌道修正手段：
本実施形態では、ポテンシャル法に基づいて、制御対象物体に人工反力を印加して、その軌道修正を逐次行なう。ポテンシャル法は、自律移動ロボットの経路計画などでしばしば用いられている（例えば、非特許文献１を参照のこと）。

ポテンシャルは、障害物から斥力（反力）を受けるような形で設計される。そのポテンシャル関数Ｕ₀（ｘ）を、下式（１）のように設定する。

但し、上式において、ρ並びにρ₀は物体間の最短距離並びにポテンシャルが作用する境界距離（反力作用領域）をそれぞれ表し、ηはスケール因子である。本実施形態では、ここで言う物体間最短距離ρとして、上記の衝突検出・衝突予測手段の出力である物体間最短距離を用いる。最短距離算出時に同時に得られる最近傍点対に対して、ポテンシャル場により生ずる力（反力）を作用させ、その最近傍点対を結ぶ単位ベクトルをその反力が作用する方向に設定する。すなわち、上式（１）で表されるポテンシャルによる人工反力は、下式（２）のように表される。

但し、ｄは最近傍点対を結ぶ単位ベクトルであり、下式（３）のように表される。

なお、上式（３）中のＰ_a及びＰ_bはそれぞれ衝突検出対象物体Ａ及びＢにおける衝突代表点（最近傍点）を表す。

上述した衝突検出・衝突予測手段と、軌道修正手段により、目標とする凸物体間最短距離・最近傍点に基づいて人工ポテンシャル場反力を生成するための手段が構成される。そして、生成される人工反力に基づいて、制御対象物体の軌道を修正することによって、物体間の衝突を未然に回避するための逐次制御を実現することができる。これにより、制御対象物体間の最短距離・最近傍点対算出手段を一般化して人工反力を求め、制御点を制御対象物体の任意の点（最近傍点対）に設定すること、すなわち一般化することで、あらかじめ決められた（言い換えれば固定された）制御点をコントロールするのに比較して、より厳密且つ柔軟な衝突回避制御を行なうことができる。

図５には、運動制御システムの機能ブロック図を示している。図示の運動制御システムは、物体間最短距離の算出によって得られた最近傍点対間を結ぶ単位ベクトル方向に人工ポテンシャル場により生ずる人工反力を作用させるという、人工物体間反力生成手法及び物体間人工反力に基づく衝突回避制御を行なう。図示のシステム１０は、主に座標系設定部１１と、衝突検出部１２と、物体間人工反力算出部１３と、衝突回避制御部１４により構成される。

座標系設定部１１は、例えば各種動力学演算を担う力学シミュレーション・エンジンなどを用いて構成され、各物体の運動を決定する。

衝突検出部１２は，シーンデータ蓄積部１２１と、シーンデータを基に計算幾何学演算を行なう計算幾何学演算部１２２より構成される。

ここで、シーンデータ蓄積部１２１は、シーンに存在する制御対象物体毎の、各物体上に設定されたローカル座標系のワールド座標系内での位置及び姿勢情報（座標系情報）と、物体の形状に関する情報（形状情報）などを、制御対象物体を構成するプリミティブ毎にシーンデータとして保持している。形状情報として、物体を構成する頂点や面等に関する３Ｄ形状モデル、物体の凸包を構成する頂点情報として３Ｄ凸包形状モデル、その頂点の接続関係を表す隣接頂点グラフ（頂点探索を高速に実行するためのグラフ構造）が保持される。また、シーンに存在する物体の組み合わせ毎に、物体間人工反力が作用する領域（反力作用領域）を定める固定値である距離境界値（反力作用領域距離境界値）も、シーンデータとして保持される。

また、計算幾何学演算部１２２は、主に、サポート写像実行部１２２１とＧＪＫアルゴリズム実行部１２２２より構成される。

サポート写像実行部１２２１は、図６に示すように、任意の凸物体の頂点のうち，あるベクトル方向で最も遠くに存在する点を算出する、計算幾何学における基本演算を実行する。なお、サポート写像の算出方法自体は本発明の要旨ではないので、詳細はＧ．ｖａｎ ｄｅｎ Ｂｅｒｇｅｎ著“Ａ Ｆａｓｔ ａｎｄ Ｒｏｂｕｓｔ ＧＪＫ ｉｍｐｌｅｍｅｎｔａｔｉｏｎ ｆｏｒ Ｃｏｌｌｉｓｉｏｎ Ｄｅｔｅｃｔｉｏｎ ｏｆ Ｃｏｎｖｅｘ Ｏｂｊｅｃｔｓ”（Ｊｏｕｒｎａｌ ｏｆ Ｇｒａｐｈｉｃｓ Ｔｏｏｌｓ，４（２），ｐｐ．７−２５，２００１）を参照されたい。

ＧＪＫアルゴリズム実行部１２２２は、衝突検出処理を行なう際に必要となるコア演算を実行する部分であり、サポート写像実行部１２２１を繰り返し用い、物体間最短距離とその最短距離を表す最近傍点対（衝突代表点）を算出し、また、最近傍点対を結ぶ単位方向ベクトル（衝突代表点方向）の算出を行なう。ここで算出された、物体間最短距離、衝突代表点対、及び衝突代表点方向は、衝突検出部１２の結果として出力される。

物体間人工反力算出部１３は、主に、衝突予測部１３１と、人工反力決定部１３２により構成される。

衝突予測部１３１は、衝突検出部１２の出力である最短距離を利用して、物体が人工ポテンシャルの作用領域（反力作用領域）内に存在するか否か、すなわち、障害物を回避するための人工反力を物体に発生させるべきか否かを判定する。

人工反力決定部１３２は、衝突予測部１３１において障害物を回避することが必要であると判定された物体に対して動作して、衝突代表点方向への物体間の最短距離に基づく人工反力を算出する。ここで算出された人工反力は、物体間人工反力算出部１３の結果として出力される。

衝突回避制御部１４は、軌道修正量算出部１４１により構成される。軌道修正量算出部１４１では、人工反力決定部１３２の出力である人工反力を、衝突検出部１２により出力された衝突代表点対に作用させた場合の、物体軌道修正量を算出する。

図７には、図５に示した運動制御システムにおいて実行される処理手順をフローチャートの形式で示している。

座標系設定処理ステップＳ１では、シーンデータ内の各物体に設定されるローカル座標系のワールド座標系における位置及び姿勢情報を設定する。座標系設定部１１は、例えば各種動力学演算を担う力学シミュレーション・エンジンなどを用いて構成され、各物体の運動を決定する。

次いで、衝突検出処理ステップＳ２では、シーンデータと計算幾何学演算部１２２内におけるＧＪＫアルゴリズム実行部１２２２により、凸形状に近似した物体間の最短距離、衝突代表点対、及び衝突代表点方向を算出する。

次いで、衝突予測ステップＳ３では、衝突検出処理ステップの出力である凸形状物体間最短距離を利用して、物体が人工ポテンシャルの作用領域（反力作用領域）内に存在するか否か、すなわち、障害物を回避するための人工反力を発生させるべきか否かを判定する。同ステップにおいて、衝突が予測された場合はステップＳ４へ進むが、衝突が予測されない場合にはＳ５へと処理が進む。

人工反力算出処理ステップＳ４では、衝突代表点方向への物体間の最短距離に基づく人工反力を、上式（２）に基づいて算出する。

軌道修正量算出処理ステップＳ５では、人工反力算出ステップで算出された人工反力を、衝突検出ステップより出力された衝突代表点対に作用させた場合の物体の軌道修正量を算出する。

これまでの説明では、制御可能な物体と制御不可能な障害物との衝突回避制御手法について述べたが、本発明を、ともに制御可能な物体間における衝突回避制御手法にも拡張することができる。後者の場合、衝突が予測された物体それぞれの運動を制御することが可能であることから、軌道修正量算出部１４１は、人工ポテンシャル場を配置するより生じる人工反力に基づいて各物体それぞれに対する軌道修正を行なうように構成する必要がある。軌道修正量算出部１４１をこのように構成することによって、衝突が予測された双方の物体に対してそれぞれ方向が反対の人工反力を作用させて各物体の軌道を修正する、あるいは、片方の物体のみに人工反力を作用させてもう一方を固定するといった、制御戦略を定めることが可能となる。

このような制御戦略を実現するために、シーンに存在する制御対象物体毎のシーンデータとして、各物体の運動若しくは行動目的（タスク）に基づくプライオリティを、制御対象物体を構成するプリミティブ毎に設定し、シーンデータとしてシーンデータ蓄積部１２に保持する。そして、人工反力決定部１３２で算出された人工反力を、シーン中のプリミティブとなるそれぞれの制御対象物体に分配する手段、すなわち人工反力分配部を新たに配設する。したがって、軌道修正量算出部１４１では、それぞれの制御対象物体に分配された人工反力に基づいて制御対象物体毎の軌道修正量を算出することができる。

ここでは、タスクに応じて制御対象物体毎にプライオリティを付与する一例を示して、制御可能な物体間における衝突回避制御手法について説明する。

まず、タスクによるプライオリティを数段階の数値（例えば、０，１，２，３）で表す。ここで、プライオリティ０を最高プライオリティとし、数値が大きくなるにつれてプライオリティが低くなるものとする。このプライオリティを制御対象物体プリミティブ毎に設定してシーンデータ蓄積部１２１に保持しておく。そして、衝突が予測された制御対象物体間に人工ポテンシャル場を配置して人工反力が決定されると、人工反力分配部は、当該タスクにおいて各制御対象物体に設定されているプライオリティを参照して、各制御対象物体に及ぼす人工反力の作用度合いを操作する。下式（４）には、人工反力分配方法の一例を示している。

但し、上式（４）において、Ｆ₀は人工ポテンシャル場から生ずる人工反力であり、上式（３）に従って算出される。また、Ｑ_pa及びＱ_pbは、それぞれ物体Ａ及びＢに設定されたタスクによるプライオリティである。そして、Ｆ_0,a及びＦ_0,bは物体Ａ及びＢそれぞれに作用する人工反力であり、人工反力Ｆ₀は、各物体Ａ及びＢが運動又は行動目的（タスク）に対して付与されたプライオリティに基づいて各物体Ａ及びＢに分配される。

上式（４）によれば、高いプライオリティが付与された物体に分配される人工反力が弱まるので、タスクによる軌道が維持される。逆に、低いプライオリティが付与された物体に分配される人工反力は高まるので、タスクによる軌道が大きく修正されることになる。したがって、設定されたタスク遂行のプライオリティが衝突回避制御にそのまま反映されたことになる。

このように、タスクに基づくプライオリティ付けを行ない、そのプライオリティを衝突回避制御に反映することで、運動目的（タスク）をより直感的に扱うことができる。また、計算コストがかかる制御系でタスクのプライオリティを管理する方法（非特許文献１を参照のこと）と比較すると、大幅に計算コストを削減することが可能になる。さらに、より精密な衝突回避制御を実現することが可能になるといった利点もある。

図８には、制御対象物体毎にタスクに応じたプライオリティを付与して衝突回避制御を行なう運動制御システムの構成を示している。図示のシステム１０は、主に座標系設定部１１と、衝突検出部１２と、物体間人工反力算出部１３と、人工反力分配部１５と、衝突回避制御部１４により構成される。

座標系設定部１１は、例えば各種動力学演算を担う力学シミュレーション・エンジンなどを用いて構成され、各物体の運動を決定する。

衝突検出部１２は，シーンデータ蓄積部１２１と、シーンデータを基に計算幾何学演算を行なう計算幾何学演算部１２２より構成される。

シーンデータ蓄積部１２１は、シーンに存在する制御対象物体毎の、各物体上に設定されたローカル座標系のワールド座標系内での位置及び姿勢情報（座標系情報）と、物体の形状に関する情報（形状情報）などを、制御対象物体を構成するプリミティブ毎にシーンデータとして保持している。形状情報として、物体を構成する頂点や面等に関する３Ｄ形状モデル、物体の凸包を構成する頂点情報として３Ｄ凸包形状モデル、その頂点の接続関係を表す隣接頂点グラフ（頂点探索を高速に実行するためのグラフ構造）が保持される。また、シーンに存在する物体の組み合わせを衝突回避物体対リストにリストアップし、リスト中の物体対毎に、物体間人工反力が作用する領域（反力作用領域）を定める固定値である距離境界値（反力作用領域距離境界値）も、シーンデータとして保持される。また、各物体の運動若しくは行動目的（タスク）に基づくプライオリティが制御対象物体を構成するプリミティブ毎に設定されており、これがシーンデータとして保持される。但し、運動プライオリティは、固定データではなく、タスクによりか変なものとする。

計算幾何学演算部１２２は、主に、サポート写像実行部１２２１とＧＪＫアルゴリズム実行部１２２２より構成される。

サポート写像実行部１２２１は、図６に示すように、任意の凸物体の頂点のうち，あるベクトル方向で最も遠くに存在する点を算出する、計算幾何学における基本演算を実行する（同上）。

ＧＪＫアルゴリズム実行部１２２２は、衝突検出処理を行なう際に必要となるコア演算を実行する部分であり、サポート写像実行部１２２１を繰り返し用い、物体間最短距離とその最短距離を表す最近傍点対（衝突代表点）を算出し、また、最近傍点対を結ぶ単位方向ベクトル（衝突代表点方向）の算出を行なう。ここで算出された、物体間最短距離、衝突代表点対、及び衝突代表点方向は、衝突検出部１２の結果として出力される。

物体間人工反力算出部１３は、主に、衝突予測部１３１と、人工反力決定部１３２により構成される。

衝突予測部１３１は、衝突検出部１２の出力である最短距離を利用して、物体が人工ポテンシャルの作用領域（反力作用領域）内に存在するか否か、すなわち、障害物を回避するための人工反力を物体に発生させるべきか否かを判定する。

人工反力決定部１３２は、衝突予測部１３１において障害物を回避することが必要であると判定された物体に対して動作して、衝突代表点方向への物体間の最短距離に基づく人工反力を算出する。ここで算出された人工反力は、物体間人工反力算出部１３の結果として出力される。

人工反力分配部１５は、人工反力決定部１３２で算出した人工反力を、上式（４）に従って、衝突予測された各制御対象物体に分配する。すなわち、シーンデータの物体情報であるタスクによる運動プライオリティに基づいて、衝突回避のための物体それぞれに作用させるべき分配人工反力を算出する。

衝突回避制御部１４は、軌道修正量算出部１４１により構成される。軌道修正量算出部１４１では、人工反力分配部１５の出力である制御対象物体毎の分配人工反力を、衝突検出部１２により出力された衝突代表点対に作用させた場合の物体軌道修正量を、制御対象物体毎に算出する。

図９には、図８に示した運動制御システムにおいて実行される処理手順をフローチャートの形式で示している。

座標系設定処理ステップＳ１１では、シーンデータ内の各物体に設定されるローカル座標系のワールド座標系における位置及び姿勢情報を設定する。座標系設定部１１は、例えば各種動力学演算を担う力学シミュレーション・エンジンなどを用いて構成され、各物体の運動を決定する。

次いで、衝突検出処理ステップＳ１２では、シーンデータと計算幾何学演算部１２２内におけるＧＪＫアルゴリズム実行部１２２２により、凸形状に近似した物体間の最短距離、衝突代表点対、及び衝突代表点方向を算出する。

次いで、衝突予測ステップＳ１３では、衝突検出処理ステップの出力である凸形状物体間最短距離を利用して、物体が人工ポテンシャルの作用領域（反力作用領域）内に存在するか否か、すなわち、障害物を回避するための人工反力を発生させるべきか否かを判定する。同ステップにおいて、衝突が予測された場合はステップＳ１４へ進むが、衝突が予測されない場合にはＳ１６へと処理が進む。

人工反力算出処理ステップＳ１４では、衝突代表点方向への物体間の最短距離に基づく人工反力を、上式（２）に基づいて算出する。

分配人工反力算出処理ステップＳ１５では、人工反力算出処理ステップＳ１４で算出した人工反力を、上式（４）に従って、衝突予測された各制御対象物体に分配する。すなわち、人工反力算出処理ステップＳ１４により算出された人工反力をシーンデータの物体情報であるタスクによる運動プライオリティに基づいて、衝突回避のための物体それぞれに作用させるべき分配人工反力を算出する。

軌道修正量算出ステップＳ１６では、人工反力分配部１５の出力である制御対象物体毎の分配人工反力を、衝突検出部１２により出力された衝突代表点対に作用させた場合の物体軌道修正量を、制御対象物体毎に算出する。

これまでの説明では、シーンに存在する物体の組み合わせを衝突回避物体対リストにリストアップし、リスト中の物体対毎に、制御期間中は常に最短距離演算を行なって、衝突検出並びに衝突予測を行なうようにしていた。高速な演算処理装置を用いて衝突検出処理を行なっているとしても、空間中の物体数が大幅に像化した場合には、その処理負荷は膨大となる。そこで、本発明に係る運動制御装置は、物体間最短距離演算をより効率的に行なう手段を備えていることが好ましい。

物体間最短距離演算をより効率的に行なう手段は、例えば、制御対象物体を膨張させてから衝突予測を行なう膨張物体衝突予測部として構成することができる。具体的には、膨張物体衝突予測部は、図１０に示すように物体に衝突予測マージン（例えば、検出対象物体と球（マージン）とのＭｉｎｋｏｗｓｋｉ和）を設定し、衝突検出の対象となる物体を膨張させ、衝突検出対象物体間の衝突を事前に予測する。そして、計算幾何学演算部１２では、膨張物体衝突予測部において衝突の危険性、すなわち衝突回避制御を行なう必要がある物体間のみ、より詳細な最短距離演算を行なうようにする。

ＧＪＫアルゴリズムベースの衝突検出処理において、物体が浸透しているか否かの判定処理は、分離軸を見つけるだけの処理となるので、最短距離演算に比較すると、高速に行なうことができる。したがって、衝突予測マージンを利用した効率的な最短距離演算算出機構を構成することで、計算コストの大幅な削減につながることから、空間中に多くの物体が存在する場合においても、効率的に衝突回避制御を行なうことが可能となる。

なお、球とのＭｉｎｋｏｗｓｋｉ和を考慮したＧＪＫアルゴリズムベースの衝突検出方法に関しては、例えば、Ｇｉｎｏ ｖａｎ ｄｅｎ Ｂｅｒｇｅｎ，“Ｃｏｌｌｉｓｉｏｎ Ｄｅｔｅｃｔｉｏｎ ｉｎ Ｉｎｔｅｒａｃｔｉｖｅ ３Ｄ Ｅｎｖｉｒｏｎｍｅｎｔｓ”（Ｍｏｒｇａｎ Ｋａｕｆｍａｎｎ Ｐｕｂｌｉｓｈｅｒｓ，２００３）を参照されたい。

図１１には、衝突予測マージンを利用して効率的な最距離演算を行なうように構成された運動制御システムの構成を示している。図示のシステム１０は、主に座標系設定部１１と、衝突検出部１２と、物体間人工反力算出部１３と、人工反力分配部１５と、衝突回避制御部１４により構成される。

座標系設定部１１は、例えば各種動力学演算を担う力学シミュレーション・エンジンなどを用いて構成され、各物体の運動を決定する。

衝突検出部１２は，シーンデータ蓄積部１２１と、シーンデータを基に計算幾何学演算を行なう計算幾何学演算部１２２と、衝突検出の対象となる物体を膨張させて衝突検出対象物体間の衝突を事前に予測する膨張物体衝突予測部１２３より構成される。

シーンデータ蓄積部１２１は、シーンに存在する制御対象物体毎の、各物体上に設定されたローカル座標系のワールド座標系内での位置及び姿勢情報（座標系情報）と、物体の形状に関する情報（形状情報）などを、制御対象物体を構成するプリミティブ毎にシーンデータとして保持している。形状情報として、物体を構成する頂点や面等に関する３Ｄ形状モデル、物体の凸包を構成する頂点情報として３Ｄ凸包形状モデル、その頂点の接続関係を表す隣接頂点グラフ（頂点探索を高速に実行するためのグラフ構造）が保持される。また、シーンに存在する物体の組み合わせを衝突回避物体対リストにリストアップし、リスト中の物体対毎に、物体間人工反力が作用する領域（反力作用領域）を定める固定値である距離境界値（反力作用領域距離境界値）も、シーンデータとして保持される。また、各物体の運動若しくは行動目的（タスク）に基づくプライオリティが制御対象物体を構成するプリミティブ毎に設定されており、これがシーンデータとして保持される。但し、運動プライオリティは、固定データではなく、タスクにより可変なものとする。また、物体間最短距離算出の計算コストを軽減するための膨張物体間衝突検出を行なう際に用いられる衝突予測マージンも保持される。

計算幾何学演算部１２２は、主に、サポート写像実行部１２２１とＧＪＫアルゴリズム実行部１２２２より構成される。

サポート写像実行部１２２１は、図６に示すように、任意の凸物体の頂点のうち，あるベクトル方向で最も遠くに存在する点を算出する、計算幾何学における基本演算を実行する（同上）。

ＧＪＫアルゴリズム実行部１２２２は、衝突検出処理を行なう際に必要となるコア演算を実行する部分であり、サポート写像実行部１２２１を繰り返し用い、物体間最短距離とその最短距離を表す最近傍点対（衝突代表点）を算出し、また、最近傍点対を結ぶ単位方向ベクトル（衝突代表点方向）の算出を行なう。ここで算出された、物体間最短距離、衝突代表点対、及び衝突代表点方向は、衝突検出部１２の結果として出力される。

膨張物体間衝突検出部１２３は、シーンデータ蓄積部１２１に保持されている物体情報である衝突予測マージンに基づいて検出の対象となる物体をそれぞれ膨張させ、これら膨張させた物体間の衝突検出を行なう。そして、膨張物体間衝突検出部１２３により衝突が起きる可能性があると判断された場合のみ、ＧＪＫアルゴリズム実行部１２２２によって、物体間最短距離算出、最近傍点対算出、及び衝突代表点方向算出処理が行なわれる。

物体間人工反力算出部１３は、主に、衝突予測部１３１と、人工反力決定部１３２により構成される。

衝突予測部１３１は、衝突検出部１２の出力である最短距離を利用して、物体が人工ポテンシャルの作用領域（反力作用領域）内に存在するか否か、すなわち、障害物を回避するための人工反力を物体に発生させるべきか否かを判定する。

人工反力決定部１３２は、衝突予測部１３１において障害物を回避することが必要であると判定された物体に対して動作して、衝突代表点方向への物体間の最短距離に基づく人工反力を算出する。ここで算出された人工反力は、物体間人工反力算出部１３の結果として出力される。

人工反力分配部１５は、人工反力決定部１３２で算出した人工反力を、上式（４）に従って、衝突予測された各制御対象物体に分配する。すなわち、シーンデータの物体情報であるタスクによる運動プライオリティに基づいて、衝突回避のための物体それぞれに作用させるべき分配人工反力を算出する。

衝突回避制御部１４は、軌道修正量算出部１４１により構成される。軌道修正量算出部１４１では、人工反力分配部１５の出力である制御対象物体毎の分配人工反力を、衝突検出部１２により出力された衝突代表点対に作用させた場合の物体軌道修正量を、制御対象物体毎に算出する。

図１２には、図１１に示した運動制御システムにおいて実行される処理手順をフローチャートの形式で示している。

座標系設定処理ステップＳ２１では、シーンデータ内の各物体に設定されるローカル座標系のワールド座標系における位置及び姿勢情報を設定する。座標系設定部１１は、例えば各種動力学演算を担う力学シミュレーション・エンジンなどを用いて構成され、各物体の運動を決定する。

次いで、膨張物体衝突予測ステップＳ２１では、衝突予測マージンにより膨張した物体間の衝突検出を実行する。同ステップにおいて膨張物体間での衝突が予測された場合には次ステップＳ３へ進み、予測されない場合は本処理ルーチンを終了する。

衝突検出処理ステップＳ２３では、シーンデータと計算幾何学演算部１２２内におけるＧＪＫアルゴリズム実行部１２２２により、凸形状に近似した物体間の最短距離、衝突代表点対、及び衝突代表点方向を算出する。

次いで、衝突予測ステップＳ２４では、衝突検出処理ステップの出力である凸形状物体間最短距離を利用して、物体が人工ポテンシャルの作用領域（反力作用領域）内に存在するか否か、すなわち、障害物を回避するための人工反力を発生させるべきか否かを判定する。同ステップにおいて、衝突が予測された場合はステップＳ２５へ進むが、衝突が予測されない場合にはＳ２７へと処理が進む。

人工反力算出処理ステップＳ２５では、衝突代表点方向への物体間の最短距離に基づく人工反力を、上式（２）に基づいて算出する。

分配人工反力算出処理ステップＳ２６では、人工反力算出処理ステップＳ２５で算出した人工反力を、上式（４）に従って、衝突予測された各制御対象物体に分配する。すなわち、人工反力算出処理ステップＳ２５により算出された人工反力をシーンデータの物体情報であるタスクによる運動プライオリティに基づいて各物体に分配して、衝突回避のための物体それぞれに作用させるべき分配人工反力を算出する。

軌道修正量算出ステップＳ２７では、人工反力分配部１５の出力である制御対象物体毎の分配人工反力を、衝突検出部１２により出力された衝突代表点対に作用させた場合の物体軌道修正量を、制御対象物体毎に算出する。

なお、運動プライオリティが固定データではなくタスクにより可変なものであることは既に述べたが、衝突予測マージンや反力作用領域（人工反力の作用する境界距離）に関しても、固定値である必要はなく、制御期間中は常に変更可能なパラメータとして扱うこともできる。その一例として、運動目的（タスク）に応じて、各物体の衝突予測マージン及び反力作用領域を可変にするよう構成することができる。

各物体の衝突予測マージン及び反力作用領域を可変にすることで、より柔軟な運動指定を実現することが可能になる。例えば、それほど精密な衝突回避を必要としないようなタスクの場合、若しくは、より衝突を嫌う制御を行ないたい場合などには、衝突予測マージン又は反力作用領域のうち少なくとも一方を大きくとることで、物体間の間隔を大きく保ちながら、物体の粗い移動制御を行なうことができ、あるいはより安全な移動制御を行なうことができる。この場合、図５におけるシーンデータにおいて、物体間人工反力が作用する領域（反力作用領域）を定める距離境界値（反力作用領域距離境界値）は、変更可能なパラメータとなり、タスクにより決定される値となる。

以上、特定の実施形態を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施形態の修正や代用を成し得ることは自明である。

本発明は、力学シミュレーション、精巧なＣＧアニメーション、移動ロボットといった、仮想空間若しくは現実空間上で物体（制御対象）の運動制御を行なうさまざまなシステムに適用することができる。

本明細書では、複数の制御対象物体間における衝突回避制御手法、若しくは制御対象物体と制御不可能な障害物における衝突回避制御手法について述べてきたが、制御対象物体が複数のプリミティブで構成された場合においても、同様に本発明を適用することが可能である。勿論、ヒューマノイドロボットにおける自己干渉回避制御にも本発明を適用することができる。

要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、特許請求の範囲を参酌すべきである。

図１は、３次元空間上に存在する、運動制御可能な物体（制御対象物体）と制御不可能な静的物体（障害物）との衝突回避問題を例示した図である。 図２は、３次元空間上に障害物が存在しないときに、制御対象物体がゴール地点を目指す軌道を例示した図である。 図３は、図１に示した制御対象物体の凸包近似例を示した図である。 図４Ａは、物体間の最近傍点算出処理を表した図である。 図４Ｂは、物体間の最浸透点算出処理を表した図である。 図５は、人工物体間反力生成手法及び物体間人工反力に基づく衝突回避制御を行なう運動制御システムの機能ブロック図である。 図６は、サポート写像により任意の凸物体の頂点のうち，あるベクトル方向で最も遠くに存在する点を算出する様子を示した図である。 図７は、図５に示した運動制御システムにおいて実行される処理手順を示したフローチャートである。 図８は、制御対象物体毎にタスクに応じたプライオリティを付与して衝突回避制御を行なう運動制御システムの構成を示した図である。 図９は、図８に示した運動制御システムにおいて実行される処理手順を示したフローチャートである。 図１０は、物体に衝突予測マージンを設定した様子を示した図である。 図１１は、衝突予測マージンを利用して効率的な最距離演算を行なうように構成された運動制御システムの構成を示した図である。 図１２は、図１１に示した運動制御システムにおいて実行される処理手順を示したフローチャートである。

符号の説明

１０…運動制御システム
１１…座標系設定部
１２…衝突検出部
１２１…シーンデータ蓄積部
１２２…計算幾何学演算部
１２３…膨張物体衝突予測部
１２２１…サポート写像実行部
１２２２…ＧＪＫアルゴリズム実行部
１３…物体間人工反力算出部
１３１…衝突予測部
１３２…人工反力決定部
１４…衝突回避制御部
１５…人工反力分配部

Claims (9)

1. 空間上における物体の運動を制御する運動制御装置であって、
   空間上に存在する各物体の位置及び姿勢に関する座標系情報と形状情報、物体間でポテンシャルが作用する距離境界値により定まる反力作用領域を、制御対象物体を構成するプリミティブ毎にシーンデータとして蓄積するシーンデータ蓄積部と、
   空間上に存在する各物体の位置及び姿勢に関する座標系情報と形状情報に基づいて、物体間の最短距離とその最短距離を表す最近傍点対（衝突代表点）と、最近傍点対を結ぶ単位方向ベクトル（衝突代表点方向）の算出を行なう衝突検出部と、
   前記衝突検出部により算出された最短距離に基づいて、障害物を回避するための人工反力を物体に発生させるべきか否かを判定する衝突予測部と、
   前記衝突予測部において障害物を回避することが必要であると判定された物体に対して、衝突代表点方向への障害物との最短距離に基づく人工反力を算出する人工反力決定部と、
   前記衝突予測部において障害物を回避することが必要であると判定された物体の組み合わせに関して、前記人工反力決定部で算出された人工反力を、前記衝突検出部により出力された衝突代表点対に作用させた場合における各物体の軌道修正量を算出する軌道修正量算出部と、
   を具備することを特徴とする運動制御装置。
2. 前記衝突検出部は、ＧＪＫ（Ｇｉｌｂｅｒｔ−Ｊｏｈｎｓｏｎ−Ｋｅｅｒｔｈｉ ｄｉｓｔａｎｃｅ ａｌｇｏｒｉｔｈｍ）アルゴリズムを用いて物体間の最短距離とその最短距離を表す最近傍点対（衝突代表点）と、最近傍点対を結ぶ単位方向ベクトル（衝突代表点方向）の算出を行なう、
   ことを特徴とする請求項１に記載の運動制御装置。
3. 前記人工反力決定部は、最近傍点対間を結ぶ単位ベクトル方向に人工ポテンシャル場を配置することにより生じる人工反力を決定する、
   ことを特徴とする請求項１に記載の運動制御装置。
4. 前記衝突予測判定部において判定された物体及び障害物はともに運動制御が可能な制御対象物体であり、
   前記軌道修正量算出部は、前記人工反力決定部により算出された人工反力に基づいて、物体及び障害物それぞれに対する軌道修正量を算出する、
   ことを特徴とする請求項１に記載の運動制御装置。
5. 前記軌道修正量算出部は、物体及び障害物それぞれに対して方向が反対の人工反力を作用させて物体及び障害物それぞれの起動を修正し、又は、物体又は障害物の一方にのみ人工反力を作用させるとともに他方を固定する、
   ことを特徴とする請求項４に記載の運動制御装置。
6. 前記シーンデータ蓄積部は、制御対象となる各物体の運動若しくは行動目的（タスク）に基づいて制御対象物体を構成するプリミティブ毎に設定されているプライオリティをシーンデータとして保持し、
   前記人工反力決定部で算出された人工反力を、運動若しくは行動目的（タスク）に設定されているプライオリティに基づいて、衝突が予測された各物体に分配する人工反力分配部をさらに備え、
   前記軌道修正量算出部は、それぞれの制御対象物体に分配された人工反力に基づいて物体毎の軌道修正量を算出する、
   ことを特徴とする請求項４に記載の運動制御装置。
7. 前記シーンデータ蓄積部は、制御対象となる各物体に設定されている衝突予測マージンをシーンデータとして保持し、
   衝突検出の対象となる物体を衝突予測マージンに基づいて膨張させ、衝突検出対象物体間の衝突を事前に予測する膨張物体衝突予測部をさらに備え、
   前記衝突検出部は、前記膨張物体衝突予測部において衝突が起きる可能性があると判断された物体間のみ、より詳細な最短距離演算を行なう、
   ことを特徴とする請求項１に記載の運動制御装置。
8. 空間上における物体の運動を制御する運動制御方法であって、
   各物体に設定されるローカル座標系のワールド座標系における位置及び姿勢情報を設定する座標系設定処理ステップと、
   凸形状に近似した物体間の最短距離、衝突代表点対、及び衝突代表点方向を算出する衝突検出処理ステップと、
   前記衝突検出処理ステップの出力である凸形状物体間最短距離に基づいて、障害物を回避するための人工反力を物体に発生させるべきか否かを判定する衝突予測ステップと、
   前記衝突予測ステップにおいて人工反力を発生させるべきと判定した物体に対して、最近傍点対間を結ぶ単位ベクトル方向に人工ポテンシャル場を配置することにより、衝突代表点方向への物体間の最短距離に基づく人工反力を算出する人工反力算出処理ステップと、
   前記人工反力算出ステップで算出された人工反力を前記衝突検出ステップより出力された衝突代表点対に作用させた場合の物体の軌道修正量を算出する軌道修正量算出処理ステップと、
   を具備することを特徴とする運動制御方法。
9. 空間上における物体の運動を制御するための処理をコンピュータ上で実行するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、前記コンピュータに対し、
   各物体に設定されるローカル座標系のワールド座標系における位置及び姿勢情報を設定する座標系設定処理手順と、
   凸形状に近似した物体間の最短距離、衝突代表点対、及び衝突代表点方向を算出する衝突検出処理手順と、
   前記衝突検出処理手順の出力である凸形状物体間最短距離に基づいて、障害物を回避するための人工反力を物体に発生させるべきか否かを判定する衝突予測手順と、
   前記衝突予測手順において人工反力を発生させるべきと判定した物体に対して、最近傍点対間を結ぶ単位ベクトル方向に人工ポテンシャル場を配置することにより、衝突代表点方向への物体間の最短距離に基づく人工反力を算出する人工反力算出処理手順と、
   前記人工反力算出手順で算出された人工反力を前記衝突検出手順より出力された衝突代表点対に作用させた場合の物体の軌道修正量を算出する軌道修正量算出処理手順と、
   を実行させることを特徴とするコンピュータ・プログラム。