WO2014021433A1 - ロボット装置およびその制御方法 - Google Patents

ロボット装置およびその制御方法 Download PDF

Info

Publication number
WO2014021433A1
WO2014021433A1 PCT/JP2013/070918 JP2013070918W WO2014021433A1 WO 2014021433 A1 WO2014021433 A1 WO 2014021433A1 JP 2013070918 W JP2013070918 W JP 2013070918W WO 2014021433 A1 WO2014021433 A1 WO 2014021433A1
Authority
WO
WIPO (PCT)
Prior art keywords
joint
torque
force torque
frictional force
estimation unit
Prior art date
Application number
PCT/JP2013/070918
Other languages
English (en)
French (fr)
Inventor
佑太 松本
泰宣 西原
純 藤田
Original Assignee
東芝機械株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 東芝機械株式会社 filed Critical 東芝機械株式会社
Priority to GB1501379.0A priority Critical patent/GB2518576B/en
Priority to CN201380041006.7A priority patent/CN104507645B/zh
Priority to JP2014528226A priority patent/JP5727103B2/ja
Priority to US14/418,416 priority patent/US9434073B2/en
Publication of WO2014021433A1 publication Critical patent/WO2014021433A1/ja

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1641—Programme controls characterised by the control loop compensation for backlash, friction, compliance, elasticity in the joints
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/39—Robotics, robotics to robotics hand
    • G05B2219/39181—Compensation of coulomb friction in joint
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/39—Robotics, robotics to robotics hand
    • G05B2219/39187—Coriolis and centripetal compensation
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/39—Robotics, robotics to robotics hand
    • G05B2219/39194—Compensation gravity

