JP7775821B2 - Orbit search device - Google Patents

Description

This disclosure relates to a trajectory search device, and more particularly to a device, method, and program that searches for a robot trajectory from both a start point and a goal point using a rapidly-exploring random tree.

Patent Document 1 uses a Rapidly-exploring Random Tree or a Bidirectional Rapidly-exploring Random Tree to search for and generate a robot's trajectory.

Japanese Patent Application Laid-Open No. 2017-151687

The applicant has discovered the following problem: In bidirectional search, of the trees rooted at the start point and the goal point, the generation of the tree with the higher difficulty (i.e., the number of constraints) does not progress, while only the generation of the tree with the lower difficulty progresses, resulting in a lot of unnecessary calculations.

This disclosure was made in consideration of these problems, and realizes a trajectory search device that can contribute to speeding up trajectory searches.

A trajectory search device according to one aspect of the present disclosure is a device that searches for a trajectory of a robot from both a start point and a goal point using a Rapidly-exploring Random Tree,
a space information acquisition unit that acquires information indicating a state space;
a state information acquisition unit that acquires information indicating the start point and the goal point of the robot in the state space;
a trajectory search unit that searches for a trajectory of the robot from both the start point and the goal point using the Rapidly-exploring Random Tree based on information indicating the state space and information indicating the start point and the goal point of the robot in the state space;
Equipped with
When the difference between the number or percentage of successful node generation in the tree rooted at the start point and the number or percentage of successful node generation in the tree rooted at the goal point is greater than a preset value, the trajectory search unit preferentially extends the tree with a lower number or percentage of successful node generation.

This disclosure makes it possible to realize a trajectory search device that can contribute to speeding up trajectory searches.

FIG. 1 is a flowchart showing a typical example of the flow of searching for a robot trajectory using a general Bidirectional Rapidly-exploring Random Tree. FIG. 10 is a diagram for explaining a specific example of searching for a robot trajectory using a general Bidirectional Rapidly-exploring Random Tree. FIG. 10 is a diagram for explaining a specific example of searching for a robot trajectory using a general Bidirectional Rapidly-exploring Random Tree. FIG. 10 is a diagram for explaining a specific example of searching for a robot trajectory using a general Bidirectional Rapidly-exploring Random Tree. FIG. 10 is a diagram for explaining a specific example of searching for a robot trajectory using a general Bidirectional Rapidly-exploring Random Tree. FIG. 10 is a diagram for explaining a specific example of searching for a robot trajectory using a general Bidirectional Rapidly-exploring Random Tree. FIG. 10 is a diagram for explaining a specific example of searching for a robot trajectory using a general Bidirectional Rapidly-exploring Random Tree. FIG. 10 is a diagram for explaining a specific example of searching for a robot trajectory using a general Bidirectional Rapidly-exploring Random Tree. FIG. 10 is a diagram for explaining a specific example of searching for a robot trajectory using a general Bidirectional Rapidly-exploring Random Tree. FIG. 10 is a diagram for explaining a specific example of searching for a robot trajectory using a general Bidirectional Rapidly-exploring Random Tree. FIG. 10 is a diagram for explaining the difference between a tree with fewer constraints and a tree with more constraints. 1 is a block diagram showing a configuration of a trajectory search device according to a first embodiment; FIG. 2 is a flowchart showing the flow of a trajectory search method according to the first embodiment. FIG. 10 is a block diagram showing the configuration of a trajectory search device according to a second embodiment. 10 is a flowchart showing the flow of a trajectory search method according to a third embodiment. FIG. 11 is a diagram illustrating the verification results of the trajectory search method according to the third embodiment. FIG. 2 is a diagram illustrating an example of a hardware configuration included in the trajectory search device.

Specific embodiments to which the present disclosure is applied are described in detail below, with reference to the drawings. However, the present disclosure is not limited to the following embodiments. Furthermore, the following description and drawings have been simplified as appropriate for clarity of explanation.

<Background to this disclosure>
First, a typical example of the flow of searching for a robot trajectory using a general bidirectional rapidly-exploring random tree will be described. Here, Fig. 1 is a flowchart showing a typical example of the flow of searching for a robot trajectory using a general bidirectional rapidly-exploring random tree.

A typical Bidirectional Rapidly-exploring Random Tree generates a tree in both directions between the start point (start state) and the goal point (goal state) in the state space to search for a robot's trajectory.

