JP2013000856A - Robot control system and robot system - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a robot control system, a robot system etc., which are capable of improving a response as well as reducing the performance requirement to control hardware.SOLUTION: The robot control system includes a target value output unit 60 for outputting a target value of feedback control of a robot 100, a force control unit 20 for controlling force based on sensor information from a force sensor 10 to output a correction value of the target value, a robot unit control unit 80 for performing the feedback control of the robot based on the target value corrected with the correction value. The force control unit 20 includes a preliminary calculation unit 22 for preliminarily calculating a control parameter varying in response to a posture of the robot, and an implementing unit 26 for implementing calculation for determining the correction value based on the sensor information and the control parameter.

Description

  The present invention relates to a robot control system, a robot system, and the like.

  In operations performed using a robot such as a manipulator, there are various constraints such as operations involving contact with an object. In such cases, force control is often required in addition to position control. For example, when tracing the surface of an object, fitting one object to another object, grasping a flexible object without breaking it, etc., in addition to simply controlling the position, respond to the reaction force from the object Movement is required.

  A typical technique for performing force control in a robot is a technique called impedance control. Impedance control is a control method for operating a robot as if it had those values suitable for work regardless of its actual mass, viscosity characteristics, and elastic characteristics. This is a control method that solves an equation of motion based on force information obtained from a force sensor attached to a robot and operates the robot according to the solution. By appropriately setting this equation of motion, it becomes possible to operate a robot such as a manipulator as if it has a predetermined mass, viscosity, and elasticity. As conventional techniques related to such impedance control and force control, techniques disclosed in Patent Documents 1, 2, and 3 are known.

JP-A-6-320451 Japanese Patent Application Laid-Open No. 2-205489 JP-A-4-369004

  Patent Document 1 discloses a technique for performing impedance control with high accuracy even in a very complicated system such as a case where a control target is flexible.

  Patent Document 2 discloses a technique for estimating the force without using a force sensor and performing impedance control with a simpler mechanical structure.

  These methods of Patent Documents 1 and 2 are intended to improve control accuracy in a more complicated system or to implement impedance control in a simpler system, but real-time performance is required as a compensation. There is a problem that many operations are required.

  For example, the method of Patent Document 1 requires calculation of a forward dynamics model and a reverse dynamics model for each loop of force control. Further, in the method of Patent Document 2, in order to estimate force for each loop of force control, it is necessary to perform calculation of an expression described in the specification.

  On the other hand, Patent Document 3 discloses one technique for reducing the amount of calculation. This is to reduce the amount of calculation by using the transposed Jacobian matrix without performing the calculation for obtaining the inverse Jacobian matrix described in Patent Documents 1 and 2 above.

  However, the method of Patent Document 3 also has a problem that many operations requiring real-time performance are required. Moreover, since there is a possibility that the manipulator may be accelerated before a desired reaction force is obtained, there is a problem in dealing with an abnormality.

  According to some aspects of the present invention, it is possible to provide a robot control system, a robot system, and the like that can reduce performance requirements for control hardware and improve responsiveness.

  One aspect of the present invention is a target value output unit that outputs a target value for feedback control of a robot, and a force control unit that performs force control based on sensor information from a force sensor and outputs a correction value of the target value And a robot control unit that performs the feedback control of the robot based on the target value corrected by the correction value, the force control unit preliminarily controlling control parameters that change according to the posture of the robot. The present invention relates to a robot control system including a pre-calculation unit that calculates, and an execution unit that executes a calculation process for obtaining the correction value based on the sensor information and the control parameter.

  According to one aspect of the present invention, pre-calculation of control parameters that change according to the posture of the robot is performed, and calculation processing for obtaining a correction value based on sensor information from the force sensor and the pre-calculated control parameters Is executed. Then, the target value is corrected based on the obtained correction value, and feedback control of the robot is performed based on the corrected target value. In this way, control parameters that change in accordance with the posture of the robot are pre-calculated before the execution timing of the arithmetic processing in the execution unit. Therefore, it is possible to provide a robot control system that can reduce performance requirements for control hardware and improve responsiveness.

  In one aspect of the present invention, the force control unit includes a FIFO in which the control parameter calculated by the pre-calculation unit is stored, and the execution unit reads and reads the control parameter from the FIFO. The calculation process for obtaining the correction value based on the control parameter may be executed.

  In this way, the pre-calculation unit can perform other processes or stop the process after storing the pre-calculated control parameters in the FIFO, so that the process efficiency can be improved. .

  In one aspect of the present invention, the FIFO may store the control parameters used in the execution unit in a first-in first-out manner for each timing at which arithmetic processing of the execution unit is executed.

  If it does in this way, the execution part will be able to perform the calculation process using the control parameter only by reading the control parameter from the FIFO at the timing when each calculation process is executed.

  In the aspect of the invention, the execution unit may pre-calculate the control parameter with respect to the pre-calculation unit when a remaining data amount of the FIFO becomes a predetermined threshold value or less. A pre-calculation request may be made to request

  In this way, the pre-calculation unit only needs to pre-calculate the control parameter at least until the remaining data amount of the FIFO disappears, so that the real-time processing request for the pre-calculation unit can be eased.

  In one aspect of the present invention, the pre-calculation unit pre-calculates an inverse Jacobian matrix element as the control parameter, and the execution unit is based on the pre-calculated matrix element of the inverse Jacobian, The correction value of the target value may be obtained.

  In this way, the inverse Jacobian matrix elements are pre-computed, and the execution unit does not need to calculate the inverse Jacobian matrix elements, thereby reducing the performance requirements for the execution unit.

  In the aspect of the invention, the pre-calculation unit pre-calculates a limiter value indicating an allowable range of the robot trajectory with respect to the planned trajectory as the control parameter, and the execution unit performs the robot with respect to the planned trajectory. When it is determined that the trajectory exceeds the allowable range, an interrupt indicating that the allowable range has been exceeded may be generated.

  In this way, the limiter value indicating the allowable range of the robot trajectory is calculated in advance, and the execution unit does not need to calculate the limiter value, so that performance requirements and the like for the execution unit can be reduced. Further, when the robot trajectory exceeds the allowable range, an interrupt is generated, and appropriate error processing can be executed.

  In the aspect of the invention, the pre-calculation unit may pre-calculate the limiter value by obtaining a Jacobian around the planned trajectory.

  In this way, even when the allowable range changes according to the posture of the robot, the limiter value indicating the allowable range can be appropriately pre-calculated.

  In the aspect of the invention, the pre-calculation unit pre-calculates a correction coefficient for correcting the sensor information of the force sensor as the control parameter, and the execution unit is based on the correction coefficient. The sensor information may be corrected.

  In this way, the correction coefficient for correcting the sensor information is calculated in advance, and the execution unit does not need to calculate the correction coefficient, so that performance requirements and the like for the execution unit can be reduced.

  In one aspect of the present invention, the target value output unit outputs a trajectory information of the robot, and performs inverse kinematics processing based on the trajectory information from the trajectory generation unit. An inverse kinematics processing unit that outputs joint angle information as the target value may be included.

  In this way, based on the trajectory information from the trajectory generator, the robot joint angle information is obtained by inverse kinematics processing, and the feedback control of the robot can be executed using the obtained joint angle information as a target value. Become.

  In the aspect of the invention, the force control unit may perform impedance control as the force control.

  In the aspect of the invention, the target value may be joint angle information of the robot, and the correction value may be a correction value of the joint angle information.

  One embodiment of the present invention relates to a robot system including any one of the robot control systems described above and the robot.

