JPH07210251A

JPH07210251A - ソフトウェアによるフローティングサーボ制御方法

Info

Publication number: JPH07210251A
Application number: JP2470994A
Authority: JP
Inventors: Tetsuro Kato; 哲朗加藤; Souichi Arita; 創一有田
Original assignee: Fanuc Corp
Current assignee: Fanuc Corp
Priority date: 1994-01-26
Filing date: 1994-01-26
Publication date: 1995-08-11

Abstract

(57)【要約】【目的】人力によって被駆動物を移動させ、障害物か
らの容易な回避を可能とする柔軟なサーボ制御方法を提
供し、さらに、駆動体の姿勢にかかわらず、駆動体先端
でのバネ定数を常に一定に制御するサーボモータの制御
方法を提供する。【構成】位置制御ループ及び速度制御ループを備える
制御系で制御されるサーボモータの制御方法において、
制御する駆動体の回転中心から駆動体先端までの回転平
面上における先端距離Ｄをソフトウェアから求め、その
求めた先端距離Ｄに応じてバネ定数が設定値となるよう
に変更した位置ループゲインＫｐ及び速度ループゲイン
Ｋｖによりサーボモータを駆動することによってフロー
ティングサーボ制御を行う。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、ロボットコントローラ
等の数値制御装置に関し、ロボットアーム等を駆動する
サーボモータにおいて位置、速度制御ループを有し、位
置を制御するサーボ制御方法に関する。

【０００２】

【従来の技術】ロボットアーム等の制御において、該ア
ームを駆動するサーボモータの制御は、位置・速度制御
ループを有し、該サーボモータの位置を制御することに
よってアーム等の被駆動体の位置を制御している。図２
は、従来の被駆動体を制御するサーボ制御系のブロック
図である。

【０００３】図２において、１は位置ループにおけるボ
ジションゲインＫｐの項であり、２は速度ループゲイン
Ｋｖの項である。３，４はモータの伝達関数の項であっ
て、３はトルク定数Ｋｔで、４はイナーシャＪである。
また、５はモータ速度ｖを積分してモータ位置ｙを求め
る伝達関数である。なお、ｓはラプラス演算子である。

【０００４】移動指令ｒとモータ位置ｙより位置偏差ｅ
を求め、該位置偏差ｅにポジションゲインＫｐを乗じて
速度指令ｖｃｍｄを求める。該速度指令ｖｃｍｄとモー
タ速度ｖより速度偏差ｅを求める。この速度偏差ｅに速
度ループゲインＫｖを乗じてトルク指令Ｔｃを求め、該
トルク指令Ｔｃによりモータに駆動する。

【０００５】ロボットアーム等のサーボモータで駆動さ
れ、位置が制御される被駆動体の制御系は、一般に上述
した制御系により制御される（なお、速度ループの制御
においては、上記図２の例でＰＩ制御を例として記載し
ているが、ＩＰ制御とすることもできる。）。

【０００６】前記した制御系でサーボモータが制御され
る場合、与えられた位置指令によって目標位置に向かっ
てロボットアーム等の被駆動体が移動している途中にお
いて、障害物があると該被駆動体はその障害物を避ける
ことができず、そのまま目標位置に向かって移動し続け
るとういう現象が生じる。例えば、ロボットがハンドに
よってるワークの工作機械への取付け作業を行う場合、
ワークの精度が悪いと指令した位置とワークの位置との
間に位置ずれが生じて、ワークの工作機械への取付けが
困難となる。しかも、このような場合には、手動によっ
てロボットアームを移動させてワークを工作機械の取付
け位置に移動させることも困難となる。これは、サーボ
モータは目標位置に向かって移動しようとするが、障害
物によりその目標位置に移動することができずに位置偏
差が増大し、この位置偏差にポジションゲインＫｐを乗
じて得られる速度指令ｖｃｍｄも増大する。この増大す
る速度指令ｖｃｍｄとモータの速度（障害物に当接して
いるときには、その速度は「０」である）との差は、速
度ループの持つ積分器によって積分されて増大し、トル
ク指令Ｔｃは大きな値となる。これにより、サーボモー
タは目標位置に達するように最大のトルクを出力するよ
うになる。そのため、人間の力τでは、簡単にアーム等
を移動して障害物を避けて目標位置に達するよう移動さ
せることは困難となる。

