Layered decoupled unmanned anthropomorphic trajectory planning method

Abstract

The application relates to a layered decoupling unmanned personification trajectory planning method, which comprises the following steps: S1, upper path planning: for different road scenes, a sampled-based path planning method, a search-based path planning method and an optimization-based path planning method are combined to plan an optimal path; S2, lower speed planning: a search method based on an ST obstacle space-time collision prediction graph and vehicle acceleration constraint is adopted to obtain a speed curve of a vehicle along the optimal path, and finally an optimal trajectory is obtained. Compared with the prior art, the application provides a safer, more reliable, stable and smoother personification driving trajectory for an unmanned vehicle.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving trajectory planning technology, and in particular to a hierarchical decoupled anthropomorphic trajectory planning method for autonomous driving. Background Technology

[0002] The autonomous driving trajectory planning module is tasked with avoiding obstacles and reaching its destination in space and time based on the vehicle's current pose, surrounding environment, and behavioral decision-making information. This module connects to the decision-making and control modules and plays a crucial role in the feasibility, comfort, and efficiency of vehicle operation. Over the past decade, extensive research and implementation efforts have been invested in autonomous vehicles both domestically and internationally. However, even for leading companies and research institutions in this field, achieving human-like navigation in dense urban environments with blind spots and highly interactive behaviors remains an extremely challenging task. In this scenario, autonomous vehicles need to predict the behavior of other road users, consider the dynamic and static environment within blind spots, and interact with uncertain dynamic obstacles in the surrounding area to plan a safe and efficient driving trajectory in space and time.

[0003] As a special type of robot, the trajectory planning methods for autonomous vehicles mostly originate from research in the field of mobile robots. This field mostly faces static deterministic environments. In response to the complex dynamic interactive environment faced by autonomous driving, some studies consider both static and dynamic environments simultaneously, attempting to solve a multi-objective optimization problem in both spatial and temporal dimensions. However, this is difficult and slow. Therefore, most studies decouple the trajectory planning problem into a path planning problem and a velocity planning problem.

[0004] For path planning, autonomous vehicles only consider static obstacles in the environment and often employ planning methods derived from the robotics field, but each method has its limitations. For example, the artificial potential field method is prone to getting stuck in dead zones in complex autonomous driving scenarios; random sampling methods suffer from non-optimal solutions and high randomness, easily leading to result jumps in highly real-time systems like autonomous driving; control rule-based sampling methods struggle to achieve consistency across multiple scenarios; road rule-based sampling methods are difficult to navigate on unstructured complex roads and face switching issues; search methods, while capable of planning a reasonable path in complex scenarios, lose completeness when geometric constraints or vehicle kinematic constraints are considered in the generation of child nodes, meaning some feasible solutions are lost; and while optimization-based methods can solve problems in continuous space, ensuring consistency in the selection and normalization of optimization indices under different environments remains a significant challenge.

[0005] For speed planning, autonomous vehicles first need to predict the trajectories of dynamic obstacles in the environment, and then determine how the trajectory of the target affects the vehicle's trajectory. Existing methods often project dynamic obstacles onto a Cartesian coordinate system (ST) graph with the longitudinal length of the planned path as the ordinate and time as the abscissa, and then seek the optimal speed curve within this graph. The methods for finding the speed curve are similar to finding the optimal path in a Cartesian coordinate system, including sampling-based methods, search-based methods, and optimization-based methods. Among these, sampling-based methods struggle to consistently satisfy the continuity of vehicle acceleration constraints, and the resulting solutions are often random. Existing search-based methods do not consider acceleration constraints and search space constraints, resulting in slow computation. Optimization-based methods struggle to establish a standard objective function during speed planning and cannot achieve consistency across different objectives in various scenarios. Summary of the Invention

[0006] The purpose of this invention is to overcome the shortcomings of the existing technology and provide a safe, stable, and efficient hierarchical decoupled anthropomorphic trajectory planning method for autonomous driving.

