JP2020151824A - Control device, robot device, control method, article manufacturing method, program, and recording medium - Google Patents

Abstract

To improve controllability of a manipulator.SOLUTION: A correction value calculation section 604 acquires measured values τ1-τ6 of a load generated in a manipulator, during positional control of the manipulator. A dynamics calculation section 602 obtains calculated values τ1^-τ6^ of the load generated in the manipulator, according to dynamics calculation using a robot model 503. The correction value calculation section 604 obtains correction values τcomp1-τcomp6 on the basis of differences between the measured values τ1-τ6 and the calculated values τ1^-τ6^. An impedance control section 601 obtains torque command values τFd1-τFd6 based on teaching data, in controlling thrust of the manipulator. A command value correction section 603 obtains command values τref1-τref6 by correcting the torque command values τFd1-τFd6 with the correction values τcomp1-τcomp6. A second control unit controls thrust of the manipulator on the basis of differences between the command values τref1-τref6 and the measured values τ1-τ6.SELECTED DRAWING: Figure 7

Description

The present invention relates to the control of a manipulator.

As the control of the manipulator in an industrial robot, position control for controlling the position of the manipulator and force control for controlling the thrust of the manipulator are known. For example, when manufacturing an assembly as an example of an article, a work is held at the tip of a manipulator, the tip of the manipulator is positioned at a target position by position control, and then the work is assembled to an object to be assembled by force control. Is being done. Patent Document 1 discloses a technique for feedback-controlling a manipulator joint based on a difference between a torque command value and a measured torque value as force control of the manipulator.

JP-A-2017-56549

However, the behavior of the manipulator may become unstable, such as the manipulator vibrating while the manipulator is force-controlled. When the manipulator is made to perform precise work, if the behavior of the manipulator becomes unstable, the precise work may be hindered, so improvement has been required.

An object of the present invention is to improve the controllability of a manipulator.

The control device of the present invention includes a control unit that controls the position or thrust of the manipulator, and the control unit acquires the first measurement value of the load generated on the manipulator while controlling the position of the manipulator. , The calculated value of the load generated in the manipulator is obtained by the calculation of dynamics using the model data of the manipulator, the correction value is obtained based on the difference between the first measured value and the calculated value, and the manipulator When controlling the thrust of the manipulator, a temporary command value based on the teaching data is obtained, the command value is obtained by correcting the temporary command value with the correction value, and the second measured value of the load generated on the manipulator is acquired. However, it is characterized in that the thrust of the manipulator is controlled based on the difference between the command value and the second measured value.

According to the present invention, the controllability of the manipulator is improved.

It is a perspective view which shows the schematic structure of the robot apparatus which concerns on 1st Embodiment. It is a block diagram which shows the structure of the control device which concerns on 1st Embodiment. It is a block diagram which shows the structure of the control device 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 of the process of the control device in 1st Embodiment. It is a flowchart of the process of the control device in 1st Embodiment. It is a block diagram of the 1st control unit which concerns on 1st Embodiment. It is the schematic of the robot arm in 1st Embodiment. (A) is a figure which shows an example of the dynamic parameters of the actual manipulator in the 1st Embodiment. (B) is a figure which shows an example of the dynamics parameter of the robot model in 1st Embodiment. (A) and (b) are schematic views of the manipulator for explaining the assembling work by the manipulator in the first embodiment. (A) is a figure which shows the parameter of the teaching point in 1st Embodiment. (B) is a figure which shows the parameter of the mechanical impedance of force control in 1st Embodiment. It is a flowchart explaining the manufacturing method of the article which concerns on 1st Embodiment. (A) and (b) are graphs showing the experimental results in the first embodiment. (C) is a graph showing the experimental results in the comparative example. It is a perspective view which shows the schematic structure of the robot apparatus which concerns on 2nd Embodiment. It is a block diagram which shows the structure of the control device which concerns on 2nd Embodiment. It is a block diagram which shows the structure of the control device which concerns on 2nd Embodiment. It is a control block diagram which shows the control system of the robot apparatus which concerns on 2nd Embodiment. It is a block diagram of the 2nd control unit which concerns on 2nd Embodiment. It is a flowchart explaining the manufacturing method of the article which concerns on 2nd Embodiment. It is a graph which shows the experimental result in 2nd Embodiment. It is a block diagram which shows the 2nd control unit which concerns on 3rd Embodiment. It is a graph which shows the experimental result in 3rd Embodiment. It is a perspective view which shows the schematic structure of the robot apparatus which concerns on 4th 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 showing a schematic configuration of a robot device according to the first embodiment. As shown in FIG. 1, the robot device 100 includes a manipulator 200 and a control device 1000 having a control unit 500 that controls the position or thrust of the manipulator 200. Further, the robot device 100 includes a teaching pendant 600 as a teaching device for transmitting teaching data to the control device 1000. The teaching pendant 600 is operated by an operator and is used to specify the operation of the manipulator 200 and the control device 1000.

The manipulator 200 includes a robot arm 251 and a hand 252, which is an example of an end effector and constitutes the tip of the manipulator 200. The robot arm 251 is a vertically articulated robot arm in this embodiment.

A hand 252 is attached to the tip of the robot arm 251. Hereinafter, the case where the end effector is the hand 252 will be described, but the present invention is not limited to this, and a tool or the like may be used. The base end portion of the robot arm 251 is fixed to the pedestal B. The hand 252 grips a work such as a part or a tool.

The manipulator 200, that is, the robot arm 251 may have one or more joints, and in the present embodiment, it has a plurality of joints, for example, six joints J _{1 to} J ₆ . Robotic arm 251 includes a servo mechanism unit ₂₀ 1 to 20 ₆ of a plurality (six) for rotating each joint _J 1 through J ₆ about the respective axes _A 1 to A _6.

In the present embodiment, the joint J ₁ through J ₆ is the rotary joints may be prismatic joint. When the joint is a rotary joint, the position of the joint represents the angle of the joint, that is, the rotational position. When the joint is a linear motion joint, the position of the joint represents the linear motion position. When the joint is a rotary joint, the thrust of the joint represents the propulsion torque of the joint, that is, the output torque. When the joint is a linear joint, the thrust of the joint represents the linear thrust of the joint.

Robotic arm 251 includes a plurality of links ₂₁₀ 0-210 _6. Links ₂₁₀ 0-210 ₆ is rotatably articulated J1 to J6. The links 210 _{0 to} 210 ₆ are connected in series in order from the proximal end side to the distal end side. The robot arm 251 can move the tip of the manipulator 200, that is, the hand 252, to an arbitrary position within the movable range.

The position, that is, the posture of the manipulator 200 can be expressed by the coordinate system To. The coordinate system To represents the base end of the robot arm 251, that is, the coordinate system fixed to the pedestal B, and the coordinate system Te represents the tip end portion of the manipulator 200, that is, the coordinate system linked to the hand 252.

The control unit 500 includes a control unit 300 which is a first control unit and a control unit 400 which is a second control unit. The control unit 300 controls the robot device 100 in an integrated manner. The control unit 400 controls the position or thrust of the manipulator 200 based on the command of the control unit 300. The control unit 400 is a servo control device that controls each of the servo mechanism units 20 _{1 to 26} of the manipulator 200.

The control unit 500 will be specifically described. FIG. 2 is a block diagram showing a configuration of a control device according to the first embodiment. As described above, the control unit 500 has control units 300 and 400. FIG. 2 illustrates in detail the hardware configuration of the control unit 300.

The control unit 300 is composed of a computer and includes a CPU (Central Processing Unit) 301 as a processing unit. Further, the control unit 300 includes a ROM (Read Only Memory) 302, a RAM (Random Access Memory) 303, and an HDD (Hard Disk Drive) 304 as storage units. Further, the control unit 300 includes a recording disk drive 305 and interfaces 306 to 309.

The CPU 301, ROM 302, RAM 303, HDD 304, recording disk drive 305, and interfaces 306 to 309 are connected by a bus 310 so as to be able to communicate with each other. The ROM 302 stores a program 330 for causing the CPU 301 to execute the processing of the control method described later. The CPU 301 executes a control method described later based on the program 330 recorded (stored) in the ROM 302. The RAM 303 is a storage device that temporarily stores various data such as the calculation processing result of the CPU 301.

The HDD 304 is a storage device that can store the arithmetic processing results of the CPU 301 and various data acquired from the outside. The recording disk drive 305 can read various data, programs, and the like recorded on the recording disk 331.

The teaching pendant 600 is connected to the interface 306. The CPU 301 acquires the teaching data of the manipulator 200 from the teaching pendant 600 via the interface 306 and the bus 310.

The control unit 400 is connected to the interface 309. The CPU 301 outputs command value data to the control unit 400 via the bus 310 and the interface 309 at a predetermined control cycle.

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 so that an external storage device 322, which is a storage device such as a rewritable non-volatile memory or an external HDD, can be connected. Teaching data and model data of the manipulator 200 can be stored in the external storage device 322.

