JP4577607B2 - Robot control device and robot system - Google Patents

Description

The present invention relates to a control device and robot system of the robot which controls the output of the joint actuators.

  Conventionally, when a peripheral device such as a workpiece, which is the target object, exists on the robot's motion trajectory, and the robot's moving part unexpectedly contacts the peripheral device, the robot controller realizes the given position command. Therefore, a large torque is generated at the joint of the robot. Generation of large torque at the joints of the robot is dangerous because it damages the robot mechanism and motors or peripheral devices. Use peripheral sensors such as tactile sensors, force sensors, ultrasonic sensors, or infrared sensors. It was necessary to take protective measures such as detecting the contact and approach of the robot and stopping the robot immediately before and after the contact. However, arranging a sensor capable of detecting contact or proximity over the entire operation range of the robot causes an increase in cost. In addition, a highly reliable sensor that can measure the operating range of the robot over a wide range is not provided. Therefore, there is a need for a method that avoids the danger caused by unexpected contact between the robot and peripheral devices without using new sensors without cost.

  In order to solve such a problem, there are a method for preventing an unexpected contact with a peripheral device by setting an allowable range of the robot and a method for detecting an unexpected contact with the peripheral device of the robot and stopping the robot.

  As a first conventional technique, in a method of setting an allowable robot reachable range to prevent unexpected contact with peripheral devices, the reachable allowable range of each joint displacement amount of the robot is defined in advance, and the measured joint displacement amount A general method is to stop the robot when (hereinafter referred to as an actual joint response) deviates from the allowable range. In addition, the robot's position and orientation reachable tolerance range is defined in advance in the robot's workspace, and when the measured position and orientation (hereinafter referred to as actual machine position response) deviate from the reachable tolerance range, There is also a known method for stopping the operation.

In addition, as a second conventional technique, a method of detecting an unexpected contact with a peripheral device of a robot and stopping the robot includes a difference between a command position to the robot in the work space and an actual machine position response or a command joint in the joint space. There is a method of monitoring the difference between the displacement amount and the actual joint response, and when the difference exceeds a threshold, the robot is stopped as a contact has occurred (Patent Document 1).
FIG. 9 is a block diagram showing a circuit configuration for one axis of a control system of a motor mounted on a robot in Patent Document 1 as a second prior art.
In FIG. 9, 11 is a servo amplifier, and 12 is a servo motor. Reference numeral 13 denotes a position deviation, which is obtained by subtracting the actual rotation angle from the command rotation angle of the servo motor 12. The position deviation 13 and the rotation angle setting value (working coordinate system setting value) 14 are compared by the comparison means 15 to detect when the position deviation 13 exceeds the rotation angle setting value 14.

Furthermore, as a third conventional technique, there is a method using a state estimation observer as a method of detecting the contact of the robot with the peripheral devices described above and stopping the robot (for example, Patent Document 2).
FIG. 10 is a diagram showing a basic configuration of the robot control device in Patent Document 2 as the third prior art.
In FIG. 10, 21 is a servo motor control means, 22 is a servo motor, 23 is a robot arm, 24 is a two-inertia state estimation observer, 25 is a contact determination means, 26 is a device protection means, and 27 is a speed reducer. The two-inertia state estimation observer 24 estimates the load-side disturbance force using the motor rotation angle, the motor rotation angular velocity, and the motor acceleration command. The contact determination unit 25 determines that the robot arm 23 has contacted an external device when the estimated disturbance force exceeds a set threshold value, and outputs a signal to the device protection unit 26.

As another method, as a fourth conventional technique, a method of detecting contact of a robot from disturbance of a current waveform flowing through a motor is disclosed (Patent Document 3).
Japanese Unexamined Patent Publication No. 09-179632 (Specification, page 18, FIG. 7) Japanese Unexamined Patent Publication No. 2000-52286 (Specification, page 6, FIG. 1) Japanese Patent Laid-Open No. 02-059291 (Specification, page 3, FIG. 1)

The conventional robot control apparatus described above has the following problems.
As in the first prior art, in the method of setting the robot reachable range to prevent contact with peripheral devices, the robot work environment changes and the surroundings are within the preset reachable range. When equipment enters, there is a possibility of contact between the robot and peripheral equipment, and there is a possibility that both of them break down at the time of contact.