In detail, first, information indicating the state space and information indicating the start point and goal point within the state space are obtained (S101). Here, the state space is a space expressed by state variables such as the angle command values for each joint of the robot and the position within the environment. Therefore, a point within the state space uniquely determines the position and posture of the robot within the environment.

Next, a sample point is set in the state space by random sampling, and an attempt is made to generate a target node toward the sample point from either the start point, target node, or connect node closest to the sample point in a tree rooted at the start point, or the goal point, target node, or connect node closest to the sample point in a tree rooted at the goal point, and it is determined whether the target node was successfully generated (S102).

Here, "the start point, target node, or connect node of a tree rooted at the start point" refers to the start point if the target node and connect node have not yet been generated, and "the goal point, target node, or connect node of a tree rooted at the goal point" refers to the goal point if the target node and connect node have not yet been generated.

If the generation of the target node fails (NO in S102), the start point, target node, or connect node of the tree rooted at the start point, or the goal point, target node, or connect node of the tree rooted at the goal point, is swapped (S103), and the process returns to step S102.

If the target node is successfully generated (YES in S102), the tree is extended by connecting the generated target node with a branch to either the start point, target node, or connect node closest to the sample point in the tree rooted at the start point, or the goal point, target node, or connect node closest to the sample point in the tree rooted at the goal point (S104).

At this time, the length of the branch can be set appropriately depending on the range of motion of the robot, etc. Note that from step S104 onwards, if one side and the other side were swapped in step S103, then the two sides will be swapped again.

Next, an attempt is made to generate a connect node from the start point, target node, or connect node closest to the target node generated in S102 in the tree rooted at the start point, or from the other of the goal point, target node, or connect node closest to the target node generated in S102 in the tree rooted at the goal point, toward the generated target node, and it is determined whether the generation of the connect node was successful (S105). In other words, it is determined whether the connect node will come into contact with an obstacle or the like in the state space.

If the generation of the connect node fails (NO in S105), the process returns to step S103. On the other hand, if the generation of the connect node is successful (YES in S105), the tree is extended by connecting the generated connect node to the start point, target node, or connect node closest to the target node generated in S102 in the tree rooted at the start point, or the other of the goal point, target node, or connect node closest to the target node generated in S102 in the tree rooted at the goal point, with a branch (S106).

Next, it is determined whether the generated connect node has reached (contacted) the start point, target node, or connect node of the tree rooted at the start point, or the goal point, target node, or connect node of the tree rooted at the goal point (S107).

If the generated connect node reaches either the start point, target node, or connect node of a tree rooted at the start point, or the goal point, target node, or connect node of a tree rooted at the goal point (YES in S107), the sequence of states connecting the start point to the goal point via the target node or connect node (i.e., the branches of the respective trees) is set as the robot's trajectory (S108).

If the generated connect node has not reached either the start point, target node, or connect node of the tree rooted at the start point, or the goal point, target node, or connect node of the tree rooted at the goal point (NO in S107), return to step S105.

Next, we will explain a specific example of searching for a robot's trajectory using the above-mentioned process. Figures 2 to 10 are diagrams for explaining a specific example of searching for a robot's trajectory. In Figures 2 to 10, constraints such as obstacles within state space C are marked with the symbol O.

First, as shown in Figure 2, a start point q _init and a goal point q _goal are set in the state space C, and a sample point q _rand1 is set in the state space C by random sampling. Next, an attempt is made to generate a target node q _new1 from the start point q _init toward the sample point q _rand1 . In Figure 2, the target node q _new1 is successfully generated, and the start point q _init and the target node q _new1 are connected by a branch _B1 , extending a tree _T1 rooted at the start point q _init .

Next, an attempt is made to generate a connect node q _connect1 from the goal point q _goal to the target node q _new1 , as shown in Figure 3. In Figure 3, the generation of the connect node q _connect1 is successful, and the goal point q _goal and the connect node q _connect1 are connected by a branch _B2 , extending a tree _T2 rooted at the goal point q _goal .

Next, an attempt is made to generate a new connect node from the connect node q _connect1 to the target node q _new1 . In Figure 3, the generation of the new connect node fails. Then, as shown in Figure 4, a sample point q _rand2 is set in the state space C by random sampling.

