JP2017056525A - Robot device robot control method, program, recording medium, and method of manufacturing assembling component - Google Patents

Abstract

PROBLEM TO BE SOLVED: To suppress the diverging or vibrations of a hand end of a robot, after supporting an article with an end effector.SOLUTION: A robot 200 has a robot arm 251; and a robot hand 252 which is provided at a tip end of the robot arm 251, and grips work W1. A force detecting portion provided on the robot 200 detects hand end force applied on a hand end of the robot 200. A control portion controls the action of the robot 200 by using a load value of a hand end load model of the hand end. The control portion obtains an estimated value φof the hand end load model by using the hand end force detected by the force detecting portion when load fluctuations occur at the hand end. The control portion sets the load value of the hand end load model after the load fluctuations, so as to gradually change from a load value φof the hand end load model before the load fluctuations toward the estimated value φwith time.SELECTED DRAWING: Figure 6

Description

  The present invention relates to a robot apparatus, a robot control method, a program, a recording medium, and an assembly part manufacturing method for controlling the operation of a robot using a hand load model.

  Industrial robots used for conveyance and assembly are required to improve control performance such as speed during assembly operations.

  As a method for improving such control performance, there is a model-based control method based on a hand load model composed of an end effector attached to the tip of a robot arm or an end effector and a supporting object. The hand load model (hand dynamic model) is the hand mass, the position of the center of gravity or the inertia tensor, or a combination thereof.

  The model-based control method is known as a method of reflecting a feedforward amount that compensates for dynamic nonlinear elements such as gravity, inertial force, centrifugal force, and Coriolis force due to the motion of the robot itself in the robot motion. In the force control required for assembly, the stability and speed during contact work can be improved by model-based control. That is, if the hand load model is accurate, stable operation can be realized even if the robot is driven at high speed.

  However, in model-based control based on the information on the hand load model, if there is a modeling error, the control performance is greatly affected. In addition, an accurate hand load model is required to perform work within the limit load of each joint of the robot.

  Patent Document 1 proposes a method of lifting a work by a minute amount and estimating a load model such as the work mass and the position of the center of gravity from the difference in force and moment applied to the hand before and after gripping.

Japanese Patent No. 5327722

  However, in Patent Document 1, it is possible to estimate the hand load model from the load torque generated at each joint of the robot. However, when the estimated work mass / center of gravity position is reflected in the control of the robot, the value of the hand load model is It changes greatly before and after the workpiece is supported. Since the value of the hand load model greatly changes before and after the workpiece is supported, in the model-based control method for controlling based on the hand load model, the position of the hand of the robot may diverge or vibrate. When the position of the hand of the robot diverges or vibrates, the robot or an object supported by the robot may collide with surrounding objects.

  Therefore, an object of the present invention is to prevent the position of the hand of the robot from diverging or vibrating after the end effector supports the object.

  The robot apparatus of the present invention includes an articulated robot arm, an end effector that is provided at the tip of the robot arm and supports an object, and the end effector supports the object or the end effector supports the object. A robot having a force detection unit that detects a hand force applied to the hand that is not in operation, and a control unit that controls the operation of the robot using a load value of the hand load model of the hand, and The control unit includes an estimation process for obtaining an estimated value of the hand load model using the hand force detected by the force detection unit when a load fluctuation occurs in the hand, and the hand load model after the load fluctuation A setting process for setting the load value of the hand load model before the load change so that the load value gradually changes toward the estimated value over time, Characterized in that it run.

  ADVANTAGE OF THE INVENTION According to this invention, it can suppress that the position of the hand of a robot diverges or vibrates when a hand load changes rapidly during the operation | work of a robot.

1 is a perspective view illustrating a schematic configuration of a robot apparatus according to a first embodiment. It is a block diagram which shows the robot control apparatus of the robot apparatus which concerns on 1st Embodiment. It is a control block diagram which shows the control system of the robot apparatus which concerns on 1st Embodiment. It is a flowchart at the time of carrying out force control of the robot in 1st Embodiment. It is a flowchart at the time of controlling the position of the robot in the first embodiment. (A) is a schematic diagram of the robot for demonstrating the assembly | attachment operation | work by a robot. (B) is a diagram for explaining the load value phi _a of the end load model. (C) is a figure for demonstrating load value (phi) _b of a hand load model. It is a flowchart which shows the manufacturing method of the assembly components which concern on 1st Embodiment. (A) is a graph which shows the target position and present position of the hand of the perpendicular direction with respect to time in 1st Embodiment. (B) is a graph which shows the correction coefficient k (t) with respect to time in 1st Embodiment. (C) is a graph which shows load value (phi) (t) of the hand load model with respect to time in 1st Embodiment. (A) is a graph which shows load value (phi) (t) of the hand load model with respect to time in a comparative example. (B) is a graph which shows the target position and present position of the hand of the perpendicular direction with respect to time in a comparative example. It is a flowchart which shows the manufacturing method of the assembly components which concern on 2nd Embodiment. (A) is a graph which shows the correction coefficient k (t) with respect to time in 2nd Embodiment. (B) is a graph which shows load value (phi) (t) of the hand load model with respect to time in 2nd Embodiment. (C) is a graph which shows the target position and the present position of the hand of the perpendicular direction with respect to time in 2nd Embodiment. It is a graph which shows the target position and present position of the hand of a perpendicular direction with respect to time when a model error is included. It is a flowchart which shows the manufacturing method of the assembly components which concern on 3rd Embodiment. (A) is a graph which shows the correction coefficient k (t) with respect to time in 4th Embodiment. (B) is a graph which shows load value (phi) (t) of the hand load model with respect to time in 4th Embodiment. (C) is a graph which shows the target position and the present position of the hand of the perpendicular direction with respect to time in 4th Embodiment. It is a flowchart which shows the manufacturing method of the assembly components which concern on 5th Embodiment. It is a flowchart which shows the manufacturing method of the assembly components which concern on 6th Embodiment.

  Hereinafter, embodiments for carrying out the present invention will be described in detail with reference to the drawings.

[First Embodiment]
FIG. 1 is a perspective view illustrating a schematic configuration of the robot apparatus according to the first embodiment. As shown in FIG. 1, the robot apparatus 100 includes an articulated robot 200 and a robot control apparatus 300 that controls the operation of the robot 200. The robot apparatus 100 also includes a teaching pendant 400 as a teaching apparatus that transmits teaching data to the robot control apparatus 300. The teaching pendant 400 is operated by an operator, and is used to specify the operation of the robot 200 or the robot control device 300.

  The robot 200 includes an articulated robot arm 251 and a robot hand 252 as an end effector attached to the tip of the robot arm 251. The robot arm 251 is a vertical articulated joint in the first embodiment. Hereinafter, although the case where the end effector is the robot hand 252 will be described, the present invention is not limited to this, and a tool or the like may be used. The base end of the robot arm 251 is fixed to the base B. The robot hand 252 holds (supports) objects (parts, tools, etc.).

Robot 200, i.e. the robot arm 251 includes a plurality of joints, for example, six joints (6 _axes) J 1 through J _6. The robot arm 251 includes a plurality (six) of servo motors 201 to 206 that rotationally drive the joints J _{1 to} J ₆ around the joint axes A _{1 to} A ₆ .

