JP2012223851A - Robot control device, and control method and control program for the same - Google Patents

Abstract

PROBLEM TO BE SOLVED: To optimally control even when failure arises in a sensor of a robot.SOLUTION: A robot control device includes: an inertia moment calculation means that calculates the inertia moment around the i joint from the i joint to the tip part based on a function that contains a joint angle from the first joint on the tip side to the i joint (i≥2); an inertia moment estimation means that calculates an inertia moment estimated value around the i joint when a rotary shaft of the i joint is a vertical direction, based on a function that contains the joint angle of the i joint; an estimation error calculation means that calculates the difference between the inertia moment around the i joint and the inertia moment estimated value around the i joint as an inertia moment error around the i joint; a failure determination means that determines the failure of a joint angle detection means by comparing the absolute value of the inertia moment error around the i joint with a determination threshold; and a joint angle instruction generation means that generates a joint angle instruction based on the determination result of failure and a work pattern of the robot.

Description

  The present invention relates to a robot control apparatus, a control method thereof, and a control program that can perform optimal control even when a failure occurs in a sensor of the robot.

  In recent years, research and development of an automatic control system configured to perform automatic control of a vehicle, an aircraft, or the like using a plurality of sensors and to continue the control even when a failure occurs in the sensor or the like has been performed. For example, an automatic control system is known that uses a feedback linearization and state estimator to identify a failed component of an aircraft and continues operation without using the failed component (see Patent Document 1). ).

Japanese National Patent Publication No. 11-507454

  When the automatic control system shown in Patent Document 1 is applied to a robot, for example, when a sensor provided in the robot fails and a normal sensor is not known, both feedback linearization and state estimation cannot be performed. There is a fear. The automatic control system includes a large number of sensors in a redundant manner to control the aircraft. Therefore, when the sensor configuration is applied to the robot, there is a problem that the mass and the size increase.

  The present invention has been made to solve such problems, and provides a robot control device capable of optimal control even when a failure occurs in a robot sensor, a control method therefor, and a control program. The main purpose is to do.

  In order to achieve the above object, one aspect of the present invention provides a plurality of links, a plurality of joints that rotatably connect the links, a plurality of drive units that drive the joints, and the joint angles. A plurality of joint angle detection means for detecting the joint angle from the first joint on the distal end side to the i-th joint (i ≧ 2) detected by each joint angle detection means; A function including an inertia moment calculating means for calculating an inertia moment around the i-th joint from the i-th joint to the tip portion based on a function including the joint angle of the i-th joint detected by the joint angle detecting means. The inertia moment estimation means for calculating the estimated moment of inertia around the i-th joint when the rotation axis of the i-th joint is in the vertical direction, and the inertia moment calculation means An estimation error calculation means for calculating a difference between an inertia moment around the i-th joint and an estimated inertia moment value around the i-th joint calculated by the inertia moment estimation means as an inertia moment error around the i-th joint. A failure determination unit that determines a failure of the joint angle detection unit by comparing an absolute value of an inertia moment error around the i-th joint calculated by the estimation error calculation unit and a determination threshold; And a joint angle command generating means for generating a joint angle command for each of the joints based on a failure determination result determined by the means and a work pattern to be performed by the robot. It is a robot control device. According to this aspect, even when a failure occurs in the sensor of the robot, optimal control can be performed.

  In this aspect, when the absolute value of the moment of inertia error around the i-th joint calculated by the estimation error calculation unit exceeds a determination threshold, the failure determination unit determines the joint angle of the plurality of joint angle detection units. One of the joint angles may be held at 0 degree, and the comparison between the absolute value of the moment of inertia error around the i-th joint and the determination threshold may be repeated to determine the failure of the joint angle detection means. As a result, it is possible to instantaneously determine whether or not each joint angle detection unit is out of order at the moment when the rotation axis of each joint faces the vertical direction without redundantly providing the joint angle detection unit.

  In this aspect, the failure determination means holds one joint angle of the plurality of joint angles at 0 degree, and the absolute value of the inertia moment error around the i-th joint is equal to or less than a determination threshold. The joint angle detection means that holds the joint angle at 0 degree is determined to be faulty, and the other joint angle detection means are determined to be normal, and the absolute value of the inertia moment error around the i-th joint exceeds the determination threshold value. At this time, it is determined that the joint angle detection means that holds the joint angle at 0 degrees is unknown, and the other joint angle detection means are failed, and the joint angle corresponding to the joint angle detection means determined to be the failure is 0. And the comparison between the absolute value of the moment of inertia error around the i-th joint and the determination threshold value may be repeated. Thereby, even when it is unclear whether or not each joint angle detection means is out of order, the failure can be determined instantaneously.

  In this aspect, the joint angle command generation means determines whether or not the work pattern can be performed based on the failure determination result determined by the failure determination means and the work pattern that the robot is to perform. If it is determined that it is impossible to execute, a joint angle command for canceling the work of the work pattern is generated. A command may be generated. As a result, even if there is a broken joint angle detection means, it is possible to appropriately determine whether or not to perform the operation and continue the work.

  In this aspect, when it is determined that there is no failed joint angle detection unit based on the determination result from the failure determination unit, the joint angle of the joint angle detection unit in all joints is determined by the joint angle command generation unit. A torque command for the drive unit that follows the generated joint angle command is generated, and when it is determined that the failed joint angle detection unit exists based on a failure determination result from the failure determination unit, a failure occurs. A torque command for the drive unit is generated so that the joint angle of the joint angle detection unit that is not following the joint angle command generated by the joint angle command generation unit, and the failed joint angle detection unit is provided. Torque command generating means for generating a preset torque command for each of the work contents for the given joint driving means is further provided. It may be. Thereby, even when there is a broken joint angle detection means, a torque command can be generated appropriately.

  In this embodiment, based on user input information or a preset program, work pattern generation means for generating a work pattern for the robot to perform work, and the work patterns generated by the work pattern generation means are classified. A parameter generation means for setting a value corresponding to the preset classification as a work classification parameter, each joint angle command from the joint angle command generation means, the work classification parameter from the parameter generation means, Torque command generation means for generating a torque command for the drive means based on the failure determination result from the failure determination means may further be provided.

  In this aspect, the torque command generation means determines that the joint angle detector has failed based on the determination result from the failure determination means, and the work classification parameter from the parameter generation means When the first predetermined value is determined to be set, a torque command for generating a predetermined torque is generated. (B) Based on the determination result from the failure determination means, the joint angle detector has failed. And when it is determined that the second predetermined value is set in the work classification parameter from the parameter generation means, 0 is set in the torque command, and (c) based on the determination result from the failure determination means Then, when it is determined that the joint angle detector is not broken or unknown, a torque command set in advance for each work pattern may be generated. Thereby, when the work is continued safely without using the broken joint angle detecting means, the work method can be changed or stopped according to the type of the work.

  In this aspect, the parameter generation means generates a reaction force toward the outside so that the work to be performed by the robot extends the finger of the robot hand based on the work pattern generated by the work pattern generation means. A first predetermined value is set as the work classification parameter, and a second predetermined value is determined when it is determined that the work to be performed by the robot does not generate a reaction force toward the outside of the finger of the robot hand. May be set as the work classification parameter.

  In this aspect, the robot includes a finger mechanism having the plurality of links and a plurality of joints that rotatably connect the links, a plurality of motors that drive the joints, and the joint angles. The robot hand may include a plurality of joint angle detection units that detect.

