JPH0786775B2 - モータの制御方法 - Google Patents

Description

【発明の詳細な説明】 ［産業上の利用分野］ 本発明は、多関節型ロボットのアーム、直交座標型ロボ
ットのテーブル等の可動部を駆動するモータの制御方法
に関する。

［従来の技術］ 従来、マイクロコンピュータにより制御されて駆動され
るモータを備えた産業用ロボットでは、アームを備えた
多関節型であろうと、直交する２個のテーブルを備えた
直交座標型であろうと、その可動部に振動を起こさせず
に滑らかに、しかも短時間で目標位置に停止させること
が要求されている。この要求に応えるべく、減速時のモ
ータ速度の時間変化を、正弦曲線あるいは振幅・周期の
異なる正弦曲線を合成してなる疑似正弦曲線に沿うよう
に制御して、モータを減速し、目標位置を位置決めする
制御方法が一般に用いられている。この制御方法の一例
として、前記正弦曲線あるいは疑似正弦曲線から移動量
と速度との関係をあらかじめ求めておき、これを位置偏
差と目標速度との関係とみなし、各位置偏差に対応した
目標速度を持つ対応表として記憶する一方、目標位置と
現在位置との差でなる位置偏差に応じて、対応表から目
標速度を求め、モータの速度をその目標速度となるよう
に制御する方法がある。この方法では、移動時に各時点
における位置偏差からただちにその時点の目標速度が求
まり、この目標速度を目標としてモータの速度を制御す
るだけで、モータを正弦曲線あるいは疑似正弦曲線に沿
って減速し、目標位置に位置決めを行える。

しかしながら、この方法では、目標位置が与えられ、こ
の目標位置と現在位置との差でなる位置偏差が大きくな
ると、目標速度も大きな速度となる。そのため、モータ
を加速するに際して、モータが回転を開始する時には、
モータは速度零の状態から急激に加速されることとな
り、モータに振動が生じ、このモータにより駆動される
産業用ロボットの可動部あるいはワークに大きな加速度
が加わる等の不都合が生じている。その対策として、モ
ータを加速するに際して第６図に示すようにモータ移動
開始時に、速度零から一定加速で加速するように目標速
度を補間し、この目標速度が位置偏差から求まる目標速
度よりも大きくなった時点で目標とする速度を位置偏差
から得られる目標速度に切換える方法が用いられる。

［発明が解決しようとする課題］ この制御方法では、モータの速度は速度零から徐々に立
ち上がり、過大な加速度は生じないが、補間による目標
速度と位置偏差から求めた目標速度との切替え時には、
目標速度は不連続となり、加速度が急激に変化し、モー
タになお振動を招く等の欠点が生じている。しかも、補
間による速度を求めた上に、位置偏差からも目標速度を
求めてさらに前記二つの目標速度を比較してその切替の
時点を検出しなければならず、プログラムが長く、また
その演算時間も長くなる等の欠点が生じている。

本発明は上記欠点を除去しようとするもので、減速時に
用いる位置偏差と目標速度との関係から加速時にも連続
的で滑かな速度変化をするように構成したモータの位置
・速度制御方法を提供しようとするものである。

［課題を解決するための手段］ 上記目的を達成するため、モータの目標位置と現在位置
との間の位置偏差が大きくなるにともなって増加し始め
て徐々に最高速度に達する目標速度データを記憶する記
憶部と、モータの回転を検出してその１制御周期の回転
量を現在速度とする回転検出部と、目標位置と現在位置
との間の位置偏差に対応する目標速度データを呼出し、
この目標速度データに対応する速度指令値をモータに与
えてモータの現在速度を目標速度に制御するサーボ制御
部とを備えたモータの速度制御方法において、 目標位置指令値が与えられて目標位置が決まると、あら
かじめ設定されたモータの１制御周期の回転量をサイク
ル速度指令値として前回の仮目標位置に加えてモータの
１制御周期の仮目標位置を算出し、これと現在位置との
差を仮位置偏差として、あらかじめ位置偏差に対応して
記憶された目標速度データからこの仮位置偏差に対応す
る目標速度データを呼出し、この目標速度データに対応
するモータの速度指令値をサーボ制御部に与えてモータ
の速度をこの目標速度データに一致するように制御する
とともに、このモータの制御を所定設定回数繰返して
後、目標位置指令値を目標位置として現在位置との間の
位置偏差を算出し、この位置偏差に対応してあらかじめ
記憶された目標速度データを呼出し、モータを目標速度
データが目標速度となるようにモータの速度制御を行う
ように構成されている。

