CN114076603B - Trajectory determination method - Google Patents

Abstract

A track determining method comprises the following steps for each piece of track data: (A) Obtaining a shielding flag of a path of the track data, and a collision time and a time vehicle distance of a front obstacle relative to each track point of the path; (B) Obtaining a crossing flag of the path and a crossing collision time of each crossing obstacle relative to each track point of the path; (C) Obtaining a buffer distance of each track point of the path; (D) obtaining a lane change flag for the path; (E) obtaining a security flag for the path; and (F) determining a target track data according to the road speed limit, the shielding flag, the intersection flag, the lane change flag and the safety flag of the path of each track data, and the speed, the collision time, the time vehicle distance, the intersection collision time and the buffer distance of each track point.

Description

Trajectory determination method

Technical Field

The present invention relates to a path decision method applied to a self-driving car, and in particular to a trajectory decision method capable of estimating an optimal driving path according to a current driving environment.

Background technique

In recent years, research on autonomous driving has flourished. Currently, the more mature and commercially available autonomous driving systems on the market mainly travel on fixed routes in closed areas. If they are to be used on roads with mixed traffic of people, cars and motorcycles, the autonomous driving system should have a strong ability to respond to changes in the external environment in order to handle more complex driving conditions and scenarios.

In order to make self-driving cars practically applicable to general road environments, major manufacturers such as Mitsubishi Electric, Apple, and Waymo have begun to develop dynamic path-related technologies for self-driving cars. Legal entities and companies such as the Industrial Technology Research Institute of Taiwan are also actively developing them. The decision-making ability of the path affects the safety of self-driving cars and their ability to handle complex scenarios. When self-driving cars encounter environmental obstacles, they can decide on response behaviors that are closer to general driving, such as overtaking the car in front, avoiding obstacles in the same lane, etc.

However, when planning a driving route, existing self-driving cars cannot infer the decision direction of the route from the current driving environment, which has great uncertainty. In addition, existing self-driving cars only make decisions based on the current environment and cannot handle environmental factors that may intervene in the route, such as pedestrians, moving cars, etc., so the planned driving route also has safety concerns.

Summary of the invention

Therefore, the purpose of the present invention is to provide a trajectory determination method that can plan the best driving path according to the current driving environment and consider the intention of obstacles to improve the accuracy and safety of path planning.

Therefore, the trajectory determination method of the present invention is implemented by a processing module, the processing module is electrically connected to a trajectory generation module, an obstacle detection module, a lane space detection module, and a road information providing module, the trajectory generation module is used to generate multiple trajectory data, each trajectory data includes a path including multiple trajectory points, and a speed of a vehicle at each trajectory point of the path during a driving period of the path, the obstacle detection module is used to detect at least one obstacle within a predetermined distance range from the vehicle to generate at least one obstacle information corresponding to the at least one obstacle, each obstacle information includes the obstacle position of the corresponding obstacle, and the obstacle moving speed and obstacle acceleration of the corresponding obstacle, the lane space detection module is used to detect two lateral distances between the vehicle and obstacles on both sides, and the road information providing module is used to provide a road speed limit, and the trajectory determination method includes the following steps:

(A) for each piece of trajectory data generated by the trajectory generation module, obtaining, based on the at least one piece of obstacle information and the speed of each trajectory point of the trajectory data, a shielding flag indicating whether there is a front obstacle in the at least one obstacle corresponding to the path, and a collision time and a time vehicle distance of the front obstacle relative to each trajectory point of the path;

(B) for each piece of trajectory data, obtaining, based on the at least one piece of obstacle information and the speed of each trajectory point of the trajectory data, an intersection flag of the path of the trajectory data indicating whether there is at least one obstacle to be intersected corresponding to the path in the at least one obstacle, and an intersection collision time of each obstacle to be intersected relative to each trajectory point of the path;

(C) for each piece of trajectory data, obtaining a buffer distance corresponding to each trajectory point of the path according to the lateral distance detected by the lane space detection module at each trajectory point of the path of the trajectory data;

(D) for each piece of trajectory data, obtaining a lane change flag of a path of the trajectory data indicating whether the path causes the vehicle to change lanes;

(E) for each piece of trajectory data, obtaining a safety flag indicating whether it is safe for the vehicle to change lanes according to the at least one piece of obstacle information and the period during which the vehicle travels along the path of the trajectory data; and

(F) Determine a target trajectory data from the trajectory data based on the road speed limit, the masking flag, intersection flag, lane change flag and safety flag of the path of each trajectory data, and the speed, collision time, time vehicle distance, intersection collision time and buffer distance of each trajectory point of the path of each trajectory data.

The trajectory determination method of the present invention, step (A) comprises the following sub-steps:

