Low-time-consumption path planning method and device for automatic driving vehicle and storage medium

Abstract

The invention discloses a low-time-consumption path planning method and device for an automatic driving vehicle and a storage medium, and the method comprises the following steps: obtaining input information, generating an initial solution, designing a cost function, converting the initial solution to a Frenet coordinate system, carrying out the iterative optimization solving, and if the path solving is successful, converting a solving result from the Frenet coordinate system to a Cartesian coordinate system. The method has the advantages that the state transition equation is simplified based on the Frenet coordinate system, only one optimization variable is reserved, the calculation efficiency of the LQR algorithm is greatly improved, and the performance requirement can be met on a low-calculation-power platform.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a low-time path planning method and storage medium for an autonomous driving vehicle. Background Art

[0002] Path planning is crucial to autonomous driving technology, serving as the intermediate link between environmental perception and control execution, and a crucial module for the intelligence of autonomous vehicles. For low-speed autonomous vehicles, trajectory planning with time-space decoupling is currently the mainstream solution due to its simple technical links and low-cost computing platform requirements. Path planning is the foundation of trajectory planning and determines the autonomous vehicle's driving space; its interactive avoidance of static obstacles determines the product's intelligence and driving safety.

[0003] For low-speed self-driving sanitation vehicles, they will often encounter various randomly appearing obstacles when driving on sidewalks and auxiliary roads, which are the so-called "dilemma". The vehicle can continue to perform the cleaning task only after it autonomously drives "out of the current dilemma" (i.e., the "escaping task"). The escape task requires planning a collision-free path that complies with kinematic constraints. Commonly used path planners include Hybrid A Star, RRT, Voronoi Plan and many other methods. In the practical problems faced by the present invention, the kinematic constraints of the vehicle cannot be considered based on Voronoi Plan; RRT is a fast random tree, and the generated path is not optimal and is extremely dependent on randomness, which makes the output of the algorithm result uncertain; Hybrid A Star is developed on the basis of A Star. Although it meets the kinematic constraints, its heuristic function design is also difficult. The node expansion behavior affects the uncontrollable path, and its planning ability in a narrow space drops sharply, making it difficult to meet actual use needs.

[0004] CN202411019423.8 discloses a constrained iteration-based LQR motion planning method, device, medium, and product. This method targets structured roads and only plans the vehicle's forward direction.

[0005] CN202410889952.7 discloses a global and local fusion path planning method, device, and storage medium. This method also only plans forward-only paths and is difficult to apply in narrow scenarios where reversing is required.

[0006] CN202410169136.9 discloses an electric vehicle control method, computer device, and computer-readable storage medium that applies an iterative LQR method to vehicle control. The method uses a dynamic vehicle model, which, in practical engineering applications, is computationally intensive and difficult to meet the real-time requirements of low-computing-power platforms. Summary of the Invention

[0007] To solve the above problems of the existing technical solutions, the present invention discloses a low-latency path planning method, device and storage medium for autonomous vehicles, and proposes a path planning method that can be applied on a low-computing-power platform, which integrates the dual actions of reversing and moving forward in the same optimization problem in a narrow space scenario to improve the solution efficiency of the problem.

[0008] To achieve the above object, the present invention adopts the following technical solutions: A low-latency path planning method for autonomous vehicles, comprising the following steps:

[0009] S1. First, obtain input information; obtain the current vehicle pose (x0, y0, θ0), that is, the starting point of the planning, where x0, y0, and θ0 respectively represent the x coordinate, y coordinate, and heading angle of the vehicle at the current position;

[0010] S2. Secondly, generate an initial solution, gradually increase the reversing distance d back from 0, and calculate the driving path for this section of distance using the iterative LQR algorithm with multi-objective optimization;

[0011] S3. Design a cost function, and the multi-objective cost function is designed as:

[0012]

[0013] where w fl , w fθ respectively represent the terminal lateral deviation constraint weight and the terminal heading deviation constraint weight, which are the constraint capabilities for the pose of the terminal target point, and w r , w κ , w dκ respectively represent the constraint weight for the path deviating from the reference line, the path curvature constraint weight, and the path curvature change rate constraint weight;

[0014] Ψ(κ k , κ max ), Ψ(l k , l up ), Ψ(l k , -l low ) respectively represent the curvature boundary constraint, the collision upper bound constraint, and the collision lower bound constraint, and perform hard constraints on the collision and kinematic boundaries in the form of obstacle functions;

[0015] κ max represents the maximum curvature of the vehicle's travel, that is, the reciprocal of the minimum turning radius of the vehicle;