Next, as shown in Fig. 4, an attempt is made to generate a target node _qnew2 from the goal _point qgoal that is closest to the sample point _qrand2 among the goal point _qgoal and _connect node qconnect1 of tree _T2 toward sample point _qrand2 . In Fig. 4, the generation of the target node _qnew2 is successful, and the goal point _qgoal and target node _qnew2 are connected by branch _B3 to extend tree _T2 .

Next, as shown in Fig. ₅ , an attempt is made to generate a connect node q connect2 from the start point q _init that is closest to the target node q _new2 among the start point q _init and the target node q _new1 of the tree _T1 toward the target node q _new2 . In Fig. 5, the generation of the connect node q _connect2 is successful, and the start point q _init and the connect node q _connect2 are connected by a branch _B4 , thereby extending the tree _T1 .

Next, an attempt is made to generate a new connect node from the connect node q _connect2 to the target node q _new2 , as shown in Fig. 6. In Fig. 6, the generation of the new connect node fails.

Next, as shown in Fig. 7, a sample point q _rand3 is set in the state space C by random sampling. Then, among the start point q _init , target node q _new1 , and connect node q _connect2 of the tree _T1 , an attempt is made to generate a target node q _new3 from the connect node q _connect2 that is closest to the sample point q _rand3 toward the sample point q _rand3 . In Fig. 7, the target node q _new3 is successfully generated, and the connect node q _connect2 and the target node q _new3 are connected by a branch _B5 , thereby extending the tree _T1 .

Next, as shown in Fig. ₈ , an attempt is made to generate a connect node q _connect3 from the target node q _new2 that is closest to the target node q _new3 among the goal point q _goal , target node q _new2 , and connect node q _connect1 of tree T2 toward the target node q _new3 . In Fig. 8, the generation of connect node q _connect3 is successful, and the target node q _new2 and connect node q _connect3 are connected by branch _B6 , thereby extending tree _T2 .

After that, as shown in Figures 9 and 10, new connect nodes q _connect4 and q _connect5 are successfully generated from connect node q _connect3 toward target node q _new3 , and connect node q _connect3 and connect node q _connect4 are connected by branch B ₇ , and connect node q _connect4 and connect node q _connect5 by branch B ₈ , thereby extending tree T _2. When connect node q _connect5 has reached target node q _new3 , the state sequence _indicated by branches B 4 , B 5 , B 8 , B 7 , B ₆ , and B ₂ of tree T ₁ and tree T 2, which connect start point q init, _{connect node q connect2 ,} target node q _new3 (q _connect5 ), connect node _{q connect4 , connect} node _{q connect3} , target node q _new2 , and goal point _{q goal} , is set as the robot's trajectory.

11 is a diagram illustrating the difference between a tree on the side with fewer constraints and a tree on the side with more constraints. In a typical Bidirectional Rapidly-exploring Random Tree, for example, as shown in FIG. 11 , when there are many constraints O, such as obstacles, around the start point q _init or the goal point q _goal in the state space C, unnecessary node generation is repeated in the tree on the side with fewer constraints O, and the number of nodes in the tree on the side with fewer constraints O may be greater than the number of nodes in the tree on the side with more constraints O. In addition, the success rate of node generation in the tree on the side with fewer constraints O may be higher than the success rate of node generation in the tree on the side with more constraints O.

This disclosure focuses on the number of nodes in a tree and the success rate of node generation, and by suppressing the generation of unnecessary nodes in trees with fewer constraints, it aims to reduce the amount of calculations, thereby realizing a trajectory search device that can contribute to speeding up trajectory searches. Here, "number of nodes" refers to the total number of target nodes and connect nodes that form the tree.

First Embodiment
First, the configuration of the trajectory search device of this embodiment will be described. Fig. 12 is a block diagram showing the configuration of the trajectory search device of this embodiment. As shown in Fig. 12, the trajectory search device 1 includes a space information acquisition unit 2, a state information acquisition unit 3, and a trajectory search unit 4.

The spatial information acquisition unit 2 acquires information indicating the state space based on the robot's range of motion and the space in which the robot operates. The state information acquisition unit 3 acquires information indicating the robot's start point and goal point within the state space. Note that this information indicating the state space and the information indicating the robot's start point and goal point within the state space can be input via an external input device.

The trajectory search unit 4 searches for the robot's trajectory in both directions, between the start point and the goal point, using a rapidly-exploring random tree based on information indicating the state space and information indicating the robot's start point and goal point within that state space.

