軌道計画方法、軌道制御方法、軌道計画システム及び軌道計画・制御システム
【課題】物体の種々の初期状態から目標状態までの軌道を、探索に制約を伴わずに少ない計算コストで求めることのできる軌道計画システム、及び求めた軌道にしたがって目標状態までの軌道を制御することのできる軌道計画・制御システムを提供する。
【解決手段】軌道計画システムは、状態空間において、該目標状態を根とし、該目標状態に至るまでの状態を枝として、予め区分された状態空間の複数のセルのそれぞれに一つの枝を有する探索木を予め作成する探索木作成部（１０１）と、該探索木を記憶する探索木記憶部（１０３）と、該物体の現在の状態を求め、該現在の状態に対応する該探索木の枝を求め、該現在の状態に対応する該探索木の枝から該根までの探索木上の経路を、物体の軌道として求める軌道作成部（１０５）と、を備える。軌道計画・制御システムは、さらに目標状態までの軌道を制御する軌道制御部（１０７）を備える。
【解決手段】軌道計画システムは、状態空間において、該目標状態を根とし、該目標状態に至るまでの状態を枝として、予め区分された状態空間の複数のセルのそれぞれに一つの枝を有する探索木を予め作成する探索木作成部（１０１）と、該探索木を記憶する探索木記憶部（１０３）と、該物体の現在の状態を求め、該現在の状態に対応する該探索木の枝を求め、該現在の状態に対応する該探索木の枝から該根までの探索木上の経路を、物体の軌道として求める軌道作成部（１０５）と、を備える。軌道計画・制御システムは、さらに目標状態までの軌道を制御する軌道制御部（１０７）を備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、物体の、目標状態に至る状態遷移系列を作成する、軌道計画方法、軌道計画システム、作成された軌道にしたがって軌道を制御する軌道制御方法及び軌道計画・制御システムに関する。
【背景技術】
【０００２】
機械の状態（姿勢・速度）は、位相空間上の位置、機械の運動は位相空間上の経路として定めることができる。機械を初期状態から目標状態へと制御したい場合がある。具体例は、以下のとおりである。
【０００３】
第一の具体例は、二足型ロボットがバランスを崩した場合である。この場合の初期状態は、バランスを崩した直後の姿勢・速度であり、目標状態は安定姿勢（直立姿勢かつ速度０など）である。
【０００４】
第二の具体例は、航空機がバランスを崩し失速した場合である。この場合の初期状態は、バランスを崩した直後の姿勢・速度であり、目標状態は、翼とボディを水平にして等速で前進する状態である。
【０００５】
その他の具体例として、バイクが走行中に石に当たった際にバランスを崩し、姿勢を修正したい場合や、自動車がスリップした場合等がある。
【０００６】
機械を初期状態から目標状態へ制御する場合に、ＰＩＤ制御やポテンシャル法を使用すると、機械がバランスを崩した後の状態によっては適切に機能しないという問題がある。この問題は、これらの制御方法は、位相空間上の目標状態までの機械状態遷移において広い範囲の初期状態を取れないことに起因している。また、ＰＩＤ制御やポテンシャル法では、機械が目標状態へ到達するまでの時間を規定することが困難であり、短時間での目標状態復帰が困難である。
【０００７】
状態空間の軌道計画を作成する方法として、状態空間における状態遷移を探索木として記述する方法がある。この方法において、根または枝から、所定数の枝を派生させ、それぞれの枝を探索することが考えられる。しかし、派生させる枝の数をＮとし、木の深さをＭとすると、木の深さＭのレベルで探索の対象となる枝の数は、ＮＭとなる。したがって、派生させる全ての枝を探索する場合には計算コストが膨大となる。
【０００８】
そこで、探索木の枝の評価関数を定め、評価関数にしたがって選択した枝のみから枝を派生させる方法が提案されている（非特許文献１）。この方法によれば、計算コストは抑えられる。しかし、評価関数は、対象ごとに設計者が経験によって決める必要がある。評価関数によっては、状態空間上の探索を十分に行えない場合も生じうる。すなわち、この方法では、状態空間の探索に制約が生じうる。
【０００９】
上記の方法を含め探索木を使用する従来の方法は、リアルタイムの軌道計画作成及び制御に使用するには計算コスト（計算時間）が大きすぎた。
【先行技術文献】
【非特許文献】
【００１０】
【非特許文献１】Pedro S. Huang:”Planning For Dynamic Motions Using a Search Tree”, a graduate thesis of Toronto Univ., (1996).
【発明の概要】
【発明が解決しようとする課題】
【００１１】
したがって、物体、すなわち機械の種々の初期状態から目標状態までの軌道を、探索に制約を伴わずに少ない計算コストで求めることのできる軌道計画方法、軌道計画システム、及び求めた軌道にしたがって目標状態までの軌道を制御することのできる軌道制御方法及び軌道計画・制御システムに対するニーズがある。
【課題を解決するための手段】
【００１２】
本発明の第１の態様による軌道計画方法は、軌道計画システムによって、物体の状態を目標状態へ制御するための軌道を求める軌道計画方法である。本方法は、探索木作成部が、状態空間において、該目標状態を根とし、該目標状態に至るまでの状態を枝として、予め複数の区域に分割された状態空間の該複数の区域のそれぞれに一つの枝を有する探索木を予め作成するするステップと、軌道作成部が、該物体の現在の状態を求めるステップと、該軌道作成部が、該現在の状態に対応する該探索木の枝を求めるステップと、該軌道作成部が、該現在の状態に対応する該探索木の枝から該根までの探索木上の経路を、物体の軌道として求めるステップと、を含む。
【００１３】
本態様の軌道計画方法によれば、予め複数の区域に分割された状態空間の該複数の区域のそれぞれに一つの枝を有する探索木を予め作成しておき、その探索木上の経路として物体の軌道を求めるので、物体の種々の初期状態から目標状態までの軌道を、探索に制約を伴わずに少ない計算コストで求めることができる。
【００１４】
本態様の軌道計画方法の実施形態によれば、状態が属することのできる状態空間の範囲を定めることにより、物体の動作を制限する。
【００１５】
本実施形態によれば、たとえば、障害物が存在する範囲を、状態が属することのできる範囲から除外することにより、障害物を回避して物体を動作させることが可能となる。
【００１６】
本態様の軌道計画方法の実施形態によれば、前記所定の時間間隔における力学的な関係に制約条件を定めることにより、物体の動作を制限する。
【００１７】
本実施形態によれば、たとえば、与えるトルクの上限値を定めることにより、計画される物体の動作が実現可能なものとなるようにすることができる。
【００１８】
本態様の軌道計画方法の実施形態によれば、該物体が、先端部を除く端部がジョイントによって接続された、１または複数の部材である。
【００１９】
本実施形態によれば、先端部を除く端部がジョイントによって接続された、１または複数の部材である物体の軌道計画を、低い計算コストで、探索に制約を伴わずに求めることができる。
【００２０】
本態様の軌道計画方法の実施形態によれば、該物体が、ロボットの部材である。
【００２１】
本実施形態によれば、ロボットの部材の軌道計画を、低い計算コストで、探索に制約を伴わずに求めることができる。
【００２２】
本態様の軌道計画方法の実施形態によれば、状態空間が位相空間である。
【００２３】
本実施形態によれば、位相空間を使用することにより、効率的に軌道を求めることができる。
【００２４】
本発明の第１の態様による軌道制御方法は、本発明の第１の態様のいずれかの軌道計画方法によって求めた前記探索木上の経路にしたがって、前記物体の軌道を制御することによって前記物体の状態を前記目標状態とする。
【００２５】
本態様の軌道制御方法によれば、種々の初期状態から目標状態までの軌道制御を少ない計算コストで実施することができる。
【００２６】
本態様の軌道制御方法の実施形態によれば、前記物体の現在の状態に近い探索木の枝を定めることによって前記探索木上の新たな経路を求め、前記探索木上の従来の経路または該新たな経路のいずれかにしたがって、前記物体の軌道を制御することによって前記物体の状態を前記目標状態とする。
【００２７】
本実施形態によれば、物体の現在状態が当初の経路、すなわち軌道から外れた場合でも、新たな経路を求めるための計算コストが小さいのでリアルタイムに対応することができる。
【００２８】
本発明の第２の態様による軌道計画システムは、物体の状態を目標状態へ制御するための軌道を求める軌道計画システムである。本態様による軌道計画システムは、状態空間において、該目標状態を根とし、該目標状態に至るまでの状態を枝として、予め複数の区域に分割された状態空間の該複数の区域のそれぞれに一つの枝を有する探索木を予め作成する探索木作成部と、該探索木を記憶する探索木記憶部と、該物体の現在の状態を求め、該現在の状態に対応する該探索木の枝を求め、該現在の状態に対応する該探索木の枝から該根までの探索木上の経路を、物体の軌道として求める軌道作成部と、を備える。
【００２９】
本態様の軌道計画システムによれば、探索木作成部が、予め複数の区域に分割された状態空間の該複数の区域のそれぞれに一つの枝を有する探索木を予め作成しておき、軌道作成部が、その探索木上の経路として物体の軌道を求めるので、物体の種々の初期状態から目標状態までの軌道を、探索に制約を伴わずに少ない計算コストで求めることができる。
【００３０】
本態様の軌道計画システムの実施形態によれば、状態が属することのできる状態空間の範囲を定めることにより、物体の動作を制限する。
【００３１】
本実施形態によれば、たとえば、障害物が存在する範囲を、状態が属することのできる範囲から除外することにより、障害物を回避して物体を動作させることが可能となる。
【００３２】
本態様の軌道計画システムの実施形態によれば、前記所定の時間間隔における力学的な関係に制約条件を定めることにより、物体の動作を制限する。
【００３３】
本実施形態によれば、たとえば、与えるトルクの上限値を定めることにより、計画される物体の動作が実現可能なものとなるようにすることができる。
【００３４】
本態様の軌道計画システムの実施形態によれば、状態空間が位相空間である。
【００３５】
本実施形態によれば、位相空間を使用することにより、効率的に軌道を求めることができる。
【００３６】
本発明の第２の態様による軌道計画・制御システムは、本発明の第２の態様のいずれかの軌道計画システムと、該軌道計画システムが求めた、前記探索木上の経路にしたがって、前記物体の軌道を制御することによって前記物体の状態を前記目標状態とする軌道制御部とを備えている。
【００３７】
本態様の軌道計画・制御システムによれば、物体の種々の初期状態から目標状態までの軌道制御を少ない計算コストで実施することができる。
【００３８】
本発明の第２の態様によるロボットは、本発明の第２の態様による軌道計画・制御システムを備えたロボットであって、該物体が、該ロボットの部材であり、前記探索木上の経路にしたがって、該ロボットの部材を制御する。
【００３９】
本態様のロボットによれば、該ロボットの部材の種々の初期状態から目標状態までの軌道制御を少ない計算コストで実施することができる。
【図面の簡単な説明】
【００４０】
【図１】本発明の一実施形態による軌道計画・制御システムの構成を示す図である。
【図２】二自由度倒立振子の構成を示す図である。
【図３】探索木作成部が位相空間上に探索木を作成する手順を示す流れ図である。
【図４】図３のステップＳ１０２０及びステップＳ１０３０の動作を説明するための図である。
【図５】複数の区域に分割された位相空間を示す図である。
【図６】図３のステップＳ１０５０及びステップＳ１０６０の動作を説明するための図である。
【図７】軌道作成部及び軌道制御部による軌道作成及び軌道制御手順を説明するための流れ図である。
【図８】位置・速度誤差フィードバック制御を説明するための流れ図である。
【図９】図７のステップＳ２０２０の動作を説明するための図である。
【図１０】図７のステップＳ２０８０の動作を説明するための図である。
【図１１】二自由度倒立振子の初期状態を示す図である。
【図１２】二自由度倒立振子の目標状態を示す図である。
【図１３】実験による、二自由度倒立振子の第１関節の角度ｑ１及び第２関節の角度ｑ２の遷移を示す図である。
【図１４】別の実験による、二自由度倒立振子の第１関節の角度ｑ１及び第２関節の角度ｑ２の遷移を示す図である。
【発明を実施するための形態】
【００４１】
図１は、本発明の一実施形態による軌道計画・制御システム１００の構成を示す図である。軌道計画・制御システム１００は、状態空間上の物体の状態に対応する枝を備えた探索木を作成する探索木作成部１０１と、探索木作成部１０１によって作成された探索木を記憶する探索木記憶部１０３と、探索木記憶部１０３に記憶された探索木を使用して目標状態までの軌道を作成する軌道作成部１０５と、軌道作成部１０５によって作成された軌道を使用して軌道制御を行う軌道制御部１０７と、物体の位置を検出する位置センサ１０９と、物体の運動を定めるアクチュエータ１１１と、を備える。各構成部の機能の詳細については、後で説明する。
【００４２】
図２は、二自由度倒立振子２００の構成を示す図である。以下の実施形態においては、２次元平面内で運動する二自由度倒立振子２００を物体として説明を行なう。二自由度倒立振子２００は、第１リンク２０５と、第２リンク２１１と、第１リンク２０５を回転可能に固定点に接続する第１関節（ジョイント）２０１と、第１リンク２０５及び第２リンク２１１を回転可能に接続する第２関節２０７と、から構成される。第１リンク２０５及び第２リンク２１１の重心を、それぞれ符号２０３及び２０９によって示す。
【００４３】
二自由度倒立振子２００の運動は、第１関節２０１及び第２関節２０７に与えるトルクによって定まり、第１リンク２０５のＸ軸に対する角度
【数１】
第２リンク２１１の第１リンク２０５に対する角度
【数２】
及びこれらの角速度
【数３】
によって表現される。そこで、本実施形態において、状態空間として以下の位相空間を採用する。
【数４】
【００４４】
図３は、探索木作成部１０１が位相空間上に探索木を作成する手順を示す流れ図である。
【００４５】
図３のステップＳ１０１０において、探索木作成部１０１は物体の目標状態を得る。
【００４６】
図３のステップＳ１０２０において、探索木作成部１０１は、位相空間において物体の目標状態を探索木の根として定める。
【００４７】
図３のステップＳ１０３０において、探索木作成部１０１が、位相空間において、所定の時間間隔前、すなわち、時刻
の状態を探索木の枝として定める。所定の時間間隔前の状態は、運動方程式や動力学シミュレータによって定める。上述のように、二自由度倒立振子２００の運動は、第１関節２０１及び第２関節２０７に与えるトルクによって定まるので、所定の時間間隔に第１関節２０１及び第２関節２０７に与えるトルクを定めれば、二自由度倒立振子２００の、所定の時間間隔前の状態が定まる。そこで、所定の時間間隔に第１関節２０１及び第２関節２０７に与えるトルクを最大値と最小値との間で、所定の増分で変化させながら、それぞれの場合に対応する時間間隔前の状態を求めてもよい。この場合に、トルクを最大値と最小値との間で、所定の増分で変化させながら、幅優先探索により網羅的に探索を行なってもよい。
【００４８】
図４は、図３のステップＳ１０２０及びステップＳ１０３０の動作を説明するための図である。図４において、円形は状態を示し、黒い円形は目標状態を示す。また、点線の矢印は計算の順序を示す。なお、図４は、簡単のために位相空間を２次元空間として表している。黒い丸で示す目標状態にある、二自由度倒立振子２００に、目標状態に至るまでの所定の時間間隔に３個の異なるトルク
【数５】
が与えられたとして、３個の異なる所定の時間間隔前の状態が計算される。探索木作成部１０１は、これらの状態を探索木の枝とする。さらに、生成された枝が示すそれぞれの状態に対して、３個の異なるトルク
【数６】
が与えられたとして、３個の異なる、さらに所定の時間間隔前の状態が計算される。
【００４９】
ここで、位相空間を予め複数の区域に分割しておく。
【００５０】
図５は、複数の区域に分割された位相空間を示す図である。分割は、想定される角度及び角速度の範囲を等分割してもよい。以下において、複数の区域のそれぞれをセルと呼称する。
【００５１】
図３のステップＳ１０４０において、探索木作成部１０１は、所定の時間間隔前の状態の属するセルにすでに探索木の枝が登録されているかどうか判断する。すでに探索木の枝が登録されていれば、ステップＳ１０６０に進む。探索木の枝が登録されていなければ、ステップＳ１０４５に進む。
【００５２】
図３のステップＳ１０４５において、探索木作成部１０１は、探索木作成部１０１が派生させた枝が表す所定の時間間隔前の状態が位相空間上の制約条件を満たすかどうか判断する。位相空間上の制約条件とは、物体である機械の性能または外部の障害物などにより、状態が存在することのできない位相空間の領域に対応する制約条件である。所定の時間間隔前の状態が位相空間上の制約条件を満たせば、ステップＳ０５０に進む。所定の時間間隔前の状態が位相空間上の制約条件を満たさなければ、ステップＳ０６０に進む。
【００５３】
図３のステップＳ１０５０において、探索木作成部１０１は、探索木作成部１０１が派生させた枝が表す所定の時間間隔前の状態を探索木の枝として登録する。
【００５４】
図３のステップＳ１０６０において、探索木作成部１０１は、探索木作成部１０１が派生させた枝が表す所定の時間間隔前の状態を破棄する。
【００５５】
図６は、図３のステップＳ１０５０及びステップＳ１０６０の動作を説明するための図である。図６において、円形は状態を示し、黒い円形は目標状態を示す。また、実線の矢印は状態の遷移を示し、点線の矢印は計算の順序を示す。実線で示される状態の遷移の方向と点線で示される計算の順序の方向は逆である。探索木の枝は、原則として１個のセル内には１本しか存在しない。探索木作成部１０１が、探索木の枝がすでに存在するセルに新たな枝を定めた場合には、その枝（状態）は破棄される。すなわち、位相空間上の位置が近い状態はセルごとに集約される。このように、従来の探索木の枝を、セルを使用して集約することによって、計算コストを大幅に低減することができる。
【００５６】
図３のステップＳ１０７０において、探索木作成部１０１は、探索が終了したかどうか判断する。探索木作成部１０１は、探索木の所定の深さまでの探索を終えた場合、あるいは所定時間の推移まで探索を終えた場合に探索が終了したと判断してもよい。探索が終了したと判断された場合には、処理が終了する。探索が終了していないと判断された場合には、ステップＳ１０３０に戻り、探索が継続される。
【００５７】
図７は、軌道作成部１０５及び軌道制御部１０７による軌道作成及び軌道制御手順を説明するための流れ図である。
【００５８】
図７のステップＳ２０１０において、軌道作成部１０５は、物体の現在状態を得る。具体的に、軌道作成部１０５は、第１ジョイント２０１及び第２ジョイント２０７に設けられた角度センサからの情報に基づいて、二自由度倒立振子２００の現在状態を得る。ここで、角度センサは、図１の位置センサ１０９に相当する。
【００５９】
図７のステップＳ２０２０において、軌道作成部１０５は、現在状態が属するセルを特定し、同じセルに属する探索木上の状態、すなわち枝を特定する。
【００６０】
図９は、図７のステップＳ２０２０の動作を説明するための図である。図９において、円形は状態、斜線を付した円形は現在状態、黒い円形は目標状態を示す。また、実線の矢印は状態の遷移を示す。斜線で示した現在状態の属するセルに属する探索木の枝が特定される。
【００６１】
図７のステップＳ２０３０において、軌道作成部１０５は、特定された探索木上の状態（枝）から目標状態（根）までの経路を特定する。図９において、特定された経路を太い線で表した。
【００６２】
図７のステップＳ２０４０において、軌道制御部１０７が、軌道作成部１０５によって特定された経路に対して、位置・速度誤差フィードバック制御を行う。位置・速度誤差フィードバック制御については後で詳細に説明する。
【００６３】
図７のステップＳ２０５０において、軌道作成部１０５は、物体の次の現在状態を得る。
【００６４】
図７のステップＳ２０６０において、軌道作成部１０５は、次の現在状態が属するセルを特定し、同じセルに属する探索木上の状態、すなわち枝を特定する。
【００６５】
図７のステップＳ２０７０において、軌道作成部１０５は、特定された探索木上の状態（枝）から目標状態（根）までの経路を特定する。
【００６６】
図７のステップＳ２０８０において、軌道作成部１０５は、従来特定した経路と新たに特定した経路に対し、位置・速度誤差フィードバック制御を用いたシミュレーションを行い、両経路について最終状態を得る。
【００６７】
図１０は、図７のステップＳ２０８０の動作を説明するための図である。図１０において、円形は状態、斜線を付した円形は位置センサ１０９によって検出された現在状態、黒い円形は目標状態を示す。また、実線の矢印は状態の遷移を示す。ノードＡは、従来特定した経路上の状態であり、ノードＢは、新たに特定した経路上の状態である。
【００６８】
図７のステップＳ２０９０において、軌道作成部１０５は、従来の経路及び新たな経路の、シミュレーションによって得られた最終状態を比較し、最終状態が目標状態に近い方の経路を選択する。図１０は、ノードＢを含む、太い線で示す経路が選択された様子を示す。
【００６９】
図７のステップＳ２１００において、軌道制御部１０７が、軌道作成部１０５によって選択された経路に対して、位置・速度誤差フィードバック制御を行う。
【００７０】
図７のステップＳ２１１０において、軌道制御部１０７が、選択された経路の末端に到達したかどうか判断する。選択された経路の末端に到達していれば、処理を終了する。選択された経路の末端に到達していなければ、ステップＳ２０５０に戻る。
【００７１】
図７に示した軌道作成及び軌道制御手順の計算コストは非常に小さい。具体的に、図７のステップＳ２０５０乃至ステップＳ２１１０の計算は、０．０１秒以内で実施することができた。計算は、インテルCore i7-640Mプロセッサ（２．８ＧＨｚ）を単スレッドで利用して実施した。また、図７に示した軌道作成及び軌道制御手順によれば、物体の現在状態が当初の経路、すなわち軌道から外れた場合でも、新たな経路を求めるための計算コストが小さいのでリアルタイムに対応することができる。
【００７２】
ここで、位置・速度誤差フィードバック制御について説明する。
【００７３】
図８は、位置・速度誤差フィードバック制御を説明するための流れ図である。
【００７４】
図８のステップＳ３０１０において、軌道制御部１０７は、探索木の枝より、ｎステップ先の短期目標状態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).
【発明の概要】
【発明が解決しようとする課題】
【００１１】
したがって、物体、すなわち機械の種々の初期状態から目標状態までの軌道を、探索に制約を伴わずに少ない計算コストで求めることのできる軌道計画方法、軌道計画システム、及び求めた軌道にしたがって目標状態までの軌道を制御することのできる軌道制御方法及び軌道計画・制御システムに対するニーズがある。
【課題を解決するための手段】
【００１２】
本発明の第１の態様による軌道計画方法は、軌道計画システムによって、物体の状態を目標状態へ制御するための軌道を求める軌道計画方法である。本方法は、探索木作成部が、状態空間において、該目標状態を根とし、該目標状態に至るまでの状態を枝として、予め複数の区域に分割された状態空間の該複数の区域のそれぞれに一つの枝を有する探索木を予め作成するするステップと、軌道作成部が、該物体の現在の状態を求めるステップと、該軌道作成部が、該現在の状態に対応する該探索木の枝を求めるステップと、該軌道作成部が、該現在の状態に対応する該探索木の枝から該根までの探索木上の経路を、物体の軌道として求めるステップと、を含む。
【００１３】
本態様の軌道計画方法によれば、予め複数の区域に分割された状態空間の該複数の区域のそれぞれに一つの枝を有する探索木を予め作成しておき、その探索木上の経路として物体の軌道を求めるので、物体の種々の初期状態から目標状態までの軌道を、探索に制約を伴わずに少ない計算コストで求めることができる。
【００１４】
本態様の軌道計画方法の実施形態によれば、状態が属することのできる状態空間の範囲を定めることにより、物体の動作を制限する。
【００１５】
本実施形態によれば、たとえば、障害物が存在する範囲を、状態が属することのできる範囲から除外することにより、障害物を回避して物体を動作させることが可能となる。
【００１６】
本態様の軌道計画方法の実施形態によれば、前記所定の時間間隔における力学的な関係に制約条件を定めることにより、物体の動作を制限する。
【００１７】
本実施形態によれば、たとえば、与えるトルクの上限値を定めることにより、計画される物体の動作が実現可能なものとなるようにすることができる。
【００１８】
本態様の軌道計画方法の実施形態によれば、該物体が、先端部を除く端部がジョイントによって接続された、１または複数の部材である。
【００１９】
本実施形態によれば、先端部を除く端部がジョイントによって接続された、１または複数の部材である物体の軌道計画を、低い計算コストで、探索に制約を伴わずに求めることができる。
【００２０】
本態様の軌道計画方法の実施形態によれば、該物体が、ロボットの部材である。
【００２１】
本実施形態によれば、ロボットの部材の軌道計画を、低い計算コストで、探索に制約を伴わずに求めることができる。
【００２２】
本態様の軌道計画方法の実施形態によれば、状態空間が位相空間である。
【００２３】
本実施形態によれば、位相空間を使用することにより、効率的に軌道を求めることができる。
【００２４】
本発明の第１の態様による軌道制御方法は、本発明の第１の態様のいずれかの軌道計画方法によって求めた前記探索木上の経路にしたがって、前記物体の軌道を制御することによって前記物体の状態を前記目標状態とする。
【００２５】
本態様の軌道制御方法によれば、種々の初期状態から目標状態までの軌道制御を少ない計算コストで実施することができる。
【００２６】
本態様の軌道制御方法の実施形態によれば、前記物体の現在の状態に近い探索木の枝を定めることによって前記探索木上の新たな経路を求め、前記探索木上の従来の経路または該新たな経路のいずれかにしたがって、前記物体の軌道を制御することによって前記物体の状態を前記目標状態とする。
【００２７】
本実施形態によれば、物体の現在状態が当初の経路、すなわち軌道から外れた場合でも、新たな経路を求めるための計算コストが小さいのでリアルタイムに対応することができる。
【００２８】
本発明の第２の態様による軌道計画システムは、物体の状態を目標状態へ制御するための軌道を求める軌道計画システムである。本態様による軌道計画システムは、状態空間において、該目標状態を根とし、該目標状態に至るまでの状態を枝として、予め複数の区域に分割された状態空間の該複数の区域のそれぞれに一つの枝を有する探索木を予め作成する探索木作成部と、該探索木を記憶する探索木記憶部と、該物体の現在の状態を求め、該現在の状態に対応する該探索木の枝を求め、該現在の状態に対応する該探索木の枝から該根までの探索木上の経路を、物体の軌道として求める軌道作成部と、を備える。
【００２９】
本態様の軌道計画システムによれば、探索木作成部が、予め複数の区域に分割された状態空間の該複数の区域のそれぞれに一つの枝を有する探索木を予め作成しておき、軌道作成部が、その探索木上の経路として物体の軌道を求めるので、物体の種々の初期状態から目標状態までの軌道を、探索に制約を伴わずに少ない計算コストで求めることができる。
【００３０】
本態様の軌道計画システムの実施形態によれば、状態が属することのできる状態空間の範囲を定めることにより、物体の動作を制限する。
【００３１】
本実施形態によれば、たとえば、障害物が存在する範囲を、状態が属することのできる範囲から除外することにより、障害物を回避して物体を動作させることが可能となる。
【００３２】
本態様の軌道計画システムの実施形態によれば、前記所定の時間間隔における力学的な関係に制約条件を定めることにより、物体の動作を制限する。
【００３３】
本実施形態によれば、たとえば、与えるトルクの上限値を定めることにより、計画される物体の動作が実現可能なものとなるようにすることができる。
【００３４】
本態様の軌道計画システムの実施形態によれば、状態空間が位相空間である。
【００３５】
本実施形態によれば、位相空間を使用することにより、効率的に軌道を求めることができる。
【００３６】
本発明の第２の態様による軌道計画・制御システムは、本発明の第２の態様のいずれかの軌道計画システムと、該軌道計画システムが求めた、前記探索木上の経路にしたがって、前記物体の軌道を制御することによって前記物体の状態を前記目標状態とする軌道制御部とを備えている。
【００３７】
本態様の軌道計画・制御システムによれば、物体の種々の初期状態から目標状態までの軌道制御を少ない計算コストで実施することができる。
【００３８】
本発明の第２の態様によるロボットは、本発明の第２の態様による軌道計画・制御システムを備えたロボットであって、該物体が、該ロボットの部材であり、前記探索木上の経路にしたがって、該ロボットの部材を制御する。
【００３９】
本態様のロボットによれば、該ロボットの部材の種々の初期状態から目標状態までの軌道制御を少ない計算コストで実施することができる。
【図面の簡単な説明】
【００４０】
【図１】本発明の一実施形態による軌道計画・制御システムの構成を示す図である。
【図２】二自由度倒立振子の構成を示す図である。
【図３】探索木作成部が位相空間上に探索木を作成する手順を示す流れ図である。
【図４】図３のステップＳ１０２０及びステップＳ１０３０の動作を説明するための図である。
【図５】複数の区域に分割された位相空間を示す図である。
【図６】図３のステップＳ１０５０及びステップＳ１０６０の動作を説明するための図である。
【図７】軌道作成部及び軌道制御部による軌道作成及び軌道制御手順を説明するための流れ図である。
【図８】位置・速度誤差フィードバック制御を説明するための流れ図である。
【図９】図７のステップＳ２０２０の動作を説明するための図である。
【図１０】図７のステップＳ２０８０の動作を説明するための図である。
【図１１】二自由度倒立振子の初期状態を示す図である。
【図１２】二自由度倒立振子の目標状態を示す図である。
【図１３】実験による、二自由度倒立振子の第１関節の角度ｑ１及び第２関節の角度ｑ２の遷移を示す図である。
【図１４】別の実験による、二自由度倒立振子の第１関節の角度ｑ１及び第２関節の角度ｑ２の遷移を示す図である。
【発明を実施するための形態】
【００４１】
図１は、本発明の一実施形態による軌道計画・制御システム１００の構成を示す図である。軌道計画・制御システム１００は、状態空間上の物体の状態に対応する枝を備えた探索木を作成する探索木作成部１０１と、探索木作成部１０１によって作成された探索木を記憶する探索木記憶部１０３と、探索木記憶部１０３に記憶された探索木を使用して目標状態までの軌道を作成する軌道作成部１０５と、軌道作成部１０５によって作成された軌道を使用して軌道制御を行う軌道制御部１０７と、物体の位置を検出する位置センサ１０９と、物体の運動を定めるアクチュエータ１１１と、を備える。各構成部の機能の詳細については、後で説明する。
【００４２】
図２は、二自由度倒立振子２００の構成を示す図である。以下の実施形態においては、２次元平面内で運動する二自由度倒立振子２００を物体として説明を行なう。二自由度倒立振子２００は、第１リンク２０５と、第２リンク２１１と、第１リンク２０５を回転可能に固定点に接続する第１関節（ジョイント）２０１と、第１リンク２０５及び第２リンク２１１を回転可能に接続する第２関節２０７と、から構成される。第１リンク２０５及び第２リンク２１１の重心を、それぞれ符号２０３及び２０９によって示す。
【００４３】
二自由度倒立振子２００の運動は、第１関節２０１及び第２関節２０７に与えるトルクによって定まり、第１リンク２０５のＸ軸に対する角度
【数１】
第２リンク２１１の第１リンク２０５に対する角度
【数２】
及びこれらの角速度
【数３】
によって表現される。そこで、本実施形態において、状態空間として以下の位相空間を採用する。
【数４】
【００４４】
図３は、探索木作成部１０１が位相空間上に探索木を作成する手順を示す流れ図である。
【００４５】
図３のステップＳ１０１０において、探索木作成部１０１は物体の目標状態を得る。
【００４６】
図３のステップＳ１０２０において、探索木作成部１０１は、位相空間において物体の目標状態を探索木の根として定める。
【００４７】
図３のステップＳ１０３０において、探索木作成部１０１が、位相空間において、所定の時間間隔前、すなわち、時刻
の状態を探索木の枝として定める。所定の時間間隔前の状態は、運動方程式や動力学シミュレータによって定める。上述のように、二自由度倒立振子２００の運動は、第１関節２０１及び第２関節２０７に与えるトルクによって定まるので、所定の時間間隔に第１関節２０１及び第２関節２０７に与えるトルクを定めれば、二自由度倒立振子２００の、所定の時間間隔前の状態が定まる。そこで、所定の時間間隔に第１関節２０１及び第２関節２０７に与えるトルクを最大値と最小値との間で、所定の増分で変化させながら、それぞれの場合に対応する時間間隔前の状態を求めてもよい。この場合に、トルクを最大値と最小値との間で、所定の増分で変化させながら、幅優先探索により網羅的に探索を行なってもよい。
【００４８】
図４は、図３のステップＳ１０２０及びステップＳ１０３０の動作を説明するための図である。図４において、円形は状態を示し、黒い円形は目標状態を示す。また、点線の矢印は計算の順序を示す。なお、図４は、簡単のために位相空間を２次元空間として表している。黒い丸で示す目標状態にある、二自由度倒立振子２００に、目標状態に至るまでの所定の時間間隔に３個の異なるトルク
【数５】
が与えられたとして、３個の異なる所定の時間間隔前の状態が計算される。探索木作成部１０１は、これらの状態を探索木の枝とする。さらに、生成された枝が示すそれぞれの状態に対して、３個の異なるトルク
【数６】
が与えられたとして、３個の異なる、さらに所定の時間間隔前の状態が計算される。
【００４９】
ここで、位相空間を予め複数の区域に分割しておく。
【００５０】
図５は、複数の区域に分割された位相空間を示す図である。分割は、想定される角度及び角速度の範囲を等分割してもよい。以下において、複数の区域のそれぞれをセルと呼称する。
【００５１】
図３のステップＳ１０４０において、探索木作成部１０１は、所定の時間間隔前の状態の属するセルにすでに探索木の枝が登録されているかどうか判断する。すでに探索木の枝が登録されていれば、ステップＳ１０６０に進む。探索木の枝が登録されていなければ、ステップＳ１０４５に進む。
【００５２】
図３のステップＳ１０４５において、探索木作成部１０１は、探索木作成部１０１が派生させた枝が表す所定の時間間隔前の状態が位相空間上の制約条件を満たすかどうか判断する。位相空間上の制約条件とは、物体である機械の性能または外部の障害物などにより、状態が存在することのできない位相空間の領域に対応する制約条件である。所定の時間間隔前の状態が位相空間上の制約条件を満たせば、ステップＳ０５０に進む。所定の時間間隔前の状態が位相空間上の制約条件を満たさなければ、ステップＳ０６０に進む。
【００５３】
図３のステップＳ１０５０において、探索木作成部１０１は、探索木作成部１０１が派生させた枝が表す所定の時間間隔前の状態を探索木の枝として登録する。
【００５４】
図３のステップＳ１０６０において、探索木作成部１０１は、探索木作成部１０１が派生させた枝が表す所定の時間間隔前の状態を破棄する。
【００５５】
図６は、図３のステップＳ１０５０及びステップＳ１０６０の動作を説明するための図である。図６において、円形は状態を示し、黒い円形は目標状態を示す。また、実線の矢印は状態の遷移を示し、点線の矢印は計算の順序を示す。実線で示される状態の遷移の方向と点線で示される計算の順序の方向は逆である。探索木の枝は、原則として１個のセル内には１本しか存在しない。探索木作成部１０１が、探索木の枝がすでに存在するセルに新たな枝を定めた場合には、その枝（状態）は破棄される。すなわち、位相空間上の位置が近い状態はセルごとに集約される。このように、従来の探索木の枝を、セルを使用して集約することによって、計算コストを大幅に低減することができる。
【００５６】
図３のステップＳ１０７０において、探索木作成部１０１は、探索が終了したかどうか判断する。探索木作成部１０１は、探索木の所定の深さまでの探索を終えた場合、あるいは所定時間の推移まで探索を終えた場合に探索が終了したと判断してもよい。探索が終了したと判断された場合には、処理が終了する。探索が終了していないと判断された場合には、ステップＳ１０３０に戻り、探索が継続される。
【００５７】
図７は、軌道作成部１０５及び軌道制御部１０７による軌道作成及び軌道制御手順を説明するための流れ図である。
【００５８】
図７のステップＳ２０１０において、軌道作成部１０５は、物体の現在状態を得る。具体的に、軌道作成部１０５は、第１ジョイント２０１及び第２ジョイント２０７に設けられた角度センサからの情報に基づいて、二自由度倒立振子２００の現在状態を得る。ここで、角度センサは、図１の位置センサ１０９に相当する。
【００５９】
図７のステップＳ２０２０において、軌道作成部１０５は、現在状態が属するセルを特定し、同じセルに属する探索木上の状態、すなわち枝を特定する。
【００６０】
図９は、図７のステップＳ２０２０の動作を説明するための図である。図９において、円形は状態、斜線を付した円形は現在状態、黒い円形は目標状態を示す。また、実線の矢印は状態の遷移を示す。斜線で示した現在状態の属するセルに属する探索木の枝が特定される。
【００６１】
図７のステップＳ２０３０において、軌道作成部１０５は、特定された探索木上の状態（枝）から目標状態（根）までの経路を特定する。図９において、特定された経路を太い線で表した。
【００６２】
図７のステップＳ２０４０において、軌道制御部１０７が、軌道作成部１０５によって特定された経路に対して、位置・速度誤差フィードバック制御を行う。位置・速度誤差フィードバック制御については後で詳細に説明する。
【００６３】
図７のステップＳ２０５０において、軌道作成部１０５は、物体の次の現在状態を得る。
【００６４】
図７のステップＳ２０６０において、軌道作成部１０５は、次の現在状態が属するセルを特定し、同じセルに属する探索木上の状態、すなわち枝を特定する。
【００６５】
図７のステップＳ２０７０において、軌道作成部１０５は、特定された探索木上の状態（枝）から目標状態（根）までの経路を特定する。
【００６６】
図７のステップＳ２０８０において、軌道作成部１０５は、従来特定した経路と新たに特定した経路に対し、位置・速度誤差フィードバック制御を用いたシミュレーションを行い、両経路について最終状態を得る。
【００６７】
図１０は、図７のステップＳ２０８０の動作を説明するための図である。図１０において、円形は状態、斜線を付した円形は位置センサ１０９によって検出された現在状態、黒い円形は目標状態を示す。また、実線の矢印は状態の遷移を示す。ノードＡは、従来特定した経路上の状態であり、ノードＢは、新たに特定した経路上の状態である。
【００６８】
図７のステップＳ２０９０において、軌道作成部１０５は、従来の経路及び新たな経路の、シミュレーションによって得られた最終状態を比較し、最終状態が目標状態に近い方の経路を選択する。図１０は、ノードＢを含む、太い線で示す経路が選択された様子を示す。
【００６９】
図７のステップＳ２１００において、軌道制御部１０７が、軌道作成部１０５によって選択された経路に対して、位置・速度誤差フィードバック制御を行う。
【００７０】
図７のステップＳ２１１０において、軌道制御部１０７が、選択された経路の末端に到達したかどうか判断する。選択された経路の末端に到達していれば、処理を終了する。選択された経路の末端に到達していなければ、ステップＳ２０５０に戻る。
【００７１】
図７に示した軌道作成及び軌道制御手順の計算コストは非常に小さい。具体的に、図７のステップＳ２０５０乃至ステップＳ２１１０の計算は、０．０１秒以内で実施することができた。計算は、インテルCore i7-640Mプロセッサ（２．８ＧＨｚ）を単スレッドで利用して実施した。また、図７に示した軌道作成及び軌道制御手順によれば、物体の現在状態が当初の経路、すなわち軌道から外れた場合でも、新たな経路を求めるための計算コストが小さいのでリアルタイムに対応することができる。
【００７２】
ここで、位置・速度誤差フィードバック制御について説明する。
【００７３】
図８は、位置・速度誤差フィードバック制御を説明するための流れ図である。
【００７４】
図８のステップＳ３０１０において、軌道制御部１０７は、探索木の枝より、ｎステップ先の短期目標状態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)
【Ｆターム（参考）】
【公開日】平成２４年８月３０日（２０１２．８．３０）
【国際特許分類】
【出願日】平成２３年２月４日（２０１１．２．４）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【Ｆターム（参考）】
[ Back to top ]