JP7262373B2 - 軌道計画生成装置、軌道計画生成方法、および軌道計画生成プログラム - Google Patents

Description

本発明は、軌道計画を生成する軌道計画生成装置、軌道計画生成方法、および軌道計画生成プログラムに関する。

本技術分野の背景技術として、特許文献１がある。特許文献１の軌道計画装置は、計画された軌道上におけるロボットと障害物との距離を算出する距離算出手段と、距離算出手段により算出された距離に基づいて、始点から終点までの軌道間を離散化する離散化幅を設定する離散化設定手段と、離散化設定手段により設定された離散化幅で離散化したときの軌道上の各位置で、ロボットと障害物とが干渉しているか否かを判定する干渉判定手段と、干渉判定手段による干渉判定結果に基づいて、ロボットの軌道を計画する軌道計画手段と、を備える。

特開２０１３-２４６５５３号公報

特許文献１の軌道計画装置は、干渉判定に基づき、周辺の干渉物から距離を一定以上離した軌道を算出する。しかしながら、特許文献１の軌道計画装置は、手先姿勢を考慮せずに軌道を算出するため、把持物の姿勢が急激に変化する可能性があり、把持物にダメージを与える可能性がある。また、把持物へのダメージを削減するために制御時刻を遅くすると、サイクルタイムが伸び、生産性が低下する。

本発明は、保持する物体の安定化を図る計画を立案することを目的とする。

本願において開示される発明の一側面となる軌道計画生成装置は、プログラムを実行するプロセッサと、前記プログラムを記憶する記憶デバイスと、を有し、複数の関節を有するロボットアームの先端部が始点から終点に移動する軌道計画を生成する軌道計画生成装置であって、前記プロセッサは、前記始点から前記終点の間における前記先端部の位置および姿勢を示す軌道点からの所定距離内の前記先端部の移動先となる複数の位置候補を探索し、前記複数の位置候補について、前記先端部の姿勢に基づく第１球面補間により、前記先端部の複数の第１姿勢候補を探索することにより、複数組の位置候補および第１姿勢候補である複数の軌道点候補を出力する第１探索処理と、前記第１探索処理によって探索された複数の軌道点候補の中から、前記軌道点からの前記先端部の移動先の位置および姿勢を示す移動先の軌道点を決定する第１決定処理と、前記軌道点および前記第１決定処理によって決定された前記移動先の軌道点を含む軌道データを出力する出力処理と、を実行することを特徴とする。

本発明の代表的な実施の形態によれば、保持する物体の安定化を図る計画を立案することができる。前述した以外の課題、構成及び効果は、以下の実施例の説明により明らかにされる。

図１は、軌道計画生成例を示す説明図である。 図２は、軌道計画生成装置のハードウェア構成例を示すブロック図である。 図３は、ロボットアーム構成ＤＢの一例を示す説明図である。 図４は、障害物構成ＤＢの一例を示す説明図である。 図５は、開始関節角度ＤＢの一例を示す説明図である。 図６は、目標位置姿勢ＤＢの一例を示す説明図である。 図７は、手先位置候補算出方法ＤＢの一例を示す説明図である。 図８は、手先姿勢変動候補算出方法ＤＢの一例を示す説明図である。 図９は、制御時刻付与方法ＤＢの一例を示す説明図である。 図１０は、軌道点ＤＢの一例を示す説明図である。 図１１は、手先探索データの一例を示す説明図である。 図１２は、軌道計画生成装置の機能的構成例を示すブロック図である。 図１３は、軌道計画生成装置による軌道計画生成処理手順例を示すフローチャートである。 図１４は、図１３に示した探索部による探索処理（ステップＳ１３０２）の詳細な処理手順例を示すフローチャートである。 図１５は、ステップＳ１４０６の手先位置候補算出方法に基づく軌道点候補の算出例を示す説明図である。 図１６は、ステップＳ１４０７の手先位置候補算出方法に基づく軌道点候補の算出例を示す説明図である。 図１７は、図１４に示した関節角度算出処理（ステップＳ１４０８）の詳細な処理手順例を示すフローチャートである。 図１８は、図１３に示した平滑化部による平滑化処理（ステップＳ１３０３）の詳細な処理手順例を示すフローチャートである。 図１９は、ステップＳ１８０４における補間曲線の計算例を示す説明図である。 図２０は、図１３に示した付与部による付与処理（ステップＳ１３０４）の詳細な処理手順例を示すフローチャートである。

＜軌道計画生成例＞
図１は、軌道計画生成例を示す説明図である。軌道計画生成装置１００は、ロボットアーム１０１の連続的な軌道点Ｐ１～Ｐｎ（ｎは１以上の整数）を計画し、ロボットアームの軌道計画データ１２０を生成する。軌道点Ｐ１～Ｐｎの一連のロボットアーム１０１の動作軌跡を軌道データＴと称す。

ロボットアーム１０１は、ｍ個（ｍは２以上の整数。図１では、ｍ＝４。）の関節Ｊ１～Ｊｍと、関節Ｊ１～Ｊｍ間を連結するｍ個のリンクＬ１～Ｌｍと、リンクＬｍに接続される先端部（「手先」ともいう）１０２と、を有するマニピュレータである。軌道点Ｐとは、ロボットアーム１０１の各関節および先端部の３次元位置と、各関節Ｊ１～Ｊｍの動作によって変化する各リンクＬ１～Ｌｍおよび先端部１０２の姿勢と、の組み合わせを示す軌道情報である。

なお、軌道点Ｐ１～Ｐｎを区別しない場合は、単に軌道点Ｐと表記する。関節Ｊ１～Ｊｍを区別しない場合は、単に関節Ｊと表記する。リンクＬ１～Ｌｍを区別しない場合は、単にリンクＬと表記する。ロボットアーム１０１は、軌道計画生成装置１００または他のコンピュータにより動作制御可能である。

なお、図１では、互いに直交するＸ軸、Ｙ軸、およびＺ軸で張られた３次元空間をグローバル座標系１１０とした場合、ロボットアーム１０１は、ＸＹ平面に載置されているものとする。ＸＹ平面は、Ｘ軸とＹ軸とによって張られる平面である。図１では、Ｚ軸は、たとえば、鉛直方向を示し、下から上に向く方向を＋Ｚ方向とし、上から下に向く方向を－Ｚ方向とする。また、Ｘ軸回りの回転角および回転方向をロールｒ、Ｙ軸回りの回転角および回転方向をピッチｐ、Ｚ軸回りの回転角および回転方向をヨーｙで表記する。また、関節Ｊ１は、原点Ｏに位置する。

また、各関節Ｊｍのローカル座標系を、その原点Ｏｎを関節Ｊｍの位置とし、互いに直交するＸｍ軸、Ｙｍ軸、およびＺｍ軸で張られた３次元空間とする。また、Ｘｍ軸、Ｙｍ軸、およびＺｍ軸の各軸回りの回転角および回転方向を、ロールｒｍ、ピッチｐｍ、およびヨーｙｍと表記する。Ｘｍ軸は、リンクＬ（ｍ－１）の長手方向であり、Ｙｍ軸は、ＸＹ平面に平行な軸とする。ただし、関節Ｊ１のＸｍ軸は、グローバル座標系１１０のＺ軸とする。

関節Ｊｍは、リンクＬｍまたは先端部１０２を回転させる。具体的には、たとえば、関節Ｊ１は、リンクＬ１をヨー方向ｙ（ロール方向ｒ１）に回転させる。関節Ｊ１は、リンクＬ１をロール方向ｒおよびピッチ方向ｐ（ピッチ方向ｐ１およびヨー方向ｙ１）に回転させない。

関節Ｊ２は、リンクＬ２をロール方向ｒ２およびピッチ方向ｐ２に回転させる。関節Ｊ３は、リンクＬ３をロール方向ｒ３およびピッチ方向ｐ３に回転させる。関節Ｊ４は、先端部１０２をロール方向ｒ４およびピッチ方向ｐ４に回転させる。先端部１０２は、物体１０３を保持する。先端部１０２は、具体的には、たとえば、吸着ノズル、磁石、またはハンドピースである。

軌道計画生成装置１００は、ロボットアーム１０１が物体１０３を保持している場合に、物体１０３を安定的に保持するために、可能な限り先端部１０２の姿勢を変化させず、かつ、障害物１０４に干渉しないような軌道点Ｐ１～Ｐｎを計画し、軌道データＴを出力する。

軌道計画データ１２０は、軌道データＴを再生するためのデータである。軌道計画データ１２０は、読み込みボタン１２１と、計算開始ボタン１２２と、情報表示部１２３と、ロボットアーム１０１の軌道を模擬するシミュレーション動画１２４と、を有する。軌道計画データ１２０は、軌道計画生成装置１００または軌道計画生成装置１００と通信可能に接続された他のコンピュータのディスプレイに表示される。

