Path generation device, path generation method, and path generation program

Abstract

The present invention provides a path generation device. The path generation device (3) includes an object setting unit (54), a device setting unit (55), a path generation unit (56), and an interference judgment unit (57). The object setting unit (54) sets an object model (71), the device setting unit (55) sets a device model (72) as a model of a robot arm (12), the path generation unit (56) gradually generates a path for the robot arm (12), and the interference judgment unit (57) judges the interference between the object model (71) and the device model (72) after moving along the path based on the distance between the object model (71) and the device model (72). When the interference judgment unit (57) judges that there is a possibility of interference between the object model (71) and the device model (72), at least one of the object setting unit (54) and the device setting unit (55) increases the density of the point cloud of the point cloud model, and the interference judgment unit (57) uses the point cloud model with the increased point cloud density for interference judgment. The path generation unit (56) generates the next path when the interference determination unit (57) determines that there is no possibility of interference.

Description

Technical Field

[0001] The present invention relates to a path generation device, a path generation method, and a path generation program. Background Art

[0002] Conventionally, techniques for generating paths for motion devices are well known. For example, Patent Document 1 discloses a device that plans a path for a robot while determining interference between the robot and obstacles.

[0003] Patent Document 1: Japanese Patent Application Laid-Open No. 2013-246553 Summary of the Invention

[0004] When generating a path for an action device, as in the device of Patent Document 1, it is necessary to judge the interference between objects such as obstacles and the action device. Interference judgment is performed multiple times in generating the path. For example, when generating a path from a specified starting position to an end position, interference judgment is performed at multiple locations on the path. Furthermore, when the interference judgment determines that an object interferes with the action device, the path is changed and the interference judgment is performed again. Usually, interference judgment is performed by calculating the distance between the object and the action device. As described above, since interference judgment is performed multiple times in path generation, when the calculation of the distance between the object and the action device takes too much time, the time for path generation will be longer. On the other hand, when the calculation of the distance between the object and the action device is simplified in order to shorten the time for path generation, the accuracy of the interference judgment between the object and the action device will decrease.

[0005] In view of the above, an object of the present invention is to shorten the time of interference judgment and improve the accuracy.

[0006] A path generation device according to the present invention generates a path for a motion device. The path generation device includes an object setting unit, a device setting unit, a path generation unit, and an interference determination unit. The object setting unit sets an object model, which is a model of an object contained in a configuration space in which the motion device is configured. The device setting unit sets a device model, which is a model of the motion device in the configuration space. The path generation unit gradually generates a path for the motion device. The interference determination unit determines interference between the object model and the device model after the object model moves along the path based on the distance between the object model and the device model. At least one of the object model and the device model is a point cloud model. When the interference determination unit determines that there is a possibility of interference, at least one of the object setting unit and the device setting unit increases the point cloud density of the point cloud model. The interference determination unit performs the interference determination again using the point cloud model with the increased point cloud density. When the interference determination unit determines that there is no possibility of interference, the path generation unit generates the next path.

[0007] The path generation method of the present invention is a path generation method for generating a path for an action device. The path generation method includes the steps of setting an object model (i.e., an object model) contained in a configuration space in which the action device is configured; setting a device model (i.e., a device model) as a model of the action device in the configuration space; gradually generating a path for the action device; and performing interference determination between the object model and the device model after movement along the path based on the distance between the object model and the device model. At least one of the object model and the device model is a point cloud model formed using a point cloud. When the interference determination determines that there is a possibility of interference, the density of the point cloud of the point cloud model is increased in at least one of the steps of setting the object model and setting the device model. In the interference determination step, the interference determination is performed again using the point cloud model with the increased point cloud density. In the path generation step, when the interference determination determines that there is no possibility of interference, a next path is generated.

[0008] In order to generate the action of the action device, the path generation program of the present invention enables the computer to realize the function of setting a model of an object contained in the configuration space where the action device is configured, namely, an object model, the function of setting a model of the action device in the configuration space, namely, a device model, the function of gradually generating the path of the action device, and the function of performing interference judgment between the object model and the device model after moving along the path based on the distance between the object model and the device model. At least one of the object model and the device model is a point cloud model formed by a point cloud. When the interference judgment determines that there is a possibility of interference, at least one of the function of setting the object model and the function of setting the device model increases the density of the point cloud of the point cloud model. The function of performing the interference judgment performs the interference judgment again using the point cloud model with a higher density of the point cloud. When the interference judgment determines that there is no possibility of interference, the function of generating the path generates the next path.

[0009] (Effects of the Invention)

[0010] According to the path generation device, it is possible to shorten the time required for interference judgment and improve the accuracy.

[0011] According to the path generation method, the time for interference judgment can be shortened and the accuracy can be improved.

[0012] According to the path generation program, it is possible to shorten the time required for interference determination and improve the accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0013] Figure 1 is a schematic diagram showing the structure of a robot system.

[0014] Figure 2 1 is a diagram showing a robot control device and a schematic hardware configuration of the control device.

[0015] Figure 3 It is a block diagram showing the structure of a control system of a control unit.

[0016] Figure 4 FIG. 1 is a diagram showing an example of an object model and a device model.

[0017] Figure 5 1 is a flowchart showing the motion control of the robot arm by the control device.

[0018] Figure 6 1 is a flowchart showing a path generation process performed by the control device on the robot arm.

[0019] Figure 7 is a schematic diagram showing a state where the first device model approaches the first object model.

[0020] Figure 8 It shows Figure 7 Schematic diagram of a state in which the first object model and the first device model are changed into the second object model and the second device model.

[0021] Figure 9 It shows Figure 8 Schematic diagram of a state in which the second object model and the second device model are changed into a third object model and a third device model. DETAILED DESCRIPTION

[0022] Hereinafter, exemplary embodiments will be described in detail with reference to the accompanying drawings. Figure 1 is a schematic diagram showing the structure of the robot system 100.

[0023] Robot system 100 includes a robot 1 and a path generation device 3 that generates a path for robot 1. Path generation device 3 includes a control device 5. Path generation device 3 may also include a three-dimensional vision sensor 4 that acquires information about the configuration space in which robot 1 is located. In robot system 100, control device 5 generates a path for robot 1 and controls the movements of robot 1, referring to the detection results of three-dimensional vision sensor 4.

[0024] In this example, the robot 1 carries out an object from the container 91 or carries an object into the container 91 .

[0025] In this example, robot 1 is an industrial robot. Robot 1 includes a robot arm 12 and a robot controller 2, which controls the entire robot 1. Robot arm 12 includes a hand 14, which serves as an end effector. Robot arm 12 grasps an object using hand 14. Robot 1 is an example of a motion device, and more specifically, robot arm 12 is an example of a motion device.

[0026] A robot coordinate system of three orthogonal axes is defined for the space in which the robot 1 is arranged. For example, the Z axis is set in the vertical direction, and the mutually orthogonal X and Y axes are set in the horizontal direction.

[0027] The robotic arm 12 is configured to move in three dimensions. Specifically, the robotic arm 12 is configured to perform translational motions with at least three degrees of freedom. In this example, the robotic arm 12 is a vertical multi-jointed robotic arm. The robotic arm 12 is supported by the base 10. The robotic arm 12 has multiple links and multiple joints, wherein the multiple joints connect the multiple links.

[0028] Specifically, the robot arm 12 includes a first link 12a connected to the base 10, a second link 12b connected to the first link 12a, a third link 12c connected to the second link 12b, a fourth link 12d connected to the third link 12c, and a fifth link 12e connected to the fourth link 12d. Furthermore, the hand 14 can also be considered a link of the robot arm 12.

[0029] Specifically, the base 10 and the first link 12a are connected to each other via a first joint 13a rotatable around an axis extending in the vertical direction. The first link 12a and the second link 12b are connected to each other via a second joint 13b rotatable around an axis extending in the horizontal direction. The second link 12b and the third link 12c are connected to each other via a third joint 13c rotatable around an axis extending in the horizontal direction. The third link 12c and the fourth link 12d are connected to each other via a fourth joint 13d rotatable around the axis of the fourth link 12d (i.e., the direction in which the fourth link 12d extends). The fourth link 12d and the fifth link 12e are connected to each other via a fifth joint 13e rotatable around an axis orthogonal to the axis of the fourth link 12d.

[0030] The hand 14 is connected to the fifth link 12e, the front end of the robot arm 12. The fifth link 12e and the hand 14 are connected via a sixth joint 13f, which is rotatable about a predetermined axis. The hand 14 has two fingers 14a that can be opened and closed. These fingers 14a are driven to open and close by an actuator such as a pneumatic cylinder.

[0031] The robot arm 12 has a servo motor 15 (see Figure 2), the servo motor 15 drives each joint to rotate. Each servo motor 15 has an encoder 15a (refer to Figure 2 ).

[0032] Figure 2 This diagram schematically illustrates the hardware configuration of the robot controller 2 and the controller 5. The robot controller 2 exchanges signals and commands with the controller 5. The robot controller 2 controls the servo motor 15 and hand 14 of the robot arm 12. For example, the robot controller 2 supplies current to the servo motor 15 in response to commands from the controller 5. At this time, the robot controller 2 performs feedback control of the supplied current based on the output of the encoder 15a. Furthermore, the robot controller 2 controls the actuators of the hand 14 to open and close the two fingers 14a.

[0033] The robot control device 2 includes a control unit 21 , a storage unit 22 , and a memory 23 .

[0034] The control unit 21 controls the entire robot control device 2. The control unit 21 performs various computations. For example, the control unit 21 is implemented using a processor such as a CPU (Central Processing Unit). Alternatively, the control unit 21 may be implemented using an MCU (Microcontroller Unit), an MPU (Microprocessor Unit), an FPGA (Field Programmable Gate Array), a PLC (Programmable Logic Controller), or a system LSI.

[0035] The storage unit 22 stores programs and various data executed by the control unit 21. The storage unit 22 is formed of a nonvolatile memory, an HDD (Hard Disk Drive), an SSD (Solid State Drive), or the like.

[0036] The memory 23 temporarily stores data, etc. For example, the memory 23 is formed of a volatile memory.

[0037] The 3D vision sensor 4 acquires spatial information of the configuration space in which the robot arm 12, i.e., the robot 1, is located. This spatial information includes information about the position and shape of objects within the configuration space. The 3D vision sensor 4 acquires three-dimensional spatial information. For example, the 3D vision sensor 4 measures the position and shape of the container 91 and the objects within it from above. The 3D vision sensor 4 outputs the position and shape of the container 91 and the objects within it as point cloud data. The 3D vision sensor 4 outputs the measurement results as spatial information to the control device 5.