(A-1) for each piece of trajectory data, determining, based on the at least one piece of obstacle information, whether there is a front obstacle in the at least one obstacle corresponding to the path of the trajectory data;

(A-2) for each piece of trajectory data, when it is determined that there is a front obstacle, a shielding flag indicating that there is a front obstacle on a path corresponding to the trajectory data is obtained, and based on obstacle information corresponding to the front obstacle and the speed of each trajectory point on the path, the collision time and the time-to-vehicle distance of the front obstacle relative to each trajectory point on the path are obtained; and

(A-3) For each piece of trajectory data, when it is determined that the obstacle ahead does not exist, the shielding flag indicating that the obstacle ahead of the path corresponding to the trajectory data does not exist is obtained, and the collision time of each trajectory point of the path is set to a first constant, and the time-to-vehicle distance of each trajectory point of the path is set to a second constant.

The needle bar device of the present invention comprises an outer cylinder seat including a cylinder surrounding wall defining an installation channel, and the elastic member is installed in the installation channel and supported between the needle seat and the cloth pressing tube.

The trajectory determination method of the present invention, step (B) comprises the following sub-steps:

(B-1) for each obstacle, estimating a plurality of estimated moving ranges and a plurality of estimated moving speeds of the obstacle during the driving period of the vehicle based on the obstacle information corresponding to the obstacle;

(B-2) for each piece of trajectory data, determining whether there is at least one obstacle to be intersected in the at least one obstacle corresponding to the path of the trajectory data according to the estimated moving range and the estimated moving speed estimated in step (B-1);

(B-3) for each piece of trajectory data, when it is determined that there is at least one obstacle to be intersected, obtaining the intersection flag indicating that there is at least one obstacle to be intersected in the path corresponding to the trajectory data, and obtaining the intersection collision time of each obstacle to be intersected relative to each trajectory point of the path according to the estimated moving speed corresponding to each obstacle to be intersected, the estimated moving range and the speed of each trajectory point of the path; and

(B-4) For each piece of trajectory data, when it is determined that the at least one obstacle to be intersected does not exist, the intersection flag indicating that the at least one obstacle to be intersected does not exist in the path corresponding to the trajectory data is obtained, and the intersection collision time of each trajectory point of the path is set to a third constant.

The trajectory determination method of the present invention, step (E) comprises the following sub-steps:

(E-1) for each path of the trajectory data that does not cause the vehicle to change lanes, setting the flag value of the safety flag to a first default value;

(E-2) for each path of the trajectory data that causes the vehicle to change lanes, determining whether there is a rear obstacle located in a lane to be changed in the path among the at least one obstacle according to the at least one obstacle information;

(E-3) for each path of the trajectory data that causes the vehicle to change lanes, when it is determined that the rear obstacle exists, obtaining, based on obstacle information corresponding to the rear obstacle, an arrival time of the rear obstacle at an image position of the vehicle mapped to the lane to be changed;

(E-4) for each path of the trajectory data that causes the vehicle to change lanes, when it is determined that the rear obstacle does not exist, setting the arrival time to a fourth fixed value; and

(E-5) For each path of the trajectory data that causes the vehicle to change lanes, a safety flag of the path is obtained based on the arrival time and the driving period of the vehicle on the path.

The trajectory determination method of the present invention, step (E-5), comprises the following sub-steps:

(E-5-1) for each path of the trajectory data that causes the vehicle to change lanes, subtract the travel period from the arrival time to obtain a time difference;

(E-5-2) for each path of the trajectory data that causes the vehicle to change lanes, determining whether the time difference is less than a preset time difference;

(E-5-3) for each path of trajectory data that causes the vehicle to change lanes, when it is determined that the time difference is not less than the preset time difference, setting a safety flag of the path to a first default value indicating that the vehicle is safe to change lanes; and

(E-5-4) For each path of trajectory data that causes the vehicle to change lanes, when it is determined that the time difference is less than the preset time difference, the safety flag of the path is set to a second default value indicating that the vehicle is not safe to change lanes.

The trajectory determination method of the present invention, step (F) comprises the following sub-steps:

(F-1) For each piece of trajectory data, a cost of the trajectory data is calculated using the following cost function according to the road speed limit v _limit , the obstruction flag F _ob , the intersection flag F _int , the lane change flag F _LC and the safety flag F _chgsafe of the path of the trajectory data, and the speed v _i , the collision time TTC _i , the time headway h _i , the minimum intersection collision time PTTC _i and the buffer distance FSP _i of each trajectory point of the path:

Wherein, n is the number of trajectory points of each path, is a preset safe time headway, f(x) is a normalized function, if F _chgsafe =1, then I[F _chgsafe ≠1]=0, if F _chgsafe =0, then I[F _chgsafe ≠1]=1; and

