JP2013119133A - Robot and robot control method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a robot and a robot control method for performing appropriate control even when a robot is provided with a mechanical mechanism having an elastic member.
A robot includes an arm 120, a force sensor 10 provided on the arm 120, a mechanical mechanism (for example, a hand 110) provided on the arm 120 via the force sensor 10 and having an elastic member, A correction unit 40 that calculates a correction value of the target value of the position and orientation of the mechanism, and a control unit 80 that corrects the target value based on the correction value and controls the arm 120 based on the corrected target value. The sense sensor 10 outputs sensor information about the detected force, the correction unit 40 uses the sensor information output by the force sensor 10 to obtain a correction value corresponding to the displacement of the elastic member based on the force, and the control unit 80 Based on the corrected target value, the arm 120 that cancels the displacement of the elastic member is controlled.
[Selection] Figure 3

Description

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

  Conventionally, a force sensor that acquires force acting on a sensing unit as sensor information and various systems using the force sensor are known.

  In the field of robot control, a system that performs force control represented by impedance control can be considered. Impedance control is a control method for operating a robot manipulator as if it had a value suitable for work regardless of its actual mass, viscosity, and elasticity. Specifically, a solution of the equation of motion is obtained based on force information obtained from a force sensor provided in the manipulator, and the manipulator is operated according to the solution.

  Impedance control allows you to control force in addition to position control, such as tracing the surface of an object, fitting one object to another object, grasping a soft object without breaking it, etc. It becomes possible to correspond to the scene.

  Patent Document 1 discloses a technique for performing impedance control with high accuracy even in a very complicated system.

JP-A-6-320451

  In a normal robot, an end effector portion (for example, a hand or the like) provided at the tip of an arm has a high rigidity. This is because it is difficult to increase the accuracy in controlling the spatial position of the end effector section (specifically, the position of the hand of the robot, etc.) with low rigidity. However, if the end effector portion is highly rigid, control that particularly takes into account the collision between the end effector portion and an operator or the like is required.

  Further, by performing impedance control in Patent Document 1, it is possible to make the robot appear to be soft (for example, to reduce the apparent elasticity, viscosity, etc.). However, this soft characteristic is only realized by impedance control, and if the control unit that performs impedance control stops operating for some reason, the mechanical characteristics inherent to the robot will appear. Become. That is, since the rigidity of the robot is increased when the control unit is stopped, there is no change in that in this case as well, control in consideration of the collision between the end effector unit and the operator is necessary.

  On the other hand, since the necessity for avoiding a collision etc. can be lowered | hung by using a thing with low rigidity for an end effector part, control becomes easy about this point. However, in this case, the hand position accuracy of the robot is lowered. Further, the external force is modulated by the end effector having low rigidity, and the external force detection accuracy of the force sensor may be deteriorated, which may hinder the operation of the robot.

  According to some aspects of the present invention, it is possible to provide a robot that performs appropriate control, a robot control method, and the like even when the robot is provided with a mechanical mechanism having an elastic member.

  One embodiment of the present invention includes an arm, a force sensor provided in the arm, a mechanical mechanism provided in the arm via the force sensor, and having an elastic member, and the position and posture of the mechanical mechanism. A correction unit that calculates a correction value of a target value; and a control unit that corrects the target value based on the correction value and controls the arm based on the corrected target value. The sensor information on the force detected by the force sensor is output, and the correction unit uses the sensor information output by the force sensor to calculate the correction value corresponding to the displacement of the elastic member based on the force. In other words, the control unit relates to a robot that controls the arm to cancel the displacement of the elastic member based on the corrected target value.

  In one embodiment of the present invention, a robot includes an arm, a force sensor provided in the arm, and a mechanical mechanism provided in the arm via the force sensor and having an elastic member. In this case, robot control for canceling the deformation of the elastic member is performed based on the sensor information of the force sensor. Therefore, when the mechanical mechanism receives an external force, it is assumed that a deviation from the target value occurs. However, the deviation can be suppressed, and the mechanical mechanism is controlled as if it is highly rigid. It becomes possible. For this reason, it is possible to improve accuracy in position control of the mechanical mechanism.

  In the aspect of the invention, the mechanical mechanism may be an end effector unit of the robot, and the correction unit may use the sensor information output from the force sensor, and the end effector unit based on the force. The correction value corresponding to the displacement may be obtained, and the control unit may perform control of the arm that cancels the displacement of the end effector unit based on the corrected target value.

  Accordingly, it is possible to cope with the case where the mechanical mechanism is an end effector portion of the robot.

  In one aspect of the present invention, the apparatus includes a force control unit that outputs a second correction value of the target value of the position and the posture of the mechanical mechanism using the sensor information output from the force sensor. The control unit may correct the target value based on the correction value and the second correction value, and control the robot based on the corrected target value.

  This makes it possible to perform force control such as impedance control in addition to control such as making the mechanical mechanism appear to be highly rigid.

  In the aspect of the invention, the force control unit may convert the sensor information in the mechanical mechanism when the mechanical mechanism converts the external force f and transmits the force F to the force sensor. The force control unit may obtain the second correction value based on an output signal from the external force detection unit.

  As a result, the external force f can be accurately obtained, and force control such as impedance control can be appropriately performed.

  In the aspect of the invention, the force control unit may perform processing for obtaining the inverse transformation and the second correction value by digital filter processing.

  As a result, the processing can be converted to a digital filter, and hardware and stability determination can be facilitated.

In the aspect of the invention, the external force detection unit may set the sensor information value at the nth (n is an integer of 3 or more) timing to _Fn , and the intermediate parameter at the (n-1) th timing to _{xn−. 1} and when the coefficient parameter of the digital filter processing is α and β, the intermediate parameter x _n at the n-th timing is obtained by x _n = αF _n + βx _n−1 and the obtained intermediate parameters x _n, and the intermediate parameter x _n-2 obtained in the first n-1 of said intermediate parameter x _n-1 and the n-2 timing determined by the timing, the coefficient parameters of the digital filter from _{C 0, C 1, C 2} , f n = - by _{{x n (C 1 x n} -1 + C 2 x n-2)} / C 0, the at the timing of the n may be calculated the sensor information f _n after the inverse transform.

  This makes it possible to perform the above processing after setting a specific digital filter.

  In the aspect of the invention, the correction unit may perform a process for obtaining the correction value by a second digital filter process.

  This makes it possible to make a digital filter for control such as making the mechanical mechanism appear to be highly rigid.

