JP6003312B2 - Robot system - Google Patents

Description

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

  In a work using a robot or the like, there is a case where a specific operation is desired to be performed while controlling force, not just position control. Force control can be used, for example, when handling a soft object or a fragile object without breaking it, or tracing with a constant force on a surface having a complicated shape. In order to perform these force controls, it is necessary to detect the force using a force sensor and incorporate the magnitude and direction of the force into the control loop.

  However, the force sensor detects not only the external force to be detected, but also the force due to the effect of the hand structure attached to the force sensor, and the force caused by the tool or work gripped by the hand (hereinafter referred to as offset). These offsets are not constant values and change according to various conditions. Therefore, it is difficult to separate the external force to be detected from the offset.

  On the other hand, Patent Document 1 discloses a technique for correcting the weight of the hand structure and the gyro effect caused by the rotation. Patent Document 1 assumes an operation of handling a rotating body such as drilling, deburring, grinding, and polishing using a robot. Specifically, the center of gravity of the tool, the center of gravity of the rotating body, and the position of the contact point in the sensor coordinate system are used as known information, and the moment that changes depending on the arm posture and the gyro effect generated by the rotating body are corrected to detect substantial external force. To do.

  Patent Document 2 discloses a technique for correcting effects such as centrifugal force in addition to Patent Document 1. Patent Document 2 corrects the centrifugal force due to the movement of the arm, and the correction processing is realized by using the mass of the tool, the position of the center of gravity, the position of the arm, the speed, the acceleration detection unit, the dynamic term, and the gravity calculation unit. Is done.

  Patent Document 3 discloses a technique for gripping and releasing an object by simply estimating the external force in the vertical direction, instead of performing fine external force correction as in Patent Documents 1 and 2. Patent Document 3 converts the force detected by the hand from the hand coordinate system to the base coordinate system of the robot, thereby performing grip control.

JP-A-6-339885 JP 2008-142810 A JP 2007-276112 A

  The methods of Patent Documents 1 to 3 are based on the assumption that a physical model (for example, a weight distribution) of a mechanical mechanism (including a hand structure and, in some cases, a tool held by the hand structure) is known. Yes. Therefore, when the physical model of the mechanical mechanism is unknown or when the physical model changes according to the situation, the influence of the mechanical mechanism on the force value (sensor value of the force sensor) is corrected appropriately. Difficult to do.

  According to some aspects of the present invention, when it is difficult to apply a known physical model to a mechanical mechanism, a control system, a program, and a mechanical apparatus that appropriately correct the influence of the mechanical mechanism on the force sensor Can be provided.

  One aspect of the present invention is a control system for a mechanical device, which is a force that acquires a force sense value from a force sensor provided corresponding to a mechanical mechanism of the mechanical device, and performs correction processing of the force sense value. A sense value correction unit; and a control unit that controls the mechanical device based on the force value after the correction process. The force value correction unit is obtained in a plurality of postures of the mechanical device. The present invention relates to a control system that performs the correction processing based on the plurality of force values obtained.

  In one embodiment of the present invention, a force sense value is acquired from a force sensor provided corresponding to a mechanical mechanism, and the mechanical device is controlled using a result obtained by performing correction processing on the force sense value. At this time, since the mechanical device takes a plurality of postures and performs the correction processing based on the force value obtained in each posture, a desirable correction processing (for example, high-precision correction processing) and the like can be performed.

  In the aspect of the invention, the haptic value correction unit may calculate the first to Nth haptic values obtained in the first to Nth (N is an integer of 2 or more) postures of the mechanical device. Based on the estimated mass center position of the mechanical mechanism, the correction process may be performed based on the estimated mass center position.

  This makes it possible to estimate the mass center position of the mechanical mechanism based on the force values acquired in a plurality of postures.

  In the aspect of the invention, the force value correction unit may act on the force sensor by the mechanical mechanism based on the mass of the mechanical mechanism, the mass center position, and the posture of the mechanical device. The correction process may be performed by estimating the force value.

  This makes it possible to analyze and correct the influence of the mechanical mechanism on the force sensor based on the estimated mass center position (physical model of the mechanical mechanism in a broad sense).

  In the aspect of the invention, the haptic value correction unit estimates the center of mass position of the mechanical mechanism based on a first haptic value obtained in the first posture of the mechanical device. And when the force sensation value correcting unit determines that the center of mass position cannot be estimated, the control unit performs a second posture different from the first posture. The haptic value correction unit is configured to control the machine based on a second haptic value acquired after the control unit performs control to change the mechanical device to the second posture. The mass center position of the mechanism may be estimated, and the correction process may be performed based on the estimated mass center position.

  Accordingly, when it is determined that the estimation process of the center of mass position is impossible with the force value in the first posture, it is possible to retry the estimation processing after changing the posture.

  In the aspect of the invention, the force sense value correction unit may perform error processing when it is determined that the estimated mass center position is not within a predetermined error.

  Thereby, the estimation accuracy of the center of mass position is determined, and when it is determined that the accuracy is low, it is possible to respond by error processing.

  In one aspect of the present invention, when it is determined that the center of mass position estimated by the force sense value correction unit is not within a predetermined error, the control unit gives an attitude of the mechanical device. The haptic value correction unit acquires the haptic value after the control unit performs the control so that the mechanical device assumes the given posture, and the acquired force A value corresponding to the sense value may be set as an offset value, and an offset removal process using the set offset value may be executed as the error process.

  Accordingly, when it is determined that the estimation accuracy of the mass center position is low, it is possible to appropriately perform the correction process by shifting to the offset removal process.

  In one aspect of the present invention, the force sensor acquires a translation force Fx, Fy, Fz about the X axis, the Y axis, and the Z axis, and a moment Mx, My, Mz around each axis. In the case of a sensor, the haptic value correction unit includes a first plane determined by Fx, Fy and Mz, a second plane determined by Fy, Fz and Mx, and Fz, Fx and My. A third plane to be determined may be obtained, and an intersection of the first to third planes may be estimated as the mass center position.

  As a result, when a 6-axis force sensor is used as the force sensor, it is possible to estimate the center of mass position as an intersection of planes specified from the force values.

とした場合に、前記力覚値補正部は、前記質量中心位置を表す３次元ベクトルｐをＭｐ＝ｑに従って求めることで、前記質量中心位置を推定してもよい。 In one aspect of the present invention, the first plane x-intercept is A1, the y-intercept is B1, the z-intercept is C1, the second plane x-intercept is A2, the y-intercept is B2, and the z-intercept is C2, and the third plane x-intercept is A3, y-intercept is B3, z-intercept is C3, matrix M and vector q are

