軌道計画方法、軌道計画システム及び軌道計画・制御システム
【課題】運動コストを最小化するように状態空間を分割し、より精度よく準コスト最小軌道を計画することのできる軌道計画方法を提供する
【解決手段】本発明による軌道計画方法は、軌道計画システム（１００）によって物体の状態を目標状態へ制御するための軌道を求める軌道計画方法であって、該物体の状態空間をセルに分割するセル作成部（１０３）が、所定のセルの数に対して、離散化による近似誤差を最小化するように該状態空間をセルに分割するステップと、探索木作成部（１０１）が、該状態空間において、該物体の状態遷移に対応する探索木を、該物体の状態に対応する、該探索木の枝のノードがセルに一つずつ含まれるように作成するステップと、軌道作成部（１０７）が、該探索木を使用して、該物体の現在の状態から目標状態までの経路を定めるステップと、を含む。
【解決手段】本発明による軌道計画方法は、軌道計画システム（１００）によって物体の状態を目標状態へ制御するための軌道を求める軌道計画方法であって、該物体の状態空間をセルに分割するセル作成部（１０３）が、所定のセルの数に対して、離散化による近似誤差を最小化するように該状態空間をセルに分割するステップと、探索木作成部（１０１）が、該状態空間において、該物体の状態遷移に対応する探索木を、該物体の状態に対応する、該探索木の枝のノードがセルに一つずつ含まれるように作成するステップと、軌道作成部（１０７）が、該探索木を使用して、該物体の現在の状態から目標状態までの経路を定めるステップと、を含む。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、物体の、目標状態に至る状態遷移系列を作成する、軌道計画方法、軌道計画システム及び上記軌道計画システムを備えた軌道計画・制御システムに関する。
【背景技術】
【０００２】
たとえば、ロボットのアームの制御において、軌道計画に基づいてアームのジョイント部のトルクを制御し、アームを目標の状態とする場合に、アームの位置及び角度のみの軌道計画では、ジョイント部のトルクを制御することはできない。ジョイント部のトルクを制御するには、アームの角度及び角速度を含む状態空間（位相空間）の軌道計画を作成する必要がある。
【０００３】
位相空間上での状態遷移を取り扱うための手法として、Rapidly-Exploring Random Trees (RRT) を用いた手法(非特許文献１)がある。しかし、RRTは確率的手法であるため必ずしもコスト最小に近い運動を計画できるとは限らない。また、状態空間の量子化を利用したTazaki の手法(非特許文献２)があるが、この手法は遷移を連続的に扱っておらず、強い線形性の仮定が要求されるため、適用できる機械の種類が限定される。
【０００４】
そこで発明者らによって、連続的な探索により準コスト最小運動を計画できる手法として、位相空間の分割と探索木の枝の集約を用いた運動計画手法が提案されている(非特許文献３)。この手法によれば、連続的な探索により、動力学的な拘束条件のもとで与えられた初期状態から目標状態の集合までの準コスト最小運動を短時間で計画できる。しかし、この手法においては、位相空間の分割の際に、各軸に均一な分割を設定し、設計者が粒度を個別に決定していた。また、運動コストを最小化するための効果的な分割の構成方法は実現されていなかった。
【０００５】
このように、運動コストを最小化するように状態空間を分割し、より精度よく準コスト最小軌道を計画することのできる軌道計画方法、軌道計画システム及び軌道計画・制御システムは開発されていなかった。
【先行技術文献】
【非特許文献】
【０００６】
【非特許文献１】S. M. LaValle and J. J. Kuffner.:”Randomized Kinodynamic Planning,”Journal of Robotics Research, Vol. 20, No. 5, pp.378-400,(2001).
【非特許文献２】Y. Tazaki and J. Imura:”Approximately Bisimilar Discrete Abstractions of Nonlinear Systems Using Variable-resolution Quantizers,”American Control Conference, pp.1015-1020, (2010).
【非特許文献３】C. H. Kim, H. Tsujino and S. Sugano: ”Rapid Short-Time Path Planning for Phase Space,” Journal of Robotics and Mechatronics, Vol. 23, No. 2, (2011).
【発明の概要】
【発明が解決しようとする課題】
【０００７】
したがって、運動コストを最小化するように状態空間を分割し、より精度よく準コスト最小軌道を計画することのできる軌道計画方法、軌道計画システム及び軌道計画・制御システムに対するニーズがある。
【課題を解決するための手段】
【０００８】
本発明による軌道計画方法は、軌道計画システムによって、物体の状態を目標状態へ制御するための軌道を求める軌道計画方法であって、該物体の状態空間をセルに分割するセル作成部が、所定のセルの数に対して、離散化による近似誤差を最小化するように該状態空間をセルに分割するステップと、探索木作成部が、該状態空間において、該物体の状態遷移に対応する探索木を、該物体の状態に対応する、該探索木の枝のノードがセルに一つずつ含まれるように作成するステップと、軌道作成部が、該探索木を使用して、該物体の現在の状態から目標状態までの経路を定めるステップと、を含む。
【０００９】
本発明による軌道計画方法においては、所定のセルの数に対して、離散化による近似誤差を最小化するように状態空間をセルに分割するので、運動コストが最小化され、より精度よく準コスト最小軌道を計画することができる。
【００１０】
本発明の一実施形態による軌道計画方法においては、前記分割するステップにおいて、セルの近似誤差を、セル内に含まれる任意の点とセルの中心点に入力を与えた場合の状態遷移先の点の距離の平均として表し、前記状態空間のセルの近似誤差の和が最小となるようにすることによって前記離散化による近似誤差を最小とする。
【００１１】
本実施形態によれば、簡単な手順によって離散化による近似誤差を最小とすることができる。
【００１２】
本発明の一実施形態による軌道計画方法においては、前記分割するステップが、前記状態空間を均一な基本セルに分割するステップと、前記所定のセルの数に達するまで、基本セルを、セルの近似誤差を最も小さくする軸の方向に分割するステップと、セルの近似誤差の和を小さくするようにセルの分割と結合を繰り返すステップとを含む。
【００１３】
本実施形態によれば、基本セルの中で近似誤差が最小となるように分割を実施し、その後、基本セルの間にまたがり、さらに大域的に平均的な近似誤差を小さくするように、分割と結合を繰り返すので、離散化による近似誤差を効率的に最小化することができる。
【００１４】
本発明による軌道計画システムは、物体の状態を目標状態へ制御するための軌道を求める軌道計画システムであって、離散化による近似誤差を最小化するように、該物体の状態空間をセルに分割するセル作成部と、該状態空間において、該物体の状態遷移に対応する探索木を、該物体の状態に対応する、該探索木の枝のノードがセルに一つずつ含まれるように作成する探索木作成部と、該探索木を使用して、該物体の現在の状態から目標状態までの軌道を定める軌道作成部と、を備える。
【００１５】
本発明による軌道計画システムは、所定のセルの数に対して、離散化による近似誤差を最小化するように状態空間をセルに分割するので、運動コストが最小化され、より精度よく準コスト最小軌道を計画することができる。
【００１６】
本発明による軌道計画・制御システムは、本発明による軌道計画システムと、該軌道計画システムが定めた軌道を実現する軌道制御部とを備えている。
【００１７】
本発明による軌道計画・制御システムは、本発明による軌道計画システムによってより精度よく計画された準コスト最小軌道を実現することができる。
【図面の簡単な説明】
【００１８】
【図１】本発明の一実施形態による軌道計画システムの構成を示す図である。
【図２】二重倒立振子の構成を示す図である。
【図３】本実施形態による軌道計画法を説明するための流れ図である。
【図４】分割された位相空間の例を示す図である。
【図５】図３のステップＳ０２０及びステップＳ０３０の動作を説明するための図である。
【図６】図３のステップＳ０５０及びステップＳ０６０の動作を説明するための図である。
【図７】セルの近似誤差の計算方法を説明するための図である。
【図８】セル作成部によって、位相空間を分割してセルを作成する方法を示す流れ図である。
【図９】図８のステップＳ１０２０の詳細を説明するための流れ図である。
【図１０】図８のステップＳ１０３０の詳細を説明するための流れ図である。
【図１１】二重倒立振子の第１の初期状態を示す図である。
【図１２】二重倒立振子の第２の初期状態を示す図である。
【図１３】状態数（最終的なセルの数）を２０４と設定した場合に、均一分割及び不均一分割の、位置姿勢セル内
【数１】
における速度空間の断面を示す図である。
【図１４】セルの数とシミュレーションによって得られた平均運動時間との関係を示す図である。
【図１５】状態数が、１６４、２０４、２４４及び２８４のそれぞれの場合について、３種類の均一分割を使用した場合と図８乃至図１０にしたがって分割を行った場合の結果を示す図である。
【発明を実施するための形態】
【００１９】
図１は、本発明の一実施形態による軌道計画システム１００の構成を示す図である。軌道計画システム１００は、ロボットのアームなどの物体の状態を目標状態とするための、状態空間における軌道を求めるシステムである。軌道計画システム１００は、状態空間上の物体の位置、すなわち状態に対応する枝を備えた探索木を作成する探索木作成部１０１と、状態空間を分割して複数のセルを作成するセル作成部１０３と、セル及び探索木を記憶するセル・探索木記憶部１０５と、探索木から、物体の状態空間における軌道を作成する軌道作成部１０７と、を備える。さらに、作成された軌道にしたがって物体の動きを制御する軌道制御部１２０を追加すれば、軌道計画・制御システムが得られる。各構成部の機能の詳細については、後で説明する。
【００２０】
図２は、二重倒立振子２００の構成を示す図である。以下の実施形態においては、物体を２次元平面内で運動する二重倒立振子２００として説明を行なう。二重倒立振子２００は、重心２０３を有する第１リンク２０５と、重心２０９を有する第２リンク２１１と、第１リンク２０５を回転可能に固定点に接続する第１関節（ジョイント）２０１と、第１リンク２０５及び第２リンク２１１を回転可能に接続する第２関節２０７と、から構成される。
【００２１】
二重倒立振子２００の運動は、第１関節２０１及び第２関節２０７に与えるトルクによって定まり、第１リンク２０５のＸ軸に対する角度
【数２】
第２リンク２１１の第１リンク２０５に対する角度
【数３】
及びこれらの角速度
【数４】
によって表現される。そこで、本実施形態において、状態空間として以下の位相空間を採用する。
【数５】
【００２２】
図３は、本実施形態による軌道計画方法を説明するための流れ図である。
【００２３】
図３のステップＳ００５において、セル作成部１０３が、位相空間を分割し複数のセルを作成しておく。セル作成部の動作については後で詳細に説明する。
【００２４】
図４は、分割された位相空間の例を示す図である。図4は、簡単のために位相空間を２次元空間として表している。また、図４においては、図を簡略にするため、想定される角度及び角速度の範囲を等分割したセルが示されている。位相空間の分割方法については後で詳細に説明する。
【００２５】
図３のステップＳ０１０において、探索木作成部１０１が、二重倒立振子２００の現在の状態及び目標状態を得る。ここで、状態は、位相空間上の位置に対応する。
【００２６】
図３のステップＳ０２０において、探索木作成部１０１が、位相空間において、現在の状態の位置を探索木の根として定める。
【００２７】
図３のステップＳ０３０において、探索木作成部１０１が、位相空間において、所定の時間間隔後の次の状態の位置を、探索木の枝のノードとして定める。所定の時間間隔後の次の状態の位置は、運動方程式や動力学シミュレータによって定める。上述のように、二重倒立振子２００の運動は、第１関節２０１及び第２関節２０７に与えるトルクによって定まるので、所定の時間間隔に第１関節２０１及び第２関節２０７に与えるトルクを定めれば、二重倒立振子２００の、所定の時間間隔後の次の状態が定まる。そこで、所定の時間間隔に第１関節２０１及び第２関節２０７に与えるトルクを最大値と最小値との間で、所定の増分で変化させながら、それぞれの場合に対応する次の状態を求めてもよい。この場合に、トルクを最大値と最小値との間で、所定の増分で変化させながら、幅優先探索により網羅的に探索を行なってもよい。
【００２８】
図５は、図３のステップＳ０２０及びステップＳ０３０の動作を説明するための図である。なお、図５は、簡単のために位相空間を２次元空間として表している。現在の状態にある、二重倒立振子２００に所定の時間間隔に３個の異なるトルク
【数６】
を与えることにより、３個の異なる次の状態が定まる。探索木作成部１０１は、これらの状態を探索木の枝のノードとする。さらに、生成された枝のノードが示すそれぞれの状態に対して、３個の異なるトルク
【数７】
を与えることにより、３個の異なる、さらに次の状態が定まる。
【００２９】
図３のステップＳ０４０において、探索木作成部１０１は、探索木の枝の集約を行う。探索木作成部１０１が、探索木を作成する場合に、上述のように、根または枝から派生させる枝の数をＮとし、木の深さをＭとすると、木の深さＭのレベルで探索の対象となる枝の数はＮＭとなる。そこで、探索木作成部１０１は、上述のセルを利用して、派生させた枝を集約する。具体的に探索木作成部１０１は、図３のステップＳ０４０において、以下の処理を行う。
【００３０】
探索木作成部１０１は、派生させた次の状態が位相空間のどのセルに属するか定める。つぎに、探索木作成部１０１は、そのセルに属する状態が、枝のノードとしてすでに登録されているかどうか判断する。登録されていなければ、ステップＳ０４５に進む。登録されていれば、ステップＳ０６０に進む。
【００３１】
図３のステップＳ０４５において、探索木作成部１０１は、派生させた状態が身体制約及び環境制約を満たすかどうか判断する。身体制約とは、一例として、ロボットの身体の部材に相当する第１及び第２のリンクが、たとえばお互いに衝突しないために設けられる角度の制約である。また、環境制約とは、身体の部材に相当する第１及び第２のリンクが、たとえば外部の障害物に衝突しないように設けられる角度の制約である。次の状態が身体制約及び環境制約を満たせば、ステップＳ０５０に進む。身体制約及び環境制約のいずれかを満たさなければ、ステップＳ０６０に進む。
【００３２】
図３のステップＳ０５０において、探索木作成部１０１は、派生させた状態を枝のノードとして登録する。
【００３３】
図３のステップＳ０６０において、探索木作成部１０１は、探索木作成部１０１が派生させた枝のノードに対応する次の状態を破棄する。
【００３４】
図６は、図３のステップＳ０５０及びステップＳ０６０の動作を説明するための図である。枝のノードは、原則として１個のセル内には１個しか存在しない。探索木作成部１０１は、枝のノード（図６の実線で示した円）がすでに存在するセルにノードが位置する新たな枝を破棄する。すなわち、位相空間上の位置が近い状態は集約される。このように、従来の探索木の枝を、セルを使用して集約することによって、計算コストを大幅に低減することができる。
【００３５】
図３のステップＳ０７０において、探索木作成部１０１は、探索が終了したかどうか判断する。探索木作成部１０１は、探索木の所定の深さまでの探索を終えた場合、あるいは所定時間の推移まで探索を終えた場合に探索が終了したと判断してもよい。探索が終了したと判断された場合には、ステップＳ０８０に進む。探索が終了していないと判断された場合には、ステップＳ０３０に戻り、探索が継続される。
【００３６】
図３のステップＳ０８０において、軌道作成部１０７は、枝のノードが表す状態の中から、目標状態との位相空間上の距離が最も近い状態を選択し、この状態を最良状態とする（最終状態最適化の場合）。あるいは、軌道作成部１０７は、枝のノードが表す状態の中から、目標状態との位相空間上の距離が所定値以下であり、探索木の深さが最も浅い枝に対応する状態を選択し、この状態を最良状態とする（到達時間最適化の場合）。軌道作成部１０７は、最良状態から現在の状態（初期状態）まで、探索木の枝の経路を求めることにより現在の状態から最良状態に至る軌道を求める。このようにして軌道計画が求まる。たとえば、ロボットは、このようにして求めた軌道計画にしたがってアームを制御することにより、アームの最終状態を最適化し、あるいは、目標状態への到達時間を最適化することができる。
【００３７】
上記の実施形態において、探索木作成部１０１は、現在の状態の位置を探索木の根として探索木を作成した。別の実施形態において、探索木作成部１０３が、目標状態を探索木の根として探索木を作成するように構成してもよい。
【００３８】
本発明による軌道計画方法について、本明細書では、説明を簡単にするために、物体を二重倒立振子としている。しかし、本発明の、既知状態登録木を使用する考え方は、任意の物体の状態空間における軌道計画方法に適用することができる。
【００３９】
ここで、セル作成部１０３の動作について説明する。セル作成１０３部は、所定のセルの数に対して、離散化による近似誤差を最小とするように状態空間（位相空間）を分割してセルを作成する。ここで、離散化とはセルによる状態の集約を意味する。
【００４０】
最初に近似誤差について説明する。図３のステップＳ０４０における状態の集約は、位相空間上の点
【数８】
のユークリッド距離
【数９】
が十分に小さい場合、これらの状態に入力ｕを与えてΔｔ後に遷移する先の位相空間上の点
【数１０】
の距離
【数１１】
はやはり近いので、近い状態
【数１２】
からの探索では、ほぼ同じ状態遷移系列が得られるという仮定に基づいている。ここで、Δｔ後の状態は以下の式で表せる。
【数１３】
である。
【００４１】
同一セル内の状態(位相空間上の点)ｘから探索して得られる系列が、セルの中心点ｘμから探索して得られる系列に近似される場合、セルの近似誤差の尺度として同じ入力ｕ（ｔ）に対するΔｔ後の遷移状態
【数１４】
の距離が利用できる。したがって、入力ｕ（ｔ）に対するセルの近似誤差の尺度は、セル内に含まれる任意の状態とセルの中心点
【数１５】
の状態遷移先の距離をセル内領域全体
【数１６】
で平均することにより表すことができる。こうして求めた入力ｕ（ｔ）に対する近似誤差の尺度について、とりうる入力の集合Ｕについて平均をとると、セルの近似誤差は以下の式で表すことができる。
【数１７】
【００４２】
図７は、セルの近似誤差の計算方法を説明するための図である。
【００４３】
式（２）のセルの近似誤差を計算するために、本実施形態においては、セルの代表点について
【数１８】
を計算し、平均をとる方法を採用する。図７に示すように、各軸方向の最大座標の点と最小座標の点をそれぞれ代表点とし、以下の式によりセルの近似誤差を計算する。ここで、Ｎは機械モデルの自由度であり、本実施形態においては、Ｎ＝２である。
【数１９】
【００４４】
つぎに、上記の手順によって求めたセルの近似誤差を使用して、セル作成部１０３によって、位相空間を分割してセルを作成する方法について説明する。
【００４５】
図８は、セル作成部１０３によって、位相空間を分割してセルを作成する方法を示す流れ図である。
【００４６】
図８のステップＳ１０１０において、セル作成部１０３は、状態空間を均一な基本セルに分割する。基本セルの数は、最終的なセルの数よりも小さくする。基本セルの分割粒度は、全ての軸方向で同じ値としてもよい。
【００４７】
図８のステップＳ１０２０において、セル作成部１０３は、全ての基本セルをセルの近似誤差を最小とする方向に分割する。
【００４８】
図９は、図８のステップＳ１０２０の詳細を説明するための流れ図である。
【００４９】
図９のステップＳ１２１０において、セル作成部１０３は、総分割数、すなわち、最終的なセルの数を取得する。
【００５０】
図９のステップＳ１２２０において、セル作成部１０３は、全ての基本セルのリストを作成する。
【００５１】
図９のステップＳ１２３０において、セル作成部１０３は、リストの先頭のセルを各軸方向に仮想的に分割した後のセルの近似誤差を求める。
【００５２】
図９のステップＳ１２４０において、セル作成部１０３は、分割後の近似誤差が最小となる軸方向にセルを分割する。ここで、分割には、文献（Y. Tazaki and J. Imura:”Approximately Bisimilar Discrete Abstractions of Nonlinear Systems Using Variable-resolution Quantizers,”American Control Conference, pp.1015-1020, (2010).）などでも使用されている二分木を基にした分割構造を使用する。この構造は、指定したセルを二つの子セルに分割する細分化の機能を備えている。
【００５３】
図９のステップＳ１２５０において、セル作成部１０３は、分割されたセルをリストの末尾に追加し、先頭のセルをリストから削除する。
【００５４】
図９のステップＳ１２６０において、セル作成部１０３は、セルの数が最終的なセルの数に達したかどうか判断する。セルの数が最終的なセルの数に達していれば処理を終了する。セルの数が最終的なセルの数に達していなければ、ステップＳ１２３０に戻る。
【００５５】
このように、図８のステップＳ１０２０において、基本セルの中で近似誤差が最小となるように分割が行われる。
【００５６】
図８のステップＳ１０３０において、セル作成部１０３はセルの分割と結合を繰り返す。
【００５７】
図１０は、図８のステップＳ１０３０の詳細を説明するための流れ図である。
【００５８】
図１０のステップＳ１４１０において、セル作成部１０３は、分割構造の末端のすべての子セルのリスト（以下において、リストＡと呼称する）及びその一階層上のすべての親セルのリスト（以下において、リストＢと呼称する）を作成する。
【００５９】
図１０のステップＳ１４２０において、セル作成部１０３は、リストＡ及びリストＢの対応するセルについて近似誤差減少量を計算する。近似誤差減少量は以下の式で表せる。
【数２０】
ここで、右辺の第１項は親セルの近似誤差であり、右辺の第２項及び第３項は子セルの近似誤差である。ｉは、ｉ軸方向の分割であることを示す。
【００６０】
図１０のステップＳ１４３０において、セル作成部１０３は、リストＡを近似誤差減少量の降順にソートする。
【００６１】
図１０のステップＳ１４４０において、セル作成部１０３は、リストＢを近似誤差減少量の昇順にソートする。
【００６２】
図１０のステップＳ１４５０において、セル作成部１０３は、リストＡの先頭に位置するセルの近似誤差減少量が、リストＢの先頭に位置するセルの近似誤差減少量よりも大きいどうか判断する。リストＡの先頭に位置するセルの近似誤差減少量が、リストＢの先頭に位置するセルの近似誤差減少量よりも大きければ、ステップＳ１４６０に進む。そうでなければ処理を終了する。
【００６３】
図１０のステップＳ１４６０において、セル作成部１０３は、リストＡの先頭に位置するセルを、分割後のセルの近似誤差が最小となる軸方向に分割し、リストＢの先頭に位置するセルの子セルを結合する。
【００６４】
図１０のステップＳ１４７０において、セル作成部１０３は、新たなセルの近似誤差減少量を計算し、リストＡに追加し、リストＡ及びリストＢを更新する。
【００６５】
このように、図８のステップＳ１０３０において、位相空間（状態空間）全体での近似誤差の平均が小さくなるように分割と結合が繰り返される。ステップＳ１０３０において、セルの総数は変化しない。
【００６６】
図８のステップＳ１０２０においては、基本セルの中で近似誤差が最小となるように分割が行われる。しかし、全基本セルについて、個々の分割数はほぼ一定となっているため、この段階では局所的な不均一分割が生成される。そこで、基本セルの間にまたがり、さらに大域的に平均的な近似誤差を小さくする処理として、分割と結合を繰り返す図８のステップＳ１０３０の処理を行う。ステップＳ１０３０では、セルの重要度を空間全体の体積
【数２１】
に占めるセルの体積
【数２２】
で表し、この重要度で重みづけられた空間全体での近似誤差の総和を最小化するように分割を再構成する。正確には、この処理では近似誤差の総和は局所解へと収束する。
【数２３】
【００６７】
つぎに、本実施形態の軌道計画システムの効果を確認するための、二重倒立振子２００のシミュレーション実験について説明する。二重倒立振子２００の第１リンク２０５のリンク長は、ｌ１であり、質量は、ｍ１であり、重心位置２０３はリンクの中央である。二重倒立振子２００の第２リンク２１１のリンク長は、ｌ２であり、質量は、ｍ２であり、重心位置２０９はリンクの中央である。また、重力加速度をｇとする。
【００６８】
表１は、使用した二重倒立振子２００のパラメータを示す。
【表１】
【００６９】
二重倒立振子２００の運動方程式は、以下のとおりである。
【数２４】
関節角度と重力の関係に対する理解を容易にするために、以下のように座標を変更した。
【数２５】
【００７０】
表２は、本実施形態の軌道計画システムのパラメータを示す表である。
【表２】
【００７１】
シミュレータの刻み時間幅Δｔは５０ミリ秒とし、最大計画運動時間は１０秒とした。運動のコストは運動時間として、各種境界条件のもとで準最短時間運動を求めた。また、距離の関数として以下の式を設定した。
【数２６】
ｗｐｉ及びｗｖｉは各軸ｉの重み定数であり、本シミュレーションでは、各関節ともｗｐｉ及びｗｖｉにはそれぞれ１．０及び０．２を使用した。
【００７２】
本シミュレーションでは、第１リンク２０５及び第２リンク２１１がともに下がって静止した第１の初期状態及びともに倒立して静止した第２の初期状態を設定した。
【００７３】
図１１は、二重倒立振子２００の第１の初期状態を示す図である。二重倒立振子２００の第１の初期状態の座標は以下のとおりである。
【数２７】
【００７４】
図１２は、二重倒立振子２００の第２の初期状態を示す図である。二重倒立振子２００の第２の初期状態の座標は以下のとおりである。
【数２８】
【００７５】
本シミュレーションにおいては、第１及び第２の初期状態から、ランダムサンプリングした目標状態への運動を計画し、計画された運動のコスト、すなわち運動時間を調べた。１０００００個の目標状態からなるサンプルデータを作成し、これらの目標状態までの運動時間の平均を様々な分割条件について比較した。
【００７６】
また、姿勢空間の分割粒度を均一に固定し、速度方向にのみ上述の方法によって不均一に分割を行った。その理由は、障害物が姿勢空間上の拘束条件として表されるためであり、また、距離のパラメータの設定の影響を小さく抑えるためである。基本セルの分割粒度は、すべての軸方向に同じ値を設定した。
【００７７】
図１３は、状態数（最終的なセルの数）を２０４と設定した場合の、一姿勢セル内
【数２９】
における速度空間の断面を示す図である。図１３（ａ）は、各姿勢方向及び速度方向に対して同じ分割粒度を設定して分割を行った場合を示し、図１３（ｂ）は、姿勢空間の分割粒度を均一に固定し、速度方向にのみ上述の方法によって不均一に分割を行った場合を示す。
【００７８】
図１４は、セルの数とシミュレーションによって得られた平均運動時間との関係を示す図である。図１４において、横軸はセルの数（総分割数）を表し、縦軸は平均時間を表す。図１４において、「均一セル」は、各姿勢方向及び速度方向に対して同じ分割粒度を設定して分割を行った場合を示し、「本発明のセル」は、姿勢空間の分割粒度を均一に固定し、速度方向にのみ上述の方法によって不均一に分割を行った場合を示す。状態数（最終的なセルの数）が増えれば近似誤差は小さくなり、より短時間で遷移する運動を発見できることが確認できる。状態数が増えるにつれた、真の最短運動時間の平均値である、ある値に収束していくことが予想される。また、全ての分割条件について、「本発明のセル」の場合は、「均一セル」を使用した場合に比較してより短時間で遷移する運動を発見できることが確認できる。
【００７９】
図１５は、状態数（セルの数）が、１６４、２０４、２４４及び２８４のそれぞれの場合について、３種類の軸方向に均一な分割粒度を設定して分割を行った場合と姿勢空間の分割粒度を均一に固定し、速度方向にのみ上述の方法によって不均一に分割を行った場合の結果を示す図である。図１５において、「均一１」、「均一２」及び「均一３」は、同じ状態数となるように、
【数３０】
方向の分割粒度を均一とし、その比を、それぞれ１：４、１：１及び４：１に設定した場合を示す。また、「本発明」は、姿勢空間の分割粒度を均一に固定し、速度方向にのみ上述の方法によって不均一に分割を行った場合を示す。図１５の縦軸は、平均時間の割合（コスト）を表す。ここで、平均時間の割合（コスト）は、状態数３０４の場合の平均時間の値を最適値として、それぞれの場合の平均時間とこの最適値との差によって表した。縦軸の値が小さいほど平均時間の割合（コスト）が小さい。割合（コスト）は、状態数によって変化する。「均一１」、「均一２」及び「均一３」の割合（コスト）を比較すると、全ての状態数の場合に他よりも割合（コスト）が小さいものは存在しない。これに対して、「本発明」の割合（コスト）は、全ての状態数の場合に、「均一１」、「均一２」及び「均一３」の割合（コスト）よりも小さい。
【００８０】
このように、本発明の軌道計画システム及び軌道計画方法によれば、同じ状態数で、よりコストの小さい運動を精度良く発見することができる。また、本発明の軌道計画システムを含む本発明の軌道計画・制御システムは、本発明の軌道計画システムが作成した軌道をフィードバック制御などによって実現することにより、よりコストの小さい運動を実現することができる。本発明の枠組みは、機械状態を位相空間で扱う汎用的なものであるので、ロボット、自動車、航空機、ロケットの運動計画など様々な分野に広く応用することができる。
【符号の説明】
【００８１】
１０１…探索木作成部、１０３…セル作成部、１０５…セル・。探索木記憶部、１０７…軌道作成部、１２０…軌道制御部
【技術分野】
【０００１】
本発明は、物体の、目標状態に至る状態遷移系列を作成する、軌道計画方法、軌道計画システム及び上記軌道計画システムを備えた軌道計画・制御システムに関する。
【背景技術】
【０００２】
たとえば、ロボットのアームの制御において、軌道計画に基づいてアームのジョイント部のトルクを制御し、アームを目標の状態とする場合に、アームの位置及び角度のみの軌道計画では、ジョイント部のトルクを制御することはできない。ジョイント部のトルクを制御するには、アームの角度及び角速度を含む状態空間（位相空間）の軌道計画を作成する必要がある。
【０００３】
位相空間上での状態遷移を取り扱うための手法として、Rapidly-Exploring Random Trees (RRT) を用いた手法(非特許文献１)がある。しかし、RRTは確率的手法であるため必ずしもコスト最小に近い運動を計画できるとは限らない。また、状態空間の量子化を利用したTazaki の手法(非特許文献２)があるが、この手法は遷移を連続的に扱っておらず、強い線形性の仮定が要求されるため、適用できる機械の種類が限定される。
【０００４】
そこで発明者らによって、連続的な探索により準コスト最小運動を計画できる手法として、位相空間の分割と探索木の枝の集約を用いた運動計画手法が提案されている(非特許文献３)。この手法によれば、連続的な探索により、動力学的な拘束条件のもとで与えられた初期状態から目標状態の集合までの準コスト最小運動を短時間で計画できる。しかし、この手法においては、位相空間の分割の際に、各軸に均一な分割を設定し、設計者が粒度を個別に決定していた。また、運動コストを最小化するための効果的な分割の構成方法は実現されていなかった。
【０００５】
このように、運動コストを最小化するように状態空間を分割し、より精度よく準コスト最小軌道を計画することのできる軌道計画方法、軌道計画システム及び軌道計画・制御システムは開発されていなかった。
【先行技術文献】
【非特許文献】
【０００６】
【非特許文献１】S. M. LaValle and J. J. Kuffner.:”Randomized Kinodynamic Planning,”Journal of Robotics Research, Vol. 20, No. 5, pp.378-400,(2001).
【非特許文献２】Y. Tazaki and J. Imura:”Approximately Bisimilar Discrete Abstractions of Nonlinear Systems Using Variable-resolution Quantizers,”American Control Conference, pp.1015-1020, (2010).
【非特許文献３】C. H. Kim, H. Tsujino and S. Sugano: ”Rapid Short-Time Path Planning for Phase Space,” Journal of Robotics and Mechatronics, Vol. 23, No. 2, (2011).
【発明の概要】
【発明が解決しようとする課題】
【０００７】
したがって、運動コストを最小化するように状態空間を分割し、より精度よく準コスト最小軌道を計画することのできる軌道計画方法、軌道計画システム及び軌道計画・制御システムに対するニーズがある。
【課題を解決するための手段】
【０００８】
本発明による軌道計画方法は、軌道計画システムによって、物体の状態を目標状態へ制御するための軌道を求める軌道計画方法であって、該物体の状態空間をセルに分割するセル作成部が、所定のセルの数に対して、離散化による近似誤差を最小化するように該状態空間をセルに分割するステップと、探索木作成部が、該状態空間において、該物体の状態遷移に対応する探索木を、該物体の状態に対応する、該探索木の枝のノードがセルに一つずつ含まれるように作成するステップと、軌道作成部が、該探索木を使用して、該物体の現在の状態から目標状態までの経路を定めるステップと、を含む。
【０００９】
本発明による軌道計画方法においては、所定のセルの数に対して、離散化による近似誤差を最小化するように状態空間をセルに分割するので、運動コストが最小化され、より精度よく準コスト最小軌道を計画することができる。
【００１０】
本発明の一実施形態による軌道計画方法においては、前記分割するステップにおいて、セルの近似誤差を、セル内に含まれる任意の点とセルの中心点に入力を与えた場合の状態遷移先の点の距離の平均として表し、前記状態空間のセルの近似誤差の和が最小となるようにすることによって前記離散化による近似誤差を最小とする。
【００１１】
本実施形態によれば、簡単な手順によって離散化による近似誤差を最小とすることができる。
【００１２】
本発明の一実施形態による軌道計画方法においては、前記分割するステップが、前記状態空間を均一な基本セルに分割するステップと、前記所定のセルの数に達するまで、基本セルを、セルの近似誤差を最も小さくする軸の方向に分割するステップと、セルの近似誤差の和を小さくするようにセルの分割と結合を繰り返すステップとを含む。
【００１３】
本実施形態によれば、基本セルの中で近似誤差が最小となるように分割を実施し、その後、基本セルの間にまたがり、さらに大域的に平均的な近似誤差を小さくするように、分割と結合を繰り返すので、離散化による近似誤差を効率的に最小化することができる。
【００１４】
本発明による軌道計画システムは、物体の状態を目標状態へ制御するための軌道を求める軌道計画システムであって、離散化による近似誤差を最小化するように、該物体の状態空間をセルに分割するセル作成部と、該状態空間において、該物体の状態遷移に対応する探索木を、該物体の状態に対応する、該探索木の枝のノードがセルに一つずつ含まれるように作成する探索木作成部と、該探索木を使用して、該物体の現在の状態から目標状態までの軌道を定める軌道作成部と、を備える。
【００１５】
本発明による軌道計画システムは、所定のセルの数に対して、離散化による近似誤差を最小化するように状態空間をセルに分割するので、運動コストが最小化され、より精度よく準コスト最小軌道を計画することができる。
【００１６】
本発明による軌道計画・制御システムは、本発明による軌道計画システムと、該軌道計画システムが定めた軌道を実現する軌道制御部とを備えている。
【００１７】
本発明による軌道計画・制御システムは、本発明による軌道計画システムによってより精度よく計画された準コスト最小軌道を実現することができる。
【図面の簡単な説明】
【００１８】
【図１】本発明の一実施形態による軌道計画システムの構成を示す図である。
【図２】二重倒立振子の構成を示す図である。
【図３】本実施形態による軌道計画法を説明するための流れ図である。
【図４】分割された位相空間の例を示す図である。
【図５】図３のステップＳ０２０及びステップＳ０３０の動作を説明するための図である。
【図６】図３のステップＳ０５０及びステップＳ０６０の動作を説明するための図である。
【図７】セルの近似誤差の計算方法を説明するための図である。
【図８】セル作成部によって、位相空間を分割してセルを作成する方法を示す流れ図である。
【図９】図８のステップＳ１０２０の詳細を説明するための流れ図である。
【図１０】図８のステップＳ１０３０の詳細を説明するための流れ図である。
【図１１】二重倒立振子の第１の初期状態を示す図である。
【図１２】二重倒立振子の第２の初期状態を示す図である。
【図１３】状態数（最終的なセルの数）を２０４と設定した場合に、均一分割及び不均一分割の、位置姿勢セル内
【数１】
における速度空間の断面を示す図である。
【図１４】セルの数とシミュレーションによって得られた平均運動時間との関係を示す図である。
【図１５】状態数が、１６４、２０４、２４４及び２８４のそれぞれの場合について、３種類の均一分割を使用した場合と図８乃至図１０にしたがって分割を行った場合の結果を示す図である。
【発明を実施するための形態】
【００１９】
図１は、本発明の一実施形態による軌道計画システム１００の構成を示す図である。軌道計画システム１００は、ロボットのアームなどの物体の状態を目標状態とするための、状態空間における軌道を求めるシステムである。軌道計画システム１００は、状態空間上の物体の位置、すなわち状態に対応する枝を備えた探索木を作成する探索木作成部１０１と、状態空間を分割して複数のセルを作成するセル作成部１０３と、セル及び探索木を記憶するセル・探索木記憶部１０５と、探索木から、物体の状態空間における軌道を作成する軌道作成部１０７と、を備える。さらに、作成された軌道にしたがって物体の動きを制御する軌道制御部１２０を追加すれば、軌道計画・制御システムが得られる。各構成部の機能の詳細については、後で説明する。
【００２０】
図２は、二重倒立振子２００の構成を示す図である。以下の実施形態においては、物体を２次元平面内で運動する二重倒立振子２００として説明を行なう。二重倒立振子２００は、重心２０３を有する第１リンク２０５と、重心２０９を有する第２リンク２１１と、第１リンク２０５を回転可能に固定点に接続する第１関節（ジョイント）２０１と、第１リンク２０５及び第２リンク２１１を回転可能に接続する第２関節２０７と、から構成される。
【００２１】
二重倒立振子２００の運動は、第１関節２０１及び第２関節２０７に与えるトルクによって定まり、第１リンク２０５のＸ軸に対する角度
【数２】
第２リンク２１１の第１リンク２０５に対する角度
【数３】
及びこれらの角速度
【数４】
によって表現される。そこで、本実施形態において、状態空間として以下の位相空間を採用する。
【数５】
【００２２】
図３は、本実施形態による軌道計画方法を説明するための流れ図である。
【００２３】
図３のステップＳ００５において、セル作成部１０３が、位相空間を分割し複数のセルを作成しておく。セル作成部の動作については後で詳細に説明する。
【００２４】
図４は、分割された位相空間の例を示す図である。図4は、簡単のために位相空間を２次元空間として表している。また、図４においては、図を簡略にするため、想定される角度及び角速度の範囲を等分割したセルが示されている。位相空間の分割方法については後で詳細に説明する。
【００２５】
図３のステップＳ０１０において、探索木作成部１０１が、二重倒立振子２００の現在の状態及び目標状態を得る。ここで、状態は、位相空間上の位置に対応する。
【００２６】
図３のステップＳ０２０において、探索木作成部１０１が、位相空間において、現在の状態の位置を探索木の根として定める。
【００２７】
図３のステップＳ０３０において、探索木作成部１０１が、位相空間において、所定の時間間隔後の次の状態の位置を、探索木の枝のノードとして定める。所定の時間間隔後の次の状態の位置は、運動方程式や動力学シミュレータによって定める。上述のように、二重倒立振子２００の運動は、第１関節２０１及び第２関節２０７に与えるトルクによって定まるので、所定の時間間隔に第１関節２０１及び第２関節２０７に与えるトルクを定めれば、二重倒立振子２００の、所定の時間間隔後の次の状態が定まる。そこで、所定の時間間隔に第１関節２０１及び第２関節２０７に与えるトルクを最大値と最小値との間で、所定の増分で変化させながら、それぞれの場合に対応する次の状態を求めてもよい。この場合に、トルクを最大値と最小値との間で、所定の増分で変化させながら、幅優先探索により網羅的に探索を行なってもよい。
【００２８】
図５は、図３のステップＳ０２０及びステップＳ０３０の動作を説明するための図である。なお、図５は、簡単のために位相空間を２次元空間として表している。現在の状態にある、二重倒立振子２００に所定の時間間隔に３個の異なるトルク
【数６】
を与えることにより、３個の異なる次の状態が定まる。探索木作成部１０１は、これらの状態を探索木の枝のノードとする。さらに、生成された枝のノードが示すそれぞれの状態に対して、３個の異なるトルク
【数７】
を与えることにより、３個の異なる、さらに次の状態が定まる。
【００２９】
図３のステップＳ０４０において、探索木作成部１０１は、探索木の枝の集約を行う。探索木作成部１０１が、探索木を作成する場合に、上述のように、根または枝から派生させる枝の数をＮとし、木の深さをＭとすると、木の深さＭのレベルで探索の対象となる枝の数はＮＭとなる。そこで、探索木作成部１０１は、上述のセルを利用して、派生させた枝を集約する。具体的に探索木作成部１０１は、図３のステップＳ０４０において、以下の処理を行う。
【００３０】
探索木作成部１０１は、派生させた次の状態が位相空間のどのセルに属するか定める。つぎに、探索木作成部１０１は、そのセルに属する状態が、枝のノードとしてすでに登録されているかどうか判断する。登録されていなければ、ステップＳ０４５に進む。登録されていれば、ステップＳ０６０に進む。
【００３１】
図３のステップＳ０４５において、探索木作成部１０１は、派生させた状態が身体制約及び環境制約を満たすかどうか判断する。身体制約とは、一例として、ロボットの身体の部材に相当する第１及び第２のリンクが、たとえばお互いに衝突しないために設けられる角度の制約である。また、環境制約とは、身体の部材に相当する第１及び第２のリンクが、たとえば外部の障害物に衝突しないように設けられる角度の制約である。次の状態が身体制約及び環境制約を満たせば、ステップＳ０５０に進む。身体制約及び環境制約のいずれかを満たさなければ、ステップＳ０６０に進む。
【００３２】
図３のステップＳ０５０において、探索木作成部１０１は、派生させた状態を枝のノードとして登録する。
【００３３】
図３のステップＳ０６０において、探索木作成部１０１は、探索木作成部１０１が派生させた枝のノードに対応する次の状態を破棄する。
【００３４】
図６は、図３のステップＳ０５０及びステップＳ０６０の動作を説明するための図である。枝のノードは、原則として１個のセル内には１個しか存在しない。探索木作成部１０１は、枝のノード（図６の実線で示した円）がすでに存在するセルにノードが位置する新たな枝を破棄する。すなわち、位相空間上の位置が近い状態は集約される。このように、従来の探索木の枝を、セルを使用して集約することによって、計算コストを大幅に低減することができる。
【００３５】
図３のステップＳ０７０において、探索木作成部１０１は、探索が終了したかどうか判断する。探索木作成部１０１は、探索木の所定の深さまでの探索を終えた場合、あるいは所定時間の推移まで探索を終えた場合に探索が終了したと判断してもよい。探索が終了したと判断された場合には、ステップＳ０８０に進む。探索が終了していないと判断された場合には、ステップＳ０３０に戻り、探索が継続される。
【００３６】
図３のステップＳ０８０において、軌道作成部１０７は、枝のノードが表す状態の中から、目標状態との位相空間上の距離が最も近い状態を選択し、この状態を最良状態とする（最終状態最適化の場合）。あるいは、軌道作成部１０７は、枝のノードが表す状態の中から、目標状態との位相空間上の距離が所定値以下であり、探索木の深さが最も浅い枝に対応する状態を選択し、この状態を最良状態とする（到達時間最適化の場合）。軌道作成部１０７は、最良状態から現在の状態（初期状態）まで、探索木の枝の経路を求めることにより現在の状態から最良状態に至る軌道を求める。このようにして軌道計画が求まる。たとえば、ロボットは、このようにして求めた軌道計画にしたがってアームを制御することにより、アームの最終状態を最適化し、あるいは、目標状態への到達時間を最適化することができる。
【００３７】
上記の実施形態において、探索木作成部１０１は、現在の状態の位置を探索木の根として探索木を作成した。別の実施形態において、探索木作成部１０３が、目標状態を探索木の根として探索木を作成するように構成してもよい。
【００３８】
本発明による軌道計画方法について、本明細書では、説明を簡単にするために、物体を二重倒立振子としている。しかし、本発明の、既知状態登録木を使用する考え方は、任意の物体の状態空間における軌道計画方法に適用することができる。
【００３９】
ここで、セル作成部１０３の動作について説明する。セル作成１０３部は、所定のセルの数に対して、離散化による近似誤差を最小とするように状態空間（位相空間）を分割してセルを作成する。ここで、離散化とはセルによる状態の集約を意味する。
【００４０】
最初に近似誤差について説明する。図３のステップＳ０４０における状態の集約は、位相空間上の点
【数８】
のユークリッド距離
【数９】
が十分に小さい場合、これらの状態に入力ｕを与えてΔｔ後に遷移する先の位相空間上の点
【数１０】
の距離
【数１１】
はやはり近いので、近い状態
【数１２】
からの探索では、ほぼ同じ状態遷移系列が得られるという仮定に基づいている。ここで、Δｔ後の状態は以下の式で表せる。
【数１３】
である。
【００４１】
同一セル内の状態(位相空間上の点)ｘから探索して得られる系列が、セルの中心点ｘμから探索して得られる系列に近似される場合、セルの近似誤差の尺度として同じ入力ｕ（ｔ）に対するΔｔ後の遷移状態
【数１４】
の距離が利用できる。したがって、入力ｕ（ｔ）に対するセルの近似誤差の尺度は、セル内に含まれる任意の状態とセルの中心点
【数１５】
の状態遷移先の距離をセル内領域全体
【数１６】
で平均することにより表すことができる。こうして求めた入力ｕ（ｔ）に対する近似誤差の尺度について、とりうる入力の集合Ｕについて平均をとると、セルの近似誤差は以下の式で表すことができる。
【数１７】
【００４２】
図７は、セルの近似誤差の計算方法を説明するための図である。
【００４３】
式（２）のセルの近似誤差を計算するために、本実施形態においては、セルの代表点について
【数１８】
を計算し、平均をとる方法を採用する。図７に示すように、各軸方向の最大座標の点と最小座標の点をそれぞれ代表点とし、以下の式によりセルの近似誤差を計算する。ここで、Ｎは機械モデルの自由度であり、本実施形態においては、Ｎ＝２である。
【数１９】
【００４４】
つぎに、上記の手順によって求めたセルの近似誤差を使用して、セル作成部１０３によって、位相空間を分割してセルを作成する方法について説明する。
【００４５】
図８は、セル作成部１０３によって、位相空間を分割してセルを作成する方法を示す流れ図である。
【００４６】
図８のステップＳ１０１０において、セル作成部１０３は、状態空間を均一な基本セルに分割する。基本セルの数は、最終的なセルの数よりも小さくする。基本セルの分割粒度は、全ての軸方向で同じ値としてもよい。
【００４７】
図８のステップＳ１０２０において、セル作成部１０３は、全ての基本セルをセルの近似誤差を最小とする方向に分割する。
【００４８】
図９は、図８のステップＳ１０２０の詳細を説明するための流れ図である。
【００４９】
図９のステップＳ１２１０において、セル作成部１０３は、総分割数、すなわち、最終的なセルの数を取得する。
【００５０】
図９のステップＳ１２２０において、セル作成部１０３は、全ての基本セルのリストを作成する。
【００５１】
図９のステップＳ１２３０において、セル作成部１０３は、リストの先頭のセルを各軸方向に仮想的に分割した後のセルの近似誤差を求める。
【００５２】
図９のステップＳ１２４０において、セル作成部１０３は、分割後の近似誤差が最小となる軸方向にセルを分割する。ここで、分割には、文献（Y. Tazaki and J. Imura:”Approximately Bisimilar Discrete Abstractions of Nonlinear Systems Using Variable-resolution Quantizers,”American Control Conference, pp.1015-1020, (2010).）などでも使用されている二分木を基にした分割構造を使用する。この構造は、指定したセルを二つの子セルに分割する細分化の機能を備えている。
【００５３】
図９のステップＳ１２５０において、セル作成部１０３は、分割されたセルをリストの末尾に追加し、先頭のセルをリストから削除する。
【００５４】
図９のステップＳ１２６０において、セル作成部１０３は、セルの数が最終的なセルの数に達したかどうか判断する。セルの数が最終的なセルの数に達していれば処理を終了する。セルの数が最終的なセルの数に達していなければ、ステップＳ１２３０に戻る。
【００５５】
このように、図８のステップＳ１０２０において、基本セルの中で近似誤差が最小となるように分割が行われる。
【００５６】
図８のステップＳ１０３０において、セル作成部１０３はセルの分割と結合を繰り返す。
【００５７】
図１０は、図８のステップＳ１０３０の詳細を説明するための流れ図である。
【００５８】
図１０のステップＳ１４１０において、セル作成部１０３は、分割構造の末端のすべての子セルのリスト（以下において、リストＡと呼称する）及びその一階層上のすべての親セルのリスト（以下において、リストＢと呼称する）を作成する。
【００５９】
図１０のステップＳ１４２０において、セル作成部１０３は、リストＡ及びリストＢの対応するセルについて近似誤差減少量を計算する。近似誤差減少量は以下の式で表せる。
【数２０】
ここで、右辺の第１項は親セルの近似誤差であり、右辺の第２項及び第３項は子セルの近似誤差である。ｉは、ｉ軸方向の分割であることを示す。
【００６０】
図１０のステップＳ１４３０において、セル作成部１０３は、リストＡを近似誤差減少量の降順にソートする。
【００６１】
図１０のステップＳ１４４０において、セル作成部１０３は、リストＢを近似誤差減少量の昇順にソートする。
【００６２】
図１０のステップＳ１４５０において、セル作成部１０３は、リストＡの先頭に位置するセルの近似誤差減少量が、リストＢの先頭に位置するセルの近似誤差減少量よりも大きいどうか判断する。リストＡの先頭に位置するセルの近似誤差減少量が、リストＢの先頭に位置するセルの近似誤差減少量よりも大きければ、ステップＳ１４６０に進む。そうでなければ処理を終了する。
【００６３】
図１０のステップＳ１４６０において、セル作成部１０３は、リストＡの先頭に位置するセルを、分割後のセルの近似誤差が最小となる軸方向に分割し、リストＢの先頭に位置するセルの子セルを結合する。
【００６４】
図１０のステップＳ１４７０において、セル作成部１０３は、新たなセルの近似誤差減少量を計算し、リストＡに追加し、リストＡ及びリストＢを更新する。
【００６５】
このように、図８のステップＳ１０３０において、位相空間（状態空間）全体での近似誤差の平均が小さくなるように分割と結合が繰り返される。ステップＳ１０３０において、セルの総数は変化しない。
【００６６】
図８のステップＳ１０２０においては、基本セルの中で近似誤差が最小となるように分割が行われる。しかし、全基本セルについて、個々の分割数はほぼ一定となっているため、この段階では局所的な不均一分割が生成される。そこで、基本セルの間にまたがり、さらに大域的に平均的な近似誤差を小さくする処理として、分割と結合を繰り返す図８のステップＳ１０３０の処理を行う。ステップＳ１０３０では、セルの重要度を空間全体の体積
【数２１】
に占めるセルの体積
【数２２】
で表し、この重要度で重みづけられた空間全体での近似誤差の総和を最小化するように分割を再構成する。正確には、この処理では近似誤差の総和は局所解へと収束する。
【数２３】
【００６７】
つぎに、本実施形態の軌道計画システムの効果を確認するための、二重倒立振子２００のシミュレーション実験について説明する。二重倒立振子２００の第１リンク２０５のリンク長は、ｌ１であり、質量は、ｍ１であり、重心位置２０３はリンクの中央である。二重倒立振子２００の第２リンク２１１のリンク長は、ｌ２であり、質量は、ｍ２であり、重心位置２０９はリンクの中央である。また、重力加速度をｇとする。
【００６８】
表１は、使用した二重倒立振子２００のパラメータを示す。
【表１】
【００６９】
二重倒立振子２００の運動方程式は、以下のとおりである。
【数２４】
関節角度と重力の関係に対する理解を容易にするために、以下のように座標を変更した。
【数２５】
【００７０】
表２は、本実施形態の軌道計画システムのパラメータを示す表である。
【表２】
【００７１】
シミュレータの刻み時間幅Δｔは５０ミリ秒とし、最大計画運動時間は１０秒とした。運動のコストは運動時間として、各種境界条件のもとで準最短時間運動を求めた。また、距離の関数として以下の式を設定した。
【数２６】
ｗｐｉ及びｗｖｉは各軸ｉの重み定数であり、本シミュレーションでは、各関節ともｗｐｉ及びｗｖｉにはそれぞれ１．０及び０．２を使用した。
【００７２】
本シミュレーションでは、第１リンク２０５及び第２リンク２１１がともに下がって静止した第１の初期状態及びともに倒立して静止した第２の初期状態を設定した。
【００７３】
図１１は、二重倒立振子２００の第１の初期状態を示す図である。二重倒立振子２００の第１の初期状態の座標は以下のとおりである。
【数２７】
【００７４】
図１２は、二重倒立振子２００の第２の初期状態を示す図である。二重倒立振子２００の第２の初期状態の座標は以下のとおりである。
【数２８】
【００７５】
本シミュレーションにおいては、第１及び第２の初期状態から、ランダムサンプリングした目標状態への運動を計画し、計画された運動のコスト、すなわち運動時間を調べた。１０００００個の目標状態からなるサンプルデータを作成し、これらの目標状態までの運動時間の平均を様々な分割条件について比較した。
【００７６】
また、姿勢空間の分割粒度を均一に固定し、速度方向にのみ上述の方法によって不均一に分割を行った。その理由は、障害物が姿勢空間上の拘束条件として表されるためであり、また、距離のパラメータの設定の影響を小さく抑えるためである。基本セルの分割粒度は、すべての軸方向に同じ値を設定した。
【００７７】
図１３は、状態数（最終的なセルの数）を２０４と設定した場合の、一姿勢セル内
【数２９】
における速度空間の断面を示す図である。図１３（ａ）は、各姿勢方向及び速度方向に対して同じ分割粒度を設定して分割を行った場合を示し、図１３（ｂ）は、姿勢空間の分割粒度を均一に固定し、速度方向にのみ上述の方法によって不均一に分割を行った場合を示す。
【００７８】
図１４は、セルの数とシミュレーションによって得られた平均運動時間との関係を示す図である。図１４において、横軸はセルの数（総分割数）を表し、縦軸は平均時間を表す。図１４において、「均一セル」は、各姿勢方向及び速度方向に対して同じ分割粒度を設定して分割を行った場合を示し、「本発明のセル」は、姿勢空間の分割粒度を均一に固定し、速度方向にのみ上述の方法によって不均一に分割を行った場合を示す。状態数（最終的なセルの数）が増えれば近似誤差は小さくなり、より短時間で遷移する運動を発見できることが確認できる。状態数が増えるにつれた、真の最短運動時間の平均値である、ある値に収束していくことが予想される。また、全ての分割条件について、「本発明のセル」の場合は、「均一セル」を使用した場合に比較してより短時間で遷移する運動を発見できることが確認できる。
【００７９】
図１５は、状態数（セルの数）が、１６４、２０４、２４４及び２８４のそれぞれの場合について、３種類の軸方向に均一な分割粒度を設定して分割を行った場合と姿勢空間の分割粒度を均一に固定し、速度方向にのみ上述の方法によって不均一に分割を行った場合の結果を示す図である。図１５において、「均一１」、「均一２」及び「均一３」は、同じ状態数となるように、
【数３０】
方向の分割粒度を均一とし、その比を、それぞれ１：４、１：１及び４：１に設定した場合を示す。また、「本発明」は、姿勢空間の分割粒度を均一に固定し、速度方向にのみ上述の方法によって不均一に分割を行った場合を示す。図１５の縦軸は、平均時間の割合（コスト）を表す。ここで、平均時間の割合（コスト）は、状態数３０４の場合の平均時間の値を最適値として、それぞれの場合の平均時間とこの最適値との差によって表した。縦軸の値が小さいほど平均時間の割合（コスト）が小さい。割合（コスト）は、状態数によって変化する。「均一１」、「均一２」及び「均一３」の割合（コスト）を比較すると、全ての状態数の場合に他よりも割合（コスト）が小さいものは存在しない。これに対して、「本発明」の割合（コスト）は、全ての状態数の場合に、「均一１」、「均一２」及び「均一３」の割合（コスト）よりも小さい。
【００８０】
このように、本発明の軌道計画システム及び軌道計画方法によれば、同じ状態数で、よりコストの小さい運動を精度良く発見することができる。また、本発明の軌道計画システムを含む本発明の軌道計画・制御システムは、本発明の軌道計画システムが作成した軌道をフィードバック制御などによって実現することにより、よりコストの小さい運動を実現することができる。本発明の枠組みは、機械状態を位相空間で扱う汎用的なものであるので、ロボット、自動車、航空機、ロケットの運動計画など様々な分野に広く応用することができる。
【符号の説明】
【００８１】
１０１…探索木作成部、１０３…セル作成部、１０５…セル・。探索木記憶部、１０７…軌道作成部、１２０…軌道制御部
【特許請求の範囲】
【請求項１】
軌道計画システムによって、物体の状態を目標状態へ制御するための軌道を求める軌道計画方法であって、
該物体の状態空間をセルに分割するセル作成部が、所定のセルの数に対して、離散化による近似誤差を最小化するように該状態空間をセルに分割するステップと、
探索木作成部が、該状態空間において、該物体の状態遷移に対応する探索木を、該物体の状態に対応する、該探索木の枝のノードがセルに一つずつ含まれるように作成するステップと、
軌道作成部が、該探索木を使用して、該物体の現在の状態から目標状態までの経路を定めるステップと、を含む、軌道計画方法。
【請求項２】
前記分割するステップにおいて、セルの近似誤差を、セル内に含まれる任意の点とセルの中心点に、入力を与えた場合の状態遷移先の距離の平均として表し、前記状態空間のセルの近似誤差の和が最小となるようにすることによって前記離散化による近似誤差を最小とする請求項１に記載の軌道計画方法。
【請求項３】
前記分割するステップが、前記状態空間を均一な基本セルに分割するステップと、前記所定のセルの数に達するまで、全ての基本セルを、セルの近似誤差を最も小さくする軸の方向に分割するステップと、セルの近似誤差の和を小さくするようにセルの分割と結合を繰り返すステップとを含む請求項１または２に記載の軌道計画方法。
【請求項４】
物体の状態を目標状態へ制御するための軌道を求める軌道計画システムであって、
離散化による近似誤差を最小化するように、該物体の状態空間をセルに分割するセル作成部と、
該状態空間において、該物体の状態遷移に対応する探索木を、該物体の状態に対応する、該探索木の枝のノードがセルに一つずつ含まれるように作成する探索木作成部と、
該探索木を使用して、該物体の現在の状態から目標状態までの軌道を定める軌道作成部と、を備える軌道計画システム。
【請求項５】
請求項４に記載の軌道計画システムと、該軌道計画システムが定めた軌道を実現する軌道制御部とを備えた軌道計画・制御システム。
【請求項１】
軌道計画システムによって、物体の状態を目標状態へ制御するための軌道を求める軌道計画方法であって、
該物体の状態空間をセルに分割するセル作成部が、所定のセルの数に対して、離散化による近似誤差を最小化するように該状態空間をセルに分割するステップと、
探索木作成部が、該状態空間において、該物体の状態遷移に対応する探索木を、該物体の状態に対応する、該探索木の枝のノードがセルに一つずつ含まれるように作成するステップと、
軌道作成部が、該探索木を使用して、該物体の現在の状態から目標状態までの経路を定めるステップと、を含む、軌道計画方法。
【請求項２】
前記分割するステップにおいて、セルの近似誤差を、セル内に含まれる任意の点とセルの中心点に、入力を与えた場合の状態遷移先の距離の平均として表し、前記状態空間のセルの近似誤差の和が最小となるようにすることによって前記離散化による近似誤差を最小とする請求項１に記載の軌道計画方法。
【請求項３】
前記分割するステップが、前記状態空間を均一な基本セルに分割するステップと、前記所定のセルの数に達するまで、全ての基本セルを、セルの近似誤差を最も小さくする軸の方向に分割するステップと、セルの近似誤差の和を小さくするようにセルの分割と結合を繰り返すステップとを含む請求項１または２に記載の軌道計画方法。
【請求項４】
物体の状態を目標状態へ制御するための軌道を求める軌道計画システムであって、
離散化による近似誤差を最小化するように、該物体の状態空間をセルに分割するセル作成部と、
該状態空間において、該物体の状態遷移に対応する探索木を、該物体の状態に対応する、該探索木の枝のノードがセルに一つずつ含まれるように作成する探索木作成部と、
該探索木を使用して、該物体の現在の状態から目標状態までの軌道を定める軌道作成部と、を備える軌道計画システム。
【請求項５】
請求項４に記載の軌道計画システムと、該軌道計画システムが定めた軌道を実現する軌道制御部とを備えた軌道計画・制御システム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【公開番号】特開２０１２−２３４２６２（Ｐ２０１２−２３４２６２Ａ）
【公開日】平成２４年１１月２９日（２０１２．１１．２９）
【国際特許分類】
【出願番号】特願２０１１−１００９１９（Ｐ２０１１−１００９１９）
【出願日】平成２３年４月２８日（２０１１．４．２８）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【出願人】（８９９００００６８）学校法人早稲田大学 (602)
【Ｆターム（参考）】
【公開日】平成２４年１１月２９日（２０１２．１１．２９）
【国際特許分類】
【出願日】平成２３年４月２８日（２０１１．４．２８）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【出願人】（８９９００００６８）学校法人早稲田大学 (602)
【Ｆターム（参考）】
[ Back to top ]