特開 2022-174858 直接教示装置及び直接教示方法

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

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

(11)【公開番号】P2022174858

(43)【公開日】2022-11-25

(54)【発明の名称】直接教示装置及び直接教示方法

(51)【国際特許分類】

   B25J 9/22 20060101AFI20221117BHJP

【ＦＩ】

B25J9/22 Z

【審査請求】未請求

【請求項の数】4

【出願形態】ＯＬ

(21)【出願番号】P 2021080865

(22)【出願日】2021-05-12

(71)【出願人】

【識別番号】000006666

【氏名又は名称】アズビル株式会社

(74)【代理人】

【識別番号】110003166

【氏名又は名称】弁理士法人山王内外特許事務所

(72)【発明者】

【氏名】大川 陽一

(72)【発明者】

【氏名】田原 鉄也

(72)【発明者】

【氏名】杉本 広大

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

3C707

【Ｆターム（参考）】

3C707KS16

3C707KS20

3C707KS29

3C707KS33

3C707KS35

3C707KV01

3C707KW05

3C707KX10

3C707LS02

3C707LS03

3C707MT01

3C707MT04

(57)【要約】      （修正有）

【課題】直接教示時において、全体の性能向上を図る動作を実現する。
【解決手段】アーム２に加えられたトルクを検知するトルク検知部４と、アームの位置姿勢を算出する位置姿勢演算部１１２と、トルクに基づいてアームに加えられた外力を検知する外力検知部１１１と、トルクに従うアームの動きを算出して駆動する第１従動制御演算部１３と、外力に従うアームの動きを算出する第２従動制御演算部１１４と、アームの位置姿勢又は速度のうちの少なくとも一つに対する制約を満たす当該アームの動きを算出する制御部と、第２従動制御演算部による算出結果と制御部による算出結果とを合成する合成部１１７と、合成部による合成結果に基づいてアームを駆動する第２駆動制御部１２２と、第１従動制御演算部１３、或いは、第２従動制御演算部、制御部、合成部及び第２駆動制御部のうちの一方を有効に切換える切換え部１１９とを備えた。
【選択図】図１

【特許請求の範囲】

【請求項1】

ロボットが有するアームに加えられたトルクを検知するトルク検知部と、
前記アームの位置又は姿勢のうちの少なくとも一方である位置姿勢を算出する位置姿勢演算部と、
前記トルク検知部により検知されたトルクに基づいて前記アームに加えられた外力を検知する外力検知部と、
前記トルク検知部により検知されたトルクに従う前記アームの動きを算出して当該アームを駆動する第１従動制御演算部と、
前記外力検知部により検知された外力に従う前記アームの動きを算出する第２従動制御演算部と、
前記位置姿勢演算部による算出結果に基づいて前記アームの位置姿勢又は速度のうちの少なくとも一つに対する制約を満たす当該アームの動きを算出する制御部と、
前記第２従動制御演算部による算出結果と前記制御部による算出結果とを合成する合成部と、
前記合成部による合成結果に基づいて前記アームを駆動する駆動制御部と、
前記第１従動制御演算部、或いは、前記第２従動制御演算部、前記制御部、前記合成部及び前記駆動制御部のうちの一方を有効に切換える切換え部と
を備えた直接教示装置。

【請求項2】

前記制御部は、
前記アームの位置姿勢に対する制約目標を取得する位置姿勢目標取得部と、
前記位置姿勢演算部により算出された位置姿勢が前記位置姿勢目標取得部により取得された制約目標に従う前記アームの動きを算出する位置姿勢制御演算部とを有する
ことを特徴とする請求項１記載の直接教示装置。

【請求項3】

前記位置姿勢演算部により算出された位置姿勢に基づいて前記アームの速度を算出する速度演算部を備え、
前記制御部は、
前記アームの速度に対する制約目標を取得する速度目標取得部と、
前記速度演算部により算出された速度が前記速度目標取得部により取得された制約目標に従う前記アームの動きを算出する速度制御演算部とを有する
ことを特徴とする請求項１記載の直接教示装置。

【請求項4】

