JP2013206043A - 動作計画装置、動作計画方法、動作計画処理プログラム - Google Patents

Abstract

【課題】効率的に最適な動作を計画することができる動作計画装置、動作計画方法、動作計画処理プログラムを提供する。
【解決手段】位相空間における制御対象の現在状態を探索木の根として定め、前記制御対象の次状態を探索木の枝として求め、前記制御対象の現在状態から目標状態までの位置姿勢動作を探索木に基づいて計画する動作計画装置１であって、運動方程式を線形化した式を求め、前記運動方程式を線形化した式を用いて次状態候補を求める第１探索と、前記運動方程式を用いて前記次状態候補から前記次状態を求める第２探索とを行うことを特徴とする。
【選択図】図２

Description

本発明は、動作計画装置、動作計画方法、動作計画処理プログラムに関するものである。

動作計画とは、制御対象物としての自動車、ロボット、エネルギープラントなどの機械装置や、当該機械装置の一部の構成などを、ある位置・速度からある位置・速度まで動作させる場合の移動履歴を計画することをいう。したがって、制御対象物を動作させる際に要する時間やエネルギーを最小化できる最適な動作移動履歴を計画することが好ましいと共に、動作を計画する際に要する時間を最小化できることが好ましい。特に、動作速度に比べ小さいトルクで運動する機械装置では、位置や速度といった動作状態を急激に変化させることができないため、慣性力、遠心力やコリオリ力を考慮した上で動作を計画する必要があるので、最適化が難しい。

従来の動作計画方法としては、探索木を用いて非線形特性を有する機械装置の運動を探索、計画する方法が開示されている（例えば、特許文献１）。上記特許文献１は、図１２に示すように、物体１０２の状態空間における状態を探索木の根として求め、所定の時間間隔が経過した後の物体の次の状態１０３，１０４，１０５，１０６を探索木の枝として求める。この場合、状態空間を分割した格子中に各状態が配置される。次いで、次の状態１０３，１０４，１０５，１０６のうち、所定の条件を満たす次の状態１０４，１０５を枝刈りし、枝刈りされなかった次の状態１０３，１０６を登録木として登録する。同様に、次の状態１０６から時間間隔が経過した後の物体のさらに次の状態１０７，１０８，１０９，１１０を探索木の枝として求め、所定の条件を満たす次の状態を枝刈りし、枝刈りされなかった次の状態を登録木として登録する。このように、順々に次の状態から登録木を伸ばしていき、登録木として登録された状態の中から適切な一連の状態を選択することによって動作を求める。これにより、上記特許文献１では、計算コストを抑えて動作を計画することができる。

特開２０１１−１６１６２４号公報

しかしながら、上記特許文献１の場合、所定の時間間隔が経過した後の物体の次の状態は、トルク粒度を一定にして求めるため、格子１１１，１１２のように探索されない格子が生じたり、１つの格子中に複数の状態が配置されたりしてしまう。探索されない格子が生じると、精度が悪化するので最適な動作を計画することが困難になる、という問題がある。また、１つの格子中に複数の状態が配置されると、その配置を行うだけの計算時間を要するので、動作を計画する際に要する時間を最小化することが困難になる、という問題がある。

そこで本発明は上記した問題点に鑑み、効率的に最適な動作を計画することができる動作計画装置、動作計画方法、動作計画処理プログラムを提供することを目的とする。

本発明の請求項１に係る動作計画装置は、位相空間における制御対象の現在状態を探索木の根として定め、前記制御対象の次状態を探索木の枝として求め、前記制御対象の現在状態から目標状態までの位置と速度の履歴を探索木に基づいて計画する動作計画装置であって、運動方程式を線形化した式を求め、前記運動方程式を線形化した式を用いて次状態候補を求める第１探索と、前記運動方程式を用いて前記次状態候補から前記次状態を求める第２探索とを行うことを特徴とする。

本発明の請求項１１に係る動作計画方法は、位相空間における制御対象の現在状態を探索木の根として定め、前記制御対象の次状態を探索木の枝として求め、前記制御対象の現在状態から目標状態までの位置姿勢動作を探索木に基づいて計画する動作計画方法であって、運動方程式を線形化した式を求めるステップと、運動方程式を線形化した式を用いて次状態候補を求める第１探索ステップと、運動方程式を用いて前記次状態候補から前記次状態を求める第２探索ステップとを備えることを特徴とする。

