特開 2023-68268 軌道計画装置、軌道計画方法および軌道計画プログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2023068268

(43)【公開日】2023-05-17

(54)【発明の名称】軌道計画装置、軌道計画方法および軌道計画プログラム

(51)【国際特許分類】

   B25J 9/10 20060101AFI20230510BHJP

【ＦＩ】

B25J9/10 A

【審査請求】未請求

【請求項の数】10

【出願形態】ＯＬ

(21)【出願番号】P 2021179181

(22)【出願日】2021-11-02

(71)【出願人】

【識別番号】000005108

【氏名又は名称】株式会社日立製作所

(74)【代理人】

【識別番号】110001678

【氏名又は名称】藤央弁理士法人

(72)【発明者】

【氏名】山内 雄太

(72)【発明者】

【氏名】月舘 統宙

(72)【発明者】

【氏名】中須 信昭

【テーマコード（参考）】

3C707

【Ｆターム（参考）】

3C707BS01

3C707BS12

3C707LT01

3C707MS14

3C707MS15

(57)【要約】

【課題】ロボットを複数連結した機構に対してもリアルタイムでの軌道計画を実現する。
【解決手段】第１のロボットと、第１のロボットの動作によって移動可能となるように第１のロボットに連結した第２のロボットと、を制御するための軌道を計画する軌道計画装置であって、第２のロボットのアームの構成を示す情報と、第２のロボットの手先が順次経由する複数の経由点における手先の位置及び姿勢を示す情報と、を保持し、第２のロボットのアームの構成と、各経由点における手先の位置及び姿勢と、に基づいて、各経由点における特異点曲面を計算し、各経由点について計算された第２のロボットの特異点曲面と、第２のロボットの可動範囲と、に基づいて、第１のロボットの目標位置を決定し、第１のロボットの目標位置までの軌道を計画し、第１のロボットの目標位置における第２のロボットの手先が複数の経由点を経由する軌道を計画する。
【選択図】図２

【特許請求の範囲】

【請求項1】

第１のロボットと、前記第１のロボットの動作によって移動可能となるように前記第１のロボットに連結した第２のロボットと、を制御するための軌道を計画する軌道計画装置であって、
処理装置と、記憶装置と、を有し、
前記記憶装置は、前記第２のロボットのアームの構成を示す情報と、前記第２のロボットの手先が順次経由する複数の経由点における前記手先の位置及び姿勢を示す情報と、を保持し、
前記処理装置は、
前記第２のロボットのアームの構成と、前記各経由点における前記手先の位置及び姿勢と、に基づいて、前記各経由点における前記第２のロボットの特異点の集合である特異点曲面を計算し、
前記各経由点について計算された前記第２のロボットの前記特異点曲面と、前記第２のロボットの可動範囲と、に基づいて、前記第１のロボットの目標位置を決定し、
前記第１のロボットの目標位置までの軌道を計画し、
前記第１のロボットの目標位置における前記第２のロボットの手先が前記複数の経由点を経由する軌道を計画することを特徴とする軌道計画装置。

【請求項2】

請求項１に記載の軌道計画装置であって、
前記処理装置は、
前記複数の経由点を、各々が１以上の前記経由点を含む複数の部分集合に分割し、
前記部分集合ごとに、前記各経由点について計算された前記第２のロボットの前記特異点曲面と、前記第２のロボットの可動範囲と、に基づいて、前記部分集合に含まれる１以上の前記経由点の全てが、前記第２のロボットの可動範囲内かつ前記各経由点の前記特異点曲面外の範囲の積集合に含まれるように、前記第１のロボットの目標位置を決定し、
前記部分集合ごとに決定した前記第１のロボットの目標位置ごとに、前記第１のロボットの軌道を計画し、
前記部分集合ごとに、前記第１のロボットの目標位置における前記第２のロボットの手先が前記部分集合に含まれる１以上の前記経由点を経由する軌道を計画することを特徴とする軌道計画装置。

【請求項3】

請求項２に記載の軌道計画装置であって、
前記処理装置は、
前記第２のロボットの手先が前記複数の経由点を経由する軌道の計画に失敗した場合、前記第１のロボットの目標位置を変更し、
変更された前記第１のロボットの目標位置における前記第２のロボットの手先が前記複数の経由点を経由する軌道を計画することを特徴とする軌道計画装置。

【請求項4】

請求項３に記載の軌道計画装置であって、
前記処理装置は、前記第２のロボットの可動範囲内で、前記第２のロボットの特異点曲面を通過せず、かつ、他の物体と干渉しない軌道を計画できなかった場合に、前記第２のロボットの手先が前記複数の経由点を経由する軌道の計画に失敗したと判定することを特徴とする軌道計画装置。

【請求項5】

請求項３に記載の軌道計画装置であって、
前記処理装置は、前記変更された第１のロボットの目標位置における前記第２のロボットの手先が前記複数の経由点を経由する軌道の計画に失敗した場合に、前記複数の経由点を、各々が１以上の前記経由点を含む複数の部分集合に分割することを特徴とする軌道計画装置。