In the first embodiment, the recording medium that can be read by a computer is the ROM 302, and the program 330 is recorded in the ROM 302, but the present invention is not limited to this. The program 330 may be recorded on any recording medium that can be read by a computer. As the recording medium for supplying the program 330, for example, a flexible disk, a hard disk, an optical disk, a magneto-optical disk, a magnetic tape, a non-volatile memory, or the like can be used.

FIG. 3 is a block diagram showing a configuration of a control device according to the first embodiment. FIG. 3 illustrates in detail the hardware configuration of the control unit 400.

The control unit 400 is composed of a computer. The control unit 400 includes a CPU 401 as a processing unit, a ROM 402 and a RAM 403 as storage units, and interfaces 406 to 409. The CPU 401, ROM 402, RAM 403, and interfaces 406 to 409 are connected by a bus 410 so as to be able to communicate with each other. The ROM 402 stores a program 430 for causing the CPU 401 to execute the processing of the control method described later.

The interface 406, the motor ₂₁ 1 to 21 ₆ to be described later is connected. The interface 407, the position sensor ₂₃ 1-23 ₆ to be described later is connected. The interface 408, the torque sensor ₂₄ 1-24 ₆ to be described later is connected. A control unit 300 is connected to the interface 409.

In the first embodiment, the recording medium that can be read by a computer is the ROM 402, and the program 430 is recorded in the ROM 402, but the present invention is not limited to this. The program 430 may be recorded on any recording medium as long as it can be read by a computer. As the recording medium for supplying the program 430, for example, a flexible disk, a hard disk, an optical disk, a magneto-optical disk, a magnetic tape, a non-volatile memory, or the like can be used.

FIG. 4 is a control block diagram showing a control system of the robot device according to the first embodiment. The CPU 301 of the control unit 300 shown in FIG. 2 functions as the torque command value generation unit 504 and the position command value generation unit 505 shown in FIG. 4 by executing the program 330. CPU401 of the control unit 400 shown in FIG. 3, by executing the program 430, the torque control unit ₅₁ 1 to 51 ₆ shown in FIG. 4, the position control unit ₅₂ 1 to 52 _6, the switching unit ₅₃ 1 to 53 ₆ and, functions as a motor control unit ₅₄ 1 to 54 _6. Servomechanism units ₂₀ 1 to 20 ₆ has a motor ₂₁ 1 to 21 _6, reducer ₂₂ 1-22 _6, the position sensor ₂₃ 1-23 ₆ and the torque sensor ₂₄ 1-24 _6.

Each motor ₂₁ 1 to 21 ₆ is an example of a drive source for driving each joint _J 1 through J ₆ manipulator 200, for example an electric motor. Each reducer ₂₂ 1-22 ₆ is to output the decelerated rotation of the rotary shaft of the motor ₂₁ 1 to 21 _6, such as harmonic gear reducer. A pair of links connected by joints _J 1 through J _6, that other links against one link, driven by the motors ₂₁ 1 to 21 _6, and the reduction gear ₂₂ 1-22 ₆ it can. The rotating shaft of the motor may be directly connected to the input shaft of the speed reducer, but a transmission mechanism (not shown) such as a belt mechanism may be arranged between the motor and the speed reducer.

Each position sensor ₂₃ 1-23 ₆ is a sensor which outputs a signal corresponding to the position (angle) of each joint _J 1 through J _6, for example an encoder. The encoder is preferably a rotary encoder. Each position sensor ₂₃ 1-23 _6, the input shaft side of the reduction gear ₂₂ 1-22 _6, i.e. provided in each motor ₂₁ 1 to 21 _6, the position of the rotation axis of the motor ₂₁ 1 to 21 ₆ (angle) The signal corresponding to the above is output to the control unit 400. Control unit 400, based on the acquired signals from the position sensors ₂₃ 1 to 23 _6, the measured value theta ₁ through? ₆ position (angle) of the rotation shaft of the motor ₂₁ 1 to 21 ₆ determined, each measurement theta obtaining a measured value _q 1 to q ₆ position (angle) of each joint _J 1 through J ₆ from ₁ through? _6. In the present embodiment, based on the reduction ratio of the reduction gear ₂₂ 1-22 ₆ converts the measured value theta ₁ through? ₆ to the measured values _q 1 to q _6. The control unit 400 outputs to the control unit 300 the measurement values _q 1 to q ₆ positions of the joints _J 1 through J ₆ (angle) at a predetermined control cycle.

Each torque sensor ₂₄ 1-24 _6, a signal corresponding to the load torque acting on each joint _J 1 through J ₆ as an example of a load acting on the manipulator 200, and outputs to the control unit 400. Control unit 400, based on the acquired signals from the torque sensor ₂₄ 1-24 _6, obtains the measurement values τ ₁ ~τ ₆ of load torque applied to the joint _J 1 through J _6. The control unit 400 outputs to the control unit 300 the measurement values τ ₁ ~τ ₆ of the load torque of each joint _J 1 through J ₆ at a predetermined control cycle.

The external storage device 322 stores force teaching data 501 and position teaching data 502 as teaching data created in advance by the user, and robot model 503 which is model data of the manipulator 200. The storage device in which these data are stored is not limited to the external storage device 322. For example, these data may be stored in a storage device (not shown) included in the HDD 304 or the teaching pendant 600.

The force teaching data 501 includes a target force F _d . The position teaching data 502 includes the target position P _d . The target force F _d is a target value (target force) of the force applied to the hand 252 of the manipulator 200, and can be set by the operator using the teaching pendant 600. The target position P _d is a target value (target position) of the position of the hand 252 of the manipulator 200, and can be set by the operator using the teaching pendant 600. The control unit 300 can read the target force F _d from the force teaching data 501, and can read the target position P _d from the position teaching data 502.

The robot model 503 is a virtual model created by the user, including design data such as mass data of elements constituting the manipulator 200, center of gravity position data, and moment of inertia data around the center of gravity.

The control unit 300 switches between a position control mode for controlling the position of the manipulator 200 and a force control mode for controlling the thrust of the manipulator 200 according to a preset robot program, and sends a command to the control unit 400. That is, the control unit 300 functions as the position command value generation unit 505 when the mode is switched to the position control mode to send the position command value, and functions as the torque command value generation unit 504 when the mode is switched to the force control mode. Send the torque command value.

Position command value generation section 505, based on the target position _{P d,} to generate a position command value _q ref1 _{to q Ref6} corresponding to the joint _J 1 through J _6. Specifically, the position command value generating unit 505, the inverse kinematics calculation using the robot model 503 converts the target position _{P d} to the position command value _q ref1 _{to q Ref6} corresponding to the joint _J 1 through J ₆ .. Position command value generation section 505, the generated position command value _q ref1 _{to q Ref6,} and outputs to the position control unit ₅₂ 1 to 52 ₆ of the control unit 400. Position control unit ₅₂ 1 to 52 _6, the feedback calculation based on the difference between the measured values _q 1 to q ₆ and the position command value _q ref1 _{to q Ref6,} for example by PID calculation, generates a torque command value τ _q1 ~τ _q6 .. Position control unit ₅₂ 1 to 52 _6, it outputs a torque command value τ _q1 ~τ _q6 to the switching unit ₅₃ 1 to 53 _6.

The torque command value generation unit 504 has a torque command value corresponding to the joints J _{1 to} J ₆ based on the target force F _d , the target position P _d , the measured values τ _{1 to} τ ₆ , and the measured values q _{1 to} q _6. to generate the _{τ ref1} ~τ ref6. The specific processing of the torque command value generation unit 504 will be described later. Torque command value generating unit 504, the generated torque command value _{τ ref1} ~τ ref6, and outputs the torque control unit ₅₁ 1 to 51 ₆ of the control unit 400. Torque control unit ₅₁ 1 to 51 _6, the feedback calculation based on the difference between the measured values τ ₁ ~τ ₆ and the torque command value _{τ ref1} ~τ ref6 the load torque, for example by PID calculation, the torque command value τ _F1 ~τ _F6 To generate. Torque control unit ₅₁ 1 to 51 _6, it outputs a torque command value τ _F1 ~τ _F6 to the switching unit ₅₃ 1 to 53 _6.

Switching unit ₅₃ 1 to 53 _6, when receiving the input of the torque command value τ _F1 ~τ _F6, the motor control unit of the torque command value τ _F1 ~τ _F6 when receiving the input as the torque command value τ _M1 ~τ _M6 54 is output to the _{1-54 6.} The motor switching unit ₅₃ 1 to 53 _6, when receiving the input of the torque command value τ _q1 ~τ _q6, a torque command value τ _q1 ~τ _q6 when receiving the input as the torque command value τ _M1 ~τ _M6 to the control unit ₅₄ 1 to 54 _6.

The motor control unit ₅₄ 1 to 54 _6, passing a current _Cur 1 _{~Cur 6} corresponding to the torque command value τ _M1 ~τ _M6 to the motor ₂₁ 1 to 21 _6.

Above, the control unit 400, when acquiring the position command value _q ref1 _{to q Ref6} from the control unit 300, based on a difference between the position command value _q ref1 _{to q Ref6} and measured values _q 1 to q _6, the position of the manipulator 200 To control. The control unit 400, when the control unit 300 obtains the torque command value _{τ ref1} ~τ ref6, based on the difference between the measured values τ ₁ ~τ ₆ and the torque command value _{τ ref1} ~τ ref6, thrust manipulator 200, that is, controlling the output torque of the joint _J 1 through J _6.