読み込みボタン１２１が押下されると、軌道計画データ１２０の生成に必要なデータが読み込まれる。計算開始ボタン１２２が押下されると、読み込まれたデータを用いて、軌道計画データ１２０を生成するための計算が開始され、起動データＴが生成される。計算が終了すると情報表示部１２３が表示される。

情報表示部１２３は、軌道計画データ１２０を計算した時の条件１３１と、当該条件１３１でのロボットアーム１０１の動作時間１３２と、シミュレーション動画１２４を再生する動作再生ボタン１３３と、を有する。動作再生ボタン１３３が押下されると、その動作時間１３２でシミュレーション動画１２４が再生される。これにより、ユーザは、条件１３１ごとのシミュレーション動画１２４により、軌道データＴが再生され、ロボットアーム１０１の軌道を確認することができる。

＜軌道計画生成装置１００のハードウェア構成例＞
図２は、軌道計画生成装置１００のハードウェア構成例を示すブロック図である。軌道計画生成装置１００は、プロセッサ２０１と、記憶デバイス２０２と、入力デバイス２０３と、出力デバイス２０４と、通信インターフェース（通信ＩＦ）２０５と、を有する。プロセッサ２０１、記憶デバイス２０２、入力デバイス２０３、出力デバイス２０４、および通信ＩＦ２０５は、バス２０６により接続される。プロセッサ２０１は、軌道計画生成装置１００を制御する。記憶デバイス２０２は、プロセッサ２０１の作業エリアとなる。また、記憶デバイス２０２は、各種プログラムやデータを記憶する非一時的なまたは一時的な記録媒体である。記憶デバイス２０２としては、たとえば、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）、フラッシュメモリがある。入力デバイス２０３は、データを入力するデバイスである。入力デバイス２０３としては、たとえば、キーボード、マウス、タッチパネル、テンキー、スキャナがある。出力デバイス２０４は、データを出力するデバイスである。出力デバイス２０４としては、たとえば、ディスプレイ、プリンタがある。通信ＩＦ２０５は、ネットワークと接続し、データを送受信する。

＜データ構成例＞
ここで、記憶デバイス２０２が記憶するデータを具体的に説明する。記憶デバイス２０２は、ロボットアーム構成ＤＢ２１１と、障害物構成ＤＢ２１２と、開始関節角度ＤＢ２１３と、目標位置姿勢ＤＢ２１４と、手先位置候補算出方法ＤＢ２１５と、手先姿勢変動候補算出方法ＤＢ２１６と、制御時刻付与方法ＤＢ２１７と、軌道点ＤＢ２１８と、手先探索データ２１９と、を記憶する。なお、これらは、軌道計画生成装置１００がアクセス可能な他のコンピュータの記憶デバイス２０２に記憶されていてもよい。以下、図３～図１１を用いて説明する。

図３は、ロボットアーム構成ＤＢ２１１の一例を示す説明図である。ロボットアーム構成ＤＢ２１１は、ロボットアーム１０１を構成する関節ＪおよびリンクＬに関するデータを記憶するデータベースであり、具体的には、たとえば、関節情報ＤＢ３０１と、リンク情報ＤＢ３０２と、を有する。

関節情報ＤＢ３０１は、関節Ｊに関する情報を記憶するデータベースである。具体的には、たとえば、関節情報ＤＢ３０１は、フィールドとして、関節名３１１と、動作種類３１２と、関節位置３１３と、回転軸３１４と、動作下限３１５と、動作上限３１６と、最大速度３１７と、最大加速度３１８と、を有する。同一行の各フィールドの値の組み合わせが、１つの関節Ｊを構成する。

関節名３１１は、関節Ｊを一意に特定する名称である。動作種類３１２は、回転や平行移動など、関節Ｊの動作に関する種類を示す。関節位置３１３は、グローバル座標系１１０における１つ前の関節Ｊ（ｍ－１）からのグローバル座標系１１０の３次元位置（Ｘ，Ｙ，Ｚ）を示す。なお、関節Ｊ１の関節位置３１３は、グローバル座標系１１０の原点Ｏとする。

回転軸３１４は、その関節Ｊのローカル座標系において関節Ｊが回転可能な軸（ａ，ｂ，ｃ）（ａ，ｂ，ｃ＝０または１）を示す。ａはＸｍ軸、ｂはＹｍ軸、ｃはＺｍ軸に対応し、「０」は回転軸３１４でないことを示し、「１」は回転軸３１４であることを示す。図３では、関節Ｊ１は、リンクＬ１を、Ｘ１軸（Ｚ軸）回りのロール方向ｒ１（ロール方向ｒ）に回転させるため、関節Ｊ１の回転軸３１４は、（１，０，０）で表現される。

動作下限３１５は、関節Ｊが動作可能な下限値であり、動作上限３１６は間接が動作可能な上限値であり、関節Ｊは、動作下限３１５から動作上限３１６までの範囲で、動作種類３１２に示した動作が可能となる。

最大速度３１７は、関節Ｊの動作速度の最大値である。動作種類３１２が「回転」であれば、動作速度は回転速度となる。最大加速度３１８は、関節Ｊの動作加速度の最大値である。動作種類３１２が「回転」であれば、動作速度は回転加速度となる。

リンク情報ＤＢ３０２は、リンクＬに関する情報を記憶するデータベースである。具体的には、たとえば、リンク情報ＤＢ３０２は、フィールドとして、リンク名３２１と、親関節名３２２と、子関節名３２３と、リンク形状３２４と、を有する。同一行の各フィールドの値の組み合わせが、１つのリンクＬの関節情報を規定する。

リンク名３２１は、リンクＬを一意に特定する名称である。親関節名３２２は、リンクＬを動作させる関節Ｊの関節名３１１である。子関節名３２３は、リンクＬｍに接続される関節Ｊ（ｍ＋１）の関節名３１１である。たとえば、リンクＬ１の場合、親関節名３２２はＪ１となり、子関節名３２３はＪ２となる。

リンク形状３２４は、リンクＬの実際の形状であり、たとえば、ＳＴＥＰ（Ｓｔａｎｄａｒｄ ｆｏｒ ｔｈｅ Ｅｘｃｈａｎｇｅ ｏｆ Ｐｒｏｄｕｃｔ ｍｏｄｅｌ ｄａｔａ）などのフォーマットで保存されるソリッドデータや、ＳＴＬ（ＳＴｅｒｅｏＬｉｔｈｏｇｒａｐｈｙ）などのフォーマットで保存されるポリゴンデータである。

図４は、障害物構成ＤＢ２１２の一例を示す説明図である。障害物構成ＤＢ２１２は、障害物を構成するデータを記憶するデータベースである。具体的には、たとえば、障害物構成ＤＢ２１２は、フィールドとして、障害物ＩＤ４０１と、障害物形状４０２と、障害物位置姿勢４０３と、を有する。同一行の各フィールドの値の組み合わせが、１つの障害物１０４を構成する。

障害物ＩＤ４０１は、障害物１０４を一意に特定する識別情報である。障害物形状４０２は、障害物１０４の実際の形状であり、たとえば、ＳＴＥＰなどのフォーマットで保存されるソリッドデータや、ＳＴＬなどのフォーマットで保存されるポリゴンデータである。障害物位置姿勢４０３は、グローバル座標系１１０における障害物１０４の中心位置（Ｘ，Ｙ，Ｚ）と姿勢（ロール角ｒ、ピッチ角ｐ、ヨー角ｙ）を規定する情報である。

図５は、開始関節角度ＤＢ２１３の一例を示す説明図である。開始関節角度ＤＢ２１３は、関節Ｊの開始関節角度５０１を記憶するデータベースである。開始関節角度５０１は、関節名３１１で特定される関節の回転軸３１４回りの角度の初期値、すなわち、動作開始前の角度である。回転軸３１４が複数ある場合（たとえば、回転軸３１４＝（１，１，０））、開始関節角度５０１は、回転軸３１４ごとに設定される。全関節Ｊ１～Ｊ４の開始関節角度５０１の値の列を、開始関節角度ベクトルθ１と称す。

図６は、目標位置姿勢ＤＢ２１４の一例を示す説明図である。目標位置姿勢ＤＢ２１４は、先端部１０２の目標位置および姿勢を記憶するデータベースである。具体的には、たとえば、目標位置姿勢ＤＢ２１４は、フィールドとして、姿勢ＩＤ６０１と、対象リンク名６０２と、位置姿勢情報６０３と、を有する。同一行の各フィールドの値の組み合わせが、１つの目標位置姿勢となる軌道点Ｐｎ（以下、目標位置姿勢Ｐｎと表記する場合もある）を構成する。

