JPH02141808A - Angular velocity control method - Google Patents

Abstract

PURPOSE:To shorten the movement time for the operation of less load by controlling the angular velocity at each time of operation based on inputted positioning data and the load applied to a front end arm so that the torque applied to a motor is maximum. CONSTITUTION:A robot main body consists of a column 1, a first arm 11, a second arm 12, motors 13 and 14 which drive respective arms, a driving circuit part which drives respective motors, and a numerical controller 3. The numerical controller 3 consists of an arithmetic unit 31, a storage device 32, and a speed command device 33, and the angular speed of motors for slope up and slope down is determined based on positioning data stored in the storage device 32 at each time of operation, and a speed command signal is inputted to the driving circuit part 2 through the speed command device 33. This signal is amplified in the driving circuit part 2 to drive first and second motors 13 and 14.

Description

【発明の詳細な説明】 産業上の利用分野 本発明は多関節型ロボットにおける第１アームと第２ア
ームの角加速度制御方法に関するものである。DETAILED DESCRIPTION OF THE INVENTION Field of the Invention The present invention relates to a method for controlling the angular acceleration of a first arm and a second arm in an articulated robot.

従来の技術 水平多関節型ロボットのヌローアップ時、ヌローダウン
時におけるアームのモータに掛るトルク７はその時の第
２アームの姿勢、移動に必要な第１アームと第２アーム
のモータ回転数比、第１アームと第２アームの回転方向
等によって異なってくる。そのため従来の角加速度制御
は２点Ｐｉ、Ｐ、＋１の位置決めデータに基づいて最低
どちらか一方のモータに掛るトルクを許容範囲内で最大
にする最適スローアップ、スローダウン時間を求めて移
動時間の短縮を図っている。Conventional technology The torque 7 applied to the motor of the arm when the horizontal articulated robot is stretched up and down is determined by the posture of the second arm at that time, the motor rotation speed ratio of the first arm and the second arm required for movement, and the It differs depending on the rotation direction of the first arm and the second arm, etc. Therefore, conventional angular acceleration control calculates the optimal slow-up and slow-down times to maximize the torque applied to at least one of the motors within an allowable range based on the positioning data of the two points Pi, P, and +1. We are trying to shorten the time.

発明が解決しようとする課題 しかしながら上記の手法では２点の位置データしかとり
扱っていないため、先端アームにかかる負荷量には安全
面より常に最大値が設定されていなければならない。こ
れは無負荷の状態のスローも アップ、スローダウン動作に対して最大負荷の掛るスロ
ーアップ、スローダウン動作と同じだけの時間を要する
ことになり、移動時間を短縮する際、大きな障害となっ
ていた。Problems to be Solved by the Invention However, since the above-mentioned method only handles position data at two points, the load applied to the tip arm must always be set at a maximum value for safety reasons. This means that the slow-up and slow-down operations under no load require the same amount of time as the slow-up and slow-down operations under the maximum load, which is a major obstacle when reducing travel time. Ta.

課題を解決するための手段 本発明は、モータのトルクに影響を与える前記負荷量Ｗ
ｉ　　を求める方法として、数値制御装置において動作
命令以前に所定の負荷量を命令もしくはセンサー装置に
て設定する方法、あるいは第２アーム先端にあり前記負
荷量Ｗｉ　　を支えるＺ軸において、このＺ軸ヲ！動す
るモータの電流慎重２とトルクＴ２　の関係を示すトル
り曲線”（Ｉ）及びこのＺ軸モータにかかるトルクＴ２
と前記負荷量Ｗｉとの関係ヲ示す負荷曲線ＷＣＴｌを記
憶し、■　２軸モータに流れる電流慎重２ヲ読みとるス
テップ、 ■　前記電流慎重２と前記トルク曲線Ｔ（Ｉ）とからＺ
軸モータにかかるトルりＴ２２求めるステップ、 ■　前記トルりＴ２　　と前記負荷曲線ＷＣＴｌがら負
荷量Ｗ、　　を求めるステップ、 にて構成されたものである。Means for Solving the Problems The present invention provides the load amount W that affects the torque of the motor.
i can be determined by setting a predetermined load amount using a command or sensor device before issuing an operation command in the numerical control device, or by setting the Z-axis at the tip of the second arm that supports the load amount Wi. ! Torque curve (I) showing the relationship between the current 2 of the moving motor and the torque T2, and the torque T2 applied to this Z-axis motor.
and the load curve WCTl indicating the relationship between the load amount Wi and the load amount Wi; 1) reading the current 2 flowing through the two-axis motor; 2) calculating Z from the current 2 and the torque curve T(I);
This step consists of the steps of determining the torque T22 applied to the shaft motor; (2) determining the load amount W from the torque T2 and the load curve WCTl;

作　　　用 本発明は、上記構成により、アーム先端に負荷される重
量を考慮した上でモータや駆動回路の能力を最大限に生
し常に最短時間で移動するよう角加速度制御することが
できる。Effects According to the present invention, with the above-described configuration, the angular acceleration can be controlled to maximize the capabilities of the motor and drive circuit and always move in the shortest time, taking into consideration the weight loaded on the tip of the arm.