The force control of the manipulator 200 in the force control mode, that is, the torque control of the joints of the manipulator 200 will be described. FIG. 5 is a flowchart of a process in which the control unit 500 force-controls the manipulator 200 in the first embodiment. First, the torque command value generation unit 504 acquires the target force F _d and the target position P _d from the external storage device 322 (S1). The torque command value generation unit 504 uses the robot model 503 to obtain a torque command value based on the target force F _d , the target position P _d , the measured load torque values τ _{1 to} τ ₆ , and the measured position values q _{1 to} q _6. τ _{ref1 to} τ _ref6 are calculated (S2). Torque control unit ₅₁ 1 to 51 _6, the PID calculation based on the difference between the torque command value _{τ ref1} ~τ ref6 and measured values τ ₁ ~τ ₆ of the load torque, and calculates the torque command value τ _F1 ~τ _F6 ( S3). Switching unit ₅₃ 1 to 53 _6, it outputs the motor control unit ₅₄ 1 to 54 ₆ a torque command value τ _F1 ~τ _F6 as the torque command value τ _M1 ~τ _M6. The motor control unit ₅₄ 1 to 54 _6, the motor ₂₁ 1 to 21 ₆ for controlling energization according to the torque command value τ _M1 ~τ _M6 (S4). Motor ₂₁ 1 to 21 ₆ generates an output torque to the joint _J 1 through J ₆ in response to energization (S5). Control unit 300, 400 obtains a measured value _q 1 to q ₆ position measurements τ ₁ ~τ ₆ and joint load torque (S6). The control unit 300 determines whether or not the drive has been completed (S7), and if not (S7: No), repeats steps S2 to S6. When the drive is completed (S7: Yes), the control unit 500 ends the control process. By driving the motor ₂₁ 1 to 21 ₆ according to the flowchart described above, it is possible to control the force F exerted on the hand 252 of the manipulator 200 to the desired target force _{F d.} The order of the flowcharts shown in FIG. 5 is not limited, and force control can be performed in any other order.

Next, the position control of the manipulator 200 in the position control mode, that is, the position control of the hand 252 of the manipulator 200 will be described. FIG. 6 is a flowchart of a process in which the control unit 500 controls the position of the manipulator 200 in the first embodiment. First, the position command value generation unit 505 acquires the target position P _d from the external storage device 322 (S11). The target position P _d is a target position for positioning the manipulator 200. Position command value generation section 505, based on the target position _{P d,} and calculates the position command value _q ref1 _{to q Ref6} joints _J 1 through J ₆ by using a robot model 503 (S12). Position control unit ₅₂ 1 to 52 _6, the PID calculation based on the difference between the measured values _q 1 to q ₆ position of the position command value _q ref1 _{to q Ref6} and joint, and calculates the torque command value τ _q1 ~τ _q6 (S13). In step S13, the measured values θ _{1 to} θ ₆ may be used instead of the measured values q _{1 to} q ₆ . In this case, the position command value _q ref1 _{to q Ref6} may be converted into a position command value theta _ref1 through? _Ref6 at the speed reduction ratio of the reduction gear ₂₂ 1-22 _6. Switching unit ₅₃ 1 to 53 _6, outputs the motor control unit ₅₄ 1 to 54 ₆ a torque command value τ _q1 ~τ _q6 as the torque command value τ _M1 ~τ _M6. The motor control unit ₅₄ 1 to 54 _6, the motor ₂₁ 1 to 21 ₆ for controlling energization according to the torque command value τ _M1 ~τ _M6 (S14). Motor ₂₁ 1 to 21 ₆ generates an output torque to the joint _J 1 through J ₆ as they are energized (S15). The control units 300 and 400 acquire the measured values q _{1 to} q ₆ of the joint position (S16). The control unit 300 determines whether or not the drive has been completed (S17), and if not (S17: No), repeats steps S12 to S16. When the drive is completed (S17: Yes), the control unit 500 ends the control process. By driving the motor ₂₁ 1 to 21 ₆ according to the flowchart described above, it is possible to control the position of the hand 252 of the manipulator 200 in a desired target position _{P d.} The order of the flowchart shown in FIG. 6 is not limited, and the position can be controlled in any other order.

By the way, the robot model 503, which is a mechanical model of the manipulator 200, does not always match the actual machine, and an error occurs. This error is called the modeling error. This modeling error also changes due to aged deterioration of the manipulator 200 and the like. Further, the measurement value τ ₁ ~τ ₆ of the torque sensor ₂₄ 1-24 _6, the influence of changes in the environment (e.g., temperature) that the manipulator is placed, the offset error is superimposed. If these errors are large, the behavior of the manipulator may become unstable, such as the manipulator vibrating during force control of the manipulator. It is conceivable to obtain the modeling error and the offset error and calibrate the model and the measured value based on these errors, but the calibration work must be performed frequently. When the manipulator 200 is allowed to perform the work in continuous operation, the cycle time increases by the amount of the calibration work time.

Therefore, in the present embodiment, the modeling error and the offset error are canceled in the control in which the manipulator 200 is operated in the production work such as the assembly work. FIG. 7 is a block diagram showing a part of the processing of the first control unit according to the first embodiment. FIG. 7 illustrates the processing of the torque command value generation unit 504. The torque command value generation unit 504 functions as an impedance control unit 601, a dynamics calculation unit 602, a command value correction unit 603, and a correction value calculation unit 604. For example, the RAM 303 shown in FIG. 2 functions as a correction value storage unit 605. The functions of each part will be described below.

Dynamic calculation unit 602, when the control unit 400 is controlling the position of the manipulator 200, taking measurements _q 1 to q ₆ position of the joint _J 1 through J ₆ is the measurement of the position of the manipulator 200 To do. In the present embodiment, the dynamics calculation unit 602 sets the position at which the force control is started as the target position P _d, and acquires the measured values q _{1 to} q ₆ when the manipulator 200 is positioned at the target position P _d . The dynamics calculation unit 602 differentiates these acquired position information to obtain the velocity and acceleration. Dynamic calculation unit 602, the position of the robot model 503 and joint _J 1 through J _6, velocity, and based on the acceleration information, the Newton-Euler method, a known dynamic calculation method, the joint _J 1 through J ₆ Calculate the calculated values τ ^ _{1 to} τ ^ ₆ of the load torque applied to. That is, when the dynamics calculation unit 602 controls the position of the manipulator 200, the calculated value of the load torque as a load generated in the manipulator 200 by the dynamics calculation using the robot model 503 τ ^ _{1 to} τ. ^ ₆ is calculated. The calculated values τ ^ _{1 to} τ ^ ₆ include a modeling error. The calculation of dynamics is not limited to this, and for example, a derivation method by formulating Lagrange may be used.

The correction value calculation unit 604 acquires the measured values τ _{1 to} τ ₆ of the load torque as the first measured value of the load generated on the manipulator 200 when the control unit 400 controls the position of the manipulator 200. In the present embodiment, the correction value calculation unit 604 acquires the measured values τ _{1 to} τ ₆ when the manipulator 200 is positioned at the target position P _d . The measured values τ _{1 to} τ ₆ include an offset error. Correction value calculation unit 604 calculates a correction value _{τ comp1} ~τ comp6 based on the difference between the measured values τ ₁ ~τ ₆ and calculated values τ _^ 1 ~τ _{^ 6.} Specifically, the correction value calculating unit 604, immediately before switching from the position control mode to a force control mode, by subtracting the measured value τ ₁ ~τ ₆ from the calculated value τ ^ ₁ ~τ ^ _6, the correction value tau _Obtain comp1 to τ _comp6 . For example, the correction value calculation unit 604 performs a process of subtracting the measured values τ _{1 to} τ ₆ from the calculated values τ ^ _{1 to} τ ^ ₆ in the position control mode at a predetermined cycle, and changes from the position control mode to the force control mode. when switched to the subtraction value immediately before the correction value _{τ comp1} ~τ comp6. The correction value calculation unit 604 stores the correction values τ _{comp1 to} τ comp ₆ in the correction value storage unit 605. Incidentally, the correction value _{τ comp1} ~τ comp6 is included modeling and offset errors.

Impedance control unit 601, so as to have the desired mechanical impedance to the hand 252 of the manipulator 200, the torque command value τ _Fd1 ~τ _Fd6 joints _J 1 through J ₆ based on the robot model 503, according to the following equation (1) calculate. Equation (1) is an impedance control equation. The mechanical impedance is, for example, a rigid impedance, a viscous impedance, and a force impedance. Torque command value τ _Fd1 ~τ _Fd6 is the command value of the temporary before correction, contains modeling error.

