Path generating device and path generating method

Abstract

The objective of the present invention is to provide technology capable of reducing the calculation load due to the generation of path candidates. This path generating device comprises: a path candidate generating unit that generates path candidates on the basis of target content and constraints; and a target path determining unit that determines a target path from the path candidates on the basis of evaluation values from an evaluation function of the path candidates. The path candidate generating unit generates a new path candidate that can be determined as a new target path on the basis of a portion of non-determined path candidates, which are path candidates other than the path candidate determined as the target path, among the plurality of path candidates generated in the past.

Description

Trajectory generation device and trajectory generation method

[0001] The present disclosure relates to a trajectory generation device and a trajectory generation method.

[0002] Trajectory generation technology has been proposed to enable a mobile body to autonomously navigate to a destination by sensing the surrounding environment of the mobile body and generating a trajectory that avoids collisions with obstacles. Trajectory generation for a mobile body must be performed in real time, taking into account not only the position where collisions with obstacles are avoided, but also the direction and speed of the avoidance.

[0003] For example, Patent Document 1 proposes a technology for generating trajectory candidates that are sequentially developed in a tree structure with respect to control input and state sequences based on a probabilistic motion model of a moving object, constraints, and target contents, and determining a target trajectory from the trajectory candidates based on an evaluation function. According to this technology, by generating trajectory candidates that are sequentially developed in a tree structure, it becomes possible to improve real-time performance and to respond to complex constraints through probabilistic judgment.

[0004] Patent No. 6494872

[0005] However, in an environment where there is an obstacle whose future behavior is uncertain, the obstacle may move onto the target trajectory after the target trajectory is determined. In such a case, if a new trajectory candidate is generated based on the motion model, constraints, and target content so that the mobile object can avoid the obstacle, a large computational load is imposed on the generation of the trajectory candidate.

[0006] Therefore, the present disclosure has been made in consideration of the above-mentioned problems, and aims to provide a technology that can reduce the calculation load caused by generating trajectory candidates.

[0007] A trajectory generation device according to the present disclosure includes a behavior planning unit that generates target contents and constraints for the movement of a moving body, a trajectory candidate generation unit that generates trajectory candidates in a tree structure that unfolds sequentially based on the target contents and the constraints, and a target trajectory determination unit that determines a target trajectory along which the moving body should move from the trajectory candidates based on evaluation values ​​of evaluation functions of the trajectory candidates, and the trajectory candidate generation unit generates new trajectory candidates that can be determined as the new target trajectory based on a portion of non-determined trajectory candidates, which are trajectory candidates other than the trajectory candidates determined as the target trajectory among a plurality of trajectory candidates generated in the past.

[0008] According to the present disclosure, a new trajectory candidate that can be determined as a new target trajectory is generated based on a portion of non-determined trajectory candidates, which are trajectory candidates other than the trajectory candidate determined as the target trajectory among multiple trajectory candidates generated in the past. This configuration can reduce the calculation load caused by generating trajectory candidates. Objects, features, aspects, and advantages of the present disclosure will become more apparent from the following detailed description and the accompanying drawings.

[0009] 1 is a plan view showing a moving body on which a trajectory generation device according to embodiment 1 is mounted. FIG. 2 is a side view showing a moving body on which a trajectory generation device according to embodiment 1 is mounted. FIG. 3 is a diagram showing an example of a map generated by the trajectory generation device according to embodiment 1. FIG. 4 is a block diagram showing the configuration of the trajectory generation device according to embodiment 1. FIG. 5 is a diagram showing a coordinate system used in the trajectory generation device according to embodiment 1. FIG. 6 is a diagram showing the operation of the trajectory generation device according to embodiment 1. FIG. 7 is a diagram showing the operation of a related device. FIG. 8 is a diagram showing the operation of the trajectory generation device according to embodiment 1. FIG. 9 is a flowchart showing the operation of the trajectory generation device according to embodiment 1. FIG. 10 is a block diagram showing the configuration of a trajectory generation device according to embodiment 2. FIG. 11 is a diagram showing the operation of the trajectory generation device according to embodiment 2. FIG. 12 is a diagram showing the operation of the trajectory generation device according to embodiment 2. FIG. 13 is a block diagram showing the hardware configuration of a trajectory generation device according to another modified example. FIG. 14 is a block diagram showing the hardware configuration of a trajectory generation device according to another modified example.

[0010] First Embodiment FIGS. 1 and 2 are a plan view and a side view, respectively, showing a moving body 1 on which a trajectory generation device according to a first embodiment is mounted.

[0011] The moving body 1 includes a control device 10, wheels 101, an actuator 102, and a battery 103. Under the control of the control device 10, the actuator 102 converts the electric power obtained from the battery 103 into driving force and provides the driving force to the wheels 101.

[0012] The three wheels 101 are arranged at intervals of approximately 120° around the center of the mobile body 1 in the circumferential direction, and the three actuators 102 can rotate the three wheels 101 independently. This allows the mobile body 1 in FIGS. 1 and 2 to move in all directions (omnidirectionally). Note that the mobile body 1 is not limited to this, and may be, for example, a differential two-wheel mobile body, a legged mobile robot, a drone, or an automobile. Of these, a differential two-wheel mobile body has two wheels that can rotate independently, and moves straight and turns by utilizing the difference in rotational speed between the wheels.

[0013] The moving object 1 further includes a wheel angle sensor 111 shown in Fig. 1, an optical range sensor 112 and a depth camera 113 shown in Fig. 2. Although not shown, the wheel angle sensor 111, the optical range sensor 112 and the depth camera 113 are connected to the control device 10 so as to be able to communicate with each other.

[0014] The wheel angle sensor 111 is provided on the wheel axle and detects the amount of rotation of the wheel 101. The wheel angle sensor 111 is configured by, for example, a rotary encoder. The control device 10 calculates the amount of movement of the mobile object 1 based on the measured amount of rotation.

[0015] The optical range sensor 112 measures physical shape data of the space in the surrounding environment of the mobile object 1 along a scanning plane. The control device 10 creates a map of the surrounding environment, including obstacles, based on the measured shape data, and estimates the position of the mobile object 1 on the map by referring to the created map. Note that the control device 10 may also estimate the position of the mobile object 1 on the map by referring to a map created in advance, such as a default map.