作用 目標位置指令値が、またあらかじめ設定されたモータの
１制御周期の回転量がサイクル速度指令値としてが与え
られ、さらにこのサイクル速度指令値を繰返し与える繰
返し回数が設定されると、目標位置指令値とは関係な
く、前回の目標位置すなわち現在位置にサイクル速度指
令値を加えた値を仮目標位置とし、現在位置と仮目標位
置との差が仮位置偏差として算出される。さらに、この
仮位置偏差に対応して記憶部からこれに対応する目標速
度データが呼出され、モータの速度がこの目標速度デー
タを目標として制御される。この時、仮位置偏差はサイ
クル速度指令値分で実際の位置偏差と比べて小さな値と
なり、この仮位置偏差に対応した目標速度データも小さ
な値となる。そのため、前記モータは速度零からゆっく
りと回転を開始する。

続いて、前記目標速度データをモータに与えてから仮目
標位置にサイクル速度指令値を加えて得られる新たな仮
目標位置と現在位置との間の仮位置偏差を算出し、前回
同様に仮位置偏差に対応した目標速度データを呼出し、
モータを速度制御する。この時、仮位置偏差はモータの
速度が目標速度に追従していない分仮位置偏差が前回よ
りも大きくなり、目標速度データも前回よりも大きくな
って、モータの速度が加速される。

上記モータの制御が繰返し行われると、モータの速度が
目標速度に追従しない分も大きくなって、仮位置偏差お
よび目標速度データが大きくなってモータの速度もどん
どん加速され、目標速度データが最高目標速度に近づく
に達する。目標速度データが最高目標に近くに達して
後、仮位置偏差が大きくなっても、あらかじめ記憶され
た目標速度データが大きくならないため、モータの速度
の増加率も徐々に小さくなる。

さらに、前記目標速度データが最高目標速度に達して、
モータの制御が繰返されると、仮位置偏差がサイクル速
度指令値分とモータの追従遅れ分とを加算した一定値と
なり、目標速度データが最高目標速度に保持され、モー
タは定速で回転する。

前記制御が繰返し設定回数だけ繰返されて後、目標位置
は前記目標位置指令値に固定され、この目標位置と現在
位置との位置偏差が算出される。さらに、この位置偏差
に対応して記憶部から目標速度データが呼出され、この
目標速度データに対応する速度指令値がモータに与えら
れ、モータはこの目標速度データを目標として速度制御
される。そのため、モータの現在位置が目標位置に接近
すると、位置偏差が小さくなり、モータに与える速度指
令値は最高目標速度から徐々に小さな目標速度指令値に
なって、モータは減速される。前記モータの現在位置が
目標位置に達して位置偏差が零になると、目標速度デー
タも零となりモータは停止する。

