JP6044511B2 - Robot control method and robot system - Google Patents

Description

  The present invention relates to an articulated robot control method and a robot system.

  In recent years, robots that hold and move an object with a hand have been developed. When such a robot performs work, the link or joint of the robot may come into contact with an external object. In such a case, it is necessary to perform a contact force relaxation operation to avoid the influence of external force (contact force) due to contact.

  Patent Document 1 discloses an impedance control device for a robot for performing a contact force relaxation operation. The impedance control device of this robot calculates a joint torque due to contact from the contact force, and calculates an escape amount in the contact force relaxation operation of the joint portion. Then, the escape amount is added to the joint angle for controlling the position and posture of the hand of the robot. As a result, the joint of the robot is driven by the designated escape amount, and the contact force relaxation operation is performed.

  FIG. 6 is a block diagram illustrating a configuration of the impedance control device for the robot disclosed in Patent Document 1. In FIG. In FIG. 6, 301 is an arm, 302 is a force sensor that measures external force acting on the arm tip, and 303 is provided with one or more to measure the magnitude and position of contact external force that acts on the arm 301 or the like. It is a contact sensor. Reference numeral 304 denotes a motor for driving the arm 301, reference numeral 305 denotes a rotation angle detector that detects the rotation angle of the motor 304, reference numeral 306 denotes a hand displacement calculator that converts hand action force information into hand position displacement of the arm 301, and reference numeral 307 denotes an arm action. A contact external force / joint torque conversion unit that converts contact force into joint disturbance torque, 308 is a joint displacement calculation unit that converts joint disturbance torque into joint angular displacement of the arm 301, and 309 sets the hand position of the arm 301 when there is no load A hand balance position setting unit 310, an adder 310, an inverse kinematic calculator 311 for calculating a joint angle from the hand position of the arm 301, an adder 312 and a servo controller 313 for controlling the motor 304 based on an angle command. is there.

  Next, the impedance control device for the robot shown in FIG. 6 will be described. First, the hand action force is measured by an external force measurement sensor (force sensor) 302 attached to the hand. The hand action force is input to the hand displacement calculation unit 306, and is set as mechanical impedance (virtual inertia, virtual viscosity, virtual rigidity) accompanying impedance control (or compliance control) for force control of the manipulator with contact with the environment. The hand tip virtual impedance value (Je, de, ke) is used to convert the hand tip displacement amount.

  This hand displacement is added by the adder 310 to the hand balance position of the hand balance position setting unit 309 (the hand balance position of the arm 301 when there is no load before contact), and a command value for the hand position is calculated. The calculated hand position command is converted into an angle command for each joint by the inverse kinematics calculation unit 311.

  The magnitude of the contact external force acting on the arm surface and the arm contact external force measured by the contact sensor 303 that measures the action position are converted into joint torque by the Jacobian matrix or the like in the contact external force / joint torque conversion unit 307, and the joint displacement Input to the calculation unit 308. The joint displacement calculation unit 308 calculates the displacement amount (outputs the displacement angle) of each joint from the set joint virtual impedance values (Jj, dj, kj).

  The displacement amount of each joint is added to the angle command of each joint from the inverse kinematics calculation unit 311 by the adder 312 and input to the servo controller 313. The servo controller 313 controls the motor 304 by comparing the difference between the input angle command and the angle information detected by the rotation angle detector 305. When the contact sensor 303 that measures the contact external force acting on the arm surface is a pressure sensor that can measure only the magnitude of the contact external force, the joint torque conversion unit 307 determines the magnitude of the contact external force and the detection position of the contact sensor 303. The joint disturbance torque is calculated from the representative point information.

  In this example, since the contact sensor 303 (high-sensitivity pressure sensor) attached to the arm surface is used as a means for measuring the torque applied to the joint, the influence of the inertial force of the arm in the case of measurement by the conventional torque sensor, The problem of inability to measure the force was improved. In addition to the contact sensor, it is also possible to measure the joint torque by increasing the detection accuracy by combining various sensors such as a torque sensor attached to the joint shaft and a pressure sensitive sensor. . However, when the joint axis torque sensor is used, it is necessary to correct the influence of the inertial force due to the movement of the arm itself as described above and the influence of the hand acting force on the joint part. Further, when measuring joint torque by combining various sensors, it is necessary to correct interference portions of various sensors.

  As described above, according to Patent Document 1, the information from each sensor is separated into the torque applied to each joint and the force applied to the hand, and by calculating the arm escape amount from the virtual impedance set for each, Appropriate avoidance action can be realized regardless of the part in contact with the environment.

  Patent Document 2 discloses a manipulator control method for performing contact force relaxation operation. In this method, the joint torque due to the contact force is estimated, and the escape amount in the contact force relaxation operation of the joint is calculated from the estimation result. Then, it is added to the joint angle for controlling the position / posture of the hand of the robot. As a result, the joint of the robot is driven by the designated escape amount, and the contact force relaxation operation is performed.

  Patent Document 3 discloses a flexible control device for a manipulator that controls movement during a contact force relaxation operation. This apparatus estimates torque due to external force (contact force), and reduces the integral gain of feedback compensation calculation when the estimated value is equal to or greater than a threshold value. Thereby, the operation | movement of the robot at the time of contact force relaxation operation | movement can be stabilized.

  Patent Document 4 discloses a robot control apparatus that can perform a flexible operation when a robot contacts an obstacle or the like.

