JPH01119809A - Robot locus control method - Google Patents

Abstract

PURPOSE:To provide a stop point on a locus and to secure the locus at the time of restarting a movement by stopping an arm finger by switching to a plan at the time of deceleration, when a stop command signal has been inputted from the outside, at the time of controlling the arm finger. CONSTITUTION:A command of a moving path, etc., of an arm finger of a robot is interpreted by an interpreter 1, and when an optimum speed plan is drawn up by a planner 2, an interpolation point on a locus is calculated successively by an interpolator 6. Subsequently, based on a target position signal, a control signal is outputted by a feedback loop part 3, and a motion of the arm finger is controlled by a driver part 4 and a mechanism part 5. When a stop signal from the outside is inputted, a speed plane for stopping immediately is replanned so as to be switched to that of the time of deceleration, switched to a deceleration line having the same inclination as a deceleration line of a decelerating part in the first speed plan, and by this interpolation processing, the robot is stopped, while continuing the generation of an interpolation point.

Description

【発明の詳細な説明】 （産業上の利用分野） 本発明は、速度計画に基づいたロボットの軌跡制御方法
に係り、特にロボットのアーム手先の非常停止時におけ
る軌跡の確保対策に関する。DETAILED DESCRIPTION OF THE INVENTION (Industrial Application Field) The present invention relates to a method for controlling the trajectory of a robot based on a speed plan, and particularly to a method for securing a trajectory during an emergency stop of a robot arm.

（従来の技術） 従来より、ロボットの軌跡制御方法において、アーム手
先が基本計画に基づく軌跡上の補間点の生成を行いなが
ら経路を移動中に、例えば装置の故障、教示等のために
外部からの停止指令信号により停止する必要が生じた場
合、直ちに軌跡」二の補間点の生成を中止し、その後の
アーム手先の減速・停止はモータ等駆動機構の制御系に
委ねられるものが一般的な技術として知られている。(Prior Art) Conventionally, in a robot trajectory control method, while an arm hand is moving along a path while generating interpolation points on a trajectory based on a basic plan, an external device has been used to When it becomes necessary to stop due to a stop command signal, the generation of interpolation points for the second trajectory is generally stopped immediately, and the subsequent deceleration and stopping of the arm end is left to the control system of the drive mechanism such as the motor. known as technology.

（発明が解決しようとする問題点） しかしながら、上記従来の方法による場合、ロボットの
各関節の惰性等があるため、そのまま停止すると、オー
バーシュートを生じて軌跡から外れた場所で停止するこ
とになる。そして、停止した位置が軌跡上から外れてい
るために、故障等が修復されて再スタートする際、その
ままでは制御のための指令を入力できず、いったん軌跡
上又は出発地点に戻ってから再度速度計画を設定しなけ
ればならないという問題があった。(Problem to be Solved by the Invention) However, in the conventional method described above, due to the inertia of each joint of the robot, if the robot stops as it is, it will overshoot and stop at a location off the trajectory. . Since the stopped position is off the trajectory, when the failure etc. is repaired and restarted, commands for control cannot be input as is, and the speed is restarted after returning to the trajectory or starting point. The problem was that a plan had to be set.

本発明は斯かる点に鑑みてなされたものであり、その目
的は、ロボットのアーム手先が移動中に外部から停止信
号を受けた場合、本来の速度計画に沿った軌跡上で停止
させることにより、移動再開時の軌跡を確保することに
ある。The present invention has been made in view of the above, and its purpose is to stop the robot arm on a trajectory that follows the original speed plan when it receives a stop signal from the outside while it is moving. The goal is to ensure a trajectory when resuming movement.

（問題点を解決するための手段） 上記目的を達成するため本発明の解決手段は、ロボット
のアーム手先が速度計画に基づいた軌跡上の補間点を生
成しながら２点（Ａ）、　　（Ｂ）間の経路（Ｃ）を移
動するように制御するロボットの軌跡制御方法を前提と
する。(Means for Solving the Problems) In order to achieve the above object, the solving means of the present invention provides two points (A), (B ) is assumed to be a trajectory control method for a robot that is controlled to move along a path (C) between ).

