JP4730440B2 - 軌道計画装置及び軌道計画方法、並びにコンピューター・プログラム - Google Patents

Description

本発明は、ロボットの移動制御や多関節アームの手先などの制御対象の、動作空間上における現在位置姿勢から目標位置姿勢までの移動経路若しくは軌道を計画する軌道計画装置及び軌道計画方法、並びにコンピューター・プログラムに係り、特に、障害物を回避するような制御対象の軌道を移動制御や軌道制御として用い易くなるように滑らかな軌道として修正する軌道計画装置及び軌道計画方法、並びにコンピューター・プログラムに関する。

例えば、ロボットのような移動体装置や、多関節アームのエンド・エフェクタとなる手先などを自律的に動作制御させる場合、動作空間上のある位置姿勢（現在位置姿勢：Ｓｔａｒｔ）からある位置姿勢（目標位置姿勢：Ｇｏａｌ）までの移動経路若しくは軌道（Ｐａｔｈ）を計画しなければならない。以下では、「経路」と「軌道」を「軌道」と総称する。また、特に断りがない限り、軌道は位置姿勢の軌道を意味するものとする。すなわち、３次元空間における軌道とは、物体の空間姿勢を記述するための６つの自由度の軌道を指す。

本願は、ロボットや多関節アームなどの現在の位置姿勢から目標とする位置姿勢までの滑らかな軌道を高速に生成することを主な目的とする。本発明者らは、かかる目的を実現するためには、以下の４つの項目を満たす必要があると思料する。

（１）最適解でなくとも、目標とする位置姿勢軌道を達成するための、何らかの初期位置姿勢軌道を高速に獲得すること。
（２）滑らかな位置姿勢軌道を獲得すること。
（３）位置姿勢軌道において、物体間の浸透（衝突若しくは干渉）を回避すること。
（４）できる限り理想の（設計者の意図を反映させた）位置姿勢軌道に近づけること。

無限に存在する軌道の中から最適解を求めることは難しい。これまで、ポテンシャル法（例えば、非特許文献１を参照のこと）、確率的ロードマップ法（例えば、非特許文献１を参照のこと）、Ｒａｐｉｄｌｙ−ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅ（ＲＲＴ）（例えば、非特許文献２を参照のこと）といった多くの軌道計画方法について提案がなされてきた。

ここで、ポテンシャル法とは、動作空間に目的姿勢に制御対象を近づける「引力ポテンシャル」、並びに、障害物から制御対象を遠ざける「斥力ポテンシャル」というポテンシャル場を定義し、制御対象の現在姿勢におけるポテンシャル場の勾配を算出し、ポテンシャルの減少する方向へ微少に移動させる手続きを繰り返すことによって軌道を計画する方法である。

例えば、移動体、障害物、及び目標物の位置及び形状の情報に基づいて生成された移動空間内のマップを用いて、移動体、障害物、及び目標物の相対位置関係に基づく引力ポテンシャル及び斥力ポテンシャルを計算し、これらの和である合成ポテンシャルを生成し、この合成ポテンシャルに基づいてマップ内の経路探索を行なって該経路探索の収束位置がローカルミニマムかどうかを判別し、収束位置がローカルミニマムの場合には該収束位置のポテンシャルを所定値だけ増加させた仮想ポテンシャルを生成するとともに、収束位置が目標位置の場合には経路探索の結果に基づいて移動体の移動経路を生成する経路生成装置について提案がなされている（例えば、特許文献１を参照のこと）。

また、確率的ロードマップ法は、配置空間内から幾つかの姿勢をランダムに選び、近接する姿勢に移動可能であれば枝でつないでいき、ロードマップが連結でなければ、障害物の多い領域にある姿勢からランダム・ウォークを行ない、ロードマップを拡張し、数理計画上の手法であるダイクストラ法を適用してロードマップにおける最短経路を求める方法である。

例えば、ダイクストラ法によって、ネットワークにおける最小コストの入ノードから出ノードまでの経路を探索する最小コスト経路探索装置について提案がなされている（例えば、特許文献２を参照のこと）。

また、ＲＲＴは、動作空間を好適に網羅する木のデータ構造、並びに当該木を生成する基本となるアルゴリズムである。ＲＲＴアルゴリズムでは、まず動作空間上に初期位置を設定した後、同動作空間内にランダムなサンプル位置を配置するとともに初期位置から当該サンプル位置に向かう最近傍位置を探索し、初期位置と最近傍位置とを結んで木に追加する。その後、新たに追加された最近傍位置についてサンプル位置の配置とさらに最近傍位置の探索と木への追加を繰り返すことで、動作空間を網羅する木を生成することができる。現在、ＲＲＴアルゴリズムは、実時間での経路計画や軌道生成などに用いられており（例えば、非特許文献３を参照のこと）、多くの研究事例がある。

ポテンシャル法は、非常に直感的であるが、目的位置姿勢以外にもポテンシャルが極小となる位置姿勢が多数表れる可能性があることから、安全な動作経路を効率よく求めることは困難である。確率的ロードマップ法は、多くの障害物に囲まれた狭い空間を通る動作計画が困難、すなわち多数の位置姿勢をサンプルしなくてはならない、といった欠点を持つ。ＲＲＴアルゴリズムは、多次元ｃｏｎｆｉｇｕｒａｔｉｏｎ ｓｐａｃｅでの比較的高速な軌道計画（Ｐａｔｈ Ｐｌａｎｎｉｎｇ）が可能なことで知られているが、ランダム探索を用いることから、探索した軌道は多くの迂回を含むという欠点を持つ。

このように、従来の軌道計画手法はそれぞれ一長一短があることが分かる。また、いずれかの手法により生成した軌道を、平滑化したり障害物を回避したりするための軌道修正を行なう必要がある。軌道を平滑化する手法として、躍度最小や（例えば、非特許文献４を参照のこと）、トルク最小、多項式（Ｓｐｌｉｎｅ）を用いた手法が考案されている。しかしながら、いずれの手法も、上記に掲げた要求項目（２）平滑な位置姿勢軌道を獲得すること、（３）物体間の浸透を回避すること、（４）最適な軌道に近づけること、を同時に満たすことはできない。言い換えれば、ロボットの移動制御や多関節アームの手先の軌道計画などのための最終的な制御に用いるものとして満足な軌道を得ることはできない。

例えば、Ｂスプラインを用いたヒューマノイド動作計画の軌道を平滑化する方法について提案がなされている（例えば、非特許文献５を参照のこと）。この方法によれば、空間上の位置及び３自由度の姿勢を取り扱って、軌道修正を行なうことができるが、計算量が過大になるという問題がある。

特開２００６−３５０７７６号公報 特開２００２−１３３３５１号公報

太田順外著「知能ロボット入門 −動作計画問題の解法−」（コロナ社、２００１） Ｓｔｅｖｅｎ Ｍ．ＬａＶａｌｌｅ，"Ｒａｐｉｄｌｙ−Ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ：Ａ Ｎｅｗ Ｔｏｏｌ ｆｏｒ Ｐａｔｈ Ｐｌａｎｎｉｎｇ"（ＴＲ．９８−１１，Ｃｏｍｐｕｔｅｒ Ｓｃｉｅｎｃｅ Ｄｐｔ．，Ｉｏｗａ Ｓｔａｔｅ Ｕｎｉｖｅｒｓｉｔｙ．Ｏｃｔ．１９９８） Ｊａｍｅｓ Ｊ．Ｋｕｆｆｎｅｒ，Ｊｒ．，"Ｄｙｎａｍｉｃａｌｌｙ−ｓｔａｂｌｅ Ｍｏｔｉｏｎ Ｐｌａｎｎｉｎｇ ｆｏｒ Ｈｕｍａｎｏｉｄ Ｒｏｂｏｔｓ"（Ａｕｔｏｎｏｍｏｕｓ Ｒｏｂｏｔｓ ｖｏｌ．１２，Ｎｏ．１，ｐｐ．１０５−１１８，２００２） Ｔ．Ｆｌａｓｈ ａｎｄ Ｎ．Ｈｏｇａｎ，"Ｔｈｅ Ｃｏｏｒｄｉｎａｔｉｏｎ ｏｆ Ａｒｍ Ｍｏｖｅｍｅｎｔｓ：Ａｎ Ｅｘｐｅｒｉｍｅｎｔａｌｌｙ Ｃｏｎｆｉｒｍｅｄ Ｍａｔｈｅｍａｔｉｃａｌ Ｍｏｄｅｌ"（Ｔｈｅ Ｊｏｕｒｎａｌ ｏｆ Ｎｅｕｒｏｓｃｉｅｎｃｅ，Ｖｏｌ．５，Ｎｏ．７，ｐｐ．１６８８−１７０３，１９８５） 原田、森澤、三浦、藤原、梶田共著「Ｂスプラインを用いたヒューマノイド動作計画のための軌道平滑化」（Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ ２００８ ＪＳＭＥ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ｍｅｃｈａｔｒｏｎｉｃｓ，２Ｐ１−Ｆ１６，２００８）

