JP2018058164A - Drive control device, torque control device, and assist control device - Google Patents

Abstract

Drive control capable of improving the accuracy of disturbance estimation as much as possible to meet the drive state of a drive unit in consideration of the change in viscosity over time even when applied to a mechanism unit having a power transmission unit having a viscosity change Providing equipment. In each part of the apparatus, motor drivers 235 and 241 drive motors of each mechanism unit coupled to an object to be controlled through actuators 230 and 232 of a drive unit, and a drive information detection unit 236 is connected to each drive unit. The drive information is detected, and the current detection units 234 and 240 detect the current of each drive unit. In the CPU 210, the disturbance estimation unit 238 estimates the load disturbance applied to each mechanism unit based on the current and drive information, and the drive control unit 213 determines the viscosity between each drive unit and each mechanism unit included in the load disturbance. In addition to controlling based on the coefficient and drive information, the value of the viscosity coefficient is calculated based on the characteristic parameters of the predetermined function data, and after the viscosity coefficient falls within a certain range after the start of driving of each mechanism unit, Power transmission of the clutch of the mechanism part is performed. [Selection] Figure 3

Description

  The present invention relates to a drive control device, a torque control device, and an assist control device.

  2. Description of the Related Art Conventionally, a technique using a disturbance observer is known as a method for estimating a disturbance or disturbance torque in a manipulator corresponding to an arm or hand of an industrial robot that is an example of an object to be controlled by this type of drive control device. In such a technique, a disturbance is estimated from the current, angular velocity, angular acceleration, and the like of a motor that is an electric motor connected to a mechanism unit coupled to a manipulator. Specifically, the disturbance is determined from the relationship between the input torque multiplied by the motor current and the torque constant, the output torque multiplied by the inertia and angular acceleration indicating the motor inertia moment, and the frictional force related to Coulomb friction and viscous friction. A case where torque is estimated can be exemplified.

  An example of a well-known technique related to estimation of disturbance and disturbance torque in such a controlled object is given below. Suitable for application to mechanical devices that are controlled by a mechanically connected driven part to a motor via a power transmission unit, even if there is a disturbance such as friction in the mechanical device or the weight of the device. This is a “mechanical device control device and mechanical device characteristic identification method” (see Patent Document 1) that can accurately identify device characteristics and execute drive control.

  The technique according to Patent Document 1 described above is for the purpose of identifying apparatus characteristics with high accuracy even when there is a disturbance such as friction in the machine apparatus or the weight of the apparatus. A technique is disclosed in which an inertial resonance system in which a motor and a driven part are connected by a power transmission part is a control target, and a torque command value is adjusted based on the identified parameters.

  However, according to the technique according to Patent Document 1, it is possible to accurately control a control target having a power transmission unit with respect to a change in friction. However, when applied to a mechanism that has a viscosity change due to a lubricant such as grease, the frictional force changes with the passage of time, making it necessary to estimate the torque in consideration of the change of the viscous friction coefficient with the passage of time. There is a problem that it ends up. That is, at present, when applied to a mechanism unit having a power transmission unit having a viscosity change, the difficulty that the accuracy of disturbance estimation deteriorates cannot be solved.

  The present invention has been made to solve such problems, and the technical problem can be expressed as follows. A drive control device and torque that can improve the accuracy of disturbance estimation as much as possible to meet the drive state of the drive unit considering the viscosity change over time even when applied to a mechanism unit having a power transmission unit with a viscosity change A control device and an assist control device are provided.

  In order to solve the above technical problem, an embodiment of the present invention includes a drive unit that drives a controlled object, a mechanism unit that is connected to the controlled object and the drive unit and has a viscosity change including a lubricant, and a control. A support unit that supports the object and the mechanism unit, a power transmission mechanism unit that is connected to the mechanism unit and the support unit and that turns on / off power transmission from the drive unit to the support unit, and drive information from the drive unit to the mechanism unit A drive information detection unit for detecting current, a current detection unit for detecting a current flowing in the drive unit, a load disturbance applied to the mechanism unit into a current detected by the current detection unit and a drive information detected by the drive information detection unit A disturbance estimation unit that estimates based on the viscosity coefficient relating to the drive unit and the mechanism unit included in the load disturbance estimated by the disturbance estimation unit, and the drive information detected by the drive information detection unit Control unit based on control and The control unit calculates a value of the viscosity coefficient estimated by the disturbance estimation unit based on a characteristic parameter of a predetermined function data, and after the drive of the mechanism unit by the drive unit is started Therefore, the power transmission mechanism unit transmits power after the viscosity coefficient falls within a certain range.

  In the present invention, with the configuration according to the above aspect, even when applied to a mechanism unit having a power transmission unit having a viscosity change, a disturbance is applied so as to match the drive state of the drive unit in consideration of the viscosity change with time. The accuracy of estimation can be improved as much as possible. Problems, configurations, and effects other than those described above will be clarified by the following description of embodiments.