姿勢ＩＤ６０１は、姿勢を一意に特定する識別情報である。対象リンク名６０２は、先端部１０２に接続されているリンクＬのリンク名３２１である。位置姿勢情報６０３は、グローバル座標系１１０における先端部１０２の中心位置（Ｘ，Ｙ，Ｚ）と姿勢を規定する情報である。先端部１０２の姿勢とは、具体的には、たとえば、先端部１０２が接続される関節Ｊｍの姿勢（ロール角ｒｍ，ピッチ角ｐｍ，ヨー角ｙｍ）である。目標位置姿勢ＤＢ２１４のエントリは１以上存在するが、外部からいずれか１つのエントリが選択される。

図７は、手先位置候補算出方法ＤＢ２１５の一例を示す説明図である。手先位置候補算出方法ＤＢ２１５は、手先位置候補算出方法７０１を規定するデータベースである。具体的には、たとえば、手先位置候補算出方法ＤＢ２１５は、フィールドとして、手先位置候補算出方法７０１と、１ステップでの最大移動距離７０と、を有する。同一行の各フィールドの値の組み合わせが、１つの手先位置候補算出方法７０１を構成する。

手先位置候補算出方法７０１は、手先位置候補を算出する方法に該当する各種算出方法である。たとえば、軸並行（ＡＸＩＳ ＰＡＲＡＬＥＬＬ）やランダムなどの手法がある。1ステップでの最大移動距離７０２は、１ステップの処理（後述するステップＳ１４０６の処理１回分）のでの先端部１０２の移動距離の最大値である。

図８は、手先姿勢変動候補算出方法ＤＢ２１６の一例を示す説明図である。手先姿勢変動候補算出方法ＤＢ２１６は、手先姿勢変動候補算出方法を規定するデータベースである。具体的には、たとえば、手先姿勢変動候補算出方法ＤＢ２１６は、フィールドとして、ロール角最大変位８０１と、ピッチ角最大変位８０２と、ヨー角最大変位８０３と、１ステップでの最大変位８０４と、を有する。同一行の各フィールドの値の組み合わせが、１つの手先姿勢変動候補算出方法を構成する。

ロール角最大変位８０１は、ロール角ｒｍの変位量の最大値である。ピッチ角最大変位８０２は、ピッチ角ｐｍの変位量の最大値である。ヨー角最大変位８０３は、ヨー角ｙｍの変位の最大値である。ロール角最大変位８０１、ピッチ角最大変位８０２およびヨー角最大変位８０３は、手先位置候補算出方法７０１で用いられ、基準姿勢からの最大変位角である。基準姿勢とは、リンクＬ（ｍ－１）を長手方向をＸｍ軸とした場合の関節Ｊｍの姿勢（ｒｍ，ｐｍ，ｙｍ）である。１ステップでの最大変位８０４は、１回の処理（後述するステップＳ１４０７の処理１回分）での先端部１０２の姿勢の最大変位である。

図９は、制御時刻付与方法ＤＢ２１７の一例を示す説明図である。制御時刻付与方法ＤＢ２１７は、制御時刻付与方法を記憶するデータベースである。具体的には、たとえば、制御時刻付与方法ＤＢ２１７は、フィールドとして、パラメータＩＤ９０１と、手先最大速度９０２と、手先最大加速度９０３と、を有する。同一行の各フィールドの値の組み合わせが、１つの制御時刻付与方法を構成する。

パラメータＩＤ９０１は、手先最大速度９０２および手先最大加速度９０３で規定される制御時刻付与方法を一意に特定する識別情報である。手先最大速度９０２は、先端部１０２の速度の最大値である。手先最大加速度９０３は、先端部１０２の加速度の最大値である。

図１０は、軌道点ＤＢ２１８の一例を示す説明図である。軌道点ＤＢ２１８は、軌道点データを記憶するデータベースである。軌道点データは、軌道計画データ１２０の元になるデータである。軌道点ＤＢ２１８は、フィールドとして、軌道点ＩＤ１００１と、関節角度１００２と、関節角速度１００３と、関節角加速度１００４と、平滑化可否１００５と、制御時刻１００６と、を有する。同一行の各フィールドの値の組み合わせが、１つの軌道点Ｐを示す軌道点データを構成する。

軌道点ＩＤ１００１は、軌道点Ｐを一意に特定する識別情報である。軌道点Ｐ１～Ｐｎのうち、初期位置の軌道点Ｐ１を「始点」、最終位置の軌道点Ｐｎを「終点」と称する場合がある。

関節角度１００２は、図１に示した軌道点Ｐ１～Ｐｎの各々における関節Ｊの回転軸３１４回りの角度である。たとえば、軌道点ＩＤ１００１の「Ｐ１」が始点であれば、その関節角度１００２は開始関節角度５０１（θ１）になる。

関節角速度１００３は、図１に示した軌道点Ｐ１～Ｐｎの各々における関節Ｊの回転軸３１４回りの角速度である。関節角加速度１００４は、図１に示した軌道点Ｐ１～Ｐｎの各々における関節Ｊの回転軸３１４回りの角加速度である。

平滑化可否１００５は、軌道点Ｐの平滑化の可否を示す情報である。平滑化とは、ロボットアーム１０１の軌道を滑らかにする処理である。「ＦＡＬＳＥ」がその軌道点Ｐの平滑化不可を示し、「ＴＲＵＥ」がその軌道点Ｐの平滑化可能を示す。初期値は「ＴＲＵＥ」であり、平滑化が実行されると、「ＦＡＬＳＥ」に更新される（ステップＳ１８１０）。ただし、始点Ｐ１および終点Ｐｎについては、平滑化可否１００５の初期値は「ＦＡＬＳＥ」である。

制御時刻１００６は、ロボットアーム１０１がその軌道点Ｐに制御される時刻である。始点Ｐ１の制御時刻１００６を「０」とし、他の軌道点Ｐの制御時刻１００６は、始点Ｐ１の制御時刻１００６からの経過時間となる。また、制御時刻１００６は、１つ前の軌道点Ｐからの経過時間でもよい。制御時刻１００６は、後述する図１３および図２０の制御時刻１００６付与処理（ステップＳ１３０４）で付与される。

図１１は、手先探索データ２１９の一例を示す説明図である。手先探索データ２１９は、記憶デバイス２０２のキャッシュメモリに一時的に記憶され、また、ログとして保存されるデータである。手先探索データ２１９は、軌道点を探索するためのデータである。

手先探索データ２１９は、フィールドとして、探索点ＩＤ１１０１と、関節角度１１０２と、手先位置１１０３と、差分姿勢１１０４と、前探索点ＩＤ１１０５と、コスト１１０６と、探索済みフラグ１１０７と、を有する。

探索点ＩＤ１１０１は、探索点を一意に特定する識別情報である。探索点とは、先端部１０２の候補位置および候補姿勢を示す先端部１０２の候補点である。候補点は、初期の探索点（探索点ＩＤ：Ｄ００１）を除き、複数生成される。候補点には、枝番が付与される。図１１の（Ａ）では、初期の探索点（探索点ＩＤ：Ｄ００１）の手先位置１１０３からの移動先候補として、複数の探索点ＩＤ：Ｄ００２－１，Ｄ００２－２，Ｄ００２－３のエントリが生成されている。

関節角度１１０２は、先端部１０２の候補姿勢となる各関節Ｊの回転軸３１４回りの角度である。手先位置１１０３は、先端部１０２の候補位置となるグローバル座標系１１０の３次元位置である。手先位置１１０３は、関節角度１１０２に基づいて順運動学により決定される。なお、手先位置１１０３の姿勢は、先端部１０２に接続される関節Ｊｍの姿勢（ロール角、ピッチ角、ヨー角）となる。

差分姿勢１１０４は、球面補間関数ＳＬＥＲＰによる球面補間姿勢からの現在の手先姿勢の差分（Δｒ_current、Δｐ_current、Δｙ_current）である。具体的には、たとえば、差分姿勢１１０４は、１つ前の探索点における差分姿勢１１０４に変位量Δｒ、Δｐ、Δｙを加算した値である。初期の探索点（探索点ＩＤ：Ｄ００１）の差分姿勢１１０４については、１つ前の探索点が存在しないため、（０，０，０）となる。変位量Δｒ、Δｐ、Δｙは、いずれも１ステップでの最大変位８０４であり、それぞれ、ロール角最大変位８０１、ピッチ角最大変位８０２、ヨー角最大変位８０３まで加算可能である。

前探索点ＩＤ１１０５は、１つ前の探索点ＩＤ１１０１である。初期の探索点（探索点ＩＤ１１０１：Ｄ００１）については、１つ前の探索点が存在しないため、前探索点ＩＤ１１０５は「ＮＵＬＬ」となる。コスト１１０６は、探索点ＩＤ１１０１で特定される探索点と目標位置との間の距離に基づく値である。本例では、コスト１１０６は、当該距離が長いほど高くなる。初期の探索点（探索点ＩＤ１１０１：Ｄ００１）のコスト１１０６は、「０」に設定される。

