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

Description

Detailed Description of the Invention

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an arc trajectory generation method for a servo mechanism such as a robot or NC gas cutter. [Prior Art] At present, when moving an end effector (welding torch, cutting torch, etc.) of a servo mechanism such as a robot or NC (numerical control device) gas cutting machine along an arc trajectory, A method is practiced in which arc interpolation is performed between points to determine a target position for each sampling time and servo control of each drive axis is performed. Here, as shown in FIG. 4, the position in space (A),
Consider the work of starting to move the end effector from the position (A) along the circular arc trajectory defined by (B) and (C), and stopping at the position (C) via the position (B). In this case, the flow chart as shown in FIG. 5, that is, the next target position (Pi) on the circular arc trajectory at step (10)
Then, in step (11), the target position (Pi) is obtained as coordinate values based on the Cartesian coordinate system (this is referred to as trajectory generation). Next, in step (12), the target position of the drive shaft (output shaft such as motor) corresponding to the coordinate value obtained in step (11) is obtained (this is called coordinate conversion). Next, at step (13), positioning control is performed so that the target position obtained at the drive axis step (12) is reached (this is called servo control). These operations are repeated at regular intervals (referred to as sampling time) until the end point is reached (see steps (14) and (15)). [Problems to be Solved by the Invention] Since the above-described circular arc trajectory generation method uses discrete time control, it is necessary to shorten the sampling time as much as possible in order not to impair the servo controllability thereof. In the case of a normal articulated robot, a time of about 30 msec or less is desirable. Also, when considering an articulated robot as the servo mechanism, the relational expression between the position of the end effector based on the Cartesian coordinate system and the position of each drive axis is a complicated non-linear equation. For coordinate conversion to find the position of each drive axis corresponding to the position,
It 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.
Therefore, it is strongly required to generate a trajectory for obtaining the end effector position on the given circular arc trajectory with reference to the Cartesian coordinate system in the shortest possible calculation time. FIG. 6 is a principle explanatory diagram for explaining a circular arc trajectory generation method which is currently generally performed. It should be noted that what is desired to be calculated here is the target position (Pi) on the circular arc trajectory defined by the positions (A), (B), and (C). That is, the distance (▲ ▼) from the origin (0) of the reference rectangular coordinate system may be obtained. The procedure will be described below. The center position (G) of the circle defined by the positions (A), (B), and (C) is obtained, and then the vector (▲ ▼) is obtained. Find the unit normal vector () of the plane (A, B, C). With respect to the vector (), any vector (Δ
Rotate only θ). Then, a 3 × 3 conversion matrix R (Δ
θ) is calculated. kykxvers (Δθ) -kzsin (Δθ) ky ² vers (Δθ) + cos (Δθ) kykzvers (Δθ) + kxsin (Δθ) However, vers (Δθ) = 1−cos (Δθ). Regarding vector (), vector (▲
Rotating ▼) by an angle (Δθ)
▼) ▲ ▼ ＝ ▲ ▼ ＋ ▲ ▼ ... The target position (Pi) is obtained from [4]. However (▲ ▼)
The initial value of is (▲ ▼). Here, in order to move at a constant linear velocity on an arc orbit, when a method of determining the central angle (θ) at a constant angular velocity () is used, at the time of starting at position (A) and at position (C). Since the linear velocity on the circular arc orbit becomes discontinuous at the time of stopping, a large vibration is generated in the mechanism. Therefore, generally at an intermediate position in some kind of criterion (A ^+), (C ^-) defines, there was determined a certain angular velocity () from zero between the position (A) to the position (A ⁺⁾ Accelerate to value ( _s ) and then constant angular velocity ( _s )
In position (C ^-) moves to. Then, a method of defining the central angle (θ) so as to decelerate () from ( _s ) to zero between the position (C ⁻ ) and the position (C) is used. That is, Δθ (= θi−θi−1) is not a constant value. [Problems to be Solved by the Invention] By the way, in order to perform the above-described steps 1 to 3, that is, to obtain all target positions (Pi) in advance and store them, a huge storage capacity is required. So not realistic. Therefore, a procedure that only needs to be performed once is performed in advance, and the procedure is performed at each sampling time when the mechanism is driven (note that (Δθ) is not a constant value in the procedure of the previous description. , The calculation will be different for each target position). Here, since the expression [2] in the above procedure is an operation expression including sin, cos and many additions / subtractions, it takes a considerable amount of operation time. As a result, the servo sampling time should be shortened sufficiently. However, the servo controllability is impaired. The present invention has been made in view of such a situation, and has an arc mechanism of a servo mechanism that shortens the calculation time for trajectory generation and that enables smooth coupling when coupling with another linear trajectory. The purpose is to provide a generation method. [Means and Actions for Solving Problems] An arc trajectory generation method for a servo mechanism according to the present invention relates to a step of designating a teaching point (A, C) including a start point and an end point of the arc trajectory, and each teaching point. 2 points to be located in time before and after the predetermined time than the time which is located to the point ^{(a -, a +, C} -,
C ⁺ ), respectively, and a step of determining a fourth-order polynomial with respect to time by each teaching point and the above-mentioned two points located before and after the teaching point, and respectively obtaining an acceleration / deceleration trajectory generated thereby, The acceleration / deceleration trajectory is used as the start and end points of the circular arc trajectory. For example starts moving from the starting point of the circular arc track, or when to stop at the end point, the point to 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 a matching Or the point (C ⁺ ) to be located at a time later than the time located at the end point by a certain time is made to coincide with the end point (C) to determine the fourth-order polynomial. As a result, acceleration and deceleration trajectory or points connecting the start point (A) and the point (A ⁺⁾ (C ^-) and deceleration trajectory connecting the end point (C) is represented by a straight line, the operation is simple calculation time Is shorter than 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 a linear trajectory, and the acceleration / deceleration trajectory generated here becomes a curved trajectory and smoothly connects with other linear trajectory. [Embodiment of the Invention] A method of generating an arc trajectory according to the present invention will be described below with reference to FIG. Now, consider the problem of moving from position (A) to position (C) on position (C) on an arcuate orbit at a constant linear velocity. (A) A first trajectory generation method from positions (A) to (A ⁺ ). First, the time required for acceleration (2ta) is determined, and the position on the circular arc orbit reached when moving linearly for the time (ta) at the constant linear velocity (V _s ) given from the position (A) is (A ⁺ ) Stipulate. In addition, the position of the position I was in before the scheduled time to start movement only time (ta) from (A) ^- defined as (A). in this case,
Since before start moving from the position (A) it is had stopped A ^- = an A. Note that FIG. 2 is an explanatory diagram regarding a (x) coordinate of a linear trajectory generation method of a fourth-order polynomial with respect to time. Here, the coordinate values (xi), (yi), of the intermediate position (Pi) from the position (A) to the position (A ⁺ ) in FIG.
(Zi) is defined as follows. Here, hi = (ti + ta) / 2ta (however, −ta ≦ ti ≦ ta) If the trajectory is generated in this way, a linear trajectory connecting the position (A) and the position (A ⁺ ) is changed from the linear velocity zero to the line. while accelerating up to speed _{(V s),} moves at a time (2ta). Also, since the linear acceleration is zero at both position (A) and position (A ⁺ ), when connecting another constant velocity orbit (orbit with zero linear acceleration) before and after the linear orbit created by this method, Allows the linear acceleration at the end points to be continuous. Here, the position (A ⁺⁾ and (A ^-) ie ^{(xa +), (ya +} ) (za +), (xa -), (ya -), (za -)
Since it can be obtained in advance, only equation [5] need be calculated for each sampling time when the mechanism is driven. Since this is a simple calculation compared with the above-mentioned conventional method, the trajectory can be generated in a short 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, the accuracy of the arc trajectory is required only for ultra-low speed operation such as welding, and the time (2ta) required to move between (AA ⁺ ) is short, so the distance (AA ⁺ ) is several It will be about mm. Therefore, a practically problematic deviation does not occur with respect to an arcuate track having a practical radius of curvature. (B) position (A ⁺⁾ from (C ^-) second trajectory generation method to. First, the time (2ta) required for deceleration is determined, and then the time (t) is given at a given constant linear velocity (V _s ) from the position (C).
The position on the circular arc track to reach when the linear movement by a) (C ^- defined as). Here, the position from the position (A ⁺⁾ (C ^-)
The intermediate position (Pi) up to is determined as follows. Position ^{(A +), (B)} , (C -) center position of the circle defined by seeking (G), and obtains the vector (▲ ▼). Surface ^{(A +, B, C -} ) obtaining a unit normal vector (). From the given constant linear velocity (V _s ), the increment value (Δθ) of the central angle for each sampling time is obtained. With respect to the vector (), any vector (Δ
A 3 × 3 conversion matrix R (Δθ) that rotates by θ) is obtained. kykxvers (Δθ) -kzsin (Δθ) ky ₂ vers (Δθ) + cos (Δθ) kykzvers (Δθ) + kxsin (Δθ) However, vers (Δθ) = 1−cos (Δθ). Regarding vector (), vector (▲
Since the vector (▲ ▼) is obtained by rotating ▼) by an angle (Δθ), ▲ ▼ ＝ ▲ ▼ ＋ ▲ ▼…

The target position (Pi) is obtained from [9]. However (▲ ▼)
The initial value of is (GA ⁺ ). When the trajectory is generated as described above, it moves at a constant linear velocity (V _s ) at a constant linear velocity (V _s ) on an arc trajectory connecting the positions (A ⁺ ) and (B) and the position (C ⁻ ). Here, the position (A ⁺⁾ and the position (C ^-) and (delta
θ) (constant value) can be performed in advance, and only the procedure needs to be performed every sampling time when the mechanism is driven. That is, compared with the above-described conventional arc trajectory generation method, the conventional procedure, that is, 3 × for rotating a vector is performed.
The calculation for obtaining the exchange matrix of 3 can be performed in advance.
Therefore, the amount of calculation performed for each sampling time is significantly reduced, and the trajectory can be generated in a short time. (C) position (C ^-) from up to (C) Third trajectory generation method. First, the position to be after the time (ta) after the scheduled time of stopping at the position (C) is defined as (C ⁺ ). In this case, C ⁺ = C because it continues to stop at position (C). Here, the position (C ^-) intermediate position from to (C) coordinate values (Pi) (x
i), (yi), and (zi) are defined as follows in the same manner as the above item (a). Here, hi = (ti + ta) / 2ta (however, −ta ≦ ti ≦ ta) If the trajectory is generated in this manner, the linear velocity connecting the position (C ⁻ ) and the position (C) on the linear velocity (V It moves in time (2ta) while decelerating from _s ) to zero linear velocity. Further, at the positions (C ⁻ ) and (C), the linear acceleration becomes zero. Since this method is a simple calculation as compared with the conventional arc trajectory generation method as described in the item (a), the trajectory can be generated in a short time. FIG. 3 is an explanatory diagram for explaining a connected state of the circular arc trajectory and the linear trajectory. That is, the position (A) to be longer than the scheduled time to arrive (ta) only before other positions the linear orbit of the supposed to be (A ^-) be determined as its arc trajectory generation process of the present invention By using it, it is possible to smoothly connect from another straight track to a circular track while changing its linear velocity and linear acceleration. Conversely, if a point on another straight line that should be after the time (ta) expected to reach the position (C) is defined as the position (C ⁺ ), a straight line from the circular arc trajectory will be obtained as described above. The connection to the track can be done smoothly. [Effect of the Invention] As described above, according to the present invention, the orbits at the start point and the end point of the circular arc orbit and the vicinity thereof are obtained by the fourth-order polynomial with respect to time, so that the calculation time is shortened as compared with the conventional method. Therefore, the servo sampling period can be shortened as compared with the conventional case. In particular,
Since the trajectory is generated by the fourth-order polynomial with respect to time, both the velocity and the acceleration coincide with those at the connection point, and smooth connection is possible from this point as well. Also,
Since it is not necessary to use any other special method for connecting the linear trajectory and the arcuate trajectory, the software for generating the trajectory is easy and the connection is smooth.

[Brief description of drawings]

FIG. 1 is a principle explanatory view for explaining a circular arc trajectory generation method according to an embodiment of the present invention, FIG. 2 is an explanatory view showing a linear trajectory generation method according to the present invention, and FIG. 3 is the circular arc trajectory and a straight line. FIG. 4 is a general explanatory view of a circular arc trajectory, FIG. 5 is a flow chart explaining its operation, and FIG. 6 is an explanatory view showing a conventional circular arc trajectory generation method. is there.

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 defining a fourth-order polynomial with respect to time by each teaching point and the above-mentioned two points located before and after the teaching point, and determining an acceleration / deceleration trajectory generated thereby, respectively. A method for generating an arc trajectory of a servo mechanism, characterized by being used as a point portion. 2. A starts moving from the starting point of the circular arc track, or when to stop at the end point, the point to 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 the point (C ⁺ ) and the end point (C) that should be located at a time that is a certain time after the time at which the end is located
The arc trajectory generation method for a servo mechanism according to claim 1, wherein the fourth order polynomial is determined by matching 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 respectively found on the linear trajectory.