JP2688372B2 - Robot trajectory control device - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Outline] A trajectory control device for an articulated robot, which aims to provide a trajectory control device for a robot capable of smoothly operating a robot in the vicinity of a singular point. Up to a predetermined sampling cycle until the target position calculation means for calculating the hand position of the robot, the current position calculation means for calculating the current position of the robot, the robot based on the output of the target position calculation means and the output of the current position calculation means Joint angular velocity calculation means for calculating the joint angular velocity of the robot, singular point neighborhood discriminating means for discriminating that the joint angle of the robot has approached a singular point at which the joint angle of the robot changes discontinuously based on the change of the joint angular velocity, and the robot has a singular point When the output of the joint angular velocity calculation means tends to increase from the previous sampling to the next sampling when in the vicinity, The output of the joint angular velocity calculation means is reduced or corrected, or when the output of the joint angular velocity calculation means after the next sampling is smaller than the output of the current joint angular velocity calculation means, the output of the current joint angular velocity calculation means is significantly increased. And a joint angular velocity correction unit that outputs a joint angular velocity that prevents the robot from overshooting in the vicinity of the singular point.

[Industrial applications]

The present invention relates to a trajectory control device for a robot, and more particularly, to a trajectory control device for a multi-joint robot for smoothly passing near a singular point peculiar to a mechanism (joint configuration) of the robot.

With the increase in precision of industrial robots in recent years, there is a demand for the robot to be applied to precision assembly work, which was conventionally only performed by humans. For this purpose, it is necessary for the robot to move smoothly while ensuring the trajectory within the movable range.

[Conventional technology]

As a conventional trajectory control device for this type of robot, for example, there is a 6-DOF articulated robot shown in FIG. In FIG. 7, 1 is a 6-DOF articulated robot,
The 6-degree-of-freedom articulated robot 1 is composed of arms 2 to 5, rotary joints 6 to 8 and swivel joints 9 to 11, and a hand 12 is attached to the tip of the arm 5. The 6-DOF articulated robot 1 is for actually performing work. The 6-DOF articulated robot 1 includes a computer 13 for controlling the 6-DOF articulated robot 1 and a computer 13 for controlling the 6-DOF articulated robot 1. A control unit 14 that calculates a control value for moving the 6-DOF articulated robot 1 based on the data and outputs an operation command to the servo system of the 6-DOF articulated robot 1 is connected.

However, in the conventional trajectory control method for the 6-DOF articulated robot 1, a joint that moves linearly near a singular point (in the 6-DOF articulated robot 1, the fourth rotary joint 7 and the The 6 revolving joint 8) suddenly moved, and the work safety could not be guaranteed. FIG. 8 (a) is a diagram showing the angular displacement θ ₆ of the sixth rotary joint 8 when passing near the singular point, and FIG. 8 (b) shows the velocity displacement θ ₆ when passing near the singular point. There is. As shown in FIG. 8 (b), when the sixth joint revolute joint 8 has a certain angular relationship, a singular point at which the joint angle changes discontinuously occurs, so work in the vicinity of the singular point is impossible. An example of a robot that controls the trajectory of a robot by checking the singularity peculiar to the 6-degree-of-freedom articulated robot 1 is disclosed in Japanese Patent Laid-Open No. 58-114888. This one checks whether the solution type of the calculated value has changed from the difference between the calculated joint angle of the robot and the current joint angle, and when the type of solution changes and the singular point becomes close, Corrects (extends) the moving time (sampling time) to obtain an angular velocity that can smoothly pass through a singular point.

[Problems to be solved by the invention]

However, in such a conventional robot trajectory control device, when passing through the vicinity of a singular point, a certain joint operates at the maximum angular velocity as shown in FIG. Even though the target position is reached, even if it is stopped near the singular point as shown in FIG. 9 (a), which shows the angular displacement θ ₆ at the time of stopping near the singular point, it is operating at the maximum speed immediately before the stop. Therefore, FIG. 9 (b)
Then, as shown by the velocity displacement ₆ at the time of stopping near the singular point, it draws a trajectory that goes a little too far and then returns. Since a certain axis goes too far in this way, the locus is deviated, and in the case of precision assembly work, parts may be broken. Further, since control in the vicinity of the singular point is not always sufficient, there is still a region in which work cannot be performed near the singular point, and the workable region must be limited.

Therefore, an object of the present invention is to provide a trajectory control device for a robot that can smoothly move the robot near a singular point.

[Means for solving the problem]

In order to achieve the above-mentioned object, a robot trajectory control device according to the present invention calculates a target position calculation means for calculating a hand position of a robot at a predetermined sampling cycle up to a designated target position, and a current position calculation for calculating a current position of the robot. Means, a joint angular velocity calculation means for calculating the joint angular velocity of the robot based on the outputs of the target position calculation means and the output of the current position calculation means, and the joint angle of the robot discontinuously changes based on the change of the joint angular velocity. If the output of the joint angular velocity calculation means tends to increase from the previous sampling to the next sampling when the robot is near the singular point, the present joint The output of the angular velocity calculation means is reduced and corrected, or the output of the joint angular velocity calculation means after the next sampling is the current joint angular velocity meter. If the output is smaller than the output of the means, the present output of the joint angular velocity calculation means is significantly reduced and corrected, and the joint angular velocity correction means for outputting the joint angular velocity for smoothly stopping the robot near the singular point is provided. Characterize.