1 is a configuration example of a robot control system and a robot system according to the present embodiment. An example of the robot system of this embodiment. FIG. 3A to FIG. 3C are explanatory diagrams regarding force control. 4A and 4B are explanatory diagrams of compliance control. FIG. 5A and FIG. 5B are explanatory diagrams of impedance control. 6A and 6B are explanatory diagrams of inverse kinematics. FIGS. 7A and 7B are explanatory diagrams of the Jacobian. Explanatory drawing about the control method by Jacobian. Explanatory drawing about the control method by Jacobian. A basic configuration example of a control system when force feedback is not included. The example of basic composition of a control system in case a force feedback is included. 2 is a basic configuration example of a control system of the robot control system of the present embodiment. 2 is a detailed configuration example of a robot control system according to the present embodiment. Explanatory drawing of the process of this embodiment using FIFO. The detailed example of a structure of the robot control system of this embodiment. The flowchart which shows the detailed process of this embodiment. The flowchart which shows the detailed process of this embodiment. FIG. 18A to FIG. 18C are flowcharts showing detailed processing of this embodiment. The flowchart which shows the detailed process of this embodiment. The flowchart which shows the detailed process of this embodiment.

  Hereinafter, preferred embodiments of the present invention will be described in detail. The present embodiment described below does not unduly limit the contents of the present invention described in the claims, and all the configurations described in the present embodiment are indispensable as means for solving the present invention. Not necessarily.

1. Configuration FIG. 1 shows a configuration example of a robot control system (manipulator control system) of the present embodiment and a robot system including the same. Note that the robot control system and the robot system according to the present embodiment are not limited to the configuration shown in FIG. 1, and various modifications such as omitting some of the components or adding other components are possible. .

  The robot control system of this embodiment includes a force control unit 20, a target value output unit 60, and a robot control unit 80. The robot system of this embodiment includes the robot control system and the robot 100 (force sensor 10).

  The target value output unit 60 outputs a target value for feedback control of the robot (manipulator in a narrow sense). Based on this target value, feedback control of the robot 100 is realized. Taking an articulated robot or the like as an example, this target value is information on the joint angle of the robot. The joint angle information of the robot is information representing the angle of each joint (angle formed by the joint axis) in the link mechanism of the robot arm, for example.

  The target value output unit 60 can include a trajectory generation unit 62 and an inverse kinematics processing unit 64. The trajectory generator 62 outputs the trajectory information of the robot. The trajectory information can include position information (x, y, z) of the end effector section (end point) of the robot and rotation angle information (u, v, w) about each coordinate axis. The inverse kinematics processing unit 64 performs inverse kinematics processing based on the trajectory information from the trajectory generation unit 62, and outputs, for example, robot joint angle information as a target value. The inverse kinematics process is a process for calculating the motion of a robot having a joint, and is a process for calculating joint angle information and the like by inverse kinematics from the position and orientation of the end effector unit of the robot.

  The force control unit 20 (impedance control unit in a narrow sense) performs force control (force control) based on sensor information from the force sensor 10, and outputs a correction value of the target value. More specifically, the force control unit 20 (impedance control unit) performs impedance control (or compliance control) based on sensor information (force information and moment information) from the force sensor 10. Force control is, for example, control in which force feedback is added to conventional position control. Impedance control is a technique for making the ease of displacement (mechanical impedance) of the end effector section (hand) with respect to external force a desirable state by control. Specifically, in the model in which the viscosity coefficient and the elastic element are connected to the mass of the end effector unit of the robot, the control is performed so as to contact the object with the viscosity coefficient and the elastic coefficient set as targets. The force sensor 10 is a sensor that detects a force or moment received as a reaction force of the force generated by the robot 100. The force sensor 10 is usually attached to the wrist portion of the arm of the robot 100, and the detected force and moment are used for various types of force control (impedance control) as sensor information.

  The robot control unit 80 performs feedback control of the robot based on the target value from the target value output unit 60. Specifically, the robot feedback control is performed based on the target value corrected by the correction value from the force control unit 20. For example, feedback control of the robot 100 is performed based on the target value corrected by the correction value and the feedback signal from the robot 100. For example, the robot control unit 80 includes a plurality of drive control units 82-1 to 82-N (in a narrow sense, motor control units), and controls the control signals for the drive units 102-1 to 102-N included in the robot 100. Is output. Here, the drive units 102-1 to 102-N are drive mechanisms for moving each joint of the robot 100, and are realized by, for example, a motor or the like.

  In the present embodiment, the force control unit 20 includes a pre-calculation unit 22 and an execution unit 26. The pre-calculation unit 22 pre-calculates control parameters (calculation parameters) that change according to the posture of the robot 100. The execution unit 26 (hard real-time processing unit) executes a calculation process for obtaining a correction value based on the sensor information and the control parameter. That is, the correction value of the target value is obtained based on the sensor information from the force sensor 10 and the control parameter calculated in advance, and is output to the target value output unit 60, for example. Thereby, the feedback control of the robot 100 is performed based on the target value corrected by the force control.

  As described above, in the present embodiment, control parameters that change according to the posture of the robot are pre-calculated, and correction values at each control timing are obtained based on the pre-calculated control parameters. Then, feedback control of the robot is performed based on the target value corrected by the correction value. In this way, if the control parameters required for robot control are obtained by pre-calculation that does not require real-time responsiveness, performance requirements for control hardware can be reduced and responsiveness of robot control can be improved. It becomes like this. The pre-calculation is, for example, that the control parameters are calculated in advance based on the planned trajectory of the robot at a timing before the timing at which the control parameters are actually required.

  The force control unit 20 includes a first-in first-out (FIFO) 24 in which the control parameters calculated by the pre-calculation unit 22 are stored. The FIFO 24 can be realized by a first-in first-out memory (register). In the FIFO 24, control parameters used in the execution unit 26 are stored in a first-in first-out manner at each timing when the arithmetic processing of the execution unit 26 is executed.

  Then, the execution unit 26 reads the control parameter from the FIFO 24 and executes a calculation process for obtaining a correction value based on the read control parameter. In other words, in the present embodiment, pre-calculated control parameters are stored in the FIFO 24. The execution unit 26 reads out the control parameter from the FIFO 24 and executes a calculation process for obtaining a correction value at each calculation process execution timing.

  In this case, the execution unit 26 pre-calculates the control parameter to the pre-calculation unit 22 when the remaining data amount of the FIFO 24 is less than a given threshold value (below the given data amount). Make a pre-calculation request to request In response to this pre-calculation request, the pre-calculation unit 22 performs a pre-calculation of the control parameter and stores the pre-calculated control parameter in the FIFO 24.

  Here, the control parameter is a parameter (calculation parameter) used in the correction value calculation processing, and is a parameter whose value changes according to the posture of the robot 100, for example. As this control parameter, various parameters such as an inverse Jacobian matrix element, a limiter value indicating an allowable range of the robot trajectory, and a correction coefficient for correcting sensor information of the force sensor 10 can be assumed.

  For example, the pre-calculation unit 22 pre-calculates inverse Jacobian matrix elements as control parameters. The execution unit 26 calculates a correction value for the target value based on the pre-calculated inverse Jacobian matrix elements. The pre-calculation unit 22 pre-calculates a limiter value indicating the allowable range of the robot trajectory with respect to the planned trajectory as a control parameter. If the execution unit 26 determines that the robot trajectory exceeds the allowable range with respect to the planned trajectory, the execution unit 26 generates an interrupt indicating that the allowable range has been exceeded. In this case, the pre-calculation unit 22 pre-calculates the limiter value by obtaining the Jacobian around the planned trajectory (the Jacobian at a given range of points from the planned trajectory). The pre-calculation unit 22 pre-calculates a correction coefficient (force sensor correction value) for correcting the sensor information of the force sensor 10 as a control parameter. Then, the execution unit 26 performs sensor information correction processing based on the correction coefficient. Based on the sensor information after the correction processing, a correction value for the target value is obtained. Details of the inverse Jacobian, the limiter value, and the correction coefficient will be described later. The robot trajectory is, for example, a movement trajectory of an end effector unit (end point), and the planned trajectory is a robot trajectory scheduled in advance by the control system.

  FIG. 2 shows an example of a robot system including the robot control system of this embodiment. This robot system includes a control device 300 (information processing device) and a robot 310 (robot 100 in FIG. 1). The control device 300 performs control processing for the robot 310. Specifically, control for operating the robot 310 is performed based on the operation sequence information (scenario information). The robot 310 includes an arm 320 and a hand (gripping unit) 330. Then, it operates according to an operation instruction from the control device 300. For example, operations such as gripping or moving a workpiece placed on a pallet (not shown) are performed. Further, information such as the posture of the robot and the position of the workpiece is detected based on captured image information acquired by an imaging device (not shown), and the detected information is sent to the control device 300.

  The robot control system of this embodiment is provided in, for example, the control device 300 in FIG. 2, and the robot control system is realized by, for example, hardware or a program of the control device 300. According to the robot control system of this embodiment, it is possible to reduce performance requirements for control hardware such as the control device 300 and to operate the robot 310 with high responsiveness. Although FIG. 2 shows an example of a single arm type, a multi-arm type robot such as a double arm type may be used.