[0007] The objective of this invention can be achieved through the following technical solutions:

[0008] This invention presents a hierarchical decoupled anthropomorphic trajectory planning method for autonomous driving, the method comprising:

[0009] Step S1, Upper-level path planning: For different road scenarios, the sampling-based path planning method, the search-based path planning method, and the optimization-based path planning method are combined to plan the optimal path;

[0010] Step S2, Lower-level velocity planning: A search method based on ST obstacle spatiotemporal collision prediction map and vehicle acceleration constraints is used to obtain the velocity curve of the vehicle along the optimal path, and finally obtain the optimal trajectory.

[0011] Preferably, step S1 specifically comprises:

[0012] On structured roads, a hybrid path planning method based on sampling and optimization is used to obtain the optimal path;

[0013] When no candidate path can be planned for unstructured roads or structured roads, a search-based path planning method is used to obtain the desired path, and then a hybrid path planning method based on sampling and optimization is used to obtain the optimal path.

[0014] Preferably, the search-based path planning method is a hybrid A* algorithm combining vehicle kinematics and Reeds-Shepp curves in lane coordinates, specifically including the following sub-steps:

[0015] Step S101: Based on vehicle kinematic constraints, obtain a candidate node sequence using a node search method;

[0016] Step S102: For all nodes in the candidate node sequence, determine whether they will collide with the environment using a collision detection algorithm, and mark the colliding nodes as invalid nodes.

[0017] Step S103: After step S102, multiple effective nodes are obtained. The sum of the loss to reach the current node and the heuristic loss from the current node to the target point is used as the evaluation index to select the local optimal node.

[0018] Step S104: Use the Reeds-Shepp curve to connect the node to the target point for exploratory path planning. If there is no collision, exit the A* algorithm search and connect the nodes obtained by the A* algorithm with the hybrid vehicle kinematics and the Reeds-Shepp curve in sequence as the desired path.

[0019] Preferably, in the node search method of step S101, the search method for the next node satisfies vehicle kinematic constraints, expressed as:

[0020]

[0021] In the formula, These are the x-coordinate, y-coordinate, and heading angle of the next node, respectively. At the current speed, Side slip angle, This refers to the front overhang length. This refers to the rear overhang length; The front wheel steering angle is processed using a grid within a set range.

[0022] Preferably, the target point selection process in the hybrid A* algorithm is as follows: a method of scattering points laterally at a certain distance along the longitudinal direction of the lane centerline is used as the candidate target point set.

[0023] Preferably, the hybrid path planning method based on sampling and optimization specifically involves: transforming the position of each node planned in the lane coordinate system by the sampling-based path planning method to the Cartesian coordinate system, and using it as the input of the optimization-based path planning algorithm; wherein, the optimization-based path planning algorithm simultaneously considers hard constraints and soft constraints to obtain the optimal path through dynamic programming.

[0024] Preferably, the sampling-based path planning method employs a point-scattering method based on road shape, specifically:

[0025] In the lane coordinate system, the horizontal coordinate 's' is taken along the tangent direction of the lane centerline, and the vertical coordinate 'd' is taken along the normal direction of the lane centerline. The longitudinal planning length is positively correlated with the vehicle speed, and the point spacing is within a set range. Horizontally, a point is placed at a set interval, and points equal to half the lane width are placed on both the left and right sides, resulting in hierarchical candidate nodes. Nodes in adjacent layers are connected using a fifth-order polynomial.

[0026] The position of each node planned in the Frenet lane coordinate system is transformed into the Cartesian coordinate system and used as input for subsequent optimization path planning algorithms.

[0027] Preferably, the hard constraints include environmental and traffic constraints, vehicle kinematic constraints, and non-integrity constraints;

