JPS6254688A - Controller for industrial robot - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。(57) [Summary] This bulletin contains application data before electronic filing, so abstract data is not recorded.

Description

【発明の詳細な説明】 〔産業上の利用分野〕 この発明は、移動中の作業対象物をロボットマニプレー
タ自体が追跡して所定の作業を行うトラッキングロボッ
トの制御装置に関するものである。DETAILED DESCRIPTION OF THE INVENTION [Industrial Application Field] The present invention relates to a tracking robot control device in which a robot manipulator itself tracks a moving work object to perform a predetermined work.

〔従来の技術〕[Conventional technology]

第７図は従来のトラッキングロボットの構成の一例を示
す斜視図である。α〔はロボットマニプレータ、■はロ
ボットの制御装置、（至）はロボットをコンベアと並行
に移送するためのトラッキング装置、（４０）は作業対
象物を移送するコンベア装置、（５０）はコンベア装［
：（４０）とトラッキング装置−との同期制御装置、（
３１）はトラッキングの駆動用サーボモータ、（３２）
は速度検出器、（４１）は作業対象物、（４２）はコン
ベア上のトラッキング開始位置を作業対象物（４１）が
通過したことを検知する同期検出器、（４３）はコンベ
アの速度検出器、（４４）はトラッキングの終了を検知
する検出器、（４５）は速度検出器（３２）、　（４３
）と同期制御装置（５０）とを接続するケーブルである
。FIG. 7 is a perspective view showing an example of the configuration of a conventional tracking robot. α [ is the robot manipulator, ■ is the robot control device, (to) is the tracking device for transporting the robot in parallel with the conveyor, (40) is the conveyor device that transports the workpiece, (50) is the conveyor equipment [
: (40) and tracking device - synchronization control device, (
31) is a tracking drive servo motor, (32)
is a speed detector, (41) is a workpiece, (42) is a synchronization detector that detects when the workpiece (41) passes the tracking start position on the conveyor, and (43) is a conveyor speed detector. , (44) is a detector that detects the end of tracking, (45) is a speed detector (32), (43
) and the synchronous control device (50).

上記のように構成された従来のトラッキングロボットに
おいて、作業対象物（４１）がコンベア上のトラッキン
グ開始位りを通過すると、同期検出器（４２）が作動す
る。次いで、同期制御装置（５０）釦よってロボットを
トラッキング開始位置と一致させ、ロボットと作業対象
物（４１）との相対速度が零となるようにコンベア装＆
（４０）の速度を速度検出器（４３〕で検出し、その速
度を駆動用サーボモータ（３１）の速度指令として与え
、速度が同一となるようにコンベア装置（４０）の速度
検出器（３２）を介してフィードバック制御を行う。そ
して、作業対象物（４１）がトラッキングの終了を検知
する検出器（４４）が作動すると、自動的にロボットが
トラッキング開始点まで戻るようになっている〇 〔発明が解決しようとする問題点〕 上記のような従来のトラッキングロボット装置では、ト
ラッキング装置端とコンベア装置（４０）とが平行に配
設され、トラッキング装置■はロボットと独立して設け
る必要があることと、ロボットの作業点教示はコンベア
装置（４０）上で行なわれるので、作業変更時のライン
停止時間が長くなったシ、また、コンベア装＠　（４０
）において速度が変動する場合の補償が困難であるなど
の問題があった。In the conventional tracking robot configured as described above, when the workpiece (41) passes the tracking start position on the conveyor, the synchronization detector (42) is activated. Next, the synchronization control device (50) button is used to align the robot with the tracking start position, and the conveyor system is adjusted so that the relative speed between the robot and the workpiece (41) becomes zero.
(40) is detected by a speed detector (43), the detected speed is given as a speed command to the drive servo motor (31), and the speed detector (32) of the conveyor device (40) is set so that the speeds are the same. ), and when the detector (44) that detects the end of tracking of the workpiece (41) is activated, the robot automatically returns to the tracking start point〇〇 [Problems to be Solved by the Invention] In the conventional tracking robot device as described above, the end of the tracking device and the conveyor device (40) are arranged in parallel, and the tracking device ■ needs to be installed independently of the robot. In addition, since the robot's work point teaching is performed on the conveyor device (40), the line stop time when changing tasks becomes longer, and the conveyor device @ (40)
), there were problems such as difficulty in compensating for speed fluctuations.

