Path planning device, method, and program

Abstract

To execute route planning with high efficiency and a high success rate.SOLUTION: An initial operation path acquisition unit 22 acquires an initial operation path in an assumed path region in a configuration space. An operation path correction unit 24 deletes an element path that interferes with an obstacle from among element paths in the initial operation path, and divides the initial operation path into a first end path, a second end path, and independent operation paths. An operation path generation unit 26 generates an operation path by: adding, to the first end path, an element path that connects a first specific via point randomly selected from the first end path and a new via point randomly set in the assumed path region and does not interfere with the obstacle; adding an element path that connects a second specific via point closest to the new via point in the second end path and a first target via point selected from the first end path and does not interfere with the obstacle; and adding an element path that connects a third specific via point closest to the new via point in all the independent operation paths and a second target via point selected from the first end path and does not interfere with the obstacle.SELECTED DRAWING: Figure 3

Description

[Technical field]

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

[0002] Conventionally, when planning a path for a robot to move to a specific position, such as the position of the hand of an arm robot, a technology has been proposed for planning a path for the robot to move while avoiding interference with obstacles in a multidimensional space (configuration space) whose elements are the angles of each joint of the arm.

[0003] For example, a robot simulation device has been proposed that automatically generates a realistic interference avoidance path for a robot regardless of the skill level of an operator. This device executes a simulation of a robot's motion program to obtain a first motion path, detects interference in the first motion path, and identifies a first teaching point and a second teaching point, which are teaching points before and after the occurrence of the interference. This device also automatically inserts at least one third teaching point between the first and second teaching points based on a search direction and a search distance determined by random numbers, and generates a second motion path that does not cause interference. Then, this device evaluates each of the second motion paths based on at least one predetermined parameter, and selects an optimal motion path from a plurality of second motion paths (see Patent Document 1). [Prior art documents] [Patent documents]

[0004] [Patent Document 1] JP 2015-160277 A Summary of the Invention [Problem to be solved by the invention]

[0005] The robot simulation device described in Patent Document 1 generates a movement path by reconnecting teaching points before and after a point of interference with an obstacle on the first movement path, which is the initial movement path, i.e., by reconnecting the end points of the path that is divided by the interference point.

[0006] However, this method may result in inefficient route generation because it deletes all routes that do not cause interference.In addition, if the end points of a divided route are surrounded by obstacles and become a so-called dead end, route generation may fail.

[0007] The present invention has been made in consideration of the above points, and has an object to execute route planning with high efficiency and a high success rate. [Means for solving the problem]

[0008] A path planning device according to a first aspect is a path planning device that plans a movement path of a robot from a start point to an end point in a configuration space of the robot, the path planning device including: an initial movement path acquisition unit that sets a path expectation area in the configuration space, and acquires an initial movement path that is an initial movement path from the start point to the end point via a plurality of element paths within the path expectation area; a movement path correction unit that, when interference occurs between any of the element paths included in the initial movement path and an obstacle, deletes the element path in which interference occurs, and, among the movement paths divided by deleting the element paths, sets one of the movement path including the start point and the movement path including the end point as a first end path, sets the other movement path as a second end path, and, when there are one or more movement paths that do not include either the start point or the end point, sets the movement path as an independent movement path; and a first process that randomly selects any of the via points included in the first end path as a first specified via point, randomly sets a new via point at a position within the path expectation area, and sets an element path connecting the first specified via point and the new via point to the first end path. a second process of adding an element path to the second end path, and if the added element path interferes with the obstacle, deleting the added element path and returning to the first process, and if there is no interference, maintaining the added element path; a third process of designating a via point that is closest to the new via point among the via points included in the second end path as a second specific via point; a third process of selecting any of the via points included in the first end path as a first target via point, and adding one or more element paths connected in series to connect the second specific via point and the first target via point, and selecting the first specific via point and the second specific via point as a first target via point, and selecting the first specific via point and the second specific via point as a first target via point, and selecting the first specific via point and the second specific via point as a first specific ... as a first specific via point as a first specific via point, and selecting the first specific via point as a first specific via point as a first specific via point as a first specific via point as a first specific via point as a first specific via point a fourth process of terminating generation of a motion path if the second specific via point and the first target via point are connected, and deleting at least the interfering portion if the added element path has a portion interfering with the obstacle; a fifth process of returning to the first process if the independent motion path does not exist, and if the independent motion path does exist, selecting a via point closest to the new via point as a third specific via point among all the via points included in the independent motion paths that exist, and setting the independent motion path to which the via point belongs as a target path; and selecting any one of the via points included in the first end path as a second target via point.and a movement path generating unit that generates a movement path by executing a sixth process, which adds one or more element paths connected in series to connect the third specific via point and the second target via point, and returns to the first process if any of the added element paths connect the third specific via point and the second target via point without interfering with the obstacle, and if any of the added element paths has a portion interfering with the obstacle, deletes at least the interfering portion and returns to the first process.

[0009] A path planning method according to a second aspect is a path planning method executed by a path planning device that plans a movement path of the robot from a start point to an end point in a configuration space of the robot, the path planning method including: a first process in which an initial movement path acquisition unit sets a path expectation area in the configuration space; and acquires an initial movement path, which is an initial movement path from the start point to the end point via a plurality of element paths in the path expectation area; a movement path correction unit, when interference occurs between any of the element paths included in the initial movement path and an obstacle, deletes the element path in which interference occurs; and, among the movement paths divided by deleting the element path, sets either one of the movement path including the start point and the movement path including the end point as a first end path and the other movement path as a second end path; and, when there are one or more movement paths that do not include either the start point or the end point, sets the movement path as an independent movement path; and a movement path generation unit randomly selects any of the via points included in the first end path as a first specified via point; randomly sets a new via point at a position within the path expectation area; and generates an element path connecting the first specified via point and the new via point. a second process of adding the element path to the first end path, and if the added element path interferes with the obstacle, deleting the added element path and returning to the first process, and if there is no interference, maintaining the added element path; a third process of designating the via point closest to the new via point among the via points included in the second end path as a second specific via point; a third process of selecting any of the via points included in the first end path as a first target via point, and adding one or more element paths connected in series to connect the second specific via point and the first target via point, and if any of the added element paths interfere with the obstacle, deleting the added element path and returning to the first process; a fourth process of terminating the generation of a motion path if the second specific via point and the first target via point are connected without any interference between the obstacle and the added element path, and deleting at least the interfering portion if the added element path has a portion interfering with the obstacle; a fifth process of returning to the first process if the independent motion path does not exist, and if an independent motion path does exist, selecting a via point closest to the new via point as a third specific via point among all the via points included in the independent motion paths that exist, and setting the independent motion path to which the via point belongs as a target path; and selecting one of the via points included in the first end path as a second target via point.a sixth process for generating a motion path, which adds one or more element paths connected in series for connecting the third specific via point and the second target via point, and if any of the added element paths connects the third specific via point and the second target via point without interfering with the obstacle, returns to the first process, and if any of the added element paths has a portion interfering with the obstacle, deletes at least the interfering portion and returns to the first process.

[0010] A path planning program according to a third aspect of the present invention is a path planning device for planning a motion path of a robot from a start point to an end point in a configuration space of the robot, the path planning program including a computer, comprising: an initial motion path acquisition unit for setting a path assumption area within the configuration space, and acquiring an initial motion path that is an initial motion path from the start point to the end point via a plurality of element paths within the path assumption area; a motion path correction unit for deleting an element path where interference has occurred between any of the element paths included in the initial motion path and an obstacle, and for setting one of the motion paths including the start point and the motion path including the end point as a first end path, and setting the other motion path as a second end path, and for setting one or more motion paths not including either the start point or the end point as an independent motion path, and a first process for randomly selecting any of the via points included in the first end path as a first specified via point, a randomly setting a new via point at a position within the path assumption area, and setting an element path connecting the first specified via point and the new via point as an independent motion path, a second process of adding the element path to the first end path, and if the added element path interferes with the obstacle, deleting the added element path and returning to the first process, and if there is no interference, maintaining the added element path; a third process of designating the via point closest to the new via point among the via points included in the second end path as a second specific via point; a third process of selecting any of the via points included in the first end path as a first target via point, and adding one or more element paths connected in series to connect the second specific via point and the first target via point, and if any of the added element paths interfere with the obstacle, a fourth process of terminating the generation of a motion path if the second specific via point and the first target via point are connected without any interference, and deleting at least the interfering portion if the added element path has a portion interfering with the obstacle; a fifth process of returning to the first process if the independent motion path does not exist, and if an independent motion path does exist, selecting a via point closest to the new via point among all the via points included in the independent motion paths that exist as a third specific via point and setting the independent motion path to which the via point belongs as a target path; and selecting any one of the via points included in the first end path as a second target via point.a sixth process that adds one or more serially connected element paths for connecting the third specific via point and the second target via point, and if any of the added element paths connects the third specific via point and the second target via point without interfering with the obstacle, returns to the first process, and if any of the added element paths has a portion interfering with the obstacle, deletes at least the interfering portion and returns to the first process. Effect of the Invention

