特開 2022-188627 制御方法、制御装置、情報処理方法、情報処理装置、ロボット装置、物品の製造方法、プログラムおよび記録媒体

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

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

(11)【公開番号】P2022188627

(43)【公開日】2022-12-21

(54)【発明の名称】制御方法、制御装置、情報処理方法、情報処理装置、ロボット装置、物品の製造方法、プログラムおよび記録媒体

(51)【国際特許分類】

   B25J 13/00 20060101AFI20221214BHJP

【ＦＩ】

B25J13/00 Z

【審査請求】未請求

【請求項の数】27

【出願形態】ＯＬ

(21)【出願番号】P 2021096818

(22)【出願日】2021-06-09

(71)【出願人】

【識別番号】000001007

【氏名又は名称】キヤノン株式会社

(74)【代理人】

【識別番号】100126240

【弁理士】

【氏名又は名称】阿部 琢磨

(74)【代理人】

【識別番号】100124442

【弁理士】

【氏名又は名称】黒岩 創吾

(72)【発明者】

【氏名】徳岡 岳海

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

3C707

【Ｆターム（参考）】

3C707AS01

3C707AS06

3C707BS11

3C707DS01

3C707ES03

3C707JS06

3C707JU02

3C707JU14

3C707KS22

3C707KS24

3C707KV01

3C707LS20

3C707LV15

3C707MS10

3C707MS11

3C707MT05

(57)【要約】

【課題】 全ての軸の制約を満たすことができない場合でも、ハードウェアの破損の危険性を可能な限り低減した停止を実行できる制御値の取得を行うことが可能な制御方法を提供する。
【解決手段】 少なくとも第１関節と第２関節と、を有するロボットを停止させる制御方法であって、停止を実行させる制御値を取得する際、第１関節における第１物理的制約と、第２関節における第２物理的制約と、を満たす制御値を取得できない場合、所定条件に基づき、第１物理的制約と第２物理的制約とのいずれかを優先して満たすように制御値を取得する、ことを特徴とする制御方法を採用した。
【選択図】 図６

【特許請求の範囲】

【請求項1】

少なくとも第１関節と第２関節と、を有するロボットを停止させる制御方法であって、
前記停止を実行させる制御値を取得する際、前記第１関節における第１物理的制約と、前記第２関節における第２物理的制約と、を満たす前記制御値を取得できない場合、所定条件に基づき、前記第１物理的制約と前記第２物理的制約とのいずれかを優先して満たすように前記制御値を取得する、
ことを特徴とする制御方法。

【請求項2】

請求項１に記載の制御方法において、
前記所定条件は、前記停止を実行する指令を取得した第１タイミングにおいて前記第１関節に生じている第１物理量と、前記第２タイミングにおいて前記第２関節に生じている第２物理量と、に基づいて、前記第１物理的制約と前記第２物理的制約とのいずれかを優先する条件である、
ことを特徴とする制御方法。

【請求項3】

請求項２に記載の制御方法において、
前記第１物理量と前記第２物理量とに基づき、値が大きい方の物理量が生じている関節における物理的制約を優先する、
ことを特徴とする制御方法。

【請求項4】

請求項２に記載の制御方法において、
前記所定条件は、前記第１タイミング以前における前記第１物理量と前記第２物理量とに基づき、最も値の大きい物理量が生じている関節における物理的制約を優先する、
ことを特徴とする制御方法。

【請求項5】

請求項２に記載の制御方法において、
前記所定条件は、前記第１タイミング以前における前記第１物理量と前記第２物理量とに基づき、所定閾値に達した回数が最も大きい関節における物理的制約を優先する、
ことを特徴とする制御方法。

【請求項6】

請求項２に記載の制御方法において、
前記所定条件は、前記第１タイミングよりも後の第２タイミングにおいて前記第１関節に生じるであろう前記第１物理量と、前記第３タイミングにおいて前記第２関節に生じるであろう前記第２物理量と、に基づいて、前記第１物理的制約と前記第２物理的制約とのいずれかを優先する条件である、
ことを特徴とする制御方法。

【請求項7】

請求項６に記載の制御方法において、
前記第２タイミングにおける前記第１物理量と前記第３物理量と、に基づき、所定閾値に対する超過量が小さい方の物理量が生じるであろう関節における物理的制約を優先する、
ことを特徴とする制御方法。

【請求項8】

請求項６または７に記載の制御方法において、
前記第２タイミングは、前記第１タイミングの１タイムステップ後のタイミングである、
ことを特徴とする制御方法。

【請求項9】

請求項８に記載の制御方法において、
前記タイムステップは、前記ロボットの制御周期によって設定される、
ことを特徴とする制御方法。

【請求項10】

請求項１から９のいずれか１項に記載の制御方法において、
前記制御値は、
第１物理的制約および前記第２物理的制約を満たして前記停止を実行できる第１目標値と、
前記第１目標値に基づいて取得される媒介変数と、
前記媒介変数に基づいて取得される第２目標値と、に基づき取得される、
ことを特徴とする制御方法。

【請求項11】

請求項１０に記載の制御方法において、
前記媒介変数は、前記第１目標値と、前記ロボットを動作させるための軌道データと、に基づき取得される、
ことを特徴とする制御方法。

【請求項12】

請求項１１に記載の制御方法において、
前記媒介変数は、前記第１目標値と、前記軌道データを直線補間することで取得される、
ことを特徴とする制御方法。

【請求項13】

請求項１０から１２のいずれか１項に記載の制御方法において、
前記第１目標値を前記制御値として使用し、前記停止を実行する、
ことを特徴とする制御方法。

【請求項14】

請求項１から１３のいずれか１項に記載の制御方法において、
前記第１物理量と前記第２物理量は、速度、加速度、ジャーク、トルク、荷重、のいずれかである、
ことを特徴とする制御方法。

