JPH03257602A - Method and device for generating operation command of multi-shaft mechanism - Google Patents

Abstract

PURPOSE:To improve tracing accuracy by calculating an inter-shaft inference removing gain as the product of an inertia ration, which is calculated from the operational load of each driving shaft related to a control object shaft and an inter-shaft angle between the control object shaft and each driving shaft, and the ratio between the driving force command and driving transform constant of an angle driving shaft to be decided from the size of the driving force command according to a non-linear relation. CONSTITUTION:The relation of an expression I is established among a driving force command gammaj of driving shafts j [(j)=1-(n)], driving force command/generated driving force transform constant KTj driving shaft position thetaj, velocity thetaj and acceleration thetaj. In such a case, since a low-speed operation to require the operation tracing accuracy is handled, the second item of the right side can be ignored and since an assembly robot is normally a horizontal multi- joint type and gravity is not operated onto an arm driving shaft, the third item of the right side can be ignored as well. Therefore, a (j) axial angular velocity can be reloaded like an expression II. Then, when the expression II is reloaded like an expression III, inter-shaft inference is removed. When the expressions II and III are made equal, the inter-shaft inference is removed by applying the (j) shaft driving force command as shown in an expression IV. In such a case, the operation is executed under the condition of alphaji=KTj<-1>AjiA*ji<-1>KTi.

Description

[Detailed Description of the Invention] [Field of Industrial Application] The present invention is applicable to driving a multi-axis mechanism such as a direct drive articulated robot having a drive mechanism with inter-axis interference by speed control or position control. The present invention relates to a method and apparatus for generating a motion command for a multi-axis mechanism, which generates a motion command by removing interference between axes and is suitable for obtaining high motion locus accuracy.

[Conventional technology]

Multi-shaft mechanisms can be classified into series-shaft-connected types, series-parallel-shaft-connected types, and parallel-shaft connected types. Of these, the parallel shaft connection type can mechanically eliminate interference between shafts, but the other two types cannot mechanically eliminate interference between shafts, and the accuracy of the motion trajectory is affected by interference between shafts. Therefore, eliminating interference between axes has become an important issue. The configuration of each drive mechanism for the horizontal articulated biaxial arm is shown in FIGS. 33 to 35. The equation of motion of each drive mechanism with appropriate mass distribution is shown as follows.

(a) Series shaft connection type (two-axis motor is at the tip of the first arm) (b) Series and parallel shaft connection type (two-axis motor is a load on the same axis as the single-shaft motor) (c) Parallel shaft connection type (Both 1- and 2-axis motors are provided on the base) Here, TJ indicates the j-axis drive torque, θ-2θ.

θ indicates the shaft angle, shaft angular velocity, and shaft angular acceleration, JIJ indicates inertia (see equation (20) below), and JIJ shown in parentheses is due to the change in the angle θ2 formed by the first and second arms. It shows that the value changes. Hereinafter, a conventional example of a method for generating an operation command for a series-shaft connection type and a series-parallel shaft connection type that requires removal of inter-shaft interference will be described.

As a method for generating operation commands for a multi-axis mechanism with serially connected axes,
For example, as a conventional example (i),
Those described in Vol. 1, No. 1 (1987), pages 1 to 8 are known. According to the same document, in a mechanism consisting of multiple drive axes with small nonlinear acting forces such as centrifugal force, the drive force command for each axis is multiplied by a constant, and the results are added to generate a modified drive force command using software. A method of generating motion commands that can eliminate interference between axes by driving an axle drive device was discussed. The constant aJI (inter-axis interference removal gain) multiplied here is the driving force command/generated driving force conversion constant K
It is expressed as the product of T J ratio and inertia ratio JIJ ratio, that is, in the case of two axes α□2 = (K tz /KT,)
(Ji2/J2□), α2□=(Kdx/KTz)CJ, , /J, 1).

The shaft drive device used in this conventional example is an AC motor type direct drive motor, and there is a linear relationship between the driving force command and the generated driving force.
A constant value is used for the driving force command/generated driving force conversion constant K T J.In addition, as a conventional example (ii) of a method for generating an operation command for a multi-axis mechanism with series and parallel shafts,
As described in Publication No. 4-20988, the equation of motion is simplified as shown in (2) by mechanically adjusting the mass distribution, and the driving force command for each axis is multiplied by a constant, and the results are added to correct the equation. A motion command generation method that can eliminate inter-axis interference by generating a driving force command was discussed. Also in this conventional example, the multiplied constant (inter-axis interference removal gain) is given as the product of the ratio of the driving force command/generated driving force conversion constant of each axis, which is a constant value, and the inertia ratio. Also in this conventional example, the driving force command/generated driving force conversion constant (corresponding to the torque constant of the motor) is treated as a constant value.

In addition, as a conventional example of a motion command generation device for a multi-axis mechanism,
In the conventional example (i), a modified driving force command is generated by software, and the result is D/A converted.

A configuration is described in which the amplification is performed by a power module to drive the shaft drive device, and conventional example (ii) is to generate a modified driving force command by inputting an analog driving force (current) command divided by a resistor to another axis. A configuration was described in which the power is generated and amplified by a power module to drive the shaft drive device.

In addition, as a conventional example (ni), JP-A No. 61-169905
As described in the publication, a device was described in which a drive shaft position closed loop inverse transfer function is multiplied by a position command to cancel the response delay of the drive shaft servo system and obtain high trajectory accuracy.

[Problem to be solved by the invention]

Conventional examples (i) and (ii) above use a motor with a linear relationship between the driving force command and the generated parking force as the shaft drive device, so when setting the inter-axle interference removal gain, the drive By using a constant value as the force command/generated driving force conversion constant, we were able to achieve optimal interference removal between axes. However, recently, motors with a non-linear relationship between the driving force command and the total power (for example, step motor type direct drive motors) have attracted attention as highly energy efficient motors with a large generated torque/self-weight ratio. It has come to be used as an axis drive device that is useful for improving the performance of multi-axis mechanisms such as drives and robots by shortening the positioning time. An example of the characteristics of this motor is shown in FIG. When using such a motor as a shaft drive device to remove interference between shafts, the normal power command/generated driving force conversion constant K T
The maximum generated driving force/maximum driving force command ratio (straight line a in the figure) used as J is significantly different from, for example, the slope of the driving force command/generated driving force command curve (straight line in the figure) for a small driving force command. If continuous trajectory operation that requires motion trajectory accuracy is performed using the former inter-axis interference removal gain α4 calculated using the driving force command/generated driving force conversion constant, the inter-axis interference removal effect will be low, so There was a problem in that the precision of the movement locus of the shaft mechanism could not be improved.