[0011] According to the route planning device, method, and program of the present invention, route planning can be executed with high efficiency and a high success rate. [Brief description of the drawings]

[0012] [Figure 1] FIG. 1 is a schematic configuration diagram of a robot control system. [Diagram 2] FIG. 2 is a block diagram showing a hardware configuration of the path planning device. [Diagram 3] 1 is a block diagram showing an example of a functional configuration of a route planning device according to a first embodiment. [Figure 4] FIG. 4 is a diagram for explaining correction of an initial operation path in the first embodiment. [Diagram 5] FIG. 2 is a diagram for explaining first and second processing in the first embodiment. [Figure 6] 13 is a diagram for explaining setting of a new via point using index points. FIG. [Figure 7] FIG. 13 is a diagram for explaining a region congested with via points; [Figure 8] FIG. 13 is a diagram for explaining a dead end caused by an obstacle. [Figure 9] FIG. 11 is a diagram for explaining third and fourth processes in the first embodiment. [Figure 10] FIG. 11 is a diagram for explaining a fourth process in the first embodiment. [Figure 11]FIG. 11 is a diagram for explaining fifth and sixth processes in the first embodiment. [Figure 12] FIG. 4 is a diagram for explaining first to fourth processes in the first embodiment. [Figure 13] 13 is a flowchart illustrating an example of a route planning process. [Figure 14] 13 is a flowchart illustrating an example of a route planning process. [Figure 15] 13 is a flowchart illustrating an example of a route planning process. [Figure 16] FIG. 13 is a diagram for explaining end point attributes and rules. [Figure 17] FIG. 11 is a block diagram showing an example of a functional configuration of a route planning device according to a second embodiment. [Figure 18] 13 is a diagram for explaining acquisition of an initial operation path in the second embodiment. FIG. [Figure 19] FIG. 11 is a diagram for explaining first and second processing in the second embodiment. [Figure 20] FIG. 11 is a diagram for explaining a fourth process in the second embodiment. [Figure 21] FIG. 13 is a diagram for explaining a sixth process in the second embodiment. [Figure 22] FIG. 23 is a diagram for explaining the replacement of parent points and child points in the sixth process. [Figure 23] 11 is a diagram for explaining the determination of a through-motion path. FIG. [Figure 24] FIG. 13 is a diagram for explaining output of a penetrating operation path. [Diagram 25] FIG. 13 is a diagram for explaining output of a penetrating operation path. [Figure 26] 13 is a flowchart illustrating an example of a replacement process. [Figure 27] FIG. 13 is a diagram for explaining generation of a movement path in the first modification example. [Figure 28] FIG. 13 is a diagram for explaining generation of a movement path in the first modification example. [Figure 29] 13 is a diagram for explaining correction of an initial movement path in Modification 2. FIG. [Diagram 30] 13 is a diagram for explaining the interchange of parent points and child points in Modification 2. FIG. [Diagram 31] 13 is a diagram for explaining the specification of a through-movement path in Modification 2. FIG. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0013] Hereinafter, an example of an embodiment of the present invention will be described with reference to the drawings. In each drawing, the same or equivalent components and parts are given the same reference numerals. In addition, the dimensions and ratios of the drawings are exaggerated for the convenience of explanation and may differ from the actual ratios.

[0014] First Embodiment As shown in FIG. 1, a robot control system 100 according to the first embodiment includes a path planning device 10, a robot controller 12, and a robot 16.

[0015] The robot 16 is, for example, a vertical articulated robot having six degrees of freedom required for operation in a three-dimensional space. The degrees of freedom of the robot 16 may be seven degrees of freedom, which is six degrees of freedom plus a redundant degree of freedom. The robot 16 operates under the control of the robot controller 12 according to the operation path output from the path planning device 10.

[0016] The robot controller 12 controls the movement of the robot 16 according to the movement path output from the path planning device 10 .

[0017] The path planning device 10 generates, in a configuration space, a movement path that avoids interference between the robot 16 and an obstacle when the robot 16 is moved according to the movement path, and outputs the movement path to the robot controller 12.

[0018] In this embodiment, the movement path is a sequence of postures of the robot 16 arranged in chronological order when a predetermined position of the tip of the robot 16 (the side where a tool such as a robot hand is attached) is moved from an arbitrary start point to an end point. The posture of the robot 16 is a series of values ​​(rotation angles) (θ) of each joint from the first joint (joint J1) to the Nth joint (joint JN, N is the number of joints of the robot) of the robot 16, assuming that a predetermined position of the tip of the robot 16 is at a certain position (x, y, z) and posture (roll, pitch, yaw). J1 ,θ J2 , ,θ JN )

[0019] Fig. 2 is a block diagram showing a hardware configuration of the path planning device 10 according to this embodiment. As shown in Fig. 2, the path planning device 10 has a CPU (Central Processing Unit) 32, a memory 34, a storage device 36, an input device 38, an output device 40, a storage medium reading device 42, and a communication I / F (Interface) 44. Each component is connected to each other via a bus 46 so as to be able to communicate with each other.

[0020] A route planning program for executing route planning processing is stored in the storage device 36. The CPU 32 is a central processing unit, and executes various programs and controls each component. That is, the CPU 32 reads the program from the storage device 36 and executes the program using the memory 34 as a working area. The CPU 32 controls each of the components and performs various calculation processes according to the program stored in the storage device 36.

[0021] The memory 34 is made up of a RAM (Random Access Memory) and temporarily stores programs and data as a working area. The storage device 36 is made up of a ROM (Read Only Memory), HDD (Hard Disk Drive), SSD (Solid State Drive), etc., and stores various programs including an operating system, and various data.

[0022] The input device 38 is a device for performing various inputs, such as a keyboard, a mouse, etc. The output device 40 is a device for outputting various information, such as a display, a printer, etc. A touch panel display may be used as the output device 40 to function as the input device 38.

[0023] The storage medium reader 42 reads data stored in various storage media such as a CD (Compact Disc)-ROM, a DVD (Digital Versatile Disc)-ROM, a Blu-ray disc, a USB (Universal Serial Bus) memory, etc., and writes data to the storage media. The communication I / F 44 is an interface for communicating with other devices, and uses standards such as Ethernet (registered trademark), FDDI, and Wi-Fi (registered trademark).

[0024] The hardware configuration of the robot controller 12 is also generally similar to the hardware configuration of the path planning device 10 shown in FIG. 2, and therefore a description thereof will be omitted.

[0025] Next, the functional configuration of the route planning device 10 according to the first embodiment will be described.

[0026] Fig. 3 is a block diagram showing an example of a functional configuration of the path planning device 10. As shown in Fig. 3, the path planning device 10 includes, as functional components, an initial movement path acquisition unit 22, a movement path correction unit 24, and a movement path generation unit 26. Each functional component is realized by the CPU 32 reading out a path planning program stored in the storage device 36, expanding it in the memory 34, and executing it.

[0027] The initial movement path acquisition unit 22 acquires an initial movement path, which is an initial movement path in planning a movement path of the robot 16 from a start point to an end point via one or more waypoints in the configuration space of the robot 16. Specifically, the initial movement path acquisition unit 22 sets a path assumption area in which a movement path is planned in the configuration space. The path assumption area may be, for example, the movable range of the robot 16 determined by the movable range of each joint of the robot 16. The initial movement path acquisition unit 22 may acquire, for example, a path that linearly connects the start point and the end point, a path estimated using a model trained in advance by machine learning, a path used in a similar situation in the past, or the like, as the initial movement path.