【請求項15】

請求項１から１４のいずれか１項に記載の制御方法において、
前記ロボットの手先にかかる第３物理量に基づき、前記第１物理量および前記第２物理量を取得する、
ことを特徴とする制御方法。

【請求項16】

請求項１に記載の制御方法において、
前記制御値の取得の際、前記第１物理的制約と前記第２物理的制約とのいずれかを優先して満たす場合と、前記第１物理的制約と前記第２物理的制約とを満たす場合とを切り換える、
ことを特徴とする制御方法。

【請求項17】

請求項１に記載の制御方法において、
前記停止に基づき、前記第１物理的制約および前記第２物理的制約を満たさない前記制御値を取得して前記停止を実行する場合に切り換える、
ことを特徴とする制御方法。

【請求項18】

請求項１７に記載の制御方法において、
前記停止は、作業者に関連する、
ことを特徴とする制御方法。

【請求項19】

請求項１から１８のいずれか１項に記載の制御方法を用いて前記ロボットを制御し、物品の製造を行うことを特徴とする物品の製造方法。

【請求項20】

少なくとも第１関節と第２関節と、を有するロボットを停止させる制御装置であって、
前記停止を実行させる制御値を取得する際、前記第１関節における第１物理的制約と、前記第２関節における第２物理的制約と、を満たす前記制御値を取得できない場合、所定条件に基づき、前記第１物理的制約と前記第２物理的制約とのいずれかを優先して満たすように前記制御値を取得する、
ことを特徴とする制御装置。

【請求項21】

請求項２０に記載の制御装置と前記ロボットとを備えたロボット装置。

【請求項22】

少なくとも第１関節と第２関節と、を有するロボットの停止に関するシミュレーションを実行する情報処理方法であって、
前記停止を実行させる制御値を取得する際、前記第１関節における第１物理的制約と、前記第２関節における第２物理的制約と、を満たす前記制御値を取得できない場合、所定条件に基づき、前記第１物理的制約と前記第２物理的制約とのいずれかを優先して満たすように前記制御値を取得し、
前記制御値に基づき、前記ロボットの前記シミュレーションを実行する、
ことを特徴とする情報処理方法。

【請求項23】

請求項２２に記載の情報処理方法において、
前記ミュレーションは、複数種類あり、
表示部に、複数種類の前記シミュレーションを実行するためのボタンを表示する、
ことを特徴とする情報処理方法。

【請求項24】

少なくとも第１関節と第２関節と、を有するロボットの停止に関するシミュレーションを実行する情報処理装置であって、
前記停止を実行させる制御値を取得する際、前記第１関節における第１物理的制約と、前記第２関節における第２物理的制約と、を満たす前記制御値を取得できない場合、所定条件に基づき、前記第１物理的制約と前記第２物理的制約とのいずれかを優先して満たすように前記制御値を取得し、
前記制御値に基づき、前記ロボットの前記シミュレーションを実行する、
ことを特徴とする情報処理装置。

【請求項25】

請求項２４に記載の情報処理装置において、
前記ミュレーションは、複数種類あり、
表示部に、複数種類の前記シミュレーションを実行するためのボタンを表示する、
ことを特徴とする情報処理装置。

【請求項26】

請求項１から１８のいずれか１項に記載の制御方法、または請求項２２または２３に記載の情報処理方法を実行可能なプログラム。

【請求項27】

請求項２６に記載のプログラムを格納した、コンピュータで読み取り可能な記録媒体。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、ロボットの制御方法、ロボットの情報処理方法に関する。

【背景技術】

【0002】

近年、工場等の生産現場において、組立・搬送等の作業は複数の関節を有するロボットアームとロボットアームにハンドやツールなどのエンドエフェクタを備えたロボットシステムによる自動化が進められている。ロボットアームを動作させる際には、ユーザが入力した目標位置、目標速度、補間方法等に基づいて、制御コントローラが各関節の目標値（目標角度）を生成し、所望の動作を実行させる。このようなロボットシステムでは、ユーザにとって想定外の動作や、何かしらの異常が発生した場合には、ユーザおよびロボットシステムの安全を確保するために直ちにロボットアームおよびエンドエフェクタを停止させる必要がある。

【0003】

下記に示す非特許文献には、通常動作の時間軸と同次元の媒介変数を導入することで、ロボットシステムの手先（エンドエフェクタ）の移動経路が通常動作と停止動作とで一致するような停止目標値を算出する方法が記載されている。任意の軸における加速度、ジャーク、トルクといった各種物理的制約に基づく媒介変数を算出し、媒介変数に基づいて他の軸の目標値を算出し、算出した各軸目標値が全ての軸の物理的制約を満たすならばその各軸目標値を採用する。各軸の目標値を同一の媒介変数を使用して算出するため、各軸の位置関係が通常軌道と同じであることが担保され、通常動作の手先の経路と停止動作の手先の経路が同一となる。

【0004】

これにより、停止前において本来移動するはずだった経路と同一の経路上で、エンドエフェクタを停止させることができ、停止時にエンドエフェクタが周辺物体と衝突するおそれを低減することができる。また、停止時の目標値の算出において物理的制約を遵守することで、アームリンクや減速機等のハードウェアに対して過大な要求となるような急減速指令とならないようにすることができ、ハードウェアの破損の危険性を低減することができる。

【先行技術文献】

【非特許文献】

【0005】