実施例 以下に本発明の一実施例を第１図にもとづいて説明する
。EXAMPLE An example of the present invention will be described below with reference to FIG.

図において１はロボット本体の支柱、１１は支柱１に接
続された第１アーム、１２は第２アームで１３と１４は
第１アーム１１と第２アーム１２を駆動するモータであ
る。２は上記モータ１３゜１４を駆動するための駆動回
路部である。３は数値制御装置で、演算装置３１．記憶
装置３２．速度指令装置３３から構成される装置 納された位置決めデータからスローアップ，スローダウ
ン時のモータの角加速度を＠作毎に決定し、速度指令装
置３３を通じて速度指令信号を前記駆動回路部２に入力
するものである。In the figure, 1 is a support of the robot body, 11 is a first arm connected to the support 1, 12 is a second arm, and 13 and 14 are motors that drive the first arm 11 and the second arm 12. Reference numeral 2 denotes a drive circuit section for driving the motors 13 and 14. 3 is a numerical control device, and an arithmetic device 31. Storage device 32. A device consisting of a speed command device 33 determines the angular acceleration of the motor during slow-up and slow-down for each operation from the stored positioning data, and inputs a speed command signal to the drive circuit section 2 through the speed command device 33. It is something to do.

次に加速度制御の方法を第２図にもとづいて説明する。Next, a method of acceleration control will be explained based on FIG. 2.

スローアップ時間が同じでもスローアップ時のトノレフ
は第２アーム１２の姿勢（θ２）、移動に必要な第１ア
ーム１１と第２アーム１２のモータ回転数比と、回転方
向によって大きく異なってくる。Even if the slow-up time is the same, the tono reflex during slow-up differs greatly depending on the attitude (θ2) of the second arm 12, the motor rotation speed ratio of the first arm 11 and the second arm 12 necessary for movement, and the rotation direction.

第２アーム先端がある点Ｐ・からある点Ｐ，＋１へ移動
する場合第１アーム１１と第２アーム１２の移動角がΔ
θ１，Δθ２　で第１アーム１１と第２アーム１２が同
時スタート、同時停止動作をするとすれば、それぞれの
モータの定常状態での角速度ω１。When the tip of the second arm moves from a certain point P to a certain point P, +1, the movement angle of the first arm 11 and second arm 12 is Δ
If the first arm 11 and the second arm 12 start and stop simultaneously at θ1 and Δθ2, the angular velocity ω1 of each motor in the steady state.

ω２は ■　Δθ１×町≦Δθ２×−の時 ■　Δθ１×町〉Δθ２ｘＸ′ｎ２　の時と表わされる
。ここで ｍｌ，ｍ２は第１アーム，第２アームの減速化ωＭはモ
ータの最大角速度 である。ω2 is expressed as: ■ When Δθ1×Town≦Δθ2×-; ■ When Δθ1×Town>Δθ2xX'n2. Here, ml, m2 are the first arm, and deceleration ωM of the second arm is the maximum angular velocity of the motor.

ところで力学の法則より第１アーム１１及び第２アーム
１２のモータに掛るトルクＴ１（ｔ）、　Ｔ、（ｔ）例
えば次のようにして表わされる。By the way, according to the laws of mechanics, the torques T1(t), T,(t) applied to the motors of the first arm 11 and the second arm 12 are expressed, for example, as follows.

は ｍｔ）＝　ｆ　ｕ（ｔ）　＊　ｏ　＜　ｔ≦１但しＴＡ
＝（ａｓ＋ａｅ６ｗ＋（ａ７＋ａＢ　・Ｗ）ｃｏｓθ２
）ω１＋（ａ９＋ａ１ｏＩＩＷ＋（ａ１１＋ａ１２・Ｗ
）ＩＩｃＯ５θ２）ω２−（ａ１３＋ａ１４ｅＷ）−（
２ω１・ω２＋ω２２）・ｓｉｎθ２但しＴＢ＝（ｂ５
＋ｂ６・Ｗ）ω２ ＋（ｂ７＋ｂ８−Ｗ＋（ｂ９＋ｂ１゜−Ｗ）　　−ｃｏ
ｓθ２）ω１＋（ｂ１１＋ｂ１２・Ｗ）ω１・ｓｉｎθ
２ここでＷはアーム先端の負荷、ωＡは第１モータの角
速度、ω２は第２モータの角速度、またａ１ｔａ２　、
・・・・・・、ａ１４及びｂｌ、ｂ２．・・・・・・、
ｂ１２はいずれも定数でロボットのアームの長さ９重量
、摩擦抵抗等によって異なってくる。mt) = f u(t) * o < t≦1, but TA
=(as+ae6w+(a7+aB ・W)cosθ2
)ω1+(a9+a1oIIW+(a11+a12・W
)IIcO5θ2)ω2−(a13+a14eW)−(
2ω1・ω2+ω22)・sinθ2 However, TB=(b5
+b6・W)ω2 +(b7+b8-W+(b9+b1°-W) -co
sθ2)ω1+(b11+b12・W)ω1・sinθ
2 Here, W is the load at the end of the arm, ωA is the angular velocity of the first motor, ω2 is the angular velocity of the second motor, and a1ta2,
......, a14 and bl, b2.・・・・・・、
b12 is a constant and varies depending on the length, weight, and frictional resistance of the robot arm.

