特許 7405730 軌道計画装置、軌道計画方法、及び軌道計画プログラム

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

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2023-12-18

(45)【発行日】2023-12-26

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

(51)【国際特許分類】

   B25J 13/00 20060101AFI20231219BHJP

【ＦＩ】

B25J13/00 Z

【請求項の数】 13

(21)【出願番号】P 2020195708

(22)【出願日】2020-11-26

(65)【公開番号】P2022084092

(43)【公開日】2022-06-07

【審査請求日】2023-05-19

(73)【特許権者】

【識別番号】000005108

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

(74)【代理人】

【識別番号】110001678

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

(72)【発明者】

【氏名】山内 雄太

(72)【発明者】

【氏名】中須 信昭

【審査官】岩▲崎▼ 優

(56)【参考文献】

【文献】米国特許出願公開第２００４／０２６５１１０（ＵＳ，Ａ１）

【文献】特開２０２０－６９６４０（ＪＰ，Ａ）

【文献】特開２０２０－４０１３２（ＪＰ，Ａ）

【文献】特開平９－３００２５５（ＪＰ，Ａ）

【文献】特開２０１９－１４１９３９（ＪＰ，Ａ）

【文献】特開２０２１－９１０１３（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｂ２５Ｊ １／００－２１／０２

Ｇ０５Ｂ １９／００－１９／４６

(57)【特許請求の範囲】

【請求項1】

可動軸を有し、把持物を把持するロボットアーム、の前記可動軸が移動する軌道を計画する軌道計画装置であって、
プロセッサとメモリとを有し、
前記メモリは、
前記把持物の重量及び重心位置を示す把持物情報と、
前記把持物にかかる把持力及びモーメントの制約を示す制約情報と、
前記軌道における経路の各点の位置を示す軌道情報と、を保持し、
前記プロセッサは、
前記把持物情報が示す前記重量及び前記重心位置に基づいて、前記制約情報が示す前記把持力及び前記モーメントの制約を満たすように、前記軌道情報が示す経路の各点を前記可動軸が通過する時刻を付与して、前記付与した時刻を前記軌道情報に格納する時刻付与処理と、
前記把持物情報が示す前記重量及び前記重心位置に基づいて、前記把持物にかかる把持力及び／又はモーメントを低下させるよう、前記軌道情報における経路に含まれる点の位置を変更する経路最適化処理と、を実行する軌道計画装置。

【請求項2】

請求項１に記載の軌道計画装置であって、
前記プロセッサは、
前記時刻付与処理と、前記経路最適化処理と、を交互に繰り返して実行し、
前記時刻付与処理の直後に、前記軌道の経路又は当該経路の各点に付与された時刻と、前回の時刻付与処理の直後の前記軌道の経路又は当該経路の各点に付与された時刻と、の変化量に基づいて、前記時刻付与処理と、前記経路最適化処理と、を終了する、軌道計画装置。

【請求項3】

請求項１に記載の軌道計画装置であって、
前記メモリは、前記可動軸の角速度及び角加速度の制約を示すロボットアーム情報を保持し、
前記プロセッサは、前記時刻付与処理において、前記ロボットアーム情報が示す前記角速度及び前記角加速度の制約を満たすように、前記軌道情報が示す経路の各点を前記可動軸が通過する時刻を付与する、軌道計画装置。

【請求項4】

請求項１に記載の軌道計画装置であって、
前記プロセッサは、前記経路最適化処理において、前記軌道情報が示す経路が滑らかになるよう前記軌道情報における経路に含まれる点の位置を変更する、軌道計画装置。

【請求項5】

請求項１に記載の軌道計画装置であって、
前記メモリは、干渉物の位置を示す干渉物情報を保持し、
前記プロセッサは、前記経路最適化処理において、前記干渉物情報が示す前記干渉物の位置に基づいて、前記軌道情報における経路に含まれる点の位置を変更する、軌道計画装置。

【請求項6】

請求項１に記載の軌道計画装置であって、
表示装置に接続され、
前記プロセッサは、前記時刻付与処理と前記経路最適化処理とが実行された後の前記軌道情報における軌道を前記可動軸が移動する場合において前記把持物にかかる把持力及び／又はモーメントの時間変化を示す情報を、前記表示装置に表示する、軌道計画装置。

【請求項7】

可動軸を有し、把持物を把持するロボットアーム、の前記可動軸が移動する軌道を計画する軌道計画装置による軌道計画方法であって、
前記軌道計画装置は、プロセッサとメモリとを有し、
前記メモリは、
前記把持物の重量及び重心位置を示す把持物情報と、
前記把持物にかかる把持力及びモーメントの制約を示す制約情報と、
前記軌道における経路の各点の位置を示す軌道情報と、を保持し、
前記軌道計画方法は、
前記プロセッサが、
前記把持物情報が示す前記重量及び前記重心位置に基づいて、前記制約情報が示す前記把持力及び前記モーメントの制約を満たすように、前記軌道情報が示す経路の各点を前記可動軸が通過する時刻を付与して、前記付与した時刻を前記軌道情報に格納する時刻付与処理と、
前記把持物情報が示す前記重量及び前記重心位置に基づいて、前記把持物にかかる把持力及び／又はモーメントを低下させるよう、前記軌道情報における経路に含まれる点の位置を変更する経路最適化処理と、を実行する軌道計画方法。

【請求項8】

請求項７に記載の軌道計画方法であって、
前記プロセッサが、前記時刻付与処理と、前記経路最適化処理と、を交互に繰り返して実行し、
前記プロセッサが、前記時刻付与処理の直後に、前記軌道の経路又は当該経路の各点に付与された時刻と、前回の時刻付与処理の直後の前記軌道の経路又は当該経路の各点に付与された時刻と、の変化量に基づいて、前記時刻付与処理と、前記経路最適化処理と、を終了する、軌道計画方法。

【請求項9】