本発明の目的は、ロボットの移動制御や多関節アームの手先などの制御対象の、動作空間上における現在位置姿勢から目標位置姿勢までの移動経路若しくは軌道を好適に計画することができる、優れた軌道計画装置及び軌道計画方法、並びにコンピューター・プログラムを提供することにある。

本発明のさらなる目的は、障害物を回避するような制御対象の軌道を発見するとともに、制御対象の移動制御や軌道制御に用い易くなるように滑らかな軌道として修正することができる、優れた軌道計画装置及び軌道計画方法、並びにコンピューター・プログラムを提供することにある。

本発明のさらなる目的は、制御対象の現在の位置姿勢から目標とする位置姿勢までの滑らかな軌道を高速に生成することができる、優れた軌道計画装置及び軌道計画方法、並びにコンピューター・プログラムを提供することにある。

本願は、上記課題を参酌してなされたものであり、請求項１に記載の発明は、動作空間における制御対象の現在位置姿勢から目標位置姿勢に至るまでの位置姿勢軌道を計画する軌道計画装置であって、
前記制御対象の前記現在位置姿勢と前記目標位置姿勢の間を繋ぐ複数のノードからなる初期の位置姿勢軌道を獲得し、前記位置姿勢軌道上の各ノードに前記制御対象に相当する剛体をそれぞれ配置する初期位置姿勢軌道獲得部と、
前記位置姿勢軌道上の前記制御対象と前記動作空間に存在する障害物との干渉を検出する干渉検出部と、
前記剛体の各々に対して、前記位置姿勢軌道が平滑で且つ前記障害物との干渉を回避するための要求項目を表現する外力を算出する外力算出部と、
前記外力算出部で算出された外力がそれぞれ作用する複数の前記剛体からなる連成運動を記述した複数の剛体運動方程式に対して多剛体系動力学シミュレーションを実行し、所定の状態に収束したときの前記剛体の各々の位置及び姿勢に基づいて、前記位置姿勢軌道上の各ノードに相当する時刻における前記制御対象の位置及び姿勢を得る多剛体系動力学シミュレーション実行部と、
を具備すること特徴とする軌道計画装置である。

また、本願の請求項２に記載の発明によれば、初期位置姿勢軌道獲得部は、前記複数のノードで複数の経路に分岐して前記動作空間上に展開された探索木に基づいて、前記制御対象の現在位置と目標位置の間の軌道を獲得するとともに、各ノードに配置する剛体に対して初期の姿勢を付与するように構成されている。

また、本願の請求項３に記載の発明によれば、干渉検出部は、ＧＪＫアルゴリズム（後述）を適用して、前記剛体の各々と前記障害物との最短距離を検出するように構成されている。

また、本願の請求項４に記載の発明によれば、外力算出部は、前記剛体のローカル座標系に固定された仮想剛体面を定義し、隣接する仮想剛体面の対応する頂点同士を所定のばね定数を持つばね及び所定のダンピング定数を持つダンパーで並列接続した力学モデルを用い、仮想剛体面の各頂点が隣接する仮想剛体面の対応する頂点との間で働くばね力及びダンパー力の合計からなる剛体間力と、前記ばね力及びダンパー力によって各仮想剛体の頂点に作用するモーメントの合計からなる剛体に作用するワールド座標系原点回りのモーメントを、前記位置姿勢軌道を平滑にするための要求項目を表現する外力として算出するように構成されている。

また、本願の請求項５に記載の発明によれば、前記干渉検出部は、前記位置姿勢軌道上の各ノードに配置された前記剛体と前記障害物との最短距離を検出し、前記外力算出部は、前記最短距離に応じて前記剛体の各々に作用する斥力を、前記位置姿勢軌道が前記障害物との干渉を回避するための要求項目を表現する外力として算出するように構成されている。

また、本願の請求項６に記載の発明によれば、外力算出部は、前記剛体のローカル座標系に固定された前記剛体の前方進行方向と隣接する前記剛体間における位置姿勢軌道の方向とのなす角に応じたモーメントを、理想の位置姿勢軌道に近づけるための要求項目を表現する外力として算出するように構成されている。

また、本願の請求項７に記載の発明は、動作空間における制御対象の現在位置姿勢から目標位置姿勢に至るまでの位置姿勢軌道を計画する軌道計画方法であって、
前記制御対象の前記現在位置姿勢と前記目標位置姿勢の間を繋ぐ複数のノードからなる初期の位置姿勢軌道を獲得し、前記位置姿勢軌道上の各ノードに前記制御対象に相当する剛体をそれぞれ配置する初期位置姿勢軌道獲得ステップと、
前記位置姿勢軌道上の前記制御対象と前記動作空間に存在する障害物との干渉を検出する干渉検出ステップと、
前記剛体の各々に対して、前記位置姿勢軌道が平滑で且つ前記障害物との干渉を回避するための要求項目を表現する外力を算出する外力算出ステップと、
前記外力算出部で算出された外力がそれぞれ作用する複数の前記剛体からなる連成運動を記述した複数の剛体運動方程式に対して多剛体系動力学シミュレーションを実行し、所定の状態に収束したときの前記剛体の各々の位置及び姿勢に基づいて、前記位置姿勢軌道上の各ノードに相当する時刻における前記制御対象の位置及び姿勢を得る多剛体系動力学シミュレーション実行ステップと、
を有すること特徴とする軌道計画方法である。

また、本願の請求項８に記載の発明は、動作空間における制御対象の現在位置姿勢から目標位置姿勢に至るまでの位置姿勢軌道を計画するための処理をコンピューター上で実行するようにコンピューター可読形式で記述されたコンピューター・プログラムであって、前記コンピューターを、
前記制御対象の前記現在位置姿勢と前記目標位置姿勢の間を繋ぐ複数のノードからなる初期の位置姿勢軌道を獲得し、前記位置姿勢軌道上の各ノードに前記制御対象に相当する剛体をそれぞれ配置する初期位置姿勢軌道獲得部、
前記位置姿勢軌道上の前記制御対象と前記動作空間に存在する障害物との干渉を検出する干渉検出部、
前記剛体の各々に対して、前記位置姿勢軌道が平滑で且つ前記障害物との干渉を回避するための要求項目を表現する外力を算出する外力算出部、
前記外力算出部で算出された外力がそれぞれ作用する複数の前記剛体からなる連成運動を記述した複数の剛体運動方程式に対して多剛体系動力学シミュレーションを実行し、所定の状態に収束したときの前記剛体の各々の位置及び姿勢に基づいて、前記位置姿勢軌道上の各ノードに相当する時刻における前記制御対象の位置及び姿勢を得る多剛体系動力学シミュレーション実行部、
として機能させるためのコンピューター・プログラムである。

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

本発明によれば、ロボットの移動制御や多関節アームの手先などの制御対象の、動作空間上における現在位置姿勢から目標位置姿勢までの移動経路若しくは軌道を好適に計画することができる、優れた軌道計画装置及び軌道計画方法、並びにコンピューター・プログラムを提供することができる。

また、本発明によれば、障害物を回避するような制御対象の軌道を発見するとともに、制御対象の移動制御や軌道制御に用い易くなるように滑らかな軌道として修正することができる、優れた軌道計画装置及び軌道計画方法、並びにコンピューター・プログラムを提供することができる。

また、本発明によれば、制御対象の現在の位置姿勢から目標とする位置姿勢までの滑らかな軌道を高速に生成することができる、優れた軌道計画装置及び軌道計画方法、並びにコンピューター・プログラムを提供することができる。

本願の請求項１、７、８に記載の発明によれば、制御対象の軌道上の各ノードを剛体モデルとして扱うことにより、制御対象の位置だけでなく姿勢の軌道も取り扱うことができる。

また、本願の請求項１、７、８に記載の発明によれば、初期位置姿勢軌道獲得部によって獲得した初期位置姿勢軌道を、多剛体系動力学シミュレーションを導入して修正することができる。