In this aspect, the inertia moment calculating means calculates the inertia moment J _i around the i-th joint using the following equation (21) as the function, and the inertia moment estimating means is the following (27 ) Is used to calculate the estimated moment of inertia J _i (hat) around the i-th joint, and the estimation error calculating means uses the following equation (29) to calculate the inertia moment error e _J around the i-th joint. _{, I} may be calculated.

  On the other hand, according to one aspect of the present invention for achieving the above object, a plurality of links, a plurality of joints rotatably connecting the links, a plurality of driving means for driving the joints, and the joints A control method of a robot control device comprising a plurality of joint angle detection means for detecting an angle, including a joint angle from the detected first joint on the distal end side to the i-th joint (i ≧ 2) A step of calculating a moment of inertia around the i-th joint from the i-th joint to the tip portion based on the function; and a function including the detected joint angle of the i-th joint. A step of calculating an estimated moment of inertia around the i-th joint when the rotation axis is in a vertical direction; the calculated moment of inertia around the i-th joint; and the calculated moment of inertia around the i-th joint. Value of Is calculated as an inertia moment error around the i-th joint, and the absolute value of the calculated inertia moment error around the i-th joint is compared with a determination threshold value to determine a failure of the joint angle detection means. And a step of generating a joint angle command for each of the joints based on a determination result of the determined failure and a work pattern to be performed by the robot. An apparatus control method may be used.

  Another aspect of the present invention for achieving the above object is to provide a plurality of links, a plurality of joints for rotatably connecting the links, a plurality of driving means for driving the joints, and the joints. A control program of a robot control device comprising a plurality of joint angle detection means for detecting an angle, including a joint angle from the detected first joint on the distal end side to the i-th joint (i ≧ 2) Based on the function, a process of calculating the moment of inertia around the i-th joint from the i-th joint to the tip portion, and on the basis of the function including the detected joint angle of the i-th joint, A process of calculating an estimated moment of inertia around the i-th joint when the rotation axis is in the vertical direction, the calculated inertia moment around the i-th joint, and the estimated inertia moment around the i-th joint The difference between the value The process of calculating the moment of inertia around the i-th joint and the process of determining a failure of the joint angle detecting means by comparing the calculated absolute value of the moment of inertia error around the i-th joint with a determination threshold. And causing the computer to execute a process of generating a joint angle command for each of the joints based on the determined failure determination result and the work pattern that the robot is to perform. It may be a control program for the robot control device.

  According to the present invention, it is possible to provide a robot control device, a control method, and a control program that can optimally perform control even when a failure occurs in a sensor of the robot.

1 is a block diagram showing a schematic system configuration of a robot control apparatus according to an embodiment of the present invention. It is a flowchart which shows the processing flow for determining the failure of the joint angle detector in i = 2 by the failure determination part of the robot controller which concerns on embodiment of this invention. It is a flowchart which shows the processing flow for determining the failure of a joint angle detector when it is known whether any of the 1st joint angle detector and the 2nd joint angle detector is out of order. (A) It is a schematic diagram which shows the finger mechanism of the robot hand which concerns on embodiment of this invention. (B) It is a schematic diagram which shows the finger mechanism of the robot hand which concerns on embodiment of this invention. (A) It is a figure which shows the time change of the joint angle of the 2nd joint in the simulation of the robot control apparatus which concerns on embodiment of this invention. (B) It is a figure which shows the time change of the joint angle of the 2nd joint in the simulation of the conventional robot control apparatus. (A) It is a figure which shows the time change to 0.015 [s] of the joint angle of the 1st joint in the simulation of the robot control apparatus which concerns on embodiment of this invention. (B) It is a figure which shows the time change to 0.5 [s] of the joint angle of the 1st joint in the simulation of the robot controller which concerns on embodiment of this invention. (A) It is a figure which shows the time change to 0.015 [s] of the joint angle of the 3rd joint in the simulation of the robot control apparatus which concerns on embodiment of this invention. (B) It is a figure which shows the time change to 0.5 [s] of the joint angle of the 3rd joint in the simulation of the robot control apparatus which concerns on embodiment of this invention. It is a figure which shows the time change of the 2nd joint surrounding moment of inertia estimation error in the simulation of the robot control apparatus which concerns on embodiment of this invention. Is a diagram illustrating a second time changes of the joints of the motor torque command T ₂ of the order to control the second joint failed joint angle detector is provided.

  Embodiments of the present invention will be described below with reference to the drawings. The robot control apparatus 1 according to an embodiment of the present invention controls a finger mechanism 131 of a robot hand 130 as shown in FIG. 4, for example.

  As shown in FIGS. 4A and 4B, the finger mechanism 131 of the robot hand 130 includes a first link 401, a first joint 402 connected to the first link 401,. 2 joints 403, i-1 link 404 connected to i-2 joint 403, i-1 joint 405 connected to i-1 link 404, and i-1 joint 405 The i-th link 406, the i-th joint 407 connected to the i-th link 406, the i-3 link 408, the i-3 joint 409 connected to the i-3 link 408, and the i-th link. An i-th link 410 connected to the three joints 409. Hereinafter, the i-th joint counted from the first joint closest to the tip of the finger mechanism 131 is referred to as the i-th joint, and similarly, the i-th link counted from the first link closest to the tip of the finger mechanism 131 is referred to as the i-th joint. This is the i-th link.

  The first joint 402,..., The i-2 joint 403, the i-1 joint 405, the i joint 407, and the i-3 joint 409 are specific examples of driving means, A motor 132 for driving each joint is provided, and each joint is rotationally driven by a motor torque generated by each motor 132. Each joint is a specific example of a joint angle detection unit, and a joint angle detector 133 that detects the rotation angle of each joint driven by each motor 132 and outputs the detected joint angle value is provided. ing.

  Here, i is a positive integer. For example, when modeling from the fingertip of the index finger to the third joint, i = 3. In addition, in FIG. 4 (a) and (b), each symbol is defined as follows.

a _{m, n} : length of a line segment connecting the m-th joint and the n-th joint (hereinafter referred to as “m-th n-th joint distance”)
φ _{m, n} : Angle formed by a line segment connecting the m + 1th link, the mth joint, and the nth joint in the mth joint (hereinafter referred to as “mth nth joint line segment m + 1 link angle”)
θ _m : m-th joint angle First, the m-th and n-th joint distance a _{m, n} and the m-th n-th joint line segment m + 1-link angle φ _{m, n} are derived. In FIG. 4A, the following formulas (1) and (2) are established.

However, in the above formulas (1) and (2), l _i represents the length of the i-th link 406.
When the above equation (1) is solved for the _i- 2th _i-th inter-joint distance a _{i-2, i} , the following equation (3) can be obtained.

When the above equation (2) is solved with respect to the (i-2) -i-th joint line segment i-1 link angle φ _{i-2, i} , the following equation (4) can be obtained.

In FIG.4 (b), the following (5) Formula and (6) Formula are materialized.

When the above equation (5) is solved with respect to the i-3th i-th inter-joint distance _{ai-3, i} , the following equation (7) can be obtained.

When the above equation (6) is solved for the i-3th i-th joint line segment and the i-2 link angle φ _{i-3, i} , the following equation (8) can be obtained.

Similarly, the following formulas (9) and (10) are established for j = 3,..., I−1.