(F-2) Determine a target trajectory data from the trajectory data according to the cost of each trajectory data.

The trajectory determination method of the present invention, in sub-step (F-1), Where x _max is the maximum x value.

The beneficial effect of the present invention is that the target trajectory data is determined according to the road speed limit, the shielding flag, intersection flag, lane change flag and safety flag of the path of each trajectory data, and the speed, collision time, time vehicle distance, intersection collision time and buffer distance of each trajectory point of the path of each trajectory data, so that the determined target trajectory data is the best driving trajectory estimated according to the current driving environment, and the determined best trajectory also takes obstacles into consideration, thereby improving the accuracy and safety of trajectory planning.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG1 is a block diagram illustrating a trajectory determination system for implementing an embodiment of the trajectory determination method of the present invention;

FIG2 is a flow chart illustrating an embodiment of the trajectory determination method of the present invention;

FIG3 is a flow chart illustrating how to obtain a shielding flag for each path, and the collision time and time-to-vehicle distance corresponding to all trajectory points of each path;

FIG4 is a schematic diagram illustrating three paths;

FIG5 is a flow chart illustrating how to obtain a junction flag for each path and the junction collision time corresponding to all trajectory points of each path;

FIG6 is a flow chart illustrating how to obtain a security flag for each path;

FIG7 is a flow chart illustrating how to obtain the safety flag for each path that causes the vehicle to change lanes;

FIG8 is a flow chart illustrating how to determine a target trajectory data from multiple trajectory data;

FIG. 9 is a schematic diagram illustrating a path for a vehicle to change lanes without a front obstacle, and a path for the vehicle not to change lanes and with the front obstacle.

Detailed ways

The present invention is described in detail below with reference to the accompanying drawings and embodiments.

1 , an embodiment of the trajectory determination method of the present invention is implemented by a trajectory determination system 1, which includes a trajectory generation module 11, an obstacle detection module 12, a lane space detection module 13, a road information providing module 14, and a processing module 15 electrically connected to the trajectory generation module 11, the obstacle detection module 12, the lane space detection module 13 and the road information providing module 14.

The trajectory generation module 11 is used to generate multiple trajectory data, each trajectory data includes a path including multiple trajectory points, and the speed of each trajectory point of a vehicle during a period of driving on the path. The trajectory generation module 11 includes a vehicle sensing device, such as a global positioning system, a gyroscope, an odometer, a speedometer, and an inertial measurement unit, and a road condition sensing device, such as an optical radar, an ultrasonic radar, a millimeter wave radar, and a camera array. The vehicle sensing device is used to locate a current position of the vehicle, and to sense a current heading angle of the vehicle, a current speed of the vehicle, and a current acceleration of the vehicle. The road condition sensing device is used to sense the road on which the vehicle is traveling to obtain a road width and a road curvature. The method of generating the trajectory data by the trajectory generation module 11 is recorded in Taiwan Patent Certificate No. I674984, and their details are omitted here for simplicity.

The obstacle detection module 12 is used to detect at least one obstacle within a predetermined distance range from the vehicle to generate at least one piece of obstacle information corresponding to the at least one obstacle, each piece of obstacle information includes the obstacle position of the corresponding obstacle, and the obstacle moving speed and obstacle acceleration of the corresponding obstacle. The obstacle detection module 12, for example, includes at least one of the optical radar, the ultrasonic radar, the millimeter wave radar and the camera array, which is arranged on the vehicle.

The lane space detection module 13 is used to detect the two lateral distances between the vehicle and obstacles on both sides. The lane space detection module 13, for example, includes at least one of the optical radar, the ultrasonic radar, the millimeter wave radar and the camera array, which is arranged on the vehicle. The operation methods of the obstacle detection module 12 and the lane space detection module 13 are described in, for example, Taiwan Patent Certificate Nos. I453697 and I535601, and their details are omitted here for simplicity.

The road information providing module 14 is used to provide a road speed limit. The road information providing module 14 is, for example, a non-volatile memory storing the road speed limit.

The processing module 15 is, for example, a vehicle computer. The vehicle computer can be installed in the vehicle and includes a processor and a storage device.

1 and 2 , the trajectory determination method according to the present invention includes the following steps.

In step 21, for each piece of trajectory data generated by the trajectory generating module 11, the processing module 15 obtains a shielding flag indicating whether there is a front obstacle corresponding to the path among the at least one obstacle, and a collision time (TTC) and a time vehicle distance of the front obstacle relative to each trajectory point of the path according to the at least one piece of obstacle information and the speed of each trajectory point of the trajectory data.

It is worth mentioning that step 21 also includes the following sub-steps (see FIG. 3 ).