また、本願の請求項１、７、８に記載の発明によれば、制御対象の位置姿勢軌道が平滑で且つ障害物との干渉を回避するために必要とされる要求項目を力学モデル、すなわち力学拘束として表現することによって、位置姿勢軌道を修正するための処理を力の次元で統一的に解決することができる。

また、本願の請求項１、７、８に記載の発明によれば、かかる力学拘束を受ける複数の剛体からなる連成運動を記述した複数の剛体運動方程式に対して多剛体系動力学シミュレーションを適用することによって、各剛体の位置及び姿勢を得ることができ、これらは位置姿勢軌道上の各ノードに相当する時刻における制御対象の位置及び姿勢に相当する。

本願の請求項１、７、８に記載の発明は、リアルタイム障害物回避に適用であるとともに、リアルタイムでの目標位置姿勢修正が可能である。

また、本願の請求項２に記載の発明によれば、初期位置姿勢軌道獲得部は、ＲＲＴアルゴリズムを適用して、軌道の平滑化を考慮せずに、最適解とは必ずしも限らない最低限目的を達成するための軌道を高速且つ確実に探索することができる。ＲＲＴアルゴリズムによれば、軌道上の各ノードの位置を獲得した後、各ノードの初期姿勢を適当に与える。ノードにおける姿勢は剛体のローカル座標系に相当し、初期姿勢としてワールド座標系に設定するようにしてもよい。

また、本願の請求項３に記載の発明によれば、ＧＪＫアルゴリズムなどの凸形図形間距離を算出するための反復的な方法を用いることによって、軌道上のノードに相当する各剛体と障害物との最近傍点対若しくは最浸透点対を結ぶ最短距離を検出することができる。

また、本願の請求項４に記載の発明によれば、隣接する剛体同士で引き寄せ合う力（若しくは斥力）、すなわち「剛体間力」という力学拘束表現によって、「滑らかな位置姿勢軌道を獲得すること」という要求項目を満たすことができる。ここで、「滑らかな位置姿勢軌道を獲得すること」という要求項目を剛体間力として表現する際、剛体を、仮想的に体積を持たない面として扱うことで、近接する剛体同士の重なりの問題が生じにくくなる。

また、本願の請求項５に記載の発明によれば、制御対象と障害物との最短距離に応じた斥力を剛体に作用させるという力学拘束表現によって、「位置姿勢軌道において、物体間の浸透を回避すること」という要求項目を満たすことができる。

また、本願の請求項６に記載の発明によれば、制御対象の理想の姿勢軌道を、「軌道上の各ノードに配置された剛体の前方進行方向がなるべく軌道に沿うこと」と定義し、剛体の前方進行方向と隣接する剛体間における位置姿勢軌道の方向とのなす角に応じたモーメントを剛体に作用させるという力学拘束表現によって、「できる限り理想の位置姿勢軌道に近づけること」という要求項目を満たすことができる。

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

図１Ａは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。 図１Ｂは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。 図１Ｃは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。 図１Ｄは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。 図１Ｅは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。 図１Ｆは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。 図１Ｇは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。 図２Ａは、ＲＲＴアルゴリズムにより得られる軌道生成例を示した図である（但し、制御対象を表示）。 図２Ｂは、ＲＲＴアルゴリズムにより得られる軌道生成例を示した図である（但し、制御対象を非表示）。 図３Ａは、ＲＲＴアルゴリズムにより得られる軌道生成例を示した図である（但し、制御対象を表示）。 図３Ｂは、ＲＲＴアルゴリズムにより得られる軌道生成例を示した図である（但し、制御対象を非表示）。 図４は、初期条件の多剛体系の一例を示した図である。 図５は、剛体に対して定義される仮想剛体面を説明するための図である。 図６は、隣接する仮想剛体面間に作用する力を説明するための図である。 図７は、制御対象の理想の姿勢軌道を実現するための力学モデルを示した図である。 図８は、制御対象の軌道計画を実現するシステムの機能的構成を模式的に示した図である。 図９は、図８に示したシステムにおいて制御対象の軌道計画を実現するための処理手順を示したフローチャートである。 図１０は、図３に示したようなＲＲＴアルゴリズムにより得られる初期軌道に対して、多剛体系動力学シミュレーションを適用して修正した結果を示した図である。 図１１は、多剛体系動力学シミュレーションを適用して修正した軌道の他の例を示した図である。 図１２は、図１１に示した軌道における剛体位置を示したグラフである。 図１３は、多剛体系動力学シミュレーションを適用して修正した軌道のさらに他の例を示した図である。 図１４は、図１３に示した軌道における剛体姿勢のピッチ角を示した図である。 図１５は、ばね定数を定数とした場合の他剛体系力学モデルの軌道生成結果のスナップショットを示した図である。 図１６は、ばね定数を剛体ｉと剛体０（現在状態）若しくは剛体Ｎ（目標状態）との距離に応じて変化する変数とした場合の軌道生成結果のスナップショットを示した図である。 図１７は、ばね定数を定数並びに変数としてそれぞれ生成した軌道上における各剛体（ノード）の剛体姿勢のグラフを示した図である。 図１８は、図３に示した状態を初期状態としたとき、図中向かって奥側の障害を動かした場合の多剛体系動力学シミュレーションの結果を示した図である。 図１９は、多剛体系動力学シミュレーション実行し、剛体群が静定している状態を示した図である。 図２０は、多剛体系動力学シミュレーション実行し、剛体群が静定している状態を示した図である。 図２１は、シミュレーション実行後に、目標位置姿勢に修正値を与え、シミュレーションが収束した状態でのスナップショットを示した図である。 図２２は、シミュレーション実行後に、目標位置姿勢に修正値を与え、シミュレーションが収束した状態でのスナップショットを示した図である。 図２３は、本発明を実現可能なコンピューターの構成例を示した図である。

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

ロボットや多関節アームなどの現在の位置姿勢から目標とする位置姿勢までの滑らかな軌道を高速に生成するという目的を達成するには、以下の４つの項目が要求される。なお、以下で単に「制御対象」と言うときはロボットや多関節アームなどを含むものとする。

（１）最適解でなくとも、目標とする位置姿勢軌道を達成するための、何らかの初期位置姿勢軌道を高速に獲得すること。
（２）滑らかな位置姿勢軌道を獲得すること。
（３）位置姿勢軌道において、物体間の浸透（衝突若しくは干渉）を回避すること。
（４）できる限り理想の（設計者の意図を反映させた）位置姿勢軌道に近づけること。

要求項目（１）の解決においては、軌道の平滑化は問題ではなく、最適解とは必ずしも限らない最低限目的を達成するための軌道を、いかに高速に且つ確実に探索することができるかが選定基準となる。そこで、本発明の一実施形態では、何らかの初期位置姿勢軌道を高速に獲得するために、前述したＲＲＴアルゴリズムを用いることとした。ＲＲＴアルゴリズムは、取り扱う問題が多次元の場合には直感的ではないといった欠点を持つものの、低次元の問題においては直感性を失うことはない。また、ＲＲＴアルゴリズムのもう１つの特徴として、多次元ｃｏｎｆｉｇｕｒａｔｉｏｎ ｓｐａｃｅでの比較的高速な軌道計画が可能なことで知られている。

ここで、ＲＲＴアルゴリズムにおけるＴｒｅｅを生成する基本となる手順について、図１Ａ〜図１Ｇを参照しながら説明しておく。

まず、動作空間上に初期位置ｑ＿ｉｎｉｔを設定する（図１Ａを参照のこと）。

次いで、動作空間上にランダムにｑ＿ｒａｎｄを配置する（図１Ｂを参照のこと）。

次いで、Ｔｒｅｅ（探索木）におけるｑ＿ｒａｎｄに最も近いノード（すなわち最近傍ノード）をｑ＿ｎｅａｒとし、ｑ＿ｎｅａｒからｑ＿ｒａｎｄに向かう方向で一定距離にｑ＿ｎｅｗを設定する（図１Ｃを参照のこと）。但し、初期状態のＴｒｅｅにはｑ＿ｉｎｉｔのみが存在する（図１Ａを参照のこと）。ここで、ｑ＿ｎｅａｒとｑ＿ｎｅｗをつないで、ｑ＿ｎｅｗをＴｒｅｅに追加する（図１Ｄを参照のこと）。