When the above equation (9) is solved with respect to the ij-th i-th inter-joint distance a _{i-j, i} , the following equation (11) can be obtained.

When the above equation (10) is solved with respect to the i-j-th inter-joint line segment i-j + 1 link angle φ _{ij, i} , the following equation (12) can be obtained.

The following formulas (13) and (14) are established for the second link and the third link.

When the above equation (13) is solved for the first i-th inter-joint distances a _{1 and i} , the following equation (15) can be obtained.

When the above equation (14) is solved for the first i-th joint line segment second link angle φ1 _{, i} , the following equation (16) can be obtained.

The m-th nth joint calculated in the above equations (3), (4), (7), (8), (11), (12), (15), and (16) The distance between the center of gravity of the m-th link and the n-th joint (hereinafter referred to as “the m-th”) using the distance a _{m, n} and the m-th n-th line segment m + 1 link angle φ _{m, n} . Bm _{, n,} which is referred to as “link center-of-gravity nth joint distance”.
In FIG. 4A, the square of the i-th link centroid i-th joint distance b _{i, i} can be derived as the following equation (17).

In FIG. 4A, the square of the (i-1) -th link centroid i-th joint distance b _{i-1, i} can be derived as the following equation (18).

The square of the i-th link center-of-gravity i-th joint distance b _{i-j, i} with respect to j = 2,..., i−1 can be derived as the following equation (19).

The moment of inertia related to the i-th joint of the j-th link with respect to j = 1,..., I using the square of the distance between the n-th joints of the m-th center of gravity of the m-th link derived based on the equations (17) to (19) J _{j and i} can be derived from the parallel axis theorem as the following equation (20).

However, in the above equation (20), J _j0 represents the moment of inertia around the center of gravity of the j-th link. The moment of inertia J _i from the i-th joint to the tip (hereinafter referred to as “i-th joint moment of inertia”) J _i is expressed as the following equation (21) using the above equation (20).

The i-th joint moment of inertia J _i calculated using the equation (21) is based on all i joint angle detection values from the first joint 402 to the i-th joint 407. The moment of inertia calculator 141 of the failure determination unit 140 described later uses the equation (21) (function) including the joint angle detection values from the first joint 402 to the i-th joint 407 to obtain the moment of inertia around the i-th joint. J _i (i = 1, 2,...) Is calculated.

ただし、上記（２２）式において各記号は以下のように定義されている。
Ｔ_ｉ：第ｉ関節モータトルク
ｄ_ｉ：第ｉ関節トルク外乱
Ｄ_ｉ：第ｉ関節粘性摩擦 Next, a method for estimating the i-th joint moment of inertia J _i based only on the i-th joint angle detection value θ _i will be described. The equation of motion around the i-th joint 407 is obtained as the following equation (22).

However, each symbol in the above equation (22) is defined as follows.
T _i : i-th joint motor torque d _i : i-th joint torque disturbance D _i : i-th joint viscous friction

The i-th joint angular velocity, which is the first-order time differential value of the i-th joint angle θ _i in the above equation (22), and the i-th joint angular acceleration, which is the second-order time derivative, are expressed by the following equations (23) and (24), respectively. ) Difference approximation can be performed as in the equation.

However, each symbol in the above equations (23) and (24) is defined as follows.
T: Control cycle k: Index indicating the value of each control cycle (represents the value in the kth control cycle)

By substituting the above equations (23) and (24) into the above equation (22), the following equation (25) can be obtained.

When the above equation (25) is solved for the i-th joint moment of inertia J _i , the i-th joint moment of inertia estimated value J _i (hat) is expressed as the following equation (26).

However, it is assumed that the i-th joint viscous friction _Di is known by design. Further, since the disturbance d _i of the i-th joint torque is d _i = 0 at the moment when each joint axis is directed in the vertical direction, at the moment when each joint axis is directed in the vertical direction, the above equation (26) is expressed as 27) can be rewritten.

An inertia moment estimator 142 of the failure determination unit 140 described later uses the above equation (27) (function) including the i-th joint angle detection value θ _i from the i-th joint angle detector 133 to An inertia moment estimated value J _i (hat) (i = 1, 2,...) Is calculated.
Further, the difference between the above equation (27) and the above equation (21) can be calculated as the i-th joint inertia moment estimation error e _{J, i} using the following equation (28).

By substituting the above equations (17) to (20) into the above equation (28), the following equation (29) can be obtained.

Here, as is clear from the above, the first term of the equation (29) is a function of only the joint angle detection value θ _i , and the second and subsequent terms are also expressed as the joint angle detection values θ ₁ to θ _i−1 . It is a dependent function. An estimation error calculator 143 of the failure determination unit 140 described later calculates an inertia moment estimation error e _{J, i} (i = 1, 2,...) Around the i-th joint using the above equation (29).

The failure determination unit 140 of the robot control apparatus 1 according to the present embodiment, for example, detects a failure of each joint angle detector 133 provided at each joint of the finger mechanism 131 of the robot hand 130 described above as an inertia moment estimation error. e _{J, i} can be used for determination.

FIG. 2 is a flowchart showing a processing flow for determining a failure of the joint angle detector at i = 2 by the failure determination unit of the robot control apparatus according to the embodiment of the present invention. First, the failure determination unit 140 of the robot control apparatus 1 determines whether or not the following expression (30) is satisfied when i = 2 (modeling up to the second joint) (step S201).

  However, in the above equation (30), δ is a failure determination threshold (determination threshold) for determining failure of the joint angle detector 133, and is set in advance in the failure determination unit 140, for example.