In one embodiment of the present invention, the value of the sensor information at the k-th (k is an integer of 2 or more) timing is F _k , and the output value of the second digital filter processing that represents the correction value is x _k. When the output value of the second digital filter processing at the (k−1) th timing is x _k−1 and the coefficient parameters of the second digital filter processing are α and β, the correction unit is The correction value x _k at the _k -th timing may be obtained by x _k = αF _k + βx _k−1 .

  This makes it possible to perform the above processing after setting a specific digital filter.

  Moreover, in one aspect of the present invention, a target value output unit that outputs the target values of the position and the posture of the mechanical mechanism may be included.

  This makes it possible to set a target value used for robot control.

  According to another aspect of the present invention, there is provided a robot control for controlling a robot including an arm, a force sensor provided in the arm, and a mechanical mechanism provided in the arm via the force sensor and having an elastic member. A method for obtaining a correction value corresponding to a displacement of the elastic member based on the force using sensor information about the force detected by the force sensor, and correcting a target value of the robot based on the correction value. Then, the present invention relates to a robot control method for controlling the arm that cancels the displacement of the elastic member based on the corrected target value.

  According to another aspect of the present invention, there is provided a robot control for controlling a robot including an arm, a force sensor provided in the arm, and a mechanical mechanism provided in the arm via the force sensor and having an elastic member. It is a method, Comprising: It is related with the robot control method which controls the said arm which cancels the displacement of the said elastic member based on the said force using the sensor information regarding the force which the said force sensor detected.

A configuration example of a force sensor. A specific example in which a mechanical mechanism is provided at the tip of a force sensor. 1 is a configuration example of a robot according to a first embodiment. 4A and 4B are examples of a robot. An example of mechanical mechanism modeling. The figure explaining the modulation | alteration in a mechanical mechanism. The example which comprises a mechanical mechanism using a damper, a spring, etc. FIG. 8A to FIG. 8C are diagrams illustrating force control according to the first embodiment. An example of a digital filter used for correction processing. The flowchart explaining the process in 1st Embodiment. The structural example of the robot of 2nd Embodiment. Another example of a digital filter used for correction processing. Another example of a digital filter used for correction processing. FIG. 14A to FIG. 14C are explanatory diagrams of force control. FIG. 15A and FIG. 15B are explanatory diagrams regarding compliance control. 16A and 16B are explanatory diagrams of impedance control. The figure explaining the relationship between the characteristic of a mechanical mechanism, and the characteristic implement | achieved by force control. The flowchart for demonstrating the process in an external force detection part. The flowchart explaining impedance control.

  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. First, the reason why a low-rigidity mechanical mechanism is used, and the matters that should be taken into account, will be described as a technique in the first embodiment. After that, a method of combining the method of the first embodiment with general impedance control and the items to be considered at that time will be described as a method of the second embodiment.

1.1 Method of First Embodiment FIG. 1 shows a configuration example of a force sensor used in this embodiment. Although various configurations can be considered for the force sensor, 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.

As shown in FIG. 1, the force sensor can be modeled by modeling the mechanical structure of the force sensor. When the equivalent mass term of the force sensor is m ₀ , the equivalent viscosity term is μ ₀ , and the equivalent elastic term is k ₀ , the characteristic can be expressed as the following equation (1).

Here, x represents the deformation (displacement) of the sensing unit measured by the deformation measurement method (specific example is as described above), and F represents the external force received by the force sensor. That is, since the equivalent mass term m ₀ , equivalent viscosity term μ ₀ , and equivalent elastic term k ₀ of the force sensor can be known by design, the external force F is obtained from the above equation (1) by measuring the displacement x. can do. The force sensor outputs F as sensor information.