【請求項6】

請求項５に記載の軌道計画装置であって、
前記処理装置は、いずれかの前記部分集合において、前記第１のロボットの目標位置における前記第２のロボットの手先が前記部分集合に含まれる１以上の前記経由点を経由する軌道の計画に失敗した場合に、前記部分集合の分割数を増加させることを特徴とする軌道計画装置。

【請求項7】

請求項１に記載の軌道計画装置であって、
前記第２のロボットは、６軸の自由度を持つロボットであることを特徴とする軌道計画装置。

【請求項8】

請求項１に記載の軌道計画装置であって、
前記第２のロボットは、６軸の垂直多関節ロボットであることを特徴とする軌道計画装置。

【請求項9】

軌道計画装置が第１のロボットと、前記第１のロボットの動作によって移動可能となるように前記第１のロボットに連結した第２のロボットと、を制御するための軌道を計画する軌道計画方法であって、
前記軌道計画装置は、処理装置と、記憶装置と、を有し、
前記記憶装置は、前記第２のロボットのアームの構成を示す情報と、前記第２のロボットの手先が順次経由する複数の経由点における前記手先の位置及び姿勢を示す情報と、を保持し、
前記軌道計画方法は、
前記処理装置が、前記第２のロボットのアームの構成と、前記各経由点における前記手先の位置及び姿勢と、に基づいて、前記各経由点における前記第２のロボットの特異点の集合である特異点曲面を計算する手順と、
前記処理装置が、前記各経由点について計算された前記第２のロボットの前記特異点曲面と、前記第２のロボットの可動範囲と、に基づいて、前記第１のロボットの目標位置を決定する手順と、
前記処理装置が、前記第１のロボットの目標位置までの軌道を計画する手順と、
前記処理装置が、前記第１のロボットの目標位置における前記第２のロボットの手先が前記複数の経由点を経由する軌道を計画する手順と、を含むことを特徴とする軌道計画方法。

【請求項10】

第１のロボットと、前記第１のロボットの動作によって移動可能となるように前記第１のロボットに連結した第２のロボットと、を制御するための軌道を計画する軌道計画装置を制御するための軌道計画プログラムであって、
前記軌道計画装置は、処理装置と、記憶装置と、を有し、
前記記憶装置は、前記第２のロボットのアームの構成を示す情報と、前記第２のロボットの手先が順次経由する複数の経由点における前記手先の位置及び姿勢を示す情報と、を保持し、
前記軌道計画プログラムは、
前記第２のロボットのアームの構成と、前記各経由点における前記手先の位置及び姿勢と、に基づいて、前記各経由点における前記第２のロボットの特異点の集合である特異点曲面を計算する手順と、
前記各経由点について計算された前記第２のロボットの前記特異点曲面と、前記第２のロボットの可動範囲と、に基づいて、前記第１のロボットの目標位置を決定する手順と、
前記第１のロボットの目標位置までの軌道を計画する手順と、
前記第１のロボットの目標位置における前記第２のロボットの手先が前記複数の経由点を経由する軌道を計画する手順と、を前記処理装置に実行させることを特徴とする軌道計画プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

本技術分野の背景技術として、特開平１１－１９８０７１号公報（特許文献１）がある。特許文献１には、「しかしながら、冗長ロボット１の冗長軸２が固定された場合に、冗長軸２以外の残りの軸において、２関節ロボットに特有の問題が生じることがある。即ち２関節ロボット１１ではアームが重なる位置に特異点５と呼ばれる地点があり、アーム上の一点を直線運動させるような場合に、その直線軌跡と特異点５とがずれているとその特異点５を経過することができずに動作不能となってしまうことがあるという問題がある。＜中略＞そこで、本発明は、アームの動作に際し、冗長軸の適正位置を決定して直線動作を補間することができる冗長ロボットの冗長軸位置決定方法を提供することを目的とする。＜中略＞かかる目的を達成するため、請求項１記載の発明の冗長ロボットの冗長軸位置決定方法では、冗長軸を構成する移動部材に関節部を介して順次連結された２つの回動可能なアームを有し、該アーム上の所定位置を直線動作させようとするとアームがある平面において冗長な自由度を有することになる冗長ロボットの冗長軸位置決定方法法において、互いに連結された２つのアームが重なったときのロボットアームの所定の位置である特異点に関して、冗長軸を動かすことによってこの特異点の軌跡を求める一方、与えられた目標位置に対する所定位置の移動方向をアクセス方向として定め、特異点軌跡と目標位置を通るアクセス方向に平行な直線とにより形成される交点を基準として冗長軸の位置を求めるようにしている。」と記載されている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開平１１－１９８０７１号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

特許文献１には、スカラロボットの特異点を回避するための冗長軸の位置を、アクセス方向を用いて決定する方法が記載されている。しかし、特許文献１の手法では、冗長軸の位置決定方法がスカラロボットに限定されており、またアクセス方向を各経由点に定める必要があるため、単軸ロボット又は複数軸のロボットの上に６軸ロボットを乗せた構造に対して適切に単軸ロボット又は複数軸ロボットの位置を計画することができない。