図１１の（Ａ）では、初期の探索点（探索点ＩＤ１１０１：Ｄ００１）の手先位置１１０３からの移動先候補として、複数の探索点ＩＤ１１０１：Ｄ００２－１，Ｄ００２－２，Ｄ００２－３のエントリが生成されている。このうち、コスト１１０６が最小なエントリは、探索点ＩＤ１１０１：Ｄ００２－２である。したがって、（Ｂ）に示すように、探索点ＩＤ１１０１：Ｄ００２－２が、初期の探索点（探索点ＩＤ１１０１：Ｄ００１）の手先位置１１０３からの移動先に確定し、探索点ＩＤ１１０１：Ｄ００２－１、Ｄ００２－３のエントリは削除される。

探索済みフラグ１１０７は、そのエントリの探索点が探索済みであるか否かを示すフラグである。探索済みであれば「Ｔｒｕｅ」、探索済みでなければ「Ｆａｌｓｅ」である。探索済みフラグ１１０７の初期値は「Ｆａｌｓｅ」である。探索済みフラグ１１０７は、そのエントリの探索点が、１つ前の確定した探索点における先端部１０２の移動先に確定した場合に、「Ｆａｌｓｅ」から「Ｔｒｕｅ」に更新される。更新されなかったエントリは削除される。

図１１の例では、探索点ＩＤ１１０１：Ｄ００２－２が、初期の探索点（探索点ＩＤ１１０１：Ｄ００１）の手先位置１１０３からの移動先に確定したため、探索済みフラグ１１０７が「Ｆａｌｓｅ」から「Ｔｒｕｅ」に更新される。そして、探索済みフラグ１１０７が「Ｆａｌｓｅ」から「Ｔｒｕｅ」に更新されなかった探索点ＩＤ１１０１：Ｄ００２－１、Ｄ００２－３のエントリは削除される。

＜軌道計画生成装置１００の機能的構成例＞
図１２は、軌道計画生成装置１００の機能的構成例を示すブロック図である。軌道計画生成装置１００は、取得部１２０１と、探索部１２０２と、平滑化部１２０３と、付与部１２０４と、出力部１２０５と、を有する。取得部１２０１、探索部１２０２、平滑化部１２０３、付与部１２０４、および出力部１２０５は、具体的には、たとえば、記憶デバイス２０２に記憶されたプログラムをプロセッサ２０１に実行させることにより実現される機能である。

＜軌道計画生成処理手順例＞
図１３は、軌道計画生成装置１００による軌道計画生成処理手順例を示すフローチャートである。取得部１２０１は、探索部１２０２、平滑化部１２０３および付与部１２０４に必要なデータをと、記憶デバイス２０２から取得して、探索部１２０２、平滑化部１２０３および付与部１２０４に出力する（ステップＳ１３０１）。

つぎに、探索部１２０２は、目標位置までの探索点を探索する探索処理を実行する（ステップＳ１３０２）。具体的には、たとえば、探索部１２０２は、Ａ－Ｓｔａｒ法により、ロボットアーム１０１の開始関節角度５０１（θ１）となる軌道点Ｐ１から先端部１０２の目標位置姿勢Ｐｎまでの軌道データＴを算出する。探索処理（ステップＳ１３０２）の詳細は、図１４で後述する。

つぎに、平滑化部１２０３は、軌道データＴを平滑化する平滑化処理を実行する（ステップＳ１３０３）。平滑化処理（ステップＳ１３０３）の詳細は、図１８で後述する。つぎに、付与部１２０４は、先端部１０２の姿勢変化を考慮して、軌道点Ｐに制御時刻１００６を付与する付与処理を実行する（ステップＳ１３０４）。付与処理（ステップＳ１３０４）の詳細は、図２０で後述する。

つぎに、出力部１２０５は、図１に示したように、軌道データＴを再生可能な軌道計画データ１２０を生成して、出力デバイス２０４または通信ＩＦ２０５から出力する。

＜探索処理（ステップＳ１３０２）＞
図１４は、図１３に示した探索部１２０２による探索処理（ステップＳ１３０２）の詳細な処理手順例を示すフローチャートである。探索部１２０２は、取得部１２０１により開始関節角度５０１（θ１）を開始関節角度ＤＢ２１３から取得し、取得部１２０１により目標位置姿勢Ｐｎを目標位置姿勢ＤＢ２１４から取得する。そして、探索部１２０２は、手先探索データ２１９の探索点ＩＤ１１０１であるＤ００１を採番して、Ｄ００１のエントリの関節角度１１０２に、取得した開始関節角度５０１（θ１）を格納する（ステップＳ１４０１）。

つぎに、探索部１２０２は、ステップＳ１４０３～Ｓ１４０８のループ処理を実行する（ステップＳ１４０２）。具体的には、たとえば、探索部１２０２は、未探索（探索済みフラグ１１０７がＦａｌｓｅ）でかつコスト１１０６が最小の探索点を手先探索データ２１９から選択し、軌道点Ｐ_currentを決定する（ステップＳ１４０３）。つぎに、探索部１２０２は、軌道点Ｐ_currentが目標位置姿勢Ｐｎに近いか否かを判断する（ステップＳ１４０４）。具体的には、たとえば、探索部１２０２は、軌道点Ｐ_currentが目標位置姿勢Ｐｎを中心とする半径Ｒの球１５０１（図１５を参照）内に位置するか否かを判断する。

遠い場合、すなわち、半径Ｒの球内に位置しない場合（ステップＳ１４０４：Ｎｏ）、探索部１２０２は、ステップＳ１４０２のループ回数が奇数であるか偶数であるかを判断する（ステップＳ１４０５）。ステップＳ１４０５は、ステップＳ１４０６およびＳ１４０７を交互に実行するための処理である。

ループ回数が奇数である場合（ステップＳ１４０５：奇数）、手先位置候補算出方法ＤＢ２１５の手先位置候補算出方法７０１に基づき、現在の先端部１０２の軌道点Ｐ_currentからの移動先となる次の軌道点候補Ｐ_nextとなる探索点を複数算出し（ステップＳ１４０６）、ステップＳ１４０８に移行する。これにより、手先位置を滑らかに変化させることができる。具体的には、たとえば、現在の先端部１０２の軌道点Ｐ_currentとは、１つ前のループのステップＳ１４０７で得られた複数の探索点からステップＳ１４０３で選択された探索点である。

ただし、ループ回数が１回目である場合は、現在の先端部１０２の軌道点とは、軌道点Ｐ１である。探索部１２０２は、複数の探索点ＩＤ１１０１を発行し、探索点ＩＤ１１０１の各エントリの手先位置１１０３に、軌道点候補Ｐ_nextとなる先端部１０２の３次元位置を格納し、また、前探索点ＩＤ１１０５も格納する。

ここで、説明の便宜上、次の軌道点候補Ｐ_nextを構成する情報のうち、位置情報をО、姿勢情報をＱとする。現在の手先位置１１０３をＯ_current、球面補間姿勢からの差分姿勢１１０４をΔＱ_currentとすると、下記式（１）～（３）により、次の軌道点候補Ｐ_nextを構成する次の位置情報Ｏ_nextと次の姿勢情報Ｑ_nextとが探索点ごとに計算される。

上記式（１）のΔＯは、手先位置候補算出方法７０１に従って生成される移動量であり、たとえば、手先位置候補算出方法７０１として軸並行（ＡＸＩＳ ＰＡＲＡＬＬＥＬ）が指定されていた場合、最大移動距離７０２をｄとすると、ΔＯは、以下の６通り先端部１０２の移動方向ベクトルとなる。このように、次の位置情報Ｏ_nextとして６通りの位置情報Ｏｉ１～Ｏｉ６が得られる。

（±ｄ，０，０）^Ｔ
（０，±ｄ，０）^Ｔ
（０，０，±ｄ）^Ｔ

また、上記式（３）のＳＬＥＲＰは、球面補間関数を意味する。ステップＳ１４０６を、図１５を用いて説明する。

＜手先位置候補算出方法７０１に基づく軌道点候補Ｐ_nextの算出例＞
図１５は、ステップＳ１４０６の手先位置候補算出方法７０１に基づく軌道点候補Ｐ_nextの算出例を示す説明図である。図１５において、五角形の図形は先端部１０２を示し、突出した頂点１５００は物体１０３の保持面に対応する。図１５では、軌道点Ｐ１，Ｐ２，Ｐ３，Ｐ４，Ｐ５，Ｐ６，…，Ｐｉの順に生成され、次の軌道点候補Ｐ_nextとして、ステップＳ１４０６により、探索点Ｐｉ１～Ｐｉ６が生成された状態を示す。探索点Ｐｉ１～Ｐｉ６の位置は、上述した位置情報Ｏｉ１～Ｏｉ６で規定される。

