JPH0366576A - Control device for locus of robot - Google Patents

Abstract

PURPOSE:To smoothly move a robot by correcting the output of a joint angle speed calculating means based on the joint angle speed prior to one sampling period and the joint angle speed of the next sampling period at the time when a robot exists close to a specific point and providing a means outputting a joint angle speed which operates a robot smoothly near a specific point. CONSTITUTION:A robot present position calculating part 23 calculating the terminal position of a present robot 1 from the number of the encoder counter of each joint output from a control part 14 is provided. The joint angle speed of the robot 1 is then calculated based on the output results of a linear interpolation part 22 and robot present position calculation part 23 and a joint angle speed smoothly operating the robot 1 near a specific point is calculated by a speed calculation part 24. The joint angle speed calculated by this speed calculation part 24 is output from a speed output part 26 to the control part 14.

Description

[Detailed Description of the Invention] [Summary] The present invention relates to a trajectory control device for an articulated robot, and aims to provide a trajectory control device for a robot that can smoothly operate the robot in the vicinity of a singular point. target position calculation means for calculating the robot's position at every predetermined sampling period; current position calculation means for calculating the robot's current position; a joint angular velocity calculation means for calculating a joint angular velocity; a singularity proximity determination means for determining based on the change in the joint angular velocity that the robot approaches a singularity point where the joint angle changes discontinuously; , the output of the joint angular velocity calculation means is corrected based on the joint angular velocity of one sampling cycle before and the joint angular velocity of the next sampling cycle, and a joint angular velocity that allows the robot 7) to operate smoothly in the vicinity of the singularity is output. Joint angular velocity correction means.

[Industrial application field]

The present invention relates to a robot trajectory control device, and more specifically,
The present invention relates to a trajectory control device for an articulated robot that allows the robot to smoothly pass through the vicinity of singular points specific to the robot's mechanism (joint configuration).

As industrial robots have become more precise in recent years, there has been a demand for robots to be used in precision assembly work that could only be performed by humans in the past. To achieve this, it is necessary to ensure the trajectory of the robot within its movable range and to operate it smoothly.

[Conventional technology]

As a conventional robot trajectory control device of this type, there is, for example, a 6-degree-of-freedom multi-jointed robot robot shown in FIG. 7th
In the figure, 1 is a six-degree-of-freedom articulated robot;
The multi-jointed robot 1 includes arm sections 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 section 5. 6
The articulated robot 1 with degrees of freedom is for actually performing work, and the articulated robot 1 with 6 degrees of freedom has a computer 13 for controlling the articulated robot 1 with 6 degrees of freedom and data from the computer 13. Based on 6 degrees of freedom, calculate the control value for moving the articulated robot
A control unit 14 is connected to the servo system of the multi-joint robot robot 1 with degrees of freedom and outputs operation commands.

However, in the conventional trajectory control method of the 6-degree-of-freedom articulated robot 1, when the vicinity of the singularity is linearly moved, a certain joint (in the case of the 6-degree-of-freedom articulated robot I-1, the fourth rotation amount node) 7 and the sixth rotation amount node 8) moved suddenly, and work safety could not be guaranteed.

FIG. 8(a) is a diagram showing the angular displacement θ6 of the sixth rotation amount node 80 when passing near the singular point, and FIG. 8(b) shows the velocity displacement 66 when passing near the singular point. Figure 8(b)
As shown in the figure, when the sixth rotary joint 8 reaches a certain angular relationship, a singular point occurs where the joint angle changes discontinuously.
It is impossible to work near the singularity. For example, Japanese Patent Application Laid-Open No. 58-1148 is an example of a device that controls the robot's trajectory by checking the singularity specific to the 6-degree-of-freedom articulated robot robot 1.
There is one described in Publication No. 88. This method checks whether the type of solution for the calculated value has changed based on the difference between the robot's calculated target joint angle and the current joint angle.
When the type of solution changes and the singularity becomes closer, the moving time (sampling time) is corrected (extended) to obtain an angular velocity that allows the singularity to be passed smoothly.