Further, as in the second prior art, in the method of detecting the contact with the peripheral device of the robot by comparing the actual joint response of the robot and the actual position response with the joint angle command or the position command given to the robot, the response of the actual device Because there is a servo lag with respect to the position command, if the joint angle command or position command changes suddenly, the difference between the actual machine joint response and the actual machine position response temporarily increases. There was a possibility of becoming.
Here, FIG. 5 shows an example of a position response waveform in one direction for explaining the third prior art.
In the figure, 501 is a position command, 502 is a position response corresponding to the position command 501, and 503 is a maximum value of the difference between the position command and the position response. In general, a position command on a step like 501 is not given, but it is used as an example for clarifying the problem. If the command changes abruptly, the robot cannot follow the change in the command even if the robot does not come into contact with the peripheral device, so that the difference between the position command and the position response becomes large. This is due to a servo delay. When the difference between the position command and the position response becomes large and exceeds a threshold value, a false detection occurs. Furthermore, in the method of comparing the actual joint response of the robot with the joint angle command given to the robot, the threshold value must be given as an angle, so that it is not suitable for appropriately limiting the robot movement range in the work space. Further, the method of comparing the actual position response of the robot with the position command given to the robot has a drawback that contact detection cannot be performed with high sensitivity when a link other than the hand contacts a peripheral device.

  Further, in the method of estimating the disturbance force by the state estimation observer as in the third prior art, there is a problem that detection is delayed by the time constant of the observer because the observer follows the actual machine response. There is also a problem that the calculation formula becomes complicated and the calculation time becomes long.

  In addition, as in the fourth prior art, the method of detecting contact from the disturbance of the current waveform also detects a sudden torque increase when the position command changes suddenly, resulting in false detection. It was.

The present invention has been made in view of the above problems, and the calculation formula is simple, the processing time is short, and even when the operation command changes suddenly, it is not unexpectedly detected without being erroneously detected as a peripheral device. contacting an object to provide a control device and robot system of the robot can be detected with high sensitivity over the entire robot arm.

In order to solve the above problems, the present invention is configured as follows.
The invention according to the controller of the robot according to claim 1, controlling a robot having a plurality of links, and means for controlling the output of the joint of the actuator between the respective link, and means for measuring the displacement amount of each joint A control arithmetic unit that calculates and outputs a control input value to the actuator, a robot model that simulates the robot, a control arithmetic model unit that simulates the control arithmetic unit, and each joint A link position / orientation calculation unit that calculates the position and orientation of at least one of the links to be monitored by the robot based on a displacement amount, and the monitoring object based on a model joint displacement amount of the robot model. A model link position / orientation calculation unit that calculates the position and orientation of the model link corresponding to the link to be linked, and the link calculated by the link position / orientation calculation unit And position of the link, that and a position and orientation comparing section for outputting a contact detection signal when the absolute value of the difference between the position of the model links calculated in the model links the position and orientation calculation unit exceeds a threshold value It is characterized by.
The invention according to the robot control apparatus of claim 2 controls a robot having a plurality of links, means for controlling an output of an actuator of a joint between the links, and means for measuring a displacement amount of each joint. A control arithmetic unit that calculates and outputs a control input value to the actuator, a robot model that simulates the robot, a control arithmetic model unit that simulates the control arithmetic unit, and each joint A link position / orientation calculation unit that calculates the position and orientation of at least one of the links to be monitored by the robot based on a displacement amount, and the monitoring object based on a model joint displacement amount of the robot model. A model link position / orientation calculation unit that calculates the position and orientation of the model link corresponding to the link to be linked, and the link calculated by the link position / orientation calculation unit A position and orientation comparison unit that outputs a contact detection signal when an absolute value of a deviation between a link position and a model link position calculated by the model link position and orientation calculation unit exceeds a threshold, and the position and orientation The calculating unit calculates a position / orientation vector having six components indicating the position and orientation of the link to be monitored, and the model link position / orientation calculating unit calculates six positions and orientations of the model link. A model link position / orientation vector having a component is calculated, and the position / orientation comparison unit individually monitors the difference between each component of the link position / orientation vector and each component of the model link position / orientation vector, and The touch detection signal is output when at least one of the magnitudes of the differences exceeds a threshold value.

