JPH0356084A - 減速器付きモータにより被駆動体に加わる実トルク測定方法及び位置と力のハイブリット制御方法 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 産業上の利用分野 本発明は、ロボット等において、組立て，ネジ締め，パ
リ取り等の作業を行う減速器付きモータの制御方法に関
する。

従来の技術 ロボット等においては、ロボットの各軸を駆動するモー
タは位置ループと該位置ループのマイナーループとして
速度ループ、該速度ループのマイナーループとして電流
ループを有する位置制御系で制御して位置の制御が行わ
れているが、力の制御は一般的に行われていない。

しかし、ロボット等の作業において、即ち、モータを駆
動制御して何らかの作業を行う場合、例えば、ネジ締め
，パリ取り作業のような場合には力の制御をも必要とす
る。さらには、組立て作業において穴に部品を嵌合する
ような場合、穴の位置に部品をロボット等により移動さ
せ位置決めし嵌合させるときに位置ずれが生じていると
、部品が穴に嵌合できず、かつ、ロボット等のモータは
部品を嵌合させるようにして位置偏差に応じて大きな力
を発生させ、結局アラームを出して停止するという事態
になる。このような時、力の制御をも可能にしておけば
このような事態を避けることも可能になる。

そこで、ロボットの関節にトルクセンサ等を取付け、各
自独自のカループを組んでカループの制御を行う方式も
採用されている。

発明が解決しようとする課題 上述したように、力の制御を行うためにロボットの関節
に特別なトルクセンサを取付けると、精度のよいトルク
センサは一般的にねじれ量や歪み量を測定してトルクを
測定するものであるから、トルクセンサ中に剛性の弱い
部分を持っており、そのため、系全体において剛性の弱
い部分を有することとなる。

また、ダイレクトドライブ方式は別として、ロボットの
各軸の駆動は減速器を介してモータによって駆動される
ものが一般的である。その結果、上述したようなトルク
センサを用いると、制御系中にはトルクセンサによる剛
性の弱さによるバネ系と減速器によるバネ系とにより、
制御系の伝達関数が非常に次数の高い特性となってしま
い、制御ループを組んだとき、安定に制御できるゲイン
の設定が非常に困難となる。

そこで、本発明の目的は、減速器を介して被駆動体をモ
ータで駆動するものにおいて、その制御系の伝達関数特
性を損うことなく、被駆動体に加わる実トルクを測定す
る方法を提供することにある。

さらに、本発明の目的は、制御系の伝達関数の特性を損
うことなく、力と位置の制御ができるハイブリッド制御
方法を提供することにある。

課題を解決するための手段 減速器を介してモータによって駆動される被駆動体にお
いて、本発明は、減速度の入力及び出力側に速度検出器
を取付け、両速度検出器で検出される速度の差．位置の
差及び出力側の速度検出器出力によって求められる加速
度によって被駆動体に加わる実トルクを測定する。

また、指令トルクと上記測定した実トルクによりカルー
プを構或すると共に、上記モータの制御として位置ルー
プと該位置ループのマイナーループとして速度ループ、
該速度ループのマイナーループとしての加速度ループを
持つ位置制御系を構威し、上記力ループから出力される
指令電圧と上記加速度ループから出力される指令電圧を
加算して上記モータへの指令電圧とすることにより位置
と力を制御することにより、上記課題を解決した。

作用 第１図は、本発明の作用を説明する説明図で、？中、符
号１はモータ、該モータ１のロータ軸には減速器４が接
続されると共に、減速器４の人力側の速度検出器として
モータ１のロータ軸に速度検出器２が接続され、また、
減速器４の出力側にも速度検出器３が取付けられ、被駆
動体５を減速器４を介してモータ１によって駆動するよ
うになっている。

減速器４のバネ定数をＫ１粘性係数をＣとし、減速器４
の入力側の速度検出器２から検出される減速器入力側の
位置，速度，加速度をθ、，，θ３，θ，，，減速器４
の出力側の速度検出器３から検出される減速器出力側の
位置，速度，加速度の入力側に換算した値を０１，θ■
，θ，とする。また、被駆動体５に直接及ぼすトルク（
加速度等を含まないもの）をτ、モータ１の軸イナーシ
ャをＪＭ，被駆動体５等の負荷イナーシャ（モータ軸換
算）ＪＴ，モータ１の出力トルクをＴとする。　そこで
、減速器４の入力側で運動方程式を立てると、次の第（
１）式となる。

Ｔ＝ＪＭ−θ、，＋（θ、，一〇〇）・Ｃ＋（θ８一０
１）・Ｋ・・・・・・（１） また、減速器４の出力側で運動方程式を立てると次の第
（２）式となる。

