JPH02137006A - 速度制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 ［産業上の利用分野］ この発明は工作機械、ロボットアーム等を指定した経路
に沿って高速・高精度に駆動するための速度制御装置、
特に経路の角度変化に応じた加減速制御に関するもので
ある。

［従来の技術］ 数値制御により工作機械やロボットアームを指令した移
動経路に沿って高速に駆動する際、経路の方向が急激に
変化する角点（コーナ点）を通過するとき機械に作用す
る衝撃や振動を防ぎ、軌跡精度を確保するため、コーナ
点の前後で速度を加減速する速度制御が必要となる。

一般に、移動経路は直線や円弧などのセグメント単位に
分割されて指令され、コーナ点はこれらのセグメントの
接続部で発生する。このため、従来はセグメント単位に
その始端・終端でそれぞれ加速・減速を行いコーナ点の
速度制御を行っている。

第６図は特願昭６１−２９２５７９号に開示されている
従来の速度制御装置の構成を示すブロック図である。図
において、（１）は移動経路の指令値をセグメント単位
に入力する人力部、（２）は速度指令生成部、（３）は
加減速処理部、（４）は積分器、（５）はセグメント終
端判別部、（６）は移動比率計算部、（７）は補間演算
部、（８ａ）　、　（８ｂ）はＸ、Ｙ各軸のサーボ回路
、（９ａ）　、　（９ｂ）はＸ、Ｙ各軸のサーボモータ
をそれぞれ示す。

従来の速度制御装置は上記のように構成され、この速度
制御装置において例えば第７図に示す２つの直線セグメ
ントＡ、Ｂからなる経路Ｐ→Ｑ→Ｒを移動する場合につ
いて考える。このときＱ点がコーナ点である。

先ず、直線セグメントＡの座標データ （Ｘａ、Ｙａ）、セグメント長さＬａ （−Ｘａ　”　＋Ｙａ　”　）及び速度指令値Ｆが入力
部（１）から読み込まれ、速度指令生成部（２）はサン
プリング周期△Ｔごとに、この間の移動量Ｆ・△Ｔを求
め、これを出力信号Ｖｉ（Ｖｌ−Ｆ・ΔＴＳ＋−０，１
，２・・・）として出力する。ここで、サフィックス　
１は出力開始からのサンプリング時刻を表している。

更に、速度指令生成部（２）はこの移動量を積算し、セ
グメント長Ｌａと比較して残距離を求め、残距離がＦ・
八Ｔより小さくなったときは残距離をＶｌとして出力す
る。

第８図は加減速処理とセグメント移動のタイミングを示
すタイムチャートであり、出力信号Ｖｌは、同図の符号
（ｌｏａ）に示すように、セグメントの始端Ｐで立上り
、終端Ｑでゼロとなるステップ状の時間関数で、速度指
令とみなすことができ、加減速処理部（３）はステップ
状の指令速度Ｖ１を受けて、この立上り及び立下り時に
一定の加速度を越さないような加減速特性をもつ出力速
度信号Ｖ１を演算する。

第９図は加減速処理部（３）の構成例を示すブロック図
で、指令速度Ｖ１と出力速度ｖ１との差を一定のゲイン
Ｋをもつ積分器（１５）で積分し、その積分値を出力速
度ｖｌとする簡単な一次フィルタをデジタル処理で構成
する。このとき加減速時定数は１／にとなり、第８図に
示すように、ステップ状の指令速度Ｖ　Ｉ　（１０ａ）
は、指数関数の加減速特性をもつ出力速度ｖｉ（ｌｌａ
）に成形される。

加減速出力速度ｖｌを積分器（４）で積分した積分値Ｌ
１は、セグメントＡ上を速度ｖ１で移動する移動点Ｐｉ
のセグメント始端Ｐからの距離であり、移動比率計算部
（Ｂ）はこれとセグメント長Ｌａとの比をとって移動比
率Ｋｌ−Ｌ１／Ｌａを求める。

補間演算部（７）は、セグメントＡの座標データ（Ｘａ
、Ｙａ）に移動比率Ｋｌを乗じることにより、速度ｖｌ
で移動する移動点Ｐ１の座標（ＸＩＹｌ）を ＸＩ　　　−Ｋｌ　　　拳　Ｘａ Ｙｌ　−ｋｌ　　−ｙａ を用いて求め、この１サンプル周期のＸ、Ｙ各軸の移動
量△ＸＩ、ΔＹ１を前回の座標値との差をとって △ＸＩ　−ＸＩ　−Ｘｉ−１ △ｙ＋　−ｙｉ　−ｙｉ−ｔ によって求め、Ｘ、Ｙ各軸のサーボ回路（８ａ）。