In the robot arm 251, a plurality of links (frames) 210 _{0 to} 210 ₆ are connected to be rotatable at the joints J _{1 to} J ₆ . Here, links 210 _{0 to} 210 ₆ are connected in series in this order from the base end side to the tip end side. If the robot arm 251 is within the movable range, the hand of the robot 200 (the tip of the robot arm 251) can be directed to the posture in any three directions at any three-dimensional position.

  The position and posture of the robot arm 251 can be expressed in a coordinate system. The coordinate system To represents a coordinate system fixed to the base end of the robot arm 251, that is, the base B, and the coordinate system Te represents a coordinate system fixed to the hand of the robot 200 (tip of the robot arm 251).

  Here, the hand of the robot 200 is the robot hand 252 in the first embodiment when the robot hand 252 does not hold (support) an object. When the robot hand 252 is gripping (supporting) an object, the robot hand 252 and the object being gripped (supported) (for example, a component or a tool) are referred to as the hand of the robot 200. In other words, regardless of whether the robot hand 252 is gripping an object or not gripping an object, the tip from the tip of the robot arm 251 is referred to as the tip.

Each servomotor 201 to 206, has an electric motor (motor) 211 to 216 for driving each joint _J 1 through J ₆ respectively, and a sensor unit 221 to 226 respectively connected to the motors 211 to 216 Yes. Each sensor unit 221-226 includes a position sensor for detecting the position (angle) of each joint _J 1 through J ₆ (angle sensor), and a torque sensor for detecting the torque of each joint _J 1 through J _6. Each servo motor 201 to 206 has a reduction gear (not shown) and is connected to a frame driven by each joint J _{1 to} J ₆ directly or via a transmission member such as a belt or a bearing (not shown). ing.

  Inside the robot arm 251, a servo control unit 230 is disposed as a drive control unit that controls driving of the electric motors 211 to 216 of the servo motors 201 to 206.

The servo control unit 230, based on the torque command value corresponding to each joint _J 1 through J ₆ input, so that the torque of each joint J1~J6 to follow the torque command value, the current to the motors 211 to 216 Output and control the driving of each of the electric motors 211 to 216. In the first embodiment, the servo control unit 230 is described as being configured by a single control device. However, the servo control unit 230 is configured by an assembly of a plurality of control devices corresponding to the electric motors 211 to 216, respectively. It may be. In the first embodiment, the servo control unit 230 is arranged inside the robot arm 251, but may be arranged inside the housing of the robot control apparatus 300.

  Next, the robot controller 300 will be described. FIG. 2 is a block diagram illustrating the robot control device of the robot device according to the first embodiment. The robot control apparatus 300 is configured by a computer and includes a CPU (Central Processing Unit) 301 as a control unit (processing unit). The robot control apparatus 300 includes a ROM (Read Only Memory) 302, a RAM (Random Access Memory) 303, and an HDD (Hard Disk Drive) 304 as storage units. The robot control apparatus 300 includes a recording disk drive 305 and various interfaces 306 to 309.

  A ROM 302, a RAM 303, an HDD 304, a recording disk drive 305, and various interfaces 306 to 309 are connected to the CPU 301 via a bus 310. The ROM 302 stores basic programs such as BIOS. A RAM 303 is a storage device that temporarily stores various types of data such as arithmetic processing results of the CPU 301.

  The HDD 304 is a storage device that stores arithmetic processing results of the CPU 301, various data acquired from the outside, and the like, and records a program 330 for causing the CPU 301 to execute arithmetic processing described later. The CPU 301 executes each process of the robot control method based on the program 330 recorded (stored) in the HDD 304.

  The recording disk drive 305 can read various data and programs recorded on the recording disk 331.

  The teaching pendant 400 is connected to the interface 306. The CPU 301 receives input of teaching data from the teaching pendant 400 via the interface 306 and the bus 310.

  The servo control unit 230 is connected to the interface 309. The CPU 301 acquires detection results from the sensor units 221 to 226 via the servo control unit 230, the interface 309, and the bus 310. Further, the CPU 301 outputs torque command value data of each joint to the servo control unit 230 via the bus 310 and the interface 309 at predetermined time intervals.

  A monitor 321 is connected to the interface 307, and various images are displayed on the monitor 321 under the control of the CPU 301. The interface 308 is configured to be connectable to an external storage device 322 that is a storage unit such as a rewritable nonvolatile memory or an external HDD.

  In the first embodiment, the case where the computer-readable recording medium is the HDD 304 and the program 330 is stored in the HDD 304 will be described, but the present invention is not limited to this. The program 330 may be recorded on any recording medium as long as it is a computer-readable recording medium. For example, as a recording medium for supplying the program 330, the ROM 302, the recording disk 331, the external storage device 322, or the like shown in FIG. 2 may be used. As a specific example, a flexible disk, a hard disk, an optical disk, a magneto-optical disk, a CD-ROM, a CD-R, a magnetic tape, a nonvolatile memory, a ROM, or the like can be used as a recording medium.

  FIG. 3 is a control block diagram illustrating a control system of the robot apparatus according to the first embodiment. The CPU 301 of the robot control device 300 functions as a force detection unit 504, a force control unit 505, and a position target value generation unit 506 by executing the program 330. The servo control unit 230 includes a plurality (six for six joints) switching control units 511 to 516, a plurality of (for six joints, six) position control units 521 to 526, and a plurality (for six joints). Therefore, it functions as six) motor control units 531 to 536.

Each of the sensor units 221 to 226 includes angle sensors (position sensors) 551 to 556 and torque sensors 541 to 546. Each angle sensor 551 to 556 is for detecting each angle (position) of the electric motors 211 to 216, or each of the joints _J 1 through J _6.

In the first embodiment, the angle sensors 551 to 556 directly detect the angles θ _{1 to} θ ₆ of the electric motors 211 to 216. The angle _q 1 to q ₆ of each joint _J 1 through J ₆ on the basis of the reduction ratio or the like of the reduction gear, not shown, can be determined from the angle theta ₁ through? _6. Thus, each angle sensor 551 to 556 would indirectly has detected an angle _q 1 to q ₆ of each joint _J 1 through J _6.

The torque sensors 541 to 546 detect the torques τ _{1 to} τ ₆ of the joints J _{1 to} J ₆ , respectively. A plurality of angle sensors 551 to 556 constitute a position detection unit 550 that detects the position P of the hand of the robot 200. In addition, a force detector 540 that detects a force (hand force) F acting on the hand of the robot 200 is configured by the plurality of torque sensors 541 to 546.

The teaching pendant 400 outputs a force target value F _ref included in the force teaching data 501 and a position target value P _ref included in the position teaching data 502 to the CPU 301 by an operation of the operator. The force target value F _ref is a target value of the hand force of the robot 200 and is set by the operator using the teaching pendant 400. The position target value _Pref is a target value of the hand position of the robot 200, and is set by the operator using the teaching pendant 400. A robot model 503 is stored in the external storage device 322.

The force detection unit 504 uses the torques τ _{1 to} τ ₆ detected by the torque sensors 541 to 546 and the angles q _{1 to} q ₆ detected by the angle sensors 551 to 556, so that the current position of the hand of the robot 200 is detected. P (t) and the hand force F of the robot 200 at that time are calculated.

The force control unit 505 inputs the robot model 503 including the hand load model, the force target value F _ref , the position target value P _ref , the current position P (t), and the force F. Then, the force control unit 505 uses these values to determine the torque command value τ _MFref1 ~τ _MFref6 for each joint _J 1 through J _6. At this time, the force deviation and hand force F and the force target value _{F ref,} to obtain a torque command value τ _MFref1 ~τ _MFref6 as positional deviation between the hand position P (t) and the target position value _{P ref} is reduced. The force control unit 505 outputs the obtained torque command values τ _{MFref1 to} τ _MFref6 to the switching control units 511 to 516.

