JP2012130214A - Motor control device and motor control method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a motor control device and a motor control method which can correct an elastic deformation of a belt in a motion mechanism including a low rigidity belt as power conveyance means, thereby making it possible to reduce a deviation between trajectory and locus.SOLUTION: A position directive is put through a prefilter 3 and, thereafter, a deviation between the position directive and a position feedback is taken, which is then put through a position control unit 2 to calculate a speed directive. A deviation between the speed directive and a speed is taken, which is then put through a speed control unit 7 to calculate a torque directive. Based on this torque directive, a motor is driven through a power converter in a torque control unit 8. Also, a load torque is estimated from the torque directive and a motor speed by a load torque observer 9, and this load torque is then multiplied by a compensatory gain necessary to compensate for an elastic deformation according to the locus of an action point of a motion mechanism to find a compensation amount. This compensation amount is put through a vibration suppression filter 13 before being added to a location deviation.

Description

  The present invention relates to a motor control apparatus and control method using a motion mechanism including a belt as force transmission means as a load.

  A motion mechanism including a belt as a force transmission means is employed in a machine such as a robot. In a motor using such a motion mechanism as a load, it is required to improve the accuracy of position control of the action point of the motion mechanism. For example, FIG. 4 is a configuration example of an articulated robot using two motors. The motor M1 drives the arm 101 via the belt B1, and the motor M2 causes the tip portion (action point) of the action portion 103 provided at the tip of the robot arm 102 to follow the predetermined trajectory via the belt B2. Move to. In such a robot, when the motors M1 and M2 are driven to move the arms 101 and 102, an external force is applied to both the motors M1 and M2, and the trajectory of the tip of the action part 103 of the robot deviates from the original trajectory. May end up. This is because one shaft on which the belt is applied is affected by the force from the other shaft, and a position error corresponding to the elastic deformation of the belt B2 occurs. If the trajectory of the action part 103 of the robot is largely deviated from the original trajectory in a transport robot or the like, there is a possibility of colliding with other surrounding objects when the object is loaded. Therefore, it is necessary to make the amount of deviation between the trajectory and the trajectory as small as possible. Therefore, conventionally, the motor is driven at a speed that is not affected by external force.

  As an example in which the influence of the elastic deformation of the mechanical system is corrected, there is one disclosed in Patent Document 1 (Japanese Patent No. 3654475) in which the torsion of the speed reducer is corrected. In this example, the motor current is detected, a twist correction amount is calculated based on the relationship between the motor current measured in advance and the twist, and the position command is corrected.

  Further, as an example in which the elastic deformation of the positioning control device is corrected, there is one shown in Patent Document 2 (Japanese Patent No. 3982308). In this example, based on the estimated disturbance force estimated in the disturbance estimation unit, the amount of elastic deformation remaining in the positioning mechanism when the control target reaches the target position is estimated, and the amount of elastic deformation is added to the target position. Positioning control is performed using the position as a new target position, and after the control target reaches the new target position, position control is performed to return the amount of elastic deformation.

