JP3151437B2 - 位置制御方法およびその位置制御装置 - Google Patents

Description

【発明の詳細な説明】

【０００１】

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

【０００２】

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

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

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

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

【０００６】（１） コンプライアンス制御系の構成 （１―１）制御系全体 図１２は、コンプライアンス制御系のブロック図であ
る。位置制御系２０５は、位置指令値２０３にしたがっ
て、サーボモータの位置２０６を偏差なく追従させる。
コンプライアンスモデル２１１aは、力センサ出力値
（Ｆｃ）２０８から、変更位置指令２０４ａを計算す
る。位置指令値２０３は、上位コントローラ（図示せ
ず）からの指令値２０１に変更位置指令値２０４ａを加
算して得られる。以降の説明の都合上、経路３００とＦ
d２０９が書いてあるがここでは、一旦除外しておく。

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

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

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

【００１０】（１−２）位置制御系 位置制御系２０５のブロック線図を図１０（ａ）に示
す。この位置制御系は一般に用いられているものであ
る。サーボモータへの位置指令値２０３は、位置２０６
を減じられて、位置偏差１５２となり、位置制御補償器
１４３を通って速度指令１５１となる。さらに速度指令
１５１は、位置２０６の微分値である速度１５０が減じ
られて、速度偏差１４９となる。速度偏差１４９は速度
制御補償器１４２を通って電流指令値１３１となる。こ
の指令により、電流制御系１３５（後述（１―３））
は、モータ電流１３６をＤＣサーボモータに流し、ＤＣ
サーボモータは、モータトルク１５７を発生する。負荷
慣性を含めたモータ慣性１４１は、モータトルク１５７
と外乱トルク１４８の合成トルク１４７により、加速さ
れ、モータ速度１４６で回転し、その積分値が位置２０
６となる。図中のパラメータは、以下の通りである。 Ｋｐ：位置制御補償器 比例ゲイン Ｋｖ：速度制御補償器 比例ゲイン Ｔｖ：速度制御補償器 積分時定数 Ｋａ：電流制御系 ゲイン Ｋτ：トルク定数 Ｊ ：ＤＣサーボモータ回転子と負荷のモータ軸まわり
換算の慣性モーメントの和 ｓ ：複素変数

【００１１】位置制御系２０５において、速度制御補償
器１４２に積分器が存在するので、外乱トルク１４８が
定常的であれば、モータ速度１４６や位置２０６に影響
しない。また、外乱トルクが時間的に変化する場合につ
いても、十分にＫｐ、Ｋｖが大きくとれていれば、わず
かな影響しか生じないので、外乱トルク１４８の存在は
無視してもよく、位置制御系は図１０（ｂ）に示すよう
に一つのブロックにまとめることができる。位置指令値
２０３から位置２０８までの伝達関数は、次式である。

【数３】 ここで Ｈ１ ＝ Ｋｖ・Ｋａ・Ｋτ／Ｊ Ｈ２ ＝ Ｈ１／Ｔｖ である。

【００１２】（１―３）電流制御系 図１１は電流制御系のブロック線図である。電流指令値
１３１からモータ電流値１３６ａを引いて、電流偏差１
３０を得、それを電流制御補償要素１２６に通してモー
タ電圧値１２９を得る。これは、物理的なモータ電圧１
３（図３（ａ）参照）に対応している。モータ電圧１３
をＤＣサーボモータ４１（図３（ａ）参照）に印加する
と、ＤＣサーボモータの逆起電力１２８を差し引いた電
圧１３４がＤＣサーボモータ４１内のモータ電機子にモ
ータ電流１３６を発生させる。

【００１３】図中のパラメータは以下の通りである。 Ｋｉ：電流制御補償要素 比例ゲイン Ｔｉ：電流制御補償要素 積分時定数 Ｌ：モータ電機子 インダクタンス Ｒ：モータ電機子 抵抗 Ｋτ：トルク定数 Ｊ ：ＤＣサーボモータ回転子と負荷のモータ軸まわり
換算の慣性モーメントの和 ｓ：複素変数

【００１４】なお、電流制御系は、Ｋｉ、Ｔｉを適当に
設定すると、速度制御系に於ける動作周波数帯域につい
て一定のゲインＫａで近似することができる。従って、
図１０の電流制御系１３５は、定数Ｋａで表現すること
ができる。