However, in practice, since the elastic term k ₀ is made relatively large, the time differential term becomes relatively small, and it is generally unnecessary to explicitly consider the equation of motion. If the elastic term is not relatively large (that is, if the force sensor is not made very hard), the deformation of the sensing unit will be large, and accordingly the space of the structure provided at the tip of the force sensor This is because the target position fluctuates. For example, in the case of a robot, it is assumed that a hand is provided at the tip of the force sensor, but if the force sensor is not hard, the spatial position of the hand, that is, the position of the hand of the robot will fluctuate. The accuracy of control will deteriorate.

  From the above, although a model of the force sensor is shown in FIG. 1, it is not necessary to consider such a model in this embodiment, and it is handled that there is no deformation of the sensing unit of the force sensor. May be. The problem in this embodiment is due to the rigidity of the mechanical mechanism.

  As shown in FIG. 2, the robot of this embodiment has an arm, and the arm is provided with a force sensor, and the force sensor is provided with a mechanical mechanism. The mechanical mechanism of the present embodiment has low rigidity (for example, a case where the mechanical mechanism is configured by a flexible structure such as an elastic member is considered). By using a mechanical mechanism with low rigidity, it is possible to suppress the influence of a collision with an operator or the like even when mechanical characteristics of the mechanical mechanism appear. Here, the “elastic member” of the mechanical mechanism means a flexible structure that is not a rigid body included in a part of the mechanical mechanism, and if all the members constituting the mechanical mechanism are flexible structures, the entire mechanical mechanism or the mechanical mechanism Moving parts.

  However, when the rigidity of a mechanical mechanism (for example, an end effector section and a hand or the like in a narrow sense) is low, the accuracy of position control of the mechanical mechanism is lowered. For this reason, there is a possibility that the operation by the robot (for example, movement of a workpiece between predetermined positions) may be hindered.

  Therefore, the present applicant proposes a method for performing appropriate control even when a mechanical mechanism having low rigidity is used. Specifically, control is performed such that the displacement (deformation) of the mechanical mechanism is obtained based on the sensor information detected by the force sensor, and the obtained displacement is canceled. Although details will be described later with reference to FIGS. 8A to 8C, since the displacement is canceled, for example, when the robot is in a stationary state, the spatial position of the mechanical mechanism (the robot's position) The hand position) can be kept constant. That is, even if an external force is applied, the hand position does not change, so that it is possible to realize a high rigidity and improve the hand position accuracy. If the robot is moving, the displacement of the mechanical mechanism corresponds to the amount of deviation from the target trajectory, so control that cancels the displacement does not cause the target trajectory to deviate from the target trajectory. It becomes. Even in this case, the high rigidity can be realized apparently.

  Further, in order to obtain the displacement of the mechanical mechanism from the sensor information of the force sensor, in the present embodiment, the characteristics of the mechanical mechanism are modeled. Details will be described later.

  The above method will be described in detail in the first embodiment. Specifically, a system configuration example will be described first, and then modeling of a mechanical mechanism will be described. Then, a method for force control will be described, and a method for realizing the force control using a digital filter will be described. Finally, details of the processing in the first embodiment will be described using a flowchart.

1.2 Method of Second Embodiment By the above-described method, the influence of collision or the like is suppressed even when the mechanical characteristics inherent to the mechanical mechanism appear while improving the accuracy of the position control of the mechanical mechanism. Is possible. However, in this control, the flexible structure is apparently made highly rigid, and it is difficult to perform operations such as tracing the surface of the object. Therefore, when such an operation is performed, it is preferable to further combine impedance control.

  In this case, there is a problem that the rigidity of the mechanical mechanism is lower than that of the force sensor. This is because the external force f applied to the mechanical mechanism is converted (modulated) by the mechanical mechanism and is transmitted to the force sensor as a force F. As an example, if the mechanical mechanism is a flexible structure, a part of the energy generated by f is used for deformation of the flexible structure, so that the force F transmitted to the force sensor is smaller than the external force f. Can be considered.

  As described above, F, which is the output value of the force sensor, does not need to take into account deformation of the force sensor itself. That is, it may be considered that the force transmitted from the mechanical mechanism to the force sensor is accurately reflected in the sensor information. However, what the system actually wants to acquire is not the force F but the external force f before conversion. This is because the impedance control should be performed based on the force applied to the workpiece to be processed by the hand, and the force is equal to the force transmitted from the workpiece to the hand as a reaction force, which is nothing but the external force f in FIG. That is, if the sensor information of the force sensor is used as it is, desired force control cannot be performed, which is not preferable.

  Therefore, the present applicant proposes a method of performing correction processing corresponding to the inverse conversion of the conversion by the mechanical mechanism on the sensor information from the force sensor. By using this method, it becomes possible to accurately detect an external force from the sensor information of the force sensor, and impedance control based on the external force can be appropriately performed.

  The above method will be described in detail in the second embodiment. Specifically, a system configuration example will be described first, and then a method for detecting external force by correcting sensor information will be described. Then, the method of impedance control will be described, and finally the details of the processing in the second embodiment will be described using a flowchart.

2. First Embodiment 2.1 System Configuration Example FIG. 3 shows a configuration example of a robot according to the present embodiment. The robot includes a force sensor 10, a correction unit 40, a target value output unit 60, a control unit 80, and a robot body 100. Note that the robot according to the present embodiment is not limited to the configuration shown in FIG. 3, and various modifications such as omitting some of the components or adding other components are possible.

  The force sensor 10 is as described above.

  The correction unit 40 obtains a correction value corresponding to the displacement (deformation) of the mechanical mechanism (in the narrow sense, the hand 110) based on the sensor information from the force sensor 10, and outputs the correction value to the target value output unit 60. A method for obtaining the correction value will be described later. In the case of the configuration as shown in FIG. 2, it is conceivable that the force sensor 10 detects a force other than an external force due to its own weight, posture, etc., such as the mechanical mechanism (hand 110) or the arm 120. Therefore, the correction unit 40 may acquire information obtained by performing preprocessing such as self-weight posture correction on the sensor information, and obtain a correction value based on the acquired information.

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

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

  The control unit 80 performs feedback control of the arm 120 based on the target value from the target value output unit 60. Specifically, feedback control of the arm 120 is performed based on the target value output as a result of the correction process based on the correction value from the correction unit 40. For example, feedback control of the arm 120 is performed based on the target value and a feedback signal from the robot body 100.

  The robot body 100 includes a hand 110 (mechanical mechanism) and an arm 120. Specifically, as shown in FIG. 2, it is assumed that the force sensor 10 is provided on the arm 120 and the hand 110 is provided at the tip of the force sensor 10. The robot may have a motor control unit (not shown in FIG. 3), and the motor control unit sets a joint angle such as the arm 120, for example. The control unit 80 controls the hand 110, the arm 120, and the like of the robot body 100, but actually acquires the value of the joint angle information as a target value and controls the motor control unit based on the target value. It will be.

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

  A system (correction unit 40, target value output unit 60, control unit 80, etc.) that performs control in the present embodiment is provided in, for example, the control device 300 in FIG. A control system is realized. According to the control system included in the robot of the present embodiment, it is possible to reduce performance requirements for control hardware such as the control device 300 and to operate the robot body 310 with high responsiveness. 4A shows an example of a single arm type, a multi-arm type robot such as a double arm type may be used.

  Further, the robot of the present embodiment is not limited to FIG. 4A, and may be configured as shown in FIG. As shown in FIG. 4B, the robot includes a robot main body 310 (having an arm 320 and a hand 330) and a base unit portion that supports the robot main body 310, and the control device 300 is stored in the base unit portion. . In the robot shown in FIG. 4B, wheels or the like are provided in the base unit portion so that the entire robot can move. The movement may be performed manually or may be performed by providing a motor for driving the wheel and controlling the motor by the control device 300.

  That is, the robot according to the present embodiment is roughly divided into the robot main body 310 (and the force sensor 10) and the control device 300 (the correction unit 40, the target value output unit 60, the control unit 80, etc. shown in FIG. 3). However, these spatial and functional relationships can be freely set. For example, as shown in FIG. 4B, the robot main body 310 and the control device 300 may be integrally formed, and may move as a unit when moving. In the case of FIG. 4B, since the robot main body 310 and the control device 300 are provided at close positions, it is easy to exchange control signals and the like. The control device 300 is unlikely to perform control other than the integrally formed robot body 310, and therefore can be configured specifically for the robot body 310, and cost reduction and performance improvement can be expected. .

  Alternatively, as shown in FIG. 4A, the robot body 310 and the control device 300 may be provided separately. In this case, the robot main body 310 and the control device 300 can be provided at positions separated from each other. There are many situations where it is dangerous for a person to approach a work site using a robot, such as welding, and there is an advantage in a configuration that can easily realize remote control. Moreover, since it is not integrally formed, it is difficult to receive spatial restrictions, and it is easy to cope with expansion such as adding the robot main body 310 (or any part included therein) later.

  Note that the term robot can be widely interpreted. As described above, both the robot main body 310 and the control device 300 may be included, or the robot main body 310 and a portion integrally formed therewith may be limited. In such a limitation, the part represented by the term robot includes the robot main body 310 and the control device 300 in the example of FIG. 4B as described above, but the example of FIG. Then, the robot body 310 is pointed and the control device 300 is not included. Alternatively, the portion represented by the term robot refers to the robot main body 310 and may not include the control device 300. That is, the invention formed as a system including the robot main body 310 as a robot and including the configuration requirements corresponding to the correction unit 40, the control unit 80, etc. in addition to the robot is also included in the scope of the present invention. Conceivable.

2.2 Modeling of the mechanical mechanism A technique for determining the displacement of the mechanical mechanism based on the sensor information of the force sensor 10 by modeling the characteristics of the mechanical mechanism will be described.

  Although various mechanical mechanism modeling methods are conceivable, in this embodiment, as shown in FIG. 5, an object having a mass m is connected to the force sensor 10 by a damper having a viscosity coefficient μ and a spring having an elastic coefficient k. Approximate with the structure. By modeling in this way, as shown in FIG. 6, the conversion in the mechanical mechanism is expressed by the following expression (2).

  Here, x represents the displacement of the mechanical mechanism. Therefore, the above equation (2) is nothing but an equation of motion. About this, as shown in FIG. 5, it can be considered that the mechanical mechanism is regarded as a structure including a mass object, a damper, and a spring. Alternatively, the characteristics of the mechanical mechanism can be expressed by a linear combination of the 1st to Nth derivative terms of x and x as in the following equation (3), but the second derivative term in the following equation (3) is used. It can also be regarded as an approximate expression.

  FIG. 5 is a conceptual diagram for approximation in modeling the mechanical mechanism, and is not necessarily a structure composed of a damper, a spring, or the like in this way. That is, as described above, the above expression (2) is an expression that approximates the characteristics of the mechanical mechanism, and is different from the actual characteristics. This can be understood from the explanation using the above equation (3).

  In general, it is unlikely that a problem such as accuracy will occur in the subsequent control even after such approximation. However, if an error in approximation becomes a problem, the actual mechanical mechanism may be constituted by a damper and a spring as shown in FIG. Considering the shape of the mechanical mechanism, the above equation (2) does not completely reflect the characteristics of the mechanical mechanism in this case as well, but compared with the case where a mechanical mechanism that is not composed of a damper and a spring is used. Thus, the error due to approximation can be reduced.

  Next, the relationship with the converted force F transmitted to the force sensor 10 will be considered. Assuming that the force sensor 10 is fixed, the force F is equal to the external force f minus the inertia term, and is expressed by the following equation (4).

  The following expression (5) is derived from the above expressions (2) and (4).

  In other words, the above equation (5) represents the balance of forces on the surface indicated by C1 in FIG. When the external force f acts on the object indicated by C2 in FIG. 5, if the behavior of the mechanical mechanism is taken into consideration, the influence of the inertia due to the object of C2 cannot be ignored, so the inertia term is included as in the above equation (2). The obtained relational expression is acquired. However, when considering the balance of forces on the surface indicated by C1 (that is, when considering the force transmitted to the force sensor 10), the object of C2 is not physically connected directly to the surface of C1. The inertia due to the object does not need to be taken into consideration, and the above expression (4) represents it. Alternatively, the force received by the surface of C1 may be considered to be limited to that received from a physically connected damper and spring, and the above equation (5) can be directly derived from such a view.

  In any case, the relationship between F and x is determined from the above equation (5). That is, the information x corresponding to the displacement can be obtained by solving the differential equation of the above equation (5) with the sensor information F as an input.

2.3 Force Control in this Embodiment A method for controlling the robot (arm 120) based on the displacement x obtained from the above equation (5) will be described with reference to FIGS. 8 (A) to 8 (C). . In the mechanical mechanism of the present embodiment as shown in FIG. 8A (FIG. 8A may represent a modeled mechanical mechanism or an actual structure), Since the mechanical mechanism has low rigidity, it is deformed as shown in FIG.

  In this embodiment, if only x is pushed in as shown in FIG. 8B, control is performed to advance the robot body 100 itself (or the arm 120 itself) by x so as to cancel the displacement. By doing so, the robot body 100 responds with a force corresponding to the applied external force. If the control of the robot main body 100 is sufficiently fast, the time during which the displacement is generated can be shortened as shown in FIG. 8B, so that the robot hand (mechanical mechanism, hand 110) keeps staying at a fixed position. Can do. In other words, “cancel the displacement” means that the position where the external force is applied (for example, the hand of the robot) is controlled so that it looks as if it stays at a certain position. For example, when the hand 110 has an elastic member, if an external force is applied to the hand 110 or the applied external force changes, the position of the hand 110 to which the external force is applied should move (displace). However, control is performed so that the position where the external force of the hand 110 is applied is seen as if it does not move.

  Even if an external force is applied, the position of the hand does not fluctuate, and this is nothing but an apparently high rigidity of the robot (or its hand 110).

  As shown in FIG. 3, if the robot includes the correction unit 40, the target value output unit 60, and the control unit 80, the correction unit 40 obtains a correction value corresponding to the displacement x, and the target value output unit 60. Then, after correcting the target value based on the correction value, the control unit 80 performs control based on the corrected target value.

  In the above, in order to simplify the description, the description has been made using one value x as a correction value. However, if a robot capable of three-dimensional behavior is used, x is represented by a three-dimensional vector or the like. In other words, the robot body 100 is controlled to move in the distance and direction corresponding to the inverse vector of x.

  Further, x obtained from the above equation (5) represents a virtual displacement when the mechanical mechanism is modeled, and is not necessarily the actual hand position itself. However, in many cases, x and the actual hand position have a simple relationship, and it is considered that conversion from x to the actual hand position is easy. In this case, the correction unit 40 obtains x from the above equation (5), further performs conversion processing to the actual hand position, and outputs the result as a correction value. The conversion from x to the actual hand position depends on the structure of the mechanical mechanism and is difficult to generalize.

2.4 Digital Filtering As described above, to obtain the displacement x from F, the linear ordinary differential equation (5) is solved. Conventionally, a Newton method, a Runge-Kutta method, or the like has been used to obtain a solution of a linear ordinary differential equation. However, these methods are not suitable for hardware and it is difficult to determine stability. Therefore, the present applicant uses a digital filter as a method for solving a linear ordinary differential equation.

  Here, the step of obtaining the solution of the ordinary differential equation is regarded as a filter that outputs a solution (displacement x in the above equation (5)) with respect to the input of the input value (sensor information of the force sensor). Then, from the form of the above equation (5), it can be considered as a one-pole analog filter.

  That is, since the solution of the ordinary differential equation can be obtained as the output of the analog filter, the ordinary differential equation can be solved by the digital filter by converting the analog filter into a digital filter.

  Various methods for converting an analog filter into a digital filter are known. For example, an impulse invariance method may be used. This is a technique for considering a digital filter that gives the same impulse response as the value obtained by sampling the impulse response of an analog filter at a discrete time T. Since the Impulse Invariance method is a known method, detailed description thereof is omitted.

  As a result, the solution of the ordinary differential equation can be obtained as the output of the digital filter. In the case of the above formula (5), a one-pole digital filter is obtained as shown in FIG. In FIG. 9, d is a delay for one sample, and α and β are filter coefficients. The filter of FIG. 9 is represented by the following formula (6).

x _n = αF _n + βx _n−1 (6)
If it is a process using a digital filter, it is easy to implement hardware and it is easy to determine stability. This is because the stability of the digital filter can be easily determined by whether or not the pole of the digital filter is inside the unit circle.

  The filter coefficients α and β are determined by the characteristics of the mechanical mechanism. In this case, the mass term m, the viscosity term μ, and the elastic term k (in some cases, in addition to the above, the driving frequency of the digital filter) T may be used). Since the characteristics of the mechanical mechanism (hand 110) are known by design, the filter coefficient can be obtained in advance. Further, even when considering replacing a plurality of hands 110, if filter coefficients corresponding to the respective hand characteristics are obtained, it is possible to easily cope with them by switching the filter coefficients. Therefore, the Runge-Kutta It is superior to conventional methods such as law.