The position target value generation unit 506 obtains angle command values (position command values) q _{ref1 to} q _ref6 of the joints J _{1 to} J ₆ by inverse kinematics calculation from the position target value P _ref of the hand, and each angle command value q _{ref1 to} qref6 are output to the position control units 521 to ₅₂₆ , respectively.

Each position control unit 521 to 526, the torque command so that the angular deviation between the angle _q 1 to q ₆ of each joint _J 1 through J ₆ and angle command value _q ref1 _{to q Ref6} of the joints _J 1 through J ₆ is reduced The values τ _{MPref1 to} τ _MPref6 are _obtained . _Reducing the angle deviation means reducing the angle deviation between the angle of each electric motor ₂₁₁ to ₂₁₆ and the angle command value _obtained by _{converting the} angle command values q _{ref1 to} q _ref6 with the reduction gear ratio of the speed reducer. Is equivalent. Each position control unit 521 to 526 outputs each torque command value τ _{MPref1 to} τ _MPref6 to each switching control unit 511 to 516.

Each switching control unit 511 to 516 performs switching control between a force control mode for performing torque-based force control and a position control mode for performing position control. Each switch control unit 511 to 516, the force control mode is output to each motor control unit 531 to 536 each torque command value τ _MFref1 ~τ _MFref6 as the torque command value τ _Mref1 ~τ _Mref6. Each switch control unit 511 to 516, the position control mode is output to each motor control unit 531 to 536 each torque command value τ _MPref1 ~τ _MPref6 as the torque command value τ _Mref1 ~τ _Mref6.

The motor control units 531 to 536 _{generate the} currents Cur _{1 to} Cur ₆ so as to realize the torque command values τ _{Mref1 to} τ _{Mref 6} based on the angles (positions) θ _{1 to} θ ₆ of the electric motors _{211 to} 216, respectively. The electric motors 211 to 216 are energized.

  Next, force control of the robot 200 will be described. FIG. 4 is a flowchart when force-controlling the robot 200 in the first embodiment.

First, the operator inputs the force target value F _ref and the position target value P _ref to the teaching pendant 400 (S1). The force target value F _ref is stored in the force teaching data 501, and the position target value P _ref is stored in the position teaching data 502. The position target value _Pref is a position where the force control operation is started.

Force control unit 505, calculates the torque command value for the motors 211 to 216 based on the robot model 503 so that the hand force F applied to the hand of the robot 200 follows the force target value _{F ref} (force) τ _MFref1 ~τ _MFref6 (S2). The robot model 503 includes a hand load model described later.

Each switch control unit 511 to 516, the output torque command value (force) τ _MFref1 ~τ _MFref6 as the torque command value τ _Mref1 ~τ _Mref6 to each motor control unit 531 to 536 (S3).

The motor control units 531 to ₅₃₆ perform energization control so as to realize the torque command values τ _{Mref1 to} τ _Mref6 based on the angles θ _{1 to} θ ₆ of the electric motors 211 to ₂₁₆ (S4).

The electric motors 211 to 216 generate the joint torques τ _{1 to} τ ₆ when energized (S5).

The angle sensors 551 to 556 detect angles q _{1 to} q ₆ of the joints J _{1 to} J ₆ (angles θ _{1 to} θ ₆ of the electric motors 211 to 216). Each torque sensor 541 to 546 detects the torque τ ₁ ~τ ₆ of each joint _J 1 ~J ₆ (S6). Angle _q 1 to q ₆ and the torque τ ₁ ~τ ₆ of each joint _J 1 through J ₆ of each joint _J 1 through J ₆ is fed back to the CPU301 of the robot controller 300.

Force detection unit 504 (CPU 301) is, the torque τ ₁ ~τ ₆ of each joint _J 1 through J ₆ based robot model 503 to angle _q 1 to q ₆ of each joint _J 1 through J _6, the current position P At (t), it is converted into a hand force F applied to the hand of the robot 200 (S7). It is also possible to use the angle theta ₁ through? ₆ of the electric motor 211 to 216 instead of the angle _q 1 to q ₆ joints _J 1 through J _6.

The CPU 301 determines whether or not the driving is finished (S8), and if not finished (S8: No), repeats steps S2 to S7. By driving the electric motors 211 to 216 according to the above flow, it is possible to control the force F applied to the hand of the robot 200 to a desired force target value F _ref . In addition, it is not limited to the order of the flowchart shown in FIG. 4, force control is possible also in another order.

  Next, position control of the robot 200 will be described. FIG. 5 is a flowchart when the position of the robot 200 is controlled in the first embodiment.

First, the operator inputs the position target value _Pref to the teaching pendant 400 (S11). The position target value P _ref is stored in the position teaching data 502.

The position target value generation unit 506 converts the position target value P _ref into angle command values q _{ref1 to} q _ref6 of the joints J _{1 to} J ₆ based on the robot model 503 (S12).

Each position control unit 521 to 526 is, for each joint _J 1 through J ₆ angle _q 1 to q ₆ angle command value _q ref1 _{to q Ref6} the motors 211-216 to follow the of the joints _J 1 through J ₆ Torque command values (angles) τ _{MPref1 to} τ _MPref6 are calculated (S13). The signal indicating the angles of the joints _J 1 through J _6, may be used angle theta ₁ through? ₆ of the electric motor 211 to 216 instead of the angle _q 1 to q _6.

Each switch control unit 511 to 516, the output torque command value (angle) τ _MPref1 ~τ _MPref6 as the torque command value τ _Mref1 ~τ _Mref6 to each motor control unit 531 to 536 (S14).