本発明の請求項２０に係る動作計画処理プログラムは、コンピュータに対して、位相空間における制御対象の現在状態を探索木の根として定め、前記制御対象の次状態を探索木の枝として求め、前記制御対象の現在状態から目標状態までの位置姿勢動作を探索木に基づいて計画する動作計画処理を実行させる動作計画処理プログラムであって、運動方程式を線形化した式を求めるステップと、運動方程式を線形化した式を用いて次状態候補を求める第１探索ステップと、運動方程式を用いて前記次状態候補から前記次状態を求める第２探索ステップとを実行させることを特徴とする。

本発明の請求項１，１１，２０によれば、運動方程式を用いて真の次状態を求める前に、仮の次状態となる次状態候補を求めることとしたので、次状態候補をより均一に分布させることができ、従来に比べ探索されない空間、及び重複して探索されてしまう空間を少なくすることができる。したがって、効率的に最適な動作を計画することができる。

本実施形態に係る動作計画装置の全体構成を模式的に示すブロック図である。 本実施形態に係る動作計画装置のステップＳＰ４の動作を示す模式図である。 本実施形態に係る動作計画装置のステップＳＰ６の動作を示す模式図である。 本実施形態に係る動作計画装置のステップＳＰ８及びステップＳＰ１０の動作を示す模式図である。 本実施形態に係る動作計画装置のステップＳＰ１２の動作を示す模式図である。 本実施形態に係る動作計画装置における動作計画処理手順を示すフローチャートである。 本実施形態に係る直列計算部と並列計算部における処理内容を示すフローチャートである。 実施例に係る３自由度倒立振子の構成を示す模式図である。 実施例に係る３自由度倒立振子の状態を示す図であり、図９Ａは初期状態、図９Ｂは目標状態を示す図である。 実施例に係る２自由度倒立振子のシミュレーションの結果を示すグラフである。 実施例に係る３自由度倒立振子のシミュレーションの結果を示すグラフである。 従来例に係る探索の手法を示す図である。

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

（全体構成）
図１に示す動作計画装置１は、当該動作計画装置１の各種機能を統括的に制御する制御部２、次状態記憶部４、線形化式記憶部５、メモリ６が制御バス８を介して接続されている。動作計画装置１は、全体として位相空間における制御対象の現在状態を探索木の根として定め、前記制御対象の次状態を探索木の枝として求め、前記制御対象の現在状態から目標状態までの位置と速度の履歴を探索木に基づいて計画する。位相空間とは、制御対象の位置（姿勢）と速度（角速度）で記述された空間をいう。

制御部２は、予め格納されている基本プログラムや、次状態探索プログラムなどの各種プログラムを読み出して、これら各種プログラムに従って動作計画装置１全体を制御する。本実施形態の場合、制御部２は、直列計算部（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ：ＣＰＵ）１０と並列計算部（Ｇｒａｐｈｉｃｓ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ：ＧＰＵ）１２とを有する。

制御部２は、運動方程式を線形化した式を用いて次状態候補を求める第１探索と、運動方程式を用いて前記次状態候補から真の次状態を求める第２探索とを行う。制御部２は、当該第２探索の結果得られた真の次状態ごとに上記第１探索と第２探索を行ってさらに真の次状態を求める。このように制御部２は第１探索と第２探索を所定の時間間隔毎に繰り返し行い、得られた真の次状態から最適な動作を求める。

また、制御部２は、第１探索の後、一定条件を満たす次状態候補を枝刈りすることにより、第２探索を行う次状態候補を減らすことにより、全体の計算量を減らすこととしてもよい。

第１探索は、慣性行列計算と、次状態予測とを含む。慣性行列計算は、図２に示すように、制御部２は、制御対象の現在状態ｓ_０の慣性行列Ａ_０ ^−１と、現在状態にある制御対象に対しトルクを０とした場合の次状態ｓ_１の慣性行列Ａ_１ ^−１を動力学計算で求める。
まず、制御部２は、運動方程式を用いて慣性行列を求める。二自由度倒立振子の運動方程式は、下記式の通りである。

