Vehicle motion planning method in low-precision navigation environment

Abstract

The invention provides a vehicle motion planning method in a low-precision navigation environment. The method comprises the following steps: step 1, constructing a semantic lane guide tree; 2, sampling is carried out, and sampling points are divided into two branches of a self-vehicle part and a surrounding vehicle part; step 3, obtaining a self-vehicle forward expansion track according to the sampling points of the self-vehicle part; 4, according to the sampling points of the surrounding vehicle part, obtaining a surrounding vehicle forward prediction trajectory; step 5, judging whether a passable safe track exists or not, if not, turning to step 6, and if yes, turning to step 7; 6, deleting the sampling point, and turning to the step 2; step 7, performing node matching on the forward expansion trajectory of the vehicle to obtain new data information, adding the new data information into the semantic lane guide tree, turning to step 2 until the semantic lane guide tree reaches a termination condition, ending circulation, and extracting a trajectory set; and step 8, selecting an optimal path from the track set to be used by the vehicle.

Description

Technical Field

[0001] The present application relates to the field of intelligent driving technology, and in particular to a vehicle motion planning method in a low-precision navigation environment. Background Art

[0002] As autonomous driving technology develops rapidly, motion planning is one of the core links, and its performance is directly related to the reliability and practicality of the autonomous driving system. However, the current development of autonomous driving motion planning technology still faces a series of challenges, especially the reliance on high-precision maps and high-precision positioning. In the field of autonomous driving applications, positioning solutions based on high-precision maps have always occupied the mainstream position in the industry. The accuracy and efficiency of high-precision maps directly affect the performance of autonomous driving vehicles.

[0003] Although high-precision maps and positioning systems provide strong support for autonomous driving, they still have limitations in practical applications, such as high collection costs, difficulty in maintaining freshness, and inability to expand to small and medium-sized cities. Obtaining high-precision maps of the entire city requires a lot of collection work and preliminary data processing work, which is expensive and difficult to popularize. At the same time, the computing power and storage resources of the autonomous driving vehicle controller platform are limited. As the operating range gradually expands, the environment that needs to be dealt with becomes more complex and changeable. The memory occupied by high-precision map storage and the computing power consumed by retrieval during positioning are also not to be ignored, which undoubtedly increases the difficulty of maintaining the autonomous driving system.

[0004] Although real-time semantic map technology can get rid of the dependence of autonomous driving on high-precision maps and achieve lightweight and real-time maps, the absolute coordinate positioning accuracy of real-time semantic maps is lower than that of high-precision maps, and the road element attribute information contained is also less. In order to achieve the widespread application and commercialization of autonomous driving in cities, it is necessary to consider the autonomous driving strategy in the low-precision real-time semantic map navigation environment. It is particularly important to develop autonomous driving motion planning technology based on real-time semantic maps. This requires us to make full use of the advantages of lightweight and real-time real-time semantic maps, while overcoming the limitations of their accuracy and richness that are weaker than high-precision maps at the autonomous driving planning level to ensure the safety and stability of the autonomous driving system. Summary of the invention

[0005] In view of this, the present invention provides a vehicle motion planning method in a low-precision navigation environment to solve the technical problem that the prior art cannot adapt to the low-precision navigation environment of autonomous driving.

