Trajectory planning device

Abstract

To provide a trajectory planning device which enables arriving at a destination through a travelable area, even when the travellable area is complicated.SOLUTION: A trajectory planning device for planning a trajectory of a mobile body: calculates a travelable area of a mobile body on the basis of peripheral information of the mobile body; generates, as polynomial trajectories, a plurality of polynomials passing through a current position of the mobile body and a target position of the mobile body; conducts evaluation related to a distance to a travelable area and evaluation of whether or not being in the travelable area, for the plurality of polynomial trajectories; selects, as a generated trajectory, one of the plurality of polynominal trajectories on the basis of the evaluation results; and outputs the generated trajectory to a motion control part that controls the mobile body.SELECTED DRAWING: Figure 32

Description

[Technical field]

[0001] The present disclosure relates to a trajectory planning device, and more particularly to a trajectory planning device that plans operations for achieving autonomous driving of a vehicle or the like. [Background technology]

[0002] In recent years, the development of autonomous mobile systems such as automatic driving of automobiles and transport vehicles has progressed. In an autonomous mobile system, a trajectory consisting of a path and a speed on which a moving body should travel is generated, and the moving body is controlled to travel along the generated trajectory. In many scenes, a trajectory plan is made to follow the center of the road and a guide such as a magnetic marker. However, in some cases, such information cannot be used in a scene near a toll booth where there are no white lines on the road, a scene where the autonomous transport vehicle moves to a destination without using a guide, and in a scene where the autonomous transport vehicle moves to a destination without using a guide. In such a scene, a trajectory is required that can reach the destination while avoiding obstacles in a space without landmark information on which to travel, and a technology has been developed that realizes a trajectory plan even without landmark information on which to travel, as disclosed in Patent Document 1, for example. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] JP 2012-145998 A Summary of the Invention [Problem to be solved by the invention]

[0004] In Patent Document 1, a method is adopted in which a driving route is generated based on a road direction formed by a line passing through the center points of multiple circles inscribed in a driveable area where no obstacles exist. In this case, in a very wide driveable area such as an airport, the inscribed circle cannot be determined, so a driving route cannot be generated and the destination cannot be reached. Also, in a driveable area with a complex shape and large changes in width, such as near a toll booth, the correct road direction cannot be calculated and the destination cannot be reached.

[0005] The present disclosure has been made to solve the above problems, and aims to provide a trajectory planning device that can reach a destination through a drivable area even when the drivable area is complex, such as near an airport or a toll booth. [Means for solving the problem]

[0006] The trajectory planning device according to the present disclosure is a trajectory planning device that plans a trajectory of a moving body, calculates a driveable area of ​​the moving body based on information about the surroundings of the moving body, generates a plurality of polynomials that pass through a current position of the moving body and a target position of the moving body to form polynomial trajectories, evaluates the distance of the plurality of polynomial trajectories from the driveable area and evaluates whether they are within the driveable area, selects one of the plurality of polynomial trajectories as a generated trajectory based on the evaluation result, and outputs the generated trajectory to a motion control unit that controls the moving body. Effect of the Invention

[0007] According to the trajectory planning device of the present disclosure, the multiple polynomial trajectories are evaluated for their distance from the drivable area and whether they are within the drivable area, and one of the multiple polynomial trajectories is selected as a generated trajectory based on the evaluation result, and the generated trajectory is output to a motion control unit that controls the moving body. Therefore, even if the drivable area is complex, it is possible to reach the destination through the drivable area. [Brief description of the drawings]

[0008] [Figure 1]1 is a block diagram showing an example of a schematic configuration of a moving body equipped with a trajectory planning device according to a first embodiment. [Diagram 2] FIG. 2 is a diagram showing an example of a travelable area in the first embodiment. [Diagram 3] FIG. 4 is a diagram showing an example of a target state quantity in the first embodiment. [Figure 4] 3 is a diagram showing an example of trajectory points calculated by a trajectory point calculation unit in the trajectory planning device of the embodiment 1. FIG. [Diagram 5] 2 is a diagram showing an example of a trajectory generated by a trajectory generating unit in the trajectory planning device of the embodiment 1. FIG. [Figure 6] 4 is a flowchart illustrating the operation of the trajectory planning device according to the first embodiment. [Figure 7] 3 is a diagram showing an example of information acquired from an information acquisition unit in the trajectory planning device of the embodiment 1. FIG. [Figure 8] 4 is a diagram showing an example in which information acquired from an information acquisition unit in the trajectory planning device of the embodiment 1 is transformed into a moving body coordinate system. FIG. [Figure 9] FIG. 2 is a diagram showing an example of prediction of a travelable area in the trajectory planning device of the embodiment 1. [Figure 10] FIG. 4 is a diagram showing another example of prediction of a travelable area in the trajectory planning device of the embodiment 1. [Figure 11] 1 is a diagram showing an example of a travelable area in the trajectory planning device of the embodiment 1. FIG. [Figure 12] 1 is a diagram showing an example of a travelable area in the trajectory planning device of the embodiment 1. FIG. [Figure 13] 1 is a diagram showing an example of a travelable area in the trajectory planning device of the embodiment 1. FIG. [Figure 14] FIG. 2 is a diagram showing an example of a target state quantity in the trajectory planning device of the embodiment 1. [Figure 15] FIG. 4 is a diagram showing an example of a target state quantity reset in the trajectory planning device of the embodiment 1. [Figure 16] FIG. 4 is a diagram showing an example of setting an upper limit value of a velocity among target state quantities in the trajectory planning device of the embodiment 1. [Figure 17] FIG. 4 is a diagram showing an example of setting an upper limit value of a velocity among target state quantities in the trajectory planning device of the embodiment 1. [Figure 18] FIG. 4 is a diagram showing an example of a state quantity predicted by a particle filter in the trajectory planning device of the embodiment 1. [Figure 19] FIG. 4 is a diagram showing an example of input values ​​set according to the shape of a travelable area in the trajectory planning device of the embodiment 1. [Figure 20] FIG. 4 is a diagram showing an example of input values ​​set according to the shape of a travelable area in the trajectory planning device of the embodiment 1. [Figure 21] 4 is a diagram showing an example of setting of input values ​​when there is a large discrepancy between a current state quantity and a target state quantity of a moving object in the trajectory planning device of the embodiment 1. FIG. [Figure 22] FIG. 2 is a diagram illustrating an example of observation variables in the trajectory planning device of the embodiment 1. [Figure 23] FIG. 4 is a diagram showing an example of weighting of particles outside a travelable area in the trajectory planning device of the embodiment 1. [Figure 24] FIG. 4 is a diagram showing an example of weighting of particles within a travelable area in the trajectory planning device of the embodiment 1. [Diagram 25] FIG. 4 is a diagram showing an example of weighting of particles within a travelable area in the trajectory planning device of the embodiment 1. [Figure 26] FIG. 4 is a diagram showing an example of weighting of particles within a travelable area in the trajectory planning device of the embodiment 1. [Figure 27] FIG. 4 is a diagram showing an example of weighting of particles within a predicted travelable area in the first embodiment. [Figure 28] FIG. 4 is a diagram showing an example of setting weights for evaluation in the trajectory planning device of the embodiment 1. [Figure 29] FIG. 4 is a diagram illustrating an example of a process for obtaining a plurality of trajectory points that reach a target state quantity in the trajectory planning device of the embodiment 1. [Diagram 30] FIG. 4 is a diagram showing an example of trajectory generation until a target state quantity is reached in the trajectory planning device of the embodiment 1. [Diagram 31] FIG. 4 is a diagram showing an example of trajectory generation until a target state quantity is reached in the trajectory planning device of the embodiment 1. [Diagram 32] FIG. 11 is a block diagram showing an example of a schematic configuration of a moving body equipped with a trajectory planning device according to a second embodiment. [Diagram 33] 13 is a diagram for explaining a method of deriving a polynomial that connects a moving body and a target position in the trajectory planning device of the embodiment 2. FIG. [Diagram 34] 13 is a diagram for explaining a method of deriving a polynomial that connects a moving body and a target position in the trajectory planning device of the embodiment 2. FIG. [Diagram 35] 13 is a diagram for explaining a method of deriving a polynomial that connects a moving body and a target position in the trajectory planning device of the embodiment 2. FIG. [Diagram 36] 11 is a diagram illustrating weighting when a predicted state quantity is significantly different from a current state quantity of a moving object. FIG. [Figure 37] FIG. 13 is a diagram for explaining weighting when a predicted state quantity is significantly different from a state quantity of a previously calculated trajectory point. [Figure 38] FIG. 2 is a diagram illustrating a hardware configuration for implementing the trajectory planning device according to the first and second embodiments. [Figure 39] FIG. 2 is a diagram illustrating a hardware configuration for implementing the trajectory planning device according to the first and second embodiments. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0009] <Embodiment 1> 1 is a block diagram showing an example of a schematic configuration of a moving body 1 equipped with a trajectory planning device according to the embodiment 1. The moving body 1 includes a trajectory planning device 200 that generates a trajectory to be taken by the moving body 1 based on information obtained from an information acquisition unit 100 that acquires destination information to be reached by the moving body 1, surrounding environment information of the moving body 1, and the self-state of the moving body 1, and a motion control unit 300 that controls the motion of the moving body 1 based on the trajectory generated by the trajectory planning device 200.