そして、上記経路（Ｃ）上の地点（Ｅ）で外部から上記
速度計画にはない停止信号が出力されたときに、その停
止信号を検出すると、そのときの速度計画を減速時の速
度計画に切換え、この切換えられた再計画に応じて軌跡
上の補間点の生成を続行しながら上記アーム手先を停止
させるようにする。When a stop signal that is not included in the speed plan is output from the outside at point (E) on the route (C), when that stop signal is detected, the speed plan at that time is changed to the speed plan for deceleration. The arm tip is stopped while continuing to generate interpolation points on the trajectory in accordance with the switched replanning.

（作用） 以上の方法により、本発明では、２つの地点（Ａ）、（
Ｂ）間の経路（Ｃ）上を基本計画に基づく軌跡上の補間
点が生成され、該補間点にアーム手先が一致するように
ロボットが制御される。(Function) By the above method, in the present invention, two points (A), (
An interpolation point is generated on the path (C) between B) and the trajectory based on the basic plan, and the robot is controlled so that the arm tip coincides with the interpolation point.

そして、上記経路（Ｃ）の途中の地点（Ｅ）で外部から
非常停止等による停止信号が出力されると、基本計画が
減速時の速度計画に切換えられ、ロボットが停止するま
でこの切換えられた速度計画に基づく軌跡上の補間点の
生成が行われる。When a stop signal such as an emergency stop is output from the outside at a point (E) in the middle of the above route (C), the basic plan is switched to a speed plan for deceleration, and this switch continues until the robot stops. Interpolation points on the trajectory are generated based on the velocity plan.

よって、オーバーシュートを抑制して軌跡」二でロボッ
トのアーム手先を停止させることができ、移動再開時の
軌跡を確保することができる。Therefore, it is possible to suppress overshoot and stop the arm end of the robot at the trajectory "2", and it is possible to secure the trajectory when the movement is resumed.

（実施例） 以下、本発明の実施例について、図面に基づき説明する
。(Example) Hereinafter, an example of the present invention will be described based on the drawings.

第２図は、本発明を適用したロボット制御装置の信号系
を示し、（１）は入力装置（図示せず）からの命令を翻
訳してその解釈どおりに実行するインタプリタ、（２）
は該インタプリタ（１）からの命令に応じて最適なロボ
ットの運動の計画としての速度計画を立案するプランナ
、（６）は該プランナ（２）で立案された速度計画に基
づきロボットのアーム手先（図示せず）の移動すべき軌
跡上の補間点を逐次算出するインタボレータ、（３）は
該インタボレータ（６）で算出された補間点についての
目標位置信号とロボットのアーム手先の実際の位置とを
比較してロボットの運動をフィードバック制御するため
のフィードバックループ部、（４）は該フィードバック
ループ部（３）からの制御信号を受けてロボットのアー
ムを駆動するためのパルス信号を出力するドライバ部、
（５）は該ドライバ部（４）の出力に応じて運動するロ
ボットの機構部である。FIG. 2 shows the signal system of a robot control device to which the present invention is applied, in which (1) is an interpreter that translates commands from an input device (not shown) and executes them as interpreted; (2)
is a planner that creates a velocity plan as an optimal robot motion plan in response to instructions from the interpreter (1); An intavolator (3) that sequentially calculates interpolation points on the locus to be moved (not shown) outputs a target position signal for the interpolation points calculated by the intavolator (6) and the actual position of the robot's arm tip. a feedback loop section for performing feedback control of the robot's motion by comparison; (4) a driver section that receives the control signal from the feedback loop section (3) and outputs a pulse signal for driving the arm of the robot;
(5) is a mechanism section of the robot that moves according to the output of the driver section (4).