2.5 Details of Processing The processing in the correction unit 40 will be described using the flowchart of FIG. When this process is started, first, acquisition of sensor information from the force sensor 10 is waited (S101). The waiting time is determined according to the sensor information output rate of the force sensor 10. Here, it is assumed that correction processing by the correction unit 40 is performed every time sensor information of the force sensor 10 is output. However, when reducing power consumption or the like, the processing may be performed for each of a plurality of outputs.

After waiting for input, sensor information is acquired (S102). As described above, the sensor information corresponds to the force F (external force modulated by the mechanical mechanism). Then, correction processing is performed based on the acquired force F (S103). Specifically, the past value (x _n−1 ) of the information x corresponding to the displacement is updated, and the current value of x (x _n ) is obtained from the updated x _n−1 and F _n . Α and β used at this time are filter coefficients of the digital filter, and are determined from m, μ, k, etc., which are characteristics when the mechanical mechanism is modeled. Thereafter, the obtained _xn is output as a correction value (S104), and it is determined whether or not to end the process (S105). If No in S105, the process returns to S101 and continues. If Yes in S105, the process ends.

The output correction value _xn is used for target value correction processing. Note that the target value setting process and the robot control based on the corrected target value are well-known techniques, and therefore description using a flowchart is omitted.

  In the above embodiment, as shown in FIG. 3, the robot is a machine having an arm 120, a force sensor 10 provided on the arm 120, and an elastic member provided on the arm 120 via the force sensor 10. A mechanism (hand 110 in a narrow sense), a correction unit 40 that calculates a correction value of the target value of the position and orientation of the mechanical mechanism, a target value is corrected based on the correction value, and the arm 120 is corrected based on the corrected target value. The control part 80 which controls is included. In this case, the force sensor 10 outputs sensor information corresponding to the detected force, and the correction unit 40 uses the sensor information output from the force sensor 10 to correct a correction value corresponding to the displacement of the elastic member based on the force. The control unit 80 controls the arm 120 that cancels the displacement of the elastic member based on the corrected target value.

  Here, the target values of the position and orientation of the mechanical mechanism are target values in the control of the robot main body 100. In an ideal situation where there is no disturbance and no error in driving, the mechanical mechanism is in accordance with the target value. Will take the position and posture. The target values of the position and orientation of the mechanical mechanism are, for example, three-axis coordinate values (xyz) in spatial coordinates and rotations (uvw) around each axis, but are not limited thereto, and are not limited to the mechanical mechanism. It may be joint angle information for taking a position and posture corresponding to xyzvw. In the method of the present embodiment, the target value is corrected using a correction value to obtain a corrected target value, and feedback control of the robot body 100 (arm 120 in a narrow sense) is performed based on the corrected target value. The value used for is determined. As a value used for feedback control, a corrected target value may be used as it is, or a result obtained by performing some processing may be used. The value used for the feedback control is generally the joint angle information of the robot main body 100, but does not prevent the hand position (xyz), the angle around each axis (uvw), and the like. As shown in FIG. 3, if the target value output unit 60 includes a trajectory generation unit 62 and an inverse kinematics processing unit 64, the trajectory generation unit 62 sets a target trajectory as position information (xyzvw). Then, it is conceivable that the inverse kinematics processing unit 64 performs inverse kinematics processing on the target trajectory to convert it into joint angle information.

  Since the correction value corresponds to the displacement of the elastic member, it is assumed that the correction value is information corresponding to the position. However, the correction value is joint angle information that is a result of performing a predetermined process (specifically, inverse kinematics process). May be. In general, it is assumed that correction processing is performed using a target trajectory (target values of position and orientation) and correction values that are position information, and inverse kinematics processing is performed on the result. This is because the inverse kinematics process is only required once. However, for example, when the processing timing and processing rate are different between the target value setting process and the process for obtaining the correction value, an inverse kinematics processing unit is provided on the correction unit 40 side, and the target trajectory and the elastic member are provided. Inverse kinematics processing may be performed for each displacement, and correction processing may be performed on the obtained joint angle information.

  In other words, the “target value”, “correction value”, “target value after correction”, and “value used for feedback control” may be information relating to the position or information relating to the joint angle. Good. As described above, for example, as described above, the target value set as position information is subjected to correction processing using a correction value that is displacement (position information), and a corrected target value is obtained as position information. Then, inverse kinematics processing may be performed on the corrected target value to obtain joint angle information, and the joint angle information may be used as a value used for feedback control.

  In addition, the elastic member of the mechanical mechanism may actually have a mechanical structure such as a spring as shown in FIG. 7, or it does not have such a mechanical structure but has rigidity as a whole. It may be low.

  As a result, even in a robot provided with a mechanical mechanism having an elastic member (specifically, low rigidity), it becomes possible to control the mechanical mechanism as if it had high rigidity. The accuracy can be improved. Furthermore, since the inherent characteristics of the mechanical mechanism are low rigidity (having an elastic member, for example, a flexible structure), even when force control stops and the original characteristics appear, a collision, etc. It becomes possible to suppress the influence by. Specifically, as described with reference to FIGS. 8A to 8C, when the mechanical mechanism is deformed by x by an external force, the robot 100 is moved by an amount corresponding to x. The hand position of the robot 100 is kept constant (strictly, control is performed so as to follow the position corresponding to the target value).

  Further, the mechanical mechanism may be an end effector portion (hand 110 in a narrow sense) of a robot. In this case, the correction unit 40 obtains a correction value corresponding to the displacement of the end effector unit based on the external force using the sensor information output from the force sensor 10. And the control part 80 performs control which cancels the displacement of an end effector part based on the corrected target value.

  This makes it possible to apply the method of the present embodiment even when the mechanical mechanism is an end effector unit of a robot. When a general industrial robot or the like as shown in FIG. 4 is considered, it is assumed that a part accompanied with contact with a work target (for example, a workpiece) is exclusively an end effector unit such as the hand 330. Therefore, it can be said that it is very important to make the end effector part a structure having an elastic member and to compensate for the accompanying deterioration in hand position accuracy.

  Further, the correction unit 40 may perform a process of obtaining a correction value by the second digital filter process.

  Here, the second digital filter processing is merely referred to as “second” for distinction from the digital filter processing of the force control unit in the second embodiment described later, and will be described in the present embodiment. Refers to digital filter processing.

  As a result, the processing for obtaining the correction value in the present embodiment can be realized by the digital filter. The correction value is obtained as a solution of the differential equation (5) above, but using the Runge-Kutta method or the like makes it difficult to implement hardware or determine stability, but uses a digital filter. This makes it possible to deal with such problems. Specifically, a filter coefficient may be obtained from the characteristics of the mechanical mechanism (for example, m, μ, k, etc.), and processing using a digital filter may be performed using the obtained filter coefficient. Since the characteristics of the mechanical mechanism are determined at the design stage, the filter coefficient can be calculated in advance. Even when the mechanical mechanism can be replaced, it is possible to easily cope with the problem by storing a plurality of sets of filter coefficients and using an appropriate one.

