(57)【要約】【課題】効率的に最適な動作を計画することができる動作計画装置、動作計画方法、動作計画処理プログラムを提供する。【解決手段】位相空間における制御対象の現在状態を探索木の根として定め、前記制御対象の次状態を探索木の枝として求め、前記制御対象の現在状態から目標状態までの位置姿勢動作を探索木に基づいて計画する動作計画装置１であって、運動方程式を線形化した式を求め、前記運動方程式を線形化した式を用いて次状態候補を求める第１探索と、前記運動方程式を用いて前記次状態候補から前記次状態を求める第２探索とを行うことを特徴とする。【選択図】図２
2013206043.tif 000002
【特許請求の範囲】
【請求項１】
位相空間における制御対象の現在状態を探索木の根として定め、前記制御対象の次状態を探索木の枝として求め、前記制御対象の現在状態から目標状態までの位置と速度の履歴を探索木に基づいて計画する動作計画装置であって、
運動方程式を線形化した式を求め、
前記運動方程式を線形化した式を用いて次状態候補を求める第１探索と、
前記運動方程式を用いて前記次状態候補から前記次状態を求める第２探索と
を行うことを特徴とする動作計画装置。
【請求項２】
前記第２探索で求めた前記次状態を登録次状態として記憶する次状態記憶部を有し、
前記登録次状態からの距離が所定範囲内にある前記次状態候補を枝刈りし、枝刈りされなかった前記次状態候補に対し前記第２探索を実行して求めた前記次状態を前記登録次状態として前記次状態記憶部に記憶する
ことを特徴とする請求項１に記載の動作計画装置。
【請求項３】
前記運動方程式を線形化した式を記憶する線形化式記憶部を有し、
前記線形化式記憶部は前記運動方程式から計算した慣性行列を含む式を記憶することを特徴とする請求項２に記載の動作計画装置。
【請求項４】
前記線形化式記憶部は前記運動方程式から計算した前記次状態の値を含む式を記憶することを特徴とする請求項３に記載の動作計画装置。
【請求項５】
前記第１探索と前記第２探索の少なくとも一方を実行する並列計算部を備えることを特徴とする請求項２に記載の動作計画装置。
【請求項６】
枝刈りを実行する直列計算部を備え、
前記直列計算部は、前記並列計算部から前記次状態候補に係る情報を受信すると共に、前記次状態候補に対し枝刈りを行い、枝刈りされなかった前記次状態候補に係る情報を前記並列計算部へ送信することを特徴とする請求項５に記載の動作計画装置。
【請求項７】
前記並列計算部は、前記次状態候補に対し順運動学計算と、前記次状態に対し制御可否判定を行うことを行うことを特徴とする請求項６に記載の動作計画装置。
【請求項８】
前記並列計算部は、前記次状態候補に対しArticulated Body Algorithm (ABA)を用いて順運動学計算を行うことを特徴とする請求項６又は７に記載の動作計画装置。
【請求項９】
前記直列計算部は、前記並列計算部で行う順運動学計算の一部の計算を行うことを特徴とする請求項８に記載の動作計画装置。
【請求項１０】
並列計算部はＧＰＵであり、
計算に必要な前記制御対象に係る情報を記憶するメモリを備える
ことを特徴とする請求項５〜９のいずれか１項に記載の動作計画装置。
【請求項１１】
位相空間における制御対象の現在状態を探索木の根として定め、前記制御対象の次状態を探索木の枝として求め、前記制御対象の現在状態から目標状態までの位置姿勢動作を探索木に基づいて計画する動作計画方法であって、
運動方程式を線形化した式を求めるステップと、
運動方程式を線形化した式を用いて次状態候補を求める第１探索ステップと、
運動方程式を用いて前記次状態候補から前記次状態を求める第２探索ステップと
を備えることを特徴とする動作計画方法。
【請求項１２】
前記第１探索ステップの後、既に記憶された登録次状態から所定範囲内にある前記次状態候補を枝刈りするステップと、
枝刈りされなかった前記次状態候補に対し前記第２探索ステップを実行して求めた前記次状態を前記登録次状態として記憶するステップと
を有することを特徴とする請求項１１に記載の動作計画方法。
【請求項１３】
前記運動方程式を線形化した式を記憶する線形化式記憶ステップを有し、
前記線形化式記憶ステップは前記運動方程式から計算した慣性行列を含む式を記憶することを特徴とする請求項１２に記載の動作計画方法。
【請求項１４】
前記線形化式記憶ステップは前記運動方程式から計算した前記次状態の値を含む式を記憶することを特徴とする請求項１３に記載の動作計画方法。
【請求項１５】
前記第１探索ステップ又は前記第２探索ステップを実行する並列計算ステップを備えることを特徴とする請求項１２に記載の動作計画方法。
【請求項１６】
枝刈りを実行する直列計算ステップを備え、
前記直列計算ステップは、前記並列計算ステップで得られた前記次状態候補に係る情報に基づき、前記次状態候補に対し枝刈りを行い、枝刈りされなかった前記次状態候補に係る情報を出力し、
前記並列計算ステップは、前記直列計算ステップで得られた前記次状態候補に係る情報に基づき前記第２探索ステップを実行する
ことを特徴とする請求項１５に記載の動作計画方法。
【請求項１７】
前記並列計算ステップは、前記次状態候補に対し順運動学計算と、前記次状態に対し制御可否判定を行うことを特徴とする請求項１６に記載の動作計画方法。
【請求項１８】
前記並列計算ステップは、前記次状態候補に対しArticulated Body Algorithm (ABA)を用いて順運動学計算を行うことを特徴とする請求項１６又は１７に記載の動作計画方法。
【請求項１９】
前記直列計算ステップは、前記並列計算ステップで行う順運動学計算の一部の計算を行うことを特徴とする請求項１８に記載の動作計画方法。
【請求項２０】
コンピュータに対して、
位相空間における制御対象の現在状態を探索木の根として定め、前記制御対象の次状態を探索木の枝として求め、前記制御対象の現在状態から目標状態までの位置姿勢動作を探索木に基づいて計画する動作計画処理を実行させる動作計画処理プログラムであって、
運動方程式を線形化した式を求めるステップと、
運動方程式を線形化した式を用いて次状態候補を求める第１探索ステップと、
運動方程式を用いて前記次状態候補から前記次状態を求める第２探索ステップと
を実行させることを特徴とする動作計画処理プログラム。
【請求項２１】
前記第１探索ステップの後、既に記憶された登録次状態から所定範囲内にある前記次状態候補を枝刈りするステップと、
枝刈りされなかった前記次状態候補に対し前記第２探索ステップを実行して求めた前記次状態を前記登録次状態として記憶するステップと
を有することを特徴とする請求項２０に記載の動作計画処理プログラム。
【請求項２２】
前記運動方程式を線形化した式を記憶する線形化式記憶ステップを有し、
前記線形化式記憶ステップは前記運動方程式から計算した慣性行列を含む式を記憶することを特徴とする請求項２１に記載の動作計画処理プログラム。
【請求項２３】
前記線形化式記憶ステップは前記運動方程式から計算した前記次状態の値を含む式を記憶することを特徴とする請求項２２に記載の動作計画処理プログラム。
【請求項２４】
前記第１探索ステップ又は前記第２探索ステップを実行する並列計算ステップを備えることを特徴とする請求項２１に記載の動作計画処理プログラム。
【請求項２５】
枝刈りを実行する直列計算ステップを備え、
前記直列計算ステップは、前記並列計算ステップで得られた前記次状態候補に係る情報に基き、前記次状態候補に対し枝刈りを行い、枝刈りされなかった前記次状態候補に係る情報を出力し、
前記並列計算ステップは、前記直列計算ステップで得られた前記次状態候補に係る情報に基づき前記第２探索ステップを実行する
ことを特徴とする請求項２４に記載の動作計画処理プログラム。
【請求項２６】
前記並列計算ステップは、前記次状態候補に対し順運動学計算と、前記次状態に対し制御可否判定を行うことを特徴とする請求項２５に記載の動作計画処理プログラム。
【請求項２７】
前記並列計算ステップは、前記次状態候補に対しArticulated Body Algorithm (ABA)を用いて順運動学計算を行うことを特徴とする請求項２５又は２６に記載の動作計画処理プログラム。
【請求項２８】
前記直列計算ステップは、前記並列計算ステップで行う順運動学計算の一部の計算を行うことを特徴とする請求項２７に記載の動作計画処理プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、動作計画装置、動作計画方法、動作計画処理プログラムに関するものである。
【背景技術】
【０００２】
動作計画とは、制御対象物としての自動車、ロボット、エネルギープラントなどの機械装置や、当該機械装置の一部の構成などを、ある位置・速度からある位置・速度まで動作させる場合の移動履歴を計画することをいう。したがって、制御対象物を動作させる際に要する時間やエネルギーを最小化できる最適な動作移動履歴を計画することが好ましいと共に、動作を計画する際に要する時間を最小化できることが好ましい。特に、動作速度に比べ小さいトルクで運動する機械装置では、位置や速度といった動作状態を急激に変化させることができないため、慣性力、遠心力やコリオリ力を考慮した上で動作を計画する必要があるので、最適化が難しい。
【０００３】
従来の動作計画方法としては、探索木を用いて非線形特性を有する機械装置の運動を探索、計画する方法が開示されている（例えば、特許文献１）。上記特許文献１は、図１２に示すように、物体１０２の状態空間における状態を探索木の根として求め、所定の時間間隔が経過した後の物体の次の状態１０３，１０４，１０５，１０６を探索木の枝として求める。この場合、状態空間を分割した格子中に各状態が配置される。次いで、次の状態１０３，１０４，１０５，１０６のうち、所定の条件を満たす次の状態１０４，１０５を枝刈りし、枝刈りされなかった次の状態１０３，１０６を登録木として登録する。同様に、次の状態１０６から時間間隔が経過した後の物体のさらに次の状態１０７，１０８，１０９，１１０を探索木の枝として求め、所定の条件を満たす次の状態を枝刈りし、枝刈りされなかった次の状態を登録木として登録する。このように、順々に次の状態から登録木を伸ばしていき、登録木として登録された状態の中から適切な一連の状態を選択することによって動作を求める。これにより、上記特許文献１では、計算コストを抑えて動作を計画することができる。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１１−１６１６２４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、上記特許文献１の場合、所定の時間間隔が経過した後の物体の次の状態は、トルク粒度を一定にして求めるため、格子１１１，１１２のように探索されない格子が生じたり、１つの格子中に複数の状態が配置されたりしてしまう。探索されない格子が生じると、精度が悪化するので最適な動作を計画することが困難になる、という問題がある。また、１つの格子中に複数の状態が配置されると、その配置を行うだけの計算時間を要するので、動作を計画する際に要する時間を最小化することが困難になる、という問題がある。
【０００６】
そこで本発明は上記した問題点に鑑み、効率的に最適な動作を計画することができる動作計画装置、動作計画方法、動作計画処理プログラムを提供することを目的とする。
【課題を解決するための手段】
【０００７】
本発明の請求項１に係る動作計画装置は、位相空間における制御対象の現在状態を探索木の根として定め、前記制御対象の次状態を探索木の枝として求め、前記制御対象の現在状態から目標状態までの位置と速度の履歴を探索木に基づいて計画する動作計画装置であって、運動方程式を線形化した式を求め、前記運動方程式を線形化した式を用いて次状態候補を求める第１探索と、前記運動方程式を用いて前記次状態候補から前記次状態を求める第２探索とを行うことを特徴とする。
【０００８】
本発明の請求項１１に係る動作計画方法は、位相空間における制御対象の現在状態を探索木の根として定め、前記制御対象の次状態を探索木の枝として求め、前記制御対象の現在状態から目標状態までの位置姿勢動作を探索木に基づいて計画する動作計画方法であって、運動方程式を線形化した式を求めるステップと、運動方程式を線形化した式を用いて次状態候補を求める第１探索ステップと、運動方程式を用いて前記次状態候補から前記次状態を求める第２探索ステップとを備えることを特徴とする。
【０００９】
本発明の請求項２０に係る動作計画処理プログラムは、コンピュータに対して、位相空間における制御対象の現在状態を探索木の根として定め、前記制御対象の次状態を探索木の枝として求め、前記制御対象の現在状態から目標状態までの位置姿勢動作を探索木に基づいて計画する動作計画処理を実行させる動作計画処理プログラムであって、運動方程式を線形化した式を求めるステップと、運動方程式を線形化した式を用いて次状態候補を求める第１探索ステップと、運動方程式を用いて前記次状態候補から前記次状態を求める第２探索ステップとを実行させることを特徴とする。
【発明の効果】
【００１０】
本発明の請求項１，１１，２０によれば、運動方程式を用いて真の次状態を求める前に、仮の次状態となる次状態候補を求めることとしたので、次状態候補をより均一に分布させることができ、従来に比べ探索されない空間、及び重複して探索されてしまう空間を少なくすることができる。したがって、効率的に最適な動作を計画することができる。
【図面の簡単な説明】
【００１１】
【図１】本実施形態に係る動作計画装置の全体構成を模式的に示すブロック図である。
【図２】本実施形態に係る動作計画装置のステップＳＰ４の動作を示す模式図である。
【図３】本実施形態に係る動作計画装置のステップＳＰ６の動作を示す模式図である。
【図４】本実施形態に係る動作計画装置のステップＳＰ８及びステップＳＰ１０の動作を示す模式図である。
【図５】本実施形態に係る動作計画装置のステップＳＰ１２の動作を示す模式図である。
【図６】本実施形態に係る動作計画装置における動作計画処理手順を示すフローチャートである。
【図７】本実施形態に係る直列計算部と並列計算部における処理内容を示すフローチャートである。
【図８】実施例に係る３自由度倒立振子の構成を示す模式図である。
【図９】実施例に係る３自由度倒立振子の状態を示す図であり、図９Ａは初期状態、図９Ｂは目標状態を示す図である。
【図１０】実施例に係る２自由度倒立振子のシミュレーションの結果を示すグラフである。
【図１１】実施例に係る３自由度倒立振子のシミュレーションの結果を示すグラフである。
【図１２】従来例に係る探索の手法を示す図である。
【発明を実施するための形態】
【００１２】
以下、図面を参照して本発明の実施形態について詳細に説明する。
【００１３】
（全体構成）
図１に示す動作計画装置１は、当該動作計画装置１の各種機能を統括的に制御する制御部２、次状態記憶部４、線形化式記憶部５、メモリ６が制御バス８を介して接続されている。動作計画装置１は、全体として位相空間における制御対象の現在状態を探索木の根として定め、前記制御対象の次状態を探索木の枝として求め、前記制御対象の現在状態から目標状態までの位置と速度の履歴を探索木に基づいて計画する。位相空間とは、制御対象の位置（姿勢）と速度（角速度）で記述された空間をいう。
【００１４】
制御部２は、予め格納されている基本プログラムや、次状態探索プログラムなどの各種プログラムを読み出して、これら各種プログラムに従って動作計画装置１全体を制御する。本実施形態の場合、制御部２は、直列計算部（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ：ＣＰＵ）１０と並列計算部（Ｇｒａｐｈｉｃｓ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ：ＧＰＵ）１２とを有する。
【００１５】
制御部２は、運動方程式を線形化した式を用いて次状態候補を求める第１探索と、運動方程式を用いて前記次状態候補から真の次状態を求める第２探索とを行う。制御部２は、当該第２探索の結果得られた真の次状態ごとに上記第１探索と第２探索を行ってさらに真の次状態を求める。このように制御部２は第１探索と第２探索を所定の時間間隔毎に繰り返し行い、得られた真の次状態から最適な動作を求める。
【００１６】
また、制御部２は、第１探索の後、一定条件を満たす次状態候補を枝刈りすることにより、第２探索を行う次状態候補を減らすことにより、全体の計算量を減らすこととしてもよい。
【００１７】
第１探索は、慣性行列計算と、次状態予測とを含む。慣性行列計算は、図２に示すように、制御部２は、制御対象の現在状態ｓ０の慣性行列Ａ０−１と、現在状態にある制御対象に対しトルクを０とした場合の次状態ｓ１の慣性行列Ａ１−１を動力学計算で求める。
まず、制御部２は、運動方程式を用いて慣性行列を求める。二自由度倒立振子の運動方程式は、下記式の通りである。
【００１８】
【数１】
2013206043.tif 000003
【００１９】
ｌ１は第１リンクの長さ、ｍ１は第１リンクの質量、θ１は第１リンクの角度、ｌ２は第２リンクの長さ、ｍ２は第２リンクの質量、θ２は第２リンクの角度である。また、重力加速度をｇとする。さらに一般化すると下記式となる。
【００２０】
【数２】
2013206043.tif 000004
τはトルクである。それぞれの項は下記式の通りである。
【００２１】
【数３】
2013206043.tif 000005
【００２２】
【数４】
2013206043.tif 000006
【００２３】
【数５】
2013206043.tif 000007
【００２４】
上記の結果から、制御部２は下記式（運動方程式から計算した次状態の値を含む式）を求め、線形化式記憶部５に記憶する。制御部２は、下記式により慣性行列Ａ−１を求める。
【００２５】
【数６】
2013206043.tif 000008
【００２６】
上記のようにして現在状態ｓ０の慣性行列Ａ０−１と、トルク０の場合の次状態ｓ１の慣性行列Ａ１−１を求めることにより、制御部２は、現在状態における制御対象物が有する慣性のパラメータと、次状態における制御対象物が有する慣性のパラメータとを求めることができる。制御部２は、慣性行列Ａ０−１とＡ１−１とから仮の慣性行列Ｂ−１（運動方程式から計算した慣性行列を含む式）を求め、線形化式記憶部５に記憶する。本実施形態の場合、Ｂ−１は、下記式の通り慣性行列Ａ０−１とＡ１−１の平均である。
【００２７】
【数７】
2013206043.tif 000009
【００２８】
本実施形態では、現在状態ｓ０と次状態ｓ１の間において慣性行列が一定（Ｂ−１）であるとみなして、図３に示すように次状態予測を行う。本図において制御部２は、Ｂ−１を用いてトルクτを入力し、次状態候補を求める。この場合、トルクτの限界値がある一定値に抑えられると、現在状態から到達できる次状態の範囲は、慣性行列が一定であると仮定したことにより、超並行四辺形（hyper parallelogram）１４となる。本実施形態の場合、次状態候補は運動方程式を線形化した式である下記式を用いて求める。
【００２９】
【数８】
2013206043.tif 000010
【００３０】
制御部２は、上記式のトルクτに最大トルク値を入力して次状態候補を求める。最大トルク値はベクトルを用い具体的には、（τ１ｍａｘ，０）、（-τ１ｍａｘ，０）、（０，τ２ｍａｘ）、（０，-τ２ｍａｘ）、（τ１ｍａｘ，τ２ｍａｘ）、（-τ１ｍａｘ，τ２ｍａｘ）、（τ１ｍａｘ，-τ２ｍａｘ）、（-τ１ｍａｘ，-τ２ｍａｘ）を用いる。このようにして制御部２は図４に示すように、超並行四辺形（hyper parallelogram）１４上に複数、本図の場合８個の次状態候補を求める。本実施形態の場合、次状態候補は、トルク０の場合の次状態ｓ１を含めると全部で９個である。実際上、制御部２は制御対象の制約条件、例えば位置、速度、トルク限界を考慮して制御可能な領域に次状態候補を求める。
枝刈りは、特に限定されるものではないが、本実施形態では既に記憶されている登録次状態から一定距離にある次状態候補を破棄する方法について説明する。
【００３１】
制御部２は、次状態記憶部４に既に記憶された登録次状態に係る情報を読み出す。そして制御部２は、次状態候補が登録次状態から距離ｄの範囲内に定められているか否かを判断し、範囲内にある次状態候補を破棄する。
【００３２】
本図の場合制御部２は、既に記憶された登録次状態１６，１８を次状態記憶部４から読み出し、次状態候補との距離を計測する。そして制御部２は、次状態候補２０Ｃ，２２Ｃが登録次状態１６から距離ｄの範囲内に定められているので、次状態候補２０Ｃ，２２Ｃを破棄する。同様に制御部２は、次状態候補２４Ｃが登録次状態１８から距離ｄの範囲内に定められているので、次状態候補２４Ｃを破棄する。
【００３３】
第２探索は、枝刈りされなかった次状態候補に対し行われる。すなわち制御部２は、次状態１６，１８からいずれも距離ｄの範囲内にない次状態候補２６Ｃ，２８Ｃ，３０Ｃ，３２Ｃ，３４Ｃに対し、動力学計算を行い、図５に示すように真の次状態２６Ｒ，２８Ｒ，３０Ｒ，３２Ｒ，３４Ｒを求める。
【００３４】
次状態候補２６Ｃ，２８Ｃ，３０Ｃ，３２Ｃ，３４Ｃは、慣性行列が一定（Ｂ−１）とみなして求めたため、実際の制御対象物の動作と誤差が生じている。したがって、制御部２は、時間と共に変化するトルクτに対応した慣性行列を用いて次状態候補２６Ｃ，２８Ｃ，３０Ｃ，３２Ｃ，３４Ｃから真の次状態２６Ｒ，２８Ｒ，３０Ｒ，３２Ｒ，３４Ｒを求める。
【００３５】
真の次状態２６Ｒ，２８Ｒ，３０Ｒ，３２Ｒ，３４Ｒは、制御可否判定を経た後、次状態記憶部４に登録次状態として記憶される。制御可否判定は、真の次状態が制御可能な領域にあるか否かを判断する。制御部２は制御可能性を確認し、制御可能な領域を外れた真の次状態を破棄する。
【００３６】
（動作計画処理手順）
次に、動作計画装置１の動作計画処理手順について説明する。制御部２は次状態探索プログラムに従って、図６に示す動作計画処理手順ＲＴ１の開始ステップから入ってステップＳＰ２へ移る。制御部２は、まずユーザによって入力された制御対象物の現在状態及び目標状態を受け取るとステップＳＰ４へ移る。
【００３７】
ステップＳＰ４において制御部２は、運動方程式を線形化した式を求めステップＳＰ６へ移る。実際上、制御部２はステップＳＰ４において第１探索処理プログラムを実行する。制御部２は、上述した通り制御対象の現在状態の慣性行列Ａ０−１と、現在状態にある制御対象に対しトルクを０とした場合の次状態の慣性行列Ａ１−１を動力学計算により求め、慣性行列Ａ０−１と慣性行列Ａ１−１の平均を算出してＢ−１とする。
【００３８】
ステップＳＰ６において制御部２は、ステップＳＰ４で求めた運動方程式を線形化した式を用いて次状態候補を求め、ステップＳＰ８へ移る。制御部２は、現在状態と次状態の間の慣性行列が一定、すなわち式Ｂ−１であるとみなすことにより、次状態候補を簡便に求めることができると共に、次状態候補を位相空間上により均等に分布させることができる。
【００３９】
ステップＳＰ８において制御部２は、枝刈り処理プログラムを実行する。制御部２は次状態記憶部４から既に記憶された登録次状態を読み出し、当該登録次状態からの距離ｄの範囲内に次状態候補があるか否かを判断する。
【００４０】
ステップＳＰ８において肯定結果が得られると、このことはステップＳＰ６で求められた次状態候補が既に記憶された登録次状態の近傍にあることを表しており、このとき制御部２はステップＳＰ１０へ移る。
【００４１】
ステップＳＰ１０において制御部２は、登録次状態からの距離ｄの範囲内にある次状態候補を破棄し、ステップＳＰ１６へ移る。
【００４２】
ステップＳＰ８において否定結果が得られると、このことはステップＳＰ６で求められた次状態候補が既に記憶された登録次状態から十分に離れた位置にあることを表し、このとき制御部２はステップＳＰ１２へ移る。
【００４３】
ステップＳＰ１２において制御部２は、登録次状態からの距離ｄの範囲内にないと判断した次状態候補に対し第２探索処理プログラムを実行する。制御部２は、式Ｂ−１を用いて求めた次状態候補に対し、運動方程式を用いて動力学計算を行い、実際の制御対象物の動作となり得る真の次状態を求め、ステップＳＰ１４へ移る。
【００４４】
ステップＳＰ１４において制御部２は、ステップＳＰ１２で求めた真の次状態を登録次状態として次状態記憶部４に記憶させ、ステップＳＰ１６へ移る。実際上、制御部２は真の次状態について制御可否判定を行い、制御可能な領域にある真の次状態のみを登録次状態として次状態記憶部４に記憶させる。
【００４５】
ステップＳＰ１６において制御部２は、探索が終了したか否かを判断する。次状態記憶部４に登録された全ての登録次状態から探索が行われたか否かを判断し、行われていれば肯定とする。ステップＳＰ１６において肯定結果が得られると、このことは次状態が目標状態まで到達したことを表し、このとき制御部２はステップＳＰ１８へ移る。
【００４６】
ステップＳＰ１６において否定結果が得られると、このことは次状態が未だ目標状態まで到達していないことを表し、このとき制御部２はステップＳＰ４へ戻る。これにより制御部２は、目標状態に到達するまでステップＳＰ４からステップＳＰ１６までの処理ループを繰り返す。
【００４７】
ステップＳＰ１８において制御部２は、次状態記憶部４に記憶された登録次状態群の中から、現在状態から目標状態に到達するまでの最適な登録次状態を選択し、動作を計画して、ステップＳＰ２０へ移る。ステップＳＰ２０において制御部２は、計画運動を実行し、ステップＳＰ２０へ移り動作計画処理手順を終了する。
【００４８】
本実施形態に係る動作計画装置１は、運動方程式を用いて真の次状態を求める前に、仮の次状態となる次状態候補を求める。動作計画装置１は、運動方程式を線形化した式を用いて次状態候補を求めることとしたので、次状態候補をより均一に分布させることができる。そうすると、動作計画装置１は、従来に比べ探索されない空間、及び重複して探索されてしまう空間を少なくすることができるので、精度を向上すると共に計算時間を短縮することができる。したがって、動作計画装置１は、効率的に最適な動作を計画することができる。
【００４９】
（直列計算処理手順と並列計算処理手順）
次に、直列計算部１０における直列計算処理手順と並列計算部１２における並列計算処理手順について図７を参照して説明する。
【００５０】
まずステップＳＰ２４において直列計算部１０は、制御対象の現在状態における末端の状態に係る情報（末端状態群の情報）を並列計算部１２へ送信する。並列計算部１２は末端状態群の情報を直列計算部１０から受信すると、ステップＳＰ２６において慣性行列計算を実行する。実際上、並列計算部１２は、受信した末端状態群の各状態に対し個別のスレッドを用いて並行して慣性行列計算を実行する。並列計算部１２は、慣性行列計算が終了したら、ステップＳＰ２８へ移る。
【００５１】
ステップＳＰ２８において並列計算部１２は、ステップＳＰ２６で求めた運動方程式を線形化した式を用いて次状態候補予測を実行する。実際上、並列計算部１２は、各状態に対し個別のスレッドを用いて並行して次状態予測を実行する。並列計算部１２は次状態予測が終了したら、ステップＳＰ３０へ移る。ステップＳＰ３０において制御部２は、ステップＳＰ２８で求めた次状態候補を直列計算部１０へ送信する。
【００５２】
直列計算部１０はステップＳＰ３２において次状態候補を並列計算部１２から受信すると、次状態記憶部４から既に記憶された登録次状態を読み出し、次状態候補に対し枝刈りを実行し、ステップＳＰ３４へ移る。ステップＳＰ３４において直列計算部１０は、枝刈りされなかった次状態候補を並列計算部１２へ送信し、直列計算処理手順を終了する。
【００５３】
並列計算部１２はステップＳＰ３８において、受信した次状態候補に対し動力学計算を行い、真の次状態を求める。実際上、並列計算部１２は、各次状態候補に対し個別のスレッドを用いて並行して動力学計算を行い真の次状態を求める。並列計算部１２は動力学計算が終了したら、真の次状態を登録次状態として次状態記憶部４に記憶させ、ステップＳＰ４０へ移り、並列計算処理手順を終了する。実際上、並列計算部１２は真の次状態について制御可否判定を行い、制御可能な領域にある真の次状態のみを登録次状態として次状態記憶部４に記憶させる。
【００５４】
上記したように、制御部２は直列計算部１０と並列計算部１２とで各種情報を送信し合いながら処理を行うことにより、計算時間を短縮することができる。本実施形態の場合、直列計算部１０において枝刈りをし、既に記憶されている登録次状態と重複していないことが確認された次状態候補に対してのみ並列計算部１２において真の次状態を求めるので、無駄な計算を省いて計算時間を短縮することができる。
【００５５】
また、動作計画装置１は、動力学計算に例えばＡｒｔｉｃｕｌａｔｅｄ  Ｂｏｄｙ  Ａｌｇｏｒｉｔｈｍ（ＡＢＡ）を用いることにより、真の次状態をより効率的に求めることができる。
【００５６】
上記実施形態では、直列計算部１０と並列計算部１２は直接各種情報を送受信する場合について説明したが、本発明はこれに限らず、例えば図１に示すように直列計算部１０と並列計算部１２が共に参照することができるメモリ６を介して各種情報を送受信することとしてもよい。
【００５７】
このように、動力学計算にＡＢＡを用い、メモリ６を用いて直列計算部１０と並列計算部１２が各種情報を送受信することにより、より計算時間を短縮することができる。
【００５８】
（実施例）
次に本発明に係る実施例について説明する。本実施例では、２自由度倒立振子と３自由度倒立振子の動作を求めた。また、比較例として上記特許文献１に示す方法により動作を求めた。
【００５９】
図８に示す３自由度倒立振子４０は、第１関節部４１、第２関節部４３、第３関節部４５を備える。第１関節部４１は、第１リンク４２と当該第１リンク４２の基端を軸支する第１関節４８とを有する。第２関節部４３は、第２リンク４４と当該第２リンク４４の基端を前記第１リンク４２の先端に軸支する第２関節５０とを有する。第３関節部４５は、第３リンク４６と当該第３リンク４６の基端を前記第２リンク４４の先端に軸支する第３関節５２とを有する。因みに２自由度倒立振子は特に図示しないが、第１関節部４１と第２関節部４３とからなる。
【００６０】
本実施例では、図９Ａに示す初期状態から図９Ｂに示す目標状態に到達するまでの動作を求めシミュレーションにより実行した。シミュレーションの設定は下表の通りである。タイムステップは、制御対象の制御周期に相当する。トルク切り替えタイムステップは、次の状態までの時間間隔に相当する。
【００６１】
【表１】
2013206043.tif 000011
【００６２】
第１関節部４１、第２関節部４３、第３関節部４５の各パラメータを下表に示す。下表において第１関節部４１の最大トルク２[Ｎｍ]とは次状態候補を求める際に入力するトルクの最大値である。また、探索木作成時のトルク粒度１０とは、比較例において探索木の新たな枝を派生させる際に最大から最小までのトルクの範囲を１０分割し、各トルクを運動方程式に代入して次状態を求めることを意味する。
【００６３】
【表２】
2013206043.tif 000012
【００６４】
図１０に２自由度倒立振子の結果、図１１に３自由度倒立振子４０の結果を示す。本図中、手法Ａは比較例に係る方法であり特許文献１の手法をＣＰＵ単体で構成した場合の結果である。手法Ｂは実施例に係る方法であり制御部２を直列計算部としてのＣＰＵ単体で構成した場合、手法Ｃは実施例に係る方法であり制御部２を直列計算部としてのＣＰＵ及び並列計算部としてのＧＰＵで構成した場合の結果である。
【００６５】
本結果から、本実施例はいずれも比較例に比べ格段と計算時間及び目標状態に到達する前の時間を短縮できることが確認できた。特に、３自由度倒立振子４０では、本実施例に係る手法Ｃは比較例に係る手法Ａに対し、同精度で比較して１／１０００以下に計算時間を短縮することができた。
【００６６】
また、制御部２をＣＰＵ及びＧＰＵで構成することにより、計算時間及び目標状態に到達する前の時間をより短縮することができる。また、自由度が増すにつれ、本実施例は比較例に比べ計算時間及び目標状態に到達する前の時間をより短縮することができる。
【００６７】
この結果から、本実施形態に係る動作計画装置１は、６自由度倒立振子でも同様に従来に比べ計算時間及び目標状態に到達する前の時間を大幅に短縮できることが予測される。したがって、動作計画装置１は、ロボットアーム、ヘリコプター、自動車など６自由度の機械装置においても最適な動作を計画することができる。
【００６８】
（変形例）
本発明は上記実施形態に限定されるものではなく、本発明の趣旨の範囲内で適宜変更することが可能である。
【００６９】
上記実施形態では、仮の慣性行列が、現在状態ｓ０の慣性行列Ａ０−１と次状態ｓ１の慣性行列Ａ１−１の平均であるＢ−１とした場合について説明したが、本発明はこれに限らず、現在状態ｓ０から次状態ｓ１まで慣性行列を積分した式を用いてもよい。
上記実施形態では、次状態候補に対し枝刈りをする場合について説明したが、本発明はこれに限らず、枝刈りをせずに真の次状態を求めることとしてもよい。
【００７０】
上記実施形態では、制御部２が直列計算部１０と並列計算部１２とを有する場合について説明したが、本発明はこれに限らず、制御部２が並列計算部１２を有していなくともよい。
【００７１】
上記実施形態では、直列計算部が枝刈りのみをする場合について説明したが、本発明はこれに限らず、並列計算部で行う動力学計算の一部を直列計算部で行うこととしてもよい。特に、制御対象物の非線形性が強いときに有効である。
【符号の説明】
【００７２】
１        ：動作計画装置
２        ：制御部
４        ：次状態記憶部
５        ：線形化式記憶部
６        ：メモリ
１０      ：直列計算部
１２      ：並列計算部
【図１】
2013206043.tif 000013
【図２】
2013206043.tif 000014
【図３】
2013206043.tif 000015
【図４】
2013206043.tif 000016
【図５】
2013206043.tif 000017
【図６】
2013206043.tif 000018
【図７】
2013206043.tif 000019
【図８】
2013206043.tif 000020
【図９】
2013206043.tif 000021
【図１０】
2013206043.tif 000022
【図１１】
2013206043.tif 000023
【図１２】
2013206043.tif 000024