[0016] l k,up represents the upper bound lateral coordinate of the collision constraint at the k-th point, and l k,lowDenote the lower bound of the lateral coordinate of the collision constraint at the k-th point;

[0017] S4. Convert the initial solution to the Frenet coordinate system; using the reference line as the coordinate axis of the Frenet coordinate system, the initial solution is converted from Cartesian coordinates to Frenet coordinates, denoted as P k (l k , θ k , κ k , dκ k , s k ) ∈ Π, (k = 0, 1,..., N - 1), where k = 0, 1,…, N - 1 indicates that the initial solution consists of N discrete points, and the k-th point (l k , θ k , κ k , dκ k , s k ) represents the lateral coordinate, heading angle, curvature, curvature change rate, and longitudinal coordinate in the Frenet coordinate system, respectively;

[0018] The state transition equation in the Frenet coordinates is designed as:

[0019]

[0020] where the state vector is the input vector is δs is the change in the longitudinal coordinate between adjacent points, determined by the initial solution; therefore, the actual input vector is only dκ k ; k f,k is the curvature corresponding to the projection point of the k-th point on the reference line;

[0021] S5. Iteratively optimize and solve. The iterative LQR solution is divided into iterative backward propagation and forward propagation; backward propagation is to solve the optimal control rate sequence starting from the end of the solution; forward propagation is to solve the state sequence by applying the optimal control rate sequence from the initial state, and iterate repeatedly until the convergence threshold is reached;

[0022] S6. If the solution path is successful, convert the solution result from the Frenet coordinate system back to the Cartesian coordinate system.

[0023] Furthermore, in S2, when applying the iterative LQR algorithm for multi-objective optimization, the following steps are taken:

[0024] Generate the initial solution, denoted as P k (x k , y k , θ k , κ k , dκ k) ∈ Γ, (k = 0, 1, ..., n - 1), (k = 0, 1, ..., n - 1), where the path points (x k , y k , θ k , κ k , dκ k ) represent the x - coordinate, y - coordinate, heading angle, curvature, and curvature change rate of the discrete path points of the initial solution in the Cartesian coordinate system respectively, and the reverse distance is d back ≥0.

[0025] Furthermore, the initial solution is constructed by three segments:

[0026] Reverse segment: from the planning start point to the shift point, a straight - line segment is used, and a line segment with a length of d back starting from the planning start point and extending backward is used as the initial solution of the reverse path;

[0027] Transition segment: from the shift point to the reference - line splicing point, with the end point of the reverse segment as the start point and a suitable point on the reference line as the end point, it is connected with a smooth curve as the first - segment initial solution of the forward path;

[0028] Target segment: from the reference - line splicing point to the target point, with the end point of the transition segment as the start point, extending along the reference line until the end point of the reference line, i.e., the target point, as the second - segment initial solution of the forward path;

[0029] When d back = 0, the vehicle does not reverse, and the vehicle only has a forward movement. Correspondingly, the initial solution only consists of two ends, namely the transition segment and the target segment.

[0030] Furthermore, in S4, the state - transfer equation is discretely linearized as:

[0031] Δ k+1 = AΔ k + Bu k

[0032] where u k = dκ k , and A, B are the state matrix and input matrix after linearization respectively, that is:

[0033]

[0034] Furthermore, finally, the path information that satisfies the kinematic constraints and collision constraints is sent to the speed - planning module, and a trajectory is generated and handed over to the control execution module.

[0035] The present invention also provides an apparatus for low - time - consuming path planning of an autonomous vehicle, including:

[0036] An information acquisition module, which is used to obtain input information; obtain the current vehicle pose (x0, y0, θ0), that is, the planned starting point, where x0, y0, and θ0 respectively represent the x coordinate, y coordinate, and heading angle of the vehicle at the current position;

[0037] An initial solution generation module, which is used to generate an initial solution, and gradually increase the reverse distance d back starting from 0, and calculate the driving path for this section of distance using the iterative LQR algorithm of multi-objective optimization;

[0038] A cost function setting module, which is used to set a multi-objective cost function:

[0039]

[0040] where, w fl , w fθ respectively represent the terminal lateral deviation constraint weight and the terminal heading deviation constraint weight, which are the constraint capabilities for the pose of the terminal target point, w r , w κ , w dκ respectively represent the constraint weight for the path deviating from the reference line, the path curvature constraint weight, and the path curvature change rate constraint weight;

[0041] Ψ(κ k , κ max ), Ψ(l k , l up ), Ψ(l k , -l low ) respectively represent the curvature boundary constraint, the collision upper bound constraint, and the collision lower bound constraint, and perform hard constraints on the collision and kinematic boundaries in the form of obstacle functions;