【非特許文献1】Ｆｒｉｅｄｒｉｃｈ Ｌａｎｇｅ ａｎｄ Ｍｉｃｈａｅｌ Ｓｕｐｐａ．“Ｔｒａｊｅｃｔｏｒｙ Ｇｅｎｅｒａｔｉｏｎ ｆｏｒ Ｉｍｍｅｄｉａｔｅ Ｐａｔｈ－Ａｃｃｕｒａｔｅ Ｊｅｒｋ－Ｌｉｍｉｔｅｄ Ｓｔｏｐｐｉｎｇ ｏｆ Ｉｎｄｕｓｔｒｉａｌ Ｒｏｂｏｔｓ”Ｐｒｏｃ．Ｉｎｔ．Ｃｏｎｆ．ｏｎ Ｒｏｂｏｔｉｃ ａｎｄ Ａｕｔｏｍａｔｉｏｎ（ＩＣＲＡ），Ｍａｙ ２０１５

【発明の概要】

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

【0006】

しかしながら上記非特許文献１に記載の方法は、停止のための各軸目標値（制御値）の取得において、全ての軸の制約を満たすことができる各軸目標値が算出できない場合は、実行中の動作においてあらかじめ教示されていた所定の目標値をそのまま使用している。よって全ての軸の制約を満たすことができる各軸目標値が算出できない場合の停止においては、ハードウェアの破損の危険性の低減に関して十分な考慮がなされていない。

【0007】

本発明は上記課題に鑑み、全ての軸の制約を満たすことができない場合でも、ハードウェアの破損の危険性を可能な限り低減した停止を実行できる制御値の取得を行うことが可能な制御方法を提供する。

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

【0008】

上記課題を解決するために本発明は、少なくとも第１関節と第２関節と、を有するロボットを停止させる制御方法であって、前記停止を実行させる制御値を取得する際、前記第１関節における第１物理的制約と、前記第２関節における第２物理的制約と、を満たす前記制御値を取得できない場合、所定条件に基づき、前記第１物理的制約と前記第２物理的制約とのいずれかを優先して満たすように前記制御値を取得する、ことを特徴とする制御方法を採用した。

【発明の効果】

【0009】

本発明によれば、全ての軸の制約を満たすことができない場合でも、ハードウェアの破損の危険性を可能な限り低減した停止を実行できる制御値の取得を行うことが可能となる。

【図面の簡単な説明】

【0010】

【図1】実施形態におけるロボットシステム１０００の概略図である。

【図2】実施形態におけるロボットシステム１０００の制御ブロック図である。

【図3】実施形態における制御フローチャートである。

【図4】実施形態におけるロボットアーム本体２００の通常動作における軌道のグラフである。

【図5】実施形態における媒介変数を示した状態の軌道のグラフである。

【図6】実施形態における媒介変数を示した状態の軌道のグラフである。

【図7】実施形態におけるロボットシステム１０００の概略図である。

【図8】実施形態における制御フローチャートである。

【図9】実施形態における情報処理装置８００の概略図である。

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

【0011】

以下、添付図面を参照して本発明を実施するための形態について説明する。なお、以下に示す実施形態はあくまでも一例であり、例えば細部の構成については本発明の趣旨を逸脱しない範囲において当業者が適宜変更することができる。また、本実施形態で取り上げる数値は、参考数値であって、本発明を限定するものではない。なお以下の図面において、図中の矢印Ｘ、Ｙ、Ｚはロボットシステムの全体の座標系を示す。一般に、ＸＹＺ３次元座標系は、設置環境全体のワールド座標系を示す。その他、制御の都合などによって、ロボットハンド、指部、関節などに関して適宜ローカル座標系を用いる場合がある。本実施形態では全体の座標系であるワールド座標系をＸＹＺ、ローカル座標系をｘｙｚで表すものとする。

【0012】

（第１の実施形態）
図１は本実施形態におけるロボットシステム１０００を示した概略図である。図１より、ロボットシステム１０００は、多関節のロボットアーム本体２００と、ロボットアーム本体２００の先端部に設けられたエンドエフェクタ３００と、ロボットアーム本体２００とエンドエフェクタ３００とを制御する制御装置４００とを備えている。また、ロボットアーム本体２００とエンドエフェクタ本体３００とを用いて操作されるワーク６００、ワーク６００が載置される載置台５００、ワークの有無を制御装置４００に伝える信号入力装置７００を備えている。さらに、ロボットアーム本体２００およびエンドエフェクタ本体３００に指令値を入力する外部入力装置８００を備えている。

【0013】

ロボットアーム本体２００は、地面と接触する基台２１０と、５つのリンク２０１、２０２、２０３、２０４、２０５を備えている。また、これらリンク２０１～２０５を図１に示す各矢印方向へ回動可能に連結する５つの関節Ｊ１、Ｊ２、Ｊ３、Ｊ４、Ｊ５を備えている。また、関節Ｊ１～Ｊ５はリンク２０１～２０５を搖動または回動させるためのアーム用モータ２１１～２１５を有している（図２）。各アーム用モータ２１１～２１５にはモータの回転を減速し、各リンク２０１～２０５を回転させるためのトルクを発生させる付図示の減速機を備えている。さらに各関節Ｊ１～Ｊ５にはエンコーダ２２１～２２５（図２）が設けられており、この各エンコーダから各関節の角度を検出し制御装置４００にフィードバックしている。これにより各リンクを所定の位置に位置させることで、エンドエフェクタ本体３００をＸＹＺ空間における所定の位置に位置させることができる。

【0014】

エンドエフェクタ本体３００はロボットハンドであり、指３０１及び３０２と、指３０１および指３０２を互いに接近または離間させる開閉動作を行うためのハンド用モータ３１２（図２）および付図示の減速機を備えている。ロボットシステム１０００は、ワーク６００をエンドエフェクタ本体３００によって把持し、別のワークへ接触させるなどの処理を行う。こうすることで、材料としてワーク６００と別のワークを用い、成果物としてワーク６００と別のワークとが組みついた物品の製造を行うことが可能となる。また、ハンド用モータ３１２には、エンコーダ３２２（図２）が設けられており、このエンコーダからハンド用モータ３１２の速度を検出し制御装置４００にフィードバックしている。なお、本実施形態ではエンドエフェクタ本体３００としてロボットハンドを例にとり説明したがこれに限られない。例えば、電動ドライバーやエンドミル等のツールを使用しても構わない。