JP 2001-38664 A JP 2007-136564 A JP 2007-105823 A JP 2008-73830 A

  However, the inventor has found that the above-described technique has the following problems. Generally, when a contact force is applied to an articulated robot, the joint force relaxation operation is performed by driving each joint of the articulated robot. On the other hand, when the articulated robot is working, it is important that the hand is in a desired position and posture. That is, if one or both of the position and posture of the hand are shifted due to the contact force relaxation operation, the subsequent work is hindered. Therefore, in an articulated robot, it is required to perform a contact force relaxation operation while maintaining the position and posture of the hand.

  On the other hand, in the techniques described in Patent Documents 1 to 4, it is not assumed that the position and posture of the hand are held when the robot contacts an external object or the like and performs the contact force relaxation operation. Therefore, when the contact force relaxation operation is performed, one or both of the position and posture of the hand are shifted.

A robot control method according to one aspect of the present invention is a robot including a movable part, and is a displacement of the movable part from information designating both or one of the position and posture of the robot by inverse kinematics calculation. 1 is a robot control method that calculates a value of 1 and includes a control that shifts in accordance with the external force when an external force that is different from the force output by the robot is detected. A specified value calculating step for calculating a specified value for specifying the displacement of the movable part from the first value, and a torque generated in the movable part by driving the movable part of the robot based on the specified value. Or a second value estimating step for measuring or estimating a second value that is a force, and a value corresponding to a torque or a force that should be generated in the movable part corresponding to the specified value by inverse dynamics calculation. A third value calculating step for calculating the value of the second value, and a fourth value for calculating a fourth value indicating the torque or force applied to the movable part by an external force by subtracting the third value from the second value. A target value calculating step of calculating a target value indicating a displacement speed of the movable part when performing a contact force relaxation operation according to the fourth value, and zero-space-converting the target value A correction target value calculating step for calculating a correction target value, a correction value calculating step for calculating a correction value from the correction target value, and a correction step for correcting the first value based on the correction value, The specified value calculating step calculates the specified value based on the first value and the correction value when the correction value has already been calculated.
Is.

  In the robot control method, the target value calculation step includes the movable for returning to the standard posture from a difference between a fifth value that is a displacement of the movable portion indicating a standard posture and the specified value. A sixth value, which is a displacement speed of the part, is calculated, and the sixth value is added to a value indicating the displacement speed of the movable part when performing the contact force relaxation operation calculated according to the fourth value. Then, it is desirable to calculate the target value.

In the above-described robot control method, the target value calculating step includes calculating the movable value based on a difference between a fifth value that is a displacement for keeping the displacement of the movable part within a movable range of the movable part and the specified value. Displacement of the movable part when calculating the sixth value, which is the displacement speed of the movable part for keeping the displacement of the part in the movable range, and performing the contact force relaxation operation calculated according to the fourth value It is desirable to calculate the target value by adding the sixth value to a value indicating speed.
It is desirable.

  In the robot control method, the movable unit is a rotary joint that performs a rotary motion, the first value is a rotation angle of the rotary motion of the rotary joint, and the second value is the A torque generated in the rotary joint, the third value is a torque to be generated in the rotary joint corresponding to the specified value, and the fourth value is a torque applied to the rotary joint by an external force, It is desirable that the displacement speed of the movable part is an angular speed of the rotational motion of the rotary joint.

  In the robot control method, the movable unit is a rotary joint that performs a rotary motion, and the first value and the fifth value are a rotation angle of the rotary motion of the rotary joint, The value of 2 is a torque generated in the rotary joint, the third value is a torque to be generated in the rotary joint corresponding to the specified value, and the fourth value is the torque generated by the external force. Preferably, the displacement speed of the movable part is an angular speed of the rotational motion of the rotary joint.

  In the robot control method, the movable portion is a linear joint that performs translational motion, the first value is a displacement amount of the translational motion of the linear motion joint, and the second value is , The force generated in the linear joint, the third value is a force generated in the linear joint corresponding to the specified value, and the fourth value is applied to the linear joint by an external force Preferably, the displacement speed of the movable part is a translational speed of the linear joint.

