JPH06214631A

JPH06214631A - 軌跡制御装置

Info

Publication number: JPH06214631A
Application number: JP2065493A
Authority: JP
Inventors: Takao Yoneda; 孝夫米田; Katsuhiro Komuro; 克弘小室; Yoshiji Yamamoto; 吉二山本
Original assignee: Toyoda Koki KK
Current assignee: Toyoda Koki KK
Priority date: 1993-01-12
Filing date: 1993-01-12
Publication date: 1994-08-05

Abstract

(57)【要約】【目的】教示点の間隔が短い場合にも、目標速度に達
するように移動軌跡を制御し得る軌跡制御装置を提供す
る。【構成】移動量加算手段が間隔の短い複数の教示点に
またがる移動量を加算し、速度制御手段が該移動量加算
手段により加算された移動量に基づき被搬送物体の送り
速度を複数の教示点にわたって制御し、速度判断手段が
速度制御手段により制御される速度が目標速度に達した
か否かを判断する。そして、該速度制御手段が該速度判
断手段により目標速度に達したと判断されるまで加速を
続け、該速度判断手段により目標速度に達したと判断さ
れると該目標速度での送りを行う。このようにして、教
示点の間隔が短い場合にも、その教示点の間隔毎に加減
速を行うことなく複数の教示点にまたがり移動量を加算
し、目標速度に達するよう速度を制御する。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、ロボットのハンド部等
の物体の移動軌跡を制御するための軌跡制御装置に関
し、特に、微細な間隔の教示点が与えられた場合にも目
標速度に達するよう移動軌跡の制御を行う軌跡制御装置
に関する。

【０００２】

【従来の技術】従来の制御（以下第１の制御法とする）
においては、図８（ａ）に例示するように、まず、物体
を移動させたい軌跡に沿った各点をＰ0 、Ｐ1 、Ｐ2 ・
・のように定めると、点Ｐ0 から点Ｐ1 に向かう方向
（Ｐ0 →Ｐ1 方向と示す。他も同様である）が決定され
る。次に、点Ｐ0 からＰ1 に向かう図８（ｂ）に示す速
度パターンＶ1 が決定される。即ち、Ｐ0 →Ｐ1 方向に
先ず加速度αで増速して、目標速度Ｖに達したらその目
標速度Ｖで移動させ、そして、−αという目標加速度
（減速度としてはαとなる）で減速させて速度ゼロまで
減速して点Ｐ1 に達する減速開始点Ａ_Oを求め、この減
速開始点Ａ_Oから減速を開始させる速度パターンを求め
る。ここで、目標速度Ｖはロボット等、軌跡を制御した
い物体の予め定められた最高速度であり、目標加速度α
は該被搬送物体の最高加速度である。この速度パターン
に従い、点Ｐ0 から点Ｐ1 に物体を送り、速度を点Ｐ1
において一旦ゼロにして、次に同様にして点Ｐ1 から点
Ｐ2 に向かい送っていた。この技術においては、各点に
おいて速度がゼロになるため被搬送物体の送り速度が遅
くなり効率が劣化した。

【０００３】この第１の制御法の改良技術として、第１
回日本ロボット学会技術講演会予稿集第１６５頁に記載
の制御（以下第２の制御法とする）が知られている。こ
の第２の制御法において、前述の図８（ａ）に示す点Ｐ
0 、点Ｐ1 、点Ｐ2が与えられると、先ず点Ｐ0 から点
Ｐ1 に向かう方向（Ｐ0 →Ｐ1 方向と示す）が決定され
る。次に、点Ｐ0 からＰ1 に向かう図８（ｃ）に示す速
度パターンＶ1'が決定される。即ち、前述の第１の制御
法と同様にＰ0 →Ｐ1 方向に先ず加速度αで増速して、
目標速度Ｖに達したらその目標速度Ｖで移動させ、そし
て、−αという目標加速度で減速させて速度ゼロまで減
速されたときにちょうど点Ｐ1に達するような減速開始
点Ａ_Oを求め（このときタイミングはｔ）、この減速開
始点Ａ_Oから減速を開始させる速度パターンを求める。