【0015】

図２は図１のロボットシステム１０００の制御系の詳細な構成を示したブロック図である。制御装置４００は、コンピュータで構成されており、プロセッサであるＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）４０１を備える。また、記憶部として、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）４０２、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）４０３、ＨＤＤ（Ｈａｒｄ Ｄｉｓｃ Ｄｒｉｖｅ）４０４、記録ディスクドライブ４０５を備えている。また、各機器と通信を行うためのインタフェース４０６、４０７、４０８、４０９、４１０、４１１を備える。ＣＰＵ４０１、ＲＯＭ４０２、ＲＡＭ４０３、インタフェース４０６～４１２は、互いに通信可能にバス４１２で接続されている。

【0016】

このうち、ＲＡＭ４０３は外部入力装置８００の操作による教示点や制御指令などのデータの一時記憶に用いられる。ＲＯＭ４０２には、ＣＰＵ４０１に、各種演算処理を実行させるためのＢＩＯＳ等の基本プログラム４３０が格納されている。ＣＰＵ４０１は、ＨＤＤ４０４に記録（格納）された制御プログラムに基づいて各種演算処理を実行する。ＨＤＤ４０４は、ＣＰＵ４０１の演算処理結果である各種のデータ等を記憶する記憶部である。記録ディスクドライブ４０５は、記録ディスク４３１に記録された各種データや制御プログラム等を読み出すことができる。更に、インタフェース４０７、４０８には、各種画像が表示されるモニタ８１１や書き換え可能な不揮発性メモリや外付けＨＤＤ等の外部記憶装置８１２が接続されている。

【0017】

外部入力装置８００は、例えばティーチングペンダント（ＴＰ）のような操作装置が考えられるが、ロボットプログラムを編集可能な他のコンピュータ装置（ＰＣやサーバ）であってもよい。外部入力装置８００は、制御装置４００に対して有線ないし無線の通信接続手段を介して接続することができ、ロボット操作および状態表示などのユーザインターフェース機能を有する。外部入力装置８００により入力された各関節Ｊ１～Ｊ５の目標関節角度をインタフェース４０６及びバス４１２を介してＣＰＵ４０１に出力する。

【0018】

ＣＰＵ４０１は、例えば外部入力装置８００で入力された教示点データをインタフェース４０６から受信する。また、外部入力装置８００から入力された教示点データに基づきロボットアーム本体２００の各軸の軌道を生成し、インタフェース４０９を介し、アーム用モータドライバ２３０を用いてアーム用モータ２１１～２１５に送信することができる。ＣＰＵ４０１は、各アーム用モータ２１１～２１５の回転角度の制御量を示す駆動指令のデータを所定間隔でバス４１２及びインタフェース４０９を介してアーム用モータドライバ２３０に出力する。

【0019】

アーム用モータドライバ２３０は、ＣＰＵ４０１から入力を受けた駆動指令に基づき、各アーム用モータ２１１～２１５への電流の出力量を演算し、各アーム用モータ２１１～２１５へ電流を供給して、各関節Ｊ１～Ｊ５の関節角度制御を行う。また、前述した各エンコーダ２２１～２２５からのパルス信号をインタフェース４０９及びバス４１２を介してＣＰＵ４０１に出力する。即ち、ＣＰＵ４０１は、アーム用モータドライバ２３０を介して、各エンコーダ２２１～２２５により検出される関節Ｊ１～Ｊ６の関節角度現在値が目標関節角度となるように、各アーム用モータ２１１～２１５のフィードバック制御を実行する。

【0020】

さらに制御装置４００はハンド用モータ３１２ともインタフェース４１０およびハンド用モータドライバ３３０を介して接続されている。ハンド用モータドライバ３３０は、ＣＰＵ４０１から入力を受けた駆動指令に基づき、ハンド用モータ３１２への電流の出力量を演算し、ハンド用モータ３１２へ電流を供給して、ハンド用モータ３１２の速度制御を行う。また、前述したエンコーダ３２２からのパルス信号をインタフェース４１０及びバス４１２を介してＣＰＵ４０１に出力する。即ち、ＣＰＵ４０１は、ハンド用モータドライバ３３０を介して、エンコーダ３２２により検出されるハンド用モータ３１２の速度の現在値が目標速度となるように、ハンド用モータ３１２のフィードバック制御を実行する。

【0021】

さらに制御装置４００は信号入力装置７００ともインタフェース４１１およびバス４１２を介して接続されている。載置台５００には不図示の感圧センサといった、ワーク６００の有無を検出可能なセンサを備えている。信号入力装置７００は、感圧センサの検出結果に基づき、インタフェース４１１およびバス４１２を介して、ワーク６００の有無を示す信号をＣＰＵ４０１に出力する。ＣＰＵ４０１は、信号入力装置７００からの信号により、載置台５００にワーク６００が存在するか否かを判定でき、その判定に基づきロボットアーム本体２００およびエンドエフェクタ本体３００を制御できる。

【0022】

ここで、ロボットアーム本体２００は、ワーク６００を別のワークの場所へ運搬するために、エンドエフェクタ本体３００を載置台５００上の所定の位置まで移動する動作を制御装置４００の命令に基づき実行中であるものとする。この動作の最中に、信号入力装置７００により制御装置４００はワーク６００が所定の位置に到達していないことを検知した。このままでは、ワーク６００を把持することなく動作を継続してしまい、作業効率が悪くなるため、制御装置４００は、ロボットアーム本体２００を可能な限り早く停止するような各軸目標値を算出し、ユーザにメンテナンスを実行させる必要がある。以下では本実施形態におけるロボットアーム本体２００の停止処理について詳述する。