ｌ_１は第１リンクの長さ、ｍ_１は第１リンクの質量、θ_１は第１リンクの角度、ｌ_２は第２リンクの長さ、ｍ_２は第２リンクの質量、θ_２は第２リンクの角度である。また、重力加速度をｇとする。さらに一般化すると下記式となる。

τはトルクである。それぞれの項は下記式の通りである。

上記の結果から、制御部２は下記式（運動方程式から計算した次状態の値を含む式）を求め、線形化式記憶部５に記憶する。制御部２は、下記式により慣性行列Ａ^−１を求める。

上記のようにして現在状態ｓ_０の慣性行列Ａ_０ ^−１と、トルク０の場合の次状態ｓ_１の慣性行列Ａ_１ ^−１を求めることにより、制御部２は、現在状態における制御対象物が有する慣性のパラメータと、次状態における制御対象物が有する慣性のパラメータとを求めることができる。制御部２は、慣性行列Ａ_０ ^−１とＡ_１ ^−１とから仮の慣性行列Ｂ^−１（運動方程式から計算した慣性行列を含む式）を求め、線形化式記憶部５に記憶する。本実施形態の場合、Ｂ^−１は、下記式の通り慣性行列Ａ_０ ^−１とＡ_１ ^−１の平均である。

本実施形態では、現在状態ｓ_０と次状態ｓ_１の間において慣性行列が一定（Ｂ^−１）であるとみなして、図３に示すように次状態予測を行う。本図において制御部２は、Ｂ^−１を用いてトルクτを入力し、次状態候補を求める。この場合、トルクτの限界値がある一定値に抑えられると、現在状態から到達できる次状態の範囲は、慣性行列が一定であると仮定したことにより、超並行四辺形（hyper parallelogram）１４となる。本実施形態の場合、次状態候補は運動方程式を線形化した式である下記式を用いて求める。

制御部２は、上記式のトルクτに最大トルク値を入力して次状態候補を求める。最大トルク値はベクトルを用い具体的には、（τ_１ｍａｘ，０）、（-τ_１ｍａｘ，０）、（０，τ_２ｍａｘ）、（０，-τ_２ｍａｘ）、（τ_１ｍａｘ，τ_２ｍａｘ）、（-τ_１ｍａｘ，τ_２ｍａｘ）、（τ_１ｍａｘ，-τ_２ｍａｘ）、（-τ_１ｍａｘ，-τ_２ｍａｘ）を用いる。このようにして制御部２は図４に示すように、超並行四辺形（hyper parallelogram）１４上に複数、本図の場合８個の次状態候補を求める。本実施形態の場合、次状態候補は、トルク０の場合の次状態ｓ_１を含めると全部で９個である。実際上、制御部２は制御対象の制約条件、例えば位置、速度、トルク限界を考慮して制御可能な領域に次状態候補を求める。
枝刈りは、特に限定されるものではないが、本実施形態では既に記憶されている登録次状態から一定距離にある次状態候補を破棄する方法について説明する。

制御部２は、次状態記憶部４に既に記憶された登録次状態に係る情報を読み出す。そして制御部２は、次状態候補が登録次状態から距離ｄの範囲内に定められているか否かを判断し、範囲内にある次状態候補を破棄する。

本図の場合制御部２は、既に記憶された登録次状態１６，１８を次状態記憶部４から読み出し、次状態候補との距離を計測する。そして制御部２は、次状態候補２０Ｃ，２２Ｃが登録次状態１６から距離ｄの範囲内に定められているので、次状態候補２０Ｃ，２２Ｃを破棄する。同様に制御部２は、次状態候補２４Ｃが登録次状態１８から距離ｄの範囲内に定められているので、次状態候補２４Ｃを破棄する。

第２探索は、枝刈りされなかった次状態候補に対し行われる。すなわち制御部２は、次状態１６，１８からいずれも距離ｄの範囲内にない次状態候補２６Ｃ，２８Ｃ，３０Ｃ，３２Ｃ，３４Ｃに対し、動力学計算を行い、図５に示すように真の次状態２６Ｒ，２８Ｒ，３０Ｒ，３２Ｒ，３４Ｒを求める。

