JP4170564B2

JP4170564B2 - Robot controller

Info

Publication number: JP4170564B2
Application number: JP2000181186A
Authority: JP
Inventors: 裕行牧田; 清石前川
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2000-06-16
Filing date: 2000-06-16
Publication date: 2008-10-22
Anticipated expiration: 2020-06-16
Also published as: JP2001353677A

Description

【０００１】
【発明の属する技術分野】
この発明は、搬送装置により移動中の作業対象物(以下、対象物)に対し、追従しながら作業を行う産業用ロボット(以下、ロボットという)のロボット制御装置に関するものである。
【０００２】
【従来の技術】
図８は、例えば特開平３−１５４１０３号公報に示された従来のロボット制御装置を示すブロック図である。図において、１０１は動作制御部、１０２は動作軌跡作成部、１０３は制御サイクル周期後到達位置作成部、１０４は制御サイクル周期後到達位置修正部、１０５は逆変換部、１０６は制御サイクル周期後指令パルス作成部、１０７はサーボ制御部、１０８は同期開始リミットスイッチ、１０９はコンベアエンコーダ、１１０は積算カウンタ、１１１は制御サイクル周期内コンベア移動パルス計測部、１１２は制御サイクル周期内コンベア移動量作成部、１１３は追従遅れ量加速補正処理部、１１４はコンベア移動量平均部、１１５は位置修正量演算部、１１６，１１７は経路、１１８はロボットである。
【０００３】
次に、動作について説明する。同期開始リミットスイッチ１０８をロボット１１８が作業を行う対象物が横切ると、カウンタ１１０がリセットされるとともに動作制御部１０１に対象物の同期開始点到達を知らせる。カウンタ１１０は、コンベアエンコーダ１０９から出力されるパルスにより、コンベアの移動に従ってパルスを積算し、制御サイクル周期コンベア移動パルス計測部１１１では、カウンタ１１０の値より、１制御サイクル周期の移動パルス数ΔＣｎを計測する。制御サイクル周期内コンベア移動作成部１１２では移動パルス数より、コンベア移動量ΔＬｎを求める。追従遅れ量加速補正処理部１１３では、追従遅れを補正するため、ロボット１１８遅れ時間分補正量をΔＬｎに加え、対象物に追いつくようにΔＬｎを補正する。コンベア移動量平均化部では、指令値の速度波形が滑らかになるように過去数回分のΔＬｎを平均し、平均化移動量ΔＬａを求める。位置修正量演算部１１５では移動量ΔＬａをコンベア方向に分解する。分解された平均化コンベア移動量は、制御サイクル周期後到達位置修正部１０４において、動作プログラムによって実行されるロボット１１８の指令値に加えられる。このようにして、ロボット１１８は、平均化移動量分コンベアの移動方向に動作しながら、対象物に対して作業を行う。
【０００４】
【発明が解決しようとする課題】
従来のロボット制御装置は、コンベアの移動量に変動があった場合に、追従遅れ量加速補正処理部において１制御サイクル周期内で全遅れ量を加え、その後平均化を行っているので、平均化後の移動量の変化が滑らかでなく、追従開始時やコンベアの速度が急激に変化した場合、ロボットの動作が振動的になるという問題があった。
【０００５】
また、コンベア移動量を過去数回分の移動量で平均化するため、平均化による遅れが生じるという問題があった。
【０００６】
この発明は、上記のような問題点を解決するためになされたものであり、コンベア等の搬送装置における移動量の変化が大きくても高速高精度な追従作業を実行可能なロボット制御装置を得ることを目的とするものである。
【０００７】
【課題を解決するための手段】
この発明の第１の構成によるロボット制御装置は、搬送装置によって搬送される作業対象物に対して追従しながらロボットに作業を行なわせるロボット制御装置において、
前記搬送装置の位置を検出する搬送装置位置検出部と、
該搬送装置位置検出部で得られる位置を変換し、仮想搬送装置位置を生成する搬送装置位置生成部と、
該搬送装置位置生成部で得られたロボットの指令値を生成する仮想搬送装置位置であって、１制御周期間隔毎に順に付される整数 i で特定される現在の時刻tiの仮想搬送装置位置と、前記時刻tiの１制御周期前の時刻ti-1における仮想搬送装置位置との差から１制御周期間に搬送装置が移動した移動量ｄＣ_iを生成する搬送装置移動量生成部と、
前記ロボットの前記搬送装置に対する追従誤差を記憶しておく追従誤差記憶部と、
前記搬送装置移動量生成部で生成された移動量ｄＣ_i、１制御周期前に求めた移動量ｄＣ_i-1と修正係数ａから次制御周期推定移動量をＶｃ＝ｄＣ_i＋ａ×（ｄＣ_i−ｄＣ_i-1）により求め、該次制御周期推定移動量Ｖｃと追従遅れ補正係数ｂの積によりロボットの追従遅れを補正する追従遅れ補正量を求め、前記追従遅れ補正量と前記追従誤差記憶部に記憶された追従誤差との和により追従目標移動量を生成する追従遅れ補正部と、
前記追従遅れ補正部で生成された前記追従目標移動量、ロボットの１制御周期前に生成した追従移動量、前記次制御周期推定移動量、ロボットの１制御周期追従加速量または減速量から３次元データである次制御周期の追従移動量Ｖｎを求め、該次制御周期の追従移動量Ｖｎの各座標軸の成分が次制御周期最大追従移動量と次制御周期最小追従移動量との範囲内になるように次回のロボットの追従移動量Ｖｎを生成する追従移動量生成部と、
前記ロボットの動作プログラムに従った動作指令値を生成する動作制御部と、
該動作制御部で生成された動作指令値に前記追従移動量生成部で生成された追従移動量を加え、ロボットへの指令値を生成する指令パルス生成部と、
前記指令パルス生成部で生成した指令値からサーボモータを制御しロボットを動作させるサーボ制御部と、
を備えたことを特徴とするロボット制御装置。
【０００８】
また、この発明の第２の構成によるロボット制御装置においては、追従遅れ補正部の追従遅れ補正係数ｂは、サーボモータの位置ループの制御ゲインＫ、ロボットの制御周期Ｔから求まる値１／（ＫＴ）と、指令パルス生成部からサーボ制御部へ指令値が入力されるまでの制御周期数ｃと、次制御周期での搬送装置の移動量を反映するための制御周期数との和により求めるものである。
【００１０】
また、この発明の第３の構成によるロボット制御装置は、搬送装置又はロボットの状態ごとの補正データを記憶する記憶領域を備え、ロボットの追従遅れ補正量は、上記搬送装置又はロボットの状態ごとの補正データにより決定する構成にしたものである。
【００１３】
【発明の実施の形態】
実施の形態１．
図１は、この発明を実施するための実施の形態１によるロボット制御装置を説明するための構成図である。図において、１はロボット、２はロボット制御装置、３は作業対象物(以下、ワーク)Ｗを搬送する搬送装置であるコンベア、４はコンベアの移動量を検知するコンベアエンコーダである。２０は動作制御部であり、ロボットの動作プログラムによって記述された動きを実現するための指令値を制御周期ごとに生成し、指令パルス生成部２１に出力する。指令パルス生成部２１では動作制御部で生成された指令値を逆変換など行い、サーボ制御部２２に指令パルスを出力する。サーボ制御部２２では指令パルスに従ってロボットのサーボモータを制御し、ロボットを動作させる。２３はエンコーダ値検出部で、コンベアエンコーダ４から出力されたパルスを受信し、パルスを積算してコンベアの位置を管理する。２４は仮想エンコーダ値生成部で、エンコーダ値検出部２３で管理しているエンコーダ値を仮想エンコーダ値に変換する。２５はコンベア移動量生成部で仮想エンコーダ値を元にコンベアの移動量を生成する。２６はサーボ制御の遅れなどを補正する追従遅れ補正部、２７はロボットの追従移動量を生成する追従移動量生成部、２８は追従誤差を記憶しておく追従誤差データ記憶部である。
【００１４】
次に動作について説明する。ワークＷがコンベア３によって運ばれ、光電センサや画像センサなどのセンサ(記載せず)により、ロボットの動作範囲に入ったことを検出すると、以下のようにして追従動作を開始する。コンベア３が動作すると同時にコンベアエンコーダ４も回転し、パルスを発生する。発生したパルスはエンコーダ値検出部２３でカウントされ、コンベアの位置が検出される。エンコーダ値検出部は例えば１６ｂｉｔの積算カウンタで構成され、０〜６５５３５の値をとる。仮想エンコーダ値生成部２４では、１制御周期ごとにエンコーダ値をエンコーダ値検出部２３から読み出し、仮想エンコーダ値を生成する。
【００１５】
仮想エンコーダ値は次ぎのようにして求める。現在の時刻をｔ_i、１制御周期前の時刻をｔ_i-1、１制御周期後の時刻をｔ_i+1とし、時刻ｔ_i-1、ｔ_iのエンコーダ値をそれぞれＥ_i-1、Ｅ_i、仮想エンコーダ値をＦ_i-1、Ｆ_iとする。また、仮想エンコーダ値の最大値、最小値をＦｍａｘ、Ｆｍｉｎとすると、Ｆ_iの値は、
Ｆ_i＝Ｆ _i-1＋ｆ（Ｅ_i−Ｅ_i-1）
【００１６】
ここで、ｆ（ｘ_i）はフィルタ関数であり、コンベアの速度変動の状態によって選択する。例えば、コンベアの速度変動があまりなく、ロボットに振動が発生しない場合は、フィルタなしとし、
ｆ（ｘ_i）＝ｘ_i
【００１７】
コンベアの変動の頻度が高く、ロボットの動作が振動的になる場合は、例えば、次に示すような平均化関数の式(１)を用いる。
【００１８】
【数１】

