JPH10286788A - 軌跡制御装置 - Google Patents

Abstract

(57)【要約】 【課題】 教示点の間隔が短い場合にも、高速かつ高精
度に移動軌跡を制御し得る軌跡制御装置を提供する。 【解決手段】 教示点列をブロックに分割する（Ｓ２
２）。この際、軌跡の折れ角が所定角度より鋭角になっ
ているとき、及び、次動作が工具の姿勢変化のみで移動
しないときに分割する。そして、ブロック始点から最高
速度まで加速するのに必要な距離だけ離れた加速終了点
と、ブロック終点から最高速度から速度０まで減速する
のに必要な距離だけ離れた減速開始点とを求め、ブロッ
ク始点からブロック終点までの内部分割点を設定する
（Ｓ２４）。その後、該各内部分割点を通って移動体を
移動させる（Ｓ２６〜Ｓ３６）。即ち、複数の教示点に
またがって新たに内部分割点を設定して工具を制御する
ため、教示点の間隔が短いときでも工具を最高速度で送
ることが可能となる。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、工具等の移動体の
移動軌跡を制御するための軌跡制御装置に関するもので
ある。

【０００２】

【従来の技術】この種の軌跡制御技術として、ＣＰ（
Ｃｏｎｔｉｎｕｏｕｓ Ｐａｔｈ ）制御が知られてい
る。この技術では、図９（Ａ）に例示するように、ま
ず、物体を移動させたい軌跡に沿った各点をＰ0 、Ｐ1
、Ｐ2 ・・のように定める。そして、点Ｐ0 と点Ｐ1
が与えられると、点Ｐ0 から点Ｐ1 に向かう方向（Ｐ0
→Ｐ1 方向と示す。他も同様である）が決定される。次
に、点Ｐ0 からＰ1 に向かう図９（Ｂ）に示す速度パタ
ーンＶ1 が決定される。即ち、Ｐ0 →Ｐ1 方向に先ず加
速度αで増速して、目標速度Ｖに達したらその目標速度
Ｖで移動させ、そして、−αという目標加速度（減速度
としてはαとなる）で減速させて速度ゼロまで減速され
たときにちょうど点Ｐ1 に達するような減速開始点ＡO
を求め（このときタイミングはｔ）、この減速開始点Ａ
O から減速を開始させる速度パターンを求める。

【０００３】このタイミングｔが決定されると、今度は
Ｐ1 →Ｐ2 方向の速度成分Ｖ2 に関し、図９（Ｂ）に示
すようなタイミングｔから加速度αで増速し、速度Ｖに
達した時以後定常速度とする速度パターンを算出する。
これら２つの速度パターンが決定されると、それぞれの
速度成分をベクトル加算することにより、実際の移動方
向と速度成分が算出される。図９（Ｂ）から明らかなよ
うに、Ｐ0 →Ｐ1 方向の速度成分Ｖ1 が減少すると同時
に点Ｐ1 から点Ｐ2 に向かう速度成分Ｖ2 が増大し、タ
イミングｔとｔ’において、いずれか一方の速度成分は
目標速度Ｖとなり、他方の速度成分はゼロとなる。従っ
て、タイミングｔとｔ’においても合成された速度はほ
ぼ目標速度Ｖを保つことになる。

【０００４】このため、ベクトル加算された方向と速度
とに従って物体を移動させると、タイミングｔ以前には
Ｐ0 →Ｐ1 方向に目標速度Ｖで移動する。タイミングｔ
からｔ’の間は、Ｐ0 →Ｐ1 方向の速度成分Ｖ1 が小さ
くなりつつＰ1 →Ｐ2 方向の速度成分Ｖ2 が大きくな
る。ここで、タイミングｔ’でＰ0 →Ｐ1 の速度成分が
ゼロになるため、タイミングｔ’で物体は点Ｐ1 と点Ｐ
2 とを結ぶ直線上にいることになる。

【０００５】なお、上記の計算が点Ｐ0 →点Ｐ1 に引き
続き、点Ｐ1 →点Ｐ2 →点Ｐ3 ・・・についても行われ
る（なお、点Ｐ3 は図示されていない）。このようにし
て、物体の移動方向と速度とが決定されると、物体は図
９（Ａ）の軌跡に示すように、点Ｐ0 →点Ｐ1 →点Ｐ2
を結ぶ直線にほぼ沿って、これをなめらかに内回りしな
がら移動することになる。

【０００６】

【発明が解決しようとする課題】ここで、図９（Ａ）に
示すように教示点間隔が長い場合には、前述の如く例え
ば点Ｐ0 から点Ｐ1 の区間に関して図９（Ｂ）に示す速
度パターンＶ1 から分かるよう、目標加速度αで加速し
て目標速度Ｖに達する。そして、目標速度Ｖで被搬送物
体を送った後に、減速開始点ＡO から点Ｐ0 、点Ｐ1 方
向の速度成分に関して−αで減速すると共に、次の点Ｐ
1 、点Ｐ2 方向の速度成分Ｖ2 について加速度αで加速
が行える。即ち、減速開始点ＡO に到達した後も、一方
の速度成分Ｖ1 を減少させると同時に他方の速度成分Ｖ
2 を増大させるため点ＡO から点ＢO までの速度成分の
合成を行う区間においても、目標速度Ｖを維持できる。
そして、次の点Ｐ1 から点Ｐ2 に向かう区間について
も、また、図示しない点Ｐ3 以降の区間においてもこの
目標速度Ｖを維持することが可能である。