In addition, the conventional example (Ichi) describes a device that multiplies the position command by the position closed loop reverse transfer characteristic to cancel the response delay of the drive shaft servo system. There is a problem in that it takes time to calculate and the sampling period becomes long, making it impossible to improve the accuracy of the motion locus of the multi-axis mechanism.

An object of the present invention is to provide a motion command method and device that optimizes the removal of interference between axes of a multi-axis mechanism using an axis drive device that has a non-linear relationship between the driving force command and the generated driving force, and improves the accuracy of the motion trajectory. There is a particular thing.

Another object of the present invention is to eliminate interference between axes, improve the response delay of a multi-axis mechanism with a simplified transfer function through simple pre-compensation, shorten the sampling period, and improve the accuracy of the motion trajectory. An object of the present invention is to provide a method and device for commanding motion to increase the number of motions.

[Means to solve the problem]

In order to achieve the above object, the present invention provides a method for generating operation commands for a multi-axis mechanism, in which a corrected driving force command for an axis to be controlled of a multi-axis mechanism in which a plurality of drive axes are linked is generated for driving each of the drive axes. The force command is multiplied by the inter-axis interference removal gain with the controlled axis to generate the total sum, and the drive device for each of the drive axes is a multi-axis mechanism having a non-linear relationship between the drive force command and the generated drive force. In the motion command generation method, the inter-axis interference removal gain is calculated from the inertia ratio obtained from the acting load of each drive axis related to the controlled axis and the axis-to-axis angle between the controlled axis and each drive axis, and the driving force command. It is characterized in that it is obtained from the size as a product of the ratio of the driving force command and the driving force conversion constant of the angle axis motion axis determined by the nonlinear relationship.

Further, in order to achieve the above object, the operation command generation device for a multi-axis mechanism of the present invention generates a modified driving force command for an axis to be controlled in a multi-axis mechanism in which a plurality of drive axes are linked, for each of the drive axes. A driving force command is multiplied by an inter-axis interference removal gain with the controlled axis and generated as the sum thereof, and the driving device for each of the driving axes is a multi-axis mechanism having a non-linear relationship between the driving force command and the generated driving force. The operation command generation device includes a central processing unit that calculates the position deviation or speed deviation of each drive shaft, and a drive force of each drive shaft in an analog quantity based on the position deviation or speed deviation determined by the central processing unit. a driving force command generating means for generating a command; an A/D converting means for A/D converting each driving force command generated by the driving force command generating means; and an A/D converting means for A/D converting each driving force command generated by the driving force command generating means; Drive force command/drive force conversion for each drive axis determined by the nonlinear relationship described above based on the inertia ratio obtained from the acting load of each related drive axis and the axis-to-axis angle between the controlled axis and each active axis, and the magnitude of the drive force command. inter-axis interference removal gain calculating means, which is calculated as a product of a constant ratio;
The A/D converted driving force command (τi) of each drive axis is determined by the axis to be controlled (
an amplification means for amplifying an inter-axis interference removal gain (α-1) with the axis (j-axis); an addition means for issuing the driving force command (τi) αJiτi) for each axis to be controlled (j-axis); The receiver is characterized in that it is equipped with a power module that outputs an output driving force command to the drive device of the axis to be controlled.

Further, another multi-axis mechanism driving force command generation device of the present invention includes:
A corrected driving force command for a controlled axis of a multi-axis mechanism in which multiple driving axes are linked is generated as the sum of the driving force commands for each of the driving axes multiplied by an inter-axis interference removal gain with the controlled axis. The drive device for each of the drive shafts is a multi-axis mechanism operation command generation device that has a non-linear relationship between the drive force command and the generated drive force, and calculates the position deviation or speed deviation of each of the drive shafts, and calculates the position deviation or speed deviation of each drive shaft. A driving force command for each drive axis is generated based on the position deviation or speed deviation, and the inter-axis interference removal gain is calculated based on the applied load of each drive axis related to the controlled axis and between the controlled axis and each active axis. It is determined as the product of the inertia ratio obtained from the axis-to-axis angle of Drive command (21
) as a modified driving force command, and a D/A converting the modified driving force command into an analog quantity by D/A converting the modified driving force command.
The present invention is characterized in that it includes an A conversion means, and a power module that amplifies the modified driving force command in analog quantity and outputs it to the active device of the axis to be controlled.

In order to achieve the above-mentioned second object, the operation command method for a multi-axis mechanism of the present invention provides a method for commanding the operation of a multi-axis mechanism in which a plurality of drive axes are linked to each other to generate a corrected driving force command for an axis to be controlled by each of the drive axes. The driving force command is multiplied by the inter-axis interference removal gain with the controlled axis to generate the total sum, and the driving device for each of the driving axes is a multi-axis motor having a non-linear relationship between the driving force command and the generated driving force. In the mechanism operation command generation method, when giving the position command or speed command for each of the drive axes, the position command is obtained by multiplying the input command by the total inverse transfer function of the position closed loop transfer function in the frequency domain, and converting it into an inverse Laplace transform. The speed command is given as an input command multiplied by a speed closed-loop transfer function in a discrete time domain and then inversely transformed.

[Effect]

As a multi-axis mechanism, an n-axis sink mechanism is generally considered. In the n-axis sink mechanism, there are interference effects and nonlinear effects between the drive axes when the drive axes are operated, and by eliminating these, the accuracy of the movement trajectory can be improved.

Continuous trajectory operation, which generally requires high motion trajectory accuracy, is driven by a multi-axis mechanism at a significantly lower generation speed than the high-speed motion that can be achieved with point control motion (point-to-point motion), resulting in non-linear effects. (Centrifugal force/Coriolica action)
The effect is small. Therefore, by generating a motion command that eliminates the interference effect, the precision of the motion trajectory of the multi-axis mechanism can be improved.

Drive axis J targeted by multi-axis mechanism (1)=1,2e...
...+n) driving force command τ4, driving force command/generated driving force conversion constant KT-, and drive shaft position θ4. Speed θ4. The following relationship holds true between the accelerations θj.