０−ＪＴ−０７＋（θ７−θＭ）・Ｃ＋（θ丁−θＭ）
・Ｋ＋て・・・・・・（２） 上記第（２）式を変形すると、次の第（３）式となり、
減速器４の入出力側の各速度検出器２，３から得られる
速度の差（θ、，一〇Ｔ）、速度を積分して得られる位
置の差（θ，．一θＴ）、出力側の速度検出器３からの
検出速度を微分して得られる加速度θ１と、減速器４の
粘性係数Ｃ，バネ定数Ｋ，負荷イナーシャＪｔより被駆
動体５に直接及ぼすトルクτを検出することができる。

τ＝（θエーθＴ）・Ｃ＋（θ．，一〇Ｔ）・Ｋ−ＪＴ
・θ．・・・・・・（３） また、第（１１式，第（２）式を加算すると次の第（４
）式が得られる。

Ｔ＝Ｊ，−θ＋ａ　＋　ＪＴ−／ｊＴ＋τ　　・・・・
・・（４）加速度θエ，ｅＴに大きな差はないとすると
、？記第（４）式は次の第（５）式となる。

Ｔ＝　（ＪＭ　＋　ＪＴ　）　　・ｌｊｙ　＋ｒ　　　
　−−（　５）そこで、（ＪＭ　＋ＪＴ　）　　・θ■
をモータの制御ループ内の加速度ループで補い、トルク
τに関しては指令トルクと第（３）式で検出されるトル
クτとのカループで補えば、指令トルクを被駆動体に及
ぼすようにモータ出力トルクをＴを制御できることとな
る。

実施例 本発明の一実施例を１軸制御のみのロボットについて説
明する。

この場合、第１図において、被駆動体５はロボットのア
ームとなる。また、速度検出器２，３とパルスコーダを
使用し、減速器４の出力側のパルスコーダ３は減速器４
の減速比に応じて、入力側のパルスコーダ３よりも分解
能の高いものを使用する。また、モータ１としてサーボ
モータを使用する。

第２図は本実施例におけるサーボモータ１の位置制御系
と力制御系のブロック線図である。第２？中において、
符号１０はサーボモータ１の位置制御系を示し、位置ル
ープとして比例制御（Ｐ制御）、位置ループのマイナー
ループとしての速度ループとして比例積分（Ｐ　Ｉ）制
御、速度ループのマイナーループとして加速度のＰＩ制
御ループを持つ例を示している。伝達関数１１のＫＰは
位置ループの比例ゲイン、伝達関数１２のＫｌ，Ｋ２は
速度ループの積分ゲイン，比例ゲイン、伝達関数１３の
Ｋ３，Ｋ４は加速度ループの積分ゲイン，比例ゲインで
ある。また、符号１４はサーボモータＩの伝達関数であ
り、Ｒはモータ巻線の抵抗、Ｌは巻線のインダクタンス
、Ｋｔはトルク定数、Ｊは全イナーシャである（Ｊ＝Ｊ
Ｍ　＋ＪＴ　）。符号１５は加速度を積分して速度を求
める伝達関数、符号１６は速度を積分して位置を求める
伝達関数である。

サーボモータ１の位置指令θ『とパルスコーダ２によっ
て求められるモータの位置θ■との差に位置ループゲイ
ンＫ，を乗じて速度指令θｒを求め、該速度指令θｒと
パルスコーダ２で検出され？サーボモータ１の実速度θ
■との差に対し伝達関数１２のＰＩ制御をなって、加速
度指令θｒを求める。そして、加速度ループのＰＩ制御
を行ってサーボモータへの位置制御系からの電圧指令Ａ
を求める。なお、伝達関数１４，１５．１６はブロック
線図上においてサーボモータ１の加速度θ、，速度θエ
，位置θ。を求める伝達関数である。

一方、カループ２０は上述した第（′Ａ）式の演算を行
ってアーム５に加わるトルク、すなわち、ロボットの作
業に直接及ぼすトルクτを求め、指令トルクτｒと該ト
ルクτとの差を伝達関数２１で示すようにＰＩ制御を行
ってカループからのサーポモータ１への電圧指令Ｂを求
める。

なお、伝達関数２２はカルーブにおけるサーボモータ部
の伝達関数を示し、Ｒは巻線抵抗、Ｌは巻線インダクタ
ンス、Ｋｔはトルク定数である。

上述したサーボモータ１の位置制御系１０からの電圧指
令Ａとカループからの電圧指令Ｂを加算し、サーボモー
タ１への電圧指令（＝Ａ＋Ｂ）としてＰＷＭ＄ｉ｛Ｊ御
を行ってサーボモータを駆動すれば、第（５）式に示す
ように、サーボモータ１の出力トルクはＴとなり、ロボ
ットのアーム５による作業に及ぼすトルクはτとなり、
指令トルクτｒに制御されることとなる。なお、伝達関
数３０，３１はモータ部の伝達関数であり、Ｒは巻線抵
抗、Ｌは巻線のインダクタンス、Ｋｔはトルク定数であ
る。

