JPH0583922B2 - - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Field of Industrial Application] The present invention relates to an articulated robot that controls an actuator to move a tool or hand along a given path with high accuracy. This invention relates to a route control method for a robot hand to be controlled.

[Conventional technology]

Path control methods for controlling the path of the robot hand include a method using position control and a method using speed control. As shown in Fig. 1, the position control method uses current position data x obtained by coordinate transformation of the pairwise displacement θ indicating the current position of the robot hand and target position data x _f indicating the end point. Calculate the target position x _o at the time of sampling. Subtract the even displacement _θ that indicates the current position from the even displacement θ _a obtained by coordinate transformation of the target position The pair speed θ of each rotating pair becomes
is determined and input to the drive system 1. The drive system 1 is provided with drive circuits having the same configuration as many degrees of freedom as the robot hand has, and is composed of a motor M, a tachogenerator TG, a rotational displacement detector PE, a counter C, a servo amplifier A1, and an amplifier A2. ing.

On the other hand, in the speed control method, as shown in Figure 2, when the robot hand receives a command to move in a straight line from the current position to the target position, it moves from the current position data x and the target position data x _f indicating the end point to the next position. A target speed u at the time of sampling is calculated, coordinate transformation is performed to obtain a pair speed θ· of each rotation pair, and this is input to the drive system 1.

In FIGS. 1 and 2, θ is the even displacement of the actuator obtained by counting the output of the rotational displacement detector PE of the actuator with a counter C,
The current position data x is obtained by converting the coordinates into a rectangular coordinate system.

[Problem to be solved by the invention]

Among the conventional path control methods for controlling the path of a robot hand, the speed control method involves setting a speed curve in advance for the planned straight path, and using that to determine the target position for the next sampling.
x _o is determined. However, since the speed of the robot hand fluctuates depending on the load, there is a steady deviation from the preset speed curve, causing it to deviate from a straight path or fluctuating in speed, making it unable to move at a constant speed. It was hot. moreover,
There was a problem in that once the vehicle deviated from the straight path, it was impossible to return to the original path.

In order to eliminate these problems with the speed control method, the position control method calculates and determines the deviation from the target position x _o at the time of the next sampling based on a predetermined path. An attempt is made to improve route accuracy by correcting the speed command value using a restoration speed proportional to the amount of deviation. However, as a result of experiments, it has been found that the route control method of the robot hand using such a position control method cannot improve the route accuracy to a sufficient degree.

FIG. 3 is a diagram illustrating an example of actual measurement of linear path accuracy obtained by a conventional robot hand path control method. This example uses an articulated 5-degree-of-freedom robot with an upper arm length of 800 mm and a forearm length of 600 mm, and a distance between two points of 160 mm.
This shows the linear path accuracy when moving at a specified speed of 100 mm/sec. The horizontal axis is time (seconds),
The sampling time is 30 milliseconds. The vertical axis shows the deviation, which is expressed as the absolute value of the amount of deviation from the designated path. Part A in the figure is the case when general speed control is performed without any correction, and part B in the figure is
Based on the specified path by the position control described above, the correction is made using a restoration speed proportional to the amount of deviation from the target _position The speed command vector is determined by giving k ₁ but not giving the coefficient k ₂ and using only a correction term proportional to the positional deviation vector e related to the coefficient k ₁ . On the other hand, C in the figure gives both coefficient k ₁ and coefficient k ₂ of equation (5),
a term proportional to the positional deviation vector e related to the coefficient k ₁ ;
The speed command vector is determined using a correction term proportional to the integral of the positional deviation with respect to the coefficient _k2 .

As is clear from Figures 3A and 3B, the route accuracy of the prior art was not yet sufficient.

SUMMARY OF THE INVENTION An object of the present invention is to provide a path control method for a robot hand that can move a tool or hand along a given path with high path accuracy in order to solve the problems of the prior art described above. It is in.

[Means to solve the problem]

In order to achieve the above object, the present invention provides a method for controlling the path of a robot that moves a tool or hand along a given path by controlling the actuators. From the displacement of , calculate the position vector of the tool or hand in Cartesian coordinates, calculate the vertical distance vector from the given path in the position vector calculated for each sampling period, and calculate the vector information of the given path. A tool or hand speed command vector having a path direction component based on the calculated vertical distance vector and a direction component that decreases the distance obtained based on the calculated vertical distance vector is generated for each sampling period, and the tool or hand speed command vector is A method for controlling the path of a robot hand, comprising: converting the coordinates of a command vector according to the mechanism of the robot, calculating a control command value for the actuator, and controlling the actuator based on the calculated control command value. It is. Moreover, the present invention
In a method of controlling the path of a robot that moves a tool or hand along a given path by controlling an actuator, the position of the tool or hand in Cartesian coordinates is determined from the displacement of the actuator of the robot sequentially detected at each sampling period. a vector, and calculates a vertical distance vector from the given path in the position vector calculated for each sampling period, and a tool or tool along the direction of the given path based on the vector information of the given path. Calculate a velocity deviation vector with respect to a specified speed of the hand, and have a path direction component based on vector information of the given path and a direction component that reduces the distance obtained based on the calculated vertical distance vector, and further A speed command vector of the tool or hand corrected based on the calculated speed deviation vector is generated at each sampling period, and the speed command vector of the tool or hand is coordinate-transformed according to the mechanism of the robot. This is a path control method for a robot hand, characterized in that a control command value for an actuator is calculated, and the actuator is controlled based on the calculated control command value.