In equation (1), M is a 6 × 6 inertial matrix, C is a 6 × 1 Coriolis / centrifugal force vector, and G is a 6 × 1 gravitational load torque vector. K _d represents the value of the rigidity of the desired mechanical impedance, and D _d represents the value of the viscosity of the desired mechanical impedance, which are 6 × 6 matrices, respectively. Jac is rheumatoid _J 1 through J and speed _6, the coordinate system To (Fig. 1) standards Jacobian 6 × 6 correlating the speed of the hand 252 is expressed. The subscript T of the Jacobian matrix Jac represents the transpose of the matrix.

As described above, when the control unit 400 controls the thrust of the manipulator 200, the impedance control unit 601 uses the equation (1) to obtain a torque command value τ _Fd1 which is a provisional command value based on the teaching data 501 and 502. Obtain _~ τ _Fd6 . Dynamic calculation section 602, M, C, and G, and the robot model 503, based on the position and velocity of the joint _J 1 through J _6, calculated by Newton-Euler method.

Command value correcting unit 603 corrects the torque command value τ _Fd1 ~τ _Fd6 correction value _{τ comp1} ~τ comp6, torque control unit ₅₁ 1 to 51 ₆ torque command value tau _ref1 is an instruction value to be outputted to ～Tau _Ref6 Ask for. Specifically, the command value correcting section 603 is subtracted from the correction value torque command value τ _Fd1 ~τ _Fd6 the correction value _{τ comp1} ~τ comp6 stored in the storage unit 605 is calculated by the impedance control unit 601 , Torque command values τ _{ref1 to} τ _ref6 are generated. As a result, the modeling error, which is the difference between the actual manipulator 200 and the model, is canceled.

The torque command value _{τ ref1} ~τ ref6, contains the offset error. Torque control unit ₅₁ 1 to 51 ₆ shown in FIG. 4, when the control unit 400 controls the thrust of the manipulator 200, the load torque of the joint _J 1 through J ₆ is a second measure of the load occurring in the manipulator 200 Obtain the measured values τ _{1 to} τ ₆ . The measured values τ _{1 to} τ ₆ include an offset error. Torque control unit ₅₁ 1 to 51 ₆ obtains a difference between the torque command value _{τ ref1} ~τ ref6 and measured values τ ₁ ~τ _6, on the basis of this difference, the torque command value τ _F1 ~τ _F6 is a feedback value Ask. In this way, the offset error is canceled at the torque command values τ _{F1 to} τ _F6 used to control the thrust of the manipulator 200.

As described above, according to the present embodiment, since both the modeling error and the offset error are canceled during the control of the thrust of the manipulator 200, the control of the manipulator 200 is stable and the controllability of the manipulator 200 is improved.

The experimental results of the control of the manipulator 200 will be described below. FIG. 8 is a schematic view of the robot arm 251 according to the first embodiment. In Figure _8, T 1 through T ₆ is a link ₂₁₀ 0-210 ₆ fixed coordinate system. The rotation axis of each joint J _{1 to} J ₆ is the Z axis, and the rotation direction is the right hand direction. The posture of the robot arm 251 shown in FIG. 8 is the posture when the measured values q _{1 to} q ₆ of the angles of the joints J _{1 to} J ₆ are 0 degrees. Also, the size of the link ₂₁₀ 0-210 ₆ is as shown in FIG.

FIG. 9A is a diagram showing an example of the dynamic parameters of the actual manipulator 200 in the first embodiment. In FIG. 9A, m ^ is the actual mass of the link. s ^ _x , s ^ _y , and s ^ _z are the actual center-of-gravity positions in the X-axis, Y-axis, and Z-axis directions as viewed from the coordinate origin of the link, respectively. I ^ _xx , I ^ _yy , and I ^ _zz are the actual moments of inertia about the X-axis, Y-axis, and Z-axis around the center of gravity of the link, respectively.

FIG. 9B is a diagram showing an example of the dynamic parameters of the robot model 503 in the first embodiment. The robot model 503 is calculated in advance by CAD from the design value of the robot arm 251 and stored in the external storage device 322. In FIG. 9B, m is the design mass of the link. s _x , _sy , and s _z are the design center-of-gravity positions in the X-axis, Y-axis, and Z-axis directions as viewed from the coordinate origin of the link, respectively. I _xx , I _yy , and I _zz are design moments of inertia with respect to the X-axis, Y-axis, and Z-axis around the center of gravity of the link, respectively. In the present embodiment, the mass of each link of the robot model 503, and that there is 10% of the error of each link 210 _{1-210 6} mass reality.

10 (a) and 10 (b) are schematic views of the manipulator 200 for explaining the assembly work by the manipulator 200 in the first embodiment. In the present embodiment, a case where the work W1 gripped by the hand 252 is assembled to the work W2 will be described. The work W1 is a part having a cylindrical convex portion, and the work W2 is a part having a cylindrical concave portion. In FIG. 10A, P1 is a teaching point at the start of force control indicated by the coordinate system Te of the hand 252 of the manipulator 200, and P2 is a target position of force control indicated by the coordinate system Te of the hand 252 of the manipulator 200. It is a teaching point showing.

FIG. 10B is an enlarged view for explaining the positional relationship between the work W1 and the work W2 when the coordinate system Te of the hand 252 of the manipulator 200 is at the teaching point P1. The outlet WA of the work W2 is chamfered. When the work W1 advances 20 mm in the X-axis direction of the coordinate system To by force control, if the work W1 is inside the outlet WA, the work W1 follows the inner surface of the work W2 and is assembled due to the flexibility of the force control. Is completed. The fact that the work W1 is inside the opening WA means that the central axis of the work W1 is within the range of 195 mm to 205 mm in the Z-axis direction of the coordinate system To. At this time, it is assumed that the inclination of the work W1 and the movement of the coordinate system To in the Y-axis direction do not affect the assembly work.

FIG. 11A is a diagram showing parameters of teaching points in the first embodiment. FIG. 11B is a diagram showing parameters of mechanical impedance of force control in the first embodiment. FIG. 11A shows the values of the position and the posture with respect to the coordinate system To of the teaching points P1 and P2. Note that α, β, and γ in FIG. 11A represent the amounts of rotation around the Z-axis, Y-axis, and X-axis, respectively. Further, FIG. 11B shows the values of the rigidity K _d , the viscosity _{D d} , and the force F _d as the parameters of the mechanical impedance.

FIG. 12 is a flowchart illustrating a method for manufacturing an article according to the first embodiment. First, the control unit 500 starts the position control of the manipulator 200, and moves the hand 252 (coordinate system Te) of the manipulator 200 to the teaching point P1 which is the position where the force control is started (S51).

Controller 500, the position of the robot model 503 and joint _J 1 through J _6, velocity, and based on the acceleration information, the calculated values τ _^ 1 ~τ _{^ 6} of the current load torque of the joint _J 1 through J ₆ Calculate (S52). At this time, the manipulator 200 does not necessarily have to be stopped.

Control unit 500 is a first measurement, taking measurements τ ₁ ~τ ₆ of the load torque of the joint _J 1 ~J ₆ (S53). At this time, the measured value τ ₁ ~τ ₆ is includes an offset error of the torque sensor ₂₄ 1-24 _6, for example, to measure tau ₂ at measurements tau ₁ and joint _{J 2} in the joints _{J 1} of the manipulator 200, It is assumed that there is an offset error of −1.0 Nm.

Control unit 500 subtracts the measured values τ ₁ ~τ ₆ from the calculated value τ _^ 1 ~τ _{^ 6,} calculates the correction value _{τ comp1} ~τ comp6 (S54). At this time, the calculated torque correction values τ _{comp1 to} τ comp ₆ are stored in the correction value storage unit 605.

The control unit 500 switches from the position control mode to the force control mode and starts force control (S55). Controller 500, the desired mechanical impedance to the hand 252 of the manipulator 200 (stiffness _{K d,} viscosity _{D d,} the force _{F d)} a torque command value τ _Fd1 ~τ _Fd6 joints J1~J6 as to have the formula (1 ) (S56).

Controller 500 calculates a torque command value _{τ ref1} ~τ ref6 by subtracting the correction value _{τ comp1} ~τ comp6 stored from the torque command value τ _Fd1 ~τ _Fd6 calculated in step S56 in the correction value storage unit 605 (S57). Controller 500, a torque command value _{τ ref1} ~τ ref6, the PID calculation based on the difference between the measured values τ ₁ ~τ ₆ is a second measured value, calculates a torque command value τ _F1 ~τ _F6 (S58 ).

Control unit 500, a torque command value τ _F1 ~τ _F6 and a torque command value τ _M1 ~τ _M6, the motor ₂₁ 1 to 21 ₆ for controlling energization according to the torque command value τ _M1 ~τ _M6 (S59). Motor ₂₁ 1 to 21 ₆ generates an output torque to the joint _J 1 through J ₆ as they are energized (S60). Control unit 500 obtains the measured values _q 1 to q ₆ position measurements τ ₁ ~τ ₆ and joint load torque (S61). The control unit 500 determines whether or not the drive has been completed (S62), and if not (S62: No), repeats steps S56 to S62. When the control unit 500 determines that the drive has ended (S62: Yes), the control unit 500 ends the control process. In the present embodiment, the control process ends when 5 seconds have elapsed from the start of the force control in step S55.