トルク検知部が、ロボットが有するアームに加えられたトルクを検知するステップと、
位置姿勢演算部が、前記アームの位置又は姿勢のうちの少なくとも一方である位置姿勢を算出するステップと、
外力検知部が、前記トルク検知部により検知されたトルクに基づいて前記アームに加えられた外力を検知するステップと、
第１従動制御演算部が、前記トルク検知部により検知されたトルクに従う前記アームの動きを算出して当該アームを駆動するステップと、
第２従動制御演算部が、前記外力検知部により検知された外力に従う前記アームの動きを算出するステップと、
制御部が、前記位置姿勢演算部による算出結果に基づいて前記アームの位置姿勢又は速度のうちの少なくとも一つに対する制約を満たす当該アームの動きを算出するステップと、
合成部が、前記第２従動制御演算部による算出結果と前記制御部による算出結果とを合成するステップと、
駆動制御部が、前記合成部による合成結果に基づいて前記アームを駆動するステップと、
切換え部が、前記第１従動制御演算部、或いは、前記第２従動制御演算部、前記制御部、前記合成部及び前記駆動制御部のうちの一方を有効に切換えるステップと
を有する直接教示方法。

【発明の詳細な説明】

【技術分野】

【0001】

この発明は、ロボットの直接教示を行う直接教示装置及び直接教示方法に関する。

【背景技術】

【0002】

産業用のロボットでは、ロボットに作業をさせるために、前もって教示（ティーチング）と呼ばれる作業が実施される。このロボットの教示を行う方法の１つとして、直接教示（ダイレクト教示）と呼ばれる方法がある。

【0003】

例えば特許文献１では、力センサを用いたロボットの直接教示方法が開示されている。また、例えば特許文献２及び非特許文献１では、トルク検出手段を用いたロボットの直接教示方法が開示されている。非特許文献１で開示されている直接教示装置の概略構成を以下に示す。

【0004】

図１１に示す直接教示装置では、まず、メインコントローラ２０１が、トルクセンサ２０２により計測されたトルク（ロボットが有する関節単位でのトルク）を用いて、ロボットが有するアーム２０３に対して操作者により加えられた外力を検知する。
次いで、メインコントローラ２０１は、検知した外力に従うアーム２０３の動きを実現するために、トルク指令値及びトルク補償値を算出する。トルク指令値は、トルクセンサ２０２により計測されたトルクのうちの重力に起因する成分の推定値とトルクセンサ２０２のオフセット値といった静的な外乱との和である。トルク補償値は、ロボットの重力による移動を防止するために必要なトルクである。なお、メインコントローラ２０１は、従動制御演算において、エンコーダ２０４により検出された角度（アーム２０３の関節角）に基づいてトルク指令値（従動制御指令値）を算出する。

【0005】

次いで、サーボドライバ２０５は、トルクセンサ２０２により計測されたトルクが、従動制御指令値と一致するようにトルク制御する。すなわち、サーボドライバ２０５は、トルク指令値からトルクセンサ２０２により計測されたトルクを差し引いてゲインを乗算した上で、トルク補償値を加算する。そして、サーボドライバ２０５は、トルク制御の結果に基づいて更新した駆動電流指令値を算出する。すなわち、サーボドライバ２０５は、トルク制御の結果に対してトルク電流変換係数を乗算する。そして、サーボドライバ２０５は、駆動電流指令値が駆動電流計測値（不図示）と一致するように、電流制御（不図示）を行い、駆動電流を出力する。
次いで、サーボモータ２０６は、サーボドライバ２０５により出力された駆動電流に従って駆動する。そして、サーボモータ２０６で発生した駆動力は減速機２０７を介してアーム２０３に伝達され、アーム２０３は駆動する。

【0006】

この一連の動作により、図１１に示す直接教示装置は、操作者により加えられた外力に従ってアーム２０３が動くように制御できる。そして、直接教示装置は、制御によりアーム２０３の位置及び姿勢が操作者の意図する状態となった場合に、その際のパラメータを教示点として記録する。この直接教示装置により記録された教示点はロボットが作業する際に使用される。

【0007】

以上のように、関節単位の駆動制御部に近い場所に配設可能なトルクセンサの値を制御することで従動制御を実現し、駆動制御部で制御の主要な演算を実行するようにすれば、むだ時間が入り込む余地を削減して速応性を高められる。すなわち、安定性を維持できるコントローラのハイゲイン化に相当する調整（関節単位の１変数制御のゲイン調整）も可能になる。そのため、軽い操作感が得られる（例えば特許文献３参照）。

【0008】

