JPH0438548B2 - - Google Patents

Description

Detailed Description of the Invention [Field of Application of the Invention] The present invention relates to a method for controlling a multi-degree-of-freedom robot using a non-contact sensor, and particularly relates to a method for controlling a robot with multiple degrees of freedom using a non-contact sensor. The present invention relates to a control method for a multi-degree-of-freedom robot in which when it is detected that the distance is within a certain range, the previous control mode is changed.

[Background of the invention]

The reality is that a multi-degree-of-freedom robot has not been actively controlled using non-contact sensors. However, at the research level, it is only rare to attach a proximity sensor (using ultrasonic waves or eddy current) to a multi-degree-of-freedom robot. However, even when a proximity sensor is installed, it only functions as a safety device, and is mostly used to control an emergency stop when the robot's hand approaches some kind of obstacle.

[Purpose of the invention]

SUMMARY OF THE INVENTION Therefore, an object of the present invention is to provide a method for controlling a multi-degree-of-freedom robot using a non-contact sensor, in which the control mode of the multi-degree-of-freedom robot can be changed in various ways based on sensor signals from the non-contact sensor.

[Summary of the invention]

For this purpose, the present invention provides that when a non-contact sensor detects that the distance between the robot hand and some movable obstacle is within a certain range, The control is changed. This not only makes it possible to stop the robot hand while maintaining a certain distance from an obstacle, but also allows the robot hand to follow the movement while maintaining a certain distance if the obstacle moves. is possible.

[Embodiments of the invention]

The present invention will be explained below with reference to FIGS. 1 to 15.

First, an overview of the multi-degree-of-freedom (1 to 6 degrees of freedom) robot according to the present invention will be explained. Figure 1 shows its appearance. According to this, hand part 2
A non-contact sensor 3 is mounted between the arm section 1 and the arm section 1, but the mounting location is not limited to this, and it can also be mounted on the hand section 2, for example. Since the object to be controlled is the hand section 2, there is no particular problem as long as information such as the position and posture of the hand section 2 can be directly obtained. Therefore, as will be described later, it is also possible to attach it to a location other than the robot.

FIG. 2 shows the appearance of the hand section and its surroundings when an ultrasonic sensor is used as a non-contact sensor. The ultrasonic sensor consists of an ultrasonic transmitter and a receiver, which are mounted in the positive and negative directions of the x, y, and z axes. When mounted in this manner, the ultrasonic sensor facing the positive direction x ₊ of the x-axis can measure the distance to an object present in that direction. Other non-contact sensors that can be used include a combination of an LED and a phototransistor, an eddy current sensor, and a length measuring device that uses a semiconductor laser. In short, any device that can measure distance without contact is sufficient.

Figure 3 shows the ultrasonic sensor in the x-axis positive direction x ₊
This figure shows how the distance Δlx ₊ between the target object 4 and the target object 4 is being measured. This does not require any special explanation, but the non-contact sensor does not necessarily need to be attached to a robot, and can be attached to other objects besides robots. FIG. 4 shows a case where a TV camera as a non-contact sensor is attached to the upper part of the robot and near the object. As shown in the figure, it is mounted on the ceiling, and when mounted in this way, it is possible to not only measure distances in a coordinate system fixed to the hand part 2, but also to measure distances in a coordinate system fixed to the base, which is a non-movable part. Distance can also be measured. When using a TV camera, not only the distance between the hand section 2 and the object 4 but also the distance between the hand section 2 and the object 4 are measured.
The attitude of the object 4 with respect to the object 4 can also be measured, for example, as a direction cosine.

Now, we will explain how the robot is controlled based on information from non-contact sensors and the like.

FIG. 5 shows an outline of the control system, which allows a position signal and a distance signal Δ between the robot and an object to be obtained from the robot equipped with a non-contact sensor. The position signal θ is obtained as a current joint angle from an encoder attached to each joint of the robot, and a distance signal to an object is obtained from a non-contact sensor. As shown in the figure, the control section 5
The position command value _r and distance reference value Δc are input to the controller 5, and the angular velocity θ for each joint is calculated by the control unit 5 according to these, the position of the robot, and the distance between the robot and the object. 6, each joint is driven.

