JP6729773B2 - Robot controller and robot system using the same - Google Patents

Description

The present invention relates to a robot controller that controls a robot and a robot system using the same.

2. Description of the Related Art In recent years, development of a robot system that allows autonomously operating autonomous robots and work objects such as workers to share a work space with the robot without being partitioned by a safety fence is under way. In such a robot system, the work object may enter the operation range of the robot, and even in such a case, it is necessary to ensure the safety of the work object.

By the way, generally, the closer the work object and the robot are to each other, and the faster they move, the higher the possibility of collision between them. Therefore, from the viewpoint of safety with respect to the work object, it is desirable to limit the operation of the robot at a stage where the two are located apart from each other and a stage where the both are moving at a low speed. On the other hand, from the viewpoint of the work efficiency of the robot, it is desirable not to limit the movement of the robot as much as possible.

Therefore, efforts have been made to make the safety of the work object compatible with the work efficiency of the robot. For example, Patent Document 1 discloses a method of avoiding a collision between a robot and a work object while operating the robot as much as possible. Specifically, the tip of the arm of the robot is used as a monitoring target unit, and when the monitoring target unit does not have a velocity component in the direction toward the work object such as a person, the operation is not limited. On the other hand, when the monitoring target unit has a velocity component in the direction toward the work object, the monitoring target unit is decelerated or decelerated based on the safety position and the distance to the work object set according to the velocity component. Stop.

JP, 2011-125975, A

However, according to the conventional technique, when a work object enters the movement trajectory of the monitoring target unit, the robot decelerates or stops the monitoring target unit in order to avoid collision with the work object. The robot itself cannot resume the work unless it is separated from the movement trajectory of the monitored part. Therefore, there is a problem that the work efficiency of the robot is reduced.

The present invention has been made in view of the above circumstances, and a robot control device that controls the operation of a robot so as to obtain high work efficiency while ensuring the safety of a work object and a robot control device using the same. The purpose of the present invention is to provide an existing robot system.

A robot control device according to the present invention is a robot control device that controls a robot so that a monitoring target part set as a monitoring target in the robot operates along a command trajectory, and the present position of the monitoring target part and the monitoring target. A first trajectory calculation unit that calculates a first trajectory from the current position to the target position based on the target position of the part, object position information including the position of the work object, and at least one point on the trajectory where the robot operates. A collision possibility estimation unit that estimates the collision possibility of the monitoring target unit with respect to the work object based on the predicted trajectory information of the monitoring target unit, and whether or not the trajectory correction of the first trajectory is necessary based on the estimated collision probability. A correction necessity determination unit that determines whether or not the correction is necessary, and a trajectory correction unit that sets a command trajectory based on the determination result of the correction necessity determination unit. If it is determined, from among the plurality of modified trajectory candidates, it is selected from the arrival time to each target position when the modified trajectory candidate is used and the estimated collision possibility, and the selected modified trajectory candidate is selected. When the correction necessity determination unit determines that the trajectory correction is not necessary, the first trajectory is set as the command trajectory.

In the robot control device and the robot system using the same according to the present invention, it is possible to control the operation of the robot so as to obtain high work efficiency while ensuring the safety of the work object.

1 is a schematic top view for explaining a robot controller and a robot system including the same according to a first embodiment of the present invention. It is a block diagram which shows the structure of the robot control apparatus shown in FIG. It is explanatory drawing which shows an example of the correction method of the 1st track|orbit performed by a track|orbit correction part. 3 is a flowchart of the operation of the robot control device according to the first embodiment of the present invention. FIG. 5 is a flowchart explaining step S2 of FIG. 4 in more detail. It is a block diagram which shows an example of a structure of the collision possibility estimation part which concerns on Embodiment 1 of this invention. It is explanatory drawing regarding an example of the calculation method of the approach distance which concerns on Embodiment 1 of this invention. It is explanatory drawing regarding an example of the method of calculating the relative speed which concerns on Embodiment 1 of this invention. It is a hardware block diagram for implement|achieving the robot control apparatus which concerns on Embodiment 1 of this invention. It is a block diagram which shows the modification of the robot control apparatus which concerns on Embodiment 1 of this invention. It is a block diagram which shows the structural example of the robot control apparatus which concerns on Embodiment 2 of this invention. It is a block diagram which shows the structural example of a collision possibility estimation part. It is the figure which showed an example of the approach distance calculation method which concerns on Embodiment 2 of this invention. It is a figure showing an example of relative velocity calculation concerning Embodiment 2 of the present invention. It is a figure which shows an example of a structure of the robot control apparatus which concerns on Embodiment 3 of this invention. 7 is a flowchart illustrating an operation of the robot controller according to the third embodiment of the present invention. 17 is a flowchart for explaining the process of step S3A of FIG. 16. It is a figure which shows an example of a structure of the robot control apparatus which concerns on Embodiment 4 of this invention. It is a figure which shows an example of a structure of the robot control apparatus which concerns on Embodiment 5 of this invention. It is a block diagram which shows the structural example of a collision possibility estimation part.

Embodiment 1.
FIG. 1 is a schematic top view for explaining a robot controller 1 according to a first embodiment of the present invention and a robot system including the same. In the figure, the robot system includes at least a robot controller 1 and a robot 3. The robot system may include the position detection device 2.

The position detection device 2 and the robot 3 are connected to the robot control device 1. The robot control device 1 outputs a control signal 10 to the robot 3 based on the detection information 20 acquired by the position detection device 2 and the information such as the joint angle acquired from the robot 3. By driving the robot 3 in response to the control signal 10 from the robot control device 1, the robot hand 3b operates along the command trajectory.