Steps S15, S16, S17, and S18 are the same as steps S4, S5, S6, and S8, and a description thereof is omitted. By driving the electric motors 211 to 216 according to the above flow, it is possible to control the hand position P of the robot 200 so as to follow the desired position target value _Pref .

  The force control in steps S1 to S8 and the position control in steps S11 to S18 are switched by switching control units 511 to 516, and either one of the controls is selected according to the work.

  Next, estimation of the hand load model will be described. The hand load model is a hand load dynamic model including a robot hand 252 that is an end effector attached to the tip of the robot arm 251 and a grasped object. The dynamic model is a target mass, a position of the center of gravity, an inertia tensor, or a combination thereof. In the first embodiment, the hand load model is the hand mass, the gravity center position, and the inertia tensor.

  In the first embodiment, the hand force F of the robot 200 after the fluctuation of the hand load is acquired, and the hand load model is estimated during the operation of the robot 200 using the information (hand force F).

  In the first embodiment, the hand load model (hand load dynamics model) fluctuates, and the workpiece (first part) having a variation is transported and the work is assembled to the jig (second part) (specifically In the following, an assembly process (fitting process) will be described as an example.

  In the assembly process, an increase in the speed of the assembly operation is required. However, in the position-based force control in which a position command value for applying a desired force to the hand is given to the servo control unit 230, the control performance is in the position control band and environmental rigidity. It is difficult to operate at high speed.

Therefore, in the first embodiment, torque command values of the joints J _{1 to} J _{6 that} cause a desired force to act on the hand of the robot 200 are calculated according to the following impedance control equation (1), and torque applied to the servo control unit 230 is calculated. Perform base force control. That is, the force of the robot 200 is controlled according to the flowchart shown in FIG.

In Equation (1), G, Λ, and ν are a gravity load torque vector, an inertia matrix in Cartesian space, and a Coriolis force / centrifugal force matrix in Cartesian space, respectively. These G, Λ, and ν are calculated from the dynamic model of the robot arm 251 and the hand load, and the positions, velocities, and accelerations of the joints J _{1 to} J ₆ . Further, Λ _d , K _d , and D _d represent values of mass, rigidity, and viscosity of desired mechanical impedance, respectively. J is a 6 × 6 Jacobian matrix that associates the speed of each joint J _{1 to} J ₆ of the robot arm 251 with the speed of the hand of the robot arm 251 expressed by the coordinate system To reference fixed to the base of the robot arm 251. It is.

  FIG. 6A is a schematic diagram of the robot 200 for explaining the assembly work by the robot 200. A case where the assembly operation for assembling the workpiece W1, which is the first component on the stage St1, to the jig W2, which is the second component on the stage St2, is performed with the robot hand 252 will be described.

  Here, the workpiece W1 is a cuboid rigid body having a weight of 0.5 [kg] that can be gripped by the robot hand 252. The robot hand 252 is a two-finger gripper attached to the tip of the robot arm 251 and can grip the workpiece W1. The jig W2 is a rigid body having a rectangular parallelepiped hole into which the work W1 is fitted, and is fixed to the stage St2.

  In FIG. 6A, the teaching point P1 is a teaching point at an empty position before gripping the workpiece W1 on the stage St1. The teaching point P2 is a teaching point at a position where the workpiece W1 is gripped. The teaching point P3 is a teaching point at a position after the workpiece W1 is moved above the stage St2. The teaching point P4 is a teaching point of a position when the workpiece W1 is assembled to the jig W2 by force control. The teaching point P1 and the teaching point P2 and the teaching point P3 and the teaching point P4 are arranged 0.1 [m] apart in the vertical direction, and the teaching point P1 and the teaching point P3 are arranged 0.2 [m] apart in the horizontal direction. .

  Here, it is assumed that the robot model 503 (FIG. 3) including the dynamic models of the robot arm 251 and the robot hand 252 is known. The robot model 503 is estimated in advance by an identification method based on a Lagrange formulation from information on the position, speed, acceleration, and torque of each joint in a plurality of postures.

  The robot hand 252 is included in the hand load model estimated this time, and is handled as an initial value of the hand load model. In the first embodiment, the robot hand 252 is 0.5 [kg].

_Let φ _{a be} the load value of the hand load model before gripping the workpiece, and let φ _{b be} the estimated value of the hand load model estimated after gripping the workpiece. 6 (b) is a diagram for explaining a load value phi _a of the end load model, FIG. 6 (c) is a view for explaining a load value phi _b of the end load model.

Load value phi _a of the end load model is a load value of the portion of the robot hand 252, the load value phi _b of the end load model, the robot hand 252 and the load value of the sum of the work W1 gripped by the robot hand 252 It is.

FIG. 7 is a flowchart showing a method of manufacturing an assembly part according to the first embodiment. The CPU 301 controls the robot arm 251 to move the robot hand 252 from the position of the teaching point P1 to a gripping position for gripping the workpiece W1 at the teaching point P2 (S51). At this time, CPU 301 has been set in advance, with a load value phi _a of the end load model of the hand of the robot 200 controls the operation of the robot 200.

  Next, the CPU 301 causes the robot hand 252 to grip the workpiece W1 (S52).

  Next, the CPU 301 controls the operation of the robot arm 251 and starts to move the robot hand 252 to the next teaching point P3 (in the first embodiment, translational movement) (S53). At this time, the hand fluctuation of the robot 200 causes a load fluctuation due to the gripping of the workpiece W1.

The CPU 301 moves the hand from the teaching point P2 to the teaching point P3 when a load fluctuation occurs at the hand, and uses the hand force F detected by the torque sensors 541 to 546 to estimate the hand load model. φ _b is obtained (S54: estimation process, estimation process).

  In the first embodiment, the load fluctuation is a change from a state in which the robot hand 252 is not gripping (supporting) the workpiece W1 to a state of gripping (supporting) the workpiece W1.

In the first embodiment, the estimated value φ _b is obtained using the hand force detected by the force detection unit 540 in a state where the hand of the robot 200 is moved (translationally moved). Therefore, when acquiring data of the hand force F from the force detection unit 540, a translational force is applied to the hand of the robot 200.

As described above, the CPU 301 obtains the estimated value φ _b of the hand load model based on the current hand force F. The timing for detecting the hand force F may be the moment when the work W1 is lifted, that is, when acceleration is applied to the hand of the robot 200, when the work W1 is lifted as in the first embodiment. In the first embodiment, the hand force F is detected at the timing when the work W1 is lifted 0.015 [m] vertically upward after the operation is started.

FIG. 8A is a graph showing the target position and the current position of the hand in the vertical direction with respect to time in the first embodiment. The timing with raised work W1 from the position of the teaching point P2 is indicated by t _a. Figure 8 (a) in _{t a} is 0.5 [sec]. At this time, the operation of the robot arm 251 is not stopped.

The estimation process in step S54 will be described in detail. First, the CPU 301 calculates hand force (S54-1). Here, the time _t the current hand force in _{a F ext,} when an external force torque applied to each joint _J 1 through J ₆ and tau _ext, hand force _{F ext} is calculated by the following equation (2). The external force torque is a torque generated at each joint _J 1 through J ₆ of the robot 200 (robot arm 251) when the hand force is exerted.

  The subscript -T in equation (2) represents the transpose of the inverse matrix.

The external force torque τ _ext is calculated by subtracting the torque command value τ _ref from the actual torque τ of each joint J _{1 to} J ₆ detected by the torque sensors 541 to 546. However, the method of calculating the external force torque is not limited to this. For example, the external torque τ _ext may be calculated by estimating the actual torque τ of each joint from the current values of the electric motors 211 to 216. Alternatively, the disturbance torque may be calculated from the torque command value τ _ref using a disturbance observer and the position / velocity / acceleration of each joint when the robot arm 251 is driven thereby.

Next, a method for calculating a hand load model from the current hand force F _ext will be described. First, the CPU 301 calculates the hand load mass (S54-2). Assuming that the Z axis of the robot coordinate system To is vertically upward, the mass m _{l of the} hand load can be calculated from the Z axis direction component F _z of the hand force F _ext and the gravitational acceleration g as shown in the following expression (3).

In the equation (3), a _z is a Z-axis direction component of the acceleration a in the translation direction of the hand of the robot 200 expressed on the basis of the coordinate system To. The acceleration a is calculated by forward kinematics from the speed and acceleration of each axis of the robot 200.

  The CPU 301 calculates the gravity center position of the hand load (S54-3). The center-of-gravity position of the hand load can be calculated as the following equation (4) using the rotational moment of the hand force.

In Equation (4), s _lx , s _ly , and s _1z mean the positions of the center of gravity of the hand load in the X-axis, Y-axis, and Z-axis directions, respectively, and M _x , M _y , and M _z are the hand forces F _ext. This means the rotational moment around the X, Y, and Z axes.

The CPU 301 calculates the inertia tensor of the hand load (S54-4). Here, the inertia tensor I _l of the hand load and be represented as follows, to explain its Determination.

In Equation (5), I _xx , I _yy , and I _zz are moments of inertia around the X-axis, Y-axis, and Z-axis of the hand load, and I _xy , I _xz , and I _yz are inertial products of the hand load. . The inertial moments I _xx , I _yy , and I _zz of the hand load can be calculated as in the following formula (6).

In Equation (6), ω _x , ω _y , and ω _z are angular velocities around the X-axis, Y-axis, and Z-axis of the hand load, respectively, and their time derivatives (angular acceleration) are the speed and acceleration of each joint of the robot arm 251. And forward kinematics from the Jacobian matrix J.

Moreover, the rotational moment M and inertia tensor I _l forces of the hand load following relationship holds.

Of the inertia tensor I _l , the moments of inertia I _xx , I _yy , and I _zz are calculated by the equation (6). Therefore, by solving the equation (7), the remaining inertial products I _xy , I _xz , and I _yz Calculate the inertia tensor.

Here, the mass of the estimate phi _b are accurately estimated, assumed to be the actual mass of the end load 1 [kg].

Next, CPU 301 may be reflected in the load value of the end load model phi used in actual control of the robot 200 estimates phi _b of the end load model estimated in step S54 (S55). Specifically, CPU 301 is a load value of the hand load model after load change phi (t), over time gradually from the load value phi _a of the front of the hand load model of load change, toward the estimated value phi _b (Setting process, setting process).

That is, in the first embodiment, the estimated value φ _b of the estimated hand load model is not reflected at a time, and the estimated value φ _b after gripping the workpiece and the hand load model before gripping the workpiece as shown in the following equation (8). multiplied by a correction coefficient k to the difference between the load value phi _a of adding to the load value phi _a. The calculation result is set as the load value φ of the hand load model used in the control after gripping the workpiece.

  Here, the correction coefficient k is a scalar quantity of 0 or more and 1 or less determined from the movement amount and the movement time from the teaching point P2 to the teaching point P3, and gradually increases monotonically from 0 to 1. The load value φ of the hand load model includes values of the hand mass, the center of gravity position, and the inertia tensor.

FIG. 8B is a graph showing the correction coefficient k (t) with respect to time in the first embodiment. FIG. 8C is a graph showing the load value φ (t) of the hand load model with respect to time in the first embodiment. In the first embodiment, the correction factor k (t), as shown in FIG. 8 (b), with respect to the elapsed time from the time starting the movement from the teaching point P2 and t _a elapsed intended to vary linearly is there. The change time (gradient) of the correction coefficient k (t) is the settling time (assuming that the response of the hand position to the torque command value is a secondary system represented by the following transfer function (Equation (9)). 0.5 seconds) as a guide. The settling time was the time until the response was within ± 5% of the target value.

  In Equation (9), ζ represents an attenuation coefficient, and ω represents a natural angular frequency, which were set to 0.3 and 20.0, respectively.

Next, the CPU 301 determines whether or not the correction coefficient k has reached 1 (S56). If it has not reached (S56: No), the correction coefficient k is updated (S57), and the process returns to step S55 to estimate. determining the load value phi of the end load model used to control and correct the value phi _b in correction coefficient k. Load value of the end load model phi (t), as shown in FIG. 8 (c), over time increases monotonically to estimate phi _b from the load value phi _a preload change.

CPU301, when correction coefficient k reaches 1 (S56: Yes), the load value of the end load model phi (t) reaches the estimated value phi _b, the reflection processing of reflecting the estimate to hand load model Then, the hand of the robot 200 is made to reach the teaching point P3 (S58).

Then, CPU 301 moves the hand to the taught point P4 Complete assembling a force control based on the load value phi _b of the end load model (S59: assembling step), the process ends. That is, in step S59, the control operation of the robot 200 using the load values phi _b of CPU301 is hand load model, assemble the workpiece W1 to the jig W2.

  In the first embodiment, the profile of the correction coefficient k has been described as being 1 when the hand of the robot 200 reaches the teaching point P3 or 1 before reaching the teaching point P3. The determination method is not limited to this. For example, when a large change in hand load is expected, the correction coefficient is determined to be 1 after the hand reaches the teaching point P3 so that a more stable hand load model can be estimated. Also good.

  By performing the processes in steps S51 to S59 described above, it is possible to suppress the position of the hand of the robot 200 from diverging or vibrating when the hand load changes rapidly during the operation of the robot 200. .

Next, as a comparative example, load values of the end load model phi a (t), will be described of changing stepwise the estimated value phi _b from the load value phi _a. FIG. 9A is a graph showing the load value φ (t) of the hand load model with respect to time in the comparative example. FIG. 9B is a graph showing the target position and the current position of the hand in the vertical direction with respect to time in the comparative example.

  As shown in FIG. 9A, since the load value φ (t) of the hand load model changes in a step shape, the torque command values of the electric motors 211 to 216 also change in a step shape. When the torque command value changes stepwise, control is performed to cause the torque of each joint to follow the torque command value, so the torque of each joint changes rapidly. Therefore, the robot arm 251 vibrates and vibrates at a mechanical resonance frequency as shown in FIG. In particular, when each electric motor 211 to 216 drives the link of the robot arm 251 via a speed reducer (not shown), since the rigidity of the speed reducer is generally low, vibration is caused by a steep fluctuation of each electric motor 211 to 216. Easy to be.

On the other hand, in the first embodiment, as shown in FIG. 8C, the load value φ (t) of the hand load model is the load value (initial value) φ _a while the correction coefficient k changes from 0 to 1. gradually approaches from the estimated value φ _b. As a result, the torque command value τ _MFref1 ~τ _MFref6 which is obtained by the force control unit 505 also changes gradually.

Therefore, in the first embodiment, the load value of the end load model phi (t) will change gradually the estimated value phi _b against time, the steep model change, i.e. the respective electric motors 211 to 216 Steep torque fluctuation can be suppressed. Therefore, as shown in FIG. 8A, hand vibration (or divergence) as shown in FIG. 9B is suppressed. Thereby, it is possible to prevent the robot 200 or the work W1 from coming into contact with surrounding objects due to vibration or divergence of the robot 200.

  In addition, since the model can be corrected while the robot arm 251 is operated, a seamless operation is possible and the cycle time is shortened.

In the first embodiment, the case where the correction coefficient k is changed with respect to the elapsed time from the start of movement has been described. However, the correction coefficient k may be changed with respect to the movement amount from the teaching point P2. Even in this case, the load value phi of the hand model, so that the change over time gradually from an initial value phi _a to estimate phi _b.

Furthermore, in the first embodiment, since the hand load is estimated based on the load torques τ _{1 to} τ ₆ of each axis, the dynamic model of the end effector added to the hand can also be corrected. Accordingly, it is possible to cope with a change in the dynamic model due to the replacement of the end effector or the deformation of the end effector itself.

[Second Embodiment]
Next, a robot apparatus according to the second embodiment will be described. FIG. 10 is a flowchart showing a method of manufacturing an assembly part according to the second embodiment. In the second embodiment, a procedure for calculating a more accurate hand load model by moving the hand to the position of the teaching point P1 again in the hand load model estimation procedure of the first embodiment is added. Since the configuration of the robot apparatus and other control operations are the same as those in the first embodiment, the same reference numerals are used and detailed description thereof is omitted. Hereinafter, description will be given along the flowchart shown in FIG.

The CPU 301 stores the hand force F _{ext_a} when the hand position of the current robot 200 is at the teaching point P1 in a storage unit (for example, the HDD 304) (S61). That is, the HDD 304 serving as the storage unit has a hand force (first value) detected by the force detection unit 540 when the robot hand 252 is not supporting the workpiece W1 and is in a predetermined position and posture positioned at the teaching point P1. Hand force) F _{ext — a} is stored. Here, the HDD 304 serving as a storage unit has a load value (first load value, initial value) of a load model (end effector load model) of the robot hand 252 in a state where the robot hand 252 does not hold (support) the workpiece W1. ) previously stores phi _a.

  Next, the CPU 301 controls the robot arm 251 to move the robot hand 252 from the teaching point P1 to the teaching point P2 (S62). Next, the CPU 301 causes the robot hand 252 to grip the workpiece W1 (S63).

  Next, the CPU 301 controls the operation of the robot arm 251 and starts moving the robot hand 252 to the teaching point P1 (S64). At this time, the hand fluctuation of the robot 200 causes a load fluctuation due to the gripping of the workpiece W1.