【0005】

そこで本発明は、６軸ロボットの可動範囲と各経由点の姿勢からなる特異点曲面を基に、もう一方のロボットの目標位置を算出し、ロボットの軌道計画を行う。その後、経由点内の各点を通過するようにもう一方のロボットの軌道計画を行うことで、実行可能な複数のロボットに跨る軌道を短時間で算出する軌道計画装置を提供する。

【課題を解決するための手段】

【0006】

上記課題を解決するために、例えば特許請求の範囲に記載の構成を採用する。本願は上記課題を解決する手段を複数含んでいるが、その一例を挙げるならば、第１のロボットと、前記第１のロボットの動作によって移動可能となるように前記第１のロボットに連結した第２のロボットと、を制御するための軌道を計画する軌道計画装置であって、処理装置と、記憶装置と、を有し、前記記憶装置は、前記第２のロボットのアームの構成を示す情報と、前記第２のロボットの手先が順次経由する複数の経由点における前記手先の位置及び姿勢を示す情報と、を保持し、前記処理装置は、前記第２のロボットのアームの構成と、前記各経由点における前記手先の位置及び姿勢と、に基づいて、前記各経由点における前記第２のロボットの特異点の集合である特異点曲面を計算し、前記各経由点について計算された前記第２のロボットの前記特異点曲面と、前記第２のロボットの可動範囲と、に基づいて、前記第１のロボットの目標位置を決定し、前記第１のロボットの目標位置までの軌道を計画し、前記第１のロボットの目標位置における前記第２のロボットの手先が前記複数の経由点を経由する軌道を計画することを特徴とする。

【発明の効果】

【0007】

本発明の一態様によれば、第２のロボットの可動範囲と特異点曲面を基に、第１のロボットの目標位置を算出することで、ロボットを複数連結した機構に対してもリアルタイムでの軌道計画を実現することができる。

【0008】

上記した以外の課題、構成及び効果は、以下の実施形態の説明により明らかにされる。

【図面の簡単な説明】

【0009】

【図1】本発明の実施例における軌道計画装置および周辺機器を含むシステムの構成の一例を示すブロック図である。

【図2】本発明の実施例における軌道計画装置の統合軌道計画部が行う軌道計画処理の一例を示すフローチャートである。

【図3】本発明の実施例におけるロボットアーム構成記憶部に格納されるロボットアーム構成データのテーブル構成の一例を示す説明図である。

【図4】本発明の実施例における干渉物構成記憶部に格納される干渉物構成データのテーブル構成の一例を示す説明図である。

【図5】本発明の実施例における経由点記憶部に格納される経由点データのテーブル構成の一例を示す説明図である。

【図6】本発明の実施例における軌道記憶部に格納される軌道データのテーブル構成の一例を示す説明図である。

【図7】本発明の実施例におけるロボットの特異点曲面の一例を示す説明図である。

【図8】本発明の実施例におけるロボットの特異点曲面の一例を示す説明図である。

【図9】本発明の実施例におけるロボットの経由点の一例を示す説明図である。

【図10】本発明の実施例におけるロボットの経由点の分割方法の一例を示す説明図である。

【図11】本発明の実施例におけるロボットの各経由点における特異点曲面の一例を示す説明図である。

【図12】本発明の実施例におけるロボットの特異点曲面の積集合の一例を示す説明図である。

【図13】本発明の実施例におけるロボットの軌道の一例を示す説明図である。

【図14】本発明の実施例におけるロボットの軌道の一例を示す説明図である。

【図15】本発明の実施例における軌道計画装置が出力する出力画面の一例を示す説明図である。

【発明を実施するための形態】

【0010】

以下、図面を用いて実施例を説明する。なお、実施の形態を説明するための全図において、同一部には原則として同一符号を付し、その繰り返しの説明は省略する。

【0011】

本実施例では、本発明の基本の形態となる軌道計画装置の例を説明する。

【0012】

［システム構成］
図１は、本発明の実施例における軌道計画装置１００および周辺機器を含むシステムの構成の一例を示すブロック図である。

【0013】

システム全体は、軌道計画装置１００、入出力装置１４０、統合制御装置１５０、単軸ロボット１６０（以下、本実施例において「ロボット１」とも記載する）、６軸ロボット１７０（以下、本実施例において「ロボット２」とも記載する）を有する。ユーザは、入出力装置１４０の操作を通じて軌道計画装置１００の機能を利用する。軌道計画装置１００は、一般的な計算機（ＰＣ、サーバ等）で構成可能であり、例えばソフトウェアプログラム処理によって特徴的な処理機能（処理装置１１０の各処理部）を実現する。

【0014】

軌道計画装置１００は、処理装置１１０、記憶装置１２０および入出力Ｉ／Ｆ（インターフェース）装置１３０などを有する。

【0015】

入出力装置１４０は、ユーザの操作によって、計測データなどの入力を行う入力装置や基準形状引当結果などの出力を行う出力装置であり、例えばキーボード、マウス、ディスプレイ、プリンタ、スマートフォン、タブレット型ＰＣなどであってもよい。