【０００７】

【発明が解決しようとする課題】そこで、前記の問題点
を解消するために、従来、位置制御ループ及び速度制御
ループを備える制御系で制御されるサーボモータの制御
方法において、柔軟制御指令に入力されると、ポジショ
ンゲイン及び速度ループゲインをそれぞれあらかじめ設
定された設定値に低下させる等のゲイン変更により、速
度指令及びトルク指令の値を小さな値に抑え、サーボモ
ータで駆動される被駆動体の手動による移動を可能とす
るフローティング制御方法が提案されている。

【０００８】しかしながら、この提案されている位置制
御ループ及び速度制御ループを備える制御系における柔
軟制御のフローティング制御方法においては、モータの
トルク指令を制御することにより、駆動体にかかる力と
移動距離の関係によって示される柔らかを制御している
ため、駆動体回転中心から実際に力のかかる駆動体先端
のまでの回転平面上での距離により、駆動体先端での柔
らかさが異なる場合がある。

【０００９】図２において、移動指令が「０」のときの
外乱トルクτからモータ位置ｙのでの伝達関数は、ｙ／τ＝１／（Ｊ・ｓ²＋Ｋｔ・Ｋｖ・ｓ＋Ｋｔ・Ｋｐ・Ｋｖ） …（１）となる。ここで、速度および加速度が小さいとすると
（ｓ，ｓ²をほぼ０とする）、上式（１）は、ｙ／τ＝１／Ｋｔ・Ｋｐ・Ｋｖ …（２）となり、あるトルクτを与えた場合のモータの移動量ｙ
は、位置ループゲインＫｐと速度ループゲインＫｖに比
例する。

【００１０】柔軟制御のフローティング制御の柔らかさ
の程度は、駆動体にかかる力τと移動距離ｙの関係で示
されるｙ／τ（以下、バネ定数という）により表され
る。

【００１１】図５に示すロボット機構図において、例え
ば、Ｊ１軸とＪ２軸の２軸により構成される駆動体は、
Ｊ１軸がモータにより駆動されて、回転平面の中心点を
駆動体中心としてＪ１軸が回転し、そのＪ１軸に取り付
けられているＪ２軸の先端の駆動体先端がワークＷに当
接して力を作用する。

【００１２】ワークＷに対して力が印加されると、逆作
用によって駆動体の先端に力ｆがかかり、該力は駆動体
を介してモータに伝達されて、モータに外乱トルクτが
作用する。この駆動体の先端にかかる力ｆとモータに印
加される外乱トルクτとの間には、次の式（２）で表さ
れる関係がある。

【００１３】 τ＝Ｄ・ｉ・ｆ …（３）ここで、Ｄは駆動体中心から駆動体先端までの、回転平
面上での距離であり、ｉはモータ側での減速比、ｆは駆
動体先端にかかる力である。一方、モータ移動距離ｙと
駆動体の移動距離Ｙ（ｒａｄ）との間には、Ｙ＝ｉ・ｙ …（４）の関係がある。前記式（３），（４）から、駆動体先端
にかかる力ｆと駆動体の移動距離Ｙとの関係により表さ
れる柔軟制御のフローティング制御の柔らかさの程度で
ある駆動体のバネ定数Ｙ／ｆは、Ｙ／ｆ＝（Ｄ・ｉ²）／（Ｋｔ・Ｋｐ・Ｋｖ） …（５）となり、前記式（２）で示した駆動体にかかる力ｆとモ
ータの移動距離ｙの関係で示されるバネ定数は、トルク
定数Ｋｔや位置ループゲインＫｐや速度ループゲインＫ
ｖが一定であっても、先端距離Ｄに応じて異なることに
なる。したがって、駆動体の先端距離によって、駆動体
にかかわる柔軟制御のフローティング制御の柔らかさの
程度を表すバネ定数は異なり、駆動体の姿勢によって駆
動体にかかわるバネ定数を調整する必要がある。

【００１４】そこで、本発明は前記した従来の問題点を
解決して、人力によって被駆動体を移動させ、障害物か
らの容易な回避を可能とする柔軟なサーボ制御方法を提
供し、さらに、駆動体の姿勢にかかわらず、駆動体先端
でのバネ定数を常に一定に制御するサーボモータの制御
方法を提供することを目的とする。