In addition, the value of sensor information at the timing of the k-th (k is an integer equal to or greater than 2) is F _k, and the output value of the second digital filter processing representing the correction value is x _k . Further, the output value of the second digital filter process at the (k−1) th timing is x _k−1, and the coefficient parameters of the second digital filter process are α and β. In this case, the correction unit 40 may obtain the correction value x _k at the _k -th timing by x _k = αF _k + βx _k−1 .

  As a result, the correction value can be obtained by the digital filter shown in FIG. By modeling the mechanical mechanism as described above, the relationship between F corresponding to the sensor information and the displacement x is expressed by the above equation (5). In order to derive the digital filter from here, for example, the above equation (5) may be regarded as an analog filter and then converted into a digital filter by the Impulse Invariance method or the like. The specific formula when the digital filter is used is as shown in the above formula (6).

  Further, as shown in FIG. 3, the robot may include a target value output unit 60 that outputs target values of the position and orientation of the mechanical mechanism.

  This makes it possible to set a target value. The details of the target value and the configuration example of the target value output unit 60 are as described above. In addition, when the target value output unit 60 includes the trajectory generation unit 62, various methods can be applied for setting the target trajectory. For example, the target trajectory may be set based on an algorithm for reducing the number of movements / movement distance of the arm 120 or the like, an algorithm for avoiding a collision, an algorithm for suppressing an influence when a grasped object falls, or the like. In addition, since the mechanical mechanism of this embodiment has an elastic member, the margin for collision avoidance etc. can be set small compared with the case where a highly rigid mechanical mechanism is used. Therefore, it may be possible to simplify the processing in the collision avoidance algorithm by the configuration and method of the present embodiment.