The position detection device 2 detects the worker 5 who works around the robot 3 in each control cycle, and outputs the detection result to the robot control device 1 as detection information 20.

The robot 3 has an arm 3a and a robot hand 3b provided at the tip of the arm 3a. A drive device (not shown) that controls the joint angle is provided at the joint of the arm 3a. The driving device is composed of an electric motor such as a servo motor or a stepping motor. The robot 3 can freely change the position and posture by changing the joint angle of each joint with a driving device. As the drive device, a cylinder using air pressure or hydraulic pressure may be used instead of the electric motor.

The robot 3 has a joint angle measuring device (not shown) such as an image sensor and an encoder for measuring the joint angle of the arm 3a. Therefore, the robot controller 1 acquires the joint angle obtained by the joint angle measuring device of the robot 3, and uses the acquired joint angle and device information such as the arm length of the arm 3a of the robot 3 to determine the robot hand 3b. The position of can be calculated.

The following description will be made by taking the case where the robot hand 3b is set as the monitoring target unit, but the monitoring target unit is not limited to this example, and the monitoring target unit is set at an arbitrary location of the robot 3. May be. Further, the monitoring target section is not limited to one location, but may be set in multiple locations.

A workbench 4 is arranged at the work site according to the present embodiment. There is a worker 5 who works around the robot 3, and the robot 3 operates in common with the worker 5 in a work space. Work areas 61 and 62 are set on the work table 4 as areas that can be used by both the robot 3 and the worker 5. In the work areas 61 and 62, the robot 3 performs work such as transportation, processing, inspection, and unloading on the work objects 71 and 72 by the robot hand 3b. In the work areas 61 and 62, the worker 5 can also perform the same work as the robot 3 on the work objects 71 and 72. The configuration of the work site such as the work area and the number of work objects is not limited to the above example.

In the figure, the worker 5 is in a state of holding the work target 72. It can be seen that the robot 3 is in a state before moving the robot hand 3b toward the work area 62 (broken line arrow).

The following description will be given by taking the case where the worker 5, that is, a person works with the robot 3 as an example. However, an autonomously operating device such as a picking robot working in a warehouse works with the robot 3. This may be the case. The work object means a device or a worker who operates autonomously with respect to the robot 3.

Here, the position detection device 2 will be described in more detail. The position detection device 2 includes a sensor such as a range sensor, an RGB-D (Red Green Blue-Depth) sensor, an ultrasonic sensor, and a capacitance sensor. An additional sensor such as a mat switch or a light curtain may be used in order to obtain the detection information 20 of the area that cannot be detected by the above-described sensor or to improve the detection accuracy of the worker 5.

Further, although the position detection device 2 is installed in the work site in FIG. 1, the position detection device 2 may be installed in the robot 3. Here, the position in the coordinate system when the position detecting device 2 is fixed to the work site can be mutually converted into the position in the coordinate system when the position detecting device 2 is fixed to the robot 3. Therefore, even when the installation location of the position detection device 2 is changed, the same calculation as in the above example can be performed.

FIG. 2 is a block diagram showing the configuration of the robot control device 1 shown in FIG. The robot control device 1 shown in FIG. 2 includes a hand information acquisition unit 101, a target point information acquisition unit 102, a person position information acquisition unit 103, a first trajectory calculation unit 104, a collision possibility estimation unit 105, and a correction necessity determination unit 106. A trajectory correction unit 107 and a robot operation control unit 108.

The hand information acquisition unit 101 calculates the hand information 1010 of the robot 3 based on the joint angle acquired by the joint angle measuring device. Then, the hand information acquisition unit 101 outputs the hand information 1010 to the first trajectory calculation unit 104.

More specifically, the hand tip information acquisition unit 101 calculates hand tip information 1010 including the hand tip position and the hand tip speed of the robot 3 by applying the joint angle and the joint angular velocity to a pre-created housing model of the robot 3. .. The housing model of the robot 3 is stored in the storage device (illustrated in FIG. 9) of the robot control device 1. The joint angular velocity is calculated by performing the time difference of the joint angle of the robot 3. Noise may be removed from the calculated joint angular velocity by using a low-pass filter.

The target point information acquisition unit 102 acquires target point information 1020 from the storage device. The target point information 1020 includes information on the target position and target posture of the robot hand 3b. The target point information 1020 is stored in advance in the storage device (described later in FIG. 9) of the robot control device 1 by the work of teaching the robot 3. Note that instead of using the target point information 1020 created and stored in advance, the target point information 1020 may be designated by the worker 5 during the operation of the robot 3.

The person position information acquisition unit 103 acquires the person position information (object position information) 1030, which is the position coordinates of the representative point of the worker 5, using the detection information 20 from the position detection device 2. The person position information 1030 is output to the collision possibility estimation unit 105 and the trajectory correction unit 107.

Here, an example of a method of acquiring the person position information 1030 from the detection information 20 will be described below. When the detection information 20 is information regarding a plurality of body parts of the worker 5 as a point group, the representative point closest to the robot 3 in the detected point group can be set as the person position information 1030.

The method of setting the representative point is not limited to the above example, and may be changed as appropriate depending on the operating conditions of the robot 3, the construction of the work site, and the like. When the detection information 20 is image information acquired by the RGB-D sensor, the human body model of the worker 5 is constructed by performing image processing on this image information. Then, the human position information 1030 may be acquired by calculating the coordinates of the joints and links of the constructed human body model. In this case, since a precise human body model can be constructed, the possibility of the robot 3 colliding with the worker 5 can be reduced.