In sub-step 211, for each piece of trajectory data, the processing module 15 determines whether there is a front obstacle corresponding to the path of the trajectory data among the at least one obstacle according to the at least one piece of obstacle information. For each piece of trajectory data, when it is determined that there is a front obstacle, the process proceeds to step 212; for each piece of trajectory data, when it is determined that there is no front obstacle, the process proceeds to step 213.

In sub-step 212, the processing module 15 obtains the shielding flag indicating the existence of the front obstacle of the path corresponding to the trajectory data (for example, the flag value of the shielding flag is set to 1 to indicate the existence of the front obstacle), and obtains the collision time and the time vehicle distance of the front obstacle relative to each trajectory point of the path according to the obstacle information corresponding to the front obstacle and the speed of each trajectory point of the path. It is worth mentioning that for each trajectory point of the path, the collision time of the trajectory point is first calculated by first calculating an obstacle distance between the front position of the vehicle when the vehicle travels to the trajectory point and the front obstacle, and the obstacle distance is obtained by adding the path length from the front position of the vehicle when traveling to the trajectory point to the end point of the path and the distance between the end point of the path and the front obstacle, then calculating a relative speed according to the speed of the trajectory point and the obstacle moving speed of the front obstacle, and finally obtaining the obstacle distance by dividing the relative speed. If the calculated relative speed is a negative value, the collision time of the trajectory point is set to a first fixed value, such as 100. For each trajectory point of the path, the time-to-vehicle distance of the trajectory point is obtained by dividing the obstacle distance by the speed of the trajectory point. In the example of FIG. 4 , there is a front obstacle 7 on the first path 01, so the flag value of the shielding flag of the first path 01 is set to 1 to indicate that there is a front obstacle 7 on the first path 01. In addition, FIG. 4 also illustrates the obstacle distance d 1 , the collision time TTC 1 , and the time-to-vehicle distance h 1 between the front position of the vehicle 9 and the front obstacle 7 when the vehicle 9 travels to the driving position shown in FIG. ₄ (that is, the driving position is on the trajectory point (not shown) corresponding to the first path 01), wherein the collision time TTC ₁ and the time _- to-vehicle distance h ₁ are both calculated based on the obstacle distance d ₁ , and both are in time units, not indicating the length of the distance, and FIG _. 4 is only for illustration.

In sub-step 213, the processing module 15 obtains a shielding flag indicating that there is no obstacle ahead of the path corresponding to the trajectory data (for example, the flag value of the shielding flag is set to 0 to indicate that there is no obstacle ahead), and sets the collision time of each trajectory point of the path to the first constant value (i.e., 100), and sets the time-to-vehicle distance of each trajectory point of the path to a second constant value, such as 100. In the example of FIG. 4 , there is no obstacle ahead of the second path 02 and the third path 03, so the flag values of the shielding flags of the second path 02 and the third path 03 are set to 0 to indicate that there is no obstacle ahead of the second path 02 and the third path 03.

In step 22, for each piece of trajectory data, the processing module 15 obtains an intersection flag of the path of the trajectory data indicating whether there is at least one obstacle to be intersected corresponding to the path in the at least one obstacle, and an intersection collision time of each obstacle to be intersected relative to each trajectory point of the path according to the at least one obstacle information and the speed of each trajectory point of the trajectory data.

It is worth mentioning that step 22 also includes the following sub-steps (see FIG. 5 ).

In sub-step 221, for each obstacle, the processing module 15 estimates multiple estimated moving ranges and multiple estimated moving speeds of the obstacle during the driving period of the vehicle according to the obstacle information corresponding to the obstacle. The estimation method of the estimated moving range and the estimated moving speed is recorded in Taiwan Patent Certificate No. I531499, which will not be described in detail here.

In sub-step 222, for each piece of trajectory data, the processing module 15 determines whether there is the at least one obstacle to be intersected corresponding to the path of the trajectory data among the at least one obstacle according to the estimated moving range and the estimated moving speed estimated in step 221. For each piece of trajectory data, when it is determined that the at least one obstacle to be intersected exists, sub-step 223 is performed; for each piece of trajectory data, when it is determined that the at least one obstacle to be intersected does not exist, sub-step 224 is performed.