【0016】

入出力Ｉ／Ｆ装置１３０は、入出力装置１４０とデータ交換などのインターフェース制御（周辺デバイス制御）の処理を行う部分である。本システムでは、処理装置１１０の処理および入出力Ｉ／Ｆ装置１３０の処理に基づいて、入出力装置１４０の画面で、グラフィカルユーザインタフェース（ＧＵＩ）を構成し、各種の情報を表示する。

【0017】

統合制御装置１５０は、ロボット１およびロボット２を、軌道計画装置１００が出力する軌道に従って制御する装置である。

【0018】

単軸ロボット１６０は、６軸ロボット１７０を動かすロボットである。本実施例では単軸のロボットとして説明するが、例えば２軸以上の併進自由度を持つロボットなどでも代替可能である。

【0019】

６軸ロボット１７０は、単軸ロボット１６０と直列に連結した（すなわち、単軸ロボット１６０の動作によって移動可能となるように当該単軸ロボット１６０に連結した）６軸のロボットである。本実施例では垂直多関節型の６軸ロボットとして説明するが、ほかにも協働ロボットにあるような垂直多関節型でない機構の６軸ロボットでも代替可能である。

【0020】

処理装置１１０は、例えばＣＰＵ（Central Processing Unit）、ＲＡＭ（Random Access Memory）およびＲＯＭ（Read Only Memory）等の公知の要素によって構成される。処理装置１１０は、本実施例の特徴的な機能を実現する処理を行う部分であり、統合軌道計画部２０１、ロボット２特異点曲面算出部２０２、経由点部分集合分割部２０３、ロボット１目標位置算出部２０４、ロボット１軌道計画部２０５およびロボット２軌道計画部２０６を有する。これらの機能は、処理装置１１０を構成するＣＰＵがＲＡＭ等に格納されたプログラムを実行することによって実現される。

【0021】

なお、軌道計画装置１００は、図示しないが、ＯＳ（Operating System）、ミドルウェア、アプリケーションなどの公知の要素を有し、特にディスプレイなどの入出力装置１４０にＧＵＩ画面を表示するための既存の処理機能を備える。処理装置１１０は、上記の既存の処理機能を用いて、所定の画面を描画し表示する処理や、画面でユーザ入力されるデータ情報の処理などを行う。

【0022】

記憶装置１２０は、例えばＨＤＤ（Hard Disk Drive）またはＳＳＤ（Solid State Drive）等の公知の要素によって構成され、ロボットアーム構成記憶部３０１、干渉物構成記憶部３０２、経由点記憶部３０３、軌道記憶部３０４を含む、記憶部ないし対応するデータ情報（例えばデータベースやテーブル）を有する。

【0023】

ロボットアーム構成記憶部３０１は、軌道計画、逆運学計算および動力学計算などで用いるロボットアーム構成データ４１０を記憶する部分である。

【0024】

干渉物構成記憶部３０２は、軌道計画において行う干渉判定に用いる、干渉物構成データ４０２を記憶する部分である。

【0025】

経由点記憶部３０３は、軌道計画において手先が通過する経由点である経由点データ４０３を記憶する部分である。

【0026】

軌道記憶部３０４は、統合軌道計画部２０１が出力する軌道データ４０４を記憶する部分である。

【0027】

［フローチャート］
図２は、本発明の実施例における軌道計画装置１００の統合軌道計画部２０１が行う軌道計画処理の一例を示すフローチャートである。

【0028】

ステップＳ１０１において、ロボット２特異点曲面算出部２０２は、ロボットアーム構成記憶部３０１に格納されているロボットアーム構成データ４０１と、経由点記憶部３０３に格納されている経由点データ４０３とに基づいて、各経由点における手先姿勢から、ロボット２の特異点曲面を計算する。ここで、特異点曲面とは、ロボット２の特異点の集合である。この計算方法に関して、ロボット２として垂直多関節ロボットを用いた場合を例として、図７、図８を参照して説明する。

【0029】

図７および図８は、本発明の実施例におけるロボットの特異点曲面の一例を示す説明図である。

【0030】

具体的には、図７および図８には、垂直多関節ロボットＦ１０１、垂直多関節ロボットＦ１０１の可動範囲Ｆ１０２、及び、垂直多関節ロボットＦ１０１の特異点曲面Ｆ１０３をそれぞれ示す。垂直多関節ロボットＦ１０１の可動範囲Ｆ１０２は、Ｐ点が取りうる位置の集合であり、Ｊ２軸の位置を中心とした半径（ｌ_１＋ｌ_２）の球面内として定義される。特異点曲面Ｆ１０３は、手先のピッチ角度θから計算される曲面であり、以下の円筒座標系（ρ，φ，ｚ）上の数式で定義される。

【0031】

【数1】

【0032】