In this case, the force value correcting unit may estimate the mass center position by obtaining a three-dimensional vector p representing the mass center position according to Mp = q.

  Thereby, it becomes possible to obtain | require a mass center position as a solution of the numerical formula which consists of a matrix and a vector.

  In the aspect of the invention, the haptic value correction unit may determine that the center of mass position cannot be estimated when the matrix M is not regular.

  Accordingly, it is possible to determine whether the center of mass position can be estimated based on whether the matrix M is regular.

  Further, in one aspect of the present invention, the haptic value correction unit calculates a runtime parameter calculation unit that calculates information including the mass center position and the mass of the mechanical mechanism as a runtime parameter, and the calculated execution. A runtime parameter storage unit that stores a time parameter.

  As a result, the center of mass position and the like can be calculated as runtime parameters, and the calculated runtime parameters can be stored.

  In one aspect of the present invention, the force sense value correction unit reads the runtime parameter stored in the runtime parameter storage unit, and based on the runtime parameter and the attitude of the mechanical device, A force that the mechanical mechanism exerts on the force sensor may be calculated as an offset value, and the correction processing of the force value may be performed based on the calculated offset value.

  This makes it possible to obtain an offset value (correction amount) based on the stored runtime parameter and posture information, and to perform a correction process using the obtained offset value.

  In one aspect of the present invention, the apparatus includes an attitude information acquisition unit that acquires attitude information of a variable unit included in the mechanical mechanism, and the force sense value correction unit is configured to change the variable based on the attitude information of the variable unit. The correction processing may be performed by estimating the mass center position of the part.

  Thereby, even when the mechanical mechanism has a variable portion, it is possible to perform appropriate correction processing and the like.

  According to another aspect of the present invention, there is provided a control system for a mechanical device, wherein a force value is acquired from a force sensor provided corresponding to a mechanical mechanism of the mechanical device, and the force value correction process is performed. A force value correction unit that performs control, a control unit that controls the mechanical device based on the force value after the correction process, and a posture information acquisition unit that acquires posture information of a variable unit included in the mechanical mechanism The force value correction unit is related to a control system that estimates the mass center position of the variable unit based on the posture information of the variable unit and performs the correction process.

  In another aspect of the present invention, it is possible to perform an appropriate correction process even when the mechanical mechanism has a variable portion.

  In one aspect of the present invention, the posture information acquisition unit acquires the posture information of the mechanical device, and the force value correction unit includes the mass center position of a fixed unit included in the mechanical mechanism, A first correction process based on the posture information of the mechanical device; a center position of the mass of the variable portion estimated based on the posture information of the variable portion; and a second position based on the posture information of the mechanical device. The correction process may be executed as the correction process.

  As a result, the mechanical mechanism is divided into a fixed portion and a variable portion, and correction processing is performed for each of them, so that correction processing for the entire mechanical mechanism can be performed.

  Further, in one aspect of the present invention, the force sense value correction unit includes a fixed part parameter storage unit that stores information including the mass center position of the fixed part and the mass of the fixed part as a fixed part parameter; The variable part parameter storage part which memorizes the mass center position of the variable part in the coordinate system set up corresponding to the variable part and the information including the mass of the variable part as a variable part parameter may be included.

  As a result, it is possible to store the fixed part parameter related to the fixed part and the variable part parameter related to the variable part, and to perform correction processing based on the stored parameters.

  In one aspect of the present invention, the force sense value correction unit reads the fixed part parameter stored in the fixed part parameter storage unit, and based on the fixed part parameter and the posture information of the mechanical device. The force that the fixing unit exerts on the force sensor may be calculated as an offset value, and the first correction process may be performed using the calculated offset value.

  This makes it possible to perform correction processing related to the fixed part based on the fixed part parameter and the attitude information of the mechanical device.

  In the aspect of the invention, the haptic value correction unit reads the variable unit parameter stored in the variable unit parameter storage unit, and the variable unit parameter, the posture information of the variable unit, and the machine Based on the posture information of the apparatus, a force exerted by the variable unit on the force sensor may be calculated as an offset value, and the second correction process may be performed using the calculated offset value.

  This makes it possible to perform correction processing related to the variable part based on the variable part parameter, the attitude information of the mechanical mechanism (variable part in the narrow sense), and the attitude information of the mechanical device.

  In the aspect of the invention, the mechanical device is a robot, the mechanical mechanism is an end effector of the robot, and the force sense correction unit is provided corresponding to the end effector of the robot. A plurality of the force values obtained in a plurality of postures of the robot are obtained from the force sensor, the force values are corrected based on the obtained force values, and the control is performed. The unit may control the robot based on the force sense value after the correction process.

  This makes it possible to use a robot as the mechanical device.

  In the aspect of the invention, the mechanical device is a robot, the mechanical mechanism is an end effector of the robot, the variable unit is a part constituting the end effector, and the posture information acquisition is performed. The unit acquires the posture information of the variable unit that is the part of the end effector, and the force value correction unit receives a force value from the force sensor provided corresponding to the end effector of the robot. And based on the posture information of the variable part, the center of mass position of the variable part is estimated to correct the force value, and the control part performs the force sense after the correction process. The robot may be controlled based on the value.

  This makes it possible to use a robot as the mechanical device.

  According to another aspect of the present invention, there is provided a force value correction unit that acquires a force value from a force sensor provided corresponding to a mechanical mechanism of a mechanical device and performs the correction process of the force value, Based on the force value after the correction process, a computer is caused to function as a control unit that controls the mechanical device, and the force value correction unit includes a plurality of the obtained force sense values in a plurality of postures of the mechanical mechanism. The present invention relates to a program that performs the correction process based on a force sense value.

  In another aspect of the present invention, it is more desirable to perform correction processing based on haptic values obtained at a plurality of postures of the mechanical device, and to control the mechanical device based on the haptic value after the correction processing ( For example, it is possible to realize a program for performing control (with high accuracy).

  According to another aspect of the present invention, there is provided a force value correction unit that acquires a force value from a force sensor provided corresponding to a mechanical mechanism of a mechanical device and performs the correction process of the force value, Based on the haptic value after the correction processing, a computer functions as a control unit that controls the mechanical device and a posture information acquisition unit that acquires posture information of a variable unit included in the mechanical mechanism, and the force sense The value correction unit relates to a program that estimates the mass center position of the variable unit based on the posture information of the variable unit and performs the correction process.

  In another aspect of the present invention, it is possible to implement a program for performing an appropriate correction process and controlling the mechanical device based on the result of the correction process even when the mechanical mechanism has a variable portion.

  In another aspect of the present invention, a force sense value is acquired from a force sensor provided corresponding to a mechanical mechanism of a mechanical device, the force sense value is corrected, and the force after the correction processing is performed. The mechanical device is controlled based on a sense value, and as the correction process, a process of correcting the force sense value based on a plurality of force values obtained in a plurality of postures of the mechanical mechanism is performed. It relates to a method for controlling a mechanical device.

  In another aspect of the present invention, it is more desirable to perform correction processing based on haptic values obtained at a plurality of postures of the mechanical device, and to control the mechanical device based on the haptic value after the correction processing ( For example, it is possible to realize a control method that performs control with high accuracy.

  In another aspect of the present invention, a force sense value is obtained from a force sensor provided corresponding to a mechanical mechanism of a mechanical device, posture information of a variable part included in the mechanical mechanism is obtained, and the force sense value is obtained. And correcting the force sense value based on the posture information of the variable section, controlling the mechanical device based on the force sense value after the correction process, and performing the variable processing as the variable processing. The present invention relates to a control method for a mechanical device that performs processing for correcting the force sense value by estimating the center of mass position of the variable portion based on the posture information of the portion.

  In another aspect of the present invention, even when the mechanical mechanism has a variable portion, it is possible to perform a proper correction process and realize a control method for controlling the mechanical device based on the result of the correction process. .

The structural example in the case of using a robot as a mechanical device. The figure explaining the positional relationship of a mechanical mechanism and a force sensor. 3A to 3C are diagrams illustrating a reset operation. 4A and 4B show examples in which the physical model of the mechanical mechanism is unknown. FIG. 5A and FIG. 5B are examples in which physical modeling of the mechanical mechanism is impossible. FIGS. 6A to 6C show examples in which the mechanical mechanism has a variable portion. FIG. 7A and FIG. 7B show examples in which no variable part is provided and the physical model of the mechanical mechanism is unknown. The system configuration example of 1st Embodiment. Explanatory drawing of the force value detected by a 6-axis force sensor. Explanatory drawing of coordinate conversion of translational force and moment. The figure explaining the correction process based on a physical model. The other figure explaining the correction process based on a physical model. The other figure explaining the correction process based on a physical model. The figure explaining the estimation process of the mass center position in XY plane. The figure showing the plane specified by Fx, Fy, and Mz. FIG. 16A and FIG. 16B are explanatory diagrams of a method for estimating the center of mass position and the like using the first to third planes. FIG. 17A and FIG. 17B show examples where the center of mass position cannot be estimated. 18A and 18B are diagrams for explaining posture change. The flowchart explaining the process in 1st Embodiment. The flowchart explaining the correction process regarding a fixing | fixed part. The flowchart explaining the process in 2nd Embodiment. The flowchart explaining a reset process. The system configuration example of 3rd Embodiment. The figure explaining the division | segmentation into the fixing | fixed part and variable part of a mechanical mechanism. FIG. 25A and FIG. 25B are diagrams illustrating processing for a variable unit. Explanatory drawing of the attitude | position information of a variable part. FIGS. 27A to 27C are explanatory diagrams of processing for obtaining a correction amount for a haptic value from a physical model, parameters, and the like. The flowchart explaining the process in 3rd Embodiment. The system configuration example of 4th Embodiment. The flowchart explaining the process in 4th Embodiment.

  Hereinafter, this embodiment will be described. In addition, this embodiment demonstrated below does not unduly limit the content of this invention described in the claim. In addition, all the configurations described in the present embodiment are not necessarily essential configuration requirements of the present invention.

1. First, the method of this embodiment will be described. Conventionally, control using a force sensor is performed in many mechanical devices. As the mechanical device, for example, a robot can be considered, and the robot control is performed based on a force value from a force sensor provided in the robot. Specifically, force control such as impedance control can be performed by detecting an external force acting on a mechanical mechanism of the robot (in a narrow sense, a hand structure such as a hand) as a force sense value.

  Further, as a mechanical device other than the robot, a power assist input device can be considered, and a rehabilitation device in the medical field or a data recording device associated therewith can be considered. In rehabilitation applications, information on what kind of force the patient is applying at what site is useful for treatment and diagnosis, and also the force applied to the user from support devices such as power assist. There is a high need for appropriate control, and the advantage of using a force sensor is great. Further, the mechanical device may be a remote control device (including a medical device). In this case, the force value from the force sensor is assumed to be used for force feedback or the like. In addition, a force recorder or the like used for scientifically analyzing craftsmanship that requires skilled techniques can also be a mechanical device of this embodiment, and an input device such as a toy or a PC can be a machine of this embodiment. It may be a device. That is, various devices can be assumed as the mechanical device of the present embodiment.

  At that time, in order to perform desired control, various correction processes may be required for the force value of the force sensor. In the case of the robot that performs the above-described force control, the control system requires an external force that acts on a portion that actually contacts the work target. For example, when a given tool is gripped by a hand and control is performed so that the tool slides on the surface of the work target (moves so as not to apply excessive force or lift), the tool selects the work target. It is necessary to detect the reaction force acting on the tool corresponding to the pressing force as an external force.

  However, it is highly conceivable that the force sensor is not provided at a position where it comes into contact with the work object, but is provided at a position corresponding to the wrist of the robot 10 as shown in FIGS. In that case, the force sensor 20 also detects a force by an end effector 12 (a mechanical mechanism in a broad sense, specifically a hand or a tool held by the hand) provided at the tip of the sensor. End up. Specifically, since gravity or the like acting on the end effector 12 is transmitted to the force sensor 20, the force value includes a force other than the above-described external force, and appropriate control of the robot 10 is difficult.

  For this, correction is conventionally performed by reset processing. As an example, consider an example in which a hand having a finger structure is attached to the tip of the force sensor 20 as shown in FIG. Here, as shown in FIG. 3B, the hand holds the tool (driver), and the end effector 12 corresponds to the hand and the tool. In the reset process, as shown in FIG. 3C, the robot 10 is caused to take the same posture as that at the time of work. At that time, if the operation is not actually performed and the external force is not applied, the force value from the force sensor 20 at that time is a value caused by the end effector 12. If the value at this time is set as an offset value, then the force value resulting from the end effector 12 matches the offset value unless the posture of the robot 10 is changed. Therefore, by performing the process of removing the offset value from the haptic value after storing the offset value until the end of the work, it is possible to suppress the influence on the haptic value by the end effector 12.

  However, in the reset processing, after the offset value is stored, the robot 10 cannot be changed in posture so as to affect the offset value until the end of the work. Therefore, Patent Documents 1 to 3 use a method of correcting the influence of the end effector 12 on the force sensor 20 by physically modeling the end effector 12 provided at the tip of the force sensor. The physical model may be, for example, a model representing a weight distribution or the like, and more specifically, a model having the center of mass position (x, y, z) and the mass m as parameters. If the information obtained by physically modeling the end effector 12 is held, the gravity and centrifugal force acting on the end effector 12 can be obtained by physical analysis, and how those forces are applied to the force sensor 20. You can analyze whether it is transmitted. In this case, unlike the reset process, even if the posture changes, if physical analysis is performed based on the posture after the change, it is possible to cope with the posture change, and therefore, control restrictions can be reduced. In Patent Literatures 1 to 3, the force sense value from the force sensor 20 is corrected by such a method to realize desired control.

  However, the conventional method assumes that the physical model of the end effector 12 is known. For this reason, there are situations where it is difficult to cope with the conventional method. Hereinafter, the three cases will be specifically described.

  In the following description, it is assumed that the mechanical device is the robot 10 and the mechanical mechanism is the end effector 12 of the robot 10. However, as described above, the mechanical device and the mechanical mechanism are not limited thereto.

  First, the physical model of the end effector 12 is unknown. In the case of the robot 10 for a specific application, the types of the end effector 12 provided at the tip of the force sensor 20 are limited, so that all the physical models of the candidate end effector 12 are held as in the conventional method. It is possible to perform analysis using an appropriate model. However, it is conceivable that the versatile robot 10 has many types of end effectors 12 that can be attached, and it may be difficult to obtain a physical model in advance. Even if a large number of physical models can be stored in advance, the end effector 12 is a hand and a tool that is gripped by the hand as shown in FIGS. If the problem remains. In the examples of FIGS. 4A and 4B, since there is a certain degree of freedom in the gripping position of the tool by the hand, even if the physical model of the hand and the physical model of the tool are known, both This is because the physical model of the end effector 12 that is included is unknown unless the gripping position of the tool by the hand is determined. That is, in the examples of FIGS. 4A and 4B, the physical model of the end effector 12 cannot be determined until the tool is actually gripped, and the conventional method of analyzing using a given known physical model is used. Application becomes difficult.

  Secondly, it may be difficult to physically model the end effector 12 in the first place. Specifically, the case where the end effector 12 includes a tool having a power cable as shown in FIG. In this case, the physical model also changes depending on whether the cable is fully stretched or slack. Alternatively, when the cable is in contact with the floor, etc., it is necessary to consider the friction between the cable and the floor, and the force required to move the tool changes because the other end of the cable is connected to the power supply. If so, the physical model can change. The same applies to the cable with a connector shown in FIG. 5B, and the relative positional relationship between the grip portion indicated by A1 in FIG. 5B and the cable portion indicated by A2 changes. Therefore, it is difficult to represent the entire end effector 12 including A1 and A2 with a single physical model. In this case, since physical modeling cannot be performed in the first place, dynamic analysis is impossible, but no method corresponding to this problem is shown in the conventional method.

  Third, the case where the end effector 12 has a variable part is conceivable. Specifically, in a hand having a finger structure as shown in FIGS. 6A to 6C, the physical model changes if the finger opening degree or the like changes. For example, when each finger structure has joints at the locations indicated by D1 and D2 as shown in FIG. 6A, the posture of the finger can be changed by setting the respective joint angles. In that case, when the joint angle is set so that the tip of each finger structure faces inward as shown in FIG. 6 (A), and the tip of each finger structure as shown in FIG. 6 (B). The physical model of the entire end effector 12 including the finger structure 12-3 and the wrist portion 12-4 is different when the joint angle is set to face outward. When the wrist portion 12-4 is rotatable as indicated by D3 in FIG. 6C (here, relative rotation with respect to the force sensor 20), the rotation amount of D3 (a given reference) The physical model of the end effector 12 can also change depending on the rotation angle relative to the position. However, Patent Documents 1 and 2 do not perform processing in consideration of this change. In addition, although a drawing of a hand having a finger structure is attached to Patent Document 3, only simple processing based on weight is performed as processing, and changes according to the state of the variable portion are taken into consideration. Absent.

  Therefore, the present applicant proposes a method for performing appropriate force value correction even when a known physical model cannot be used as it is for analysis. Specifically, in the case where the physical model is unknown, the physical model of the end effector 12 is estimated based on the force value acquired from the force sensor 20. This process will be described in detail in the first embodiment.

  Further, in the case where physical modeling of the end effector 12 is difficult, the processing itself is reset as in the conventional case. However, here, the estimation process described in the first embodiment is tried, and whether or not physical modeling of the end effector 12 is possible is determined based on the result of the trial. If physical modeling is possible, the estimation result may be used as in the first embodiment, and if it is not possible, the process may be shifted to reset processing. This process will be described in detail in the second embodiment.

  In the case where the physical model changes by including a variable part, the physical model representing the entire end effector 12 is obtained by separating the end effector 12 into a fixed part and a variable part, and performing processing based on posture information for each. The process which asks for is performed. This process will be described in detail in the third embodiment. In the third embodiment, as will be described later with reference to FIGS. 24 to 26 and the like, the joint portion of the end effector is assumed to be the portion indicated by D2 in FIG. The joint indicated by D1, the rotation indicated by D3 in FIG. 6C, etc. are not taken into consideration. However, this does not prevent the end effector 12 from having a structure such as a joint in D1, D3, or other portions.

  In addition, the method demonstrated by 1st-3rd embodiment is not limited to what is used independently, You may combine multiple. For example, in the example of FIGS. 4A and 4B, the tool can be gripped by the hand including the movable part and the gripping position can be changed. Therefore, the first and third embodiments may be combined. If it is assumed that the tool to be grasped cannot be physically modeled as shown in FIGS. 5A and 5B, it is necessary to combine it with the second embodiment. . These combinations will be described in detail in the fourth embodiment.

2. First Embodiment In the first embodiment, a method for estimating a physical model of an end effector based on a force value acquired from a force sensor and performing a correction process using the estimated physical model will be described. Hereinafter, a system configuration example, a physical model estimation process, and details of the process will be described.

  In the examples of FIGS. 4A and 4B, the end effector includes a variable finger structure, so that it is necessary to use the technique described later in the third embodiment. Therefore, here, an example in which the end effector 12 is configured without including a variable portion by the suction hand 12-1 or the like shown in FIGS. 7A and 7B will be considered. Note that the suction hand 12-1 shown in FIGS. 7A and 7B fixes (grips) the tool 12-2 at the tip. In this case, when the vicinity of the center of the tool 12-2 is sucked as shown in FIG. 7A, and the position near the end of the tool 12-2 is sucked as shown in FIG. 7B. Then, the physical model of the entire end effector 12 including the suction hand 12-1 and the tool 12-2 is different, and the physical model is unknown unless the suction position is known in advance.

2.1 System Configuration Example FIG. 8 shows a configuration example of a system (here, a robot system) including the robot 10, the end effector 12, the force sensor 20, and the robot control system 100 of the present embodiment.

  The end effector 12 is a mechanism provided in the robot 10, and particularly represents a mechanism provided before the force sensor 20 in the robot 10. The end effector 12 includes a hand and a tool (for example, a driver) gripped by the hand.

  The force sensor 20 is a sensor that detects a force acting on the sensing unit. The force sensor 20 may have various configurations. Basically, the external force is detected by measuring a minute deformation of the sensing unit due to the external force by some method. As a deformation measuring method, a method using a strain gauge, a method using an electrostatic sensor, a method using an optical sensor, a method using a piezoelectric element such as a crystal element, and the like can be considered. In the present embodiment, as the force sensor 20, a six-axis force sensor that detects a three-axis translational force and a moment around each axis as shown in FIG. 9 is used.

  The robot control system 100 includes a control unit 110, a force value correction unit 120, and a posture information acquisition unit 130. However, the control system 100 is not limited to the configuration shown in FIG. 8, and various modifications such as omitting some of these components or adding other components are possible.

  The control unit 110 controls the robot 10 and the end effector 12. In the present embodiment, the robot 10 and the end effector 12 are controlled based on the force value after the correction processing by the force value correction unit 120. Here, the control of the robot 10 or the like may be, for example, control of the angle change amount and change speed of the joint portion.

  The haptic value correction unit 120 acquires sensor information (force sensation value) from the force sensor 20 and performs correction processing on the acquired haptic value. The haptic value correction unit 120 includes a first correction processing unit 121, a second correction processing unit 123, a fixed unit parameter storage unit 124, a runtime parameter calculation unit 126, and a runtime parameter storage unit 127. Including.

  In the present embodiment, as shown in FIG. 7 (A), a given member (referred to as a fixed portion, hereinafter referred to as a fixed portion. Specifically, a suction hand 12-1 in FIG. 7 (A)) is used. A case where the physical model of the entire end effector 12 is unknown is assumed by grasping an arbitrary position of the tool 12-2) in FIG. The first correction processing unit 121 performs a correction process for performing dynamic analysis on the fixed unit to suppress the influence of the fixed unit on the force sense value. Specifically, the fixed part parameter is read from the fixed part parameter storage unit 124 that stores the physical model of the fixed part as a fixed part parameter, and the read fixed part parameter and the robot output from the posture information acquisition unit 130 The correction processing is performed based on the 10 posture information. In the output from the first correction processing unit 121, the force value due to the fixed unit is removed (reduced in a broad sense). Details of the correction processing corresponding to the fixed portion will be described later.

  The runtime parameter calculation unit 126 estimates a physical model of a member attached (gripped) to the fixed unit based on the force value output from the first correction processing unit 121. Details will be described later. The runtime parameter storage unit 127 stores the physical model estimated by the runtime parameter calculation unit 126 as a runtime parameter.

  The second correction processing unit 123 performs correction processing based on the runtime parameters stored in the runtime parameter storage unit 127 and the posture information of the robot 10 output from the posture information acquisition unit 130. In the output from the second correction processing unit 123, the haptic value caused by the member whose physical model is unknown is removed (reduced in a broad sense). That is, the force value output from the second correction processing unit 123 to the control unit 110 is obtained by removing the value resulting from the end effector 12.

  The posture information acquisition unit 130 acquires posture information of the robot 10 and the end effector 12. Here, the posture information represents the posture of the robot 10 or the end effector 12, and can express the joint angle as a parameter, for example. If values are determined for all joint angles, the posture of the robot 10 can be uniquely determined by forward kinematics. Specifically, the posture information acquisition unit 130 may acquire a value from an encoder or the like corresponding to each joint of the robot 10 or the like.

2.2 Details of Correction Process in First Correction Processing Unit A force value correction process for the fixed unit performed in the first correction processing unit will be described. As shown in FIG. 10, a case is considered in which a translational force is given by F and a moment is given by M in a certain coordinate system 1 (F and M are, for example, three-dimensional vectors). In this case, when the coordinate system 2 is a coordinate system in which the translation vector of the origin from the coordinate system 2 to the coordinate system 1 is t and the rotation matrix from the coordinate system 2 to the coordinate system 1 is R, the above force is expressed as a coordinate system. When observed in the system 2, the translation force F ′ and the moment M ′ satisfy the following expression (1). Note that the second term on the right side of the second equation represents the outer product of the vectors.

F '= RF
M ′ = RM + t × F (1)
Here, as shown in FIG. 11, when the sensor coordinate system is used as a reference, the force sensor 20 is operated by the end effector 12 that can be modeled as having a mass m at the position (x, y, z). Calculate the force. Here, as shown in FIG. 12, a sensor coordinate system is considered as the coordinate system 2 of FIG. 10, the center of mass position (x, y, z) is the origin, and the Z-axis direction is vertically upward as the coordinate system 1 of FIG. Consider a coordinate system.

  In this case, the above F and M in the coordinate system 1 may be the gravity acting on the object of mass m, so that F = (0,0, −m) and M = (0,0,0). Further, when there is no rotation in the coordinate systems 1 and 2 as shown in FIG. 12, it is not necessary to consider R, and the above t is t = (x, y, z). Thus, F ′ = (0, 0, −m) and M ′ = (− my, mx, 0) based on the above equation (1).

  That is, in the example of FIG. 12, the force by the end effector 12 having the mass m at the position (x, y, z) in the sensor coordinate system is F ′ = (0, 0, −m), M in the sensor coordinate system. Observed as' = (-my, mx, 0). This is nothing but the translational force F ′ = (0,0, −m) and the moment M ′ = (− my, mx, 0) are applied to the force sensor 20. Therefore, by removing the obtained values of F ′ and M ′ from the haptic value, it is possible to suppress the influence of the end effector 12 on the haptic value.

  If there is a rotation in the coordinate systems 1 and 2, up to R may be considered. In FIG. 13, the Z axis of the coordinate system 1 is not changed vertically, and F = (0, 0, −m) and M = (0, 0, 0). However, in FIG. 13, the force sensor 20 is tilted with respect to the vertical direction, and the X axis and the Y axis are also different from the coordinate system 1. The element is determined based on the value of the angle and the yaw angle. In this case, correction processing may be performed based on F ′ and M ′ obtained by the above equation (1).

2.3 Physical Model Estimation Processing Next, physical model estimation processing according to the present embodiment will be described. In the following description, the physical model is a model represented by the mass m and (x, y, z) representing the center position of the mass (corresponding to the force action point).

2.3.1 Basic Method The force sensor 20 of the present embodiment is a commonly used 6-axis force sensor, and Fx, Fy, Fz, which are the respective translational forces of the X-axis, the Y-axis, and the Z-axis. Then, Mx, My, and Mz, which are moments around each axis, are acquired as force sense values. If the end effector 12 is not working to apply a force, the force value is a force caused by the end effector 12 (in a narrow sense, a force corresponding to gravity acting on the end effector 12). Assume that the physical model of the end effector 12 is estimated.

  Specifically, the point of action of the force is estimated based on the force value from the force sensor 20, and here, the projection onto each plane is considered. First, consider the force in the XY plane as shown in FIG. From the force sensor 20, the values of the translational force Fx in the X-axis direction, the translational force Fy in the Y-axis direction, and the moment Mz around the Z-axis perpendicularly passing through the XY plane are acquired. In this case, the following formula (2) is established as is apparent from FIG. Here, F corresponds to the magnitude of the force projected on the end effector 12 onto the XY plane, and r corresponds to the distance from the origin to the force action line. θ is the angle shown in FIG. 14, and corresponds to the angle formed between the force action line and the Y axis.

Fx = −Fsinθ
Fy = −Fcosθ
Mz = −r × F (2)
The following equation (3) is obtained by modifying the above equation (2).

  As described above, since Fx, Fy, and Mz are acquired from the force sensor 20 as force values, F, r, and θ can be obtained based on the above equation (3). However, the magnitude (F) of the force and its direction (determined by r and θ) are determined, that is, the line of action of the force can be determined, but the point of action of the force cannot be determined.

  If this is expanded to a three-dimensional space, one plane including the force application point (mass center position) can be determined based on Fx, Fy, and Mz as shown in FIG. It cannot be determined where the point of action is on the plane.

  However, one plane is determined from Fy, Fz, and Mx as shown in FIG. 16A, and similarly one plane is also determined from Fz, Fx, and My as shown in FIG. 16B. Is done. Since each plane is obtained under the condition that it includes the center of mass position, the intersection of the three planes, that is, the intersection of the three planes can be obtained as the center of mass position. In the example of FIG. 16B, the point indicated by B1 is the point of action of the force, that is, the center of mass position. In addition, the magnitude of the force (corresponding to mass) at that time may be the magnitude of a vector obtained by combining the force vectors in each plane. Here, the force vector means the magnitude of the force (F in the above equation (3)) used for determining the plane shown in FIG. 15 and the like, and the direction of the force (r and θ in the above equation (3)). ).

  That is, the force vector in the XY plane determined from Fx, Fy, and Mz (B2 in FIG. 16B) and the force vector in the YZ plane determined from Fy, Fz, and Mx (in FIG. 16B) The magnitude of the vector obtained by combining B3) and the force vector (B4 in FIG. 16B) on the ZX plane determined from Fz, Fx, and My corresponds to the mass m to be obtained.

  Through the above processing, the unknown physical model (x, y, z, m) can be estimated. After the physical model is estimated, F ′ and M ′ are obtained from the above equation (1) using the physical model and the posture information of the robot 10 in the same manner as the correction method for the fixed unit described above, and the haptic value is calculated. Correction processing may be performed.

  A general expression representing a plane is represented by the following expression (4) and the following expression (5) obtained by modifying the X, Y, and Z axis intercepts as A, B, and C, respectively. Therefore, if the intercepts of the first plane are A1, B1, and C1, the intercepts of the second plane are A2, B2, and C2, and the intercepts of the third plane are A3, B3, and C3, the center of mass position ( x, y, z) is a solution of the simultaneous equations of the following equation (6), and the following equation (7) in which the following equation (6) is expressed by an equation using a matrix is established.