この発明はかかる問題点を解決するためになされたもの
で、ロボット自体でトラッキングを行うようにして、コ
ンベア装置とロボットとの位置関係が動作範囲以外では
制約をなくして、ロボットに対する作業教示をコンベア
装置上で行なわ表ぐ°Ｃもよく、かつ、コンベアの速度
変動が発生しても速度予測を行って補償できるようなト
ラッキングロボットの制御装ｆｆｌ得ることを目的とす
る。This invention was made in order to solve this problem, and the robot itself performs tracking, eliminating restrictions on the positional relationship between the conveyor device and the robot outside of the operating range, and giving work instructions to the robot on the conveyor. It is an object of the present invention to provide a control device ffl for a tracking robot, which has a good temperature range expressed on the device and can predict and compensate for speed fluctuations of a conveyor even if speed fluctuations occur.

〔問題点を解決するための手段〕[Means for solving problems]

この発明に係るロボットの制御装置は、ロボットの作業
対象物を移送するコンベアの速度を検出し、この検出値
を累積してコンベア上の作業対象物の刻々と変化する位
ｆＩｔｔ−演算し、コンベアの移送速度の予測を行うと
ともにロボット座標系基準で設定されたマニプレータ手
先の目標位置を移動中の作業対象物の基準座標系に変換
してから、マニプレータ手先の軌跡計画を行なって予測
されたコンベア移送速度を補償するものである。The robot control device according to the present invention detects the speed of the conveyor that transports the workpiece of the robot, accumulates this detected value, calculates the ever-changing rate fItt of the workpiece on the conveyor, and calculates the speed of the conveyor that transfers the workpiece of the robot. At the same time, the target position of the manipulator hand set based on the robot coordinate system is converted to the reference coordinate system of the moving workpiece, and then the trajectory of the manipulator hand is planned. This compensates for the transfer speed.

〔作用〕[Effect]

この発明におけるロボットマニプレータの手先は、作業
対象物を移送するコンベアの速度予測によって、負荷変
動などで数十％のコンベア速度の変動が生じても、正確
に作業対象物を追跡するとともに、所望通ｐの軌跡を描
いて作業を行う。By predicting the speed of the conveyor that transports the workpiece, the hands of the robot manipulator in this invention can accurately track the workpiece and achieve the desired result even if the conveyor speed fluctuates by several tens of percent due to load fluctuations. Work by drawing the trajectory of p.

〔実施例〕〔Example〕