FIG. 6 shows what is shown in FIG. 5 in more detail. According to this, the position command value _r and the distance reference value Δc are given to the control unit 5 from the outside, while the robot 14 side receives the distance Δ between the robot and the object from the non-contact sensor 3, and also receives the distance Δ from the encoder 11. Each joint angle is given from a counter 12 that counts pulse signals. Each joint angle is converted into the position/attitude of the robot hand by the converter 10 and then input to the main controller 7, which calculates the speed that the robot hand should take. be. Since this speed is in the coordinate system fixed to the robot hand (translational speed and rotational speed), the coordinate conversion unit 8 converts the speed _b in the coordinate system fixed to the base based on the position/posture and each joint angle. It is starting to be converted to . Based on this velocity _b , the joint angular velocity calculation unit 9 calculates the angular velocity θ that each joint should take, but depending on these angular velocity θ, the servo amplifier 6
The motor 13 and, therefore, the robot 14 are driven through the motor 13 and, therefore, the robot 14.

As described above, the main control section has an extremely important function, and this function will be explained in detail as follows.

That is, in general, the motion of a rigid body with one degree of freedom in the absence of external force is expressed by equation (1).

mdv/dt+cv+k(x- _r x)=0...(1) However, m is the mass, c is the viscosity coefficient, k is the spring coefficient, x is the position, v is the velocity, and _r The equilibrium positions are shown respectively.

In this equation (1), when v _t=0 = 0 and x _t=0 = 0, a step input _{( rs x}・u
(t)), the motion is as follows.

x= _rs x{1−e ^pt (cosqt−p/qsinqt)}……(
2) However, p=-c/2cm and q=4mk- ^c2 .

As inferred from equation (2), the motion shown in equation (1) is _r
The position x can be controlled by manipulating x, but this is the basic function of the main control unit when Δc and Δ are excluded, that is, when there are no parameters related to the non-contact sensor. The robot is controlled so that the motion of the robot's hand becomes the motion of a rigid body given by equation (1).

That is, the speed that the robot hand should take is calculated by the main control section using equation (3).

v=1/m∫{-cv-k(x- _r x)}dt...(3) Equation (3) can be derived by solving equation (1) for v, but equation (3) If the robot is equipped with a speed detector such as a tachogenerator, v in the right-hand side is taken as the speed detection signal (feedback signal). However, as shown in FIG. 6, if the robot is not equipped with any speed detector, v on the right side of equation (3) is the value calculated by itself using equation (3).

By the way, in an actual system, calculations are processed as a discrete value system in a computer, so instead of formula (3), formula
(4) is now being used.

v _o = Δt/m{−k(x _o-1 − _r x _o-1 )} −(Δt/mc−1) v _o-1 …(4) However, Δt is the sampling time, and n is the value at the nth sampling time, and _o and _o-1 are the values at the next and next sampling time, respectively.
It acts as a previous integral effect signal.

Therefore, when expanded to multiple degrees of freedom, Equation (3),
(4) are converted and expressed as equations (5) and (6), respectively.

= [M] ^-1 ∫[-[C]-[K](- _r )]d
t
...(5) _o = Δt[M] ^-1 {−[K] ( _o-1 − _{r o-1} )} −(Δt[M] ^-1・[C]−[I]) _o-1 ... ...(6) However, [M], [C], and [K] are matrices corresponding to the mass, viscosity coefficient, and spring coefficient, respectively, and
[I] indicates an identity matrix. matrix [M],
[C] and [K] are usually diagonal matrices and do not have values that the actual robot mechanism has, but they can be set freely within a certain range as virtual control parameters on the software. It is now possible to exercise. That is, by allowing these virtual control parameters to be changed within a certain range, the stopping characteristics (damping rate, natural frequency), follow-up characteristics, etc. can be adjusted preferably.
Further, the position (translational position and rotational angle) and velocity (translational speed and rotational speed) are vectors, and are usually six-dimensional.

As described above, the position of the robot can be controlled using equations (5) and (6), and FIG. 7 shows equation (5) as a block diagram. However, in this case, the parameters Δ and Δc are ignored. In the judgment calculation processing block 15, ( _r- ) is normally calculated. Incidentally, an outline of the behavior shown by equation (2) is shown as part of FIG. 8.