つまり、質量中心位置は上式（７）を変形した下式（８）から求めることができる。

That is, the mass center position can be obtained from the following equation (8) obtained by modifying the above equation (7).

2.3.2 Modification The center of mass position can be obtained by the above processing (for example, processing for solving the above equation (8)). However, when the direction of the external force is in the origin direction of the coordinate system (specifically, the sensor coordinate system set for the force sensor), the center of mass position cannot be obtained.

  Specifically, in the situation shown in FIG. 17A, the value of the moment Mz becomes zero. That is, the amount of information obtained from the force sensor is reduced, and appropriate processing cannot be performed. This can be easily understood by converting the coordinate system so that one of the three axes coincides with the direction of the force as shown in FIG. As shown in FIG. 17B, when the direction of force coincides with the Y ′ axis among the X ′ axis, Y ′ axis, and Z ′ axis constituting the coordinate system, the translation is performed on the X′Z ′ plane. The forces Fx ′ and Fz ′ are both 0, and the moment My ′ around the Y ′ axis is also 0. In other words, the haptic value has no information in this plane. Therefore, there are substantially only two planes containing information, and as shown in FIG. 16B, it is impossible to estimate the center of mass position obtained as the intersection of the three planes.

This is apparent from the above equation (7). When the direction of the force is the origin direction of the coordinate system, the rank of the matrix M is lowered, and the inverse matrix M ⁻¹ cannot be obtained. (M is not regular). That is, the center of mass position cannot be obtained from the above equation (8).

  From the above, in this embodiment, instead of determining the mass center position by a single estimation process, the posture of the robot 10 is changed (the direction of the force acting on the end effector 12 in the sensor coordinate system changes). It is recommended to perform the estimation process multiple times. The posture change is performed, for example, by changing from the state shown in FIG. 18A to the state shown in FIG.

  Specifically, it is determined whether or not the process for estimating the center of mass can be performed. If it is determined that the process cannot be estimated, the posture is changed. As described above, the determination may be made based on whether or not the matrix M expressed by the above equation (7) is regular. Then, the center of mass position is estimated again from the changed posture. In this way, it is possible to appropriately cope with the case where the center of mass position cannot be estimated. If the estimation accuracy is not a problem, a situation in which the center of mass position cannot be obtained can be avoided.

  However, even if the direction of the force does not completely coincide with the origin direction of the coordinate system, the moment value becomes extremely small when passing through a position close to the origin. For this reason, the influence of noise and the like is increased, and a temporary solution can be obtained by solving the above equation (8), but there remains a problem in its accuracy. In addition, when the noise of the sensor or the like is very large, the estimation accuracy may be lowered regardless of the posture.

  Therefore, if the improvement of estimation accuracy is taken into consideration, not only the posture is changed when it is determined that the estimation is impossible, but the posture change and the center of mass position can be estimated even if it is determined that the estimation is possible. The estimation process may be performed again. For example, at least k (k is an integer greater than or equal to 2) estimated values may be acquired, and an average of these may be used as the final mass center position. In this way, improvement in estimation accuracy can be expected.