A third aspect of the present invention, switches Oite to claim 1 or 2, notifying means for notifying the operator the results in accordance with the comparison result of the position and orientation comparator unit, stopping means for stopping the robot, an operation command of the robot At least one of the operation command switching means or the control input switching means for switching the control input of the robot is provided.

A fourth aspect of the present invention, Oite to any one of claims 1 to 3, further comprising a model operation visualization means for providing to the operator in synchronization with the operation of the robot operation of the robot model as an image It is a feature.

The invention of claim 5 is have you to any one of claims 1-4, wherein the robot model is characterized by constituting side by side 1 inertia model the same number and the number of joint robot.

The invention related to the robot system according to claim 6 is characterized by comprising a robot and the control device for the robot according to any one of claims 1 to 5 .
A seventh aspect of the invention is characterized in that in the robot system of the sixth aspect , the robot has redundant degrees of freedom.

According to the invention described in claim 1 or 6 , since the joint displacement amount of the robot is affected by the contact, the model joint displacement amount is not affected by the contact. Therefore, contact detection can be performed by comparing the two. it can. Since the robot model can simulate the servo delay of the actual machine part, there is also an advantage that there is no false detection due to a sudden change in the command value to the robot. In addition, by monitoring the position and orientation of a plurality of links, it is possible to detect with high sensitivity even when a contact with a peripheral device occurs in a portion other than the hand. Further, it is possible to reliably detect when the hand position / posture of the actual machine part deviates from the hand position / posture of the robot model due to the contact. In addition, since the allowable deviation distance in the work space of the link position / posture of the robot and the robot model is a threshold value, the threshold value can be easily set.
According to the second aspect of the present invention, it is possible to reliably detect contact in one direction and allow contact in another direction. The contact detection threshold can be individually set for each component of the robot link position and orientation.

According to the third aspect of the invention, it is possible to reliably take measures to reduce damage to the robot and peripheral devices when contact occurs.

According to the invention described in claim 4 , the operator can observe the operation state of the robot model simultaneously with the operation state of the robot. By comparing the two, the operator can confirm whether the robot is operating normally. If there is an abnormality, the operator can easily find the abnormal part.

According to the fifth aspect of the present invention, the actual machine part of the robot can be simulated by a very simple model. The amount of calculation for updating the state quantity of the robot model can be reduced.

According to the seventh aspect of the present invention, it is possible to monitor the presence or absence of contact with peripheral devices over the entire robot arm even with respect to the redundant robot arm, as compared with the case where only the joint angle and the hand position / posture are monitored. Contact can be detected with high sensitivity.

  Embodiments of the present invention will be described below with reference to the drawings.

FIG. 1 is a block diagram of a robot control apparatus showing an embodiment of the present invention.
In the figure, 101 is a robot to be controlled, 102 is a robot model simulating the robot 101, 103 is a control calculation unit, 104 is a control calculation model unit, 105 is a link position / posture calculation unit, and 106 is a model link position / posture calculation unit. 107 is a position / orientation comparison unit, 108 is a device protection unit, 109 is a notification unit, and 110 is a motion visualization unit that provides a motion image of the robot model. 111 is the target joint displacement amount of the robot 101, 112 is the joint displacement amount, 113 is the model joint displacement amount, 114 is the control input value to the actuator, 115 is the control input value to the robot model, and 116 is the monitoring target 1 A link position / posture indicating the position and posture of one or more links, 117 is a model link position / posture which is a signal corresponding to the link position / posture in the robot model 102, and 118 is a contact detection signal. The robot model 102 and the control calculation model unit 104 are configured to simulate the response characteristics of the joint displacement amount 112 with respect to the target joint displacement amount 111. The robot 101 has means for controlling the output of the actuator of each joint and means for measuring the amount of displacement of each joint (all not shown).