At this time, as described below, if the difference between the number of nodes in the tree rooted at the start point and the number of nodes in the tree rooted at the goal point is greater than a preset value, the trajectory search unit 4 will prioritize extending the tree with the fewer nodes.

The difference in the number of nodes can be said to be the difference in the number of times nodes were successfully generated. Therefore, if the difference between the number of times nodes were successfully generated in a tree rooted at the start point and the number of times nodes were successfully generated in a tree rooted at the goal point is greater than a preset value, the trajectory search unit 4 can be said to preferentially extend the tree with fewer successful node generation attempts.

Next, the flow of the trajectory search method of this embodiment will be explained. Figure 13 is a flowchart showing the flow of the trajectory search method of this embodiment. First, the spatial information acquisition unit 2 acquires information indicating the state space, and the state information acquisition unit 3 acquires information indicating the start point and goal point within that state space (S1). Note that it is sufficient if this information indicating the state space and the information indicating the start point and goal point within that state space is acquired before executing the following processing.

First, the trajectory search unit 4 sets a sample point in the state space by random sampling, and attempts to generate a target node from either the start point, target node, or connect node closest to the sample point in a tree rooted at the start point, or the goal point, target node, or connect node closest to the sample point in a tree rooted at the goal point, toward the sample point, and determines whether the target node was successfully generated (S2).

Here, as above, "the start point, target node, or connect node of a tree rooted at the start point" refers to the start point if the target node and connect node have not yet been generated, and "the goal point, target node, or connect node of a tree rooted at the goal point" refers to the goal point if the target node and connect node have not yet been generated.

If the generation of the target node fails (NO in S2), the trajectory search unit 4 swaps one side of the start point, target node, or connect node of the tree rooted at the start point, or the goal point, target node, or connect node of the tree rooted at the goal point (S3), and returns to step S2.

If the target node is successfully generated (YES in S2), the trajectory search unit 4 extends the tree by connecting the generated target node with a branch to either the start point, target node, or connect node closest to the sample point in the tree rooted at the start point, or the goal point, target node, or connect node closest to the sample point in the tree rooted at the goal point (S4). Note that from step S4 onwards, if one side and the other side were swapped in step S3, the two sides will be swapped again.

Next, the trajectory search unit 4 attempts to generate a connect node from the start point, target node, or connect node closest to the target node generated in step S2 in the tree rooted at the start point, or from the other of the goal point, target node, or connect node closest to the target node generated in step S2 in the tree rooted at the goal point, toward the generated target node, and determines whether the connect node was successfully generated (S5).

If the connect node is successfully generated (YES in S5), the trajectory search unit 4 extends the tree by connecting the generated connect node to the start point, target node, or connect node closest to the target node generated in step S2 in the tree rooted at the start point, or the other of the goal point, target node, or connect node closest to the target node generated in step S2 in the tree rooted at the goal point, with a branch (S6).

Next, the trajectory search unit 4 determines whether the generated connect node has reached either the start point, target node, or connect node of a tree rooted at the start point, or the goal point, target node, or connect node of a tree rooted at the goal point (S7).

If the generated connect node reaches either the start point, target node, or connect node of a tree rooted at the start point, or the goal point, target node, or connect node of a tree rooted at the goal point (YES in S7), the trajectory search unit 4 determines the sequence of states connecting the start point to the goal point via the target node or connect node as the robot's trajectory (S8).

If the generated connect node has not reached either the start point, target node, or connect node of the tree rooted at the start point, or the goal point, target node, or connect node of the tree rooted at the goal point (NO in S7), the trajectory search unit 4 returns to step S5.

If the generation of a connect node fails (NO in S5), the trajectory search unit 4 determines whether the difference (absolute value of the difference) between the number of nodes in the tree rooted at the start point and the number of nodes in the tree rooted at the goal point is greater than a preset value (S9). Here, the preset value can be set appropriately based on constraints within the state space, etc. In other words, the trajectory search unit 4 determines whether the difference between the number of times nodes were successfully generated in the tree rooted at the start point and the number of times nodes were successfully generated in the tree rooted at the goal point is greater than a preset value.