In the above robot control method, the movable part is a linear joint that performs translational motion,
The first value and the fifth value are displacement amounts of translational motion of the linear motion joint, the second value is a force generated in the linear motion joint, and the third value is The fourth value is a force applied to the linear motion joint by an external force corresponding to the specified value, and the displacement speed of the movable part is the translation of the linear motion joint The speed of movement is desirable.

  In the robot control method, the target value calculating step preferably calculates the target value using a mass damper model based on torque or force applied to the movable part by external force.

  In the robot control method described above, it is preferable that the second value is a value obtained by measuring a torque or a force of the movable part with a sensor.

  A robot system according to one aspect of the present invention includes a robot having a plurality of movable parts, and a robot control device that controls a contact force relaxation operation of the robot, and the robot control apparatus is a robot having a movable part. A method of controlling a robot that calculates a first value, which is a displacement of the movable part, from information designating the position and / or orientation of the robot by inverse kinematics calculation, A control method for a robot comprising a control for shifting in accordance with the external force when an external force that is a force different from the force being applied is detected, wherein a designated value for designating a displacement of the movable part is specified from the first value. A designated value calculating step to calculate, and driving or moving the movable part of the robot based on the designated value to measure or estimate a second value that is a torque or a force generated in the movable part A second value estimating step, and a third value calculating step of calculating a third value that is a value corresponding to a torque or a force to be generated in the movable part corresponding to the specified value by inverse dynamics calculation A fourth value calculating step of subtracting the third value from the second value to calculate a fourth value indicating a torque or force applied to the movable part by an external force; and the fourth value A target value calculating step for calculating a target value indicating a displacement speed of the movable part when performing a contact force relaxation operation, and a correction target value calculation for calculating a correction target value by performing zero-space conversion on the target value A correction value calculating step for calculating a correction value from the correction target value, and a correction step for correcting the first value based on the correction value, wherein the specified value calculating step includes: If already calculated, the first value And the specified value is calculated based on the correction value.

  In the above robot system, the robot control device is configured such that the displacement of the movable unit for returning to the standard posture from the difference between the fifth value that is the displacement of the movable unit indicating a standard posture and the specified value. A sixth value that is a speed is calculated, and the sixth value is added to a value that indicates the displacement speed of the movable part when performing the contact force relaxation operation calculated according to the fourth value, It is desirable to calculate the target value.

  In the robot system described above, the robot control device determines the displacement of the movable part from the difference between the fifth value that is a displacement for keeping the displacement of the movable part within the movable range of the movable part and the specified value. 6 is calculated as the displacement speed of the movable part for keeping the movable range within the movable range, and the displacement speed of the movable part when performing the contact force relaxation operation calculated according to the fourth value is shown. It is desirable to calculate the target value by adding the sixth value to the value.

  In the robot system, the movable unit is a rotary joint that performs a rotary motion, the first value is a rotation angle of the rotary motion of the rotary joint, and the second value is the rotary joint. The third value is a torque to be generated in the rotary joint corresponding to the specified value, and the fourth value is a torque applied to the rotary joint by an external force, and the movable The displacement speed of the part is preferably an angular speed of the rotational motion of the rotary joint.

  In the robot system, the movable unit is a rotary joint that performs a rotary motion, and the first value and the fifth value are a rotation angle of the rotary motion of the rotary joint, and the second value The value is a torque generated in the rotary joint, the third value is a torque to be generated in the rotary joint corresponding to the specified value, and the fourth value is applied to the rotary joint by an external force. Preferably, the displacement speed of the movable part is an angular speed of the rotational motion of the rotary joint.

  In the robot system, the movable portion is a linear joint that performs translational motion, the first value is a displacement amount of translational motion of the linear motion joint, and the second value is The third value is a force generated in the linear motion joint corresponding to the specified value, and the fourth value is a force applied to the linear motion joint by an external force. In addition, it is desirable that the displacement speed of the movable part is a translational speed of the linear motion joint.

  In the robot system, the movable portion is a linear motion joint that performs translational motion, and the first value and the fifth value are displacement amounts of translational motion of the linear motion joint, The value of 2 is a force generated at the linear motion joint, the third value is a force generated at the linear motion joint corresponding to the specified value, and the fourth value is the force generated by the external force. It is a force applied to the moving joint, and the displacement speed of the movable part is preferably the speed of the translational motion of the linear motion joint.

  In the robot system, the target value is preferably calculated using a mass damper model based on torque or force applied to the movable part by external force.

  In the robot system described above, the second value is preferably a value obtained by measuring a torque or force of the movable part with a sensor.

  According to the present invention, the contact force relaxation operation can be performed while maintaining the position and posture of the hand in the robot.

1 is a diagram schematically showing a configuration of a robot system 1000 according to a first exemplary embodiment. 1 is a block diagram schematically showing a configuration of a robot control apparatus 100 according to a first embodiment. FIG. 3 is a control block diagram illustrating a contact force relaxation operation of the robot according to the first exemplary embodiment. It is a block diagram which shows typically the structure of the robot control apparatus 200 concerning Embodiment 2. FIG. FIG. 6 is a control block diagram illustrating a contact force relaxation operation of the robot according to the second exemplary embodiment. It is a block diagram which shows the structure of the impedance control apparatus of the robot disclosed by patent document 1. FIG.