【０００７】これに対して、図９（Ｃ）に示すように教
示点の間隔が短い場合には、例えば点Ｐ0 から点Ｐ1 の
区間に関して図９（Ｄ）に示す速度パターンＶ1 から分
かるように、目標加速度αで加速中に減速開始点ＡO に
達し、加速度−αで減速を開始せねばならず目標速度Ｖ
に達することができなかった。このため目標速度Ｖに達
しない三角形の速度パターンＶ1 とこれに続く三角形の
速度パターンＶ2 （点Ｐ1 から点Ｐ2 方向の速度成分）
を減速開始点ＡO から合成しても、合成された速度はこ
れらの速度パターンの三角形の頂点を結ぶ線上にあり、
そして、次の点Ｐ1 から点Ｐ2 向かう速度成分Ｖ2 の区
間以降においても、合成される速度はこれら三角形の頂
点を結ぶ線上（二点鎖線で示す）の値Ｖ0 に留まった。

【０００８】即ち、ＣＰ制御では、教示点の間隔が短い
場合に移動体の送り速度は遅くなる。このため、特願平
８−１５０５２６に示されているパラレルリンクを用い
る加工機で、金型加工を行おうとすると、金型加工は教
示点の間隔が非常に短いく、上記ＣＰ制御では、パラレ
ルリンクの特徴である高速性を生かすことができない。

【０００９】一方、上記ＣＰ制御の問題に対応するた
め、本出願人は、特開平６−１９５１１４号にて、移動
体の送り速度を早める技術を提案した。この技術では、
複数の教示点にまたがって制御を行うことにより速度を
高めている。例えば、図９（Ｃ）に示す教示点Ｐ１〜Ｐ
９を処理する際に、教示点Ｐ１、Ｐ３、Ｐ５、Ｐ７、Ｐ
９に基づき制御することで、速度を高めている。しかし
ながら、この方法で、速度を高めることができる反面、
教示点に沿って忠実に工具を送ることができないため、
金型加工に求められる加工精度を得ることができないこ
とがあった。

【００１０】更に、本出願人は、特開平６−１９５１１
６号にて、複数の教示点にまたがって制御を行うことに
より速度を高める方法を提案した。例えば、図９（Ｃ）
に示す教示点Ｐ１〜Ｐ９を処理する際に、教示点Ｐ１、
Ｐ２、Ｐ３、Ｐ４、Ｐ５、Ｐ６、Ｐ７、Ｐ８、Ｐ９を一
連の通路と見なし制御することで、速度を高めている。
しかしながら、この技術は、終点Ｐ９のひとつ手前の教
示点Ｐ８から減速を開始するため、終点Ｐ９と教示点Ｐ
８との距離が短い場合には、減速を行い得ない。即ち、
終点Ｐ９と終点手前の教示点Ｐ８との距離が短い際に
は、終点（教示点）Ｐ９の位置変更をオペレータに求め
ており、移載ロボット等では、この位置変更は可能であ
るが、工作機械において位置変更を行うことは現実的で
はない。

【００１１】本発明は、上述した課題を解決するために
なされたものであり、その目的とするところは、教示点
の間隔が短い場合にも、高速かつ高精度に移動軌跡を制
御し得る軌跡制御装置を提供することにある。

【００１２】

【課題を解決するための手段】上記目的を達成するた
め、請求項１の軌跡制御装置では、最始点および最終点
を含む指令点列に沿って移動体の軌跡を制御する軌跡制
御装置において、前記指令点列をブロック始点及びブロ
ック終点を含む指令点列からなるブロックに分割するブ
ロック分割手段と、前記軌跡上でかつ速度０から最高速
度まで加速するのに必要な距離だけ前記ブロック始点か
ら離れた加速終了点を演算する手段と、前記軌跡上でか
つ最高速度から速度０まで減速するのに必要な距離だけ
前記ブロック終点から離れた減速開始点を演算する手段
と、前記軌跡上でかつ前記ブロック始点から前記加速終
了点までの間に移動体を加速運動させるための内部分割
点を設定する手段と、前記軌跡上でかつ前記加速終了点
から前記減速開始点までの間に移動体を等速運動させる
ための内部分割点を設定する手段と、前記軌跡上でかつ
前記減速開始点から前記ブロック終点までの間に移動体
を減速運動させるための内部分割点を設定する手段と、
前記ブロック始点から前記各内部分割点を通って前記ブ
ロック終点に至る軌跡上を移動体を移動させる手段と、
からなることを技術的特徴とする。