FIG. 2 is a block diagram of a control system for explaining the details based on FIG. That is, the configurations of the robot 101, the robot model 102, the control calculation unit 103, and the control calculation model unit 104 shown in FIG. 1 correspond to the configurations 101 ′, 102 ′, 103 ′, and 104 ′ in FIG.
In FIG. 2, 101 ′ is a robot arm having n rotary joints and a motor capable of controlling the torque of each joint, and 102 ′ is a one-inertia system that simulates the response characteristics of the joint angle with respect to each joint torque of the robot arm 101 ′. An arm model in which n models are arranged, 103 ′ is a control arithmetic unit for controlling each joint angle of the robot arm 101 ′, 104 ′ is a control arithmetic model unit, and 105 ′ is k link positions and orientations to be monitored. Is a model link position / posture calculation unit corresponding to the k link positions / postures to be monitored, and 107 ′ is a position / posture comparison unit. Reference numeral 108 'denotes a brake serving as a device protection means for stopping the robot, 109' denotes an alarm for notifying the operator of contact detection, and 110 'denotes a monitor for providing the motion of the model as a moving image.
Further, when the brake 108 'receives the contact detection signal 118', it immediately sends a stop signal to the robot arm 101 'to stop the operation of the joint portion. As a result, the peripheral arm that the robot arm keeps moving and the destruction of the robot arm itself are prevented.
Further, the operation of the arm model 102 'is displayed on the monitor 110' in real time to assist the operator in grasping the situation. The operator can compare the robot arm 101 ′ and the arm model 102 ′ to find an abnormality, and easily determine what action the robot has made as a result of the robot's action during contact detection. I can know. Even when the distance between the operator and the robot is large, the operation status of the robot can be grasped to some extent by the display on the monitor 110 '.

For the following description, a mathematical expression of the signal in the figure is given.
111 ′ is a joint angle command of the robot arm 101 ′, and θ ^r = [θ ₁ ^r ,..., Θ _n ^r ] ^T (1)
112 ′ is a joint angle of the robot arm 101 ′, and θ = [θ ₁ ,..., Θ _n ] ^T (2)
113 ′ is the joint angle of the model, and θ ^m = [θ ₁ ^m ,..., Θ _n ^m ] ^T (3)
114 ′ is a torque command value to the motor, and τ = [τ ₁ ,..., Τ _n ] ^T (4)
115 ′ is a torque input value to the robot model, and τ ^m− = [τ ₁ ^m ,..., Τ _n ^m ] ^T (5)
It expresses .
1 16 'represented by the following formula the position and orientation vectors of the position and orientation of the link about the k links to be monitored link i (6).
X ₁ ,..., X _k , X _i == [x _i ¹ , x _i ² , x _i ³ , x _i ⁴ , x _i ⁵ , x _i ⁶ ] ^T , i = 0,. (6)
1 17 'link position and orientation 116' is the position and orientation of the model links corresponding to, expressed by the following equation the position and orientation vectors of the model links (7).
_{^{X m 1, ···, X m}} k, X m i == [x i m1, x i m2, x i m3, x i m4, x i m5, x i m6] T, i = 0, ·· ., K (7)
Respectively. Reference numeral 118 'denotes a contact detection signal. Reference numeral 221 denotes a torque applied to the first joint of the robot arm, which is a first component of the torque command value 114 ′. A torque 222 is applied to the nth joint of the robot arm and is an nth component of the torque command value 114 ′. 223 is a first component of the joint angle 112 ′, and 224 is an n-th component. Reference numeral 225 denotes a first component of the torque input value 115 ′ to the robot model, and 226 denotes an nth component. 227 is the first component of the joint angle 113 ′ of the model, and 228 is the nth component.

  2, 201 is a position control gain circuit, 202 is a differentiator, 203 is a speed control gain circuit, 204 is an output limiter, 205 is a gravity / friction force compensator, and 206 is an output. It is a conversion constant circuit. By configuring the control calculation unit in this way, the robot arm 101 ′ tries to follow the joint angle command 111. However, when the joint angle command changes rapidly, the robot arm 101 'follows the target with a delay, that is, there is a servo delay, so the difference between the joint angle command 111' and the joint angle 112 'is temporarily different. May increase.