[0038] The control device 5 generates a path for the robot arm 12 and outputs a command to the robot control device 2 to move the robot arm 12 along the path. In this example, the path is generated so that the robot arm 12 can move from a predetermined starting position to a target position while avoiding interference with other objects. More specifically, the control device 5 causes the robot arm 12 to perform a pick-and-place operation, in which the robot arm 12 removes a predetermined object A from the container 91 and transports the object A to a loading platform (not shown).

[0039] The control device 5 sends and receives signals and information with the robot control device 2. The detection results of the three-dimensional vision sensor 4 are input to the control device 5. Figure 2 As shown, the control device 5 includes a control unit 51, a storage unit 52, and a memory 53. The control device 5 may further include an input operation unit for the user to operate to make settings related to motion generation, and a display that displays the settings.

[0040] The control unit 51 controls the entire control device 5. The control unit 51 performs various computations. For example, the control unit 51 is implemented using a processor such as a CPU (Central Processing Unit). Alternatively, the control unit 51 may be implemented using an MCU (Microcontroller Unit), an MPU (Microprocessor Unit), an FPGA (Field Programmable Gate Array), a PLC (Programmable Logic Controller), or a system LSI.

[0041] The storage unit 52 stores programs and various data executed by the control unit 51. For example, the storage unit 52 stores a motion control program 61 for controlling the motion of the robot arm 12 and a path generation program 62 for generating a path for the robot arm 12. The storage unit 52 is formed using a nonvolatile memory, an HDD (Hard Disk Drive), an SSD (Solid State Drive), or the like.

[0042] The memory 53 temporarily stores data, etc. For example, the memory 53 is formed using a volatile memory.

[0043] Figure 3 This is a block diagram showing the structure of the control system of the control unit 51. The control unit 51 implements various functions by reading the path generation program 62 from the storage unit 52 into the memory 53 and developing it. Specifically, the control unit 51 functions as an object setting unit 54, a device setting unit 55, a path generation unit 56, an interference determination unit 57, and an action command unit 58.

[0044] The object setting unit 54 sets the object model 71, which is a model of an object contained in the configuration space in which the robot arm 12 is configured. In this example, the object model 71 is a point cloud model formed by a point cloud. In this example, the container 91, the mounting table, and the object in the container 91 are equivalent to the object contained in the configuration space. The object setting unit 54 generates an object model 71 for each object such as the container 91. The object setting unit 54 can change the density of the point cloud of the object model 71 on an object basis. Hereinafter, unless otherwise specified, "density" refers to the density of the point cloud. For example, the object setting unit 54 can generate the object model 71 by downsampling the point cloud data output from the three-dimensional vision sensor 4 to generate the object model 71, thereby generating an object model 71 with a different point cloud density. The object setting unit 54 can change the density of a part of the point cloud of the object model 71. The object setting unit 54 can also change the density of all the point clouds of the object model 71.

[0045] In this example, object setting unit 54 changes the density of the point cloud of object model 71 between three levels: a predetermined first density, a second density higher than the first density, and a third density higher than the second density. Furthermore, object setting unit 54 changes the density of a portion of the point cloud of object model 71. Hereinafter, object model 71 having the highest point cloud density of the first density will be referred to as "first object model 71A," object model 71 having the highest point cloud density of the second density will be referred to as "second object model 71B," and object model 71 having the highest point cloud density of the third density will be referred to as "third object model 71C." When not distinguishing between first object model 71A, second object model 71B, and third object model 71C, they will be referred to simply as "object model 71." Figure 4 is a diagram showing an example of an object model 71 and a device model 72. Figure 4 , the object model 71 of the container 91 in the object model 71 is shown. Figure 4 A first object model 71A of a container 91 is shown.

[0046] The path generation unit 56 gradually generates the path of the robot arm 12. In this example, the path generation unit 56 generates a path from the starting position to the target position passing through one or more intermediate positions (i.e., nodes) between the starting position and the target position of the robot arm 12. Here, since the robot arm 12 has multiple degrees of freedom, the position of the robot arm 12 is uniquely determined by the positions of each joint and the fingers of the hand 14, and can also be regarded as the posture of the robot arm 12 (that is, the starting position can also be regarded as the starting posture, the target position can be regarded as the target posture, and the intermediate position can be regarded as the intermediate posture). In addition, the path of the robot arm 12 is defined by the positions of multiple joints and the positions of the fingers of the hand 14, and is further defined by the rotation angle of each joint.

[0047] For example, the path generation unit 56 sequentially generates intermediate positions of the robot arm 12 from the starting position toward the target position. The generation of the intermediate positions can be regarded as the generation of a path to the intermediate position. The path generation unit 56 generates new intermediate positions randomly or according to predetermined rules (constraints). Prescribed rules include, for example, the robot arm 12 approaching the target position, the robot arm 12 moving away from the object when the robot arm 12 interferes with the object, or the upper limit of the movement amount of each of the multiple joints and the fingers of the hand 14. The path generation unit 56 can also generate intermediate positions by comprehensively judging these rules.

[0048] The generated intermediate position, i.e., the path, is subjected to interference determination by the interference determination unit 57. The path generation unit 56 selectively changes the intermediate position (i.e., changes the path) and generates the next intermediate position (i.e., generates the next path) based on the result of the interference determination.

[0049] The device setting unit 55 sets a device model 72, which is a model of the robot arm 12 in the configuration space. In this example, the device model 72 is a point cloud model formed using a point cloud. The device setting unit 55 generates the device model 72 at the intermediate position of the model generated by the path generation unit 56.

[0050] The device setting unit 55 can change the density of the point cloud of the device model 72. The device setting unit 55 can change the density of a portion of the point cloud of the device model 72. The device setting unit 55 can also change the density of the entire point cloud of the device model 72.

[0051] The storage unit 52 stores point cloud data of the robot arm 12. The point cloud data of the robot arm 12 represents the shape of the robot arm 12, at least the outer shape. Specifically, the point cloud data of the robot arm 12 includes point cloud data of each link. Furthermore, the storage unit 52 stores point cloud data of the robot arm 12 having different densities. Specifically, the storage unit 52 stores point cloud data of the robot arm 12 having a predetermined first density, point cloud data of the robot arm 12 having a second density higher than the first density, and point cloud data of the robot arm 12 having a third density higher than the second density. In addition, the first density, second density, and third density of the point cloud data of the robot arm 12 are substantially the same as the first density, second density, and third density of the first object model 71A, the second object model 71B, and the third object model 71C, respectively.

[0052] The device setting unit 55 reads the point cloud data of the robot arm 12 from the storage unit 52 and places the read point cloud data at the intermediate position generated by the path generation unit 56. In this manner, the device setting unit 55 sets the device model 72 of the robot arm 12. At this time, the device setting unit 55 can change the density of the device model 72 by changing the point cloud data read from the storage unit 52. Furthermore, the device setting unit 55 can read point cloud data of different densities for different parts of the robot arm 12. The device setting unit 55 can also generate a device model 72 with uniform density across the entire robot arm 12. Hereinafter, a device model 72 whose highest point cloud density is the first density will be referred to as "first device model 72A," a device model 72 whose highest point cloud density is the second density will be referred to as "second device model 72B," and a device model 72 whose highest point cloud density is the third density will be referred to as "third device model 72C." When the first device model 72A, the second device model 72B, and the third device model 72C are not distinguished from each other, they are simply referred to as “device model 72 ”. Figure 4 It is shown that a first device model 72A of the entire robot arm 12 is formed using point cloud data of a first density.

[0053] The interference determination unit 57 determines the interference between the device model 72 and the object model 71 after the device model 72 has moved along the path (i.e., at the generated intermediate position) based on the distance between the object model 71 and the device model 72. Here, since the object model 71 and the device model 72 are point cloud models, the distance between the object model 71 and the device model 72 is the distance between the points contained in the point cloud of the object model 71 and the points contained in the point cloud of the device model 72. The interference determination unit 57 uses the point cloud models with lower density to perform interference determination in sequence. The interference determination unit 57 performs interference determination using the object model 71 and the device model 72 with lower density. If there is no possibility of interference, the interference determination is terminated. If it is determined that there is a possibility of interference, the density of the point cloud of at least one of the object model 71 and the device model 72 is increased, and the interference determination is performed again.

[0054] Specifically, when the inter-model distance, which is the shortest distance between the object model 71 and the device model 72, is less than a predetermined safety threshold, the interference determination unit 57 determines that there is a possibility of interference between the object model 71 and the device model 72. On the other hand, when the inter-model distance is greater than the safety threshold, the interference determination unit 57 determines that there is no possibility of interference between the object model 71 and the device model 72. Furthermore, when the interference determination unit 57 determines that there is no possibility of interference, it calculates a margin distance, which is a value obtained by subtracting the safety threshold from the inter-model distance.

[0055] Here, the closest points in each of object model 71 and device model 72 form the inter-model distance, or the shortest distance. In this example, since object model 71 and device model 72 are point cloud models, the inter-model distance is formed by the two closest points in each of object model 71 and device model 72. These points are referred to as "closest points." In other words, the inter-model distance is formed by the closest points in object model 71 and device model 72.

[0056] When the inter-model distance is below the safety threshold, the interference determination unit 57 determines whether the inter-model distance is below an interference threshold smaller than the safety threshold. When the inter-model distance is below the interference threshold, the interference determination unit 57 determines that the object model 71 and the device model 72 interfere with each other.

[0057] Specifically, interference determination unit 57 determines whether interference is possible, not possible, or present based on the distance between object model 71 and device model 72. If interference is possible, interference determination unit 57 increases the density of the point cloud for at least one of object model 71 and device model 72, and performs the same interference determination.

[0058] For example, interference determination unit 57 first determines interference between first object model 71A and first device model 72A. If there is a possibility of interference, interference determination unit 57 then determines interference between second object model 71B and second device model 72B. If there is also a possibility of interference, interference determination unit 57 further determines interference between third object model 71C and third device model 72C.

[0059] In this case, each of second object model 71B and second device model 72B does not need to be entirely of the second density. That is, only the portion of the point cloud containing the closest point in second object model 71B may be of the second density, while the rest may be of the first density. Alternatively, only the portion of the point cloud containing the closest point in second device model 72B may be of the second density, while the rest may be of the first density. Similarly, in determining interference between third object model 71C and third device model 72C, only the portion of the point cloud containing the closest point in third object model 71C may be of the third density, while the rest may be of the first or second density. Alternatively, only the portion of the point cloud containing the closest point in third device model 72C may be of the third density, while the rest may be of the first or second density.