The left side shows the j-axis generated torque, the first term on the right side shows the inertia term, the second term shows the centrifugal/Coriolica term, and the third term shows the 9th term of torque compensation for bias acting force such as gravity. A a i in the first term on the right side indicates inertia. Here, as mentioned above, we are dealing with low-speed motion that requires high motion trajectory accuracy, so the second term on the right-hand side can be ignored.As assembly robots are usually horizontally articulated and gravity does not act on the arm drive axis, the right-hand side Assuming that the third term can also be ignored, the j-axis angular acceleration in equation (5) can be rewritten as in equation (6).

i=1 Here, if equation (6) can be rewritten as equation (7), inter-axis interference will be eliminated. Furthermore, AJ
t is a constant.

During a position control operation, the device control device generates a driving force command based on the deviation between the 1-position command value and the position output, and outputs a low current command to the shaft drive device as a driving force command monitor signal smoothed through a resistor. The signal is detectable as a voltage. The driving force command of each axis, 1, is amplified by the optimum inter-axis interference elimination gain (αji) for each control axis and the j-axis in the inter-axis interference elimination circuit, and is added to the NtlflN power command (6). When Equation (7) is used, inter-axis interference is removed by giving the j-axis driving force command τi as shown in Equation (8).

However, Mi α15 = KTJ-1AJIA□-1K T 1
(9) Here, α1K is called inter-axis interference removal gain.

The above method is realized in the shaft drive control device (operation command generation device) as follows. (for shaft drive). The amplifying means for multiplying the inter-axis interference elimination gain is composed of an operational amplifier, a logarithmic amplifier, etc., and its multiplication factor (inter-axis interference elimination gain) can be varied by a command from the arithmetic unit. If the driving force command/generated driving force conversion constant of the shaft drive device is not constant with respect to the magnitude of the driving force command, the driving force command is detected by A/D conversion, and the driving force command corresponding to the driving force command is detected.・Select the generated driving force conversion constant and calculate the inter-axis interference removal gain using software.

By transmitting the signal to the amplifying means, it is possible to make the inter-axis interference removal gain variable according to the driving force command, and it is possible to perform optimal inter-axis interference removal regardless of the magnitude of the driving force command.

In addition to the above-mentioned devices, the device for removing the interference between axes is one that generates a canter power command using software, sends a command to a power module via a D/A conversion means, and drives a shaft drive device. There is. In this configuration, the inter-axes interference can be removed by calculating the inter-axes interference removal gain and calculating the sum of products with the driving force command, all using software.

In addition, regarding the cancellation of the response delay of the drive shaft servo system by pre-compensation using a partial inverse transfer function, for example, for the position closed loop transfer function shown in equation (10), which is simplified by inter-axis interference removal, ( By performing second-order precompensation as shown in equation 11), the position control system bandwidth can be changed to the angular frequency:
ωnJ→2πT−”, and a sufficiently high responsiveness can be obtained in a practical sense without performing third-order precompensation.

ωnJ P・・”=(TJ S + 1) (s・+2ζ, . . .
5+. 1, ·) (here, 2πTJ-'>>ωnJ) ......(10) ......(11) [Example] Hereinafter, the first example of the present invention will be described as the first example. This will be explained using FIGS. 21 to 21. This example describes a multi-axis interference removal gain setting method for optimizing inter-axis interference removal in a multi-axis mechanism using an axis drive device that has a non-linear relationship between the driving force command and the generated driving force. This paper describes a method and apparatus for generating operation commands for a mechanism. An n-axis mechanism will be described using FIGS. 1 to 9, and a horizontal articulated robot that performs positioning in a horizontal plane using two axes will be described using FIGS. 10 to 21. 1 and 2 are block diagrams of the operation command generation device for the multi-axis mechanism of this embodiment, FIGS. 3 and 4 are block diagrams of the inter-axis interference removal device of this embodiment, and FIGS. FIG. 6 is a hardware configuration diagram corresponding to FIGS. 1 and 2, and FIGS. 7 to 9 are hardware configuration diagrams of the inter-axis interference removal device. 10th
11A is an external view of a horizontally articulated robot, FIG. 11B is a rigid body model thereof, and FIGS. The figure is a block diagram of the robot's 1st and 2nd axis motion command generation device shown in Figure 11A.
Figure 4 shows the torque command/generated torque characteristics of the 1° 2-axis direct driving motor in Figure 11A, Figure 15, and Figure 6 show the changes in the torque command/generated torque conversion constant KT due to the torque command. Figures 17 and 18 are diagrams showing the frequency characteristics of the interference position transfer gain of the robot shown in Figure 11A with and without inter-axis interference removal, and Figure 19 is the diagram showing the frequency characteristics of the interference position transfer gain of the robot shown in Figure 11A. Experimental results showing the relationship between the interference removal gain and the robot attitude angle. Figures 20 and 21 show the relationship between the inter-axis interference removal gain and the robot attitude angle for two types of motor torque command/generated torque conversion constant ratios. The calculation results shown are shown below.

First, the configuration of an operation command generation device for an n-axis long-axis mechanism according to the present invention will be explained using FIG.

This embodiment shows a motion command generation device that performs a position control unit used when a multi-axis mechanism performs a continuous trajectory operation that requires high motion trajectory accuracy. This device uses position command θ1. For speed command θ4. The position control unit that generates the proportional derivative leading type P
An example is shown in which the speed controller is configured by an ID (I-PD) control system and the speed control section is configured by a proportional control system. Since the I-PD control system takes the integral amount of angular axis position deviation and the proportional/derivative amount of the position and generates the speed command, there is no sudden change in the driving force command seen in a normal PID control system even when a step-like position command is input. It has the advantage of preventing

Further, in this embodiment, in order to improve the responsiveness of the speed control system, the speed control system is configured to be an analog control system, and is configured to perform analog speed feedback. In the figure, for each axis position command θJr given, the position control unit 101 generates a speed command θjr, the D/A converter 102 converts it into analog, and the proportional operation power command generation unit 103 generates the driving force for each axis. The command τ□ is generated, and the inter-axis interference removal unit 104 calculates the product sum of the inter-axis interference removal gain α□ given by the central processing unit (not shown) and the driving force command j (=Σ α4.τ4).
The converting section 105 converts the driving force into a driving force T, and the driving force is applied to each axis driving device of the first mechanism section 106, thereby obtaining a speed θ4 and a position output θ for each axis.

Here, the shaft drive device uses the driving force command τ- and the generated driving force TJ.
The relationship shown in equation (12) does not hold between the two, and for example, (
13) It has a nonlinear relationship as shown in equation 13).