そして、上記フィードバックループ部（３）において、
（７）は上記プランナ（２）からの速度計画に基づく目
標位置信号と後述の補償器（１５）からのロボットのア
ーム手先の位置信号との代数差を演算する第１加え合せ
点、（８）は該第１加え合せ点（７）の演算結果に位置
ゲインに１を乗じる第１ループゲイン器、（９）は該第
１ループゲイン器（８）の乗算結果と補償器（１５）か
らのロボットのアーム手先の速度信号との代数差を演算
する第２加え合せ点、（１０）は該第２加え合せ点（９
）の演算結果に速度ゲインに２を乗じてロボットのアー
ムを駆動するためのトルク信号を出力する第２ループゲ
イン器である。また、（１２）はロボットのアーム手先
の位置信号を受けて実際の速度を演算する第１微分要素
、（１３）は該第１微分要素で演算されたロボットのア
ーム手先の速度をさらに微分して加速度を演算する第２
微分要素、（１４）は」−２ロボットのアーム手先の位
置信号と、第１微分要素（１２）で演算された速度と、
第２微分要素（１３）で演算された加速度とを加算して
いわゆる速度のＰＩＤ成分を演算する第４加え合せ点、
（１５）は該第４加え合せ点（１４）からのＰＩＤ信号
のゲインを補償するための補償器である。Then, in the feedback loop section (3),
(7) is the first addition point that calculates the algebraic difference between the target position signal based on the speed plan from the planner (2) and the position signal of the robot arm tip from the compensator (15), which will be described later; ) is the first loop gainer that multiplies the position gain by 1 to the calculation result of the first addition point (7), and (9) is the result of the multiplication of the first loop gainer (8) and the compensator (15). The second summing point (10) is the second summing point (9) that calculates the algebraic difference between the speed signal of the robot's arm tip and the second summing point (9).
) is multiplied by the velocity gain by 2 to output a torque signal for driving the arm of the robot. In addition, (12) is a first differential element that calculates the actual speed by receiving a position signal of the robot's arm tip, and (13) further differentiates the speed of the robot arm tip that is calculated by the first differential element. The second step calculates the acceleration using
The differential element (14) is the position signal of the arm tip of the ``-2 robot, the velocity calculated by the first differential element (12),
a fourth addition point for calculating the so-called PID component of velocity by adding the acceleration calculated by the second differential element (13);
(15) is a compensator for compensating the gain of the PID signal from the fourth summing point (14).

次に、」二記ドライバ部（４）において、（１７）は」
−記第２加え合せ点（１０）からのトルク信号と、」二
記機構部（５）を駆動するための電流のフィードバック
信号との代数差を演算して位相遅れを補償する第３加え
合せ点、（１８）は該第３加え合せ点（１７）からのト
ルク信号をパルス信号に変換していわゆるＰＷＭ信号を
出力する信号変換器である。Next, in "2 driver section (4), (17) is"
- A third addition that compensates for the phase delay by calculating the algebraic difference between the torque signal from the second addition point (10) and the current feedback signal for driving the mechanism section (5). Point (18) is a signal converter that converts the torque signal from the third summing point (17) into a pulse signal and outputs a so-called PWM signal.

そして、上記機構部（５）において、（１９）は上記信
号変換器（１８）からのＰＷＭ信号を増幅するためのパ
ワーアンプ、（２０）は該パワーアンプ（１９）で増幅
されたＰＷＭ信号に応じて回転するサーボモータ、（２
１）は該サーボモータ（２０）により駆動される関節ダ
イナミクスとしての積分要素である。In the mechanism section (5), (19) is a power amplifier for amplifying the PWM signal from the signal converter (18), and (20) is for amplifying the PWM signal from the power amplifier (19). A servo motor that rotates according to the
1) is an integral element as joint dynamics driven by the servo motor (20).

すなわち、ロボットのアーム手先の移動経路等の指令は
上記インタプリタ（１）で翻訳され、その翻訳された指
令に基づいてプランナ（２）で最適な速度計画が立案さ
れると、該速度計画に応じてインタボレータ（６）で軌
跡上の補間点つまり目標位置信号が逐次算出され、該目
標位置信号に基づきフィードバックループ部（３）で制
御信号が出力され、次に、ドライバ部（４）でＰＷＭ信
号に変換されて機構部（５）が駆動され、ロボットのア
ーム手先の運動が制御されるようになされている。That is, commands such as the movement path of the robot's arm tip are translated by the interpreter (1), and when an optimal speed plan is drawn up by the planner (2) based on the translated commands, The interpolation points on the trajectory, that is, the target position signal, are sequentially calculated by the inverter (6), a control signal is output by the feedback loop unit (3) based on the target position signal, and then the PWM signal is output by the driver unit (4). This is converted into a mechanical unit (5), and the movement of the arm end of the robot is controlled.