2.4 Details of Processing The flow of correction processing according to this embodiment will be described with reference to the flowchart of FIG. When this process is started, an output value from the force sensor 20 (force value before correction processing) is first acquired (step S101). And the correction process corresponding to a fixed part is performed with respect to the acquired force value (step S102).

  The flowchart of FIG. 20 shows the flow of correction processing of the fixed part in step S102. In the correction process of the fixing unit, first, posture information of the robot 10 is acquired (step S201). Here, the posture information of the robot 10 is installation information indicating the installation state of the robot 10 (for example, information indicating whether the installation target is a floor, a wall, or a ceiling, and specifically, an angle with respect to the direction of gravity. Information) and the joint angle value of the joint portion of the robot 10. Then, based on the posture information and the fixed part parameters (corresponding to the physical model x, y, z, m of the fixed part as described above), a fixed part correction amount that is a correction amount for the haptic value is calculated (step S202). Specifically, the fixed portion correction amount corresponds to the elements of F ′ and M ′ represented by the above formula (1). Then, based on the force sense value acquired in step S101 and the fixed portion correction amount calculated in step S202, the force sense value after the fixed portion correction process is output (step S203). The output of step S203 corresponds to the output of the first correction processing unit 121 in FIG.

  When the haptic value after the correction process related to the fixed part (the output value of the process of step S102) is obtained, the above equation (7) is obtained based on the value (step S103). Specifically, as described above, three planes may be obtained from the translational forces of the three axes and the moments around the respective axes, and the matrix M and the vector q may be determined from the values of the respective intercepts.

  When the matrix M is obtained, it is determined whether or not M is regular (step S104). As described above, the case where M is not regular corresponds to the case where the direction of the force by the end effector 12 coincides with the origin direction of the sensor coordinate system, and the case where the center of mass position cannot be estimated. Therefore, when M is not regular (in the case of No in step S104), the control unit 110 is instructed to change the posture, and the process returns to step S101. In other words, after changing the posture, the haptic value is acquired again.

  On the other hand, when M is regular (in the case of Yes in step S104), it means that an inverse matrix of M can be obtained. Therefore, the center of mass position is obtained according to the above equation (8), and FIG. As shown, the mass m is obtained as the magnitude of the combined vector of force vectors (B2 to B4) on each plane, and (x, y, z, m) is used as a runtime parameter (step S106).

  Next, it is determined whether to perform the process again (step S107). This is a determination relating to the accuracy improvement described above. For example, if a method of obtaining at least k (k is an integer of 2 or more) runtime parameters and determining a final runtime parameter based on them is used, the determination in step S107 is acquired at that time. No if the number of run-time parameters is greater than or equal to k, Yes if it is less than k. In the case of Yes in step S107, the process proceeds to step S105 in the same manner as in the case of No in step S104. After changing the posture, the process returns to step S101, the force value is acquired, and the above-described processing is performed.

  If No in step S107, it is determined whether a plurality of processes have been performed (step S108). Step S108 is actually a determination as to whether or not a plurality of runtime parameters have been obtained. Even if it is determined No in step S104 and the posture is changed, the acquired runtime parameter is 1. If there is only one, it is determined No. In the case of No in step S108, the obtained single runtime parameter is stored as it is as a runtime parameter used for the subsequent correction processing (step S109).

  In the case of Yes in step S108, it is determined whether a plurality of obtained runtime parameters are within a predetermined error (step S110). Of the runtime parameters used in this embodiment, (x, y, z) representing the position is assumed to be a value in the sensor coordinate system. The mass center position (x, y, z) in the sensor coordinate system should be constant even if the posture of the robot 10 changes unless the configuration of the end effector 12 itself is changed. Even if the runtime parameter is expressed using a coordinate system other than the sensor coordinate system, if the value is converted to the sensor coordinate system, the characteristic does not change depending on the posture of the robot 10. The mass m is a constant value regardless of the posture of the robot 10 and whatever coordinate system is used, unless the configuration of the end effector 12 itself is changed. is there. In other words, in an ideal situation where no error occurs at all, the runtime parameters obtained multiple times should all have the same value. In reality, errors occur in haptic values, posture information, etc., so it is unlikely that all runtime parameters match, but it is still expected to take values that are close to some extent. In other words, if the value varies more than a predetermined error, the mass center position estimated by some factor is low in reliability.

  From the above, when the execution time parameter is within the predetermined error (Yes in step S110), the obtained execution time parameter is assumed to be reliable and the process proceeds to step S109 to store the execution time parameter at that time. To do. What is stored in step S109 may be a representative value among a plurality of runtime parameters, or may be obtained from an average value or the like.

  On the other hand, in the case of No in step S110, the obtained runtime parameter is unreliable, so the runtime parameter at that time is not stored, an error is output (step S111), and the process is terminated. .

  In the present embodiment described above, the control system 100 is a control system for the robot (mechanical device) 10 and is a force sense provided corresponding to the end effector (mechanical mechanism) 12 of the robot 10 as shown in FIG. A force value correction unit 120 that acquires a force value from the sensor 20 and corrects the force value, and a control unit 110 that controls the robot 10 based on the force value after the correction process are included. The haptic value correction unit 120 performs correction processing based on a plurality of haptic values obtained in a plurality of postures of the robot 10.

  As a result, the posture of the robot 10 is changed, and the force sense value correction process can be performed based on the force sense value obtained in each posture. As described above, the force value from the force sensor 20 includes an element caused by a force that is unnecessary in the control of the robot 10. Specifically, the force value is provided before the force sensor 20. The thing by the end effector 12 can be considered. Here, since it is assumed that the influence of the end effector 12 on the force sensor 20 varies depending on the posture of the robot 10, different situations are used in combination by performing correction processing using a plurality of postures. Correction processing becomes possible. In other words, force sensor correction processing is necessary for proper control of the robot 10, but more accurate correction processing can be performed using a plurality of postures. Considering this point, it is desirable that the plurality of postures in the present embodiment are a plurality of postures in which the influence of the end effector 12 on the force sensor 20 is different, for example, FIG. 18 (A) and FIG. 18 (B). It is better to make changes such as turning the wrist part of the robot.

  In addition, the haptic value correction unit 120 determines the mass of the end effector 12 based on the first to Nth haptic values obtained in the first to Nth (N is an integer of 2 or more) postures of the robot 10. The center position may be estimated, and correction processing may be performed based on the estimated mass center position. Specifically, the force sense value correction unit 120 estimates a force sense value acting on the force sensor 20 by the end effector 12 based on the mass of the end effector 12, the center of mass position, and the posture of the robot 10. Thus, correction processing may be performed.

  As a result, the mass center position (x, y, z) can be used as a physical model of the end effector 12. If the end effector 12 can be modeled as an object of mass m located at the center of mass, the influence of the end effector 12 on the force sensor 20 is mechanical as described in FIG. 10 and the above equation (1). It becomes possible to analyze. In the above equation (1), the center of mass position corresponds to the translation vector t, and the rotation R is obtained from the posture of the robot 10.

  Further, the haptic value correction unit 120 performs processing for estimating the mass center position of the end effector 12 based on the first haptic value obtained in the first posture of the robot 10, and the control unit 110 When the force value correction unit 120 determines that the center of mass position cannot be estimated, control may be performed to change the posture of the robot 10 to a second posture different from the first posture. Then, the haptic value correction unit 120 determines the center of mass position of the end effector 12 based on the second haptic value acquired after the control unit 110 performs the control of the robot 10 in the second posture. Based on the estimated mass center position, correction processing is performed.

  This makes it possible to estimate an appropriate center of mass position. As shown in FIG. 17A, the first posture is a case where the direction of the force acting by the end effector 12 faces the origin of the sensor coordinate system. In this case, M is not regular as described above. The above equation (8) cannot be solved. This can be solved by changing the posture of the robot 10 because it is only necessary to avoid the situation where the direction of the force acting by the end effector 12 is directed to the origin of the sensor coordinate system. Note that, as described above, the posture change in the present embodiment is not limited to a situation where estimation cannot be performed, but may be always performed for the purpose of improving accuracy.

  Further, as shown in FIG. 9, the force sensor 20 obtains X-axis, Y-axis, and Z-axis translational forces Fx, Fy, and Fz and moments Mx, My, and Mz around each axis. In the case of the sensor, the haptic value correction unit 120 includes a first plane determined by Fx, Fy, and Mz, a second plane determined by Fy, Fz, and Mx, and Fz, Fx, and My. A third plane to be determined may be obtained, and the intersection of the first to third planes may be estimated as the mass center position. Specifically, the first plane x-intercept is A1, y-intercept B1, z-intercept is C1, the second plane x-intercept is A2, y-intercept B2, z-intercept is C2, and the third When the x-intercept of the plane is A3, the y-intercept is B3, the z-intercept is C3, and the matrix M and the vector q are those shown in the above equation (7), the haptic value correcting unit 120 determines the mass center position. The center of mass position may be estimated by obtaining a three-dimensional vector p to be represented according to Mp = q.

  Thereby, it becomes possible to estimate the center of mass position by the methods shown in FIGS. 15 to 16B and the above equations (6) to (8).

  Further, the force sense value correction unit 120 may determine that the center of mass position cannot be estimated when the matrix M is not regular.

  This makes it possible to determine whether the center of mass position cannot be estimated as shown in FIG. 17A based on whether M is regular. If the estimation is impossible, the posture may be changed as described above, and the situation where no estimated value (runtime parameter) can be obtained can be avoided.

  Further, the haptic value correction unit 120 includes a runtime parameter calculation unit 126 that calculates information including the mass center position and the mass of the end effector 12 as runtime parameters as illustrated in FIG. A runtime parameter storage unit 127 that stores parameters may be included. Specifically, the force sense value correction unit 120 reads the runtime parameter stored in the runtime parameter storage unit 127, and based on the runtime parameter and the posture of the robot 10, the end effector 12 detects the force sensor 20. The force exerted on is calculated as an offset value, and the haptic value is corrected using the calculated offset value.

  As a result, a physical model such as the center of mass position can be calculated (estimated) as a runtime parameter and stored. As shown in FIG. 27C, using the stored runtime parameters, correction processing similar to that of the fixed unit is performed, so that correction processing can be appropriately performed even for the end effector 12 whose physical model is unknown. .

  Further, as described above in the present embodiment, the mechanical device may be the robot 10, and the mechanical mechanism may be the end effector 12 of the robot 10. In that case, the force value correction unit 120 acquires a plurality of force values obtained in a plurality of postures of the robot 10 from the force sensor 20 provided corresponding to the end effector 12 of the robot 10. The force value correction process is performed based on the plurality of force values, and the control unit 110 controls the robot 10 based on the force value after the correction process.

  Here, the end effector 12 of the robot 10 is a device attached to the end point of the robot 10 and is a concept including a hand, a tool, and the like. Considering that the mechanical device may be a device other than the robot 10 described in the present embodiment, the mechanical mechanism is not limited to the end effector 12, and is a concept including other mechanisms.

  This makes it possible to use the robot 10 as a mechanical device. In this case, the force applied to the force sensor 20 by the end effector 12 itself can be corrected, and for example, force control (for example, impedance control) using an external force acting on the end effector 12 of the robot 10 can be performed.

  Note that the control system 100 or the like of the present embodiment may realize part or most of the processing by a program. In this case, the control system 100 according to the present embodiment is realized by a processor such as a CPU executing a program. Specifically, a program stored in the information storage medium is read, and a processor such as a CPU executes the read program. Here, the information storage medium (computer-readable medium) stores programs, data, and the like, and functions as an optical disk (DVD, CD, etc.), HDD (hard disk drive), or memory (card type). It can be realized by memory, ROM, etc. A processor such as a CPU performs various processes according to the present embodiment based on a program (data) stored in the information storage medium. That is, in the information storage medium, a program for causing a computer (an apparatus including an operation unit, a processing unit, a storage unit, and an output unit) to function as each unit of the present embodiment (a program for causing the computer to execute processing of each unit) Is memorized.

