A path planning method, device and domain controller

Abstract

This invention discloses a path planning method, apparatus, and domain controller. The method includes: constructing a Frenet coordinate system based on the lane centerline and obtaining vehicle characteristics; obtaining the current pose information of the vehicle's rear axle center and front center in a geodetic coordinate system; mapping road boundary information, obstacle information, the current pose information of the rear axle center, and the current pose information of the front center to the Frenet coordinate system to generate a road environment model; creating vehicle kinematic constraints, rear axle center collision constraints, front center collision constraints, and front sway speed constraints based on the road environment model and vehicle characteristics, and generating a cost function; optimizing and solving the cost function based on the vehicle kinematic constraints, rear axle center collision constraints, front center collision constraints, and front sway speed constraints to obtain the planned path for the vehicle's rear axle center. The technical solution provided by this invention reduces the sway amplitude of the vehicle's front end during obstacle avoidance when the vehicle is traveling on the planned path.

Description

Technical Field

[0001] This invention relates to the field of path planning, and more specifically to a path planning method, apparatus, and domain controller. Background Technology

[0002] In autonomous driving systems, trajectory planning often employs a path-velocity decoupled planning strategy. Path planning combines the vehicle's own information with the surrounding environment to provide a spatially continuous local driving path, which serves as the spatial basis for a spatiotemporally continuous global trajectory. Currently, the segmented acceleration algorithm is one of the mainstream path planning algorithms and is widely used. This algorithm, first proposed by the Baidu Apollo open-source project, treats the vehicle as a point mass and transforms the surrounding obstacles and road boundaries into the Frenet coordinate system to construct an optimal control problem, which is then numerically solved to obtain the path. However, this approach has a problem: treating the vehicle as a point mass (usually the rear axle center) ignores the vehicle's rigidity. For long vehicles, the driving path planned based on the rear axle center during actual turns can easily lead to interference between the vehicle's front end and obstacles.

[0003] To address the aforementioned issues, the book *Theory and Practice of Autonomous Driving Decision-Making and Planning Technology*, edited by Li Bai, Zhang Youmin, et al., proposes a multi-disc equivalent replacement method for vehicle rigid body geometry. For example... Figure 1 As shown, the main steps are: S1. Transform the road scene into a tunnel; S2. Cover the vehicle body with an infinite number of disks, the centers of which densely cover the longitudinal axis of the vehicle body, and their diameters are all the width of the vehicle body, to obtain the rigid body representation of the entire vehicle body; S3. Construct collision avoidance constraints based on the geometric relationship of the vehicle body rigid body; S4. Discretize the optimal control problem by adding a piecewise acceleration algorithm to the collision avoidance constraints; S5. Solve the optimal control problem.

[0004] This technical solution considers the rigid body characteristics of the vehicle rather than treating it as a point mass. It uses multiple disks to equivalently represent the vehicle model, constructs collision avoidance constraints based on this equivalent model, and incorporates an optimal control problem to ensure the planned path avoids interference with obstacles. While this solution guarantees that the vehicle does not interfere with obstacles, in practical applications, it has been found that the planned path causes excessive swaying of the car's front end during obstacle avoidance, resulting in poor ride comfort for the user. Summary of the Invention

[0005] In view of this, embodiments of the present invention provide a path planning method, apparatus and domain controller, thereby reducing the sway amplitude of the vehicle's front end during obstacle avoidance when the vehicle is traveling on the planned path.

[0006] According to a first aspect, embodiments of the present invention provide a path planning method, the method comprising: constructing a Frenet coordinate system based on a lane centerline and obtaining vehicle characteristics; obtaining the current pose information of the rear axle center of the vehicle in a geodetic coordinate system, and calculating the current pose information of the front center of the vehicle corresponding to the current pose information of the rear axle center based on the vehicle characteristics; mapping road boundary information, obstacle information, the current pose information of the rear axle center, and the current pose information of the front center to the Frenet coordinate system to generate a road environment model; creating vehicle kinematic constraints, rear axle center collision constraints, front center collision constraints, and front sway speed constraints based on the road environment model and the vehicle characteristics; generating a cost function based on the road environment model; and optimizing and solving the cost function based on the vehicle kinematic constraints, rear axle center collision constraints, front center collision constraints, and front sway speed constraints to obtain a planned path for the rear axle center of the vehicle.

[0007] Optionally, creating vehicle kinematic constraints based on the road environment model and the vehicle characteristics includes: discretizing the road environment model into multiple discrete segments, with the endpoints of each segment being discrete points; creating traditional kinematic constraints for a piecewise acceleration algorithm based on the discretized road environment model; determining the transformation relationship between the rear axle center lateral offset parameter and the front center lateral offset parameter in Frenet coordinates using the vehicle characteristics; determining optimization variables for the starting discrete point based on the current pose information of the rear axle center and the current pose information of the front center mapped to the Frenet coordinate system; pre-setting optimization variables for the ending discrete point; and using the transformation relationship, the traditional kinematic constraints, the optimization variables for the starting discrete point, and the optimization variables for the ending discrete point as the vehicle kinematic constraints.

[0008] Optionally, creating a rear axle center collision constraint based on the road environment model and the vehicle characteristics includes: discretizing the mapping results formed by the road boundary information and obstacle information mapped to the Frenet coordinate system based on the discretized road environment model, determining the road upper boundary function and road lower boundary function that the vehicle can pass through; subtracting the vehicle width from the range between the road upper boundary function and the road lower boundary function, and using the remaining range as the rear axle center collision constraint.