【００１５】（１−４）コンプライアンスモデル ここで、図１２のコンプライアンスモデル２１１aにつ
いて説明をする。今、バネ、質量、ダンパで構成された
一自由度振動系を考える。一自由度振動系の模式図を図
１９に示す。質量１７２は、バネ１７１とダンパ１７０
によって地面１７４に結合されている。質量１７２に
は、力Ｆ１７３が加えられ、安定点から偏差Ｘ１７５だ
け変位するとする。図中のパラメータは以下の通りであ
る。 Ｍ：質量 Ｄ：減衰係数 Ｋ：バネ定数 この系の微分方程式は、次のようになる。

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

【数５】 ここで Ｘ（ｓ）：Ｘ(t)のラプラス変換 Ｆ（ｓ）：Ｆ(t)のラプラス変換 である。あるいは次のようにも書ける。

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

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

【００１６】（１―５）機械系 次に、コンプライアンス制御系の機械部分についての具
体例を図３を用いて説明する。エンコーダ４０は、ＤＣ
サーボモータ４１の出力軸４１ａに結合され、出力軸４
１ａは、カップリング４３を介してボールネジ４６に結
合している。ボールネジ４６は、リニアスライド４４に
よって案内されたナット４５を直線上に駆動する。ナッ
ト４５には取り付け治具４７を介し、ハンド４９が取り
付けてある。取り付け治具４７は、駆動方向に比較的柔
らかい構造となっており、歪ゲージ式の力センサ４８が
付けられている。従って、この力センサは、駆動方向の
力を検出する。リニアスライド４４とＤＣサーボモータ
４１、ボールネジ４６は台４２に固定されている。

【００１７】モータ電圧１３は、ＤＣサーボモータ４１
のコイル４１ｂに印加され、電流センサ９は、モータ電
流１３６を測定し、モータ電流信号１７を出力する。エ
ンコーダ４０は、モータ回転角度を測定し、エンコーダ
信号１０を出力する。力センサ４８は、ハンドに加えら
れた外力を測定し、外乱トルク２２を出力する。

【００１８】

【発明が解決しようとする課題】図３（ａ）、（ｂ）に
おいて、ハンド４９でワーク５０をつかみ、ＤＣサーボ
モータ４１を回転させて別のワーク５１の上に載せると
いう接触作業を行う場合を考える。ある段階に於て図３
（ｂ）の様にワーク５０とワーク５１が接触する。この
時、位置Ｘｃ２０６と力センサ出力値Ｆｃ２０８との間
には、次式で表現される関係が成立する。

【数８】 ここで Ｋｅ ： 各ワーク、ハンド４９、及び治具４７等を総
合した機械剛性の等価ばね定数 Ｆｄ ： ワーク５０とワーク５１の接触以外の原因に
よる検出され得る外力 すなわち、ワークどうしが接触すると図１２破線３００
で示す経路が生じ、閉ループ３０１の安定性が問題とな
る。そこで、その安定性を根軌跡を用いて検討する。

【００１９】図１３（ａ）は、閉ループ３０１に対する
根軌跡（各極（図中の×印）から出発する矢印）を概念
的に書いたものである。図全体は実軸３１９、虚軸３１
８を持つ複素平面（ｓ−平面）であり、図中の極（×
印）、零点（○印）のグループ３１０は、位置制御系２
０５の極、零点であり、極（×印）のグループ３１１
は、コンプライアンスモデル２１１ａの極である。

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

【００２１】また、本明細書において明確な説明を行う
ために、根軌跡の２つの性質を挙げる。（性質１）根軌
跡は、一巡伝達関数の極から出発し（ループゲイン=
0）、零点に終わる（ループゲイン=∞）。（性質２）一
巡伝達関数の極の数をｎ、一巡伝達関数の零点の数をｍ
とすると、根軌跡の分枝の中で、（ｎ―ｍ）個は無限遠
点の零点に到達する。そのときの漸近線の傾き角は、

【数９】 である。根軌跡法の解説に関しては、例えば、相良節夫
著「基礎自動制御」p.132（森北出版株式会社）を参照
されたい。

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

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

【００２４】閉ループ３０１が不安定となれば発振現象
が起こり、ワーク５０とワーク５１の接触と離脱が周期
的に起こり、このままでは図３（ｂ）の接触作業は、不
可能となるばかりでなく、ワーク５０、５１の破損を招
く事もある。本発明は、上記の事情に鑑みてなされたも
ので、位置制御型の駆動装置に任意のコンプライアンス
を付与し、かつ剛体接触時の不安定振動が起こらないよ
うにする制御装置及び制御方法を提供する事を目的とす
るものである。