Malfunction determining unit 140, the (30) if it is determined to satisfy the equation (YES in step S201), the joint angle detection value theta ₁ and theta ₂ are normal (a first joint angle detector 133 second joint angle detector 133 is determined not to have failed (step S202), and the process proceeds to (step S301) in FIG.

On the other hand, the malfunction determining unit 140, the (30) when it is determined not to satisfy the expression (NO in step S201), at least one of the joint angle detection value theta ₁ and theta ₂ is judged to be abnormal (step S203), For example, negative torque is generated in the motor 132 of the first joint, the joint angle detection value θ ₁ = 0 is held, and the above equation (30) is satisfied in a state where the joint angle detection value θ ₁ = 0. Is determined (step S204).

Malfunction determining unit 140, when determining that satisfies the above equation (30) (YES in step S204), the joint angle detection value theta ₁ is abnormal (first joint angle detector 133 has failed), the joint angle detection determines that the value theta ₂ normal (second joint angle detector 133 has not failed) (step S205), and proceeds to (step S301) the processing of FIG.

Whether the malfunction determining unit 140, the (30) when it is determined not to satisfy the expression (NO in step S204), the joint angle detection value theta ₁ is whether normal (first joint angle detector 133 has failed ) Is unknown, and it is determined that the detected joint angle value θ ₂ is abnormal (the second joint angle detector 133 has failed) (step S206), and a negative torque is generated in the motor 132 of the second joint. The joint angle detection value θ ₂ = 0 is held, and it is determined whether or not the above equation (30) is satisfied in a state where the joint angle detection value θ ₂ = 0 (step S207).

Malfunction determining unit 140, the (30) if it is determined to satisfy the equation (YES in step S207), the joint angle detection value theta ₁ is normal (first joint angle detector 133 has not failed), the joint angle detection It determines that the value theta ₂ is abnormal (have second joint angle detector 133 fails) (step S208), and the process proceeds to (step S301) in FIG.

Malfunction determining unit 140, the (30) when it is determined not to satisfy the expression (NO in step S207), the joint angle detection value theta ₁ and theta ₂ is abnormal (first joint angle detector 133 and a second joint angle detection (Step S209), and the process proceeds to (Step S301) in FIG.

  FIG. 3 is a flowchart showing a processing flow for determining a failure of the joint angle detector when it is known which of the first joint angle detector and the second joint angle detector is broken. is there.

  The failure determination unit 140 holds the joint angle detection value determined to be abnormal when i = j and j ≧ 3 as 0 (step S204), and holds the joint angle detection value at 0. It is determined whether or not it is satisfied (step S301).

If the failure determination unit 140 determines that the above equation (30) is satisfied (YES in step S301), the failure determination unit 140 determines that the detected joint angle value θ _j is normal (the jth joint angle detector 133 has not failed) ( Step S302), the process returns to the above (Step S301).

On the other hand, if the failure determination unit 140 determines that the above equation (30) is not satisfied (NO in step S301), the joint angle detection value θ _j is abnormal (the j-th joint angle detector 133 has failed). Determination is made (step S303), and the processing returns to the above (step S301).

In this way, even in a robot having a mass and size that can coexist with a person equipped with only the minimum sensors necessary for operation, the failed joint angle detector 133 can be easily identified.
When the failure determination unit 140 completes the failure determination of all the joint angle detectors 133 using the processing flow shown in FIGS. 2 and 3 described above, the safety control unit 120 determines that the determination result and the robot hand 130 The j-th joint motor torque T _j shown in the following equation (31) is generated according to the type of work to be performed.

However, in the above equation (31), T _jr is a target value of the motor torque generated by the motor 132 of the j-th joint when the j-th joint angle detector 133 has not failed (hereinafter referred to as “j-th joint motor”). This is referred to as “torque command”. Further, s _j is a parameter for classifying the type of work to be performed by the robot hand 130 (hereinafter referred to as “work classification parameter”), and is set, for example, as in the above equation (32). Yes.

When the work classification parameter s _j is set to 1 (first predetermined value), the finger mechanism 131 generates a reaction force toward the outside, while the work classification parameter s _j is set to 0 (second predetermined value). When set, the finger mechanism 131 does not generate a reaction force toward the outside.

The three motor torque commands T _j in the above equation (31) can be defined as follows, for example.
(A) T _j <0: In an operation in which the j-th joint angle detector 133 is out of order and the robot hand 130 is to perform, the finger mechanism 131 applies a reaction force toward the outside (the direction in which the finger extends). If it needs to occur (s _j = 1).

In this case, by setting the j-th joint motor torque command T _j to a negative torque (predetermined torque), the j-th joint is extended, and a reaction force is applied to the object when the finger mechanism 131 contacts the object. Will be able to give.
(B) T _j = 0: When the j-th joint angle detector 133 is out of order and the robot hand 130 is about to perform, it is not necessary for the finger mechanism 131 to generate a reaction force toward the outside. (S _j = 0).

In this case, by setting the j-th joint motor torque command T _j to 0, the j-th joint rotates in the positive direction (in the bending direction) when the finger mechanism 131 is pushed inward by the object, and the finger mechanism When 131 is pushed outward from the object, the finger mechanism 131 rotates in the negative direction (in the extending direction) until it reaches the extended state. In any case, when reaching the end of the movable range of the j-th joint, the angle is maintained as long as the force from the object is applied in the same direction.
(C) T _j = T _jr : When the j-th joint angle detector 133 is not broken or it is not known whether or not it is broken.

In this case, a j-th joint motor torque command T _j (a safe motor torque command preset for each work pattern) necessary for the work to be performed by the robot hand 130 is generated, and this j-th joint motor torque command T the j joint drive according _j.

In the present embodiment, by generating the j-th joint motor torque T _j by the above equation (31), the runaway of the j-th joint motor 132 is avoided even when the j-th joint angle detector 133 is out of order. In addition, the work that the robot hand 130 is trying to perform can be safely continued. Hereinafter, T _j shown in the above equation (31) is referred to as “safety joint motor torque command”.

  FIG. 1 is a block diagram showing a schematic system configuration of a robot control apparatus according to an embodiment of the present invention. The robot control apparatus 1 according to the present embodiment includes a joint angle command generation unit 110, a safety control unit 120, a robot hand 130, and a failure determination unit 140.

  The joint angle command generation unit 110 generates a joint angle command for driving each joint based on the failure determination result of each joint angle detector 133 output from the failure determination unit 140, and generates each generated joint angle command. Is output to the safety control unit 120.

  The joint angle command generation unit 110 includes a work pattern generator 111 and a joint angle command generator 112.

  The work pattern generator 111 is a specific example of a work pattern generation unit. For example, the robot hand 130 performs the next operation according to information input by the user of the robot hand 130 or a program preset in the robot control device 1 or the like. A work pattern (motion data) necessary for the work to be performed is generated and output to the joint angle command generator 112 and the safety control unit 120.

  The joint angle command generator 112 is a specific example of a joint angle command generation unit, and based on the failure determination result of each joint angle detector 133 from the failure determination unit 140, there is no failure joint angle detector 133. When the determination is made, based on the work pattern from the work pattern generator 111, a joint angle command necessary for performing the operation represented by the work pattern is generated and output to the safety control unit 120.

  When the joint angle command generator 112 determines that there is a failed joint angle detector 133 based on the failure determination result of each joint angle detector 133 from the failure determination unit 140, the joint angle detector 133 that has failed Based on the position of 133 and the work pattern to be executed by the robot hand 130, it is determined whether or not the work pattern can be executed.

  When it is determined that the work content cannot be performed, the joint angle command generator 112 generates a joint angle command necessary to stop the work pattern work and outputs the joint angle command to the safety control unit 120. Here, the joint angle command necessary for canceling the work content is, for example, a joint angle command that allows the robot hand 130 to take a safe posture using only the joint angle detector 133 that has not failed. is there.

  On the other hand, when it is determined that the work pattern is feasible, the joint angle command generation unit 110 generates each joint angle command for a joint other than the joint where the failed joint angle detector 133 is arranged, and sends it to the safety control unit 120. Output.

  The safety control unit 120 includes a joint angle command from the joint angle command generation unit 110, a failure determination result of each joint angle detector 133 from the failure determination unit 140, and a joint angle from each joint angle detector 133 of the robot hand 130. Based on the detected value, a safe joint motor torque command for safely driving the motor 132 of each joint of the robot hand 130 is generated and output to the motor 132 of the robot hand 130. Here, the safe joint motor torque command is, for example, a motor torque command for each joint such that the detected joint angle values at all joints of the finger mechanism 131 of the robot hand 130 follow the respective joint angle commands.

  The safety control unit 120 includes a parameter generator 121 and a motor torque command generator 122.

The parameter generator 121 is a specific example of parameter generation means, and obtains the work to be performed by the robot hand 130 for each joint based on the work pattern from the work pattern generator 111 of the joint angle command generation unit 110. The operation classification parameters s _j (j = 1, 2,...) Are set, and the set operation parameters s _j are output to the motor torque command generator 122.

The motor torque command generator 122 is a specific example of torque command generation means. Each joint angle command from the joint angle command generation unit 110, the work classification parameter s _j from the parameter generator 121, and the failure determination unit 140. , The safety joint motor torque command T _j (j = 1, 2,...) Expressed by the above equation (31) is generated based on the failure determination result of each joint angle detector 133 from the robot hand 130. Output to each of the motors 132.

For example, when the motor torque command generator 122 determines that there is no failed joint angle detector 133 based on the failure determination result of each joint angle detector 133 from the failure determination unit 140, the finger mechanism of the robot hand 130 A safety joint motor torque command T _j is generated such that the joint angle detection value of the joint angle detector 133 in all the joints 131 follows each joint angle command generated by the joint angle command generator 112, and the robot hand 130 Output to each motor 132.

On the other hand, when the motor torque command generator 122 determines that there is a failed joint angle detector 133 based on the failure determination result of each joint angle detector 133 from the failure determination unit 140, the joint angle that has not failed is determined. The detected joint angle value of the detector 133 follows the joint angle command generated by the joint angle command generator 112, and the motor torque for the motor 132 of the joint having the failed joint angle detector 133 is set for each work pattern. A safe joint motor torque command T _j is generated so as to obtain a safe motor torque, and is output to each motor 132 of the robot hand 130. Here, the motor torque command generator 122 converts the safe joint motor torque command T _j into a motor current and outputs it to the motor 132 of each joint of the robot hand 130.

The robot hand 130 is driven by a finger mechanism 131, a plurality of motors 132 that drive each joint based on a safety joint motor torque command T _j from the motor torque command generator 122 of the safety control unit 120, and each motor 132. And a plurality of joint angle detectors 133 that detect the rotation angle of each joint and output as a joint angle detection value. The finger mechanism 131 is a mechanism that includes a plurality of links and a plurality of joints that rotatably connect adjacent links.

  Based on the joint angle detection values from the joint angle detectors 133 of the robot hand 130, the failure determination unit 140 turns the rotation axes of the joints in the vertical direction when the robot hand 130 is not in contact with the object. At the moment, the presence / absence and position of the broken joint angle detector 133 is determined, and the failure determination result of each joint angle detector 133 is output to the joint angle command generation unit 110 and the safety control unit 120.

  The failure determination unit 140 includes an inertia moment calculator 141, an inertia moment estimator 142, an inertia moment estimation error calculator 143, and a failure determiner 144.

The inertia moment calculator 141 is a specific example of the inertia moment calculator, and is based on the first to i-th joint angle detection values from the joint angle detectors 133 and the above equation (21). The inertia moment J _i (i = 1, 2,...) Around the joint is calculated and output to the estimation error calculator 143.

The inertia moment estimator 142 is a specific example of the inertia moment estimation means, and the rotation of the i-th joint is based on the i-th joint angle detection value from each joint angle detector 133 and the above equation (27). An inertia moment estimated value J _i (hat) (i = 1, 2,...) Around the i-th joint when the axis is in the vertical direction is calculated and output to the inertia moment estimation error calculator 143.

The inertia moment estimation error calculator 143 is a specific example of the estimation error calculator, and the inertia moment J _i from the inertia moment calculator 141 and the inertia moment estimate J _i (hat) from the inertia moment estimator 142 Are calculated as an inertia moment estimation error e _{J, i} (i = 1, 2,...) Around the i-th joint using the above equation (29) and output to the failure determination unit 144.

The failure determiner 144 is a specific example of the failure determination means. Based on the inertia moment estimation error e _{J, i} from the inertia moment estimation error calculator 143 and the above equation (30), FIG. 2 and FIG. 3 determines whether each joint angle detector 133 has failed according to the processing flow shown in FIG. 3, and outputs the failure determination result of each joint angle detector 133 to the joint angle command generation unit 110 and the safety control unit 120. To do.

  The failure determination unit 140 performs the determination calculation of each joint angle detector 133 described above at the moment when the rotation axis of each joint is directed in the vertical direction before contacting the object during the operation of the robot hand 130. .

  As described above, according to the present embodiment, the joint axis detector 133 is not redundantly provided, and the rotation axis of each joint is vertical while the robot hand 130 is not in contact with an object during the work. It is possible to instantaneously determine whether or not each joint angle detector 133 is out of order at the moment of turning. Further, based on the determination result, the robot hand 130 can safely perform the next operation or can be stopped without using the failed joint angle detector 133.

  Next, the results of simulation using the robot control apparatus 1 according to the present embodiment will be described. In this simulation, when only the second joint angle detector 133 is broken in the index finger of the robot hand 130 composed of three joints and three links, a plurality of postcards using the index finger and thumb are used. Simulates stopping with rubber bands. The numerical values used for the simulation are set as follows.

l ₁ = 20 × 10 ⁻³ [m], l ₂ = 20 × 10 ⁻³ [m], l ₃ = 20 × 10 ⁻³ [m], l ₁₀ = 10 × 10 ⁻³ [m], l ₂₀ = 10 × 10 ⁻³ [m], l ₃₀ = 10 × 10 ⁻³ [m], m ₁ = 30 × 10 ⁻³ [kg], m ₂ = 30 × 10 ⁻³ [kg], m ₃ = 30 × 10 ⁻³ [kg], J ₁₀ = 1.42 × 10 ⁻⁶ [kg · m ² ], J ₂₀ = 1.42 × 10 ⁻⁶ [kg · m ² ], J ₃₀ = 1.42 × 10 ⁻⁶ [kg · m ² ], D ₁ = 1 × 10 ⁻⁴ [N · m · s / rad], D ₂ = 1 × 10 ⁻⁴ [N · m · s / rad], D ₃ = 1 × ^{10 -4 [N · m · s} / rad], θ 1min = 0 [rad], θ 1max = π [rad], θ 2min = 0 [rad], θ 2max = π [rad], θ 3min = 0 [ _{rad], θ 3max = π [} rad] ^{_{c v2 = -1 × 10 -1 [}} rad / s], T = 1 × 10 -3 [s], K p1 = 40 [s -1], K v1 = 40 × 2 × π [s -1], K _vj1 = K _v1 × (J ₁₀ + m ₁ × l ₁₀ ² ) [N · m ² · s ⁻¹ ], K _p2 = 40 [s ⁻¹ ], K _v2 = 40 × 2 × π [s ⁻¹ ] K _vj2 = K _v2 × (J ₂₀ + m ₂ × l ₂₀ ² ) [N · m ² · s ¹ ], K _p3 = 40 [s ⁻¹ ], K _v3 = 40 × 2 × π [s ⁻¹ ], K _vj3 = K _v3 × (J ₃₀ + m ₃ × l ₃₀ ² ) [N · m ² · s ⁻¹ ], T _1c = −1 × 10 ⁻¹ [N · m], T _2c = −1 × 10 ^{− 1} [N · m], T _3c = −1 × 10 ⁻¹ [N · m], θ _1f = 1 × 10 ⁻¹ [rad], θ _2f = 1 × 10 ⁻¹ [rad], θ _3f = 1 _{^{× 10 -1 [rad], v}} 1c = 5 × 10 -1 [rad _{s], v 2c = 5 ×} 10 -1 [rad / s], v 3c = 5 × 10 -1 [rad / s], δ = 5 × 10 -3 [kg · m 2],
g = 9.8 [m / s ⁻² ]
However, the above symbols are defined as follows.
l _i0 : Distance between the centroid of the i-th link and the i-th joint (hereinafter referred to as “distance between the ith link centroid and the i-th joint”)
_θimin : i-th joint angle lower limit,
θ _imax : i-th joint angle upper limit value,
c _v2 : second joint speed detection value drift amount,
K _pi : i-th joint angle proportional control gain,
K _vi : normalized i-th joint velocity proportional control gain,
K _vji : i-th joint velocity proportional control gain,
T _ic : i-th joint motor torque for holding the i-th joint angle at the i-th joint angle lower limit (hereinafter referred to as “i-th joint angle lower limit holding motor torque”)
θ _if : i-th joint angle final target value when the i-th joint angle detector 133 has not failed (hereinafter referred to as “final i-th joint angle target value”)
v _ic : i-th joint speed command change amount in each control cycle when the i-th joint angle detector 133 is not out of order (hereinafter referred to as “i-th joint speed command change amount”)
g: Gravity acceleration

The motion equation of the index finger of the finger mechanism 131 of the robot hand 130 can be derived as the following equations (33) to (35).

Here, in order to pass the index finger of the finger mechanism 131 represented by the above (33) to (35) through the rubber band, it is assumed that each joint is slightly bent with the rotation axis of the joint facing the vertical direction. To do. When expanding the rubber band, each link of the index finger needs to generate an outward reaction force on the rubber band, so the work classification parameters are s ₁ = 1, s ₂ = 1, and s ₃ = 1.

In the initial state, it is assumed that the index finger is in the extended state (θ ₁ = 0, θ ₂ = 0, θ ₃ = 0). First, it is unknown whether or not the first joint angle detector 133 to the third joint angle detector 133 are out of order. Therefore, the first joint motor torque command T _1r , the second joint motor torque command T _2r , and the third joint motor torque command T _3r represented by the equations (36) to (38) are used for each joint. Control the angle.

In the above formulas (36) to (38), the joint angle command generator 112 outputs the first joint angle command θ _1r , the second joint angle command θ _2r , and the third joint angle command θ _{3r according} to the following (39). Calculation is performed as a first-order time integration of Expressions (41) to (41).

However, in the above equations (39) to (41), c _θi is the failure determination result of the failed i-th joint angle detector 133 (1: unknown whether or not it is broken, 0: not broken, − 1: Defective)

From the above equations (39) to (41), until the i-th joint angle detection value θ _i reaches the i-th joint angle final target value θ _if , the i-th joint angle command θ _ir is The first-order time differential value is increased by the joint speed command change amount _vic , and the value is held when reaching the i-th joint angle final target value θ _if .

  Hereinafter, the failure determination of each joint angle detector 133 of the index finger controlled as described above will be described with reference to FIGS.

FIG. 5A shows a temporal change in the joint angle of the second joint in the simulation of the robot control apparatus according to the present embodiment, and FIG. 5B shows the second change in the simulation of the conventional robot control apparatus. The time change of the joint angle of the joint is shown. 5A and 5B, the solid line indicates the actual second joint angle θ ₂ , the alternate long and short dash line indicates the second joint angle detection value θ _2m , and the broken line indicates the second joint angle command θ _2r .

Figure in 5 (a), is implemented operations of FIG. 2 to time 0.003 [s] from (step S201) (step S203), the actual second among joint angles theta ₁ and theta ₂ at least one of abnormal I know only that. During this time, the broken joint angle detector 133 has not been identified, and therefore, the broken second joint angle command θ _2r is increased according to the first expression of the above expression (40). Here, the second joint angle command theta _2r varies stepwise is because the second joint angle command theta _2r is generated according to a discrete-time control cycle ^{T = 1 × 10 -3 [s} ]. In the present embodiment, it is assumed that the second joint angle detector 133 is out of order and outputs a one-time integral value of the erroneous second joint speed detection value θ _2m represented by the following equation (42).

The erroneous second joint angle detection value θ _2m represented by the above equation (42) changes as indicated by the broken line. That is, until the second joint angle detection value θ _2m is converged to the second joint angle command θ _2r until 0.003 [s], the actual second joint angle θ ₂ is controlled, and as a result, the actual second joint angle θ _2m is controlled. joint angle theta ₂ is increased as shown by the solid line.

In 0.003 [s] in FIG. 5A, the calculation from (step S204) to (step S206) in FIG. 2 is performed, and a failure of the second joint angle detector 133 is detected. In (step S207) Based on the first expression of the above expression (31), the safe joint motor torque is output and held at the actual second joint angle θ ₂ = 0 [rad]. Actually, after 0.003 [s], the actual second joint angle θ ₂ is held at 0 [rad], and it can be seen that the second joint is held in an extended state. During this time, since the second joint angle detector 133 remains broken, the erroneous second joint angle detection value θ _2m continues to be output.

On the other hand, in the conventional robot control apparatus, as shown in FIG. 5B, the actual second joint angle θ ₂ shown by the solid line is divergent and continues to bend until the second joint of the index finger is completely broken. I understand.

FIG. 6A shows a time change of the joint angle of the first joint to 0.015 [s] in the simulation of the robot control apparatus according to the present embodiment, and FIG. The time change to 0.5 [s] of the joint angle of the 1st joint in the simulation of the robot control apparatus which concerns on this form is shown. 6A and 6B, the solid line indicates the actual first joint angle θ ₁ , and the broken line indicates the first joint angle command θ _1r .

The calculation of (Step S207) of FIG. 2 is completed at 0.006 [s] of FIG. 6A, and then the calculation of (Step S208) is performed, and the first joint angle detector 133 has failed. It is determined that there is no. Therefore, even after 0.006 [s], the joint angle command generator 112 outputs the first joint angle command θ _1r indicated by the broken line in FIG. _6A according to the second formula of the above formula (39). Therefore, the actual first joint angle θ ₁ also increases so as to follow the first joint angle command θ _1r .

As shown in FIG. 6B, the first joint angle command θ _1r indicated by the broken line reaches the first joint angle final target value θ _1f = 0.1 [rad] at 0.2 [s], and thereafter. Is held at θ _1r = θ _1f . The actual first joint angle θ ₁ indicated by the solid line is controlled by the safe joint motor torque command output from the motor torque command generator 122 based on the third equation (31) and the equation (36). Asymptotically converges to the first joint angle final target value θ _1f . In FIGS. 6A and 6B, the first joint angle detection value matches the actual first joint angle.

FIG. 7A shows the time change of the joint angle of the third joint to 0.015 [s] in the simulation of the robot control apparatus according to the present embodiment, and FIG. The time change to 0.5 [s] of the joint angle of the 3rd joint in the simulation of the robot control apparatus concerning this form is shown. 7A and 7B, the solid line indicates the actual third joint angle θ ₃ , and the broken line indicates the third joint angle command θ _3r .

7A, (step S201) to (step S208) in FIG. 2 and (step S301) in FIG. 3 (when j = 3) are performed until 0.009 [s]. As a result, at 0.009 [s] (step S302), the failure determination unit 144 determines that the third joint angle detector 133 has not failed. Accordingly, even after 0.009 [s], the joint angle command generator 112 generates the third joint angle command θ _3r indicated by the broken line in FIG. 7A based on the second expression of the above expression (41). And output to the motor torque command generator 122. As a result, the actual third joint angle θ ₃ indicated by the solid line also increases so as to follow the third joint angle command θ _3r .

As shown in FIG. 7B, the third joint angle command θ _3r indicated by the broken line reaches the third joint angle final target value θ _3f = 0.1 [rad] at 0.2 [s], and Thereafter, the third joint angle command θ _3r = the third joint angle final target value θ _3f is held. Further, the actual third joint angle θ ₃ indicated by the solid line is determined by the safe joint motor torque command output from the motor torque command generator 122 based on the third equation of the equation (31) and the equation (38). Controlled and asymptotically converges to the third joint angle final target value θ _3f .

FIG. 8 shows the time change of the second joint moment of inertia moment estimation error in the simulation of the robot control apparatus according to the present embodiment. The solid line indicates the moment of inertia estimation error e _{J, 2} around the second joint calculated by the estimation error calculator 143 based on the above equation (29). The absolute value of the inertia moment estimation error e _{J, 2} increases from 0 [kg · m ² ] and is 0.003 [s] because the actual second joint angle θ ₂ = 0 at 0 [s]. The failure determination threshold value δ = 5 × 10 ⁻³ [kg · m ² ] of the joint angle detector 133 is exceeded. For this reason, as described above, the failure determination unit 144 and the second joint angle detector 133 are determined to have failed in the processing of (step S206) in FIG.

Next, the failure determiner 144 proceeds to the processing of (Step S207). At this time, as shown in FIG. 8, the inertia moment estimation error e _{J, 2} is decreased. Then, at 0.006 [s], the failure determiner 144 determines that the above expression (30) is satisfied (YES in step S207), the process proceeds to (step S208), and the first joint angle detection is performed. It is determined that the device 133 has not failed.

Figure 9 is a diagram illustrating a second time changes of the joints of the motor torque command T ₂ of the order to control the second joint failed joint angle detector is provided.

As shown in FIG. 9, until 0.003 [s], the failure determination unit 144 performs the processing up to the processing of (Step S <b> 204) in FIG. 2, and the first joint angle detector 133 or the second joint angle detector 133. It has only been found that at least one of the For this reason, the motor torque command generator 122 outputs the second joint motor torque command T ₂ = T _2r to the second joint motor 132 based on the third expression of the above expression (31). .

In addition, after 0.003 [s], the failure determiner 144 executes the process of (Step S207) in FIG. 2, and the motor torque command generator 122 holds the actual second joint angle θ ₂ = 0. In addition, the safe joint motor torque command T ₂ = T _2c represented by the first expression of the above expression (31) is output to the motor 132 of the second joint.

  As described above, according to the robot control apparatus 1 according to the present embodiment, for example, the rotation of each joint without touching the object during the operation of the robot hand 130 without depending on the redundant design of the joint angle detector 133. When the axis is oriented in the vertical direction, it can be instantaneously determined whether or not each joint angle detector 133 has failed, and the robot hand 130 can be used without using the failed joint angle detector 133 based on the determination result. Can safely perform the next operation or stop the operation. That is, optimal control can be performed even when a failure occurs in the sensor of the robot hand 130.

  Further, in the conventional technique shown in Patent Document 1, when it is unclear which joint angle detector has failed, generally, the correct joint angle detection value used by the observer is output and becomes observable. A sufficient joint angle detector cannot be identified. Therefore, it is not possible to specify a broken joint angle detector. On the other hand, according to the robot control apparatus 1 according to the present embodiment, there is a merit that it is possible to identify the joint angle detector 133 that has failed reliably in a short time regardless of the position and number of the joint angle detector 133 that has failed. Have.

  Furthermore, for the reasons described above, according to the prior art shown in Patent Document 1, for example, as shown in FIG. 5A, the joint of the finger mechanism of the failed robot hand becomes uncontrollable, and the next work cannot be performed. Also, depending on the type of work to be performed, there is a possibility that it is not sufficiently safe for surrounding objects and people. On the other hand, according to the robot control apparatus 1 according to the present embodiment, as shown in FIG. 5B, the control is performed so as not to be affected by the failed joint angle detector 133, and thus the operation is continued. Depending on the type of work to be performed, the work can be safely stopped.

  Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention.

For example, in the above-described embodiment, the i-th joint speed command change amount _vic in the above-described formulas (39) to (41) is set to a constant value. It is also possible to apply an arbitrary variable amount that converges to.

  In the above embodiment, the i-th joint motor torque command in the equations (36) to (38) is calculated by feedback linearization control. However, the present invention is not limited to this, and is calculated according to any other control law. May be.

  Further, in the above-described embodiment, the finger mechanism 131 of the robot hand 130 has a configuration including three links and three joints. However, the configuration is not limited thereto, and any configuration can be applied.

  In the above-described embodiment, an example of the work of hanging a rubber band on a plurality of postcards has been described. However, the present invention is not limited to this, and can be applied to any work using the robot hand 130.

  In the above embodiment, the joint angle command generation unit 110, the safety control unit 120, and the failure determination unit 140 can be realized as software installed in a microcomputer, but may be realized as hardware such as an ASIC. . The present invention can be realized, for example, by causing a CPU to execute a computer program.

  The program may be stored using various types of non-transitory computer readable media and supplied to a computer. Non-transitory computer readable media include various types of tangible storage media. Examples of non-transitory computer readable media are magnetic recording media (eg flexible disks, magnetic tapes, hard disk drives), magneto-optical recording media (eg magneto-optical disks), CD-ROM, CD-R, CD-R / W. Semiconductor memory (for example, mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM).

  The program may also be supplied to the computer by various types of transitory computer readable media. Examples of transitory computer readable media include electrical signals, optical signals, and electromagnetic waves. The temporary computer-readable medium can supply the program to the computer via a wired communication path such as an electric wire and an optical fiber, or a wireless communication path.

  In the present invention, the robot can be controlled safely and optimally in consideration of the failure of the joint angle detector 133 provided at each joint. For example, the robot is small, light, low in price, and safe for surrounding people and objects. It can be applied to all service robots that are equipped with robot hands, such as robots that help with housework, nursing robots, musical instrument playing robots, and robots that help with office work.

DESCRIPTION OF SYMBOLS 110 Joint angle command generation part 111 Work pattern generator 112 Joint angle command generator 120 Safety control part 121 Parameter generator 122 Motor torque command generator 130 Robot hand 131 Finger mechanism 132 Motor 133 Joint angle detector 140 Failure determination part 141 Inertia Moment calculator 142 Inertia moment estimator 143 Inertia moment estimation error calculator 144 Failure determiner 401 First link 402 First joint 403 i-2 joint 404 i-1 link 405 i-1 joint 406 ilink 407 I-th joint 408 i-3 link 409 i-3 joint 410 i-2 link

Claims (12)

A robot provided with a plurality of links, a plurality of joints that rotatably connect the links, a plurality of drive units that drive the joints, and a plurality of joint angle detection units that detect the joint angles. A control device,
Based on the function including the joint angle from the first joint on the tip side to the i-th joint (i ≧ 2) detected by each joint angle detection means, the area around the i-th joint from the i-th joint to the tip portion A moment of inertia calculating means for calculating a moment of inertia;
Based on a function including the joint angle of the i-th joint detected by the joint angle detection means, an estimated moment of inertia around the i-th joint when the rotation axis of the i-th joint is in a vertical direction is calculated. A moment of inertia estimating means;
The difference between the inertia moment around the i-th joint calculated by the inertia moment calculation means and the estimated inertia moment around the i-th joint calculated by the inertia moment estimation means is the inertia around the i-th joint. Estimated error calculation means for calculating as a moment error;
A failure determination means for comparing the absolute value of the moment of inertia error around the i-th joint calculated by the estimation error calculation means and a determination threshold to determine a failure of the joint angle detection means;
A joint angle command generating means for generating a joint angle command for each of the joints based on the determination result of the failure determined by the failure determining means and the work pattern that the robot is to perform;
A robot control device comprising: The robot control device according to claim 1,
When the absolute value of the moment of inertia error around the i-th joint calculated by the estimation error calculation unit exceeds a determination threshold, the failure determination unit is one joint among the joint angles of the plurality of joint angle detection units A robot control apparatus characterized by holding the angle at 0 degree and repeatedly comparing the absolute value of the moment of inertia error around the i-th joint with a determination threshold value to determine a failure of the joint angle detection means. The robot control device according to claim 2,
The failure determination means holds one joint angle of the plurality of joint angles at 0 degree,
When the absolute value of the moment of inertia error around the i-th joint is equal to or less than the determination threshold, the joint angle detection unit holding the joint angle at 0 degree is broken, and the other joint angle detection units are determined to be normal And
When the absolute value of the moment of inertia error around the i-th joint exceeds a determination threshold, it is determined that the joint angle detection unit that holds the joint angle at 0 degrees is unknown, and the other joint angle detection units are failed. The robot control is characterized in that the joint angle corresponding to the joint angle detection means determined to be faulty is held at 0 degrees, and the comparison between the absolute value of the inertia moment error around the i-th joint and the determination threshold is repeated. apparatus. The robot control device according to any one of claims 1 to 3,
The joint angle command generation means determines whether or not the work pattern can be performed based on the determination result of the failure determined by the failure determination means and the work pattern that the robot is to perform,
When it is determined that the work pattern cannot be performed, a joint angle command for stopping the work pattern is generated,
A robot control device that generates a joint angle command for a joint other than the joint in which the faulty joint angle detection means is installed when it is determined that it can be implemented. The robot control device according to any one of claims 1 to 4,
When it is determined that there is no failed joint angle detection means based on the determination result from the failure determination means, the joint angles of the joint angle detection means in all joints are generated by the joint angle command generation means. Generating a torque command for the drive means to follow the command,
Based on the failure determination result from the failure determination means, when it is determined that the joint angle detection means that has failed is present, the joint angle of the joint angle detection means that has not failed is generated by the joint angle command generation means. A torque command for the drive means is generated so as to follow the joint angle command, and a torque command preset for each work content is given to the joint drive means provided with the failed joint angle detection means. Generate,
A robot control apparatus further comprising torque command generation means. The robot control device according to any one of claims 1 to 4,
A work pattern generating means for generating a work pattern for the robot to perform work based on user input information or a preset program;
Classifying the work pattern generated by the work pattern generation means, parameter generation means for setting a value corresponding to the preset classification as a work classification parameter;
Generate a torque command for the drive unit based on each joint angle command from the joint angle command generation unit, the work classification parameter from the parameter generation unit, and a failure determination result from the failure determination unit. Torque command generating means for performing,
The robot control device further comprising: The robot control device according to claim 6,
The torque command generating means
(A) Based on the determination result from the failure determination means, it is determined that the joint angle detector has failed, and it is determined that a first predetermined value is set for the work classification parameter from the parameter generation means. When generating a torque command to generate a predetermined torque,
(B) Based on the determination result from the failure determination means, it is determined that the joint angle detector has failed, and it is determined that a second predetermined value is set for the work classification parameter from the parameter generation means. Set the torque command to 0,
(C) Based on the determination result from the failure determination means, when it is determined that the joint angle detector is not broken or unknown, a torque command set in advance for each work pattern is generated. A robot control device. The robot control device according to claim 6 or 7,
The parameter generation means is based on the work pattern generated by the work pattern generation means.
When it is determined that the work to be performed by the robot generates a reaction force toward the outside so as to extend the finger of the robot hand, a first predetermined value is set as the work classification parameter;
When it is determined that the work to be performed by the robot does not generate a reaction force toward the outside of the finger of the robot hand, a second predetermined value is set as the work classification parameter.
A robot controller characterized by that. The robot control device according to any one of claims 1 to 8,
The robot is
A finger mechanism having the plurality of links and a plurality of joints rotatably connecting the links;
A plurality of motors for driving the joints;
A plurality of joint angle detectors for detecting each joint angle;
A robot control device comprising a robot hand. The robot control device according to any one of claims 1 to 9,
The inertia moment calculation means calculates an inertia moment J _i around the i-th joint using the equation (21) as the function,
The inertia moment estimating means calculates an inertia moment estimated value J _i (hat) around the i-th joint using the equation (27) as the function,
The robot apparatus characterized in that the estimation error calculation means calculates an inertia moment error e _{J, i} around the i-th joint using equation (29).

However, the expression (21), in (27), and (29), the following equation is satisfied, _{l m} is the m-th link length, _{m i} is the i link _{mass, a m, n} is the m The length of the line segment connecting the joint and the n-th joint, b _{m, n} is the distance between the n-th joint of the m-th center of gravity of the m-th link, and φ _{m, n} is the m-th joint connecting the m + 1-th link, the m-th joint, and the n-th joint. The angle formed by the line segment, θ _m is the m-th joint angle, J _{j, i} is the moment of inertia related to the i-th joint of the j-th link, J _j0 is the moment of inertia around the center of gravity of the j-th link, and T _i is the i-th joint motor torque. , _Di is the i-th joint viscous friction, T is the control cycle, and k is an index indicating the value of each control cycle.

A robot provided with a plurality of links, a plurality of joints that rotatably connect the links, a plurality of drive units that drive the joints, and a plurality of joint angle detection units that detect the joint angles. A control method for a control device, comprising:
Calculating an inertia moment around the i-th joint from the i-th joint to the tip portion based on the detected function including the joint angle from the first joint to the i-th joint (i ≧ 2). When,
Calculating an estimated moment of inertia around the i-th joint when the rotation axis of the i-th joint is in a vertical direction based on the detected function including the joint angle of the i-th joint;
Calculating a difference between the calculated inertia moment around the i-th joint and the calculated estimated inertia moment around the i-th joint as an inertia moment error around the i-th joint;
Comparing the calculated absolute value of the moment of inertia error around the i-th joint with a determination threshold value to determine a failure of the joint angle detection means;
Generating a joint angle command for each joint based on the determination result of the determined failure and a work pattern that the robot is to perform;
A control method for a robot control apparatus, comprising: A robot provided with a plurality of links, a plurality of joints that rotatably connect the links, a plurality of drive units that drive the joints, and a plurality of joint angle detection units that detect the joint angles. A control program for a control device,
A process of calculating a moment of inertia around the i-th joint from the i-th joint to the tip based on a function including a joint angle from the detected first joint to the i-th joint (i ≧ 2) on the tip side. When,
A process of calculating an estimated moment of inertia around the i-th joint when the rotation axis of the i-th joint is in a vertical direction based on the detected function including the joint angle of the i-th joint;
A process of calculating a difference between the calculated inertia moment around the i-th joint and an estimated inertia moment value around the i-th joint as an inertia moment error around the i-th joint;
A process of comparing the calculated absolute value of the moment of inertia error around the i-th joint with a determination threshold to determine a failure of the joint angle detection means;
A process of generating a joint angle command for each joint based on the determination result of the determined failure and a work pattern that the robot is to perform;
A control program for a robot control apparatus, characterized in that a computer is executed.

Images