請求項７に記載の軌道計画方法であって、
前記メモリは、前記可動軸の角速度及び角加速度の制約を示すロボットアーム情報を保持し、
前記軌道計画方法は、前記プロセッサが、前記時刻付与処理において、前記ロボットアーム情報が示す前記角速度及び前記角加速度の制約を満たすように、前記軌道情報が示す経路の各点を前記可動軸が通過する時刻を付与する、軌道計画方法。

【請求項10】

請求項７に記載の軌道計画方法であって、
前記プロセッサが、前記経路最適化処理において、前記軌道情報が示す経路が滑らかになるよう前記軌道情報における経路に含まれる点の位置を変更する、軌道計画方法。

【請求項11】

請求項７に記載の軌道計画方法であって、
前記メモリは、干渉物の位置を示す干渉物情報を保持し、
前記軌道計画方法は、前記プロセッサが、前記経路最適化処理において、前記干渉物情報が示す前記干渉物の位置に基づいて、前記軌道情報における経路に含まれる点の位置を変更する、軌道計画方法。

【請求項12】

請求項７に記載の軌道計画方法であって、
前記軌道計画装置は、表示装置に接続され、
前記軌道計画方法は、前記プロセッサが、前記時刻付与処理と前記経路最適化処理とが実行された後の前記軌道情報における軌道を前記可動軸が移動する場合において前記把持物にかかる把持力及び／又はモーメントの時間変化を示す情報を、前記表示装置に表示する、軌道計画方法。

【請求項13】

可動軸を有し、把持物を把持するロボットアーム、の前記可動軸が移動する軌道の計画を軌道計画装置に実行させる軌道計画プログラムであって、
前記軌道計画装置は、プロセッサとメモリとを有し、
前記メモリは、
前記把持物の重量及び重心位置を示す把持物情報と、
前記把持物にかかる把持力及びモーメントの制約を示す制約情報と、
前記軌道における経路の各点の位置を示す軌道情報と、を保持し、
前記軌道計画プログラムは、
前記把持物情報が示す前記重量及び前記重心位置に基づいて、前記制約情報が示す前記把持力及び前記モーメントの制約を満たすように、前記軌道情報が示す経路の各点を前記可動軸が通過する時刻を付与して、前記付与した時刻を前記軌道情報に格納する時刻付与処理と、
前記把持物情報が示す前記重量及び前記重心位置に基づいて、前記把持物にかかる把持力及び／又はモーメントを低下させるよう、前記軌道情報における経路に含まれる点の位置を変更する経路最適化処理と、を前記プロセッサに実行させる軌道計画プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

本技術分野の背景技術として、特開２００５－１０５５号公報（特許文献１）、及び特開２０１９－１４１９３９号公報（特許文献２）がある。

【0003】

特許文献１には、「ロボットアーム先端に装着されたハンドで容器を把持し、容器内に収容された液体、粉体等を搬送する。容器の姿勢は、搬送開始時には重力加速度ベクトルと同方向（真下）を向いているが、加速過程で加速度が増加するにつれて慣性加速度ベクトルが徐々に大きくなり、加速度の向きと逆方向に傾斜が増し、加速度が減少に転ずると慣性加速度ベクトルが徐々に小さくなり、その傾斜は減少する。加速過程が完了して速度一定となると傾斜は解消される。減速過程では、減速度が増すにつれて、加速過程とは逆方向に傾斜が増し、減速度が減少に転ずるとその傾斜は減少し、ロボットが停止すると傾斜は解消され、搬送開始時の真下方向を向く姿勢に戻る。」と記載されている。

【0004】

また、特許文献２には、「シミュレーション装置１０は、ワーク、ワークを保持した弾性を有する吸着パッド、及び吸着パッドが取り付けられたロボットアームを含むシミュレーションモデルを動作指令に基づいて加速度運動させるシミュレーションを実行した際、ロボットアームの加速度とワークの加速度との差により吸着パッドに発生する負荷モーメントを導出する負荷導出部１０５と、負荷導出部１０５で導出した負荷モーメントが閾値より大きい場合に、負荷導出部１０５で導出される負荷モーメントが閾値以下となるように、シミュレーションの実行条件を変更するための処理を行なう実行条件変更部１０８と、を含む。」と記載されている。

【先行技術文献】

【特許文献】

【0005】

【文献】特開２００５－１０５５号公報

【文献】特開２０１９－１４１９３９号公報

【発明の概要】

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

【0006】

特許文献１に記載の技術は、慣性加速度ベクトルに従って把持物の姿勢を変更するが、例えば、遠心力などの慣性力が把持物にかからないように、把持物が通過する位置を変更することはできない。特許文献２に記載の技術は、シミュレーション結果に従って、吸着パッドにっ掛かるモーメントを小さくするように、加速度を調整するが、モーメントを低下させるように把持物が通る軌道を変更することはできない。

【0007】

つまり、特許文献１に記載の技術及びと特許文献２に記載の技術は、把持物にかかる力やモーメントを考慮していないため、把持物に与えるダメージが大きくなるおそれがある。そこで本発明の一態様は、把持物に与えるダメージを抑制しつつロボットを高速に動作させる。

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

【0008】

上記課題を解決するために、本発明の一態様は以下の構成を採用する。可動軸を有し、把持物を把持するロボットアーム、の前記可動軸が移動する軌道を計画する軌道計画装置は、プロセッサとメモリとを有し、前記メモリは、前記把持物の重量及び重心位置を示す把持物情報と、前記把持物にかかる把持力及びモーメントの制約を示す制約情報と、前記軌道における経路の各点の位置を示す軌道情報と、を保持し、前記プロセッサは、前記把持物情報が示す前記重量及び前記重心位置に基づいて、前記制約情報が示す前記把持力及び前記モーメントの制約を満たすように、前記軌道情報が示す経路の各点を前記可動軸が通過する時刻を付与して、前記付与した時刻を前記軌道情報に格納する時刻付与処理と、前記把持物情報が示す前記重量及び前記重心位置に基づいて、前記把持物にかかる把持力及び／又はモーメントを低下させるよう、前記軌道情報における経路に含まれる点の位置を変更する経路最適化処理と、を実行する。

