CN117464661A - Control method and control device based on dynamic torque compensation - Google Patents

Control method and control device based on dynamic torque compensation Download PDF

Info

Publication number
CN117464661A
CN117464661A CN202310904028.7A CN202310904028A CN117464661A CN 117464661 A CN117464661 A CN 117464661A CN 202310904028 A CN202310904028 A CN 202310904028A CN 117464661 A CN117464661 A CN 117464661A
Authority
CN
China
Prior art keywords
load
axis
command value
shaft
speed
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202310904028.7A
Other languages
Chinese (zh)
Inventor
桃泽义秋
花冈正志
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nidec Instruments Corp
Original Assignee
Nidec Instruments Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nidec Instruments Corp filed Critical Nidec Instruments Corp
Publication of CN117464661A publication Critical patent/CN117464661A/en
Pending legal-status Critical Current

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1641—Programme controls characterised by the control loop compensation for backlash, friction, compliance, elasticity in the joints
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1612—Programme controls characterised by the hand, wrist, grip control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1638—Programme controls characterised by the control loop compensation for arm bending/inertia, pay load weight/inertia
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
    • G05B19/404—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by control arrangements for compensation, e.g. for backlash, overshoot, tool offset, tool wear, temperature, machine construction errors, load, inertia
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D13/00—Control of linear speed; Control of angular speed; Control of acceleration or deceleration, e.g. of a prime mover
    • G05D13/62—Control of linear speed; Control of angular speed; Control of acceleration or deceleration, e.g. of a prime mover characterised by the use of electric means, e.g. use of a tachometric dynamo, use of a transducer converting an electric value into a displacement

Abstract

In performing dynamic torque feedforward compensation, the computational load of calculation of the load state quantity of each shaft performed before performing inverse dynamic calculation of the calculated load disturbance torque compensation value is reduced. When the device has a plurality of axes, a transfer function from a position command value of the axis to a load side position when a nominal two-inertia system is targeted is expressed by a desired transfer function having a denominator of 4 times and a numerator of 0 times for each axis, and a load state quantity of the axis is predicted using the desired transfer function. And performing inverse dynamics operation related to the equipment according to the predicted load state quantity of each of the plurality of shafts of the equipment, and calculating a load disturbance torque compensation value of each shaft.

Description