Japanese Patent No. 3654475 Japanese Patent No. 3982308

  However, the technique disclosed in Patent Document 1 only corrects the twist corresponding to the torque generated by the motor. Patent Document 1 does not suggest a technique for compensating for elastic deformation of the belt due to disturbance. Moreover, since the technique of patent document 2 aims at the improvement of the positioning accuracy of a control object, it is compensation centering on the correction at the time of positioning. Therefore, Patent Document 2 does not compensate for the locus of the position to be controlled in the process of positioning. In addition, since the techniques described in Patent Documents 1 and 2 do not take into account vibrations of the mechanical system, it is not possible to suppress mechanical vibrations when elastic deformation compensation is added.

  An object of the present invention is to solve such a problem, and by correcting the elastic deformation of the belt in a motion mechanism including a belt having low rigidity as a force transmission means, the deviation between the trajectory and the trajectory is achieved. It is an object of the present invention to provide a motor control device and a control method capable of reducing the amount.

  Another object of the present invention is to provide a motor control device and a control method in which the trajectory accuracy is not greatly reduced even if the motor is driven at high speed without exciting vibration of the motion mechanism.

  The motor control device of the present invention includes a position detection unit, a speed calculation unit, a position control unit, a speed control unit, a torque control unit, and a compensation amount calculation unit. The position detection unit detects the position of the motor (rotational position of the rotor) in order to control a motor loaded with an exercise mechanism including a belt as a force transmission unit. The speed calculation unit calculates the speed of the motor (rotational speed of the rotor). The position control unit performs position control by outputting a speed command so that the position feedback indicating the position of the motor fed back from the position detection unit matches the position command. The speed control unit performs a speed control by outputting a torque command so that the speed fed back from the speed calculation unit matches the speed command. The torque control unit performs torque control based on the torque command. The compensation amount calculation unit calculates a compensation amount to be added to the position deviation input to the position control unit so as to reduce a deviation amount between the locus of the action point of the motion mechanism and the locus of the action point. The compensation amount calculation unit includes a load torque estimation unit, a compensation gain storage unit, and a multiplication unit. The load torque estimating unit estimates a load torque including a torsion torque that is calculated by the motion mechanism based on the speed calculated by the speed calculating unit and the torque command. The compensation gain storage unit stores, for each of a plurality of consecutive position command sections, a compensation gain that has been determined by a prior test to reduce the deviation amount. The multiplication unit multiplies the compensation gain acquired from the compensation gain storage unit by the position command as input and the load torque to calculate the compensation amount.

  In the present invention, the load torque is estimated from the torque command and the motor speed. Then, this load torque is multiplied by a compensation gain for elastic deformation compensation according to a predetermined trajectory of the action point of the motion mechanism to obtain a compensation amount. Since the load torque includes the torsional torque generated by the elastic deformation of the belt in the motion mechanism, the compensation amount obtained by multiplying this load torque by the compensation gain is optimal for compensating the elastic deformation. Become. Therefore, in the present invention, by adding this compensation amount to the position deviation, the deviation amount between the trajectory and the locus of the action point of the motion mechanism caused by the elastic deformation is reduced. In particular, in the present invention, since the compensation gain is determined for each of the plurality of position command sections by a prior test, an appropriate compensation amount can be obtained and the deviation amount can be appropriately reduced.

  It is preferable to provide a first filter (vibration suppression filter) that removes a vibration component that finely vibrates the motion mechanism included in the compensation amount. Further, it is preferable to further provide a second filter (pre-filter) that removes a vibration component that finely vibrates the motion mechanism included in the position command. When such a filter is used, it is possible to suppress fine vibration at the action point. Note that the first and second filters (vibration suppression filters) can be configured by notch filters, for example.

  In the motor control method of the present invention, a compensation gain necessary to reduce the deviation amount is determined by a prior test for each of a plurality of consecutive position command sections. Further, the load torque including the torsion torque is estimated based on the speed calculated by the speed calculation unit and the torque command. Then, the compensation gain corresponding to the position command section determined based on the position command is multiplied by the load torque, the compensation amount for reducing the deviation amount is calculated, and the compensation amount is calculated as a feedback value indicating the position command and the motor position. Is added to the position deviation.