【発明の効果】

【0009】

本発明の一態様によれば、把持物に与えるダメージを抑制しつつロボットを高速に動作させることができる。

【0010】

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

【図面の簡単な説明】

【0011】

【図1】実施例１における軌道計画装置の構成例を示すブロック図である。

【図2】実施例１におけるロボットアーム構成テーブルの一例である。

【図3】実施例１における干渉物構成テーブルの一例である。

【図4】実施例１における開始関節角度テーブルの一例である。

【図5】実施例１における目標位置姿勢テーブルの一例である。

【図6】実施例１における把持力・モーメント制約テーブルの一例である。

【図7】実施例１における把持物情報テーブルの一例である。

【図8】実施例１における把持方法情報テーブルの一例である。

【図9】実施例１における軌道テーブルの一例である。

【図10】実施例１における軌道計画処理の一例を示すフローチャートである。

【図11】実施例１における制御時刻付与処理の一例を示すフローチャートである。

【図12】実施例１における把持物考慮経路最適化処理の一例を示すフローチャートである。

【図13】実施例１における結果出力画面の一例である。

【図14】実施例１における把持物にかかる把持力、及び把持物にかかるモーメントの計算処理の一例を示す説明図である。

【図15】実施例１における軌道計画処理における軌道の変化の一例を示す説明図である。

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

【0012】

以下、本発明の実施形態を図面に基づいて詳細に説明する。本実施形態において、同一の構成には原則として同一の符号を付け、繰り返しの説明は省略する。なお、本実施形態は本発明を実現するための一例に過ぎず、本発明の技術的範囲を限定するものではないことに注意すべきである。

【実施例1】

【0013】

［システム構成］
図１は、軌道計画装置の構成例を示すブロック図である。軌道計画装置１００は、例えば、ＣＰＵ（Ｃｏｎｔｒｏｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１１０、メモリ１２０、補助記憶装置１３０、通信装置１４０、及び入出力インターフェース１５０を有する計算機（例えば、ＰＣ（Ｐｅｒｓｏｎａｌ Ｃｏｍｐｕｔｅｒ）又はサーバ等）によって構成される。

【0014】

ＣＰＵ１１０は、プロセッサを含み、メモリ１２０に格納されたプログラムを実行する。メモリ１２０は、不揮発性の記憶素子であるＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）及び揮発性の記憶素子であるＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）を含む。ＲＯＭは、不変のプログラム（例えば、ＢＩＯＳ（Ｂａｓｉｃ Ｉｎｐｕｔ／Ｏｕｔｐｕｔ Ｓｙｓｔｅｍ））などを格納する。ＲＡＭは、ＤＲＡＭ（Ｄｙｎａｍｉｃ Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）のような高速かつ揮発性の記憶素子であり、ＣＰＵ１１０が実行するプログラム及びプログラムの実行時に使用されるデータを一時的に格納する。

【0015】

補助記憶装置１３０は、例えば、磁気記憶装置（ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ））、フラッシュメモリ（ＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ））等の大容量かつ不揮発性の記憶装置であり、ＣＰＵ１１０が実行するプログラム及びプログラムの実行時に使用されるデータを格納する。すなわち、プログラムは、補助記憶装置１３０から読み出されて、メモリ１２０にロードされて、ＣＰＵ１１０によって実行される。

【0016】

入出力インターフェース１５０は、オペレータからの入力を受けるインターフェースであり、プログラムの実行結果をオペレータが視認可能な形式で出力するインターフェースでもある。入出力インターフェース１５０には、キーボードやマウスなどの入力装置、及びディスプレイ装置やプリンタなどの出力装置を含む入出力装置１６０が接続される。また、入出力インターフェース１５０には、把持物情報や干渉物情報を計測する３次元センサ１７０が接続される。３次元センサ１７０は、例えば、画像センサ、レーザースキャナ、及びライダーを含む。なお、スマートフォンやタブレットＰＣ等の計算機が入出力装置１６０として機能してもよい。

【0017】

通信装置１４０は、所定のプロトコルに従って、他の装置との通信を制御するネットワークインターフェース装置である。また、通信装置１４０は、例えば、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）等のシリアルインターフェースを含んでもよい。

【0018】

ＣＰＵ１１０が実行するプログラムは、リムーバブルメディア（ＣＤ－ＲＯＭ、フラッシュメモリなど）又はネットワークを介して軌道計画装置１００に提供され、非一時的記憶媒体である不揮発性の補助記憶装置１３０に格納される。このため、軌道計画装置１００は、リムーバブルメディアからデータを読み込むインターフェースを有するとよい。

【0019】

ＣＰＵ１１０は、例えば、データ読込部２０１、干渉判定部２０２、動力学計算部２０３、軌道計画部２０４、初期経路計画部２０５、把持物考慮時刻付与部２０６、把持物考慮経路最適化部２０７、及び結果出力部２０８を含む。

【0020】

例えば、ＣＰＵ１１０は、メモリ１２０にロードされたデータ読込プログラムに従って動作することで、データ読込部２０１として機能し、メモリ１２０にロードされた干渉判定部プログラムに従って動作することで、干渉判定部２０２として機能する。ＣＰＵ１１０に含まれる他の機能部についても、プログラムと機能部の関係は同様である。

【0021】

データ読込部２０１は、例えばユーザからの入力指示に従って、補助記憶装置１３０に格納された各種データを読み込む。干渉判定部２０２は、ロボットアーム及び把持物が干渉物と干渉するかを判定する。動力学計算部２０３は、把持物にかかる把持力、及び把持物にかかるモーメントを算出する。

【0022】