【００１５】

【課題を解決するための手段】本発明は、位置制御ルー
プ及び速度制御ループを備える制御系で制御されるサー
ボモータの制御方法において、制御する駆動体の回転中
心から駆動体先端までの回転平面上における先端距離を
ソフトウェアから求め、その求めた先端距離に応じてバ
ネ定数が設定値となるように変更した位置ループゲイン
及び速度ループゲインによりサーボモータを駆動してサ
ーボ制御することにより、フローティングサーボ制御を
行うことにより、前記目的を達成する。本発明におい
て、駆動体は駆動体の回転中心と被駆動体と接触する駆
動体の駆動体先端との間の、駆動体の回転平面上におけ
る距離を先端距離とし、被駆動体に対して力を印加して
被駆動体を駆動するものであり、その先端距離は、駆動
体の姿勢変化に応じて変更されるものであり、例えば駆
動体を構成する軸の長さと軸間の相互の角度により表す
ことができ、駆動体の駆動を制御するソフトウェアから
求めることができる。また、本発明のバネ定数は位置制
御ループ及び速度制御ループを備える制御系において、
駆動体にかかる力と移動距離との関係により定まる定数
であり、サーボモータのトルク出力の柔らかさを表すも
のである。そして、本発明においては、駆動体の姿勢変
化にかかわらず、このバネ定数が設定した値を保つよう
に位置ループゲイン及び速度ループゲインを求め、この
位置ループゲイン及び速度ループゲインによりサーボ制
御を行うものである。

【００１６】また、本発明の位置ループゲインと速度ル
ープゲインの比を一定とすることにより、制御の安定性
を得ることができる。

【００１７】

【作用】本発明によれば、位置制御ループ及び速度制御
ループを備える制御系で制御されるサーボモータの制御
方法において、通常のサーボ制御のための移動指令、モ
ータ位置を求め、さらに、駆動体にかかる力と移動距離
との関係により定まる値であるサーボモータのトルク出
力の柔らかさを表す定数をあらかじめ設定し、制御する
駆動体の回転中心から駆動体先端までの回転平面上にお
ける先端距離をソフトウェアから求め、その求めた先端
距離に応じて設定したバネ定数が一定となるとなるよう
に位置ループゲイン及び速度ループゲインを変更し、そ
の変更したた位置ループゲイン及び速度ループゲインに
よりサーボモータを駆動してフローティングサーボ制御
を行う。

【００１８】本発明の先端距離は、駆動体を構成する軸
の長さと軸間の相互の角度により表すことができ、その
角度は駆動体の駆動を制御するソフトウェアから求める
ことができる。

【００１９】したがって、被駆動体を駆動体する駆動体
の姿勢が変化すると、その姿勢変化に応じて駆動体の回
転中心から駆動体先端までの回転平面上における先端距
離が変化する。制御系はこの先端距離に応じて、制御系
を構成する位置制御ループ及び速度制御ループの位置ル
ープゲイン及び速度ループゲインを、バネ定数が一定と
なるように変更し、その変更した値によりサーボ制御を
行う。これにより、バネ定数は常に一定となって、駆動
体にかかる力と移動距離との関係が常に一定となり、サ
ーボモータのトルク出力を設定された柔らかさに保持す
ることができる。

【００２０】

【実施例】以下、本発明の実施例を図を参照しながら詳
細に説明するが、本発明は実施例に限定されるものでは
ない。

【００２１】図１は、本発明のソフトウェアによるフロ
ーティングサーボ制御方法を実施するサーボ制御系のブ
ロック図である。図２に示す従来のサーボ制御系と相違
する点は、位置制御ループのポジションゲインＫｐ及び
速度制御ループゲインＫｖを駆動体の駆動中心からワー
クと接触する先端位置までの回転平面上における先端距
離Ｄに応じて変更して、常に設定されたバネ定数を保つ
ことにある。

