Robot system

Abstract

To distinguish between a robot and an interfering object and to detect an approach of the interfering object.SOLUTION: A controller 4 of a robot system includes: a link parameter storage part 62 for storing a value of a link parameter of a robot; a point cloud data acquisition part 63 for acquiring point cloud data from a three-dimensional sensor; a joint angle acquisition part 64 for acquiring a current joint angle of each joint of a robot arm; a robot model generator 65 for generating a robot model that shows a posture that is similar to a current posture of the robot on the basis of the link parameter and the joint angle; an interfering object data setting part 66 for removing at least points indicating the robot from the point cloud data by using the robot model, and setting remaining point cloud data as interfering object data; and an approach determination part 67 for calculating a distance between the interfering object and the robot by using the interfering object data and the robot model, and determining whether or not the interfering object is approaching the robot on the basis of the distance.SELECTED DRAWING: Figure 2

Description

[Technical Field]

[0001] The present invention relates to a robot system that detects the approach of an interfering object such as a person. [Background technology]

[0002] In recent years, collaborative robots that work in collaboration with humans without safety fences have been attracting attention. Safety measures for collaborative robots include a method of having the robot avoid danger when it detects contact with an interfering object such as a human, and a method of having the robot avoid danger when it detects the approach of an interfering object. The latter method commands the robot to perform a danger avoidance action before contact with the interfering object, and is therefore considered to be safer than the former method. As an example of the latter method, Patent Document 1 describes a system that uses a proximity sensor that detects the proximity of an object by a change in electrostatic capacitance that occurs between the robot and the object. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Patent No. 7109562 Summary of the Invention [Problem to be solved by the invention]

[0004] However, a problem with capacitance-type proximity sensors is that they may detect the robot's external wiring, installation base, etc. as interfering objects. The robot control device that controls the robot's operation stores the positions of the robot's accessories and each part in advance and controls the robot arm to prevent contact with these, so there is no need to detect them as interfering objects. If the robot's accessories or each part were detected as interfering objects, the robot may not be able to perform its intended operation or its range of operation may be limited, which could significantly reduce production efficiency.

[0005] The present invention has been made in view of the above-mentioned circumstances, and a main object of the present invention is to provide a robot system that can distinguish between a robot and an interfering object and detect the approach of the interfering object. [Means for solving the problem]

[0006] In order to achieve the above-mentioned object, the present invention provides a robot system comprising: one or more robots each having a robot arm; one or more three-dimensional sensors that capture an image of a space including an operational range of the robot; and one or more control devices that control the operation of the robot and the three-dimensional sensors, wherein the control device comprises: a link parameter storage unit that stores values of link parameters of the robot; a point cloud data acquisition unit that acquires point cloud data, which is a collection of points indicating the surface positions of an object placed in the space, from the three-dimensional sensor; a joint angle acquisition unit that acquires current joint angles of each joint of the robot arm; a robot model generation unit that generates a robot model that shows a posture similar to the current posture of the robot based on the link parameters and the joint angles; an interference object data setting unit that uses the robot model to remove at least points that represent the robot from the point cloud data, and sets the remaining point cloud data as interference object data, which is a collection of points that indicate the position of an interference object; and an approach determination unit that calculates a distance between the interference object and the robot using the interference object data and the robot model, and determines whether the interference object and the robot are approaching each other based on the distance between the interference object and the robot. The robot system is characterized by comprising:

[0007] The robot model generation unit may calculate points indicating the positions of each part of the robot as reference points based on the link parameters and the joint angles, and generate the robot model based on the reference points.

[0008] In addition, the robot model generation unit may set a spatial figure defined in accordance with the reference points as a robot object, and the interference data setting unit may remove from the point cloud data at least points included inside the robot object, and generate the interference data.

[0009] The approach determination unit may also divide the interfering object data into groups and calculate the distance between the interfering object and the robot for each group.