従ってスローアップ曲線を第３図のような滑らかな曲線 と定め第１モータ、第２モータの定常時における角速度
をω１．ω２　とし、スローアップ時間をｔｕ。Therefore, the slow-up curve is set as a smooth curve as shown in FIG. 3, and the angular velocities of the first and second motors at steady state are set to ω1. ω2 and the slow-up time is tu.

動作開始時の第２アーム１２の姿勢角を０２８とすると
スローアップ中、各モータに掛るトμりは次のようにＷ
、θ２８．ω１．ω２ｍ　’ｕ　　の関数として書き表
わすことができる。If the attitude angle of the second arm 12 at the start of operation is 028, the torque applied to each motor during slow-up is W as follows.
, θ28. ω1. It can be expressed as a function of ω2m'u.

ＴＩ（ｔ）＝ｃｒ１　（Ｗ、θ２ｓ會ω１＋’２１ｔｕ
）Ｔ２（ｔ）＝ｑ２（Ｗ、θ２８．ω１．ω２ｓ’ｕ）
よって とおくと １α１く１のときω１＝ωＭｔ　ω２”　ａ””Ｍ１α
１〉１のときω１シー・ωＭ、ω２−ωＭα であるから、Ｗ、θ２ｓ、αをパラメータとして、それ
ぞれのＷ、０２Ｂ、α　を有する動作に対して要する最
小のスローアップ時間ｔｕｉ　ｋ第４図のフローチャー
トに従って計算することができる。TI(t)=cr1 (W, θ2s meeting ω1+'21tu
)T2(t)=q2(W, θ28.ω1.ω2s'u)
Therefore, when 1α1 × 1, ω1 = ωMt ω2” a””M1α
When 1>1, ω1C・ωM, ω2−ωMα, so using W, θ2s, α as parameters, the minimum slow-up time tui k required for the operation with each W, 02B, α Fig. 4 It can be calculated according to the flowchart.

次ニフローチャート（第４図）のアルゴリズムを説明す
る。The algorithm of the next flowchart (FIG. 4) will be explained.

■　スローアップ時間ｔｕＴｈ適尚な初期値’ｕ。■ Slow-up time tuTh Appropriate initial value 'u.

とする。shall be.

■　スローアップ中の第１アーム、第２アームのトルり
Ｔ　１（ｔ）　ｔ　’ｒ　２（ｔ）を求める■ さもなければ ｔ求める。■ Find the torque T 1 (t) t 'r 2 (t) of the first arm and the second arm during slow-up ■ Otherwise, find t.

■　ｔｕ−＝ｍａｘ（ｔｕｌ、ｔｕ、）として■へ戻る
。■ Set tu-=max(tul, tu,) and return to ■.

但しｍａｘ（ｔｕｌ、ｔｕ２）＝ ここで ■　Ｔ１＝ｍａｘＴ１（ｔ）、Ｔ２＝ｍｒｘＴ２（ｔ）
として１’ｒ１−”Ｍｌｌ＜ε１またはＩ”２−”Ｍ２
＋＜ε２ならば現在のｔｕが実行中のパラメータθ２８
．αに対して最適スローアップ時間’ｕｉとなる。However, max (tul, tu2) = where ■ T1 = maxT1 (t), T2 = mrxT2 (t)
as 1′r1−”Mll<ε1 or I”2−”M2
If +<ε2, the parameter θ28 for which the current tu is being executed
．． The optimum slow-up time 'ui is given to α.

ＴＭｌは第１モータに許される最大出力トルり。TMl is the maximum output torque allowed for the first motor.

Ｔ勧つは第２モータに詐弐飴ス畠＋■壱し＋ｌ＋ｊ＋以
下これを繰返すことによって任意のＷ、θ２Ｊαに対し
て最適スローアップ時間ｔｕｉを求めることができる。The recommendation is to set the second motor to the second motor.By repeating this process, the optimum slow-up time tui can be found for any W and θ2Jα.

このようにして求まったｔｕｉに対して最適スローアッ
プ曲線は ■　ω１ｉ（ｔｕｉ）−０Ｍのとき ■ ω２ｉ　（ｔｕｉ）−′Ｍ のとき 但し０≦ｔ　りｔｕｉ として求まる。The optimal slow-up curve for tui thus determined is determined as follows: (1) When ω1i (tui)-0M; (2) When ω2i (tui)-'M, where 0≦t t tui.

スローダウンの場合もθ２Ｅｅ動作の終点Ｐｌ＋１にお
ける第２アームの姿勢角とすると、０２Ｅ’αをパラメ
ータとして、スローアップの場合と同様の手法によって として求まる。In the case of slowdown, if θ2Ee is the attitude angle of the second arm at the end point Pl+1 of the motion, it is determined by using the same method as in the case of slowup, using 02E'α as a parameter.

なる性質がある。There is a characteristic that

このようにして求まった角速度データは、速度指令装置
３３へ入力され、速度信号となって駆動回路部２に出力
される。さらに駆動回路部２で増幅されて、第１モータ
１３と第２モータ１４が駆動される構成になっている。The angular velocity data thus determined is input to the velocity command device 33 and outputted to the drive circuit section 2 as a velocity signal. The signal is further amplified by the drive circuit section 2, and the first motor 13 and the second motor 14 are driven.

次に第２アーム先端にかかる前記負荷量Ｗｉの算出方法
を第８図に基づいて説明する。Next, a method for calculating the load amount Wi applied to the tip of the second arm will be explained based on FIG. 8.

■　まず、プログラム命令もしくはセンサー装置により
負荷量Ｗ、が設定されているかどうかを判断する。(1) First, it is determined whether the load amount W is set by a program instruction or a sensor device.

■　負荷量Ｗ１が設定されていなければ、次に第２アー
ム先端でこのＷｉを支えているＺ軸のモータ電流値ｚ　
　’にカレントトランス及びＡ／Ｄコンバータ等で読み
とる。モータが交流モータである場合は、一般に以下の
変換を行えば直流モータ電流と等価な電流慎重、を求め
ることがＷ、　＝Ｗ　（’ｒ２） 以上の重施例では第１モータと第２モータの最大角速度
が同じであるとして説明したがその必要はない。以上の
アルゴリズムをより一般的にフローチャートラ用いて説
明すると次のようになる（第６図参照）。■ If the load amount W1 is not set, then the Z-axis motor current value z that supports this Wi at the tip of the second arm
' is read using a current transformer, A/D converter, etc. If the motor is an AC motor, you can generally calculate the current equivalent to the DC motor current by performing the following conversion: W, = W ('r2) In the above multiple example, the first motor and the second motor Although the explanation has been made assuming that the maximum angular velocities of the two are the same, this is not necessary. The above algorithm can be explained more generally using a flowchart as follows (see FIG. 6).