In the arm model 102 'in FIG. 2, 211 is a one-inertia rigid body model that simulates the response to the applied torque command of the first joint of the robot arm, and 212 simulates the response of the robot arm to the applied torque of the nth joint. This is a one-inertia rigid body model. The input / output relationship of the 1-inertia rigid body model of the i-th joint is as follows.
L (θ _i ^m ) = 1 / (j _i s ² ) · L (τ _i ) i = 0,..., N (8)
Here, L (•) represents a Laplace transform operator, and s is Laplace transform s. J _i is an equivalent moment of inertia around the i-th joint of the robot arm. Actually, the moment of inertia around the joint of the robot arm varies in a complicated manner depending on the arm posture, but the model represents it by a constant J _i . Reference numerals 207 to 210 constitute the control calculation model unit 104, 207 is a position control gain circuit, 208 is a differentiator, 209 is a speed control gain circuit, and 210 is an output limiter.

Therefore, since the robot control apparatus according to the first embodiment is configured as the arm model 102 ′ and the control calculation model unit 104 ′ as described above, the joint angle command 111 in the control calculation unit 103 ′ and the robot arm 101 ′ is used. The response of 'joint angle 112' to 'can be simulated. As a result, if contact between the robot arm and the peripheral device does not occur, the joint angle 113 of the model substantially coincides with the joint angle 112 ′.
FIG. 6 is a diagram showing examples of position response waveforms of the robot and robot model in one direction of one link for explaining the embodiment of the present invention. In the figure, reference numeral 601 denotes a model position response waveform. Even if the position command changes abruptly, there is little difference between the position response and the model position response, so no erroneous detection occurs. When the robot arm receives a contact force, the link position / orientation 116 ′ is affected, but the model link position / orientation 117 ′ is not affected by the contact, so that a divergence occurs between the two. This is monitored by the position / orientation comparison unit to detect contact.