[0009] Optionally, creating the front center collision constraint based on the road environment model and the vehicle characteristics includes: deriving the discrete position of the current front center using the vehicle characteristics and the discrete position of the current rear axle center; calculating the upper and lower boundary results of the road upper boundary function and the road lower boundary function within a target range, where the target range is the length range from the discrete position of the current rear axle center to the discrete position of the current front center; extracting the upper and lower boundary values ​​of the front center representing the narrowest boundary from the upper and lower boundary results; subtracting the vehicle width from the range between the extracted upper and lower boundary values ​​of the front center, and using the remaining range as the front center collision constraint for the discrete position of the current front center.

[0010] Optionally, creating the vehicle front swing speed constraint includes: presetting a derivative range for the derivative of the lateral offset parameter of the vehicle front center, and using the preset derivative range as the vehicle front swing speed constraint.

[0011] Optionally, generating the cost function based on the road environment model includes: generating an upper boundary function and a lower boundary function for the vehicle front center using the upper and lower boundary values ​​of the vehicle front center calculated at each discrete point; determining the vehicle front driving reference line at the vehicle front center using the upper and lower boundary functions; and generating an improved cost function based on the difference between the lateral offset parameter of the vehicle front center in Frenet coordinates and the vehicle front driving reference line.

[0012] Optionally, the method further includes: converting the planned path of the rear axle center of the vehicle to a geodetic coordinate system and performing collision detection on the converted path; if the detection result indicates that no collision has occurred on the converted path, then sending the converted path to the vehicle's control processing unit.

[0013] According to a second aspect, embodiments of the present invention provide a path planning device, the device comprising: an initialization module, configured to construct a Frenet coordinate system based on a lane centerline and acquire vehicle characteristics; a vehicle information acquisition module, configured to acquire the current pose information of the rear axle center of the vehicle in a geodetic coordinate system and calculate the current pose information of the front center corresponding to the current pose information of the rear axle center based on the vehicle characteristics; a road environment simulation module, configured to map road boundary information, obstacle information, the current pose information of the rear axle center and the current pose information of the front center to the Frenet coordinate system to generate a road environment model; a constraint generation module, configured to create vehicle kinematic constraints, rear axle center collision constraints, front center collision constraints and front sway speed constraints based on the road environment model and the vehicle characteristics; a cost function generation module, configured to generate a cost function based on the road environment model; and an optimization calculation module, configured to optimize and solve the cost function based on the vehicle kinematic constraints, rear axle center collision constraints, front center collision constraints and front sway speed constraints to obtain the planned path of the rear axle center of the vehicle.

[0014] According to a third aspect, embodiments of the present invention provide a domain controller, including: a perception processing unit, a decision processing unit, a control processing unit, and a communication unit, wherein the perception processing unit, the decision processing unit, the control processing unit, and the communication unit are communicatively connected to each other, the decision processing unit stores computer instructions, and the decision processing unit executes the computer instructions to perform the method described in the first aspect, or any optional embodiment of the first aspect.

[0015] According to a fourth aspect, embodiments of the present invention provide a computer-readable storage medium storing computer instructions for causing the computer to perform the method described in the first aspect, or any alternative embodiment of the first aspect.

[0016] The technical solution provided in this application has the following advantages:

[0017] The technical solution provided in this application first constructs a Frenet coordinate system based on the lane centerline and obtains vehicle characteristics, including geometric parameters such as vehicle length, width, and wheelbase. Then, it obtains the current pose information of the vehicle's rear axle center in the geodetic coordinate system and calculates the corresponding current pose information of the vehicle's front center based on the vehicle characteristics. Road boundary information, obstacle information, the current pose information of the rear axle center, and the current pose information of the front center are mapped into the Frenet coordinate system to generate a road environment model that the vehicle needs to pass through. Then, based on the road environment model and vehicle characteristics, vehicle kinematic constraints are created to control the continuous movement of the vehicle. Rear axle center collision constraints and front center collision constraints are created in conjunction with the vehicle characteristics to avoid collisions between the vehicle and road boundaries or obstacles at the rigid body level. Furthermore, a front sway speed constraint is created to limit the amplitude of the front sway when the vehicle is maneuvering around obstacles. Finally, a cost function for a piecewise acceleration algorithm is generated based on the road environment model. Based on the vehicle kinematic constraints, rear axle center collision constraints, front center collision constraints, and front sway speed constraints, the cost function is optimized and solved to obtain the planned path for the vehicle's rear axle center. The path generated through the above steps takes into account the rigid structure of the vehicle during its movement, thus avoiding collisions. Furthermore, it limits the amplitude of the vehicle's front-end sway based on the constraint of the front-end sway speed, preventing excessive swaying of the vehicle's front end when navigating obstacles and improving the comfort of the driver and passengers. Attached Figure Description

[0018] The features and advantages of the invention will be more clearly understood by referring to the accompanying drawings, which are schematic and should not be construed as limiting the invention in any way. In the drawings:

[0019] Figure 1 A schematic diagram illustrating the processing effect on the car body in the prior art is shown;

[0020] Figure 2 The diagram illustrates the steps of a path planning method according to one embodiment of the present invention.

[0021] Figure 3 A flowchart illustrating a path planning method according to one embodiment of the present invention is shown.

[0022] Figure 4 A schematic diagram of the mapping structure of road boundary information in one embodiment of the present invention is shown;

