JP2864461B2

JP2864461B2 - 位置制御方法およびその位置制御装置

Info

Publication number: JP2864461B2
Application number: JP12046589A
Authority: JP
Inventors: 聡小松; 智規倉富; 池田　　智夫
Original assignee: SHICHIZUN TOKEI KK
Current assignee: SHICHIZUN TOKEI KK
Priority date: 1989-05-16
Filing date: 1989-05-16
Publication date: 1999-03-03
Anticipated expiration: 2014-03-03
Also published as: JPH02300808A

Description

【発明の詳細な説明】〔産業上の利用分野〕本発明は工作機械、自動機械、産業用ロボット、建設
機械に代表される産業機械及び家庭用ロボット、医療機
器、民生機器の運動軸を駆動する駆動装置の位置制御方
法および位置制御装置に関する。

〔従来の技術〕

従来の位置制御装置は、「メカトロニクスのためのサ
ーボ技術入門」（日刊工業）等記述されているように、
その主たる目的は、サーボモータの回転角速度をサーボ
モータに取り付けられた負荷の摺動部摩擦、作業による
外力の存在にも関わらず指令された値に偏差なく追従さ
せることであった。

一方、産業用ロボット等の行う動作の中には、運動軸
が外乱トルクに対して位置偏差なく追従することが不適
当な場合が存在する。例えば、産業用ロボットが部品を
把持し、平面上に置かれた他の部品への組み付け作業を
行おうとするとき、部品同士の接触による拘束のもたら
す外乱トルクに対し柔軟に運動する動作を行わせる必要
がある。

従来の位置制御型サーボモータ制御装置では、制御の
目的が外乱トルクに対し、位置偏差なく動作することに
あるので、当然、サーボモータが外乱トルクに対し、柔
軟に回転することは許されない。従って運動軸が拘束を
伴うような接触作業を行っている場合に、作業対象物の
破壊、サーボモータ制御装置の過負荷による異常停止等
を引き起こす。あるいは、これを機械的に回避しようと
すれば、運動軸にバネをつける等の特殊な工夫を必要と
し、コスト高、メインテナンスが大変になる、あるいは
仕様変更が容易でないといった問題があった。また、制
御系を変更し、外乱トルクによる位置偏差を許すことに
よって、前記拘束の問題を解決しようとすれば、運動軸
の摩擦トルク等によっても位置偏差が生じることにな
り、指令値に対する追従性が不必要に悪化するという問
題があった。

この問題に対して、サーボモータの出力軸が外部から
受ける力（以降、外力と称する）を検出する力センサを
設け、このセンサからの出力を制御系に入力し処理する
事により、外部から受ける力に対して所与の動的特性を
有する運動を行わせようとする、コンプライアンス制御
という技術が開発されている。この技術を従来の位置制
御型サーボモータ制御装置で実現する典型的な方法を、
以下に説明する。

（１）コンプライアンス制御系の構成（１−１）制御系全体第12図は、コンプライアンス制御系のブロック図であ
る。位置制御系205は、位置指令値203にしたがって、サ
ーボモータの位置206を偏差なく追従させる。コンプラ
イアンスモデル211aは、力センサ出力値（Fc）208か
ら、変更位置指令204aを計算する。位置指令値203は、
上位コントローラ（図示せず）からの指令値201に変更
位置指令値204aを加算して得られる。以降の説明の都合
上、経路300とFd209が書いてあることがここでは、一旦
除外しておく。

この制御系の考え方の骨子は、次のようなものであ
る。まず位置制御系205が理想的に設計されており、位
置指令値203からサーボモータの位置206に至る伝達関数
が１、つまり位置指令値203はサーボモータの位置に完
全に一致すると仮定してよいものとする。すると第12図
より明らかなように、力センサ出力値208からサーボモ
ータの位置206に至る伝達関数は、コンプライアンスモ
デル211aの伝達関数と完全に一致する。すなわち、力セ
ンサ出力値208を外力と等価とみなせば、サーボモータ
は、外力に対して、コンプライアンスモデル211aで記述
された通りの動的特性をもつのである。つまり、コンプ
ライアンスモデルを任意に設定できるようにしておけ
ば、サーボモータの動的特性を自由に制御できることに
なる。