【００２２】図３は、本発明の一実施例を実施するロボ
ット制御系のブロック図である。図３中、１０はロボッ
トを制御するホストコンピュータ、１１は該ホストコン
ピュータから出力される移動指令や制御信号を後述のデ
ィジタルサーボ回路のプロセッサに引渡し、逆にディジ
タルサーボ回路のプロセッサからの各種信号をホストコ
ンピュータ１１に引き渡すための共有メモリである。１
２は、上述したサーボ制御を実行するディジタルサーボ
（ソフトウェアサーボ）回路で、プロセッサ、ＲＯＭ、
ＲＡＭ等のメモリ等で構成される。１３は、ロボット１
４における欠くサーボモータの位置のフィードバック
値、速度のフィードバック値、電流のフィードバック値
等が書き込まれる帰還レジスタである。図５の駆動体
は、Ｊ１軸とＪ２軸の２軸により構成される駆動体の例
であり、Ｊ１軸がモータにより駆動されて、回転平面の
中心点を駆動体中心としてＪ１軸が回転し、そのＪ１軸
に取り付けられているＪ２軸の先端の駆動体先端がワー
クＷに当接して力を作用する。Ｊ２軸のＪ１軸に対する
姿勢は、Ｊ１軸の回転平面に対してＪ２軸のなす角度θ
により表される。ここで、Ｊ１軸のアーム長はＬ１、Ｊ
２軸のアーム長はＬ２により表されるものとする。

【００２３】本発明のソフトウェアによるフローティン
グ制御は、図中のＪ１軸の駆動により実施される。

【００２４】図５に示すロボット機構図において、前記
したように、駆動体先端にかかる力ｆ（例えば、ｋｇ）
と駆動体の移動距離Ｙ（例えば、ｃｍ）との関係により
表される柔軟制御のフローティング制御の柔らかさの程
度である駆動体のバネ定数Ｙ／ｆは、Ｄを駆動体中心か
ら駆動体先端までの回転平面上での距離、ｉをモータ側
での減速比、Ｋｐを位置ループゲイン、Ｋｖを速度ルー
プゲインとすると、Ｙ／ｆ＝（Ｄ・ｉ²）／（Ｋｔ・Ｋｐ・Ｋｖ） …（５）である。この式を位置ループゲインＫｐと速度ループゲ
インＫｖとの積の値について解くと、Ｋｐ・Ｋｖ＝（Ｄ・ｉ²）／｛Ｋｔ・（Ｙ／ｆ）｝ …（６）となる。ここで、バネ定数Ｙ／ｆをα（ｒａｇ／ｋｇ）
と設定した場合、前記式（６）は次式（７）となる。

【００２５】Ｋｐ・Ｋｖ＝（Ｄ・ｉ²）／（Ｋｔ・α） …（７）したがって、前記式（７）において、駆動体の姿勢が変
化して先端距離Ｄの値が変更されても、はじめに設定し
たバネ定数αの値が変化しないようにするためには、位
置ループゲインＫｐと速度ループゲインＫｖとの積の値
Ｋｐ・Ｋｖを先端距離Ｄの値に応じて前記（７）の右辺
に一致するように変更させればよい。

【００２６】一方、図に示す２軸の駆動体において、Ｊ
１軸は水平回転軸であり、駆動体の先端距離ＤはＪ２軸
の角度θとＪ２軸のアーム長Ｌ２に依存し、次式（８）
により表される。