【０００４】このタイミングｔが決定されると、今度は
Ｐ1 →Ｐ2 方向の速度成分Ｖ2'に関し、図８（ｃ）に示
すようなタイミングｔから加速度αで増速し、速度Ｖに
達した時以後定常速度とする速度パターンを算出する。
これら２つの速度パターンが決定されると、それぞれの
速度成分をベクトル加算することにより、実際の移動方
向と速度成分が算出される。即ち、第１の制御法におい
ては減速開始点Ａ_Oから実際に減速が行われたのに対
し、この第２の制御法では図８（ｃ）から明らかなよう
に、Ｐ0 →Ｐ1 方向の速度成分Ｖ1'の減少を開始すると
同時に点Ｐ1 から点Ｐ2 に向かう速度成分Ｖ2'を増大さ
せ、タイミングｔとｔ’においても合成された速度を目
標速度Ｖを保つ。

【０００５】このため、ベクトル加算された方向と速度
とに従って物体を移動させると、タイミングｔ以前には
Ｐ0 →Ｐ1 方向に目標速度Ｖで移動する。タイミングｔ
からｔ’の間は、Ｐ0 →Ｐ1 方向の速度成分Ｖ1'が小さ
くなりつつＰ1 →Ｐ2 方向の速度成分Ｖ2'が大きくな
る。ここで、タイミングｔ’でＰ0 →Ｐ1 の速度成分が
ゼロになるため、タイミングｔ’で物体は点Ｐ1 と点Ｐ
2 とを結ぶ直線上にいることになる。従って移動軌跡は
タイミングｔとｔ’において、点Ａ₀と点Ｂ₀とを結ぶ
曲線Ｍ0 上にあることになる。なお、上記の計算が点Ｐ
0 →点Ｐ1 に引き続き、点Ｐ1 →点Ｐ2 →点Ｐ3 ・・・
についても行われる（なお、点Ｐ3 は図示されていな
い）。

【０００６】

【発明が解決しようとする課題】ここで、図８（ａ）に
示すように教示点の間隔が長い場合には、第２の制御法
を用いれば、例えば前述の点Ｐ0 から点Ｐ1 の区間に関
して図８（ｃ）に示す速度パターンの如く、目標加速度
αで加速して一度目標速度Ｖに達すると、減速開始点Ａ
_Oに到達した後も、速度成分Ｖ1'を減少させると同時に
次の速度成分Ｖ2'を増大させるため点Ａ_Oから点Ｂ_Oま
での速度成分の合成を行う区間においても目標速度Ｖを
維持できる。そして、次の点Ｐ1 から点Ｐ2 に向かう区
間についても、また、図示しない点Ｐ3 以降の区間にお
いてもこの目標速度Ｖを維持することが可能になる。

【０００７】これに対して、図９（ａ）に示すように教
示点の間隔が短い場合には、従来の第１の制御法におい
ては、一の点から次の点に向かい加速度αで加速中に減
速開始点Ａ_Oに到達して減速を開始するため、速度パタ
ーンは、図９（ｂ）に示すように、三角のパターンの連
続となる。同様に第２の制御法を用いても、例えば点Ｐ
0 から点Ｐ1 の区間に関して図９（ｃ）に示す速度パタ
ーンＶ1'から分かるよう、目標加速度αで加速中に減速
開始点Ａ_Oに達し、加速度−αで減速を開始せねばなら
ず目標速度Ｖに達することができない。このため目標速
度Ｖに達しない三角形の速度パターンＶ1'とこれに続く
三角形の速度パターンＶ2'（点Ｐ1 から点Ｐ2 方向の速
度成分）を減速開始点Ａ_Oから合成しても、合成された
速度はこれらの速度パターンの三角形の頂点を結ぶ線上
にあり、そして、次の点Ｐ2 から点Ｐ3 向かう速度成分
Ｖ3'の区間以降においても、合成される速度はこれら三
角形の頂点を結ぶ線上（二点鎖線で示す）の値Ｖ0 に留
まった。このため教示点の間隔が短い場合には、第２の
制御法を用いても被搬送物体の送り速度は遅くなり、サ
イクルタイムが長くなって効率が劣化した。

【０００８】本発明は、上記課題を解決するために成さ
れたもので、その目的とするところは、教示点の間隔が
短い場合にも、目標速度に達するように移動軌跡を制御
し得る軌跡制御装置を提供することにある。