■　上記モータ電流値Ｉ、　　（もしくはＩ、）と既に
メモリに記憶されているトルク曲線”（Ｉ）とからｚ軸
モータのトルりＴ２　は次式により求められる。(2) The torque T2 of the z-axis motor is obtained from the above motor current value I, (or I,) and the torque curve "(I) already stored in the memory by the following equation.

Ｔ　　＝’ｒ（ｔ２） ■上記２軸モータのトルクＴ２　と既にメモリに記憶さ
れている負荷曲線−とから負荷量Ｗｉは次式により求め
られる。T='r(t2) (2) The load amount Wi is determined by the following equation from the torque T2 of the two-axis motor and the load curve already stored in the memory.

但しｆｄ（０）−１，ｆｄ（１）−〇 を入力。However, fd(0)-1, fd(1)-〇 Enter.

また、２軸モータのトルり定数ＫＴ　及び、トルりと負
荷量Ｗｉ　　との関係を示す負荷曲線−を入力。Also, input the torque constant KT of the two-axis motor and the load curve showing the relationship between the torque and the load amount Wi.

■　２点ＰＰ・　のデータを入力。また、負ｉ−ｔ＋１ 荷量Ｗｉ　　を入力もしくは算出。■Enter the data for 2 points PP. Also, negative it+1 Enter or calculate the load amount Wi.

■　点Ｐ１　から点Ｐｌ＋１への移動におけるスローア
ップ中、第１モータ、第２モータに掛るトルク”ｕｌ（
ｔ）　−”ｕ２（ｊ）　　がｔＥｃｏ、ｔｕ）に対して ｌ　Ｔｕｌ（ｔ）　ｌ≦Ｙｕ１　ｔ　ｌ　Ｔｕ２（ｔ）
ｌ≦Ｙｕ２であり、かつ ｔ（２（０、ｔｕ）に対して ｌ　Ｔ　ｕｌ　（ｔ）　ｌ　＝７ｕ　１またはｌ　Ｔｕ
２（ｔ）　ｌ　＝〒ｕ２とならしめるｔｕ　を第４図の
最適化手法によって求める。ここでＴｕｌ　ｓ”ｕ２　
　はスローアップ時、第１モータ、第２モータに許され
る最大トルり。■ Torque “ul(
t) −”u2(j) for tEco, tu) l Tul(t) l≦Yu1 t l Tu2(t)
l≦Yu2, and for t(2(0, tu), l T ul (t) l =7u 1 or l Tu
tu which makes 2(t) l =〒u2 is determined by the optimization method shown in FIG. Here Tul s”u2
is the maximum torque allowed for the first and second motors during slow-up.

■　ステップ■で求めた最適・スローアップ時間を今ｕ
１　とおく。■ The optimal slow-up time obtained in step ■ is now u.
Set it as 1.

■　第１モータ、第２モークの角速度をω１４（ｔ）＊
ω２．（１）とすると、最適スローアップ曲線は次のよ
うにして求まる。■ The angular velocity of the first motor and second moke is ω14(t)*
ω2. (1), the optimal slow-up curve is found as follows.

■　ω１ｉ（ｔｕｉ）”０１Ｍのとき ■　ω２ｉ（ｔｕｉ””０２Ｍのとき 以下、Ｐｉ＋１が最終点に到来するまでステップ■から
ステップ■を繰返す。■ When ω1i (tui) is 01M ■ When ω2i (tui) is 02M From then on, steps 2 to 2 are repeated until Pi+1 reaches the final point.

又、最適スローダウン曲線も全く同様に考えることがで
きる。Moreover, the optimal slowdown curve can be considered in exactly the same way.

次にもう１つの実施例について述べる。Next, another example will be described.

実施例１の説明により、負荷量Ｗ、と点ＰｉＰ、＋１の
データから点Ｐ、での第２アームの姿勢角θ２８とモー
タの回転数比αとモータ（アーム）の回転方向が分れば
その時の最適スローアップ時間ｔｕｉ　を求めることが
可能である。According to the explanation of Example 1, if the attitude angle θ28 of the second arm at point P, the rotation speed ratio α of the motor, and the rotation direction of the motor (arm) are known from the load amount W, the data at point PiP, +1, then It is possible to find the optimal slow-up time tui at that time.

そこでＷ、θ２８とαを離散値パラメータとして、それ
らの最適スローアップ時間を前もって計算し、次のよう
に表わすことができる（ここでは簡単のため第１モータ
と第２モータの最大回転数を同じとし、〜で表わす）。Therefore, by using W, θ28, and α as discrete value parameters, their optimal slow-up times can be calculated in advance and expressed as follows (here, for simplicity, the maximum rotation speeds of the first and second motors are assumed to be the same). ).

