(57)【要約】【課題】ロボットや多関節アームなどの現在の位置姿勢から目標とする位置姿勢までの滑らかな軌道を高速に生成する。【解決手段】ＲＲＴにより探索した軌道を制御対象の初期軌道とし、この初期軌道上の各ノードの位置を剛体の代表点の初期位置とするとともに、ノード毎の剛体の初期姿勢を適当に割り当て、それらを初期位置姿勢とした多剛体系力学モデルを構築する。この多剛体系に加わる外力モデルとして、各要求項目を表現した力学拘束を多剛体系の各剛体に付加して、多剛体系動力学シミュレーションを実行する。【選択図】  図８
2010155328.tif 000002
【特許請求の範囲】
【請求項１】
動作空間における制御対象の現在位置姿勢から目標位置姿勢に至るまでの位置姿勢軌道を計画する軌道計画装置であって、
前記制御対象の前記現在位置姿勢と前記目標位置姿勢の間を繋ぐ複数のノードからなる初期の位置姿勢軌道を獲得し、前記位置姿勢軌道上の各ノードに前記制御対象に相当する剛体をそれぞれ配置する初期位置姿勢軌道獲得部と、
前記位置姿勢軌道上の前記制御対象と前記動作空間に存在する障害物との干渉を検出する干渉検出部と、
前記剛体の各々に対して、前記位置姿勢軌道が平滑で且つ前記障害物との干渉を回避するための要求項目を表現する外力を算出する外力算出部と、
前記外力算出部で算出された外力がそれぞれ作用する複数の前記剛体からなる連成運動を記述した複数の剛体運動方程式に対して多剛体系動力学シミュレーションを実行し、所定の状態に収束したときの前記剛体の各々の位置及び姿勢に基づいて、前記位置姿勢軌道上の各ノードに相当する時刻における前記制御対象の位置及び姿勢を得る多剛体系動力学シミュレーション実行部と、
を具備すること特徴とする軌道計画装置。
【請求項２】
前記初期軌道獲得部は、前記複数のノードで複数の経路に分岐して前記動作空間上に展開された探索木に基づいて、前記制御対象の現在位置と目標位置の間の軌道を獲得するとともに、各ノードに配置する剛体に対して初期の姿勢を付与する、
ことを特徴とする請求項１に記載の軌道計画装置。
【請求項３】
前記干渉検出部は、ＧＪＫ（Ｇｉｌｂｅｒｔ−Ｊｏｈｎｓｏｎ−Ｋｅｅｒｔｈｉ ｄｉｓｔａｎｃｅ ａｌｇｏｒｉｔｈｍ）アルゴリズムを適用して、前記剛体の各々と前記障害物との最短距離を検出する、
ことを特徴とする請求項１に記載の軌道計画装置。
【請求項４】
前記外力算出部は、前記剛体のローカル座標系に固定された仮想剛体面を定義し、隣接する仮想剛体面の対応する頂点同士を所定のばね定数を持つばね及び所定のダンピング定数を持つダンパーで並列接続した力学モデルを用い、仮想剛体面の各頂点が隣接する仮想剛体面の対応する頂点との間で働くばね力及びダンパー力の合計からなる剛体間力と、前記ばね力及びダンパー力によって各仮想剛体の頂点に作用するモーメントの合計からなる剛体に作用するワールド座標系原点回りのモーメントを、前記位置姿勢軌道を平滑にするための要求項目を表現する外力として算出する、
ことを特徴とする請求項１に記載の軌道計画装置。
【請求項５】
前記干渉検出部は、前記位置姿勢軌道上の各ノードに配置された前記剛体と前記障害物との最短距離を検出し、
前記外力算出部は、前記最短距離に応じて前記剛体の各々に作用する斥力を、前記位置姿勢軌道が前記障害物との干渉を回避するための要求項目を表現する外力として算出する、
ことを特徴とする請求項１に記載の軌道計画装置。
【請求項６】
前記外力算出部は、前記剛体のローカル座標系に固定された前記剛体の前方進行方向と隣接する前記剛体間における位置姿勢軌道の方向とのなす角に応じたモーメントを、理想の位置姿勢軌道に近づけるための要求項目を表現する外力として算出する、
ことを特徴とする請求項１に記載の軌道計画装置。
【請求項７】
動作空間における制御対象の現在位置姿勢から目標位置姿勢に至るまでの位置姿勢軌道を計画する軌道計画方法であって、
前記制御対象の前記現在位置姿勢と前記目標位置姿勢の間を繋ぐ複数のノードからなる初期の位置姿勢軌道を獲得し、前記位置姿勢軌道上の各ノードに前記制御対象に相当する剛体をそれぞれ配置する初期位置姿勢軌道獲得ステップと、
前記位置姿勢軌道上の前記制御対象と前記動作空間に存在する障害物との干渉を検出する干渉検出ステップと、
前記剛体の各々に対して、前記位置姿勢軌道が平滑で且つ前記障害物との干渉を回避するための要求項目を表現する外力を算出する外力算出ステップと、
前記外力算出部で算出された外力がそれぞれ作用する複数の前記剛体からなる連成運動を記述した複数の剛体運動方程式に対して多剛体系動力学シミュレーションを実行し、所定の状態に収束したときの前記剛体の各々の位置及び姿勢に基づいて、前記位置姿勢軌道上の各ノードに相当する時刻における前記制御対象の位置及び姿勢を得る多剛体系動力学シミュレーション実行ステップと、
を有すること特徴とする軌道計画方法。
【請求項８】
動作空間における制御対象の現在位置姿勢から目標位置姿勢に至るまでの位置姿勢軌道を計画するための処理をコンピューター上で実行するようにコンピューター可読形式で記述されたコンピューター・プログラムであって、前記コンピューターを、
前記制御対象の前記現在位置姿勢と前記目標位置姿勢の間を繋ぐ複数のノードからなる初期の位置姿勢軌道を獲得し、前記位置姿勢軌道上の各ノードに前記制御対象に相当する剛体をそれぞれ配置する初期位置姿勢軌道獲得部、
前記位置姿勢軌道上の前記制御対象と前記動作空間に存在する障害物との干渉を検出する干渉検出部、
前記剛体の各々に対して、前記位置姿勢軌道が平滑で且つ前記障害物との干渉を回避するための要求項目を表現する外力を算出する外力算出部、
前記外力算出部で算出された外力がそれぞれ作用する複数の前記剛体からなる連成運動を記述した複数の剛体運動方程式に対して多剛体系動力学シミュレーションを実行し、所定の状態に収束したときの前記剛体の各々の位置及び姿勢に基づいて、前記位置姿勢軌道上の各ノードに相当する時刻における前記制御対象の位置及び姿勢を得る多剛体系動力学シミュレーション実行部、
として機能させるためのコンピューター・プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの移動制御や多関節アームの手先などの制御対象の、動作空間上における現在位置姿勢から目標位置姿勢までの移動経路若しくは軌道を計画する軌道計画装置及び軌道計画方法、並びにコンピューター・プログラムに係り、特に、障害物を回避するような制御対象の軌道を移動制御や軌道制御として用い易くなるように滑らかな軌道として修正する軌道計画装置及び軌道計画方法、並びにコンピューター・プログラムに関する。
【背景技術】
【０００２】
例えば、ロボットのような移動体装置や、多関節アームのエンド・エフェクタとなる手先などを自律的に動作制御させる場合、動作空間上のある位置姿勢（現在位置姿勢：Ｓｔａｒｔ）からある位置姿勢（目標位置姿勢：Ｇｏａｌ）までの移動経路若しくは軌道（Ｐａｔｈ）を計画しなければならない。以下では、「経路」と「軌道」を「軌道」と総称する。また、特に断りがない限り、軌道は位置姿勢の軌道を意味するものとする。すなわち、３次元空間における軌道とは、物体の空間姿勢を記述するための６つの自由度の軌道を指す。
【０００３】
本願は、ロボットや多関節アームなどの現在の位置姿勢から目標とする位置姿勢までの滑らかな軌道を高速に生成することを主な目的とする。本発明者らは、かかる目的を実現するためには、以下の４つの項目を満たす必要があると思料する。
【０００４】
（１）最適解でなくとも、目標とする位置姿勢軌道を達成するための、何らかの初期位置姿勢軌道を高速に獲得すること。
（２）滑らかな位置姿勢軌道を獲得すること。
（３）位置姿勢軌道において、物体間の浸透（衝突若しくは干渉）を回避すること。
（４）できる限り理想の（設計者の意図を反映させた）位置姿勢軌道に近づけること。
【０００５】
無限に存在する軌道の中から最適解を求めることは難しい。これまで、ポテンシャル法（例えば、非特許文献１を参照のこと）、確率的ロードマップ法（例えば、非特許文献１を参照のこと）、Ｒａｐｉｄｌｙ−ｅｘｐｌｏｒｉｎｇ  Ｒａｎｄｏｍ  Ｔｒｅｅ（ＲＲＴ）（例えば、非特許文献２を参照のこと）といった多くの軌道計画方法について提案がなされてきた。
【０００６】
ここで、ポテンシャル法とは、動作空間に目的姿勢に制御対象を近づける「引力ポテンシャル」、並びに、障害物から制御対象を遠ざける「斥力ポテンシャル」というポテンシャル場を定義し、制御対象の現在姿勢におけるポテンシャル場の勾配を算出し、ポテンシャルの減少する方向へ微少に移動させる手続きを繰り返すことによって軌道を計画する方法である。
【０００７】
例えば、移動体、障害物、及び目標物の位置及び形状の情報に基づいて生成された移動空間内のマップを用いて、移動体、障害物、及び目標物の相対位置関係に基づく引力ポテンシャル及び斥力ポテンシャルを計算し、これらの和である合成ポテンシャルを生成し、この合成ポテンシャルに基づいてマップ内の経路探索を行なって該経路探索の収束位置がローカルミニマムかどうかを判別し、収束位置がローカルミニマムの場合には該収束位置のポテンシャルを所定値だけ増加させた仮想ポテンシャルを生成するとともに、収束位置が目標位置の場合には経路探索の結果に基づいて移動体の移動経路を生成する経路生成装置について提案がなされている（例えば、特許文献１を参照のこと）。
【０００８】
また、確率的ロードマップ法は、配置空間内から幾つかの姿勢をランダムに選び、近接する姿勢に移動可能であれば枝でつないでいき、ロードマップが連結でなければ、障害物の多い領域にある姿勢からランダム・ウォークを行ない、ロードマップを拡張し、数理計画上の手法であるダイクストラ法を適用してロードマップにおける最短経路を求める方法である。
【０００９】
例えば、ダイクストラ法によって、ネットワークにおける最小コストの入ノードから出ノードまでの経路を探索する最小コスト経路探索装置について提案がなされている（例えば、特許文献２を参照のこと）。
【００１０】
また、ＲＲＴは、動作空間を好適に網羅する木のデータ構造、並びに当該木を生成する基本となるアルゴリズムである。ＲＲＴアルゴリズムでは、まず動作空間上に初期位置を設定した後、同動作空間内にランダムなサンプル位置を配置するとともに初期位置から当該サンプル位置に向かう最近傍位置を探索し、初期位置と最近傍位置とを結んで木に追加する。その後、新たに追加された最近傍位置についてサンプル位置の配置とさらに最近傍位置の探索と木への追加を繰り返すことで、動作空間を網羅する木を生成することができる。現在、ＲＲＴアルゴリズムは、実時間での経路計画や軌道生成などに用いられており（例えば、非特許文献３を参照のこと）、多くの研究事例がある。
【００１１】
ポテンシャル法は、非常に直感的であるが、目的位置姿勢以外にもポテンシャルが極小となる位置姿勢が多数表れる可能性があることから、安全な動作経路を効率よく求めることは困難である。確率的ロードマップ法は、多くの障害物に囲まれた狭い空間を通る動作計画が困難、すなわち多数の位置姿勢をサンプルしなくてはならない、といった欠点を持つ。ＲＲＴアルゴリズムは、多次元ｃｏｎｆｉｇｕｒａｔｉｏｎ  ｓｐａｃｅでの比較的高速な軌道計画（Ｐａｔｈ  Ｐｌａｎｎｉｎｇ）が可能なことで知られているが、ランダム探索を用いることから、探索した軌道は多くの迂回を含むという欠点を持つ。
【００１２】
このように、従来の軌道計画手法はそれぞれ一長一短があることが分かる。また、いずれかの手法により生成した軌道を、平滑化したり障害物を回避したりするための軌道修正を行なう必要がある。軌道を平滑化する手法として、躍度最小や（例えば、非特許文献４を参照のこと）、トルク最小、多項式（Ｓｐｌｉｎｅ）を用いた手法が考案されている。しかしながら、いずれの手法も、上記に掲げた要求項目（２）平滑な位置姿勢軌道を獲得すること、（３）物体間の浸透を回避すること、（４）最適な軌道に近づけること、を同時に満たすことはできない。言い換えれば、ロボットの移動制御や多関節アームの手先の軌道計画などのための最終的な制御に用いるものとして満足な軌道を得ることはできない。
【００１３】
例えば、Ｂスプラインを用いたヒューマノイド動作計画の軌道を平滑化する方法について提案がなされている（例えば、非特許文献５を参照のこと）。この方法によれば、空間上の位置及び３自由度の姿勢を取り扱って、軌道修正を行なうことができるが、計算量が過大になるという問題がある。
【先行技術文献】
【特許文献】
【００１４】
【特許文献１】特開２００６−３５０７７６号公報
【特許文献２】特開２００２−１３３３５１号公報
【非特許文献】
【００１５】
【非特許文献１】太田順外著「知能ロボット入門 −動作計画問題の解法−」（コロナ社、２００１）
【非特許文献２】Ｓｔｅｖｅｎ  Ｍ．ＬａＶａｌｌｅ，“Ｒａｐｉｄｌｙ−Ｅｘｐｌｏｒｉｎｇ  Ｒａｎｄｏｍ  Ｔｒｅｅｓ：Ａ  Ｎｅｗ  Ｔｏｏｌ  ｆｏｒ  Ｐａｔｈ  Ｐｌａｎｎｉｎｇ”（ＴＲ．９８−１１，Ｃｏｍｐｕｔｅｒ  Ｓｃｉｅｎｃｅ  Ｄｐｔ．，Ｉｏｗａ  Ｓｔａｔｅ  Ｕｎｉｖｅｒｓｉｔｙ．Ｏｃｔ．１９９８）
【非特許文献３】Ｊａｍｅｓ  Ｊ．Ｋｕｆｆｎｅｒ，Ｊｒ．，“Ｄｙｎａｍｉｃａｌｌｙ−ｓｔａｂｌｅ  Ｍｏｔｉｏｎ  Ｐｌａｎｎｉｎｇ  ｆｏｒ  Ｈｕｍａｎｏｉｄ  Ｒｏｂｏｔｓ”（Ａｕｔｏｎｏｍｏｕｓ  Ｒｏｂｏｔｓ  ｖｏｌ．１２，Ｎｏ．１，ｐｐ．１０５−１１８，２００２）
【非特許文献４】Ｔ．Ｆｌａｓｈ  ａｎｄ  Ｎ．Ｈｏｇａｎ，“Ｔｈｅ  Ｃｏｏｒｄｉｎａｔｉｏｎ  ｏｆ  Ａｒｍ  Ｍｏｖｅｍｅｎｔｓ：Ａｎ  Ｅｘｐｅｒｉｍｅｎｔａｌｌｙ  Ｃｏｎｆｉｒｍｅｄ  Ｍａｔｈｅｍａｔｉｃａｌ  Ｍｏｄｅｌ”（Ｔｈｅ  Ｊｏｕｒｎａｌ  ｏｆ  Ｎｅｕｒｏｓｃｉｅｎｃｅ，Ｖｏｌ．５，Ｎｏ．７，ｐｐ．１６８８−１７０３，１９８５）
【非特許文献５】原田、森澤、三浦、藤原、梶田共著「Ｂスプラインを用いたヒューマノイド動作計画のための軌道平滑化」（Ｐｒｏｃｅｅｄｉｎｇｓ  ｏｆ  ｔｈｅ  ２００８  ＪＳＭＥ  Ｃｏｎｆｅｒｅｎｃｅ  ｏｎ  Ｒｏｂｏｔｉｃｓ  ａｎｄ  Ｍｅｃｈａｔｒｏｎｉｃｓ，２Ｐ１−Ｆ１６，２００８）
【発明の概要】
【発明が解決しようとする課題】
【００１６】
本発明の目的は、ロボットの移動制御や多関節アームの手先などの制御対象の、動作空間上における現在位置姿勢から目標位置姿勢までの移動経路若しくは軌道を好適に計画することができる、優れた軌道計画装置及び軌道計画方法、並びにコンピューター・プログラムを提供することにある。
【００１７】
本発明のさらなる目的は、障害物を回避するような制御対象の軌道を発見するとともに、制御対象の移動制御や軌道制御に用い易くなるように滑らかな軌道として修正することができる、優れた軌道計画装置及び軌道計画方法、並びにコンピューター・プログラムを提供することにある。
【００１８】
本発明のさらなる目的は、制御対象の現在の位置姿勢から目標とする位置姿勢までの滑らかな軌道を高速に生成することができる、優れた軌道計画装置及び軌道計画方法、並びにコンピューター・プログラムを提供することにある。
【課題を解決するための手段】
【００１９】
本願は、上記課題を参酌してなされたものであり、請求項１に記載の発明は、動作空間における制御対象の現在位置姿勢から目標位置姿勢に至るまでの位置姿勢軌道を計画する軌道計画装置であって、
前記制御対象の前記現在位置姿勢と前記目標位置姿勢の間を繋ぐ複数のノードからなる初期の位置姿勢軌道を獲得し、前記位置姿勢軌道上の各ノードに前記制御対象に相当する剛体をそれぞれ配置する初期位置姿勢軌道獲得部と、
前記位置姿勢軌道上の前記制御対象と前記動作空間に存在する障害物との干渉を検出する干渉検出部と、
前記剛体の各々に対して、前記位置姿勢軌道が平滑で且つ前記障害物との干渉を回避するための要求項目を表現する外力を算出する外力算出部と、
前記外力算出部で算出された外力がそれぞれ作用する複数の前記剛体からなる連成運動を記述した複数の剛体運動方程式に対して多剛体系動力学シミュレーションを実行し、所定の状態に収束したときの前記剛体の各々の位置及び姿勢に基づいて、前記位置姿勢軌道上の各ノードに相当する時刻における前記制御対象の位置及び姿勢を得る多剛体系動力学シミュレーション実行部と、
を具備すること特徴とする軌道計画装置である。
【００２０】
また、本願の請求項２に記載の発明によれば、初期位置姿勢軌道獲得部は、前記複数のノードで複数の経路に分岐して前記動作空間上に展開された探索木に基づいて、前記制御対象の現在位置と目標位置の間の軌道を獲得するとともに、各ノードに配置する剛体に対して初期の姿勢を付与するように構成されている。
【００２１】
また、本願の請求項３に記載の発明によれば、干渉検出部は、ＧＪＫアルゴリズム（後述）を適用して、前記剛体の各々と前記障害物との最短距離を検出するように構成されている。
【００２２】
また、本願の請求項４に記載の発明によれば、外力算出部は、前記剛体のローカル座標系に固定された仮想剛体面を定義し、隣接する仮想剛体面の対応する頂点同士を所定のばね定数を持つばね及び所定のダンピング定数を持つダンパーで並列接続した力学モデルを用い、仮想剛体面の各頂点が隣接する仮想剛体面の対応する頂点との間で働くばね力及びダンパー力の合計からなる剛体間力と、前記ばね力及びダンパー力によって各仮想剛体の頂点に作用するモーメントの合計からなる剛体に作用するワールド座標系原点回りのモーメントを、前記位置姿勢軌道を平滑にするための要求項目を表現する外力として算出するように構成されている。
【００２３】
また、本願の請求項５に記載の発明によれば、前記干渉検出部は、前記位置姿勢軌道上の各ノードに配置された前記剛体と前記障害物との最短距離を検出し、前記外力算出部は、前記最短距離に応じて前記剛体の各々に作用する斥力を、前記位置姿勢軌道が前記障害物との干渉を回避するための要求項目を表現する外力として算出するように構成されている。
【００２４】
また、本願の請求項６に記載の発明によれば、外力算出部は、前記剛体のローカル座標系に固定された前記剛体の前方進行方向と隣接する前記剛体間における位置姿勢軌道の方向とのなす角に応じたモーメントを、理想の位置姿勢軌道に近づけるための要求項目を表現する外力として算出するように構成されている。
【００２５】
また、本願の請求項７に記載の発明は、動作空間における制御対象の現在位置姿勢から目標位置姿勢に至るまでの位置姿勢軌道を計画する軌道計画方法であって、
前記制御対象の前記現在位置姿勢と前記目標位置姿勢の間を繋ぐ複数のノードからなる初期の位置姿勢軌道を獲得し、前記位置姿勢軌道上の各ノードに前記制御対象に相当する剛体をそれぞれ配置する初期位置姿勢軌道獲得ステップと、
前記位置姿勢軌道上の前記制御対象と前記動作空間に存在する障害物との干渉を検出する干渉検出ステップと、
前記剛体の各々に対して、前記位置姿勢軌道が平滑で且つ前記障害物との干渉を回避するための要求項目を表現する外力を算出する外力算出ステップと、
前記外力算出部で算出された外力がそれぞれ作用する複数の前記剛体からなる連成運動を記述した複数の剛体運動方程式に対して多剛体系動力学シミュレーションを実行し、所定の状態に収束したときの前記剛体の各々の位置及び姿勢に基づいて、前記位置姿勢軌道上の各ノードに相当する時刻における前記制御対象の位置及び姿勢を得る多剛体系動力学シミュレーション実行ステップと、
を有すること特徴とする軌道計画方法である。
【００２６】
また、本願の請求項８に記載の発明は、動作空間における制御対象の現在位置姿勢から目標位置姿勢に至るまでの位置姿勢軌道を計画するための処理をコンピューター上で実行するようにコンピューター可読形式で記述されたコンピューター・プログラムであって、前記コンピューターを、
前記制御対象の前記現在位置姿勢と前記目標位置姿勢の間を繋ぐ複数のノードからなる初期の位置姿勢軌道を獲得し、前記位置姿勢軌道上の各ノードに前記制御対象に相当する剛体をそれぞれ配置する初期位置姿勢軌道獲得部、
前記位置姿勢軌道上の前記制御対象と前記動作空間に存在する障害物との干渉を検出する干渉検出部、
前記剛体の各々に対して、前記位置姿勢軌道が平滑で且つ前記障害物との干渉を回避するための要求項目を表現する外力を算出する外力算出部、
前記外力算出部で算出された外力がそれぞれ作用する複数の前記剛体からなる連成運動を記述した複数の剛体運動方程式に対して多剛体系動力学シミュレーションを実行し、所定の状態に収束したときの前記剛体の各々の位置及び姿勢に基づいて、前記位置姿勢軌道上の各ノードに相当する時刻における前記制御対象の位置及び姿勢を得る多剛体系動力学シミュレーション実行部、
として機能させるためのコンピューター・プログラムである。
【００２７】
本願の請求項８に係るコンピューター・プログラムは、コンピューター上で所定の処理を実現するようにコンピューター可読形式で記述されたコンピューター・プログラムを定義したものである。換言すれば、本願の請求項８に係るコンピューター・プログラムをコンピューターにインストールすることによって、コンピューター上では協働的作用が発揮され、本願の請求項１に係る軌道計画装置と同様の作用効果を得ることができる。
【発明の効果】
【００２８】
本発明によれば、ロボットの移動制御や多関節アームの手先などの制御対象の、動作空間上における現在位置姿勢から目標位置姿勢までの移動経路若しくは軌道を好適に計画することができる、優れた軌道計画装置及び軌道計画方法、並びにコンピューター・プログラムを提供することができる。
【００２９】
また、本発明によれば、障害物を回避するような制御対象の軌道を発見するとともに、制御対象の移動制御や軌道制御に用い易くなるように滑らかな軌道として修正することができる、優れた軌道計画装置及び軌道計画方法、並びにコンピューター・プログラムを提供することができる。
【００３０】
また、本発明によれば、制御対象の現在の位置姿勢から目標とする位置姿勢までの滑らかな軌道を高速に生成することができる、優れた軌道計画装置及び軌道計画方法、並びにコンピューター・プログラムを提供することができる。
【００３１】
本願の請求項１、７、８に記載の発明によれば、制御対象の軌道上の各ノードを剛体モデルとして扱うことにより、制御対象の位置だけでなく姿勢の軌道も取り扱うことができる。
【００３２】
また、本願の請求項１、７、８に記載の発明によれば、初期位置姿勢軌道獲得部によって獲得した初期位置姿勢軌道を、多剛体系動力学シミュレーションを導入して修正することができる。
【００３３】
また、本願の請求項１、７、８に記載の発明によれば、制御対象の位置姿勢軌道が平滑で且つ障害物との干渉を回避するために必要とされる要求項目を力学モデル、すなわち力学拘束として表現することによって、位置姿勢軌道を修正するための処理を力の次元で統一的に解決することができる。
【００３４】
また、本願の請求項１、７、８に記載の発明によれば、かかる力学拘束を受ける複数の剛体からなる連成運動を記述した複数の剛体運動方程式に対して多剛体系動力学シミュレーションを適用することによって、各剛体の位置及び姿勢を得ることができ、これらは位置姿勢軌道上の各ノードに相当する時刻における制御対象の位置及び姿勢に相当する。
【００３５】
本願の請求項１、７、８に記載の発明は、リアルタイム障害物回避に適用であるとともに、リアルタイムでの目標位置姿勢修正が可能である。
【００３６】
また、本願の請求項２に記載の発明によれば、初期位置姿勢軌道獲得部は、ＲＲＴアルゴリズムを適用して、軌道の平滑化を考慮せずに、最適解とは必ずしも限らない最低限目的を達成するための軌道を高速且つ確実に探索することができる。ＲＲＴアルゴリズムによれば、軌道上の各ノードの位置を獲得した後、各ノードの初期姿勢を適当に与える。ノードにおける姿勢は剛体のローカル座標系に相当し、初期姿勢としてワールド座標系に設定するようにしてもよい。
【００３７】
また、本願の請求項３に記載の発明によれば、ＧＪＫアルゴリズムなどの凸形図形間距離を算出するための反復的な方法を用いることによって、軌道上のノードに相当する各剛体と障害物との最近傍点対若しくは最浸透点対を結ぶ最短距離を検出することができる。
【００３８】
また、本願の請求項４に記載の発明によれば、隣接する剛体同士で引き寄せ合う力（若しくは斥力）、すなわち「剛体間力」という力学拘束表現によって、「滑らかな位置姿勢軌道を獲得すること」という要求項目を満たすことができる。ここで、「滑らかな位置姿勢軌道を獲得すること」という要求項目を剛体間力として表現する際、剛体を、仮想的に体積を持たない面として扱うことで、近接する剛体同士の重なりの問題が生じにくくなる。
【００３９】
また、本願の請求項５に記載の発明によれば、制御対象と障害物との最短距離に応じた斥力を剛体に作用させるという力学拘束表現によって、「位置姿勢軌道において、物体間の浸透を回避すること」という要求項目を満たすことができる。
【００４０】
また、本願の請求項６に記載の発明によれば、制御対象の理想の姿勢軌道を、「軌道上の各ノードに配置された剛体の前方進行方向がなるべく軌道に沿うこと」と定義し、剛体の前方進行方向と隣接する剛体間における位置姿勢軌道の方向とのなす角に応じたモーメントを剛体に作用させるという力学拘束表現によって、「できる限り理想の位置姿勢軌道に近づけること」という要求項目を満たすことができる。
【００４１】
本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。
【図面の簡単な説明】
【００４２】
【図１Ａ】図１Ａは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。
【図１Ｂ】図１Ｂは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。
【図１Ｃ】図１Ｃは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。
【図１Ｄ】図１Ｄは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。
【図１Ｅ】図１Ｅは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。
【図１Ｆ】図１Ｆは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。
【図１Ｇ】図１Ｇは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。
【図２Ａ】図２Ａは、ＲＲＴアルゴリズムにより得られる軌道生成例を示した図である（但し、制御対象を表示）。
【図２Ｂ】図２Ｂは、ＲＲＴアルゴリズムにより得られる軌道生成例を示した図である（但し、制御対象を非表示）。
【図３Ａ】図３Ａは、ＲＲＴアルゴリズムにより得られる軌道生成例を示した図である（但し、制御対象を表示）。
【図３Ｂ】図３Ｂは、ＲＲＴアルゴリズムにより得られる軌道生成例を示した図である（但し、制御対象を非表示）。
【図４】図４は、初期条件の多剛体系の一例を示した図である。
【図５】図５は、剛体に対して定義される仮想剛体面を説明するための図である。
【図６】図６は、隣接する仮想剛体面間に作用する力を説明するための図である。
【図７】図７は、制御対象の理想の姿勢軌道を実現するための力学モデルを示した図である。
【図８】図８は、制御対象の軌道計画を実現するシステムの機能的構成を模式的に示した図である。
【図９】図９は、図８に示したシステムにおいて制御対象の軌道計画を実現するための処理手順を示したフローチャートである。
【図１０】図１０は、図３に示したようなＲＲＴアルゴリズムにより得られる初期軌道に対して、多剛体系動力学シミュレーションを適用して修正した結果を示した図である。
【図１１】図１１は、多剛体系動力学シミュレーションを適用して修正した軌道の他の例を示した図である。
【図１２】図１２は、図１１に示した軌道における剛体位置を示したグラフである。
【図１３】図１３は、多剛体系動力学シミュレーションを適用して修正した軌道のさらに他の例を示した図である。
【図１４】図１４は、図１３に示した軌道における剛体姿勢のピッチ角を示した図である。
【図１５】図１５は、ばね定数を定数とした場合の他剛体系力学モデルの軌道生成結果のスナップショットを示した図である。
【図１６】図１６は、ばね定数を剛体ｉと剛体０（現在状態）若しくは剛体Ｎ（目標状態）との距離に応じて変化する変数とした場合の軌道生成結果のスナップショットを示した図である。
【図１７】図１７は、ばね定数を定数並びに変数としてそれぞれ生成した軌道上における各剛体（ノード）の剛体姿勢のグラフを示した図である。
【図１８】図１８は、図３に示した状態を初期状態としたとき、図中向かって奥側の障害を動かした場合の多剛体系動力学シミュレーションの結果を示した図である。
【図１９】図１９は、多剛体系動力学シミュレーション実行し、剛体群が静定している状態を示した図である。
【図２０】図２０は、多剛体系動力学シミュレーション実行し、剛体群が静定している状態を示した図である。
【図２１】図２１は、シミュレーション実行後に、目標位置姿勢に修正値を与え、シミュレーションが収束した状態でのスナップショットを示した図である。
【図２２】図２２は、シミュレーション実行後に、目標位置姿勢に修正値を与え、シミュレーションが収束した状態でのスナップショットを示した図である。
【図２３】図２３は、本発明を実現可能なコンピューターの構成例を示した図である。
【発明を実施するための形態】
【００４３】
以下、図面を参照しながら本発明の実施形態について詳解する。
【００４４】
ロボットや多関節アームなどの現在の位置姿勢から目標とする位置姿勢までの滑らかな軌道を高速に生成するという目的を達成するには、以下の４つの項目が要求される。なお、以下で単に「制御対象」と言うときはロボットや多関節アームなどを含むものとする。
【００４５】
（１）最適解でなくとも、目標とする位置姿勢軌道を達成するための、何らかの初期位置姿勢軌道を高速に獲得すること。
（２）滑らかな位置姿勢軌道を獲得すること。
（３）位置姿勢軌道において、物体間の浸透（衝突若しくは干渉）を回避すること。
（４）できる限り理想の（設計者の意図を反映させた）位置姿勢軌道に近づけること。
【００４６】
要求項目（１）の解決においては、軌道の平滑化は問題ではなく、最適解とは必ずしも限らない最低限目的を達成するための軌道を、いかに高速に且つ確実に探索することができるかが選定基準となる。そこで、本発明の一実施形態では、何らかの初期位置姿勢軌道を拘束に獲得するために、前述したＲＲＴアルゴリズムを用いることとした。ＲＲＴアルゴリズムは、取り扱う問題が多次元の場合には直感的ではないといった欠点を持つものの、低次元の問題においては直感性を失うことはない。また、ＲＲＴアルゴリズムのもう１つの特徴として、多次元ｃｏｎｆｉｇｕｒａｔｉｏｎ  ｓｐａｃｅでの比較的高速な軌道計画が可能なことで知られている。
【００４７】
ここで、ＲＲＴアルゴリズムにおけるＴｒｅｅを生成する基本となる手順について、図１Ａ〜図１Ｇを参照しながら説明しておく。
【００４８】
まず、動作空間上に初期位置ｑ＿ｉｎｉｔを設定する（図１Ａを参照のこと）。
【００４９】
次いで、動作空間上にランダムにｑ＿ｒａｎｄを配置する（図１Ｂを参照のこと）。
【００５０】
次いで、Ｔｒｅｅ（探索木）におけるｑ＿ｒａｎｄに最も近いノード（すなわち最近傍ノード）をｑ＿ｎｅａｒとし、ｑ＿ｎｅａｒからｑ＿ｒａｎｄに向かう方向で一定距離にｑ＿ｎｅｗを設定する（図１Ｃを参照のこと）。但し、初期状態のＴｒｅｅにはｑ＿ｉｎｉｔのみが存在する（図１Ａを参照のこと）。ここで、ｑ＿ｎｅａｒとｑ＿ｎｅｗをつないで、ｑ＿ｎｅｗをＴｒｅｅに追加する（図１Ｄを参照のこと）。
【００５１】
次いで、動作空間上にランダムにｑ＿ｒａｎｄを再び配置し（図１Ｅを参照のこと）、Ｔｒｅｅにおけるｑ＿ｒａｎｄの最近傍ノードをｑ＿ｎｅａｒとし、前回と同様にｑ＿ｎｅａｒとｑ＿ｒａｎｄからｑ＿ｎｅｗを生成して、ｑ＿ｎｅｗをＴｒｅｅに追加する（図１Ｆを参照のこと）。
【００５２】
そして、上述したような操作を繰り返して実施することで、動作空間を好適に網羅するＴｒｅｅを生成することができる（図１Ｇを参照のこと）。
【００５３】
ＲＲＴによれば、比較的高速な軌道計画が可能であり、最適解とは言わなくてもそれなりの軌道を生成することができる。
【００５４】
図２並びに図３には、ＲＲＴアルゴリズムにより得られる軌道生成例を示している。各図では、説明の簡素化のため、制御対象の形状をＢｏｘ形状（若しくは直方体）とした。但し、本発明の要旨は特定の制御対象の形状に限定されるものではなく、いかなる形状に対しても適用でき、いかなる形状に対しても高速に軌道計画が可能であることを十分理解されたい。軌道計画方法の詳細に関しては後述に譲る。
【００５５】
図２Ａ並びに図３Ａには、Ｂｏｘ形状の制御対象を表示している。また、図２Ｂ並びに図３Ｂには、制御対象を非表示にし、その重心をノードで表示している。また、図２Ａ並びに図３Ａにおいて、両端のＢｏｘが現在位置姿勢（−ｘ方向側）、目標位置姿勢（＋ｘ方向側）での制御対象をそれぞれ表し、これらを繋ぐ一連のＢｏｘがＲＲＴアルゴリズムにより発見された軌道上の制御対象を表している。また、ほぼ中央のＢｏｘは、３次元空間上に存在する障害物を表し、この障害物Ｂｏｘを避けるように制御対象の軌道が探索されている。
【００５６】
図２及び図３で示したＲＲＴアルゴリズムのｃｏｎｆｉｇｕｒａｔｉｏｎ  ｓｐａｃｅの次元は位置（ｘ，ｙ，ｚ）の３次元としたが、姿勢も含めた６次元としても、本発明を実現する上で何ら影響はない。強いて言えば、非常に多くの障害物があり、制御対象物の姿勢を頻繁に変化させなければ通過することができないような場合の初期軌道探索で大きな威力を発揮する。しかし、その場合、ＲＲＴアルゴリズムによる初期軌道探索に関して速度の低下は否めない。このため、本明細書では、より高速な初期軌道探索を実現するために、位置の３次元をｃｏｎｆｉｇｕｒａｔｉｏｎ  ｓｐａｃｅとするＲＲＴアルゴリズムを１つの実施形態として示した。
【００５７】
なお、ＲＲＴアルゴリズムを用いて初期位置姿勢軌道を獲得する際には、障害物の回避の際の物体間浸透の問題が並存する。そこで、本実施形態では、ＲＲＴアルゴリズムの実行において、障害物の回避の際の物体間浸透の問題に関してＧＪＫ（Ｇｉｌｂｅｒｔ−Ｊｏｈｎｓｏｎ−Ｋｅｅｒｔｈｉｄｉｓｔａｎｃｅ ａｌｇｏｒｉｔｈｍ）アルゴリズムを適用する。ＧＪＫアルゴリズムは、凸形図形間距離を算出するための反復的な方法であり、衝突代表点対（最近傍点対又は最浸透点対）若しくは物体間の最短距離を、干渉検出結果として出力する。干渉検出結果は、力学モデルで表現して扱われるが、この点の詳細については後述に譲る。また、ＧＪＫアルゴリズムの詳細については、例えば、Ｇ．ｖａｎ  ｄｅｎ  Ｂｅｒｇｅｎ著“Ｃｏｌｌｉｓｉｏｎ  Ｄｅｔｅｃｔｉｏｎ  ｉｎ  Ｉｎｔｅｒａｃｔｉｖｅ  ３Ｄ  Ｅｎｖｉｒｏｎｍｅｎｔｓ”（Ｍｏｒｇａｎ  Ｋａｕｆｍａｎｎ  Ｐｕｂｌｉｓｈｅｒｓ，２００４）を参照されたい。
【００５８】
図２並びに図３を参照して分かるように、ＲＲＴアルゴリズムにより探索した初期軌道は、障害物の回避のため多くの迂回を含み、軌道としては明らかに無駄が多い。このため、ＲＲＴアルゴリズムを用いて初期位置姿勢軌道を高速に獲得した後、さらに上述した要求項目（２）〜（４）の解決が必要となる。
【００５９】
本出願人に既に譲渡されている特願２００８−５５６８１号明細書に開示されている軌道生成装置では、上記の要求項目（２）〜（４）を解決する際に、多質点系動力学シミュレーションを用いている。これは、制御対象の軌道上の各ノードを質点として扱い、軌道上の隣接するノード間、並びに最適経路上の対応するノードとの間において、軌道計画への要求を表現した力が作用するという力学モデルを構築し、動力学シミュレーション演算を実行することで、各質点の位置すなわち軌道を修正するものである。但し、軌道上の各ノードは質点で表現されるため、軌道上の制御対象の位置を求めることはできても、軌道上の制御対象の姿勢を求めることはできない。
【００６０】
これに対し、本実施形態では、要求項目（２）〜（４）を解決するに際して、多剛体系動力学シミュレーションを導入する。「多剛体系動力学シミュレーション」とは、軌道上の各ノードの位置において、制御対象に相当する剛体モデルを逐次配置していき、上記要求項目（２）〜（４）を各剛体に作用する外力として表現して、動力学シミュレーションを実行することで、各剛体の位置及び姿勢を求める手法である。軌道上の各ノードに配置された剛体の位置姿勢は、現在位置姿勢から目標位置姿勢まで移動する際の該当する時刻における制御対象の位置姿勢に相当する。多剛体系動力学シミュレーションは、制御対象の軌道上の各ノードに対して軌道計画への上記要求項目（２）〜（４）を表現した力を作用させるという点では、上記の多質点系動力学シミュレーションに類似するが、ノードに質点ではなく剛体を配置することによって、軌道生成において制御対象の位置とともに姿勢も取り扱うことが可能となる。
【００６１】
まず、多剛体系動力学シミュレーションの初期条件として、ＲＲＴアルゴリズムにより探索した軌道を制御対象の初期軌道とし（前述）、この初期軌道上の各ノードの位置を、剛体の代表点の初期位置とする。また、ノード毎の剛体の初期姿勢を適当に割り当てて、それらを初期位置姿勢とした多剛体系力学モデルを構築する。例えば、各ノードの剛体の初期姿勢（すなわち、剛体のローカル座標系）として、ワールド座標系に一様に設定するようにしてもよい。図４には、初期軌道上の各ノードの位置に、ワールド座標系に初期姿勢が設定された剛体がそれぞれは位置された多剛体系を示している。
【００６２】
なお、以下では、剛体の重心位置を剛体上の代表点とし、この代表点がノード位置と一致するように各ノードに剛体を配置するものとして説明するが、本発明の要旨はこのような剛体の配置方法に限定されるものではない。また、本発明の要旨は、初期姿勢の割り当てに関し特定の方法に限定されるものではなく、スプライン補間により姿勢を求める、あるいは軌道を計画する設計者が任意に姿勢を設定するようにしてもよい。
【００６３】
そして、本実施形態では、このような多剛体系に加わる外力モデルとして、上記の各要求項目（２）〜（４）を力学拘束として表現し、その力学拘束を多剛体系の各剛体に対し付加して、多剛体系動力学シミュレーションを実行する。これにより、軌道計画に必要な要求項目（１）〜（４）をすべて満足する軌道生成が実現される。つまり、すべての要求項目を力の次元で統合することで、統合的に問題解決することになる。
【００６４】
以下では、本発明の中核となる、上記の各要求項目（２）〜（４）に対する力学拘束モデルの表現方法について順に説明する。
【００６５】
まず、要求項目（２）「滑らかな位置姿勢軌道を獲得すること」の力学拘束表現について説明する。
【００６６】
図２並びに図３は、説明の簡素化のため、制御対象をＢｏｘ形状で表したが、現実にはＢｏｘ形状であることは稀であり、通常は複雑な形状を有する。また、ＲＲＴアルゴリズムの探索最小距離（ノード間距離）と物体形状の関係によっては、軌道上の各ノードに配置した多剛体系の隣接する剛体同士で重なりを持つ可能性を秘めている。また、できる限り生成する軌道を平滑に得るために、探索最小距離を小さくとることを考えると、多剛体系の各剛体間に重なりがない方がむしろ稀である。「滑らかな位置姿勢軌道を獲得すること」という要求項目を隣接する剛体同士で引き寄せ合う力、すなわち「剛体間力」という力学モデルで表現する際、剛体同士が重なり合うと、力学モデルは取り扱いにくいものとなる。
【００６７】
そこで、本実施形態では、軌道上の各ノードに（制御対象に相当する）剛体を順次配置した多剛体系における剛体間の重なりの問題を解決するために、各剛体に対して「仮想剛体面」を定義する。剛体を、仮想的に体積を持たない面として扱うことで、近接する剛体同士の重なりが生じにくくなることは自明であろう。
【００６８】
仮想剛体面は、各剛体に設定される姿勢すなわちローカル座標系に固定され、軌道計画中、元の剛体との相対位置関係は変化しない。但し、剛体のローカル座標系に対して任意に仮想剛体面を定義することができ、本発明の要旨は、仮想剛体面の特定の定義方法に限定されるものではない。
【００６９】
図５には、計画軌道上のｉ番目の剛体に対して定義された仮想剛体面を示している。図示の例では、剛体ｉのローカル座標系Σiのｘｙ平面との断面を仮想剛体面として定義されている。剛体ｉの仮想剛体面は、４角形状であり、４つの仮想剛体頂点ｘi,jで表現される（但し、ｊは頂点を指す通し番号で、０〜３の整数とする）。
【００７０】
多剛体系力学モデルにおいて、平滑な位置姿勢軌道を獲得するための力の拘束表現として、隣接する剛体間に力を作用させることで、各剛体の位置及び姿勢が変化し、無駄のない連続で滑らかな軌道に変換され、上記要求項目（２）を実現することができる。上記のように位置姿勢軌道上の各ノードに配置した剛体を仮想剛体面として扱う場合（図５を参照のこと）、隣接する仮想剛体面の対応する頂点の間をばね及びダンパーで並列接続する力学モデルにより、隣接する剛体同士が引き寄せ合う（若しくは斥力）剛体間力を作用させ、動力学シミュレーション（後述）を実行すると、無駄のない連続で滑らかな位置姿勢軌道に変換される。これによって、上記要求項目（２）を満足させることができる。
【００７１】
図６には、ｉ番目の剛体の仮想剛体頂点ｘi,jとこれに隣接する（ｉ＋１）番目の各剛体の仮想剛体頂点ｘi+1,ｊとを、ばね定数ｋsのばねと、ダンピング定数ｋdのダンパーで並列接続した、多剛体系の力学モデルを示している（ｊ＝０，１，２，３）。但し、動力学シミュレーションを実行する際、各仮想剛体頂点はワールド座標系での表現に変換されたものとする。なお、ばね定数ｋs及びダンピング定数ｋdは系全体として均一であるとは限らず、ノードｉ毎、あるいは頂点ｊ毎に異なるばね定数及びダンピング定数を使用することができる。
【００７２】
隣接する仮想剛体頂点ｘi,jとｘi+1,j間に働くばね力ｆi,i+1s,ｊは、下式（１）のように表される。
【００７３】
【数１】
2010155328.tif 000003
【００７４】
但し、上式（１）中のｌi,i+1jは隣接仮想剛体頂点ｘi,j、ｘi+1,j間のばねの自然長であり、ｋi,i+1s,jは当該頂点間を接続するばねのばね定数である。また、ｒi,i+1jは、仮想剛体頂点ｘi,jから隣接の対応する仮想剛体頂点ｘi+1,j間へ向かうベクトルであり、下式（２）のように表される。
【００７５】
【数２】
2010155328.tif 000004
【００７６】
隣接する仮想剛体頂点ｘi,jとｘi+1,j間でばねにより働くばね力ｆi,i+1s,ｊは、上式（１）に示したように、正規化されたばねの方向とばねの力の積で表される。
【００７７】
一方、隣接する仮想剛体頂点ｘi,jとｘi+1,j間にダンパーにより働くダンピング力ｆi,i+1d,ｊは、下式（３）のように表される。但し、当該頂点間を接続するダンパーのダンピング係数をｋi,i+1d,jとし、仮想剛体頂点ｘi,jの速度ベクトルをｖi,i+1jとする。
【００７８】
【数３】
2010155328.tif 000005
【００７９】
すなわち、２つの隣接仮想剛体頂点間の位置からダンピング力の方向を定め、相対速度の方向成分がダンピング力の大きさと正負符号を決定する。
【００８０】
上式（１）、（３）でそれぞれ示したばね力ｆi,i+1s,ｊ及びダンピング力ｆi,i+1d,ｊは、（ｉ＋１）番目の剛体のｊ番目の仮想剛体頂点からｉ番目の剛体のｊ番目の仮想剛体頂点に作用する力であるが、逆にｉ番目の剛体のｊ番目の仮想剛体頂点から（ｉ＋１）番目の剛体のｊ番目の仮想剛体頂点への作用力は、ｆi,i+1s,ｊ、ｆi,i+1d,ｊの符号をそれぞれ反転することで得られる。
【００８１】
（ｉ＋１）番目の剛体からｉ番目の剛体に作用する剛体間力ｆi,i+1nは、剛体に定義された仮想剛体面の各仮想剛体頂点ｊに作用するばね力ｆi,i+1s,ｊ及びダンピング力ｆi,i+1d,ｊの合計であり、下式（４）のように表される。
【００８２】
【数４】
2010155328.tif 000006
【００８３】
また、剛体間力ｆi,i+1nによりｉ番目の剛体に作用するワールド座標系原点回りのモーメントτi,i+1nは、各仮想剛体頂点に作用するモーメントの合計であり、下式（５）のように表される。
【００８４】
【数５】
2010155328.tif 000007
【００８５】
ｉ番目の剛体に作用する剛体間力ｆin、τinは、隣接する（ｉ−１）番目及び（ｉ＋１）番目の剛体からそれぞれ作用する剛体間力、モーメントの和であり、下式（６）に示す通りとなる。
【００８６】
【数６】
2010155328.tif 000008
【００８７】
剛体間力ｆi,i+1ｎは剛体ｉの位置を変化させ、隣接する剛体間で作用するモーメントτi,i+1nは剛体ｉの姿勢を変化させる。すなわち、隣接する剛体（ｉ−１）、（ｉ＋１）から剛体ｉに作用する斥力ｆip及びモーメントτipが上記要求項目（４）の力学拘束表現である。このような剛体間力、モーメントを計画軌道上の各ノードに配置された剛体に作用させることで、滑らかな位置姿勢軌道になり、上記要求項目（２）を満足させるということを理解されたい。
【００８８】
続いて、要求項目（３）「位置姿勢軌道において、物体間の浸透を回避すること」の力学拘束表現について説明する。
【００８９】
多剛体系力学モデルにおいて、物体間の浸透を回避するために課される力の拘束表現として、制御対象と障害物との最短距離に応じた斥力を剛体に作用させることで、多剛体系動力学シミュレーションを実行することにより各剛体の位置及び姿勢が変化し、無駄のない連続で滑らかな位置姿勢軌道に変換され、上記要求項目（３）を実現することができる。
【００９０】
多剛体系動力学シミュレーションを実行中は、常にＧＪＫアルゴリズム（前述）に基づく物体間の干渉検出機能が起動しており、制御対象と障害物との最短距離演算を行なっている。ここで、衝突代表点対、すなわち最短距離を構成する障害物上の点をＸonear（ｘonear，ｙonear，ｚonear）とし、制御対象上の点をＸrnear（ｘrnear，ｙrnear，ｚrnear）としたとき、制御対象と障害物との斥力ポテンシャルを、下式（７）に示すガウシアン関数で表現する。但し、同式中のｋpはポテンシャルの大きさを表す定数、σはポテンシャルの広がりを表す定数である。
【００９１】
【数７】
2010155328.tif 000009
【００９２】
このとき、制御対象（軌道上のｉ番目のノードに配置された剛体）上の点Ｘrnear（ｘrnear，ｙrnear，ｚrnear）に作用する人工斥力ｆipは、下式（８）のように表される。
【００９３】
【数８】
2010155328.tif 000010
【００９４】
上式（８）では、厳密な干渉検出演算、最短距離演算を利用しているが、計算量を削減するために、最短距離を構成する制御対象（剛体ｉ）上の点と障害物上の点の代わりに、それぞれの重心位置ｘi、ｘoc（但し、ワールド座標系表現とする）を用いてもよい。後者の場合、制御対象（軌道上のｉ番目のノードに配置された剛体）上の点Ｘrnear（ｘrnear，ｙrnear，ｚrnear）に作用する人工斥力ｆip,nearは、下式（９）のように表される。
【００９５】
【数９】
2010155328.tif 000011
【００９６】
但し、上式（９）において、（ｘoc，ｙoc，ｚoc）Tは障害物の重心位置であり、ポテンシャルの中心を表すことになる。
【００９７】
また、上記の斥力ｆipによって制御対象（軌道上のｉ番目のノードに配置された剛体）に作用する、ワールド座標系原点回りのモーメントτipは、下式（１０）のように表現される。
【００９８】
【数１０】
2010155328.tif 000012
【００９９】
人工斥力ｆipは剛体ｉの位置を変化させ、斥力により剛体ｉに作用するモーメントτipは剛体の姿勢を変化させる。すなわち、制御対象と障害物間の距離に応じて剛体ｉに作用する斥力ｆip及びモーメントτipが上記要求項目（３）の力学拘束表現である。このような斥力及びモーメントを計画軌道上の各ノードに配置された剛体に作用させることで、物体間の浸透を回避する位置姿勢軌道になり、上記要求項目（３）を満足させるということを理解されたい。
【０１００】
続いて、要求項目（４）「できる限り理想の位置姿勢軌道に近づけること」の力学拘束表現について説明する。
【０１０１】
本実施形態では、制御対象の理想の姿勢軌道を、「軌道上の各ノードに配置された剛体の前方進行方向がなるべく軌道に沿うこと」と定義することにする。ここで、剛体の前方進行方向は、剛体のローカル座標系に固定されるものである。但し、本発明の要旨は、剛体のローカル座標系に対する特定の方向に剛体の前方進行方向が限定されるものではなく、ローカル座標系に対して固定されていれば任意の方向を前方進行方向に決定できるものとする。
【０１０２】
図７には、制御対象の理想の姿勢軌道を上記のように定義した場合における力学モデルを図解している。以下では、同図を参照しながら、「できる限り理想の位置姿勢軌道に近づけること」の力学拘束表現について詳解する。
【０１０３】
説明の簡素化のため、位置姿勢軌道上の各ノードに配置する剛体ｉの代表点であるその重心位置が原点となるようにローカル座標系Σiが設定されているもとする。また、剛体ｉの前方進行方向はこのローカル座標系Σiの原点を始点として固定されたある方向ｄilであるとする。
【０１０４】
多剛体系力学モデルにおいて、ｉ番目並びに（ｉ＋１）番目の各剛体の重心位置をそれぞれｘi、ｘi+1とすると、これら隣接する剛体間における位置姿勢軌道の方向ｄi,i+1は、下式（１１）のように表される。
【０１０５】
【数１１】
2010155328.tif 000013
【０１０６】
理想の位置姿勢軌道に関する上記の定義に従うと、上式（１１）に示した剛体ｉから剛体（ｉ＋１）へ向かう方向ｄi,i+1が剛体ｉの前方進行方向ｄilにとっての理想の位置姿勢軌道となる。
【０１０７】
このとき、剛体ｉから剛体（ｉ＋１）へ向かう方向ｄi,i+1と、剛体ｉのローカル座標系に固定された前方進行方向ｄilとのなす角に、あるばね定数ｋτを乗じたばね力を、ｄi,i+1とｄilの外積方向回りの力として多剛体系力学モデルに付加することで、理想の姿勢軌道にできる限り近づけるための力学拘束を表現する。すなわち、この場合の理想軌道に近づけるために作用するモーメントτioは下式（１２）のように表される。
【０１０８】
【数１２】
2010155328.tif 000014
【０１０９】
なお、上式（１２）では、ｋτを定数として扱っているが、変数として扱うこともできる。例えば、初期状態（制御対象の現在位置に相当する剛体０）から剛体ｉまでの距離、若しくは、剛体ｉから目標状態（制御対象の目標位置に相当する剛体Ｎ）までの距離に応じて変化する変数として、ｋτを扱うようにしてもよい。このように距離に応じて変化する変数として扱うことで、他の力学拘束と統合した場合の整合性を保つのが容易になり、より目的とする滑らかな軌道を得ることができる。
【０１１０】
なお、理想軌道にできる限り近づけるために剛体ｉに作用する力ｆioは、下式（１３）に示すように０とする。
【０１１１】
【数１３】
2010155328.tif 000015
【０１１２】
また、上記要求項目（４）の力学拘束表現では、多剛体系動力学シミュレーションの収束をコントロールするために、空気抵抗のような粘性抵抗による力も併せて導入する。剛体ｉの速度をｖic、剛体ｉに作用する粘性係数をｋicとすると、剛体ｉに作用する粘性抵抗による力ｆidは、下式（１４）のように表される。
【０１１３】
【数１４】
2010155328.tif 000016
【０１１４】
また、上式（１４）に示した粘性力ｆidによるワールド座標系原点回りのモーメントは、下式（１５）のように表される。
【０１１５】
【数１５】
2010155328.tif 000017
【０１１６】
これまで、多剛体系力学モデルの剛体ｉに対して作用する、各要求項目（２）〜（４）に対する力学拘束を表現する力、トルクについて説明してきた。これら剛体ｉに作用する外力を加算した全外力Ｆiは、下式（１６）のように表される。
【０１１７】
【数１６】
2010155328.tif 000018
【０１１８】
剛体ｉの運動記述する６個の独立な方程式は、下式（１７）、（１８）にそれぞれ示すように、当業界において周知のニュートンの運動方程式及びオイラーの運動方程式として与えられる。
【０１１９】
【数１７】
2010155328.tif 000019
【０１２０】
【数１８】
2010155328.tif 000020
【０１２１】
但し、上式（１７）中のＰi、ｍiはそれぞれ剛体ｉの運動量及び質量を表す。また、上式（１８）中のＬi、Ｉi、ωiは剛体ｉの各運動量、ワールド座標系における慣性テンソル、角速度ベクトルをそれぞれ表す。
【０１２２】
上式（１６）に示した、剛体ｉに作用する全外力Ｆi＝［ｆi，τi］Tを、上式（１７）、（１８）にそれぞれ示したニュートンの方程式及びオイラーの方程式の外力として付加し、多剛体系動力学シミュレーションを実行する。
【０１２３】
すなわち、Ｎ個の剛体からなる連成運動をＮ個の剛体運動方程式として表現し、各シミュレーション・ステップにおいて数値積分を実行することで、各時刻における各剛体ｉすなわちノードの位置姿勢を得ることができる。この結果、制御対象の現在の位置姿勢から目標とする位置姿勢までの滑らかな軌道を高速に生成するという目的を達成するための、すべての要求項目（１）〜（４）を満足する軌道計画が実現される。
【０１２４】
図８には、これまで説明してきた、制御対象の軌道計画を実現するシステムの機能的構成を模式的に示している。図示のシステムは、ＲＲＴ実行部８１と、干渉検出部８２と、外力算出部８３と、多剛体系動力学シミュレーション実行部８４で構成される。
【０１２５】
ＲＲＴ実行部８１は、軌道計画指令を受けると、３次元空間上の障害物情報と制御対象物体の現在位置姿勢及び目標位置姿勢とから、ＲＲＴアルゴリズムを用いて、最低限の目的である現在位置姿勢から目標位置姿勢までの軌道を探索又は生成する。ＲＲＴ実行部８１で得られる初期位置姿勢軌道は、制御対象の現在位置姿勢と目標位置姿勢の間を繋ぐ複数のノードからなる。
【０１２６】
干渉検出部８２は、ＲＲＴ実行部８１で制御対象の初期位置姿勢軌道を探索又は生成する際における、干渉検出演算を担い、軌道探索又は生成時の制御対象と障害物との浸透回避を考慮する。したがって、ＲＲＴ実行部８１から出力される制御対象の初期位置軌道は、多くの迂回を含むが、障害物との干渉のないものとなる。障害物の回避の際の物体間浸透の問題に関してＧＪＫアルゴリズム（前述）を適用することができる。
【０１２７】
外力算出部８３は、ＲＲＴ実行部８１で探索又は生成された制御対象の初期位置姿勢軌道上の各ノードに配置した制御対象に相当する剛体に対して、障害物との干渉を回避しながら位置姿勢軌道を修正するための要求項目を表現する外力を算出する。本実施形態では、外力算出部８３は、初期位置姿勢軌道を迂回の無い平滑な位置姿勢軌道にするための力学拘束である隣接剛体間力を算出する隣接剛体間力算出部８３Ａと、制御対象とその動作空間上に存在する障害物との干渉を回避するための力学拘束である障害物回避斥力を算出する障害物回避斥力算出部８３Ｂと、できる限り理想的な位置姿勢軌道を近づけるための力学拘束である最適軌道力を算出する最適軌道力算出部８３Ｃより構成される。
【０１２８】
多剛体系動力学シミュレーション実行部８４は、外力算出部８３で算出された、軌道上の各剛体に作用する各外力（すなわち、隣接剛体間力、障害物回避斥力、及び最適軌道力）を、各剛体運動の外力拘束として作用させて、シミュレーション・サイクル毎に多剛体系の動力学演算を実行する。
【０１２９】
多剛体系動力学シミュレーション実行部８４による多剛体系動力学演算は、設定した状態（例えば、剛体の静定）に収束したとき終了し、そのときの多剛体系が形成するノード列が、目的とする平滑化された位置姿勢軌道として出力される。
【０１３０】
図９には、図８に示したシステムにおいて制御対象の軌道計画を実現するための処理手順をフローチャートの形式で示している。
【０１３１】
まず、軌道探索・生成処理として、ＲＲＴ実行部８１が、軌道計画指令を受け、３次元空間上の障害物情報と制御対象の現在位置姿勢及び目標位置姿勢とから、最低限の目的である現在位置姿勢から目標位置姿勢までの軌道を探索又は生成する（ステップＳ１）。生成した初期位置姿勢は迂回を含む可能性がある。
【０１３２】
次いで、外力算出処理部８３では、シミュレーション・サイクル毎に、軌道平滑化、障害物干渉回避、理想軌道への吸引を実現するための力学拘束を表現する各外力の算出処理を行なう（ステップＳ２）。
【０１３３】
次いで、多剛体系動力学シミュレーション部８４では、シミュレーション・サイクル毎に多剛体系動力学演算を実行し、多剛体系の状態（すなわち、制御対象の軌道上の各ノードの位置姿勢）を更新する（ステップＳ３）。
【０１３４】
制御対象の位置姿勢軌道があらかじめ設定した静定状態に収束するまで、ステップＳ２に戻り、ステップＳ２〜Ｓ４の処理を繰り返し実行する。
【０１３５】
そして、多剛体系が設定した状態に収束すると、多剛体系動力学シミュレーション部８４による動力学演算を終了するとともに、軌道生成処理全体を終了する。この結果、障害物との干渉がなく、平滑化され、理想軌道に近い、制御対象の位置姿勢軌道が生成される。
【０１３６】
図１０には、図３に示したようなＲＲＴアルゴリズムにより得られる初期軌道に対して、上記の多剛体系動力学シミュレーションを適用して修正した結果を示している。得られた制御対象の軌道は、位置及び姿勢ともに平滑で、且つ、障害物との干渉が回避されていることを確認できる。
【０１３７】
図１１には、上記の多剛体系動力学シミュレーションを適用して修正した軌道の他の例を示している。そして、図１２には、図１１に示した軌道における剛体位置を示している。図１２から、ｘ、ｙ、ｚすべての軌道は平滑化された無駄のない軌道に修正されていることが確認できる。
【０１３８】
また、図１３には、上記の多剛体系動力学シミュレーションを適用して修正した軌道のさらに他の例を示している。そして、図１４には、図１３に示した軌道における剛体姿勢のピッチ角を示している。図１４から、剛体姿勢軌道においても、平滑化された無駄のない軌道に修正されていることが期待できる。
【０１３９】
本実施形態では、要求項目（２）「滑らかな位置姿勢軌道を獲得すること」の力学拘束表現として、隣接する剛体間にばね力を作用させている（前述）。ここで、ばね定数ｋsを、系全体で均一すなわち定数としても、現在状態である剛体０若しくは目標状態である剛体Ｎとの距離に応じて変化する変数とすることもできる。
【０１４０】
図１５には、ばね定数を定数とした場合の多剛体系力学モデルの軌道生成結果のスナップショットを示している。また、図１６には、ばね定数を定数ではなく剛体ｉと現在状態である剛体０若しくは目標状態である剛体Ｎとの距離に応じて変化する変数とした場合の軌道生成結果のスナップショットを示している。また、図１７には、ばね定数を定数並びに変数としてそれぞれ生成した軌道上における各剛体（ノード）の剛体姿勢のグラフを示している。図１７中、黒丸でプロットした曲線がばね定数を定数とした場合（すなわち、図１５に対応）の剛体姿勢の変化を示し、白抜きまるでプロットした曲線がばね定数を距離に応じた変数とした場合（すなわち、図１６に対応）の剛体姿勢の変化を示している。図１７から、ばね定数を距離に応じた変数とみた方が、より滑らか且つ急激な変化の無い軌道になっていることが確認することができる。
【０１４１】
また、本実施形態では、要求項目（３）「位置姿勢軌道において、物体間の浸透を回避すること」の力学拘束表現として、制御対象と障害物との最短距離に応じた斥力を各剛体に作用させている（前述）。図１８には、図３に示した状態を初期状態としたとき、図中向かって奥側の障害を動かした場合の多剛体系動力学シミュレーションの結果を示している。同図より、障害物の移動に合わせて、生成した軌道が修正されていくのを確認することができる。
【０１４２】
本実施形態では、要求項目（２）〜（４）をそれぞれ剛体に作用する力学拘束として表現し、これら剛体に作用する外力を加算した全外力を運動方程式の外力として付加し、多剛体系動力学シミュレーションを実行する。
【０１４３】
図１９、図２０には、多剛体系動力学シミュレーション実行し、剛体群が静定している状態を示している。また、図２１、図２２には、シミュレーション実行後に、目標位置姿勢に修正値を与え、シミュレーションが収束した状態でのスナップショットを示している。但し、図１９及び図２１は制御対象物体を表示したものであり、図２０及び図２２は制御対象物体を非表示にし、剛体群の重心位置をノードで表示したものとなっている。図１９乃至図２２より、リアルタイムに目標位置姿勢を修正した場合も軌道生成は良好に行なわれることを確認することができる。
【０１４４】
以上より、本発明に係る軌道計画方法によれば、前述した目的を達成するために必要とされる要求項目を全て満足し，目的とした軌道を良好に生成できることがわかる．また，本手法では，障害物干渉回避，目標位置姿勢修正といった制御を，他のアルゴリズムを別に用意することなく，統一的に扱うことができ，かつリアルタイムに実行することも可能であり，この点においても本手法は有用である
【０１４５】
上述した制御対象の軌道を生成する処理（図８に示したシステム）は、例えばコンピューターなどの情報処理装置上で所定のアプリケーションを実行するという形態で実現される。図２３には、本発明を実現可能なコンピューターの構成例を示している。
【０１４６】
ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）１は、オペレーティング・システム（ＯＳ）が提供するプログラム実行環境下で、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）２やハード・ディスク・ドライブ（ＨＤＤ）１１に格納されているプログラムを実行する。例えば、多リンク構造体の動作制御若しくは運動データを生成するための処理をＣＰＵ１が所定のプログラムを実行するという形態で実現することもできる。
【０１４７】
ＲＯＭ２は、ＰＯＳＴ（Ｐｏｗｅｒ  Ｏｎ  Ｓｅｌｆ  Ｔｅｓｔ）やＢＩＯＳ（Ｂａｓｉｃ  Ｉｎｐｕｔ  Ｏｕｔｐｕｔ  Ｓｙｓｔｅｍ）などのプログラム・コードを恒久的に格納する。ＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）３は、ＲＯＭ２やＨＤＤ１１に格納されているプログラムをＣＰＵ１が実行する際にロードしたり、実行中のプログラムの作業データを一時的に保持したりするために使用される。これらはＣＰＵ１のローカル・ピンに直結されたローカル・バス４により相互に接続されている。
【０１４８】
ローカル・バス４は、ブリッジ５を介して、ＰＣＩ（Ｐｅｒｉｐｈｅｒａｌ  Ｃｏｍｐｏｎｅｎｔ  Ｉｎｔｅｒｃｏｎｎｅｃｔ）バスなどの入出力バス６に接続されている。
【０１４９】
キーボード８と、マウスなどのポインティング・デバイス９は、ユーザにより操作される入力デバイスである。ディスプレイ１０は、ＬＣＤ（Ｌｉｑｕｉｄ  Ｃｒｙｓｔａｌ  Ｄｉｓｐｌａｙ）又はＣＲＴ（Ｃａｔｈｏｄｅ  Ｒａｙ  Ｔｕｂｅ）などから成り、各種情報をテキストやイメージで表示する。
【０１５０】
ＨＤＤ１１は、記録メディアとしてのハード・ディスクを内蔵したドライブ・ユニットであり、ハード・ディスクを駆動する。ハード・ディスクには、オペレーティング・システムや各種アプリケーションなどＣＰＵ１が実行するプログラムをインストールしたり、データ・ファイルなどを保存したりするために使用される。例えば、多リンク構造体の動作制御若しくは運動データを生成する処理を実現するアプリケーション・プログラムをＨＤＤ１１にインストールしたり、生成された運動データのファイルをＨＤＤ１１に蓄積したりする。
【０１５１】
通信部１２は、例えば有線通信若しくは無線通信のＬＡＮ（Ｌｏｃａｌ  Ａｒｅａ  Ｎｅｔｗｏｒｋ）インターフェースである。例えば、ロボットや多関節アームなどの現在位置姿勢から目標位置姿勢までの軌道を生成する処理を実現するアプリケーション・プログラムを、ネットワーク経由でインストールすることができる。また、軌道データをネットワーク経由でダウンロードしたり、あるいは、生成した軌道データをネットワーク経由でアップロードしたりすることもできる。
【産業上の利用可能性】
【０１５２】
以上、特定の実施形態を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施形態の修正や代用を成し得ることは自明である。
【０１５３】
本発明は、ロボットの移動制御や多関節アームのエンド・エフェクタの軌道制御などに適用することができる。
【０１５４】
要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、特許請求の範囲を参酌すべきである。
【符号の説明】
【０１５５】
１…ＣＰＵ
２…ＲＯＭ
３…ＲＡＭ
４…ローカル・バス
５…ブリッジ
６…入出力バス
７…入出力インターフェース
８…キーボード
９…ポインティング・デバイス（マウス）
１０…ディスプレイ
１１…ＨＤＤ
１２…通信部
８１…ＲＲＴ実行部
８２…干渉検出部
８３…外力算出部
８３Ａ…隣接剛体間力算出部
８３Ｂ…障害物回避斥力算出部
８３Ｃ…最適軌道力算出部
８４…多剛体系動力学シミュレーション実行部
【図４】
2010155328.tif 000021
【図８】
2010155328.tif 000022
【図９】
2010155328.tif 000023
【図１２】
2010155328.tif 000024
【図１４】
2010155328.tif 000025
【図１７】
2010155328.tif 000026
【図２３】
2010155328.tif 000027
【図１Ａ】
2010155328.tif 000028
【図１Ｂ】
2010155328.tif 000029
【図１Ｃ】
2010155328.tif 000030
【図１Ｄ】
2010155328.tif 000031
【図１Ｅ】
2010155328.tif 000032
【図１Ｆ】
2010155328.tif 000033
【図１Ｇ】
2010155328.tif 000034
【図２Ａ】
2010155328.tif 000035
【図２Ｂ】
2010155328.tif 000036
【図３Ａ】
2010155328.tif 000037
【図３Ｂ】
2010155328.tif 000038
【図５】
2010155328.tif 000039
【図６】
2010155328.tif 000040
【図７】
2010155328.tif 000041
【図１０】
2010155328.tif 000042
【図１１】
2010155328.tif 000043
【図１３】
2010155328.tif 000044
【図１５】
2010155328.tif 000045
【図１６】
2010155328.tif 000046
【図１８】
2010155328.tif 000047
【図１９】
2010155328.tif 000048
【図２０】
2010155328.tif 000049
【図２１】
2010155328.tif 000050
【図２２】
2010155328.tif 000051