[0023] Figure 5 This invention illustrates a schematic diagram of the geometric relationship between the center of the front of the vehicle and the center of the rear axle in one embodiment of the invention.

[0024] Figure 6 A schematic diagram of a path planning device according to one embodiment of the present invention is shown;

[0025] Figure 7 A schematic diagram of a domain controller according to one embodiment of the present invention is shown. Detailed Implementation

[0026] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, 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 a part of the embodiments of the present invention, and not all of them. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0027] Please see Figure 2 and Figure 3 In one implementation, a path planning method specifically includes the following steps:

[0028] Step S101: Construct a Frenet coordinate system based on the lane centerline and obtain vehicle characteristics.

[0029] Step S102: Obtain the current pose information of the rear axle center of the vehicle in the geodetic coordinate system, and calculate the current pose information of the front center of the vehicle corresponding to the current pose information of the rear axle center based on the vehicle characteristics.

[0030] Step S103: Map the road boundary information, obstacle information, current pose information of the rear axle center, and current pose information of the front center into the Frenet coordinate system to generate a road environment model.

[0031] Step S104: Create vehicle kinematic constraints, rear axle center collision constraints, front center collision constraints, and front sway speed constraints based on the road environment model and vehicle characteristics.

[0032] Step S105: Generate a cost function based on the road environment model.

[0033] Step S106: Based on vehicle kinematic constraints, rear axle center collision constraints, front center collision constraints, and front swing speed constraints, optimize the cost function to obtain the planned path for the rear axle center of the vehicle.

[0034] Specifically, this embodiment of the invention proposes further improvements based on the segmented acceleration algorithm. First, a discrete set of points representing the centerline of the vehicle's lane is obtained from the map module, and a Frenet coordinate system is constructed based on the lane centerline. Then, the current heading θ of the vehicle's rear axle center is obtained from the positioning module. r_ego The geodetic coordinates (x, y) of the current position of the rear axle center. r_ego ,y r_egoThis allows us to obtain the current pose information of the rear axle center of the vehicle. We also acquire vehicle characteristics representing the rigid geometry of the vehicle (e.g., dump trucks, buses, passenger cars), and deduce the pose information of the front center of the vehicle (x-axis) based on these characteristics. f_ego ,y f_ego ,θ f_ego ,κ f_ego ), where x f_ego y f_ego θ is the geodetic coordinate of the current center of the train's front end. f_ego It is the current heading of the center of the front of the train, κ f_ego This refers to the curvature of the center of the vehicle's front. Additionally, road boundary information is acquired and mapped to the Frenet coordinate system, yielding the constant functions ruw(s) and rlw(s) for the upper and lower boundaries, which do not yet consider obstacles. These are constant functions that start from the road centerline and vary with the road centerline's extension length s. For example: [Reference] Figure 4 For a given location on the road centerline, at a distance s = 100 from the starting point, and 5 meters from both the upper and lower boundaries of the road, then ruw(100) = 5 and rlw(100) = -5. The values ​​of the upper and lower boundary functions change with s; for example, the number of lanes may change as the road extends. Additionally, obstacle information is obtained, including the obstacle's heading and its geodetic coordinates.

[0035] The aforementioned road boundary information, obstacle information, current pose information of the rear axle center, and current pose information of the front center are mapped to the Frenet coordinate system to generate a road environment model. The pose information of the vehicle's rear axle center and front center is transformed from the geodetic coordinate system to the Frenet coordinate system to determine the planning starting point (s). init ,l init ,l′ init ,l″ init ,l f_init ), that is, where the current road starts from. Where, s init This represents the distance between the current position of the vehicle's rear axle center on the road centerline and the initial Frenet coordinate position s0. init l′ represents the lateral offset of the vehicle's rear axle center relative to the road centerline. init It is the first derivative of the lateral offset, used to represent the velocity of the lateral offset, l″ init This is the second derivative of the lateral offset, used to represent the acceleration of the lateral offset. Mapping road boundary and obstacle information into the Frenet coordinate system is to determine the road width that the vehicle can pass through at each position during its movement, primarily used to create collision constraints for the vehicle's rigid body. The road conditions can be determined based on the road environment model generated by this embodiment of the invention.

[0036] Then, based on the road environment model and vehicle characteristics, kinematic constraints for controlling the continuous motion of the vehicle were created. The rear axle center and the front center of the vehicle were considered separately, and collision constraints for the rear axle center and the front center of the vehicle were created based on the rigid body of the vehicle. A separate front sway velocity constraint was also created for the front center. At the same time, the cost function of the piecewise acceleration algorithm was generated.

[0037] Finally, by utilizing vehicle kinematic constraints, rear axle center collision constraints, front center collision constraints, and front sway speed constraints, the cost function is optimized to obtain the planned path for the rear axle center. On one hand, the rigid structure of the vehicle is considered during operation, preventing collisions; on the other hand, the front sway speed constraint limits the amplitude of the front sway, preventing excessive front sway when the vehicle is maneuvering around obstacles and improving passenger comfort.

[0038] Specifically, in one embodiment, step S104, which involves creating vehicle kinematic constraints based on the road environment model and vehicle characteristics, includes the following steps:

[0039] Step 1: Discretize the road environment model into multiple discrete segments, with each segment's endpoint being a discrete point.

[0040] Specifically, the road environment model is first discretized using n uniformly distributed discrete points {s}. k The mileage interval is divided into (n-1) discrete segments Δs:

[0041]