【００１３】また、請求項２では、請求項１において、
前記ブロック分割手段は、軌跡の折れ角が所定角度より
鋭角になっているとき、ブロック始点から指令点までの
距離がブロック始点から前記指令点の一つ手前の指令点
までの距離よりも短くなっているとき、指令点が最終点
であるとき、のいずれかの条件を満たす指令点におい
て、ブロックに分割することを技術的特徴とする。

【００１４】上記目的を達成するため、請求項３の軌跡
制御装置では、基台に対してパラレルリンク機構を介し
て保持された工具を備え、工具の位置および姿勢を制御
可能な工作機械において、工具を最始点および最終点を
含む指令点列に沿って移動させる軌跡制御装置であっ
て、前記指令点列をブロック始点およびブロック終点を
含む指令点列からなるブロックに分割するブロック分割
手段と、前記軌跡上でかつ速度０から最高速度まで加速
するのに必要な距離だけ前記ブロック始点から離れた加
速終了点を演算する手段と、前記軌跡上でかつ最高速度
から速度０まで減速するのに必要な距離だけ前記ブロッ
ク終点から離れた減速開始点を演算する手殴と、前記軌
跡上でかつ前記ブロック始点から前記加速終了点までの
間に工具を加速運動させるための内部分割点を設定する
手段と、前記軌跡上でかつ前記加速終了点から前記減速
開始点までの間に工具を等速運動させるための内部分割
点を設定する手段と、前記軌跡上でかつ前記加速終了点
から前記ブロック終点までの間に工具を加速運動させる
ための内部分割点を設定する手段と、前記ブロック始点
から前記各内部分割点を通って前記ブロック終点に至る
軌跡上を工具を移動させる手段とを備え、前記ブロック
分割手段は、軌跡の折れ角が所定角度より鋭角になって
いるとき、ブロック始点から指令点までの距離がブロッ
ク始点から前記指令点の一つ手前の指令点までの距離よ
りも短くなっているとき、指令点が最終点であるとき、
次動作が工具の姿勢制御のみで移動しないときのいずれ
かの条件を満たす指令点において、ブロックに分割する
ことを技術的特徴とする。

【００１５】請求項１の発明では、指令点列をブロック
に分割し、ブロック始点から最高速度まで加速するのに
必要な距離だけ離れた加速終了点と、ブロック終点から
最高速度から速度０まで減速するのに必要な距離だけ離
れた減速開始点とを求め、ブロック始点からブロック終
点までの内部分割点を設定し、該各内部分割点を通って
移動体を移動させる。即ち、複数の指令点にまたがって
新たに内部分割点を設定して移動体を制御するため、指
令点の間隔が短いときでも移動体を最高速度で送ること
が可能となる。

【００１６】請求項２の発明では、軌跡の折れ角が所定
角度より鋭角になっているとき、ブロック始点から指令
点までの距離がブロック始点から前記指令点の一つ手前
の指令点までの距離よりも短くなっているとき、指令点
が最終点であるときに、ブロックへ分割するため、移動
体を高い精度で送ることができる。

【００１７】請求項３の発明では、指令点列をブロック
に分割し、ブロック始点から最高速度まで加速するのに
必要な距離だけ離れた加速終了点と、ブロック終点から
最高速度から速度０まで減速するのに必要な距離だけ離
れた減速開始点とを求め、ブロック始点からブロック終
点までの内部分割点を設定し、該各内部分割点を通って
移動体を移動させる。即ち、複数の指令点にまたがって
新たに内部分割点を設定して工具を制御するため、指令
点の間隔が短いときでも工具を最高速度で送ることが可
能となる。また、軌跡の折れ角が所定角度より鋭角にな
っているとき、及び、次動作が工具の姿勢変化のみで移
動しないときに、ブロックへ分割するため、工具を高い
精度で送ることができる。

【００１８】

【発明の実施の形態】以下、本発明の軌跡制御装置の実
施形態について図を参照して説明する。図１は本第１実
施態様の工作機械１０を適用した工作機械全体の構成を
示した図である。工作機械１０は工作物Ｗを載置するテ
ーブル５２を有した門型のフレーム５０の天井に支持柱
５１を介して取り付けられており、この工作機械１０に
取り付けられた工具Ｔを所望の位置に移動して工作物Ｗ
を加工するものである。

【００１９】次に、図２に基づき工作機械１０について
詳細に説明をする。図２に示すように工作機械１０は、
主に、支持柱５１によって外部に固定される基台１１
と、ドリルやエンドミル等の工具Ｔを取り付けるトラベ
リングプレート１２と、前記基台１１および前記トラベ
リングプレート１２を連結する６本のアーム１４とから
構成されている。

【００２０】基台１１は６角形状の部材であり、周囲に
３つの支持部１１ａが等間隔で設けられており、この支
持部１１ａに後述するアーム１４が２本ずっ合計６本連
結されている。アーム１４はロッド１５およびガイド２
０から構成されており、６本とも同じ構成となってい
る。