Tノ=KTJ τノ
・・・・・・(12) TJ=KTJτJ (However, αj≠1) ・・・・・・(13) Therefore, (1
In order to treat the DC and AC motors in the same way as shown in equation 2), it is necessary to treat the driving force command/generated waste power conversion constant as shown in equation (14).

TJ = KTI (τJ) τ− By using the KTJ obtained in this way.

Inter-axis interference removal gain αat (= Kta−”AJtA t
The optimal value of t-”KTI) can be found (14
) In order to obtain K r J in the equation, it is necessary to detect the driving force command, so the driving force command τi is
By digitizing the data at step 7 and transmitting it to a central processing unit (not shown), information useful for creating optimal αj1 data can be obtained. By adopting such a configuration of the operation command generation device for a multi-axis mechanism, interference between the axes of the multi-axis mechanism can be eliminated to the greatest extent possible, regardless of the magnitude of the driving force command.

Next, another block diagram of the operation command generation device for the multi-axis mechanism of this embodiment is shown using FIG. This device uses a digital control system to generate the corrected driving force command, and sets τ4 to D.
/A conversion, and is converted into driving force T4 by the driving force conversion unit (axis driving device) 105, and the driving force is applied to each axis driving device of the mechanism unit 106. Speed detection is not performed, but position output θ- is detected, and the position control section 101 is configured by an I-PD control system. Since the detection of the driving force command also refers to the calculation results in the software sequence, there is no need for an increase in hardware, and it is easy to calculate the optimal inter-axis interference removal gain.

Figure 3 shows the configuration of the calculation contents of the inter-axis interference removal section of this embodiment.
This will be explained using FIG. FIG. 3 shows an optimal inter-axis interference removal method expressed by equation (8) in which the nonlinear terms of centrifugal, Coriolis, and bias rotational forces can be ignored. An inter-axis interference removal method is shown in which the driving force for each axis is corrected based on the product sum of each axis driving force command τi and the inter-axis interference removal gain αji. In this case, the relationship between the driving force command that allows the inter-axis interference to be removed and the corrected driving force command is given by equation (15).

i=1 αJ*=Kia AjtA it KTi
-(15)τiL indicates a nonlinear term, and the inter-axis interference removal gain α□ is the same as when the nonlinear term can be ignored.

Calculating τiL is somewhat complicated and takes time. As shown in Figure 1, when treating the driving force command as an analog quantity, τiL
A means for D/A converting the calculation result is required, which increases the hardware complexity. As shown in FIG. 2, when the driving force command is treated as a digital quantity, the hardware is simple, but there is a problem that the total amount of calculation becomes large. In the following explanation, a case where the influence of the nonlinear term can be ignored will be exemplified, but if the nonlinear term cannot be ignored, it can be realized by modifying the block diagram as shown in FIG.

Next, the specific hardware configuration of the operation command generation device for the multi-axis mechanism shown in FIGS. 1 and 2 will be explained using FIGS. 5 and 6.

FIG. 5 corresponds to FIG. 1 and shows the hardware configuration of the operation command generation device when the speed control system is configured as an analog control system. The central processing unit 1 is a central processing unit (CPU
) 2, read-only memory (ROM) 3, and random access memory (RAM) 4. The central processing unit 1 performs position detection, inter-axis interference removal gain setting, A/D, D/
Performs A conversion management, etc. The speed command θ, generated by the I-PD operation based on the position command in the central processing unit 1 is converted into analog by the D/A converter 11, and the speed control circuit 12 detects the analog speed! Based on the speed feedback signal θ4 inputted from 118 (for example, a tacho generator), a driving force command τi is generated based on a proportional operation. The driving force command τJ is sent to the central processing unit 1 via the A/D converter 10, and the driving force command of the shaft tilting device corresponding to the driving force command/generated driving force conversion constant KTJ (τi) is the driving force command.
is calculated, and based on the inertia A calculated based on the load conditions, the angle between the shafts, etc., the interference removal gain α between the shafts is calculated based on equation (9).
J, and transmits a command using an analog amplifier. The output corrected driving force command τ□ is amplified by the amplifier 16 and applied to the shaft drive device to generate the driving force TJ and drive each axis of the multi-axis mechanism. The position detector 19 is connected to each drive shaft of the multi-axis mechanism, and the position detection circuit 9 sends the signal to the temporary storage circuit 7 in synchronization with the signal from the sampling pulse timer 8, and then to the central processing unit 1. Operation management of the A/D converter 10, the D/A converter 11, and the -time storage circuit 14 is performed by the central processing unit 1 via the decoder 13. 1st
It corresponds to KTJ shown in the figure.

FIG. 6 corresponds to FIG. 2 and shows the hardware configuration of an operation command generation device when one-position control is performed by digital control. Since there is no need to feed back the speed detection amount as an analog amount, the speed detector can be omitted and a simple configuration can be achieved.

A digital detector such as an encoder is used as the position detector 19, and the central processing unit 1 performs I-PD operation, generation of driving force command τ- by proportional operation, and generation of corrected driving force command τi for removing interference between axes. The data is converted into analog data by the D/A converter 11. The corrected driving force command τJ is amplified by the amplifier 16, activates the shaft drive device, and activates the multi-axis mechanism. In this device, interference removal between axes is performed by software.

Here, a specific example of the configuration of the inter-axis interference removing section 15 used in FIG. 5 will be described using FIGS. 7 to 9. The inter-axis interference removal section 15 has a two-stage configuration of a multiplication section and an addition section. The following two methods can be considered as multiplication methods.

(I) A method of operationally amplifying the driving force command (II) A method of nonlinearly amplifying the N power command Normally, the inter-axle interference removal gain α is given as O≦αJi≦1 when the polarity of each axis piece power command is the same. It will be done.

Regarding method (1)> Since the amplification factor is determined by the resistance ratio of the operational amplifier circuit,
The following two methods can be considered as methods for making the resistance value variable based on commands sent from the central processing unit 1.

(1) A method in which a voltage variable resistor whose resistance value can be changed by applying an analog voltage obtained by D/A converting a command value is incorporated into an operational amplifier circuit.