実施例 以下、実施例を図面について説明する。第１図におい
て、１はモータ２の回転を受けて移動する多関節型のア
ーム（図示せず）あるいは直交して位置する２個のテー
ブル（図示せず）でなる可動部を備えた産業用ロボット
の制御装置である。この制御装置１は、第1CPU3、第1RO
M4、第1RAM5、キーボード６、表示部７およびDMAコント
ローラ８を備えた主制御部1a、これから目標位置の位置
情報を受けてモータ速度を制御するための演算を行うサ
ーボ制御部1b、サーボ制御部1bから発信される指令値に
応じてモータ２を駆動するモータ駆動部の一例のPWM駆
動部1cからなっている。前記主制御部1aのDMAコントロ
ーラ８はサーボ制御部1bの第2RAM9に接続されており、
主制御部1aの第1CPU3とサーボ制御部1bの第2RAM9とが接
続可能に構成され、キーボード６から後記する減速比決
定定数k1およびサーボ強さ決定定数k2が入力されると、
DMAコントローラ８を通じて第2RAM9の所定アドレスに記
憶されるように構成されている。また、前記第2RAM9に
はキーボード６から最高目標速度データよりも小さな目
標速度データから得られるモータ２の１制御周期の回転
量がサイクル指令値として、またこのサイクル指令値を
繰返し与える回数を設定する繰返し回数指令値が入力さ
れると、これらが前記DMAコントローラ８を通じて記憶
されるように構成されている。

また、前記主制御部1aの第1ROM4には、電源がオンとな
り、作業準備が完了して後、移動指令信号により目標位
置の位置情報を目標位置指令値として呼出し、この位置
決めが完了すると、作業ユニット（図示せず）に作業開
始指令信号を発信する作業プログラムが記憶されてい
る。この作業プログラムは、第２図に示すように、 １）キーボード６から入力される減速比決定定数k1、サ
ーボ強さ決定定数k2、サイクル速度指令値および繰返し
回数指令値を待つ。

２）これら入力情報をDMAコントローラ８を通じて第2RA
M9の所定アドレスに転送する。

３）移動指令信号を待つ。

４）移動指令信号により目標位置の位置情報を目標位置
指令値として第1RAM5から呼出す。

５）この目標位置指令値をDMAコントローラ８を通じて
サーボ制御部1bの第2RAM9の所定アドレス内に転送し、
これを記憶する。

６）DMAコントローラ８を通じて第2RAM9内の位置決め完
了フラグが記憶されるアドレスの入力情報を読込む。

７）第2RAM9内に位置決め完了フラグが入力されたかを
判断し、位置決め完了フラグが入力されない時、６）に
戻る。

８）位置決め完了フラグが入力されると、作業ユニット
に所定の作業指令信号を出力して作業ユニットから発信
される作業完了信号を待つ。

９）位置完了信号を受けると、サイクル完了かを判断
し、サイクル完了でない時、３）に戻る。

10）エンドとなる。

ように構成されている。

なお、前記サイクル速度指令値は最高目標速度データを
呼出してこの最高目標速度データから算出されるモータ
２の１制御周期の回転量としてもよい。また、前記繰返
し回数指令値は、目標位置指令値をサイクル速度指令値
で分割して得られる分割数として、各位置偏差に応じて
記憶しておけばよい。この場合、前記目標位置指令値が
呼出される毎に繰返し回数指令値を算出するように構成
することもできる。

前記サーボ制御部1bは、第2CPU10、第2ROM11、第2RAM
9、演算部12およびI/Oポート13を有し、演算部12で算出
した値をI/Oポート13を介してPWM駆動部1cに送るように
構成されている。また、前記第2ROM11には速度と移動時
間との関係を示す正弦曲線あるいは異なる振幅・周期の
正弦曲線を合成して得られる疑似正弦曲線を、第５図に
示す目標速度−位置偏差の関係を示す特性曲線に変換し
て、この特性曲線から各位置偏差に対応してあらかじめ
算出された目標速度データが記憶されている。この特性
曲線から算出される目標速度データは、位置偏差が小さ
い範囲では、零から急激に増加する値であって、位置偏
差が大きくなるに従って、緩やかに増加して最高速度に
対する値となっている。しかも、各目標速度データは、
それぞれから算出されるモータ２の１制御周期の回転量
が前記目標速度データに対応する位置偏差よりも小さく
なるように構成されている。また、前記第2ROM11には目
標速度出力プログラムが記憶され、目標位置の位置情報
と後記するパルスエンコーダ14の位置検出パルスを積算
する積算器15の積算値から得られる現在位置との差でな
る位置偏差（校正位置偏差）に応じて、これに対応する
目標速度をデータを呼出すように構成されている。

