JPH05297916A - Track control method for robot - Google Patents

Abstract

PURPOSE:To shorten a wait time at an instruction point when a feedback servo system is used to zero and to improve track precision by converting the coordinates of the instruction points at the start point and end point of a track to a 3rd coordinate system and performing interpolation calculation based upon a linear speed and an interpolation calculation period. CONSTITUTION:A 2nd coordinate system (X, Y, Z) is defined on the basis of the center of rotation of an axis q2 as its origin. The 3rd coordinate system (X1, Y1, Z1) whose axes are parallel to those of the 2nd coordinate system (X, Y, Z) is defined on the basis of a current instruction point P0 as its origin. Further, a 3rd coordinate system (X2, Y2, Z2) is defined even for a track L2 (P1, P2) which starts at the end point P1 of the track from P0 to P1. The coordinates of the instruction point P0 at the start point of the track L1 and the instruction point at the end point P1 are converted, the interpolation calculation based upon the linear speed and interpolation calculation period is performed, and feedback servo system control depending upon acceleration in the directions of the tracks L1 and L2 is carried out to find an interpolation point as its controlled variable. Further, the process is carried on by plural servo systems until deviation becomes sufficiently small, and a composite interpolation point is calculated to find a final interpolation point.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an articulated robot which is composed of a rotating type and a rotating type. In detail, multiple trajectories defined in a coordinate system independent of the joint coordinate system of the articulated robot can be smoothly moved to the next locus without reducing the speed at the teaching point that is the connection point of the loci as much as possible. The present invention relates to a control method for improving the trajectory of a robot by shifting to the control of 1.

[0002]

2. Description of the Related Art Conventionally, an articulated robot has been widely used because it has a wider working range than other Cartesian coordinates, polar coordinates, and cylindrical coordinates. Although the tip end effector of the manipulator in this articulated robot is effective as a control of each joint, it is not suitable as a method for controlling a trajectory. Further, a method has been proposed in which, in a second coordinate system independent of the joint coordinate system, interpolation points of equal pitch and interpolation points corrected by acceleration / deceleration control are associated by nonlinear servo control (according to the applicant). Filed on February 27, 1992, title of invention: "Robot trajectory control method"). That is, in a trajectory control method of a multi-joint robot in which a joint drive system including an actuator receives a joint angle and a joint angular velocity as command values and independently configures a servo system for each joint, the torque that can be generated in the actuator at each joint is determined for each joint. Even if there are different restrictions, this is a trajectory control method for a robot that follows a trajectory that does not deviate from the target trajectory.

However, this method does not disclose a method of shifting to the control of the next locus at the teaching point which is the connection point of the locus. Further, it is said that the simplest application method is to shift to the control of the next locus after the corrected interpolation point reaches the target teaching point. This has a drawback that a waiting time is generated at a teaching point, or an extremely dead time such as acceleration after sufficiently decelerating is generated. further,
There is a drawback that the loci cannot be connected at the teaching point without decelerating as much as possible.

FIG. 2 is a block diagram of a distance servo system. The target distance S _d and the feasible distance S are given. In a multi-joint robot, generally, in a control system according to a teaching / reproducing system, each joint is controlled by an independently configured servo system. In this servo system, the target distance S _d and the feedback amount from the feasible distance S are compared, and the deviation is subjected to non-linear processing (acceleration / deceleration limitation). FIG. 2 shows a feedback servo system for obtaining the feasible distance S with respect to the target distance S _d on one trajectory. Although the acceleration is limited by the torque limitation of the actuator of each joint of the robot, the feasible distance S that can be achieved without causing the torque saturation is calculated based on the target distance S _d by simulation.

[0005]

FIG. 3 is a diagram showing the acceleration / deceleration control on the locus. Specifically, FIG. 3A shows the time change of the distance from the starting point for one locus. The figure also shows the temporal changes in the target distance Sd and the distance S operable by the acceleration limitation. The horizontal axis represents time, and the vertical axis represents the distance from the starting point on the locus. FIG. 3B is a diagram showing a temporal change in linear velocity. Similarly, the horizontal axis is time,
The vertical axis is the linear velocity on the locus. Distance command S at time t _0
d is constant. Here, it is possible to start the calculation of the next locus, but t = t ₀ actually instructing the robot.
The time distance S has a deviation of ε _{s from} S _d, and it is necessary to wait for time t _{1 at which} it becomes sufficiently small and then perform similar control for the next locus.