ここでｌ　、　ｍ　、　ｎは　ｌ　＝　１．２　、・・
・−、ｋｍ＝１．２．・・・・・・、に′ ｎ＝１．２．・・・・・・、に″ であり、又θ７＊”ｍｔβ。、Ｗｎは ０≦θｌ−１〈θ１　＜　１８０　’ −１≦ａｍ−１〈α□≦１ 一１≦β。−１〈β。≦１ ０≦Ｗｎ−１〈Ｗｎ≦”ｗａｘ　（最大許容負荷）であ
る。Here l, m, n are l = 1.2,...
・-, km=1.2. ......, n=1.2. ......, ni'', and θ7*''mtβ. , Wn is 0≦θl-1<θ1<180'-1≦am-1<α□≦1 -1≦β. −1〈β. ≦1 0≦Wn-1〈Wn≦”wax (maximum allowable load).

これらのｔｕ、ｔｕ　　は全て、記憶装置３２に前もっ
て記憶させておくことができるので、プログラドに従っ
て近似的に決定することができる。Since these tu and tu can all be stored in advance in the storage device 32, they can be approximately determined according to the program.

スローアップ時間’ｕｉが求まれば実施例１と同様に、
その時の最適なスローアップ曲線を求めることができる
。Once the slow-up time 'ui is found, as in Example 1,
The optimal slow-up curve at that time can be found.

以上のことをフローチャーＦに表わすと第７図のように
なる。The above is expressed in flowchart F as shown in FIG.

又、スローダウンについても全く同様の考え方ができる
のは明らかである。Furthermore, it is clear that the same concept can be applied to slowdown.

さらに第１モータと第２モータの最大角速度が異なる場
合も実施例１で示したごとく、同様の考え方で拡張でき
る。Furthermore, even when the maximum angular velocities of the first motor and the second motor are different, the same concept can be applied as shown in the first embodiment.

この実施例ではモータのトルりに影Ｖｔ−与える要素と
してθ２とαとモータの回転方向を用いたが、これに換
る別の要素を用いてもよい。要は表現はどうであれＷｌ
とＰＬｖ　Ｐｉ　＋１に応じてモータのトルクに影響を
与える全ての要素が含まれていればよい。In this embodiment, θ2, α, and the rotational direction of the motor are used as elements that affect the torque of the motor (Vt-), but other elements may be used instead. The point is, no matter how you express it, Wl
It is sufficient that all elements that affect the motor torque according to and PLv Pi +1 are included.

実施例１においては最適スローアップ、スローダウン曲
線を求めることが可能でおる。実施例２においてはパラ
メータθ２．α及びＷを離散値化して求めるという意味
において準最適なスローアップ、スローダウン曲線とな
るが一般にＷｉと点ＰＬとＰ、＋１　のデータを読込ん
でからスローアップ。In the first embodiment, it is possible to obtain optimal slow-up and slow-down curves. In the second embodiment, the parameter θ2. This is a sub-optimal slow-up/slow-down curve in the sense that α and W are obtained by converting them into discrete values, but in general, the slow-up is performed after reading the data of Wi, points PL, P, +1.

スローダウン曲線を求めるまでの処理時間は前者に比べ
て大巾に短縮できるという特徴がある。A feature of this method is that the processing time required to obtain the slowdown curve can be significantly shortened compared to the former method.

発明の効果 このように本発明は動作毎に、入力された位置決めデー
タと先端アームにかかる負荷量に基づいて、スローアッ
プ、スローダウン時、第１アーム又は第２アームの最低
どちらか一方のモータに掛るトルりが、許容範囲内で最
大となるように角加速度制御をしている。従って従来の
ように先端アームにかかる負荷量を最大許容量に固定し
て角加速度制御した場合に比べると負荷量の少ない動作
に対して移動時間が短縮できるといった効果がある。Effects of the Invention As described above, the present invention controls the motor of at least one of the first arm or the second arm during slow-up or slow-down, based on the input positioning data and the load applied to the tip arm for each operation. The angular acceleration is controlled so that the torque applied to the shaft is maximized within the allowable range. Therefore, compared to the conventional case where the load applied to the distal end arm is fixed at the maximum allowable amount and the angular acceleration is controlled, there is an effect that the movement time can be shortened for operations with a small load.

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