3. Second Embodiment In the second embodiment, as shown in FIG. 5A and FIG. 5B, a case using an end effector 12 that is difficult to be physically modeled will be described. Note that the method of this embodiment does not attempt physical modeling by complicated analysis processing, but the system automatically detects whether physical modeling is possible, and if it is determined to be impossible, the conventional reset is performed. Switching to processing.

  Since the system configuration example is shown in FIG. 8 as in the first embodiment, detailed description thereof is omitted. The correction process of this embodiment will be described with reference to the flowchart of FIG.

  As is clear from FIG. 21, Steps S301 to S310 are the same as Steps S101 to S110 in FIG. That is, also in this embodiment, since it is not yet known whether the end effector 12 can be physically modeled, the estimation process is tried. Since the details of the estimation process are the same as those in the first embodiment, a detailed description thereof will be omitted. At this time, if the estimation process is performed only once, that is, only one runtime parameter is acquired, it is difficult to determine whether or not the physical model represented by the runtime parameter is appropriate. Therefore, in practice, it is assumed that a plurality of estimation processes are performed, and it is determined whether or not a plurality of runtime parameters obtained therefrom are within a predetermined error (step S310). Accordingly, step S308 in FIG. 21 may be omitted in the processing of the present embodiment, and the processing may be shifted to step S310 after the processing in step S307.

  In step S310, if the execution time parameter is within a predetermined error, it means that the end effector 12 is appropriately expressed by the estimated physical model. Therefore, the execution time at that time is the same as in the first embodiment. The parameters may be stored (step S309) and correction processing based on the physical model may be performed.

  On the other hand, if the runtime parameter is larger than the predetermined error in step S310, it is determined that the end effector 12 is difficult to physically model as shown in FIGS. 5 (A) and 5 (B). This is because, as described above, in FIG. 5A, the physical model representing the end effector 12 is shown by the extension of the cable and the friction between the cable and the floor, and in FIG. 5B, the change in the relative positional relationship between A1 and A2. Can change frequently. In other words, if the end effector 12 can be physically modeled, it is assumed that the runtime parameters obtained as described above in the first embodiment fall within a certain amount of error. In the case of the end effector 12 that is difficult to be physically modeled, even if there is no error in the sensor or the like, the run-time parameters required depending on the situation change. From this, if the obtained runtime parameter varies greatly, it is determined that the end effector 12 itself is not suitable for physical modeling.

  That is, in the case of No in step S310, the physical modeling of the end effector 12 may be abandoned and a conventional reset process may be performed (step S311). Details of the reset processing are shown in the flowchart of FIG. When the reset process is started, first, the controller 110 is instructed to assume the same posture as that at the time of work (step S401). If the posture is the same as that at the time of work and no force is applied unlike the time of actual work, the value (offset value) detected as the force sense value at that time is the force caused by the end effector 12. It represents the force acting on the sensor, and its value should not change during work. That is, the posture of the entire robot 10 does not have to be the same as that at the time of work, and it is sufficient that the offset value is a value that is sufficiently close to that at the time of work.

  Then, when the posture is changed by the control unit 110 according to the instruction, the haptic value after the posture change is stored as an offset value (step S402). Then, after step S402, until the end of the work, an offset removal process for removing the offset value is performed on the force value acquired from the force sensor 20 (step S403).

  In the present embodiment described above, the force sense value correction unit 120 executes error processing when it is determined that the estimated mass center position is not within a predetermined error. Specifically, when it is determined that the center of mass position estimated by the force value correction unit 120 is not within a predetermined error, the control unit 110 performs control to set the posture of the robot 10 to a given posture. The haptic value correction unit 120 acquires a haptic value after the control of the robot 10 is performed in a given posture by the control unit 110, and sets an offset value corresponding to the acquired haptic value. The offset removal process using the set offset value is executed as an error process.

  As a result, as shown in FIGS. 5A and 5B, when the end effector 12 cannot be physically modeled in the first place, the control system 100 can automatically detect this. Furthermore, it becomes possible to perform exception handling when physical modeling is impossible. Note that, as described above in the first embodiment, the control system 100 only determines whether or not the center of mass position is within a predetermined error, and outputs an error if it is determined to be greater than or equal to the predetermined error. It may be only. In that case, the response to the error is left to the user. However, as described in the present embodiment, if the shift to the reset process is automatically performed when a predetermined error is exceeded, the frequency of manual system operation by the user can be reduced, and the convenience is improved. Etc. can be expected.