The reason is that shifting to the next locus at time t ₀ is the most effective for minimizing the cycle time, but in this case, the linear velocity at time 0 of the control of the new locus is 0.
However, the linear velocity at the time t = t ₀ of the old trajectory control does not become 0, and the linear velocity differs between the two. Therefore, if the switching is simply performed, a large change in the linear velocity will occur, and it cannot be practically used at all. To this end, it is conceivable that the time t ₀ is made as close as possible to t ₁ to reduce the shock in practice, but this is not an essential solution, and the difference between the switching times t ₀ and t ₁ becomes a dead time. There was a flaw. The present invention is premised on switching to the next trajectory control at time t ₀ , and is intended to substantially reduce the fluctuation of the linear velocity caused by the above switching. Desirably, it is intended to completely set it to zero.
At the same time, it aims to achieve the minimization of cycle time.

[0007]

The present invention is a trajectory control method for an articulated robot that continuously follows a plurality of trajectories defined by a second coordinate system that is independent of the joint coordinates of the articulated robot. And is provided by a control method having at least the following means. a) The first teaching point, which is the starting point of the trajectory, and the second teaching point, which is the ending point.
And the interpolation calculation means on the third coordinate for the number of divisions determined by the linear velocity and the interpolation calculation cycle, and b) the distance from the origin in the trajectory direction at the third coordinate. A feedback servo system in which the control amount is the distance resulting from the limitation of the maximum and minimum accelerations in the trajectory direction, and c) the distance between the origin on the third coordinate and the second taught point. And means for obtaining an interpolation point of the third coordinate system corresponding to the distance which is the control amount of the feedback servo system from the ratio of the number of divisions,

D) If a plurality of (m) feedback servo systems for the distance are provided and the deviation between the target and the control amount becomes sufficiently small, the processing is continued even during the control of the next locus, and all Multiple thirds corresponding to ongoing servo system
Means for obtaining a combined interpolation point from the interpolation point of the coordinate system of the second coordinate system, and e) the second point indicating the origin of the third coordinate system corresponding to the oldest servo system in the process of obtaining the combined interpolation point. Means for setting the sum of the coordinate system value and the coordinate value of the synthetic interpolation point as the second coordinate of the final interpolation point, and f) means for managing the plurality of servo systems, which is the oldest first When the processing ends, the second servo system transfers the state quantities of all the second and subsequent processing servo systems to the previous one, and always makes the first processing servo system the oldest processing system. It is characterized by

[0009]

This is a trajectory control method for an articulated robot that continuously follows a plurality of trajectories defined by a second coordinate system that is independent of the joint coordinates of the articulated robot. First, the first teaching point, which is the starting point of the locus, and the second teaching point, which is the ending point, are set to the third teaching point.
And the interpolation calculation on the third coordinate for the number of divisions determined by the linear velocity and the interpolation calculation cycle is performed. Next, with a target from the origin in the trajectory direction, control by a feedback servo system depending on the acceleration in the trajectory direction, a distance from the origin on the third coordinate to the second teaching point, and The interpolation point of the third coordinate system corresponding to the distance, which is the control amount of the feedback servo system, is obtained from the ratio to the number of divisions. Furthermore, a plurality of these feedback servo systems (m)
Mochi, until the deviation between the target and the control amount becomes sufficiently small,
The process is continued even during the control of the next locus, and the sum of the interpolation points of the plurality of third coordinate systems corresponding to all the ongoing servo systems is set as the synthetic interpolation point. The sum of the value of the second coordinate system indicating the origin of the third coordinate system corresponding to the old servo system and the coordinate value of the synthetic interpolation point is set as the second coordinate of the final interpolation point. .. At the end of processing, the oldest first servo system sequentially transfers the state quantities of all the second and subsequent servo systems to the previous one, and always sets the first servo to the oldest processing servo. The present invention works by controlling the system. According to the present invention, the waiting time at the teaching point using the feedback servo system can be made substantially zero. Furthermore, the trajectory accuracy is good, the moving direction can be smoothly changed without lowering the linear velocity at the teaching point, and the cycle time can be improved.

[0010]