第１図は本発明の一実施例における水平多関節型ロボッ
トの概略構成図、第２図は第１アームと第２アームの角
速度曲線およびその時モータに掛るトルク曲線の関係を
示す図、第３図は標準のスローアップ曲線図、第４図は
最適スローアップ時間を求めるためのフローチャート図
、第６図は第１実施例の最適スローアップ、スローダウ
ン曲線を求めるフローチャート図、第６図はプログラム
データに基づいて最適スローアップ時間をテーブルから
サーチするためのフローチャート図、第７図は第２実施
例の最適スローアップ曲線を求めるフローチャート図、
第８図は負荷量Ｗｌ　　を求めるフローチャート図であ
る。 １・・・・・・ロボット本体の支柱、１１・・・・・・
第１アーム、１２・・・・・・第２アーム、１３・・・
・・・第１モータ、１４・・・・・・第２モータ、２・
・・・・・モータ駆動回路部、３・・・・・・数値制御
装置、３１・・・・・・演算装置、３２・・・・・・記
憶装置、３３・・・・・・速度指令装置。 代理人の氏名　弁理士　粟　野　重　孝　ほか１名ｒｓ
−ｚ鞠 ３１・−濱筐裂１ ３１−一・署乙＋Ａ邑、１に量 第４図 区 Ｃ９ 法 第 図FIG. 1 is a schematic configuration diagram of a horizontal articulated robot according to an embodiment of the present invention, FIG. 2 is a diagram showing the relationship between the angular velocity curves of the first arm and the second arm and the torque curve applied to the motor at that time, and FIG. The figure shows a standard slow-up curve, Figure 4 is a flowchart for determining the optimal slow-up time, Figure 6 is a flowchart for determining the optimal slow-up and slow-down curves of the first embodiment, and Figure 6 is a program. A flowchart for searching the optimum slow-up time from a table based on data; FIG. 7 is a flowchart for determining the optimum slow-up curve of the second embodiment;
FIG. 8 is a flowchart for determining the load amount Wl. 1... Support of the robot body, 11...
First arm, 12... Second arm, 13...
...First motor, 14...Second motor, 2.
...Motor drive circuit section, 3 ... Numerical control device, 31 ... Arithmetic device, 32 ... Memory device, 33 ... Speed command Device. Name of agent: Patent attorney Shigetaka Awano and 1 other person rs
-Zmari 31・-Hamakagi Rifle 1 31-1・Sign Otsu+A Village, 1 Quantity Figure 4 Ward C9 Law Figure

Claims (2)