2. Next, the robot control method of this embodiment will be described in detail.

2.1 Overview This embodiment aims to improve the responsiveness by minimizing the portion that requires real-time responsiveness and reduce the performance requirements of the control hardware. That is, force control and impedance control used for a robot (manipulator) are realized by a simple and low-cost control system. The reason for the high cost of the current control system is that a portion requiring high-speed hard real-time performance is large, and the high cost is allowed because the entire current system is expensive. . Therefore, in order to reduce the cost, it is necessary to reduce the portion where high-speed real-time performance is required and relax the performance requirement.

  The basic idea of this embodiment is based on the following facts. In other words, even if impedance control is used, the trajectory is often predictable.

  For example, when a double-arm or single-arm robot (manipulator) carries the target object while controlling the impedance, the trajectory can be predicted.

  For example, consider a case where an object is fitted to another object using a robot, or a case where the surface of an object is traced, such as welding. In such a case, the conventional method does not require any groping, and the position is determined to some extent by using a jig, or in most cases, the position is determined by using an imaging device in recent years. is there. Also in this case, the position and trajectory are almost predictable.

  In short, both in the conventional case and in recent years, when using the impedance control, the trajectory is very rare, and in many cases it can be predicted in advance. That is, using impedance control or the like, the trajectory change due to external constraints is very small.

  In this way, if the main trajectory is predictable and the trajectory change due to impedance control or the like is small, by adopting the optimal structure and control method for it, the responsiveness is improved and the control hardware Performance requirements can be reduced. That is, Jacobian and inverse Jacobian related to impedance control depend on the posture of the robot. However, if the trajectory can be predicted, the Jacobian or the inverse Jacobian can be calculated in advance for a certain point on the trajectory. This part does not require real-time responsiveness.

  As a result, it is possible to improve responsiveness by pre-calculating values at each point, and to reduce hardware performance requirements by not requiring real-time responsiveness to pre-calculate the values. .

  The basic idea of the present embodiment as described above is summarized as follows.

  That is, in the conventional case, it has been necessary to calculate a parameter that changes in response to a change in posture with respect to a trajectory that changes sequentially without any time delay. However, the trajectory can be predicted even if impedance control is performed, except when there is a time-varying interaction with a person or other equipment. This is a significant part of industrial machine tasks. That is, in an industrial machine, a situation where the track changes due to an unpredictable external force is an abnormal situation.

  If the trajectory can be predicted, there are quite a few parts that can be pre-calculated out of the parts that require high-speed real-time performance. This pre-calculation is performed in advance and put in a FIFO (first-in first-out memory). By appropriately taking the length of the FIFO, the real-time property of the pre-calculation process is greatly relaxed. Real-time processing is processing in which the worst value of response time (the worst value of time until an actual response occurs after the occurrence of an event) is guaranteed. The value of the response is gradually lowered with time, and the soft real-time processing is rapidly reduced, and the hard real-time processing is rapidly lowered. For example, the robot control and air bag control processes are hard real-time processes.

  In the present embodiment, the pre-calculation is performed at a time interval such that the FIFO does not become empty. The part that performs high-speed hard real-time processing reads out the control parameter from the FIFO and uses the control parameter.

  Further, when the result of the hard real-time processing exceeds the range assumed in the pre-calculation, this is notified to the pre-calculation unit. Also performs specified error handling.

  Specifically, in the present embodiment, the impedance control unit that has been conventionally integrated is divided into an upper control layer and a lower control layer. That is, as shown in FIG. 1, the pre-calculation unit 22 that is the upper control layer, the execution unit 26 that is the hard real-time processing unit of the lower control layer, and the FIFO 24 interposed therebetween are divided. Here, the execution unit 26 and the FIFO 24 are given for each joint. In addition, although the patent document 3 mentioned above also performs the hierarchy division | segmentation, the content is completely different from this embodiment.

  In addition, the time intervals of processing in these control layers are made different. That is, the execution unit 26, which is a lower control layer that performs real-time response, performs processing at a time interval of, for example, 100 μs to 1 ms. Processing is performed at time intervals of tens of ms. That is, multirate processing is performed.

  The values pre-calculated together in the upper control layer are sent to the FIFO 24 which is a first-in first-out memory, and the lower control layer sequentially reads the pre-calculated values from the FIFO and performs hard real-time processing.

  Here, the parameters pre-calculated by the pre-calculation unit 22 and communicated through the FIFO 24 are, for example, an inverse Jacobian, a limiter value, and a sensor orientation correction coefficient (hand weight correction coefficient), which will be described in detail later.

  The pre-calculation unit 22 performs pre-calculation of the above parameters. That is, the pre-calculation unit 22 that is the upper control layer performs pre-calculation at appropriate intervals while looking at the state of the FIFO 24. The appropriate interval may be such that the pre-calculation can be performed with a margin so that the FIFO 24 does not become empty. Specifically, the pre-calculation unit 22 is activated when the remaining data amount in the FIFO 24 decreases. Thereby, the real-time processing request to the pre-calculation unit 22 is relaxed. And the execution part 26 which is a hard real-time processing part performs an impedance control process using the correction coefficient of a sensor attitude | position, and an inverse Jacobian. The execution unit 26 notifies the precalculation unit 22 of an error when the result of the impedance control process exceeds the limiter value range. The execution unit 26 performs predetermined error processing when an error occurs.

  In this embodiment, it is assumed that the trajectory is predictable, and the limiter value is for detecting that there has been a trajectory change due to impedance control that is so large that it deviates from the assumption. In such a state, for example, an interrupt process to the upper control layer is requested.

  According to the method of the present embodiment described above, the responsiveness is improved by pre-calculating the parameters, and the performance requirement to the control hardware is reduced by not requiring real-time responsiveness for the pre-calculation. Can be realized.