[0060] Furthermore, interference determination unit 57 terminates the interference determination between first object model 71A and first device model 72A if it determines that there is no possibility of interference or that there is interference. The same applies to interference determination between second object model 71B and second device model 72B. The same applies to interference determination between third object model 71C and third device model 72C.

[0061] Furthermore, when determining interference between object model 71 (i.e., third object model 71C) with the highest density and device model 72 (i.e., third device model 72C), interference determination unit 57 utilizes only the interference threshold. Specifically, interference determination unit 57 determines that interference exists when the inter-model distance between third object model 71C and third device model 72C is less than the interference threshold, and determines that there is no likelihood of interference when the inter-model distance between third object model 71C and third device model 72C is greater than the interference threshold.

[0062] Furthermore, the safety threshold for interference determination can be adjusted based on the density of the point cloud model. For example, the higher the point cloud density of the point cloud model, the smaller the safety threshold is set. In this example, the first safety threshold, which is the safety threshold for interference determination between first object model 71A and first device model 72A, is higher than the second safety threshold, which is the safety threshold for interference determination between second object model 71B and second device model 72B.

[0063] On the other hand, even if the density of the point cloud changes, the interference threshold used for interference determination can be the same. In this example, the interference threshold used for interference determination between first object model 71A and first device model 72A, between second object model 71B and second device model 72B, and between third object model 71C and third device model 72C is the same.

[0064] Furthermore, when changing the density of the point cloud model during interference determination, the density of either object model 71 or device model 72 can also be changed. For example, when increasing the density of the point cloud model from first object model 71A and first device model 72A, only first object model 71A can be changed to second object model 71B, while first device model 72A remains unchanged. By simply increasing the density of at least one of the point clouds of object model 71 and device model 72, interference determination can be performed using combinations of object models 71 and device models 72 with varying densities.

[0065] If the interference determination unit 57 determines that there is no possibility of interference, the route generation unit 56 generates the next intermediate position, i.e., the next route. On the other hand, if the interference determination unit 57 determines that there is interference, the route generation unit 56 changes the intermediate position, i.e., the route. The route generation unit 56 also receives the interference determination made by the interference determination unit 57 for the next intermediate position or the changed intermediate position.

[0066] Furthermore, when generating an intermediate position (i.e., a path), if the margin distance between the intermediate position immediately preceding the current intermediate position and the device model 72 is greater than the amount of movement of the device model 72 at the current intermediate position, the path generation unit 56 omits or skips the interference determination for the current intermediate position and generates the next intermediate position. Specifically, if the inter-model distance in the interference determination for the previous intermediate position is sufficiently greater than the amount of movement of the device model 72 toward the current intermediate position, the path generation unit 56 generates the next intermediate position without requiring the interference determination unit 57 to perform the interference determination for the current intermediate position.

[0067] The path generation unit 56 repeats this process to gradually generate a path that is an intermediate position from the start position to the target position. When the path generation unit 56 completes the generation of the path from the start position to the target position, it outputs information on the generated path to the motion instruction unit 58.

[0068] The motion command unit 58 creates a command value based on the path information output from the path generation unit 56 , and outputs the created command value to the robot controller 2 .

[0069] The robot controller 2 drives the servo motor 15 based on the command value from the motion command unit 58. At this time, the robot controller 2 performs feedback control on the current supplied to the servo motor 15 based on the detection result of the encoder 15a. In this way, the robot arm 12 moves from the starting position to the target position along the path generated by the path generation unit 56.

[0070] Next, the operation of the robot system 100 configured as described above will be described. Figure 5 This flowchart shows the motion control performed by the control device 5 on the robot arm 12. This flowchart describes the pick-and-place operation in which the robot arm 12 removes a predetermined object A from the container 91 and transfers it to the loading platform. The control device 5 divides the pick-and-place operation into a first operation and a second operation, which the robot arm 12 executes. The first operation is the operation that ends when the robot arm 12 retrieves the object A, and the second operation is the operation in which the robot arm 12 transfers the object A from the container 91 to the loading platform. This motion control is performed by the control unit 51 reading the motion control program 61 from the storage unit 52 into the memory 53 and developing it.

[0071] First, the control unit 51 causes the robot arm 12 to execute a first action until it picks up object A. In step S1, the control unit 51 acquires spatial information via the 3D vision sensor 4. From above the container 91 and the loading platform, the 3D vision sensor 4 measures the position and shape of the container 91, the loading platform, and the objects within the container 91. The 3D vision sensor 4 outputs the position and shape of the container 91, the loading platform, and the objects within the container 91 to the control device 5 in the form of point cloud data.

[0072] Next, in step S2, the control unit 51 obtains the target position of the robot arm 12. Specifically, the path generation unit 56 first detects the position of the object A based on the spatial information from the three-dimensional vision sensor 4. The path generation unit 56 then determines the position of the robot arm 12 that would allow it to grasp the object A at the detected position without interfering with other objects, and sets this position as the target position.

[0073] Next, in step S3, the control unit 51 obtains the starting position of the robot arm 12. Specifically, the path generation unit 56 calculates the current position of the robot arm 12 based on the detection results of each encoder 15a and sets this current position as the starting position. The path generation unit 56 can obtain the rotation angle of each joint from the detection results of each encoder 15a. The dimensions of each link and hand of the robot arm 12 are known. Therefore, the path generation unit 56 can calculate the position of each part of the robot arm 12 based on the rotation angle of each joint and the dimensions of each link and hand.

[0074] Next, in step S4, the path generation unit 56 generates a path from the start position to the target position of the robot arm 12. In step S4, a path generation process is executed to generate a path.

[0075] In step S5, the control unit 51 causes the robot arm 12 to perform an operation. Specifically, the operation command unit 58 generates a command value for moving the robot arm 12 along the path output from the path generation unit 56 and outputs this command value to the robot control device 2. The robot control device 2 controls the servo motor 15 based on the command value to operate the robot arm 12. The robot arm 12 moves along the path.

[0076] In step S6, the control unit 51 determines whether the target position is the final target position. In other words, sometimes, as in the case of a pick-and-place operation, an action is divided and includes multiple target positions. In step S6, it is determined whether the current target position is the final target position, that is, whether there are any remaining target positions to be set. Since, in the current process, after the first action is completed, a second action remains, it is determined that the current target position is not the final target position.

[0077] When the current target position is the final target position, the control unit 51 ends the motion control of the robot arm 12. In addition, when there is only one target position, the current target position is naturally the final target position.

[0078] If the current target position is not the final target position, the control unit 51 returns to step S2 and repeats the process starting from step S2. In step S2, the target position for the next movement is reset, and the process from step S3 onwards is executed. In the current process, the control unit 51 sets the target position of the robot arm 12 for the second movement as the new target position. Specifically, the path generation unit 56 determines the position of the robot arm 12 that will place object A on the loading platform without interfering with other objects, and sets this position as the new target position.

[0079] In this way, the target position of the robot arm 12 is updated. Then, the processing of steps S3 to S6 is repeated for the new target position. In the current processing, the robot arm 12 finally moves to the target position of the second operation and places the object A on the mounting table.

[0080] When the robot arm 12 reaches the new target position, the control unit 51 again determines in step S7 whether the target position is the final target position. The above process is repeated until the target position reaches the final target position. Since the target position of the second action is the final target position in the current process, the control unit 51 ends the motion control of the robot arm 12.

[0081] Next, the path generation process will be described. Figure 6 2 is a flowchart showing a path generation process of the robot arm 12 performed by the control device 5. The path generation process is executed by the control unit 51 reading the path generation program 62 from the storage unit 52 into the memory 53 and developing the program.

[0082] First, in step sb1, the path generation unit 56 generates a path for the robot arm 12. In the initial step sb1 of the path generation process, the path generation unit 56 generates an intermediate position that is initially passed through from the starting position. As described above, the path generation unit 56 generates the intermediate position randomly or according to a predetermined rule (constraint). The generation of the intermediate position can be considered as the generation of a path to the intermediate position. Hereinafter, when referring to the "current" intermediate position or path, it refers to the most recent intermediate position and path generated in step sb1.

[0083] Next, in step sb2, object setting unit 54 generates first object model 71A. First object model 71A is the object model 71 of the object contained in the configuration space of robot arm 12 and has the lowest density. First object model 71A is entirely formed using a point cloud of the first density. In this example, object setting unit 54 generates first object model 71A for each of container 91, the mounting platform, and the object within container 91.

[0084] Furthermore, in step sb3, the device setting unit 55 generates a first device model 72A. This first device model 72A is the device model 72 of the robot arm 12 in the configuration space and has the lowest density. The entire first device model 72A is formed using a point cloud of the first density. At this point, the device setting unit 55 generates the device model 72 of the robot arm 12 at the current intermediate position.

[0085] Next, in step sb4, interference determination unit 57 determines whether to skip interference determination. Specifically, if the margin distance of the intermediate position immediately preceding the current intermediate position is stored in storage unit 52, interference determination unit 57 determines whether the margin distance is greater than the movement amount of first device model 72A at the current intermediate position. Since the margin distance of the immediately preceding intermediate position is not stored in storage unit 52 in the initial step sb4 of the path generation process, interference determination unit 57 determines not to skip interference determination. The details of step sb4 will be described later.

[0086] If interference determination is not skipped, interference determination unit 57 determines in step sb5 whether the inter-model distance between first object model 71A and first device model 72A is below a first safety threshold. The first safety threshold is the inter-model distance at which, given the density of first object model 71A and first device model 72A, it can be estimated that first object model 71A and first device model 72A do not interfere with each other. Basically, interference determination unit 57 calculates the distance between two points for all combinations of points included in first object model 71A and points included in first device model 72A, and finds the shortest distance between the two points, i.e., the inter-model distance. Interference determination unit 57 then determines whether the calculated inter-model distance is below the first safety threshold. However, if the distance between two points is below the first safety threshold, interference determination unit 57 can still determine that the inter-model distance is below the first safety threshold even if calculations of all distances between two points have not yet been completed.

[0087] At this time, when the target area (described later) is stored in the storage unit 52, the interference determination unit 57 prioritizes the points corresponding to the target area among the points included in the first object model 71A and the points included in the first device model 72A, i.e., the points corresponding to the target area, and first calculates the distance between the two points. The target area is a potential area for interference, and the distance between the two points is more likely to fall below the first safety threshold than other areas. Therefore, the determination of whether the inter-model distance is below the first safety threshold can sometimes be completed earlier, reducing the computational load for the distance between the two points.