【００２７】Ｄ＝Ｌ２・ｃｏｓθ …（８）また、バネ定数（＝Ｙ／ｆ）をα（ｒａｇ／ｋｇ）と設
定した場合の位置ループゲインｐと速度ループゲインＫ
ｖの関係は前記式（７），（８）からＫｐ・Ｋｖ＝（Ｄ・ｉ²）／（Ｋｔ・α）＝（Ｌ２・ｃｏｓθ・ｉ²）／（Ｋｔ・α） …（９）となる。この式（９）において、駆動体の姿勢変化は角
度θの角度変化として現れる。この角度θの変化により
先端距離Ｄの値が変更されても、はじめに設定したバネ
定数αの値が変化しないようにするためには、式（９）
の左辺の項である位置ループゲインＫｐと速度ループゲ
インＫｖとの積の値Ｋｐ・Ｋｖが、θの値に応じて変化
する式（９）の右辺の値となるように変更させればよ
く、さらに、その制御安定性を崩さないためには位置ル
ープゲインＫｐと速度ループゲインＫｖの比が一定であ
ることが条件となる。そこで、通常動作時における位置
ループゲインをＫｐｏ、速度ループゲインをＫｖｏとし
て、位置ループゲインＫｐと速度ループゲインＫｖの比
が一定となるように変更すると、Ｋｐｏ：Ｋｖｏ＝Ｋｐ：Ｋｖ …（１０）の関係から、Ｋｐｏ・Ｋｖ＝Ｋｖｏ・Ｋｐ …（１１）の条件式を満足する必要がある。この条件式（１１）と
前記式（９）から、位置ループゲインＫｐ及び速度ルー
プゲインＫｖについて解き、フローティング制御を行う
ときの位置ループゲイン、速度ループゲインをＫｐｆ、
Ｋｖｆとすると、それぞれ、Ｋｐｆ＝｛（Ｋｐｏ・ｉ²・Ｌ２・ｃｏｓθ）／（Ｋｖｏ・Ｋｔ・α）｝^1/2 …（１２）Ｋｖｆ＝｛（Ｋｖｏ・ｉ²・Ｌ２・ｃｏｓθ）／（Ｋｐｏ・Ｋｔ・α）｝^1/2 …（１３）となる。

【００２８】したがって、ソフトウェアによって、位置
ループゲインＫｐ及び速度ループゲインＫｖの値を、上
記式（１２），（１３）により設定される値に変更して
フローティング制御を行うことにより、あらかじめ定め
た値のバネ定数により希望とする柔らかさの制御を行う
ことができる。

【００２９】（本発明の実施例の作用）次に、前記した
図３のロボット制御系におけるディジタルサーボ回路１
２のプロセッサが実行する、本発明のソフトウェアによ
るフローティングサーボ制御の処理について、図５，６
に示す２軸の駆動体の例について、図４のフローチャー
トにおいてステップＳの符号を用いて説明する。

【００３０】なお、柔軟制御指令及び解除指令は手動で
オペレータに入力してもよく、また、あらかじめ柔軟な
制御を行うプログラム領域をプログラムで指定してもよ
い。また、通常のサーボ制御を行うときのポジションゲ
インＫｐ、速度ループゲインＫｖの値は、Ｋｐｏ、Ｋｖ
ｏとして設定されている。

【００３１】ロボットの動作を開始させると、ディジタ
ルサーボ回路１２のプロセッサは、位置・速度ループ処
理周期毎、図３に示す処理を実行する。

【００３２】ディジタルサーボ回路１２のプロセッサ
は、ホストコンピータ１０から共有メモリ１１を介して
送られてくる移動指令より移動指令ｒを読み込み（ステ
ップＳ１）、ロボット１４のモータ位置ｙを読み込む
（ステップＳ２）。このモータ位置ｙは、帰還レジスタ
１３に書き込まれている位置フィードバック値を用いる
ことができる。また、図示していない入力装置により共
有メモリ１１にバネ定数αを入力して、柔軟制御の柔ら
かさの程度の設定を行う（ステップＳ３）。次に、ホス
トコンピータ１０からＪ２軸のアーム角度θを取り込む
（ステップＳ４）。このＪ２軸のアーム角度θは、ワー
クに力を印加するための駆動体を駆動するために、ホス
トコンピータ１０から駆動体に送られる角度信号を用い
ることができる。前記ステップＳ４で取り込んだＪ２軸
のアーム角度θを用いて、駆動体の駆動体先端の位置を
算出する（ステップＳ５）。この駆動体の先端位置は、
図５，６において、駆動体の回転平面上における駆動体
中心から駆動体先端の投影位置までの距離Ｄとして求め
られ、次式により表される。