Definitions

  • the present invention relates to a robot apparatus that drives a joint by a drive motor and a control method thereof.
  • Non-Patent Document 1 In a conventional robot apparatus that controls driving in consideration of friction, control is performed by arbitrarily providing a hysteresis called a dead zone in a region where the speed is near 0 (Patent Document 1).
  • Patent Document 1 On the other hand, when controlling the drive of a controlled object in consideration of friction, since the controlled object is affected by Coulomb friction and viscous friction, both are identified and compensated (Patent Literature). 2).
  • the area is affected by the Coulomb friction and the area affected by the viscous friction, and the friction force in each area is estimated based on the model formula of the discontinuous friction model.
  • the present invention solves the above-mentioned problem that occurs in a conventional robot apparatus that controls the driving of a joint in consideration of the frictional force torque acting on the joint. It is an object of the present invention to provide a robot apparatus that is smoothly driven and controlled even when shifting to an affected area or when shifting from an area affected by viscous friction to an area affected by Coulomb friction. .
  • a robot apparatus includes a joint driven by a drive motor, a rotation angle detection unit that detects a rotation angle of the drive motor, and a detection value of the rotation angle detection unit.
  • a control device that performs correction based on an estimated value of a joint torque estimator that estimates a joint torque acting on the joint when controlling driving, and the joint torque estimator includes an inertia torque estimator and a Coriolis force torque estimator.
  • the friction force torque estimation unit includes a Coulomb friction force torque estimation unit, a viscous friction force torque estimation unit, and a Coulomb friction force torque.
  • a transition interval calculation unit that smoothes the transition from the estimation unit to the viscous friction force torque estimation unit and the transition from the viscous friction force torque estimation unit to the Coulomb friction force torque estimation unit. And butterflies.
  • control method of the robot apparatus of the present invention is a control method of the robot apparatus that performs correction from the estimated value of the joint torque acting on the joint when driving the joint from the detected value of the rotation angle of the drive motor.
  • the transition from coulomb friction force torque to viscous friction force torque is made.
  • the frictional force torque during the transition from the frictional force torque and the viscous frictional force torque to the coulomb frictional force torque is calculated and estimated by the transition interval calculation unit.
  • FIG. 1 is a perspective view showing a robot apparatus according to an embodiment of the present invention.
  • FIG. 2 is a schematic diagram illustrating a structure of a joint of the robot apparatus.
  • FIG. 3 is a control block diagram of the controller of the robot apparatus.
  • FIG. 4 is a control block diagram of a joint torque estimation unit of the control device.
  • FIG. 5 is a graph showing the friction torque estimated by the friction torque estimation unit of the joint torque estimation unit.
  • 6 is a flowchart of the robot apparatus shown in FIG.
  • FIG. 7 is a flowchart of the joint torque estimation unit.
  • FIG. 8 is a flowchart of the friction torque estimating unit.
  • FIG. 1 to FIG. 8 are diagrams showing an embodiment of the present invention.
  • the robot apparatus 1 includes a robot body 2 and a control device 3 connected to the robot body 2 and controlling the driving of the robot body 2.
  • the robot body 2 is provided with a base 10 and sequentially from the base 10 and includes six joints 20 to 25 (first joint 20, second joint 21, third joint 22, fourth joint 23, fifth joint 24, and Six arm bodies 30 to 35 (the first arm body 30, the second arm body 31, the third arm body 32, the fourth arm body 33, the fifth arm body) connected to each other via the sixth joint 25). 34, and a sixth arm body 35).
  • the arm shaft 40 that is provided integrally with the first arm body 30 and constitutes the first joint 20 is connected at one end to the output side of the speed reducer 41 attached to the base 10. Yes.
  • a drive motor 42 is connected to the input side of the speed reducer 41, and the drive motor 42 is provided with an encoder 43 that is a rotation angle detection means for detecting the rotation angle.
  • the drive motor 42 rotates forward or backward, the rotation is decelerated by the speed reducer 41 and transmitted to the arm shaft 40, and the first arm body 30 rotates in the forward or reverse direction.
  • the second joint 21 to the sixth joint 25 also have the arm shaft 40 as in the first joint 20, and are provided with a speed reducer 41, a drive motor 42, and an encoder 43, and operate in the same manner. Is omitted.
  • control device 3 will be described with reference to FIGS.
  • the control device 3 includes a position command unit 50, a ⁇ ⁇ ⁇ ′ ⁇ ⁇ ”calculation unit 51, a servo control calculation unit 52, a joint torque estimation unit 53, an external force calculation unit 54, A compliance model calculation unit 55 and a control target unit 56 are provided.
  • the control device 3 Based on the detection value of the encoder 43 provided in each drive motor 42 of each joint 20 to 25 of the robot body 2, the control device 3 causes the position command unit 50 to issue a command regarding the target position / posture at a predetermined control cycle.
  • the control target unit 56 that has issued the command and operates the respective drive motors 42 drives the joints 20-25.
  • the ⁇ ⁇ ⁇ ′ ⁇ ⁇ ”calculation unit 51 adds a command related to the target position / posture calculated by the position command unit 50 and a correction amount obtained by the compliance model calculation unit 55 described later, and is based on inverse kinematics. Calculation is performed to calculate a joint angle command ⁇ , a joint angular velocity command ⁇ ′, and a joint angular acceleration command ⁇ ′′.
  • the servo control calculation unit 52 generates a torque command issued to the control target unit 56 based on the joint angle command ⁇ , the joint angular velocity command ⁇ ′, and the joint angular acceleration command ⁇ ′′ calculated by the ⁇ ⁇ ⁇ ′ ⁇ ⁇ ′′ calculation unit 51. Is calculated.
  • the joint torque estimation unit 53 estimates the joint torque acting on each joint 20 to 25 based on the dynamic model, and details thereof will be described later.
  • the external force calculation unit 54 calculates the estimated external force based on the error torque obtained by subtracting the estimated torque value of the joint torque estimated by the joint torque estimation unit 53 from the torque command calculated by the servo control calculation unit 52. calculate.
  • the compliance model calculation unit 55 calculates a correction amount related to the target position / attitude based on the estimated external force calculated by the external force calculation unit 54.
  • the joint torque estimating unit 53 estimates the joint torque based on the model formula of the dynamic model.
  • Estimated joint torque T ( ⁇ ) acting on n joints is expressed by the following equation.
  • T ( ⁇ ) M ( ⁇ ) ⁇ ′′ + B ( ⁇ ) [ ⁇ a′ ⁇ b ′] + C ( ⁇ ) [ ⁇ ′ 2 ] + G ( ⁇ ) + F ( ⁇ ′)
  • T ( ⁇ ) M ( ⁇ ) ⁇ ′′ + B ( ⁇ ) [ ⁇ a′ ⁇ b ′] + C ( ⁇ ) [ ⁇ ′ 2 ] + G ( ⁇ ) + F ( ⁇ ′)
  • ⁇ ′ 2 Joint angular velocity product matrix of the same joint (matrix of n rows and 1 column)
  • ⁇ a′ ⁇ b ′ Joint angular velocity product matrix of different joints ( ⁇ n ⁇ (n ⁇ 1) / 2 ⁇ ⁇ 1 matrix)
  • the joint torque estimation unit 53 is based on the above-described equation of the estimated joint torque T ( ⁇ ), and the inertia torque estimation unit 60, the Coriolis force torque estimation unit 61, the centrifugal force torque estimation unit 62, and the gravity torque.
  • An estimation unit 63 and a frictional force torque estimation unit 64 are provided.
  • the inertia torque estimation unit 60 applies a joint angle command ⁇ and a joint angular acceleration command ⁇ ′′ issued at a predetermined control period based on the term M ( ⁇ ) ⁇ ′′ in the above-described estimated joint torque T ( ⁇ ). Thus, the inertia torque generated by the inertia is calculated.
  • the Coriolis force torque estimation unit 61 generates a joint angle command ⁇ and a joint angular velocity command issued at a predetermined control cycle based on the term B ( ⁇ ) [ ⁇ a′ ⁇ b ′] in the above-described estimated joint torque T ( ⁇ ). Coriolis force torque generated by Coriolis force is calculated with respect to ⁇ ′.
  • the centrifugal torque estimation unit 62 generates a joint angle command ⁇ and a joint angular velocity command ⁇ issued in a predetermined control cycle based on the term C ( ⁇ ) [ ⁇ ′ 2 ] in the above-described estimated joint torque T ( ⁇ ). For ', calculate the centrifugal torque generated by the centrifugal force.
  • the gravitational torque estimating unit 63 calculates the gravitational torque generated by gravity with respect to the joint angle command ⁇ issued at a predetermined control cycle based on the term G ( ⁇ ) in the above-described estimated joint torque T ( ⁇ ). To do.
  • the frictional force torque estimation unit 64 applies a joint angle command ⁇ and a joint angular velocity command ⁇ ′ issued at a predetermined control cycle based on the term F ( ⁇ ′) in the above-described estimated joint torque T ( ⁇ ).
  • the frictional force torque generated by the frictional force is calculated.
  • the joint torque estimation unit 53 includes the inertia torque calculated by the inertia torque estimation unit 60, the Coriolis force torque calculated by the Coriolis force torque estimation unit 61, the centrifugal force torque calculated by the centrifugal force torque estimation unit 62, The joint torque is estimated by adding the gravity torque calculated by the gravity torque estimation unit 63 and the friction force torque calculated by the friction force torque estimation unit 64 to obtain an estimated value of the joint torque.
  • the friction force torque estimation unit 64 includes a Coulomb friction force torque estimation unit 70 based on a model equation of the Coulomb friction model, and viscous friction. And a viscous frictional force torque estimating unit 71 based on the model equation of the model.
  • the frictional force torque estimation unit 64 further includes a transition from the Coulomb frictional force torque estimation unit 70 to the viscous frictional force torque estimation unit 71, and a viscous frictional force torque estimation unit 71. Is provided with a transition interval calculation unit 72 that smoothes the transition from the coulomb friction force torque estimation unit 70 to the coulomb friction force torque estimation unit 70.
  • the frictional force torque estimation unit 64 selects one of the coulomb frictional force torque estimation unit 70, the viscous frictional force torque estimation unit 71, and the transition section calculation unit 72 according to the joint angular velocity. Also have.
  • fv ( ⁇ ′) represents a predetermined function according to the model formula of the viscous friction model using the joint angular velocity command ⁇ ′ as a variable.
  • k represents a frictional force torque coefficient which will be described later
  • ⁇ s represents a joint angle command at the previous command in a predetermined control cycle.
  • the frictional force torque coefficient k is applied to each joint 20-25 when the joints 20-25 are driven in advance, and the friction affecting each joint 20-25 shifts from coulomb friction to viscous friction as the joint angle increases. It is determined by measuring the acting joint torque and analyzing the measurement result. A plurality of current values of the drive motor 42 and detection values of the encoder 43 of each joint 20 to 25 are measured at a predetermined sampling period, and the current value of the drive motor 42 is converted into a joint torque and the detection value of the encoder 43 is converted into a joint angle. To do.
  • the inertia torque, the Coriolis torque, the centrifugal torque, and the gravity torque acting on each joint 20 to 25 at the time of measurement are calculated and subtracted from the converted joint torque.
  • the frictional force torque acting on each joint 20 to 25 at the time of measurement is extracted.
  • Fm ( ⁇ ) between the joint angle and the friction force torque based on the measurement is obtained.
  • the relationship Fm ( ⁇ ) between the joint angle based on the measurement and the friction force torque is such that, in the region where the joint angle is small, the friction force torque is substantially constant with respect to the change in the joint angle due to the effect of Coulomb friction. Beyond this minute region, the friction torque increases smoothly as the joint angle increases. In the region where the frictional force torque increases smoothly, the frictional torque gradient km with respect to the joint angle is extracted, and the inclination km is defined as the frictional force torque coefficient k.
  • the current value of the drive motor 42 is converted into the joint torque, and the detection value of the encoder 43 is converted into the joint angle.
  • the joint torque and the joint angle are measured using another measuring device. Good.
  • the frictional force torque coefficient k for each robot device 1, but any one of the plurality of robot devices 1 of the same specification manufactured in the same process is selected.
  • the frictional force torque coefficient k of one robot apparatus 1 may be obtained and the frictional force torque coefficient k may be applied to the remaining robot apparatuses 1.
  • the changeover switch 73 is one of the coulomb friction force torque estimation unit 70, the viscous friction force torque estimation unit 71, and the transition section calculation unit 72 in accordance with the joint angular velocity command ⁇ ′ of each joint 20-25. Select.
  • the joint angular velocity command ⁇ ′ is a positive value
  • the joint angular velocity command ⁇ ′ is a negative value.
  • the changeover switch 73 selects and connects the Coulomb friction force torque estimation unit 70 to estimate the friction force torque.
  • the unit 64 estimates the frictional force torque acting on each joint 20 to 25 by being calculated by the Coulomb frictional force torque estimating unit 70.
  • ⁇ 1 ′ represents a Coulomb friction switching joint speed command ⁇ 1 ′, which will be described later.
  • the changeover switch 73 selects and connects the transition section calculation unit 72 to estimate the frictional force torque.
  • the unit 64 estimates the frictional force torque acting on each of the joints 20 to 25 by being calculated by the transition section calculating unit 72.
  • ⁇ 2 ′ represents a viscous friction switching joint speed command ⁇ 2 ′, which will be described later.
  • the changeover switch 73 selects and connects the viscous frictional force torque estimating unit 71, and the frictional force torque estimating unit 64 selects the viscous frictional force torque.
  • the frictional force torque acting on each joint 20 to 25 is estimated. Note that the direct switching from the coulomb friction force torque estimating unit 70 to the viscous friction force torque estimating unit 71 and the direct switching from the viscous friction force torque estimating unit 71 to the coulomb friction force torque estimating unit 70 are not performed.
  • the Coulomb friction switching joint speed command ⁇ 1 ′ is calculated based on the Coulomb friction force torque Fc ( ⁇ ) calculated by the Coulomb friction force torque estimation unit 70 according to the Coulomb friction model, and the relationship Fm ( ⁇ ) and the joint angular velocity when Fm ( ⁇ ) exceeds Fc ( ⁇ ) is applied.
  • the viscous friction switching joint speed command ⁇ 2 ′ includes the viscous frictional force torque Fv ( ⁇ ) calculated by the viscous frictional force torque estimating unit 71 according to the viscous friction model, and the relationship Fm ( ⁇ ) and the joint angular velocity when Fm ( ⁇ ) reaches the minimum value of Fv ( ⁇ ) is applied.
  • the changeover switch 73 switches between the Coulomb friction force torque estimation unit 70 and the transition section calculation unit 72, and the switch between the viscous friction force torque estimation unit 71 and the transition section calculation unit 72 is a joint angular velocity command. This is performed by comparing ⁇ ′ with the Coulomb friction switching joint speed command ⁇ 1 ′ and the viscous friction switching joint speed command ⁇ 2 ′, but the conditions for switching may be changed or added as appropriate.
  • the switching from the transition section calculating unit 72 to the viscous friction force torque estimating unit 71 is such that the joint angular velocity command ⁇ ′ is equal to or greater than the viscous friction switching joint velocity command ⁇ 2 ′ ( ⁇ 2 ′ ⁇
  • the joint angular velocity command ⁇ ′ becomes faster than the viscous friction switching joint velocity command ⁇ 2 ′ ( ⁇ 2 ′ ⁇
  • FIG. 5 shows a graph of the friction force torque estimated by the friction force torque estimation unit 64 of the present embodiment for each joint 20-25.
  • the horizontal axis indicates the joint angular velocity
  • the vertical axis indicates the friction force torque
  • the white circle ( ⁇ ) indicates the friction force torque estimated by the friction force torque estimation unit 64.
  • the joint angular velocity command ⁇ ′ is in the range of 0 ⁇
  • the frictional torque estimation unit 64 performs the Coulomb frictional force torque estimation unit in a predetermined control cycle.
  • the frictional torque is estimated by calculation at 70.
  • the joint angular velocity command ⁇ ′ is in the range of ⁇ 1 ′ ⁇
  • the frictional torque estimation unit 64 performs the transition section calculation unit 72 in a predetermined control cycle. And the frictional torque is estimated.
  • the joint angular velocity command ⁇ ′ is in the range of ⁇ 2 ′ ⁇
  • the frictional torque estimation unit 64 is the viscous frictional force torque estimation unit 71 in a predetermined control cycle.
  • the frictional torque is calculated and estimated.
  • the friction torque of the sections B and B ′ estimated by the transition section calculation unit 72 is estimated based on the friction force torque of the sections A and A ′ estimated based on the model expression of the Coulomb friction model and the model expression of the viscous friction model.
  • the frictional force torques in the sections C and C ′ are smoothly connected.
  • the frictional force torque estimation unit 64 includes the transition section calculation unit 72
  • the coulomb frictional force torque estimation unit 70 changes to the viscous frictional force torque estimation unit 71 via the transition section calculation unit 72. Then, the transition is smoothly made from the viscous frictional force torque estimation unit 71 to the coulomb frictional force torque estimation unit 70 via the transition interval calculation unit 72.
  • the robot apparatus 1 is servo-on in step S100, and the control apparatus 3 is activated.
  • step S101 the control device 3 reads the parameter of the equation of the estimated joint torque T ( ⁇ ) based on the dynamic model stored in the memory.
  • the parameters to be read are applied to the inertia torque estimating unit 60, the Coriolis force torque estimating unit 61, the centrifugal torque estimating unit 62, and the gravity torque estimating unit 63 of the joint torque estimating unit 53, and based on the dimensions and mass of the robot body 2 in advance.
  • step S102 the control device 3 reads the target position and posture to be reached at the tip of the robot body 2 stored in the memory.
  • step S103 the control device 3 uses the position command section 50 to set the target position / value at a predetermined control cycle based on the detection value of the encoder 43 provided in each drive motor 42 of each joint 20-25. Posture is calculated and a command related to the target position / posture is issued.
  • step S104 the control device 3 performs a calculation based on the inverse kinematics in the ⁇ ⁇ ⁇ ' ⁇ ⁇ "calculation unit 51 based on the command related to the target position / orientation, thereby obtaining the joint angle command ⁇ and the joint angular velocity command.
  • ⁇ ′ and the joint angular acceleration command ⁇ ′′ are calculated.
  • step S105 the control device 3 calculates a torque command value by the servo control calculation unit 52 based on the joint angle command ⁇ , the joint angular velocity command ⁇ ′, and the joint angular acceleration command ⁇ ′′.
  • step S106 based on the torque command, the control device 3 causes the control target unit 56 to operate the respective drive motors 42 to rotate the joints 20 to 25 of the robot body 2.
  • step S107 if the position / posture of each joint 20-25 of the robot body 2 has reached the target value to be reached, the control device 3 proceeds to step S108 and servo-off. If the target value has not been reached, the process returns to step S103.
  • step S109 the control device 3 uses the joint torque estimation unit 53 to calculate each joint based on the joint angle command ⁇ , the joint angular velocity command ⁇ ′, and the joint angular acceleration command ⁇ ′′ calculated in step S104.
  • the joint torque acting on each of 20 to 25 is estimated, and in step S110, the error command is calculated by subtracting the torque command calculated in step S105 from the estimated torque value of the estimated joint torque.
  • step S111 the control device 3 calculates an estimated external force in the external force calculation unit 54 based on the error torque, and in step S112, the compliance model calculation unit 55 calculates the target position based on the estimated external force. ⁇ Calculate the amount of posture correction.
  • the correction amount of the target position / posture is added to the target position / posture calculated in step S103, and the calculated target position / posture becomes the target position / posture considering the compliance control.
  • the joint torque estimation unit 53 starts estimating the joint torque in step S200.
  • step S201 the joint torque estimation unit 53 calculates the inertia torque by the inertia torque estimation unit 60.
  • step S202 the joint torque estimation unit 53 calculates the Coriolis force torque by the Coriolis force torque estimation unit 61.
  • step S203 the joint torque estimating unit 53 calculates the centrifugal force torque by the centrifugal torque estimating unit 62.
  • step S204 the joint torque estimation unit 53 calculates the gravity torque by the gravity torque estimation unit 63.
  • step S205 the joint torque estimating unit 53 calculates the friction force torque by the friction force torque estimating unit 64.
  • step S206 the joint torque estimation unit 53 adds the inertia torque, the Coriolis force torque, the centrifugal force torque, the gravity torque, and the friction force torque calculated in steps S201 to S205 to estimate the joint torque.
  • step S207 the joint torque estimation unit 53 ends the estimation of the joint torque.
  • the frictional force torque estimation unit 64 starts estimating the frictional force torque in step S300.
  • step S301 the frictional torque estimation unit 64 determines whether or not the coulomb frictional force torque estimation section is in the coulomb frictional force torque estimation section, that is, the joint angular velocity command ⁇ ′ is in the range of 0 ⁇
  • step S ⁇ b> 305 the frictional force torque estimation unit 64 estimates the frictional force torque by calculating the Coulomb frictional force torque Fc ( ⁇ ) in the coulomb frictional force torque estimation unit 70.
  • step S302 the frictional torque estimation unit 64 determines whether or not the changeover switch 73 is in the transition section, that is, whether the joint angular velocity command ⁇ ′ is in the range of ⁇ 1 ′ ⁇
  • step S ⁇ b> 306 the frictional force torque estimation unit 64 estimates the frictional force torque by calculating the transitional interval frictional force torque Ft ( ⁇ ) in the transitional interval calculating unit 72.
  • step S303 the frictional torque estimation unit 64 determines whether the frictional torque torque estimation section 64 is in the viscous frictional force torque estimation section, that is, whether the joint angular velocity command ⁇ ′ is in the range of ⁇ 2 ′ ⁇
  • step S307 the frictional force torque estimation unit 64 calculates the viscous frictional force torque Fv ( ⁇ ) by the viscous frictional force torque estimation unit 71, thereby estimating the frictional force torque.
  • step S304 the frictional force torque estimation unit 64 ends the estimation of the frictional force torque.
  • the present invention can be implemented with appropriate modifications depending on the specific configuration of the robot apparatus. Depending on the work to be performed, the number of joints and arm bodies of the robot body and the number of control points of the control device that controls them are determined.

Abstract

　ロボット装置（１）は、各関節（２０～２５）に作用する関節トルクを推定する関節トルク推定部（５３）の推定値より補正を行う制御装置（３）を備え、関節トルク推定部（５３）は、クーロン摩擦力トルク推定部（７０）と粘性摩擦力トルク推定部（７１）と、クーロン摩擦力トルク推定部（７０）から粘性摩擦力トルク推定部（７１）への移行と粘性摩擦力トルク推定部（７１）からクーロン摩擦力トルク推定部（７０）への移行を滑らかにする移行区間演算部（７２）とを有する。

Description

ロボット装置およびその制御方法
　本発明は、駆動モータにより関節を駆動するロボット装置およびその制御方法に関する。
　ロボット装置の関節の駆動を制御する場合、バックラッシュ等の機械的精度により、関節の回転方向が正回転から負回転に、または、負回転から正回転に反転するときの関節角速度と関節トルクの関係が定まらないという問題が生じ易い。
　摩擦を考慮して駆動を制御する従来のロボット装置では、この問題に対して、速度が０付近の領域に不感帯と呼ばれるヒステリシスを任意に設けて制御を行っている（特許文献１）。
　一方、摩擦を考慮して制御対象の駆動を制御する場合に、制御対象がクーロン摩擦の影響と粘性摩擦の影響を受けることから、両者を同定して補償することも行われている（特許文献２）。
　なお、移動物体の速度に応じて、クーロン摩擦の影響を受ける領域と粘性摩擦の影響を受ける領域とに区分し、それぞれの領域における摩擦力を不連続摩擦モデルのモデル式に基づいて推定することも行われている（非特許文献１）。
特開平７－２８５２７号公報 特開平７－３３３０８４号公報
菊植亮，武居直行，佐野明人，望山洋，藤本英雄："有限時間で零速度に収束する不連続摩擦モデル"，第１０回ロボティクスシンポジア予稿集，ｐｐ．２１５－２２０，２００５，
　しかし、不連続摩擦モデルや不感帯を導入し、関節に作用するクーロン摩擦や粘性摩擦により生じる摩擦力トルクを考慮してロボット装置の関節の駆動を制御する場合、クーロン摩擦の影響を受ける領域から粘性摩擦の影響を受ける領域に移行する際や、粘性摩擦の影響を受ける領域からクーロン摩擦の影響を受ける領域に移行する際に、境界となる不連続点で、推定された摩擦力トルクと実際に関節に作用する摩擦力トルクとが合わず、関節の駆動が滑らかに制御されないという問題があった。
　本発明は、関節に作用する摩擦力トルクを考慮して関節の駆動を制御する従来のロボット装置に生じる前述の問題を解決し、関節の駆動が、クーロン摩擦の影響を受ける領域から粘性摩擦の影響を受ける領域に移行するときや、粘性摩擦の影響を受ける領域からクーロン摩擦の影響を受ける領域に移行するときであっても、滑らかに駆動制御されるロボット装置を提供することを課題とする。
　上記課題を解決するため、本発明のロボット装置は、駆動モータにより駆動される関節と、前記駆動モータの回転角を検出する回転角検出手段と、前記回転角検出手段の検出値より前記関節の駆動を制御する際に、前記関節に作用する関節トルクを推定する関節トルク推定部の推定値より補正を行う制御装置とを備え、前記関節トルク推定部は、慣性トルク推定部とコリオリ力トルク推定部と遠心力トルク推定部と重力トルク推定部と摩擦力トルク推定部とを有し、前記摩擦力トルク推定部は、クーロン摩擦力トルク推定部と粘性摩擦力トルク推定部と、クーロン摩擦力トルク推定部から粘性摩擦力トルク推定部への移行と粘性摩擦力トルク推定部からクーロン摩擦力トルク推定部への移行を滑らかにする移行区間演算部とを有することを特徴とする。
　また、本発明のロボット装置の制御方法は、駆動モータの回転角の検出値より関節を駆動する際に、関節に作用する関節トルクの推定値より補正を行うロボット装置の制御方法であって、関節に作用する慣性トルクとコリオリ力トルクと遠心力トルクと重力トルクと摩擦力トルクとを推定して関節の関節トルクの推定値を得るときに、クーロン摩擦力トルクから粘性摩擦力トルクへ移行する間の摩擦力トルクと粘性摩擦力トルクからクーロン摩擦力トルクへ移行する間の摩擦力トルクを、移行区間演算部で演算して推定することを特徴とする。
　本発明によれば、ロボット装置の関節の駆動が、クーロン摩擦の影響を受ける領域から粘性摩擦の影響を受ける領域に移行するときや、粘性摩擦の影響を受ける領域からクーロン摩擦の影響を受ける領域に移行するときであっても、滑らかに駆動制御される。
図１は、本発明の一実施形態によるロボット装置を示す斜視図。 図２は、ロボット装置の関節の構造を示す模式図。 図３は、ロボット装置の制御装置の制御ブロック図。 図４は、制御装置の関節トルク推定部の制御ブロック図。 図５は、関節トルク推定部の摩擦トルク推定部により推定される摩擦トルクを示すグラフ。 図６は、図１に示されたロボット装置のフローチャート。 図７は、関節トルク推定部のフローチャート。 図８は、摩擦トルク推定部のフローチャート。
　以下に本発明の一実施形態について、図１から図８を参照して説明する。ここで、図１から図８は、本発明の一実施形態を示す図である。
　まず、図１および図２により、本発明の一実施形態によるロボット装置１の概略について説明する。
　図１に示すように、ロボット装置１は、ロボット本体２と、ロボット本体２に接続されロボット本体２の駆動を制御する制御装置３とを備えている。
　ロボット本体２は、ベース１０と、ベース１０から順次設けられ、６個の関節２０～２５（第１関節２０、第２関節２１、第３関節２２、第４関節２３、第５関節２４、および第６関節２５）を介して、互いに連結された６個のアーム体３０～３５（第１アーム体３０、第２アーム体３１、第３アーム体３２、第４アーム体３３、第５アーム体３４、および第６アーム体３５）とを備えている。
　図２に示すように、第１アーム体３０に一体的に設けられて第１関節２０を構成するアーム軸４０は、その一端がベース１０に取付けられた減速機４１の出力側に接続されている。減速機４１の入力側には駆動モータ４２が接続され、駆動モータ４２にはその回転角を検出する回転角検出手段であるエンコーダ４３が設けられている。駆動モータ４２が正回転または逆回転すると、その回転は減速機４１で減速されてアーム軸４０に伝達され、第１アーム体３０が正方向または逆方向に回転する。
　なお、第２関節２１～第６関節２５についても、それぞれ第１関節２０と同様にアーム軸４０を有し、減速機４１、駆動モータ４２、エンコーダ４３が設けられ、同様に動作するので、説明を省略する。
　次に、図３および図４により、制御装置３について説明する。
　図３に示すように、制御装置３は、位置指令部５０と、θ・θ’・θ”演算部５１と、サーボ制御演算部５２と、関節トルク推定部５３と、外力演算部５４と、コンプライアンスモデル演算部５５と、制御対象部５６とを備えている。
　制御装置３は、ロボット本体２の各関節２０～２５のそれぞれの駆動モータ４２に設けられたエンコーダ４３の検出値に基づき、所定の制御周期にて位置指令部５０が目標位置・姿勢に関する指令を発し、指令を受けた制御対象部５６がそれぞれの駆動モータ４２を作動させ、各関節２０～２５を駆動する。
　θ・θ’・θ”演算部５１は、位置指令部５０が算出した目標位置・姿勢に関する指令と、後述するコンプライアンスモデル演算部５５で得た修正量とを加算して、逆運動学に基づく演算を行い、関節角度指令θ、関節角速度指令θ’、関節角加速度指令θ”を算出する。
　サーボ制御演算部５２は、θ・θ’・θ”演算部５１で算出された関節角度指令θ、関節角速度指令θ’、関節角加速度指令θ”を基に、制御対象部５６に発するトルク指令を算出する。
　関節トルク推定部５３は、動力学モデルに基づき、各関節２０～２５に作用する関節トルクを推定するが、その詳細については後述する。
　外力演算部５４は、サーボ制御演算部５２で算出されたトルク指令から関節トルク推定部５３で推定された関節トルクの推定トルク値を減算することにより求められた誤差トルクを基に、推定外力を算出する。
　コンプライアンスモデル演算部５５は、外力演算部５４で算出された推定外力を基に、目標位置・姿勢に関する修正量を算出する。
　ここで、関節トルク推定部５３による、各関節２０～２５に作用する関節トルクの推定について、詳細を説明する。
　関節トルク推定部５３は、動力学モデルのモデル式に基づき、関節トルクを推定する。
　ｎ個の関節に作用する推定関節トルクＴ（Θ）は、以下の式で表される。
　Ｔ（Θ）＝Ｍ（Θ）Θ”＋Ｂ（Θ）［Θａ’Θｂ’］＋Ｃ（Θ）［Θ’２］＋Ｇ（Θ）＋Ｆ（Θ’）
　上式の各項は以下の通りである。
　Ｔ（Θ）：推定関節トルク行列（ｎ行１列の行列）
　Θ、Θ’、Θ”：それぞれ関節角度行列、関節角速度行列、関節角加速度行列（ｎ行１列の行列）
　Θ’２：同じ関節の関節角速度積行列（ｎ行１列の行列）
　Θａ’Θｂ’：異なる関節の関節角速度積行列（｛ｎ・（ｎ－１）／２｝行１列の行列）
　Ｍ（Θ）：慣性（質量）行列（ｎ行ｎ列の行列）
　Ｂ（Θ）：コリオリ力行列（ｎ行｛ｎ・（ｎ－１）／２｝列の行列）
　Ｃ（Θ）：遠心力行列（ｎ行ｎ列の行列）
　Ｇ（Θ）：重力行列（ｎ行１列の行列）
　Ｆ（Θ’）：摩擦力行列（ｎ行１列の行列）
　なお、本実施形態では、ロボット本体２が６個の関節２０～２５を有するので、ｎ＝６となる。
　図４に示すように、関節トルク推定部５３は、上述の推定関節トルクＴ（Θ）の式に基づき、慣性トルク推定部６０とコリオリ力トルク推定部６１と遠心力トルク推定部６２と重力トルク推定部６３と摩擦力トルク推定部６４を備えている。
　慣性トルク推定部６０は、上述の推定関節トルクＴ（Θ）の式のＭ（Θ）Θ”の項に基づき、所定の制御周期で発せられる関節角度指令θと関節角加速度指令θ”に対して、慣性により生ずる慣性トルクを算出する。
　コリオリ力トルク推定部６１は、上述の推定関節トルクＴ（Θ）の式のＢ（Θ）［Θａ’Θｂ’］の項に基づき、所定の制御周期で発せられる関節角度指令θと関節角速度指令θ’に対して、コリオリ力により生ずるコリオリ力トルクを算出する。
　遠心力トルク推定部６２は、上述の推定関節トルクＴ（Θ）の式のＣ（Θ）［Θ’２］の項に基づき、所定の制御周期で発せられる関節角度指令θと関節角速度指令θ’に対して、遠心力により生ずる遠心力トルクを算出する。
　重力トルク推定部６３は、上述の推定関節トルクＴ（Θ）の式のＧ（Θ）の項に基づき、所定の制御周期で発せられる関節角度指令θに対して、重力により生ずる重力トルクを算出する。
　摩擦力トルク推定部６４は、上述の推定関節トルクＴ（Θ）の式のＦ（Θ’）の項に基づき、所定の制御周期で発せられる関節角度指令θと関節角速度指令θ’に対して、摩擦力により生ずる摩擦力トルクを算出する。
　関節トルク推定部５３は、慣性トルク推定部６０で算出された慣性トルクと、コリオリ力トルク推定部６１で算出されたコリオリ力トルクと、遠心力トルク推定部６２で算出された遠心力トルクと、重力トルク推定部６３で算出された重力トルクと、摩擦力トルク推定部６４で算出された摩擦力トルクとを加算することにより関節トルクを推定し、関節トルクの推定値を得る。
　各関節２０～２５に作用する摩擦力トルクを算出するに当たり、関節角速度の増加に応じて各関節２０～２５に作用する摩擦がクーロン摩擦から粘性摩擦に移行すること、および、関節角速度の減少に応じて各関節２０～２５に作用する摩擦が粘性摩擦からクーロン摩擦に移行することから、摩擦力トルク推定部６４は、クーロン摩擦モデルのモデル式に基づくクーロン摩擦力トルク推定部７０と、粘性摩擦モデルのモデル式に基づく粘性摩擦力トルク推定部７１とを備えている。
　そして、クーロン摩擦モデルのモデル式と粘性摩擦モデルのモデル式が不連続である場合に、その接続が不連続となる関節角速度の前後で、推定される摩擦力トルクが急激に変化して各関節２０～２５の駆動が乱れることを避けるために、摩擦力トルク推定部６４は、さらに、クーロン摩擦力トルク推定部７０から粘性摩擦力トルク推定部７１への移行と、粘性摩擦力トルク推定部７１からクーロン摩擦力トルク推定部７０への移行を滑らかにする移行区間演算部７２を備えている。
　摩擦力トルク推定部６４は、クーロン摩擦力トルク推定部７０と粘性摩擦力トルク推定部７１と移行区間演算部７２との中から、関節角速度に応じて、いずれか１つを選択する切替えスイッチ７３も有している。
　次に、クーロン摩擦力トルク推定部７０と粘性摩擦力トルク推定部７１と移行区間演算部７２とにおける摩擦力トルクの推定と、切替えスイッチ７３による切替えについて説明する。
　クーロン摩擦力トルク推定部７０は、クーロン摩擦モデルに準じ、各関節２０～２５の関節角度指令θと関節角速度指令θ’に対して、各関節２０～２５に作用するクーロン摩擦力トルクＦｃ（θ）を、Ｆｃ（θ）＝ｆｃ×ｓｇｎ（θ’）の式に従って演算して推定する。ただし、ｆｃはクーロン摩擦モデルのモデル式に準じた所定の定数を表す。
　粘性摩擦力トルク推定部７１は、粘性摩擦モデルに準じ、各関節２０～２５の関節角度指令θと関節角速度指令θ’に対して、各関節２０～２５に作用する粘性摩擦力トルクＦｖ（θ）を、Ｆｖ（θ）＝ｆｖ（θ’）の式に従って演算して推定する。ただし、ｆｖ（θ’）は、関節角速度指令θ’を変数とする粘性摩擦モデルのモデル式に準じた所定の関数を表す。
　移行区間演算部７２は、各関節２０～２５の関節角度指令θと関節角速度指令θ’に対して、各関節２０～２５に作用する移行区間摩擦力トルクＦｔ（θ）を、Ｆｔ（θ）＝ｋ×（θ－θｓ）＋ｆｃ×ｓｇｎ（θ’）の式に従って演算して推定する。ただし、ｋは後述する摩擦力トルク係数を表し、θｓは所定の制御周期における前回指令時の関節角度指令を表す。
　摩擦力トルク係数ｋは、予め各関節２０～２５を駆動し、各関節２０～２５に影響を及ぼす摩擦が、関節角度の増加によりクーロン摩擦から粘性摩擦に移行する際の各関節２０～２５に作用する関節トルクを測定して、その測定結果を解析することにより定められる。各関節２０～２５の駆動モータ４２の電流値とエンコーダ４３の検出値とを所定のサンプリング周期で複数測定し、駆動モータ４２の電流値を関節トルクに、エンコーダ４３の検出値を関節角度に換算する。ロボット本体２の寸法や質量等に基づき、測定時に各関節２０～２５に作用していた慣性トルクとコリオリ力トルクと遠心力トルクと重力トルクを算出し、換算された関節トルクからそれらを減算することにより、測定時に各関節２０～２５に作用していた摩擦力トルクを抽出する。換算された関節角度と抽出された摩擦力トルクにより、測定に基づく関節角度と摩擦力トルクとの関係Ｆｍ（θ）を得る。
　大略、測定に基づく関節角度と摩擦力トルクとの関係Ｆｍ（θ）は、関節角度が微小な領域では、クーロン摩擦の影響により関節角度の変化に対して摩擦力トルクはほぼ一定となるが、この微小な領域を超えると、関節角度の増加に従い摩擦力トルクが滑らかに増加する。この摩擦力トルクが滑らかに増加する領域において、関節角度に対する摩擦力トルクの傾きｋｍを抽出し、その傾きｋｍを摩擦力トルク係数ｋとする。
　なお、本実施形態では、駆動モータ４２の電流値を関節トルクに、エンコーダ４３の検出値を関節角度に換算しているが、他の測定装置を用いて関節トルクと関節角度を測定してもよい。
　また、ロボット装置１が複数台ある場合は、それぞれのロボット装置１について摩擦力トルク係数ｋを求めることが好ましいが、同一工程で製作された同一仕様の複数台のロボット装置１については、いずれか１台のロボット装置１の摩擦力トルク係数ｋを求め、その摩擦力トルク係数ｋを残りのロボット装置１に適用してもよい。
　切替えスイッチ７３は、各関節２０～２５の関節角速度指令θ’に応じて、クーロン摩擦力トルク推定部７０と粘性摩擦力トルク推定部７１と移行区間演算部７２との中から、いずれか１つを選択する。
　各関節２０～２５の回転方向が正回転の場合、関節角速度指令θ’は正の値となり、逆回転の場合、関節角速度指令θ’は負の値となる。
　関節角速度指令θ’が、０≦｜θ’｜≦θ１’（０＜θ１’）の範囲にあるとき、切替えスイッチ７３はクーロン摩擦力トルク推定部７０を選択して接続し、摩擦力トルク推定部６４は、クーロン摩擦力トルク推定部７０にて演算することにより、各関節２０～２５に作用する摩擦力トルクを推定する。なお、θ１’は、クーロン摩擦切替え関節速度指令θ１’を表し、これについては後述する。
　関節角速度指令θ’が、θ１’＜｜θ’｜＜θ２’（θ１’＜θ２’）の範囲にあるとき、切替えスイッチ７３は移行区間演算部７２を選択して接続し、摩擦力トルク推定部６４は、移行区間演算部７２にて演算することにより、各関節２０～２５に作用する摩擦力トルクを推定する。なお、θ２’は、粘性摩擦切替え関節速度指令θ２’を表し、これについては後述する。
　関節角速度指令θ’が、θ２’≦｜θ’｜の範囲にあるとき、切替えスイッチ７３は粘性摩擦力トルク推定部７１を選択して接続し、摩擦力トルク推定部６４は、粘性摩擦力トルク推定部７１にて演算することにより、各関節２０～２５に作用する摩擦力トルクを推定する。
　なお、クーロン摩擦力トルク推定部７０から粘性摩擦力トルク推定部７１への直接の切替えと、粘性摩擦力トルク推定部７１からクーロン摩擦力トルク推定部７０への直接の切替えは行われない。
　クーロン摩擦切替え関節速度指令θ１’は、クーロン摩擦力トルク推定部７０がクーロン摩擦モデルに準じて算出したクーロン摩擦力トルクＦｃ（θ）と、測定に基づく関節角度と摩擦力トルクとの関係Ｆｍ（θ）とを比較し、Ｆｍ（θ）がＦｃ（θ）を超えるときの関節角速度が適用される。
　粘性摩擦切替え関節速度指令θ２’は、粘性摩擦力トルク推定部７１が粘性摩擦モデルに準じて算出した粘性摩擦力トルクＦｖ（θ）と、測定に基づく関節角度と摩擦力トルクとの関係Ｆｍ（θ）とを比較し、Ｆｍ（θ）がＦｖ（θ）の最小値に到達するときの関節角速度が適用される。
　本実施形態において、切替えスイッチ７３によるクーロン摩擦力トルク推定部７０と移行区間演算部７２との間の切替え、粘性摩擦力トルク推定部７１と移行区間演算部７２の間の切替えは、関節角速度指令θ’とクーロン摩擦切替え関節速度指令θ１’、粘性摩擦切替え関節速度指令θ２’とを比較することにより行われるが、切替えのための条件を、適宜、変更や付加してもよい。
　例えば、本実施の形態において、移行区間演算部７２から粘性摩擦力トルク推定部７１への切替えは、関節角速度指令θ’が粘性摩擦切替え関節速度指令θ２’以上になった（θ２’≦｜θ’｜）時点で切替わるとしているが、関節角速度指令θ’が粘性摩擦切替え関節速度指令θ２’よりも速くなり（θ２’＜｜θ’｜）、かつ、移行区間摩擦力トルクＦｔ（θ）が粘性摩擦力トルクＦｖ（θ）の最小値よりも大きくなった（｜Ｆｖ（θ）の最小値｜＜｜Ｆｔ（θ）｜）時点で切替わるとしてもよい。
　本実施形態では、上述の関節トルクの測定、測定結果の解析、測定に基づく関節角度と摩擦トルクとの関係Ｆｍ（θ）の取得、関節角度に対する摩擦力トルクの傾きｋｍの取得、および、クーロン摩擦切替え関節速度指令θ１’、粘性摩擦切替え関節速度指令θ２’の取得は、オペレータによりオフラインにて実施されるが、制御装置３が実行するようにプログラムソフトを追加してもよい。
　図５に、各関節２０～２５に対し、本実施形態の摩擦力トルク推定部６４により推定される摩擦力トルクのグラフを示す。
　横軸は関節角速度を、縦軸は摩擦力トルクを示し、白丸（○）印は摩擦力トルク推定部６４により推定された摩擦力トルクを示す。
　区間Ａと区間Ａ’は、関節角速度指令θ’が０≦｜θ’｜≦θ１’の範囲であって、所定の制御周期にて、摩擦力トルク推定部６４が、クーロン摩擦力トルク推定部７０で演算して、摩擦力トルクを推定したものである。
　区間Ｂと区間Ｂ’は、関節角速度指令θ’がθ１’＜｜θ’｜＜θ２’の範囲であって、所定の制御周期にて、摩擦力トルク推定部６４が、移行区間演算部７２で演算して、摩擦力トルクを推定したものである。
　区間Ｃと区間Ｃ’は、関節角速度指令θ’がθ２’≦｜θ’｜の範囲であって、所定の制御周期にて、摩擦力トルク推定部６４が、粘性摩擦力トルク推定部７１で演算して、摩擦力トルクを推定したものである。
　移行区間演算部７２で推定された区間Ｂ、Ｂ’の摩擦トルクは、クーロン摩擦モデルのモデル式に基づき推定された区間Ａ、Ａ’の摩擦力トルクと、粘性摩擦モデルのモデル式に基づき推定された区間Ｃ、Ｃ’の摩擦力トルクを滑らかに接続している。
　区間Ｂ、Ｂ’の黒丸（●）印で示すように、従来通り、この区間の摩擦力トルクをクーロン摩擦力トルク推定部７０により推定すると、区間Ｂと区間Ｃの境界、および、区間Ｂ’と区間Ｃ’の境界において、摩擦力トルクが急激に変化するため、各関節２０～２５の駆動が乱れる可能性がある。
　本実施形態によれば、摩擦力トルク推定部６４が移行区間演算部７２を備えているので、クーロン摩擦力トルク推定部７０から移行区間演算部７２を介して粘性摩擦力トルク推定部７１へ、および、粘性摩擦力トルク推定部７１から移行区間演算部７２を介してクーロン摩擦力トルク推定部７０へ、滑らかに移行する。
　次に、本実施形態によるロボット装置１の動作を、図６に示すフローチャートを参照して説明する。
　ロボット装置１は、図６に示すフローチャートの如く制御される。
　ロボット装置１は、ステップＳ１００においてサーボオンされ、制御装置３が起動する。
　ステップＳ１０１において、制御装置３は、そのメモリに保存されている動力学モデルに基づく推定関節トルクＴ（Θ）の式のパラメータを読み込む。読み込まれるパラメータは、関節トルク推定部５３の慣性トルク推定部６０、コリオリ力トルク推定部６１、遠心力トルク推定部６２、重力トルク推定部６３に適用され、ロボット本体２の寸法や質量に基づき予め求められるアーム質量、リンクオフセット量、重心オフセット、重心周り慣性モーメント等と、摩擦力トルク推定部６４に含まれるクーロン摩擦力トルク推定部７０のクーロン摩擦力トルクＦｃ（θ）の式に関する情報、粘性摩擦力トルク推定部７１の粘性摩擦力トルクＦｖ（θ）の式に関する情報、移行区間演算部７２の移行区間摩擦力トルクＦｔ（θ）の式の摩擦力トルク係数ｋ、切替えスイッチ７３のクーロン摩擦切替え関節速度指令θ１’、粘性摩擦切替え関節速度指令θ２’等である。
　ステップＳ１０２において、制御装置３は、そのメモリに保存されているロボット本体２の先端の到達すべき目標位置、姿勢を読み込む。
　ステップＳ１０３において、制御装置３は、その位置指令部５０にて、各関節２０～２５のそれぞれの駆動モータ４２に設けられたエンコーダ４３の検出値に基づき、所定の制御周期にて、目標位置・姿勢を算出し、目標位置・姿勢に関する指令を発する。
　ステップＳ１０４において、制御装置３は、目標位置・姿勢に関する指令を基に、そのθ・θ’・θ”演算部５１にて、逆運動学に基づく演算を行い、関節角度指令θ、関節角速度指令θ’、関節角加速度指令θ”を算出する。
　ステップＳ１０５において、制御装置３は、関節角度指令θ、関節角速度指令θ’、関節角加速度指令θ”を基に、そのサーボ制御演算部５２により、トルク指令値を算出する。
　ステップＳ１０６において、制御装置３は、トルク指令を基に、制御対象部５６がそれぞれの駆動モータ４２を作動させ、ロボット本体２の各関節２０～２５を回転させる。
　ステップＳ１０７において、制御装置３は、ロボット本体２の各関節２０～２５の位置・姿勢が到達すべき目標値になっていればステップＳ１０８に進みサーボオフする。目標値に達していなければ、ステップＳ１０３に戻る。
　なお、ステップＳ１０９において、制御装置３は、ステップＳ１０４にて算出された関節角度指令θ、関節角速度指令θ’、関節角加速度指令θ”を基に、その関節トルク推定部５３にて、各関節２０～２５のそれぞれに作用する関節トルクを推定し、ステップＳ１１０において、その推定された関節トルクの推定トルク値からステップＳ１０５にて算出されたトルク指令を減算して、誤差トルクを算出する。
　ステップＳ１１１において、制御装置３は、誤差トルクを基に、その外力演算部５４にて、推定外力を算出し、ステップＳ１１２において、推定外力を基に、そのコンプライアンスモデル演算部５５にて、目標位置・姿勢の修正量を演算する。
　この目標位置・姿勢の修正量は、ステップＳ１０３において演算される目標位置・姿勢に加算され、算出された目標位置・姿勢はコンプライアンス制御を考慮した目標位置・姿勢となる。
　次に、関節トルク推定部５３の動作を、図７に示すフローチャートを参照して説明する。
　関節トルク推定部５３は、ステップＳ２００において関節トルクの推定を開始する。
　ステップＳ２０１において、関節トルク推定部５３は、慣性トルク推定部６０により慣性トルクを算出する。
　ステップＳ２０２において、関節トルク推定部５３は、コリオリ力トルク推定部６１によりコリオリ力トルクを算出する。
　ステップＳ２０３において、関節トルク推定部５３は、遠心力トルク推定部６２により遠心力トルクを算出する。
　ステップＳ２０４において、関節トルク推定部５３は、重力トルク推定部６３により重力トルクを算出する。
　ステップＳ２０５において、関節トルク推定部５３は、摩擦力トルク推定部６４により摩擦力トルクを算出する。
　ステップＳ２０６において、関節トルク推定部５３は、ステップＳ２０１～ステップＳ２０５で算出された慣性トルクとコリオリ力トルクと遠心力トルクと重力トルクと摩擦力トルクを加算して、関節トルクを推定する。
　そして、ステップＳ２０７において、関節トルク推定部５３は、関節トルクの推定を終了する。
　次に、摩擦力トルク推定部６４の動作を、図８に示すフローチャートを参照して説明する。
　摩擦力トルク推定部６４は、ステップＳ３００において摩擦力トルクの推定を開始する。
　ステップＳ３０１において、摩擦力トルク推定部６４は、切替えスイッチ７３により、クーロン摩擦力トルク推定区間にあるか、ないか、すなわち関節角速度指令θ’が０≦｜θ’｜≦θ１’の範囲にあるか、ないかを判定する。関節角速度指令θ’が０≦｜θ’｜≦θ１’の範囲にある場合はステップＳ３０５に進み、ない場合はステップＳ３０２に進む。
　ステップＳ３０５において、摩擦力トルク推定部６４は、クーロン摩擦力トルク推定部７０にてクーロン摩擦力トルクＦｃ（θ）を演算することにより、摩擦力トルクを推定する。
　ステップＳ３０２において、摩擦力トルク推定部６４は、切替えスイッチ７３により、移行区間にあるか、ないか、すなわち関節角速度指令θ’がθ１’＜｜θ’｜＜θ２’の範囲にあるか、ないかを判定する。関節角速度指令θ’がθ１’＜｜θ’｜＜θ２’の範囲にある場合はステップＳ３０６に進み、ない場合はステップＳ３０３に進む。
　ステップＳ３０６において、摩擦力トルク推定部６４は、移行区間演算部７２にて移行区間摩擦力トルクＦｔ（θ）を演算することにより、摩擦力トルクを推定する。
　ステップＳ３０３において、摩擦力トルク推定部６４は、切替えスイッチ７３により、粘性摩擦力トルク推定区間にあるか、ないか、すなわち関節角速度指令θ’がθ２’≦｜θ’｜の範囲にあるか、ないかを判定する。関節角速度指令θ’がθ２’≦｜θ’｜の範囲にある場合はステップＳ３０７に進み、ない場合は摩擦力トルクが推定されないものとして、ステップＳ３０４に進む。
　ステップＳ３０７において、摩擦力トルク推定部６４は、粘性摩擦力トルク推定部７１にて粘性摩擦力トルクＦｖ（θ）を演算することにより、摩擦力トルクを推定する。
　ステップＳ３０４において、摩擦力トルク推定部６４は、摩擦力トルクの推定を終了する。
　なお、本発明を実施するに当たっては、ロボット装置の具体的な構成により、適宜変更して実施できる。実行する作業に応じて、ロボット本体の関節およびアーム体の個数、および、それを制御する制御装置の制御点数が決定される。

Claims (4)

  1. 　ロボット装置において、
    　駆動モータにより駆動される関節と、
    　前記駆動モータの回転角を検出する回転角検出手段と、
    　前記回転角検出手段の検出値より前記関節の駆動を制御する際に、前記関節に作用する関節トルクを推定する関節トルク推定部の推定値より補正を行う制御装置とを備え、
    　前記関節トルク推定部は、慣性トルク推定部と、コリオリ力トルク推定部と、遠心力トルク推定部と、重力トルク推定部と、摩擦力トルク推定部とを有し、
    　前記摩擦力トルク推定部は、クーロン摩擦力トルク推定部と、粘性摩擦力トルク推定部と、前記クーロン摩擦力トルク推定部から前記粘性摩擦力トルク推定部への移行と前記粘性摩擦力トルク推定部から前記クーロン摩擦力トルク推定部への移行を滑らかにする移行区間演算部とを有することを特徴とするロボット装置。
  2. 　前記移行区間演算部は、予め測定された前記関節の関節トルクより抽出された摩擦力トルク係数を用いて、前記関節の摩擦力トルクを演算することを特徴とする請求項１に記載のロボット装置。
  3. 　駆動モータの回転角の検出値より関節を駆動する際に、前記関節に作用する関節トルクの推定値より補正を行うロボット装置の制御方法において、
    　前記関節に作用する慣性トルクとコリオリ力トルクと遠心力トルクと重力トルクと摩擦力トルクとを推定して前記関節の関節トルクの推定値を得る工程を備え、
    　前記関節の関節トルクの推定値を得る工程は、クーロン摩擦力トルクから粘性摩擦力トルクへ移行する間の摩擦力トルクと、粘性摩擦力トルクからクーロン摩擦力トルクへ移行する間の摩擦力トルクとを、移行区間演算部で演算して推定する工程を含むことを特徴とするロボット装置の制御方法。
  4. 　前記移行区間演算部は、予め測定された前記関節の関節トルクより抽出された摩擦力トルク係数を用いて、前記関節の摩擦力トルクを演算することを特徴とする請求項３に記載のロボット装置の制御方法。
PCT/JP2013/070918 2012-08-02 2013-08-01 ロボット装置およびその制御方法 WO2014021433A1 (ja)

Priority Applications (4)

Application Number Priority Date Filing Date Title
GB1501379.0A GB2518576B (en) 2012-08-02 2013-08-01 Robotic apparatus and control method therefor
CN201380041006.7A CN104507645B (zh) 2012-08-02 2013-08-01 机器人装置及其控制方法
JP2014528226A JP5727103B2 (ja) 2012-08-02 2013-08-01 ロボット装置およびその制御方法
US14/418,416 US9434073B2 (en) 2012-08-02 2013-08-01 Robot apparatus and control method therefor

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2012172007 2012-08-02
JP2012-172007 2012-08-02

Publications (1)

Publication Number Publication Date
WO2014021433A1 true WO2014021433A1 (ja) 2014-02-06

Family

ID=50028100

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2013/070918 WO2014021433A1 (ja) 2012-08-02 2013-08-01 ロボット装置およびその制御方法

Country Status (5)

Country Link
US (1) US9434073B2 (ja)
JP (1) JP5727103B2 (ja)
CN (1) CN104507645B (ja)
GB (1) GB2518576B (ja)
WO (1) WO2014021433A1 (ja)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109129525A (zh) * 2017-06-15 2019-01-04 电装波动株式会社 机器人的负载重心位置推定装置及负载重心位置推定方法
JP6906711B1 (ja) * 2020-03-26 2021-07-21 三菱電機株式会社 摩擦補償装置、衝突検知装置、トルクフィードフォワード演算装置およびロボット制御装置並びに摩擦補償方法
US11161242B2 (en) 2016-12-16 2021-11-02 Panasonic Intellectual Property Management Co., Ltd. Method for controlling robot

Families Citing this family (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2016196512A1 (en) * 2015-05-29 2016-12-08 Abb Technology Ag Method and system for robotic adaptive production
JP6924145B2 (ja) * 2015-10-14 2021-08-25 川崎重工業株式会社 ロボット教示方法及びロボットアーム制御装置
JP6956081B2 (ja) * 2015-11-11 2021-10-27 マコ サージカル コーポレーション ロボットシステム及びロボットシステムをバックドライブする方法
DE102016000187B3 (de) * 2016-01-11 2017-01-26 Kuka Roboter Gmbh Bestimmung einer Orientierung eines Roboters relativ zu einer Gravitationsrichtung
WO2019012942A1 (ja) * 2017-07-11 2019-01-17 パナソニックＩｐマネジメント株式会社 ロボット制御装置
JP6844462B2 (ja) * 2017-07-21 2021-03-17 株式会社デンソーウェーブ 角度検出器の偏心誤差補正方法、ロボットシステム
JP6661676B2 (ja) * 2018-01-18 2020-03-11 ファナック株式会社 ロボット制御装置
JP7124440B2 (ja) * 2018-05-23 2022-08-24 セイコーエプソン株式会社 ロボット制御装置およびロボットシステム
JP6989542B2 (ja) * 2019-01-31 2022-01-05 ファナック株式会社 ロボット制御装置
CN113128018B (zh) * 2019-12-31 2023-04-07 深圳市优必选科技股份有限公司 摩擦力计算方法、装置、机器人及可读存储介质
JP7283421B2 (ja) * 2020-03-05 2023-05-30 トヨタ自動車株式会社 トルク推定システム、トルク推定方法、及びプログラム
US11691285B2 (en) * 2020-05-07 2023-07-04 Mujin, Inc. Method and computing system for estimating parameter for robot operation
CN114102603B (zh) * 2021-12-13 2023-12-26 佗道医疗科技有限公司 一种基于笛卡尔空间的零力拖动方法
CN114102636B (zh) * 2021-12-31 2024-03-19 华中科技大学 遥操作机器人的焊缝打磨控制系统及其设计方法和应用
CN116460859B (zh) * 2023-06-19 2023-10-03 广东隆崎机器人有限公司 Scara机器人运动补偿方法、装置、设备及存储介质

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2006146572A (ja) * 2004-11-19 2006-06-08 Yaskawa Electric Corp サーボ制御装置および方法
JP2010076012A (ja) * 2008-09-24 2010-04-08 Toshiba Corp マニピュレータシステムおよびその制御方法

Family Cites Families (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4641251A (en) * 1982-02-16 1987-02-03 Inoue-Japax Research Incorporated Robot
CA1233222A (en) * 1984-03-09 1988-02-23 Nobuhiko Onda Movable apparatus driving system
US5101472A (en) * 1990-10-04 1992-03-31 Repperger Daniel W Military robotic controller with majorizing function and nonlinear torque capability
JPH0728527A (ja) 1993-07-12 1995-01-31 Fanuc Ltd クーロン摩擦の補正処理方法
JP3463355B2 (ja) 1994-06-03 2003-11-05 株式会社安川電機 制御対象の特性を表す関数の定数の同定・補償方法
SE9700767D0 (sv) * 1997-03-04 1997-03-04 Asea Brown Boveri Förfarande för bestämning av lastparametrar hos en industrirobot
JP3981773B2 (ja) * 1997-05-28 2007-09-26 株式会社安川電機 ロボット制御装置
US6385508B1 (en) * 2000-10-31 2002-05-07 Fanuc Robotics North America, Inc. Lead-through teach handle assembly and method of teaching a robot assembly
SE0301531L (sv) * 2003-05-22 2004-11-23 Abb Ab A Control method for a robot
EP1915963A1 (en) * 2006-10-25 2008-04-30 The European Atomic Energy Community (EURATOM), represented by the European Commission Force estimation for a minimally invasive robotic surgery system
EP1932629B1 (en) * 2006-12-11 2019-04-24 ABB Research Ltd. A method and a control system for monitoring the condition of an industrial robot
US7986118B2 (en) * 2007-04-23 2011-07-26 Honda Motor Co., Ltd. Open-loop torque control on joint position-controlled robots
JP2009066685A (ja) * 2007-09-11 2009-04-02 Sony Corp ロボット装置及びロボット装置の制御方法
US7912612B2 (en) * 2007-11-30 2011-03-22 Caterpillar Inc. Payload system that compensates for rotational forces
JP5242342B2 (ja) * 2008-10-31 2013-07-24 株式会社東芝 ロボット制御装置
JP5238885B2 (ja) * 2009-09-24 2013-07-17 株式会社東芝 ロボット制御装置
JP5198514B2 (ja) * 2010-07-22 2013-05-15 株式会社東芝 ロボット制御装置
KR20120060578A (ko) * 2010-12-02 2012-06-12 삼성전자주식회사 보행 로봇 및 그 자세 제어 방법
KR101778027B1 (ko) * 2010-12-21 2017-09-13 삼성전자주식회사 보행 로봇 및 그 자세 제어 방법
KR101953113B1 (ko) * 2011-05-30 2019-03-05 삼성전자주식회사 로봇 및 그 제어방법

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2006146572A (ja) * 2004-11-19 2006-06-08 Yaskawa Electric Corp サーボ制御装置および方法
JP2010076012A (ja) * 2008-09-24 2010-04-08 Toshiba Corp マニピュレータシステムおよびその制御方法

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11161242B2 (en) 2016-12-16 2021-11-02 Panasonic Intellectual Property Management Co., Ltd. Method for controlling robot
CN109129525A (zh) * 2017-06-15 2019-01-04 电装波动株式会社 机器人的负载重心位置推定装置及负载重心位置推定方法
JP6906711B1 (ja) * 2020-03-26 2021-07-21 三菱電機株式会社 摩擦補償装置、衝突検知装置、トルクフィードフォワード演算装置およびロボット制御装置並びに摩擦補償方法
WO2021192181A1 (ja) * 2020-03-26 2021-09-30 三菱電機株式会社 摩擦補償装置、衝突検知装置、トルクフィードフォワード演算装置およびロボット制御装置並びに摩擦補償方法
CN115336167A (zh) * 2020-03-26 2022-11-11 三菱电机株式会社 摩擦补偿装置、碰撞检测装置、扭矩前馈运算装置及机器人控制装置以及摩擦补偿方法
US11691282B2 (en) 2020-03-26 2023-07-04 Mitsubishi Electric Corporation Friction compensation device, and robot control device
CN115336167B (zh) * 2020-03-26 2023-09-22 三菱电机株式会社 摩擦补偿装置、碰撞检测装置、扭矩前馈运算装置及机器人控制装置以及摩擦补偿方法

Also Published As

Publication number Publication date
US20150258685A1 (en) 2015-09-17
JP5727103B2 (ja) 2015-06-03
US9434073B2 (en) 2016-09-06
CN104507645B (zh) 2016-04-27
GB2518576B (en) 2015-09-02
CN104507645A (zh) 2015-04-08
GB2518576A (en) 2015-03-25
JPWO2014021433A1 (ja) 2016-07-21
GB201501379D0 (en) 2015-03-11

Similar Documents

Publication Publication Date Title
JP5727103B2 (ja) ロボット装置およびその制御方法
JP5198514B2 (ja) ロボット制御装置
JP5902425B2 (ja) ロボット制御装置、外乱判定方法およびアクチュエータ制御方法
CN111788040B (zh) 机器人的动力学参数辨识方法、机器人和存储装置
US10690558B2 (en) Robot collision detection method
JP5327722B2 (ja) ロボットの負荷推定装置及び負荷推定方法
US9533414B2 (en) Torque detecting method and arm device
JP6044511B2 (ja) ロボットの制御方法及びロボットシステム
EP3078459A1 (en) Robot controlling method, robot apparatus, program and recording medium
JP4858229B2 (ja) ロボットに取り付けられた負荷の質量と重心位置の算出方法
WO2003068464A1 (fr) Procede de commande d'entrainement et controleur d'entrainement
JPWO2011161765A1 (ja) ロボット制御装置
Madsen et al. Adaptive feedforward control of a collaborative industrial robot manipulator using a novel extension of the Generalized Maxwell-Slip friction model
JP2019181610A (ja) モータエンコーダ及びセンサを用いて学習制御を行うロボットシステム
JP2018058181A (ja) 外乱オブザーバ及びロボット制御装置
JP2018158418A (ja) ロボットの負荷重心位置推定装置及びロボットの負荷重心位置推定方法
US20170261529A1 (en) Method for identifying friction parameter for linear module
CN114102606A (zh) 机器人运动信息规划方法及相关装置
JP5708091B2 (ja) ロボットの制御方法およびロボットの制御装置
Angel et al. Parametric identification of a delta type parallel robot
JP4305340B2 (ja) ロボットに取り付けられた負荷の質量と重心位置の算出方法
JP4449693B2 (ja) ロボット制御装置およびその制御方法
WO2020149020A1 (ja) ロボット制御装置、ロボット制御方法、及びロボット制御プログラム
JP2019195892A (ja) 撓み量推定装置、ロボット制御装置、及び撓み量推定方法
JP5473889B2 (ja) 力制御装置

Legal Events

Date Code Title Description
WWE Wipo information: entry into national phase

Ref document number: 201380041006.7

Country of ref document: CN

121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 13826398

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2014528226

Country of ref document: JP

Kind code of ref document: A

ENP Entry into the national phase

Ref document number: 1501379

Country of ref document: GB

Kind code of ref document: A

Free format text: PCT FILING DATE = 20130801

WWE Wipo information: entry into national phase

Ref document number: 1501379.0

Country of ref document: GB

WWE Wipo information: entry into national phase

Ref document number: 14418416

Country of ref document: US

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 13826398

Country of ref document: EP

Kind code of ref document: A1