(57)【要約】【課題】障害物を回避する軌道を発見するとともに、移動制御や軌道計画に用い易くなるように滑らかな軌道として修正する。【解決手段】まずＲＲＴを用いて、現在位置から目標位置までの軌道を高速に獲得する。次いで、ＲＲＴで発見した軌道上の各ノードを質点とした多質点系動力学シミュレーションに、軌道の円滑化、最適化、及び物体間の浸透回避という要求をそれぞれ力で表現して各質点に付加し、ＲＲＴで発見した軌道上の位置を初期条件とした動力学シミュレーションを実行することで、すべての要求を満たす軌道の生成を行なう。【選択図】  図９
2009211571.tif 000002
【特許請求の範囲】
【請求項１】
探索空間における物体の軌道を計画する軌道計画装置であって、
物体の現在位置と目標位置の間を繋ぐ複数のノードからなる軌道を獲得する軌道獲得手段と、
前記軌道獲得主段が獲得した軌道を初期条件とし、該軌道を構成する各ノードを質点として扱い、該軌道に課される１以上の要求項目を力で表現して各質点に付加して該軌道を修正する軌道修正手段と、
を具備することを特徴とする軌道計画装置。
【請求項２】
前記軌道獲得手段は、複数のノードで複数の経路に分岐して前記探索空間上に展開された探索木に基づいて、前記現在位置と前記目標位置の間の軌道を獲得する、
ことを特徴とする請求項１に記載の軌道計画装置。
【請求項３】
前記軌道獲得手段は、前記探索空間上の初期位置ｑ＿ｉｎｉｔに対してランダムにｑ＿ｒａｎｄを配置するとともにｑ＿ｒａｎｄの最近傍ノードｑ＿ｎｅａｒからｑ＿ｒａｎｄに向かう方向で一定距離にｑ＿ｎｅｗを設定し、ｑ＿ｎｅｗを最近傍ノードｑ＿ｎｅａｒに繋いで前記探索木に追加し、さらに、ｑ＿ｎｅｗについてｑ＿ｒａｎｄのランダム配置とｑ＿ｒａｎｄの最近傍ノードｑ＿ｎｅａｒからｑ＿ｒａｎｄに向かう方向で一定距離にｑ＿ｎｅｗを設定して前記探索木に追加する処理を繰り返し実行することで、前記探索木を生成する、
ことを特徴とする請求項２に記載の軌道計画装置。
【請求項４】
前記軌道修正手段は、前記軌道獲得手段が獲得した軌道上の各ノードを質点とした多質点系に対して、該軌道に課される１以上の要求項目を表現した力を付加して、動力学シミュレーションを実行することで、前記要求項目を満たす軌道の生成を行なう、
ことを特徴とする請求項１に記載の軌道計画装置。
【請求項５】
前記軌道修正手段は、円滑な軌道を獲得すること、できる限り望む軌道に近づけること、又は、物体間の浸透を回避することのうち少なくとも１つを要求項目として、該要求項目を表現した力を該軌道上のノードに相当する各質点に付加する、
ことを特徴とする請求項１に記載の軌道計画装置。
【請求項６】
前記軌道修正手段は、前記軌道獲得手段が獲得した軌道上の隣接するノード間をバネ及びダンパにより接続し、円滑な軌道を獲得するという要求項目を満たすよう各バネ及びダンパを用いてノード間に引力を作用させることで、前記要求項目を満たす軌道の生成を行なう、
ことを特徴とする請求項５に記載の軌道計画装置。
【請求項７】
前記軌道修正手段は、前記軌道獲得手段が獲得した軌道上の各ノードとできる限り望む軌道上の対応点とをバネ及びダンパにより接続し、できる限り望む軌道に近づけるという要求項目を満たすよう各バネ及びダンパを用いて各ノードに対し仮想引力を作用させることで、前記要求項目を満たす軌道の生成を行なう、
ことを特徴とする請求項５に記載の軌道計画装置。
【請求項８】
前記軌道修正手段は、前記物体と前記探索空間上に存在する障害物との最短距離演算を実行し、算出された最短距離による斥力ポテンシャルに応じた人工斥力を前記軌道獲得手段が獲得した軌道上の各ノードに作用させることで、物体間の浸透を回避するという要求項目を満足させる、
ことを特徴とする請求項５に記載の軌道計画装置。
【請求項９】
前記軌道修正手段は、前記軌道獲得手段が獲得した軌道上の各ノードに対し、より安定に動作させるための粘性抵抗による力を併せて作用させる、
ことを特徴とする請求項８に記載の軌道計画装置。
【請求項１０】
探索空間における物体の軌道を計画する軌道計画方法であって、
物体の現在位置と目標位置の間を繋ぐ複数のノードからなる軌道を獲得する軌道獲得ステップと、
前記軌道獲得ステップにより獲得した軌道を初期条件とし、該軌道を構成する各ノードを質点として扱い、該軌道に課される１以上の要求項目を力で表現して各質点に付加して該軌道を修正する軌道修正ステップと、
を具備することを特徴とする軌道計画方法。
【請求項１１】
前記軌道獲得ステップでは、複数のノードで複数の経路に分岐して前記探索空間上に展開された探索木に基づいて、前記現在位置と前記目標位置の間の軌道を獲得する、
ことを特徴とする請求項１０に記載の軌道計画方法。
【請求項１２】
前記軌道獲得ステップでは、前記探索空間上の初期位置ｑ＿ｉｎｉｔに対してランダムにｑ＿ｒａｎｄを配置するとともにｑ＿ｒａｎｄの最近傍ノードｑ＿ｎｅａｒからｑ＿ｒａｎｄに向かう方向で一定距離にｑ＿ｎｅｗを設定し、ｑ＿ｎｅｗを最近傍ノードｑ＿ｎｅａｒに繋いで前記探索木に追加し、さらに、ｑ＿ｎｅｗについてｑ＿ｒａｎｄのランダム配置とｑ＿ｒａｎｄの最近傍ノードｑ＿ｎｅａｒからｑ＿ｒａｎｄに向かう方向で一定距離にｑ＿ｎｅｗを設定して前記探索木に追加する処理を繰り返し実行することで、前記探索木を生成する、
ことを特徴とする請求項１１に記載の軌道計画方法。
【請求項１３】
前記軌道修正ステップでは、前記軌道獲得ステップにより獲得した軌道上の各ノードを質点とした多質点系に対して、該軌道に課される１以上の要求項目を表現した力を付加して、動力学シミュレーションを実行することで、前記要求項目を満たす軌道の生成を行なう、
ことを特徴とする請求項１０に記載の軌道計画方法。
【請求項１４】
前記軌道修正ステップでは、円滑な軌道を獲得すること、できる限り望む軌道に近づけること、又は、物体間の浸透を回避することのうち少なくとも１つを要求項目として、該要求項目を表現した力を該軌道上のノードに相当する各質点に付加する、
ことを特徴とする請求項１０に記載の軌道計画方法。
【請求項１５】
前記軌道修正ステップでは、前記軌道獲得ステップにより獲得した軌道上の隣接するノード間をバネ及びダンパにより接続し、円滑な軌道を獲得するという要求項目を満たすよう各バネ及びダンパを用いてノード間に引力を作用させることで、前記要求項目を満たす軌道の生成を行なう、
ことを特徴とする請求項１４に記載の軌道計画方法。
【請求項１６】
前記軌道修正ステップでは、前記軌道獲得ステップにより獲得した軌道上の各ノードとできる限り望む軌道上の対応点とをバネ及びダンパにより接続し、できる限り望む軌道に近づけるという要求項目を満たすよう各バネ及びダンパを用いて各ノードに対し仮想引力を作用させることで、前記要求項目を満たす軌道の生成を行なう、
ことを特徴とする請求項１４に記載の軌道計画方法。
【請求項１７】
前記軌道修正ステップでは、前記物体と前記探索空間上に存在する障害物との最短距離演算を実行し、算出された最短距離による斥力ポテンシャルに応じた人工斥力を前記軌道獲得手段が獲得した軌道上の各ノードに作用させることで、物体間の浸透を回避するという要求項目を満足させる、
ことを特徴とする請求項１４に記載の軌道計画方法。
【請求項１８】
前記軌道修正ステップでは、前記軌道獲得ステップにより獲得した軌道上の各ノードに対し、より安定に動作させるための粘性抵抗による力を併せて作用させる、
ことを特徴とする請求項１７に記載の軌道計画方法。
【請求項１９】
探索空間における物体の軌道を計画するための処理をコンピュータ上で実行するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、前記コンピュータを、
物体の現在位置と目標位置の間を繋ぐ複数のノードからなる軌道を獲得する軌道獲得手段と、
前記軌道獲得主段が獲得した軌道を初期条件とし、該軌道を構成する各ノードを質点として扱い、該軌道に課される１以上の要求項目を力で表現して各質点に付加して該軌道を修正する軌道修正手段と、
として機能させるためのコンピュータ・プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、２次元又は３次元の作業空間上のある位置（現在位置：Ｓｔａｒｔ）からある位置（目標位置：Ｇｏａｌ）までの移動経路若しくは軌道（Ｐａｔｈ）を計画する軌道計画装置及び軌道計画方法、並びにコンピュータ・プログラムに係り、特に、障害物を回避する軌道を発見するとともに、ロボットの移動制御や多関節アームの手先の軌道計画などのため用い易くなるように滑らかな軌道として修正する軌道計画装置及び軌道計画方法、並びにコンピュータ・プログラムに関する。
【背景技術】
【０００２】
例えば、ロボットのような移動体装置や、多関節アームのエンド・エフェクタとなる手先などを自律的に動作制御させる場合、２次元又は３次元の作業空間上のある位置（現在位置：Ｓｔａｒｔ）からある位置（目標位置：Ｇｏａｌ）までの移動経路若しくは軌道（Ｐａｔｈ）を計画しなければならない。しかしながら、無限に存在する軌道の中から最適解を求めることは不可能に等しい。
【０００３】
これまで、ポテンシャル法（例えば、非特許文献１を参照のこと）、確率的ロードマップ法（例えば、非特許文献１を参照のこと）、Ｒａｐｉｄｌｙ−ｅｘｐｌｏｒｉｎｇ  Ｒａｎｄｏｍ  Ｔｒｅｅ（ＲＲＴ）（例えば、非特許文献２を参照のこと）といった多くの軌道計画方法について提案がなされてきた。
【０００４】
ここで、ポテンシャル法とは、探索空間に目的姿勢に操作対象を近づける「引力ポテンシャル」、並びに、障害物から操作対象を遠ざける「斥力ポテンシャル」というポテンシャル場を定義し、操作対象の現在姿勢におけるポテンシャル場の勾配を算出し、ポテンシャルの減少する方向へ微少に移動させる手続きを繰り返すことによって軌道を計画する方法である。
【０００５】
例えば、移動体、障害物、及び目標物の位置及び形状の情報に基づいて生成された移動空間内のマップを用いて、移動体、障害物、及び目標物の相対位置関係に基づく引力ポテンシャル及び斥力ポテンシャルを計算し、これらの和である合成ポテンシャルを生成し、この合成ポテンシャルに基づいてマップ内の経路探索を行なって該経路探索の収束位置がローカルミニマムかどうかを判別し、収束位置がローカルミニマムの場合には該収束位置のポテンシャルを所定値だけ増加させた仮想ポテンシャルを生成するとともに、収束位置が目標位置の場合には経路探索の結果に基づいて移動体の移動経路を生成する経路生成装置について提案がなされている（例えば、特許文献１を参照のこと）。
【０００６】
また、確率的ロードマップ法は、配置空間内から幾つかの姿勢をランダムに選び、近接する姿勢に移動可能であれば枝でつないでいき、ロードマップが連結でなければ、障害物の多い領域にある姿勢からランダム・ウォークを行ない、ロードマップを拡張し、ダイクストラ法を適用してロードマップにおける最短経路を求める方法である。
【０００７】
例えば、数理計画上の手法であるダイクストラ法によって、ネットワークにおける最小コストの入ノードから出ノードまでの経路を探索する最小コスト経路探索装置について提案がなされている（例えば、特許文献２を参照のこと）。
【０００８】
また、ＲＲＴは、探索空間を好適に網羅する木のデータ構造、並びに当該木を生成する基本となるアルゴリズムである。ＲＲＴでは、まず探索空間上に初期位置を設定した後、同探索空間内にランダムなサンプル位置を配置するとともに初期位置から当該サンプル位置に向かう最近傍位置を探索し、初期位置と最近傍位置とを結んで木に追加する。その後、新たに追加された最近傍位置についてサンプル位置の配置とさらに最近傍位置の探索と木への追加を繰り返すことで、探索空間を網羅する木を生成することができる。現在、ＲＲＴは、実時間での経路計画や軌道生成などに用いられており（例えば、非特許文献３を参照のこと）、多くの研究事例がある。
【０００９】
ポテンシャル法は、非常に直感的であるが、目的姿勢以外にもポテンシャルが極小となる姿勢が多数表れる可能性があることから、安全な動作経路を効率よく求めることは困難である。確率的ロードマップ法は、多くの障害物に囲まれた狭い空間を通る動作計画が困難、すなわち多数の姿勢をサンプルしなくてはならない、といった欠点を持つ。ＲＲＴは、取り扱う問題が多次元の場合に直感的ではないといった欠点を持つものの、低次元の問題においては、直感性を失うことはない。また、ＲＲＴのもう１つの特徴として、多次元ｃｏｎｆｉｇｕｒａｔｉｏｎ  ｓｐａｃｅでの比較的高速な軌道計画（Ｐａｔｈ  Ｐｌａｎｎｉｎｇ）が可能なことで知られている。
【００１０】
このように、従来の軌道計画手法はそれぞれ一長一短があることが分かる。また、いずれかの手法により生成した軌道を円滑化したり、障害物を回避したりするための軌道修正を行なう必要がある。軌道を円滑化する手法として、躍度最小や（例えば、非特許文献４を参照のこと）、トルク最小、多項式（Ｓｐｌｉｎｅ）を用いた手法が考案されているが、円滑な軌道を獲得すること、最適な軌道に近づけること、物体間の浸透を回避することを同時に満たすことはできない。言い換えれば、ロボットの移動制御や多関節アームの手先の軌道計画などのための最終的な制御に用いるものとして満足な軌道を得ることはできない。
【００１１】
【特許文献１】特開２００６−３５０７７６号公報
【特許文献２】特開２００２−１３３３５１号公報
【非特許文献１】太田順外著「知能ロボット入門 −動作計画問題の解法−」（コロナ社、２００１）
【非特許文献２】Ｓｔｅｖｅｎ  Ｍ．ＬａＶａｌｌｅ，“Ｒａｐｉｄｌｙ−Ｅｘｐｌｏｒｉｎｇ  Ｒａｎｄｏｍ  Ｔｒｅｅｓ：Ａ  Ｎｅｗ  Ｔｏｏｌ  ｆｏｒ  Ｐａｔｈ  Ｐｌａｎｎｉｎｇ”（ＴＲ．９８−１１，Ｃｏｍｐｕｔｅｒ  Ｓｃｉｅｎｃｅ  Ｄｐｔ．，Ｉｏｗａ  Ｓｔａｔｅ  Ｕｎｉｖｅｒｓｉｔｙ．Ｏｃｔ．１９９８）
【非特許文献３】Ｊａｍｅｓ  Ｊ．ｋｕｆｆｎｅｒ，Ｊｒ．，“Ｄｙｎａｍｉｃａｌｌｙ−ｓｔａｂｌｅ  Ｍｏｔｉｏｎ  Ｐｌａｎｎｉｎｇ  ｆｏｒ  Ｈｕｍａｎｏｉｄ  Ｒｏｂｏｔｓ”（Ａｕｔｏｎｏｍｏｕｓ  Ｒｏｂｏｔｓ  ｖｏｌ．１２，Ｎｏ．１，ｐｐ．１０５−１１８，２００２）
【非特許文献４】Ｔ．Ｆｌａｓｈ  ａｎｄ  Ｎ．Ｈｏｇａｎ，“Ｔｈｅ  Ｃｏｏｒｄｉｎａｔｉｏｎ  ｏｆ  Ａｒｍ  Ｍｏｖｅｍｅｎｔｓ：Ａｎ  Ｅｘｐｅｒｉｍｅｎｔａｌｌｙ  Ｃｏｎｆｉｒｍｅｄ  Ｍａｔｈｅｍａｔｉｃａｌ  Ｍｏｄｅｌ”（Ｔｈｅ  Ｊｏｕｒｎａｌ  ｏｆ  Ｎｅｕｒｏｓｃｉｅｎｃｅ，Ｖｏｌ．５，Ｎｏ．７，ｐｐ．１６８８−１７０３，１９８５）
【発明の開示】
【発明が解決しようとする課題】
【００１２】
本発明の目的は、２次元又は３次元の作業空間上のある位置からある位置までの移動経路若しくは軌道を好適に計画することができる、優れた軌道計画装置及び軌道計画方法、並びにコンピュータ・プログラムを提供することにある。
【００１３】
本発明のさらなる目的は、障害物を回避する軌道を発見するとともに、移動制御や軌道計画に用い易くなるように滑らかな軌道として修正することができる、優れた軌道計画装置及び軌道計画方法、並びにコンピュータ・プログラムを提供することにある。
【課題を解決するための手段】
【００１４】
本発明は、上記課題を参酌してなされたものであり、その第１の側面は、探索空間における物体の軌道を計画する軌道計画装置であって、
物体の現在位置と目標位置の間を繋ぐ複数のノードからなる軌道を獲得する軌道獲得手段と、
前記軌道獲得主段が獲得した軌道を初期条件とし、該軌道を構成する各ノードを質点として扱い、該軌道に課される１以上の要求項目を力で表現して各質点に付加して該軌道を修正する軌道修正手段と、
を具備することを特徴とする軌道計画装置である。
【００１５】
例えば、ロボットのような移動体装置や、多関節アームのエンド・エフェクタとなる手先などを自律的に動作制御させる場合、移動経路や軌道を計画する必要があるが、無限に存在する軌道の中から最適解を求めることは不可能に等しい。従来提案されている軌道計画方法はそれぞれ一長一短がある。また、いずれかの手法により生成した軌道に対して、円滑化したり、障害物を回避したりするための軌道修正を行なう必要がある。
【００１６】
これに対し、本発明に係る軌道計画装置では、まず軌道獲得手段がＲＲＴなどの既存の軌道計画手法を用いて物体の現在位置と目標位置の間を繋ぐ複数のノードからなる軌道を獲得する。続いて、軌道修正手段が、獲得した軌道を初期条件とし、該軌道を構成する各ノードを質点として扱い、該軌道に課される１以上の要求項目を力で表現して各質点に付加することで、該軌道を修正する。具体的には、ＲＲＴで発見した軌道上の各ノードを質点とした多質点系に、軌道の円滑化、最適化、及び物体間の浸透回避という要求をそれぞれ力で表現して各質点に付加し、ＲＲＴで発見した軌道上の位置を初期条件とした動力学シミュレーションを実行することで軌道の修正を行ない、すべての要求を満たす軌道の生成を行なう。
【００１７】
したがって、本発明によれば、ＲＲＴを用いて無限に存在する軌道の１つを発見するとともに、多質点系の動力学シミュレーションを用いることで、障害物の回避や軌道の円滑化といった問題を力の次元で同時に解決することで、最終的な制御で用いることができる（すなわち、移動制御や軌道計画に用い易い）軌道を安価な計算量で算出することができる。
【００１８】
ここで、軌道獲得手段は、ＲＲＴを用いる場合、複数のノードで複数の経路に分岐して前記探索空間上に展開された探索木に基づいて、前記現在位置と前記目標位置の間の軌道を獲得することができる。また、軌道獲得手段は、探索空間上の初期位置ｑ＿ｉｎｉｔに対してランダムにｑ＿ｒａｎｄを配置するとともにｑ＿ｒａｎｄの最近傍ノードｑ＿ｎｅａｒからｑ＿ｒａｎｄに向かう方向で一定距離にｑ＿ｎｅｗを設定し、ｑ＿ｎｅｗを最近傍ノードｑ＿ｎｅａｒに繋いで前記探索木に追加し、さらに、ｑ＿ｎｅｗについてｑ＿ｒａｎｄのランダム配置とｑ＿ｒａｎｄの最近傍ノードｑ＿ｎｅａｒからｑ＿ｒａｎｄに向かう方向で一定距離にｑ＿ｎｅｗを設定して探索木に追加する処理を繰り返し実行することで、探索空間を好適に網羅する探索木を生成することができる。
【００１９】
また、上述したＲＲＴで発見される軌道は最適解とは限らない。現在位置から目標位置に至るまでの滑らかな軌道を生成するには、円滑な軌道を獲得すること、できる限り望む軌道に近づけること、並びに、物体間の浸透を回避すること、という３つの要求項目が課される。本発明に係る軌道計画装置によれば、軌道修正手段は、円滑な軌道を獲得すること、できる限り望む軌道に近づけること、又は、物体間の浸透を回避すること、という３つを要求項目とし、これらすべての要求項目をそれぞれに対応する力として表現し、力の次元で統合することで、総合的に解決するようにしている。
【００２０】
具体的には、軌道修正手段は、獲得した軌道上の隣接するノード間をバネ及びダンパにより接続し、円滑な軌道を獲得するという要求項目を満たすよう各バネ及びダンパを用いてノード間に引力を作用させることで、当該要求項目を満たす軌道の生成を行なうことができる。
【００２１】
また、軌道修正手段は、獲得した軌道上の各ノードとできる限り望む軌道上の対応点とをバネ及びダンパにより接続し、できる限り望む軌道に近づけるという要求項目を満たすよう各バネ及びダンパを用いて各ノードに対し仮想引力を作用させることで、当該要求項目を満たす軌道の生成を行なうことができる。
【００２２】
また、軌道修正手段は、制御対象となる物体と探索空間上に存在する障害物との最短距離演算を実行し、算出された最短距離による斥力ポテンシャルに応じた人工斥力を獲得した軌道上の各ノードに作用させることで、物体間の浸透を回避するという要求項目を満足させるようにする。その際、獲得した軌道上の各ノードに対し、より安定に動作させるための粘性抵抗による力を併せて作用させるようにしてもよい。
【００２３】
また、本発明の第２の側面は、探索空間における物体の軌道を計画するための処理をコンピュータ上で実行するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、前記コンピュータを、
物体の現在位置と目標位置の間を繋ぐ複数のノードからなる軌道を獲得する軌道獲得手段と、
前記軌道獲得主段が獲得した軌道を初期条件とし、該軌道を構成する各ノードを質点として扱い、該軌道に課される１以上の要求項目を力で表現して各質点に付加して該軌道を修正する軌道修正手段と、
として機能させるためのコンピュータ・プログラムである。
【００２４】
本発明の第２の側面に係るコンピュータ・プログラムは、コンピュータ上で所定の処理を実現するようにコンピュータ可読形式で記述されたコンピュータ・プログラムを定義したものである。換言すれば、本発明の第２の側面に係るコンピュータ・プログラムをコンピュータにインストールすることによって、コンピュータ上では協働的作用が発揮され、本発明の第１の側面に係る軌道計画装置と同様の作用効果を得ることができる。
【発明の効果】
【００２５】
本発明によれば、２次元又は３次元の作業空間上のある位置からある位置までの移動経路若しくは軌道を好適に計画することができる、優れた軌道計画装置及び軌道計画方法、並びにコンピュータ・プログラムを提供することができる。
【００２６】
また、本発明によれば、障害物を回避する軌道を発見するとともに、移動制御や軌道計画に用い易くなるように滑らかな軌道として修正することができる、優れた軌道計画装置及び軌道計画方法、並びにコンピュータ・プログラムを提供することができる。
【００２７】
本発明に係る軌道計画装置は、ＲＲＴを用いて無限に存在する軌道の１つを発見するとともに、多質点系の動力学シミュレーションを用いることで、障害物の回避や軌道の円滑化といった問題を力の次元で同時に解決することで、最終的な制御で用いることができる（すなわち、移動制御や軌道計画に用い易い）軌道を安価な計算量で算出することができる。また、ロボットや多関節アームなどの制御期間中にわたって、本発明に係る軌道計画装置を用いて、適宜軌道を生成し、また常に軌道の修正を行なうことで、軌道計画だけでなく、リアルタイムに動作する障害物回避を実現することができる。
【００２８】
本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。
【発明を実施するための最良の形態】
【００２９】
以下、図面を参照しながら本発明の実施形態について詳解する。
【００３０】
本発明者は、２次元又は３次元の作業空間上のある位置（現在位置：Ｓｔａｒｔ）からある位置（目標位置：Ｇｏａｌ）までの滑らかな軌道を生成するという目的を達成するには、以下の４項目が要求されていると思料する。
（１）最適解ではなくとも最低限目的（ＳｔａｒｔからＧｏａｌまでの軌道を生成すること）を達成するための軌道を高速に獲得すること。
（２）円滑な軌道を獲得すること。
（３）できる限り望む軌道（最適軌道）に近づけること。
（４）物体間の浸透を回避すること。
【００３１】
本実施形態では、要求項目（１）に関しては、既存の軌道計画アルゴリズムを用いて解決する。既存の軌道計画アルゴリズムとして、ポテンシャル法や確率的ロードマップ法、Ｒａｐｉｄｌｙ−ｅｘｐｌｏｒｉｎｇ  Ｒａｎｄｏｍ  Ｔｒｅｅ（ＲＲＴ）を挙げることができるが、それぞれ手法には一長一短がある（前述）。このうちＲＲＴは、取り扱う問題が多次元の場合には直感的ではないといった欠点を持つものの、低次元の問題においては直感性を失うことはない。また、ＲＲＴのもう１つの特徴として、多次元ｃｏｎｆｉｇｕｒａｔｉｏｎ  ｓｐａｃｅでの比較的高速な軌道計画が可能なことで知られている。
【００３２】
本実施形態において取り扱う次元が低次元であることから、上記の要求項目のうち（１）である最低限の目的を達成するための軌道を高速に求める手段として、比較的高速な軌道計画が可能なＲＲＴを用いることとする。したがって、他の要求項目（２）〜（４）を解決する上で、３次元空間上で無数に存在する軌道のうち少なくとも１つが得られていることを前提とすることができる。
【００３３】
ここで、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムについて、図１Ａ〜図１Ｇを参照しながら説明しておく。
【００３４】
まず、探索空間上に初期位置ｑ＿ｉｎｉｔを設定する（図１Ａを参照のこと）。
【００３５】
次いで、探索空間上にランダムにｑ＿ｒａｎｄを配置する（図１Ｂを参照のこと）。
【００３６】
次いで、Ｔｒｅｅ（探索木）におけるｑ＿ｒａｎｄに最も近いノード（すなわち最近傍ノード）をｑ＿ｎｅａｒとし、ｑ＿ｎｅａｒからｑ＿ｒａｎｄに向かう方向で一定距離にｑ＿ｎｅｗを設定する（図１Ｃを参照のこと）。但し、初期状態のＴｒｅｅにはｑ＿ｉｎｉｔのみが存在する（図１Ａを参照のこと）。ここで、ｑ＿ｎｅａｒとｑ＿ｎｅｗをつないで、ｑ＿ｎｅｗをＴｒｅｅに追加する（図１Ｄを参照のこと）。
【００３７】
次いで、探索空間上にランダムにｑ＿ｒａｎｄを再び配置し（図１Ｅを参照のこと）、Ｔｒｅｅにおけるｑ＿ｒａｎｄの最近傍ノードをｑ＿ｎｅａｒとし、前回と同様にｑ＿ｎｅａｒとｑ＿ｒａｎｄからｑ＿ｎｅｗを生成して、ｑ＿ｎｅｗをＴｒｅｅに追加する（図１Ｆを参照のこと）。
【００３８】
そして、上述したような操作を繰り返して実施することで、探索空間を好適に網羅するＴｒｅｅを生成することができる（図１Ｇを参照のこと）。
【００３９】
ＲＲＴによれば、比較的高速な軌道計画が可能であり、最適解とは言わなくてもそれなりの軌道を生成することができる。
【００４０】
図２には、障害物である中央の立方体を回避しながら制御対象となる立方体を同図の右上から左下に向かって移動させたときの軌道を示している。また、図３には、制御対象物を非表示にして、ＲＲＴにより発見された軌道を示している。
【００４１】
なお、ＲＲＴの実行において、障害物の回避の際の物体間浸透の問題に関しては、ＧＪＫ（Ｇｉｌｂｅｒｔ−Ｊｏｈｎｓｏｎ−Ｋｅｅｒｔｈｉｄｉｓｔａｎｃｅ ａｌｇｏｒｉｔｈｍ）を想定する。ＧＪＫアルゴリズムは、凸形図形間距離を算出するための反復的な方法であり、衝突代表点対（最近傍点対又は最浸透点対）を、干渉検出結果として出力する。ＧＪＫアルゴリズムの詳細については、例えば、Ｇ．ｖａｎ  ｄｅｎ  Ｂｅｒｇｅｎ，“Ｃｏｌｌｉｓｉｏｎ  Ｄｅｔｅｃｔｉｏｎ  ｉｎ  Ｉｎｔｅｒａｃｔｉｖｅ  ３Ｄ  Ｅｎｖｉｒｏｎｍｅｎｔｓ”（Ｍｏｒｇａｎ  Ｋａｕｆｍａｎｎ  Ｐｕｂｌｉｓｈｅｒｓ，２００４）を参照されたい。
【００４２】
図２並びに図３に見られるように、ＲＲＴにより発見された軌道は、明らかに無駄が多い。すなわち、ＲＲＴから最適解が求まる訳ではないから、ロボットの移動制御や多関節アームの手先の軌道計画などのために用い易くなるように滑らかな軌道として修正する必要がある。
【００４３】
本実施形態では、できる限り最適軌道に近づけることと、物体間の浸透を回避すること、すなわち、残る３つの要求項目（２）〜（４）を満足するために、多質点系の動力学シミュレーションを導入する。すなわち、ＲＲＴで発見した軌道上の各ノードを質点とした多質点系に、軌道の円滑化、最適化、及び物体間の浸透回避という要求をそれぞれ力で表現して各質点に付加し、ＲＲＴで発見した軌道上の位置を初期条件とした動力学シミュレーションを実行することで、すべての要求を満たす軌道の生成を行なう。つまり、要求項目（２）〜（４）として挙げているすべての要求を、それぞれに対応する力として表現し、力の次元で統合することで、総合的に解決することになる。各要求項目（２）〜（４）に対する力への変換方法について、以下に説明する。
【００４４】
図４には、要求項目（２）の円滑な軌道を獲得するための力の拘束表現を示している。図中、両端点がそれぞれ開始位置Ｓｔａｒｔと目標位置Ｇｏａｌのノードであり、これらに接続されている複数のノードはＲＲＴにより算出された、開始位置及び目標位置間の軌道上のノードである。そして、隣接するノード間をバネ及びダンパにより接続し、これらバネ及びダンパを用いて円滑な軌道への引力を表現する。すなわち、当該要求項目を満たすようこれらバネ及びダンパを用いてノード間に引力を作用させることで、無駄のない軌道へと変換される。
【００４５】
ここで、３次元空間上でのｎ番目のノードＮｏｄｅ（ｎ）の位置をｘn＝［ｘn，ｙn，ｚn］T、ｎ＋１番目のノードＮｏｄｅ（ｎ＋１）の位置をｘn+1＝［ｘn+1，ｙn+1，ｚn+1］T、ノードＮｏｄｅ（ｎ）からノードＮｏｄｅ（ｎ＋１）へ向かうベクトルをｒn,n+1、ノードＮｏｄｅ（ｎ）とノードＮｏｄｅ（ｎ＋１）間のバネの自然長をｌn,n+1、そのバネ定数をｋn,n+1としたとき、ノードＮｏｄｅ（ｎ）とノードＮｏｄｅ（ｎ＋１）の間に働くバネ力Ｆｂn,n+1は、下式（１）のように表される。
【００４６】
【数１】
2009211571.tif 000003
【００４７】
すなわち、ノードＮｏｄｅ（ｎ）とノードＮｏｄｅ（ｎ＋１）間に働くバネ力は、正規化されたバネ力の方向とバネ力の積で表される。
【００４８】
また、ダンピング係数をｃn,n+1、速度ベクトルをｖn,n+1とすると、ノードＮｏｄｅ（ｎ）とノードＮｏｄｅ（ｎ＋１）間に働くダンパによる力Ｆｄn,n+1は同様に、下式（２）のように表される。
【００４９】
【数２】
2009211571.tif 000004
【００５０】
すなわち、２つのノード間の位置からダンピング力の方向を定め、相対速度の作用方向成分がダンピング力の大きさと正負の符号を決定する。
【００５１】
ノードＮｏｄｅ（ｎ＋１）への作用力は、上式（１）から求まるバネ力Ｆｂn,n+1と、上式（２）から求まるダンピング力Ｆｄn,n+1の符号を反転することで得られる。以上をまとめると、ノードＮｏｄｅ（ｎ＋１）からノードＮｏｄｅ（ｎ）に作用するノード間引力Ｆa,ｎ,ｎ+1は、下式（３）のように表される。
【００５２】
【数３】
2009211571.tif 000005
【００５３】
また、図５には、要求項目（３）のできる限り望む軌道に近づけるための力の拘束表現を示している。図中、両端点がそれぞれ開始位置Ｓｔａｒｔと目標位置Ｇｏａｌのノードであり、点線は開始位置Ｓｔａｒｔから目標位置Ｇｏａｌへ向かう最適と思われる経路である。そして、この点線上の×で表される各点は、当該経路上の各ノードの望ましい位置に相当し、該当するノードとの間をバネ及びダンパにより接続し、これらバネ及びダンパを用いて最適経路への仮想引力を表現する。すなわち、当該要求項目を満たすようこれらバネ及びダンパを用いて各ノードに対して最適経路への仮想引力を作用させることで、できる限り望む軌道へと変換される。なお、同図には、図４に示した、円滑な軌道を獲得するための力の拘束を表したノード間のバネ及びダンパを点線で描いている。
【００５４】
ここで、３次元空間上でのｎ番目のノードＮｏｄｅ（ｎ）の最適経路位置をｘopt,n＝［ｘopt,n，ｙopt,n，ｚopt,n］T、ノードＮｏｄｅ（ｎ）から最適経路位置に向かうベクトルをｒopt,n、その相対速度ベクトルをｖopt,n、バネ定数をｋopt,n、ダンピング定数をｃopt,n、バネの自然長及び最適経路位置の速度をともに０としたとき、ノードＮｏｄｅ（ｎ）に働く最適経路位置への仮想引力Ｆopt,nは、上式（３）に示したノード間引力と同様に、下式（４）のように表される。
【００５５】
【数４】
2009211571.tif 000006
【００５６】
続いて、要求項目（４）の、物体間の浸透を回避するために課される力の拘束表現について説明する。
【００５７】
動力学シミュレーションの実行時は、常に干渉検出機能が働いており、制御対象と障害物との最短距離演算が動作している。この最短距離演算で算出される最短距離をρとしたとき、制御対象と障害物との距離ρよる斥力ポテンシャルＵ0（ρ）は、下式（５）のように表される。
【００５８】
【数５】
2009211571.tif 000007
【００５９】
このとき、ｎ番目のノードＮｏｄｅ（ｎ）に作用する人工斥力Ｆr,nは、下式（６）のように表される。
【００６０】
【数６】
2009211571.tif 000008
【００６１】
【数７】
2009211571.tif 000009
【００６２】
但し、上式（６）において、ρ0は人工斥力が作用する領域を表す閾値、Ｐb並びにＰaはそれぞれ制御対象と障害物の最短距離を表わす最近傍点である。すなわち、上式（７）で表わされるｄは正規化された力の作用方向を示している。ここで得られた斥力を多質点系に作用させる。
【００６３】
また、多質点系  動力学シミュレーションの発散を抑制し、より安定に動作させるために粘性抵抗（空気抵抗のようなもの）による力も併せて導入する。ｎ番目のノードＮｏｄｅ（ｎ）の速度をｖn、粘性係数をγnとすると、ノードＮｏｄｅ（ｎ）に作用する粘性抵抗による力Ｆdmp,nは、下式（８）のように表わされる。
【００６４】
【数８】
2009211571.tif 000010
【００６５】
図６には、ｎ番目のノードＮｏｄｅ（ｎ）すなわち質点ｎに対し、上述したすべての外力Ｆa,ｎ,ｎ+1、Ｆopt,n、Ｆr,nを作用させた様子を示している。ノードＮｏｄｅ（ｎ）に作用する外力Ｆnは下式（９）の通りとなる。但し、同式の右辺左から順に、粘性力、ノードＮｏｄｅ（ｎ−１）からノードＮｏｄｅ（ｎ）に作用するノード間引力、ノードＮｏｄｅ（ｎ＋１）からＮｏｄｅ（ｎ）に作用するノード間引力、浸透回避ポテンシャル斥力、最適経路引力を表している。
【００６６】
【数９】
2009211571.tif 000011
【００６７】
これまで説明してきたすべての力の拘束を、多質点系の外力Ｆとして付加して動力学シミュレーションを実行する。すなわち、下式（１０）に示す各ノード（質点）の運動方程式に対して上式（９）に示した外力Ｆnを代入することで質点の運動方程式を記述する。そして、ｎ個のノードからなる連成運動をｎ個の連立線形常微分方程式として表現し、ＲＲＴで得られた位置を初期条件とした数値積分を行なうことで数値シミュレーションを実行する。数値積分として、例えばＲｕｎｎｇｅ−Ｋｕｔｔａ法を用いることができる。なお、力の算出は、数値シミュレーションのステップ毎に実行される。
【００６８】
【数１０】
2009211571.tif 000012
【００６９】
図７並びに図８には、軌道上の各ノードに対し上記力の拘束を付加した場合のシミュレーション例を示している。図７Ａ、図７Ｂには、ＲＲＴが発見した経路を示し、図８Ａ、図８Ｂには軌道に力の拘束を加えて軌道を修正した結果を示している。これらの図から分かるように、ＲＲＴで発見された初期の経路は要求項目（２）〜（４）の要求通りに円滑化されたことが確認できよう。なお、図示の最適経路は、ＳｔａｒｔとＧｏａｌを結ぶ直線として設定している。
【００７０】
図９には、これまで説明してきた軌道計画方法を実現するためのシステム構成を機能ブロック図の形式で示している。図示のシステム１０は、ＲＲＴ実行部１１と、干渉検出部１２と、外力算出部１３と、多質点系の動力学シミュレーション実行部１４で構成される。例えば、パーソナル・コンピュータのような一般的な計算機システム上で所定のプログラムを実行するという形態で、図示の機能的構成を実現することが可能である。
【００７１】
ＲＲＴ実行部１１は、経路計画指令を受け取ると、３次元の探索空間上の障害物情報と制御対象物の現在位置（Ｓｔａｒｔ）及び目標位置（Ｇｏａｌ）位置とから、ある経路を生成する。その際、干渉検出及び最短距離演算を担う干渉検出部１２を用いることにより、軌道（Ｐａｔｈ）生成時の浸透を考慮する。すなわち、ＲＲＴは干渉の無い軌道生成を行なう。
【００７２】
外力算出部１３は、ＲＲＴ実行部１１で生成された起動を円滑化するためのノード間引力を算出するノード間引力算出部１３Ａと、空間上に存在する障害物と制御対象との浸透を回避するための障害物回避斥力算出部１３Ｂと、望んだ経路に近づけるために作用させる最適経路引力を算出するための最適経路引力算出部１３Ｃより構成され、それぞれにおいて算出された引力や斥力などの外力を出力する。
【００７３】
多質点系の動力学シミュレーション実行部１４は、外力算出部１３で算出された各ノードにおける外力を、各ノードに対する外力拘束として付加する。多質点系の動力学シミュレーション実行部１４では、シミュレーション・サイクル毎の動力学演算を担う。
【００７４】
なお、多質点系の動力学シミュレーション実行時においても、物体間の浸透を検出し、若しくは物体間の最短距離算出のために、干渉検出部１２が利用される。多質点系の動力学シミュレーションは、各ノードが設定した状態（例えば、物体の静止）に収束したときに終了し、そのときのノード群が求める円滑な経路となる。
【００７５】
図１０には、図９に示した軌道計画システム１０が実行する軌道計画処理の手順をフローチャートの形式で示している。
【００７６】
まず、ＲＲＴ実行部１１が経路計画指令を受け、３次元の探索空間上の障害物情報と制御対象物の現在位置（Ｓｔａｒｔ）及び目標位置（Ｇｏａｌ）とから、ある経路を生成する（ステップＳ１）。そして、生成した経路を、後続の外力算出処理に引渡す。
【００７７】
次いで、外力算出処理では、各シミュレーション・サイクルにおいて、経路上の各ノードに付加される外力算出を行なう（ステップＳ２）。
【００７８】
次いで、多質点系実行部１４はシミュレーションを実行する（ステップＳ３）。
【００７９】
そして、上述した多質点系の動力学シミュレーションは、各ノードが設定したノード静定条件を満たすまで繰り返される（ステップＳ４）。
【００８０】
図９に示した軌道計画システム１０により物体の軌道計画を行なった場合、図７及び図８に示したような結果を得ることができる。すなわち，高速にＲＲＴにより空間上の移動経路を獲得し、その獲得した経路を円滑化するとともに、望む経路（最適経路）に近づけることが容易になる。
【００８１】
また、多質点系の動力学シミュレーションにおける演算は、剛体系の動力学シミュレーションに比し、大幅に計算量を抑えることが可能であることから、多質点系の動力学シミュレーションを物体移動制御実行時に常に動作させることで、リアルタイムで障害物回避といった軌道修正を行なうことも可能である。
【００８２】
図１１には、多質点系の動力学シミュレーションを物体移動制御実行時に常に動作させて、リアルタイムで障害物回避を行なった結果を示している。図１１Ａは、ＲＲＴを実行して初期条件としての起動を獲得した後、ノードが静定するまで多質点系の動力学シミュレーションを実行した状態を示している。また、図１１Ｂには、ノードが静定した後、空間上の障害物が移動したときの軌道の修正状況を示している。
【産業上の利用可能性】
【００８３】
以上、特定の実施形態を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施形態の修正や代用を成し得ることは自明である。
【００８４】
本発明は、ロボットの移動制御や多関節アームのエンド・エフェクタの軌道制御などに適用することができる。
【００８５】
要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、特許請求の範囲を参酌すべきである。
【図面の簡単な説明】
【００８６】
【図１Ａ】図１Ａは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。
【図１Ｂ】図１Ｂは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。
【図１Ｃ】図１Ｃは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。
【図１Ｄ】図１Ｄは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。
【図１Ｅ】図１Ｅは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。
【図１Ｆ】図１Ｆは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。
【図１Ｇ】図１Ｇは、ＲＲＴのＴｒｅｅを生成する基本となるアルゴリズムを説明するための図である。
【図２】図２は、障害物である中央の立方体を回避しながら制御対象となる立方体を同図の右上から左下に向かって移動させた様子を示した図である。
【図３】図３は、制御対象物を非表示にして、ＲＲＴにより発見された軌道を示した図である。
【図４】図４は、要求項目（２）の円滑な軌道を獲得するための力の拘束表現を説明するための図である。
【図５】図５は、要求項目（３）のできる限り望む軌道に近づけるための力の拘束表現を説明するための図である。
【図６】図６は、ｎ番目のノードＮｏｄｅ（ｎ）にすべての外力Ｆa,ｎ,ｎ+1、Ｆopt,n、Ｆr,nを作用させた様子を示した図である。
【図７Ａ】図７Ａは、ＲＲＴが発見した経路を示した図である。
【図７Ｂ】図７Ｂは、ＲＲＴが発見した経路を示した図である。
【図８Ａ】図８Ａは、軌道に力の拘束を加えて軌道を修正した結果を示した図である。
【図８Ｂ】図８Ｂは、軌道に力の拘束を加えて軌道を修正した結果を示した図である。
【図９】図９は、本発明に係る軌道計画方法を実現するためのシステム構成を示した機能ブロック図である。
【図１０】図１０は、図９に示した軌道計画システム１０が実行する軌道計画処理の手順を示したフローチャートである。
【図１１Ａ】図１１Ａは、ＲＲＴを実行して初期条件としての起動を獲得した後、ノードが静定するまで多質点系の動力学シミュレーションを実行した状態を示した図である。
【図１１Ｂ】図１１Ｂは、ノードが静定した後、空間上の障害物が移動したときの軌道の修正状況を示した図である。
【符号の説明】
【００８７】
１０…軌道計画システム
１１…ＲＲＴ実行部
１２…干渉検出部
１３…外力算出部
１３Ａ…ノード間引力算出部
１３Ｂ…障害物回避斥力算出部
１３Ｃ…最適経路引力算出部
１４…多質点系実行部
【図１Ａ】
2009211571.tif 000013
【図１Ｂ】
2009211571.tif 000014
【図１Ｃ】
2009211571.tif 000015
【図１Ｄ】
2009211571.tif 000016
【図１Ｅ】
2009211571.tif 000017
【図１Ｆ】
2009211571.tif 000018
【図１Ｇ】
2009211571.tif 000019
【図６】
2009211571.tif 000020
【図９】
2009211571.tif 000021
【図１０】
2009211571.tif 000022
【図２】
2009211571.tif 000023
【図３】
2009211571.tif 000024
【図４】
2009211571.tif 000025
【図５】
2009211571.tif 000026
【図７Ａ】
2009211571.tif 000027
【図７Ｂ】
2009211571.tif 000028
【図８Ａ】
2009211571.tif 000029
【図８Ｂ】
2009211571.tif 000030
【図１１Ａ】
2009211571.tif 000031
【図１１Ｂ】
2009211571.tif 000032