2.2 Force Control, Impedance Control Next, an overview of force control and impedance control (compliance control) will be described.

  FIG. 3A shows a situation where the object OB is sandwiched between the left arm AL and the right arm AR of the robot. For example, with position control alone, there is a risk of dropping or destroying an object. With force control, a flexible object or a fragile object is sandwiched with appropriate force from both sides as shown in FIG. It can be moved.

  Further, according to the force control, as shown in FIG. 3B, it is possible to trace the surface SF of an uncertain object with the arm AM or the like. Such control cannot be realized only by position control. Further, according to the force control, as shown in FIG. 3C, after rough positioning, it is possible to probe and align the object OB into the hole HL.

  However, there is a problem that the application is limited in force control using actual mechanical parts such as a spring. In addition, in such force control using mechanical parts, it is difficult to dynamically switch characteristics.

  On the other hand, torque control for controlling the torque of the motor is simple, but there is a problem that the positional accuracy is deteriorated. In addition, a problem such as a collision occurs when an abnormality occurs. For example, in FIG. 3A, when an abnormal situation occurs and the object OB is dropped, there is no reaction force to be balanced in the torque control, so the left and right arms AL and AR collide. Problems arise.

  On the other hand, impedance control (compliance control) has the disadvantage of high versatility and safety, although it has the disadvantage of complicated control.

  FIG. 4A and FIG. 4B are diagrams for explaining the compliance control which is one of the impedance controls. Compliance means the reciprocal of the spring constant, and the spring constant represents hardness, whereas compliance means softness. Control that provides compliance, which is mechanical flexibility, when interaction between the robot and the environment works is called compliance control.

  For example, in FIG. 4A, a force sensor SE is attached to the arm AM of the robot. The robot arm AM is programmed so that its posture changes according to sensor information (force / torque information) obtained by the force sensor SE. Specifically, the robot is controlled as if a virtual spring indicated by A1 in FIG. 4A is attached to the tip of the arm AM.

  For example, it is assumed that the spring constant of the spring indicated by A1 is 100 kg / m. If this is pressed with a force of 5 kg as shown at A2 in FIG. 4B, the spring contracts by 5 cm as shown at A3. In other words, if it shrinks by 5 cm, it can be said that it is pushed with a force of 5 kg. That is, force information and position information are associated with each other.

  In the compliance control, control is performed as if the virtual spring indicated by A1 is attached to the tip of the arm AM. Specifically, the robot operates in response to the input of the force sensor SE, and is controlled to move back by 5 cm as shown in A3 with respect to the 5 kg weight shown in A2, corresponding to the force information. The position information is controlled to change.

  Such simple compliance control does not include a time term, but the control including the time term and considering the second order term is impedance control. Specifically, the second-order term is a mass term, the first-order term is a viscosity term, and the impedance control model can be expressed by an equation of motion as shown in the following equation (1).

  In the above equation (1), m is mass, μ is a viscosity coefficient, k is an elastic coefficient, f is a force, and x is a displacement from a target position. The first and second derivatives of x correspond to speed and acceleration, respectively. In the impedance control, a control system for providing the end effector unit, which is the tip of the arm, with the characteristic of the above formula (1) is configured. That is, control is performed as if the tip of the arm has the virtual mass, virtual viscosity coefficient, and virtual elastic coefficient represented by the above equation (1).

  As described above, impedance control is control that makes a contact with an object with a viscosity coefficient and an elastic coefficient set as an object in a model in which a viscous element and an elastic element are connected to the mass of the tip of the arm in each direction. .

  For example, as shown in FIG. 5A, let us consider a control in which an object OB is held by robot arms AL and AR and moved along a trajectory TR. In this case, the trajectory TRL is a trajectory through which the point PL set inside the left side of the object OB passes, and is a virtual left-hand trajectory determined on the assumption of impedance control. The trajectory TRR is a trajectory through which the point PR set inside the right side of the object OB passes, and is a virtual right-hand trajectory determined on the assumption of impedance control. In this case, the arm AL is controlled so that a force corresponding to the distance difference between the tip of the arm AL and the point PL is generated. The arm AR is controlled so that a force corresponding to the distance difference between the tip of the arm AR and the point PR is generated. In this way, it is possible to realize impedance control that moves the object OB while grasping it softly. In the impedance control, even if the situation where the object OB falls as shown by B1 in FIG. 5A occurs, the ends of the arms AL and AR are points PL and PR as shown by B2 and B3. It is controlled to stop at the position. That is, if the virtual trajectory is not a collision trajectory, it is possible to prevent the arms AL and AR from colliding.

  In addition, as shown in FIG. 5B, when the control is performed so that the surface SF of the object is traced, the impedance control is performed according to the distance difference DF between the virtual trajectory TRVA and the tip with respect to the tip of the arm AM. It is controlled so that the applied force works. Therefore, the arm AM can be controlled to trace the surface SF while applying a force.