【０００９】

【課題を解決するための手段】上記課題を解決するため
の発明の構成は、図１に示すように、複数の教示点にま
たがる移動量を加算する移動量加算手段と、前記移動量
加算手段により加算された移動量に基づき被搬送物体の
送り速度を複数の教示点にまたがり制御する速度制御手
段と、速度制御手段により制御される速度が目標速度に
達したか否かを判断する速度判断手段とを有し、該速度
制御手段が速度判断手段により目標速度に達したと判断
されるまで加速を続け、該速度判断手段により目標速度
に達したと判断されると該目標速度での送りを行うこと
を特徴とする。

【００１０】

【作用】上記の手段によれば、移動量加算手段が間隔の
短い複数の教示点にまたがる移動量を加算し、速度制御
手段が該移動量加算手段により加算された移動量に基づ
き被搬送物体の送り速度を複数の教示点にわたって制御
し、速度判断手段が速度制御手段により制御される速度
が目標速度に達したか否かを判断する。そして、該速度
制御手段が該速度判断手段により目標速度に達したと判
断されるまで加速を続け、該速度判断手段により目標速
度に達したと判断されると該目標速度での送りを行う。
このようにして、教示点の間隔が短い場合にも、その教
示点の間隔毎に加減速を行うことなく、複数の教示点に
またがり移動量を加算し、目標速度に達するよう速度を
制御する。

【００１１】

【実施例】以下に、本発明の軌跡制御装置の一実施例に
ついて図を参照して説明する。先ず、本実施例の軌跡制
御装置により制御される６関節を有するロボット１０に
ついて、図２を参照して説明する。ロボット１０は、ベ
ース１３に固定された脚柱１２に旋回自在に取り付けら
れたコラム１４と、第１アーム１５と、第２アーム１６
と、第３アーム１７と、塗布剤をシーリングするシーラ
ーのノズル１９とから構成されている。そして、第１関
節ａ、第２関節ｂ、第３関節ｃ、第４関節ｄ、第５関節
ｅ、第６関節ｆにより、６自由度で自在にノズル１９を
送るようにされている。

【００１２】次に、本実施例の軌跡制御装置５０の構成
を図３を参照して説明する。図２に示したロボット１０
の位置制御のための演算を行うＣＰＵ１１には、前記第
２の制御法を行うための制御情報と本実施例の高速軌跡
追従制御を行うための制御情報とを記憶したＲＯＭ２０
及びＲＡＭ３０と、制御指令を入力するオペレーティン
グボックス２７及び操作盤２６と、該ＣＰＵ１１の作成
した制御情報等を保持する外部記憶装置２９と、サーボ
制御部４０とが接続されている。このサーボ制御部４０
は、ロボット１０の第１関節ａを駆動するサーボモータ
Ｍ１乃至第６関節ｆを駆動するサーボモータＭ６までが
接続され、そして、各々のサーボモータＭ１乃至Ｍ６に
取り付けられたエンコーダＥ１乃至Ｅ６からのフィード
バック信号が帰還されるように構成されている。この軌
跡制御装置５０は、入力された教示点を基にＣＰＵ１１
が移動軌跡を演算してサーボ制御部４０に出力を送り、
該サーボ制御部４０はサーボモータＭ１乃至Ｍ６を制御
してロボット１０を駆動し、シーリングを行うノズル１
９に所望の移動軌跡を描かせるよう構成されている。

【００１３】次に、本実施例の高速軌跡追従制御の概念
について説明する。例えばＣＡＤの作成したデータを用
いるときのように、図４に示す如く短い間隔の教示点列
Ｐ0 、Ｐ1 、Ｐ2 、・・・Ｐ10が与えられた場合、従来
の第１の制御法或いは第２の制御法でこれら教示点に基
づいてロボットの制御を行うと、図９（ｂ）、（ｃ）を
参照して前述したように目標速度Ｖに達することができ
ない。そこでこの高速軌跡追従制御では、各教示点（Ｐ
0 、Ｐ1 、Ｐ2 、・・・Ｐ10）間の移動量を加算して、
図５に示す速度パターンのように全体の移動量を疑似的
に１つの間隔として扱うよう制御する。この高速軌跡追
従制御の詳細について以下更に述べる。

