JPH08227320A - 直交座標系上で柔らかさが調節可能なサーボ系 - Google Patents

直交座標系上で柔らかさが調節可能なサーボ系

Info

Publication number
JPH08227320A
JPH08227320A JP7055189A JP5518995A JPH08227320A JP H08227320 A JPH08227320 A JP H08227320A JP 7055189 A JP7055189 A JP 7055189A JP 5518995 A JP5518995 A JP 5518995A JP H08227320 A JPH08227320 A JP H08227320A
Authority
JP
Japan
Prior art keywords
coordinate system
axis
servo
softness
robot
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
JP7055189A
Other languages
English (en)
Other versions
JP3681431B2 (ja
Inventor
Tetsuro Kato
哲朗 加藤
Yukinobu Tsuchida
行信 土田
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Fanuc Corp
Original Assignee
Fanuc Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Fanuc Corp filed Critical Fanuc Corp
Priority to JP05518995A priority Critical patent/JP3681431B2/ja
Priority to EP96902445A priority patent/EP0766161B1/en
Priority to PCT/JP1996/000329 priority patent/WO1996026473A1/ja
Priority to US08/732,409 priority patent/US5742138A/en
Publication of JPH08227320A publication Critical patent/JPH08227320A/ja
Application granted granted Critical
Publication of JP3681431B2 publication Critical patent/JP3681431B2/ja
Anticipated expiration legal-status Critical
Expired - Fee Related legal-status Critical Current

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • B25J9/1687—Assembly, peg and hole, palletising, straight line, weaving pattern movement
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40587—Measure force indirectly by using deviation in position

Abstract

(57)【要約】 【目的】 直交座標系上でソフトフロート機能をロボッ
トに調節すること。 【構成】 直交座標系Σ0 上における位置偏差と設定さ
れたパラメータ（Ｋx ，Ｋy ，Ｋz ）を用い、直交座標
系Σ0 上における力（Ｆx ，Ｆy ，Ｆz ）を求める。ス
テップＳ２で、求められた力（Ｆx ，Ｆy ，Ｆz ）をロ
ボットの姿勢データを用いてツール座標系Σn+1 上のデ
ータ（Ｆx(n+1)，Ｆy(n+1)，Ｆz(n+1)）に変換する。更
に、（Ｆx(n+1)，Ｆy(n+1)，Ｆz(n+1)）＝（ｆx(n+1)，
ｆy(n+1)，ｆz(n+1)）として、Ｎｅｗｔｏｎ−Ｅｕｌｅ
ｒ法により、各軸のトルクＴi を計算し（ステップＳ
３）、Ｔi ／（Ｋp Ｋv ）から位置ループ入力Ｅi を計
算する（ステップＳ４）。位置ループ入力Ｅi を入力と
する位置ループ処理、位置ループ出力を入力とする積分
処理と速度ループ処理、積分処理と速度ループ処理の出
力を加算するトルク指令作成処理等を実行すれば（ステ
ップＳ５）、ソフトフロート機能が発揮される。積分処
理と速度ループ処理の出力の加算は、設定された制限範
囲を越えないように行なわれる。

Description

【発明の詳細な説明】
【０００１】
【産業上の利用分野】本願発明は産業用ロボット（以
下、単に「ロボット」と言う。）の制御技術に関し、更
に詳しく言えば、ロボット軸駆動用のサーボモータを制
御するサーボ系の柔らかさを必要に応じて変更し、可変
に指定乃至設定すること（以下、「調節」と言う。）が
出来るようにする為の技術に関する。
【０００２】
【従来の技術】ロボットのアームを駆動する各軸のサー
ボモータの制御は、通常、位置制御ループ及び速度制御
ループを有するサーボ系によって制御されている。図１
は、これを示したブロック図で、符号１は位置ループゲ
インＫp の項、符号２は速度ループゲインＫv の項であ
る。また符号３，４はモータの伝達関数の項であり、３
はトルク定数Ｋｔ、４はイナーシャＪである。更に、符
号５はモータ速度ｖを積分してモータ位置ｑを求める伝
達関数である。なお、ｓはラプラス演算子を表わしてい
る。
【０００３】ロボットコントローラの内部で作成される
移動指令ｒとモータ位置ｑより位置偏差ｅが算出され、
該位置偏差ｅv に位置ループゲインＫｐを乗じて速度指
令ｖc が出力される。更に、速度偏差ｅv が速度指令ｖ
c とモータ速度ｖより算出され、この速度偏差ｅに速度
ループゲインＫｖを乗じてトルク指令ｔc が出力され
る。そして、該トルク指令ｔc に応じた駆動電流がモー
タに供給される。なお、速度ループの制御においては、
Ｐ制御に代えてＰＩ制御またはＩＰ制御が適用されるこ
ともある。
【０００４】このようなサーボ系（ＰＩ制御もしくはＩ
Ｐ制御）によって各軸のモータが制御されるロボットの
ツール先端点が目標位置に向かって移動中になんらかの
障害物に遭遇・接触した場合、ロボットのツール先端点
は障害物に抗して目標位置に向かって移動を続行しよう
とする現象が生じる。
【０００５】この現象は次のように説明される。即ち、
障害物の存在に関わらずサーボモータは目標位置に向か
って移動しようとするが、実際には障害物によりその目
標位置に移動することが阻まれ位置偏差ｅが増大する。
その結果、この位置偏差に位置ゲインＫｐを乗じて得ら
れる速度指令ｖc も増大する。そして、この増大する速
度指令ｖc とモータの速度ｖ（障害物に当接していると
きには、速度ｖは「０」に近いと考えられる。）との差
は、速度ループの持つ積分器によって積分されて増大
し、トルク指令Ｔc は大きな値となる。
【０００６】結局、サーボモータは目標位置への移動を
実現しようとして最大のトルクを出力するようになり、
ロボットの停止やワーク、エンドエフェクタ等の破壊事
故（干渉事故）を招く原因となる。そこで、このような
不都合を回避する手段として、必要に応じて位置ループ
ゲインＫｐ及び速度ループゲインＫｖを低下させること
により速度指令ｖc 及びトルク指令ｔc の値の増大を抑
止する方式が用いられている。
【０００７】この方式においては、通常、柔軟制御用の
ゲイン値Ｋｐ’，Ｋｖ’が予め設定され、柔軟制御指令
の入力時にゲインの値がＫｐ，ＫｖからＫｐ’，Ｋｖ’
に各々切り換えられる。
【０００８】このような従来方式は、サーボ系の柔らか
さを各ロボット軸（以下、単に「各軸」とも言う。）毎
に調節することを通して「各軸空間上でのソフトフロー
ト機能」を実現するものである。
【０００９】例えば、ロボットの手首側の３軸について
非常に小さなＫｐ’，Ｋｖ’を設定し、手首側の３軸に
ついて特に柔らかいサーボ制御が実現される。また、全
軸について非常に小さなＫｐ’，Ｋｖ’を設定すれば、
全軸について非常に柔らかいサーボ系が構成される。
【００１０】
【発明が解決しようとする課題】しかし、上記した従来
方式では、サーボ系の柔らかさは各軸上でしか調節する
ことが出来ないので、作業空間内の方向別に柔らかさを
調節し、嵌め合い、バリ取り加工等、実際の作業に適合
した柔らかさをロボットに持たせることが困難であっ
た。
【００１１】例えば、嵌め合いにおいては、一般に、ロ
ボットに把持されたワークの挿入方向に関しては硬く、
挿入方向と直交する方向に関しては柔らかいサーボ系に
よる制御が行なわれることが望ましいと考えられが、作
業空間内に定義された座標系上でサーボ系の柔らかさを
調節出来ない従来方式ではこのようなニーズには応えら
れない。
【００１２】また、バリ取り加工においては、加工線の
延在方向とそれに直交する２方向の動作について各々別
の柔らかさを持たせることが望ましいが、それをロボッ
トの各軸についてサーボ系の柔らかさを調節する従来方
式で実現することも極めて困難である。
【００１３】そこで本願発明の目的は、作業空間内に定
義された直交座標系（以下、単に「直交座標系」とも言
う。）上で柔らかさを調節することが出来るサーボ系を
提供し、ロボットに直交座標系上でのソフトフロート機
能を持たせることにある。また、本願発明は、そのこと
を通して、ロボットを用いた作業の精度と安全性を向上
させることを企図するものである。
【００１４】
【課題を解決するための手段】本願発明は、ロボットの
各軸を駆動するサーボモータを制御する為に各軸毎に位
置ループ処理と速度ループ処理を含む処理を行なうサー
ボ系に、直交座標系上でソフトフロート機能を持たせる
ようにしたものである。先ず、作業空間内に定義された
直交座標系上でのサーボの柔らかさを表わすパラメータ
の組を設定する。設定は、例えばロボットコントローラ
に付属した教示操作盤から画面入力することで行なわれ
る。ソフトフロート機能を有効化した場合には、前記設
定されたパラメータの組と、直交座標系上での位置偏差
から直交座標系上での力を計算する。
【００１５】そして、この力を逆動力学で解いて各軸の
トルクＴi （ｉ＝１，２・・・ｎ；但し、ｎはロボット
の軸数）を求め、このトルクＴi と位置ループ処理のゲ
インＫp と、速度ループ処理のゲインＫv に基づいて算
出されたＥi を、各軸の前記位置ループ処理の入力とし
た処理を行なうことにより、直交座標系上で指定された
柔らかさが実現される。柔らかさの調節は、パラメータ
の組の設定を通して実行される。位置ループ処理の入力
Ｅi は、前記トルクＴi （ｉ＝１，２・・・ｎ；但し、
ｎはロボットの軸数）を前記位置ループ処理のゲインＫ
p と速度ループ処理のゲインＫv の積で除すことによっ
て算出することが出来る。
【００１６】重力の影響を考慮する必要のある場合に
は、位置ループ処理の出力が予め定められた積分ゲイン
で積分され、速度ループ処理の出力に足し込まれてトル
ク指令が出力される。その際の足し込み量には一定の制
限を設けることで、柔らかさが失われることを防止す
る。
【００１７】
【作用】本願発明は、ロボットの各軸を駆動するサーボ
モータを制御するサーボ系についてソフトフロート機能
を拡充するものである。即ち、本願発明によれば、直交
座標系上で柔らかさを指定することが出来るサーボ系が
構築され、「直交座標系上でのソフトフロート機能」が
実現される。本願発明の原理の概要を説明すれば、次の
通りである。
【００１８】直交座標系（Ｏ−ＸＹＺ）上でのロボット
動作制御の柔らかさは、次のように表わすことが出来
る。 Ｋx （ｘd −ｘ）＝Ｆx ・・・（１） Ｋy （ｙd −ｙ）＝Ｆy ・・・（２） Ｋz （ｚd −ｚ）＝Ｆz ・・・（３） ここで、 Ｋx ，Ｋy ，Ｋz ；Ｘ，Ｙ，Ｚ各座標系軸方向に関する
柔らかさの程度を表わすパラメータ ｘ，ｙ，ｚ ；直交座標系上でのフィードバック（Ｘ，
Ｙ，Ｚ各成分） ｘd ，ｙd ，ｚd ；直交座標系上での位置指令（Ｘ，
Ｙ，Ｚ各成分） Ｆx ，Ｆy ，Ｆz ；ツール先端点にかかる力（位置指令
で指定された位置に移動しようとする力のＸ，Ｙ，Ｚ各
成分）、 である。
【００１９】従って、直交座標系上でソフトフロート機
能を実現する為には、ロボットの動作時に上記（１）〜
（３）式中のパラメータＫx ，Ｋy ，Ｋz の値を（可変
に定められた）一定値に保つような各軸の柔軟制御を行
なうサーボ系を構築すれば良いことになる。
【００２０】上記（１）〜（３）式は、ツール先端点に
かかる力Ｆx ，Ｆy ，Ｆz が、パラメータＫx ，Ｋy ，
Ｋz と、直交座標系上における位置偏差で表わされるこ
とを示している。
【００２１】一方、逆動力学のアルゴリズムとして良く
知られているＮｅｗｔｏｎ−Ｅｅｌｅｒ法（Ｌｕｈのア
ルゴリズムとも呼ばれる。）を用いれば、ツール先端点
にかかる力Ｆx ，Ｆy ，Ｆz から、ロボットの各軸に必
要なトルクτi （ｉ＝１，２・・・ｎ；ｎはロボットの
軸数、以下同様。）を求めることが出来る。
【００２２】Ｎｅｗｔｏｎ−Ｅｅｌｅｒ法は、例えば
「機械系のためのロボティクス」（遠山茂樹 著；総合
電子出版社、１９９１年５月３０日第２版発行；ｐ５５
〜ｐ６５）に詳しいのでその詳細は省略し、使用される
アルゴリズムの結論部分のみを記せば次のようになる。
このアルゴリズムは、初期化、フォワードルーチン、バ
ックワードルーチンで構成される。
【００２３】フォワードルーチンでは、ベース座標系Σ
0 側からロボット手先側に向かって各リンクの運動学的
な情報が計算される。一方、バックルーチンでは、ロボ
ット手先側からベース座標系Σ0 側へ向かって各リンク
に作用する力／モーメントかが計算される。
【００２４】
【数１】 各記号の意味は次の通りである。 ｎi (i)；ｉ−１番目の軸で駆動されるリンク（以下、リ
ンクｉ−１などと言う。）からリンクｉに及ぼされるト
ルクのΣi 表示。但し、Σi はｉ番目の軸上に設定され
た座標系Σi を表わす。 ｆi (i)；リンクｉ−１からリンクｉに及ぼされる力のΣ
i 表示 Ｆi (i)；リンクｉに及ぼされる外力ベクトルのΣi 表示 Ｎi (i)；リンクｉに及ぼされるモーメントベクトルのΣ
i 表示 Ｒi ；座標系変換行列Ａi の主座の３×３行列で、回転
を表わす。なお、座標系変換行列Ａi は、ｉ番目の軸上
に設定された座標系Σi からｉ−１番目の軸上に設定さ
れた座標系Σi-1 への座標変換行列である。但し、座標
系Σ0 は前記直交座標系よ一致させたベース座標系にと
る。
【００２５】ｐi *；Σi 表示のΣi-1 原点からΣi 原点
への位置ベクトル ｇ；重力ベクトル ｓi バー；Σi 表示のリンクｉの重心位置を示す位置ベ
クトル Ｉi ；Σi 表示のリンクｉの重心周りの慣性モーメント ｚ0 ；Σ0 のＺ軸方向の単位ベクトルで、Σ0 上では
（０，０，１）と表わされる ｂi ；ｉ番目の関節軸の粘性抵抗 ｑi ；ｉ番目の関節軸の軸変数で、ｑi ＝θi （リンク
ｉの角度）として良い ω，ωドット；Σ0 表示のリンクｉの角速度、角加速度 上記アルゴリズムにおいて使用される諸量は、ロボット
の構造パラメータ、各軸の位置、速度、加速度等から計
算される。また、前述の式（１）〜（３）に従って位置
偏差（ｘd −ｘ），（ｙd −ｙ），（ｚd −ｚ）と柔ら
かさを表わすパラメータＫx ，Ｋy ，Ｋz から定められ
るＦx ，Ｆy ，Ｆz は、上記アルゴリズム中のｆn+1
(n+1) に取入れられる。ｆn+1(n+1) は、Σn+1 をツー
ル座標系として定義することにより、ツール座標系の原
点に働く力をツール座標系上で表わしていると考えるこ
とが出来る。
【００２６】そこで、ツール座標系の姿勢を表わす現在
データを行列Ｕとし、Ｆ＝（Ｆx ，Ｆy ，Ｆz ）T ＝
（Ｆx(0)，Ｆy(0)，Ｆz(0)）T とすれば、行列演算ＵＦ
によって、Ｆx ，Ｆy ，Ｆz のツール座標系上の表現が
計算される。これをＦ(n+1) ＝（Ｆx(n+1)，Ｆy(n+1)，
Ｆz(n+1)）T とすれば、ｆn+1(n+1)＝Ｆ(n+1) となる。
【００２７】ここで、重力を考慮しない条件；ｇ＝
（０，０，０）で上記アルゴリズムを用いて求められた
軸ｉのトルクτi （ｇ＝０）をＴi とすれば、その軸の
サーボ系は次のように表わされる。
【００２８】 Ｋp Ｋv Ｅi ＝Ｔi ・・・（５） 但し、Ｋp は位置ループゲイン、Ｋv は速度ループゲイ
ン、Ｅi は軸ｉの位置ループの入力を表わしている。図
２は、このサーボ系を図１に準じた表記で示したもので
ある（Ｋp ，Ｋv は便宜上各軸共通の表記としたが、各
軸毎に設定される場合にはＫp ，Ｋv に代えてＫpi，Ｋ
viとすれば良い）。図１と図２の差異は位置ループ処理
への入力がｅからＥi に変わっていることである。即
ち、各軸の位置ループ入力Ｅi は従来のサーボ系（図１
参照）における位置偏差ｅのように、その軸ｉの位置指
令ｒと位置フィードバックｑの偏差で定義されるもので
はないことである。
【００２９】このＥi は、直交座標系上でのサーボの柔
らかさを表わすパラメータの組Ｋx，Ｋy ，Ｋz と直交
座標系上での位置偏差（ｘd −ｘ），（ｙd −ｙ），
（ｚd−ｚ）から計算される力を逆動力学で解いて求め
られた各軸のトルクＴi と、位置ループゲインＫp と、
速度ループゲインＫv に基づいて算出されたものであ
る。
【００３０】換言すれば、このＥi は、直交座標系上で
パラメータＫx ，Ｋy ，Ｋz で表わされる柔らかさを実
現させる為にそのロボット軸のサーボ系が持つべき柔ら
かさに応じて各軸の位置偏差ｅを修正したものと解釈す
ることも出来る。Ｅを次式（６）で記す。 Ｅ＝Ｔ／（Ｋp Ｋv ） ・・・（６） 以上のことから、各軸について上記（６）式で決定され
るＥi を各軸の位置ループ入力とすることにより、前述
の関係式（１）〜（３）で表わされる直交座標系上での
柔らかさを実現するに必要なトルクＴi が発生する。
【００３１】次に重力の作用を考慮する。重力の作用を
考慮に入れた制御は、図２に示したサーボ系に積分器を
付加することで達成出来る。これを、図２に準じた表記
で図３に示す。図３において、Ｋp ，Ｋv は便宜上各軸
共通の表記としたが、各軸毎に設定される場合にはＫp
，Ｋv に代えてＫpi，Ｋviとすれば良いことは図２の
場合と同様である。図２と図３の差異は、Ｅi を入力と
する位置ループ処理で出力される速度指令ｖc に対して
重力分を補償する積分項６（Ｋg ／ｓ；但し、Ｋg は積
分器のゲイン）が計算され、速度指令ｖc を入力とする
速度ループ処理で計算されるトルクＴi に足し込まれる
ことである。
【００３２】この積分項６は、鉛直上向き方向について
トルク不足となる状態を回避する手段を提供するもので
ある。例えば直交座標系のＺ軸を鉛直上向き方向にと
り、Ｋz を小さく（柔らかさは大）に設定した場合、積
分項６がない場合にはＴi だけでは重力に打ち勝つこと
さえ出来ず、落下事故を起こす可能性がある。積分項６
があれば、Ｔi だけでは重力に打ち勝つことが出来なく
とも、ゲインＫg に応じてトルクＴi'が増大し、十分な
トルクが発生する。
【００３３】但し、この積分項６で表わされる積分器に
は重力分のトルクが溜まる性質があるので、積分器の出
力に一定の制限値を設ける必要がある。もし、制限値を
設けないと、積分器に外力以上の力が溜り、柔らかさを
失ってその軸の指令位置に向かう強制的な動きが発生す
る。
【００３４】そこで、ソフトフロート機能起動時に積分
器に溜っている値（出力）をＩGRとして、ソフトフロー
ト機能時には次のような制限を設ける。ＩSOFT-LIMの値
はチューニング等によって各軸毎に適当に設定される。 IF Ｉ＞ＩGR＋ＩSOFT-LIM THEN Ｉ＝ＩGR＋ＩSOFT-LIM ・・・（７） IF Ｉ＜ＩGR−ＩSOFT-LIM THEN Ｉ＝ＩGR−ＩSOFT-LIM ・・・（８） 以上説明した手法に従って各軸のサーボ系を構成するこ
とによって、ロボットに直交座標系上で指定された柔ら
かさを持たせることが出来る。例えば、Ｚ軸方向を挿入
方向とする嵌め合いにおいては、Ｋx とＫy は比較的小
さく、Ｋz は比較的大きく指定することで、嵌め合いを
スムーズに行なうことが出来る。また、バリ取り加工に
おいては、加工線の延在方向（例えばＸ軸方向）とそれ
に直交する２方向（例えばＹ軸、Ｚ軸方向）について、
個別に適正なＫx ，Ｋy ，Ｋz の値を設定し、バリ取り
加工の作業精度を向上させることが出来る。
【００３５】
【実施例】図４は、本願発明の実施例で使用されるロボ
ットコントローラＲＣを関連機器と共に要部ブロック図
で示したものである。先ず、ソフトフローティングサー
ボ制御に直接関係のある部分から説明すると、符号１０
はシステム全体を制御するホストコンピュータを表わし
ている。符号１６は、ホストコンピュータ１０から出力
される移動指令や制御信号を後述のディジタルサーボ回
路のプロセッサに引渡し、あるいは、逆にディジタルサ
ーボ回路のプロセッサからの各種信号をホストコンピュ
ータ１０に引き渡すための共有ＲＡＭメモリを表わして
いる。
【００３６】符号１７は、上述したサーボ制御を実行す
るディジタルサーボ（ソフトウェアサーボ）回路で、プ
ロセッサ、ＲＯＭ、ＲＡＭ等のメモリ等で構成される。
符号１８は、ロボット３０における各軸のサーボモータ
の位置のフィードバック値、速度のフィードバック値、
電流のフィードバック値等が書き込まれる帰還レジスタ
を表わしている。
【００３７】他の部分について見ると、ホストＣＰＵ１
０にはバスライン１９を介してＲＯＭ１１、ＲＡＭ１
２、不揮発性メモリ１３、外部装置４０とのインターフ
ェイスの役割を果たす入出力装置（Ｉ／Ｏ）１４、教示
操作盤２０とのインターフェイス（Ｉ／Ｆ）１５が接続
されている。ＲＯＭ１１には、各種のシステムプログラ
ムが格納される。ＲＡＭ１２は、ホストＣＰＵ１０によ
るデータの一時記憶に使用されるメモリである。不揮発
性メモリ１３には、ロボット３０及び外部装置４０の動
作内容に関する各種プログラム、関連設定値等が格納さ
れる。
【００３８】教示操作盤２０は、液晶ディスプレイ（Ｌ
ＣＤ）及びキーボードＫＢを備え、プログラム再生運転
指令、ジョグ送り指令、プログラムデータの入力／変
更、関連設定値入力／変更等が可能である。本実施例で
は、後述する柔らかさ設定画面で画面入力されるＸＹＺ
各軸方向の柔らかさのデータ、並びに教示操作盤２０か
ら入力されるＩSOFT-LIMの値が不揮発性メモリ１３に格
納される。
【００３９】入出力装置１４に接続される外部装置は、
アプリケーションによって異なる。例えば、ワークの把
持を伴う作業であればロボットハンド、バリ取り加工で
あればバリ取りツール（電源等を含む。）が入出力装置
１４に接続される。
【００４０】図５は、本願発明の適用例として考えられ
る嵌め合い作業について説明する図である。同図におい
て、符号Ｗ1 ，Ｗ2 は嵌め合い対象のワークを表わして
いる。ワークＷ1 は図示を省略したロボットハンドに把
持されて、ワークＷ2 の孔ＨＬ内に嵌入される。嵌入方
向はロボットコントローラＲＣに設定済みの直交座標系
Σ0 のＺ軸方向と一致しているものとする。符号ＧＰは
ワークＷ1 の先端部に設けられたガイドピンを表わし、
ＴＣＰはツール先端点を表わす。
【００４１】また、Ｐ1 ，Ｐ2 は各々後述する動作プロ
グラムでイチ［１］及びイチ［２］として教示された位
置を表わす。このような嵌め合いにおいては、教示ライ
ンＰ1 Ｐ2 が孔ＨＬの軸線とある程度ずれることが多
い。従って、イチ［１］からイチ［２］への移動時に通
常の硬いサーボ制御を行なうと、ＴＣＰはあくまで教示
ラインＰ1 Ｐ2 に沿って進もうとするので、ガイドピン
ＧＰが孔ＨＬの入口でつかえて動けなくなり、場合によ
ってはワークＷ1 ，Ｗ2 やロボットハンドの破損事故を
起こす。本願発明を適用すれば、このような事態が回避
される。以下、その為の手順を説明する。
【００４２】先ず、ユーザは、教示操作盤２０に付設さ
れたＬＣＤに図６に示されたような柔らかさ入力画面を
呼び出す。そして、Σ0 のＸ軸、Ｙ軸、Ｚ軸方向の各々
について個別に所望の値を画面入力する。入力される数
値が小さい程柔らかい状態が設定され、入力される数値
が大きい程硬い状態が設定さる。ここでは、Ｘ軸方向；
Ｋx ＝２００ｇ／ｃｍ、Ｙ軸方向；Ｋy ＝２００ｇ／ｃ
ｍ、Ｚ軸方向；Ｋz ＝５００ｇ／ｃｍが各々設定される
例が示されている。
【００４３】図７は、図５に示した嵌め合いを実行する
為に直交座標系ソフトフロート機能を用いた動作プログ
ラムの一例を表わしている。
【００４４】図６の画面入力後に図７の動作プログラム
の再生運転を開始すると、ロボットのツール先端点はイ
チ［１］まで各軸移動形式で移動し、位置決めされる。
そして、ハンドを閉じてワークＷ1 を把持した後に、直
交座標系ソフトフロート機能を有効にして、イチ［２］
に向かって指令速度５０ｍｍ／ｓｅｃで移動する。直交
座標系ソフトフロート機能が有効にされると図８に示し
た処理が所定周期で実行され、作用の欄で説明した原理
によって、ソフトフロート機能が発揮される。先ず、ス
テップＳ１では、直交座標系Σ0 上における位置偏差と
設定されたパラメータ（Ｋx ，Ｋy ，Ｋz ）を用いて、
直交座標系Σ0 上における力（Ｆx，Ｆy ，Ｆz ）を求
める。直交座標系Σ0 上における位置偏差は、各軸にお
ける位置偏差から順運動学で計算出来る。続くステップ
Ｓ２では、直交座標系Σ0 上で求められた力（Ｆx ，Ｆ
y ，Ｆz ）をロボットの姿勢データを用いてツール座標
系Σn+1 上のデータ（Ｆx(n+1)，Ｆy(n+1)，Ｆz(n+1)）
に変換する。
【００４５】更に、（Ｆx(n+1)，Ｆy(n+1)，Ｆz(n+1)）
＝（ｆx(n+1)，ｆy(n+1)，ｆz(n+1)）として、作用の欄
で説明したＮｅｗｔｏｎ−Ｅｕｌｅｒ法により、各軸の
トルクＴi を計算し（ステップＳ３）、Ｔi ／（Ｋp Ｋ
v ）から位置ループ入力Ｅiを計算する（ステップＳ
４）。計算された位置ループ入力Ｅi を入力とする位置
ループ処理、位置ループ出力を入力とする積分処理と速
度ループ処理、積分処理と速度ループ処理の出力を加算
するトルク指令作成処理等を実行すれば（ステップＳ
５）、図６の設定画面通りのソフトフロート機能が発揮
される。積分処理と速度ループ処理の出力の加算は、設
定されたＩSOFT-LIMの下で、前出の（７），（８）式の
制限を守った範囲で行なわれる。
【００４６】これにより、ワークＷ1 のガイドピンＧＰ
が孔ＨＬの上縁部に当接すると、ツール先端点ＴＣＰは
孔ＨＬの内壁をなぞるように孔ＨＬの軸線ＷＪ上の点Ｑ
（教示位置Ｐ2 に対応する位置）まで円滑に移動する。
次いで、ロボットハンドを開放して、ワークＷ1 の把持
を解けば嵌め合い作業は完了する。
【００４７】なお、上記実施例では、直交座標系上で柔
らかさを表わすパラメータの設定は画面入力で行なった
が、動作プログラムの中で指定する方式としても良い。
例えば、バリ取りへのアプリケーションにおいて、加工
線の延在方向が屈曲している場合に、動作プログラム中
でそれに応じたパラメータ変更を行なえば、加工線の延
在方向に即した柔らかさの設定が可能となる。
【００４８】
【発明の効果】本願発明によれば、作業空間内に定義さ
れた直交座標系上で柔らかさを調節することが出来る。
本願発明の手法に従って各軸のサーボ系を構成すること
によって、例えば、嵌め合い、バリ取り等の作業精度を
向上させることが出来る。
【図面の簡単な説明】
【図１】従来のサーボ系の構成を示したブロック図であ
る。
【図２】重力を考慮しない場合の本願発明のサーボ系の
構成を示したブロック図である。
【図３】重力を考慮した場合の本願発明のサーボ系の構
成を示したブロック図である。
【図４】本願発明の実施例で使用されるロボットコント
ローラの概要を説明する要部ブロック図である。
【図５】本願発明の適用例として考えられる嵌め合い作
業について説明する図である。
【図６】直交座標系上で柔らかさを入力する為の画面を
説明する図である。
【図７】図５に示した嵌め合いを実行する為に直交座標
系ソフトフロート機能を用いた動作プログラムの一例を
表わたものである。
【図８】実施例におけるソフトフロート機能有効化時の
処理の概要を説明するフローチャートである。
【符号の説明】
１ 位置ループゲインの項 ２ 速度ループゲインの項 ３，４ モータの伝達関数の項 ５ モータ速度からモータ位置を求める伝達関数 ６ 重力分を補償する積分項 １０ ホストＣＰＵ １１ ＲＯＭ １２ ＲＡＭ １３ 不揮発性メモリ １４ 入出力装置 １５ インターフェイス １６ 共有ＲＡＭ １７ ディジタルサーボ回路 １８ 帰還レジスタ １９ バスライン ２０ 教示操作盤 ３０ ロボット ４０ 外部装置 ＫＢ キーボード Ｋp 位置ゲイン Ｋv 速度ループゲイン Ｋt トルク定数 Ｋg 積分ゲイン ＧＰ ガイドピン ＨＬ 孔 ＬＣＤ 液晶ディスプレイ Ｐ1 ，Ｐ2 教示点 ＲＣ ロボットコントローラ Ｗ1 ，Ｗ2 嵌め合い対象ワーク ＷＪ 孔の軸線

Claims (5)

    【特許請求の範囲】
  1. 【請求項１】 ロボットの各軸を駆動するサーボモータ
    を制御する為に前記各軸毎に位置ループ処理と速度ルー
    プ処理を含む処理を行なうサーボ系であって、 作業空間内に定義された直交座標系上での前記サーボの
    柔らかさを表わすパラメータの設定値の組と、前記直交
    座標系上での位置偏差から計算される前記直交座標系上
    での力を逆動力学で解いて求められた各軸のトルクＴi
    （ｉ＝１，２・・・ｎ；但し、ｎはロボットの軸数）
    と、前記位置ループ処理のゲインＫp と、速度ループ処
    理のゲインＫv に基づいて算出されたＥi を、各軸の前
    記位置ループ処理の入力とした、直交座標系上で柔らか
    さが調節可能な前記サーボ系。
  2. 【請求項２】 ロボットの各軸を駆動するサーボモータ
    を制御する為に前記各軸毎に位置ループ処理と速度ルー
    プ処理を含む処理を行なうサーボ系であって、 作業空間内に定義された直交座標系上での前記サーボの
    柔らかさを表わすパラメータの設定値の組と前記直交座
    標系上での位置偏差から計算される前記直交座標系上で
    の力を逆動力学で解いて求められた各軸のトルクＴi
    （ｉ＝１，２・・・ｎ；但し、ｎはロボットの軸数）を
    前記位置ループ処理のゲインＫp と速度ループ処理のゲ
    インＫv の積で除した値Ｅi ＝Ｔi ／（Ｋp Ｋv ）を、
    各軸の前記位置ループ処理の入力とした、直交座標系上
    で柔らかさが調節可能な前記サーボ系。
  3. 【請求項３】 ロボットの各軸を駆動するサーボモータ
    を制御する為に前記各軸毎に位置ループ処理と速度ルー
    プ処理を含む処理を行なうサーボ系であって、 作業空間内に定義された直交座標系上での前記サーボの
    柔らかさを表わすパラメータの設定値の組と前記直交座
    標系上での位置偏差から計算される前記直交座標系上で
    の力を逆動力学で解いて求められた各軸のトルクＴi
    （ｉ＝１，２・・・ｎ；但し、ｎはロボットの軸数）
    と、前記位置ループ処理のゲインＫp と、速度ループ処
    理のゲインＫv に基づいて算出されたＥi を、各軸の前
    記位置ループ処理の入力とすると共に、 前記位置ループ処理の出力を予め定められた積分ゲイン
    で積分し、前記速度ループ処理の出力に予め定められた
    所定の制限内で足し込んでトルク指令を出力するように
    した、直交座標系上で柔らかさが調節可能な前記サーボ
    系。
  4. 【請求項４】 ロボットの各軸を駆動するサーボモータ
    を制御する為に前記各軸毎に位置ループ処理と速度ルー
    プ処理を含む処理を行なうサーボ系であって、 作業空間内に定義された直交座標系上での前記サーボの
    柔らかさを表わすパラメータの設定値の組と前記直交座
    標系上での位置偏差から計算される前記直交座標系上で
    の力を逆動力学で解いて求められた各軸のトルクＴi
    （ｉ＝１，２・・・ｎ；但し、ｎはロボットの軸数）を
    前記位置ループ処理のゲインＫp と速度ループ処理のゲ
    インＫv の積で除した値Ｅi ＝Ｔi ／（Ｋp Ｋv ）を、
    各軸の前記位置ループ処理の入力とすると共に、 前記位置ループ処理の出力を予め定められた積分ゲイン
    で積分し、前記速度ループ処理の出力に予め定められた
    所定の制限内で足し込んでトルク指令を出力するように
    した、直交座標系上で柔らかさが調節可能な前記サーボ
    系。
  5. 【請求項５】 前記作業空間内に定義された直交座標系
    上での前記サーボの柔らかさを表わすパラメータの組
    が、画面入力により設定される請求項１〜請求項４いず
    れか１項に記載された、直交座標系上で柔らかさが調節
    可能な前記サーボ系。
JP05518995A 1995-02-21 1995-02-21 直交座標系上で柔らかさが調節可能なサーボ系 Expired - Fee Related JP3681431B2 (ja)

Priority Applications (4)

Application Number Priority Date Filing Date Title
JP05518995A JP3681431B2 (ja) 1995-02-21 1995-02-21 直交座標系上で柔らかさが調節可能なサーボ系
EP96902445A EP0766161B1 (en) 1995-02-21 1996-02-15 Control method of servo system capable of adjusting flexibility on orthogonal coordinate system
PCT/JP1996/000329 WO1996026473A1 (fr) 1995-02-21 1996-02-15 Procede de commande de systeme d'asservissement capable d'adapter la flexibilite sur un systeme de coordonnees orthogonales
US08/732,409 US5742138A (en) 1995-02-21 1996-02-15 Control method for servo system with adjustable softness in rectangular coordinate system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP05518995A JP3681431B2 (ja) 1995-02-21 1995-02-21 直交座標系上で柔らかさが調節可能なサーボ系

Related Child Applications (1)

Application Number Title Priority Date Filing Date
JP2005030542A Division JP2005219205A (ja) 2005-02-07 2005-02-07 直交座標系上で柔らかさが調節可能なサーボ系

Publications (2)

Publication Number Publication Date
JPH08227320A true JPH08227320A (ja) 1996-09-03
JP3681431B2 JP3681431B2 (ja) 2005-08-10

Family

ID=12991762

Family Applications (1)

Application Number Title Priority Date Filing Date
JP05518995A Expired - Fee Related JP3681431B2 (ja) 1995-02-21 1995-02-21 直交座標系上で柔らかさが調節可能なサーボ系

Country Status (4)

Country Link
US (1) US5742138A (ja)
EP (1) EP0766161B1 (ja)
JP (1) JP3681431B2 (ja)
WO (1) WO1996026473A1 (ja)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2000218577A (ja) * 1999-01-27 2000-08-08 Mitsubishi Electric Corp ロボット制御装置
US7212886B2 (en) 2002-12-12 2007-05-01 Kabushiki Kaisha Yaskawa Denki Robot control apparatus and method
JP2007285525A (ja) * 2007-07-06 2007-11-01 Honda Motor Co Ltd 変速機の制御装置
JP2011241881A (ja) * 2010-05-17 2011-12-01 Toyota Motor Corp 制御装置

Families Citing this family (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3300625B2 (ja) * 1997-01-27 2002-07-08 ファナック株式会社 ロボットの制御方式
JP3291224B2 (ja) * 1997-06-30 2002-06-10 オークマ株式会社 速度制御装置
US6470225B1 (en) 1999-04-16 2002-10-22 Siemens Energy & Automation, Inc. Method and apparatus for automatically tuning feedforward parameters
US6198246B1 (en) 1999-08-19 2001-03-06 Siemens Energy & Automation, Inc. Method and apparatus for tuning control system parameters
US6204619B1 (en) 1999-10-04 2001-03-20 Daimlerchrysler Corporation Dynamic control algorithm and program for power-assisted lift device
JP3473834B2 (ja) 1999-11-29 2003-12-08 株式会社安川電機 ロボットの制御装置
JP2002187040A (ja) * 2000-12-19 2002-07-02 Murata Mach Ltd ローダ制御装置
DE10297481B4 (de) * 2002-04-05 2014-01-09 Mitsubishi Denki K.K. Verfahren zum betrieb einer motorsteuervorrichtung
JP4236103B2 (ja) * 2003-11-25 2009-03-11 本田技研工業株式会社 アクチュエータの制御装置
ITVA20040004A1 (it) * 2004-02-06 2004-05-06 St Microelectronics Srl Metodo e circuito di pilotaggio ad anello aperto in tensione di un motore a corrente continua
US7211979B2 (en) * 2005-04-13 2007-05-01 The Broad Of Trustees Of The Leland Stanford Junior University Torque-position transformer for task control of position controlled robots
US7490213B2 (en) * 2005-05-06 2009-02-10 Xiv Ltd. Dynamic allocation of storage for hierarchical copies
US7904182B2 (en) 2005-06-08 2011-03-08 Brooks Automation, Inc. Scalable motion control system
JP2008073830A (ja) * 2006-09-25 2008-04-03 Fanuc Ltd ロボット制御装置
JP5845311B2 (ja) 2014-04-30 2016-01-20 ファナック株式会社 ロボットの柔軟制御を行う制御装置
EP2954986B1 (de) * 2014-06-10 2020-05-06 Siemens Aktiengesellschaft Vorrichtung und Verfahren zum Steuern und Regeln eines Mehrkörpersystems
CN109773781B (zh) * 2018-12-26 2021-05-25 南京埃斯顿机器人工程有限公司 一种无传感器的工业机器人直线软浮动控制方法
CN109676633B (zh) * 2019-02-22 2022-05-20 哈尔滨工程大学 三轴机械臂定点控制系统及方法

Family Cites Families (24)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE3240251A1 (de) * 1982-10-30 1984-05-03 Deutsche Forschungs- und Versuchsanstalt für Luft- und Raumfahrt e.V., 5000 Köln Verfahren zum programmieren von bewegungen und erforderlichenfalls von bearbeitungskraeften bzw. -momenten eines roboters oder manipulators und einrichtung zu dessen durchfuehrung
US4547858A (en) * 1983-06-13 1985-10-15 Allied Corporation Dynamic control for manipulator
JPS6126106A (ja) * 1984-07-16 1986-02-05 Fanuc Ltd 工具位置補正方式
US4629629A (en) * 1985-09-03 1986-12-16 David Constant V Method for processing avocados
JPS63145512A (ja) * 1986-12-09 1988-06-17 Toshiba Corp 柔軟マニピユレ−タの制御方法
JPH08384B2 (ja) * 1987-09-16 1996-01-10 ファナック株式会社 ツール先端点の自動設定方式
JPS6478308A (en) * 1987-09-19 1989-03-23 Fanuc Ltd Robot control system based on load conditions
US4925312A (en) * 1988-03-21 1990-05-15 Staubli International Ag Robot control system having adaptive feedforward torque control for improved accuracy
US4969108A (en) * 1988-04-08 1990-11-06 Cincinnati Milacron Inc. Vision seam tracking method and apparatus for a manipulator
JPH02262982A (ja) * 1989-03-31 1990-10-25 Fanuc Ltd ツールセンタポイントの設定方法
JP2864461B2 (ja) * 1989-05-16 1999-03-03 シチズン時計株式会社 位置制御方法およびその位置制御装置
US5194791A (en) * 1990-07-19 1993-03-16 Mcdonnell Douglas Corporation Compliant stereo vision target
US5179514A (en) * 1990-08-08 1993-01-12 The Research Foundation Of State University Of New York Method and apparatus for trajectory control of robot manipulators or the like
JPH04104315A (ja) * 1990-08-23 1992-04-06 Nec Software Kansai Ltd 磁気テープ装置のファイル検索方式
EP0520099A1 (en) * 1990-12-25 1992-12-30 Shukyohojin, Kongo Zen Sohonzan Shorinji Applied motion analysis and design
JPH04104315U (ja) * 1991-02-21 1992-09-08 エヌオーケー株式会社 コンプライアンス制御のパラメータ変更装置
JPH04267409A (ja) * 1991-02-21 1992-09-24 Nok Corp コンプライアンス制御方法及び制御装置
JP3230826B2 (ja) * 1991-10-16 2001-11-19 ファナック株式会社 スポット溶接ガンの位置補正方法及びロボット溶接装置
JPH06332538A (ja) * 1993-05-19 1994-12-02 Fanuc Ltd 柔軟なサーボ制御方法
JP3283650B2 (ja) * 1993-07-05 2002-05-20 ファナック株式会社 ロボット制御装置
US5373221A (en) * 1993-07-30 1994-12-13 Fanuc Robotics North America, Inc. Method and system for estimating robot tool center point speed
US5457367A (en) * 1993-08-06 1995-10-10 Cycle Time Corporation Tool center point calibration apparatus and method
JPH07210251A (ja) * 1994-01-26 1995-08-11 Fanuc Ltd ソフトウェアによるフローティングサーボ制御方法
JPH07319547A (ja) * 1994-05-20 1995-12-08 Fanuc Ltd ロボットの倣い制御方法

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2000218577A (ja) * 1999-01-27 2000-08-08 Mitsubishi Electric Corp ロボット制御装置
US7212886B2 (en) 2002-12-12 2007-05-01 Kabushiki Kaisha Yaskawa Denki Robot control apparatus and method
JP2007285525A (ja) * 2007-07-06 2007-11-01 Honda Motor Co Ltd 変速機の制御装置
JP2011241881A (ja) * 2010-05-17 2011-12-01 Toyota Motor Corp 制御装置

Also Published As

Publication number Publication date
EP0766161A1 (en) 1997-04-02
US5742138A (en) 1998-04-21
EP0766161A4 (en) 1998-05-20
JP3681431B2 (ja) 2005-08-10
WO1996026473A1 (fr) 1996-08-29
EP0766161B1 (en) 2002-09-18

Similar Documents

Publication Publication Date Title
JP3681431B2 (ja) 直交座標系上で柔らかさが調節可能なサーボ系
US9156164B2 (en) Method for adjusting parameters of impedance control
JP6924145B2 (ja) ロボット教示方法及びロボットアーム制御装置
JP3808321B2 (ja) ロボット制御装置
US9764462B2 (en) Robot apparatus and robot controlling method
US20200282558A1 (en) System and method for controlling a robot with torque-controllable actuators
CN101152719A (zh) 机器人控制装置
JPH079606B2 (ja) ロボット制御装置
JP2006236031A (ja) ロボット軌跡制御方法及び装置並びにロボット軌跡制御方法のプログラム
JP3300625B2 (ja) ロボットの制御方式
US11839979B2 (en) Dual mode free-drive of robot arm
JPH07319547A (ja) ロボットの倣い制御方法
WO1995002214A1 (fr) Procede de commande d'un servomoteur permettant d'affecter de la souplesse aux coordonnees de travail
JPH05108108A (ja) コンプライアンス制御方法及び制御装置
JP2013000856A (ja) ロボット制御システム、ロボットシステム
US10532460B2 (en) Robot teaching device that sets teaching point based on motion image of workpiece
JP4232196B2 (ja) ロボットの直接教示装置およびロボットシステム
JP2005219205A (ja) 直交座標系上で柔らかさが調節可能なサーボ系
JP4647919B2 (ja) 制御方法および制御装置
JPH1177572A (ja) ロボット制御装置
JPH058187A (ja) ロボツト
JPH11345010A (ja) ロボットの制御装置
US11712803B2 (en) Teaching method
JP3162638B2 (ja) 溶接チップ摩耗量検出方法
JPH08263128A (ja) ロボットの位置決め制御時の加減速制御方法

Legal Events

Date Code Title Description
A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20040622

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20041207

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20050207

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20050419

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20050518

R150 Certificate of patent or registration of utility model

Free format text: JAPANESE INTERMEDIATE CODE: R150

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20080527

Year of fee payment: 3

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20090527

Year of fee payment: 4

LAPS Cancellation because of no payment of annual fees