【００２１】ガイド２０はベース２２、スライドテーブ
ル２６、ボールネジ２４およびモータ２５Ｕ、２５ｕ、
２５Ｖ、２５ｖ、２５Ｗ、２５ｗから構成されている。
該モータ２５Ｕ、２５ｕ、２５Ｖ、２５ｖ、２５Ｗ、２
５ｗには、それぞれモータ位置検出用エンコーダ３１
Ｕ、３１ｕ、３１Ｖ、３１ｖ、３１Ｗ、３１ｗが取り付
けられている。ベース２２は断面形状がコ型をした部材
であり、それぞれのベース２２は基台１１に対して所定
角度α（例えば４５度）傾斜して放射状に基台１１に固
定されている。このベース２２にはその長手方向にスラ
イドテーブル２６が摺動可能に支持されている。また、
ベース２２にはスライドテーブル２６の図略のナットと
螺合するボールネジ２４が回動可能に支持されており、
ベース２２に固定され前記ボールネジ２４に連結される
モータ２５Ｕ、２５ｕ、２５Ｖ、２５ｖ、２５Ｗ、２５
ｗ（駆動装置）を駆動することにより、ボールネジ２４
を回動し、結果としてスライドテーブル２６をベース２
２の長手方向に移動するようになっている。

【００２２】上述したスライドテーブル２６にはそれぞ
れロッド１５がボールジョイント１６（対偶）により連
結され、ボールジョイント１６を支点としてロッド１５
はスライドテーブル２６に対して３次元方向に揺動可能
となっている。また、各ロッド１５の他端はトラベリン
グプレート１２にボールジョイント１７（対偶）にて連
結され、ボールジョイント１７を支点としてロッド１５
はトラベリングプレート１２に対して３次元方向に揺動
可能となっている。

【００２３】トラベリングプレート１２は前記ロッド１
５の他端がボールジョイント１７により同一平面上に連
結されており、下部にはドリルやエンドミル等の工具を
取り付ける図略の主軸が設けられている。以上の構成に
より、図略の制御装置より動作指令を与えることによっ
て、ガイド２０のモータ２５Ｕ、２５ｕ、２５Ｖ、２５
ｖ、２５Ｗ、２５ｗを個別に駆動して６本のロッド１５
をそれぞれ独立して揺動させる。すると、この６本のロ
ッド１５の揺動の組み合わせにより、トラベリングプレ
ート１２を６自由度制御（位置および姿勢制御）するこ
とができる。つまり、各支持部１１ａに支持された２本
１組のアーム１４を同期して３組のアーム１４を個別に
駆動することによりトラベリングプレートの位置を決定
し、各支持部１１ａに固定された２本１組のアーム１４
のうちの各１本、即ち合計３本のアーム１４を駆動する
ことによりトラベリングプレート１２の姿勢を決定する
ことができるようになっている。そして、トラベリング
プレート１２に取り付けられた工具を所望の位置および
姿勢に移動し、工作物の加工を行うようになっている。

【００２４】以上に述べたように本第１実施態様の工作
機械は、１本のロッド１５で構成されるアーム１４を６
本にてトラベリングプレート１２を駆動するので、トラ
ベリングプレート１２（工具Ｔ）の位置制御および姿勢
制御（６自由度制御）が行える。また、トラベリングプ
レート１２を駆動するアーム１４の構成をスライドテー
ブル２６を移動させることにより揺動する１本のロッド
１５にて行うようにしたため、加工によりトラベリング
プレート１２に作用した加工反力は、すべてのロッド１
５に作用する圧縮力または引っ張り力に変換される。そ
して、この加工反力はスライドテーブル２６とロッド１
５の両端を連結するボールジョイント１６、１７（対
偶）を介してスライドテーブル２６を駆動する軸力とし
て伝わる。つまり、加工反力はロッド１５およびボール
ネジ２４の軸方向の荷重として作用するため、直接モー
タ２５Ｕ〜２５ｗに大きな負荷が作用することがないと
ともに、アーム１４に曲げ力が一切作用しないため、高
負荷の作用する加工が高速で行える。

【００２５】引き続き、図３を参照して制御装置７０の
構成について説明する。制御装置７０は、ＣＰＵ７１、
メモリ７２、インタフェイス７３、７４から構成されて
いる。メモリ７２には実加工処理を実行するためのプロ
グラムが記憶されている。インタフェイス７３には、上
述したサーボモータ２５Ｕ、２５ｕ、２５Ｖ、２５ｖ、
２５Ｗ、２５ｗを駆動するデジタルサーボユニット８１
〜８６が接続されている。各デジタルサーボユニット８
１〜８６はＣＰＵ７１からの指令値（目標値）に基づい
てサーボモータ２５Ｕ、２５u、２５Ｖ、２５ｖ、２５
Ｗ、２５ｗをそれぞれ駆動し、各モータ位置検出用エン
コーダ３１Ｕ、３１ｕ、３１Ｖ、３１ｖ、３１Ｗ、３１
ｗからのに出力によってフィードバック制御を行う。そ
して、サーボモータ２５Ｕ、２５u、２５Ｖ、２５ｖ、
２５Ｗ、２５ｗによって駆動されるそれぞれのスライド
テーブル２６を所望の位置にそれぞれ移動することによ
り、結果として、６本のロッド１５を介して連結される
トラベリングプレート１２を所望の位置及び姿勢に制御
するようになっている。

