Control method, robot system, article manufacturing method, control program, and recording medium

Abstract

To provide a robot performing force control which reduces duration until the robot outputs control force according to sensor information after the robot acquires the sensor information, and achieves force control excellent in responsibility while keeping high stability.SOLUTION: A robot device includes a joint having a drive part, a drive control part for controlling force generated by the drive part, and an operation command part for outputting a command relating to operation of the joint to the drive control part, wherein the operation command part acquires information on a motion state of the robot device, calculates rigid information of the joint and / or an attenuation coefficient of the joint, and outputs the command including the calculation result to the drive control part, and the drive control part controls force generated by the drive part using the command.SELECTED DRAWING: Figure 3

Description

[Technical Field]

[0001] The present invention relates to a robot How the system is controlled etc. [Background technology]

[0002] In recent years, the uses of articulated robot manipulators have expanded, and they are now being applied to fields that require robots to perform flexible movements, such as collaborative work with humans and component assembly work in industrial product assembly. Because such robots require stable, wide-band force control functions to follow external forces, it is desirable to configure a control system based on joint-level torque control (torque servo) instead of a motion control system based on joint position control (position servo).

[0003] The mechanism (torque sensor, force sensor) for measuring the output torque (or translational force, thrust) in such robot joints is generally installed between the output of the drive unit, which consists of a motor and a reduction mechanism, and the drive shaft of the output node, via a bearing that supports the output node so that it can rotate freely.

[0004] Non-Patent Document 1 describes a torque sensor that includes an elastic member that deforms in response to applied torque, and measures output torque by detecting the amount of deformation or strain.

[0005] Patent Document 1 describes a robot having two hierarchically arranged control law calculation units in a motion control system, which is composed of a (1) joint control system and a (2) arm control system. The (1) joint control system is a servo system responsible for controlling the state of the joints, and for example, executes control to adjust the torque output by the joints so that it becomes a value specified by the (2) arm control system, which is a higher-level system. The (2) arm control system is an integrated control unit that controls the motion and / or force of the entire robot manipulator (robot arm) to a desired state. For example, it has the function of controlling each joint in a coordinated manner so that the motion and force of the hand are in a desired state. [Prior art documents] [Non-patent literature]

[0006] [Non-Patent Document 1] G. Hirzinger, A. Albu-Schaeffer, M. Haehnle, I. Schaefer, N. Sporer, “On a New Generation of Torque Controlled Light-Weight Robots”, Proceedings of the 2001 IEEE International Conference on Robotics & Automation, pp. 3356-3363, 2001 [Patent documents]

[0007] [Patent Document 1] Japanese Patent Publication No. 2020-151824 Summary of the Invention [Problem to be solved by the invention]

[0008] In conventional configurations, the torque to be output by a robot joint (joint torque command value) is calculated using a dynamics model. This calculation is performed based on the robot's state, i.e., information on the robot's displacement, velocity, acceleration, etc., which is fed back to the controller, and is therefore performed in the arm control system, which is the overall control unit of the robot device. Note that this calculation includes an inverse dynamics calculation that calculates the torque to be output by the robot joint from the robot's motion state, i.e., the joint displacement, velocity, acceleration, etc.

[0009] The joint torque command value calculated in the arm control system, which is the overall control unit of the robot device, is transmitted to the servo control unit, which is the joint control system. The servo control unit receives the transmitted joint torque command and controls the drive of each joint's motor so that each joint achieves the joint torque command value. More specifically, the servo control unit performs feedback control based on the measured joint torque value and the acquired torque command value so that they match.

[0010] In a robot control device configured as described above, the time required from acquiring sensor information to controlling the output is determined by the update cycle of the joint torque command executed by the arm control system of the overall control unit. In general, arm control systems must perform computations with high computational loads, such as inverse dynamics calculations, as well as control processing of communication interfaces such as communication control between the overall control unit and servo units and communication with external devices of the robot. For this reason, it is difficult to speed up the control cycle simply by improving the machine specifications (such as by increasing the clock frequency of the computing device).

[0011] If the control period becomes long due to the long calculation time, it becomes impossible to increase the control gain of the robot's force control system, making it impossible to achieve good control characteristics with enhanced damping. This results in problems such as poor stability during free movement, difficulty in positioning the robot to a desired position, difficulty in properly controlling the repulsion upon contact, and the need for trial and error when teaching the robot's movements. In some cases, only skilled technicians in robot teaching can properly use the robot in force control mode.

[0012] Therefore, there was a need for technology in force-controlled robots that could reduce the time required from acquiring sensor information to outputting the corresponding control force, thereby achieving force control with excellent responsiveness while maintaining high stability. [Means for solving the problem]

[0013] A first aspect of the present invention is a control method for a robot system including a robot including a joint having a drive unit, an angle sensor that detects the angle of the joint, and a torque sensor that detects the torque of the joint, a first control unit that controls the drive unit, and a second control unit that outputs a command value related to the operation of the drive unit to the first control unit, wherein the second control unit outputs information related to the impedance of the joint to the first control unit, and the first control unit controls the drive unit using the information related to the impedance, a value from the angle sensor, and a value from the torque sensor. death , The second control unit outputs information about the impedance to the first control unit in a first cycle, and the first control unit controls the drive unit in a second cycle that is shorter than the first cycle. The control method is characterized by the above.

[0014] A second aspect of the present invention is a control method for a robot system including a robot including a first joint having a first drive unit and a first angle sensor that detects the angle of the first joint, a second joint having a second drive unit and a second angle sensor that detects the angle of the second joint, a first control unit that controls the first drive unit, and a second control unit that controls the second drive unit, wherein the second control unit outputs a value of the second angle sensor to the first control unit, and the first control unit controls the first drive unit using information related to impedance at the first joint, the value from the first angle sensor, and the value from the second angle sensor. A third aspect of the present invention is a robot system including a robot including a joint having a drive unit, an angle sensor that detects the angle of the joint, and a torque sensor that detects the torque of the joint, a first control unit that controls the drive unit, and a second control unit that outputs a command value related to the operation of the drive unit to the first control unit, wherein the second control unit outputs information related to the impedance of the joint to the first control unit, and the first control unit controls the drive unit using the information related to the impedance, a value from the angle sensor, and a value from the torque sensor. death , The second control unit outputs information about the impedance to the first control unit in a first cycle, and the first control unit controls the drive unit in a second cycle that is shorter than the first cycle. The robot system is characterized by the above. Moreover, a fourth aspect of the present invention is A robot system comprising: a robot including a first joint having a first drive unit and a first angle sensor that detects the angle of the first joint; a second joint having a second drive unit and a second angle sensor that detects the angle of the second joint; a first control unit that controls the first drive unit; and a second control unit that controls the second drive unit, wherein the second control unit outputs a value of the second angle sensor to the first control unit, and the first control unit controls the first drive unit using information related to impedance at the first joint, the value from the first angle sensor, and the value from the second angle sensor. is. [Effects of the Invention]

[0015] According to the present invention, in a robot that performs force control, the time required from acquiring sensor information to outputting a control force corresponding to that information can be reduced, thereby realizing force control that is highly stable and highly responsive. [Brief explanation of the drawings]

[0016] [Figure 1] FIG. 1 is a perspective view showing a schematic configuration of a robot device according to a first embodiment. [Figure 2] FIG. 1 is a block diagram showing the configuration of a robot control device according to a first embodiment. [Figure 3] FIG. 2 is a block diagram showing the configuration of a control system of the robot device according to the first embodiment. [Figure 4] FIG. 3 is a block diagram showing the configuration of a motor control unit for an i-th axis in the first embodiment. [Figure 5] 4 is a flowchart illustrating a procedure in which the robot device performs an assembly task in the first embodiment. [Figure 6] 5 is a flowchart for explaining processing executed by a force control system of a robot when executing a subtask in the first embodiment. [Figure 7] 5 is a flowchart illustrating a procedure for joint control executed by a servo control unit in the first embodiment. [Figure 8] 10 is a flowchart for explaining the processing executed by the force control system of the robot when executing a subtask in the second embodiment. [Figure 9] FIG. 10 is a block diagram showing the configuration of a servo control unit according to a second embodiment. [Figure 10] 10 is a flowchart for explaining the processing procedure of joint control executed by a servo control unit in the second embodiment. [Figure 11] FIG. 10 is a block diagram showing the configuration of a servo control unit according to a third embodiment. [Figure 12] 11 is a flowchart illustrating a specific processing procedure executed by a servo control unit in the third embodiment. DETAILED DESCRIPTION OF THE INVENTION

[0017] A robot device, a control method for a robot device, and the like according to embodiments of the present invention will be described with reference to the drawings. In the drawings referred to in the following description of the embodiments, elements denoted by the same reference numerals have similar functions unless otherwise specified. The embodiments described below are merely examples, and those skilled in the art can appropriately modify and implement the detailed configurations, for example, without departing from the spirit of the present invention.

[0018] [Embodiment 1] Fig. 1 is a perspective view showing a schematic configuration of a robot device according to this embodiment. As shown in Fig. 1, the robot device 100 includes an articulated robot 200 and a robot control device 300 that controls the operation of the robot 200. The robot device 100 also includes a teaching pendant 400 as a teaching device that operates the robot control device 300 and programs the operation.

