JPH03213284A - Robot controller - Google Patents

Abstract

PURPOSE:To enable the determination of an orbit without depending on a velocity by generating a biquadratic Bezier curve on the basis of three points, a point prior to via distance from a via point, a via point and a point after via distance from the via point, while installing such a means as interpolates this besel curve with a quartic equation of time, in a via pattern orbit generating means. CONSTITUTION:A via pattern orbit of a robot manipulator 30 is determined by an interpolating means in an orbit pattern generating part 90 with a biquadratic Bezier curve based on three points, a point prior to via distance from a via point, a via point and a point after via distance from the via point, and this Bezier curve is interpolated by the quartic equation of time. With this constitution, a via starting point of the via pattern orbit in the robot manipulator 30 and a via finishing point are determined without depending on a designated operation speed, whereby the speed on the via orbit is varied in leaving the continuity of acceleration intact from the operating speed prior to the via to that after the via, and even in the case where designation of the traveling speed differs, it comes to go through on top of the same orbit at all times in consequence.

Description

[Detailed Description of the Invention] <Industrial Application Field> The present invention relates to a robot control device that moves a robot manipulator to one gage point or to a target point via a specified waypoint. In particular, it relates to improving operational performance.

<Prior Art> One of the devices filed by the applicant as a robot control device for moving a robot manipulator from a starting point to a finishing point via multiple transit points is as shown in Fig. β.
By comparing the deceleration distance at the specified speed before the transit section and the acceleration distance after the transit section, and setting the shorter one as the transit distance, the transit start point P is determined. and the route end point P2 are calculated, and these route start points P. There is one that moves on a curve determined by three points: a route end point P2 and a route position P1.

<Problems to be Solved by the Invention> However, with this conventional robot control device, there is a problem in that the locus of the robot manipulator during transit operations varies depending on the specified moving speed of the robot manipulator.

In other words, if a machine whose trajectory has been taught at a low speed is operated at a high speed, the trajectory becomes more inward, making it impossible to use the taught trajectory or causing a risk of contact with an obstacle.

The present invention has been made with attention to these points,
The purpose is to provide a robot control device that can determine a trajectory independent of speed.

<Means for Solving the Problems> The present invention to solve the above problems stores the current position of a robot manipulator and moves the robot manipulator to an externally specified target point via an externally specified way point. In a moving robot control device, means for storing the current position of the robot manipulator, means for receiving and storing the position of a via point designated from the outside, and means for receiving and storing the position of the target point designated from the outside. a means for calculating parameters such as travel distance and speed related to the movement of the robot manipulator; a means for generating an acceleration pattern trajectory for accelerating the robot manipulator; and a constant velocity pattern trajectory for moving the robot manipulator at a constant speed. A means for generating an acceleration pattern trajectory for decelerating the robot manipulator; A means for generating a transit pattern trajectory for moving the robot manipulator; and the elapsed time after the start of the operation and the distance traveled by the robot manipulator. and pattern selection means for selecting one of the four trajectory generation means based on the movement speed, means for calculating the position of the robot manipulator from the movement distance of the robot manipulator, and means for calculating the position of each joint from the position of the robot manipulator. The pattern selection means has means for calculating the shorter of the deceleration and stopping distance at the maximum operating speed and the acceleration distance after passing as the transit distance, and the transit pattern trajectory generating means calculates the transit distance from the transit point. It has means for generating a fourth-order Bezier curve based on three points: a previous point, a waypoint, and a point after the waypoint by a distance from the waypoint, and interpolating on the Bezier curve using a quartic equation of time, and starting the way. The point and the end point of the route are determined without depending on the specified motion speed, and the motion speed is changed from the motion speed before the transit to the motion speed after the transit while maintaining the continuity of acceleration on the transit trajectory. It is something.

In the robot control device of the present invention, the route pattern trajectory of the robot manipulator is a fourth-order Bezier curve based on three points: a point before the route point, the route point, and a point after the route point. determined, the Bezier curve is 4 times
Interpolated using the following formula.

As a result, the start point and end point of the robot manipulator's route pattern trajectory are determined without depending on the specified motion speed, and the speed on the route trajectory changes from the motion speed before the route to the motion speed after the route. It changes while maintaining continuity, and even if the specified movement speed is different, it will always travel on the same orbit.

<Example> Hereinafter, an example of the present invention will be described in detail with reference to the drawings.