[0028] The soft constraints include lateral deviation constraints from the desired path, path curvature constraints, obstacle repulsion constraints, frame-to-frame similarity constraints, and collision constraints between the pre-aimed path and obstacles; wherein, the pre-aimed path is a speed-related distance extended in the direction of the endpoint.

[0029] Preferably, in the similarity constraint between frames, the mathematical expression of the similarity index is:

[0030]

[0031] in, It is the standard normal distribution function. , , These represent the Fraser distance, the closest distance, and the farthest distance between the two curves, respectively. This is the set similarity sensitivity coefficient.

[0032] Preferably, step S2 includes the following sub-steps:

[0033] Step S201: Given the optimal path, predict the motion of dynamic obstacles in the environment, identify the impact on the longitudinal distance in the optimal path and the corresponding time, and project the information onto the ST graph with time as the horizontal axis and the longitudinal distance of the optimal path as the vertical axis.

[0034] Step S202: The improved hybrid A* algorithm is used for searching. Specifically, the maximum longitudinal acceleration and deceleration interval of the vehicle is rasterized and used as the input for node search. After obtaining multiple nodes, the absolute value of longitudinal acceleration, longitudinal jerk, lateral acceleration, longitudinal distance from the target point, and difference from the desired speed are used as local optimization indicators. The locally optimal node is selected and added to the open table of the improved hybrid A* algorithm, and the close table search is performed. The exit condition of the improved hybrid A* algorithm is to stop searching when a certain specific time is reached. In other words, the speed planning search method of the present invention seeks the farthest distance that can be reached within a certain time to avoid the situation of no solution as much as possible.

[0035] Compared with the prior art, the present invention has the following advantages:

[0036] 1) The trajectory planning algorithm, which adopts a hierarchical decoupling of path and speed, can provide safer, more reliable, stable and smoother anthropomorphic driving trajectories for autonomous driving;

[0037] 2) A combined path planning method based on sampling, search and optimization is adopted to plan paths for different road scenarios and spontaneous switching based on decision input, so as to efficiently obtain feasible optimal solutions and cope with safe and efficient driving in any road scenario.

[0038] 3) For unstructured roads or when no candidate path can be planned on structured roads, a search-based path planning method is adopted. Grid sampling is performed in the lane coordinate system, and points are scattered along a vertical line of a certain length along the longitudinal direction of the lane. The target point and feasible paths to the target point are searched by trial and error, thereby solving the problem of the difficulty in determining the target point.

[0039] 4) Compared with existing methods that only consider the curvature and lateral deviation of the planned path, the optimization-based path planning method adopted in this invention additionally considers obstacle repulsion factors, adjacent frame similarity factors, and pre-aiming planning factors that mimic human drivers, making the planned path safer, more stable, and more human-like.

[0040] 5) The speed A* search method based on ST graph uses vehicle acceleration as the basis for node search. The search range and search space satisfy vehicle kinematic constraints. In addition, the heuristic function used for selecting the local optimal target point in the improved hybrid A* algorithm is only related to the longitudinal distance of the path. The cost function considers longitudinal acceleration, longitudinal jerk, lateral acceleration and the difference from the desired speed, making the speed planned by this method smoother and more in line with road traffic constraints.

[0041] 6) The speed A* search method based on the ST graph degenerates the exit condition from the existing algorithm's requirement to reach a specific node in the ST graph to simply searching for a certain time node while ensuring the travel distance is maximized. From a physical perspective, since path planning and speed planning are performed in real time, it is not guaranteed that the vehicle will reach the target point within a finite time during the iteration process. However, it is certain that a speed curve that reaches the farthest distance within the search time can be found. This method improves the stability of speed planning. Attached Figure Description

[0042] Figure 1 Architecture diagram for a layered, decoupled, anthropomorphic planning method for autonomous driving. Detailed Implementation

[0043] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0044] Example

[0045] This embodiment presents a hierarchical decoupled anthropomorphic trajectory planning method for autonomous driving, which includes:

[0046] The upper-level path planner combines sampling-based, search-based, and optimization-based methods to handle any road scenario. The search-based method solves the problem of difficult target point determination by conducting a trial search in the lane coordinate system. The optimization-based method obtains a more human-like driving path by considering factors such as lateral deviation, curvature, obstacle repulsion force, inter-frame path similarity, and whether the pre-aimed path will collide.

[0047] The lower-level speed planner is based on the spatiotemporal collision graph of dynamic obstacles and the optimal path. It also considers vehicle acceleration constraints and uses the A* algorithm with hybrid vehicle kinematics to search for speed curves in the path-time graph (ST graph). The selection of local optimal nodes considers both lateral and longitudinal acceleration, longitudinal jerk and the difference with the desired speed. The algorithm exit condition degenerates from a certain time to a certain place to searching to a certain time. The weight of the heuristic function of the A* algorithm is strengthened to represent the anthropomorphic intention of wanting to go further in the same amount of time.

[0048] The aforementioned layered decoupled trajectory planning algorithm enables autonomous vehicles to plan trajectories in complex environments, allowing them to traverse mixed traffic sections in a human-like manner, providing a safer, more reliable, stable, and smooth human-like driving trajectory for autonomous driving.

[0049] In actual operation, see Figure 1 First, the optimal path is generated using the path planner of the upper-level hybrid method. The upper-level framework operates as follows:

[0050] Step 1: The upper-level path planner is input with a reference path derived from global planning using a high-precision map, scene understanding information from the decision module, and static obstacle information obtained through perception. Based on these inputs, the path planner needs to choose a method: when the road ahead is structured, a planner based on lane shape sampling and optimization is used; when the road ahead is unstructured, lane information is destroyed and cannot be used, so a search-based planner is needed to obtain a rough expected path within the feasible space, which is then input into the sampling-based and optimization-based planners to obtain the optimal path.

[0051] Step 2: The sampling-based planner first transforms the global Cartesian coordinate system into the Frenet coordinate system. This reduces the dimensionality of subsequent computations in the underlying framework. These represent the corresponding node position, velocity, acceleration, orientation angle, and curvature, respectively. These represent the longitudinal and lateral distances along the desired path, respectively. Next, sampling points are hierarchically distributed along the Frenet coordinate system, and a fifth-order polynomial is used to connect these sampling points hierarchically. This initial screening process identifies paths that might collide with obstacles or curbs, resulting in candidate paths. It's important to note that if no solution is found for the output candidate path, although the decision information suggests driving along a structured road, this sampling-based planner may fail to find a solution due to the numerous constraints imposed. In this case, the path planner will spontaneously use a search-based planner to seek a feasible solution.

[0052] Step 3: The search-based planner still searches for feasible solutions in the Frenet coordinate system, using the hybrid A* algorithm. Considering the vehicle's kinematic model, the next node is sought using the following formula. The heuristic function uses the Manhattan distance between nodes and the destination. Furthermore, when a collision-free connection between the next node and the destination can be achieved using a Reeds-Shepp curve, the A* algorithm search is terminated to improve efficiency. The path obtained using this algorithm is fed as the desired path into a sample-based planner, allowing the autonomous vehicle to find more feasible solutions in space, which is especially important when navigating complex roads.

[0053] Step 4: The optimization-based planner uses dynamic programming to optimize layer by layer to reduce computational dimensionality. Hard constraints consider obstacles and traffic constraints, vehicle kinematics constraints, and vehicle incompleteness constraints; soft constraints consider lateral deviation from the desired path, curvature, obstacle repulsion force, path similarity between frames, and whether there are obstacles on the target path. Ultimately, the path planning result is a feasible, reliable, safe, and stable anthropomorphic optimal path.

[0054] For both soft and hard constraints, the specifics are as follows:

[0055] 1) Incorporate obstacle repulsion factors that consider the artificial potential field method into soft constraints, and "grayscale" the obstacle factors;

[0056] 2) Use the Fraser distance as the distance between the two curves, and use...

[0057]

[0058] As a similarity metric, the similarity between frames is compared to mitigate path planning jumps. It is the standard normal distribution function. , , These represent the Fraser distance, the closest distance, and the farthest distance between the two curves, respectively. The similarity sensitivity coefficient is set to 3 in this embodiment;

[0059] 3) Since people always have an expectation of the vehicle's trajectory when driving, they will always turn the steering wheel in advance when avoiding obstacles, rather than turning the steering wheel sharply when they are close to the obstacle. Therefore, this patent uses the distance related to speed extended along the end direction of each alternative path as the aiming distance, and judges whether it will collide with the static obstacle as an optimization index to imitate the intention of people to avoid obstacles in advance.

[0060] See Figure 1 The modeling and computation method of the lower-level velocity planning framework based on the ST obstacle spatiotemporal collision prediction map and search method is as follows:

[0061] Step 1: Perform spatiotemporal collision detection between the predicted trajectory of the dynamic obstacle and the optimal path to determine when the dynamic obstacle affects the optimal driving path, and project the collision point in spatiotemporal onto the path length-time graph with the driving path as the vertical axis and time as the horizontal axis.

[0062] Step Two: After projecting the dynamic obstacles onto the ST graph, a controllable velocity curve for the vehicle needs to be obtained from this graph to achieve trajectory planning. This paper adopts the A* algorithm with vehicle acceleration as the search input, and the nodes are calculated using the following formula. The cost function for each node is shown below:

[0063] .

[0064] in This represents the total length of the optimal path to the target point. The desired speed for the current lane. , In this embodiment, the cost coefficients are used to select acceleration for the previous node and the current node, respectively. .

[0065] Step 3: Existing search methods for speed curves in the ST graph aim to reach a certain position at a certain time. This exit condition often prevents speed planning from finding a feasible solution. At this point, the vehicle will stop, which is an overly conservative driving strategy unlike that of human drivers. This is because the search termination condition is too stringent. In fact, humans adopt a step-by-step, tentative strategy, not thinking about when to reach a certain place, but hoping to go as far as possible within a given time. Based on this goal, we set the exit condition of the search algorithm to exceed a certain time and strengthened the intention to go further, making the speed curve acquisition easier and more human-like, achieving the goal of not being overly conservative in complex road sections.

[0066] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any person skilled in the art can easily conceive of various equivalent modifications or substitutions within the technical scope disclosed in the present invention, and these modifications or substitutions should all be covered within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A hierarchical decoupled anthropomorphic trajectory planning method for autonomous driving, characterized in that, The method includes: Step S1, Upper-level path planning: For different road scenarios, the sampling-based path planning method, the search-based path planning method, and the optimization-based path planning method are combined to plan the optimal path, specifically as follows: On structured roads, a hybrid path planning method based on sampling and optimization is used to obtain the optimal path; When no candidate path can be planned for unstructured roads or structured roads, a search-based path planning method is used to obtain the desired path, and then a hybrid path planning method based on sampling and optimization is used to obtain the optimal path. The hybrid path planning method based on sampling and optimization specifically involves transforming the position of each node obtained by the sampling-based path planning method in the lane coordinate system to the Cartesian coordinate system, which is then used as the input to the optimization-based path planning algorithm. The optimization-based path planning algorithm simultaneously considers both hard and soft constraints and obtains the optimal path through dynamic programming. Step S2, Lower-level velocity planning: A search method based on the ST obstacle spatiotemporal collision prediction map and vehicle acceleration constraints is used to obtain the vehicle's velocity curve along the optimal path, ultimately obtaining the optimal trajectory. This includes the following sub-steps: Step S201: Given the optimal path, predict the motion of dynamic obstacles in the environment, identify the impact on the longitudinal distance in the optimal path and the corresponding time, and project the information onto the ST graph with time as the horizontal axis and the longitudinal distance of the optimal path as the vertical axis. Step S202: Use the improved hybrid A* algorithm for searching. Specifically, the maximum longitudinal acceleration and deceleration interval of the vehicle is rasterized and used as the input for node search. After obtaining multiple nodes, the absolute value of longitudinal acceleration, longitudinal jerk, lateral acceleration, longitudinal distance from the target point, and difference from the desired speed are used as local optimization indicators. The locally optimal node is selected and added to the open table of the improved hybrid A* algorithm, and the close table search is performed. The exit condition of the improved hybrid A* algorithm is to stop searching when a certain specific time is reached.