軌道計画部２０４は、ロボットアームの軌道を計画する。初期経路計画部２０５は、ロボットアームの初期経路を生成する。以下、ロボットアームが通過する経路に時刻を付与したものを軌道と呼ぶ（即ち経路は時刻を問わないロボットアームの道筋を示す概念である）。把持物考慮時刻付与部２０６は、軌道の各点における関節角速度、関節角速度、及び制御時刻を付与又は修正する。把持物考慮経路最適化部２０７は、把持物にかかる把持力、及びモーメントが小さくなるよう軌道の経路を最適化する。結果出力部２０８は、軌道計画の結果を入出力装置１６０に出力する。

【0023】

軌道計画装置１００は、物理的に一つの計算機上で、又は、論理的又は物理的に構成された複数の計算機上で構成される計算機システムであり、同一の計算機上で別個のスレッドで動作してもよく、複数の物理的計算機資源上に構築された仮想計算機上で動作してもよい。

【0024】

なお、図示しないが、軌道計画装置１００は、ＯＳ（Ｏｐｅｒａｔｉｎｇ Ｓｙｔｅｍ）、ミドルウェア、及びアプリケーションなどの構成を有し、特にディスプレイなどの入出力装置１６０にＧＵＩ（Ｇｒａｐｈｉｃａｌ Ｕｓｅｒ Ｉｎｔｅｒｆａｃｅ）を表示するための処理機能を有する。ＣＰＵ１１０は、当該処理機能により、所定の画面を描画し表示する処理や、ユーザが入力したデータの処理などを行うことができる。

【0025】

なお、ＣＰＵ１１０に含まれる機能部による機能の一部又は全部が、例えば、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）やＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）等のハードウェアによって実現されてもよい。

【0026】

補助記憶装置１３０は、例えば、ロボットアーム構成テーブル３１０、干渉物構成テーブル３２０、開始関節角度テーブル３３０、目標位置姿勢テーブル３４０、把持物・モーメント制約テーブル３５０、把持物情報テーブル３６０、把持方法情報テーブル３７０、及び軌道テーブル３８０を含む。なお、補助記憶装置１３０に格納されている一部又は全部の情報は、メモリ１２０に格納されていてもよいし、軌道計画装置１００に接続されているデータベース等に格納されていてもよい。

【0027】

ロボットアーム構成テーブル３１０は、軌道計画や逆運学計算、動力学計算などで用いられるロボットアーム構成の情報を保持する。干渉物構成テーブル３２０は、軌道計画における干渉判定で用いられる、干渉物構成の情報を保持する。開始関節角度テーブル３３０は、軌道計画でのロボットアームの始点となる開始関節角度の情報を保持する。

【0028】

目標位置姿勢テーブル３４０は、軌道計画でのロボットアームの終点となる目標姿勢の情報を保持する。把持力・モーメント制約テーブル３５０は、軌道計画の時刻付与や最適化計算に用いられる、把持物への把持力及び把持物のモーメントに関する制約の情報を保持する。

【0029】

把持物情報テーブル３６０は、軌道計画において、ロボットアームが把持する把持物の情報を保持する。把持方法情報テーブル３７０は、軌道計画で用いる、把持方法データ４０７を記憶する部分である。軌道テーブル３８０は、結果出力部２０８で出力する軌道データ４０８を記憶する部分である。

【0030】

なお、本実施形態において、病院選定システムが使用する情報は、データ構造に依存せずどのようなデータ構造で表現されていてもよい。本実施形態ではテーブル形式で情報が表現されているが、例えば、リスト、データベース又はキューから適切に選択したデータ構造体が、情報を格納することができる。

【0031】

［ロボットアーム構成テーブル３１０］
図２は、ロボットアーム構成テーブル３１０の一例である。ロボットアーム構成テーブル３１０は、例えば、ロボットごとに定義されている。ロボットアーム構成テーブル３１０は、例えば、項番欄３１１、分類欄３１２、項目欄３１３、及び値欄３１４を含む。項番欄３１１は、ロボットアーム構成テーブル３１０のレコードを識別する番号を保持する。分類欄３１２は、ロボットアームの構成が属する分類を示す情報を保持する。

【0032】

ロボットアームの構成が属する分類は、例えば、関節情報及びリンク情報を含む。ロボットの可動軸は関節とリンクとを含む。項目欄３１３は、分類に対応する項目名を示す情報を保持する。値欄３１４は、各項目の値を示す情報を保持する。なお、関節情報は、ロボットアームに含まれる各関節の情報であり、例えば、関節名、関節の種類、関節の位置、関節の向き、関節の動作下限、関節の動作上限、最大加速度、及び最大速度などの情報を含む。

【0033】

リンク情報は、ロボットアームに含まれるリンクの情報であり、例えば、リンク名、親関節名、子関節名、及びリンク形状などの情報を含む。リンク形状とは、リンクの実際の形状である。ＳＴＥＰ（Ｓｔａｎｄａｒｄ ｆｏｒ ｔｈｅ Ｅｘｃｈａｎｇｅ ｏｆ Ｐｒｏｄｕｃｔ ｍｏｄｅｌ ｄａｔａ）などのフォーマットで保存されるソリッドデータや、ＳＴＬ（ＳＴｅｒｅｏＬｉｔｈｏｇｒａｐｈｙ）などのフォーマットで保存されるポリゴンデータ等で表現されるである。

【0034】

［干渉物構成テーブル３２０］
図３は、干渉物構成テーブル３２０の一例である。干渉物構成テーブル３２０は、例えば、干渉物ＩＤ欄３２１、干渉物形状欄３２２、及び干渉物姿勢欄３２３を含む。干渉物ＩＤ欄３２１は、干渉物を識別する干渉物ＩＤを保持する。

【0035】