[0010] In addition, the approach determination unit may calculate the distance between a point included in the interference object data or a spatial figure defined in accordance with the interference object data, and a point included in the robot model or a spatial figure defined in accordance with the robot model, as the distance between the interference object and the robot. [Effects of the Invention]

[0011] The present invention provides a robot system that can distinguish between a robot and an interfering object and detect the approach of an interfering object. [Brief explanation of the drawings]

[0012] [Figure 1] FIG. 1 is a diagram illustrating an outline of a robot system according to an embodiment of the present invention. [Figure 2] A block diagram showing the configuration of the control device of FIG. 1. [Figure 3] 1 is a flowchart showing an example of a process flow realized by the control device of FIG. [Figure 4] A diagram illustrating an example of a robot model [Figure 5] FIG. 10 is a diagram for explaining the process performed by the interference data setting unit; [Figure 6] FIG. 10 is a diagram for explaining a process performed by the approach determination unit; DETAILED DESCRIPTION OF THE INVENTION

[0013] An embodiment of the present invention will be described in detail below with reference to the drawings. FIG. 1 is a diagram illustrating an outline of a robot system according to an embodiment of the present invention. As shown in FIG. 1, the robot system 1 includes one or more robots 2 each having a robot arm 21, one or more three-dimensional sensors 3 that capture images of a space including the operable range of the robot 2, and one or more control devices 4 that control the operation of the robot 2 and the three-dimensional sensor 3. A workpiece W or a table T may be placed in the space captured by the three-dimensional sensor 3. The robot 2 is a collaborative robot, and a person M may be present in the space captured by the three-dimensional sensor 3.

[0014] The robot 2 has an installation base 22 that supports a robot arm 21, and an end effector 23 is attached to the tip of the robot arm 21. The robot arm 21 is composed of a link mechanism having a plurality of links, and has joints that connect the links. Each joint is provided with a drive motor (not shown). An example of the robot 2 is a vertical multi-joint robot with six joints. However, the present invention is also applicable to robots with five or fewer or seven or more joints, horizontal multi-joint robots, etc. Furthermore, the robot 2 is not limited to a single-arm robot, and may be a dual-arm robot.

[0015] The three-dimensional sensor 3 is a sensor capable of measuring the three-dimensional position of an object, and generates point cloud data, which is a collection of points indicating the positions of detected points on the surface of an object placed in the space of the object to be photographed. The three-dimensional sensor 3 may be, for example, an active stereo camera, a passive stereo camera, a three-dimensional LiDAR (Light Detection And Ranging) sensor, or a ToF (Time of Flight) sensor. Examples of point cloud data include, but are not limited to, a three-dimensional camera (Flight) and the like. The point cloud data generated by the three-dimensional sensor 3 is Cartesian coordinate point cloud data expressed in three-dimensional Cartesian coordinates (x, y, z). The range that each component of the three-dimensional coordinates (x, y, z) can take depends on the angle of view and detection distance range of the three-dimensional sensor 3. The file format of the point cloud data is, for example, PCD (Point Cloud Data), which is developed by the open source library PCL (Point Cloud Library). Examples of such files include, but are not limited to, data files.

[0016] The 3D sensor 3 may be installed on a ceiling or the like to capture images from above the robot 2, or may be installed on a wall or a tripod to capture images from the horizontal direction of the robot 2. The 3D sensor 3 may also be installed on the robot arm 21, end effector 23, or the like to capture images so that only a portion of the robot 2 is included in the imaging range. Furthermore, if there is an area that cannot be imaged by a single 3D sensor 3 (such as an area in the shadow of the robot 2), multiple 3D sensors 3 may be installed and the point cloud data acquired from the multiple 3D sensors 3 may be combined.