【００３３】Ｄ＝Ｌ２・ｃｏｓθ …（１４）なお、Ｌ２はＪ２軸のアーム長である。次に、式（１
４）で表される駆動体の先端距離Ｄを用いて、本発明の
フローティング制御を行うときの位置ループゲインＫｐ
ｆ、及び速度ループゲインＫｖｆを算出する（ステップ
Ｓ６）。この位置ループゲインＫｐｆ、及び速度ループ
ゲインＫｖｆの算出においては、前記式（１２），（１
３），及び式（１４）から得られる、Ｋｐｆ＝｛（Ｋｐｏ・ｉ²・Ｄ）／（Ｋｖｏ・Ｋｔ・α）｝^1/2 …（１２）Ｋｖｆ＝｛（Ｋｖｏ・ｉ²・Ｄ）／（Ｋｐｏ・Ｋｔ・α）｝^1/2 …（１３）の式を用いて行うことができる。なお、ｉは減速比であ
る。これにより、共有メモリ１１には、通常の制御を行
う場合の位置ループゲインＫｐｏ、及び速度ループゲイ
ンＫｖｏと、フローティング制御を行うときの位置ルー
プゲインＫｐｆ、及び速度ループゲインＫｖｆが格納さ
れることになる。

【００３４】前記ステップＳ６までの工程により、サー
ボモータの制御を行う場合において、通常の制御に用い
る位置ループゲインＫｐｏ、及び速度ループゲインＫｖ
ｏと、フローティング制御に用いる位置ループゲインＫ
ｐｆ、及び速度ループゲインＫｖｆのゲイン値が準備で
きたことになる。そこで、次に、サーボモータの制御に
おいて、通常の制御を行うか、あるいはループゲインを
駆動体の姿勢に応じて変更するフローティング制御を行
うかの判定を行い（ステップＳ７）、その判定に応じて
位置ループゲイン値の設定を行う（ステップＳ８，
９）。ステップＳ７の判定は、ホストコンピータ１０か
らの柔軟制御指令、あるいは手動によるオペレータから
の柔軟制御指令の有無を判断することにより行うことが
でき、例えば、共有メモリ１１内に設けたフラグを柔軟
制御指令、及び解除指令によりセット、リセットし、そ
のフラグを監視することにより行うことができる。

【００３５】前記ステップＳ７の判定において、フロー
ティング制御によりループゲインを駆動体の姿勢に応じ
て変更する場合には、フローティング制御に用いる位置
ループゲインＫｐｆをＫｐの値とし（ステップＳ８）、
一方、通常制御を行う場合には通常の位置ループゲイン
ＫｐｏをＫｐの値とし（ステップＳ９）、それぞれの値
を共有メモリ１１から読み出して設定する。

【００３６】次に、前記ステップＳ１で読み込んだ移動
指令ｒから前記ステップＳ２で読み込んだモータ位置ｙ
を減算して位置偏差ｅを求め、その位置偏差ｅに前記ス
テップＳ８，９で設定した位置ループゲインＫｐを乗算
して速度指令ｖｃｍｄを算出する（ステップＳ１０）。
次に、モータ速度の取込みを行う（ステップＳ１１）。
このモータ速度の取込みは、例えば、帰還レジスタ１３
内に書き込まれている速度フィードバック値を用いるこ
とができる。そして、速度ループゲインの設定を行うた
めに、サーボモータの制御において、通常の制御を行う
か、あるいはループゲインを駆動体の姿勢に応じて変更
するフローティング制御を行うかの判定を行い（ステッ
プＳ１２）、その判定に応じて速度ループゲイン値の設
定を行う（ステップＳ１３，１４）。ステップＳ１２の
判定は、前記ステップＳ７と同様に行うことができる。
前記ステップＳ１２の判定において、フローティング制
御によりループゲインを駆動体の姿勢に応じて変更する
場合には、フローティング制御に用いる速度ループゲイ
ンＫｖｆをＫｖの値とし（ステップＳ１３）、一方、通
常制御を行う場合には通常の速度ループゲインＫｖｏを
Ｋｖの値とし（ステップＳ１４）、それぞれの値を共有
メモリ１１から読み出して設定する。

【００３７】次に、前記ステップＳ１０で算出した速度
指令ｖｃｍｄから前記ステップＳ１１で読み込んだモー
タ速度ｖを減算して速度偏差を求め、その速度偏差に前
記ステップＳ１３，１４で設定した速度ループゲインＫ
ｖを乗算してトルク指令Ｔｃｍｄを算出する（ステップ
Ｓ１５）。この算出したトルク指令Ｔｃｍｄをアンプ等
に出力して、電流ループに引渡し（ステップＳ１６）、
当該位置・速度ループ処理周期の処理を終了する。