13 (a), 13 (b) and 13 (c) are graphs showing the experimental results. FIG. 13A is a graph showing the position of the hand 252 in the X-axis direction with respect to the time when the force is controlled in the first embodiment.

As shown in FIG. 13A, the target position P _d of the impedance control is a position moved 50 mm in the X-axis direction from the teaching point P1 in the coordinate system To. It was assumed that the impedance was linearly changed by 50 mm in the X-axis direction for 3 seconds from the start of impedance control.

The graph of FIG. 13B shows the position change of the coordinate system Te of the hand 252 of the manipulator 200 when the manipulator 200 is force-controlled in the first embodiment. The horizontal axis is the position of the coordinate system Te in the coordinate system To in the X-axis direction. The vertical axis is the position of the coordinate system Te in the coordinate system To in the Z-axis direction. The position change due to the contact between the work W1 and the work W2 shall not be considered.

As shown in FIG. 13B, the coordinate system Te is the range in which the work W1 enters the jump port WA, that is, when the coordinate system Te reaches the position 270 mm in the X-axis direction of the coordinate system To, the coordinate system To The position in the Z-axis direction is within the range of 195 mm to 205 mm. Therefore, the assembly of the work W1 to the work W2 is successful.

On the other hand, if not corrected torque command value τ _F1 ~τ _F6 in step S57, the words, and comparative example where the torque command value τ _F1 ~τ _F6 is the torque command value _{τ ref1} ~τ ref6. The graph of FIG. 13C shows the position change of the coordinate system Te of the hand 252 of the manipulator 200 when the manipulator 200 is force-controlled in the comparative example.

As shown in FIG. 13 (c), when the coordinate system Te reaches the position 270 mm in the X-axis direction of the coordinate system To, it is below the position 185 mm in the Z-axis direction of the coordinate system To and can be assembled at the position 195 mm. It deviates from the range of ~ 205 m by 10 mm or more. Therefore, the assembly of the work W1 to the work W2 fails.

Thus, in the present embodiment, even in unknown difference between the actual and model of the manipulator 200, by correcting a torque command value τ _Fd1 ~τ _Fd6 correction value _{τ comp1} ~τ comp6, modeling error Can be canceled. Even unknown offset error included in the measured values τ ₁ ~τ ₆ of the load torque, the feedback calculation, determining the difference between the measured values τ ₁ ~τ ₆ and the torque command value _{τ ref1} ~τ ref6 With, the offset error can be canceled. Therefore, it is possible to prevent a sudden change in the torque of the joint _J 1 through J _6.

Further, since the robot model 503 does not need to be strictly close to the actual manipulator 200, it is possible to reduce the labor and time required for preparation such as estimation of the robot model 503. Further, the offset error of the torque sensor 24 _{1-24 6} due to environmental changes or aging also eliminates the need to examine in advance, it is possible to also reduce time-consuming and time calibration work. It is possible to prevent deterioration of control performance and unexpected operation due to modeling error and sensor offset error for each force control operation without having to perform a predetermined calibration operation in advance.

[Second Embodiment]
The robot device according to the second embodiment will be described. FIG. 14 is a perspective view showing a schematic configuration of the robot device according to the second embodiment. In the robot device 100A of the second embodiment, the same components as those of the robot device 100 of the first embodiment are designated by the same reference numerals, and detailed description thereof will be omitted. As shown in FIG. 14, the robot device 100A includes a manipulator 200 having the same configuration as that of the first embodiment, and a control device 1000A having a control unit 500A for controlling the position or thrust of the manipulator 200. Further, the robot device 100A includes a teaching pendant 600 as a teaching device for transmitting teaching data to the control device 1000A.

The control unit 500A includes a control unit 300A which is a first control unit and a control unit 400A which is a second control unit. The control unit 300A comprehensively controls the robot device 100A. The control unit 400A controls the position or thrust of the manipulator 200 based on a command from the control unit 300A. The control unit 400A is a servo controller for controlling the respective servo mechanisms ₂₀ 1 to 20 ₆ of the manipulator 200.

The control unit 500A will be specifically described. 15 and 16 are block diagrams showing the configuration of the control unit 500A according to the second embodiment. As described above, the control unit 500A has control units 300A and 400A. FIG. 15 shows a detailed configuration of the control unit 300A, and FIG. 16 illustrates a detailed configuration of the control unit 400A.

The control unit 300A is composed of a computer. The hardware of the control unit 300A is the same as that of the first embodiment, and includes a CPU 301, a ROM 302, a RAM 303, an HDD 304, a recording disk drive 305, and interfaces 306 to 309. The CPU 301, ROM 302, RAM 303, HDD 304, recording disk drive 305, and interfaces 306 to 309 are connected by a bus 310 so as to be able to communicate with each other. The ROM 302 stores a program 330A for causing the CPU 301 to execute the processing of the control method.

In the second embodiment, the recording medium that can be read by the computer is the ROM 302, and the program 330A is recorded in the ROM 302, but the present invention is not limited to this. Program 330A may be recorded on any recording medium that can be read by a computer. As the recording medium for supplying the program 330A, for example, a flexible disk, a hard disk, an optical disk, a magneto-optical disk, a magnetic tape, a non-volatile memory, or the like can be used.

The control unit 400A shown in FIG. 16 is composed of a computer. The hardware of the control unit 400A is the same as that of the first embodiment, and includes a CPU 401, a ROM 402, a RAM 403, and interfaces 406 to 409. The CPU 401, ROM 402, RAM 403, and interfaces 406 to 409 are connected by a bus 410 so as to be able to communicate with each other. The ROM 402 stores a program 430A for causing the CPU 401 to execute the processing of the control method.

The interface 406, the motor ₂₁ 1 to 21 ₆ are connected. The interface 407, the position sensor ₂₃ 1-23 ₆ is connected. The interface 408, the torque sensor ₂₄ 1-24 ₆ is connected. A control unit 300A is connected to the interface 409.

In the second embodiment, the recording medium that can be read by a computer is the ROM 402, and the program 430A is recorded in the ROM 402, but the present invention is not limited to this. The program 430A may be recorded on any recording medium as long as it can be read by a computer. As the recording medium for supplying the program 430A, for example, a flexible disk, a hard disk, an optical disk, a magneto-optical disk, a magnetic tape, a non-volatile memory, or the like can be used.

FIG. 17 is a control block diagram showing a control system of the robot device according to the second embodiment. The CPU 301 of the control unit 300A shown in FIG. 15 functions as the torque command value generation unit 504A and the position command value generation unit 505 shown in FIG. 17 by executing the program 330A. CPU401 of the control unit 400A shown in FIG. 16, by executing a program 430A, the torque control unit _51A 1 _{~51A 6} shown in FIG. 17, the position control unit ₅₂ 1 to 52 _6, the switching unit ₅₃ 1 to 53 ₆ and, functions as a motor control unit ₅₄ 1 to 54 _6. Servomechanism units ₂₀ 1 to 20 ₆ has a motor ₂₁ 1 to 21 _6, reducer ₂₂ 1-22 _6, the position sensor ₂₃ 1-23 ₆ and the torque sensor ₂₄ 1-24 _6.

Torque command value generating unit 504A, the robot model 503, the target force _{F d,} the target position _{P d,} the measured values _q 1 to q ₆ position of the joint _J 1 through J _6, the measured value of the torque of the joint _J 1 through J ₆ τ _{1 to} τ ₆ are input. The torque command value generating unit 504A, using these values, to obtain a torque command value _{τ ref1} ~τ ref6 for rheumatoid _J 1 through J _6. Torque command value generating unit 504A includes a torque command value _{τ ref1} ~τ ref6 obtained, and outputs the torque control unit _51A 1 _{~51A 6.}

Torque control unit _51A 1 _{~51A 6} is such that the difference between the torque command value _{τ ref1} ~τ ref6 joints _J 1 through J _6, and the measured values τ ₁ ~τ ₆ torque joints _J 1 through J ₆ is reduced to obtain the torque command value τ _F1 ~τ _F6, and outputs to the switching unit ₅₃ 1 to 53 _6. Details of the torque control units 51A _{1 to} 51A ₆ will be described later. The position control unit ₅₂ 1 to 52 _6, the switching unit ₅₃ 1 to 53 _6, the motor control unit ₅₄ 1 to 54 ₆ performs the same processing as in the first embodiment, the detailed description thereof is omitted.

Above, the control unit 400A, when acquiring the position command value _q ref1 _{to q Ref6} from the control unit 300A, on the basis of the difference between the position command value _q ref1 _{to q Ref6} and measured values _q 1 to q _6, the position of the manipulator 200 To control. The control unit 400A, when the control unit 300A obtains the torque command value _{τ ref1} ~τ ref6, based on the difference between the measured values τ ₁ ~τ ₆ and the torque command value _{τ ref1} ~τ ref6, thrust manipulator 200, i.e. controlling the output torque of the joint _J 1 through J _6.