[0017] The control device 4 includes a robot control device 4a that controls the operation of the robot 2, and a three-dimensional sensor control device 4b that controls the operation of the three-dimensional sensor 3. The robot control device 4a may be built into the installation base 22 of the robot 2, or may be installed external to the robot 2. In the latter case, the robot 2 and the robot control device 4a are communicatively connected via a communication cable or wirelessly. Similarly, the three-dimensional sensor control device 4b may be integrated with the three-dimensional sensor 3, or may be separate. In the latter case, the three-dimensional sensor 3 and the three-dimensional sensor control device 4b are communicatively connected via a communication cable or wirelessly. Furthermore, the robot control device 4a and the three-dimensional sensor control device 4b are communicatively connected via a communication cable or wirelessly.

[0018] The CPU (Central Processing Unit) 41a, memory 42a, auxiliary storage device 43a, and input / output interface 44a of the robot control device 4a are connected via a bus 45a. The CPU 41a loads a control program stored in advance in the auxiliary storage device 43a or the like into the memory 42a and sequentially executes a plurality of commands. The auxiliary storage device 43a is a hard disk drive, solid state drive, or the like, and stores data used in the processing described below. The input / output interface 44a inputs and outputs signals from the robot 2, the three-dimensional sensor control device 4b, and other devices.

[0019] The CPU (Central Processing Unit) of the three-dimensional sensor control device 4b The CPU 41b, memory 42b, auxiliary storage device 43b, and input / output interface 44b are connected via a bus 45b. The CPU 41b reads a control program stored in advance in the auxiliary storage device 43b or the like into the memory 42b and sequentially executes a plurality of commands. The auxiliary storage device 43b is a hard disk drive, a solid state drive, or the like, and stores data used in the processing described below. The input / output interface 44b inputs signals from the 3D sensor 3, the robot control device 4a, other devices, and outputs signals to them.

[0020] 1 shows two control devices 4, namely, a robot control device 4a and a three-dimensional sensor control device 4b, but the number of control devices 4 may be one or three or more. In the following, the robot system 1 will be described as having two control devices 4, a robot control device 4a and a three-dimensional sensor control device 4b, and these control devices 4 cooperate with each other to execute processing.

[0021] Fig. 2 is a block diagram showing the configuration of the control device in Fig. 1. The robot control device 4a and the three-dimensional sensor control device 4b have the configuration shown in Fig. 2 through cooperation between software such as a control program and hardware resources such as CPUs 41a and 41b.

[0022] The robot control device 4a includes a robot operation control unit 51 and a joint angle providing unit 52. The robot operation control unit 51 controls the rotation and stopping of the drive motor of the robot arm 21 based on a control program, commands from an external device, etc. The joint angle providing unit 52 provides the three-dimensional sensor control device 4b with the current joint angles of each joint of the robot arm 21 in the current state of the robot 2.

[0023] The three-dimensional sensor control device 4b includes a three-dimensional sensor operation control unit 61, a link parameter storage unit 62, a point cloud data acquisition unit 63, a joint angle acquisition unit 64, a robot model generation unit 65, an interference object data setting unit 66, an approach determination unit 67, and an operation command notification unit 68. The three-dimensional sensor operation control unit 61 controls the operation of the three-dimensional sensor 3 based on a control program, commands from an external device, etc. The link parameter storage unit 62 stores values of link parameters of the robot 2.

[0024] Link parameters are parameters that determine the geometric positional relationship between the links and joints of the robot arm 21. For example, link parameters that use the Denavit-Hartenberg notation (DH method) are known. Link parameters using the DH method are also called DH parameters. An overview of link parameters using the DH method is as follows: 1. Links and joints are ordered in order of proximity to the base of the robot 2. 2. A coordinate system (x[i], y[i], z[i]) is set for link i, with joint axis i as the z[i] axis. 3. The x[i] axis is the common perpendicular line between the z[i] axis and the z[i+1] axis, and the direction toward the z[i+1] axis is positive. 4. The y[i] axis is set to form a right-handed coordinate system. 5. The link parameters are: (1) inter-link angle θ[i] = angle from x[i-1] axis to x[i] axis relative to z[i] axis; (2) inter-link distance d[i] = distance from x[i-1] axis to x[i] axis along z[i] axis; (3) link torsion angle α[i] = angle from z[i-1] axis to z[i] axis relative to x[i-1] axis; and (4) link length a[i] = distance from z[i-1] axis to z[i] axis along x[i-1] axis.