4). Third Embodiment In the third embodiment, a case will be described in which the end effector 12 includes a variable portion as shown in FIGS. 6 (A) to 6 (C). In the present embodiment, when it is not necessary to consider the tool (12-2 in FIG. 7A) gripped by the hand as the end effector 12, or when the tool is gripped but its gripping position is fixed, etc. It is assumed that the physical model is unknown or cannot be estimated in the first place. That is, in this embodiment, although the shape, mass, and the like of the end effector 12 are known, the physical model of the end effector 12 changes due to, for example, a change in the finger joint angle as shown in FIG. Is a problem.

4.1 System Configuration Example FIG. 23 shows a system configuration example of this embodiment. Compared to the first embodiment, the second correction processing unit 123, the execution time parameter calculation unit 126, and the execution time parameter storage unit 127 of the haptic value correction unit 120 are removed, and the third correction processing unit 122 and the variable value are changed. A configuration parameter storage unit 125 is added.

  The fixed part parameter storage unit 124 stores fixed part parameters corresponding to the physical model of the fixed part (the palm portion 12-3 in FIG. 24, etc.) of the end effector 12. Then, the first correction processing unit 121 performs a force value correction process for the fixed unit based on the fixed unit parameter and the posture information of the robot 10 from the posture information acquisition unit 130.

  The variable part parameter storage unit 125 stores variable part parameters corresponding to the physical model of the variable part (such as the finger portion 12-4 in FIG. 24) of the end effector 12. Then, the third correction processing unit 122 performs force value correction processing for the variable unit based on the variable unit parameter, the posture information of the robot 10 from the posture information acquisition unit 130, and the posture information of the end effector 12. I do.