Now, a description will be given of a robot control method according to the present invention, which adds control using a non-contact sensor to the robot position control performed as described above.

Figure 9 shows that while controlling the position of a robot in a work area, if the robot enters a dangerous area, a non-contact sensor is used to prevent the robot from entering the dangerous area. It is something to do. As mentioned above, the robot's position is controlled by manipulating _r , but from this,
If _r is taken within the workspace space, it is expected that the robot will remain approximately within the workspace space no matter how it is operated. However, in reality, due to delays in the set parameters [M], [C], [K] or the robot drive system, or due to malfunctions or disturbances, the robot may move out of the work area, that is, into the dangerous area. It is possible that it may enter.
In the example shown in FIG. 9, walls 16,
17 and the distance from the walls 16 and 17 is within Δlc is set as a dangerous area. Therefore, the robot requires at least two non-contact sensors to measure the distance between the walls 17 and 16 in the positive and negative directions of the x-axis, and these non-contact sensors allow the robot hand to It is known that the person has invaded one of the dangerous areas.
For example, assuming that the robot hand is approaching the wall 17, the distance Δl _x+ from the wall 17 is Δl _x+ <
It is known that a dangerous area has been entered when the value becomes Δlc. If the robot hand enters the dangerous area, it is necessary to push the robot hand back so that Δl _x+ becomes Δlc. As shown in FIG. 10, when it is detected that the vehicle has entered a dangerous area at a certain time tc, the robot immediately takes the behavior shown in FIG. 8 to smoothly escape from the dangerous area. Of course, it is also possible to make an emergency stop at that point tc, as has been done in the past, or to make the vehicle escape from the dangerous area not smoothly but immediately with a large acceleration.

By the way, the above-described control using a non-contact sensor can be easily achieved by adding a feedback signal from the non-contact sensor to the control method described above. FIG. 7 shows the block configuration of the main control section, and the judgment calculation processing block 15, which performs an important function, will be explained below.

That is, as shown in FIG. 7, _r and (Δc - Δ) are input to the judgment calculation processing block, of which Δc and Δ are composed of six components as shown in equations (7) and (8). It has become a thing.

Δc=Δlc, x ₊ Δlc, x _- Δlc, y ₊ Δlc, y _- Δlc, z ₊ Δlc, z _- ……(7) Δ=Δlx ₊ Δlx _- Δly ₊ Δly _- Δlz ₊ Δlz _- ……(8) However, in equation (7), for example, Δlc,x+ indicates the reference value of the distance in the positive direction of the x-axis, and Δlx+ in equation (8) is the distance between the robot and the robot in the positive direction of the x-axis measured by the non-contact sensor. It is used to indicate the distance between objects (walls, etc.).

Therefore, if we consider only the positive direction of the x-axis, if Δlx _- -Δlc,x ₊ 0, the robot is within the work area, so normal position control can be continued. . That is, ( _r- ) is output as the output of the judgment calculation processing block. Furthermore, if Δlcx ₊ −Δlc,x ₊ <0, the robot is within the danger area, and it is therefore necessary to immediately move it out of the danger area. In such a case, (Δlcx ₊ −Δlc, x ₊ ) is output as the output.

In addition to the above-mentioned control, control as shown in FIG. 11 has also been considered. In Figures 11a and b, the robot approaches the human and the distance between them is Δlc, x ₊
If the robot becomes smaller than , the robot is controlled to move away from the human and return to the position Δlc,x ₊ , and
Conversely, it has been shown that when a human approaches a robot, it is controlled in the same way. The control shown in FIG. 11 corresponds to the case where the wall is movable, so to speak, and such control enables non-contact direct teaching.

In both of the above two control examples, the distance between the robot and the object is detected, and the robot is controlled based on the relationship with a certain reference value. As an extension of such control, a system as shown in FIG. 12 has been considered. 12th
In the robot shown in the figure, a force sensor 18 as a contact sensor is also attached to the robot in addition to the non-contact sensor 3, making hybrid control possible.

