JPH0488510A - 軌跡制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 ［産業上の利用分野］ この発明は、例えば、工作機械、ロボット等の数値制御
装置において、指令された移動経路を高速、且つ滑らか
な速度で制御する軌跡制御装置に関するものである。

［従来の技術］ 工作機械の工具や日本゛ットのハンドなどを、与えられ
た経路に沿って高速に動かす場合、移動の開始時や停止
時、あるいは曲線経路のカーフ゛部分を移動する際には
、急激な加速度によって振動や経路誤差が発生しないよ
うに滑らかに速度制御を行なう必要がある。

一般に５空間上の自由曲線を移動経路として移動する場
合、曲線を予め微小な長さの線分セフ幻トに分割ｊして
近似する。この各セフ゛ｌ＞ｈの移動速度として、例え
ば該当する区間の曲線の曲率等に応じた速度指令値を付
加し、これらを１つのブロックテ゛−タとして数値制御
装置に順次与え、経路をスプライン関数などで補間して
移動軌跡を生成する。第４図は移動経路を示す説明図で
ある。図に示すＸ−Ｙ平面上の直線経路指令、フ′ロッ
クに＋５）、７’ロツクに＋ｌ　（５）、フ’＋１＋５
りに＋２（７）・・・を受けて移動する場合、フ′ロッ
ク指令値として与えられた移動へ゛クトルＸ、（Ｘｋ＝
（Ｘｂ、ＹＪ　：　（Ｘｍ、Ｙｉ＝）はｘ−ｙ各軸の移
動量〕、移動速度Ｆｋ、　７゛ロツクの経路長しに、及
びフ゛口ｑ’）の始端、終端における補間曲線の近似接
線へ゛クトルＴ　ｋｌ、Ｔ１．ｌＩなどから７′ロヲク
テ′−夕　を構成する。　このフ゛σツクテ゛−夕　を
数値側ｔＳ装置に順次与え、経路をスプライン関数など
で補間すれば、滑らかな移動軌跡（８）が生成される。

この時、軌跡の中間や接続点で、許容された加速度値を
越えずに５且つできるだけ高速な軌跡を発生させること
が重要である。

従来、このような軌跡制御技術としては、例えば、特開
昭６３−１４５１０５号公報、特開平１−２７９［ｉ９
９号公報などに開示されている軌跡ｉｌｌ装置がある。

その−構成例のブロック図を第５図に示す。大きく分け
て、指令値を入力して軌跡発生の前準備をするテ゛−タ
入力部（１）、軌跡上の速度信号を発生する速度生成部
（２）、速度に合わせて軌跡位置を求める軌跡生成部（
３）、及び軌跡位置に合わせ機械を駆動するサー本゛駆
動部（４）から成る。テ゛−タ入力部ＣＩ）において、
（１０）は指令値を入力する人力部、ｆｌｌｌはブロッ
クデータ　を格納するブロックデータ八ツファ　、　　
（１２＋は経路長計算部、（１３）は接線ベクトル計算
部である。速度生成部（２）において、（１６）は速度
信号発生部、（１７）は加減速処理部、（１８）加算器
である。軌跡生成部（３）において、（１５）はバッフ
ァ続出し部、（１９）はバラタ−９計算部、（２０）は
スプライン関数補間計算部、（２２）はブロック終端判
別部である。サーネ゛駆動部（４）において、（２１ａ
）はＸ軸サーボ駆動部、（２１ｂ）はＹ軸サーボ駆動部
である。

次にその動作を、第４図に示すＸ−Ｙ平面上の直線経路
指令、　フ゛ｏ、りｋ（５１，ブロックに＋１（６１、
７’（１７りに＋２（７）・・・を受けて移動する場合
について、第６図に示すフローチャートを用いながら詳
細に説明する。

まず、起動信号と共に最初のフ゛口ｑりの指令値が入力
部（１０）から７′ロフクテ′−夕へ′ツファ（１１）
に読み込まれる　（ステップｌ）。　一つのへ゛ヲフ７
　、　例えば７′ロヲクテ゛−タハ′ツ１ｙｋの内容は
、ブロック指令値として与えられた移動へ′クトル　Ｘ
　ｋ　　（Ｘ　ｋ　＝　ｆＬ、Ｙｉ＝ン　：　ｆＸｋ、
Ｙ、）はＸ−Ｙ　　各軸の移動量）、移動速度Ｆｋ、　
７’ロツクの経路長Ｌｋ。