The first trajectory calculation unit 104 calculates the first trajectory 1040 using the hand information 1010 and the target point information 1020. Specifically, the first trajectory calculation unit 104 first acquires the current position of the robot hand 3b from the hand information 1010, and uses the target point information 1020 prestored in the storage device of the robot control device 1 to target the robot hand 3b. Get the position. Next, the first trajectory calculation unit 104 calculates the first trajectory from the current position to the target point. Then, the first trajectory 1040 is output to the collision possibility estimation unit 105 and the trajectory correction unit 107.

Here, the first trajectory 1040 has a hand position (predicted position), a predicted posture, and a predicted posture of the robot hand 3b at respective times (t=t1, t2,..., Tn-1, tn,...) Corresponding to the control cycle. Includes predicted trajectory information about hand speed (predicted speed). The speed of the robot hand 3b is derived as follows, for example. Hand positions (P(0), P(t1), P(t2),..., P(tn) corresponding to times (t=t1, t2,..., tn-1, tn,...) On the first trajectory 1040. −1), P(tn),...) For each of the positions at the time one control cycle before (for example, P(tn)−P(tn−1)), and the difference value is (tn By dividing by -tn-1), the hand speed of the robot hand 3b at each time can be calculated.

Note that the method of deriving the trajectory of the first trajectory 1040 is deriving as a PTP (Point To Point) trajectory that connects the start point and the end point at a plurality of points so that a smooth trajectory is obtained. A joint space interpolation trajectory is used as a method for generating this smooth trajectory, but the method is not limited to this. A CP (Continuous Path) orbit may be used instead of the PTP orbit, and in this CP orbit, the orbits from the start point to the end point are derived as a continuous function.

The collision possibility estimation unit 105 determines the possibility of collision between the worker 5 and the robot hand 3b based on the predicted trajectory information of the robot hand 3b and the human position information 1030 of the worker 5 at at least one point on the first trajectory 1040. Is estimated. A specific method of estimating the collision possibility 1050 will be described in detail together with the collision possibility calculation unit 1055 of FIG.

The correction necessity determination unit 106 determines, based on the collision possibility 1050, whether or not the first trajectory 1040 needs to be corrected. For example, the correction necessity determination unit 106 determines that the first trajectory 1040 needs to be corrected when the calculated collision possibility 1050 exceeds a preset threshold, and the correction necessity determination unit 106 causes the trajectory to be corrected. The correction instruction signal 1060 is output to the correction unit 107. The correction necessity determination unit 106 outputs the collision possibility 1050 to the trajectory correction unit 107 together with the correction instruction signal 1060.

When the correction instruction signal 1060 is input from the correction necessity determination unit 106, the trajectory correction unit 107 determines, based on the human position information 1030 and the first trajectory 1040, that the collision possibility is lower than that of the first trajectory 1040. Generate two orbits. The trajectory correction unit 107 sets the generated second trajectory as the command trajectory 1070. When the correction instruction signal 1060 is not input from the correction necessity determination unit 106, the trajectory correction unit 107 outputs the first trajectory 1040 as the command trajectory 1070.

FIG. 3 is an explanatory diagram showing a method of correcting the first trajectory 1040 by the trajectory correcting unit 107. In the figure, the second trajectory Q is an example of the trajectory after the first trajectory P is corrected. In the figure, P0 and Pf indicate the position and the target position of the robot 3, respectively. Further, L0 indicates a line segment connecting the person positions H1 and P0, and Lf indicates a line segment connecting the target position Pf and the person position H1. The line C0 is the center H1 and indicates a circle whose radius is the length of the line segment L0.

The second trajectory Q is set so as to pass outside the first trajectory P at all points of the second trajectory Q when viewed from the human position H1, for example. As shown in FIG. 3, the second trajectory Q may be set so as to have an outwardly convex curve when viewed from the human position H1. By setting the second trajectory as described above, the approach distance between the worker 5 and the robot hand 3b is longer than that of the first trajectory P, so that the trajectory is less likely to collide with the worker 5. It is possible to reduce the possibility of collision.

The second trajectory Q shown in FIG. 3 has an arcuate trajectory (C0) centered on the human position H1 and having a radius of the line segment L0, and an arcuate trajectory centered on the human position H1 and having a radius of the line segment Lf. The orbit smoothly transitions to the orbit. In other words, in FIG. 3, as the robot hand 3b moves from the position P0 at the time of trajectory correction to the target position Pf, the distance from the robot hand 3b to the worker 5 monotonously changes (monotonically decreases or monotonically increases). , The second trajectory Q is generated. Since the second trajectory Q is generated in this manner, the shortest distance from the robot hand 3b to the worker 5 at each point of the second trajectory Q is the length of the line segment L0 or the line segment Lf. Therefore, the robot hand 3b can be prevented from approaching the worker 5 too much, and the possibility of collision can be reduced.

In addition, by setting the second trajectory Q so that the curve is a convex curve outward when viewed from the human position H1, the speed component of the moving speed (hand speed) of the robot hand 3b toward the human position H1 is set. Get smaller. As a result, the possibility of collision of the robot hand 3b can be reduced. Further, when the velocity component toward the human position H1 becomes small, it is possible to increase the hand velocity and shorten the time required to reach the target position Pf. Note that FIG. 3 shows an example of the second trajectory Q, and the second trajectory Q may be another trajectory.