[Problem to be solved by the invention]

However, in such a conventional robot trajectory control device, when passing near a singularity as shown in Figure 8(b), a certain joint moves at the maximum angular velocity, which cannot be said to be smooth. Although it reaches the target position,
When stopping near the singularity, as shown in Fig. 9(a), the angular displacement θ when stopping near the singularity, the operation is at maximum speed until immediately before stopping, so Fig. 9(b) As shown by the velocity displacement δ when stopped near the singularity, it draws a trajectory that goes a little too far and then returns.

In this way, if a certain axis goes too far, the trajectory will be off, which may cause parts to break, etc. in cases such as precision assembly work. Furthermore, since control in the vicinity of the singularity is not necessarily sufficient, there still exists an area in the vicinity of the singularity in which work cannot be performed, and the workable area is inevitably limited.

Therefore, an object of the present invention is to provide a trajectory-based iTJ device for a robot that can smoothly operate a robot near a singularity.

[Means to solve the problem]

In order to achieve the above object, the robot trajectory control device according to the present invention includes a target position calculation means for calculating the position of the robot at each predetermined sampling period up to a specified target position;
current position calculation means for calculating the current position of the robot; joint angular velocity calculation means for calculating the joint angular velocity of the robot based on the output of the target position calculation means and the output of the current position calculation means; Singular point proximity determination means for determining whether the robot approaches a singular point where the joint angle changes discontinuously; and when the robot is near the singular point, the joint angular velocity one sampling period before and the joint angular velocity in the next sampling period. and joint angular velocity correction means for correcting the output of the joint angular velocity calculation means based on the above, and outputting a joint angular velocity that allows the robot to smoothly operate in the vicinity of the singular point.

[Effect]

In this embodiment, when the robot enters the vicinity of the singularity, the angular velocity of the robot is corrected based on the angular velocity one sampling cycle before and the angular velocity of the next sampling cycle so that the robot passes and stops smoothly in the vicinity of the singularity. Ru.

Therefore, by looking at the next sampling period, the singular point is checked while looking ahead to the next step, and the optimal speed for passing and stopping is determined while determining the distance the robot will move next. As a result, passing near the singularity,
Stopping is performed smoothly, making precise assembly work possible, and work near singularities is possible, greatly expanding the workable area.

〔Example〕

Hereinafter, the present invention will be explained based on the drawings.

1 to 6 are diagrams showing one embodiment of a robot trajectory control device according to the present invention, and this embodiment is an example in which the present invention is applied to a six-degree-of-freedom articulated robot.

First, the configuration will be explained. The configuration itself of the 6-degree-of-freedom articulated robot 1 for actually carrying out work is the same as the conventional example shown in FIG. 7, so a description thereof will be omitted.

The present invention differs from the conventional example in the control method for controlling the 6-degree-of-freedom articulated robot l. In FIG. 1, reference numeral 21 denotes a control device (robot trajectory control device) for controlling the trajectory of the robot.
A robot that calculates the current hand position of the robot 1 from the linear interpolation unit (target position calculation means) 22 that calculates the hand position and posture of the robot 1, and the encoder counter number of each joint output from the control unit (robot control device) 14. Current position calculation section (
The joint angular velocity of the robot 7) 1 is calculated based on the output results of the robot current position calculation means) 23, the linear interpolation unit 22, and the robot current position calculation unit 23, and the joint angular velocity is used to smoothly move the robot i in the vicinity of the singularity. Velocity calculation unit (joint angular velocity calculation means, joint angular velocity correction means) 2 that calculates
4, a singularity vicinity determination unit (singularity vicinity determination means) 25 that determines whether or not it is in the vicinity of a singularity based on changes in the velocity of each joint;
a speed output unit 26 that outputs the joint angular velocity calculated by the speed calculation unit 24 to the control unit (robot control device) 14;
It is made up of.