次に、上記プランナ（２）における速度計画の設定方法
について説明する。　　　□ 第１図に示すように、ロボットのアーム手先が、地点（
Ａ）を出発地点とし地点（Ｂ）を目標地点とする曲線状
の経路（Ｃ）上を移動する場合を例にとる。上記経路（
Ｃ）における速度計画は、第３図に示すように、速度が
リニアに増大する加速部（Ｃ＋）（図中■−１の部分）
と、速度が一定となる定速部（Ｃ２）（図中■−２の部
分）と、速度がリニアに減少する減速部（Ｃａ）（図中
■−３の部分）とからなる台形状の速度線図で一般的に
表される。Next, a method of setting a speed plan in the planner (2) will be explained. □ As shown in Figure 1, the arm tip of the robot moves to a point (
Let us take as an example the case of moving on a curved route (C) with A) as the starting point and point (B) as the destination point. The above route (
As shown in Figure 3, the speed plan in C) is an acceleration section (C+) where the speed increases linearly (part -1 in the figure).
, a trapezoidal shape consisting of a constant speed section (C2) where the speed is constant (section ■-2 in the figure) and a deceleration section (Ca) where the speed decreases linearly (section ■-3 in the figure). It is commonly represented by a velocity diagram.

上記目標軌跡である経路（Ｃ）上における内挿処理につ
いて説明する。上記のような速度計画に基づく各部（Ｃ
１）〜（Ｃ３）における補間点は、一般的には、順に Ｐ＝ｆ＋　　（ｔ）＋ｓｌ）　　　　　　　　　　（１
）Ｐ−ｆ２　（ｔ）＋ｓｐ　　　　　　　　　　（２）
Ｐ−１３（ｔ）＋Ｓｐ　　　　　　　　　　（３）（た
だし、ｆ＋　　（ｔ）〜ｆ３　（ｔ）は区間固有の関数
、ｓｐは始点の座標である）のように表わされる。Interpolation processing on route (C), which is the target trajectory, will be explained. Each part (C
In general, the interpolation points in 1) to (C3) are as follows: P=f+ (t)+sl) (1
)P-f2 (t)+sp (2)
It is expressed as P-13(t)+Sp (3) (where f+(t) to f3(t) are functions specific to the section, and sp is the coordinate of the starting point).

そして、上記２点（Ａ）、（Ｂ）間の経路（Ｃ）に沿っ
た移動を表わす各点つまり補間点の時間に対する変化を
それまでに進んだ距離を経路（Ｃ）全体の距離で除した
いわゆる媒介変数ｐｅｔ（パーセント）で表わすと、該
媒介変数１）ｅｔは、第４図に示すように、上記速度計
画線図に基づいて零から１まで変化するものとなる。す
なわち、加速部（Ｃ１）に対応する部分ではその変化率
を次第に増加させる正の２次関数となり、定速部（Ｃ２
）に対応する部分では、変化率を一定とする直線となり
、減速部（Ｃ３）に対応する部分では変化率が次第に減
少する負の２次関数となる。Then, the change over time of each point representing the movement along the route (C) between the above two points (A) and (B), that is, the interpolation point, is divided by the distance traveled so far by the entire distance of the route (C). When expressed as a so-called parametric variable pet (percentage), the parametric variable 1)et changes from zero to 1 based on the velocity planning diagram, as shown in FIG. In other words, the part corresponding to the acceleration part (C1) becomes a positive quadratic function whose rate of change gradually increases, and the part corresponding to the constant speed part (C2
) is a straight line with a constant rate of change, and a portion corresponding to the deceleration portion (C3) is a negative quadratic function where the rate of change gradually decreases.

また、ロボットの制御モードの種類により、上記補間点
を生成するための内挿処理時に使用する関数は異なり、
以下の３通りについて切換可能になされている。Also, depending on the type of control mode of the robot, the function used during the interpolation process to generate the above interpolation points differs.
It is possible to switch between the following three ways.

■、ジヨイントモードで行う場合 この場合、始点（Ａ）の位置ベクトルとして始点（Ａ）
での各関節の角度Ｊｌ　　（ｉ）を、始点（Ａ）から終
点（Ｂ）へのペクト４ルとして各関節の移動量Ｄｊ　　
（ｉ）を用いる。上記媒介変数１）ｅｔを用いて、ロボ
ットの各関節の位置を表わすと、 Ｊｎ　　（ｉ）　＝Ｊｎｌ　（ｉ）　＋ｐｃｔ−Ｄｊｎ
　（ｉ）　（４）（ただし、１≦ｉ≦軸数）となる。■, When performing in joint mode In this case, the position vector of the starting point (A) is the starting point (A).
The angle Jl of each joint at
Use (i). Using the above parameter 1) et to represent the position of each joint of the robot, Jn (i) = Jnl (i) + pct - Djn
(i) (4) (1≦i≦number of axes).