FIG. 1 is a block diagram showing an embodiment of the present invention. In the figure, reference numeral 10 denotes a command position data receiver that receives target position data and transit position data via communication from an external device such as a host computer. is stored in the transit position register 22. Note that these target position register 21 and route position register 22 constitute a command recognition section 20. 30 is a multi-joint robot manipulator, for example, a 6-joint robot manipulator has shoulders 11 and l1.
Make 6 movements of wrist and arm: vertical rotation and twisting. 40
is a position detector attached to each joint of the robot manipulator 30, and includes, for example, an encoder that determines the rotation angle of the joint (i1F+) and a mark reading device that reads marks attached to each joint.

Reference numeral 50 denotes a current status recognition unit for grasping the current position of the robot manipulator 30, and is composed of a current position register 5.1 and a previous target position register 52. The current position register 51 stores, for example, the hand position calculated from the rotation angle of each joint.

60 is a target position register 21. This is a parameter calculator that calculates parameters during movement from data stored in the transit position register 22 and the previous target position register 52. The parameter calculator 60 uses a representative moving distance as a parameter during movement.

Parameters of acceleration/deceleration, acceleration distance and deceleration distance at the maximum operating speed, etc. are calculated and output to various data storage devices 70 and also to a generation pattern selector 80. The output signal of the generated pattern selector 80 is transmitted to the trajectory pattern generator 90.
added to. The trajectory pattern generator 90 includes a current position, an acceleration pattern generator 911, a constant velocity pattern generator 92, and a constant velocity pattern generator necessary for movement in each section of the target position and transit position. A deceleration pattern generator 93 and a transit pattern generator 94 are provided. The acceleration pattern generator 91 generates a trajectory pattern that accelerates from the current speed to a specified speed, the constant velocity pattern generator 92 generates a trajectory pattern that moves uniformly at a specified speed, and the deceleration pattern generator 93 generates a trajectory pattern that stops smoothly at the target position, and the route pattern generator 94 generates a trajectory pattern for performing the route operation by combining a deceleration operation before the route and an acceleration operation after the route near the route position. .

Each of these pattern generators 91 to 94 feeds back an end signal to the generated pattern selector 80 to prompt selection of the next pattern. Such trajectory pattern selection is repeated until the target position is reached.

The hand position calculation unit 100 calculates the position of the hand of the robot manipulator 30 based on movement distance data output from an acceleration pattern generator 91 , a constant velocity pattern generator 92 , and a deceleration pattern generator 93 . Joint position calculator 11
0 is added with the output signal of the hand position calculator 100 and the output signal of the route pattern generator 94, and performs calculations for converting the hand position into the positions of each joint. Each joint position signal calculated by the joint position calculator 110 is given to a drive circuit 120 that drives the robot manipulator 30 as a target position of the robot manipulator 30.

The operation of the device configured in this way will be explained.

FIG. 2 is a diagram showing the trajectory of the robot manipulator when one transit position is specified. In the figure, P is x
, y, and z, ■1 to v4 are designated speeds, and Pa-Pd and Pa' to Pd' are moving distances. The pre-given position is the current position Ps
, the transit position P3, and the target position PE. When generating a trajectory pattern from the current position Ps to the target position P8 via the vicinity of the transit position P, start the generation pattern selector 80 in sections I to ■, and select from the movement distance speed etc. at that point. One of the pattern generators 91 to 94 is selected to operate the robot manipulator 30.

■Operation of parameter calculator 60 during movement Given the current position P and transit position P1, the following 5
Perform seed operations.

A1 Representative travel distance D-I P, -PS B, Distance D to reach specified speed V +-+ -(1/2)(T-p + Ta-) V
V2 designated speed T,...: Acceleration time To = deceleration time C, distribution coefficient in x, y, z directions S1...1-
Buy one (PL-P s-) / DS +-1--(
P+-Ps-)/DS+-0--(P+
-P s-) / DD, coefficient K during acceleration/deceleration, p-(1/2) (V/T up) Kd, -
(1/2) (V/T,,) E, acceleration distance and deceleration distance when operating at maximum speed V M 1 M 1) D≧(1/2) (T-p+T+-)v,,,・・・・
Trapezoidal drive, -(1/2)T,,V,,.