【0023】

図３は本実施形態におけるロボットアーム本体２００を停止させるための軸目標値（制御値）を取得する方法の処理を示す制御フローチャートである。図３におけるフローチャートはステップＳ１からステップＳ７で構成されている。以下、ステップＳ１からステップＳ７における各処理の内容について図を用いて詳細に説明する。また以下の処理は、制御装置４００のＣＰＵ４０１により実行されるものとする。

【0024】

まずステップＳ１では、通常動作を実行させるための各関節（各軸）における軸目標値（通常軸目標値）を取得する。実施形態では２つ以上の関節を有するロボット装置の制御であれば如何なる軸数であっても適用可能である。今回は説明の簡略化のため２つの関節の制御を例にとり詳述する。

【0025】

図４はステップＳ１で取得した通常動作における軸目標値（通常軸目標値）を表したグラフである。図４（ａ）は第１関節の通常軸目標値、図４（ｂ）は第２関節の通常動作における通常軸目標値を示している。図４（ａ）（ｂ）において、横軸はタイムステップｋ、縦軸は軸目標値［ｄｅｇ］を示している。ｉは第１関節または第２関節を示す値であり、図４（ａ）では第１関節、図４（ｂ）では第２関節の通常軸目標値を示している。タイムステップはロボットアーム本体２００の制御周期により設定される。

【0026】

図４（ａ）より、第１関節のタイムステップｋの範囲が０≦ｋ≦５における通常軸目標値ｑｄ１（ｋ）は、ｑｄ１（ｋ）＝［０，０．５５０，０．９５０，１．２００，１．２７５，１．３５０］である。図４（ｂ）より、第２関節のタイムステップｋの範囲が０≦ｋ≦５における通常軸目標値ｑｄ２（ｋ）は、ｑｄ２（ｋ）＝［０，０．１００，０．２７０，０．５３０，０．９００，１．２００］である。今回は、タイムステップｋ＝３で停止指令を受け取り、ｋ＝３より時間的に後における通常動作から停止動作に切り替えるため、ｋ＝３より時間的に後における通常軸目標値を停止のために変更する。

【0027】

次にステップＳ２では停止命令に基づき、各関節について物理的制約の制限を守った上で減速した場合の軸目標値（制限軸目標値）を取得する。物理的制約として具体的には加速度・ジャーク（躍度）・トルク等の制限値（上限値）が考えられる。今回は加速度を制限する場合を例にとって説明する。以下の説明ではタイムステップｋにおける軸ｉにおける停止動作のための軸目標値（停止軸目標値）をｑｃｉ（ｋ）［ｄｅｇ］で表すものとする。速度にタイムステップ幅を乗算した値をｖｃｉ（ｋ）［ｄｅｇ］とする。そして、軸ｉにおける加速度の制限値にタイムステップ幅の２乗を乗算した値がａｉ′［ｄｅｇ］であるとき、加速度制約の上限で減速した場合の軸目標値（制限軸目標値）ｑａｉ（ｋ）［ｄｅｇ］は式（１）により算出できる。

【0028】

【数1】

【0029】

本実施形態において、各関節の加速度の制限値は式（２）の値となる。

【0030】

【数2】

【0031】

この時、タイムステップｋ＝４において第１関節の制限軸目標値は、式（３）の値となる。

【0032】

【数3】

【0033】

同様に第２関節の制限軸目標値は、式（４）の値となる。

【0034】

【数4】

【0035】

尚、本実施形態においては加速度制約を考慮することとしているが、トルク制約やジャーク制約を同時に考慮することも可能である。その場合は各物理的制約に基づいて算出した制限軸目標値のうち、現在の関節の値（角度）から最も遠い（離れた）制限軸目標値を本ステップＳ２で採用すればよい。

【0036】

次にステップＳ３では、ステップＳ２で算出した制限軸目標値を、時間軸に射影した媒介変数Ｓａｉ（ｋ）を取得する。図５は制限軸目標値とそれを時間軸に射影した媒介変数を示した図である。図５（ａ）では第１関節、図５（ｂ）では第２関節の軸目標値を示している。

【0037】

図５、図６に示すように媒介変数Ｓａｉ（ｋ）は通常動作における通常軸目標値を線形補間して求めればよく、Ｓａ１（４）とＳａ２（４）はそれぞれ式（５）、式（６）の値となる。

【0038】

【数5】

【0039】

【数6】

【0040】

ここで、式（５）と式（６）で媒介変数を取得るためのタイムステップが異なるのは、下記理由に基づくものである。第１関節の制限軸目標値ｑａ１（４）が通常軌道の通常軸目標値ｑｄ１（４）を上回るのに対し、第２関節の制限軸目標値ｑａ２（４）が通常軌道の通常軸目標値ｑｄ２（４）を下回る。よって、タイムステップ４からタイムステップ５間の直線補間では媒介変数Ｓａ２（４）を取得できないため、タイムステップ３からタイムステップ４間を使用する。

【0041】

次にステップＳ４では、ステップＳ３で求めたそれぞれの関節における媒介変数から各関節の停止軸目標値の候補を取得する。図６は、各媒介変数を、第１関節における通常軌道のグラフ、第２関節における通常軌道のグラフに示した図である。図６の上段グラフが第１関節における通常軌道のグラフ、図６の下段グラフが第２関節における通常軌道のグラフである。

【0042】

まず、第１関節における制限軸目標値より取得された媒介変数Ｓａ１（４）から各関節における停止軸目標値を取得する。第１関節における停止軸目標値は、ステップ２で求めた制限目標値と等しく、式（７）で示す値となる。

【0043】

【数7】

【0044】