【００２６】インタフェイス７４には、後述する加工デ
ータ等を入力するキーボード７６、加工データや現在の
工作機械１０の状態等を表示する画像表示装置（ＣＲ
Ｔ）７７、加工データを記憶する外部記憶装置（例えば
ハードディスク）７３が接続されている。

【００２７】引き続き、制御装置７０による処理につい
て、図４〜図６のフローチャートを参照して説明する。
制御装置７０は、先ず、工作機械１０による金型加工用
のプログラムを入力する（Ｓ１２）。次に、該プログラ
ムを解読（デコード）し（Ｓ１４）、デコードした命令
が終了命令かを判断する（Ｓ１６）。終了命令でない限
り（Ｓ１６がＮｏ）、命令が動作命令かを判断する（Ｓ
１８）。動作命令ではない場合には（Ｓ１８がＮｏ）、
非動作処理を進める（Ｓ２０）。例えば、冷却・潤滑用
のクーラントの噴出、停止、工具送りの一時停止等の処
理を行う。

【００２８】一方、命令が動作命令のときは（Ｓ１８が
Ｙｅｓ）、ステップ２２のブロック生成処理を行う。該
ブロック処理では、与えられた教示点列を、１つの加速
−等速−減速にて工具を送れるブロックへ分割して行
く。この処理のサブルーチンを示す図５を参照して、当
該ブロック生成処理について詳細に説明する。ここで
は、図７（Ａ）、図７（Ｃ）に示す教示点（指令点）列
を処理するものとして説明する。なお、ティーチングプ
レイバッグ式のロボット等においては教示点、数値制御
工作機械においては指令点という表現が適切と思われる
が、本実施態様においては、教示点と指令点とは、特に
区別していない。

【００２９】先ず、開始点Ｐi を取得する（Ｓ５２）。
図７（Ａ）に示す例では、開始点として教示点Ｐ１を取
得する。次に、変数ｎを０に初期化する（Ｓ５４）。そ
して、次の教示点Ｐi+1+n 、ここでは教示点Ｐ２を取得
し（Ｓ５６）、点Ｐi+n （教示点Ｐ１）からＰi+1+n
（教示点Ｐ２）までのベクトルを演算し（Ｓ５８）、該
開始点Ｐi （教示点Ｐ１）からＰi+1+n （教示点Ｐ２）
までの距離を演算する（Ｓ６０）。その後、次次の教示
点Ｐi+2+n 、ここでは教示点Ｐ３を取得し（Ｓ６２）、
次点（教示点Ｐ２）から次次点Ｐi+2+n （教示点Ｐ３）
までのベクトルを演算し（Ｓ６４）、該開始点Ｐi （教
示点Ｐ１）からＰi+2+n （教示点Ｐ３）までの距離を演
算する（Ｓ６６）。

【００３０】引き続き、上記ステップ５８にて求めたＰ
i+n −Ｐi+1+n のベクトルと、上記ステップ６４にて求
めたＰi+1+n −Ｐi+2+n のベクトルとから、両者の角度
が所定角度（例えば１５０°）以上かを判断する（Ｓ６
８）。即ち、教示点Ｐ１から教示点Ｐ２へ、更に教示点
Ｐ３へ工具を送った際に、軌跡の折れ角が１５０°以
上、言い換えるならば、該教示点Ｐ２にて方向が大きく
変化するかを判断する。方向が大きく変化し高速で工具
を送ると、工具軌跡が教示点から離れてしまう場合には
（Ｓ６８がＮｏ）、ステップ７８へ移行する。他方、大
きく変化せず高速で工具を送っても工具軌跡が教示点Ｐ
２から離れない場合には（Ｓ６８がＹｅｓ）、ステップ
７０へ進む。

【００３１】ステップ７０では、開始点Ｐi から次点Ｐ
i+1+n までの距離が、開始点Ｐi から次次点Ｐi+2+n ま
での距離よりも短いかを後述する理由から判断する。短
い場合には（Ｓ７０がＹｅｓ）、ステップ７２に進む。

【００３２】ステップ７２では、次点Ｐi+1+n と次次点
Ｐi+2+n とが等しいか、即ち、次点では、姿勢変化のみ
で先端の移動が無いかを判断する。図１及び図２を参照
した工作機械１０では、金型加工において、工具の工作
物Ｗに当たる角度を変えることがあり、この際には、工
具の移動を停止しないと適切に加工が進められなくな
る。このため、姿勢変化のみで先端の移動が無いときに
は（Ｓ７２がＮｏ）、ステップ７８へ移行して該次点で
工具の送りを一旦停止するための処理を行う。他方、先
端の移動が有る際には（Ｓ７２がＹｅｓ）、ステップ７
４へ進む。

【００３３】ステップ７４では、教示点の最終点かを判
断する。教示点の最終点では（Ｓ７４がＹｅｓ）、工具
の移動を停止させるためステップ７８へ移行する。他
方、最終点では無いときには（Ｓ７４がＮｏ）、ステッ
プ７６へ進み、変数ｎに１を加え、ステップ５６へ戻
る。