(2) Multiple types of resistors are arranged in parallel to obtain multiple stages of amplification factors, multiplexers are connected to both ends of the resistors, and a single resistor is selected by giving a resistor selection command to the multiplexers at both ends and the amplification factor is set. A method that allows settings.

Each method will be explained using FIG. 7 and FIG. 8, respectively.

In method (1)-(1), the command (resistance value data of voltage variable resistor R2Jl) transmitted from the temporary storage circuit 14 in FIG. 5 is determined as follows. In this method, in FIG. 7, since an inverting amplifier circuit is constructed using an operational amplifier 24, each resistance value and the driving force command amplification factor α. Equation (16) holds true between.

R engineering Therefore, given αji, R2J in its variable range.

By selecting the fixed resistance R so that it is variable and creating R, 41 resistance data in the central processing unit 1, it becomes possible to obtain the inversion multiplication driving force command -α, 1τJ. Further, the virtual ground resistance 23 of the operational amplifier 24 is as follows.

Since it is known that it is appropriate to select R1R2J l / (R + R2J l ), a voltage variable resistor is also provided for the resistor 23. The voltage variable resistor is constructed using, for example, a magnetoresistive element, a photoconductive element, or the like. Further, the interference canceling unit adding unit also outputs the inverted multiplication output −α4 obtained by the multiplying unit. τi
is input in parallel for the j-axis through a resistor 25 installed in parallel, and the resultant signal is inverted and amplified by an operational amplifier 26 at an amplification factor l.

The configuration of the addition section of the inter-axis interference removal section was the same for the other systems described below.

Next, methods (1) and (2) will be explained using FIG.

The point that it is constructed using a two-stage inverting amplifier circuit is the method (
1)-(1), and the variable resistor R2J1 is selected and set by inputting the resistance selection command R to the multiplexer 29.30. Multiplexer 29.3
The amplification factor α1i that can be set by 0 is the method (1)-(1)
Generally speaking, it can be set in only a small number of steps. Therefore, the resistance value to be selected must be selected such that the difference from the optimum value calculated from equation (16) is minimized.

<About method (II)> Next, an example using a logarithmic amplifier for method (n) will be described in the ninth section.
This will be explained using figures. The driving force command τ is provided by the operational amplifier 24.
A bias voltage ■^ is applied to a portion A of the transistor Q1, and a current 11 flows through the transistor Q1 of the paired transistor 37. Since the transistor Q2 is adjusted so that a constant current 12 flows, the nonlinear characteristics (logarithmic characteristics) of the transistor at point B
A voltage expressed by equation (17) is generated.

In the operational amplifier 38, non-inverting amplification is performed by the resistors R4 and R, and the zero point output voltage becomes the value shown by equation (18).

τ 1 Vc= A Qog-o −B (A>O)
-(18) R1J 1 Here, it is desired to set it to be Vc''-αJlτi, so the resistor RIJ136 is configured with a voltage variable resistor, and the resistor is connected from the central processing unit 1 via the D/A converter 35 By instructing the value to be the value of equation (19), desired multiplication can be performed.

B-α41τ amount R-Jl = x t ・10
...(19) Next, an example in which the above-mentioned operation command generation method and device for an n-axis shaft mechanism is applied to a 4-degree-of-freedom horizontal articulated direct drive robot will be explained using Figs. 10 to 21. explain.

In FIG. 10, the robot main body 107 is configured such that each axis drive device generates a driving force in the driving direction indicated by τi, so that the robot main body 107 can be positioned at any position within the movement range in space.

The detailed structure of the robot body 107 will be described later using FIG. 11. The robot main body 107 is controlled based on the movement pattern and movement speed data of each axis stored in the robot control device 108 by the operator 110 via the teaching device 109. The robot main body 107 is driven by supplying power to each axis drive device via a (power supply) cable 114 from 108, and the movement amount and movement speed of each axis are controlled via the (signal transmission) cable 114 to the robot control device 10.
8 and is used to generate operation commands such as driving force commands. In the figure, the robot is performing a task of fitting and inserting an inserter 112 into a hole of an inserter 113 installed on a workbench 111.

It is desired that robots operate at high speed and with high precision, and the operation command generation device, which will be described later using FIGS. 12 and 13, is a robot control bag! 1108 to enable the above operations.

FIG. 11A is a simplified structural diagram of the robot.

This robot has a configuration in which the first arm 40 and the second arm 42 are directly driven by drive motors 39 and 41, respectively.
The wrist portion (spline shaft) 49 transmits the rotational power of the motor 43 to the ball screw shaft 46 via a pulley, belt, and pulley (not shown), and is rotated in the vertical direction together with a bracket 47 connected to a pole screw nut portion that is screwed thereto. driven by
Furthermore, it is configured to be rotationally driven by a motor 44 via a speed reducer 45, a pulley, a belt, a pulley, and a spline bearing (not shown). The spline shaft 49 is rotatably supported by the bracket 47, and has a tool 48 attached to its lower end. The drive motor for each axis
A speed detector is provided. Further, there are provided a specific position detector/detected object for detecting an overrun in the operating range and the origin position, and a stopper section and a collision member for stopping an overrun and further excessive operation.

Note that the wrist rotation axis is structured to rotate 360'', and is not provided with an overrun detector, a stopper, or a collision member.Furthermore, a brake for preventing slipping is provided on the wrist vertical axis.

When the active motors of all four axes of this robot are energized and generate torque TJ, there is no interference between the opening motors 43 and 44 of the wrist vertical axis and the rotation axis, as seen from FIG. 11A. On the other hand, opening motors 43 and 44 for the wrist vertical axis and rotation axis
There is shaft interference between the drive motors, the two-axis drive motor 41, and the single-axis movement motor 39. this house,
The motor torques of the active motors 43 and 44 for the wrist vertical axis and rotation axis are smaller than the torques of the one- and two-axis motors, and it is thought that simultaneous acceleration and deceleration is rare. Therefore, 1.
Focusing on the interference between the two axes, we created a two-arm rigid body model in 11B, which assumes that all wrist loads are included in the second arm.
As shown in the figure. 1. The first arm 40 and the second arm 42, which are opened and operated by the two-axis motors 39 and 41, have arm lengths a, a2, center of gravity □l Q2, concentrated mass m and 2 m, respectively.
2. Concentrated inertia around center of gravity I IZI I zZ
It is assumed that the rigidity is high enough to be handled by a rigid body model. In this case, the first arm angle θ4.1, the angle between the two wheels θ2.1,
2-axis motor torque T1. Assuming T2, the equation of motion is (
20) is shown by the formula.

