JPH0736545A - 単軸機構もしくは多軸機構の制御方法 - Google Patents

Abstract

(57)【要約】 【構成】位置出力６より速度相当量を演算し帰還する速
度相当量演算部９を、後退差分演算部１２と高次速度相
当量演算部１１を速度判定部１３で速度絶対値をしきい
値と比較し、しきい値より大きい場合は、１１を選択
し、小さい場合は、１２を選択する構成とすることによ
り高精度化を図った。 【効果】低速動作時の速度検出誤差は、後退差分法が、
多サンプル周期前位置利用法より良く、高速動作時の速
度検出誤差は、多サンプル周期前位置利用法が、後退差
分法より良いため、切り換えて速度検出を行うことによ
り、高精度の速度帰還が可能になり、動作時の整定時間
を短くすることができる。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は産業用ロボット，ディス
ク駆動装置ヘッド部など移動体の高速、高精度な位置決
めが要求される位置決め装置において、位置もしくは速
度を高速・高精度に制御する装置の速度検出方法に関す
る。

【０００２】

【従来の技術】従来、移動体の速度検出装置は、タコジ
ェネレータなどのアナログの速度検出装置が用いられて
きた。しかるに、機構の簡略化などの観点から、近年の
位置決め装置では、パルスエンコーダなどの位置検出装
置の検出信号から速度相当量を生成し、速度相当量をフ
ィードバックする方式がしばしば用いられる。

【０００３】そのような速度検出方法は、従来、ディジ
タル制御（昭和６０年，岩波書店）第８９頁〜第９０頁
に記載のように、一サンプル周期前の位置との差分によ
り求める方式（後退差分法）が提案されている。

【０００４】また、特開平3−228272 号公報では、位置
の一サンプル周期前の位置のみならず、二サンプル周期
前まで、三サンプル周期前まで、の位置の差分を用いて
速度を検出することにより（以下、二サンプル周期前位
置利用法，三サンプル周期前位置利用法と呼称する）、
高精度の速度検出を可能にすることが述べられている。

【０００５】

【発明が解決しようとする課題】従来の速度検出方法に
おいて、後退差分法と、二サンプル周期前位置利用法，
三サンプル周期前位置利用法の得失を図１１，図１２、
及び表１を用いて説明する。各速度検出方式に基づく速
度の検出式は、数１，数２，数３で示される。

【０００６】

【数１】

【０００７】

【数２】

【０００８】

【数３】

【０００９】ここで、θ（ｋ）は、第ｋサンプリング時
点における位置検出量であり、Ｔｓはサンプル周期、θ
（ｋ）は、第ｋサンプリング時点における速度である。
ここでは、一サンプル周期内にカウントされるパルス数
θ（ｋ）Ｔｓを速度相当量と定義する。

【００１０】図１１は、速度フィードバックを伴うモー
タ制御系の構成例（微分先行型PID制御系）を示し、図
１２は、位置検出器（エンコーダ）パルス周期とサンプ
ル周期の関係を示し、表１は、低速等速動作時の速度検
出値のばらつきを示している。図１１において、モータ
制御系は、位置指令が与えられると、検出位置との偏差
に基づき比例・積分補償がなされ、速度相当量に微分ゲ
インを乗じた量との差が増幅され、モータトルク指令が
生成され、印加される。このようなモータ制御系では、
モータと直結された位置検出器の発生パルスのサンプル
周期当りのカウント数が低速域で著しく少なくなり、そ
の差分により検出される速度の検出精度が著しく劣化す
る問題が指摘されている。ここでは、図１２に示されて
いるように、等速で低速動作時のエンコーダパルスタイ
ミングとサンプリングパルスタイミングが合致する時点
から次に合致する時点を考えることにする。ここでは、
エンコーダパルス分解能ｐ（パルス／回転、ｐ／ｒと表
記する），動作速度ｖ（回転／秒，ｒ／ｓと表記す
る），サンプル周期Ｔｓ（ｓ）とすると、一サンプル周
期当りのパルスカウント数ｎは数４により示される。