【００３８】前記位置・速度ループ処理を所定周期毎に
繰り返すことにより、ソフトウェアによるフローティン
グサーボ制御が行われ、駆動体の姿勢に応じて先端位置
を求めて、その先端位置に応じた位置ループゲインと速
度ループゲインによりサーボ制御を行い、設定したバネ
定数の柔らかさを実施することができる。

【００３９】（他の実施例の作用）次に、図７，８に示
す３軸の駆動体の例において、本発明のソフトウェアに
よるフローティングサーボ制御の処理を行う場合につい
て説明する。

【００４０】図７，８に示す３軸の駆動体は、ＪＩ軸，
Ｊ２軸，及びＪ３軸からなり、各軸が同一平面上にある
例であり、ＪＩ軸はモータに接続されて該軸を駆動体中
心として回転する。ＪＩ軸の先端にはＪ２軸が接続さ
れ、さらに該Ｊ２軸の先端にはＪ３軸が接続され、Ｊ３
軸の先端を駆動体先端としてワークＷと当接する。そし
て、Ｊ２軸のＪＩ軸に対する取付け角度は回転平面に対
してθ２であり、Ｊ３軸のＪ２軸に対する取付け角度は
回転平面に対してθ３である。したがって、この駆動体
の姿勢は、この角度θ２，及びθ３により決定されるこ
とになる。なお、Ｊ２軸，及びＪ３軸のアーム長は、そ
れぞれＬ２，及びＬ３とする。この駆動体における先端
距離Ｄは、図においてＪ２軸，及びＪ３軸のアーム長を
回転平面に投影した長さＤ２，Ｄ３の和であり、Ｄ＝Ｄ２＋Ｄ３ …（１４）により表される。

【００４１】なお、Ｄ２，及びＤ３は次式で表される。

【００４２】Ｄ２＝Ｌ２・ｃｏｓθ２ …（１５）Ｄ３＝Ｌ３・ｃｏｓθ３ …（１６）この駆動体を用いて、本発明のソフトウェアによるフロ
ーティングサーボ制御の処理は、前記実施例の作用で説
明したフローチャートとほぼ同様にして行われる。前記
作用のフローチャートとの相違は、ステップＳ４のアー
ム角度θの取込みと、ステップＳ５の先端位置の算出で
ある。

【００４３】アーム角度θの取込みにおいては、Ｊ２軸
のＪＩ軸に対する取付け角度を表す角度θ２と、Ｊ３軸
のＪ２軸に対する取付け角度を表す角度θ３を取り込む
ことにより行うことができ、また、先端位置の算出にお
いては、前記式（１５），（１６），及び（１４）を用
いて行うことができる。

【００４４】さらに、別の駆動体に例について、図９，
１０に示す３軸の駆動体の例において、本発明のソフト
ウェアによるフローティングサーボ制御の処理を行う場
合について説明する。

【００４５】図９，１０に示す３軸の駆動体は、ＪＩ
軸，Ｊ２軸，及びＪ３軸からなり、Ｊ３軸はＪＩ軸とＪ
２軸で形成される平面に対して角度を有して姿勢の例で
あり、ＪＩ軸はモータに接続されて該軸を駆動体中心と
して回転する。そして、ＪＩ軸の先端にはＪ２軸が接続
され、さらに該Ｊ２軸の先端にはある角度をなしてＪ３
軸が接続され、そのＪ３軸の先端を駆動体先端としてワ
ークＷと当接する。そして、Ｊ２軸のＪＩ軸に対する取
付け角度は回転平面に対してθ２であり、Ｊ３軸のＪ２
軸に対する取付け角度は回転平面に対してθ３で、Ｊ１
軸とＪ２軸とで形成される平面に対するＪ３軸の取付け
角度はψ３である。したがって、この駆動体の姿勢は、
この角度θ２，θ３，及びψ３により決定されることに
なる。なお、Ｊ２軸，及びＪ３軸のアーム長は、それぞ
れＬ２，及びＬ３とする。この駆動体における先端距離
Ｄは、図においてＪ２軸のアーム長Ｌ２を回転平面に投
影した長さＤ２と、Ｊ３軸のアーム長Ｌ３のＪ１軸とＪ
２軸とで形成される平面へ投影した長さをさらに回転平
面に投影した長さＤ３の和となり、Ｄ＝Ｄ２＋Ｄ３＝Ｌ２・ｃｏｓθ２＋Ｌ３・ｃｏｓθ３・ｃｏｓψ３ …（１７）により表される。