2.3 Jacobian, inverse Jacobian In the above impedance control, compliance control, etc., the operation is basically calculated in a Cartesian coordinate system. Inverse kinematics processing is required to translate the movement of the robot's hand in the Cartesian coordinate system into the movement of each joint of the robot. For example, in FIG. 6A, when the force indicated by C1 is applied and the tip PT of the arm AM moves as indicated by C2, in order to obtain the angles of the joints J1 and J2, inverse kinematics processing is performed. I need it.

  However, as shown in FIG. 6B, inverse kinematics processing is generally very difficult processing. On the other hand, the forward kinematics process, which is a process for setting the values of each joint of the robot and converting it into the movement of the hand, is a simple process. In such processing, if the posture change is minute, Jacobian using forward kinematics can be used.

  For example, as shown in the following equation (2), consider a function f that takes m variables as inputs and gives n outputs. Alternatively, n sets of functions f which take m variables as inputs and give one output are prepared.

  In this case, the Jacobian (Jacobi matrix) is a matrix represented by the following expression (3).

  That is, the Jacobian is a linear approximation of the relationship between x and y in the vicinity of “a certain value” of x. Therefore, in general, when “a certain value” of x changes, the Jacobian also changes.

  Here, if x is the joint angle and y is the posture of the hand of the robot (xyzuvw), the function of the above equation (2) becomes a forward kinematic function, which can be easily obtained.

  If we consider the function of the above equation (2) as a forward kinematics function, the Jacobian (Jacobi matrix) of the above equation (3) shows the minute change of the hand when each joint changes minutely in a certain posture. It becomes the relational expression to give. That is, in FIG. 7 (A), when the angle of the joint J1 is displaced by a minute unit amount, the relational expression represents how much the distal end PT is displaced. Alternatively, in FIG. 7B, when the angle of the joint J2 is displaced by a minute unit amount, the relational expression represents how much the tip PT is displaced.

  In this case, the correspondence between the minute change of each joint angle and the minute change of the hand varies depending on the posture. In other words, the Jacobian needs to be calculated each time the posture changes.

  As described above, the inverse kinematics process for obtaining the joint angle from the position of the hand is generally a very difficult process.

  However, if the inverse Jacobian, which is the inverse matrix of Jacobian, can be obtained as shown in the following equation (4), a relational expression for converting a minute change of the hand into a minute change of each joint angle can be obtained.

  However, in general, the Jacobian is not a square matrix and rank (rank) is low, and there is no inverse matrix in the normal sense. For example, in a 7-axis robot, there are only 6 degrees of freedom of the hand, so a 7 × 6 matrix is formed instead of 7 × 7. However, there can be a generalized inverse matrix having properties similar to the inverse matrix of ordinary meaning. By using this generalized inverse matrix, a process that requires an inverse kinematics function can be executed using the forward kinematics function. By utilizing this, the present embodiment simplifies the processing. Specifically, the inverse Jacobian matrix elements are pre-calculated as control parameters, and the correction value of the target value is obtained based on the pre-calculated inverse Jacobian matrix elements.

2.4 Limiter Value Now, let us consider a case where the tip PT of the arm AM is moved along the planned trajectory TRP in FIG. In this case, in a robot for industrial use, the robot trajectory does not deviate as much as the trajectory TRA, and when it becomes such a trajectory TRA, it can be determined that it is in an abnormal state.

  Therefore, in the present embodiment, as a control parameter, a limiter value (for example, a limiter value of a motor encoder value) indicating an allowable range RT of the robot trajectory with respect to the planned trajectory TRP is pre-calculated. Then, it is determined whether or not the robot trajectory exceeds the allowable range RT with respect to the planned trajectory TRP. If the allowable range is exceeded, an interrupt indicating that is generated.

  Specifically, in this embodiment, the limiter value is pre-calculated by obtaining the Jacobian around the planned trajectory TRP. That is, the limiter value is pre-calculated with the range in which the Jacobian error is allowed as the allowable range RT. For example, a limiter value is pre-calculated by setting a range where an error of a Jacobian matrix element is equal to or less than a given value as an allowable range RT.

  In this case, as shown in FIG. 8, the allowable range RT is not constant, and the size of the allowable range RT depends on the posture of the robot arm AM. For example, the allowable range RT is not the same in the case of the posture shown by D1 in FIG. 8 and the case of the posture shown by D2. The values of Jacobian (reverse Jacobian) are different between D1 and D2. For this reason, in this embodiment, the limiter value is pre-calculated by obtaining the Jacobian around each point of the planned trajectory TRP (by obtaining the Jacobian in each posture).

  In this case, the posture for calculating the Jacobian is the posture shown by the solid line in FIG. 9 and is not the posture shown by the broken line. That is, as described with reference to FIG. 5A, in the impedance control, control is performed with a virtual trajectory passing through the points PL and PR set inside the object as a target. For example, control is performed so that a force corresponding to the difference between the tip PT of the arm AM in FIG. 9 and the virtual point VPT (PL, PR) is generated. When calculating the Jacobian, the Jacobian is calculated in the posture indicated by the solid line, not the posture indicated by the broken line corresponding to the virtual trajectory.