干渉物形状欄３２２は、干渉物の形状を示す情報を保持する。干渉物の形状は、例えば、ＳＴＥＰなどのフォーマットで保存されるソリッドデータや、ＳＴＬなどのフォーマットで保存されるポリゴンデータなどで表現される。干渉物姿勢欄３２３は、干渉物が空間上のどの位置に置かれているかを示す情報である。干渉物の姿勢は、例えば、ＡＦＦＩＮＥ変換行列や、３次元空間上での位置とそのときのＲｏｌｌ―Ｐｉｔｃｈ－Ｙａｗなどで表現される。

【0036】

［開始関節角度テーブル３３０］
図４は、開始関節角度テーブル３３０の一例である。開始関節角度テーブル３３０は、例えば、関節名欄３３１及び開始関節角度欄３３２を有する。関節名欄３３１は、関節の名称（例えばロボットアーム構成テーブル３１０の関節名の項目に対応する値と同じ値が含まれる）を保持する。開始関節角度欄３３２は、関節の動作開始時の角度（即ち初期角度）を示す情報を保持する。

【0037】

［目標位置姿勢テーブル３４０］
図５は、目標位置姿勢テーブル３４０の一例である。目標位置姿勢テーブル３４０は、姿勢ＩＤ欄３４１、対象リンク名欄３４２、及び位置姿勢情報欄３４３を含む。姿勢ＩＤ欄３４１は、目標位置姿勢を識別する姿勢ＩＤを保持する。

【0038】

対象リンク名欄３４２は、対象のリンクの名称（例えばロボットアーム構成テーブル３１０のリンク名の項目に対応する値と同じ値が含まれる）を保持する。また、位置姿勢情報欄３４３はリンクの目標位置が空間上のどの位置であるかを示す情報である。目標位置姿勢は、例えば、ＡＦＦＩＮＥ変換行列や、３次元空間上での位置とそのときのＲｏｌｌ―Ｐｉｔｃｈ－Ｙａｗなどで表現される。

【0039】

［把持力・モーメント制約テーブル３５０］
図６は、把持力・モーメント制約テーブル３５０の一例である。把持力・モーメント制約テーブル３５０は、例えば、ロボットごとに定義されていてもよいし、アーム及びリンクごとに定義されていてもよいし、把持物ごとに定義されていてもよい。把持力・モーメント制約テーブル３５０は、例えば、項番欄３５１、項目欄３５２、及び値欄３５３を含む。項番欄３５１は、把持力・モーメント制約テーブル３５０のレコードを識別する番号を保持する。

【0040】

項番欄３５１は、把持力・モーメント制約テーブル３５０のレコードを識別する番号を保持する。項目欄３５２は、把持力やモーメントに関する制約を示す項目を保持する。把持力やモーメントに関する制約は、例えば、把持物にかかる最大把持力、及び把持物にかかる最大モーメントなどを含む。最大把持力は、把持物やロボットのハンドが許容できる最大の把持力であり、例えば、３次元の情報で表現される。最大モーメントは、把持物やハンドが許容できる最大のモーメントであり、例えば、３次元の情報で表現される。

【0041】

［把持物情報テーブル３６０］
図７は、把持物情報テーブル３６０の一例である。把持物情報テーブル３６０は、例えば、把持物ごとに定義されている。把持物情報テーブル３６０は、例えば、項番欄３６１、項目欄３６２、及び値欄３６３を含む。項番欄３６１は、把持物情報テーブル３６０のレコードを識別する番号を保持する。

【0042】

項目欄３６２は、把持物に関する情報の項目を保持する。項目欄３６２に格納される項目は、例えば、把持物名、把持物形状、把持物の重心位置、及び把持物の質量等を含む。把持物形状は、例えば、ＳＴＥＰなどのフォーマットで保存されるソリッドデータや、ＳＴＬなどのフォーマットで保存されるポリゴンデータなどで表現される。重心位置は、把持物形状の重心位置を示し、例えば、３次元データで表現される。質量は、把持物の質量示し、例えば、スカラー値で表現される。

【0043】

［把持方法情報テーブル３７０］
図８は、把持方法情報テーブル３７０の一例である。把持方法情報テーブル３７０は、例えば、把持方法ごとに定義されている。把持方法情報テーブル３７０は、例えば、項番欄３７１、項目欄３７２、及び値欄３７３を含む。項番欄３７１は、把持方法情報テーブル３７０のレコードを識別する番号を保持する。

【0044】

項目欄３７２は、把持方法に関する情報の項目を保持する。項目欄３７２に格納される項目は、例えば、把持方法及び把持位置等を含む。把持方法は、ロボットがどのようにして把持物を把持するかを示し、吸着を示すＤＩＯ、及び摩擦把持を示すＦＲＩＣＴＩＯＮなどは、いずれも把持方法の一例である。把持位置とは、把持物情報テーブル４６０が示す把持物がどの位置で把持されるかを示し、例えば、３次元データで表現される。

【0045】

［軌道テーブル３８０］
図９は、軌道テーブル３８０の一例である。軌道テーブル３８０は、軌道計画装置１００による処理によって出力されるテーブルである。軌道テーブル３８０は、例えば、軌道点ＩＤ欄３８１、関節角度情報欄３８２、関節角速度情報欄３８３、関節角加速度情報欄３８４、及び制御時刻欄３８５を含む。

【0046】

軌道点ＩＤ欄３８１は、ロボットアーム（関節）の軌道の経路上の点を識別する軌道点ＩＤを保持する。関節角度情報欄３８２は、制御時刻欄３８５が示す時刻における各関節の角度を示す情報を保持する。図９の例では、関節角度情報欄３８２のセルには、制御時刻欄３８５が示す時刻における各関節の角度を要素とするベクトルが格納されている。関節角速度情報欄３８３は、制御時刻欄３８５が示す時刻における各関節の角速度を示す情報を保持する。図９の例では、関節角速度情報欄３８３のセルには、制御時刻欄３８５が示す時刻における各関節の角速度を要素とするベクトルが格納されている。

【0047】