[0088] In this example, the interference determination unit 57 calculates the inter-model distance for each of the multiple links of the robot arm 12 and compares the inter-model distance with the first safety threshold for each link. The links here also include the hand 14. Specifically, the inter-model distance is calculated for each of the first link 12a, the second link 12b, the third link 12c, the fourth link 12d, the fifth link 12e, and the hand 14, and a determination is made as to whether the inter-model distance is below the first safety threshold. The interference determination unit 57 determines whether the inter-model distance of at least one of the multiple links is below the first safety threshold. Furthermore, the first safety threshold can be the same for multiple links or different for each link.

[0089] Furthermore, the interference determination unit 57 sequentially calculates the inter-model distances starting with the link on the front end side of the robot arm 12 among the multiple links, and compares the calculated inter-model distances with the first safety threshold. In this case, the interference determination unit 57 sequentially compares the inter-model distances with the first safety threshold starting with the link on the front end side. If the inter-model distance of a particular link is below the first safety threshold, the interference determination unit 57 can determine that the inter-model distance of at least one of the multiple links is below the first safety threshold, even without performing inter-model distance calculations for the remaining links.

[0090] In addition, as will be described in detail later, when a link for which interference judgment is omitted is stored in the storage unit 52, the interference judgment unit 57 does not perform interference judgment on the link (i.e., calculation of the distance between models and comparison of the distance between models with the safety threshold, etc.).

[0091] If the inter-model distance is greater than the first safety threshold, interference determination unit 57 determines that there is no possibility of interference between first object model 71A and first device model 72A. If this determination indicates no possibility of interference, the current intermediate position is determined to be an intermediate position between the start position and the target position, i.e., a path. At this point, interference determination unit 57 subtracts the first safety threshold from the inter-model distance and stores it in storage unit 52 as the margin distance. This concludes the interference determination for the current intermediate position, i.e., the path.

[0092] In the example where the inter-model distance is calculated for each link, the interference determination unit 57 compares the inter-model distance for each link with the first safety threshold. Therefore, if the inter-model distance for all of the multiple links is greater than the first safety threshold, the interference determination unit 57 determines that there is no possibility of interference. The interference determination unit 57 calculates the margin distance for each link and stores the margin distance for each link in the storage unit 52.

[0093] Then, the path generation unit 56 determines whether the path has reached the target position in step sb13. Specifically, the path generation unit 56 determines whether the current intermediate position is the intermediate position before the target position. If the current intermediate position is the intermediate position before the target position, it is determined that the path has reached the target position, and the path generation unit 56 returns to step sb13. Figure 5 The flow of motion control of the robot arm 12 is shown (ie, step S5). On the other hand, if the current intermediate position is not the intermediate position immediately before the target position, the path generation unit 56 returns to step sb1 to generate the next intermediate position.

[0094] In step sb1, the path generation unit 56 generates a path for the next intermediate position immediately following the determined intermediate position (hereinafter referred to as the "previous intermediate position"). Then, in step sb2, the first object model 71A is set. If the object model 71 already generated is the first object model 71A, the object setting unit 54 uses the first object model 71A as is. Next, in step sb3, the device setting unit 55 generates a first device model 72A for the robot arm 12 at the new intermediate position.

[0095] In the next step sb4, path generation unit 56 determines whether to skip the interference determination. If the margin distance at the intermediate position immediately preceding the current intermediate position is stored in storage unit 52, path generation unit 56 determines whether the margin distance is greater than the movement amount of first device model 72A toward the current intermediate position. The movement amount of first device model 72A toward the current intermediate position is the movement amount of first device model 72A's closest point (i.e., the portion closest to first object model 71A) at the previous intermediate position when first device model 72A moves from the previous intermediate position to the current intermediate position.

[0096] When the margin distance is greater than the movement amount of the first device model 72A, the interference determination is omitted or skipped. The path generation unit 56 determines whether the path has reached the target position in step sb13. When the path has reached the target position, the path generation unit 56 returns to step sb14. Figure 5, which shows the flow of motion control for the robot arm 12 (i.e., step S5). If the path has not reached the target position, the path generation unit 56 returns to step sb1 to generate the next intermediate position. In this way, the path generation unit 56 skips interference determination for the current intermediate position, confirms the current intermediate position, and then executes subsequent processing.

[0097] If the margin distance is greater than the amount of movement of first device model 72A toward the current intermediate position, there is a high probability that the inter-model distance will exceed the first safety distance in the interference determination for first device model 72A at the current intermediate position. In other words, if the interference determination is not skipped, it is predicted that the process will proceed to step sb13 after calculating the inter-model distance in step sb5. Therefore, by skipping the interference determination, interference determination unit 57 can omit the calculation of the inter-model distance at the current intermediate position.

[0098] After skipping the interference determination and returning to step sb4 via steps sb1, sb2, and sb3, the margin distance at the previous intermediate position is not stored in storage unit 52. Therefore, in step sb5, interference determination unit 57 calculates the inter-model distance between first object model 71A and first device model 72A at the new intermediate position to determine whether the inter-model distance is below the first safety threshold. If the inter-model distance is greater than the first safety threshold, as described above, interference determination unit 57 determines that there is no possibility of interference, confirms the current intermediate position, and stores the current margin distance in storage unit 52. Processing then returns to step sb1, and path generation unit 56 generates the next intermediate position.

[0099] When generating a path for robot arm 12 from a starting position far from the object, the above process is repeated until first device model 72A approaches any first object model 71A. Intermediate positions, or paths, are sequentially generated from the starting position toward the target position. Ultimately, when first device model 72A approaches first object model 71A, the inter-model distance may fall below the first safety threshold. Figure 7 is a schematic diagram showing a state where the first device model 72A is close to the first object model 71A. Figure 7 , a first object model 71A of the container 91 and a first device model 72A of the robot arm 12 are shown.

[0100] In the example where the inter-model distance is calculated for each link, the path generation unit 56 determines in step sb4 whether to omit, or skip, the interference check for the path for each link. Specifically, the path generation unit 56 determines whether the margin distance for each of the multiple links is greater than the amount of movement of the first device model 72A toward the current intermediate position. The amount of movement of the first device model 72A toward the current intermediate position is the amount of movement of the closest point of each link. If the margin distance for all links is greater than the amount of movement of the first device model 72A, interference check is omitted for the entire robot arm 12, and the path generation unit 56 proceeds to step sb13. On the other hand, if the margin distance for some links, but not all links, is greater than the amount of movement of the first device model 72A, the path generation unit 56 does not omit interference check for the entire robot arm 12. In this case, the path generation unit 56 stores the links for which interference check was omitted in the storage unit 52. Then, in step sb5, the interference determination unit 57 omits interference determination for a link for which interference determination is omitted, when the link is stored in the storage unit 52. Since the movable range of the robot arm 12 varies for each link, the likelihood of interference with an object also varies for each link. Therefore, since the inter-model distance is sufficient, interference determination is omitted for links with a low likelihood of interference with an object, and interference determination is performed only for the remaining links. This reduces the load of inter-model distance calculations and other factors.

[0101] If the inter-model distance is below the first safety threshold in step sb5, interference determination unit 57 determines that there is a possibility of interference between first object model 71A and first device model 72A. Interference determination unit 57 stores a portion of the first object model 71A and first device model 72A, each including a predetermined range of closest points, as a target portion in storage unit 52. In other words, a set of target portions, namely, the target portion of the first object model 71A and the target portion of the first device model 72A, is stored in storage unit 52. The closest points can be considered as portions determined to be potentially interfering. Furthermore, if the distance between two points is below the first safety threshold, interference determination unit 57 stores a set of target portions, namely, the first object model 71A and the first device model 72A, each including a predetermined range of points within the first safety threshold.

[0102] In the example where the inter-model distance is calculated for each link, the interference determination unit 57 compares the inter-model distance with the first safety threshold for each link. Therefore, if the inter-model distance of at least one of the multiple links is below the first safety threshold, the interference determination unit 57 determines that there is a possibility of interference. Then, the portion of the specified range encompassing the closest point of the link whose inter-model distance is below the first safety threshold and the corresponding portion of the specified range encompassing the closest point of the first object model 71A are stored in the storage unit 52 as a single set of target portions. In this case, if there are multiple links whose inter-model distances are below the first safety threshold, the portion of the specified range encompassing the closest point of each of the multiple links whose inter-model distances are below the first safety threshold and the corresponding portion of the specified range encompassing the closest point of the first object model 71A are stored in the storage unit 52 as a single set of target portions. In other words, multiple sets of target portions are stored in the storage unit 52.

[0103] If the inter-model distance is below the first safety threshold, interference determination unit 57 determines in step sb6 whether the inter-model distance is below the interference threshold. The interference threshold is a value less than the first safety threshold. The interference threshold is the same as the interference threshold used in interference determinations between second object model 71B and second device model 72B, and between third object model 71C and third device model 72C. The interference threshold is the inter-model distance at which interference between third object model 71C and third device model 72C can be estimated, given the density of third object model 71C and third device model 72C.

[0104] When the inter-model distance is below the interference threshold, interference determination unit 57 determines that first object model 71A and first device model 72A interfere with each other. Furthermore, when the distance between two points falls below the interference threshold, interference determination unit 57 can still determine that the inter-model distance is below the interference threshold even if calculations for all distances between two points have not yet been completed. This concludes interference determination for the current intermediate position, i.e., the path.

[0105] In step sb7, the path generation unit 56 changes the intermediate position generated in step sb1. Alternatively, the path generation unit 56 may regenerate the intermediate position such that at least the device model 72 is further away from the object model 71. The process then returns to step sb2. In step sb2, if the object model 71 already generated is the first object model 71A, the object setting unit 54 uses the first object model 71A as is. Next, in step sb3, the device setting unit 55 generates the first device model 72A of the robot arm 12 located at the changed intermediate position. In the first step sb4 after the path change in step sb7, the interference determination unit 57 determines not to skip the interference determination. Then, as described above, the interference determination unit 57 determines whether the inter-model distance is below the first safety threshold (step sb5) and, if necessary, whether the inter-model distance is below the interference threshold (step sb6).