なお、加速度ループ及びカループの積分ゲイン，比例ゲ
インＫ３，Ｋ５，Ｋ４，Ｋ６は次のように設定する。

Ｋ３ヨ　　Ｊ−Ｒ Ｔα　●　Ｋｔ Ｋ４＝＝　　　Ｊ−Ｌ Ｔα　●Ｋｔ なお、 Ｔαは加速度ループの時定数、 Ｔｆは力 ループの時定数であり、加速度を安定した後、力を安定
させる必要からＴα＜＜Ｔｆとして干渉を防ぐようにす
る。

第２図において、加速度ループからの電圧指令Ａについ
て検討すると、次の第（６）式が成立する。

Ａ・　　　Ｉ　　　　　Ｋｔ　　＝θ　　・・・・・・
（６）Ｒ＋ＬＳ　　　　　　Ｊ　　　　　　　’上記第
（６）式より Ａ＝θ　（Ｒ＋ＬＳ）・　１　　　・・・・・・（７）
’　　　　　　　　　　　　　　Ｋｔ また、カループからの電圧指令Ｂは次のようになる。

Ｂ．　　Ｋｔ Ｒ＋ＬＳ　　＝・　　　　　−゜−　（８）故に、 Ｂ＝τ・　（Ｒ＋ＬＳ）／Ｋｔ　　　　　・・・・・・
（９）上記第（７）式，第（８）式で求められる電圧指
令Ａ，　　Ｂを加算してサーボモー夕への電圧指令とし
てサーボモータを駆動すると、サーボモータ１からの出
力トルクＴは次の第（１０）式のようになる。

Ｔ＝　（Ａ＋Ｂ） （　１　　）・Ｋｔ Ｒ＋ＬＳ Ｋｔ ＝（θ，　（Ｒ＋ＬＳ）　Ｊ／Ｋｔ＋τ●（Ｒ　＋　Ｌ
　Ｓ　）　／Ｋ　ｔ　）　Ｒ　＋Ｌ　Ｓ＝Ｊ●θ８＋τ ・・・・・・（ｌＯ） Ｊ＝ＪＭ　＋ＪＴであるから、上記第（１０）式は第（
５）式で示される式と同一となり、サーボモータ１から
は出力トルクＴが出力され、ロボットのアーム５が作業
する力τはカループで指令トルクτｒになるように制御
されることとなる。

第３図は上記実施例を実行するロボット制御系のブロッ
ク線図である。図中、４０はロボットの軸、すなわち、
アーム５を駆動するサーボモータ１への位置指令ｏｒを
出力する数値制御装置等のホストコンピュータ（以下、
ホストＣＰＵという）、４１はホストＣＰＵから書込ま
れた位置指令ｏｒを指令トルクτｒ等デジタルサーボ回
路４２のプロセッサへ受け渡す共有メモリである。また
、４２はデジタルシグナルプロセッサ等で構威されるデ
ジタルサーボ回路であり、ロボットのサーボモータ１の
位置，速度，電流のフィードバックループ制御を行うも
のである。４３は減速器４の人力側，出力側に設けられ
たバルスコーダ２，３テ検出された速度θ２，θ．等が
書込まれる帰還信号レジスタである。

なお、上記第３図に示すロボットの制御系は従来から公
知のデジタルサーボ制御の制御系と同一であり、帰還レ
ジスタ４３内に減速器４の人力側．出力側のパルスコー
ダ２，３からの速度信号θ．，θ７が書込まれるレジス
タ（従来は例えば速度信号θ８のレジスタのみ）が設け
られている点が相違するのみである。

第４図は上記デジタルサーボ回路４２のＣＰＵが加速度
ループ処理周期毎に実行する処理のフローチャートで、
デジタルサーボ回路４２のＣＰＵは従来と同様にホスト
ＣＰＵ４０から共有メモリ４１に書込まれた位置指令θ
ｒに基いて、位置ループ処理及び速度ループ処理を所定
周期毎実行し、第２図に示すように加速度指令θｒを算
出する。

？して、位置，速度ループ処理周期より早い周期の加速
度ループ処理周期毎第４図に示す処理を実行し、加速度
ループ処理，カループ処理を実行する。

まず、共有メモリ４１に書込まれているトルク指令τｒ
を読出すと共に、帰還レジスタ４３内に書込まれている
減速器４の出力側のパルスコーダ３からの速度信号（入
力側に換算した値）θアを読出す（ステップＳＬ）。次
に、速度信号θ１から減速器４の出力側の位置θ■，加
速度θ１を算出する（ステップＳ２）。すなわち、該加
速度ループ処理周期の各周期のステップＳ１で読取った
速度信号θ■を累積することによって速度信号θ１の積
分値として位置θ■を求め、また、前周期と今周期の速
度信号θ１の差によって速度信号θアを微分した加速度
θアを求める。