及びフ゛ロフクの始端、終端における補間曲線の近似接
線へ゛クトルＴ。、Ｔｋ！から成る。ここで、Ｌ工は経
路長計算部（１２）で計算され、Ｔｋ＄ｉ’Ｔｌｌは接
線へ゛クトル計算部（１３）で計算される。この計算は
、人力部（Ｔ０）がブロックテ゛−夕を入力するごとに
行なわれ、その手法は特開平１−２７９６９９号公報に
開示されている。計算結果はブロックテ′−タハ′ツフ
ｙ　［１１）に格納される（ステップ゛２）、　最初の
ブロックテ゛−タハ゛ツファ（１１）にテ゛−夕が準備
できると、ｍを１に設定し、ハ゛ウフ７読出部（１５）
をイ寸勢して、　最初のブロックテ゛−タハ゛ラファ（
＋１）のテ′−タＩを送り出しくステップ°３）、軌跡
の生成が開始される。

以降、一定数のブロックテ′−タハ′フファ（１１）に
空が生ずるごとに、入力部（ｌＯ）から指令値が読み込
まれ５テ゛−タが補充される（ステップ゛４．５．６）
。

速度生成部（２）、及び軌跡生成部（３）は、一定のサ
ンブリンク′周期ΔＴごとに処理が実行され所望の軌跡
を生成する。第７図（ａｔ　、　　（ｂＪ　は各々横軸
に時間ｔ、縦軸に速度Ｖで示す速度波形である。横軸の
時間は、ブロックｍ−ブロックｍ＋２の途中までを表わ
している。

速度信号発生部（１６）は、へ゛ツ７ア読出部（１５）
で読出された経路長り、と指令速度Ｆ、力むブトされる
と、４ｊ７７’す＞り゛周期ΔＴごとに経路長し、を△
Ｔ間の移動量Ｆ、・Δ丁に分割し、速度信号Ｖ（Ｖ＝Ｆ
、・ΔＴ）として出力する。経路長り、をすへて出力し
終ると　、　次のフ′ロヲクテ゛−夕　がへ′フファ　
読出部（１５）からセットされるまで七゛口を出力する
（ステップ°１０）。この速度信号発生部（１６）から
出力された速度信号の波形が第７図（ａ）に示すもので
ある。

加減速処理部（１７）は、速度信号発生部（１６）の出
力する矩形波状の速度信号Ｖの立ち上がり、立ち下がり
部分において、傾斜を制限した波形の平滑速度信号Ｖを
作成するもので、テ′シ′タルフィルタなどで構成され
る（ステラ７°１１）。加減速処理部（１７）で出方さ
れる平滑速度Ｖは第７図（ｂ）に示すような波形となる
。加算器（１８）は、加減速処理部（１７）の出力速度
Ｖをサンブリンク゛周期Δ丁ごとに加算積分して、フ′
むり始端からの移動距離Ｄ（・Ｄ◆■）を求める（ステ
ヲ７’１２＋　。パラメータ計算部（１９）は、経路長
し、と移動距離りを用いて補間ハ”うｌ−９５（ｓ　＝
　Ｄ／Ｌ、）を計算する（ステップ゛１４＋　。スフ゛
ライン関数補間計算部（２０）は、上記で計算された補
間パラメータＳ、ブロックｍの移動へ′クトルｘ、、及
びブロックの始端、終端における接戦へ゛クトルＴ　＠
　ｃ、Ｔ　ＩＩ　Ｅを用いて、ブロック始端に対する現
すンブリンク゛時点の位置へ゛クトル　Ｐ、　　を次式
（１）　のスフ゛ライン関数補間式により計算する。

Ｐｌ＝Ｔｋ１・ｓ÷（３Ｘ　ｍ　−２Ｔ　ｋｇＴ　＋＋
ｔｌ　”　Ｓ　”−（２Ｘｓ　　Ｔｍｓ　　Ｔｂ１ｌ’
Ｓ　３−（１１さらに、現すンブリンク゛周期の移動速
度へ′クトルＶ（Ｖ・（ｖｘ、■、））を現時点の位置
へ゛クトルＰ、と前回のサンブリンク゛時点の位置へ゛
クトルＰ、−１の差として、式（２）によって求める。

ｖ＝Ｐ＋　　Ｐ＋−＋　　　　　　・・・（２）移動速
度へ゛クトルＶのＸ軸成分ＶｘをＸ軸す−本′駆動部（
２１ａ）に、Ｙ軸成分ＶＶをＹ軸す−本″駆動部（２１
ｂ＋　にそれぞれ出力する（ステップ＋５）。

フ゛口ｑ’）終端判別部（２２）は、フ′ロックの移動
距離りと経路長り、とを比較して、移動距ｆｉＤが現在
の経路長１５．を越える場合は、オーバ′分を加算器（
１８）の出力りに設定し、へ゛ブファ読出部（１５）を
付勢して次のフ゛ロフクテ゛−タへ゛ツファ　閣◆１の
指令１直を読み出す　（ステヲブＩ３）。この時はステ
ップ１４から直ちに次のブロックテ゛−タ処理に移る。