また、コンプライアンスモデルとしては通常、人間が
感覚的によく理解できるバネ，質量，ダンパから成る力
学系のモデル（これを以後一自由度振動モデルと称す
る）が使用される。

しかしながらこの方法は、構造的に困難な問題を内在
しており、実用に供することが難しかった。これを示す
ために、更に詳しく制御系の特性を説明する。

（１−２）位置制御系位置制御系205のブロック線図を第10（ａ）図に示
す。この位置制御系は一般に用いられているものであ
る。サーボモータへの位置指令値203は、位置206を減じ
られて、位置偏差152となり、位置制御補償器143を通っ
て速度指令151となる。さらに速度指令151は、位置206
の微分値である速度150が減じられて、速度偏差149とな
る。速度偏差149は速度制御補償器142を通って電流指令
値131となる。この指令により、電流制御系135（後述
（１−３））は、モータ電流136をDCサーボモータに流
し、DCサーボモータは、モータトルク157を発生する。
負荷慣性を含めたモータ慣性141は、モータトルク157と
外乱トルク148の合成トルク147により、加速され、モー
タ速度146で回転し、その積分値が位置206となる。

図中のパラメータは、以下の通りである。

Kp ：位置制御補償器比例ゲイン Kv ：速度制御補償器比例ゲイン Tv ：速度制御補償器積分時定数 Ka ：電流制御系ゲインＫτ：トルク定数Ｊ :DCサーボモータ回転子と負荷のモータ軸まわり
換算の慣性モーメントの和ｓ：複素変数位置制御系205において、速度制御補償器142に積分器
が存在するので、外乱トルク148が定常的であれば、モ
ータ速度146や位置206に影響しない。また外乱トルクが
時間的に変化する場合についても、十分にKp、Kvが大き
くとれていれば、わずかな影響しか生じないので、外乱
トルク148の存在は無視してもよく、位置制御系は第10
図（ｂ）図に示すように一つのブロックにまとめること
ができる。位置指令値203から位置208までの伝達関数
は、次式である。

ここで H1＝Kv・Ka・Ｋτ/J H2＝K1/Tv である。

（１−３）電流制御系第11図は電流制御系のブロック線図である。電流指令
値131からモータ電流値136aを引いて、電流偏差130を
得、それを電流制御補償要素126に通してモータ電圧値1
29を得る。これは、物理的なモータ電圧13（第３図
（ａ）参照）に対応している。モータ電圧13をDCサーボ
モータ41（第３図（ａ）参照）に印加すると、DCサーボ
モータの逆起電力128を差し引いた電圧134がDCサーボモ
ータ41内のモータ電機子にモータ電流136を発生させ
る。

図中のパラメータは以下の通りである。

Ki:電流制御補償要素比例ゲイン Ti:電流制御補償要素積分時定数 L:モータ電機子インダクタンス R:モータ電機子抵抗Ｋτ：トルク定数Ｊ :DCサーボモータ回転子と負荷のモータ軸まわり
換算の慣性モーメントの和 s:複素変数なお、電流制御系は、Ki、Tiを適当に設定すると、速
度制御系に於ける動作周波数帯域について一定のゲイン
Kaで近似することができる。従って、第10図の電流制御
系142は、定数Kaで表現することができる。

（１−４）コンプライアンスモデルここで、第12図２のコンプライアンスモデル211aにつ
いて説明をする。今、バネ、質量、ダンパで構成された
一自由度振動系を考える。一自由度振動系の模式図を第
19図に示す。質量172は、バネ171とダンパ170によって
地面174に結合されている。質量172には、力F173が加え
られ、安定点から偏差X175だけ変位するとする。

図中のパラメータは以下の通りである。

M:質量 D:減衰係数 K:バネ定数この系の微分方程式は、次のようになる。