...(20) Here, J□,=J□,. -J, cosθ2J□z
“Jzl”Jz□−J14cosθ2J□3 =2Jx
4=2. L,=2m2a engineering flood 2 FIGS. 12 and 13 are block diagrams of a position control motion command generation device when the motion command generation method described in this embodiment is applied to a robot. In the robot of this embodiment, no bias force due to gravity acts on the 1-axis and 2-axis motors. In addition, in low-speed operations that require precision in the movement trajectory, centrifugal and Coriolis forces are also small. Therefore, an inter-axis interference removal method that focuses on inertial torque can be applied.

FIG. 12 shows a two-axis motion command generation device corresponding to FIG. 1, and FIG. 13 corresponds to FIG. 2. Here, α1 when the self-axis inter-axis interference removal gain α□□=α22=1
We deal with the problem of finding □, α2. Speed θ and torque command τ
The relationship expressed by equation (21) holds true between them.

Here, Δ is expressed by equation (23).

Δ=(J,,J22 JL2")S"+(JttK
zKy2+J2□ to KT1-J□2KT□α, 2 to 2
-J□2KT2αzxK1)S+KTlKT2KzK2
(ICE12α21) (23) From the above, the speed output θ is expressed by equation (24).

Here, the response characteristics are determined when centrifugal and Coriolis forces are ignored. (Here, it is written as Laplace transform L(θ)20) Transfer function 6□2. When α2□ becomes zero, inter-axis interference is eliminated. Therefore, from equation (22), α, 2. α21 is (25
) can be calculated as follows.

Kr, J, □ Kr2J□, in this case, G□1. G2□ undergoes pole-zero cancellation, and becomes (26
) is simplified as Eq.

K J K T J Considering the case where the response period of the position control system is sufficiently long compared to the sampling period, and treating it as a continuous system, the position transfer function is expressed by equation (27).

θJ=L−' (PadOar) 1 a ω nJ (T*s+1)(s”+2ζJ ωnJS + (+)
nJ2)...(27) Here, the position interference transfer characteristic Po (1≠j) is measured.

We will describe the results of a comparison between cases with and without inter-axle interference removal. The driving force (torque) T generated by the 1st and 2nd axis motors and the driving force (
Example of relationship between torque) command τ (Mechanical Design Vol. 31 No. 13 P)
P3O-37 (1987-9) as stated in article P32)
is shown in Figure 14. From this, nonlinearity can be seen between T and τ. Here, τM and TM indicate their respective maximum values. Figure 15 shows the variation of the torque command/torque conversion constant KT with respect to the torque command in order to treat the torque constant as T and τ ratio. The relationship between the torque command of the two axes and the torque command of the generated torque conversion constant ratio KT/KT2 is shown in FIG. From this, it was found that there was a difference of about 30% depending on the torque command.

First, FIGS. 17 and 18 show the frequency characteristics of the position interference transfer gain for minute commands in the vicinity of zero position command with and without inter-axis interference removal. From now on, without inter-axis interference removal, position interference transfer gain P2□ at low frequency
is about 45% of the positional main transfer gains P1□, P2□, but it can be seen that it is reduced to about 10% with inter-axis interference removal.

When the inter-axis interference removal gain in this case was changed to find the combination (α, 2.α21) in which the positional interference transfer gains P1□, P21 were minimized, the actual measurement results shown in FIG. 19 were obtained. On the other hand, using the K T 1/K T Z values at the torque commands τH and O in FIG. When the change in the robot posture angle (see FIG. 11B) of α21 was calculated, the results were as shown in FIGS. 20 and 21, and it was found that the results in FIG. 21 were in good agreement with the measured values shown in FIG. 19. This indicates that optimal inter-axle interference cannot be achieved when using KTx/KTz determined by the TM/τH ratio at the maximum generated torque, which is commonly used, and inter-axle interference that meets the driving force command conditions This shows that optimal inter-axis interference removal is possible by determining the removal gain. Optimal interference between axes can be removed by using the operation command generation method that involves inter-axis interference removal gain calculation using the driving force command/generated parking force conversion constant based on the detected driving force command value as described in this example. be.

Next, a second embodiment of this embodiment will be described using FIGS. 22 to 32. This embodiment is based on the transfer characteristics of a multi-axis mechanism that has been simplified by eliminating interference between axes, and has a configuration in which the command is multiplied by the full reverse transfer characteristic or partial reverse transfer characteristic by pre-compensation that cancels out the response delay. This will widen the bandwidth of the control system, increase response, and improve the accuracy of the motion trajectory. Figure 22 is a block diagram of a position control/motion command generation device for a multi-axis mechanism in which inter-axis interference has been removed and centrifugal and Coriolis effects can be ignored, and Figure 23 is a diagram showing the frequency characteristics of the position transfer function in Figure 22. , FIG. 24 is a block diagram of a position control/motion command generation device for a multi-axis mechanism in which partial inverse transfer function pre-compensation is applied to the position control system shown in FIG. 22, and FIG.
Figure 26 is a block diagram of a position control/motion command generation device for a multi-axis mechanism in which full inverse transfer function pre-compensation is performed on the position control system shown in Figure 22; Figure 27 is a diagram showing the frequency characteristics of the position transfer function in Figure 26, and Figure 28 is a diagram showing the frequency characteristics of the position transfer function in Figure 26. Figure 29 is a block diagram of a speed control/operation command generation device for a multi-axis mechanism with position compensation, and Figure 29 is a diagram showing the frequency characteristics of the speed transfer function with and without pre-compensation of the speed control system shown in Figure 28. , FIG. 30 shows a block diagram of an operation command generation device for a multi-axis mechanism capable of switching between a position control system and a speed control system.

First, the characteristics of the position control system of a multi-axis mechanism in which interference between axes has been removed will be explained using FIGS. 22 and 23. The position control system transfer function is shown in equation (27) in the case of two axes, but the same applies to the case of multiple axes, and is shown as shown in FIG. Time constant T, -1 and ω. , has a magnitude relationship as shown in FIG. 23, and it is assumed that 2πTJ-">>ωnJ. In this case, the relationship between the angular frequency and the position transfer function gain is as follows.