1 shows a schematic configuration of a manipulator device to which a drive control device according to an embodiment of the present invention is applied. The figure (a) is the figure which showed the external appearance of the manipulator from the side direction, The figure (b) is a block diagram which concerns on one form of the connection structure of the manipulator main body provided with the drive control apparatus, the manipulator to be controlled, and the peripheral device It is. FIG. 2 shows a schematic configuration of a manipulator device that can be substituted for the connection configuration of the manipulator device shown in FIG. FIG. 4A is a block diagram according to another embodiment of a connection configuration of a manipulator body provided with a drive control device, a manipulator to be controlled, and peripheral devices, and FIG. 5B is a block diagram showing a manipulator body provided with the drive control device and control. It is a block diagram which concerns on another form of a connection structure with a target manipulator and a peripheral device. It is the block diagram which showed the detailed structure of the manipulator apparatus shown in FIG.2 (b) including a peripheral device. It is the schematic which showed the external appearance structure of the input part with which the manipulator main body of the manipulator apparatus shown in FIG. 3 is equipped. FIG. 4 illustrates a circuit configuration when a disturbance estimation unit provided in the manipulator body of the manipulator device shown in FIG. 3 is a disturbance torque estimation unit. FIG. 4A is a circuit configuration diagram without a low-pass filter, and FIG. 4B is a circuit configuration diagram with a low-pass filter. FIG. 6 is a block diagram illustrating a configuration example of a torque control circuit in the case where torque control is performed using a disturbance torque estimation value obtained by a disturbance torque estimation unit illustrated in FIG. FIG. 7 is a schematic diagram of an image for performing assist control using the torque control circuit shown in FIG. 6. FIG. 5A is a characteristic diagram of the force required for the operation indicated by the relationship of the force change with respect to the position change compared with the operation state transition of the manipulator, and FIG. 5B is the operation force command value for the operation force of the external force. FIG. 7 is a flowchart showing an operation process related to a drive control unit in a CPU provided in the manipulator body of the manipulator device shown in FIG. 3 when assist control is performed using the torque control circuit shown in FIG. 6. It is the figure which illustrated the circuit structure at the time of using the disturbance torque estimation part shown in FIG.5 (b) for correction | amendment of a viscous friction coefficient. FIG. 10 is a characteristic diagram illustrating a relationship between a viscous friction coefficient and a frictional force with respect to a rotational angular velocity when the disturbance torque estimation unit illustrated in FIG. 9 is applied. FIG. 4A is a characteristic diagram of the viscous friction coefficient with respect to the rotational angular velocity, and FIG. 4B is a characteristic diagram of the frictional force with respect to the rotational angular velocity. When the disturbance torque estimation unit shown in FIG. 9 is applied, the characteristic in the case of correcting the viscous friction coefficient by changing the rotation angular velocity in a different pattern corresponding to the drive time of the mechanism unit by the drive unit is compared with the moving rotation distance. FIG. 4A is a diagram related to the characteristic parameter pattern 1, FIG. 4B is a diagram related to the characteristic parameter pattern 2, and FIG. 4C is a diagram related to the characteristic parameter pattern 3. FIG. When the disturbance torque estimation unit shown in FIG. 9 is applied, the characteristic in the case of correcting the viscous friction coefficient by changing the rotation angular velocity in the middle corresponding to the drive time of the mechanism by the drive unit is compared with the moving rotation distance. FIG. FIG. 4A is a diagram related to the characteristic parameter pattern 4, and FIG. 4B is a diagram related to the characteristic parameter pattern 5. In the characteristic parameter pattern 4 shown in FIG. 12A, the third time before the viscous friction coefficient gradually approaches the first viscous friction coefficient after a sufficient time has elapsed since the rotation angular speed is started to drive at the first rotation angular speed. It is the figure which contrasted and showed the characteristic of the viscous friction coefficient with respect to time at the time of changing to rotation angular velocity of this, and the characteristic of rotational angular velocity. FIG. 9 is a flowchart relating to a calculation process of a viscous friction coefficient based on a driving time after driving to a mechanism unit via a driving unit by the driving control unit described in FIG. 8 and an operation process of clutch power transmission. FIG. 9 is a flowchart relating to an operation process of calculating a viscous friction coefficient based on a moving rotational distance after driving to a mechanism unit via a drive unit by the drive control unit described in FIG. 8 and a power transmission operation of a clutch. FIG. 9 is a timing chart showing the ON / OFF relationship of power transmission by the clutch when the viscous friction coefficient is calculated based on the driving time after driving the mechanism through the driving unit by the driving control unit described in FIG. is there.

  Hereinafter, the drive control device, the torque control device, and the assist control device of the present invention will be described in detail with reference to the accompanying drawings.

  FIG. 1 shows a schematic configuration of a manipulator device 200A to which a drive control device according to an embodiment of the present invention is applied. FIG. 4A is a diagram showing the appearance of the manipulator from the side, and FIG. 4B is a block diagram according to one embodiment of a connection configuration between a manipulator body 201 having a drive control device, a manipulator to be controlled, and peripheral devices. is there.

  Referring to FIGS. 1A and 1B, the manipulator device 200A includes a base 102 in which a base portion of the other end portion of the arm 101 whose one end side serves as a hand is attached and fixed to the base plate 108. It is pivotally supported and becomes a joint. In addition, the arm 101 is coupled to the mechanism unit as a manipulator of a controlled object having one degree of freedom in which the arm 101 can rotate with the drive shaft 103 as an axis. On this, the drive controller provided in the manipulator body 201 is configured to drive the electric motor of the mechanism unit. Here, the drive center 103 of the manipulator is connected to a speed reducer 106 with a clutch 111 serving as a power transmission mechanism interposed therebetween, and is rotationally driven by power from a motor 105 as an electric motor. As a result, the power of the motor 105 connected to the speed reducer 106 is transmitted from the clutch 111, and the arm 101 is rotationally driven with respect to the drive center 103. A motor encoder (ENC) 104 capable of measuring the amount of rotation is attached to the motor 105. Here, the mechanism unit shows a configuration including a motor 105 and a speed reducer 106 and a motor encoder 104 connected to a drive unit that drives the motor 105 (or the arm 101 may be included). The base 102 serves as a support part for the mechanism part and the motor 105 and the arm 101 of the manipulator. The clutch 111 is connected to the mechanism unit and the base 102 and functions as a power transmission mechanism unit that turns on / off power transmission from the drive unit to the base 102. The manipulator itself has a structure in which a speed reducer 106 with a clutch 111 interposed and a motor 105 to which a motor encoder 104 is attached are fixed to an arm 101 and a drive center 103 is fixed to a base 102. In addition, an external connection device 107, which will be described later, is connected to the manipulator body 201. That is, the manipulator device 200A here has a configuration in which one motor 101, one speed reducer 106, and one clutch 111 are provided for each arm 101 that is a control target.

  FIG. 2 shows a schematic configuration of manipulator devices 200B and 200C that can be substituted for the connection configuration of the manipulator device 200A shown in FIG. FIG. 5A is a block diagram showing another embodiment of a connection configuration between a manipulator body 200 having a drive control device, a manipulator to be controlled, and peripheral devices, and FIG. 5B is a manipulator body having the drive control device. It is the block diagram which showed another form of the connection structure of 200, the manipulator of control object, and a peripheral device.

  In the connection configuration of the manipulator device 200B shown in FIG. 2A, one motor 105, two speed reducers 106 and 110, and two clutches 111 and 112 are provided for one arm 101 that is a controlled object. It has a configuration. Again, the arm 101 is rotatably supported by the drive center 103. The drive center 103 of the manipulator is connected to a speed reducer 106 having a clutch 111 interposed on one side of the end face of the arm 101 and a speed reducer 110 having a clutch 112 interposed on the other side of the end face of the arm 101. Thereby, the power of the motor 105 connected to the speed reducer 106 is transmitted from the clutches 111 and 112, and the arm 101 is rotationally driven with respect to the drive center 103. Specifically, the motor 105 rotates the gear 113 interposed between the reduction gear 106 and the rotation of the gear 113 rotates the other gear 114 interposed between the reduction gear 106 and the gear 113. Further, the gear 114 rotates the shaft 115, and the shaft 115 rotates the gear 116 provided outside the speed reducer 110, so that power is finally transmitted to the speed reducer 110. The power transmission to the speed reducer 110 is configured to be rotational power in the opposite direction to the power to the speed reducer 106. Furthermore, the motor encoder 104 is also attached here so that the rotation amount of the motor 105 can be measured. The reduction gears 106 and 110 and the motor 105 are fixed to the arm 101, and the drive center 103 is fixed to the base 102.

  That is, in the manipulator device 200B, the speed reducer 106 connected to the motor 105 and the drive unit and the gears 113 and 114 function as a first mechanism unit having a viscosity change including a lubricant. In addition, the speed reducer 110, the gear 116, and the shaft 115 connected to the drive unit function as a second mechanism unit having a viscosity change including a lubricant that is driven in a direction opposite to the first mechanism unit. Further, the base 102 serves as a support portion that supports the motor 105 and the first mechanism portion and the second mechanism portion. In addition, the clutch 111 connected to the first mechanism unit and the base 102 serves as a first power transmission mechanism unit that turns on / off power transmission from the drive unit to the base 102. In addition, the clutch 112 connected to the second mechanism unit and the base 102 serves as a second power transmission mechanism unit that turns on / off power transmission from the drive unit to the base 102.

  In the connection configuration of the manipulator device 200 </ b> C shown in FIG. 2B, two motors 105 and 109, speed reducers 106 and 110, and two clutches 111 and 112 are provided for one arm 101. . Again, the arm 101 is rotatably supported by the drive center 103. The drive center 103 of the manipulator is connected to a speed reducer 106 having a clutch 111 interposed on one side of the end face of the arm 101 and a speed reducer 110 having a clutch 112 interposed on the other side of the end face of the arm 101. Thereby, the power of the motors 105 and 109 connected to the speed reducers 106 and 110 is transmitted from the clutches 111 and 112, respectively, and the arm 101 is rotationally driven with respect to the drive center 103. Here, the power transmission to the speed reducer 110 is also configured to be rotational power in the direction opposite to the power to the speed reducer 106. Furthermore, the motor encoders 104 and 117 are also attached here so that the rotation amounts of the motors 105 and 109 can be measured. The reduction gears 106 and 110 and the motors 105 and 109 are fixed to the arm 101, and the drive center 103 is fixed to the base 102.

  In other words, in the manipulator device 200C, the motor 105 and the speed reducer 106 connected to the first drive unit that drives the motor 105 function as a first mechanism unit having a viscosity change including a lubricant. In addition, as a second mechanism unit having a viscosity change in which the reduction gear 110 connected to the motor 109 and the second driving unit that drives the motor 109 includes a lubricant that is driven in the opposite direction to the first mechanism unit. work. Further, the base 102 serves as a support portion that supports the motors 105 and 109 and the first mechanism portion and the second mechanism portion. In addition, the clutch 111 is a first power transmission mechanism unit that turns on / off power transmission from the first drive unit to the base 102 by the clutch 111 connected to the first mechanism unit and the base 102. In addition, the clutch 112 connected to the second mechanism unit and the base 102 serves as a second power transmission mechanism unit that turns on / off power transmission from the second drive unit to the base 102.

  In the above-described manipulator devices 200A, 200B, and 200C, the mechanism portion having the single clutch 111 and the first mechanism portion and the second mechanism portion having the pair of clutches 111 and 112 exhibit viscosity changes including a lubricant. It is assumed that you have one. Incidentally, although the controlled object is the arm 101 of the manipulator here, it can also be applied to a wave gear device, a machine tool or the like including a lubricant such as grease. In such a case, the motor 105 of the first mechanism unit and the motor 109 of the second mechanism unit may be prime movers in a broad sense including other fluid machines and heat engines.

  FIG. 3 is a block diagram showing a detailed configuration of the above-described manipulator device 200C including peripheral devices.

  Referring to FIG. 3, the manipulator device 200 </ b> C here is configured by connecting a power supply unit 291 corresponding to the external connection device 107 described above as a peripheral device to the manipulator body 201 and a host controller 281. When image information is required for manipulator operation, an image input device 271 is further connected to the host controller 281. Incidentally, the manipulator device 200 </ b> C itself may have a stand-alone configuration in which all of these are mounted in the manipulator body 201, except that the manipulator body 201, the host controller 281, and the power supply unit 291 are configured separately.

  The detailed configuration of the manipulator body 201 includes a CPU 210 that performs operation control on the actuator 230 of the first drive unit and the actuator 232 of the second drive unit via the motor driver 235 and the motor driver 241. The communication control unit 211 in the CPU 210 is connected to the host controller 281 through the communication network 282, and various information is transmitted from the host controller 281 to the drive control unit 213 in the CPU 210 via the communication network 282 and the communication control unit 211. It is like that. The various information includes operation mode information, various parameters necessary for drive control, target movement position information of the manipulator, and the like.

  The CPU 210 receives signals from various sensors 220 and various switches 221. For example, an operation mode can be input and set from an input unit 239 that can be connected to the CPU 210. At this time, an orbit stop operation such as an emergency stop for the manipulator is performed by an operation command of the drive control unit 213 based on the signal information. Further, a clock 223 is connected to the CPU 210 via an input terminal and an output terminal, and a clock signal is taken in.

  In addition, the CPU 210 is equipped with a storage unit 214 having various memory functions for rewritably storing operation programs and information necessary for operation control of the manipulator, and a timer 212 for counting. The drive control unit 213 connected to these outputs drive commands to the actuators 230 and 232 by the motor drivers 235 and 241 as operation control of the manipulator. At this time, the drive control unit 213 outputs a drive command based on the viscosity coefficient related to the mechanism unit including the motors 105 and 109 and the drive unit including the actuators 230 and 232 included in the load disturbance estimated by the disturbance estimation unit 238. To control. That is, each joint of the manipulator is performed by driving the motors 105 and 109 by the actuators 230 and 232 driven by the motor drivers 235 and 241 that have received a drive command from the drive control unit 213.

  The drive information of the actuators 230 and 232 is input to the drive information detection unit 236 as signals from the encoders 231 and 233 attached thereto. The drive information detection unit 236 converts the signals from the encoders 231 and 233 into drive information such as a movement amount, a movement speed, and a movement acceleration.

  Further, the current flowing through the actuators 230 and 232 is detected by the first current detection unit 234 and the second current detection unit 240. The drive information detected by the drive information detection unit 236 and the current detected by the current detection units 234 and 240 are input to the disturbance estimation unit 238 in the CPU 210. The disturbance estimation unit 238 estimates a load disturbance applied to the mechanism unit based on the input drive information and current and sends the estimated load disturbance to the drive control unit 213.

  The drive control unit 213 generates a target command value based on the drive information detected by the drive information detection unit 236, the load disturbance value estimated by the disturbance estimation unit 238, and the operation mode set by the input unit 239 and the like. . At this time, the drive control unit 213 sends a drive command to the motor drivers 235 and 241 so as to drive according to the target command value, and drives the actuators 230 and 232 by feedback control. As a result, the motors 105 and 109 shown in FIG. 2B are controlled to rotate along the target command value. The load disturbance estimated by the disturbance estimation unit 238 includes a viscosity coefficient related to the mechanism unit including the motors 105 and 109 and the drive unit including the actuators 230 and 232.

  The motor driver 235 drives the motor 105 of the electric motor, and the motor driver 241 drives the motor 109 of the electric motor. The mechanism unit including the motors 105 and 109 and the drive unit including the actuators 230 and 232 have a viscosity change including a lubricant. The drive information detection unit 236 detects drive information of the actuators 230 and 232 by the motor drivers 235 and 241. The current detection units 234 and 240 detect currents flowing through the motor drivers 235 and 241.

  The disturbance estimation unit 238 estimates the load disturbance applied to the actuators 230 and 232 based on the current detected by the current detection units 234 and 240 and the drive information detected by the drive information detection unit 236. The drive control unit 213 is a viscosity coefficient related to the mechanism unit including the motors 105 and 109 and the drive unit including the actuators 230 and 232 included in the load disturbance estimated by the disturbance estimation unit 238 based on the drive command to the motor drivers 235 and 241. And the drive information detected by the drive information detector 236. Each component configuration for driving the first mechanism unit including the motor 105 and the second mechanism unit including the motor 109 may be referred to as a drive control device. Specifically, the motor drivers 235 and 241 and the actuators 230 and 232 with the encoders 231 and 233 described above are included, and the drive information detection unit 236 and the current detection units 234 and 240, the disturbance estimation unit 238, and the drive control unit 213 are applicable. To do.

  In addition, the drive control unit 213 here calculates the value of the viscosity coefficient estimated by the disturbance estimation unit 238 based on a predetermined characteristic parameter as a characteristic function. In addition, after the viscosity coefficient is within a certain range after the start of driving of the motor 105 by the actuator 230 via the motor driver 235 and the motor 109 by the actuator 232 via the motor driver 241, the power transmission of the clutches 111 and 112 is performed. Let each do.

  Incidentally, in the case of the manipulator device 200A shown in FIG. 1B, the drive control unit 213 similarly calculates the value of the viscosity coefficient based on a predetermined characteristic parameter. Then, after the start of driving of the motor 105 by the actuator 230 via the single motor driver 235, the function of transmitting power of the single clutch 111 is performed after the viscosity coefficient falls within a certain range. On the other hand, in the case of the manipulator device 200B shown in FIG. 2A, the drive control unit 213 similarly calculates the value of the viscosity coefficient based on a predetermined characteristic parameter. Then, after the viscosity coefficient falls within a certain range from the start of driving of the motor 105 by the actuator 230 via the single motor driver 235, the power transmission of the pair of clutches 111 and 112 is performed.

  If it has such a function, the accuracy of disturbance estimation will be improved as much as possible to match the drive state of the drive unit, taking into account the change in viscosity over time even when applied to a mechanism unit having a power transmission unit with a change in viscosity. can do. Incidentally, the load disturbance includes the weight of the apparatus in addition to the load disturbance torque.

  By the way, the disturbance estimation unit 238 described above estimates, for example, a load disturbance torque applied to a mechanism unit including a single motor 105 or a first mechanism unit including a pair of motors 105 and 109 and a second mechanism unit. May be. In such a case, the drive control unit 213 treats the viscous friction coefficient as the viscosity coefficient, and performs the above-described characteristic function processing on the value of the viscous friction coefficient when estimating the load disturbance torque by the disturbance torque estimation unit. Again, for the correction of the value of the viscous friction coefficient, a calculation result relating to the characteristics of a predetermined correction parameter is used.

  In addition, as another characteristic function, the drive control unit 213 changes a characteristic of the correction parameter in a linear function after the operation of the motor 105 by the actuator 230 and the motor 109 by the actuator 232 starts, and is a predetermined value. Set asymptotically. Alternatively, it is set so that it gradually decreases exponentially from the start of operation and approaches a predetermined value. In the configuration of FIG. 1B and FIG. 2A, the motor 105 driven by the actuator 230 is an object.

  Furthermore, the above-described drive information detection unit 236 may be a rotation angular velocity detection unit that detects the rotation angular velocity of the motor 105 by the actuator 230 and the motor 109 by the actuator 232 as drive information. In the configuration of FIG. 1B and FIG. 2A, the motor 105 driven by the actuator 230 is an object. In such a case, the drive control unit 213 preferably has a function capable of switching the rotation angular velocity by the rotation angular velocity detection unit for making the viscosity coefficient asymptotically approach a predetermined value.

  Incidentally, the above-described drive control device calculates the value of the viscous friction coefficient by estimating the load disturbance torque, and after the viscous friction coefficient falls within a certain range, the power of the single clutch 111 or the pair of clutches 111 and 112 is calculated. Has a function to transmit. In this drive control device, the actuators 230 and 232 of the pair of first drive units and the second drive unit drive the motors 105 and 109 of the pair of first mechanism units and second mechanism units, respectively. It may be called a control device. In the configuration of FIG. 1B, the motor 105 provided in a single mechanism unit driven by the actuator 230 of a single drive unit is a target. Further, in the configuration of FIG. 2A, the motor 105 provided in either one of the pair of the first mechanism unit and the second mechanism unit driven by the actuator 230 of the single driving unit is targeted. Become. In addition, the torque control device from this point of view is based on the output of the motors 105 and 109 of the pair of first mechanism unit and second mechanism unit against the external force of the operation force command value from the input unit 239. Since the assist control of the joint of the arm 101 is performed, it may be called an assist control device. In the configuration of FIGS. 1B and 2A, the joint of the arm 101 of the control object is assisted and controlled by the output of the single motor 105.

  The drive control device provided in the manipulator body 201 of the manipulator devices 200A, 200B, and 200C according to the embodiment can use a viscosity coefficient that changes with a time function when estimating the load disturbance. The target of load disturbance estimation includes a drive unit including a single actuator 230 or a first drive unit and a second drive unit including a pair of actuators 230 and 232. Further, a mechanism unit including a single motor 105, a first mechanism unit including a pair of motors 105 and 109, and a second mechanism unit may be used. When estimating the load disturbance, a method for changing the viscosity coefficient according to a constant value or rotation information has been established, and the viscosity of a lubricant such as grease contained in the mechanism section changes with time. It is also known to have characteristics. Therefore, if the viscosity coefficient used in the estimation of the load disturbance is changed as a function of time, the viscosity coefficient becomes close to an actual machine model, so that the accuracy of the estimation of the load disturbance is improved. Here, specifically, the case where the load disturbance estimation is the load disturbance torque estimation and the viscosity coefficient is the viscous friction coefficient can be exemplified as described above.

  FIG. 4 is a schematic diagram illustrating an external configuration of the input unit 239 provided in the manipulator body 201 of the above-described manipulator device 200C.

  Referring to FIG. 4, the input unit 239 includes a touch panel a that is used for manipulator operation by a user or for creating an action profile, and a numeric keypad b for creating an action profile. In addition, an execution button c for starting the operation of the manipulator, a power button d for turning on the power, and an emergency stop button e for urgently stopping the operation of the manipulator are provided.

  When the user uses the input unit 239, when the power button d is turned on, the touch panel a is displayed, and the manipulator can be operated according to the display content. Therefore, the user creates an operation profile with the touch panel a or the numeric keypad b, and then starts the operation of the manipulator with the execution button c. At this time, it is possible to switch between a mode for correcting the viscosity coefficient and the viscous friction coefficient described above and a mode for not correcting.

  For example, when “select operation mode” is selected from a menu displayed on the touch panel a, an on / off switch for whether to correct the viscosity coefficient or the viscous friction coefficient is displayed on the touch panel a. Therefore, the user can switch the correction mode on / off using the numeric keypad b or the touch panel a. Further, when the correction mode is turned off, the default value of the viscosity coefficient or the viscous friction coefficient can be determined. Information regarding the on / off of the correction mode and the default value are input to the CPU 210 and used by the disturbance estimation unit 238 or the disturbance torque estimation unit.

  FIG. 5 illustrates a circuit configuration when the disturbance estimation unit 238 provided in the manipulator body 201 of the manipulator device 200C described above is a disturbance torque estimation unit. FIG. 5A illustrates a circuit configuration without a low-pass filter. FIG. 4B is a circuit configuration diagram having a low-pass filter. However, here, in order to simplify the description, the case where one system of output is performed based on the current from the one system of current detection unit 234 is shown.

  The disturbance torque estimation unit shown in FIG. 5A corresponds to the case where the torque estimation unit 403A for the actual machine unit 404 does not have a low-pass filter. On the other hand, the disturbance torque estimation unit shown in FIG. 5B corresponds to the case where the torque estimation unit 403B for the actual unit 404 has a low-pass filter 405 for pseudo differentiation. For any disturbance torque estimation unit, the actual unit 404 is usually a block diagram of a multi-inertia system including the speed reducers 106 and 110, but here it is simplified and shown as a one-inertia system. Further, here, the output of the actual unit 404 is determined based on the detection result of the current input from the current detection unit 234 and the disturbance torque and Coulomb friction included in the drive information obtained from the drive information detection unit 236. Is the angle θ of the motor shaft obtained by integrating 1 / s. In addition, as the moment of inertia J, a value obtained by converting the moment of inertia related to the first mechanism unit or the speed reducer 106 to the motor shaft is used. Note that 1 / s indicates integration in terms of Laplace transform.

  In the real machine unit 404 common to FIGS. 5A and 5B, a value obtained by multiplying a difference between a torque constant Kt corresponding to the current detection result and a calculation parameter described later by an inverse 1 / J of the moment of inertia J. The rotational angular velocity ω of the motor 105 is calculated from the value obtained by integrating 1 / s. Further, the motor shaft angle θ is output by a value obtained by integrating 1 / s of the rotational angular velocity ω. Since the force of the gravity term adding unit 401 differs depending on the first mechanism unit connected to the motor 105, the gravity term g (θ) is determined by a function with respect to the angle θ output from the mechanism unit, and the disturbance torque and Add as a parameter to the added value of Coulomb friction. The viscous friction coefficient adding unit 402 calculates the viscous friction coefficient C related to the motor 105, the speed reducer 106, and the first mechanism unit corresponding to the rotational angular velocity ω between the disturbance torque and the Coulomb friction added value, and the gravity term g (θ). Add to the added value to make it a calculation parameter.

  Further, in the torque estimation unit 403A in FIG. 5A, a disturbance torque estimated value is obtained by subtracting Coulomb friction and a calculation parameter described later from the torque constant Kt indicating the estimated value. This calculation parameter uses a disturbance observer and is obtained as a value obtained by adding an inertia moment J indicating an estimated value to a value obtained by adding a viscous friction coefficient C indicating an estimated value to the gravity term g (θ).

  Further, the torque estimation unit 403B in FIG. 5B calculates an addition value of the torque constant Kt indicating the estimated value and the value obtained by multiplying the rotational angular velocity ω by the moment of inertia J / time constant τ indicating the estimated value. Further, a value obtained by subtracting the gravitational term g (θ) and the Coulomb friction from the value obtained by subtracting the viscous friction coefficient C indicating the estimated value corresponding to the rotational angular velocity ω from this added value is obtained by the pseudo-differentiation 1 / (Sτ + 1). Further, a disturbance torque estimated value is obtained by subtracting the value of the moment of inertia J / time constant τ indicating the previous estimated value from the value of the pseudo differential 1 / (sτ + 1).

  In any case, the torque constant Kt, the moment of inertia J, the viscous friction coefficient C, or the estimated values thereof, which are various parameters used in the actual machine unit 404 and the torque estimation units 403A and 403B, include errors. It is necessary to identify the parameters and match the values. Practically, for example, multiple regression analysis on statistical processing is performed using experimental data, or parameters are identified from frequency response analysis.

  FIG. 6 is a block diagram illustrating a configuration example of a torque control circuit in the case where torque control is performed using the disturbance torque estimation value obtained by the disturbance torque estimation unit illustrated in FIG.

  Referring to FIG. 6, the torque control circuit feeds back the estimated disturbance torque value obtained by the disturbance torque estimation unit shown in FIG. 5B, and compares the estimated torque value with the torque command as the operation force target. The result obtained by subtracting from the torque command value is obtained. The PID calculation is performed by the PID controller 501 that performs PID calculation including proportionality, integration, and differentiation on the result. The current driver 502 that has received the result of the PID calculation supplies a current value to the motor 105. The current value at this time is input to the disturbance torque estimation unit. By performing such feedback control, torque control that follows the torque command value can be accurately performed. This torque control circuit uses the gravity term g (θ) as a calculation parameter, but does not perform dynamic compensation such as gravity compensation. Therefore, if further compensation such as friction compensation, gravity compensation, inertial force, and the like is performed, feedback control with better followability and feedforward control for canceling load disturbance can be performed with high accuracy.

  FIG. 7 shows a schematic diagram of an image of performing assist control using the torque control circuit shown in FIG. FIG. 4A is a characteristic diagram of the force required for the operation indicated by the relationship of the force change with respect to the position change compared with the operation state transition of the manipulator 503, and FIG. 4B is an operation force command for the operation force of the external force. It is the figure which showed the relationship of the value.

  Referring to FIG. 7A, when assist control is performed by the torque control circuit shown in FIG. 6, when the manipulator 503 is moved from one position P to another position P ′, it depends on the value of the torque command. Set the target value of operating force. As a result, a part of the force required for the operation is supplemented by the assist force of the motor output and becomes a resultant force to the operation force of the external force, so that the operation following the target value can be performed with the operation force of the constant external force. Referring to FIG. 7B, the target value F of the operating force command value is a dead zone with respect to the target value F of the operating force of the external force as in the section of changes in operating force of the external force -F1 to F1. A profile that has As a result, sudden operation and oscillation can be suppressed, and stable operation is possible.

  FIG. 8 is a flowchart showing an operation process related to the drive control unit 213 in the CPU 210 provided in the manipulator body 201 of the manipulator device 200C shown in FIG. 3 when assist control is performed using the torque control circuit shown in FIG. .

  Referring to FIG. 8, in the operation process related to the assist control of the drive control unit 213, first, an operation force target value setting as a torque command is set (step S1), and then the position of the manipulator 503 is detected by the joint angle (step S2). ) Thereafter, the drive control unit 213 detects the operating force of the external force of the manipulator 503 (step S3). Next, it is determined whether or not there is an operation based on the detection result (step S4). As a result of this determination, if there is no operation, the process returns to before the process of detecting the position of the manipulator 503 based on the joint angle (step S2) and the subsequent processes are repeated. On the other hand, if there is an operation, the assist control is started. The processing flow so far shows that the drive control unit 213 can start assist control by determining the presence or absence of an operation by the input unit 239 from the change in the position of the manipulator 503, the rotation angular velocity ω, and the change in the external force operation force. Yes.

  When the assist control is started, the drive control unit 213 performs the operation target value error detection (step S5) based on the detection result of the initially set operation force target value of the manipulator 503, and then outputs the torque output of the motor 105. A value is calculated (step S6). This torque output value is obtained by multiplying the inertia indicating the rotational system moment of inertia of the motor and the angular acceleration obtained by differentiating the rotational angular velocity ω. Accordingly, the drive control unit 213 drives the motor with the calculated torque output value (step S7), and again detects the position of the manipulator 503 based on the joint angle (step S8), and then detects the operating force of the external force of the manipulator 503 ( Step S9).

  Further, it is determined whether or not there is an operation based on the detection result (step S10). As a result of the determination, if there is an operation, the process returns to the process before the operation target value error detection (step S5) and the subsequent processes are repeated. On the other hand, if there is no operation, the assist control is terminated. In the processing flow so far, the motor 105 is driven and controlled based on the operation target value error detected by the drive control unit 213, and compared with the operation force of the external force, the operation force of the external force becomes the operation target value. It shows that the motor 105 is feedback-controlled so that As a result, the assist control operation can be performed with high accuracy.

  FIG. 9 is a diagram illustrating a circuit configuration when the disturbance torque estimation unit illustrated in FIG. 5B is used for correcting the viscous friction coefficient.

  Referring to FIG. 8, the disturbance torque estimating unit is a first mechanism in which the viscous friction coefficient adding unit 601 in the actual machine unit 404 has a viscosity change caused by a lubricant such as grease, as compared with the circuit configuration of FIG. Considering that the viscous friction coefficient C varies depending on the driving time t and the rotational angular velocity ω when the part is targeted. Therefore, the viscous friction coefficient C (ω, t) is used as a parameter. Further, the torque estimation unit 403B has a viscous friction coefficient indicating an estimated value for subtraction with respect to an addition value of a torque constant Kt indicating an estimated value and a value obtained by multiplying the rotation angular velocity ω by an inertia moment J / time constant τ indicating an estimated value. The difference is that C (ω, t) is used as a parameter.

  By the way, it is preferable to use data values corresponding to the drive time t and the rotational angular velocity ω acquired in advance as parameters used in the torque estimation unit 403B. As the data, a change in the driving time t of the value of the viscous friction coefficient Cx when driving at a constant rotational angular velocity ω and when the rotational angular velocity ω is changed in the middle is acquired by a combination of a plurality of rotational angular velocities ω. In addition, a function of the value of the viscous friction coefficient Cx with respect to the driving time t and the rotational angular velocity ω based on these data is used. Incidentally, the case where x of the viscous friction coefficient Cx is 0 to ∞ can be exemplified. If these data are stored in the storage unit 214 as a plurality of table formats, highly accurate torque estimation can be performed.

  In addition, since the above-mentioned data is often different for each individual, it is desirable to acquire for each individual. As a method for correcting the viscous friction coefficient Cx, the circuit configuration shown in FIG. 9 has been described as an example, but in this embodiment, the technical gist is to correct the viscous friction coefficient Cx with respect to the driving time t and the rotational angular velocity ω. . For this reason, in addition to the circuit configuration shown in FIG. 9, for example, a method of estimating from a current value polynomial, a method of estimating from a current, voltage, and magnetic flux can be applied. Even when the friction compensation is performed without estimating the torque, the viscous friction coefficient Cx can be corrected.

  FIG. 10 is a characteristic diagram showing the relationship between the viscous friction coefficient C and the frictional force f with respect to the rotational angular velocity ω when the disturbance torque estimation unit shown in FIG. 9 is applied. FIG. 4A is a characteristic diagram of the viscous friction coefficient C with respect to the rotational angular velocity ω, and FIG. 4B is a characteristic diagram of the frictional force f with respect to the rotational angular velocity ω.

  Referring to FIG. 10A, in the characteristic of the viscous friction coefficient C with respect to the rotational angular velocity ω, the viscous friction coefficient C drives the first mechanism portion or the second mechanism portion from the initial value C0 of the rotational angular velocity ω = 0. It is shown that when the rotational angular velocity ω is increased from the start, the characteristic changes so as to be attenuated and lowered according to the driving time t.

  Specifically, in the characteristic diagram of FIG. 10 (a), the constant value is maintained at the initial value t0 of the driving time t, but the rotational angular velocity ω after a predetermined time t1 or after a long time t∞ has elapsed. This indicates that the faster the is, the lower the viscous friction coefficient C is. The viscous friction coefficient C after the elapse of a long time t∞ is the second viscous friction at the second rotational angular velocity ω2 that is higher than the value of the first viscous friction coefficient C1 at the first rotational angular velocity ω1. The value of the coefficient C2 is smaller.

  In short, from FIG. 10A, it can be seen that the characteristic of the viscous friction coefficient C tends to decrease with the driving time t, and the higher the rotational angular velocity ω, the lower the value and characteristic of the viscous friction coefficient C. I know that

  On the other hand, referring to FIG. 10B, in the characteristic of the frictional force f with respect to the rotational angular velocity ω, the frictional force f is changed from the initial value F0 of the rotational angular velocity ω = 0 to the first mechanism unit or the second mechanism unit. It is shown that if the rotational angular velocity ω is increased by starting driving, the characteristic changes so as to increase and rise in different curves according to the driving time t.

  Specifically, in the characteristic diagram of FIG. 10 (b), as the viscous friction coefficient C decreases as the rotational angular velocity ω increases, the initial value t0 of the driving time t, a long time t1 after the elapse of the predetermined time t1. It shows that the characteristic of the frictional force f gradually decreases after elapse of ∞. However, the decrease in the characteristic of the frictional force f here is not proportional to the increase in the rotational angular velocity ω, and the first rotational angular velocity ω1 on the characteristic indicating the magnitude of the viscous friction coefficient C according to the driving time t, It shows that the characteristic is such that the slope at the second rotational angular velocity ω2 gradually decreases. That is, the first slope after the elapse of the predetermined time t1 and after the elapse of the long time t∞, from the characteristic gradient at the first rotational angular velocity ω1 and the second rotational angular velocity ω2 at the initial value t0 of the driving time t. The characteristic inclination at the time of the rotational angular velocity ω1 and the second rotational angular velocity ω2 gradually decreases.

  In short, it can be seen from FIG. 10B that the characteristic of the frictional force f tends to increase with the driving time t, and that the value and characteristic of the frictional force f increase as the rotational angular velocity ω increases. I understand.

  FIG. 11 shows a rotational angular velocity corresponding to the driving time t for the first mechanism unit by the first driving unit or the second mechanism unit by the second driving unit when the disturbance torque estimating unit shown in FIG. 8 is applied. It is the figure which showed the characteristic parameter in the case of correct | amending the viscous friction coefficient C by changing (omega) by a different pattern, and contrasting with the moving rotation distance. 4A is a diagram related to the characteristic parameter pattern 1, FIG. 4B is a diagram related to the characteristic parameter pattern 2, and FIG. 4C is a diagram related to the characteristic parameter pattern 3. FIG.

  Referring to FIG. 11A, in the characteristic parameter pattern 1, the viscous friction coefficient C is obtained by combining different first and second rotational angular velocities ω1 and ω2 with the drive to the mechanism unit kept on. Shows a characteristic that becomes a constant value after being attenuated and lowered in different curved shapes from the initial value C0 when not operating. That is, in FIG. 11 (a), as a characteristic parameter, a pattern in which the correction characteristic of the viscous friction coefficient C is set so that the characteristic parameter attenuates exponentially after the start of operation of the mechanism and approaches a predetermined value is used. It corresponds to.

  Specifically, in the characteristic diagram of FIG. 11A, the lower limit of attenuation of the first viscous friction coefficient C1 at the first rotational angular velocity ω1, the second smaller than that at the second rotational angular velocity ω2. It shows that the lower limit of attenuation of the viscous friction coefficient C2 is the lower limit of attenuation of the third viscous friction coefficient C3, which is smaller than that at the second rotational angular velocity ω2.

  Further, the moving rotational distance at that time shows a state obtained by integrating the absolute value of the rotational angular velocity ω indicating the first rotational angular velocity ω1 or the second rotational angular velocity ω2 by the on-time. That is, FIG. 11A shows a characteristic in which the value of the viscous friction coefficient C suddenly decreases with changes in the rotational angular velocity ω and the driving time t, as in the case of FIG. The drive time t may be regarded as the same as the elapsed time, and has an initial value t0. However, since the characteristic of the viscous friction coefficient C varies depending on the type of grease and the like, the characteristic is not limited to the illustrated example, and the correction may be performed by determining the characteristic of the viscous friction coefficient C based on actually measured values. The way of changing the characteristic of the viscous friction coefficient C is expressed by an exponential function, a polynomial function, a proportionality, etc. with respect to the change of the rotational angular velocity ω and the driving time t. Further, the correction formula may be changed by dividing into sections according to the way of change.

  On the other hand, referring to FIG. 11B, in the characteristic parameter pattern 2, the combination of the first rotational angular velocity ω1 and the second rotational angular velocity ω2 which are different in a state where the drive to the mechanism unit is kept on. It shows a characteristic that the viscous friction coefficient C becomes a constant value after being attenuated by a linearly different slope from the initial value C0 when not operating. That is, FIG. 11B corresponds to an example of a case where a pattern is used as the characteristic parameter, which is set so as to attenuate gradually in a linear function after the start of the operation of the mechanism unit and gradually approach a predetermined value. .

  Specifically, in the characteristic diagram of FIG. 11B, the lower limit of attenuation of the first viscous friction coefficient C1 at the first rotational angular velocity ω1, the second smaller than that at the second rotational angular velocity ω2. It shows that the lower limit of attenuation of the viscous friction coefficient C2 is the lower limit of attenuation of the third viscous friction coefficient C3, which is smaller than that at the second rotational angular velocity ω2.

  Further, the moving rotational distance at that time shows a state obtained by integrating the absolute value of the rotational angular velocity ω indicating the first rotational angular velocity ω1 or the second rotational angular velocity ω2 by the on-time. FIG. 11B approximates the entire correction characteristic of the viscous friction coefficient C that changes so as to rapidly attenuate with respect to the change in the rotational angular velocity ω and the driving time t as shown in FIG. This corresponds to the characteristics when correcting.

  Further, referring to FIG. 11C, in the characteristic parameter pattern 3, the viscous friction coefficient is obtained by combining the first rotational angular velocity ω1 and the second rotational angular velocity ω2 which are different in the state where the driving of the mechanism unit is kept on. C shows a characteristic in which C becomes a constant value after being attenuated step by step with a linearly different slope from the initial value C0 when not operating. That is, FIG. 11C corresponds to another example in which a pattern that is set so that the characteristic parameter is attenuated in a linear function and gradually approaches a predetermined value after the operation of the mechanism unit is started as the characteristic parameter. Yes.

  Specifically, also in the characteristic diagram of FIG. 11C, the lower limit of attenuation of the first viscous friction coefficient C1 at the first rotational angular velocity ω1, the second smaller than that at the second rotational angular velocity ω2. This indicates that the lower limit of attenuation of the viscous friction coefficient C2 is smaller than the lower limit of attenuation of the third viscous friction coefficient C3 at the second rotational angular velocity ω2.

  Further, the moving rotational distance at that time shows a state obtained by integrating the absolute value of the rotational angular velocity ω indicating the first rotational angular velocity ω1 or the second rotational angular velocity ω2 by the on-time. FIG. 11C shows a linear function by dividing the correction characteristic of the viscous friction coefficient C that changes so as to be rapidly attenuated with respect to changes in the rotational angular velocity ω and the driving time t as shown in FIG. This corresponds to the characteristic when correction is performed by approximation for each stage. As shown in FIG. 11C, when there is a lot of data such as a function approximated by a linear function, if the storage unit 214 has these data in a table format in advance, the arithmetic processing in the CPU 210 is reduced or used. The storage capacity in the storage unit 214 can be reduced.

  In the characteristic parameter patterns 2 and 3 described above, the drive control unit 213 in the CPU 210 performs the following correction on the value of the viscous friction coefficient C. That is, the first viscous friction coefficient C1 when the rotational angular velocity ω at the rotational angular velocity detector used as the drive information detector 236 is the first rotational angular velocity ω1 and the second viscous friction when the second rotational angular velocity ω2 is used. In relation to the coefficient C2, when the first rotational angular velocity ω1 <the second rotational angular velocity ω2, the second viscous friction coefficient C2 <the first viscous friction coefficient C1 is set. With such a setting, although the viscous friction coefficient C decreases as the rotational angular velocity ω increases, the actual viscous friction coefficient C is not proportional to the rotational angular velocity ω, so that the torque is used rather than using a value proportional to the rotational angular velocity ω. The accuracy of estimation is improved.

  FIG. 12 shows the rotational angular velocity corresponding to the driving time t for the first mechanism unit by the first driving unit or the second mechanism unit by the second driving unit when the disturbance torque estimating unit shown in FIG. 9 is applied. It is the figure which showed the characteristic parameter in the case of changing omega on the way and correct | amending viscous friction coefficient C contrasted with the movement rotation distance. FIG. 4A is a diagram related to the characteristic parameter pattern 4, and FIG. 4B is a diagram related to the characteristic parameter pattern 5.

  Referring to FIG. 12A, in the characteristic parameter pattern 4, the viscous friction coefficient C is not operating in the state where the rotational angular velocity ω is delayed from the first rotational angular velocity ω1 to the third rotational angular velocity ω3. From the initial value C0 to the attenuation lower limit value of the first viscous friction coefficient C1, and using this as an initial value, a larger increase value of the third viscous friction coefficient C3 is obtained.

  In addition, the moving rotational distance at that time is a time period obtained by integrating the absolute value of the first rotational angular velocity ω1 by the time during driving at the first rotational angular velocity ω1, and driving at the third rotational angular velocity ω3. It shows a state of being divided into sections obtained by integrating the absolute value of the third rotational angular velocity ω3 by the time in the hour. That is, in FIG. 12A, the higher the rotational angular velocity ω, the smaller the viscous friction coefficient C. Therefore, the third viscous friction coefficient C3 is higher than the first viscous friction coefficient C1. The asymptotic value of the third viscous friction coefficient C3 is the same as the asymptotic value when driving at the third rotational angular velocity ω3 from the beginning. When the drive time t is used as a reference, the time of the first rotation angular velocity ω1 and the third rotation angular velocity ω3, and when the drive rotation distance is used as a reference, the first rotation angular velocity ω1 and the third rotation angular velocity. The viscous friction coefficient C is changed with respect to the moving rotation distance at ω3.

  On the other hand, referring to FIG. 12B, in the characteristic parameter pattern 5, the viscous friction coefficient C is not obtained in the state where the rotational angular velocity ω is increased from the first rotational angular velocity ω1 to the fourth rotational angular velocity ω4. The characteristic shows the initial value C0 in operation to the lower limit of attenuation of the first viscous friction coefficient C1, and the lower limit of attenuation of the fourth viscous friction coefficient C4 with the initial value as the initial lower limit.

  In addition, the moving rotational distance at that time is a section obtained by integrating the absolute value of the first rotational angular velocity ω1 by the time during driving at the first rotational angular velocity ω1, and driving at the fourth rotational angular velocity ω4. It shows a state in which it can be divided into sections obtained by integrating the absolute value of the fourth rotational angular velocity ω4 by the time in the hour. That is, in FIG. 12B, the viscous friction coefficient C decreases as the rotational angular velocity ω increases, so the fourth viscous friction coefficient C4 is smaller than the first viscous friction coefficient C1. Here again, the value of the fourth viscous friction coefficient C4 asymptotic is the same as the value asymptotic when driven at the fourth rotational angular velocity ω4 from the beginning. Furthermore, when the drive time t is used as a reference, the time of the first rotation angular velocity ω1 and the fourth rotation angular velocity ω4, and when the movement rotation distance is used as a reference, the first rotation angular velocity ω1 and the fourth rotation angular velocity ω4 are used. The viscous friction coefficient C is changed with respect to the moving rotational distance of the rotational angular velocity ω4.

  The drive control unit 213 in the CPU 210 performs the following correction on the value of the viscous friction coefficient C via the characteristic parameter patterns 4 and 5 and the interface cable 300. More specifically, the first viscous friction coefficient C1 when the rotation angular velocity ω in the rotation angular velocity detection unit used as the drive information detection unit 236 is the first rotation angular velocity ω1 and the first angular velocity ω3 when the rotation angular velocity ω3 is the third rotation angular velocity ω3. Attention is paid to the relationship with the viscous friction coefficient C3 of 3 or the fourth viscous friction coefficient C4 at the fourth rotational angular velocity ω4. Therefore, when switching from the first rotational angular velocity ω1 to the third rotational angular velocity ω3 or the fourth rotational angular velocity ω4, the first viscous friction coefficient C1 to the third viscous friction coefficient C3 or the fourth viscous friction coefficient C4. A value that changes continuously in time is set. With such a setting, although the viscous friction coefficient C differs depending on the rotational angular velocity ω, the viscous friction coefficient C does not change at the moment when the rotational angular velocity ω is changed, but at the rotational angular velocity ω with the driving time t. Since it approaches the viscous friction coefficient C, the accuracy of torque estimation can be improved in consideration of such points.

  FIG. 13 shows a characteristic parameter pattern 4 shown in FIG. 12A. When the rotational angular velocity ω starts to drive at the first rotational angular velocity ω1, sufficient time has elapsed and the viscous friction coefficient C becomes the first viscous friction coefficient. It is the figure which contrasted and showed the characteristic of the viscous friction coefficient C with respect to time at the time of changing to 3rd rotational angular velocity (omega) 3 before approaching C1, and rotational angular velocity (omega).

  Referring to FIG. 13, when the rotational angular velocity pattern A is smaller than the third viscous friction coefficient C3 with respect to the viscous friction coefficient C at the time of switching the rotational angular velocity ω, the rotational angular velocity pattern B is the third viscous friction coefficient. The case where it is larger than C3 is shown. In addition, in any of the rotational angular velocity patterns A and B, it is shown that the viscous friction coefficient C changes so as to approach the third viscous friction coefficient C3.

  FIG. 14 shows the drive time t after the drive control unit 213 drives the first mechanism unit via the first drive unit or the second mechanism unit via the second drive unit. 6 is a flowchart according to the calculation processing of the viscous friction coefficient C based on this and the operation processing of power transmission of the clutches 111 and 112. However, here, a table having function data calculated and acquired in advance is prepared, and the viscous friction coefficient C is calculated on the basis of the driving time t during driving and the rotational angular velocity ω at regular intervals from the start of driving.

  Referring to FIG. 14, the drive control unit 213 first performs viscous friction coefficient data acquisition (step S <b> 1) processing that reads and acquires viscous friction coefficient C data stored in the storage unit 214 in a table format. Next, it is determined whether or not the first mechanism unit or the second mechanism unit is instructed to be driven by operating the input unit 239 or the like to determine whether or not to start driving (step S2). If the result of this determination is that driving has not started, the process returns to before this determination (step S2) and waits so as to repeat the processing. On the other hand, if the driving has been started, a driving time measurement start process (step S3) for starting the measurement of the driving time t of the first mechanism section or the second mechanism section is performed. Furthermore, the drive control unit 213 performs drive time acquisition (step S4) processing for acquiring the drive time t.

  Thereafter, the drive control unit 213 determines whether or not a predetermined time has elapsed (step S5). As a result of this determination, if the predetermined time has not elapsed, the process returns to the process before the drive time acquisition (step S4) and the subsequent processes are repeated. On the other hand, if the fixed time has elapsed, the rotational angular velocity detection (step S6) processing for detecting the rotational angular velocity ω of the motor 105 included in the first mechanism unit and the motor 109 included in the second mechanism unit is performed. Do. Further, the drive control unit 213 performs a process of calculating the viscous friction coefficient C from the acquired drive time t and the detected rotational angular velocity ω (step S7). Furthermore, the drive control unit 213 determines whether or not the difference from the previous value of the viscous friction coefficient C is less than a certain value (step S8).

  As a result of this determination, if the difference is not less than a certain value but greater than a certain value, the process returns to the previous drive time acquisition (step S4) and the subsequent processes are repeated. On the other hand, if the difference is less than a certain value, the process of turning on the power transmission by the clutches 111 and 112 is performed (step S9), and then the operation process ends.

  FIG. 15 shows the movement rotation after the drive control unit 213 moves after driving to the first mechanism unit via the first drive unit or the second mechanism unit via the second drive unit. It is a flowchart which concerns on the calculation process of the viscous friction coefficient C based on distance, and the operation | movement process of the power transmission of the clutch 111,112. However, here, a table having function data calculated and acquired in advance is prepared, and the viscous friction coefficient C is calculated on the basis of the moving rotational distance and rotational angular velocity ω being driven at regular intervals from the start of driving.

  Referring to FIG. 15, the processes from the viscous friction coefficient data acquisition (step S1) to the rotational angular velocity detection (step S6) described in FIG. Here, after the rotation angular velocity detection (step S6), the drive control unit 213 performs a movement rotation distance calculation (step S7) that calculates a movement rotation distance. Further, the drive control unit 213 performs a process of calculating a viscous friction coefficient C from the calculated moving rotational distance and the detected rotational angular velocity ω (step S8). Thereafter, the drive control unit 213 determines whether or not the difference from the previous value of the viscous friction coefficient C is less than a certain value (step S9). As a result of this determination, if the difference is not less than a certain value but greater than a certain value, the process returns to the previous drive time acquisition (step S4) and the subsequent processes are repeated. On the other hand, if the difference is less than a certain value, the process of turning on the power transmission by the clutches 111 and 112 is performed (step 10), and then the operation process is terminated.

  14 and 15, the change in the viscous friction coefficient C is not a change in units of several milliseconds but is often a change in units of several seconds to several tens of seconds. For this reason, when considering the arithmetic processing, it is desirable to carry out a determination (step S5) as to whether or not a certain period of time has passed as if they were shared by both, but the values are updated sequentially. You may do it.

  FIG. 16 is based on the drive time t after the drive control unit 213 drives the first mechanism unit via the first drive unit or the second mechanism unit via the second drive unit. 6 is a timing chart showing the on / off relationship of power transmission by the clutches 111 and 112 when the viscous friction coefficient C is calculated.

  Referring to FIG. 16, here, the viscous friction coefficient C is calculated every 5 seconds (s) after the start of driving, and if the difference from the previous value of the viscous friction coefficient C is less than 1.0E-5, the drive control is performed. The part 213 shows a setting for turning on the power transmission by the clutches 111 and 112. The on-timing of the clutches 111 and 112 corresponds to when the drive time t has elapsed 40 seconds (s) and the motors 105 and 109 have a rotational speed of 300 [rad / s]. Incidentally, the set value 1.0E-5 here can be changed by operating the touch panel a of the input unit 239.

  As described above, according to the various control devices according to the embodiment, the drive control unit 213 determines the value of the viscosity coefficient or the viscous friction coefficient C estimated by the disturbance estimation unit 238 based on a predetermined characteristic parameter. calculate. Then, after the drive of the mechanism unit by the drive unit is started, the power transmission of the single clutch 111 or the pair of clutches 111 and 112 as the power transmission mechanism unit is performed after the viscosity coefficient or the viscous friction coefficient C falls within a certain range. Make it. For this reason, even if it is applied to a mechanism unit having a power transmission unit having a viscosity change, it is possible to improve the accuracy of disturbance estimation as much as possible so as to match the driving state of the driving unit in consideration of the viscosity change with time. it can.