初期値＝０としてラプラス変換して、変形すると、次
式が得られる。

ここでＸ（ｓ）:X（ｔ）のラプラス変換Ｆ（ｓ）:F（ｔ）のラプラス変換あるいは次のようにも書ける。

さらに（５）式の二つの極（分母＝０の根）をα１、
α２とすると、次の式を得る。

コンプライアンスモデル211aは、（８）式をそのまま
用いている。すなわちサーボモータに、一自由度振動モ
デルと同一の動的特性を持たせようとするのである。

（１−５）機械系次に、コンポライアンス制御系の機械部分についての
具体例を第３図を用いて説明する。エンコーダ40は、DC
サーボモータ41の出力軸41aに結合され、出力軸41aは、
カップリング43を介してボールネジ46に結合している。
ボールネジ46は。リニアスライド44によって案内された
ナット45を直線上に駆動する。ナット45には取り付け治
具47を介し、ハンド49が取り付けてある。取り付け治具
47は、駆動方向に比較的柔らかい構造となっており、歪
ゲージ式の力センサ48が付けられている。従って、この
力センサは、駆動方向の力を検出する。リニアスライド
44とDCサーボモータ41、ボールネジ46は台42に固定され
ている。

モータ電圧13は、DCサーボモータ41のコイル41bに印
加され、電流センサ９は、モータ電流136を測定し、モ
ータ電流信号17を出力する。エンコーダ40は、モータ回
転角度を測定し、エンコーダ信号10を出力する。力セン
サ48は、ハンドに加えられた外力を測定し、外乱トルク
22を出力する。

〔発明が解決しようとする課題〕

第３図（ａ）、（ｂ）において、ハンド49でワーク50
をつかみ、DCサーボモータ41を回転させて別のワーク51
の上に載せるという接触作業を行う場合を考える。ある
段階に於て第３（ｂ）図の様にワーク50とワーク51が接
触する。この時、位置Xc206と力センサ出力値Fc208との
間には、次式で表現される関係が成立する。

Fc＝−Ke・Xc＋Fd −−−−−（６）式ここで Ke: 各ワーク、ハンド49、及び治具47等を総合し
た機械剛性の等価ばね定数 Fd: ワーク50とワーク51の接触以外の原因による
検出され得る外力すなわち、ワークどうしが接触すると
第12図破線300で示す経路が生じ、閉ループ301の安定性
が問題となる。そこで、その安定性を根軌跡を用いて検
討する。

第13（ａ）図は、閉ループ301に対する根軌跡（各極
（図中の×印）から出発する矢印）を概念的に書いたも
のである。図全体は実軸319、虚軸318を持つ複素平面
（ｓ−平面）であり、図中の極（×印）、零点（○印）
のグループ310は、位置制御系205の極、零点であり、極
（×印）のグループ311は、コンプライアンスモデル211
aの極である。

根軌跡によって閉ループの安定性を調べる手法は、一
般的に知られているので、詳細な説明は省くが、簡単に
いえばループゲイン（この場合はKc＊Ke）を小さい値か
ら大きくしてゆくとき、閉ループ伝達関数の極の位置が
どのように変化してゆくかを見るものであり、あるルー
プゲインにおいて複素平面の右半面（不安定領域と呼ば
れる）に極が現れれば、そのループゲインにおいて閉ル
ープは不安定、さもなくば安定であると判定することが
できる。

また、本明細書において明確な説明を行うために、根
軌跡の２つの性質を挙げる。（性質１）根軌跡は、一巡
伝達関数の極から出発し（ループゲイン＝０）、零点に
終わる（ループゲイン＝∞）。（性質２）一巡伝達関数
の極の数をｎ、一巡伝達関数の零点の数をｍとすると、
根軌跡の分枝の中で、（ｎ−ｍ）個は無限遠点の零点に
到達する。そのときの漸近線の傾き角は、である。根軌跡法の解説に関しては、例えば、相良節男
著「基礎自動制御」p.132（森北出版株式会社）を参照
されたい。