[0010] The information acquisition unit 100 includes a target value acquisition unit 110, a self-state acquisition unit 120, and a surrounding environment acquisition unit .

[0011] The target value acquisition unit 110 acquires information such as a target position, a target speed, and a target azimuth angle to be reached by the moving body. The target value acquisition unit 110 acquires information from, for example, infrastructure information received from a traffic control, information designated in advance by a user, a predetermined position of map information held by the moving body, etc. Here, the map information held by the moving body is different from a high-precision map, and refers to a car navigation map, a point cloud map generated by SLAM (Simultaneous Localization and Mapping), etc.

[0012] Examples of the target position include the entrance or bar position of a gate at a toll booth, a withdrawal position on a highway, the nose wheel of an airplane on a towing tractor, and the position of the moving object 1 specified by the user. Examples of the target speed include the legal speed limit and a specified speed previously set by the user. The target azimuth is the angle of the target when passing through the target position, and examples of the target azimuth include the vertical direction to the gate when passing through the gate.

[0013] The self-state acquisition unit 120 acquires the current state of the moving object itself. Examples of the self-state acquisition unit 120 include a speed sensor, an acceleration sensor, an inertial measurement unit, a steering angle sensor, a steering torque sensor, a yaw rate sensor, and a Global Navigation Satellite System (GNSS) sensor. Here, the inertial measurement unit is hereinafter referred to as an IMU (Inertial Measurement Unit) sensor.

[0014] The surrounding environment acquisition unit 130 acquires information on walls around the moving body, the positions and speeds of moving obstacles, and azimuth angles of obstacle-free spaces in which the moving body can travel. Examples of the surrounding environment acquisition unit 130 include millimeter wave radar, cameras, LiDAR (Light Detection and Ranging), sonar, vehicle-to-vehicle communication devices, and road-to-vehicle communication devices.

[0015] The trajectory planning device 200 has a travelable area calculation unit 210 , a target state calculation unit 220 , a state prediction unit 230 , a predicted state evaluation unit 240 , a trajectory point calculation unit 250 , and a trajectory generation unit 260 .

[0016] The drivable area calculation unit 210 calculates a drivable area in which no obstacles exist and in which the moving body 1 can travel, based on the surrounding information of the moving body 1 acquired from the surrounding environment acquisition unit 130. FIG. 2 shows an example of the drivable area. In FIG. 2, a stationary obstacle SOB exists on the left side of the moving body 1 in the traveling direction, and a moving obstacle MOB is about to enter a traveling lane defined by the left and right lane boundaries LB from the right side in the traveling direction, and a drivable area TA in which the stationary obstacle SOB and the moving obstacle MOB do not exist is shown by a thick line. As shown in FIG. 2, the drivable area TA is not necessarily limited to the traveling lane defined by the lane boundaries LB such as white lines on the road.

[0017] The target state calculation unit 220 calculates a target state quantity at the destination where the moving object 1 should reach, based on the information from the target value acquisition unit 110. This target state quantity includes at least the target position of the moving object 1. An example of the target state quantity is shown in FIG.

[0018] FIG. 3 shows a road without white lines, such as in front of an ETC gate. The lane boundary LB here is not a white line, but a wall or a guardrail. In FIG. 3, the target state TG is the coordinates of the target position at time t (x t ,y t ), target azimuth θ t , target speed v t The current state of the moving object 1 is (x e ,y e ,θ e ,v e ).

[0019] The state prediction unit 230 generates one or more trajectory candidates by predicting at least the current state quantity of the moving body 1 and the state quantity of the moving body 1 at one or more positions between the current position and the target position of the moving body 1. For this purpose, for example, a predetermined number of inputs are input to the motion model of the moving body by a state estimation calculation using the motion model of the moving body, and the one or more trajectory candidates are generated by predicting the state quantity at least one step ahead for the multiple inputs, that is, one sampling time ahead in the control period. In this embodiment, a particle filter is used as an example of a state estimation method.

[0020] A particle filter is a method for predicting time series data using a probability density distribution, and is sometimes called a sequential Monte Carlo method. A particle filter as a state estimation calculation approximates the probability density distribution of a state using multiple particles. For example, if there are many particles with a certain state quantity, it is possible to estimate the entire probability density distribution by using a particle filter as the state estimation calculation, and therefore the frequency of outputting a local optimal solution can be reduced.

[0021] The predicted state evaluation unit 240 assigns a weight to each predicted state quantity, i.e., each particle, to generate weighted trajectory candidates, evaluates the weighted trajectory candidates based on the weights, and outputs an evaluation result. At this time, weighting is performed based on the target state quantity calculated by the target state calculation unit 220 and the drivable area calculated by the drivable area calculation unit 210. When calculating trajectory points in the trajectory point calculation unit 250 described later, the weights are calculated based on the weight values ​​of the predicted state quantities, and a weighted average of multiple state quantities predicted by the state prediction unit 230 can be calculated to obtain a plausible state quantity.

[0022] For example, if the weighting coefficient of a state quantity outside the drivable area is set to 0, then when calculating the weighted average, that state quantity is multiplied by the weighting coefficient of 0, preventing the trajectory point from being outside the drivable area, and generating a trajectory that is guaranteed to be within the drivable area TA.

[0023] The trajectory point calculation unit 250 calculates trajectory points from the trajectory candidates. Specifically, the trajectory point calculation unit 250 performs a weighted average of the predicted state quantities predicted by the state prediction unit 230 according to the weights assigned by the predicted state evaluation unit 240, and performs a calculation to set the weighted averaged state quantities as trajectory points. A conceptual diagram of this calculation is shown in FIG.

[0024] 4, there are a particle group G0 with a state quantity with a weighting coefficient of 0, a particle group GL with a state quantity with a low weighting coefficient, and a particle group GH with a state quantity with a high weighting coefficient in and around the drivable area TA, and the weighted average of the particle group GH with the state quantity with the high weighting coefficient is set as the trajectory point TP. Also, the state quantity with the highest weight given by the predicted state evaluation unit 240 can be set as the trajectory point.

[0025] The trajectory generating unit 260 generates a trajectory from the trajectory candidates based on the evaluation result, and outputs the trajectory to the motion control unit 300 that controls the moving body 1 based on the generated trajectory. Specifically, the trajectory generating unit 260 outputs a point sequence composed of trajectory points for each discrete time calculated by the trajectory point calculation unit 250 to the motion control unit 300 as a generated trajectory. A conceptual diagram of the generated trajectory is shown in FIG.

[0026] As shown in FIG. 5, trajectory points TP1, TP2, TP3, TP4 and TP5 at discrete times t1, t2, t3, t4 and t5 are shown in the drivable area TA, and a generating trajectory GT is obtained from the five trajectory points.

[0027] The motion control unit 300 includes a control amount calculation unit 310 and an actuator control unit 320 .

[0028] The control amount calculation unit 310 calculates target control values ​​for the moving body 1 to travel along the target trajectory, using the trajectory generated by the trajectory generation unit 260 as the target trajectory, and outputs the target control values ​​to the actuator control unit 320.

[0029] The actuator control unit 320 is a controller mounted on the moving body 1, and operates the actuators so that the moving body follows the target control value calculated by the control amount calculation unit 310. Examples of the actuators include a steering wheel, a drive motor, and a brake.

[0030] Next, an example of the operation of the trajectory planning device 200 according to the first embodiment will be described with reference to the flowchart shown in Fig. 6. In the following, a case where a particle filter is used will be described. In addition, in the following, "one step" refers to one sampling time in a control period.

[0031] First, as input information for the trajectory planning device 200, target values ​​such as target position, target speed, and target azimuth angle are acquired from the target value acquisition unit 110, self-states such as the position, speed, and azimuth angle of the moving body are acquired from the self-state acquisition unit 120, and surrounding environment information such as the coordinates of each vertex of the travelable area, and the positions and speeds of moving obstacles are acquired from the surrounding environment acquisition unit 130 (step S101). A conceptual diagram of the input information at this time is shown in Fig. 7.

[0032] In FIG. 7, the coordinates of the multiple vertices VTA that define the drivable area TA are x-coordinates: f1 ,x f2 ,x f3 x fi and the y coordinate is y f1 ,y f2 ,y f3 y fi It is expressed as:

[0033] The target state quantity TG is the coordinate of the target position (x t ,y t ), target azimuth θ t , target speed v t The current self-state of the moving object 1 is expressed as x coordinate x e , the y-coordinate is y e , the speed is v e , and the azimuth angle is θ e It is expressed as:

[0034] In addition, the coordinates of each moving obstacle MOB are x coordinate xO1 ,x O2 ,x O3 x Oi and the y-coordinate is y O1 ,y O2 ,y O3 y Oi and the velocity is v O1 ,v O2 ,v O3 v Oi and the azimuth angle is θ O1 ,θ O2 ,θ O3 θ Oi It is expressed as:

[0035] In FIG. 7, a part of the drivable area TA is missing due to the presence of an obstacle, and multiple vertices VTA are shown along the contour of the obstacle.

[0036] In this embodiment, the vertices of the travelable area TA are extracted as information on the travelable area TA, but information on lines such as circles or ellipses can also be used.

[0037] In addition, by using the self-state obtained from the self-state acquisition unit 120, it is also possible to use values ​​converted into a moving body coordinate system in which the position of the moving body 1 is the origin, the direction of the moving body 1 is the x-axis, and the direction perpendicular to the direction of the moving body is the y-axis, as shown in Fig. 8. In the following, it is assumed that values ​​in this moving body coordinate system are used.

[0038] As shown in Fig. 8, in the moving body coordinate system, the coordinates of multiple vertices VTA are f1 ,X f2 ,X f3 X fi and the Y coordinate is Y f1 ,Y f2 ,Y f3 Y fi It is expressed as:

[0039] The target state quantity TG is the coordinate of the target position (X t ,Y t ), target azimuth Θ t , target speed V tThe current self-state of the moving object 1 is expressed as the X coordinate of X e , the Y coordinate is Y e , the speed is V e , and the azimuth angle is Θ e It is expressed as:

[0040] In addition, the coordinates of each moving obstacle MOB are X coordinates X O1 ,X O2 ,X O3 X Oi and the Y coordinate is Y O1 ,Y O2 ,Y O3 Y Oi and the speed is V O1 ,V O2 ,V O3 ···V Oi and the azimuth angle is Θ O1 ,Θ O2 ,Θ O3 Θ Oi It is expressed as:

[0041] The following formula (1) is used for coordinate conversion of each value related to the target value information.

[0042]

number

[0043] The following formula (2) is used for coordinate conversion of each value related to the moving obstacle information.

[0044]

number

[0045] The following equation (3) is used for coordinate conversion of each value for each vertex of the drivable area TA.

[0046]

number

[0047] Returning now to the explanation of the flowchart in Fig. 6. After acquiring the surrounding environment information in step S101, the travelable area calculation unit 210 calculates the travelable area TA in which no obstacles exist and in which the mobile object 1 can travel, based on the information acquired from the surrounding environment acquisition unit 130 (step S102). In this embodiment, the X-coordinate X f1 ~X fi and the Y coordinate Y f1 ~Y fi is used to generate the trajectory.

[0048] The X coordinate X of each vertex VTA of the drivable area TA is f1 ~X fi and the Y coordinate Y f1 ~Y fi Instead of using the above for trajectory generation, a drivable area at a time later than the currently calculated drivable area can be predicted based on the time series changes in the shape of the previously calculated drivable area TA, and the predicted drivable area can be combined to form a drivable area that can be used for trajectory generation.

[0049] Fig. 9 is a conceptual diagram showing a method for predicting the drivable area based on the time series change in the shape of the drivable area TA. In Fig. 9, the current time is t as time T, one sampling time before that in the control cycle is t-1, another sampling time before that is t-2, and one sampling time after time t is t+1.

[0050] In FIG. 9, the moving object 1 is moving forward in the direction of the arrow, and the drivable area TA is partially missing due to the presence of an obstacle ahead. From the past drivable area TA at time t-2 and the past drivable area TA at time t-1, and the change over time of the current drivable area TA at time t, it can be seen that there is a part NP1 that changes forward as the moving object 1 moves forward, and a part NP2 that does not change or changes little even if the host vehicle moves forward. The part NP1 is likely to be a stationary obstacle, and the part NP2 is likely to be a moving obstacle. When the future drivable area at time t+1 is predicted from such information, the hatched area in the rightmost diagram of FIG. 9 becomes the predicted drivable area ETA, and the current drivable area TA can be extended by combining the current drivable area TA at time t with the predicted drivable area ETA.

[0051] By using the predicted free space ETA, it is possible to generate a trajectory for an area farther away than the free space currently recognized by the external sensors. This area is likely to become a free space in the future.

[0052] In addition, a predicted travelable area can be calculated based on the type of obstacle outside the travelable area TA and used for trajectory generation.

[0053] FIG. 10 is a conceptual diagram showing a method for predicting a drivable area based on the type of obstacle. The types of obstacles include, for example, a wall, a stationary vehicle, and a moving vehicle. When the obstacle is a moving vehicle, the surrounding environment acquisition unit 130 acquires not only the position but also the speed of the moving vehicle. Then, the drivable area calculation unit 210 calculates a predicted drivable area based on the position, speed, and the like of the moving vehicle. Therefore, the predicted drivable area includes an area that is not included in the currently calculated drivable area. This means that even if an area is currently determined to be non-drivable, the area will be determined to be drivable in the future.

[0054] In Fig. 10, velocity v eThe left diagram shows the current travelable area TA of the mobile unit 1 at the current time t, and the right diagram shows the area TA from the current time t x The figure shows the predicted driving area TAX for the future.

[0055] In the left diagram of Fig. 10, the maximum recognition distance of the driving area TA is L max The dashed line indicates the area NR beyond the farthest part of the drivable area TA, where there are no obstacles. Also, the stationary obstacle SOB and the speed v O Due to the presence of moving obstacles MOBs, the drivable area TA is partially missing.

[0056] In the right diagram of FIG. 10, the point where there is a stationary obstacle SOB ahead is an area R1 that will not be drivable in the future, and the point where there is a moving obstacle MOB ahead is an area where the drivable area will extend in the direction of the moving obstacle MOB in the future, and the drivable area TA is extended to an extended area R2. The extension length is (v O -v e )×t x In addition, the maximum recognition distance L max Once the vehicle reaches the point where the vehicle is in the running state, it is assumed that the vehicle can continue running beyond the point, and the running area TA is extended to the extended area R3. The extended length is v e ×t x It is.

[0057] By using the predicted drivable area TAX, it is possible to generate a trajectory for a position farther away than the drivable area currently obtained by the external sensors. The drivable area is predicted based on the actual movement of obstacles, so the reliability is high. Furthermore, the drivable area TA can be predicted not only based on the actual movement of obstacles, but also by taking into account the time-series changes in the shape of the drivable area TA as shown in Figure 9. This further increases the reliability.

[0058] In addition, in the calculation of the travelable area TA in step S102, if there is an area that is travelable but may be a dead end, a process is performed to remove the area from the travelable area TA as necessary. Figure 11 is a conceptual diagram explaining this process.

[0059] 11 shows a scene in which there is a narrow passage such as an ETC gate ahead of the moving object 1, and the target position TGP is inside the passable gate. The other gates are not passable and are dead ends DE where future travel is not possible, and are removed from the passable area TA as removed areas AR.

[0060] For example, a region that may be a dead end is a place where the value in the x-axis direction is almost the same as the target position TGP, but there is a discrepancy in the value in the y-axis direction. Alternatively, when an overhead view or an aerial photograph such as that shown in FIG. 11 is obtained, an image processing technique is used to detect a region where there is a wall between the target position TGP and the target position TGP, or a region where the target position TGP is surrounded by a wall. When infrastructure information is used to obtain location information such as an ETC gate that is under construction and cannot be used, the impassable region is compared with the drivable region TA, and the region in the drivable region TA where the mobile object 1 cannot travel in the future is determined to be a potential dead end.

[0061] It is possible to generate a trajectory that avoids a situation in which the vehicle becomes stuck at a dead end before reaching the target position TGP, even within the travelable area TA.

[0062] In addition, in the calculation of the travelable area TA in step S102, if there is an area that is travelable but entry is prohibited, a process is performed to remove the area from the travelable area TA as necessary. Fig. 12 is a conceptual diagram explaining this process.

[0063] 12 shows a scene in which a no-entry area IPA is set up ahead of the moving object 1. An example of a no-entry area IPA is an area under construction that is not surrounded by obstacles such as fences. The area where the travelable area TA and the no-entry area IPA overlap is removed from the travelable area TA as a removed area AR.

[0064] When an overhead view or aerial photograph such as that shown in Fig. 12 is obtained, the no-entry area IPA detects areas that are under construction or other areas that cannot be entered by using image processing technology, for example, using a front camera attached to the moving object 1. When location information of areas where construction is taking place is obtained by utilizing infrastructure information, the areas under construction or other areas are compared with the drivable area, and the areas under construction or other areas in the drivable area TA are designated as no-entry areas IPA.