次状態候補２６Ｃ，２８Ｃ，３０Ｃ，３２Ｃ，３４Ｃは、慣性行列が一定（Ｂ^−１）とみなして求めたため、実際の制御対象物の動作と誤差が生じている。したがって、制御部２は、時間と共に変化するトルクτに対応した慣性行列を用いて次状態候補２６Ｃ，２８Ｃ，３０Ｃ，３２Ｃ，３４Ｃから真の次状態２６Ｒ，２８Ｒ，３０Ｒ，３２Ｒ，３４Ｒを求める。

真の次状態２６Ｒ，２８Ｒ，３０Ｒ，３２Ｒ，３４Ｒは、制御可否判定を経た後、次状態記憶部４に登録次状態として記憶される。制御可否判定は、真の次状態が制御可能な領域にあるか否かを判断する。制御部２は制御可能性を確認し、制御可能な領域を外れた真の次状態を破棄する。

（動作計画処理手順）
次に、動作計画装置１の動作計画処理手順について説明する。制御部２は次状態探索プログラムに従って、図６に示す動作計画処理手順ＲＴ１の開始ステップから入ってステップＳＰ２へ移る。制御部２は、まずユーザによって入力された制御対象物の現在状態及び目標状態を受け取るとステップＳＰ４へ移る。

ステップＳＰ４において制御部２は、運動方程式を線形化した式を求めステップＳＰ６へ移る。実際上、制御部２はステップＳＰ４において第１探索処理プログラムを実行する。制御部２は、上述した通り制御対象の現在状態の慣性行列Ａ_０ ^−１と、現在状態にある制御対象に対しトルクを０とした場合の次状態の慣性行列Ａ_１ ^−１を動力学計算により求め、慣性行列Ａ_０ ^−１と慣性行列Ａ_１ ^−１の平均を算出してＢ^−１とする。

ステップＳＰ６において制御部２は、ステップＳＰ４で求めた運動方程式を線形化した式を用いて次状態候補を求め、ステップＳＰ８へ移る。制御部２は、現在状態と次状態の間の慣性行列が一定、すなわち式Ｂ^−１であるとみなすことにより、次状態候補を簡便に求めることができると共に、次状態候補を位相空間上により均等に分布させることができる。

ステップＳＰ８において制御部２は、枝刈り処理プログラムを実行する。制御部２は次状態記憶部４から既に記憶された登録次状態を読み出し、当該登録次状態からの距離ｄの範囲内に次状態候補があるか否かを判断する。

ステップＳＰ８において肯定結果が得られると、このことはステップＳＰ６で求められた次状態候補が既に記憶された登録次状態の近傍にあることを表しており、このとき制御部２はステップＳＰ１０へ移る。

ステップＳＰ１０において制御部２は、登録次状態からの距離ｄの範囲内にある次状態候補を破棄し、ステップＳＰ１６へ移る。

ステップＳＰ８において否定結果が得られると、このことはステップＳＰ６で求められた次状態候補が既に記憶された登録次状態から十分に離れた位置にあることを表し、このとき制御部２はステップＳＰ１２へ移る。

ステップＳＰ１２において制御部２は、登録次状態からの距離ｄの範囲内にないと判断した次状態候補に対し第２探索処理プログラムを実行する。制御部２は、式Ｂ^−１を用いて求めた次状態候補に対し、運動方程式を用いて動力学計算を行い、実際の制御対象物の動作となり得る真の次状態を求め、ステップＳＰ１４へ移る。

ステップＳＰ１４において制御部２は、ステップＳＰ１２で求めた真の次状態を登録次状態として次状態記憶部４に記憶させ、ステップＳＰ１６へ移る。実際上、制御部２は真の次状態について制御可否判定を行い、制御可能な領域にある真の次状態のみを登録次状態として次状態記憶部４に記憶させる。

ステップＳＰ１６において制御部２は、探索が終了したか否かを判断する。次状態記憶部４に登録された全ての登録次状態から探索が行われたか否かを判断し、行われていれば肯定とする。ステップＳＰ１６において肯定結果が得られると、このことは次状態が目標状態まで到達したことを表し、このとき制御部２はステップＳＰ１８へ移る。