[0006] The present invention provides a vehicle motion planning method in a low-precision navigation environment, which is applied to an automatic driving vehicle. The method comprises: step 1, constructing a semantic lane guide tree according to data information of the own vehicle and other vehicles; step 2, sampling according to the data information to obtain sampling points, and dividing the sampling points into two branches of the own vehicle part and the surrounding vehicle part; step 3, generating semantic preview points from the sampling point positions of the own vehicle part, selecting the optimal parent node, and calculating the forward extension trajectory of the own vehicle through a fast curve extension algorithm; step 4, filtering out semantically related vehicles according to the sampling of the surrounding vehicle part, and calculating the forward predicted trajectory of the surrounding vehicle through a fast curve extension algorithm; step 5, judging whether there is a passable safe trajectory according to the forward extension trajectory of the own vehicle and the forward predicted trajectory of the surrounding vehicle, if not, turning to step 6, if yes, turning to step 7; step 6, deleting the sampling point, turning to step 2, and resampling; step 7, performing node matching on the forward extension trajectory of the own vehicle, obtaining new data information, adding it to the semantic lane guide tree, and then turning to step 2 until the semantic lane guide tree reaches the termination condition, ending the loop, and extracting a trajectory set; step 8, selecting a function according to the optimal trajectory: Select the optimal path from the trajectory set for the vehicle to use, where t1 is any passable trajectory extracted from the semantic lane guidance tree, cost_t(t1) represents the cost of trajectory t1 under the evaluation system; efficiency cost efficiency(t1) represents the calculated efficiency of trajectory t1 on the current road, δ e is the weight of the traffic efficiency cost; the safety cost safety(t1) represents the safety of trajectory t1 relative to obstacles, δ s represents the weight of the trajectory safety cost; navigation(t1) represents the navigation cost of trajectory t1, δ n is the weight of the curvature cost.

[0007] Furthermore, the data information includes posture information, topological information, semantic information and trajectory information of the vehicle.

[0008] Furthermore, the forward extension trajectory of the vehicle is (V parent ,V sample ,E), where V parent represents the optimal parent node of the selected sampling point, V sample represents the sampling point, and E represents the extended trajectory.

[0009] Furthermore, the forward prediction trajectory of the surrounding vehicle is (Agent id ,E), where Agent id represents the number of the semantically associated vehicle, and E represents the forward predicted trajectory of the semantically associated vehicle.

[0010] Furthermore, the method of performing node matching on the forward extended trajectory of the vehicle to obtain new data information includes: step 71, performing node matching on the forward extended trajectory of the vehicle (V parent ,V sample ,E) to match nodes and obtain the matched nodes (V parent ,V child ,E); Step 72, according to the matched node (V parent ,V child ,E), get the edge (V child ,E); Step 73, expand the new node V generated by the self-vehicle forward new With edge (V child ,E) combined into new data information (V new ,E new ).

[0011] Furthermore, the termination condition is: termination_condition=(Num Trajectory >Num threshold )||(Time calculation >Time threshold ), where Num Trajectory Indicates the number of trajectories, Num threshold Indicates the end mark of the track number, Time calculation Indicates the single calculation time, Time threshold Indicates the upper limit of a single calculation time.

[0012] Furthermore, the optimal trajectory is a traversable trajectory with the lowest cost in the trajectory set.

[0013] Furthermore, the traffic efficiency cost efficiency(t1)=δ e1 *abs(velocity ego -velocity desire )+δ e2 *abs(velocity ego -velocity leadingVehicle ), where velocity ego is the average vehicle speed, velocity desire is the current road target cruising speed, velocity leadingVehicle is the speed of the vehicle ahead, δ e1 and δ e2 are two weights.

[0014] Furthermore, the safety cost safety(t1) is evaluated using a responsibility-sensitive safety model.

[0015] Furthermore, the navigation cost navigation(t1)=∑δ reward *Trajectory recommend +∑δ punish *Trajectory non-recommend , where Trajectory recommend Indicates the recommended part of the vehicle’s current trajectory, Trajectory non-recommend represents the unrecommended part of the vehicle’s current trajectory, δ reward represents the reward coefficient, δ punish Represents the penalty coefficient.

[0016] The present invention provides a vehicle motion planning method in a low-precision navigation environment. The technical solution proposes a semantic lane-guided rapidly-exploring random tree algorithm (Semantic Lane-Guided Rapidly-exploring RandomTree, SLG-RRT), including a sampling method based on vehicle behavior semantic guidance, a rapid pruning method based on the SLG feature tree, and a trajectory optimization method that includes a weighted fusion of multiple constraints of traffic efficiency, safety and semantic guidance. It is mainly used to solve the low sampling efficiency of motion planning algorithms in existing semantic map environments, the exponential explosion of computational difficulty in interactive games with surrounding vehicles in urban road environments, and the optimal trajectory selection problem after multiple solutions are obtained in the autonomous driving motion planning algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 It is a flow chart of a vehicle motion planning method in a low-precision navigation environment provided by the present invention;

