(57)【要約】【課題】制御対象となる移動体の衝突をあらかじめ検知又は予測し、衝突若しくは浸透を回避するための軌道を修正して、制御対象が持つ動作や行動目的（タスク）を効率的且つスムーズに遂行する。【解決手段】空間上に存在する物体それぞれに衝突の可能性があることを予測するための反力作用領域を配置し、その領域を侵し合う物体間に、物体間の最短距離に応じた反力を発生させることで、物体の軌道を逐次修正し、未然に衝突を回避する。また、物体それぞれの軌道修正量を算出する際には、物体の動作目的のプライオリティを物体の移動プライオリティに反映させる。【選択図】  図５
2008242859.tif 000002
【特許請求の範囲】
【請求項１】
空間上における物体の運動を制御する運動制御装置であって、
空間上に存在する各物体の位置及び姿勢に関する座標系情報と形状情報、物体間でポテンシャルが作用する距離境界値により定まる反力作用領域を、制御対象物体を構成するプリミティブ毎にシーンデータとして蓄積するシーンデータ蓄積部と、
空間上に存在する各物体の位置及び姿勢に関する座標系情報と形状情報に基づいて、物体間の最短距離とその最短距離を表す最近傍点対（衝突代表点）と、最近傍点対を結ぶ単位方向ベクトル（衝突代表点方向）の算出を行なう衝突検出部と、
前記衝突検出部により算出された最短距離に基づいて、障害物を回避するための人工反力を物体に発生させるべきか否かを判定する衝突予測部と、
前記衝突予測部において障害物を回避することが必要であると判定された物体に対して、衝突代表点方向への障害物との最短距離に基づく人工反力を算出する人工反力決定部と、
前記衝突予測部において障害物を回避することが必要であると判定された物体の組み合わせに関して、前記人工反力決定部で算出された人工反力を、前記衝突検出部により出力された衝突代表点対に作用させた場合における各物体の軌道修正量を算出する軌道修正量算出部と、
を具備することを特徴とする運動制御装置。
【請求項２】
前記衝突検出部は、ＧＪＫ（Ｇｉｌｂｅｒｔ−Ｊｏｈｎｓｏｎ−Ｋｅｅｒｔｈｉ ｄｉｓｔａｎｃｅ ａｌｇｏｒｉｔｈｍ）アルゴリズムを用いて物体間の最短距離とその最短距離を表す最近傍点対（衝突代表点）と、最近傍点対を結ぶ単位方向ベクトル（衝突代表点方向）の算出を行なう、
ことを特徴とする請求項１に記載の運動制御装置。
【請求項３】
前記人工反力決定部は、最近傍点対間を結ぶ単位ベクトル方向に人工ポテンシャル場を配置することにより生じる人工反力を決定する、
ことを特徴とする請求項１に記載の運動制御装置。
【請求項４】
前記衝突予測判定部において判定された物体及び障害物はともに運動制御が可能な制御対象物体であり、
前記軌道修正量算出部は、前記人工反力決定部により算出された人工反力に基づいて、物体及び障害物それぞれに対する軌道修正量を算出する、
ことを特徴とする請求項１に記載の運動制御装置。
【請求項５】
前記軌道修正量算出部は、物体及び障害物それぞれに対して方向が反対の人工反力を作用させて物体及び障害物それぞれの起動を修正し、又は、物体又は障害物の一方にのみ人工反力を作用させるとともに他方を固定する、
ことを特徴とする請求項４に記載の運動制御装置。
【請求項６】
前記シーンデータ蓄積部は、制御対象となる各物体の運動若しくは行動目的（タスク）に基づいて制御対象物体を構成するプリミティブ毎に設定されているプライオリティをシーンデータとして保持し、
前記人工反力決定部で算出された人工反力を、運動若しくは行動目的（タスク）に設定されているプライオリティに基づいて、衝突が予測された各物体に分配する人工反力分配部をさらに備え、
前記軌道修正量算出部は、それぞれの制御対象物体に分配された人工反力に基づいて物体毎の軌道修正量を算出する、
ことを特徴とする請求項４に記載の運動制御装置。
【請求項７】
前記シーンデータ蓄積部は、制御対象となる各物体に設定されている衝突予測マージンをシーンデータとして保持し、
衝突検出の対象となる物体を衝突予測マージンに基づいて膨張させ、衝突検出対象物体間の衝突を事前に予測する膨張物体衝突予測部をさらに備え、
前記衝突検出部は、前記膨張物体衝突予測部において衝突が起きる可能性があると判断された物体間のみ、より詳細な最短距離演算を行なう、
ことを特徴とする請求項１に記載の運動制御装置。
【請求項８】
空間上における物体の運動を制御する運動制御方法であって、
各物体に設定されるローカル座標系のワールド座標系における位置及び姿勢情報を設定する座標系設定処理ステップと、
凸形状に近似した物体間の最短距離、衝突代表点対、及び衝突代表点方向を算出する衝突検出処理ステップと、
前記衝突検出処理ステップの出力である凸形状物体間最短距離に基づいて、障害物を回避するための人工反力を物体に発生させるべきか否かを判定する衝突予測ステップと、
前記衝突予測ステップにおいて人工反力を発生させるべきと判定した物体に対して、最近傍点対間を結ぶ単位ベクトル方向に人工ポテンシャル場を配置することにより、衝突代表点方向への物体間の最短距離に基づく人工反力を算出する人工反力算出処理ステップと、
前記人工反力算出ステップで算出された人工反力を前記衝突検出ステップより出力された衝突代表点対に作用させた場合の物体の軌道修正量を算出する軌道修正量算出処理ステップと、
を具備することを特徴とする運動制御方法。
【請求項９】
空間上における物体の運動を制御するための処理をコンピュータ上で実行するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、前記コンピュータに対し、
各物体に設定されるローカル座標系のワールド座標系における位置及び姿勢情報を設定する座標系設定処理手順と、
凸形状に近似した物体間の最短距離、衝突代表点対、及び衝突代表点方向を算出する衝突検出処理手順と、
前記衝突検出処理手順の出力である凸形状物体間最短距離に基づいて、障害物を回避するための人工反力を物体に発生させるべきか否かを判定する衝突予測手順と、
前記衝突予測手順において人工反力を発生させるべきと判定した物体に対して、最近傍点対間を結ぶ単位ベクトル方向に人工ポテンシャル場を配置することにより、衝突代表点方向への物体間の最短距離に基づく人工反力を算出する人工反力算出処理手順と、
前記人工反力算出手順で算出された人工反力を前記衝突検出手順より出力された衝突代表点対に作用させた場合の物体の軌道修正量を算出する軌道修正量算出処理手順と、
を実行させることを特徴とするコンピュータ・プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、仮想空間若しくは現実空間上における制御対象となる物体の運動を制御する運動制御装置及び運動制御方法、並びにコンピュータ・プログラムに係り、特に、ともに制御対象となる物体同士の重なりを未然に防ぎながら制御対象となる物体の運動を制御する運動制御装置及び運動制御方法、並びにコンピュータ・プログラムに関する。
【０００２】
さらに詳しくは、本発明は、制御対象となる各物体に人工ポテンシャル場を配置して物体間に人工反力を作用させることで物体間の衝突を回避する運動制御装置及び運動制御方法、並びにコンピュータ・プログラムに係り、特に、制御対象となる移動体の衝突をあらかじめ検知又は予測し、衝突若しくは浸透を回避するための軌道を修正して、制御対象が持つ動作や行動目的（タスク）を効率的且つスムーズに遂行する運動制御装置及び運動制御方法、並びにコンピュータ・プログラムに関する。
【背景技術】
【０００３】
力学シミュレーション、精巧なＣＧアニメーション、移動ロボットといった、仮想空間若しくは現実空間からなるシーン上で制御対象となる各物体の運動制御を行なうシステムにおいては、物体同士の重なりを未然に防ぐ必要がある。何故ならば、物体間の重なりは、仮想世界では非現実的な現象であり、現実世界では物体や環境の損傷を招来するからである。
【０００４】
空間上に存在する物体間の衝突検出を行ない、衝突が検出又は予測されたときに、その衝突検出情報を基に制御対象の動作を停止すれば、物体間の浸透（重なり）を未然に防ぐことができる。しかしながら、その制御対象が持つ動作や行動目的（タスク）を効率的且つスムーズに遂行するためには、ただ動作を停止するのではなく、あらかじめ衝突を検知又は予測し、衝突若しくは浸透を回避するための軌道を逐次修正する必要がある。
【０００５】
制御不可能な障害物（静的物体）と制御対象（マニピュレータ）との衝突回避制御として、Ｋｈａｔｉｂらは、人工ポテンシャル場に基づく人工反力生成手段を有したリアルタイム衝突回避制御手法を提案した（例えば、非特許文献１を参照のこと）。この手法によれば、物体間最短距離に基づく人工ポテンシャル場による物体間人工反力を制御物体に作用させ、リアルタイムに衝突回避を実現することができる。しかしながら、人工ポテンシャル場による反力とその方向を、制御対象のある特定の制御点にしか設定することができない。このため、エンドエフェクターの一点といった特定のポイントのみの衝突を考慮して、特定のタスクのみを実現することは可能であるが、任意のポイントに人工反力を発生させることはできないので、衝突予測や衝突回避の制御に厳密性を欠くとともに、複雑な動きを必要とするタスクや運動には不向きである。また、この手法は、制御不可能な障害物と制御対象との衝突回避制御を扱うものであり、制御対象となる移動体を制御不可能な障害物との衝突を回避するように制御することはできるが、ともに制御可能な物体間（例えば、１つのロボットの部位間）で衝突が検知又は予測された場合についての各物体の制御手法若しくは制御戦略に関しては論じられていない。
【０００６】
また、Ｊ．Ｋｕｆｆｎｅｒらは、ヒューマノイドロボットの自己干渉問題解決手法として、凸多面体における高速な距離判定法を用いた、自己干渉問題を考慮した実時間歩行計画手法を提案した（例えば、非特許文献２を参照のこと）。しかしながら、この手法は、あくまでもロボットの動作計画（プランニング）における手法であり、リアクティブな衝突回避制御を行なうためのものではない。
【０００７】
また、Ｆ．Ｓｅｔｏらは、人間と協調作業を行なうロボットにおける実時間での自己衝突回避制御の実現を目的とした，ロボットの弾性要素表現“ＲｏＢＥ（Ｒｅｐｒｅｓｅｎｔａｔｉｏｎ  ｏｆ  Ｂｏｄｙ  ｂｙ  Ｅｌａｓｔｉｃ  ｅｌｅｍｅｎｔｓ”及びＲｏＢＥを用いたロボット単体での自己衝突回避制御手法（例えば、非特許文献３を参照のこと）、人間との協調作業中においても自己衝突回避を可能とする制御手法（例えば、非特許文献４を参照のこと）について提案し、後者の制御手法に関しては実機（ロボット）を用い有効性を確認している。
【０００８】
これらの手法は、ロボット形状をＲｏＢＥという弾性要素を用いて仮想的に表現し、このような弾性要素モデルを用いることでロボットが運動した際の弾性要素間の接触を検出し、接触した弾性要素間に仮想的な反力を発生させ、この仮想的な反力を用いて実際のロボットを制御することにより、自己衝突の回避を実現するものである。しかしながら、これらの手法では、計算負荷を軽減する目的から、弾性要素を球と円柱として用いていることから、制御対象は粗い物体形状近似として扱われるため、厳密な衝突回避動作を行なうのは困難である。また、仮想的な反力を作用させる方向においても、弾性要素の中心を結んだ直線上であると近似することで問題を簡潔にしているが、ここにおいても、厳密且つ緻密な動作計画を実行するためには問題が多い。
【０００９】
さらにＦ．Ｓｅｔｏらのグループは、さまざまに変化する作業状態や環境に応じ、制御対象にどのような回避運動を行なわせるべきかという問題を考慮し、作業拘束や環境から受ける拘束力に応じたプライオリティ付けに基づく自己干渉回避動作戦略を提案した（例えば、非特許文献５を参照のこと）。この制御戦略では、制御対象となるそれぞれの物体に生じる拘束力に基づいてプライオリティ（プライオリティを反映する変数）を決定するようになっている。しかしながら、ロボットの各部位に課される拘束力は、ロボットの動作計画若しくは行動目的を反映するものではない。言い換えれば、拘束力に基づいて各部位のプライオリティを決定して衝突回避動作を行なっても、ロボット制御の本質、すなわち、ロボットの運動目的（タスク遂行・実行・実現）を直感的且つ厳密に扱うことにはならない。
【００１０】
【非特許文献１】Ｏｕｓｓａｍａ  Ｋｈａｔｉｂ“Ｒｅａｌ−ｔｉｍｅ  ｏｂｓｔａｃｌｅ  ａｖｏｉｄａｎｃｅ  ｆｏｒ  ｍａｎｉｐｕｌａｔｏｒｓ  ａｎｄ  ｍｏｂｉｌｅ  ｒｏｂｏｔｓ”（Ｉｎｔ．Ｊ．ｏｆ  Ｒｏｂｏｔｉｃｓ  Ｒｅｓｅａｒｃｈ，５（１）：９０−９８，１９８６）
【非特許文献２】Ｊ．Ｋｕｆｆｎｅｒ，Ｋ．Ｎｉｓｈｉｗａｋｉ，Ｓ．Ｋａｇａｍｉ，Ｙ．Ｋｕｎｉｙｏｓｈｉ，Ｍ．Ｉｎａｂａ，ａｎｄ  Ｈ．Ｉｎｏｕｅ“Ｓｅｌｆ−ｃｏｌｌｉｓｉｏｎ  Ｄｅｔｅｃｔｉｏｎ  ａｎｄ  Ｐｒｅｖｅｎｔｉｏｎ  ｆｏｒ  Ｈｕｍａｎｏｉｄ  Ｒｏｂｏｔ”（Ｐｒｏｃｅｅｄｉｎｇｓ  ｏｆ  ＩＥＥＥ  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｃｏｎｆｅｒｅｎｃｅ  ｏｎ  Ｒｏｂｏｔｉｃｓ  ａｎｄ  Ａｕｔｏｍａｔｉｏｎ，ｐｐ．２２６５−２２７０，２００２）
【非特許文献３】Ｆ．Ｓｅｔｏ，Ｋ．Ｋｏｓｕｇｅ，ａｎｄ  Ｙ．Ｈｉｒａｔａ“Ｒｅａｌ−ｔｉｍｅ  Ｃｏｎｔｒｏｌ  ｏｆ  Ｓｅｌｆ−Ｃｏｌｌｉｓｉｏｎ  Ａｖｏｉｄａｎｃｅ  ｆｏｒ  Ｒｏｂｏｔ  ｕｓｉｎｇ  ＲｏＢＥ”（Ｐｒｏｃｅｅｄｉｎｇｓ  ｏｆ  ＩＥＥＥ−ＲＡＳ  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｃｏｎｆｅｒｅｎｃｅ  ｏｎ  Ｈｕｍａｎｏｉｄ  Ｒｏｂｏｔｓ  ２００３，２００３）
【非特許文献４】瀬戸、小菅、平田共著「ＲｏＢＥを用いたモバイルマニピュレータの実時間自己衝突回避制御−第３報  人間とのインタラクションを考慮した自己衝突回避−」（日本機会学会［Ｎｏ．０４−４］ロボティクス・メカトロニクス講演会‘０４講演論文集、２００４）
【非特許文献５】瀬戸、小菅、平田共著「ＲｏＢＥを用いたモバイルマニピュレータの実時間自己衝突回避制御−第５報  一般性を有する自己衝突回避動作に対する優先度の導入−」（第１０回ロボティクスシンポジア予稿集、２００５）
【発明の開示】
【発明が解決しようとする課題】
【００１１】
本発明の目的は、仮想空間若しくは現実空間上において、ともに制御対象となる物体同士の重なりを未然に防ぎながら制御対象となる物体の運動を好適に制御することができる、優れた運動制御装置及び運動制御方法、並びにコンピュータ・プログラムを提供することにある。
【００１２】
本発明のさらなる目的は、制御対象となる各物体に人工ポテンシャル場を配置して物体間に人工反力を作用させることで物体間の衝突を好適に回避することができる、優れた運動制御装置及び運動制御方法、並びにコンピュータ・プログラムを提供することにある。
【００１３】
本発明のさらなる目的は、制御対象となる移動体の衝突をあらかじめ検知又は予測し、衝突若しくは浸透を回避するための軌道を修正して、制御対象が持つ動作や行動目的（タスク）を効率的且つスムーズに遂行することができる、優れた運動制御装置及び運動制御方法、並びにコンピュータ・プログラムを提供することにある。
【課題を解決するための手段】
【００１４】
本発明は、上記課題を参酌してなされたものであり、その第１の側面は、空間上における物体の運動を制御する運動制御装置であって、
空間上に存在する各物体の位置及び姿勢に関する座標系情報と形状情報、物体間でポテンシャルが作用する距離境界値により定まる反力作用領域をシーンデータとして蓄積するシーンデータ蓄積部と、
空間上に存在する各物体の位置及び姿勢に関する座標系情報と形状情報に基づいて、物体間の最短距離とその最短距離を表す最近傍点対（衝突代表点）と、最近傍点対を結ぶ単位方向ベクトル（衝突代表点方向）の算出を行なう衝突検出部と、
前記衝突検出部により算出された最短距離に基づいて、障害物を回避するための人工反力を物体に発生させるべきか否かを判定する衝突予測部と、
前記衝突予測部において障害物を回避することが必要であると判定された物体に対して、衝突代表点方向への障害物との最短距離に基づく人工反力を算出する人工反力決定部と、
前記衝突予測部において障害物を回避することが必要であると判定された物体の組み合わせに関して、前記人工反力決定部で算出された人工反力を、前記衝突検出部により出力された衝突代表点対に作用させた場合における各物体の軌道修正量を算出する軌道修正量算出部と、
を具備することを特徴とする運動制御装置である。
【００１５】
力学シミュレーション、精巧なＣＧアニメーション、移動ロボットといった、仮想空間若しくは現実空間からなるシーン上で制御対象となる各物体の運動制御を行なうシステムにおいては、物体同士の重なりを未然に防ぐ必要がある。
【００１６】
例えば、人工ポテンシャル場に基づく人工反力生成手段を有したリアルタイム衝突回避制御手法によれば、物体間最短距離に基づく人工ポテンシャル場による物体間人工反力を制御物体に作用させて、リアルタイムに衝突回避を実現することができる。しかしながら、従来は、人工ポテンシャル場による反力の作用点とその方向を、制御対象物体のある特定の制御点にしか設定することができない。このため、任意のポイントに人工反力を発生させることはできないので、衝突予測や衝突回避の制御に厳密性を欠くとともに、複雑な動きを必要とするタスクや運動には不向きである。
【００１７】
また、従来の衝突回避制御手法の多くは、制御対象となる移動体を制御不可能な障害物との衝突を回避するような衝突回避制御を扱うものであり、ともに制御可能な物体間（例えば、１つのロボットの部位間）で衝突が検知又は予測された場合についての各物体の制御手法若しくは制御戦略に関しては十分には論じられていない。
【００１８】
これに対し、本発明に係る運動制御装置は、前記衝突検出部は、ＧＪＫ（Ｇｉｌｂｅｒｔ−Ｊｏｈｎｓｏｎ−Ｋｅｅｒｔｈｉｄｉｓｔａｎｃｅ ａｌｇｏｒｉｔｈｍ）アルゴリズムを用いて物体間の最短距離とその最短距離を表す最近傍点対（衝突代表点）と、最近傍点対を結ぶ単位方向ベクトル（衝突代表点方向）の算出を行なうようになっている。
【００１９】
したがって、前記人工反力決定部は、最近傍点対間を結ぶ単位ベクトル方向に人工ポテンシャル場を配置することによって、より厳密に人工反力を決定することができる。すなわち、ＧＪＫ衝突アルゴリズムを用いて算出された最近傍点対という任意のポイントに人工反力を発生させることで、衝突予測や衝突回避の制御に厳密性を確保するとともに、複雑な動きを必要とするタスクや運動に適応することができる。
【００２０】
また、本発明に係る運動制御装置は、前記衝突予測判定部において判定された物体及び障害物がともに運動制御が可能な制御対象物体である場合には、前記軌道修正量算出部は、前記人工反力決定部により算出された人工反力に基づいて、物体及び障害物それぞれに対する軌道修正量を算出するように構成されている。
【００２１】
したがって、本発明によれば、衝突が予測された双方の物体に対してそれぞれ方向が反対の人工反力を作用させて各物体の軌道を修正する、あるいは、片方の物体のみに人工反力を作用させてもう一方を固定するといった、制御戦略を定めることが可能となる。
【００２２】
このような制御戦略を実現するために、シーンに存在する制御対象物体毎のシーンデータとして、各物体の運動若しくは行動目的（タスク）に基づくプライオリティを、制御対象物体を構成するプリミティブ毎に設定し、シーンデータとしてシーンデータ蓄積部に保持する。そして、人工反力決定部で算出された人工反力を、シーン中のプリミティブとなるそれぞれの制御対象物体に分配するための手段、すなわち人工反力分配部を新たに配設する。したがって、軌道修正量算出部は、人工反力分配部によってそれぞれの制御対象物体に分配された人工反力に基づいて制御対象物体毎の軌道修正量を算出することができる。
【００２３】
人工反力分配部は、人工反力決定部で算出された人工反力を、運動若しくは行動目的（タスク）に設定されているプライオリティに基づいて、衝突が予測された各物体に分配する。高いプライオリティが付与された物体に分配される人工反力が弱まるので、タスクによる軌道が維持される。逆に、低いプライオリティが付与された物体に分配される人工反力は高まるので、タスクによる軌道が大きく修正されることになる。したがって、設定されたタスク遂行のプライオリティが衝突回避制御にそのまま反映されたことになる。
【００２４】
また、すべての制御対象物体対について衝突検出並びに衝突予測を行なうと、空間中の物体数が大幅に増加した場合には、その処理負荷は膨大になってしまう、という問題がある。
【００２５】
そこで、本発明に係る運動制御装置は、物体に衝突予測マージン（例えば、検出対象物体と球（マージン）とのＭｉｎｋｏｗｓｋｉ和）を設定し、衝突検出の対象となる物体を膨張させ、衝突検出対象物体間の衝突を事前に予測する膨張物体衝突予測部を備えていても良い。このような場合、衝突予測部では、膨張物体衝突予測部において衝突の危険性、すなわち衝突回避制御を行なう必要がある物体間のみ、より詳細な最短距離演算を行なうようして、物体間最短距離演算をより効率的に行なうことができる。
【００２６】
ＧＪＫアルゴリズムベースの衝突検出処理において、物体が浸透しているか否かの判定処理は、分離軸を見つけるだけの処理となるので、最短距離演算に比較すると、高速に行なうことができる。したがって、衝突予測マージンを利用した効率的な最短距離演算算出機構を構成することで、計算コストの大幅な削減につながることから、空間中に多くの物体が存在する場合においても、効率的に衝突回避制御を行なうことが可能となる。
【００２７】
また、上述した衝突予測マージンや反力作用領域を、タスクに応じて可変にすることで、より柔軟な運動指定を行なうことができる。
【００２８】
また、本発明の第２の側面は、空間上における物体の運動を制御するための処理をコンピュータ上で実行するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、前記コンピュータに対し、
各物体に設定されるローカル座標系のワールド座標系における位置及び姿勢情報を設定する座標系設定処理手順と、
凸形状に近似した物体間の最短距離、衝突代表点対、及び衝突代表点方向を算出する衝突検出処理手順と、
前記衝突検出処理手順の出力である凸形状物体間最短距離に基づいて、障害物を回避するための人工反力を物体に発生させるべきか否かを判定する衝突予測手順と、
前記衝突予測手順において人工反力を発生させるべきと判定した物体に対して、最近傍点対間を結ぶ単位ベクトル方向に人工ポテンシャル場を配置することにより、衝突代表点方向への物体間の最短距離に基づく人工反力を算出する人工反力算出処理手順と、
前記人工反力算出手順で算出された人工反力を前記衝突検出手順より出力された衝突代表点対に作用させた場合の物体の軌道修正量を算出する軌道修正量算出処理手順と、
を実行させることを特徴とするコンピュータ・プログラムである。
【００２９】
本発明の第２の側面に係るコンピュータ・プログラムは、コンピュータ上で所定の処理を実現するようにコンピュータ可読形式で記述されたコンピュータ・プログラムを定義したものである。換言すれば、本発明の第２の側面に係るコンピュータ・プログラムをコンピュータにインストールすることによって、コンピュータ上では協働的作用が発揮され、本発明の第１の側面に係る運動制御装置と同様の作用効果を得ることができる。
【発明の効果】
【００３０】
本発明によれば、仮想空間若しくは現実空間上において、ともに制御対象となる物体同士の重なりを未然に防ぎながら制御対象となる物体の運動を好適に制御することができる、優れた運動制御装置及び運動制御方法、並びにコンピュータ・プログラムを提供することができる。
【００３１】
本発明に係る運動制御装置は、制御対象となる各物体に人工ポテンシャル場を配置して物体間に人工反力を作用させることで物体間の衝突回避を実現するものであるが、制御対象となる移動体の衝突をあらかじめ検知又は予測し、衝突若しくは浸透を回避するための軌道を修正して、制御対象が持つ動作や行動目的（タスク）を効率的且つスムーズに遂行することができる。
【００３２】
本発明によれば、ＧＪＫアルゴリズムに基づく物体間最短距離及び最近傍点の算出処理によって得られた最近傍点対を、人工反力を作用させる制御点とすることで、決められた制御点をコントロールする従来の衝突回避手法と比較して、より厳密で柔軟、且つ精密な衝突回避制御を行なうことができる。
【００３３】
勿論、本発明は、複雑なリンク構成からなるオブジェクト、例えばヒューマノイドのような多リンク系オブジェクトにおける自己衝突回避にも適用することができる。
【００３４】
本発明に係る運動制御方法を、ロボットの運動制御にも適用する場合、ロボット制御の本質であるロボットの運動目的達成を考慮した、リアルタイム衝突検出機構を提供することができる。空間上に存在する物体それぞれに衝突の可能性があることを予測するための領域（すなわち反力作用領域）を配置し、その領域を侵し合う物体間に、物体間の最短距離に応じた反力を発生させることで、物体の軌道を逐次修正し、未然に衝突を回避する。
【００３５】
また、本発明によれば、タスクに基づいて物体を形成するプリミティブ毎にプライオリティ付けを行なうことで、物体それぞれの軌道修正量を算出する際には、物体の動作目的（タスク）のプライオリティを物体の移動プライオリティに反映させるとともに、より精密な衝突回避や自己干渉回避を実現することができる。また、制御系でのタスクのプライオリティに基づく動作実現にかかる計算コストを低減するとともに、より緻密且つ直感的な移動体の動作計画が可能となり、物体の動作目的（タスク）が効率的且つスムーズに実現される。
【００３６】
また、物体間の衝突検出を行なう際に、衝突予測マージンを導入することによって、最短距離算出処理を効率化して、計算コストを大幅に削減して、効率的な衝突・干渉回避制御を行なうことができる。
【００３７】
本発明によれば、計算コストを抑えることで、リアルタイムに衝突回避制御を行なうことができる。
【００３８】
本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。
【発明を実施するための最良の形態】
【００３９】
以下、図面を参照しながら本発明の実施形態について詳解する。
【００４０】
まず、図１に示すような、３次元空間上に存在する、運動制御可能な物体（制御対象物体）と制御不可能な静的物体（障害物）との衝突回避問題を例にとって考察する。同図中の制御対象物体は、凹形状で表されており、その動作は制御可能なものとする。また、障害物は、操作対象ではなく、その動きや位置を制御することができない。同図中で、○で表されたポイントは、制御対象物体が最終的に目指す目標地点（ゴール）であり、制御対象の運動目的（タスク）を「このゴール地点に到達すること」に設定する。また、障害物と制御対象物体は同一空間を占有すること、すなわち浸透（重なり）は許されないものとする。
【００４１】
制御対象物体は、例えば自律型のヒューマノイドロボットのように、ゴールを目指す際に必要となる目標地点への移動計画を行なう軌道計画手段を具備しているものと仮定する。３次元空間上に障害物が存在しないときには、制御対象物体は、図２に示すような軌道でゴール地点を目指す。一方、図１に示すように、障害物が３次元空間中に存在するときには、制御対象をゴール地点に到達させるには、障害物を回避するための障害物回避手段、すなわち、逐次衝突回避手段が必要である。このような逐次衝突回避手段を実現するための主な技術課題として、（１）衝突検出若しくは衝突予測を行なう手段（すなわち物体間最短距離算出手段）と、（２）衝突を回避するための物体軌道を修正するための軌道修正手段が挙げられる。以下では、これら２つの技術課題について順次述べていく。
【００４２】
衝突検出・衝突予測手段：
本実施形態では、衝突を検出又は予測する制御対象物体の形状を、凸包を用いて近似する。図３には、図１に示した制御対象物体の凸包近似例を示している。凸包は、頂点集合を包含する最小の凸多角形頂点集合からなるが、球や円柱、四角柱などによる近似に比べると物体へのフィット率が高く、物体形状を比較的厳密に扱うことが可能である。また、物体形状に関する幾何演算を比較的安価に行なうことが可能である。
【００４３】
なお、頂点集合を包含する最小の凸多角形頂点集合を得るための凸包演算として、例えばＧｒａｈａｍ法を用いることができる。Ｇｒａｈａｍ法については、Ｒｏｎａｌｄ  Ｇｒａｈａｍ，“Ａｎ  Ｅｆｆｉｃｉｅｎｔ  Ａｌｇｏｒｉｔｈｍ  ｆｏｒ  Ｄｅｔｅｒｍｉｎｉｎｇ  ｔｈｅ  Ｃｏｎｖｅｘ  Ｈｕｌｌ  ｏｆ  ａ  Ｆｉｎｉｔｅ  Ｐｌａｎａｒ  Ｓｅｔ”（Ｉｎｆｏ．Ｐｒｏｃ．Ｌｅｔｔｅｒｓ，１，ｐｐ．１３２−１３３，１９７２）を参照されたい。
【００４４】
本実施形態では、凸形状物体間衝突検出アルゴリズムとして、ＧＪＫ（Ｇｉｌｂｅｒｔ−Ｊｏｈｎｓｏｎ−Ｋｅｅｒｔｈｉ ｄｉｓｔａｎｃｅａｌｇｏｒｉｔｈｍ）を想定する。ＧＪＫアルゴリズムは、凸形図形間距離を算出するための反復的な方法であり、衝突代表点対（最近傍点対又は最浸透点対）を、干渉検出結果として出力する。図４には、ＧＪＫアルゴリズムの概略図を示している。図４Ａは物体間の最近傍点算出処理を表し、図４Ｂは物体間の最浸透点算出処理を表している。同図では、簡素化のため２次元空間表現を用いているが、３次元空間においても成立することを十分理解されたい。
【００４５】
なお、ＧＪＫアルゴリズムの詳細については、例えば、Ｇ．ｖａｎ  ｄｅｎ  Ｂｅｒｇｅｎ著“Ａ  Ｆａｓｔ  ａｎｄ  Ｒｏｂｕｓｔ  ＧＪＫ  ｉｍｐｌｅｍｅｎｔａｔｉｏｎ  ｆｏｒ  Ｃｏｌｌｉｓｉｏｎ  Ｄｅｔｅｃｔｉｏｎ  ｏｆ  Ｃｏｎｖｅｘ Ｏｂｊｅｃｔｓ”（Ｊｏｕｒｎａｌ  ｏｆ  Ｇｒａｐｈｉｃｓ  Ｔｏｏｌｓ，４（２），ｐｐ．７−２５，２００１）を参照されたい。
【００４６】
以下の説明では、特に断りがない限り、最近傍点や最浸透点を算出するアルゴリズムをＧＪＫアルゴリズムと総称して取り扱うことにする。但し、実際には、最近傍点と最浸透点をそれぞれ算出するアルゴリズムやその使用目的は異なる。すなわち、物体同士が浸透していない場合には最近傍点算出アルゴリズムとしてＧＪＫ  ｄｉｓｔａｎｃｅアルゴリズムが用いられ、物体同士が浸透している場合には、最浸透点算出アルゴリズムとしてＥＰＡ（Ｅｘｐａｎｄｉｎｇ  Ｐｏｌｙｔｏｐｅ  Ａｌｇｏｒｉｔｈｍ）が用いられる。
【００４７】
軌道修正手段：
本実施形態では、ポテンシャル法に基づいて、制御対象物体に人工反力を印加して、その軌道修正を逐次行なう。ポテンシャル法は、自律移動ロボットの経路計画などでしばしば用いられている（例えば、非特許文献１を参照のこと）。
【００４８】
ポテンシャルは、障害物から斥力（反力）を受けるような形で設計される。そのポテンシャル関数Ｕ0（ｘ）を、下式（１）のように設定する。
【００４９】
【数１】
2008242859.tif 000003
【００５０】
但し、上式において、ρ並びにρ0は物体間の最短距離並びにポテンシャルが作用する境界距離（反力作用領域）をそれぞれ表し、ηはスケール因子である。本実施形態では、ここで言う物体間最短距離ρとして、上記の衝突検出・衝突予測手段の出力である物体間最短距離を用いる。最短距離算出時に同時に得られる最近傍点対に対して、ポテンシャル場により生ずる力（反力）を作用させ、その最近傍点対を結ぶ単位ベクトルをその反力が作用する方向に設定する。すなわち、上式（１）で表されるポテンシャルによる人工反力は、下式（２）のように表される。
【００５１】
【数２】
2008242859.tif 000004
【００５２】
但し、ｄは最近傍点対を結ぶ単位ベクトルであり、下式（３）のように表される。
【００５３】
【数３】
2008242859.tif 000005
【００５４】
なお、上式（３）中のＰa及びＰbはそれぞれ衝突検出対象物体Ａ及びＢにおける衝突代表点（最近傍点）を表す。
【００５５】
上述した衝突検出・衝突予測手段と、軌道修正手段により、目標とする凸物体間最短距離・最近傍点に基づいて人工ポテンシャル場反力を生成するための手段が構成される。そして、生成される人工反力に基づいて、制御対象物体の軌道を修正することによって、物体間の衝突を未然に回避するための逐次制御を実現することができる。これにより、制御対象物体間の最短距離・最近傍点対算出手段を一般化して人工反力を求め、制御点を制御対象物体の任意の点（最近傍点対）に設定すること、すなわち一般化することで、あらかじめ決められた（言い換えれば固定された）制御点をコントロールするのに比較して、より厳密且つ柔軟な衝突回避制御を行なうことができる。
【００５６】
図５には、運動制御システムの機能ブロック図を示している。図示の運動制御システムは、物体間最短距離の算出によって得られた最近傍点対間を結ぶ単位ベクトル方向に人工ポテンシャル場により生ずる人工反力を作用させるという、人工物体間反力生成手法及び物体間人工反力に基づく衝突回避制御を行なう。図示のシステム１０は、主に座標系設定部１１と、衝突検出部１２と、物体間人工反力算出部１３と、衝突回避制御部１４により構成される。
【００５７】
座標系設定部１１は、例えば各種動力学演算を担う力学シミュレーション・エンジンなどを用いて構成され、各物体の運動を決定する。
【００５８】
衝突検出部１２は，シーンデータ蓄積部１２１と、シーンデータを基に計算幾何学演算を行なう計算幾何学演算部１２２より構成される。
【００５９】
ここで、シーンデータ蓄積部１２１は、シーンに存在する制御対象物体毎の、各物体上に設定されたローカル座標系のワールド座標系内での位置及び姿勢情報（座標系情報）と、物体の形状に関する情報（形状情報）などを、制御対象物体を構成するプリミティブ毎にシーンデータとして保持している。形状情報として、物体を構成する頂点や面等に関する３Ｄ形状モデル、物体の凸包を構成する頂点情報として３Ｄ凸包形状モデル、その頂点の接続関係を表す隣接頂点グラフ（頂点探索を高速に実行するためのグラフ構造）が保持される。また、シーンに存在する物体の組み合わせ毎に、物体間人工反力が作用する領域（反力作用領域）を定める固定値である距離境界値（反力作用領域距離境界値）も、シーンデータとして保持される。
【００６０】
また、計算幾何学演算部１２２は、主に、サポート写像実行部１２２１とＧＪＫアルゴリズム実行部１２２２より構成される。
【００６１】
サポート写像実行部１２２１は、図６に示すように、任意の凸物体の頂点のうち，あるベクトル方向で最も遠くに存在する点を算出する、計算幾何学における基本演算を実行する。なお、サポート写像の算出方法自体は本発明の要旨ではないので、詳細はＧ．ｖａｎ  ｄｅｎ  Ｂｅｒｇｅｎ著“Ａ  Ｆａｓｔ  ａｎｄ  Ｒｏｂｕｓｔ  ＧＪＫ  ｉｍｐｌｅｍｅｎｔａｔｉｏｎ  ｆｏｒ  Ｃｏｌｌｉｓｉｏｎ  Ｄｅｔｅｃｔｉｏｎ  ｏｆ  Ｃｏｎｖｅｘ  Ｏｂｊｅｃｔｓ”（Ｊｏｕｒｎａｌ  ｏｆ  Ｇｒａｐｈｉｃｓ  Ｔｏｏｌｓ，４（２），ｐｐ．７−２５，２００１）を参照されたい。
【００６２】
ＧＪＫアルゴリズム実行部１２２２は、衝突検出処理を行なう際に必要となるコア演算を実行する部分であり、サポート写像実行部１２２１を繰り返し用い、物体間最短距離とその最短距離を表す最近傍点対（衝突代表点）を算出し、また、最近傍点対を結ぶ単位方向ベクトル（衝突代表点方向）の算出を行なう。ここで算出された、物体間最短距離、衝突代表点対、及び衝突代表点方向は、衝突検出部１２の結果として出力される。
【００６３】
物体間人工反力算出部１３は、主に、衝突予測部１３１と、人工反力決定部１３２により構成される。
【００６４】
衝突予測部１３１は、衝突検出部１２の出力である最短距離を利用して、物体が人工ポテンシャルの作用領域（反力作用領域）内に存在するか否か、すなわち、障害物を回避するための人工反力を物体に発生させるべきか否かを判定する。
【００６５】
人工反力決定部１３２は、衝突予測部１３１において障害物を回避することが必要であると判定された物体に対して動作して、衝突代表点方向への物体間の最短距離に基づく人工反力を算出する。ここで算出された人工反力は、物体間人工反力算出部１３の結果として出力される。
【００６６】
衝突回避制御部１４は、軌道修正量算出部１４１により構成される。軌道修正量算出部１４１では、人工反力決定部１３２の出力である人工反力を、衝突検出部１２により出力された衝突代表点対に作用させた場合の、物体軌道修正量を算出する。
【００６７】
図７には、図５に示した運動制御システムにおいて実行される処理手順をフローチャートの形式で示している。
【００６８】
座標系設定処理ステップＳ１では、シーンデータ内の各物体に設定されるローカル座標系のワールド座標系における位置及び姿勢情報を設定する。座標系設定部１１は、例えば各種動力学演算を担う力学シミュレーション・エンジンなどを用いて構成され、各物体の運動を決定する。
【００６９】
次いで、衝突検出処理ステップＳ２では、シーンデータと計算幾何学演算部１２２内におけるＧＪＫアルゴリズム実行部１２２２により、凸形状に近似した物体間の最短距離、衝突代表点対、及び衝突代表点方向を算出する。
【００７０】
次いで、衝突予測ステップＳ３では、衝突検出処理ステップの出力である凸形状物体間最短距離を利用して、物体が人工ポテンシャルの作用領域（反力作用領域）内に存在するか否か、すなわち、障害物を回避するための人工反力を発生させるべきか否かを判定する。同ステップにおいて、衝突が予測された場合はステップＳ４へ進むが、衝突が予測されない場合にはＳ５へと処理が進む。
【００７１】
人工反力算出処理ステップＳ４では、衝突代表点方向への物体間の最短距離に基づく人工反力を、上式（２）に基づいて算出する。
【００７２】
軌道修正量算出処理ステップＳ５では、人工反力算出ステップで算出された人工反力を、衝突検出ステップより出力された衝突代表点対に作用させた場合の物体の軌道修正量を算出する。
【００７３】
これまでの説明では、制御可能な物体と制御不可能な障害物との衝突回避制御手法について述べたが、本発明を、ともに制御可能な物体間における衝突回避制御手法にも拡張することができる。後者の場合、衝突が予測された物体それぞれの運動を制御することが可能であることから、軌道修正量算出部１４１は、人工ポテンシャル場を配置するより生じる人工反力に基づいて各物体それぞれに対する軌道修正を行なうように構成する必要がある。軌道修正量算出部１４１をこのように構成することによって、衝突が予測された双方の物体に対してそれぞれ方向が反対の人工反力を作用させて各物体の軌道を修正する、あるいは、片方の物体のみに人工反力を作用させてもう一方を固定するといった、制御戦略を定めることが可能となる。
【００７４】
このような制御戦略を実現するために、シーンに存在する制御対象物体毎のシーンデータとして、各物体の運動若しくは行動目的（タスク）に基づくプライオリティを、制御対象物体を構成するプリミティブ毎に設定し、シーンデータとしてシーンデータ蓄積部１２に保持する。そして、人工反力決定部１３２で算出された人工反力を、シーン中のプリミティブとなるそれぞれの制御対象物体に分配する手段、すなわち人工反力分配部を新たに配設する。したがって、軌道修正量算出部１４１では、それぞれの制御対象物体に分配された人工反力に基づいて制御対象物体毎の軌道修正量を算出することができる。
【００７５】
ここでは、タスクに応じて制御対象物体毎にプライオリティを付与する一例を示して、制御可能な物体間における衝突回避制御手法について説明する。
【００７６】
まず、タスクによるプライオリティを数段階の数値（例えば、０，１，２，３）で表す。ここで、プライオリティ０を最高プライオリティとし、数値が大きくなるにつれてプライオリティが低くなるものとする。このプライオリティを制御対象物体プリミティブ毎に設定してシーンデータ蓄積部１２１に保持しておく。そして、衝突が予測された制御対象物体間に人工ポテンシャル場を配置して人工反力が決定されると、人工反力分配部は、当該タスクにおいて各制御対象物体に設定されているプライオリティを参照して、各制御対象物体に及ぼす人工反力の作用度合いを操作する。下式（４）には、人工反力分配方法の一例を示している。
【００７７】
【数４】
2008242859.tif 000006
【００７８】
但し、上式（４）において、Ｆ0は人工ポテンシャル場から生ずる人工反力であり、上式（３）に従って算出される。また、Ｑpa及びＱpbは、それぞれ物体Ａ及びＢに設定されたタスクによるプライオリティである。そして、Ｆ0,a及びＦ0,bは物体Ａ及びＢそれぞれに作用する人工反力であり、人工反力Ｆ0は、各物体Ａ及びＢが運動又は行動目的（タスク）に対して付与されたプライオリティに基づいて各物体Ａ及びＢに分配される。
【００７９】
上式（４）によれば、高いプライオリティが付与された物体に分配される人工反力が弱まるので、タスクによる軌道が維持される。逆に、低いプライオリティが付与された物体に分配される人工反力は高まるので、タスクによる軌道が大きく修正されることになる。したがって、設定されたタスク遂行のプライオリティが衝突回避制御にそのまま反映されたことになる。
【００８０】
このように、タスクに基づくプライオリティ付けを行ない、そのプライオリティを衝突回避制御に反映することで、運動目的（タスク）をより直感的に扱うことができる。また、計算コストがかかる制御系でタスクのプライオリティを管理する方法（非特許文献１を参照のこと）と比較すると、大幅に計算コストを削減することが可能になる。さらに、より精密な衝突回避制御を実現することが可能になるといった利点もある。
【００８１】
図８には、制御対象物体毎にタスクに応じたプライオリティを付与して衝突回避制御を行なう運動制御システムの構成を示している。図示のシステム１０は、主に座標系設定部１１と、衝突検出部１２と、物体間人工反力算出部１３と、人工反力分配部１５と、衝突回避制御部１４により構成される。
【００８２】
座標系設定部１１は、例えば各種動力学演算を担う力学シミュレーション・エンジンなどを用いて構成され、各物体の運動を決定する。
【００８３】
衝突検出部１２は，シーンデータ蓄積部１２１と、シーンデータを基に計算幾何学演算を行なう計算幾何学演算部１２２より構成される。
【００８４】
シーンデータ蓄積部１２１は、シーンに存在する制御対象物体毎の、各物体上に設定されたローカル座標系のワールド座標系内での位置及び姿勢情報（座標系情報）と、物体の形状に関する情報（形状情報）などを、制御対象物体を構成するプリミティブ毎にシーンデータとして保持している。形状情報として、物体を構成する頂点や面等に関する３Ｄ形状モデル、物体の凸包を構成する頂点情報として３Ｄ凸包形状モデル、その頂点の接続関係を表す隣接頂点グラフ（頂点探索を高速に実行するためのグラフ構造）が保持される。また、シーンに存在する物体の組み合わせを衝突回避物体対リストにリストアップし、リスト中の物体対毎に、物体間人工反力が作用する領域（反力作用領域）を定める固定値である距離境界値（反力作用領域距離境界値）も、シーンデータとして保持される。また、各物体の運動若しくは行動目的（タスク）に基づくプライオリティが制御対象物体を構成するプリミティブ毎に設定されており、これがシーンデータとして保持される。但し、運動プライオリティは、固定データではなく、タスクによりか変なものとする。
【００８５】
計算幾何学演算部１２２は、主に、サポート写像実行部１２２１とＧＪＫアルゴリズム実行部１２２２より構成される。
【００８６】
サポート写像実行部１２２１は、図６に示すように、任意の凸物体の頂点のうち，あるベクトル方向で最も遠くに存在する点を算出する、計算幾何学における基本演算を実行する（同上）。
【００８７】
ＧＪＫアルゴリズム実行部１２２２は、衝突検出処理を行なう際に必要となるコア演算を実行する部分であり、サポート写像実行部１２２１を繰り返し用い、物体間最短距離とその最短距離を表す最近傍点対（衝突代表点）を算出し、また、最近傍点対を結ぶ単位方向ベクトル（衝突代表点方向）の算出を行なう。ここで算出された、物体間最短距離、衝突代表点対、及び衝突代表点方向は、衝突検出部１２の結果として出力される。
【００８８】
物体間人工反力算出部１３は、主に、衝突予測部１３１と、人工反力決定部１３２により構成される。
【００８９】
衝突予測部１３１は、衝突検出部１２の出力である最短距離を利用して、物体が人工ポテンシャルの作用領域（反力作用領域）内に存在するか否か、すなわち、障害物を回避するための人工反力を物体に発生させるべきか否かを判定する。
【００９０】
人工反力決定部１３２は、衝突予測部１３１において障害物を回避することが必要であると判定された物体に対して動作して、衝突代表点方向への物体間の最短距離に基づく人工反力を算出する。ここで算出された人工反力は、物体間人工反力算出部１３の結果として出力される。
【００９１】
人工反力分配部１５は、人工反力決定部１３２で算出した人工反力を、上式（４）に従って、衝突予測された各制御対象物体に分配する。すなわち、シーンデータの物体情報であるタスクによる運動プライオリティに基づいて、衝突回避のための物体それぞれに作用させるべき分配人工反力を算出する。
【００９２】
衝突回避制御部１４は、軌道修正量算出部１４１により構成される。軌道修正量算出部１４１では、人工反力分配部１５の出力である制御対象物体毎の分配人工反力を、衝突検出部１２により出力された衝突代表点対に作用させた場合の物体軌道修正量を、制御対象物体毎に算出する。
【００９３】
図９には、図８に示した運動制御システムにおいて実行される処理手順をフローチャートの形式で示している。
【００９４】
座標系設定処理ステップＳ１１では、シーンデータ内の各物体に設定されるローカル座標系のワールド座標系における位置及び姿勢情報を設定する。座標系設定部１１は、例えば各種動力学演算を担う力学シミュレーション・エンジンなどを用いて構成され、各物体の運動を決定する。
【００９５】
次いで、衝突検出処理ステップＳ１２では、シーンデータと計算幾何学演算部１２２内におけるＧＪＫアルゴリズム実行部１２２２により、凸形状に近似した物体間の最短距離、衝突代表点対、及び衝突代表点方向を算出する。
【００９６】
次いで、衝突予測ステップＳ１３では、衝突検出処理ステップの出力である凸形状物体間最短距離を利用して、物体が人工ポテンシャルの作用領域（反力作用領域）内に存在するか否か、すなわち、障害物を回避するための人工反力を発生させるべきか否かを判定する。同ステップにおいて、衝突が予測された場合はステップＳ１４へ進むが、衝突が予測されない場合にはＳ１６へと処理が進む。
【００９７】
人工反力算出処理ステップＳ１４では、衝突代表点方向への物体間の最短距離に基づく人工反力を、上式（２）に基づいて算出する。
【００９８】
分配人工反力算出処理ステップＳ１５では、人工反力算出処理ステップＳ１４で算出した人工反力を、上式（４）に従って、衝突予測された各制御対象物体に分配する。すなわち、人工反力算出処理ステップＳ１４により算出された人工反力をシーンデータの物体情報であるタスクによる運動プライオリティに基づいて、衝突回避のための物体それぞれに作用させるべき分配人工反力を算出する。
【００９９】
軌道修正量算出ステップＳ１６では、人工反力分配部１５の出力である制御対象物体毎の分配人工反力を、衝突検出部１２により出力された衝突代表点対に作用させた場合の物体軌道修正量を、制御対象物体毎に算出する。
【０１００】
これまでの説明では、シーンに存在する物体の組み合わせを衝突回避物体対リストにリストアップし、リスト中の物体対毎に、制御期間中は常に最短距離演算を行なって、衝突検出並びに衝突予測を行なうようにしていた。高速な演算処理装置を用いて衝突検出処理を行なっているとしても、空間中の物体数が大幅に像化した場合には、その処理負荷は膨大となる。そこで、本発明に係る運動制御装置は、物体間最短距離演算をより効率的に行なう手段を備えていることが好ましい。
【０１０１】
物体間最短距離演算をより効率的に行なう手段は、例えば、制御対象物体を膨張させてから衝突予測を行なう膨張物体衝突予測部として構成することができる。具体的には、膨張物体衝突予測部は、図１０に示すように物体に衝突予測マージン（例えば、検出対象物体と球（マージン）とのＭｉｎｋｏｗｓｋｉ和）を設定し、衝突検出の対象となる物体を膨張させ、衝突検出対象物体間の衝突を事前に予測する。そして、計算幾何学演算部１２では、膨張物体衝突予測部において衝突の危険性、すなわち衝突回避制御を行なう必要がある物体間のみ、より詳細な最短距離演算を行なうようにする。
【０１０２】
ＧＪＫアルゴリズムベースの衝突検出処理において、物体が浸透しているか否かの判定処理は、分離軸を見つけるだけの処理となるので、最短距離演算に比較すると、高速に行なうことができる。したがって、衝突予測マージンを利用した効率的な最短距離演算算出機構を構成することで、計算コストの大幅な削減につながることから、空間中に多くの物体が存在する場合においても、効率的に衝突回避制御を行なうことが可能となる。
【０１０３】
なお、球とのＭｉｎｋｏｗｓｋｉ和を考慮したＧＪＫアルゴリズムベースの衝突検出方法に関しては、例えば、Ｇｉｎｏ  ｖａｎ  ｄｅｎ  Ｂｅｒｇｅｎ，“Ｃｏｌｌｉｓｉｏｎ  Ｄｅｔｅｃｔｉｏｎ  ｉｎ    Ｉｎｔｅｒａｃｔｉｖｅ  ３Ｄ  Ｅｎｖｉｒｏｎｍｅｎｔｓ”（Ｍｏｒｇａｎ  Ｋａｕｆｍａｎｎ  Ｐｕｂｌｉｓｈｅｒｓ，２００３）を参照されたい。
【０１０４】
図１１には、衝突予測マージンを利用して効率的な最距離演算を行なうように構成された運動制御システムの構成を示している。図示のシステム１０は、主に座標系設定部１１と、衝突検出部１２と、物体間人工反力算出部１３と、人工反力分配部１５と、衝突回避制御部１４により構成される。
【０１０５】
座標系設定部１１は、例えば各種動力学演算を担う力学シミュレーション・エンジンなどを用いて構成され、各物体の運動を決定する。
【０１０６】
衝突検出部１２は，シーンデータ蓄積部１２１と、シーンデータを基に計算幾何学演算を行なう計算幾何学演算部１２２と、衝突検出の対象となる物体を膨張させて衝突検出対象物体間の衝突を事前に予測する膨張物体衝突予測部１２３より構成される。
【０１０７】
シーンデータ蓄積部１２１は、シーンに存在する制御対象物体毎の、各物体上に設定されたローカル座標系のワールド座標系内での位置及び姿勢情報（座標系情報）と、物体の形状に関する情報（形状情報）などを、制御対象物体を構成するプリミティブ毎にシーンデータとして保持している。形状情報として、物体を構成する頂点や面等に関する３Ｄ形状モデル、物体の凸包を構成する頂点情報として３Ｄ凸包形状モデル、その頂点の接続関係を表す隣接頂点グラフ（頂点探索を高速に実行するためのグラフ構造）が保持される。また、シーンに存在する物体の組み合わせを衝突回避物体対リストにリストアップし、リスト中の物体対毎に、物体間人工反力が作用する領域（反力作用領域）を定める固定値である距離境界値（反力作用領域距離境界値）も、シーンデータとして保持される。また、各物体の運動若しくは行動目的（タスク）に基づくプライオリティが制御対象物体を構成するプリミティブ毎に設定されており、これがシーンデータとして保持される。但し、運動プライオリティは、固定データではなく、タスクにより可変なものとする。また、物体間最短距離算出の計算コストを軽減するための膨張物体間衝突検出を行なう際に用いられる衝突予測マージンも保持される。
【０１０８】
計算幾何学演算部１２２は、主に、サポート写像実行部１２２１とＧＪＫアルゴリズム実行部１２２２より構成される。
【０１０９】
サポート写像実行部１２２１は、図６に示すように、任意の凸物体の頂点のうち，あるベクトル方向で最も遠くに存在する点を算出する、計算幾何学における基本演算を実行する（同上）。
【０１１０】
ＧＪＫアルゴリズム実行部１２２２は、衝突検出処理を行なう際に必要となるコア演算を実行する部分であり、サポート写像実行部１２２１を繰り返し用い、物体間最短距離とその最短距離を表す最近傍点対（衝突代表点）を算出し、また、最近傍点対を結ぶ単位方向ベクトル（衝突代表点方向）の算出を行なう。ここで算出された、物体間最短距離、衝突代表点対、及び衝突代表点方向は、衝突検出部１２の結果として出力される。
【０１１１】
膨張物体間衝突検出部１２３は、シーンデータ蓄積部１２１に保持されている物体情報である衝突予測マージンに基づいて検出の対象となる物体をそれぞれ膨張させ、これら膨張させた物体間の衝突検出を行なう。そして、膨張物体間衝突検出部１２３により衝突が起きる可能性があると判断された場合のみ、ＧＪＫアルゴリズム実行部１２２２によって、物体間最短距離算出、最近傍点対算出、及び衝突代表点方向算出処理が行なわれる。
【０１１２】
物体間人工反力算出部１３は、主に、衝突予測部１３１と、人工反力決定部１３２により構成される。
【０１１３】
衝突予測部１３１は、衝突検出部１２の出力である最短距離を利用して、物体が人工ポテンシャルの作用領域（反力作用領域）内に存在するか否か、すなわち、障害物を回避するための人工反力を物体に発生させるべきか否かを判定する。
【０１１４】
人工反力決定部１３２は、衝突予測部１３１において障害物を回避することが必要であると判定された物体に対して動作して、衝突代表点方向への物体間の最短距離に基づく人工反力を算出する。ここで算出された人工反力は、物体間人工反力算出部１３の結果として出力される。
【０１１５】
人工反力分配部１５は、人工反力決定部１３２で算出した人工反力を、上式（４）に従って、衝突予測された各制御対象物体に分配する。すなわち、シーンデータの物体情報であるタスクによる運動プライオリティに基づいて、衝突回避のための物体それぞれに作用させるべき分配人工反力を算出する。
【０１１６】
衝突回避制御部１４は、軌道修正量算出部１４１により構成される。軌道修正量算出部１４１では、人工反力分配部１５の出力である制御対象物体毎の分配人工反力を、衝突検出部１２により出力された衝突代表点対に作用させた場合の物体軌道修正量を、制御対象物体毎に算出する。
【０１１７】
図１２には、図１１に示した運動制御システムにおいて実行される処理手順をフローチャートの形式で示している。
【０１１８】
座標系設定処理ステップＳ２１では、シーンデータ内の各物体に設定されるローカル座標系のワールド座標系における位置及び姿勢情報を設定する。座標系設定部１１は、例えば各種動力学演算を担う力学シミュレーション・エンジンなどを用いて構成され、各物体の運動を決定する。
【０１１９】
次いで、膨張物体衝突予測ステップＳ２１では、衝突予測マージンにより膨張した物体間の衝突検出を実行する。同ステップにおいて膨張物体間での衝突が予測された場合には次ステップＳ３へ進み、予測されない場合は本処理ルーチンを終了する。
【０１２０】
衝突検出処理ステップＳ２３では、シーンデータと計算幾何学演算部１２２内におけるＧＪＫアルゴリズム実行部１２２２により、凸形状に近似した物体間の最短距離、衝突代表点対、及び衝突代表点方向を算出する。
【０１２１】
次いで、衝突予測ステップＳ２４では、衝突検出処理ステップの出力である凸形状物体間最短距離を利用して、物体が人工ポテンシャルの作用領域（反力作用領域）内に存在するか否か、すなわち、障害物を回避するための人工反力を発生させるべきか否かを判定する。同ステップにおいて、衝突が予測された場合はステップＳ２５へ進むが、衝突が予測されない場合にはＳ２７へと処理が進む。
【０１２２】
人工反力算出処理ステップＳ２５では、衝突代表点方向への物体間の最短距離に基づく人工反力を、上式（２）に基づいて算出する。
【０１２３】
分配人工反力算出処理ステップＳ２６では、人工反力算出処理ステップＳ２５で算出した人工反力を、上式（４）に従って、衝突予測された各制御対象物体に分配する。すなわち、人工反力算出処理ステップＳ２５により算出された人工反力をシーンデータの物体情報であるタスクによる運動プライオリティに基づいて各物体に分配して、衝突回避のための物体それぞれに作用させるべき分配人工反力を算出する。
【０１２４】
軌道修正量算出ステップＳ２７では、人工反力分配部１５の出力である制御対象物体毎の分配人工反力を、衝突検出部１２により出力された衝突代表点対に作用させた場合の物体軌道修正量を、制御対象物体毎に算出する。
【０１２５】
なお、運動プライオリティが固定データではなくタスクにより可変なものであることは既に述べたが、衝突予測マージンや反力作用領域（人工反力の作用する境界距離）に関しても、固定値である必要はなく、制御期間中は常に変更可能なパラメータとして扱うこともできる。その一例として、運動目的（タスク）に応じて、各物体の衝突予測マージン及び反力作用領域を可変にするよう構成することができる。
【０１２６】
各物体の衝突予測マージン及び反力作用領域を可変にすることで、より柔軟な運動指定を実現することが可能になる。例えば、それほど精密な衝突回避を必要としないようなタスクの場合、若しくは、より衝突を嫌う制御を行ないたい場合などには、衝突予測マージン又は反力作用領域のうち少なくとも一方を大きくとることで、物体間の間隔を大きく保ちながら、物体の粗い移動制御を行なうことができ、あるいはより安全な移動制御を行なうことができる。この場合、図５におけるシーンデータにおいて、物体間人工反力が作用する領域（反力作用領域）を定める距離境界値（反力作用領域距離境界値）は、変更可能なパラメータとなり、タスクにより決定される値となる。
【産業上の利用可能性】
【０１２７】
以上、特定の実施形態を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施形態の修正や代用を成し得ることは自明である。
【０１２８】
本発明は、力学シミュレーション、精巧なＣＧアニメーション、移動ロボットといった、仮想空間若しくは現実空間上で物体（制御対象）の運動制御を行なうさまざまなシステムに適用することができる。
【０１２９】
本明細書では、複数の制御対象物体間における衝突回避制御手法、若しくは制御対象物体と制御不可能な障害物における衝突回避制御手法について述べてきたが、制御対象物体が複数のプリミティブで構成された場合においても、同様に本発明を適用することが可能である。勿論、ヒューマノイドロボットにおける自己干渉回避制御にも本発明を適用することができる。
【０１３０】
要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、特許請求の範囲を参酌すべきである。
【図面の簡単な説明】
【０１３１】
【図１】図１は、３次元空間上に存在する、運動制御可能な物体（制御対象物体）と制御不可能な静的物体（障害物）との衝突回避問題を例示した図である。
【図２】図２は、３次元空間上に障害物が存在しないときに、制御対象物体がゴール地点を目指す軌道を例示した図である。
【図３】図３は、図１に示した制御対象物体の凸包近似例を示した図である。
【図４Ａ】図４Ａは、物体間の最近傍点算出処理を表した図である。
【図４Ｂ】図４Ｂは、物体間の最浸透点算出処理を表した図である。
【図５】図５は、人工物体間反力生成手法及び物体間人工反力に基づく衝突回避制御を行なう運動制御システムの機能ブロック図である。
【図６】図６は、サポート写像により任意の凸物体の頂点のうち，あるベクトル方向で最も遠くに存在する点を算出する様子を示した図である。
【図７】図７は、図５に示した運動制御システムにおいて実行される処理手順を示したフローチャートである。
【図８】図８は、制御対象物体毎にタスクに応じたプライオリティを付与して衝突回避制御を行なう運動制御システムの構成を示した図である。
【図９】図９は、図８に示した運動制御システムにおいて実行される処理手順を示したフローチャートである。
【図１０】図１０は、物体に衝突予測マージンを設定した様子を示した図である。
【図１１】図１１は、衝突予測マージンを利用して効率的な最距離演算を行なうように構成された運動制御システムの構成を示した図である。
【図１２】図１２は、図１１に示した運動制御システムにおいて実行される処理手順を示したフローチャートである。
【符号の説明】
【０１３２】
１０…運動制御システム
１１…座標系設定部
１２…衝突検出部
１２１…シーンデータ蓄積部
１２２…計算幾何学演算部
１２３…膨張物体衝突予測部
１２２１…サポート写像実行部
１２２２…ＧＪＫアルゴリズム実行部
１３…物体間人工反力算出部
１３１…衝突予測部
１３２…人工反力決定部
１４…衝突回避制御部
１５…人工反力分配部
【図４Ａ】
2008242859.tif 000007
【図４Ｂ】
2008242859.tif 000008
【図５】
2008242859.tif 000009
【図７】
2008242859.tif 000010
【図８】
2008242859.tif 000011
【図９】
2008242859.tif 000012
【図１１】
2008242859.tif 000013
【図１２】
2008242859.tif 000014
【図１】
2008242859.tif 000015
【図２】
2008242859.tif 000016
【図３】
2008242859.tif 000017
【図６】
2008242859.tif 000018
【図１０】
2008242859.tif 000019