Path planning device, method, and program

Abstract

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 that does not interfere with the obstacle; adds 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 that does not interfere with the obstacle; and adds 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 that does not interfere with the obstacle.

Description

Path planning device, method, and program

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

[0002] Conventionally, when planning a motion path for a specific position of a robot, 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) in which the angles of each joint of the arm are used as elements.

[0003] For example, a robot simulation device has been proposed that automatically generates a realistic interference-avoidance path for a robot regardless of the operator's level of skill. This device executes a simulation of a robot's motion program to obtain a first motion path, detects interference on the first motion path, and identifies first and second teaching points, which are teaching points before and after 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 search distance determined by random numbers, thereby generating a second motion path that does not cause interference. This device then evaluates each of the second motion paths based on at least one predetermined parameter and selects an optimal motion path from multiple second motion paths (see Patent Document 1).

[0004] Japanese Patent Application Laid-Open No. 2015-160277

[0005] The robot simulation device described in Patent Document 1 generates a movement path by reconnecting the teaching points before and after the point of interference with an obstacle on the first movement path, which is the initial movement path, i.e., 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 routes that do not cause interference. Also, if the endpoints of a divided route are surrounded by obstacles, making it a so-called dead end, route generation may fail.

[0007] The present disclosure has been made in consideration of the above points, and aims to execute route planning with high efficiency and a high success rate.

[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 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 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 where the interference occurred, and, of 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, 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, and randomly sets a new via point at a position within the path expectation area and sets a element path connecting the first specified via point and the new via point as 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 selecting a via point included in the second end path that is closest to the new via point as a second specific via point; a third process of selecting any via point 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 a fourth process in which, if the second specific via point and the first target via point are connected, the generation of the movement path is terminated, and if there is a part of the added component path that interferes with the obstacle, at least the interfering part is deleted; if the independent movement path does not exist, the process returns to the first process; if there is, the process takes the via point closest to the new via point among the via points included in all the existing independent movement paths as a third specific via point and the independent movement path to which that via point belongs as a target path; and a fifth process in which any via point included in the first end path is selected as a second target via point.a movement path generating unit that generates a movement path by executing a sixth process that adds one or more serially connected element paths for connecting the third specific way point and the second target way point, and returns 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 part of the added element path interferes with the obstacle, deletes at least the interfering part and then returns to the first process.

[0009] a movement path correction unit, when interference occurs between any of the movement paths included in the initial movement path and an obstacle, deletes the movement path where the interference occurs, and, of the movement paths divided by deleting the movement path, sets 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, 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, when there are one or more movement paths that do not include either the start point or the end point, sets a movement path included in the first end path as a first specified 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 selecting one of the way points included in the second end path that is closest to the new way point as a second specific way point; a third process of selecting any of the way points included in the first end path as a first target way point, and adding one or more element paths connected in series to connect the second specific way point and the first target way point; a fourth process of terminating the generation of a movement 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 there is a portion of the added component path that interferes with the obstacle; a fifth process of returning to the first process if the independent movement path does not exist, and if there is a via point included in all the existing independent movement paths, selecting the via point closest to the new via point as a third specific via point and setting the independent movement path to which the via point belongs as a target path; and selecting any via point included in the first end path as a second target via point.A method for generating a motion path by executing a sixth process in which one or more serially connected element paths are added to connect the third specific via point and the second target via point, and if any of the added element paths connect the third specific via point and the second target via point without interfering with the obstacle, the method returns to the first process, and if any part of the added element path interferes with the obstacle, the method deletes at least the interfering part and then 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 robot movement path from a start point to an end point in a configuration space of the robot, the program including: an initial movement path acquisition unit that sets a path expectation 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 expectation area; a movement path correction unit that, when interference between any of the element paths included in the initial movement path and an obstacle occurs, deletes the element path where interference has occurred, and, of the movement paths divided by deleting the element path, 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; a first process that randomly selects any of the via points included in the first end path as a first specified via point; a new via point that is randomly set at a position within the path expectation area, and sets an element path connecting the first specified via point and the new via point as an independent movement path; a second process of adding an 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 selecting one of the way points included in the second end path that is closest to the new way point as a second specific way point; a third process of selecting any of the way points included in the first end path as a first target way point, and adding one or more element paths connected in series to connect the second specific way point and the first target way point, and if any of the added element paths interfere with the obstacle, a fourth process of terminating the generation of a movement 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 there is a portion of the added component path that interferes with the obstacle; a fifth process of returning to the first process if the independent movement path does not exist, and if there is, designating the via point closest to the new via point among the via points included in all the existing independent movement paths as a third specific via point and designating the independent movement path to which the via point belongs as a target path; and selecting any via point 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 then returns to the first process.

[0011] According to the route planning device, method, and program disclosed herein, route planning can be performed with high efficiency and a high success rate.

[0012] 1 is a schematic configuration diagram of a robot control system. FIG. 2 is a block diagram showing the hardware configuration of a path planning device. FIG. 3 is a block diagram showing an example of the functional configuration of the path planning device according to the first embodiment. FIG. 4 is a diagram showing correction of an initial movement path in the first embodiment. FIG. 5 is a diagram showing first and second processes in the first embodiment. FIG. 6 is a diagram showing setting of a new via point using index points. FIG. 7 is a diagram showing a congested area of ​​via points. FIG. 8 is a diagram showing a dead end caused by an obstacle. FIG. 9 is a diagram showing third and fourth processes in the first embodiment. FIG. 10 is a diagram showing fourth process in the first embodiment. FIG. 11 is a diagram showing fifth and sixth processes in the first embodiment. FIG. 12 is a diagram showing first to fourth processes in the first embodiment. A flowchart showing an example of path planning processing. A flowchart showing an example of path planning processing. A flowchart showing an example of path planning processing. A diagram showing endpoint attributes and rules. A block diagram showing an example of the functional configuration of a path planning device according to the second embodiment. A diagram showing acquisition of an initial movement path in the second embodiment. A diagram showing first and second processes in the second embodiment. A diagram showing fourth process in the second embodiment. A diagram showing sixth process in the second embodiment. A diagram showing replacement of parent points and child points in the sixth process. FIG. 1 is a diagram for explaining the specification of a through-movement path. FIG. 2 is a diagram for explaining the output of a through-movement path. FIG. 3 is a diagram for explaining the output of a through-movement path. FIG. 4 is a flowchart showing an example of a replacement process. FIG. 5 is a diagram for explaining the generation of a movement path in modified example 1. FIG. 6 is a diagram for explaining the generation of a movement path in modified example 1. FIG. 7 is a diagram for explaining the correction of an initial movement path in modified example 2. FIG. 8 is a diagram for explaining the replacement of parent points and child points in modified example 2. FIG. 9 is a diagram for explaining the specification of a through-movement path in modified example 2.

[0013] An example of an embodiment of the present disclosure will be described below with reference to the drawings. Note that the same or equivalent components and parts in each drawing are given the same reference numerals. Also, the dimensions and proportions of the drawings are exaggerated for the convenience of explanation and may differ from the actual proportions.

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

[0015] The robot 16 is, for example, a vertically articulated robot having six degrees of freedom necessary for movement in 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 movement 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 a movement path in a configuration space that avoids interference between the robot 16 and obstacles when the robot 16 is moved along the movement path, and outputs the movement path to the robot controller 12.

[0018] In this embodiment, the movement path is a time sequence of the postures of the robot 16 when a predetermined position of the hand 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 the predetermined position of the hand of the robot 16 is at a certain position (x, y, z) and posture (roll, pitch, yaw). Ｊ１ , θ Ｊ２ , ..., θ ＪＮ )