次いで、動作空間上にランダムにｑ＿ｒａｎｄを再び配置し（図１Ｅを参照のこと）、Ｔｒｅｅにおけるｑ＿ｒａｎｄの最近傍ノードをｑ＿ｎｅａｒとし、前回と同様にｑ＿ｎｅａｒとｑ＿ｒａｎｄからｑ＿ｎｅｗを生成して、ｑ＿ｎｅｗをＴｒｅｅに追加する（図１Ｆを参照のこと）。

そして、上述したような操作を繰り返して実施することで、動作空間を好適に網羅するＴｒｅｅを生成することができる（図１Ｇを参照のこと）。

ＲＲＴによれば、比較的高速な軌道計画が可能であり、最適解とは言わなくてもそれなりの軌道を生成することができる。

図２並びに図３には、ＲＲＴアルゴリズムにより得られる軌道生成例を示している。各図では、説明の簡素化のため、制御対象の形状をＢｏｘ形状（若しくは直方体）とした。但し、本発明の要旨は特定の制御対象の形状に限定されるものではなく、いかなる形状に対しても適用でき、いかなる形状に対しても高速に軌道計画が可能であることを十分理解されたい。軌道計画方法の詳細に関しては後述に譲る。

図２Ａ並びに図３Ａには、Ｂｏｘ形状の制御対象を表示している。また、図２Ｂ並びに図３Ｂには、制御対象を非表示にし、その重心をノードで表示している。また、図２Ａ並びに図３Ａにおいて、両端のＢｏｘが現在位置姿勢（−ｘ方向側）、目標位置姿勢（＋ｘ方向側）での制御対象をそれぞれ表し、これらを繋ぐ一連のＢｏｘがＲＲＴアルゴリズムにより発見された軌道上の制御対象を表している。また、ほぼ中央のＢｏｘは、３次元空間上に存在する障害物を表し、この障害物Ｂｏｘを避けるように制御対象の軌道が探索されている。

図２及び図３で示したＲＲＴアルゴリズムのｃｏｎｆｉｇｕｒａｔｉｏｎ ｓｐａｃｅの次元は位置（ｘ，ｙ，ｚ）の３次元としたが、姿勢も含めた６次元としても、本発明を実現する上で何ら影響はない。強いて言えば、非常に多くの障害物があり、制御対象物の姿勢を頻繁に変化させなければ通過することができないような場合の初期軌道探索で大きな威力を発揮する。しかし、その場合、ＲＲＴアルゴリズムによる初期軌道探索に関して速度の低下は否めない。このため、本明細書では、より高速な初期軌道探索を実現するために、位置の３次元をｃｏｎｆｉｇｕｒａｔｉｏｎ ｓｐａｃｅとするＲＲＴアルゴリズムを１つの実施形態として示した。

なお、ＲＲＴアルゴリズムを用いて初期位置姿勢軌道を獲得する際には、障害物の回避の際の物体間浸透の問題が並存する。そこで、本実施形態では、ＲＲＴアルゴリズムの実行において、障害物の回避の際の物体間浸透の問題に関してＧＪＫ（Ｇｉｌｂｅｒｔ−Ｊｏｈｎｓｏｎ−Ｋｅｅｒｔｈｉ ｄｉｓｔａｎｃｅ ａｌｇｏｒｉｔｈｍ）アルゴリズムを適用する。ＧＪＫアルゴリズムは、凸形図形間距離を算出するための反復的な方法であり、衝突代表点対（最近傍点対又は最浸透点対）若しくは物体間の最短距離を、干渉検出結果として出力する。干渉検出結果は、力学モデルで表現して扱われるが、この点の詳細については後述に譲る。また、ＧＪＫアルゴリズムの詳細については、例えば、Ｇ．ｖａｎ ｄｅｎ Ｂｅｒｇｅｎ著“Ｃｏｌｌｉｓｉｏｎ Ｄｅｔｅｃｔｉｏｎ ｉｎ Ｉｎｔｅｒａｃｔｉｖｅ ３Ｄ Ｅｎｖｉｒｏｎｍｅｎｔｓ”（Ｍｏｒｇａｎ Ｋａｕｆｍａｎｎ Ｐｕｂｌｉｓｈｅｒｓ，２００４）を参照されたい。

図２並びに図３を参照して分かるように、ＲＲＴアルゴリズムにより探索した初期軌道は、障害物の回避のため多くの迂回を含み、軌道としては明らかに無駄が多い。このため、ＲＲＴアルゴリズムを用いて初期位置姿勢軌道を高速に獲得した後、さらに上述した要求項目（２）〜（４）の解決が必要となる。

本出願人に既に譲渡されている特願２００８−５５６８１号明細書に開示されている軌道生成装置では、上記の要求項目（２）〜（４）を解決する際に、多質点系動力学シミュレーションを用いている。これは、制御対象の軌道上の各ノードを質点として扱い、軌道上の隣接するノード間、並びに最適経路上の対応するノードとの間において、軌道計画への要求を表現した力が作用するという力学モデルを構築し、動力学シミュレーション演算を実行することで、各質点の位置すなわち軌道を修正するものである。但し、軌道上の各ノードは質点で表現されるため、軌道上の制御対象の位置を求めることはできても、軌道上の制御対象の姿勢を求めることはできない。

これに対し、本実施形態では、要求項目（２）〜（４）を解決するに際して、多剛体系動力学シミュレーションを導入する。「多剛体系動力学シミュレーション」とは、軌道上の各ノードの位置において、制御対象に相当する剛体モデルを逐次配置していき、上記要求項目（２）〜（４）を各剛体に作用する外力として表現して、動力学シミュレーションを実行することで、各剛体の位置及び姿勢を求める手法である。軌道上の各ノードに配置された剛体の位置姿勢は、現在位置姿勢から目標位置姿勢まで移動する際の該当する時刻における制御対象の位置姿勢に相当する。多剛体系動力学シミュレーションは、制御対象の軌道上の各ノードに対して軌道計画への上記要求項目（２）〜（４）を表現した力を作用させるという点では、上記の多質点系動力学シミュレーションに類似するが、ノードに質点ではなく剛体を配置することによって、軌道生成において制御対象の位置とともに姿勢も取り扱うことが可能となる。

まず、多剛体系動力学シミュレーションの初期条件として、ＲＲＴアルゴリズムにより探索した軌道を制御対象の初期軌道とし（前述）、この初期軌道上の各ノードの位置を、剛体の代表点の初期位置とする。また、ノード毎の剛体の初期姿勢を適当に割り当てて、それらを初期位置姿勢とした多剛体系力学モデルを構築する。例えば、各ノードの剛体の初期姿勢（すなわち、剛体のローカル座標系）として、ワールド座標系に一様に設定するようにしてもよい。図４には、初期軌道上の各ノードの位置に、ワールド座標系に初期姿勢が設定された剛体がそれぞれ配置された多剛体系を示している。

なお、以下では、剛体の重心位置を剛体上の代表点とし、この代表点がノード位置と一致するように各ノードに剛体を配置するものとして説明するが、本発明の要旨はこのような剛体の配置方法に限定されるものではない。また、本発明の要旨は、初期姿勢の割り当てに関し特定の方法に限定されるものではなく、スプライン補間により姿勢を求める、あるいは軌道を計画する設計者が任意に姿勢を設定するようにしてもよい。

そして、本実施形態では、このような多剛体系に加わる外力モデルとして、上記の各要求項目（２）〜（４）を力学拘束として表現し、その力学拘束を多剛体系の各剛体に対し付加して、多剛体系動力学シミュレーションを実行する。これにより、軌道計画に必要な要求項目（１）〜（４）をすべて満足する軌道生成が実現される。つまり、すべての要求項目を力の次元で統合することで、統合的に問題解決することになる。

以下では、本発明の中核となる、上記の各要求項目（２）〜（４）に対する力学拘束モデルの表現方法について順に説明する。

まず、要求項目（２）「滑らかな位置姿勢軌道を獲得すること」の力学拘束表現について説明する。

図２並びに図３は、説明の簡素化のため、制御対象をＢｏｘ形状で表したが、現実にはＢｏｘ形状であることは稀であり、通常は複雑な形状を有する。また、ＲＲＴアルゴリズムの探索最小距離（ノード間距離）と物体形状の関係によっては、軌道上の各ノードに配置した多剛体系の隣接する剛体同士で重なりを持つ可能性を秘めている。また、できる限り生成する軌道を平滑に得るために、探索最小距離を小さくとることを考えると、多剛体系の各剛体間に重なりがない方がむしろ稀である。「滑らかな位置姿勢軌道を獲得すること」という要求項目を隣接する剛体同士で引き寄せ合う力、すなわち「剛体間力」という力学モデルで表現する際、剛体同士が重なり合うと、力学モデルは取り扱いにくいものとなる。