Control method and control device based on dynamic torque compensation
Technical Field
The present invention relates to a control method and a control device for controlling an operation of an industrial robot (hereinafter, also referred to as "robot") or the like based on dynamic torque compensation.
Background
Generally, a robot has a plurality of joint axes (hereinafter, also simply referred to as "axes"), and links of the respective axes are connected in series to form a manipulator. When a robot having a decelerator on a joint shaft is moved at a high speed, the tip of a manipulator is liable to vibrate due to torsion of the joint shaft, and errors occur in the trajectory of the tip of the manipulator due to inter-shaft interference force and torsion of the joint shaft. In order to reduce the trajectory error of the manipulator tip, it is necessary to make the responsiveness of each joint axis angle (i.e., the position of the load) to the position command of each joint axis motor the same, independent of the movement of each other. Patent document 1 discloses that the drive torque of each shaft is made to approach the nominal torque of the own shaft amount by dynamic torque feedforward compensation, so that the position control responsiveness of all the shafts is the same. The control device described in patent document 1 is provided with: a load state calculation unit that calculates a state quantity (angle, angular velocity, and angular acceleration of a joint) on the load side of the joint axis torsion, using the position command value of the motor as an input, based on a transfer function from the position command value of the motor to a speed (joint angular velocity) on the load side of the joint axis torsion; an inverse dynamics calculation unit that calculates a disturbance torque compensation value on the load side, using the calculated state quantity as an input; and a current command conversion unit that converts the disturbance torque compensation value into a current command compensation value for canceling a response from the disturbance torque on the load side to the speed on the load side, and adds the current command compensation value to the current command value in the control device. The torque compensation value is a load disturbance torque compensation value represented by the sum of the torque fluctuation amount and the disturbance force from the other shaft, and the torque fluctuation amount is a change amount of the inertia moment of the shaft with the change in attitude relative to the nominal torque.
In the control method shown in patent document 1 and the like, in which the current command value compensation value is calculated using dynamic torque feedforward compensation and the calculated current command value compensation value is added to the current command value, there is a possibility that the position control performance is lost due to a reduction in the calculation accuracy when the robot repeatedly performs positioning or the like. To solve such a problem in dynamic torque feedforward compensation, patent document 2 discloses the following technique: the load disturbance torque compensation value is converted into a feedforward compensation value (for example, a speed command compensation value) for canceling the response from the load side disturbance to the load side speed in the apparatus, and the feedforward compensation value is input to an addition point located in a preceding stage of the integral compensator of the control deviation from an input position of an external command to a generation of a current command value to the motor in the control system.
However, in a control device (controller) used for controlling operations of equipment such as a robot, a plurality of axes are provided in the equipment, a position command value for a motor of each axis is generated based on an operation command from a host device, and servo control for the motor of the axis is performed based on the position command value for each axis. The portion for performing servo control of the motor further includes a circuit of a power system such as an inverter circuit for driving the motor. Therefore, in general, a motor drive control board in which circuits necessary for servo control of the motor are integrated, and a full-axis command calculation board in which circuits necessary for generating a position command value and the like for each axis are integrated are provided for each axis of the motor in the control device, and a microprocessor and the like are provided for each of the motor drive control board and the full-axis command calculation board to perform calculations necessary for control. Patent document 3 discloses the following technique: in a controller for controlling a robot having a mechanism in which a plurality of links are connected in order from a base side, in order to average an operation load when an interference torque of each shaft is calculated in a processor provided for each shaft, interference torque calculation processing for at least one shaft of the links that are relatively close to the base side of the robot is performed in a processor provided in correspondence with a shaft that drives a link that is relatively far from the base side than the link that is driven by the shaft.
[ Prior Art literature ]
[ patent literature ]
Patent document 1: japanese patent No. 4008207
Patent document 2: japanese patent laid-open No. 2022-26069
Patent document 3: japanese patent laid-open No. 8-106312
The techniques described in patent documents 1 and 2 each include: a load state calculation unit that calculates a load state amount by taking a position command value as an input, based on a transfer function from the position command value to the load side speed; an inverse dynamics calculation unit that calculates a load-side torque compensation value (load disturbance torque compensation value) for each axis, using the calculated load state quantity as an input; and a command conversion unit that converts the load disturbance torque compensation value obtained by the inverse dynamics calculation unit into a feedforward compensation value, and adds the feedforward compensation value obtained to the command value corresponding to the corresponding shaft, in order to cancel out the response from the disturbance torque on the load side to the speed on the load side. Since the feedforward compensation value of the other shaft is not used in the calculation of the command conversion section, the command conversion section can be provided on the motor drive control board for each shaft. In contrast, since the inverse dynamics calculation requires command values of all axes, it is necessary to provide an inverse dynamics calculation unit for all-axis command calculation substrates. The load state calculating unit calculates the load state quantity for each axis, but it is required to be provided in the front stage of the inverse dynamics calculating unit, and therefore it is also required to be provided on the all-axis command calculating board. In addition, in the techniques described in patent documents 1 and 2, a transfer function from the position command value to the load side position may be used instead of a transfer function from the position command value to the load side speed.
Since each shaft of the device is generally configured such that the motor drives the load via the decelerator, the load state calculation unit is configured such that, as an example, the load state calculation unit is configured to calculate a transfer function W from a position command value in the position control system of each shaft to a load side position when the nominal two-inertia resonance system is a control target r (s). In a position and velocity control system configured by assembling a control loop including 1 integral compensator into a two-inertial resonance system, if the state estimation accuracy of a state observer is sufficient and errors are ignored, a transfer function W r The number of denominators of(s) is at least 5 times and the number of numerator is at most 5 times. In the full-axis command calculation substrate, it is necessary to perform such transfer function W in the number of joint axes r Calculation of(s), the calculation load in the all-axis command calculation board becomes large. In a control device, even if a full-axis command calculation board and a motor drive control board are not separated in structure, a processor for performing calculation of control for the full axis and a motor for each axis are performed in order to disperse calculation loadThe processors of the calculation of the servo control are usually separate, in order to transfer the function W r The same applies to the calculation of(s), in which the computational load of the former processor is increased.
The present invention provides a control method and a control device for performing dynamic torque feedforward compensation, which can reduce the calculation load of calculating the load state quantity of each shaft, which is executed before performing inverse dynamic calculation of a calculated torque compensation value.
Disclosure of Invention
Technical means for solving the technical problems
According to one aspect of the present invention, in an apparatus having a plurality of axes, a control method for controlling a motor of each axis according to a position command value of each axis includes: calculating a load disturbance torque compensation value for each shaft; a step of converting, for each shaft, a load disturbance torque compensation value of the shaft into a feedforward compensation value for canceling a response from a load side disturbance input to a load side speed in the apparatus; and a step of inputting the feedforward compensation value of each shaft to an addition point in a section of the control system from the input position of the position command value to the position at which the current command value is output to the motor. In this control method, a transfer function from the position command value to the load side position of the shaft when the nominal two-inertia system is targeted is expressed by a desired transfer function having a denominator of 4 times and a numerator of 0 times, the load state quantity of the shaft is predicted using the desired transfer function, and the inverse dynamic operation with respect to the device is performed based on the load state quantities predicted for the plurality of shafts, whereby the load disturbance torque compensation value for each shaft is calculated.
In the control method according to the above aspect, in the calculation of the load disturbance torque compensation value required for the dynamic torque feedforward compensation, the load state quantity is predicted by using the desired transfer function having the denominator of 4 times and the numerator of 0 times, so that the calculation load of the load state quantity per shaft performed before the inverse dynamic calculation can be reduced.
In the control method according to one embodiment, it is preferable that the pre-compensation is performed on the position command value for each axis. The transfer function used in the pre-compensation is preferably a function obtained by dividing a desired transfer function by a transfer function from a position command value when the pre-compensation is not performed to a load side position. By performing such pre-compensation, it is possible to absorb the difference between the actual transfer function from the position command value to the load side position and the desired transfer function. When the pre-compensation is performed, it is preferable that the pre-compensation is performed before calculating the positional deviation of the motor for each axis. By performing the pre-compensation as the position feedforward compensation before the calculation of the positional deviation, it is possible to improve the accuracy of the control and suppress an increase in the computational load.
In the control method according to one embodiment, it is preferable that the addition point is located in a preceding stage of the integral compensator that controls the deviation. With this configuration, it is possible to prevent the feedforward compensation value from including an error or the integral value of the integral compensator from increasing, and to maintain the calculation accuracy and the position control performance when compensating the load-side disturbance torque. In this case, the position deviation, which is the difference between the position command value and the position of the motor, can be calculated, the speed deviation, which is the difference between the speed command value based on the position deviation and the speed of the motor, can be calculated, and the current command value can be generated using at least the speed deviation. By setting the speed command compensation value as the feedforward compensation value, the number of transfer functions used for calculation can be reduced, and thus the calculation load can be reduced, as compared with the case where the feedforward compensation value corresponding to the position command value is obtained in order to compensate for the load-side disturbance torque.
According to another aspect of the present invention, in a device provided with a plurality of axes, a control device for controlling a motor of each axis, the control device generating a current command value based on a position command value of each axis, the control device comprising: a load disturbance torque compensation value calculation unit that calculates a load disturbance torque compensation value for each shaft; and an instruction conversion unit provided for each axis, the instruction conversion unit converting a load disturbance torque compensation value of the axis into a feedforward compensation value for canceling a response from a load side disturbance input to a load side speed in the apparatus. Here, the load disturbance torque compensation value calculation unit has: a load state calculation unit that expresses a transfer function from a position command value of a nominal two-inertia system for each axis to a load side position when the nominal two-inertia system is set for each axis, using a desired transfer function having a denominator of 4 times and a numerator of 0 times, and predicts a load state quantity of the axis using the desired transfer function; and an inverse dynamics calculation unit that performs inverse dynamics calculation on the basis of the load state amounts predicted for the plurality of axes, calculates a load disturbance torque compensation value for each axis, and inputs the feedforward compensation value to an addition point located in a section from the input to the output current command value of the control device.
In the control method according to the above aspect, in the load disturbance torque compensation value calculation means for calculating the load disturbance torque compensation value required for performing the dynamic torque feedforward compensation, the load state quantity is predicted by using the desired transfer function having the denominator of 4 times and the numerator of 0 times, whereby the calculation load of the calculation of the load state quantity for each shaft performed before the inverse dynamic calculation can be reduced.
In the control device according to one embodiment, when the control device includes a full-axis command calculation board provided for a plurality of axes in common and a motor drive control board provided for each axis, the load disturbance torque compensation value calculation means may be provided on the full-axis command calculation board, and the command conversion means may be provided on the motor drive control board of the corresponding axis. With this configuration, the calculation load required for calculating the load state amount can be reduced in the full-axis command calculation board having a generally larger calculation load than the motor drive control boards.
In the control device according to one embodiment, it is preferable that a predistorter for performing predistorting on the position command value of the corresponding shaft is provided on the motor drive control board, and in this case, it is preferable that a function obtained by dividing a desired transfer function by a transfer function from the position command value when the predistorter is not provided to the load side position is used as the transfer function of the predistorter. By providing the predistorter in this way, it is possible to absorb the difference between the actual transfer function from the position command value to the load side position and the desired transfer function. When the control device includes a position controller that calculates a position deviation, which is a difference between a position command value and a position of the motor, the predistorter is preferably provided on the input position side of the position command value than the position controller. By providing the predistorter in the front stage of the position controller, the control accuracy can be improved and an increase in the computational load can be suppressed.
The control device according to one embodiment may further include: a speed controller that calculates a speed deviation, which is a difference between a speed command value based on an output of the position controller and a speed of the motor, and generates a current command value using at least the speed deviation; and an integral compensator that performs an integral operation with respect to the control deviation, in which case the feedforward compensation value is preferably a speed command compensation value that compensates for the speed command value, and is input to an addition point located at a preceding stage of the integral compensator. By setting the speed command compensation value as the feedforward compensation value, the number of transfer functions used for calculation can be reduced, and thus the calculation load can be reduced, as compared with the case where the feedforward compensation value corresponding to the position command value is obtained in order to compensate for the load-side disturbance torque.
Effects of the invention
According to the present invention, when dynamic torque feedforward compensation is performed, the load state quantity of each shaft is predicted using the desired transfer function having the denominator of 4 times and the numerator of 0 times, and therefore, the calculation load of the load state quantity calculation performed before the inverse dynamic calculation for calculating the torque compensation value can be reduced.
Drawings
Fig. 1 is a diagram showing a hardware configuration of a robot system.
Fig. 2 is a block diagram showing a robot as a device.
Fig. 3 is a block diagram showing an example of the structure of a conventional control device.
Fig. 4 is a block diagram showing the configuration of a control device according to the first embodiment of the present invention.
Fig. 5 is a block diagram illustrating the principles of the present invention.
Fig. 6 is a block diagram of a control system showing one axis.
Fig. 7 is a block diagram showing another example of the structure of a conventional control device.
Fig. 8 is a block diagram showing a control system of one axis in the control device of the second embodiment.
Detailed Description
Hereinafter, preferred embodiments of the present invention will be described with reference to the accompanying drawings. In the following, various embodiments for implementing the present invention will be described by taking a case where a device to be controlled is a multi-axis robot and dynamic feedforward compensation is performed in a control device (robot controller) that controls the robot. First, a robot system including a control device and a robot to be controlled will be described. Fig. 1 shows an example of a hardware structure of a robot system.
The robot 10 has a 3-link structure, wherein one end of a first link is connected to a base (not shown) via a joint 1, one end of a second link is connected to the other end of the first link via a joint 2, and one end of a finger is connected to the other end of the second link via a joint 3. In the robot 10, a portion constituted by the first link, the second link, and the finger is collectively referred to as a manipulator 11. The joints 1 to 3 are driven by motors 12 via reducers 13, respectively. An encoder 14 for detecting the current position of the motor 12 is mounted on the motor 12 of each shaft. For each shaft, the torque output by the motor 12 is transmitted to the joints of the shaft via the decelerator 13. In the following description, the axis related to the joint 1 will be referred to as a first axis, and the axis related to the joint 2 will be referred to as a second axis.
On the other hand, the control device 15 includes: a full-axis command calculation board 16 provided with a circuit for inputting an operation command for commanding the finger position of the manipulator 11 and the like from a host device, and calculating a position command value of each axis relative to the motor 12 and a load disturbance torque value of each axis; and a motor drive control board 17 provided corresponding to each axis of the robot 10. The motor position detection value is fed back from the encoder 14 connected to the motor 12 of the shaft to the motor drive control board 17 of each shaft. The motor drive control board 17 for each axis performs servo control of the motor 12 for that axis based on the position command value received from the all-axis command calculation board 16 and the motor position detection value fed back, but performs compensation of the load disturbance torque for that axis based on the load disturbance torque compensation value received from the all-axis command calculation board 16 when performing calculation of the servo control. Further, the motor drive control board 17 for each shaft is provided with an inverter circuit or the like for generating a drive current for the motor 12 for the shaft based on the calculation result for servo control.
Fig. 2 is a block diagram showing the structure of a dynamic system of the robot 10 as a device. In fig. 2, the robot 10 is shown as a two-inertial system by the motor 12 and the load 28 in terms of each of the first axis, the second axis, etc. A current command value is input to the motor 12 instead of the current for driving the motor 12. In the figure, when elements constituting the transfer function are expressed, the elements of the i-th axis (i=1, 2, …) are indicated by using the index i, but since the structure of each axis is the same on the frame diagram, the differences between the axes are not indicated without using the index i. On each shaft, there is a reduction ratio R g Is a reducer 13 of (1), the spring constant of the reducer 13 is K s . In the figure, J m And D m Moment of inertia and viscous drag, J, of motor 12, respectively L And D L The moment of inertia and viscous frictional resistance of the load 28, respectively. For the load 28, a load-side disturbance torque is also applied. The load-side disturbance torque includes interference from other axes and an influence of a change in the moment of inertia of the own axis with a change in attitude with respect to the nominal torque.
When the explanation is made by using the block diagram shown in fig. 2, the current outputted from the control device 15 passes through the torque constant K t The gain element 21 of (a) is converted into torque, and the torque is applied to the motor 12 by subtracting the output from the deceleration element 32 corresponding to the speed reducer 13 from the subtraction element 22. In the drawing of the figure,the output of the motor 12 is the motor speed, which is converted to a motor position (i.e., the angle of rotation of the motor 12) by the integration element 24. The output of the motor 12 is output through a speed reducing element 25 corresponding to a speed reducer, a subtracting element 26, an integrating element 30, and a spring constant K s The corresponding gain element 31 is added with the load-side disturbance torque by the addition element 27 and applied to the load 28. The load-side speed as the output of the load 28 becomes the load-side position through the integration element 29. The output of the gain element 31 is also input to the deceleration element 32. The subtracting element 26 subtracts the load-side speed from the output of the decelerating element 25.
In the load 28, the friction resistance D against the viscosity L The corresponding gain element 36 inputs the load side speed, and the value obtained by subtracting the output of the gain element 36 from the input to the load 28 by the subtraction element 37 is input to the load side position θ L While the moment of inertia J is varied L A corresponding gain element 38. Corresponding to moment of inertia J L The output of the gain element 38 of (c) is the joint angular acceleration. The value obtained by integrating the joint angular acceleration by the integration element 39 becomes the load-side speed as the output of the load 28. The angular joint acceleration in the load 28 causes interference torque to other axes. In a robot having N axes, the relationship between the joint angular acceleration of each axis and the interference torque generated by these joint angular accelerations and acting on each axis can be shown by an expression including an n×n matrix, and therefore this expression is shown as inverse dynamics 35 in the figure. The inverse dynamics 35 is input with the joint angular acceleration of each shaft, and generates an interference torque acting on the shaft for each shaft. The interference torque of each shaft is input as a load-side interference torque to the addition element 27 of the shaft.
First embodiment
A control device according to a first embodiment of the present invention will be described. First, before explaining the control device of the first embodiment, the structure of a conventional control device will be described. Fig. 3 shows a configuration when dynamic torque feedforward compensation is performed on the robot 10 shown in fig. 2 using the control device described in patent document 2. In the control device described in patent document 2, the load disturbance torque compensation value is converted into a feedforward compensation value for canceling a response from the load side disturbance to the load side speed in the plant, and the feedforward compensation value is input to an addition point in the control system, which is located in a section from an input position of an external command to output of a current command value to the motor and is located in a stage preceding the integral compensator that controls the deviation, but here, the converted feedforward compensation value is a speed command compensation value that compensates the speed command.
As described above, the control device 15 includes the full axis command calculation board 16 provided for the plurality of axes of the robot 10 in common, and the motor drive control board 17 provided for each axis of the robot 10. The full axis command calculation board 16 has a circuit for calculating a position command value or the like for each axis based on an operation command input from a host device. In the control device 15 shown in fig. 3, the full axis command calculation board 15 further includes: a load state calculation unit 41 that is provided for each axis of the robot 10 and predicts a load state amount of the axis; and an inverse dynamics calculation unit 42 that receives the load state quantity predicted values (i.e., load state predicted values) from all the load state calculation units 41, and calculates the load disturbance torque compensation value for each shaft. The load-side disturbance torque compensation value output by the inverse dynamics calculation unit 42 corresponds to the load-side disturbance torque applied to each axis of the robot 10. Assuming that there is no interference torque from another axis, if the transfer function from the position command value of the axis to the load side position of the axis in the robot 10 is set to G(s) for each axis when the predistorter is not provided, the load state calculation unit 41 provided for each axis predicts the transfer function W when the load state quantity r (s) is designed to be equal to G(s). Specifically, the load state calculation unit 41 performs dynamic compensation in the closed-loop position of each axis in the robot 10, completely eliminates interference from other axes, takes as input a position command value, and uses a transfer function W of the system from the position command value to the load side position r (s) calculating the state of the load 28, i.e., the position, speed and acceleration of the load. The inverse dynamics calculation unit 42 is a part that actually performs calculation for dynamic torque feedforward compensation, and calculates interference force from other axes and changes in posture in response to the interference forceThe torque fluctuation amount, which is the amount of change in the moment of inertia of the present shaft relative to the nominal torque, is calculated as the sum of the interference force from the other shaft and the torque fluctuation amount, and the load disturbance torque compensation value is calculated. The calculation performed by the inverse dynamics calculation unit 42 is described in, for example, patent document 1.
On the other hand, the motor drive control board 17 provided for each shaft has: a position controller 51 to which a position command value of the shaft is input and to which a motor position is fed back; a speed controller 52 that receives a speed command value for the shaft and feeds back a motor speed to generate a current command value; a speed command conversion unit 53 that outputs a speed command compensation value; and an addition element 54 that adds the output of the position controller and the speed command compensation value, and supplies the speed command value to the speed controller 52. The speed command conversion unit 53 converts the transfer function V from the input of the load-side disturbance torque to the speed command tm (s) converting the load disturbance torque compensation value of the shaft calculated in the full-shaft command calculation substrate 16 into a speed command compensation value to be added to the speed command value in order to cancel the amount of change in the load-side speed caused by the load-side disturbance torque. The transfer function V is described in patent document 2 tm Details of(s). In the control system shown in fig. 3, the dynamic torque compensation value is calculated by the load state calculation unit 41, the inverse dynamic calculation unit 42, and the speed command conversion unit 53. In fig. 3 and 4, details of a portion where a current flowing to the motor 12 is generated based on the output of the speed controller 52 are not shown for convenience of explanation.
Here, the transfer function W of the load state calculation unit 41 in the control system shown in fig. 3 r (s) study. W (W) r (s) is a slave position command value θ when the nominal two inertial systems are the control targets ref (s) to the load side position θ L Transfer function of(s). As described later, when the state observer 77 (see fig. 6 to 8) is provided in the speed controller 52, the state observer 77 receives the motor speed as an input and outputs estimated values of the respective torsion angles output from the motor speed, the load speed, and the integration element 30. The motor speed is typically measured by electricityThe pseudo-differential of the detection value of the motor angle is calculated. When a sensor for detecting the load speed or the torsion angle is provided and the load speed or the torsion angle can be directly detected, the motor speed, the load speed, and the torsion angle can be directly input to the gain 78 (see fig. 6 to 8) without using the state observer 77, and the state can be directly fed back to the subtraction element 79 (see fig. 6 to 8). In the case of thus performing direct feedback of the state, W r The number of times of the denominator of(s) is 5 times obtained by adding the number 1 of integration elements (for example, the integration element 74 in fig. 6) for performing proportional-integral control in the speed controller 52 to the number 4 of integration elements (integration elements included in transfer functions of the integration elements 29, 30, 39 and the motor 12 in fig. 2) from the current command value to the load side position in the robot 10 as the device. The number of times of denominator at this time is 5 times W r The maximum number of molecules of(s). When the state observer 77 is used in the speed controller 52 to estimate the state of the device, the state observer is typically 3 times, so W r The denominator of(s) and the number of numerator become correspondingly large. Since the load state calculating section 41 sets the number corresponding to the number of axes on the full-axis command calculating substrate 16, W r The larger number of denominators or numerator of(s) increases the computational load on the full-axis command calculation board 16.
Fig. 4 shows a control device according to a first embodiment of the present invention, which is an improvement of the conventional control device shown in fig. 3. The control device 15 of the first embodiment is different from the conventional control device 15 shown in fig. 3 in that a position FF compensation unit 56 for performing position feedforward compensation is provided on the input side of a position controller 51 in a motor drive control board 17, and that a transfer function is replaced with W r The load state calculation unit 41 represented by(s) is provided with a transfer function represented by W rh A load state calculation unit 43 indicated by(s). The position command value of each shaft is input to the position controller 51 via a position FF compensation unit 56 as a predistorter. As shown in expression (1), the transfer function W in the load state calculation unit 43 rh The denominator of(s) is 4 times and the numerator is 0 times.
W rh (s)＝b 0 /(a 4 s 4 +a 3 s 3 +a 2 s 2 +a 1 s+a 0 )(1)
In formula (1), a 0 、a 1 、a 2 、a 3 、a 4 And b 0 Is a constant. Transfer function W rh (s) is also referred to as the desired transfer function. In the present embodiment, instead of using a transfer function W reflecting the state of the actual device r (s) and the number of passes is greater than that of W r (s) a desired transfer function W to be small rh (s) to approximate W r (s) thereby reducing the computation load of the load state quantity.
In order to set the transfer function from the position command value to the load side position as W represented by formula (1) rh (s) the transfer function from the position command value to the load side position when the position FF compensation unit 56 is not provided is G(s), which is the transfer function FF in the position FF compensation unit 56 of the predistorter p (s) is determined as shown in formula (2).
FF p (s)＝W rh (s)/G(s)(2)
The transfer function W used in the load state calculation unit 43 provided in the all-axis command calculation board 16 rh Since the denominator of(s) is 4 times and the numerator is 0 times (i.e., a constant), the control device according to the first embodiment can greatly reduce the computational load on the all-axis command calculation board 16. By providing the position command FF compensation portion 56, the calculation load in the motor drive control board 17 increases, but the amount of increase in each motor drive control board 17 is smaller than the amount of decrease in the calculation load in the full-axis command calculation board 16.
FIG. 5 is a diagram illustrating the principle of the present invention, showing the slave position command value θ cmd (s) to the load side position θ L Transfer function G(s) of(s) and command value θ according to position cmd (s) the calculated load state prediction value θ est (s) relationships between(s). The position command value directly given to G(s) is used as the control command value theta cmd (s). FIG. 5A shows a conventional control device shown in FIG. 3, in which the transfer function W is defined as a denominator of 5 times or more and a numerator of the denominator times or less r (s) The load state calculation unit 41 of (=g (s)) calculates the load state prediction value θ est (s). In the system shown in fig. 5 (a), it is shown that the transfer function FF is disposed in the front stage of the load state calculation unit 41 and in the front stage of the transfer function G(s) p Fig. 5 (b) shows the states of the position FF compensation units 46 and 56 shown in(s). By selecting a transfer function FF p (s) the position FF compensation unit 56 and the transfer function G s The denominator of the transfer function of the entire block 48 composed of(s) is set to 4 times and the numerator is set to 0 times. Therefore, in the configuration shown in fig. 5 (b), if the position FF compensation unit 46 and the load state calculation unit 41 are collectively replaced as the load state calculation unit 43, the transfer function W of the load state calculation unit 43 is as shown in fig. 5 (c) rh The denominator of(s) is 4 times and the numerator is 0 times. As a result, the amount of computation by the load state computation unit 43 provided on the full-axis command computation board 16 can be significantly reduced.
Fig. 6 shows a structure of a control system of one axis in the robot system having the control device 15 shown in fig. 4. Since the axes are not specified in fig. 6, no subscript indicating which axis is used for the display of the elements constituting the transfer function. In fig. 6, an addition element 54 for adding the speed command compensation value to the speed command is provided inside the speed controller 52. Hereinafter, an example of the structure of the position controller 51 and the speed controller 52 provided on the motor drive control substrate 17 will be described with reference to fig. 6.
The position command value generated in the full-axis command calculation board 16 is transmitted to the motor drive control board 17, and is input to the above-described position FF compensation unit 56. The position command value subjected to the position feedforward compensation in the position FF compensation unit 56 is input to the position controller 51, and the motor position is fed back. The position controller 51 has: a subtracting element 61 for subtracting the motor position from the position command value output from the position FF compensating unit 56 to calculate a position deviation; and a gain element 62 for multiplying the position deviation outputted from the subtracting element 61 by a proportional gain K pp As instructions based on the positional deviation.
Input motor speed to speed controller 52A command based on the positional deviation is input from the position controller 51, and a speed command compensation value is input from the speed command conversion unit 53. The speed controller 52 has: the addition element 54 that adds the speed command value from the position controller 51 and the speed command compensation value; a subtracting element 72 that subtracts the motor speed from the output of the adding element 54 and outputs a speed deviation; multiplying the speed deviation by the speed gain K v Gain element 73 of (a); an integral element 74 of the input speed deviation; the output of the integrating element 74 is multiplied by an integrating gain K i A gain element 75 of (2); and an addition element 76 that adds the output of the gain element 73 and the output of the gain element 75. The addition element 54 is an addition point for inputting a speed command compensation value. Further, the speed controller 52 includes: a state observer 77 that inputs the motor speed; a gain 78 provided at the output of the state observer 77; and a subtracting element 79 that subtracts the output of the gain 78 from the output of the adding element 76 as a current command value. Since gain elements 73, 75 and integral element 74 are provided, the transfer function of the sum thereof is shown as (K) v s+K i ) S, the speed controller 52 performs PI (proportional integral) control. The integration element 74 and the gain element 75 constitute an integration compensator for a speed deviation as a control deviation. The speed controller 52 also performs control based on the state feedback of the state observer 77, and the subtraction element 79 subtracts the value of the state feedback from the control amount of PI control as a current command value. The current command value is converted into a current for driving the motor 12 by a current control unit 80 including an inverter circuit or the like, and is output to the motor 12. As is clear from a general control theory, if there is no integral compensation, the motor position cannot be matched with the position command value due to disturbance, and therefore, as PI control, it is necessary to provide the integral element 74 or the integral compensator in the interval from the calculation of the position deviation to the output of the current command value.
In the control device according to the first embodiment described above, the transfer function used in the load state calculating unit 43 for calculating the load state quantity for each axis in order to calculate the load disturbance torque compensation value is set to W having the denominator of 4 times and the numerator of 0 times rh (s)，The computational load in the full-axis command calculation substrate 16 can be reduced. In this embodiment, the feedforward compensation value converted from the load disturbance torque compensation value is input to the addition point of the preceding stage of the integral compensator positioned in the control system, whereby it is possible to prevent the feedforward compensation value from including an error and the integral value of the integral compensator from increasing, and to maintain the calculation accuracy and the position control performance when compensating for the load disturbance torque.
Second embodiment
Next, a second embodiment of the present invention will be described. The control device 15 according to the first embodiment is a control device to which the present invention is applied to the control device described in patent document 2, but the present invention can also be applied to the control device described in patent document 1. Before explaining the control device according to the second embodiment, a configuration of a robot system including the control device described in patent document 1 will be described with reference to fig. 7. Fig. 7 shows a configuration of a control system of one axis in a robot system including the control device described in patent document 1. The control device 15 shown in fig. 7 is the same as the control device 15 shown in fig. 3, but is different from the control device 15 shown in fig. 3 in that the load disturbance torque compensation value for each axis is converted into a current command compensation value added to the current command value. Therefore, the speed command conversion unit 53 and the addition element 54 are not provided, and instead, the current command conversion unit 57 and the addition element 58 are provided in the motor drive control board 17. The output of the adding element 58 is connected to a terminal that outputs a current command value to the robot 10 side in the motor drive control board 17. The structures of the position controller 51 and the speed controller 52 are the same as those shown in fig. 6. The current command conversion unit 57 is based on a transfer function T from the input of the load-side disturbance torque to the input of the current command value m And(s) converting the load side disturbance torque compensation value into a current command compensation value to be added to the current command value in order to cancel the load side disturbance torque. Transfer function T m Details of(s) are described in patent documents 1 and 2. The addition element 58 is an addition point provided between the speed controller 52 and the robot 10 as a device, and outputs a current command value to the speed controller 52The current command compensation value outputted from the current command conversion unit 57 is added. The current command value output from the adder 58 is converted into a current for driving the motor 12 by a current control unit 80 including an inverter circuit or the like, and is output to the motor 12. In the control device 15 shown in fig. 7, as in fig. 3, the transfer function W of the load state calculation unit 41 provided for each axis of the motor 12 in the all-axis command calculation board 16 is also the same as that shown in fig. 3 r The number of times the denominator of(s) is 5 times or more and the number of times the numerator is less than the number of times the denominator is less, and the calculation load in the full-axis command calculation board 16 is large.
Fig. 8 shows a configuration of a control system of one axis in the robot system having the control device 15 of the second embodiment. In the control system according to the second embodiment shown in fig. 8, a position FF compensation unit 56 is added to the control system shown in fig. 7, a position command value is input to the position controller 51 via the position FF compensation unit 56, and a transfer function W is set instead of the load state calculation unit 41 rh A load state calculation unit 43 indicated by(s). The load state calculation unit 43 and the position FF compensation unit 56 used in the second embodiment are the same as those used in the first embodiment. Transfer function W rh (s) as shown in equation (1), the denominator is 4 times and the numerator is 0 times, and represents the transfer function FF of the position FF compensation unit 56 p (s) is represented by formula (2). Due to transfer function W rh Since the number of denominators and numerator of(s) is small, the control device 15 of the second embodiment can reduce the computational load on the all-axis command calculation board 16.
Symbol description
10 … robot; 11 … manipulator; 12 … motor; 13 … reducer; 14 … encoder; 15 … control means; 16 … full axis command computing substrate; 17 … motor drive control substrate; 41. 43 … load state calculating unit; 42 … inverse dynamics calculation unit; 51 … position controller; 52 … speed controller; 53 … speed command converting unit; 56 … position FF compensation unit; 57 … current command converting unit; 80 … current control.