[0018] Figure 2 This is a fast expansion random tree algorithm architecture diagram for semantic lane guidance provided by the present invention;

[0019] Figure 3 It is a flow chart of a method provided by the present invention for performing node matching on a forward extension trajectory of a self-vehicle to obtain new data information. DETAILED DESCRIPTION

[0020] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0021] Embodiment 1

[0022] The present invention provides a vehicle motion planning method in a low-precision navigation environment, which is applied to an autonomous driving vehicle, such as Figure 1 As shown, the method includes the following steps.

[0023] Step 1: construct a semantic lane guidance tree based on the data information of the vehicle itself and other vehicles;

[0024] In the semantic lane environment, we define the self-driving car as the ego car, and other traffic participants as surrounding cars. The ego car and surrounding cars are collectively referred to as intelligent agents. The semantic lane guidance (SLG) tree is a tree structure containing a node set V and an edge set E. The node set V contains the vehicle's posture information (coordinate x, coordinate y, heading, speed v, acceleration a) and topological information (parent node, child node set), and the edge set contains the semantic information connecting the parent node to the child node (lateral behavior semantics, longitudinal behavior semantics) and trajectory information (a set of points containing vehicle posture information).

[0025] Step 2, sampling is performed according to the data information to obtain sampling points, and the sampling points are divided into two branches: the vehicle part and the surrounding vehicle part;

[0026] Step 3: Generate semantic preview points from the sampling point positions of the ego vehicle, select the optimal parent node, and calculate the forward extension trajectory of the ego vehicle through the fast curve extension algorithm;

[0027] Step 4: According to the sampling of the surrounding vehicles, the semantically related vehicles are screened out, and the forward prediction trajectory of the surrounding vehicles is calculated by the fast curve extension algorithm;

[0028] Step 5: According to the forward extended trajectory of the vehicle and the forward predicted trajectory of the surrounding vehicles, determine whether there is a safe trajectory that can be passed. If not, go to step 6; if so, go to step 7;

[0029] Step 6, delete the sampling point, go to step 2, and resample;

[0030] Step 7: Perform node matching on the forward extended trajectory of the vehicle to obtain new data information, add it to the semantic lane guidance tree, and then go to step 2 until the semantic lane guidance tree reaches the termination condition, end the loop, and extract the trajectory set;

[0031] Step 8, select the function according to the optimal trajectory: Select the optimal path from the trajectory set for the vehicle to use, where t1 is any passable trajectory extracted from the semantic lane guidance tree, cost_t(t1) represents the cost of trajectory t1 under the evaluation system; efficiency cost efficiency(t1) represents the calculated efficiency of trajectory t1 on the current road, δ eis the weight of the traffic efficiency cost; the safety cost safety(t1) represents the safety of trajectory t1 relative to obstacles, δ s represents the weight of the trajectory safety cost; navigation(t1) represents the navigation cost of trajectory t1, δ n is the weight of the curvature cost.

[0032] The present invention provides a vehicle motion planning method in a low-precision navigation environment. The technical solution proposes a semantic lane-guided rapidly-exploring random tree algorithm (Semantic Lane-Guided Rapidly-exploring RandomTree, SLG-RRT), including a sampling method based on vehicle behavior semantic guidance, a rapid pruning method based on the SLG feature tree, and a trajectory optimization method that includes a weighted fusion of multiple constraints of traffic efficiency, safety and semantic guidance. It is mainly used to solve the low sampling efficiency of motion planning algorithms in existing semantic map environments, the exponential explosion of computational difficulty in interactive games with surrounding vehicles in urban road environments, and the optimal trajectory selection problem after multiple solutions are obtained in the autonomous driving motion planning algorithm.