関節角加速度情報欄３８４は、制御時刻欄３８５が示す時刻における各関節の角加速度を示す情報を保持する。図９の例では、関節角加速度情報欄３８４のセルには、制御時刻欄３８５が示す時刻における各関節の角加速度を要素とするベクトルが格納されている。制御時刻欄３８５は、把持物が各軌道点の経路を通る時刻を示す情報を保持する。

【0048】

［フローチャート］
図１０は、軌道計画処理の一例を示すフローチャートである。データ読込部２０１は、軌道の計画に必要な各種データを読み込む（Ｓ１０１）。具体的には、データ読込部２０１は、ユーザによって入出力装置１６０に入力された情報に基づいて（例えば、軌道計画の対象のロボット、把持物、関節、及び／又は把持方法等が入力され、これらに対応するテーブルを読み込む）、ロボットアーム構成テーブル３１０、干渉物構成テーブル３２０、開始関節角度テーブル３３０、目標位置姿勢テーブル３４０、及び把持力・モーメント制約テーブル３５０を読み込む。なお、これらのテーブルの情報はいずれも、例えば、ユーザが入出力装置１６０に入力することによって登録される。

【0049】

また、データ読込部２０１は、（３次元センサ１７０を通して得られた情報である）把持物情報テーブル３６０及び把持方法情報テーブル３７０を読み込む。なお、ここではデータ読込部２０１が読み込んだデータが、入出力装置１６０や３次元センサ１７０を経由して取得される例を説明したが、過去の入力データをはじめとする、既に取得済みのデータが用いられてもよい。

【0050】

初期経路計画部２０５は、開始関節角度テーブル３３０と、目標位置姿勢テーブル３４０と、干渉物構成テーブル３２０と、に基づいて、動作する関節の開始関節角度θ_１から、当該関節に結ばれたリンクの目標姿勢Ｐ_ｎまでをロボットアーム及び把持物が干渉物と干渉しないよう繋ぐ初期経路を算出する（Ｓ１０２）。初期経路計画部２０５は、どのような手段で干渉しない初期経路を求めてもよいが、例えば、ＲＲＴ（Ｒａｐｉｄｌｙ－ｅｘｐｒｏｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅ）法を用いて初期経路を算出する。そして、初期経路計画部２０５は、算出した初期経路の各点に軌道点ＩＤを付与し、各軌道点ＩＤに示す各点（各軌道点）おける関節角度を軌道テーブル３８０に格納する。

【0051】

把持物考慮時刻付与部２０６は、軌道テーブル３８０に格納されている各軌道点に対して、把持力・モーメント制約テーブル３５０に基づいて、関節角速度、関節角速度、及び制御時刻を付与又は修正する（Ｓ１０３）。なお、ステップＳ１０３の詳細については図１１を用いて後述する。

【0052】

軌道計画部２０４は、軌道テーブル３８０の軌道の変化が無くなったか判定する（Ｓ１０４）。具体的には、例えば、軌道計画部２０４は、直前のステップＳ１０５で更新された軌道における各軌道点における位置又は時刻と、当該更新の直前の軌道における各軌道点における位置又は時刻と、の差の合計値が所定値以下である場合に軌道の変化が無くなったと判定し、所定値を超える場合に軌道の変化があったと判定する。

【0053】

なお、初回のステップＳ１０４においては、軌道計画部２０４は、例えば、必ず軌道テーブル３８０の値の変化があったと判定してもよいし、ステップＳ１０３で付与した各軌道点の位置又は時刻と、所定の軌道の各軌道点の位置又は時刻と、の差の合計値に基づいて軌道の変化があったかないかを判定してもよい。軌道計画部２０４は、軌道テーブル３８０の軌道の変化が無くなったと判定した場合（Ｓ１０４：ＹＥＳ）、ステップＳ１０６に遷移する。

【0054】

軌道計画部２０４が軌道テーブル３８０の軌道の変化があったと判定した場合（Ｓ１０４：ＮＯ）、把持物考慮経路最適化部２０７は、把持力・モーメント制約テーブル３５０に基づいて、動力学計算部２０３等を用いて、把持物にかかる把持力、及びモーメントが小さくなるよう軌道の経路を変更し、軌道テーブル３８０の関節角度、関節角速度、及び関節角速度を更新することで軌道を最適化する（Ｓ１０５）。なお、ステップＳ１０５の詳細については図１２を用いて後述する。

【0055】

結果出力部２０８は、補助記憶装置１３０に格納されているデータに基づいて結果表示画面を生成し、入出力装置１６０に表示する（Ｓ１０６）。結果表示画面の一例については、図１３を用いて後述する。

【0056】

［把持物考慮時刻付与処理］
図１１は、ステップＳ１０３における制御時刻付与処理の一例を示すフローチャートである。把持物考慮時刻付与部２０６は、本処理においては、軌道テーブル３８０における関節角度は変更せず、関節角速度、関節角加速度、及び制御時刻を変更する。以下、本処理について説明する。

【0057】

把持物考慮時刻付与部２０６は、軌道テーブル３８０から軌道のデータを取得する（Ｓ２０１）。把持物考慮時刻付与部２０６は、ロボットアーム構成テーブル３１０に格納されている、各関節軸の最大速度情報に基づいて、全ての軌道点に対して時刻を付与し、付与した時刻を軌道テーブル３８０の制御時刻欄３８５に格納する。把持物考慮時刻付与部２０６は、この時刻Δｔ_ｉを、例えば、以下の式によって決定する。

【0058】

【数1】

【0059】

なお、上記式のｋは関節を識別する関節番号であり、ｉは軌道点ＩＤでありθ_ｋ，ｉは関節番号ｋの関節の軌道点ＩＤがｉの軌道点における関節角度である。また、Ｖｍａｘ_ｋは、関節ｋの最大速度情報である。

【0060】

把持物考慮時刻付与部２０６は、ステップＳ２０４からステップＳ２０８までの処理を、時刻の更新がなくなるまで繰り返す（Ｓ２０３）。把持物考慮時刻付与部２０６は、ステップＳ２０５からステップＳ２０８までの処理を、軌道テーブル３８０に格納されている全ての軌道点に対して順番に行う。