第４図に示す指令に対し５上記のような処理によって滑
らかな移動軌跡（８）が生成される。この時、軌跡上の
速度は第７図ｆｂ）に示すように各フ゛Ｏフクごとに一
旦七′口まで加減速される。

［発明が解決しようとする課題］ 従来の軌跡制御装置は以上のように各指令ブロックごと
に速度が完全にｔ′口まで減速された後、次のブロック
の処理が開始されることになっており、加減速に要する
時間が長くかかり高速移動に問題があった。

この発明は、上記のような問題点を解決するためになさ
れたもので、７゛ｏｏツク接続の速度を一定の値までし
か減速しないように制御して、高速移動を可能にする軌
跡制御装置を得ることを目的とする。

［課題を解決するための手段コ この発明に係る軌跡制御装置は、指令）゛ロックごとに
速度信号を生成する速度信号発生部、この速度信号発生
部の出力速度を平滑化して出力すると共に、ホールド信
号を受けて出力を一定に保つ加減速処理部、この加減速
処理部の出力速度を加算して指令７′口ｑ’）の移動距
離を計算する加算器、移動距離と指令ブロックの経路長
を比較し、次の指令フ゛ロフクの開始時期を判断するフ
′ロック終端判別部、加減速処理部の出力速度が次の指
令ブロックの指令速度より小さい時、加減速処理部に本
−ルド信号を送出する速度比較部、及び加算器の出力す
る移動距離と指令ブロックの経路長の比をパラメータと
して経路の補間演算を行なう補間計算部を備えたもので
ある。

［作用］ この発明において、速度比較部は軌跡の実移動速度を与
える加減速部の出力速度を、次の寅行ブロックの指令速
度より小さくなる時点で本−ルドさせるように作用し、
これによって速度を一旦ｔ′口まで減速せずに、次の指
令ブロックに迅速に接続することができる。

［実施例コ この発明の一寅施例による軌跡制御装置の構成を第１図
に示す。図において、（３０）は速度比較部である。ま
た、へ゛ブフ７読出部（３１）、及び加減速処理部（３
２）はそれぞれ従来のものの機能を一部改良したもので
あり、その他の構成部は従来と同−又は相当の機能をも
つものである。

この実施例による軌跡制御装置の動作を第２図に示すフ
ローチャート、及び第３図（ａ）　、　　（ｂｌ　に示
す速度の波形図を参照して説明する。第２図のフローチ
ャートにおいて、第６図に示す従来例と同一、又は相当
のステップについては、その記載を省略する。第３図（
ａｌ　、　（ｂｌは横軸に時間ｔ、縦軸に速度■を示し
たものである。

まず、テ′−タ入力部（１）の動作は従来とほぼ同じで
あり、起動信号によって経路のフ′ロック指令値が入力
部（１）から入力され、経路長、及びブロックの始端、
終端における接線へ′クトルが計算され、指令値と共に
フ′ロックテ゛−タハ゛テファ（１１）に順次格納され
る。

フ゛ロフクテ′−タハ′ツファ（ｌ　ｌ）に最初の２　
フ゛ロヲク　のテ′−夕が準備できたとき、ｍを１とし
て５へ′ツファ読出部（３１）を付勢して、　フ゛ロブ
クチ′−タハ　ツファ（川から最初の）゛ロックｍのテ
゛−夕を読み出して軌跡生成を開始する。この時、へ′
ヲフ７読出部（３１）には、次のフ゛口９りの指令速度
Ｆ１．も読み出される。

速度信号発生部（２）は、従来のものと全く同じでへ′
ヲファ読出部（３１）の読み出した経路長し、と速度指
令値Ｆ、がセットされると、△ＴＦ’Ｊ期のサンツブり
ンク゛割込ごとに経路長し、をこの間の移動量Ｆ、・６
丁に分割し、速度信号Ｖとして出力する。経路長し、を
全で出力し終ると、次の７′ロフクテ゛−夕がへ゛ツフ
ァ読出部（３１）からセットされるまで七′口を出力す
る（ステップ２０）、これにより速度信号■は、第３図
ｆａ）に示すように断続する矩形波状の信号となる。第
３図はブロックｍからブロックｍ＋２の途中までの波形
を示している。

加減速処理部（３２）は、通常は従来と同じく速度信号
発生部（１６）が出力する矩形波状の速度信号Ｖを平滑
化するフィルタとして動作し、平滑速度信号Ｖを出力す
る（ステヲブ２１＋。

速度比較器（３０）は、平滑速度信号■とハ゛ツフ７読
出部（３１）から出力される次のフ゛ロヲクの指令速度
Ｆ。

、１・６丁とを比較し、Ｖ＜Ｆ＊＊＋・６丁の時、加減
速処理部（３２）にホールド信号を送る（ステラ７°２
２＋。