[0065] In addition, in the calculation of the travelable area TA in step S102, if there is a crossing prohibition line that prohibits crossing, a process is performed to allow travel only in the area where the travelable area TA and the crossing prohibition line overlap. Fig. 13 is a conceptual diagram explaining this process.

[0066] 13 shows a scene in which no-crossing lines NSL are provided on the left and right of the moving object 1 in the traveling direction. Examples of no-crossing lines NSL include solid white lines on the road surface that prohibit crossing the line, and solid yellow lines that prohibit crossing the line for overtaking. They also include rules such as not changing lanes 30 meters from an intersection, and not changing lanes just before an ETC gate. The area surrounded by the drivable area TA and the no-crossing lines NSL is removed from the drivable area TA as a removed area AR, and only the area where the drivable area TA and the no-crossing lines overlap is drivable.

[0067] 11 to 13, the travelable area calculation unit 210 predicts areas in which the moving body 1 cannot travel in the future based on the surrounding environment information of the moving body 1, and calculates the area excluding this area as the travelable area TA. Note that the areas in which the moving body 1 cannot travel in the future are not limited to the areas shown in FIGS.

[0068] Returning now to the explanation of the flowchart in Fig. 6. After the travelable area TA is calculated in step S102, the target state calculation unit 220 calculates a target state quantity that the moving object 1 should reach based on the target value acquired by the target value acquisition unit 110 (step S103). A schematic diagram of the target state quantity in this embodiment is shown in Fig. 14.

[0069] As shown in FIG. 14, the target position (X t ,Y t ), target speed V t , target azimuth Θ t The target lateral position Y is the position perpendicular to the target azimuth direction of the target position calculated using L , target speed V t , target azimuth Θ t In addition, the distance D between the moving obstacle MOB in the danger area D O Target distance D t In order to achieve the above, the target vehicle distance D t is included in the target state quantity, and the target state quantity P t is expressed by the following equation (4).

[0070]

number

[0071] The target state quantity is the target speed V t By including the above, it is possible to generate a trajectory that allows driving at any speed, such as observing the legal speed limit.

[0072] The target state quantity is the target azimuth angle Θ t By including the above, for example, when entering a narrow passage ahead, such as in a scene of passing through a toll gate, a trajectory can be generated in which the vehicle enters the gate straight by setting the azimuth angle perpendicular to the gate as the target state quantity.

[0073] Target state quantity is set to target lateral position Y LBy including the above, for the moving body 1 which cannot move straight sideways, only the lateral component of the target position is set as the target state quantity, so that the lateral deviation from the target position can be reduced early and the moving body 1 can quickly pass the target position at the target azimuth angle.

[0074] The danger area D shown in Fig. 14 is defined as the safe distance to be maintained between the moving vehicle 1 and the dynamic preceding vehicle and the stationary obstacle, and the distance that people and other vehicles should not approach because the possibility of collision increases when the moving vehicle 1 is moving and the damage caused by the collision is large and dangerous. In Fig. 14, a moving obstacle MOB exists within the danger area D, and the distance D between the moving obstacle MOB and the moving obstacle MOB is defined as the safe distance to be maintained between the moving vehicle 1 and the dynamic preceding vehicle and the stationary obstacle. O is the target distance D t It is smaller than.

[0075] In addition, the target state quantity P t is the target position (X t ,Y t ) can also be expressed as the following equation (5).

[0076]

number

[0077] In addition, the target state quantity P t is at least the target position (X t ,Y t ) is included, the target speed V t , target azimuth Θ t and target distance D t This means that the target position (X t ,Y t ) is included, the trajectory generating unit 260 can generate a trajectory that can reach the target position.

[0078] Furthermore, when the target state quantity is outside the travelable area TA, the target state calculation unit 220 can also perform processing to set the state quantity in the travelable area TA that is closest to the target state quantity as the target state quantity. Figure 15 is a conceptual diagram explaining this processing.

[0079] In FIG. 15, the target position (X t ,Y t ) is used, and the original target position OTGP is outside the drivable area TA. The target position within the drivable area TA that is closest in a straight line to the original target position OTGP is near the right corner of the drivable area TA, so the target position TGP is set here.

[0080] By performing such processing, the generated trajectory is more likely to be set within the travelable area TA compared to when the target position TGP is outside the travelable area TA, thereby enabling safe travel.

[0081] Furthermore, the target state calculation unit 220 can also set an upper limit value for at least the target state quantity related to speed among the target state quantities, according to the shape of the travelable area TA calculated by the travelable area calculation unit 210. For example, when the travelable area TA is narrow and the recognition distance of the travelable area TA is short, such that high-speed travel is not possible, an upper limit value can be set for the target state quantity related to speed.

[0082] FIG. 16 is a conceptual diagram showing a scene in which the travelable area TA is narrow. As shown in FIG. 16, stationary obstacles SOB are present close to the left and right of the moving body 1 along the traveling direction of the moving body 1. The maximum recognition distance of the travelable area TA is L max1 , which is as long as the length of the stationary obstacle SOB, but its width is narrow.

[0083] In such a case, by setting an upper limit value for the target state quantity related to the target position or the target azimuth angle, the generated trajectory is more likely to be set within the driving area TA, making it possible to drive the moving body 1 without imposing a mental burden on the occupants.

[0084] 17 is a conceptual diagram showing a scene in which the recognition distance of the travelable area TA is short. As shown in FIG. 17, the recognition distance L max2 The maximum recognition distance L of the travelable area TA shown in Fig. 16 max1 It is now much smaller and cannot be seen for long distances.

[0085] Such a scene is expected when the moving body 1 is a towing tractor and there is an airplane to be towed in front of it, when there is a wall close to the front, or when the forward detection distance of the camera sensor attached to the moving body 1 is short.

[0086] In such a case, by setting an upper limit value for the target state quantity related to the speed, the generated trajectory is more likely to be set within the travelable area TA, making it possible for the moving body 1 to travel without imposing a mental burden on the occupants.

[0087] Returning now to the explanation of the flowchart in Fig. 6, after the target state quantity is calculated in step S103, the state prediction unit 230 predicts N based on the current state of the moving object. p The number of particles is defined (step S104). p Each particle has a different state. N p is an integer equal to or greater than 2. In this embodiment, the state quantity P of the particle is expressed as the two-dimensional position X p and Y p , azimuth Θ p , velocity V p , steering angle δ p , acceleration a p and steering angle velocity u p and is expressed by the following formula (6).

[0088]

number

[0089] where the two-dimensional position X p and Y pand azimuth angle Θ p Let be expressed in the coordinate system of Fig. 8 based on the current position and current azimuth angle of the moving object 1. In addition, the state quantity of the n-th particle is P n The initial values ​​of the state variables for all particles are the same, and the two-dimensional position X p and Y p and azimuth angle Θ p The initial value of is 0, and the speed V p The initial value of is the current speed of the moving object 1, the steering angle δ p The initial values ​​of are the steering angle and acceleration a of the current moving object 1. p and steering angle velocity u p The initial value of is 0. A weight W is defined for each particle, and the initial value of the weight W is the same for all particles and is expressed by the following formula (7). At time T p is defined and set to an initial value of 0.

[0090]

number

[0091] The number of particles can be varied according to the shape and area of ​​the travelable area TA, and can also be varied according to the degree of deviation from the target state quantity.

[0092] After the particles are defined in step S104, the state prediction unit 230 inputs random numbers corresponding to the number of particles using uniform random numbers to each particle, thereby calculating the discrete time width T d The future state quantity is predicted (step S105). A method for predicting the state of the particle will be described below.

[0093] The state variables of the particle are predicted using a system model. The model used in this embodiment is explained below. The state variables of the system model are the two-dimensional position X p and Y p , azimuth Θ p , velocity V p and steering angle δ p The state quantity is expressed by the following equation (8).

[0094]

number

[0095] In addition, the input value P u is composed of the vehicle acceleration a and steering angular velocity u, and is expressed by the following equation (9).

[0096]

number

[0097] Moreover, the sideslip angle β of the moving body 1 is expressed by the following formula (10).

[0098]

number

[0099] Here, the system model is expressed as a differential equation by using the wheelbase L of the moving body 1, as shown in the following formula (11).

[0100]

number

[0101] It should be noted that the above-described system model can be said to be a kinematic model that approximates four wheels to two wheels and does not take dynamics into account, but other vehicle motion models can also be used, such as a two-wheel model, which is a dynamic model that approximates four wheels to two wheels.

[0102] Among the input variables to the system model, acceleration a is set to an arbitrary upper limit a mx and any lower limit a mn A value that satisfies the following formula (12) is determined for each particle using uniform random numbers.

[0103]

number

[0104] In addition, among the input variables to the system model, the steering angle speed u is limited to a preset upper limit u mx For (>0), the first constraint condition for the steering angular velocity u is to satisfy the following formula (13).

[0105]

number

[0106] In addition, the upper limit value δ mx For (>0), the discrete time span T d The second constraint condition for the steering angle velocity u is that the rear steering angle δ' satisfies the following formula (14).