このように、ロボットの直接教示は、操作者がアームを直接操作して位置及び姿勢を教示するため、操作者にとって直観的でわかりやすいという利点を持つ。一方で、アームが操作者により加えられた外力に従ってそのまま動くという特徴は利点ばかりではない。

【0009】

例えば、操作者が、ロボットに対して、ある点Ｐの位置を教示した後に、その真下の別の点Ｑの位置を教示するという場合を考える。この場合では、操作者は、真下方向（Ｚ軸方向）にアームを動かすことになる。しかしながら、操作者がアームに対して正確に真下方向へ外力を加えることは難しく、アームのＸ軸座標及びＹ軸座標がずれることが多い。後からアームのＸ軸座標及びＹ軸座標だけをティーチングペンダント等を用いて修正することは可能であるが、それでは直接教示の利点が減ずる。

【0010】

また、例えば、操作者が、アームの先端に設けられたエンドエフェクタを真下に向けたまま教示を行う場合を考える。この場合においても、操作者がアームに対して正確に真下に向けた状態を維持したままアームを直接操作することは難しく、アームの姿勢がずれることが多い。

【0011】

以上のように、直接教示は、直観的で分かりやすい反面、操作が難しい場合も存在する。

【0012】

これに対し、例えば特許文献４，５では、上記のような課題を解決する技術が開示されている。また、特許文献５では、入力装置により、動作モードを、拘束モード又は全方向移動モードに切換え可能とする直接教示装置が開示されている。拘束モードでは、エンドエフェクタの先端を特定の拘束軸又は拘束面に沿って移動可能としている。また、全方向移動モードでは、通常の直接教示を実施可能としている。この直接教示装置は、直接教示の途中で拘束モードに切換えることにより、アームの先端を正確に上下方向に動かすことが可能となり、直接教示における操作の難しさが軽減される。更に特許文献６では、操作者がアームを操作している状態のまま通常の直接教示と拘束付き直接教示を切換え可能な直接教示装置が開示されている。

【先行技術文献】

【特許文献】

【0013】

【特許文献1】特開平０５－２０４４４１号公報

【特許文献2】特開平０５－２５００２９号公報

【特許文献3】特開２０２０－１４６７６５号公報

【特許文献4】特開平０５－２８５８７０号公報

【特許文献5】特開平０５－３０３４２５号公報

【特許文献6】特開２０１９－２０２３８３号公報

【非特許文献】

【0014】

【非特許文献1】Kodai Sugimoto, Koji Shimizu, Tetsuya Tabaru; Lead-Through Programming of Robotic Manipulators by Joint Torque Control, Proc. of the SICE Annual Conference 2020, pp. 462-466, 2020)

【発明の概要】

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

【0015】

非特許文献１で示した各関節単位でのトルク制御（各軸トルク制御）による直接教示動作は、特許文献３に示されているように操作感を軽くできる一方で、位置姿勢の拘束といった制約付き直接教示動作における制約性能を上げるのが難しいという課題がある。

【0016】

これは、拘束制御では位置姿勢を扱う必要があるが、関節制御部がトルク制御だと位置姿勢を直接制御できないため、制御が複雑になるからである。例えばＺ軸方向に拘束して直接教示動作を行う場合、上位のメイン制御部において拘束位置からの位置偏差を抑制するような制御指令値を算出するが、関節制御部がトルク制御なので、制御指令値はトルクで与えなければならない。このため、トルクの指令値を介して間接的に位置を制御するという複雑な制御になる。

【0017】

また、関節制御部が位置姿勢を制御できないためメイン制御部によるフィードバック制御を主体とした制御になるが、一般的にメイン制御部と関節制御部の間には通信遅延があるため、安定な動作を実現するためにはメイン制御部の制御ゲインを上げづらい。以上のような理由により、各軸トルク制御の方式では、拘束位置からの制御偏差を十分に小さくすることが難しいという課題がある。

【0018】

なお、同様な課題は拘束付きの直接教示以外でも、位置又は速度の制御を同時に行う機能を持つ直接教示で起こりうる。例えば操作者の安全のために、アームの手先の速度又は各関節の角速度を所定の値以下に制御したい場合、各軸トルク制御の方式ではトルクを介して速度を制御することが必要であり、同様の課題が生じる。

【0019】