【００４６】この駆動体を用いて、本発明のソフトウェ
アによるフローティングサーボ制御の処理は、前記実施
例の作用で説明したフローチャートとほぼ同様にして行
われる。前記作用のフローチャートとの相違は、ステッ
プＳ４のアーム角度θの取込みと、ステップＳ５の先端
位置の算出であり、アーム角度θ，ψの取込みにおいて
は、Ｊ２軸のＪＩ軸に対する取付け角度を表す角度θ２
と、Ｊ３軸のＪ２軸に対する取付け角度を表す角度θ３
と、Ｊ１軸とＪ２軸とで形成される平面に対するＪ３軸
の取付け角度を表すψ３を取り込むことにより行うこと
ができる。

【００４７】したがって、前記実施例の作用の説明で
は、駆動体が２軸の場合、３軸の場合を例として説明し
ているが、多軸の場合においても同様にして、ホストコ
ンピータ１０から駆動体に送られる角度信号等によりソ
フトウェアからの制御信号を用いることにより軸相互の
位置関係を表す角度を逐次取込んで駆動体の姿勢を求
め、その姿勢における位置ループゲインと速度ループゲ
インを求め、その求めたゲインにより制御を行うことが
でき、これにより、駆動体の姿勢変化した場合でも設定
したバネ定数により所望の柔らかさによる制御を行うこ
とができる。

【００４８】

【発明の効果】以上説明したように、本発明によれば、
サーボモータの制御方法において、人力によって被駆動
物を移動させ、障害物からの容易な回避を可能とする柔
軟なサーボ制御を可能とし、さらに、駆動体の姿勢にか
かわらず、駆動体先端でのバネ定数を常に一定に制御す
ることができる。

【図面の簡単な説明】

【図１】本発明のソフトウェアによるフローティングサ
ーボ制御方法を実施するサーボ制御系のブロック図であ
る。

【図２】従来のサーボ制御系のブロック図である。

【図３】本発明の一実施例を実施するロボット制御系の
ブロック図である。

【図４】本発明の実施例における位置・速度ループ処理
のフローチャートである。

【図５】本発明のロボット機構の一駆動体図である。

【図６】本発明のロボット機構の同駆動体の平面図であ
る。

【図７】本発明のロボット機構の他の駆動体図である。

【図８】本発明のロボット機構の同駆動体の平面図であ
る。

【図９】本発明のロボット機構の他の駆動体図である。

【図１０】本発明のロボット機構の同駆動体の平面図で
ある。

【符号の説明】

１位置ループのボジションゲインの項２速度ループゲインの項３，４モータの伝達関数の項５モータ速度からモータ位置を求める伝達関数１０ホスト１１共有メモリ１２ディジタルサーボ回路１３帰還レジスタ１４ロボットＫｐポジションゲインＫｖ速度ループゲインＫｔトルク定数Ｊイナーシャ

─────────────────────────────────────────────────────

【手続補正書】

【提出日】平成６年２月８日

【手続補正１】

【補正対象書類名】図面

【補正対象項目名】全図

【補正方法】変更

【補正内容】

【図１】

【図２】

【図３】

【図５】

【図４】

【図６】

【図７】

【図８】

【図９】

【図１０】

Claims

【特許請求の範囲】

【請求項１】位置制御ループ及び速度制御ループを備
える制御系で制御されるサーボモータの制御方法におい
て、制御する駆動体の回転中心から駆動体先端までの回
転平面上における先端距離をソフトウェアから求め、該
先端距離に応じてバネ定数が設定値となるように変更し
た位置ループゲイン及び速度ループゲインによりサーボ
モータを駆動することによってサーボ制御することを特
徴とするソフトウェアによるフローティングサーボ制御
方法。
【請求項２】前記位置ループゲインと速度ループゲイ
ンの比は一定である請求項１記載のソフトウェアによる
フローティングサーボ制御方法。
【請求項３】前記先端距離は駆動体を構成する軸間の
角度により求める請求項１，２記載のソフトウェアによ
るフローティングサーボ制御方法。