JPH0224076A - Positioning compensating device - Google Patents

Abstract

PURPOSE:To lighten the load applied onto a calculator and improve responsiveness by installing a means which provides the value which is obtained by adding offset in the reverse direction to the direction of a force in a certain direction onto a true aimed value, as an aimed value, and by permitting the displacement quantity of an objective article due to the force in a certain direction to be corrected in simple manners without carrying out a large quantity of calculations. CONSTITUTION:In a control system in which an external force in a certain direction such as gravity acts, an objective article which is to be positioned stops at a time point when a designated value in proportion to the difference between an inputted aimed value and the position at present is balanced with the external force. Taking account of this difference, a positioning compensation logic 103 provides the value which is obtained by adding offset onto a true aimed value, as an aimed value to be inputted. Therefore, the true aimed value and the position at present can be allowed to coincide with each other.

Description

[Detailed Description of the Invention] [Industrial Application Field] The present invention relates to a positioning control system in an object handling device such as a manipulator, and in particular compensates for positioning errors caused by forces in a fixed direction such as gravity to achieve the necessary high precision. The present invention relates to a positioning compensation device suitable for a control system that performs positioning.

[Prior Art] Gravity and external force compensating means in conventional robot manipulators are, for example, those of R-P-P.
This is particularly discussed in Chapter 6, Dynamics, from pages 152 to 188 of "Robot Manipulator" by Robert Aul, translated by Tsuneo Yoshikawa, published by Corona Publishing, July 30, 1985.