[0019] 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 base end of the robot arm 251 is fixed to a pedestal B. The robot hand 252 grasps an object (such as an assembly part or a work tool).

[0020] The robot arm 251 has multiple joints, for example, six joints, J1 to J6, extending from the base end of the robot to the periphery. The robot is equipped with six actuators 201 to 206 that drive the joints J1 to J6 around the respective rotation axes A1 to A6.

[0021] The robot arm 251 has multiple links 2100 to 2106 rotatably connected at joints J1 to J6. The links 2100 to 2106 are connected in a linear chain from the base end to the periphery, and the robot's hand (the tip of the robot arm / link 2016) can be oriented in any direction within its range of motion.

[0022] The coordinate system To represents a coordinate system fixed to the base end of the robot arm 251, i.e., the base B, and the coordinate system Te represents a coordinate system fixed to the hand (the tip of the robot arm 251) of the robot 200. The position and posture of the robot can be expressed in various other coordinate systems.

[0023] The actuators 201-206 include electric motors 211-216 that drive the respective joints J1-J6, and sensor units 221-226 connected to the respective joints. The sensor units 221-226 include position sensors (angle sensors) that detect the positions (rotation angles) of the joints J1-J6, and torque sensors that detect the torque of the joints J1-J6. The actuators 201-206 include a speed reduction mechanism (not shown) and are supported by one of the links 2100-2206. The electric motor drives the speed reduction mechanism directly or via a power transmission mechanism such as a timing belt or gears, thereby driving the links 2100-2206. The actuator units do not necessarily need to include speed reduction mechanisms; a system in which the electric motor directly supports / drives the links or torque sensors may also be used.

[0024] A servo control unit 230 serving as a drive control unit that controls the drive of the actuators (motors) of each joint is disposed inside the robot arm 251. The servo control unit 230 controls the drive of the actuators 201 to 206 by controlling the drive current of the electric motor based on the input torque command value so that the torque of each joint follows the torque command value.

[0025] In this embodiment, the servo control unit 230 is described as being configured as one control device, but the servo control unit may be configured as a plurality of control boards or control devices respectively corresponding to the servo motors 211 to 216. Furthermore, although the servo control unit 230 is disposed inside the robot arm 251, it may also be disposed inside the housing of the robot control device 300.

[0026] Next, the robot control device 300 (operation command unit) will be described. Fig. 2 is a block diagram showing the robot control device of the robot device according to this embodiment. The robot control device 300 has a central processing unit (CPU) 301 as a control unit (processing unit). The robot control device 300 also has a read only memory (ROM) 302, a random access memory (RAM) 303, and a hard disk drive (HDD) 304 as storage units. The robot control device 300 also has a recording disk drive 305 and various interfaces 306 to 309.

[0027] The CPU 301 is connected to a ROM 302, a RAM 303, a HDD 304, a recording disk drive 305, and various interfaces 306 to 309 via a bus 310. The ROM 302 stores basic programs such as a BIOS. The RAM 303 is a storage device that temporarily stores various data such as the results of calculations performed by the CPU 301. The HDD 304 is a storage device that stores the results of calculations performed by the CPU 301 and various data acquired from the outside. The HDD 304 stores a program 330 for causing the CPU 301 to execute calculations (described later), as well as a robot model and an environment model to be used for motion planning and motion control of the robot. The CPU 301 can record various types of information to a recording disk 331 and read various types of information from the recording disk 331 via the recording disk drive 305.

[0028] The CPU 301 executes each step of the robot control method based on a program 330 recorded (stored) on the HDD 304. The teaching pendant 400 is connected to an interface 306, and the CPU 301 receives input from the teaching pendant 400 via the interface 306 and a bus 310. The interface 307 is connected to a monitor 321, and the CPU 301 can display various information related to the control process on the monitor 321 and provide it to the user. The interface 308 is connected to a storage device 322, and the CPU 301 can store various programs and data in the storage device 322 and read them out from the storage device 322.

[0029] A servo control unit 230 that controls the servo motors 211 to 216 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. The CPU 301 transmits command values ​​for each joint to the servo control unit 230 via the bus 310 and the interface 309 at predetermined time intervals.

[0030] FIG. 3 is a block diagram showing the configuration of a control system of a robot device according to this embodiment. A CPU 301 of a robot control device 300 (motion command unit) executes a program 330, including a robot motion control program, to function as a force control unit 504, a trajectory generation unit 505, a joint target value generation unit 507, a state estimation unit 506, and the like. While FIG. 3 illustrates functional blocks representing functional elements necessary for explaining the features of this embodiment, general functional elements not directly related to the principles of the present invention are omitted. Furthermore, the functional elements illustrated in FIG. 3 are conceptual functional elements and do not necessarily need to be physically configured as illustrated. For example, the specific form of distribution and integration of each functional block is not limited to the illustrated example. All or part of the functional blocks can be functionally or physically distributed and integrated in any unit depending on the usage situation, etc. Each functional block can be configured using hardware or software.

[0031] In this embodiment, the computer-readable recording medium is the HDD 304, and a program 330 for implementing functional blocks such as servo control is stored in the HDD 304. However, the present invention is not limited to this. The program 330 may be recorded on any computer-readable recording medium. For example, the recording medium for providing the program 330 may be the ROM 302, a recording disk 331, or an external storage device (not shown). Specific examples include a flexible disk, an optical disk, a magneto-optical disk, a magnetic tape, a non-volatile memory such as a USB memory, an SSD, etc.

[0032] The servo control unit 230 provided in the robot 200 has motor control units 531-536 and a signal relay distribution unit 530. The number of motor control units is determined according to the number of joints provided in the robot's arm, and since the illustrated robot has six joints, six motor control units 531-536 are provided. Of course, the number of motor control units can be changed depending on the type of robot. The signal relay distribution unit 530 receives commands sent from the robot control device 300 (motion control unit) and distributes them to the motor control units 531-536 that control the respective motors.

[0033] Each of the six joints J1 to J6 has its own motor and sensor unit. For example, joint Jn has a motor 21n and a sensor unit 22n, and sensor unit 22n is provided with a position sensor 55n and a torque sensor 541n (n is an integer from 1 to 6). For ease of explanation below, the individual correspondences may not be distinguished and the sensor units 221 to 226 may be collectively described as "sensor units 221 to 226 have angle sensors 541 to 546 that measure the position or angle of the joints, and torque sensors 551 to 556." The position sensors detect the rotational positions of the servo motors 211 to 216 or the angles or positions of the joints J1 to J6.

[0034] It should be noted that various parameters referred to in the following description may be shown in vector format in the drawings, as in the example below, but this does not mean that the content differs from the description. (1) Rotation angles θ1 to θ6 of servo motors 211 to 216: θ=(θ1,θ2,θ3,θ4,θ5,θ6) T (2) Rotation angles q1 to q6 of joints J1 to J6: q=(q1,q2,q3,q4,q5,q6) T (3) Joint torques τ1 to τ6 acting on joints J1 to J6: τ=(τ1,τ2,τ3,τ4,τ5,τ6) T

[0035] In the following description, when each parameter indicates that it is an estimated value, it is sometimes written as "_hat" after the parameter name in the text and a hat symbol is added to the parameter name in the drawings. When each parameter indicates that it represents a displacement, such as the difference between a command value and a current value, it is sometimes written as "_tilde" after the parameter name in the text and a tilde symbol is added to the parameter name in the drawings.

[0036] In this embodiment, optical rotary encoders or magnetic rotary encoders connected to the rotation shafts of the servo motors 211 to 216 are used as position sensors to detect the rotation angles θ1 to θ6 of the servo motors 211 to 216. However, the form of the position sensors is not limited to this example.

[0037] The rotation angles of the joints J1 to J6 can be calculated based on the detection results of the rotation angles θ1 to θ6 of the servo motors 211 to 216 and the reduction ratios of the reduction mechanisms provided in the drive trains, and q1_hat to q6_hat can be obtained as estimated values ​​of the joint rotation angles. Furthermore, angle sensors 541 to 546 that directly measure the output rotation angles of the joints may be disposed at each joint as position sensors to detect the rotation angles q1 to q6 of the joints J1 to J6. Torque sensors 551 to 556 detect joint torques τ1 to τ6 acting around the joints J1 to J6.

[0038] The rotation angles θ1 to θ6 of the servo motors 211 to 216, the rotation angles q1 to q6 of the joints J1 to J6 (measurement values ​​of the angle sensors 541 to 546), and the measurement values ​​of the joint torques τ1 to τ6 are treated as values ​​representing the states of the joints J1 to J6. In addition, the estimated values ​​q1_hat to q6_hat of the rotation angles of the joints J1 to J6 are also treated as values ​​representing the states of the joints J1 to J6.

[0039] The values ​​representing the states of the above joints J1 to J6 are acquired by the servo control unit at a predetermined cycle. That is, these values ​​are sampled every sampling cycle Ts (every second cycle) so as to be in time for the servo control cycle. When an estimated value is calculated based on a measured value, the sampling cycle Ts (second cycle) is set so that the entire process from measurement to calculation falls within the sampling cycle Ts. The sampling cycle Ts, which is also the control cycle by the servo control unit, is set to, for example, 100 μs (microseconds).

