Trajectory generation system, trajectory generation method, and non-transitory storage medium

Abstract

A trajectory generation system includes one or more processors configured to: continuously generate a first trajectory candidate and a second trajectory candidate in parallel, the second trajectory candidate being generated at a lower generation rate than a generation rate of the first trajectory candidate and being more likely to be passable by a moving body than the first trajectory candidate; and generate a trajectory using either or both of a generated first trajectory candidate that is passible by the moving body and the generated second trajectory candidate.

What is claimed is: 
     
         1 . A trajectory generation system comprising one or more processors configured to:
 set a start position and a target position of a plurality of first trajectory candidates and a second trajectory candidate;   start a generation of the plurality of first trajectory candidates and the second trajectory candidate from the start position to the target position simultaneously;   determine whether a moving body is able to pass through at least one of the plurality of first trajectory candidates while avoiding an interfering object, when a first predetermined period is passed after the start of the generation of the plurality of first trajectory candidates and the second trajectory candidate;   generate a trajectory using the one of the plurality of first trajectory candidates which is determined that the moving body is able to pass through in a case where the one or more processors determine that the moving body is able to pass through the at least one of the plurality of first trajectory candidates while avoiding the interfering object;   determine whether the second trajectory candidate is generated in a case where the one or more processors determine that the moving body is not able to pass through the at least one of the plurality of first trajectory candidates while avoiding the interfering object; and   generate the trajectory using the second trajectory candidate in a case where the one or more processors determine that the second trajectory candidate is generated, wherein the one or more processors are configured to
 periodically connect a plurality of microtrajectories to the start position of the plurality of first trajectory candidates or an immediately preceding microtrajectory at every second predetermined period and generate the plurality of the first trajectory candidates by sequentially connecting the plurality of the microtrajectories from the start position to the target position, 
 the plurality of the microtrajectories is a part along which the moving body or a grip unit of the moving body moves in the predetermined period, and is a part on the start position side of the plurality of the first trajectory candidates, and 
 generate the second trajectory candidate using a rapidly-exploring random tree method. 
   
     
     
         2 . The trajectory generation system according to  claim 1 , wherein a third period from setting the start position and the target position to generating the trajectory is 100 msec. 
     
     
         3 . The trajectory generation system according to  claim 1 , wherein a three dimensional interference determination at points in the plurality of first trajectory candidates is performed in a case where the one or more processors determine whether the moving body is able to pass through the at least one of the plurality of first trajectory candidates while avoiding the interfering object. 
     
     
         4 . A trajectory generation method that is performed by a trajectory generation system, the trajectory generation method comprising:
 setting a start position and a target position of a plurality of first trajectory candidates and a second trajectory candidate;   starting a generation of the plurality of first trajectory candidates and the second trajectory candidate from the start position to the target position simultaneously;   determining whether a moving body is able to pass through at least one of the plurality of first trajectory candidates while avoiding an interfering object, when a first predetermined period is passed after the start of the generation of the plurality of first trajectory candidates and the second trajectory candidate;   generating a trajectory using the one of the plurality of first trajectory candidates which is determined that the moving body is able to pass through in a case where the moving body is determined to be able to pass through the at least one of the plurality of first trajectory candidates while avoiding the interfering object;   determining whether the second trajectory candidate is generated in a case where the moving body is determined to not able to pass through the at least one of the plurality of first trajectory candidates while avoiding the interfering object; and   generating the trajectory using the second trajectory candidate in a case where the second trajectory candidate is determined to be generated, wherein:
 a plurality of microtrajectories are periodically connected to the start position of the plurality of first trajectory candidates or an immediately preceding microtrajectory at every second predetermined period and the plurality of the first trajectory candidates are generated by sequentially connecting the plurality of the microtrajectories from the start position to the target position, 
 the plurality of the microtrajectories is a part along which the moving body or a grip unit of the moving body moves in the predetermined period, and is a part on the start position side of the plurality of the first trajectory candidates, and 
 the second trajectory candidate is generated using a rapidly-exploring random tree method. 
   
     
     
         5 . A non-transitory storage medium storing instructions that are executable by one or more processors and that cause the one or more processors to perform functions comprising:
 setting a start position and a target position of a plurality of first trajectory candidates and a second trajectory candidate;   starting a generation of the plurality of first trajectory candidates and the second trajectory candidate from the start position to the target position simultaneously;   determining whether a moving body is able to pass through at least one of the plurality of first trajectory candidates while avoiding an interfering object, when a first predetermined period is passed after the start of the generation of the plurality of first trajectory candidates and the second trajectory candidate;   generating a trajectory using the one of the plurality of first trajectory candidates which is determined that the moving body is able to pass through in a case where the moving body is determined to be able to pass through the at least one of the plurality of first trajectory candidates while avoiding the interfering object;   determining whether the second trajectory candidate is generated in a case where the moving body is determined to not able to pass through the at least one of the plurality of first trajectory candidates while avoiding the interfering object; and   generating the trajectory using the second trajectory candidate in a case where the second trajectory candidate is determined to be generated, wherein:
 a plurality of microtrajectories are periodically connected to the start position of the plurality of first trajectory candidates or an immediately preceding microtrajectory at every second predetermined period and the plurality of the first trajectory candidates are generated by sequentially connecting the plurality of the microtrajectories from the start position to the target position, 
 the plurality of the microtrajectories is a part along which the moving body or a grip unit of the moving body moves in the predetermined period, and is a part on the start position side of the plurality of the first trajectory candidates, and 
 the second trajectory candidate is generated using a rapidly-exploring random tree method. 
   
     
     
         6 . The trajectory generation method according to  claim 4 , wherein a third period from setting the start position and the target position to generating the trajectory is 100 msec. 
     
     
         7 . The trajectory generation method according to  claim 4 , wherein a three dimensional interference determination at points in the plurality of first trajectory candidates is performed in a case where the moving body is determined to be able to pass through the at least one of the plurality of first trajectory candidates while avoiding the interfering object. 
     
     
         8 . The non-transitory storage medium according to  claim 5 , wherein a third period from setting the start position and the target position to generating the trajectory is 100 msec. 
     
     
         9 . The non-transitory storage medium according to  claim 5 , wherein a three dimensional interference determination at points in the plurality of first trajectory candidates is performed in a case where the moving body is determined to be able to pass through the at least one of the plurality of first trajectory candidates while avoiding the interfering object.