[0106] On the other hand, when the inter-model distance exceeds the interference threshold, in step sb8, the object setting unit 54 and the device setting unit 55 increase the density of object model 71 and device model 72 by one level, respectively. Step sb8 is reached when the inter-model distance exceeds the interference threshold and is below the first safety threshold. In other words, it is neither possible nor impossible to determine that first object model 71A and first device model 72A are interfering, but only possible. Therefore, the interference determination unit 57 uses the object model 71 and device model 72 with a higher density level to perform a more detailed interference determination. Specifically, the object setting unit 54 changes the first object model 71A to the second object model 71B with a higher density, and the device setting unit 55 changes the first device model 72A to the second device model 72B with a higher density. Furthermore, the position of the second device model 72B is the same as that of the first device model 72A. Figure 8 It shows Figure 7 Schematic diagram of a state in which the first object model 71A and the first device model 72A are changed into the second object model 71B and the second device model 72B.

[0107] At this time, the object setting unit 54 increases the density of the point cloud of the focus portion of the object model 71. The focus portion of the object model 71 is the portion determined in step sb5 to contain the possibility of interference, that is, the portion closest to the point, and is stored in the storage unit 52. In this example, Figure 8 As shown, the object setting unit 54 forms a portion of the container 91 , that is, a portion of the wall 91 a , which is a portion of interest, using a point cloud of the second density, and forms the other portions using a point cloud of the first density.

[0108] Similarly, the device setting unit 55 increases the density of the point cloud of the attention portion in the device model 72. The attention portion of the device model 72 is the portion determined to contain the possibility of interference in the interference determination in step sb5, that is, the portion closest to the point, and is stored in the storage unit 52. Figure 8 In the example, the device setting unit 55 forms a focus portion of the robot arm 12, that is, a portion 12f of the fourth link 12d, using a point cloud of the second density, and forms other portions using a point cloud of the first density.

[0109] In addition, when the distance between models in multiple links of the robot arm 12 is greater than the interference threshold and below the first safety threshold, the object setting unit 54 increases the density of the point cloud of multiple focus parts in the object model 71, and the device setting unit 55 increases the density of the point cloud of multiple focus parts in the device model 72.

[0110] Then, in step sb9 , the interference determination unit 57 determines whether the inter-model distance between the second object model 71B and the second device model 72B is equal to or smaller than the second safety threshold.

[0111] The second safety threshold is the inter-model distance at which, given the density of second object model 71B and second device model 72B, it can be estimated that second object model 71B and second device model 72B do not interfere with each other. The second safety threshold is a value smaller than the first safety threshold and larger than the interference threshold. Specifically, since the second safety threshold is used to determine the likelihood of interference between second object model 71B, which has a higher density than first object model 71A and first device model 72A, and second device model 72B, it is set smaller than the first safety threshold for more accurate determination.

[0112] The interference determination unit 57 calculates the distance between two points included in the second object model 71B and the second device model 72B to determine the inter-model distance. In this case, the points included in the second object model 71B and the second device model 72B for which the distance calculation is performed are points included in the second density point cloud, i.e., the focus area. The distance calculation is not performed for points included in the second object model 71B and the second device model 72B that are not included in the second density point cloud. This is because the focus area in the second object model 71B and the second device model 72B is determined to be potentially interfering. By calculating the distance between two points in this manner, the amount of computation can be reduced. In this example, the interference determination unit 57 calculates the distance between a point included in the portion 91a of the wall of the container 91 in the second object model 71B and a point included in the portion 12f of the fourth link 12d in the second device model 72B.

[0113] The interference determination unit 57 updates the portion of the second object model 71B and the second device model 72B, which includes a predetermined range of each closest point, as a focus portion and stores the portion in the storage unit 52 .

[0114] If the inter-model distance is greater than the second safety threshold, interference determination unit 57 determines that there is no possibility of interference between second object model 71B and second device model 72B. If this determination indicates no possibility of interference, the current intermediate position is determined to be an intermediate position between the start position and the target position, i.e., a path. At this point, interference determination unit 57 subtracts the second safety threshold from the inter-model distance as the margin distance and stores it in storage unit 52. This concludes interference determination for the current intermediate position, i.e., the path.

[0115] Then, the path generation unit 56 determines whether the path has reached the target position in step sb13. When the path has reached the target position, the path generation unit 56 returns to step sb14. Figure 5 The flow of the motion control of the robot arm 12 is shown (ie, step S5). On the other hand, when the path has not reached the target position, the path generation unit 56 returns to step sb1 to generate the next intermediate position.

[0116] On the other hand, when the inter-model distance is below the second safety threshold, interference assessment unit 57 determines that there is a possibility of interference between second object model 71B and second device model 72B. Furthermore, when the distance between two points falls below the second safety threshold, interference assessment unit 57 can determine that the inter-model distance is below the second safety threshold, even if calculations of distances between all two points have not yet been completed. In this case, interference assessment unit 57 updates the portion of the second object model 71B and second device model 72B, including a predetermined range of each of the two points whose distance is below the second safety threshold, as a focus portion, and stores it in storage unit 52.

[0117] When the inter-model distance is equal to or smaller than the second safety threshold, the interference determination unit 57 determines in step sb10 whether the inter-model distance is equal to or smaller than the interference threshold. As described above, the interference threshold is the same value as that used in the determination in step sb6.

[0118] When the inter-model distance is below the interference threshold, interference determination unit 57 determines that second object model 71B and second device model 72B interfere with each other. Furthermore, when the distance between two points is below the interference threshold, interference determination unit 57 can determine that the inter-model distance is below the interference threshold even if calculations for all distances between two points have not yet been completed. This concludes interference determination for the current intermediate position, i.e., the path.

[0119] In step sb7, path generation unit 56 changes the intermediate position generated in step sb1. The processing after step sb7 is the same as when the inter-model distance is below the interference threshold in the interference determination between first object model 71A and first device model 72A. Furthermore, since the process returns to steps sb2 and sb3 after step sb7, interference determination is performed for first object model 71A and first device model 72A for the changed intermediate position.

[0120] On the other hand, when the inter-model distance exceeds the interference threshold, in step sb11, the object setting unit 54 and the device setting unit 55 increase the density of object model 71 and device model 72 by one level, respectively. Step sb11 is reached when the inter-model distance exceeds the interference threshold and is below the second safety threshold. In other words, it is neither possible to determine that the second object model 71B and the second device model 72B are interfering nor that the second object model 71B and the second device model 72B are not interfering, but only possible to determine that there is interference. Therefore, the interference determination unit 57 uses the object model 71 and device model 72 with a density one level higher for interference determination. Specifically, the object setting unit 54 changes the second object model 71B to the third object model 71C with a higher density, and the device setting unit 55 changes the second device model 72B to the third device model 72C with a higher density. Furthermore, the position of the third device model 72C is the same as that of the second device model 72B. Figure 9 It shows Figure 8 Schematic diagram of a state in which the second object model 71B and the second device model 72B are changed into the third object model 71C and the third device model 72C.

[0121] At this time, the object setting unit 54 increases the density of the point cloud of the focus portion of the object model 71. The focus portion of the object model 71 is the portion that is determined to have the possibility of interference in the interference determination in step sb9, that is, the portion closest to the point, and is stored in the storage unit 52. In this example, Figure 9 As shown, the object setting unit 54 forms a portion of the container 91 , that is, a portion of the wall 91 a , which is a portion of interest, using a point cloud of the third density, and forms the other portions using a point cloud of the first density.

[0122] Similarly, the device setting unit 55 increases the density of the point cloud of the attention portion in the device model 72. The attention portion of the object model 72 is the portion that is determined to have the possibility of interference in the interference determination in step sb9, that is, the portion closest to the point, and is stored in the storage unit 52. In this example, Figure 9 As shown, the device setting unit 55 forms a portion of the robot arm 12 , that is, a portion 12 f of the fourth link 12 d , which is a focus portion, using a point cloud of the third density, and forms the remaining portion using a point cloud of the first density.

[0123] Then, in step sb12, the interference determination unit 57 determines whether the inter-model distance between the third object model 71C and the third device model 72C is equal to or smaller than the interference threshold. As described above, the interference threshold is the same value as that used in the determinations in steps sb6 and sb10.

[0124] The interference determination unit 57 calculates the distance between two points included in the third object model 71C and the third device model 72C to determine the inter-model distance. In this case, the points included in the third object model 71C and the third device model 72C for which the distance calculation is performed are those included in the third density point cloud, i.e., the focus area. The distance calculation is not performed for points included in the third object model 71C and the third device model 72C that are not included in the third density point cloud. This is because the focus area in the third object model 71C and the third device model 72C is determined to be potentially interfering. By calculating the distance between these two points, the amount of computation can be reduced. In this example, the interference determination unit 57 calculates the distance between two points: a point included in the portion 91a of the wall of the container 91 in the third object model 71C and a point included in the portion 12f of the fourth link 12d in the third device model 72C.

[0125] The interference determination unit 57 updates the portion of the third object model 71C and the third device model 72C, which includes the closest point of each, within a predetermined range, as the focus portion, and stores the portion in the storage unit 52 .

[0126] If the inter-model distance is greater than the interference threshold, interference determination unit 57 determines that there is no possibility of interference between third object model 71C and third device model 72C. If this determination indicates no possibility of interference, the current intermediate position is determined to be an intermediate position between the start position and the target position, i.e., a path. At this point, interference determination unit 57 subtracts the interference threshold from the inter-model distance as the margin distance and stores it in storage unit 52. This concludes the interference determination for the current intermediate position, i.e., the path.

[0127] Then, the path generation unit 56 determines whether the path has reached the target position in step sb13. When the path has reached the target position, the path generation unit 56 returns to step sb14. Figure 5 The flow of the motion control of the robot arm 12 is shown (ie, step S5). On the other hand, when the path has not reached the target position, the path generation unit 56 returns to step sb1 to generate the next intermediate position.

[0128] On the other hand, when the inter-model distance is below the interference threshold, interference determination unit 57 determines that third object model 71C and third device model 72C interfere with each other. Furthermore, when the distance between two points is below the interference threshold, interference determination unit 57 can determine that the inter-model distance is below the interference threshold even if calculations for all distances between two points have not yet been completed. Thus, interference determination for the current intermediate position, i.e., the path, is terminated.

[0129] In step sb7, path generation unit 56 changes the intermediate position generated in step sb1. The processing after step sb7 is the same as when the inter-model distance is below the interference threshold in the interference determination between first object model 71A and first device model 72A. Furthermore, since the process returns to steps sb2 and sb3 after step sb7, interference determination is performed for first object model 71A and first device model 72A for the changed intermediate position.