2.5 Configuration of Control System FIG. 10 shows a basic configuration example of a control system when force feedback is not included.

  The trajectory generation unit 562 generates trajectory information p (xyzuvw) and outputs it to the inverse kinematics processing unit 564. Here, the trajectory information p includes, for example, position information (xyz) of the tip of the arm (end effector section) and rotation information (uvw) about each axis. Then, the inverse kinematics processing unit 564 performs inverse kinematics processing based on the trajectory information p, and generates and outputs a joint angle θ of each joint as a target value. Then, by performing motor control based on the joint angle θ, the operation control of the robot arm is performed. In this case, the control of the motor (M) in FIG. 10 is realized by known PID control. Since this PID control is a known technique, a detailed description thereof is omitted here.

  In FIG. 10, the trajectory generation unit 562 and the inverse kinematics processing unit 564 constitute a target value output unit. The processing of this target value output unit is the overall processing of the robot. On the other hand, the latter-stage motor control is control for each joint.

  In the prior art of Patent Document 3 described above, the upper control layer and the lower control layer are divided and divided. However, the upper control layer in Patent Document 3 is a part of the trajectory generation unit 562 and the inverse kinematics processing unit 564 in FIG. 10, and the lower control layer is a part of motor control, which is different from the division division of this embodiment. It has become.

  FIG. 11 shows a basic configuration example of the control system in the case of including force feedback. 11, a force sensor 510, a posture correction unit 532, a hand / tool self-weight correction unit 534, a motion equation processing unit 536, and a forward kinematics processing unit 540 are further provided with respect to FIG. Yes.

  In FIG. 11, in response to sensor information from the force sensor 510, the posture correction unit 532 corrects the posture of the sensor, and the hand / tool self-weight correction unit 534 corrects the hand / tool self-weight. Then, the equation of motion processing unit 536 performs processing for obtaining a solution of the equation of motion as shown in the above equation (1), and outputs a correction value Δp. By correcting the trajectory information p with this correction value Δp, a correction process for the joint angle θ, which is a target value, is performed. The forward kinematics processing unit 540 performs forward kinematics processing, obtains the trajectory information p ′ of the robot, and feeds it back to the trajectory generation unit 562. Information for specifying the posture is output to the posture correction unit 532 and the hand / tool correction unit 534.

  FIG. 12 shows a basic configuration example of the control system of the present embodiment. This control system includes force feedback as in FIG. FIG. 12 differs from FIG. 11 in that an inverse Jacobian processing unit 38 is further provided in FIG.

  Specifically, in FIG. 11, p output from the trajectory generation unit 562 is corrected by Δp from the motion equation processing unit 536. On the other hand, in FIG. 12, based on Δp from the motion equation processing unit 536, the inverse Jacobian processing unit 38 performs an inverse Jacobian process for obtaining Δθ. That is, in the above equation (4), Δy (Δy1, Δy2...) Becomes Δp and Δx (Δx1, Δx2...) Becomes Δθ, and reverse Jacobian processing is performed to obtain Δθ. Then, the joint angle θ as the target value is corrected by the obtained Δθ. With such a configuration, the processing result of the equation of motion can be decomposed for each joint.

  In FIG. 12, in the processing of the posture correction unit 32, the hand / tool self-weight correction unit 34, the inverse Jacobian processing unit 38, and the forward kinematics processing unit 40, the value of the parameter used changes according to the robot posture.

  However, if the posture change of the robot is pre-programmed or the difference from it is small, these parameter values can be calculated in advance when the trajectory is programmed. Therefore, in the present embodiment, a method for obtaining these parameter values by pre-calculation is adopted.

  The above-mentioned “small difference” means that when the error in each correction becomes an error in the solution of the equation of motion, the error is within an allowable range, and the result of the Jacobian processing as a result thereof Is within an acceptable range.

  This “small difference” is dependent on the small difference between the actual posture of the robot and the programmed posture as described above, and the difference is larger than expected. In some cases, it must be handled as an error. Therefore, in this embodiment, as described with reference to FIG. 8, a limiter value corresponding to the allowable range RT is calculated in advance, and an interrupt is generated when it is determined that the robot trajectory exceeds the allowable range RT. ing.