さて、第12図の閉ループ301に対する一巡伝達関数の
極と零点は、コンプライアンスモデル211aと位置制御系
205の極の零点の事であるから、前述の通り第13（ａ）
図に示されている。これに（性質１）（性質２）をあて
はめると、グループ301の実軸上の極は実軸上の零点に
漸近し、残る４つの極が、π/4,3π/4,5π/4,7π/4の傾
き角を持って、無限遠に遠ざかる。この事から、グルー
プ311の極すなわちコンプライアンスモデル211aの極
が、ループゲインの増加に伴い、根軌跡の分枝313,314
を辿って不安定領域309に突入し、閉ループ301は不安定
となることがわかる。

ハンド49及び治具37（第３図）の機械剛性が高ければ
高いほど、又ワーク50、51の機械剛性が高ければ高いほ
ど、Kcは大きくなる。前者の機械剛性は位置精度を保つ
ため、極力大きく設計するのが通常であり、ワーク50、
51は金属部品同士であるのが常であるから（これを剛体
接触と呼ぶ）後者の機械剛性は極めて大きい。つまり、
ほとんどの事例に於てKeは、ループゲインを限界値より
大きくしてしまい、閉ループ301は不安定となる。

閉ループ301が不安定となれば発振現象が起こり、ワ
ーク50とワーク51の接触と離脱が周期的に起こり、この
ままでは第３（ｂ）図の接触作業は、不可能となるばか
りでなく、ワーク50、51の破損を招く事もある。

本発明は、上記の事情に鑑みてなされたもので、位置
制御型の駆動装置に任意のコンプライアンスを付与し、
かつ剛体接触時の不安定振動が起こらないようにする制
御装置及び制御方法を提供する事を目的とするものであ
る。

〔課題を解決するための手段〕上記の目的を達成するため、本発明は一自由度振動系
の伝達関数に２つの零点を付け加えた伝達関数（（10）
式）をコンプライアンスモデルとした。

K1 ；コンプライアンスモデルゲイン定数 α１、α２；コンプライアンスモデル極 β１、β２；コンプライアンスモデル零点ｓ；複素変数〔作用〕第13（ｂ）図において、グループ310の極と零点は第1
3（ａ）図と変わらないか、第13（ｂ）図のグループ312
は、第13（ａ）図のグループ311に２個の零点（（10）
式のβ１、β２）を付け加えたものであり、これに前述
の（性質１）（性質２）をあてはめると、グループ310
の実軸上の極は実軸上の零点に漸近し、残る４つの極の
内２つが、グループ312の新たに付け加えられた零点に
吸収され、残る２つの極が、π/2,3π/2の傾き角を持っ
て無限遠に遠ざかる。

第13図（ｂ）においては、グループ312の極（（10）
式α１、α２）が零点に吸収される様子を示している。
これらの極は新たに追加した零点を引っ張られ、第13図
（ａ）の様に不安定領域309に突入する事がない。ま
た、グループ310の実軸上にない極は上下方向に飛散す
るため、不安定領域309に突入する事がなく、閉ループ3
01は常に安定である。

また、グループ312の極と零点が近すぎると、コンプ
ライアンスモデルの特性が一自由度振動計と特性と異な
ってしまうが、これはグループ312の零点と極の位置を
十分離すことによって回避される。

つまり、ワーク51、52（第３（ｂ）図）の接触時にル
ープゲインが極めて大きくなっても、コンプライアンス
制御系の安定性が確保され、予期せぬ発振現象を招くこ
となく、目的の接触作業を行わせることができるのであ
る。

〔実施例〕

以下、本発明による一実施例を図面を基に説明する。