Next, a second embodiment of the present invention will be described.
FIG. 3 is a block diagram of the position / orientation comparison unit 105 ′ according to the second embodiment of the present invention (Configuration Example 1).
In the following, among the links of the robot arm 101 ′, k links to be monitored are numbered from 1 to k, and are expressed as links i (i = 0,..., K), respectively. In the configuration example 1, the position / orientation comparison unit 105 ′ is arranged in parallel from the position / orientation comparison unit of the link 1 to the position / orientation comparison unit of the link k. The corresponding link position / posture and model link position / posture are input to the position / posture comparison unit of each link, and a contact detection signal is output based on the result of the comparison operation performed in parallel.
In FIG. 3, 301 is a position and orientation comparison unit of link 1, 302 is a position and orientation comparison unit of link i, 303 is an absolute value calculation unit, 304 is a comparison calculation unit, 311 is a position and orientation of link 1, and 312 is a link i Position and posture 313, Position and posture of link k, 314, Position and posture of model link 1, 315, Position and posture of model link i, 316, Position and posture of model link k, 317, Absolute value of position and posture deviation of link i, 318 Is the threshold δ _i i = 1,..., K (9) of the link i.
It is. First, the difference between the input position and orientation of the link i and the position and orientation of the model link i is calculated, and the absolute value calculation means 303 calculates the absolute value 317 of the position and orientation deviation of the link i by the following equation.
^{_{| X i - X m i |}} = (X i - X m i ^{_{) T (X i - X m}} i ^1/2 ... (10)
The comparison calculation means 304 compares the absolute value 317 of the position and orientation deviation of the link i with the threshold value 318 of the link i,
^{_{δ i <| X i - X}} m i | ··· (11)
When this happens, a contact detection signal is output.
Therefore, in the second embodiment, if the state comparison unit 105 ′ is configured as described above, it is possible to accurately detect contact between the robot arm and the peripheral device by a simple calculation.

FIG. 7 is a schematic diagram showing a comparison with a robot arm model when contact is made between the hand of the robot arm and a peripheral device.
In the figure, 101 ″ is a three-link robot arm, 102 ″ is a corresponding arm model, 701 is a peripheral device, 711 is a link 1 absolute position deviation value, 712 is a link 2 absolute position error value, and 713 is a link. 3 is a position deviation absolute value.
Since the link 3 is affected by contact with peripheral devices, the divergence from the model link 3 that is not affected by contact is large. In the figure, the deviation of only the position is displayed, but the divergence between the robot arm and the model also occurs in the link posture. A contact detection signal is output when the absolute value of the deviation exceeds a threshold value.

Next, a third embodiment of the present invention will be described (Configuration Example 2).
FIG. 4 is a block diagram of the state comparison unit 105 ′ showing the third embodiment of the present invention (configuration example 2).
The third embodiment differs from the second embodiment in that the state comparison unit 105 ′ calculates the position and orientation deviations of the robot arm and the arm model for each component and compares them with individual threshold values. Note that, from the position and orientation comparison unit of the link 1 to the position and orientation comparison unit of the link k are arranged in parallel as in the configuration example 1.
In FIG. 4, 401 is a position and orientation comparison unit of link 1, 402 is a position and orientation comparison unit of link i, 403 is an absolute value vector calculation unit, 404 is a component comparison calculation unit, 411 is a threshold vector of link i [δx _i ¹ , Δx _i ² ,..., Δx _i ⁶ ] ^T (12)
Reference numeral 412 denotes a position / posture deviation absolute value vector [| x _i ¹ −x _i ^m1 |, | x _i ² −x _i ^m2 |,..., | X _i ⁶ −x _i ^m6 |] ^T.・ (13)
It is. First, the difference between the input position and orientation of the link i and the position and orientation of the model link i is calculated, and the absolute value vector calculation means 403 calculates the absolute value of each component of the link i position and orientation deviation as shown in Equation 13. The component comparison calculation means 404 compares the position / posture deviation absolute value vector 412 of the link i with the threshold vector 411 of the link i,
δx _i ^j <| x _i -x _i ^mj | (14)
Is satisfied for at least one of j = 1, 2,..., 6, a contact detection signal is output.
Therefore, in the third embodiment, as described above, it is possible to detect contact in a certain direction with high sensitivity and not to detect contact in a certain direction. When the robot performs work that involves contact with peripheral devices, the threshold for the direction in which contact occurs with the work is increased so that unnecessary contact detection does not occur. The threshold may be set as usual and an unexpected contact may be reliably detected.

Next, another effect of the present embodiment will be described with reference to FIG.
FIG. 8 is a schematic diagram showing a comparison with a robot arm when a link in the middle of a multi-degree-of-freedom robot arm contacts a peripheral device.
In FIG. 8, 801 is a multi-degree-of-freedom robot arm having redundant degrees of freedom, 802 is a corresponding arm model, 803 is a link 1 for monitoring position / posture, 804 is a link 2 for monitoring position / posture, 805 is a peripheral device Represents.
A part of the robot arm 801 including the link 804 is largely separated from the arm model 802 due to contact with the peripheral device 805, but the robot arm 801 and the arm model 802 are substantially coincident with each other regarding the link 803.
Therefore, only the link 803, that is, the position / posture monitoring of only the hand cannot detect contact with peripheral devices as shown in the figure. However, if the number of links to be monitored is increased to two, 803 and 804, and the position and orientation of the position / posture monitoring link 2 are compared and monitored with the actual unit and model, the robot arm Contact with peripheral devices can be detected with high sensitivity. As a result, the presence or absence of contact can be monitored over the entire robot arm, and contact can be detected with higher sensitivity than when only the joint angle and the position / posture of the hand are monitored. The robot model can also be used to verify whether speed, torque, and the like can be achieved in addition to the position by giving a command only to the robot model in advance before actually operating the robot. This makes it possible to confirm in advance whether the robot does not move unexpectedly. This is particularly effective in preventing sudden movements due to abnormal solutions in robots with redundant degrees of freedom.

Finally, the points that the present invention differs from Patent Documents 1 to 3 corresponding to the prior arts 2 to 4 are summarized and described below.
The present invention differs from Patent Document 1 in the following two points.
(1) A robot model that simulates a robot is provided, and the response of the robot is compared with the response of the robot model. Thus, by using the robot model, the state where the robot should be can be obtained from the robot model, and the contact detection with high accuracy can be performed by comparing it with the actual state of the robot.
Further, since the robot model also simulates a response delay with respect to the target joint displacement amount of the robot, it is possible to solve the problem of the method of Patent Document 1 in which erroneous detection can be performed when the command changes rapidly.
(2) In the position / orientation comparison unit, the difference between the link position / orientation and the robot model is monitored for a plurality of links. As a result, the entire robot can be monitored, and highly sensitive detection is possible even when parts other than the hand touch.

  The present invention is different from Patent Document 2 in that contact detection is performed by comparing the response of an actual machine and the response of a model without performing disturbance force estimation by an observer. Thereby, the calculation required for the comparison is very simple, and contact detection can be performed with a small amount of calculation.

  The present invention differs from Patent Document 3 in that a robot model that simulates a robot is provided, and the response of the robot and the response of the robot model are compared with respect to the position and orientation of a plurality of links. This solves the problem of Patent Document 3 in which a sudden change in the position command is erroneously detected.

  In the first embodiment, the control calculation unit 103 ′ and the control calculation model unit 104 ′ do not need to have the same structure, and the robot arm 101 and the arm model 102 do not need to be exactly the same. It is only necessary that the response characteristic of the joint angle 113 'of the model with respect to the joint angle command 111' can simulate the response characteristic of the joint angle 112 'with respect to the joint angle command 111'.

The alarm 109 'is used to turn on an alarm lamp when a contact detection signal is received and notify the operator of contact detection. In addition to the lamp, a buzzer, a character display on the operation panel screen, or an operator is used. Anything that can be detected is acceptable.

By comparing the robot and robot model for the position and orientation of multiple links with a model that simulates the robot and the control calculation unit, contact detection can be performed with a small amount of calculation regardless of changes in the target joint displacement, and a new Since it does not require a sensor, it can be widely applied to robots that can come into contact with the surrounding environment, such as many industrial robots and home robots.

1 is a block diagram of a robot control apparatus showing a first embodiment of the present invention. Block diagram of the control system for explaining in detail based on FIG. Block diagram of a state comparison unit showing a second embodiment of the present invention (configuration example 1) Block diagram of a state comparison unit showing a third embodiment of the present invention (configuration example 2) The figure which showed the example of the position response waveform in one direction for demonstrating the 3rd prior art The figure which showed the example of the position response waveform of the robot in one direction of one link for describing the Example of this invention, and a robot model Schematic diagram showing comparison between robot and arm model when contact between robot arm hand and peripheral device occurs Schematic diagram showing comparison between robot and arm model when a link in the middle of a multi-degree-of-freedom robot arm is in contact with a peripheral device The block diagram which showed the circuit structure for 1 axis | shaft of the control system of the motor mounted in the robot in patent document 1 used as 2nd prior art The figure which showed the basic composition of the robot control apparatus in patent document 2 used as the 3rd prior art

Explanation of symbols

101 Robot 101 ′ Robot arm 101 ″ Three-link robot arm 102 ′, 102 ″, 802 Arm model 102 Robot model 103, 103 ′ Control calculation unit 104, 104 ′ Control calculation model unit 105, 105 ′ Link position / posture calculation unit 106, 106 ′ Model link position / orientation calculation unit 107, 107 ′ Position / orientation comparison unit 108 Device protection unit 108 ′ Brake 109 Notification unit 109 ′ Alarm 110 Operation visualization unit 110 ′ Monitor 111 Target joint displacement amount 111 ′ Joint angle command 112 Joint Displacement amount 112 'Joint angle 113 Model joint displacement amount 113' Model joint angle 114 Control input value 114 'to actuator 114 Torque command value 115 Control input value 115' to robot model Torque input value 116, 116 'to robot model Link position / posture 117, 17 'Model link position and orientation 118, 118' Contact detection signal 201, 207 Position control gain circuit 202, 208 Differentiator 203, 209 Speed control gain circuit 204, 210 Output limiter 205 Gravity / friction force compensator 206 Output conversion constant circuit 211 One-inertia rigid body model 212 that simulates response to applied torque of first joint 212 One-inertia rigid body model 221 that simulates response to applied torque of n-th joint First component 222 of torque signal 114 n-th component of torque signal 114 223 First component 224 of actual machine joint angle 112 224th nth component 225 of actual machine joint angle 113 First component 226 of torque signal 115 to arm model 227 nth component of torque signal 115 to arm model 227 First joint angle 228 nth joint angles 301, 40 of the arm model Link 1 position / orientation comparison unit 302, 402 Link i position / orientation comparison unit 303 Absolute value calculation unit 304 Comparison operation unit 311 Link 1 position / orientation 312 Link i position / orientation 313 Link k position / orientation 314 Model link 1 position Posture 315 Position / posture 316 of model link i Position / posture 317 of model link k Position / posture deviation absolute value 318 of link i Threshold value 403 of link i Absolute value vector calculation means 404 Component comparison calculation means 411 Threshold vector 412 of link i Position / attitude deviation absolute value vector 501 Position command 502 Position response 503 Maximum difference 601 between position command and position response Model position response 701 Peripheral device 805 Peripheral device 711 Link 1 position deviation absolute value 712 Link 2 position deviation absolute value 713 Link 3 absolute value of position deviation 801 Freedom robot arm 803 Link 1 for monitoring the position and orientation
804 Link 2 for monitoring the position and orientation

Claims (7)

A control device for controlling a robot having a plurality of links, means for controlling the output of an actuator of a joint that moves the links, and means for measuring a displacement amount of each joint,
A control operation unit that calculates and outputs a control input value to the actuator;
A robot model for simulating the robot;
A control calculation model unit that simulates the control calculation unit;
A link position and orientation calculation unit that calculates the position and orientation of at least one of the links to be monitored by the robot based on the displacement amount of each joint;
A model link position / orientation calculating unit that calculates a position and orientation of a model link corresponding to the link to be monitored based on a model joint displacement amount of the robot model;
Position and orientation that outputs a contact detection signal when the absolute value of the deviation between the position of the link calculated by the link position and orientation calculation unit and the position of the model link calculated by the model link position and orientation calculation unit exceeds a threshold value And a control unit for the robot. A control device for controlling a robot having a plurality of links, means for controlling the output of an actuator of a joint that moves the links, and means for measuring a displacement amount of each joint,
A control operation unit that calculates and outputs a control input value to the actuator;
A robot model for simulating the robot;
A control calculation model unit that simulates the control calculation unit;
A link position and orientation calculation unit that calculates the position and orientation of at least one of the links to be monitored by the robot based on the displacement amount of each joint;
A model link position / orientation calculating unit that calculates a position and orientation of a model link corresponding to the link to be monitored based on a model joint displacement amount of the robot model;
A position and orientation comparison unit that compares the position and orientation of the link calculated by the link position and orientation calculation unit with the position and orientation of the model link calculated by the model link position and orientation calculation unit and outputs a result. ,
The position / orientation calculation unit calculates a position / orientation vector having six components indicating the position and orientation of the link to be monitored;
The model link position / orientation calculation unit calculates a model link position / orientation vector having six components indicating the position and orientation of the model link;
The position and orientation comparison unit
When the magnitude of the difference between each component of the link position and orientation vector and each component of the model link position and orientation vector is individually monitored, and at least one of the magnitudes of the differences exceeds a threshold value A robot control device that outputs a contact detection signal. A notification means for notifying an operator of the result according to the comparison result of the position / orientation comparison section, a stop means for stopping the robot, an operation command switching means for switching the operation command of the robot, or a control input switching means for switching the control input of the robot. The robot control device according to claim 1, wherein at least one of them is provided. The robot control apparatus according to any one of claims 1 to 3, further comprising model motion visualization means that provides the operator with the motion of the robot model as an image in synchronization with the motion of the robot. 5. The robot control apparatus according to claim 1, wherein the robot model is configured by arranging one inertial system model in the same number as the number of joints of the robot. 6. With robots,
A robot system comprising: the robot control device according to claim 1. The robot system according to claim 6, wherein the robot has redundant degrees of freedom.

Images