3. Second Embodiment 3.1 System Configuration Example FIG. 11 shows a configuration example of a robot according to this embodiment. The robot of the present embodiment includes a force sensor 10, a force control unit 20, a correction unit 40, a correction value synthesis unit 50, a target value output unit 60, a control unit 80, and a robot body 100. The robot according to the present embodiment is not limited to the configuration shown in FIG. 11, and various modifications such as omitting some of the components or adding other components are possible.

  The force sensor 10, the correction unit 40, the control unit 80, and the robot body 100 are the same as those in the first embodiment, and thus detailed description thereof is omitted. The target value output unit 60 also differs only in the correction value acquired for the target value correction process, and the other configurations are the same, and thus detailed description thereof is omitted.

  The force control unit 20 performs force control (force control) based on the sensor information from the force sensor 10, and outputs a second correction value of the target value (different from the correction value in the correction unit 40). To do. More specifically, the force control unit 20 performs impedance control (or compliance control) based on sensor information (force information, moment information) from the force sensor 10.

  The force control unit 20 can include an impedance processing unit 22, a control parameter storage unit 24, and an external force detection unit 26. The impedance processing unit 22 performs impedance control as force control. A specific method of impedance control will be described later. The control parameter storage unit 24 stores control parameters used in force control (impedance control).

  The external force detection unit 26 performs a correction process on the sensor information (including information obtained by performing preprocessing such as self-weight posture correction after acquisition). In the external force detection unit 26, assuming that the external force f received by the mechanical mechanism is converted into the force F by the mechanical mechanism and transmitted to the force sensor 10, a correction process corresponding to the inverse conversion of the conversion by the mechanical mechanism is performed. To obtain the external force f. The mechanical mechanism itself and the conversion in the mechanical mechanism are modeled, and the inverse conversion is obtained based on the modeling. Details will be described later.

  The correction value synthesizing unit 50 synthesizes the correction value (first correction value) obtained by the correction unit 40 and the second correction value obtained by the force control unit, and uses the corrected correction value as a target value. Output to the output unit 60.

3.2 External force detection method An external force detection method in the external force detection unit 26 will be described. Here, as described in the first embodiment, the modulation by the mechanical mechanism is modeled as shown in FIG. 6, and the above equations (2) and (5) derived by the modeling are used.

  What is desired here is a method for obtaining the external force f from F corresponding to the sensor information, but the relationship between f and F can be described by the above equations (2) and (5). Specifically, the conversion by the mechanical mechanism can be regarded as a system that takes f as an input and outputs F based on the above equations (2) and (5). Therefore, the inverse transformation may be regarded as a system that takes F as an input and obtains f based on the above equations (5) and (2). Specifically, F is acquired as sensor information, and x is obtained by solving the differential equation of the above equation (5). Then, the obtained x may be substituted into the above equation (2) to obtain f.

  Note that, as in the first embodiment, the linear ordinary differential equation of the above equation (5) is converted into a digital filter, and the process for obtaining f from F can also be converted into a digital filter. First, the linear ordinary differential equation (equation of motion) of the above equation (2) becomes a two-pole digital filter as shown in FIG. In FIG. 12, C0, C1, and C2 are filter coefficients, which are expressed in the following equation (7).

_{x n = C 0 f n +} C 1 x n-1 + C 2 x n-2 ····· (7)
The filter coefficients C0, C1, and C2 are determined by the characteristics of the mechanical mechanism. In this case, the mass term m, the viscosity term μ, and the elastic term k (in some cases, in addition to the above, the digital filter Drive frequency T may be used). Similar to the first embodiment, the filter coefficient can be obtained in advance, and even when changing the plurality of hands 110 is considered, it can be easily handled by switching the filter coefficient. is there.

  The above equations (6) and (7) can be transformed into the following equation (8).

_{C 0 f n = αF n +} βx n-1 - (C 1 x n-1 + C 2 x n-2) ····· (8)
FIG. 12 illustrates the above equation (8). By using the digital filter of FIG. 12, it is possible to realize a correction process (inverse conversion) in which the force F detected by the force sensor 10 is input and the external force f received by the mechanical mechanism is output.

3.3 Specific Example of Impedance Control Next, a specific example of impedance control will be described. The impedance control here is performed in the impedance processing unit 22 based on the external force f obtained by the external force detection unit 26.

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

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

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

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

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

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

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

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

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

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