（１）ハードウェア構成第２図は本発明によるサーボモータ制御装置のハード
ウェア構成図である。まず本制御装置は、ディジタルサ
ーボ方式を採用している。サーボCPU（Central Process
ing Unit）１には、積和演算を高速に行うことができる
DSP（Digital Signal Processor）を用い、サーボに必
要なその他の機能ブロックは、CPU1のI/Oポートに結合
されている。機能ブロックには、位置検出ブロック３、
PWM（駆動信号出力）ブロック５、電流検出ブロック
６、外乱トルク（外力）検出ブロック７、上位コントロ
ーラインターフェースブロック８がある。この他、モー
タに電力を供給するためにパワーアンプ４も備えてい
る。また、CPU1周辺のメモリ回路、アドレスデコーダ等
は、説明は省略した。

位置検出ブロック３はエンコーダ信号10を、電流検出
ブロック６はモータ電流信号17を、外乱トルク検出ブロ
ック７は力センサ出力22を、それぞれ制御対象及びセン
サ部80から入力する。

PWMブロック５はトランジスタスイッチング信号14を
パワーアンプ４に出力し、パワーアンプ４はトランジス
タスイッチング信号14に応じてモータ電圧13を制御対象
及びセンサ部80へ出力する。

上位コントローラインターフェースブロック８は、上
位コントローラ90から、コントローラ出力データ166を
入力し、上位コントローラ90へ、コントローラー入力デ
ータ165を出力する。

次に制御対象及びセンサ部80の具体例については、従
来の技術で第３（ａ）図を用いて説明した（１−５）機
械系と同一とする。

次に各機能ブロック機能を第４図から第９図を用いて
説明する。第４図は位置検出ブロック３の詳細図であ
る。エンコーダ信号10は、Ａ相、Ｂ相信号という90゜だ
け位相の異なる矩形波であり、これをデコーダ30にか
け、16ビットバイナリーアンプダウンカウンタ33をコン
トローラする信号、すなわち回転パルス31、回転方向32
を発生させる。16ビットバイナリーアップダウンカウン
タ33は、クロック39を回転パルス31、回転方向32に従っ
て計数し、モータ回転角度に対応する16ビットバイナリ
ーデータ34を生成する。これを16ビットフリップフロッ
プ付きバッファを通して位置データ12としてCPU1へ出力
する。なお、16ビット＝65536パルスを越える回転角度
が生じたとき、計数値が０に戻るが、その場合において
も、ソフトウェアにより正しく積算されるようにした。

第５図は、PWMブロックの詳細図である。DCサーボモ
ータ41（第３図（ａ））のモータ電圧13は、PWM（Pulse
Width Modulation）方式で出力している。CPU1から
は、モータ電圧値129を実現するPWMデータ16が出力さ
れ、このPWMデータ16は11ビットフリップフロップ60に
保持させる。11ビットフリップフロップの出力の内の10
ビットは、電圧パルスのデューティ比データ67で、パル
ス幅変調回路57の出力となる。パターン幅変調回路57
は、デューティ比データ67で表現されるデューティ比の
矩形波61を発生する。また、11ビットフリップフロップ
60の出力のうち、１ビットは正負符号68であり、これは
矩形波61と共に、符号制御回路55に入力され、トランジ
スタスイッチング信号14、14a〜14dになる。

第６図はパワーアンプ４の詳細図である。PWMブロッ
ク５の出力14、14a〜14dは、それぞれ、ゲートドライバ
ー118〜121に入力され、それらからの出力114〜117は、
Ｈブリッジを構成したパワーMOSFET110〜113のゲートを
ON、OFFする。そのスイッチングパターンは、片側スイ
ッチングパターンである。これによりDCサーボモータ41
（第３図（ａ））のモータ電圧13は、供給電圧Vsの振幅
を持つ矩形波電圧となり、近似的にモータ電圧値129を
具現化する。

第７図は、電流検出ブロック６の詳細図である。モー
タ電流信号17は、ローパスフィルタ兼ゲインアンプ81を
通って、信号82となり、サンプルホールダ83に入力され
る。サンプルホールダ83の出力ホールドデータ84は12ビ
ットA/Dコンバータ85のアナログ入力となる。12ビットA
/Dコンバータ85のディジタル86はバッファ87を介して電
流データ21として、CPU1へ出力される。電流検出時はサ
ンプルホールダ83によって12ビットA/Dコンバータ85の
アナログ入力をホールドした後、12ビットA/Dコンバー
タ85によりA/D変換する。