ステップＳＰ１６において否定結果が得られると、このことは次状態が未だ目標状態まで到達していないことを表し、このとき制御部２はステップＳＰ４へ戻る。これにより制御部２は、目標状態に到達するまでステップＳＰ４からステップＳＰ１６までの処理ループを繰り返す。

ステップＳＰ１８において制御部２は、次状態記憶部４に記憶された登録次状態群の中から、現在状態から目標状態に到達するまでの最適な登録次状態を選択し、動作を計画して、ステップＳＰ２０へ移る。ステップＳＰ２０において制御部２は、計画運動を実行し、ステップＳＰ２０へ移り動作計画処理手順を終了する。

本実施形態に係る動作計画装置１は、運動方程式を用いて真の次状態を求める前に、仮の次状態となる次状態候補を求める。動作計画装置１は、運動方程式を線形化した式を用いて次状態候補を求めることとしたので、次状態候補をより均一に分布させることができる。そうすると、動作計画装置１は、従来に比べ探索されない空間、及び重複して探索されてしまう空間を少なくすることができるので、精度を向上すると共に計算時間を短縮することができる。したがって、動作計画装置１は、効率的に最適な動作を計画することができる。

（直列計算処理手順と並列計算処理手順）
次に、直列計算部１０における直列計算処理手順と並列計算部１２における並列計算処理手順について図７を参照して説明する。

まずステップＳＰ２４において直列計算部１０は、制御対象の現在状態における末端の状態に係る情報（末端状態群の情報）を並列計算部１２へ送信する。並列計算部１２は末端状態群の情報を直列計算部１０から受信すると、ステップＳＰ２６において慣性行列計算を実行する。実際上、並列計算部１２は、受信した末端状態群の各状態に対し個別のスレッドを用いて並行して慣性行列計算を実行する。並列計算部１２は、慣性行列計算が終了したら、ステップＳＰ２８へ移る。

ステップＳＰ２８において並列計算部１２は、ステップＳＰ２６で求めた運動方程式を線形化した式を用いて次状態候補予測を実行する。実際上、並列計算部１２は、各状態に対し個別のスレッドを用いて並行して次状態予測を実行する。並列計算部１２は次状態予測が終了したら、ステップＳＰ３０へ移る。ステップＳＰ３０において制御部２は、ステップＳＰ２８で求めた次状態候補を直列計算部１０へ送信する。

直列計算部１０はステップＳＰ３２において次状態候補を並列計算部１２から受信すると、次状態記憶部４から既に記憶された登録次状態を読み出し、次状態候補に対し枝刈りを実行し、ステップＳＰ３４へ移る。ステップＳＰ３４において直列計算部１０は、枝刈りされなかった次状態候補を並列計算部１２へ送信し、直列計算処理手順を終了する。

並列計算部１２はステップＳＰ３８において、受信した次状態候補に対し動力学計算を行い、真の次状態を求める。実際上、並列計算部１２は、各次状態候補に対し個別のスレッドを用いて並行して動力学計算を行い真の次状態を求める。並列計算部１２は動力学計算が終了したら、真の次状態を登録次状態として次状態記憶部４に記憶させ、ステップＳＰ４０へ移り、並列計算処理手順を終了する。実際上、並列計算部１２は真の次状態について制御可否判定を行い、制御可能な領域にある真の次状態のみを登録次状態として次状態記憶部４に記憶させる。

上記したように、制御部２は直列計算部１０と並列計算部１２とで各種情報を送信し合いながら処理を行うことにより、計算時間を短縮することができる。本実施形態の場合、直列計算部１０において枝刈りをし、既に記憶されている登録次状態と重複していないことが確認された次状態候補に対してのみ並列計算部１２において真の次状態を求めるので、無駄な計算を省いて計算時間を短縮することができる。

また、動作計画装置１は、動力学計算に例えばＡｒｔｉｃｕｌａｔｅｄ Ｂｏｄｙ Ａｌｇｏｒｉｔｈｍ（ＡＢＡ）を用いることにより、真の次状態をより効率的に求めることができる。