Claims (10)

1. A control method for controlling a motor of each shaft according to a position command value of each shaft in an apparatus having a plurality of shafts, the control method comprising:
calculating a load disturbance torque compensation value for each shaft;
a step of converting, for each shaft, the load disturbance torque compensation value of that shaft into a feedforward compensation value for canceling a response from a load side disturbance input to a load side speed in the apparatus; and
a step of inputting the feedforward compensation value of each shaft to an addition point in a section from an input position of the position command value to a position at which a current command value is output to the motor in a control system for each shaft,
the load disturbance torque compensation value for each shaft is calculated by expressing a transfer function from the position command value to a load side position of the shaft when the nominal two inertial system is targeted by a desired transfer function having a denominator of 4 times and a numerator of 0 times, predicting a load state quantity of the shaft using the desired transfer function, and performing inverse dynamics operation with respect to the device based on the load state quantities predicted for the plurality of shafts.
2. The control method according to claim 1, wherein,
for each axis, pre-compensation is performed on the position command value,
the transfer function of the pre-compensation is obtained by dividing the desired transfer function by a transfer function from the position command value of the shaft to the load side position when the pre-compensation is not performed.
3. The control method according to claim 2, wherein,
for each axis, the pre-compensation is performed before calculating the positional deviation of the motor.
4. A control method according to any one of claim 1 to 3,
the summing junction is located in front of an integral compensator controlling the deviation.
5. The control method according to claim 4, wherein,
calculating a position deviation, which is a difference between the position command value and the position of the motor, calculating a speed deviation, which is a difference between a speed command value based on the position deviation and the speed of the motor, generating the current command value using at least the speed deviation,
the feedforward compensation value is a speed command compensation value that compensates for the speed command value.
6. In a device provided with a plurality of axes, a control device for generating a current command value from a position command value for each axis and controlling a motor for each axis, the control device comprising:
a load disturbance torque compensation value calculation unit that calculates a load disturbance torque compensation value for each shaft; and
a command conversion unit provided for each axis, converting the load disturbance torque compensation value of the axis into a feedforward compensation value for canceling a response from a load side disturbance input to a load side speed in the apparatus,
the load disturbance torque compensation value calculation unit has:
load state calculation means provided for each axis, for expressing a transfer function from the position command value to a load side position of the axis when the nominal two inertial systems are targeted, using a desired transfer function having a denominator of 4 times and a numerator of 0 times, and for predicting a load state quantity of the axis using the desired transfer function; and
an inverse dynamics calculation unit that performs inverse dynamics calculation with respect to the plant based on the load state amounts predicted for the plurality of shafts, calculates the load disturbance torque compensation value for each shaft,
the feedforward compensation value is input to an addition point located in a section from the input of the control device to a position at which the current command value is output.
7. The control device of claim 6, wherein,
the control device has a full-axis command calculation substrate provided in common for the plurality of axes and a motor drive control substrate provided for each axis,
the load disturbance torque compensation value calculation unit is provided on the full-axis command calculation substrate,
the command conversion units are provided on motor drive control substrates of the corresponding shafts.
8. The control device of claim 7, wherein,
and a predistorter provided on the motor drive control board and configured to perform predistorting on the position command value of the corresponding shaft,
the precompensated transfer function is obtained by dividing the desired transfer function by a transfer function from a position command value of the shaft to a load side position when the precompensater is not provided.
9. The control device of claim 8, wherein,
further comprising a position controller for calculating a position deviation, which is a difference between the position command value and the position of the motor,
the predistorter is provided on an input position side closer to the position command value than the position controller.
10. The control apparatus according to claim 9, characterized by comprising:
a speed controller that calculates a speed deviation, which is a difference between a speed command value based on an output of the position controller and a speed of the motor, and generates the current command value using at least the speed deviation; and
an integral compensator that performs an integral action on the control deviation,
the feedforward compensation value is a speed command compensation value that compensates the speed command value, and is input to the addition point located in a preceding stage of the integral compensator.
CN202310904028.7A 2022-07-28 2023-07-21 Control method and control device based on dynamic torque compensation Pending CN117464661A (en)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2022-120828 2022-07-28
JP2022120828A JP2024017883A (en) 2022-07-28 2022-07-28 Control method and control device based on dynamic torque compensation