【００１９】
ここで、ｍは正の整数である。振動が少ない場合は、ｍの値を小さく設定し、振動が大きい場合は、ｍの値を大きく設定する。
【００２０】
上記のように求めたＦ_iに対し、Ｆ_iが最大値Ｆｍａｘを越えた場合は、
Ｆ_i＝Ｆ_i−Ｆｍａｘ＋Ｆｍｉｎ
また、Ｆ_iが最小値Ｆｍｉｎ以下になった場合は、
Ｆ_i＝Ｆ_i＋Ｆｍａｘ−Ｆｍｉｎ
とする。Ｆｍａｘ，Ｆｍｉｎは自由に設定可能である。１６ｂｉｔのカウンタではワークの搬送中にエンコーダが６５５３５パルス分以上回ってしまうと、桁あふれが発生し、位置が管理できなくなるが、Ｆｍａｘ，Ｆｍｉｎを大きく設定することにより、長いコンベアにも対応が可能となる。
【００２１】
コンベア移動量生成部２５では、まず仮想エンコーダ値生成部２４で生成された仮想エンコーダ値Ｆ_i，Ｆ_i-1から１制御周期でのエンコーダ値変化量ｄＦ_iを求める。次に、あらかじめ求めておいた３次元空間における１パルスあたりの移動量［ｄｘ，ｄｙ，ｄｚ]を使って、１制御周期あたりのコンベアの３次元空間における移動量ｄＣ_i＝［ｄｘ×ｄＦ_i，ｄｙ×ｄＦ_i，ｄｚ×ｄＦ_i］を求める。
【００２２】
追従遅れ補正部２６では、まず、コンベア移動量生成部２５で得られたコンベアの移動量ｄＣ_iから、次の１制御周期間のコンベア移動量Ｖｃを推定する。コンベア次制御周期推定移動量Ｖｃは例えば次式で求める。
Ｖｃ＝ｄＣ_i＋ａ×（ｄＣ_i−ｄＣ_i-1）
ここで、ａは修正係数、ｄＣ_i-1は時刻ｔ_i-2からｔ_i-1におけるコンベアの移動量である。ａの値はコンベアの速度変動に応じて決定する。コンベアの速度変動がほとんどない場合は、ａ＝０とし、現在の速度と同一の速度で動作したものとしてコンベア推定移動量Ｖｃを求める。また、速度変動が大きく、ずれが大きくなる場合、ａ＝１．０とすることにより、速度変動によるずれを補正することが可能である。速度変動により、ロボットの動作が振動的になる場合は、ａに負の値、例えば、ａ＝−０．５とすることにより、振動を抑制する効果がある。
【００２３】
次に追従遅れ補正量ｄＤ_i+1を次式により求める。
ｄＤ_i+1＝ｂ×Ｖｃ
ｂは追従遅れ補正係数であり、次式で求める。
ｂ＝１／（ＫＴ）＋ｃ＋１
ここで、Ｋはサーボモータの位置ループの制御ゲイン、Ｔはロボットの制御周期、ｃは指令パルス生成部２１で指令値を生成してからサーボ制御部２２に指令値が入力されるまでの制御周期数である。右辺第３項の１は次制御周期中のコンベアの移動量を反映するためのものである。位置ループゲインＫはコンベアの進行方向にロボットを動作させたときに最も動作量の大きい関節のサーボモータのゲインを用いれば良い。
【００２４】
上記の補正後、コンベアの速度が変化しても追従誤差が発生しない場合は、上記のbの値で良いが、追従誤差が変化する場合は次のようにして補正できる。コンベア速度ｖ１[ｍｍ／ｓ]のとき、ｘ１[ｍｍ]、ｖ２[ｍｍ／ｓ]のときｘ２[ｍｍ]の遅れが発生する場合、上記のｂの代わりに次式を用いるとよい。
ｂ＝１／（ＫＴ）＋ｃ＋（ｘ２−ｘ１）／（（ｖ２−ｖ１）Ｔ）＋１
【００２５】
次に、生成された追従遅れ補正量ｄＤ_i+1を元にロボット追従目標移動量Ｄ_i+1を求める。まず、前回追従誤差Ｒ_iを追従誤差データ記憶部２８から読み出す。追従遅れ補正量ｄＤ_i+1と追従誤差Ｒ_iより、追従目標移動量Ｄ_i+1を次式により求める。
Ｄ_i+1＝ｄＤ_i+1＋Ｒ_i
【００２６】
追従移動量生成部２７では、追従遅れ補正部２６で生成された追従目標移動量Ｄ_i+1とロボットの前制御周期での追従移動量Ｖｒ、コンベアの次制御周期での推定移動量Ｖｃより、次制御周期でのロボットの追従移動量Ｖｎを求める。次制御周期におけるロボットの追従移動量Ｖｎの求め方について図２を用いて示す。なお、Ｖｒ，Ｖｃ，Ｄ_i+1などは３次元データであり、ｘ，ｙ，ｚ方向の３要素を持つが、図２の処理をそれぞれの要素について行うことによりＶｎを求める。以下では、Ｖｒ、Ｖｃ、Ｄ_i+1の各要素をそれぞれｖｒ、ｖｃ、ｄとする。なお、各移動量の符号はコンベアの進行方向を正とする。
【００２７】
まず、ＳＴ１ではロボットの前制御周期追従移動量ｖｒの正負を判断し、正の場合は図２に示すフローに従いステップＳＴ２に、また負の場合はＳＴ３０へ進む。ＳＴ３０の詳細な手順は図３に示す。ステップＳＴ２では、次制御周期でロボットが加速した場合、減速した場合の次制御周期最大追従移動量ｖａ，次制御周期最小追従移動量ｖｄを次式により求める。
ｖａ＝ｖｒ＋ａｃｃ
ｖｄ＝ｖｒ−ｄｅｃ
ここで、ａｃｃ，ｄｅｃはそれぞれ１制御周期追従加速量、減速量であり、共に正の値である。これらの値はロボットの特性を考慮して決定される。このようにして求めた次制御周期最大追従移動量ｖａとロボットの1制御周期最大移動量ｖｍａｘを比較し、ｖａがｖｍａｘより大きい場合はｖａ＝ｖｍａｘとする。
【００２８】
ステップＳＴ３では、コンベアの推定移動量ｖｃとロボットの前制御周期追従移動量ｖｒを比較し、ｖｃの方が大きい場合はロボットを加速しなければならないので、ステップＳＴ４で加減速度ａｄにａｃｃをセットし、ｖｒの方が大きい場合は、ロボットは減速しなければならないので加減速度ａｄに−ｄｅｃをセットする。
【００２９】
ステップＳＴ５ではコンベアとロボットが等速度になるまでに要する制御周期数nを次式により求める。
ｎ＝ｉｎｔ（（ｖｃ−ｖｒ）／ａｄ）
ここで、ｉｎｔ（ｘ）はｘを超えない最大の整数を求める関数である。ステップＳＴ１６で、ｎが０でない正の整数の場合、ステップＳＴ７に進み、ｎ制御周期後にロボットが追従目標移動量ｄ移動するために必要な次制御周期でのロボット追従移動量ｖｎを次式で求める。
ｖｎ＝（ｄ＋ｖｃ×（ｎ−１））／ｎ−（ｎ−１）×ａｄ／２
【００３０】
ステップＳＴ１６においてｎが０の場合、ロボットとコンベアは等速度であるので、ステップＳＴ１１で次制御周期のロボット追従移動量ｖｎをｖｎ＝ｄとする。
【００３１】
ステップＳＴ８では、ステップＳＴ７，ＳＴ１１で求めたｖｎと、ステップＳＴ２で求めた次制御周期最大追従移動量ｖａの大小を比較し、ｖｎがｖａより大きい場合は、あらかじめ設定した加速度以上に加速しないようにｖｎ＝ｖａに修正する。小さい場合はステップＳＴ１２に進み、ｖｎと次制御周期最小追従移動量ｖｄと大小を比較する。ｖｎがｖｄより小さい場合はあらかじめ設定した減速度以上に減速しないように、ｖｎ＝ｖｄに修正する。ｖｎの方が大きい場合は修正せずにそのままの値を次制御周期の追従移動量とする。
【００３２】
一方、ステップＳＴ１でｖｒが０以下に場合は、コンベアとロボットが反対方向に動作、またはロボットが停止している場合であり、ＳＴ３０へ進む。図３はＳＴ３０における処理手順を示す。
【００３３】
ステップＳＴ１４では、ロボットの次制御周期最大追従移動量ｖａと次制御周期最小追従移動量ｖｄを次式により求める
ｖａ＝ｖｒ−ａｃｃ
ｖｄ＝ｖｒ＋ｄｅｃ
ただし、ｖａがロボット最大追従移動量ｖｍａｘより大きい場合は、ｖａ＝−ｖｍａｘとする。
【００３４】
次にステップＳＴ１５では、次式によりコンベアとロボットが等速度になるまでに要する制御周期数ｎを求める。
ｎａ＝ｉｎｔ（ａｂｓ（ｖｃ／ａｃｃ））＋１
ｎｄ＝ｉｎｔ（ａｂｓ（ｖｒ／ｄｅｃ））
ｎ＝ｎａ＋ｎｄ
ここで、ａｂｓ（ｘ）はｘの絶対値を求める関数であり、ｎａはロボットが速度０からコンベアの速度に達するまでの制御周期数、ｎｄはロボットが現在速度から速度０に減速するまでの制御周期数である。
【００３５】
ステップＳＴ１６では、ｎｄが０でない正の整数であるか否かを判断し、０でない正の整数の場合はステップＳＴ１７へ進み、０の場合はステップＳＴ２０へ進む。
【００３６】
ステップＳＴ１７では、制御周期ｎでロボットがワークに追いつくために必要な次制御周期ロボット追従移動量ｖｎを次式から求める。
ｘｃ＝ｄ＋ｖｃ×（ｎ−１）
ｘｒ＝ｎａ×（ｎａ−１）×ａｃｃ／２
ｖｎ＝（ｘｃ−ｘｒ）／ｎｄ−（ｎｄ−１）×ｄｅｃ／２
ここで、ｘｃ，ｘｒはそれぞれｎ制御周期後までのワーク、ロボットの移動量である。
【００３７】
ステップＳＴ１６でｎｄが０の場合はステップＳＴ２０に進み、ｖｃと次制御周期最小追従移動量ｖｄの比較を行い、ｖｃの方が大きい場合はステップＳＴ２１に進み、そうでない場合はステップＳＴ２２に進む。
【００３８】
ステップＳＴ２１では次式により、ｖｎを求める。
ｎ＝ｉｎｔ（（ｖｃ−ｖｄ）／ａｃｃ）＋１
ｘｃ＝ｄ＋ｖｃ×（ｎ−１）
ｖｎ＝ｘｃ／ｎ−ａｃｃ×（ｎ−１）／２
ここで、ｘｃはロボットがワークに追いつくまでにワークが移動する量である。
【００３９】
一方、ステップＳＴ２０でｖｃがｖｄと等しいか、またはｖｄより小さい場合、ロボットとコンベアは等速度と考えられるので、ｖｎ＝ｄとする。
【００４０】
ステップＳＴ１８では、ｖｎとｖａを比較し、ｖｎがｖａより小さい場合はステップＳＴ１９に進み、ｖｎの値をｖａに修正する。また、ステップＳＴ１８でｖｎがｖａより小さくない場合は、ステップＳＴ２３に進み、ｖｎとｖｄを比較する。ステップＳＴ２３で、ｖｎがｖｄより大きい場合はステップＳＴ２４でｖｎの値をｖｄに修正する。ｖｎがｖｄより大きくない場合は、ステップＳＴ１７，ＳＴ２１，ＳＴ２２で求めたｖｎの値をそのまま用いる。
【００４１】
以上のようにして求めたロボット追従移動量Ｖｎは指令パルス生成部２１に出力される。また、次式で追従誤差Ｒ_i+1を次式により求め、追従誤差データ記憶部２８に保存される。
Ｒ_i+1＝Ｒ_i＋ｄＣ_i−Ｖｎ
【００４２】
動作制御部２０では、ロボットプログラムに従った動作指令値を生成し、指令パルス生成部２１に出力する。指令パルス生成部２１では、動作制御部２０で生成された指令値と、追従移動量生成部２７で生成された追従移動量を合成し、得られた指令データを逆変換してサーボモータのパルスデータをサーボ制御部２２に出力する。サーボ制御部２２では、指令パルス生成部２１で生成されたパルスデータに従ってロボットのサーボモータを制御することにより、ロボットに所望の動作をさせる。
【００４３】
図４はコンベア速度とロボットの追従速度の変化を表す説明図である。図において、ｖｃはコンベア速度、ｖｒはロボットの追従速度を表す。時刻ｔ０にロボットはコンベアに対し追従作業を開始し、追従後れを取り戻すため、時刻ｔ１まで制御周期ごとに次制御周期の追従速度を図２、図３の手順によって求め、加速する。時刻ｔ１において次制御周期の追従速度を計算すると、このまま加速するとコンベア上の目的位置を追い越してしまうため減速に入る。時刻ｔ２ではコンベアの速度がｖ１からｖ２に高速に変化したため、再びロボットの追従速度も加速し、時刻ｔ３で、目的位置を追い越さないように減速をはじめ、時刻ｔ４でコンベアに追いつく。
【００４４】
実施の形態１では、ロボットの動作可能な最大速度、最大加減速度(加速パターン、減速パターン)を考慮して、追従移動量を決定するようにしたので、コンベアの速度に変動があっても高速にワークに追従することが可能となる。実施の形態１では加速パターン、減速パターンとして１制御周期追従加速量及び減速量を用いた例を示したが、加速時間、減速時間を用いてもよい。なお、コンベア速度に対し、ロボットの最高速度が十分速い場合、ロボットの最大速度を考慮せずに追従移動量を決定してもよい。
【００４５】
また、滑らかに加減速を行うので、振動を起こさずに追従でき、高精度に追従作業が可能となる。
【００４６】
また、コンベアエンコーダの最大値、最小値を自由に決定できるので、長いコンベアでの追従作業が可能である。さらに、エンコーダ値に対してフィルタをかけることができるので、コンベアが振動的であっても滑らかに追従することが可能である。
【００４７】
上記実施の形態１では、追従遅れ補正部２６において、ｂ＝１／ＫＴ＋ｃ＋１という式を用いて追従遅れ補正量を求めたが、あらかじめコンベアの移動量ごとやロボットの位置、姿勢ごとに補正量を求め、テーブルとして記憶し、動作時にはロボットの現在の位置や速度から補正量をテーブルのデータから補間して求めてもよい。また、補正量の代わりにｂの値をテーブルに記憶しておいてもよい。図５は補正係数ｂの値をロボットの位置ｘの値ごとに記憶したテーブルである。追従遅れ補正部２６で追従遅れ補正量を計算するときにロボットの現在位置を示すｘの値がｘ１＜ｘ＜ｘ２であったとすると次式で補正係数ｂを求める。
ｂ＝（ｂ２−ｂ１）（ｘ−ｘ１）／（ｘ２−ｘ１）＋ｂ１
このｂの値を用いて補正量を求めることにより、ロボットの位置による追従誤差をなくすことが可能である。上記実施の形態１では、ロボットの位置、姿勢ごとに補正量を求めているが、このほか、ロボットの状態を表す量、またはコンベアの速度等の状態を表す量を用いてもよい。
【００４８】
また、上記実施の形態１では、追従遅れ補正部２６において、補正係数bをロボットの位置姿勢から動的に計算して求めても同様な効果が得られることはいうまでもない。
【００４９】
上記実施の形態１では、過渡追従遅れ補正量生成部２７で生成したロボット追従移動量を直接指令パルス生成部に渡したが、指令値を滑らかにするため、最近のｍ制御周期分追従移動量を平均してもよい。その場合、追従遅れ補正部２６のｂにｍを加える、例えば、ｂをｂ＝１／ＫＴ＋ｃ＋１と言う式を用いて求めている場合、ｂ＝１／ＫＴ＋ｃ＋ｍ＋１とすることによって、平均することによる遅れを補正することができる。
【００５０】
上記実施の形態１では、仮想エンコーダ値生成部２４において、コンベアのエンコーダ値を仮想エンコーダ値に変換していたが、エンコーダ値検出部２３のカウンタのビット数が追従作業領域に対し、十分大きい場合や、コンベアの動作が振動的でない場合は図６に示すように、図１に示す仮想エンコーダ値生成部２４がない構成でも同様の効果があることは言うまでもない。図６の構成では、エンコータ値検出部２３により検出されたエンコーダ値が直接コンベア移動量生成部２５へ送られ、このエンコータ値に基づきコンベア移動量が演算される。
【００５１】
上記実施の形態１では、コンベアエンコーダによってコンベアの位置を把握し、コンベアに追従しながら作業を行う場合について述べたが、コンベア以外の搬送装置、例えば、ｘｙテーブルや他のロボットによって搬送される場合でも、それらの現在位置を直接コントローラに入力することによって同様に追従作業が可能である。
【００５２】
実施の形態２．
図７は、この発明を実施するための実施の形態２によるロボット制御装置を説明するための図である。図において、図１と同一の符号を付したものは、同一またはこれに相当するものである。また、図７において、２９は関節追従遅れ補正部であり、指令パルス生成部２１で生成された各関節ごとの指令パルスに対し、追従遅れを補正する。
【００５３】
次に動作について説明する。コンベアエンコーダ４、エンコーダ値検出部２３、コンベア移動量生成部２５は実施の形態１と同様の動作を行う。追従移動量生成部２７では、実施の形態１では追従遅れ補正部で生成された次の１制御周期における推定コンベア移動量の代わりに、コンベア移動量生成部２５で生成されたコンベア移動量を用いてロボットの追従移動量を生成する。生成方法は、実施の形態１と同様である。
【００５４】
動作制御部２０では、ロボットプログラムに従った動作指令値を生成し、指令パルス生成部２１に出力する。指令パルス生成部２１では、動作制御部２０で生成された指令値と、追従移動量生成部２７で生成された追従移動量を合成し、得られた指令データを逆変換してサーボモータのパルスデータを関節追従遅れ補正部２９に出力する。
【００５５】
関節追従遅れ補正部２９では、各関節ごとに遅れ量を補正する。実施の形態１では、追従遅れ補正部でコンベアの移動量から次制御周期のコンベア推定移動量を求めたが、関節追従遅れ補正部２９では、コンベア移動量の代わりにロボット関節移動量を用いて追従遅れ分補正した関節移動量を生成する。
【００５６】
サーボ制御部２２に出力する。サーボ制御部２２では、関節追従遅れ補正部２９で生成されたパルスデータに従ってロボットのサーボモータを制御することにより、ロボットに所望の動作をさせる。
【００５７】
上記実施の形態２では、各関節ごとに遅れ量を補正するように構成したので、より高精度な追従作業が可能となる。
【００５８】
また、上記実施の形態２では、コンベアエンコーダによってコンベアの位置を把握し、コンベアに追従しながら作業を行う場合について述べたが、コンベア以外の搬送装置、例えば、ｘｙテーブルや他のロボットによって搬送される場合でも、それらの現在位置を直接コントローラに入力することによって同様に追従作業が可能である。
【００５９】
この発明の第１の構成であるロボット制御装置によれば、ロボットの動作可能な最大速度、最大加減速度を考慮して、追従移動量を決定するようにしたので、高速に追従作業が可能となるという効果がある。また、滑らかに加減速を行うので、振動を起こさずに追従でき、高精度に追従作業が可能となるという効果がある。
【００６１】
また、この発明の第２の構成であるロボット制御装置によれば、定常的な追従誤差を補正する時に、サーボモータの制御ゲインから補正係数を決めるようにしたので、容易に高精度な追従作業を実現できるという効果がある。
【００６２】
また、この発明の第３の構成であるロボット制御装置によれば、追従遅れ補正部であらかじめ記憶領域に記憶された搬送装置の速度やロボットの位置姿勢ごとの補正データを用いて追従補正を行うため、搬送装置の速度変動、ロボットの位置姿勢によらず高精度な追従作業が可能となるという効果がある。
【図面の簡単な説明】
【図１】この発明の実施の形態１によるロボット制御装置を説明するための構成図である。
【図２】この発明の実施の形態１による追従移動量の生成手順を説明するためのフロー図である。
【図３】この発明の実施の形態１による追従移動量の生成手順を説明するためのフロー図である。
【図４】この発明の実施の形態１によるロボットと搬送装置の速度と時間の関係を説明するための図である。
【図５】この発明の実施の形態１による追従補正データの記憶領域を説明するための図である。
【図６】この発明の実施の形態１によるロボット制御装置を説明するための構成図である。
【図７】この発明の実施の形態２によるロボット制御装置を説明するための構成図である。
【図８】従来のロボット制御装置を説明するための構成図である。
【符号の説明】
１ロボット、２ロボット制御装置、３搬送装置、４コンベアエンコーダ、２０動作制御部、２１指令パルス生成部、２２サーボ制御部、２３搬送装置位置検出部、２４仮想搬送装置位置生成部、２５搬送装置移動量生成部、２６追従遅れ補正部、２７追従移動量生成部、２８追従誤差記憶部、２９関節追従遅れ補正部。[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a robot control device for an industrial robot (hereinafter referred to as a robot) that performs an operation while following a work target (hereinafter referred to as an object) that is being moved by a transfer device.
[0002]
[Prior art]
FIG. 8 is a block diagram showing a conventional robot control device disclosed in, for example, Japanese Patent Laid-Open No. 3-154103. In the figure, 101 is an operation control unit, 102 is an operation trajectory creation unit, 103 is an arrival position creation unit after a control cycle period, 104 is an arrival position modification unit after a control cycle period, 105 is an inverse conversion unit, and 106 is after a control cycle cycle. Command pulse creation unit 107, servo control unit 108, synchronization start limit switch 109, conveyor encoder 110, integration counter 110, conveyor movement pulse measurement unit 111 within the control cycle period, 112 conveyor movement amount creation within the control cycle period , 113 is a tracking delay amount acceleration correction processing unit, 114 is a conveyor movement amount averaging unit, 115 is a position correction amount calculation unit, 116 and 117 are paths, and 118 is a robot.
[0003]
Next, the operation will be described. When the object on which the robot 118 works is moved across the synchronization start limit switch 108, the counter 110 is reset and the operation control unit 101 is notified of the arrival of the synchronization start point of the object. The counter 110 accumulates pulses according to the movement of the conveyor using the pulses output from the conveyor encoder 109, and the control cycle period conveyor movement pulse measuring unit 111 calculates the number of movement pulses ΔCn in one control cycle period from the value of the counter 110. measure. The conveyor movement creation unit 112 within the control cycle period obtains the conveyor movement amount ΔLn from the number of movement pulses. In order to correct the follow-up delay, the follow-up delay amount acceleration correction processing unit 113 adds a correction amount corresponding to the delay time of the robot 118 to ΔLn and corrects ΔLn so as to catch up with the object. The conveyor movement amount averaging unit averages ΔLn for the past several times so that the velocity waveform of the command value becomes smooth to obtain an average movement amount ΔLa. The position correction amount calculation unit 115 disassembles the movement amount ΔLa in the conveyor direction. The disassembled average conveyor movement amount is added to the command value of the robot 118 executed by the operation program in the arrival position correcting unit 104 after the control cycle period. In this manner, the robot 118 operates on the object while moving in the moving direction of the conveyor by the averaged moving amount.
[0004]
[Problems to be solved by the invention]
In the conventional robot control device, when the movement amount of the conveyor is changed, the follow-up delay amount acceleration correction processing unit adds the total delay amount within one control cycle period, and then performs averaging. There is a problem in that the movement of the robot becomes vibrational when the subsequent change in the amount of movement is not smooth, and when the follow-up starts or the speed of the conveyor changes abruptly.
[0005]
In addition, since the conveyor movement amount is averaged by the movement amount of the past several times, there is a problem that a delay due to the averaging occurs.
[0006]
The present invention has been made to solve the above-described problems, and provides a robot control device capable of performing high-speed and high-accuracy follow-up work even if the amount of movement in a transfer device such as a conveyor is large. It is for the purpose.
[0007]
[Means for Solving the Problems]
  A robot control apparatus according to a first configuration of the present invention is a robot control apparatus that causes a robot to perform an operation while following a work object transferred by a transfer device.
A transport device position detector for detecting the position of the transport device;
A transfer device position generation unit that converts a position obtained by the transfer device position detection unit and generates a virtual transfer device position;
Generates a robot command value obtained by the transfer device position generation unitVirtual transport device position, an integer that is added in order at every control cycle interval i Identified byThe position of the virtual transfer device at the current time ti,Virtual transfer device position at time ti-1 one control cycle before time tiWhenThe amount of movement dC that the transport device has moved in one control cycle from the difference between_iA transfer device movement amount generation unit for generating
A tracking error storage unit that stores a tracking error of the robot with respect to the transfer device;
Movement amount dC generated by the transfer device movement amount generation unit_iThe amount of movement dC obtained before one control cycle_i-1And the next control cycle estimated movement amount from the correction coefficient a and Vc = dC_i+ A × (dC_i-DC_i-1), The tracking delay correction amount for correcting the tracking delay of the robot is obtained by the product of the estimated movement amount Vc of the next control cycle and the tracking delay correction coefficient b, and is stored in the tracking delay correction amount and the tracking error storage unit. A tracking delay correction unit that generates a tracking target movement amount by the sum of the tracking error, and
Three-dimensional from the following target movement amount generated by the following delay correction unit, the following movement amount generated one control period before the robot, the next control period estimated movement amount, the one control period following acceleration amount or the deceleration amount of the robot The follow-up movement amount Vn of the next control cycle, which is data, is obtained, and the component of each coordinate axis of the follow-up movement amount Vn of the next control cycle falls within the range of the next control cycle maximum follow-up movement amount and the next control cycle minimum follow-up movement amount. A follow-up movement amount generation unit for generating a follow-up movement amount Vn of the next robot,
An operation control unit that generates an operation command value according to the operation program of the robot;
A command pulse generator for adding a follow-up movement amount generated by the follow-up movement amount generator to a motion command value generated by the operation controller, and generating a command value for the robot;
A servo control unit for controlling the servo motor from the command value generated by the command pulse generation unit and operating the robot;
A robot control device comprising:
[0008]
  In the robot control apparatus according to the second configuration of the present invention, the tracking delay correction coefficient of the tracking delay correction unitb is a value 1 / (KT) obtained from the control gain K of the position loop of the servo motor, the control cycle T of the robot, and the control cycle number c until the command value is input from the command pulse generator to the servo controller. , Obtained by the sum of the number of control cycles to reflect the amount of movement of the transport device in the next control cycleIs.
[0010]
In addition, the present invention3The robot control device having the configuration includes a storage area for storing correction data for each state of the transfer device or the robot, and the follow-up delay correction amount of the robot isthe aboveIt is configured to be determined by correction data for each state of the transfer device or the robot.
[0013]
DETAILED DESCRIPTION OF THE INVENTION
Embodiment 1 FIG.
FIG. 1 is a block diagram for explaining a robot control apparatus according to Embodiment 1 for carrying out the present invention. In the figure, 1 is a robot, 2 is a robot control device, 3 is a conveyor which is a transfer device for transferring a work object (hereinafter referred to as a workpiece) W, and 4 is a conveyor encoder which detects the amount of movement of the conveyor. An operation control unit 20 generates a command value for realizing a motion described by the robot operation program for each control cycle, and outputs the command value to the command pulse generation unit 21. The command pulse generation unit 21 performs reverse conversion or the like on the command value generated by the operation control unit, and outputs a command pulse to the servo control unit 22. The servo control unit 22 controls the servo motor of the robot according to the command pulse to operate the robot. Reference numeral 23 denotes an encoder value detection unit which receives pulses output from the conveyor encoder 4 and integrates the pulses to manage the position of the conveyor. A virtual encoder value generation unit 24 converts the encoder value managed by the encoder value detection unit 23 into a virtual encoder value. Reference numeral 25 denotes a conveyor movement amount generation unit that generates the movement amount of the conveyor based on the virtual encoder value. A tracking delay correction unit 26 corrects a servo control delay, the tracking movement amount generation unit 27 generates a tracking movement amount of the robot, and a tracking error data storage unit 28 stores a tracking error.
[0014]
Next, the operation will be described. When it is detected that the workpiece W is carried by the conveyor 3 and has entered the movement range of the robot by a sensor (not shown) such as a photoelectric sensor or an image sensor, the following operation is started as follows. Simultaneously with the operation of the conveyor 3, the conveyor encoder 4 also rotates to generate pulses. The generated pulses are counted by the encoder value detection unit 23 to detect the position of the conveyor. The encoder value detection unit is composed of, for example, a 16-bit integration counter, and takes a value of 0 to 65535. The virtual encoder value generation unit 24 reads the encoder value from the encoder value detection unit 23 for each control period, and generates a virtual encoder value.
[0015]
  The virtual encoder value is obtained as follows. The current time is t_i, The time before one control cycle is t_i-1, The time after one control cycle is t_{i + 1}And time t_i-1, T_iEncoder values of E_i-1, E_i, F is the virtual encoder value_i-1, F_iAnd If the maximum and minimum values of the virtual encoder value are Fmax and Fmin, F_iThe value of
  F_i=F _i-1+ F (E_i-E_i-1)
[0016]
Where f (x_i) Is a filter function, which is selected according to the state of speed fluctuation of the conveyor. For example, if there is not much fluctuation in the speed of the conveyor and the robot does not vibrate, there is no filter.
f (x_i) = X_i
[0017]
When the frequency of change of the conveyor is high and the robot operation becomes oscillating, for example, the following equation (1) of the averaging function is used.
[0018]
[Expression 1]

[0019]
Here, m is a positive integer. When the vibration is small, the value of m is set small, and when the vibration is large, the value of m is set large.
[0020]
F calculated as above_iOn the other hand, F_iExceeds the maximum value Fmax,
F_i= F_i-Fmax + Fmin
F_iIs less than the minimum value Fmin,
F_i= F_i+ Fmax-Fmin
And Fmax and Fmin can be set freely. With the 16-bit counter, if the encoder rotates more than 65535 pulses during workpiece transfer, overflow will occur and the position will not be manageable, but by setting Fmax and Fmin large, it is possible to handle long conveyors. It becomes.
[0021]
In the conveyor movement amount generation unit 25, first, the virtual encoder value F generated by the virtual encoder value generation unit 24._i, F_i-1To encoder value change amount dF in one control cycle_iAsk for. Next, using the movement amount [dx, dy, dz] per pulse in the three-dimensional space obtained in advance, the movement amount dC of the conveyor in the three-dimensional space per control cycle._i= [Dx x dF_i, Dy x dF_i, Dz × dF_i].
[0022]
In the follow-up delay correction unit 26, first, the conveyor movement amount dC obtained by the conveyor movement amount generation unit 25 is obtained._iFrom this, the conveyor movement amount Vc during the next one control cycle is estimated. The conveyor next control cycle estimated movement amount Vc is obtained by the following equation, for example.
Vc = dC_i+ A × (dC_i-DC_i-1)
Where a is a correction coefficient and dC_i-1Is the time t_i-2To t_i-1Is the amount of movement of the conveyor. The value of a is determined according to the speed fluctuation of the conveyor. When there is almost no change in the speed of the conveyor, a = 0 is set, and the estimated conveyor movement amount Vc is obtained assuming that the conveyor operates at the same speed as the current speed. When the speed fluctuation is large and the deviation becomes large, it is possible to correct the deviation due to the speed fluctuation by setting a = 1.0. When the movement of the robot is vibrated due to speed fluctuation, setting a to a negative value, for example, a = −0.5, has an effect of suppressing vibration.
[0023]
Next, follow-up delay correction amount dD_{i + 1}Is obtained by the following equation.
dD_{i + 1}= B x Vc
b is a follow-up delay correction coefficient, and is calculated by the following equation.
b = 1 / (KT) + c + 1
Here, K is the control gain of the position loop of the servo motor, T is the control cycle of the robot, c is the control from when the command value is generated by the command pulse generation unit 21 until the command value is input to the servo control unit 22 The number of cycles.rightThe third term 1 in the side is for reflecting the amount of movement of the conveyor during the next control cycle. As the position loop gain K, the gain of the servo motor of the joint having the largest operation amount when the robot is operated in the traveling direction of the conveyor may be used.
[0024]
If the tracking error does not occur even if the conveyor speed changes after the above correction, the value b may be used, but if the tracking error changes, it can be corrected as follows. When a delay of x1 [mm] occurs when the conveyor speed is v1 [mm / s] and x2 [mm] occurs when v2 [mm / s], the following equation may be used instead of the above b.
b = 1 / (KT) + c + (x2-x1) / ((v2-v1) T) +1
[0025]
Next, the generated tracking delay correction amount dD_{i + 1}Based on the robot movement target movement amount D_{i + 1}Ask for. First, the previous tracking error R_iIs read from the tracking error data storage unit 28. Tracking delay correction amount dD_{i + 1}And tracking error R_iFrom the following target movement amount D_{i + 1}Is obtained by the following equation.
D_{i + 1}= DD_{i + 1}+ R_i
[0026]
In the follow-up movement amount generation unit 27, the follow-up target movement amount D generated by the follow-up delay correction unit 26._{i + 1}Then, the following movement amount Vn of the robot in the next control cycle is obtained from the following movement amount Vr in the previous control cycle of the robot and the estimated movement amount Vc in the next control cycle of the conveyor. A method for obtaining the follow-up movement amount Vn of the robot in the next control cycle will be described with reference to FIG. Vr, Vc, D_{i + 1}Are three-dimensional data and have three elements in the x, y, and z directions. Vn is obtained by performing the processing of FIG. 2 for each element. In the following, Vr, Vc, D_{i + 1}These elements are denoted by vr, vc, and d, respectively. In addition, the sign of each moving amount is positive in the traveling direction of the conveyor.
[0027]
  First, in ST1, it is determined whether the previous control period follow-up movement amount vr of the robot is positive or negative. If positive, the process proceeds to step ST2 according to the flow shown in FIG. The detailed procedure of ST30 is shown in FIG. In step ST2, when the robot accelerates or decelerates in the next control cycle,Maximum follow-up movement amount for the next control cycleva,Next control cycle minimum follow-up travelvd is obtained by the following equation.
  va = vr + acc
  vd = vr-dec
Here, acc and dec are a control period tracking acceleration amount and a deceleration amount, respectively, and both are positive values. These values are determined in consideration of the characteristics of the robot. Next control cycle maximum obtained in this wayFollowingThe movement amount va is compared with the maximum movement amount vmax in one control cycle of the robot. When va is larger than vmax, va = vmax is set.
[0028]
In step ST3, the estimated movement amount vc of the conveyor and the previous control period tracking movement amount vr of the robot are compared. If vc is larger, the robot must be accelerated, so in step ST4, acc is set to the acceleration / deceleration speed ad. If vr is larger, the robot must decelerate, so -dec is set to the acceleration / deceleration ad.
[0029]
In step ST5, the control cycle number n required for the conveyor and the robot to reach the same speed is obtained by the following equation.
n = int ((vc-vr) / ad)
Here, int (x) is a function for obtaining the maximum integer not exceeding x. If n is a positive integer other than 0 in step ST16, the process proceeds to step ST7, where the robot follow-up movement amount vn in the next control cycle necessary for the robot to move the follow-up target movement amount d after n control cycles is expressed by the following equation. Ask.
vn = (d + vc × (n−1)) / n− (n−1) × ad / 2
[0030]
If n is 0 in step ST16, the robot and the conveyor are at the same speed, so the robot follow-up movement amount vn in the next control cycle is set to vn = d in step ST11.
[0031]
In step ST8, vn obtained in steps ST7 and ST11 is compared with the next control cycle maximum follow-up movement amount va obtained in step ST2. When vn is larger than va, acceleration is not accelerated beyond a preset acceleration. To vn = va. If it is smaller, the process proceeds to step ST12, where vn is compared with the next control cycle minimum follow-up movement amount vd. When vn is smaller than vd, it is corrected to vn = vd so as not to decelerate more than a preset deceleration. If vn is larger, the value without modification is used as the follow-up movement amount of the next control cycle.
[0032]
On the other hand, if vr is 0 or less in step ST1, the conveyor and the robot are operating in opposite directions, or the robot is stopped, and the process proceeds to ST30. FIG. 3 shows a processing procedure in ST30.
[0033]
In step ST14, the next control cycle maximum follow-up movement amount va and the next control cycle minimum follow-up movement amount vd of the robot are obtained by the following equations.
va = vr-acc
vd = vr + dec
However, when va is larger than the robot maximum follow-up movement amount vmax, va = −vmax.
[0034]
Next, in step ST15, the number n of control cycles required until the conveyor and the robot reach the same speed is obtained by the following equation.
na = int (abs (vc / acc)) + 1
nd = int (abs (vr / dec))
n = na + nd
Here, abs (x) is a function for obtaining the absolute value of x, na is the number of control cycles until the robot reaches the speed of the conveyor from the speed 0, and nd is the time until the robot decelerates from the current speed to the speed 0. The number of control cycles.
[0035]
In step ST16, it is determined whether or not nd is a positive integer that is not 0. If it is a positive integer that is not 0, the process proceeds to step ST17, and if it is 0, the process proceeds to step ST20.
[0036]
In step ST17, the following control cycle robot follow-up movement amount vn necessary for the robot to catch up with the workpiece in the control cycle n is obtained from the following equation.
xc = d + vc × (n−1)
xr = na * (na-1) * acc / 2
vn = (xc−xr) / nd− (nd−1) × dec / 2
Here, xc and xr are the movement amounts of the workpiece and the robot until n control cycles, respectively.
[0037]
If nd is 0 in step ST16, the process proceeds to step ST20, where vc and the next control cycle minimum follow-up movement amount vd are compared. If vc is larger, the process proceeds to step ST21, and if not, the process proceeds to step ST22.
[0038]
In step ST21, vn is obtained by the following equation.
n = int ((vc−vd) / acc) +1
xc = d + vc × (n−1)
vn = xc / n-acc.times. (n-1) / 2
Here, xc is the amount that the work moves before the robot catches up with the work.
[0039]
On the other hand, if vc is equal to or smaller than vd in step ST20, the robot and the conveyor are considered to be at the same speed, so vn = d.
[0040]
In step ST18, vn and va are compared. If vn is smaller than va, the process proceeds to step ST19, and the value of vn is corrected to va. If vn is not smaller than va in step ST18, the process proceeds to step ST23 where vn and vd are compared. In step ST23, if vn is larger than vd, the value of vn is modified to vd in step ST24. When vn is not larger than vd, the value of vn obtained in steps ST17, ST21, ST22 is used as it is.
[0041]
The robot follow-up movement amount Vn obtained as described above is output to the command pulse generator 21. Also, the following error R_{i + 1}Is obtained by the following equation and stored in the tracking error data storage unit 28.
R_{i + 1}= R_i+ DC_i-Vn
[0042]
The operation control unit 20 generates an operation command value according to the robot program and outputs it to the command pulse generation unit 21. The command pulse generation unit 21 synthesizes the command value generated by the operation control unit 20 and the follow-up movement amount generated by the follow-up movement amount generation unit 27, and inversely converts the obtained command data to generate a pulse of the servo motor. Data is output to the servo control unit 22. The servo control unit 22 controls the servo motor of the robot according to the pulse data generated by the command pulse generation unit 21 to cause the robot to perform a desired operation.
[0043]
FIG. 4 is an explanatory diagram showing changes in the conveyor speed and the follow-up speed of the robot. In the figure, vc represents the conveyor speed, and vr represents the follow-up speed of the robot. At time t0, the robot starts following work on the conveyor, and in order to recover the follow-up delay, the following speed in the next control cycle is obtained for each control cycle until time t1 and accelerated. When the follow-up speed of the next control cycle is calculated at time t1, if acceleration is continued as it is, the target position on the conveyor is overtaken, so deceleration is started. At time t2, since the speed of the conveyor has changed from v1 to v2, the robot follow-up speed is accelerated again. At time t3, the speed starts to decelerate so as not to overtake the target position, and catches up with the conveyor at time t4.
[0044]
In the first embodiment, the follow-up movement amount is determined in consideration of the maximum speed at which the robot can operate and the maximum acceleration / deceleration (acceleration pattern, deceleration pattern). It is possible to follow the workpiece. In the first embodiment, an example in which one control cycle follow acceleration amount and deceleration amount are used as the acceleration pattern and deceleration pattern has been shown, but acceleration time and deceleration time may be used. If the maximum robot speed is sufficiently higher than the conveyor speed, the follow-up movement amount may be determined without considering the maximum robot speed.
[0045]
In addition, since the acceleration / deceleration is smoothly performed, it is possible to follow without causing vibration, and the follow-up work can be performed with high accuracy.
[0046]
Moreover, since the maximum value and the minimum value of the conveyor encoder can be determined freely, it is possible to follow the work on a long conveyor. Furthermore, since the encoder value can be filtered, it is possible to smoothly follow even if the conveyor is vibrating.
[0047]
In the first embodiment, the follow-up delay correction unit 26 calculates the follow-up delay correction amount using the formula b = 1 / KT + c + 1. However, the correction amount is previously set for each moving amount of the conveyor, and for each position and posture of the robot. It may be obtained and stored as a table, and during operation, the correction amount may be obtained by interpolation from the table data based on the current position and speed of the robot. Further, the value b may be stored in the table instead of the correction amount. FIG. 5 is a table in which the value of the correction coefficient b is stored for each value of the robot position x. If the value of x indicating the current position of the robot is x1 <x <x2 when the tracking delay correction unit 26 calculates the tracking delay correction amount, the correction coefficient b is obtained by the following equation.
b = (b2-b1) (x-x1) / (x2-x1) + b1
By obtaining the correction amount using the value of b, it is possible to eliminate the tracking error due to the position of the robot. In the first embodiment, the correction amount is obtained for each position and posture of the robot. However, an amount representing the state of the robot or an amount representing the state such as the speed of the conveyor may be used.
[0048]
Further, in the first embodiment, it is needless to say that the same effect can be obtained even when the tracking delay correction unit 26 calculates the correction coefficient b dynamically from the position and orientation of the robot.
[0049]
In the first embodiment, the robot follow-up movement amount generated by the transient follow-up delay correction amount generation unit 27 is directly passed to the command pulse generation unit. However, in order to smooth the command value, the follow-up movement amount for the recent m control period May be averaged. In this case, when m is added to b of the follow-up delay correction unit 26, for example, when b is obtained by using an expression b = 1 / KT + c + 1, a delay due to averaging by setting b = 1 / KT + c + m + 1. Can be corrected.
[0050]
In the first embodiment, the virtual encoder value generation unit 24 converts the encoder value of the conveyor into the virtual encoder value, but the number of bits of the counter of the encoder value detection unit 23 is sufficiently larger than the follow-up work area. Of course, when the operation of the conveyor is not oscillating, as shown in FIG. 6, the configuration without the virtual encoder value generating unit 24 shown in FIG. In the configuration of FIG. 6, the encoder value detected by the encoder value detector 23 is directly sent to the conveyor movement amount generator 25, and the conveyor movement amount is calculated based on this encoder value.
[0051]
In the first embodiment, the case where the position of the conveyor is grasped by the conveyor encoder and the work is performed while following the conveyor has been described. However, when the work is carried by a transfer device other than the conveyor, for example, an xy table or another robot. However, the follow-up operation can be performed in the same manner by directly inputting those current positions to the controller.
[0052]
Embodiment 2. FIG.
FIG. 7 is a diagram for explaining a robot control apparatus according to Embodiment 2 for carrying out the present invention. In the figure, the same reference numerals as those in FIG. 1 denote the same or corresponding parts. In FIG. 7, reference numeral 29 denotes a joint follow-up delay correcting unit, which corrects the follow-up delay for the command pulse for each joint generated by the command pulse generating unit 21.
[0053]
Next, the operation will be described. The conveyor encoder 4, the encoder value detection unit 23, and the conveyor movement amount generation unit 25 perform the same operations as those in the first embodiment. In the follow-up movement amount generation unit 27, in the first embodiment, the conveyor movement amount generated by the conveyor movement amount generation unit 25 is used instead of the estimated conveyor movement amount in the next one control cycle generated by the tracking delay correction unit. To generate the follow-up movement amount of the robot. The generation method is the same as in the first embodiment.
[0054]
The operation control unit 20 generates an operation command value according to the robot program and outputs it to the command pulse generation unit 21. The command pulse generation unit 21 synthesizes the command value generated by the operation control unit 20 and the follow-up movement amount generated by the follow-up movement amount generation unit 27, and inversely converts the obtained command data to generate a pulse of the servo motor. The data is output to the joint tracking delay correction unit 29.
[0055]
The joint tracking delay correction unit 29 corrects the delay amount for each joint. In the first embodiment, the follow-up delay correction unit calculates the estimated conveyor movement amount of the next control cycle from the conveyer movement amount, but the joint follow-up delay correction unit 29 uses the robot joint movement amount instead of the conveyor movement amount. A joint movement amount corrected for the following delay is generated.
[0056]
Output to the servo controller 22. The servo control unit 22 controls the servo motor of the robot in accordance with the pulse data generated by the joint tracking delay correction unit 29, thereby causing the robot to perform a desired operation.
[0057]
In the second embodiment, since the delay amount is corrected for each joint, a more accurate follow-up operation is possible.
[0058]
In the second embodiment, the position of the conveyor is grasped by the conveyor encoder and the work is performed while following the conveyor. However, the conveyor is transported by a transport device other than the conveyor, for example, an xy table or another robot. Even when the current position is directly input to the controller, the following operation can be performed in the same manner.
[0059]
According to the robot controller of the first configuration of the present invention,Considering the maximum operating speed and maximum acceleration / deceleration of the robot,Determine the amount of follow-up movementI did soThere is an effect that the follow-up work can be performed at high speed.In addition, since the acceleration / deceleration is smoothly performed, it is possible to follow without causing vibration, and the following work can be performed with high accuracy.
[0061]
  In addition, the present invention2According to the robot control apparatus having the above configuration, when the steady tracking error is corrected, the correction coefficient is determined from the control gain of the servo motor, so that it is possible to easily realize a highly accurate tracking work. .
[0062]
  In addition, the present invention3According to the robot controller configured as described above, the correction data for each speed and position and orientation of the robot stored in the storage area in advance by the follow-up delay correction unit.ForSince the tracking correction is performed, there is an effect that the tracking operation with high accuracy can be performed regardless of the speed fluctuation of the transfer device and the position and orientation of the robot.
[Brief description of the drawings]
FIG. 1 is a configuration diagram for explaining a robot control apparatus according to Embodiment 1 of the present invention;
FIG. 2 is a flowchart for explaining a procedure for generating a follow-up movement amount according to Embodiment 1 of the present invention;
FIG. 3 is a flowchart for explaining a procedure for generating a follow-up movement amount according to Embodiment 1 of the present invention;
FIG. 4 is a diagram for explaining the relationship between the speed and time of a robot and a transfer device according to Embodiment 1 of the present invention;
FIG. 5 is a diagram for explaining a storage area for tracking correction data according to Embodiment 1 of the present invention;
FIG. 6 is a configuration diagram for explaining a robot control apparatus according to Embodiment 1 of the present invention;
FIG. 7 is a configuration diagram for explaining a robot control apparatus according to a second embodiment of the present invention.
FIG. 8 is a configuration diagram for explaining a conventional robot control device.
[Explanation of symbols]
DESCRIPTION OF SYMBOLS 1 Robot, 2 Robot control apparatus, 3 Conveyance apparatus, 4 Conveyor encoder, 20 Operation control part, 21 Command pulse generation part, 22 Servo control part, 23 Conveyance apparatus position detection part, 24 Virtual conveyance apparatus position generation part, 25 Conveyance apparatus Movement amount generation unit, 26 tracking delay correction unit, 27 tracking movement amount generation unit, 28 tracking error storage unit, 29 joint tracking delay correction unit.

Claims

In a robot control apparatus that causes a robot to perform work while following a work object conveyed by a conveyance apparatus,
A transport device position detector for detecting the position of the transport device;
A transfer device position generation unit that converts a position obtained by the transfer device position detection unit and generates a virtual transfer device position;
A virtual transport device position to generate a command value obtained robot by said conveying device position determining unit, the virtual transport device the position of the current time ti identified by an integer i which is assigned in the order for each control period interval When a conveying device moving amount generating unit for generating a movement amount dC _i where the conveyance device from the difference between the virtual transport device located on the first control peripheral period has moved at time ti-1 of one control period before the time ti,
A tracking error storage unit that stores a tracking error of the robot with respect to the transfer device;
From the movement amount dC _i generated by the transfer device movement amount generation unit, the movement amount dC _i-1 obtained one control period before, and the correction coefficient a, the next control period estimated movement amount is calculated as Vc = dC _i + a × (dC _i -DC _i-1 ), a follow-up delay correction amount for correcting the follow-up delay of the robot is obtained by the product of the estimated movement amount Vc of the next control cycle and the follow-up delay correction coefficient b, and the follow-up delay correction amount and the follow-up error memory are obtained. A tracking delay correction unit that generates a tracking target movement amount by the sum of the tracking error stored in the unit,
Three-dimensional from the following target movement amount generated by the following delay correction unit, the following movement amount generated one control period before the robot, the next control period estimated movement amount, the one control period following acceleration amount or the deceleration amount of the robot The follow-up movement amount Vn of the next control cycle, which is data, is obtained, and the component of each coordinate axis of the follow-up movement amount Vn of the next control cycle falls within the range of the next control cycle maximum follow-up movement amount and the next control cycle minimum follow-up movement amount. A follow-up movement amount generation unit for generating a follow-up movement amount Vn of the next robot,
An operation control unit that generates an operation command value according to the operation program of the robot;
A command pulse generator for adding a follow-up movement amount generated by the follow-up movement amount generator to a motion command value generated by the operation controller, and generating a command value for the robot;
A servo control unit for controlling the servo motor from the command value generated by the command pulse generation unit and operating the robot;
A robot control device comprising:

The tracking delay correction coefficient b is a value 1 / (KT) obtained from the control gain K of the servo motor position loop, the control cycle T of the robot, and the control until the command value is input from the command pulse generator to the servo controller. The robot control apparatus according to claim 1, wherein the robot control apparatus is obtained by a sum of the number of periods c and the number of control periods for reflecting the movement amount of the transfer apparatus in the next control period.

The storage area for storing correction data for each state of the transfer device or the robot is provided, and the tracking delay correction amount of the robot is determined by the correction data for each state of the transfer device or the robot. Robot controller.