そこで、本実施形態では、軌道上の各ノードに（制御対象に相当する）剛体を順次配置した多剛体系における剛体間の重なりの問題を解決するために、各剛体に対して「仮想剛体面」を定義する。剛体を、仮想的に体積を持たない面として扱うことで、近接する剛体同士の重なりが生じにくくなることは自明であろう。

仮想剛体面は、各剛体に設定される姿勢すなわちローカル座標系に固定され、軌道計画中、元の剛体との相対位置関係は変化しない。但し、剛体のローカル座標系に対して任意に仮想剛体面を定義することができ、本発明の要旨は、仮想剛体面の特定の定義方法に限定されるものではない。

図５には、計画軌道上のｉ番目の剛体に対して定義された仮想剛体面を示している。図示の例では、剛体ｉのローカル座標系Σ_iのｘｙ平面との断面を仮想剛体面として定義されている。剛体ｉの仮想剛体面は、４角形状であり、４つの仮想剛体頂点ｘ_i,jで表現される（但し、ｊは頂点を指す通し番号で、０〜３の整数とする）。

多剛体系力学モデルにおいて、平滑な位置姿勢軌道を獲得するための力の拘束表現として、隣接する剛体間に力を作用させることで、各剛体の位置及び姿勢が変化し、無駄のない連続で滑らかな軌道に変換され、上記要求項目（２）を実現することができる。上記のように位置姿勢軌道上の各ノードに配置した剛体を仮想剛体面として扱う場合（図５を参照のこと）、隣接する仮想剛体面の対応する頂点の間をばね及びダンパーで並列接続する力学モデルにより、隣接する剛体同士が引き寄せ合う（若しくは斥力）剛体間力を作用させ、動力学シミュレーション（後述）を実行すると、無駄のない連続で滑らかな位置姿勢軌道に変換される。これによって、上記要求項目（２）を満足させることができる。

図６には、ｉ番目の剛体の仮想剛体頂点ｘ_i,jとこれに隣接する（ｉ＋１）番目の各剛体の仮想剛体頂点ｘ_i+1,ｊとを、ばね定数ｋ_sのばねと、ダンピング定数ｋ_dのダンパーで並列接続した、多剛体系の力学モデルを示している（ｊ＝０，１，２，３）。但し、動力学シミュレーションを実行する際、各仮想剛体頂点はワールド座標系での表現に変換されたものとする。なお、ばね定数ｋ_s及びダンピング定数ｋ_dは系全体として均一であるとは限らず、ノードｉ毎、あるいは頂点ｊ毎に異なるばね定数及びダンピング定数を使用することができる。

隣接する仮想剛体頂点ｘ_i,jとｘ_i+1,j間に働くばね力ｆ^i,i+1 _s,ｊは、下式（１）のように表される。

但し、上式（１）中のｌ^i,i+1 _jは隣接仮想剛体頂点ｘ_i,j、ｘ_i+1,j間のばねの自然長であり、ｋ^i,i+1 _s,jは当該頂点間を接続するばねのばね定数である。また、ｒ^i,i+1 _jは、仮想剛体頂点ｘ_i,jから隣接の対応する仮想剛体頂点ｘ_i+1,j間へ向かうベクトルであり、下式（２）のように表される。

隣接する仮想剛体頂点ｘ_i,jとｘ_i+1,j間でばねにより働くばね力ｆ^i,i+1 _s,ｊは、上式（１）に示したように、正規化されたばねの方向とばねの力の積で表される。

一方、隣接する仮想剛体頂点ｘ_i,jとｘ_i+1,j間にダンパーにより働くダンピング力ｆ^i,i+1 _d,ｊは、下式（３）のように表される。但し、当該頂点間を接続するダンパーのダンピング係数をｋ^i,i+1 _d,jとし、仮想剛体頂点ｘ_i,jの速度ベクトルをｖ^i,i+1 _jとする。

すなわち、２つの隣接仮想剛体頂点間の位置からダンピング力の方向を定め、相対速度の方向成分がダンピング力の大きさと正負符号を決定する。

上式（１）、（３）でそれぞれ示したばね力ｆ^i,i+1 _s,ｊ及びダンピング力ｆ^i,i+1 _d,ｊは、（ｉ＋１）番目の剛体のｊ番目の仮想剛体頂点からｉ番目の剛体のｊ番目の仮想剛体頂点に作用する力であるが、逆にｉ番目の剛体のｊ番目の仮想剛体頂点から（ｉ＋１）番目の剛体のｊ番目の仮想剛体頂点への作用力は、ｆ^i,i+1 _s,ｊ、ｆ^i,i+1 _d,ｊの符号をそれぞれ反転することで得られる。

（ｉ＋１）番目の剛体からｉ番目の剛体に作用する剛体間力ｆ^i,i+1 _nは、剛体に定義された仮想剛体面の各仮想剛体頂点ｊに作用するばね力ｆ^i,i+1 _s,ｊ及びダンピング力ｆ^i,i+1 _d,ｊの合計であり、下式（４）のように表される。

また、剛体間力ｆ^i,i+1 _nによりｉ番目の剛体に作用するワールド座標系原点回りのモーメントτ^i,i+1 _nは、各仮想剛体頂点に作用するモーメントの合計であり、下式（５）のように表される。

ｉ番目の剛体に作用する剛体間力ｆⁱ _n、τⁱ _nは、隣接する（ｉ−１）番目及び（ｉ＋１）番目の剛体からそれぞれ作用する剛体間力、モーメントの和であり、下式（６）に示す通りとなる。

剛体間力ｆ^i,i+1 _ｎは剛体ｉの位置を変化させ、隣接する剛体間で作用するモーメントτ^i,i+1 _nは剛体ｉの姿勢を変化させる。すなわち、隣接する剛体（ｉ−１）、（ｉ＋１）から剛体ｉに作用する斥力ｆⁱ _p及びモーメントτⁱ _pが上記要求項目（４）の力学拘束表現である。このような剛体間力、モーメントを計画軌道上の各ノードに配置された剛体に作用させることで、滑らかな位置姿勢軌道になり、上記要求項目（２）を満足させるということを理解されたい。

続いて、要求項目（３）「位置姿勢軌道において、物体間の浸透を回避すること」の力学拘束表現について説明する。

多剛体系力学モデルにおいて、物体間の浸透を回避するために課される力の拘束表現として、制御対象と障害物との最短距離に応じた斥力を剛体に作用させることで、多剛体系動力学シミュレーションを実行することにより各剛体の位置及び姿勢が変化し、無駄のない連続で滑らかな位置姿勢軌道に変換され、上記要求項目（３）を実現することができる。

多剛体系動力学シミュレーションを実行中は、常にＧＪＫアルゴリズム（前述）に基づく物体間の干渉検出機能が起動しており、制御対象と障害物との最短距離演算を行なっている。ここで、衝突代表点対、すなわち最短距離を構成する障害物上の点をＸ^o _near（ｘ^o _near，ｙ^o _near，ｚ^o _near）とし、制御対象上の点をＸ^r _near（ｘ^r _near，ｙ^r _near，ｚ^r _near）としたとき、制御対象と障害物との斥力ポテンシャルを、下式（７）に示すガウシアン関数で表現する。但し、同式中のｋ_pはポテンシャルの大きさを表す定数、σはポテンシャルの広がりを表す定数である。

このとき、制御対象（軌道上のｉ番目のノードに配置された剛体）上の点Ｘ^r _near（ｘ^r _near，ｙ^r _near，ｚ^r _near）に作用する人工斥力ｆⁱ _pは、下式（８）のように表される。

上式（８）では、厳密な干渉検出演算、最短距離演算を利用しているが、計算量を削減するために、最短距離を構成する制御対象（剛体ｉ）上の点と障害物上の点の代わりに、それぞれの重心位置ｘ_i、ｘ^o _c（但し、ワールド座標系表現とする）を用いてもよい。後者の場合、制御対象（軌道上のｉ番目のノードに配置された剛体）上の点Ｘ^r _near（ｘ^r _near，ｙ^r _near，ｚ^r _near）に作用する人工斥力ｆⁱ _p,nearは、下式（９）のように表される。