[0028] The initial motion path is a motion path that starts from a starting point and passes through multiple element paths to an end point. An example of the initial motion path is shown in the upper diagram of FIG. 4. In FIG. 4, the black circle indicates the starting point q start , intermediate point, and end point q goal The lines connecting the points represent element paths. An element path is a single path connecting two points. That is, the starting point q start , intermediate point, and end point q goal configures the end points of the component path. As shown in the upper diagram of Figure 4, the initial motion path includes component paths that interfere with obstacles. An "obstacle" is an object that prevents the robot 16 from achieving a posture specified in the configuration space, and includes surrounding objects outside the robot 16 and the structure of the robot 16 itself that interferes with itself.

[0029] The movement path correction unit 24 judges whether or not each element path included in the initial movement path interferes with an obstacle based on the initial movement path acquired by the initial movement path acquisition unit 22 and specification information including kinematics information, shape information, etc. of the robot 16. Specifically, the movement path correction unit 24 acquires information on the joint angles of the robot from the point of the object for which interference judgment is to be performed within the path expected area. Based on the acquired joint angle information, information on the shape of the arm of the robot 16, and surrounding obstacle information, the movement path correction unit 24 judges whether or not interference occurs within the simulator by a method such as AABB (Axis Aligned Bounding Box). The arrangement of each part of the robot 16 in the simulation is determined in order from the base by forward kinematics while taking into account the joint angles.

[0030] The movement path correction unit 24 divides the initial movement path by deleting element paths that interfere with obstacles from among the element paths included in the initial movement path, and corrects the initial movement path into a plurality of divided movement paths, as shown in the lower diagram of Fig. 4. In the lower diagram of Fig. 4, the element paths shown by dashed lines represent the element paths to be deleted.

[0031] In addition, the movement path correction unit 24 corrects the start point q of the divided movement path. start A split motion path including the end point q goal The movement path correction unit 24 determines one of the divided movement paths including the start point q as a first end path, and the other as a second end path. start A split motion path including the end point q goal It may be possible to select which of the divided motion paths including the first end path should be the first end path. For example, the motion path correction unit 24 may select based on a predetermined rule such that the divided motion path selected as the first end path alternates each time there is an opportunity for selection. Also, for example, the motion path correction unit 24 may accept a selection designation from a user via a user interface and select the designated divided motion path as the first end path.

[0032] In the lower diagram of Figure 4, the starting point q start The divided motion path including the first end path, the end point qgoal In this example, the divided motion path including the start point q is set as the second end path. start and end point q goal A divided motion path that does not include any of the above is defined as an independent motion path.

[0033] The movement path generating unit 26 randomly sets a new via point at a position within the path expectation area. The movement path generating unit 26 also generates a movement path by adding one or more element paths that connect a via point selected corresponding to the new via point and the new via point among the via points included in the divided movement path and that do not interfere with an obstacle.

[0034] Specifically, the motion path generating unit 26 executes a first process of randomly selecting any one of the via points included in the first end path as the first specific via point. Note that the term "random" here includes cases where there is a bias in the distribution. The via point selected here may be an end point of the first end path, or may be a via point other than the end point.

[0035] The motion path generating unit 26 also executes a second process of adding an element path to the first end path. In the second process, as shown in FIG. 5, a new via point q is added to a position in the path expectation area, the position being the first new path length away from the first specific via point in a direction randomly selected. new For example, the movement path generating unit 26 may set an index point q at a position randomly selected within the path prediction area as shown in FIG. index In addition, the movement path generating unit 26 sets an index point q index Then, the movement path generating unit 26 selects the route point closest to the index point q as the first specific route point. index A new waypoint q is placed at the 0th new route length L0 away in the direction of new The 0th new route length L0 may be a fixed value or may be set to the new route point q new It is also possible to allow variation, such as by determining randomly each time the value is set.

[0036] In addition, the second process is the first specific route point and the new route point q new and adding an element path connecting the first end path and the second end path to the first end path. The second process also includes, if the added element path interferes with the obstacle, deleting the added element path and returning to the first process, and, if there is no interference, maintaining the added element path.

[0037] Here, consider a case where a congested area of ​​via points (a dashed line area in FIG. 7) exists in the first end path as shown in FIG. 7, and this congested area corresponds to the position of a dead end caused by an obstacle as shown in FIG. 8. In this case, it is difficult for an element path extending from a via point selected from this congested area to avoid interference with the obstacle. In such a case, if a first specific via point is simply selected randomly from the via points included in the first end path, there is a high probability that a via point in the congested area will be selected. Therefore, the process of selecting a via point from the congested area is repeated many times until an element path capable of avoiding interference with the obstacle is generated, which increases the processing time. On the other hand, in this embodiment, as described above, a new via point q is selected at a random position within the expected path area. new Then, the first specific route point is selected and an element route is added, so that the above problems can be easily avoided.

[0038] In addition, the movement path generating unit 26 selects a new via point q from among the via points included in the second end path. new A third process is executed to select a route point closest to the first route point as a second specific route point.

[0039] The movement path generating unit 26 also executes a fourth process of adding an element path to the second end path. The fourth process includes selecting any one of the via points included in the first end path as a first target via point, as shown in FIG. 9. For example, the movement path generating unit 26 selects a new via point q from among the via points included in the first end path as a first target via point. new or the way point that is closest to the second specific way point may be selected as the first destination way point.

[0040] The fourth process also includes adding one or a plurality of element paths connected in series to connect the second specific via point and the first target via point. The fourth process also includes terminating the generation of the motion path when the second specific via point and the first target via point are connected without any of the added element paths interfering with an obstacle. The fourth process also includes deleting at least the interfering portion when the added element path has a portion interfering with an obstacle.

[0041] A specific description will be given of a case where a plurality of element paths that connect the second specific via point and the first target via point in series are added in the fourth process. The movement path generating unit 26 sets either the second specific via point or the first target via point as the origin via point, and the other as the destination via point. FIG. 9 shows an example where the second specific via point is the origin via point, and the first target via point is the destination via point. The movement path generating unit 26 repeats the process of adding an element path with a length of the first new path length L1 from the origin via point to the destination via point until the remaining distance to the destination via point is equal to or less than the first new path length L1. If the added element path with the length L1 interferes with an obstacle during the repetition of the process of adding the element path, the movement path generating unit 26 deletes the added element path and ends the repetition. Furthermore, when the remaining distance is equal to or less than the first new path length L1, the movement path generating unit 26 adds an element path connecting an end point of the already added element path and the extension destination via point, and ends the generation of the movement path if the added element path does not interfere with an obstacle. On the other hand, when the added element path interferes with an obstacle, the movement path generating unit 26 deletes the added element path.

[0042] Note that, although Fig. 9 describes the case where an element path is added from the second specific way point side, an element path may be added from the first target way point side, or from both sides. "Adding" an element path includes "attempting to add" an element path, which is attempted but cancelled because it is determined that the element path will interfere with an obstacle. "Deleting" an element path includes "cancelling the addition", which is attempted but cancelled because it is determined that the element path will interfere with an obstacle.

[0043] A case where one element path connecting the second specific via point and the first target via point is added in the fourth process will be specifically described. As shown in the upper diagram of FIG. 10, the movement path generating unit 26 adds a single element path having the second specific via point and the first target via point as both end points, and sets an interference judgment point at a predetermined interval on the single element path. The movement path generating unit 26 repeats the judgment of whether or not the element path interferes with an obstacle, starting from the interference judgment point closest to the second specific via point or the first target via point. The example in the lower diagram of FIG. 10 shows an example of judging from the interference judgment point on the second specific via point side. When the movement path generating unit 26 judges that there is no interference for all the interference judgment points, it ends the generation of the movement path. On the other hand, when it judges that there is interference, the movement path generating unit 26 sets the interference judgment point immediately before the interference judgment point where interference occurs as the end point, as shown in the lower diagram of FIG. 10, and deletes the part from the end point of the element path to the first target via point or the second specific via point.

[0044] In addition, when there is no independent movement path, the movement path generating unit 26 returns to the first process, and when there is an independent movement path, the movement path generating unit 26 selects a new via point q among the via points included in all the independent movement paths. new A fifth process is executed to select the route point closest to the first route as the third specific route point.

[0045] The movement path generating unit 26 also executes a sixth process of taking the independent movement path including the third specific via point as a target path and adding the target path to the first end path. The sixth process includes selecting any one of the via points included in the first end path as a second target via point. For example, the movement path generating unit 26 selects a new via point q from among the via points included in the first end path as a second target via point. new or the way point that is closest to the third specific way point may be selected as the second target way point.