In sub-step 223, the processing module 15 obtains the intersection flag indicating the existence of the at least one obstacle to be intersected corresponding to the path of the trajectory data (for example, the flag value of the intersection flag is set to 1 to indicate the existence of the at least one obstacle to be intersected), and obtains the intersection collision time of each obstacle to be intersected relative to each trajectory point of the path according to the estimated moving speed, the estimated moving range and the speed of each trajectory point of the path corresponding to each obstacle to be intersected. It is worth mentioning that for each trajectory point of the path, the intersection collision time of the trajectory point relative to a certain obstacle to be intersected is calculated by calculating the obstacle distance of the trajectory point relative to the estimated moving range of the certain obstacle to be intersected, then calculating the relative speed relative to the certain obstacle to be intersected according to the speed of the trajectory point and the estimated moving speed of the certain obstacle to be intersected, and finally dividing the obstacle distance relative to the certain obstacle to be intersected by the relative speed to obtain the intersection collision time. If the calculated relative speed is a negative value, the intersection collision time of the trajectory point is set to a third fixed value, such as 100. In the example of FIG4 , there is an obstacle 8 to be intersected on the third path 03, so the flag value of the intersection flag of the third path 03 is set to 1 to indicate that there is the obstacle 8 to be intersected on the third path 03. In addition, FIG4 illustrates the obstacle distances d ₂ , d ₃ , d ₅ , d ₇ of the estimated moving range 40 between different trajectory points 032 , 033 , 035 , 037 of the third path 03 and the obstacle 8 to be intersected, and the intersection collision times PTTC ₂ , PTTC ₇ of the trajectory points 032 , 037 of the third path 03, wherein the unit of the intersection collision time is time, and does not indicate the length of the distance, and FIG4 is only for illustration.

In sub-step 224, the processing module 15 obtains an intersection flag indicating that the at least one obstacle to be intersected does not exist in the path corresponding to the trajectory data (for example, the flag value of the intersection flag is set to 0 to indicate that the at least one obstacle to be intersected does not exist), and sets the intersection collision time of each trajectory point of the path to the third fixed value (i.e., 100). In the example of FIG. 4 , there is no obstacle to be intersected on the first path 01 and the second path 02, so the flag value of the intersection flag of the first path 01 and the second path 02 is set to 0 to indicate that there is no obstacle to be intersected on the first path 01 and the second path 02.

In step 23, for each piece of trajectory data, the processing module 15 obtains a buffer distance corresponding to each trajectory point of the path according to the lateral distance corresponding to each trajectory point of the path of the trajectory data detected by the lane space detection module 13. In this embodiment, the buffer distance corresponding to each trajectory point is the minimum lateral distance among the lateral distances corresponding to each trajectory point.

In step 24, for each piece of trajectory data, the processing module 15 obtains a lane change flag of the path of the trajectory data indicating whether the path causes the vehicle to change lanes. Taking FIG. 4 as an example, the first path 01 and the third path 03 both cause the vehicle to change lanes, so the flag value of the lane change flag, for example, can be set to 1 to indicate that the path causes the vehicle to change lanes, while the second path 02 does not cause the vehicle to change lanes, so the flag value of the lane change flag, for example, can be set to 0 to indicate that the path does not cause the vehicle to change lanes.

In step 25 , for each piece of trajectory data, the processing module 15 obtains a safety flag indicating whether it is safe for the vehicle to change lanes according to the at least one piece of obstacle information and the period during which the vehicle travels along the path of the trajectory data.

It is worth mentioning that step 25 also includes the following sub-steps (see FIG. 6 ).

In sub-step 251 , for each path of the trajectory data that does not cause the vehicle to change lanes (ie, a path where the lane change flag is 0), the processing module 15 sets the flag value of the safety flag to a first default value, such as 1.

In sub-step 252, for each path of the trajectory data that causes the vehicle to change lanes, the processing module 15 determines whether there is a rear obstacle located in a lane to be changed in the path according to the at least one obstacle information. For each path of the trajectory data that causes the vehicle to change lanes, when it is determined that the rear obstacle exists, the process proceeds to step 253; for each path of the trajectory data that causes the vehicle to change lanes, when it is determined that the rear obstacle does not exist, the process proceeds to step 254. In the example of FIG. 4 , there is a rear obstacle 6 in the first path 01, and there is no rear obstacle in the second path 02 and the third path 03.

In sub-step 253 , the processing module 15 obtains the arrival time of the rear obstacle at a mapping position of the vehicle mapped to the lane to be changed according to the obstacle information corresponding to the rear obstacle.

In sub-step 254, the processing module 15 sets the arrival time to a fourth fixed value, such as 1000. It is worth mentioning that when it is determined that there is no rear obstacle, the arrival time is infinite, so the arrival time is set to the fourth fixed value to avoid the doubt of "no rear obstacle, no arrival time (i.e. no fourth fixed value)".

In sub-step 255, for each path of the trajectory data that causes the vehicle to change lanes, the processing module 15 obtains the safety flag of the path according to the arrival time and a driving period of the vehicle on the path. FIG4 illustrates a mapping position 9' of the vehicle 9 mapped to the lane to be changed, the arrival time tr and the driving period th of the first path 01, wherein the arrival time and the driving period are in units of time, not indicating the length of the distance, and FIG4 is only for illustration.

It is worth mentioning that sub-step 255 also includes the following sub-steps (see FIG. 7 ).