In the above equation (9), m _I is mass, μ _I is a viscosity coefficient, k _I is an elastic coefficient, f is a force (external force acquired by the inverse transformation process in the external force detection unit 26), and u is from the target position. Displacement. The first and second derivatives of u correspond to speed and acceleration, respectively. In the impedance control, a control system for providing the characteristic of the above equation (9) to the end effector section that is the tip of the arm is configured. That is, control is performed as if the tip of the arm has the virtual mass, virtual viscosity coefficient, and virtual elastic coefficient represented by the above equation (9).

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

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

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

By performing the impedance control described above, the robot 100 can be controlled as if it had mass m _I , viscosity μ _I , and elasticity k _I as shown in FIG. Originally, since the hand 110 (mechanical mechanism) has low rigidity, the actual characteristics (m, μ, k) of the hand 110 cannot be ignored, and the robot 100 as a whole has characteristics (m _I , μ apparently acquired by control). _I , k _I ) and the actual characteristics (m, μ, k) of the mechanical mechanism seem to appear.

However, as described in the first embodiment, since the hand 110 behaves as if it is highly rigid by the processing in the correction unit 40, it is not necessary to consider the characteristics (m, μ, k) of the mechanical mechanism. Therefore, the target characteristics (m _I , μ _I , k _I ) in the control may be set as desired characteristics.

  The above equation (9) can also be converted to a digital filter in the same manner as the correction process in the correction unit 40 and the inverse conversion in the external force detection unit 26.

3.4 Details of Processing The processing according to this embodiment will be described with reference to the flowcharts of FIGS. FIG. 18 is a flowchart for explaining the external force detection process. When this process is started, first, acquisition of sensor information from the force sensor 10 is waited (S201). The waiting time is determined according to the sensor information output rate of the force sensor 10. Here, it is assumed that the acquisition process and the correction process in the external force detection unit 26 are performed for each output of the sensor information of the force sensor 10. However, when reducing power consumption or the like, the processing may be performed for each of a plurality of outputs.

After waiting for input, sensor information is acquired (S202). As described above, the sensor information corresponds to the force F (external force modulated by the mechanical mechanism). Then, correction processing is performed based on the acquired force F (S203). Here, x is an intermediate parameter and physically corresponds to the deformation (displacement) of the mechanical mechanism. Specifically, the past values of x (x _n−1 and x _n−2 ) are updated, and the current value of x (x _n ) is obtained from the updated x _n−1 and F _n . Then, the _{x n} obtained, thereby obtaining the _{f n} from the updated _{x n-1, x n-2} Prefecture.

Then, outputs _{f n} obtained as corrected external force (S204), and determines whether the process ends (S205). If No in S205, the process returns to S201 and continues. If Yes in S205, the process ends.

FIG. 19 is a flowchart for explaining impedance control in the impedance processing unit 22. When this process is started, first, after waiting for the output timing of the external force value f _n from the external force detector 26, the external force f _n is acquired (S301, 302). And the solution of a linear ordinary differential equation (kinetic equation) is calculated | required based on acquired fn. Since the equation of motion is a second-order linear ordinary differential equation, it can be processed by a two-pole digital filter in the same manner as shown in FIG. Specifically, when the output value corresponding to the second correction value is y, the past values of y (y _n−1 and y _n−2 ) are updated, and y _n−1 after the update and The current value of y (y _n ) is obtained from y _n−2 and f _n (S303). At this time, digital filter coefficients C ₀ ′, C ₁ ′, C ₂ ′ determined based on target characteristics (m _I , μ _I , k _I ) of impedance control are used.

Then, outputs _{y n} obtained as the second correction value (S304), and determines whether the process ends (S305). If No in S305, the process returns to S301 and continues. If Yes in S305, the process ends.

After the process of FIGS. 18 and 19 has been performed, the correction value x _n in the correction value synthesizer 50 _(at the timing corresponding to y _n, and those obtained by the process of FIG. 10) and a second correction value y _n is combined and the combined correction value is output to the target value output unit 60. Note that the target value setting process and the robot control based on the corrected target value are well-known techniques, and therefore description using a flowchart is omitted.

  In the above embodiment, as shown in FIG. 11, the robot outputs force control for outputting the second correction value of the target value of the position and orientation of the mechanical mechanism based on the sensor information output from the force sensor 10. Part 20 is included. Then, the control unit 80 corrects the target value based on the correction value (obtained by the correction unit 40) and the second correction value obtained by the force control unit 20, and arm based on the corrected target value. 120 is controlled.

Here, the second correction value is used for the force control of the robot, and the second correction value is the same as the correction value obtained by the correction unit 40 in that it is used for the target value correction process. Similarly, the second correction value may be position information or joint angle information. However, the correction value of the correction unit 40 is obtained from the above equation (5) based on the characteristics (μ, k) of the mechanical mechanism, whereas the second correction value is a target characteristic that the robot 100 apparently wants to have. The points obtained from the above equation (9) based on (for example, m _I , μ _I , k _I ) are different.

  Thereby, in addition to the method of the first embodiment, another force control unit (for example, impedance control) can be performed. Although the method of the first embodiment is also control based on sensor information from the force sensor 10, it is only for making the flexible structure look solid, and a typical situation where force control is required (the surface of the object is The response to tracing, etc.) is not considered. Therefore, more flexible control is possible by combining impedance control and the like. Since the method of the first embodiment and the impedance control of this embodiment are common in that a correction value for the target value is output, these combinations can be easily realized. For example, as shown in FIG. 11, the correction value combining unit 50 may be provided, the correction values obtained from each may be combined, and the combined value may be output to the target value output unit 60.

  Further, as shown in FIG. 11, when the mechanical mechanism converts the external force f and transmits the force F to the force sensor 10 as shown in FIG. 11, the force control unit 20 reversely converts the conversion by the mechanical mechanism with respect to the sensor information. An external force detection unit 26 for performing the above may be included. Then, the force control unit 20 obtains a second correction value based on the output signal from the external force detection unit 26.

  As a result, the external force f can be accurately obtained from the signal detected by the force sensor 10. Since the mechanical mechanism of the present embodiment has an elastic member and has low rigidity, conversion (modulation) to the external force f by the mechanical mechanism cannot be ignored. With the configuration shown in FIG. 2, even if the mechanical mechanism receives an external force f, the force received by the force sensor 10 should be F due to modulation. However, what is necessary for impedance control is a force acting on a mechanical mechanism (an end effector section such as a hand), so f is not F. Therefore, it is desirable for the force control unit 20 to perform processing for obtaining the external force f from the sensor information F. Specifically, the external force detection unit 26 performs reverse conversion of conversion by a mechanical mechanism. Even in this case, if the mechanical mechanism is modeled, the processing becomes easy, and the same modeling method as in the first embodiment can be used. Therefore, the above equations (2), (4), (5), etc. are common to the processing in the external force detection unit 26. Specifically, f is obtained from F by the above equations (5) and (2). be able to.

  Further, the force control unit 20 may perform a process of obtaining an inverse transformation and a second correction value by a digital filter process.

  As a result, it is possible to digitalize the inverse transformation process (the process in the external force detection unit 26) and the process for obtaining the second correction value (the process in the impedance processing unit 22). As described above, the inverse transformation is a process for solving the above equations (2) and (5), and since both are linear ordinary differential equations, digital filtering can be performed in the same manner as the process in the correction unit 40. Since the second correction value is obtained as the solution u of the linear ordinary differential equation of the above equation (9), it can be digitally converted in the same manner. The advantage of using a digital filter is that, as described above, it is easy to make hardware and determine stability.