■、直線モードで行う場合 この場合には、上記Ｊｌ　　（ｉ）の代わりに始点（Ａ
）の座標（Ｘ、ｙ、ｚ）および姿勢の各パラメータＰｓ
ｉ（ｉ）を、Ｄｊｎ（ｉ）（７）代わりに各パラメータ
の移動量Ｄｐｓ（ｉ）を用いる。■ When performing in straight line mode In this case, instead of Jl (i) above, start point (A
) coordinates (X, y, z) and posture parameters Ps
i(i) and Djn(i)(7), the movement amount Dps(i) of each parameter is used.

そして、上記媒介変数ｐｅｔを用いて目標直線上の内挿
点を表わすと、 Ｐｓ　　（ｉ）−Ｐｓｉ　（ｉ）＋ｐｃｔ−Ｄｐｓ　（
ｉ）（５）（ただし、１≦ｉ≦パラメータ数）となる。Then, when the interpolation point on the target straight line is expressed using the parametric variable pet, Ps (i) - Psi (i) + pct - Dps (
i) (5) (1≦i≦number of parameters).

■０円円弧間モードの場合 この場合、第５図に示すように、中心と始点（Ａ）とを
結ぶ線分と、中心と内挿点とを結ぶ線分との間の角度θ
をｐｅｔに合わせて変化させることにより、円弧軌跡上
で滑らかな加減速を行う。■ In the case of 0-circle arc mode In this case, as shown in Figure 5, the angle θ between the line segment connecting the center and the starting point (A) and the line segment connecting the center and the interpolation point
By changing according to the pet, smooth acceleration and deceleration is performed on the circular arc trajectory.