前記目標速度出力プログラムは、第３図に示すように、 １）第2RAM9から目標位置指令値、サイクル速度指令
値、繰返し回数指令値を受取ると、前記目標位置指令値
を目標位置として、これと積算器15の積算値から得られ
るモータ２の現在位置とから、位置偏差を算出するとと
もに、制御回数記憶部を零にセットする。

２）位置偏差が所定位置決め精度よりも小さい時、位置
偏差が零とみなされ、第2RAM9内に所定アドレスに位置
決め完了フラグをセットする。

３）前記位置偏差に減速比決定定数k1を乗じて校正位置
偏差を算出する。

４）制御回数記憶部に１を加え、繰返し回数指令値と等
しければ、８）へジャンプする。

５）校正位置偏差からモータの極性を決定して後、モー
タ２の仮目標位置にサイクル速度指令値を加え、新たな
仮目標位置とする。

６）この仮目標位置と現在位置とから仮位置偏差を算出
し、これに減速比決定定数k1を乗じて校正仮位置偏差を
算出するとともに、あらかじめ記憶された目標速度デー
タの中から前記校正仮位置偏差に対応した目標速度デー
タを呼出す。

７）前記目標速度データにサーボ制御強さk2を乗じて電
流指令値を算出する。この電流指令値に応じてPWM駆動
部1cを駆動し、４）に戻る。

８）前記目標位置指令値を新たな目標位置とする。

９）目標位置と現在位置との差を位置偏差とし、前記減
速比決定定数k1を乗じて校正位置偏差を算出する。この
校正位置偏差が所定位置決め精度より大きければ、あら
かじめ記憶している目標速度データの中から前記校正位
置偏差に対応した目標速度データを呼出す。

10）前記目標速度データと現在速度との速度偏差を求
め、これにサーボ制御強さを乗じて電流指令値を算出す
る。この電流指令値に応じてPWM駆動部1cを駆動し、前
記目標速度データを目標としてモータ２の速度を制御
し、９）に戻る。

11）位置決め完了信号として位置決め完了フラグを第2R
AM9の所定アドレス内に送り、１）に戻る。

ように構成されている。

上記の場合、サーボ強さ決定定数k2が大きいと、モータ
２の追従性が上がり、力が強くなるが、動作が振動的と
なる。一方、前記サーボ強さ決定定数K2が小さいと、モ
ータ２の追従性が落ちてその動きが安定することとな
る。

前記PWM駆動部1cは、PWMパルス幅選択部16およびPWM駆
動回路17を備えており、PWMパルス幅選択部16は前記電
流指令値に対応するPWMのパルス幅を選択するように構
成されている。また、前記電流指令値に対応したPWMの
パルス幅はPWM駆動回路17に送られ、駆動電圧をモータ
２に供給するように構成されている。前記モータ２には
その回転を検出するパルスエンコーダ14が取付けられて
おり、この位置検出パルスが積算器15を介して前記サー
ボ制御部1bにフィードバックされて第４図に示す制御系
が構成されており、位置・速度制御が行われるように構
成されている。

上記産業用ロボットの制御装置では、作業開始前に、主
制御部1aのキーボード６からモータ２の減速比決定定数
k1、サーボ強さ決定定数k2、サイクル速度指令値および
繰返し回数指令値が入力されると、これらがDMAコント
ローラ８を通じてサーボ制御部1bの第2RMA19に送られ
る。その後、主制御部1aが移動指令信号を受けると、あ
らかじめ第1RAM5に設定された作業位置から最初の作業
位置を選択し、これを目標位置指令値としてDMAコント
ローラ８を介して第2RAM9の所定アドレスに出力する。
同時に、DMAコントローラ８を介して第2RAM9の所定アド
レス内の入力情報を読込み、位置決め完了フラグの入力
を待つ。