ω〈ωnJ PJ, +:OdBω
. ,≦(13<2 πTJ−' PJJ:
40 d B/decω≧2 yt TJ-1PJJ
: -60d B/dec From now on, by setting the control system whose bandwidth is ωn to a bandwidth of 2πTJ""1,
The responsiveness of the control system is significantly improved. for that purpose,(
This is possible by generating the corrected position command shown in equation (29) using the pre-compensation element F shown in equation (28).

* θ,,:L-1(F(S)・■Jr)
(29) FIG. 25 shows the frequency characteristics of the position control system transfer function when this pre-compensation is applied.

Next, FIG. 26 shows a block diagram when the full inverse transfer function of the position control system is used as the pre-compensation element. The frequency characteristics of the position control system transfer function in this case are shown in FIG. From this, we found that the position control system bandwidth can be made infinite. However, there is a problem that the amount of calculation increases compared to the case of using partial inverse transfer functions, and if sufficient bandwidth can be obtained by using partial inverse transfer functions as precompensation elements, then partial inverse transfer functions can be used. This method can shorten the sampling period and is considered to be better in practice.

Next, FIG. 28 shows a pre-compensation method when speed control is performed to drive a multi-axis mechanism. By removing interference between axes, the speed transfer function of a multi-axis mechanism is simplified to a first-order lag system, so by using an upper-order advance element as a precompensation element, the bandwidth can be reduced as shown in Figure 29. Can be made infinite. For control systems with simple transfer functions, there is no problem with using the full inverse transfer function as a precompensation element.

Next, FIG. 30 shows a block diagram of a speed control/position control operation command generation device. By using a speed control precompensation element during speed control and a position control precompensation element during position control, responsiveness can be improved in either control mode.

In addition, although the above explanation was handled in continuous time form, for control systems that need to be treated as a discrete time system where the sampling period is not sufficiently small compared to the inherent synchronization of the control system, the transfer function can be described in the Z domain. , can be handled in the same way by using its full inverse transfer function or partial inverse transfer function as a precompensation element.

As described above, by performing pre-compensation using the full or partial reverse transfer characteristic of the multi-axis mechanism control system with inter-axle interference removed as described in this example, the responsiveness of the multi-axis mechanism can be significantly improved. It is possible to significantly improve the accuracy of the motion trajectory.

In addition, the behavior during adjustment during positioning of the multi-axis mechanism can be changed as shown in Figures 31 and 32, making it possible to shorten the settling time and speed up one assembly operation. sell.

The above explanation deals with the case where handling is possible in the frequency domain (S domain) where the sample period is minute compared to the control system response period, but the case where the sample period is negligible compared to the control system response period is described. If it is necessary to treat the transfer function in the discrete time domain (Z domain), the transfer function can be similarly expressed as 2 and 5=1−Z
-1) When generating the corrected position command, the same effect can be obtained by inversely Z-transforming the equation (29) as shown in the equation (30).

Book θ ar=Z-1(F (z)OJ, (z))-(
30) [Effects of the Invention] Since the present invention is configured as described above, it produces the effects described below.

(1) #i A multi-axis mechanism driven by an axis drive device that has a non-linear relationship between the power command and the generated driving force is detected by detecting the driving force command and converting the corresponding driving force command/generated driving force conversion constant. In order to calculate the inter-axis interference removal gain using
Optimal inter-axis interference removal can be achieved.

(2) Simplified by removing the interference between axes as described above! i1
By modifying the command using a pre-compensation element having full reverse transfer characteristics or partial reverse transfer characteristics in the speed control system or the speed control system, the bandwidth of the control system can be expanded with simple calculations. Responsiveness is improved and high motion trajectory accuracy can be obtained at the tip of the multi-axis mechanism.

[Brief explanation of drawings]

1 and 2 are block diagrams of an operation command generation device for an n-axis shaft mechanism according to a first embodiment of the present invention, and FIGS. 3 and 4 are block diagrams of an inter-axis interference removal section of the present invention, Figures 5 and 6 are hardware configuration diagrams corresponding to Figures 1 and 2, and Figures 7 to 9.
The figure is a hardware configuration diagram of the inter-axis interference removal unit, Figure 10 is a sketch diagram showing the operation status of the robot driven by the motion command generation device of the present invention, and Figure 11A is an external view of the horizontal articulated direct drive robot. , Figure 11B is a diagram showing the rigid body model in Figure 11A, Figures 12 and 13 are block diagrams of the 1st and 2nd axis motion command generation device of the robot shown in Figure j111A, and Figure 14 is a diagram showing the rigid body model in Figure 11A. Figure 15 is a diagram showing the torque command/generated torque conversion constant, and Figure 16 is the torque command/generated torque for each axis. Figures 17 and 18 are diagrams showing the relationship between torque conversion constant ratio and torque command, Figures 17 and 18 are diagrams showing the frequency characteristics of position interference transfer gain without and with inter-axle interference removal compensation, and Figure 19 is a diagram showing the frequency characteristics of position interference transfer gain with and without inter-axle interference removal compensation. Diagram showing the relationship between the measured value of interference removal gain and the robot posture angle, 2nd
Figure 0 and Figure 21 are diagrams showing the relationship between the robot attitude angle and the calculated value of the inter-axis interference removal gain for two types of torque command/generated torque conversion constant ratio, and Figure 22 is a diagram showing the relationship between the robot attitude angle and the calculated value of the inter-axis interference removal gain for two types of torque command/generated torque conversion constant ratio. A block diagram of the position control system of the mechanism, Figure 23 is the second
A diagram showing the frequency characteristics of the position control system transfer function shown in Figure 2,
Fig. 24 is a block diagram of the position control system shown in Fig. 22 in which the partial inverse transfer function is provided as a precompensation element, and Fig. 25 is a diagram showing the frequency characteristics of the position control system transfer function shown in Fig. 24. , Fig. 26 is a block diagram of the position control system shown in Fig. 22 with its full inverse transfer function provided as a precompensation element, and Fig. 27 shows the frequency characteristics of the position control system transfer function shown in Fig. 26. Figure 28 is a block diagram of a speed control system in which the total inverse transfer function is provided as a precompensation element, and Figure 29 is a block diagram of the speed control system shown in Figure 28 with no precompensation element distribution. Diagram showing frequency characteristics of transfer function, 3rd
Figure 0 is a block diagram of the speed control/position control system, No. 31.
Figure 32 shows the behavior of the multi-axis mechanism during positioning when the control system bandwidth is narrow, Figure 32 shows the behavior of the multi-axis mechanism during positioning when the control system bandwidth is wide, and Figure 33 shows the behavior of the multi-axis mechanism during positioning when the control system bandwidth is wide. Figures 35 to 35 are configuration diagrams of series-shaft-connected, series-parallel-shaft-connected, and parallel-shaft-connected horizontal articulated robots, and Figure 36 is a shaft drive device with a nonlinear relationship between driving force command and generated yoke force. It is a figure which shows the example of a characteristic. 1... Central processing unit, 2... Central processing unit @ (cpu
), 3...ROM, 4...RAM, 5...address bus, 6...data bus, 7, 14...-hour storage circuit, 8...sampling pulse timer, 9...・Position detection circuit, 10... A/D conversion section, 11, 20.3
5... D/A conversion section, 12... Speed control circuit, 13
... Decoder, 15... Inter-axis interference removal section, 16...
- Amplifying section, 17... Drive mechanism and multi-axis mechanism connected thereto, 18... Speed detector, 19... Position detector, 21, 22, 23° 25.27, 28, 33, 34 ．．
36... Resistor, 24.26, 38... Operational amplifier,
29, 30, 31° 32... multiplexer, 37.
・Pair transistor, 39... Direct drive motor for 1 axis, 40... 1st arm, 41... Direct drive motor for 2 axes, 42... 2nd arm, 43... Vertical axis drive Motor, 44...Wrist rotating shaft drive motor, 45...Reducer, 46...Ball screw shaft, 47...Bracket, 48...Tool, 49...Spline shaft, 101...
...Position control section, 102...D/A conversion section, 103.
... Driving force command generation unit, 104... Inter-axis interference removal unit,
105... Driving force conversion unit, 106... Robot mechanism unit, 107... Robot body, 108... Robot control device, 109... Teaching device, 110... Operator, 1 1... ·Workbench. 12 ... Inserted body, ... Inserted body. 14... Cable.

Claims (1)

[Claims] 1. A correction driving force command for a controlled axis of a multi-axis mechanism in which a plurality of drive axes are linked is added to the driving force command of each of the drive axes to remove inter-axis interference with the controlled axis. In the operation command generation method for a multi-axis mechanism in which the drive device for each drive shaft has a nonlinear relationship between the drive force command and the generated drive force, the inter-axis interference removal gain is , the inertia ratio determined from the acting load of each drive shaft related to the controlled axis, the inter-axle angle between the controlled axis and each drive axis, and the magnitude of the driving force command, and the drive of the angular drive axis determined by the nonlinear relationship. A method for generating a motion command for a multi-axis mechanism, characterized in that it is obtained as a product of a force command and a ratio of a driving force conversion constant. 2. In the method for generating an operation command for a multi-axis mechanism according to claim 1, when giving a position command or a speed command for each of the drive axes, the position command is made by applying a total inverse transfer function of a position closed loop transfer function to the input command. The multi-axis multi-axis system is characterized in that the speed command is multiplied in the frequency domain and given as an inverse Laplace transform, and the speed command is given as a result of multiplying the input command by a speed closed loop transfer function in the discrete time domain and inverse Z-transformed. A method for generating operation commands for mechanisms. 3. The corrected driving force command for the controlled axis of a multi-axis mechanism in which multiple driving axes are linked is calculated by multiplying the driving force command of each of the driving axes by the inter-axis interference removal gain with the controlled axis, and then calculating the sum of the results. The drive device for each drive shaft calculates the position deviation or speed deviation of each drive shaft in a multi-axis mechanism operation command generation device that has a nonlinear relationship between the drive force command and the generated drive force. a central processing section, a driving force command generation means for generating an analog driving force command for each drive shaft based on the positional deviation or speed deviation obtained by the central processing section; An A/D conversion means for A/D converting each driving force command, and an inter-axis interference removal gain,
an inertia ratio obtained from the acting load of each drive shaft related to the controlled axis and the interaxial angle between the controlled axis and each drive axis;
an inter-axis interference removal gain calculating means that calculates the magnitude of the driving force command as a product of the ratio of the driving force conversion constant of the driving force command of each drive axis determined by the non-linear relationship, and the A/D converted drive of each drive axis. an amplification means for amplifying the force command (τ_i) by an inter-axis interference elimination gain (α_j_i) with the axis to be controlled (j-axis) obtained by the inter-axis interference elimination gain calculation means;
The driving force command (τ_i) for each axis to be controlled (j-axis)
There is an addition means that adds the product of and inter-axis interference removal gain (α_j_i) (▲There are mathematical formulas, chemical formulas, tables, etc.▼), and an output from the addition means (▲There are mathematical formulas, chemical formulas, tables, etc.▼
) as a modified driving force command for the axis to be controlled (j-axis) and a power module for outputting it to a drive device for the axis to be controlled. 4. The corrected driving force command for the controlled axis of a multi-axis mechanism in which multiple driving axes are linked is calculated by multiplying the driving force command of each of the driving axes by the inter-axis interference removal gain with the controlled axis, and then calculating the sum of the results. The drive device for each drive shaft calculates the position deviation or speed deviation of each drive shaft in an operation command generation device for a multi-axis mechanism that has a nonlinear relationship between the drive force command and the generated drive force. , generates a driving force command for each drive axis based on the position deviation or speed deviation, and calculates the inter-axis interference removal gain by calculating the applied load of each drive axis related to the controlled axis and the controlled axis/each drive. It is obtained as the product of the inertia ratio obtained from the inter-axle angle between the axes and the ratio of the driving force command/driving force conversion constant of each drive axis determined by the nonlinear relationship from the magnitude of the driving force command, and the control target axis (j Drive command (
The sum of products (τ_i) and inter-axis interference removal gain (α_j_i)
▲There are mathematical formulas, chemical formulas, tables, etc.▼) as the corrected driving force command, and a central processing unit that calculates the corrected driving force command as D/
A multi-axis device comprising: a D/A converter that performs A conversion into an analog quantity; and a power module that amplifies the corrected driving force command in the analog quantity and outputs it to the drive device of the controlled axis. Mechanism operation command generation device. 5. A robot comprising the motion command generation device for a multi-axis mechanism according to claim 3 or 4.