【００１４】先ず、ここでは、図４に示す教示点列Ｐ0
、Ｐ1 、Ｐ2 、・・・Ｐ10が与えられた場合に、これ
ら教示点に従う本実施例の軌跡制御装置５０の処理につ
いて図７のフローチャート及び図５の速度パターンを参
照して説明する。

【００１５】オペレータにより教示点列Ｐ0 、Ｐ1 、Ｐ
2 、・・・Ｐ10を含むプログラムが入力されると（ステ
ップ１）、ＣＰＵ１１は、プログラムのデコード処理を
開始する（ステップ２）。そして、デコードされた命令
が動作終了命令でない場合（判断ステップ３がＮｏ）に
は、その命令が動作命令か非動作命令かを判断する（判
断ステップ４）。ここで、命令が非動作命令の場合は
（判断ステップ４がＮｏ）、ステップ５に進みその非動
作命令に従う処理を行う。そして、命令が動作命令の場
合（判断ステップ４がＹｅｓ）、ステップ６の処理へ進
み、補間に必要なデータを作成する。ここでは、先ず入
力された教示点列に基づき第２の制御法を行うことによ
り目標速度Ｖに達し得るかを判断し、目標速度Ｖに達す
ることができる場合には第２の制御法を行うことを決定
し、また、目標速度Ｖに達しえない場合には本実施例の
高速軌跡追従制御を行うことを決定する。そして更に、
高速軌跡追従制御を行う場合には、間隔が最小分解能
（１補間周期で移動できる距離）より小さな教示点につ
いて、その教示点をパスして次の教示点を制御対象とす
るために、教示点の選択を行う。この例では、点Ｐ1 か
ら点Ｐ10までの教示点列の間隔が短いために高速軌跡追
従制御を行うことが選択され、かつ、各教示点の間隔が
全て一補間周期よりも長いためにいずれの教示点もパス
されないとして以下の説明を続ける。

【００１６】次にステップ７で、軌跡制御タスクを起動
し先ず教示点Ｐ1 へロボット１０のノズル１９を送る処
理を開始する。そして、ステップ８で指令速度、加速
度、最小減速距離Ｓを算出する。この最小減速距離Ｓ
は、図６に示すように、目標速度Ｖから目標加速度−α
で減速を行い指定された教示点で停止することができる
移動量、即ち、目標速度Ｖから減速し得る最少の移動距
離であり、図６に示す速度パターンの斜線部分に相当す
る面積として表される。これをＣＰＵ１１は、次の数１
に基づき算出する。

【数１】Ｓ＝Ｖ²／２α ・・・（数１）

【００１７】次に判断ステップ９で、この教示点が最終
のデータ（点Ｐ10）か否かを判断する。ここでは判断ス
テップ９がＮｏとなり、次の判断ステップ１０へ進む。
判断ステップ１０では、送り速度が目標速度Ｖに達した
か否かを判断する。ここでは移動開始時であるために、
判断ステップ１０がＮｏとなり、次の加速区間処理ステ
ップ１１及び補間データ作成ステップ１２での加速処理
が行われる。これらステップでは、一補間周期毎に目標
位置（点Ｐ1 ）に向かい加速度αで増速するためのデー
タを作成すると同時に、該加速を行いつつ移動した移動
量の加算を行う。次に、ステップ１６で、この作成され
たデータを基に図３に示すサーボ制御部４０に制御指令
を送りノズル１９を教示点Ｐ1 に向けて加速度αで増速
する。そして、判断ステップ１７で、１つの間隔が終了
したか、即ち、目標の教示点（ここでは点Ｐ1 ）に達し
たかを判断し、まだ点Ｐ1 に達しておらず更に補間を続
ける必要がある場合には（判断ステップ１７がＮｏ）、
ステップ１０に戻り、ステップ１１以降の処理を一補間
周期毎に繰り返す。そして、目標の教示点Ｐ1 に達する
と（判断ステップ１７がＹｅｓ）、ステップ２に戻りプ
ログラムのデコード処理を行い、次の目標教示点Ｐ2 へ
向かう処理を開始する。