[0107]

number

[0108] discrete time width T d The rear steering angle δ' is expressed by the following equation (15).

[0109]

number

[0110] Therefore, the second constraint condition is expressed by the following formula (16).

[0111]

number

[0112] Input value P to the system model u Among these, the steering angle velocity u is determined to have a value that satisfies the first constraint condition and the second constraint condition by using uniform random numbers for each particle.

[0113] As described above, the steering angle upper limit δmx The input value P determined based on u Using the above system model, the discrete time span T d The state quantity P after x This makes it possible to predict the state of particles while taking into account constraints.

[0114] The particle state quantity is the predicted state quantity P x ' and the input value P u The update is expressed by the following formula (17). d The added value is the updated time.

[0115]

number

[0116] Time T d The predicted state of each particle after nx ', and the discrete time span T d A conceptual diagram of future state prediction is shown in FIG. 18. In FIG. 18, there is a time T d The predicted state quantity P of the next n particles nx In this way, by predicting the state quantity using multiple particles, it is possible to evaluate whether the particle is inside or outside the drivable area TA based on the positional relationship between the point of each particle and the drivable area TA, so that a trajectory can be generated that can be guaranteed to be within the drivable area TA.

[0117] In the calculations in state prediction unit 230, the input is determined using uniform random numbers following a uniform distribution to provide a random input, but normal random numbers following a normal distribution or random inputs following other distributions can also be used.

[0118] In addition, in the calculations in the state prediction unit 230, the input is set so as not to exceed an arbitrary upper limit value set in advance, but the input can also be a value that varies according to the shape of the drivable area TA.

[0119] FIG. 19 is a conceptual diagram for explaining a case where an input is set according to a narrow and long shape of the travelable area TA. As shown in FIG. 19, if the travelable area TA has a vertically long shape, the predicted state quantity P x An upper limit value of the input of the steering angle velocity u is set so that the input does not fall outside the travelable area TA as shown in the figure, or the distribution of the input of the steering angle velocity u is narrowed. By performing such processing, it becomes easier to generate a predicted state quantity within the travelable area TA.

[0120] Furthermore, when the travelable area TA has a narrow and long shape, the amount of calculation can be reduced by reducing the number of inputs.

[0121] FIG. 20 is a conceptual diagram for explaining a case where an input is set according to the shape of the travelable area TA that is horizontally long. As shown in FIG. 20, if the travelable area TA is horizontally long, the predicted state quantity P x Either set an upper limit value for the input of acceleration a so that the acceleration a does not fall outside the travelable area TA as shown in the figure, or widen the distribution of the input of acceleration a, or increase the number of inputs. By performing such processing, it becomes easier to generate a predicted state quantity within the travelable area TA.

[0122] Furthermore, when the travelable area TA has a horizontally long shape, a more suitable trajectory can be obtained by increasing the number of inputs.

[0123] In addition, the state prediction unit 230 can increase the number of input values ​​to the motion model of the moving body 1 when the deviation between the current state quantity and the target state quantity of the moving body 1 is large, and can decrease the number of input values ​​when the deviation is small. Fig. 21 is a conceptual diagram for explaining the setting of input values ​​when the deviation between the current state quantity and the target state quantity is large.

[0124] As shown in FIG. 21, the target state quantity Θ of the azimuth angle t and the azimuth angle Θ at the current position of the mobile unit 1. e If the deviation from the target position is large and the steering angle must be increased in order to reach the target position, the steering angle velocity is added as an input value to the motion model, and the trajectory is predicted so that the steering angle velocity increases.

[0125] In this case, the distribution of the steering angle is made wider. In other words, the range of the upper and lower limit values ​​of the steering angle is made wider. Also, by increasing the number of input values, a more suitable trajectory is more easily obtained. This makes it easier to reach the target position even if there is a large deviation between the current state quantity of the moving body 1 and the target state quantity. On the other hand, when the deviation is small, the number of input values ​​can be reduced to reduce the amount of calculation and the calculation load. In this way, by varying the number of input values ​​according to the degree of deviation between the current state quantity and the target state quantity, it is possible to generate a smooth trajectory of the moving body 1 and reduce the calculation load.

[0126] Returning now to the explanation of the flowchart in Fig. 6, after the state quantity of each particle is predicted in step S105, the predicted state evaluation unit 240 obtains an observation value from the updated state quantity of each particle (step S106). The observation variables are defined based on the target state quantity calculated by the target state calculation unit 220. In this embodiment, the goals are to reach a target lateral position, which is a position in a direction perpendicular to the target azimuth direction at the target position, to maintain a target speed, to reach a target azimuth, and to maintain a safe distance from moving obstacles. Based on these goals, the particle and the target lateral position Y L Deviation from L e , the particle velocity state quantity V p , the particle's azimuth angle state quantity Θ p , Approach distance D to danger area D p is the observed variable, and the observed value P y is expressed by the following formula (18).

[0127]

number

[0128] FIG. 22 shows the observed variables. In FIG. 22, the current moving object 1 is d Future predicted state quantity P x Particle P with ' d The particle P is shown diagrammatically. dA danger zone D is set based on the position and azimuth of the obstacle MOB, and a moving obstacle MOB is entering the danger zone D. p is the particle P d Azimuth angle Θ p It is specified by the distance in a direction parallel to the

[0129] Here, the danger area D is the particle P d Azimuth angle Θ p This is a rectangular region with its long side tilted in the direction of the particle P. d Distance D ahead of x , distance D to the left and right y Here, the distance D x is the particle P d The speed V and the pre-set safety time T s Using this, it is expressed by the following formula (19).

[0130]

number

[0131] Also, the distance D y is the preset parameter T sy Using this, it is expressed by the following formula (20).

[0132]

number

[0133] After the observation value of each particle is obtained in step S106, the predicted state evaluation unit 240 calculates the observation value P y and the ideal observation value P yi The weight W of each particle is updated based on the difference between the ideal observed value P yi is an observation value for the moving object 1 in a virtually set target state quantity, and when the moving object 1 satisfies the target state quantity, the moving object 1 is in an ideal state. In this embodiment, the ideal observation value P yiis the deviation L from the target lateral deviation based on the target state quantity. enom , target vehicle speed V nom , target azimuth Θ tnom and target approach distance D nom and is expressed by the following formula (21).

[0134]

number

[0135] If the particle position is outside the travelable area TA calculated in step S102, the weight of each particle is set to 0 or a value lower than that of particles inside the travelable area TA. The inside / outside determination of the travelable area TA is performed, for example, by determining whether the particle's two-dimensional position X p and Y p The determination is made based on whether or not there is a

[0136] Here, in the above, the weight of particles outside the travelable area TA is set to 0, but the weight assigned to particles outside the travelable area TA can also be made variable depending on the degree of deviation from the travelable area TA.

[0137] Fig. 23 is a conceptual diagram of a process for varying the weighting given to particles outside the travelable area TA. In Fig. 23, a particle PW4 in the travelable area TA ahead of the moving body 1 is given a weighting W4, and the travelable area TA can be called a weighting W4-given area RW4.

[0138] Moreover, outside the weight W4-applied region RW4, a weight W3-applied region RW3 is set, and a particle PW3 present therein is assigned a weight W3. Further outside the weight W3-applied region RW3, a weight W2-applied region RW2 is set, and if a particle is present there, a weight W2 is assigned. Further outside the weight W2-applied region RW2, a weight W1-applied region RW1 is set, and a particle PW1 present therein is assigned a weight W1. The weight W4 is the heaviest, followed by weights W3, W2, and W1 in that order.

[0139] In this way, by not deleting particles (prediction points) outside the drivable area TA and evaluating the predicted state quantity according to the distance from the drivable area TA, it is possible to leave as trajectory candidates even areas that are actually within the drivable area TA but are recognized by the external sensor as being outside the drivable area TA due to the limit of the recognizable range of the external sensor, and it is possible to evaluate the prediction points for the remaining trajectory candidates according to the distance from the drivable area TA, i.e., the reliability, making it easier to succeed in trajectory planning. In addition, by giving a lighter weight to particles that are farther away from the drivable area TA, it is possible to reduce the possibility that a trajectory will be generated based on that particle, and to increase the safety of the generated trajectory.

[0140] Also, in FIG. 23, the weights assigned to the particles are discontinuous values, but the weights assigned may be values ​​that change continuously depending on the distance from the travelable area TA.

[0141] Furthermore, for particles within the travelable area TA, the weighting given to the particles can be made variable depending on the distance from the boundary that defines the travelable area TA.

[0142] Fig. 24 is a conceptual diagram of a process for varying the weights assigned to particles in a travelable area TA. In Fig. 24, in the travelable area TA ahead of the moving body 1, a weighted area RW1 assigned with a weight W1, a weighted area RW2 assigned with a weight W2, a weighted area RW3 assigned with a weight W3, and a weighted area RW4 assigned with a weight W4 are set in this order from the boundary side defining the travelable area TA toward the inside. The process in each weighted area is the same as the process described using Fig. 23, and the weight W4 is the heaviest, followed by weights W3, W2, and W1 in that order.