Instead of modifying the first trajectory 1040 as described above, the trajectory modifying unit 107 uses a plurality of pre-created modified trajectories to modify a trajectory having a smaller collision possibility 1050 than the first trajectory 1040, for example, collision probability 1050. May be selected as the second trajectory. Further, the trajectory correcting unit 107 has a collision possibility 1050 smaller than that of the first trajectory 1040, selects a correction trajectory having a good balance between the collision possibility 1050 and the arrival time to the target position Pf, and selects the selected correction trajectory as the first trajectory. It may be used as two orbits. The plurality of corrected trajectories are stored in the storage device (illustrated in FIG. 9) of the robot controller 1.

The robot operation control unit 108 outputs the control signal 10 to the robot 3 in each control cycle based on the command trajectory 1070.

The robot operation control unit 108 uses the hand trajectory information 1010 and the human position information 1030 in addition to the command trajectory 1070, and according to the collision possibility calculated for each control cycle during the operation of the robot hand 3b. The speed may be changed. Specifically, the robot motion control unit 108 determines the collision possibility calculated from the human position information 1030 and the hand information 1010 for each control cycle as the value of the collision possibility increases (decreases). The control signal 10 for decreasing (increasing) the speed may be generated.

FIG. 4 is a flowchart of the operation of the robot controller 1 according to this embodiment. First, in step S1, the robot control device 1 acquires the detection information 20 from the position detection device 2 and the data such as the joint angle from the robot 3.

In step S2, the robot controller 1 sets a command trajectory 1070 based on the obtained data. In step S3, the robot controller 1 outputs the control signal 10 for each control cycle based on the command trajectory 1070. When the robot hand 3b reaches the target point in step S4 (YES), the robot 3 ends the operation. If the robot hand 3b has not reached the target point in step S4 (NO), the process returns to step S3.

FIG. 5 is a flowchart explaining step S2 of FIG. 4 in more detail. In step S21, data required for controlling the robot 3 is prepared. Specifically, first, the hand information acquisition unit 101, the target point information acquisition unit 102, and the human position information acquisition unit 103 respectively acquire the current hand information 1010, the target point information 1020, and the human position information 1030 of the robot 3. To do.

In step S22, the first trajectory calculation unit 104 calculates the first trajectory 1040 using the hand tip information 1010 and the target point information 1020 of the robot 3.

In step S23, collision possibility estimation section 105 estimates collision possibility 1050 using person position information 1030 and predicted trajectory information in first trajectory 1040.

In step S24, the correction necessity determination unit 106 determines whether the first trajectory 1040 needs to be corrected based on the value of the collision possibility 1050. Specifically, if the collision possibility 1050 is greater than or equal to the threshold value in step S24 (YES), the correction instruction signal 1060 is generated and the process proceeds to step S25, but if the collision possibility 1050 is smaller than the threshold value (NO). , Does not generate the correction instruction signal 1060, and proceeds to step S26.

In step S25, the trajectory correcting unit 107 receives the correction instruction signal 1060, corrects the second trajectory with a lower possibility of collision than the first trajectory 1040, and sets the second trajectory as the command trajectory 1070. To do.

In step S26, the trajectory correction unit 107 sets the first trajectory 1040 as the command trajectory 1070 because the correction instruction signal 1060 is not input.

FIG. 6 is a block diagram showing an example of the configuration of the collision possibility estimation unit 105. The collision possibility estimation unit 105 includes a hand position acquisition unit 1051, an approach distance calculation unit 1052, a hand speed acquisition unit 1053, a relative speed calculation unit 1054, and a collision possibility calculation unit 1055.

The hand position acquisition unit 1051 acquires the hand position 10510 of the robot 3 from the predicted trajectory information of the first trajectory 1040. Thereafter, the acquired hand position 10510 is output to the approach distance calculation unit 1052 and the relative speed calculation unit 1054 described later.

The hand velocity acquisition unit 1053 acquires the hand velocity 10530 from the predicted trajectory information at at least one point on the first trajectory 1040. The acquired hand speed 10530 of the robot 3 is input to the relative speed calculation unit 1054.

The approach distance calculation unit 1052 calculates the approach distance 10520 between the robot hand 3b and the worker 5 using the hand position 10510 and the person position information 1030.

Here, a method of calculating the approach distance 10520 will be described in detail with reference to FIG. 7. FIG. 7 is an explanatory diagram regarding an example of a method of calculating the approach distance 10520.

First, for the n-point hand positions P1, P2, P3,..., Pn set on the first trajectory P, the approach distance calculation unit 1052 determines each of the n-point hand positions P1 to Pn and the human position H1. The line segments connecting the two are respectively set as line segments L1, L2, L3,..., Ln. Specifically, the line segments L1, L2, and L3 respectively connect the hand position P1 and the person position H1, the line segment connecting the hand position P2 and the person position H1, and the hand position P3 and the person position H1. It is a line segment that connects and. In the following description, n=3, that is, the case where three hand positions are set will be described as an example, but at least one hand position may be set.

Next, the approach distance calculation unit 1052 derives the approach distance 10520 based on the lengths of the line segments L1 to L3. When a plurality of line segments are derived as in the above example, for example, the line segment having the smallest length is selected from the line segments L1 to L3, and the lengths of the selected line segments are approached. Select as distance 10520. For example, in FIG. 7, among the line segments L1 to L3, the length of the line segment L2 having the minimum length is set as the approach distance 10520.