第８図は外乱トルク発生ブロック７の詳細図である。
これは、第７図と基本的に同じものでブロックへの入力
をモータ電流信号17から力センサ出力22に代えたもので
ある。このブロックによってCPU1へ外乱トルクデータ25
が出力される。

第９図は上位コントローラインターフェースブロック
８の詳細図である。CPU1から上位コントローラ90へのデ
ータ転送は出力データ161を出力用F1FO（ファーストイ
ンファーストアウト）メモリ163へ出力することによっ
て行われる。出力データ161が出力されると、上位コン
トローラ90へのデータの存在を示す出力フラグ167がセ
ットされ、上位コントローラ90に対し、受信処理を要求
する。上位コントローラ90は、出力用FIFOメモリ163か
ら、コントローラ入力データ165を読み出す。FIFO中の
全てのデータの読みだしが完了すると、出力フラグ167
は、リセットされて、データ転送は、完了する。

上位コントローラからCPU1へのデータ転送も、上位コ
ントローラ90が、入力用FIFOメモリ164に対して、コン
トローラ出力データ166を出力することによって、CPU1
へのデータの存在を示す入力フラグ168が、セットさ
れ、CPU1に対して、受信処理を要求する。CPU1が入力用
FIFOメモリ164から、全てのデータを入力データ162とし
て入力すると、入力フラグ168は、リセットされ、デー
タ転送は、完了する。

以上、本サーボモータ制御装置の一実施例としてハー
ドウェア構成をサーボに必要な機能ブロックを中心に説
明したが、細部の制御信号等に関して説明を省略した。
また、各機能ブロックや制御対象、センサの実施例はこ
れにとどまらず、各部の置き換えは考えられる。例えば
エンコーダは、Ａ相、Ｂ相出力のインクリメンタル型で
あるが、これをアブソリュート型に変更し、対応する位
置検出ブロックにすることは、本発明の主旨に反しな
い。

（２）制御系の構成本サーボモータ制御装置に於ける制御系の構成をブロ
ック図を基に説明する。本サーボモータ制御装置は、デ
ィジタル制御、即ち離散時間制御を行っている。サンプ
リング周期を0.19msecといった非常に短周期に設定し、
使用サーボ帯域500Hz以下においては、連続時間系で近
似できるようにして、設計を行った。従って、以下の説
明においても、連続時間系で近似した形で説明をする。
又、以下の説明に於いて、位置は、サーボモータの回転
角度をさし、速度は、回転角速度をさす。

（２−１）制御系全体第１図は、本発明サーボモータ制御装置の制御系概念
図である。位置制御系205は、位置指令値203にしたがっ
て、サーボモータ位置の206を偏差なく追従させる。コ
ンプライアンスモデル211は、力センサ出力値（Fc）208
から、変更位置指令値204を計算する。位置指令値203
は、上位コントローラ90からの指令値201（（３）ソフ
トウェア構成参照）に変更位置指令値205をka加算して
得られる。

（２−２）位置制御系本サーボモータ制御装置に於ける位置制御系は、従来
の技術で説明した（１−２）位置制御系と同一である。

（２−３）電流制御系本サーボモータ制御装置に於ける電流制御系は、従来
の技術で説明した（１−３）電流制御系と同一である。

（２−４）コンプライアンスモデルコンプライアンスモデル（第１図211）の構成例を、
第14（ａ）図に基づいて説明する。