[0143] In this way, by assigning weights to the predicted state quantities within the drivable area TA that decrease as the quantity approaches the boundaries that define the drivable area TA, the weights of the predicted state quantities located in the center of the drivable area TA are higher than those near the boundaries of the drivable area TA, allowing for a trajectory plan that avoids the boundaries of the drivable area TA as much as possible, thereby increasing the safety of the generated trajectory.

[0144] Also, in FIG. 24, the weights assigned to the particles are discontinuous values, but the weights assigned may be values ​​that change continuously depending on the distance from the boundary that defines the travelable area TA.

[0145] In addition, for particles within the travelable area TA, even if the particles are within the travelable area TA, particles predicted to be at a point that may become a dead end if they proceed can be processed so that a weight of 0 or a small weight is assigned. Figure 25 is a conceptual diagram explaining this processing.

[0146] In Fig. 25, a scene is shown in which there is a narrow passage such as an ETC gate ahead of the moving object 1, and the target position TGP is inside the passable gate. Other gates are not passable, and are dead ends DE, which are areas where the moving object 1 cannot pass in the future if it proceeds. The particle PW predicted in this area has a weight of 0 (W = 0).

[0147] In this way, a trajectory can be generated that avoids a situation in which the vehicle becomes unable to move due to coming to a dead end before reaching the target position TGP, even within the travelable area TA.

[0148] In addition, whether or not a particle is likely to be a dead end is determined, for example, if the deviation in the x direction from the target position TGP in Fig. 25 is small and the deviation in the y direction is large, that is, if the particle is displaced in the horizontal direction (y direction) from the target position TGP, it is blocked by an obstacle in front of the target position. Therefore, it is possible to determine whether or not a particle is predicted to be at a point that may be a dead end depending on whether the deviation in the x direction from the target position TGP is small and the deviation in the y direction is large.

[0149] As explained using FIG. 11, dead-end locations can be excluded from the travelable area TA in advance, and particles predicted to be at potential dead-end locations can be assigned a weight W by treating them as being outside the travelable area TA.

[0150] As for particles within the travelable area, if there is a forbidden crossing line within the travelable area, and if the particle calculated in step S105 is at a position that crosses the forbidden crossing line with respect to the trajectory point generated by the trajectory point calculation unit 250 one step before, or with respect to the moving body 1, the weight W of the particle can be set to 0 or a small weight can be assigned. Figure 26 is a conceptual diagram explaining this process.

[0151] Figure 26 shows a scene in which no-crossing lines NSL are set on both the left and right sides of the moving body 1's direction of travel, and a generated trajectory GT consisting of multiple trajectory points TP is set within the driving lane defined by the left and right no-crossing lines NSL. Some of the particles currently being evaluated by the predicted state evaluation unit 240 are particles PW predicted to be in a position that crosses the no-crossing lines NSL relative to the trajectory points TP generated by the trajectory point calculation unit 250 one step ago, and have a weight of 0 (W = 0).

[0152] In this way, by setting the weight of a particle predicted to be at a position that crosses the prohibited crossing line NSL relative to the trajectory point TP to 0, it is possible to prevent a trajectory from being set in an area where the moving body 1 cannot travel in the future, thereby increasing the safety of the generated trajectory.

[0153] As explained using Figure 13, the area surrounded by the travelable area TA and the no-crossing line NSL can be excluded from the travelable area TA as a removal area AR, and a weight W can be assigned to particles predicted at that point as being outside the travelable area TA.

[0154] Furthermore, for particles outside the currently recognized drivable area, if there is a predicted drivable area, if the particle is located in the predicted drivable area, the weight W of the particle can be processed so that it is not set to 0, and a weight smaller than the weight of particles within the currently recognized drivable area can be assigned. Figure 27 is a conceptual diagram explaining the weighting process for particles within the predicted drivable area.

[0155] In FIG. 27, a predicted drivable area ETA is set beyond the currently recognized drivable area TA, and a weight W1 is assigned to a particle PW1 that is within the predicted drivable area ETA. A weight W2 is assigned to a particle PW2 that is within the drivable area TA. Here, the weight W1 is smaller than the weight W2 (W1 <W2)。

[0156] By using the predicted drivable area ETA, it is possible to generate a trajectory to a position farther away than the drivable area TA currently recognized by the external sensor. Also, since the reliability of the predicted drivable area ETA is not high, a highly reliable trajectory can be generated by relatively increasing the weight of the predicted state quantity within the drivable area TA currently recognized by the external sensor.

[0157] Here, in step S105, the discrete time width T dFor particles whose subsequent state quantities have been predicted, the predicted trajectory of a moving obstacle obtained from the surrounding environment acquisition unit 130 is used to determine whether a predicted obstacle exists in the area around each particle at the same time, i.e., at the same time as the predicted time of the particle, using the predicted trajectory. If an obstacle exists, the weight W of the particle is set to 0.

[0158] The weight W of each particle before update is W p The weight W is proportional to the weight before the update and the likelihood LLV, and is updated so that the integrated value of the weights of all particles becomes 1 using the following formula (22).

[0159]

number

[0160] Here, the likelihood LLV is the state quantity P x Covariance matrix Q and observed values ​​P y Using the covariance matrix R for

[0161]

number

[0162] Here, the matrix Π is expressed by the following equation (24).

[0163]

number

[0164] where the value H of the measurement matrix H for the nth particle is n , P x =P xn Then, the measurement function h is the state quantity P x and is expressed by the following equation (25).

[0165]

number

[0166] In addition, the measurement function h is the state quantity P x From the observed value P y This is a function to calculate the following equation (26).

[0167]

number

[0168] Regarding the weight update of each particle based on the difference between the observed value of each particle and the ideal observed value, it is also possible to set the evaluation weight, which is the degree to which an item is to be evaluated with emphasis, based on the difference between the moving body 1 and the target state quantity or on the application scene. Figure 28 is a conceptual diagram explaining the process of setting the evaluation weight.

[0169] In FIG. 28, the moving object 1 is at a target lateral position Y L and the target lateral position Y L In the area IA1, where it is important to reduce the deviation from the target lateral position Y L The evaluation weights are set so that the likelihood of a particle with a small deviation to is increased.

[0170] With such settings, for example, in a scene of passing through a toll gate, when the target position is the gate that the vehicle wishes to pass through and it is necessary to pass through the gate perpendicularly, by placing more importance on the difference with the target state quantity related to the horizontal position than on the difference with the target state quantity related to the vertical position, it is possible to align the horizontal position before aligning the vertical position, and generate a trajectory that enables the vehicle to quickly reach a position where it can enter perpendicularly to the gate.

[0171] In addition, in FIG. 28, the azimuth angle Θ of the moving body 1 e and the target azimuth angle Θ in the target state TG t The deviation from the target azimuth angle Θ tIn the area IA2, where it is important to reduce the deviation from the target azimuth angle Θ t The evaluation weight is set so that the likelihood of a particle with a small deviation from the target is increased. The evaluation weight is set by increasing the weighting coefficient for particles with a smaller deviation.

[0172] With such settings, for example, in a scene of passing through a toll gate, when the target position is the gate that the vehicle wishes to pass through, the weight of the evaluation of the state quantity related to the azimuth angle is lowered at points far from the target position, and the weight of the evaluation of the state quantity related to the azimuth angle is increased at points close to the target position. This makes it possible to generate a trajectory in which the state quantity related to the azimuth angle of the trajectory has a high degree of freedom at points far from the target position, and the state quantity related to the azimuth angle is adjusted as the vehicle approaches the target position.

[0173] Returning now to the description of the flowchart in Fig. 6, after updating the weight of each particle in step S107, the predicted state evaluation unit 240 resamples the particles based on the weight of each particle (step S108). However, in order to prevent a significant decrease in the number of particles, the effective number of particles N eff Resampling is performed only if Nthr is greater than or equal to the threshold, otherwise nothing is done in this step.

[0174] Here, the effective number of particles is N eff is expressed by the following equation (27).

[0175]

number

[0176] In resampling, as in normal particle filters, sampling is performed at equal intervals from the empirical distribution function. When resampling is performed, the weights of each particle are reset based on the following formula (28), assuming that the weights of each particle are equal.

[0177]

number

[0178] After the particle resampling is performed in step S108, the trajectory point calculation unit 250 calculates a weighted average value for the particle position and velocity in the trajectory generation unit 260 based on the weight calculated by the predicted state evaluation unit 240, and the weighted average point composed of at least the position data and the velocity data is stored as a trajectory point, for example, in a memory (not shown) in the trajectory generation unit 260 (step S109).