例えば、手先が真下、つまりピッチ角度θ＝πの時の特異点曲面Ｆ１０３は、図７に示すように垂直多関節ロボットの軸中心上に中心がある円を、Ｊ１軸を中心に回転させた球面となる。一方、図８に示すように手先を４５度傾けた姿勢、つまりピッチ角度θ＝π＋π／４の場合の特異点曲面Ｆ１０３は、中心位置がずれた円を、Ｊ１軸を中心に回転させた曲面になる。つまり手先姿勢とロボットの機構を元に、特異点曲面を計算することができる。本実施例では垂直多関節ロボットの機構を元に特異点曲面の計算方法を示したが、６自由度を持つロボットであれば、同様の考え方に基づいて、手先姿勢から特異点曲面は計算可能である。

【0033】

ステップＳ１０２において、経由点部分集合分割部２０３は、経由点記憶部３０３にある経由点データ４０３に基づいて、経由点を指定された分割数Ｎに分割する。これを図９、図１０を用いて説明する。

【0034】

図９は、本発明の実施例におけるロボットの経由点の一例を示す説明図である。

【0035】

図１０は、本発明の実施例におけるロボットの経由点の分割方法の一例を示す説明図である。

【0036】

具体的には、図９は、単軸ロボット１の上に垂直多関節ロボット２が乗ったロボットを用いて、経由点Ｐ１からＰ５を通過させるときの経由点の一例を示す。このような経由点Ｐ１からＰ５の順列を区切ることで、それぞれが１以上の経由点を含む複数の経由点の部分集合に分割する処理が、分割処理である。分割によって生成される経由点の部分集合の数（すなわち分割数）をＮとすると、経由点部分集合分割部２０３は、区切り位置を（Ｎ－１）個挿入する。例えば分割数Ｎが２の場合は、１つの区切り位置が挿入される。この区切り位置は、分割したときに部分集合の重心Ｖと、経由点との位置差分が最小となる個所に挿入される。つまり、経由点部分集合分割部２０３は、以下の数式が最小となるように区切り位置を挿入する。

【0037】

【数2】

【0038】

なお、ｎは各経由点の個数を示し、Ｖ_ｋは、各部分集合ｋの重心位置（すなわち、各部分集合ｋに属する１以上の経由点の位置の重心）を示す。例えば、図９の事例で２分割にする場合は、Ｐ３とＰ４の間に分割点が設定される。

【0039】

ステップＳ１０３において、統合軌道計画部２０１は、Ｓ１０４からＳ１０９の処理を、Ｓ１０２で計算した部分集合ごとに繰り返し実行する。

【0040】

ステップＳ１０４において、ロボット１目標位置算出部２０４は、ステップＳ１０１で計算した経由点における姿勢から計算した特異点曲面と可動範囲とを基に、ロボットの２の可動範囲内かつ特異点曲面外の範囲の積集合を計算する。この計算について、図１１および図１２を参照して説明する。

【0041】

図１１は、本発明の実施例におけるロボットの各経由点における特異点曲面の一例を示す説明図である。

【0042】

図１２は、本発明の実施例におけるロボットの特異点曲面の積集合の一例を示す説明図である。

【0043】

ここでは、例として、図１０に示すように経由点Ｐ１、Ｐ２およびＰ３を含む第１の部分集合および経由点Ｐ４およびＰ５を含む第２の部分集合が得られた場合の、第１の部分集合に関する特異点曲面の積集合を説明する。例えば、図１１のように経由点Ｐ１、Ｐ２、Ｐ３での手先姿勢から可動範囲Ｆ１０２および特異点曲面Ｆ１０３が得られた場合、図１２のように特異点曲面と可動範囲の間の範囲の積集合を計算する。具体的には、図１１においてハッチングによって示される範囲が、経由点Ｐ１、Ｐ２、Ｐ３における可動範囲Ｆ１０２の内側かつ特異点曲面Ｆ１０３の外側の範囲である。これらの範囲の積集合が、図１２においてハッチングで示されている。

【0044】

ステップＳ１０５において、ロボット１目標位置算出部２０４は、ステップＳ１０４で計算した特異点曲面の積集合を基に、例えば図１３に示す通り、積集合内にすべての経由点が含まれるようにロボット１の目標位置を計算する。この計算方法としてはいろいろな手段が考えられるが、例えばロボット１を可動範囲内で様々に動かし、経由点すべてが積集合内に含まれているか判定することで計算可能である。

【0045】

図１３は、本発明の実施例におけるロボット１の軌道の一例を示す説明図である。

【0046】

図１３の例では、ロボット２の手先の現在位置、経由点Ｐ１、Ｐ２およびＰ３のいずれもがステップＳ１０４において第１の部分集合に関して計算された積集合に含まれるように、ロボット１の目標位置が計算される。

【0047】

ステップＳ１０６において、ロボット１軌道計画部２０５は、干渉物構成記憶部３０２に含まれる干渉物構成データ４０２と干渉しないよう、ロボット１、つまり単軸ロボット１６０を動かす軌道を計画し、計画した軌道を軌道記憶部３０４に記憶する。この軌道の計算方法は様々な計算が考えられるが、例えば、ＲＲＴ（Rapidly-Exploring Random Trees）法、または、単純に現在位置と目標位置までを等間隔に区切り干渉がないか判定する単純な手法など、任意の方法（公知の方法であってもよい）を採用することができる。例えば、ＲＲＴ法については、Steven M. LaValle, “Rapidly-Exploring Random Trees: A New Tool for Path Planning”, http://msl.cs.illinois.edu/~lavalle/papers/Lav98c.pdf に記載されている。