上記実施形態では、直列計算部１０と並列計算部１２は直接各種情報を送受信する場合について説明したが、本発明はこれに限らず、例えば図１に示すように直列計算部１０と並列計算部１２が共に参照することができるメモリ６を介して各種情報を送受信することとしてもよい。

このように、動力学計算にＡＢＡを用い、メモリ６を用いて直列計算部１０と並列計算部１２が各種情報を送受信することにより、より計算時間を短縮することができる。

（実施例）
次に本発明に係る実施例について説明する。本実施例では、２自由度倒立振子と３自由度倒立振子の動作を求めた。また、比較例として上記特許文献１に示す方法により動作を求めた。

図８に示す３自由度倒立振子４０は、第１関節部４１、第２関節部４３、第３関節部４５を備える。第１関節部４１は、第１リンク４２と当該第１リンク４２の基端を軸支する第１関節４８とを有する。第２関節部４３は、第２リンク４４と当該第２リンク４４の基端を前記第１リンク４２の先端に軸支する第２関節５０とを有する。第３関節部４５は、第３リンク４６と当該第３リンク４６の基端を前記第２リンク４４の先端に軸支する第３関節５２とを有する。因みに２自由度倒立振子は特に図示しないが、第１関節部４１と第２関節部４３とからなる。

本実施例では、図９Ａに示す初期状態から図９Ｂに示す目標状態に到達するまでの動作を求めシミュレーションにより実行した。シミュレーションの設定は下表の通りである。タイムステップは、制御対象の制御周期に相当する。トルク切り替えタイムステップは、次の状態までの時間間隔に相当する。

第１関節部４１、第２関節部４３、第３関節部４５の各パラメータを下表に示す。下表において第１関節部４１の最大トルク２[Ｎｍ]とは次状態候補を求める際に入力するトルクの最大値である。また、探索木作成時のトルク粒度１０とは、比較例において探索木の新たな枝を派生させる際に最大から最小までのトルクの範囲を１０分割し、各トルクを運動方程式に代入して次状態を求めることを意味する。

図１０に２自由度倒立振子の結果、図１１に３自由度倒立振子４０の結果を示す。本図中、手法Ａは比較例に係る方法であり特許文献１の手法をＣＰＵ単体で構成した場合の結果である。手法Ｂは実施例に係る方法であり制御部２を直列計算部としてのＣＰＵ単体で構成した場合、手法Ｃは実施例に係る方法であり制御部２を直列計算部としてのＣＰＵ及び並列計算部としてのＧＰＵで構成した場合の結果である。

本結果から、本実施例はいずれも比較例に比べ格段と計算時間及び目標状態に到達する前の時間を短縮できることが確認できた。特に、３自由度倒立振子４０では、本実施例に係る手法Ｃは比較例に係る手法Ａに対し、同精度で比較して１／１０００以下に計算時間を短縮することができた。

また、制御部２をＣＰＵ及びＧＰＵで構成することにより、計算時間及び目標状態に到達する前の時間をより短縮することができる。また、自由度が増すにつれ、本実施例は比較例に比べ計算時間及び目標状態に到達する前の時間をより短縮することができる。

この結果から、本実施形態に係る動作計画装置１は、６自由度倒立振子でも同様に従来に比べ計算時間及び目標状態に到達する前の時間を大幅に短縮できることが予測される。したがって、動作計画装置１は、ロボットアーム、ヘリコプター、自動車など６自由度の機械装置においても最適な動作を計画することができる。

（変形例）
本発明は上記実施形態に限定されるものではなく、本発明の趣旨の範囲内で適宜変更することが可能である。

上記実施形態では、仮の慣性行列が、現在状態ｓ_０の慣性行列Ａ_０ ^−１と次状態ｓ_１の慣性行列Ａ_１ ^−１の平均であるＢ^−１とした場合について説明したが、本発明はこれに限らず、現在状態ｓ_０から次状態ｓ_１まで慣性行列を積分した式を用いてもよい。
上記実施形態では、次状態候補に対し枝刈りをする場合について説明したが、本発明はこれに限らず、枝刈りをせずに真の次状態を求めることとしてもよい。