探索部１２０２は、探索点Ｐｉ１～Ｐｉ６から、軌道点Ｐ１と目標位置姿勢Ｐｎを繋いだベクトル１５０２への正射影をとった長さとして、上記式（２）により、ｔｉ１～ｔｉ６を算出する。探索部１２０２は、このｔｉ１～ｔｉ６を用いて、上記式（３）の球面補間関数ＳＬＥＲＰにより、各探索点Ｐｉ１～Ｐｉ６での姿勢情報Ｑｉ１～Ｑｉ６を算出する。このようにして、探索点Ｐｉ１＝（Ｏｉ１，Ｑｉ１）～Ｐｉ６＝（Ｏｉ６，Ｑｉ６）が得られる。上記式（１）～（３）を用いることで、先端部１０２の姿勢変化を最小にしつつ、空間上を移動する先端部１０２の軌道点候補Ｐ_nextを算出することができる。

図１４に戻る。一方、ループ回数が偶数である場合（ステップＳ１４０５：偶数）、手先姿勢変動候補算出方法ＤＢ２１６に基づき、現在の先端部１０２の軌道点Ｐ_currentでの変位先となる次の軌道点候補Ｐ_nextを複数算出し（ステップＳ１４０７）、ステップＳ１４０８に移行する。これにより、手先姿勢を滑らかに変化させることができる。具体的には、たとえば、現在の先端部１０２の軌道点Ｐ_currentとは、１つ前のループのステップＳ１４０６で得られた複数の探索点からステップＳ１４０３で選択された探索点である。探索部１２０２は、複数の探索点ＩＤ１１０１を発行し、探索点ＩＤ１１０１の各エントリの手先位置１１０３に、軌道点候補Ｐ_nextとなる先端部１０２の３次元位置を格納し、前探索点ＩＤ１１０５も格納する。

具体的には、たとえば、探索部１２０２は、ステップＳ１４０６の軌道点候補Ｐ_nextを、下記式（４）～（１０）を用いて算出する。

上記式（４）～（７）のΔｒ_next，Δｐ_next、Δｙ_nextは、ロール角ｒ、ピッチ角ｐ、ヨー角ｙの変位量を示し、手先姿勢変動候補算出方法ＤＢ２１６の１ステップでの最大変位８０４である。ただし、Δｒ_next，Δｐ_next、Δｙ_nextは、軌道点Ｐが生成される都度累積加算されるため、ロール角最大変位８０１、ピッチ角最大変位８０２およびヨー角最大変位８０３を超えないように設定される。上記式（４）～（６）の右辺のΔｒ_current、Δｐ_current、およびΔｙ_currentは、球面補間姿勢からの差分姿勢１１０４である。

また、上記式（７）は、姿勢変位量ΔＱ_nextを算出する式である。上記式（７）の右辺のＲｘ，ＲｙおよびＲｚはそれぞれ、関節Ｊｎのｘｎ軸、ｙｎ軸、ｚｎ軸回りの回転を意味する。探索部１２０２は、これらの値から基準姿勢からの姿勢変位量ΔＱ_nextを計算し、姿勢変位量ΔＱ_nextを元に、次の姿勢情報Ｑ_nextを計算する。なお、このとき、軌道点候補Ｐ_nextの位置は変化しない。この候補算出処理を、図１６を用いて説明する。

＜手先姿勢変動候補算出方法に基づく軌道点候補Ｐｎｅｘｔの算出例＞
図１６は、ステップＳ１４０７の手先姿勢変動候補算出方法に基づく軌道点候補Ｐ_nextの算出例を示す説明図である。図１５と同様、五角形の図形は先端部１０２を示し、突出した頂点１５００は物体１０３の保持面に対応する。

図１６では、軌道点Ｐ１、Ｐ２，Ｐ３，Ｐ４，Ｐ５，Ｐ６，…，Ｐｊの順に生成され、次の軌道点候補Ｐ_nextとして、ステップＳ１４０７により、探索点Ｐｊ１～Ｐｊ２が生成された状態を示す。なお、軌道点Ｐｊは、１ステップ前のステップＳ１４０６で生成された探索点Ｐｉ１～Ｐｉ６の中でコスト１１０６が最小な探索点となる軌道点候補Ｐ_nextである。

探索点Ｐｊ１～Ｐｊ２の位置情報Ｏｊ１～Ｏｊ２は、軌道点Ｐｊの位置情報と同一であり（上記式（１０）を参照）、次の軌道点候補Ｐ_nextとなる探索点Ｐｊ１～Ｐｊ２の姿勢情報Ｑｉ１～Ｑｉ２は、上記式（９）の姿勢情報Ｑ_nextである。上記式（４）～（７）により、探索部１２０２は、先端部１０２の位置を変化させずに、姿勢だけを変化させた候補を算出することができる。この姿勢探索により、球面補間で姿勢を変化させるだけでなく、当該位置からの許容を許した姿勢を探索することが可能となる。

図１４に戻り、ステップＳ１４０６またはＳ１４０７のあと、探索部１２０２は、関節角度算出処理を実行する（ステップＳ１４０８）。関節角度算出処理（ステップＳ１４０８）は、先端部１０２の軌道点候補Ｐ_nextとなるようなロボットアーム１０１の関節角度θ_nextを算出する処理である。関節角度算出処理（ステップＳ１４０８）の詳細は、図１７で後述する。関節角度算出処理（ステップＳ１４０８）によって関節角度θ_nextの算出後、ループ処理（ステップＳ１４０２）により、ステップＳ１４０３に戻る。

＜関節角度算出処理（ステップＳ１４０８）＞
図１７は、図１４に示した関節角度算出処理（ステップＳ１４０８）の詳細な処理手順例を示すフローチャートである。探索部１２０２は、ステップＳ１４０６またはＳ１４０７で得られた軌道点候補分のループ処理を実行する（ステップＳ１７０１）。具体的には、たとえば、探索部１２０２は、軌道点候補Ｐ_nextになるような関節角度θ_nextを逆運動学により算出する（ステップＳ１７０２）。つぎに、探索部１２０２は、軌道点候補Ｐ_nextおよび関節角度θ_nextで特定されるロボットアーム１０１が、障害物構成ＤＢ２１２の障害物１０４に干渉するか否かを判断する（ステップＳ１７０３）。

干渉しない場合（ステップＳ１７０３：Ｎｏ）、探索部１２０２は、手先探索データ２１９の軌道点候補Ｐ_nextのエントリにおいて、関節角度１１０２に関節角度θ_nextを追加し、前探索点ＩＤ１１０５に、現在の探索点ＩＤ１１０１を追加し、差分姿勢１１０４に（Δｒ_next，Δｐ_next、Δｙ_next）を追加する（ステップＳ１７０４）。そして、探索部１２０２は、軌道点候補Ｐ_nextのコスト１１０６（Ｃｏｓｔ_next）を算出する（ステップＳ１７０５）。軌道点候補Ｐ_nextのコスト１１０６は、下記式（１１）により算出される。

Ｃｏｓｔ_next＝Ｃｏｓｔ_current＋Ｄｉｓｔａｎｃｅ（Ｐ_current，Ｐ_next）・・・（１１）

Ｄｉｓｔａｎｃｅ（Ａ，Ｂ）は、地点Ａ，Ｂ間のユークリッド距離を算出する距離関数である。上記式（１１）により、ステップＳ１４０６で複数の軌道点候補Ｐ_nextが得られた場合には、軌道点Ｐ_currentの位置と軌道点候補Ｐ_nextの位置との間の距離が算出される。なお、軌道点Ｐ_currentの位置および軌道点候補Ｐ_nextの位置は、それぞれ、先端部１０２の位置でもよく、先端部１０２および関節Ｊ１～Ｊｎの位置でもよい。

また、ステップＳ１４０７で複数の軌道点候補Ｐ_nextが得られた場合には、軌道点Ｐ_currentの姿勢と軌道点候補Ｐ_nextの姿勢との間の差分が距離として算出される。なお、軌道点Ｐ_currentの姿勢および軌道点候補Ｐ_nextの姿勢は、それぞれ、先端部１０２の姿勢でもよく、先端部１０２および関節Ｊ１～Ｊｎの姿勢でもよい。

軌道点Ｐ_currentと軌道点候補Ｐ_nextとの間の距離が短いほど、コスト１１０６は小さくなり、ステップＳ１４０３で選択されやすくなる。このあと、探索部１２０２は、探索済みフラグ１１０７をＦａｌｓｅからＴｒｕｅに更新する（ステップＳ１７０６）。そして、ステップＳ１７０８に移行する。