こうして求められた減速器４の出力側の位置θ１，速度
θア，加速度θ■と、サーボモータ１の位置，速度ルー
プ処理によって求められている減速器４の入力側の位置
θ．，速度θ■により第？３）式の演算を行ってトルク
τを求める（ステップＳ３）。次に、ステップＳ１で読
出したトルク指令τｒとステップＳ３で算出したトルク
τに対して第２図の伝達関数２１で示すＰＩ制御を行っ
てカループの電圧指令Ｂを求める（ステップＳ４）。即
ち、指令トルクτｒと算出トルクτの差を積分し（各周
期毎求められるトルク偏差（τｒ一τ）を累積して積分
値を求める）、その積分値に積分定数Ｋ５を乗じて得ら
れる値に指令トルクτｒと算出トルクτの差（τｒ一τ
）に比例定数Ｋ６を乗じて電圧指令Ｂを求める。

次に、減速器４の入力側、即ち、サーボモータ１のロー
タ軸に取付けられたパルスコーダ２から検出される速度
θ８を微分し（前周期と今周期の速度θ２の差）、加速
度θ、，を算出し（ステップＳ５）、速度ループ処理で
算出されている加速度指令θｒとステップＳ５で求めた
加速度θ■により第２図の伝達関数１３で示すＰＩ制御
処理を行って加速度ループによる電圧指令Ａを求める（
ステップＳ６）。そして、ステップＳ６及びステップＳ
５で求めた電圧指令Ａ，　　Ｂを加算して、この値（Ａ
＋Ｂ）を電圧指令として出力して（ステップＳ７）、当
該周期の加速度ループ処理を終了する。こうして出力さ
れた電圧指令に対し、従来と同様に、ＰＷＭ制御を行っ
てサーボモータ１を制御してアーム５の先端、即ち、作
業点の位置が指令位置θｒになるように位置制御を行う
と共に、ロボットがアーム５の先端で行う作業に及ぼす
力が指令トルクτｒになるように力制御を行うこととな
る。

なお、上記実施例では１軸のロボットの例を示したが、
多軸のロボットに対しても同様であり、各軸を駆動する
サーボモータに対して上述したような力と位置の制御を
行えばよい。また、ロボット以外のものにおいても、位
置と力の制御の制御を必要なものにおいては、上述した
ように減速器の人力側，出力側に各々パルスコーダを取
り付け、パルスコーダから検出される速度を人力側また
は出力側、どちらか一方へ換算して上述した処理を実行
させれば、位置と力の制御が可能となる。

発明の効果 本発明は、減速器の入力側と出力側にそれぞれ速度検出
器を取付けるのみで、特別なトルクセンサを設けること
なくモータによって駆動される被駆動体に加わるトルク
を検出できるようにしたから減速器のバネ系のみとなり
、制御系の伝達関数特性を損なうことなく安定した制御
のできるゲインの選択範囲が増え、力の早い応答性が得
られ、位置と力が同時に安定に制御できる。

【図面の簡単な説明】

第１図は、本発明の作用を説明する説明図、第２図は本
発明の一実施例の制御系のブロック線図、第３図は同実
施例における制御系のブロック図、第４図は同実施例に
おける加速度ループ処理周期の加速度ループとカループ
の処理を示すフローチャートである。 １・・・モータ（サーボモータ）、２・・・減速器入力
側の速度検出器（パルスコーダ）、３・・・減速器出力
側の速度検出器（パルスコーダ）、４・・・減速器、５
・・・被駆動体（アーム）、１０・・・位置制御系のブ
ロック線図、 ２０・・・カループのブロック線図。

Claims (2)

【特許請求の範囲】
1. （１）減速器を介してモータによって駆動される被駆動
   体において、減速度の入力及び出力側に速度検出器を取
   付け、両速度検出器で検出される速度の差、位置の差及
   び出力側の速度検出器出力によって求められる加速度に
   よって被駆動体に加わる実トルクを測定することを特徴
   とする実トルク測定方法。
2. （２）減速器を介してモータによって駆動される被駆動
   体において、減速度の入力及び出力側に速度検出器を取
   付け、両速度検出器で検出される速度の差、位置の差及
   び出力側の速度検出器出力によって求められる加速度に
   よって被駆動体に加わる実トルクを検出し、指令トルク
   と検出トルクにより力ループを構成すると共に、上記モ
   ータの制御として位置ループと該位置ループのマイナー
   ループとしての速度ループ、該速度ループのマイナール
   ープとしての加速度ループを持つ位置制御系を構成し、
   上記力ループから出力される指令電圧と上記加速度ルー
   プから出力される指令電圧を加算して上記モータへの指
   令電圧とすることにより位置と力を制御するようにした
   ハイブリット制御方法。