[0033] Embodiment 2

[0034] The present invention provides a vehicle motion planning method in a low-precision navigation environment, which is applied to an autonomous driving vehicle, such as Figure 1 As shown, the method includes the following steps.

[0035] Step 1: construct a semantic lane guidance tree based on the data information of the vehicle itself and other vehicles;

[0036] Step 2, sampling is performed according to the data information to obtain sampling points, and the sampling points are divided into two branches: the vehicle part and the surrounding vehicle part;

[0037] As mentioned above, data information includes vehicle posture information, topology information, semantic information and trajectory information. First, sampling is required based on the semantic guidance of vehicle behavior in a semantic lane environment. The semantic action of vehicle behavior is a high-level action that can be directly interpreted by human senses. First, the semantic action of vehicle behavior is divided into lateral semantic behavior and longitudinal semantic behavior. Among them, the lateral semantic behavior includes three behaviors: lane keeping, left lane change, and right lane change, which are represented by (K, L, R) respectively; the longitudinal semantic behavior includes three behaviors: speed keeping, acceleration, and deceleration, which are represented by (M, A, D). For example, changing lanes to the left and decelerating on public roads directly constitute a set of behavioral semantic actions, which can be expressed by (L, D). The semantic action actually contains multiple small motion planning steps. To realize a set of actions of changing lanes to the left and decelerating (L, D), it is necessary to involve multiple lateral corner sequences and multiple longitudinal acceleration sequences. However, if such local actions (lateral corners and longitudinal accelerations) are considered in the sampling algorithm, the number of layers of the SLG tree will be too deep, resulting in the problem of explosion of computational dimensions, affecting the real-time performance of autonomous driving vehicles. After sampling, a sampling point will be obtained, and the sampling point will be divided into two branches: the vehicle part and the surrounding vehicle part.

[0038] Step 3: Generate semantic preview points from the sampling point positions of the ego vehicle, select the optimal parent node, and calculate the forward extension trajectory of the ego vehicle through the fast curve extension algorithm;

[0039] As mentioned above, the sampling points are divided into the ego vehicle part and the surrounding vehicle part. In the ego vehicle part, semantic preview points are generated according to the sampling point positions, and the optimal parent node is selected and the forward extension trajectory (V parent ,V sample ,E). Among them, V parent represents the optimal parent node of the selected sampling point, V sample represents the sampling point, and E represents the extended trajectory.

[0040] Step 4: According to the sampling of the surrounding vehicles, the semantically related vehicles are screened out, and the forward prediction trajectory of the surrounding vehicles is calculated by the fast curve extension algorithm;

[0041] Similarly, in the surrounding vehicle part, semantically related vehicles are selected through sampling point semantic lane judgment, and the surrounding vehicle forward prediction trajectory of semantically related vehicles is calculated by fast clothoid expansion algorithm as (Agent id ,E), where Agent id represents the number of the semantically associated vehicle, and E represents the forward predicted trajectory of the semantically associated vehicle.

[0042] Step 5: According to the forward extended trajectory of the vehicle and the forward predicted trajectory of the surrounding vehicles, determine whether there is a safe trajectory that can be passed. If not, go to step 6; if so, go to step 7;

[0043] Step 6, delete the sampling point, go to step 2, and resample;

[0044] Step 7: Perform node matching on the forward extended trajectory of the vehicle to obtain new data information, add it to the semantic lane guidance tree, and then go to step 2 until the semantic lane guidance tree reaches the termination condition, end the loop, and extract the trajectory set;

[0045] Step 8, select the function according to the optimal trajectory: Select the optimal path from the trajectory set for the vehicle to use, where t1 is any passable trajectory extracted from the semantic lane guidance tree, cost_t(t1) represents the cost of trajectory t1 under the evaluation system; efficiency cost efficiency(t1) represents the calculated efficiency of trajectory t1 on the current road, δ e is the weight of the traffic efficiency cost; the safety cost safety(t1) represents the safety of trajectory t1 relative to obstacles, δ s represents the weight of the trajectory safety cost; navigation(t1) represents the navigation cost of trajectory t1, δ n is the weight of the curvature cost.