【0048】

ステップＳ１０７において、ロボット２軌道計画部２０６は、干渉物構成記憶部３０２に含まれる干渉物構成データ４０２と干渉しないよう、ロボット２、つまり６軸ロボット１７０を動かす軌道を計画し、計画した軌道を軌道記憶部３０４に記憶する。この軌道の計算方法は様々な計算が考えられるが、例えば前述のＲＲＴ法、または、単純に現在位置と目標位置までを等間隔に区切り干渉がないか判定する単純な手法など、公知の方法を含む任意の方法のいずれかを採用することができる。これによって、図１４に示す通り、現在位置からＰ１、Ｐ２、Ｐ３を通過する軌道を計画することが可能となる。

【0049】

図１４は、本発明の実施例におけるロボット２の軌道の一例を示す説明図である。

【0050】

図１４には、ステップＳ１０５において計算されたロボット１の目標位置において、ロボット２の手先の現在位置、経由点Ｐ１、Ｐ２およびＰ３のいずれもがステップＳ１０４において第１の部分集合に関して計算された積集合に含まれるように計画された軌道の例を示している。

【0051】

ステップＳ１０８において、統合軌道計画部２０１は、ステップＳ１０７で軌道が計画できたかを判定し、計画の成否に応じて処理を分岐する。例えば、干渉しない軌道が見つからなかった場合、および、軌道の途中で特異点を通過してしまい手先姿勢の変化に対して関節角度が大きく動いてしまった場合などが失敗理由に当たり、それらの失敗理由に該当しない場合には軌道が計画できた、該当する場合には軌道計画に失敗したと判定される。

【0052】

ステップＳ１０８においてロボット２の軌道計画に失敗したと判定された場合（ステップＳ１０８：Ｆ）、ステップＳ１０９において、ロボット１目標位置算出部２０４は、ロボット１の目標位置を指定回数以上変更したかを判定する。指定回数以上すでに目標位置を変更していた場合（ステップＳ１０９：Ｔ）、ステップＳ１０３のループ処理から抜けて、ステップＳ１１１に進む。

【0053】

ステップＳ１０９においてロボット１の目標位置を指定回数以上変更していないと判定された場合（ステップＳ１０９：Ｆ）、ステップＳ１１０において、ロボット１目標位置算出部２０４は、ロボット１の目標位置を変更する。この目標位置の変更に関しても、ステップＳ１０５に記載した方法と同様に、ロボット１を可動範囲内で様々に動かし、経由点すべてが積集合内に含まれているか判定することで計算可能である。

【0054】

ステップＳ１１１において、統合軌道計画部２０１は、経由点の分割数Ｎがすでに経由点の個数と同じになっているかどうか判定する。分割数Ｎが経由点の個数と同じである場合（ステップＳ１１１：Ｔ）、統合軌道計画部２０１は、これ以上分割できないため、ステップＳ１１４に移り、計画失敗と判断する。

【0055】

ステップＳ１１２において、統合軌道計画部２０１は、経由点の分割数ＮをＮ＋１に増加させ、ステップＳ１０２からの処理を再度行う。

【0056】

ステップＳ１０８においてロボット２の軌道計画に成功したと判定された場合（ステップＳ１０８：Ｔ）、ステップＳ１１３において、統合軌道計画部２０１は、計画した軌道に従ってロボット１およびロボット２を制御する。

【0057】

ステップＳ１１４において、統合軌道計画部２０１は、軌道計画に失敗したと判断し、ユーザに軌道計画に失敗した旨を通知し、処理を終了する。

【0058】

なお、経由点の分割数Ｎの初期値は１であってもよい。その場合、ステップＳ１０２が最初に実行されるときには、経由点部分集合分割部２０３は経由点集合の分割を行わず、ステップＳ１１２を経て２回目のステップＳ１０２が実行されるときに、分割数Ｎが２となり、その後、処理が繰り返されるたびに分割数Ｎが増加する。

【0059】

また、分割数Ｎが増加するときに、経由点部分集合分割部２０３は、既に生成された部分集合をさらに分割することで分割数を増やしてもよいし、それ以外の方法で分割数を増やしてもよい。

【0060】

［ロボットアーム構成データ］
図３は、本発明の実施例におけるロボットアーム構成記憶部３０１に格納されるロボットアーム構成データ４０１のテーブル構成の一例を示す説明図である。

【0061】

ロボットアーム構成データ４０１のテーブルは、関節情報およびリンク情報の分類から構成される。

【0062】

関節情報とは、ロボットアームを構成する各関節の情報であり、例えば、関節名、関節の種類、関節の位置、関節の向き、関節の動作下限、関節の動作上限、最大加速度および最大速度などの情報である。

【0063】