[0046] The sixth process also includes adding one or more element paths connected in series to connect the third specific via point and the second target via point, as shown in FIG. 11. The sixth process also includes returning to the first process if the third specific via point and the second target via point are connected without any of the added element paths interfering with an obstacle. The sixth process also includes returning to the first process after deleting at least the interfering part in the added element path if there is a part interfering with an obstacle. The specific process of adding one or more element paths connected in series to connect the third specific via point and the second target via point is similar to the specific process of adding an element path to connect the second specific via point and the first target via point in the fourth process.

[0047] 12 shows how the first to fourth processes are repeatedly executed after the sixth process is completed. As shown in the upper diagram of FIG. 12, the motion path generating unit 26 executes the first process and the second process to generate a first specific via point selected from the new first end path and a newly set new via point q new 12, the movement path generating unit 26 executes the third process and the fourth process to select a new second specific via point from the second end path, and adds an element path connecting the newly set first target via point and the second specific via point. The movement path generating unit 26 generates a movement path by repeatedly executing the above first to sixth processes.

[0048] Next, the operation of the robot control system 100 according to the first embodiment will be described. Figures 13 to 15 are flowcharts showing the flow of a path planning process executed by the CPU 32 of the path planning device 10. The CPU 32 reads out a path planning program from the storage device 36, deploys it in the memory 34, and executes it, whereby the CPU 32 functions as each functional component of the path planning device 10, and the path planning process shown in Figure 13 is executed.

[0049] In step S10, the initial movement path acquisition unit 22 sets a path assumption area in the configuration space of the robot 16. Next, in step S12, the initial movement path acquisition unit 22 acquires an initial movement path in the set configuration space. Next, in step S14, the movement path correction unit 24 determines whether or not each element path included in the initial movement path interferes with an obstacle. If an element path where interference occurs exists, the process proceeds to step S16, and if no interference occurs, the path planning process ends.

[0050] In step S16, the movement path correction unit 24 divides the initial movement path by deleting the element paths that interfere with the obstacle from among the element paths included in the initial movement path, and corrects the initial movement path into a plurality of divided movement paths. Next, in step S18, the movement path correction unit 24 divides the initial movement path into a plurality of divided movement paths from the start point q start A split motion path including the end point q goal The movement path correction unit 24 selects which of the divided movement paths including the first end path to be the first end path, the independent movement path, and the second end path from the divided movement paths.

[0051] Next, in step S20, the movement path generating unit 26 randomly selects one of the via points included in the first end path as a first specific via point. Next, in step S22, the movement path generating unit 26 selects a new via point q at a position within the path expectation area and at a position away from the first specific via point by the 0th new path length L0 in a direction randomly selected. new Then, the movement path generating unit 26 sets the first specific via point and the new via point q newAn element path connecting the above is added to the first end path.

[0052] Next, in step S24, the movement path generating unit 26 judges whether the added element path interferes with an obstacle. If it interferes, the process proceeds to step S26, and if it does not interfere, the process proceeds to step S24 (FIG. 14). In step S26, the movement path generating unit 26 deletes the added element path and returns to step S18.

[0053] In step S28, the movement path generating unit 26 selects a new via point q from among the via points included in the second end path. new Next, in step S30, the movement path generating unit 26 selects a new via point q new or the via point closest to the second specific via point, is selected as the first target via point.

[0054] Next, in step S32, the motion path generating unit 26 judges whether the distance between the second specific via point and the first target via point is equal to or less than the first new path length L1. If the distance between the second specific via point and the first target via point is equal to or less than L1, the process proceeds to step S38, and if the distance exceeds L1, the process proceeds to step S34.

[0055] In step S34, the movement path generating unit 26 sets either the second specific via point or the first target via point as the origin via point and the other as the destination via point, and adds an element path of length L1 from the origin via point to the destination via point. Next, in step S36, the movement path generating unit 26 determines whether the added element path interferes with an obstacle. If it does interfere, the process proceeds to step S42, and if it does not interfere, the process returns to step S28.

[0056] On the other hand, in step S38, the movement path generating unit 26 adds an element path that connects the end point of the already added element path and the extension destination via point. Next, in step S40, the movement path generating unit 26 judges whether the added element path interferes with an obstacle. If it interferes, the process proceeds to step S42, and if it does not interfere, the path planning process ends.

[0057] In step S42, the movement path generating unit 26 deletes the element path added in step S34 or S38. Next, in step S44, the movement path generating unit 26 judges whether or not an independent movement path exists. If an independent movement path exists, the process proceeds to step S46 (FIG. 15), and if not, the process returns to step S18 (FIG. 13).

[0058] In step S46, the movement path generating unit 26 selects a new via point q from among the via points included in all the independent movement paths. new Next, in step S48, the movement path generating unit 26 selects the closest via point to the new via point q as the third specific via point, and sets the independent movement path including the third specific via point to the target path. new or the via point closest to the third specific via point, any of the via points included in the first end route is selected as the second target via point.

[0059] Next, in step S50, the motion path generating unit 26 judges whether the distance between the third specific via point and the second target via point is equal to or less than the second new path length L2. If the distance between the third specific via point and the second target via point is equal to or less than L2, the process proceeds to step S56, and if the distance exceeds L2, the process proceeds to step S52.

[0060] In step S52, the movement path generating unit 26 sets either the third specific via point or the second target via point as the origin via point and the other as the destination via point, and adds an element path of length L2 from the origin via point to the destination via point. Next, in step S54, the movement path generating unit 26 determines whether the added element path interferes with an obstacle. If it does interfere, the process proceeds to step S60, and if it does not interfere, the process returns to step S46.

[0061] On the other hand, in step S56, the movement path generating unit 26 adds an element path that connects the end point of the already added element path and the extension destination via point. Next, in step S58, the movement path generating unit 26 judges whether the added element path interferes with an obstacle. If it interferes, the process proceeds to step S60, and if it does not interfere, the process proceeds to step S62.

[0062] In step S60, the movement path generating unit 26 deletes the element path added in step S52 or S56, and returns to step S18 (FIG. 13). On the other hand, in step S62, the movement path generating unit 26 sets the first end path to which the target path is added as a new first end path, and returns to step S18 (FIG. 13).

[0063] The path planning device 10 outputs the movement path generated by executing the above-mentioned path planning process to the robot controller 12. The robot controller 12 controls the movement of the robot 16 according to the movement path input from the path planning device 10.

[0064] As described above, in the robot control system according to the first embodiment, the path planning device plans a motion path of the robot from a start point to an end point via one or more via points in the configuration space of the robot. The path planning device acquires an initial motion path, which is an initial motion path from a start point to an end point via multiple element paths, within a path expectation area set in the configuration space. The path planning device also divides the initial motion path by deleting element paths that cause interference with an obstacle from among the element paths included in the initial motion path, and modifies the initial motion path into multiple divided motion paths. Then, the path planning device randomly sets new via points at positions within the path expectation area. For each of the multiple divided motion paths, the path planning device generates a motion path by adding element paths that do not cause interference with an obstacle and that are one or more element paths that connect a via point selected corresponding to a new via point from among the via points included in the divided motion path and the new via point, among the via points included in the divided motion path.

[0065] In this way, in this embodiment, when deleting the interference part of the initial movement path, the calculation cost of the path planning can be reduced by leaving the non-interference area and reducing the distance that needs to be reconnected. That is, high efficiency can be realized. In addition, in this embodiment, instead of reconnecting the end points of the divided movement paths, a movement path is generated by adding an element path that connects a way point selected randomly from one of the divided movement paths and a new way point. This can increase the success rate of the path planning by avoiding the failure of the generation of the movement path due to the path reconnection never being completed even if the end point of the divided movement path exists in a dead end.

[0066] <Second embodiment> Next, a description will be given of a second embodiment. In the robot control system according to the second embodiment, the same components as those in the robot control system 100 according to the first embodiment will be denoted by the same reference numerals and detailed description will be omitted.

[0067] As shown in FIG. 1, a robot control system 200 according to the second embodiment includes a path planning device 210, a robot controller 12, and a robot 16.

[0068] In the first embodiment, since a way point other than the end point of each divided motion path may be connected to a way point of another divided motion path, a complex branched motion path may be generated. Therefore, in the second embodiment, in the generated motion path, the start point q start to end point q goal We propose a method for easily identifying a penetrating motion path, which is a motion path that penetrates all the way to the target object.