Dd,,-(1/2) Tdfiv-,t2)D<
(1/2) (T u, 10 Ta-> v---triangular drive, -T,, D/(T,, +Td,) Da, -Ti,
, D/(D, , 10T, n) The results of each of the above calculations are stored in various data storage devices 70.

■ Movement distance P b of the acceleration pattern generator 91 from the motion generation pattern selector 80 to the present
, current speed V. and acceleration end time T. debt.

Once obtained, the moving distance p (t) is calculated according to the following equation.

P(t)=P, -Kup (t'/T, 2)+2K
up (t3/T, ,) 10Vct (0≦t≦T,) ■ Movement distance P1 of the constant velocity pattern generator 92 from the operation generation pattern selector 80 to the present.
Current speed V and acceleration end time TI! is given,
The moving distance p (t) is calculated according to the following formula.

P(L)-P, +V, t (0≦tsTg) ■ Movement distance P1 of the deceleration pattern generator 93 from the operation generation pattern selector 80 to the present.
Given the current speed V and acceleration end time TO, the moving distance MP(L) is calculated according to the following equation.

P (t) - P C ten KON (t' / To 2
)2KDN (t'/To) +vet(0≦t
≦To) ■ Current position P from the operation generation pattern selector 80 of the transit pattern generator 94. , via position P
1. Via end position fir2. Given the transit end time of 1 day and the current speed ve, the hand position P (t
) is calculated.

P(t)−(Po 2F+ +P2)t””2
(Po 2P+ +P2) t' s+2
(P+ −Pa)t′ +P.

Here, L' - ((VcTB /2L) 11(L/TR
) '-2((v, To /2L)-1t(t/T8
) '+(v c Te /2L)(t/ TB
)L ””l P o P t (0≦[≦TB) ■The movement distance P(O is given from the motion acceleration pattern generator 912, constant velocity pattern generator 92, and deceleration pattern generator 93 of the hand position calculator 100) Then, the hand position P(t) is calculated according to the following equation.

■Operation of generation pattern selector In FIG. 2, the current position Ps and the transit position P.

The operation from IPs to P2 when the target position tpi is specified will be explained step by step.

That is, the parameter calculator 60 provides the following for each section.

・Section P, →P1 D -NI V-NI Dial-N r K11ll
-NI K+1+-NI Sal -N* D up
-NI Dd++-N・Section P, -P.

D -Pr V-P+ Dial-F + KuP-P
+ Kd*-P+ S l5111-1'l D u
++-P+ D da-P where, D -o: Distance traveled V -O: Specified speed D l @ l -0''-p-0' Coefficient during acceleration to the distance traveled to reach -0 n -o 'Coefficient during deceleration S la+++-0' x, 511 Z-direction distribution coefficient Dup-D' Maximum speed ■ 41, acceleration distance D
a++-o: Maximum speed V□, deceleration distance generation pattern selector 80 at 8 o'clock selects each interval 1 to 8 as shown below according to these data given from parameter calculator 60.
(2) Select an appropriate pattern generator 91 to 93 for each step.

a) Section I P, -〇T, = (Vl Vo ) (T, -/V-N) P
, -P, +(1/2)(Vl +vo)T.

, the acceleration pattern generator 91 is selected.

Then, when the generation of the acceleration pattern ends, the process moves to section (3).

b) Section■ Maximum speed V11. Performs constant speed operation until the deceleration start point at .

1) DN P b > Da--N (with constant velocity section)
V, -V.

TE-(DNPb, Da--N)/
The constant velocity pattern generator 92 is selected as VlPCIIMP, +vI IIT6.

Then, when the generation of the constant velocity pattern ends, the process moves to section (3).

2) DN Pb≦Da-s (no constant velocity section) Pc-P
.

, move to section ■.

C) Section■ In order to avoid changing the transit trajectory depending on the specified speed, the travel distance DB at the time of red and white is set to Dlow 11n (D-N Pc, Due-p).
, the route start position P is always the same. , the movement is performed on a curve determined by way point P1° and way end position P2.

Therefore, the following processing is performed depending on the DaM-NI Due P relationship.

1) D, -N-P, ≦D up-P V2-V.

P, -P.

, move to section ■.

2) 11. -Pc>Du, -p First, the speed v2 at the start of the route is shall be. Therefore, When V1≦vP, P, -Pc V, -V.

TE −(D−NP, −Da)/V+P, −P,
+V, ・T9, the constant velocity pattern generator 92 is selected. and,
When the generation of the constant velocity pattern ends, the process moves to section (3).

When V,>v, d-(1/4)(Vl'-V,'/Km,-N
)+D[l, and further, if D-, -Pc-d>0, Pb""P.

eIwvIT! ! = (D-NPtd)/VIPc-
Pb +v, ・TI! , the constant velocity pattern generator 92 is selected. and,
When the generation of the constant velocity pattern ends, the process moves to section (3) again. On the other hand, when IIN-Pc-d≦0, V, -V.

■2″′vP Ta −(VI V2 /V−N) (T, 1fi
)Pd-Pc+(1/2)(VI+V2)T.

, the deceleration pattern generator 93 is selected. and,
When the generation of the deceleration pattern ends, the process moves to section ■.

d) Section ■ First, the speed v4 of the post-transit section is calculated.