リンク情報とは、ロボットアームを構成するリンクの構成を表す情報であり、例えば、リンク名、親関節名、子関節名およびリンク形状からなる。リンク形状とは、リンクの実際の形状であり、例えば、ＳＴＥＰ（Standard for the Exchange of Product model data）などのフォーマットで保存されるソリッドデータ、または、ＳＴＬ（STereoLithography）などのフォーマットで保存されるポリゴンデータなどである。

【0064】

［干渉物構成データ］
図４は、本発明の実施例における干渉物構成記憶部３０２に格納される干渉物構成データ４０２のテーブル構成の一例を示す説明図である。

【0065】

干渉物構成データ４０２のテーブルは、干渉物ＩＤ４２１、干渉物形状４２２および干渉物位置姿勢４２３を有する。干渉物形状４２２は、干渉物の形状を示すものであり、例えばＳＴＥＰなどのフォーマットで保存されるソリッドデータ、または、ＳＴＬなどのフォーマットで保存されるポリゴンデータなどである。干渉物位置姿勢４２３は、干渉物が空間上のどの位置にどのような姿勢で置かれるかを示す情報であり、例えばＡＦＦＩＮＥ変換行列、または、３次元空間上での位置とそのときのＲｏｌｌ―Ｐｉｔｃｈ－Ｙａｗなどであらわされる姿勢を示す情報である。

【0066】

［経由点データ］
図５は、本発明の実施例における経由点記憶部３０３に格納される経由点データ４０３のテーブル構成の一例を示す説明図である。

【0067】

経由点データ４０３のテーブルは、姿勢ＩＤ４３１、対象リンク名４３２および位置姿勢情報４３３からなる。対象リンク名４３２は、ロボットアーム構成データ４０１に含まれるリンク情報内のリンク名と対応するものである。また、位置姿勢情報４３３は、各経由点におけるロボット２の手先の位置および姿勢を示す情報であり、例えばＡＦＦＩＮＥ変換行列、または、３次元空間上での位置とそのときのＲｏｌｌ―Ｐｉｔｃｈ－Ｙａｗなどであらわされる姿勢を示す情報である。

【0068】

［軌道データ］
図６は、本発明の実施例における軌道記憶部３０４に格納される軌道データ４０４のテーブル構成の一例を示す説明図である。

【0069】

軌道データ４０４は、本実施例の軌道計画装置１００の出力に相当する。軌道データ４０４のテーブルは、軌道点ＩＤ４４１、コントローラ４４２、関節角度情報４４３および制御時刻４４４からなる。軌道点ＩＤ４４１は、各ロボットの軌道上の点（すなわち軌道点）を識別する。例えば本実施例の経由点Ｐ１からＰ５は、軌道点に含まれる。コントローラ４４２は、制御対象のロボット（本実施例ではロボット１またはロボット２）を示す。関節角度情報４４３は、制御対象のロボットがあるタイミングでとる関節角度の集合であり、関節名とその関節値の集合である。制御時刻４４４は、各軌道点への到達時刻を示す。例えば、ある軌道点（例えばＴ１０２）の制御時刻４４４は、そのひとつ前の軌道点（例えばＴ１０１）への到達時刻（例えば１．２）に、当該一つ前の軌道点（例えばＴ１０１）から当該軌道点（例えばＴ１０２）まで移動するための時間（例えば０．０１）を加算した時刻（例えば１．２１）である。

【0070】

［出力画面］
図１５は、本発明の実施例における軌道計画装置１００が出力する出力画面の一例を示す説明図である。

【0071】

ユーザがボタンＯ１０１を操作することで、入出力インターフェース装置１３０を介して入力された入力データが読み込まれる。ユーザがボタンＯ１０２を操作することで、軌道計画装置１００は軌道計画計算を実行し、シミュレート画面、表Ｏ１０３およびロボット１位置グラフＯ１０４などを作成する。表Ｏ１０３には、例えば経由点の分割数および動作時間などが表示され、表Ｏ１０３内の行を選択することでロボット１位置を表したグラフＯ１０４も変化する。また、ユーザが表Ｏ１０３内の動作再生ボタンを押すことで、作成された動作計画に従うロボット１およびロボット２の動作の表示（例えばロボットの動作のアニメーション表示又は各経由点のロボットの位置の順次表示等）がシミュレート画面上で行われる。これによって、シミュレータ上で動作計画に基づく動作を確認することができる。

【0072】

［効果等］
以上に説明したように、本実施例の軌道計画装置１００によれば、ロボットを複数連結した機構に対してもリアルタイムでの軌道計画を実現することが可能となる。

【0073】

また、本発明の実施形態のシステムは次のように構成されてもよい。

【0074】