[0179] Also, based on the weight calculated by the predicted state evaluation unit 240, the particle with the largest weight, that is, the largest weighting coefficient, can be set as the trajectory point.

[0180] Further, the trajectory points are not limited to position data and speed data, but may also include azimuth data, steering angle data, and the like.

[0181] Also, a trajectory point may be a point having only position data.

[0182] After storing the trajectory points in step S109, it is determined whether or not the time T reaches the planning horizon Tthr (step S110). If the time is less than the planning horizon (if No), the processes from step S104 onwards are repeated. If the time T is equal to or greater than the planning horizon Tthr (if Yes), the sequence of trajectory points, which are composed of position data and velocity data for each discrete time and which the trajectory generating unit 260 has stored as trajectory points, is output to the motion control unit 300 as a generated trajectory.

[0183] A conceptual diagram of the process of repeating the calculations from step S104 to step S110 until the time reaches or exceeds the planning horizon Tthr to obtain multiple trajectory points is shown in Fig. 29. In Fig. 29, the number of particles is set to four, and the particles in the figure represent the state after resampling.

[0184] As shown in Fig. 29, within the drivable area TA, there is a particle group G1 at time T = T1, a particle group G2 at time T = T2, and a particle group G3 at time T = T3, and at time T = T3, the planned horizon Tthr is exceeded. The weighted average state quantities of the four particles in each of the particle groups G1 to G3 are the trajectory points TP1, TP2, and TP3. The sequence of these trajectory points becomes the generated trajectory, so that plausible trajectory points can be generated based on the evaluation by the predicted state evaluation unit 240.

[0185] Moreover, the same effect as above can be obtained by setting the particle with the largest weight, that is, the largest weighting coefficient, in each of the particle groups G1 to G3 as the trajectory point.

[0186] An example of trajectory generation until the target state quantity is reached using the trajectory planning device 200 of this embodiment described above will be described with reference to Figs. 30 and 31. Figs. 30 and 31 are schematic diagrams showing an example of trajectory generation, and the left diagram of Fig. 30 shows a generated trajectory GT1 when the moving body 1 is located at a position far from the target state quantity TG. The right diagram of Fig. 30 shows a generated trajectory GT2 at a position slightly further forward than the left diagram, using the generated trajectory GT1 in the left diagram, by moving forward along the generated trajectory GT1. The left diagram of Fig. 31 shows a generated trajectory GT3 at a position slightly further forward than the right diagram of Fig. 30, using the generated trajectory GT2 in the right diagram of Fig. 30. The right diagram of Fig. 31 shows a generated trajectory GT4 after moving forward along the generated trajectory GT3 in the left diagram and reaching the target state quantity TG, using the generated trajectory GT3 in the left diagram.

[0187] As described above, according to the trajectory planning device 200 of the first embodiment, a trajectory to a target state quantity is evaluated based on the target state quantity including the state quantity of the position of the moving body 1 and the driveable area, and a trajectory is generated based on the evaluation result. Therefore, even if the driveable area is complex, it is possible to reach the destination through the driveable area.

[0188] <Embodiment 2> Fig. 32 is a block diagram showing an example of a schematic configuration of a moving body 1 equipped with a trajectory planning device according to embodiment 2. In Fig. 32, the same components as those of the moving body 1 described using Fig. 1 are denoted by the same reference numerals, and duplicated descriptions will be omitted.

[0189] 32, the configuration of a trajectory planning device 200A that generates a trajectory to be taken by the moving body 1 is different from that of the trajectory planning device 200 of the embodiment 1. That is, the trajectory planning device 200A of the embodiment 2 generates a trajectory by calculating a polynomial that passes through the current position and the target position of the moving body 1 in a state prediction unit 230 instead of using a particle filter for state estimation, and therefore differs from the trajectory planning device 200 in that it does not have a trajectory point calculation unit 250.