Then, using the calculated speed v4, the route is terminated, VB -min (Vz, VB) shall be.

Furthermore, P o = P 3 + D B ・$ l *+++−
NP 2 = P 1 + D B ・$lsn+-
PTo ""4 ・DB / (V2 +V3)P,
- Select the transit pattern generator 94 as Dll.

Then, when the generation of the transit pattern ends, the process moves to section V.

e) In order to perform the operation after passing through section V, set the target position from IP to Ps.
Then, copy from P5 to P1. Furthermore, various coefficients 0
-Copy P to 0-N.

1) V, -V4 P, -P.

, move to section ■.

2) V, <V4 c-VB T-= (V4 VB ) (T,, /V-N)Pb
-P, ' + (1/2) (V4 +V3)
The acceleration pattern generator 91 is selected as Tu.

Then, when the generation of the acceleration pattern ends, the process moves to section (3).

f) Section■ Perform the same processing as in section ■, and move to section ■ after the processing.

g) Section ■ 1) D-, -P,'> (1/4) (V4'/K
DN-N)p, 'm pc ■ゎ-■4 T, -ID-N-Pc (1/4) (V4'/KDN-N)l/V4P
, =Ph' +V4 ·Tp, and the constant velocity pattern generator 92 is selected.

Then, when the generation of the constant velocity pattern ends, the process moves to section (3) again.

2) D-, -Pc'> (1/4) (V4'/K
DN-N) ■, -v4 Ta"= (V4/VN) Ta.

, the deceleration pattern generator 92 is selected.

Then, after the pattern generation ends, it stops.

It should be noted that the selection order of the pattern generators is not necessarily as shown in FIG. 2, but is based on the current position PS.

It may change depending on the relationship between the transit position PI and the target position PE, specified speed, acceleration/deceleration time, etc.

Further, although the above description has been about interpolation of the orthogonal coordinate system (x, y, z), the same applies to joint interpolation of a robot manipulator having n joints.

Further, the number of way points is not limited to one, and multiple way points can be specified.

<Effects of the Invention> As described in detail above, according to the present invention, it is possible to provide a robot control device that can determine a trajectory without depending on speed, and it becomes easy to teach a robot manipulator.

As a result, for example, even if a machine is taught a route at low speed and operated at high speed, the route will not become inward, making the taught route unusable, or causing contact with obstacles, unlike in the past. .

[Brief explanation of drawings]

FIG. 1 is a configuration block diagram showing an embodiment of the present invention, FIG. 2 is an explanatory diagram of the operation of FIG. 1, and FIG. 3 is an explanatory diagram of the operation of a conventional device. 10... Command-data receiver 20... Command recognizing unit 30... Robot manipulator 40... Position detector 50... Current status recognizing unit 6
0...Parameter calculator 70...Various data storage device 80...Generation pattern selector 90...Trajectory pattern generator 91...Acceleration pattern generator 92...Constant velocity pattern generator 93... ...Deceleration pattern generator 94...Transit pattern generator 100...Hand position calculator

Claims (1)

[Scope of Claims] A robot control device that stores the current position of a robot manipulator and moves the robot manipulator to an externally specified target point via an externally specified way point, comprising: means for receiving and storing the position of a via point specified from the outside; means for receiving and storing the position of the target point specified from the outside; and a movement distance related to the movement of the robot manipulator; means for calculating parameters such as speed, means for generating an acceleration pattern trajectory for accelerating the robot manipulator, means for generating a uniform velocity pattern trajectory for causing the robot manipulator to operate at a constant speed, and means for decelerating the robot manipulator. a means for generating a trajectory of an acceleration pattern; a means for generating a trajectory of a transit pattern that causes the robot manipulator to operate via the robot manipulator; a pattern selection means for selecting a robot manipulator; a means for calculating a position of the robot manipulator from a moving distance of the robot manipulator; and a means for calculating a position of each joint from a position of the robot manipulator; It has means for calculating the shorter of the deceleration and stopping distance at speed and the acceleration distance after transit as the transit distance, and the transit pattern trajectory generating means calculates the transit distance from a point before the transit point, the transit point, and the transit distance from the transit point. It has means for generating a fourth-order Bezier curve based on the three subsequent points and interpolating on the Bezier curve using a time-based fourth-order equation, so that the start point and the end point of the route reach a specified operating speed. A robot control device characterized by being determined without dependence and changing an operating speed on a transit trajectory from a motion speed before transit to a motion speed after transit while maintaining continuity of acceleration.