2. The hierarchical decoupled anthropomorphic trajectory planning method for unmanned driving according to claim 1, characterized in that, The search-based path planning method is a hybrid A* algorithm combining vehicle kinematics and Reeds-Shepp curves in lane coordinates, specifically including the following sub-steps: Step S101: Based on vehicle kinematic constraints, obtain a candidate node sequence using a node search method; Step S102: For all nodes in the candidate node sequence, determine whether they will collide with the environment using a collision detection algorithm, and mark the colliding nodes as invalid nodes. Step S103: After step S102, multiple effective nodes are obtained. The sum of the loss to reach the current node and the heuristic loss from the current node to the target point is used as the evaluation index to select the local optimal node. Step S104: Use the Reeds-Shepp curve to connect the node to the target point for exploratory path planning. If there is no collision, exit the A* algorithm search and connect the nodes obtained by the A* algorithm with the hybrid vehicle kinematics and the Reeds-Shepp curve in sequence as the desired path.

3. The hierarchical decoupled anthropomorphic trajectory planning method for unmanned driving according to claim 2, characterized in that, In the node search method of step S101, the search method for the next node satisfies the vehicle kinematic constraints, and the expression is: In the formula, These are the x-coordinate, y-coordinate, and heading angle of the next node, respectively. Current speed; Side slip angle, This refers to the front overhang length. This refers to the rear overhang length; The front wheel steering angle is processed using a grid within a set range.

4. The hierarchical decoupled anthropomorphic trajectory planning method for unmanned driving according to claim 2, characterized in that, The target point selection process in the hybrid A* algorithm is as follows: a set of candidate target points is selected by scattering points laterally at a certain distance along the longitudinal direction of the lane centerline.

5. The hierarchical decoupled anthropomorphic trajectory planning method for unmanned driving according to claim 1, characterized in that, The sampling-based path planning method employs a point-scattering method based on road shape, specifically: In the lane coordinate system, the horizontal coordinate s is taken along the tangent direction of the lane centerline, and the vertical coordinate d is taken along the normal direction of the lane centerline. The longitudinal planning length is positively correlated with the vehicle speed. The point spacing is within the set range. A point is placed every set distance in the horizontal direction. Points of half the lane width are placed on the left and right sides to obtain hierarchical candidate nodes.

6. The hierarchical decoupled anthropomorphic trajectory planning method for unmanned driving according to claim 1, characterized in that, The hard constraints include environmental and traffic constraints, vehicle kinematic constraints, and non-integrity constraints; The soft constraints include lateral deviation constraints from the desired path, path curvature constraints, obstacle repulsion constraints, frame-to-frame similarity constraints, and collision constraints between the pre-aimed path and obstacles; wherein, the pre-aimed path is a speed-related distance extended in the direction of the endpoint.

7. The hierarchical decoupled anthropomorphic trajectory planning method for unmanned driving according to claim 6, characterized in that, In the frame-to-frame similarity constraint, the mathematical expression for the similarity index is: in, It is the standard normal distribution function. , , These represent the Fraser distance, the closest distance, and the farthest distance between the two curves, respectively. This is the set similarity sensitivity coefficient.

Images