[0069] In the second embodiment, as shown in the upper diagram of FIG. 16, an element path constituting a motion path has two end points, one of which is a parent point and the other of which is a child point. In addition, in the second embodiment, as shown in the lower diagram of FIG. 16, a rule is set that multiple parent points must not overlap at one via point. This is because, when identifying a through motion path by tracing from a parent point to a child point, if parent points overlap at one via point, the motion path cannot be uniquely identified. Note that multiple child points are allowed to overlap at one via point. Details will be described later, but when a violation of the above rule occurs, a process is executed to swap the parent point and child point of the relevant element path.

[0070] As shown in FIG. 17, the path planning device 210 includes, as its functional components, an initial movement path acquiring unit 222, a movement path correcting unit 24, a movement path generating unit 226, and a through movement path identifying unit 228.

[0071] The initial movement path acquiring unit 222 acquires an initial movement path in the same manner as the initial movement path acquiring unit 22 in the first embodiment. At this time, the initial movement path acquiring unit 222 sets end point attributes for each end point of the element paths included in the initial movement path such that, as shown in Fig. 18, an end point of one of two element paths having via points included in the initial movement path as end points is a parent point and an end point of the other element path is a child point.

[0072] The movement path generating unit 226 generates a movement path by executing the first to sixth processes in the same manner as the movement path generating unit 26 of the first embodiment. At this time, as shown in FIG. 19, in the second process, the movement path generating unit 226 generates a movement path by performing the first specific via point and the new via point q new The movement path generating unit 226 sets the first specific via point as a child point for the element path connecting the second specific via point and the first destination via point in the fourth process as shown in Fig. 20. Similarly, the movement path generating unit 226 sets the second specific via point as a child point for the element path connecting the second specific via point and the first destination via point in the sixth process as shown in Fig. 21.

[0073] Furthermore, in the second, fourth, and sixth processes, if parent point overlap occurs when an element path is added, the movement path generating unit 226 replaces the parent point and child point of the element path other than the added element path. Furthermore, the movement path generating unit 226 repeats the process of replacing the parent point and child point of the other element path of the parent point overlap newly generated by the replacement until the parent point overlap is eliminated.

[0074] A specific description will be given by taking the sixth process shown in FIG. 21 as an example. As shown in FIG. 21, when an element path for connecting the second destination via point and the third specific via point is added, parent point overlap occurs at the second destination via point. In this case, the movement path generating unit 226 replaces the parent point and the child point of the other element path whose parent point overlaps with the added element path, as shown in the upper diagram of FIG. 22. As a result, parent point overlap occurs again for the element path whose parent point and child point have been replaced. Therefore, the movement path generating unit 226 replaces the parent point and the child point of the other element path, as shown in the lower diagram of FIG. 22. As a result, the parent point overlap is resolved, and the repeated process of replacing the parent point and the child point is terminated.

[0075] The movement path generating unit 226 repeatedly executes the first to sixth processes, similar to the movement path generating unit 26 of the first embodiment. At this time, the movement path generating unit 226 executes the process of eliminating parent point overlap in the second process, the fourth process, and the sixth process, as described above. As a result, a movement path as shown in the upper diagram of FIG. 23 is finally generated.

[0076] After the generation of the movement path is completed in the fourth process, the through movement path identification unit 228 identifies the starting point q start Or end point q goal From the child point, the starting point q start Or end point q goal A plurality of element paths that are connected up to the end of the element path and are linked such that a child point of one element path becomes a parent point of the other element path are identified as a through motion path. In the example in the lower diagram of Fig. 23, the linked element paths shown by double lines are identified as through motion paths.

