CONTROL METHOD, ROBOT SYSTEM, PRODUCTION METHOD OF ARTICLE, 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] This invention relates to robotic devices and the like. [Background technology]

[0002] In recent years, the applications of articulated robotic manipulators have expanded, and their use is growing in fields that require robots to perform flexible movements, such as collaborative work with humans and assembly of parts in industrial product manufacturing. Such robots require stable and broadband force control functions to adapt to external forces, so 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] In such robot joints, the measurement mechanism for output torque (or translational force, thrust) (torque sensor, force sensor) is typically installed between the output of the drive unit, which consists of a motor and reduction mechanism, and the drive shaft of the output joint, via a bearing that rotatably supports the output joint.

[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 its motion control system, which consists of (1) a joint control system and (2) an arm control system. (1) The joint control system is a servo system responsible for controlling the state of the joints, and for example, it performs control to adjust the torque output by the joints to a value specified by the higher-level system, (2) the arm control system. (2) The arm control system is an overall control unit that controls the motion and / or both of the entire robot manipulator (robot arm) to a desired state. For example, it is responsible for the function of coordinating the control of each joint so that the movement and force of the end-effector reach 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 [Overview of the project] [Problems that the invention aims to solve]

[0008] In conventional configurations, the torque that a robot's joints should output (joint torque command value) is calculated using a dynamics model. This calculation is performed in the arm control system, which is the overall control unit of the robot device, based on information about the robot's state, such as displacement, velocity, and acceleration, that is fed back to the controller. This calculation includes inverse dynamics calculations, which determine the torque that a robot joint should output from the robot's motion state, i.e., joint displacement, velocity, and acceleration.

[0009] The joint torque command values ​​calculated in the arm control system, which is the overall control unit of the robot device, are transmitted to the servo control unit, which is the joint control system. The servo control unit receives the transmitted joint torque command and drives and controls the motors of each joint so that each joint realizes the joint torque command value. More specifically, the servo control unit performs feedback control based on the measured joint torque and the acquired torque command value, so that they match.

[0010] In a robot control system with this configuration, 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 require computationally intensive calculations such as inverse dynamics calculations, as well as control processing for communication interfaces, including communication between the main control unit and servo units, and communication with external robot equipment. Therefore, it is difficult to speed up the control cycle simply by improving machine specifications (such as increasing the clock frequency of the computing unit).

[0011] If the control cycle becomes long due to the large time required for calculations, 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. As a result, in addition to poor stability during free motion, it becomes difficult to position the robot to the desired location, it becomes difficult to properly control the rebound upon contact, and trial and error is required when teaching the robot's movements. In some cases, only skilled operators proficient in robot teaching could properly utilize the robot in force control mode.

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

[0013] A first aspect of the present invention is a robotic device comprising: a joint having a drive unit; a drive control unit for controlling the force generated by the drive unit; and a motion command unit for outputting commands relating to the operation of the joint to the drive control unit, wherein the motion command unit acquires information relating to the motion state of the robotic device, calculates stiffness information of the joint and / or a damping coefficient of the joint, outputs a command including the calculation result to the drive control unit, and the drive control unit uses the command to control the force generated by the drive unit.

[0014] Furthermore, a second aspect of the present invention is a control method for a robot device comprising: a joint having a drive unit; a drive control unit for controlling the force generated by the drive unit; and an operation command unit for outputting commands relating to the operation of the joint to the drive control unit, wherein the operation command unit acquires information relating to the motion state of the robot device, calculates stiffness information of the joint and / or the damping coefficient of the joint, outputs a command including the calculation result to the drive control unit, and the drive control unit uses the command to control the force generated by the drive unit. [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 corresponding control force can be reduced, and force control with excellent responsiveness while maintaining high stability can be achieved. [Brief explanation of the drawing]

[0016] [Figure 1] A perspective view showing the schematic configuration of the robot device according to Embodiment 1. [Figure 2] A block diagram showing the configuration of the robot control device according to Embodiment 1. [Figure 3] A block diagram showing the configuration of the control system of the robot device according to Embodiment 1. [Figure 4] A block diagram showing the configuration of the motor control unit for the i-th axis in Embodiment 1. [Figure 5]A flowchart illustrating the procedure by which the robotic device performs assembly work in Embodiment 1. [Figure 6] A flowchart illustrating the processes performed by the robot's force control system when executing a subtask in Embodiment 1. [Figure 7] A flowchart illustrating the joint control processing procedure performed by the servo control unit in Embodiment 1. [Figure 8] A flowchart illustrating the processes performed by the robot's force control system when executing a subtask in Embodiment 2. [Figure 9] A block diagram showing the configuration of the servo control unit according to Embodiment 2. [Figure 10] A flowchart illustrating the joint control processing procedure performed by the servo control unit in Embodiment 2. [Figure 11] A block diagram showing the configuration of the servo control unit according to Embodiment 3. [Figure 12] A flowchart illustrating the specific processing steps performed by the servo control unit in Embodiment 3. [Modes for carrying out the invention]

[0017] With reference to the drawings, embodiments of the present invention, such as a robot device, a control method for the robot device, etc., will be described. In the drawings referenced in the following description of embodiments, elements indicated by the same reference numeral will have the same function unless otherwise specified. The embodiments described below are illustrative examples, and those skilled in the art may modify the details of the configuration as appropriate without departing from the spirit of the present invention.

[0018] [Embodiment 1] Figure 1 is a perspective view showing the schematic configuration of a robot device according to this embodiment. As shown in Figure 1, the robot device 100 comprises a multi-joint robot 200 and a robot control device 300 that controls the movement of the robot 200. The robot device 100 also includes a teaching pendant 400, which serves as a teaching device for operating the robot control device 300 and programming its movements.

[0019] The robot 200 comprises a multi-jointed robotic arm 251 and a robotic hand 252 as an end effector attached to the tip of the robotic arm 251. The base of the robotic arm 251 is fixed to a base B. The robotic hand 252 grasps objects (such as assembly parts or work tools).

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

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

[0022] Coordinate system To represents the coordinate system fixed to the base of the robot arm 251, i.e., the pedestal B, while coordinate system Te represents the coordinate system fixed to the end effector of the robot 200 (the tip of the robot arm 251). The robot's position and orientation can also be expressed in various other coordinate systems.

[0023] Actuators 201-206 each include electric motors 211-216 that drive each of the joints J1-J6, and sensor units 221-226 connected to each joint. Sensor units 221-226 have position sensors (angle sensors) that detect the position (rotation angle) of joints J1-J6 and torque sensors that detect the torque of joints J1-J6. Actuators 201-206 are equipped with a reduction mechanism (not shown) and are supported by one of the links 2100-2206. The electric motors drive the reduction mechanism directly or via a power transmission mechanism such as a timing belt or gears, and drive the links 2100-2206. The actuator units do not necessarily need to be equipped with a reduction mechanism; the electric motors may directly support / drive the links or torque sensors.

[0024] Inside the robot arm 251 is a servo control unit 230, which acts as a drive control unit that controls the driving of the actuators (motors) of each joint. Based on the input torque command value, the servo control unit 230 controls the driving of actuators 201 to 206 by controlling the drive current of the electric motor 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 composed of a single control device, but the servo control unit may be composed of multiple control boards or control devices, each corresponding to one of the servo motors 211 to 216. Also, although the servo control unit 230 is located inside the robot arm 251, it may be located inside the housing of the robot control device 300.

[0026] Next, the robot control device 300 (operation command unit) will be described. Figure 2 is a block diagram showing the robot control device of the robot device according to this embodiment. The robot control device 300 includes a central processing unit (CPU) 301 as a control unit (processing unit). The robot control device 300 also includes a ROM (Read Only Memory) 302, a RAM (Random Access Memory) 303, and an HDD (Hard Disk Drive) 304 as storage units. The robot control device 300 also includes a recording disk drive 305 and various interfaces 306 to 309.

[0027] The CPU 301 is connected to the ROM 302, RAM 303, HDD 304, recording disk drive 305, and various interfaces 306-309 via the bus 310. The ROM 302 stores basic programs such as the 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 external sources. It stores the program 330 that causes the CPU 301 to perform calculations described later, as well as robot models and environment models used for robot motion planning and motion control. The CPU 301 can record various information to the recording disk 331 and read various 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 the program 330 recorded (stored) in the HDD 304. The teaching pendant 400 is connected to interface 306, and the CPU 301 receives input from the teaching pendant 400 via interface 306 and bus 310. Interface 307 is connected to monitor 321, and the CPU 301 can display various information related to the control process on monitor 321 and provide it to the user. Interface 308 is connected to storage device 322, and the CPU 301 can store various programs and data in storage device 322 and read them from storage device 322.

[0029] Interface 309 is connected to a servo control unit 230 that controls servo motors 211 to 216. The CPU 301 acquires detection results from sensor units 221 to 226 via the servo control unit 230, interface 309, and bus 310. The CPU 301 transmits command values ​​for each joint to the servo control unit 230 via bus 310 and interface 309 at predetermined time intervals.

[0030] Figure 3 is a block diagram showing the configuration of the control system of the robot device according to this embodiment. The CPU 301 of the robot control device 300 (motion command unit) functions as a force control unit 504, a trajectory generation unit 505, a joint target value generation unit 507, a state estimation unit 506, etc., by executing a program 330 that includes a robot motion control program. In Figure 3, functional elements necessary to explain the features of this embodiment are represented as functional blocks, but general functional elements not directly related to the problem-solving principle of the present invention are omitted from the description. Furthermore, each functional element shown in Figure 3 is a functional concept and does not necessarily have to be physically configured as shown. For example, the specific forms of distribution and integration of each functional block are not limited to the examples shown, and all or part of them 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 an HDD 304, and the HDD 304 stores a program 330 for realizing functional blocks such as servo control. However, the embodiment is not limited to this. The program 330 may be recorded on any recording medium that is computer-readable. For example, a ROM 302, a recording disk 331, or an external storage device (not shown) may be used as the recording medium for supplying the program 330. To give specific examples, flexible disks, optical disks, magneto-optical disks, magnetic tapes, non-volatile memory such as USB memory, SSDs, etc., can be used as recording media.

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

[0033] Each of the six joints J1 to J6 is individually equipped with a motor and a sensor unit. For example, joint Jn is equipped with a motor 21n and a sensor unit 22n, and the sensor unit 22n is equipped with a position sensor 55n and a torque sensor 541n (n is any integer from 1 to 6). For the sake of clarity, in the following explanation, individual correspondences may not be distinguished, and they may be grouped together as follows: "Sensor units 221 to 226 have angle sensors 541 to 546 that measure the position or angle of the joint, and torque sensors 551 to 556." The position sensor detects the rotational position of servo motors 211-216, or the angle or position of joints J1-J6.

[0034] Please note that while the various parameters mentioned in the following explanation may be represented in vector format in the diagrams, as shown in the example below, this does not mean that the diagrams depict content different from what is explained. (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-τ6 acting on joints J1-J6: τ=(τ1,τ2,τ3,τ4,τ5,τ6) T

[0035] Furthermore, in the following explanation, when indicating that each parameter is an estimated value, the text may be followed by "_hat" and the diagrams may show a hat symbol next to each parameter name. Also, when indicating that each parameter represents a displacement, such as the difference between the command value and the current value, the text may be followed by "_tilde" and the diagrams may show a tilde symbol next to each parameter name.

[0036] In this embodiment, an optical rotary encoder or a magnetic rotary encoder connected to the rotation axis of the servo motors 211 to 216 is used as a position sensor to detect the rotation angles θ1 to θ6 of the servo motors 211 to 216. However, the configuration of the position sensor is not limited to this example.

[0037] The rotation angles of joints J1 to J6 can be calculated based on the detection results of the rotation angles θ1 to θ6 of servo motors 211 to 216 and the reduction ratio of the reduction mechanism in the drive system, and estimated joint rotation angles q1_hat to q6_hat can be obtained. Alternatively, angle sensors 541 to 546 that directly measure the output rotation angle of the joint may be placed at each joint to detect the rotation angles q1 to q6 of joints J1 to J6. Torque sensors 551 to 556 detect the joint torques τ1 to τ6 acting around joints J1 to J6.

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

[0039] The values ​​representing the state of joints J1 to J6 are acquired by the servo control unit at predetermined intervals. That is, these values ​​are sampled at sampling intervals Ts (every second cycle) to keep pace with the servo control cycle. Furthermore, if estimated values ​​are calculated based on measured values, the sampling interval Ts (second cycle) is set so that the entire process, from measurement to calculation, fits within the sampling interval Ts. The sampling interval Ts, which is also the control cycle of the servo control unit, is set to, for example, 100 μs (microseconds).

[0040] The values ​​representing the state of joints J1 to J6, acquired by the servo control unit, are transmitted to the robot control device 300 via the signal relay and distribution unit 530 at predetermined transmission cycles. The transmission cycle is determined by the sampling cycle T of the robot control device 300. A This can also be called the (first cycle), and it is set according to the time required for the robot control device 300 to perform information processing (calculation) related to one control cycle of the robot. Sampling period T A The first period is, for example, 1 ms (millisecond). The acquired values ​​representing the state of joints J1 to J6 are input to the state estimation unit 506.

[0041] The state estimation unit 506 uses the input values ​​representing the states of joints J1 to J6 and the robot model 503 to calculate (estimate) the position x of the point of interest, the displacement x_tilde of the position, and the force moment Fe acting on the point of interest. The robot model 503 is composed of, for example, a geometric model and a mechanical model of the robot arm.

[0042] The point of focus is, for example, the end-effector tool center point (TCP) of a robot hand. In this case, the displacement x_tilde of the TCP is a 6-dimensional vector representing the difference between the commanded end-effector position (commanded position, equilibrium point of the end-effector position) and the measured current position. Furthermore, the force moment Fe acting on the point of focus can be expressed as a vector as shown below. (4) Force moment Fe acting at the point of operation:

number

[0043] The information processing performed by the state estimation unit 506 further includes processing to estimate the state of the robot necessary 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., assembly sequence) to be performed by the robot. The former includes, for example, the estimation of the end-effector velocity x' and acceleration x''. The latter includes, for example, the detection of the contact state between the robot and a workpiece or other object. The state estimation unit 506 outputs the results obtained from these information processing operations to the force control unit 504, etc.

[0044] Here, we will describe the force teaching data 501 and position teaching data 502 stored in the storage device 322 of the robot control device 300. The operator can teach the robot to perform actions and create a robot action program using a man-machine interface such as a teaching pendant 400. The operator can teach the robot to perform a task by describing the procedure (work sequence) of the task to be performed, the subtasks that make up the task, the completion conditions for the task and subtasks, and the state transition methods between subtasks, and inputting teaching data. The teaching data includes force teaching data 501, which are parameters related to force control, and position teaching data 502, which specifies the motion to be performed by the robot.

[0045] The force teaching data 501 is composed of the stiffness Kd (stiffness information), damping Dd (damping information) applied to the robot's end, the command value Fd of the contact force for the assembly operation, and the like. The stiffness Kd and damping Dd applied to the robot's end are 6-row and 6-column matrices specified according to the coordinate system described above, and are specified according to the desired contact state. For example, when pressing the grasped workpiece against the environment, the stiffness in the pressing direction is set small to prevent an 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 related to the position and orientation of the robot's end at the key frame of the taught motion, that is, the position p of the end expressed in three-dimensional space d,i and the orientation R d,i , and the target angle q of the joint d,i are input (i is the teaching point number / key frame number). Incidentally, the position p of the end d,i and the orientation R d,i may be expressed as follows. (5) Position p of the end d,i :

Number

Number

[0047] The trajectory generation unit 505 reads out the position p of the end d,i and the orientation R d,i , the target angle q of the joint d,i and the like from the position teaching data 502, and smoothly connects them to generate the time-series end target position xd(t) as the end command value. Incidentally, t represents time, and the end target position x d(t) is the sampling period T of the robot control device 300. A This is a vector sequence that has a value every (every first period).

[0048] The joint target value generation unit 507 performs inverse kinematic analysis calculations based on the robot model 503 for the end-effector target position xd(t) input from the trajectory generation unit 505. Then, it generates the joint target position q, which is the target value of the joint angle. d (t), the joint angular velocity q, which is its first derivative. d '(t), the second derivative of the joint angular acceleration q d The function ''(t) is calculated and the result is output to the force control unit 504.

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

[0050] Furthermore, robot model 503 may include a load model in addition to the robot's geometric and mechanical models. Here, the robot's geometric model is a shape model that includes geometric information such as the dimensions of the link system that makes up the robot itself and the arrangement of joints connecting the links. The robot's mechanical model is a mechanical model that is composed of, for example, the mass, moment of inertia, and position of the center of gravity of the links that make up the robot itself. The load model is a load model that includes, for example, an end effector attached to the robot's end effector or a workpiece grasped by the robot.

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

[0052] Furthermore, the end-effector target position x at the current time, generated by the trajectory generation unit 505, is also generated. d (t) and the joint target position q at the current time, generated by the joint target value generation unit 507. d (t) is input. Furthermore, the force control unit 504 receives x_hat, which relates to the robot's current state, as well as x'_hat, x_tilde, F_hat, q_hat, and q'_hat, which are estimated values ​​of velocity, and q'_hat, which is an estimated value of angular velocity, all of which are input to the state estimation unit 506.

[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. Transmission can be done via Ethernet, for example, and the transmission period is the sampling period of the robot control device 300 (the time required for the calculation to be completed reliably) T A This corresponds to 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: namely, the dynamic compensation term τ of the torque command value for each joint. cmp And the coordinate system T in task space e The stiffness K specified by d The stiffness command value K at the joint level, which is obtained by transforming (mapping) it into joint space. q (Stiffness information) and the coordinate system T of the task space. e Damping D specified by d The joint-level attenuation command value D is obtained by transforming (mapping) it into joint space. q (Damping coefficient) and motor angle command value Q d (t) and the command value F of the contact force at the fingertips level. d τ converted to joint-level torqued,ext And, one or more of these. In this embodiment, for example, all of this information can be output to the servo control unit 230 as a command value.

[0055] The servo control unit 230 receives command values ​​transmitted from the robot control device 300. The signal relay distribution unit 530 distributes the received command values ​​to each motor control unit 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 d,i Calculate (i is the joint number, one of 1 to 6).

[0056] Then, the output torque of each joint is equal to the torque command value τ d,i To follow (torque command value τ) d,i and joint torque τ i The motor drive current is controlled by calculating the feedback compensation amount for joint torque (so that the difference is minimized) and controlling the motor's energization. The above calculation processes for calculating the command value of joint torque and the feedback compensation amount involve time changes in the state of the joint that cannot actually be measured (for example, θ). i 'ya τ i This may include processes such as estimating ').

[0057] Figure 4 is a block diagram showing the configuration of the motor control unit for the i-th axis (i.e., the i-th joint from the base) in this embodiment. Each joint's motor control unit has a coefficient setting unit 5311, a joint stiffness / damping control unit 5312, and a torque control unit 5313, respectively.

[0058] The coefficient setting unit 5311 receives the joint stiffness K related to joint i from the signal relay distribution unit 530. q,i,i And the damping coefficient D related to joint i q,i,i The system receives these and sets them in the joint stiffness / damping control unit 5312. q,m,n The joint stiffness matrix K is an N x N matrix (where N is the number of joints).q This represents the m-row and n-column components of the joint stiffness K. q,i,i This is the joint stiffness matrix K q These are the diagonal elements.

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

[0060] The joint stiffness / damping control unit 5312 receives 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 the following equation 4, the stiffness and damping term τ related to the joint (joint i) of the torque command value is calculated. pd,i The calculation is performed and output to the torque control unit 5313. The time required for the calculation, i.e., the period for outputting the calculation result, is the sampling period of the servo control unit, T. s It's seconds.

number

[0061] The torque control unit 5313 outputs the joint torque τ i However, the specified torque command value τ d,i The motor drive current I follows i (motor output torque τ) m,i This is a torque servo control system that controls the energization of the joint torque. d,iThe calculation is performed as follows: The torque control unit 5313 receives the torque command value τ related to the stiffness and damping of its own joint, which is calculated by the joint stiffness / damping control unit 5312. pd,i The period T s The 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 (inertia, centrifugal force, Coriolis force, and the effects of gravity, etc.) cmp,i update period T A It receives the data in seconds. Then, it performs the following calculation (number 5) to determine the command value τ of the joint torque that each joint should specifically achieve. d,i to T s It is calculated on a second-by-second basis.

number

[0062] Next, the torque control unit 5313 performs T s Torque sensor measurements are taken every second to determine joint torque τ i It senses and T s Motor output torque τ per second m,i (=K T I i However, K T I is the torque constant of the motor. i The following calculation process is performed to update the motor current.

number

[0063] Equation 6 is the command value τ of the joint torque. d,i and current value τ i The torque (feedback compensation amount) that the motor should output is given as an argument τ d,m,i This is a function that determines [something]. While various specific configurations are possible, the most basic is 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 the motor current control system, which is located as a lower-level control loop. The motor current control system then checks if the motor current actually supplied is the specified value (I i The motor drive is controlled so that the result is as follows.

[0065] Next, with reference to Figures 5 to 7, the specific operation procedure of the robot's force control system in this embodiment will be described. This description will use the example of an assembly robot performing assembly work in a product production site. The product to be manufactured will be assembled into a product assembly by performing multiple processes (tasks).

[0066] In this embodiment, the assembly process is described assuming that the robot grasps the part workpiece with an end effector, assembles the workpiece into the product assembly, and pushes the workpiece to the completed assembly position. For example, this process involves grasping a timing pulley with a keyway with a hand, inserting it into the assembly-side shaft of the product assembly which already has a parallel key installed, and inserting it to the completed insertion position (when the pulley abuts against the spigot portion of the shaft).

[0067] As shown in the flowchart of Figure 5, one assembly process in this embodiment consists of N units (N steps) of operation (subtasks). Each operation (operation step 1 to N) is, for example, (1) picking the workpiece (part removal operation by gripping the workpiece with the end effector), (2) moving the robot to the assembly start position, (3) installing the workpiece into the assembly, (4) releasing the end effector, and (5) retracting the robot (return operation).

[0068] The above subtask (3) is further comprised of (3-1) approaching the opening of the hole, (3-2) coaxial alignment between the workpiece and assembly using force sensing control, (3-3) a probing operation to adjust and match the phase of the parallel key and keyway, and (3-4) insertion operation until the pulley abuts against the spigot portion of the shaft / detection of completion of assembly.

[0069] Having completed the previous process (the component assembly process preceding this process), the robot begins the current process. It reads a program describing the actions of the current process stored in its memory unit, reads the teaching data, and starts executing the robot's action program for the current process. The program describes the robot's action procedure, the completion conditions for each action step, and the state transition methods between steps. The teaching data includes force teaching data 501 and position teaching data 502 (step S-1-1).

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

[0071] Next, the robot executes a step (step S-1-3) to create a motion plan for the robot based on the recognition results and teaching data. In step (S-1-3), the robot plans the working state that it should ultimately achieve in each motion step (subtask) based on the teaching data, the sequence of actions for the current task, the method of state transitions between subtasks, and the completion conditions for the task and subtasks. The working 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 robot plans the end-effector target position x that it should refer to at each sampling time in each subtask. d (t), contact condition, contact force F d , stiffness K at the reference point d Damping D d This is generated.

[0072] In each step from step S-1-4 onward, the robot is controlled to achieve planned movements and contact states in order to perform specific assembly tasks such as inserting parts into assembly axes, and transitions between each operation according to planned transition / 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 the states of free motion in the air, control of contact between parts (control of collision processes), and control of contact forces (control of quantitative pressing force).

[0074] After repeatedly executing these unit movements 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). Specifically, the determination is made based on the processing results of the state estimation unit based on internal sensors (such as force information based on torque sensors) and external sensors such as vision, which determine the contact state between the workpiece and assembly, the positional relationships between parts, etc.

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

[0076] Figure 6 is a flowchart illustrating a series of processes performed by the robot's force control system when executing each subtask described in Figure 5 in this embodiment, and shows the processes performed by the robot control device 300, which is the arm control system. The arm control system updates information to be transmitted to the joint control system, i.e., the joint servo control unit, which is a control system lower than the arm control system, in order to realize the state planned by the motion planning unit (steps S-2-1 to S-2-5). Then, it transmits this information to the servo control unit (step S-2-6) and receives the processing results (sensor feedback values) performed by the servo control unit, which is a lower-level 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 This process is repeated for seconds until each subtask is completed (step S-2-11: YES).

[0077] Before explaining the specific procedures for the processes and calculations performed at each step, we will introduce the mathematical model of robot dynamics (dynamic model, equations of motion). As is well known, the equations of motion for a 6-degree-of-freedom articulated robot that takes joint flexibility into account are expressed as shown 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 No. 8 respectively represent the dynamics of the link system and the motor rotor system, and they are connected by the joint torque τ.

[0079] Step (S-2-1) is a process in which the robot control system calls 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 to be achieved is the hand target position x at the target position of the specified reference point d (t), the contact force F d , the stiffness K at the reference point d , the damping D d .

[0080] Step S-2-2 is a kinematic operation process in which the robot control device 300 executes an inverse kinematic analysis operation based on the robot model 503 with the hand target position x d (t) as the target trajectory and calculates the target state at the joint level at the current time t1.

[0081] That is, the joint target value generation unit 507 calculates the speed x d ’(t), the acceleration x d ’’(t), etc. based on the input hand target position xd(t). Furthermore, the target state of the joint required for the calculation of the subsequent control amount, that is, the joint target position q d (t1), the joint angular velocity q d ’(t1), the joint angular acceleration q d ’’(t1), etc. are calculated by the following mathematical formulas.

Equation

Equation

Equation

number

number

[0082] In step S-2-3, the following calculations are performed based on the processing results of step S-2-2, the robot's joint state, the dynamics model (equations of motion) in the robot's joint space, and the dynamics parameters: namely, the dynamical compensation term τ for the torque command value for each joint. cmp Calculate. τ cmp This is a nonlinear feedforward term that compensates for the joint torque required for the robot's own movement, and like the number of joints, it is a 6-dimensional vector.

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) is calculated. e ) specified stiffness K d Damping D d Each of these is mapped to the joint space, and the resulting joint-level stiffness command value K q and joint level damping command value D q The following process is performed to convert it to the following:

number

[0084] The subsequent step S-2-5 generates a torque τ that is proportional to the displacement from the equilibrium point specified by impedance control and proportional to the velocity, creating an elastic restoring force. imp =τ pd +τ couple Among them, the component τ represents interference between joints. couple,i Calculate τ. pd This will be discussed later.

number

number

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

number

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

number

[0087] Assuming that x_tilde and x'_tilde are infinitesimal, we can use the differential kinematic relationship dx=J(q)dq to τ imp This can be transformed as follows:

number

[0088] Thus, τ is responsible for generating the elastic restoring force and damping force of mechanical impedance control. imp The feedback unit τ of the state of the joint pd And, as shown in equation 30, the feedback law (interference term, cross-axial feedback) τ of the state of a joint other than one's own joint couple It can be expressed as the sum of the numbers, as shown in the number 35.

number

[0089] In step S-2-5, the calculation cannot be performed using only the state of the joint itself, that is, the τ term related to interference from other joints. couple The robot control device 300 performs the calculation. The above is an overview of the command value generation process performed by the robot control device 300.

[0090] In step S-2-6, the result of the calculation in the above step is τ cmp,i , τ couple,i , θ d,i , K q,i,i , D q,i,i This value is transmitted from the robot control device to the servo control unit via Ethernet or similar means as a command value to the motor control unit of the i-th joint (i=1···6).

[0091] Step S-2-7 is the step in which the servo control unit, having received the transmitted command values, senses the state of each joint and manipulates the driving force (driving current) of each motor to specifically control the robot's joints. The specific processing in the servo control unit will be described later with reference to Figure 7.

[0092] In the subsequent step S-2-8, the robot control device receives the control results from the servo control unit via a communication means such as Ethernet, similar to the transmission process. The main data received are the motor angle θ (joint angle q) and joint torque τ, which are the results of sensing the joints.

[0093] In step S-2-9, the robot control device inputs the received joint state (θ, τ, etc. above) into the state estimation unit and estimates the state necessary for signal generation for robot motion control, such as the robot's current position x and contact force F. e We estimate this.

[0094] Step S-2-10 is the process for determining the completion of the current task. 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. By doing so, the progress of the assembly task in the currently planned assembly task is understood, and it is determined whether the current task is completed or not. If the completion determination is YES, the process transitions to the next planned motion task; if NO, it returns to Step S-2-1 and performs the calculation of motion control for the next time step. The update cycle period in this series of processes is the control cycle of the robot control device, T A It's seconds.

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

[0096] (Step S-3-2: Updating the number of iterations) The variable k, which stores the number of iterations, is incremented (k ← k + 1), and the number of times the joint control process in the servo control unit has been executed is counted. The joint control process in the servo control unit is executed repeatedly until the next command value is transmitted from the robot control device. The number of iterations k is cleared (k ← 0) when the servo control unit receives the command value. Subsequent processing is executed as quickly as possible within the limits allowed by the processing unit and sensing mechanism, and its update cycle is T S It's seconds.

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

[0098] (Step S-3-3: Obtaining joint status) The subsequent steps involve processing by each motor control unit. The motor control unit accesses the sensor units located at each joint and measures 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 This is obtained. Step S-3-3 is a sensory feedback process to the motor control unit.

[0099] (Step S-3-4: Estimation of joint condition) This process estimates the state of the joint and obtains the estimated values ​​necessary for subsequent processing. In steps S-3-5 (calculation of the command value of the joint torque) and S-3-7 (calculation process to calculate the feedback compensation amount for servo control of the joint torque), the time change value of the joint state, i.e., the motor rotation speed θ', is obtained. i (=dθ i / dt) and torque change rate τ' i This becomes necessary. However, these cannot be measured directly. On the other hand, numerical differentiation (pseudodifferentiation) that records the state at the previous time and takes the difference between that and the current value becomes problematic due to numerical noise and phase lag. Therefore, observers using dynamical models or probabilistic models are used to estimate the state θ'. i _hat, τ' i The process to find _hat is executed.

[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 terms τ related to the joint (joint i) of the torque command value are as follows: pd,i The result is calculated and output to the torque control unit.

number

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

number

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

[0103] The latest τ updated in the processing up to step S-3-6 d,i , τ i、 τ' i Based on _hat, the output torque of the joint is determined by the torque command τ d,i To follow (i.e., τ d,i and τ d The feedback compensation amount for joint torque is calculated (to minimize the difference). Then, the torque τ that the motor should output is determined based on the calculation result. d,m,i This is determined by performing the process described in number 6 above. Calculated motor torque command value (= motor current command value) τ d,m,i This is output to the motor current control system, which is located as a lower-level control loop, and the motor current control system determines that the actual motor current supplied is the specified value (I i The motor drive is controlled so that the result is as follows.

[0104] (Step S-3-8) The servo control unit evaluates the variable k, which stores the number of iterations, to confirm whether the joint control process in the servo control unit has completed the specified number of iterations. If k is less than the specified number (NO), the process returns to step S-3-2 and the joint control process for the next time step is executed again. If k is equal to the specified number (YES), the process proceeds to step S-3-9, which involves sending the control result to the robot control device.

[0105] (Step S-3-9: Send joint status to robot control device) The joint state is the result of the control (the sensing result and the joint state estimation result θ) i , τ i θ' i_hat, τ’ i _hat, I i (such as) are stored from the motor control unit to the gateway controller. They are transmitted to the robot control device via communication means such as Ethernet together with a control status flag that stores whether the joint control system is operating normally. This transmission process is T, which is the update period of the robot control device. A It is performed every T (END) seconds.

[0106] The motion characteristics (dynamics) of the robot with such control are set within an appropriate range using sufficiently accurate dynamics parameters K d , D d under, and are as follows. [Equation] Here, [Equation] is.

[0107] According to the robot control device with such a configuration, the servo unit can calculate and output a feedback control amount based on the motion state of the robot without going through the integrated control unit. Since the servo unit can acquire sensor information and update the control law at a much faster cycle than the integrated control unit, the time required from when the robot device acquires sensor information until the drive unit outputs a control force can be reduced. According to the present embodiment, since the control gain of the control system can be increased to enhance the damping property of the robot, force control with high responsiveness and high stability can be performed.

[0108] [Embodiment 2] Referring to FIGS. 8 to 10, Embodiment 2 will be described. Regarding matters common to Embodiment 1, the description will be simplified or omitted. In Embodiment 1, the robot control device 300 performed interference term calculation processing to reduce the effects of interference between joints (Figure 6: Step S-2-5), but in this embodiment, the servo control unit 230 of the robot arm performs this processing.

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

[0110] Similar to Embodiment 1, the arm control system updates information to be transmitted to the joint control system, i.e., the joint servo control unit, which is a lower-level control system than the arm control system, in order to realize the state planned by the motion planning unit (steps S-6-1 to S-6-4). Then, it transmits this information to the servo control unit (step S-6-6) and receives the processing results (sensor feedback values) performed by the lower-level control system, the servo control unit (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 processes is the update cycle T of the robot control device. A This process is repeated for seconds until each subtask is completed (step S-6-11: YES).

[0111] The difference from Embodiment 1 is that the interference term calculation step in S-2-5 (Figure 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 the corresponding control force, as will be described later, in this embodiment the calculation of the interference term is performed within the servo control unit 230.

[0112] Figure 9 is a block diagram showing the configuration of the servo control unit 230 in this embodiment. Referring to this figure, the communication between the robot control device 300 and the servo control unit 230 will be described.

[0113] The servo control unit 230 receives each command value transmitted from the robot control device 300, and the signal relay and distribution unit 530 distributes these commands to the motor control units 531 to 536. The signal relay and distribution unit 530 consists of a signal conversion unit 5301 that is responsible for communication 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 to T A Every second, K q , D q , τ cmp , θ d The system receives the parameter values ​​for each parameter and stores them in the shared memory unit 5302. Each parameter stored in the shared memory unit 5302, namely K q , D q , τ cmp , θ d The update cycle is T A It's seconds.

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

[0116] The motor control unit 53i of the i-th joint accesses the shared memory unit 5302 and sets the command value for the i-th joint as K q , D q , τ cmp,i , θ d And the motor angle θ related to other joints k and angular velocity θ' k The data is read out (k=1~6, except i). Then, the read data and the rotation angle θ of the motor detected by the joint angle sensor 54i are used. i And the current joint torque τ detected by the torque sensor 55i in the joint i Based on this, the motor control unit 53i of the i-th joint commands the joint torque value τ that the joint should specifically achieve. d,iThe joint torque τ is calculated. i However, the command value τ d,i The drive current Ii (or output torque τ of the motor 21i) of the motor 21i is adjusted to follow the motor 21i. m,i The process executes torque servo control to control the joint. The current motor rotation angle θ of the joint is detected during this process. i and angular velocity θ' i This is stored in the shared memory unit 5302.

[0117] Furthermore, the angular velocity θ' i This is calculated or measured during the series of servo control processes described above. For example, the motor rotation angle θ measured by the angle sensor 54i. i The angular velocity θ' is obtained by numerically differentiating (difference) and then applying a low-pass filter to perform a pseudo-differentiation process. i It is possible to calculate the angular velocity θ' by mounting a velocity sensor on the joint. i The angular velocity θ' can be measured directly, or a state estimator such as a Kalman filter can be used to estimate the angular velocity θ' based on the sensor's observation data and a dynamic model. i It is also possible to estimate this.

[0118] Here, the update period of the servo cycle that executes the series of processes of the motor control unit 53i is T S Seconds, the rotation angle θ of the joint's motor, which has been sensed. i and angular velocity θ' i The period for storing the data in shared memory is T. G Set to seconds. Data storage period (T G ) should be made as small as possible, T G If possible, T S It is set to be equal to, but if there are issues with transfer speed, etc., T S It may be set to a larger value.

[0119] According to this embodiment, the motor control unit 53i receives information relating to the state of joints other than its own (other-axis feedback amount), specifically the rotation angle θ of the motors of other joints. i (Or joint angle) and angular velocity θ' iThis information can be obtained without going through the robot control device 300. In other words, each motor control unit 53i within the servo control unit 230 can share (send and receive) information related to the state of the joints with each other at high speed.

[0120] As a result, in this embodiment, the control period, which was limited by the control cycle of the robot control device 300, can be shortened to, for example, about one-tenth of its original length. Therefore, it becomes possible to perform feedback compensation with less delay, and good force control with effective damping characteristics can be achieved.

[0121] Figure 10 is a flowchart illustrating the specific processing steps performed by the servo control unit 230 in this embodiment. (Step S-4-1: Receipt of command value)

[0122] This is the same process as in step S-3-1 in Embodiment 1. The signal conversion unit (gateway controller: GW) of the signal relay distribution unit 530 of the servo control unit 230 receives each command value transmitted from the robot control device 300 and stores it in the shared memory unit 5302.

[0123] (Step S-4-2: Updating the number of iterations) This is the same process as in step S-3-2 in Embodiment 1. The processes from this step onward are performed in parallel for each joint by each motor controller up to step S-4-10. Unless otherwise specified, the update cycle is T S It's seconds. (Step S-4-3: Obtaining joint status) This process is the same as step S-3-3 in Embodiment 1. It is a sensor feedback process to the motor control unit 53i. Each motor control unit 53i accesses the sensor unit located at its own joint and obtains the latest observation result. (Step S-4-4: Estimation of joint condition) This is the same process as step S-3-4 in Embodiment 1. Each motor control unit 53i calculates the state of the self-joint for use in subsequent processes through pseudo-differential processing. A state observer using a mechanical model or a probability model may be used.

[0124] (Step S-4-5: Access to Shared Memory) In the shared memory unit 5302, data K q , D q , τ cmp , θ d obtained from the robot control device 300, and θ = (θ1, θ2, θ3, θ4, θ5, θ6) T and θ’ = (θ’1, θ’2, θ’3, θ’4, θ’5, θ’6) T are stored.

[0125] The i-th motor control unit 53i (i = 1 ··· 6) of the i-th joint accesses the shared memory unit 5302 to obtain the command values K q,i,k (k = 1 ··· 6), D q,i,k (k = 1 ··· 6), τ cmp,i , θ d = (θ d1 , θ d2 , θ d3 , θ d4 , θ d5 , θ d6 ) T transmitted from the robot control device 300. At the same time, the i-th motor control unit 53i obtains information (feedback amount) related to the states of other joints (joints other than the i-th joint) transmitted from the j-th motor control unit 53j (j = 1 ··· 6, excluding i) of other joints from the shared memory unit 5302. Information related to the states of other joints is the rotation angle θk (k = 1 ··· 6, excluding i) and the angular velocity θ’k (k = 1 ··· 6, excluding i) of the motors of other joints. The i-th motor control unit 53i accesses the command values and the feedback amount as described above, and at the same time stores the information (latest observation results) related to the latest state of the self-joint, θ i and θ’ i in the shared memory unit 5302.

[0126] The access cycle for the shared memory of the motor control units 531-536 is the communication cycle (T) between the robot control device 300 and the servo control unit 230. A T shorter than ) G It is in seconds. Access period T G This is the motor control unit's update cycle (servo cycle) T S It is desirable to set them to be the same, but they should be set appropriately depending on the load on each processing unit and the transfer speed of communication between 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 using the command value parameters obtained from the shared memory in step S-4-5, the amount of feedback from other joints, and the information related to the state of its own joint (amount of feedback from its own joint) obtained in steps S-4-3 and S-4-4. Specifically, it calculates the interference term τ from the joint torque command values ​​that generate the elastic restoring force and damping force, which is calculated from information spanning multiple joints (displacement and velocity of multiple joints). couple,i Update.

number

[0128] The update cycle of this calculation is set to be equal to the update cycle of the other-axis feedback quantity, i.e., the access cycle to the shared memory. G It is seconds. G and T S If the cycles are set to different periods and there is no access to the shared memory of the motor control unit, this step is skipped, and τ couple,i The same value as the last update will be retained.

[0129] (Step S-4-7: Calculation of stiffness and damping terms related to the joint itself) This is the same process as step S-3-5 in Embodiment 1. The i-th motor control unit 53i (i=1···6) corresponding to the i-th joint receives the latest θ updated in the previous process.d,i , K q,i,i , D q,i,i , θ i Based on this, the stiffness and damping terms τ related to the joint of the torque command value pd,i The data is updated and output to the torque control unit.

number

[0130] (Step S-4-8: Calculation of joint torque command) This is the same process as step S-3-6 in Embodiment 1. The i-th motor control unit 53i (i=1···6) receives the latest τ updated by the processes up to this point. cmp,i , τ pd,i , τ couple,i Based on this, the command value τ of the joint torque that each joint should specifically achieve. d,i Update.

number

[0131] (Step S-4-9: Perform joint torque control processing) This is the same process as in step S-3-7 in Embodiment 1. The i-th motor control unit 53i (i=1···6) corresponding to the i-th joint outputs the joint torque τ i The specified torque command value τ d,i The motor's drive current is controlled to follow this behavior. (Step S-4-10) This is the same process as step S-3-8 in Embodiment 1.

[0132] (Step S-4-11: Send joint status to robot control device) This process is the same as step S-3-9 in Embodiment 1. The gateway controller reads the joint states (θ, θ', τ, etc.) stored in the shared memory and transmits them to the robot control device 300 via a communication means such as Ethernet, along with the control status flag. This transmission process is performed at the update cycle T of the robot control device 300. A This is performed 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 state of other axes without going through the robot control device, which is an integrated control unit. Since the servo unit can acquire joint state (sensor information) and update the control law at a significantly faster cycle than the integrated control unit, the time required from when the robot device acquires sensor information until the drive unit outputs the control force can be reduced.

[0134] In the configuration according to this embodiment, the acquisition of other joint states and the updating of the control law based on them are particularly accelerated, especially the interference term τ in the joint torque command value. couple,i It is possible to increase the speed at which the data is updated. Therefore, it is possible to perform force control with greater accuracy and stability.

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

[0136] In this embodiment, the signal relay and distribution unit 530 and the motor control units 531 to 536 for each axis, which are arranged corresponding to the joints, are each configured as independent control circuits (e.g., circuit boards on which microcontrollers or microprocessors are mounted). They are connected to each other in a bus-type or daisy-chain-type network topology so that they can communicate with one another. Their connection is made via a communication means such as RS485. In this embodiment, the processing performed by the robot's force control system (robot control device) is the same as the processing described in Embodiment 2 with reference to Figure 8.

[0137] Referring to Figure 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 and distribution unit 530 transmits these commands to the motor control units. More specifically, the signal relay and distribution unit 530 transmits each command value received from the robot control device to the first motor control unit 531. The motor control unit 531, upon receiving this, transmits it to the second motor control unit 532 corresponding to the next joint. Similarly, each motor control unit transmits the command value to the motor control unit of the next joint, and so on, until the command value is transmitted up to the sixth motor control unit 536. Through this transmission path, the command values ​​are distributed to each motor control unit.

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

[0139] The signal conversion unit 5301 converts the data transmission period of the robot control device to T A Every second, K q , D q , τ cmp , θ d The system receives the parameter values ​​for each parameter and stores them in the shared memory unit 5302. Each parameter stored in the shared memory unit 5302, namely K q , D q , τ cmp , θ d The update cycle is T A It's seconds.

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

[0141] The motor control units 531 to 536 perform three types of processing at different intervals: (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 conversion and distribution unit and acquires the command value K q , D q , τ cmp , θ d This process involves acquiring the command value and then transmitting the acquired command value to the motor control unit corresponding to the next joint, thereby sharing the command value.

[0143] (2) is the communication process with the anterior and posterior joints (second joint). This involves the latest control result observed by the motor control unit of the joint up to the previous cycle, i.e., the rotation angle θ of the motor sensed by the sensor unit of the joint. i and angular velocity θ' i This information is transmitted to the motor control units (53i-1 and 53i+1) of the anterior and posterior joints (the joint itself and the adjacent joints). At the same time, the rotation angle θ, which is information related to the state of the anterior and posterior joints, is transmitted. i―1 and angular velocity θ' i―1 , rotation angle θ i+1 and angular velocity θ' i+1 The system performs communication processing to receive the data. Thus, the rotation angle θ = (θ1, θ2, θ3, θ4, θ5, θ6) is used as the feedback amount for all joints. T and angular velocity θ'=(θ'1, θ'2, θ'3, θ'4, θ'5, θ'6) T They share this information with each other. In addition, the information relating to the state of all joints shared during this process is simultaneously written to the shared memory when the first motor control unit accesses the signal relay distribution unit to obtain command values ​​in the process of (1).

[0144] (3) Joint control processing is the processing related to the control of the robot's joints, other than the communication processing in (1) and (2). The command values ​​obtained in (1) and (2), the amount of feedback from other axes, and the rotation angle θ sensed by the sensor part of the 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 joint should specifically realize. d,i The output τ of the joint torque is calculated. i The specified torque command value τ d,i This is a torque servo control process that controls the motor's drive current to follow the movement.

[0145] The execution period of process (1) is T A It is set to seconds. This is because the update cycle of the command value parameter is defined by the data transmission cycle of the robot control device. The execution cycle of process (2) is T G It is seconds, T G is, T A >T G ≧T S It is set within the range that satisfies the following. The execution period of process (3) is the servo cycle itself, T S It takes seconds. It is executed as quickly as possible, within the limits of what the processing unit and sensing mechanism can handle.

[0146] As described above, in terms of reducing the time required for control, the processing cycle (T) of the motor control unit sharing information related to the state of other joints is important. G ) should be made as small as possible, T G If possible, T S It is set to be equal to . However, if there are issues with transfer speed, etc., T S It may be made 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 its own joint within the servo control unit without going through the robot control device. Specifically, the motor angle θ related to other joints. k and angular velocity θ' kThis makes it possible to share these with each other (k=1~6, except i). As a result, in this embodiment, the time required, which was limited by the control cycle of the robot control device 300, can be reduced to, for example, about one-tenth. Therefore, it becomes possible to perform feedback compensation with less delay, and good force control with effective damping characteristics can be achieved.

[0148] Figure 12 is a flowchart illustrating a specific processing procedure performed by the servo control unit 230 in this embodiment. (Step S-5-1: Receiving the command value) This is the same process as step S-4-1 in Embodiment 2. (Step S-5-2: Updating the number of repetitions) This is the same process as step S-4-2 in Embodiment 2. (Step S-5-3: Obtaining joint status) This is the same process as step S-4-3 in Embodiment 2. (Step S-5-4: Estimation of joint condition) This is the same process as step S-4-4 in Embodiment 2.

[0149] (Step S-5-5: Communication processing with the other axis motor control unit, command value acquisition processing) This is the command value acquisition process described above (1). In other words, the command values ​​received from the robot control device 300 are transmitted sequentially from the proximal joint to the motor control units corresponding to the distal joints, thereby sharing the command values ​​related to the control of each joint among the motor control units of all joints. First, the motor control unit 531 of the first axis, which is the motor control unit of the first joint located at the proximal end, accesses the shared memory of the signal relay distribution unit 530 and receives the command value K q , D q , τ cmp , θ dThe command values ​​are obtained. These command values ​​are then transmitted via communication to the motor control unit of the next joint (the motor control unit 532 of the second axis). The second axis motor control unit, having received the command values ​​from the first axis motor control unit, performs the same process and transmits the command values ​​for the next joint. Similarly, the i-th axis motor control unit transmits the command values ​​to the i+1 motor control unit of the next joint, and the process continues until the distal joint (the sixth joint) is reached.

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

[0151] (Step S-5-5: Communication processing with other axis motor control unit 2, sharing of joint state) This is the process of (2) sharing the state with other joints, as mentioned above. The motor control units communicate with each other and share the information related to the state of the joint observed by each motor control unit with all joints and the shared memory. The motor control unit of the i-th joint (motor control unit 53i of the i-th axis) shares the information (rotation angle θ) measured by the motor control unit of its own joint. i , angular velocity θ' i The following information is transmitted 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 θ, which is information related to the state of the front and rear joints, is transmitted. i―1 and angular velocity θ' i―1 , rotation angle θ i+1 and angular velocity θ' i+1 The system performs communication processing to receive the data. The execution cycle is T G It's seconds.

[0152] (Step S-5-6: Calculation of the interference term) This is the same process as step S-4-6 in Embodiment 2. The interference term of the joint torque command value is updated using the command value parameters obtained from the previous joint or shared memory, and the cross-axial feedback amounts obtained from the anterior and posterior joints. The update period for this calculation is set to be equal to the update period for the cross-axial feedback amounts. G It is seconds.G and T S If the interval is set to a different period and the amount of feedback on the other axis is not updated, this step is also skipped, and the interference term retains the same value as when it was last updated.

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

[0154] With a robot control system configured in this way, each motor control unit of the servo control unit can calculate and output feedback control amounts based on the motion state of other axes, without going through the robot control system, 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 state of other axes without going through the robot control device, which is an integrated control unit. Since the servo unit can acquire joint state (sensor information) and update the control law at a significantly faster cycle than the integrated control unit, the time required from when the robot device acquires sensor information until the drive unit outputs the control force can be reduced.

[0156] In the configuration according to this embodiment, the acquisition of other joint states and the updating of the control law based on them are particularly accelerated, especially the interference term τ in the joint torque command value. couple,i It is possible to increase the speed at which the data is updated. Therefore, it is possible to perform force control with greater accuracy and stability.

[0157] In particular, in this embodiment, the signal relay and distribution unit 530 and the motor control units 531 to 536 for each axis, which are arranged corresponding to the joints, are each configured as independent control circuits (for example, circuit boards on which microcontrollers or microprocessors are mounted). They are interconnected in a bus-type or daisy-chain-type network topology. This makes it possible to adopt configurations such as using a common design for each motor control unit or mounting the motor control unit on the robot body, thereby increasing the design flexibility of the robot system. By distributing each motor control unit near the joints that are driven, maintainability can be improved through unitization of the joint mechanism and standardization of design. Furthermore, it becomes possible to standardize the design of the joints and control boards, thereby reducing costs.

[0158] [Other embodiments] It should be noted that the present invention is not limited to the embodiments and examples described above, and many modifications are possible within the technical concept of the present invention.

[0159] For example, when calculating interference terms to compensate for the dynamic effects of other joints on one's own joint, we treated the joints adjacent to both sides of the own joint as other joints, but this is not the only approach. For instance, if the joint located at the most proximal or most distal end is considered the own joint, it may suffice to consider the influence of only one of the adjacent joints.

[0160] Embodiments of the present invention also include a control program capable of executing the control method related to the servo control operation described above, and a recording medium readable by a computer storing the control program. Each functional element shown in the referenced drawings is a functional concept and does not necessarily have to be physically configured as shown. For example, the specific forms of distribution and integration of each functional block are not limited to the illustrated examples, 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, and processing machines operated by computer numerical control, in addition to production equipment. For example, it is a control of machines and equipment that can automatically perform movements such as extension and retraction, bending and straightening, vertical movement, horizontal movement, or rotation, or combinations thereof, based on information stored in a memory device provided in the control device.

[0162] The robotic apparatus of this embodiment can perform a wide variety of tasks, including those involving force control operations that require high precision and high response speed. For example, it can be widely implemented in the manufacturing of 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 implements one or more of the functions of the embodiments to a system or device via a network or storage medium, and by having one or more processors in the computer of that system or device read and execute the program. It can also be realized by a circuit (e.g., an ASIC) that implements 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 estimation unit / 507...Joint target value generation unit / 508...Environment model / 530...Signal relay distribution unit / 531~536...Motor control unit / 541~546...Angle sensor / 551~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 an angle of the joint, and a torque sensor that detects a 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 regarding 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. A control method comprising:

2. The information regarding 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 a 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 a 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 compensating 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 it 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 mechanical 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. A control method according to claim 4, wherein the control method is a control method for controlling a power supply.

8. The second control unit outputs information about the impedance to the first control unit for each first period, The first control unit controls the drive unit for each second period that is shorter than the first period, using information related to the impedance.

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

9. The drive unit is a motor, and the first control unit includes a servo control unit having a control period equal to the second period.

9. The control method according to claim 8.

10. The servo control unit samples the value from the angle sensor and the value from the torque sensor every second period.

10. The control method according to claim 9.

11. The servo control unit transmits the sampled values ​​from the angle sensor and the sampled values ​​from the torque sensor to the second control unit for each of the first period.

11. The control method according to claim 9 or 10.

12. The first control unit acquires information representing a state of the joint for each of the second cycles and transmits the information to the second control unit for each of the first cycles.

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

13. 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.

13. A control method according to any one of the preceding claims.

14. The angle sensor is a sensor that detects an angle of the drive unit or a sensor that detects an angle of the joint.

14. The control method according to claim 1,

15. A control method for a robot system including a robot having 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 regarding impedance at the first joint, a value from the first angle sensor, and a value from the second angle sensor. A control method comprising:

16. A robot system comprising: a robot having a joint with a drive unit, an angle sensor for detecting the angle of the joint, and a torque sensor for detecting the torque of the joint; a first control unit for controlling the drive unit; and a second control unit for outputting a command value related to the operation of the drive unit to the first control unit, The second control unit outputs information regarding 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. A robot system comprising:

17. The information regarding 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 a torque to be generated by the drive unit based on the stiffness information, the damping coefficient, and the angle value.

17. The robot system according to claim 16.

18. 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 a 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 compensating for dynamics of the robot; 20. The robot system according to claim 17 .

19. The first control unit acquires information representing a state of the other joint and outputs it 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.

20. The robot system of claim 18.

20. The first control unit acquires information representing a state of the other joint, calculates an interference term that compensates for a mechanical effect of the other joint on the joint, and controls the drive unit using the interference term.

20. The robot system of claim 18.

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.

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

22. 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.

22. The robot system according to claim 19, wherein the robot system is a robotic system having a plurality of actuators.

23. The second control unit outputs information regarding the impedance to the first control unit for each first period, The first control unit controls the drive unit for each second period that is shorter than the first period, using information related to the impedance.

23. The robot system according to claim 16, wherein the robot system is a movable member.

24. The drive unit is a motor, and the first control unit includes a servo control unit having a control period equal to the second period.

24. The robotic system of claim 23.

25. The servo control unit samples the value from the angle sensor and the value from the torque sensor every second period.

25. The robotic system of claim 24.

26. The servo control unit transmits the sampled values ​​from the angle sensor and the sampled values ​​from the torque sensor to the second control unit for each of the first period.

26. The robot system according to claim 24 or 25.

27. ​​The first control unit acquires information representing a state of the joint for each of the second cycles and transmits the information to the second control unit for each of the first cycles.

27. The robot system according to claim 23,

28. The second control unit outputs force teaching data related to force control of the joint and position teaching data specifying a movement to be executed by the robot to the first control unit.

28. The robot system according to claim 16, wherein the robot system is a robotic system having a plurality of actuators.

29. A robot system comprising: a robot having a first joint having a first drive unit and a first angle sensor for detecting an angle of the first joint, a second joint having a second drive unit and a second angle sensor for detecting an angle of the second joint, a first control unit for controlling the first drive unit, and a second control unit for controlling 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 regarding impedance at the first joint, a value from the first angle sensor, and a value from the second angle sensor. A robot system comprising:

30. A method for manufacturing an article, comprising the steps of: controlling the robot using the control method according to any one of claims 1 to 15; and manufacturing the article.

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

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

Images