【特許請求の範囲】[Claims] （１）モータに速度指令を与え、回転可能な第１アーム
と連結した第２アームを作動させる角加速度制御方法で
あって前記第２アームの先端が、点Ｐ＿ｉから点Ｐ＿ｉ
＿＋＿１へ移動する際、スローアップ、スローダウンの
角速度指令を与える方法として、標準スローアップ曲線 ｆ＿ｕ（ｔ／ｔ＿ｕ）、０≦ｔ≦ｔ＿ｕ と標準スローダウン曲線 ｆ＿ｄ（ｔ／ｔ＿ｄ）、０＜ｔ≦ｔ＿ｄ 但し　ｆ＿ｕ（０）＝ｆ＿ｄ（１）＝０ を記憶し、 ［１］２点Ｐ＿ｉとＰ＿ｉ＿＋＿１のデータを読込むス
テップ。 ［２］アーム先端にかかる負荷量Ｗ＿ｉのデータを読み
込むステップ。 ［３］点Ｐ＿ｉから点Ｐ＿ｉ＿＋＿１への移動時に用い
るスローアップ曲線において、第１アームまたは第２ア
ームのモータにかかるトルクの少なくともどちらか一方
を許容範囲内で最大ならしめる最適スローアップ時間ｔ
＿ｕ＿ｉを求めるステップ、［４］第１アーム、第２ア
ームのモータ角速度をω＿１＿ｉ（ｔ）、ω＿２＿ｉ（
ｔ）として最適スローアップ曲線を｛ω＿１＿ｉ（ｔ）
＝Ｃ＿１＿ｉ・ｆ＿ｕ（ｔ／■＿ｕ＿ｉ）、０≦ｔ≦■
＿ｕ＿ｉω＿２＿ｉ（ｔ）＝Ｃ＿２＿ｉ・ｆ＿ｕ（ｔ／
■＿ｕ＿ｉ）、０≦ｔ≦■＿ｕ＿ｉ｝但しＣ＿１＿ｉ、
Ｃ＿２＿ｉは定数 として求めるステップ、 ［５］点Ｐ＿ｉから点Ｐ＿ｉ＿＋＿１へ移動時に用いる
スローダウン曲線において、第１アームまたは第２アー
ムのモータにかかるトルクの少なくともどちらか一方を
許容範囲内で最大ならしめる最適スローダウン時間■＿
ｄ＿ｉを求めるステップ、［６］最適スローダウン曲線
を ｛ω＿１＿ｉ（ｔ）＝Ｃ＿１＿ｉ・ｆ＿ｄ［（ｔ−ｔ＿
ｓ）／■＿ｄ＿ｉ］、０≦ｔ−ｔ＿ｓ≦ｔ＿ｄ＿ｉω＿
２＿ｉ（ｔ）＝Ｃ＿２＿ｉ・ｆ＿ｄ［（ｔ−ｔ＿ｓ）／
■＿ｄ＿ｉ］、０≦ｔ−ｔ＿ｓ≦■＿ｄ＿ｉ｝但しｔ＿
ｓはスローダウン開始時間とし て求めるステップ、 を繰返して構成された角加速度制御方法において、モー
タのトルクに影響を与える前記負荷量Ｗ＿ｉを求める方
法として、数値制御装置において動作命令以前に所定の
負荷量を命令もしくはセンサー装置にて設定する方法、
あるいは第２アーム先端にあり前記負荷量Ｗ＿ｉを支え
るＺ軸において、このＺ軸を駆動するモータの電流値Ｉ
＿ＺとトルクＴ＿Ｚの関係を示すトルク曲線Ｔ＿（＿Ｉ
＿）及びこのＺ軸モータにかかるトルクＴ＿Ｚと前記負
荷量Ｗ＿ｉとの関係を示す負荷曲線Ｗ＿（＿Ｔ＿）を記
憶し、 ［１］Ｚ軸モータに流れる電流値Ｉ＿Ｚを読みとるステ
ップ、 ［２］前記電流値Ｉ＿Ｚと前記トルク曲線Ｔ＿（＿Ｉ＿
）とからＺ軸モータにかかるトルクＴ＿Ｚを求めるステ
ップ、 ［３］前記トルクＴ＿Ｚと前記負荷曲線Ｗ＿（＿Ｔ＿）
から負荷量Ｗ＿ｉを求めるステップ、 にて構成された角加速度制御方法。(1) An angular acceleration control method in which a speed command is given to a motor to actuate a second arm connected to a rotatable first arm, in which the tip of the second arm moves from point P_i to point P_i.
When moving to ___+_1, a standard slow-up curve f_u (t/t_u), 0≦t≦t_u and a standard slow-down curve f_d (t/t_d), 0<t ≦t_d However, f_u(0)=f_d(1)=0 is stored, and [1] Step of reading data of two points P_i and P_i_+_1. [2] A step of reading data on the amount of load W_i applied to the tip of the arm. [3] In the slow-up curve used when moving from point P_i to point P_i_+_1, the optimal slow-up time t that maximizes at least one of the torques applied to the motor of the first arm or the second arm within the allowable range.
Step of calculating _u_i, [4] The motor angular velocities of the first arm and the second arm are ω_1_i(t), ω_2_i(
t) and the optimal slow-up curve as {ω_1_i(t)
=C_1_i・f_u(t/■_u_i), 0≦t≦■
＿u_iω_2_i(t)=C_2_i・f_u(t/
■＿u_i), 0≦t≦■＿u_i}However, C_1_i,
C_2_i is the step of finding a constant; [5] In the slowdown curve used when moving from point P_i to point P_i_+_1, the optimum value is determined to maximize at least one of the torques applied to the motor of the first arm or the second arm within the allowable range. Slowdown time■＿
Step of calculating d_i, [6] Optimum slowdown curve {ω_1_i(t)=C_1_i・f_d[(t-t_
s)/■_d_i], 0≦t−t_s≦t_d_iω_
2_i(t)=C_2_i・f_d[(t-t_s)/
■_d_i], 0≦t−t_s≦■＿d_i}However, t_
In an angular acceleration control method configured by repeating the step of determining s as a slowdown start time, as a method for determining the load amount W_i that affects the torque of the motor, a predetermined load amount is determined in a numerical control device before an operation command is issued. A method of setting by command or sensor device,
Alternatively, in the Z-axis that is at the tip of the second arm and supports the load amount W_i, the current value I of the motor that drives this Z-axis
Torque curve T_(_I
_) and a load curve W_(_T_) showing the relationship between the torque T_Z applied to the Z-axis motor and the load amount W_i, [1] reading the current value I_Z flowing through the Z-axis motor, [2] the above step; The current value I_Z and the torque curve T_(_I_
) and calculating the torque T_Z applied to the Z-axis motor from [3] the torque T_Z and the load curve W_(_T_);
An angular acceleration control method comprising: calculating a load amount W_i from. （２）モータに速度指令を与え、回転可能な第１アーム
およびこの第１アームと連結した第２アームを作動させ
る角加速度制御方法であって前記第２アームの先端が、
点Ｐ＿ｉから点Ｐ＿ｉ＿＋＿１へ移動する際、スローア
ップ、スローダウンの角速度指令を与える方法として、
モータのトルクに影響を与える負荷量Ｗ＿ｉのとりうる
値を離散値化して、それを＠Ｗ＠＿（＿ｊ＿）、１≦ｊ
≦ｋ＿０とし、同じくモータのトルクに影響を与え、Ｐ
＿ｉ、Ｐ＿ｉ＿＋＿１のデータから決まる要素をａ＿１
、・・・・・・、ａ＿ｎとし、各々のとり得る値を離散
値化し、それを ▲数式、化学式、表等があります▼ 但しｊおよびｋ＿０、ｋ＿１、ｋ＿２、・・・・・・、
ｋ＿ｎは整数として表わし、これら＠Ｗ＠＿（＿ｊ＿）
、ａ＿１（ｊ）、ａ＿２（ｊ）、・・・・・・、ａ＿ｎ
（ｊ）を組み合わせて出来る全てのスローアップ曲線と
、スローダウン曲線において第１アームと第２アームの
モータの少なくとも一方にかかるトルクを許容範囲内で
最大にする最適スローアップ時間と最適スローダウン時
間を前もって求め、これらをメモリ記憶しておき、任意
のＷ＿ｉが与えられるとそれに最も近い＠Ｗ＠＿（＿ｊ
＿）を求め、同じく任意のＰ＿ｉとＰ＿ｉ＿＋＿１が与
えられるとＰ＿ｉとＰ＿ｉ＿＋＿１のデータから要素ａ
＿１、ａ＿２、・・・・・・、ａ＿ｎを計算し、それに
最も近いａ＿１（ｊ）、ａ＿２（ｊ）、・・・・・・、
ａ＿ｎ（ｊ）を求め、それらに対するスローアップ時間
とスローダウン時間をメモリから呼び出すことによって
負荷量Ｗ＿ｉとＰ＿ｉ、Ｐ＿ｉ＿＋＿１の位置決めデー
タを読込んでから、最適スローアップ曲線と最適スロー
ダウン曲線を求めるまでの時間を短縮する角加速度制御
方法において、モータのトルクに影響を与える前記負荷
量Ｗ＿ｉを求める方法として、数値制御装置において動
作命令以前に所定の負荷量を命令もしくはセンサー装置
にて設定する方法、あるいは第２アーム先端にあり前記
負荷量Ｗ＿ｉを支えるＺ軸において、このＺ軸を駆動す
るモータの電流値Ｉ＿ｚとトルＴ＿Ｚの関係を示すトル
ク曲線Ｔ＿（＿Ｉ＿）及びこのＺ軸モータにかかるトル
クＴ＿Ｚと前記負荷量Ｗ＿ｉとの関係を示す負荷曲線Ｗ
＿（＿Ｔ＿）を記憶し、 ［１］Ｚ軸モータに流れる電流値Ｉ＿Ｚを読みとるステ
ップ、 ［２］前記電流値Ｉ＿Ｚと前記トルク曲線Ｔ＿（＿Ｉ＿
）とからＺ軸モータにかかるトルクＴ＿Ｚを求めるステ
ップ、 ［３］前記トルクＴ＿Ｚと前記負荷曲線Ｗ＿（＿Ｔ＿）
から負荷量Ｗ＿ｉを求めるステップ、 にて構成された角加速度制御方法。(2) An angular acceleration control method in which a speed command is given to a motor to operate a rotatable first arm and a second arm connected to the first arm, the tip of the second arm being
When moving from point P_i to point P_i_+_1, a method of giving slow-up and slow-down angular velocity commands is as follows:
The possible values of the load amount W_i that affect the motor torque are converted into discrete values, and they are expressed as @W@_(_j_), 1≦j
≦k_0, which also affects the motor torque, P
The element determined from the data of _i, P_i_+_1 is a_1
, ......, a_n, and convert each possible value into a discrete value, and convert it into ▲ mathematical formulas, chemical formulas, tables, etc. ▼ However, j and k_0, k_1, k_2, ......
k_n is expressed as an integer, and these @W@_(_j_)
, a_1(j), a_2(j), ......, a_n
The optimum slow-up time and optimum slow-down time that maximize the torque applied to at least one of the motors of the first arm and the second arm within the allowable range in all slow-up curves and slow-down curves created by combining (j). are calculated in advance, stored in memory, and given any W_i, the nearest @W@_(_j
), and given arbitrary P_i and P_i_+_1, element a is calculated from the data of P_i and P_i_+_1.
Calculate _1, a_2, ......, a_n, and find the closest a_1(j), a_2(j), ......
a_n(j), and read the positioning data of the loads W_i, P_i, and P_i_+_1 by calling the slow-up time and slow-down time for them from the memory, and then calculating the optimal slow-up curve and the optimal slow-down curve. In the angular acceleration control method for reducing time, the method of determining the load amount W_i that affects the torque of the motor is a method of setting a predetermined load amount by a command or a sensor device before an operation command in a numerical control device, or In the Z-axis located at the tip of the second arm and supporting the load W_i, a torque curve T_(_I_) showing the relationship between the current value I_z of the motor driving this Z-axis and the torque T_Z, and the torque T_Z applied to this Z-axis motor. Load curve W showing the relationship with the load amount W_i
[1] Reading the current value I_Z flowing through the Z-axis motor; [2] The step of memorizing the current value I_Z and the torque curve T_(_I_
) and calculating the torque T_Z applied to the Z-axis motor from [3] the torque T_Z and the load curve W_(_T_);
An angular acceleration control method comprising: calculating a load amount W_i from.