軌道計画方法、軌道計画システム及びロボット
【課題】計算コストを抑えながら状態空間の探索に制約が生じることのない軌道計画方法及び軌道計画システムを提供する。
【解決手段】物体の状態を状態空間において探索木の根として定め、所定の時間間隔における力学的な関係から時間経過した後の該物体の次の状態を状態空間において探索木の枝として定める探索木作成部１０１を備える。さらに既知状態登録木を記憶する既知状態登録木記憶部１０５と、予め区分された状態空間の複数のセルから次の状態が属するセルを定め、該セルに属する状態が既知状態登録木の枝としてすでに登録されているかどうか判断し、登録されている場合に次の状態を破棄し、そうでない場合に次の状態を該既知状態登録木の枝として登録する既知状態登録木作成部１０３と、を備える。既知状態登録木の枝として登録された状態の中から目標状態までの距離がもっとも小さい状態を選択し、その状態から軌道を求める軌道作成部１０７を備える。
【解決手段】物体の状態を状態空間において探索木の根として定め、所定の時間間隔における力学的な関係から時間経過した後の該物体の次の状態を状態空間において探索木の枝として定める探索木作成部１０１を備える。さらに既知状態登録木を記憶する既知状態登録木記憶部１０５と、予め区分された状態空間の複数のセルから次の状態が属するセルを定め、該セルに属する状態が既知状態登録木の枝としてすでに登録されているかどうか判断し、登録されている場合に次の状態を破棄し、そうでない場合に次の状態を該既知状態登録木の枝として登録する既知状態登録木作成部１０３と、を備える。既知状態登録木の枝として登録された状態の中から目標状態までの距離がもっとも小さい状態を選択し、その状態から軌道を求める軌道作成部１０７を備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、物体の、目標状態に至る状態遷移系列を作成する、軌道計画方法、軌道計画システム及び上記軌道計画システムを備えたロボットに関する。
【背景技術】
【０００２】
たとえば、ロボットのアームの制御において、軌道計画に基づいてアームのジョイント部のトルクを制御し、アームを目標の状態とする場合に、アームの位置及び角度のみの軌道計画では、ジョイント部のトルクを制御することはできない。ジョイント部のトルクを制御するには、アームの角度及び角速度を含む状態空間（位相空間）の軌道計画を作成する必要がある。
【０００３】
状態空間の軌道計画を作成する方法として、状態空間における状態遷移を探索木として記述する方法がある。この方法において、根または枝から、所定数の枝を派生させ、それぞれの枝を探索することが考えられる。しかし、派生させる枝の数をＮとし、木の深さをＭとすると、木の深さＭのレベルで探索の対象となる枝の数は、ＮＭとなる。したがって、派生させる全ての枝を探索する場合には計算コストが膨大となる。
【０００４】
そこで、探索木の枝の評価関数を定め、評価関数にしたがって選択した枝のみから枝を派生させる方法が提案されている（非特許文献１）。この方法によれば、計算コストは抑えられる。しかし、評価関数は、対象ごとに設計者が経験によって決める必要がある。評価関数によっては、状態空間上の探索を十分に行えない場合も生じうる。すなわち、この方法では、状態空間の探索に制約が生じうる。
【先行技術文献】
【非特許文献】
【０００５】
【非特許文献１】Pedro S. Huang: ”Planning For Dynamic Motions Using a Search Tree”, a graduate thesis of Toronto Univ., (1996).
【発明の概要】
【発明が解決しようとする課題】
【０００６】
したがって、計算コストを抑えながら、状態空間の探索に制約が生じることのない、軌道計画方法及び軌道計画システムに対するニーズがある。
【課題を解決するための手段】
【０００７】
本発明による軌道計画方法は、軌道計画システムによって、物体の状態を目標状態へ制御するための軌道を求める方法である。本方法は、該軌道計画システムの探索木作成部が、該物体の状態を、状態空間において探索木の根として定めるステップと、該探索木作成部が、所定の時間間隔における力学的な関係から、該時間間隔が経過した後の、該物体の次の状態を、状態空間において探索木の枝として定めるステップと、を含む。本方法は、さらに、該軌道計画システムの既知状態登録木作成部が、予め区分された状態空間の複数のセルから、該次の状態が属するセルを定め、該セルに属する状態が、既知状態登録木の枝としてすでに登録されているかどうか判断するステップと、該セルに属する状態が登録されている場合に、該次の状態を破棄するステップと、該セルに属する状態が登録されていない場合に、該セルに属する該次の状態を該既知状態登録木の枝として既知状態登録木記憶部に登録するステップと、を含む。本方法は、さらに、該軌道計画システムの軌道作成部が、探索が終了した後に、該既知状態登録木記憶部に該既知状態登録木の枝として登録された状態の中から、該目標状態までの距離がもっとも小さい状態を選択し、その状態から、該既知状態登録木の根側に遡って求めた一連の状態によって軌道を求めるステップ、を含む。
【０００８】
本発明の方法によれば、既知状態登録木作成部が、探索木の枝を、セルを使用して既知登録木の枝に集約するので、計算コストを大幅に低減することができる。このように、本発明の方法によれば、計算コストを抑えつつ、状態空間の探索に制約が生じることのない、軌道計画方法が得られる。
【０００９】
本発明の実施形態によれば、状態が属することのできる状態空間の範囲を定めることにより、物体の動作を制限する。
【００１０】
本実施形態によれば、たとえば、障害物が存在する範囲を、状態が属することのできる範囲から除外することにより、障害物を回避して物体を動作させることが可能となる。
【００１１】
本発明の実施形態によれば、前記所定の時間間隔における力学的な関係に制約条件を定めることにより、物体の動作を制限する。
【００１２】
本実施形態によれば、たとえば、与えるトルクの上限値を定めることにより、計画される物体の動作が実現可能なものとなるようにすることができる。
【００１３】
本発明の実施形態によれば、該探索木作成部が、部材の状態空間上の次の状態を、探索木の枝として求める際に、探索木の末端の全ての枝から次の状態に対応する枝を派生させる。
【００１４】
本実施形態によれば、探索木の末端の全ての枝から次の状態に対応する枝を派生させ、このように派生させた探索木の全ての枝を対象として網羅的な探索を行うことができる。この結果、状態空間を一様に探索することができる。
【００１５】
本発明の実施形態によれば、該探索木作成部が、部材の状態空間上の次の状態を、探索木の枝として求める際に、探索木の枝の内、選択した枝から次の状態に対応する枝を派生させる。
【００１６】
本発明の実施形態によれば、評価関数などによって選択的な探索を行う場合にも、計算コストを大幅に低減することができる。
【００１７】
本発明の実施形態によれば、該既知状態登録木作成部が、該次の状態が属するセルが該目標状態を含む場合には、該セルに属する状態が、該既知状態登録木の枝としてすでに登録されている場合にも、該次の状態を破棄せず、該セルに属する該次の状態を該既知状態登録木の枝として既知状態登録木記憶部に登録する。
【００１８】
本実施形態によれば、さらに目標状態に近い最終状態を得ることができる。
【００１９】
本発明の実施形態によれば、該既知状態登録木作成部が、該セルに属する状態が登録されていない場合に、該次の状態を該セルに属する状態として登録した後に、該次の状態と該目標状態との状態空間における距離を求め、該距離が、これまでに該既知状態登録木の枝として登録された状態の該目標状態との距離の最小値よりも小さければ、該次の状態を最終最良状態として記録し、該軌道作成部が、該目標状態までの距離がもっとも小さい状態として、該最終最良状態を選択する。
【００２０】
本実施形態によれば、既知状態登録木記憶部に既知状態登録木の枝として登録された状態の中から、目標状態までの距離がもっとも小さい状態を効率的に選択し、軌道を求めることができる。
【００２１】
本発明の実施形態によれば、該物体が、先端部を除く端部がジョイントによって接続された、１または複数の部材である。
【００２２】
本実施形態によれば、先端部を除く端部がジョイントによって接続された、１または複数の部材である物体の軌道計画を、低い計算コストで、探索に制約を伴わずに求めることができる。
【００２３】
本発明の実施形態によれば、該物体が、ロボットの部材である。
【００２４】
本実施形態によれば、ロボットの部材の軌道計画を、低い計算コストで、探索に制約を伴わずに求めることができる。
【００２５】
本発明の実施形態によれば、状態空間が位相空間である。
【００２６】
本実施形態によれば、位相空間を使用することにより、効率的に軌道計画を求めることができる。
【００２７】
本発明による軌道計画システムは、物体の状態を目標状態とするための軌道を求める。本システムは、該物体の状態を、状態空間において探索木の根として定め、所定の時間間隔における力学的な関係から、該時間間隔が経過した後の、該物体の次の状態を、状態空間において探索木の枝として定める探索木作成部を備える。本システムは、さらに、既知状態登録木を記憶する既知状態登録木記憶部と、予め区分された状態空間の複数のセルから、該次の状態が属するセルを定め、該セルに属する状態が、既知状態登録木の枝としてすでに登録されているかどうか判断し、該セルに属する状態が登録されている場合に、該次の状態を破棄し、該セルに属する状態が登録されていない場合に、該セルに属する該次の状態を該既知状態登録木の枝として該既知状態登録木記憶部に登録する、既知状態登録木作成部と、を備える。本システムは、さらに、探索が終了した後に、該既知状態登録木記憶部に該既知状態登録木の枝として登録された状態の中から、該目標状態までの距離がもっとも小さい状態を選択し、その状態から、該既知状態登録木の根側に遡って求めた一連の状態によって軌道を求める軌道作成部を備える。
【００２８】
本発明のシステムによれば、既知状態登録木作成部が、探索木の枝を、セルを使用して既知登録木の枝に集約するので、計算コストを大幅に低減することができる。このように、本発明によれば、計算コストを抑えることにより、状態空間の探索に制約が生じることのない、軌道計画システムが得られる。
【００２９】
本発明の実施形態によれば、状態が属することのできる状態空間の範囲を定めることにより、物体の動作を制限する。
【００３０】
本実施形態によれば、たとえば、障害物が存在する範囲を、状態が属することのできる範囲から除外することにより、障害物を回避して物体を動作させることが可能となる。
【００３１】
本発明の実施形態によれば、前記所定の時間間隔における力学的な関係に制約条件を定めることにより、物体の動作を制限する。
【００３２】
本実施形態によれば、たとえば、与えるトルクの上限値を定めることにより、計画される物体の動作が実現可能なものとなるようにすることができる。
【００３３】
本発明による軌道計画・制御システムは、上記の軌道系画システムと、該軌道計画システムが求めた軌道をフィードバック制御によって実現する軌道制御部とを備えた、軌道計画・制御システムである。
【００３４】
本発明による軌道計画・制御システムにおいては、軌道計画システムによって計画された軌道を、フィードバック制御によってリアルタイムで実現することができる。
【００３５】
本発明によるロボットは、本発明による軌道計画・制御システムを備えたロボットであって、該物体が、該ロボットの部材であり、該軌道にしたがって、該ロボットの部材を制御する。
【００３６】
本発明によるロボットにおいては、ロボットの部材の軌道計画を、低い計算コストで、探索に制約を伴わずに求めることができる。また、計画された軌道を、フィードバック制御によってリアルタイムで実現することができる。
【図面の簡単な説明】
【００３７】
【図１】本発明の一実施形態による軌道計画システム１００の構成を示す図である。
【図２】二自由度倒立振子の構成を示す図である。
【図３】本実施形態による軌道計画法を説明するための流れ図である。
【図４】図３のステップＳ０２０及びステップＳ０３０の動作を説明するための図である。
【図５】分割された位相空間を示す図である。
【図６】図３のステップＳ０５０及びステップＳ０６０の動作を説明するための図である。
【図７】二自由度倒立振子の初期状態を示す図である。
【図８】二自由度倒立振子の目標状態を示す図である。
【図９】実験１によって得られた第１関節位相空間の軌道を示す図である。
【図１０】実験１によって得られた第２関節位相空間の軌道を示す図である。
【図１１】実験１において、探索木の枝として求められた全ての第１関節の状態の位置を第１関節位相空間上に示した図である。
【図１２】実験１において、探索木の枝として求められた全ての第２関節の状態の位置を第２関節位相空間上に示した図である。
【図１３】実験２によって得られた第１関節位相空間の軌道を示す図である。
【図１４】実験２によって得られた第２関節位相空間の軌道を示す図である。
【図１５】実験２において、探索木の枝として求められた全ての第１関節の状態の位置を第１関節位相空間上に示した図である。
【図１６】実験２において、探索木の枝として求められた全ての第２関節の状態の位置を第２関節位相空間上に示した図である。
【図１７】実験３によって得られた第１関節位相空間の軌道、及び実験３において、探索木の枝として求められた全ての第１関節の状態の位置を示した図である。
【図１８】実験３によって得られた第２関節位相空間の軌道、及び実験３において、探索木の枝として求められた全ての第２関節の状態の位置を示した図である。
【図１９】図１に示した軌道計画・制御システムの微細軌道データ作成部及び軌道制御部の構成を示す図である。
【図２０】軌道制御の動作を説明するための流れ図である。
【図２１】軌道データ作成部によって作成された５ミリ秒ごとの軌道データ（細い線）と微細軌道データ作成部によって作成された５ミリ秒ごとの軌道データ（太い線）とを示す図である。
【図２２】微細軌道データ作成部によって作成された５ミリ秒ごとの軌道データ（濃い線）と軌道生成の際にトルクに５％のノイズを加えた場合の軌道データ（薄い線）とを示す図である。
【図２３】図２２に示したノイズを加えた場合に、ＰＩＤコントローラによる制御を適用した場合の軌道データを示す図である。
【図２４】複数の関節によって接続された複数の部材から構成されるロボットを示す図である。
【発明を実施するための形態】
【００３８】
図１は、本発明の一実施形態による軌道計画システム１００の構成を示す図である。軌道計画システム１００は、ロボットのアームなどの物体の状態を目標状態とするための、状態空間における軌道を求めるシステムである。軌道計画システム１００は、状態空間上の物体の位置に対応する枝を備えた探索木を作成する探索木作成部１０１と、探索木の枝の内、所定のものを枝とする既知状態登録木を作成する既知状態登録木作成部１０３と、既知状態登録木を記憶する既知状態登録木記憶部１０５と、既知状態登録木から、物体の状態空間における軌道を作成する軌道作成部１０７、微細軌道データを作成する微細軌道データ作成部１１０及び微細軌道データに基づいて軌道を制御する軌道制御部１２０を備える。各構成部の機能の詳細については、後で説明する。
【００３９】
図２は、二自由度倒立振子２００の構成を示す図である。以下の実施形態においては、２次元平面内で運動する二自由度倒立振子２００を物体として説明を行なう。二自由度倒立振子２００は、第１リンク２０５と、第２リンク２１１と、第１リンク２０５を回転可能に固定点に接続する第１関節（ジョイント）２０１と、第１リンク２０５及び第２リンク２１１を回転可能に接続する第２関節２０７と、から構成される。
【００４０】
二自由度倒立振子２００の運動は、第１関節２０１及び第２関節２０７に与えるトルクによって定まり、第１リンク２０５のＸ軸に対する角度
【数１】
第２リンク２１１の第１リンク２０５に対する角度
【数２】
及びこれらの角速度
【数３】
によって表現される。そこで、本実施形態において、状態空間として以下の位相空間を採
用する。
【数４】
【００４１】
図３は、本実施形態による軌道計画方法を説明するための流れ図である。
【００４２】
図３のステップＳ０１０において、探索木作成部１０１が、二自由度倒立振子２００の現在の状態及び目標状態を得る。ここで、状態とは、位相空間上の位置を意味する。
【００４３】
図３のステップＳ０２０において、探索木作成部１０１が、位相空間において、現在の状態の位置を探索木の根として定める。
【００４４】
図３のステップＳ０３０において、探索木作成部１０１が、位相空間において、所定の時間間隔後の次の状態の位置を、探索木の枝として定める。所定の時間間隔後の次の状態の位置は、運動方程式や動力学シミュレータによって定める。上述のように、二自由度倒立振子２００の運動は、第１関節２０１及び第２関節２０７に与えるトルクによって定まるので、所定の時間間隔に第１関節２０１及び第２関節２０７に与えるトルクを定めれば、二自由度倒立振子２００の、所定の時間間隔後の次の状態が定まる。そこで、所定の時間間隔に第１関節２０１及び第２関節２０７に与えるトルクを最大値と最小値との間で、所定の増分で変化させながら、それぞれの場合に対応する次の状態を求めてもよい。この場合に、トルクを最大値と最小値との間で、所定の増分で変化させながら、幅優先探索により網羅的に探索を行なってもよい。
【００４５】
図４は、図３のステップＳ０２０及びステップＳ０３０の動作を説明するための図である。なお、図４は、簡単のために位相空間を２次元空間として表している。現在の状態にある、二自由度倒立振子２００に所定の時間間隔に３個の異なるトルク
【数５】
を与えることにより、３個の異なる次の状態が定まる。探索木作成部１０１は、これらの
状態を探索木の枝とする。さらに、生成された枝が示すそれぞれの状態に対して、３個の
異なるトルク
【数６】
を与えることにより、３個の異なる、さらに次の状態が定まる。
【００４６】
図３のステップＳ０４０において、既知状態登録木作成部１０３は、探索木の枝の内、所定のものを枝とする既知状態登録木を作成する。既知状態登録木を作成するため、位相空間を予め、分割しておく。
【００４７】
図５は、分割された位相空間を示す図である。分割は、想定される角度及び角速度の範囲を等分割してもよい。以下において、分割された位相空間をセルと呼称する。
【００４８】
探索木作成部１０１が、探索木を作成する場合に、上述のように、根または枝から派生させる枝の数をＮとし、木の深さをＭとすると、木の深さＭのレベルで探索の対象となる枝の数は、ＮＭとなる。そこで、既知状態登録木作成部１０３は、上述のセルを利用して、探索木作成部１０１が派生させた枝を集約する。具体的に、既知状態登録木作成部１０３は、図３のステップＳ０４０において、以下の処理を行う。
【００４９】
既知状態登録木作成部１０３は、探索木作成部１０１が派生させた枝が表す次の状態が位相空間のどのセルに属するか定める。つぎに、既知状態登録木作成部１０３は、そのセルに属する状態が、既知状態登録木の枝としてすでに登録されているかどうか判断する。登録されていなければ、ステップＳ０４５に進む。登録されていれば、ステップＳ０６０に進む。
【００５０】
図３のステップＳ０４５において、既知状態登録木作成部１０３は、探索木作成部１０１が派生させた枝が表す次の状態が身体制約及び環境制約を満たすかどうか判断する。身体制約とは、身体の部材に相当する第１及び第２のリンクが、たとえばお互いに衝突しないために設けられる角度または角速度の制約である。また、環境制約とは、身体の部材に相当する第１及び第２のリンクが、たとえば外部の障害物に衝突しないように設けられる角度または角速度の制約である。次の状態が身体制約及び環境制約を満たせば、ステップＳ０５０に進む。身体制約及び環境制約のいずれかを満たさなければ、ステップＳ０６０に進む。
【００５１】
図３のステップＳ０５０において、既知状態登録木作成部１０３は、探索木作成部１０１が派生させた枝が表す次の状態を既知登録木の枝として登録する。
【００５２】
既知状態登録木作成部１０３は、既知登録木の枝を登録する際に、その枝が表す状態と目標状態との距離を求め、該距離を、すでに存在する既知登録木の枝が表す状態と目標状態との距離の内、最短の距離と比較し、該距離の方が短ければ、該距離に対応する状態を最終最良状態として、該距離とともに既知状態登録木記憶部１０５に記憶させてもよい。
【００５３】
図３のステップＳ０６０において、既知状態登録木作成部１０３は、探索木作成部１０１が派生させた枝が表す次の状態を破棄する。
【００５４】
図６は、図３のステップＳ０５０及びステップＳ０６０の動作を説明するための図である。既知登録木の枝は、原則として１個のセル内には１本しか存在しない。探索木作成部１０１が、既知登録木の枝（図６の実線で示した円）がすでに存在するセルに新たな枝（図６の点線で示した円）を定めた場合には、その枝は破棄される。すなわち、位相空間上の位置が近い状態は集約される。このように、従来の探索木の枝を、セルを使用して既知登録木の枝に集約することによって、計算コストを大幅に低減することができる。
【００５５】
なお、既知状態登録木作成部１０３は、探索木作成部１０１が派生させた枝が、目標状態の属するセルに属する場合には、そのセルに属する状態に対応する既知状態登録木の枝がすでに登録されていても、例外的に新しい枝をさらに登録するようにしてもよい。このようにすることにより、さらに目標状態に近い最終状態を得ることが可能となる。
【００５６】
図３のステップＳ０７０において、探索木作成部１０１は、探索が終了したかどうか判断する。探索木作成部１０１は、探索木の所定の深さまでの探索を終えた場合、あるいは所定時間の推移まで探索を終えた場合に探索が終了したと判断してもよい。探索が終了したと判断された場合には、ステップＳ０８０に進む。探索が終了していないと判断された場合には、ステップＳ０３０に戻り、探索が継続される。
【００５７】
図３のステップＳ０８０において、軌道作成部１０７は、既知状態登録木の枝が表す状態の中から、目標状態との位相空間上の距離が最も近い状態を選択し、この状態を最良状態とする（最終状態最適化の場合）。既知状態登録木作成部１０３が最終最良状態を求めている場合には、最終最良状態を最良状態とする。あるいは、軌道作成部１０７は、既知状態登録木の枝が表す状態の中から、目標状態との位相空間上の距離が所定値以下であり、探索木の深さが最も浅い枝に対応する状態を選択し、この状態を最良状態とする（到達時間最適化の場合）。軌道作成部１０７は、最良状態から現在の状態（初期状態）まで、既知登録木の枝の経路を求めることにより現在の状態から最良状態に至る軌道を求める。このようにして軌道計画が求まる。たとえば、ロボットは、このようにして求めた軌道計画にしたがってアームを制御することにより、アームの最終状態を最適化し、あるいは、目標状態への到達時間を最適化することができる。
【００５８】
本発明による軌道計画方法について、本明細書では、説明を簡単にするために、物体を二自由度倒立振子としている。しかし、本発明の、既知状態登録木を使用する考え方は、任意の物体の状態空間における軌道計画方法に適用することができる。
【００５９】
つぎに、図２に示した二自由度倒立振子２００のシミュレーションによる制御実験について説明する。二自由度倒立振子２００の第１リンク２０５のリンク長は、ｌ１であり、質量は、ｍ１であり、重心位置２０３はリンクの中央である。二自由度倒立振子２００の第２リンク２１１のリンク長は、ｌ２であり、質量は、ｍ２であり、重心位置２０９はリンクの中央である。また、重力加速度をｇとする。二自由度倒立振子２００の運動方程式は以下のとおりである。
【数７】
関節角度と重力の関係に対する理解を容易にするために、以下のように座標を変更した。
【数８】
【００６０】
図７は、二自由度倒立振子２００の初期状態を示す図である。二自由度倒立振子２００の初期状態の座標は以下のとおりである。
【数９】
【００６１】
図８は、二自由度倒立振子２００の目標状態を示す図である。二自由度倒立振子２００の目標状態の座標は以下のとおりである。
【数１０】
【００６２】
実験は、図３に示した軌道計画方法の手順及び二自由度倒立振子２００の運動をシミュレーションすることによって実行した。
【００６３】
実験１
表１は、実験１のシミュレーションの設定を示す。上述の次の状態までの時間間隔が、表１のトルク切替タイムステップに相当する。
【表１】
【００６４】
表２は、第１関節及び第２関節のパラメータを示す。表２において、第１関節の最大トルク2 [Nm]、探索木作成時のトルク粒度１０分割とは、探索木の新たな枝を派生させる際に、-2 [Nm]から2 [Nm]までのトルクの範囲を１０分割し、たとえば、 -1.8 [Nm], -1.4[Nm], -1.0[Nm]... 1.0[Nm], 1.4[Nm], 1.8 [Nm]というようにトルクを与えて、運動方程式から次の状態（角度及び角速度）を求めること
を意味する。実験１においては、第１関節及び第２関節に、それぞれ１０個の異なるトルクを与えるので、１００個の異なる次の状態が派生する。
【表２】
【００６５】
図９は、実験１によって得られた第１関節位相空間の軌道を示す図である。第１関節は、１．３秒後に目標状態に到達している。図９において、初期状態の角度ｑの座標は、πであり、−πに等しい。
【００６６】
図１０は、実験１によって得られた第２関節位相空間の軌道を示す図である。第２関節は、１．３秒後に目標状態に到達している。
【００６７】
図１１は、実験１において、探索木の枝として求められた全ての第１関節の状態の位置を第１関節位相空間上に示した図である。
【００６８】
図１２は、実験１において、探索木の枝として求められた全ての第２関節の状態の位置を第２関節位相空間上に示した図である。
【００６９】
図１１及び図１２から、位相空間上のほぼ全体が探索されていることがわかる。このことは、位相空間上の任意の状態を目標状態として軌道計画が得られることを意味する。
【００７０】
ここで、トルク粒度及び位相空間の状態分割数は、図１１及び図１２において、状態の位置が第１関節位相空間及び第２関節位相空間の十分広い範囲に一様に分布するように調整しながら定める。
【００７１】
実験２
表３は、実験２のシミュレーションの設定を示す。上述の次の状態までの時間間隔が、表３のトルク切替タイムステップに相当する。
【表３】
【００７２】
表４は、第１関節及び第２関節のパラメータを示す。実験２においては、実験１の場合よりも最大トルクを小さくしている。第２関節には、トルクを与えない。表４において、最大トルク1.5 [Nm]、探索木作成時のトルク粒度１０分割とは、探索木の新たな枝を派生させる際に、-1.5 [Nm]から1.5[Nm]までのトルクの範囲を１０分割し、たとえば、 -1.35 [Nm],-1.05[Nm], -0.75[Nm]... 0.75[Nm], 1.05[Nm], 1.35 [Nm]というようにトルクを与えて、運動方程式から次の状態（角度及び角速度）を求めることを意味する。実験２においては、第１関節に１０個の異なるトルクを与え、第２関節にト
ルクを与えないので、１０個の異なる次の状態が派生する。
【表４】
【００７３】
図１３は、実験２によって得られた第１関節位相空間の軌道を示す図である。第１関節は、４．９秒後に目標状態に到達している。図１３において、初期状態の角度ｑの座標は、πであり、−πに等しい。
【００７４】
図１４は、実験２によって得られた第２関節位相空間の軌道を示す図である。第２関節は、４．９秒後に目標状態に到達している。
【００７５】
図１５は、実験２において、探索木の枝として求められた全ての第１関節の状態の位置を第１関節位相空間上に示した図である。
【００７６】
図１６は、実験２において、探索木の枝として求められた全ての第２関節の状態の位置を第２関節位相空間上に示した図である。
【００７７】
図１５を図１１と比較し、図１６を図１２と比較すると、探索されていない領域が増加している。これは、最大トルクを小さく設定した結果、最大トルクで到達可能な領域が縮小したためである。最大トルクで到達可能な領域に対しては一様な探索が行なわれている。
【００７８】
実験３
実験３は、第１位相空間上の、角度が−９０°（−π／２）乃至０°の領域の状態となることがないように軌道を定める点を除いて、実験２と同じ条件で行った。位相空間上の領域を制限するには、図３のステップＳ０４５に示した手順による。
【００７９】
図１７は、実験３によって得られた第１関節位相空間の軌道、及び実験３において、探索木の枝として求められた全ての第１関節の状態の位置を示した図である。図１７によれば、第１位相空間上の、角度が−９０°（−π／２）乃至０°の領域（図１７のＡで示した領域）の状態は選択されていない。
【００８０】
図１８は、実験３によって得られた第２関節位相空間の軌道、及び実験３において、探索木の枝として求められた全ての第２関節の状態の位置を示した図である。
【００８１】
図１７と図１３及び図１８と図１４をそれぞれ比較すると、軌道は異なっているが、図１７及び図１８に示すように最終的に目標に到達している。
【００８２】
計算コストの評価
表５は、実験１、実験２及び実験１と同じ条件で、既知状態登録木を使用しなかった場合の探索状態数及び計算時間を示す表である。既知状態登録木を使用しなかった場合には、上述のように、根または枝から派生させる枝の数をＮとし、最深部の木の深さをＭとすると、最深部において探索の対象となる枝（状態）の数は、ＮＭとなる。実験１の条件によると、
N=10x10=100
M=5[s]/0.1[s]=50
となる。最深部以外の深さの枝の数を加えると、探索状態数は、以下のおとりである。
【数１１】
計算時間は、Intel Core2 Duo T7500 搭載のうち1コアのみを使用して測定した。
【００８３】
表５によると、既知状態登録木を使用しなかった場合の探索状態数は膨大となり、網羅的な全探索を行うことはできない。本実施形態の実験１によれば、従来技術では実現できなかった、網羅的な全探索を行うことが可能となる。このように本発明によれば、位相空間上の探索が制約を受けることはない。
【００８４】
また、本発明の既知状態登録木は、網羅的な全探索を行う場合だけではなく、評価関数などを使用して選択的に探索木を探索する場合にも適用することができる。
【表５】
【００８５】
軌道制御
つぎに、作成された軌道の制御について説明する。
【００８６】
図１９は、図１に示した軌道計画・制御システムの微細軌道データ作成部１１０及び軌道制御部１２０の構成を示す図である。
【００８７】
微細軌道データ作成部１１０は、軌道作成部１０７の作成した軌道データ（１００ミリ秒ごとの角度、角速度、トルク）から、微細軌道データ（５ミリ秒ごとの角度
【数１２】
、角速度
【数１３】
、トルク
【数１４】
）を作成する。
【００８８】
軌道制御部１２０は、ＰＩＤコントローラ１２１、サーボ・モータ１２３及び角度センサ１２５を含む。軌道制御部１２０のＰＩＤコントローラ１２１は、微細軌道データの角度を実現するように５ミリ秒ごとにサーボ・モータを操作する。このようにして、リアルタイムの軌道制御を実現することができる。
【００８９】
図２０は、軌道制御の動作を説明するための流れ図である。
【００９０】
図２０のステップＳ１１０において、微細軌道データ作成部１１０は、軌道作成部１０７から軌道データを受け取る。受け取る軌道データは、１００ミリ秒ごとの角度、角速度及びトルクである。
【００９１】
図２０のステップＳ１２０において、微細軌道データ作成部１１０は、１００ミリ秒ごとの角度、角速度及びトルクである軌道データから、運動方程式を用いて、５ミリ秒ごとの角度
【数１５】
、角速度
【数１６】
及びトルク
【数１７】
である微細軌道データを作成する。
【００９２】
図２０のステップＳ１３０において、軌道制御部１２０は、微細軌道データに基づいてフィードバック制御を行う。具体的に、軌道制御部１２０のＰＩＤコントローラ１２１は、５ミリ秒ごとに、微細軌道データから求めたｎ時間間隔後の角度
【数１８】
を目標値とし、現在の角度測定値
【数１９】
を出力値として、該目標値と該出力値との偏差からＰＩＤ制御によってトルク目標値
【数２０】
を操作量として定める。ｎは整数であり、ｎ時間間隔とは５ｎミリ秒である。フィードバックゲインが十分である場合にはｎ＝１が最適である。フィードバックゲインが不十分である場合には、ｎは、１より大きな整数から選択する。ここで、ノイズがない場合には、ＰＩＤコントローラ１２１がサーボ・モータ１２３へ出力するトルク目標値
【数２１】
は、微細軌道データに含まれる予測トルク値
【数２２】
と一致する。予測トルク値
【数２３】
は、実験２において説明したようにトルク限界を考慮して計算されているため、ＰＩＤコントローラ１２１によるトルク目標値もトルク限界を踏まえた値の周囲に収まる。
【００９３】
図２１は、軌道データ作成部１０７によって作成された１００ミリ秒ごとの軌道データ（細い線）と微細軌道データ作成部１１０によって作成された５ミリ秒ごとの軌道データ（太い線）とを示す図である。１００ミリ秒ごとの軌道データは、図１４に示した軌道データと同じである。
【００９４】
図２２は、微細軌道データ作成部１１０によって作成された５ミリ秒ごとの軌道データ（濃い線）と軌道生成の際にトルクに５％のノイズを加えた場合の軌道データ（薄い線）とを示す図である。図２２から明らかなように、ノイズによって軌道は変化する。
【００９５】
図２３は、図２２に示したノイズを加えた場合に、ＰＩＤコントローラ１２１による制御を適用した場合の軌道データを示す図である。図２３に示す軌道データは、図２２に示すノイズがない場合の軌道データと同じである。すなわち、フィードバック制御によってノイズはキャンセルされ、微細軌道データ作成部１１０によって作成された軌道が実現される。図２３の軌道における色の濃さは、ＰＩＤコントローラ１２１の操作量の大きさを示している。
【００９６】
ロボットへの応用
ここでロボットとは、少なくとも１個以上の関節（ジョイント）で接続された部材を有し、部材の位置を制御することのできる機械装置を指す。
【００９７】
図２４は、肘軸、肩軸、腰軸、足首軸など複数の関節によって接続された複数の部材から構成されるロボットを示す図である。ロボットの付近には障害物が存在し、ロボットの腕に相当する部材は、障害物を避けながら目標に到達するように制御される。
【００９８】
図２４に示すようなロボットにおいて、手先目標または全身姿勢目標へ向けて上半身に相当する部材と下半身に相当する部材を協調して動作させることを考える。この場合には、位相空間上における現在状態から次状態への遷移を、ロボットの全身（全部材）の運動方程式を用いて予測すればよい。
【００９９】
図２４に示すようなロボットにおいて、多関節を同時に制御することを考える。この場合には、全ての関節の関節角度及び関節角速度を、関節数の２倍の次元を有する高次元位相空間上に写像して機械状態を記述すればよい。
【０１００】
図２４に示すようなロボットにおいて、身体制約（ロボット自体の制約）を満たす運動を計画する。身体制約として、トルク制約を設ける場合には、実験２で説明したように、次状態の推定に使用するトルクに限界を設定する。身体制約として、位置または速度限界を設ける場合には、図３のステップＳ０４５に関して説明したように、制約条件を満たさない次の状態を破棄する。この場合の結果は、実験３に関連して説明した。
【０１０１】
図２４に示すようなロボットにおいて、障害物を回避する運動を計画する。図３のステップＳ０４５に関して説明したように、障害物と接触する場合に次の状態を破棄する。
【０１０２】
図２４に示すようなロボットを、作成した軌道にしたがってリアルタイムで動作させることを考える。この場合には、微細軌道データ作成部１１０によって微細軌道データを作成し、微細軌道データに基づいて図１９に示したＰＩＤコントローラ１２１によるＰＩＤ制御などのフィードバック制御によってサーボ・モータ１２３を操作し、作成した微細軌道に沿って部材が動作するようにする。
【０１０３】
上記のように、本発明による軌道計画方法及び軌道計画システムは、複数の関節で接続された複数の部材から構成されるロボットにおいて、手先目標または全身姿勢目標へ向けて上半身に相当する部材と下半身に相当する部材を協調して動作させる場合、多関節を同時に制御する場合、身体制約を満たす運動を計画する場合、障害物を回避する運動を計画する場合、作成した軌道にしたがってリアルタイムでロボットを動作させる場合に有効に使用することができる。
【符号の説明】
【０１０４】
１０１…探索木作成部、１０３…既知状態登録木作成部、１０５…既知状態登録木記憶部、１０７…軌道作成部、１１０…微細軌道データ作成部、１２０…軌道制御部
【技術分野】
【０００１】
本発明は、物体の、目標状態に至る状態遷移系列を作成する、軌道計画方法、軌道計画システム及び上記軌道計画システムを備えたロボットに関する。
【背景技術】
【０００２】
たとえば、ロボットのアームの制御において、軌道計画に基づいてアームのジョイント部のトルクを制御し、アームを目標の状態とする場合に、アームの位置及び角度のみの軌道計画では、ジョイント部のトルクを制御することはできない。ジョイント部のトルクを制御するには、アームの角度及び角速度を含む状態空間（位相空間）の軌道計画を作成する必要がある。
【０００３】
状態空間の軌道計画を作成する方法として、状態空間における状態遷移を探索木として記述する方法がある。この方法において、根または枝から、所定数の枝を派生させ、それぞれの枝を探索することが考えられる。しかし、派生させる枝の数をＮとし、木の深さをＭとすると、木の深さＭのレベルで探索の対象となる枝の数は、ＮＭとなる。したがって、派生させる全ての枝を探索する場合には計算コストが膨大となる。
【０００４】
そこで、探索木の枝の評価関数を定め、評価関数にしたがって選択した枝のみから枝を派生させる方法が提案されている（非特許文献１）。この方法によれば、計算コストは抑えられる。しかし、評価関数は、対象ごとに設計者が経験によって決める必要がある。評価関数によっては、状態空間上の探索を十分に行えない場合も生じうる。すなわち、この方法では、状態空間の探索に制約が生じうる。
【先行技術文献】
【非特許文献】
【０００５】
【非特許文献１】Pedro S. Huang: ”Planning For Dynamic Motions Using a Search Tree”, a graduate thesis of Toronto Univ., (1996).
【発明の概要】
【発明が解決しようとする課題】
【０００６】
したがって、計算コストを抑えながら、状態空間の探索に制約が生じることのない、軌道計画方法及び軌道計画システムに対するニーズがある。
【課題を解決するための手段】
【０００７】
本発明による軌道計画方法は、軌道計画システムによって、物体の状態を目標状態へ制御するための軌道を求める方法である。本方法は、該軌道計画システムの探索木作成部が、該物体の状態を、状態空間において探索木の根として定めるステップと、該探索木作成部が、所定の時間間隔における力学的な関係から、該時間間隔が経過した後の、該物体の次の状態を、状態空間において探索木の枝として定めるステップと、を含む。本方法は、さらに、該軌道計画システムの既知状態登録木作成部が、予め区分された状態空間の複数のセルから、該次の状態が属するセルを定め、該セルに属する状態が、既知状態登録木の枝としてすでに登録されているかどうか判断するステップと、該セルに属する状態が登録されている場合に、該次の状態を破棄するステップと、該セルに属する状態が登録されていない場合に、該セルに属する該次の状態を該既知状態登録木の枝として既知状態登録木記憶部に登録するステップと、を含む。本方法は、さらに、該軌道計画システムの軌道作成部が、探索が終了した後に、該既知状態登録木記憶部に該既知状態登録木の枝として登録された状態の中から、該目標状態までの距離がもっとも小さい状態を選択し、その状態から、該既知状態登録木の根側に遡って求めた一連の状態によって軌道を求めるステップ、を含む。
【０００８】
本発明の方法によれば、既知状態登録木作成部が、探索木の枝を、セルを使用して既知登録木の枝に集約するので、計算コストを大幅に低減することができる。このように、本発明の方法によれば、計算コストを抑えつつ、状態空間の探索に制約が生じることのない、軌道計画方法が得られる。
【０００９】
本発明の実施形態によれば、状態が属することのできる状態空間の範囲を定めることにより、物体の動作を制限する。
【００１０】
本実施形態によれば、たとえば、障害物が存在する範囲を、状態が属することのできる範囲から除外することにより、障害物を回避して物体を動作させることが可能となる。
【００１１】
本発明の実施形態によれば、前記所定の時間間隔における力学的な関係に制約条件を定めることにより、物体の動作を制限する。
【００１２】
本実施形態によれば、たとえば、与えるトルクの上限値を定めることにより、計画される物体の動作が実現可能なものとなるようにすることができる。
【００１３】
本発明の実施形態によれば、該探索木作成部が、部材の状態空間上の次の状態を、探索木の枝として求める際に、探索木の末端の全ての枝から次の状態に対応する枝を派生させる。
【００１４】
本実施形態によれば、探索木の末端の全ての枝から次の状態に対応する枝を派生させ、このように派生させた探索木の全ての枝を対象として網羅的な探索を行うことができる。この結果、状態空間を一様に探索することができる。
【００１５】
本発明の実施形態によれば、該探索木作成部が、部材の状態空間上の次の状態を、探索木の枝として求める際に、探索木の枝の内、選択した枝から次の状態に対応する枝を派生させる。
【００１６】
本発明の実施形態によれば、評価関数などによって選択的な探索を行う場合にも、計算コストを大幅に低減することができる。
【００１７】
本発明の実施形態によれば、該既知状態登録木作成部が、該次の状態が属するセルが該目標状態を含む場合には、該セルに属する状態が、該既知状態登録木の枝としてすでに登録されている場合にも、該次の状態を破棄せず、該セルに属する該次の状態を該既知状態登録木の枝として既知状態登録木記憶部に登録する。
【００１８】
本実施形態によれば、さらに目標状態に近い最終状態を得ることができる。
【００１９】
本発明の実施形態によれば、該既知状態登録木作成部が、該セルに属する状態が登録されていない場合に、該次の状態を該セルに属する状態として登録した後に、該次の状態と該目標状態との状態空間における距離を求め、該距離が、これまでに該既知状態登録木の枝として登録された状態の該目標状態との距離の最小値よりも小さければ、該次の状態を最終最良状態として記録し、該軌道作成部が、該目標状態までの距離がもっとも小さい状態として、該最終最良状態を選択する。
【００２０】
本実施形態によれば、既知状態登録木記憶部に既知状態登録木の枝として登録された状態の中から、目標状態までの距離がもっとも小さい状態を効率的に選択し、軌道を求めることができる。
【００２１】
本発明の実施形態によれば、該物体が、先端部を除く端部がジョイントによって接続された、１または複数の部材である。
【００２２】
本実施形態によれば、先端部を除く端部がジョイントによって接続された、１または複数の部材である物体の軌道計画を、低い計算コストで、探索に制約を伴わずに求めることができる。
【００２３】
本発明の実施形態によれば、該物体が、ロボットの部材である。
【００２４】
本実施形態によれば、ロボットの部材の軌道計画を、低い計算コストで、探索に制約を伴わずに求めることができる。
【００２５】
本発明の実施形態によれば、状態空間が位相空間である。
【００２６】
本実施形態によれば、位相空間を使用することにより、効率的に軌道計画を求めることができる。
【００２７】
本発明による軌道計画システムは、物体の状態を目標状態とするための軌道を求める。本システムは、該物体の状態を、状態空間において探索木の根として定め、所定の時間間隔における力学的な関係から、該時間間隔が経過した後の、該物体の次の状態を、状態空間において探索木の枝として定める探索木作成部を備える。本システムは、さらに、既知状態登録木を記憶する既知状態登録木記憶部と、予め区分された状態空間の複数のセルから、該次の状態が属するセルを定め、該セルに属する状態が、既知状態登録木の枝としてすでに登録されているかどうか判断し、該セルに属する状態が登録されている場合に、該次の状態を破棄し、該セルに属する状態が登録されていない場合に、該セルに属する該次の状態を該既知状態登録木の枝として該既知状態登録木記憶部に登録する、既知状態登録木作成部と、を備える。本システムは、さらに、探索が終了した後に、該既知状態登録木記憶部に該既知状態登録木の枝として登録された状態の中から、該目標状態までの距離がもっとも小さい状態を選択し、その状態から、該既知状態登録木の根側に遡って求めた一連の状態によって軌道を求める軌道作成部を備える。
【００２８】
本発明のシステムによれば、既知状態登録木作成部が、探索木の枝を、セルを使用して既知登録木の枝に集約するので、計算コストを大幅に低減することができる。このように、本発明によれば、計算コストを抑えることにより、状態空間の探索に制約が生じることのない、軌道計画システムが得られる。
【００２９】
本発明の実施形態によれば、状態が属することのできる状態空間の範囲を定めることにより、物体の動作を制限する。
【００３０】
本実施形態によれば、たとえば、障害物が存在する範囲を、状態が属することのできる範囲から除外することにより、障害物を回避して物体を動作させることが可能となる。
【００３１】
本発明の実施形態によれば、前記所定の時間間隔における力学的な関係に制約条件を定めることにより、物体の動作を制限する。
【００３２】
本実施形態によれば、たとえば、与えるトルクの上限値を定めることにより、計画される物体の動作が実現可能なものとなるようにすることができる。
【００３３】
本発明による軌道計画・制御システムは、上記の軌道系画システムと、該軌道計画システムが求めた軌道をフィードバック制御によって実現する軌道制御部とを備えた、軌道計画・制御システムである。
【００３４】
本発明による軌道計画・制御システムにおいては、軌道計画システムによって計画された軌道を、フィードバック制御によってリアルタイムで実現することができる。
【００３５】
本発明によるロボットは、本発明による軌道計画・制御システムを備えたロボットであって、該物体が、該ロボットの部材であり、該軌道にしたがって、該ロボットの部材を制御する。
【００３６】
本発明によるロボットにおいては、ロボットの部材の軌道計画を、低い計算コストで、探索に制約を伴わずに求めることができる。また、計画された軌道を、フィードバック制御によってリアルタイムで実現することができる。
【図面の簡単な説明】
【００３７】
【図１】本発明の一実施形態による軌道計画システム１００の構成を示す図である。
【図２】二自由度倒立振子の構成を示す図である。
【図３】本実施形態による軌道計画法を説明するための流れ図である。
【図４】図３のステップＳ０２０及びステップＳ０３０の動作を説明するための図である。
【図５】分割された位相空間を示す図である。
【図６】図３のステップＳ０５０及びステップＳ０６０の動作を説明するための図である。
【図７】二自由度倒立振子の初期状態を示す図である。
【図８】二自由度倒立振子の目標状態を示す図である。
【図９】実験１によって得られた第１関節位相空間の軌道を示す図である。
【図１０】実験１によって得られた第２関節位相空間の軌道を示す図である。
【図１１】実験１において、探索木の枝として求められた全ての第１関節の状態の位置を第１関節位相空間上に示した図である。
【図１２】実験１において、探索木の枝として求められた全ての第２関節の状態の位置を第２関節位相空間上に示した図である。
【図１３】実験２によって得られた第１関節位相空間の軌道を示す図である。
【図１４】実験２によって得られた第２関節位相空間の軌道を示す図である。
【図１５】実験２において、探索木の枝として求められた全ての第１関節の状態の位置を第１関節位相空間上に示した図である。
【図１６】実験２において、探索木の枝として求められた全ての第２関節の状態の位置を第２関節位相空間上に示した図である。
【図１７】実験３によって得られた第１関節位相空間の軌道、及び実験３において、探索木の枝として求められた全ての第１関節の状態の位置を示した図である。
【図１８】実験３によって得られた第２関節位相空間の軌道、及び実験３において、探索木の枝として求められた全ての第２関節の状態の位置を示した図である。
【図１９】図１に示した軌道計画・制御システムの微細軌道データ作成部及び軌道制御部の構成を示す図である。
【図２０】軌道制御の動作を説明するための流れ図である。
【図２１】軌道データ作成部によって作成された５ミリ秒ごとの軌道データ（細い線）と微細軌道データ作成部によって作成された５ミリ秒ごとの軌道データ（太い線）とを示す図である。
【図２２】微細軌道データ作成部によって作成された５ミリ秒ごとの軌道データ（濃い線）と軌道生成の際にトルクに５％のノイズを加えた場合の軌道データ（薄い線）とを示す図である。
【図２３】図２２に示したノイズを加えた場合に、ＰＩＤコントローラによる制御を適用した場合の軌道データを示す図である。
【図２４】複数の関節によって接続された複数の部材から構成されるロボットを示す図である。
【発明を実施するための形態】
【００３８】
図１は、本発明の一実施形態による軌道計画システム１００の構成を示す図である。軌道計画システム１００は、ロボットのアームなどの物体の状態を目標状態とするための、状態空間における軌道を求めるシステムである。軌道計画システム１００は、状態空間上の物体の位置に対応する枝を備えた探索木を作成する探索木作成部１０１と、探索木の枝の内、所定のものを枝とする既知状態登録木を作成する既知状態登録木作成部１０３と、既知状態登録木を記憶する既知状態登録木記憶部１０５と、既知状態登録木から、物体の状態空間における軌道を作成する軌道作成部１０７、微細軌道データを作成する微細軌道データ作成部１１０及び微細軌道データに基づいて軌道を制御する軌道制御部１２０を備える。各構成部の機能の詳細については、後で説明する。
【００３９】
図２は、二自由度倒立振子２００の構成を示す図である。以下の実施形態においては、２次元平面内で運動する二自由度倒立振子２００を物体として説明を行なう。二自由度倒立振子２００は、第１リンク２０５と、第２リンク２１１と、第１リンク２０５を回転可能に固定点に接続する第１関節（ジョイント）２０１と、第１リンク２０５及び第２リンク２１１を回転可能に接続する第２関節２０７と、から構成される。
【００４０】
二自由度倒立振子２００の運動は、第１関節２０１及び第２関節２０７に与えるトルクによって定まり、第１リンク２０５のＸ軸に対する角度
【数１】
第２リンク２１１の第１リンク２０５に対する角度
【数２】
及びこれらの角速度
【数３】
によって表現される。そこで、本実施形態において、状態空間として以下の位相空間を採
用する。
【数４】
【００４１】
図３は、本実施形態による軌道計画方法を説明するための流れ図である。
【００４２】
図３のステップＳ０１０において、探索木作成部１０１が、二自由度倒立振子２００の現在の状態及び目標状態を得る。ここで、状態とは、位相空間上の位置を意味する。
【００４３】
図３のステップＳ０２０において、探索木作成部１０１が、位相空間において、現在の状態の位置を探索木の根として定める。
【００４４】
図３のステップＳ０３０において、探索木作成部１０１が、位相空間において、所定の時間間隔後の次の状態の位置を、探索木の枝として定める。所定の時間間隔後の次の状態の位置は、運動方程式や動力学シミュレータによって定める。上述のように、二自由度倒立振子２００の運動は、第１関節２０１及び第２関節２０７に与えるトルクによって定まるので、所定の時間間隔に第１関節２０１及び第２関節２０７に与えるトルクを定めれば、二自由度倒立振子２００の、所定の時間間隔後の次の状態が定まる。そこで、所定の時間間隔に第１関節２０１及び第２関節２０７に与えるトルクを最大値と最小値との間で、所定の増分で変化させながら、それぞれの場合に対応する次の状態を求めてもよい。この場合に、トルクを最大値と最小値との間で、所定の増分で変化させながら、幅優先探索により網羅的に探索を行なってもよい。
【００４５】
図４は、図３のステップＳ０２０及びステップＳ０３０の動作を説明するための図である。なお、図４は、簡単のために位相空間を２次元空間として表している。現在の状態にある、二自由度倒立振子２００に所定の時間間隔に３個の異なるトルク
【数５】
を与えることにより、３個の異なる次の状態が定まる。探索木作成部１０１は、これらの
状態を探索木の枝とする。さらに、生成された枝が示すそれぞれの状態に対して、３個の
異なるトルク
【数６】
を与えることにより、３個の異なる、さらに次の状態が定まる。
【００４６】
図３のステップＳ０４０において、既知状態登録木作成部１０３は、探索木の枝の内、所定のものを枝とする既知状態登録木を作成する。既知状態登録木を作成するため、位相空間を予め、分割しておく。
【００４７】
図５は、分割された位相空間を示す図である。分割は、想定される角度及び角速度の範囲を等分割してもよい。以下において、分割された位相空間をセルと呼称する。
【００４８】
探索木作成部１０１が、探索木を作成する場合に、上述のように、根または枝から派生させる枝の数をＮとし、木の深さをＭとすると、木の深さＭのレベルで探索の対象となる枝の数は、ＮＭとなる。そこで、既知状態登録木作成部１０３は、上述のセルを利用して、探索木作成部１０１が派生させた枝を集約する。具体的に、既知状態登録木作成部１０３は、図３のステップＳ０４０において、以下の処理を行う。
【００４９】
既知状態登録木作成部１０３は、探索木作成部１０１が派生させた枝が表す次の状態が位相空間のどのセルに属するか定める。つぎに、既知状態登録木作成部１０３は、そのセルに属する状態が、既知状態登録木の枝としてすでに登録されているかどうか判断する。登録されていなければ、ステップＳ０４５に進む。登録されていれば、ステップＳ０６０に進む。
【００５０】
図３のステップＳ０４５において、既知状態登録木作成部１０３は、探索木作成部１０１が派生させた枝が表す次の状態が身体制約及び環境制約を満たすかどうか判断する。身体制約とは、身体の部材に相当する第１及び第２のリンクが、たとえばお互いに衝突しないために設けられる角度または角速度の制約である。また、環境制約とは、身体の部材に相当する第１及び第２のリンクが、たとえば外部の障害物に衝突しないように設けられる角度または角速度の制約である。次の状態が身体制約及び環境制約を満たせば、ステップＳ０５０に進む。身体制約及び環境制約のいずれかを満たさなければ、ステップＳ０６０に進む。
【００５１】
図３のステップＳ０５０において、既知状態登録木作成部１０３は、探索木作成部１０１が派生させた枝が表す次の状態を既知登録木の枝として登録する。
【００５２】
既知状態登録木作成部１０３は、既知登録木の枝を登録する際に、その枝が表す状態と目標状態との距離を求め、該距離を、すでに存在する既知登録木の枝が表す状態と目標状態との距離の内、最短の距離と比較し、該距離の方が短ければ、該距離に対応する状態を最終最良状態として、該距離とともに既知状態登録木記憶部１０５に記憶させてもよい。
【００５３】
図３のステップＳ０６０において、既知状態登録木作成部１０３は、探索木作成部１０１が派生させた枝が表す次の状態を破棄する。
【００５４】
図６は、図３のステップＳ０５０及びステップＳ０６０の動作を説明するための図である。既知登録木の枝は、原則として１個のセル内には１本しか存在しない。探索木作成部１０１が、既知登録木の枝（図６の実線で示した円）がすでに存在するセルに新たな枝（図６の点線で示した円）を定めた場合には、その枝は破棄される。すなわち、位相空間上の位置が近い状態は集約される。このように、従来の探索木の枝を、セルを使用して既知登録木の枝に集約することによって、計算コストを大幅に低減することができる。
【００５５】
なお、既知状態登録木作成部１０３は、探索木作成部１０１が派生させた枝が、目標状態の属するセルに属する場合には、そのセルに属する状態に対応する既知状態登録木の枝がすでに登録されていても、例外的に新しい枝をさらに登録するようにしてもよい。このようにすることにより、さらに目標状態に近い最終状態を得ることが可能となる。
【００５６】
図３のステップＳ０７０において、探索木作成部１０１は、探索が終了したかどうか判断する。探索木作成部１０１は、探索木の所定の深さまでの探索を終えた場合、あるいは所定時間の推移まで探索を終えた場合に探索が終了したと判断してもよい。探索が終了したと判断された場合には、ステップＳ０８０に進む。探索が終了していないと判断された場合には、ステップＳ０３０に戻り、探索が継続される。
【００５７】
図３のステップＳ０８０において、軌道作成部１０７は、既知状態登録木の枝が表す状態の中から、目標状態との位相空間上の距離が最も近い状態を選択し、この状態を最良状態とする（最終状態最適化の場合）。既知状態登録木作成部１０３が最終最良状態を求めている場合には、最終最良状態を最良状態とする。あるいは、軌道作成部１０７は、既知状態登録木の枝が表す状態の中から、目標状態との位相空間上の距離が所定値以下であり、探索木の深さが最も浅い枝に対応する状態を選択し、この状態を最良状態とする（到達時間最適化の場合）。軌道作成部１０７は、最良状態から現在の状態（初期状態）まで、既知登録木の枝の経路を求めることにより現在の状態から最良状態に至る軌道を求める。このようにして軌道計画が求まる。たとえば、ロボットは、このようにして求めた軌道計画にしたがってアームを制御することにより、アームの最終状態を最適化し、あるいは、目標状態への到達時間を最適化することができる。
【００５８】
本発明による軌道計画方法について、本明細書では、説明を簡単にするために、物体を二自由度倒立振子としている。しかし、本発明の、既知状態登録木を使用する考え方は、任意の物体の状態空間における軌道計画方法に適用することができる。
【００５９】
つぎに、図２に示した二自由度倒立振子２００のシミュレーションによる制御実験について説明する。二自由度倒立振子２００の第１リンク２０５のリンク長は、ｌ１であり、質量は、ｍ１であり、重心位置２０３はリンクの中央である。二自由度倒立振子２００の第２リンク２１１のリンク長は、ｌ２であり、質量は、ｍ２であり、重心位置２０９はリンクの中央である。また、重力加速度をｇとする。二自由度倒立振子２００の運動方程式は以下のとおりである。
【数７】
関節角度と重力の関係に対する理解を容易にするために、以下のように座標を変更した。
【数８】
【００６０】
図７は、二自由度倒立振子２００の初期状態を示す図である。二自由度倒立振子２００の初期状態の座標は以下のとおりである。
【数９】
【００６１】
図８は、二自由度倒立振子２００の目標状態を示す図である。二自由度倒立振子２００の目標状態の座標は以下のとおりである。
【数１０】
【００６２】
実験は、図３に示した軌道計画方法の手順及び二自由度倒立振子２００の運動をシミュレーションすることによって実行した。
【００６３】
実験１
表１は、実験１のシミュレーションの設定を示す。上述の次の状態までの時間間隔が、表１のトルク切替タイムステップに相当する。
【表１】
【００６４】
表２は、第１関節及び第２関節のパラメータを示す。表２において、第１関節の最大トルク2 [Nm]、探索木作成時のトルク粒度１０分割とは、探索木の新たな枝を派生させる際に、-2 [Nm]から2 [Nm]までのトルクの範囲を１０分割し、たとえば、 -1.8 [Nm], -1.4[Nm], -1.0[Nm]... 1.0[Nm], 1.4[Nm], 1.8 [Nm]というようにトルクを与えて、運動方程式から次の状態（角度及び角速度）を求めること
を意味する。実験１においては、第１関節及び第２関節に、それぞれ１０個の異なるトルクを与えるので、１００個の異なる次の状態が派生する。
【表２】
【００６５】
図９は、実験１によって得られた第１関節位相空間の軌道を示す図である。第１関節は、１．３秒後に目標状態に到達している。図９において、初期状態の角度ｑの座標は、πであり、−πに等しい。
【００６６】
図１０は、実験１によって得られた第２関節位相空間の軌道を示す図である。第２関節は、１．３秒後に目標状態に到達している。
【００６７】
図１１は、実験１において、探索木の枝として求められた全ての第１関節の状態の位置を第１関節位相空間上に示した図である。
【００６８】
図１２は、実験１において、探索木の枝として求められた全ての第２関節の状態の位置を第２関節位相空間上に示した図である。
【００６９】
図１１及び図１２から、位相空間上のほぼ全体が探索されていることがわかる。このことは、位相空間上の任意の状態を目標状態として軌道計画が得られることを意味する。
【００７０】
ここで、トルク粒度及び位相空間の状態分割数は、図１１及び図１２において、状態の位置が第１関節位相空間及び第２関節位相空間の十分広い範囲に一様に分布するように調整しながら定める。
【００７１】
実験２
表３は、実験２のシミュレーションの設定を示す。上述の次の状態までの時間間隔が、表３のトルク切替タイムステップに相当する。
【表３】
【００７２】
表４は、第１関節及び第２関節のパラメータを示す。実験２においては、実験１の場合よりも最大トルクを小さくしている。第２関節には、トルクを与えない。表４において、最大トルク1.5 [Nm]、探索木作成時のトルク粒度１０分割とは、探索木の新たな枝を派生させる際に、-1.5 [Nm]から1.5[Nm]までのトルクの範囲を１０分割し、たとえば、 -1.35 [Nm],-1.05[Nm], -0.75[Nm]... 0.75[Nm], 1.05[Nm], 1.35 [Nm]というようにトルクを与えて、運動方程式から次の状態（角度及び角速度）を求めることを意味する。実験２においては、第１関節に１０個の異なるトルクを与え、第２関節にト
ルクを与えないので、１０個の異なる次の状態が派生する。
【表４】
【００７３】
図１３は、実験２によって得られた第１関節位相空間の軌道を示す図である。第１関節は、４．９秒後に目標状態に到達している。図１３において、初期状態の角度ｑの座標は、πであり、−πに等しい。
【００７４】
図１４は、実験２によって得られた第２関節位相空間の軌道を示す図である。第２関節は、４．９秒後に目標状態に到達している。
【００７５】
図１５は、実験２において、探索木の枝として求められた全ての第１関節の状態の位置を第１関節位相空間上に示した図である。
【００７６】
図１６は、実験２において、探索木の枝として求められた全ての第２関節の状態の位置を第２関節位相空間上に示した図である。
【００７７】
図１５を図１１と比較し、図１６を図１２と比較すると、探索されていない領域が増加している。これは、最大トルクを小さく設定した結果、最大トルクで到達可能な領域が縮小したためである。最大トルクで到達可能な領域に対しては一様な探索が行なわれている。
【００７８】
実験３
実験３は、第１位相空間上の、角度が−９０°（−π／２）乃至０°の領域の状態となることがないように軌道を定める点を除いて、実験２と同じ条件で行った。位相空間上の領域を制限するには、図３のステップＳ０４５に示した手順による。
【００７９】
図１７は、実験３によって得られた第１関節位相空間の軌道、及び実験３において、探索木の枝として求められた全ての第１関節の状態の位置を示した図である。図１７によれば、第１位相空間上の、角度が−９０°（−π／２）乃至０°の領域（図１７のＡで示した領域）の状態は選択されていない。
【００８０】
図１８は、実験３によって得られた第２関節位相空間の軌道、及び実験３において、探索木の枝として求められた全ての第２関節の状態の位置を示した図である。
【００８１】
図１７と図１３及び図１８と図１４をそれぞれ比較すると、軌道は異なっているが、図１７及び図１８に示すように最終的に目標に到達している。
【００８２】
計算コストの評価
表５は、実験１、実験２及び実験１と同じ条件で、既知状態登録木を使用しなかった場合の探索状態数及び計算時間を示す表である。既知状態登録木を使用しなかった場合には、上述のように、根または枝から派生させる枝の数をＮとし、最深部の木の深さをＭとすると、最深部において探索の対象となる枝（状態）の数は、ＮＭとなる。実験１の条件によると、
N=10x10=100
M=5[s]/0.1[s]=50
となる。最深部以外の深さの枝の数を加えると、探索状態数は、以下のおとりである。
【数１１】
計算時間は、Intel Core2 Duo T7500 搭載のうち1コアのみを使用して測定した。
【００８３】
表５によると、既知状態登録木を使用しなかった場合の探索状態数は膨大となり、網羅的な全探索を行うことはできない。本実施形態の実験１によれば、従来技術では実現できなかった、網羅的な全探索を行うことが可能となる。このように本発明によれば、位相空間上の探索が制約を受けることはない。
【００８４】
また、本発明の既知状態登録木は、網羅的な全探索を行う場合だけではなく、評価関数などを使用して選択的に探索木を探索する場合にも適用することができる。
【表５】
【００８５】
軌道制御
つぎに、作成された軌道の制御について説明する。
【００８６】
図１９は、図１に示した軌道計画・制御システムの微細軌道データ作成部１１０及び軌道制御部１２０の構成を示す図である。
【００８７】
微細軌道データ作成部１１０は、軌道作成部１０７の作成した軌道データ（１００ミリ秒ごとの角度、角速度、トルク）から、微細軌道データ（５ミリ秒ごとの角度
【数１２】
、角速度
【数１３】
、トルク
【数１４】
）を作成する。
【００８８】
軌道制御部１２０は、ＰＩＤコントローラ１２１、サーボ・モータ１２３及び角度センサ１２５を含む。軌道制御部１２０のＰＩＤコントローラ１２１は、微細軌道データの角度を実現するように５ミリ秒ごとにサーボ・モータを操作する。このようにして、リアルタイムの軌道制御を実現することができる。
【００８９】
図２０は、軌道制御の動作を説明するための流れ図である。
【００９０】
図２０のステップＳ１１０において、微細軌道データ作成部１１０は、軌道作成部１０７から軌道データを受け取る。受け取る軌道データは、１００ミリ秒ごとの角度、角速度及びトルクである。
【００９１】
図２０のステップＳ１２０において、微細軌道データ作成部１１０は、１００ミリ秒ごとの角度、角速度及びトルクである軌道データから、運動方程式を用いて、５ミリ秒ごとの角度
【数１５】
、角速度
【数１６】
及びトルク
【数１７】
である微細軌道データを作成する。
【００９２】
図２０のステップＳ１３０において、軌道制御部１２０は、微細軌道データに基づいてフィードバック制御を行う。具体的に、軌道制御部１２０のＰＩＤコントローラ１２１は、５ミリ秒ごとに、微細軌道データから求めたｎ時間間隔後の角度
【数１８】
を目標値とし、現在の角度測定値
【数１９】
を出力値として、該目標値と該出力値との偏差からＰＩＤ制御によってトルク目標値
【数２０】
を操作量として定める。ｎは整数であり、ｎ時間間隔とは５ｎミリ秒である。フィードバックゲインが十分である場合にはｎ＝１が最適である。フィードバックゲインが不十分である場合には、ｎは、１より大きな整数から選択する。ここで、ノイズがない場合には、ＰＩＤコントローラ１２１がサーボ・モータ１２３へ出力するトルク目標値
【数２１】
は、微細軌道データに含まれる予測トルク値
【数２２】
と一致する。予測トルク値
【数２３】
は、実験２において説明したようにトルク限界を考慮して計算されているため、ＰＩＤコントローラ１２１によるトルク目標値もトルク限界を踏まえた値の周囲に収まる。
【００９３】
図２１は、軌道データ作成部１０７によって作成された１００ミリ秒ごとの軌道データ（細い線）と微細軌道データ作成部１１０によって作成された５ミリ秒ごとの軌道データ（太い線）とを示す図である。１００ミリ秒ごとの軌道データは、図１４に示した軌道データと同じである。
【００９４】
図２２は、微細軌道データ作成部１１０によって作成された５ミリ秒ごとの軌道データ（濃い線）と軌道生成の際にトルクに５％のノイズを加えた場合の軌道データ（薄い線）とを示す図である。図２２から明らかなように、ノイズによって軌道は変化する。
【００９５】
図２３は、図２２に示したノイズを加えた場合に、ＰＩＤコントローラ１２１による制御を適用した場合の軌道データを示す図である。図２３に示す軌道データは、図２２に示すノイズがない場合の軌道データと同じである。すなわち、フィードバック制御によってノイズはキャンセルされ、微細軌道データ作成部１１０によって作成された軌道が実現される。図２３の軌道における色の濃さは、ＰＩＤコントローラ１２１の操作量の大きさを示している。
【００９６】
ロボットへの応用
ここでロボットとは、少なくとも１個以上の関節（ジョイント）で接続された部材を有し、部材の位置を制御することのできる機械装置を指す。
【００９７】
図２４は、肘軸、肩軸、腰軸、足首軸など複数の関節によって接続された複数の部材から構成されるロボットを示す図である。ロボットの付近には障害物が存在し、ロボットの腕に相当する部材は、障害物を避けながら目標に到達するように制御される。
【００９８】
図２４に示すようなロボットにおいて、手先目標または全身姿勢目標へ向けて上半身に相当する部材と下半身に相当する部材を協調して動作させることを考える。この場合には、位相空間上における現在状態から次状態への遷移を、ロボットの全身（全部材）の運動方程式を用いて予測すればよい。
【００９９】
図２４に示すようなロボットにおいて、多関節を同時に制御することを考える。この場合には、全ての関節の関節角度及び関節角速度を、関節数の２倍の次元を有する高次元位相空間上に写像して機械状態を記述すればよい。
【０１００】
図２４に示すようなロボットにおいて、身体制約（ロボット自体の制約）を満たす運動を計画する。身体制約として、トルク制約を設ける場合には、実験２で説明したように、次状態の推定に使用するトルクに限界を設定する。身体制約として、位置または速度限界を設ける場合には、図３のステップＳ０４５に関して説明したように、制約条件を満たさない次の状態を破棄する。この場合の結果は、実験３に関連して説明した。
【０１０１】
図２４に示すようなロボットにおいて、障害物を回避する運動を計画する。図３のステップＳ０４５に関して説明したように、障害物と接触する場合に次の状態を破棄する。
【０１０２】
図２４に示すようなロボットを、作成した軌道にしたがってリアルタイムで動作させることを考える。この場合には、微細軌道データ作成部１１０によって微細軌道データを作成し、微細軌道データに基づいて図１９に示したＰＩＤコントローラ１２１によるＰＩＤ制御などのフィードバック制御によってサーボ・モータ１２３を操作し、作成した微細軌道に沿って部材が動作するようにする。
【０１０３】
上記のように、本発明による軌道計画方法及び軌道計画システムは、複数の関節で接続された複数の部材から構成されるロボットにおいて、手先目標または全身姿勢目標へ向けて上半身に相当する部材と下半身に相当する部材を協調して動作させる場合、多関節を同時に制御する場合、身体制約を満たす運動を計画する場合、障害物を回避する運動を計画する場合、作成した軌道にしたがってリアルタイムでロボットを動作させる場合に有効に使用することができる。
【符号の説明】
【０１０４】
１０１…探索木作成部、１０３…既知状態登録木作成部、１０５…既知状態登録木記憶部、１０７…軌道作成部、１１０…微細軌道データ作成部、１２０…軌道制御部
【特許請求の範囲】
【請求項１】
軌道計画システムによって、物体の状態を目標状態へ制御するための軌道を求める軌道計画方法であって、
該軌道計画システムの探索木作成部が、該物体の状態を、状態空間において探索木の根として定めるステップと、
該探索木作成部が、所定の時間間隔における力学的な関係から、該時間間隔が経過した後の、該物体の次の状態を、状態空間において探索木の枝として定めるステップと、
該軌道計画システムの既知状態登録木作成部が、予め区分された状態空間の複数のセルから、該次の状態が属するセルを定め、該セルに属する状態が、既知状態登録木の枝としてすでに登録されているかどうか判断するステップと、
該既知状態登録木作成部が、該セルに属する状態が登録されている場合に、該次の状態を破棄するステップと、
該既知状態登録木作成部が、該セルに属する状態が登録されていない場合に、該セルに属する該次の状態を該既知状態登録木の枝として既知状態登録木記憶部に登録するステップと、
該軌道計画システムの軌道作成部が、探索が終了した後に、該既知状態登録木記憶部に該既知状態登録木の枝として登録された状態の中から、該目標状態までの距離がもっとも小さい状態を選択し、その状態から、該既知状態登録木の根側に遡って求めた一連の状態によって軌道を求めるステップと、を含む軌道計画方法。
【請求項２】
状態が属することのできる状態空間の範囲を定めることにより、物体の動作を制限する請求項１に記載の軌道計画方法。
【請求項３】
前記所定の時間間隔における力学的な関係に制約条件を定めることにより、物体の動作を制限する請求項１または２に記載の軌道計画方法。
【請求項４】
該探索木作成部が、物体の状態空間上の次の状態を、探索木の枝として求める際に、探索木の末端の全ての枝から次の状態に対応する枝を派生させる請求項１から３のいずれかに記載の軌道計画方法。
【請求項５】
該探索木作成部が、物体の状態空間上の次の状態を、探索木の枝として求める際に、探索木の枝の内、選択した枝から次の状態に対応する枝を派生させる請求項１から３のいずれかに記載の軌道計画方法。
【請求項６】
該既知状態登録木作成部が、該次の状態が属するセルが該目標状態を含む場合には、該セルに属する状態が、該既知状態登録木の枝としてすでに登録されている場合にも、該次の状態を破棄せず、該セルに属する該次の状態を該既知状態登録木の枝として既知状態登録木記憶部に登録する請求項１から５のいずれかに記載の軌道計画方法。
【請求項７】
該既知状態登録木作成部が、該セルに属する状態が登録されていない場合に、該次の状態を該セルに属する状態として登録した後に、該次の状態と該目標状態との状態空間における距離を求め、該距離が、これまでに該既知状態登録木の枝として登録された状態の該目標状態との距離の最小値よりも小さければ、該次の状態を最終最良状態として記録し、該軌道作成部が、該目標状態までの距離がもっとも小さい状態として、該最終最良状態を選択する請求項１から６のいずれかに記載の軌道計画方法。
【請求項８】
該物体が、先端部を除く端部がジョイントによって接続された、１または複数の部材である請求項１から７のいずれかに記載の軌道計画方法。
【請求項９】
該物体が、ロボットの部材である請求項１から８のいずれかに記載の軌道計画方法。
【請求項１０】
状態空間が位相空間である請求項１から９のいずれかに記載の軌道計画方法。
【請求項１１】
物体の状態を目標状態とするための軌道を求める軌道計画システムであって、
該物体の状態を、状態空間において探索木の根として定め、所定の時間間隔における力学的な関係から、該時間間隔が経過した後の、該物体の次の状態を、状態空間において探索木の枝として定める探索木作成部と、
既知状態登録木を記憶する既知状態登録木記憶部と、
予め区分された状態空間の複数のセルから、該次の状態が属するセルを定め、該セルに属する状態が、既知状態登録木の枝としてすでに登録されているかどうか判断し、該セルに属する状態が登録されている場合に、該次の状態を破棄し、該セルに属する状態が登録されていない場合に、該セルに属する該次の状態を該既知状態登録木の枝として該既知状態登録木記憶部に登録する、既知状態登録木作成部と、
探索が終了した後に、該既知状態登録木記憶部に該既知状態登録木の枝として登録された状態の中から、該目標状態までの距離がもっとも小さい状態を選択し、その状態から、該既知状態登録木の根側に遡って求めた一連の状態によって軌道を求める軌道作成部と、を備えた軌道計画システム。
【請求項１２】
状態が属することのできる状態空間の範囲を定めることにより、物体の動作を制限する請求項１１に記載の軌道計画システム。
【請求項１３】
前記所定の時間間隔における力学的な関係に制約条件を定めることにより、物体の動作を制限する請求項１１または１２に記載の軌道計画システム。
【請求項１４】
状態空間が位相空間である請求項１１から１３のいずれかに記載の軌道計画システム。
【請求項１５】
請求鋼１１から１４のいずれかに記載の軌道計画システムと、該軌道計画システムが求めた軌道をフィードバック制御によって実現する軌道制御部とを備えた、軌道計画・制御システム。
【請求項１６】
請求項１５に記載の軌道計画・制御システムを備えたロボットであって、該物体が、該ロボットの部材であり、該軌道にしたがって、該ロボットの部材を制御するロボット。
【請求項１】
軌道計画システムによって、物体の状態を目標状態へ制御するための軌道を求める軌道計画方法であって、
該軌道計画システムの探索木作成部が、該物体の状態を、状態空間において探索木の根として定めるステップと、
該探索木作成部が、所定の時間間隔における力学的な関係から、該時間間隔が経過した後の、該物体の次の状態を、状態空間において探索木の枝として定めるステップと、
該軌道計画システムの既知状態登録木作成部が、予め区分された状態空間の複数のセルから、該次の状態が属するセルを定め、該セルに属する状態が、既知状態登録木の枝としてすでに登録されているかどうか判断するステップと、
該既知状態登録木作成部が、該セルに属する状態が登録されている場合に、該次の状態を破棄するステップと、
該既知状態登録木作成部が、該セルに属する状態が登録されていない場合に、該セルに属する該次の状態を該既知状態登録木の枝として既知状態登録木記憶部に登録するステップと、
該軌道計画システムの軌道作成部が、探索が終了した後に、該既知状態登録木記憶部に該既知状態登録木の枝として登録された状態の中から、該目標状態までの距離がもっとも小さい状態を選択し、その状態から、該既知状態登録木の根側に遡って求めた一連の状態によって軌道を求めるステップと、を含む軌道計画方法。
【請求項２】
状態が属することのできる状態空間の範囲を定めることにより、物体の動作を制限する請求項１に記載の軌道計画方法。
【請求項３】
前記所定の時間間隔における力学的な関係に制約条件を定めることにより、物体の動作を制限する請求項１または２に記載の軌道計画方法。
【請求項４】
該探索木作成部が、物体の状態空間上の次の状態を、探索木の枝として求める際に、探索木の末端の全ての枝から次の状態に対応する枝を派生させる請求項１から３のいずれかに記載の軌道計画方法。
【請求項５】
該探索木作成部が、物体の状態空間上の次の状態を、探索木の枝として求める際に、探索木の枝の内、選択した枝から次の状態に対応する枝を派生させる請求項１から３のいずれかに記載の軌道計画方法。
【請求項６】
該既知状態登録木作成部が、該次の状態が属するセルが該目標状態を含む場合には、該セルに属する状態が、該既知状態登録木の枝としてすでに登録されている場合にも、該次の状態を破棄せず、該セルに属する該次の状態を該既知状態登録木の枝として既知状態登録木記憶部に登録する請求項１から５のいずれかに記載の軌道計画方法。
【請求項７】
該既知状態登録木作成部が、該セルに属する状態が登録されていない場合に、該次の状態を該セルに属する状態として登録した後に、該次の状態と該目標状態との状態空間における距離を求め、該距離が、これまでに該既知状態登録木の枝として登録された状態の該目標状態との距離の最小値よりも小さければ、該次の状態を最終最良状態として記録し、該軌道作成部が、該目標状態までの距離がもっとも小さい状態として、該最終最良状態を選択する請求項１から６のいずれかに記載の軌道計画方法。
【請求項８】
該物体が、先端部を除く端部がジョイントによって接続された、１または複数の部材である請求項１から７のいずれかに記載の軌道計画方法。
【請求項９】
該物体が、ロボットの部材である請求項１から８のいずれかに記載の軌道計画方法。
【請求項１０】
状態空間が位相空間である請求項１から９のいずれかに記載の軌道計画方法。
【請求項１１】
物体の状態を目標状態とするための軌道を求める軌道計画システムであって、
該物体の状態を、状態空間において探索木の根として定め、所定の時間間隔における力学的な関係から、該時間間隔が経過した後の、該物体の次の状態を、状態空間において探索木の枝として定める探索木作成部と、
既知状態登録木を記憶する既知状態登録木記憶部と、
予め区分された状態空間の複数のセルから、該次の状態が属するセルを定め、該セルに属する状態が、既知状態登録木の枝としてすでに登録されているかどうか判断し、該セルに属する状態が登録されている場合に、該次の状態を破棄し、該セルに属する状態が登録されていない場合に、該セルに属する該次の状態を該既知状態登録木の枝として該既知状態登録木記憶部に登録する、既知状態登録木作成部と、
探索が終了した後に、該既知状態登録木記憶部に該既知状態登録木の枝として登録された状態の中から、該目標状態までの距離がもっとも小さい状態を選択し、その状態から、該既知状態登録木の根側に遡って求めた一連の状態によって軌道を求める軌道作成部と、を備えた軌道計画システム。
【請求項１２】
状態が属することのできる状態空間の範囲を定めることにより、物体の動作を制限する請求項１１に記載の軌道計画システム。
【請求項１３】
前記所定の時間間隔における力学的な関係に制約条件を定めることにより、物体の動作を制限する請求項１１または１２に記載の軌道計画システム。
【請求項１４】
状態空間が位相空間である請求項１１から１３のいずれかに記載の軌道計画システム。
【請求項１５】
請求鋼１１から１４のいずれかに記載の軌道計画システムと、該軌道計画システムが求めた軌道をフィードバック制御によって実現する軌道制御部とを備えた、軌道計画・制御システム。
【請求項１６】
請求項１５に記載の軌道計画・制御システムを備えたロボットであって、該物体が、該ロボットの部材であり、該軌道にしたがって、該ロボットの部材を制御するロボット。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２４】
【図２２】
【図２３】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２４】
【図２２】
【図２３】
【公開番号】特開２０１１−１６１６２４（Ｐ２０１１−１６１６２４Ａ）
【公開日】平成２３年８月２５日（２０１１．８．２５）
【国際特許分類】
【出願番号】特願２０１０−２７４００５（Ｐ２０１０−２７４００５）
【出願日】平成２２年１２月８日（２０１０．１２．８）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【Ｆターム（参考）】
【公開日】平成２３年８月２５日（２０１１．８．２５）
【国際特許分類】
【出願日】平成２２年１２月８日（２０１０．１２．８）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【Ｆターム（参考）】
[ Back to top ]