If the difference between the number of nodes in the tree rooted at the start point and the number of nodes in the tree rooted at the goal point is greater than a preset value (YES in S9), the trajectory search unit 4 determines whether it is necessary to swap one side of the start point, target node, or connect node of the tree rooted at the start point in the above-mentioned process, or the other side of the goal point, target node, or connect node of the tree rooted at the goal point, in order to preferentially extend the tree with the fewer nodes (S10). In other words, the trajectory search unit 4 determines whether it is necessary to swap the tree on the side where the target node is generated with the tree on the side where the connect node is generated.

If switching is necessary (YES in S10), the trajectory search unit 4 returns to step S3. On the other hand, if switching is not necessary (NO in S10), the trajectory search unit 4 returns to step S2. As a result, connect nodes are preferentially generated and connected to the tree with the fewer nodes until the difference between the number of nodes in the tree with the more nodes and the number of nodes in the tree with the fewer nodes falls below a preset value.

In this way, when the difference between the number of nodes in the tree rooted at the start point and the number of nodes in the tree rooted at the goal point is greater than a preset value, the trajectory search device 1 and trajectory search method of this embodiment preferentially extend the tree with fewer nodes, i.e., the tree with more constraints.

As a result, the trajectory search device 1 and trajectory search method of this embodiment can suppress the generation of unnecessary nodes in the tree with fewer constraints compared to a typical Bidirectional Rapidly-exploring Random Tree. This reduces the amount of calculation required to search for a robot's trajectory, contributing to faster trajectory search.

<Second Embodiment>
The trajectory search device according to the first embodiment preferentially extends the tree on the side with a smaller number of nodes, i.e., a smaller number of times that nodes have been successfully generated, whereas the trajectory search device according to the second embodiment preferentially extends the tree on the side with a smaller percentage of nodes that have been successfully generated.

Figure 14 is a block diagram showing the configuration of the trajectory search device of this embodiment. As shown in Figure 14, the trajectory search device 1a includes a spatial information acquisition unit 2, a state information acquisition unit 3, and a trajectory search unit 4a. Note that the spatial information acquisition unit 2 and the state information acquisition unit 3 have the same configuration as those included in the trajectory search device 1 of embodiment 1, and therefore a description thereof will be omitted.

If the difference between the rate at which nodes are successfully generated in a tree rooted at the start point and the rate at which nodes are successfully generated in a tree rooted at the goal point is greater than a preset value, the trajectory search unit 4a prioritizes extending the tree with a lower rate of successful node generation.

The rate at which nodes are successfully created is calculated by dividing the number of times a node was successfully created by the number of attempts to create a node. The rate at which nodes are successfully created is also called the success rate. The number of times a node was successfully created is also called the number of successes. The number of times a node was attempted to create is also called the number of attempts.

The success rate may be calculated based on the number of successes and attempts since tree generation began. In such cases, the number of successes corresponds to the number of nodes in the tree, and the success rate can be calculated based on the number of nodes in the tree. The trajectory search unit 4a is assumed to store the number of attempts from the start to the present in a memory or the like.

The success rate may also be calculated based on the number of successes and attempts within a predetermined period from the present. For example, the period from when a sample point is set in the state space by random sampling until the generation of a target node or connect node fails may be considered one period, and a predetermined number of periods may be considered a predetermined period. If the difficulty of node generation changes as the tree grows, the trajectory search unit 4a can appropriately update the success rate by not using data older than a predetermined time. For example, if a tree grows from the inside to the outside of a space with many constraints, the difficulty of node generation is considered to decrease.

Next, the flow of the trajectory search method of this embodiment will be explained with reference to Figure 13. Note that the processing other than step S9 is the same as the processing of the trajectory search method of embodiment 1, and therefore explanations will be omitted.

If the generation of a connect node fails (NO in S5), the trajectory search unit 4a determines whether the difference (absolute value of the difference) between the rate at which nodes are successfully generated in the tree rooted at the start point (success rate) and the rate at which nodes are successfully generated in the tree rooted at the goal point (success rate) is greater than a preset value (S9). Here, the preset value can be set appropriately based on constraints within the state space, etc.

If the difference between the success rate of the tree rooted at the start point and the success rate of the tree rooted at the goal point is greater than a preset value (YES in S9), the trajectory search unit 4a determines whether or not it is necessary to swap the tree on which the target node is generated with the tree on which the connect node is generated (S10). If swapping is necessary (YES in S10), the trajectory search unit 4a returns to step S3. On the other hand, if swapping is not necessary (NO in S10), the trajectory search unit 4a returns to step S2. As a result, connect nodes are preferentially generated and connected to the tree on the lower success rate until the difference between the success rate of the tree on the higher success rate and the success rate of the tree on the lower success rate falls below the preset value.