[0025] The inter-link angle θ[i] is the current joint angle of each joint of the robot arm 21, provided by the joint angle providing unit 52 of the robot control device 4a. The link parameter storage unit 62 stores in advance three design values, excluding the inter-link angle θ[i], namely, the inter-link distance d[i], the link torsion angle α[i], and the link length a[i].

[0026] The point cloud data acquisition unit 63 acquires, from the 3D sensor 3, point cloud data, which is a collection of points indicating the surface position of an object placed in space and imaged by the 3D sensor 3. The joint angle acquisition unit 64 acquires the current joint angle of each joint of the robot arm 21 from the robot operation control unit 51. The robot model generation unit 65 generates a robot model showing a posture similar to the current posture of the robot 2 based on the link parameters and joint angles. The interference data setting unit 66 uses the robot model to remove at least points indicating the robot from the point cloud data, and sets the remaining point cloud data as interference data, which is a collection of points indicating the position of the interference object. The approach determination unit 67 calculates the distance between the interference object and the robot using the interference object data and the robot model, and determines whether the interference object and the robot are approaching each other based on the distance between the interference object and the robot. The action command notification unit 68 notifies the robot control device 4a of an action command based on the determination result by the approach determination unit 67. Details of these components will be described later with reference to Figures 3 to 6.

[0027] Fig. 3 is a flowchart showing an example of the flow of processing realized by the control device of Fig. 1. As shown in Fig. 3, the three-dimensional sensor control device 4b checks whether or not to execute a pre-contact detection judgment loop (step S1). In the pre-contact detection judgment loop, a judgment process for detecting the approach of an interfering object before the robot 2 comes into contact with the interfering object, i.e., the processes from step S2 to step S10, are repeated.

[0028] On the other hand, if the three-dimensional sensor control device 4b has not received a command to stop the pre-contact detection judgment loop from the robot control device 4a, it executes the pre-contact detection judgment loop (Yes in step S1). On the other hand, if the three-dimensional sensor control device 4b receives a command to stop the pre-contact detection judgment loop from the three-dimensional sensor control device 4b, it ends the execution of the pre-contact detection judgment loop (No in step S1). In the case of an operation mode in which a person directly touches the robot 2, such as direct teaching, the robot control device 4a sends a command to stop the pre-contact detection judgment loop to the three-dimensional sensor control device 4b.

[0029] When the pre-contact detection judgment loop is entered, the point cloud data acquisition unit 63 of the three-dimensional sensor control device 4b acquires point cloud data from the three-dimensional sensor 3 (step S2). As described above, the point cloud data is expressed in three-dimensional Cartesian coordinates (x, y, z). The point cloud data includes points that indicate the positions of the surfaces of objects such as the robot 2, workpiece W, table T, and person M.

[0030] Next, the joint angle acquisition unit 64 of the three-dimensional sensor control device 4b acquires the current joint angles of each joint of the robot arm 21 from the joint angle provision unit 52 of the robot control device 4a (step S3). For example, the joint angle provision unit 52 of the robot control device 4a transmits the joint angles detected by the encoders mounted on the drive motors of each joint to the three-dimensional sensor control device 4b.