一方、第２関節における停止軸目標値は、通常動作における通常軸目標値から線形補間して求めればよく、式（８）で示す値となる。

【0045】

【数8】

【0046】

つづいて、第２関節における制限軸目標値より取得された媒介変数Ｓａ２（４）から各関節における停止軸目標値を取得する。第１関節における停止軸目標値は、通常動作における通常軸目標値から線形補間して求めればよく、式（９）で示す値となる。

【0047】

【数9】

【0048】

一方、第２関節における停止軸目標値は、ステップＳ２で求めた制限軸目標値と等しく、式１０で示す値となる。

【0049】

【数10】

【0050】

次にステップＳ５では、ステップＳ４で求めた停止軸目標値の候補において、全軸（全関節）の物理的制約を満たすような停止軸目標値が存在するか判定する。

【0051】

まず、媒介変数Ｓａ１（４）から取得された停止軸目標値を採用した場合、各軸の加速度の絶対値が物理的制約を超過するかどうか判定する。第１関節における停止軸目標値は式（７）で示す値であり、式（２）より第１関節および第２関節の加速度制約値から取得した値なので、第１関節における加速度は制約を超過しない。

【0052】

一方、媒介変数Ｓａ１（４）から取得された停止軸目標値を採用した場合の第２関節における加速度の値は、式（１１）で示す値となる。

【0053】

【数11】

【0054】

式（２）式（１１）より、媒介変数Ｓａ１（４）から取得された停止軸目標値を採用した場合の第２関節における加速度の値は、第２関節における加速度制約値ａ２´＝０．１５０を上回る。よって、媒介変数ｓａ１（４）から算出された停止軸目標値は全軸の物理的制約を満たす停止軸目標値ではない。

【0055】

つづいて、媒介変数Ｓａ２（４）から取得された停止軸目標値を採用した場合、各軸の加速度の絶対値が物理的制約を超過するかどうか判定する。第２関節における停止軸目標値は式（８）で示す値あり、式（４）より第２関節の加速度制約値から取得した値なので、第２関節における加速度は制約を超過しない。

【0056】

一方、媒介変数Ｓａ２（４）から取得された停止軸目標値を採用した場合の第１関節における加速度の値は、式（１２）で示す値となる。

【0057】

【数12】

【0058】

式（２）式（１２）より、媒介変数Ｓａ２（４）から取得された停止軸目標値を採用した場合の第１関節における加速度の値は、第１関節における加速度制約値ａ１´＝０．１５０を上回る。よって、媒介変数Ｓａ２（４）から取得された停止軸目標値は全軸の物理的制約を満たすような停止軸目標値ではない。

【0059】

ここで、ステップＳ４にて取得した停止軸目標値の候補のうち、全軸の物理的制約を満たすような停止軸目標値が存在していた場合には、ステップＳ５：Ｎｏとなり、ステップＳ７へ進む。ステップＳ４にて取得した停止軸目標値の候補のうち、全軸の物理的制約を満たす停止軸目標値が存在する場合は、ステップＳ５：Ｙｅｓとなり、ステップＳ６に進み、全軸の物理的制約を満たす停止軸目標値を用いてロボットアーム本体２００を停止させる。

【0060】

ここで、今回のように、全軸の物理的制約を満たすような停止軸目標値が存在しない場合、停止を実行することができなくなってしまう。そこで、このような場合にも可能な限り物理的制約を満たして停止させるために、ステップＳ７にて、予め用意した評価方法（所定条件）に基づき、ステップＳ４で取得した停止軸目標値を評価する。そして評価値の高い停止軸目標値を、ロボットアーム本体２００を停止するための停止軸目標値として使用することが大きな特徴である。

【0061】

本実施形態におけるステップＳ７での停止軸目標値の決定方法は、停止指令を受けた際のタイムステップ（本実施形態ではｋ＝３）における各関節の速度に基づき、速度が大きい関節における物理的制約を優先して停止軸目標値を決定する。停止指令を受けた際のタイムステップにおける速度の大きい関節の制約によって取得された媒介変数Ｓａｉ（ｋ）によって取得された停止軸目標値を、ロボットアーム本体２００を停止させるための停止軸目標値として用いる。

【0062】

停止指令を受けた際のタイムステップにおける各関節の速度はそれぞれ式（１３）、式（１４）で示した値となる。

【0063】

【数13】

【0064】

【数14】

【0065】

式（１３）式（１４）より、第２関節の速度が最も大きいため、第２関節の物理的制約によって取得された媒介変数Ｓａ２（４）によって取得された停止軸目標値候補を用いる。第１関節における媒介変数Ｓａ２（４）によって取得された停止軸目標値は式（９）よりｑｃ１（４）＝１．２２２であり、第２関節における停止軸目標値は式（４）よりｑｃ２（４）＝０．６４０である。この際、第１関節における加速度の絶対値は式（１２）より｜ａｃ１（４）｜＝０．２２８であり、第２関節における加速度の絶対値は式（２）より｜ａｃ２（４）｜＝ａ２´＝０．１５００である。以上により、最も大きい速度で動作している軸（今回は第２関節）に関しては物理的制約を守りながら、停止軸目標値を決定できる。なお、本実施形態では第１関節の物理的制約および第２関節の物理的制約が同じ場合を例にとり説明したが、異なる場合に実施してもかまわない。

【0066】

以上の操作を繰り返すことで、最も大きい速度で動作している軸に関しては可能な限り物理的制約を守りながら、ロボットアーム本体２００を停止させることができる。これにより、速度が大きく伝達機構に負荷が大きくかかる関節における物理的制約を優先的に順守する。よって全ての関節（軸）の制約を満たすことができる停止軸目標値が取得できない場合でも、ハードウェア（ロボットアーム本体２００）の破損の危険性を可能な限り低減できるよう、可能な限り物理的制約を満たした停止を実行させることができる。