但し、上式（９）において、（ｘ^o _c，ｙ^o _c，ｚ^o _c）^Tは障害物の重心位置であり、ポテンシャルの中心を表すことになる。

また、上記の斥力ｆⁱ _pによって制御対象（軌道上のｉ番目のノードに配置された剛体）に作用する、ワールド座標系原点回りのモーメントτⁱ _pは、下式（１０）のように表現される。

人工斥力ｆⁱ _pは剛体ｉの位置を変化させ、斥力により剛体ｉに作用するモーメントτⁱ _pは剛体の姿勢を変化させる。すなわち、制御対象と障害物間の距離に応じて剛体ｉに作用する斥力ｆⁱ _p及びモーメントτⁱ _pが上記要求項目（３）の力学拘束表現である。このような斥力及びモーメントを計画軌道上の各ノードに配置された剛体に作用させることで、物体間の浸透を回避する位置姿勢軌道になり、上記要求項目（３）を満足させるということを理解されたい。

続いて、要求項目（４）「できる限り理想の位置姿勢軌道に近づけること」の力学拘束表現について説明する。

本実施形態では、制御対象の理想の姿勢軌道を、「軌道上の各ノードに配置された剛体の前方進行方向がなるべく軌道に沿うこと」と定義することにする。ここで、剛体の前方進行方向は、剛体のローカル座標系に固定されるものである。但し、本発明の要旨は、剛体のローカル座標系に対する特定の方向に剛体の前方進行方向が限定されるものではなく、ローカル座標系に対して固定されていれば任意の方向を前方進行方向に決定できるものとする。

図７には、制御対象の理想の姿勢軌道を上記のように定義した場合における力学モデルを図解している。以下では、同図を参照しながら、「できる限り理想の位置姿勢軌道に近づけること」の力学拘束表現について詳解する。

説明の簡素化のため、位置姿勢軌道上の各ノードに配置する剛体ｉの代表点であるその重心位置が原点となるようにローカル座標系Σ_iが設定されているもとする。また、剛体ｉの前方進行方向はこのローカル座標系Σ_iの原点を始点として固定されたある方向ｄⁱ _lであるとする。

多剛体系力学モデルにおいて、ｉ番目並びに（ｉ＋１）番目の各剛体の重心位置をそれぞれｘ_i、ｘ_i+1とすると、これら隣接する剛体間における位置姿勢軌道の方向ｄ_i,i+1は、下式（１１）のように表される。

理想の位置姿勢軌道に関する上記の定義に従うと、上式（１１）に示した剛体ｉから剛体（ｉ＋１）へ向かう方向ｄ_i,i+1が剛体ｉの前方進行方向ｄⁱ _lにとっての理想の位置姿勢軌道となる。

このとき、剛体ｉから剛体（ｉ＋１）へ向かう方向ｄ_i,i+1と、剛体ｉのローカル座標系に固定された前方進行方向ｄⁱ _lとのなす角に、あるばね定数ｋ_τを乗じたばね力を、ｄ_i,i+1とｄⁱ _lの外積方向回りの力として多剛体系力学モデルに付加することで、理想の姿勢軌道にできる限り近づけるための力学拘束を表現する。すなわち、この場合の理想軌道に近づけるために作用するモーメントτⁱ _oは下式（１２）のように表される。

なお、上式（１２）では、ｋ_τを定数として扱っているが、変数として扱うこともできる。例えば、初期状態（制御対象の現在位置に相当する剛体０）から剛体ｉまでの距離、若しくは、剛体ｉから目標状態（制御対象の目標位置に相当する剛体Ｎ）までの距離に応じて変化する変数として、ｋ_τを扱うようにしてもよい。このように距離に応じて変化する変数として扱うことで、他の力学拘束と統合した場合の整合性を保つのが容易になり、より目的とする滑らかな軌道を得ることができる。

なお、理想軌道にできる限り近づけるために剛体ｉに作用する力ｆⁱ _oは、下式（１３）に示すように０とする。

また、上記要求項目（４）の力学拘束表現では、多剛体系動力学シミュレーションの収束をコントロールするために、空気抵抗のような粘性抵抗による力も併せて導入する。剛体ｉの速度をｖⁱ _c、剛体ｉに作用する粘性係数をｋⁱ _cとすると、剛体ｉに作用する粘性抵抗による力ｆⁱ _dは、下式（１４）のように表される。

また、上式（１４）に示した粘性力ｆⁱ _dによるワールド座標系原点回りのモーメントは、下式（１５）のように表される。

これまで、多剛体系力学モデルの剛体ｉに対して作用する、各要求項目（２）〜（４）に対する力学拘束を表現する力、トルクについて説明してきた。これら剛体ｉに作用する外力を加算した全外力Ｆ_iは、下式（１６）のように表される。

剛体ｉの運動記述する６個の独立な方程式は、下式（１７）、（１８）にそれぞれ示すように、当業界において周知のニュートンの運動方程式及びオイラーの運動方程式として与えられる。

但し、上式（１７）中のＰ_i、ｍ_iはそれぞれ剛体ｉの運動量及び質量を表す。また、上式（１８）中のＬ_i、Ｉ_i、ω_iは剛体ｉの各運動量、ワールド座標系における慣性テンソル、角速度ベクトルをそれぞれ表す。

上式（１６）に示した、剛体ｉに作用する全外力Ｆ_i＝［ｆ_i，τ_i］^Tを、上式（１７）、（１８）にそれぞれ示したニュートンの方程式及びオイラーの方程式の外力として付加し、多剛体系動力学シミュレーションを実行する。

すなわち、Ｎ個の剛体からなる連成運動をＮ個の剛体運動方程式として表現し、各シミュレーション・ステップにおいて数値積分を実行することで、各時刻における各剛体ｉすなわちノードの位置姿勢を得ることができる。この結果、制御対象の現在の位置姿勢から目標とする位置姿勢までの滑らかな軌道を高速に生成するという目的を達成するための、すべての要求項目（１）〜（４）を満足する軌道計画が実現される。

図８には、これまで説明してきた、制御対象の軌道計画を実現するシステムの機能的構成を模式的に示している。図示のシステムは、ＲＲＴ実行部８１と、干渉検出部８２と、外力算出部８３と、多剛体系動力学シミュレーション実行部８４で構成される。

ＲＲＴ実行部８１は、軌道計画指令を受けると、３次元空間上の障害物情報と制御対象物体の現在位置姿勢及び目標位置姿勢とから、ＲＲＴアルゴリズムを用いて、最低限の目的である現在位置姿勢から目標位置姿勢までの軌道を探索又は生成する。ＲＲＴ実行部８１で得られる初期位置姿勢軌道は、制御対象の現在位置姿勢と目標位置姿勢の間を繋ぐ複数のノードからなる。

干渉検出部８２は、ＲＲＴ実行部８１で制御対象の初期位置姿勢軌道を探索又は生成する際における、干渉検出演算を担い、軌道探索又は生成時の制御対象と障害物との浸透回避を考慮する。したがって、ＲＲＴ実行部８１から出力される制御対象の初期位置軌道は、多くの迂回を含むが、障害物との干渉のないものとなる。障害物の回避の際の物体間浸透の問題に関してＧＪＫアルゴリズム（前述）を適用することができる。

外力算出部８３は、ＲＲＴ実行部８１で探索又は生成された制御対象の初期位置姿勢軌道上の各ノードに配置した制御対象に相当する剛体に対して、障害物との干渉を回避しながら位置姿勢軌道を修正するための要求項目を表現する外力を算出する。本実施形態では、外力算出部８３は、初期位置姿勢軌道を迂回の無い平滑な位置姿勢軌道にするための力学拘束である隣接剛体間力を算出する隣接剛体間力算出部８３Ａと、制御対象とその動作空間上に存在する障害物との干渉を回避するための力学拘束である障害物回避斥力を算出する障害物回避斥力算出部８３Ｂと、できる限り理想的な位置姿勢軌道を近づけるための力学拘束である最適軌道力を算出する最適軌道力算出部８３Ｃより構成される。

多剛体系動力学シミュレーション実行部８４は、外力算出部８３で算出された、軌道上の各剛体に作用する各外力（すなわち、隣接剛体間力、障害物回避斥力、及び最適軌道力）を、各剛体運動の外力拘束として作用させて、シミュレーション・サイクル毎に多剛体系の動力学演算を実行する。

多剛体系動力学シミュレーション実行部８４による多剛体系動力学演算は、設定した状態（例えば、剛体の静定）に収束したとき終了し、そのときの多剛体系が形成するノード列が、目的とする平滑化された位置姿勢軌道として出力される。