In sub-step 551 , for each path of the trajectory data that causes the vehicle to change lanes, the processing module 15 subtracts the travel period from the arrival time to obtain a time difference.

In sub-step 552, for each path of the trajectory data that causes the vehicle to change lanes, the processing module 15 determines whether the time difference is less than a preset time difference. In this embodiment, the preset time difference is, for example, 1.8 seconds. For each path of the trajectory data that causes the vehicle to change lanes, when it is determined that the time difference is not less than the default time difference, the process proceeds to sub-step 553; for each path of the trajectory data that causes the vehicle to change lanes, when it is determined that the time difference is less than the default time difference, the process proceeds to sub-step 554.

In sub-step 553 , the processing module 15 sets the safety flag of the path to the first default value, such as 1, indicating that the vehicle is safe to change lanes.

In sub-step 554 , the processing module 15 sets the safety flag of the path to a second default value, such as 0, indicating that it is not safe for the vehicle to change lanes.

In step 26, the processing module 15 determines a target trajectory data from the trajectory data according to the road speed limit, the shielding flag, the intersection flag, the lane change flag and the safety flag of the path of each trajectory data, and the speed of each trajectory point of the path of each trajectory data, the collision time, the time vehicle distance, each intersection collision time and the buffer distance.

It is worth mentioning that step 26 also includes the following sub-steps (see FIG. 8 ).

In sub-step 261, for each piece of trajectory data, the processing module 15 calculates a cost cost of the trajectory data using a cost function according to the road speed limit v _limit , the obstruction flag F _ob , the intersection flag F _int , the lane change flag F _LC and the safety flag F _chgsafe of the path of the trajectory data, and the speed v _i , the collision time TTC _i , the time vehicle distance h _i , the minimum intersection collision time PTTC _i and the buffer distance FSP _i of each trajectory point of the path. In this embodiment, the cost function can be expressed as the following formula (1).

Wherein, n is the number of trajectory points of each path, is a preset safe time distance. is a normalization function, x _max is the maximum x value, if F _chgsafe = 1, then I[F _chgsafe ≠1] = 0, if F _chgsafe = 0, then I[F _chgsafe ≠1] = 1. For example, taking f(v _{limit -vi} ) as an example, x = v _{limit -vi} , x _max is the maximum of the differences between the speed limit v _limit of the road and the speeds of different trajectory points.

In sub-step 262, the processing module 15 determines the target trajectory data from the trajectory data according to the cost of each trajectory data. In this embodiment, the determined target trajectory data corresponds to the path with the minimum cost.

It is worth mentioning that the trajectory data determined by the trajectory determination method of the present invention has the following characteristics: First, when there is no vehicle ahead that is slower than the speed limit of the road (i.e., an obstacle ahead), the vehicle travels at the speed limit of the road. Since the determined trajectory data has the minimum cost, if the cost of the trajectory data is to be minimized, the trajectory data must be minimized. The smaller the value, the more The smaller the value, the closer vi will be to the speed limit of the road, so the selected optimal trajectory will satisfy the first characteristic mentioned above; second, keep the safe time distance with the vehicle in front (that is, the obstacle in front). Since the determined trajectory data has the minimum cost, if you want to make the cost of the trajectory data as small as possible, you must make the trajectory data/> The smaller the value, the more The smaller the value, the closer hi will be to the safe time vehicle distance, so the selected optimal trajectory will satisfy the second characteristic mentioned above; third, when the space behind the adjacent lane is safe (that is, the safety flag indicates that the vehicle is safe to change lanes), the lane change can be performed. Since the determined trajectory data has the minimum cost, if the cost of the trajectory data is to be minimized, the (1-I[F _chgsafe ≠1]) value of the trajectory data must be as equal to 1 as possible (for example, the safety flag F _chgsafe ＝1). If the (1-I[F _chgsafe ≠1]) value of the trajectory data is 0, due to is the denominator of the cost function, which will lead to a very large cost of the trajectory data or the cost of the trajectory data cannot be calculated at all. Therefore, the trajectory data that changes lanes and is not in a safe state when changing lanes cannot be selected as the best trajectory data, so the best trajectory data selected will satisfy the third characteristic mentioned above. In addition, taking FIG. 9 as an example, assuming that there is an obstacle in front of the lane where the vehicle is currently traveling, the trajectory data generated by the trajectory generation module 11 includes a path 04 that allows the vehicle to change lanes and does not exist in the front obstacle, and a path 05 that does not allow the vehicle to change lanes and exists in the front obstacle. At this time, when selecting a path, there is a high chance that the path 04 that allows the vehicle to change lanes and does not exist in the front obstacle will be selected as the best path. Since the path 04 corresponding to the path without the front obstacle is The path 05 corresponding to the obstacle ahead Therefore, the cost corresponding to the path 04 without the obstacle ahead has a high chance of being lower than the cost corresponding to the path 05 with the obstacle ahead. However, it still depends on the values of other parameters of the cost function. This is just for illustration./> The influence of parameters; Fourth, when there is enough road space, the vehicle can perform same-lane obstacle avoidance and return to the center of the lane after avoiding the obstacle. When performing same-lane obstacle avoidance, the selected path is a path that is in the same lane and can avoid obstacles. If you want to make the cost of the path as small as possible, you must make the path /> The larger the value, the larger the buffer distance FSP _i should be _{, and the safer the driving is. After avoiding obstacles, in order to make the buffer distance FSP i larger} , a path located in the center of the lane will be selected, so the selected optimal path will satisfy the fourth characteristic mentioned above; Fifth, when lane change, same-lane obstacle avoidance and following the vehicle cannot be performed, the vehicle stops. If there is an obstacle in front of the lane where the vehicle is currently traveling, but the vehicle cannot perform lane change, same-lane obstacle avoidance and following the vehicle, and at the same time, there is a risk of collision between the vehicle and the obstacle in front (that is, the collision time TTC _i is too small and following the vehicle is no longer possible), the f(TTC _i ) value must be made larger to be safer, and then the cost will be smaller. At this time, the vehicle will be stopped so that the f(TTC _i ) value can be made larger, so the path that stops the vehicle is the optimal path.