Embodiment 1
The contact force relaxation operation of the robot according to the first embodiment will be described. In this embodiment, when any part of the articulated robot comes into contact with another object and a contact force is applied to the contact part, the articulated robot maintains the position and posture of the hand and reduces the influence of the contact force. Performs contact force relaxation.

  The contact force relaxation operation of the robot according to the present embodiment is applied to a robot having redundancy. For example, the position and orientation of an object held by the hand of the robot has 6 degrees of freedom. Therefore, the contact force relaxation operation of the robot according to the present embodiment can be typically applied to a robot having seven or more axes. However, if the degree of freedom to be held is less than 6 among the positions and orientations of the object gripped by the hand of the robot, it does not exclude that there is a case where the robot can be applied to 6 or less axes.

  In the present embodiment, the contact force relaxation operation is performed while maintaining the position and posture of the object or the like held by the robot hand so that the position and posture of the robot hand are not shifted. The contact force relaxation operation described below is performed for each joint of the robot. Hereinafter, for example, in a seven-axis robot, a contact force relaxation operation at each joint and a configuration of a control device that controls the operation will be described with reference to the drawings.

  FIG. 1 is a diagram schematically illustrating a configuration of a robot system 1000 according to the first embodiment. The robot system 1000 includes a robot 101, a torque sensor 102, and a robot control device 100.

  The robot 101 is an articulated robot. FIG. 1 schematically illustrates a seven-joint robot as an example. The joints J1, J3, J5, and J7 are joints that rotate about a direction parallel to the paper surface of FIG. The joints J2, J4, and J6 are joints that rotate about a direction perpendicular to the paper surface of FIG. HD is an object gripping part.

  The torque sensor 102 measures the torque of each joint when the robot 101 is operating, and outputs a torque τ indicated by a vector. In the following description, vectors and matrices are shown in bold.

  The robot control apparatus 100 controls the operation of the robot 101 by giving a control signal CON. In addition, when the robot 101 comes into contact with another object and a contact force is applied to the contact portion, the robot control apparatus 100 performs a contact force relaxation operation that reduces the influence of the contact force while maintaining the position and posture of the hand. Let the robot 101 perform it. However, the contact force relaxation operation according to the present embodiment is not necessarily limited to controlling both the position and posture of the hand. That is, the contact force relaxation operation according to the present embodiment includes an operation of holding only one of the hand position and posture. Further, the contact force relaxation operation according to the present embodiment includes an operation of holding only one component or two or more components among the coordinate direction components indicating the position and posture of the hand. When performing an operation of holding two or more components, the combination of components to be held includes a combination of components indicating the position of the hand, a combination of components indicating the position of the hand, and a component indicating the position of the hand and the posture of the hand. Any combination with the components shown may be used.

FIG. 2 is a block diagram schematically illustrating a configuration of the robot control apparatus 100 according to the first embodiment. The robot control apparatus 100 includes a joint angle calculation unit 1, a joint angle control unit 2, a torque calculation unit 4, an angular velocity calculation unit 5, a null space conversion unit 6, and a gain application unit 7. 2 is assumed to be included in the robot 101. The robot control apparatus 100 outputs a drive signal Sdr to the joint drive unit 3 that drives each joint of the robot based on the planned hand position X _plan of the robot and the actually measured torque T _act . Thereby, the robot control apparatus 100 controls the contact force relaxation operation of each joint of the robot 101. The measured torque T _act is a value obtained by actually measuring the torque of each joint by the joint torque sensor 102.

  FIG. 3 is a control block diagram illustrating the contact force relaxation operation of the robot according to the first embodiment. The contact force alleviating operation in the present embodiment includes a joint angle calculation step S1, an addition step S2, a joint angle control step S3, a joint drive step S4, a torque calculation step S5, a subtraction step S6, an angular velocity calculation step S7, and a null space conversion step. It consists of S8 and gain provision step S9.

  Hereinafter, the configuration of the robot control device 100 and the contact force relaxation operation will be described with reference to FIGS. 2 and 3.

式（１）において、なお、手先座標ベクトルＰ及び関節角度ベクトルｑの上に付した黒点は、１階の時間微分を示す。Ｊはヤコビアン行列である。以下、式（１）の成立を前提として説明する。 In general, the following equation (1) is established between the hand coordinate vector P of the robot 101 and the joint angle vector q.

In equation (1), a black dot on the hand coordinate vector P and the joint angle vector q indicates the first-order time differentiation. J is a Jacobian matrix. Hereinafter, description will be made on the premise that the expression (1) is established.