図９には、図８に示したシステムにおいて制御対象の軌道計画を実現するための処理手順をフローチャートの形式で示している。

まず、軌道探索・生成処理として、ＲＲＴ実行部８１が、軌道計画指令を受け、３次元空間上の障害物情報と制御対象の現在位置姿勢及び目標位置姿勢とから、最低限の目的である現在位置姿勢から目標位置姿勢までの軌道を探索又は生成する（ステップＳ１）。生成した初期位置姿勢は迂回を含む可能性がある。

次いで、外力算出処理部８３では、シミュレーション・サイクル毎に、軌道平滑化、障害物干渉回避、理想軌道への吸引を実現するための力学拘束を表現する各外力の算出処理を行なう（ステップＳ２）。

次いで、多剛体系動力学シミュレーション部８４では、シミュレーション・サイクル毎に多剛体系動力学演算を実行し、多剛体系の状態（すなわち、制御対象の軌道上の各ノードの位置姿勢）を更新する（ステップＳ３）。

制御対象の位置姿勢軌道があらかじめ設定した静定状態に収束するまで、ステップＳ２に戻り、ステップＳ２〜Ｓ４の処理を繰り返し実行する。

そして、多剛体系が設定した状態に収束すると、多剛体系動力学シミュレーション部８４による動力学演算を終了するとともに、軌道生成処理全体を終了する。この結果、障害物との干渉がなく、平滑化され、理想軌道に近い、制御対象の位置姿勢軌道が生成される。

図１０には、図３に示したようなＲＲＴアルゴリズムにより得られる初期軌道に対して、上記の多剛体系動力学シミュレーションを適用して修正した結果を示している。得られた制御対象の軌道は、位置及び姿勢ともに平滑で、且つ、障害物との干渉が回避されていることを確認できる。

図１１には、上記の多剛体系動力学シミュレーションを適用して修正した軌道の他の例を示している。そして、図１２には、図１１に示した軌道における剛体位置を示している。図１２から、ｘ、ｙ、ｚすべての軌道は平滑化された無駄のない軌道に修正されていることが確認できる。

また、図１３には、上記の多剛体系動力学シミュレーションを適用して修正した軌道のさらに他の例を示している。そして、図１４には、図１３に示した軌道における剛体姿勢のピッチ角を示している。図１４から、剛体姿勢軌道においても、平滑化された無駄のない軌道に修正されていることが期待できる。

本実施形態では、要求項目（２）「滑らかな位置姿勢軌道を獲得すること」の力学拘束表現として、隣接する剛体間にばね力を作用させている（前述）。ここで、ばね定数ｋ_sを、系全体で均一すなわち定数としても、現在状態である剛体０若しくは目標状態である剛体Ｎとの距離に応じて変化する変数とすることもできる。

図１５には、ばね定数を定数とした場合の多剛体系力学モデルの軌道生成結果のスナップショットを示している。また、図１６には、ばね定数を定数ではなく剛体ｉと現在状態である剛体０若しくは目標状態である剛体Ｎとの距離に応じて変化する変数とした場合の軌道生成結果のスナップショットを示している。また、図１７には、ばね定数を定数並びに変数としてそれぞれ生成した軌道上における各剛体（ノード）の剛体姿勢のグラフを示している。図１７中、黒丸でプロットした曲線がばね定数を定数とした場合（すなわち、図１５に対応）の剛体姿勢の変化を示し、白抜きまるでプロットした曲線がばね定数を距離に応じた変数とした場合（すなわち、図１６に対応）の剛体姿勢の変化を示している。図１７から、ばね定数を距離に応じた変数とみた方が、より滑らか且つ急激な変化の無い軌道になっていることが確認することができる。

また、本実施形態では、要求項目（３）「位置姿勢軌道において、物体間の浸透を回避すること」の力学拘束表現として、制御対象と障害物との最短距離に応じた斥力を各剛体に作用させている（前述）。図１８には、図３に示した状態を初期状態としたとき、図中向かって奥側の障害を動かした場合の多剛体系動力学シミュレーションの結果を示している。同図より、障害物の移動に合わせて、生成した軌道が修正されていくのを確認することができる。

本実施形態では、要求項目（２）〜（４）をそれぞれ剛体に作用する力学拘束として表現し、これら剛体に作用する外力を加算した全外力を運動方程式の外力として付加し、多剛体系動力学シミュレーションを実行する。

図１９、図２０には、多剛体系動力学シミュレーション実行し、剛体群が静定している状態を示している。また、図２１、図２２には、シミュレーション実行後に、目標位置姿勢に修正値を与え、シミュレーションが収束した状態でのスナップショットを示している。但し、図１９及び図２１は制御対象物体を表示したものであり、図２０及び図２２は制御対象物体を非表示にし、剛体群の重心位置をノードで表示したものとなっている。図１９乃至図２２より、リアルタイムに目標位置姿勢を修正した場合も軌道生成は良好に行なわれることを確認することができる。

以上より、本発明に係る軌道計画方法によれば、前述した目的を達成するために必要とされる要求項目を全て満足し，目的とした軌道を良好に生成できることがわかる．また，本手法では，障害物干渉回避，目標位置姿勢修正といった制御を，他のアルゴリズムを別に用意することなく，統一的に扱うことができ，かつリアルタイムに実行することも可能であり，この点においても本手法は有用である

上述した制御対象の軌道を生成する処理（図８に示したシステム）は、例えばコンピューターなどの情報処理装置上で所定のアプリケーションを実行するという形態で実現される。図２３には、本発明を実現可能なコンピューターの構成例を示している。

ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１は、オペレーティング・システム（ＯＳ）が提供するプログラム実行環境下で、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）２やハード・ディスク・ドライブ（ＨＤＤ）１１に格納されているプログラムを実行する。例えば、多リンク構造体の動作制御若しくは運動データを生成するための処理をＣＰＵ１が所定のプログラムを実行するという形態で実現することもできる。

ＲＯＭ２は、ＰＯＳＴ（Ｐｏｗｅｒ Ｏｎ Ｓｅｌｆ Ｔｅｓｔ）やＢＩＯＳ（Ｂａｓｉｃ Ｉｎｐｕｔ Ｏｕｔｐｕｔ Ｓｙｓｔｅｍ）などのプログラム・コードを恒久的に格納する。ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）３は、ＲＯＭ２やＨＤＤ１１に格納されているプログラムをＣＰＵ１が実行する際にロードしたり、実行中のプログラムの作業データを一時的に保持したりするために使用される。これらはＣＰＵ１のローカル・ピンに直結されたローカル・バス４により相互に接続されている。

ローカル・バス４は、ブリッジ５を介して、ＰＣＩ（Ｐｅｒｉｐｈｅｒａｌ Ｃｏｍｐｏｎｅｎｔ Ｉｎｔｅｒｃｏｎｎｅｃｔ）バスなどの入出力バス６に接続されている。

キーボード８と、マウスなどのポインティング・デバイス９は、ユーザにより操作される入力デバイスである。ディスプレイ１０は、ＬＣＤ（Ｌｉｑｕｉｄ Ｃｒｙｓｔａｌ Ｄｉｓｐｌａｙ）又はＣＲＴ（Ｃａｔｈｏｄｅ Ｒａｙ Ｔｕｂｅ）などから成り、各種情報をテキストやイメージで表示する。

ＨＤＤ１１は、記録メディアとしてのハード・ディスクを内蔵したドライブ・ユニットであり、ハード・ディスクを駆動する。ハード・ディスクには、オペレーティング・システムや各種アプリケーションなどＣＰＵ１が実行するプログラムをインストールしたり、データ・ファイルなどを保存したりするために使用される。例えば、多リンク構造体の動作制御若しくは運動データを生成する処理を実現するアプリケーション・プログラムをＨＤＤ１１にインストールしたり、生成された運動データのファイルをＨＤＤ１１に蓄積したりする。

通信部１２は、例えば有線通信若しくは無線通信のＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）インターフェースである。例えば、ロボットや多関節アームなどの現在位置姿勢から目標位置姿勢までの軌道を生成する処理を実現するアプリケーション・プログラムを、ネットワーク経由でインストールすることができる。また、軌道データをネットワーク経由でダウンロードしたり、あるいは、生成した軌道データをネットワーク経由でアップロードしたりすることもできる。

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