【００３４】ステップ５６〜ステップ６０では、次点Ｐ
i+1+n を教示点Ｐ３として処理を進め、ステップ６２〜
ステップ６６では、次次点Ｐi+2+n を教示点Ｐ４として
処理を進める。この処理を繰り返し、次点Ｐi+1+n を図
７（Ａ）に示すＰ５とした際、Ｐi+n （Ｐ４）−Ｐi+1+
n （Ｐ５）と、Ｐi+1+n （Ｐ５）−Ｐi+2+n （Ｐ６）と
の角度が図中に示すように１５０°以上となり、ステッ
プ６８における判断がＮｏとなる。即ち、教示点Ｐ４か
ら教示点Ｐ５へ、更に教示点Ｐ６へ工具を送ったとした
なら、該教示点Ｐ５にて方向が大きく変化し、工具軌跡
が教示点Ｐ５から離れてしまう。このため、ステップ７
８へ移行し、ブロック始点をＰ１とし、ブロック終点を
Ｐ５とする処理を行い、リターンする。即ち、図中で示
すＰ６を新たに開始点として上述した様に教示点をブロ
ックに分割する処理を開始する。

【００３５】上述した処理を繰り返し、図７（Ｃ）に示
すＰ１７を開始点とし、次点Ｐi+1+n をＰ２１とした
際、開始点Ｐi （Ｐ１７）から次点Ｐi+1+n （Ｐ２１）
までの距離Ｌ１が、開始点Ｐi （Ｐ１７）から次次点Ｐ
i+2+n （Ｐ２２）までの距離Ｌ２よりも図中に示す様に
短くなる。即ち、開始点Ｐ１から教示点Ｐ２２への軌跡
がＵターンしている。たとえ、軌跡の折れ角が所定角度
以上であっても、このように軌跡がＵターンする教示点
Ｐ１７からＰ２２までを高速で送ると、連続する教示点
列を正確にトレースすることが困難であるため、ステッ
プ７０の判断がＮｏとなり、ステップ７８へ移行し、ブ
ロック始点をＰ１７とし、ブロック終点をＰ２１とする
処理を行い、リターンする。

【００３６】上記ブロック生成処理の終了により、図４
に示す内部分割点設定処理（Ｓ２４）へ進む。この処理
のサブルーチンを示す図６を参照して、当該内部分割点
設定処理について詳細に説明する。この内部分割点設定
処理では、分割されたブロックの始点から終点までを工
具を送るための内部分割点を設定する処理を行う。ここ
では、図７（Ａ）に示す上記ブロック生成処理にて分割
された教示点を列処理するものとして説明する。

【００３７】先ず、ブロック始点Ｐi （Ｐ１）からブロ
ック終点Ｐi+1+n （Ｐ５）までの折れ線の全長を算出す
る（Ｓ８２）。即ち、Ｐ１→Ｐ２、Ｐ２→Ｐ３、Ｐ３→
Ｐ４、Ｐ４→Ｐ５間の距離をそれぞれ求め、求めた距離
を加算する。

【００３８】次に、設定加速度αと算出した全長から加
速終了点と減速開始点を算出する（Ｓ８４）。即ち、図
７（Ｄ）に示す様に、始点Ｐ１→教示点Ｐ２→教示点Ｐ
３方向に先ず加速度αで増速して、目標速度Ｖに達する
地点（加速終了点Ａ０）を求め、目標速度Ｖに達したら
その目標速度Ｖで移動させ、そして、−αという目標加
速度（減速度としてはαとなる）で減速させて速度ゼロ
まで減速されたときにちょうど終点点Ｐ５に達するよう
な減速開始点Ｃ０を求め、この減速開始点Ｃ０から減速
を開始させる速度パターンを求める。ここで、目標速度
Ｖは工具等、軌跡を制御したい移動体の予め定められた
最高速度であり、目標加速度αは該移動体の最高加速度
である。

【００３９】そして、図７（Ｄ）に示す速度パターンに
沿って補間周期で分割し、補間周期毎の移動距離を演算
する（Ｓ８６）。

【００４０】引き続き、ステップ８６にて演算した移動
距離を基に、図７（Ａ）に示す始点Ｐ１から終点Ｐ５ま
での折れ線に沿って、図７（Ｂ）に示すように内部分割
点を設定する（Ｓ８８）。即ち、始点Ｐ１から加速完了
点までを工具を加速運動させるための内部分割点Ａ１を
設定する。次に、ブロックの終点かを判断する（Ｓ９
０）。ここでは、該判断がＮｏとなり、ステップ８６へ
戻り、補間周期毎の移動距離を演算し、ステップ８８に
て、更に、加速完了点までを加速運動させるための内部
分割点Ａ２を設定する。上記ステップ８６、８８を繰り
返すことで、加速完了点までを加速運動させるための内
部分割点Ａ３を、該加速完了点から減速開始点Ｃ０まで
を等速運動させるための内部分割点Ｂ１、Ｂ２、Ｂ３、
Ｂ４を、更に、該減速開始点から終点Ｐ５までを減速運
動させるための内部分割点Ｃ１、Ｃ２、Ｃ３を設定す
る。そして、終点Ｐ５まで処理を進めると、ステップ９
０の判断がＹｅｓとなって、該内部分割点設定処理が終
了する。