In summary, the trajectory determination method of the present invention determines the target trajectory data according to the road speed limit, the shielding flag, the intersection flag, the lane change flag and the safety flag of the path of each trajectory data, and the speed of each trajectory point of the path of each trajectory data, the collision time, the time vehicle distance, each intersection collision time and the buffer distance. The determined optimal trajectory data can be determined according to the current driving environment to drive according to the road speed limit, vehicle following, lane change, same lane obstacle avoidance and vehicle stopping, thereby satisfying the above five characteristics, and the determined optimal path has the intention of taking obstacles into consideration, thereby improving the accuracy and safety of path planning, so that the purpose of the present invention can be achieved.

However, the above is only an embodiment of the present invention and should not be used to limit the scope of the present invention. All simple equivalent changes and modifications made according to the claims and description of the present invention are still within the scope of the present invention.

Claims (7)

1. A trajectory determination method, implemented by a processing module, the processing module is electrically connected to a trajectory generation module, an obstacle detection module, a lane space detection module, and a road information providing module, the trajectory generation module is used to generate a plurality of trajectory data, each trajectory data includes a path including a plurality of trajectory points, and a speed of a vehicle at each trajectory point of the path during a driving period of the path, the obstacle detection module is used to detect at least one obstacle within a predetermined distance range from the vehicle to generate at least one obstacle information corresponding to the at least one obstacle, each obstacle information includes an obstacle position of the corresponding obstacle, and an obstacle moving speed and an obstacle acceleration of the corresponding obstacle, the lane space detection module is used to detect two lateral distances between the vehicle and obstacles on both sides, and the road information providing module is used to provide a road speed limit, characterized in that the trajectory determination method comprises the following steps: (A) for each piece of trajectory data generated by the trajectory generation module, obtaining, based on the at least one piece of obstacle information and the speed of each trajectory point of the trajectory data, a shielding flag indicating whether there is a front obstacle corresponding to the path among the at least one obstacle, and a collision time and a time vehicle distance of the front obstacle relative to each trajectory point of the path; (B) for each piece of trajectory data, obtaining, based on the at least one piece of obstacle information and the speed of each trajectory point of the trajectory data, an intersection flag of the path of the trajectory data indicating whether there is at least one obstacle to be intersected corresponding to the path among the at least one obstacle, and an intersection-collision time of each obstacle to be intersected relative to each trajectory point of the path; (C) for each piece of trajectory data, obtaining a buffer distance corresponding to each trajectory point of the path according to the lateral distance detected by the lane space detection module at each trajectory point of the path of the trajectory data; (D) for each piece of trajectory data, obtaining a lane change flag of the path of the trajectory data indicating whether the path causes the vehicle to change lanes; (E) for each piece of trajectory data, obtaining a safety flag of the path indicating whether it is safe for the vehicle to change lanes based on the at least one piece of obstacle information and the period during which the vehicle travels along the path of the trajectory data; and (F) Determine a target trajectory data from the trajectory data based on the road speed limit, the masking flag, the intersection flag, the lane change flag and the safety flag of the path of each trajectory data, and the speed of each trajectory point on the path of each trajectory data, the collision time, the time vehicle distance, each intersection collision time and the buffer distance. 2. The trajectory determination method according to claim 1, wherein step (A) comprises the following sub-steps: (A-1) for each piece of trajectory data, determining, based on the at least one piece of obstacle information, whether there is a front obstacle in the at least one obstacle corresponding to the path of the trajectory data; (A-2) for each piece of trajectory data, when it is determined that there is a front obstacle, a shielding flag indicating that there is a front obstacle on a path corresponding to the trajectory data is obtained, and based on obstacle information corresponding to the front obstacle and the speed of each trajectory point on the path, the collision time and the time-to-vehicle distance of the front obstacle relative to each trajectory point on the path are obtained; and (A-3) For each piece of trajectory data, when it is determined that the obstacle ahead does not exist, the shielding flag indicating that the obstacle ahead of the path corresponding to the trajectory data does not exist is obtained, and the collision time of each trajectory point of the path is set to a first constant, and the time-to-vehicle distance of each trajectory point of the path is set to a second constant. 3. The trajectory determination method according to claim 1, wherein step (B) comprises the following sub-steps: (B-1) for each obstacle, estimating a plurality of estimated moving ranges and a plurality of estimated moving speeds of the obstacle during the driving period of the vehicle based on the obstacle information corresponding to the obstacle; (B-2) for each piece of trajectory data, determining whether there is at least one obstacle to be intersected in the at least one obstacle corresponding to the path of the trajectory data according to the estimated moving range and the estimated moving speed estimated in step (B-1); (B-3) for each piece of trajectory data, when it is determined that there is at least one obstacle to be intersected, obtaining the intersection flag indicating that there is at least one obstacle to be intersected in the path corresponding to the trajectory data, and obtaining the intersection collision time of each obstacle to be intersected relative to each trajectory point of the path according to the estimated moving speed corresponding to each obstacle to be intersected, the estimated moving range and the speed of each trajectory point of the path; and (B-4) For each piece of trajectory data, when it is determined that the at least one obstacle to be intersected does not exist, the intersection flag indicating that the at least one obstacle to be intersected does not exist in the path corresponding to the trajectory data is obtained, and the intersection collision time of each trajectory point of the path is set to a third constant. 4. The trajectory determination method according to claim 1, wherein step (E) comprises the following sub-steps: (E-1) for each path of the trajectory data that does not cause the vehicle to change lanes, setting the flag value of the safety flag to a first default value; (E-2) for each path of the trajectory data that causes the vehicle to change lanes, determining whether there is a rear obstacle located in a lane to be changed in the path among the at least one obstacle according to the at least one obstacle information; (E-3) for each path of the trajectory data that causes the vehicle to change lanes, when it is determined that the rear obstacle exists, obtaining, based on obstacle information corresponding to the rear obstacle, an arrival time of the rear obstacle at an image position of the vehicle mapped to the lane to be changed; (E-4) for each path of the trajectory data that causes the vehicle to change lanes, when it is determined that the rear obstacle does not exist, setting the arrival time to a fourth fixed value; and (E-5) For each path of the trajectory data that causes the vehicle to change lanes, a safety flag of the path is obtained based on the arrival time and the driving period of the vehicle on the path. 5. The trajectory determination method according to claim 4, characterized in that, in step (E-5), the following sub-steps are included: (E-5-1) for each path of the trajectory data that causes the vehicle to change lanes, subtract the travel period from the arrival time to obtain a time difference; (E-5-2) for each path of the trajectory data that causes the vehicle to change lanes, determining whether the time difference is less than a preset time difference; (E-5-3) for each path of trajectory data that causes the vehicle to change lanes, when it is determined that the time difference is not less than the preset time difference, setting a safety flag of the path to a first default value indicating that the vehicle is safe to change lanes; and (E-5-4) For each path of trajectory data that causes the vehicle to change lanes, when it is determined that the time difference is less than the preset time difference, the safety flag of the path is set to a second default value indicating that the vehicle is not safe to change lanes. 6. The trajectory determination method according to claim 1, wherein step (F) comprises the following sub-steps: (F-1) For each piece of trajectory data, a cost of the trajectory data is calculated using the following cost function according to the road speed limit v _limit , the obstruction flag F _ob , the intersection flag F _int , the lane change flag F _LC and the safety flag F _chgsafe of the path of the trajectory data, and the speed v _i , the collision time TTC _i , the time headway h _i , the minimum intersection collision time PTTC _i and the buffer distance FSP _i of each trajectory point of the path: Wherein, n is the number of trajectory points of each path, is a preset safe time headway, f(x) is a normalized function, if F _chgsafe =1, then I[F _chgsafe ≠1]=0, if F _chgsafe =0, then I[F _chgsafe ≠1]=1; and (F-2) Determine a target trajectory data from the trajectory data according to the cost of each trajectory data. 7. The trajectory determination method according to claim 6, characterized in that in sub-step (F-1), Where x _max is the maximum x value.