2.6 Detailed Configuration Example FIG. 13 shows a detailed configuration example of the robot control system of this embodiment. FIG. 13 shows a configuration corresponding to the control system described in FIG. Note that the robot control system of the present embodiment is not limited to the configuration shown in FIG. 13, and various modifications such as omitting some of the components or adding other components are possible. For example, as the robot control system of the present embodiment, the configuration of the control system described with reference to FIG.

  The inverse Jacobian pre-calculation unit 42, limiter value pre-calculation unit 44, and correction coefficient pre-calculation unit 46 in FIG. 13 correspond to the pre-calculation unit 22 in FIG. Further, the FIFOs 50, 52, and 54 in FIG. 13 correspond to the FIFO 24 in FIG. Further, the hand / tool self-weight / posture correction unit 35, the motion equation processing unit 36, the inverse Jacobian processing unit 38, the limiter processing unit 39, and the interrupt generation unit 48 of FIG. 13 correspond to the execution unit 26 of FIG. The FIFO 56 is a FIFO for storing the joint angle θ from the inverse kinematics processing unit 64.

  The inverse Jacobian pre-calculation unit 42 pre-calculates the inverse Jacobian (matrix element of the inverse Jacobian) of the above formula (4) and stores the pre-calculated inverse Jacobian (value corresponding to the inverse Jacobian) in the FIFO 50. Then, the inverse Jacobian processing unit 38 reads out the pre-calculated inverse Jacobian from the FIFO 50 at each arithmetic processing execution timing, performs the inverse Jacobian processing as shown in the above-described equation (4), and corrects the joint angle. The correction value of is output.

  The limiter value pre-calculating unit 44 pre-calculates the limiter value corresponding to the allowable range of the robot trajectory described with reference to FIG. 8 and stores the pre-calculated limiter value in the FIFO 52. Then, the limiter processing unit 39 reads the limiter value calculated in advance from the FIFO 52 at each calculation processing execution timing, and determines whether or not the robot trajectory exceeds the allowable range with respect to the planned trajectory. When it is determined that the allowable range has been exceeded, the interrupt generation unit 48 generates an interrupt indicating that the allowable range has been exceeded.

  The correction coefficient pre-calculation unit 46 pre-calculates a correction coefficient for correcting the sensor information of the force sensor 10 and stores the pre-calculated correction coefficient in the FIFO 54. Then, the hand / tool self-weight / posture correction unit 35 performs sensor information correction processing based on a pre-calculated correction coefficient. The motion equation processing unit 36 performs processing for obtaining a solution of the motion equation corresponding to the above-described equation (1) using the sensor information after the correction processing, and outputs the processing result to the inverse Jacobian processing unit 38. Then, the inverse Jacobian processing unit 38 performs the inverse Jacobian processing shown in the above equation (4) based on the processing result from the motion equation processing unit 38 and the pre-calculated inverse Jacobian, thereby correcting the joint angle. Generate a value.

  Here, the hand / tool self-weight / posture correction unit 35 corresponds to the hand / tool self-weight correction unit 34 and the posture correction unit 32 of FIG. 12. Here, the hand / tool self-weight correction is a correction process for offsetting the influence of the weight of the robot hand and the weight of the tool held by the hand from the sensor information (force information) from the force sensor 10. The posture correction is a correction process for offsetting the influence of the posture of the force sensor 10 from the sensor information (force information). These hand tool self-weight correction and posture correction can be expressed as, for example, the following expression (5).

  In the above equation (5), Fx, Fy, Fz, Fu, Fv, and Fw are force information and torque information that are sensor information from the force sensor 10. Bx, By, Bz, Bu, Bv, and Bw are bias terms. Then, the corrected sensor information (force information, torque information) fx, fy, fz, fu, fv, fw is input to the motion equation processing unit 36. Since the data has a fixed value, the substantial correction coefficient is 6 × 7 = 42. Since these hand tool self-weight correction and posture correction are known correction processes, detailed description thereof will be omitted.

  FIG. 14 is a diagram for explaining processing of this embodiment using a FIFO. In FIG. 14, TM1, TM2, TM3,... Are calculation processes of the execution unit 26 (hand / tool self-weight / posture correction unit 35, motion equation processing unit 36, inverse Jacobian processing unit 38, limiter processing unit 39, etc.). It is execution timing. As described above, in the present embodiment, the execution unit 26 uses the FIFO (FIFO 50) to calculate the control parameters (inverse Jacobian, limiter value, correction coefficient) calculated in advance at the respective arithmetic processing execution timings of TM1, TM2, TM3. , 52, 54).

  In E1 of FIG. 14, the remaining data amount of the FIFO (remaining data amount of the control parameter) is equal to or less than the threshold value, so that the execution unit 26, which is the lower control layer, is the upper control layer as shown in E2. A pre-calculation request for control parameters is made to the pre-calculation unit 22 (inverse Jacobian pre-calculation unit 42, limiter value pre-calculation unit 44, correction coefficient pre-calculation unit 46). Then, the pre-calculation unit 22 performs pre-calculation of the control parameters, and stores the pre-calculated control parameters in the FIFO as indicated by E3.

  In this case, since the control parameters previously calculated in the FIFO remain in the FIFO, the timing indicated by E4 is an interval that is sufficiently longer than the calculation processing execution timings of TM1, TM2, TM3,. Can take. Therefore, the execution unit 26 requires high-speed and hard real-time processing, but the pre-calculation unit 22 requires a certain amount of processing delay, and therefore requires low-speed or soft real-time processing. It will only be done. Accordingly, the pre-calculation unit 22 can execute a calculation process with a high load such as obtaining an inverse Jacobian, a limiter value, and a correction coefficient with sufficient time. On the other hand, since the execution unit 26 does not need to perform a heavy load calculation process such as obtaining an inverse Jacobian, a limiter value, or a correction coefficient, it can cope with high-speed and hard real-time processing. It becomes possible.

  FIG. 15 is a diagram illustrating an example of a connection relationship between the robot control unit 80 and the execution unit 26. FIG. 15 shows a case where there are two motors (drive units in a broad sense) controlled by the robot control unit 80 of FIG.

  The target value (joint angle) from the inverse kinematics processing unit 64 is input to the motor control unit 84-1 via the FIFO 56-1. In this case, the target value is corrected by the correction value from the inverse Jacobian processing unit 38 and input to the motor control unit 84-1. Then, the motor control unit 84-1 controls the motor M1 based on the corrected target value.

  Similarly, the target value (joint angle) from the inverse kinematics processing unit 64 is input to the motor control unit 84-2 via the FIFO 56-2. In this case, the target value is corrected by the correction value from the inverse Jacobian processing unit 38 and input to the motor control unit 84-2. Then, the motor control unit 84-2 controls the motor M2 based on the corrected target value.

  Further, the limiter value pre-calculated by the limiter value pre-calculating unit 44 is input to the limiter processing unit 39-1 via the FIFO 52-1. The limiter processing unit 39-1 determines whether or not the robot trajectory exceeds the allowable range based on the limit value calculated in advance and the encoded value (rotation angle detection information) from the motor control unit 84-1. When it is determined that the allowable range is exceeded, the interrupt generation unit 48 generates an interrupt.

  Similarly, the limiter value pre-calculated by the limiter value pre-calculating unit 44 is input to the limiter processing unit 39-2 via the FIFO 52-2. The limiter processing unit 39-2 determines whether or not the robot trajectory exceeds the allowable range based on the limit value calculated in advance and the encoded value from the motor control unit 84-2. When it is determined that the allowable range is exceeded, the interrupt generation unit 48 generates an interrupt.