[0019] Fig. 2 is a block diagram showing the hardware configuration of the route planning device 10 according to this embodiment. As shown in Fig. 2, the route planning device 10 includes a CPU (Central Processing Unit) 32, a memory 34, a storage device 36, an input device 38, an output device 40, a storage medium reader 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] The storage device 36 stores a route planning program for executing route planning processing. The CPU 32 is a central processing unit that 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 work area. The CPU 32 controls each component and performs various arithmetic processing in accordance with the program stored in the storage device 36.

[0021] The memory 34 is configured with a random access memory (RAM) and serves as a working area to temporarily store programs and data. The storage device 36 is configured with a read-only memory (ROM), a hard disk drive (HDD), a solid state drive (SSD), etc., and stores various programs including the operating system and various data.

[0022] The input device 38 is a device for performing various inputs, such as a keyboard or a mouse. The output device 40 is a device for outputting various information, such as a display or a printer. 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 CD (Compact Disc)-ROM, DVD (Digital Versatile Disc)-ROM, Blu-ray Disc, 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 the 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 when the CPU 32 reads out a path planning program stored in the storage device 36, expands it in the memory 34, and executes 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, which starts from a start point, passes through one or more waypoints, and reaches an end point in the configuration space of the robot 16. Specifically, the initial movement path acquisition unit 22 sets an assumed path area within the configuration space, which is an area in which the movement path is planned. The assumed path area may be, for example, the range of motion of the robot 16, which is determined by the range of motion of each joint of the robot 16. The initial movement path acquisition unit 22 may acquire, as the initial movement path, 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.

[0028] The initial motion path is a motion path that starts from a start point, passes through multiple element paths, and reaches an end point. An example of the initial motion path is shown in the upper diagram of Figure 4. In Figure 4, the black circle indicates the start point q ｓｔａｒｔ , via point, and end point q ｇｏａｌ The lines connecting the points represent the element paths. Note that an element path is a single path connecting two points. That is, the starting point q ｓｔａｒｔ , via point, and end point q ｇｏａｌ constitute 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 the 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 determines 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 robot joint angle information from a target point within the path prediction area for which interference detection is desired. The movement path correction unit 24 determines whether or not interference will occur within the simulator based on the acquired joint angle information, information about the shape of the arm of the robot 16, and information about surrounding obstacles, using a method such as AABB (Axis Aligned Bounding Box). The placement of each part of the robot 16 in the simulation is determined sequentially from the base using forward kinematics, taking into account 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 indicated by dashed lines represent the element paths to be deleted.

[0031] In addition, the movement path correction unit 24 corrects the start point q ｓｔａｒｔ and an end point q ｇｏａｌ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. ｓｔａｒｔ and an end point q ｇｏａｌ It may be possible to select which of the divided movement paths including the divided movement path (i.e., the first end path) should be the first end path. For example, the movement path correction unit 24 may make the selection based on a predetermined rule, such as selecting the divided movement path to be selected as the first end path alternately each time there is an opportunity for selection. Furthermore, for example, the movement path correction unit 24 may receive a selection specification from a user via a user interface and select the specified divided movement path as the first end path.