It is a figure which shows the structure of the system containing the structure of an example of embodiment of the motor control apparatus of this invention. FIG. 5 is a diagram showing an example of the relationship between the motor speed and the position accuracy of the action point (tip position accuracy) when the compensation mechanism is not added to the position deviation when the motion mechanism shown in FIG. 4 is driven. It is a figure which shows the relationship between the speed of a motor at the time of adding a compensation amount to a position deviation, and the position accuracy (tip position accuracy) of an action point. It is a figure which shows an example of the movement mechanism of the articulated robot using two motors.

  Hereinafter, an example of an embodiment of a motor control device of the present invention that performs the method of the present invention will be described in detail with reference to the drawings. FIG. 1 shows a system including a configuration of an example of an embodiment of a motor control device of the present invention. This system is used for controlling the motor M2 shown in FIG. Therefore, the mechanical system MS driven by the motor M2 is an exercise mechanism including the belt B2, the arm 102, and the action part 103 of FIG. In this system, an encoder E is provided as a position detection unit that detects the position of the motor M2 to be controlled. The output of the encoder E is position feedback indicating the position of the output shaft of the motor M2. The speed calculation unit 1 is configured to calculate the speed of the motor M2 based on the output of the encoder E, and the output of the speed calculation unit 1 is speed feedback. The speed feedback indicates the speed of the output shaft of the motor M2. The position controller 2 is configured to control the position by outputting a speed command so that the position feedback indicating the position of the motor M2 fed back from the encoder E as a position detector matches the position command. . In the present embodiment, the position command given from the host controller is given to the subtracting means 4 through the pre-filter (second filter) 3. The pre-filter 3 is configured by a vibration suppression filter such as a notch filter that removes a vibration component that finely vibrates the motion mechanism included in the position command. The subtracting means 4 takes the deviation between the position feedback indicating the position of the motor M2 output from the encoder E and the filtered position command and outputs it as a position deviation.

  The position deviation is input to the adding means 5. The adding means 5 adds the position deviation and a compensation amount, which will be described later, and outputs the result to the position control unit 2. The position control unit 2 outputs a speed command to the subtracting unit 6 based on an added value obtained by adding the position deviation and a compensation amount described later.

  The subtracting means 6 obtains a deviation between the speed command and the rotational speed of the output shaft of the motor M2 calculated by the speed calculating section 1 as a speed deviation and outputs it to the speed control section 7. The speed control unit 7 performs speed control by outputting a torque command so that the speed fed back from the speed calculation unit 1 and the speed command coincide with each other by proportional-integral control. The torque command is input to the torque control unit 8, and the torque control unit 8 performs torque control of the motor M2 based on the torque command.

  In the present embodiment, the load torque observer 9 is used as a load torque estimation unit that estimates the load torque received by the motor M2. The load torque observer 9 is configured to estimate load torque including friction torque and torsion torque based on the torque command and speed. The details of this type of observer 9 are substantially the same as those of the disturbance torque estimator shown in “Electronics” D110, Vol. 11, page 1126 to page 1132, published in 1990.

  The load torque estimated by the load torque observer 9 is input to the compensation amount calculation unit 10. The compensation amount calculation unit 10 is configured to reduce the deviation amount between the trajectory of the tip (action point) of the action unit 103 (FIG. 4) of the predetermined motion mechanism (MS) and the locus of the action point. The compensation amount to be added to the position deviation input to is calculated. The compensation amount calculation unit of the present embodiment includes a multiplication unit 11 and a compensation gain storage unit 12. The multiplication unit 11 multiplies the compensation gain acquired from the compensation gain storage unit 12 by using the position command as input and the load torque estimated by the load torque observer 9 to calculate the compensation amount. In the present embodiment, the compensation amount is passed through the vibration suppression filter (first filter) 13 to remove the vibration component that finely vibrates the motion mechanism included in the compensation amount, and the compensation amount is given to the adding means 5. ing.

  In the compensation gain storage unit 12, a compensation gain necessary for reducing the amount of deviation between the trajectory of the action point of the predetermined motion mechanism and the trajectory of the action point is previously tested for each of a plurality of continuous position command sections. I remember what I have determined. FIG. 2 shows an example of the relationship between the speed of the motor M2 and the position accuracy of the action point (tip position accuracy) when the compensation mechanism is not added to the position deviation when the motion mechanism shown in FIG. 4 is driven. FIG. 3 shows the relationship between the speed of the motor M2 and the position accuracy (tip position accuracy) of the operating point when the compensation amount is added to the position deviation under the same conditions as in FIG. Here, the position accuracy of the action point (tip position accuracy) indicates the amount of deviation between the trajectory and the locus of the action point of the motion mechanism, and if there is no deviation, the position accuracy line is linear. 2 and 3, the position accuracy (tip position accuracy) of the action point is obtained by measuring the locus of the action point using a known position measuring device that measures the position of the moving object using laser light. Obtained. In FIG. 2, the reason why the position accuracy (tip position accuracy) of the operating point varies greatly is that the belt B <b> 2 (FIG. 4) is included in the motion mechanism and this belt B <b> 2 expands and contracts. If there is no expansion and contraction of the belt B2, the position accuracy (tip position accuracy) of the action point hardly changes.

  In the present embodiment, as shown in FIG. 3, the compensation necessary to reduce the deviation amount between the trajectory of the action point and the trajectory of the action point for each of a plurality of consecutive position command sections S1 to S3. The gains G1 to G3 are determined in advance by a preliminary test. The compensation gains G1 to G3 measure the position accuracy (FIG. 2) of the action point when the compensation amount is not added during the preliminary test, and the position command sections S1 to S3 to be compensated based on the measurement result. And a compensation gain suitable for the specified position command sections S1 to S3 is determined. As shown in FIG. 3, when the position command sections S1 to S3 are long, the compensation accuracy is inevitably deteriorated. How to specify the position command sections S1 to S3 and what value to set the compensation gain may be determined according to the use of the motion mechanism. Incidentally, the compensation gain capable of obtaining the position accuracy of the action point shown in FIG. 3 can be sufficiently used for controlling the motor used for driving the motion mechanism of the transfer robot.

  In the present embodiment, the position command is passed through the pre-filter 3, and then the deviation between the position command and the position feedback is taken and passed through the position control unit 2 to calculate the speed command. Then, the deviation between the speed command and the speed is taken and passed through the speed control unit 7 to calculate the torque command. Based on this torque command, the motor is driven through the power converter in the torque control unit 8. Further, the load torque is estimated by the load torque observer 9 from the torque command and the motor speed, and the compensation amount is obtained by multiplying the load torque by a compensation gain for elastic deformation compensation according to the trajectory of the action point of the motion mechanism. . This compensation amount is passed through the vibration suppression filter 13 and added to the position deviation. In the present embodiment, the three-stage compensation is performed on the trajectory of the action point of the motion mechanism, but it is needless to say that the trajectory can be further improved by compensating more stages.

  According to the present invention, the load torque is estimated by the load torque estimator, the elastic deformation is compensated according to the load torque and the trajectory of the action point of the motion mechanism, and the motion mechanism is excited without exciting the vibration due to the elastic deformation compensation. It is possible to improve the trajectory accuracy of the action point of the motion mechanism by suppressing the influence of the external force on the action point.