For example, assuming that a shaft-shaped workpiece 19 is to be assembled to a workpiece 20 having a hole drilled therein, the robot must convey the workpiece 19 to the vicinity of the workpiece 20 after gripping the workpiece 19. Although it is desirable that the conveyance to the workpiece 20 be performed at high speed, in the case of high-speed conveyance, a large impact is received upon contact with the workpiece 20, so it is necessary to reduce the conveyance speed immediately before contact. Generally, if only a contact sensor is installed, contact will be detected only after a large impact, or the workpiece will be transported at a low speed to avoid a large impact, but regardless of whether the contact is caused by a large impact or a low speed. This will cause problems such as: However, when a non-contact sensor is used in combination, a large impact can be avoided and the workpiece can be transported at high speed.

That is, the non-contact sensor 3 detects the moment immediately before the workpiece 19 contacts the workpiece 20, the conveyance speed is reduced, and after the workpiece 19 contacts the workpiece 20 while being conveyed at low speed, the contact sensor 18 detects the workpiece 19. 19 is to be assembled onto the workpiece 20.

Such control can also be realized by basically the same method by extending the control method described above. This is because equations (9) and (10) can be used instead of equations (5) and (6).

= [M] ^-1 ∫ [(- _r ) - [C] - [K] (
− _r )] dt……(9) = Δt [M] ⁻¹ [( _o-1 − _{r o-1} ) − [K] ( _o-
1 − _{r o-1} )] − (Δt[M] ⁻¹ [C] − [I]) _o-1
...(10) However, represents the force signal from the contact sensor (force sensor), and _r represents the force command value.

Therefore, in order to perform such control, what is shown in FIGS. 5, 6, and 7 is as shown in FIG.
This will replace FIGS. 14 and 15.

〔Effect of the invention〕

As explained above, in the present invention, when a non-contact sensor detects that the distance between the robot hand and an object such as an obstacle is within a certain range, the control mode for the robot is set at the time of detection. It was designed to change the . Therefore,
In the case of the present invention, the robot is appropriately controlled in relation to the target object, and furthermore, when a contact sensor is used together, cooperative control with it is also possible, and various control operation characteristics can be adjusted with parameters on the software. There are effects that can be easily achieved just by making changes.

[Brief explanation of drawings]

Fig. 1 is a diagram showing the external appearance of an example of a multi-degree-of-freedom robot, Fig. 2 is a diagram showing the external appearance of an example of a robot hand with a non-contact sensor attached nearby, and Fig. 3. Figure 4 shows how the distance to an object is measured by a non-contact sensor.
The figure shows an example in which the non-contact sensor is attached to a device other than a robot, FIG. 5 shows an overview of the control system according to the present invention, and FIG. 7 is a diagram showing the functions of the main parts of the control section, FIG. 8 is a diagram showing the step response of the control performed by the main control section, and FIG. Fig. 10 is a diagram for explaining control to avoid the robot hand from entering a dangerous area using a non-contact sensor, and Figs. FIG. 12 is a diagram for explaining control in a case where robots are considered and active control of a robot by a human, and FIG. 13 is a diagram for explaining cooperative control with a contact sensor.
Figures 14 and 15 show the fifth phase of the cooperative control.
FIG. 7 is a diagram corresponding to FIGS. 2... (robot) hand section, 3... non-contact sensor, 7... main control section, 8... coordinate conversion section, 9
. . . Joint angular velocity calculation unit, 15 . . . Judgment calculation processing block, 18 . . . Force sensor (contact sensor).

Claims (1)

[Claims] 1 In a method for controlling a multi-degree-of-freedom robot using a non-contact sensor that controls the speed of the robot hand while detecting the distance between the robot hand and the object using a non-contact sensor, In a state where it is detected that the distance of After the multiplication, a product obtained by multiplying the current speed of the robot hand by a virtual viscosity constant as a second parameter that determines the motion of the robot hand is added to the multiplication result, and based on the time integral value of the addition result. While the robot hand is moved to the commanded position by the speed, if it is detected that the distance between the robot hand and the object is less than a certain distance, the robot hand is moved to the commanded position from the outside and the current position. The difference between the robot hand and the object is replaced by the distance between the robot hand and the object minus the fixed distance above.
A method for controlling a multi-degree-of-freedom robot using a non-contact sensor, in which the speed control mode of a robot hand is switched.