EXAMPLES Examples will be described with reference to the drawings.
FIG. 1 is for explaining the coordinate system of an articulated robot that implements the robot trajectory control method of the present invention. One coordinate system (x, y, z) is defined for the articulated robot.
The origin may be anywhere, but generally it is convenient to set the rotation center of the q ₂ axis. Next, it is assumed that the current teaching point P ₀ is the origin and the third coordinate (x ₁ , y ₁ , z ₁ ) whose axes are parallel to the second coordinate (x, y, z) is defined. this is,
One is determined for the locus L ₁ (P ₀ , P ₁ ). Also, P
A locus L ₂ starting from the end point P ₁ of the locus L ₁ from ₀ to P ₁
Similarly, for (P ₁ , P ₂ ), the third coordinates (x ₂ , y ₂ ,
z ₂ ) is defined.

[0011] Between the locus L _i the linear velocity V _i and interpolation period [delta] _t from the interpolation number n _i of the number 1 is established. Here, if the distance of the locus is L _i , ...

[Equation 1] Is required as. At this time, the j-th interpolation point is obtained as Equation 2. However, X _ie is the end point on L _i . The starting point is always 0.

[Equation 2] Next, the maximum acceleration in the locus L _i direction is a _i . This a _i
Is known to be calculated from the maximum acceleration of each joint.

The maximum acceleration a _i in the locus L _i direction is used, and the distance from the starting point on the locus is shown in the block diagram of the feedback servo system by taking FIG. 4 as an example. By configuring the servo system for the distance on the locus shown in FIG. 4, the nonlinear control as shown in FIG. 3 is performed. Consider the distance from the start point on the trajectory and S _i, an instruction distance S _di intended to be obtained the maximum acceleration a _i feasible is limited by the S _i trajectory direction when instructing the S _di = vt is there. Thus, from the allowable angular acceleration of each joint, the maximum acceleration a for the distance on the locus is obtained.
_i , that is, the maximum acceleration in the trajectory direction is obtained in advance, the achievable speed in the movement direction of the trajectory is obtained from this, and this is associated with time to determine whether or not the maximum acceleration can be achieved by limiting the maximum acceleration. If it is not possible, the interpolation point that can be accelerated / decelerated with respect to the interpolation point commanded immediately before is set.
Specifically, with reference to the drawings, the distance S _i from the starting point on the locus is determined every predetermined period for the distance on the locus.
Is assigned as the interpolation point number i, and a first-order lag servo system is configured to increase the current number from the number deviation ε _i which is the deviation between the interpolation point number i and the current number. The linear control limit value determined from the time constant T of the servo system and the maximum acceleration a _{i in the} trajectory direction is obtained, and the linear control limit value is set to a linear range within the number deviation ε _i . In other cases, the number deviation ε _i And a value obtained by multiplying the maximum acceleration by half the product of twice the difference between the linear control limit value and half of the linear control limit value are given as increment values so that these do not exceed the maximum acceleration limit. The process of obtaining the increment value can be installed in the forward loop of the servo system, and the current number obtained as a result can be obtained as the corrected interpolation point number. It should be noted that when the above-described processing is installed in the forward loop of the servo system, it is effective during deceleration, but it is necessary to further check during acceleration whether or not the variation of v _i exceeds the allowable value a _i . For this, it is necessary to compare the absolute value of v _i with a _i, and re-determine v _i by _{vi i} = v _if + a _{i if} the former is large. Here, v _if is the previous v _i .
By integrating v _i obtained as described above, the distance from the starting point on the locus can be obtained as S _i . If the left end of FIG. 4 is the target distance Sd _i and the control amount S _i , the interpolation point number j
_i is given by Equation 3.

[Equation 3] From this result, a feasible interpolation point number j _i considering the acceleration of each joint is obtained, and further, by substituting the equation 3 into the equation 3, the equation 4 is obtained, and the j-th interpolation point is obtained.

[Equation 4]

Next, by using a plurality of (m) feedback servo systems shown in FIG. 4 and repeatedly executing the process until the deviation ε _i becomes sufficiently small, all the sums given by the _equation 4 are calculated at the point P ₀ . The sum of the two coordinates is used as the second coordinate of the interpolation point, and is calculated according to Equation 5.