By the way, when performing precise assembly work with a manipulator, the manipulator is moved to a predetermined position where assembly is started in a position control mode in which the position of the manipulator is controlled. After that, the manipulator may be forced to perform precision assembly after switching to the force control mode in which the manipulator is force-controlled. When switching the control mode from the position control mode to the force control mode, if the torque command value to the motor that drives the joint of the manipulator changes suddenly, the joint will move steeply, and as a result, the hand of the manipulator will vibrate. May become a target. This is because the feedback value is once reset to zero when switching from the position control mode to the force control mode.

FIG. 18 is a block diagram showing a part of the processing of the second control unit according to the second embodiment. FIG. 18 illustrates the processing of the torque control units 51A _{1 to} 51A ₆ . Torque control unit _51A 1 _{~51A 6} mainly includes a feedback control unit ₆₀ 1 to 60 _6, the feed forward calculation unit ₆₂ 1 to 62 _6, and functions as a summing unit ₆₃ 1 to 63 _6. Furthermore, RAM 403 shown in FIG. 16 functions as a first storage unit ₆₁ 1 to 61 ₆ and the second storage unit ₇₁ 1 to 71 _6. The functions of each part will be described below.

First storage unit ₆₁ 1 to 61 _6, in the position control mode before switching to a force control mode, a first measurement value, the measurement value tau ₁ ~ load torque measured using the torque sensor ₂₄ 1-24 ₆ the tau _6, is stored as an offset value _{τ offset1} ~τ offset6. That is, the torque control unit _51A 1 ～51A _6, the offset value τ _offset1 ~τ based on the first measurement value of the load occurring in the manipulator 200 when the position control unit ₅₂ 1 to 52 ₆ is controlling the position of the manipulator 200 _{Find offset6} . Torque control unit _51A 1 ～51A ₆ in the first storage section ₆₁ 1-61 ₆ and allowed to store the offset value _{τ offset1} ~τ offset6 acquired for position control. Offset value _{τ offset1} ~τ offset6 stored in the first storage unit ₆₁ 1 to 61 _6, after switching from the position control mode to a force control mode, it is added to the feedback value τ _FB1 ~τ _FB6.

In this embodiment, a trigger that the position of the manipulator 200 has reached the target position, the measured value τ ₁ ~τ ₆ at the time the offset value _{τ offset1} ~τ offset6. The offset value _{τ offset1} ~τ offset6 during position control, may be updated from time to time at a predetermined control cycle. In this case, when switching from the position control mode to a force control mode, so that the last updated value is held in the first storage unit 61 ₁ to 61 _6.

Feedback control unit ₆₀ 1 to 60 _6, in a force control mode, at a predetermined control period, a torque command value _{τ ref1} ~τ ref6 from the torque command value generating unit 504A, was measured using the torque sensor ₂₄ 1-24 ₆ The measured values τ _{1 to} τ _{6 of the} load torque are acquired. Feedback control unit ₆₀ 1 to 60 _6, a torque command value _{τ ref1} ~τ ref6, based on the difference between the measured values τ ₁ ~τ _6, calculates the feedback value τ _FB1 ~τ _FB6. Feedback control unit ₆₀ 1 to 60 ₆ is performed as an example of the feedback calculation, proportional operation, integral operation, and a PID operation consisting of a differentiation operation.

Feedforward calculation unit ₆₂ 1 to 62 _6, at the time of the force control, obtains the feedforward value τ _FF1 ~τ _FF6 corresponding to the torque command value _{τ ref1} ~τ ref6. Adding unit ₆₃ 1 to 63 _6, the feedback value τ _FB1 ~τ _FB6, the offset value _{τ offset1} ~τ offset6, adds the feedforward value τ _FF1 ~τ _FF6, and its value, the torque command value tau _F1 ~ and outputs to the switching unit ₅₃ 1 to 53 ₆ as tau _F6.

Be described by way of a specific example process of the feedforward calculation unit ₆₂ 1 to 62 _6. Here, among the torque command value _{τ ref1} ~τ ref6 the torque control unit _51A 1 _{~51A 6} receives an input command value to receive the first input when the force control (second command value), torque command value .tau.0 _ref1 ~ _Let τ0 _ref6 . That is, the torque command value generation unit 504A first obtains the torque command values τ0 _{ref1 to} τ0 _ref6 when the control of the thrust of the manipulator 200 is started. The torque control unit _51A 1 _{~51A 6} is a torque command value τ0 _ref1 ~τ0 command value receiving input after the _Ref6 (first command value) torque command value τ1 _ref1 ~τ1 _ref6. That is, the torque command value generating unit 504A calculates a torque command value _{τ1 ref1} ~τ1 ref6 after the torque command value τ0 _ref1 ~τ0 _ref6. Thus, the torque command value _{τ0 ref1} ~τ0 ref6, in the torque control unit _51A 1 _{~51A 6,} and the torque command value receiving input before the torque command value τ1 _ref1 ~τ1 _ref6.

Feedforward calculation unit ₆₂ 1 to 62 _6, in this embodiment, includes a model torque calculating section ₇₀ 1-70 _6, a subtraction unit ₇₃ 1 to 73 _6. The first control cycle for force control will be described. Model torque calculating section ₇₀ 1-70 _6, the calculation of the kinetics using a robot model 503, corresponding to the torque command value _{τ0 ref1} ~τ0 ref6, the second calculated value of the load torque is a load generated in the manipulator 200 finding a certain calculated value τ0 _FF1 ~τ0 _FF6. The robot model 503 includes parameters such as load inertia _{J M} and the viscosity coefficient _{D M,} and the reduction gear ₂₂ 1-22 ₆ stiffness value K and the reduction ratio N of the motor ₂₁ 1 to 21 _6. In the present embodiment, the model torque calculation unit ₇₀ 1 to 70 _6, the torque command value _{τ0 ref1} ~τ0 ref6, based parameter _{J M,} D M included in the robot model 503, K, and N, the calculated values τ0 _{FF1 to} τ0 FF6 are _{obtained according} to the following equation (2). s is a Laplace operator.

The second storage unit ₇₁ 1 to 71 _6, the calculated value τ0 _FF1 ~τ0 _FF6, is stored as a reference value τ0 _FF1 ~τ0 _FF6 is an offset value. The second storage unit ₇₁ 1 to 71 _6, while the force control is continued, for holding the calculated value τ0 _FF1 ~τ0 _FF6 as a reference value τ0 _FF1 ~τ0 _FF6. When switching from the force control mode to position control mode, the second storage unit 71 ₁ to 71 ₆ is reset.

Subtraction unit ₇₃ 1 to 73 _6, a value obtained by subtracting the calculated value τ0 _FF1 ~τ0 _FF6 reference value τ0 _FF1 ~τ0 _FF6 from the feedforward value τ _FF1 ~τ _FF6, and outputs the result to adding section ₆₃ 1-63 ₆ .. Therefore, in the present embodiment, the initial value of the feedforward value τ _FF1 ~τ _FF6 is zero.

Next, the following control cycle after the first control cycle will be described. Model torque calculating section ₇₀ 1-70 _6, the calculation of the kinetics using a robot model 503, corresponding to the torque command value _{τ1 ref1} ~τ1 ref6, the first calculated value of the load torque is a load generated in the manipulator 200 finding a certain calculated value τ1 _FF1 ~τ1 _FF6. In the present embodiment, the model torque calculation unit ₇₀ 1 to 70 _6, a torque command value _{τ1 ref1} ~τ1 ref6, based parameter _{J M,} D M included in the robot model 503, K, and N, the calculated values τ1 _{FF1 to} τ1 FF6 are _{obtained according} to the following equation (3). Subtraction unit ₇₃ 1 to 73 _6, a value obtained by subtracting the calculated value τ1 _FF1 ~τ1 _FF6 reference value τ0 _FF1 ~τ0 _FF6 from the feedforward value τ _FF1 ~τ _FF6, and outputs the result to adding section ₆₃ 1-63 ₆ ..

That is, in the joint _J 1 through J ₆ manipulator 200, the torsion amount of variation of the load applied is changed, the hand 252 is vibrated. In the present embodiment, with reference to the load at the time of starting the power control, load fluctuation (difference between two calculated) by the feed-forward value τ _FF1 ~τ _FF6, enhance the responsiveness of the force control As a result, the vibration of the hand 252 is suppressed.

In the second embodiment, the reference value stored in the second storage unit ₇₁ 1 to 71 _6, and starts controlling the thrust of the manipulator 200 Calculated an initial value is first determined τ0 _FF1 ~τ0 _FF6 is preferable, but the present invention is not limited to this. For example, the value next to the initial value may be used. That is, the reference value, prior to the calculated value τ1 _FF1 ~τ1 _FF6 there, than the torque command value τ1 _FF1 ~τ1 _FF6 obtained in accordance with the previous torque command value, the calculation value of the load occurring in the manipulator 200 All you need is.

FIG. 19 is a flowchart illustrating a method of manufacturing an article according to a second embodiment. In the second embodiment, as in the first embodiment, as shown in FIG. 10A, the work W1 is held by the hand 252 and the work W1 is assembled to the work W2 to manufacture the article. To do.