（１）第１のロボット（例えばロボット１６０）と、第１のロボットの動作によって移動可能となるように第１のロボットに連結した第２のロボット（例えばロボット１７０）と、を制御するための軌道を計画する軌道計画装置（例えば軌道計画装置１００）であって、処理装置（例えば処理装置１１０）と、記憶装置（例えば記憶装置１２０）と、を有し、記憶装置は、第２のロボットのアームの構成を示す情報（例えばロボットアーム構成記憶部３０１内の情報）と、第２のロボットの手先が順次経由する複数の経由点における手先の位置及び姿勢を示す情報（例えば経由点記憶部３０３内の情報）と、を保持し、処理装置は、第２のロボットのアームの構成と、各経由点における手先の位置及び姿勢と、に基づいて、各経由点における第２のロボットの特異点の集合である特異点曲面を計算し（例えばステップＳ１０４）、各経由点について計算された第２のロボットの特異点曲面と、第２のロボットの可動範囲と、に基づいて、第１のロボットの目標位置を決定し（例えばステップＳ１０５）、第１のロボットの目標位置までの軌道を計画し（例えばステップＳ１０６）、第１のロボットの目標位置における第２のロボットの手先が複数の経由点を経由する軌道を計画する（例えばステップＳ１０７）。

【0075】

（２）上記（１）において、処理装置は、複数の経由点を、各々が１以上の経由点を含む複数の部分集合に分割し、部分集合ごとに、各経由点について計算された第２のロボットの特異点曲面と、第２のロボットの可動範囲と、に基づいて、部分集合に含まれる１以上の経由点の全てが、第２のロボットの可動範囲内かつ各経由点の特異点曲面外の範囲の積集合に含まれるように、第１のロボットの目標位置を決定し、部分集合ごとに決定した第１のロボットの目標位置ごとに、第１のロボットの軌道を計画し、部分集合ごとに、第１のロボットの目標位置における第２のロボットの手先が部分集合に含まれる１以上の経由点を経由する軌道を計画する。

【0076】

（３）上記（２）において、処理装置は、第２のロボットの手先が複数の経由点を経由する軌道の計画に失敗した場合、第１のロボットの目標位置を変更し（例えばステップＳ１１０）、変更された第１のロボットの目標位置における第２のロボットの手先が複数の経由点を経由する軌道を計画する。

【0077】

（４）上記（３）において、処理装置は、第２のロボットの可動範囲内で、第２のロボットの特異点曲面を通過せず、かつ、他の物体と干渉しない軌道を計画できなかった場合に、第２のロボットの手先が複数の経由点を経由する軌道の計画に失敗したと判定する（例えばステップＳ１０８）。

【0078】

（５）上記（３）において、処理装置は、変更された第１のロボットの目標位置における第２のロボットの手先が複数の経由点を経由する軌道の計画に失敗した場合に、複数の経由点を、各々が１以上の前記経由点を含む複数の部分集合に分割する（例えばステップＳ１１２）。

【0079】

（６）上記（５）において、処理装置は、いずれかの部分集合において、第１のロボットの目標位置における第２のロボットの手先が部分集合に含まれる１以上の経由点を経由する軌道の計画に失敗した場合に、部分集合の分割数を増加させる（例えばステップＳ１１２）。

【0080】

（７）上記（１）において、第２のロボットは、６軸の自由度を持つロボットである。

【0081】

（８）上記（１）において、第２のロボットは、６軸の垂直多関節ロボットである。

【0082】

以上の構成によって、第２のロボットの可動範囲と特異点曲面を基に、第１のロボットの目標位置を算出することで、ロボットを複数連結した機構に対してもリアルタイムでの軌道計画を実現することができる。

【0083】

なお、本発明は上記した実施例に限定されるものではなく、様々な変形例が含まれる。例えば、上記した実施例は本発明のより良い理解のために詳細に説明したのであり、必ずしも説明の全ての構成を備えるものに限定されるものではない。また、ある実施例の構成の一部を他の実施例の構成に置き換えることが可能であり、また、ある実施例の構成に他の実施例の構成を加えることが可能である。また、各実施例の構成の一部について、他の構成の追加・削除・置換をすることが可能である。

【0084】

また、上記の各構成、機能、処理部、処理手段等は、それらの一部又は全部を、例えば集積回路で設計する等によってハードウェアで実現してもよい。また、上記の各構成、機能等は、プロセッサがそれぞれの機能を実現するプログラムを解釈し、実行することによってソフトウェアで実現してもよい。各機能を実現するプログラム、テーブル、ファイル等の情報は、不揮発性半導体メモリ、ハードディスクドライブ、ＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）等の記憶デバイス、または、ＩＣカード、ＳＤカード、ＤＶＤ等の計算機読み取り可能な非一時的データ記憶媒体に格納することができる。

【0085】

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

【符号の説明】

【0086】

１００・・・軌道計画装置
１１０・・・処理装置
１２０・・・記憶装置
１３０・・・入出力インターフェース装置
１４０・・・入出力装置
１５０・・・統合制御装置
１６０・・・ロボット１
１７０・・・ロボット２
２０１・・・統合軌道計画部
２０２・・・ロボット２特異点曲面算出部
２０３・・・経由点部分集合分割部
２０４・・・ロボット１目標位置算出部
２０５・・・ロボット１軌道計画部
２０６・・・ロボット２軌道計画部
３０１・・・ロボットアーム構成記憶部
３０２・・・干渉物構成記憶部
３０３・・・経由点記憶部
３０４・・・軌道記憶部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】