【００１８】次の教示点Ｐ2 についても、同様にステッ
プ３からステップ９までの処理が行われる。そして、判
断ステップ１０は、図５の速度パターンに示すように教
示点Ｐ2 への処理が開始された際にまだ目標速度Ｖに達
していないのでＮｏとなり、ステップ１１、１２、１６
で、教示点Ｐ1 までの移動量に加えて新たな移動量を加
算し、この移動量を基に加速度を求め増速を行う加速処
理が遂行される。これら処理により目標教示点Ｐ2 に達
すると判断ステップ１７がＹｅｓとなり、再び、ステッ
プ２に戻りプログラムのデコード処理を行い、次の目標
教示点Ｐ3 へ向かっての処理を開始し、ステップ３から
ステップ９までの処理を行う。そして、判断ステップ１
０は、図５の速度パターンに示すように教示点Ｐ3 に向
かう処理の開始当初は目標速度Ｖに達していないのでＮ
ｏとなり、ステップ１１、１２、１６で、教示点Ｐ2 ま
での移動量に加えて新たな移動量を加算し、この移動量
を基に加速度を求め増速を行う加速処理を継続する。そ
して、補間周期単位で加速を続け、タイミングｔ１にお
いて目標速度Ｖに到達すると判断ステップ１０がＹｅｓ
となる。

【００１９】目標速度Ｖに達して判断ステップ１０がＹ
ｅｓとなると、等速区間処理のステップ１３及び補間デ
ータ作成ステップ１２での等速処理が開始される。これ
らステップでは、一補間周期毎に等速度（目標速度）で
の送りを行うためのデータを作成すると同時に、該目標
速度で移動した移動量の加算を行う。そして、ステップ
１６で、この作成されたデータを基に図３に示すサーボ
制御部４０に制御指令を送りロボット１０のノズル１９
を目標速度Ｖで送る。そして、目標教示点Ｐ3に到達す
ると、判断ステップ１７がＹｅｓとなりステップ２の処
理に戻り、次の教示点Ｐ4 以降について、図４に示す各
教示点Ｐ4 Ｐ5 ・・Ｐ9 上を目標速度Ｖで送る等速処理
を行っていく。

【００２０】教示点Ｐ9 までノズル１９が送られ、最終
の教示点である点Ｐ10についの処理が開始されると、最
終データか否かの判断ステップ９がＹｅｓとなり、次の
判断ステップ１８で、最終教示点Ｐ10までの移動量（間
隔）が最小減速距離Ｓ以下か否かの判断を行う。この間
隔が最小減速距離Ｓよりも小さい場合（判断ステップ１
８がＮｏ）にはステップ１９へ進む。ステップ１９で
は、入力された教示点では減速を行い得ないとしてブザ
ーを鳴らし、オペレータに教示点の変更を促す。これに
より、オペレータは最終教示点Ｐ10を更にＰ9 から離し
た位置に変更する。他方、間隔が最小減速距離Ｓよりも
大きい場合（判断ステップ１８がＹｅｓ）には、判断ス
テップ１０以降の処理へ進む。

【００２１】ステップ１０の判断は、教示点Ｐ10へ向か
う処理の処理開始時に目標速度Ｖに達しているのでＹｅ
ｓとなり、同様に判断ステップ１３もＹｅｓとなり、ス
テップ１４へ進み、等速区間処理を行い目標速度Ｖでノ
ズル１９を送り続ける。そして、タイミングｔ２におい
て教示点Ｐ10までの残り移動量が最小減速距離Ｓと等し
くなると、判断ステップ１３がＮｏとなり、最終点処理
ステップ１５及び補間データ作成ステップ１２での減速
処理を開始する。ここでは、一補間周期毎に減速を行う
ためのデータを作成すると同時に、該減速を行いつつ移
動した移動量の加算を行う。そして、ステップ１６で、
この作成されたデータを基にサーボ制御部４０に制御指
令を送りロボット１０のノズル１９を加速度−αで減速
する。最終の教示点Ｐ10に到達すると、ステップ２での
デコード処理により動作終了の命令がデコードされ（判
断ステップ３がＹｅｓ）、全ての処理が終了する。