7 and 8, the hand positions P1 to P3 are set so that the movement distance from each hand position to the hand position adjacent to the hand position becomes a preset distance. On the other hand, the hand positions P1 to P3 may be set so that the movement time from each hand position to the hand position adjacent to the hand position becomes a preset time.

Here, the processing of the relative speed calculation unit 1054 will be described with reference to FIGS. 6 and 8. As illustrated in FIG. 6, the relative speed calculation unit 1054 calculates the relative speed 10540 of the robot hand 3b with respect to the worker 5 based on the human position information 1030, the hand position 10510, and the hand speed 10530.

FIG. 8 is an explanatory diagram regarding an example of a method of calculating the relative velocity 10540. A method of calculating the relative speed 10540 by the relative speed calculator 1054 will be described in detail with reference to FIG. In the figure, V1 to V3 indicate the hand speeds 10530 at the hand positions P1 to P3 calculated by the hand speed acquisition unit 1053.

First, the relative speed calculation unit 1054 calculates the relative speed vectors RV1 to RV3 of the robot hand 3b with respect to the worker 5 by projecting the hand speeds V1 to V3 in the directions of the line segments L1 to L3. For example, in FIG. 8, RV1, RV2, and RV3 are the relative velocity vector obtained by projecting the hand velocity V1 onto the line segment L1, the relative velocity vector obtained by projecting the hand velocity V2 onto the line segment L2, and the line segment relative to the hand velocity V3. It is a relative velocity vector projected onto L3.

Next, the relative speed calculation unit 1054 derives the relative speed 10540 based on the values of the relative speed vectors RV1 to RV3. When a plurality of relative velocity vectors are calculated as in the above-described example, for example, of the relative velocity vectors RV1 to RV3, the relative velocity vector toward the worker 5 and the relative velocity of which the absolute value is the largest. Let the vector be the relative velocity 10540. Specifically, of the relative velocity vectors RV1 to RV3, relative velocity vectors RV1 and RV2, which are the relative velocity vectors toward the worker 5, are selected, and the absolute values of the selected two vectors are compared, and the relative velocity vector having a large absolute value is selected. Let RV1 be the relative velocity 10540.

Returning to FIG. 6, the remaining configuration of the collision possibility estimation unit 105 will be described. The collision possibility calculation unit 1055 calculates the collision possibility 1050 based on the input approach distance 10520 and relative speed 10540, for example. An example of a method of calculating the collision possibility 1050 is calculated by referring to the collision evaluation index created in advance for the predicted trajectory information and the position information of the worker 5. The collision evaluation index is stored in the storage device (illustrated in FIG. 9) included in the robot controller 1. The collision evaluation index is set such that the collision possibility 1050 becomes a larger value as the approach distance 10520 becomes smaller and the relative speed 10540 becomes larger.

Further, as another example of the collision evaluation index, a virtual collision prediction time between the worker 5 and the robot 3 is calculated using the moving speed of the worker 5 calculated from the change over time of the person position information 1030. Next, the collision evaluation index created so that the possibility of collision increases as the calculated collision prediction time decreases. Furthermore, a range in which a collision with the robot 3 is expected when the worker 5 enters is set in advance around the position where the robot 3 is installed, and whether or not the human position information 1030 exists within this range. The possibility of collision may be estimated by

Although the example in which the collision possibility 1050 is calculated based on both the approach distance 10520 and the relative speed 10540 has been described, the collision possibility 1050 is calculated based on only one of the approach distance 10520 and the relative speed 10540. Good. In this case, the collision evaluation index is created such that the collision possibility 1050 changes according to only one of the approach distance 10520 and the relative speed 10540. At this time, the predicted trajectory information may include either the hand position 10510 or the hand speed 10530.

According to the present embodiment, when it is determined in advance that the collision possibility on the command trajectory of the robot 3 is high, the first trajectory 1040 is set so as to reduce the collision possibility 1050 between the robot 3 and the worker 5. Because of the correction, even when the worker 5 is expected to enter the first trajectory 1040 of the robot 3, the robot operation can be performed while maintaining the work efficiency of the robot 3.

FIG. 9 is a hardware configuration diagram for realizing the robot control device 1. As shown in FIG. 9, a part or all of the robot control device 1 is specifically a CPU 151 (Central Processing Unit), a storage device 152, an IO (INPUT/OUTPUT) interface 153, a system bus 154, and the like. It is composed of The storage device 152 includes a ROM (Read Only Memory), an HDD (Hard Disk Drive), and the like. An input device 201 and an output device 202 are connected to the IO interface 153 of the robot controller 1 via a cable 155. The storage device 152 stores the above-described housing model of the robot 3, target point information 1020, a collision evaluation index, a modified trajectory that is a candidate for the second trajectory used by the trajectory modification unit 107, and the like.

Each process of the robot controller 1 is executed by the CPU 151. Regarding the hand information acquisition unit 101, the target point information acquisition unit 102, the human position information acquisition unit 103, and the robot operation control unit 108, the external configuration of the robot control device 1 (the position detection device 2 and the robot is controlled via the IO interface 153). 3) and the like.

FIG. 10 is a block diagram showing a modification of the robot controller 1 according to the first embodiment of the present invention. In FIG. 2, the hand tip information acquisition unit 101 acquires the hand tip information 1010 using the information from the joint angle measurement device of the robot 3, but like the robot control device 1A shown in FIG. 104A may be configured to derive the current minion information 1010 using the control signal 10. In this case, the hand information acquisition unit 101 can be omitted in the robot controller 1A.