[0031] Next, the robot model generation unit 65 of the three-dimensional sensor control device 4b generates a robot model based on the link parameters stored in the link parameter storage unit 62 and the joint angles acquired by the joint angle acquisition unit 64 (step S4). The values of the link parameters stored in the link parameter storage unit 62 are calculated based on the inter-link distance d i , link torsion angle α i and link length a i The joint angles acquired by the joint angle acquisition unit 64 are the inter-link angle θ iThe robot model generation unit 65 uses the values of these four link parameters to perform forward kinematic calculations and analytically calculates points indicating the position of each part of the robot 2 (each joint, end effector 23, etc.), and sets these as reference points. The robot model generation unit 65 then generates a robot model that indicates the current state of the robot 2 based on the reference points. This allows the robot model generation unit 65 to quickly generate a robot model with sufficient accuracy to determine whether the robot 2 is approaching an interfering object. Consequently, the robot system 1 can detect the approach of an interfering object to the robot 2 in time to perform danger avoidance operation.

[0032] Fig. 4 is a diagram illustrating an example of a robot model. The robot model 70 is a model that approximates the robot 2 and is generated in a three-dimensional Cartesian coordinate space. However, Fig. 4 simply illustrates the model as a two-dimensional Cartesian coordinate plane viewed from a specific direction in the three-dimensional Cartesian coordinate space. The same applies to Figs. 5 and 6, which will be described later.

[0033] Points 71a to 71e, shown as rectangles, are reference points that indicate the positions of each joint, the end effector 23, and the like, calculated by the robot model generation unit 65. In the example of FIG. 4, there are five reference points, but the number of reference points is not particularly limited. Points 72a to 72j, shown as triangles, are linearly interpolated points generated according to line segments 73a to 73d that connect two adjacent reference points 71a to 71e. The number of linearly interpolated points may vary for each of the line segments 73a to 73d. In the example of FIG. 4, the line segment 73a has zero linearly interpolated points, the line segment 73b has five linearly interpolated points, the line segment 73c has four linearly interpolated points, and the line segment 73d has one linearly interpolated point. The number of linearly interpolated points is not limited to the example of FIG. 4 and can be changed as appropriate depending on the size of the robot 2, etc. The robot model generation unit 65 may generate the robot model 70 using only the reference points without generating linearly interpolated points.

[0034] The robot model generation unit 65 sets a spatial figure defined according to the reference points as a robot object. The robot object is one or more virtual figures that approximate the robot 2. The robot objects shown in FIG. 4 are spheres 74a to 74o with a predetermined radius and centers on reference points 71a to 71e and linearly interpolated points 72a to 72j generated according to the reference points 71a to 71e. The radii of the spheres 74a to 74o may differ depending on the sizes of each part, such as the robot arm 21 and the end effector 23.

[0035] The robot object is not limited to the spheres 74a to 74o shown in Fig. 4. The robot object may be any shape that can approximate the robot 2 so as to include accessories such as external wiring of the robot 2 and parts other than the robot arm 21, such as the installation base 22. For example, the robot object may be a plurality of cubes centered on the reference points 71a to 71e and the linear interpolation points 72a to 72j. Furthermore, for example, the robot object may be a plurality of cylinders or rectangular parallelepipeds centered on line segments 73a to 73d generated according to the reference points 71a to 71e.

[0036] Returning to the explanation of FIG. 3, the interfering object data setting unit 66 of the three-dimensional sensor control device 4b removes at least the points representing the robot 2 from the point cloud data acquired by the point cloud data acquisition unit 63. Here, the points representing the robot 2 include points representing the robot arm 21, the installation base 22, the end effector 23, accessories of the robot 2, etc. The interfering object data setting unit 66 then sets the remaining point cloud data that has not been removed as interfering object data (step S5). This allows the three-dimensional sensor control device 4b to separate the points included in the point cloud data into points representing the robot 2 and points representing interfering objects. In other words, the three-dimensional sensor control device 4b can distinguish between the robot 2 and interfering objects.