[0042] κ max represents the maximum curvature of the vehicle's driving, that is, the reciprocal of the minimum turning radius of the vehicle;

[0043] l k,up represents the upper bound lateral coordinate of the collision constraint at the kth point, and l k,low represents the lower bound lateral coordinate of the collision constraint at the kth point;

[0044] A Frenet coordinate system conversion module, which is used to convert the initial solution to the Frenet coordinate system; using the reference line as the coordinate axis of the Frenet coordinate system, the initial solution is converted from Cartesian coordinates to Frenet coordinates, denoted as P k (l k , θ k , κ k , dκ k , s k) ∈ Π, (k = 0, 1, ..., N - 1), where k = 0, 1, …, N - 1 indicates that the initial solution consists of N discrete points, and the k-th point (l k , θ k , k k , dκ k , s k ) represent the lateral coordinate, heading angle, curvature, curvature change rate, and longitudinal coordinate in the Frenet coordinate system, respectively;

[0045] The state transition equation in the Frenet coordinate system is designed as:

[0046] l k+1 = l k + (1 - κ f，k × l k ) × tanθ k × δs

[0047] where the state vector is The input vector is δs is the longitudinal coordinate change between adjacent points, determined by the initial solution; therefore, the actual input vector is only dκ k ; k f,k is the curvature corresponding to the projection point of the k-th point on the reference line;

[0048] The iterative optimization solving module is used for iterative optimization solving. The iterative LQR solving is divided into iterative backward propagation and forward propagation; backward propagation means solving the optimal control rate sequence starting from the end of the solution; forward propagation means applying the optimal control rate sequence from the initial state to solve the state sequence, and iterating repeatedly until the convergence threshold is reached;

[0049] The coordinate system rotation module is used to rotate the solution result back from the Frenet coordinate system to the Cartesian coordinate system when the solution path is successful.

[0050] Furthermore, when the initial solution generation module generates the initial solution by applying the iterative LQR algorithm for multi-objective optimization, it is denoted as P k (x k , y k , θ k , κ k , dκ k ) ∈ Γ, (k = 0, 1, ..., n - 1), (k = 0, 1, ..., n - 1), where the path points (x k , y k , θ k , κ k , dκ k ) represent the x coordinate, y coordinate, heading angle, curvature, and curvature change rate of the initial solution discrete path points in the Cartesian coordinate system, respectively, and the reverse distance is dback ≥ 0.

[0051] Furthermore, the initial solution is constructed from three segments:

[0052] Reverse segment (from the planning start point to the shift point): A straight line segment is used, extending backward from the planning start point by a length d back of the line segment as the initial solution of the reverse path;

[0053] Transition segment: From the shift point to the reference line splicing point, starting from the end point of the reverse segment and ending at a suitable point on the reference line, a smooth curve is used to connect as the first initial solution of the forward path;

[0054] Target segment: From the reference line splicing point to the target point, starting from the end point of the transition segment and extending along the reference line to the end point of the reference line, i.e., the target point, as the second initial solution of the forward path;

[0055] When d back = 0, the vehicle does not reverse, and the vehicle only has a forward movement. Accordingly, the initial solution only consists of two ends, namely the transition segment and the target segment.

[0056] Furthermore, when the Frenet coordinate system conversion module processes coordinate conversion, the state transition equation is discretized and linearized as:

[0057] Δ k+1 = AΔ k + Bu k

[0058] where u k = dκ k , and A and B are the state matrix and input matrix after linearization, respectively, i.e.:

[0059]

[0060] A computer-readable storage medium of the present invention includes a computer program, and when the computer program is executed by one or more processors, the low-latency path planning method for an autonomous vehicle described in any one of the above is implemented.

[0061] The present invention has the following advantages: Based on the Frenet coordinate system, the state transition equation is simplified, and only one optimization variable is retained, so that the calculation efficiency of the LQR algorithm is greatly improved, and the performance requirements can also be met on a low-computing-power platform.

[0062] The dual actions of reversing and forward movement are integrated into one optimization problem, unifying the problem-solving space.

[0063] Through the path optimization method of traversing the reverse distance, the optimization dimension is reduced from three dimensions to one dimension, simplifying the design difficulty, and at the same time ensuring the controllability and interpretability of the planned path. Brief Description of the Drawings

[0064] Figure 1 It is a schematic flowchart of a low-time-consuming path planning method for an autonomous vehicle according to the present invention;

[0065] Figure 2 It is a schematic diagram of the drivable channel space according to the present invention;