Further, as shown in the above equation (6), the external force detection unit 26 sets the sensor information value at the _nth timing (n is an integer of 3 or more) as the Fn and the intermediate parameter at the (n-1) th timing. When x _{n-1 is} assumed and the coefficient parameters of the digital filter processing are α and β, the intermediate parameter x _n at the n-th timing may be obtained by x _n = αF _n + βx _n−1 . Then, as shown in the above equation (7), the obtained x _n , the intermediate parameter x _n−1 obtained at the ( _n−1) th timing, and the intermediate parameter x _n obtained at the (n−2) th timing. _-2, the coefficient of the digital filter parameters _{C 0, C 1, C 2} , f n = - by _{{x n (C 1 x n} -1 + C 2 x n-2)} / C 0, of the n Sensor information (external force) f _n after correction processing at timing is obtained.

  Here, the intermediate parameter x corresponds to the displacement of the elastic member of the mechanical mechanism. Therefore, until the middle of the digital filter processing corresponding to the external force detection unit 26, the digital filter processing corresponding to the correction unit 40 is the same. Therefore, in some cases, the digital filter corresponding to the correction unit 40 and the digital filter corresponding to the external force detection unit 26 may be shared.

  Thereby, the external force f can be obtained from F based on the above equations (6) and (7). FIG. 13 is a digital filter of this process. Subsequent impedance processing and the like can be appropriately performed by accurately obtaining the external force f.

  The two embodiments 1 and 2 to which the present invention is applied and the modifications thereof have been described above, but the present invention is not limited to the embodiments 1 and 2 and the modifications as they are, The constituent elements can be modified and embodied without departing from the spirit of the invention. Various inventions can be formed by appropriately combining a plurality of constituent elements disclosed in the above-described first and second embodiments and modifications. For example, you may delete a some component from all the components described in each Embodiment 1-2 or 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.

10 force sensor, 20 force control unit, 22 impedance processing unit,
24 control parameter storage unit, 26 external force detection unit, 40 correction unit,
50 correction value synthesis unit, 60 target value output unit, 62 trajectory generation unit,
64 inverse kinematics processing unit, 80 control unit, 100 robot body,
110 hand, 120 arm, 300 controller, 310 robot body,
320 arms, 330 hands

Claims (11)

Arm,
A force sensor provided on the arm;
A mechanical mechanism provided on the arm via the force sensor and having an elastic member;
A correction unit for obtaining a correction value of a target value of the position and orientation of the mechanical mechanism;
A control unit that corrects the target value based on the correction value and controls the arm based on the corrected target value;
Including
The force sensor is
Output sensor information about the force detected by the force sensor,
The correction unit is
Using the sensor information output by the force sensor, find the correction value corresponding to the displacement of the elastic member based on the force,
The controller is
A robot that controls the arm to cancel the displacement of the elastic member based on the corrected target value. In claim 1,
The mechanical mechanism is
An end effector section of the robot,
The correction unit is
Using the sensor information output by the force sensor, the correction value corresponding to the displacement of the end effector unit based on the force is obtained,
The controller is
A robot that controls the arm that cancels the displacement of the end effector unit based on the corrected target value. In claim 1 or 2,
A force control unit that outputs a second correction value of the target value of the position and the posture of the mechanical mechanism using the sensor information output by the force sensor;
The controller is
A robot that corrects the target value based on the correction value and the second correction value, and controls the robot based on the corrected target value. In claim 3,
The force control unit
When the mechanical mechanism converts an external force f and transmits the force F as the force F to the force sensor, an external force detection unit that performs reverse conversion of the conversion in the mechanical mechanism with respect to the sensor information;
The force control unit
A robot characterized in that the second correction value is obtained based on an output signal from the external force detector. In claim 4,
The force control unit
A robot characterized by performing a process of obtaining the inverse transformation and the second correction value by a digital filter process. In claim 5,
The external force detector is
The value of the sensor information at the nth (n is an integer of 3 or more) timing is _Fn , the intermediate parameter at the ( _{n-1) th} timing is _xn-1, and the coefficient parameters of the digital filter processing are α and β. If
x _n = αF _n + βx _n−1 to obtain the intermediate parameter x _n at the n-th timing,
The obtained intermediate parameter x _n, and the intermediate parameter x _n-2 obtained in the first n-1 of said intermediate parameter x _n-1 and the n-2 timing determined by the timing, the digital filter From the coefficient parameters C ₀ , C ₁ , C ₂ of the process,
The sensor information f _n after the inverse transformation at the n-th timing is obtained by f _n = {x _n − (C ₁ x _n−1 + C ₂ x _n−2 )} / C ₀ Robot to do. In any one of Claims 1 thru | or 6.
The correction unit is
A robot that performs a process of obtaining the correction value by a second digital filter process. In claim 7,
At the k-th timing (k is an integer of 2 or more), the value of the sensor information is F _k , the output value of the second digital filter processing representing the correction value is x _k, and the timing at the (k−1) -th timing. When the output value of the second digital filter processing is x _k−1 and the coefficient parameters of the second digital filter processing are α and β,
The correction unit is
Robot and obtaining the correction value _{x k} at a timing of the first k by _{x k = αF k + βx k} -1. In any one of Claims 1 thru | or 8.
A robot including a target value output unit that outputs the target value of the position and the posture of the mechanical mechanism. A robot control method for controlling a robot including an arm, a force sensor provided in the arm, and a mechanical mechanism provided in the arm via the force sensor and having an elastic member,
Using sensor information about the force detected by the force sensor, a correction value corresponding to the displacement of the elastic member based on the force is obtained,
A robot control method comprising: correcting the target value of the robot based on the correction value, and controlling the arm to cancel the displacement of the elastic member based on the corrected target value. A robot control method for controlling a robot including an arm, a force sensor provided in the arm, and a mechanical mechanism provided in the arm via the force sensor and having an elastic member,
A robot control method comprising: controlling the arm that cancels the displacement of the elastic member based on the force using sensor information relating to the force detected by the force sensor.

Images