（８ａ）へ出力する。

一方、セグメント終端判別部（５）は、移動点ＰＩの移
動量ｉ＋１１Ｌｌがセグメント長Ｌａに等しくなったか
どうかにより、Ｑ点への到達・完了を判別し、完了と同
時に入力部（１）を付勢して次のセグメントＢのデータ
を入力し、その移動処理に入る。

このようにして加減速処理さ・れたＸ、Ｙ各軸の移動指
令値ΔＸｌ、ΔＹｊにより、サーボ回路（８ａ）、　　
（８ｂ）を介してサーボモータ（９ａ）、　　（９ｂ）
を駆動し、経路制御が行われる。

［発明が解決しようとする課題］ 従来の速度制御装置は以上のように構成されているので
、経路のコーナ点を精度よく移動するためには、コーナ
点で速度を一旦ゼロまで減速した後、再び加速するよう
な加減速制御が必要であり、加減速に要す時間が長くか
・す、高速移動ができなかった。

この発明は、上記のような問題点を解消するためになさ
れたもので、経路のコーナ点を高精度に、かつ高速に移
動できるようにした速度制御装置を得ることを目的とす
る。

［課題を解決するための手段］ この発明に係る速度制御装置は、コーナ点における経路
の角度変化量を計算する第１の計算部と、角度変化量及
び与えられた許容加速度値からコーナ点における許容速
度を計算する第２の計算部と、コーナ点へ向って移動速
度を前記許容速度まで減速し、コーナ点に到達した後、
移動速度を指令速度まで加速するように制御する加減速
制御部とを備えたものである。

［作　用］ この発明においては、経路のコーナ点の移動速度を経路
の角度変化量で決まる許容速度までしか減速しないため
、加減速に要する時間が短く、高速な移動が可能となっ
ている。

［実施例］ 以下、この発明の実施例を図について説明する。

第１図はこの発明の一実施例に係る速度制御装置の構成
を示すブロック図である。図において、第６図と同一符
号のものは全く同一のものを示す。

（２０）はコーナ点を形成する２つのセグメントデータ
を同時に人力する人力部、（２１）はコーナ角計算部、
（２２）はコーナ速度計算部で、（２３）はコーナ点で
指定された速度まで加減速する加減速処理部である。

上記のように構成された速度制御装置の動作を、第２図
に示すＰ−Ｑ−Ｒ−５の経路移動の場合について説明す
る。

先ず入力部（２０）は、移動開始する直線セグメントＡ
のデータＸａ、Ｙａ、Ｌａ及び移動速度Ｆと、次のセグ
メントデータＸｂ、Ｙｂ、Ｌｂとを同時に人力する。コ
ーナ角計算部（２１）は入力されたデータを用いて直線
セグメントＡとＢのなす角度θを余弧定理を用いて次式
で計算する。

ＣＯＳθ■ ２　Ｌａ　−Ｌｂ コーナ速度計算部（２２）は、コーナ点Ｑを移動する許
容速度Ｖｃをこのコーナ角度θに応じて求めるものであ
る。

第３図はコーナ速度決定法の説明図で、図に示すように
、コーナ点Ｑを速度Ｖで回ったとき発生する速度ベクト
ルの変化量△Ｖは余弧定理を用い△Ｖ、、　２　ｖ２　
　　　２ ２２ｖ　　　ｃｏｓθ 、、△ｖ　−ｖ　　２（Ｌ−ｃｏｓ　　Ｏ）となる。こ
の速度変化が１サンプリング周期Δτ間に起るとすれば
、その加減速の大きさはとなる。

一方、駆動系や機械系に作用する最大許容加減速αがこ
れらの設計値から与えられるため、（１）式の加速度が
、これをオーバしないようにする条件から、コーナ点に
おける許容速度ＶＱはα　昏　八Ｔ Ｖｅ　　−・・・（２） Ｅバ「：コτｉ］ となり、コーナ角度θの値を用いて（２）式により決め
ることができる。

加減速処理部（２３）は、速度指令生成部（２）の出力
する指令速度Ｖ＋を上記の許容速度■ｃを用いてコーナ
点Ｑで許容速度ｖｃを満たすように加減速処理し、出力
速度ｖｌを求めるもので、例えば第４図のフローチャー
トに示す処理によって実現される。

この一連の処理はサンプリング周期△Ｔごとの割込み信
号で起動する。サンプリング時刻１において、先ず速度
指令生成部（２）の出力する指令速度Ｖｊとコーナ速度
計算部（２２）の出力する許容速度ＶＣを入力する（ス
テップ８１）。

次に、指令速度■１と前回の出力速度■１−１の差を積
分して、遅れ距離ＤＩを次式で求める（ステップＳ２）
。