[Effect]

According to the robot hand path control method of the present invention, a vertical distance vector from a given path in a position vector calculated for each sampling period is calculated, and a path direction component based on vector information of the given path is calculated. Since a tool or hand speed command vector having a directional component that decreases the distance obtained based on the calculated vertical distance vector is generated at each sampling period, the path accuracy is higher than that of the conventional technology. can move the tool or hand. In addition, by correcting the speed command vector of the tool or hand based on the vector information of the given path and the speed deviation vector with respect to the specified speed of the tool or hand along the direction of the path, high path accuracy can be achieved. can be obtained and maintain constant velocity at the specified speed. That is, if the control is based only on the vertical distance vector from the route, the speed will vary depending on the magnitude of the vertical distance from the route, and constant speed control cannot be performed. However, constant speed control can be performed by correcting based on the speed deviation vector.

Next, the principle of the present invention will be explained using Fig. 4.
This will be explained using a straight route as an example. Starting point A to ending point B
The straight line up to is the given route. Also, let P be a point in the middle of the movement of the robot hand, and let Q be the leg of a perpendicular line drawn from point P to a given straight path. Furthermore, the position vectors of points A, B, P, and Q are respectively x→ _s , x→ _f , x→ _t , and x→ _q,t , and the direction vector of the straight path is a→. In this way, the positional deviation vector e→ and the velocity deviation vector e→ _v can be expressed as follows.

x→ _q,t =x→ _f −{(x→ _f −x→ _t )・a→}a→……
(1) e→=x→ _q,t −x→ _t …(2) v→ _t = (x→ _q,t −x→ _q,t-1 )/T …(3) e→ _v = v→ _t −V _p a→ ...(4) Here, v→ _t is the velocity vector of the hand,
vo is the designated speed and T indicates the sampling time. Here, the specified speed is the interval when moving the robot hand in a certain interval AB.
This is the speed command value at AB, not the speed command value for each sampling time. Also, the subscript t indicates the number of sampling times, t-1
indicates the value at the previous sampling.

Based on these, the speed command vector u→ is determined as follows.

u→=v _t a→+k ₁ e→/T+k ₂ Σe→/T ……(5) Or u→=v _t a→+k ₁ e→/T+k ₂ Σe→/T +k ₃ e→ _v +k ₄ Σe→ _v ...(6) Here, Σe→ indicates the accumulated amount of positional deviation for each sampling from the starting point A, and Σe→ _v represents the velocity deviation for each sampling from the first sampling point in the constant velocity region. It shows the amount of accumulation. Further, k ₁ to _{k 4} are proportional constants. Equation (5) above is useful for highly accurate route control, and Equation (6) is useful for constant speed control as well as highly accurate route control. This speed command vector u→
The route is determined based on the . Note that the term related to the coefficient k ₁ is a correction term proportional to the positional deviation vector e, and the term related to the coefficient k ₂ is a correction term proportional to the integral of the positional deviation, and the correction amount is calculated as the deviation in Fig. 6. is calculated. If the command speed is determined using only the correction term related to the coefficient k ₁ , the path error will be reduced as shown in Figure 3 (b), and if the correction term related to the coefficient k ₂ is used, the path error will be further reduced as shown in c. decreases.

〔Example〕

Hereinafter, the present invention will be explained in more detail with reference to embodiments shown in the accompanying drawings.

FIG. 5 is a block diagram showing an embodiment of a control device for carrying out the robot hand path control method of the present invention. In FIG. 5, the processor 11 is
Through the bus 12, a memory 13, a table 14, a multiplier/divider 15, an output port 16, an input port 17,
The output port 16 and the input port 17 form an input/output section between the robot body 19 and the bus 12.

The processor 11 performs various calculation controls and the bus 12.
Memory 13, table 14, multiplier/divider 1
5. Manages the output port 16, input port 17, and teaching box 18. The memory 13 stores programs for calculation control and various data. The table 14 is a data table that stores trigonometric functions and inverse trigonometric functions. The multiplier/divider 15 is
This is hardware dedicated to multiplication and division. The teaching box 18 is a console for man-machine interface during teaching. Teaching data obtained from the robot's drive system or sensors as instructed by the teaching box 18 is stored in the memory 13.

During teaching, a predetermined portion of the robot body 19 is driven to a predetermined position based on instructions from the teaching box 18. Then, the displacement of the actuator at this position is detected by a rotational displacement detector (not shown), and the position of the robot hand in space (orthogonal coordinate system) is calculated from this value and stored in the memory 13 as teaching data.