[0016] The depth camera 113 measures physical shape data of the space around the moving object 1 as range information together with an image. The control device 10 uses the range information measured by the depth camera 113 to supplement the shape data on the scanning plane measured by the optical range sensor 112, i.e., shape data cut out from a plane in three-dimensional space. This allows the control device 10 to more accurately create a map of the surrounding environment, including obstacles.

[0017] 3 is a diagram showing an example of a map generated by the control device 10. The physical shape data of the space obtained by the optical range sensor 112 or the like includes the shape of an obstacle 500, such as a person 500a or an object 500b, that impedes the movement of the mobile object 1. The control device 10 determines that the obstacle 500 is an object that the mobile object 1 should avoid, and controls the actuator 102 based on the determination result. In the following description, the obstacle 500 will be described as a person 500a or an object 500b, but is not limited to these as long as it impedes the movement of the mobile object 1.

[0018] 4 is a block diagram showing the configuration of a trajectory generation device 300 according to the first embodiment, which is realized by the control device 10. In the first embodiment, the driving device 100 includes the above-described wheels 101, actuators 102, and battery 103, and the observation device 200 includes the above-described wheel angle sensors 111, optical range sensor 112, and depth camera 113, but these are not limited to these. For example, the components of the driving device 100 may be changed to suit the environment in which the moving object 1 moves, and the components of the observation device 200 may be changed to suit the method of obtaining shape data in the environment in which the moving object 1 moves.

[0019] The control device 10 includes a processor and memory, which will be described later, and the processor executes a program stored in the memory to realize the functions of the trajectory generation device 300. The control device 10 has a function of generating a target trajectory along which the moving body 1 should move, and a function of controlling the movement of the moving body 1 based on the target trajectory. For example, an embedded computer mounted on the moving body 1 is used as such a control device 10.

[0020] 4 includes a behavior planning unit 310 and an operation planning unit 320. As will be described later, the behavior planning unit 310 generates target contents and constraints for the movement of the moving object 1, and the operation planning unit 320 generates a target trajectory along which the moving object should move, based on the generated target contents and constraints.

[0021] The behavior planning unit 310 includes a state estimation unit 311 , a moving target calculation unit 312 , and a state constraint calculation unit 313 .

[0022] The state estimation unit 311 estimates self-position information indicating the position of the moving object 1 from the map in Fig. 3. The state estimation unit 311 outputs the estimated self-position information to the movement target calculation unit 312 and the motion planning unit 320.

[0023] The movement target calculation unit 312 generates target content to be achieved by the movement of the moving body 1 based on the self-position information output from the state estimation unit 311 and obstacle information, which is information on the obstacles 500 obtained from the map of FIG. 3. The target content includes, for example, a reference trajectory of the moving body 1, a target point to be reached by the moving body 1, and information on the obstacles 500 to be avoided by the moving body 1. The movement target calculation unit 312 outputs the generated target content to the motion planning unit 320.

[0024] The state constraint calculation unit 313 generates constraint information, which is a constraint on the moving body 1, from the map of Fig. 3 and outputs it to the operation planning unit 320. The constraint information includes, for example, a relative position between the moving body 1 and the obstacle 500 where there is a certain possibility that the moving body 1 will be interfered with by the obstacle 500 and where the moving body 1 should not be present in relation to the obstacle 500. The state constraint calculation unit 313 may generate the constraint information including the relative position based on the position of the moving body 1, the position of the obstacle 500, and the shape of the obstacle 500, which are included in the self-position information, the obstacle information, etc.

[0025] In the first embodiment, the state constraint calculation unit 313 not only outputs constraint information to the motion planning unit 320, but also outputs a mathematical expression model for generating trajectory candidates for the moving body 1 to the motion planning unit 320. The mathematical expression model may be output at the time of initial operation of the moving body 1 and the trajectory generation device 300. However, if the motion planning unit 320 has a mathematical expression model by default, the state constraint calculation unit 313 does not need to output the mathematical expression model to the motion planning unit 320. The mathematical expression model will be described in detail later.

[0026] In the above description, the behavior planning unit 310 generates various pieces of information to be output to the operation planning unit 320 from the map in Fig. 3, but this is not limiting. For example, the behavior planning unit 310 may generate various pieces of information based on information obtained directly from the observation device 200, such as the relative position obtained from the optical range sensor 112. Furthermore, for example, the behavior planning unit 310 may generate new information based on information obtained directly from one or more sensors of the observation device 200, and then generate various pieces of information based on the generated new information.

[0027] Furthermore, the behavior planning unit 310 may generate various types of information based on the calculation results of software that uses values ​​that mathematically represent phenomena, rather than using information directly obtained from the observation device 200. For example, the behavior planning unit 310 may generate constraint information by placing a virtual obstacle 500 on a map, or may generate constraint information by assuming that an obstacle 500 will appear probabilistically from outside the observation range of the observation device 200. Furthermore, for example, the behavior planning unit 310 may generate a trajectory of the obstacle 500 based on a model, and generate constraint information by taking the trajectory into consideration.

[0028] The motion planning unit 320 includes a trajectory candidate generating unit 321 , a target trajectory determining unit 322 , and a target trajectory storing unit 323 .

[0029] The trajectory candidate generation unit 321 generates trajectory candidates in a tree structure that is sequentially developed based on the self-location information, target content, constraint information, and mathematical model output from the behavior planning unit 310. Note that if the target content and constraint information are expressed by the relative position between the moving body 1 and the obstacle 500, which already reflects the self-location information, the trajectory candidate generation unit 321 may generate trajectory candidates without using the self-location information.

[0030] Next, an example will be described in which the trajectory candidate generating unit 321 uses Input-Based RRT (Rapidly Exploring Random Trees) to generate a tree-structured trajectory by sampling random input in an input space.