一方、ステップＳ１７０３において、干渉する場合（ステップＳ１７０３：Ｙｅｓ）、探索部１２０２は、当該軌道点候補Ｐ_nextのエントリを手先探索データ２１９から消去し（ステップＳ１７０７）、ステップＳ１７０８に移行する。ステップＳ１７０８では、探索部１２０２は、未選択の軌道点候補Ｐ_nextがあれば、ステップＳ１７０２に戻り、未選択の軌道点候補Ｐ_nextがなければ、ステップＳ１４０３に戻る。

＜平滑化処理（ステップＳ１３０３）＞
図１８は、図１３に示した平滑化部１２０３による平滑化処理（ステップＳ１３０３）の詳細な処理手順例を示すフローチャートである。平滑化部１２０３は、軌道データＴ内の各軌道点Ｐ１～Ｐｎの平滑化が終了するまで、ステップＳ１８０２～Ｓ１８１２のループ処理を実行する。すなわち、ステップＳ１８１３では、平滑化されていない軌道点Ｐがあれば、ステップＳ１８０２に戻り、平滑化されていない軌道点Ｐがなければ、付与処理（ステップＳ１３０４）に移行する。

平滑化部１２０３は、軌道データＴ内の平滑化不可点に挟まれる平滑化可能点の集合を出得する（ステップＳ１８０２）。平滑化不可点とは、平滑化可否１００５が「ＦＡＬＳＥ」の軌道点Ｐである。初期状態では、平滑化可否１００５が「ＦＡＬＳＥ」である軌道点Ｐは、軌道点Ｐ１、Ｐｎである。平滑化可能点とは、平滑化可否１００５が「ＴＲＵＥ」の軌道点Ｐである。初期状態では、軌道データＴ内の平滑化不可点に挟まれる平滑化可否１００５が「ＴＲＵＥ」の軌道点Ｐは、軌道点Ｐ２～Ｐｎ－１である。

平滑化部１２０３は、平滑化可能点の集合分、ステップＳ１８０４～Ｓ１８１１のループ処理を実行する（ステップＳ１８０３）。すなわち、ステップＳ１８１２では、ステップＳ１８０２で未取得の集合があれば、ステップＳ１８０４に戻り、未取得の集合がなければ、ステップＳ１８０２に戻る。

平滑化部１２０３は、関節角度空間１９００上での補間曲線を計算する（ステップＳ１８０４）。関節角度空間１９００とは、関節Ｊ１～Ｊｎで張られ、各軌道点の関節角度１００２が存在するベクトル空間である。平滑化可能点の集合の前後にある２つの平滑化不可点の関節角度１００２をそれぞれθ_ａ，θ_ｂとする。この関節角度θ_ａ，θ_ｂと、その前後の関節角度θ_ａ‐１，θ_ｂ＋１の値から、以下の制約を満たす補間方程式Ｆ（ｔ）（補間曲線ともいう）を計算する。

Ｆ（０）＝θ_ａ・・・（１２）
Ｆ（１）＝θ_ｂ・・・（１３）
Ｆ´（０）＝θ_ａ－θ_ａ－１・・・（１４）
Ｆ´（１）＝θ_ｂ＋１‐θ_ｂ・・・（１５）
Ｆ´´（０）＝Ｆ´´（１）＝０・・・（１６）

上記式（１２）～（１６）は、区間［θ_ａ－１、θ_ａ］、［θ_ｂ、θ_ｂ－１］を直線としたときの補間曲線における制約である。この補間曲線の求め方は一例であり、たとえば、すべての平滑化不可の軌道点に対応する関節角度１００２を通る３次のスプライン補間曲線の内、この区間に該当するものでもよい。

平滑化部１２０３は、補間曲線が計算されると、集合内の平滑化可能点の数分、ステップＳ１８０６～Ｓ１８１０のループ処理を実行する（ステップＳ１８０５）。具体的には、たとえば、平滑化部１２０３は、関節角度θｉについて平滑化後のθｉ´を算出する（ステップＳ１８０６）。平滑化部１２０３は、関節角度θｉがθｉ´である場合に、ロボットアーム１０１が障害物１０４に干渉するか否かを判断する（ステップＳ１８０８）。

干渉しない場合（ステップＳ１８０８：Ｎｏ）、軌道データＴ内の関節角度θｉをθｉ´に更新して（ステップＳ１８０９）、ステップＳ１８１１に移行する。ステップＳ１８１１では、平滑化部１２０３は、集合内に未選択の平滑化可能点があれば、ステップＳ１８０６に戻り、未選択の平滑化可能点がなければ、ステップＳ１８１２に移行する。

一方、干渉する場合（ステップＳ１８０８：Ｙｅｓ）、平滑化部１２０３は、軌道データＴ内の関節角度θｉの平滑化可否１００５を「不可」に更新して（ステップＳ１８１０）、ステップＳ１８１２に移行する。ステップＳ１８１２では、平滑化部１２０３は、未選択の平滑化可能点の集合があれば、ステップＳ１８０３に移行して未選択の平滑化可能点の集合を選択し、未選択の平滑化可能点の集合がなければ、ステップＳ１８１３に移行する。

図１９は、ステップＳ１８０４における補間曲線の計算例を示す説明図である。図１９は、区間［θ_ａ，θ_ｂ］を補間する例を示す。θ_aは、ある軌道点Ｐ_aにおける関節Ｊの関節角度１１０２である。θ_a-1は、ある軌道点Ｐ_a-1における関節Ｊの関節角度１１０２である。軌道点Ｐ_a-1は、時間的に始点Ｐ１以降でかつ軌道点Ｐ_a以前に出現する軌道点である。軌道点Ｐ_a-1は始点Ｐ１でもよい。θ_bは、ある軌道点Ｐ_bにおける関節Ｊの関節角度１１０２である。θ_b+1は、ある軌道点Ｐ_b+1における関節Ｊの関節角度１１０２である。軌道点Ｐ_bは、時間的に軌道点Ｐ_aより後に出現する軌道点である。軌道点Ｐ_b+1は、時間的に軌道点Ｐ_bより後でかつ終点Ｐｎ以前に出現する軌道点である。軌道点Ｐ_b+1 は終点Ｐｎでもよい。補間曲線１９０１は、関節角度空間１９００に存在する。関節角度空間１９００は、関節Ｊ１～Ｊｎを座標軸とするベクトル空間である。

区間［θ_ａ，θ_ｂ］の実線は、平滑化前において、区間［θ_ａ，θ_ｂ］の軌道点における関節角度θｉを結ぶ補間対象である。区間［θ_ａ，θ_ｂ］の一点鎖線は、軌道データＴ内の関節角度θｉをθｉ´に更新したときの補間曲線１９０１である。区間［θ_ａ，θ_ｂ］の点線は、ステップＳ１８０４で計算された補間曲線１９０１である。このように、平滑化部１２０３は、前後の関節角度θ_ａ‐１，θ_ｂ＋１から、補間方程式を求め、補間方程式上の点に向かって、少しずつ関節角度１００２を変更することで、軌道データＴの平滑化を実現する。これにより、ロボットアーム１０１は滑らかな軌道データＴで物体１０３を安定的に移動させる。

＜付与処理（ステップＳ１３０４）＞
図２０は、図１３に示した付与部１２０４による付与処理（ステップＳ１３０４）の詳細な処理手順例を示すフローチャートである。付与部１２０４は、平滑化処理（ステップＳ１３０３）後の軌道データＴを取得する（ステップＳ２００１）。つぎに、付与部１２０４は、関節情報ＤＢ３０１に記憶されている各関節Ｊの最大速度３１７を元に、すべての軌道点Ｐ１～Ｐｎに対して時刻Δｔｉを付与し、図１０のエントリである軌道点データの制御時刻１００６に書き込む。この時刻Δｔｉは、下記式（１７）で算出される。

上記式（１７）において、なお、ｋは関節番号を示し、ｉは軌道点ＩＤ１００１を示す。また、Ｖｍａｘ_ｋは、関節Ｊｋの最大速度３１７を示す。

つぎに、付与部１２０４は、制御時刻１００６の更新がなくなるまで、ステップＳ２００３～Ｓ２００７のループ処理を実行する（ステップＳ２００３）。ステップＳ２００８では、制御時刻１００６の更新がなくなった場合、ステップＳ１３０５に移行する。

つぎに、付与部１２０４は、すべての軌道点Ｐに対して、ステップＳ２００５～Ｓ２００６のループ処理を実行する（ステップＳ２００４）。ステップＳ２００７では、未選択の軌道点Ｐがあれば、ステップＳ２００５に戻り、未選択の軌道点Ｐがなければ、ステップＳ２００８に移行する。