The CPU 301 moves the hand from the teaching point P2 to the teaching point P1 when a load fluctuation occurs at the hand, and uses the hand force F detected by the torque sensors 541 to 546 to estimate the hand load model. φ _b is obtained (S65: estimation process, estimation process). In the second embodiment, the load fluctuation is a change from a state where the robot hand 252 is not gripping (supporting) the workpiece W1 to a state where the robot is holding (supporting) the workpiece W1. The estimation process in step S65 is the same as the estimation process in step S54 described in the first embodiment.

Here, for the sake of explanation, the estimated value φ _b of the estimated hand load model (mass) includes a modeling error, and is deviated by 0.5 [kg] from the mass 1 [kg] of the actual hand load. Suppose that it is 5 kg.

The CPU 301 calculates a distance L between the current position P of the hand of the robot 200 in the Cartesian space and the teaching point P1 from the angles q _{1 to} q ₆ of each joint of the current robot arm 251 (S66).

  The CPU 301 determines whether or not the calculated distance L is equal to or less than a preset threshold value Lth for a preset fixed time (S67).

  That the distance L is equal to or less than the threshold value for a certain period of time indicates that the hand of the robot 200 has moved to a predetermined position and orientation (including an error equal to or less than the threshold value), and in the second embodiment, to the teaching point P1. The fixed time is, for example, 0.2 [second], and the threshold Lth is, for example, 0.015 [m].

  If the CPU 301 is equal to or less than the threshold value Lth for a certain time (S67: Yes), the process proceeds to step S71. If not (S67: No), the process proceeds to step S68.