[Action]

According to the present invention, the current output of the joint angular velocity calculation means is reduced and corrected based on the change tendency of the output of the joint angular velocity calculation means. Too much is prevented.

EXAMPLES Hereinafter, the present invention will be described with reference to the drawings.

1 to 6 are diagrams showing an embodiment of a trajectory control apparatus for a robot according to the present invention. This embodiment is an example in which the present invention is applied to a 6-DOF articulated robot.

First, the configuration will be described. The structure itself of the 6-DOF articulated robot 1 for actually performing the work is the same as that of the conventional example shown in FIG.

The present invention is different from the conventional example in the control method for controlling the 6-DOF articulated robot 1. In FIG. 1, reference numeral 21 is a control device (robot trajectory control device) for controlling the trajectory of the robot. The control device 21 is a 6-DOF articulated robot 1 with a predetermined sampling cycle up to a designated target position. The present robot 1 based on the linear interpolation unit (target position calculation means) 22 for calculating the hand position and posture of the robot and the encoder counter number of each joint output from the control unit (robot control device) 14.
Robot current position calculation unit (robot current position calculation marking means) 23 for calculating the hand position of the robot, and the robot 1 based on the output results of the linear interpolation unit 22 and the robot current position calculation unit 23.
Of the joint angular velocities (joint angular velocity calculation means, joint angular velocity correction means) 24 for calculating the joint angular velocities of the robot 1 and the joint 1 for smoothly operating the robot 1 near the singular point, and the vicinity of the singular point based on the change in the velocity of each joint. A singular point neighborhood determination unit (singular point neighborhood determination means) 25 for determining whether or not, a velocity output unit 26 for outputting the joint angular velocity calculated by the velocity calculation unit 24 to the control unit (robot control device) 14, It is composed by.

 Next, the operation will be described.

FIG. 2 is a flowchart showing a program of a robot trajectory control method in the vicinity of a singular point, and this program is executed once every predetermined period (for example, 30 mS). Pn in the figure
(N = 1, 2, ...) Indicates each step of the program.

First, the hand position of the robot 1 is calculated at a predetermined sampling cycle for the linear motion designated by P ₁ . The articulated robot 1 needs six parameters XYZαβγ in space. Next, at P ₂ , the current position of the robot 1 is read from the encoder and the speed to the position where the robot 1 moves to the next target position is calculated. The joint angles at the position of the robot that should move after the next sampling period are (θ ₁ , θ ₂ , θ
_3, ......, _{θ 6),} the six degrees of freedom articulated joint angle of the robot 1 at the current position _{(θ 1 ', θ 2'} , θ 3 ', ......, θ 6')
Then, the target speed for moving each joint to the next position is calculated by the following equation.

However, at DT: sampling time P ₄ , it is determined whether or not the speed i calculated in P ₃ is abruptly changing (increasing), and if it is changing, it is judged to be near the singular point and the process proceeds to P ₅ . as it is when they are not being changed P ₉
Proceed to. That is, in P ₄ , assuming that the speed output one sampling time before is _i ′, it is determined that the speed is near the singular point when the following expression is satisfied, and the speed calculated by the expression is output when the following expression is not satisfied.

However, when it is judged that α is a constant near a singular point, the speed of the previous target position is determined in P ₅ , that is, the speed _i ″ (i
= 1 to 6) is calculated (see FIG. 3). That is, it is known that the velocity of the target position is calculated by the linear interpolation by the equation, and thus it is possible to calculate how much the joint angle at the hand position of the robot 1 ahead by one is displaced. .

Next, at P ₆ , it is judged whether or not the speed _i has drastically decreased, and when the speed has drastically decreased, it is judged that the speed must be reduced in order to properly stop the robot 1 at the target position. Decrease the speed at P ₇ by the method described in, and increase the speed at a ratio of P ₈ when the speed does not decrease rapidly. That is, since the robot 1 has to be stopped at the target position with the angular velocity = 0, a rule is set that the velocity should not be increased unnecessarily. Therefore, the angular displacement θ ₆ of the sixth joint near the singular point is shown in FIG. 4, and the velocity displacement ₆ of the portion surrounded by the ellipse A in FIG. 4 is shown in FIG. When the speed _i ′ output one sampling time before increases to the current speed _i and further increases to the speed _i ″ after the previous sampling time, that is, when the following expression is satisfied, The speed _i is controlled by multiplying it by a certain ratio β. The speed after speed control is shown as _6H , and the speed is reduced as shown in FIG.