【００２５】

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

【数１０】 Ｋ１ ；コンプライアンスモデルゲイン定数 α１、α１ ； コンプライアンスモデル極 β１、β１ ； コンプライアンスモデル零点 ｓ ；複素変数

【００２６】図１３（ｂ）において、グループ３１０の
極と零点は図１３（ａ）と変わらないが、図１３（ｂ）
のグループ３１２は、図１３（ａ）のグループ３１１に
２個の零点（（９）式のβ１、β２）を付け加えたもの
である。これに前述の（性質１）（性質２）をあてはめ
ると、グループ３１０の実軸上の極は実軸上の零点に漸
近し、残る４つの極の内２つが、グループ３１２の新た
に付け加えられた零点に吸収され、残る２つの極が、π
/2,3π/2の傾き角を持って無限遠に遠ざかる。

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

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

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

【００３０】なお（９）式からなるコンプライアンスモ
デルは、伝達関数

【数１１】 で表される、一自由度振動モデルと、伝達関数 １ ＋ γs で表される微分要素との直列結合と、該直列結合と伝達
関数 λ で表される比例要素との並列結合で表すことができる。
ここにおいて、 Ｋｃ ; 一自由度振動モデルゲイン定数 ζｃ ; 減衰係数 ωｃ ; 固有角振動数 γ ; 微分要素ゲイン λ ; 比例要素ゲイン ｓ ；ラプラス演算子 である。

【００３１】

【発明の実施の形態】以下、本発明による一実施例を図
面を基に説明する。 （１）ハードウェア構成 図２は本発明によるサーボモータ制御装置のハードウェ
ア構成図である。まず本制御装置は、ディジタルサーボ
方式を採用している。サーボＣＰＵ（CentralProcessin
g Unit）１には、積和演算を高速に行うことができるＤ
ＳＰ（DigitalSignal Processor ）を用い、サーボに必
要なその他の機能ブロックは、ＣＰＵ１のＩ／Ｏポート
に結合されている。機能ブロックには、位置検出ブロッ
ク３、ＰＷＭ（駆動信号出力）ブロック５、電流検出ブ
ロック６、外乱トルク（外力）検出ブロック７、上位コ
ントローラインターフェースブロック８がある。この
他、モータに電力を供給するためにパワーアンプ４も備
えている。また、ＣＰＵ１周辺のメモリ回路、アドレス
デコーダ等は、説明を省略した。

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

【００３３】ＰＷＭブロック５はトランジスタスイッチ
ング信号１４をパワーアンプ４に出力し、パワーアンプ
４はトランジスタスイッチング信号１４に応じてモータ
電圧１３を制御対象及びセンサ部８０へ出力する。

【００３４】上位コントローラインターフェースブロッ
ク８は、上位コントローラ９０から、コントローラ出力
データ１６６を入力し、上位コントローラ９０へ、コン
トローラ入力データ１６５を出力する。

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

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

【００３７】図５は、ＰＷＭブロックの詳細図である。
ＤＣサーボモータ４１（図３（ａ））のモータ電圧１３
は、ＰＷＭ（Ｐｕｌｓｅ Ｗｉｄｔｈ Ｍｏｄｕｌａｔ
ｉｏｎ）方式で出力している。ＣＰＵ１からは、モータ
電圧値１２９を実現するＰＷＭデータ１６が出力され、
このＰＷＭデータ１６は１１ビットフリップフロップ６
０に保持させる。１１ビットフリップフロップの出力の
内の１０ビットは、電圧パルスのデューティ比データ６
７で、パルス幅変調回路５７の入力となる。パルス幅変
調回路５７は、デューティ比データ６７で表現されるデ
ューティ比の矩形波６１を発生する。また、１１ビット
フリップフロップ６０の出力のうち、１ビットは正負符
号６８であり、これは矩形波６１と共に、符号制御回路
５５に入力され、トランジスタスイッチング信号１４、
１４ａ〜１４ｄになる。

【００３８】図６はパワーアンプ４の詳細図である。Ｐ
ＷＭブロック５の出力１４、１４ａ〜１４ｄは、それぞ
れ、ゲートドライバー１１８〜１２１に入力され、それ
らからの出力１１４〜１１７は、Ｈブリッジを構成した
パワーＭＯＳＦＥＴ１１０〜１１３のゲートをＯＮ、Ｏ
ＦＦする。そのスイッチングパターンは、片側スイッチ
ングパターンである。これによりＤＣサーボモータ４１
（図３（ａ））のモータ電圧１３は、供給電圧Ｖｓの振
幅を持つ矩形波電圧となり、近似的にモータ電圧値１２
９を具現化する。

