WO2019035362A1 - ロボット制御装置およびこれを用いたロボットシステム - Google Patents

ロボット制御装置およびこれを用いたロボットシステム Download PDF

Info

Publication number
WO2019035362A1
WO2019035362A1 PCT/JP2018/028971 JP2018028971W WO2019035362A1 WO 2019035362 A1 WO2019035362 A1 WO 2019035362A1 JP 2018028971 W JP2018028971 W JP 2018028971W WO 2019035362 A1 WO2019035362 A1 WO 2019035362A1
Authority
WO
WIPO (PCT)
Prior art keywords
trajectory
robot
unit
correction
control device
Prior art date
Application number
PCT/JP2018/028971
Other languages
English (en)
French (fr)
Inventor
卓矢 岡原
Original Assignee
三菱電機株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 三菱電機株式会社 filed Critical 三菱電機株式会社
Priority to JP2019508266A priority Critical patent/JPWO2019035362A1/ja
Publication of WO2019035362A1 publication Critical patent/WO2019035362A1/ja

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/06—Safety devices
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/10—Programme-controlled manipulators characterised by positioning means for manipulator elements

Definitions

  • the present invention relates to a robot control device that controls a robot and a robot system using the same.
  • 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, with the tip of the arm of the robot as the monitoring target unit, operation restriction is not performed if the monitoring target unit does not have a velocity component in the direction toward the work object such as a person. On the other hand, when the monitoring target has a velocity component in the direction toward the work object, the monitoring target is decelerated or reduced based on the safety position and the distance to the work object set according to the velocity component. Stop it.
  • the robot decelerates or stops the monitoring target part in order to avoid a collision with the work object.
  • the robot itself can not resume work unless it is separated from the motion trajectory of the monitoring target unit. Therefore, there is a problem that the working efficiency of the robot is reduced.
  • the present invention has been made in view of the above-described circumstances, and a robot control apparatus for controlling the operation of a robot so as to obtain high work efficiency while securing the safety of a work object, and a robot control apparatus Aims to provide a robot system that
  • the robot control apparatus is a robot control apparatus that controls a robot such that a monitoring target unit set as a monitoring target in the robot operates along a command track, and the current position of the monitoring target unit and the monitoring target First trajectory calculation unit that calculates a first trajectory from the current position to the target position based on the target position of the unit, object position information including the position of the work object, and a monitoring target at at least one point on the first trajectory It is judged whether the trajectory correction of the first trajectory is necessary or not based on the collision possibility estimation unit that estimates the collision possibility of the monitoring target unit against the work object based on the predicted trajectory information of the unit, and the estimated collision possibility.
  • a trajectory correction unit that sets a command trajectory based on the judgment result of the correction necessity judgment unit, and the trajectory correction unit requires the trajectory correction in the correction necessity judgment unit If it is determined, the second track having a collision possibility smaller than the collision possibility of the first track is set as the commanded track based on the object position information and the first track, and the correction necessity determination unit If it is determined that no correction is necessary, the first trajectory is set as the commanded trajectory.
  • the operation of the robot can be controlled to obtain high work efficiency while securing the safety of the work object.
  • FIG. 1 is a schematic top view for explaining a robot control apparatus according to a first embodiment of the present invention and a robot system provided with the same. 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
  • FIG. 5 is a flowchart for 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.
  • FIG.16 S3A It is a figure which shows an example of a structure of the robot control apparatus based on Embodiment 4 of this invention. It is a figure which shows an example of a structure of the robot control apparatus based on Embodiment 5 of this invention. It is a block diagram which shows the structural example of a collision possibility estimation part.
  • FIG. 1 is a schematic top view for explaining a robot control device 1 according to a first embodiment of the present invention and a robot system provided with the same.
  • the robot system comprises at least a robot control device 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.
  • the robot hand 3b operates along the commanded trajectory.
  • the position detection device 2 detects the worker 5 who works around the robot 3 at 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.
  • the joint of the arm 3a is provided with a drive device (not shown) for controlling the joint angle.
  • the driving device is constituted by an electric motor such as a servomotor or a stepping motor.
  • the robot 3 can freely change the position and posture by changing the joint angle of each joint by a drive device.
  • a drive device instead of the electric motor, a cylinder using air pressure or oil pressure may be used.
  • the robot 3 has a joint angle measurement device (not shown) such as an image sensor for measuring the joint angle of the arm 3a and an encoder. Therefore, the robot control device 1 acquires the joint angle obtained by the joint angle measurement device of the robot 3, and uses the acquired joint angle and the device information such as the arm length of the arm 3a of the robot 3 to obtain the robot hand 3b. The position of can be calculated.
  • a joint angle measurement device such as an image sensor for measuring the joint angle of the arm 3a and an encoder. Therefore, the robot control device 1 acquires the joint angle obtained by the joint angle measurement device of the robot 3, and uses the acquired joint angle and the device information such as the arm length of the arm 3a of the robot 3 to obtain the robot hand 3b. The position of can be calculated.
  • 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 unit is not limited to one place, and may be set at a plurality of places.
  • a workbench 4 is disposed at the work site according to the present embodiment.
  • a worker 5 who performs work is present around the robot 3, and the robot 3 operates with the worker 5 sharing a work space.
  • Work areas 61 and 62 are set on the work table 4 as areas available to both the robot 3 and the worker 5.
  • the robot 3 carries out work such as transport, processing, inspection, and removal with respect to the work objects 71 and 72 with the robot hand 3b.
  • 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-described example.
  • the worker 5 is in a state where the work object 72 is held. It can be seen that the robot 3 is in a state before moving the robot hand 3b toward the work area 62 (indicated by the broken line arrow).
  • the work object means an apparatus or operator that operates autonomously with respect to the robot 3.
  • the position detection device 2 is configured by a sensor such as a range sensor, an RGB-D (red green blue-depth) sensor, an ultrasonic sensor, and a capacitance sensor.
  • 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 to acquire detection information 20 of an area that can not be detected by the above-described sensor or to improve the detection accuracy of the worker 5.
  • the position detection apparatus 2 is installed in the work site in FIG. 1, the position detection apparatus 2 may be installed in the robot 3.
  • the position in the coordinate system when the position detection device 2 is fixed to the work site can be mutually converted with the position in the coordinate system when the position detection device 2 is fixed to the robot 3. Therefore, even when the installation place of the position detection device 2 is changed, calculation can be performed in the same manner as the above-described example.
  • 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 tip information acquisition unit 101, a target point information acquisition unit 102, a human 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 tip information acquisition unit 101 calculates hand tip information 1010 of the robot 3 based on the joint angle acquired by the joint angle measurement device. Thereafter, the hand tip information acquisition unit 101 outputs the hand tip information 1010 to the first trajectory calculation unit 104.
  • the hand tip information acquisition unit 101 calculates hand tip information 1010 including the hand tip position and hand tip velocity of the robot 3 by applying the joint angle and the joint angular velocity to the previously created housing model of the robot 3 .
  • the housing model of the robot 3 is stored in a storage device (shown in FIG. 9) of the robot control device 1.
  • the joint angular velocity is calculated by performing time difference of the joint angle of the robot 3.
  • a low pass filter may be used to remove noise from the calculated joint angular velocity.
  • 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 the target attitude of the robot hand 3b.
  • the target point information 1020 is stored in advance in a storage device (described later in FIG. 9) of the robot control device 1 by teaching work to the robot 3.
  • the target point information 1020 may be designated by the operator 5 while the robot 3 is operating, instead of using the target point information 1020 created and stored in advance.
  • the human position information acquisition unit 103 acquires human position information (object position information) 1030 which is position coordinates of a representative point of the worker 5 using the detection information 20 from the position detection device 2.
  • the human position information 1030 is output to the collision possibility estimation unit 105 and the trajectory correction unit 107.
  • the detection information 20 is information in which a plurality of body parts of the worker 5 are point clouds
  • representative points at a closest distance from the robot 3 among the detected point clouds can be set as the human position information 1030.
  • the method of setting the representative points is not limited to the above-described example, and may be appropriately changed depending on the operation conditions of the robot 3, the configuration of the work site, and the like.
  • 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 the image information.
  • human position information 1030 may be obtained by calculating coordinates of joints and links of the constructed human body model. In this case, since a precise human body model can be constructed, the possibility of collision of the robot 3 with the operator 5 can be reduced.
  • the first trajectory calculation unit 104 calculates the first trajectory 1040 using the hand tip 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 the target point information 1020 stored in advance in the storage device of the robot control device 1 targets the robot hand 3b. Get the position. Next, the first trajectory calculation unit 104 calculates a first trajectory from the current position to the target point. Thereafter, the first trajectory 1040 is output to the collision probability estimation unit 105 and the trajectory correction unit 107.
  • the velocity of the robot hand 3b is derived, for example, as follows.
  • the method of deriving the trajectory of the first trajectory 1040 is derived as a PTP (Point To Point) trajectory connecting a start point and an end point at a plurality of points so as to obtain a smooth trajectory.
  • a joint space interpolation trajectory is used as a method of generating the smooth trajectory, but the method is not limited to this.
  • a CP (Continuous Path) orbit may be used, and in this CP orbit, the orbits to the start point and the end point are derived as a continuous function.
  • the collision possibility estimation unit 105 is a 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 track 1040. Estimate the collision probability 1050 indicating. A concrete estimation method of 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 correction of the first trajectory 1040 is necessary. For example, if the calculated possibility of collision 1050 exceeds a preset threshold, the correction necessity determination unit 106 determines that the correction of the first trajectory 1040 is necessary, and the correction necessity determination unit 106 determines the trajectory.
  • the correction instruction signal 1060 is output to the correction unit 107.
  • the correction necessity determination unit 106 outputs a collision possibility 1050 to the trajectory correction unit 107 together with the correction instruction signal 1060.
  • the trajectory correction unit 107 determines that the collision possibility is lower than the first trajectory 1040 based on the human position information 1030 and the first trajectory 1040. Generate two orbits. The trajectory correction unit 107 sets the generated second trajectory as a commanded 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 a command trajectory 1070.
  • FIG. 3 is an explanatory view showing a method of correcting the first track 1040 by the track correction unit 107.
  • the second trajectory Q is an example of the trajectory after the first trajectory P is corrected.
  • P0 and Pf indicate the position of the robot 3 and the target position, respectively.
  • L0 represents a line segment connecting the human position H1 and P0
  • Lf represents a line segment connecting the target position Pf and the human position H1.
  • the line C0 is a center H1 and indicates a circle whose radius is the length of the line segment L0.
  • the second trajectory Q is set to pass outside the first trajectory P at all points of the second trajectory Q, for example, when viewed from the human position H1.
  • the second trajectory Q may be set to be a curve convex outward when viewed from the human position H1.
  • the second trajectory Q shown in FIG. 3 is arc-shaped from the arc-shaped trajectory (C0) having the person position H1 as the center and the line segment L0 as the radius, and the person position H1 as the center and the line segment Lf as the radius It is a track that smoothly transitions to the track.
  • the distance from the robot hand 3b to the worker 5 monotonically changes (monotonously decreases or monotonously) as the robot hand 3b moves from the position P0 to the target position Pf at the time of trajectory correction.
  • the second trajectory Q is generated.
  • the shortest distance from the robot hand 3b to the worker 5 becomes the length of the line segment L0 or the line segment Lf. Therefore, the robot hand 3 b can be prevented from coming too close to the worker 5, and the possibility of collision can be reduced.
  • FIG. 3 shows an example of the second trajectory Q, and the second trajectory Q may be another trajectory.
  • the trajectory correction unit 107 may use a plurality of previously generated correction trajectories to make a correction trajectory having a smaller collision probability 1050 than the first trajectory 1040, for example, the collision probability 1050.
  • the smallest corrected trajectory may be selected, and the selected corrected trajectory may be used as the second trajectory.
  • the trajectory correction unit 107 selects a correction trajectory having a collision probability 1050 smaller than that of the first trajectory 1040 and having a good balance between the collision probability 1050 and the arrival time to the target position Pf. It may be used as two orbits.
  • the plurality of corrected trajectories are stored in the storage device (shown in FIG. 9) of the robot control device 1.
  • the robot operation control unit 108 outputs a control signal 10 to the robot 3 for each control cycle based on the command track 1070.
  • the robot operation control unit 108 uses hand tip information 1010 and human position information 1030 in addition to the command track 1070, and during the operation of the robot hand 3b, according to the possibility of collision calculated for each control cycle.
  • the speed may be changed.
  • the robot motion control unit 108 increases or decreases the value of the collision possibility of the robot hand 3b.
  • a control signal 10 may be created to slow (increase) the speed.
  • FIG. 4 is a flowchart of the operation according to the robot control device 1 of the present embodiment.
  • the robot control device 1 acquires detection information 20 from the position detection device 2, and acquires data such as joint angles from the robot 3.
  • step S2 the robot control device 1 sets a commanded trajectory 1070 based on the obtained data.
  • step S3 robot control device 1 outputs control signal 10 for each control cycle based on command track 1070. If the robot hand 3b reaches the target point in step S4 (YES), the robot 3 ends its 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 for explaining step S2 of FIG. 4 in more detail.
  • step S21 data necessary for control of the robot 3 is prepared. Specifically, first, the hand tip information acquiring unit 101, the target point information acquiring unit 102, and the human position information acquiring unit 103 respectively acquire the current hand tip information 1010, the target point information 1020, and the human position information 1030 of the robot 3. Do.
  • step S22 the first trajectory calculation unit 104 calculates the first trajectory 1040 using the hand information 1010 and the target point information 1020 of the robot 3.
  • step S23 the collision possibility estimation unit 105 uses the human position information 1030 and the predicted trajectory information in the first trajectory 1040 to estimate the collision probability 1050.
  • 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 collision probability 1050 is equal to or greater than the threshold (YES) in step S24, correction instruction signal 1060 is generated and the process proceeds to step S25, but if collision probability 1050 is smaller than the threshold (NO) The process proceeds to step S26 without generating the correction instruction signal 1060.
  • step S 25 track correction unit 107 receives an input of correction instruction signal 1060, corrects it to a second track having a lower possibility of collision than first track 1040, and sets the second track as command track 1070. Do.
  • step S26 the trajectory correction unit 107 sets the first trajectory 1040 as the command trajectory 1070 because there is no input of the correction instruction signal 1060.
  • FIG. 6 is a block diagram showing an example of the configuration of the collision probability 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 track 1040. Thereafter, the acquired hand position 10510 is output to an approach distance calculation unit 1052 and a relative velocity calculation unit 1054 which will be described later.
  • the hand speed acquisition unit 1053 acquires the hand speed 10530 from predicted trajectory information at at least one point on the first trajectory 1040.
  • the obtained 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 3 b and the worker 5 using the hand position 10510 and the human position information 1030.
  • FIG. 7 is an explanatory view of an example of a method of calculating the approach distance 10520. As shown in FIG.
  • the approach distance calculation unit 1052 sets each of the hand positions P1 through Pn of the n points and the person position H1.
  • line segments L1, L2 and L3 are line segments connecting hand position P1 and person position H1, line segments connecting hand position P2 and person position H1, and hand position P3 and person position H1, respectively.
  • the approach distance calculation unit 1052 derives the approach distance 10520 based on the lengths of the line segments L1 to L3.
  • the line segment having the smallest length among the line segments L1 to L3 is selected, and the length of the selected line segment is approximated.
  • the length of the line segment L2 at which the length of the line segment is the minimum value is set as the approaching distance 10520.
  • the hand positions P1 to P3 are set such that the movement distance from each hand position to the hand position adjacent to the hand position is a preset distance.
  • the hand tip positions P1 to P3 may be set such that the movement time from each hand tip position to the hand tip position adjacent to the hand tip position becomes a preset time.
  • the relative velocity computing unit 1054 computes the relative velocity 10540 of the robot hand 3 b with respect to the worker 5 based on the person position information 1030, the hand position 10510 and the hand speed 10530.
  • FIG. 8 is an explanatory diagram of an example of a method of calculating the relative velocity 10540.
  • a method of calculating the relative velocity 10540 by the relative velocity calculating unit 1054 will be described in detail with reference to FIG. V1 to V3 in the figure indicate the hand speed 10530 at the hand positions P1 to P3 calculated by the hand speed acquisition unit 1053.
  • relative velocity calculation unit 1054 calculates relative velocity vectors RV1 to RV3 of robot hand 3b with respect to operator 5 by projecting hand speeds V1 to V3 in the directions of line segments L1 to L3.
  • RV1, RV2 and RV3 respectively indicate relative velocity vectors obtained by projecting the hand velocity V1 to the line segment L1, relative velocity vectors obtained by projecting the hand velocity V2 onto the line segment L2, and a line segment for the hand velocity V3. It is a relative velocity vector projected to L3.
  • relative speed calculation unit 1054 derives relative speed 10540 based on the values of relative speed vectors RV1 to RV3.
  • a plurality of relative velocity vectors are calculated as in the above-mentioned example, for example, among the relative velocity vectors RV1 to RV3, the relative velocity vector directed to the worker 5 and whose absolute value becomes maximum is the relative velocity vector.
  • the vector be relative velocity 10540.
  • relative velocity vectors RV1 and RV2 directed to worker 5 are selected, and the relative values of the selected vectors are compared to obtain a relative velocity vector having a large absolute value.
  • RV1 be a relative velocity 10540.
  • the collision possibility calculation unit 1055 calculates the collision possibility 1050, for example, based on the approach distance 10520 and the relative velocity 10540 input. As an example of the method of calculating the collision possibility 1050, it is calculated by referring to the collision evaluation index created in advance with respect to the predicted trajectory information and the position information of the worker 5.
  • the collision evaluation index is stored in a storage device (shown in FIG. 9) that the robot control device 1 has.
  • the collision evaluation index is set such that, for example, as the approach distance 10520 decreases and as the relative velocity 10540 increases, the collision probability 1050 becomes a large value.
  • a virtual collision predicted time between the worker 5 and the robot 3 is calculated using the moving speed of the worker 5 calculated from the time-dependent change of the human position information 1030.
  • a collision evaluation index created so as to increase the possibility of collision as the calculated collision prediction time becomes shorter is used.
  • a range where a collision with the robot 3 is expected is set in advance around the position where the robot 3 is installed, and whether or not the human position information 1030 exists in this range. The collision probability may be estimated by
  • the collision probability 1050 is calculated based on only one of the approaching distance 10520 and the relative velocity 10540. It is also good.
  • the collision evaluation index is created such that the collision probability 1050 changes depending on only one of the approaching distance 10520 and the relative velocity 10540.
  • the predicted trajectory information may include either one of the hand position 10510 or the hand speed 10530.
  • the first trajectory 1040 is reduced so as to reduce the possibility of collision 1050 between the robot 3 and the worker 5.
  • robot operation maintaining the work efficiency of the robot 3 becomes possible.
  • FIG. 9 is a hardware configuration diagram for realizing the robot control device 1.
  • the robot control device 1 specifically includes a CPU 151 (Central Processing Unit), a storage device 152, an IO (INPUT OUTPUT: input / output) interface 153, a system bus 154, etc. It consists of The storage device 152 is configured of a read only memory (ROM), a hard disk drive (HDD), and the like.
  • An input device 201 and an output device 202 are connected to the IO interface 153 of the robot control device 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, and a corrected trajectory which is a candidate for the second trajectory used by the trajectory correcting unit 107.
  • Each process of the robot control device 1 is executed by the CPU 151.
  • the external configuration of the robot control device 1 position detection device 2 and robot, via the IO interface 153) for 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. 3) input and output.
  • FIG. 10 is a block diagram showing a modified example of the robot control device 1 according to the first embodiment of the present invention.
  • the hand tip information obtaining unit 101 obtains the hand tip information 1010 using information from the joint angle measuring device of the robot 3.
  • the configuration may be such that the current hand tip information 1010 is derived using the control signal 10.
  • the hand tip information acquisition unit 101 can be omitted in the robot control device 1A.
  • FIG. 11 is a block diagram showing a configuration example of a robot control apparatus 1B according to Embodiment 2 of the present invention.
  • the point which is provided with the collision possibility estimation part 105A instead of the collision possibility estimation part 105 differs from the above-mentioned embodiment.
  • 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 human movement speed calculation unit (movement speed calculation unit) 1056, and calculates collision possibility 1050 in consideration of the movement speed of the worker 5 as shown in FIG. This is different from the collision possibility estimation unit 105 shown in FIG.
  • FIG. 13 is a diagram showing an example of a method of calculating the approaching 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.
  • the approach distance calculation unit 1052A obtains the movement destination 10521 of the human 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 movement speed HV. In other words, the approach distance calculation unit 1052A estimates the movement destination 10521 of the human position at the time after the time when the human position information 1030 is acquired, using the acquired human position information 1030 and the human movement speed 10560. The movement destination 10521 of the human position is output to the relative speed calculation unit 1054A.
  • Person positions H1 to H3 indicate person positions in the control cycle corresponding to hand positions P1 to P3, respectively.
  • the destination 10521 of the human position corresponds to H2 and H3.
  • the human position H1 is obtained by the hand position obtaining unit 1051 before the operation of the robot hand 3b starts.
  • the human movement speed HV is calculated by a human movement speed calculation unit 1056.
  • the approach distance calculation unit 1052A shown 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. Thereafter, the approach distance 10520 is output to the collision probability calculation unit 1055.
  • La1, La2 and La3 are respectively created as line segments between the human positions H1 to H3 and the hand positions P1 to P3 shown in FIG.
  • 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
  • the line segment between them is La3.
  • the approaching distance 10520 is calculated using the line segments La1 to La3.
  • the minimum value among the lengths of the line segments La1 to La3 is set as the approaching distance 10520.
  • three points of hand tip positions are set is illustrated in the above-mentioned example, one or more hand tip positions should just be provided.
  • a relative velocity calculation unit 1054A shown in FIG. 12 outputs a relative velocity 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 velocity 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.
  • V1 to V3 in the figure indicate the hand speed 10530 at the hand positions P1 to P3 calculated by the hand speed acquisition unit 1053.
  • the relative velocity calculation unit 1054A first calculates the relative velocity vectors RVa1 to RVa3 of the robot hand 3b with respect to the operator 5 by projecting the hand speeds V1 to V3 in the directions of the line segments La1 to La3. Further, relative speed calculation unit 1054A derives relative speed 10540 based on relative speed vectors RVa1 to RaV3.
  • the relative velocity vector directed to the worker 5 and whose relative absolute value becomes maximum is the relative velocity vector.
  • the vector be relative velocity 10540.
  • the relative velocity vectors RVa1 to RVa3 relative velocity vectors RVa1 and RVa2 directed to the worker 5 are selected, the absolute values of the selected vectors are compared, and the relative velocity vector RVa1 having a large absolute value is compared. It is assumed that the speed is 10540.
  • the embodiment in order to determine the possibility of collision in consideration of the moving speed of the worker 5 in the case where the worker 5 is operating to approach the commanded trajectory of the robot 3 temporarily, the embodiment is carried out.
  • the collision distance is calculated to be higher because the approach distance is smaller, and as a result, the correction of the first trajectory is facilitated.
  • the collision distance is calculated to be lower due to the approach distance becoming larger compared to the first embodiment, and as a result, it is unnecessary. No correction of the first trajectory is made. Therefore, in comparison with the robot control device of the first embodiment, the robot control device of the present embodiment can improve the safety for the worker 5 and the operation efficiency of the robot.
  • FIG. 15 is a view showing an example of the configuration of a robot control apparatus 1C according to Embodiment 3 of the present invention.
  • 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 apparatus 1C includes a trajectory correction unit 107C instead of the trajectory correction unit 107, and includes a robot operation control unit 108A instead of the robot operation control unit 108.
  • FIG. 16 is a flow chart for explaining the operation of the robot control apparatus 1C according to the third embodiment of the present invention.
  • FIG. 17 is a flowchart for explaining the process of step S3A of FIG.
  • step S1 is the same as step S1 in FIG. 4.
  • step S2 is the same as step S2 of FIG. 4, that is, step S21 to S26 of FIG.
  • step S21 to S26 is the same as step S2 of FIG.
  • step S3A as shown in FIG. 16, the robot operation control unit 108A performs drive control of the robot 3.
  • step S3A will be described in detail with reference to FIG.
  • step S31A the robot operation control unit 108A acquires current hand tip information 1010 and person position information 1030.
  • step S32A the robot operation control unit 108A calculates the collision possibility in a preset cycle using the above-described collision evaluation index based on the current hand tip information 1010 and the human position information 1030.
  • the current hand tip information 1010 and the person position information 1030 indicate the latest hand tip information 1010 and the person position information 1030 that have been acquired.
  • step S33A robot operation control unit 108A determines whether the calculated collision probability is equal to or less than a threshold. If the collision possibility is equal to or less than the threshold (YES) in step S33A, the robot operation control unit 108A generates the control signal 10. On the other hand, if the collision possibility is larger than the threshold in step S33A (NO), the process proceeds to step S35A, and the robot operation control unit 108A generates the replanning command 1080.
  • step S3B it is determined whether there is a replanning instruction 1080.
  • step S3B when the replanning instruction 1080 is not generated by the robot operation control unit 108A (YES), the process proceeds to step S4.
  • the replanning command 1080 is generated by the robot operation control unit 108A in step S3B (NO)
  • the process returns to step S25, and the trajectory correction unit 107C corrects the command trajectory 1070.
  • the trajectory correction unit 107C When the replanning command 1080 is input, the trajectory correction unit 107C generates a second trajectory using the commanded trajectory 1070 instead of the first trajectory 1040 (step S25). The trajectory correction unit 107C sets the generated second trajectory as a new command trajectory 1070. The newly set commanded trajectory is generated by the trajectory correction unit 107C so that the collision possibility becomes lower than the previously set commanded trajectory 1070.
  • step S4 robot operation control unit 108A determines whether or not robot hand 3b has reached the target point, and if the target point has been reached (YES), the operation is ended, otherwise (NO ) Return to step S3A.
  • steps S25 to S3B are repeated until the collision probability of the commanded track 1070 becomes equal to or less than the threshold.
  • FIG. 18 is a view showing an example of the configuration of a robot control apparatus 1D according to Embodiment 4 of the present invention. The same or corresponding components as or to those in FIG.
  • robot control device 1D is connected to sensor 6 for acquiring information on members around robot hand 3b, as compared with robot control device 1 (first embodiment).
  • the difference is that the robot control device 1D includes the operation environment acquisition unit 109 and the interference determination unit 110.
  • the sensor 6 may be exemplified by a range sensor, an RGB-D (red green blue-depth) sensor, an ultrasonic sensor, and a capacitance sensor, and may be used in combination with the position detection device 2 in FIG.
  • the operating environment acquisition unit 109 uses the information from the sensor 6 to acquire positional information 1090 of members disposed in the operating range of the robot hand 3b. Thereafter, position information 1090 relating to this member is output by the operation environment acquisition unit 109 to the interference determination unit 110.
  • the members disposed in the operation range of the robot hand 3 b mean the configuration of the robot 3 other than the robot hand 3 b (the base of the robot 3, etc.).
  • the interference determination unit 110 uses the position information 1090 of the member and the trajectory prediction information of the first trajectory 1040 to determine whether the robot hand 3 b interferes with this member. If it is determined by the interference determination unit 110 that the trajectory needs to be corrected, the trajectory correction instruction signal 1100 is output to the trajectory correction unit 107D. Thus, the trajectory correction unit 107D corrects the trajectory based on the predicted trajectory information of the first trajectory 1040 and the position information 1090 of the members. On the other hand, when it is determined by the interference determination unit 110 that the trajectory correction is not necessary, the trajectory correction unit 107D does not perform the trajectory correction because the trajectory correction instruction signal 1100 is not input to the trajectory correction unit 107D.
  • the present embodiment it is possible to obtain a commanded trajectory corrected so as not to collide with a member present in the movement 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 to be described later.
  • FIG. 19 is a view showing an example of the configuration of a robot control apparatus 1E according to Embodiment 5 of the present invention.
  • the robot control device 1E shown in FIG. 19 is a modification of the robot control device 1B according to the second embodiment.
  • FIG. 19 the same components as those in FIG.
  • the trajectory correction unit 107 generates the second trajectory using the human position information 1030 output from the human position information acquisition unit 103.
  • the collision possibility estimation unit 105E outputs the reference position 1200 for generating the second trajectory, and the trajectory correction unit 107 substitutes 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.
  • collision possibility estimation unit 105E shown in FIG. 20 collision possibility calculation unit 1055E selects and outputs reference position 1200 from among the person position included in person position information 1030 and movement destination 10521 of the person position. The point is different from the collision possibility estimation unit 105A shown in FIG.
  • the collision possibility operation unit 1055E determines the reference position 1200 from the human positions H1, H2, H3 shown in FIG. 13 or 14 with reference to the approaching distance 10520 or the relative velocity 10540. For example, the collision possibility operation unit 1055E refers to the approach distance or the relative velocity corresponding to each of the person positions H1, H2, H3, selects the person position having the highest possibility of the 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 destination of the worker 5 can be reworded as the predicted position of the worker 5 in the future.
  • the robot control device 1E of the present embodiment it is possible to generate a movement trajectory of the robot hand 3b that has a lower possibility of collision with the worker 5.
  • Robot control device Control signal 2 Position detection device 20 Detection information 3 Robot 4 Work bench 5 Worker (work 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 motion environment acquisition unit 1056 human movement speed calculation unit (movement speed calculation unit)

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

作業物体の安全性を確保しつつ、高い作業効率が得られるようにロボット（３）の動作を制御するロボット制御装置（１）を得る。ロボット制御装置（１）は、現在位置から目標位置へ向かう第１軌道（１０４０）上の少なくとも１つの点において算出された衝突可能性（１０５０）に基づき、軌道修正の要否を判断する修正要否判断部（１０６）と、修正要否判断部（１０６）の判断結果に基づき、指令軌道を設定する軌道修正部（１０７）とを備える。軌道修正部（１０７）は、修正要否判断部（１０６）にて軌道修正が必要と判断された場合には、物体位置情報および第１軌道（１０４０）に基づき、第１軌道（１０４０）の衝突可能性（１０５０）よりも小さい衝突可能性を有する第２軌道を指令軌道として設定し、修正要否判断部（１０６）にて軌道修正が必要でないと判断された場合には、第１軌道（１０４０）を指令軌道として設定する。

Description

ロボット制御装置およびこれを用いたロボットシステム
　本発明は、ロボットを制御するロボット制御装置およびこれを用いたロボットシステムに関する。
　近年、自律的に動作する自律型ロボット、作業者等の作業物体が、安全柵で仕切られることなくロボットと作業空間を共有して作業を行うことができるロボットシステムの開発が進められている。このようなロボットシステムでは、作業物体がロボットの動作範囲に進入する場合があり、このような場合でも作業物体の安全性を担保する必要がある。
　ところで、一般的に作業物体とロボットとが近くに位置すればする程、そして両者が高速に移動すればする程、両者の衝突可能性が高まる。よって、作業物体に対する安全性の観点からすると、両者が互いに離れて位置する段階、そして両者が低速で移動している段階で、ロボットの動作を制限することが望ましい。一方で、ロボットの作業効率の観点からすると、できる限りロボットの動作を制限しないことが望ましい。
　そこで、作業物体への安全性とロボットの作業効率とを両立させるための取り組みがなされている。例えば特許文献１では、ロボットを極力動作させながらロボットと作業物体との衝突を回避する手法が開示されている。具体的には、ロボットのアームの先端を監視対象部として、この監視対象部が人等の作業物体に向かう方向の速度成分を有していない場合は動作制限を実施しない。一方、監視対象部が当該作業物体に向かう方向の速度成分を有している場合は、その速度成分に応じて設定される安全位置および作業物体との距離に基づいて、監視対象部を減速又は停止させる。
特開２０１１－１２５９７５号公報
　しかしながら、従来の技術によれば、作業物体が監視対象部の動作軌道上に侵入した場合、ロボットは、作業物体との衝突を回避するために監視対象部を減速又は停止させるため、作業物体が監視対象部の動作軌道上から離れなければ、ロボット自身では作業を再開することができない。したがってロボットの作業効率が低下するという問題があった。
　本発明は、上述のような事情を鑑みてなされたものであって、作業物体の安全性を確保しつつ、高い作業効率が得られるようにロボットの動作を制御するロボット制御装置およびこれを用いたロボットシステムを提供することを目的とする。
　本発明に係るロボット制御装置は、ロボットにおいて監視対象として設定された監視対象部が指令軌道に沿って動作するようにロボットを制御するロボット制御装置であって、監視対象部の現在位置および監視対象部の目標位置に基づき、現在位置から目標位置へ向かう第１軌道を演算する第１軌道演算部と、作業物体の位置を含む物体位置情報、および第１軌道上の少なくとも１つの点における監視対象部の予測軌道情報に基づき、監視対象部の作業物体に対する衝突可能性を推定する衝突可能性推定部と、推定された衝突可能性に基づき、第１軌道の軌道修正が必要か否かを判断する修正要否判断部と、修正要否判断部の判断結果に基づき、指令軌道を設定する軌道修正部とを備え、軌道修正部は、修正要否判断部にて軌道修正が必要と判断された場合には、物体位置情報および第１軌道に基づき、第１軌道の衝突可能性よりも小さい衝突可能性を有する第２軌道を指令軌道として設定し、修正要否判断部にて軌道修正が必要でないと判断された場合には、第１軌道を指令軌道として設定するものである。
　本発明に係るロボット制御装置およびこれを用いたロボットシステムにあっては、作業物体の安全性を確保しつつ、高い作業効率が得られるようにロボットの動作を制御することができる。
図１は本発明の実施の形態１に係るロボット制御装置およびこれを備えたロボットシステムを説明するための上面概略図である。 図１に示すロボット制御装置の構成を示すブロック図である。 軌道修正部にて行われる第１軌道の修正方法の一例示す説明図である。 本発明の実施の形態１に係るロボット制御装置の動作のフローチャートである。 図５は図４のステップＳ２をより詳細に説明するフローチャートである。 本発明の実施の形態１に係る衝突可能性推定部の構成の一例を示すブロック図である。 本発明の実施の形態１に係る接近距離の演算方法の一例に関する説明図である。 本発明の実施の形態１に係る相対速度を演算する方法の一例に関する説明図である。 本発明の実施の形態１に係るロボット制御装置を実現するためのハードウェア構成図である。 本発明の実施の形態１に係るロボット制御装置の変形例を示すブロック図である。 本発明の実施の形態２に係るロボット制御装置の構成例を示すブロック図である。 衝突可能性推定部の構成例を示すブロック図である。 本発明の実施の形態２に係る接近距離演算法の一例を示した図である。 本発明の実施の形態２に係る相対速度演算の一例を示した図である。 本発明の実施の形態３に係るロボット制御装置の構成の一例を示す図である。 本発明の形態３に係るロボット制御装置の動作を説明するフローチャートである。 図１６のステップＳ３Ａの処理を説明するためのフローチャートである。 本発明の実施の形態４に係るロボット制御装置の構成の一例を示す図である。 本発明の実施の形態５に係るロボット制御装置の構成の一例を示す図である。 衝突可能性推定部の構成例を示すブロック図である。
実施の形態１．
　図１は本発明の実施の形態１に係るロボット制御装置１およびこれを備えたロボットシステムを説明するための上面概略図である。図中、ロボットシステムは、少なくともロボット制御装置１およびロボット３を備える。ロボットシステムが位置検出装置２を備えていてもよい。
　ロボット制御装置１には、位置検出装置２およびロボット３が接続される。ロボット制御装置１は、位置検出装置２が取得した検出情報２０およびロボット３から取得した関節角度等の情報に基づいて、ロボット３に対して制御信号１０を出力する。ロボット制御装置１からの制御信号１０に応じてロボット３が駆動されることにより、ロボット手先３ｂが指令軌道に沿って動作する。
　位置検出装置２は、ロボット３の周囲にて作業する作業者５を制御周期ごとに検出し、検出結果を検出情報２０としてロボット制御装置１へ出力する。
　ロボット３は、アーム３ａおよびアーム３ａの先端に設けられたロボット手先３ｂを有する。アーム３ａの関節には、関節角度を制御する駆動装置（図示省略）が設けられる。なお、駆動装置はサーボモータ、ステッピングモータ等の電動モータにより構成される。ロボット３は各関節の関節角度を駆動装置により変更することで位置および姿勢を自在に変更可能である。駆動装置として、電動モータの代わりに、空気圧又は油圧を利用したシリンダを用いてもよい。
　ロボット３は、アーム３ａの関節角度を計測する画像センサ、エンコーダ等の関節角度計測装置（図示省略）を有する。よって、ロボット制御装置１は、ロボット３の関節角度計測装置で得られた関節角度を取得し、この取得した関節角度およびロボット３のアーム３ａのアーム長などの機器情報を用いて、ロボット手先３ｂの位置を演算可能である。
　なお、ロボット手先３ｂを監視対象部として設定した場合を例に挙げて以下の説明を行うが、監視対象部はこの例には限定されず、監視対象部をロボット３の任意の場所に設定してもよい。また、監視対象部は一箇所に限らず複数箇所設定してもよい。
　本実施の形態に係る作業現場には作業台４が配置される。ロボット３の周囲には作業を行う作業者５が存在し、ロボット３はこの作業者５と共に作業空間を共有して動作する。作業台４上には、ロボット３および作業者５の双方が利用可能な領域として、作業領域６１および６２が設定される。この作業領域６１および６２において、ロボット３は、作業対象物７１および７２に対してロボット手先３ｂで運搬、加工、検査、搬出等の作業を施す。作業領域６１および６２において、作業者５も作業対象物７１および７２に対してロボット３と同様な作業を実施できる。なお、作業領域、作業対象物の個数等の作業現場の構成は上述の例に限定されるものではない。
　図中、作業者５が作業対象物７２を保持した状態である。ロボット３が作業領域６２へ向かって（破線矢印）ロボット手先３ｂを移動する前の状態であることがわかる。
　なお、作業者５すなわち人がロボット３と共に作業を行う場合を例に挙げて以下の説明を行うが、倉庫内等で作業を行うピッキングロボット等の自律的に動作する装置がロボット３と作業する場合であってもよい。作業物体はロボット３に対して自律的に動作する装置または作業者を意味する。
　ここで、位置検出装置２についてより詳細に説明する。位置検出装置２は、レンジセンサ、ＲＧＢ－Ｄ（Ｒｅｄ　Ｇｒｅｅｎ　Ｂｌｕｅ－Ｄｅｐｔｈ）センサ、超音波センサ、静電容量センサ等のセンサにより構成される。上述のセンサでは検出ができない領域の検出情報２０を取得するために、又は作業者５の検出精度を向上するために、マットスイッチ、ライトカーテン等の追加のセンサを用いてもよい。
　また、図１では位置検出装置２が作業現場に設置されているが、位置検出装置２がロボット３に設置されていてもよい。ここで、位置検出装置２を作業現場に固定した場合の座標系での位置は、位置検出装置２をロボット３に固定した場合の座標系での位置と、相互に変換可能である。よって、位置検出装置２の設置場所を変更した場合でも、上述の例と同様に計算できる。
　図２は図１に示すロボット制御装置１の構成を示すブロック図である。図２に示すロボット制御装置１は、手先情報取得部１０１、目標点情報取得部１０２、人位置情報取得部１０３、第１軌道演算部１０４、衝突可能性推定部１０５、修正要否判断部１０６、軌道修正部１０７、およびロボット動作制御部１０８を備える。
　手先情報取得部１０１は、関節角度計測装置で取得された関節角度に基づき、ロボット３の手先情報１０１０を演算する。その後、手先情報取得部１０１は、この手先情報１０１０を第１軌道演算部１０４へ出力する。
　より具体的には、手先情報取得部１０１は、関節角度と関節角速度をロボット３の予め作成された筺体モデルへ適用することにより、ロボット３の手先位置、手先速度を含む手先情報１０１０を演算する。ロボット３の筺体モデルは、ロボット制御装置１の記憶装置（図９に図示）に記憶される。関節角速度はロボット３の関節角度の時間差分を行うことにより演算される。この演算された関節角速度に対して、ローパスフィルタを用いて、ノイズを除去してもよい。
　目標点情報取得部１０２は、記憶装置から目標点情報１０２０を取得する。目標点情報１０２０は、ロボット手先３ｂの目標位置および目標姿勢に関する情報を含む。目標点情報１０２０は予めロボット３への教示作業によりロボット制御装置１の記憶装置（図９に後述）に記憶される。なお、予め作成して記憶された目標点情報１０２０を用いる代わりに、目標点情報１０２０がロボット３の動作中に作業者５によって指定されてもよい。
　人位置情報取得部１０３は、位置検出装置２からの検出情報２０を用いて、作業者５の代表点の位置座標である人位置情報（物体位置情報）１０３０を取得する。人位置情報１０３０は、衝突可能性推定部１０５および軌道修正部１０７へ出力される。
　ここで、検出情報２０から人位置情報１０３０を取得する方法の一例を以下に述べる。検出情報２０が作業者５の複数の身体部位を点群とした情報である場合、検出した点群のうちロボット３から最も近い距離にある代表点を人位置情報１０３０として設定できる。
　なお、代表点の設定方法は上述の例に限らず、ロボット３の動作条件、作業現場等の構成によって適宜変更してもよい。検出情報２０がＲＧＢ－Ｄセンサによって取得された画像情報である場合、この画像情報に対し画像処理を行うことで作業者５の人体モデルを構築する。その後、この構築された人体モデルの関節およびリンクの座標を計算することで、人位置情報１０３０を取得してもよい。この場合、精密な人体モデルを構築できるため、ロボット３の作業者５に対する衝突可能性を低減することができる。
　第１軌道演算部１０４は、手先情報１０１０および目標点情報１０２０を用いて、第１軌道１０４０を計算する。具体的には、第１軌道演算部１０４は、まず手先情報１０１０からロボット手先３ｂの現在位置を取得し、ロボット制御装置１の記憶装置に予め記憶された目標点情報１０２０からロボット手先３ｂの目標位置を取得する。次に、第１軌道演算部１０４は現在位置から目標点へ向かう第１軌道を計算する。その後、第１軌道１０４０が衝突可能性推定部１０５および軌道修正部１０７へ出力される。
　ここで、第１軌道１０４０は、制御周期に対応したそれぞれの時刻（ｔ＝ｔ１，t２，…，ｔｎ－１，ｔｎ，…）におけるロボット手先３ｂの手先位置（予測位置）、予測姿勢、および手先速度（予測速度）に関する予測軌道情報を含む。なお、ロボット手先３ｂの速度は、例えば以下のとおり導出される。第１軌道１０４０上の時刻（ｔ＝ｔ１，t２，…，ｔｎ－１，ｔｎ，…）に対応する手先位置（Ｐ（０），Ｐ（ｔ１），Ｐ（ｔ２），…，Ｐ（ｔｎ－１），Ｐ（ｔｎ），…）のそれぞれについて、１制御周期前の時刻における位置との差分を取り（例えば、Ｐ（ｔｎ）－Ｐ（ｔｎ－１））、この差分値を（ｔｎ―ｔｎ－１）で割ることで、各時刻におけるロボット手先３ｂの手先速度が演算できる。
　なお、第１軌道１０４０の軌道の導出方法は、滑らかな軌道となるように始点と終点を複数の点で結ぶＰＴＰ（Ｐｏｉｎｔ　Ｔｏ　Ｐｏｉｎｔ）軌道として導出する。この滑らかな軌道を生成する方法として関節空間補間軌道を用いるがこれに限定されるものではない。またＰＴＰ軌道の代わりにＣＰ（Ｃｏｎｔｉｎｕｏｕｓ　Ｐａｔｈ）軌道を用いてもよく、このＣＰ軌道では始点と終点までの軌道が連続関数として導出される。
　衝突可能性推定部１０５は、第１軌道１０４０上の少なくとも一点におけるロボット手先３ｂの予測軌道情報と作業者５の人位置情報１０３０とに基づき、作業者５とロボット手先３ｂとの衝突の可能性を示す衝突可能性１０５０を推定する。衝突可能性１０５０の具体的な推定方法については図６の衝突可能性演算部１０５５と共に詳細に説明する。
　修正要否判断部１０６は、衝突可能性１０５０に基づいて、第１軌道１０４０の修正が必要か否かを判断する。例えば、修正要否判断部１０６は算出された衝突可能性１０５０が予め設定された閾値を超えた場合に、第１軌道１０４０の修正が必要であると判断し、修正要否判断部１０６は軌道修正部１０７へ修正指示信号１０６０を出力する。なお、修正要否判断部１０６は、修正指示信号１０６０と共に衝突可能性１０５０を軌道修正部１０７へ出力する。
　修正要否判断部１０６から修正指示信号１０６０が入力された場合には、軌道修正部１０７は、人位置情報１０３０および第１軌道１０４０に基づいて、第１軌道１０４０よりも衝突可能性が低い第２軌道を生成する。軌道修正部１０７は、生成された第２軌道を指令軌道１０７０として設定する。修正要否判断部１０６から修正指示信号１０６０が入力されない場合には、軌道修正部１０７は第１軌道１０４０を指令軌道１０７０として出力する。
　図３は、軌道修正部１０７による第１軌道１０４０の修正方法を示す説明図である。図中、第２軌道Ｑは、第１軌道Ｐを修正した後の軌道の一例である。図中、Ｐ０およびＰｆはそれぞれロボット３の位置および目標位置を示す。また、Ｌ０は人位置Ｈ１とＰ０とを結ぶ線分を示し、Ｌｆは目標位置Ｐｆと人位置Ｈ１とを結ぶ線分を示す。線Ｃ０は中心Ｈ１であり、半径が線分Ｌ０の長さである円を示す。
　第２軌道Ｑは、例えば、人位置Ｈ１から見たときに、第２軌道Ｑの全ての点において第１軌道Ｐよりも外側を通過するように設定される。図３に示すように第２軌道Ｑは人位置Ｈ１から見たときに外側に凸の曲線となるように設定されていてもよい。上述のように第２軌道を設定することにより、第１軌道Ｐと比較して、作業者５とロボット手先３ｂとの接近距離が長くなっているため、作業者５に衝突しにくい軌道とすることができ、衝突可能性を低くすることができる。
　図３に示す第２軌道Ｑは、人位置Ｈ１を中心とし、線分Ｌ０を半径とする円弧状の軌道（Ｃ０）から、人位置Ｈ１を中心とし、線分Ｌｆを半径とする円弧状の軌道へと滑らかに移行する軌道となっている。言い換えると、図３において、ロボット手先３ｂが軌道修正時の位置Ｐ０から目標位置Ｐｆへと向かうにつれて、ロボット手先３ｂから作業者５までの距離が単調に変化（単調減少または単調増加）するように、第２軌道Ｑが生成される。このように第２軌道Ｑが生成されることによって、第２軌道Ｑの各点において、ロボット手先３ｂから作業者５までの最短距離は、線分Ｌ０又は線分Ｌｆの長さとなる。したがって、ロボット手先３ｂが作業者５に近付き過ぎることを防止でき、衝突可能性を低くすることができる。
　また、人位置Ｈ１から見たときに外側に凸の曲線となるように第２軌道Ｑを設定することで、ロボット手先３ｂの移動速度（手先速度）のうち、人位置Ｈ１に向かう速度成分が小さくなる。この結果、ロボット手先３ｂの衝突可能性を低くすることができる。また、人位置Ｈ１に向かう速度成分が小さくなると、手先速度を速くして目標位置Ｐｆへの到達時間を短縮することも可能となる。なお、図３は第２軌道Ｑの一例を示しており、第２軌道Ｑとしては他の軌道であっても良い。
　上述のように第１軌道１０４０を修正する代わりに、軌道修正部１０７は予め作成された複数の修正軌道から、第１軌道１０４０よりも衝突可能性１０５０が小さな修正軌道、例えば、衝突可能性１０５０が最も小さな修正軌道を選択し、選択した修正軌道を第２軌道として用いてもよい。また、軌道修正部１０７は、第１軌道１０４０よりも衝突可能性１０５０が小さく、衝突可能性１０５０と目標位置Ｐｆへの到達時間とのバランスの良い修正軌道を選択し、選択した修正軌道を第２軌道として用いてもよい。なお、複数の修正軌道はロボット制御装置１の記憶装置（図９に図示）に保存される。
　ロボット動作制御部１０８は、指令軌道１０７０に基づき、ロボット３に対して制御周期ごとに制御信号１０を出力する。
　なお、ロボット動作制御部１０８は、指令軌道１０７０に加えて手先情報１０１０と人位置情報１０３０を用いて、ロボット手先３ｂの動作中に、制御周期ごとに算出される衝突可能性に応じて、その速度を変更する構成としてもよい。具体的には、ロボット動作制御部１０８は、人位置情報１０３０および手先情報１０１０から制御周期ごとに算出される衝突可能性について、その衝突可能性の値が大きく（小さく）なるにつれてロボット手先３ｂの速度を低下（増加）させる制御信号１０を作成してもよい。
　図４は本実施の形態のロボット制御装置１に係る動作のフローチャートである。まず、ステップＳ１にて、ロボット制御装置１は、位置検出装置２から検出情報２０を取得し、ロボット３から関節角等のデータを取得する。
　ステップＳ２にて、ロボット制御装置１は得られたデータに基づいて指令軌道１０７０を設定する。ステップＳ３にて、ロボット制御装置１は、指令軌道１０７０に基づき、制御周期ごとに制御信号１０を出力する。ステップＳ４にてロボット手先３ｂが目標点に到達すれば（ＹＥＳ）、ロボット３は動作を終了する。ステップＳ４にてロボット手先３ｂが目標点に到達していなければ（ＮＯ）、ステップＳ３に戻る。
　図５は図４のステップＳ２をより詳細に説明するフローチャートである。ステップＳ２１にて、ロボット３の制御に必要なデータを準備する。具体的には、まず手先情報取得部１０１、目標点情報取得部１０２、および人位置情報取得部１０３は、それぞれロボット３の現在の手先情報１０１０、目標点情報１０２０、および人位置情報１０３０を取得する。
　ステップＳ２２にて、第１軌道演算部１０４がロボット３の手先情報１０１０および目標点情報１０２０を用いて、第１軌道１０４０を演算する。
　ステップＳ２３にて、衝突可能性推定部１０５は、人位置情報１０３０および第１軌道１０４０における予測軌道情報を用いて、衝突可能性１０５０を推定する。
　ステップＳ２４にて、修正要否判断部１０６は、衝突可能性１０５０の値に基づき、第１軌道１０４０の修正が必要か否かを判断する。具体的には、ステップＳ２４にて、衝突可能性１０５０が閾値以上であれば（ＹＥＳ）、修正指示信号１０６０を生成してステップＳ２５に進むが、衝突可能性１０５０が閾値より小さければ（ＮＯ）、修正指示信号１０６０を生成せずステップＳ２６に進む。
　ステップＳ２５にて、軌道修正部１０７は、修正指示信号１０６０の入力を受けて、第１軌道１０４０に比べて、衝突可能性が低い第２軌道へ修正し、第２軌道を指令軌道１０７０として設定する。
　ステップＳ２６では、軌道修正部１０７は、修正指示信号１０６０の入力がないため、第１軌道１０４０を指令軌道１０７０として設定する。
　図６は、衝突可能性推定部１０５の構成の一例を示すブロック図である。衝突可能性推定部１０５は、手先位置取得部１０５１、接近距離演算部１０５２、手先速度取得部１０５３、相対速度演算部１０５４、および衝突可能性演算部１０５５を備える。
　手先位置取得部１０５１は、第１軌道１０４０の予測軌道情報から、ロボット３の手先位置１０５１０を取得する。その後、この取得された手先位置１０５１０は、後述する接近距離演算部１０５２および相対速度演算部１０５４へ出力される。
　手先速度取得部１０５３は、第１軌道１０４０上の少なくとも一点における予測軌道情報から手先速度１０５３０を取得する。この取得されたロボット３の手先速度１０５３０は、相対速度演算部１０５４へ入力される。
　接近距離演算部１０５２は、手先位置１０５１０および人位置情報１０３０を用いて、ロボット手先３ｂと作業者５との接近距離１０５２０を演算する。
　ここで、図７を用いて接近距離１０５２０を演算する方法を詳細に説明する。図７は接近距離１０５２０の演算方法の一例に関する説明図である。
　まず、接近距離演算部１０５２は、第１軌道Ｐ上に設定されたｎ点の手先位置Ｐ１，Ｐ２，Ｐ３，～，Ｐｎについて、このｎ点の手先位置Ｐ１～Ｐｎのそれぞれと人位置Ｈ１とを結ぶ線分を、それぞれ線分Ｌ１，Ｌ２，Ｌ３，～，Ｌｎとして設定する。具体的には、線分Ｌ１、Ｌ２、およびＬ３は、それぞれ手先位置Ｐ１と人位置Ｈ１とを結ぶ線分、手先位置Ｐ２と人位置Ｈ１とを結ぶ線分、および手先位置Ｐ３と人位置Ｈ１とを結ぶ線分である。また、以下の説明では、ｎ＝３すなわち３つの手先位置を設定した場合を例に挙げて説明を行うが、少なくとも１つの手先位置を設定していればよい。
　次に、接近距離演算部１０５２は、線分Ｌ１～Ｌ３の長さに基づいて、接近距離１０５２０を導出する。上述の例のように複数の線分が導出される場合には、例えば、線分Ｌ１～Ｌ３のうちその長さが最も小さい線分を選択し、その選択された線分の長さを接近距離１０５２０として選択する。例えば、図７では、線分Ｌ１～Ｌ３のうち線分の長さが最小値となる線分Ｌ２の長さを接近距離１０５２０としている。
　なお、図７および図８では、各手先位置からその手先位置に隣接する手先位置までの移動距離が予め設定された距離となるように手先位置Ｐ１～Ｐ３を設定している。一方で、各手先位置からその手先位置に隣接する手先位置までの移動時間が予め設定された時間となるように手先位置Ｐ１～Ｐ３を設定してもよい。
　ここで、図６および図８を用いて相対速度演算部１０５４の処理を説明する。図６に示すとおり、相対速度演算部１０５４は、人位置情報１０３０、手先位置１０５１０、および手先速度１０５３０に基づき、ロボット手先３ｂの作業者５に対する相対速度１０５４０を演算する。
　図８は、相対速度１０５４０を演算する方法の一例に関する説明図である。図８を用いて相対速度演算部１０５４が相対速度１０５４０を演算する方法を詳細に説明する。図中Ｖ１～Ｖ３は手先速度取得部１０５３にて演算された手先位置Ｐ１～Ｐ３における手先速度１０５３０を示す。
　最初に、相対速度演算部１０５４は、手先速度Ｖ１～Ｖ３を線分Ｌ１～Ｌ３方向へ射影することにより、作業者５に対するロボット手先３ｂの相対速度ベクトルＲＶ１～ＲＶ３を演算する。例えば、図８ではＲＶ１、ＲＶ２およびＲＶ３は、それぞれ手先速度Ｖ１を線分Ｌ１へ射影した相対速度ベクトル、手先速度Ｖ２を線分Ｌ２へ射影した相対速度ベクトル、および手先速度Ｖ３に対して線分Ｌ３へ射影した相対速度ベクトルである。
　次に、相対速度演算部１０５４は相対速度ベクトルＲＶ１～ＲＶ３の値に基づき、相対速度１０５４０を導出する。上述の例のように複数の相対速度ベクトルが演算された場合には、例えば、相対速度ベクトルＲＶ１～ＲＶ３のうち、作業者５へ向かう相対速度ベクトルであってその絶対値が最大となる相対速度ベクトルを相対速度１０５４０とする。具体的には、相対速度ベクトルＲＶ１～ＲＶ３のうち、作業者５に向かう相対速度ベクトルであるＲＶ１、ＲＶ２を選択し、選択された両ベクトルの絶対値を比較して絶対値が大きい相対速度ベクトルＲＶ１を相対速度１０５４０とする。
　図６に戻り、衝突可能性推定部１０５の残りの構成の説明を行う。衝突可能性演算部１０５５は、例えば、入力された接近距離１０５２０および相対速度１０５４０に基づき、衝突可能性１０５０を演算する。衝突可能性１０５０を演算する方法の一例としては、予測軌道情報と作業者５の位置情報について予め作成された衝突評価指標を参照することで算出される。衝突評価指標はロボット制御装置１が有する記憶装置（図９に図示）に保存される。衝突評価指標は、例えば接近距離１０５２０が小さくなるにつれて、また相対速度１０５４０が大きくなるにつれて、衝突可能性１０５０が大きな値となるように設定される。
　また、衝突評価指標の別の例として、人位置情報１０３０の経時変化から算出された作業者５の移動速度を用い、作業者５とロボット３との仮想的な衝突予測時間を算出する。次に、算出された衝突予測時間が短くなるにつれて衝突可能性が高くなるように作成された衝突評価指標を用いる。さらに、作業者５が侵入した場合にロボット３との衝突が予想される範囲を、ロボット３が設置された位置の周囲に予め設定し、この範囲内に人位置情報１０３０が存在するか否かにより、衝突可能性を推定してもよい。
　なお、接近距離１０５２０および相対速度１０５４０の両方に基づき衝突可能性１０５０を演算する例を説明したが、接近距離１０５２０および相対速度１０５４０のいずれか一方のみに基づき、衝突可能性１０５０を演算する構成としてもよい。この場合、衝突評価指標は、接近距離１０５２０および相対速度１０５４０のいずれか一方のみに応じて衝突可能性１０５０が変化するように作成される。このとき、予測軌道情報は手先位置１０５１０又は手先速度１０５３０のいずれか一方を含んでいればよい。
　本実施の形態によれば、ロボット３の指令軌道上で予め衝突可能性が高いと判定した場合には、ロボット３と作業者５との衝突可能性１０５０を低下させるように第１軌道１０４０を修正するため、ロボット３の第１軌道１０４０に作業者５の侵入が予想される場合においてもロボット３の作業効率を維持したロボット動作が可能となる。
　図９はロボット制御装置１を実現するためのハードウェア構成図である。図９に示すとおり、上記ロボット制御装置１の一部又は全部は、具体的には、ＣＰＵ１５１（Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）、記憶装置１５２、ＩＯ（ＩＮＰＵＴ　ＯＵＴＰＵＴ：入出力）インターフェース１５３、およびシステムバス１５４等により構成される。記憶装置１５２はＲＯＭ（Ｒｅａｄ　Ｏｎｌｙ　Ｍｅｍｏｒｙ）、ＨＤＤ（Ｈａｒｄ　Ｄｉｓｋ　Ｄｒｉｖｅ）等から構成される。ロボット制御装置１のＩＯインターフェース１５３には、入力装置２０１および出力装置２０２がケーブル１５５を介して接続される。なお、記憶装置１５２には、先述したロボット３の筺体モデル、目標点情報１０２０、衝突評価指標、軌道修正部１０７で用いられる第２軌道の候補となる修正軌道等が保存されている。
　ロボット制御装置１の各処理はＣＰＵ１５１により実行される。手先情報取得部１０１、目標点情報取得部１０２、人位置情報取得部１０３、およびロボット動作制御部１０８については、ＩＯインターフェース１５３を介して、ロボット制御装置１の外部構成（位置検出装置２およびロボット３等）との入力および出力を行う。
　図１０は、本発明の実施の形態１に係るロボット制御装置１の変形例を示すブロック図である。図２では手先情報取得部１０１がロボット３の関節角度計測装置からの情報を用いて手先情報１０１０を取得していたが、図１０に示したロボット制御装置１Ａのように、第１軌道演算部１０４Ａが、制御信号１０を用いて現在の手先情報１０１０を導出する構成としてもよい。この場合、ロボット制御装置１Ａにおいて手先情報取得部１０１を省略することができる。
実施の形態２．
　図１１は本発明の実施の形態２に係るロボット制御装置１Ｂの構成例を示すブロック図である。衝突可能性推定部１０５の代わりに衝突可能性推定部１０５Ａを備える点が上述の実施の形態とは異なる。
　図１２は衝突可能性推定部１０５Ａの構成例を示すブロック図である。図１２に示す衝突可能性推定部１０５Ａは、人移動速度演算部（移動速度演算部）１０５６をさらに備え、作業者５の移動速度を考慮して衝突可能性１０５０を演算する点が、図６に示した衝突可能性推定部１０５と異なる。図１３は本発明の実施の形態２に係る接近距離１０５２０を演算する方法の一例を示した図である。図中、手先位置を３点設定した場合を示す。図中、人位置Ｈ１、ロボット３の第１軌道Ｐにおける手先位置Ｐ１～Ｐ３は、図７と同様である。
　最初に、接近距離演算部１０５２Ａは、現在の人位置情報１０３０および人移動速度１０５６０を用いて、各手先位置に対応する時刻（制御周期）における人位置の移動先１０５２１を求める。具体的には、図１３における人位置Ｈ２および人位置Ｈ３は、人位置Ｈ１および人移動速度ＨＶを用いて演算される。言い換えると、接近距離演算部１０５２Ａは、取得された人位置情報１０３０および人移動速度１０５６０を用いて、人位置情報１０３０が取得された時刻より後の時刻における人位置の移動先１０５２１を推定する。人位置の移動先１０５２１は相対速度演算部１０５４Ａに出力される。人位置Ｈ１～Ｈ３は、それぞれ手先位置Ｐ１～Ｐ３に対応した制御周期における人位置を示す。人位置の移動先１０５２１は、Ｈ２およびＨ３に対応する。ここで、人位置Ｈ１はロボット手先３ｂの動作開始前に、手先位置取得部１０５１で取得される。人移動速度ＨＶは人移動速度演算部１０５６により演算される。
　次に、図１２に示す接近距離演算部１０５２Ａは、人位置情報１０３０、人位置の移動先１０５２１、および手先位置１０５１０に基づいて、ロボット手先３ｂと作業者５との接近距離１０５２０を演算する。その後、接近距離１０５２０は、衝突可能性演算部１０５５へ出力される。
　より詳細には、図１３に示した人位置Ｈ１～Ｈ３と手先位置Ｐ１～Ｐ３との間の線分として、それぞれＬａ１，Ｌａ２，Ｌａ３を作成する。例えば図１３では、手先位置Ｐ１と人位置Ｈ１との間の線分がＬａ１であり、手先位置Ｐ２と人位置Ｈ２との間の線分がＬａ２であり、手先位置Ｐ３と人位置Ｈ３との間の線分がＬａ３である。さらに、線分Ｌａ１～Ｌａ３を用いて接近距離１０５２０を演算する。具体的には、線分Ｌａ１～Ｌａ３の長さのうちの最小値を接近距離１０５２０とする。なお、上述の例では手先位置を３点設定した場合を例示しているが、手先位置は一点以上設けられていればよい。
　図１２示す相対速度演算部１０５４Ａは、手先位置１０５１０、人位置の移動先１０５２１、手先速度１０５３０、および人移動速度１０５６０に基づいて、ロボット手先３ｂの作業者５に対する相対速度１０５４０を出力する。
　図１４は本発明の実施の形態２に係る相対速度１０５４０の演算方法の一例を示した図である。図１４において、図１３と同様の構成は同符号を付し説明を省略する。図８と同様に、図中Ｖ１～Ｖ３は手先速度取得部１０５３にて演算された手先位置Ｐ１～Ｐ３における手先速度１０５３０を示す。
　相対速度演算部１０５４Ａは、まず、手先速度Ｖ１～Ｖ３を線分Ｌａ１～Ｌａ３方向へ射影することにより、作業者５に対するロボット手先３ｂの相対速度ベクトルＲＶａ１～ＲＶａ３を演算する。さらに相対速度演算部１０５４Ａは、相対速度ベクトルＲＶａ１～ＲａＶ３に基づき、相対速度１０５４０を導出する。
　上述の例のように複数の相対速度ベクトルが演算された場合には、例えば、相対速度ベクトルＲＶａ１～ＲＶａ３のうち、作業者５へ向かう相対速度ベクトルであってその絶対値が最大となる相対速度ベクトルを、相対速度１０５４０とする。例えば、相対速度ベクトルＲＶａ１～ＲＶａ３のうち、作業者５に向かう相対速度ベクトルであるＲＶａ１、ＲＶａ２を選択し、選択された両ベクトルの絶対値を比較して絶対値が大きい相対速度ベクトルＲＶａ１を相対速度１０５４０とする。
　本実施の形態によれば、作業者５の人移動速度を考慮して衝突可能性を判定するために仮に作業者５がロボット３の指令軌道に接近するように動作していた場合では、実施の形態１に比べ接近距離が小さくなることでより衝突可能性が高く算出され、結果として第１軌道の修正が行われやすくなる。一方で、作業者５がロボット３の指令軌道から遠ざかるように動作していた場合では、実施の形態１に比べ接近距離が大きくなることでより衝突可能性が低く算出され、結果として不必要な第１軌道の修正が行われない。したがって、実施の形態１のロボット制御装置と比較して、本実施の形態のロボット制御装置では、作業者５への安全性とロボットの動作効率を向上させることができる。
実施の形態３．
　図１５は本発明の実施の形態３に係るロボット制御装置１Ｃの構成の一例を示す図である。図１５において、図２と同様の構成は同符号を付し説明を省略する。ただし、図１５に示す衝突可能性推定部１０５は、図１１および図１２に示す衝突可能性推定部１０５Ａであってもよく、後述する図１９および図２０に示す衝突可能性推定部１０５Ｅであってもよい。
　ロボット制御装置１Ｃは、軌道修正部１０７に代わり軌道修正部１０７Ｃを備え、ロボット動作制御部１０８に代わりロボット動作制御部１０８Ａを備える。
　図１５～図１７を用いて本実施の形態に係るロボット制御装置１Ｃの構成および動作を説明する。図１６は、本発明の実施の形態３に係るロボット制御装置１Ｃの動作を説明するフローチャートである。図１７は、図１６のステップＳ３Ａの処理を説明するためのフローチャートである。なお、図１６では、ステップＳ１は図４のステップＳ１と同様である。また、図中、ステップＳ２（ステップＳ２１～ステップＳ２６）は、図４のステップＳ２つまり図５のステップＳ２１～Ｓ２６と同様である。よって、それらのステップの説明は繰り返し行わない。
　ステップＳ３Ａにて、図１６に示すようにロボット動作制御部１０８Ａはロボット３の駆動制御を行う。ここで図１７を用いて、ステップＳ３Ａを詳細に説明する。
　まず、ステップＳ３１Ａでは、ロボット動作制御部１０８Ａが現在の手先情報１０１０および人位置情報１０３０を取得する。次に、ステップＳ３２Ａにて、ロボット動作制御部１０８Ａは、現在の手先情報１０１０および人位置情報１０３０に基づき、上述した衝突評価指標を用いて、予め設定された周期で衝突可能性を算出する。ここで、現在の手先情報１０１０および人位置情報１０３０とは、取得されている最新の手先情報１０１０および人位置情報１０３０を指す。ステップＳ３３Ａにて、ロボット動作制御部１０８Ａは、算出された衝突可能性が閾値以下であるか否かを判断する。ステップＳ３３Ａにて、衝突可能性が閾値以下（ＹＥＳ）であれば、ロボット動作制御部１０８Ａは制御信号１０を生成する。一方で、ステップＳ３３Ａにて、衝突可能性が閾値より大きければ（ＮＯ）、ステップＳ３５Ａに進み、ロボット動作制御部１０８Ａは再計画指令１０８０を生成する。
　図１６に戻り、ロボット制御装置１ＣのステップＳ３Ｂ以降の動作を説明する。ステップＳ３Ｂにて、再計画指令１０８０の有無を判断する。ステップＳ３Ｂにて、ロボット動作制御部１０８Ａにより再計画指令１０８０が生成されていない場合（ＹＥＳ）、ステップＳ４に進む。一方で、ステップＳ３Ｂにて、ロボット動作制御部１０８Ａにより再計画指令１０８０が生成されている場合は（ＮＯ）ステップＳ２５に戻り、軌道修正部１０７Ｃにより指令軌道１０７０を修正する。
　なお、再計画指令１０８０が入力されたとき、軌道修正部１０７Ｃは、第１軌道１０４０の代わりに指令軌道１０７０を用いて、第２軌道を生成する（ステップＳ２５）。軌道修正部１０７Ｃは、この生成された第２軌道を新たな指令軌道１０７０として設定する。この新たに設定された指令軌道は、予め設定された指令軌道１０７０に比べて衝突可能性が低くなるように軌道修正部１０７Ｃにより生成される。
　ステップＳ４にて、ロボット動作制御部１０８Ａはロボット手先３ｂが目標点に到達したか否かを判断し、目標点に到達している場合は（ＹＥＳ）動作を終了し、そうでない場合は（ＮＯ）ステップＳ３Ａに戻る。
　図１６のフローチャートでは、再計画指令が入力された場合に、指令軌道１０７０の衝突可能性が閾値以下となるまでステップＳ２５～ステップＳ３Ｂを繰り返す。
　本実施の形態によれば、ロボット３と作業者５との衝突可能性が高まる場合において、ロボット３の指令軌道１０７０を再計画することで作業者５への安全性とロボット３の動作効率が向上する。
実施の形態４．
　図１８は本発明の実施の形態４に係るロボット制御装置１Ｄの構成の一例を示す図である。図２と同じ又は対応する構成については同符号を付しその説明を繰り返さない。
　本実施の形態のロボット制御装置１Ｄでは、ロボット制御装置１（実施の形態１）と比較して、ロボット制御装置１Ｄがロボット手先３ｂの周囲の部材の情報を取得するセンサ６に接続されている点、および、ロボット制御装置１Ｄが動作環境取得部１０９と干渉判定部１１０とを有する点が異なる。センサ６はレンジセンサ、ＲＧＢ－Ｄ（Ｒｅｄ　Ｇｒｅｅｎ　Ｂｌｕｅ－Ｄｅｐｔｈ）センサ、超音波センサ、静電容量センサを例示することができ、図１における位置検出装置２と併用してもよい。
　動作環境取得部１０９は、センサ６からの情報を用いて、ロボット手先３ｂの動作範囲に配置されている部材の位置情報１０９０を取得する。その後、この部材に関する位置情報１０９０は動作環境取得部１０９により干渉判定部１１０へ出力される。
　なお、ロボット手先３ｂの動作範囲に配置された部材とは、ロボット３以外の部材に加えて、ロボット手先３ｂ以外のロボット３の構成（ロボット３の土台等）を意味する。
　干渉判定部１１０は、部材の位置情報１０９０および第１軌道１０４０の軌道予測情報を用いてロボット手先３ｂがこの部材へ干渉するか否かを判断する。干渉判定部１１０により軌道の修正が必要と判定された場合、軌道修正指示信号１１００が軌道修正部１０７Ｄへ出力される。これにより軌道修正部１０７Ｄは、第１軌道１０４０の予測軌道情報および部材の位置情報１０９０に基づき、軌道の修正を行う。一方で、干渉判定部１１０により軌道の修正が必要でない判定された場合、軌道修正指示信号１１００が軌道修正部１０７Ｄへ入力されないため、軌道修正部１０７Ｄによる軌道修正が行われない。
　本実施の形態によれば、ロボット手先３ｂの動作範囲に存在する部材に衝突しないように修正された指令軌道を得ることが可能である。なお、本実施の形態は、上述の実施の形態のロボット制御装置１Ａ～１Ｃ、後述する実施の形態のロボット制御装置１Ｅのいずれとも組み合わせることができる。
実施の形態５．
　図１９は本発明の実施の形態５に係るロボット制御装置１Ｅの構成の一例を示す図である。図１９に示すロボット制御装置１Ｅは、実施の形態２に係るロボット制御装置１Ｂの変形例となる。図１９において、図１１と同様の構成は同符号を付し説明を省略する。
　図１１では、軌道修正部１０７が、人位置情報取得部１０３から出力される人位置情報１０３０を用いて第２軌道を生成していた。一方、図１９に示したロボット制御装置１Ｅのように、衝突可能性推定部１０５Ｅが、第２軌道を生成するための基準位置１２００を出力し、軌道修正部１０７が人位置情報１０３０の代わりに基準位置１２００を用いて第２軌道を生成する構成としてもよい。軌道修正部１０７の構成は、第２軌道を生成する際に、人位置情報１０３０の代わりに基準位置１２００を用いる他は、図１１に示されるものと同一である。
　図２０は、衝突可能性推定部１０５Ｅの構成例を示すブロック図である。図２０に示す衝突可能性推定部１０５Ｅは、衝突可能性演算部１０５５Ｅが、人位置情報１０３０に含まれる人位置、及び人位置の移動先１０５２１の中から、基準位置１２００を選択して出力する点が、図１２に示した衝突可能性推定部１０５Ａと異なる。
　衝突可能性演算部１０５５Ｅは、図１３又は図１４に示す人位置Ｈ１、Ｈ２、Ｈ３の中から、接近距離１０５２０又は相対速度１０５４０を参照して基準位置１２００を決定する。例えば、衝突可能性演算部１０５５Ｅは、人位置Ｈ１、Ｈ２、Ｈ３のそれぞれに対応する接近距離又は相対速度を参照して、最も衝突の可能性が高い人位置を選択して基準位置１２００とする。したがって、基準位置１２００は、現在の作業者５の位置、又は推定される作業者５の移動先を表す。作業者５の移動先は、将来における作業者５の予測位置と言い換えることもできる。
　本実施の形態のロボット制御装置１Ｅによれば、作業者５への衝突の可能性がより低い、ロボット手先３ｂの移動軌道を生成することが可能となる。
１　ロボット制御装置
１０　制御信号
２　位置検出装置
２０　検出情報
３　ロボット
４　作業台
５　作業者（作業物体）
６１、６２　作業領域
７１、７２　作業対象物
１０１　手先情報取得部
１０１０　手先情報
１０２　目標点情報取得部
１０２０　目標点情報
１０３　人位置情報取得部
１０３０　人位置情報（物体位置情報）
１０４　第１軌道演算部
１０４０　第１軌道
１０５　衝突可能性推定部
１０６　修正要否判断部
１０７　軌道修正部
１０８　ロボット動作制御部
１０９　動作環境取得部
１０５６　人移動速度演算部（移動速度演算部）

Claims (11)

  1. 　ロボットにおいて監視対象として設定された監視対象部が指令軌道に沿って動作するように前記ロボットを制御するロボット制御装置であって、
    　前記監視対象部の現在位置および前記監視対象部の目標位置に基づき、前記現在位置から前記目標位置へ向かう第１軌道を演算する第１軌道演算部と、
    　作業物体の位置を含む物体位置情報、および前記第１軌道上の少なくとも１つの点における前記監視対象部の予測軌道情報に基づき、前記監視対象部の前記作業物体に対する衝突可能性を推定する衝突可能性推定部と、
    　前記推定された衝突可能性に基づき、前記第１軌道の軌道修正が必要か否かを判断する修正要否判断部と、
    　前記修正要否判断部の判断結果に基づき、前記指令軌道を設定する軌道修正部と
    　を備え、
    　前記軌道修正部は、
    　前記修正要否判断部にて前記軌道修正が必要と判断された場合には、前記物体位置情報および前記第１軌道に基づき、前記第１軌道の衝突可能性よりも小さい衝突可能性を有する第２軌道を前記指令軌道として設定し、
    　前記修正要否判断部にて前記軌道修正が必要でないと判断された場合には、前記第１軌道を前記指令軌道として設定する
    　ロボット制御装置。
  2. 　前記軌道修正部は、前記作業物体の位置から見たときに、前記第２軌道の全ての点において前記第２軌道が前記第１軌道よりも外側を通過するように、前記第２軌道を設定する
    　請求項１に記載のロボット制御装置。
  3. 　前記軌道修正部は、前記作業物体の位置から見たときに、前記第２軌道が外側に凸の曲線となるように、前記第２軌道を設定する
    　請求項２に記載のロボット制御装置。
  4. 　前記軌道修正部は、前記監視対象部が前記現在位置から前記目標位置へと向かうにつれて、前記監視対象部から前記作業物体までの距離が単調に変化するように、前記第２軌道を設定する
    　請求項１から請求項３のいずれか一項に記載のロボット制御装置。
  5. 　前記衝突可能性推定部は、前記作業物体の移動速度を演算する移動速度演算部を備え、前記作業物体の前記移動速度に基づいて前記作業物体に対する衝突可能性を推定する
    　請求項１から請求項４のいずれか一項に記載のロボット制御装置。
  6. 　前記衝突可能性推定部は、前記作業物体の前記移動速度に基づいて前記作業物体の移動先を推定し、推定された前記作業物体の前記移動先に基づいて衝突可能性を推定する
    　請求項５に記載のロボット制御装置。
  7. 　前記軌道修正部は、前記作業物体の前記移動先に基づいて前記第２軌道を設定する
    　請求項６に記載のロボット制御装置。
  8. 　前記監視対象部の動作範囲に配置された部材の位置情報および前記予測軌道情報を用いて、前記監視対象部が前記部材に衝突するか否かを判断する干渉判定部と
    　を備え、
    　前記軌道修正部は、前記干渉判定部の判定結果に応じて、前記指令軌道を修正する
    　請求項１から請求項７のいずれか一項に記載のロボット制御装置。
  9. 　前記指令軌道に基づき前記監視対象部の移動を制御するロボット動作制御部をさらに備え、
    　前記ロボット動作制御部は、
    　前記監視対象部の前記現在位置および前記物体位置情報に基づき、予め設定された制御周期ごとに衝突可能性を算出すると共に、前記算出された衝突可能性が予め設定された閾値を越えた場合には、再計画指令を生成し、
    　前記軌道修正部は、前記再計画指令が入力された場合には、前記指令軌道を修正する
    　請求項１から請求項８のいずれか一項に記載のロボット制御装置。
  10. 　前記指令軌道に基づき前記監視対象部の移動を制御するロボット動作制御部をさらに備え、
    　前記監視対象部の前記現在位置は、前記ロボット動作制御部から出力された制御信号から導出されたものである
    　請求項１から請求項８のいずれか一項に記載のロボット制御装置。
  11. 　請求項１から請求項１０のいずれか一項に記載のロボット制御装置と、
    　前記ロボット制御装置により制御されて動作する前記ロボットと
    　を備えたロボットシステム。
PCT/JP2018/028971 2017-08-18 2018-08-02 ロボット制御装置およびこれを用いたロボットシステム WO2019035362A1 (ja)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2019508266A JPWO2019035362A1 (ja) 2017-08-18 2018-08-02 ロボット制御装置およびこれを用いたロボットシステム

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2017-158127 2017-08-18
JP2017158127 2017-08-18

Publications (1)

Publication Number Publication Date
WO2019035362A1 true WO2019035362A1 (ja) 2019-02-21

Family

ID=65362422

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2018/028971 WO2019035362A1 (ja) 2017-08-18 2018-08-02 ロボット制御装置およびこれを用いたロボットシステム

Country Status (2)

Country Link
JP (2) JPWO2019035362A1 (ja)
WO (1) WO2019035362A1 (ja)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114661038A (zh) * 2020-12-22 2022-06-24 北京配天技术有限公司 一种机器人回程序原点控制方法、装置及相关组件
JP7329902B2 (ja) 2017-08-25 2023-08-21 オムロン株式会社 ロボット制御装置、ロボットシステム、ロボット制御方法、および、ロボット制御プログラム

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3689555A1 (de) * 2019-02-04 2020-08-05 Siemens Aktiengesellschaft Kraftbegrenztes verfahren mindestens eines elements einer produktionsmaschine im manuellen betrieb
IT202100010472A1 (it) * 2021-04-26 2022-10-26 Milano Politecnico Metodo e relativo sistema di controllo in sicurezza di un robot
WO2023007574A1 (ja) 2021-07-27 2023-02-02 三菱電機株式会社 ロボット制御装置およびロボット制御方法
WO2023026589A1 (ja) * 2021-08-27 2023-03-02 オムロン株式会社 制御装置、制御方法および制御プログラム

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH09212229A (ja) * 1996-01-30 1997-08-15 Komatsu Ltd ロボットの教示装置
JP2010055250A (ja) * 2008-08-27 2010-03-11 Toyota Motor Corp 移動体の位置情報取得システム及び取得方法
JP2010155328A (ja) * 2009-01-01 2010-07-15 Sony Corp 軌道計画装置及び軌道計画方法、並びにコンピューター・プログラム
JP2011125975A (ja) * 2009-12-18 2011-06-30 Denso Wave Inc ロボットの干渉回避装置
WO2016104265A1 (ja) * 2014-12-25 2016-06-30 株式会社エクォス・リサーチ 移動体
JP2016196069A (ja) * 2015-04-03 2016-11-24 三菱電機株式会社 産業用ロボットの回避軌道生成装置および回避軌道生成方法
JP2016209991A (ja) * 2015-05-01 2016-12-15 キヤノン株式会社 ロボットの制御装置、制御方法およびシステム
US20170225333A1 (en) * 2014-08-25 2017-08-10 Google Inc. Natural Pitch and Roll
JP2018012159A (ja) * 2016-07-21 2018-01-25 富士電機株式会社 ロボットシステム、ロボットシステムの制御方法、プログラム

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2004243461A (ja) * 2003-02-13 2004-09-02 Yaskawa Electric Corp ロボットの教示システム
JP4967458B2 (ja) * 2006-05-30 2012-07-04 トヨタ自動車株式会社 経路作成装置及び経路作成方法
JP4143103B2 (ja) * 2006-12-20 2008-09-03 本田技研工業株式会社 移動装置、ならびにその制御システム、制御プログラムおよび監督システム
CN101959653B (zh) * 2008-07-09 2012-10-24 松下电器产业株式会社 路径危险性评价装置
JP5235649B2 (ja) * 2008-12-25 2013-07-10 株式会社東芝 移動体軌道生成方法および移動体軌道生成装置
CH709347A2 (de) * 2014-03-10 2015-09-15 Tecan Trading Ag Verfahren zur Wegfindung in einem automatisierten Handhabungssystem sowie Handhabungssystem mit entsprechendem Kontrollmodul zur Wegfindung.

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH09212229A (ja) * 1996-01-30 1997-08-15 Komatsu Ltd ロボットの教示装置
JP2010055250A (ja) * 2008-08-27 2010-03-11 Toyota Motor Corp 移動体の位置情報取得システム及び取得方法
JP2010155328A (ja) * 2009-01-01 2010-07-15 Sony Corp 軌道計画装置及び軌道計画方法、並びにコンピューター・プログラム
JP2011125975A (ja) * 2009-12-18 2011-06-30 Denso Wave Inc ロボットの干渉回避装置
US20170225333A1 (en) * 2014-08-25 2017-08-10 Google Inc. Natural Pitch and Roll
WO2016104265A1 (ja) * 2014-12-25 2016-06-30 株式会社エクォス・リサーチ 移動体
JP2016196069A (ja) * 2015-04-03 2016-11-24 三菱電機株式会社 産業用ロボットの回避軌道生成装置および回避軌道生成方法
JP2016209991A (ja) * 2015-05-01 2016-12-15 キヤノン株式会社 ロボットの制御装置、制御方法およびシステム
JP2018012159A (ja) * 2016-07-21 2018-01-25 富士電機株式会社 ロボットシステム、ロボットシステムの制御方法、プログラム

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP7329902B2 (ja) 2017-08-25 2023-08-21 オムロン株式会社 ロボット制御装置、ロボットシステム、ロボット制御方法、および、ロボット制御プログラム
CN114661038A (zh) * 2020-12-22 2022-06-24 北京配天技术有限公司 一种机器人回程序原点控制方法、装置及相关组件

Also Published As

Publication number Publication date
JP6729773B2 (ja) 2020-07-22
JP2019206080A (ja) 2019-12-05
JPWO2019035362A1 (ja) 2019-11-07

Similar Documents

Publication Publication Date Title
JP6729773B2 (ja) ロボット制御装置およびこれを用いたロボットシステム
US10618164B2 (en) Robot system having learning control function and learning control method
CN108568815B (zh) 机器人系统和机器人控制方法
CN109153134B (zh) 机器人控制装置
US20170241790A1 (en) Path plan generating apparatus for mobile body
JP5080333B2 (ja) 自律移動体のための物体認識装置
US10162364B2 (en) Robot and control method thereof
US20100087948A1 (en) Collision preventing device incorporated in numerical control apparatus
US11040449B2 (en) Robot control system and method of controlling a robot
KR20170088228A (ko) 다중로봇의 자기위치인식에 기반한 지도작성 시스템 및 그 방법
JP2006146491A (ja) 移動ロボットおよびその追従方法
JP2003236787A (ja) 駆動制御方法および駆動制御装置
US20200238530A1 (en) Robot system, method for controlling robot, robot controller, and non-transitory computer-readable storage medium
US11579615B2 (en) Remote control apparatus, system, method, and program
JP2009193097A (ja) 移動ロボットの制御装置および移動ロボットシステム
JPWO2020161910A1 (ja) 制御装置、制御方法、プログラム
JP2017144488A (ja) 作業領域推定装置、制御装置、制御システム、作業領域推定方法およびプログラム
US10814482B2 (en) Robot controller
JP2020093373A (ja) ロボット干渉判定装置、ロボット干渉判定方法、ロボット制御装置、およびロボット制御システム
CN116149311A (zh) 动态运动规划系统
JPWO2019163116A1 (ja) 補正関数生成装置、ロボット制御システム及びロボットシステム
CN111195908B (zh) 轨道生成装置
CN111699078A (zh) 机器人的运行
WO2023095926A1 (ja) ロボット制御装置、ロボット制御システム、及びロボット制御方法
US10635080B2 (en) Work region estimation device, control device, control system, work region estimation method, and non-transitory computer-readable recording medium

Legal Events

Date Code Title Description
ENP Entry into the national phase

Ref document number: 2019508266

Country of ref document: JP

Kind code of ref document: A

121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 18846073

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 18846073

Country of ref document: EP

Kind code of ref document: A1