関節制御部が位置制御又は速度制御であれば、上述の課題は無い。この場合は関節制御部に位置（関節角）又は速度（角速度）の指令値を与えることができるので、トルクの指令値を計算しなくても位置又は速度が機能上の要求を満たすように直接制御できるからである。しかしながら、各軸トルク制御の利点である軽い操作感は失われることになる。

【0020】

この発明は、上記のような課題を解決するためになされたもので、従来に対し、制約を課さない通常の直接教示を行う場合には軽い操作感で動かすことができ、制約を課した直接教示を行う場合には全体の性能向上が図られた直接教示動作を実現できる直接教示装置を提供することを目的としている。

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

【0021】

この発明に係る直接教示装置は、ロボットが有するアームに加えられたトルクを検知するトルク検知部と、アームの位置又は姿勢のうちの少なくとも一方である位置姿勢を算出する位置姿勢演算部と、トルク検知部により検知されたトルクに基づいてアームに加えられた外力を検知する外力検知部と、トルク検知部により検知されたトルクに従うアームの動きを算出して当該アームを駆動する第１従動制御演算部と、外力検知部により検知された外力に従うアームの動きを算出する第２従動制御演算部と、位置姿勢演算部による算出結果に基づいてアームの位置姿勢又は速度のうちの少なくとも一つに対する制約を満たす当該アームの動きを算出する制御部と、第２従動制御演算部による算出結果と制御部による算出結果とを合成する合成部と、合成部による合成結果に基づいてアームを駆動する第２駆動制御部と、第１従動制御演算部、或いは、第２従動制御演算部、制御部、合成部及び第２駆動制御部のうちの一方を有効に切換える切換え部とを備えたことを特徴とする。

【発明の効果】

【0022】

この発明によれば、上記のように構成したので、従来に対し、制約を課さない通常の直接教示を行う場合には軽い操作感で動かすことができ、制約を課した直接教示を行う場合には全体の性能向上が図られた直接教示動作を実現できる。

【図面の簡単な説明】

【0023】

【図1】実施の形態１に係る直接教示装置の構成例を示す図である。

【図2】実施の形態１に係る直接教示装置の動作例（拘束なし直接教示の場合）を示すフローチャートである。

【図3】実施の形態１に係る直接教示装置の動作例（拘束あり直接教示の場合）を示すフローチャートである。

【図4】実施の形態２に係る直接教示装置の構成例を示す図である。

【図5】拘束軸を説明する図である。

【図6】拘束面を説明する図である。

【図7】拘束方向を説明する図である。

【図8】図８Ａ、図８Ｂは、拘束回転軸を説明する図である。

【図9】曲面拘束と方向拘束を組み合わせた拘束を説明する図である。

【図10】実施の形態３に係る直接教示装置の構成例を示す図である。

【図11】従来の直接教示装置の構成例を示す図である。

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

【0024】

以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態１．
図１はこの発明の実施の形態１に係る直接教示装置（ロボット制御装置）１の構成例を示す図である。
直接教示装置１は、ロボットの直接教示を行う。ロボットは、関節毎に、アーム２を駆動するモータ（不図示）、及び、エンコーダ３を有している。モータ（図１ではアーム２）及びエンコーダ３は、それぞれ、直接教示装置１に対して電力線等により接続されている。エンコーダ３は、対応する関節の角度を検出する。

【0025】

また、ロボットには、関節毎に、トルク検知部４が設けられている。トルク検知部４は、操作者によりアーム２に加えられたトルクを検知する。
例えば、トルク検知部４は、アーム２のモータ駆動軸に取付けられたトルクセンサを用い、このトルクセンサにより計測されたトルク（τ）を上記トルクとして検知してもよい。また、トルク検知部４は、トルクセンサを用いてトルクを直接計測するのではなく、アーム２が有するモータの電力又はアーム２の関節角（θ）の計測値から間接的にトルクを算出するトルクオブザーバを用いて上記トルクを検知してもよい。

【0026】

直接教示装置１は、直接教示方法として、アーム２の手先の位置姿勢及びアーム２の手先の速度に対して制約を課さない通常の直接教示と、アーム２の手先の位置姿勢又はアーム２の手先の速度を制約する制約付き直接教示とを切換え可能である。

【0027】