3. Detailed Processing Next, a detailed processing example of this embodiment will be described with reference to the flowcharts of FIGS.

  FIG. 16 is a flowchart of the overall processing of the execution unit which is a hard real-time processing unit.

  First, it is determined whether it is time to update the control parameter (step S1). If it is the update timing, as described with reference to FIG. 14, it is determined whether or not there is remaining data (control parameter) in the FIFO (step S2). If there is no remaining data, an interrupt without a control parameter is generated (step S3), and the process proceeds to an interrupt process (step S4). On the other hand, if there is remaining data in the FIFO, a control parameter is acquired from the FIFO (step S5).

  Next, it is determined whether or not the remaining data amount of the FIFO is larger than a threshold value (step S6). If it is equal to or less than the threshold value, as described with reference to FIG. 14, the pre-calculation unit is requested to create a control parameter (step S7). On the other hand, when the remaining data amount is larger than the threshold value, the impedance control process is executed (step S8).

  Next, it is determined whether or not the impedance control process is normally completed (step S9). And when not complete | finished normally, the update of a control parameter is requested | required with respect to the prior calculation part (step S10). On the other hand, if the process ends normally, the process returns to step S1.

  FIG. 17 is a flowchart of the impedance control process in step S8 of FIG.

  First, a 6-dimensional input value is obtained from the force sensor (step S11), and a product with a 7 × 7 correction matrix is calculated (step S12). That is, Fx, Fy, Fz, Fu, Fv, and Fw of the above equation (5) are obtained, and hand / self weight correction and posture correction are performed. Then, impedance filter processing is performed based on the obtained corrected value (step S13). That is, the above-described equation of motion (1) is processed.

  Next, the product of the output value of the impedance filter and the 7 × 6 Jacobian matrix is obtained (step S14). That is, the matrix calculation of the above-described equation (4) is performed. And the correction process which adds the value obtained by step S14 with respect to the command value of each joint is performed (step S15).

  Next, an encoder value (rotation angle) of the motor is obtained (step S16). Then, it is determined whether or not the encoder value is within the limiter value range (step S17). If the encoder value is not within the limiter value range, a limiter error interrupt is generated (step S18). On the other hand, when it is within the range of the limiter value, it returns to the Loop of FIG. 16 as being in a normal state.

  18A to 18C are flowcharts for interrupt processing.

  As shown in FIG. 18A, when an interrupt without a control parameter occurs in step S3 of FIG. 16, a preset stop process is entered (step S21).

  As shown in FIG. 18B, when a limiter error interrupt occurs in step S18 of FIG. 17, a preset stop process is entered (step S22). That is, the upper limit controller (upper control layer) sets an allowable range limiter value, and stops in any event that exceeds that limit value.

  Alternatively, as shown in FIG. 18C, when a limiter error interrupt is generated in step S18 of FIG. 17, NOP (No OPeration) may be set, and the loop may return to Loop as an abnormal situation occurs (step S23). . That is, the upper controller (upper control layer) sets a limiter value that allows for a margin, and allows overshoot of the lower controller (lower control layer). The host controller that received the interrupt updates the trajectory and overwrites the FIFO with new data. On the other hand, the lower controller continues to operate with new data. If the upper controller cannot update the trajectory, an interrupt to the lower controller is generated to stop the operation.

  FIG. 19 is a flowchart for the processing of the pre-calculation unit.

  First, the pre-calculation unit waits for an interrupt to occur (step S31), and if an interrupt occurs, determines whether the interrupt is a control parameter-less interrupt (step S32). In the case of an interrupt without a control parameter, the control parameter is calculated and set in the FIFO (step S33). Then, the hard real-time processing unit (execution unit) is restarted (step S34).

  On the other hand, if it is not an interrupt without a control parameter, it is determined whether or not it is a control parameter creation interrupt (step S35). In the case of the control parameter creation interrupt, the FIFO free data amount is acquired (step S36), the control parameter is calculated and set in the FIFO (step S37).

  If it is not a control parameter creation interrupt, it is determined whether or not it is a control parameter update interrupt (step S38). In the case of a control parameter update interrupt, the current robot posture is acquired to obtain a recovery trajectory (step S39). Then, the control parameter is calculated and overwritten in the FIFO (step S40).

  FIG. 20 is a flowchart of the parameter calculation process.

  First, it is determined whether or not there is a vacancy in the FIFO (step S41). If there is a vacancy, a planned trajectory is obtained (step S42). Then, the Jacobian and reverse Jacobian of the planned trajectory are obtained (Step S43), and the value of the reverse Jacobian is set in the FIFO (Step S44).

  Next, as described with reference to FIG. 8, a Jacobian around the planned trajectory is obtained, a range in which the difference is equal to or smaller than a predetermined value is obtained (step S45), and a limiter value is set in the FIFO (step S46). Further, a force sensor correction value (correction coefficient) for the planned trajectory is obtained (step S47), and the force sensor correction value is set in the FIFO (step S48).

  Although the present embodiment has been described in detail as described above, it will be easily understood by those skilled in the art that many modifications can be made without departing from the novel matters and effects of the present invention. Accordingly, all such modifications are intended to be included in the scope of the present invention. For example, a term described at least once together with a different term having a broader meaning or the same meaning in the specification or the drawings can be replaced with the different term in any part of the specification or the drawings. All combinations of the present embodiment and the modified examples are also included in the scope of the present invention. In this embodiment, the robot control system, the configuration and operation of the robot system, the control parameter pre-calculation method, the method for storing the control parameter in the FIFO, the method for executing the arithmetic processing based on the control parameter, the configuration of the control system, etc. It is not limited to what was demonstrated, Various deformation | transformation implementation is possible.

OB object, AL, AR, AM arm, SF surface, HL hole, J1-J3 joint,
PT tip, M, M1, M2 motor,
10 force sensor, 20 force control unit (impedance control unit), 22 pre-calculation unit,
24 FIFO, 26 execution unit, 32 posture correction unit, 34 hand / tool self-weight correction unit,
35 hand / tool self-weight / posture correction unit, 36 equation of motion processing unit,
38 Reverse Jacobian processing unit, 39, 39-1, 39-2 Limiter processing unit,
40 forward kinematics processing unit, 42 inverse Jacobian pre-calculation unit,
44 Limiter value pre-calculation unit, 46 Correction coefficient pre-calculation unit,
50, 52, 52-1, 52-2, 54, 56, 56-1, 56-2 FIFO,
60 target value output unit, 62 trajectory generation unit, 64 inverse kinematics processing unit,
80 Robot controller, 82-1 to 82-N Drive controller,
84-1, 84-2 Motor control unit, 100 robot, 102-1 to 102-N drive unit,
300 control device, 310 robot, 320 arm, 330 hand

Claims (12)

A target value output unit for outputting a target value for feedback control of the robot;
A force control unit that performs force control based on sensor information from the force sensor and outputs a correction value of the target value;
A robot control unit that performs the feedback control of the robot based on the target value corrected by the correction value;
Including
The force control unit
A pre-calculation unit that pre-calculates control parameters that change according to the posture of the robot;
An execution unit for executing a calculation process for obtaining the correction value based on the sensor information and the control parameter;
A robot control system comprising: In claim 1,
The force control unit
Including a FIFO in which the control parameters calculated by the pre-calculation unit are stored;
The execution unit is
The robot control system, wherein the control parameter is read from the FIFO, and the arithmetic processing for obtaining the correction value is executed based on the read control parameter. In claim 2,
The FIFO includes
The robot control system according to claim 1, wherein the control parameter used in the execution unit is stored in a first-in first-out manner at each timing when the calculation process of the execution unit is executed. In claim 2 or 3,
The execution unit is
When the remaining data amount of the FIFO is equal to or less than a given threshold value, a pre-calculation request is made to the pre-calculation unit that requests to pre-calculate the control parameter. Robot control system. In any one of Claims 1 thru | or 4,
The pre-calculation unit
Pre-calculate inverse Jacobian matrix elements as the control parameter,
The execution unit is
The robot control system characterized in that the correction value of the target value is obtained based on the matrix element of the inverse Jacobian calculated in advance. In any one of Claims 1 thru | or 5,
The pre-calculation unit
As the control parameter, a limiter value indicating the allowable range of the robot trajectory with respect to the planned trajectory is pre-calculated,
The execution unit is
An interrupt indicating that the allowable range has been exceeded is generated when it is determined that the robot trajectory has exceeded the allowable range with respect to the planned trajectory. In claim 6,
The pre-calculation unit
A robot control system, wherein the limiter value is pre-calculated by obtaining a Jacobian around the planned trajectory. In any one of Claims 1 thru | or 7,
The pre-calculation unit
As the control parameter, pre-calculate a correction coefficient for correcting the sensor information of the force sensor,
The execution unit is
A robot control system, wherein the sensor information is corrected based on the correction coefficient. In any one of Claims 1 thru | or 8.
The target value output unit
A trajectory generator that outputs trajectory information of the robot;
An inverse kinematics processing unit that performs inverse kinematics processing based on the trajectory information from the trajectory generation unit, and outputs joint angle information of the robot as the target value;
A robot control system comprising: In any one of Claims 1 thru | or 9,
The force control unit
A robot control system that performs impedance control as the force control. In any one of Claims 1 thru | or 10.
The robot control system according to claim 1, wherein the target value is joint angle information of the robot, and the correction value is a correction value of the joint angle information. The robot control system according to any one of claims 1 to 11,
The robot;
A robot system characterized by including:

Images