[0066] Figure 3 It is a schematic diagram of converting the solution result from the Frenet coordinate system back to the Cartesian coordinate system according to the present invention. Detailed Embodiments

[0067] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0068] As Figures 1-3 shown, a low-time-consuming path planning method for an autonomous vehicle includes the following steps:

[0069] S1. First, obtain input information; for example, according to the map positioning module and vehicle chassis information, obtain the current vehicle pose (x0, y0, θ0), that is, the starting point of the planning, where x0, y0, and θ0 respectively represent the x coordinate, y coordinate, and heading angle of the vehicle at the current position; the reference line information issued by the vehicle task module is the rough path information for the vehicle to execute the task, guiding the direction of the vehicle's advance, but not meeting the kinematic and collision constraints. According to the vehicle vision, radar and other environmental perception modules, obtain the environmental fusion information, and based on the reference line, obtain the drivable channel space of the vehicle. The drivable channel space describes the upper and lower bounds that can be reached in the space along the reference line direction. See attached Figure 2 , the area enclosed by the green dashed line is the drivable area of the vehicle; the yellow dashed box represents the outer contour line of the vehicle; the red dot represents the center of the rear axle of the vehicle; the blue dashed line represents the operation reference line issued by the task module; the yellow solid line represents the initial solution composed of multiple segments.

[0070] S2. Secondly, generate an initial solution, gradually increase the reverse distance d back from 0, and use the iterative LQR algorithm of multi-objective optimization to calculate the driving path for this section of distance;

[0071] S3. Design a cost function, and the multi-objective cost function is designed as:

[0072]

[0073] where, w fl, w fθ respectively represent the lateral deviation constraint weight of the terminal and the heading deviation constraint weight of the terminal, which are the constraint capabilities for the pose of the terminal target point, w r , w κ , w dκ respectively represent the constraint weight of the path deviating from the reference line, the path curvature constraint weight, and the path curvature change rate constraint weight;

[0074] Ψ(κ k , κ max ), Ψ(l k , l up ), Ψ(l k , -l low ) respectively represent the curvature boundary constraint, the collision upper bound constraint, and the collision lower bound constraint, which perform hard constraints on the collision and kinematic boundaries in the form of an obstacle function;

[0075] κ max represents the maximum curvature of the vehicle's travel, that is, the reciprocal of the minimum turning radius of the vehicle;

[0076] l k,up represents the upper bound lateral coordinate of the collision constraint at the k-th point, and l k,low represents the lower bound lateral coordinate of the collision constraint at the k-th point;

[0077] Ψ(x, b) = q × e p(x-b) + q × e p(-x-b) represents the obstacle function, b represents the constraint boundary, and q and p represent the coefficients of the obstacle function, which are used to adjust the shape of the obstacle function.

[0078] S4. The initial solution is transformed into the Frenet coordinate system; taking the reference line as the coordinate axis of the Frenet coordinate system, the initial solution is transformed from Cartesian coordinates to Frenet coordinates, denoted as P k (l k , θ k , κ k , dκ k , s k ) ∈ Π, (k = 0, 1,..., N - 1), where k = 0, 1,…, N - 1 indicates that the initial solution consists of N discrete points, and the k-th point (l k , θ k , κ k , dκ k , s k ) respectively represent the lateral coordinate, heading angle, curvature, curvature change rate, and longitudinal coordinate in the Frenet coordinate system;

[0079] The state transition equation in the Frenet coordinates is designed as:

[0080]

[0081] Among them, the state vector is The input vector is δs is the longitudinal coordinate change between two adjacent points, which is determined by the initial value; therefore, the actual input vector has only dκ k ;

[0082] Among them, k f,k (which can also be expressed as: \kappa_{f,k}, the same below), is the curvature corresponding to the projection point of the k-th point on the reference line;

[0083] S5. Iterative optimization solution. The iterative LQR solution is divided into iterative backward propagation and forward propagation; backward propagation means solving the optimal control rate sequence starting from the end of the solution; forward propagation means applying the optimal control rate sequence from the initial state to solve the state sequence, and iterating repeatedly until the convergence threshold is reached;

[0084] S6. If the solution path is successful, then convert the solution result from the Frenet coordinate system back to the Cartesian coordinate system. The schematic diagram of the final solution result is shown in the appendix Figure 3 , the area enclosed by the green dashed line is the vehicle's drivable area; the blue dashed line represents the operation reference line issued by the task module; the yellow dashed box represents the vehicle outline, that is, the vehicle envelope area formed by the planned path in space; the red dot represents the center of the vehicle's rear axle.