First, the control unit 500A starts the position control of the manipulator 200 (S71), and drives the manipulator 200 by the position control until it reaches the position where the force control is started, that is, the teaching point P1 shown in FIG. 10A. (S72).

It is determined whether or not the manipulator 200 has been driven to a position where force control is started (S73). When the manipulator 200 moves to that position (S73: Yes), the control unit 500A acquires the measured torque values τ _{1 to} τ ₆ at that position. Control unit 500A in the first storage section ₆₁ 1-61 ₆ shown in FIG. 18, and stores the measured value tau ₁ ～Tau ₆ as the offset value _{τ offset1} ~τ offset6 (S74).

Next, the control unit 500A switches the control mode for controlling the manipulator 200 from the position control mode to the force control mode (S75), and starts the force control of the manipulator 200 (S76). After force control start, the control unit 500A is input with a torque command value _{τ0 ref1} ~τ0 ref6, joints _J 1 calculated value of the load torque in ~J ₆ τ0 _FF1 ~τ0 _FF6 second storage unit ₇₁ 1 as a reference value a 71 ₆ to be stored in (S77).

Control unit 500A includes a feedback value τ _FB1 ~τ _FB6, based on the value obtained by adding the offset value _{τ offset1} ~τ offset6 feedforward value τ _FF1 ~τ _FF6, controls the thrust of the manipulator 200. As a result, the control unit 500A is driven so that the force acting on the hand 252 of the manipulator 200 becomes the target force (S78).

The control unit 500A determines whether or not the drive is completed, that is, whether or not the assembly is completed (S79). If the drive is not completed (S79: No), the control unit 500A repeatedly executes the loops of steps S78 and S79 until the assembly is completed. When the control unit 500A determines that the drive is finished (S79: Yes), the control unit 500A ends the control process.

As described above, according to the second embodiment, immediately before the switching from the position control mode to a force control mode, the measured value tau ₁ ～Tau ₆ torque in the first storage unit ₆₁ 1 to 61 _6, the offset value _{τ offset1} ~τ offset6 Is remembered as. After switching to the force control mode, the feedback value τ _FB1 ~τ _FB6, the offset value _{τ offset1} ~τ offset6 is added. As a result, the continuity of the torque command values τ _{M1 to} τ _M6 is compensated when the control is switched from the position control mode to the force control mode. Since the continuity of the motor torque command values τ _{M1 to} τ _M6 is compensated, it is prevented that the currents Cur _{1 to} Cur ₆ supplied to the motor fluctuate sharply. Therefore, steep movements can be prevented at each joint of the manipulator 200, and the hand 252 of the manipulator 200 can be prevented from vibrating. In particular, when performing precise assembly work using the manipulator 200, vibration of the hand 252 of the manipulator 200 is prevented, so that it is possible to prevent the assembled work or the work to be assembled from being deformed or destroyed. In addition, assembly defects can be reduced, retry operations are reduced, and the productivity of articles is improved.

Further, according to the second embodiment, the feedback value τ _FB1 ~τ _FB6, since the feedforward value τ _FF1 ~τ _FF6 is added, the responsiveness of the control of the joint _J 1 through J ₆ in the manipulator 200 is increased .. The feedforward value τ _FF1 ~τ _FF6 from calculated values τ1 _FF1 ~τ1 _FF6 of the load torque, the reference value τ0 _FF1 ~τ0 _FF6 is obtained by being removed as a frequency that is offset, the torque command value tau _It is prevented that _{M1 to} τ _M6 fluctuate sharply. Therefore, it is possible to effectively prevent the hand 252 from vibrating when the control mode is switched. As described above, according to the second embodiment, the controllability of the manipulator 200 when switching from the position control mode to the force control mode is improved.

FIG. 20 is a graph showing the experimental results in the second embodiment. In FIG. 20, the horizontal axis is time and the vertical axis is torque. FIG. 20 illustrates the torque command value τ _M1 of the joint J1 and the measured value τ ₁ of the load torque of the joint J1.

When the control mode is switched, the torque command value τ _M1 switches from the torque command value τ _q1 to the torque command value τ _F1 , but the torque command value τ _M1 switches smoothly without making a sharp change. Therefore, the vibration of the measured torque value τ ₁ after switching the control mode is small. Further, during force control, since the feedforward value τ _FF1 is added to the feedback value τ _{FB 1} , it can be confirmed that the measured value τ ₁ has good responsiveness and follows the torque command value τ _M1 .

From the above, according to the second embodiment, when the control mode is switched from the position control mode to the force control mode, the continuity of the torque command values τ _{M1 to} τ _M6 at the time of switching the control mode is compensated and the force control is performed. The responsiveness in the mode can be improved. This enables highly accurate and highly responsive force control of the manipulator 200 even when the control mode is switched.

[Third Embodiment]
Next, the robot device of the third embodiment will be described. Since the configuration of the robot device is the same as that of the second embodiment, the description thereof will be omitted.

In the second embodiment, the reference value should be stored in the second storage unit ₇₁ 1 to 71 _6, and the initial value of the calculated values in the model torque calculation unit ₇₀ 1 to 70 _6. Immediately after the switching of the control mode, feedforward value τ _FF1 ~τ _FF6 becomes zero. Therefore, in this case, the torque command values τ _{M1 to} τ _M6 become constant values for a short period of time and do not change smoothly.

FIG. 21 is a block diagram showing a part of the processing of the second control unit according to the third embodiment. In the third embodiment, the model torque calculation unit 70 ₁ to 70 _6, the position control executed before the force control execution, carried out in parallel arithmetic processing for obtaining the second calculated value, this value second storage unit 71 advance and stored in _{1-71 6.}

Specifically, the torque command value generating unit 504A, when position control is being executed, before the torque command value _{τ1 ref1} ~τ1 ref6 a first command value when the force control is executed, the obtaining a torque command value _{τ2 ref1} ~τ2 ref6 a second command value. Torque command value generating unit 504A acquires the target force _{F d,} the target position _{P d,} the measured value τ ₁ ~τ _6, and the measured values _q 1 to q ₆ in the position control torque command value _{τ2 ref1} ~τ2 ref6 Is generated based on these acquired values.

Torque command value generating unit 504A outputs when the position control is being executed, the torque command value _{τ2 ref1} ~τ2 ref6 the model torque calculation unit ₇₀ 1 to 70 _6. The model torque calculating section ₇₀ 1-70 ₆ obtains a calculated value τ2 _FF1 ~τ2 _FF6 is a second calculated value of the load torque corresponding to the torque command value _{τ2 ref1} ~τ2 ref6 obtained. The model torque calculating section ₇₀ 1-70 _6, and stores the calculated value τ2 _FF1 ~τ2 _FF6 as a reference value τ2 _FF1 ~τ2 _FF6 in the second storage unit ₇₁ 1 to 71 _6. Incidentally, when the position control, a feedback control unit ₆₀ 1 to 60 _6, the subtraction unit ₇₃ 1 to 73 _6, and the addition unit ₆₃ 1 to 63 ₆ are not performed because there is no need to perform calculation processing.

When performing power control, the feedback control unit ₆₀ 1 to 60 _6, based on a difference between the torque command value _{τ1 ref1} ~τ1 ref6 and measured values τ ₁ ~τ _6, obtains the feedback value τ _FB1 ~τ _FB6. Model torque calculating section ₇₀ 1-70 ₆ obtains a calculated value τ1 _FF1 ~τ1 _FF6 is first calculated value corresponding to the torque command value _{τ1 ref1} ~τ1 ref6. Subtraction unit ₇₃ 1 to 73 _6, from the calculated value τ1 _FF1 ~τ1 _FF6 by subtracting the reference value τ2 _FF1 ~τ2 _FF6, obtaining the feedforward value τ _FF1 ~τ _FF6. Then, the addition section ₆₃ 1-63 _6, the feedback value τ _FB1 ~τ _FB6, the offset value _{τ offset1} ~τ offset6, and by adding the feedforward value τ _FF1 ~τ _FF6, torque command value τ _F1 ~τ _{Find F6} .

As a result, even immediately after switching to the force control mode, the torque command values τ _{F1 to} τ _F6 , that is, the torque command values τ _{M1 to} τ _M6 shown in FIG. 17 change smoothly with time. As a result, it is possible to further reduce the vibration of the hand 252 of the manipulator 200 when the control mode is switched.

FIG. 22 is a graph showing the experimental results in the third embodiment. The graph of FIG. 22 also shows the experimental results of the second embodiment. Figure 22 illustrates a torque command value tau _M1 joints _{J 1.} The time change rate of the torque command value τ _M1 in the first embodiment is constant in the position control mode, but does not change immediately after switching from the position control mode to the force control mode. On the other hand, the time change rate of the torque command value τ _M1 in the second embodiment is constant even immediately after switching from the position control mode to the force control mode.

According to the third embodiment, when the control mode is switched from the position control mode to the force control mode, the torque command values τ _{M1 to} τ _M6 at the time of switching the control mode can be continuously and smoothly made. As a result, the vibration of the hand 252 of the manipulator 200 can be further suppressed when the control mode is switched, and highly accurate and highly responsive force control becomes possible.