Next, the effect will be explained.

FIG. 2 is a flowchart showing a program for a robot trajectory control method near a singular point, and this program is executed once every predetermined period (for example, 3QmS). In the figure, Pn (n=t, 2, . . . ) indicates each step of the program.

First, the hand position of the robot l is calculated at a predetermined sampling period for the linear motion instructed by Pl. The articulated robot 1 requires six parameters: XYZαβγ in space. Next, at P□, the current position of the robot throat 1 is read from the encoder, and the speed to move to the next target position is calculated. The joint angle at the position of the robot to be moved after the next sampling period is (θ8,
θ2, θ3, ......, θ,), the joint angles of the 6-degree-of-freedom articulated robot soto 1 at the current position are (θ1', θ1, θ
.

..., θ6'), the target speed θ for moving each joint to the next position is calculated using the following equation.

However, DT: At sampling time P4, the speed d branch calculated by P suddenly changes (increases).
If it is changing, it is determined that it is near the singularity and the process proceeds to P. If it is not changing, the process directly proceeds to P. That is, in P4, the speed output one sampling time ago is set to 6. ′, when the following formula 〇 is satisfied, it is determined that the singularity is near, and when the following formula 〇 is not satisfied, the speed calculated using the 0th formula is output.

σ.

〉α (i=1 to 6) ・・・・・・Table 06 However, α: constant When it is determined that it is near the singular point, P is the velocity of the previous target position, that is, the velocity after one sampling period t) i (
i=1 to 6) (see Figure 3). In other words, since we know that the velocity at the target position is calculated by linear interpolation using the 0th equation, we can calculate how much the joint angle at the hand position of robot 1, which is currently one robot ahead, will be displaced. I can do it.

Next, it is determined whether the speed 61 has suddenly decreased at P, and if the speed has suddenly decreased, it is determined that the speed must be reduced in order to properly stop the robot l at the target position, and the following steps are performed. The speed is reduced by P by the method described in , and if the speed is not rapidly decreasing, the speed is increased by a ratio of P8. In other words, at the target position, the robot l
must be stopped with the angular velocity = O, so a rule is set that the speed must not be increased unnecessarily.

Therefore, the angular displacement θ of the sixth joint near the singularity is shown in FIG. 4, and the velocity displacement σ of the part surrounded by the tank circle A in the middle of FIG.
, is shown in Fig. 5. As shown in Fig. 5, the speed d, / output one sampling time ago increases to the current speed δi, and further increases to the speed 6i'' after one sampling time. In other words, when the following formula (■) is satisfied, the speed is suppressed by multiplying the speed/j% by a certain ratio β according to the following formula (■).The speed after speed control is shown by 66) I, and as shown in Fig. 5 The speed is reduced as shown in .

/ji J , /θikuδi“ ・・・・・・■θ,□
=6. × β (0 × β < 1) ・・・・・・■ However,
β: constant Also, when the velocity displacement σ of the part surrounded by the tank circle B in Figure 4 is shown in Figure 6, as shown in Figure 6, the velocity 1jl'' after the next sampling time is the current If the speed is smaller than d, that is, if the robot 1 must be stopped immediately when stopped near the singularity as shown in the tank circle B in Fig. 4 or in Fig. 9 (a) mentioned above, the following Expression■
to reduce the speed strongly in advance. The speed at this time is indicated by σ6H, and 6. ′, 11616 ′ and σ
The relationship with 6H is shown in FIG.