加減速処理部（３２）は、減速中にこの信号を受は取る
と減速を中断し、前回のＶの出力値をそのまま保持して
出力する（ステヲブ２３）。

以下、平滑速度信号Ｖを受けて順次動作する加算器（１
８）、パラメータ計算部（１９）、スプライン関数補間
計算部（２０）、およびフ゛ロフク終端判別器（２１）
は従来と全く同じである。即ち、加算器（１８）は各サ
ンプｌｊ）り゛周期ごとに加減速処理部（３３）の出力
する平滑速度信号■を加算してブロック始端からの移動
距離りを求め（ステップ１２）５八°ラメ一タ計算部（
１９）は移動距離りと経路長Ｌｍから補間パラメータＳ
　　（ｓ　＝　Ｄ／Ｌｍ）を求める（ステップ１４）１
次に、スプライン関数補間計算部（２０）はパラメータ
Ｓ　とフ゛ロフクテ’−９Ｘ−、Ｔｂ−１ＴｋＥを用い
て軌跡位置を補間計算し、各サン７’ｌＪ）り−周期の
Ｘ、Ｙ各軸の移動速度Ｖ　Ｘ　、Ｖ　ｖを求めて、Ｘ軸
す−本゛駆動部（２１ａｌ　、およびＹ軸す−本゛駆勅
部（２１ｂ＋の対応する軸へ出力する（ステラ７’１５
）。フ゛ロフク終端判別器（２２）は、移動距離りが経
路長り、を越えた時、へ′ッファ読出部（３１）を付勢
して次ぎのブロックテ゛−タｌ◆１を読み出しくステラ
７°１３）、これによってステップ！４以降は直ちにそ
の処理を開始することになる。

上記の処理により、平滑速度Ｖは第３図（ｂｌ　のよう
な波形となる。即ち、速度の大きさが次の指令）゛ロッ
クの指令速度より小さくなり、かつ位置がブロック終端
に達したとき、次の指令ブロックの処理が直ちに開始さ
れる。従って、次の指令速度に近い値までしか減速しな
いため、無駄な減速時間を省いた高速な軌跡の移動が可
能となる。

［発明の効果］ 上記のように５この発明によれば、入力した指令値に基
づいて制御対象の？Ｉ数の可動軸を駆動して、軌跡の制
御を行なう軌跡制御装置において、指令ブロックごとに
速度信号を生成する速度信号発生部、この速度信号発生
部の出力速度を平滑化して出力すると共に、ホールド信
号を受けて出力を一定に保つ加減速処理部、この加減速
処理部の出力速度を加算して指令フロックの移動距離を
計算する加算器、移動距離と指令フ゛口ｙりの経路長を
比較して、次の指令ブロックの開始時期を判断するブロ
ック終端判別部、加減速処理部の出力速度が次の指令ブ
ロックの指令速度より小さい時、加減速処理部にホール
ド信号を送出する速度比較部、及び加算器の出力する移
動距離と指令ブロックの経路長の比をハ。

ラメータとして経路の補間演算を行なう補間計算部を備
えたことにより、指令ブロックの接続部で速度を一旦セ
゛口まで減速することなく、次の指令速度に近い値まで
しか減速しないため、高速な軌跡の移動が可能となる軌
跡制御装置が得られる効果がある。

【図面の簡単な説明】

第１図はこの発明の一実施例による軌跡制御装置の構成
を示した７’＋７ツク図、第２図は一実施例による軌跡
制御装置の動作を示すフローチャート、第３図は一実施
例に係る速度を示す波形図、第４図は移動杆路を示す説
明図、第５図は従来の軌跡制御装置の一例を示す構成図
５第６図は従来の軌跡ｆｆ１１１ｍ装置の動作を示すフ
ローチャート、第７図は従来の軌跡制御装置に係る速度
を示す波形図である。 なお、図中５同一符号は同一、又は相当部分を示す。

Claims (1)

【特許請求の範囲】
1. 　入力した指令値に基づいて制御対象の複数の可動軸を
   駆動し、軌跡の制御を行なう軌跡制御装置において、指
   令ブロックごとに速度信号を生成する速度信号発生部、
   この速度信号発生部の出力速度を平滑化して出力すると
   共に、ホールド信号を受けて出力を一定に保つ加減速処
   理部、この加減速処理部の出力速度を加算して上記指令
   ブロックの移動距離を計算する加算器、上記移動距離と
   指令ブロックの経路長を比較し、次の指令ブロックの開
   始時期を判断するブロック終端判別部、上記加減速処理
   部の出力速度が次の指令ブロックの指令速度より小さい
   時、上記加減速処理部にホールド信号を送出する速度比
   較部、及び上記加算器の出力する移動距離と指令ブロッ
   クの経路長の比をパラメータとして経路の補間演算を行
   なう補間計算部を備えたことを特徴とする軌跡制御装置
   。