[0040] The values ​​representing the states of the joints J1 to J6 acquired by the servo control unit are transmitted to the robot control device 300 via the signal relay / distribution unit 530 at predetermined transmission intervals. The transmission interval is the sampling interval T A The sampling period T (first period) is set according to the time required for the robot control device 300 to execute information processing (calculation processing) related to one control cycle of the robot. A The (first period) is, for example, 1 ms (millisecond). The acquired values ​​representing the states of the joints J1 to J6 are input to the state estimation unit 506.

[0041] The state estimation unit 506 calculates (estimates), for example, the position x of the motion point of interest, the position displacement x_tilde, and the force moment Fe acting on the motion point, using the input values ​​representing the states of the joints J1 to J6 and the robot model 503. The robot model 503 is configured, for example, by a geometric model and a dynamic model of the robot arm.

[0042] The operating point of interest is, for example, the tool center point (TCP) of the end-effector of a robot hand. In this case, the displacement x_tilde of the TCP position is a six-dimensional vector that represents the difference between the commanded end-effector position (commanded position, equilibrium point of the end-effector position) and the measured current position. In addition, the force moment Fe acting on the operating point can be expressed as a vector as follows: (4) Force moment acting at the operating point Fe:

number

[0043] The information processing performed by the state estimation unit 506 further includes processing to estimate the state of the robot required to generate signals to control its motion, and processing to acquire information used as a criterion for determining the transition of the sequence of operations (e.g., an assembly sequence) to be performed by the robot. The former includes, for example, processing to estimate the velocity x' and acceleration x'' of the hand. The latter includes, for example, processing to detect the contact state between the robot and a workpiece or another object. The state estimation unit 506 outputs the results acquired by these information processing operations to the force control unit 504, etc.

[0044] Here, we will explain the force teaching data 501 and position teaching data 502 stored in the storage device 322 of the robot control device 300. An operator can use a man-machine interface such as the teaching pendant 400 to teach the robot how to move and create a robot operation program. The operator can teach the robot how to move by describing the procedure (task sequence) of the work to be performed by the robot, the subtasks that make up the work, the end conditions for the work and subtasks, the state transition method between subtasks, etc., and inputting the teaching data. The teaching data includes force teaching data 501, which is a parameter related to force control, and position teaching data 502, which specifies the movement to be performed by the robot.

[0045] The force teaching data 501 is composed of stiffness Kd (stiffness information) and damping Dd (damping information) to be applied to the robot's hand, a command value Fd for the contact force in the assembly work, etc. The stiffness Kd and damping Dd to be applied to the robot's hand are a 6-by-6 matrix specified according to the coordinate system described above, and are specified according to the desired contact state. For example, when pressing a gripped workpiece against the environment, the stiffness in the pressing direction is set small to prevent excessive contact force from acting, and the stiffness in other directions is set relatively large.

[0046] The position teaching data 502 includes, for example, parameters relating to the position and posture of the robot's hand in the key frame of the taught motion, that is, the hand position p d,i and posture R d,i , the target joint angle q d,i is entered (i is the teaching point number / keyframe number). Furthermore, the position of the hand p d,i And posture R d,i can also be written as follows: (5) Hand position p d,i :

number

number

[0047] The trajectory generation unit 505 calculates the position p of the hand from the position teaching data 502. d,i and posture R d,i , the target joint angle q d,i By reading out the above and smoothly connecting them, the time series of the hand target position xd(t) is generated as the hand command value. d(t) is the sampling period T A It is a vector sequence with a value every (every first period).

[0048] The joint target value generating unit 507 performs calculations of inverse kinematics analysis based on the robot model 503 for the hand target position xd(t) input from the trajectory generating unit 505. Then, the joint target position q d (t), and its first derivative, the joint angular velocity q d '(t), the second derivative of the joint angular acceleration q d ″(t) and outputs the result to the force control unit 504.

[0049] The inverse kinematics analysis performed in this case may compensate for joint displacement using a dynamic model in addition to a geometric model. For example, when converting the hand position into a joint position using a geometric model of the robot, dynamics (gravity and inertial forces) acting on the robot may be taken into account, and displacement due to elastic deformation of the robot and the environment may also be taken into account.

[0050] The robot model 503 may include a load model in addition to the geometric and dynamic models of the robot. The geometric model of the robot is a shape model that includes geometric information such as the dimensions of the link system that constitutes the robot itself and the arrangement of the joints that connect the links. The dynamic model of the robot is a dynamic model that includes, for example, the mass, moment of inertia, and position of the center of gravity of the links that constitute the robot itself. The load model is a load model that includes, for example, an end effector attached to the tip of the robot's hand and a workpiece grasped by the robot.

[0051] Next, various types of information input to the force control unit 504 will be described. A robot model 503, an environment model 508, force teaching data 501, and position teaching data 502 are input to a force control unit 504. The environment model 508 may be configured using design data prepared in advance using 3D-CAD or the like, or may be a model based on information generated / acquired by an external sensor 509 such as a visual sensor.

[0052] Also, the target hand position x at the current time generated by the trajectory generation unit 505 d (t), and the joint target position q at the current time generated by the joint target value generating unit 507. d Furthermore, the force control unit 504 receives x_hat relating to the current state of the robot acquired by the state estimation unit 506, x'_hat, x_tilde, F_hat, q_hat which are estimated values ​​of velocity, q'_hat which is estimated value of angular velocity, and the like.

[0053] The force control unit 504 performs calculations based on the input information to generate the following command values, and outputs (transmits) them to the servo control unit 230. The transmission may be performed via Ethernet, for example, and the transmission period is T A seconds (for example, 1 ms).

[0054] The force control unit 504 outputs one or more of the following pieces of information as command values ​​to the servo control unit 230. That is, the dynamic compensation term τ of the torque command value for each joint: cmp and the coordinate system T in the task space e The stiffness K specified by d is transformed (mapped) into the joint space to obtain the stiffness command value K q (stiffness information) and the task space coordinate system T e The damping D specified by d is transformed (mapped) into the joint space and the damping command value D q (damping coefficient) and motor angle command value Q d (t) and the command value F d converted to joint-level torque τd,ext In this embodiment, for example, all of this information can be output to the servo control unit 230 as command values.

[0055] The servo control unit 230 receives a command value transmitted from the robot control device 300. The signal relay / distribution unit 530 distributes the received command value to each of the motor control units 531 to 536. Each of the motor control units 531 to 536 receives the input command value and the motor rotation angle θ fed back from the sensor unit. i and joint torque τ i Based on this, the command value τ of the joint torque that each joint should specifically achieve is d,i (i is the joint number between 1 and 6).