δ, ′-6N δ, 8= ...... ■On the other hand, P
If the speed does not decrease rapidly at step 6, it is determined that Robot 1 is near the singularity and it is necessary to increase the speed, and P
[Increase the speed by a certain ratio of l and proceed to P. At P, the speed calculated using the stethoscope P3, P, or pH is output from the speed output section 26 to the control section (robot control device) 14, and PIO determines whether or not the end point, which is the final target value, has been reached. It is determined, and when the end point is reached, the current processing is finished,
If it is not the end point, return to Pl.

As described above, in this embodiment, the speed is constantly monitored, and when the slope (velocity change) exceeds a certain value, it is determined that it has entered the vicinity of the singularity, and the speed σ is determined based on the speeds σ and # after one sampling period. , is corrected so that it stops smoothly at the target position and reaches 1ffl. Therefore, passing near the singularity,
Because the robot can stop smoothly, it is now possible to work near a singularity, whereas previously it was impossible to do so near a singularity, greatly expanding the workable range of the robot. Furthermore, since the control accuracy is improved, there is no need to worry about damage to parts near the singularity. Furthermore, it is no longer necessary to carefully consider the installation positions of robots, assembly parts, etc. in advance to avoid work near singularities as much as possible.

Although this embodiment is an example in which the present invention is applied to trajectory control of a 6-degree-of-freedom articulated robot 1, this control is not limited to this and can be applied to trajectory control of any robot that has a mechanical singularity. equipment is applicable.

〔Effect of the invention〕

According to the present invention, even when the robot is near a singularity, it is possible to operate the robot smoothly, the workable area can be greatly expanded, and even when the robot is stopped near the singularity, the trajectory can be changed. It can prevent misalignment and can be applied to precision assembly work.

[Brief explanation of drawings]

1 to 6 are diagrams showing one embodiment of a robot trajectory control device according to the present invention. FIG. 1 is a block diagram showing the robot trajectory control device, and FIG. Figure 3 is a flowchart showing the program for the robot trajectory control method, Figure 3 is a diagram showing the relationship between the angular displacement and angular velocity of the sixth joint near the singularity, and Figure 4 is a diagram showing the relationship between the angular displacement and angular velocity of the sixth joint near the singularity. Figure 5 is the angular velocity displacement diagram of the 6th joint when passing near the singularity, Figure 6 is the angular velocity displacement diagram of the 6th joint when stopped near the singularity, and Figures 7 to 9 are the conventional FIG. 7 is a diagram illustrating the robot's trajectory control device, and FIG. 7 is an overall configuration diagram showing the 6-degree-of-freedom multi-joint robot robot, and FIG. 8 (a) is a diagram of the angular displacement of the 6th joint when iJ1 passes near the singularity. , Figure 8(b) is the angular velocity displacement diagram of the sixth joint when passing near the singularity, Figure 9(a) is the angular displacement diagram of the sixth joint when stopped near the singularity, Figure 9( b) is an angular velocity displacement diagram of the sixth joint when stopped near the singularity. 〆21. Control ml position 1...6 degrees of freedom multi-joint robot (robot), 2-5...arm section, 6-8...rotating joint, 9-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...Singularity Point vicinity determination unit (singularity vicinity determination means), 26... Speed output unit. Fig. 3 shows the relationship between the angular displacement and angular velocity of the 6th joint. Fig. 3 shows the angular displacement of the s6 joint near the singular point of the first embodiment. Fig. 4 shows the relationship between the angular displacement of the 6th joint and the angular velocity. Figure 6: Angular velocity displacement diagram of

Claims (1)

[Claims] Target position calculation means for calculating the hand position of the robot at every predetermined sampling period up to a specified target position; current position calculation means for calculating the current position of the robot; and output of the target position calculation means. and a 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 determining based on the change in the joint angular velocity that the robot has approached a singular point where the joint angle changes discontinuously. a singular point vicinity determination means; when the robot is in the vicinity of the singular point, correcting the output of the joint angular velocity calculation means based on the joint angular velocity one sampling period before and the joint angular velocity of the next sampling period;
A robot trajectory control device comprising: joint angular velocity correction means for outputting a joint angular velocity that allows the robot to smoothly operate in the vicinity of a singular point.