[0077] The through motion path specification unit 228 outputs information on the specified through motion path. For example, assume that a through motion path as shown in FIG. 24 is specified in the configuration space. The number assigned to each via point is a via point number. In this case, each element path included in the through motion path is a start point q corresponding to the parent point and child point of the element path. start , the number of the intermediate point, and the end point q goal 25. In the example of FIG. 25, each row corresponds to one element path. The through-movement path identification unit 228 may use, for example, [q start ,(1),(2),(3),(4),(5),(6),(7),(8),q goal The through motion path may be output in the format:

[0078] The hardware configuration of the route planning device 210 is similar to the hardware configuration of the route planning device 10 according to the first embodiment shown in FIG. 2, and therefore a description thereof will be omitted.

[0079] Next, the operation of the robot control system 200 according to the second embodiment will be described. In the second embodiment, as in the first embodiment, the path planning process shown in Fig. 13 to Fig. 15 is executed. In the second embodiment, after steps S22, S34, S38, S52, and S56, a replacement process shown in Fig. 26 is executed.

[0080] In step S70, the motion path generating unit 226 judges whether or not parent point overlap occurs in the added element path. If parent point overlap occurs, the process proceeds to step S72, and if parent point overlap does not occur, the process returns to the path planning process.

[0081] In step S72, the movement path generating unit 226 exchanges the parent point and the child point of the other element path of the added element path. Next, in step S74, the movement path generating unit 226 judges whether or not a new parent point overlap has occurred due to the exchange in the above step S72. If a parent point overlap has occurred, the process proceeds to step S76.

[0082] In step S76, the motion path generating unit 226 exchanges the parent point and child point of the element path of the other element path of the previously exchanged element path, and returns to step S74. If it is determined in step S74 that no parent point overlap has occurred, the process returns to the path planning process.

[0083] In addition, in the path planning process of the second embodiment, if a negative judgment is made in step S14 or S40, i.e., if the generation of the movement path is completed, the path planning process ends after a step in which the through movement path identification unit 228 identifies and outputs the through movement path.

[0084] As described above, in the robot control system according to the second embodiment, the path planning device generates a motion path using element paths having end point attributes of parent points and child points so that parent points do not overlap. Then, the path planning device identifies, in the generated motion path, a plurality of element paths connected from a start point or an end point, which is a parent point, to a start point or an end point, which is a child point, and linked such that a child point of one element path becomes a parent point of the other element path, as a through motion path. This makes it possible to easily identify a through motion path even when a motion path that branches in a complex manner is generated.

[0085] <Variation 1> In the above second embodiment, a case has been described in which the added element path has the first specific via point, the second specific via point side, and the third specific via point side as child points in each of the second, fourth, and sixth processes. In the first modified example, the motion path generating unit has the first specific via point as the parent point of the added element path in the second process, has the second specific via point side as the parent point of the added element path in the fourth process, and has the third specific via point side as the parent point of the added element path in the sixth process.

[0086] For example, in the case of the second process, as shown in the upper diagram of FIG. 27, the movement path generating unit according to the first modification defines the first specific via point as the parent point, the new via point q new An element path having a child point of is added. If a parent point overlap occurs in the added element path, the parent point and child point of the other element path are swapped, as shown in the lower diagram of FIG. 27. Also, as in the case of the second embodiment, if a new parent point overlap occurs as a result of swapping a parent point and a child point, the process of swapping the parent point and child point of the other element path of the previously swapped element path is repeated until the parent point overlap is resolved. The swapping of parent points and child points is also performed in the fourth and sixth processes.

[0087] Furthermore, in the fourth process, when the second specific via point and the first target via point are connected, the motion path generating unit according to the first modification replaces the parent point and child point of the element path having either the start point or the end point as the parent point, and further replaces the parent point and child point of the other element path with which the parent point newly generated by the replacement overlaps, repeating the process until the parent point overlaps. Fig. 28 shows a case where the parent point and child point of the start point are replaced, and a case where the parent point and child point of the end point are replaced. In the lower diagram of Fig. 28, the element path where the parent point and child point have been replaced is shown by a thick line.

[0088] <Variation 2> In variant example 2, the swapping of parent points and child points always targets the independent operation path, and when the first end path and the second end path are connected, the parent points and child points are not swapped, i.e., the parent point overlap is not eliminated.

[0089] The movement path correction unit according to the second modification, after correcting the initial movement path into a plurality of divided movement paths, swaps parent points and child points of element paths whose end points are start points or end points having the end point attribute of a parent point. The movement path correction unit according to the second modification also repeats the process of swapping parent points and child points of the other element path with which the parent point newly generated by the swap overlaps, until there is no parent point overlap. In the example in the upper diagram of FIG. 29, the start point q start Since is the parent point, the starting point q start The parent and child points of the element path with the end point of the element are swapped. Because this swapping creates a new parent point overlap, the parent and child points of the other element path are swapped to eliminate the parent point overlap, as shown in the lower diagram of Figure 29.

[0090] In addition, in the second processing step, the motion path generation unit of the second variant example takes the first specific via point as a child point of the added element path, in the fourth processing step, the added element path takes the extension source via point side as a child point, and in the sixth processing step, the added element path takes the third specific via point side as a parent point.

[0091] Furthermore, in the sixth process, if an overlap of parent points occurs when an element path is added as shown in the upper diagram of Fig. 30, the motion path generating unit according to the second modification replaces the parent point and child point of the element path other than the added element path as shown in the lower diagram of Fig. 30, and further, repeats the process of replacing the parent point and child point of the other element path with which the newly generated parent point overlaps as a result of the replacement, until the parent point overlaps no longer exists. As a result, in the second modification, the parent point and child point of the element path are replaced on the independent motion path side.

[0092] Furthermore, when the movement path generating unit according to the second modification ends the generation of the movement path in the fourth process, it sets the via point where parent point overlap occurs as a connection via point, as shown in the upper diagram of FIG.

[0093] After the generation of the motion path is completed in the fourth process, the through motion path identification unit according to the second modification identifies, as through motion paths, a plurality of element paths connected from a connection via point, which is a parent point, to a start point and an end point, which are child points, and linked so that a child point of one element path becomes a parent point of the other element path, as shown in the lower diagram of Fig. 31. In the lower diagram of Fig. 31, from the connection via point q to the start point q start The chained element path is drawn with a solid double line from the connection via point to the end point q goal The element path chained up to is shown by a double dashed line.

[0094] When an initial motion path is divided into a first end path, a second end path, and an independent motion path, the independent motion path often has fewer element paths than the first end path and the second end path. Therefore, when the replacement of parent points and child points is limited to the independent motion path side as in the modified example 2, the number of times of the replacement process of parent points and child points is reduced compared to the second embodiment and the modified example 1, and the calculation cost can be reduced in many cases. In addition, in the modified example 2, by specifying the through motion path from the connection via point to each of the start point and the end point, the replacement of parent points and child points at the time when the generation of the motion path is completed can be omitted, and the calculation cost can be reduced.

[0095] In the above embodiments, the path planning device and the robot controller are separate devices, but the path planning device may be incorporated in the robot controller. Also, each functional configuration of the path planning device may be realized by a different device, and the above path planning process may be realized by distributed processing.

[0096] In addition, the route planning process executed by the CPU by reading the software (program) in each of the above embodiments may be executed by various processors other than the CPU. In this case, examples of the processor include a PLD (Programmable Logic Device) such as an FPGA (Field-Programmable Gate Array) whose circuit configuration can be changed after manufacture, and a dedicated electric circuit such as an ASIC (Application Specific Integrated Circuit) which is a processor having a circuit configuration designed exclusively for executing a specific process. In addition, the route planning process may be executed by one of these various processors, or may be executed by a combination of two or more processors of the same or different types (for example, a plurality of FPGAs, and a combination of a CPU and an FPGA). In addition, the hardware structure of these various processors is, more specifically, an electric circuit that combines circuit elements such as semiconductor elements.

[0097] In addition, in each of the above embodiments, the route planning program is described as being pre-stored (installed) in the storage device, but the present invention is not limited to this. The program may be provided in a form stored in a storage medium such as a CD-ROM, a DVD-ROM, a Blu-ray disc, or a USB memory. The program may also be downloaded from an external device via a network.

[0098] The following are additional notes regarding this disclosure.

[0099] (Additional note 1) A path planning device (10) for planning a motion path of a robot (16) from a start point to an end point in a configuration space of the robot, comprising: an initial movement path acquisition unit (22) that sets a movement path assumption area within the configuration space and acquires an initial movement path that is an initial movement path from the start point to the end point via a plurality of element paths within the movement path assumption area; a movement path correction unit (24) for deleting the element path where the interference occurs when any of the element paths included in the initial movement path interferes with an obstacle, and for defining, among the movement paths divided by deleting the element paths, one of the movement path including the start point and the movement path including the end point as a first end path, and the other movement path as a second end path, and for defining, when there are one or more movement paths not including either the start point or the end point, the movement path as an independent movement path; a first process of randomly selecting any one of the via points included in the first end route as a first specific via point; a second process of randomly setting a new via point at a position within the route assumed area, adding a component route connecting the first specific via point and the new via point to the first end route, deleting the added component route and returning to the first process if the added component route interferes with the obstacle, and maintaining the added component route if there is no interference; a third process of determining a via point that is closest to the new via point among the via points included in the second end route as a second specific via point; a fourth process of selecting any one of the via points included in the first end path as a first target via point, adding one or more element paths connected in series to connect the second specific via point and the first target via point, terminating generation of the motion path if any of the added element paths connects the second specific via point and the first target via point without interfering with the obstacle, and deleting at least the interfering portion if any of the added element paths has a portion interfering with the obstacle; If the independent motion path does not exist, the process returns to the first process. If the independent motion path does exist, the process returns to the first process. If the independent motion path does exist, the process selects a via point that is closest to the new via point as a third specific via point and selects the independent motion path to which the via point belongs as a target via point. a sixth process of selecting any one of the way points included in the first end path as a second target way point, adding one or more element paths connected in series for connecting the third specific way point and the second target way point, returning to the first process if any of the added element paths connects the third specific way point and the second target way point without interfering with the obstacle, and, if any of the added element paths has a portion interfering with the obstacle, deleting at least the interfering portion and returning to the first process; A movement path generating unit (26) that generates a movement path by executing A path planning device comprising:

[0100] (Additional note 2) The path planning device according to supplementary item 1, wherein the movement path correction unit (24) is capable of selecting either a movement path including the start point or a movement path including the end point to be the first end path.

[0101] (Additional note 3) The path planning device according to claim 1 or 2, wherein in the fourth process, the operation path generation unit (26) selects, from among the via points included in the first end path, a via point closest to the new via point or the second specific via point as the first target via point.

[0102] (Additional note 4) The path planning device according to any one of Supplementary Items 1 to 3, wherein, in the fourth process, when adding a plurality of serially connected element paths for connecting the second specific via point and the first target via point, the movement path generation unit (26) sets one of the second specific via point or the first target via point as an origin via point and the other as a destination via point, and repeats a process of adding an element path of a first new route length in a direction from the origin via point to the destination via point until a remaining distance to the destination via point becomes equal to or less than the first new route length, and if the added element path interferes with the obstacle in the repetition of the adding process, deletes the added element path and terminates the repetition, and if the remaining distance is equal to or less than the first new route length, adds an element path connecting an end point of the already added element path and the destination via point, and terminates generation of the movement path if the added element path does not interfere with the obstacle, and deletes the added element path if it interferes.

[0103] (Additional note 5) The path planning device according to any one of Supplementary Items 1 to 4, wherein, in the fourth process, when adding one element path connecting the second specific via point and the first target via point, the movement path generation unit (26) sets interference judgment points at predetermined intervals on the one element path, and repeatedly judges whether or not there is interference with the obstacle, starting from the interference judgment point closest to the second specific via point or the first target via point, and if there is no interference, ends generation of the movement path, and if there is interference, sets the interference judgment point immediately before the interference judgment point where interference occurs as the terminal point, and deletes the portion of the one element path from the terminal point to the first target via point or the second specific via point.

[0104] (Additional note 6) The path planning device according to any one of Supplementary Items 1 to 5, wherein in the sixth process, the operation path generation unit (26) selects, from among the via points included in the first end path, a via point closest to the new via point or the third specific via point as the second target via point.

[0105] (Additional note 7) The path planning device according to any one of Supplementary Items 1 to 6, wherein, in the sixth processing step, when adding a plurality of serially connected element paths for connecting the third specific via point and the second target via point, the operation path generation unit (26) sets one of the third specific via point or the second target via point as an origin via point and the other as a destination via point, and repeats a processing step of adding an element path having a length of a second new route length in a direction from the origin via point to the destination via point until a remaining distance to the destination via point becomes equal to or less than the second new route length, and if the added element path interferes with the obstacle in the repetition of the adding processing, deletes the added element path and ends the repetition, and if the remaining distance is equal to or less than the second new route length, adds an element path connecting an end point of the already added element path and the destination via point, and if the added element path interferes with the obstacle, returns to the first processing step, and if it interferes, deletes the added element path and returns to the first processing step.

[0106] (Additional note 8) The path planning device according to any one of Supplementary Items 1 to 6, wherein, in the sixth processing step, when adding one element path connecting the third specific via point and the second target via point, the movement path generation unit (26) sets interference judgment points at predetermined intervals on the one element path, and repeatedly judges whether or not there is interference with the obstacle, starting from the interference judgment point closest to the third specific via point or the second target via point, and if there is no interference, sets the independent movement path to which the third specific via point belongs as a first end path and returns to the first processing step, and if there is interference, sets the interference judgment point immediately before the interference judgment point where interference occurs as the terminal end, deletes the portion of the one element path from the terminal end to the second target via point or the third specific via point, and returns to the first processing step.

[0107] (Additional note 9) The element path is a single path connecting two end points, one of which has an end point attribute of a parent point and the other of which has an end point attribute of a child point; the initial movement path acquisition unit (222) sets the end point attribute for each end point of an element path included in the initial movement path so that an end point of one of two element paths having respective via points included in the initial movement path as end points becomes a parent point and an end point of the other element path becomes a child point; The motion path generating unit (226) in the second process, the fourth process, and the sixth process, if an overlap of parent points occurs at one via point when an element path is added, the parent point and child point of the element path other than the added element path are swapped, and the parent point and child point of the element path opposite the overlap of the parent point newly generated by the swap are swapped, and this process is repeated until the overlap of parent points is eliminated; Further, after the generation of the motion paths is completed in the fourth process, a through motion path identification unit (228) is included which identifies, as through motion paths, a plurality of element paths connected from the start point or the end point, which is a parent point, to the start point or the end point, which is a child point, such that a child point of one element path becomes a parent point of the other element path. A route planning device (210) according to any one of supplementary items 1 to 8.

[0108] (Additional note 10) The motion path generating unit (226) In the second process, the added element path has the first specific via point as a child point, In the fourth process, the added element path has the second specific via point side as a child point, In the sixth process, the added element path has the third specific via point as a child point. 10. The route planning device according to claim 9.

[0109] (Additional note 11) The motion path generating unit (226) In the second process, the added element path has the first specific via point as a parent point, In the fourth process, the added element path has the second specific via point as a parent point, In the sixth process, the added element path has the third specific via point as a parent point. In the fourth process, when the second specific via point and the first target via point are connected, a parent point and a child point of an element path having either the start point or the end point as a parent point are swapped, and further, a process of swapping a parent point and a child point of an element path of a partner with which a parent point newly generated by the swapping overlaps is repeated until the parent point overlaps. 10. The route planning device according to claim 9.

[0110] (Additional note 12) The element path is composed of two end points and a single path connecting the two end points, one of the two end points having an end point attribute of a parent point and the other end point having an end point attribute of a child point, the initial movement path acquisition unit (222) sets the end point attribute for each end point of an element path included in the initial movement path so that an end point of one of two element paths having respective via points included in the initial movement path as end points becomes a parent point and an end point of the other element path becomes a child point; the motion path correction unit (24) repeats a process of exchanging a parent point and a child point of an element path having the start point or the end point having an end point attribute of a parent point with a parent point, and exchanging a parent point and a child point of a partner element path with which the parent point newly generated by the exchanging overlaps, until the overlap of parent points is eliminated; The motion path generating unit (226) In the second process, the added element path has the first specific via point as a child point, In the fourth process, the added element path has the extension source via point side as a child point, In the fourth process, when the generation of the motion path is completed, the via point where the parent point overlaps is set as a connection via point; In the sixth process, the added element path has the third specific via point as a parent point, In the sixth process, if an overlap of parent points occurs when an element path is added, the parent point and child point of the element path other than the added element path are swapped, and further, the process of swapping the parent point and child point of the other element path with which the newly generated parent point overlaps is repeated until the overlap of parent points is eliminated; Further, after the generation of the motion paths is completed in the fourth process, a through motion path identification unit (228) is included which identifies, as a through motion path, a plurality of element paths connected from the connection via point which is a parent point to the start point and the end point which are child points, and linked such that a child point of one element path becomes a parent point of the other element path. 5. The path planning device according to claim 4. [Explanation of symbols]

[0111] 100, 200 Robot control system 10, 210 Path planning device 12 Robot Controller 16. Robot 22, 222 Initial operation path acquisition unit 24 Motion path correction unit 26, 226 Motion path generation unit 228 Penetration movement path identification unit 32 CPU 34 Memory 36 Storage device 38 Input Devices 40 Output Device 42 Storage media reader 44 Communication I / F 46 Bus

Claims

1. A path planning device that plans a motion path of a robot from a start point to an end point in a configuration space of the robot, An initial movement path acquisition unit that sets a path assumption area within the configuration space and acquires an initial movement path that is an initial movement path from the start point to the end point via a plurality of element paths within the path assumption area; a movement path correction unit which, when interference occurs between any of the element paths included in the initial movement path and an obstacle, deletes the element path where interference occurs, and among the movement paths divided by deleting the element paths, sets one of the movement path including the start point and the movement path including the end point as a first end path, sets the other movement path as a second end path, and, when there are one or more movement paths not including either the start point or the end point, sets the movement path as an independent movement path; a first process of randomly selecting any one of the via points included in the first end route as a first specific via point; a second process of randomly setting a new via point at a position within the route assumed area, adding a component route connecting the first specific via point and the new via point to the first end route, deleting the added component route and returning to the first process if the added component route interferes with the obstacle, and maintaining the added component route if there is no interference; a third process of determining a via point that is closest to the new via point among the via points included in the second end route as a second specific via point; a fourth process of selecting any one of the via points included in the first end path as a first target via point, adding one or more element paths connected in series to connect the second specific via point and the first target via point, terminating generation of the motion path if any of the added element paths connects the second specific via point and the first target via point without interfering with the obstacle, and deleting at least the interfering portion if any of the added element paths has a portion interfering with the obstacle; A fifth process is performed by returning to the first process if the independent motion path does not exist, and by setting the via point closest to the new via point as a third specific via point among all via points included in the independent motion paths that exist if the independent motion path does not exist, and setting the independent motion path to which the via point belongs as a target path; and a sixth process of selecting any one of the way points included in the first end route as a second target way point, adding one or more element routes connected in series for connecting the third specific way point and the second target way point, returning to the first process if any of the added element routes connects the third specific way point and the second target way point without interfering with the obstacle, and deleting at least the interfering part if the added element route has an interfering part with the obstacle, and then returning to the first process; A movement path generating unit that generates a movement path by executing A path planning device comprising:

2. The path planning device according to claim 1 , wherein the movement path correction unit is capable of selecting, as the first end path, either a movement path including the start point or a movement path including the end point.

3. The path planning device according to claim 1 or claim 2, wherein in the fourth process, the operation path generation unit selects, as the first target via point, one of the via points included in the first end path that is closest to the new via point or the second specific via point.

4. 3. The path planning device according to claim 1, wherein, in the fourth process, when adding a plurality of serially connected element paths for connecting the second specific via point and the first target via point, either the second specific via point or the first target via point is set as an origin via point and the other is set as a destination via point, and the operation path generation unit repeats a process of adding an element path of a length of a first new route length in the direction from the origin via point to the destination via point until the remaining distance to the destination via point becomes equal to or less than the first new route length, and in the repetition of the adding process, if the added element path interferes with the obstacle, the added element path is deleted and the repetition is terminated, and if the remaining distance is equal to or less than the first new route length, an element path connecting an end point of the already added element path and the destination via point is added, and if the added element path does not interfere with the obstacle, generation of the operation path is terminated, and if it interferes, the added element path is deleted.

5. 3. The path planning device according to claim 1, wherein, when adding one element path connecting the second specific via point and the first target via point in the fourth process, the movement path generation unit sets interference judgment points at predetermined intervals on the one element path, and repeatedly judges whether or not there will be interference with the obstacle, starting from the interference judgment point closest to the second specific via point or the first target via point, and if there is no interference, terminates generation of the movement path, and if there is interference, sets the interference judgment point immediately before the interference judgment point where interference occurs as the terminal point, and deletes the portion of the one element path from the terminal point to the first target via point or the second specific via point.

6. The path planning device according to claim 1 or claim 2, wherein in the sixth process, the operation path generation unit selects, as the second target via point, one of the via points included in the first end path that is closest to the new via point or the third specific via point.

7. 3. The path planning device according to claim 1 or 2, wherein, in the sixth process, when adding a plurality of serially connected element paths for connecting the third specific via point and the second target via point, either the third specific via point or the second target via point is set as an origin via point and the other is set as a destination via point, and the process of adding an element path of a length of a second new route length in the direction from the origin via point to the destination via point is repeated until the remaining distance to the destination via point becomes equal to or less than the second new route length, and in the repetition of the adding process, if the added element path interferes with the obstacle, the added element path is deleted and the repetition is terminated, and if the remaining distance is equal to or less than the second new route length, an element path connecting an end point of the already added element path and the destination via point is added, and if the added element path interferes with the obstacle, the process returns to the first process, and if there is interference, the added element path is deleted and the repetition is terminated.

8. 3. The path planning device according to claim 1, wherein, when adding one element path connecting the third specific via point and the second target via point in the sixth process, the movement path generation unit sets interference judgment points at predetermined intervals on the one element path, and repeatedly judges whether or not there is interference with the obstacle, starting from the interference judgment point closest to the third specific via point or the second target via point, and if there is no interference, the independent movement path to which the third specific via point belongs is set as a first end path and the process returns to the first process, and if there is interference, the interference judgment point immediately before the interference judgment point where interference occurs is set as an end point, and a portion from the end point of the one element path to the second target via point or the third specific via point is deleted, and the process returns to the first process.

9. The element path is a single path connecting two end points, one of which has an end point attribute of a parent point and the other of which has an end point attribute of a child point; the initial movement path acquisition unit sets the end point attribute for each end point of an element path included in the initial movement path such that an end point of one of two element paths having respective via points included in the initial movement path as end points is a parent point and an end point of the other element path is a child point; the operation path generation unit, when adding an element path in the second process, the fourth process, and the sixth process, if an overlap of parent points occurs at one via point, replaces the parent point and child point of the element path other than the added element path, and further repeats a process of replacing the parent point and child point of the element path opposite the overlap of the parent point newly generated by the replacement until the overlap of parent points is eliminated; Further, after the generation of the motion paths is completed in the fourth process, a through motion path identification unit is included which identifies, as through motion paths, a plurality of element paths connected from the start point or the end point, which is a parent point, to the start point or the end point, which is a child point, such that a child point of one element path becomes a parent point of the other element path. The route planning device according to claim 1 or 2.

10. The movement path generating unit In the second process, the added element path has the first specific via point as a child point, In the fourth process, the added element path has the second specific via point side as a child point, In the sixth process, the added element path has the third specific via point side as a child point.

10. The path planning device according to claim 9.

11. The movement path generating unit In the second process, the added element path has the first specific via point as a parent point, In the fourth process, the added element path has the second specific via point as a parent point, In the sixth process, the added element path has the third specific via point as a parent point, In the fourth process, when the second specific via point and the first target via point are connected, a parent point and a child point of an element path having either the start point or the end point as a parent point are swapped, and further, a process of swapping a parent point and a child point of an element path of a partner with which a parent point newly generated by the swapping overlaps is repeated until the parent point overlaps.

10. The path planning device according to claim 9.

12. The element path is composed of two end points and a single path connecting the two end points, one of the two end points having an end point attribute of a parent point and the other end point having an end point attribute of a child point, the initial movement path acquisition unit sets the end point attribute for each end point of an element path included in the initial movement path such that an end point of one of two element paths having respective via points included in the initial movement path as end points is a parent point and an end point of the other element path is a child point; the motion path correction unit repeats a process of exchanging a parent point and a child point of an element path having the start point or the end point having an end point attribute of a parent point with a parent point, and exchanging a parent point and a child point of a partner element path with which a parent point newly generated by the exchanging overlaps, until the overlap of parent points is eliminated; The movement path generating unit In the second process, the added element path has the first specific via point as a child point, In the fourth process, the added element path has the extension source via point side as a child point, In the fourth process, when the generation of the movement path is completed, the via point where the parent point overlaps is set as a connection via point; In the sixth process, the added element path has the third specific via point as a parent point, In the sixth process, if an overlap of parent points occurs when an element path is added, the parent point and child point of the element path other than the added element path are swapped, and further, the process of swapping the parent point and child point of the other element path with which the newly generated parent point overlaps is repeated until the overlap of parent points is eliminated; and a through motion path identification unit that identifies, after the generation of the motion paths is completed in the fourth process, a plurality of element paths connected from the connection via point, which is a parent point, to the start point and the end point, which are child points, such that a child point of one element path becomes a parent point of the other element path, as through motion paths. The path planning device according to claim 4 .

13. A path planning method executed by a path planning device that plans a motion path of a robot from a start point to an end point in a configuration space of the robot, comprising: An initial movement path acquisition unit sets a movement path assumption area in the configuration space, and acquires an initial movement path that is an initial movement path from the start point to the end point via a plurality of element paths in the movement path assumption area; a movement path correction unit, when interference occurs between any of the element paths included in the initial movement path and an obstacle, deletes the element path where interference occurs, and among the movement paths divided by deleting the element paths, defines one of the movement path including the start point and the movement path including the end point as a first end path and the other movement path as a second end path, and when there are one or more movement paths that do not include either the start point or the end point, defines the movement path as an independent movement path; The motion path generating unit a first process of randomly selecting any one of the via points included in the first end route as a first specific via point; a second process of randomly setting a new via point at a position within the route assumed area, adding a component route connecting the first specific via point and the new via point to the first end route, deleting the added component route and returning to the first process if the added component route interferes with the obstacle, and maintaining the added component route if there is no interference; a third process of determining a via point that is closest to the new via point among the via points included in the second end route as a second specific via point; a fourth process of selecting any one of the via points included in the first end path as a first target via point, adding one or more element paths connected in series to connect the second specific via point and the first target via point, terminating generation of the motion path if any of the added element paths connects the second specific via point and the first target via point without interfering with the obstacle, and deleting at least the interfering portion if any of the added element paths has a portion interfering with the obstacle; A fifth process is performed by returning to the first process if the independent motion path does not exist, and by setting the via point closest to the new via point as a third specific via point among all via points included in the independent motion paths that exist if the independent motion path does not exist, and setting the independent motion path to which the via point belongs as a target path; and a sixth process of selecting any one of the way points included in the first end route as a second target way point, adding one or more element routes connected in series for connecting the third specific way point and the second target way point, returning to the first process if any of the added element routes connects the third specific way point and the second target way point without interfering with the obstacle, and deleting at least the interfering part if the added element route has an interfering part with the obstacle, and then returning to the first process; Generate a motion path by executing Route planning methods.

14. Computer, A path planning device that plans a motion path of a robot from a start point to an end point in a configuration space of the robot, An initial movement path acquisition unit that sets a path assumption area within the configuration space and acquires an initial movement path that is an initial movement path from the start point to the end point via a plurality of element paths within the path assumption area; a movement path correction unit which, when interference occurs between any of the element paths included in the initial movement path and an obstacle, deletes the element path where interference occurs, and among the movement paths divided by deleting the element paths, sets one of the movement path including the start point and the movement path including the end point as a first end path, sets the other movement path as a second end path, and, when there are one or more movement paths not including either the start point or the end point, sets the movement path as an independent movement path; and a first process of randomly selecting any one of the via points included in the first end route as a first specific via point; a second process of randomly setting a new via point at a position within the route assumed area, adding a component route connecting the first specific via point and the new via point to the first end route, deleting the added component route and returning to the first process if the added component route interferes with the obstacle, and maintaining the added component route if there is no interference; a third process of determining a via point that is closest to the new via point among the via points included in the second end route as a second specific via point; a fourth process of selecting any one of the via points included in the first end path as a first target via point, adding one or more element paths connected in series to connect the second specific via point and the first target via point, terminating generation of the motion path if any of the added element paths connects the second specific via point and the first target via point without interfering with the obstacle, and deleting at least the interfering portion if any of the added element paths has a portion interfering with the obstacle; A fifth process is performed by returning to the first process if the independent motion path does not exist, and by setting the via point closest to the new via point as a third specific via point among all via points included in the independent motion paths that exist if the independent motion path does not exist, and setting the independent motion path to which the via point belongs as a target path; and a sixth process of selecting any one of the way points included in the first end route as a second target way point, adding one or more element routes connected in series for connecting the third specific way point and the second target way point, returning to the first process if any of the added element routes connects the third specific way point and the second target way point without interfering with the obstacle, and deleting at least the interfering part if the added element route has an interfering part with the obstacle, and then returning to the first process; A movement path generating unit that generates a movement path by executing A route planning program to function as a

Images