[0085] In some embodiments, in S2, when applying the iterative LQR algorithm of multi-objective optimization, the following steps are carried out:

[0086] Generate an initial solution, denoted as P k (x k , y k , θ k , κ k , dκ k ) ∈ Γ, (k = 0, 1,..., n - 1), (k = 0, 1,..., n - 1), where the path points (x k , y k , θ k , κ k , dκ k ) respectively represent the x coordinate, y coordinate, heading angle, curvature, and curvature change rate of the discrete path points of the initial solution in the Cartesian coordinate system, and the reverse distance is d back ≥0.

[0087] The initial solution can be constructed from three segments:

[0088] Reverse segment (from the planning start point to the shifting point): Use a straight line segment, and extend backward from the planning start point by a length of d back of the line segment as the initial solution of the reverse path;

[0089] Transition section (shift point to reference line splicing point): Starting from the end point of the reverse section and ending at a suitable point on the reference line, connect them with a smooth curve (such as a Bezier curve) as the first initial solution of the forward path;

[0090] Target section (reference line splicing point to target point): Starting from the end point of the transition section, extend along the reference line until the end point of the reference line, which is the target point, as the second initial solution of the forward path;

[0091] When d back = 0, the vehicle does not reverse, and the vehicle only has a forward movement. The corresponding initial solution only consists of the transition section and the target section.

[0092] In S4, the state transition equation is discretized and linearized as:

[0093] Δ k+1 = AΔ k + Bu k

[0094] where u k = dκ k , and A and B are the state matrix and input matrix after linearization, respectively, that is:

[0095]

[0096] Finally, send the path information that satisfies the kinematic constraints and collision constraints to the speed planning module, and generate a trajectory for the control execution module.

[0097] The present invention also provides a low-latency path planning device for an autonomous driving vehicle, including:

[0098] An information acquisition module, which is used to obtain input information; according to the map positioning module and vehicle chassis information, obtain the current vehicle pose (x0, y0, θ0), that is, the starting point of the planning, where x0, y0, and θ0 respectively represent the x coordinate, y coordinate, and heading angle of the vehicle at the current position; the reference line information released by the vehicle task module is the rough path information for the vehicle to execute tasks, which guides the direction of the vehicle's forward movement but does not satisfy the kinematic and collision constraints. According to the vehicle vision, radar and other environmental perception modules, obtain the environmental fusion information, and based on the reference line, obtain the vehicle's drivable channel space. The drivable channel space describes the upper and lower bounds of the space that can be reached along the reference line direction. See the attached Figure 2 , the area enclosed by the green dashed line is the vehicle's drivable area; the yellow dashed box represents the vehicle's outer contour line; the red dot represents the center of the vehicle's rear axle; the blue dashed line represents the operation reference line released by the task module; the yellow solid line represents the initial solution composed of multiple segments.

[0099] An initial solution generation module, which is used to generate an initial solution for the reverse distance d back starting from 0 and gradually increasing, and calculating the driving path for this section of distance using an iterative LQR algorithm with multi-objective optimization;

[0100] A cost function setting module, which is used to set a multi-objective cost function:

[0101]

[0102] where w fl , w fθ respectively represent the terminal lateral deviation constraint weight and the terminal heading deviation constraint weight, which are the constraint capabilities for the pose of the terminal target point. w r , w κ , w dκ respectively represent the constraint weight for the path deviating from the reference line, the path curvature constraint weight, and the path curvature change rate constraint weight, which are the constraint capabilities for fitting the reference line;

[0103] Ψ(κ k , k max ), Ψ(l k , l up ), Ψ(l k , -l low ) respectively represent the curvature boundary constraint, the collision upper bound constraint, and the collision lower bound constraint, which perform hard constraints on collisions and kinematic boundaries in the form of obstacle functions;

[0104] κ max represents the maximum curvature of the vehicle's travel, that is, the reciprocal of the vehicle's minimum turning radius;

[0105] l k,up represents the upper bound lateral coordinate of the collision constraint at the k-th point, and l k,low represents the lower bound lateral coordinate of the collision constraint at the k-th point;