In this way, the trajectory search device 1a and trajectory search method of this embodiment prioritize growing the tree with the lower success rate, i.e., the tree with more constraints, when the difference between the success rate of the tree rooted at the start point and the success rate of the tree rooted at the goal point is greater than a preset value.

As a result, the trajectory search device 1a and trajectory search method of this embodiment can achieve the same effects as those of embodiment 1.

<Third Embodiment>
In the first embodiment, if the difference in the number of nodes is greater than a predetermined value, the tree on the side where the target node is generated and the tree on the side where the connect node is generated are swapped. The inventors have discovered that the performance of the trajectory search device according to the first embodiment is degraded under certain constraint conditions.

The trajectory search device according to the first embodiment operates so that the number of nodes in the two trees remains constant. Therefore, if it is difficult to extend one of the trees, it may attempt to extend it endlessly until the number of nodes in that tree increases. As a result, even if it would be easier to search for a trajectory by extending the other tree, a large amount of computational resources will be allocated to the inefficient search. The cause of this behavior is the deterministic behavior of the trajectory search device according to the first embodiment.

In this third embodiment, the tree that will generate the target node is selected based on a probability (called the selection probability). A node (target node) is generated from the selected tree. The selection probability is set so that trees with fewer nodes are more likely to be selected.

Instead of the number of nodes, the ratio described in embodiment 2, i.e., the ratio of successful node generation, may be used. Embodiment 3 can prevent a decrease in the performance of the trajectory search device according to embodiment 2 under specific constraints. The following describes the case where the number of nodes is used.

An example of the flow of the trajectory search method according to the third embodiment will be described with reference to Figure 15. Comparing Figure 13 with Figure 15, step S9 has been replaced with step S11. The processing other than that related to step S11 is the same as the processing in the trajectory search method according to the first embodiment, and therefore will not be described again.

If generation of a target node fails (NO in S2) or if generation of a connect node fails (NO in S5), a random real number x between 0 and 1 is generated, and it is determined whether the random number x is less than a threshold | _TA |/(| _TA |+| _TB |) (S11). | _TA | represents the number of nodes in the tree (also called tree A) on the side where the target node is to be generated, and | _TB | represents the number of nodes in the tree (also called tree B) on the side where the connect node is to be generated.

If the determination result is true (YES in S11), the tree on the side where the target node is generated and the tree on the side where the connect node is generated are swapped (step S3). If the determination result is false (NO in S11), the tree on the side where the target node is generated and the tree on the side where the connect node is generated are not swapped, and the process returns to step S2.

In such a case, the probability p that tree A will be selected again as the tree that generates the target node is expressed as |T _B |/(|T _A |+|T _B |). The probability p that tree A will be selected increases as the number of nodes in tree A decreases.

On the other hand, the probability q = 1-p that tree B will be selected as the tree that generates the target node is expressed as |T _A |/(|T _A |+|T _B |). The probability q that tree B will be selected increases as the number of nodes in tree B decreases.

The tree with the fewer nodes is more likely to be selected as the tree that selects the target node, i.e., the tree that attempts to generate the node first, but there is still room for the tree with the more nodes to be selected. This prevents a decrease in the performance of the trajectory search device under certain constraints.

Next, the verification results will be explained with reference to Figure 16. The inventors set four types of constraints in two-dimensional space and executed the trajectory search method according to the conventional technology, the trajectory search method according to the first embodiment, and the trajectory search method according to the third embodiment 1,000 times each.

The top four diagrams represent four types of constraints. Constraints O1, O2, O3, and O4 are set in state space C. The two points in state space C represent the start point and the goal point.

Below the diagram representing each constraint, the verification results for that constraint are shown. The horizontal axis corresponds to the conventional technology, embodiment 1, and embodiment 3, and the vertical axis corresponds to the time required for trajectory search (referred to as planning time). The unit of planning time is ms. The distribution of planning time is expressed as a so-called box-and-whisker plot. The solid line contained within the box represents the median, and the dotted line contained within the box represents the mean. The bottom of the vertical line represents the 5th percentile, the bottom of the box represents the 25th percentile, the top of the box represents the 75th percentile, and the top of the vertical line represents the 95th percentile.