Publications (1)

Publication Number Publication Date
CN117464661A true CN117464661A (en) 2024-01-30

Family

ID=89628150

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310904028.7A Pending CN117464661A (en) 2022-07-28 2023-07-21 Control method and control device based on dynamic torque compensation

Country Status (3)

Country Link
JP (1) JP2024017883A (en)
KR (1) KR20240016203A (en)
CN (1) CN117464661A (en)

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS5571817A (en) 1978-11-20 1980-05-30 Toray Ind Inc Splittable conjugate fiber
JPH08106312A (en) 1994-10-05 1996-04-23 Fanuc Ltd Computation processing method for interference torque
JP2022026069A (en) 2020-07-30 2022-02-10 日本電産サンキョー株式会社 Control method and controller based on kinetic torque compensation

Also Published As

Publication number Publication date
KR20240016203A (en) 2024-02-06
JP2024017883A (en) 2024-02-08

Similar Documents

Publication Publication Date Title
CN109664297B (en) Vibration suppression method, system and device for robot and computer readable memory
JP3506157B2 (en) Motor position control device
JP2020078247A (en) Drive device, actuator unit, robot device, drive device control method, actuator unit control method, and robot device control method
KR101688344B1 (en) Weaving control device of multi-joint robot
EP1023973B1 (en) Robot control method and device
JP7117827B2 (en) MOTOR CONTROL SYSTEM, CONTROL METHOD FOR MOTOR CONTROL SYSTEM, AND ROBOT SYSTEM
CN114063662B (en) Control method and control device based on dynamic torque compensation
US7433758B2 (en) Control apparatus of robot
JPH10309684A (en) Compliance control method of manipulator
JP4639417B2 (en) Robot control device
JPH09212203A (en) Robot controller
CN117464661A (en) Control method and control device based on dynamic torque compensation
JPH10217173A (en) Non-interferential control device of robot
JP2000218577A (en) Robot control device
US20210138652A1 (en) Robot Control Using Model-Predictive Interaction
JP2002258922A (en) Numeric control system
JP4008207B2 (en) Robot control method and control apparatus
JP4507071B2 (en) Motor control device
JP2783321B2 (en) Control device for articulated robot
US11791751B2 (en) Motor control system, motor control method, and program
JP4552328B2 (en) Robot control device
JPH03250307A (en) Route controller for rigid body
JP2021136824A (en) Motor controller, motor control system, and motor control method
JPH06266425A (en) Controller of robot
JPH0638212B2 (en) Object position control device

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination