ロボットの軌道計画システム及び軌道計画方法
【課題】少ないノードで軌道計画の成功率を向上させることができる、ロボットの軌道計画システムを提供する。
【解決手段】本発明の一形態に係るロボットの軌道計画システム１０は、グラフ構造データ格納手段に格納されているグラフ構造に対して、スタートノード及びゴールノードを追加し、さらに最終姿勢のハンドの周辺空間にノードを追加して、追加したノードと、隣接するノードとをエッジで接続し、スタートノードからゴールノードまでを結ぶ経路を探索するグラフ構造探索処理手段と、グラフ構造探索処理手段により探索された経路に含まれるノードが示す姿勢が、環境情報と干渉するか否かを判定し、干渉していると判定した場合には、ノードをグラフ構造から除外するノード有効性判定手段と、を備える。
【解決手段】本発明の一形態に係るロボットの軌道計画システム１０は、グラフ構造データ格納手段に格納されているグラフ構造に対して、スタートノード及びゴールノードを追加し、さらに最終姿勢のハンドの周辺空間にノードを追加して、追加したノードと、隣接するノードとをエッジで接続し、スタートノードからゴールノードまでを結ぶ経路を探索するグラフ構造探索処理手段と、グラフ構造探索処理手段により探索された経路に含まれるノードが示す姿勢が、環境情報と干渉するか否かを判定し、干渉していると判定した場合には、ノードをグラフ構造から除外するノード有効性判定手段と、を備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの軌道計画システム及び軌道計画方法に関する。
【背景技術】
【０００２】
ロボットは、環境情報を反映したロボットアームの軌道計画を、様々に変化する環境下においても自律的に行うことが求められる。ここで、ロボットアームの軌道計画とは、ロボット自身と環境とが干渉しないように、ロボットアームのエンドエフェクタの位置姿勢が、初期姿勢から最終姿勢へと到達するような関節軸情報の系列を求める問題である。
【０００３】
この問題の解法の一つとして、ロボットの関節軸情報であるノードを複数有したグラフ構造を使用する手法が知られている。当該グラフ構造では、ノード間がエッジにより接続され、初期姿勢であるスタートノードから最終姿勢であるゴールノードへと至るノードの系列である経路が探索される。このようなグラフ構造を使用して軌道計画を行う技術が、例えば、特許文献１に開示されている。
【０００４】
特許文献１には、最終位置を複数設定することで、取り得る経路を多くして適切な軌道を設定し易くしている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００６−４８３７２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
上述のようなグラフ構造を使用した手法において、軌道計画を確実に行うためにノードの数を多くすると、軌道の探索に時間が掛る。このことは、特許文献１に開示されている技術のように、最終位置を複数設定した場合も同様のことが言える。
一方、軌道の探索に掛る時間を短縮するためにノードの数を少なくすると、軌道の探索を失敗する可能性がある。
【０００７】
本発明は、このような問題を解決するためになされたものであり、少ないノードで軌道計画の成功率を向上させることができる、ロボットの軌道計画システム及び軌道計画方法を提供することを目的とするものである。
【課題を解決するための手段】
【０００８】
本発明の一形態に係るロボットの軌道計画システムは、
グラフ構造を用いたロボットの軌道設計システムであって、
前記ロボットの周辺の環境の環境情報を取得する環境認識手段と、
前記ロボットのハンド及びアームの関節軸情報を示すノードと、前記ノード間を接続するエッジと、を有するグラフ構造を格納するグラフ構造データ格納手段と、
前記グラフ構造データ格納手段に格納されている前記グラフ構造に対して、前記ハンド及び前記アームの初期姿勢を示すスタートノード、及び前記ハンド及び前記アームの最終姿勢を示すゴールノードを追加し、さらに前記ゴールノードで示される、最終姿勢の前記ハンドの周辺空間にノードを追加して、追加した前記ノードと、当該ノードに隣接するノードとをエッジで接続し、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索するグラフ構造探索処理手段と、
前記グラフ構造探索処理手段により探索された経路に含まれるノードについて、当該ノードが示す姿勢が、前記環境認識手段により取得された前記環境情報と干渉するか否かを判定し、干渉していると判定した場合には、当該ノードを前記グラフ構造から除外するノード有効性判定手段と、
を備える。
【０００９】
前記グラフ構造探索処理手段は、最終姿勢の前記ハンドの後方空間にノードを追加すること、が好ましい。
前記グラフ構造探索処理手段は、前記ノードを半円弧上に略等間隔で追加すること、が好ましい。
前記グラフ構造探索処理手段は、前記ノードを仮想半円球内に追加すること、が好ましい。
【００１０】
本発明の一形態に係るロボットの軌道設計方法は、
グラフ構造を用いたロボットの軌道設計方法であって、
前記ロボットの周辺の環境の環境情報を取得するステップと、
前記ロボットのハンド及びアームの関節軸情報を示すノードと、前記ノード間を接続するエッジと、を有するグラフ構造に対して、前記ハンド及び前記アームの初期姿勢を示すスタートノード、及び前記ハンド及び前記アームの最終姿勢を示すゴールノードを追加し、さらに前記ゴールノードで示される、最終姿勢の前記ハンドの周辺空間にノードを追加して、追加した前記ノードと、当該ノードに隣接するノードとをエッジで接続し、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索するステップと、
探索された経路に含まれるノードについて、前記ノードが示す姿勢が、取得された前記環境情報と干渉するか否かを判定し、干渉していると判定した場合には、前記ノードを前記グラフ構造から除外するステップと、
を備える。
【発明の効果】
【００１１】
以上、説明したように、本発明によると、少ないノードで軌道計画の成功率を向上させることができる、ロボットの軌道計画システム及び軌道計画方法を提供することができる。
【図面の簡単な説明】
【００１２】
【図１】実施の形態に係るロボットの概略構成を示す全体図である。
【図２】実施の形態に係るロボットの軌道計画システムの構成図である。
【図３】実施の形態に係るロボットの全身軌道計画部の構成図である。
【図４】実施の形態に係るグラフ構造の概念図である。
【図５】追加するノードとハンドとの配置関係を示す概略図である。
【図６】実施の形態に係る軌道計画処理のフローチャート図である。
【図７】（ａ）は、追加したノードとハンドとの配置関係を概略的に示す平面図である。（ｂ）は、追加したノードとハンドとの配置関係を概略的に示す側面図である。
【図８】追加したノードとハンドとの異なる配置関係を示す概略図である。
【図９】軌道探索の様子を示す概念図である。
【発明を実施するための形態】
【００１３】
以下、図面を参照して本発明の実施の形態について説明する。なお、以下では、本文中の説明においては、必要に応じそれ以前に述べた符号を用いるものとする。
【００１４】
図１は、本実施の形態に係るロボットの概略構成を示す全体図である。
図１では、ロボット１００の外観構成を例示しており、ロボット１００は、胴体１と、胴体１に連結されたアーム２と、アーム２に連結されたハンド３と、を備えている。アーム２は、複数の関節と、関節を介して接続される複数のリンクと、を備えている。また、ハンド３は、手首関節を介してアーム２の端部に連結されている。なお、詳細についての図示は省略するが、ハンド３は、掌部と、関節を介して掌部に接続される複数本の指と、を備えていてもよい。各指は、複数の指関節と、指関節を介して接続される複数のリンクと、を備えていてもよい。
【００１５】
各関節に備えられたモーターなどのアクチュエータ（不図示）が駆動されることで、各関節は、所望の角度及び角速度で動作する。その結果、各座標系において、ロボット１００の全身の位置及び姿勢と、手先の位置及び姿勢と、を所望の位置及び姿勢に制御することができる。このように、アクチュエータは、ロボット１００が動作する際の、動力発生手段に相当する。
【００１６】
ロボット１００は、ユーザから指定される様々なタスク（環境内に存在する物体に対する作業）を実行する。例えば、ロボット１００は、環境内に存在する物体を、ハンド３により把持する。ロボット１００は、このようなタスクを実行するためにアーム２及びハンド３の軌道計画を実行し、計画した軌道に追従するように、各関節を駆動制御する。なお、軌道とは、初期姿勢を示す関節角度ベクトルから最終姿勢を示す関節角度ベクトルへと至る関節角度ベクトルの系列であり、後述するグラフ構造を用いて求められる。
【００１７】
図２は、本実施の形態に係るロボットの軌道計画システムの構成図である。軌道計画システム１０は、環境認識部１１と、タスク管理部１２と、最終全身位置・姿勢決定部１３と、初期全身位置・姿勢取得部１４と、全身軌道計画部１５と、補間部１６と、モーター駆動部１７と、を備えている。
【００１８】
環境認識部１１は、ロボット１００の周辺の環境の環境情報を取得する。環境認識部１１は、各種のセンサを用いて構成され、ロボット１００の周辺の環境内に存在する物体の位置や形状などの環境情報を取得する。センサには、例えば、ステレオカメラやレーザーレンジファインダを用いることができ、環境認識部１１は、環境内の物体について３次元の点群情報を取得し、ロボット１００から物体までの距離情報を取得することができる。環境認識部１１は、取得した物体の距離情報に基づいて、環境内の物体を、例えば、直方体形状のボクセル情報や、３次元の物体モデルとして生成し、環境情報としてタスク管理部１２と、最終全身位置・姿勢決定部１３と、全身軌道計画部１５と、に出力する。
【００１９】
タスク管理部１２は、ロボット１００に実行させるタスクを管理する。タスク管理部１２は、タスクの内容に応じて、環境認識部１１で取得した環境情報から必要な情報を取得する。なお、タスクの内容は、軌道計画システム１０に備えられたインタフェース（不図示）を介してユーザにより入力されるものとしてもよい。
【００２０】
例えば、物体を把持するタスクでは、ロボット１００に把持させる物体、その把持方法などを示す情報がタスクに含まれており、タスク管理部１２は、環境認識部１１で取得した環境情報から、把持対象となる物体を示す情報と、その形状や位置・姿勢などを示す情報などを取得する。
【００２１】
最終全身位置・姿勢決定部１３は、環境情報と、タスク管理部１２から出力されたタスク情報から、タスク実行時のロボット１００の最終全身位置・姿勢を決定する。
【００２２】
初期全身位置・姿勢取得部１４は、タスク管理部１２から出力されたタスク情報から、タスク実行時のロボット１００の初期全身位置・姿勢を取得する。なお、初期全身位置・姿勢は、ロボット１００の現在の位置・姿勢をそのまま与えるものとしてもよい。
【００２３】
全身軌道計画部１５は、環境情報と、最終全身位置・姿勢と、初期全身位置・姿勢と、に基づいて、ロボット１００の全身の軌道計画を行う。軌道計画は各部位についての軌道を探索することで行われ、初期全身位置・姿勢から最終全身位置・姿勢へと至る軌道であって、かつ、環境と干渉しない軌道が計画される。なお、全身軌道計画部１５の詳細については後述する。
【００２４】
補間部１６は、全身軌道計画部１５で求められた全身関節軌道を、モーターを駆動する制御周期に合わせて適切となるように補間する。補間部１６により、全身軌道計画部１５で求めた軌道と比較してより滑らかな軌道が求められる。補間部１６は、その軌道に含まれる関節角度ベクトルに基づいてロボット１００のアクチュエータ（モーター）へのモーター指令値を生成する。
【００２５】
モーター駆動部１７は、補間部１６で求められたモーター指令値に基づいて、フィードバック制御を行う。
【００２６】
次に、図３乃至図９を参照して、全身軌道計画部１５の詳細について説明する。
図３は、全身軌道計画部１５の構成図である。全身軌道計画部１５は、グラフ構造データ格納部１５１と、探索用グラフ構造データ保持部１５２と、グラフ構造生成処理部１５３と、グラフ構造探索処理部１５４と、ノード有効性判定処理部１５５と、を備えている。
【００２７】
グラフ構造データ格納部１５１は、複数のグラフ構造を格納しており、本実施の形態では、部位ごとのグラフ構造を格納している。本実施の形態では、ロボット１００の全身の全ての関節軸を表現したグラフ構造を利用して軌道計画を行うことに代えて、部位ごとに用意したグラフ構造を利用して、部位ごとに軌道を計画する。例えば、グラフ構造データ格納部１５１に格納されるグラフ構造には、アーム２及びハンド３のグラフ構造が含まれる。
【００２８】
図４は、グラフ構造の概念図である。図５は、最終姿勢のハンド３と追加したノードａ１〜ａ６との位置関係を示す図である。但し、図４では、経路を簡潔に示すために、図５において追加されたノードとして図示されている一部のノードａ３〜ａ６を省略している。
【００２９】
グラフ構造２０は、複数のノードｑ１〜ｑ９と、ノード間を接続する線分（以下、エッジと称する場合がある。）と、を有している。グラフ構造２０を用いて、スタートノードＳとゴールノードＧとを結ぶ経路が探索される。グラフ構造のノード構成は、タスクの内容を考慮してユーザにより設定されており、各ノードは、ユーザにより予め注意深く選ばれている。
【００３０】
なお、ノードは、ハンドの関節軸θ１及びアームの関節軸θ２（但し、ハンドが複数の関節軸を有する場合には、θ１はベクトルを示し、同様にアームが複数の関節軸を有する場合には、θ２はベクトルを示す。）の情報等を持つ。また、エッジは、ノード間の距離情報を持つ。
【００３１】
探索用グラフ構造データ保持部１５２は、軌道探索に用いるグラフ構造を一時的に保持する。例えば、軌道探索に使用中のグラフ構造を探索用グラフ構造データ保持部１５２に一時的に保持する。また、探索した軌道を探索用グラフ構造データ保持部１５２に保持しておき、ロボット１００が移動した場合に、保持しているグラフ構造を更新する、などのために探索用グラフ構造データ保持部１５２を利用してもよい。
【００３２】
グラフ構造生成処理部１５３は、グラフ構造データ格納部１５１に格納されているグラフ構造から軌道探索対象となる部位のグラフ構造を選択し、選択したグラフ構造を探索用グラフ構造データ保持部１５２に保持する。そして、グラフ構造生成処理部１５３は、入力されるスタートノードＳとゴールノードＧとを探索用グラフ構造データ保持部１５２に保持されているグラフ構造に追加する。
【００３３】
さらにグラフ構造生成処理部１５３は、図４及び図５に示すように、ゴールノードＧに示される、最終姿勢のハンド３の周辺空間にノードａ１〜ａ６を追加する。つまり、ゴールノードＧに示される、アーム２の手先位置の周辺空間にノードａ１〜ａ６を追加する。
【００３４】
そして、グラフ構造生成処理部１５３は、追加したそれぞれのノードについて近傍ノードとの間をエッジにより接続する。ここで、スタートノードＳは、ロボット１００の初期全身位置・姿勢における軌道探索部位の位置・姿勢を示すノードであり、ゴールノードＧは、ロボット１００の最終全身姿勢における軌道探索部位の位置・姿勢を示すノードである。なお、スタートノードＳは、軌道探索部位の姿勢のみを示すノードであってもよい。
【００３５】
グラフ構造探索処理部１５４は、探索用グラフ構造データ保持部１５２に保持されている、ノード及びエッジが追加されたグラフ構造を用いて、スタートノードＳからゴールノードＧまでを結ぶ経路（スタートノードＳからゴールノードＧへと至るノードの経路であり、軌道と称する場合がある。）を探索する。なお、グラフ構造探索処理部１５４による探索方法については限定されず、ダイクストラ法などの公知の手法を用いればよい。
【００３６】
ノード有効性判定処理部１５５は、グラフ構造探索処理部１５４により探索された経路に含まれるノードについて、ノードが示す姿勢が、環境内の物体やロボット１００自身と干渉していないか否かをチェックし、干渉している場合には、探索用グラフ構造データ保持部１５５のグラフ構造から当該ノードを除外する。
【００３７】
図６は、全身軌道計画部１５の動作フローチャート図である。
先ず、グラフ構造生成処理部１５３は、グラフ構造データ格納部１５１に格納されているグラフ構造から軌道探索対象となる部位のグラフ構造（本実施の形態では、ハンド及びアームのグラフ構造）を選択し（Ｓ１０１）、選択したグラフ構造を探索用グラフ構造データ保持部１５２に保持する。
【００３８】
次に、グラフ構造生成処理部１５３は、スタートノードＳとゴールノードＧとを、探索用グラフ構造データ保持部１５２に格納されている当該グラフ構造に追加する（Ｓ１０２）。さらに、グラフ構造生成処理部１５３は、ゴールノードＧに示される、最終姿勢のハンド３の周辺空間にノードａ１〜ａ６を追加する（Ｓ１０３）。なお、追加するノードの数は適宜設定させる。
【００３９】
本実施の形態のグラフ構造生成処理部１５３は、最終姿勢のハンド３の後方空間にノードａ１〜ａ６を追加している。このとき、グラフ構造生成処理部１５３は、図７（ａ）及び（ｂ）に示すように、最終姿勢のハンド３の後方空間に形成された仮想半円球空間内にノードａ１〜ａ６を追加すると良い。このようにノードを追加する空間を特定することにより、最終姿勢のハンド３の後方空間にノードをランダムに追加する場合に比べて、追加するノードの数を削減することができる。これらのノードａ１〜ａ６は、隣接するノードの間隔がグラフ構造２０における隣接するノードの間隔より狭くなるように、ランダムに追加すると良い。なお、仮想半円球空間の大きさは、ユーザにより適宜設定される。
【００４０】
または、グラフ構造生成処理部１５３は、図８に示すように、最終姿勢のハンド３の後方空間において、ノードａ１〜ａ６を円弧上に追加すると良い。例えば、最終姿勢のハンド３に対して上下方向に直径を有し、且つ手先位置の座標Ｏから仮想的に後方に向かって描かれた同心円弧Ｒ１、Ｒ２上にそれぞれノードａ１、ａ３、ａ５及びノードａ２、ａ４、ａ６が追加される。このとき、円弧Ｒ１上に配置されるノードａ１、ａ３、ａ５の間隔は略等しく、円弧Ｒ２上に配置されるノードａ２、ａ４、ａ６の間隔は略等しく配置される。また、ノードａ１とノードａ２とは座標Ｏから延びる線Ｌ１上に配置され、ノードａ３とノードａ４とは座標Ｏから延びる線Ｌ２上に配置され、ノードａ５とノードａ６とは座標Ｏから延びるＬ３上に配置される。このとき、Ｌ１とＬ２とで形成する角度θ３と、Ｌ１とＬ３とで形成する角度θ４とは、略等しく設定される。この場合もノードを追加する空間を特定するので、最終姿勢のハンド３の後方空間にノードをランダムに追加する場合に比べて、追加するノードの数を削減することができる。また、上下方向にノードを追加するので、障害物が存在しても、当該障害物を避けるように上方又は下方から把持対象である物体にアプローチすることができ、軌道設計の成功率を向上させることができる。なお、円弧の直径や当該円弧上に配置されるノード間の間隔は適宜設定される。また、本実施の形態では、上下方向にノードを追加したが、追加する方向は特に限定されない。要するに、障害物を避けるように把持対象である物体に良好にアプローチできれば良い。
【００４１】
次に、グラフ構造生成処理部１５３は、追加したそれぞれのノードについて近傍ノードとの間をエッジにより接続する（Ｓ１０４）。そして、グラフ構造探索処理部１５４は、ノード及びエッジが追加されたグラフ構造を用いて、スタートノードＳからゴールノードＧまでを結ぶ軌道を探索する（Ｓ１０５）。また、グラフ構造探索処理部１５４は、軌道探索の結果、軌道が存在するか否かを判定する（Ｓ１０６）。軌道が存在しない場合（Ｓ１０６で「無」の場合）には、軌道を探索することができなかったとして、処理を終了する。
【００４２】
次に、ノード有効性判定処理部１５５は、軌道が存在した場合（Ｓ１０６で「有」の場合）には、探索された経路に含まれるノードについて、ノードが示す姿勢が、環境内の物体（障害物）やロボット１００自身と干渉していないか否かをチェックし（Ｓ１０７）、干渉の有無を検出する（Ｓ１０８）。そして、ノード有効性判定処理部１５５は、干渉検出の結果、経路に含まれるいずれか１つのノードが干渉している場合（Ｓ１０８の「ＮＧ」の場合）には、グラフ構造から当該ノードを除外する（Ｓ１０９）。その後、ノード有効性判定処理部１５５は、ノード除外後のグラフ構造を用いて再び軌道探索を行う。例えば、ノード有効性判定処理部１５５は、図９に示すように、ゴールノードＧ近傍のノードｑ３やノードｑ８が干渉している場合、当該ノードｑ３やノードｑ８を除外して、再び軌道検索を実行する。
一方、ノード有効性判定処理部１５５は、干渉検出の結果、経路に含まれるいずれかのノードも干渉していない場合（Ｓ１０８の「ＯＫ」の場合）には、軌道を出力する。本実施の形態では、スタートノードＳ→ノードｑ１→ノードａ１→ノードａ２→ゴールノードＧを軌道として出力する。
【００４３】
通例、ハンドやアームに干渉する障害物は、把持対象である物体の周辺、特にハンドやアームの軌道である、当該物体の後方空間に存在することが多い。そこで、本実施の形態では、最終姿勢のハンド３の周辺空間にノードを追加している。このように障害物の存在が予想される把持対象である物体の周辺空間、特に当該物体の後方空間にノードａ１〜ａ６を追加しているため、障害物が存在することが予想される位置近傍の経路の候補を局所的に増やすことができる。そのため、例えば図９に示すように、ゴールノードＧ近傍のノードｑ３やノードｑ８が干渉している場合でも、軌道計画の成功率を向上させることができる。また、障害物が存在することが予想される位置近傍の経路の候補を局所的に増やすことができるので、他の空間にノードを追加する必要がなく、比較的ノードの数の少ないグラフ構造を用いることができる。よって、演算量を削減することができ、演算時間の削減及び演算装置の小型化に寄与できる。
【００４４】
その他の実施の形態．
上述した実施の形態では、ハンドで把持対象である物体を把持するタスクを実行するために軌道計画を行ったが、既にハンドで把持した物体を他の位置に移動するタスクを実行する場合も、略同様に軌道計画を実行することができる。この場合、当該タスクに応じたグラフ構造を用いればよい。
【００４５】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上記実施の形態では、胴体を有するロボット１００を用いたが、アームとハンドのみを備えるロボットを用いても同様に実施できる。
【符号の説明】
【００４６】
１００ ロボット
１ 胴体
２ アーム
３ ハンド
１０ 軌道計画システム
１１ 環境認識部
１２ タスク管理部
１３ 最終全身姿勢決定部
１４ 初期全身位置・姿勢取得部
１５ 全身軌道計画部
１６ 補間部
１７ モーター駆動部
２０ グラフ構造
１５１ グラフ構造データ格納部
１５２ 探索用グラフ構造データ保持部
１５３ グラフ構造生成処理部
１５４ グラフ構造探索処理部
１５５ ノード有効性判定処理部
ａ１〜ａ６ ノード
ｑ１〜ｑ９ ノード
Ｇ ゴールノード
Ｓ スタートノード
【技術分野】
【０００１】
本発明は、ロボットの軌道計画システム及び軌道計画方法に関する。
【背景技術】
【０００２】
ロボットは、環境情報を反映したロボットアームの軌道計画を、様々に変化する環境下においても自律的に行うことが求められる。ここで、ロボットアームの軌道計画とは、ロボット自身と環境とが干渉しないように、ロボットアームのエンドエフェクタの位置姿勢が、初期姿勢から最終姿勢へと到達するような関節軸情報の系列を求める問題である。
【０００３】
この問題の解法の一つとして、ロボットの関節軸情報であるノードを複数有したグラフ構造を使用する手法が知られている。当該グラフ構造では、ノード間がエッジにより接続され、初期姿勢であるスタートノードから最終姿勢であるゴールノードへと至るノードの系列である経路が探索される。このようなグラフ構造を使用して軌道計画を行う技術が、例えば、特許文献１に開示されている。
【０００４】
特許文献１には、最終位置を複数設定することで、取り得る経路を多くして適切な軌道を設定し易くしている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００６−４８３７２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
上述のようなグラフ構造を使用した手法において、軌道計画を確実に行うためにノードの数を多くすると、軌道の探索に時間が掛る。このことは、特許文献１に開示されている技術のように、最終位置を複数設定した場合も同様のことが言える。
一方、軌道の探索に掛る時間を短縮するためにノードの数を少なくすると、軌道の探索を失敗する可能性がある。
【０００７】
本発明は、このような問題を解決するためになされたものであり、少ないノードで軌道計画の成功率を向上させることができる、ロボットの軌道計画システム及び軌道計画方法を提供することを目的とするものである。
【課題を解決するための手段】
【０００８】
本発明の一形態に係るロボットの軌道計画システムは、
グラフ構造を用いたロボットの軌道設計システムであって、
前記ロボットの周辺の環境の環境情報を取得する環境認識手段と、
前記ロボットのハンド及びアームの関節軸情報を示すノードと、前記ノード間を接続するエッジと、を有するグラフ構造を格納するグラフ構造データ格納手段と、
前記グラフ構造データ格納手段に格納されている前記グラフ構造に対して、前記ハンド及び前記アームの初期姿勢を示すスタートノード、及び前記ハンド及び前記アームの最終姿勢を示すゴールノードを追加し、さらに前記ゴールノードで示される、最終姿勢の前記ハンドの周辺空間にノードを追加して、追加した前記ノードと、当該ノードに隣接するノードとをエッジで接続し、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索するグラフ構造探索処理手段と、
前記グラフ構造探索処理手段により探索された経路に含まれるノードについて、当該ノードが示す姿勢が、前記環境認識手段により取得された前記環境情報と干渉するか否かを判定し、干渉していると判定した場合には、当該ノードを前記グラフ構造から除外するノード有効性判定手段と、
を備える。
【０００９】
前記グラフ構造探索処理手段は、最終姿勢の前記ハンドの後方空間にノードを追加すること、が好ましい。
前記グラフ構造探索処理手段は、前記ノードを半円弧上に略等間隔で追加すること、が好ましい。
前記グラフ構造探索処理手段は、前記ノードを仮想半円球内に追加すること、が好ましい。
【００１０】
本発明の一形態に係るロボットの軌道設計方法は、
グラフ構造を用いたロボットの軌道設計方法であって、
前記ロボットの周辺の環境の環境情報を取得するステップと、
前記ロボットのハンド及びアームの関節軸情報を示すノードと、前記ノード間を接続するエッジと、を有するグラフ構造に対して、前記ハンド及び前記アームの初期姿勢を示すスタートノード、及び前記ハンド及び前記アームの最終姿勢を示すゴールノードを追加し、さらに前記ゴールノードで示される、最終姿勢の前記ハンドの周辺空間にノードを追加して、追加した前記ノードと、当該ノードに隣接するノードとをエッジで接続し、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索するステップと、
探索された経路に含まれるノードについて、前記ノードが示す姿勢が、取得された前記環境情報と干渉するか否かを判定し、干渉していると判定した場合には、前記ノードを前記グラフ構造から除外するステップと、
を備える。
【発明の効果】
【００１１】
以上、説明したように、本発明によると、少ないノードで軌道計画の成功率を向上させることができる、ロボットの軌道計画システム及び軌道計画方法を提供することができる。
【図面の簡単な説明】
【００１２】
【図１】実施の形態に係るロボットの概略構成を示す全体図である。
【図２】実施の形態に係るロボットの軌道計画システムの構成図である。
【図３】実施の形態に係るロボットの全身軌道計画部の構成図である。
【図４】実施の形態に係るグラフ構造の概念図である。
【図５】追加するノードとハンドとの配置関係を示す概略図である。
【図６】実施の形態に係る軌道計画処理のフローチャート図である。
【図７】（ａ）は、追加したノードとハンドとの配置関係を概略的に示す平面図である。（ｂ）は、追加したノードとハンドとの配置関係を概略的に示す側面図である。
【図８】追加したノードとハンドとの異なる配置関係を示す概略図である。
【図９】軌道探索の様子を示す概念図である。
【発明を実施するための形態】
【００１３】
以下、図面を参照して本発明の実施の形態について説明する。なお、以下では、本文中の説明においては、必要に応じそれ以前に述べた符号を用いるものとする。
【００１４】
図１は、本実施の形態に係るロボットの概略構成を示す全体図である。
図１では、ロボット１００の外観構成を例示しており、ロボット１００は、胴体１と、胴体１に連結されたアーム２と、アーム２に連結されたハンド３と、を備えている。アーム２は、複数の関節と、関節を介して接続される複数のリンクと、を備えている。また、ハンド３は、手首関節を介してアーム２の端部に連結されている。なお、詳細についての図示は省略するが、ハンド３は、掌部と、関節を介して掌部に接続される複数本の指と、を備えていてもよい。各指は、複数の指関節と、指関節を介して接続される複数のリンクと、を備えていてもよい。
【００１５】
各関節に備えられたモーターなどのアクチュエータ（不図示）が駆動されることで、各関節は、所望の角度及び角速度で動作する。その結果、各座標系において、ロボット１００の全身の位置及び姿勢と、手先の位置及び姿勢と、を所望の位置及び姿勢に制御することができる。このように、アクチュエータは、ロボット１００が動作する際の、動力発生手段に相当する。
【００１６】
ロボット１００は、ユーザから指定される様々なタスク（環境内に存在する物体に対する作業）を実行する。例えば、ロボット１００は、環境内に存在する物体を、ハンド３により把持する。ロボット１００は、このようなタスクを実行するためにアーム２及びハンド３の軌道計画を実行し、計画した軌道に追従するように、各関節を駆動制御する。なお、軌道とは、初期姿勢を示す関節角度ベクトルから最終姿勢を示す関節角度ベクトルへと至る関節角度ベクトルの系列であり、後述するグラフ構造を用いて求められる。
【００１７】
図２は、本実施の形態に係るロボットの軌道計画システムの構成図である。軌道計画システム１０は、環境認識部１１と、タスク管理部１２と、最終全身位置・姿勢決定部１３と、初期全身位置・姿勢取得部１４と、全身軌道計画部１５と、補間部１６と、モーター駆動部１７と、を備えている。
【００１８】
環境認識部１１は、ロボット１００の周辺の環境の環境情報を取得する。環境認識部１１は、各種のセンサを用いて構成され、ロボット１００の周辺の環境内に存在する物体の位置や形状などの環境情報を取得する。センサには、例えば、ステレオカメラやレーザーレンジファインダを用いることができ、環境認識部１１は、環境内の物体について３次元の点群情報を取得し、ロボット１００から物体までの距離情報を取得することができる。環境認識部１１は、取得した物体の距離情報に基づいて、環境内の物体を、例えば、直方体形状のボクセル情報や、３次元の物体モデルとして生成し、環境情報としてタスク管理部１２と、最終全身位置・姿勢決定部１３と、全身軌道計画部１５と、に出力する。
【００１９】
タスク管理部１２は、ロボット１００に実行させるタスクを管理する。タスク管理部１２は、タスクの内容に応じて、環境認識部１１で取得した環境情報から必要な情報を取得する。なお、タスクの内容は、軌道計画システム１０に備えられたインタフェース（不図示）を介してユーザにより入力されるものとしてもよい。
【００２０】
例えば、物体を把持するタスクでは、ロボット１００に把持させる物体、その把持方法などを示す情報がタスクに含まれており、タスク管理部１２は、環境認識部１１で取得した環境情報から、把持対象となる物体を示す情報と、その形状や位置・姿勢などを示す情報などを取得する。
【００２１】
最終全身位置・姿勢決定部１３は、環境情報と、タスク管理部１２から出力されたタスク情報から、タスク実行時のロボット１００の最終全身位置・姿勢を決定する。
【００２２】
初期全身位置・姿勢取得部１４は、タスク管理部１２から出力されたタスク情報から、タスク実行時のロボット１００の初期全身位置・姿勢を取得する。なお、初期全身位置・姿勢は、ロボット１００の現在の位置・姿勢をそのまま与えるものとしてもよい。
【００２３】
全身軌道計画部１５は、環境情報と、最終全身位置・姿勢と、初期全身位置・姿勢と、に基づいて、ロボット１００の全身の軌道計画を行う。軌道計画は各部位についての軌道を探索することで行われ、初期全身位置・姿勢から最終全身位置・姿勢へと至る軌道であって、かつ、環境と干渉しない軌道が計画される。なお、全身軌道計画部１５の詳細については後述する。
【００２４】
補間部１６は、全身軌道計画部１５で求められた全身関節軌道を、モーターを駆動する制御周期に合わせて適切となるように補間する。補間部１６により、全身軌道計画部１５で求めた軌道と比較してより滑らかな軌道が求められる。補間部１６は、その軌道に含まれる関節角度ベクトルに基づいてロボット１００のアクチュエータ（モーター）へのモーター指令値を生成する。
【００２５】
モーター駆動部１７は、補間部１６で求められたモーター指令値に基づいて、フィードバック制御を行う。
【００２６】
次に、図３乃至図９を参照して、全身軌道計画部１５の詳細について説明する。
図３は、全身軌道計画部１５の構成図である。全身軌道計画部１５は、グラフ構造データ格納部１５１と、探索用グラフ構造データ保持部１５２と、グラフ構造生成処理部１５３と、グラフ構造探索処理部１５４と、ノード有効性判定処理部１５５と、を備えている。
【００２７】
グラフ構造データ格納部１５１は、複数のグラフ構造を格納しており、本実施の形態では、部位ごとのグラフ構造を格納している。本実施の形態では、ロボット１００の全身の全ての関節軸を表現したグラフ構造を利用して軌道計画を行うことに代えて、部位ごとに用意したグラフ構造を利用して、部位ごとに軌道を計画する。例えば、グラフ構造データ格納部１５１に格納されるグラフ構造には、アーム２及びハンド３のグラフ構造が含まれる。
【００２８】
図４は、グラフ構造の概念図である。図５は、最終姿勢のハンド３と追加したノードａ１〜ａ６との位置関係を示す図である。但し、図４では、経路を簡潔に示すために、図５において追加されたノードとして図示されている一部のノードａ３〜ａ６を省略している。
【００２９】
グラフ構造２０は、複数のノードｑ１〜ｑ９と、ノード間を接続する線分（以下、エッジと称する場合がある。）と、を有している。グラフ構造２０を用いて、スタートノードＳとゴールノードＧとを結ぶ経路が探索される。グラフ構造のノード構成は、タスクの内容を考慮してユーザにより設定されており、各ノードは、ユーザにより予め注意深く選ばれている。
【００３０】
なお、ノードは、ハンドの関節軸θ１及びアームの関節軸θ２（但し、ハンドが複数の関節軸を有する場合には、θ１はベクトルを示し、同様にアームが複数の関節軸を有する場合には、θ２はベクトルを示す。）の情報等を持つ。また、エッジは、ノード間の距離情報を持つ。
【００３１】
探索用グラフ構造データ保持部１５２は、軌道探索に用いるグラフ構造を一時的に保持する。例えば、軌道探索に使用中のグラフ構造を探索用グラフ構造データ保持部１５２に一時的に保持する。また、探索した軌道を探索用グラフ構造データ保持部１５２に保持しておき、ロボット１００が移動した場合に、保持しているグラフ構造を更新する、などのために探索用グラフ構造データ保持部１５２を利用してもよい。
【００３２】
グラフ構造生成処理部１５３は、グラフ構造データ格納部１５１に格納されているグラフ構造から軌道探索対象となる部位のグラフ構造を選択し、選択したグラフ構造を探索用グラフ構造データ保持部１５２に保持する。そして、グラフ構造生成処理部１５３は、入力されるスタートノードＳとゴールノードＧとを探索用グラフ構造データ保持部１５２に保持されているグラフ構造に追加する。
【００３３】
さらにグラフ構造生成処理部１５３は、図４及び図５に示すように、ゴールノードＧに示される、最終姿勢のハンド３の周辺空間にノードａ１〜ａ６を追加する。つまり、ゴールノードＧに示される、アーム２の手先位置の周辺空間にノードａ１〜ａ６を追加する。
【００３４】
そして、グラフ構造生成処理部１５３は、追加したそれぞれのノードについて近傍ノードとの間をエッジにより接続する。ここで、スタートノードＳは、ロボット１００の初期全身位置・姿勢における軌道探索部位の位置・姿勢を示すノードであり、ゴールノードＧは、ロボット１００の最終全身姿勢における軌道探索部位の位置・姿勢を示すノードである。なお、スタートノードＳは、軌道探索部位の姿勢のみを示すノードであってもよい。
【００３５】
グラフ構造探索処理部１５４は、探索用グラフ構造データ保持部１５２に保持されている、ノード及びエッジが追加されたグラフ構造を用いて、スタートノードＳからゴールノードＧまでを結ぶ経路（スタートノードＳからゴールノードＧへと至るノードの経路であり、軌道と称する場合がある。）を探索する。なお、グラフ構造探索処理部１５４による探索方法については限定されず、ダイクストラ法などの公知の手法を用いればよい。
【００３６】
ノード有効性判定処理部１５５は、グラフ構造探索処理部１５４により探索された経路に含まれるノードについて、ノードが示す姿勢が、環境内の物体やロボット１００自身と干渉していないか否かをチェックし、干渉している場合には、探索用グラフ構造データ保持部１５５のグラフ構造から当該ノードを除外する。
【００３７】
図６は、全身軌道計画部１５の動作フローチャート図である。
先ず、グラフ構造生成処理部１５３は、グラフ構造データ格納部１５１に格納されているグラフ構造から軌道探索対象となる部位のグラフ構造（本実施の形態では、ハンド及びアームのグラフ構造）を選択し（Ｓ１０１）、選択したグラフ構造を探索用グラフ構造データ保持部１５２に保持する。
【００３８】
次に、グラフ構造生成処理部１５３は、スタートノードＳとゴールノードＧとを、探索用グラフ構造データ保持部１５２に格納されている当該グラフ構造に追加する（Ｓ１０２）。さらに、グラフ構造生成処理部１５３は、ゴールノードＧに示される、最終姿勢のハンド３の周辺空間にノードａ１〜ａ６を追加する（Ｓ１０３）。なお、追加するノードの数は適宜設定させる。
【００３９】
本実施の形態のグラフ構造生成処理部１５３は、最終姿勢のハンド３の後方空間にノードａ１〜ａ６を追加している。このとき、グラフ構造生成処理部１５３は、図７（ａ）及び（ｂ）に示すように、最終姿勢のハンド３の後方空間に形成された仮想半円球空間内にノードａ１〜ａ６を追加すると良い。このようにノードを追加する空間を特定することにより、最終姿勢のハンド３の後方空間にノードをランダムに追加する場合に比べて、追加するノードの数を削減することができる。これらのノードａ１〜ａ６は、隣接するノードの間隔がグラフ構造２０における隣接するノードの間隔より狭くなるように、ランダムに追加すると良い。なお、仮想半円球空間の大きさは、ユーザにより適宜設定される。
【００４０】
または、グラフ構造生成処理部１５３は、図８に示すように、最終姿勢のハンド３の後方空間において、ノードａ１〜ａ６を円弧上に追加すると良い。例えば、最終姿勢のハンド３に対して上下方向に直径を有し、且つ手先位置の座標Ｏから仮想的に後方に向かって描かれた同心円弧Ｒ１、Ｒ２上にそれぞれノードａ１、ａ３、ａ５及びノードａ２、ａ４、ａ６が追加される。このとき、円弧Ｒ１上に配置されるノードａ１、ａ３、ａ５の間隔は略等しく、円弧Ｒ２上に配置されるノードａ２、ａ４、ａ６の間隔は略等しく配置される。また、ノードａ１とノードａ２とは座標Ｏから延びる線Ｌ１上に配置され、ノードａ３とノードａ４とは座標Ｏから延びる線Ｌ２上に配置され、ノードａ５とノードａ６とは座標Ｏから延びるＬ３上に配置される。このとき、Ｌ１とＬ２とで形成する角度θ３と、Ｌ１とＬ３とで形成する角度θ４とは、略等しく設定される。この場合もノードを追加する空間を特定するので、最終姿勢のハンド３の後方空間にノードをランダムに追加する場合に比べて、追加するノードの数を削減することができる。また、上下方向にノードを追加するので、障害物が存在しても、当該障害物を避けるように上方又は下方から把持対象である物体にアプローチすることができ、軌道設計の成功率を向上させることができる。なお、円弧の直径や当該円弧上に配置されるノード間の間隔は適宜設定される。また、本実施の形態では、上下方向にノードを追加したが、追加する方向は特に限定されない。要するに、障害物を避けるように把持対象である物体に良好にアプローチできれば良い。
【００４１】
次に、グラフ構造生成処理部１５３は、追加したそれぞれのノードについて近傍ノードとの間をエッジにより接続する（Ｓ１０４）。そして、グラフ構造探索処理部１５４は、ノード及びエッジが追加されたグラフ構造を用いて、スタートノードＳからゴールノードＧまでを結ぶ軌道を探索する（Ｓ１０５）。また、グラフ構造探索処理部１５４は、軌道探索の結果、軌道が存在するか否かを判定する（Ｓ１０６）。軌道が存在しない場合（Ｓ１０６で「無」の場合）には、軌道を探索することができなかったとして、処理を終了する。
【００４２】
次に、ノード有効性判定処理部１５５は、軌道が存在した場合（Ｓ１０６で「有」の場合）には、探索された経路に含まれるノードについて、ノードが示す姿勢が、環境内の物体（障害物）やロボット１００自身と干渉していないか否かをチェックし（Ｓ１０７）、干渉の有無を検出する（Ｓ１０８）。そして、ノード有効性判定処理部１５５は、干渉検出の結果、経路に含まれるいずれか１つのノードが干渉している場合（Ｓ１０８の「ＮＧ」の場合）には、グラフ構造から当該ノードを除外する（Ｓ１０９）。その後、ノード有効性判定処理部１５５は、ノード除外後のグラフ構造を用いて再び軌道探索を行う。例えば、ノード有効性判定処理部１５５は、図９に示すように、ゴールノードＧ近傍のノードｑ３やノードｑ８が干渉している場合、当該ノードｑ３やノードｑ８を除外して、再び軌道検索を実行する。
一方、ノード有効性判定処理部１５５は、干渉検出の結果、経路に含まれるいずれかのノードも干渉していない場合（Ｓ１０８の「ＯＫ」の場合）には、軌道を出力する。本実施の形態では、スタートノードＳ→ノードｑ１→ノードａ１→ノードａ２→ゴールノードＧを軌道として出力する。
【００４３】
通例、ハンドやアームに干渉する障害物は、把持対象である物体の周辺、特にハンドやアームの軌道である、当該物体の後方空間に存在することが多い。そこで、本実施の形態では、最終姿勢のハンド３の周辺空間にノードを追加している。このように障害物の存在が予想される把持対象である物体の周辺空間、特に当該物体の後方空間にノードａ１〜ａ６を追加しているため、障害物が存在することが予想される位置近傍の経路の候補を局所的に増やすことができる。そのため、例えば図９に示すように、ゴールノードＧ近傍のノードｑ３やノードｑ８が干渉している場合でも、軌道計画の成功率を向上させることができる。また、障害物が存在することが予想される位置近傍の経路の候補を局所的に増やすことができるので、他の空間にノードを追加する必要がなく、比較的ノードの数の少ないグラフ構造を用いることができる。よって、演算量を削減することができ、演算時間の削減及び演算装置の小型化に寄与できる。
【００４４】
その他の実施の形態．
上述した実施の形態では、ハンドで把持対象である物体を把持するタスクを実行するために軌道計画を行ったが、既にハンドで把持した物体を他の位置に移動するタスクを実行する場合も、略同様に軌道計画を実行することができる。この場合、当該タスクに応じたグラフ構造を用いればよい。
【００４５】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上記実施の形態では、胴体を有するロボット１００を用いたが、アームとハンドのみを備えるロボットを用いても同様に実施できる。
【符号の説明】
【００４６】
１００ ロボット
１ 胴体
２ アーム
３ ハンド
１０ 軌道計画システム
１１ 環境認識部
１２ タスク管理部
１３ 最終全身姿勢決定部
１４ 初期全身位置・姿勢取得部
１５ 全身軌道計画部
１６ 補間部
１７ モーター駆動部
２０ グラフ構造
１５１ グラフ構造データ格納部
１５２ 探索用グラフ構造データ保持部
１５３ グラフ構造生成処理部
１５４ グラフ構造探索処理部
１５５ ノード有効性判定処理部
ａ１〜ａ６ ノード
ｑ１〜ｑ９ ノード
Ｇ ゴールノード
Ｓ スタートノード
【特許請求の範囲】
【請求項１】
グラフ構造を用いたロボットの軌道設計システムであって、
前記ロボットの周辺の環境の環境情報を取得する環境認識手段と、
前記ロボットのハンド及びアームの関節軸情報を示すノードと、前記ノード間を接続するエッジと、を有するグラフ構造を格納するグラフ構造データ格納手段と、
前記グラフ構造データ格納手段に格納されている前記グラフ構造に対して、前記ハンド及び前記アームの初期姿勢を示すスタートノード、及び前記ハンド及び前記アームの最終姿勢を示すゴールノードを追加し、さらに前記ゴールノードで示される、最終姿勢の前記ハンドの周辺空間にノードを追加して、追加した前記ノードと、当該ノードに隣接するノードとをエッジで接続し、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索するグラフ構造探索処理手段と、
前記グラフ構造探索処理手段により探索された経路に含まれるノードについて、当該ノードが示す姿勢が、前記環境認識手段により取得された前記環境情報と干渉するか否かを判定し、干渉していると判定した場合には、当該ノードを前記グラフ構造から除外するノード有効性判定手段と、
を備えるロボットの軌道設計システム。
【請求項２】
前記グラフ構造探索処理手段は、最終姿勢の前記ハンドの後方空間にノードを追加する請求項１に記載のロボットの軌道設計システム。
【請求項３】
前記グラフ構造探索処理手段は、前記ノードを半円弧上に略等間隔で追加する請求項２に記載のロボットの軌道設計システム。
【請求項４】
前記グラフ構造探索処理手段は、前記ノードを仮想半円球内に追加する請求項２に記載のロボットの軌道設計システム。
【請求項５】
グラフ構造を用いたロボットの軌道設計方法であって、
前記ロボットの周辺の環境の環境情報を取得するステップと、
前記ロボットのハンド及びアームの関節軸情報を示すノードと、前記ノード間を接続するエッジと、を有するグラフ構造に対して、前記ハンド及び前記アームの初期姿勢を示すスタートノード、及び前記ハンド及び前記アームの最終姿勢を示すゴールノードを追加し、さらに前記ゴールノードで示される、最終姿勢の前記ハンドの周辺空間にノードを追加して、追加した前記ノードと、当該ノードに隣接するノードとをエッジで接続し、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索するステップと、
探索された経路に含まれるノードについて、前記ノードが示す姿勢が、取得された前記環境情報と干渉するか否かを判定し、干渉していると判定した場合には、前記ノードを前記グラフ構造から除外するステップと、
を備えるロボットの軌道設計方法。
【請求項１】
グラフ構造を用いたロボットの軌道設計システムであって、
前記ロボットの周辺の環境の環境情報を取得する環境認識手段と、
前記ロボットのハンド及びアームの関節軸情報を示すノードと、前記ノード間を接続するエッジと、を有するグラフ構造を格納するグラフ構造データ格納手段と、
前記グラフ構造データ格納手段に格納されている前記グラフ構造に対して、前記ハンド及び前記アームの初期姿勢を示すスタートノード、及び前記ハンド及び前記アームの最終姿勢を示すゴールノードを追加し、さらに前記ゴールノードで示される、最終姿勢の前記ハンドの周辺空間にノードを追加して、追加した前記ノードと、当該ノードに隣接するノードとをエッジで接続し、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索するグラフ構造探索処理手段と、
前記グラフ構造探索処理手段により探索された経路に含まれるノードについて、当該ノードが示す姿勢が、前記環境認識手段により取得された前記環境情報と干渉するか否かを判定し、干渉していると判定した場合には、当該ノードを前記グラフ構造から除外するノード有効性判定手段と、
を備えるロボットの軌道設計システム。
【請求項２】
前記グラフ構造探索処理手段は、最終姿勢の前記ハンドの後方空間にノードを追加する請求項１に記載のロボットの軌道設計システム。
【請求項３】
前記グラフ構造探索処理手段は、前記ノードを半円弧上に略等間隔で追加する請求項２に記載のロボットの軌道設計システム。
【請求項４】
前記グラフ構造探索処理手段は、前記ノードを仮想半円球内に追加する請求項２に記載のロボットの軌道設計システム。
【請求項５】
グラフ構造を用いたロボットの軌道設計方法であって、
前記ロボットの周辺の環境の環境情報を取得するステップと、
前記ロボットのハンド及びアームの関節軸情報を示すノードと、前記ノード間を接続するエッジと、を有するグラフ構造に対して、前記ハンド及び前記アームの初期姿勢を示すスタートノード、及び前記ハンド及び前記アームの最終姿勢を示すゴールノードを追加し、さらに前記ゴールノードで示される、最終姿勢の前記ハンドの周辺空間にノードを追加して、追加した前記ノードと、当該ノードに隣接するノードとをエッジで接続し、前記スタートノードから前記ゴールノードまでを結ぶ経路を探索するステップと、
探索された経路に含まれるノードについて、前記ノードが示す姿勢が、取得された前記環境情報と干渉するか否かを判定し、干渉していると判定した場合には、前記ノードを前記グラフ構造から除外するステップと、
を備えるロボットの軌道設計方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【公開番号】特開２０１２−１８７６９７（Ｐ２０１２−１８７６９７Ａ）
【公開日】平成２４年１０月４日（２０１２．１０．４）
【国際特許分類】
【出願番号】特願２０１１−５５４８１（Ｐ２０１１−５５４８１）
【出願日】平成２３年３月１４日（２０１１．３．１４）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２４年１０月４日（２０１２．１０．４）
【国際特許分類】
【出願日】平成２３年３月１４日（２０１１．３．１４）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]