上記実施形態では、制御部２が直列計算部１０と並列計算部１２とを有する場合について説明したが、本発明はこれに限らず、制御部２が並列計算部１２を有していなくともよい。

上記実施形態では、直列計算部が枝刈りのみをする場合について説明したが、本発明はこれに限らず、並列計算部で行う動力学計算の一部を直列計算部で行うこととしてもよい。特に、制御対象物の非線形性が強いときに有効である。

１ ：動作計画装置
２ ：制御部
４ ：次状態記憶部
５ ：線形化式記憶部
６ ：メモリ
１０ ：直列計算部
１２ ：並列計算部

Claims (28)

1. 位相空間における制御対象の現在状態を探索木の根として定め、前記制御対象の次状態を探索木の枝として求め、前記制御対象の現在状態から目標状態までの位置と速度の履歴を探索木に基づいて計画する動作計画装置であって、
   運動方程式を線形化した式を求め、
   前記運動方程式を線形化した式を用いて次状態候補を求める第１探索と、
   前記運動方程式を用いて前記次状態候補から前記次状態を求める第２探索と
   を行うことを特徴とする動作計画装置。
2. 前記第２探索で求めた前記次状態を登録次状態として記憶する次状態記憶部を有し、
   前記登録次状態からの距離が所定範囲内にある前記次状態候補を枝刈りし、枝刈りされなかった前記次状態候補に対し前記第２探索を実行して求めた前記次状態を前記登録次状態として前記次状態記憶部に記憶する
   ことを特徴とする請求項１に記載の動作計画装置。
3. 前記運動方程式を線形化した式を記憶する線形化式記憶部を有し、
   前記線形化式記憶部は前記運動方程式から計算した慣性行列を含む式を記憶することを特徴とする請求項２に記載の動作計画装置。
4. 前記線形化式記憶部は前記運動方程式から計算した前記次状態の値を含む式を記憶することを特徴とする請求項３に記載の動作計画装置。
5. 前記第１探索と前記第２探索の少なくとも一方を実行する並列計算部を備えることを特徴とする請求項２に記載の動作計画装置。
6. 枝刈りを実行する直列計算部を備え、
   前記直列計算部は、前記並列計算部から前記次状態候補に係る情報を受信すると共に、前記次状態候補に対し枝刈りを行い、枝刈りされなかった前記次状態候補に係る情報を前記並列計算部へ送信することを特徴とする請求項５に記載の動作計画装置。
7. 前記並列計算部は、前記次状態候補に対し順運動学計算と、前記次状態に対し制御可否判定を行うことを行うことを特徴とする請求項６に記載の動作計画装置。
8. 前記並列計算部は、前記次状態候補に対しArticulated Body Algorithm (ABA)を用いて順運動学計算を行うことを特徴とする請求項６又は７に記載の動作計画装置。
9. 前記直列計算部は、前記並列計算部で行う順運動学計算の一部の計算を行うことを特徴とする請求項８に記載の動作計画装置。
10. 並列計算部はＧＰＵであり、
    計算に必要な前記制御対象に係る情報を記憶するメモリを備える
    ことを特徴とする請求項５〜９のいずれか１項に記載の動作計画装置。
11. 位相空間における制御対象の現在状態を探索木の根として定め、前記制御対象の次状態を探索木の枝として求め、前記制御対象の現在状態から目標状態までの位置姿勢動作を探索木に基づいて計画する動作計画方法であって、
    運動方程式を線形化した式を求めるステップと、
    運動方程式を線形化した式を用いて次状態候補を求める第１探索ステップと、
    運動方程式を用いて前記次状態候補から前記次状態を求める第２探索ステップと
    を備えることを特徴とする動作計画方法。
12. 前記第１探索ステップの後、既に記憶された登録次状態から所定範囲内にある前記次状態候補を枝刈りするステップと、
    枝刈りされなかった前記次状態候補に対し前記第２探索ステップを実行して求めた前記次状態を前記登録次状態として記憶するステップと
    を有することを特徴とする請求項１１に記載の動作計画方法。