[0037] FIG. 5 is a diagram illustrating the processing by the interfering object data setting unit. In FIG. 5, the robot model 70 and point cloud data 75 are shown superimposed. The interfering object data setting unit 66 removes at least the points included inside the robot object (= spheres 74a to 74o) from the point cloud data 75, and sets the remaining point cloud data 75 that has not been removed as the interfering object data 76. This allows the interfering object data setting unit 66 to quickly identify the robot 2 and the interfering object with sufficient accuracy to determine whether the robot 2 is approaching the interfering object. Consequently, the robot system 1 can detect the approach of the interfering object to the robot 2 in time to perform a risk avoidance operation.

[0038] Here, the interference data setting unit 66 may remove points representing objects other than the robot 2 that do not need to be recognized as interferences, rather than only removing points representing the robot 2, from the point cloud data 75. Examples of objects other than the robot 2 that do not need to be recognized as interferences include the workpiece W and the table T. For example, if the end effector 23 is a tool that grips the workpiece W, contact between the end effector 23 and the workpiece W should be permitted, and the workpiece W does not need to be recognized as an interference. Furthermore, for example, if the table T is a stationary object whose position is known in advance, the robot control device 4a controls the operation of the robot 2 by restricting the operable range of the robot arm 21 so that it does not come into contact with the table T. In this case, the table T does not need to be recognized as an interference. For example, the interference data setting unit 66 may store an environmental map including the position information of the workpiece W and the table T in advance, and remove points representing the workpiece W and the table T from the point cloud data 75, similar to the points representing the robot 2.

[0039] Returning to the explanation of Fig. 3, the approach determination unit 67 of the three-dimensional sensor control device 4b calculates the distance between the interfering object and the robot 2 using the interfering object data 76 and the robot model 70, and determines whether the interfering object and the robot 2 are approaching each other based on the distance between the interfering object and the robot 2 (step S6).

[0040] FIG. 6 is a diagram illustrating the processing by the approach determination unit. In FIG. 6, the robot model 70 and the interfering object data 76 are shown superimposed. The interfering object is not limited to a single object; it is conceivable that multiple interfering objects approach the robot 2 from various directions. Therefore, the approach determination unit 67 divides the interfering object data 76 into groups and calculates the distance between the robot 2 and the interfering object for each group. This allows the approach determination unit 67 to quickly and accurately determine whether the interfering object is approaching the robot 2. Consequently, the robot system 1 can detect the approach of an interfering object to the robot 2 in time to perform a risk avoidance operation.

[0041] Since the interfering object data 76 is point cloud data, grouping the interfering object data 76 means grouping the point cloud data. The approach determination unit 67 performs grouping by applying, for example, a clustering method (= a method using the "Euclidean Cluster Extraction" class) of PCL (Point Cloud Library), an open source software library, to the interfering object data 76. This clustering method allows unsorted point cloud data to be divided into smaller parts.

[0042] The approach determination unit 67 also applies a PCL shape approximation method (= a method using the "MomentOfInertiaEstimation" class) to each of the grouped interfering object data 76 to generate a bounding box (boundary box) as a spatial figure that encompasses the interfering object. The bounding box is a rectangular parallelepiped that includes the given point cloud data. Note that the spatial figure that approximates the interfering object is not limited to a bounding box (rectangular parallelepiped), and may be another spatial figure such as a sphere or a cylinder.

[0043] The approach determination unit 67 then calculates the Euclidean distance between the bounding box and the reference point or linearly interpolated point. Since there are multiple reference points or linearly interpolated points, the approach determination unit 67 calculates the Euclidean distance between the bounding box and each point, and determines the minimum value (shortest distance value) as the distance between the interfering object and the robot 2. In the example of FIG. 6, bounding boxes 77a and 77b are illustrated, each including a portion of the interfering object data 76. For example, the bounding box 77a is a person's left arm, and the bounding box 77b is the person's right arm. Distances 78a and 78b are the distances between the bounding boxes 77a and 77b and the reference point 71e, respectively, and are the shortest distance values among the multiple reference points or linearly interpolated points. If the distance between at least one interfering object in the grouped interfering object data 76 and the robot 2 is equal to or less than a threshold, the approach determination unit 67 determines that the interfering object and the robot 2 are approaching each other. In the example of FIG. 4, the approach determination unit 67 determines that the interfering object and the robot 2 are approaching each other when at least one of the distances 78a and 78b is equal to or less than a threshold value.