【００４１】図４に示す内部分割点設定処理（Ｓ２４）
に続いて、ステップ２６にて加速区間かを判断する。図
７（Ｂ）に示す加速区間（例えば内部分割点Ａ１→Ａ
２）を工具を送る際には（Ｓ２６がＹｅｓ）、ステップ
２８へ移行して、工具を該内部分割点Ａ１→Ａ２に沿っ
て加速度αにて送る加速処理を行う。また、等速区間
（例えば内部分割点Ｂ２→Ｂ３）を工具を送る際には
（Ｓ３０がＹｅｓ）、ステップ３２へ移行して、工具を
該内部分割点Ｂ２→Ｂ３に沿って速度Ｖにて送る等速処
理を行う。減速区間（例えば内部分割点Ｃ１→Ｃ２）を
工具を送る際には（Ｓ３４がＹｅｓ）、ステップ３６へ
移行して、工具を該内部分割点Ｃ１→Ｃ２に沿って減速
加速度αにて送る減速処理を行う。そして、終点Ｐ５ま
で工具を送ることで、ステップ３８の判断がＹｅｓとな
って、ステップ１４へ戻り次の命令をデコードし、上記
処理を続ける。全ての命令を処理することで、ステップ
１６の判断がＹｅｓとなって、制御装置７０による処理
が完了する。

【００４２】引き続き、本実施態様の軌跡制御方式にお
ける折れ角と軌跡誤差との関係について、図８を参照し
て説明する。ここで、従来の教示点から次の教示点まで
送るＰ−Ｔ−Ｐ方式においては、図７（Ｅ）に示す、点
Ｂ1n→Ｐn →Ｂ2nの経路を通っていたのが、本実施態様
の方式では、点Ｂ1n→Ｂ2nの経路を通る。このため、指
令速度（５０、３０、１０m/min ）及び折れ角β（１５
０°、１６０°、１７０°）を変えた際の、図中ｈで示
す軌跡誤差、図中Ｆに沿った速度（実速度）を検証した
結果を図８中に示す。

【００４３】図８中に示すように指令速度を５０m/min
（単位時間内移動距離８．３３mm）で、折れ角が１５０
°のときに要求精度値（１mm）を越える。従って、折れ
角を１５０°に設定した際には、指令速度は１０〜４０
m/min が適切である。このため、上述したように本実施
態様では、教示点の折れ角が１５０°以上のときには、
該教示点を終点として設定することでブロックを分割
し、工具の移動軌跡が該教示点から離れるのを防止して
いる。

【００４４】一方、速度の変化は、１５０°のときで最
低の９６．５％であり、いずれも９５％以上であるた
め、速度変化は加工品質に影響する程ではないと判断さ
れる。すなわち、要求精度値が１mm程度の金型の荒加
工、中仕上加工において４０m/min の高速加工が可能と
なる。

【００４５】前述の実施態様においては、軌跡制御装置
を金型加工様のパラレルリンク式ロボット１０に適用す
る例について説明したが、本発明の軌跡制御装置はシリ
アル式ロボットハンド等の物体の移動軌跡を制御する一
般の装置に適用できることは言うまでもない。

【００４６】

【発明の効果】請求項１の発明では、指令点（教示点）
列をブロックに分割し、ブロック始点からブロック終点
までの内部分割点を設定し、該各内部分割点を通って移
動体を移動させる。即ち、複数の指令点にまたがって新
たに内部分割点を設定して移動体を制御するため、指令
点の間隔が短いときでも移動体を最高速度で送ることが
可能となる。

【００４７】請求項２の発明では、軌跡の折れ角が所定
角度より鋭角になっているとき、ブロック始点から指令
点までの距離がブロック始点から前記指令点の一つ手前
の指令点までの距離よりも短くなっているとき、指令点
が最始点であるとき、次動作が移動体の姿勢変化のみで
移動しないときに、ブロックへ分割するため、移動体を
高い精度で送ることができる。

【００４８】請求項３の発明では、指令点列をブロック
に分割し、ブロック始点からブロック終点までの内部分
割点を設定し、該各内部分割点を通って移動体を移動さ
せる。即ち、複数の指令点にまたがって新たに内部分割
点を設定して工具を制御するため、指令点の間隔が短い
ときでも工具を最高速度で送ることが可能となる。ま
た、軌跡の折れ角が所定角度より鋭角になっていると
き、及び、次動作が工具の姿勢変化のみで移動しないと
きに、ブロックへ分割するため、移動体を高い精度で送
ることができる。

【図面の簡単な説明】

【図１】本発明の一実施態様に係る軌跡制御装置により
制御されるロボットの機械的構成を示す斜視図である。