【0067】

なお上述した実施形態では、停止指令を受けた際のタイムステップにおける各関節の速度に基づき、優先すべき関節の物理的制約を決定したがこれに限られない。例えば、停止指令を受ける前（ｋ＝３のタイミング以前）までのタイムステップにおいて、最も大きい速度に達した関節の物理的制約を優先しても構わない。また、速度の所定閾値をあらかじめ設定しておき、停止指令を受ける前までのタイムステップにおいて、閾値に達した回数が最も大きい関節における物理的制約を優先するようにしてもかまわない。

【0068】

（第２の実施形態）
次に本発明における第２の実施形態につき詳述する。本実施形態では、上述の第１の実施形態に対してステップＳ７における停止軸目標値の決定方法が異なる。本実施形態では、ステップＳ４における停止軸目標値のうち、停止を行うタイムステップ（停止指令を受けたｋ＝３より１タイムステップ後のｋ＝４）における物理的制約に対する超過量が最も少ない関節における物理的制約を用いて停止軸目標値を決定する。なお、以下では必要に応じ、第１の実施形態とは異なるハードウェアや制御系の構成の部分について図示し説明する。また、第１の実施形態と同様の部分については上記と同様の構成ならびに作用が可能であるものとし、その詳細な説明は省略するものとする。

【0069】

式（９）より媒介変数Ｓａ１（４）から算出された停止軸目標値は第２関節の加速度制約値を０．２１０／０．１５０＝１．４０倍超過する。一方、式（１２）より媒介変数Ｓａ２（４）から算出された停止軸目標値は第２関節の加速度制約値を０．２２８／０．１５０＝１．５２倍超過する。よって、実際に停止を行う際には、第１関節における媒介変数Ｓａ１（４）を用いたほうが、物理的制約の超過を抑えることができる。

【0070】

以上により、全ての関節（軸）の制約を満たすことができる停止軸目標値が取得できない場合でも、ハードウェア（ロボットアーム本体２００）の破損の危険性を可能な限り低減できるよう、可能な限り物理的制約を満たした停止を実行させることができる。

【0071】

（第３の実施形態）
次に本発明における第３の実施形態につき詳述する。本実施形態は、上述の第１の実施形態及び第２の実施形態に対してステップＳ７における停止軸目標値の決定方法が異なる。以下では必要に応じ、上述の実施形態とは異なるハードウェアや制御系の構成の部分について図示し説明する。また、第１の実施形態と同様の部分については上記と同様の構成ならびに作用が可能であるものとし、その詳細な説明は省略するものとする。

【0072】

本実施形態では、ステップＳ４で算出された停止軸目標値を使用せず、式（３）式（４）で求めた制限軸目標値を採用する。これにより全ての関節（軸）の制約を満たすことができる停止軸目標値が算出できない場合でも、ハードウェア（ロボットアーム本体２００）の破損の危険性を低減できる停止を実行させることができる。なお、全関節の物理的制約を満たして停止軸目標を取得させる場合と、いずれかの関節の物理的制約を満たす停止軸目標値を取得させる場合とを切り換えてもかまわない。

【0073】

（第４の実施形態）
次に本発明における第４の実施形態につき詳述する。本実施形態は、上述の第１の実施形態及び第２の実施形態に対してステップＳ７における停止軸目標値の決定方法が異なる。以下では必要に応じ、上述の実施形態とは異なるハードウェアや制御系の構成の部分について図示し説明する。また、第１の実施形態と同様の部分については上記と同様の構成ならびに作用が可能であるものとし、その詳細な説明は省略するものとする。

【0074】

本実施形態では、取得した停止指令において、その停止指令が、作業者に関連する場合を例にとり説明する。作業者に関連する停止の場合、作業者の安全を確保するために即座にロボットアーム本体２００を停止させる必要がある。そこで制御装置４００は、取得する停止軸目標値において、物理的制約を満たす、満たさないに関わらず、現在位置で停止するようにロボットアーム本体２００を停止させる。

【0075】

図７は本実施形態におけるロボットシステム１０００を示す。本実施形態におけるロボットアーム本体２００の基台２１０には、作業者を検知するための検知センサ２６０を備えている。検知センサ２６０は光照射型のセンサであり、照射した光が作業者によって反射された際、その反射光を取得し、作業者の有無を検知する。そして、作業者を検知すれば、制御装置４００に、作業者接近による停止指令として信号を送信する。なお、本実施形態では、光学式のセンサを用いたが、これに限られず、磁気式や撮像装置等、作業者を検知できるセンサを用いて構わない。

【0076】

図８は本実施形態における制御フローチャートを示した図である。上述の実施形態と比べ、本実施形態では、ステップＳ２１、ステップＳ２２が存在している点で異なる。また以下の処理は、制御装置４００のＣＰＵ４０１により実行されるものとし、異なるステップであるステップＳ２１、ステップＳ２２、ステップＳ２３について詳述する。

【0077】

図８より、本実施形態では、ステップＳ１に移る前に、ステップＳ２１により、停止指令が、作業者接近によるものか否かを判定するステップを設けている。ステップＳ２１：Ｙｅｓすなわち、作業者接近による停止指令あるならば、ステップＳ２２に進む。作業者接近によるものでなければ、ステップＳ２１：Ｎｏより、ステップＳ１に進み、上述した種々の実施形態に基づく停止処理を行う。

【0078】

ステップＳ２２では、作業者の安全を確保すべく即座にロボットアーム本体２００の停止を行うため、ロボットアーム本体２００の各物理的制約を満たずように停止する処理から、物理的制約にかかわらず即時停止する処理に切り換える。

【0079】

そして、ステップＳ２３では、物理的制約にかかわらず、即時に停止させるために、ロボットアーム本体２００の各関節の現在位置を停止軸目標値として使用し、ロボットアーム本体２００を停止させる。