式（２）において、ｑ_ｋはロボット１０１のｋ回目の繰り返し演算後の関節の角度、Ｊ^＋はヤコビアン行列の疑似逆行列、ｘ_ｋ−１はｋ回目の繰り返し演算後の番目の関節の座標を示す。繰り返し演算で得られた結果は、関節角度ｑ_ｐｌａｎとして出力される。なお、関節角度ｑ_ｐｌａｎを、第１の値とも称する。ｋは、繰り返し演算の回数を示すパラメータである。ロボット１０１の関節の角度ｑ_ｐｌａｎは、ロボット１０１に接触力が作用しないときに、ロボット１０１の手先を計画手先位置Ｘ_ｐｌａｎで指定される位置及び姿勢に保持するために、関節に設定される関節角度を示している。 Joint angle calculation step S1
First, the joint angle calculation unit 1 receives the planned hand position X _plan of the robot 101. The planned hand position X _plan of the robot 101 is information indicating the position and posture of the hand determined by the trajectory plan for robot motion control, and is expressed in a format such as 3D orthogonal coordinates or 3D polar coordinates, for example. The The joint angle calculation unit 1 uses the planned hand position X _plan to calculate the rotational motion angle (joint angle) q _plan of the joint of the robot 101 by inverse kinematic calculation that performs numerical repetitive calculation such as Newton's method. calculate. The numerical repetitive calculation can be performed using the following formula (2).

In Equation (2), q _k is the angle of the joint after the k-th iteration operation of the robot 101, J ⁺ is a pseudo inverse matrix of the Jacobian matrix, and x _k−1 is the coordinate of the first joint after the k-th iteration operation. Indicates. The result obtained by the repetitive calculation is output as the joint angle q _plan . The joint angle q _plan is also referred to as a first value. k is a parameter indicating the number of repeated operations. The joint angle q _plan of the robot 101 is a joint set in the joint to hold the hand of the robot 101 at the position and posture specified by the planned hand position X _plan when no contact force acts on the robot 101. Shows the angle.

Addition step S2
The joint angle control unit 2 adds the joint angle q _plan of the robot 101 and the correction angle Δq _null from the gain applying unit 7 to calculate the designated angle q _ref . The correction angle Δq _null is also referred to as a correction value. The designated angle q _ref is also referred to as a designated value.

Joint angle control step S3
The joint angle control unit 2 generates a drive signal Sdr based on the specified angle q _ref .

Joint drive step S4
The joint drive unit 3 drives the joint based on the drive signal Sdr. At the same time, the joint drive unit 3 measures the actual torque T _act generated at the node by the torque sensor, and outputs the measurement result. The actually measured torque T _act is also referred to as a second value.

Torque calculation step S5
The torque calculation unit 4 adds the joint angle q _plan of the robot 101 and the correction angle Δq _null from the gain applying unit 7 to calculate the specified angle q _ref . Torque calculator 4, the inverse dynamics operation using the specified angle q _ref, calculate the torque T _cal required to drive the joint by the specified angle q _ref, and outputs the calculated result. The torque T _cal is also referred to as a third value.

Subtraction step S6
The angular velocity calculation unit 5 subtracts the torque Tcal from the actually measured torque T _act to calculate the torque τ due to the external force. The torque τ is also referred to as a fourth value.

Angular velocity calculation step S7
The angular velocity calculation unit 5 calculates a target angular velocity ω _avoid for avoiding the influence of the external force applied to the joint by performing calculation using a mass damper model using the torque τ due to the external force. Note that the target angular velocity ω _avoid is also referred to as a target value.

Zero space conversion step S8
The null space conversion unit 6 converts the vector space in which the target angular velocity ω _{avoid of} each joint is included so that the vector space formed by the corrected angular velocity ω _null of each joint after conversion becomes a null space. Thereby, even if the correction angular velocity of each joint of the robot 101 is specified, the position and posture of the hand of the robot 101 obtained as an output can be held. The correction angular velocity ω _null is also referred to as a correction target value.

Gain grant step S9
The gain applying unit 7 multiplies the correction angular velocity ω _null by the gain Δt to calculate a correction angle Δq _null for avoiding the influence of an external force applied to the joint. As described above, the corrected angular velocity ω _null of each joint constitutes a null space. By adding the correction angle Δq _{null of} each joint, the joint angle control unit 2 can generate the drive signal Sdr so that the position and posture of the hand of the robot 101 obtained as an output can be held.

  In general, when controlling the posture of a robot using only an angle, only the angle of the movement destination is specified, so it is not possible to specify other than the coordinates of the movement destination. In this case, for example, it is difficult to realize a flexible robot operation by finely controlling the moving speed to the destination. In addition, when a part of the robot comes into contact with an external object at the movement destination and pushes the contact point with a constant force, it is difficult to control the pressing force because only the coordinates can be specified in the posture control based on the coordinates. Also, when tasks with different movements are overlapped, it is difficult to control the movements of the overlapped tasks simply by giving an angle.

  On the other hand, in this Embodiment, the contact force relaxation operation | movement which relieve | moderates the influence by external force using angular velocity is performed. Therefore, the avoidance operation of the robot can be performed at an avoidance speed corresponding to the magnitude of the external force. As a result, it is possible to prevent a situation in which the operation is unnecessary and the avoidance speed is insufficient. In addition, when a part of the robot 101 comes into contact with an external object at the movement destination and the contact point is pushed in with a constant force, the pressing force can be easily controlled by designating the angular velocity at the movement destination. Also, when tasks with different movements are overlaid, the movements of the overlaid tasks can be controlled by using each speed. Therefore, it is possible to realize a more complex and flexible robot movement than the control using the angle.

  As described above, according to the present embodiment, it is possible to perform the contact force relaxation operation while controlling the motion state of the robot while maintaining the hand position and posture. This makes it possible to perform more accurate and flexible posture control of the robot.