力センサ出力値208を一自由度振動モデル211a（第12
図で説明したコンプライアンスモデルと同じモデル）に
入力し、モデル入力337を得る。モデル出力337をγ・ｓ
の特性（γは微分器ゲイン、ｓは複素変数）をもつ微分
器331によって微分し、モデル出力337に加算することに
よって、位相回復モデル出力336を得る。ここでモデル
出力337から位相回復モデル出力336までの伝達関数は１
＋γｓとなり、この伝達回数と一自由度振動モデル211a
は直列に接続されている。さらに、力センサ出力値208
を、バイパスゲインλ（330）倍して、バイパス出力335
を得る。さらに、位相回復モデル出力336とバイパス出
力335との和をとり、変更位置指令204bを得る。つまり
バイパスゲイン330は、力センサ出力値208から位相回復
モデル出力に至る経路と並列に接続される。さらに、上
位コントローラから指令値201に変更位置指令値204bを
加算し、位置制御系205の位置指令値203を得る。微分器
331や、バイパス出力335は、一自由度振動モデル211aの
位相遅れを回復させる働きがある。ブロック線図の変形
を行えば、第14（ｂ）になり、次式で表されるコンプラ
イアンスモデル211bが構成されたことになる。

（λs²＋（λ・２ζｃωｃ＋Kcωc²γ）ｓ＋（λωc²＋Kcωｃ））／（s²＋２ζωcs＋ωc²） −−−−−（７）式さきに述べた零点の選び方に適合するように微分器ゲ
イン、バイパスゲインを設定した。ωｃ＝6rad/sec,ζ
ｃ＝0.6の場合について、微分器ゲインγは、ωｃの0.1
倍とし、バイパスゲインは、0.01程度に設定した。この
値は、コンプライアンスモデル211bの特性と、コンプラ
イアンスモデル211aの特性とが、あまり離れないように
選んだものである。

実際は、制御系の各部に存在するモデル化されていな
い遅れ要素の影響で、根軌跡は、第13（ｂ）図のように
ならず、やはり、根の一部が、高ゲイン時に不安定域に
はいる。そこで、それらの遅れ要素を考慮して、根軌跡
を計算し直し、最も安定性が高いバイパスゲインを選択
すると共に、あるいは、実験的に、取り付け治具47（第
３図参照）の機械剛性をやや低くして、（６）式のKeが
発振限界のゲインを越えないようにする。

念のために言っておくと、（７）式は分母、分子とも
に複素変数ｓに関する２次式であり、分母＝９の根をα
１、α２、分子＝０の根をβ１、β２とすると、（７）
式は（10）式に変形することができる。

（３）ソフトウエア構成本サーボモータ制御装置のCPU1のソフトウエアは、第
15図に示すように、システム管理プログラム部520と制
御プログラム部521によって構成される。システム管理
プログラム部520は、割り込み処理の管理、メモリ管理
を受け持つ。制御プログラム部521は、サーボモータの
制御を行う部分で、システム管理プログラム部520によ
ってCPU1の内部タイマー割り込み処理の一つとして起動
される。すなわち、制御プログラム部521は、サンプリ
ング周期0.19msec毎に起動される。

制御プログラム部521をプログラム流れ図（第16図）
を基に説明する。まず、入力データ162（第２図参照）
を、入力する（入力510）。これには、上位コントロー
ラ90（第２図）からの指令値201（第14（ａ）図）が含
まれる。次に、外乱トルクデータ25（第２図参照）を、
力センサ出力値208（第12図（ａ）図参照）として、入
力する（入力511）。次に、一自由度振動モデル211a
（第14（ａ）図参照）の計算を行う（処理512）。次
に、微分要素331（第14（ａ）図参照）の計算を行う
（処理513）。次に、変更位置指令204bの計算を行い、
位置指令値203の計算を行う（処理514,515）。最後に位
置制御系205の処理を行い（処理516）、システム管理プ
ログラム部520（第15図参照）へ、リターンする。

位置制御系205の処理（処理516）の流れ図を第17図に
示す。まず、位置データ12（第２図参照）を位置206
（第10（ａ）図参照）として、入力する（入力501）、
つぎに速度を計算する（処理502）。次に、位置制御の
計算を行う（処理503）。次に速度制御の計算を行う
（処理504）。次に電流データ21を、モータ電流値136a
として、入力する（入力505）。次に電流制御の計算を
行う（処理506）。最後に、モータ電圧値129を実現する
PWMデータ16を出力する（出力507）。