[0046] The present invention provides a vehicle motion planning method in a low-precision navigation environment. The technical solution proposes a semantic lane-guided rapidly-exploring random tree algorithm (Semantic Lane-Guided Rapidly-exploring RandomTree, SLG-RRT), including a sampling method based on vehicle behavior semantic guidance, a rapid pruning method based on the SLG feature tree, and a trajectory optimization method that includes a weighted fusion of multiple constraints of traffic efficiency, safety and semantic guidance. It is mainly used to solve the low sampling efficiency of motion planning algorithms in existing semantic map environments, the exponential explosion of computational difficulty in interactive games with surrounding vehicles in urban road environments, and the optimal trajectory selection problem after multiple solutions are obtained in the autonomous driving motion planning algorithm.

[0047] Embodiment 3

[0048] The present invention provides a vehicle motion planning method in a low-precision navigation environment, which is applied to an autonomous driving vehicle, such as Figure 2 As shown, the method includes the following steps.

[0049] Step 1: construct a semantic lane guidance tree based on the data information of the vehicle itself and other vehicles;

[0050] Step 2, sampling is performed according to the data information to obtain sampling points, and the sampling points are divided into two branches: the vehicle part and the surrounding vehicle part;

[0051] Step 3: Generate semantic preview points from the sampling point positions of the ego vehicle, select the optimal parent node, and calculate the forward extension trajectory of the ego vehicle through the fast curve extension algorithm;

[0052] Step 4: According to the sampling of the surrounding vehicles, the semantically related vehicles are screened out, and the forward prediction trajectory of the surrounding vehicles is calculated by the fast curve extension algorithm;

[0053] Step 5: According to the forward extended trajectory of the vehicle and the forward predicted trajectory of the surrounding vehicles, determine whether there is a safe trajectory that can be passed. If not, go to step 6; if so, go to step 7;

[0054] We conduct a decision scenario game on the forward extension trajectory of the ego vehicle and the forward predicted trajectory of the semantically associated vehicle. If the decision scenario game fails, the sampling point is discarded and the behavior semantics guided sampling is returned; if it passes, (V parent ,V sample ,E) to match nodes and get (V parent ,V child ,E), and the nodes and edges (V child ,E) into a new node and edge combination (V new ,E new ) added to the SLG tree.

[0055] Step 6, delete the sampling point, go to step 2, and resample;

[0056] Step 7: Perform node matching on the forward extended trajectory of the vehicle to obtain new data information, add it to the semantic lane guidance tree, and then go to step 2 until the semantic lane guidance tree reaches the termination condition, end the loop, and extract the trajectory set;

[0057] like Figure 3 As shown, the method for performing node matching on the forward extension trajectory of the vehicle to obtain new data information is as follows.

[0058] Step 71, expand the forward trajectory of the vehicle (V parent ,V sample ,E) to match nodes and obtain the matched nodes (V parent ,V child ,E);

[0059] Step 72, according to the matched node (V parent ,V child ,E), get the edge (V child ,E);

[0060] Step 73: Expand the new node V generated by the vehicle forward new With edge (V child ,E) combined into new data information (V new ,E new ).

[0061] The termination condition is: termination_condition=(Num Trajectory >

[0062] Num threshold )||(Time calculation >Time threshold ), where Num Trajectory Indicates the number of trajectories, Num threshold Indicates the end mark of the track number, Time calculation Indicates the single calculation time, Time threshold Indicates the upper limit of a single calculation time.

[0063] After the above steps, the SLG tree is updated, and the behavior semantics-guided sampling is continued, and steps 2-7 are repeated to cyclically update the SLG tree until the SLG tree reaches the termination condition, ending the cycle.