[0056] Then, the output torque of each joint is calculated based on the torque command value τ d,i (torque command value τ d,i and joint torque τ i The motor drive current is controlled by calculating the feedback compensation amount for the joint torque (so that the difference between θ and θ is small) and controlling the current supply to the motor. The calculation process for calculating the command value of the joint torque and the feedback compensation amount involves time-dependent changes in the state of the joint (for example, θ i ' and τ i ') may be included.

[0057] 4 is a block diagram showing the configuration of the motor control unit for the ith axis (i.e., the ith joint counting from the base end) in this embodiment. The motor control unit for each joint has a coefficient setting unit 5311, joint stiffness / damping control unit 5312, and torque control unit 5313.

[0058] The coefficient setting unit 5311 calculates the joint stiffness K q,i,i and the damping coefficient D for joint i q,i,i and set them in the joint stiffness / damping control section 5312. q,m,n is the joint stiffness matrix K, which is an N-by-N matrix (N is the number of joints).q It represents the m-row n-column component of the joint stiffness K q,i,i is the joint stiffness matrix K q are the diagonal components of

[0059] To improve the stability of the robot's motion and ensure smooth operation, it is desirable to update these coefficients as quickly as possible, but the update period is determined by the communication period between the robot control device and the servo control unit. (The period is T A The joint stiffness / damping control unit 5312 controls the A Every second, the motor angle command value θ for joint i is d,i is received from the signal relay distribution unit 530, and the joint stiffness K q,i,i and the damping coefficient D q,i,i is received from the coefficient setting unit 5311. Then, the current motor angle θ measured by the angle sensor 54i is i is fed back every Ts seconds, which is the sampling period of the servo control unit.

[0060] The joint stiffness / damping control unit 5312 calculates the latest motor angle command value θ d,i , joint stiffness K q,i,i , damping coefficient D q,i,i , rotation angle θ i Based on this, the stiffness and damping term τ of the torque command value for the own joint (joint i) is calculated using the following equation 4. pd,i and outputs it to the torque control unit 5313. The time required for the calculation, that is, the period for outputting the calculation result, is the sampling period T s seconds.

number

[0061] The torque control unit 5313 controls the joint torque output τ i is the specified torque command value τ d,i The motor drive current I i (Motor output torque τ m,i ) is a torque servo control system that controls the energization of the joint torque command value τ d,iThe calculation of is performed as follows: That is, the torque control unit 5313 calculates the torque command value τ pd,i The period T s Interference term (stiffness / damping term related to other joints: joint torque determined in proportion to the displacement and velocity of other joints different from the own joint) τ of the torque command value received in seconds and transmitted from the signal relay / distribution unit 530 couple,i and a torque τ that compensates for the robot's own dynamics (such as inertia, centrifugal force, Coriolis force, and gravity effects). cmp,i update period T A Then, the following equation 5 is calculated to obtain the command value τ of the joint torque that each joint should specifically realize. d,i T s Calculated in seconds.

number

[0062] Next, the torque control unit 5313 s The torque sensor is measured every second to measure the joint torque τ i Sensing and T s Motor output torque τ per second m,i (=K T I i , but K T is the motor torque constant, I i is the motor current) is updated as follows:

number

[0063] Equation 6 is the command value τ of the joint torque d,i and the current value τ i The torque (feedback compensation amount) that the motor should output is calculated as τ d,m,i It is a function that determines the value of the parameter. There are various possible configurations, but the most basic one is a PID control with feedforward as shown below.

number

[0064] The torque control unit 5313 outputs the calculated motor torque command value (=motor current command value) to a motor current control system arranged as a lower control loop. The motor current control system determines whether the actual motor current is equal to or smaller than the specified value (I i ) and control the motor drive.

[0065] Next, a specific operation procedure of the force control system of the robot in this embodiment will be described with reference to Figures 5 to 7. In this description, an example will be taken of an assembly robot performing assembly work at a product production site. It is assumed that a product to be produced is assembled into a product assembly by performing a plurality of processes (tasks).

[0066] In this embodiment, the assembly process will be described assuming a process in which a robot grips a component workpiece with an end effector, assembles the workpiece into a product assembly, and butts the workpiece to the assembly completion position. For example, this is a process in which a timing pulley with a key groove is gripped by hand and inserted into the assembly-side shaft of the product assembly, which already has a parallel key attached, until it is inserted to the insertion completion position (where the pulley butts against the spigot portion of the shaft).

[0067] 5, one assembly process in this embodiment is made up of N operation units (subtasks) (N steps). Each operation (operation steps 1 to N) is, for example, (1) picking a workpiece (a component removal operation by grasping a workpiece with an end effector), (2) moving the robot to the assembly start position, (3) assembling the workpiece into the assembly, (4) releasing the end effector, and (5) retracting the robot (returning operation).

[0068] The above subtask (3) is further composed of (3-1) approaching the hole opening, (3-2) coaxial alignment between the workpiece and assembly using force-sensing control, (3-3) searching to adjust and match the phase of the parallel key and key groove, and (3-4) insertion operation up to the abutting position where the pulley hits the indentation part of the shaft / detection of assembly completion.

[0069] The robot has completed the previous process (the part assembly process preceding this process) and has started the current process. The robot reads out a program describing the operation of the current process stored in the memory unit, reads in the teaching data, and starts executing the robot's operation program for the current process. The program describes the robot's operation procedures, the conditions for ending the operation steps, the method of state transition between steps, etc. The teaching data includes force teaching data 501 and position teaching data 502 (step S-1-1).

[0070] The robot recognizes the assembly status of the workpiece and the robot's own working status based on the loaded environmental model and information from the robot's visual sensors and external sensors of peripheral devices outside the robot (step S-1-2).

[0071] Then, a step is executed to create a robot motion plan based on the recognition results and teaching data (step S-1-3). In step (S-1-3), the work state that the robot should ultimately achieve in each operation step (subtask) is planned based on the teaching data and the sequence of operations for the current task, the state transition method between subtasks, the termination conditions for tasks and subtasks, etc. The work state includes, for example, the relative positional relationship between the workpiece and the assembly, the contact state, and the contact force. Based on this, the hand target position x that the robot should refer to at each sampling time in each subtask is calculated. d (t), contact state, contact force F d , stiffness at the reference point K d , attenuation D d is generated.

[0072] In each step from step S-1-4 onwards, the robot is controlled to achieve the planned movements and contact states in order to carry out specific assembly tasks such as inserting parts into the assembly axis, and transitions between each operation according to the planned transition conditions / termination conditions (steps S-1-4 to S-1-6).

[0073] In each task, the robot is controlled by mechanical impedance control, which can seamlessly handle each state of free movement in the air, control of contact between parts (control of collision process), and control of contact force (control of quantitative pressing force).

[0074] After repeatedly executing this control of unit operations and completing a series of subtasks (step S-1-6), the robot determines whether the assembly task of the current process was successful (step S-1-7). That is, the robot makes a determination based on the results of processing by the state estimation unit based on internal sensors (force information based on torque sensors, etc.) and external sensors such as vision, such as the contact state between the workpiece and assembly, and the positional relationship between parts.

[0075] If it is determined to be a failure, an error recovery process (step S-1-8) is executed to resolve the assembly failure state, and if it is determined to be a success, the current process is terminated and the process moves to the next process task (END / next process).

[0076] FIG. 6 is a flowchart illustrating a series of processes executed by the force control system of the robot when executing each subtask described in FIG. 5 in this embodiment, and shows the processes executed by the robot control device 300, which is the arm control system. The arm control system updates the information to be sent to the joint control system, which is a control system lower than the arm control system, i.e., the joint servo control unit, so as to realize the state planned by the motion planning unit (steps S-2-1 to S-2-5). Then, it sends this information to the servo control unit (step S-2-6) and receives the processing results (sensor feedback values) executed by the servo control unit, which is a lower control system (steps S-2-7 to S-2-8). Furthermore, the arm control system starts the next process based on the updated robot state information (steps S-2-9 to S-2-10). The update cycle of this series of processes is the update cycle T of the robot control device. A The subtasks are executed repeatedly until they are completed (step S-2-11: YES).

[0077] Before explaining the specific procedures for the processing and calculations performed at each step, we will introduce a mathematical model of robot dynamics (dynamic model, equations of motion). As is well known, the equations of motion for an articulated robot with six degrees of freedom, taking into account the flexibility of the joints, are expressed as in Equation 8 below.

number

number

number

number

number

number

number

number

number

number

number

number

number

number

number

[0078] The two equations listed in Equation 8 express the dynamics of the link system and the motor rotor system, respectively, and are connected by a joint torque τ.

[0079] In step (S-2-1), the robot control system retrieves information related to the planned motion state of the robot and reads out the command value that the robot should achieve at each time. The command value that should be achieved is the target position of the specified reference point, i.e., the hand target position x d (t), contact force F d , stiffness at the reference point K d , attenuation D d is.

[0080] In step S-2-2, the robot control device 300 calculates the target trajectory of the hand target position x d This is a kinematics calculation process that executes calculations of inverse kinematics analysis based on the robot model 503 based on (t) and calculates a target state at the joint level at the current time t1.

[0081] That is, the joint target value generating unit 507 calculates the velocity x based on the input hand target position xd(t). d '(t) and acceleration x d ''(t), etc. Furthermore, the target states of the joints required for the calculation of the subsequent control variables, i.e., the joint target positions q d (t1), the joint angular velocity q d '(t1), joint angular acceleration q d ''(t1) etc. are calculated using the following formula.

number

number

number

number

number

[0082] In step S-2-3, the following calculation is performed based on the processing result of step S-2-2, the joint state of the robot, the dynamics model (equation of motion) in the joint space of the robot, and the dynamics parameters. That is, the dynamic compensation term τ of the torque command value for each joint is calculated. cmp Calculate. τ cmp is a nonlinear feedforward term that compensates for the joint torque required for the robot's own motion, and is a six-dimensional vector, the same as the number of joints.

number

[0083] In step S-2-4, the mechanical impedance in the joint space (stiffness command value K q and damping command value D q ) is calculated. Based on the Jacobian matrix J(q) of the robot at the reference point calculated in step S-2-2, the planned task space (coordinate system T e ) specified stiffness K d and damping D d The stiffness command value K q and the joint-level damping command value D q The following process is performed to convert it into

number

[0084] The next step S-2-5 is to generate the torque τ that generates the elastic restoring force proportional to the displacement from the equilibrium point specified by the impedance control and the damping force proportional to the velocity. imp =τ pd +τ couple Among them, the component τ that represents the interference between joints couple,i Calculate τ pd More on this later.

number

number

[0085] The mechanical impedance control executed in this embodiment is a type of force control that uses torque generated at a joint. First, a tool coordinate system T is fixed to a coordinate system fixed to a reference point of interest in the task space (for example, the tip of a robot). e At the equilibrium point x d Then, force control is performed so that an elastic restoring force (force proportional to the displacement x (variation)) and a damping force (force proportional to the velocity) set in the reference coordinate system are generated around it. The elastic restoring force and damping force from the equilibrium point to be generated are expressed in the task space as in equation 32.

number

[0086] To realize these forces using torques generated at the joints, the restoring force and damping force described in task space are mapped to joint space and converted into joint driving forces. Specifically, the Jacobian matrix J(q) related to the kinematics of the robot is used to obtain the following equation (33).

number

[0087] Assuming that x_tilde and x'_tilde are infinitesimal, using the differential kinematics relation dx=J(q)dq, τ imp can be transformed as follows:

number

[0088] In this way, the τ imp is the feedback part τ of the state of the own joint pd and the feedback law (interference term, other axis feedback) τ of the state of a joint other than the own joint shown in Equation 30 couple It can be expressed as the sum of the numbers 35.

number

[0089] In step S-2-5, calculation cannot be performed based on the state of the joint itself, that is, τ couple The calculation of is performed by the robot control device 300. The above is an outline of the command value generation process executed by the robot control device 300.

[0090] In step S-2-6, the calculation result of the above step, τ cmp,i , τ couple,i , θ d,i , K. q,i,i , D q,i,i is sent from the robot controller to the servo controller via Ethernet or other means as a command value for the motor controller of the ith joint (i=1...6).

[0091] In step S-2-7, the servo control unit receives the transmitted command values, senses the state of each joint, and controls the driving force (driving current) of each motor to specifically control the joints of the robot. Specific processing in the servo control unit will be described later with reference to FIG. 7.

[0092] In the next step S-2-8, the robot control device receives the control results from the servo control unit via a communication means such as Ethernet, just as it did when transmitting. The main data received is the motor angle θ (joint angle q), joint torque τ, etc., which are the results of sensing the joints.

[0093] In step S-2-9, the robot control device inputs the received joint states (such as the above θ and τ) to the state estimation unit, and estimates the states necessary for generating signals for the robot's motion control, such as the robot's current position x and contact force F e Estimate.

[0094] Step S-2-10 is a process for determining whether the current task has been completed. Based on the results of step S-2-9, the contact state of the workpiece is estimated, and the assembly state of the workpiece-assembly is estimated based on information from external sensors such as vision. This allows the progress of the currently planned assembly task to be grasped, and determines whether the current task has been completed. If the completion determination is YES, the process moves to the next planned operation task, and if NO, the process returns to step S-2-1, and calculations for motion control at the next time are performed. The update cycle in this series of processes is T, which is the control cycle of the robot control device. A seconds.

[0095] Next, with reference to FIG. 7, a specific operation procedure will be described for the joint control process executed by the servo control unit in step S-2-7 (FIG. 6). (Step S-3-1: Receive command values ​​from the robot control device) The servo control unit calculates each command value τ cmp,i , τ couple,i , θ d,i, K. q,i,i , D q,i,i (i=1...6) are received. Then, a signal relay distribution unit 530 (gateway controller: GW) distributes these commands to motor control units 531 to 536 corresponding to each joint.

[0096] (Step S-3-2: Update the number of iterations) The variable k that stores the number of iterations is incremented (k←k+1) and the number of times the joint control process has been executed in the servo control unit is counted. The joint control process in the servo control unit is repeatedly executed until the next command value is sent from the robot control device. The number of iterations k is cleared (k←0) when the servo control unit receives a command value. Subsequent processes are executed as fast as possible within the range allowed by the processing unit and sensing mechanism, and the update cycle is T S seconds.

[0097] On the other hand, the interval between the communication processing between the servo control unit and the robot control device is T A The processing time of steps S-3-1 and S-3-9 is determined by the performance of the hardware. S =0.1×10 -4 seconds, T A =1×10 -3 The servo cycle update period (sampling or calculation period of the servo control unit) is set to 10 times faster than the communication cycle.

[0098] (Step S-3-3: Acquire joint status) The subsequent steps are processed by each motor control unit. The motor control unit accesses the sensor unit arranged at each joint and obtains the current motor angle θ measured by the angle sensor (rotary encoder). i and the current joint torque τ measured by the torque sensor i and the motor drive current I i Step S-3-3 is a sensory feedback process to the motor control unit.

[0099] (Step S-3-4: Estimation of joint condition) This is a process for estimating the state of the joint and obtaining the estimated value required for subsequent processing. In step S-3-5 (calculation of the command value of the joint torque) and step S-3-7 (calculation process for calculating the feedback compensation amount for servo control of the joint torque), the time change value of the state of the joint, that is, the motor rotation speed θ' is calculated. i (=dθ i / dt) and torque change rate τ' i However, they cannot be measured directly. On the other hand, in the case of numerical differentiation (pseudo-differential) processing, which records the state at the previous time and calculates the difference between that and the current value, numerical noise and phase lag become problems. Therefore, an observer using a dynamic model or a probability model is used to estimate the state θ' i _hat, τ' i Execute the process to find _hat.

[0100] (Step S-3-5: Calculation of stiffness and damping terms) The latest θ updated by the above process d,i , K. q,i,i , D q,i,i , θ i Based on this, the stiffness and damping term τ for the torque command value of the own joint (joint i) is pd,i is calculated and output to the torque control unit.

number

[0101] (Step S-3-6: Calculation of joint torque command) The latest updated τ cmp,i , τ pd,i , τ couple,i Based on this, the command value τ of the joint torque that each joint should specifically achieve is d,i Calculate.

number

[0102] (Step S-3-7: Execute joint torque control (torque servo) processing) Joint torque output τ i is the specified torque command value τ d,i The motor drive current I i (=motor output torque τ m,i ) is a torque servo process that controls the energization of the

[0103] The latest τ updated in the process up to step S-3-6 d,i , τ i、 τ' i Based on the _hat, the output torque of the joint is d,i (i.e., τ d,i and τ d Then, based on the calculation results, the torque τ that the motor should output is calculated. d,m,i That is, the process of the above-mentioned equation 6 is performed. Calculated motor torque command value (= motor current command value) τ d,m,i is output to the motor current control system, which is arranged as a lower control loop, and the motor current control system determines whether the actual motor current is the specified value (I i ) and control the motor drive.

[0104] (Step S-3-8) The servo control unit evaluates the variable k that stores the number of iterations, and checks whether the joint control process in the servo control unit has completed the specified number of iterations. If k is smaller than the specified number (NO), the process returns to step S-3-2 and executes the joint control process again at the next time. If k is equal to the specified number (YES), the process proceeds to step S-3-9, which is a process of sending the control results to the robot control device.

[0105] (Step S-3-9: Send the joint status to the robot control device) The joint state, which is the control result (θ i , τ i , θ' i_hat, τ' i _hat, I i The motor control unit stores the information in the gateway controller. These are sent to the robot control unit via a communication means such as Ethernet, along with a control status flag that stores whether the joint control system has operated normally. This sending process is performed every T A It is carried out every second (END).

[0106] The motion characteristics (dynamics) of the robot controlled in this way are determined by using sufficiently accurate dynamic parameters and setting K d , D d Under this, it will be something like this:

number

number

[0107] With a robot control device configured as described above, the servo unit can calculate and output a feedback control amount based on the robot's motion state without using the integrated control unit. The servo unit can acquire sensor information and update the control law at a much faster cycle than the integrated control unit, thereby reducing the time required from when the robot device acquires sensor information until the drive unit outputs a control force. According to this embodiment, the control gain of the control system can be increased to improve the damping performance of the robot, thereby enabling stable force control while improving responsiveness.

[0108] [Embodiment 2] The second embodiment will be described with reference to Figures 8 to 10. Explanations of matters common to the first embodiment will be simplified or omitted. In the first embodiment, the robot control device 300 performs a calculation process of the interference term (FIG. 6: step S-2-5) to reduce the influence of interference between joints, but in this embodiment, the servo control unit 230 of the robot arm performs this process.

[0109] FIG. 8 is a flowchart illustrating a series of processes executed by the robot control device 300 (arm control system) when executing each subtask (operation step 1 to operation step N in FIG. 5) in this embodiment.

[0110] As in the first embodiment, the arm control system updates the information to be sent to the joint control system, which is a control system lower than the arm control system, i.e., the servo control unit of the joint, so as to realize the state planned by the motion planning unit (steps S-6-1 to S-6-4). Then, it sends this information to the servo control unit (step S-6-6) and receives the processing results (sensor feedback values) executed by the servo control unit, which is a lower control system (steps S-6-7 to S-6-8). Furthermore, the arm control system starts the next processing based on the updated robot state information (steps S-6-9 to S-6-10). The update cycle of this series of processing is the update cycle T A The subtasks are executed repeatedly until they are completed (step S-6-11: YES).

[0111] The difference from the first embodiment is that the step of calculating the interference term in S-2-5 (FIG. 6) is omitted in the processing flow of the robot control device 300 of this embodiment. In order to shorten the time required from acquiring sensor information to outputting a control force corresponding to that information, in this embodiment, the calculation of the interference term is executed within the servo control unit 230, as will be described later.

[0112] 9 is a block diagram showing the configuration of the servo control unit 230 in this embodiment. Communication between the robot control device 300 and the servo control unit 230 will be described with reference to the same figure.

[0113] The servo control unit 230 receives each command value sent from the robot control device 300, and the signal relay distribution unit 530 distributes these commands to each of the motor control units 531 to 536. The signal relay distribution unit 530 is made up of a signal conversion unit 5301 that controls the communication function with the robot control device, and a shared memory unit 5302 that holds the received data.

[0114] The signal conversion unit 5301 converts the data transmission period of the robot control device into the A K per second q , D q , τ cmp , θ d and stores them in the shared memory unit 5302. Each parameter stored in the shared memory unit 5302, i.e., K q , D q , τ cmp , θ d The period in which is updated is T A seconds.

[0115] In this embodiment, the shared memory unit 5302 is configured using an FPGA (Field Programmable Gate Array), but is not limited to this example. For example, a method using an SRAM or a method of integrating the signal conversion unit 5301 and the shared memory unit using a microcomputer may also be adopted.

[0116] The motor control unit 53i of the ith joint accesses the shared memory unit 5302 and outputs K as a command value for the ith joint. q , D q , τ cmp,i , θ d and the motor angle θ for the other joints k and angular velocity θ' k (k=1 to 6, excluding i). Then, the read data and the motor rotation angle θ detected by the angle sensor 54i of the own joint are compared. i and the current joint torque τ detected by the torque sensor 55i of the own joint. i Based on this, the motor control unit 53i of the ith joint calculates a command value τ of the joint torque that the own joint should specifically realize. d,iThen, the output joint torque τ i is the command value τ d,i The drive current Ii of the motor 21i (or the output torque τ m,i ) is controlled by the torque servo control process. The current motor rotation angle θ of the own joint is detected during this process. i and angular velocity θ' i is stored in the shared memory unit 5302.

[0117] Furthermore, the angular velocity θ' i is calculated or measured during the series of servo control processes. For example, the rotation angle θ of the motor measured by the angle sensor 54i i is numerically differentiated (subtracted), and then subjected to low-pass filtering to obtain the angular velocity θ' i Alternatively, a velocity sensor can be mounted on the joint to calculate the angular velocity θ'. i The angular velocity θ' can be measured directly, or a state estimator such as a Kalman filter can be used to calculate the angular velocity θ' based on the sensor observation data and a dynamic model. i may be estimated.

[0118] Here, the update period of the servo cycle for executing a series of processes of the motor control unit 53i is T S Seconds, sensed rotation angle θ of the motor of the own joint i and angular velocity θ' i The period for storing G The data storage period (T G ) should be as small as possible, and T G If possible, T S However, if there are issues with the transfer speed, etc., S It may be set larger than this.

[0119] According to this embodiment, the motor control unit 53i receives information (another axis feedback amount) relating to the state of a joint other than the own joint, specifically, the rotation angle θ of the motor of the other joint. i (or joint angle) and angular velocity θ' ican be acquired without going through the robot control device 300. That is, the motor control units 53i in the servo control unit 230 can share (transmit and receive) information relating to the states of the joints with each other at high speed.

[0120] As a result, in this embodiment, the control period, which was previously determined by the control cycle of the robot control device 300, can be shortened to, for example, about one-tenth. This makes it possible to perform feedback compensation with little delay, and to perform good force control with effective damping characteristics.

[0121] FIG. 10 is a flowchart illustrating a specific processing procedure executed by the servo control unit 230 in this embodiment. (Step S-4-1: Receiving command value)

[0122] This is the same processing as step S-3-1 in embodiment 1. Each command value transmitted from the robot control device 300 is received by the signal conversion unit (gateway controller: GW) of the signal relay distribution unit 530 of the servo control unit 230 and stored in the shared memory unit 5302.

[0123] (Step S-4-2: Update the number of iterations) This is the same processing as step S-3-2 in the first embodiment. The processing after this step is performed in parallel for each joint by each motor controller up to step S-4-10. Unless otherwise specified, the update period is T S seconds. (Step S-4-3: Acquire joint status) This is the same processing as step S-3-3 in embodiment 1. This is sensor feedback processing to the motor control units 53i. Each motor control unit 53i accesses the sensor unit arranged in its own joint and acquires the latest observation results. (Step S-4-4: Estimation of joint condition) This is the same processing as step S-3-4 in embodiment 1. Each motor control unit 53i calculates the state of its own joint by pseudo-differential processing to be used in subsequent processing. A state observer using a dynamic model or a probability model may be used.

[0124] (Step S-4-5: Access to shared memory) The shared memory unit 5302 stores data K acquired from the robot control device 300. q , D q , τ cmp , θ d and θ=(θ1, θ2, θ3, θ4, θ5, θ6) which is data acquired from the motor control units 531 to 536. T and θ'=(θ'1, θ'2, θ'3, θ'4, θ'5, θ'6) T However, it is retained.

[0125] The i-th motor control unit 53i of the i-th joint (i=1...6) accesses the shared memory unit 5302 and stores the command value K q,i,k (k=1···6), D q,i,k (k=1···6), τ cmp,i , θ d =(θ d1 ,θ d2 ,θ d3 ,θ d4 ,θ d5 ,θ d6 ) T At the same time, the ith motor control unit 53i acquires, from the shared memory unit 5302, information (feedback amount) relating to the state of the other joint transmitted from the jth motor control unit 53j (j=1...6, excluding i) of the other joint (joint other than the ith joint). The information relating to the state of the other joint is the rotation angle θk (k=1...6, excluding i) and angular velocity θ'k (k=1...6, excluding i) of the motor of the other joint. The ith motor control unit 53i accesses the command value and feedback amount as described above, and at the same time, acquires θ, which is information relating to the latest state of its own joint (latest observation result). i and θ' i is stored in the shared memory unit 5302.

[0126] The access cycle of the motor control units 531 to 536 to the shared memory is the communication cycle (T A ) shorter than T G The access period is T G is the motor control unit update period (servo cycle) T S However, it may be set appropriately depending on the load of each processing unit and the transfer speed of communication between the processing units.

[0127] (Step S-4-6: Calculation of the interference term) The i-th motor control unit 53i (i = 1...6) corresponding to the i-th joint performs the following calculation process using the command value parameters acquired from the shared memory in step S-4-5, the feedback amount from the other joint, and the information on the state of its own joint (its own joint feedback amount) acquired in steps S-4-3 and S-4-4. That is, among the joint torque command values ​​that generate the elastic restoring force and the damping force, the interference term τ calculated from information across multiple joints (displacements and velocities of multiple joints) is calculated. couple,i Update.

number

[0128] The update period for this calculation is set equal to the update period for the other axis feedback amount, that is, the access period to the shared memory. G Seconds. T G and T S If τ is set to a different period and there is no access to the shared memory of the motor control unit, this step is skipped and τ couple,i The value remains the same as it was at the time of the previous update.

[0129] (Step S-4-7: Calculation of stiffness and damping terms related to the joint) This is the same processing as step S-3-5 in the first embodiment. The ith motor control unit 53i (i=1 to 6) corresponding to the ith joint uses the latest θd,i , K. q,i,i , D q,i,i , θ i Based on this, the stiffness and damping term τ pd,i is updated and output to the torque control unit.

number

[0130] (Step S-4-8: Calculation of joint torque command) This is the same processing as step S-3-6 in the first embodiment. The i-th motor control unit 53i (i=1...6) uses the latest τ cmp,i , τ pd,i , τ couple,i Based on this, the command value τ of the joint torque that each joint should specifically achieve is d,i Update.

number

[0131] (Step S-4-9: Execute joint torque control processing) This is the same processing as step S-3-7 in the first embodiment. The ith motor control unit 53i (i=1...6) corresponding to the ith joint outputs the joint torque τ i is the specified torque command value τ d,i The motor drive current is controlled to follow the (Step S-4-10) This is the same processing as step S-3-8 in the first embodiment.

[0132] (Step S-4-11: Send the joint status to the robot control device) This is the same process as step S-3-9 in the first embodiment. The gateway controller reads out the joint states (θ, θ', τ, etc.) stored in the shared memory and transmits them together with the control state flag to the robot control device 300 via a communication means such as Ethernet. This transmission process is performed every 10 seconds, which is the update period of the robot control device 300. A It is carried out every second (END).

[0133] According to the robot control system of this embodiment, the servo control unit, which is a servo unit, can calculate and output feedback control amounts based on the motion states of other axes without going through the robot control device, which is an integrated control unit. The servo unit can acquire joint states (sensor information) and update control laws at a much faster cycle than the integrated control unit, so it is possible to reduce the time required from when the robot device acquires sensor information until the drive unit outputs a control force.

[0134] In the configuration according to this embodiment, the speed of acquiring the other joint states and updating the control law based on them is increased, and in particular, the speed of the interference term τ couple,i It is possible to increase the update speed of the force, which makes it possible to perform force control with higher accuracy and stability.

[0135] [Embodiment 3] Embodiment 3 will be described with reference to Figures 11 and 12. Descriptions of matters common to embodiment 1 or embodiment 2 will be simplified or omitted. Figure 11 is a block diagram showing the configuration of a servo control unit 230 in this embodiment.

[0136] In this embodiment, the signal relay / distribution unit 530 and the motor control units 531 to 536 for each axis arranged corresponding to the joints are each configured as an independent control circuit (for example, a circuit board on which a microcontroller or a microprocessor is mounted). They are connected to each other so that they can communicate with each other in a bus-type or daisy-chain-type network topology. They are connected via a communication means such as RS485. In this embodiment, the processing executed by the force control system (robot control device) of the robot is the same as the processing explained with reference to FIG. 8 in the second embodiment.

[0137] Referring to FIG. 11, the communication between the robot control device 300 and the servo control unit 230 in this embodiment will be described. The servo control unit 230 receives each command value transmitted from the robot control device 300, and the signal relay / distribution unit 530 transmits these commands to the motor control units. More specifically, the signal relay / distribution unit 530 transmits each command value received from the robot control device to a first motor control unit 531. The motor control unit 531, having received this, transmits it to a second motor control unit 532 corresponding to the next joint. Similarly, each motor control unit transmits a command value to the motor control unit of the next joint, and the command values ​​are transmitted up to a sixth motor control unit 536. The command values ​​are distributed to each motor control unit via such a transmission path.

[0138] The signal relay / distribution unit 530 is made up of a signal conversion unit (gateway controller GW) 5301 that controls the communication function with the robot control device, and a shared memory unit 5302 that stores received data.

[0139] The signal conversion unit 5301 converts the data transmission period of the robot control device into the A K per second q , D q , τ cmp , θ d and stores them in the shared memory unit 5302. Each parameter stored in the shared memory unit 5302, i.e., K q , D q , τ cmp , θ d The period in which is updated is T A seconds.

[0140] In this embodiment, the shared memory unit 5302 is configured using an FPGA (Field Programmable Gate Array), but is not limited to this example. For example, a method using an SRAM or a method of integrating the signal conversion unit 5301 and the shared memory unit using a microcomputer may also be adopted.

[0141] The motor control units 531 to 536 execute three types of processing at different cycles: (1) command value acquisition processing, (2) state sharing with other joints, and (3) joint control.

[0142] (1) The command value acquisition process accesses the shared memory of the signal distribution unit and acquires the command value K q , D q , τ cmp , θ d and a process of transmitting the acquired command value to the motor control unit corresponding to the next joint and sharing the command value.

[0143] (2) is the communication process with the front and rear joints (second joints). The latest control result observed by the motor control unit of the own joint up to the previous cycle, that is, the motor rotation angle θ sensed by the sensor unit of the own joint, i and angular velocity θ' i is transmitted to the motor control units (53i-1 and 53i+1) of the front and rear joints (joints adjacent to the own joint). At the same time, the rotation angle θ i―1 and angular velocity θ' i―1 , rotation angle θ i+1 and angular velocity θ' i+1 In this way, the rotation angle θ = (θ1, θ2, θ3, θ4, θ5, θ6) is received as feedback for all joints. T and angular velocity θ'=(θ'1, θ'2, θ'3, θ'4, θ'5, θ'6) T In addition, the information on the states of all the joints shared in this process is written into the shared memory at the same time when the first motor control unit accesses the signal relay / distribution unit to obtain the command value in the process of (1).

[0144] (3) Joint control processing is processing related to the specific control of the robot joints, other than the communication processing of (1) and (2). The command values ​​and other axis feedback amounts obtained by (1) and (2), and the rotation angle θ sensed by the sensor unit of the own joint are used. i and the current joint torque τ measured by the torque sensor i Based on this, the command value τ of the joint torque that the own joint should specifically achieve is d,i Then, the joint torque output τ i The torque command value τ is specified d,i This is a torque servo control process that controls the drive current of the motor so as to follow the

[0145] The execution period of process (1) is T A This is because the update period of the command value parameter is determined by the data transmission period of the robot control device. The execution period of process (2) is set to T G seconds, and T G is T A >T G ≧T S The execution period of process (3) is the servo cycle itself, and is set within a range that satisfies T S seconds, and is performed as fast as the processing unit and sensing mechanism will allow.

[0146] As mentioned above, in order to reduce the time required for control, the cycle of the process in which the motor control unit shares information on the state of other joints (T G ) should be as small as possible, and T G If possible, T S However, if there are issues with the transfer speed, etc., S It may be larger than that.

[0147] According to this embodiment, the motor control unit can share information (feedback amount) related to the state of joints other than the own joint within the servo control unit without going through the robot control device. Specifically, the motor angle θ related to the other joints k and angular velocity θ' k(k=1 to 6, excluding i). As a result, in this embodiment, the required time, which was previously determined by the control cycle of the robot control device 300, can be reduced to, for example, about one-tenth. This makes it possible to perform feedback compensation with little delay, and to perform good force control with effective damping characteristics.

[0148] FIG. 12 is a flowchart illustrating a specific processing procedure executed by the servo control unit 230 in this embodiment. (Step S-5-1: Receiving command value) This is the same processing as step S-4-1 in the second embodiment. (Step S-5-2: Update the number of iterations) This is the same processing as step S-4-2 in the second embodiment. (Step S-5-3: Acquire joint status) This is the same processing as step S-4-3 in the second embodiment. (Step S-5-4: Estimation of joint condition) This is the same processing as step S-4-4 in the second embodiment.

[0149] (Step S-5-5: Communication processing with other axis motor control units, command value acquisition processing) This is the (1) command value acquisition process described above. That is, it is a process in which the command values ​​received from the robot control device 300 are transmitted in order from the base joint to the motor control units corresponding to the peripheral joints, so that the command values ​​relating to the control of each joint are shared by the motor control units of all the joints. First, the motor control unit 531 of the first axis, which is the motor control unit of the first joint arranged on the base end side, accesses the shared memory of the signal relay distribution unit 530 and acquires the command value K q , D q , τ cmp , θ dThen, these command values ​​are transmitted to the motor control unit of the next joint (second axis motor control unit 532) via the communication means. The second axis motor control unit, which has received the command value from the first axis motor control unit, performs the same process and transmits the command value for the next joint. Similarly, the i-th axis motor control unit transmits a command value to the (i+1)-th motor control unit of the next joint, and the process continues until the distal joint (sixth joint) is reached.

[0150] The execution period of this step is T A This is because the update period of the command value parameter is determined by the data transmission period of the robot control device 300. If there is no update of the command value, this step is skipped.

[0151] (Step S-5-5: Communication process 2 with other axis motor control units, sharing of joint status) This is the process of (2) sharing the state with other joints, as mentioned above. The motor control units communicate with each other, and the information on the state of the joints observed by each motor control unit is shared among all joints and the shared memory. The motor control unit of the ith joint (motor control unit 53i of the ith axis) receives information measured by the motor control unit of its own joint (rotation angle θ i , angular velocity θ' i ) to the motor control units (53i-1 and 53i+1) of the front and rear joints (second joints). At the same time, the rotation angle θ i―1 and angular velocity θ' i―1 , rotation angle θ i+1 and angular velocity θ' i+1 The execution cycle is T G seconds.

[0152] (Step S-5-6: Calculation of the interference term) This is the same processing as step S-4-6 in the second embodiment. The interference term of the joint torque command value is updated using the command parameter obtained from the previous joint or shared memory and the other axis feedback amount obtained from the front and rear joints. The update period of this calculation is set equal to the update period of the other axis feedback amount. G Seconds. TG and T S is set to a different period, and if the other axis feedback amount is not updated, this step is also skipped and the interference term is maintained at the same value as at the previous update.

[0153] (Step S-5-7: Calculation of stiffness and damping terms related to the joint) This is the same processing as step S-4-7 in embodiment 2. Based on the command value parameters and the observation results of the own joint, the stiffness and damping terms of the torque command value related to the own joint are updated and output to the torque control unit of the own joint. (Step S-5-8: Calculation of joint torque command) This is the same processing as step S-4-8 in the second embodiment. (Step S-5-9: Execute joint torque control processing) This is the same processing as step S-4-9 in the second embodiment. (Step S-5-10) This is the same processing as step S-4-10 in the second embodiment. (Step S-5-11: Send the joint status to the robot control device) This is the same processing as step S-4-11 in the second embodiment.

[0154] With a robot control device configured in this way, each motor control unit in the servo control unit, which is a servo unit, can calculate and output feedback control amounts based on the motion state of other axes without going through the robot control device, which is an integrated control unit.

[0155] According to the robot control system of this embodiment, the servo control unit, which is a servo unit, can calculate and output feedback control amounts based on the motion states of other axes without going through the robot control device, which is an integrated control unit. The servo unit can acquire joint states (sensor information) and update control laws at a much faster cycle than the integrated control unit, so it is possible to reduce the time required from when the robot device acquires sensor information until the drive unit outputs a control force.

[0156] In the configuration according to this embodiment, the speed of acquiring the other joint states and updating the control law based on them is increased, and in particular, the speed of the interference term τ couple,i It is possible to increase the update speed of the force, which makes it possible to perform force control with higher accuracy and stability.

[0157] In particular, in this embodiment, the signal relay / distribution unit 530 and the motor control units 531 to 536 for each axis arranged corresponding to the joints are each configured as an independent control circuit (e.g., a circuit board on which a microcontroller or microprocessor is mounted). They are interconnected using a bus-type or daisy-chain-type network topology. This makes it possible to adopt a configuration in which each motor control unit has a common design or the motor control unit is mounted on the robot body, thereby increasing the design flexibility of the robot system. By distributing each motor control unit near the joint to be driven, it is possible to improve maintainability by unitizing the joint mechanism and standardizing the design. Furthermore, it is possible to standardize the design of the joints and control board, thereby reducing costs.

[0158] [Other embodiments] The present invention is not limited to the above-described embodiments and examples, and many modifications are possible within the technical concept of the present invention.

[0159] For example, when calculating an interference term for compensating for the dynamic influence of another joint on the own joint, the joints adjacent to both sides of the own joint are treated as the other joints, but this is not limited to this. For example, if the joint located most proximally or most distally is treated as the own joint, it may be sufficient to consider the influence of only one of the adjacent joints.

[0160] A control program capable of executing the control method relating to the servo control operation described above and a computer-readable recording medium storing the control program are also included in the embodiments of the present invention. The functional elements shown in the referenced drawings are functional concepts and do not necessarily have to be physically configured as shown. For example, the specific form of distribution and integration of each functional block is not limited to the illustrated example, and all or part of them can be functionally or physically distributed and integrated in any unit depending on the usage situation, etc.

[0161] The information processing method and information processing device of the present invention can be applied to the control of various machines and equipment, such as industrial robots, service robots, processing machines operated by computer numerical control, etc., in addition to production equipment. For example, the control of machines and equipment that can automatically perform operations such as extension and contraction, bending and stretching, vertical movement, horizontal movement, or rotation, or a combination of these operations, based on information stored in a storage device provided in the control device.

[0162] The robot device of the embodiment can perform a wide variety of tasks, including tasks involving force control operations that require high precision and high response speed, such as in manufacturing methods for articles, including processes such as assembling, removing, transporting, processing, and painting parts.

[0163] The present invention can also be realized by supplying a program that realizes one or more functions of the embodiments to a system or device via a network or a storage medium, and having one or more processors in the computer of the system or device read and execute the program.The present invention can also be realized by a circuit (e.g., ASIC) that realizes one or more functions. [Explanation of symbols]

[0164] 100···Robot device / 200···Robot / 201-206···Actuator / 211-216··Motor / 221-226··Sensor unit / 230···Servo control unit / 251···Robot arm / 252···Robot hand / 300···Robot control device / 301···CPU / 330···Program / 322···Storage device / 400···Teaching Pendant / 501 ··· Force teaching data / 502 ··· Position teaching data / 503 ··· Robot model / 504 ··· Force control unit / 505 ··· Trajectory generation unit / 506 ··· State estimator / 507 ··· Joint target value generation unit / 508 ··· Environment model / 530 ··· Signal relay distribution unit / 531 to 536 ··· Motor control unit / 541 to 546 ··· Angle sensor / 551 to 556 ··· Torque sensor

Claims

1. A control method for a robot system including a robot including a joint having a drive unit, an angle sensor that detects the angle of the joint, and a torque sensor that detects the torque of the joint, a first control unit that controls the drive unit, and a second control unit that outputs a command value related to an operation of the drive unit to the first control unit, the second control unit outputs information about the impedance of the joint to the first control unit; the first control unit controls the drive unit using information related to the impedance, a value from the angle sensor, and a value from the torque sensor; the second control unit outputs information about the impedance to the first control unit in a first period; the first control unit controls the drive unit at a second period shorter than the first period; A control method comprising:

2. the information about the impedance is stiffness information of the joint and a damping coefficient of the joint; the second control unit outputs an angle value to be realized at the joint to the first control unit; the first control unit obtains a value of torque to be generated by the drive unit based on the stiffness information, the damping coefficient, and the angle value; 2. The control method according to claim 1.

3. the second control unit outputs to the first control unit a torque value at a joint other than the joint and a torque value that compensates for dynamics of the robot; the first control unit obtains a value of the torque to be generated by the drive unit based on the stiffness information, the damping coefficient, the angle value, the torque value at the other joint, and a torque value that compensates for dynamics of the robot; 3. The control method according to claim 2.

4. the first control unit acquires information representing a state of the other joint and outputs the information to the second control unit; the second control unit calculates an interference term that compensates for a dynamic effect of the other joint on the joint, using information representing a state of the other joint, and outputs the interference term to the first control unit.

4. The control method according to claim 3.

5. the first control unit acquires information representing a state of the other joint, calculates an interference term that compensates for a dynamic effect of the other joint on the joint, and controls the drive unit using the interference term; 4. The control method according to claim 3.

6. The first control unit that controls the joint and the first control unit that controls the other joint are capable of communicating with each other.

6. The control method according to claim 4 or 5.

7. The first control unit that controls the joint and the first control unit that controls the other joint are capable of communicating with each other via a shared memory.

7. The control method according to claim 4, wherein the control method is a control method for controlling a power supply.

8. the drive unit is a motor, and the first control unit includes a servo control unit having the second period as a control period; 8. The control method according to claim 1, wherein the control method is a control method for controlling a power supply.

9. the servo control unit samples the value from the angle sensor and the value from the torque sensor every second period; 9. The control method according to claim 8.

10. the servo control unit transmits the sampled value from the angle sensor and the sampled value from the torque sensor to the second control unit in the first period; 10. The control method according to claim 8 or 9.

11. the first control unit acquires information representing a state of the joint for each second period and transmits the information to the second control unit for each first period; 11. The control method according to claim 1 ,

12. The second control unit receives force teaching data relating to the force control of the joint and a force teaching instruction data to be executed by the robot. and outputting position teaching data specifying a motion to be performed to the first control unit.

12. The control method according to any one of claims 1 to 11.

13. the angle sensor is a sensor that detects the angle of the drive unit or a sensor that detects the angle of the joint; 13. The control method according to any one of claims 1 to 12.

14. A control method for a robot system including a robot including a first joint having a first drive unit and a first angle sensor that detects an angle of the first joint, a second joint having a second drive unit and a second angle sensor that detects an angle of the second joint, a first control unit that controls the first drive unit, and a second control unit that controls the second drive unit, the second control unit outputs a value of the second angle sensor to the first control unit; the first control unit controls the first drive unit using information related to impedance at the first joint, a value from the first angle sensor, and a value from the second angle sensor. A control method comprising:

15. A robot system comprising: a robot including a joint having a drive unit, an angle sensor that detects the angle of the joint, and a torque sensor that detects the torque of the joint; a first control unit that controls the drive unit; and a second control unit that outputs a command value related to an operation of the drive unit to the first control unit, the second control unit outputs information about the impedance of the joint to the first control unit; the first control unit controls the drive unit using information related to the impedance, a value from the angle sensor, and a value from the torque sensor; the second control unit outputs information about the impedance to the first control unit in a first period; the first control unit controls the drive unit at a second period shorter than the first period; A robot system characterized by:

16. the information about the impedance is stiffness information of the joint and a damping coefficient of the joint; the second control unit outputs an angle value to be realized at the joint to the first control unit; the first control unit obtains a value of torque to be generated by the drive unit based on the stiffness information, the damping coefficient, and the angle value; 16. The robot system according to claim 15.

17. the second control unit outputs to the first control unit a torque value at a joint other than the joint and a torque value that compensates for dynamics of the robot; the first control unit obtains a value of the torque to be generated by the drive unit based on the stiffness information, the damping coefficient, the angle value, the torque value at the other joint, and a torque value that compensates for dynamics of the robot; 17. The robot system of claim 16.

18. the first control unit acquires information representing a state of the other joint and outputs the information to the second control unit; the second control unit calculates an interference term that compensates for a dynamic effect of the other joint on the joint, using information representing a state of the other joint, and outputs the interference term to the first control unit.

18. The robot system of claim 17.

19. the first control unit acquires information representing a state of the other joint, calculates an interference term that compensates for a dynamic effect of the other joint on the joint, and controls the drive unit using the interference term; 18. The robot system of claim 17.

20. The first control unit that controls the joint and the first control unit that controls the other joint are capable of communicating with each other.

20. The robot system according to claim 18 or 19.

21. The first control unit that controls the joint and the first control unit that controls the other joint are capable of communicating with each other via a shared memory.

21. The robot system according to claim 18, wherein the robot system is a robotic system.

22. the drive unit is a motor, and the first control unit includes a servo control unit having the second period as a control period; 22. The robot system according to claim 15, wherein the robot system is a robotic system.

23. the servo control unit samples the value from the angle sensor and the value from the torque sensor in the second period; 23. The robotic system of claim 22.

24. the servo control unit transmits the sampled value from the angle sensor and the sampled value from the torque sensor to the second control unit in the first period; 24. The robot system according to claim 22 or 23.

25. the first control unit acquires information representing a state of the joint in the second cycle and transmits the information to the second control unit in the first cycle; 25. The robot system according to claim 15, wherein the robot system is a robotic system.

26. the second control unit outputs, to the first control unit, force teaching data relating to force control of the joint and position teaching data specifying a motion to be executed by the robot; 26. The robot system according to claim 15, wherein the robot system is a robotic system.

27. A robot system comprising: a robot including a first joint having a first drive unit and a first angle sensor that detects an angle of the first joint; a second joint having a second drive unit and a second angle sensor that detects an angle of the second joint; a first control unit that controls the first drive unit; and a second control unit that controls the second drive unit, the second control unit outputs a value of the second angle sensor to the first control unit; the first control unit controls the first drive unit using information related to impedance at the first joint, a value from the first angle sensor, and a value from the second angle sensor. A robot system characterized by:

28. A method for manufacturing an article, comprising controlling the robot using the control method according to any one of claims 1 to 14, and manufacturing the article.

29. A control program for causing a computer to execute the control method according to any one of claims 1 to 14.

30. A computer-readable recording medium having the control program according to claim 29 recorded thereon.

Images