During actual operation, teaching data is first read out from the memory 13 according to the operation mode. Next, according to instructions from the teaching box 18 (for example, speed instructions), point-to-point correction processing is performed on the teaching data.

The correction process between points will be explained using FIG. 6. First, route data a indicating the route direction is calculated as ideal route data from starting point position data (not shown) in the memory 13 and target position data x _f indicating the ending point position. Here, ideal route data a
corresponds to the direction vector a→ of the straight path described above. Next, the processor 11 calculates the pairwise displacement θ of the robot hand obtained by counting the output of the rotational displacement detector PE of the robot body using the counter C.
The coordinates are transformed using the multiplier/divider 15 and the table 14, and the current position data x _t in the orthogonal coordinate system is calculated.
Next, this current position data x _t and target position data x _f
and route direction data a, etc., formulas (1), (2), (3),
(4), (5) or expressions (1), (2), (3), (4), (6) are
1 and the multiplier/divider 15 to determine the speed command value u for the next sampling.

Next, this speed command value u is subjected to coordinate transformation using the processor 11, table 14, and multiplier/divider 15.
Let the speed command value θ· of each pair be the command value to the drive system 1 of the robot.

Here, the speed command value u is a positional deviation vector e which indicates the error between the current position of the hand and the ideal path →
Since it is calculated based on the speed command value θ, it is possible to move the hand with high route accuracy by using the speed command value θ·.

An example in which a robot hand is actually controlled using the above embodiment is shown in C in FIG. Various control conditions are exactly the same as in the conventional example shown in A and B in FIG. In addition, in the example shown in Figure 3, Equation (6)
The proportionality constant was set as follows. That is, k ₁ =0.6, k ₂ =0.03, k ₃ =0, and k ₄ =0.
That is, control based on equation (5) was performed. As is clear from FIG. 3, it has become possible to improve the route precision by about 3 to 4 times as compared to the conventional example shown in FIG.

〔Effect of the invention〕

According to the present invention, a vertical distance vector from a given route in a position vector calculated for each sampling period is calculated, and a route direction component based on vector information of the given route and the calculated vertical distance vector are combined. Since a tool or hand speed command vector having a direction component that decreases the distance obtained based on the distance is generated at each sampling period, the tool or hand can be moved with high path accuracy and controllability is improved. This has the effect of significantly improving the Further, according to the present invention, the tool or the hand can be moved along the path at a substantially constant speed without the speed varying depending on the vertical distance from the path.

[Brief explanation of the drawing]

1 and 2 are block diagrams showing a conventional robot hand path control method, and FIG. 3 is an example of the accuracy of path control by the conventional robot hand path control method and the robot hand path control method of the present invention. FIG. 4 is an explanatory diagram for explaining the principle of the robot hand path control method of the present invention, and FIG. 5 is a block diagram showing an example of a control device for carrying out the robot hand path control method of the present invention. Figure, 6th
This figure is a block diagram for explaining route control executed in the control device shown in FIG. 5. 1... Drive system, 11... Processor, 12...
Bus, 13...Memory, 14...Table, 15
... Multiplier/divider, 16 ... Output port, 17 ... Input port, 18 ... Teaching box, 19 ...
...The robot body.

Claims (1)

[Claims] 1. In a method for controlling the path of a robot that moves a tool or hand along a given path by controlling an actuator, an orthogonal Calculate the position vector of the tool or hand in the coordinates, calculate the vertical distance vector from the given path in the position vector calculated for each sampling period, and calculate the velocity path based on the vector information of the given path. A tool or hand speed command vector having a direction component and a direction component that decreases the distance obtained based on the calculated vertical distance vector is generated for each sampling period, and the tool or hand speed command vector is A path control method for a robot hand, comprising: calculating a control command value for the actuator by performing coordinate transformation according to a mechanism of the robot; and controlling the actuator based on the calculated control command value. 2. The robot hand path control method according to claim 1, wherein the path is a series of linear path sections. 3. In a method of controlling the path of a robot that moves a tool or hand along a given path by controlling an actuator, the displacement of the robot's actuator detected sequentially at each sampling period is used to determine the position of the tool or hand in Cartesian coordinates. A tool that calculates a position vector, calculates a vertical distance vector from the given path in the position vector calculated for each sampling period, and moves along the direction of the given path based on vector information of the given path. Alternatively, calculate a speed deviation vector for the designated speed of the hand, and calculate a path direction component of the speed based on the vector information of the given path and a direction component that reduces the distance obtained based on the calculated vertical distance vector. and further generate a speed command vector of the tool or hand corrected based on the calculated speed deviation vector at each sampling period, and coordinate transform the speed command vector of the tool or hand according to the mechanism of the robot. A path control method for a robot hand, comprising: calculating a control command value for the actuator; and controlling the actuator based on the calculated control command value. 4. The robot hand path control method according to claim 3, wherein the path includes a series of straight path sections.