[0044] In the above description, the approach determination unit 67 determines whether the interfering object and the robot 2 are approaching each other based on the distance between the interfering object and the robot 2. However, the approach determination unit 67 may also determine this based on the relative speed between the interfering object and the robot 2. For example, the approach determination unit 67 can calculate the relative speed between the interfering object and the robot 2 by calculating the displacement of the relative position between the interfering object and the robot 2 from the time point cloud data was last acquired to the time point cloud data is last acquired, and dividing this by the time from the time point cloud data was last acquired to the time point cloud data is last acquired. Then, for example, the approach determination unit 67 uses a value proportional to the relative speed between the interfering object and the robot 2 as the threshold value for the distance between the interfering object and the robot 2. This makes it possible to issue a danger avoidance command at a timing when danger can be avoided, even if the speed at which the interfering object and the robot 2 are approaching each other is fast.

[0045] In the above description, the approach determination unit 67 calculates the distance between the spatial figure approximating the interfering object and a point (reference point or linear interpolation point) indicating the position of the robot 2 as the distance between the interfering object and the robot 2. However, other calculation methods may be used. For example, the approach determination unit 67 may calculate the distance between the spatial figure approximating the interfering object and a spatial figure (robot object) approximating the robot 2. For example, the approach determination unit 67 may calculate the distance between a point included in the interfering object data 76 and a spatial figure approximating the robot 2. For example, the approach determination unit 67 may calculate the distance between a point included in the interfering object data 76 and a point indicating the position of the robot 2. In summary, the approach determination unit 67 calculates the distance between a point included in the interfering object data 76 or a spatial figure defined in accordance with the interfering object data, and a point included in the robot model 70 or a spatial figure defined in accordance with the robot model 70, as the distance between the interfering object and the robot 2.

[0046] Returning to the explanation of Figure 3, if the approach determination unit 67 determines that the interfering object and the robot 2 are approaching each other (Yes in step S6), the operation command notification unit 68 of the three-dimensional sensor control device 4b checks whether the robot 2 is in operation (step S7). If the robot 2 is in operation (Yes in step S7), the operation command notification unit 68 notifies the robot control device 4a of an instruction to stop the robot 2 as a danger avoidance instruction, and the process returns to step S1 (step S8). If the robot 2 is not in operation (No in step S7), the operation command notification unit 68 does nothing and returns to step S1.

[0047] On the other hand, if the approach determination unit 67 determines that the interfering object and the robot 2 are not approaching each other (No in step S6), the operation command notification unit 68 checks whether the robot 2 is in operation (step S9). If the robot 2 is in operation (Yes in step S9), the operation command notification unit 68 does nothing and returns to step S1. If the robot 2 is not in operation (No in step S9), the operation command notification unit 68 notifies the robot control device 4a of an operation restart command for the robot 2 as a restriction relaxation command, and returns to step S1 (step S10).

[0048] The danger avoidance command notified in step S8 is not limited to a stop command, but may be, for example, a deceleration command, a route change command along a contact avoidance route, etc. Similarly, the restriction easing command notified in step S10 is not limited to an operation restart command, but may be, for example, an acceleration command, a route change command along the shortest route, etc.

[0049] As described above, the robot system 1 according to the embodiment of the present invention can distinguish between the robot 2 and an interfering object and detect the approach of the interfering object. The robot system 1 according to the embodiment of the present invention uses the 3D sensor 3, and by appropriately setting the installation location of the 3D sensor 3 and the positional relationship between the robot 2 and the 3D sensor 3, it can detect the approach of an interfering object over a wide area. Furthermore, the robot system 1 according to the embodiment of the present invention can be applied regardless of the shape or size of the robot 2, and therefore there is no need to change the hardware configuration for each model of robot 2, making it easy to introduce the system.