[0064] Step 8, select the function according to the optimal trajectory: Select the optimal path from the trajectory set for the vehicle to use, where t1 is any passable trajectory extracted from the semantic lane guidance tree, cost_t(t1) represents the cost of trajectory t1 under the evaluation system; efficiency cost efficiency(t1) represents the calculated efficiency of trajectory t1 on the current road, δ e is the weight of the traffic efficiency cost; the safety cost safety(t1) represents the safety of trajectory t1 relative to obstacles, δ s represents the weight of the trajectory safety cost; navigation(t1) represents the navigation cost of trajectory t1, δ n is the weight of the curvature cost.

[0065] The present invention provides a vehicle motion planning method in a low-precision navigation environment. The technical solution proposes a semantic lane-guided rapidly-exploring random tree algorithm (Semantic Lane-Guided Rapidly-exploring RandomTree, SLG-RRT), including a sampling method based on vehicle behavior semantic guidance, a rapid pruning method based on the SLG feature tree, and a trajectory optimization method that includes a weighted fusion of multiple constraints of traffic efficiency, safety and semantic guidance. It is mainly used to solve the low sampling efficiency of motion planning algorithms in existing semantic map environments, the exponential explosion of computational difficulty in interactive games with surrounding vehicles in urban road environments, and the optimal trajectory selection problem after multiple solutions are obtained in the autonomous driving motion planning algorithm.

[0066] Embodiment 4

[0067] The present invention provides a vehicle motion planning method in a low-precision navigation environment, which is applied to an autonomous driving vehicle, such as Figure 1 As shown, the method includes the following steps.

[0068] Step 1: construct a semantic lane guidance tree based on the data information of the vehicle itself and other vehicles;

[0069] Step 2, sampling is performed according to the data information to obtain sampling points, and the sampling points are divided into two branches: the vehicle part and the surrounding vehicle part;

[0070] Step 3: Generate semantic preview points from the sampling point positions of the ego vehicle, select the optimal parent node, and calculate the forward extension trajectory of the ego vehicle through the fast curve extension algorithm;

[0071] Step 4: According to the sampling of the surrounding vehicles, the semantically related vehicles are screened out, and the forward prediction trajectory of the surrounding vehicles is calculated by the fast curve extension algorithm;

[0072] Step 5: According to the forward extended trajectory of the vehicle and the forward predicted trajectory of the surrounding vehicles, determine whether there is a safe trajectory that can be passed. If not, go to step 6; if so, go to step 7;

[0073] Step 6, delete the sampling point, go to step 2, and resample;

[0074] Step 7: Perform node matching on the forward extended trajectory of the vehicle to obtain new data information, add it to the semantic lane guidance tree, and then go to step 2 until the semantic lane guidance tree reaches the termination condition, end the loop, and extract the trajectory set;

[0075] Step 8, select the function according to the optimal trajectory: Select the optimal path from the trajectory set for the vehicle to use, where t1 is any passable trajectory extracted from the semantic lane guidance tree, cost_t(t1) represents the cost of trajectory t1 under the evaluation system; efficiency cost efficiency(t1) represents the calculated efficiency of trajectory t1 on the current road, δ e is the weight of the traffic efficiency cost; the safety cost safety(t1) represents the safety of trajectory t1 relative to obstacles, δ s The weight representing the safety cost of the trajectory;

[0076] navigation(t1) represents the navigation cost of trajectory t1, δ n is the weight of the curvature cost.

[0077] The optimal trajectory is the traversable trajectory with the lowest cost in the trajectory set. In the trajectory set, the optimal trajectory T is calculated by the optimal trajectory method that integrates the weighted fusion of multiple constraints such as traffic efficiency, safety, and semantic guidance. optimal .