4.2 Details of Correction Process for Variable Part As described above in the first embodiment as the correction process for the fixed part, the sensor coordinate system is also set for the variable part when the coordinate system 1 having the center of mass position as the origin is set. If the translation vector t and the rotation matrix R from the coordinate system 2 to the coordinate system 1 can be obtained, the force value correction process can be performed.

  Here, to simplify the description, as shown in FIG. 25B, only the force sensor 20 and one finger structure in FIG. 25A are considered. Further, as shown in FIG. 26, the finger structure is structured to be rotatable around the X axis at the origin of the finger rotation coordinate system (C1 in FIG. 25B), and the rotation angle is θ. That is, here, it is assumed that there is no joint in the other part of the finger structure.

  In this case, since the physical model of the finger structure itself is known, the translation vector t2 and the mass m from the origin of the finger rotation coordinate system to the center of mass position (C2 in FIG. 25B) and the mass m are stored in advance as variable part parameters. Has been. In addition, since the origin C1 of the finger rotation coordinate system is a joint part (that is, a rotation axis for defining the rotation angle θ), the relative position with respect to the origin of the sensor coordinate system is invariable, and C1 from the origin of the sensor coordinate system. The translation vector t3 to is also stored as a known variable part parameter. In addition, the rotation between the sensor coordinate system and the finger rotation coordinate system is unknown considering that the finger rotation coordinate system rotates according to the value of θ, but θ is a given reference value The roll angle, pitch angle, and yaw angle (u, v, w) between these coordinate systems are known and are similarly stored as variable part parameters.

  In other words, as long as the parameter θ representing the movement of the variable portion is determined, a displacement vector representing the mass center position in the sensor coordinate system (from the origin of the sensor coordinate system to the mass center position is obtained by θ, t2, t3, u, v, and w. Vector), and the displacement vector is nothing but the translation vector t shown in FIG. As an example, the translation vector t2 is subjected to coordinate transformation based on a rotation matrix represented by θ and u, v, and w to be transformed into a vector in the sensor coordinate system, and the vector of the transformed vector and t3 It is possible to seek the sum. In general, it may be obtained as an element of a 4 × 4 homogeneous matrix. In this case, there is an advantage that both translation and rotation can be processed by the matrix.

  As described above, from the variable part parameters (t2, t3, u, v, w) and the attitude information (θ) of the end effector 12, the translation vector t (mass center position x, y based on the sensor coordinate system). , Z)), and a known value can be used as it is for the mass m. That is, since the value corresponding to the fixed part parameter can also be calculated for the variable part, as described above in the first embodiment, the subsequent processing is based on the above equation (1) to calculate F ′ and M ′. The obtained F ′ and M ′ values may be removed from the haptic value. By doing in this way, it becomes possible to suppress the influence on the force sense value by a variable part.

  A summary of the above processing is shown in FIGS. As for the fixed part described in the first embodiment, (x, y, z, m) is stored as a fixed part parameter as shown in FIG. If the rotation matrix R is obtained, the fixed portion correction amount can be obtained, thereby enabling correction processing. On the other hand, as shown in FIG. 27B, for the variable part, (t2, t3, u, v, w) is stored as an example of the variable part parameter as described above. (X, y, z, m) is obtained based on the parameters and the posture information θ of the end effector 12 (variable part in the narrow sense). Then, the variable portion correction amount is obtained from the obtained (x, y, z, m) and the rotation matrix R, and correction processing is performed.

  In addition, when the estimation process shown in the first embodiment is performed, parameters are not stored in advance, but (x, y, z) based on the force value as shown in FIG. , M). After the estimation result is obtained, the correction process is performed by obtaining the correction amount from the rotation matrix R in the same manner as the fixed unit.

4.3 Details of Processing The correction processing of this embodiment will be described with reference to the flowchart of FIG. When this processing is started, an output value from the force sensor 20 (force value before correction processing) is acquired (step S501). And the correction process corresponding to a fixed part is performed with respect to the acquired force sense value (step S502).

  Since the correction process of the fixed part in step S502 is the same as that in FIG. 20 described in the first embodiment, detailed description thereof is omitted. Next, based on the variable part parameter (t2, t3, u, v, w in the above example) and the finger joint parameter (θ in the above example), the correction parameter (t = (x, y, z) and m) are obtained (step S503). Based on the obtained correction parameter and the posture of the robot 10, the variable portion correction amount is calculated (step S504). In the above example, the process of step S504 corresponds to the process of obtaining R based on the posture information of the robot 10 and obtaining F ′ and M ′ by the above equation (1).

  Then, the haptic value after the fixed portion correction process, which is the output of step S502, is corrected by the variable portion correction amount obtained in step S504 (step S505), and the result is the haptic value after the correction processing. (Step S506). The output in step S506 is a force sense value that has been subjected to the correction processing for the entire end effector 12 because the correction processing for both the fixed portion and the variable portion has been performed.

  In the present embodiment described above, the control system 100 includes the posture information acquisition unit 130 that acquires the posture information of the variable unit (for example, 12-4 in FIG. 24) of the end effector 12, as shown in FIG. Then, the haptic value correction unit 120 performs correction processing by estimating the mass center position of the variable unit based on the posture information of the variable unit.

  In the above embodiment, the control system 100 is a control system for the robot 10, acquires a force value from the force sensor 20 provided corresponding to the end effector 12 of the robot 10, and determines the force value. Posture information acquisition that acquires the posture information of the haptic value correction unit 120 that performs correction processing, the control unit 110 that controls the robot 10 based on the haptic value after the correction processing, and the variable unit of the end effector 12 The haptic value correction unit 120 including the unit 130 may perform correction processing by estimating the mass center position of the variable unit based on the posture information of the variable unit.

  6A to 6C, even if the end effector 12 includes a variable part, the physical model of the end effector 12 is appropriately set according to the state of the variable part. It is possible to obtain and perform correction processing. The state of the variable portion can be represented by the posture information of the variable portion, and the posture information is, for example, the angle θ shown in FIG. Note that the number of parameters of the posture information is determined in accordance with the number of movable parts (joint parts in the case of a robot) of the variable part.

  In addition, the posture information acquisition unit 130 acquires posture information of the robot 10, and the haptic value correction unit 120 determines the mass center position of the fixed unit (for example, 12-3 in FIG. 24) included in the end effector 12, and the robot. The first correction processing based on the posture information of 10, the mass center position of the variable portion estimated based on the posture information of the variable portion, and the second correction processing based on the posture information of the robot 10 are executed as correction processing. May be.

  Here, the first correction processing corresponds to the processing performed by the first correction processing unit 121 in FIG. 23, and the second correction processing corresponds to the processing performed by the third correction processing unit 122.

  As a result, as shown in FIG. 24, the end effector 12 is divided into a fixed part having no movable part and a variable part having a movable part, and correction processing is performed for each, thereby correcting the entire end effector 12. Processing can be performed.

  Further, the haptic value correction unit 120 includes a fixed part parameter storage unit 124 that stores information including the mass center position of the fixed part and the mass of the fixed part as a fixed part parameter, and coordinates set corresponding to the variable part. A variable part parameter storage unit 125 that stores information including the center position of the variable part in the system and the mass of the variable part as variable part parameters may be included.

  Then, the force sense value correction unit 120 reads the fixed part parameter stored in the fixed part parameter storage unit 124, and the force exerted on the force sensor 20 by the fixed part based on the fixed part parameter and the posture information of the robot 10. As an offset value, and a first correction process is performed using the calculated offset value. Further, the haptic value correction unit 120 reads the variable unit parameters stored in the variable unit parameter storage unit 125, and the variable unit is based on the variable unit parameters, the posture information of the variable unit, and the posture information of the robot 10. The force exerted on the force sensor 20 is calculated as an offset value, and the second correction process is performed using the calculated offset value.

  As a result, it is possible to store the necessary information for each of the fixed part and the variable part as a parameter and perform correction processing using the parameter. Specifically, as shown in FIGS. 27A and 27B, the fixed part parameters are, for example, the mass center position (x, y, z) and the mass m, and the variable part parameter is the variable part parameter. It is a parameter whose contents change according to the structure. For the fixed part, as shown in FIG. 27A, the fixed part parameter is converted by the rotation matrix R obtained from the posture information, and F ′ and M ′ are obtained as offset values. As for the variable part, as shown in FIG. 27 (B), the center of mass position (x, y, z) and the mass are obtained by performing processing using the variable part posture information on the variable part parameter. Then, m is obtained, and then conversion by the rotation matrix R is performed in the same manner as the fixed portion, and F ′ and M ′ are obtained as offset values. That is, the variable part parameter is information that can be used to estimate the mass center position and mass by combining the variable part attitude information and the variable part parameter. An example of the variable part parameter is a variable part coordinate system (in a narrow sense) as described above. , The vector t2 from the origin of the finger rotation coordinate system to the center of mass position, the vector t3 from the origin of the sensor coordinate system to the origin of the variable part coordinate system, and the rotation between the sensor coordinate system and the variable part coordinate system Each angle information of roll, pitch, and yaw representing the angle can be considered.

  Further, as described above in the present embodiment, the mechanical device may be the robot 10, the mechanical mechanism may be the end effector 12 of the robot 10, and the variable unit constitutes the end effector 12. It may be a part. In that case, the posture information acquisition unit 130 acquires posture information of a variable unit that is a part of the end effector 12, and the force value correction unit 120 is a force sensor 20 provided corresponding to the end effector 12 of the robot 10. The haptic value is acquired from the position, the center of mass position of the variable portion is estimated based on the posture information of the variable portion, and the haptic value is corrected. The control unit 110 is based on the haptic value after the correction processing. Thus, the robot 10 is controlled.

  Accordingly, the control system 100 described in the present embodiment can also be applied to the robot system, and specifically, it is possible to perform force control of the robot 10 and the like.