一方、サーボ制御部1bは第2RAM19の所定アドレス内の目
標位置指令値と積算器15の積算値との差でなる位置偏差
を算出する。この位置偏差が所定位置決め精度よりも大
きいと、前記位置偏差に減速比決定定数k1を乗じて校正
位置偏差が算出される。

続いて、前記校正位置偏差からサイクル速度指令値の極
性が決定され、さらに前記目標位置指令値とは関係な
く、前回の目標位置すなわち現在位置にサイクル速度指
令値を加えた値を仮目標位置とし、現在位置と仮目標位
置との間の仮位置偏差が算出され、すなわちモータ２の
速度偏差が算出される。さらに、この仮位置偏差から校
正仮位置偏差が算出され、これに対応して第2ROM11から
目標速度データが呼出される。この時、校正仮位置偏差
はサイクル速度指令値分で小さな値となり、この校正仮
位置偏差に対応した目標速度データも小さな値となる。
その後、この目標速度データにサーボ強さ決定定数k2を
乗じて電流指令値が算出される。この電流指令値に応じ
て、PWMパルス幅選択部16で所定パルス幅のパルスが発
振され、このパルスに応じた電圧がPWM駆動回路17から
モータ２に送られ、モータ２は前記目標速度データを目
標として速度零から立ちあがる。

続いて、前記目標速度データをモータ２に与えてから仮
目標位置にサイクル速度指令値を加えて得らえる新たな
仮目標位置と現在位置との間の仮位置偏差を算出し、前
回同様に校正仮位置偏差を算出してこれに対応した目標
速度データを呼出し、これに対応する速度指令値として
電流指令値をサーボ制御部1bを経由してPWM駆動部1cのP
WMパルス幅選択部16,PWM駆動部回路17に与え、モータ２
の速度を制御する。この時、仮位置偏差はモータ２の速
度が目標速度データに追従していない分仮位置偏差が前
回より大きくなり、これにともない校正仮位置偏差が大
きくなり、目標速度データも前回よりも大きくなって、
モータ２の速度が加速される。

上記モータ２の制御が繰返し行われると、校正仮位置偏
差が徐々に大きくなり、目標速度データも大きくなっ
て、モータ２の速度もどんどん加速される。目標速度デ
ータが大きくなると、仮位置偏差もどんどん大きくな
り、目標速度データが最高目標速度に近くに達する。目
標速度データが最高目標速度に近くに達すると、仮位置
偏差が大きくなって、校正仮位置偏差が大きくなって
も、目標速度データが大きくならないため、モータ２の
速度の増加率も徐々に小さくなる。

さらに、前記目標速度データが最高目標速度に達して、
モータ２の制御が繰返されると、仮位置偏差が一定値の
サイクル速度指令値分と一定値のモータ２の追従遅れ分
とを加算した一定値となり、従って校正仮位置偏差も一
定となり、目標速度データが最高目標速度に保持され、
モータ２は定速で回転する。

前記制御が繰返し回数指令値の回数だけ繰返されると、
目標位置は前記目標指令値に固定され、この目標位置と
現在位置との位置偏差が算出される。さらに、この位置
偏差に対応して第2ROM11から目標速度データが呼出さ
れ、モータ２はこの目標速度データは目標として速度制
御される。そのため、モータ２の現在位置が目標位置に
接近すると、位置偏差が小さくなり、モータ２に与える
サイクル指令値は最高目標速度から徐々に小さな目標速
度指令値になって、モータ２は減速される。前記モータ
２の現在位置が目標位置に達して位置偏差が所定位置決
め精度よりも小さくなると、モータ２に停止指令信号が
与えられ、モータ２が停止するとともに、第2RAM9の所
定アドレスに位置決め完了フラグが記憶される。位置決
め完了フラグが記憶されると、作業完了信号、サイクル
完了信号を待ってモータ２の全制御を完了する。