In step S68, CPU 301 is similar to step S55, the load value of the end load model phi (t), over time gradually from the load value phi _a of the front of the hand load model of load change, toward the estimated value phi _b Set to change (setting process, setting process).

  In step S69, the CPU 301 determines whether or not the correction coefficient k has reached 1, as in step S56. If the correction coefficient k has reached 1 (S69: Yes), the CPU 301 returns to step S66 to calculate the distance L. If the correction coefficient k has not reached 1 (S69: No), the CPU 301 sets the correction coefficient k. Update (S70) and return to step S66 to calculate the distance L.

In step S70, CPU 301 from the difference between the hand force _{F Ext_a} the current hand force _{F Ext_b} stored in step S61, calculates an estimated value phi _c of more precise hand load model (estimation processing, estimation step).

The CPU 301 _detects the hand force F _{ext_a} and the hand force detected by the force detection unit 540 when the robot hand 252 is gripping (supporting) the workpiece W1 (the second hand). Force) The difference ΔF _ext from F _{ext — b} is calculated.

CPU301 is the difference [Delta] F _ext thus determined load value of the object load model is a load model of workpiece W1 robot hand 252 grips (second load value) is calculated, the first load value phi _a second load value by adding the bets to calculate the estimated value phi _c.

Here, the mass of the workpiece W1 is mw, the center of gravity is sw, and the difference between the hand forces F _{ext_a} and F _{ext_b} is ΔF _ext = F _{ext_b} −F _{ext_a} . The mass mw and the center-of-gravity position sw of the workpiece W1 can be calculated in the same procedure by replacing F _ext in steps S54-2 and S54-3 described in the first embodiment with ΔF _ext .

Then, these estimated mass of the workpiece W1 mw, the center of gravity position sw, respectively added to the load value phi _a of the end load model (mass and center of gravity position), the load of the new hand load model (mass and center of gravity position) the value φ _c. Mass, model information other than the center of gravity position, specifically, the inertia tensor, and leaves the load value phi _b.

Here, it is assumed that the mass value in the estimated value φ _c of the hand load model is accurately estimated and is the mass 1 [kg] of the actual hand load.

CPU301 is estimated in step S71, to actually be reflected in the load value phi of the end load model used in the control of the robot 200 estimates phi _c of more precise hand load model (S72). Processing contents are the same as those obtained by replacing the load value of the end load model in step S55 phi _a and an estimate phi _b each load value phi _b and estimate phi _c. Specifically, the CPU 301 gradually shifts the load value φ (t) of the hand load model after the load change from the load value φ _b of the hand load model before the load change to the estimated value φ _c over time. (Setting process, setting process).

  FIG. 11A is a graph showing a correction coefficient k (t) with respect to time in the second embodiment. FIG. 11B is a graph showing the load value φ (t) of the hand load model with respect to time in the second embodiment. FIG. 11C is a graph showing the target position and the current position of the hand in the vertical direction with respect to time in the second embodiment. The correction coefficient k is changed as shown in FIG. 11B, and the change in the load value φ of the hand load model is changed as shown in FIG.

  Since the processes of steps S73 to S76 are the same as steps S56 to S59 of FIG. 7 described in the first embodiment, the description thereof is omitted.

Above processing by performing is CPU301 of step S71 described, it can be determined from the difference of the hand force before and after gripping the workpiece is W, the estimated value phi _c of more accurate hand load model.

FIG. 12 is a graph showing the target position and current position of the hand in the vertical direction with respect to time when a model error is included. 12, a broken line represents a position of the hand when there in the procedure of the first embodiment, is an error in the actual hand load and the estimated value phi _b of the end load model estimated in step S54. In FIG. 12, since the estimated model contains an error, it can be seen that a stationary deviation remains. The mass of the estimated hand load model phi _b in step S54 is 1.5 [kg], the actual mass of the hand load 1 [kg].

  On the other hand, the broken line in FIG. 11C represents the position of the hand when the hand load model is re-estimated from the difference in hand force before and after gripping the workpiece W1 at the teaching point P1 in the procedure of the second embodiment. ing. As shown in FIG. 11C, it can be seen that the steady-state deviation of the hand position as shown in FIG. 12 is reduced.

  Thereby, according to 2nd Embodiment, the mass and center-of-gravity position of a hand in a hand load model can be calculated | required more correctly, and a model error can be reduced more.

[Third Embodiment]
Next, a robot apparatus according to a third embodiment will be described. FIG. 13 is a flowchart showing a method of manufacturing an assembly part according to the third embodiment. The third embodiment is different from the second embodiment in that the hand load model is not moved again to the position of the teaching point P1, but moved to the next teaching point P3, which is the next movement destination, in the estimation procedure of the hand load model of the second embodiment. Since the configuration of the robot apparatus and other control operations are the same as those in the first and second embodiments, the same reference numerals are used and detailed description thereof is omitted. Hereinafter, description will be given along the flowchart shown in FIG.

  However, Steps S81 to S83, S85, S88 to S90, and S93 to S97 are the same as those in the second embodiment, and thus description thereof will be omitted, and only added or changed steps will be described in detail.