[Fourth Embodiment]
The robot device of the fourth embodiment will be described. FIG. 23 is a perspective view showing a schematic configuration of the robot device according to the fourth embodiment. In the robot device 100B of the fourth embodiment, the same components as those of the robot device of the first, second or third embodiment are designated by the same reference numerals, and detailed description thereof will be omitted. As shown in FIG. 23, the robot device 100B includes a manipulator 200 having the same configuration as that of the first embodiment, and a control device 1000B having a control unit 500B for controlling the manipulator 200. Further, the robot device 100B includes a teaching pendant 600 as a teaching device for transmitting teaching data to the control device 1000B.

The control unit 500B has a control unit 300 similar to that of the first embodiment and a control unit 400A similar to that of the second or third embodiment. That is, the control unit 300 performs the processing described in the first embodiment, and the control unit 400A performs the processing described in the second or third embodiment.

The control unit 300 _obtains correction values τ _{comp1 to} τ _comp6 (FIG. 7) when the control unit 400A controls the position of the manipulator 200. Control unit 300, when to control the thrust of the manipulator 200 to the control unit 400A, to obtain a torque command value τ _Fd1 ~τ _Fd6 a command value of the tentative based on force teaching data 501 and the position teaching data 502 is teaching data .. Then, the control unit 300 obtains a torque command value _{τ ref1} ~τ ref6 by correcting the torque command value τ _Fd1 ~τ _Fd6 correction value _{τ comp1} ~τ comp6, torque command value tau _ref1 ~ to the control unit 400A _Outputs τ _ref6 .

On the other hand, the control unit 400A, when controlling the position of the manipulator 200, as shown in FIG. 18, previously obtained offset value _{τ offset1} ~τ offset6. The control unit 400A, at the time of controlling the thrust of the manipulator 200, obtains the feedback value τ _FB1 ~τ _FB6 and feedforward value τ _FF1 ~τ _FF6. Then, the control unit 400A is the feedback value τ _FB1 ~τ _FB6, the offset value _{τ offset1} ~τ offset6, adds the feedforward value τ _FF1 ~τ _FF6, and generates a torque command value τ _F1 ~τ _F6 To do. The control unit 400A controls the thrust of the manipulator 200 based on the torque command values τ _{F1 to} τ _F6 .

As described above, in the fourth embodiment, the control unit 500B executes the control process by combining the control process described in the first embodiment and the control process described in the second or third embodiment. The controllability of the manipulator 200 is further improved.

The present invention is not limited to the embodiments described above, and many modifications can be made within the technical idea of the present invention. Moreover, the effects described in the embodiments are merely a list of the most preferable effects arising from the present invention, and the effects according to the present invention are not limited to those described in the embodiments.

In the above-described embodiment, the case where the robot arm 251 is a vertically articulated robot arm has been described, but the present invention is not limited to this. For example, various robot arms such as a horizontal articulated robot arm, a parallel link robot arm, and a orthogonal robot may be used.

(Other Examples)
The present invention supplies a program that realizes one or more functions of the above-described embodiment to a system or device via a network or storage medium, and one or more processors in the computer of the system or device reads and executes the program. It can also be realized by the processing to be performed. It can also be realized by a circuit (for example, ASIC) that realizes one or more functions.

100 ... Robot device 200 ... Manipulator 300 ... Control unit (first control unit)
400 ... Control unit (second control unit)
500 ... Control unit 1000 ... Control device

Claims (18)

Equipped with a control unit that controls the position or thrust of the manipulator
The control unit
While controlling the position of the manipulator, the first measured value of the load generated on the manipulator is acquired, and the calculated value of the load generated on the manipulator is obtained by the calculation of kinetics using the model data of the manipulator. , The correction value is obtained based on the difference between the first measured value and the calculated value.
When controlling the thrust of the manipulator, a tentative command value based on the teaching data is obtained, the tentative command value is corrected by the correction value to obtain the command value, and the second measured value of the load generated on the manipulator is obtained. The control device is characterized in that the thrust of the manipulator is controlled based on the difference between the command value and the second measured value. The control device according to claim 1, wherein the control unit controls the output torque of the joint of the manipulator as the thrust of the manipulator. The control device according to claim 1 or 2, wherein the control unit acquires the first measured value when the manipulator is positioned at a target position and obtains the correction value. The control device according to any one of claims 1 to 3, wherein the control unit controls the position of the tip end portion of the manipulator as the position of the manipulator. The control unit
The first control unit that outputs the command value and
A claim comprising a second control unit that acquires the command value from the first control unit and controls the thrust of the manipulator based on the difference between the command value and the second measured value. The control device according to any one of 1 to 4. Equipped with a control unit that controls the position or thrust of the manipulator
The control unit
An offset value based on the first measured value of the load generated on the manipulator while controlling the position of the manipulator is obtained.
When controlling the thrust of the manipulator, a command value is obtained based on the teaching data, a second measured value of the load generated on the manipulator is acquired, and feedback is provided based on the difference between the command value and the second measured value. A control device for obtaining a value and controlling the thrust of the manipulator based on a value obtained by adding the offset value and the feed forward value according to the command value to the feedback value. The control unit obtains a calculated value of the load generated on the manipulator according to the command value by a dynamics calculation using the model data of the manipulator, and subtracts a reference value from the calculated value. The control device according to claim 6, wherein the control device has a feed forward value. The calculated value is the first calculated value, the command value is the first command value, and the like.
The reference value is a second calculated value of the load generated in the manipulator, which is obtained in response to the second command value before the first command value before the first calculated value. The control device according to claim 7. The control device according to claim 8, wherein the second command value is obtained when the control of the thrust of the manipulator is started. The control unit
The first control unit that outputs the command value and
The control device according to any one of claims 6 to 9, further comprising a second control unit that acquires the command value from the first control unit and controls the thrust of the manipulator. The first control unit and
A second control unit that controls the position or thrust of the manipulator based on the command of the first control unit is provided.
The first control unit is
When the second control unit controls the position of the manipulator, the first measured value of the load generated on the manipulator is acquired, and the calculation of kinetics using the model data of the manipulator results in the manipulator. The calculated value of the load is obtained, and the correction value is obtained based on the difference between the first measured value and the calculated value.
When the second control unit controls the thrust of the manipulator, a tentative command value based on the teaching data is obtained, and the tentative command value is corrected by the correction value to obtain the command value. The command value is output to the control unit,
The second control unit is
An offset value based on the first measured value is obtained while controlling the position of the manipulator.
When controlling the thrust of the manipulator, the second measured value of the load generated in the manipulator is acquired, the feedback value is obtained based on the difference between the command value and the second measured value, and the feedback value is used as the above. A control device characterized in that the thrust of the manipulator is controlled based on a value obtained by adding an offset value and a feedback value according to the command value. The control device according to any one of claims 1 to 11.
A robot device including the manipulator. The control unit is a control method that controls the position or thrust of the manipulator.
When the control unit controls the position of the manipulator, the first measured value of the load generated on the manipulator is acquired, and the load generated on the manipulator is calculated by kinetic calculation using the model data of the manipulator. The calculated value of is obtained, and the correction value is obtained based on the difference between the measured value and the calculated value.
When the control unit controls the thrust of the manipulator, it obtains a tentative command value based on the teaching data, corrects the tentative command value with the correction value to obtain the command value, and causes a load on the manipulator. A control method characterized in that the thrust of the manipulator is controlled based on the difference between the command value and the second measured value. The control unit is a control method that controls the position or thrust of the manipulator.
The control unit
An offset value based on the first measured value of the load generated on the manipulator while controlling the position of the manipulator is obtained.
When controlling the thrust of the manipulator, a command value is obtained based on the teaching data, a second measured value of the load generated on the manipulator is acquired, and feedback is provided based on the difference between the command value and the second measured value. A control method characterized in that a value is obtained, and the thrust of the manipulator is controlled based on a value obtained by adding the offset value and the feedforward value according to the command value to the feedback value. It is a control method by the first control unit and the second control unit that controls the position or thrust of the manipulator based on the command of the first control unit.
The first control unit
When the second control unit controls the position of the manipulator, the first measured value of the load generated on the manipulator is acquired, and the calculation of kinetics using the model data of the manipulator results in the manipulator. The calculated value of the load is obtained, and the correction value is obtained based on the difference between the first measured value and the calculated value.
When the second control unit controls the thrust of the manipulator, a tentative command value based on the teaching data is obtained, and the tentative command value is corrected by the correction value to obtain the command value. The command value is output to the control unit,
The second control unit
An offset value based on the first measured value is obtained while controlling the position of the manipulator.
When controlling the thrust of the manipulator, the second measured value of the load generated in the manipulator is acquired, the feedback value is obtained based on the difference between the command value and the second measured value, and the feedback value is used as the above. A control method characterized in that the thrust of the manipulator is controlled based on a value obtained by adding an offset value and a feedforward value according to the command value. A method for manufacturing an article in which the manipulator is controlled by the control method according to any one of claims 13 to 15. A program for causing a computer to execute the control method according to any one of claims 13 to 15. A computer-readable recording medium on which the program according to claim 17 is recorded.

Images