[0130] In this manner, interference determination is performed sequentially for the generated intermediate locations, i.e., the paths, starting with the object models 71 and device models 72 with the lowest density. Specifically, the first object model 71A and the first device model 72A are first used to determine whether the inter-model distance is greater than a first safety threshold, or less than the first safety threshold and greater than an interference threshold, or less than the interference threshold. If the inter-model distance is greater than the first safety threshold, it is determined that there is no possibility of interference in the generated path, and the next path is generated. If the inter-model distance is less than the interference threshold, it is determined that interference has occurred in the generated path, and the path is changed. If the inter-model distance is less than the first safety threshold and greater than the interference threshold, it is determined that there is a possibility of interference.

[0131] When the possibility of interference is determined, an interference determination is performed using object model 71 and device model 72, each one level higher in density, to more accurately assess the presence of interference. Specifically, the second object model 71B and the second device model 72B are used to determine whether the inter-model distance is greater than a second safety threshold, or less than the second safety threshold and greater than an interference threshold, or less than the interference threshold. If the inter-model distance is greater than the second safety threshold, the generated path is determined to have no possibility of interference, and the next path is generated. If the inter-model distance is less than the interference threshold, the generated path is determined to have interference, and the path is changed. If the inter-model distance is less than the second safety threshold and greater than the interference threshold, the generated path is determined to have a possibility of interference.

[0132] When the possibility of interference is determined, to more fully assess the presence of interference, the object model 71 and device model 72, each one level higher in density, are used to perform an interference determination. Specifically, the third object model 71C and the third device model 72C are used to determine whether the inter-model distance is greater than or less than the interference threshold. Since the third object model 71C and the third device model 72C have the highest density, only the inter-model distance is determined to be less than the interference threshold. If the inter-model distance is greater than the interference threshold, the generated path is deemed free of interference, and the next path is generated. If the inter-model distance is less than the interference threshold, the generated path is deemed to have interference, and the path is altered.

[0133] This interference determination is performed based on the inter-model distance between object model 71 and device model 72. The inter-model distance is calculated by calculating the distance between points included in object model 71 and points included in device model 72. Therefore, as the number of points included in the point cloud model increases, the amount of calculation increases, and the calculation time also increases.

[0134] To address this, path generation device 3 uses the lower-density first object model 71A and first device model 72A to perform interference analysis on the generated path. If there is no possibility of interference, interference analysis ends and the path is determined. If interference is determined to be possible, interference analysis is repeated using the higher-density second object model 71B and second device model 72B. This allows interference analysis to be performed sequentially, starting with the lower-density object model 71 and device model 72. Therefore, when interference analysis using the lower-density object model 71 and device model 72 determines the presence or absence of interference, the computational effort required to calculate the distance between points included in first object model 71A and points included in first device model 72A is reduced. However, using the lower-density object model 71 reduces the accuracy of interference analysis. To address this issue, when the interference determination using first object model 71A and first device model 72A fails, path generation device 3 determines that interference is possible, switches from first object model 71A and first device model 72A to second object model 71B and second device model 72B, which have higher density, and performs interference determination again. Using the higher-density second object model 71B and second device model 72B allows for more accurate interference determination. Thus, interference determination by path generation device 3 can shorten the time required for interference determination while improving its accuracy.

[0135] In this example, path generation device 3 sets the density of object models 71 to three levels. Specifically, path generation device 3 uses, in descending order of density, first object model 71A, second object model 71B, and third object model 71C, and first device model 72A, second device model 72B, and third device model 72C to perform interference determination. This allows for a gradual balance between shortening the time required for interference determination and improving accuracy.

[0136] Furthermore, in the path generation device 3, both the object model 71 and the device model 72 are formed using point cloud models. Therefore, the distance calculation between the object model 71 and the device model 72 becomes the distance calculation between two points. This simplifies the distance calculation between the object model 71 and the device model 72.

[0137] Furthermore, the path generation device 3 changes the density of the device model 72 to match the density of the object model 71. Specifically, the device model 72 can be formed by point cloud models having successively higher first, second, and third densities. When performing interference determination using the first object model 71A, which prioritizes reduced computational effort, the first device model 72A having the highest density of the first density is used. By using the first device model 72A with the lowest density in addition to the first object model 71A, the distance calculations between points included in the first object model 71A and points included in the first device model 72A can be reduced. When performing interference determination using the third object model 71C, which prioritizes improved interference determination accuracy, the third device model 72C having the highest density of the third density is used. By using the third device model 72C with the highest density in addition to the third object model 71C, interference determination between the third object model 71C and the third device model 72C can be performed with higher accuracy. When performing interference determination using second object model 71B, which aims to achieve a balance between reducing computational complexity and improving interference determination accuracy, second device model 72B, whose density is the second highest, is used. By using second device model 72B, which has an intermediate density, in addition to second object model 71B, the computational load can be reduced while improving interference determination accuracy. By aligning the density trends of device model 72 with those of object model 71, both interference determination time and accuracy can be shortened.

[0138] As described above, the path generation device 3 for generating the path of the robot arm 12 (action device) includes an object setting unit 54, a device setting unit 55, a path generation unit 56, and an interference judgment unit 57. The object setting unit 54 sets an object model 71, which is a model of an object contained in the configuration space in which the robot arm 12 is configured. The device setting unit 55 sets a device model 72, which is a model of the robot arm 12 in the configuration space. The path generation unit 56 gradually generates the path of the robot arm 12. The interference judgment unit 57 determines the path of the robot arm 12 based on the object model. The distance between the object model 71 and the device model 72 is determined, and interference judgment is performed between the object model 71 and the device model 72 after moving along the path. At least one of the object model 71 and the device model 72 is a point cloud model formed by a point cloud. When the interference judgment unit 57 determines that there is a possibility of interference, at least one of the object setting unit 54 and the device setting unit 55 increases the density of the point cloud model. The interference judgment unit 57 performs interference judgment again using the point cloud model with a higher point cloud density. The path generation unit 56 generates the next path when the interference judgment unit 57 determines that there is no possibility of interference.

[0139] In other words, the path generation method for generating a path for the robot arm 12 (action device) includes the steps of setting an object model 71, setting a device model 72, gradually generating a path for the robot arm 12, and performing interference judgment between the object model 71 and the device model 72 after moving along the path based on the distance between the object model 71 and the device model 72, wherein the object model 71 is a model of an object contained in a configuration space in which the robot arm 12 is configured, the device model 72 is a model of the robot arm 12 in the configuration space, at least one of the object model 71 and the device model 72 is a point cloud model formed using a point cloud, and when the interference judgment determines that there is a possibility of interference, in at least one of the steps of setting the object model 71 and setting the device model 72, the point cloud density of the point cloud model is increased, and in the step of performing interference judgment, interference judgment is performed again using the point cloud model with a higher point cloud density, and in the step of generating a path, when the interference judgment determines that there is no possibility of interference, the next path is generated.

[0140] In addition, in order to generate the movement of the robot arm 12 (movement device), the path generation program 62 allows the computer to realize the function of setting the object model 71, the function of setting the device model 72, the function of gradually generating the path of the robot arm 12, and the function of performing interference judgment between the object model 71 and the device model 72 after moving along the path based on the distance between the object model 71 and the device model 72, wherein the object model 71 is a model of an object contained in the configuration space in which the robot arm 12 is configured, the device model 72 is a model of the robot arm 12 in the configuration space, and at least one of the object model 71 and the device model 72 is a point cloud model formed by a point cloud. When the interference judgment determines that there is a possibility of interference, at least one of the function of setting the object model 71 and the function of setting the device model 72 increases the density of the point cloud of the point cloud model, the function of performing interference judgment performs interference judgment again using the point cloud model with a higher point cloud density, and the function of generating a path generates the next path when the interference judgment determines that there is no possibility of interference.

[0141] Based on these structures, the path of the robot arm 12 is gradually generated. Once the path is generated, interference determination is performed between the object model 71 and the device model 72 after it has moved along the path. This interference determination is performed based on the distance between the object model 71 and the device model 72. In this case, at least one of the object model 71 and the device model 72 is a point cloud model. If the interference determination results in a potential interference, the point cloud density of the point cloud model is increased, and interference determination is performed again. If it is determined that there is no potential interference, the next path is generated. In this way, interference determination is first performed using the point cloud model with a lower density. Because interference determination is performed based on the distance between the object model 71 and the device model 72, the distance calculation load is lower in the interference determination using the lower density point cloud model. In some cases, the interference determination using the lower density point cloud model may determine that there is no potential interference, reducing the distance calculation load. On the other hand, if the interference determination using the lower density point cloud model determines that there is a potential interference, interference determination is performed again using the higher density point cloud model. The higher density of the point cloud model enables more detailed determination of the presence or absence of interference. As a result, the distance calculation load can be reduced and the accuracy of interference judgment can be improved.

[0142] In addition, since at least one of the object model 71 and the device model 72 is a point cloud model, both the object model 71 and the device model 72 may be point cloud models, only the object model 71 may be a point cloud model, or only the device model 72 may be a point cloud model.

[0143] When the interference determination unit 57 determines that there is a possibility of interference, at least one of the object setting unit 54 and the device setting unit 55 increases the density of a portion of the point cloud model including the portion determined to be likely to interfere.

[0144] According to this configuration, when at least one of the object setting unit 54 and the device setting unit 55 (i.e., the one that sets the point cloud model) determines that there is a possibility of interference, the density of the point cloud model is increased not for all points in the point cloud model, but for a portion of the point cloud model that is included in the portion determined to be likely to interfere. This reduces the amount of processing required when at least one of the object setting unit 54 and the device setting unit 55 increases the density of the point cloud model.

[0145] Furthermore, the interference determination unit 57 determines that there is a possibility of interference when the inter-model distance, which is the shortest distance between the object model 71 and the device model 72, is below a predetermined safety threshold, and determines that there is no possibility of interference when the inter-model distance is greater than the safety threshold.

[0146] According to this configuration, the possibility of interference is determined by determining whether the inter-model distance between the object model 71 and the device model 72 is equal to or smaller than a safety threshold.

[0147] Furthermore, the interference determination unit 57 determines that interference exists when the inter-model distance is equal to or smaller than an interference threshold value that is smaller than a safety threshold value. The path generation unit 56 changes the path when the interference determination unit 57 determines that interference exists.

[0148] With this configuration, the interference determination unit 57 determines three states based on the inter-model distance: no possibility of interference, possibility of interference, and presence of interference. Specifically, when the inter-model distance is greater than a safety threshold, it is determined that there is no possibility of interference. When the inter-model distance is below the safety threshold and greater than the interference threshold, it is determined that there is a possibility of interference. When the inter-model distance is below the interference threshold, it is determined that there is interference.