本発明は、ロボットの移動制御や多関節アームのエンド・エフェクタの軌道制御などに適用することができる。

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

１…ＣＰＵ
２…ＲＯＭ
３…ＲＡＭ
４…ローカル・バス
５…ブリッジ
６…入出力バス
７…入出力インターフェース
８…キーボード
９…ポインティング・デバイス（マウス）
１０…ディスプレイ
１１…ＨＤＤ
１２…通信部
８１…ＲＲＴ実行部
８２…干渉検出部
８３…外力算出部
８３Ａ…隣接剛体間力算出部
８３Ｂ…障害物回避斥力算出部
８３Ｃ…最適軌道力算出部
８４…多剛体系動力学シミュレーション実行部

Claims (6)

1. 障害物が存在する動作空間における制御対象の現在位置姿勢から目標位置姿勢に至るまでの位置姿勢軌道を計画する軌道計画装置であって、
   物体間の干渉を検出する干渉検出部と、
   前記制御対象の前記現在位置姿勢と前記目標位置姿勢の間を繋ぐ複数のノードからなり、前記制御対象に相当する剛体を各ノードに配置したときに前記干渉検出部が各ノードに配置した剛体と前記障害物との干渉を検出しなくなる初期の位置姿勢軌道を獲得する初期位置姿勢軌道獲得部と、
   前記剛体のローカル座標系に固定された仮想剛体面を定義し、隣接する仮想剛体面の対応する頂点同士を所定のばね定数を持つばね及び所定のダンピング定数を持つダンパーで並列接続した力学モデルを用い、仮想剛体面の各頂点が隣接する仮想剛体面の対応する頂点との間で働くばね力及びダンパー力の合計からなる剛体間力と、前記ばね力及びダンパー力によって各仮想剛体の頂点に作用するモーメントの合計からなる剛体に作用するワールド座標系原点回りのモーメントを、前記位置姿勢軌道を平滑にするための要求項目を表現する外力として算出し、前記位置姿勢軌道上の各ノードに配置された前記剛体と前記障害物との最短距離を算出し、該最短距離に応じて前記剛体の各々に作用する斥力を、前記位置姿勢軌道が前記障害物との干渉を回避するための要求項目を表現する外力として算出し、前記剛体のローカル座標系に固定された前記剛体の前方進行方向と隣接する前記剛体間における位置姿勢軌道の方向とのなす角に応じたモーメントを、理想の位置姿勢軌道に近づけるための要求項目を表現する外力として算出する外力算出部と、
   前記外力算出部で算出された外力がそれぞれ作用する複数の前記剛体からなる連成運動を記述した複数の剛体運動方程式に対して多剛体系動力学シミュレーションを実行し、所定の状態に収束したときの前記剛体の各々の位置及び姿勢に基づいて、前記位置姿勢軌道上の各ノードに相当する時刻における前記制御対象の位置及び姿勢を得る多剛体系動力学シミュレーション実行部と、
   を具備すること特徴とする軌道計画装置。
2. 前記初期軌道獲得部は、前記複数のノードで複数の経路に分岐して前記動作空間上に展開された探索木に基づいて、前記制御対象の現在位置と目標位置の間の軌道を獲得するとともに、各ノードに配置する剛体に対して初期の姿勢を付与する、
   ことを特徴とする請求項１に記載の軌道計画装置。
3. 前記干渉検出部は、ＧＪＫ（Ｇｉｌｂｅｒｔ−Ｊｏｈｎｓｏｎ−Ｋｅｅｒｔｈｉ ｄｉｓｔａｎｃｅ ａｌｇｏｒｉｔｈｍ）アルゴリズムを適用して物体間の最短距離を算出し、該最短距離に基づいて物体間の干渉を検出する、
   ことを特徴とする請求項１に記載の軌道計画装置。
4. 前記外力算出部は、前記剛体のローカル座標系に固定された前記剛体の前方進行方向と隣接する前記剛体間における位置姿勢軌道の方向とのなす角に応じたモーメントを、理想の位置姿勢軌道に近づけるための要求項目を表現する外力として算出する、
   ことを特徴とする請求項１に記載の軌道計画装置。
5. 障害物が存在する動作空間における制御対象の現在位置姿勢から目標位置姿勢に至るまでの位置姿勢軌道を計画する軌道計画方法であって、
   前記制御対象の前記現在位置姿勢と前記目標位置姿勢の間を繋ぐ複数のノードからなり、前記制御対象に相当する剛体を各ノードに配置したときに該剛体と前記障害物との干渉を検出しなくなる初期位置姿勢軌道獲得ステップと、
   前記剛体のローカル座標系に固定された仮想剛体面を定義し、隣接する仮想剛体面の対応する頂点同士を所定のばね定数を持つばね及び所定のダンピング定数を持つダンパーで並列接続した力学モデルを用い、仮想剛体面の各頂点が隣接する仮想剛体面の対応する頂点との間で働くばね力及びダンパー力の合計からなる剛体間力と、前記ばね力及びダンパー力によって各仮想剛体の頂点に作用するモーメントの合計からなる剛体に作用するワールド座標系原点回りのモーメントを、前記位置姿勢軌道を平滑にするための要求項目を表現する外力として算出し、前記位置姿勢軌道上の各ノードに配置された前記剛体と前記障害物との最短距離を算出し、該最短距離に応じて前記剛体の各々に作用する斥力を、前記位置姿勢軌道が前記障害物との干渉を回避するための要求項目を表現する外力として算出し、前記剛体のローカル座標系に固定された前記剛体の前方進行方向と隣接する前記剛体間における位置姿勢軌道の方向とのなす角に応じたモーメントを、理想の位置姿勢軌道に近づけるための要求項目を表現する外力として算出する外力算出ステップと、
   前記外力算出ステップで算出された外力がそれぞれ作用する複数の前記剛体からなる連成運動を記述した複数の剛体運動方程式に対して多剛体系動力学シミュレーションを実行し、所定の状態に収束したときの前記剛体の各々の位置及び姿勢に基づいて、前記位置姿勢軌道上の各ノードに相当する時刻における前記制御対象の位置及び姿勢を得る多剛体系動力学シミュレーション実行ステップと、
   を有すること特徴とする軌道計画方法。
6. 障害物が存在する動作空間における制御対象の現在位置姿勢から目標位置姿勢に至るまでの位置姿勢軌道を計画するための処理をコンピューター上で実行するようにコンピューター可読形式で記述されたコンピューター・プログラムであって、前記コンピューターを、
   物体間の干渉を検出する干渉検出部、
   前記制御対象の前記現在位置姿勢と前記目標位置姿勢の間を繋ぐ複数のノードからなり、前記制御対象に相当する剛体を各ノードに配置したときに前記干渉検出部が各ノードに配置した剛体と前記障害物との干渉を検出しなくなる初期の位置姿勢軌道を獲得する初期位置姿勢軌道獲得部、
   前記剛体のローカル座標系に固定された仮想剛体面を定義し、隣接する仮想剛体面の対応する頂点同士を所定のばね定数を持つばね及び所定のダンピング定数を持つダンパーで並列接続した力学モデルを用い、仮想剛体面の各頂点が隣接する仮想剛体面の対応する頂点との間で働くばね力及びダンパー力の合計からなる剛体間力と、前記ばね力及びダンパー力によって各仮想剛体の頂点に作用するモーメントの合計からなる剛体に作用するワールド座標系原点回りのモーメントを、前記位置姿勢軌道を平滑にするための要求項目を表現する外力として算出し、前記位置姿勢軌道上の各ノードに配置された前記剛体と前記障害物との最短距離を算出し、該最短距離に応じて前記剛体の各々に作用する斥力を、前記位置姿勢軌道が前記障害物との干渉を回避するための要求項目を表現する外力として算出し、前記剛体のローカル座標系に固定された前記剛体の前方進行方向と隣接する前記剛体間における位置姿勢軌道の方向とのなす角に応じたモーメントを、理想の位置姿勢軌道に近づけるための要求項目を表現する外力として算出する外力算出部、
   前記外力算出部で算出された外力がそれぞれ作用する複数の前記剛体からなる連成運動を記述した複数の剛体運動方程式に対して多剛体系動力学シミュレーションを実行し、所定の状態に収束したときの前記剛体の各々の位置及び姿勢に基づいて、前記位置姿勢軌道上の各ノードに相当する時刻における前記制御対象の位置及び姿勢を得る多剛体系動力学シミュレーション実行部、
   として機能させるためのコンピューター・プログラム。