A trajectory planning method for structured narrow roads in a port

Abstract

The application discloses a track planning method for a structured narrow road in a port. For the structured narrow road in the port, firstly, a narrow environment is modeled, a map guide line is used as an initial solution of track planning, and an optimization method based on B-spline is used to construct a track planning problem, and finally, a safe and smooth optimal path can be solved.

Description

Technical Field

[0001] This invention belongs to the technical field, and specifically relates to a trajectory planning method for narrow, structured roads in ports. Background Technology

[0002] With the rapid development of automotive technology, more and more car companies and technology innovation companies are no longer satisfied with the research and development of traditional automotive chassis, seats, powertrains, and suspensions, and are instead investing in the research and development of intelligent driving systems. Ports, with their regular road structures and large transport volumes, represent a prime application scenario for autonomous driving. These scenarios often include right-angle bends and narrow work lanes, posing significant challenges to autonomous driving planning technology.

[0003] In scenarios with irregular road boundaries and severely limited free space, generating a trackable and smooth driving trajectory is a challenge for autonomous driving trajectory planning.

[0004] The information disclosed in this background section is intended only to enhance the understanding of the overall background of the invention and should not be construed as an admission or in any way implying that the information constitutes prior art known to those skilled in the art. Summary of the Invention

[0005] The purpose of this invention is to address the limited research on path planning technology for autonomous driving in narrow road conditions. This patent provides a method for planning a route through narrow roads based on surrounding road boundary information and obstacle information. This method can be applied to the development of intelligent vehicle path planning and reduce the need for manual intervention in narrow and extreme road conditions.

[0006] To achieve the above objectives, this invention provides a trajectory planning method for narrow, structured roads in ports, comprising the following steps:

[0007] Step 1: Narrow passage environment modeling. A segmented boundary fitting method based on structured roads is used, which preserves road characteristics with fewer parameters and improves computational efficiency. Each boundary segment is represented by two parameters, and n consecutive boundaries can be represented by n+1 parameters.

[0008] Step 2: Calculation and processing of boundary conditions. Match the current road segment ID, correspond the endpoint parameters of the road segment with that ID, and connect the fault boundary with a straight line to ensure the continuity of the boundary.

[0009] Step 3: Initial B-spline construction. During initialization, the constraint boundary is expanded outward. Each iteration of optimization recalculates the boundary using the previous optimization result until the boundary shrinks to the initial value or the optimization fails. When the optimization fails after n iterations, the limitations of the current boundary can be explicitly expressed, providing a basis for boundary adjustment.

[0010] Step 4, Constraint Setting: The constraints for solving this problem are the boundary constraints from Step 3, the vehicle's curvature constraints, and the start and end point attitude constraints.

[0011] Step 5, Cost Function Design: The cost function includes the zero, first, and second order reciprocals of the B-spline and the distance to the obstacle.

[0012] Step 6: Optimize the problem solution.

[0013] Based on the analysis of the background technology, this application focuses on the structured narrow roads in ports. First, the narrow environment is modeled, and the map guidance line is used as the initial solution for trajectory planning. Then, an optimization method based on B-splines is used to construct the trajectory planning problem, and finally, a safe and smooth optimal path can be solved.

[0014] Preferably, in the above technical solution, since port roads are mostly block-structured roads:

[0015] 1: The boundary can be represented by multiple straight lines, and each segment only requires the coordinates of the first and last endpoints.

[0016] 2: When querying boundaries, you only need to match the current road segment ID and the endpoint parameters of that road segment to quickly calculate the left and right boundaries.

[0017] 3: Even if there is a break at the current road section boundary, it can be connected with a straight line to ensure the continuity of the boundary.

[0018] Preferably, in the above technical solution, step 1, calculation The projection of each line segment onto the B-spline yields the projection range of each line segment along the length S of the B-spline. And store it on the line segment;

[0019] Step 2, based on the S value of the current trajectory point, take... A straight line segment located within a certain range before and after S;

[0020] Step 3: Set the rear axle center of the vehicle to obtain the vehicle body profile at the trajectory point, and calculate the minimum distance between the vehicle body profile and the straight line segment in 6.2, which serves as the hard constraint for the left and right boundaries of the trajectory point. ; Traverse each trajectory point to obtain the hard constraints of the left and right boundaries of all trajectory points to be optimized. ;

[0021] Step 4: Set the start and end point restrictions;

[0022] starting point ;

[0023] end ;

[0024] Step 5, Cost Function, consider the first derivative of the B-spline. Second derivative Third derivative Distance to obstacles, curvature constraints, and corresponding weighting coefficients. ; ;

[0025] Step 6: Substitute the constraints and cost function into the solver to obtain the optimized B-spline function. By sampling at equal intervals on the new B-spline function, a series of pose points are obtained. , This is the final optimized solution result.

[0026] Preferably, the dynamic weight adjustment method included in the above technical solution can dynamically adjust the cost function according to the size of the free space, so as to find feasible solutions to the greatest extent possible and approach the target point at the fastest speed.

[0027] Preferably, in the above technical solution, the estimated curvature is ensured to be continuous while obtaining the optimal solution.

[0028] Compared with the prior art, the present invention has the following beneficial effects: the B-spline-based optimization method constructs the trajectory planning problem and can ultimately solve for a safe and smooth optimal path. Attached Figure Description

[0029] Figure 1 This is a schematic diagram of boundary processing;

[0030] Figure 2 A schematic diagram for boundary optimization;

[0031] Figure 3 This is a schematic diagram of the optimized boundary. Detailed Implementation

[0032] The specific embodiments of the present invention will be described in detail below, but it should be understood that the scope of protection of the present invention is not limited to the specific embodiments.