[0149] If there is no possibility of interference, the next path is generated. If there is interference, the path is changed. If there is still interference, the interference is determined again using a denser point cloud model. By repeating this process, the path is gradually generated.

[0150] The safety threshold is set to be smaller as the density of the point cloud of the point cloud model is higher.

[0151] This structure enables interference determination using an appropriate safety threshold based on the density of the point cloud model. Specifically, when the point cloud model density is low and the point cloud model is not detailed, the safety threshold can be set higher. This prevents the situation where interference is mistakenly determined to be absent even when it is present. On the other hand, when the point cloud model density is high and the point cloud model is detailed, the safety threshold can be set lower. Because the point cloud model is detailed, even with a lower safety threshold, the likelihood of interference can still be accurately determined.

[0152] In addition, the robot arm 12 serving as a motion device has multiple links and multiple joints, and the multiple joints connect the multiple links. The interference judgment unit 57 calculates the model distance for each of the multiple links. When the model distance of at least one link of the multiple links is below the safety threshold, it is judged that there is a possibility of interference. When the model distance of all links of the multiple links is greater than the safety threshold, it is judged that there is no possibility of interference.

[0153] According to this configuration, the inter-model distance is calculated for each link of the robot arm 12 , the inter-model distance is compared with the safety threshold for each link, and the possibility of interference is determined for each link.

[0154] Then, the interference determination unit 57 sequentially obtains the inter-model distance starting from the link on the front end side of the robot arm 12 among the plurality of links, and compares the obtained inter-model distance with the safety threshold value.

[0155] According to this structure, the possibility of interference is judged in sequence starting from the links located on the front end side of the robot arm 12. Since the movable range of the links closer to the front end side of the robot arm 12 is larger, the possibility of interference with the object is higher for links closer to the front end side. The interference judgment unit 57 judges that there is a possibility of interference as long as there is at least one link whose inter-model distance is below the safety threshold. In other words, if a link whose inter-model distance is below the safety threshold is found among multiple links, the calculation of the inter-model distance can be omitted for the remaining links. By judging the possibility of interference in sequence starting from the links on the front end side, links with the possibility of interference can be discovered early, increasing the possibility of reducing the load of the calculation of the inter-model distance.

[0156] Furthermore, when the interference judgment unit 57 judges that there is no possibility of interference, it calculates a margin distance which is a value obtained by subtracting a safety threshold from the distance between models. When the path generation unit 56 generates a path, if the margin distance of the previous path connected to the current path is greater than the movement amount of the device model 72 of the current path, the interference judgment on the current path is omitted.

[0157] According to this configuration, when path generation unit 56 generates a new path, if the margin distance in the interference determination for the previous path is greater than the movement of device model 72 along that path, interference determination for the current path is omitted and the next path is generated. Even if device model 72 moves along the current path, there is a high probability that the inter-model distance between object model 71 and device model 72 will exceed the safety threshold. This skipping of interference determination reduces the computational load associated with interference determination and simplifies the overall process.

[0158] In addition, the robot arm 12 serving as a motion device has multiple links and multiple joints, and the multiple joints connect the multiple links. The interference judgment unit 57 calculates the model-to-model distance for each of the multiple links. When the model-to-model distance of at least one link of the multiple links is below a specified safety threshold, it is judged that there is a possibility of interference. When the model-to-model distance of all links of the multiple links is greater than the safety threshold, it is judged that there is no possibility of interference. A margin distance is calculated for each of the multiple links, and when generating a path, the path generation unit 56 determines whether to omit the interference judgment for the current path based on the multiple links.

[0159] This structure further reduces the computational load associated with interference determination. Specifically, in a configuration where the entire robot arm 12, acting as a motion device, determines whether to omit interference determination, even if the robot arm 12 includes a link for which interference determination can be omitted, if it includes a link for which interference determination cannot be omitted, the robot arm 12 does not omit interference determination. On the other hand, in a configuration where whether to omit interference determination is determined on a link-by-link basis, if the robot arm 12 includes a link for which interference determination can be omitted and a link for which interference determination cannot be omitted, interference determination can be omitted for the link for which interference determination can be omitted, and interference determination can be performed only on the link for which interference determination cannot be omitted. This further reduces the computational load associated with interference determination.

[0160] Furthermore, among the object model 71 and the device model 72 , at least the object model 71 is a point cloud model.

[0161] According to this structure, even for unknown objects in the configuration space, interference judgment using a point cloud model can be easily achieved. In detail, usually, when forming the device model 72 using a point cloud model, point cloud data of the device model 72 is often prepared in advance, that is, saved in advance. Therefore, when forming the device model 72 using a point cloud model, point cloud data needs to be prepared in advance. In view of this, information about objects in the configuration space is usually obtained by a device that obtains object information, such as a three-dimensional vision sensor 4. According to such a device, point cloud data of the object can be easily obtained. Therefore, when forming the device model 72 using a point cloud model, point cloud data does not need to be prepared in advance, and point cloud data can be obtained on-site by a three-dimensional vision sensor 4, etc. In this way, even for unknown objects, interference judgment using a point cloud model can be easily achieved.

[0162] Furthermore, both the object model 71 and the device model 72 are point cloud models.

[0163] With this structure, the distance between object model 71 and device model 72 is calculated as the distance between points included in object model 71 and points included in device model 72. In other words, distance calculations are calculated as the distance between two points, simplifying the computation itself. On the other hand, as the number of points included in object model 71 and device model 72 increases, the computational load increases. To address this, as described above, interference determination is performed by sequentially using point cloud models starting with those with lower density, thereby reducing the computational load.

[0164] Furthermore, when the interference judgment unit 57 determines that there is a possibility of interference, the object setting unit 54 increases the density of the point cloud of the object model 71, and the device setting unit 55 increases the density of the point cloud of the device model 72, and the interference judgment unit 57 performs interference judgment on the device model 72 with a higher point cloud density and the object model 71 with a higher point cloud density.

[0165] According to this configuration, when interference is determined to be possible, the point cloud densities of both object model 71 and device model 72 are increased. Specifically, to reduce computational load, when the density of object model 71 is low, the density of device model 72 is also low, thereby contributing to a reduction in computational load. On the other hand, to improve the accuracy of interference determination, when the density of object model 71 is high, the density of device model 72 is also high, thereby contributing to improved accuracy of interference determination.

[0166] Furthermore, the object setting unit 54 increases the density of the point cloud of a portion of the object model 71 that is included in the portion judged to have the possibility of interference in the interference judgment, and the device setting unit 55 increases the density of the point cloud of a portion of the device model 72 that is included in the portion judged to have the possibility of interference in the interference judgment.

[0167] According to this configuration, when the object setting unit 54 determines that there is a possibility of interference, it increases the density of a portion of the point cloud of object model 71, not the entire point cloud, but rather the portion determined to be potentially interfering. This reduces the amount of processing required when the object setting unit 54 increases the density of object model 71. Similarly, when the device setting unit 55 determines that there is a possibility of interference, it increases the density of a portion of the point cloud of device model 72, not the entire point cloud. This reduces the amount of processing required when the device setting unit 55 increases the density of device model 72.

[0168] (Other embodiments)

[0169] As described above, the embodiments are described as examples of the technology disclosed in this application. However, the technology disclosed in the present invention is not limited thereto, and can also be applied to embodiments that have undergone appropriate changes, replacements, additions, omissions, etc. Furthermore, the various constituent elements described in the embodiments can also be combined as new embodiments. Furthermore, the constituent elements recorded in the drawings and detailed descriptions include not only the constituent elements necessary to solve the problem, but also constituent elements that are not necessary to solve the problem in order to illustrate the technology. Therefore, it should not be immediately assumed that those non-essential constituent elements are necessary just because they are recorded in the drawings and detailed descriptions.

[0170] For example, the motion device is not limited to the robot arm 12 and the robot 1. Any device that performs motion can be a motion device. For example, the motion device may be a robot that does not include a robot arm, such as a self-propelled robot. Furthermore, the motion device may be a mobile object such as a drone. Furthermore, the robot 1 is not limited to an industrial robot.

[0171] Furthermore, the actuator may be the finger 14 a of the hand 14 instead of the robot arm 12 .

[0172] The point cloud data generated by the object setting unit 54 and serving as the basis for the object model 71 is not limited to the point cloud data output by the three-dimensional vision sensor 4. The object setting unit 54 may also generate point cloud data based on, for example, an RGB-D image output by an RGB-D camera. Furthermore, the object setting unit 54 may generate point cloud data based on an RGB image output by a stereo camera, a depth image, voxels, or the like, in addition to an RGB-D image.

[0173] Furthermore, the object setting unit 54 is not limited to generating point cloud data based on data output from devices such as sensors and cameras, but may also generate the object model 71 based on point cloud data previously acquired and stored in the storage unit 52 or other devices.

[0174] Either the object model 71 or the device model 72 need not be a point cloud model. For example, the object setting unit 54 may generate the object model 71 using a point cloud model, while the device setting unit 55 may generate the device model 72 using a model other than a point cloud model. Conversely, the object setting unit 54 may generate the object model 71 using a model other than a point cloud model, while the device setting unit 55 may generate the device model 72 using a model other than a point cloud model. As an example of a model other than a point cloud model, when the device model 72 is a model other than a point cloud model, the device setting unit 55 may generate a device model 72 having the same outer surface as the actual motion device (in this example, the robot arm 12). Alternatively, the device setting unit 55 may generate the device model 72 by approximating the outer shape of the actual motion device to a cylinder or polygon, or by forming the device model 72 using polygons. In this case, the distance between a point included in the object model and the device model is the distance between the point included in the object model and the surface forming the device model 72. The same applies when the object setting unit 54 generates a model other than a point cloud model.

[0175] The object setting unit 54 changes the density of the object model 71 using three levels, but the present invention is not limited to this. The object setting unit 54 may also change the density of the object model 71 using two levels, or four or more levels. The object setting unit 54 may also change the density of the entire object model 71, rather than just the density of a portion of the object model 71 including the portion of interest.

[0176] The device setting unit 55 changes the density of the device model 72 using three levels, but this is not limiting. The device setting unit 55 may also change the density of the device model 72 using two levels, or four or more levels. Furthermore, the first density, second density, and third density of the device model 72 do not necessarily have to be equal to the first density, second density, and third density of the object model 71, respectively. When changing the density of the device model 72, the device setting unit 55 may change the density of the entire device model 72, i.e., the entire robot arm 12, rather than just the density of the link containing the portion of interest within the device model 72. Alternatively, the device setting unit 55 may change the density of only a portion of the link containing the portion of interest, rather than just the density of the entire link containing the portion of interest.

