JPS6037009A - Controller of articulated robot - Google Patents

Abstract

PURPOSE:To perform positioning of a robot quickly and with high accuracy by providing a speed control mechanism to both articulation and right angle coordinate systems respectively and at the same time positioning the robot with deceleration at a target angle to each articulation. CONSTITUTION:The 1st devices 11, 12, 14-16, 21 and 22 fetch the data on the robot target position and the data on the robot position set before movement which are shown by the coordinate system different from an articulation coordinate system and delivers the indicated speed data having time change at a prescribed accelerating/decelerating speed for each sampling timing. The 2nd devices 13 and 17-19 convert the indicated speed data into that of the articulation coordinate system and then into a designated speed which has a time change at an allowable accelerating/decelerating speed of each articulation to deliver it to each articulation driving part. At the same time, a deceleration curve having an angle speed set at zero is stored in each articulation at a target angle obtained by converting the target position data into an articulation coordinate system. Then a switch deciding device 20 delivers a signal to switch all at once each articulation driving part to the position control based on said deceleration curve when a robot gets close to its target position.

Description

DETAILED DESCRIPTION OF THE INVENTION Technical Field of the Invention The present invention relates to a multi-joint type robot that allows a control part such as a robot hand to approach a target position along a planned trajectory and quickly positions it at the target position. This invention relates to a robot control device.

Background of the Technology When making an articulated robot perform a task, it is common practice to give instructions using data in an XYZ rectangular coordinate system that is easy for humans to understand or that is easy to adapt to the external environment.

On the other hand, an articulated robot has a coordinate system that comes from its structure (an articulated robot consists of multiple arms that can rotate or extend and contract, so the coordinate system that uses these arms as the coordinate axis) Since the robot operates in a coordinate system (referred to as a coordinate system), each joint drive unit of the robot is operated while performing coordinate conversion calculations from a rectangular coordinate system to a joint coordinate system in real time.