In step S <b> 81, the CPU 301 stores a hand force when the current hand position of the robot 200 is at the teaching point P <b> 1 in a storage unit (e.g., HDD 304). That is, the HDD 304 serving as the storage unit has a hand force (first force) detected by the force detection unit 540 when the robot hand 252 does not support the workpiece W1 and is in the first position and posture positioned at the teaching point P1. 1 hand force) is memorized. Here, the load value (first load value, initial value) φ of the load model (end effector load model) of the robot hand 252 when the robot hand 252 is not gripping the workpiece W1 is stored in the HDD 304 serving as a storage unit. _a is stored in advance.

  In step S84, the CPU 301 controls the operation of the robot arm 251 and starts moving the robot hand 252 to the teaching point P3 next to the teaching point P2.

In step S86, the CPU 301 calculates a distance L between the current position P of the hand of the robot 200 and the teaching point P3 in the Cartesian space from the angles q _{1 to} q ₆ of each joint of the current robot arm 251.

  In step S87, the CPU 301 determines whether or not the calculated distance L is equal to or less than a preset threshold value Lth for a preset fixed time. That the distance L is equal to or less than the threshold value for a certain time indicates that the hand of the robot 200 has moved to the second position / posture (including an error equal to or less than the threshold value), which is the teaching point P3 in the third embodiment.

  If the CPU 301 is equal to or less than the threshold value Lth for a certain time (S87: Yes), the process proceeds to step S91; otherwise (S87: No), the process proceeds to step S88.

In step S91, the CPU 301 uses the hand force stored in the HDD 304 in step S81 to obtain the hand force (first hand force) when the robot hand 202 does not support the workpiece W1 and is in the position and orientation of the teaching point P3. presume. Specifically, the CPU 301 calculates a hand force F _ext3 equivalent to the work point before gripping the workpiece at the teaching point P3 based on the hand force at the teaching point P1 stored in step S81 by the following equation (10).

Note that F _ext1 is a hand force before gripping the workpiece at the teaching point P1, and R _3-1 represents a rotation matrix from the teaching point P3 to the teaching point P1. However, if the posture at the teaching point P3 is the same as the posture at the teaching point P1, the rotation matrix R _3-1 is a 3 × 3 unit matrix, and the calculation in Expression (10) is not necessary.

  Next, the CPU 301 re-estimates the hand load model from the difference between the hand force before gripping the workpiece at the teaching point P3 calculated in step S91 and the current actual hand force (S92).

That is, the CPU 301 detects the hand force (second hand force) F _{ext_b} detected by the force detection unit 540 when F _ext3 and the robot hand 252 are gripping the workpiece W1 and the teaching point P3 is in the position and orientation. The difference ΔF _ext is calculated.

CPU301 is the difference [Delta] F _ext thus determined load value of the object load model is a load model of workpiece W1 robot hand 252 grips (second load value) is calculated, the first load value phi _a second load value by adding the bets to calculate the estimated value phi _c.

As described above, the CPU 301 performs the processing of steps S91 and S92, so that a more accurate estimated value φ _{c of the} hand load model is obtained from the difference between the hand force before gripping the workpiece W1 at the teaching point P3 and the current actual hand force. Can be requested.

  Further, since the hand of the robot 200 can be moved directly to the next moving position without moving again to the position before gripping the workpiece W1 for re-estimating the hand load model, the work can be performed without increasing the cycle time. .

[Fourth Embodiment]
Next, a robot apparatus according to a fourth embodiment will be described. FIG. 14A is a graph showing a correction coefficient k (t) with respect to time in the fourth embodiment.

  The fourth embodiment differs from the first embodiment in that the correction coefficient k (t) has a profile that changes nonlinearly with time. Since the configuration of the robot apparatus and other control operations are the same as those in the first embodiment, the same reference numerals are used and detailed description thereof is omitted.

  FIG. 14A shows a case where the correction coefficient k (t) is a fifth-order polynomial profile. The correction factor k (t) is a function of time that can be differentiated in time. The correction coefficient k (t) is increased smoothly and monotonously from 0.5 [seconds] to 1.0 [seconds]. The fifth order polynomial k (t) is given by the following equation (11).

k _f represents the end point value of k (t), and t ₀ and t _f represent the start point and end point times, respectively.

  FIG. 14B is a graph showing the load value φ (t) of the hand load model with respect to time in the fourth embodiment. FIG. 14C is a graph showing the target position and the current position of the hand in the vertical direction with respect to time in the fourth embodiment.

  As shown in FIG. 14B, it can be seen that the load value φ (t) of the hand load model changes smoothly in the same manner as the change of the correction coefficient k (t). Moreover, when FIG.14 (c) is seen, compared with the change of the hand position in FIG. 12, it turns out that a vibration is reduced more.

  As described above, according to the fourth embodiment, since the correction coefficient k (t) is a smooth non-linear function, the vibration of the robot 200 is further reduced. The profile of the differentiable correction coefficient k (t) is not limited to this, and may be a sigmoid function or a spline curve. Further, by applying the correction coefficient of the fourth embodiment to the correction coefficient of the second or third embodiment, a more accurate and stable hand load model can be estimated.

[Fifth Embodiment]
Next, a robot apparatus according to a fifth embodiment will be described. FIG. 15 is a flowchart showing a method of manufacturing an assembly part according to the fifth embodiment. The fifth embodiment is different from the first embodiment in that an estimated value of the hand load model is obtained a plurality of times. Since the configuration of the robot apparatus and other control operations are the same as those in the first embodiment, the same reference numerals are used and detailed description thereof is omitted. In the following, description will be given along the flowchart shown in FIG.

  In the fifth embodiment, the processing contents of steps S101 to S109 are the same as steps S51 to S59 of FIG. 7 described in the first embodiment, but in the fifth embodiment, the process proceeds to step S104 after step S107.

  That is, in the first embodiment, the hand load model is estimated only once immediately after the start of the movement of the hand of the robot 200, whereas in the fifth embodiment, the hand load model is performed a plurality of times until the correction coefficient reaches 1. To converge.

In step S104, the CPU 301 calculates an average of the estimated values of the past hand load models estimated a plurality of times, and uses this as the estimated value φ _b of the hand load model used in the next step S105.

As described above, according to the fifth embodiment, CPU 301 by performing a convergence calculation of the estimated value, obtained an estimate phi _b precise hand load model. Note that the average calculation method of the past hand load models estimated a plurality of times in step S104 may be either an overall average or a moving average. Also, a median value may be calculated instead of the average and used.

[Sixth Embodiment]
Next, a robot apparatus according to a sixth embodiment will be described. FIG. 16 is a flowchart showing a method of manufacturing an assembly part according to the sixth embodiment. The sixth embodiment is different from the first embodiment in that a rotational movement of the hand is simultaneously performed when the hand of the robot 200 moves. Since the configuration of the robot apparatus and other control operations are the same as those in the first embodiment, the same reference numerals are used and detailed description thereof is omitted. Hereinafter, description will be given along the flowchart shown in FIG.

  In the sixth embodiment, the processing contents of steps S111 to S112 and S114 to S119 are the same as steps S51 to S52 and S54 to S59 of FIG. 7 described in the first embodiment.

  In step S113, the movement from the teaching point P2 to the teaching point P3 is started. At this time, the hand is rotated a plurality of times (about three times) around the XYZ axes so that not only the translational force but also the rotational force acts on the hand. At the teaching point P3, a rotational motion that returns to the original angle is added during movement.

  By applying such rotational motion to the hand, not only the translational acceleration but also the angular acceleration in the rotational direction is generated at the hand, and the rotational component of the hand force is also generated. Thereby, the center of gravity position of the hand load and the inertia tensor as calculated by the equations (4) to (7) can be easily converged, and a more accurate hand load model can be estimated.

  In the sixth embodiment, after step S117, the process proceeds to step S114 as in the fifth embodiment. That is, in the first embodiment, the hand load model is estimated only once immediately after the start of hand movement, whereas in the sixth embodiment, the hand load model is converged several times until the correction coefficient reaches 1. .