[Equation 5] This X _j is given as the j-th interpolation point. However, m _a
Is the number of servo system currently being processed, from i = 1 m _a
Will be added up to. Regarding the motion control of the robot, generally, the displacement of the joint driven by the actuator,
The relationship between the hand position and the posture of the manipulator in the working coordinate system is expressed by the equation (6), and is known as a formula of forward kinematics. Here, x is a work coordinate vector that represents the hand position and posture of the manipulator in the work coordinate system, and q is a joint coordinate vector that is the relative displacement of the links at each joint of the manipulator.

[Equation 6]

This further leads to the relationship shown in equation (7). This is generally known as the inverse kinematics relational expression.

[Equation 7] Using this equation 6, q _j can be known from X _j obtained in equation 5. When the deviation of the locus L _i in Expression 5 becomes sufficiently small, the base point is sequentially changed from P ₀ to P ₁ as shown in Expression 8.

[Equation 8] Further, when the control of a new locus is entered, the control is performed so as to obtain X _j by sequentially increasing _ma as shown in Expression 9.

[Equation 9]

Next, the teaching point P ₀ is passed through P ₁ and P ₂
Regarding the control up to, the method for obtaining x when passing the teaching point P ₁ will be described with reference to FIG. Same figure (A)
Is the horizontal axis and the vertical axis is the distance from the starting point P ₀ on the locus L ₁ . In the same (B), the horizontal axis is time, and the vertical axis is the distance from P ₁ on the locus L ₂ . In (C), the horizontal axis represents time, and the vertical axis represents the time change of the second coordinate of the interpolation point. The time axis is moving on the locus L ₁ as shown in (A) of the same figure, and in the same (B) shows the state of being connected to the locus L ₂ at time t _{0 on the} horizontal axis. The figure is shown shifted by t ₀ . That is, from time 0 to t _0, the locus of the tip end effector of the manipulator moves on L ₁ , and the j-th interpolation point X _{j in} the meantime is obtained by the formula 10.

[Equation 10]

Furthermore, from time t _0, the number 11 until the deviation epsilon ₁ is sufficiently small t ₁ of L _1, the first j interpolation point X _j further addition of component X _2j that moves on the trajectory L ₁ ..

[Equation 11] X _2j is ₀ at time t ₀ as shown in FIG.
It is shown that the connection to the locus L ₂ is smoothly made by gradually accelerating. At time t ₁ , the moving component of the locus L ₁ ends, and thereafter, the interpolation point X _j
Is calculated.

[Equation 12] FIG. 6 is a flowchart showing the method of managing the feedback servo system. Here, the processing of the servo system from 1 to m _a, and the oldest of 1, m from 1 to all of the state variables of the servo system from 2 when epsilon ₁ is sufficiently reduced to m _{a a} transferred to -1, and the number m _a = m _a -1 of the servo system during the process. Further, until the deviation (ε _i ) between the target and the control amount becomes sufficiently small, the processing is continued even during the control of the next locus. Furthermore, although not shown here, when controlling the new trajectory, m
Use the + 1st _a, it may execute the flow of Figure 6 as m _a + 1. This is first in first
The feedback servo system of the out can be considered as a buffer for one data. Incidentally, m _a is the maximum value of the servo system during the process.

FIG. 7 shows a simulation result in the case where the manipulator has many degrees of freedom, one of which has two degrees of freedom. FIG. 8 shows a conventional method, which is an example in which the trajectory is controlled in the next direction after the deviation ε ₁ is sufficiently small. 7 and 8, the horizontal axis represents time, the horizontal axis represents time, the vertical axis represents motor current of each joint, and the vertical axis represents motor current.
In the figure, (1) is the motor current of the first axis and (2) is the motor current of the second axis. FIG. 6B shows the change over time in the angular velocity of each joint, and the vertical axis represents the angular velocity. In the figure, (1) is the first axis and (2) is the second axis.
Indicates the angular velocity of the shaft. FIG. 6C shows a temporal change in the linear velocity during movement of the locus, and the vertical axis represents the linear velocity. In the figure, (1) is the commanded linear velocity, and (2) is the actual linear velocity. FIG. 3D shows a locus on one plane, and each axis is x, y corresponding to the second coordinate. Here, time t ₁
Indicates that the buffer is switched. Specifically, point P
It is shown that the movement control from ₀ to P ₁ is completed and the calculation by the formula 12 is started. In FIG. 8D, after stopping at the point P _1, it moves toward the next locus P ₂ . From these figures, in the present invention, as compared with FIG.
In (C), it can be seen that the degree of decrease in the solid linear velocity is large. Further, according to the present invention, as compared with FIG.
It will also be appreciated that in (D) the length of the trajectory is considerably large. It will therefore be appreciated that the present invention can be used to increase the length of the trajectory in the same time period, i.e., the distance traveled by the tip end effector of the manipulator.