[0032] In the lower diagram of FIG. 4, the starting point q ｓｔａｒｔ The divided motion path including the first end path, the end point q ｇｏａｌ In this example, the divided movement path including the start point q is set as the second end path. ｓｔａｒｔ and end point q ｇｏａｌ 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 assumption 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 from among the via points included in the divided movement paths, and that do not interfere with obstacles.

[0034] Specifically, the movement path generator 26 executes a first process of randomly selecting one of the via points included in the first end path as the first specific via point. Note that "random" here also 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 generator 26 also executes a second process of adding an element path to the first end path. As shown in FIG. 5 , the second process adds a new via point q to the first end path, which is a position in the path prediction area and is the first new path length away from the first specific via point in a direction randomly selected. ｎｅｗ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. ｉｎｄｅｘ Furthermore, the movement path generating unit 26 sets the index point q ｉｎｄｅｘ Then, the movement path generating unit 26 selects the closest way point to the index point q as the first specific way point. ｉｎｄｅｘ A new via point q is located at a distance of the 0th new route length L0 in the direction of ｎｅｗ The 0th new route length L0 may be a fixed value or may be set to the new route point q ｎｅｗ It is also possible to allow fluctuations, such as determining the value randomly each time it is set.

[0036] The second process is a process of calculating the first specific route point and the new route point q ｎｅｗ The second process includes adding a component path connecting the first end path and the second end path to the first end path. If the added component path interferes with the obstacle, the process returns to the first process, and if there is no interference, the process maintains the added component path.

[0037] Here, consider a case where a congested area of ​​via points (the area indicated by the dashed line in FIG. 7) exists in the first end path as shown in FIG. 7, and this congested area corresponds to the location of a dead end caused by an obstacle as shown in FIG. 8. In this case, it is difficult for a component 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 a component path that can avoid 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. ｎｅｗ Then, the first specific route point is selected and the element route is added, so that the above-mentioned problems can be easily avoided.

[0038] Furthermore, the movement path generating unit 26 determines a new via point q from among the via points included in the second end path. ｎｅｗA third process is executed to select the route point closest to the first route as the 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. ｎｅｗ Alternatively, the route point added as the first specific route point or the route point closest to the second specific route point may be selected as the first target route point.

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

[0041] A specific example of adding multiple component paths that connect a second specific via point and a first destination via point in series in the fourth process will be described below. The movement path generation unit 26 sets either the second specific via point or the first destination via point as the source via point and the other as the destination via point. Figure 9 shows an example in which the second specific via point is the source via point and the first destination via point is the destination via point. The movement path generation unit 26 repeats the process of adding component paths with a first new path length L1 in the direction from the source 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 component path with length L1 interferes with an obstacle during the repetition of the process of adding component paths, the movement path generation unit 26 deletes the added component path and ends the repetition. Furthermore, if the remaining distance is equal to or less than the first new path length L1, the movement path generation unit 26 adds a component path connecting the end point of the added component path and the extension destination via point, and if the added component path does not interfere with an obstacle, ends the generation of the movement path. On the other hand, if the added component path interferes with an obstacle, the movement path generation unit 26 deletes the added component path.

[0042] 9 illustrates the case where a component path is added from the second specific way point side, but the component path may be added from the first target way point side or from both sides. "Adding" a component path includes "attempting to add" a component path, which is when an attempt is made to add the component path but the addition is canceled because it is determined that the component path will interfere with an obstacle. "Deleting" a component path includes "cancelling the addition" a component path, which is when an attempt is made to add the component path but the addition is canceled because it is determined that the component path will interfere with an obstacle.

[0043] A specific example of adding one component path connecting the second specific via point and the first destination via point in the fourth process will be described. As shown in the upper diagram of FIG. 10 , the movement path generation unit 26 adds a single component path having the second specific via point and the first destination via point as its end points, and sets collision detection points at predetermined intervals on the single component path. The movement path generation unit 26 repeatedly determines whether or not there will be interference with an obstacle, starting with the collision detection point closest to the second specific via point or the first destination via point. The example shown in the lower diagram of FIG. 10 illustrates an example in which the determination is made starting with the collision detection point on the second specific via point side. If the movement path generation unit 26 determines that there will be no interference for all collision detection points, it terminates the generation of the movement path. On the other hand, if it determines that there will be interference, the movement path generation unit 26 sets the collision detection point immediately preceding the collision detection point where interference occurs as the end point, as shown in the lower diagram of FIG. 10 , and deletes the portion of the component path from the end point to the first destination via point or the second specific via point.

[0044] If there are no independent movement paths, the movement path generating unit 26 returns to the first process. If there are independent movement paths, the movement path generating unit 26 selects a new via point q from among the via points included in all the independent movement paths. ｎｅｗ A fifth process is executed to select the route point closest to the first route as the third specific route point.

[0045] Furthermore, the movement path generating unit 26 executes a sixth process of setting an 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 of the via points included in the first end path as the 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. ｎｅｗ Alternatively, the route point added as the second destination route point or the route point closest to the third specific route point may be selected as the second destination route point.