[0033] Unless otherwise expressly stated, throughout the specification and claims, the term "comprising" or its variations such as "including" or "comprises" shall be understood to include the stated elements or components without excluding other elements or other components.

[0034] 1. Boundary handling:

[0035] Purple represents unprocessed road boundary information, which is represented by the straight line ax+by+c=0.

[0036] Each boundary segment can be represented by two parameters, and n consecutive boundaries can be represented by n+1 parameters. As shown in the figure, there are abrupt boundary changes in the original boundary. This abrupt change is smoothed with a straight line to avoid the distortion of the planned trajectory caused by hard boundary changes on the left and right sides during optimization.

[0037] During the initial planning phase, the road boundary is expanded outwards as shown in the figure:

[0038] Based on the original guide lines of the high-precision map, left and right boundary constraints can be set. Push the left and right boundary constraints outward by 1m. In each iteration of optimization, based on the result of the previous iteration, shrink the left and right boundaries inward by 0.1m.

[0039] 2. Optimization process:

[0040] 2.1 Calculation The projection of each line segment onto the B-spline yields the projection range of each line segment along the length S of the B-spline. And store it on the line segment;

[0041] 2.2 Based on the S value of the current trajectory point, take... A straight line segment located within a certain range before and after S;

[0042] 2.3 Place the rear axle center at each trajectory point to obtain the vehicle body profile at the trajectory point, and calculate the minimum distance between the vehicle body profile and the straight line segment in 2.2, which serves as the hard constraint for the left and right boundaries of that trajectory point. ;

[0043] Iterate through each trajectory point to obtain the hard constraints of the left and right boundaries of all trajectory points to be optimized. ;

[0044] 2.4 Set restrictions for the start and end points.

[0045] starting point ;

[0046] end ;

[0047] Cost function, considering the first derivative of B-splines Second derivative Third derivative Distance from the obstacle Distance to obstacles, corresponding weight coefficient .

[0048] ,

[0049] Substituting the constraints and cost function into the solver yields the optimized B-spline function. By sampling at equal intervals on the new B-spline function, a series of pose points are obtained. , This is the final optimized solution result.

[0050] The foregoing description of specific exemplary embodiments of the invention is for illustrative and explanatory purposes. These descriptions are not intended to limit the invention to the precise forms disclosed, and it will be apparent that many changes and variations can be made in accordance with the foregoing teachings. The exemplary embodiments were chosen and described in order to explain the specific principles of the invention and its practical application, thereby enabling those skilled in the art to implement and utilize various different exemplary embodiments of the invention, as well as various different choices and variations. The scope of the invention is intended to be defined by the claims and their equivalents.

Claims

1. A trajectory planning method for narrow, structured roads in ports, characterized in that, The method includes the following steps: Step 1: Narrow passage environment modeling. The segmented boundary fitting method of structured roads is adopted. Each boundary segment is represented by two parameters, and n consecutive boundaries can be represented by n+1 parameters. Step 2: Calculation and processing of boundary conditions. Match the current road segment ID with the endpoint parameters corresponding to the current road segment ID, and connect the fault boundary with a straight line to ensure the continuity of the boundary. Step 3: Initial B-spline construction. During initialization, the constraint boundary is expanded outward. In each optimization iteration, the boundary is recalculated using the previous optimization result until the boundary shrinks to the initial value or the optimization fails. When the optimization fails after n iterations, the limitations of the current boundary can be explicitly expressed, providing a basis for boundary adjustment. Step 4, Constraint Setting: The constraints to be solved are the boundary constraints from Step 3, the vehicle's curvature constraints, and the start and end point attitude constraints. Step 5, Cost function design, the cost function includes the zero, first and second order reciprocals of the B-spline and the distance to the obstacle; Step 6, optimize the problem solution; Steps 3-6, more specifically: Calculation The projection of each line segment onto the B-spline yields the projection range of each line segment along the length S of the B-spline. And store it on the line segment; Based on the S value of the current trajectory point, take A straight line segment located within a certain range before and after S; By placing the rear axle center at each trajectory point, the vehicle body profile at that trajectory point is obtained. The minimum distance between the vehicle body profile and the straight line segment is calculated and used as the hard constraint for the left and right boundaries of that trajectory point. ; Traverse each trajectory point to obtain the hard constraints of the left and right boundaries of all trajectory points to be optimized. ; Set restrictions for the start and end points; Start point and ; Cost function, considering the first derivative of B-splines Second derivative Third derivative Distance to obstacles, curvature constraints, and corresponding weighting coefficients. ; ; Substituting the constraints and cost function into the solver yields the optimized B-spline function. By sampling at equal intervals on the new B-spline function, a series of pose points are obtained. , This is the final optimized solution result.

2. The trajectory planning method for structured narrow roads in ports according to claim 1, characterized in that: In step 1, the port roads are set as block-structured roads: Step 1.1: Represent the boundary as multiple straight lines, each segment only requires the coordinates of its first and last endpoints; Step 1.2: When querying the boundary, you only need to match the current road segment ID and the endpoint parameters corresponding to the current road segment ID to quickly calculate the left and right boundaries; Step 1.3: Even if there is a break at the current road segment boundary, it can be connected with a straight line to ensure the continuity of the boundary.

3. The trajectory planning method for structured narrow roads in ports according to claim 2, characterized in that: The included dynamic weight adjustment method can dynamically adjust the cost function according to the size of the free space, so as to find feasible solutions to the greatest extent possible and approach the target point as quickly as possible.

4. The trajectory planning method for structured narrow roads in ports according to claim 2, characterized in that: While finding the optimal solution, ensure that the estimated curvature is continuous.

Images