［発明の効果］ 以上説明したように、本発明はモータの目標位置と現在
位置との間の位置偏差に対応する目標速度データを記憶
しておき、目標位置となる目標位置指令値を受けると、
モータ始動時には仮目標位置を設けてモータの１制御周
期の回転量を仮目標位置とし、モータの制御周期ごとに
これを仮目標位置に加えて各制御周期の仮位置偏差を算
出し、この仮位置偏差に対応する目標速度データを呼出
してモータをこの目標速度データを目標として速度制御
するとともに、このモータ制御を所定設定回数繰返し行
い、その後は目標位置を前記目標位置指令値として固定
し、この目標位置指令値と現在位置との差でなる位置偏
差を求め、前記減速特性に基づいて前記位置偏差に対応
した目標速度データを呼出し、この目標速度データを目
標としてモータの回転を制御し、ついには停止させるよ
うに構成しているため、モータを加速する際に、目標位
置と現在位置との差でなる位置偏差が大きくても、モー
タが回転を開始する時には小さな目標速度データが呼出
され、モータは零からゆっくりと回転を始め、その後は
最高目標速度で回転することができ、モータの制御周期
ごとに所定設定回数だけ仮目標位置を設定してこれをそ
の制御周期ごとに所定値だけ増加させる簡単なプログラ
ムを加えるだけでモータの円滑な加速が可能となる等の
利点がある。

【図面の簡単な説明】

第１図は本発明の全体説明図、第２図は本発明に係わる
作業プログラムのフローチャート、第３図は本発明に係
わる目標速度出力プログラムのフローチャート、第４図
は本発明に係わるソフトウエアサーボ制御の制御系を示
すブロック図、第５図は本発明に係わるモータの目標速
度−位置偏差の関係を示す減速特性の説明図、第６図は
従来のモータの目標速度−位置偏差の関係を示す減速特
性の説明図である。 １……制御装置、1a……主制御部、 1b……サーボ制御部、1c……PWM駆動体、 ２……モータ、３……第1CPU、 ４……第1ROM、５……第1RAM、 ６……キーボード、７……表示部、 ８……DMAコントローラ、９……第2RAM、 10……第2CPU、11……第2ROM、 12……演算部、13……I/Oポート、 14……パルスエンコーダ、15……積算器、 16……PWMパルス幅選択部、17……PWM駆動回路、 18……………、19……モータ制御部、

Claims (2)

【特許請求の範囲】
1. 【請求項１】モータの目標位置と現在位置との間の位置
   偏差が大きくなるにともなって増加し始めて徐々に最高
   速度に達する目標速度データを記憶する記憶部と、モー
   タの回転を検出して１制御周期の回転量を現在速度とす
   る回転検出部と、目標位置と現在位置との間の位置偏差
   に対応する目標速度データを呼出し、この目標速度デー
   タに対応する速度指令値をモータに与えてモータの現在
   速度を目標速度に制御するサーボ制御部とを備えたモー
   タの速度制御方法において、 目標指令値が与えられて目標位置が決まると、１制御周
   期のモータの回転量としてあらかじめ設定された所定の
   サイクル速度指令値を与えて仮目標位置を算出し、これ
   と現在位置との差を仮位置偏差として、あらかじめ位置
   偏差に対応して記憶された目標速度データからこの仮位
   置偏差に対応する目標速度データを呼出し、モータをこ
   の目標速度データを目標として速度制御するととも、こ
   のモータの制御を所定設定回数繰返して後、目標指令値
   を目標位置として現在位置との間の位置偏差を算出し、
   この位置偏差に対応してあらかじめ記憶された目標速度
   データを呼出し、モータを目標速度データが目標速度と
   なるようにモータの速度制御を行うことを特徴とするモ
   ータの速度制御方法。
2. 【請求項２】前記繰返し設定回数は、目標位置指令値と
   現在位置との間の位置偏差を１制御周期のモータの移動
   量で分割して得られる値としたことを特徴とする請求項
   １に記載のモータの速度制御方法。