[0018]

As described above, according to the present invention, the waiting time at the teaching point using the feedback servo system that enables the acceleration / deceleration on the trajectory of the tip end effector of the manipulator of the articulated robot can be reduced. It can be substantially zero. Therefore, it is possible to smoothly change the moving direction without reducing the linear velocity at the teaching point as much as possible while maintaining good locus accuracy, and also to improve the cycle time.

[0019]

[Brief description of drawings]

FIG. 1 is an explanatory diagram of a coordinate system of an articulated robot that implements a robot trajectory control method of the present invention.

FIG. 2 is a block diagram of a distance servo system.

FIG. 3 is a diagram showing acceleration / deceleration control on a locus, FIG. 3 (A) is a diagram showing a time change of a distance from a starting point with respect to one locus, and FIG. 3 (B) is a line. It is the figure which showed the time change of speed.

FIG. 4 is a block diagram of a feedback servo system for a distance on a trajectory.

5 is a diagram showing a method of obtaining x when passing a teaching point P ₁ , FIG. 5 (A) is a temporal change of a distance on a locus L ₁ , and FIG. 5 (B) is a locus L _2. FIG. 6C is a diagram showing the time variation of the upper distance, and the time variation of the second coordinate of the interpolation point.

FIG. 6 is a flowchart showing a method of managing a feedback servo system.

FIG. 7 is a diagram showing simulation results for the case of two degrees of freedom according to the present invention. The same figure (A) shows the time change of the motor current of each joint, the same figure (B) shows the time change of the angular velocity of each joint, and the same figure (C) shows the time change of the linear velocity during trajectory movement. , (D) shows a locus on one plane, and each axis is x, y corresponding to the second coordinate.

FIG. 8 illustrates a simulation result of a conventional method. The same figure (A) shows the time change of the motor current of each joint, the same figure (B) shows the time change of the angular velocity of each joint, and the same figure (C) shows the time change of the linear velocity during trajectory movement. , (D) shows a locus on one plane, and each axis is x, y corresponding to the second coordinate.

[Explanation of symbols]

P Teaching point L Distance of locus ε _s Deviation δ _t Interpolation cycle n _i Interpolation number S _i Control amount x _ij jth interpolation point

Claims (1)

[Claims] 1. A trajectory control method for an articulated robot that continuously follows a plurality of trajectories defined by a second coordinate system that is independent of the joint coordinates of the articulated robot, comprising: a) at the start point of the trajectory. There is a first teaching point and a second ending point.
And the interpolation calculation means on the third coordinate for the number of divisions determined by the linear velocity and the interpolation calculation cycle, and b) the distance from the origin in the trajectory direction at the third coordinate. A feedback servo system in which the control amount is the distance resulting from the limitation of the maximum and minimum accelerations in the trajectory direction, and c) the distance between the origin on the third coordinate and the second taught point. And a means for obtaining an interpolation point of the third coordinate system corresponding to a distance which is a control amount of the feedback servo system from the ratio of the division number, and d) a plurality of feedback servo systems of the distance (m). Until the deviation between the target and the control amount becomes sufficiently small, the processing is continued even during the control of the next locus, and the interpolation of the plurality of third coordinate systems corresponding to all the ongoing servo systems is performed. And a means to obtain a synthetic interpolation point from e) The sum of the value of the second coordinate system indicating the origin of the third coordinate system corresponding to the oldest servo system in the process of obtaining the synthetic interpolation point and the coordinate value of the synthetic interpolation point is calculated. Means for setting the second coordinate of the final interpolation point, and f) means for managing the plurality of servo systems, wherein the oldest first servo system is in the process of the second and subsequent processes at the end of the process. The method for controlling the trajectory of a robot, which comprises sequentially moving the state quantities of all the servo systems to the previous one, and always making the first one the oldest servo system being processed.