【００１１】

【数４】 ｎ＝ｐｖＴｓ …（数４） ここで、ｎの値は、ｐｖＴｓの小数点以下がゼロとなら
ない場合は、ｉｎｔ（ｐｖＴｓ），ｉｎｔ（ｐｖＴｓ）
＋１、の二種類のパルス検出形態が存在する。ここで、
ｉｎｔ（Ａ）は、Ａの整数化記号を示している。図中、
エンコーダパルス分解能ｐ（＝６４００００（ｐ／
ｒ)），サンプル周期Ｔｓ＝０.００３(ｓ)，動作速度ｖ
＝０.０１(ｒ／ｓ）の場合を示しているが、１９，２０
の二種類となり、図１２より、後退差分法，二サンプル
周期前位置利用法，三サンプル周期前位置利用法を用い
ると各サンプリング時点で次のような位置検出形態があ
る。

【００１２】（１）後退差分法…１９，２０の二形態 （２）二サンプル周期前位置利用法… １９−１９，１９−２０，２０−１９の三形態 （３）三サンプル周期前位置利用法… １９−１９−１９，１９−１９−２０，１９−２０−１
９，２０−１９−１９の四形態 これから、各動作速度に対して数１〜数３に基づき速度
を計算したところ、表１のようになり、０.０１(ｒ／
ｓ）以下では速度検出方式によらず、速度検出精度は悪
い。

【００１３】

【表１】

【００１４】しかし、極低速域では、後退差分法では常
に符号の誤りはないのに対して、二サンプル周期前位置
利用法，三サンプル周期前位置利用法を用いると逆符号
となることがあり、逆符号の速度フィードバック信号に
基づきトルク指令が生成される可能性があり、整定動作
が不安定化する可能性がある。

【００１５】次に、これらの速度検出方式を用いた場合
のロボット高速加減速動作時の速度検出精度の比較を図
１３ないし図１７を用いて説明する。図１３は、水平多
関節型ロボットの動作パターン例を示し、図１４は、図
１３の動作パターンで動作時のモータ速度と検出速度の
時間応答特性を示しており、図１５ないし図１７は後退
差分法，二サンプル周期前位置利用法，三サンプル周期
前位置利用法を用いた場合のモータ速度と検出速度の差
（速度検出誤差）の時間変化特性を示している。ここで
は、水平多関節型ロボットの動作パターンとして、第一
アーム３２を回転駆動する１軸モータ３１，第二アーム
３５を回転駆動する二軸モータ３４を広い動作角にわた
って動かす動作を扱うが（図１３参照）、この動作は負
荷移載作業などによく用いられる動作である。この動作
時の速度ならびに検出速度の時間変化を図１４に示し
た。加減速動作はサイクロイド曲線を用いて実現してい
る。このときの速度と検出速度の差（速度検出誤差）を
各速度検出方式に対して図１５，図１６，図１７に示し
た。これから、後退差分法，二サンプル周期前位置利用
法，三サンプル周期前位置利用法の順に速度検出誤差が
小さくなっており、高精度な速度検出が可能であること
を示している。

【００１６】以上から、後退差分法は、二，三サンプル
周期前位置利用法と比して、速度検出精度が悪く、二サ
ンプル周期前位置利用法，三サンプル周期前位置利用法
は、極低速域において逆符号の速度検出を行う可能性が
あるという問題がある。本発明の目的は、全速度域にお
いて高精度な速度検出を行い、良好な速度フィードバッ
ク信号を生成し、単軸機構，多軸機構を制御することに
より高速・高精度動作を実現することにある。

【００１７】

【課題を解決するための手段】全速度域における速度検
出精度を高めるため、本発明では、高速域において、二
サンプル周期以上前位置利用法を用い、極低速域におい
て後退差分法を用いて速度を検出しフィードバック制御
することにより、速度によらず、高精度の速度検出が可
能になる。

【００１８】

【作用】本速度検出方式は、速度検出方式を速度に応じ
て切り換える方式をとることにより、速度検出精度の向
上を図るものである。発明が解決しようとする課題で述
べたように、極低速域以外で、二，三サンプル周期前位
置利用法が高精度の速度検出を可能にする。また、極低
速域では、後退差分法は逆符号とならない速度検出を可
能にする。これらの特性を組み合わせることにより全速
度域で高精度の速度検出を実現できる。このように検出
された速度を用いてモータのトルク指令を生成し、アナ
ログ変換し、増幅器を介してモータの巻線に通電するこ
とにより、実速度とのミスマッチが少ない速度信号を帰
還していることになるため、移動体を高速整定制御でき
る。

【００１９】

【実施例】本発明の実施例を図１ないし図１０を用いて
説明する。図１は、本発明の制御方法を用いたモータ制
御系のブロック図を示し、図２は、図１の高次速度相当
量演算部を二サンプル周期前位置利用法により実現する
場合のブロック図を示し、図３は、図１の高次速度演算
部を三サンプル周期前位置利用法により実現する場合の
ブロック図を示し、図４は、本発明の制御方法のフロー
チャートを示し、図５は、低速度における速度検出誤差
と速度検出方式の関係を示し、図６は、高速加減速動作
時の速度検出誤差と速度検出方式切り換え速度の関係を
示し、図７は、本方式を用いる場合と用いない場合の整
定時間の差異を示し、図８は、本発明の制御装置のハー
ドウェアを示し、図９は、本発明の制御対象である単軸
機構（ディスク装置）の構成例を示し、図１０は、本発
明の制御対象である多軸機構（産業用ロボット）の構成
例を示している。

【００２０】まず、図１を用いて本発明の単軸機構もし
くは多軸機構の制御方法を説明する。本発明で対象とす
る単軸機構，多軸機構とは、単数，複数の軸（関節）に
モータを配置し、直接もしくは減速機等動力伝達機構を
介して駆動力を印加することにより、移動される移動体
を有し、その移動体の先端に作業部を有する機構を意味
する。このような機構（例えば、ロボット，ディスク装
置等）は、作業部を所定の位置へ高速・高精度で位置決
めすることが求められる。図１において、各サンプル時
点での各軸の位置指令１（これは速度制御の場合は、サ
ンプル時点毎に位置指令が速度の積分量だけ加算される
と考えてよい）が与えられると、位置出力６との偏差を
補償部２で補償し、位置出力６より速度相当量演算部９
で算出された速度相当量に微分ゲイン１０を乗じた量と
の差をもとに、トルク指令生成部３，Ｄ／Ａ変換部４，
パワーアンプ２３を介して制御対象５の駆動モータに電
流を印加することにより駆動し、モータに直結された位
置検出器より位置出力６を検出し、帰還することにより
制御を行っている。

【００２１】ここで、速度相当量演算部９は、後退差分
演算部１２，高次速度相当量演算部１１，速度判定部１
３，切り換え部１４よりなっている。例えば後退差分法
を用いて求められた速度相当量より、速度判定部１３に
てその絶対値があるしきい値より大きいか小さいかを判
定し大きければ、高次速度相当量演算部１１の演算結果
を出力し、小さければ、後退差分演算部１２の演算結果
を出力する構成となっている。

【００２２】高次速度相当量演算部１１の構成として二
サンプル周期前位置利用法を用いた場合を図２に示し
た。これは、数５で示される二サンプル周期前位置を用
いて位置出力より高次速度相当量を求める方式である。

【００２３】

【数５】

【００２４】次に、高次速度相当量演算部１１の構成と
して三サンプル周期前位置利用法を用いた場合を図３に
示した。これは、数６で示される三サンプル周期前位置
を用いて位置出力より高次速度相当量を求める方式であ
る。

【００２５】

【数６】

【００２６】図１ないし図３の単軸機構もしくは多軸機
構の制御方法のフローチャートを図４に示した。位置が
サンプリングされると、ｉサンプル周期前までの位置
（ｉ＝２，３，…）が記憶される。その後、速度相当量
を後退差分法、ｉサンプル周期前位置利用法により求
め、その絶対値がしきい値以下であれば、後退差分法に
より速度相当量を算出し、しきい値より大きい場合は、
ｉサンプル周期前位置利用法により算出する。このよう
な速度検出方式をとることによる特徴を説明する。低速
動作時の速度検出精度と速度検出方式の関係を各速度に
ついて図５に示した。図中で軸駆動モータに装着されて
いる位置検出器（エンコーダ）は六十四万ｐ／ｒの分解
能を有し、移動体のある動作速度で等速駆動時の速度検
出精度（カウントパルス数の形態により定まる最大検出
誤差の真値との比）と動作速度の関係を示している。こ
れから、０.０１ｒ／ｓ 以下の速度では、後退差分法が
最も精度が良く、二サンプル周期前位置利用法，三サン
プル周期前位置利用法の順に精度は低下する。

【００２７】次に、高速加減速動作時の速度検出誤差
（実速度と検出速度の差）と速度検出方式の関係を、水
平多関節型ロボットの関節を最大１６０（°／ｓ）で駆
動し、エンコーダ分解能六十四万ｐ／ｒの場合につき、
図６に示した。これから、後退差分法，二サンプル周期
前位置利用法，三サンプル周期前位置利用法の順に検出
誤差が低減されることが明らかになった。

【００２８】以上から、高速域では、二サンプル周期前
位置利用法，三サンプル周期前位置利用法を用い、極低
速域では、後退差分法を用いるのが良い。

【００２９】本速度検出方式切り換え方式を用いた場合
と用いない場合の動作整定挙動を図７に示した。これか
ら、本方式を用いることにより整定時間短縮を図ること
ができる。

【００３０】次に、本発明の単軸機構もしくは多軸機構
の制御装置のハードウェア構成を図８に示した。制御対
象５には、それを駆動するモータ１６とその位置を検出
するエンコーダ１７が設けられている。エンコーダ１７
より出されるＡ相，Ｂ相信号は、信号処理回路１８でア
ップダウンパルスに変換され、カウンタ１９で移動方向
検知及び移動量計数が行われる。移動量の検出は、マイ
クロプロセッサユニット（ＭＰＵ）１５よりサンプル周
期毎に発生されるサンプリングパルス２０がカウンタ１
９に加わる度に割込みが入り更新され、ＭＰＵ１５に取
り込まれ、ランダムアクセスメモリ（ＲＡＭ）２１に記
憶される。ＭＰＵ１５は、リードオンリーメモリ（ＲＯ
Ｍ）２２に記憶されているアルゴリズムに基づき速度相
当量を算出し、その絶対値に基づき検出速度を切り換え
・選択し、位置偏差の補償値と加算して、トルク指令を
生成し、Ｄ／Ａ変換部４でアナログ変換し、パワーアン
プ２３で増幅し、モータに通電し制御を行う。

【００３１】図９，図１０に制御対象である単軸機構
（ディスク装置），多軸機構（産業用ロボット）の構成
例を示す。

【００３２】図９は、計算機の外部記憶装置である磁気
ディスク装置を示す。磁気ディスク装置は、ディスク２
４の面にデータのみならず位置信号２６が書き込まれて
おり、ボイスコイルモータ２５により揺動駆動される部
位の先端のヘッド部４１に設けられた読み書き装置によ
りディスク面へのデータの読み書きと共に位置信号の読
み出しを行う。読み出された位置２８は、制御装置２７
に取り込まれ、速度相当量を演算し、その絶対値としき
い値の大小関係から検出方式を切り換え、速度を検出
し、ボイスコイルモータ２５の駆動力指令を生成し、電
流３０を通電することにより制御される。

【００３３】図１０は、水平多関節型産業用ロボットを
示し、第一アーム３２は、一軸モータ（＋減速機）３１
により回転駆動３３され、第二アーム３５は、二軸モー
タ（＋減速機）３４により回転駆動３６され、第二アー
ム先端に設けられた上下軸３７は、下端に作業対象を着
脱可能な機構を有している。上下軸３７は、上下方向３
８，回転方向３９に図示しない駆動装置により駆動され
る。各軸の位置は、サンプル周期毎に制御装置２７に取
り込まれ、前述のアルゴリズムに基づきトルク指令が生
成され、出力され、制御される。

【００３４】このように、本発明の制御方法及び制御装
置は、単軸機構（例えばディスク装置），多軸機構（例
えば産業用ロボット）の動作制御に利用できる。

【００３５】

【発明の効果】本発明は、上述のように構成されている
ため、以下に示す効果を奏する。

【００３６】 高速加減速動作時に高精度の速度検出
を可能にする。

【００３７】 低速動作時に高精度の速度検出を可能
にする。

【００３８】 ，により全速度域で高精度に検出
された速度信号をもとにトルク指令を生成し、モータを
駆動することにより高精度の位置制御・速度制御を実現
可能にする。

【図面の簡単な説明】

【図１】本発明の制御方法を用いたモータ制御系のブロ
ック図。

【図２】高次速度相当量演算部の説明図。

【図３】高次速度相当量演算部の説明図。

【図４】本発明の制御方法のフローチャート。

【図５】低速等速動作時の速度検出精度と速度検出方式
の関係の特性図。

【図６】高速加減速動作時の速度検出誤差と速度検出方
式の関係の説明図。

【図７】本方式を用いた場合と用いない場合の整定時間
の差異の説明図。

【図８】本発明の制御装置のハードウェアのブロック
図。

【図９】本発明の制御対象である単軸機構の説明図。

【図１０】本発明の制御対象である多軸機構の説明図。

【図１１】モータ制御系のブロック図。

【図１２】エンコーダパルス周期とサンプル周期の関係
の説明図。

【図１３】水平多関節型ロボットの動作パターン例の説
明図。

【図１４】動作時の速度、検出速度の時間変化の説明
図。

【図１５】後退差分法による速度検出誤差の時間変化の
説明図。

【図１６】二サンプル周期前位置利用法による速度検出
誤差の時間変化の説明図。

【図１７】三サンプル周期前位置利用法による速度検出
誤差の時間変化の説明図。

【符号の説明】

１…位置指令、２…補償部、３…トルク指令生成部、４
…Ｄ／Ａ変換部、５…制御対象、６…位置出力、７…サ
ンプラ、８…位置帰還ゲイン、９…速度相当量演算部、
１０…微分ゲイン、１１…高次速度相当量演算部、１２
…後退差分演算部、１３…速度判定部。

Claims (1)

【特許請求の範囲】
1. 【請求項１】単軸機構もしくは多軸機構の関節を駆動す
   るモータの動作位置のみサンプル周期毎に検出し、その
   一サンプル以上前の単数もしくは複数の時点の動作位置
   検出値との差分により速度相当量を算出し、前記速度相
   当量を用いて前記モータの駆動力指令を生成・出力する
   ことにより、前記モータの動作位置もしくは動作速度を
   制御する制御方法において、速度相当量絶対値があるし
   きい値以上で動作中は、サンプル時点の動作位置検出量
   及び一サンプル周期前の位置検出値のみならず二サンプ
   ル以上前の位置検出値も用いて速度相当量を演算し、速
   度相当量絶対値が前記しきい値未満で動作中は、サンプ
   ル時点の動作位置検出量及び一サンプル周期前の位置検
   出値のみ用いて速度相当量を演算することを特徴とする
   単軸機構もしくは多軸機構の制御方法。