Embodiment 2.
FIG. 11 is a block diagram showing a configuration example of the robot controller 1B according to the second embodiment of the present invention. The difference from the above-described embodiment is that a collision possibility estimation unit 105A is provided instead of the collision possibility estimation unit 105.

FIG. 12 is a block diagram showing a configuration example of the collision possibility estimation unit 105A. The collision possibility estimation unit 105A shown in FIG. 12 further includes a person movement speed calculation unit (movement speed calculation unit) 1056, and the collision possibility calculation unit 1050 calculates the collision possibility 1050 in consideration of the movement speed of the worker 5. The collision possibility estimation unit 105 shown in FIG. FIG. 13 is a diagram showing an example of a method of calculating the approach distance 10520 according to the second embodiment of the present invention. In the figure, the case where three hand positions are set is shown. In the figure, the human position H1 and the hand positions P1 to P3 on the first trajectory P of the robot 3 are the same as in FIG.

First, the approach distance calculation unit 1052A uses the current person position information 1030 and the person movement speed 10560 to obtain the movement destination 10521 of the person position at the time (control cycle) corresponding to each hand position. Specifically, the person position H2 and the person position H3 in FIG. 13 are calculated using the person position H1 and the person moving speed HV. In other words, the approach distance calculation unit 1052A uses the acquired person position information 1030 and the person moving speed 10560 to estimate the movement destination 10521 of the person position at the time after the time when the person position information 1030 is acquired. The movement destination 10521 of the person position is output to the relative speed calculation unit 1054A. The human positions H1 to H3 indicate the human positions in the control cycle corresponding to the hand positions P1 to P3, respectively. The movement destination 10521 of the person position corresponds to H2 and H3. Here, the human position H1 is acquired by the hand position acquisition unit 1051 before the operation of the robot hand 3b is started. The person movement speed HV is calculated by the person movement speed calculation unit 1056.

Next, the approach distance calculation unit 1052A illustrated in FIG. 12 calculates the approach distance 10520 between the robot hand 3b and the worker 5 based on the person position information 1030, the movement destination 10521 of the person position, and the hand position 10510. Then, the approach distance 10520 is output to the collision possibility calculation unit 1055.

More specifically, La1, La2, and La3 are created as line segments between the human positions H1 to H3 and the hand positions P1 to P3 shown in FIG. 13, respectively. For example, in FIG. 13, the line segment between the hand position P1 and the person position H1 is La1, the line segment between the hand position P2 and the person position H2 is La2, and the line segment between the hand position P3 and the person position H3. The line segment between them is La3. Further, the approach distance 10520 is calculated using the line segments La1 to La3. Specifically, the minimum value of the lengths of the line segments La1 to La3 is set as the approach distance 10520. Although the above example illustrates the case where three hand positions are set, one or more hand positions may be provided.

The relative speed calculation unit 1054A shown in FIG. 12 outputs the relative speed 10540 of the robot hand 3b to the worker 5 based on the hand position 10510, the movement destination 10521 of the person position, the hand speed 10530, and the person movement speed 10560.

FIG. 14 is a diagram showing an example of a method of calculating the relative velocity 10540 according to the second embodiment of the present invention. 14, the same components as those in FIG. 13 are designated by the same reference numerals, and the description thereof will be omitted. Similar to FIG. 8, V1 to V3 in the drawing indicate hand speeds 10530 at hand positions P1 to P3 calculated by the hand speed acquisition unit 1053.

The relative speed calculation unit 1054A first calculates the relative speed vectors RVa1 to RVa3 of the robot hand 3b with respect to the worker 5 by projecting the hand speeds V1 to V3 in the directions of the line segments La1 to La3. Further, the relative speed calculation unit 1054A derives the relative speed 10540 based on the relative speed vectors RVa1 to RaV3.

When a plurality of relative velocity vectors are calculated as in the above-described example, for example, of the relative velocity vectors RVa1 to RVa3, the relative velocity vector toward the worker 5 and the relative velocity of which the absolute value is the largest. The vector is the relative velocity 10540. For example, of the relative velocity vectors RVa1 to RVa3, the relative velocity vectors RVa1 and RVa2 that are toward the worker 5 are selected, the absolute values of the two selected vectors are compared, and the relative velocity vector RVa1 having a large absolute value is compared. The speed is 10540.

According to the present embodiment, in the case where the worker 5 is operating to approach the command trajectory of the robot 3 in order to determine the possibility of collision in consideration of the human movement speed of the worker 5, Since the approach distance is smaller than in the first mode, the possibility of collision is calculated to be higher, and as a result, the first trajectory is easily corrected. On the other hand, when the worker 5 is operating to move away from the command trajectory of the robot 3, the collision distance is calculated to be lower because the approach distance is larger than in the first embodiment, and as a result, it is unnecessary. The first orbit is not corrected. Therefore, as compared with the robot controller of the first embodiment, the robot controller of the present embodiment can improve the safety for the worker 5 and the operation efficiency of the robot.

Embodiment 3.
FIG. 15 is a diagram showing an example of the configuration of a robot controller 1C according to the third embodiment of the present invention. 15, the same components as those in FIG. 2 are designated by the same reference numerals and the description thereof will be omitted. However, the collision possibility estimation unit 105 shown in FIG. 15 may be the collision possibility estimation unit 105A shown in FIGS. 11 and 12, and is the collision possibility estimation unit 105E shown in FIGS. 19 and 20 described later. May be.

The robot control device 1C includes a trajectory correction unit 107C instead of the trajectory correction unit 107, and a robot operation control unit 108A instead of the robot operation control unit 108.