Embodiment 2
The contact force relaxation operation of the robot according to the second embodiment will be described. FIG. 4 is a block diagram schematically illustrating a configuration of the robot control apparatus 200 according to the second embodiment. The robot control device 200 has a configuration in which a return angular velocity calculation unit 8 is added to the robot control device 100 according to the first embodiment, and the angular velocity calculation unit 5 of the robot control device 100 is replaced with an angular velocity calculation unit 9. The other configuration of the robot control device 200 is the same as that of the robot control device 100, and thus description thereof is omitted. When performing the contact force relaxation operation, the robot control device 200 can further perform a return operation to a predetermined posture so that the robot 101 does not become an unstable posture, for example.

  FIG. 5 is a control block diagram illustrating the contact force relaxation operation of the robot according to the second embodiment. The contact force relaxation operation in the present embodiment is obtained by deleting the angular velocity calculation step S7 of the contact force relaxation operation of the robot according to the first embodiment in FIG. 3 and adding steps S21 to S23. Steps S1 to S6, S8, and S9 in FIG. 5 are the same as those in FIG.

Return angular velocity calculation step S21
The return angular velocity calculation unit 8 receives the joint angle _qinit indicating the standard posture and the specified angle _qref . The return angular velocity calculation unit 8 calculates a return angular velocity ω _init necessary for returning to the standard posture from the difference between the joint angle q _init indicating the standard posture and the specified angle q _ref . The joint angle q _init indicating the standard posture is also referred to as a fifth value. The return angular velocity ω _init is also referred to as a sixth value.

Angular velocity calculation step S22
The angular velocity calculation unit 9 subtracts the torque T _cal from the measured torque T _act to calculate the torque τ due to the external force. The angular velocity calculation unit 5 performs an operation by a mass damper model using the torque τ due to the external force, and calculates an angular velocity ω _avoid1 for avoiding the influence of the external force applied to the joint. Here, the angular velocity ω _avoid1 corresponds to the target angular velocity ω _avoid in the first embodiment.

Angular velocity calculation step S23
The angular velocity calculation unit 9 adds the angular velocity ω _avoid1 and the return angular velocity ω _init and outputs the target angular velocity ω _avoid .

From the above, the target angular velocity ω _avoid input to the null space conversion unit 6 in the present embodiment reflects not only the angular velocity ω _avoid1 for avoiding the influence of the external force applied to the joint, but also the return angular velocity ω _init . . As a result, even if the contact force relaxation operation is performed, the robot 101 can be returned to a predetermined position and posture.

Other Embodiments The present invention is not limited to the above-described embodiments, and can be appropriately changed without departing from the spirit of the present invention. For example, for example, it is needless to say that the contact force relaxation operation according to the above-described embodiment can be applied to a number of articulated robots other than seven axes as long as it has redundancy.

  The robot control apparatus 100 can also be realized by hardware resources such as a computer. In this case, the program can be similarly performed by causing the robot control device 100 to execute a program that defines the procedure of the contact force relaxation operation according to the first embodiment.

In the second embodiment described above, the return to the standard posture has been described, but this is only an example. For example, the movable range of the joint may be considered instead of the standard posture. In this case, it can be realized by replacing the joint angle q _{init of the} second embodiment with a joint angle indicating the movable range of the joint. As a result, each joint constituting the robot can be moved so as to be within the movable range.

  In the above-described embodiment, only a rotary joint is assumed as a joint, and attention is paid to the rotation angle, angular velocity, and torque of the joint angle, but this is merely an example. An articulated robot can also have a configuration having a linear joint capable of translational movement (extension and contraction). In this case, in the motion control of the linear motion joint, the displacement amount of the translational motion of the linear motion joint instead of the joint angle of the rotary joint, the displacement rate of the translational motion of the linear motion joint instead of the angular velocity of the rotational joint, By paying attention to the force applied to the translational motion of the linear motion joint instead of the torque, the same contact force relaxation operation can be realized using the above method.

  As described above, it is possible to perform the contact force relaxation operation of an articulated robot having a rotary joint and a linear motion joint. Hereinafter, the rotary joint and the linear motion joint are referred to as a movable part, and the joint angle of the rotary joint and the displacement of the translational motion of the linear motion joint are referred to as the displacement of the movable part. The angular velocity of the rotary joint and the translational velocity of the linear motion joint are referred to as the displacement speed of the movable part.

  That is, the first value and the fifth value described above include the rotation angle of the rotary joint and the amount of translational displacement of the linear motion joint. The second value described above includes the torque generated in the rotary joint and the force generated in the linear motion joint. The third value described above includes the torque that should be generated in the rotary joint corresponding to the specified value, and the force generated in the linear motion joint corresponding to the specified value. The fourth value described above includes a torque applied to the rotary joint by an external force and a force applied to the linear motion joint by an external force. The sixth value described above includes the angular velocity of the rotary joint for returning to the standard posture and the displacement velocity of the translational motion of the linear motion joint for returning to the standard posture. Further, the sixth value described above includes the angular velocity of the rotary joint for keeping the rotation angle of the rotary joint within the movable range and the translational motion of the linear joint for keeping the translational displacement of the linear joint within the movable range. Displacement speed. The above-mentioned specified value includes the specified angle of the rotary joint and the specified displacement of the linear motion joint. The correction value described above includes the correction angle of the rotary joint and the correction displacement of the linear motion joint. The target value described above includes the target angular velocity of the rotary joint and the target displacement velocity of the translational motion of the linear motion joint. The correction target value described above includes the correction angular velocity of the rotary joint and the correction displacement velocity of the translational motion of the linear motion joint.

  In the above-described embodiment, the example in which the torque is calculated by the torque sensor has been described, but this is merely an example. For example, instead of measuring the torque, the torque of the joint may be estimated from another physical quantity indicating the motion of the joint (for example, the current value of the motor that drives the joint) measured during the operation of the joint.