付与部１２０４は、現在の制御時刻１００６で角加速度制約、先端部１０２の速度制約、および加速度制約を充足しているか否かを判断する（ステップＳ２００５）。軌道点Ｐｉにおける、関節の角速度θｉ´、角加速度θｉ´´、先端部１０２の速度ｖｉ、先端部１０２の加速度ａｉはそれぞれ、下記式（１８）～（２１）で算出される。

なお、上記式（１８）～（２１）で、Ｊはヤコビ行列を示す。上記式（１８）～（２１）により、現在の制御時刻１００６での角加速度、先端部１０２の速度、先端部１０２の加速度といったパラメータが計算可能である。付与部１２０４は、算出されたこれらパラメータがすべて、対応する上限値（関節Ｊの最大速度３１７、最大加速度３１８、手先最大速度９０２、手先最大加速度９０３）以下であれば、制約を充足していると判定して（ステップＳ２００５：Ｙｅｓ）、ステップＳ２００７に移行し、算出されたこれらパラメータがすべて、対応する上限値以下でない場合は制約を充足していないと判定して（ステップＳ２００５：Ｎｏ）、ステップＳ２００６に移行する。ステップＳ２００６では、付与部１２０４は、現在の制御時刻１００６に時刻Δｔｉを加算して（ステップＳ２００６）、ステップＳ２００７に移行する。

この付与処理（ステップＳ１３０４）により、各軌道点間の動作は、制約を満たし、かつ、制御時間内での動作となる。したがって、軌道計画の再現性の向上を図ることができる。

以上に説明したように軌道計画生成装置１００によれば、軌道点Ｐを通りかつ、先端部１０２の姿勢を滑らかにつなぎ、ロボットアーム１０１が動作しやすい軌道データＴを生成することができる。これにより、物体１０３の安定的に保持し、障害物への緩衝を抑制し、高速に動作するロボットアーム１０１の軌道データＴの計画を生成することができる。

また、上述した実施例にかかる軌道計画生成装置１００は、下記（１）～（１３）のように構成することもできる。

（１）たとえば、軌道計画生成装置１００は、始点Ｐ１から終点（目標位置姿勢）Ｐｎの間における先端部１０２の位置および姿勢を示す第１軌道点Ｐ_currentからの所定距離（１ステップでの最大移動距離７０２）内の先端部１０２の移動先となる複数の位置候補を探索する第１探索処理（ステップＳ１４０６）と、始点Ｐ１および終点Ｐｎでの先端部１０２の姿勢に基づく球面補間により許容範囲（１ステップでの最大変位８０４）内で変化する先端部１０２の複数の姿勢候補を探索する第２探索処理（ステップＳ１４０７）と、第１探索処理によって探索された複数の位置候補および第２探索処理によって探索された複数の姿勢候補に基づいて、第１軌道点Ｐ_currentからの先端部１０２の移動先の位置および姿勢を示す第２軌道点Ｐ_nextを決定する決定処理（ステップＳ１４０３）と、第１軌道点Ｐ_currentおよび決定処理によって決定された第２軌道点Ｐ_nextを含む軌道データＴを出力する出力処理（ステップＳ１３０５）と、を実行する。

これにより、先端部１０２で保持する物体１０３の安定化を図る計画を立案することができる。

（２）また、上記（１）において、軌道計画生成装置１００は、複数の位置候補の各々について、第１軌道点Ｐ_currentからの距離に関するコストを算出する算出処理（ステップＳ１７０５）を実行し、決定処理（ステップＳ１４０３）では、軌道計画生成装置１００は、算出処理によって算出された複数のコストに基づいて、第２軌道点Ｐ_nextの位置を決定する。

これにより、先端部１０２で保持する物体１０３が安定化する適切な位置候補を第２軌道点Ｐ_nextの位置に決定することができる。

（３）また、上記（２）において、決定処理（ステップＳ１４０３）では、軌道計画生成装置１００は、複数のコストのうち最小コストの算出元となる位置候補を第２軌道点Ｐ_nextの位置に決定する。

これにより、先端部１０２の移動距離が最小となる位置候補を第２軌道点Ｐ_nextの位置に決定することができ、先端部１０２で保持する物体１０３の安定化を図る計画を立案することができる。

（４）また、上記（１）において、軌道計画生成装置１００は、複数の姿勢候補の各々について、第１軌道点Ｐ_currentからの姿勢変化に関するコストを算出する算出処理（ステップＳ１７０５）を実行し、決定処理（ステップＳ１４０３）では、軌道計画生成装置１００は、算出処理によって算出された複数のコストに基づいて、第２軌道点Ｐ_nextの姿勢を決定する。

これにより、先端部１０２で保持する物体１０３が安定化する適切な姿勢候補を第２軌道点Ｐ_nextの姿勢に決定することができる。

（５）また、上記（４）において、決定処理（ステップＳ１４０３）では、軌道計画生成装置１００は、複数のコストのうち最小コストの算出元となる姿勢候補を第２軌道点Ｐ_nextの姿勢に決定する。

これにより、先端部１０２の姿勢変化が最小となる位置候補を第２軌道点Ｐ_nextの姿勢に決定することができ、先端部１０２で保持する物体１０３の安定化を図る計画を立案することができる。

（６）また、上記（１）において、軌道計画生成装置１００は、複数の位置候補の各々について、複数の関節Ｊの回転角を算出する算出処理（ステップＳ１７０２）と、ロボットアーム１０１が存在する空間に配置された障害物１０４の構造データ（障害物構成ＤＢ２１２）に基づいて、算出処理による算出結果に基づくロボットアーム１０１の複数の姿勢の各々においてロボットアーム１０１が障害物１０４に干渉するか否かを判定する判定処理（ステップＳ１７０３）と、を実行し、決定処理（ステップＳ１４０３）では、軌道計画生成装置１００は、判定処理による判定結果に基づいて、第２軌道点Ｐ_nextを決定する。

これにより、障害物１０４に干渉しないようにロボットアーム１０１の形状を決めることができ、障害物１０４への干渉による先端部１０２からの物体１０３の脱落を抑制することができる。

（７）また、上記（１）において、軌道計画生成装置１００は、第１軌道点と第２軌道点との間の軌道を平滑化する平滑化処理（ステップＳ１３０３）を実行し、出力処理（ステップＳ１３０５）では、軌道計画生成装置１００は、平滑化処理による平滑化後の軌道データＴを出力する。

これにより、ロボットアーム１０１が可動しても、先端部１０２で保持する物体１０３の安定化を図る計画を立案することができる。

（８）また、上記（１）において、平滑化処理（ステップＳ１３０３）では、軌道計画生成装置１００は、始点Ｐ以降でかつ第１軌道点Ｐ_currentより前の第３軌道点における前記複数の関節Ｊの関節角度θａと、第２軌道点Ｐ_nextより後でかつ前記終点Ｐｎ以前の第４軌道点における前記複数の関節Ｊの関節角度θｂと、の間を結ぶ補間曲線１９０１を所定の制約に基づいて生成し、補間曲線１９０１に近づくように、第１軌道点Ｐ_currentにおける複数の関節Ｊの関節角度と、第２軌道点Ｐ_nextにおける複数の関節Ｊの関節角度と、を更新する（Ｓ１８０６、図１９）。

物体１０３を保持する先端部１０２の移動軌跡が安定化するようなロボットアーム１０１の形状を特定することができる。

（９）また、上記（８）において、所定の制約は、第３軌道点における複数の関節の角速度および第４軌道点における複数の関節の角速度の差が許容範囲内である（式（１４）、（１５））。

（１０）また、上記（８）において、所定の制約は、第３軌道点における複数の関節の角加速度および第４軌道点における複数の関節の角加速度の差が許容範囲内である（式（１６））。

これにより、ロボットアーム１０１の可動による先端部１０２からの物体１０３の脱落を抑制することができる。

（１１）また、上記（８）において、平滑化処理（ステップＳ１３０３）では、軌道計画生成装置１００は、ロボットアーム１０１が存在する空間に配置された障害物１０４の構造データ（障害物構成ＤＢ２１２）に基づいて、第１軌道点Ｐ_currentにおける更新後の複数の関節の関節角度と、第２軌道点Ｐ_nextにおける更新後の複数の関節の関節角度と、によるロボットアーム１０１の姿勢において、ロボットアーム１０１が障害物１０４に干渉するか否かを判定し、ロボットアーム１０１が障害物１０４に干渉しないと判定された場合に、第１軌道点Ｐ_currentにおける更新後の複数の関節の関節角度と、第２軌道点Ｐ_nextにおける更新後の複数の関節の関節角度と、により、第１軌道点Ｐ_currentおよび第２軌道点Ｐ_nextを更新する（Ｓ１８０９）。