そして、始点（Ａ）の位置ベクトル１ｐｓ（ｉ）と、目
標点（Ｂ）までの角度の移動ｉＤθを用いると、内挿点
ＰＩ）Ｓ（ｉ）は Ｐｐｓ（ｉ）　− ぼｐｓ＋［６ｉ　ＩＩ　（ｃｏｓ　（ｐｃｔＩＩＤθ）
　−１１−ｃｉ　　−５ｉｎ　　　（ｐｃｔ　−Ｄ　θ
）　コ　−ｒ　　　　　　　　　（６）（ただし、ｉ＝
ｘ、ｙ、ｚ、５は中心から始点（Ａ）へ向う単位ベクト
ル、σは円弧の面内で５に直交する単位ベクトルである
）となる。Then, using the position vector 1ps(i) of the starting point (A) and the angular movement iDθ to the target point (B), the interpolation point PI)S(i) is Pps(i) − ps+[6i II (cos (pctIIDθ)
-11-ci -5in (pct -D θ
) Cor −r (6) (where i=
x, y, z, 5 are unit vectors directed from the center to the starting point (A), and σ is a unit vector perpendicular to 5 within the plane of the circular arc.

また、姿勢については、直線モードと同様の（５）式で
内挿を行う。Further, regarding the posture, interpolation is performed using equation (5) similar to the linear mode.

すなわち、各制御モードに応じて上記（４）〜（６）式
を選択し、上記（１）〜（３）式に基づき経路（Ｃ）の
各部（Ｃ１）〜（Ｃ３）における軌跡上の内挿点を算出
することにより、補間点を生成してロボットのアーム手
先が基本計画に基づく軌跡上を移動するようになされて
いる。That is, the above equations (4) to (6) are selected according to each control mode, and the inside of the trajectory at each part (C1) to (C3) of the route (C) is determined based on the above equations (1) to (3). By calculating the interpolation point, an interpolation point is generated so that the arm tip of the robot moves on a trajectory based on the basic plan.

次に、上記のような速度計画の再設定に基づいたロボッ
トのアーム手先の軌跡制御について、第６図のフローチ
ャートに基づき説明する。Next, trajectory control of the robot's arm end based on the resetting of the speed plan as described above will be explained based on the flowchart of FIG. 6.

まず、ステップＳ１で最初に立案された速度計画に基づ
いて内挿点を算出しく補間点の生成）、ロボットのアー
ム手先の移動する軌跡が上記算出された内挿点に一致す
るように上記サーボモータ（２０）を駆動してアーム手
先の制御を行う。そして、ステップＳ２で外部からの停
止信号が入力されたか否かを判別し、その結果がＮｏで
あれば、通常の制御状態であると判断して上記ステップ
Ｓ１を繰り返す。一方、外部からの停止信号が出力され
、ステップＳ２における判別がＹＥＳになる（地点（Ｅ
））と、非常停止時であると判断してステップＳ３に進
み、速度計画を減速時の速度計画に切換えるように再計
画する。そして、ステップＳ４で、アーム手先が停止す
るまで、上記再計画に基づく内挿点の算出（補間点の生
成）とアーム手先の制御とを行う。First, an interpolation point is calculated based on the velocity plan first drawn up in step S1 (generation of interpolation point), and the servo is adjusted so that the locus of movement of the robot's arm tip matches the interpolation point calculated above. The arm hand is controlled by driving the motor (20). Then, in step S2, it is determined whether or not a stop signal has been input from the outside. If the result is No, it is determined that the control state is normal, and step S1 is repeated. On the other hand, a stop signal is output from the outside, and the determination in step S2 becomes YES (point (E
)), it is determined that an emergency stop is occurring, and the process proceeds to step S3, where the speed plan is re-planned to switch to a speed plan for deceleration. Then, in step S4, calculation of interpolation points (generation of interpolation points) based on the replanning and control of the arm end are performed until the arm end stops.

したがって、上記実施例では、例えば第７図に示すよう
に、速度計画における定速部（Ｃ２）（図中■−２）途
中の点（Ｅ）で外部からの停止信号を受けた場合、直ち
に停止するための速度計画を減速時のものに切換えるよ
うに再計画し、最初の速度計画における減速部（Ｃ３）
の減速線（第３図■−３）と同じ傾斜を持った減速線（
第７図」二図■−３）に切換え、該減速線に沿って、つ
まり上記一般式（３）に基づく内挿処理によって補間点
の生成を持続しながら停止する。そのとき、媒介変数ｐ
ｅｔは同下図に示すように、その変化率が次第に減少す
る二次関数に沿って変化しながら軌跡」二で停止するこ
とになる。Therefore, in the above embodiment, as shown in FIG. 7, when an external stop signal is received at a point (E) in the middle of the constant speed section (C2) (■-2 in the figure) in the speed plan, immediately Replanning the speed plan for stopping to the one for deceleration, and changing the speed at the deceleration section (C3) in the first speed plan.
A deceleration line (Fig. 3 ■-3) with the same slope as the deceleration line (Fig. 3 ■-3)
7" and 2-3), and stops along the deceleration line, that is, while continuing to generate interpolation points by interpolation processing based on the above general formula (3). Then, the parametric variable p
As shown in the figure below, et changes along a quadratic function whose rate of change gradually decreases until it stops at trajectory 2.

また、第８図上図に示すように、加速部（図中■−１）
途中の点（Ｆ）で停止信号を受けた場合にも、速度計画
を基本速度計画における減速部（Ｃ３）の減速線（第３
図■−３）と同じ傾斜を持った減速線（第８図上図■−
３）に切換え、該再計画に基づく内挿処理によって補間
点の生成を行いながら停止し、媒介変数ｐｅｔは同下図
に示すように、その変化率が次第に減少する二次関数に
沿って変化しながら軌跡上で停止することになる。In addition, as shown in the upper diagram of Fig. 8, the acceleration part (■-1 in the figure)
Even if a stop signal is received at point (F) midway, the speed plan is changed to the deceleration line (third
Deceleration line with the same slope as Figure ■-3) (Figure 8 upper figure ■-
3), the process stops while generating interpolation points through interpolation processing based on the replanning, and the parametric variable pet changes along a quadratic function whose rate of change gradually decreases, as shown in the figure below. It will stop on the trajectory.

よって、従来のように停止点が軌跡上からずれることが
なく、オーバルシュートを抑制して軌跡上でアーム手先
を停止させることができ、移動再開時の軌跡を有効に確
保することができるのである。Therefore, the stopping point does not deviate from the trajectory as in the past, and it is possible to suppress oval shoots and stop the arm tip on the trajectory, effectively securing the trajectory when resuming movement. .

（発明の効果） 以上説明したように、本発明の軌跡制御方法によれば、
ロボットのアーム手先が基本速度計画に基づき所定の経
路を移動する軌跡の制御において、外部からの停止指令
信号が入力されたときにも、アーム手先が停止するまで
再計画に基づいた軌跡の内挿処理を行うようにしたので
、オーバーシュートを抑制して軌跡上でアーム手先を停
止させることができ、移動再開時の軌跡を確保すること
ができる。(Effects of the Invention) As explained above, according to the trajectory control method of the present invention,
When controlling the trajectory of a robot's arm tip moving along a predetermined path based on a basic speed plan, even when an external stop command signal is input, the trajectory is interpolated based on the re-plan until the arm tip stops. Since this process is performed, overshoot can be suppressed and the arm tip can be stopped on the trajectory, and the trajectory when the movement is resumed can be secured.

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

図面は本発明の実施例を示し、第１図はロボットのアー
ム手先の移動する経路図、第２図はロボットの制御装置
の構成を示すブロック線図、第３図は基本速度計画に基
づく経路上の速度線図、第４図は第３図の速度線図に対
応する媒介変数の変化特性図、第５図は円弧補間モード
におけるベクトルの説明図、第６図は軌跡制御方法を示
すフローチャート図、第７図上下図はそれぞれ定速部に
おける非常停止時の速度および媒介変数の変化を示す図
、第８図上下図は加速部における上記第７図上下図に相
当する図である。 （Ａ）・・・出発地点、（Ｂ）・・・到達地点、（Ｃ）
・・・経路、（Ｅ）・・・非常停止開始地点。 特　許　出　願　人　　ダイキン工業株式会社代　　理
　　人　　　　弁理士　前　１）　弘第３図 ○ 時間を 第４図 峙面ｔThe drawings show an embodiment of the present invention, in which Fig. 1 is a path diagram of the movement of the robot's arm end, Fig. 2 is a block diagram showing the configuration of the robot's control device, and Fig. 3 is a path based on the basic speed plan. The velocity diagram above, Figure 4 is a change characteristic diagram of parametric variables corresponding to the velocity diagram in Figure 3, Figure 5 is an explanatory diagram of vectors in circular interpolation mode, and Figure 6 is a flowchart showing the trajectory control method. The upper and lower diagrams in FIG. 7 are diagrams respectively showing changes in speed and parameter variables during an emergency stop in the constant speed section, and the upper and lower diagrams in FIG. 8 are diagrams corresponding to the upper and lower diagrams in FIG. 7 in the acceleration section. (A)...Departure point, (B)...Destination point, (C)
... Route, (E) ... Emergency stop start point. Patent Applicant Daikin Industries, Ltd. Representative Patent Attorney 1) Hiroshi Figure 3 ○ Time Figure 4 Facing t

Claims (1)

【特許請求の範囲】[Claims] （１）ロボットのアーム手先が速度計画に基づいた軌跡
上の補間点を生成しながら２点（Ａ）、（Ｂ）間の経路
（Ｃ）を移動するように制御するロボットの軌跡制御方
法において、上記経路（Ｃ）上の地点（Ｅ）で外部から
上記速度計画にはない停止信号が出力されたときに、そ
の停止信号を検出すると、そのときの速度計画を減速時
の速度計画に切換え、この切換えられた再計画に応じて
軌跡上の補間点の生成を続行しながら上記アーム手先を
停止させることを特徴とするロボットの軌跡制御方法。(1) In a robot trajectory control method in which the arm tip of the robot is controlled to move along a path (C) between two points (A) and (B) while generating interpolated points on the trajectory based on a velocity plan. , When a stop signal that is not included in the speed plan is output from the outside at point (E) on the route (C), when that stop signal is detected, the speed plan at that time is switched to the speed plan for deceleration. A method for controlling a trajectory of a robot, characterized in that the tip of the arm is stopped while continuing to generate interpolation points on the trajectory in accordance with the switched replanning.