Ｄ　ｉ　−Ｄ　ｉ−１＋Ｖ　１−　ｖ　１−１但し、Ｄ
Ｏ−０，ｖｏ　−０ この遅れ距離ＤＩは、指令値がセグメント終端に達して
ｖｌ−０となった後は、出力速度ｖｉで動く移動点ＰＩ
からセグメント終端までの残距離を表している。

次に、指令速度Ｖ＋と全快の出力速度ｖ　Ｉ−１を比較
しくステップＳ３）、前者が後者より大きい場合は加速
処理で、小さい場合は減速処理となる。

減速処理の場合はコーナ許容速度ｖｃと前回出力速度ｖ
　Ｉ−１を比較しくステップＳ４）、前者が後者より小
さい場合は上記の加速の場合と同一の一次フィルタ処理
（ステップＳ５）に入り、大きい場合はコーナ速度設定
処理（ステップＳＳ）に入る。

−次フィルタ処理は、第９図に示した従来の加減速回路
の動作を差分演算で実現するもので次式により出力速度
ｖｉを求める。

ここで、Ｔは加減速の時定数である。

コーナ速度設定処理（ステップＳ８）はコーナ許容速度
ｖｌに等しいか、これより小さく、かつこれに近い値で
、セグメントの残り距離Ｄｉを整数回のサンプリング処
理で移動し終る速度ｖｎを求めるもので次式により計算
する。

Ｃ Ｄ＋ ｖｌ　　−ｖｎ 上記の処理によって各サンプリング周期ごとの出力速度
ｖｌが得られる。

この出力速度ｖ１を積分器（４）で積分して移動距離Ｌ
１を求め、補間演算部（７）により補間演算を行い、そ
の出力△Ｘｉ、ΔＹｉに基づいてサーボ回路（８ａ）、
　　（８ｂ）がサーボモータ（９ａ）、　　（９ｂ）を
駆動し、また、セグメント終端判別部（５）によりセグ
メント終端の判別を行うことは従来のものと全く同様で
ある。

セグメント終端判別部（５）のセグメントＡの移動完了
出力により、人力部（２０）を付勢してセグメントＣの
データを入力し、次はセグメントＢとＣのデータを用い
てセグメントＢの移動処理に入る。

第５図は加減速処理の動作とセグメント移動のタイミン
グを示すタイムチャートである。図から明らかな用に、
この発明においては、従来のコーナ速度をゼロまで落す
場合に比べ、セグメントＡの移動を終了してセグメント
Ｂの移動に移る時間がＴｏたけ短縮し、その分だけ高速
化されている。

なお、上記実施例では、加減速処理部（２３）に−次フ
ィルタを用いる場合を示したが、２次以上の高次フ、イ
ルタや一定加速度の加減速特性をもつ直線加減速回路を
用いてもよい。

［発明の効果］ 以上のようにこの発明によれば、移動経路のコーナ点の
角度に応じコーナ速度を自動的に決定し、その速度まで
自動加減速するように構成したので、高速・高精度の移
動制御が実現できると共に、移動速度のプログラミング
が容易になるという効果が得られる。

【図面の簡単な説明】

第１図はこの発明の一実施例に係る速度制御装置の構成
を示すブロック図、第２図は第１図の速度制御装置の動
作説明のための移動経路を示した図、第３図はこの発明
におけるコーナ速度決定法の説明図、第４図は第１図の
加減速処理部の動作を示すフローチャート、第５図は第
１図の速度制御装置の動作タイミングを示すタイムチャ
ートである。 第６図は従来の速度制御装置の構成を示すブロック図、
第７図は従来の速度制御装置の動作説明のための移動経
路を示した図、第８図は従来の速度制御装置の動作タイ
ミングを示すフローチャート、第９図は従来の速度制御
装置の加減速回路の構成を示すブロック図である。 図において、（２１）はコーナ角計算部、（２２）はコ
ーナ速度計算部、（２３）は加減速処理部である。 なお、図中同一符号は同−又は相当部分を示す。 代理人　弁理士　佐　々　木　宗　治 第 図 第 図

Claims (1)

1. 【特許請求の範囲】 複数のセグメントで構成される移動経路の移動速度を制
   御する速度制御装置において、 コーナ点における経路の角度変化量を計算する第１の計
   算部と、 該角度変化量及び与えられた許容加速度値から該コーナ
   点における許容速度を計算する第２の計算部と、 コーナ点へ向って移動速度を前記許容速度まで減速し、
   コーナ点に到達した後、移動速度を指令速度まで加速す
   るように制御する加減速制御部とを備えたことを特徴と
   する速度制御装置。