【００２２】なお、本実施例では、間隔の短い教示点の
処理についてのみ説明を行ったが、本実施例の軌跡制御
装置５０は、前述したようにステップ６の補間に必要な
データを作成の際に、入力された教示点列の間隔に基づ
き第２の制御法を行うことにより目標移動速度Ｖに達し
得るかを判断し、目標速度に達することができる場合に
は第２の制御法を行うことを、また、目標速度に達しえ
ない場合には上述した高速軌跡追従制御を行うことを決
定する。従って、間隔の長い教示点列の一部に間隔の短
い教示点列の区間が存在する場合には自動的に、間隔の
長い教示点列においては第２の制御法を行い、そして、
間隔の短い教示点列については高速軌跡追従制御を行い
常に目標速度Ｖを維持する。

【００２３】この本実施例によれば、短い間隔の教示点
の移動量を加算し、得られた長い移動量に基づき速度制
御を行うため、目標速度Ｖに達し得ると共に、指定され
た教示点に従い忠実にノズル１９を送ることができる。
即ち、短い間隔の教示点が与えられた場合の送り速度を
高める別の方法として、本発明者は、短い間隔の教示点
をパス（無視）して入力された教示点列中のいくつかの
教示点のみを選択することにより長い移動量を得て送り
速度を高める方法も案出したが、この方法では本発明の
方法と異なり間隔の短い教示点をパスするため教示点に
忠実な軌跡を描きえなかった。これに対して本実施例の
方法は教示点上を忠実に軌跡を描くことができる利点が
ある。また、この実施例によればノズル１９を一定速度
Ｖで送ることができる。例えば、ロボットにシーリング
を行わせる場合には、速度変化があると塗布剤を均一に
塗布することが難しくなるが、本実施例の軌跡制御装置
は、ノズル１９を常に目標速度Ｖで送ることが可能なの
で、シーリング作業等の速度変化を小さくしたい適用箇
所に好適に用いることができる。

【００２４】なお、この実施例においては、プログラム
のデコード処理を行いつつ教示点に従いロボットの制御
を行ったが、ロボットの動作を開始する以前に前処理と
して速度パターンを算出しておくことも可能である。こ
の場合には、前述の例で教示点Ｐ9 についての処理の終
了後、最終教示点Ｐ10でノズル１９の停止を行い得るか
否かの判断を行ったのに対し、前処理により速度パター
ンを算出することにより教示点Ｐ9 に達する以前から減
速を開始することも可能になる。また、前述の実施例に
おいては、軌跡制御装置５０をシーラー用のロボット１
０に適用する例について説明したが、本発明の軌跡制御
装置はロボットのハンド等の物体の移動軌跡を制御する
一般の装置に適用できることは言うまでもない。

【００２５】

【発明の効果】本発明は、以上説明したように構成され
ており、教示点の間隔が短くても、目標速度に達するこ
とができるので、サイクルタイムを短縮でき、最適時間
での軌跡制御が可能となる。

【図面の簡単な説明】

【図１】本発明を明示するための全体構成図。

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

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

【図４】本発明の実施例の高速軌跡追従制御により軌跡
制御を行う教示点を示す図である。

【図５】本実施例の高速軌跡追従制御における速度成分
のパターンを示す図である。

【図６】本実施例の高速軌跡追従制御における速度成分
の減速のパターンを示す図である。

【図７】本実施例に係る軌跡制御装置の動作を示すフロ
ーチャートである。

【図８】従来技術の軌跡制御方式において教示点の間隔
が長い場合の軌跡と、その速度成分Ｖ1 、Ｖ２の台形の
パターンを示す図である。

【図９】従来技術の軌跡制御方式において教示点の間隔
が短い場合の軌跡と、その速度成分の三角形のパターン
を示す図である。

【符号の説明】

１０ロボット１１ＣＰＵ１９ノズル３０ＲＡＭＶ目標速度 α 目標加速度Ｓ最少移動距離

Claims

【特許請求の範囲】

【請求項１】複数の教示点にまたがる移動量を加算
する移動量加算手段と、前記移動量加算手段により加算された移動量に基づき被
搬送物体の送り速度を複数の教示点にまたがり制御する
速度制御手段と、速度制御手段により制御される速度が目標速度に達した
か否かを判断する速度判断手段とを有し、前記速度制御手段が前記速度判断手段により目標速度に
達したと判断されるまで加速を続け、該速度判断手段に
より目標速度に達したと判断されると該目標速度での送
りを行うことを特徴とする軌跡制御装置。