JPH0743605B2 - Method for generating circular arc trajectory of servo mechanism - Google Patents

Description

TECHNICAL FIELD The present invention relates to a circular arc trajectory generation method for a servo mechanism such as a robot or an NC gas cutting machine.

[Prior art] When attempting to move an end effector (welding torch, cutting torch, etc.) of a servo mechanism such as a robot or NC gas cutting machine along an arc trajectory, arc interpolation between given points is performed. Then, a method of determining a target position for each sampling time and performing servo control of each drive axis is currently generally used.

Here, as shown in FIG. 4, the end effector starts moving from position A along the arcuate trajectory defined by positions A, B, and C in space, and stops at memory C via position B. Think about the case.

In this case, the servo mechanism is driven by the procedure shown in the flow chart of FIG. 5, and the following procedures (1) to (3) are repeated at regular intervals (this is called sampling time) until the end point is reached. To do.

(1) Obtain the next target position on the circular arc trajectory as the coordinate value based on the Cartesian coordinate system (this is called trajectory generation).

(2) Find the position of the drive shaft (output shaft such as motor) that corresponds to the value found in (1) (this is called coordinate conversion).

(3) Positioning control is performed so that the drive axis becomes the position obtained in (2) (this is called servo control).

By the way, since this is a discrete time control, it is necessary to shorten the sampling time as much as possible in order not to impair the servo controllability (in the case of a normal articulated robot, it is desired to reduce it to about 30 msec or less). It is said.).
Also, when considering an articulated robot as the servo mechanism, the relational expression between the position of the Cartesian coordinate system reference end effector and the position of each drive axis becomes a complicated non-linear equation, and the Cartesian coordinate system reference end effector Coordinate conversion for obtaining the position of each drive axis corresponding to the position requires a considerable amount of calculation time. And, as for this coordinate conversion algorithm, only one method can be found according to the configuration of the drive shaft, and there is little room for improvement.

For this reason, it is currently strongly required to generate a trajectory for obtaining an end effector position on a given circular arc trajectory with reference to a Cartesian coordinate system in as short a calculation time as possible.

Here, a circular arc trajectory generation method which is currently generally used will be described with reference to FIG. Here, the positions are A, B,
It is the target position Pi on the circular arc trajectory defined by C. In other words, ▲ ▼ should be obtained. The procedure is shown below.

(A) Obtain the center position G of the circle defined by the positions A, B, C,
Find the vector ▲ ▼.

(B) Obtain the unit normal vector of the plane ABC.

(C) Regarding the vector, a 3 × 3 conversion matrix R (Δθ) that rotates an arbitrary vector by Δθ is obtained.

However, vers (Δθ) = 1−cos (Δθ).

(D) Regarding the vector, ▲ ▼ is obtained by rotating the vector ▲ ▼ by Δθ. ▲ ▼ = R (Δθ) ▲ ▼ _-1 ......
(3) ▲ ▼ ＝ ▼▼ ＋ ▲ ▼ …… Find the target position Pi from (4). However, the initial value of ▲ ▼ is ▲ ▼.

Here, in order to move at a constant linear velocity on the circular arc orbit, when the method of determining the central angle θ at a constant angular velocity is used, the line on the circular arc orbit is started at the position A and stopped at the position C. Since the speed becomes discontinuous, a large vibration is generated in the mechanism.

Therefore, in general, the positions A ⁺ and C ⁻ are determined by some standard, the angular velocity is accelerated from zero to a certain value s between the positions A and A ⁺ , and then the constant angular velocity s is increased.
In position C ^- moves to the position C ^- a between from to a position C so as to decelerate from s to zero, the method for determining the central angle θ is used. That is, Δθ (= θi−θ
i _-1 ) is not a constant value.

By the way, it is not practical to perform the above-mentioned steps (a) to (d) in advance, that is, to obtain and store all the target positions Pi in advance, because a huge storage capacity is required. Therefore, you only have to do it once (a), (b)
Is performed in advance, and steps (c) and (d) are performed at each sampling time when the mechanism is driven.

[Problems to be solved by the invention]

In the conventional arc trajectory generation method, since the above-mentioned steps (c) and (d) have different calculated values for each target position because Δθ is not a constant value, they are performed every sampling time when the servo mechanism is driven. However, the formula (2) in step (c) is sin, co
s, it is an arithmetic expression including many addition and subtraction multiplications, and it takes a considerable amount of calculation time. As a result, the servo sampling time cannot be shortened sufficiently, and the servo controllability is impaired.

The present invention has been made in view of such a situation, and shortens the calculation time for generating a trajectory, and also enables an arc mechanism of a servo mechanism that enables smooth coupling when coupling with another linear trajectory. The purpose is to provide a generation method.

[Means and Actions for Solving Problems]

