ロボットの動作生成システム及び動作生成方法
【課題】達成させたい作業の動作を一度で確実に生成可能とし、短時間で動作生成を実行可能とする。また、軌道の計画可能性を保証しつつ、簡単な環境では短時間で軌道計画を可能とする。
【解決手段】複数のアームを備えるロボットの動作生成システムであり、アームの関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するグラフ構造をアームごとにグラフ構造データ格納部１５１に格納し、グラフ構造探索処理部１５４が、各アームに設定された優先度に従って、アームごとのグラフ構造を用いて、スタートノードからゴールノードまでを結ぶ経路を探索し、ノード有効性判定部１５５が、探索された経路に含まれるノード環境情報と干渉するか否かを判定し、干渉していると判定した場合には、当該ノードをグラフ構造から除外する。
【解決手段】複数のアームを備えるロボットの動作生成システムであり、アームの関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するグラフ構造をアームごとにグラフ構造データ格納部１５１に格納し、グラフ構造探索処理部１５４が、各アームに設定された優先度に従って、アームごとのグラフ構造を用いて、スタートノードからゴールノードまでを結ぶ経路を探索し、ノード有効性判定部１５５が、探索された経路に含まれるノード環境情報と干渉するか否かを判定し、干渉していると判定した場合には、当該ノードをグラフ構造から除外する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの動作生成システム及び動作生成方法に関する。
【背景技術】
【０００２】
近年、様々に変化する環境下においても、環境情報を反映した動作をロボットが自律的に生成可能とする技術が開発されている。ロボットの動作生成は、ロボットが備える各関節について、関節軸情報の系列である関節軌道（以下、単に軌道と称する場合がある。）が計画され、計画された軌道に基づいて関節が駆動されることで実現される。
【０００３】
ロボットがアームを備えるロボットである場合には、ロボットアームの軌道計画とは、ロボット自身と環境とが干渉しないように、ロボットアームのエンドエフェクタの位置姿勢が、初期姿勢から最終姿勢へと到達するような関節軸情報の系列を求める問題である。
【０００４】
この問題の解法の一つとして、ロボットの関節軸情報であるノードを複数有したグラフ構造を使用する手法が知られている。当該グラフ構造では、ノード間がエッジにより接続され、初期姿勢であるスタートノードから最終姿勢であるゴールノードへと至るノードの系列である経路が探索される。このようなグラフ構造を使用して軌道計画を行う技術が、例えば、特許文献１、非特許文献１乃至４に開示されている。
【０００５】
非特許文献１には、アーム毎のグラフ構造による動作生成手法が開示されている。また、非特許文献２には、１本のアームに適用したグラフ構造を用いて動作計画を行う手法が開示されている。非特許文献３には、ロボットの部位（例えば、右腕、左腕、腰など）ごとのグラフ構造を予め用意しておき、軌道計画実行時に、これら部位ごとのグラフ構造を組み合わせて、全身の動作生成を行う手法が開示されている。
【０００６】
特許文献１及び非特許文献４には、グラフ構造を変化させる、或いは、規模の異なる複数のグラフ構造を利用することで、動作生成を行う手法が開示されている。特許文献１に開示された経路作成装置は、ノードの個数を設定して、その設定された個数分のノードの分布が密集している場合に、ノード間を接続する線分長を短くすることで、障害物を回避できるようグラフ構造を複雑にして経路の作成を行う。また、非特許文献４には、規模の異なる複数のグラフ構造を用いた動作生成手法が開示されている。
【０００７】
なお、本発明に関連する他の技術として、特許文献２には、複数のロボットを動作させるためのティーチングデータの作成において、各ロボットに優先順位を設定し、優先順位に基づいて相互干渉確認を行うことで、干渉の無いロボットの動作を生成する技術が開示されている。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特開２００８−２０４１６１号公報
【特許文献２】特開２００４−３５８６３０号公報
【非特許文献】
【０００９】
【特許文献１】"Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces", L.Kavraki, P.Svestka, J.Latombe, M.Overmars, IEEE transactions on robotics and automation, Vol.12, No.4, Aug.1996.
【特許文献２】"Motion Planning Using Dynamic Roadmaps", Marcelo Kallmann and Maja Mataric, Proceedings of the IEEE International Conference on Robotics and Automation (ICRA2004), April, 2004.
【特許文献３】"Roadmap composition for multi-arm systems path planning", M. Gharbi, J. Cortes,T. Simeon, 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems, Saint Louis, MO, USA (October 2009).
【特許文献４】"Sampling-Based Roadmap of Trees for Parallel Motion Planning", Erion Plaku, Kostas E.Bekris, Brian Y.Chen, Andrew M.Ladd, Lydia E.Kavraki, IEEE transactions on robotics and automation, Vol.21, No.4, 2005.
【発明の概要】
【発明が解決しようとする課題】
【００１０】
上記した従来技術のように、グラフ構造を用いた軌道計画手法によれば、複雑かつ様々に変化する環境下においても軌道を算出することが可能となる。しかし、軌道計画の対象となるロボットが複数本のアームを備えるものである場合には、以下の問題がある。
【００１１】
まず第１の問題を説明する。例えば双腕ロボットがグラフ構造を用いて軌道計画を行う際には、２本のアームの各関節の全ての関節軸情報を１つのノードに持たせた上で、このようなノードが複数登録された１つのグラフ構造を利用して、初期姿勢から最終姿勢へと至る軌道を求めるという手法が考えられる。
【００１２】
しかしながら、ロボットの関節軸空間はｎ次元（ｎは関節軸の個数）となることから、これと同じ密度の関節軸情報を１つのノードに持たせて探索した場合の計算時間は、ｎ乗に比例することになる。双腕ロボットは通常１０次元以上の関節軸空間となるために、軌道を発見するまでには、多くの場合に多大な計算時間を必要とすることになる。
【００１３】
一方で、非特許文献３に開示される技術のように、完全に分離可能なロボットの部位ごとに、それら部位内の関節軸情報を１つのノードに持たせた上で、このようなノードが複数登録された複数のグラフ構造を利用して軌道を求めるという手法も考えられる。
【００１４】
部位ごとに別々のグラフ構造を用意し、これら複数のグラフ構造を用いて軌道計画を行うことで、探索空間の大きな１つのグラフ構造で探索を行う場合と比べて、探索空間のより小さな個々のグラフ構造で探索を行うことができ、軌道計画に要する探索時間を大幅に削減することが可能となる。
【００１５】
しかしながら、ロボットに達成させたい作業（タスク）には複雑な作業が多く存在する。このため、単純に部位ごとのグラフ構造を用いて軌道計画を行うものとしては、達成させたい作業の動作を実現するための軌道を一度の軌道計画では求めることができないことがあり、余計な計算時間を要してしまうという問題がある。
【００１６】
例えば双腕ロボットに対して、右腕により必ずその作業の動作を実行させ、可能であれば左腕によってもその作業の動作を実行させたいことがある。非特許文献３などに開示される従来技術では、各部位の軌道計画についての優先度が考慮されていないために、右腕或いは左腕のいずれか１つの腕についてでもその軌道を求められなかったときには、軌道計画に失敗したとの出力をして処理が終了することになる。或いは、１回目の軌道計画でいずれか１つの腕についての軌道を求められなかったとしても、続く２回目の軌道計画でいずれか１つの腕についてのみの軌道を求め、さらに、続く３回目の軌道計画で他の腕についてのみの軌道を求めるというように順々に軌道を求めていくことも考えられる。しかし、このように１度の軌道計画で確実に軌道を求めることができず複数回にわたって軌道計画を行うものとしては、達成させたい作業についての軌道を最終的に求めるまでに多くの計算時間を要してしまう。
【００１７】
従って、本発明の第１の課題は、達成させたい作業の動作を一度で確実に生成可能とし、短時間で動作生成を実行可能なロボットの動作生成システム及び動作生成方法を提供することを目的とする。
【００１８】
なお、特許文献２に開示される技術では、各アームロボットに対して優先順位を付与して動作を生成するものであるが、複数の並列に配置されたロボットのうちで例えばその中央に配置されているロボットの優先順位を高く設定することで、ロボット間での相互干渉防止のために一時停止する回数を減らして、作業時間が伸びることを防ぐものにすぎない。
【００１９】
次に第２の問題を説明する。ロボットの姿勢をノードとして、初期姿勢から最終姿勢へと至る軌道を複数のノードを登録したグラフ構造を利用して求める際には、グラフ構造に予め登録したノードの個数が環境に対して十分でないために軌道を発見できないことがある。多数のノードを有する大規模なグラフ構造を予め準備しておけば軌道を確実に求めることができるものの、簡単な環境に対してはこのような大規模なグラフ構造は冗長なものとなる。すなわち、簡単な環境下においても大規模なグラフ構造を用いては、初期姿勢から最終姿勢へと至る軌道を発見するまでに一定時間以上の計算時間を要することになってしまう。このため、軌道が発見できることを保証しつつも、簡単な環境においては可能な限り高速に軌道を計画できることが求められる。
【００２０】
これを解決する従来手法としては、特許文献１や非特許文献４に開示される手法のように、軌道が発見できるまでの間、逐次ランダムに（或いは、所定の規則に沿った方法により）新たなノードをグラフ構造に追加することで、グラフ構造を拡張していくという手法がある。
【００２１】
例えば図１４に示す例では、ノード８１〜ノード８９が登録されたグラフ構造８０を用いて、スタートノードＳからゴールノードＧまでのノードの経路を探索した場合に、環境に存在する物体との干渉などを理由として、ノード８３及びノード８８を使用することができず、スタートノードＳからゴールノードＧまでの解（経路）を得ることができない。このため、図１５に示す例では、図１４に示したグラフ構造８０に対してノード９０とそのエッジを追加したことで、ゴールノードＧまでの解を得ている。
【００２２】
しかしながら、逐次ランダムに新たな姿勢（ノード）を作成してグラフ構造に追加すると、実際上は、見た目に不自然な姿勢や、背中に手を回すような使えない姿勢が多数追加されることになり、グラフ構造での探索に時間を要してしまう。また、ノードを新たにグラフ構造に追加する際には、追加したノードの最近傍ノードの検索や、追加に伴ってグラフを頻繁に解く必要が生じるため、計算時間の増加を招いてしまう。
【００２３】
従って、本発明の第２の課題は、軌道の計画可能性を保証しつつ、簡単な環境では短時間で軌道計画が可能なロボットの動作生成システム及び動作生成方法を提供することを目的とする。
【課題を解決するための手段】
【００２４】
本発明に係る第一の態様のロボットの動作生成システムは、複数のアームを備えるロボットと、前記ロボットの周辺の環境の環境情報を取得する環境認識手段と、前記アームの関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するグラフ構造を、前記アームごとに格納するグラフ構造データ格納手段と、前記複数のアームのそれぞれに優先度が設定され、当該設定された優先度に従って、前記グラフ構造データ格納手段に格納されている前記アームごとのグラフ構造を用いて、前記アームの初期姿勢を示すスタートノードから前記アームの最終姿勢を示すゴールノードまでを結ぶ経路を探索するグラフ構造探索処理手段と、前記グラフ構造探索処理手段により探索された経路に含まれるノードについて、当該ノードが示す姿勢が、前記環境認識手段により取得された前記環境情報と干渉するか否かを判定し、干渉していると判定した場合には、当該ノードを前記グラフ構造から除外するノード有効性判定手段と、を備えるものである。
【００２５】
これにより、達成させたい作業の動作を一度で確実に生成可能とし、短時間で動作生成を実行可能とすることができる。
【００２６】
また、前記ロボットは上体と下体とを連結する腰を更に備え、前記グラフ構造データ格納手段は、前記アームの関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するアーム用グラフ構造と、前記アームの関節軸情報及び前記腰の関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するアーム及び腰用グラフ構造とを、前記アームごとにそれぞれ格納し、前記グラフ構造探索処理手段は、最も高い優先度が設定されたアームについての経路を探索する場合には、前記グラフ構造データ格納手段に格納されている前記アーム及び腰用グラフ構造を用いて、前記アーム及び前記腰の初期姿勢を示すスタートノードから前記アーム及び前記腰の最終姿勢を示すゴールノードまでを結ぶ経路を探索するようにしてもよい。これにより、腰との同期を取りながら動作を生成することができる。
【００２７】
さらにまた、前記ロボットに実行させるタスクを管理するタスク管理手段を更に備え、前記タスク管理手段は、前記環境認識手段により取得された前記環境情報に基づいて、前記複数のアームのそれぞれに優先度を設定するようにしてもよい。これにより、アームの優先度を環境情報に基づいて設定することができる。
【００２８】
また、前記環境認識手段により取得された前記環境情報には前記環境内の把持対象物体の位置情報が含まれ、前記タスク管理手段は、前記把持対象物体と前記複数のアームのそれぞれとの位置関係に基づいて、前記複数のアームのそれぞれに優先度を設定するようにしてもよい。これにより、アームの優先度を環境情報に基づいてより簡単に設定することができる。
【００２９】
さらにまた、前記グラフ構造データ格納手段は、前記ノードの個数がそれぞれ異なる前記グラフ構造を前記アームごとに複数格納し、前記グラフ構造探索処理手段は、前記グラフ構造データ格納手段に格納されている前記アームごとの複数のグラフ構造のうちで第１のグラフ構造を用いて、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索し、当該探索した経路に含まれるいずれか１のノードが、前記ノード有効性判定手段により前記環境情報と干渉する姿勢を示すノードであると判定された場合には、前記第１のグラフ構造のノードのうちで前記ノード有効性判定手段により前記環境情報と干渉しない姿勢を示すノードであると判定されたノードと、前記第１のグラフ構造のノードのうちで前記探索がなされなかったノードと、を、前記第１のグラフ構造のノードの個数よりも多い個数のノードを有する第２のグラフ構造へと継承させ、当該第２のグラフ構造を用いて、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索するようにしてもよい。これにより、軌道の計画可能性を保証しつつ、簡単な環境では短時間で軌道計画が可能となる。
【００３０】
本発明に係る第二の態様のロボットの動作生成方法は、複数のアームと、上体と下体とを連結する腰と、を備えるロボットの動作生成方法であって、前記アームの関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するアーム用グラフ構造と、前記アームの関節軸情報及び前記腰の関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するアーム及び腰用グラフ構造とが、前記アームごとにそれぞれグラフ構造データ格納部に格納されており、前記ロボットの周辺の環境の環境情報を取得するステップと、前記複数のアームのそれぞれに優先度が設定され、最も高い優先度が設定されたアームについての経路を探索する場合に、前記グラフ構造データ格納部に格納されている前記アーム及び腰用グラフ構造を用いて、前記アーム及び前記腰の初期姿勢を示すスタートノードから前記アーム及び前記腰の最終姿勢を示すゴールノードまでを結ぶ経路を探索するステップと、２番目に高い又はそれ以降に高い優先度が設定されたアームについての経路を探索する場合に、前記設定された優先度に従って、前記グラフ構造データ格納部に格納されている前記アームごとのグラフ構造を用いて、前記アームの初期姿勢を示すスタートノードから前記アームの最終姿勢を示すゴールノードまでを結ぶ経路を探索するステップと、前記探索された経路に含まれるノードについて、当該ノードが示す姿勢が、前記取得した前記環境情報と干渉するか否かを判定し、干渉していると判定した場合には、当該ノードを前記グラフ構造から除外するステップと、を有するものである。
【００３１】
これにより、達成させたい作業の動作を一度で確実に生成可能とし、短時間で動作生成を実行可能とすることができる。
【００３２】
また、前記グラフ構造データ格納部には、前記ノードの個数がそれぞれ異なる前記グラフ構造が前記アームごとに複数格納されており、前記グラフ構造データ格納部に格納されている前記アームごとの複数のグラフ構造のうちで第１のグラフ構造を用いて、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索するステップと、前記探索した経路に含まれるいずれか１のノードが、前記環境情報と干渉する姿勢を示すノードであるか否かを判定し、干渉すると判定した場合に、前記第１のグラフ構造のノードのうちで前記環境情報と干渉しない姿勢を示すノードであると判定したノードと、前記第１のグラフ構造のノードのうちで前記探索がなされなかったノードと、を、前記第１のグラフ構造のノードの個数よりも多い個数のノードを有する第２のグラフ構造へと継承させるステップと、前記第２のグラフ構造を用いて、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索するステップと、を有するようにしてもよい。これにより、軌道の計画可能性を保証しつつ、簡単な環境では短時間で軌道計画が可能となる。
【発明の効果】
【００３３】
本発明は、上記従来の問題点を解決することができるロボットの動作生成システム及び動作生成方法を提供するものである。すなわち、第１の課題に対しては、達成させたい作業の動作を一度で確実に生成可能とし、短時間で動作生成を実行可能とし、第２の課題に対しては、軌道の計画可能性を保証しつつ、簡単な環境では短時間で軌道計画を可能とする。
【図面の簡単な説明】
【００３４】
【図１】実施の形態１に係るロボットの概略構成を示す全体図である。
【図２】実施の形態１に係るロボットの動作生成システムの構成図である。
【図３】実施の形態１に係るロボットの全身軌道計画部の構成図である。
【図４】実施の形態１に係るグラフ構造の概念図である。
【図５】実施の形態１に係る軌道計画処理のフローチャート図である。
【図６】実施の形態１に係る部位ごとのグラフ構造の概念図である。
【図７】実施の形態１に係る部位ごとのグラフ構造の概念図である。
【図８】実施の形態１に係る第１優先の部位についての軌道探索処理のフローチャート図である。
【図９】実施の形態１に係る第２優先の部位についての軌道探索処理のフローチャート図である。
【図１０】実施の形態２に係る軌道探索処理のフローチャート図である。
【図１１】実施の形態２に係る小規模なグラフ構造の概念図である。
【図１２】実施の形態２に係る大規模なグラフ構造の概念図である。
【図１３】実施の形態３に係る軌道計画処理のフローチャート図である。
【図１４】本発明の課題を説明するための図である。
【図１５】本発明の課題を説明するための図である。
【発明を実施するための形態】
【００３５】
実施の形態１．
以下、図面を参照して本発明の実施の形態について説明する。なお、以下では、本文中の説明においては、必要に応じそれ以前に述べた符号を用いるものとする。
【００３６】
図１は、本実施の形態に係るロボットのリンク構造の概略構成を示す全体図である。図１（ａ）は正面図を示し、図１（ｂ）は右側面図を示し、図１（ｃ）は左側面図を示す。なお、図に示す例では、ロボット１と路面との接触地点にロボット基準座標を定め、ロボット１の首、右手先、左手先にそれぞれ局所座標を定めている。
【００３７】
図１に示すように、ロボット１は、頭部と、胴体と、胴体に連結された右アーム及び左アームと、各アームに連結された右ハンド及び左ハンドと、を備えている。胴体は、上体と、腰と、腰を介して上体に連結された下体と、を備えている。右アームは、複数の関節（Ｒ１〜Ｒ７）と、これら関節を介して接続される複数のリンクと、を備えている。左アームは、複数の関節（Ｌ１〜Ｌ７）と、これら関節を介して接続される複数のリンクと、を備えている。腰は、腰関節Ｗ１と、関節を介して上体及び下体と接続する複数のリンクと、を備えている。なお、上体と頭部を接続する首関節については図示を省略した。また、詳細についての図示は省略するが、各ハンドは、掌部と、関節を介して掌部に接続される複数本の指と、を備えていてもよい。また、各指は、複数の指関節と、指関節を介して接続される複数のリンクと、を備えていてもよい。
【００３８】
各関節に備えられたモーターなどのアクチュエータ（不図示）が駆動されることで、各関節は、所望の角度及び角速度で動作する。その結果、各座標系において、ロボット１の全身の位置及び姿勢と、手先の位置及び姿勢と、を所望の位置及び姿勢に制御することができる。このように、アクチュエータは、ロボット１が動作する際の、動力発生手段に相当する。
【００３９】
ロボット１は、ユーザから指定される様々なタスクを実行するために動作を生成する。タスクとは、環境内に存在する物体に対する作業であり、当該作業は、作業を実現するための一連の動作を含んでいる。例えば、環境内に存在する物体を、ハンドにより把持するなどのタスクが挙げられる。ロボット１は、このようなタスクを実行するために右アームと、左アームと、腰と、の軌道を計画する。そして、計画した軌道に追従するように各関節を駆動制御することで、動作を生成する。なお、軌道とは、初期姿勢を示す関節角度ベクトルから最終姿勢を示す関節角度ベクトルへと至る関節角度ベクトルの系列であり、後述するグラフ構造を用いて求められる。
【００４０】
図２は、本実施の形態に係るロボットの動作生成システムの構成図である。動作生成システム１０は、環境認識部１１と、タスク管理部１２と、最終全身姿勢決定部１３と、初期全身位置・姿勢取得部１４と、全身軌道計画部１５と、補間部１６と、モーター駆動部１７と、を備えている。
【００４１】
環境認識部１１は、ロボット１の周辺の環境の環境情報を取得する。環境認識部１１は、各種のセンサを用いて構成され、ロボット１の周辺の環境内に存在する物体の位置や形状などの環境情報を取得する。センサには、例えば、ステレオカメラやレーザーレンジファインダを用いることができ、環境認識部１１は、環境内の物体について３次元の点群情報を取得し、ロボット１から物体までの距離情報を取得することができる。環境認識部１１は、取得した物体の距離情報に基づいて、環境内の物体を、例えば、直方体形状のボクセル情報や、３次元の物体モデルとして生成し、環境情報としてタスク管理部１２と、最終全身姿勢決定部１３と、全身軌道計画部１５と、に出力する。
【００４２】
タスク管理部１２は、ロボット１に実行させるタスクを管理する。タスク管理部１２は、タスクの内容に応じて、環境認識部１１で取得した環境情報から必要な情報を取得する。そして、タスク管理部１２は、タスクの内容と環境情報とに基づいて、各部位に優先度を設定し、部位の優先度を含むタスク情報を出力する。なお、タスクの内容や各部位の優先度は、動作生成システム１０に備えられたインタフェース（不図示）を介してユーザにより入力されるものとしてもよい。
【００４３】
例えば、物体を把持するタスクでは、ロボット１に把持させる物体、その把持方法などを示す情報がタスクに含まれており、タスク管理部１２は、環境認識部１１で取得した環境情報から、把持対象となる物体を示す情報と、その形状や位置・姿勢などを示す情報などを取得する。そして、タスク管理部１２は、例えば、ロボット１の各部位と把持対象物体との位置関係に応じて、把持に際しての各部位の優先度を設定する。
【００４４】
例えば、物体を把持するタスクにおいて、左アームのハンドに比べて右アームのハンドのほうが把持対象物体により近い場合には、タスク管理部１２は、右アームの優先度を「１」とし、左アームの優先度を「２」として設定する（数字が小さいほど優先度が高く、「１」が最高）。
【００４５】
また、例えば、物体を把持するタスクにおいて、右アームのハンドで必ず物体を把持し、左アームのハンドでは可能であれば把持して欲しいという状況では、ユーザにより、右アームの優先度を「１」とし、左アームの優先度を「２」として設定するようにしてもよい。このような優先度設定の具体例としては、コップとスプーンという２つの把持対象物体が机上に存在した場合に、右手では必ずコップを把持し、左手では可能であれば同時にスプーンを把持して欲しい状況や、コップの置かれたトレイを右手で把持した状態で、可能であれば左手で他のコップを把持してそのトレイに置いて欲しい状況などが挙げられる。
【００４６】
最終全身姿勢決定部１３は、環境情報と、タスク管理部１２で求められたタスク情報から、タスク実行時のロボット１の最終全身姿勢を決定する。
【００４７】
初期全身位置・姿勢取得部１４は、タスク管理部１２から出力されたタスク情報から、タスク実行時のロボット１の初期全身位置・姿勢を取得する。なお、初期全身位置・姿勢は、ロボット１の現在の位置・姿勢をそのまま与えるものとしてもよい。
【００４８】
全身軌道計画部１５は、環境情報と、部位の優先度を含むタスク情報と、最終全身姿勢と、初期全身位置・姿勢と、に基づいて、ロボット１の全身の軌道計画を行う。軌道計画は各部位についての軌道を探索することで行われ、初期全身位置・姿勢から最終全身姿勢へと至る軌道であって、かつ、環境と干渉しない軌道が計画される。なお、全身軌道計画部１５の詳細については後述する。
【００４９】
補間部１６は、全身軌道計画部１５で求められた全身関節軌道を、モーターを駆動する制御周期に合わせて適切となるように補間する。補間部１６により、全身軌道計画部１５で求めた軌道と比較してより滑らかな軌道が求められる。補間部１６は、その軌道に含まれる関節角度ベクトルに基づいてロボット１のアクチュエータ（モーター）へのモーター指令値を生成する。
【００５０】
モーター駆動部１７は、補間部１６で求められたモーター指令値に基づいて、フィードバック制御を行う。
【００５１】
次に、図３乃至図９を参照して、全身軌道計画部１５の詳細について説明する。
図３は、全身軌道計画部１５の構成図である。全身軌道計画部１５は、グラフ構造データ格納部１５１と、探索用グラフ構造データ保持部１５２と、グラフ構造生成処理部１５３と、グラフ構造探索処理部１５４と、ノード有効性判定処理部１５５と、を備えている。
【００５２】
グラフ構造データ格納部１５１は、複数のグラフ構造を格納しており、本実施の形態では、部位ごとのグラフ構造を格納している。本実施の形態では、ロボット１の全身の全ての関節軸を表現したグラフ構造を利用して軌道計画を行うことに代えて、部位ごとに用意したグラフ構造を利用して、部位ごとに軌道を計画する。そして、各部位に設定された優先度に従って軌道計画を行うことで、達成させたい作業の動作を一度で確実に生成可能にすると共に、探索範囲を限定することにより短時間で動作生成を実行可能とする。なお、詳細は後述するように、部位のうち腰については、優先度の最も高い部位のグラフ構造に含めて、その軌道を計画する。例えば、ロボット１が腰関節を有した双腕ロボットである場合には、グラフ構造データ格納部１５１に格納されるグラフ構造には、右アーム用のグラフ構造、左アーム用のグラフ構造、右アーム及び腰用のグラフ構造、左アーム及び腰用のグラフ構造、が含まれる。
【００５３】
図４は、グラフ構造の概念図である。
グラフ構造２０は、複数のノード２１〜２９と、ノード間を接続する線分（以下、エッジと称する場合がある。）と、を有している。グラフ構造２０を用いて、スタートノードＳとゴールノードＧとを結ぶ経路が探索される。グラフ構造のノード構成は、タスクの内容と、各部位の優先度と、を考慮してユーザにより設定されており、各ノードは、ユーザにより予め注意深く選ばれている。
【００５４】
なお、ノードｑｎは、以下の数（１）に示すように、アームの関節軸θ（アームが複数の関節軸を有する場合には、θはベクトルを示す。）と、手先位置ｘと、重み行列Ｗの情報を持つ。重み行列Ｗは、動作を実現するに際して各関節の重要度を反映させるための情報であり、各関節の配置関係に基づいて定められる。例えば、アームの関節のうちで、手首関節に比べてより大きな重み付けが肘関節に設定される。また、ノードが持つ情報は、少なくともアームの関節軸θの情報を備えていればよく、手先座標ｘについては必須ではない。
【数１】
また、エッジｃは、以下の数（２）に示すように、ノード間の距離情報を持つ。
【数２】
【００５５】
探索用グラフ構造データ保持部１５２は、軌道探索に用いるグラフ構造を一時的に保持する。例えば、軌道探索に使用中のグラフ構造を探索用グラフ構造データ保持部１５２に一時的に保持する。また、探索した軌道を探索用グラフ構造データ保持部１５２に保持しておき、ロボット１が移動した場合に、保持しているグラフ構造を更新する、などのために探索用グラフ構造データ保持部１５２を利用してもよい。
【００５６】
グラフ構造生成処理部１５３は、優先度に従って軌道を探索する部位を特定し、グラフ構造データ格納部１５１に格納されているグラフ構造から軌道探索対象となる部位のグラフ構造を選択し、選択したグラフ構造を探索用グラフ構造データ保持部１５２に保持する。そして、グラフ構造生成処理部１５３は、入力されるスタートノードとゴールノードを探索用グラフ構造データ保持部１５２に保持されているグラフ構造に追加し、追加したそれぞれのノードについて近傍ノードとの間をエッジにより接続する。ここで、スタートノードは、ロボット１の初期全身位置・姿勢における軌道探索部位の位置・姿勢を示すノードであり、ゴールノードは、ロボット１の最終全身姿勢における軌道探索部位の姿勢を示すノードである。なお、スタートノードは、軌道探索部位の姿勢のみを示すノードであってもよい。
【００５７】
グラフ構造探索処理部１５４は、探索用グラフ構造データ保持部１５２に保持されているグラフ構造を用いて、スタートノードからゴールノードまでを結ぶ経路（スタートノードからゴールノードへと至るノードの経路であり、軌道と称する場合がある。）を探索する。なお、グラフ構造探索処理部１５４による探索方法については限定されず、ダイクストラ法などの公知の手法を用いればよい。
【００５８】
ノード有効性判定処理部１５５は、グラフ構造探索処理部１５４により探索された経路に含まれるノードについて、ノードが示す姿勢が、環境内の物体やロボット１自身と干渉していないか否かをチェックし、干渉している場合には、探索用グラフ構造データ保持部１５５のグラフ構造から当該ノードを除外する。
【００５９】
図５は全身軌道計画部１５による軌道計画処理のフローチャート図である。
まず、全身軌道計画部１５は、最も高い優先度が設定されたアームを、第１優先のアーム（腕）として決定する（Ｓ１０１）。例えば、物体と手先との距離などを基準として、手先の位置が物体により近いアームについて最も高い優先度が設定される。
【００６０】
全身軌道計画部１５は、第１優先のアームと腰についての軌道を探索する（Ｓ１０２）。全身軌道計画部１５は、第１優先のアームと腰用のグラフ構造を用いて軌道を探索する。図６は、第１優先のアームと腰用のグラフ構造を例示する概念図である。グラフ構造３０は、複数のノード３１〜３９と、ノード間を接続するエッジと、を有している。ここで、ノード３１〜３９の各ノードは、第１優先のアームの関節軸及び手先位置と、腰の関節軸と、の情報を有している。なお、Ｓ１０２での処理の詳細については後述する。
【００６１】
全身軌道計画部１５は、Ｓ１０２での軌道探索の結果、第１優先のアームと腰についての有効な軌道が存在するか否かを判定する（Ｓ１０３）。軌道が存在しない場合（Ｓ１０３で「無」の場合）には、第１優先のアームと腰についての軌道を探索することができなかったとして、処理を終了する。なお、有効な軌道とは、スタートノードからゴールノードへと至るノードの経路であって、この経路に含まれる全てのノードが干渉しないものをいう。また、スタートノードからゴールノードへと至るノードの経路は複数探索される可能性があるが、経路の評価基準に従って、最も良い評価基準の経路を第１候補軌道とする（例えば、経路上のノード間距離の総和が最小の経路を、第１候補軌道とする）。
【００６２】
全身軌道計画部１５は、第１優先のアームと腰についての有効な軌道が存在した場合（Ｓ１０３で「有」の場合）には、第２優先のアームについての軌道を探索する（Ｓ１０４）。全身軌道計画部１５は、第２優先のアーム用のグラフ構造を用いて軌道を探索する。図７は、第２優先のアームのグラフ構造を例示する概念図である。グラフ構造４０は、複数のノード４１〜４９と、ノード間を接続するエッジと、を有している。ここで、ノード４１〜４９の各ノードは、第２優先のアームの関節軸及び手先位置の情報を有している。なお、Ｓ１０４での処理の詳細については後述する。
【００６３】
全身軌道計画部１５は、Ｓ１０４での軌道探索の結果、第２優先のアームについての有効な軌道が存在するか否かを判定する（Ｓ１０５）。有効な軌道が存在した場合（Ｓ１０５で「有」の場合）には、Ｓ１０２で探索した第１優先のアームと腰の軌道と、Ｓ１０４で探索した第２優先のアームの軌道と、を出力する。
【００６４】
全身軌道計画部１５は、Ｓ１０４での軌道探索の結果、第２優先のアームについての有効な軌道が存在しない場合（Ｓ１０５で「無」の場合）には、再びＳ１０２へと戻り、第１優先のアームと腰についての軌道を探索するが、Ｓ１０２で探索された経路のうち、２番目に評価基準が良かった軌道が存在する場合には、当該軌道を採用してＳ１０４へと進み、存在しない場合には、第１優先のアームと腰と、第２優先のアームと、のいずれについても軌道を探索することができなかったとして処理を終了する。
【００６５】
従って、図５に示した軌道計画処理によれば、全身軌道計画部１５による出力は、（ｉ）第１優先のアームと腰の軌道と、第２優先のアームの軌道と、の両方の出力、（ｉｉ）第１優先のアームと腰についての軌道を探索することができなかったとの出力、（ｉｉｉ）第１優先のアームと腰と、第２優先のアームと、のいずれについても軌道を探索することができなかったとの出力、となる。
【００６６】
次に、図８を参照して、図５に示したＳ１０２での処理の詳細について説明する。
まず、グラフ構造生成処理部１５３は、第１優先のアーム（腰を含む）についてのグラフ構造を生成する（Ｓ２０１）。グラフ構造生成処理部１５３は、入力されるスタートノードとゴールノードをグラフ構造に追加すると共に、追加したノードについて近傍ノードとの間をエッジにより接続する（Ｓ２０２）。
【００６７】
グラフ構造探索処理部１５４は、第１優先のアームについてのグラフ構造を用いて、スタートノードからゴールノードまでを結ぶ経路を探索する（Ｓ２０３）。グラフ構造探索処理部１５４は、軌道探索の結果、軌道が存在するか否かを判定する（Ｓ２０４）。軌道が存在しない場合（Ｓ２０４で「無」の場合）には、軌道を探索することができなかったとして、処理を終了する。
【００６８】
ノード有効性判定処理部１５５は、軌道が存在した場合（Ｓ２０４で「有」の場合）には、探索された経路に含まれる全てのノードについて、ノードが示す姿勢が、環境内の物体やロボット１自身と干渉していないか否かをチェックし（Ｓ２０５）、干渉の有無を検出する（Ｓ２０６）。なお、第１優先のアームについての干渉チェックを行う際には、第２優先のアームを除外したロボット１の形状（第１優先のアーム、胴体、頭部など）に対してのみ干渉チェックを行えばよい。
【００６９】
ノード有効性判定処理部１５５は、干渉検出の結果、経路に含まれるいずれか１つのノードが干渉している場合（Ｓ２０６で「ＮＧ」の場合）には、グラフ構造から当該ノードを除外する（Ｓ２０７）。グラフ構造探索処理部１５４により、ノード除外後のグラフ構造を用いて再び軌道探索を行う。
【００７０】
ノード有効性判定処理部１５５は、干渉検出の結果、経路に含まれるいずれのノードも干渉していない場合（Ｓ２０６で「ＯＫ」の場合）には、第１優先のアームについての軌道を出力する。
【００７１】
次に、図９を参照して、図５に示したＳ１０４での処理の詳細について説明する。ただし、Ｓ１０４での第２優先のアームについての軌道探索処理は、Ｓ１０２での第１優先のアームについての軌道探索処理と基本的には同じであり、図８を参照して説明したＳ２０５での干渉チェックについてのみ異なる。このため、以下では、Ｓ１０４での第２優先のアームについての軌道探索処理に関して、図９を参照して、干渉チェックの詳細についてのみを説明する。
【００７２】
ノード有効性判定処理部１５５は、繰り返し回数Ｎを決定する（Ｓ３０１）。繰り返し回数Ｎは、以下のＳ３０３乃至Ｓ３０７の繰り返し回数である。本実施の形態では、第１優先のアームについて探索した経路に含まれるノードの個数をＬ、第２優先のアームについて探索した経路に含まれるノードの個数をＭとしたとき、ＬとＭで値が大きなものをＮの値として設定する。
【００７３】
ノード有効性判定処理部１５５は、カウンタｎの値を初期化する（Ｓ３０２）。ここでは、カウンタｎ＝１とする。ノード有効性判定処理部１５５は、カウンタｎの値がＭより大きいか否かを判定する（Ｓ３０３）。ノード有効性判定処理部１５５は、Ｓ３０３での判定の結果、カウンタｎの値がＭより大きくない場合（Ｓ３０３でＮｏの場合）には、カウンタｎの値がＬより小さいか否かを判定する（Ｓ３０４）。
【００７４】
ノード有効性判定処理部１５５は、Ｓ３０４での判定の結果、カウンタｎの値がＬより大きくない場合（Ｓ３０３でＮｏの場合）には、第１優先のアーム（腰を含む）の探索経路についてｎ番目のノードが示す姿勢において、第２優先のアームの探索経路についてｎ番目のノードが示す姿勢の干渉チェックを行う（Ｓ３０５）。腰関節の駆動に伴ってロボット１の上体の姿勢が変化するため、上体に設けられた第２優先のアームについてもその姿勢が相対的に変化する。このため、第１優先のアームについて探索された経路に含まれる腰の姿勢を基準として、第２優先のアームの探索経路のノードが示す姿勢についての干渉チェックを行う。
【００７５】
ノード有効性判定処理部１５５は、Ｓ３０３での判定の結果、カウンタｎの値がＭより大きい場合（Ｓ３０３でＹｅｓの場合）には、第１優先のアーム（腰を含む）の探索経路についてｎ番目のノードが示す姿勢において、第２優先のアームの探索経路についてＭ番目のノードが示す姿勢の干渉チェックを行う（Ｓ３０８）。
【００７６】
ノード有効性判定処理部１５５は、Ｓ３０４での判定の結果、カウンタｎの値がＬより大きい場合（Ｓ３０４でＹｅｓの場合）には、第１優先のアーム（腰を含む）の探索経路についてＬ番目のノードが示す姿勢において、第２優先のアームの探索経路についてｎ番目のノードが示す姿勢の干渉チェックを行う（Ｓ３０９）。
【００７７】
ノード有効性判定処理部１５５は、カウンタｎが繰り返し回数Ｎに到達した否かを判定し（Ｓ３０６）、到達した場合（Ｓ３０６でＹｅｓの場合）には処理を終了する。到達していない場合には、カウンタｎの値を１増加させ（Ｓ３０７）、Ｓ３０３へと戻る。
【００７８】
図９に示したように、第１優先のアーム（腰を含む）の探索経路のノードと、第２優先のアームの探索経路のノードと、について対応するノードの干渉チェックを行うことで、アームごとのグラフ構造を用いた場合であっても、共に干渉していない両アームの動作を求めることができる。そして、第１優先のアームの探索経路は腰についての関節軸情報を含んでおり、第２優先のアームの探索経路は第１優先のアームの探索経路に対応したものであるため、結果として、作業を行うアーム（第１優先のアーム及び第２優先のアーム）と、腰との同期が取れた動作を生成することができる。
【００７９】
次に、本実施の形態による効果について説明する。
まず、ロボット１が、Ｌ個の関節軸を含む右アームと、Ｍ個の関節軸を含む左アームと、を備える双腕ロボットであり、全部でＮ個の関節軸を有する場合に、一つの関節軸のとりうる範囲をＸ分割すると全ての関節軸の組み合わせはＸＮ通りとなり、ある姿勢を探索する範囲は、Ｎ乗に比例して大きくなる。これに対して、探索範囲を部位（アーム）ごとに分割して軌道探索することで、探索範囲はＸＬ＋ＸＭとなり、全ての関節軸を一度に探索するのと比べて、探索範囲を大幅に減少させることができる。また、グラフ構造を利用して軌道を探索する場合に、探索範囲が小さければ同じ分解能であってもノードの個数を減らすことができるため、計算時間を短縮することができる。
【００８０】
また、実際にタスクを行う部位（優先度の高いアーム）について腰関節を含むグラフ構造を用いて軌道探索を行った上で、他の部位（より優先度の低いアーム）についての軌道探索を行うため、腰との同期を含めてタスクを行わせたい部位（両アーム）の同期が可能である。また、双腕による作業の場合においても、いずれかのアームを第１優先の腕とすることで対応することができる。
【００８１】
ロボット１に達成させたい作業（タスク）には複雑な作業が多く存在し、例えば、右手では必ず作業を行い、左手では可能であれば実行して欲しいという作業がある。本実施の形態では、各部位に優先度を設定して軌道探索を行うことで、優先度の高い部位についての軌道を実現した上で、優先度のより低い部位の軌道を実現するように軌道計画（動作生成）を行う。例えば、部位の優先度として、右アームに最も高い優先度を設定し、左アームに右アームよりも低い優先度を設定した場合（右アームで必ず作業を行い、できれば左アームでも作業を行って欲しい場合）には、（ｉ）右アームでの作業を実現させる軌道と、右アームでの作業を実現させる軌道を前提とした左アームでの作業を実現させる軌道、（ｉｉ）必ず実現させたかった右アームでは軌道を求めることができなかった、（ｉｉｉ）両アーム共に軌道を求めることができなかった、という出力を得ることができる。このため、従来手法と比較して、複雑な作業に対しても１度の軌道計画で解（軌道）を求められる可能性が高く、達成させたい作業の動作を一度でより確実に生成可能とし、より短時間で動作生成を実行することができる。
【００８２】
実施の形態２．
次に、本発明の実施の形態２について説明する。上述の実施の形態においては、ノード構成（ノードの個数、エッジなど）が固定されたグラフ構造を利用して軌道を探索するものであったが、本実施の形態においては、軌道探索の結果を継承させながらノード構成を変化させるグラフ構造を利用して軌道を探索するものである。なお、図１に示したロボット１の概略構成や、図２に示した動作生成システム１０の構成及び処理については上述した実施の形態１と同一であるためその詳細な説明を省略し、以下では、実施の形態１と異なる点を中心に説明する。
【００８３】
図１０乃至図１２を参照して、本実施の形態による軌道探索処理について説明する。
図１０は、規模の異なる複数のグラフ構造を用いた軌道探索処理のフローチャート図である。本実施の形態では、グラフ構造データ格納部１５１は、ノードの規模が異なる複数のグラフ構造を部位ごとに格納しており、ここでは、ノード数が比較的少ない小規模なグラフ構造と、その小規模なグラフ構造を含み、かつ、ノード数が比較的多い大規模なグラフ構造と、の２つのグラフ構造を有しているものとして説明する。また、図に示す軌道探索処理が、第１優先の部位（腰を含む）、第２優先の部位のいずれについてもそれぞれ適用される。
【００８４】
本実施の形態に係る全身軌道計画部１５は、小規模なグラフ構造を用いて軌道を探索する（Ｓ４０１）。なお、Ｓ４０１での軌道探索処理には、ノードの干渉チェックが含まれている。次いで、Ｓ４０１での軌道探索の結果、有効な軌道が存在するか否かを判定し（Ｓ４０２）、軌道が存在する場合（Ｓ４０２で「有」の場合）には、その軌道を出力して処理を終了する。
【００８５】
Ｓ４０１での軌道探索の結果、軌道探索の結果、有効な軌道が存在しない場合には（Ｓ４０２で「無」の場合）には、Ｓ４０１での軌道探索結果を、大規模なグラフ構造に継承させる（Ｓ４０３）。継承させる軌道探索結果には、Ｓ４０１で用いた小規模なグラフ構造のノードのうち、軌道探索の際に探索されなかったノードと、干渉チェックの結果、干渉しないと判定されたノードと、を示す情報と、が含まれる。
【００８６】
次いで、小規模なグラフ構造による軌道探索結果を継承させた大規模なグラフ構造を用いて、軌道を探索する（Ｓ４０４）。次いで、Ｓ４０４での軌道探索の結果、有効な軌道が存在するか否かを判定する（Ｓ４０５）。判定の結果、軌道が存在する場合（Ｓ４０５で「有」の場合）には、その軌道を出力して処理を終了し、軌道が存在しない場合（Ｓ４０５で「無」の場合）には、軌道を探索することができなかったとして処理を終了する。
【００８７】
図１１及び図１２を参照して、図１０に示した処理の具体例を説明する。
図１１は、小規模なグラフ構造を例示する概念図である。図１２は、大規模なグラフ構造を例示する概念図である。グラフ構造５０を用いて軌道を探索した結果、ノード５３及びノード５８が干渉すると判定され、有効な軌道を探索することができないと判定された。このため、グラフ構造６０を用いて軌道が探索される結果、ノード７４を介することで、スタートノードＳからゴールノードＧまでの経路を探索することができる。
【００８８】
以上説明したように、本実施の形態では、まずは比較的小規模なグラフ構造を用いて軌道を探索し、軌道を求められない場合には、軌道探索結果を継承させたより大規模なグラフ構造を用いて軌道を探索する。このように、前回の軌道探索結果を継承させながら徐々に複雑なグラフ構造へと切替えていくことで、軌道の計画可能性を保証しつつ、簡単な環境では短時間で軌道計画が可能となる。
【００８９】
例えば、障害物が特に存在しない環境下で机上に置かれた物体までアームを伸ばすといった比較的簡単な状況下では、アームの軌道を瞬時に探索できることが求められる。グラフ構造を用いて軌道を探索する手法では、ノード数及びエッジ数が少ないほど高速に計算が可能であるため、このような環境下では小規模なグラフ構造を利用して軌道を探索することが好ましい。一方で、目標物体の周囲に多数の障害物が存在するような複雑な環境下で、軌道探索に最初に用いたグラフ構造のノード数などが不十分であったために、物体との干渉チェックにより多くのノードが無効となり、この結果、初期姿勢から最終姿勢までのアーム軌道を探索できないことがある。
【００９０】
従って、本実施の形態では、最初に用いる小規模なグラフ構造で軌道が求まらない場合には、ノード数やエッジ数がより多い大規模なグラフ構造に切替えて探索を行う。大規模なグラフ構造では小規模なグラフ構造に比べると計算時間を要するものの、解（軌道）が求まる可能性が高いためである。そして、大規模なグラフ構造は、その前の探索で用いた小規模なグラフ構造による探索結果を包含するため、干渉チェックにより干渉しないと判定されたノードについては以降の干渉チェックを省き、干渉すると判定されたノードについては大規模グラフ構造から予め省くことで、軌道の計画可能性を保証しつつ、より短時間で軌道計画が可能となる。
【００９１】
なお、本実施の形態では、小規模なグラフ構造と大規模なグラフ構造の２つのグラフ構造を用いた場合を例に説明したが、規模のそれぞれ異なる３以上のグラフ構造をグラフ構造データ格納部１５１に格納し、順次、より規模の大きなグラフ構造に切替えて軌道を探索するものとしてもよい。
【００９２】
実施の形態３．
次に、本発明の実施の形態３について説明する。
図１３は、実施の形態３に係る全身軌道計画部１５による軌道計画処理のフローチャート図である。実施の形態３では、実施の形態１の図５に示した処理と同一の処理を行うが、Ｓ５０１及びＳ５０３では、実施の形態２の図１０で示した処理を行う。すなわち、Ｓ５０１及びＳ５０３での軌道探索処理は、まずは比較的小規模なグラフ構造を用いて軌道を探索し、軌道を求められない場合には、軌道探索結果を継承させたより大規模なグラフ構造を用いて軌道を探索することで行われる。
【００９３】
その他の実施の形態．
上述した実施の形態では、ロボット１が右アーム及び左アームを有する双腕ロボットである場合を例として説明したが、３本以上のアームを有し、各アームに対して優先度を設定するものとしてもよい。
【００９４】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
【符号の説明】
【００９５】
１ ロボット、
Ｒ１〜Ｒ７、Ｌ１〜Ｌ７ 関節、
１０ 動作生成システム、
１１ 環境認識部、
１２ タスク管理部、
１３ 最終全身姿勢決定部、
１４ 初期全身位置・姿勢取得部、
１５ 全身軌道計画部、
１５１ グラフ構造データ格納部、
１５２ 探索用グラフ構造データ保持部、
１５３ グラフ構造生成処理部、
１５４ グラフ構造探索処理部、
１５５ ノード有効性判定処理部、
１６ 補間部、
１７ モーター駆動部、
２０、３０、４０、５０、６０、８０ グラフ構造、
２０〜２９、３０〜３９、４０〜４９、５０〜５９、６０〜７７、８１〜９０ ノード、
Ｓ スタートノード、
Ｇ ゴールノード
【技術分野】
【０００１】
本発明は、ロボットの動作生成システム及び動作生成方法に関する。
【背景技術】
【０００２】
近年、様々に変化する環境下においても、環境情報を反映した動作をロボットが自律的に生成可能とする技術が開発されている。ロボットの動作生成は、ロボットが備える各関節について、関節軸情報の系列である関節軌道（以下、単に軌道と称する場合がある。）が計画され、計画された軌道に基づいて関節が駆動されることで実現される。
【０００３】
ロボットがアームを備えるロボットである場合には、ロボットアームの軌道計画とは、ロボット自身と環境とが干渉しないように、ロボットアームのエンドエフェクタの位置姿勢が、初期姿勢から最終姿勢へと到達するような関節軸情報の系列を求める問題である。
【０００４】
この問題の解法の一つとして、ロボットの関節軸情報であるノードを複数有したグラフ構造を使用する手法が知られている。当該グラフ構造では、ノード間がエッジにより接続され、初期姿勢であるスタートノードから最終姿勢であるゴールノードへと至るノードの系列である経路が探索される。このようなグラフ構造を使用して軌道計画を行う技術が、例えば、特許文献１、非特許文献１乃至４に開示されている。
【０００５】
非特許文献１には、アーム毎のグラフ構造による動作生成手法が開示されている。また、非特許文献２には、１本のアームに適用したグラフ構造を用いて動作計画を行う手法が開示されている。非特許文献３には、ロボットの部位（例えば、右腕、左腕、腰など）ごとのグラフ構造を予め用意しておき、軌道計画実行時に、これら部位ごとのグラフ構造を組み合わせて、全身の動作生成を行う手法が開示されている。
【０００６】
特許文献１及び非特許文献４には、グラフ構造を変化させる、或いは、規模の異なる複数のグラフ構造を利用することで、動作生成を行う手法が開示されている。特許文献１に開示された経路作成装置は、ノードの個数を設定して、その設定された個数分のノードの分布が密集している場合に、ノード間を接続する線分長を短くすることで、障害物を回避できるようグラフ構造を複雑にして経路の作成を行う。また、非特許文献４には、規模の異なる複数のグラフ構造を用いた動作生成手法が開示されている。
【０００７】
なお、本発明に関連する他の技術として、特許文献２には、複数のロボットを動作させるためのティーチングデータの作成において、各ロボットに優先順位を設定し、優先順位に基づいて相互干渉確認を行うことで、干渉の無いロボットの動作を生成する技術が開示されている。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特開２００８−２０４１６１号公報
【特許文献２】特開２００４−３５８６３０号公報
【非特許文献】
【０００９】
【特許文献１】"Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces", L.Kavraki, P.Svestka, J.Latombe, M.Overmars, IEEE transactions on robotics and automation, Vol.12, No.4, Aug.1996.
【特許文献２】"Motion Planning Using Dynamic Roadmaps", Marcelo Kallmann and Maja Mataric, Proceedings of the IEEE International Conference on Robotics and Automation (ICRA2004), April, 2004.
【特許文献３】"Roadmap composition for multi-arm systems path planning", M. Gharbi, J. Cortes,T. Simeon, 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems, Saint Louis, MO, USA (October 2009).
【特許文献４】"Sampling-Based Roadmap of Trees for Parallel Motion Planning", Erion Plaku, Kostas E.Bekris, Brian Y.Chen, Andrew M.Ladd, Lydia E.Kavraki, IEEE transactions on robotics and automation, Vol.21, No.4, 2005.
【発明の概要】
【発明が解決しようとする課題】
【００１０】
上記した従来技術のように、グラフ構造を用いた軌道計画手法によれば、複雑かつ様々に変化する環境下においても軌道を算出することが可能となる。しかし、軌道計画の対象となるロボットが複数本のアームを備えるものである場合には、以下の問題がある。
【００１１】
まず第１の問題を説明する。例えば双腕ロボットがグラフ構造を用いて軌道計画を行う際には、２本のアームの各関節の全ての関節軸情報を１つのノードに持たせた上で、このようなノードが複数登録された１つのグラフ構造を利用して、初期姿勢から最終姿勢へと至る軌道を求めるという手法が考えられる。
【００１２】
しかしながら、ロボットの関節軸空間はｎ次元（ｎは関節軸の個数）となることから、これと同じ密度の関節軸情報を１つのノードに持たせて探索した場合の計算時間は、ｎ乗に比例することになる。双腕ロボットは通常１０次元以上の関節軸空間となるために、軌道を発見するまでには、多くの場合に多大な計算時間を必要とすることになる。
【００１３】
一方で、非特許文献３に開示される技術のように、完全に分離可能なロボットの部位ごとに、それら部位内の関節軸情報を１つのノードに持たせた上で、このようなノードが複数登録された複数のグラフ構造を利用して軌道を求めるという手法も考えられる。
【００１４】
部位ごとに別々のグラフ構造を用意し、これら複数のグラフ構造を用いて軌道計画を行うことで、探索空間の大きな１つのグラフ構造で探索を行う場合と比べて、探索空間のより小さな個々のグラフ構造で探索を行うことができ、軌道計画に要する探索時間を大幅に削減することが可能となる。
【００１５】
しかしながら、ロボットに達成させたい作業（タスク）には複雑な作業が多く存在する。このため、単純に部位ごとのグラフ構造を用いて軌道計画を行うものとしては、達成させたい作業の動作を実現するための軌道を一度の軌道計画では求めることができないことがあり、余計な計算時間を要してしまうという問題がある。
【００１６】
例えば双腕ロボットに対して、右腕により必ずその作業の動作を実行させ、可能であれば左腕によってもその作業の動作を実行させたいことがある。非特許文献３などに開示される従来技術では、各部位の軌道計画についての優先度が考慮されていないために、右腕或いは左腕のいずれか１つの腕についてでもその軌道を求められなかったときには、軌道計画に失敗したとの出力をして処理が終了することになる。或いは、１回目の軌道計画でいずれか１つの腕についての軌道を求められなかったとしても、続く２回目の軌道計画でいずれか１つの腕についてのみの軌道を求め、さらに、続く３回目の軌道計画で他の腕についてのみの軌道を求めるというように順々に軌道を求めていくことも考えられる。しかし、このように１度の軌道計画で確実に軌道を求めることができず複数回にわたって軌道計画を行うものとしては、達成させたい作業についての軌道を最終的に求めるまでに多くの計算時間を要してしまう。
【００１７】
従って、本発明の第１の課題は、達成させたい作業の動作を一度で確実に生成可能とし、短時間で動作生成を実行可能なロボットの動作生成システム及び動作生成方法を提供することを目的とする。
【００１８】
なお、特許文献２に開示される技術では、各アームロボットに対して優先順位を付与して動作を生成するものであるが、複数の並列に配置されたロボットのうちで例えばその中央に配置されているロボットの優先順位を高く設定することで、ロボット間での相互干渉防止のために一時停止する回数を減らして、作業時間が伸びることを防ぐものにすぎない。
【００１９】
次に第２の問題を説明する。ロボットの姿勢をノードとして、初期姿勢から最終姿勢へと至る軌道を複数のノードを登録したグラフ構造を利用して求める際には、グラフ構造に予め登録したノードの個数が環境に対して十分でないために軌道を発見できないことがある。多数のノードを有する大規模なグラフ構造を予め準備しておけば軌道を確実に求めることができるものの、簡単な環境に対してはこのような大規模なグラフ構造は冗長なものとなる。すなわち、簡単な環境下においても大規模なグラフ構造を用いては、初期姿勢から最終姿勢へと至る軌道を発見するまでに一定時間以上の計算時間を要することになってしまう。このため、軌道が発見できることを保証しつつも、簡単な環境においては可能な限り高速に軌道を計画できることが求められる。
【００２０】
これを解決する従来手法としては、特許文献１や非特許文献４に開示される手法のように、軌道が発見できるまでの間、逐次ランダムに（或いは、所定の規則に沿った方法により）新たなノードをグラフ構造に追加することで、グラフ構造を拡張していくという手法がある。
【００２１】
例えば図１４に示す例では、ノード８１〜ノード８９が登録されたグラフ構造８０を用いて、スタートノードＳからゴールノードＧまでのノードの経路を探索した場合に、環境に存在する物体との干渉などを理由として、ノード８３及びノード８８を使用することができず、スタートノードＳからゴールノードＧまでの解（経路）を得ることができない。このため、図１５に示す例では、図１４に示したグラフ構造８０に対してノード９０とそのエッジを追加したことで、ゴールノードＧまでの解を得ている。
【００２２】
しかしながら、逐次ランダムに新たな姿勢（ノード）を作成してグラフ構造に追加すると、実際上は、見た目に不自然な姿勢や、背中に手を回すような使えない姿勢が多数追加されることになり、グラフ構造での探索に時間を要してしまう。また、ノードを新たにグラフ構造に追加する際には、追加したノードの最近傍ノードの検索や、追加に伴ってグラフを頻繁に解く必要が生じるため、計算時間の増加を招いてしまう。
【００２３】
従って、本発明の第２の課題は、軌道の計画可能性を保証しつつ、簡単な環境では短時間で軌道計画が可能なロボットの動作生成システム及び動作生成方法を提供することを目的とする。
【課題を解決するための手段】
【００２４】
本発明に係る第一の態様のロボットの動作生成システムは、複数のアームを備えるロボットと、前記ロボットの周辺の環境の環境情報を取得する環境認識手段と、前記アームの関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するグラフ構造を、前記アームごとに格納するグラフ構造データ格納手段と、前記複数のアームのそれぞれに優先度が設定され、当該設定された優先度に従って、前記グラフ構造データ格納手段に格納されている前記アームごとのグラフ構造を用いて、前記アームの初期姿勢を示すスタートノードから前記アームの最終姿勢を示すゴールノードまでを結ぶ経路を探索するグラフ構造探索処理手段と、前記グラフ構造探索処理手段により探索された経路に含まれるノードについて、当該ノードが示す姿勢が、前記環境認識手段により取得された前記環境情報と干渉するか否かを判定し、干渉していると判定した場合には、当該ノードを前記グラフ構造から除外するノード有効性判定手段と、を備えるものである。
【００２５】
これにより、達成させたい作業の動作を一度で確実に生成可能とし、短時間で動作生成を実行可能とすることができる。
【００２６】
また、前記ロボットは上体と下体とを連結する腰を更に備え、前記グラフ構造データ格納手段は、前記アームの関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するアーム用グラフ構造と、前記アームの関節軸情報及び前記腰の関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するアーム及び腰用グラフ構造とを、前記アームごとにそれぞれ格納し、前記グラフ構造探索処理手段は、最も高い優先度が設定されたアームについての経路を探索する場合には、前記グラフ構造データ格納手段に格納されている前記アーム及び腰用グラフ構造を用いて、前記アーム及び前記腰の初期姿勢を示すスタートノードから前記アーム及び前記腰の最終姿勢を示すゴールノードまでを結ぶ経路を探索するようにしてもよい。これにより、腰との同期を取りながら動作を生成することができる。
【００２７】
さらにまた、前記ロボットに実行させるタスクを管理するタスク管理手段を更に備え、前記タスク管理手段は、前記環境認識手段により取得された前記環境情報に基づいて、前記複数のアームのそれぞれに優先度を設定するようにしてもよい。これにより、アームの優先度を環境情報に基づいて設定することができる。
【００２８】
また、前記環境認識手段により取得された前記環境情報には前記環境内の把持対象物体の位置情報が含まれ、前記タスク管理手段は、前記把持対象物体と前記複数のアームのそれぞれとの位置関係に基づいて、前記複数のアームのそれぞれに優先度を設定するようにしてもよい。これにより、アームの優先度を環境情報に基づいてより簡単に設定することができる。
【００２９】
さらにまた、前記グラフ構造データ格納手段は、前記ノードの個数がそれぞれ異なる前記グラフ構造を前記アームごとに複数格納し、前記グラフ構造探索処理手段は、前記グラフ構造データ格納手段に格納されている前記アームごとの複数のグラフ構造のうちで第１のグラフ構造を用いて、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索し、当該探索した経路に含まれるいずれか１のノードが、前記ノード有効性判定手段により前記環境情報と干渉する姿勢を示すノードであると判定された場合には、前記第１のグラフ構造のノードのうちで前記ノード有効性判定手段により前記環境情報と干渉しない姿勢を示すノードであると判定されたノードと、前記第１のグラフ構造のノードのうちで前記探索がなされなかったノードと、を、前記第１のグラフ構造のノードの個数よりも多い個数のノードを有する第２のグラフ構造へと継承させ、当該第２のグラフ構造を用いて、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索するようにしてもよい。これにより、軌道の計画可能性を保証しつつ、簡単な環境では短時間で軌道計画が可能となる。
【００３０】
本発明に係る第二の態様のロボットの動作生成方法は、複数のアームと、上体と下体とを連結する腰と、を備えるロボットの動作生成方法であって、前記アームの関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するアーム用グラフ構造と、前記アームの関節軸情報及び前記腰の関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するアーム及び腰用グラフ構造とが、前記アームごとにそれぞれグラフ構造データ格納部に格納されており、前記ロボットの周辺の環境の環境情報を取得するステップと、前記複数のアームのそれぞれに優先度が設定され、最も高い優先度が設定されたアームについての経路を探索する場合に、前記グラフ構造データ格納部に格納されている前記アーム及び腰用グラフ構造を用いて、前記アーム及び前記腰の初期姿勢を示すスタートノードから前記アーム及び前記腰の最終姿勢を示すゴールノードまでを結ぶ経路を探索するステップと、２番目に高い又はそれ以降に高い優先度が設定されたアームについての経路を探索する場合に、前記設定された優先度に従って、前記グラフ構造データ格納部に格納されている前記アームごとのグラフ構造を用いて、前記アームの初期姿勢を示すスタートノードから前記アームの最終姿勢を示すゴールノードまでを結ぶ経路を探索するステップと、前記探索された経路に含まれるノードについて、当該ノードが示す姿勢が、前記取得した前記環境情報と干渉するか否かを判定し、干渉していると判定した場合には、当該ノードを前記グラフ構造から除外するステップと、を有するものである。
【００３１】
これにより、達成させたい作業の動作を一度で確実に生成可能とし、短時間で動作生成を実行可能とすることができる。
【００３２】
また、前記グラフ構造データ格納部には、前記ノードの個数がそれぞれ異なる前記グラフ構造が前記アームごとに複数格納されており、前記グラフ構造データ格納部に格納されている前記アームごとの複数のグラフ構造のうちで第１のグラフ構造を用いて、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索するステップと、前記探索した経路に含まれるいずれか１のノードが、前記環境情報と干渉する姿勢を示すノードであるか否かを判定し、干渉すると判定した場合に、前記第１のグラフ構造のノードのうちで前記環境情報と干渉しない姿勢を示すノードであると判定したノードと、前記第１のグラフ構造のノードのうちで前記探索がなされなかったノードと、を、前記第１のグラフ構造のノードの個数よりも多い個数のノードを有する第２のグラフ構造へと継承させるステップと、前記第２のグラフ構造を用いて、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索するステップと、を有するようにしてもよい。これにより、軌道の計画可能性を保証しつつ、簡単な環境では短時間で軌道計画が可能となる。
【発明の効果】
【００３３】
本発明は、上記従来の問題点を解決することができるロボットの動作生成システム及び動作生成方法を提供するものである。すなわち、第１の課題に対しては、達成させたい作業の動作を一度で確実に生成可能とし、短時間で動作生成を実行可能とし、第２の課題に対しては、軌道の計画可能性を保証しつつ、簡単な環境では短時間で軌道計画を可能とする。
【図面の簡単な説明】
【００３４】
【図１】実施の形態１に係るロボットの概略構成を示す全体図である。
【図２】実施の形態１に係るロボットの動作生成システムの構成図である。
【図３】実施の形態１に係るロボットの全身軌道計画部の構成図である。
【図４】実施の形態１に係るグラフ構造の概念図である。
【図５】実施の形態１に係る軌道計画処理のフローチャート図である。
【図６】実施の形態１に係る部位ごとのグラフ構造の概念図である。
【図７】実施の形態１に係る部位ごとのグラフ構造の概念図である。
【図８】実施の形態１に係る第１優先の部位についての軌道探索処理のフローチャート図である。
【図９】実施の形態１に係る第２優先の部位についての軌道探索処理のフローチャート図である。
【図１０】実施の形態２に係る軌道探索処理のフローチャート図である。
【図１１】実施の形態２に係る小規模なグラフ構造の概念図である。
【図１２】実施の形態２に係る大規模なグラフ構造の概念図である。
【図１３】実施の形態３に係る軌道計画処理のフローチャート図である。
【図１４】本発明の課題を説明するための図である。
【図１５】本発明の課題を説明するための図である。
【発明を実施するための形態】
【００３５】
実施の形態１．
以下、図面を参照して本発明の実施の形態について説明する。なお、以下では、本文中の説明においては、必要に応じそれ以前に述べた符号を用いるものとする。
【００３６】
図１は、本実施の形態に係るロボットのリンク構造の概略構成を示す全体図である。図１（ａ）は正面図を示し、図１（ｂ）は右側面図を示し、図１（ｃ）は左側面図を示す。なお、図に示す例では、ロボット１と路面との接触地点にロボット基準座標を定め、ロボット１の首、右手先、左手先にそれぞれ局所座標を定めている。
【００３７】
図１に示すように、ロボット１は、頭部と、胴体と、胴体に連結された右アーム及び左アームと、各アームに連結された右ハンド及び左ハンドと、を備えている。胴体は、上体と、腰と、腰を介して上体に連結された下体と、を備えている。右アームは、複数の関節（Ｒ１〜Ｒ７）と、これら関節を介して接続される複数のリンクと、を備えている。左アームは、複数の関節（Ｌ１〜Ｌ７）と、これら関節を介して接続される複数のリンクと、を備えている。腰は、腰関節Ｗ１と、関節を介して上体及び下体と接続する複数のリンクと、を備えている。なお、上体と頭部を接続する首関節については図示を省略した。また、詳細についての図示は省略するが、各ハンドは、掌部と、関節を介して掌部に接続される複数本の指と、を備えていてもよい。また、各指は、複数の指関節と、指関節を介して接続される複数のリンクと、を備えていてもよい。
【００３８】
各関節に備えられたモーターなどのアクチュエータ（不図示）が駆動されることで、各関節は、所望の角度及び角速度で動作する。その結果、各座標系において、ロボット１の全身の位置及び姿勢と、手先の位置及び姿勢と、を所望の位置及び姿勢に制御することができる。このように、アクチュエータは、ロボット１が動作する際の、動力発生手段に相当する。
【００３９】
ロボット１は、ユーザから指定される様々なタスクを実行するために動作を生成する。タスクとは、環境内に存在する物体に対する作業であり、当該作業は、作業を実現するための一連の動作を含んでいる。例えば、環境内に存在する物体を、ハンドにより把持するなどのタスクが挙げられる。ロボット１は、このようなタスクを実行するために右アームと、左アームと、腰と、の軌道を計画する。そして、計画した軌道に追従するように各関節を駆動制御することで、動作を生成する。なお、軌道とは、初期姿勢を示す関節角度ベクトルから最終姿勢を示す関節角度ベクトルへと至る関節角度ベクトルの系列であり、後述するグラフ構造を用いて求められる。
【００４０】
図２は、本実施の形態に係るロボットの動作生成システムの構成図である。動作生成システム１０は、環境認識部１１と、タスク管理部１２と、最終全身姿勢決定部１３と、初期全身位置・姿勢取得部１４と、全身軌道計画部１５と、補間部１６と、モーター駆動部１７と、を備えている。
【００４１】
環境認識部１１は、ロボット１の周辺の環境の環境情報を取得する。環境認識部１１は、各種のセンサを用いて構成され、ロボット１の周辺の環境内に存在する物体の位置や形状などの環境情報を取得する。センサには、例えば、ステレオカメラやレーザーレンジファインダを用いることができ、環境認識部１１は、環境内の物体について３次元の点群情報を取得し、ロボット１から物体までの距離情報を取得することができる。環境認識部１１は、取得した物体の距離情報に基づいて、環境内の物体を、例えば、直方体形状のボクセル情報や、３次元の物体モデルとして生成し、環境情報としてタスク管理部１２と、最終全身姿勢決定部１３と、全身軌道計画部１５と、に出力する。
【００４２】
タスク管理部１２は、ロボット１に実行させるタスクを管理する。タスク管理部１２は、タスクの内容に応じて、環境認識部１１で取得した環境情報から必要な情報を取得する。そして、タスク管理部１２は、タスクの内容と環境情報とに基づいて、各部位に優先度を設定し、部位の優先度を含むタスク情報を出力する。なお、タスクの内容や各部位の優先度は、動作生成システム１０に備えられたインタフェース（不図示）を介してユーザにより入力されるものとしてもよい。
【００４３】
例えば、物体を把持するタスクでは、ロボット１に把持させる物体、その把持方法などを示す情報がタスクに含まれており、タスク管理部１２は、環境認識部１１で取得した環境情報から、把持対象となる物体を示す情報と、その形状や位置・姿勢などを示す情報などを取得する。そして、タスク管理部１２は、例えば、ロボット１の各部位と把持対象物体との位置関係に応じて、把持に際しての各部位の優先度を設定する。
【００４４】
例えば、物体を把持するタスクにおいて、左アームのハンドに比べて右アームのハンドのほうが把持対象物体により近い場合には、タスク管理部１２は、右アームの優先度を「１」とし、左アームの優先度を「２」として設定する（数字が小さいほど優先度が高く、「１」が最高）。
【００４５】
また、例えば、物体を把持するタスクにおいて、右アームのハンドで必ず物体を把持し、左アームのハンドでは可能であれば把持して欲しいという状況では、ユーザにより、右アームの優先度を「１」とし、左アームの優先度を「２」として設定するようにしてもよい。このような優先度設定の具体例としては、コップとスプーンという２つの把持対象物体が机上に存在した場合に、右手では必ずコップを把持し、左手では可能であれば同時にスプーンを把持して欲しい状況や、コップの置かれたトレイを右手で把持した状態で、可能であれば左手で他のコップを把持してそのトレイに置いて欲しい状況などが挙げられる。
【００４６】
最終全身姿勢決定部１３は、環境情報と、タスク管理部１２で求められたタスク情報から、タスク実行時のロボット１の最終全身姿勢を決定する。
【００４７】
初期全身位置・姿勢取得部１４は、タスク管理部１２から出力されたタスク情報から、タスク実行時のロボット１の初期全身位置・姿勢を取得する。なお、初期全身位置・姿勢は、ロボット１の現在の位置・姿勢をそのまま与えるものとしてもよい。
【００４８】
全身軌道計画部１５は、環境情報と、部位の優先度を含むタスク情報と、最終全身姿勢と、初期全身位置・姿勢と、に基づいて、ロボット１の全身の軌道計画を行う。軌道計画は各部位についての軌道を探索することで行われ、初期全身位置・姿勢から最終全身姿勢へと至る軌道であって、かつ、環境と干渉しない軌道が計画される。なお、全身軌道計画部１５の詳細については後述する。
【００４９】
補間部１６は、全身軌道計画部１５で求められた全身関節軌道を、モーターを駆動する制御周期に合わせて適切となるように補間する。補間部１６により、全身軌道計画部１５で求めた軌道と比較してより滑らかな軌道が求められる。補間部１６は、その軌道に含まれる関節角度ベクトルに基づいてロボット１のアクチュエータ（モーター）へのモーター指令値を生成する。
【００５０】
モーター駆動部１７は、補間部１６で求められたモーター指令値に基づいて、フィードバック制御を行う。
【００５１】
次に、図３乃至図９を参照して、全身軌道計画部１５の詳細について説明する。
図３は、全身軌道計画部１５の構成図である。全身軌道計画部１５は、グラフ構造データ格納部１５１と、探索用グラフ構造データ保持部１５２と、グラフ構造生成処理部１５３と、グラフ構造探索処理部１５４と、ノード有効性判定処理部１５５と、を備えている。
【００５２】
グラフ構造データ格納部１５１は、複数のグラフ構造を格納しており、本実施の形態では、部位ごとのグラフ構造を格納している。本実施の形態では、ロボット１の全身の全ての関節軸を表現したグラフ構造を利用して軌道計画を行うことに代えて、部位ごとに用意したグラフ構造を利用して、部位ごとに軌道を計画する。そして、各部位に設定された優先度に従って軌道計画を行うことで、達成させたい作業の動作を一度で確実に生成可能にすると共に、探索範囲を限定することにより短時間で動作生成を実行可能とする。なお、詳細は後述するように、部位のうち腰については、優先度の最も高い部位のグラフ構造に含めて、その軌道を計画する。例えば、ロボット１が腰関節を有した双腕ロボットである場合には、グラフ構造データ格納部１５１に格納されるグラフ構造には、右アーム用のグラフ構造、左アーム用のグラフ構造、右アーム及び腰用のグラフ構造、左アーム及び腰用のグラフ構造、が含まれる。
【００５３】
図４は、グラフ構造の概念図である。
グラフ構造２０は、複数のノード２１〜２９と、ノード間を接続する線分（以下、エッジと称する場合がある。）と、を有している。グラフ構造２０を用いて、スタートノードＳとゴールノードＧとを結ぶ経路が探索される。グラフ構造のノード構成は、タスクの内容と、各部位の優先度と、を考慮してユーザにより設定されており、各ノードは、ユーザにより予め注意深く選ばれている。
【００５４】
なお、ノードｑｎは、以下の数（１）に示すように、アームの関節軸θ（アームが複数の関節軸を有する場合には、θはベクトルを示す。）と、手先位置ｘと、重み行列Ｗの情報を持つ。重み行列Ｗは、動作を実現するに際して各関節の重要度を反映させるための情報であり、各関節の配置関係に基づいて定められる。例えば、アームの関節のうちで、手首関節に比べてより大きな重み付けが肘関節に設定される。また、ノードが持つ情報は、少なくともアームの関節軸θの情報を備えていればよく、手先座標ｘについては必須ではない。
【数１】
また、エッジｃは、以下の数（２）に示すように、ノード間の距離情報を持つ。
【数２】
【００５５】
探索用グラフ構造データ保持部１５２は、軌道探索に用いるグラフ構造を一時的に保持する。例えば、軌道探索に使用中のグラフ構造を探索用グラフ構造データ保持部１５２に一時的に保持する。また、探索した軌道を探索用グラフ構造データ保持部１５２に保持しておき、ロボット１が移動した場合に、保持しているグラフ構造を更新する、などのために探索用グラフ構造データ保持部１５２を利用してもよい。
【００５６】
グラフ構造生成処理部１５３は、優先度に従って軌道を探索する部位を特定し、グラフ構造データ格納部１５１に格納されているグラフ構造から軌道探索対象となる部位のグラフ構造を選択し、選択したグラフ構造を探索用グラフ構造データ保持部１５２に保持する。そして、グラフ構造生成処理部１５３は、入力されるスタートノードとゴールノードを探索用グラフ構造データ保持部１５２に保持されているグラフ構造に追加し、追加したそれぞれのノードについて近傍ノードとの間をエッジにより接続する。ここで、スタートノードは、ロボット１の初期全身位置・姿勢における軌道探索部位の位置・姿勢を示すノードであり、ゴールノードは、ロボット１の最終全身姿勢における軌道探索部位の姿勢を示すノードである。なお、スタートノードは、軌道探索部位の姿勢のみを示すノードであってもよい。
【００５７】
グラフ構造探索処理部１５４は、探索用グラフ構造データ保持部１５２に保持されているグラフ構造を用いて、スタートノードからゴールノードまでを結ぶ経路（スタートノードからゴールノードへと至るノードの経路であり、軌道と称する場合がある。）を探索する。なお、グラフ構造探索処理部１５４による探索方法については限定されず、ダイクストラ法などの公知の手法を用いればよい。
【００５８】
ノード有効性判定処理部１５５は、グラフ構造探索処理部１５４により探索された経路に含まれるノードについて、ノードが示す姿勢が、環境内の物体やロボット１自身と干渉していないか否かをチェックし、干渉している場合には、探索用グラフ構造データ保持部１５５のグラフ構造から当該ノードを除外する。
【００５９】
図５は全身軌道計画部１５による軌道計画処理のフローチャート図である。
まず、全身軌道計画部１５は、最も高い優先度が設定されたアームを、第１優先のアーム（腕）として決定する（Ｓ１０１）。例えば、物体と手先との距離などを基準として、手先の位置が物体により近いアームについて最も高い優先度が設定される。
【００６０】
全身軌道計画部１５は、第１優先のアームと腰についての軌道を探索する（Ｓ１０２）。全身軌道計画部１５は、第１優先のアームと腰用のグラフ構造を用いて軌道を探索する。図６は、第１優先のアームと腰用のグラフ構造を例示する概念図である。グラフ構造３０は、複数のノード３１〜３９と、ノード間を接続するエッジと、を有している。ここで、ノード３１〜３９の各ノードは、第１優先のアームの関節軸及び手先位置と、腰の関節軸と、の情報を有している。なお、Ｓ１０２での処理の詳細については後述する。
【００６１】
全身軌道計画部１５は、Ｓ１０２での軌道探索の結果、第１優先のアームと腰についての有効な軌道が存在するか否かを判定する（Ｓ１０３）。軌道が存在しない場合（Ｓ１０３で「無」の場合）には、第１優先のアームと腰についての軌道を探索することができなかったとして、処理を終了する。なお、有効な軌道とは、スタートノードからゴールノードへと至るノードの経路であって、この経路に含まれる全てのノードが干渉しないものをいう。また、スタートノードからゴールノードへと至るノードの経路は複数探索される可能性があるが、経路の評価基準に従って、最も良い評価基準の経路を第１候補軌道とする（例えば、経路上のノード間距離の総和が最小の経路を、第１候補軌道とする）。
【００６２】
全身軌道計画部１５は、第１優先のアームと腰についての有効な軌道が存在した場合（Ｓ１０３で「有」の場合）には、第２優先のアームについての軌道を探索する（Ｓ１０４）。全身軌道計画部１５は、第２優先のアーム用のグラフ構造を用いて軌道を探索する。図７は、第２優先のアームのグラフ構造を例示する概念図である。グラフ構造４０は、複数のノード４１〜４９と、ノード間を接続するエッジと、を有している。ここで、ノード４１〜４９の各ノードは、第２優先のアームの関節軸及び手先位置の情報を有している。なお、Ｓ１０４での処理の詳細については後述する。
【００６３】
全身軌道計画部１５は、Ｓ１０４での軌道探索の結果、第２優先のアームについての有効な軌道が存在するか否かを判定する（Ｓ１０５）。有効な軌道が存在した場合（Ｓ１０５で「有」の場合）には、Ｓ１０２で探索した第１優先のアームと腰の軌道と、Ｓ１０４で探索した第２優先のアームの軌道と、を出力する。
【００６４】
全身軌道計画部１５は、Ｓ１０４での軌道探索の結果、第２優先のアームについての有効な軌道が存在しない場合（Ｓ１０５で「無」の場合）には、再びＳ１０２へと戻り、第１優先のアームと腰についての軌道を探索するが、Ｓ１０２で探索された経路のうち、２番目に評価基準が良かった軌道が存在する場合には、当該軌道を採用してＳ１０４へと進み、存在しない場合には、第１優先のアームと腰と、第２優先のアームと、のいずれについても軌道を探索することができなかったとして処理を終了する。
【００６５】
従って、図５に示した軌道計画処理によれば、全身軌道計画部１５による出力は、（ｉ）第１優先のアームと腰の軌道と、第２優先のアームの軌道と、の両方の出力、（ｉｉ）第１優先のアームと腰についての軌道を探索することができなかったとの出力、（ｉｉｉ）第１優先のアームと腰と、第２優先のアームと、のいずれについても軌道を探索することができなかったとの出力、となる。
【００６６】
次に、図８を参照して、図５に示したＳ１０２での処理の詳細について説明する。
まず、グラフ構造生成処理部１５３は、第１優先のアーム（腰を含む）についてのグラフ構造を生成する（Ｓ２０１）。グラフ構造生成処理部１５３は、入力されるスタートノードとゴールノードをグラフ構造に追加すると共に、追加したノードについて近傍ノードとの間をエッジにより接続する（Ｓ２０２）。
【００６７】
グラフ構造探索処理部１５４は、第１優先のアームについてのグラフ構造を用いて、スタートノードからゴールノードまでを結ぶ経路を探索する（Ｓ２０３）。グラフ構造探索処理部１５４は、軌道探索の結果、軌道が存在するか否かを判定する（Ｓ２０４）。軌道が存在しない場合（Ｓ２０４で「無」の場合）には、軌道を探索することができなかったとして、処理を終了する。
【００６８】
ノード有効性判定処理部１５５は、軌道が存在した場合（Ｓ２０４で「有」の場合）には、探索された経路に含まれる全てのノードについて、ノードが示す姿勢が、環境内の物体やロボット１自身と干渉していないか否かをチェックし（Ｓ２０５）、干渉の有無を検出する（Ｓ２０６）。なお、第１優先のアームについての干渉チェックを行う際には、第２優先のアームを除外したロボット１の形状（第１優先のアーム、胴体、頭部など）に対してのみ干渉チェックを行えばよい。
【００６９】
ノード有効性判定処理部１５５は、干渉検出の結果、経路に含まれるいずれか１つのノードが干渉している場合（Ｓ２０６で「ＮＧ」の場合）には、グラフ構造から当該ノードを除外する（Ｓ２０７）。グラフ構造探索処理部１５４により、ノード除外後のグラフ構造を用いて再び軌道探索を行う。
【００７０】
ノード有効性判定処理部１５５は、干渉検出の結果、経路に含まれるいずれのノードも干渉していない場合（Ｓ２０６で「ＯＫ」の場合）には、第１優先のアームについての軌道を出力する。
【００７１】
次に、図９を参照して、図５に示したＳ１０４での処理の詳細について説明する。ただし、Ｓ１０４での第２優先のアームについての軌道探索処理は、Ｓ１０２での第１優先のアームについての軌道探索処理と基本的には同じであり、図８を参照して説明したＳ２０５での干渉チェックについてのみ異なる。このため、以下では、Ｓ１０４での第２優先のアームについての軌道探索処理に関して、図９を参照して、干渉チェックの詳細についてのみを説明する。
【００７２】
ノード有効性判定処理部１５５は、繰り返し回数Ｎを決定する（Ｓ３０１）。繰り返し回数Ｎは、以下のＳ３０３乃至Ｓ３０７の繰り返し回数である。本実施の形態では、第１優先のアームについて探索した経路に含まれるノードの個数をＬ、第２優先のアームについて探索した経路に含まれるノードの個数をＭとしたとき、ＬとＭで値が大きなものをＮの値として設定する。
【００７３】
ノード有効性判定処理部１５５は、カウンタｎの値を初期化する（Ｓ３０２）。ここでは、カウンタｎ＝１とする。ノード有効性判定処理部１５５は、カウンタｎの値がＭより大きいか否かを判定する（Ｓ３０３）。ノード有効性判定処理部１５５は、Ｓ３０３での判定の結果、カウンタｎの値がＭより大きくない場合（Ｓ３０３でＮｏの場合）には、カウンタｎの値がＬより小さいか否かを判定する（Ｓ３０４）。
【００７４】
ノード有効性判定処理部１５５は、Ｓ３０４での判定の結果、カウンタｎの値がＬより大きくない場合（Ｓ３０３でＮｏの場合）には、第１優先のアーム（腰を含む）の探索経路についてｎ番目のノードが示す姿勢において、第２優先のアームの探索経路についてｎ番目のノードが示す姿勢の干渉チェックを行う（Ｓ３０５）。腰関節の駆動に伴ってロボット１の上体の姿勢が変化するため、上体に設けられた第２優先のアームについてもその姿勢が相対的に変化する。このため、第１優先のアームについて探索された経路に含まれる腰の姿勢を基準として、第２優先のアームの探索経路のノードが示す姿勢についての干渉チェックを行う。
【００７５】
ノード有効性判定処理部１５５は、Ｓ３０３での判定の結果、カウンタｎの値がＭより大きい場合（Ｓ３０３でＹｅｓの場合）には、第１優先のアーム（腰を含む）の探索経路についてｎ番目のノードが示す姿勢において、第２優先のアームの探索経路についてＭ番目のノードが示す姿勢の干渉チェックを行う（Ｓ３０８）。
【００７６】
ノード有効性判定処理部１５５は、Ｓ３０４での判定の結果、カウンタｎの値がＬより大きい場合（Ｓ３０４でＹｅｓの場合）には、第１優先のアーム（腰を含む）の探索経路についてＬ番目のノードが示す姿勢において、第２優先のアームの探索経路についてｎ番目のノードが示す姿勢の干渉チェックを行う（Ｓ３０９）。
【００７７】
ノード有効性判定処理部１５５は、カウンタｎが繰り返し回数Ｎに到達した否かを判定し（Ｓ３０６）、到達した場合（Ｓ３０６でＹｅｓの場合）には処理を終了する。到達していない場合には、カウンタｎの値を１増加させ（Ｓ３０７）、Ｓ３０３へと戻る。
【００７８】
図９に示したように、第１優先のアーム（腰を含む）の探索経路のノードと、第２優先のアームの探索経路のノードと、について対応するノードの干渉チェックを行うことで、アームごとのグラフ構造を用いた場合であっても、共に干渉していない両アームの動作を求めることができる。そして、第１優先のアームの探索経路は腰についての関節軸情報を含んでおり、第２優先のアームの探索経路は第１優先のアームの探索経路に対応したものであるため、結果として、作業を行うアーム（第１優先のアーム及び第２優先のアーム）と、腰との同期が取れた動作を生成することができる。
【００７９】
次に、本実施の形態による効果について説明する。
まず、ロボット１が、Ｌ個の関節軸を含む右アームと、Ｍ個の関節軸を含む左アームと、を備える双腕ロボットであり、全部でＮ個の関節軸を有する場合に、一つの関節軸のとりうる範囲をＸ分割すると全ての関節軸の組み合わせはＸＮ通りとなり、ある姿勢を探索する範囲は、Ｎ乗に比例して大きくなる。これに対して、探索範囲を部位（アーム）ごとに分割して軌道探索することで、探索範囲はＸＬ＋ＸＭとなり、全ての関節軸を一度に探索するのと比べて、探索範囲を大幅に減少させることができる。また、グラフ構造を利用して軌道を探索する場合に、探索範囲が小さければ同じ分解能であってもノードの個数を減らすことができるため、計算時間を短縮することができる。
【００８０】
また、実際にタスクを行う部位（優先度の高いアーム）について腰関節を含むグラフ構造を用いて軌道探索を行った上で、他の部位（より優先度の低いアーム）についての軌道探索を行うため、腰との同期を含めてタスクを行わせたい部位（両アーム）の同期が可能である。また、双腕による作業の場合においても、いずれかのアームを第１優先の腕とすることで対応することができる。
【００８１】
ロボット１に達成させたい作業（タスク）には複雑な作業が多く存在し、例えば、右手では必ず作業を行い、左手では可能であれば実行して欲しいという作業がある。本実施の形態では、各部位に優先度を設定して軌道探索を行うことで、優先度の高い部位についての軌道を実現した上で、優先度のより低い部位の軌道を実現するように軌道計画（動作生成）を行う。例えば、部位の優先度として、右アームに最も高い優先度を設定し、左アームに右アームよりも低い優先度を設定した場合（右アームで必ず作業を行い、できれば左アームでも作業を行って欲しい場合）には、（ｉ）右アームでの作業を実現させる軌道と、右アームでの作業を実現させる軌道を前提とした左アームでの作業を実現させる軌道、（ｉｉ）必ず実現させたかった右アームでは軌道を求めることができなかった、（ｉｉｉ）両アーム共に軌道を求めることができなかった、という出力を得ることができる。このため、従来手法と比較して、複雑な作業に対しても１度の軌道計画で解（軌道）を求められる可能性が高く、達成させたい作業の動作を一度でより確実に生成可能とし、より短時間で動作生成を実行することができる。
【００８２】
実施の形態２．
次に、本発明の実施の形態２について説明する。上述の実施の形態においては、ノード構成（ノードの個数、エッジなど）が固定されたグラフ構造を利用して軌道を探索するものであったが、本実施の形態においては、軌道探索の結果を継承させながらノード構成を変化させるグラフ構造を利用して軌道を探索するものである。なお、図１に示したロボット１の概略構成や、図２に示した動作生成システム１０の構成及び処理については上述した実施の形態１と同一であるためその詳細な説明を省略し、以下では、実施の形態１と異なる点を中心に説明する。
【００８３】
図１０乃至図１２を参照して、本実施の形態による軌道探索処理について説明する。
図１０は、規模の異なる複数のグラフ構造を用いた軌道探索処理のフローチャート図である。本実施の形態では、グラフ構造データ格納部１５１は、ノードの規模が異なる複数のグラフ構造を部位ごとに格納しており、ここでは、ノード数が比較的少ない小規模なグラフ構造と、その小規模なグラフ構造を含み、かつ、ノード数が比較的多い大規模なグラフ構造と、の２つのグラフ構造を有しているものとして説明する。また、図に示す軌道探索処理が、第１優先の部位（腰を含む）、第２優先の部位のいずれについてもそれぞれ適用される。
【００８４】
本実施の形態に係る全身軌道計画部１５は、小規模なグラフ構造を用いて軌道を探索する（Ｓ４０１）。なお、Ｓ４０１での軌道探索処理には、ノードの干渉チェックが含まれている。次いで、Ｓ４０１での軌道探索の結果、有効な軌道が存在するか否かを判定し（Ｓ４０２）、軌道が存在する場合（Ｓ４０２で「有」の場合）には、その軌道を出力して処理を終了する。
【００８５】
Ｓ４０１での軌道探索の結果、軌道探索の結果、有効な軌道が存在しない場合には（Ｓ４０２で「無」の場合）には、Ｓ４０１での軌道探索結果を、大規模なグラフ構造に継承させる（Ｓ４０３）。継承させる軌道探索結果には、Ｓ４０１で用いた小規模なグラフ構造のノードのうち、軌道探索の際に探索されなかったノードと、干渉チェックの結果、干渉しないと判定されたノードと、を示す情報と、が含まれる。
【００８６】
次いで、小規模なグラフ構造による軌道探索結果を継承させた大規模なグラフ構造を用いて、軌道を探索する（Ｓ４０４）。次いで、Ｓ４０４での軌道探索の結果、有効な軌道が存在するか否かを判定する（Ｓ４０５）。判定の結果、軌道が存在する場合（Ｓ４０５で「有」の場合）には、その軌道を出力して処理を終了し、軌道が存在しない場合（Ｓ４０５で「無」の場合）には、軌道を探索することができなかったとして処理を終了する。
【００８７】
図１１及び図１２を参照して、図１０に示した処理の具体例を説明する。
図１１は、小規模なグラフ構造を例示する概念図である。図１２は、大規模なグラフ構造を例示する概念図である。グラフ構造５０を用いて軌道を探索した結果、ノード５３及びノード５８が干渉すると判定され、有効な軌道を探索することができないと判定された。このため、グラフ構造６０を用いて軌道が探索される結果、ノード７４を介することで、スタートノードＳからゴールノードＧまでの経路を探索することができる。
【００８８】
以上説明したように、本実施の形態では、まずは比較的小規模なグラフ構造を用いて軌道を探索し、軌道を求められない場合には、軌道探索結果を継承させたより大規模なグラフ構造を用いて軌道を探索する。このように、前回の軌道探索結果を継承させながら徐々に複雑なグラフ構造へと切替えていくことで、軌道の計画可能性を保証しつつ、簡単な環境では短時間で軌道計画が可能となる。
【００８９】
例えば、障害物が特に存在しない環境下で机上に置かれた物体までアームを伸ばすといった比較的簡単な状況下では、アームの軌道を瞬時に探索できることが求められる。グラフ構造を用いて軌道を探索する手法では、ノード数及びエッジ数が少ないほど高速に計算が可能であるため、このような環境下では小規模なグラフ構造を利用して軌道を探索することが好ましい。一方で、目標物体の周囲に多数の障害物が存在するような複雑な環境下で、軌道探索に最初に用いたグラフ構造のノード数などが不十分であったために、物体との干渉チェックにより多くのノードが無効となり、この結果、初期姿勢から最終姿勢までのアーム軌道を探索できないことがある。
【００９０】
従って、本実施の形態では、最初に用いる小規模なグラフ構造で軌道が求まらない場合には、ノード数やエッジ数がより多い大規模なグラフ構造に切替えて探索を行う。大規模なグラフ構造では小規模なグラフ構造に比べると計算時間を要するものの、解（軌道）が求まる可能性が高いためである。そして、大規模なグラフ構造は、その前の探索で用いた小規模なグラフ構造による探索結果を包含するため、干渉チェックにより干渉しないと判定されたノードについては以降の干渉チェックを省き、干渉すると判定されたノードについては大規模グラフ構造から予め省くことで、軌道の計画可能性を保証しつつ、より短時間で軌道計画が可能となる。
【００９１】
なお、本実施の形態では、小規模なグラフ構造と大規模なグラフ構造の２つのグラフ構造を用いた場合を例に説明したが、規模のそれぞれ異なる３以上のグラフ構造をグラフ構造データ格納部１５１に格納し、順次、より規模の大きなグラフ構造に切替えて軌道を探索するものとしてもよい。
【００９２】
実施の形態３．
次に、本発明の実施の形態３について説明する。
図１３は、実施の形態３に係る全身軌道計画部１５による軌道計画処理のフローチャート図である。実施の形態３では、実施の形態１の図５に示した処理と同一の処理を行うが、Ｓ５０１及びＳ５０３では、実施の形態２の図１０で示した処理を行う。すなわち、Ｓ５０１及びＳ５０３での軌道探索処理は、まずは比較的小規模なグラフ構造を用いて軌道を探索し、軌道を求められない場合には、軌道探索結果を継承させたより大規模なグラフ構造を用いて軌道を探索することで行われる。
【００９３】
その他の実施の形態．
上述した実施の形態では、ロボット１が右アーム及び左アームを有する双腕ロボットである場合を例として説明したが、３本以上のアームを有し、各アームに対して優先度を設定するものとしてもよい。
【００９４】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
【符号の説明】
【００９５】
１ ロボット、
Ｒ１〜Ｒ７、Ｌ１〜Ｌ７ 関節、
１０ 動作生成システム、
１１ 環境認識部、
１２ タスク管理部、
１３ 最終全身姿勢決定部、
１４ 初期全身位置・姿勢取得部、
１５ 全身軌道計画部、
１５１ グラフ構造データ格納部、
１５２ 探索用グラフ構造データ保持部、
１５３ グラフ構造生成処理部、
１５４ グラフ構造探索処理部、
１５５ ノード有効性判定処理部、
１６ 補間部、
１７ モーター駆動部、
２０、３０、４０、５０、６０、８０ グラフ構造、
２０〜２９、３０〜３９、４０〜４９、５０〜５９、６０〜７７、８１〜９０ ノード、
Ｓ スタートノード、
Ｇ ゴールノード
【特許請求の範囲】
【請求項１】
複数のアームを備えるロボットと、
前記ロボットの周辺の環境の環境情報を取得する環境認識手段と、
前記アームの関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するグラフ構造を、前記アームごとに格納するグラフ構造データ格納手段と、
前記複数のアームのそれぞれに優先度が設定され、当該設定された優先度に従って、前記グラフ構造データ格納手段に格納されている前記アームごとのグラフ構造を用いて、前記アームの初期姿勢を示すスタートノードから前記アームの最終姿勢を示すゴールノードまでを結ぶ経路を探索するグラフ構造探索処理手段と、
前記グラフ構造探索処理手段により探索された経路に含まれるノードについて、当該ノードが示す姿勢が、前記環境認識手段により取得された前記環境情報と干渉するか否かを判定し、干渉していると判定した場合には、当該ノードを前記グラフ構造から除外するノード有効性判定手段と、
を備えるロボットの動作生成システム。
【請求項２】
前記ロボットは上体と下体とを連結する腰を更に備え、
前記グラフ構造データ格納手段は、
前記アームの関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するアーム用グラフ構造と、前記アームの関節軸情報及び前記腰の関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するアーム及び腰用グラフ構造とを、前記アームごとにそれぞれ格納し、
前記グラフ構造探索処理手段は、
最も高い優先度が設定されたアームについての経路を探索する場合には、
前記グラフ構造データ格納手段に格納されている前記アーム及び腰用グラフ構造を用いて、前記アーム及び前記腰の初期姿勢を示すスタートノードから前記アーム及び前記腰の最終姿勢を示すゴールノードまでを結ぶ経路を探索する
ことを特徴とする請求項１に記載のロボットの動作生成システム。
【請求項３】
前記ロボットに実行させるタスクを管理するタスク管理手段を更に備え、
前記タスク管理手段は、
前記環境認識手段により取得された前記環境情報に基づいて、前記複数のアームのそれぞれに優先度を設定する
ことを特徴とする請求項１又は２に記載のロボットの動作生成システム。
【請求項４】
前記環境認識手段により取得された前記環境情報には前記環境内の把持対象物体の位置情報が含まれ、
前記タスク管理手段は、
前記把持対象物体と前記複数のアームのそれぞれとの位置関係に基づいて、前記複数のアームのそれぞれに優先度を設定する
ことを特徴とする請求項３に記載のロボットの動作生成システム。
【請求項５】
前記グラフ構造データ格納手段は、
前記ノードの個数がそれぞれ異なる前記グラフ構造を前記アームごとに複数格納し、
前記グラフ構造探索処理手段は、
前記グラフ構造データ格納手段に格納されている前記アームごとの複数のグラフ構造のうちで第１のグラフ構造を用いて、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索し、当該探索した経路に含まれるいずれか１のノードが、前記ノード有効性判定手段により前記環境情報と干渉する姿勢を示すノードであると判定された場合には、前記第１のグラフ構造のノードのうちで前記ノード有効性判定手段により前記環境情報と干渉しない姿勢を示すノードであると判定されたノードと、前記第１のグラフ構造のノードのうちで前記探索がなされなかったノードと、を、前記第１のグラフ構造のノードの個数よりも多い個数のノードを有する第２のグラフ構造へと継承させ、当該第２のグラフ構造を用いて、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索する
ことを特徴とする請求項１乃至４いずれか１項に記載のロボットの動作生成システム。
【請求項６】
複数のアームと、上体と下体とを連結する腰と、を備えるロボットの動作生成方法であって、
前記アームの関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するアーム用グラフ構造と、前記アームの関節軸情報及び前記腰の関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するアーム及び腰用グラフ構造とが、前記アームごとにそれぞれグラフ構造データ格納部に格納されており、
前記ロボットの周辺の環境の環境情報を取得するステップと、
前記複数のアームのそれぞれに優先度が設定され、最も高い優先度が設定されたアームについての経路を探索する場合に、前記グラフ構造データ格納部に格納されている前記アーム及び腰用グラフ構造を用いて、前記アーム及び前記腰の初期姿勢を示すスタートノードから前記アーム及び前記腰の最終姿勢を示すゴールノードまでを結ぶ経路を探索するステップと、
２番目に高い又はそれ以降に高い優先度が設定されたアームについての経路を探索する場合に、前記設定された優先度に従って、前記グラフ構造データ格納部に格納されている前記アームごとのグラフ構造を用いて、前記アームの初期姿勢を示すスタートノードから前記アームの最終姿勢を示すゴールノードまでを結ぶ経路を探索するステップと、
前記探索された経路に含まれるノードについて、当該ノードが示す姿勢が、前記取得した前記環境情報と干渉するか否かを判定し、干渉していると判定した場合には、当該ノードを前記グラフ構造から除外するステップと、
を有するロボットの動作生成方法。
【請求項７】
前記グラフ構造データ格納部には、
前記ノードの個数がそれぞれ異なる前記グラフ構造が前記アームごとに複数格納されており、
前記グラフ構造データ格納部に格納されている前記アームごとの複数のグラフ構造のうちで第１のグラフ構造を用いて、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索するステップと、
前記探索した経路に含まれるいずれか１のノードが、前記環境情報と干渉する姿勢を示すノードであるか否かを判定し、干渉すると判定した場合に、前記第１のグラフ構造のノードのうちで前記環境情報と干渉しない姿勢を示すノードであると判定したノードと、前記第１のグラフ構造のノードのうちで前記探索がなされなかったノードと、を、前記第１のグラフ構造のノードの個数よりも多い個数のノードを有する第２のグラフ構造へと継承させるステップと、
前記第２のグラフ構造を用いて、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索するステップと、
を有することを特徴とする請求項６に記載のロボットの動作生成方法。
【請求項１】
複数のアームを備えるロボットと、
前記ロボットの周辺の環境の環境情報を取得する環境認識手段と、
前記アームの関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するグラフ構造を、前記アームごとに格納するグラフ構造データ格納手段と、
前記複数のアームのそれぞれに優先度が設定され、当該設定された優先度に従って、前記グラフ構造データ格納手段に格納されている前記アームごとのグラフ構造を用いて、前記アームの初期姿勢を示すスタートノードから前記アームの最終姿勢を示すゴールノードまでを結ぶ経路を探索するグラフ構造探索処理手段と、
前記グラフ構造探索処理手段により探索された経路に含まれるノードについて、当該ノードが示す姿勢が、前記環境認識手段により取得された前記環境情報と干渉するか否かを判定し、干渉していると判定した場合には、当該ノードを前記グラフ構造から除外するノード有効性判定手段と、
を備えるロボットの動作生成システム。
【請求項２】
前記ロボットは上体と下体とを連結する腰を更に備え、
前記グラフ構造データ格納手段は、
前記アームの関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するアーム用グラフ構造と、前記アームの関節軸情報及び前記腰の関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するアーム及び腰用グラフ構造とを、前記アームごとにそれぞれ格納し、
前記グラフ構造探索処理手段は、
最も高い優先度が設定されたアームについての経路を探索する場合には、
前記グラフ構造データ格納手段に格納されている前記アーム及び腰用グラフ構造を用いて、前記アーム及び前記腰の初期姿勢を示すスタートノードから前記アーム及び前記腰の最終姿勢を示すゴールノードまでを結ぶ経路を探索する
ことを特徴とする請求項１に記載のロボットの動作生成システム。
【請求項３】
前記ロボットに実行させるタスクを管理するタスク管理手段を更に備え、
前記タスク管理手段は、
前記環境認識手段により取得された前記環境情報に基づいて、前記複数のアームのそれぞれに優先度を設定する
ことを特徴とする請求項１又は２に記載のロボットの動作生成システム。
【請求項４】
前記環境認識手段により取得された前記環境情報には前記環境内の把持対象物体の位置情報が含まれ、
前記タスク管理手段は、
前記把持対象物体と前記複数のアームのそれぞれとの位置関係に基づいて、前記複数のアームのそれぞれに優先度を設定する
ことを特徴とする請求項３に記載のロボットの動作生成システム。
【請求項５】
前記グラフ構造データ格納手段は、
前記ノードの個数がそれぞれ異なる前記グラフ構造を前記アームごとに複数格納し、
前記グラフ構造探索処理手段は、
前記グラフ構造データ格納手段に格納されている前記アームごとの複数のグラフ構造のうちで第１のグラフ構造を用いて、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索し、当該探索した経路に含まれるいずれか１のノードが、前記ノード有効性判定手段により前記環境情報と干渉する姿勢を示すノードであると判定された場合には、前記第１のグラフ構造のノードのうちで前記ノード有効性判定手段により前記環境情報と干渉しない姿勢を示すノードであると判定されたノードと、前記第１のグラフ構造のノードのうちで前記探索がなされなかったノードと、を、前記第１のグラフ構造のノードの個数よりも多い個数のノードを有する第２のグラフ構造へと継承させ、当該第２のグラフ構造を用いて、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索する
ことを特徴とする請求項１乃至４いずれか１項に記載のロボットの動作生成システム。
【請求項６】
複数のアームと、上体と下体とを連結する腰と、を備えるロボットの動作生成方法であって、
前記アームの関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するアーム用グラフ構造と、前記アームの関節軸情報及び前記腰の関節軸情報を示すノードと、当該ノード間を接続するエッジと、を有するアーム及び腰用グラフ構造とが、前記アームごとにそれぞれグラフ構造データ格納部に格納されており、
前記ロボットの周辺の環境の環境情報を取得するステップと、
前記複数のアームのそれぞれに優先度が設定され、最も高い優先度が設定されたアームについての経路を探索する場合に、前記グラフ構造データ格納部に格納されている前記アーム及び腰用グラフ構造を用いて、前記アーム及び前記腰の初期姿勢を示すスタートノードから前記アーム及び前記腰の最終姿勢を示すゴールノードまでを結ぶ経路を探索するステップと、
２番目に高い又はそれ以降に高い優先度が設定されたアームについての経路を探索する場合に、前記設定された優先度に従って、前記グラフ構造データ格納部に格納されている前記アームごとのグラフ構造を用いて、前記アームの初期姿勢を示すスタートノードから前記アームの最終姿勢を示すゴールノードまでを結ぶ経路を探索するステップと、
前記探索された経路に含まれるノードについて、当該ノードが示す姿勢が、前記取得した前記環境情報と干渉するか否かを判定し、干渉していると判定した場合には、当該ノードを前記グラフ構造から除外するステップと、
を有するロボットの動作生成方法。
【請求項７】
前記グラフ構造データ格納部には、
前記ノードの個数がそれぞれ異なる前記グラフ構造が前記アームごとに複数格納されており、
前記グラフ構造データ格納部に格納されている前記アームごとの複数のグラフ構造のうちで第１のグラフ構造を用いて、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索するステップと、
前記探索した経路に含まれるいずれか１のノードが、前記環境情報と干渉する姿勢を示すノードであるか否かを判定し、干渉すると判定した場合に、前記第１のグラフ構造のノードのうちで前記環境情報と干渉しない姿勢を示すノードであると判定したノードと、前記第１のグラフ構造のノードのうちで前記探索がなされなかったノードと、を、前記第１のグラフ構造のノードの個数よりも多い個数のノードを有する第２のグラフ構造へと継承させるステップと、
前記第２のグラフ構造を用いて、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索するステップと、
を有することを特徴とする請求項６に記載のロボットの動作生成方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【公開番号】特開２０１２−５６０２３（Ｐ２０１２−５６０２３Ａ）
【公開日】平成２４年３月２２日（２０１２．３．２２）
【国際特許分類】
【出願番号】特願２０１０−２０１７７０（Ｐ２０１０−２０１７７０）
【出願日】平成２２年９月９日（２０１０．９．９）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２４年３月２２日（２０１２．３．２２）
【国際特許分類】
【出願日】平成２２年９月９日（２０１０．９．９）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]