[0078] The traffic efficiency cost efficiency(t1) = δ e1 *abs(velocity ego -velocity desire )+δ e2 *abs(velocity ego -velocity leadingVehicle ), where velocity ego is the average vehicle speed, velocity desire is the current road target cruising speed, velocity leadingVehicle is the speed of the vehicle ahead, δ e1 and δ e2 The calculation of efficiency cost efficiency(t1) mainly includes two parts: the average vehicle speed velocity of trajectory t1 and the average vehicle speed velocity of trajectory t1. ego The target cruising speed for the current road is velocity desire The difference between the velocity and the average speed of trajectory t1 ego Velocity with the vehicle ahead leadingVehicle The speed difference (the value is 0 if there is no vehicle ahead).

[0079] The safety cost safety(t1) is mainly used to calculate the safety level of the current trajectory. The safety cost safety(t1) is evaluated using a responsibility-sensitive safety model.

[0080] The calculation of navigation cost navigation(t1) mainly includes two parts: the current trajectory in the recommended lane part Trajectory recommend The reward, and the current trajectory is not in the recommended lane part Trajectory non-recommend The navigation cost navigation(t1)=∑δ reward *Trajectory recommend +∑δ punish *Trajectory non-recommend , where Trajectory recommend Indicates the recommended part of the vehicle’s current trajectory, Trajectory non-recommend represents the unrecommended part of the vehicle’s current trajectory, δ reward represents the reward coefficient, δ punish Represents the penalty coefficient. The recommended lanes and non-recommended lanes described here are data obtained from semantic lane navigation. For example, if the vehicle navigation is to turn left at the intersection ahead, the left-turn lane ahead is the recommended lane, and non-left-turn lanes are all non-recommended lanes.

[0081] The above three costs need to be normalized before calculation.

[0082] The present invention provides a vehicle motion planning method in a low-precision navigation environment. The technical solution proposes a semantic lane-guided rapidly-exploring random tree algorithm (Semantic Lane-Guided Rapidly-exploring RandomTree, SLG-RRT), including a sampling method based on vehicle behavior semantic guidance, a rapid pruning method based on the SLG feature tree, and a trajectory optimization method that includes a weighted fusion of multiple constraints of traffic efficiency, safety and semantic guidance. It is mainly used to solve the low sampling efficiency of motion planning algorithms in existing semantic map environments, the exponential explosion of computational difficulty in interactive games with surrounding vehicles in urban road environments, and the optimal trajectory selection problem after multiple solutions are obtained in the autonomous driving motion planning algorithm.

[0083] In summary, an embodiment of the present invention provides a vehicle motion planning method in a low-precision navigation environment. This technical solution reduces the sampling space of the RRT algorithm by using the driver's semantic guidance branch in the action space, thereby greatly improving the sampling efficiency. This technical solution does not directly consider all possible combinations of surrounding vehicle intentions on the road to compete with the vehicle itself, but rather uses the semantic behavior conditions of the vehicle itself to screen out scenarios with potential risks in surrounding vehicles, so as to reduce the computational consumption in the process of the game between the vehicle itself and surrounding vehicles, thereby reducing the computational difficulty. The present invention also proposes an optimal trajectory selection method that integrates the weighted fusion of multiple constraints of traffic efficiency, safety, and semantic guidance to improve the comfort and safety of the optimal trajectory.