The arc trajectory generation method for the servo mechanism according to the present invention includes a step of designating a teaching point (A, C) including a start point and an end point of the arc trajectory, and a predetermined time before and after the time at which each teaching point is located at that point. 2 points to be located at time ^{(a -, a +, C} -, C +)
And a step of determining a quadratic polynomial with respect to time by each teaching point and the above-mentioned two points located before and after each teaching point, and obtaining an acceleration / deceleration trajectory generated by this. It is used as the start and end points of an arc trajectory.

For example, to stop from the starting point of the circular arc track at the start moving or endpoint, it should be located just before the time a predetermined time than the time which is located at the starting point (A ^-) and the starting point (A) and to match the, Alternatively, the quadratic polynomial is determined by matching the end point (C) with the point (C ⁺ ) which should be located at a time later than the time located at the end point by a certain time. As a result, the starting point (A) and the point (A ⁺⁾ connecting the deceleration trajectory or point (C ^-) and end point (C)
The acceleration / deceleration trajectory connecting to and is represented by a straight line, and its calculation is simple and the calculation time is shorter than that of the conventional calculation method.

Also, start and end points of the arc orbit if each bonded to another straight track, the point to be located in a time earlier by a predetermined time than the time which is located at the starting point (A ^-) and located at the end point A point (C ⁺ ) that should be located at a time after a certain time after the time is obtained on each linear trajectory, and the acceleration / deceleration trajectory generated here is smoothly connected to another linear trajectory.
Embodiments Embodiments of the present invention will be described below with reference to the drawings.

Now, from position A to position C shown in FIG.
Let us consider the case of moving at a constant linear velocity on a circular orbit.

(a) Trajectory generation method from position A to position A ⁺ ; The time required for acceleration (2ta) is set, and the arc trajectory is reached when the vehicle moves linearly by ta at the constant linear velocity v _s given from position A. The position is defined as A ⁺ . In addition, the position that was ta before the scheduled time to start moving from position A is defined as A ⁻ . In this case, A ⁻ = A because it was stopped before the movement from the position A. This is shown in FIG.

Here, the coordinates xi, y of the intermediate position Pi from position A to position A ⁺
i and Zi are defined as follows.

Thus trajectory generation, while accelerating the linear orbits connecting the position A and the position A ⁺ from linear speed zero to the linear velocity v _s, moves time 2ta.

Here, the position A ^+, A ^- i.e. _{^{x A +, y A +,}} Z A +, x A -,
Since y _A ⁻ and Z _A ⁻ can be obtained in advance, only equation (5) needs to be calculated for each sampling time when the mechanism is driven.

Since this is a simple calculation compared to the conventional arc trajectory generation method, the trajectory can be generated in a short time.

The above is the method of generating an acceleration linear trajectory by a quadratic polynomial with respect to time.

By the way, as shown in FIG. 1, from the position A to the position A ^{+, a} deviation occurs from the original circular arc trajectory.

However, arc trajectory accuracy is required only for ultra-low speed operations such as welding, and the time required to move between AA ⁺
Since ta is short, the distance AA ⁺ is about a few mm. Therefore, there is no problematic deviation in the arcuate orbit having a radius of curvature that is practically used, and there is no problem in practical use.

(b) Trajectory generation method from position A ⁺ to position C ⁻ : Decide the time (2ta) required for deceleration, and on the circular orbit reached when moving linearly by ta at the constant linear velocity v _s given from position C Position C ^- is defined.

Here, the position C from the position A ^{+ -} defining up to the middle position Pi as follows.

(i) Position A ^+, B, C ^- find the center position G of the circle defined by, determining a vector ▲ ▼ ^+.

(ii) surface A ⁺ BC ^- obtaining a unit normal vector.

(iii) The increment value Δθ of the central angle for each sampling time is obtained from the given constant linear velocity v _s .

(iv) Regarding a vector, a 3 × 3 conversion matrix R (Δθ) that rotates an arbitrary vector by Δθ is obtained.

However, vers (Δθ) = 1−cos (Δθ).

(v) For the vector, change the vector ▲ ▼ _-1 to Δ
Since the vector rotated by θ is the vector ▲ ▼, the target position Pi is obtained from ▲ ▼ = R (Δθ) ▲ ▼ _-1 (8) ▲ ▼ = ▲ ▼ + ▲ ▼ (9). However, the initial value of ▲ ▼ is ▲ ▼ ⁺ .

When the trajectory is generated in this way, the trajectory moves at a constant linear velocity v _s on the circular arc trajectory connecting the position A ⁺ , the position B, and the position C ⁻ .

Here, since the positions A ⁺ , C ⁻ and Δθ (constant value) can be obtained in advance, steps (i) to (iv) can be performed in advance, and only step (v) can be performed at each sampling time when the mechanism is driven. You can go to

In other words, compared to the conventional arc trajectory generation method, the procedure
(C) That is, the calculation for obtaining the 3 × 3 conversion matrix for rotating the vector can be performed in advance. Therefore,
Since the amount of calculation performed for each sampling time is significantly reduced, the trajectory can be generated in a short time.

(c) Position C ^- Trajectory to the position C from; the position should have after only ta the scheduled time stopping at position C
Defined as C ⁺ . In this case, C ⁺ = because it continues to stop at position C
It is C.

Here, the position C ^- in the middle position Pi to the position C from the coordinates xi, y
i and Zi are defined as follows, as in (a).

xi = (x _c ⁺ -2x _c + x _c ⁻ ) hi ² ＋2 (x _c -x _c ⁻ ) hi ＋ x _c ⁻ yi ＝ (y _c ^＋ −2y _c + y _c ⁻ ) hi ² ＋2 (y _c -y _c ⁻ ) hi ＋ y _c ⁻ …… (1
0) Zi = (Z _c ⁺ -2Z _c + Z _c ⁻ ) hi ² +2 (Z _c -Z _c ⁻ ) hi + Z _c ⁻ Thus trajectory generation, position C ^- linear orbits connecting the a position C, while decelerating from the linear velocity v _s to linear velocity zero,
Move in 2ta time.

Since this method is a simple operation as compared with the conventional circular arc trajectory generation method as described in (a), the trajectory can be generated in a short time.

By the way, as shown in FIG. 3, if a point on another linear trajectory which should be before ta by the time to reach the position A is defined as the position A ⁻ , the arc trajectory generation method of the present invention By using as it is, it is possible to generate a trajectory that connects to another circular trajectory while smoothly changing the linear velocity from another linear trajectory.

On the contrary, if the point on the other straight line that should be ta after the scheduled time to reach the position C is defined as the position C ⁺ ,
By using the circular arc trajectory generation method of the present invention as it is, it is possible to generate a trajectory that is connected to another linear trajectory while smoothly changing the linear velocity from the circular arc trajectory.

In other words, it is not necessary to use any other special method for connecting the linear trajectory and the arc trajectory, which facilitates the generation of the trajectory generation software.

〔The invention's effect〕

As described above, according to the present invention, the trajectories of the starting point and the ending point of the circular arc trajectory and the vicinity thereof are obtained by the quadratic polynomial, so that the calculation time is shortened as compared with the conventional method, and therefore only that much. The servo sampling period can be shortened as compared with the conventional case. Further, when the circular arc trajectory is connected to another linear trajectory, the two are connected to generate a trajectory, so that a smooth connection is possible. In particular, since the trajectory is generated by a quadratic polynomial with respect to time, the velocity matches that of the connection point, and smooth connection is possible from this point as well.

[Brief description of drawings]

FIG. 1 is an explanatory diagram of an arc trajectory generation method according to an embodiment of the present invention, FIG. 2 is an explanatory diagram of a linear trajectory generation method by a quadratic polynomial with respect to time, and FIG. 3 is another embodiment of the present invention. In the explanatory view of the circular arc trajectory generation method, the case where the circular arc trajectory and the linear trajectory are connected is shown. FIG. 4 is an explanatory view of a circular arc trajectory, FIG. 5 is a flow chart showing a driving method of a servo mechanism, and FIG. 6 is an explanatory view of a conventional circular arc trajectory generation method.

─────────────────────────────────────────────────── ─── Continuation of the front page (51) Int.Cl. ⁶ Identification code Internal reference number FI Technical display location G05B 19/4103 9064-3H G05B 19/415 Z

Claims (3)

[Claims] 1. A teaching point (A,
Process and, each teaching point for the two points to be located in time before and after the predetermined time than the time located in the point that specifies ^{C) (A -, A +} , C -, obtains respective position of the C ⁺⁾ The method includes a step and a step of determining a quadratic polynomial with respect to time by each teaching point and the above-mentioned two points located before and after the teaching point, and obtaining a trajectory generated by the quadratic polynomial. A method for generating an arc trajectory of a servo mechanism, which is characterized in that. 2. A case of stopping the starts moving from the starting point of the circular arc track or endpoint, a point to be located in front of the time by a certain time than the time which is located at the starting point (A ^-) and the starting point (A)
And a point (C ⁺ ) that should be located at a time later than the time located at the end point by a certain time, and the end point (C) are matched to determine the quadratic polynomial. 1
A method for generating an arc trajectory of a servo mechanism described in the item. Wherein when the starting point and end point of the circular arc track are respectively connected to the other straight track are points to be located in front of the time by a certain time than the time which is located at the starting point (A ^-) and the end point The arc trajectory generation method for a servo mechanism according to claim 1, wherein points (C ⁺ ) that should be positioned at a time after a certain time after the time positioned at are located on the linear trajectory.