[0190] Regarding the method of calculating the polynomial trajectory connecting the moving object 1 and the target state quantity TG, the target state quantity TG is calculated as the target position (x g , y g 33 is a conceptual diagram for explaining a method of deriving a polynomial connecting the moving object 1 and the target position TG.

[0191] As shown in FIG. 33, if the position of the moving body 1 is the origin, the direction of the moving body 1 is the x-axis, and the direction perpendicular to the direction of the moving body is the y-axis, the polynomial y=f(x) connecting the moving body 1 and the target position TG is given by the following equation (29).

[0192]

number

[0193] Then, the polynomial is constrained such that it must pass through the position of the moving body 1 (origin) and the target position TG, and that the direction must be along the x-axis (angle 0 degrees) at the position of the moving body 1, and that it must face a specified direction (angle) at the target position.

[0194] This can be expressed mathematically as follows: By solving the simultaneous equations for the boundary conditions, that is, the conditions for the moving body position and the target position, each coefficient c iThe simultaneous equations are expressed by the following equations (30) and (31). Here, x0 is the x-coordinate value of the moving object position, x g is the x-coordinate value of the target position.

[0195]

number

[0196]

number

[0197] Next, a case where the polynomial is the following equation (32) will be described.

[0198]

number

[0199] For example, if the function value (f(x0)) at the position of the moving object is the y coordinate value of the moving object 1, the function value (f'(x0)) representing the inclination at the position of the moving object is the azimuth angle of the moving object 1, and the function value (f''(x0)) representing the curvature at the position of the moving object is given a boundary condition of 0, the coefficients C0, C1, and C2 can be uniquely determined and the remaining x 3 By randomly assigning the terms, a polynomial orbit with the number of random terms is created, as shown in Figure 34.

[0200] FIG. 34 shows three patterns of polynomial trajectories PT1, PT2 and PT3 for obtaining a polynomial trajectory connecting the moving object 1 and the target position TG while avoiding a plurality of obstacles OB when there are a plurality of obstacles OB.

[0201] The polynomials giving the polynomial trajectories PT1, PT2 and PT3 are expressed by the following formulas (33), (34) and (35), respectively.

[0202]

number

[0203]

number

[0204]

number

[0205] The above multiple polynomial trajectories are evaluated to determine whether the arrival value to the target position TG and the polynomial trajectory are away from the boundary of the travelable area TA and within the travelable area TA, and the polynomial trajectory with the highest evaluation is determined as the generated trajectory. Taking Fig. 34 as an example, the polynomial trajectory PT3 is determined as the generated trajectory.

[0206] In addition, when the polynomial is the above-mentioned formula (32), the polynomial trajectory can also be obtained by the following method.

[0207] For example, the function value (f(x0)) at the moving body position is the y coordinate value of the moving body 1, the function value (f'(x0)) representing the inclination at the moving body position is the azimuth angle of the moving body 1, and the function value (f(x g )) is the y-coordinate value of the target position TG, and the function value (f'(x g If we give the boundary condition that C3, C2, C1, and C0 are the azimuth angles of the target position TG, we can uniquely determine the coefficients C3, C2, C1, and C0. By randomly changing each coefficient around the determined values ​​of C3, C2, C1, and C0, we can create multiple polynomial trajectories as shown in Figure 35.

[0208] FIG. 35 shows three patterns of polynomial trajectories PT1, PT2 and PT3 for obtaining a polynomial trajectory connecting the moving object 1 and the target position TG while avoiding a plurality of obstacles OB when there are a plurality of obstacles OB.

[0209] The polynomials giving the polynomial trajectories PT1, PT2, and PT3 are expressed by the following formulas (36), (37), and (38), respectively.

[0210]

number

[0211]

number

[0212]

number

[0213] The above multiple polynomial trajectories are evaluated to determine whether the arrival value to the target position TG and the polynomial trajectory are away from the boundary of the travelable area TA and are within the travelable area TA, and the polynomial trajectory with the highest evaluation is determined as the generated trajectory. Taking Fig. 35 as an example, the polynomial trajectory PT1 is determined as the generated trajectory.

[0214] In this way, the trajectory planning device 200A of embodiment 2 obtains a generated trajectory by calculating a polynomial that passes through the current position and the target position of the moving body 1, instead of using a particle filter for state estimation, and therefore does not calculate trajectory points and does not have a trajectory point calculation unit 250.

[0215] Here, an advantage of using the method of obtaining a generated trajectory by calculating a polynomial is that the calculation load is low. That is, one candidate trajectory can be calculated just by solving the simultaneous equations described above, but when using a particle filter, the calculation must be repeated for the length of the trajectory to generate the state transitions of many particles, which results in a high calculation load.

[0216] On the other hand, an advantage of using a method of obtaining a generated trajectory using a particle filter is that it is possible to generate a trajectory that is guaranteed to be within the drivable area TA. In other words, the particle filter is a method of obtaining one trajectory point using a large number of predicted points (predicted state quantities), but since it is possible to evaluate whether a trajectory point is inside or outside the drivable area TA from the positional relationship between each particle point and the drivable area TA, it is possible to generate a trajectory that can be guaranteed to be within the drivable area TA. On the other hand, in the method of obtaining a generated trajectory by calculating a polynomial, although it is possible to generate a trajectory connecting the moving body 1 and the target position TG, it is not possible to include "being within the drivable area TA" and "avoiding obstacles" in the polynomial formula or to take these factors into consideration, so there is a possibility that the generated trajectory will be outside the drivable area TA.

[0217] Another advantage of using a particle filter to obtain a generated trajectory is that the search range of the trajectory is wide. In other words, in a method of obtaining a generated trajectory by calculating a polynomial, only trajectories that can be expressed by a polynomial can be obtained, resulting in a small search range, but when a particle filter is used, the search range is wide because the predicted points of the state are used as the base, i.e., the trajectory is expressed by points. In addition, nonlinear trajectories can also be obtained, and trajectories that cannot be expressed by a polynomial can also be generated.

[0218] <Weighting Variations> The predicted state evaluation unit 240 of the first embodiment described above assigns a weight to each predicted state quantity, i.e., each particle, to generate a weighted trajectory candidate. At this time, if the predicted state quantity is significantly different from the current state quantity of the moving body 1, or is significantly different from the state quantity of the previously calculated trajectory point, if a trajectory is generated using a trajectory point calculated based on such a predicted state quantity, the trajectory will suddenly change, causing the riding comfort of the moving body 1 to deteriorate.

[0219] Therefore, in the predicted state evaluation unit 240, when the predicted state quantity significantly deviates from the current state quantity of the moving body 1 and when the predicted state quantity significantly deviates from the state quantity of the previously calculated trajectory point, the weighting coefficient is made smaller to prevent the generation of a trajectory that changes suddenly, thereby improving the ride comfort of the moving body 1.

[0220] FIG. 36 shows a conceptual diagram of weighting when the predicted state quantity is significantly different from the current state quantity of the moving object 1. As shown in FIG. 36, the current state quantity of the moving object 1 (x e ,y e ,θ e ,v e ) and a particle group GM that does not deviate so much from the current state quantity of the moving body 1, the predicted state evaluation unit 240 reduces the weighting coefficients of the particles of the particle group GS. On the other hand, the predicted state evaluation unit 240 does not reduce the weighting coefficients of the particles of the particle group GM.

[0221] Here, the degree of deviation between the current state quantity of the moving body 1 and the predicted state quantity (particle) can be determined based on a predetermined threshold value of the difference between the two state quantities; if the difference is greater than the threshold value, the deviation is determined to be large, and if the difference is equal to or less than the threshold value, the deviation is determined to be small.

[0222] Furthermore, the weighting of the particles can be changed by changing the weighting coefficient based on the absolute value of the difference between the current state quantity and the predicted state quantity (particle) of the moving body 1. For example, a weighting coefficient that changes stepwise based on the absolute value of the difference between the two state quantities can be set, and the weighting coefficient can be made smaller stepwise as the difference between the two state quantities increases, and made larger stepwise as the difference between the two state quantities decreases.

[0223] Fig. 37 shows a conceptual diagram of weighting when the predicted state quantity is significantly different from the state quantity of the previously calculated trajectory point. As shown in Fig. 37, when there exists a particle group GS that is significantly different from the state quantity of the previously calculated trajectory point TP2 and a particle group GM that is not so different from the state quantity of the previously calculated trajectory point TP2, the predicted state evaluation unit 240 reduces the weighting coefficient of the particles of the particle group GS. On the other hand, the predicted state evaluation unit 240 does not reduce the weighting coefficient of the particles of the particle group GM.

[0224] Here, the degree of deviation between the state quantity of the previously calculated trajectory point TP2 and the predicted state quantity (particle) can be determined based on a predetermined threshold value of the difference between the two state quantities. If the difference is greater than the threshold value, the deviation is determined to be large, and if the difference is equal to or less than the threshold value, the deviation is determined to be small.

[0225] In addition, the weighting of the particles can be changed based on the absolute value of the difference between the state quantity of the previously calculated trajectory point TP2 and the predicted state quantity (particles). For example, a weighting coefficient that changes stepwise based on the absolute value of the difference between the two state quantities can be set, and the weighting coefficient can be decreased stepwise as the difference between the two state quantities increases, and the weighting coefficient can be increased stepwise as the difference between the two state quantities decreases.

[0226] Each component of the trajectory planning devices 200 and 200A according to the above-described first and second embodiments can be configured using a computer, and is realized by the computer executing a program. That is, the trajectory planning devices 200 and 200A are realized by, for example, a processing circuit 50 shown in FIG. 38. A processor such as a CPU or a DSP (Digital Signal Processor) is applied to the processing circuit 50, and the function of each part is realized by executing a program stored in a storage device.

[0227] Dedicated hardware may be applied to the processing circuit 50. When the processing circuit 50 is dedicated hardware, the processing circuit 50 corresponds to, 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 of these.

[0228] In the trajectory planning devices 200 and 200A, the functions of the respective components may be realized by individual processing circuits, or the functions may be collectively realized by a single processing circuit.

[0229] Also, FIG. 39 shows a hardware configuration in the case where the processing circuit 50 is configured using a processor. In this case, the functions of each part of the trajectory planning devices 200 and 200A are realized by a combination of software, etc. (software, firmware, or software and firmware). The software, etc. are described as a program and stored in the memory 52. ​​The processor 51 functioning as the processing circuit 50 realizes the functions of each part by reading and executing the program stored in the memory 52 (storage device). In other words, it can be said that this program causes a computer to execute the procedure and method of operation of the components of the trajectory planning devices 200 and 200A.

[0230] Here, the memory 52 may be, for example, a non-volatile or volatile semiconductor memory such as RAM, ROM, flash memory, EPROM (Erasable Programmable Read Only Memory), EEPROM (Electrically Erasable Programmable Read Only Memory), HDD (Hard Disk Drive), magnetic disk, flexible disk, optical disk, compact disk, mini disk, DVD (Digital Versatile Disc) and its drive device, or any storage medium to be used in the future.

[0231] The above describes a configuration in which the functions of the components of the trajectory planning devices 200 and 200A are realized by either hardware or software. However, the present invention is not limited to this, and some components of the trajectory planning devices 200 and 200A may be realized by dedicated hardware, and other components may be realized by software. For example, some components may be realized by a processing circuit 50 as dedicated hardware, and other components may be realized by the processing circuit 50 as a processor 51 reading and executing a program stored in a memory 52.

[0232] As described above, the trajectory planning devices 200 and 200A can realize the above-mentioned functions by hardware, software, or a combination of these.

[0233] Although the present disclosure has been described in detail, the above description is illustrative in all respects and does not limit the present disclosure. It is understood that countless variations not illustrated can be assumed without departing from the scope of the present disclosure.

[0234] In addition, within the scope of the present disclosure, it is possible to freely combine the respective embodiments, and to appropriately modify or omit the respective embodiments.

Claims

1. A trajectory planning device for planning a trajectory of a moving object, comprising: Calculating a travelable area of ​​the moving object based on peripheral information of the moving object; A plurality of polynomials passing through the current position of the moving body and the target position of the moving body are generated to form polynomial trajectories; Evaluating the distance between the plurality of polynomial trajectories and the travelable area and evaluating whether the plurality of polynomial trajectories are within the travelable area; A trajectory planning device that selects one of the plurality of polynomial trajectories as a generated trajectory based on an evaluation result, and outputs the generated trajectory to a motion control unit that controls the moving body.

2. The trajectory planning device according to claim 1 , wherein the polynomial is generated based on a boundary condition regarding a state quantity including a current position of the moving object.

3. The trajectory planning device according to claim 2 , wherein the polynomial is generated based on a boundary condition regarding a state quantity including the target position of the moving body.

4. 2. The trajectory planning device according to claim 1, further comprising: a trajectory planning unit that predicts a future drivable area beyond the current drivable area based on time series changes in the shape of past drivable areas calculated before the present, and sets the predicted drivable area as the predicted drivable area; and extends the drivable area by combining the current drivable area with the predicted drivable area.

5. 2. The trajectory planning device according to claim 1, further comprising: a predictor for a future drivable area beyond a current drivable area based on the type of obstacle outside a past drivable area calculated before the present; the predictor for a future drivable area is determined as a predicted drivable area; and the current drivable area is combined with the predicted drivable area to extend the drivable area.

6. 6. The trajectory planning device according to claim 5, wherein, if the obstacle outside the past drivable area is a stationary obstacle, it is determined that the area beyond the stationary obstacle will remain a non-drivable area in the future as well, and if the obstacle outside is a moving obstacle, the drivable area is extended in the future based on the moving direction and moving speed of the moving obstacle, and in an area where there is no obstacle outside, the drivable area is extended based on the speed of the moving body.

7. 2. The trajectory planning device according to claim 1, further comprising: predicting an area in which the moving body cannot travel in the future based on surrounding environment information; and excluding the area to determine the travelable area.

Images