なお、アーム２の手先の位置姿勢とは、アーム２の手先の位置及びアーム２の手先の姿勢のうちの少なくとも一方を意味する。また、アーム２の手先の位置とは、アーム２の先端に設けられたエンドエフェクタの先端又は先端を基準とした任意の点の位置を意味する。また、アーム２の手先の姿勢とは、エンドエフェクタの向きを意味する。なお、アーム２の手先の位置姿勢を、アーム２の位置姿勢と記述する場合があるが、特に断りのない場合、同じ意味である。

【0028】

また、アーム２の手先の速度とは、エンドエフェクタの移動速度を意味する。なお、アーム２の手先の速度を、アーム２の速度と記述する場合があるが、特に断りのない場合、同じ意味である。

【0029】

制約付き直接教示の応用例には、次のような制約を課しながら直接教示する場合が挙げられる。制約の例としては、アーム２の手先が取りうる位置に制約を課す位置拘束、アーム２の手先が取りうる姿勢に制約を課す姿勢拘束、又は、アーム２の手先の速度が予め定めた所定の値を超過しないよう制限を課す速度制限といったものが挙げられる。

【0030】

この直接教示装置１は、図１に示すように、メイン制御部１１、及び複数の関節制御部１２を備えている。関節制御部１２は、ロボットが有する関節毎に設けられている。なお、メイン制御部１１と各関節制御部１２との間は通信線により接続されている。図１では、１つの関節制御部１２のみを示している。

【0031】

メイン制御部１１は、各関節制御部１２に指令値を出力することで、ロボット全体を制御する。メイン制御部１１は、図１に示すように、外力検知部１１１、位置姿勢演算部１１２、速度演算部１１３、第２従動制御演算部１１４、位置姿勢制御部１１５、速度制御部１１６、合成部１１７、トルク指令値演算部１１８及び切換え部１１９を備えている。
関節制御部１２は、第１駆動制御部１２１及び第２駆動制御部（駆動制御部）１２２を備えている。

【0032】

なお、直接教示装置１は、システムＬＳＩ（Ｌａｒｇｅ－Ｓｃａｌｅ Ｉｎｔｅｇｒａｔｉｏｎ）等の処理回路、又はメモリ等に記憶されたプログラムを実行するＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）等により実現される。

【0033】

外力検知部１１１は、トルク検知部４により検知されたトルクを用い、操作者によりアーム２に加えられた外力を検知する。
なお、トルク検知部４により検知されるトルクは、センサの構造等によっては、操作者によりアーム２に加えられた外力だけではなく、重力に起因する成分が重畳されている場合がある。そこで、このような場合には、外力検知部１１１は、重力に起因する成分を算出し、検知した外力から当該重力に起因する成分を差し引くことで、外力成分のみを算出する。これは重力補償と呼ばれる公知技術であり、例えば特許文献７等に開示されている。

【特許文献7】特開平０１－０６６７１５号公報

【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】

次いで、速度演算部１１３は、位置姿勢演算部１１２により算出されたアーム２の位置姿勢に基づいて、アーム２の速度を算出する（ステップＳＴ３０３）。

【0059】

次いで、第２従動制御演算部１１４は、外力検知部１１１により検知された外力に従うアーム２の動き（従動制御指令値）を算出する（ステップＳＴ３０４）。

【0060】

また、位置姿勢制御部１１５は、位置姿勢演算部１１２により算出されたアーム２の位置姿勢に対する制約を満たすアーム２の動きを算出する（ステップＳＴ３０５）。

【0061】

また、速度制御部１１６は、速度演算部１１３により算出されたアーム２の速度に対する制約を満たすアーム２の動きを算出する（ステップＳＴ３０６）。

【0062】

次いで、合成部１１７は、第２従動制御演算部１１４による算出結果と、位置姿勢制御部１１５による算出結果と、速度制御部１１６による算出結果とを合成することで、合成制御指令値を得る（ステップＳＴ３０７）。

【0063】

次いで、第２駆動制御部１２２は、合成部１１７により得られた合成制御指令値に従ってアーム２を駆動する（ステップＳＴ３０８）。

【0064】

このように、実施の形態１に係る直接教示装置１では、制約付き直接教示を行う場合、第２従動制御演算部１１４により算出された外力に従うアーム２の動き（従動制御指令値）と、位置姿勢制御部１１５により算出されたアーム２の位置姿勢に対する制約を満たすアーム２の動きと、速度制御部１１６により算出されたアーム２の速度に対する制約を満たすアーム２の動きとを合成する。

【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】

【図10】

【図11】