[Problems to be Solved by the Invention] Although the above-mentioned conventional technology can find exact solutions for dynamic characteristics including gravity, the amount of calculation is extremely large and the burden on the computer is large, resulting in long sampling intervals and slow response speed. The positioning device (
There is a problem in that if there is a change in the mass, etc. of the robot (robot) or the object, all the changed parameters must be input to the control device.

An object of the present invention is to provide a positioning compensation device that can perform accurate positioning in a positioning control system where forces in a fixed direction, such as gravity, act, with a small amount of calculations, while flexibly responding to changes in the situation. be.

[Means for Solving the Problem] The above object is to provide a target value to the control system in a positioning control system when a force in a fixed direction due to gravity or the like is acting on at least one of the positioning device and the object to be positioned. This is achieved by a positioning compensation device that uses a force in a constant direction and a certain offset in the opposite direction to the true target value.The offset value can also be given by a fuzzy function, and the offset Position information for determining the value of can also be obtained by using images.

[Operation] In a control system where an external force such as gravity acts in a certain direction, the positioning compensation device described above is positioned when a specified value proportional to the difference between the input target value and the current position balances the external force. Since the object stops, the stop position, that is, the current position, does not match the input target value, so take this deviation into consideration and give the true target value plus the offset as the input target value. This allows the true target value to match the current position, and the offset value can be given using a fuzzy function, and it is also possible to use images to obtain position information for determining the offset value. It is.

[Example] Examples of the present invention will be described below with reference to FIGS. 1 to 8.

FIG. 2 is a system configuration diagram of a robot showing an embodiment of the positioning compensation device according to the present invention.

In FIG. 2, 201 is a central processing unit that controls the entire robot, 202 is a D/A converter, 203 is a power amplifier, 204 to 209 are first to sixth joint drive devices of the robot having six joints, Reference numeral 210 denotes a gripping unit driving device that drives a gripping unit that grips an object; 211 to 216 denote first to sixth joint angle detectors provided at the first to sixth joints; and 217, first to sixth joint angle detection units. This is a digital input device that inputs the detection results of the devices 211 to 216 to the central processing unit 201.

With this configuration, the first to sixth
Joint drive devices 204 to 209 and grip drive device 21
The output value to 0 is converted into an analog signal by the D/A converter 202, and then passes through the power amplifier 203 to drive the first to sixth joint drive devices 204 to 209 and the grip portion drive device 210. Note that the same reference numerals or symbols indicate the same or corresponding parts throughout the drawings.

FIG. 3 is an external view of the manipulator section of the robot shown in FIG. 2. In FIG. 3, 301 to 306 are six first to sixth joints of the manipulator section, and 307 is a gripping section. 1st to 6th joints 301 of the manipulator part in FIG.
306 has built-in first to sixth joint drive devices 204 to 209 and various signal lines for rotating the manipulator around each axis, and the first to sixth joints 301 to 306 have built-in devices that detect each joint angle. The first to sixth joint angle detectors 211 to 216 are attached. The gripping portion 307 of the manipulator portion is driven by a gripping portion driving device 210. The central processing unit 201 controls the positioning of the reference position of the gripping portion 307 at the tip of the manipulator.

FIG. 4 is a model configuration diagram of one joint of the robot shown in FIG. 2. In FIG. 4, 401 is a pulley and 402 is a mass point. Next, in order to explain the configuration of the control system, in order to simply explain the principle and concept of the positioning compensation device according to the present invention, we will start by using a model in which only one joint is taken out, as shown in FIG. The case will be explained below. One joint drive device 204 in FIG.
is driven to wind up the mass point 402 against gravity. The position at this time is detected by the joint angle detector 211, digitized by the digital input device 217, and input to the central calculation bag @201. The central processing unit 201 outputs a designated value based on this input, and the designated value is converted into an analog signal by a D/A converter and drives the joint driving device 204 via the power amplifier 203.

Next, we will formulate this control system. Now joint drive device 2
04 target value is θ1, and the current angle detected by the joint angle detector 211 is θ. and set the gain of the power amplifier 203 to K.
a, the voltage applied to the joint drive device 204 V Pw
M is given by the following equation.

VpwM=Ka(θ, −〇, ) (
1) Next, the voltage v pwM in the joint drive device 204
The relationship between the current i and the current i is given by the following equation.

Here, is a back electromotive force constant, ω is the angular velocity of the joint drive device 204, R is the internal resistance of the joint drive device 204, and L is the inductance of the joint drive device 204. Next, when the output torque of the joint drive device 204 is τ and the torque constant is Kt, the following equation is given.

τ=Kti (3) Also, the sum of the moment of inertia of the joint drive device 204 and the moment of inertia of the pulley 401 is J, the viscous resistance constant of the joint drive device 204 is B, the mass of the mass point 402 is m, the radius of the pulley 401 is r, If the gravitational acceleration is g, a first-order equation of motion is obtained.

Also ω and θ. The relationship is as shown in the following equation.

The following equation is obtained by Laplace transform of the above equations (1) to (5).

V pwM=Ka (θ'l-13'0)
(1)'v pyH=6ω'+Ri'+Ls i
(2) 'τ = Kti'
(3)'τ=J S (Ll'+B (ll
'+Illgr(4)'ω=Sθ'.
(5)'Here, state variables that have been Laplace transformed are marked with a dash.

Here, from equations (1)′ to (5) 1, VPVM+ τ , 1
, ω and θ′, and θ′. To find the relationship, use the following equation as θ', substitute step input (=1/s), and obtain θ when sufficient time has passed. is given by the following equation using the final value theorem of Laplace transform.

From this equation (7), we can see that Rmgr for the input
A deviation of /KaKt remains. The purpose of the positioning compensation device according to the present invention is to bring this deviation closer to O and perform more accurate control.

FIG. 5 is a block diagram of the control system shown in FIG. 4.

In Fig. 5, the part excluding the area inside the dashed dotted line is
)' to (5) 1 are expressed in a block diagram, and 5
02 is θ', 503 is V pwx, 504 is i, 50
5 is τ506 is ω, and 507 is θ'. , and 508 represents the torque due to gravity in the third term on the right side of equation (4)1.

The area inside the one-dot chain line 508 in FIG. 5 shows the logic of positioning compensation according to the present invention, and 509 is a holder. The compensation logic 508 according to the present invention does not directly give θ' in equation (6) as the target value 502, but
True target value 501 (θ', r) and current angle 507 (θ
'. ) is added to the target value as an offset, and the value is set as the target value 502 (θ', ). In other words, the target value 502 (θ'1) is reset taking into account the deviation.

Next, we will formulate the concept of this positioning compensation logic 508. Let the true target value 501 be O'ir.

Since the holder 509 of the compensation logic 508 and the feedback around the holder are replaced by an integral term 1/s in a continuous system, the target value 502 (θ',) is given by the following equation.

θ′, =-! -(θir-〇', > (8) Expression (8) is (
6) By substituting into the equation and summarizing θ'0, the following equation is obtained.

Here, a step input (=1/S) is substituted as θ ir, and O8 when sufficient time has elapsed is given by the following equation based on the final value theorem of Laplace transform.

= 1
(10) As shown above, the deviation was 0 even without considering dynamic characteristics in the control logic. The concept described above for the case of only one joint can be applied to the entire system of the robot shown in FIG.

FIG. 1 is a block diagram of a robot control algorithm showing an embodiment of the positioning compensation device according to the present invention. FIG. 1 shows a block diagram in which the concept of the positioning compensation logic 508 in FIG. 5 described above is applied to the entire system of the robot in FIG. It represents internal processing. 1, 101 is the target value generation logic, 102 is the Laplace inverse transform, 103 is the compensation logic for positioning, and 105-
110 is the target value of the posture/position of the gripping part 307 at the tip of the manipulator, 111 to 116 are the first to sixth joint angle values, 117 to 122 are the first to sixth joint command values, and 123 to 1
28 is the current joint angle of the first to sixth joints.

With this control system configuration, the target value generation logic 101 in the central processing unit 201 causes the grip portion 307 at the tip of the manipulator to
Target values 105 to 110 of the posture and position of are determined. This posture/position target value 105 to 110 is the posture target value 1
05 to 107 roll angle target value 105, pitch angle target value 106, yaw angle target value 107, and position target value 1
08 to 110, target value 108 for the position in the front and back direction, target value 109 for the position in the left and right direction, target value 11 for the position in the vertical direction
The target value 110 of the vertical position including 0 is the compensation logic 103 corresponding to the positioning compensation logic 508 of FIG.
will be corrected. These target values 105 to 110 are received and inversely transformed by Laplace inverse transformation 102 to obtain joint angle target values 111 to 111 of the first to sixth joints 301 to 306 of the manipulator.
Find 116. These first to sixth joint angle target values 11
1 to 116 to the first to sixth joints 301 of the manipulator
The current joint angles 123 to 128 detected by the first to sixth joint angle detectors 211 to 216 of ~306 are subtracted, and the first to sixth joint drive devices of those first to sixth joints 301 to 306 Command values 117 to 122 to 204 to 209 are output from the central processing unit 201. Also, the central processing unit 20
1st to 6th joints 301 of the manipulator output to 1
In addition to being used for the above feedback subtraction, the current joint angles 123 to 128 of 306 are used for the Laplace positive transformation 104
Perform a positive transformation to obtain the current angle 507 (θ') in Figure 5.
Obtain the vertical position 129 of the manipulator corresponding to
This value is fed back to compensation logic 103. This feedback corrects the vertical displacement of the gripping portion 307 at the tip of the manipulator due to gravity.

FIG. 6 is a block diagram of a robot control algorithm showing another embodiment of the positioning compensation device according to the present invention. FIG. 6 shows an example of a positioning compensation device that corrects a target value using fuzzy control using a fuzzy function, 601 is a fuzzy rule, and 602 to 607 are posture/position target values 101 determined by the fuzzy rule 601.
It is a correction value to ~110. The difference between FIG. 6 and FIG. 1 is that the target values 105 to 110 of the posture/position between the target value generation circuit 101 and the inverse conversion 102 are corrected using the current vertical position 129 of the manipulator in FIG. Compensation logic 103
Instead of correcting the target value 110 of the position in the vertical direction with This is done by steps 602-607. The hardware system configuration of this control system is the same as that of the embodiment shown in FIG. 1, and is the robot system shown in FIGS. 2 and 3.

Next, the fuzzy rule 601 having this configuration will be explained. Assume that the following three Plant's laws are adopted as this fuzzy model.

v:sfn 工＋Y' is Big then Y'H
= f : L' : if 5A "Is Small
and Z is Bigthen Yj= f
7 L:l: if % is Small and Z
is Small then y:= f3. (
i=1-6) Here, X is the gripping part 3 at the tip of the manipulator
Target value of longitudinal position of 07 108. Y has a target value of 109 for the horizontal position.2 is a target value of 110 for the vertical position.

Also, the consequent function f:lft+f? is a function based on an empirical rule, and when i=1, the correction value 602 for the posture roll angle target value 105 of the grip part 307 at the tip of the manipulator, i=2
and the correction value 603 for the pitch angle target value 106, i=3
, the correction value 604 for the yaw angle target value 107, the correction value 605 for the longitudinal direction position target value 108 for i=4, i=
5, a correction value 606 for the horizontal position target value 109,
When i=6, the correction value is 10 for the vertical position target value 110.
7 shall be given. Next, let us define the membership function of the fuzzy set rBigJ in 5A as A, , Jx2+
The membership function of the fuzzy set r S mall J in y2 is At, the fuzzy set rBig in Z
Assuming that the membership function of J is A3 and the membership function of the fuzzy set rsmallJ in Z is A4, the correction value 60 for each target value of 105 to 110 for each i=1 to 6.
The correction value mH of 2 to 607 (i=1 to 6) is given by the following equation.

FIG. 7 is a block diagram of a robot control algorithm showing still another embodiment of the positioning compensation device according to the present invention. FIG. 7 shows an example of a positioning compensation device that corrects a target value using image information, 701 is an image information analysis logic, 702 is an image information analysis logic, and vertical positions 702 and 703 from the image information analysis logic 701 are image signals. be. FIG. 8 is a system configuration diagram of the robot shown in FIG. 7. 8th
In the figure, 801 is an image input device, and 802 is an image processing device. The difference between FIG. 8 and FIG. 2 is that the image input device 8
01 and the image processing device 802, the appearance of the grip portion 307 at the tip of the manipulator is similar to that of the image input device 8.
01, and after image processing by the image processing device 802, the central processing unit 201 handles the grip part 3 at the tip of the hemanipulator.
07 as a signal at the vertical position 703. At this time, the difference between the block diagram of the control algorithm inside the central processing unit 201 in FIG. 7 and FIG. 1 is that in FIG. The current vertical position 129 of the gripping part 307 at the tip of the manipulator obtained by performing positive transformation 104 from the current joint angles 123 to 128 is used, whereas in FIG. Instead of the position 129, the current vertical position 702 of the gripping part 307 at the tip of the manipulator is obtained by analyzing the image signal 703 from the image processing device 802 using the image information analysis logic 701, as corresponding to the vertical position 129. compensation logic 1
03 as a feedback signal. According to this embodiment, since the current position in the vertical direction of the gripping section 307 at the tip of the actual manipulator is fed back to the compensation logic 103, it is possible to cope with deflection of the arm of the manipulator section, etc.

According to the embodiments described above, by performing correction using the positioning compensation means in the control system of the positioning device, it is possible to correct the displacement of the gripping portion at the tip of the manipulator due to gravity or the like by manipulating the target value. Note that the above embodiment describes a positioning control system for a robot system where gravity acts.
The present invention is similarly applicable to a positioning control system in which a force in a fixed direction acts on at least one of a positioning device and an object to be positioned.

[Effects of the Invention] According to the present invention, in a positioning control system in which a force in a fixed direction acts on at least one of a positioning device and an object to be positioned, it is possible to easily control the force in a fixed direction without performing a large amount of calculations. Since the amount of displacement of the object can be corrected, the design of the control system is simplified, and the responsiveness is improved by reducing the load on the computer, which has the effect of allowing highly accurate positioning control.

[Brief explanation of the drawing]

FIG. 1 is a block diagram showing an embodiment of the positioning compensation device according to the present invention, FIG. 2 is a system configuration diagram of FIG. 1,
Fig. 3 is an external view of the manipulator, Fig. 4 is a model configuration diagram of only one joint in Fig. 1, Fig. 5 is a block diagram of Fig. 4, and Fig. 6 is a block diagram showing another embodiment according to the present invention. FIG. 7 is a block diagram showing still another embodiment of the present invention, and FIG. 8 is a system configuration diagram of FIG. 7. 101... Target value generation logic, 102... Laplace inverse transformation, 103... Positioning compensation logic, 104... Laplace forward transformation, 105-110... Target value of posture/position, 111-116...・Joint angle target value, 117-122
...Joint command value, 123-128...Current joint angle, 129...Current vertical position, 201...Central processing unit, 202...D/A converter, 203... Power amplifier, 204-209...Joint drive device, 21
0...Gripper driving device, 211-216...Joint angle detector, 217...Digital input device, 301-30
6... Joint, 307... Grasping part, 508... Positioning compensation logic, 601... Fuzzy rule, 701...
. . . Image information analysis logic, 702 . . . Current vertical position, 703 . . . Image signal, 801 . . . Image input device, 802 . Agent Patent Attorney Tadashi Akimoto Minoru \, □Mata 21Q Figures 301-306 1st to 6th joints 307z No.6 Figure 6

Claims (1)

[Claims] 1. In a positioning control system where a force in a fixed direction is acting on at least one of the positioning device and the object to be positioned, the direction of the force in the fixed direction with respect to the true target value. A positioning compensation device characterized by comprising means for giving a target value obtained by adding an offset in the opposite direction to the above. 2. The positioning compensation device according to claim 1, wherein the offset value is given by a fuzzy function. 3. The positioning compensation apparatus according to claim 1, wherein the positional information of the object for determining the offset value is obtained by using an image. 4. The positioning compensation device according to claim 1, wherein the force in a certain direction is gravity.