[0046] The sixth process also includes adding one or more serially connected component paths for connecting the third specific via point and the second destination 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 destination via point are connected without any of the added component paths interfering with an obstacle. The sixth process also includes deleting at least the interfering portion and then returning to the first process if the added component path has a portion that interferes with an obstacle. The specific process of adding one or more serially connected component paths for connecting the third specific via point and the second destination via point is the same as the specific process of adding a component path for connecting the second specific via point and the first destination 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 movement 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 ｎｅｗ 12, the movement path generation 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 generation unit 26 generates a movement path by repeatedly executing the first to sixth processes described above.

[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 the path planning process executed by the CPU 32 of the path planning device 10. The CPU 32 reads the path planning program from the storage device 36, expands it into the memory 34, and executes it, causing the CPU 32 to function 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 within 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 in which interference occurs exists, the process proceeds to step S16; 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 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. Next, in step S18, the movement path correction unit 24 divides the initial movement path into a plurality of divided movement paths by deleting element paths that interfere with obstacles from among the divided movement paths. ｓｔａｒｔ and an end point q ｇｏａｌ The movement path correction unit 24 then selects which of the divided movement paths including the first movement path and the second movement path to be the first end path, the independent movement path, and the second end path.

[0051] Next, in step S20, the movement path generating unit 26 randomly selects any 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 generates a new via point q at a position within the path envisaged area, the position being a 0th new path length L0 away from the first specific via point in a direction selected at random. ｎｅｗ Then, the movement path generating unit 26 sets the first specific via point and the new via point q ｎｅｗ An element path connecting the two is added to the first end path.

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

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

[0054] Next, in step S32, the motion path generating unit 26 determines 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 generation unit 26 sets either the second specific via point or the first target via point as the source via point and the other as the destination via point, and adds a component path of length L1 from the source via point to the destination via point. Next, in step S36, the movement path generation unit 26 determines whether the added component path will interfere with an obstacle. If it will interfere, the process proceeds to step S42, and if it will not interfere, the process returns to step S28.

[0056] On the other hand, in step S38, the movement path generation unit 26 adds a component path that connects the endpoint of the already added component path with the extension destination via point. Next, in step S40, the movement path generation unit 26 determines whether the added component path interferes with an obstacle. If there is interference, the process proceeds to step S42, and if there is no interference, the path planning process ends.

[0057] In step S42, the movement path generation unit 26 deletes the element paths added in step S34 or S38. Next, in step S44, the movement path generation unit 26 determines 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.ｎｅｗ Next, in step S48, the movement path generating unit 26 selects the closest via point to the new via point q from among the via points included in the first end path. ｎｅｗ the route added as the second target route, or the route closest to the third specific route, is selected as the second target route.

[0059] Next, in step S50, the motion path generating unit 26 determines 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 generation unit 26 sets either the third specific via point or the second target via point as the source via point and the other as the destination via point, and adds a component path of length L2 from the source via point to the destination via point. Next, in step S54, the movement path generation unit 26 determines whether the added component path will interfere with an obstacle. If it will interfere, the process proceeds to step S60, and if it will not interfere, the process returns to step S46.

[0061] On the other hand, in step S56, the movement path generation unit 26 adds a component path that connects the end point of the already added component path with the extension destination via point. Next, in step S58, the movement path generation unit 26 determines whether the added component path interferes with an obstacle. If it does interfere, 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 generation unit 26 deletes the element path added in step S52 or S56, and the process returns to step S18 (FIG. 13). On the other hand, in step S62, the movement path generation unit 26 sets the first end portion path to which the target path has been added as a new first end portion path, and the process returns to step S18 (FIG. 13).

[0063] The path planning device 10 outputs the movement path generated by executing the path planning process to the robot controller 12. The robot controller 12 controls the movement of the robot 16 in accordance with 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 robot movement path in the robot's configuration space, from a start point to an end point via one or more via points. The path planning device acquires an initial movement path, which is an initial movement path from a start point to an end point via multiple component paths within a path expectation area set in the configuration space. The path planning device also divides the initial movement path into multiple divided movement paths by deleting component paths that interfere with obstacles from among the component paths included in the initial movement path. The path planning device then randomly sets new via points within the path expectation area. For each of the multiple divided movement paths, the path planning device generates a movement path by adding one or more component paths that connect a via point selected corresponding to the new via point from among the via points included in the divided movement path and the new via point, and that do not interfere with obstacles.

[0065] In this way, in this embodiment, when removing the interfering portion of the initial movement path, the calculation cost of the path planning can be reduced by leaving the non-interfering area and reducing the distance that needs to be reconnected. In other words, high efficiency can be achieved. Furthermore, in this embodiment, instead of reconnecting the endpoints 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 with a new way point. This prevents the path reconnection from being completed indefinitely, which would result in the generation of the movement path failing, even if the endpoint of the divided movement path is in a dead end, thereby increasing the success rate of the path planning.

[0066] Second Embodiment Next, a second embodiment will be described. 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 are denoted by the same reference numerals, and detailed descriptions thereof 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. ｓｔａｒｔ From the end point q ｇｏａｌ We propose a method for easily identifying a penetrating motion path, which is a motion path that penetrates all the way to the target.

[0069] In the second embodiment, an element path that makes up a movement path has two endpoints, one of which has the attribute of a parent point and the other of which has the attribute of a child point, as shown in the upper diagram of Figure 16. Furthermore, in the second embodiment, as shown in the lower diagram of Figure 16, a rule is set that multiple parent points must not overlap at one via point. This is because, when identifying a through movement path by tracing from a parent point to a child point, if parent points overlap at one via point, the movement path cannot be uniquely identified. However, multiple child points are permitted to overlap at one via point. As will be described in detail later, if a violation of the above rule occurs, a process is executed to swap the parent point and child point of the element path in question.

[0070] As shown in FIG. 17, the path planning device 210 includes, as functional components, an initial movement path acquisition unit 222, a movement path correction unit 24, a movement path generation unit 226, and a through movement path identification unit 228.

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

[0072] The movement path generating unit 226 generates a movement path by executing the first to sixth processes, similar to the movement path generating unit 26 of the first embodiment. At this time, as shown in FIG. 19, the movement path generating unit 226 generates a movement path by performing the first to sixth processes. ｎｅｗ 20, the movement path generation unit 226 sets the element path for connecting the second specific via point and the first destination via point so that the second specific via point side is a child point. Similarly, as shown in Fig. 21, the movement path generation unit 226 sets the element path for connecting the third specific via point and the second destination via point so that the third specific via point side is a child point.

[0073] Furthermore, in the second, fourth, and sixth processes, if parent point overlap occurs when an element path is added, the movement path generation unit 226 swaps the parent point and child point of the element path that is not the added element path. Furthermore, the movement path generation unit 226 repeats the process of swapping the parent point and child point of the element path that is the other side of the parent point overlap that newly occurs as a result of the swap, until the parent point overlap is eliminated.

[0074] A specific description will be given using the sixth process shown in FIG. 21 as an example. As shown in FIG. 21, when an element path is added to connect the second destination via point and the third specific via point, parent point overlap occurs at the second destination via point. In this case, the movement path generation unit 226 swaps the parent point and 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 swapped. Therefore, the movement path generation unit 226 swaps the parent point and 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 swapping parent points and child points 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 overlapping parent points in the second process, the fourth process, and the sixth process, as described above. As a result, a movement path such as that 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 specifying unit 228 determines the starting point q ｓｔａｒｔ or end point q ｇｏａｌ From the child point, the starting point q ｓｔａｒｔ or end point q ｇｏａｌ 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 another element path are identified as through-motion paths. In the example in the lower diagram of Figure 23, the linked element paths indicated by double lines are identified as through-motion paths.

[0077] The through-motion path specifying unit 228 outputs information on the specified through-motion path. For example, suppose 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 assigned a start point q corresponding to the parent point and child point of the element path. ｓｔａｒｔ , the number of the via point, and the end point q ｇｏａｌ25, each row corresponds to one element path. For example, the through-movement path specifying unit 228 may use [q ｓｔａｒｔ , (1), (2), (3), (4), (5), (6), (7), (8), q ｇｏａｌ ] may be output as the through motion path.

[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 Figures 13 to 15 is executed. In the second embodiment, after steps S22, S34, S38, S52, and S56, the replacement process shown in Figure 26 is executed.

[0080] In step S70, the motion path generation unit 226 determines 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 generation unit 226 swaps the parent point and child point of the element path of the other side of the added element path. Next, in step S74, the movement path generation unit 226 determines whether or not a new parent point overlap has occurred as a result of the swap in step S72. If a parent point overlap has occurred, the process proceeds to step S76.

[0082] In step S76, the movement path generating unit 226 exchanges the parent point and child point of the element path opposite to the element path previously exchanged, 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] Furthermore, in the path planning process according to 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 movement path using element paths having end point attributes of parent and child points so that the parent points do not overlap. The path planning device then identifies, as through movement paths, a plurality of element paths connected from a start point or an end point (parent point) to a start point or an end point (child point) in the generated movement path, where the child point of one element path is linked to the parent point of the other element path. This makes it possible to easily identify through movement paths even when movement paths with complex branching are generated.

[0085] <Modification 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 Modification 1, the movement path generation unit sets the added element path to the first specific via point as a parent point in the second process, sets the added element path to the second specific via point side as a parent point in the fourth process, and sets the added element path to the third specific via point side as a parent point 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 ｎｅｗ An element path is added whose child point is

[0000] . 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 Figure 27. Also, as in 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 destination via point are connected, the motion path generation unit according to the first modification swaps 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 repeats the process of swapping the parent point and child point of the other element path with which the parent point newly generated by the swap overlaps, until the parent point overlaps. Figure 28 shows the case where the parent point and child point of the start point are swapped, and the case where the parent point and child point of the end point are swapped. In the lower diagram of Figure 28, the element path where the parent point and child point have been swapped is shown by a thick line.

[0088] <Variant 2> In variant 2, the swapping of parent points and child points always targets the independent operation path side, and when the first end path and the second end path are connected, the swapping of parent points and child points is not performed, i.e., the case where parent point overlap is not eliminated is described.

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

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

[0091] Furthermore, in the sixth process, if overlapping of parent points occurs when an element path is added as shown in the upper diagram of Fig. 30, the movement path generation unit according to the second modification swaps the parent point and child point of the element path that is not the added element path as shown in the lower diagram of Fig. 30, and further repeats the process of swapping the parent point and child point of the other element path that overlaps with the parent point newly generated by the swapping until there are no more overlapping parent points. As a result, in the second modification, swapping of parent points and child points of element paths is executed on the independent movement path side.

[0092] Furthermore, when the movement path generation unit according to the second modification example finishes generating the movement path in the fourth process, it sets the via point where the parent point overlaps 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 specifying unit according to the second modification specifies, 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, ｓｔａｒｔ The element path chained from the connection via point to the end point q is shown with a double solid line. ｇｏａｌ 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 interchange of parent points and child points is limited to the independent motion path side as in Modification 2, the number of times the parent point and child point interchange process is performed is reduced compared to the second embodiment and Modification 1, and the calculation cost can often be reduced. Furthermore, in Modification 2, by specifying through motion paths from the connection via point to each of the start point and the end point, the interchange of parent points and child points at the time when the generation of the motion path is completed can be omitted, thereby reducing the calculation cost.

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

[0096] Furthermore, the path planning process executed by the CPU in each of the above embodiments by reading software (programs) may be executed by various processors other than the CPU. Examples of processors in this case include programmable logic devices (PLDs) (such as field-programmable gate arrays (FPGAs)) whose circuit configuration can be changed after manufacture, and dedicated electrical circuits, such as application-specific integrated circuits (ASICs), which are processors having a circuit configuration specifically designed to execute specific processes. The path planning process may be executed by one of these various processors, or by a combination of two or more processors of the same or different types (e.g., multiple FPGAs, or a combination of a CPU and an FPGA). The hardware structure of these various processors is, more specifically, an electrical 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 a storage device, but this is not limiting. The program may be provided in a form stored in a storage medium such as a CD-ROM, DVD-ROM, Blu-ray Disc, or 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] (Supplementary Item 1) A path planning device (10) for planning a movement 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) for setting a path expectation area in the configuration space and acquiring 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 (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 setting one of the movement path including the start point and the movement path including the end point as a first end path, the other movement path as a second end path, and for setting one or more movement paths that do not include either the start point or the end point as an independent movement path when there are any one or more movement paths; and a first process for randomly selecting any of the via points included in the first end path as a first specific via point. a second process of randomly setting a new via point at a position within the route prediction area, adding a component route connecting the first specified via point and the new via point to the first end route, deleting the added component route if the added component route interferes with the obstacle and returning to the first process, and maintaining the added component route if there is no interference; a third process of setting the via point closest to the new via point among the via points included in the second end route as a second specified via point; a fourth process of selecting any of the via points included in the first end route as a first target via point, adding one or more serially connected component routes for connecting the second specified via point and the first target via point, terminating generation of the operation route if the second specified via point and the first target via point are connected without any of the added component routes interfering with the obstacle, and deleting at least the interfering portion if there is a portion of the added component route that interferes with the obstacle; If the independent movement path does not exist, the process returns to the first process, and if the independent movement path does exist, the process selects the via point closest to the new via point as a third specific via point and sets the independent movement path to which the via point belongs as a target path; anda movement path generation unit (26) that generates a movement path by executing a sixth process in which: a sixth process is performed in which one of the way points included in the first end path is selected as a second target way point; one or more serially connected element paths are added to connect the third specific way point and the second target way point; if any of the added element paths connect the third specific way point and the second target way point without interfering with the obstacle, the process returns to the first process; and if any of the added element paths has a part that interferes with the obstacle, the process deletes at least the interfering part and then returns to the first process.

[0100] (Supplementary Item 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 as the first end path.

[0101] (Supplementary Item 3) The path planning device according to Supplementary Item 1 or Supplementary Item 2, wherein in the fourth process, the operation path generation unit (26) 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.

[0102] (Supplementary Item 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 specified via point and the first target via point, the movement path generation unit (26) sets one of the second specified 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 element paths 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 during 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 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 the added element path interferes.

[0103] (Supplementary Item 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 detection points at predetermined intervals on the one element path, and repeatedly determines whether or not there will be interference with the obstacle, starting from the interference detection 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 detection point immediately preceding the interference detection point at which interference occurs as the end point, and deletes the portion of the one element path from the end point to the first target via point or the second specific via point.

[0104] (Supplementary Item 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, 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.

[0105] (Supplementary Item 7) The path planning device according to any one of Supplementary Items 1 to 6, wherein the motion path generation unit (26), in the sixth process, when adding a plurality of serially connected element paths for connecting the third specified via point and the second target via point, sets one of the third specified via point or the second target via point as an origin via point and the other as a destination via point, and repeats a process of adding element paths of a length of a second new route length 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 second new route length, and if the added element path interferes with the obstacle during 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 second new route length, adds an element path connecting an end point of the added element path and the destination via point, and if the added element path interferes with the obstacle, returns to the first process, and if there is interference, deletes the added element path and returns to the first process.

[0106] (Supplementary Item 8) The path planning device according to any one of Supplementary Items 1 to 6, wherein, when adding one element path connecting the third specific via point and the second target via point in the sixth processing, the movement path generation unit (26) sets interference detection points at predetermined intervals on the one element path, and repeatedly determines whether or not there will be interference with the obstacle, starting from the interference detection 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, and if there is interference, sets the interference detection point immediately before the interference detection 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.

[0107] (Supplementary Item 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 the element path included in the initial movement path so that, of two element paths having respective via points included in the initial movement path as end points, the end point of one of the element paths becomes a parent point and the end point of the other element path becomes a child point, the movement path generation unit (226) 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, swaps the parent point and child point of the element path other than the added element path, and further repeats the process of swapping the parent point and child point of the element path opposite the overlap of parent points newly generated by the swap until there are no more overlapping parent points, The path planning device (210) according to any one of Supplementary Items 1 to 8 further includes a through movement path identification unit (228) that, after generation of the movement path is completed in the fourth process, identifies, as through movement 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, and linked such that a child point of one element path becomes a parent point of the other element path.

[0108] (Supplementary Item 10) The path planning device according to Supplementary Item 9, wherein the movement path generation unit (226): in the second process, the added element path has the first specified via point as a child point; in the fourth process, the added element path has the second specified via point as a child point; and in the sixth process, the added element path has the third specified via point as a child point.

[0109] (Supplementary Item 11) The path planning device described in Supplementary Item 9, wherein the operation path generation unit (226): in the second processing, the added element path has the first specified via point as a parent point; in the fourth processing, the added element path has the second specified via point as a parent point; in the sixth processing, the added element path has the third specified via point as a parent point; in the fourth processing, when the second specified via point and the first target via point are connected, swaps the parent point and child point of the element path having either the start point or the end point as a parent point, and further repeats the process of swapping the parent point and child point of the other element path with which the parent point newly generated by the swap overlaps, until there are no more overlapping parent points.

[0110] (Additional Item 12) The element path is composed of two end points and a single path connecting the 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 the element path included in the initial movement path so that, of two element paths having respective via points included in the initial movement path as end points, the end point of one of the element paths becomes a parent point and the end point of the other element path becomes a child point, the movement path correction unit (24) swaps the parent point and child point of an element path having the start point or the end point as an end point which has an end point attribute of a parent point, and repeats the process of swapping the parent point and child point of the other element path with which the parent point newly generated by the swapping overlaps, until there is no more overlap of parent points, the movement path generation unit (226) the route planning device according to appended clause 4, further comprising: in the fourth processing, the added element route has the extension source via point side as a child point; in the fourth processing, when generation of the motion route is completed, the via point where overlapping of parent points occurs is set as a connecting via point; in the sixth processing, the added element route has the third specified via point side as a parent point; in the sixth processing, when overlapping of parent points occurs when adding an element route, the parent point and child point of the element route other than the added element route are swapped, and further, the process of swapping the parent point and child point of the other element route with which the parent point newly generated by the swapping overlaps is repeated until the overlapping of parent points is eliminated; and further, after generation of the motion route is completed in the fourth processing, the device includes a through operation route identification unit (228) that identifies, as through operation routes, a plurality of element routes connected from the connecting via point which is a parent point to the start point and the end point which are child points, and which are linked so that a child point of one element route becomes a parent point of the other element route.

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

Claims

1. 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, comprising: an initial motion path acquisition unit for setting a path expectation area within the configuration space, and acquiring an initial motion path which is an initial motion path from the start point to the end point via a plurality of element paths within the path expectation area; a motion path correction unit for deleting the element path where interference occurs 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 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 when there are any one or more motion paths not including either the start point or the end point; a first process for randomly selecting any of the via points included in the first end path as a first specific via point; a second process of randomly setting a new via point at a position within the path assumption area, adding an element path connecting the first specific via point and the new via point to the first end path, deleting the added element path and returning to the first process if the added element path interferes with the obstacle, and maintaining the added element path if there is no interference; a third process of setting 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 fourth process of selecting any of the via points included in the first end path as a first target via point, adding one or a plurality of element paths connected in series to connect the second specific via point and the first target via point, terminating generation of the operation path if the second specific via point and the first target via point are connected without any of the added element paths interfering with the obstacle, and deleting at least the interfering portion if there is a portion of the added element path that interferes 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; anda movement path generating unit that generates a movement path by executing a sixth process: selecting any of the way points included in the first end path as a second target way point, adding one or more serially connected element paths for connecting the third specific way point and the second target way point, and if any of the added element paths connect the third specific way point and the second target way point without interfering with the obstacle, returning to the first process, 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.

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

3. A route planning device as described in claim 1 or claim 2, wherein in the fourth process, the operation route generation unit selects, from among the route points included in the first end route, the route point closest to the new route point or the second specific route point as the first target route point.

4. The path planning device of claim 1 or 2, wherein, in the fourth process, when adding a plurality of serially connected element paths to connect the second specific via point and the first target via point, the operation path generation unit 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 repeats the 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 if the added element path interferes with the obstacle during 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 operation path if the added element path does not interfere with the obstacle, and deletes the added element path if it does interfere.

5. A path planning device as described in claim 1 or claim 2, 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 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. A path planning device as described in claim 1 or claim 2, wherein in the sixth process, the operation path generation unit selects, from among the via points included in the first end path, the via point closest to the new via point or the third specific via point as the second target via point.

7. 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, the operation path generation unit 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 process of adding an element path of a length of the second 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 second new route length, and if the added element path interferes with the obstacle during 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 second new route length, adds an element path connecting an end point of the added element path and the destination via point, and if the added element path interferes with the obstacle, returns to the first process, and if it does interfere, deletes the added element path and returns to the first process.

8. A path planning device as described in claim 1 or claim 2, wherein, when the movement path generation unit adds one element path connecting the third specific via point and the second target via point in the sixth process, it 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, it sets the independent movement path to which the third specific via point belongs as a first end path and returns to the first process, and if there is interference, it sets the interference judgment point immediately before the interference judgment point where interference occurs as the terminal, deletes the portion from the terminal of the one element path to the second target via point or the third specific via point, and 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 has an end point attribute of a child point, the initial movement path acquisition unit sets the end point attributes for each end point of the element path included in the initial movement path so that, of the two element paths having end points at respective via points included in the initial movement path, the end point of one of the element paths is a parent point and the end point of the other element path is a child point, the movement 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, swaps the parent point and child point of the element path other than the added element path, and further repeats the process of swapping the parent point and child point of the element path opposite the overlap of parent points newly generated by the swap until the overlap of parent points is eliminated, The path planning device according to claim 1 or claim 2, further comprising a through movement path identification unit that, after generation of the movement path is completed in the fourth process, identifies as a through movement path a plurality of element paths connected from the starting point or the ending point, which is a parent point, to the starting point or the ending point, which is a child point, and which are linked such that a child point of one element path becomes a parent point of the other element path.

10. The path planning device of claim 9, wherein the operation path generation unit, in the second process, sets the added element path to the first specific via point as a child point, in the fourth process, sets the added element path to the second specific via point as a child point, and in the sixth process, sets the added element path to the third specific via point as a child point.

11. The path planning device of claim 9, wherein the operation path generation unit: in the second processing, the added element path has the first specified via point as a parent point; in the fourth processing, the added element path has the second specified via point as a parent point; in the sixth processing, the added element path has the third specified via point as a parent point; and when the second specified via point and the first target via point are connected in the fourth processing, swaps the parent point and child point of the element path having either the start point or the end point as a parent point, and further repeats the process of swapping the parent point and child point of the other element path with which the newly generated parent point overlaps, until there are no more overlapping parent points.

12. The element path is composed of two end points and a single path connecting the two end points, one of which has an end point attribute of a parent point and the other 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 the element path included in the initial movement path such that, of the two element paths having end points of each via point included in the initial movement path, the end point of one of the element paths is a parent point and the end point of the other element path is a child point, the movement path modification unit swaps the parent point and child point of an element path having an end point of the start point or the end point having an end point attribute of a parent point, and repeats the process of swapping the parent point and child point of the other element path with which the parent point newly generated by the swapping overlaps, until there is no more overlapping parent points, the movement path generation unit: in the second process, the added element path has the first specified via point as a child point, and in the fourth process, the added element path has the extension source via point side as a child point, 5. The path planning device according to claim 4, further comprising: when generation of the movement path is completed in the fourth process, a via point where a parent point overlap occurs is set as a connecting via point; in the sixth process, the added element path sets the third specified via point as a parent point; when an overlap of parent points occurs when an element path is added in the sixth process, a parent point and a child point of the element path other than the added element path are swapped, and further a process of swapping a parent point and a child point of the other element path with which the newly generated parent point overlaps as a result of the swapping is repeated until the parent point overlaps, and further comprising: a through movement path identification unit that, after generation of the movement path is completed in the fourth process, identifies as through movement paths a plurality of element paths connected from the connecting via point which is a parent point to the start point and the end point which are child points, and which are linked so that a child point of one element path becomes a parent point of the other element path.

13. A path planning method executed by 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, comprising: 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, of the movement paths divided by deleting the element paths, 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 that performs a first process of randomly selecting any via point included in the first end path as a first specific via point; a second process of randomly setting a new via point at a position within the path assumption area, adding an element path connecting the first specific via point and the new via point to the first end path, deleting the added element path and returning to the first process if the added element path interferes with the obstacle, and maintaining the added element path if there is no interference; a third process of setting 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 fourth process of selecting any of the via points included in the first end path as a first target via point, adding one or a plurality of element paths connected in series to connect the second specific via point and the first target via point, terminating generation of the operation path if the second specific via point and the first target via point are connected without any of the added element paths interfering with the obstacle, and deleting at least the interfering portion if there is a portion of the added element path that interferes 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; anda 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 serially connected element paths for connecting the third specific way point and the second target way point, and 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 then returning to the first process, thereby generating a motion path.

14. 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, comprising: an initial motion path acquisition unit for setting a path expectation area in the configuration space, and acquiring an initial motion path which is an initial motion path from the start point to the end point via a plurality of element paths within the path expectation area; a motion path correction unit for deleting the 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 the other motion path as a second end path, and for setting the motion path as an independent motion path when there are one or more motion paths not including either the start point or the end point; and a first process for randomly selecting any of the via points included in the first end path as a first specific via point. a second process of randomly setting a new via point at a position within the path assumption area, adding an element path connecting the first specific via point and the new via point to the first end path, deleting the added element path and returning to the first process if the added element path interferes with the obstacle, and maintaining the added element path if there is no interference; a third process of setting 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 fourth process of selecting any of the via points included in the first end path as a first target via point, adding one or a plurality of element paths connected in series to connect the second specific via point and the first target via point, terminating generation of the operation path if the second specific via point and the first target via point are connected without any of the added element paths interfering with the obstacle, and deleting at least the interfering portion if there is a portion of the added element path that interferes 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; anda sixth process of selecting any one of the via points included in the first end path as a second target via point, adding one or more element paths connected in series to connect the third specific via point and the second target via point, and if any of the added element paths connect the third specific via point and the second target via point without interfering with the obstacle, returning to the first process, 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.

Images