これにより、障害物１０４に干渉しないようにロボットアーム１０１の移動軌跡を決めることができ、障害物１０４への干渉による先端部１０２からの物体１０３の脱落を抑制することができる。

（１２）また、上記（１）において、軌道計画生成装置１００は、関節Ｊの速度制約に基づいて、ロボットアーム１０１が第１軌道点Ｐ_currentから第２軌道点Ｐ_nextへの軌道となるように動作する制御時間を付与する付与処理（ステップＳ１３０４）を実行する。

これにより、先端部１０２で保持する物体１０３が安定化するようなロボットアーム１０１の動作時間を確認することができる。

（１３）また、上記（１２）において、付与処理（ステップＳ１３０４）では、軌道計画生成装置１００は、速度制約を充足しない場合、第１軌道点Ｐ_currentから第２軌道点Ｐ_nextへの軌道に追加時間を付与する。

これにより、ロボットアーム１０１の動きに応じた時間を付与することができ、ロボットアーム１０１の動作の再現性の向上を図ることができる。

なお、本発明は前述した実施例に限定されるものではなく、添付した特許請求の範囲の趣旨内における様々な変形例及び同等の構成が含まれる。例えば、前述した実施例は本発明を分かりやすく説明するために詳細に説明したものであり、必ずしも説明した全ての構成を備えるものに本発明は限定されない。また、ある実施例の構成の一部を他の実施例の構成に置き換えてもよい。また、ある実施例の構成に他の実施例の構成を加えてもよい。また、各実施例の構成の一部について、他の構成の追加、削除、または置換をしてもよい。

また、前述した各構成、機能、処理部、処理手段等は、それらの一部又は全部を、例えば集積回路で設計する等により、ハードウェアで実現してもよく、プロセッサ２０１がそれぞれの機能を実現するプログラムを解釈し実行することにより、ソフトウェアで実現してもよい。

各機能を実現するプログラム、テーブル、ファイル等の情報は、メモリ、ハードディスク、ＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）等の記憶装置、又は、ＩＣ（Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）カード、ＳＤカード、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）の記録媒体に格納することができる。

また、制御線や情報線は説明上必要と考えられるものを示しており、実装上必要な全ての制御線や情報線を示しているとは限らない。実際には、ほとんど全ての構成が相互に接続されていると考えてよい。

１００ 軌道計画生成装置
１０１ ロボットアーム
１０２ 先端部
１０３ 物体
１０４ 障害物
１２０ 軌道計画データ
２１１ ロボットアーム構成ＤＢ
２１２ 障害物構成ＤＢ
２１３ 開始関節角度ＤＢ
２１４ 目標位置姿勢ＤＢ
２１５ 手先位置候補算出方法ＤＢ
２１６ 手先姿勢変動候補算出方法ＤＢ
２１７ 制御時刻付与方法ＤＢ
２１８ 軌道点ＤＢ
２１９ 手先探索データ
３０１ 関節情報ＤＢ
３０２ リンク情報ＤＢ
１２０１ 取得部
１２０２ 探索部
１２０３ 平滑化部
１２０４ 付与部
１２０５ 出力部

Claims (10)

1. プログラムを実行するプロセッサと、前記プログラムを記憶する記憶デバイスと、を有し、複数の関節を有するロボットアームの先端部が始点から終点に移動する軌道計画を生成する軌道計画生成装置であって、
   前記プロセッサは、
   前記始点から前記終点の間における前記先端部の位置および姿勢を示す軌道点からの所定距離内の前記先端部の移動先となる複数の位置候補を探索し、前記複数の位置候補について、前記先端部の姿勢に基づく第１球面補間により、前記先端部の複数の第１姿勢候補を探索することにより、複数組の位置候補および第１姿勢候補である複数の軌道点候補を出力する第１探索処理と、
   前記第１探索処理によって探索された複数の軌道点候補の中から、前記軌道点からの前記先端部の移動先の位置および姿勢を示す移動先の軌道点を決定する第１決定処理と、
   前記軌道点および前記第１決定処理によって決定された前記移動先の軌道点を含む軌道データを出力する出力処理と、
   を実行することを特徴とする軌道計画生成装置。
2. 請求項１に記載の軌道計画生成装置であって、
   前記第１決定処理では、前記プロセッサは、前記複数の位置候補の各々と前記軌道点の位置との距離に関する第１コストに基づいて、前記移動先の軌道点を決定する、
   ことを特徴とする軌道計画生成装置。
3. 請求項１に記載の軌道計画生成装置であって、
   前記プロセッサは、
   前記第１決定処理によって決定された前記移動先の軌道点の姿勢に基づく第２球面補間により、前記先端部の複数の第２姿勢候補を探索する第２探索処理と、
   前記第２探索処理によって探索された前記複数の第２姿勢候補の各々と前記軌道点での前記先端部の姿勢との距離に関する第２コストに基づいて、前記複数の第２姿勢候補の中から前記軌道点の姿勢を再決定する第２決定処理と、
   を実行することを特徴とする軌道計画生成装置。
4. 請求項１に記載の軌道計画生成装置であって、
   前記プロセッサは、
   前記複数の軌道点候補の各々について、前記先端部が前記軌道点候補となるような前記複数の関節の関節角度を逆運動学により算出し、前記軌道点候補に前記関節角度を追加する算出処理と、
   を実行することを特徴とする軌道計画生成装置。
5. 請求項４に記載の軌道計画生成装置であって、
   前記算出処理では、前記プロセッサは、前記ロボットアームが存在する空間に配置された障害物の構造データに基づいて、前記軌道点候補および前記関節角度で特定される前記ロボットアームが、前記障害物に干渉しない場合に、前記軌道点候補に前記関節角度を追加する、
   ことを特徴とする軌道計画生成装置。
6. 請求項１に記載の軌道計画生成装置であって、
   前記プロセッサは、
   前記軌道点と前記移動先の軌道点との間の軌道を平滑化する平滑化処理を実行し、
   前記出力処理では、前記プロセッサは、前記平滑化処理による平滑化後の軌道データを出力する、
   ことを特徴とする軌道計画生成装置。
7. 請求項１に記載の軌道計画生成装置であって、
   前記プロセッサは、
   前記関節の速度制約に基づいて、前記ロボットアームが前記軌道点から前記移動先の軌道点への軌道となるように動作する制御時間を付与する付与処理と、
   を実行することを特徴とする軌道計画生成装置。
8. 請求項７に記載の軌道計画生成装置であって、
   前記付与処理では、前記プロセッサは、前記速度制約を充足しない場合、前記軌道点から前記移動先の軌道点への軌道に追加時間を付与する、
   ことを特徴とする軌道計画生成装置。
9. プログラムを実行するプロセッサと、前記プログラムを記憶する記憶デバイスと、を有し、複数の関節を有するロボットアームの先端部が始点から終点に移動する軌道計画を生成する軌道計画生成装置が実行する軌道計画生成方法であって、
   前記軌道計画生成方法は、
   前記プロセッサが、
   前記始点から前記終点の間における前記先端部の位置および姿勢を示す軌道点からの所定距離内の前記先端部の移動先となる複数の位置候補を探索し、前記複数の位置候補について、前記先端部の姿勢に基づく第１球面補間により、前記先端部の複数の第１姿勢候補を探索することにより、複数組の位置候補および第１姿勢候補である複数の軌道点候補を出力する第１探索処理と、
   前記第１探索処理によって探索された複数の軌道点候補の中から、前記軌道点からの前記先端部の移動先の位置および姿勢を示す移動先の軌道点を決定する第１決定処理と、
   前記軌道点および前記第１決定処理によって決定された前記移動先の軌道点を含む軌道データを出力する出力処理と、
   を実行することを特徴とする軌道計画生成方法。
10. 複数の関節を有するロボットアームの先端部が始点から終点に移動する軌道計画の生成をプロセッサに実行させる軌道計画生成プログラムであって、
    前記プロセッサに、
    前記始点から前記終点の間における前記先端部の位置および姿勢を示す軌道点からの所定距離内の前記先端部の移動先となる複数の位置候補を探索し、前記複数の位置候補について、前記先端部の姿勢に基づく第１球面補間により、前記先端部の複数の第１姿勢候補を探索することにより、複数組の位置候補および第１姿勢候補である複数の軌道点候補を出力する第１探索処理と、
    前記第１探索処理によって探索された複数の軌道点候補の中から、前記軌道点からの前記先端部の移動先の位置および姿勢を示す移動先の軌道点を決定する第１決定処理と、
    前記軌道点および前記第１決定処理によって決定された前記移動先の軌道点を含む軌道データを出力する出力処理と、
    を実行させることを特徴とする軌道計画生成プログラム。

Images