[0031] FIG. 5 is a diagram showing a typical coordinate system. The X, Y coordinate system in FIG. 5 is an inertial coordinate system, and (x ｒ，ｋ , y ｒ，ｋ ) represents the position of the center of gravity of the moving body 1 in the inertial coordinate system at a certain time step k. ｒ，ｋ , y ｒ，ｋ ) does not have to be the center of gravity of the moving body 1 as long as it can represent the position of the moving body 1, and may be, for example, the center point of the shape of the moving body 1, the installation point of the depth camera 113, or the installation point of the optical range sensor 112.

[0032] (v ｘ，ｋ , v ｙ，ｋ ) are the velocities of the moving body 1 in the X and Y directions in the inertial coordinate system at a certain time step k. ｏ，ｋ , y ｏ，ｋ ) represents the position of the representative point of the obstacle 500 in the inertial coordinate system at a certain time step k. The representative point of the obstacle 500 may be one or more points, and may be the point on the obstacle 500 that is the shortest distance from the moving body 1, the center point of the shape of the obstacle 500, or the center of gravity of the obstacle 500.

[0033] The trajectory candidate generation unit 321 calculates (x ｒ，ｋ , y ｒ，ｋ ) and apply the initial position of the moving object 1 to (v ｘ，ｋ＋１ , v ｙ，ｋ＋１) and apply one or more sampled random inputs. Δt is a discrete time, which corresponds to the interval of the time step. As will be described later, the mathematical model is not limited to the model expressed by the following equation (1).

[0034]