5. Fourth Embodiment The methods described in the first to third embodiments are not limited to those used independently, and a plurality of methods may be combined. Here, as shown in FIG. 4A, when a tool is gripped by a hand having a variable finger structure and there is a degree of freedom in the gripping position of the tool, the tool shown in FIG. ), Etc., where the physical modeling is difficult. In this case, since the end effector 12 has a variable part, it is necessary to use the method described in the third embodiment, and further, since the physical model is unknown, it is necessary to use the estimation method described in the first embodiment. . Furthermore, since physical modeling itself may not be possible, the method of the second embodiment for determining whether estimation is possible may be used.

5.1 System Configuration Example FIG. 29 shows a system configuration example of the present embodiment. As shown in FIG. 29, the haptic value correction unit 120 of the control system 100 includes the units shown in FIGS. Although not shown in FIG. 23, the haptic value correction unit 120 instructs the control unit 110 to change the posture based on the calculation result of the runtime parameter calculation unit 126. May be included. The posture change instruction unit may be included in the haptic value correction unit 120 of FIG. 8 described above in the first embodiment.

  The first correction processing unit 121 is connected to the third correction processing unit 122. The third correction processing unit 122 is connected to the second correction processing unit 123 and the runtime parameter calculation unit 126. The runtime parameter calculation unit 126 is connected to the runtime parameter storage unit 127.

  When the haptic value correction unit 120 includes a posture change instruction unit, the posture change instruction unit changes the posture of the robot 10 with respect to the control unit 110 based on the result of calculation processing by the runtime parameter calculation unit 126. Instruct. Specifically, when it is determined that the calculation of the runtime parameter is not possible, or when it is desired to obtain a runtime parameter in a posture different from the calculated runtime parameter, the posture change instruction unit instructs the control unit 110 to To change the posture.

5.2 Details of Processing The correction processing according to the present embodiment will be described with reference to the flowchart of FIG. When this process is started, an output value from the force sensor 20 (force value before the correction process) is acquired (step S601). And the correction process corresponding to a fixed part and a variable part is performed with respect to the acquired force value (step S602). Since the correction process of the fixed part in step S602 is the same as that in FIG. 20 described in the first embodiment, detailed description thereof is omitted. The variable portion correction processing is the same as steps S503 to S506 of FIG. 28 described in the third embodiment.

  Subsequent steps S603 to S611 are the same as steps S303 to S311 of FIG. 21 described in the second embodiment. That is, as an example of the processing procedure of the present embodiment in which the first to third embodiments are combined, first, correction processing is performed on the fixed portion / variable portion, and estimation processing is performed based on the haptic value after the correction processing. Do. Then, it is determined whether or not physical modeling of the end effector 12 is possible from the result of the estimation process. If physical modeling is possible, the correction process is performed using the estimation result. On the other hand, if physical modeling is not possible, the correction process using the physical model is given up, and the correction process is performed by the reset process.

  As mentioned above, although four Embodiments 1-4 and its modification to which this invention was applied were demonstrated, this invention is not limited to each Embodiment 1-4 and its modification as it is, An implementation step The constituent elements can be modified and embodied without departing from the spirit of the invention. Moreover, various inventions can be formed by appropriately combining a plurality of constituent elements disclosed in the above-described first to fourth embodiments and modified examples. For example, you may delete a some component from all the components described in each Embodiment 1-4 and the modification. Furthermore, you may combine suitably the component demonstrated in different embodiment and modification. In addition, a term described together with a different term having a broader meaning or the same meaning at least once in the specification or the drawings can be replaced with the different term anywhere in the specification or the drawings. Thus, various modifications and applications are possible without departing from the spirit of the invention.

Fx, Fy, Fz translational force, Mx, My, Mz moment, R rotation matrix,
t translation vector,
10 robots, 12 end effectors, 20 force sensors,
100 control system, 110 control unit, 120 haptic value correction unit,
121 1st correction process part, 122 3rd correction process part, 123 2nd correction process part,
124 fixed part parameter storage part, 125 variable part parameter storage part,
126 runtime parameter calculator, 127 runtime parameter storage,
130 Posture information acquisition unit

Claims (8)

A robot system,
With robots,
A force value correction unit that acquires a force value from a force sensor provided corresponding to the mechanical mechanism of the robot and performs correction processing of the force value;
A control unit that controls the robot based on the force value after the correction process;
Including
The force value correction unit
Based on the first to Nth haptic values obtained in the first to Nth (N is an integer of 2 or more) postures of the robot, the mass center position of the mechanical mechanism is estimated, and the estimated mass Based on the center position, the correction process is performed,
When the force sensor is a six-axis force sensor that obtains translational forces Fx, Fy, Fz on the X, Y, and Z axes and moments Mx, My, Mz around each axis,
The force value correction unit
Obtaining a first plane determined by Fx, Fy and Mz, a second plane determined by Fy, Fz and Mx, and a third plane determined by Fz, Fx and My;
A robot system , wherein an intersection of the first to third planes is estimated as the mass center position . In claim 1 ,
The force value correction unit
The correction processing is performed by estimating the force value acting on the force sensor by the mechanical mechanism based on the mass of the mechanical mechanism, the center position of the mass, and the posture of the robot. A robot system. In claim 1 or 2 ,
The force value correction unit
Based on the first haptic value obtained in the first posture of the robot, a process of estimating the mass center position of the mechanical mechanism is performed,
The controller is
If the force value correction unit determines that the center of mass position cannot be estimated, control is performed to change the posture of the robot to a second posture different from the first posture;
The force value correction unit
The mass center position of the mechanical mechanism is estimated based on a second haptic value acquired after the control unit performs control for the robot to be in the second posture, and the estimated mass center position The robot system is characterized in that the correction process is performed based on the above. In any one of Claims 1 thru | or 3 ,
The force value correction unit
An error process is executed when it is determined that the estimated center of mass position is not within a predetermined error. In claim 4 ,
When it is determined that the mass center position estimated by the force value correction unit is not within a predetermined error,
The controller is
Control the robot posture to a given posture,
The force value correction unit
The control unit acquires the force value after the robot is controlled to be in the given posture, sets a value corresponding to the acquired force value as an offset value, and sets the offset A robot system, wherein offset removal processing by value is executed as the error processing. In any one of Claims 1 thru | or 5 ,
The first plane x-intercept is A1, y-intercept B1, z-intercept is C1, the second plane x-intercept is A2, y-intercept B2, z-intercept is C2, and the third plane The x-intercept is A3, the y-intercept is B3, the z-intercept is C3, and the matrix M and vector q are

If
The force value correction unit
A three-dimensional vector p representing the center of mass position is expressed as Mp = q
The robot system is characterized in that the center of mass position is estimated by obtaining according to In claim 6 ,
The force value correction unit
When the matrix M is not regular, it is determined that the center of mass position cannot be estimated. In any one of Claims 1 thru | or 7 ,
The force value correction unit
A runtime parameter calculation unit that calculates information including the mass center position and the mass of the mechanical mechanism as a runtime parameter;
A runtime parameter storage unit for storing the calculated runtime parameter;
A robot system characterized by including:

Images