As shown in Figure 1, the robot hand H moves from point A to point B.
To explain the case of moving along a linear trajectory while keeping the direction constant, first, the magnitude of the linear velocity 's o+ from point A to point B on the rectangular coordinate system xyz is determined according to the position at each point in time. Decide including acceleration/deceleration control. This is expressed as X(11(i
=o, 1.2. ), then after 61 seconds (Δ
T is the sampling time, and the coordinate X (ill) to be located at (determined by taking into account the coordinate transformation time, etc.) is × (i + I) = X (11 + X (1) · ΔT
It can be seen as formula +11. By converting the coordinates to obtain the coordinates θ(1) of the joint coordinate system, the commanded speed θ(11) to be given to each joint of the robot can be calculated as θ(1)'.
The point on the joint coordinate system corresponding to The reason for using is the patent application 1878-1983.
87, it has the following effects. In other words, Fig. 2 compares the instructed speed V+ and the actual moving speed ■2 for one joint, but due to the physical constraint that the output torque of the motor is finite, the actual speed V2
is the instruction speed that is updated in steps every ΔT seconds■
1 and deviates from the planned position θ(ill) by the area of the hatched triangle.

However, in the method of equation (2) called the tracking difference method, θ(1)
Since the actual measured value is used for ', this deviation is automatically corrected at the next commanded speed. As will be described later, if the indicated speed is not changed in steps as in V+, but as inclined as in V2, the shaded area will be slightly smaller.
Problems such as saturation due to excessive input received by each joint drive unit of the robot are eliminated, the robot moves smoothly, and the gain of each joint drive unit can be increased to perform highly accurate control.

Prior Art and Problems Although it has become possible to approach point B by drawing a linear trajectory as shown in FIG. 1 in this way, there still remains a problem in precisely positioning the object at point B.

This is because, as shown in FIG. 2, the commanded speed and the actual speed do not completely match, so even if the drive position Xi+1 and therefore θ,+1 match the target position B at point B, the equation ( 2) Even if i is calculated and used as the commanded speed, the vehicle will not necessarily arrive at the point B exactly after T seconds. Generally, at this point, each axis has the speed bi, so in the next 61 seconds it will pass point B, and return control will be performed based on the actually measured current position Xi.
The robot hand approaches point B while vibrating or forming a loop around point B. Furthermore, it is not possible for C to reach point B completely; it oscillates around point B forever due to the quantization error in the indicated speed and the roughness of the time interval at which the indicated speed is updated. The problem is that sometimes. As mentioned above, the linear velocity's in the rectangular coordinate system
Since to is determined based on acceleration/deceleration, f, and l, it is small near the target position, but since the joint coordinate system is not in a proportional relationship with the orthogonal coordinate system, it must be converted to the joint coordinate system. Then, in October, the indicated speed may be large even near the target position iυ, which tends to cause the above problem.

The problem is that when approaching point B in Figure 1, not only the linear velocity &(1) is gradually reduced at the level of the orthogonal coordinate system (this is rough control because it is for every ΔT), but also This level can also be improved by decelerating each axis as it approaches point B.

At first glance, decelerating the vehicle as it approaches point B at two levels, the Cartesian coordinate system and the joint coordinate system, seems like twice the effort, but if only the Cartesian coordinate system level is used, ΔT is large and the control becomes rough. Difficulty in positioning quickly and with high precision;
When decelerating toward point B in the figure, if deceleration is performed only at the joint coordinate system level, the robot hand will not accurately follow the planned trajectory because it will be decelerated by the acceleration unique to each joint. ■Since the transformation between Cartesian coordinate system and joint coordinate system is nonlinear, as mentioned above, when a certain joint is fully extended, even if the displacement is minute in the Cartesian coordinate system, the velocity and acceleration will be infinite in the joint coordinate system. (This is called a singularity).

Of course, the driving parts of each joint cannot follow infinite speeds and accelerations, and when using pulse motors, etc., they will step out, and when using closed-loop control with DC motors, the control circuit will become saturated and there is a risk of runaway. be. Therefore, acceleration and deceleration control only at the level of the rectangular coordinate system poses security problems, and unless acceleration and deceleration control is performed at the joint coordinate level, processing such as moving the singular point far away is required. Because there are circumstances unique to articulated robots, such as this, the double deceleration described above is by no means a waste.

To perform double deceleration, the coordinate data (target value of each joint angle) of each joint at point B in Figure 1 is set in advance in the control circuit installed for each joint (.

The control circuit for each of these joints is configured to control the target angle (
A deceleration curve is generated with the origin at the target position), and the robot hand is guided to the target angle and positioned there while decelerating the rotational speed of each joint according to this deceleration curve.

However, a new problem arises here.

This will be explained using an example using the simplest two-degree-of-freedom polar coordinate robot, which does not move in a straight line from point A to point B by manipulating the R and θ axes. is an arm of the robot that can extend, contract, and rotate, and L is a locus of the tip of the arm. Here, we want to position the tip of the arm AR at point B, so we set the target positions of the R-axis and θ-axis at point B in advance to the respective servo control circuits, and start from point A. As is clear from the drawing, R2 at the starting point A
Since O is different from that at target point B (R is large and θ is also large in terms of angle from the horizontal axis), a movement is started to reduce the difference to 0, and as a result, the tip of arm AR moves along a straight line. It heads towards B, but at point 0 on the way, the length of the R axis becomes the same as that at point B. As a result, although the θ-axis continues to move from point A to point B, positioning for the R-axis is completed at point 0 (because each joint is controlled independently). Therefore, from point A to point 0, the indicated speed of each axis is carefully adjusted, so the tip of the arm moves in a straight line (but from point 0 to point B, the movement of the R axis stops, so see Figure 3). It moves in an arc as shown by the broken line, and the trajectory is incorrect.

OBJECTS OF THE INVENTION The present invention aims to improve the above points and provide a robot control method that controls the robot hand to quickly and accurately reach a target position along a predetermined trajectory without causing any vibrations. It is something.

Composition of the Invention The control device for an articulated robot of the present invention takes in robot target position data and pre-movement position data expressed in a coordinate system different from the joint coordinate system determined from the structure of the articulated robot, and performs predetermined adjustment. a first device that outputs instruction speed data that changes over time according to the speed at each sampling timing;
The commanded speed data is inputted, converted to commanded speed data in the joint coordinate system, further changed to a commanded speed that changes over time according to the permissible acceleration/deceleration of each joint, and outputted to each joint drive unit of the robot, and A second device has a built-in deceleration curve for each joint in which the angular velocity becomes zero at the robot target angle obtained by converting the position data to the joint coordinate system, and a second device that controls each joint drive unit when the robot approaches the target position. The present invention is characterized by comprising a switching determination device that outputs a signal for switching all at once from speed control to position control according to the deceleration curve, which will be described in detail below with reference to embodiments.

Embodiment of the Invention As described in FIG. 3, even if the trajectory is a simple straight line in Cartesian coordinates, in joint coordinates, the R axis decreases from the starting length to the target length, further decreases below that, and then increases to reach the target length. The goal point is reached only when the object reaches the target point, and this happens frequently with articulated robots. To solve this problem, it is best not to complete positioning at point C in Figure 3 with respect to the R axis, and for this purpose, deceleration on each axis of R9θ should be done until starting from point A and passing through point C. It is better to kill the positioning function. Then, only when the positioning operation becomes truly necessary as the positioning operation approaches point B in FIG. 3, the deceleration and positioning functions of each axis are activated to accurately position the position at point B. The latter control near point B is called a positioning control mode, and the former control at A = Crjl etc. is called a speed control mode.

The following two methods can be considered for determining the approach to point B in FIG. When this determination is established, all axes are instructed to switch from the speed control mode to the positioning control mode. (2) On the Cartesian coordinate system, set a half-nr circle (or sphere for a robot with more joints) centered on point B in Figure 3, and determine whether the robot is inside this circle (or sphere). (2) Determine whether all axes have approached their respective target positions by a certain value d on the joint coordinate system with respect to target positions preset for each axis.

If you choose the radius r or the value d sufficiently small, you can see C in Figure 3.
There is no longer a need to move in an arc from point to point B.

Furthermore, if point C and point B are close to each other as shown in FIG. 4, there is a risk of this happening, but since it is a short circular arc and can be approximated to a straight line, there is no problem in practice.

However, it is not true that r or d can be made as small as desired. In the case of ■ above, if d is set to 0, the fifth
As shown in the figure, when the robot hand is already at target position B in a state where it has been decelerated but still has a speed of V20, it is said that the robot hand is about to position at that target position. Therefore, as long as the moving speed v2 is not 0, the target position B will be exceeded and you will have to go back.Therefore, the way to give r or d is to ensure that the moving speed does not exceed the deceleration curve as shown in Figure 6. If d is set as such, when the robot hand reaches the position (target position - d), it switches to the positioning control mode, decelerates to the target position according to the deceleration curve, and performs highly accurate and quick positioning.

The above assumes a simple example of a polar coordinate robot with two degrees of freedom shown in FIG. 3, but the same can be said of general articulated robots with higher degrees of freedom.

There still remains a problem here. There are two cases: one is to overshoot the target position B once as shown in Figure 5 and then to be positioned to the target position, and the other is to be positioned to the target position smoothly without overshooting as shown in Figure 6. It is clear that this is preferable. Therefore, in order to achieve the state shown in Figure 6, it is necessary to: ■ make r or d sufficiently large, and ■ accurately decelerate the linear velocity planned on the rectangular coordinate system as it approaches point B. However, regarding (2), there is a limit because if r or d is set too large, it will often move in an arc shape between points C and B as shown in FIG. Regarding (2), if the linear velocity deceleration control is performed precisely, smooth positioning as shown in Figure 6 can be expected even if r or d is small, but in the case of articulated robots, there is a singularity as mentioned above. Therefore, if point B is directly above or near the singular point, no matter how much you reduce the linear velocity using method (■), the indicated velocity for a certain axis may become a value close to infinity. In such cases, a state of vibration is unavoidable. The present invention enables reliable positioning even in such a case by allowing only one overshoot, and the switching from speed control to position control is not performed individually in each joint drive unit. When it becomes below r or d, all the joint drive parts are operated at the same time. Examples are shown in FIGS. 7 and 8.

Figure 7 shows an example in which the switching from the speed control mode to the positioning control mode is determined by entering the radius r centered on the target position XB on the Cartesian coordinate system, and Figure 8 shows the joint coordinate system. In the above, an example is shown in which this is performed when all the axes approach the target angle θB set in advance for each axis to a distance of θB−d.

Assuming that the robot is an articulated robot with six degrees of freedom, the position vector X of the robot hand in the Cartesian coordinate system is expressed as X-(x, y, z, α, β, γ). That is, the origin and coordinate axes are set on the table on which the robot is attached, and the position of the hand is expressed by x, y, and z, and the direction is expressed by α, β, and γ (Euler angles).

Then, xA is the coordinate of the hand before movement, XB is the target position of the hand to be positioned by the next movement, X(1) is the coordinate of the hand as a function of discrete time during movement, and X(il
l) is the coordinate of the hand 61 seconds after that. Note that vector symbols and symbols will be omitted as appropriate. On the other hand, the position vector θ of the robot hand in the joint coordinate system is expressed as θ=(θ1.θ2.θ3.θ4.θ5.θ6), where θ■ to θ6 are rotation angles of each joint of the robot. In this case, it is assumed that the robot's arms do not extend or contract. Then, θA is the rotation angle of the hand before starting the movement, θB is the target angle to be positioned by the next movement, θ(1) is the angle as a discrete time function during the movement, and θ(ill) is the 61
The angle at which the robot should be positioned after seconds, θ'(1) is the angle at which the actual rotation angle was actually measured, and θft+ is the angle commanded to the robot as a continuous time function.

To explain each block, numeral 11 is a movement length calculation unit which calculates the movement length (straight line distance between points A and B) SB from the coordinates XA and XB of the hand before and after movement. SR"' l XB X
p and l are calculated as. Here, tr representing the direction of the hand,
Since β and T are in radians (rad), 'l
'iM Multiply by 1 m to get the length. Generally, any radius can be set, but here it is set to 1 m for simplicity. 12 is a calculation unit for unit direction vector e (XB XA
)/l XB

13 is a coordinate conversion unit, which converts the target position vector x in rectangular coordinates.
Convert B into a target position vector θB in joint coordinates. 1
4 is a linear velocity control unit that controls the linear velocity &(1) having a trapezoidal moving distance-linear velocity characteristic, for example, as shown in the figure, where the horizontal axis is the moving distance and the vertical axis is the linear velocity, and the maximum value is limited to sM. Output.

&(1) commands the magnitude of the linear velocity of the hand, especially its tip, while it is moving.

This value indicates the moving speed of the hand itself if there is no change in the direction of the hand, and if the hand position fiffi (rotation axis) is fixed and only the direction changes, this value is extended by 1 m from the hand position in the direction of the hand. Indicates the magnitude of the circumferential velocity of a point. When the position and direction of the hand change at the same time, this is the combined speed. 15 is a calculation unit that calculates the linear velocity vector X fil of the hand during movement as e − S (. Note that the unit vector e is a constant value during one movement in linear movement. 16 is the hand coordinate vector X(ill) after 61 seconds as
11・ΔT is the arithmetic unit, 17 is the rectangular coordinate χ(1+
a coordinate conversion unit 18 that converts the
The commanded (angular) velocity calculating section calculates the commanded speed (1) from the above equation (2). Reference numeral 19 denotes a servo control circuit, which will be described in detail later, but the point is that it receives δ(11) and gives a command angle θ(1) to the robot RB.

20 is a circuit for determining the switching timing from the speed control mode to the positioning control mode, and 21 is an arithmetic circuit for calculating the travel distance S (11) up to the present time, which is one input for this determination.

To explain the operation, when the target position xB is input, the calculation section 11 calculates the total line length SB of the next movement, and the calculation section 12 calculates the direction vector e of the movement. In addition, the converting unit 13 converts 〆B into θB using the coordinate transformation vector A, and sets θB as a target angle in the servo control circuit 19 of each axis. The movement starts when the target position XB is input, and the control unit 14 outputs the linear velocity 5(O) as a small value other than 0 based on the total line length SB and the movement distance up to the current point m S fil. 5(0), that is, ζ(11
(- and others are not explained, but the same is true) is decomposed into a velocity vector calculate. Here, X(0) is the coordinate XA before movement. The next conversion unit 7 uses the coordinate conversion vector A to calculate θ(1) as the joint coordinates to be located 61 seconds later. Furthermore, the calculation unit 18 calculates the actual measurement value θ'(0) from the angle sensor of each joint at the current moment (this is also before movement, so θ
A) is used to calculate the average angular velocity θ (0) of each joint angle for ΔT seconds and output it to the control circuit 19. Note that there is a connection between the conversion section 17 and the calculation section 18 according to the patent application No. 56-1018.
A trajectory interpolation unit may be provided as described in 52. This requires a long calculation time to convert from Cartesian coordinates to joint coordinates, and therefore the indicated value X (
n+) can only be obtained in discrete values (sampling time is large), but this makes the robot's motion less smooth.

Therefore, by interpolating and generating a larger number of instruction values between the discrete instruction values X(i+1), the movement of the robot can be made smoother. The control circuit 19 generates a continuous function θ(tl) for driving each joint of the robot while smoothly accelerating until θ(0) as shown in (2) in FIG. 2, and applies this to the robot RB.i While increasing by 1 every 61 seconds, the command 'S (11 is set to the maximum value sM) from the calculation unit 14
When the number of rotations is increased until , each calculation section generates an output in the same manner as described above, the rotation of each joint of the robot is controlled, and the movement of the robot hand is accelerated accordingly. Then, when 's o+ reaches sM, from then on, ゐ(11=
SM, and each arithmetic unit etc. performs an operation corresponding to this.

In the arithmetic unit 14, S (If = S B &
(1) Deceleration curve part f (SB-3(1
1) (the descending part of the trapezoid in Figure 7), and when the hand position approaches xB, so) and t (SB-3 (1
1) become equal, from then on, f (SB-3(1
1) is set to 11, and the command is given to decelerate.Then, when the value of 5B-3 (11) becomes smaller than the switching judgment value r for switching to the positioning control mode, the judgment circuit 20 sends a command to the control circuit 19 as it further approaches xB. A switching command Sw from speed control mode to positioning control mode is output.

Then, the control circuit 19 sets (enables) a deceleration curve as shown in FIG. Then I reached θB! Then, the positioning operation from point A to point B is completed.

The servo control circuit 19 is shown in FIG. 9 for one axis. This is described in detail in Japanese Patent Application Laid-Open No. 56'-22106, and the outline is as follows. 45 is a motor that drives one axis of the robot, and 44 is an amplifier for driving the motor.

When the target angle θ8 is given and the speed command θi is given from the above-mentioned calculator 18, the count value θj of the counter 34 is 0 at this point, so the comparator 31 changes the switch 33 to the contact a side according to θi>θj. After switching, the counter 34 counts up the output pulses of the oscillator 32. The oscillation frequency of the oscillator 32 has a value corresponding to the maximum acceleration that the servo system can take, and therefore the counter 34 that counts this
The output θj represents a velocity time function when the vehicle is accelerated at the maximum acceleration. Of course, there is no problem if the acceleration is below the maximum acceleration, so the oscillation frequency can be lower than that (also, the oscillation frequency should be set to a value higher than the acceleration/deceleration rate limited by the control unit 14 in order to correspond to the speed command output by the control unit 14. The output θr is currently equal to θ8, and 1θB-θA1 is the ROM36
join. The ROM 36 outputs the aforementioned deceleration curve 1], and outputs the square root of 1θB-θr1 as an attos. Since this output θS is large at this moment, the comparator 37 switches the switch 38 to the a side according to θB>θj. Therefore, the output θj of the counter 34 is
9 and converted to analog, this is input to a voltage-frequency converter 40 and converted into a frequency, and the frequency is counted by a counter 41 and becomes the position command θr.

On the other hand, an encoder 46 is connected to the rotating shaft of the motor 45, and a counter 47 counts the output pulses of the encoder 46 to produce an output θ'i indicating the current position. The adder/subtractor 48 receives θr and θ'1 and outputs 0 for the difference.
This is converted into analog by a D/A converter 49.

The output of the D/A converter 49 indicates the position error, which is input to the closed loop control circuit 51 and also to the speed deviation estimating circuit 50, where it is differentiated and becomes the speed deviation. input. The closed loop control circuit 51 generates an output that follows the positional deviation θr in both the speed control mode and the position control mode, and adds this to the amplifier 44 through the adder 43 to drive the motor 45. An acceleration component obtained by differentiating the output of the D/A converter 39 by a differentiating circuit 42 is also input to the adder 43, and the motor 45 is supplied with an acceleration current commanded by this acceleration component.

The counter 31 continues counting, and when θi=θj, the comparator 31 switches the switch 33 to a contact state, and the counter 34 stops counting. Therefore, θj remains constant, and the motor 45 is controlled at a constant speed. θi is V+ in the second WJ
corresponds to , and changes in steps. In response to this change, the comparator 31 switches the switch 33 to a if θl>θj, and θi=
If θj, the switch is switched to b, and if θi is less than θj, the switch is switched to C, and at the C position, the counter 34 counts down. In Fig. 2, vl, which changes stepwise, is the command speed, but in the circuit of Fig. 9, the command speed is corrected by the maximum acceleration/deceleration (taking an angle) by the function of the counter 34, etc.
The motor 45 can perform smooth acceleration and deceleration.

As the output θr of the counter 41 approaches the target position θB, the difference 10B-θr1 becomes small, and the output θS of the ROM 36 begins to decrease. The comparator 37 switches the switch 38 to the contact side when the switching command Sw is input from the judgment circuit 20 in the case of FIG. 7 or from the judgment circuit 23 in the case of FIG. Leads to 39. Therefore, the motor 45 is subjected to position control according to the deceleration curve output by the ROM 36, and is reliably stopped at the target position.

FIG. 8 is the same as FIG. 7 except that when θB-0(tl becomes less than the above d, the determination circuit 23 determines this and outputs the switching signal SW.

As is clear from the detailed description of the invention, according to the present invention, even when moving a joint coordinate robot by coordinate transformation so that it appears to be a Cartesian coordinate robot, the target angle (θB) for each joint can be adjusted. By performing positioning while decelerating, rapid and highly accurate positioning becomes possible. In addition, by providing acceleration/deceleration control mechanisms in each of the joint coordinate system and Cartesian coordinate system, there is no need to worry about runaway due to saturation of the closed-loop servo system due to the finite output torque of the drive unit of each joint, and Since all joint drive units simultaneously switch from speed control to position control near the target position, motion control with high trajectory accuracy is possible.

[Brief explanation of the drawing]

Figures 1 to 6 are explanatory diagrams of the operation of the articulated robot, Figures 7 and 8 are block diagrams showing embodiments of the present invention, and Figure 9 is a partial detail of Figures 7 and 8. FIG. In the drawings, 11.12.14 to 16 and 21.22 are first devices, 13.17 to 19 are second devices, and 20.23 is a switching determination device. Applicant Fujitsu Ltd. Representative Patent Attorney Minoru Aoyagi Figure 1 B Figure 2 1△T- Figure 4 Figure 5 Figure 6

Claims (1)

[Claims] (11. Robot target position data and pre-movement position data expressed in a coordinate system different from the joint coordinate system determined by the structure of the articulated robot are taken in and changed over time at a predetermined acceleration/deceleration rate. a first device that outputs instruction speed data at each sampling timing; a first device that receives the instruction speed data and converts it into instruction speed data in a joint coordinate system; and an instruction that changes over time according to the allowable acceleration/deceleration of each joint; A second curve that includes a deceleration curve for each joint in which the angular velocity becomes zero at the robot target angle obtained by converting the target position data into the joint coordinate system and converting the target position data into the joint coordinate system. Control of an articulated robot characterized by comprising: a device; and a switching determination device that outputs a signal for switching all joint drive units from speed control to position control according to the deceleration curve when the robot approaches a target position. (2) The switching determination device captures the robot's current position in a coordinate system different from the joint coordinate system, and outputs a switching signal when it enters a sphere with a predetermined radius centered on the target position. The first claim characterized in that
A control device for the articulated robot described in Section 1. (3) The switching determination device captures the current position of the robot in the joint coordinate system, and outputs a switching signal when the current position of the robot is within a predetermined value with respect to the target position. A control device for an articulated robot according to scope 1.