【0080】

以上により、停止指令を取得して、即時にロボットアーム本体２００を停止させることができるため、作業者の安全を確実に確保することができる。また、即時にロボットアーム本体２００を停止させるので、ロボットアーム本体２００と作業者との接触により、例えば作業者が身に着けている安全具等によりロボットアーム本体２００のハードウェアが破損することを可能な限り低減することができる。

【0081】

（第５の実施形態）
次に本発明における第５の実施形態につき詳述する。上述の種々の実施形態は実機におけるロボットアーム本体２００の停止動作について記述した。しかしながら本発明は、ロボットアームのシミュレーションを行う情報処理装置（シミュレーション装置）においても実施可能である。以下で詳述する。

【0082】

図９に示すのは本実施形態におけるロボットアーム本体２００の動作のシミュレーションを行う情報処理装置９００の概略図である。情報処理装置９００は、ＯＳ（Ｏｐｅｒａｔｉｎｇ Ｓｙｓｔｅｍ）９０１、ディスプレイ９０２、キーボード９０３、マウス９０４を備えたデスクトップパソコンである。

【0083】

ＯＳ９０１にはロボットアーム本体２００及びロボットハンド本体３００のＣＡＤデータと、実機の制御装置４００と同等の軌道生成エンジンが格納されている。表示部となるディスプレイ９０２にロボットアーム本体２００のシミュレーション画面９０５が表示されている。また、通常動作実行ボタン９０６、停止ボタン９０７、停止ボタン９０８、停止ボタン９０９、停止ボタン９１０が表示されている。

【0084】

ここで、停止ボタン９０７は、上述の第１の実施形態における停止処理のシミュレートを実行するボタンである。停止ボタン９０８は、上述の第２の実施形態における停止処理のシミュレートを実行するボタンである。停止ボタン９０９は、上述の第３の実施形態のシミュレートを実行するボタンである。停止ボタン９１０は、上述の第４の実施形態のシミュレートを実行するボタンである。

【0085】

作業者は、キーボード９０３およびまたはマウス９０４を用いて所定の情報（通常軌道の軌道データ等）を入力し、通常動作実行ボタン９０６をクリックすることで、ロボットアーム本体２００の動作シミュレーションを実行することができる。通常動作を再生中に停止ボタン９０７、停止ボタン９０８、停止ボタン９０９、停止ボタン９１０のいずれかをクリックする。クリックすると、クリックしたタイミングで停止指令が出力されたものとして、そこから上述の実施形態を適用して停止軸目標値を取得する停止処理を実行する。これにより様々な条件によるロボットアーム本体２００のハードウェアの破損の危険性を低減する複数種類の停止動作をシミュレーションにより確認することができる。

【0086】

（その他の実施形態）
以上述べた上述の実施形態の処理手順は具体的には制御装置または情報処理装置により実行されるものである。従って上述した機能を実行可能なソフトウェアのプログラムを記録した記録媒体を、各制御装置を統合する装置に供給し、統合的に処理を行うＣＰＵが記録媒体に格納されたプログラムを読み出し実行することによって達成されるよう構成することができる。この場合、記録媒体から読み出されたプログラム自体が上述した各実施形態の機能を実現することになり、プログラム自体およびそのプログラムを記録した記録媒体は本発明を構成することになる。

【0087】

また、各実施形態では、コンピュータで読み取り可能な記録媒体が各ＲＯＭ或いは各ＲＡＭ或いは各フラッシュＲＯＭであり、ＲＯＭ或いはＲＡＭ或いはフラッシュＲＯＭに制御プログラムが格納される場合について説明した。しかしながら本発明はこのような形態に限定されるものではない。本発明を実施するための制御プログラムは、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、制御プログラムを供給するための記録媒体としては、ＨＤＤ、外部記憶装置、記録ディスク等を用いてもよい。

【0088】

また上述した種々の実施形態では、ロボットアーム本体２００が複数の関節を有する多関節ロボットアームを用いた場合を説明したが、関節の数はこれに限定されるものではない。ロボットアームの形式として、垂直多軸構成を示したが、パラレルリンク型など異なる形式の関節においても上記と同等の構成を実施することができる。

【0089】

また、上述の実施形態では、停止指令を受けた際のタイムステップにおける各関節の速度、加速度に基づき、優先すべき関節の物理的制約を決定したが、ジャーク、トルク、荷重等、状況に応じ、あらゆる物理量を用いても実施可能である。

【0090】

また、上述の実施形態では、関節の速度を直接センサにより取得することで実施したがこれに限られない。例えばロボットハンド本体３００にセンサを設け、ロボットハンド本体３００に生じる物理量に基づき、各関節に生じる物理量を取得して実施してもかまわない。

【0091】

また上述した種々の実施形態は、制御装置に設けられる記憶装置の情報に基づき、伸縮、屈伸、上下移動、左右移動もしくは旋回の動作またはこれらの複合動作を自動的に行うことができる機械に適用可能である。

【0092】

なお本発明は、上述した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されない。

【符号の説明】

【0093】

２００ ロボットアーム本体
２０１、２０２、２０３、２０４、２０５ リンク
２１０ 基台
３００ ロボットハンド本体
３０１、３０２ 指
４００ 制御装置
５００ 載置台
６００ ワーク
７００ 信号入力装置
８００ 外部入力装置
９００ 情報処理装置
９０１ ＯＳ
９０２ ディスプレイ
９０３ キーボード
９０４ マウス
９０５ シミュレーション画面
９０６ 通常動作実行ボタン
９０７、９０８、９０９、９１０ 動作停止ボタン
１０００ ロボットシステム
Ｊ１、Ｊ２、Ｊ３、Ｊ４、Ｊ５ 関節

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】