DESCRIPTION OF SYMBOLS 1 Joint angle calculation part 2 Joint angle control part 3 Joint drive part 4 Torque calculation part 5 Angular speed calculation part 6 Zero space conversion part 7 Gain provision part 8 Return angular speed calculation part 9 Angular speed calculation part 100, 200 Robot control apparatus 101 Robot 102 Torque Sensor 200 Robot controller 301 Arm 302 Force sensor 303 Contact sensor 304 Motor 305 Rotation angle detector 306 Hand displacement calculator 307 Joint torque converter 308 Joint displacement calculator 309 Position setting unit 310 Adder 311 Inverse kinematic calculator 312 Addition Device 313 Servo Controller 1000 Robot System

Claims (18)

A robot having a movable part, and a robot control method for calculating a first value that is a displacement of the movable part from information specifying both or one of the position and posture of the robot by inverse kinematics calculation, A control method for a robot comprising a control to change in accordance with the external force when an external force that is different from the force output by the robot is detected,
A designated value calculating step for calculating a designated value for designating a displacement of the movable part from the first value;
A second value estimating step of measuring or estimating a second value that is a torque or a force generated in the movable part by driving the movable part of the robot based on the specified value;
A third value calculating step of calculating a third value that is a value corresponding to a torque or a force to be generated in the movable part corresponding to the specified value by inverse dynamics calculation;
A fourth value calculating step of subtracting the third value from the second value to calculate a fourth value indicating a torque or force applied to the movable part by an external force;
A target value calculating step of calculating a target value indicating a displacement speed of the movable part when performing a contact force relaxation operation according to the fourth value;
A correction target value calculating step of calculating a correction target value by performing zero-space conversion on the target value;
A correction value calculating step of calculating a correction value from the correction target value;
A correction step of correcting the first value based on the correction value,
The specified value calculating step calculates the specified value based on the first value and the correction value when the correction value has already been calculated.
Robot control method. The target value calculating step includes:
A sixth value that is a displacement speed of the movable portion for returning to the standard posture is calculated from a difference between a fifth value that is a displacement of the movable portion that indicates a standard posture and the specified value,
Adding the sixth value to a value indicating the displacement speed of the movable part when performing the contact force relaxation operation calculated according to the fourth value to calculate the target value;
The robot control method according to claim 1. The target value calculating step includes:
The displacement of the movable part for keeping the displacement of the movable part in the movable range from the difference between the fifth value, which is the displacement for placing the displacement of the movable part in the movable range of the movable part, and the specified value. Calculate the sixth value that is the speed,
Adding the sixth value to a value indicating the displacement speed of the movable part when performing the contact force relaxation operation calculated according to the fourth value to calculate the target value;
The robot control method according to claim 1. The movable part is a rotary joint that performs a rotary motion,
The first value is a rotation angle of the rotational movement of the rotary joint,
The second value is a torque generated in the rotary joint,
The third value is a torque to be generated in the rotary joint corresponding to the specified value,
The fourth value is a torque applied to the rotary joint by an external force,
The displacement speed of the movable part is an angular speed of the rotational motion of the rotary joint.
The robot control method according to any one of claims 1 to 3. The movable part is a rotary joint that performs a rotary motion,
The first value and the fifth value are rotation angles of the rotational motion of the rotary joint,
The second value is a torque generated in the rotary joint,
The third value is a torque to be generated in the rotary joint corresponding to the specified value,
The fourth value is a torque applied to the rotary joint by an external force,
The displacement speed of the movable part is an angular speed of the rotational motion of the rotary joint.
The robot control method according to claim 2 or 3. The movable part is a linear motion joint that performs translational motion,
The first value is a translational displacement amount of the linear joint;
The second value is a force generated in the linear joint,
The third value is a force generated in the linear joint corresponding to the specified value,
The fourth value is a force applied to the linear joint by an external force,
The displacement speed of the movable part is the speed of translational motion of the linear motion joint.
The robot control method according to any one of claims 1 to 3. The movable part is a linear motion joint that performs translational motion,
The first value and the fifth value are displacement amounts of translational motion of the linear motion joint,
The second value is a force generated in the linear joint,
The third value is a force generated in the linear joint corresponding to the specified value,
The fourth value is a force applied to the linear joint by an external force,
The displacement speed of the movable part is the speed of translational motion of the linear motion joint.
The robot control method according to claim 2 or 3. The target value calculating step calculates the target value using a mass damper model based on torque or force applied to the movable part by external force.
The robot control method according to claim 1. The second value is a value obtained by measuring the torque or force of the movable part with a sensor.
The robot control method according to claim 1. A robot having a plurality of movable parts;
A robot controller for controlling a contact force relaxation operation of the robot having the plurality of movable parts,
The robot controller is
A robot having a movable part, and a robot control method for calculating a first value that is a displacement of the movable part from information specifying both or one of the position and posture of the robot by inverse kinematics calculation, A control method for a robot comprising a control to change in accordance with the external force when an external force that is different from the force output by the robot is detected,
A designated value calculating step for calculating a designated value for designating a displacement of the movable part from the first value;
A second value estimating step of measuring or estimating a second value that is a torque or a force generated in the movable part by driving the movable part of the robot based on the specified value;
A third value calculating step of calculating a third value that is a value corresponding to a torque or a force to be generated in the movable part corresponding to the specified value by inverse dynamics calculation;
A fourth value calculating step of subtracting the third value from the second value to calculate a fourth value indicating a torque or force applied to the movable part by an external force;
A target value calculating step of calculating a target value indicating a displacement speed of the movable part when performing a contact force relaxation operation according to the fourth value;
A correction target value calculating step of calculating a correction target value by performing zero-space conversion on the target value;
A correction value calculating step of calculating a correction value from the correction target value;
A correction step of correcting the first value based on the correction value,
The specified value calculating step calculates the specified value based on the first value and the correction value when the correction value has already been calculated.
Robot system. The robot controller is
A sixth value that is a displacement speed of the movable portion for returning to the standard posture is calculated from a difference between a fifth value that is a displacement of the movable portion that indicates a standard posture and the specified value,
Adding the sixth value to a value indicating the displacement speed of the movable part when performing the contact force relaxation operation calculated according to the fourth value to calculate the target value;
The robot system according to claim 10. The robot controller is
The displacement of the movable part for keeping the displacement of the movable part in the movable range from the difference between the fifth value, which is the displacement for placing the displacement of the movable part in the movable range of the movable part, and the specified value. Calculate the sixth value that is the speed,
Adding the sixth value to a value indicating the displacement speed of the movable part when performing the contact force relaxation operation calculated according to the fourth value to calculate the target value;
The robot system according to claim 10. The movable part is a rotary joint that performs a rotary motion,
The first value is a rotation angle of the rotational movement of the rotary joint,
The second value is a torque generated in the rotary joint,
The third value is a torque to be generated in the rotary joint corresponding to the specified value,
The fourth value is a torque applied to the rotary joint by an external force,
The displacement speed of the movable part is an angular speed of the rotational motion of the rotary joint.
The robot system according to any one of claims 10 to 12. The movable part is a rotary joint that performs a rotary motion,
The first value and the fifth value are rotation angles of the rotational motion of the rotary joint,
The second value is a torque generated in the rotary joint,
The third value is a torque to be generated in the rotary joint corresponding to the specified value,
The fourth value is a torque applied to the rotary joint by an external force,
The displacement speed of the movable part is an angular speed of the rotational motion of the rotary joint.
The robot system according to claim 11 or 12. The movable part is a linear motion joint that performs translational motion,
The first value is a translational displacement amount of the linear joint;
The second value is a force generated in the linear joint,
The third value is a force generated in the linear joint corresponding to the specified value,
The fourth value is a force applied to the linear joint by an external force,
The displacement speed of the movable part is the speed of translational motion of the linear motion joint.
The robot system according to any one of claims 10 to 12. The movable part is a linear motion joint that performs translational motion,
The first value and the fifth value are displacement amounts of translational motion of the linear motion joint,
The second value is a force generated in the linear joint,
The third value is a force generated in the linear joint corresponding to the specified value,
The fourth value is a force applied to the linear joint by an external force,
The displacement speed of the movable part is the speed of translational motion of the linear motion joint.
The robot system according to claim 11 or 12. The target value is calculated using a mass damper model based on torque or force applied to the movable part by external force.
The robot system according to any one of claims 10 to 16. The second value is a value obtained by measuring the torque or force of the movable part with a sensor.
The robot system according to any one of claims 10 to 17.

Images