【００３９】図７は、電流検出ブロック６の詳細図であ
る。モータ電流信号１７は、ローパスフィルタ兼ゲイン
アンプ８１を通って、信号８２となり、サンプルホール
ダ８３に入力される。サンプルホールダ８３の出力ホー
ルドデータ８４は１２ビットＡ／Ｄコンバータ８５のア
ナログ入力となる。１２ビットＡ／Ｄコンバータ８５の
ディジタル出力８６はバッファ８７を介して電流データ
２１として、ＣＰＵ１へ出力される。電流検出時はサン
プルホールダ８３によって１２ビットＡ／Ｄコンバータ
８５のアナログ入力をホールドした後、１２ビットＡ／
Ｄコンバータ８５によりＡ／Ｄ変換する。

【００４０】図８は外乱トルク検出ブロック７の詳細図
である。これは、図７と基本的に同じものでブロックへ
の入力をモータ電流信号１７から力センサ出力２２に代
えたものである。このブロックによってＣＰＵ１へ外乱
トルクデータ２５が出力される。

【００４１】図９は上位コントローラインターフェース
ブロック８の詳細図である。ＣＰＵ１から上位コントロ
ーラ９０へのデータ転送は出力データ１６１を出力用Ｆ
ＩＦＯ（ファーストインファーストアウト）メモリ１６
３へ出力することによって行われる。出力データ１６１
が出力されると、上位コントローラ９０へのデータの存
在を示す出力フラグ１６７がセットされ、上位コントロ
ーラ９０に対し、受信処理を要求する。上位コントロー
ラ９０は、出力用ＦＩＦＯメモリ１６３から、コントロ
ーラ入力データ１６５を読み出す。ＦＩＦＯ中の全ての
データの読みだしが完了すると、出力フラグ１６７は、
リセットされて、データ転送は、完了する。

【００４２】上位コントローラ９０からＣＰＵ１へのデ
ータ転送も、上位コントローラ９０が、入力用ＦＩＦＯ
メモリ１６４に対して、コントローラ出力データ１６６
を出力することによって、ＣＰＵ１へのデータの存在を
示す入力フラグ１６８が、セットされ、ＣＰＵ１に対し
て、受信処理を要求する。ＣＰＵ１が入力用ＦＩＦＯメ
モリ１６４から、全てのデータを入力データ１６２とし
て入力すると、入力フラグ１６８は、リセットされ、デ
ータ転送は、完了する。

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

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

【００４５】（２―１）制御系全体 図１は、本発明サーボモータ制御装置の制御系概念図で
ある。位置制御系２０５は、位置指令値２０３にしたが
って、サーボモータの位置２０６を偏差なく追従させ
る。コンプライアンスモデル２１１は、力センサ出力値
（Ｆｃ）２０８から、変更位置指令値２０４を計算す
る。位置指令値２０３は、上位コントローラ９０からの
指令値２０１（（３）ソフトウェア構成参照）に変更位
置指令値２０４をｋａ加算して得られる。

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

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

【００４８】（２―４）コンプライアンスモデル コンプライアンスモデル（図１中２１１）の構成例を、
図１４（ａ）に基づいて説明する。

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

【数１２】

【００５０】さきに述べた零点の選び方に適合するよう
に微分器ゲイン、バイパスゲインを設定した。ωｃ＝６
rad/sec,ζｃ＝０．６の場合について、微分器ゲインγ
は、ωｃの０．１倍とし、バイパスゲインは、０．０１
程度に設定した。この値は、コンプライアンスモデル２
１１ｂの特性と、コンプライアンスモデル２１１ａの特
性とが、あまり離れないように選んだものである。

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

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

【００５３】（３）ソフトウエア構成 本サーボモータ制御装置のＣＰＵ１のソフトウエアは、
図１５に示すように、システム管理プログラム部５２０
と制御プログラム部５２１によって構成される。システ
ム管理プログラム部５２０は、割り込み処理の管理、メ
モリ管理を受け持つ。制御プログラム部５２１は、サー
ボモータの制御を行う部分で、システム管理プログラム
部５２０によってＣＰＵ１の内部タイマー割り込み処理
の一つとして起動される。すなわち、制御プログラム部
５２１は、サンプリング周期０．１９ｍｓｅｃ毎に起動
される。