[0035] The trajectory candidate generation unit 321 calculates one or more (x ｒ，ｋ＋１ , y ｒ，ｋ＋１ Then, the trajectory candidate generating unit 321 calculates one or more (x ｒ，ｋ＋１ , y ｒ，ｋ＋１ For example, the trajectory candidate generating unit 321 determines whether (x ｒ，ｋ＋１ , y ｒ，ｋ＋１ ) is within a certain range from the reference orbit, and (x ｒ，ｋ , y ｒ，ｋ ) and is not in a relative position where the moving body 1 should not be present in relation to the obstacle 500, it is determined that the target content and constraint information are satisfied.

[0036] The trajectory candidate generation unit 321 does not satisfy the target content and constraint information (x ｒ，ｋ＋１ , y ｒ，ｋ＋１ ) and satisfy the goal content and constraint information (x ｒ，ｋ＋１ , y ｒ，ｋ＋１ Then, the trajectory candidate generation unit 321 stores the (x ｒ，ｋ＋１ , y ｒ，ｋ＋１ ) and one or more random inputs are applied (v ｘ，ｋ＋２ , v ｙ，ｋ＋２ ) and one or more (x ｒ，ｋ＋２ , y ｒ，ｋ＋２ By repeating the above, the trajectory candidate generation unit 321 calculates (x ｒ，ｋ , y ｒ，ｋ ), (x ｒ，ｋ＋１ , y ｒ，ｋ＋１ ), ... to generate a tree structure of trajectory candidates.

[0037] The trajectory candidate generation unit 321 may use a method other than Input-Based RRT as long as it can generate trajectory candidates in a tree structure that is sequentially expanded. The trajectory candidate generation unit 321 outputs the generated trajectory candidates to the target trajectory determination unit 322.

[0038] Fig. 6 is a diagram showing an example of tree-structured trajectory candidates generated by the trajectory candidate generation unit 321. Fig. 6 shows trajectory candidates 351a and 351b branching off from the position of the moving object 1, and trajectory candidate 351c branching off midway through trajectory candidate 351b. For convenience in the following description, when there is no need to distinguish between trajectory candidates 351a, 351b, and 351c, they may be referred to as trajectory candidate 351.

[0039] Each trajectory candidate 351 includes a node 361 and an edge 371. In the first embodiment, the node 361 is a node that is connected to the state (x （ｉ） ｒ，ｋ , y （ｉ） ｒ，ｋ ) and input (v （ｉ） ｘ，ｋ , v （ｉ） ｙ，ｋ ), where (i) is the index of the node and k is a different index for each time step.

[0040] An edge 371 connects multiple nodes 361. The time step of the node 361 at the tip of the arrow of the edge 371 is one time step larger than the time step of the node 361 at the end of the arrow of the edge 371, and therefore the edge 371 indicates a change in the node 361 for one time step. Of the two nodes 361 connected by the edge 371, the node 361 with the smaller time step is called a parent node, and the node 361 with the larger time step is called a child node.

[0041] Furthermore, a node 361 having one or more branches is called a branch node 362, a node 361 having no parent node is called a root node 363, and a node 361 having no child node is called a leaf node 364. One end of all of the trajectory candidates 351 is located at the moving object 1, so the root node 363 corresponds to the current position of the moving object 1. In this way, the trajectory candidate 351 includes a plurality of nodes 361 connected in time step order, that is, in chronological order.

[0042] 4 determines a target trajectory from the trajectory candidate 351 based on a cost value, which is an evaluation value of an evaluation function of the trajectory candidate 351 generated by the trajectory candidate generation unit 321. For example, if the cost value of the evaluation function decreases as the deviation between the trajectory candidate 351 and the reference trajectory decreases, the target trajectory determination unit 322 may determine the trajectory candidate 351 with the smallest cost value as the target trajectory.

[0043] The cost value of the evaluation function may vary based on at least one of the deviation between the trajectory candidate 351 and the reference trajectory, the magnitude of the acceleration vector of the trajectory candidate 351, the distance between the trajectory candidate 351 and the target point, and the distance between the trajectory candidate 351 and the obstacle 500 to be avoided. In this specification, for example, "at least one of A, B, C, ..., and Z" means any one of all combinations of one or more types extracted from the group of A, B, C, ..., and Z. For example, the cost value of the evaluation function may decrease when the magnitude of the acceleration vector of the trajectory candidate 351 is small, when the distance between the trajectory candidate 351 and the target point is small, or when the distance between the trajectory candidate 351 and the obstacle 500 to be avoided is large.

[0044] The target trajectory determination unit 322 outputs the determined target trajectory to the target trajectory storage unit 323. The target trajectory storage unit 323 stores the target trajectory output from the target trajectory determination unit 322.

[0045] The motion planning unit 320 extracts a necessary target trajectory from the target trajectories stored in the target trajectory storage unit 323 and generates a motion plan including the necessary target trajectory. Then, the motion planning unit 320 generates control information to be used by the drive unit 100 based on the motion plan and outputs the control information to the drive unit 100. The drive unit 100 moves the moving body 1 based on the control information, thereby enabling the moving body 1 to move in accordance with the motion plan.

[0046] FIG. 7 is a diagram showing the operation of a device (hereinafter referred to as an "associated device") related to the trajectory generation device 300 according to the first embodiment. The associated device has a configuration similar to the configuration of the trajectory generation device 300 according to the first embodiment described above. The following description will be given assuming that FIG. 6 shows the state at a past time step k and FIG. 7 shows the state at a current time step k+1. The following description will also be given assuming that, of the trajectory candidates 351a, 351b, and 351c generated at the past time step k, the trajectory candidate 351a has been determined as the target trajectory.

[0047] As a result of determining the trajectory candidate 351a as the target trajectory, the moving body 1 is located at node 361, which is one edge 371 along the trajectory candidate 351a from node 361 where the moving body 1 is located in Fig. 6, as shown in Fig. 7. Also in Fig. 7, the obstacle 500 has moved slightly to the left and is located on the trajectory candidate 351a, which is the target trajectory. In this case, the moving body 1 needs to avoid the obstacle 500, but there is no branch in the trajectory candidate 351a, which is the target trajectory, between the moving body 1 and the obstacle 500 that would allow the moving body 1 to avoid the obstacle 500.

[0048] In such a case, it is considered that the related device should generate new trajectory candidates based on the self-location information, target content, constraint information, and mathematical model, and determine a new target trajectory. However, in order for the moving body 1 in Fig. 7 to reliably avoid the obstacle 500, it is necessary to generate trajectory candidates more frequently and generate trajectory candidates that are widely distributed in space, which results in a problem that the generation of trajectory candidates imposes a large calculation load.

[0049] In contrast to this, the trajectory generation device 300 according to the first embodiment is capable of reducing the calculation load due to the generation of trajectory candidates, as will be described below.

[0050] Fig. 8 is a diagram showing the operation of the trajectory generation device 300 according to the first embodiment. The following description will be given assuming that Fig. 6 shows the state at past time step k and Fig. 8 shows the state at current time step k+1. The following description will also be given assuming that of the trajectory candidates 351a, 351b, and 351c generated at past time step k, trajectory candidate 351a is determined as the target trajectory.

[0051] The trajectory candidate generation unit 321 extracts non-determined trajectory candidates, which are trajectory candidates other than the trajectory candidate determined as the target trajectory, from among multiple trajectory candidates 351 generated in the past. The trajectory candidate generation unit 321 then generates tentative trajectory candidates based on a portion of the non-determined trajectory candidates. The tentative trajectory candidates are trajectories that serve as the basis for new trajectory candidates that can be determined as new target trajectories by the target trajectory determination unit 322. In the case of Figure 8, since the trajectory candidate 351a has been determined as the target trajectory, the trajectory candidate generation unit 321 generates tentative trajectory candidates based on a portion of the trajectory candidates 351b and 351c, which are non-determined trajectory candidates.

[0052] In the first embodiment, the trajectory candidate generation unit 321 generates a tentative trajectory candidate by connecting the next node, which is the first node of the non-determined trajectory candidate at the time next to the current time, with the current node, which is the second node of the target trajectory at the current time. In the case of Fig. 8, the trajectory candidate generation unit 321 generates a tentative trajectory candidate by connecting the next nodes 361b and 361c of the trajectory candidates 351b and 351c at the time step k+2 next to the current time step k+1 with the current node 361a of the trajectory candidate 351a at the current time step k+1.

[0053] Here, the node 361 of the trajectory candidate 351 according to the first embodiment is determined based on the state (x （ｉ） ｒ，ｋ , y （ｉ） ｒ，ｋ ) and input (v （ｉ） ｘ，ｋ , v （ｉ） ｙ，ｋIn such a case, in order to generate a new trajectory candidate, not only is the state quantity of the non-determined trajectory candidate changed by connecting the next node of the non-determined trajectory candidate with the current node, but also the input quantity needs to be changed.

[0054] Therefore, the trajectory candidate generation unit 321 according to the first embodiment updates the input amount of the next node of the non-determined trajectory candidate, or updates the input amount of the next node and the input amount of the child node of the next node, based on the constraint conditions of the moving object 1 itself. Note that the constraint conditions of the moving object 1 itself are different from the constraint information related to interference with the obstacle 500 described above, and are constraint conditions resulting from the movement performance of the moving object 1, such as the maximum speed of the moving object 1 and the degree of freedom in the movement direction. The trajectory candidate generation unit 321 may have these constraint conditions by default, or may acquire them from the motion planning unit 320. Next, a first example and a second example of updating the input amount will be described.

[0055] <First Example> In the first example, when the constraint condition is a constraint condition used in the difference equation of the mathematical model of the moving body 1, the trajectory candidate generation unit 321 updates the input amount of the next node of the non-determined trajectory candidate. In this case, the trajectory candidate generation unit 321 updates the provisional input amount (v ｘ，ｋ＋１ , v ｙ，ｋ＋１ ) is calculated.

[0056]

[0057] In equation (2), (r) is the index of the current node, which is the root node, k is the current time step, and (x （ｒ） ｒ，ｋ , y （ｒ） ｒ，ｋ ) is the state of the current node, and (x ｒ，ｋ＋１ , y ｒ，ｋ＋１ ) is the state quantity of the next node of the non-determined trajectory candidate.

[0058] The trajectory candidate generation unit 321 determines whether the provisional input amount calculated by equation (2) satisfies a constraint (e.g., an upper limit value of speed). If the provisional input amount satisfies the constraint, the trajectory candidate generation unit 321 updates the input amount of the next node of the non-determined trajectory candidate by the provisional input amount. On the other hand, if the provisional input amount does not satisfy the constraint, the trajectory candidate generation unit 321 excludes the provisional trajectory candidate generated from the non-determined trajectory candidate for which the provisional input amount was calculated.

[0059] In the second example, when the constraint is a constraint used for an optimization problem in which the input amount of the current node of the target trajectory and the input amount of the child node of the current node are used as reference values, the trajectory candidate generation unit 321 updates the input amount of the next node of the non-determined trajectory candidate and the input amount of the child node of the next node. In this case, the trajectory candidate generation unit 321 calculates the input amount u of the next node of the non-determined trajectory candidate using the following equation (3): Ｂｒ，ｋ＋１ and the input amount u of the child node of the next node Ｂｒ，ｋ＋２ It is determined whether or not it is possible to calculate

[0060]

[0061] Due to format restrictions in the specification, Ｂ , u Ｂ , Q Ｂ , R Ｂ Let represent the bold x, u, Q, and R in equation (3), and x^ Ｂ , u^ Ｂ In equation (3), x and u are written in bold with a ^ above them. Ｂｒ is (x ｒ , y ｒ ), and u Ｂｒ (v ｘ , v ｙ ), τ is an index of any node 361 from the root node to the leaf node of each trajectory candidate 351, and the capital letter τ is a set of τ. Ｂｒ , u^ Ｂｒ are the state and input quantities before updating, and Q Ｂ , R Ｂ are the weights of the state quantity and the input quantity, respectively. Note that the second and third equations in equation (3) correspond to constraint conditions.

[0062] If the trajectory candidate generation unit 321 can solve the optimization problem of equation (3) and calculate the input amount of the next node of the non-determined trajectory candidate and the input amount of the child node of the next node, it updates the input amount of the corresponding node of the non-determined trajectory candidate using those input amounts. On the other hand, if the trajectory candidate generation unit 321 cannot solve the optimization problem of equation (3) and calculate the input amount of the next node of the non-determined trajectory candidate and the input amount of the child node of the next node, it excludes the tentative trajectory candidate generated from the non-determined trajectory candidate.

[0063] The trajectory candidate generation unit 321 generates a new trajectory candidate that can be determined as a new target trajectory by the target trajectory determination unit 322 based on a tentative trajectory candidate generated from a portion of the non-determined trajectory candidates and a mathematical model of the moving body 1.

[0064] For example, the trajectory candidate generation unit 321 may acquire expansion nodes from the nodes 361 of any tentative trajectory candidate after the current time based on at least one of random, depth (number of extensions) priority, and width (number of branches) priority. Then, the trajectory candidate generation unit 321 may perform a process on the expansion nodes similar to the generation of trajectory candidates using the mathematical model of Equation (1), thereby generating a new trajectory candidate that can be determined as a new target trajectory by the target trajectory determination unit 322.

[0065] In this case, the trajectory candidate generation unit 321 can generate a new trajectory candidate that is an extension or branching of the tentative trajectory candidate. In the example of Fig. 8, a trajectory candidate 351d is generated as a new trajectory candidate from a node 361 of the trajectory candidate 351b, which is a tentative trajectory candidate.

[0066] Furthermore, the trajectory candidate generation unit 321 may generate a new trajectory candidate that can be determined as a new target trajectory by the target trajectory determination unit 322, based on a tentative trajectory candidate generated from a portion of the non-determined trajectory candidates, the target trajectory, and the mathematical expression model of the moving body 1. For example, the trajectory candidate generation unit 321 may acquire an expansion node from the node 361 of the target trajectory after the current time in the same manner as described above, and generate a new trajectory candidate by performing processing on the expansion node in the same manner as in generating a trajectory candidate using the mathematical expression model of equation (1).

[0067] The target trajectory determination unit 322 determines a new target trajectory from the new trajectory candidate based on a cost value, which is an evaluation value of the evaluation function of the new trajectory candidate.

[0068] The trajectory candidate generation unit 321 according to the first embodiment configured as described above converts trajectory candidates generated in past time steps so that they can be used as trajectory candidates for the current time step. By using them in this way, even if the processing for generating trajectory candidates for the current time based on the self-position information, target contents, constraint information, and mathematical model is reduced, the number of trajectory candidates for the current time can be secured, and the calculation load due to the generation of trajectory candidates can be reduced.

[0069] <Operation> Fig. 9 is a flowchart showing the operation of the trajectory generation device 300 according to the present embodiment 1. The operation in Fig. 9 may be performed sequentially regardless of whether the obstacle 500 is located on the target trajectory or not, or may be performed when the obstacle 500 is located on the target trajectory.

[0070] In step S1, the trajectory candidate generation unit 321 acquires past trajectory candidates. In step S2, the trajectory candidate generation unit 321 extracts non-determined trajectory candidates, which are trajectory candidates other than the trajectory candidate determined as the target trajectory, from the past trajectory candidates, and generates tentative trajectory candidates based on some of the non-determined trajectory candidates.

[0071] In step S3, the trajectory candidate generating unit 321 acquires expansion nodes from among the nodes 361 after the current time step of the tentative trajectory candidate and the target trajectory.

[0072] In step S4, the trajectory candidate generating unit 321 generates a new node by performing the same process as that for generating a trajectory candidate using the mathematical model of Equation (1) on the expansion node, and connects the new node to the expansion node.

[0073] In step S5, the trajectory candidate generating unit 321 determines whether the process of step S3 has been performed a predetermined number of times or more N. If it is determined that the process of step S3 has been performed a predetermined number of times or more, the operation in Fig. 9 ends, and if it is determined that the process of step S3 has not been performed a predetermined number of times or more, the process proceeds to step S3.

[0074] As a result, the process of step S3 is performed N times to generate new trajectory candidates. Thereafter, the target trajectory determination unit 322 determines a target trajectory from the trajectory candidates based on cost values, which are evaluation values ​​of the evaluation functions of the trajectory candidates generated by the trajectory candidate generation unit 321.

[0075] Summary of First Embodiment According to the trajectory generation device 300 of the first embodiment described above, new trajectory candidates that can be determined as new target trajectories are generated based on some of the non-determined trajectory candidates, which are trajectory candidates other than the trajectory candidate determined as the target trajectory among the multiple trajectory candidates generated in the past. With this configuration, even if the processing of generating new trajectory candidates based on the self-position information, target content, constraint information, and mathematical model is reduced, the number of new trajectory candidates can be secured, and therefore the calculation load due to the generation of trajectory candidates can be reduced.

[0076] Furthermore, in the first embodiment, new trajectory candidates are generated based on a part of the non-determined trajectory candidates and a mathematical model of the moving body. With this configuration, the quality of the new trajectory candidates can be improved.

[0077] In addition, in this first embodiment, a new trajectory candidate is generated by connecting the first node at the time next to the current time among the non-determined trajectory candidates with the second node at the current time of the target trajectory. This configuration reduces the calculation load due to the generation of trajectory candidates.

[0078] In addition, in the first embodiment, the input amount of the first node is updated based on the constraints of the moving object itself, or the input amount of the first node and the input amount of the child node of the first node are updated. With this configuration, it is possible to generate new trajectory candidates for the sequence of states and inputs.

[0079] <Modification> In the first embodiment, the mathematical model is the model expressed by equation (1), but it is not limited to this.

[0080] For example, the mathematical model may be a model expressed by the following equation (4) and used for a mobile object that can move in all directions. Then, the trajectory candidate generation unit 321 calculates the provisional state quantity (x ｒ，ｋ＋１ , y ｒ，ｋ＋１ ) and input quantity (v ｘ，ｋ＋１ , v ｙ，ｋ＋１ ) may be calculated.

[0081]

[0082]

[0083] For example, the mathematical model may be a model expressed by the following equation (6) and used for a differential two-wheel vehicle. Then, the trajectory candidate generating unit 321 calculates the provisional state quantity θ of the non-determined trajectory candidate using the following equation (7) related to the difference equation. ｒ，ｋ＋１ and the input amount (v ｋ＋１ , w ｋ＋１ , w ｋ＋２ ) may be calculated. ｋ＋１ , w ｋ＋２ ) is the angular velocity of the moving body 1 at a certain time step k, k+1.

[0084]

[0085]

[0086] For example, the mathematical model may be a model expressed by the following equation (8) and used for a differential two-wheel vehicle. Then, the trajectory candidate generating unit 321 calculates the provisional state quantity (x ｒ，ｋ＋１ , y ｒ，ｋ＋１ , θ ｒ，ｋ＋１, θ ｒ，ｋ＋２ ) and input quantity (v ｋ＋１ , w ｋ＋１ , w ｋ＋２ ) may be calculated.

[0087]

[0088]

[0089] In the first embodiment, the trajectory candidate generation unit 321 generates tentative trajectory candidates based on a portion of the non-determined trajectory candidates, and generates new trajectory candidates that can be determined as a new target trajectory based on the tentative trajectory candidates and the mathematical model. However, the trajectory candidate generation unit 321 may directly generate the tentative trajectory candidates generated based on a portion of the non-determined trajectory candidates as new trajectory candidates that can be determined as a new target trajectory by the target trajectory determination unit 322.

[0090] 10 is a block diagram showing the configuration of a trajectory generation device 300 according to Embodiment 2. Among the components according to Embodiment 2, components that are the same as or similar to the components described above are given the same or similar reference numerals, and different components will be mainly described below.

[0091] The configuration of the trajectory generation device 300 in Fig. 10 is the same as the configuration of the motion planning unit 320 in Fig. 4 , with a motion pattern setting unit 324 added. The motion pattern setting unit 324 sets a motion pattern for the moving body 1. The motion patterns are patterns assumed as motion and trajectory candidates for the moving body 1, such as stopping temporarily, moving straight, avoiding to the left, avoiding to the right, and retreating. Each motion pattern is defined, for example, by a discrete value or a speed function based on human motion. The motion pattern setting unit 324 outputs the motion pattern to the trajectory candidate generation unit 321.

[0092] The trajectory candidate generation unit 321 selects a non-determined trajectory candidate to be used for generating a new trajectory candidate from among a plurality of non-determined trajectory candidates based on a movement pattern. In the second embodiment, the trajectory candidate generation unit 321 selects a non-determined trajectory candidate that corresponds to the movement pattern from among the plurality of non-determined trajectory candidates as a non-determined trajectory candidate to be used for generating a new trajectory candidate. For example, the trajectory candidate generation unit 321 calculates a similarity between a non-determined trajectory candidate and a discrete value of the movement pattern or a speed function based on human movement, and determines that the non-determined trajectory candidate corresponds to the movement pattern if the similarity is equal to or greater than a threshold.

[0093] 11 and 12 are diagrams showing the operation of the trajectory generation device 300 according to the second embodiment. The following description will be given assuming that Fig. 11 shows the state at past time step k, and Fig. 12 shows the state at current time step k+1. The following description will be given assuming that, of the trajectory candidates 351e to 351i generated at past time step k, trajectory candidate 351f is determined as the target trajectory, and that trajectory candidates 351f, 351g, and 351h correspond to the operation patterns of turning left, going straight, and turning right, respectively.

[0094] In this case, the trajectory candidate generation unit 321 selects the trajectory candidates 351g and 351h that correspond to the movement pattern from among the trajectory candidates 351e, 351g, 351h, and 351i that are the multiple non-determined trajectory candidates, as tentative trajectory candidates to be used for generating new trajectory candidates. In other words, the trajectory candidate generation unit 321 excludes the trajectory candidates 351e and 351i that do not correspond to the movement pattern from the tentative trajectory candidates to be used for generating new trajectory candidates.

[0095] Summary of Second Embodiment According to the trajectory generation device 300 of the second embodiment, a non-determined trajectory candidate corresponding to the motion pattern of the moving body 1 is selected as a non-determined trajectory candidate to be used for generating a new trajectory candidate. This configuration can reduce the calculation load due to the processing of steps S3 to S5 in Fig. 9 and enable the target trajectory determination unit 322 to efficiently determine a target trajectory.

[0096] In the above description, for example, one non-determined trajectory candidate corresponds to one movement pattern, but multiple non-determined trajectory candidates may correspond to one movement pattern. In such a case, the trajectory candidate generation unit 321 may select a non-determined trajectory candidate to be used to generate a new trajectory candidate from the multiple non-determined trajectory candidates based on the evaluation value of the evaluation function of the non-determined trajectory candidate and the movement pattern.

[0097] For example, when multiple non-determined trajectory candidates correspond to one motion pattern, the trajectory candidate generation unit 321 may select the non-determined trajectory candidate with the smallest cost value, which is the evaluation value of the evaluation function of the non-determined trajectory candidate, from the multiple non-determined trajectory candidates. For example, when the non-determined trajectory candidate has a large number of nodes or when the movement distance from the current node to the next node of the non-determined trajectory candidate is small, the cost value of the evaluation function may be small. This configuration can reduce the calculation load due to the processing of steps S3 to S5 in FIG. 9 and enable the target trajectory determination unit 322 to efficiently determine the target trajectory.

[0098] <Other Modifications> The above-described behavior planning unit 310, trajectory candidate generation unit 321, and target trajectory determination unit 322 shown in FIG. 4 are hereinafter referred to as the "behavior planning unit 310, etc." The behavior planning unit 310, etc. are realized by a processing circuit 81 shown in FIG. 13. That is, the processing circuit 81 includes: a behavior planning unit 310 that generates target content and constraints for the movement of the moving object; a trajectory candidate generation unit 321 that generates trajectory candidates in a tree structure that are sequentially expanded based on the target content and constraints, and generates new trajectory candidates that can be determined as a new target trajectory based on some non-determined trajectory candidates, which are trajectory candidates other than the trajectory candidates determined as the target trajectory among multiple trajectory candidates generated in the past; and a target trajectory determination unit 322 that determines a target trajectory along which the moving object should move from the trajectory candidates based on evaluation values ​​of evaluation functions of the trajectory candidates. The processing circuit 81 may be implemented using dedicated hardware, or may be implemented using a processor that executes a program stored in memory. Examples of the processor include a central processing unit, a processing unit, an arithmetic unit, a microprocessor, a microcomputer, and a DSP (Digital Signal Processor).

[0099] When the processing circuitry 81 is dedicated hardware, the processing circuitry 81 may be, for example, a single circuit, a composite circuit, a programmed processor, a parallel programmed processor, an ASIC (Application Specific Integrated Circuit), an FPGA (Field Programmable Gate Array), or a combination thereof. The functions of each unit, such as the behavior planning unit 310, may be realized by a circuit in which the processing circuits are distributed, or the functions of each unit may be realized by a single processing circuit.

[0100] When the processing circuit 81 is a processor, the functions of the behavior planning unit 310 and the like are realized in combination with software and the like. Software and the like may include, for example, software, firmware, or both software and firmware. The software and the like are written as a program and stored in memory. As shown in FIG. 14 , the processor 82 applied to the processing circuit 81 realizes the functions of each unit by reading and executing a program stored in memory 83. That is, the trajectory generation device 300 includes a memory 83 for storing a program that, when executed by the processing circuit 81, ultimately executes the following steps: generating target content and constraints for the movement of a moving object; generating trajectory candidates in a tree structure that are sequentially expanded based on the target content and constraints; determining a target trajectory along which the moving object should move from the trajectory candidates based on evaluation values ​​of evaluation functions of the trajectory candidates; and generating new trajectory candidates that can be determined as a new target trajectory based on a portion of non-determined trajectory candidates, which are trajectory candidates other than the trajectory candidates determined as the target trajectory among multiple trajectory candidates generated in the past. In other words, this program can be said to cause a computer to execute the procedures and methods of the behavior planning unit 310 and the like. Here, the memory 83 may be, for example, a non-volatile or volatile semiconductor memory such as a RAM (Random Access Memory), a ROM (Read Only Memory), a flash memory, an EPROM (Erasable Programmable Read Only Memory), or an EEPROM (Electrically Erasable Programmable Read Only Memory), a HDD (Hard Disk Drive), a magnetic disk, a flexible disk, an optical disk, a compact disk, a mini disk, a DVD (Digital Versatile Disc), a drive device for any of these, or any storage medium to be used in the future.

[0101] The above describes a configuration in which each function of the behavior planning unit 310 and the like is realized either by hardware or software, etc. However, the present invention is not limited to this, and a configuration in which part of the behavior planning unit 310 and the like is realized by dedicated hardware and another part is realized by software, etc. For example, the function of the behavior planning unit 310 can be realized by the processing circuitry 81 as dedicated hardware, and the other functions can be realized by the processing circuitry 81 as the processor 82 reading and executing programs stored in the memory 83.

[0102] As described above, the processing circuitry 81 can realize the above-described functions by hardware, software, or a combination of these.

[0103] The trajectory generation device described above can also be applied to a trajectory generation system constructed as a system by appropriately combining a processing device, a communication terminal, the functions of an application installed on at least one of the processing device and the communication terminal, and a server. Communication terminals include, for example, mobile phones, smartphones, and tablets. The functions or components of the trajectory generation device described above may be distributed among the devices that construct the system, or may be concentrated in one of the devices.

[0104] In this disclosure, 'a' and 'an' mean one or more. Therefore, 'a', 'an', 'one or more', and 'at least one' can be used interchangeably.

[0105] It should be noted that the embodiments and modifications may be freely combined, and the embodiments and modifications may be modified or omitted as appropriate. The above description is illustrative in all respects and is not limiting. It is understood that countless modifications not illustrated can be envisioned.

[0106] Various aspects of the present disclosure are summarized below as appendices.

[0107] (Supplementary Note 1) A trajectory generation device comprising: an action planning unit that generates target contents and constraints for the movement of a moving body; a trajectory candidate generation unit that generates trajectory candidates in a tree structure that are sequentially expanded based on the target contents and the constraints; and a target trajectory determination unit that determines a target trajectory along which the moving body should move from the trajectory candidates based on evaluation values ​​of evaluation functions of the trajectory candidates, wherein the trajectory candidate generation unit generates new trajectory candidates that can be determined as the new target trajectory based on a portion of non-determined trajectory candidates that are trajectory candidates other than the trajectory candidates determined as the target trajectory among a plurality of trajectory candidates generated in the past.

[0108] (Supplementary Note 2) The trajectory generation device according to Supplementary Note 1, wherein the trajectory candidate generation unit generates the new trajectory candidate based on a portion of the non-determined trajectory candidate and the target trajectory.

[0109] (Supplementary Note 3) The trajectory generation device according to Supplementary Note 1 or Supplementary Note 2, wherein the trajectory candidate generation unit generates the new trajectory candidate based on a portion of the non-determined trajectory candidates and a mathematical model of the moving body.

[0110] (Supplementary Note 4) A trajectory generation device according to any one of Supplementary Note 1 to Supplementary Note 3, wherein the trajectory candidates include a plurality of nodes connected in chronological order, and the trajectory candidate generation unit generates the new trajectory candidate by connecting a first node at a time next to a current time among the non-determined trajectory candidates with a second node at a current time of the target trajectory.

[0111] (Supplementary Note 5) The trajectory generation device according to any one of Supplementary Note 1 to Supplementary Note 4, further comprising a motion pattern setting unit that sets a motion pattern of the moving body, wherein the trajectory candidate generation unit selects, from among the plurality of non-deterministic trajectory candidates, a non-deterministic trajectory candidate to be used for generating the new trajectory candidate, based on the motion pattern.

[0112] (Supplementary Note 6) The trajectory generation device according to Supplementary Note 5, wherein the trajectory candidates include a plurality of nodes connected in chronological order, and the trajectory candidate generation unit selects, from the plurality of non-deterministic trajectory candidates, a non-deterministic trajectory candidate to be used for generating the new trajectory candidate, based on an evaluation value of an evaluation function of the non-deterministic trajectory candidate and the motion pattern.

[0113] (Supplementary Note 7) A trajectory generation device according to Supplementary Note 4, wherein the trajectory candidate generation unit updates the input amount of the first node, or updates the input amount of the first node and the input amount of a child node of the first node, based on constraints on the moving body itself.

[0114] (Supplementary Note 8) The trajectory generation device according to Supplementary Note 7, wherein the constraint condition is a constraint condition used in a difference equation of a mathematical model of the moving body.

[0115] (Supplementary Note 9) The trajectory generation device according to Supplementary Note 7, wherein the constraint is a constraint used for an optimization problem in which an input amount of the second node and an input amount of a child node of the second node are used as reference values.

[0116] (Supplementary Note 10) A trajectory generation method comprising: generating target contents and constraints for the movement of a moving body; generating trajectory candidates in a tree structure that are sequentially expanded based on the target contents and the constraints; determining a target trajectory along which the moving body should move from the trajectory candidates based on evaluation values ​​of evaluation functions of the trajectory candidates; and generating a new trajectory candidate that can be determined as the new target trajectory based on a portion of non-determined trajectory candidates that are trajectory candidates other than the trajectory candidates determined as the target trajectory among a plurality of trajectory candidates generated in the past.

[0117] 1 Moving body, 300 Trajectory generation device, 310 Action planning unit, 321 Trajectory candidate generation unit, 322 Target trajectory determination unit, 324 Motion pattern setting unit, 351 Trajectory candidate.

Claims

1. A trajectory generation device comprising: an action planning unit that generates target contents and constraints for the movement of a moving body; a trajectory candidate generation unit that generates trajectory candidates in a tree structure that are sequentially expanded based on the target contents and the constraints; and a target trajectory determination unit that determines a target trajectory along which the moving body should move from the trajectory candidates based on evaluation values ​​of evaluation functions of the trajectory candidates, wherein the trajectory candidate generation unit generates new trajectory candidates that can be determined as the new target trajectory based on a portion of non-determined trajectory candidates that are trajectory candidates other than the trajectory candidates determined as the target trajectory out of multiple trajectory candidates generated in the past.

2. A trajectory generation device according to claim 1, wherein the trajectory candidate generation unit generates the new trajectory candidate based on a portion of the non-determined trajectory candidate and the target trajectory.

3. A trajectory generation device according to claim 1 or claim 2, wherein the trajectory candidate generation unit generates the new trajectory candidate based on a portion of the non-determined trajectory candidates and a mathematical model of the moving body.

4. A trajectory generation device according to any one of claims 1 to 3, wherein the trajectory candidates include a plurality of nodes connected in chronological order, and the trajectory candidate generation unit generates the new trajectory candidate by connecting a first node at the time next to the current time among the non-determined trajectory candidates with a second node at the current time of the target trajectory.

5. A trajectory generation device according to any one of claims 1 to 4, further comprising a movement pattern setting unit that sets a movement pattern of the moving body, wherein the trajectory candidate generation unit selects, from among a plurality of non-deterministic trajectory candidates, a non-deterministic trajectory candidate to be used in generating the new trajectory candidate, based on the movement pattern.

6. A trajectory generation device according to claim 5, wherein the trajectory candidates include a plurality of nodes connected in chronological order, and the trajectory candidate generation unit selects, from the plurality of non-deterministic trajectory candidates, a non-deterministic trajectory candidate to be used in generating the new trajectory candidate, based on an evaluation value of an evaluation function of the non-deterministic trajectory candidate and the movement pattern.

7. A trajectory generation device according to claim 4, wherein the trajectory candidate generation unit updates the input amount of the first node, or updates the input amount of the first node and the input amount of a child node of the first node, based on constraints on the moving body itself.

8. A trajectory generation device according to claim 7, wherein the constraints are constraints used in a difference equation of a mathematical model of the moving body.

9. A trajectory generation device according to claim 7, wherein the constraint is a constraint used for an optimization problem in which the input amount of the second node and the input amount of a child node of the second node are used as reference values.

10. A trajectory generation method comprising: generating target contents and constraints for the movement of a moving body; generating trajectory candidates in a tree structure that are sequentially expanded based on the target contents and the constraints; determining a target trajectory along which the moving body should move from the trajectory candidates based on evaluation values ​​of evaluation functions of the trajectory candidates; and generating a new trajectory candidate that can be determined as the new target trajectory based on a portion of non-determined trajectory candidates that are trajectory candidates other than the trajectory candidates determined as the target trajectory among multiple trajectory candidates generated in the past.

Images