【0061】

把持物考慮時刻付与部２０６は、軌道点ｉにおける関節角速度、関節角加速度を計算し、軌道テーブル３８０の関節角速度、関節角加速度を修正する（Ｓ２０５）。なお、把持物考慮時刻付与部２０６は、関節角速度θ’_ｉ、及び関節角加速度θ’’_ｉを、例えば、以下の数値微分による式によって算出する。

【0062】

【数2】

【0063】

【数3】

【0064】

動力学計算部２０３は、軌道点ｉにおいて把持物にかかる把持力、及び把持物にかかるモーメントを算出し、把持物考慮時刻付与部２０６は、算出された把持力、及びモーメントが、把持力・モーメント制約テーブル３５０に格納されている制約を満たしているかを判定する（Ｓ２０６）。

【0065】

ステップＳ２０６における計算処理を、図１４を用いて説明する。図１４は、把持物にかかる把持力、及び把持物にかかるモーメントの計算処理の一例を示す説明図である。図１４の例では、ロボットアームに装着された吸盤が把持物を吸着把持する。まず、動力学計算部２０３は、軌道点ｉにおける関節角速度θ’_ｉ及び関節角加速度θ’’_ｉから、把持物の加速度ａ_ｉを以下の式によって算出する。

【0066】

【数4】

【0067】

なお、Ｊ_ｉは、軌道点ｉにおける把持物の位置及び姿勢の変化方向を表すヤコビ行列である。動力学計算部２０３は、上記の式を計算することで、水平方向の移動に伴う加速度だけでなく、円運動に伴う中心方向に向かう遠心加速度等をまとめて計算できる。図１４の状態において、把持物の重心と一緒に動く座標系上においては、把持物は静止しているとみなせるため、重力ｍｇと把持力Ｔ_ｉ、及び慣性力－ｍａ_ｉの力のつり合いより以下の式が成り立つ。

【0068】

【数5】

【0069】

また、動力学計算部２０３は、把持物の重心周りでのモーメントＭ_ｉを、把持物の重心位置から見た把持位置ｒを用いて以下の式によって算出する。

【0070】

【数6】

【0071】

把持物考慮時刻付与部２０６は、ステップＳ２０６で算出された把持力Ｔ_ｉ、およびモーメントＭ_ｉ等が、把持力・モーメント制約テーブル３５０に格納されている制約を満たしているかを判定する。

【0072】

図１１の説明に戻る。把持物考慮時刻付与部２０６は、ステップＳ２０６において制約を満たしていないと判定した場合（Ｓ２０６：ＮＯ）、Δｔ_ｉを所定の時刻分増加させ、軌道テーブル３８０の制御時刻を更新する（Ｓ２０７）。ステップＳ２０５～ステップＳ２０７の処理のループによって制御時刻の増加が繰り返されることで、関節角速度及び関節角速度が小さくなっていくため、把持物にかかる慣性力やモーメントも小さくなり、ステップＳ２０６の判定における制約がいずれ満たされることになり、最終的にステップＳ２０３のループを抜ける条件が満たされる。

【0073】

そして、把持物考慮時刻付与部２０６がΔｔ_ｉを少しずつ増加させて制御時刻を伸ばしていくことで、ステップＳ２０３のループを抜けるタイミングでは、制約を満たしつつも制約値に近い値でロボットアームが動く軌道を実現できる。

【0074】

［把持物考慮経路最適化処理］
図１２は、ステップＳ１０５における把持物考慮経路最適化処理の一例を示すフローチャートである。本処理では、把持物考慮経路最適化部２０７は、軌道テーブル３８０おける制御時刻は変更せず、関節角度、関節角速度、及び関節角加速度を変更する。本処理では、把持物考慮経路最適化部２０７は、軌道における経路を曲線σととらえ、経路に対する評価関数Ｆ（σ）を定義し、この評価関数が最小となる経路を生成することで、把持力・モーメントが小さくなるように軌道テーブル３８０の軌道を修正する。軌道に対する評価関数の一例を以下に示す。

【0075】

【数7】

【0076】

ここで、積分内の第一項は把持力に関する評価値であり、第二項はモーメントに関する評価値であり、第三項は軌道の滑らかさに関する評価値である。なお、出力される軌道が関節角度空間でも平滑化されるようにするために、上記式の第三項のような軌道の滑らかさを評価する指標が、Ｆ（σ）に含まれているとよいが、含まれていなくてもよい。また、α、β、γはそれぞれ、各評価値の重みである。φはソフトプラス関数であり、入力値が閾値以上で大きくなるほど出力の傾きが増し、入力値が閾値より小さい場合には傾きが緩やかである関数である。以下、本処理について説明する。

【0077】

上記積分内の第一項の把持力に関する評価値が低下することにより把持物を把持するロボットアームの吸着パッド等が外れる可能性が低下し、第二項のモーメントに関する評価値が低下することにより把持物が破損する可能性が低下し、第三項の軌道の滑らかさに関する評価値が低下することにより角加速度が低下する。

【0078】

把持物考慮経路最適化部２０７は、軌道テーブル３８０に格納されている軌道のデータを取得する（Ｓ３０１）。把持物考慮経路最適化部２０７は、最適化計算によって軌道σが収束するまでステップＳ３０３からステップＳ３０８までの処理をループ演算する（Ｓ３０２）。

【0079】

把持物考慮経路最適化部２０７は、把持力やモーメントからなる評価関数Ｆ（σ）から評価値を計算する（Ｓ３０３）。把持物考慮経路最適化部２０７は、把持力やモーメントを、ステップＳ２０６の説明で述べた手法で算出できる。

【0080】