【００５４】制御プログラム部５２１をプログラム流れ
図（図１６）を基に説明する。まず、入力データ１６２
（図２参照）を、入力する（入力５１０）。これには、
上位コントローラ９０（図２）からの指令値２０１（図
１４（ａ））が含まれる。次に、外乱トルクデータ２５
（図２参照）を、力センサ出力値２０８（図１２（ａ）
参照）として、入力する（入力５１１）。次に、一自由
度振動モデル２１１ａ（図１４（ａ）参照）の計算を行
う（処理５１２）。次に、微分要素３３１（図１４
（ａ）参照）の計算を行う（処理５１３）。次に、変更
位置指令２０４ｂの計算を行い、位置指令値２０３の計
算を行う（処理５１４，５１５）。最後に位置制御系２
０５の処理を行い（処理５１６）、システム管理プログ
ラム部５２０（図１５参照）へ、リターンする。

【００５５】位置制御系２０５の処理（処理５１６）の
流れ図を図１７に示す。まず、位置データ１２（図２参
照）を位置２０６（図１０（ａ）参照）として、入力す
る（入力５０１）。つぎに速度を計算する（処理５０
２）。次に、位置制御の計算を行う（処理５０３）。次
に速度制御の計算を行う（処理５０４）。次に電流デー
タ２１を、モータ電流値１３６ａとして、入力する（入
力５０５）。次に電流制御の計算を行う（処理５０
６）。最後に、モータ電圧値１２９を実現するＰＷＭデ
ータ１６を出力する（出力５０７）。

【００５６】

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

【図面の簡単な説明】

【図１】本発明の制御系の概念図である。

【図２】ハードウエア構成図である。

【図３】制御対象及びセンサ部を示す図で、（ａ）は、
全体図、（ｂ）は、作業中の一状態を示す図である。

【図４】位置検出ブロックの詳細図である。

【図５】ＰＷＭブロックの詳細図である。

【図６】パワーアンプの詳細図である。

【図７】位置検出ブロックの詳細図である。

【図８】外乱トルク検出ブロックの詳細図である。

【図９】上位コントローラインターフェースブロックの
詳細図である。

【図１０】位置制御系のブロック線図である。

【図１１】電流制御系のブロック線図である。

【図１２】従来のコンプライアンス制御系の一例のブロ
ック線図である。

【図１３ａ】従来のコンプライアンス制御系の根軌跡図
である。

【図１３ｂ】本発明のコンプライアンス制御系の根軌跡
図である。

【図１４ａ】本発明のコンプライアンス制御系のブロッ
ク線図である。

【図１４ｂ】本発明のコンプライアンス制御系の別表記
のブロック線図である。

【図１５】本発明サーボモータ制御装置のソフトウエア
構成図である。

【図１６】制御プログラム部のプログラム流れ図であ
る。

【図１７】位置制御系の処理のプログラム流れ図であ
る。

【図１８ａ】従来のサーボモータ制御装置の速度制御型
の制御系のブロック線図である。

【図１８ｂ】従来のサーボモータ制御装置の速度制御型
の制御系のブロック線図である。

【図１９】一自由度振動モデルの模式図である。

【符号の説明】