DESCRIPTION OF SYMBOLS 1 Speed calculation part 2 Position control part 3 Pre filter (2nd filter)
4 Subtraction means 5 Addition means 6 Subtraction means 7 Speed control section 8 Torque control section 9 Load torque observer (load torque estimation section)
DESCRIPTION OF SYMBOLS 10 Compensation amount calculating part 11 Multiplication part 12 Compensation gain memory | storage part 13 Vibration suppression filter (1st filter)
102 Arm 103 Action part M2 Motor MS Mechanical system B2 Belt M Motor E Encoder S1-S3 Position command section G1-G3 Compensation gain

Claims (6)

A position detector for detecting the position of a motor loaded with an exercise mechanism including a belt as force transmission means;
A speed calculation unit for calculating the speed of the motor;
A position control unit that performs position control by outputting a speed command so that a position feedback indicating a position of the motor fed back from the position detection unit and a position command coincide with each other;
A speed control unit that performs speed control by outputting a torque command so that the speed fed back from the speed calculation unit and the speed command match;
A torque control unit that performs torque control based on the torque command;
Compensation to be added to the position deviation between the position command and position feedback input to the position control unit so as to reduce the amount of deviation between the trajectory of the action point of the motion mechanism and the locus of the action point. A compensation amount calculation unit for calculating the amount,
The compensation amount calculation unit is configured to estimate a load torque including a torsion torque to be calculated for the motion mechanism based on the speed calculated by the speed calculation unit and the torque command; and
A compensation gain storage unit for storing a compensation gain necessary for reducing the deviation amount by a prior test for each of a plurality of consecutive position command sections;
A motor control device comprising: a multiplication unit that multiplies the compensation gain acquired from the compensation gain storage unit by using the position command as input and the load torque to calculate the compensation amount.   The motor control device according to claim 1, further comprising a first filter that removes a vibration component that finely vibrates the motion mechanism included in the compensation amount.   The motor control device according to claim 1, further comprising a second filter that removes a vibration component that finely vibrates the motion mechanism included in the position command. A position detector for detecting the position of a motor loaded with an exercise mechanism including a belt as force transmission means;
A speed calculation unit for calculating the speed of the motor;
A position control unit that performs position control by outputting a speed command so that a position feedback indicating a position of the motor fed back from the position detection unit and a position command coincide with each other;
A speed control unit that performs speed control by outputting a torque command so that the speed fed back from the speed calculation unit and the speed command match;
Motor control that reduces a deviation amount between a predetermined trajectory of the motion mechanism and the trajectory of the action point using a motor control device including a torque control unit that performs torque control based on the torque command A method,
For each of a plurality of continuous position command sections, a compensation gain necessary to reduce the deviation amount is determined by a preliminary test,
Estimating a load torque including a torsion torque to be calculated for the motion mechanism based on the speed calculated by the speed calculator and the torque command;
The compensation gain corresponding to the position command section determined based on the position command is multiplied by the load torque to calculate a compensation amount for reducing the deviation amount, and the compensation amount is calculated as the position command and the position. A motor control method comprising adding to a position deviation from feedback.   The motor control method according to claim 4, wherein a vibration component that finely vibrates the motion mechanism included in the compensation amount is removed.   The motor control method according to claim 4, wherein a vibration component that finely vibrates the motion mechanism included in the position command is removed.

Images