【図２】図１に示すロボットの機械的構成を示す斜視図
である。

【図３】本発明の一実施態様に係る制御装置の構成を示
すブロック図である。

【図４】本実施態様の制御装置による処理のメインルー
チンを示すフローチャートである。

【図５】図４に示すブロック生成処理のサブルーチンを
示すフローチャートである。

【図６】図４に示す内部分割点設定処理のサブルーチン
を示すフローチャートである。

【図７】図７（Ａ）は教示点列を示す説明図、図７
（Ｂ）は内部分割点の設定を示す説明図、図７（Ｃ）は
教示点列を示す説明図、図７（Ｄ）は本実施態様の軌跡
制御方式における速度成分の台形のパターンを示す説明
図、図７（Ｅ）は本実施態様の移動軌跡の説明図であ
る。

【図８】本実施態様の軌跡制御方式における折れ角と軌
跡誤差との関係を示す図表である。

【図９】図９（Ａ）は従来技術の軌跡制御方式において
教示点の間隔が長い場合の軌跡の説明図、図９（Ｂ）
は、図９（Ａ）の速度成分Ｖ1 、Ｖ２の台形のパターン
を示す説明図である。図９（Ｃ）は従来技術の軌跡制御
方式において教示点の間隔が短い場合の軌跡の説明図、
図９（Ｄ）は図９（Ｃ）の速度成分の三角形のパターン
を示す説明図である。

【符号の説明】

１０ ロボット ７０ 制御装置 ７２ メモリ ７３ 外部記憶装置 ７４ ＣＰＵ ７６ 入力装置 ７７ ＣＲＴ Ｔ 工具（移動体） Ｗ 工作物

Claims (3)

【特許請求の範囲】
1. 【請求項１】 最始点および最終点を含む指令点列に沿
   って移動体の軌跡を制御する軌跡制御装置において、 前記指令点列をブロック始点及びブロック終点を含む指
   令点列からなるブロックに分割するブロック分割手段
   と、 前記軌跡上でかつ速度０から最高速度まで加速するのに
   必要な距離だけ前記ブロック始点から離れた加速終了点
   を演算する手段と、 前記軌跡上でかつ最高速度から速度０まで減速するのに
   必要な距離だけ前記ブロック終点から離れた減速開始点
   を演算する手段と、 前記軌跡上でかつ前記ブロック始点から前記加速終了点
   までの間に移動体を加速運動させるための内部分割点を
   設定する手段と、 前記軌跡上でかつ前記加速終了点から前記減速開始点ま
   での間に移動体を等速運動させるための内部分割点を設
   定する手段と、 前記軌跡上でかつ前記減速開始点から前記ブロック終点
   までの間に移動体を減速運動させるための内部分割点を
   設定する手段と、 前記ブロック始点から前記各内部分割点を通って前記ブ
   ロック終点に至る軌跡上を移動体を移動させる手段と、 からなることを特徴とする軌跡制御装置。
2. 【請求項２】 前記ブロック分割手段は、 軌跡の折れ角が所定角度より鋭角になっているとき、 ブロック始点から指令点までの距離がブロック始点から
   前記指令点の一つ手前の指令点までの距離よりも短くな
   っているとき、 指令点が最終点であるとき、 のいずれかの条件を満たす指令点において、ブロックに
   分割することを特徴とする請求項１の軌跡制御装置。
3. 【請求項３】 基台に対してパラレルリンク機構を介し
   て保持された工具を備え、工具の位置および姿勢を制御
   可能な工作機械において、工具を最始点および最終点を
   含む指令点列に沿って移動させる軌跡制御装置であっ
   て、 前記指令点列をブロック始点およびブロック終点を含む
   指令点列からなるブロックに分割するブロック分割手段
   と、 前記軌跡上でかつ速度０から最高速度まで加速するのに
   必要な距離だけ前記ブロック始点から離れた加速終了点
   を演算する手段と、 前記軌跡上でかつ最高速度から速度０まで減速するのに
   必要な距離だけ前記ブロック終点から離れた減速開始点
   を演算する手殴と、 前記軌跡上でかつ前記ブロック始点から前記加速終了点
   までの間に工具を加速運動させるための内部分割点を設
   定する手段と、 前記軌跡上でかつ前記加速終了点から前記減速開始点ま
   での間に工具を等速運動させるための内部分割点を設定
   する手段と、 前記軌跡上でかつ前記加速終了点から前記ブロック終点
   までの間に工具を加速運動させるための内部分割点を設
   定する手段と、 前記ブロック始点から前記各内部分割点を通って前記ブ
   ロック終点に至る軌跡上を工具を移動させる手段とを備
   え、 前記ブロック分割手段は、軌跡の折れ角が所定角度より
   鋭角になっているとき、ブロック始点から指令点までの
   距離がブロック始点から前記指令点の一つ手前の指令点
   までの距離よりも短くなっているとき、指令点が最終点
   であるとき、次動作が工具の姿勢制御のみで移動しない
   ときのいずれかの条件を満たす指令点において、ブロッ
   クに分割することを特徴とする軌跡制御装置。