The configuration and operation of the robot controller 1C according to the present embodiment will be described with reference to FIGS. FIG. 16 is a flowchart explaining the operation of the robot controller 1C according to the third embodiment of the present invention. FIG. 17 is a flowchart for explaining the process of step S3A of FIG. Note that in FIG. 16, step S1 is the same as step S1 in FIG. Further, in the figure, step S2 (steps S21 to S26) is the same as step S2 in FIG. 4, that is, steps S21 to S26 in FIG. Therefore, the description of those steps will not be repeated.

In step S3A, the robot operation control unit 108A controls the drive of the robot 3 as shown in FIG. Here, step S3A will be described in detail with reference to FIG.

First, in step S31A, the robot operation control unit 108A acquires the current hand tip information 1010 and the current person position information 1030. Next, in step S32A, the robot operation control unit 108A calculates the possibility of collision at a preset cycle using the above-described collision evaluation index based on the current hand information 1010 and human position information 1030. Here, the current minion information 1010 and person position information 1030 refer to the latest acquired minion information 1010 and person position information 1030. In step S33A, the robot operation control unit 108A determines whether the calculated collision possibility is less than or equal to the threshold value. If the possibility of collision is less than or equal to the threshold value (YES) in step S33A, the robot operation control unit 108A generates the control signal 10. On the other hand, in step S33A, if the possibility of collision is larger than the threshold value (NO), the process proceeds to step S35A, and the robot operation control unit 108A generates a re-planning command 1080.

Returning to FIG. 16, the operation of the robot controller 1C after step S3B will be described. In step S3B, it is determined whether there is a re-planning instruction 1080. If the re-planning command 1080 is not generated by the robot operation control unit 108A in step S3B (YES), the process proceeds to step S4. On the other hand, in step S3B, when the re-planning command 1080 is generated by the robot operation control unit 108A (NO), the process returns to step S25, and the trajectory correction unit 107C corrects the command trajectory 1070.

When the re-planning command 1080 is input, the trajectory correction unit 107C uses the command trajectory 1070 instead of the first trajectory 1040 to generate the second trajectory (step S25). The trajectory correction unit 107C sets the generated second trajectory as a new command trajectory 1070. The newly set command trajectory is generated by the trajectory correction unit 107C so that the possibility of collision is lower than that of the preset command trajectory 1070.

In step S4, the robot operation control unit 108A determines whether or not the robot hand 3b has reached the target point. If the robot hand 3b has reached the target point, the operation is terminated (YES). ) Return to step S3A.

In the flowchart of FIG. 16, when the re-planning command is input, steps S25 to S3B are repeated until the possibility of collision of the command trajectory 1070 becomes equal to or less than the threshold value.

According to the present embodiment, when the possibility of collision between the robot 3 and the worker 5 increases, the command trajectory 1070 of the robot 3 is re-planned so that the safety for the worker 5 and the operation efficiency of the robot 3 are improved. improves.

Fourth Embodiment
FIG. 18 is a diagram showing an example of the configuration of the robot controller 1D according to the fourth embodiment of the present invention. 2 that are the same as or corresponding to those in FIG. 2 are designated by the same reference numerals and the description thereof will not be repeated.

In the robot control device 1D of the present embodiment, the robot control device 1D is connected to a sensor 6 that acquires information on members around the robot hand 3b, as compared with the robot control device 1 (first embodiment). The difference is that the robot control device 1D includes an operating environment acquisition unit 109 and an interference determination unit 110. Examples of the sensor 6 include a range sensor, an RGB-D (Red Green Blue-Depth) sensor, an ultrasonic sensor, and a capacitance sensor, and the sensor 6 may be used in combination with the position detection device 2 in FIG. 1.

The operating environment acquisition unit 109 uses the information from the sensor 6 to acquire the position information 1090 of the member arranged in the operating range of the robot hand 3b. Then, the position information 1090 regarding this member is output to the interference determination unit 110 by the operating environment acquisition unit 109.

The members arranged in the movement range of the robot hand 3b mean the members other than the robot 3 and the configuration of the robot 3 other than the robot hand 3b (the base of the robot 3, etc.).

The interference determination unit 110 determines whether the robot hand 3b interferes with this member using the position information 1090 of the member and the trajectory prediction information of the first trajectory 1040. When the interference determination unit 110 determines that the trajectory needs to be corrected, the trajectory correction instruction signal 1100 is output to the trajectory correction unit 107D. Thereby, the trajectory correcting unit 107D corrects the trajectory based on the predicted trajectory information of the first trajectory 1040 and the member position information 1090. On the other hand, when the interference determination unit 110 determines that the trajectory correction is not necessary, the trajectory correction instruction signal 1100 is not input to the trajectory correction unit 107D, and thus the trajectory correction unit 107D does not perform the trajectory correction.

According to the present embodiment, it is possible to obtain a command trajectory corrected so as not to collide with a member existing in the operation range of the robot hand 3b. The present embodiment can be combined with any of the robot control devices 1A to 1C of the above-described embodiment and the robot control device 1E of the embodiment described later.

Embodiment 5.
FIG. 19 is a diagram showing an example of the configuration of the robot controller 1E according to the fifth embodiment of the present invention. The robot controller 1E shown in FIG. 19 is a modification of the robot controller 1B according to the second embodiment. In FIG. 19, the same components as those in FIG. 11 are designated by the same reference numerals and the description thereof will be omitted.