把持物考慮経路最適化部２０７は、評価関数Ｆ（σ）を小さくする方向を示す汎関数微分δＦ（σ）／δσを計算する（Ｓ３０４）。把持物考慮経路最適化部２０７は、例えば、経路を少しずつ変更することで数値微分として汎関数微分を計算してもよいし、ＳＴＯＭＰ（Ｓｔｏｃｈａｓｔｉｃ Ｔｒａｊｅｃｔｏｒｙ Ｏｐｔｉｍｉｚａｔｉｏｎ ｆｏｒ Ｍｏｔｉｏｎ Ｐｌａｎｎｉｎｇ）法によって、正規分布に従ったノイズを与えた軌道を複数作成し、軌道の変化と評価関数の変化から汎関数微分を計算してもよい。

【0081】

把持物考慮経路最適化部２０７は、軌道σ_ｎｅｗを以下の式で算出する（Ｓ３０５）。

【0082】

【数8】

【0083】

干渉判定部２０２は、ロボットアーム構成テーブル３１０と干渉物構成テーブル３２０とを参照して使い、軌道σ_ｎｅｗにおいてロボットアーム及び把持物が干渉物と干渉していないかを判定する（Ｓ３０６）。

【0084】

干渉判定部２０２は、干渉が発生していないと判定した場合（Ｓ３０６：ＮＯ）、把持物考慮経路最適化部２０７は、軌道σ_ｎｅｗを新たな軌道σとして採用して、軌道テーブル３８０の関節角度を更新し、さらに制御時刻の情報にしたがい、各軌道点における関節速度及び関節角加速度を、ステップＳ２０５で説明した数式を用いて更新する（Ｓ３０７）。干渉判定部２０２は、干渉が発生していると判定した場合（Ｓ３０６：ＹＥＳ）、ステップＳ３０７の処理へと遷移せずにステップＳ３０２のループ判定へと遷移する。

【0085】

把持物考慮経路最適化部２０７がステップＳ３０３からステップＳ３０８の処理を繰り返すことで、把持力及び／又はモーメントが小さくなるように経路を変更することができる。

【0086】

図１５は、軌道計画処理における軌道の変化の一例を示す説明図である。図１５の、横方向には把持物を平行移動する軌道に関する、最適化計算の回数と付与される時刻、およびその時の把持物の姿勢と把持物にかかるモーメントの関係を示す。ステップＳ１０２で算出された初期軌道（ａ）の各軌道点に対して、仮に最大速度で時刻が付与されると、モーメントが上限値を超える経由点が発生する。

【0087】

そこで、把持物考慮時刻付与部２０６は、ステップＳ１０３の処理において、各軌道点に付与する時間間隔をのばすことで、角加速度を小さくし、モーメントが上限値を超えないような時刻を付与することで（ｂ）の軌道が得られる。

【0088】

次に、把持物考慮経路最適化部２０７は、ステップＳ１０５の処理において、モーメントが最小となるよう、把持物を傾けた経路を生成することで（ｃ）の軌道が得られる。これにより、把持物を傾ける軌道が得られるためモーメントが上限値より小さくなり、加速度を上げることができるため、把持物考慮時刻付与部２０６は、再度ステップＳ１０３の処理を実行して、時間間隔を短くするような時刻を付与して（ｄ）の軌道を得る。これらの処理が繰り返されることで、最大速度、最大加速度で把持物を移動させながら、モーメントが上限値以下となる軌道が生成される。

【0089】

［出力画面］
図１３は、結果出力画面の一例である。結果出力画面１３００は、例えば、読込ボタン１３０１、計算開始ボタン１３０２、動作時間表１３０３、把持力変化グラフ１３０４、モーメント変化グラフ１３０５、及びシミュレート表示１３０６を含む。読込ボタン１３０１が選択されると、入出力インターフェース１５０を介して入力された入力データが読み込まれる。

【0090】

計算開始ボタン１３０２が選択されると、軌道計画計算（ステップＳ１０２以降の処理）が実行され、動作時間表１３０３、把持力変化グラフ１３０４、モーメント変化グラフ１３０５、及びシミュレート表示１３０６が表示される。把持力変化グラフ１３０４は、軌道の各制御時刻における把持力の変化を示し、モーメント変化グラフ１３０５は、軌道の各制御時刻におけるモーメントの変化を示す。なお、結果出力画面１３００において、把持力変化グラフ１３０４又はモーメント変化グラフ１３０５の一方が表示されなくてもよい。

【0091】

動作時間表１３０３には、例えば、把持力・モーメント制約テーブル３５０、把持物情報テーブル３６０、及び把持方法情報テーブル３７０等の条件や制約に従って軌道計画を行った場合の動作時間が表示される。また、これらのテーブルの条件が変更されることで、把持力変化グラフ１３０４やモーメント変化グラフ１３０５も変化する。また、動作時間表１３０３内の動作再生ボタンが選択されると、シミュレート表示領域においてロボットアームの動作が再生表示される。

【0092】

［効果等］
以上に説明したように本実施例の軌道計画装置１００は、把持物にかかる把持力やモーメントの制約を満たしつつ、始点から終点までを高速に繋ぐ軌道を生成することができる。

【0093】

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

【0094】

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

【0095】

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

【符号の説明】

【0096】

１００ 軌道計画装置、１１０ ＣＰＵ、１２０ メモリ、１３０ 補助記憶装置、１４０ 通信装置、１５０ 入出力インターフェース、１６０ 入出力装置、１５０ ３次元センサ、２０１ データ読込部、２０２ 干渉判定部、２０３ 動力学計算部、２０４ 軌道計画部、２０５ 初期経路探索部、２０６ 把持物考慮時刻付与部、２０７ 把持物考慮経路最適化部、２０８ 結果出力部、３１０ ロボットアーム構成テーブル、３２０ 干渉物構成テーブル、３３０ 開始関節角度テーブル、３４０ 目標位置姿勢テーブル、３５０ 把持力・モーメント制約テーブル、３６０ 把持物情報テーブル、３７０ 把持方法情報テーブル、３８０ 軌道テーブル

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】