In step S114, the CPU 301 calculates the average of the estimated values of the past hand load models estimated a plurality of times, and sets the average as the estimated value φ _b of the hand load model used in the next step S115.

As described above, according to the sixth embodiment, CPU 301 by performing a convergence calculation of the estimated value, obtained an estimate phi _b precise hand load model. Note that the average calculation method of the past hand load models estimated a plurality of times in step S114 may be either the overall average or the moving average. Also, a median value may be calculated instead of the average and used.

  Although the sixth embodiment has been described on the assumption that a rotational motion is added when moving from the teaching point P2, the sixth embodiment moves to the teaching point P1 once and converges as in the second embodiment. An additional rotational motion may be performed to further converge the model.

  The present invention is not limited to the embodiments described above, and many modifications are possible within the technical idea of the present invention. In addition, the effects described in the embodiments of the present invention only list the most preferable effects resulting from the present invention, and the effects of the present invention are not limited to those described in the embodiments of the present invention.

  The present invention supplies a program that realizes one or more functions of the above-described embodiments to a system or apparatus via a network or a storage medium, and one or more processors in a computer of the system or apparatus read and execute the program This process can be realized. It can also be realized by a circuit (for example, ASIC) that realizes one or more functions.

  In the above-described embodiment, the case where the robot arm 251 has a vertical articulated joint has been described. However, the present invention is not limited to this. The robot arm may be various robot arms such as a horizontal articulated robot arm, a parallel link robot arm, and an orthogonal robot. That is, the driving direction of the joint is not limited to driving in the rotational direction, but includes driving in the linear motion direction (extension / retraction driving). Furthermore, although the case where the electric motor drives the joint has been described, the present invention is not limited to this, and may be, for example, an artificial muscle or the like. In the above-described embodiment, the case where the robot arm has six joints has been described. However, the number of joints is not limited to this.

  In the above-described embodiment, the case where the force detection unit is configured by a plurality of torque sensors has been described. However, the present invention is not limited to this, and a force sensor provided at the tip of the robot arm may be used. Good.

DESCRIPTION OF SYMBOLS 100 ... Robot apparatus, 200 ... Robot, 251 ... Robot arm, 252 ... Robot hand (end effector), 301 ... CPU (control part), 540 ... Force detection part

Claims (17)

An articulated robot arm, an end effector that is provided at the tip of the robot arm and supports an object, and a hand that is in a state in which the end effector supports an object or a state in which the end effector does not support an object A robot having a force detection unit for detecting the hand force applied to the
A control unit that controls the operation of the robot using a load value of the hand load model of the hand, and
The controller is
An estimation process for obtaining an estimated value of the hand load model using a hand force detected by the force detection unit when a load fluctuation occurs in the hand;
A setting process for setting the load value of the hand load model after the load change to gradually change from the load value of the hand load model before the load change to the estimated value over time. The robot apparatus characterized by performing.   The robot apparatus according to claim 1, wherein the load fluctuation is a change from a state where the end effector does not support the object to a state where the object is supported.   The said control part calculates | requires the said estimated value using the hand force detected by the said force detection part in the state which is moving the said hand in the said estimation process, The Claim 1 or 2 characterized by the above-mentioned. Robot device.   The said control part calculates | requires the said estimated value using the hand force detected by the said force detection part in the said estimation process in the state which provided the translational force or the rotational force to the said hand. The robot apparatus described.   The robot apparatus according to claim 3, wherein the control unit obtains the estimated value a plurality of times in the estimation process. A storage unit that stores in advance a first load value of an end effector load model of the end effector;
The control unit, as the estimation process,
The first hand force detected by the force detection unit when the end effector does not support the object and is in a predetermined position and posture, and the predetermined position while the end effector supports the object. Calculating the second load value of the object load model of the object supported by the end effector from the difference from the second hand force detected by the force detection unit at the time of posture;
The robot apparatus according to claim 2, wherein the estimated value is calculated by adding the first load value and the second load value. A storage unit that stores in advance a first load value of an end effector load model of the end effector;
The control unit, as the estimation process,
From the hand force detected by the force detection unit when the end effector does not support the object and in the first position and posture, the second position and orientation is determined while the end effector does not support the object. Estimate the first hand force at
From the difference between the first hand force and the second hand force detected by the force detector when the end effector is supporting the object and in the second position and posture, the end effector Calculating a second load value of the object load model of the supported object;
The robot apparatus according to claim 2, wherein the estimated value is calculated by adding the first load value and the second load value.   The robot apparatus according to any one of claims 1 to 7, wherein the hand load model is a mass, a gravity center position, an inertia tensor, or a combination thereof of the hand. The load value of the hand load model before the load change and phi _a,
The estimated value and phi _b,
When the correction coefficient that changes between 0 and 1 is k,
In the setting process, the control unit

9. The load value of the hand load model after the load change is calculated by gradually increasing the correction coefficient k from 0 to 1 over time according to the equation: The robot apparatus according to any one of claims.   The robot apparatus according to claim 9, wherein the correction coefficient k changes linearly with time.   The robot apparatus according to claim 9, wherein the correction coefficient k changes nonlinearly with time.   The robot apparatus according to claim 11, wherein the correction coefficient k is a function of time that can be differentiated with respect to time. The robot arm has a plurality of motors that drive each joint, and a drive control unit that controls the driving of each motor based on each torque command value for each motor,
The robot apparatus according to claim 1, wherein the control unit obtains each torque command value using a hand force detected by the force detection unit and the load model. A robot is an articulated robot arm, an end effector that is provided at the tip of the robot arm and supports the object, and the end effector supports the object or the end effector does not support the object A force detection unit that detects a hand force applied to the hand, and the control unit controls the operation of the robot using a load value of the hand load model of the hand,
An estimation step for obtaining an estimated value of the hand load model by using the hand force detected by the force detection unit when a load fluctuation occurs in the hand at the control unit;
The control unit sets the load value of the hand load model after the load change to gradually change from the load value of the hand load model before the load change toward the estimated value over time. A robot control method comprising: a setting step of:   The program for making a computer perform each process of the robot control method of Claim 14.   A computer-readable recording medium on which the program according to claim 15 is recorded. A robot is an articulated robot arm, an end effector that is provided at the tip of the robot arm and supports the object, and the end effector supports the object or the end effector does not support the object A force detection unit that detects a hand force applied to the hand, and a control unit controls an operation of the robot using a hand load model of the hand to manufacture an assembly part. A manufacturing method comprising:
An estimation step of obtaining an estimated value of the hand load model using the hand force detected by the force detection unit when the control unit supports the first component on the end effector;
A load value of the hand load model after a load change due to the control unit supporting the first component on the end effector is changed over time from a load value of the hand load model before the load change. Gradually, a setting step for setting to change toward the estimated value;
An assembly step of controlling the operation of the robot using the load value of the hand load model set in the setting step, and assembling the first part to the second part. An assembly part manufacturing method characterized by the above.

Images