第１図はこの発明の一実施例によるトラッキングロボッ
ト装置の制御の全体構成図である。図において、（１０
０）はロボット制御装置、（１２０）はロボットマニプ
レータの関節を駆動するサーボモータ、（１２１）はこ
のサーボモータ（１２０）の位置検出器、（１２３）は
コンベア装置、　（１２４）はコンベア速度検出器、（
１２５）はコンベア装置（１２！ｌ）上を作業対象物（
以下ワークと言う）の基準位！（以下ワークペースと言
う）が特定の位置（以下コンベアペースと１゛う）を通
過するときに１″の信号を発生する同期信号発生器、（
１０１）はロボットのプログラム記憶部、　　（１０２
）はプログラム実行部、（１０３）は手先の目標位置を
ロボット座標基準からワークペース基準に変換する目標
位置変換部、（１０４）は刻々に変化するコンベア移送
速度をΔを時間ごとに検出して同期信号発生器（１２４
）が１”の信号を発光すると距離累積を零とし、そね以
外ではΔを時間ごとのコンベアの啓動距離を累積する距
離累積部、（１０５）はプログラム（１０２）の出力で
あるロボット座標系基準のコンベア移送方向における方
向余弦に累積距離を分解するとともに、コンベアベース
の位置に位置加算してロボット座標系基準のコンベアベ
ースの位置を演算し、この演算結果と目標位置変換部（
１０３）の出力であるワークベース基準の目標位置とを
位置加算することによって、現在（時刻ｎΔｔ）におけ
るコンベア上のワークの目標点をロボット座標系基準に
変換する目標位置生成部、（１０６）は例えば上記目標
点とロボットの現在の手先位置とをプログラム実行部（
１０２）の出力によって直線移動するために、近未来の
時刻（ｎ＋１）Δｔの手先位置を計画する軌跡計画部、
　　（１０７）はコンベア速度とその変化率とから次の
Δを時間のコンベア移送距離を演算するコンベア速度予
測部、（１０８）はこの予測距離をコンベア移送方向の
方向余弦に分溶するコンベア速度のベクトル生成部、（
１０９）は軌跡計画部（１０６）の出力とベクトル生成
部（１０８）との出力を位置加η−するコンベア速度補
償部、（１１０）は与えられた手先の位置全実現するた
めに、マニプレータの各関節角度を与え、指令角度とし
て位置決め制御部へ出力するマニプレータ座標変換部、
（１１１１ｊマニプレークに取Ｑつけられた位置検出器
の値が指令角度と一致するように腺差電圧を出力する位
置決めフィードバック制御部、（１１２）は誤差電圧を
サーボモータ駆動電流に変換・増幅する電力増幅部であ
る。FIG. 1 is an overall configuration diagram of the control of a tracking robot device according to an embodiment of the present invention. In the figure, (10
0) is a robot control device, (120) is a servo motor that drives the joints of the robot manipulator, (121) is a position detector for this servo motor (120), (123) is a conveyor device, and (124) is a conveyor speed detection vessel,(
125) carries the workpiece (
Reference position for (hereinafter referred to as work)! A synchronizing signal generator (hereinafter referred to as work pace) that generates a 1" signal when the work pace (hereinafter referred to as work pace) passes a specific position (hereinafter referred to as conveyor pace),
101) is the program storage unit of the robot, (102
) is a program execution unit, (103) is a target position conversion unit that converts the target position of the hand from the robot coordinate reference to the work pace reference, and (104) is a unit that detects the ever-changing conveyor transfer speed by time. Synchronous signal generator (124
) emits a signal of 1", the distance accumulation is zero, and in other cases, Δ is the distance accumulation section that accumulates the conveyor movement distance for each time. (105) is the robot coordinate system that is the output of the program (102). The accumulated distance is decomposed into the direction cosine in the reference conveyor transfer direction, and the position is added to the conveyor base position to calculate the conveyor base position based on the robot coordinate system, and this calculation result is combined with the target position conversion unit (
A target position generation unit (106) converts the current (time nΔt) target point of the workpiece on the conveyor to the robot coordinate system standard by adding the target position of the work base standard which is the output of 103). For example, the program execution unit (
a trajectory planning unit that plans the position of the hand at time (n+1)Δt in the near future in order to move in a straight line according to the output of 102);
(107) is a conveyor speed prediction unit that calculates the conveyor transfer distance in time using the following Δ from the conveyor speed and its rate of change, and (108) is a conveyor speed prediction unit that divides this predicted distance into the direction cosine of the conveyor transfer direction. Vector generator, (
109) is a conveyor speed compensator that performs a positional adjustment η- on the output of the trajectory planning unit (106) and the output of the vector generator (108), and (110) is a conveyor speed compensator that performs a position adjustment η- on the output of the trajectory planning unit (106) and the output of the vector generator (108). a manipulator coordinate conversion unit that gives each joint angle and outputs it as a command angle to the positioning control unit;
(1111j Positioning feedback control unit that outputs the differential voltage so that the value of the position detector Q attached to the manipulator matches the command angle, (112) is the power that converts and amplifies the error voltage into servo motor drive current This is the amplification section.

次に動作について説明する。第２図はマニプレータ手先
とワークの移動位置を示す説明図、第６図は制御装置の
動作を示すフローチャート、第４図は移動制御処理のフ
ローチャート、ａ￥５図は作業プログラム記憶部の構成
を示す説明図、第６図はこの作業プログラム記憶部にお
ける便数の記号などの説明図である。Next, the operation will be explained. Fig. 2 is an explanatory diagram showing the movement position of the manipulator hand and workpiece, Fig. 6 is a flowchart showing the operation of the control device, Fig. 4 is a flowchart of movement control processing, and Fig. a￥5 shows the configuration of the work program storage unit. The explanatory diagram shown in FIG. 6 is an explanatory diagram of the symbol of the number of flights, etc. in this work program storage section.

先ず、全体の処理の流れ全第６図のフローチャートに基
づいて説明する。First, the entire process flow will be explained based on the flowchart shown in FIG.

■　ポインタ初期化部（３０１）において、プログラム
ポインタ（１０１ａ）にＯを代入する。(2) The pointer initialization unit (301) assigns O to the program pointer (101a).

■　プログラム実行部（１０２）において、プログラム
ポインタ（１０１ａ）の示す命令コードをとシ出す。(2) The program execution unit (102) outputs the instruction code indicated by the program pointer (101a).

すなわち、第５図の例ではワークベース定義命令コード
であるＯＯが取シ出されて、プログラム実行部（１０２
）がこの００を判断すると命令コードの次に記憶されて
いるワークベース値を第６図の作業費数ＰＷＢＳへ代入
する。That is, in the example shown in FIG. 5, the work base definition instruction code OO is extracted and executed by the program execution unit (102
) determines this value of 00, it assigns the work base value stored next to the instruction code to the work cost number PWBS in FIG.

■　命令判別部（６０２）において、上記命令コードが
終了命令コードの９９であると終了へ、または移動命令
コードの０４であればラベルＭＯＶＳへ分岐し、それ以
外のときはラベルＮＥＸＴへ分岐させる。この例ではラ
ベルＮＥＸＴへ分岐する。(2) In the instruction determination unit (602), if the instruction code is the end instruction code 99, the process branches to the end, or if the move instruction code is 04, the process branches to the label MOVS, otherwise it branches to the label NEXT. In this example, the process branches to label NEXT.

■　プログラムポインタ歩道部（３０５）において、プ
ログラムポインタ（１０１ａ）に１”を加算してラベル
Ｍｌｏａｐへ分岐する。(2) In the program pointer walkway section (305), add 1'' to the program pointer (101a) and branch to label Mloap.

■　ラベルＭｌｏｏｐでは再びプログラム実行部（１０
２）が実行されて上記■の場合と同様にしてワークベー
ス値が作業変数ＰＣＢＳへ代入され、■。■ At label Mloop, the program execution part (10
2) is executed and the work base value is assigned to the work variable PCBS in the same way as in case ① above.

■と同様の処理によってプログラムポインタ（１０１ａ
）が２”となる。The program pointer (101a
) becomes 2”.

■　プログラム実行部（１０２）においては、コンベア
ベクトル定輻−コーＦ″′Ｃある０２が取フ出され、次
の演算が行なわれる。(2) In the program execution unit (102), the conveyor vector constant convergence code F'''C is extracted and the following calculation is performed.

この結果が作業変数Ｐｃｖｃｔｌに代入され、ロボット
座標系からみたコンベア移送方向ベクトルの方向余弦と
なる。次いで、■、■と同様の処理が行なわれてプログ
ラムポインタ（１０１ａ）が１３”トする。This result is assigned to the work variable Pcvctl, and becomes the direction cosine of the conveyor transfer direction vector as seen from the robot coordinate system. Next, the same processing as (1) and (2) is performed, and the program pointer (101a) is incremented by 13".

■　プログラム実行部（１０２）においては、手先速度
定義コードである０３をと力出し、ワークベース値を作
業変数Ｆに代入する。次いで、■、■と同様の処理が行
なわれてプログラムポインタ（１０１ａ）が′４”とな
る〇 ■　プログラム実行部（１０２）においては、直線移動
コードである０４を取り出し、手先の目標位置が作業変
数Ｐｄｅｓｔ　Ｋ代入され、■の処理に基づいてラベル
ＭＯＶＳへ分岐する。(2) The program execution unit (102) outputs the hand speed definition code 03 and substitutes the work base value into the work variable F. Next, the same processing as in ■ and ■ is performed, and the program pointer (101a) becomes '4''.〇■ In the program execution unit (102), the linear movement code 04 is taken out, and the target position of the hand is set to ``4''. The variable Pdest K is assigned, and the process branches to the label MOVS based on the process of (2).

■　目標位置変換部（１０３）においては、作業変数Ｐ
ｄｅｓｔ　、　ＰｗＢＢ　　に対して次の演算を行なっ
て作業変数Ｐｄｅｓｔ　Ｄを作成する。■ In the target position converter (103), the work variable P
The following operation is performed on dest and PwBB to create a work variable Pdest D.

ＰｄｅｓｔＤ＝　（ＰＷＢＳ）　　■（ＰｄｅＲｔ）こ
こで％ＰＷＢＳ右肩の−１はＰｗｎｓの逆マトリクスを
、演算■はマトリックス積を示しておフ、以下位置加算
と呼ぶ。上記演算結果である作業変数ＰｄｅｓｔＤは、
ワークペースを示す作業変数ＰＷＢＳを基準としたとき
の目標位置を表わしておシ、第２図におけるＰｄｅｓｔ
　　となる。PdestD= (PWBS) (PdeRt) Here, -1 on the right side of %PWBS indicates the inverse matrix of Pwns, and operation (2) indicates matrix product, hereinafter referred to as positional addition. The work variable PdestD, which is the result of the above calculation, is
Pdest in Fig. 2 represents the target position based on the work variable PWBS indicating the work pace.
becomes.

［相］　次に、移動制御部（３０４）に処理が移行し、
移動完了判断部（３０３）が完了と判断するまで繰り返
し処理が実行される。[Phase] Next, the process moves to the movement control unit (304),
The process is repeated until the movement completion determination unit (303) determines that the movement is complete.

０　上記移動が完了すると、茅動完了判断部（３０３）
はラベルＮＥＸＴへ分岐してブふグラムポインタ（１０
１ａ）は５”になる。0 When the above movement is completed, the movement completion judgment unit (303)
branches to the label NEXT and points to the Bufugram pointer (10
1a) becomes 5”.

■　プログラム実行部（１０２）は終了コード９９をを
９出し、■の処理に戻して終了へ分岐し、作業を完了す
る。(2) The program execution unit (102) issues an end code 99, returns to the process (2), branches to the end, and completes the work.

次に第４図のフローチャートに基づいて移動制御部の動
作について説明する。Next, the operation of the movement control section will be explained based on the flowchart of FIG.

０　Δｔタイマ部（４０１）は移動制御をΔｔごとの離
散時間制御において実現するためのタイマで、以下現在
の時刻をｎΔｔ１近過去の時刻を（ｎ−１）Δｔ、近未
来の時刻を（ｎ＋１）Δｔと呼ぶ。0 The Δt timer section (401) is a timer for realizing movement control in discrete time control for each Δt. Hereinafter, the current time is expressed as nΔt1, the time in the near past is expressed as (n-1)Δt, and the time in the near future is expressed as (n+1). ) is called Δt.

［相］　コンベア速度入力部（４０２）は作業変数Ｖｎ
−１の値をｖｎ−２に代入、またｖｎの値をＶｎ−１に
代入して、（ｎ−１）Δｔ　からｎΔｔ’ｔでのコンベ
アの移送距離すなわちΔを間の距離を第１図のコンベア
速度検出器（１２４）から入力してｖｎに代入するＯ ＠　移送距離累積部（１０４）において、作業変数σｎ
−１の値をσ。に代入してから第１図における同期信号
発生部（１２５）の状態を入力して作業変数σ。に代入
する。[Phase] The conveyor speed input section (402) is the work variable Vn
By substituting the value of -1 into vn-2 and the value of vn into Vn-1, we can calculate the transport distance of the conveyor from (n-1)Δt to nΔt't, that is, the distance between Δ. The work variable σn is inputted from the conveyor speed detector (124) and substituted into vn.
-1 value is σ. Then, the state of the synchronization signal generator (125) in FIG. 1 is input to obtain the work variable σ. Assign to .

ｅ若し、σｎ−１＝Ｑでσ。＝１であればコンベアベー
スからの移送距離を表わす作業変数ｌｎにＯを代入して
ｌｎ＝０とする。If e, σn-1=Q and σ. If =1, O is substituted into the work variable ln representing the transfer distance from the conveyor base to make ln=0.

［相］若し、σｎ−１＝Ｑでなくσ。＝１でもなければ
Ｉｎ”　Ａ！　Ｂ　”　Ｖ　ｎとし、時刻ｎ・Δｔのコ
ンベア移送距離を作成する〇 ［相］　目標位置生成部（１０５）においては、上記■
と■の結果、作業変数ＰｃｖｃＴｔ　Ｐｄｅｓｔ　と上
記ｌｎに対して下記の操作を施して結果を作業変数Ｐｄ
ｅｓｔ　ｎに代入する。[Phase] If σn-1=σ instead of Q. If it is not equal to 1, set In"A!B"Vn and create the conveyor transfer distance at time n・Δt.
As a result of and ■, perform the following operations on the work variable PcvcTt Pdest and the above ln, and convert the result to the work variable Pd.
Assign to est n.

Ｐｄｅｓｔｎ＝（Ｐｃｎｓ）■（（Ｐｃｖｃｒ）（ｌｎ
月■（Ｐｄｅｓｔ　Ｄ）作業変数Ｐｄｅａｔｎは時刻ｎ
Δｔのワーク上の目標位置をコンベアベース（ＰＣＢＳ
）を起点として、コンベア移送方向（ＰＣＶＣＴ）方向
に距離ｉｎだけ移送し、た位置、すなわち第２図におけ
るワーク位［Ｃを示している。Pdestn=(Pcns)■((Pcvcr)(ln
Month ■ (Pdest D) Work variable Pdeatn is time n
The target position of Δt on the workpiece is set on the conveyor base (PCBS).
) as a starting point, the workpiece is transferred by a distance in in the conveyor transfer direction (PCVCT), and the workpiece position [C] in FIG. 2 is shown.

０　軌跡計画部（１０６）は、上記■の結果であるワー
ク上の移動速度を表わす作業変数Ｆと、時刻ｎΔｔの手
先の現在値Ｐｎと０の出力である目標位１Ｐｄｅａｔｎ
とを下記の式のように操作して、時刻（ｎ＋１）Δｔ　
の手先の近未来値を作成し、作業変数Ｐｎ＋ＩＫ代入す
る。0 The trajectory planning unit (106) calculates the work variable F representing the moving speed on the workpiece, which is the result of the above (■), the current value Pn of the hand at time nΔt, and the target position 1Pdeatn, which is the output of 0.
By operating as shown in the formula below, time (n+1)Δt
Create the near future value of the hand and substitute it as the work variable Pn+IK.

ここで、若しＰｎ＋１がＰｄｅｇｔｎを超えているとき
は、Ｐｙｌ＋１＝Ｐｄｅｓｔｎおよび移動完了フラグを
ＯＮとして上記０の処理に寄与する。Here, if Pn+1 exceeds Pdegtn, Pyl+1=Pdestn and the movement completion flag are turned ON to contribute to the process of 0 above.

０　コンベア速度予測部（１０７）は、上記＠の出力で
ある作業変数Ｖ　ｎ　＊　Ｖｎ−１＊　Ｖｎ−２に下記
の操作を行なって、時刻ｎΔｔから（ｎ＋１）Δｔ　の
間のコンベア移送距離Ｖｋｎ　　を予測する。0 The conveyor speed prediction unit (107) performs the following operation on the work variable V n * Vn-1 * Vn-2 which is the output of the above @, and calculates the conveyor transfer distance Vkn between time nΔt and (n+1)Δt. Predict.

Ｖｋｎ＝Ｖｎ”Ｋｔ　（ｖｎ−ｖｎ−１）　”Ｋｔ　（
（ｖｎ−ｖｎ−１）−−ｖｎ−２）’ ここで、Ｋｌおよびに、は、０≦に１≦２〜３１Ｏ≦に
、≦２〜３の値をとるフォーワードゲインである。Vkn=Vn"Kt (vn-vn-1) "Kt (
(vn-vn-1)--vn-2)' Here, Kl and 2 are forward gains that take values of 0≦, 1≦2 to 31O≦, and ≦2 to 3.

［相］　コンベア速度ベクトル生成部（１０８）は、上
記作業変数ｖｋｎ　と■の出力でちる作業変数Ｐ。Ｂ８
と■の出力ＰＣＶ　（ＴＫ　ｓ下記の操作を行って、時
刻ｎΔｔから（ｎ＋１）Δｔ　間にコンベアが移送する
と予測される距離であるＶ　　をロボット座標系ｎ 基準のベクトル成分に分解して、作業変数Ｐｖｋｎを得
る。[Phase] The conveyor speed vector generation unit (108) generates a work variable P, which is determined by the work variable vkn and the output of ■. B8
Output PCV of and Obtain the variable Pvkn.

Ｐ　　　＝（Ｐ　　　）■（ＰＣＶＣＴ’　（ｖｋｎ）
Ｖｌｊｎ　　　Ｃ１０ ［相］　コンベア速度補償部（１０９）は上記作業変数
Ｐｖｋｎ１！：ｏの出力であるＰ　　に下記の操作を行
ｎ＋１ なって、時刻ｎｌｔから（ｎ＋１）Δｔの間のコンベア
移送分の補償を行う。P = (P) ■ (PCVCT' (vkn)
Vljn C10 [Phase] The conveyor speed compensator (109) uses the above work variable Pvkn1! :The following operation is performed on P, which is the output of o, to compensate for the conveyor transfer between time nlt and (n+1)Δt.

Ｐｎ＋１＝（Ｐ　　　　）■（Ｐ　　　　）ｎ＋１ｖｋ
ｎ ＠　マニプレータ座標変換部（１１０）は、上記作業変
数Ｐ　　に手先が一致するようなマニプレータｎ＋１ の各関節角度Ｊ　　を演ｑ、シて、位置決め制御部ｎ＋
１ （１１１）へ出力する。Pn+1=(P)■(P)n+1vk
n @ The manipulator coordinate conversion unit (110) calculates each joint angle J of the manipulator n+1 such that the hand matches the work variable P, and converts the positioning control unit n+
1 (111).

Ｊｎ＋１＝ｇ（Ｐ　　　） ｎ＋１ ここで、ｇは手元の位置と姿勢から角度を求めるための
関数である。Jn+1=g(P)n+1 Here, g is a function for determining the angle from the position and posture of the hand.

また、時刻（ｎ　＋　１）Δものときの手先の現在値に
Ｐ　　を代入してＰｎ＝Ｐｎ＋１とすることにより、ｎ
＋１ ０ボツトの手先が移送中のワークに追従しながら所望の
軌跡をワーク上に描くことになる。Also, by substituting P for the current value of the hand at time (n + 1)Δ and setting Pn=Pn+1, n
+10 The robot's hand follows the workpiece being transferred and draws a desired trajectory on the workpiece.

なお、上記実施例では直線軌跡の場合について説明した
が、関数軌跡の場合であってもよく、また、移動命令が
複数個であっても、あるいは、移動命令以外の場合でも
、ワーク上の現在位置を保持しながら作業を行う場合に
おいても適用することができる。In the above embodiment, the case of a straight line trajectory was explained, but the case of a function trajectory may also be used.Also, even if there are multiple movement commands, or even in cases other than movement commands, the current position on the workpiece may be It can also be applied when working while maintaining the position.

〔発明の効果〕〔Effect of the invention〕

この発明は以上説明したとおり、コンベアの移送速度予
測部を軌跡計画後の未来値に作用させるとともに、作業
目標点をワークペース上に変換し、移送中のワークペー
スの現在位置を刻々に表現することによって、手先の軌
跡を所望の軌跡とすることが可能となり、トラッキング
装置を用いなくても移送速度に変動があるコンベア上の
ワークに追従しながら作業を行う高精度のロボット装置
が安価に得られる効果がある。As explained above, this invention operates the transfer speed prediction unit of the conveyor on the future value after trajectory planning, converts the work target point onto the work pace, and expresses the current position of the work pace during transfer moment by moment. This makes it possible to set the trajectory of the hand to the desired trajectory, and it is possible to obtain a high-precision robot device that can work while following a workpiece on a conveyor whose transfer speed fluctuates without using a tracking device at a low cost. It has the effect of

【図面の簡単な説明】[Brief explanation of the drawing]

第１図はこの発明の一実施例によるロボット制御の全体
構成を示すブロック図、第２図はこの発明によるマニプ
レータ手先とワークの移動位置を示す説明図、第６図は
この発明によるｆｌｌ＋御装置の動作を示すフローチャ
ート、＠４図はこの発明による移動制御処理を示すフロ
ーチャート、第５図はこの発明による作業プログラム記
憶部の構成を示す説明図、第６図は上記作業プログラム
記憶部に：ひける変数の記号などを示す説明図、第７図
は従来のトラッキングロボットの構成の一例を示す図で
ある。 図において、（１０１）はプログラムも己１念部、（１
０２）はプログラム実行部、（１０４）は距離累積部、
（１０６）は軌跡計画部、（１０７）！′２°コンベア
速度予測部、（１１０）はマニプレーク部標変移：部、
（１１１）は位置決めフィードバック制御部、（１１２
）は電力増幅部。 なお、図中同一符号は同一部分を示す。 代理人　弁理士　佐　藤　　正　　年 第３図 第４図 第５図FIG. 1 is a block diagram showing the overall configuration of robot control according to an embodiment of the present invention, FIG. 2 is an explanatory diagram showing the movement positions of the manipulator hand and workpiece according to the present invention, and FIG. 6 is a full + control device according to the present invention. FIG. 4 is a flowchart showing the movement control processing according to the present invention, FIG. FIG. 7 is an explanatory diagram showing symbols of variables, etc., and is a diagram showing an example of the configuration of a conventional tracking robot. In the figure, (101) means that the program is also self-centered, (1
02) is a program execution unit, (104) is a distance accumulation unit,
(106) is the trajectory planning department, (107)! '2° conveyor speed prediction section, (110) is the manipulae section target displacement: section,
(111) is a positioning feedback control unit, (112)
) is the power amplification section. Note that the same reference numerals in the figures indicate the same parts. Agent: Patent Attorney Masaru Sato Figure 3, Figure 4, Figure 5

Claims (1)

【特許請求の範囲】 ロボットのマニプレータに一連の作業を与えるプログラ
ムを記憶する記憶部、上記プログラムを逐次解読・実行
する実行部、この実行部の出力であるロボットのマニプ
レータ手先の目標位置および軌跡と速度とに基づいて近
未来の上記マニプレータ手先の位置を演算する軌跡計画
部、この軌跡計画部の出力を上記マニプレータの各関節
角度に変換する座標変換部、この座標変換部の出力と刻
々変化する上記マニプレータの関節角度を位置検出器を
介して検出し、これらの角度誤差を出力する位置決め制
御部、この位置決め制御部の出力を電力増幅して上記マ
ニプレータの各関節を駆動するための電力を供給する電
力増幅部、作業対象物を移送するコンベアの速度を検出
し、この検出値を累積して上記コンベアの移送距離を得
るコンベア距離累積部、上記コンベアの速度およびその
変化率とからコンベア速度を予測するコンベア速度予測
部を備え、 上記作業対象物を移送する上記コンベアの速度を検出し
、この検出値を上記コンベア距離累積部に累積させると
ともに、上記コンベア速度予測部などによつて変化する
上記作業対象物の位置を演算し、この演算に基づいて上
記マニプレータ手先の軌跡計画を行うことを特徴とする
産業用ロボットの制御装置。[Scope of Claims] A storage unit that stores a program for giving a series of tasks to the manipulator of the robot, an execution unit that sequentially decodes and executes the program, and a target position and trajectory of the manipulator hand of the robot that is the output of this execution unit. a trajectory planning section that calculates the position of the hand of the manipulator in the near future based on the speed; a coordinate conversion section that converts the output of the trajectory planning section into each joint angle of the manipulator; and a coordinate conversion section that changes every moment with the output of the coordinate conversion section. A positioning control unit that detects the joint angles of the manipulator via a position detector and outputs these angle errors, and a power amplification of the output of this positioning control unit to supply power for driving each joint of the manipulator. a power amplification unit that detects the speed of the conveyor that transfers the object to be worked on, and a conveyor distance accumulation unit that calculates the transfer distance of the conveyor by accumulating the detected value; a conveyor speed is calculated from the speed of the conveyor and its rate of change; A conveyor speed prediction unit detects the speed of the conveyor that transfers the work object, accumulates this detected value in the conveyor distance accumulation unit, and changes the speed by the conveyor speed prediction unit or the like. A control device for an industrial robot, characterized in that the position of a work object is calculated, and the trajectory of the manipulator hand is planned based on the calculation.