〔発明の効果〕

コンプライアンスモデルとして一自由度振動モデルに
前記の条件を満足する零点を加えたことにより、理想と
する一自由度振動モデルの特性とコンプライアンスモデ
ルの応答特性をあまり変えずに、剛体接触時にも安定な
制御系を構成することが出来、位置制御装置に任意のコ
ンプライアンスを付与し、かつ接触作業を行わせること
が可能となった。

【図面の簡単な説明】

第１図は、本発明の制御系の概念図である。第２図は、
ハードウエア構成図である。第３図は、制御対象及びセ
ンサ部を示す図で、（ａ）は、全体図、（ｂ）は、作業
中の一状態を示す図である。第４図は、位置検出ブロッ
クの詳細図、第５図は、PWMブロックの詳細図、第６図
は、パワーアンプの詳細図、第７図は、位置検出ブロッ
クの詳細図、第８図は外乱トルク検出ブロックの詳細
図、第９図は上位コントローラインターフェースブロッ
クの詳細図である。第10（ａ）図は位置制御系のブロッ
ク線図で、第10（ｂ）はそれをひとつにまとめたブロッ
ク図、第11図は、電流制御系のブロック線図であり、第
12図は、従来のコンプライアンス制御系の一例のブロッ
ク線図で、第13（ａ）は、従来のコンプライアンス制御
系の根軌跡図である。第13図（ｂ）は、本発明のコンプ
ライアンス制御系の根軌跡図である。第14図は、（ａ）
本発明のコンプライアンス制御系のブロック線図であ
り、第14（ｂ）は、それをひとつにまとめたブロック図
である。第15図は、本発明サーボモータ制御装置のソフ
トウエア構成図である。第16図は、制御プログラム部の
プログラム流れ図である。第17図は、位置制御系の処理
のプログラム流れ図である。第18図は、従来のサーボモ
ータ制御装置の制御系のブロック線図であり、（ａ）
は、速度制御型であり、（ｂ）は、位置制御型である。
第19図は、一自由度振動モデルの模式図である。１……CPU、３……位置検出ブロック、４……パワーアンプ、５……PWMブロック、６……電流検出ブロック、７……外乱トルク検出ブロック、８……上位コントローラインターフェースブロック、 211……コンプライアンスモデル。

───────────────────────────────────────────────────── フロントページの続き (58)調査した分野(Int.Cl.⁶，ＤＢ名) G05D 3/12 B23Q 15/22

Claims

(57)【特許請求の範囲】

【請求項１】制御体の実位置をフィードバックして位置
指令に追従させる位置制御方法において、前記制御体にかかる外力を検出し、この外力を伝達関数 Kl;コンプライアンスモデルゲイン定数 α１、α2;コンプライアンスモデル極 β１、β2;コンプライアンスモデル零点 s;複素変数で表されるコンプライアンスモデルに入力し、このコン
プライアンスモデルによって得られる変更位置指令を前
記位置指令に加えることを特徴とする位置制御方法。
【請求項２】各種信号を入力し、これらの信号を用いて
演算を行い制御信号を出力するCPUと、制御体の位置信号を前記CPUに伝達する位置検出ブロッ
クと、前記制御体に作用する外力に対応する信号を前記CPUに
伝達する外力検出ブロックと、上位コントローラの位置指令を前記CPUに伝達する上位
コントローラインターフェースブロックと、前記制御信号をもとに前記制御体の駆動信号を出力する
駆動信号出力ブロックとを備え、前記CPUは、前記外力に対する信号を、伝達関数 Kl;コンプライアンスモデルゲイン定数 α１、α2;コンプライアンスモデル極 β１、β2;コンプライアンスモデル零点 s;複素変数で表されるコンプライアンスモデルに入力し、このコン
プライアンスモデルの出力である変更位置信号を演算に
よって求め、さらに前記CPUは、前記変更位置信号と前記位置指令と前記制御体の位置信号とを用いて演算し、前記制御信号を出力することを特徴とする位置制御装
置。