[0106] A Frenet coordinate system conversion module, which is used to convert the initial solution to the Frenet coordinate system; with the reference line as the coordinate axis of the Frenet coordinate system, the initial solution is converted from Cartesian coordinates to Frenet coordinates, denoted as P k (l k , θ k , κ k , dκ k , s k ) ∈ Π, (k = 0, 1,..., N - 1), where k = 0, 1,..., N - 1 indicates that the initial solution consists of N discrete points, and the k-th point (l k , θ k , κ k , dκ k , s krespectively represent the lateral coordinate, heading angle, curvature, curvature change rate, and longitudinal coordinate in the Frenet coordinate system;

[0107] The state transition equation in the Frenet coordinate is designed as:

[0108]

[0109] where the state vector is the input vector is δs is the change in the longitudinal coordinate between two adjacent points, determined by the initial solution; therefore, the actual input vector is only dκ k ; k f,k is the curvature corresponding to the projection point of the k-th point on the reference line;

[0110] The iterative optimization solution module is used for iterative optimization and solution. The iterative LQR solution is divided into iterative backward propagation and forward propagation; backward propagation means solving the optimal control rate sequence starting from the end of the solution; forward propagation means applying the optimal control rate sequence from the initial state to solve the state sequence, and iterating repeatedly until the convergence threshold is reached;

[0111] The coordinate system rotation module is used to rotate the solution result back from the Frenet coordinate system to the Cartesian coordinate system when the solution path is successful. The schematic diagram of the final solution result is shown in the appendix Figure 3 , the area enclosed by the green dashed line is the vehicle's drivable area; the blue dashed line represents the operation reference line issued by the task module; the yellow dashed box represents the vehicle's outer contour line, that is, the vehicle envelope area formed by the planned path in space; the red dot represents the center of the vehicle's rear axle.

[0112] When generating the initial solution using the iterative LQR algorithm with multi-objective optimization, the initial solution generation module is denoted as P k (x k , y k , θ k , κ k , dκ k ) ∈ Γ, (k = 0, 1,..., n - 1), (k = 0, 1,..., n - 1), where the path points (x k , y k , θ k , κ k , dκ k ) respectively represent the x coordinate, y coordinate, heading angle, curvature, and curvature change rate of the discrete path points of the initial solution in the Cartesian coordinate system, and the reverse distance is d back ≥0.

[0113] The initial solution is constructed from three segments:

[0114] Reverse section (from the planned starting point to the shifting point): A straight line segment is adopted, extending backward from the planned starting point by a length of d back as the initial solution of the reverse path;

[0115] Transition section (from the shifting point to the reference line splicing point): Starting from the end point of the reverse section and ending at a suitable point on the reference line, it is connected by a smooth curve (such as a Bezier curve) as the first initial solution of the forward path;

[0116] Target section (from the reference line splicing point to the target point): Starting from the end point of the transition section and extending along the reference line to the end point of the reference line, which is the target point, as the second initial solution of the forward path;

[0117] When d back = 0, the vehicle does not reverse, and the vehicle only has a forward movement. The corresponding initial solution only consists of the transition section and the target section at both ends.

[0118] In some embodiments, when the Frenet coordinate system conversion module processes coordinate conversion, the state transition equation is discretely linearized as:

[0119] Δ k+1 = AΔ k + Bu k

[0120] where u k = dκ k , and A and B are the state matrix and input matrix after linearization, respectively, that is:

[0121]

[0122] In the present invention, replacing the vehicle kinematic model can be used as an alternative solution of the present invention. The vehicle kinematic model refers to models in multiple coordinate systems, including but not limited to the Cartesian coordinate system, the Frenet coordinate system, etc.

[0123] A computer-readable storage medium of the present invention includes a computer program, and when the computer program is executed by one or more processors, the low-latency path planning method for an autonomous vehicle described in any one of the above is implemented.

[0124] The present invention has the following advantages: Based on the Frenet coordinate system, the state transition equation is simplified, and only one optimization variable is retained, greatly improving the calculation efficiency of the LQR algorithm, and the performance requirements can also be met on a low-computing-power platform.

[0125] The dual actions of reversing and moving forward are integrated into one optimization problem, unifying the problem-solving space. It also performs excellently in narrow space scenarios.

[0126] The path optimization method by traversing the reverse distance reduces the optimization dimension from three - dimensional to one - dimensional, simplifies the design difficulty, and at the same time ensures the controllability and interpretability of the planned path.

[0127] The above - mentioned is only the preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention, according to the technical solution and inventive concept of the present invention, making equivalent replacements or changes should be covered within the protection scope of the present invention.

Claims

1. A low-latency path planning method for autonomous vehicles, characterized in that, It includes the following steps: S1. First, obtain the input information; obtain the current vehicle pose (x0, y0, θ0), which is the starting point of the planning, where x0, y0, and θ0 respectively represent the x coordinate, y coordinate, and heading angle of the vehicle at the current position; S2. Secondly, generate an initial solution for the reverse distance d back starting from 0 and gradually increasing, and calculate the driving path for this section of distance using the iterative LQR algorithm with multi-objective optimization; S3. Design the cost function, and the multi-objective cost function is designed as: where, w fl , w fθ respectively represent the lateral deviation constraint weight of the terminal and the heading deviation constraint weight of the terminal, which are the constraint capabilities for the pose of the terminal target point. w r , w κ , W dκ respectively represent the constraint weight of the path deviating from the reference line, the path curvature constraint weight, and the path curvature change rate constraint weight, which are the constraint capabilities for fitting the reference line; Ψ(k k , k max ), Ψ(l k , l up ), Ψ(l k , l low ) respectively represent the curvature boundary constraint, the collision upper bound constraint and the collision lower bound constraint, and perform hard constraints on the collision and kinematic boundaries in the form of obstacle functions; κ max represents the maximum curvature of the vehicle's travel, that is, the reciprocal of the minimum turning radius of the vehicle; l k，up represents the upper bound transverse coordinate of the collision constraint at the k-th point, l k，low represents the lower bound transverse coordinate of the collision constraint at the k-th point; S4. The initial solution is transformed into the Frenet coordinate system. Using the reference line as the coordinate axis of the Frenet coordinate system, the initial solution is transformed from Cartesian coordinates to Frenet coordinates, denoted as P k (l k , θ k , k k , dk k , s k ) ∈ Π, (k = 0, 1,..., N - 1), where k = 0, 1,..., N - 1 indicates that the initial solution consists of N discrete points, and the k-th point (l k , θ k , κ k , dκ k , s k ) represents the lateral coordinate, heading angle, curvature, curvature change rate, and longitudinal coordinate in the Frenet coordinate system, respectively; The state transition equation in the Frenet coordinate system is designed as: Among them, the state vector is The input vector is δs is the longitudinal coordinate change amount between two adjacent points, which is determined by the initial solution; therefore, the actual input vector is only dk k ; k f，k is the curvature corresponding to the projection point of the k-th point on the reference line; S5. Iteratively optimize and solve. The iterative LQR solution is divided into iterative backward propagation and forward propagation; backward propagation means solving the optimal control rate sequence starting from the end of the solution; forward propagation means applying the optimal control rate sequence from the initial state to solve the state sequence, and iterating repeatedly until the convergence threshold is reached; S6. If the solution path is successful, convert the solution result from the Frenet coordinate system back to the Cartesian coordinate system.

2. The low-latency path planning method for an autonomous vehicle according to claim 1, wherein In S2, when applying the iterative LQR algorithm for multi-objective optimization, follow the following steps: Generate the initial solution, denoted as P k (x k , y k , θ k , k k , dk k ) ∈ Γ, (k = 0, 1,..., n - 1), (k = 0, 1,..., n - 1), where the path points (x k , y k , θ k , k k , dk k ) represent the x - coordinate, y - coordinate, heading angle, curvature, and curvature change rate of the discrete path points of the initial solution in the Cartesian coordinate system respectively, and the reverse distance is d back ≥0.

3. The low-latency path planning method for an autonomous vehicle according to claim 2, wherein The initial solution is constructed by three segments: Reversing section: From the planned starting point to the shifting point, a straight line section is adopted, and a line segment with a length of d extending backward from the planned starting point is used as the initial solution of the reversing path; back ​ Transition segment: from the gear shift point to the reference line splicing point, starting from the end point of the reverse segment and ending at a suitable point on the reference line, and connecting with a smooth curve (such as a bezier curve) as the first initial solution of the forward path; Target segment: from the reference line splicing point to the target point, starting from the end point of the transition segment and extending along the reference line until the end point of the reference line, which is the target point, as the second initial solution of the forward path; When d back = 0, the host vehicle does not reverse and only has a forward movement. Accordingly, the initial solution only consists of two ends, namely the transition section and the target end.

4. The low-latency path planning method for an autonomous vehicle according to any one of claims 1-3, characterized in that, In S4, the state transition equation is discretized and linearized as: Δ k+1 = AΔ k + Bu k where u k = dk k , A and B are the linearized state matrix and input matrix respectively, that is:

5. The low-latency path planning method for an autonomous vehicle according to any one of claims 1-3, characterized in that, Finally, send the path information that satisfies the kinematic constraints and collision constraints to the speed planning module, and generate a trajectory for the control execution module.

6. An apparatus for low-latency path planning of an autonomous vehicle, characterized in that, It includes: An information acquisition module for obtaining input information; Obtain the current vehicle pose (x0, y0, θ0), which is the starting point of the planning, where x0, y0, and θ0 respectively represent the x coordinate, y coordinate, and heading angle of the vehicle at the current position; An initial solution generation module for generating an initial solution for the reverse distance d back starting from 0 and gradually increasing, and calculating the driving path for this section of the distance using the iterative LQR algorithm with multi-objective optimization; A cost function setting module for setting the multi-objective cost function: Among them, w fl , w fθ respectively represent the constraint weight of the terminal lateral deviation and the constraint weight of the terminal heading deviation, which are the constraint capabilities for the pose of the terminal target point. w r , w κ , w dκ respectively represent the constraint weight of the path deviation from the reference line, the constraint weight of the path curvature, and the constraint weight of the path curvature change rate, which are the constraint capabilities for conforming to the reference line; Ψ(κ k , k max ), Ψ(l k , l up ), Ψ(l k , -l low ) represent the curvature boundary constraint, the collision upper bound constraint, and the collision lower bound constraint respectively, and perform hard constraints on the collision and kinematic boundaries in the form of obstacle functions; κ max represents the maximum curvature of the vehicle's travel, that is, the reciprocal of the minimum turning radius of the vehicle; l k，up represents the upper bound transverse coordinate of the collision constraint at the k-th point, l k,low represents the lower bound transverse coordinate of the collision constraint at the k-th point; The Frenet coordinate system conversion module is used to convert the initial solution to the Frenet coordinate system; taking the reference line as the coordinate axis of the Frenet coordinate system, the initial solution is converted from Cartesian coordinates to Frenet coordinates, denoted as P k (l k , θ k , κ k , dκ k , s k ) ∈ Π, (k =, 1,..., N - 1), where k = 0, 1,..., N - 1 indicates that the initial solution consists of N discrete points, and the k-th point (l k , θ k , κ k , dκ k , s k ) represents the lateral coordinate, heading angle, curvature, curvature change rate, and longitudinal coordinate in the Frenet coordinate system respectively; The state transition equation in the Frenet coordinate system is designed as: Among them, the state vector is The input vector is δs is the longitudinal coordinate change amount between adjacent two points, which is determined by the initial solution; therefore, the actual input vector is only dk k ; k f，k is the curvature corresponding to the projection point of the k-th point on the reference line; An iterative optimization and solution module for iteratively optimizing and solving. The iterative LQR solution is divided into iterative backward propagation and forward propagation; backward propagation means solving the optimal control rate sequence starting from the end of the solution; forward propagation means applying the optimal control rate sequence from the initial state to solve the state sequence, and iterating repeatedly until the convergence threshold is reached; A coordinate system rotation module for converting the solution result from the Frenet coordinate system back to the Cartesian coordinate system when the solution path is successful.

7. The low-latency path planning device for an autonomous vehicle according to claim 6, wherein Initial solution generation module. When generating the initial solution using the iterative LQR algorithm for multi-objective optimization, it is denoted as P k (x k , y k , θ k , k k , dk k ) ∈ Γ, (k = 0, 1,..., n - 1, (k = 0, 1,..., n - 1), where the path points (x k , y k , θ k , k k , dk k ) represent the x-coordinate, y-coordinate, heading angle, curvature, and curvature change rate of the initial solution discrete path points in the Cartesian coordinate system respectively, and the reverse distance is d back ≥0.

8. The low-latency path planning device for an autonomous vehicle according to claim 7, wherein The initial solution is constructed by three segments: Reverse section (from the planned starting point to the shift point): A straight section is adopted, and a line segment with a length of d extending backward from the planned starting point is used as the initial solution of the reverse path; back ​ Transition segment (from the gear shift point to the reference line splicing point): starting from the end point of the reverse segment and ending at a suitable point on the reference line, and connecting with a smooth curve (such as a bezier curve) as the first initial solution of the forward path; Target segment (from the reference line splicing point to the target point): starting from the end point of the transition segment and extending along the reference line until the end point of the reference line, which is the target point, as the second initial solution of the forward path; When d back = 0, the host vehicle does not reverse and only moves forward. Accordingly, the initial solution only consists of two ends, namely the transition section and the target end.

9. The method for low-latency path planning of an autonomous vehicle according to claim 8, wherein When the Frenet coordinate system conversion module processes the coordinate conversion, the state transition equation is discretized and linearized as: Δ k+1 = AΔ k + Bu k where u k = dk k , A and B are the linearized state matrix and input matrix respectively, that is:

10. A computer-readable storage medium comprising a computer program, characterized in that, When the computer program is executed by one or more processors, it implements the low-latency path planning method for an autonomous vehicle according to any one of claims 1-5.

Images