13. 前記運動方程式を線形化した式を記憶する線形化式記憶ステップを有し、
    前記線形化式記憶ステップは前記運動方程式から計算した慣性行列を含む式を記憶することを特徴とする請求項１２に記載の動作計画方法。
14. 前記線形化式記憶ステップは前記運動方程式から計算した前記次状態の値を含む式を記憶することを特徴とする請求項１３に記載の動作計画方法。
15. 前記第１探索ステップ又は前記第２探索ステップを実行する並列計算ステップを備えることを特徴とする請求項１２に記載の動作計画方法。
16. 枝刈りを実行する直列計算ステップを備え、
    前記直列計算ステップは、前記並列計算ステップで得られた前記次状態候補に係る情報に基づき、前記次状態候補に対し枝刈りを行い、枝刈りされなかった前記次状態候補に係る情報を出力し、
    前記並列計算ステップは、前記直列計算ステップで得られた前記次状態候補に係る情報に基づき前記第２探索ステップを実行する
    ことを特徴とする請求項１５に記載の動作計画方法。
17. 前記並列計算ステップは、前記次状態候補に対し順運動学計算と、前記次状態に対し制御可否判定を行うことを特徴とする請求項１６に記載の動作計画方法。
18. 前記並列計算ステップは、前記次状態候補に対しArticulated Body Algorithm (ABA)を用いて順運動学計算を行うことを特徴とする請求項１６又は１７に記載の動作計画方法。
19. 前記直列計算ステップは、前記並列計算ステップで行う順運動学計算の一部の計算を行うことを特徴とする請求項１８に記載の動作計画方法。
20. コンピュータに対して、
    位相空間における制御対象の現在状態を探索木の根として定め、前記制御対象の次状態を探索木の枝として求め、前記制御対象の現在状態から目標状態までの位置姿勢動作を探索木に基づいて計画する動作計画処理を実行させる動作計画処理プログラムであって、
    運動方程式を線形化した式を求めるステップと、
    運動方程式を線形化した式を用いて次状態候補を求める第１探索ステップと、
    運動方程式を用いて前記次状態候補から前記次状態を求める第２探索ステップと
    を実行させることを特徴とする動作計画処理プログラム。
21. 前記第１探索ステップの後、既に記憶された登録次状態から所定範囲内にある前記次状態候補を枝刈りするステップと、
    枝刈りされなかった前記次状態候補に対し前記第２探索ステップを実行して求めた前記次状態を前記登録次状態として記憶するステップと
    を有することを特徴とする請求項２０に記載の動作計画処理プログラム。
22. 前記運動方程式を線形化した式を記憶する線形化式記憶ステップを有し、
    前記線形化式記憶ステップは前記運動方程式から計算した慣性行列を含む式を記憶することを特徴とする請求項２１に記載の動作計画処理プログラム。
23. 前記線形化式記憶ステップは前記運動方程式から計算した前記次状態の値を含む式を記憶することを特徴とする請求項２２に記載の動作計画処理プログラム。
24. 前記第１探索ステップ又は前記第２探索ステップを実行する並列計算ステップを備えることを特徴とする請求項２１に記載の動作計画処理プログラム。
25. 枝刈りを実行する直列計算ステップを備え、
    前記直列計算ステップは、前記並列計算ステップで得られた前記次状態候補に係る情報に基き、前記次状態候補に対し枝刈りを行い、枝刈りされなかった前記次状態候補に係る情報を出力し、
    前記並列計算ステップは、前記直列計算ステップで得られた前記次状態候補に係る情報に基づき前記第２探索ステップを実行する
    ことを特徴とする請求項２４に記載の動作計画処理プログラム。
26. 前記並列計算ステップは、前記次状態候補に対し順運動学計算と、前記次状態に対し制御可否判定を行うことを特徴とする請求項２５に記載の動作計画処理プログラム。
27. 前記並列計算ステップは、前記次状態候補に対しArticulated Body Algorithm (ABA)を用いて順運動学計算を行うことを特徴とする請求項２５又は２６に記載の動作計画処理プログラム。
28. 前記直列計算ステップは、前記並列計算ステップで行う順運動学計算の一部の計算を行うことを特徴とする請求項２７に記載の動作計画処理プログラム。