[0042] In the formula, s max It is the total length of the road environment model, and i represents the subscript of the discrete point.

[0043] Step 2: Create traditional kinematic constraints for the piecewise acceleration algorithm based on the discretized road environment model.

[0044] Specifically, based on the discretized road environment model described above, traditional kinematic constraints for the piecewise acceleration algorithm are created, as shown in the following equation:

[0045]

[0046]

[0047]

[0048] l′ min ≤l′ i ≤l′ max

[0049] l″ min ≤l″i ≤l″ max

[0050] l″′ min ≤l″′ i ≤l″′ max

[0051] In the formula, i represents the i-th discrete point s i subscript, l i L′ represents the offset distance of the rear axle center at the i-th discrete point relative to the road centerline. i The first derivative of the rear axle center offset distance with respect to s is represented by l″. i Let l″′ represent the second derivative of the rear axle center offset distance with respect to s. i Let l′ represent the third derivative of the rear axle center offset distance with respect to s. min and l′ max These are the preset minimum and maximum ranges of the first derivative, used to limit the velocity at which the rear axle center shifts laterally, l″′ min and l″ max These are the preset minimum and maximum ranges of the second derivative, used to limit the acceleration that causes lateral displacement of the rear axle center, l″′ min and l″′ max These are the preset minimum and maximum ranges of the third derivative, used to limit the jerk caused by lateral shift of the rear axle center, in order to avoid excessive acceleration that could affect passenger comfort. The traditional kinematic constraints of the aforementioned piecewise jerk algorithm are necessary constraints for trajectory planning; this part is prior art and will not be elaborated upon in this invention.

[0052] Step 3: Determine the conversion relationship between the rear axle center lateral offset parameter and the front center lateral offset parameter in Frenet coordinates using vehicle characteristics.

[0053] Specifically, in order to limit the swaying of the vehicle's front end, this embodiment of the invention needs to consider the planned trajectory of the front end separately. This requires considering the collision constraints of the rear axle center and the front end center separately. The prerequisite is to create a Frenet coordinate transformation relationship between the rear axle center and the front end center as a kinematic constraint in order to associate the trajectories of the rear axle center and the front end center and comprehensively determine whether a collision will occur. The specific derivation process of the transformation relationship is as follows:

[0054] First, obtain vehicle characteristic information, specifically including: L w Front and rear wheelbase, L r Vehicle rear overhang distance, L f Vehicle front overhang distance, L b Vehicle width. For example... Figure 5As shown, the lateral offset l(s) of the rear axle center relative to the road centerline and the lateral offset l of the front center relative to the road centerline are... f (s) satisfies the following geometric relationship:

[0055] R·sinθ+l(s=l f (s), R=L w +L f

[0056] In the formula, θ is the angle between the vehicle's axis and the centerline of the road.

[0057] because:

[0058]

[0059] so:

[0060]

[0061] l′ f (s)=R·l″(s)+l′(s)

[0062] The conversion relationship between the rear axle center lateral offset parameter and the front center lateral offset parameter is as follows:

[0063] l f (s)=R·l′(s)+l(s)

[0064] Step 4: Determine the optimization variables for the starting discrete point based on the current pose information of the rear axle center and the current pose information of the front center mapped to the Frenet coordinate system.

[0065] Step 5: Preset the optimization variables for the endpoint discrete point.

[0066] Specifically, the next step is to construct the optimization variables for the optimal control problem based on the discretized interval, including l i ,l′ i ,l″ i ,l fi , respectively represent in The lateral displacement of the rear axle center, the first derivative of the lateral displacement of the rear axle center with respect to s, the second derivative of the lateral displacement of the rear axle center with respect to s, and the lateral displacement of the front center are the optimization variables as follows:

[0067] l i ,l′ i ,l″ i ,l fi i = 0, ..., n-1

[0068] In this embodiment of the invention, the preset endpoint position has no lateral movement or motion trend, so that when the vehicle reaches the endpoint, it can match the centerline of the road. That is, the optimization variable of the preset endpoint discrete point is l. end =0,l′ end =0,l″ end =0,l f_end =0.

[0069] The following vehicle kinematic constraints are constructed based on the optimization variables of the endpoint discrete point and the starting discrete point.

[0070]

[0071] Step Six: Finally, the transformation relationship, traditional kinematic constraints, optimization variables of the starting discrete point, and optimization variables of the ending discrete point are used as vehicle kinematic constraints.

[0072] Specifically, in this embodiment, the step of creating the rear axle center collision constraint in step S104 above specifically includes:

[0073] Step 7: Based on the discretized road environment model, the mapping results of road boundary information and obstacle information mapped to the Frenet coordinate system are discretized to determine the upper and lower boundary functions of the road that vehicles can pass through.

[0074] Step 8: Subtract the vehicle width from the range between the road boundary function and the road lower boundary function, and use the remaining range as the rear axle center collision constraint.

[0075] Specifically, the mapped obstacle information is tunneled together with the road's own boundaries rlw(s) and ruw(s) to obtain the road contour that vehicles can pass through (see reference). Figure 1 Then, the road profile is discretized to obtain the upper and lower boundary functions ub(i) of the road that the vehicle can pass through. For each discrete point, the upper and lower boundary values ​​of the road are subtracted from the vehicle's half width value to obtain the rear axle center collision constraint. As long as the lateral offset of the rear axle center in the Frenet coordinate system does not exceed the above constraint, from the perspective of the vehicle's rigid body, a collision will not occur at the rear axle position. The expression is as follows:

[0076]

[0077] In the formula, the function value of the lower boundary function lb(i) is negative, and the function value of the upper boundary function ub(i) is positive.

[0078] Specifically, in this embodiment, the step of creating the front center collision constraint in step S104 above specifically includes:

[0079] Step 9: Use the vehicle characteristics and the discrete point position of the current rear axle center to deduce the discrete point position of the current front center of the vehicle.

[0080] Step 10: Calculate the upper and lower boundary results of the road boundary function and the road lower boundary function within the target range. The target range is the length range from the discrete point where the current rear axle center is located to the discrete point where the current front center is located.

[0081] Step 11: Extract the upper boundary value of the front center and the lower boundary value of the front center, which represent the narrowest boundary, from the upper and lower boundary results.

[0082] Step 12: Subtract the vehicle width from the range between the extracted upper and lower boundary values ​​of the vehicle front center, and use the remaining range as the collision constraint for the vehicle front center at the current discrete point location.

[0083] Specifically, in order to further consider the frontal collision from the perspective of vehicle rigidity, this embodiment creates the following expression for the frontal center collision constraint:

[0084]

[0085] lb f (i)=max[lb(i),…,lb(min(i+index,n-1))]

[0086] ub f (i)=min[ub(i),…,ub(min(i+index,n-1))]

[0087] In the formula, ceil represents the rounding up algorithm, index represents the subscript increment derived using vehicle characteristics, and L... w L represents the wheelbase between the front and rear wheels. f This represents the front overhang distance of the vehicle. If the current discrete point where the rear axle center is located is the i-th discrete point, then it is deduced that the current discrete point where the front center of the vehicle is located is the i+index-th discrete point.

[0088] The above formula represents the physical meaning of calculating the upper and lower boundary values ​​from the i-th discrete point to the (i+index)-th discrete point using the road's upper and lower boundary functions, and then selecting the narrowest upper and lower boundaries. Since the lower boundary is negative relative to the road centerline, the operation to select the narrowest lower boundary is a maximum value operation (max operation). It's important to note that if the current vehicle center position calculated using the index exceeds the mileage endpoint (the (n-1)-th discrete point), then the narrowest boundary from the i-th discrete point to the (n-1)-th discrete point is selected. After the above calculations, the upper and lower boundary values ​​of the vehicle center at each discrete point are obtained, lb. f (i) and ub f (i) are the lower boundary function of the vehicle head and the upper boundary function of the vehicle head, respectively.

[0089] Next, the vehicle width is subtracted from the upper and lower boundary values ​​of the vehicle front center at each discrete point, and the subtracted vehicle width L is used as the deducted value. b The range is used as the collision constraint for the center of the vehicle's front end, and the lateral offset parameter l of the center of the vehicle's front end is determined. f (s) Within the collision constraint at the center of the vehicle's front end, from the perspective of the vehicle's rigid body, the planned trajectory can be guaranteed so that the front end of the vehicle will not collide with the narrowest boundary, as shown in the following formula:

[0090]

[0091]

[0092] Specifically, in this embodiment, it is also necessary to create a vehicle front swing speed constraint to limit the swing amplitude of the vehicle front. The specific steps are as follows:

[0093] Step 13: Preset the derivative range of the lateral offset parameter of the front center of the vehicle, and use the preset derivative range as the constraint on the front swing speed.

[0094] Specifically, the constraint on the sway speed of the vehicle front is as follows:

[0095] l′ f-min ≤l′ fi ≤l′ f-max

[0096] Based on the transformation relationship derived in step three, we obtain

[0097] l′ f-min ≤(L w + f )·l″ i +l′ i ≤l′ f-max

[0098] In the formula, l′ f-minl′ is the minimum value of the derivative of the lateral offset of the vehicle's front end, which is the preset value. f-max The maximum value of the derivative of the lateral offset of the vehicle's front end is preset. Thus, the swaying speed of the vehicle's front end can be constrained by the above constraints, so that the swaying of the vehicle's front end is reduced when the vehicle travels along the planned path and avoids obstacles.

[0099] Specifically, in one embodiment, step S105 above includes the following steps:

[0100] Step Fourteen: Generate the upper boundary function and lower boundary function of the vehicle front center using the upper and lower boundary values ​​of the vehicle front center calculated at each discrete point.

[0101] Step 15: Determine the reference line for the vehicle's front end by using the upper boundary function and the lower boundary function of the vehicle's front end.

[0102] Step 16: Based on the difference between the lateral offset parameter of the vehicle's front center and the vehicle's driving reference line in the Frenet coordinates, generate an improved cost function.

[0103] Specifically, in this embodiment, to further reduce the sway amplitude of the vehicle's front end during driving, an improved piecewise acceleration algorithm cost function is proposed, with an additional front end optimization term added. Due to the presence of obstacles in the road, the vehicle cannot travel entirely along the road center, therefore, it first calculates the cost function ub based on the upper boundary function of the front end center. f (i) and the lower boundary function lb of the front center f (i) Determine the vehicle front driving reference line at the center of the vehicle front, that is, the optimal driving route of the vehicle front based on the upper and lower boundaries of the vehicle front center when the vehicle is driving. In this embodiment, let the vehicle front driving reference line be... This is just one example, and not a limitation. The reference line for the vehicle's front can also be drawn at the center of the vehicle's front using the boundary function ub. f (i) and the lower boundary function lb of the front center f (i) is user-defined. Then, based on the lateral offset parameter l of the vehicle's front center in Frenet coordinates. fi Reference line for vehicle front driving The difference between them, add a front-end optimization item. In the original cost function, the vehicle's front end is made to stay as close to the reference line as possible during travel, thus improving the cost function. In subsequent steps, the cost function provided in this embodiment is optimized based on constraints, which further reduces the vehicle's front end sway when maneuvering around obstacles. Specifically, the improved cost function is as follows:

[0104]

[0105] In the formula, w represents the preset weight of each term in the cost function, which is adjusted according to the user's actual needs; This represents the difference between the lateral offset of the rear axle center and the rear axle center reference line. A smaller value indicates that the rear axle center is closer to the rear axle center reference line when the vehicle is moving. In this embodiment, the rear axle center reference line can be set as follows: That is, the median of the upper and lower boundary functions of the road, which can also be customized according to user needs. This is just an example and is not limited to this. This indicates the speed at which the rear axle center shifts laterally; This represents the acceleration indicating the lateral offset of the rear axle center. This represents the acceleration due to the lateral offset of the rear axle center; the smaller the velocity, acceleration, and jerk of the lateral offset, the more comfortable the ride for the passengers; the last item... This refers to the difference between the lateral offset of the vehicle's front center and the reference line of the vehicle's front center, which is used to optimize the swing amplitude of the vehicle's front.

[0106] Specifically, in one embodiment, after the improved cost function and constraints are constructed, the cost function and constraints are converted into a QP problem form that can be computed by a computer. The path curve sequence can be obtained by solving the problem using a numerical solver, such as OSQP.

[0107] Specifically, in one embodiment, after the planned path of the vehicle's rear axle center is calculated, the embodiment of the present invention also converts the planned path of the vehicle's rear axle center to the geodetic coordinate system and performs collision detection on the converted path; if the detection result indicates that no collision has occurred on the converted path, the converted path is sent to the vehicle's control processing unit so that the vehicle responds to the planned path to control the vehicle's speed and steering; otherwise, the converted path is not sent to the vehicle's control processing unit to further ensure the safety of the vehicle's autonomous driving.

[0108] Through the above steps, the technical solution provided in this application first constructs a Frenet coordinate system based on the lane centerline and obtains vehicle characteristics, including geometric parameters such as vehicle length, vehicle width, and front and rear wheelbase; then, it obtains the current pose information of the rear axle center of the vehicle in the geodetic coordinate system and calculates the current pose information of the front center corresponding to the current pose information of the rear axle center based on the vehicle characteristics; it maps the road boundary information, obstacle information, the current pose information of the rear axle center, and the current pose information of the front center to the Frenet coordinate system to generate a road environment model that the vehicle needs to pass through; then, it creates vehicle kinematic constraints based on the road environment model and vehicle characteristics to control the continuous movement of the vehicle, and creates rear axle center collision constraints and front center collision constraints in combination with vehicle characteristics, thereby avoiding collisions between the vehicle and road boundaries or obstacles at the rigid body level of the vehicle, and creating front sway speed constraints to limit the front sway amplitude when the vehicle is bypassing obstacles. Subsequently, a cost function for the piecewise acceleration algorithm is generated based on the road environment model. This cost function is then optimized and solved based on vehicle kinematic constraints, rear axle center collision constraints, front axle center collision constraints, and front axle sway speed constraints to obtain the planned path for the vehicle's rear axle center. The path generated through these steps considers the vehicle's rigid structure during travel, preventing collisions. Furthermore, the front axle sway speed constraint limits the amplitude of the sway, preventing excessive swaying when the vehicle is maneuvering around obstacles and improving passenger comfort.

[0109] like Figure 6 As shown, this embodiment also provides a path planning device, which includes:

[0110] Initialization module 101 is used to construct a Frenet coordinate system based on the lane centerline and obtain vehicle characteristics. For details, please refer to the relevant description of step S101 in the above method embodiment, which will not be repeated here.

[0111] The vehicle information acquisition module 102 is used to acquire the current pose information of the rear axle center of the vehicle in the geodetic coordinate system, and calculate the current pose information of the front center of the vehicle corresponding to the current pose information of the rear axle center based on the vehicle characteristics. For details, please refer to the relevant description of step S102 in the above method embodiment, which will not be repeated here.

[0112] The road environment simulation module 103 is used to map road boundary information, obstacle information, current pose information of the rear axle center, and current pose information of the vehicle front center into the Frenet coordinate system to generate a road environment model. For details, please refer to the relevant description of step S103 in the above method embodiment, which will not be repeated here.

[0113] The constraint generation module 104 is used to create vehicle kinematic constraints, rear axle center collision constraints, front center collision constraints, and front sway speed constraints based on the road environment model and vehicle characteristics. For details, please refer to the relevant description of step S104 in the above method embodiment, which will not be repeated here.

[0114] The cost function generation module 105 is used to generate a cost function based on the road environment model. For details, please refer to the relevant description of step S105 in the above method embodiment, which will not be repeated here.

[0115] The optimization calculation module 106 is used to optimize and solve the cost function based on vehicle kinematic constraints, rear axle center collision constraints, front center collision constraints, and front sway speed constraints to obtain the planned path for the rear axle center of the vehicle. For details, please refer to the relevant description of step S106 in the above method embodiment, which will not be repeated here.

[0116] The present invention provides a path planning device for executing a path planning method provided in the above embodiments. Its implementation and principle are the same. For details, please refer to the relevant descriptions of the above method embodiments, which will not be repeated here.

[0117] Through the collaborative efforts of the aforementioned components, the technical solution provided in this application first constructs a Frenet coordinate system based on the lane centerline and obtains vehicle characteristics, including geometric parameters such as vehicle length, width, and wheelbase. Then, it obtains the current pose information of the vehicle's rear axle center in the geodetic coordinate system and calculates the corresponding current pose information of the vehicle's front center based on the vehicle characteristics. Road boundary information, obstacle information, the current pose information of the rear axle center, and the current pose information of the vehicle's front center are mapped into the Frenet coordinate system to generate a road environment model that the vehicle needs to traverse. Then, based on the road environment model and vehicle characteristics, vehicle kinematic constraints are created to control the continuous movement of the vehicle. Rear axle center collision constraints and front center collision constraints are created in conjunction with the vehicle characteristics, thereby preventing collisions between the vehicle and road boundaries or obstacles at the rigid body level. Furthermore, a front sway speed constraint is created to limit the amplitude of the front sway when the vehicle navigates around obstacles. Subsequently, a cost function for the piecewise acceleration algorithm is generated based on the road environment model. This cost function is then optimized and solved based on vehicle kinematic constraints, rear axle center collision constraints, front axle center collision constraints, and front axle sway speed constraints to obtain the planned path for the vehicle's rear axle center. The path generated through these steps considers the vehicle's rigid structure during travel, preventing collisions. Furthermore, the front axle sway speed constraint limits the amplitude of the sway, preventing excessive swaying when the vehicle is maneuvering around obstacles and improving passenger comfort.

[0118] Figure 7This invention illustrates a domain controller according to an embodiment of the present invention. The domain controller includes at least a perception processing unit 901, a decision processing unit 902, a control processing unit 903, and a communication unit 904. The perception processing unit 901, the decision processing unit 902, the control processing unit 903, and the communication unit 904 can communicate with each other via a bus or other means. Figure 7 Taking the bus method as an example.

[0119] In this embodiment, the perception processing unit 901 and the decision processing unit 902 each include an independent processor. The perception processing unit 901 and the decision processing unit 902 may each include an independent memory, or they may use a shared memory.

[0120] In this embodiment of the invention, the perception processing unit 901 is mainly applied to engineering machinery scenarios. Its main function is to perform perception fusion processing on sensor data to obtain environmental information of the current environment of the engineering machinery, and then send the environmental information to the control processing unit 903 or the decision processing unit 902 according to the data type of the environmental information signal.

[0121] Memory, as a non-transitory computer-readable storage medium, can be used to store non-transitory software programs, non-transitory computer-executable programs, and modules, such as the program instructions / modules corresponding to the methods in the above method embodiments. The perception processing unit 901 executes various functional applications and data processing of the processor by running the non-transitory software programs, instructions, and modules stored in the memory, thereby implementing the methods in the above method embodiments.

[0122] The function of the decision processing unit 902 is to formulate driving or operation strategies by combining information such as the surrounding environment, operation scenario, and vehicle status, and finally issue control commands.

[0123] The main function of the control processing unit 903 is to convert between different types of signals, such as communication protocol conversion (CAN, Ethernet, LIN, etc.), AD conversion (sensor input), and DA conversion (control drive). For example, to convert the signal scanned by the LiDAR into point cloud data, the control processing unit 903 can be an MCU with chips such as the Texas Instruments (TI) TDA4VM, Mobileye's EyeQ series, Renesas' R-CAR H3, or Horizon Robotics' Journey series.

[0124] The main function of the communication unit 904 is to conduct wireless communication, including but not limited to 5G / 4G network communication, Wi-Fi communication, and satellite communication, and to communicate with the cloud server. Its main functions include uploading device-related status and information to the cloud service, requesting the cloud server to assist in calculation and processing, downloading data from the cloud server, and performing OTA software upgrades on the controller. It also communicates with nearby devices, receiving status information from other devices and collaboratively completing tasks. The communication unit 110 of the control module can be a 5G module, Wi-Fi module, Bluetooth module, etc.

[0125] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The implemented program can be stored in a computer-readable storage medium. When the program is executed, it can include the processes of the embodiments of the above methods. The storage medium can be a magnetic disk, optical disk, read-only memory (ROM), random access memory (RAM), flash memory, hard disk drive (HDD), or solid-state drive (SSD), etc.; the storage medium can also include combinations of the above types of memory.

[0126] Although embodiments of the invention have been described in conjunction with the accompanying drawings, those skilled in the art can make various modifications and variations without departing from the spirit and scope of the invention, and such modifications and variations all fall within the scope defined by the appended claims.

Claims

1. A path planning method characterized by, The method comprises: constructing a Frenet coordinate system based on a lane center line, and obtaining vehicle characteristics; obtaining current pose information of a rear axle center of the vehicle in a geodetic coordinate system, and calculating current pose information of a vehicle head center corresponding to the current pose information of the rear axle center based on the vehicle characteristics; mapping road boundary information, obstacle information, the current pose information of the rear axle center and the current pose information of the vehicle head center into the Frenet coordinate system to generate a road environment model; creating vehicle kinematic constraints, rear axle center collision constraints, vehicle head center collision constraints and vehicle head swing speed constraints based on the road environment model and the vehicle characteristics; the creation of the vehicle kinematic constraints based on the road environment model comprises: discretizing the road environment model into a plurality of discrete segments, and the end points of each segment are discrete points; creating traditional kinematic constraints of a piecewise jerk algorithm based on the discretized road environment model; determining a conversion relationship between a rear axle center lateral offset parameter and a vehicle head center lateral offset parameter in the Frenet coordinate system by using the vehicle characteristics; determining optimization variables of a starting discrete point based on the current pose information of the rear axle center and the current pose information of the vehicle head center mapped into the Frenet coordinate system; presetting optimization variables of a terminal discrete point; and taking the conversion relationship, the traditional kinematic constraints, the optimization variables of the starting discrete point and the optimization variables of the terminal discrete point as the vehicle kinematic constraints; generating a cost function based on the road environment model; optimizing and solving the cost function based on the vehicle kinematic constraints, the rear axle center collision constraints, the vehicle head center collision constraints and the vehicle head swing speed constraints to obtain a planning path of the rear axle center of the vehicle.

2. The method of claim 1, wherein, The creation of the rear axle center collision constraints based on the road environment model and the vehicle characteristics comprises: discretizing the mapping results of the road boundary information and the obstacle information mapped into the Frenet coordinate system based on the discretized road environment model to determine an upper boundary function and a lower boundary function of a road through which the vehicle can pass; deducting the width of the vehicle body from the range between the upper boundary function and the lower boundary function, and taking the remaining range as the rear axle center collision constraint.

3. The method of claim 1, wherein, The creation of the vehicle head center collision constraints based on the road environment model and the vehicle characteristics comprises: deriving the discrete point position of the current vehicle head center by using the vehicle characteristics and the discrete point position of the current rear axle center; calculating upper and lower boundary results of the upper boundary function and the lower boundary function in a target range, the target range being a length range from the discrete point position of the current rear axle center to the discrete point position of the current vehicle head center; extracting a vehicle head center upper boundary value and a vehicle head center lower boundary value representing the narrowest boundary from the upper and lower boundary results; deducting the width of the vehicle body from the range between the extracted vehicle head center upper boundary value and the vehicle head center lower boundary value, and taking the remaining range as the vehicle head center collision constraint of the discrete point position of the current vehicle head center.

4. The method of claim 1, wherein, The creation of the vehicle head swing speed constraint comprises: A derivative range of the derivative of the lateral offset parameter of the vehicle head center is preset, and the preset derivative range is taken as the vehicle head swing speed constraint.

5. The method of claim 3, wherein, The cost function is generated based on the road environment model, including: An upper boundary function of the vehicle head center and a lower boundary function of the vehicle head center are generated by using the upper boundary value and the lower boundary value of the vehicle head center calculated by each discrete point; A vehicle head driving reference line of the vehicle head center is determined by the upper boundary function of the vehicle head center and the lower boundary function of the vehicle head center; An improved cost function is generated based on the difference relationship between the lateral offset parameter of the vehicle head center in the Frenet coordinate and the vehicle head driving reference line.

6. The method of claim 1, wherein, The method further includes: The planned path of the vehicle rear axle center is converted to the geodetic coordinate system, and collision detection is performed on the converted path; If the detection result indicates that the converted path does not collide, the converted path is sent to the control processing unit of the vehicle.

7. A route planning apparatus characterized by comprising: The device includes: An initialization module is configured to construct a Frenet coordinate system based on a lane center line and obtain vehicle characteristics; A vehicle information acquisition module is configured to obtain current pose information of a rear axle center of a vehicle in a geodetic coordinate system, and calculate current pose information of a vehicle head center corresponding to the current pose information of the rear axle center based on the vehicle characteristics; A road environment simulation module is configured to map road boundary information, obstacle information, the current pose information of the rear axle center and the current pose information of the vehicle head center into the Frenet coordinate system to generate a road environment model; A constraint generation module is configured to create vehicle kinematics constraints, rear axle center collision constraints, vehicle head center collision constraints and vehicle head swing speed constraints based on the road environment model and the vehicle characteristics; the creation of the vehicle kinematics constraints based on the road environment model and the vehicle characteristics includes: discretizing the road environment model into a plurality of discrete segments, with the endpoints of each segment being discrete points; creating traditional kinematics constraints of a piecewise jerk algorithm based on the discretized road environment model; determining a conversion relationship between a lateral offset parameter of a rear axle center and a lateral offset parameter of a vehicle head center in the Frenet coordinate by using the vehicle characteristics; determining optimization variables of a starting discrete point based on the current pose information of the rear axle center and the current pose information of the vehicle head center mapped into the Frenet coordinate system; presetting optimization variables of an ending discrete point; and taking the conversion relationship, the traditional kinematics constraints, the optimization variables of the starting discrete point and the optimization variables of the ending discrete point as the vehicle kinematics constraints; A cost function generation module is configured to generate a cost function based on the road environment model; An optimization calculation module is configured to optimize and solve the cost function based on the vehicle kinematics constraints, the rear axle center collision constraints, the vehicle head center collision constraints and the vehicle head swing speed constraints to obtain a planned path of a vehicle rear axle center.

8. A domain controller, characterized by It includes: a perception processing unit, a decision processing unit, a control processing unit and a communication unit, which are communicatively connected to each other, the decision processing unit stores computer instructions, and the decision processing unit executes the computer instructions to perform the method according to any one of claims 1-6.

9. A computer-readable storage medium, characterized in that, The computer readable storage medium stores computer instructions for causing the computer to perform the method according to any one of claims 1-6.

Images