[0177] Furthermore, when it is determined in the interference determination that there is a possibility of interference, at least the density of the object model 71 only needs to be changed, and the density of the device model 72 does not need to be changed.

[0178] The path of the motion device is not limited to the path in the above-mentioned pick-and-place action. This technology can be applied to the generation of paths in various actions of the motion device.

[0179] The starting position of the path is obtained by the detection result of the encoder 15a, but is not limited to this. The target position is obtained by performing image recognition on the spatial information from the three-dimensional visual sensor 4 using image recognition technology, but is not limited to this. The starting position or the target position can also be obtained by the control unit 51 by accepting input from the outside. For example, the control device 5 can also have an operation input unit such as a touch panel, and the user can select the starting position and / or the target position from the image of the configuration space displayed on the touch panel. Alternatively, the starting position and / or the target position can also be determined in advance and stored in the storage unit 52, etc.

[0180] The safety threshold is set to decrease as the density of the point cloud of the object model 71 increases, but the present invention is not limited thereto. In other words, the first safety threshold is not necessarily greater than the second safety threshold. For example, the first safety threshold and the second safety threshold may be the same value.

[0181] The interference threshold is the same value in interference determination for object models 71 of different densities, but is not limited thereto. For example, the interference threshold may be set to be smaller as the density of the point cloud of the object model 71 increases.

[0182] At the skip point in step sb4 of the path generation process, if the margin distance is greater than the movement amount of first device model 72A toward the current intermediate position, interference determination is skipped. The movement amount of first device model 72A toward the current intermediate position is the movement amount of the closest point of first device model 72A when first device model 72A moves from the previous intermediate position to the current intermediate position, but is not limited to this. The movement amount of first device model 72A used in this skip determination may also be the maximum movement amount of each component of first device model 72A (e.g., multiple joints and fingers of hand 14). Alternatively, if an upper limit value for the movement amount of each component of device model 72 (multiple joints and fingers of hand 14) is set as a rule for generating the intermediate position, this upper limit value may also be used.

[0183] Furthermore, the interference determination in step sb4 may be skipped not only for the next intermediate position but also for multiple intermediate positions thereafter. For example, if the margin distance is greater than the movement amount of the first device model 72A to the multiple intermediate positions thereafter, the interference determination may be skipped for these multiple intermediate positions.

[0184] In addition, the process of skipping the interference determination in step sb4 may be omitted.

[0185] In the path generation process, if the density of object model 71 is changed in two levels, steps sb9, sb11, and sb12 are omitted. Specifically, a determination is made as to whether the inter-model distance between second object model 71B and second device model 72B is below the interference threshold. A determination as to whether the inter-model distance is below the second safety threshold is not made. If the inter-model distance is below the interference threshold, the intermediate position is changed in step sb7. If the inter-model distance is greater than the interference threshold, a determination is made in step sb13 as to whether the target position has been reached.

[0186] Furthermore, during the path generation process, when the density of object model 71 changes by four or more levels, the third object model 71C and the third device model 72C are also subjected to a determination of whether the inter-model distance is below the third safety threshold, which is a safety threshold, before determining whether the inter-model distance is below the interference threshold. Subsequently, when determining whether the inter-model distance is below the third safety threshold, the interference determination between the object model 71 and the device model 72 with the highest density is performed instead of determining whether the inter-model distance is below the safety threshold.

[0187] In the above flowchart, the control device 5 starts moving the robot arm 12 after generating each path from the starting position to the target position, but the present invention is not limited to this. The control device 5 may also move the robot arm 12 along each path generated step by step from the starting position to the target position after each path is generated and determined.

[0188] Furthermore, in the flowcharts, the order of the steps may be changed, a plurality of steps may be processed in parallel, steps may be omitted, or steps may be added, as long as the desired processing can be achieved.

[0189] The functions of the structures disclosed in this embodiment can be performed using circuits or processing circuits, which include general-purpose processors, special-purpose processors, integrated circuits, application-specific integrated circuits (Application Specific Integrated Circuits), conventional circuits, and / or combinations thereof that are constructed or programmed to perform the disclosed functions. Since the processor includes transistors and other circuits, it is considered to be a processing circuit or circuit. In this disclosure, a circuit, unit, or device is hardware for performing the functions listed or hardware programmed to perform the functions listed. The hardware can be either the hardware disclosed in this specification, or other known hardware that is programmed or constructed to perform the functions listed. When the hardware is a processor that is considered to be a type of circuit, the circuit, device, or unit is a combination of hardware and software, and the software is used for the structure of the hardware and / or processor.

[0190] (Explanation of Symbols)

[0191] 100-Robot system; 1-Robot; 12-Robot arm (action device); 3-Path generation device; 54-Object setting unit; 55-Device setting unit; 56-Path generation unit; 57-Interference judgment unit; 62-Path generation program; 71-Object model; 72-Device model.

Claims

1. A path generating device for generating a path of an action device, characterized in that: The path generation device includes an object setting unit, a device setting unit, a path generation unit, and an interference determination unit. The object setting unit sets a model of an object contained in a configuration space in which the action device is configured, namely, an object model. The device setting unit sets a model of the action device in the configuration space, namely, a device model. The path generation unit gradually generates a path for the action device. The interference determination unit determines interference between the object model and the device model after the object model moves along the path based on the distance between the object model and the device model. At least one of the object model and the device model is a point cloud model formed by a point cloud, When the interference determination unit determines that there is a possibility of interference, at least one of the object setting unit and the device setting unit increases the density of the point cloud of the point cloud model. The interference determination unit performs the interference determination again using the point cloud model with a higher density of the point cloud. The path generating unit generates a next path when the interference determining unit determines that there is no possibility of interference.

2. The path generation device according to claim 1, wherein: When the interference determination unit determines that there is a possibility of interference, at least one of the object setting unit and the device setting unit increases the density of the point cloud of a portion of the point cloud model included in the portion determined to have the possibility of interference in the interference determination.

3. The path generation device according to claim 1 or 2, characterized in that: The interference determination unit determines that there is a possibility of interference when the inter-model distance, which is the shortest distance between the object model and the device model, is equal to or smaller than a predetermined safety threshold. When the distance between the models is greater than the safety threshold, it is determined that there is no possibility of interference.

4. The path generation device according to claim 3, wherein: The interference determination unit determines that there is interference when the distance between the models is below an interference threshold value that is smaller than the safety threshold value. The path generating unit changes the path when the interference determining unit determines that there is interference.

5. The path generation device according to claim 3 or 4, characterized in that: The safety threshold is set to be smaller as the density of the point cloud of the point cloud model increases.

6. The path generation device according to any one of claims 3 to 5, characterized in that: The motion device is a robot arm having a plurality of links and a plurality of joints connecting the plurality of links. The interference determination unit calculates the inter-model distance for each of the plurality of links. When the inter-model distance of at least one link of the plurality of links is below the safety threshold, it is determined that there is a possibility of interference. When the inter-model distances of all the links of the plurality of links are greater than the safety threshold, it is determined that there is no possibility of interference.

7. The path generation device according to claim 6, wherein: The interference determination unit sequentially obtains the inter-model distance starting from the link on the front end side of the robot arm among the plurality of links, and compares the obtained inter-model distance with the safety threshold value.

8. The path generation device according to claim 3, wherein: When the interference determination unit determines that there is no possibility of interference, it determines a margin distance as a value obtained by subtracting the safety threshold from the distance between the models. When generating the path, the path generating unit omits the interference determination on the current path when the margin distance of the previous path connected to the current path is greater than the movement amount of the device model of the current path.

9. The path generation device according to claim 8, wherein: The motion device is a robot arm having a plurality of links and a plurality of joints connecting the plurality of links. The interference determination unit calculates the inter-model distance for each of the plurality of links. When the inter-model distance of at least one link of the plurality of links is below the safety threshold, it is determined that there is a possibility of interference. When the inter-model distances of all the links are greater than the safety threshold, it is determined that there is no possibility of interference, and the margin distance is calculated for each of the links. When generating the path, the path generation unit determines whether to omit the interference determination on the current path for each of the plurality of links.

10. The path generation device according to any one of claims 1 to 7, characterized in that: At least the object model among the object model and the device model is the point cloud model.

11. The path generation device according to any one of claims 1 to 10, characterized in that: Both the object model and the device model are the point cloud models.

12. The path generation device according to claim 11, wherein: When the interference judgment unit determines that there is a possibility of interference, the object setting unit increases the density of the point cloud of the object model, the device setting unit increases the density of the point cloud of the device model, and the interference judgment unit performs interference judgment between the device model with the increased density of the point cloud and the object model with the increased density of the point cloud.

13. The path generation device according to claim 12, wherein: The object setting unit increases the density of the point cloud of a portion of the object model including the portion determined as having a possibility of interference in the interference determination. The device setting unit increases the density of the point cloud of a portion of the device model including a portion determined as having a possibility of interference in the interference determination.

14. A path generation method for generating a path of an action device, characterized by: The path generation method includes the steps of setting an object model, which is a model of an object contained in a configuration space in which the motion device is configured; setting a device model, which is a model of the motion device in the configuration space; gradually generating a path for the motion device; and determining interference between the object model and the device model after the object model moves along the path based on a distance between the object model and the device model. At least one of the object model and the device model is a point cloud model formed by a point cloud, When the interference determination determines that there is a possibility of interference, the density of the point cloud of the point cloud model is increased in at least one of the steps of setting the object model and setting the device model. In the step of performing the interference judgment, the interference judgment is performed again using the point cloud model with a higher density of the point cloud. In the step of generating the path, when it is determined by the interference determination that there is no possibility of interference, a next path is generated.

15. A path generation program, characterized in that: The path generation program causes a computer to implement functions of setting an object model, i.e., a model of an object contained in a configuration space in which the action device is configured, and setting a device model, i.e., a model of the action device in the configuration space, in order to generate an action of the action device; a function of gradually generating a path for the action device; and a function of determining interference between the object model and the device model after the object model moves along the path based on the distance between the object model and the device model. At least one of the object model and the device model is a point cloud model formed by a point cloud, When the interference determination determines that there is a possibility of interference, at least one of the function of setting the object model and the function of setting the device model increases the density of the point cloud of the point cloud model. The function for performing the interference judgment performs the interference judgment again using the point cloud model with a higher density of the point cloud. The function of generating the path generates a next path when the interference determination determines that there is no possibility of interference.

Images