１ ＣＰＵ ２ 指令 ３ 位置検出ブロック ４ パワーアンプ ５ ＰＷＭブロック ６ 電流検出ブロック ７ 外乱トルク検出ブロック ８ 上位コントローラインターフェースブロック ９ 電流センサ １０ エンコーダ信号 １２ 位置データ １３ モータ電圧 １４、１４ａ〜１４ｄ トランジスタスイッチング信号 １５ 配線 １６ ＰＷＭデータ １７ モータ電流信号 ２１ 電流データ ２２ 外センサ出力 ２５ 外乱トルクデータ ３０ デコーダ ３１ 回転パルス ３２ 回転方向 ３３ １６ビットバイナリーアップダウンカウンタ ３４ １６ビットバイナリーデータ ３９ クロック ４０ エンコーダ ４１ ＤＣサーボモータ ４１ａ 出力軸 ４１ｂ コイル ４２ 台 ４３ カップリング ４４ リニアスライド ４５ ナット ４６ ボールネジ ４７ 取り付け治具 ４８ 力センサ ４９ ハンド ５０〜５２ ワーク ５５ 符号制御回路 ５７ パルス幅変調回路 ６０ １１ビットフリップフロップ ６１ 矩形波 ６７ デューティ比データ ６８ 正負信号 ８０ センサ部 ８１ ローパスフィルタ兼ゲインアンプ ８２ 信号 ８３ サンプルホルダ ８４ 出力ホールドデータ ８５ １２ビットＡ／Ｄコンバータ ８６ ディジタル出力 ８７ バッファ ９０ 上位コントローラ １１０〜１１３ パワーＭＯＳＦＥＴ １１４〜１１７ 出力 １１８〜１２１ ゲートドライバー １２６ 電流制御補償要素 １２８ 逆起電力 １２９ モータ電圧値 １３０ 電流偏差 １３１ 電流指令値 １３４ 電圧 １３５ 電流制御系 １３６、１３６ａ モータ電流 １４１ モータ慣性 １４２ 速度制御補償器 １４３ 位置制御補償器 １４６ モータ速度 １４７ 合成トルク １４８ 外乱トルク １４９ 速度偏差 １５０ 速度 １５１ 速度指令 １５２ 位置偏差 １５５ 接点 １５７ モータートルク １６１ 出力データ １６２ 入力データ １６３ 出力用ＦＩＦＯメモリ １６４ 入力用ＦＩＦＯメモリ １６５ コントローラ入力データ １６６ コントローラ出力データ １６７ 出力フラグ １６８ 入力フラグ １７０ ダンパ １７１ バネ １７２ 質量 １７３ 力 １７４ 地面 １７５ 偏差 ２０１ 上位コントローラからの指令値 ２０２ 接点 ２０３ 位置指令値 ２０４ａ、２０４ｂ 変更位置指令値 ２０５ 位置制御系 ２０６ サーボモータの位置 ２０８ 力センサ出力値 ２０９ Ｆｄ ２１１、２１１ａ、２１１ｂ コンプライアンスモデル ３００ 経路

───────────────────────────────────────────────────── フロントページの続き (58)調査した分野(Int.Cl.⁷，ＤＢ名) G05D 3/00 - 3/20

Claims (2)

(57)【特許請求の範囲】
1. 【請求項１】 制御体の実位置をフィードバックして位
   置指令に追従させる位置制御方法において、 前記制御体にかかる外力を検出し、この外力を伝達関数 【数１】 で表される、一自由度振動モデルと、伝達関数 １ ＋ γs で表される微分要素との直列結合と、該直列結合と伝達
   関数 λ で表される比例要素との並列結合で表されるコンプライ
   アンスモデルに入力し、このコンプライアンスモデルに
   よって得られる変更位置指令を前記位置指令に加えるこ
   とを特徴する位置制御方法。ここにおいて、 Ｋｃ ; 一自由度振動モデルゲイン定数 ζｃ ; 減衰係数 ωｃ ; 固有角振動数 γ ; 微分要素ゲイン λ ; 比例要素ゲイン ｓ ；ラプラス演算子
2. 【請求項２】 各種信号を入力し、これらの信号を用い
   て演算を行い制御信号を出力するＣＰＵと、 制御体の位置信号を前記ＣＰＵに伝達する位置検出ブロ
   ックと、前記制御体に作用する外力に対応する信号を前
   記ＣＰＵに伝達する外力検出ブロックと、上位コントロ
   ーラの位置指令を前記ＣＰＵに伝達する上位コントロー
   ラインターフェースブロックと、前記制御信号をもとに
   前記制御体の駆動信号を出力する駆動信号出力ブロック
   とを備え、前記ＣＰＵは、前記外力に対応する信号を、
   伝達関数 【数２】 で表される、一自由度振動モデルと、伝達関数 １ ＋ γs で表される微分要素との直列結合と、該直列結合と伝達
   関数 λ で表される比例要素との並列結合によって構成されるコ
   ンプライアンスモデルに入力し、このコンプライアンス
   モデルの出力である変更位置信号を演算によって求め、
   さらに前記ＣＰＵは、 前記変更位置信号と前記位置指令と前記制御体の位置信
   号とを用いて演算し、 前記制御信号を出力することを特徴とする位置制御装
   置。ここにおいて、 Ｋｃ ; 一自由度振動モデルゲイン定数 ζｃ ; 減衰係数 ωｃ ; 固有角振動数 γ ; 微分要素ゲイン λ ; 比例要素ゲイン ｓ ； 複素変数 である。