DESCRIPTION OF SYMBOLS 101 Arm 102 Base 103 Drive center 104, 117 Motor encoder 105, 109 Motor 106, 110 Reducer 107 External connection device 108 Base plate 111, 112 Clutch 113, 114, 116 Gear 115 Shaft 200 Manipulator device 201 Manipulator main body 210 CPU
211 Communication control unit 212 Timer 213 Drive control unit 214 Storage unit 220 Various sensors 221 Various switches 223 Clock 230, 232 Actuators 231, 233 Encoder 234, 240 Current detection unit 235 Motor driver 236 Drive information detection unit 238 Disturbance estimation unit 239 Input unit 401 Gravity term adding section 402, 601 Viscous friction coefficient adding section 403A, 403B Torque estimating section 404 Actual machine section 501 PID controller 502 Current driver 503 Manipulator

JP 2011-015550 A

Claims (16)

A drive unit that drives a control object; a mechanism unit that is connected to the control object and the drive unit and has a viscosity change including a lubricant; and a support unit that supports the control object and the mechanism unit; A power transmission mechanism unit connected to the mechanism unit and the support unit to turn on / off power transmission from the drive unit to the support unit, and drive information detection for detecting drive information from the drive unit to the mechanism unit A current detection unit that detects a current flowing through the drive unit, a load disturbance applied to the mechanism unit, the current detected by the current detection unit, and the drive information detected by the drive information detection unit A disturbance estimation unit that estimates based on the viscosity, a viscosity coefficient relating to the drive unit and the mechanism unit included in the load disturbance estimated by the disturbance estimation unit, and a drive information detection unit that detects a drive command of the drive unit Is A drive control apparatus and a control section for controlling, based on said driving information,
The control unit calculates the value of the viscosity coefficient estimated by the disturbance estimation unit based on a characteristic parameter of a predetermined function data, and the viscosity coefficient is calculated after the drive unit starts driving the mechanism unit. A drive control device characterized in that power transmission of the power transmission mechanism section is performed after being within a certain range. The drive control device according to claim 1,
The control unit uses, as the characteristic parameter, a pattern set so as to be asymptotic to a predetermined value by changing in a linear function after the driving unit starts driving the mechanism unit. Drive control device. The drive control device according to claim 1,
The control unit uses, as the characteristic parameter, a pattern set so as to gradually decay to an exponential value after the start of driving of the mechanism unit by the driving unit. Drive control device. The drive control device according to claim 1,
The drive information detection unit is a rotation angular velocity detection unit that detects a rotation angular velocity of the mechanism unit as the drive information,
The drive control device, wherein the control unit is capable of switching the rotation angular velocity by the rotation angular velocity detection unit for making the viscosity coefficient asymptotic to the predetermined value.   A torque control device comprising the drive control device according to claim 1, wherein an electric motor provided in the mechanism portion is driven by an actuator of the drive portion. A drive unit that drives a control object; a first mechanism unit that is connected to the control object and the drive unit and has a viscosity change including a lubricant; and a first mechanism unit that is connected to the drive unit, A second mechanism part having a viscosity change including a lubricant driven in a direction opposite to the part, and a support part for supporting the control object, the first mechanism part, and the second mechanism part A first power transmission mechanism unit connected to the first mechanism unit and the support unit and configured to turn on / off power transmission from the drive unit to the support unit; and the second mechanism unit and the support unit. A second power transmission mechanism connected to the drive unit for turning on / off the power transmission from the drive unit to the support unit, and driving information of the first mechanism unit and the second mechanism unit by the drive unit A drive information detection unit for detecting current, a current detection unit for detecting current flowing in the drive unit, A disturbance estimation unit that estimates a load disturbance applied to the first mechanism unit and the second mechanism unit based on the current detected by the current detection unit and the drive information detected by the drive information detection unit And the respective viscosity coefficients related to the drive unit, the first mechanism unit, and the second mechanism unit included in the load disturbance estimated by the disturbance estimation unit with a drive command to the drive unit, and A drive control device comprising: a control unit that performs control based on the drive information detected by the drive information detection unit;
The control unit calculates a value of the viscosity coefficient estimated by the disturbance estimation unit based on a characteristic parameter of predetermined function data, and the first mechanism unit and the second mechanism by the driving unit. A drive control device that causes the first power transmission mechanism unit and the second power transmission mechanism unit to perform power transmission after the viscosity coefficient falls within a certain range from the start of driving of the unit. The drive control device according to claim 6,
The control unit is set as the characteristic parameter so as to gradually approach a predetermined value by changing in a linear function after the driving unit starts driving the first mechanism unit and the second mechanism unit. A drive control apparatus using the patterned pattern. The drive control device according to claim 6,
The control unit sets the characteristic parameter so as to gradually approach a predetermined value by exponentially decaying from the start of driving of the first mechanism unit and the second mechanism unit by the drive unit. A drive control apparatus using the patterned pattern. The drive control device according to claim 6,
The drive information detection unit is a rotation angular velocity detection unit that detects a rotation angular velocity of at least one of the first mechanism unit and the second mechanism unit as the drive information,
The drive control device, wherein the control unit is capable of switching the rotation angular velocity by the rotation angular velocity detection unit for making the viscosity coefficient asymptotic to the predetermined value.   A drive control device according to any one of claims 6 to 9, comprising: an electric motor provided in one of the first mechanism portion and the second mechanism portion by an actuator of the drive portion. Torque control device characterized by the above. A first drive unit that drives the controlled object, a second drive unit that drives the controlled object, and a viscosity change that includes a lubricant connected to the controlled object and the first drive unit. A first mechanism having a second mechanism having a viscosity change including a lubricant that is connected to the control object and the second drive unit and is driven in a direction opposite to the first mechanism unit. A support unit that supports the control unit, the first mechanism unit, and the second mechanism unit, and the first drive unit connected to the first mechanism unit and the support unit. A first power transmission mechanism that turns on / off power transmission to the support, and the second mechanism and the support connected to the second drive and transfer the power from the second drive to the support. A second power transmission mechanism that turns on and off; the first mechanism by the first drive; and the second A driving information detecting unit for detecting driving information with respect to the second mechanism unit by the moving unit; a first current detecting unit for detecting a first current flowing through the first driving unit; and the second driving. A second current detection unit for detecting a second current flowing through the first part, the first mechanism unit, and a load disturbance applied to the second mechanism unit detected by the first current detection unit. , A disturbance estimation unit that estimates the current based on the second current detected by the second current detection unit and the drive information detected by the drive information detection unit, and the first drive unit The first drive unit, the second drive unit and the first mechanism unit, and the second drive unit included in the load disturbance estimated by the disturbance estimation unit with a drive command to the second drive unit. And the respective viscosity coefficients relating to the mechanism part of the motor, and detected by the drive information detection part A drive control apparatus and a control section for controlling, based on the serial driving information,
The control unit calculates a value of the viscosity coefficient estimated by the disturbance estimation unit based on a characteristic parameter of predetermined function data, and the first mechanism unit by the first drive unit, the first After the start of the driving of the second mechanism by the second driving unit, the power transmission of the first power transmission mechanism and the second power transmission mechanism is performed after the viscosity coefficient is within a certain range. A drive control device. The drive control device according to claim 11,
The control unit changes as the characteristic parameter in a linear function from the start of driving of the first mechanism unit by the first drive unit and the second mechanism unit by the second drive unit. A drive control apparatus using a pattern set so as to be asymptotic to a predetermined value. The drive control device according to claim 11,
The control unit attenuates exponentially as the characteristic parameter from the start of driving of the first mechanism unit by the first drive unit and the second mechanism unit by the second drive unit. A drive control apparatus using a pattern set so as to be asymptotic to a predetermined value. The drive control device according to claim 11,
The drive information detection unit is a rotation angular velocity detection unit that detects a rotation angular velocity of at least one of the first mechanism unit and the second mechanism unit as the drive information,
The drive control device, wherein the control unit is capable of switching the rotation angular velocity by the rotation angular velocity detection unit for making the viscosity coefficient asymptotic to the predetermined value.   15. A drive control apparatus according to claim 11, comprising the drive mechanism according to any one of claims 11 to 14, wherein the first mechanism section and the second mechanism section are provided by an actuator of the first drive section and the second drive section. A torque control device that drives an electric motor.   A torque control device according to any one of claims 5, 10, and 15 is provided, and assist control of the joint of the control object is performed by an output of the electric motor with respect to an external force of an operation force command value from an input unit. Assist control device characterized.