[0050] Furthermore, in the case of a capacitance-type proximity sensor in the prior art, the distance over which a change in capacitance can be detected is short, and there is a risk that it may not be possible to take action to avoid danger in time after detecting the approach of an interfering object.In contrast, the robot system 1 according to the embodiment of the present invention uses a three-dimensional sensor 3 that can detect the approach of an interfering object over a wide range, rather than a capacitance-type proximity sensor, so that the approach of an interfering object can be detected in time to allow for action to avoid danger.

[0051] Furthermore, in the case of a conventional capacitance-type proximity sensor, detection of an interfering object becomes unstable when water droplets form on the detection surface or when humidity changes. On the other hand, the robot system 1 according to the embodiment of the present invention uses a three-dimensional sensor 3 that is not affected by environmental changes such as humidity, so it can stably detect approaching interfering objects regardless of environmental changes.

[0052] While the preferred embodiments of the robot system and the like according to the present invention have been described above with reference to the accompanying drawings, the present invention is not limited to these examples. It is clear that those skilled in the art can conceive of various modifications and alterations within the scope of the technical ideas disclosed herein, and it is understood that these modifications and alterations also fall within the technical scope of the present invention. [Explanation of symbols]

[0053] 1. Robot system 2. Robot 3...3D sensor 4. Control device 4a...Robot control device 4b...3D sensor control device 21...Robot arm 22...Installation base 23...End effector 51: Robot operation control unit 52...Joint angle providing unit 61...3D sensor operation control section 62: Link parameter storage section 63: Point cloud data acquisition unit 64: Joint angle acquisition unit 65: Robot model generation unit 66: Interference data setting section 67……Approach judgment section 68……Operation command notification section 70...Robot model 71a~71e……Reference point 72a~72j...Linear interpolation points 73a~73d...Line segments 74a~74o………Sphere (Robot object) 75...Point cloud data 76...Interference data 77a, 77b...Bounding box 78a, 78b……distance

Claims

1. A robot system comprising one or more robots each having a robot arm, one or more three-dimensional sensors that capture an image of a space including an operable range of the robot, and one or more control devices that control the operation of the robot and the three-dimensional sensor, The control device a link parameter storage unit that stores values of link parameters of the robot; a point cloud data acquisition unit that acquires point cloud data, which is a collection of points indicating the positions of the surfaces of objects placed in the space, from the three-dimensional sensor; a joint angle acquisition unit that acquires a current joint angle of each joint of the robot arm; a robot model generation unit that generates a robot model showing a posture similar to a current posture of the robot based on the link parameters and the joint angles; an interference data setting unit that uses the robot model to remove at least points that indicate the robot from the point cloud data and sets the remaining point cloud data as interference data that is a collection of points that indicate positions of interference objects; a proximity determination unit that calculates a distance between the interfering object and the robot using the interfering object data and the robot model, and determines whether the interfering object and the robot are approaching each other based on the distance between the interfering object and the robot; A robot system comprising:

2. The robot model generation unit calculates points indicating the positions of each part of the robot as reference points based on the link parameters and the joint angles, and generates the robot model based on the reference points.

2. The robot system according to claim 1.

3. the robot model generation unit defines a spatial figure defined according to the reference point as a robot object; The interfering object data setting unit removes at least points included inside the robot object from the point cloud data, and generates the interfering object data.

3. The robot system according to claim 2.

4. The approach determination unit divides the interfering object data into groups and calculates the distance between the interfering object and the robot for each group.

2. The robot system according to claim 1.

5. The approach determination unit calculates, as the distance between the interfering object and the robot, a distance between a point included in the interfering object data or a spatial figure defined in accordance with the interfering object data and a point included in the robot model or a spatial figure defined in accordance with the robot model.

5. The robot system according to claim 1, wherein the first and second axes are parallel to each other.

Images