[0084] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A vehicle motion planning method in a low-precision navigation environment, applied to an autonomous driving vehicle, characterized in that: The method comprises: Step 1: construct a semantic lane guidance tree based on the data information of the vehicle itself and other vehicles; Step 2, sampling is performed according to the data information to obtain sampling points, and the sampling points are divided into two branches: the vehicle part and the surrounding vehicle part; Step 3: Generate semantic preview points from the sampling point positions of the ego vehicle, select the optimal parent node, and calculate the forward extension trajectory of the ego vehicle through the fast curve extension algorithm; Step 4: According to the sampling points of the surrounding vehicles, semantically related vehicles are selected, and the forward prediction trajectory of the surrounding vehicles is calculated by the fast curve extension algorithm; Step 5: According to the forward extended trajectory of the vehicle and the forward predicted trajectory of the surrounding vehicles, determine whether there is a safe trajectory that can be passed. If not, go to step 6; if so, go to step 7; Step 6, delete the sampling point, go to step 2, and resample; Step 7: Perform node matching on the forward extended trajectory of the vehicle to obtain new data information, add it to the semantic lane guidance tree, and then go to step 2 until the semantic lane guidance tree reaches the termination condition, end the loop, and extract the trajectory set; Step 8, select the function according to the optimal trajectory: Select the optimal path from the trajectory set for the vehicle to use, where t1 is any passable trajectory extracted from the semantic lane guidance tree, cost_t(t1) represents the cost of trajectory t1 under the evaluation system; efficiency cost efficiency(t1) represents the calculated efficiency of trajectory t1 on the current road, δ e is the weight of the traffic efficiency cost; the safety cost safety(t1) represents the safety of trajectory t1 relative to obstacles, δ s represents the weight of the trajectory safety cost; navigation(t1) represents the navigation cost of trajectory t1, δ n is the weight of the curvature cost.

2. According to the method for vehicle motion planning in a low-precision navigation environment of claim 1, it is characterized in that: The data information includes posture information, topological information, semantic information and trajectory information of the vehicle.

3. According to the method of vehicle motion planning in a low-precision navigation environment as described in claim 1, it is characterized in that: The forward extension trajectory of the vehicle is (V parent ,V sample ,E), where V parent represents the optimal parent node of the selected sampling point, V sample represents the sampling point, and E represents the extended trajectory.

4. According to the method for vehicle motion planning in a low-precision navigation environment of claim 1, it is characterized in that: The forward prediction trajectory of the surrounding vehicle is (Agent id ,E), where Agent id represents the number of the semantically associated vehicle, and E represents the forward predicted trajectory of the semantically associated vehicle.

5. According to claim 3 or 4, a vehicle motion planning method in a low-precision navigation environment is characterized in that: The method for performing node matching on the forward extension trajectory of the vehicle to obtain new data information includes: Step 71: Extend the forward trajectory of the vehicle (V parent ,V sample ,E) to match nodes and obtain the matched nodes (V parent ,V child ,E); Step 72, according to the matched node (V parent ,V child ,E), get the edge (V child ,E); Step 73: Expand the new node V generated by the vehicle forward new With edge (V child ,E) combined into new data information (V new ,E new ).

6. According to the method of vehicle motion planning in a low-precision navigation environment as claimed in claim 1, it is characterized in that: The termination condition is: termination_condition=(Num Trajectory >Num threshold )||(Time calculation >Time threshold ), where Num Trajectory Indicates the number of trajectories, Num threshold Indicates the end mark of the track number, Time calculation Indicates the single calculation time, Time threshold Indicates the upper limit of a single calculation time.

7. According to the method of vehicle motion planning in a low-precision navigation environment as described in claim 1, it is characterized in that: The optimal trajectory is a traversable trajectory with the lowest cost in the trajectory set.

8. According to claim 1, a vehicle motion planning method in a low-precision navigation environment is characterized in that: The traffic efficiency cost efficiency(t1) = δ e1 *abs(velocity ego -velocity desire )+δ e2 *abs(velocity ego -velocity leadingVehicle ), where velocity ego is the average vehicle speed, velocity desire is the target cruising speed on the current road, velocity leadingVehicle is the speed of the vehicle ahead, δ e1 and δ e2 are two weights.

9. According to claim 1, a vehicle motion planning method in a low-precision navigation environment is characterized in that: The safety cost safety(t1) is evaluated using a responsibility-sensitive safety model.

10. The vehicle motion planning method in a low-precision navigation environment according to claim 1, characterized in that: The navigation cost navigation(t1)=∑δ reward *Trajectory recommend +∑δ punish *Trajectory non-recommend , where Trajectory recommend Indicates the recommended part of the vehicle’s current trajectory, Trajectory non-recommend represents the unrecommended part of the vehicle’s current trajectory, δ reward represents the reward coefficient, δ punish Represents the penalty coefficient.

Images