In FIG. 11, the trajectory correction unit 107 has generated the second trajectory using the person position information 1030 output from the person position information acquisition unit 103. On the other hand, as in the robot controller 1E shown in FIG. 19, the collision possibility estimation unit 105E outputs the reference position 1200 for generating the second trajectory, and the trajectory correction unit 107 replaces the human position information 1030. The reference position 1200 may be used to generate the second trajectory. The configuration of the trajectory correction unit 107 is the same as that shown in FIG. 11 except that the reference position 1200 is used instead of the human position information 1030 when generating the second trajectory.

FIG. 20 is a block diagram showing a configuration example of the collision possibility estimation unit 105E. In the collision possibility estimation unit 105E illustrated in FIG. 20, the collision possibility calculation unit 1055E selects and outputs the reference position 1200 from the person position and the movement destination 10521 of the person position included in the person position information 1030. The point is different from the collision possibility estimation unit 105A shown in FIG.

The collision possibility calculation unit 1055E determines the reference position 1200 from the human positions H1, H2, H3 shown in FIG. 13 or 14 by referring to the approach distance 10520 or the relative speed 10540. For example, the collision possibility calculation unit 1055E refers to the approach distance or the relative speed corresponding to each of the human positions H1, H2, and H3, and selects the human position having the highest possibility of collision as the reference position 1200. .. Therefore, the reference position 1200 represents the current position of the worker 5, or the estimated movement destination of the worker 5. The movement destination of the worker 5 can be restated as a predicted position of the worker 5 in the future.

According to the robot control device 1E of the present embodiment, it is possible to generate a movement trajectory of the robot hand 3b that is less likely to collide with the worker 5.

1 robot control device 10 control signal 2 position detection device 20 detection information 3 robot 4 workbench 5 worker (working object)
61, 62 Work area 71, 72 Work object 101 Hand information acquisition unit 1010 Hand information 102 Target point information acquisition unit 1020 Target point information 103 Person position information acquisition unit 1030 Person position information (object position information)
104 first trajectory calculation unit 1040 first trajectory 105 collision possibility estimation unit 106 correction necessity determination unit 107 trajectory correction unit 108 robot operation control unit 109 operating environment acquisition unit 1056 human movement speed calculation unit (moving speed calculation unit)

Claims (11)

A robot control device for controlling the robot so that a monitoring target part set as a monitoring target in the robot operates along a command trajectory,
A first trajectory calculation unit that calculates a first trajectory from the current position to the target position based on the current position of the monitoring target unit and the target position of the monitoring target unit;
The possibility of collision of the monitoring target section with the work object is estimated based on object position information including the position of the work object and predicted trajectory information of the monitoring target section at at least one point on the trajectory in which the robot moves. A collision possibility estimation unit,
A correction necessity determination unit that determines whether or not it is necessary to correct the trajectory of the first trajectory based on the estimated possibility of collision,
A trajectory correction unit that sets the command trajectory based on the determination result of the correction necessity determination unit,
The trajectory correction unit,
If the correction necessity determination unit determines that the trajectory correction is necessary, it is estimated from the plurality of correction trajectory candidates that the arrival time to each target position when the correction trajectory candidate is used Selected from the collision possibility that has been set, the selected modified trajectory candidate is set as the command trajectory,
A robot controller that sets the first trajectory as the command trajectory when the correction necessity determination unit determines that the trajectory correction is not necessary. The trajectory correcting unit selects, from the plurality of trajectory candidates, the trajectory candidate having a collision possibility lower than at least the collision possibility estimated in the first trajectory. 1. The robot controller according to 1. The plurality of modified trajectory candidates include modified trajectory candidates set to pass outside the first trajectory at all points when viewed from the position of the work object. The robot controller according to claim 1 or claim 2. The trajectory correction unit, the distance from the monitoring target unit to the work object, the distance from the current position and the work object as the monitoring target unit from the current position toward the target position, the The robot controller according to any one of claims 1 to 3, wherein the modified trajectory candidate that monotonously changes to a distance between a target position and the work object is generated. The robot controller according to any one of claims 1 to 4, further comprising a storage device that stores the created plurality of modified trajectory candidates. The collision possibility estimation unit includes a movement speed calculation unit that calculates a movement speed of the work object, and estimates the collision possibility of the work object based on the movement speed of the work object. 5. The robot controller according to any one of 5 above. The collision possibility estimation unit estimates a movement destination of the work object based on the moving speed of the work object, and estimates a collision possibility based on the estimated movement destination of the work object. The robot controller according to 1. An interference determination unit that determines whether or not the monitoring target unit collides with the member by using the position information of the member arranged in the operation range of the monitoring target unit and the predicted trajectory information,
The robot control device according to claim 1, wherein the trajectory correction unit corrects the command trajectory according to a determination result of the interference determination unit. Further comprising a robot operation control unit that controls the movement of the monitoring target unit based on the command trajectory,
The robot operation control unit,
Based on the current position of the monitored part and the object position information, the collision possibility is calculated for each preset control cycle, and when the calculated collision possibility exceeds a preset threshold value. Generates a replanning directive,
The robot control device according to any one of claims 1 to 8, wherein the trajectory correction unit corrects the command trajectory when the replanning command is input. Further comprising a robot operation control unit that controls the movement of the monitoring target unit based on the command trajectory,
The robot control device according to claim 1, wherein the current position of the monitoring target unit is derived from a control signal output from the robot operation control unit. A robot controller according to any one of claims 1 to 10,
A robot system comprising: the robot controlled and operated by the robot controller.

Images