軌道計画方法、軌道制御方法、軌道計画システム及び軌道計画・制御システム
【課題】物体初期状態から目標状態までの軌道を、探索に制約を伴わずに少ない計算コスト、かつ高い精度で求めることのできる軌道計画システムを提供する。
【解決手段】軌道計画システムは、状態空間において、該目標状態を根とし、予め複数の区域に分割された状態空間のそれぞれの区域に含まれる枝のノードの数を制限することによって枝を集約した逆方向探索木を作成する探索木作成部１０１と、該状態空間において、該逆方向探索木上の点に対して、その領域内であれば、該逆方向探索木にしたがって該目標状態に到達しうる、移行可能領域を定める移行可能領域決定部１０３と、探索木及び移行可能領域を記憶する探索木記憶部１０４と、該逆方向探索木を使用して該移行可能領域内の点から該根までの、該物体の軌道を定める軌道作成部１０５と、を備える。
【解決手段】軌道計画システムは、状態空間において、該目標状態を根とし、予め複数の区域に分割された状態空間のそれぞれの区域に含まれる枝のノードの数を制限することによって枝を集約した逆方向探索木を作成する探索木作成部１０１と、該状態空間において、該逆方向探索木上の点に対して、その領域内であれば、該逆方向探索木にしたがって該目標状態に到達しうる、移行可能領域を定める移行可能領域決定部１０３と、探索木及び移行可能領域を記憶する探索木記憶部１０４と、該逆方向探索木を使用して該移行可能領域内の点から該根までの、該物体の軌道を定める軌道作成部１０５と、を備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、物体の、目標状態に至る状態遷移系列を作成する、軌道計画方法、軌道計画システム、作成された軌道にしたがって軌道を制御する軌道制御方法及び軌道計画・制御システムに関する。
【背景技術】
【０００２】
機械の状態（姿勢・速度）は、位相空間上の位置、機械の運動は位相空間上の経路として定めることができる。機械を初期状態から目標状態へと制御したい場合がある。具体例は、以下のとおりである。
【０００３】
第一の具体例は、二足型ロボットがバランスを崩した場合である。この場合の初期状態は、バランスを崩した直後の姿勢・速度であり、目標状態は安定姿勢（直立姿勢かつ速度０など）である。
【０００４】
第二の具体例は、航空機がバランスを崩し失速した場合である。この場合の初期状態は、バランスを崩した直後の姿勢・速度であり、目標状態は、翼とボディを水平にして等速で前進する状態である。
その他の具体例として、バイクが走行中に石に当たった際にバランスを崩し、姿勢を修正したい場合や、自動車がスリップした場合等がある。
【０００５】
機械を初期状態から目標状態へ制御する場合に、ＰＩＤ制御やポテンシャル法を使用すると、機械がバランスを崩した後の状態によっては適切に機能しないという問題がある。この問題は、これらの制御方法は、位相空間上の目標状態までの機械状態遷移において広い範囲の初期状態を取れないことに起因している。また、ＰＩＤ制御やポテンシャル法では、機械が目標状態へ到達するまでの時間を規定することが困難であり、短時間での目標状態復帰が困難である。
【０００６】
状態空間の軌道計画を作成する方法として、状態空間における状態遷移を探索木として記述する方法がある。この方法において、根または枝から、所定数の枝を派生させ、それぞれの枝を探索することが考えられる。しかし、派生させる枝の数をＮとし、木の深さをＭとすると、木の深さＭのレベルで探索の対象となる枝の数は、ＮＭとなる。したがって、派生させる全ての枝を探索する場合には計算コストが膨大となる。
【０００７】
そこで、探索木の枝の評価関数を定め、評価関数にしたがって選択した枝のみから枝を派生させる方法が提案されている（非特許文献１）。この方法によれば、計算コストは抑えられる。しかし、評価関数は、対象ごとに設計者が経験によって決める必要がある。評価関数によっては、状態空間上の探索を十分に行えない場合も生じうる。すなわち、この方法では、状態空間の探索に制約が生じうる。
【０００８】
上記の方法を含め探索木を使用する従来の方法は、リアルタイムの軌道計画作成及び制御に使用するには計算コスト（計算時間）が大きすぎるか、探索に制約が存在した。また、従来の方法は、物体の初期状態から目標状態までの軌道を高い精度で定めることが困難であった。
【先行技術文献】
【非特許文献】
【０００９】
【非特許文献１】Pedro S. Huang:”Planning For Dynamic Motions Using a Search Tree”, a graduate thesis of Toronto Univ., (1996).
【発明の概要】
【発明が解決しようとする課題】
【００１０】
したがって、物体、すなわち機械の種々の初期状態から目標状態までの軌道を、探索に制約を伴わずに少ない計算コスト、かつ高い精度で求めることのできる軌道計画方法、軌道計画システム、及び求めた軌道にしたがって目標状態までの軌道を制御することのできる軌道制御方法及び軌道計画・制御システムに対するニーズがある。
【課題を解決するための手段】
【００１１】
本発明の第１の態様による軌道計画方法は、軌道計画システムによって、物体の状態を目標状態へ制御するための軌道を求める軌道計画方法であって、探索木作成部が、状態空間において、該目標状態を根として、予め複数の区域に分割された該状態空間のそれぞれの区域に含まれる枝のノードの数を制限することによって枝を集約した逆方向探索木を作成するするステップと、移行可能領域決定部が、該状態空間において、該逆方向探索木上の点に対して、その領域内であれば、該逆方向探索木にしたがって該目標状態に到達しうる、移行可能領域を定めるステップと、軌道作成部が、該逆方向探索木を使用して、該状態空間における該移行可能領域内の点から該根までの、該物体の軌道を定めるステップと、を含む。
【００１２】
本態様の軌道計画方法において、探索に制約はない。また、逆方向探索木は区域を使用して集約されているので、計算コストは従来の方法に比較して少ない。さらに、逆方向探索木の根は、目標状態と一致するので、目標状態を終点とする軌道の精度は高い。したがって、本態様の軌道計画方法によれば、逆方向探索木上の点の移行可能領域内の点から目標状態までの軌道を、探索に制約を伴わずに少ない計算コスト、かつ高い精度で求めることができる。
【００１３】
本発明の第１の態様の第１の実施形態によれば、前記探索木作成部が、前記状態空間において、前記物体の初期状態を根として、前記状態空間の前記それぞれの区域に含まれる枝のノードの数を制限することによって枝を集約した順方向探索木を作成するするステップと、前記軌道作成部が、前記移行可能領域のいずれかにおいて、前記順方向探索木による軌道と前記逆方向探索木による軌道を接続して、前記物体の前記初期状態から前記目標状態までの軌道を定めるステップと、をさらに含む。
【００１４】
本実施形態によれば、移行可能領域において、順方向探索木による軌道と逆方向探索木による軌道を接続して、物体の初期状態から目標状態までの軌道を定めることができる。逆方向探索木の根は、目標状態と一致し、順方向探索木の根は、初期状態と一致するので、初期状態を起点とし目標状態を終点とするする軌道の精度は高い。
【００１５】
本発明の第１の態様の第２の実施形態によれば、前記移行可能領域を定めるステップにおいて、前記移行可能領域決定部は、前記逆方向探索木上の点の近傍の複数の点に対して、フィードバック制御を実施することにより、前記目標状態に到達しうるかどうか定め、その結果に基づいて前記移行可能領域を定める。
【００１６】
本実施形態によれば、フィードバック制御を実施することにより、確実に移行可能領域を定めることができる。
【００１７】
本発明の第１の態様の第３の実施形態によれば、前記移行可能領域決定部は、前記目標状態に到達することができなかった点のうち、前記探索木上の点に最も近い点の、前記探索木上の点までの距離に基づいて前記移行可能領域を定める。
【００１８】
本実施形態によれば、目標状態に到達することができなかった点のうち、逆方向探索木上の点に最も近い点の、逆方向探索木上の点までの距離を使用することにより、高い精度で移行可能領域を定めることができる。
【００１９】
本発明の第２の態様による軌道制御方法は、本発明の第１の態様による軌道計画方法によって求めた軌道にしたがって、前記物体を制御することによって前記物体の状態を前記目標状態とする。
【００２０】
本態様による軌道制御方法によれば、目標状態までの軌道を、探索に制約を伴わずに少ない計算コスト、かつ高い精度で求め、その軌道にしたがって物体の状態を目標状態とすることができる。
【００２１】
本発明の第３の態様による軌道計画システムは、物体の状態を目標状態へ制御するための軌道を求める軌道計画システムであって、状態空間において、該目標状態を根とし、予め複数の区域に分割された状態空間のそれぞれの区域に含まれる枝のノードの数を制限することによって枝を集約した逆方向探索木を作成する探索木作成部と、該状態空間において、該逆方向探索木上の点に対して、その領域内であれば、該逆方向探索木にしたがって該目標状態に到達しうる、移行可能領域を定める移行可能領域決定部と、探索木及び移行可能領域を記憶する探索木記憶部と、該逆方向探索木を使用して該状態空間における該移行可能領域内の点から該根までの、該物体の軌道を定める軌道作成部と、を備える。
【００２２】
本態様の軌道計画システムにおいて、探索に制約はない。また、逆方向探索木は区域を使用して集約されているので、計算コストは従来の方法に比較して少ない。さらに、逆方向探索木の根は、目標状態と一致するので、目標状態を終点とする軌道の精度は高い。したがって、本態様の軌道計画システムによれば、逆方向探索木上の点の移行可能領域内の点から目標状態までの軌道を、探索に制約を伴わずに少ない計算コスト、かつ高い精度で求めることができる。
【００２３】
本発明の第３の態様の第１の実施形態による軌道計画システムにおいては、前記探索木作成部が、前記状態空間において、前記物体の初期状態を根として、前記状態空間のそれぞれの区域に含まれる枝のノードの数を制限することによって枝を集約した順方向探索木を作成し、前記軌道作成部が、前記移行可能領域のいずれかにおいて、前記順方向探索木による軌道と前記逆方向探索木による軌道を接続して、前記物体の前記初期状態から前記目標状態までの軌道を定める。
【００２４】
本実施形態によれば、移行可能領域において、順方向探索木による軌道と逆方向探索木による軌道を接続して、物体の初期状態から目標状態までの軌道を定めることができる。逆方向探索木の根は、目標状態と一致し、順方向探索木の根は、初期状態と一致するので、初期状態を起点とし目標状態を終点とするする軌道の精度は高い。
【００２５】
本発明の第４の態様による、軌道計画・制御システムは、本発明の第２の態様による軌道計画システムと、該軌道計画システムが求めた軌道にしたがって、前記物体を制御することによって前記物体の状態を前記目標状態とする軌道制御部とを備えている。
【００２６】
本態様の軌道計画・制御システムによれば、目標状態までの軌道を、探索に制約を伴わずに少ない計算コスト、かつ高い精度で求め、その軌道にしたがって物体の状態を目標状態とすることができる。
【図面の簡単な説明】
【００２７】
【図１】本発明の一実施形態による軌道計画・制御システムの構成を示す図である。
【図２】二重倒立振子の構成を示す図である。
【図３】探索木作成部が位相空間上に探索木を作成する手順を示す流れ図である。
【図４】図３のステップＳ１０２０及びステップＳ１０３０の動作を説明するための図である。
【図５】複数の区域に分割された位相空間を示す図である。
【図６】図３のステップＳ１０５０及びステップＳ１０６０の動作を説明するための図である。
【図７】移行可能領域決定部が移行可能領域を定める方法を説明するための流れ図である。
【図８】図７のステップＳ２０２０の動作を説明するための図である。
【図９】図７のステップＳ２０３０及びステップＳ２０４０の動作を説明するための図である。
【図１０】図７のステップＳ２０６０を説明するための図である。
【図１１】軌道の作成方法を説明するための流れ図である。
【図１２】フィードバック制御を説明するための流れ図である。
【図１３】図１２のステップＳ４０１０乃至ステップＳ４０５０の動作を説明するための図である。
【図１４】二重倒立振子２００の目標状態を示す図である。
【図１５】図３の流れ図に示した方法によって求めた逆探索木による軌跡と、該軌跡上の点について図７の流れ図に示した方法によって作成した移行可能領域とを示す図である。
【図１６】図１５に示した逆方向探索木による軌跡と順方向探索木による軌跡とを移行可能領域において接続した状態を示す図である。
【図１７】順方向探索木による軌跡の移行点から、目標状態に向けてフィードバック制御を行った場合の軌跡を示す図である。
【発明を実施するための形態】
【００２８】
図１は、本発明の一実施形態による軌道計画・制御システム１００の構成を示す図である。軌道計画・制御システム１００は、状態空間上の物体の状態に対応する枝を備えた探索木を作成する探索木作成部１０１と、探索木上の点の移行可能領域を定める移行可能領域決定部１０３と、探索木作成部１０１によって作成された探索木及び移行可能領域を記憶する探索木記憶部１０４と、探索木記憶部１０４に記憶された探索木及び移行可能領域を使用して目標状態までの軌道を作成する軌道作成部１０５と、軌道作成部１０５によって作成された軌道を使用して軌道制御を行う軌道制御部１０７と、を備える。探索木上の点の移行可能領域については後で説明する。また、各構成部の機能の詳細については、後で説明する。
【００２９】
図２は、二重倒立振子２００の構成を示す図である。以下の実施形態においては、２次元平面内で運動する二重倒立振子２００を物体として説明を行なう。二重倒立振子２００は、第１リンク２０５と、第２リンク２１１と、第１リンク２０５を回転可能に固定点に接続する第１関節（ジョイント）２０１と、第１リンク２０５及び第２リンク２１１を回転可能に接続する第２関節２０７と、から構成される。第１リンク２０５及び第２リンク２１１の重心を、それぞれ符号２０３及び２０９によって示す。
【００３０】
二重倒立振子２００の運動は、第１関節２０１及び第２関節２０７に与えるトルクによって定まり、第１リンク２０５のＸ軸に対する角度
【数１】
第２リンク２１１の第１リンク２０５に対する角度
【数２】
及びこれらの角速度
【数３】
によって表現される。そこで、本実施形態において、状態空間として以下の位相空間を採用する。
【数４】
【００３１】
図３は、探索木作成部１０１が位相空間上に探索木を作成する手順を示す流れ図である。
【００３２】
図３のステップＳ１０１０において、探索木作成部１０１は物体の目標状態を得る。
【００３３】
図３のステップＳ１０２０において、探索木作成部１０１は、位相空間において物体の目標状態を探索木の根として定める。
【００３４】
図３のステップＳ１０３０において、探索木作成部１０１が、位相空間において、所定の時間間隔前、すなわち、時刻ｔ−Δｔの状態を探索木の枝のノードとして定める。枝のノードとは枝が分岐する点または枝の先端の点である。所定の時間間隔前の状態は、運動方程式や動力学シミュレータによって定める。上述のように、二重倒立振子２００の運動は、第１関節２０１及び第２関節２０７に与えるトルクによって定まるので、所定の時間間隔に第１関節２０１及び第２関節２０７に与えるトルクを定めれば、二重倒立振子２００の、所定の時間間隔前の状態が定まる。そこで、所定の時間間隔に第１関節２０１及び第２関節２０７に与えるトルクを最大値と最小値との間で、所定の増分で変化させながら、それぞれの場合に対応する時間間隔前の状態を求めてもよい。この場合に、トルクを最大値と最小値との間で、所定の増分で変化させながら、幅優先探索により網羅的に探索を行なってもよい。
【００３５】
図４は、図３のステップＳ１０２０及びステップＳ１０３０の動作を説明するための図である。図４において、円形は状態を示し、黒い円形は目標状態を示す。また、点線の矢印は計算の順序を示す。なお、図４は、簡単のために位相空間を２次元空間として表している。黒い丸で示す目標状態にある、二重倒立振子２００に、目標状態に至るまでの所定の時間間隔に３個の異なるトルク
【数５】
が与えられたとして、３個の異なる所定の時間間隔前の状態が計算される。探索木作成部１０１は、これらの状態を探索木の枝とする。さらに、生成された枝が示すそれぞれの状態に対して、３個の異なるトルク
【数６】
が与えられたとして、３個の異なる、さらに所定の時間間隔前の状態が計算される。
【００３６】
ここで、位相空間を予め複数の区域に分割しておく。
【００３７】
図５は、複数の区域に分割された位相空間を示す図である。分割は、想定される角度及び角速度の範囲を等分割してもよい。以下において、複数の区域のそれぞれをセルと呼称する。
【００３８】
図３のステップＳ１０４０において、探索木作成部１０１は、所定の時間間隔前の状態の属するセルにすでに探索木の枝のノードが登録されているかどうか判断する。すでに探索木の枝のノードが登録されていれば、ステップＳ１０６０に進む。探索木の枝のノードが登録されていなければ、ステップＳ１０４５に進む。
【００３９】
図３のステップＳ１０４５において、探索木作成部１０１は、探索木作成部１０１が派生させた枝が表す所定の時間間隔前の状態が位相空間上の制約条件を満たすかどうか判断する。位相空間上の制約条件とは、物体である機械の性能または外部の障害物などにより、状態が存在することのできない位相空間の領域に対応する制約条件である。所定の時間間隔前の状態が位相空間上の制約条件を満たせば、ステップＳ０５０に進む。所定の時間間隔前の状態が位相空間上の制約条件を満たさなければ、ステップＳ０６０に進む。
【００４０】
図３のステップＳ１０５０において、探索木作成部１０１は、探索木作成部１０１が派生させた枝が表す所定の時間間隔前の状態を探索木の枝のノードとして登録する。
【００４１】
図３のステップＳ１０６０において、探索木作成部１０１は、探索木作成部１０１が派生させた枝が表す所定の時間間隔前の状態を破棄する。
【００４２】
図６は、図３のステップＳ１０５０及びステップＳ１０６０の動作を説明するための図である。図６において、円形は状態を示し、黒い円形は目標状態を示す。また、実線の矢印は状態の遷移を示し、点線の矢印は計算の順序を示す。実線で示される状態の遷移の方向と点線で示される計算の順序の方向は逆である。探索木の枝のノードは、原則として１個のセル内には１本しか存在しない。探索木作成部１０１が、探索木の枝のノードがすでに存在するセルに新たな枝のノードを定めた場合には、その枝（状態）は破棄される。すなわち、位相空間上の位置が近い状態はセルごとに集約される。このように、従来の探索木の枝を、セルを使用して集約することによって、計算コストを大幅に低減することができる。
【００４３】
図３のステップＳ１０７０において、探索木作成部１０１は、探索が終了したかどうか判断する。探索木作成部１０１は、探索木の所定の深さまでの探索を終えた場合、あるいは所定時間の推移まで探索を終えた場合に探索が終了したと判断してもよい。探索が終了したと判断された場合には、処理が終了する。探索が終了していないと判断された場合には、ステップＳ１０３０に戻り、探索が継続される。
【００４４】
図３の手順によって作成された探索木は、目標状態を根として、過去の時点の状態を枝のノードとするものである。この探索木の根から枝の先端への方向は、時間の進行方向と逆であるので、逆方向探索木と呼称する。逆方向探索木上の点から、枝を時間の進行方向、すなわち順方向にたどることにより、探索木の根、すなわち目標状態に到達することができる。このように、逆方向探索木は、探索木上の任意の点が表す状態から目標状態までの軌道を与える。
【００４５】
ここで、探索木上の点の移行可能領域について説明する。移行可能領域は、状態空間上の移行可能点の集合である。移行可能点は、その点の表す状態に対して適切な制御を行うことにより、探索木の軌道に沿って目標状態に到達することが可能な点である。
【００４６】
図７は、移行可能領域決定部１０３が移行可能領域を定める方法を説明するための流れ図である。
【００４７】
図７のステップＳ２０１０において、移行可能領域決定部１０３は、インデクスｊを０とする。
【００４８】
図７のステップＳ２０２０において、移行可能領域決定部１０３は、探索木上の点Ｓｉに対して、位置と速度にランダムに誤差を加えたＳｉ’を作成する。
【００４９】
図８は、図７のステップＳ２０２０の動作を説明するための図である。図８において、Ｓｋは、目標状態を示し、目標状態に至る実線は、探索木を示す。位置及び速度を
【数７】
としたとき、探索木上の点
【数８】
に対して、ランダムな誤差（Ｒ１，Ｒ２）を加えて点
【数９】
を生成する。ここで、
【数１０】
である。
【００５０】
図７のステップＳ２０３０において、移行可能領域決定部１０３は、Ｓｉ’に対してフィードバック制御を行って目標状態Ｓｋに到達可能であるかどうか判断する。到達可能でなければ、ステップＳ２０４０に進む。到達可能であれば、ステップＳ２０５０に進む。なお、フィードバック制御については後で説明する。
【００５１】
図７のステップＳ２０４０において、移行可能領域決定部１０３は、Ｓｉ’を移行不可能な点のリストＭｌに登録する。
【００５２】
図９は、図７のステップＳ２０３０及びステップＳ２０４０の動作を説明するための図である。Ｓｉ’から目標状態Ｓｋにまでフィードバック制御をかけた後の点Ｓ’ｉｇｏａｌに対して、Ｓ’ｉｇｏａｌとＳｋとの距離が閾値Ｃｏｎｓｔ以上であれば、Ｓｉ’を移行不可能な点として、移行不可能な点のリストＭｌに登録する。
【００５３】
図７のステップＳ２０５０において、移行可能領域決定部１０３は、インデクスｊがＮに達したかどうか判断する。ここで、Ｎは、移行可能かどうか判断される点Ｓｉ’の数である。インデクスｊがＮに達していれば、ステップＳ２０６０に進む。ｊがＮに達していなければ、ステップＳ２０２０に戻る。
【００５４】
図７のステップＳ２０６０において、移行可能領域決定部１０３は、移行不可能な点のリストＭｌ中でＳｉに最も近い点を境界とする半径Ｒｉの超球または超直方体を移行可能領域として定める。
【００５５】
図１０は、図７のステップＳ２０６０を説明するための図である。Ｍｌ中でＳｉに最も近い点を境界とする半径Ｒｉの超球を定めたので、超球の半径Ｒｉ内に存在する点は、フィードバック制御をかけることで目標状態Ｓｋへ到達させることができる。
【００５６】
図７のステップＳ２０７０において、移行可能領域決定部１０３は、インデクスｉがＫに達したかどうか判断する。ここで、Ｋは、移行可能領域を定める点の数である。インデクスｉがＫに達していれば、処理を終了する。ｉがＫに達していなければ、ステップＳ２０９０に戻る。
【００５７】
図７のステップＳ２０８０において、移行可能領域決定部１０３は、インデクスｊに１を加算する。
【００５８】
図７のステップＳ２０９０において、移行可能領域決定部１０３は、インデクスｉに１を加算し、インデクスｊを０とする。
【００５９】
つぎに、軌道の作成方法について説明する。
【００６０】
図１１は、軌道の作成方法の一例を説明するための流れ図である。
【００６１】
図１１のステップＳ３０１０において、探索木作成部１０１が、図３の流れ図に示した方法にしたがって逆方向探索木を作成する。なお、逆方向探索木は予め準備して探索木記憶部１０４に記憶させておいてもよい。
【００６２】
図１１のステップＳ３０２０において、移行可能領域決定部１０３が、図７の流れ図に示した方法にしたがって、逆方向探索木上の各点の移行可能領域を定める。
【００６３】
図１１のステップＳ３０２５において、移行可能領域決定部１０３は、物体の初期状態がいずれかの移行可能領域に含まれるかどうか判断する。物体の初期状態がいずれの移行可能領域にも含まれない場合には、ステップＳ３０３０に進む。物体の初期状態がいずれかの移行可能領域に含まれる場合には、ステップＳ３０５５に進む。
【００６４】
図１１のステップＳ３０３０において、探索木作成部１０１が、逆方向探索木上のある一点を目標状態として、順方向探索木を作成する。順方向探索木とは、物体の初期状態を根として、時間の進行にしたがって枝を成長させた探索木である。順方向探索木は、図３に示した逆方向探索木の作成方法と同様な方法で作成することができる。逆方向探索木と同様にセルを使用して枝を集約する。
【００６５】
なお、逆方向探索木上のある一点を目標状態として、順方向探索木を作成する代わりに、予め作成した順方向探索木から目標状態の近くへ続く木を探すようにしてもよい。
【００６６】
図１１のステップＳ３０４０において、軌道作成部１０５は、順方向探索木の目標状態、すなわち軌道の終点が、逆方向探索木上の点の移行可能領域内であるかどうか判断する。移行可能領域内でなければ、ステップＳ３０３０に戻る。移行可能領域内であれば、ステップＳ３０５０に進む。
【００６７】
図１１のステップＳ３０５０において、軌道作成部１０５は、物体の初期状態を根とする順方向探索木による軌道と物体の目標状態を根とする逆方向探索木による軌道とを移行可能領域で接続して全体の軌道を定める。
【００６８】
図１１のステップＳ３０５５において、軌道作成部１０５は、物体の目標状態を根とする逆方向探索木による軌道を定める。
【００６９】
図１１のステップＳ３０６０において、軌道制御部１０７は、フィードバック制御によって物体の運動を生成する。フィードバック制御については後で説明する。
【００７０】
目標状態を根とする逆方向探索木を使用した軌跡は、目標状態に関する精度が高い。他方、 初期状態(現在状態)を根とする順方向探索木を使用した軌跡は、初期状態に関する精度が高い。したがって、逆方向探索木を使用した軌跡と順方向探索木を使用した軌跡とを接続した軌跡は、目標状態及び初期状態に関する精度が高い。
【００７１】
ここで、フィードバック制御について説明する。
【００７２】
図１２は、フィードバック制御を説明するための流れ図である。
【００７３】
図１３は、図１２の流れ図の動作を説明するための図である。図１３において、Ａは探索木の軌道を示し、Ｂは、フィードバック制御による軌道を示す。
【００７４】
図１２のステップＳ４０１０において、軌道制御部１０７は、探索木の枝より、ｎステップ先の短期目標状態Splan(t+n)を取得する。
【００７５】
図１２のステップＳ４０２０において、軌道制御部１０７は、現在状態Scur(t)より、ランダムなトルク（計画済のトルクTplanを中心とした一定区間内の一様乱数から選択したトルクTcomp）をかけてｎステップ経過した後の状態Spred(t+n)を求める。
【００７６】
図１２のステップＳ４０３０において、軌道制御部１０７は、Spred(t+n)とSplan(t+n)の誤差を計算する。
【００７７】
図１２のステップＳ４０４０において、軌道制御部１０７は、所定回数の誤差計算を終えたかどうか判断する。所定回数は、例として２００回である。所定回数の誤差計算を終えていれば、ステップＳ４０５０に進む。所定回数の誤差計算を終えていなければ、ステップＳ４０２０に戻る。
【００７８】
図１２のステップＳ４０５０において、軌道制御部１０７は、誤差最小となるTplan+Tcompを求め、トルク目標値として機械のアクチュエータへ送信し、１ステップ経過するまで同じトルクを維持し、処理を終了する。
【００７９】
図１１のステップＳ３０１０の逆方向探索木を予め準備している場合には、ステップＳ３０３０の演算は必要なくなり、予め作成された（準備された）順方向探索木から目標状態の近くに続く木を探せばよい。
【００８０】
本発明による軌道計画方法について、本明細書では、説明を簡単にするために、物体を二重倒立振子としている。しかし、逆方向探索木及び移行可能領域を使用する本発明の考え方は、任意の物体の状態空間における軌道計画方法に適用することができる。
【００８１】
つぎに、図２に示した二重倒立振子２００のシミュレーションによる制御実験について説明する。二重倒立振子２００の第１リンク２０５のリンク長は、ｌ１であり、質量は、ｍ１であり、重心位置２０３はリンクの中央である。二重倒立振子２００の第２リンク２１１のリンク長は、ｌ２であり、質量は、ｍ２であり、重心位置２０９はリンクの中央である。また、重力加速度をｇとする。二重倒立振子２００の運動方程式は以下のとおりである。
【数１１】
関節角度と重力の関係に対する理解を容易にするために、以下のように座標を変更した。
【数１２】
【００８２】
表１は、実験のシミュレーションの設定を示す。現在状態から隣接する状態までの時間間隔が、表１のトルク切り替えタイムステップに相当する。
【表１】
【００８３】
表２は、第１関節及び第２関節のパラメータを示す。表２において、第１関節の最大トルク2 [Nm]、探索木作成時のトルク粒度１０分割とは、探索木の新たな枝を派生させる際に、-2 [Nm]から2 [Nm]までのトルクの範囲を１０分割し、たとえば、 -1.8 [Nm], -1.4[Nm], -1.0[Nm]... 1.0[Nm], 1.4[Nm], 1.8 [Nm]というようにトルクを与えて、運動方程式から次の状態（角度及び角速度）を求めること
を意味する。実験においては、第１関節及び第２関節に、それぞれ１０個の異なるトルクを与えるので、１００個の異なる次の状態が派生する。ここで、最大トルクの値を調整することにより物体の動作を制限することもできる。
【表２】
【００８４】
実験では、二重倒立振子２００が完全に倒立した状態を目標状態とした。
【００８５】
図１４は、二重倒立振子２００の目標状態を示す図である。二重倒立振子２００の目標状態の座標は以下のとおりである。
【数１３】
【００８６】
図１５は、図３の流れ図に示した方法によって求めた逆探索木による軌跡と、該軌跡上の点について図７の流れ図に示した方法によって作成した移行可能領域とを示す図である。図１５においては、目標状態をＡで示し、移行可能領域をＲで示す。図１５（ａ）は、第１関節の位相空間を示し、図１５（ｂ）は、第２関節の位相空間を示す。
【００８７】
図１６は、図１５に示した逆方向探索木による軌跡と順方向探索木による軌跡とを移行可能領域において接続した状態を示す図である。図１６において、逆方向探索木による軌跡を実線で示し、順方向探索木による軌跡を点線で示す。また、逆方向探索木による軌跡上の目標状態をＡで示し、移行点をＢで示し、順方向探索木による軌跡上の初期状態をＣで示す。二つの軌跡を移行可能領域内で接続することにより初期状態から目標状態までの運動を実行することができた。図１６（ａ）は、第１関節の位相空間を示し、図１６（ｂ）は、第２関節の位相空間を示す。
【００８８】
図１７は、順方向探索木による軌跡の終点から、目標状態に向けてフィードバック制御を行った場合の軌跡を示す図である。図１７において、フィードバック制御による軌跡を実線で示し、順方向探索木による軌跡を点線で示す。図１７において、目標状態をＡで示し、順方向探索木による軌跡の終点をＢで示し、順方向探索木による軌跡上の初期状態をＣで示す。順方向探索木による軌跡の終点Ｂからフィードバック制御によって目標状態に到達することはできなかった。
【００８９】
このように、逆方向探索木及び移行可能領域を使用する本発明の方法及び装置は、単なるフィードバック制御と比較して有効に機能する。
【００９０】
本発明は、安定状態にある機械をある不安定状態を経由させて次の安定状態へと遷移させるような場合に応用することができる。一例として、投球運動を行う機械は、通常、初期状態として安定状態を選択する。投球を行う瞬間は、機械にとって不安定状態である。機械は、この不安定状態を経由して、元の安定状態へ戻る。この場合に、投球前の安定状態を初期状態とする順方向探索木による軌跡と、投球後の安定状態を初期状態とする逆方向探索木による軌跡とを接続して、安定状態にある機械をある不安定状態を経由させて次の安定状態へと遷移させる軌跡を求めることができる。安定状態から不安定状態を経由して安定状態に戻る運動には、投球運動の他に、テニスのスイング動作や反動をつけた荷物の持ち上げなど多数の運動がある。
【符号の説明】
【００９１】
１０１…探索木作成部、１０３…移行可能領域決定部、１０４…探索木記憶部、１０５…軌道作成部、１０７…軌道制御部、
【技術分野】
【０００１】
本発明は、物体の、目標状態に至る状態遷移系列を作成する、軌道計画方法、軌道計画システム、作成された軌道にしたがって軌道を制御する軌道制御方法及び軌道計画・制御システムに関する。
【背景技術】
【０００２】
機械の状態（姿勢・速度）は、位相空間上の位置、機械の運動は位相空間上の経路として定めることができる。機械を初期状態から目標状態へと制御したい場合がある。具体例は、以下のとおりである。
【０００３】
第一の具体例は、二足型ロボットがバランスを崩した場合である。この場合の初期状態は、バランスを崩した直後の姿勢・速度であり、目標状態は安定姿勢（直立姿勢かつ速度０など）である。
【０００４】
第二の具体例は、航空機がバランスを崩し失速した場合である。この場合の初期状態は、バランスを崩した直後の姿勢・速度であり、目標状態は、翼とボディを水平にして等速で前進する状態である。
その他の具体例として、バイクが走行中に石に当たった際にバランスを崩し、姿勢を修正したい場合や、自動車がスリップした場合等がある。
【０００５】
機械を初期状態から目標状態へ制御する場合に、ＰＩＤ制御やポテンシャル法を使用すると、機械がバランスを崩した後の状態によっては適切に機能しないという問題がある。この問題は、これらの制御方法は、位相空間上の目標状態までの機械状態遷移において広い範囲の初期状態を取れないことに起因している。また、ＰＩＤ制御やポテンシャル法では、機械が目標状態へ到達するまでの時間を規定することが困難であり、短時間での目標状態復帰が困難である。
【０００６】
状態空間の軌道計画を作成する方法として、状態空間における状態遷移を探索木として記述する方法がある。この方法において、根または枝から、所定数の枝を派生させ、それぞれの枝を探索することが考えられる。しかし、派生させる枝の数をＮとし、木の深さをＭとすると、木の深さＭのレベルで探索の対象となる枝の数は、ＮＭとなる。したがって、派生させる全ての枝を探索する場合には計算コストが膨大となる。
【０００７】
そこで、探索木の枝の評価関数を定め、評価関数にしたがって選択した枝のみから枝を派生させる方法が提案されている（非特許文献１）。この方法によれば、計算コストは抑えられる。しかし、評価関数は、対象ごとに設計者が経験によって決める必要がある。評価関数によっては、状態空間上の探索を十分に行えない場合も生じうる。すなわち、この方法では、状態空間の探索に制約が生じうる。
【０００８】
上記の方法を含め探索木を使用する従来の方法は、リアルタイムの軌道計画作成及び制御に使用するには計算コスト（計算時間）が大きすぎるか、探索に制約が存在した。また、従来の方法は、物体の初期状態から目標状態までの軌道を高い精度で定めることが困難であった。
【先行技術文献】
【非特許文献】
【０００９】
【非特許文献１】Pedro S. Huang:”Planning For Dynamic Motions Using a Search Tree”, a graduate thesis of Toronto Univ., (1996).
【発明の概要】
【発明が解決しようとする課題】
【００１０】
したがって、物体、すなわち機械の種々の初期状態から目標状態までの軌道を、探索に制約を伴わずに少ない計算コスト、かつ高い精度で求めることのできる軌道計画方法、軌道計画システム、及び求めた軌道にしたがって目標状態までの軌道を制御することのできる軌道制御方法及び軌道計画・制御システムに対するニーズがある。
【課題を解決するための手段】
【００１１】
本発明の第１の態様による軌道計画方法は、軌道計画システムによって、物体の状態を目標状態へ制御するための軌道を求める軌道計画方法であって、探索木作成部が、状態空間において、該目標状態を根として、予め複数の区域に分割された該状態空間のそれぞれの区域に含まれる枝のノードの数を制限することによって枝を集約した逆方向探索木を作成するするステップと、移行可能領域決定部が、該状態空間において、該逆方向探索木上の点に対して、その領域内であれば、該逆方向探索木にしたがって該目標状態に到達しうる、移行可能領域を定めるステップと、軌道作成部が、該逆方向探索木を使用して、該状態空間における該移行可能領域内の点から該根までの、該物体の軌道を定めるステップと、を含む。
【００１２】
本態様の軌道計画方法において、探索に制約はない。また、逆方向探索木は区域を使用して集約されているので、計算コストは従来の方法に比較して少ない。さらに、逆方向探索木の根は、目標状態と一致するので、目標状態を終点とする軌道の精度は高い。したがって、本態様の軌道計画方法によれば、逆方向探索木上の点の移行可能領域内の点から目標状態までの軌道を、探索に制約を伴わずに少ない計算コスト、かつ高い精度で求めることができる。
【００１３】
本発明の第１の態様の第１の実施形態によれば、前記探索木作成部が、前記状態空間において、前記物体の初期状態を根として、前記状態空間の前記それぞれの区域に含まれる枝のノードの数を制限することによって枝を集約した順方向探索木を作成するするステップと、前記軌道作成部が、前記移行可能領域のいずれかにおいて、前記順方向探索木による軌道と前記逆方向探索木による軌道を接続して、前記物体の前記初期状態から前記目標状態までの軌道を定めるステップと、をさらに含む。
【００１４】
本実施形態によれば、移行可能領域において、順方向探索木による軌道と逆方向探索木による軌道を接続して、物体の初期状態から目標状態までの軌道を定めることができる。逆方向探索木の根は、目標状態と一致し、順方向探索木の根は、初期状態と一致するので、初期状態を起点とし目標状態を終点とするする軌道の精度は高い。
【００１５】
本発明の第１の態様の第２の実施形態によれば、前記移行可能領域を定めるステップにおいて、前記移行可能領域決定部は、前記逆方向探索木上の点の近傍の複数の点に対して、フィードバック制御を実施することにより、前記目標状態に到達しうるかどうか定め、その結果に基づいて前記移行可能領域を定める。
【００１６】
本実施形態によれば、フィードバック制御を実施することにより、確実に移行可能領域を定めることができる。
【００１７】
本発明の第１の態様の第３の実施形態によれば、前記移行可能領域決定部は、前記目標状態に到達することができなかった点のうち、前記探索木上の点に最も近い点の、前記探索木上の点までの距離に基づいて前記移行可能領域を定める。
【００１８】
本実施形態によれば、目標状態に到達することができなかった点のうち、逆方向探索木上の点に最も近い点の、逆方向探索木上の点までの距離を使用することにより、高い精度で移行可能領域を定めることができる。
【００１９】
本発明の第２の態様による軌道制御方法は、本発明の第１の態様による軌道計画方法によって求めた軌道にしたがって、前記物体を制御することによって前記物体の状態を前記目標状態とする。
【００２０】
本態様による軌道制御方法によれば、目標状態までの軌道を、探索に制約を伴わずに少ない計算コスト、かつ高い精度で求め、その軌道にしたがって物体の状態を目標状態とすることができる。
【００２１】
本発明の第３の態様による軌道計画システムは、物体の状態を目標状態へ制御するための軌道を求める軌道計画システムであって、状態空間において、該目標状態を根とし、予め複数の区域に分割された状態空間のそれぞれの区域に含まれる枝のノードの数を制限することによって枝を集約した逆方向探索木を作成する探索木作成部と、該状態空間において、該逆方向探索木上の点に対して、その領域内であれば、該逆方向探索木にしたがって該目標状態に到達しうる、移行可能領域を定める移行可能領域決定部と、探索木及び移行可能領域を記憶する探索木記憶部と、該逆方向探索木を使用して該状態空間における該移行可能領域内の点から該根までの、該物体の軌道を定める軌道作成部と、を備える。
【００２２】
本態様の軌道計画システムにおいて、探索に制約はない。また、逆方向探索木は区域を使用して集約されているので、計算コストは従来の方法に比較して少ない。さらに、逆方向探索木の根は、目標状態と一致するので、目標状態を終点とする軌道の精度は高い。したがって、本態様の軌道計画システムによれば、逆方向探索木上の点の移行可能領域内の点から目標状態までの軌道を、探索に制約を伴わずに少ない計算コスト、かつ高い精度で求めることができる。
【００２３】
本発明の第３の態様の第１の実施形態による軌道計画システムにおいては、前記探索木作成部が、前記状態空間において、前記物体の初期状態を根として、前記状態空間のそれぞれの区域に含まれる枝のノードの数を制限することによって枝を集約した順方向探索木を作成し、前記軌道作成部が、前記移行可能領域のいずれかにおいて、前記順方向探索木による軌道と前記逆方向探索木による軌道を接続して、前記物体の前記初期状態から前記目標状態までの軌道を定める。
【００２４】
本実施形態によれば、移行可能領域において、順方向探索木による軌道と逆方向探索木による軌道を接続して、物体の初期状態から目標状態までの軌道を定めることができる。逆方向探索木の根は、目標状態と一致し、順方向探索木の根は、初期状態と一致するので、初期状態を起点とし目標状態を終点とするする軌道の精度は高い。
【００２５】
本発明の第４の態様による、軌道計画・制御システムは、本発明の第２の態様による軌道計画システムと、該軌道計画システムが求めた軌道にしたがって、前記物体を制御することによって前記物体の状態を前記目標状態とする軌道制御部とを備えている。
【００２６】
本態様の軌道計画・制御システムによれば、目標状態までの軌道を、探索に制約を伴わずに少ない計算コスト、かつ高い精度で求め、その軌道にしたがって物体の状態を目標状態とすることができる。
【図面の簡単な説明】
【００２７】
【図１】本発明の一実施形態による軌道計画・制御システムの構成を示す図である。
【図２】二重倒立振子の構成を示す図である。
【図３】探索木作成部が位相空間上に探索木を作成する手順を示す流れ図である。
【図４】図３のステップＳ１０２０及びステップＳ１０３０の動作を説明するための図である。
【図５】複数の区域に分割された位相空間を示す図である。
【図６】図３のステップＳ１０５０及びステップＳ１０６０の動作を説明するための図である。
【図７】移行可能領域決定部が移行可能領域を定める方法を説明するための流れ図である。
【図８】図７のステップＳ２０２０の動作を説明するための図である。
【図９】図７のステップＳ２０３０及びステップＳ２０４０の動作を説明するための図である。
【図１０】図７のステップＳ２０６０を説明するための図である。
【図１１】軌道の作成方法を説明するための流れ図である。
【図１２】フィードバック制御を説明するための流れ図である。
【図１３】図１２のステップＳ４０１０乃至ステップＳ４０５０の動作を説明するための図である。
【図１４】二重倒立振子２００の目標状態を示す図である。
【図１５】図３の流れ図に示した方法によって求めた逆探索木による軌跡と、該軌跡上の点について図７の流れ図に示した方法によって作成した移行可能領域とを示す図である。
【図１６】図１５に示した逆方向探索木による軌跡と順方向探索木による軌跡とを移行可能領域において接続した状態を示す図である。
【図１７】順方向探索木による軌跡の移行点から、目標状態に向けてフィードバック制御を行った場合の軌跡を示す図である。
【発明を実施するための形態】
【００２８】
図１は、本発明の一実施形態による軌道計画・制御システム１００の構成を示す図である。軌道計画・制御システム１００は、状態空間上の物体の状態に対応する枝を備えた探索木を作成する探索木作成部１０１と、探索木上の点の移行可能領域を定める移行可能領域決定部１０３と、探索木作成部１０１によって作成された探索木及び移行可能領域を記憶する探索木記憶部１０４と、探索木記憶部１０４に記憶された探索木及び移行可能領域を使用して目標状態までの軌道を作成する軌道作成部１０５と、軌道作成部１０５によって作成された軌道を使用して軌道制御を行う軌道制御部１０７と、を備える。探索木上の点の移行可能領域については後で説明する。また、各構成部の機能の詳細については、後で説明する。
【００２９】
図２は、二重倒立振子２００の構成を示す図である。以下の実施形態においては、２次元平面内で運動する二重倒立振子２００を物体として説明を行なう。二重倒立振子２００は、第１リンク２０５と、第２リンク２１１と、第１リンク２０５を回転可能に固定点に接続する第１関節（ジョイント）２０１と、第１リンク２０５及び第２リンク２１１を回転可能に接続する第２関節２０７と、から構成される。第１リンク２０５及び第２リンク２１１の重心を、それぞれ符号２０３及び２０９によって示す。
【００３０】
二重倒立振子２００の運動は、第１関節２０１及び第２関節２０７に与えるトルクによって定まり、第１リンク２０５のＸ軸に対する角度
【数１】
第２リンク２１１の第１リンク２０５に対する角度
【数２】
及びこれらの角速度
【数３】
によって表現される。そこで、本実施形態において、状態空間として以下の位相空間を採用する。
【数４】
【００３１】
図３は、探索木作成部１０１が位相空間上に探索木を作成する手順を示す流れ図である。
【００３２】
図３のステップＳ１０１０において、探索木作成部１０１は物体の目標状態を得る。
【００３３】
図３のステップＳ１０２０において、探索木作成部１０１は、位相空間において物体の目標状態を探索木の根として定める。
【００３４】
図３のステップＳ１０３０において、探索木作成部１０１が、位相空間において、所定の時間間隔前、すなわち、時刻ｔ−Δｔの状態を探索木の枝のノードとして定める。枝のノードとは枝が分岐する点または枝の先端の点である。所定の時間間隔前の状態は、運動方程式や動力学シミュレータによって定める。上述のように、二重倒立振子２００の運動は、第１関節２０１及び第２関節２０７に与えるトルクによって定まるので、所定の時間間隔に第１関節２０１及び第２関節２０７に与えるトルクを定めれば、二重倒立振子２００の、所定の時間間隔前の状態が定まる。そこで、所定の時間間隔に第１関節２０１及び第２関節２０７に与えるトルクを最大値と最小値との間で、所定の増分で変化させながら、それぞれの場合に対応する時間間隔前の状態を求めてもよい。この場合に、トルクを最大値と最小値との間で、所定の増分で変化させながら、幅優先探索により網羅的に探索を行なってもよい。
【００３５】
図４は、図３のステップＳ１０２０及びステップＳ１０３０の動作を説明するための図である。図４において、円形は状態を示し、黒い円形は目標状態を示す。また、点線の矢印は計算の順序を示す。なお、図４は、簡単のために位相空間を２次元空間として表している。黒い丸で示す目標状態にある、二重倒立振子２００に、目標状態に至るまでの所定の時間間隔に３個の異なるトルク
【数５】
が与えられたとして、３個の異なる所定の時間間隔前の状態が計算される。探索木作成部１０１は、これらの状態を探索木の枝とする。さらに、生成された枝が示すそれぞれの状態に対して、３個の異なるトルク
【数６】
が与えられたとして、３個の異なる、さらに所定の時間間隔前の状態が計算される。
【００３６】
ここで、位相空間を予め複数の区域に分割しておく。
【００３７】
図５は、複数の区域に分割された位相空間を示す図である。分割は、想定される角度及び角速度の範囲を等分割してもよい。以下において、複数の区域のそれぞれをセルと呼称する。
【００３８】
図３のステップＳ１０４０において、探索木作成部１０１は、所定の時間間隔前の状態の属するセルにすでに探索木の枝のノードが登録されているかどうか判断する。すでに探索木の枝のノードが登録されていれば、ステップＳ１０６０に進む。探索木の枝のノードが登録されていなければ、ステップＳ１０４５に進む。
【００３９】
図３のステップＳ１０４５において、探索木作成部１０１は、探索木作成部１０１が派生させた枝が表す所定の時間間隔前の状態が位相空間上の制約条件を満たすかどうか判断する。位相空間上の制約条件とは、物体である機械の性能または外部の障害物などにより、状態が存在することのできない位相空間の領域に対応する制約条件である。所定の時間間隔前の状態が位相空間上の制約条件を満たせば、ステップＳ０５０に進む。所定の時間間隔前の状態が位相空間上の制約条件を満たさなければ、ステップＳ０６０に進む。
【００４０】
図３のステップＳ１０５０において、探索木作成部１０１は、探索木作成部１０１が派生させた枝が表す所定の時間間隔前の状態を探索木の枝のノードとして登録する。
【００４１】
図３のステップＳ１０６０において、探索木作成部１０１は、探索木作成部１０１が派生させた枝が表す所定の時間間隔前の状態を破棄する。
【００４２】
図６は、図３のステップＳ１０５０及びステップＳ１０６０の動作を説明するための図である。図６において、円形は状態を示し、黒い円形は目標状態を示す。また、実線の矢印は状態の遷移を示し、点線の矢印は計算の順序を示す。実線で示される状態の遷移の方向と点線で示される計算の順序の方向は逆である。探索木の枝のノードは、原則として１個のセル内には１本しか存在しない。探索木作成部１０１が、探索木の枝のノードがすでに存在するセルに新たな枝のノードを定めた場合には、その枝（状態）は破棄される。すなわち、位相空間上の位置が近い状態はセルごとに集約される。このように、従来の探索木の枝を、セルを使用して集約することによって、計算コストを大幅に低減することができる。
【００４３】
図３のステップＳ１０７０において、探索木作成部１０１は、探索が終了したかどうか判断する。探索木作成部１０１は、探索木の所定の深さまでの探索を終えた場合、あるいは所定時間の推移まで探索を終えた場合に探索が終了したと判断してもよい。探索が終了したと判断された場合には、処理が終了する。探索が終了していないと判断された場合には、ステップＳ１０３０に戻り、探索が継続される。
【００４４】
図３の手順によって作成された探索木は、目標状態を根として、過去の時点の状態を枝のノードとするものである。この探索木の根から枝の先端への方向は、時間の進行方向と逆であるので、逆方向探索木と呼称する。逆方向探索木上の点から、枝を時間の進行方向、すなわち順方向にたどることにより、探索木の根、すなわち目標状態に到達することができる。このように、逆方向探索木は、探索木上の任意の点が表す状態から目標状態までの軌道を与える。
【００４５】
ここで、探索木上の点の移行可能領域について説明する。移行可能領域は、状態空間上の移行可能点の集合である。移行可能点は、その点の表す状態に対して適切な制御を行うことにより、探索木の軌道に沿って目標状態に到達することが可能な点である。
【００４６】
図７は、移行可能領域決定部１０３が移行可能領域を定める方法を説明するための流れ図である。
【００４７】
図７のステップＳ２０１０において、移行可能領域決定部１０３は、インデクスｊを０とする。
【００４８】
図７のステップＳ２０２０において、移行可能領域決定部１０３は、探索木上の点Ｓｉに対して、位置と速度にランダムに誤差を加えたＳｉ’を作成する。
【００４９】
図８は、図７のステップＳ２０２０の動作を説明するための図である。図８において、Ｓｋは、目標状態を示し、目標状態に至る実線は、探索木を示す。位置及び速度を
【数７】
としたとき、探索木上の点
【数８】
に対して、ランダムな誤差（Ｒ１，Ｒ２）を加えて点
【数９】
を生成する。ここで、
【数１０】
である。
【００５０】
図７のステップＳ２０３０において、移行可能領域決定部１０３は、Ｓｉ’に対してフィードバック制御を行って目標状態Ｓｋに到達可能であるかどうか判断する。到達可能でなければ、ステップＳ２０４０に進む。到達可能であれば、ステップＳ２０５０に進む。なお、フィードバック制御については後で説明する。
【００５１】
図７のステップＳ２０４０において、移行可能領域決定部１０３は、Ｓｉ’を移行不可能な点のリストＭｌに登録する。
【００５２】
図９は、図７のステップＳ２０３０及びステップＳ２０４０の動作を説明するための図である。Ｓｉ’から目標状態Ｓｋにまでフィードバック制御をかけた後の点Ｓ’ｉｇｏａｌに対して、Ｓ’ｉｇｏａｌとＳｋとの距離が閾値Ｃｏｎｓｔ以上であれば、Ｓｉ’を移行不可能な点として、移行不可能な点のリストＭｌに登録する。
【００５３】
図７のステップＳ２０５０において、移行可能領域決定部１０３は、インデクスｊがＮに達したかどうか判断する。ここで、Ｎは、移行可能かどうか判断される点Ｓｉ’の数である。インデクスｊがＮに達していれば、ステップＳ２０６０に進む。ｊがＮに達していなければ、ステップＳ２０２０に戻る。
【００５４】
図７のステップＳ２０６０において、移行可能領域決定部１０３は、移行不可能な点のリストＭｌ中でＳｉに最も近い点を境界とする半径Ｒｉの超球または超直方体を移行可能領域として定める。
【００５５】
図１０は、図７のステップＳ２０６０を説明するための図である。Ｍｌ中でＳｉに最も近い点を境界とする半径Ｒｉの超球を定めたので、超球の半径Ｒｉ内に存在する点は、フィードバック制御をかけることで目標状態Ｓｋへ到達させることができる。
【００５６】
図７のステップＳ２０７０において、移行可能領域決定部１０３は、インデクスｉがＫに達したかどうか判断する。ここで、Ｋは、移行可能領域を定める点の数である。インデクスｉがＫに達していれば、処理を終了する。ｉがＫに達していなければ、ステップＳ２０９０に戻る。
【００５７】
図７のステップＳ２０８０において、移行可能領域決定部１０３は、インデクスｊに１を加算する。
【００５８】
図７のステップＳ２０９０において、移行可能領域決定部１０３は、インデクスｉに１を加算し、インデクスｊを０とする。
【００５９】
つぎに、軌道の作成方法について説明する。
【００６０】
図１１は、軌道の作成方法の一例を説明するための流れ図である。
【００６１】
図１１のステップＳ３０１０において、探索木作成部１０１が、図３の流れ図に示した方法にしたがって逆方向探索木を作成する。なお、逆方向探索木は予め準備して探索木記憶部１０４に記憶させておいてもよい。
【００６２】
図１１のステップＳ３０２０において、移行可能領域決定部１０３が、図７の流れ図に示した方法にしたがって、逆方向探索木上の各点の移行可能領域を定める。
【００６３】
図１１のステップＳ３０２５において、移行可能領域決定部１０３は、物体の初期状態がいずれかの移行可能領域に含まれるかどうか判断する。物体の初期状態がいずれの移行可能領域にも含まれない場合には、ステップＳ３０３０に進む。物体の初期状態がいずれかの移行可能領域に含まれる場合には、ステップＳ３０５５に進む。
【００６４】
図１１のステップＳ３０３０において、探索木作成部１０１が、逆方向探索木上のある一点を目標状態として、順方向探索木を作成する。順方向探索木とは、物体の初期状態を根として、時間の進行にしたがって枝を成長させた探索木である。順方向探索木は、図３に示した逆方向探索木の作成方法と同様な方法で作成することができる。逆方向探索木と同様にセルを使用して枝を集約する。
【００６５】
なお、逆方向探索木上のある一点を目標状態として、順方向探索木を作成する代わりに、予め作成した順方向探索木から目標状態の近くへ続く木を探すようにしてもよい。
【００６６】
図１１のステップＳ３０４０において、軌道作成部１０５は、順方向探索木の目標状態、すなわち軌道の終点が、逆方向探索木上の点の移行可能領域内であるかどうか判断する。移行可能領域内でなければ、ステップＳ３０３０に戻る。移行可能領域内であれば、ステップＳ３０５０に進む。
【００６７】
図１１のステップＳ３０５０において、軌道作成部１０５は、物体の初期状態を根とする順方向探索木による軌道と物体の目標状態を根とする逆方向探索木による軌道とを移行可能領域で接続して全体の軌道を定める。
【００６８】
図１１のステップＳ３０５５において、軌道作成部１０５は、物体の目標状態を根とする逆方向探索木による軌道を定める。
【００６９】
図１１のステップＳ３０６０において、軌道制御部１０７は、フィードバック制御によって物体の運動を生成する。フィードバック制御については後で説明する。
【００７０】
目標状態を根とする逆方向探索木を使用した軌跡は、目標状態に関する精度が高い。他方、 初期状態(現在状態)を根とする順方向探索木を使用した軌跡は、初期状態に関する精度が高い。したがって、逆方向探索木を使用した軌跡と順方向探索木を使用した軌跡とを接続した軌跡は、目標状態及び初期状態に関する精度が高い。
【００７１】
ここで、フィードバック制御について説明する。
【００７２】
図１２は、フィードバック制御を説明するための流れ図である。
【００７３】
図１３は、図１２の流れ図の動作を説明するための図である。図１３において、Ａは探索木の軌道を示し、Ｂは、フィードバック制御による軌道を示す。
【００７４】
図１２のステップＳ４０１０において、軌道制御部１０７は、探索木の枝より、ｎステップ先の短期目標状態Splan(t+n)を取得する。
【００７５】
図１２のステップＳ４０２０において、軌道制御部１０７は、現在状態Scur(t)より、ランダムなトルク（計画済のトルクTplanを中心とした一定区間内の一様乱数から選択したトルクTcomp）をかけてｎステップ経過した後の状態Spred(t+n)を求める。
【００７６】
図１２のステップＳ４０３０において、軌道制御部１０７は、Spred(t+n)とSplan(t+n)の誤差を計算する。
【００７７】
図１２のステップＳ４０４０において、軌道制御部１０７は、所定回数の誤差計算を終えたかどうか判断する。所定回数は、例として２００回である。所定回数の誤差計算を終えていれば、ステップＳ４０５０に進む。所定回数の誤差計算を終えていなければ、ステップＳ４０２０に戻る。
【００７８】
図１２のステップＳ４０５０において、軌道制御部１０７は、誤差最小となるTplan+Tcompを求め、トルク目標値として機械のアクチュエータへ送信し、１ステップ経過するまで同じトルクを維持し、処理を終了する。
【００７９】
図１１のステップＳ３０１０の逆方向探索木を予め準備している場合には、ステップＳ３０３０の演算は必要なくなり、予め作成された（準備された）順方向探索木から目標状態の近くに続く木を探せばよい。
【００８０】
本発明による軌道計画方法について、本明細書では、説明を簡単にするために、物体を二重倒立振子としている。しかし、逆方向探索木及び移行可能領域を使用する本発明の考え方は、任意の物体の状態空間における軌道計画方法に適用することができる。
【００８１】
つぎに、図２に示した二重倒立振子２００のシミュレーションによる制御実験について説明する。二重倒立振子２００の第１リンク２０５のリンク長は、ｌ１であり、質量は、ｍ１であり、重心位置２０３はリンクの中央である。二重倒立振子２００の第２リンク２１１のリンク長は、ｌ２であり、質量は、ｍ２であり、重心位置２０９はリンクの中央である。また、重力加速度をｇとする。二重倒立振子２００の運動方程式は以下のとおりである。
【数１１】
関節角度と重力の関係に対する理解を容易にするために、以下のように座標を変更した。
【数１２】
【００８２】
表１は、実験のシミュレーションの設定を示す。現在状態から隣接する状態までの時間間隔が、表１のトルク切り替えタイムステップに相当する。
【表１】
【００８３】
表２は、第１関節及び第２関節のパラメータを示す。表２において、第１関節の最大トルク2 [Nm]、探索木作成時のトルク粒度１０分割とは、探索木の新たな枝を派生させる際に、-2 [Nm]から2 [Nm]までのトルクの範囲を１０分割し、たとえば、 -1.8 [Nm], -1.4[Nm], -1.0[Nm]... 1.0[Nm], 1.4[Nm], 1.8 [Nm]というようにトルクを与えて、運動方程式から次の状態（角度及び角速度）を求めること
を意味する。実験においては、第１関節及び第２関節に、それぞれ１０個の異なるトルクを与えるので、１００個の異なる次の状態が派生する。ここで、最大トルクの値を調整することにより物体の動作を制限することもできる。
【表２】
【００８４】
実験では、二重倒立振子２００が完全に倒立した状態を目標状態とした。
【００８５】
図１４は、二重倒立振子２００の目標状態を示す図である。二重倒立振子２００の目標状態の座標は以下のとおりである。
【数１３】
【００８６】
図１５は、図３の流れ図に示した方法によって求めた逆探索木による軌跡と、該軌跡上の点について図７の流れ図に示した方法によって作成した移行可能領域とを示す図である。図１５においては、目標状態をＡで示し、移行可能領域をＲで示す。図１５（ａ）は、第１関節の位相空間を示し、図１５（ｂ）は、第２関節の位相空間を示す。
【００８７】
図１６は、図１５に示した逆方向探索木による軌跡と順方向探索木による軌跡とを移行可能領域において接続した状態を示す図である。図１６において、逆方向探索木による軌跡を実線で示し、順方向探索木による軌跡を点線で示す。また、逆方向探索木による軌跡上の目標状態をＡで示し、移行点をＢで示し、順方向探索木による軌跡上の初期状態をＣで示す。二つの軌跡を移行可能領域内で接続することにより初期状態から目標状態までの運動を実行することができた。図１６（ａ）は、第１関節の位相空間を示し、図１６（ｂ）は、第２関節の位相空間を示す。
【００８８】
図１７は、順方向探索木による軌跡の終点から、目標状態に向けてフィードバック制御を行った場合の軌跡を示す図である。図１７において、フィードバック制御による軌跡を実線で示し、順方向探索木による軌跡を点線で示す。図１７において、目標状態をＡで示し、順方向探索木による軌跡の終点をＢで示し、順方向探索木による軌跡上の初期状態をＣで示す。順方向探索木による軌跡の終点Ｂからフィードバック制御によって目標状態に到達することはできなかった。
【００８９】
このように、逆方向探索木及び移行可能領域を使用する本発明の方法及び装置は、単なるフィードバック制御と比較して有効に機能する。
【００９０】
本発明は、安定状態にある機械をある不安定状態を経由させて次の安定状態へと遷移させるような場合に応用することができる。一例として、投球運動を行う機械は、通常、初期状態として安定状態を選択する。投球を行う瞬間は、機械にとって不安定状態である。機械は、この不安定状態を経由して、元の安定状態へ戻る。この場合に、投球前の安定状態を初期状態とする順方向探索木による軌跡と、投球後の安定状態を初期状態とする逆方向探索木による軌跡とを接続して、安定状態にある機械をある不安定状態を経由させて次の安定状態へと遷移させる軌跡を求めることができる。安定状態から不安定状態を経由して安定状態に戻る運動には、投球運動の他に、テニスのスイング動作や反動をつけた荷物の持ち上げなど多数の運動がある。
【符号の説明】
【００９１】
１０１…探索木作成部、１０３…移行可能領域決定部、１０４…探索木記憶部、１０５…軌道作成部、１０７…軌道制御部、
【特許請求の範囲】
【請求項１】
軌道計画システムによって、物体の状態を目標状態へ制御するための軌道を求める軌道計画方法であって、
探索木作成部が、状態空間において、該目標状態を根として、予め複数の区域に分割された該状態空間のそれぞれの区域に含まれる枝のノードの数を制限することによって枝を集約した逆方向探索木を作成するするステップと、
移行可能領域決定部が、該状態空間において、該逆方向探索木上の点に対して、その領域内であれば、該逆方向探索木にしたがって該目標状態に到達しうる、移行可能領域を定めるステップと、
軌道作成部が、該逆方向探索木を使用して、該状態空間における該移行可能領域内の点から該根までの、該物体の軌道を定めるステップと、を含む軌道計画方法。
【請求項２】
前記探索木作成部が、前記状態空間において、前記物体の初期状態を根として、前記状態空間の前記それぞれの区域に含まれる枝のノードの数を制限することによって枝を集約した順方向探索木を作成するするステップと、
前記軌道作成部が、前記移行可能領域のいずれかにおいて、前記順方向探索木による軌道と前記逆方向探索木による軌道を接続して、前記物体の前記初期状態から前記目標状態までの軌道を定めるステップと、をさらに含む請求項１に記載の軌道計画方法。
【請求項３】
前記移行可能領域を定めるステップにおいて、前記移行可能領域決定部は、前記逆方向探索木上の点の近傍の複数の点に対して、フィードバック制御を実施することにより、前記目標状態に到達しうるかどうか定め、その結果に基づいて前記移行可能領域を定める請求項１または２に記載の軌道計画方法。
【請求項４】
前記移行可能領域決定部は、前記目標状態に到達することができなかった点のうち、前記逆方向探索木上の点に最も近い点の、前記逆方向探索木上の点までの距離に基づいて前記移行可能領域を定める請求項３に記載の軌道計画方法。
【請求項５】
請求項１から４のいずれかに記載の軌道計画方法によって求めた軌道にしたがって、前記物体を制御することによって前記物体の状態を前記目標状態とする軌道制御方法。
【請求項６】
物体の状態を目標状態へ制御するための軌道を求める軌道計画システムであって、
状態空間において、該目標状態を根とし、予め複数の区域に分割された状態空間のそれぞれの区域に含まれる枝のノードの数を制限することによって枝を集約した逆方向探索木を作成する探索木作成部と、
該状態空間において、該逆方向探索木上の点に対して、その領域内であれば、該逆方向探索木にしたがって該目標状態に到達しうる、移行可能領域を定める移行可能領域決定部と、
探索木及び移行可能領域を記憶する探索木記憶部と、
該逆方向探索木を使用して該状態空間における該移行可能領域内の点から該根までの、該物体の軌道を定める軌道作成部と、を備える軌道計画システム。
【請求項７】
前記探索木作成部が、前記状態空間において、前記物体の初期状態を根として、前記状態空間のそれぞれの区域に含まれる枝のノードの数を制限することによって枝を集約した順方向探索木を作成し、前記軌道作成部が、前記移行可能領域のいずれかにおいて、前記順方向探索木による軌道と前記逆方向探索木による軌道を接続して、前記物体の前記初期状態から前記目標状態までの軌道を定める請求項６に記載の軌道計画システム。
【請求項８】
請求項６または７に記載の軌道計画システムと、該軌道計画システムが求めた軌道にしたがって、前記物体を制御することによって前記物体の状態を前記目標状態とする軌道制御部とを備えた、軌道計画・制御システム。
【請求項１】
軌道計画システムによって、物体の状態を目標状態へ制御するための軌道を求める軌道計画方法であって、
探索木作成部が、状態空間において、該目標状態を根として、予め複数の区域に分割された該状態空間のそれぞれの区域に含まれる枝のノードの数を制限することによって枝を集約した逆方向探索木を作成するするステップと、
移行可能領域決定部が、該状態空間において、該逆方向探索木上の点に対して、その領域内であれば、該逆方向探索木にしたがって該目標状態に到達しうる、移行可能領域を定めるステップと、
軌道作成部が、該逆方向探索木を使用して、該状態空間における該移行可能領域内の点から該根までの、該物体の軌道を定めるステップと、を含む軌道計画方法。
【請求項２】
前記探索木作成部が、前記状態空間において、前記物体の初期状態を根として、前記状態空間の前記それぞれの区域に含まれる枝のノードの数を制限することによって枝を集約した順方向探索木を作成するするステップと、
前記軌道作成部が、前記移行可能領域のいずれかにおいて、前記順方向探索木による軌道と前記逆方向探索木による軌道を接続して、前記物体の前記初期状態から前記目標状態までの軌道を定めるステップと、をさらに含む請求項１に記載の軌道計画方法。
【請求項３】
前記移行可能領域を定めるステップにおいて、前記移行可能領域決定部は、前記逆方向探索木上の点の近傍の複数の点に対して、フィードバック制御を実施することにより、前記目標状態に到達しうるかどうか定め、その結果に基づいて前記移行可能領域を定める請求項１または２に記載の軌道計画方法。
【請求項４】
前記移行可能領域決定部は、前記目標状態に到達することができなかった点のうち、前記逆方向探索木上の点に最も近い点の、前記逆方向探索木上の点までの距離に基づいて前記移行可能領域を定める請求項３に記載の軌道計画方法。
【請求項５】
請求項１から４のいずれかに記載の軌道計画方法によって求めた軌道にしたがって、前記物体を制御することによって前記物体の状態を前記目標状態とする軌道制御方法。
【請求項６】
物体の状態を目標状態へ制御するための軌道を求める軌道計画システムであって、
状態空間において、該目標状態を根とし、予め複数の区域に分割された状態空間のそれぞれの区域に含まれる枝のノードの数を制限することによって枝を集約した逆方向探索木を作成する探索木作成部と、
該状態空間において、該逆方向探索木上の点に対して、その領域内であれば、該逆方向探索木にしたがって該目標状態に到達しうる、移行可能領域を定める移行可能領域決定部と、
探索木及び移行可能領域を記憶する探索木記憶部と、
該逆方向探索木を使用して該状態空間における該移行可能領域内の点から該根までの、該物体の軌道を定める軌道作成部と、を備える軌道計画システム。
【請求項７】
前記探索木作成部が、前記状態空間において、前記物体の初期状態を根として、前記状態空間のそれぞれの区域に含まれる枝のノードの数を制限することによって枝を集約した順方向探索木を作成し、前記軌道作成部が、前記移行可能領域のいずれかにおいて、前記順方向探索木による軌道と前記逆方向探索木による軌道を接続して、前記物体の前記初期状態から前記目標状態までの軌道を定める請求項６に記載の軌道計画システム。
【請求項８】
請求項６または７に記載の軌道計画システムと、該軌道計画システムが求めた軌道にしたがって、前記物体を制御することによって前記物体の状態を前記目標状態とする軌道制御部とを備えた、軌道計画・制御システム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【公開番号】特開２０１２−２３４２６８（Ｐ２０１２−２３４２６８Ａ）
【公開日】平成２４年１１月２９日（２０１２．１１．２９）
【国際特許分類】
【出願番号】特願２０１１−１００９５１（Ｐ２０１１−１００９５１）
【出願日】平成２３年４月２８日（２０１１．４．２８）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【出願人】（８９９００００６８）学校法人早稲田大学 (602)
【Ｆターム（参考）】
【公開日】平成２４年１１月２９日（２０１２．１１．２９）
【国際特許分類】
【出願日】平成２３年４月２８日（２０１１．４．２８）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【出願人】（８９９００００６８）学校法人早稲田大学 (602)
【Ｆターム（参考）】
[ Back to top ]