Under certain constraints (e.g., constraint O4), the planning time when using embodiment 1 increases compared to the planning time when using conventional technology. In this case, it can be seen that using embodiment 3 can suppress the increase in planning time. Furthermore, while the conventional technology and embodiment 1 have their advantages and disadvantages for each problem, it was found that embodiment 3 exhibits performance close to the better of the two.

According to the third embodiment, by introducing probabilistic behavior, it is possible to suppress a decrease in the performance of the trajectory search device according to the first and second embodiments under specific constraint conditions. In other words, according to the third embodiment, it is possible to prevent a decrease in the performance of the trajectory search device caused by the reliable selection of a tree with a small number of nodes, etc.

<Other embodiments>
The trajectory search device according to the above-described embodiments may have the following hardware configuration. Fig. 17 is a diagram showing an example of the hardware configuration included in the trajectory search device. As has been described in the various embodiments above regarding the processing procedures in the trajectory search device, the present disclosure may also be embodied as a trajectory search method.

The device shown in FIG. 17 includes a processor 101 and memory 102, as well as an interface 103. Part of the configuration of the trajectory search device described in the above embodiment is realized by the processor 101 reading and executing a trajectory search program stored in memory 102. In other words, this program causes the processor 101 to function as part of the configuration of the trajectory search device. This program can be said to cause the trajectory search device to execute processing in its configuration or part of it.

Here, a program includes instructions (or software code) that, when loaded into a computer, cause the computer to perform one or more functions. The program may be stored on a non-transitory computer-readable medium or a tangible storage medium. By way of example and not limitation, computer-readable medium or tangible storage medium includes random-access memory (RAM), read-only memory (ROM), flash memory, solid-state drive (SSD) or other memory technology, CD-ROM, digital versatile disc (DVD), Blu-ray (registered trademark) disc or other optical disk storage, magnetic cassette, magnetic tape, magnetic disk storage or other magnetic storage device. The program may also be transmitted on a transitory computer-readable medium or communication medium. By way of example and not limitation, transitory computer-readable medium or communication medium includes electrical, optical, acoustic, or other forms of propagated signals.

Note that this disclosure is not limited to the above-described embodiments and can be modified as appropriate without departing from the spirit of the disclosure.

For example, in the above embodiment, if the difference between the number of nodes in the tree rooted at the start point and the number of nodes in the tree rooted at the goal point is less than a preset value, the tree that generates connect nodes, etc. is replaced each time an attempt to generate connect nodes, etc. fails, just like a typical Bidirectional Rapidly-exploring Random Tree. However, the frequency with which the tree that generates connect nodes, etc. is replaced can be changed as appropriate.

1, 1a Trajectory search device 2 Spatial information acquisition unit 3 State information acquisition unit 4, 4a Trajectory search unit 101 Processor 102 Memory 103 Interfaces _B1 , _B2 , _B3 , _B4 , _B5 , _B6 , _B7 , _B8 Branch C State space O Constraints q _connect1 , q _connect2 , q _connect3 , q _connect4 , q _connect5 Connect nodes q _goal Goal point q _init Start point q _new1 , q _new2 , q _new3 Target nodes q _rand1 , q _rand2 , q _rand3 Sample point _T1 Tree rooted at start point _T2 Tree rooted at goal point

Claims (3)

A device that searches for a robot trajectory from both a start point and a goal point using a Rapidly-exploring Random Tree,
a space information acquisition unit that acquires information indicating a state space;
a state information acquisition unit that acquires information indicating the start point and the goal point of the robot in the state space;
a trajectory search unit that searches for a trajectory of the robot from both the start point and the goal point using the Rapidly-exploring Random Tree based on information indicating the state space and information indicating the start point and the goal point of the robot in the state space;
Equipped with
The trajectory search unit, based on the number or percentage of successful node generation in a tree rooted at the start point and the number or percentage of successful node generation in a tree rooted at the goal point, preferentially extends the tree with a lower number or percentage of successful node generation. when a difference between the number or rate of successful node generation in the tree rooted at the start point and the number or rate of successful node generation in the tree rooted at the goal point is greater than a preset value, the trajectory search unit preferentially extends the tree with a lower number or rate of successful node generation;
The trajectory search device according to claim 1. The trajectory search device according to claim 1 , wherein the trajectory search unit selects a tree based on a probability that a tree with a smaller number or rate of successful node generation is more likely to be selected, and attempts to generate a node from the selected tree.