i ′ <i <i ″ ... _iH = _i × β (0 <β <1), where β: constant Also, FIG. 6 shows the velocity displacement ₆ of the portion surrounded by the ellipse B in FIG. As shown in FIG. 6, when the speed _i ″ of the next sampling time number is smaller than the current speed _i , that is, the ellipse B in FIG.
As shown in FIG. 7A, when the robot 1 has to be stopped quickly when stopped near the singular point, the speed is strongly reduced beforehand by the following equation. Speed at this time is represented by _{6H, 6} ', the relationship between the _{6, 6} "and _6H is shown in Figure 6.

On the other hand, when the speed does not decrease sharply at P _6, it is judged that the robot 1 is near the singular point and it is necessary to increase the speed, and the speed is increased at a ratio of P ₈ and the process proceeds to P ₉ . In P ₉ , the speed calculated in the above steps P ₃ , P ₇ or P ₈ is used as the speed output section.
Control unit from 26 output to (a robot controller) 14, to determine whether it is an end point which is the final target value P _10, is completed this process when it becomes the end point, making it the end point If not, return to P ₁ .

As described above, in this embodiment, the speed is constantly monitored,
When the inclination (change in speed) exceeds a certain value, it is determined that the vehicle has entered the vicinity of the singular point, and the speed _i is corrected so that it stops and passes smoothly at the target position based on the speed _i ″ after one sampling period. Since it is possible to smoothly pass and stop near the singular point, it was possible to work near the singular point in the past, but it is now possible to work near the singular point, greatly increasing the workable area of the robot. In addition, the improved control accuracy eliminates the need to worry about damage to parts near the singular point, etc. Furthermore, robots and assembly parts are installed in advance to avoid work near the singular point as much as possible. It is no longer necessary to consider the position.

Although the present embodiment is an example in which the present invention is applied to the trajectory control of the 6-DOF articulated robot 1, the present invention is not limited to this.
The present controller can be applied to trajectory control of all robots having singular points mechanically.

〔The invention's effect〕

According to the present invention, the current output of the joint angular velocity calculation means is reduced and corrected based on the change tendency of the output of the joint angular velocity calculation means. Therefore, the output in the vicinity of the singular point can be optimized and when the vicinity of the singular point is stopped. It is possible to obtain the special effect of preventing the overshooting of.

[Brief description of the drawings]

1 to 6 are views showing an embodiment of a robot trajectory control device according to the present invention. FIG. 1 is a block configuration diagram showing the robot trajectory control device, and FIG. FIG. 3 is a flowchart showing a program of the robot trajectory control method of FIG. 3, FIG. 3 is a diagram showing the relationship between the angular displacement and angular velocity of the sixth joint near its singular point, and FIG. Fig. 5 is an angular displacement diagram, Fig. 5 is an angular velocity displacement diagram of the sixth joint when passing near the singular point, Fig. 6 is an angular velocity displacement diagram of the sixth joint when stopped near the singular point, and Figs. It is a figure which shows the locus | trajectory control apparatus of a robot, FIG. 7 is the whole block diagram which shows the 6-degree-of-freedom articulated robot, and FIG. 8 (a) is the angular displacement figure of the 6th joint at the time of the vicinity of the singular point. , Fig. 8 (b) is the angular velocity displacement diagram of the sixth joint when passing near the singular point. FIG. 9 (a) is an angular displacement diagram of the sixth joint when stopped near the singular point, and FIG. 9 (b) is an angular velocity displacement diagram of the sixth joint when stopped near the singular point. 1 ... 6 degree of freedom articulated robot (robot), 2-5
...... Arm part, 6 to 8 ...... Rotary joint, 9 to 11 ...... Rotating joint, 12 ...... Hand, 14 ...... Control unit, 21 ...... Control device (robot trajectory control device), 22 ...... Linear interpolation Section (target position calculation means), 23 ... Robot current position calculation section (robot current position calculation means), 24 ... Velocity calculation section (joint angular velocity calculation means, joint angular velocity correction means), 25 ... Singular point vicinity determination section (Means for discriminating near singular point), 26 ... Velocity output section.

Claims (1)

(57) [Claims] 1. A target position calculating means for calculating a hand position of a robot at a predetermined sampling cycle up to a designated target position, a current position calculating means for calculating a current position of the robot, and an output of the target position calculating means. Joint angular velocity calculation means for calculating the joint angular velocity of the robot based on the output of the current position calculation means, and singularity for determining that the joint angle of the robot has approached a singular point where the robot joint angle changes discontinuously based on the change of the joint angular velocity When the output of the joint angular velocity calculation means tends to increase from the previous sampling to the next sampling when the robot is near the singular point and the point neighborhood determination means, the output of the current joint angular velocity calculation means is corrected to decrease, Alternatively, if the output of the joint angular velocity calculation means after the next sampling is smaller than the output of the current joint angular velocity calculation means, the current joint Joint angular velocity correction means for significantly reducing the output of the angular velocity calculation means and outputting the joint angular velocity for preventing the robot from overshooting near the singular point,
A trajectory control device for a robot, comprising: