Local path planning method for pure electric automatic driving commercial vehicle in park logistics scene

Abstract

A local path planning method for a pure electric automatic driving commercial vehicle in a park logistics scene is characterized by comprising the following steps: step 1, constructing a load-centroid migration model of the commercial vehicle; 2, calculating the transverse and longitudinal acceleration and turning radius limit of the commercial vehicle under the current load according to the vehicle kinematic model; 3, converting the Cartesian coordinate system of the vehicle and the obstacle on the global reference path into a Frenet coordinate system; 4, performing transverse and longitudinal dynamic sampling on the global reference path; 5, according to the sampling state, L (s) and s (t) planning functions are generated, and then a local path trajectory is generated; 6, performing feasibility evaluation on the local path trajectory, and taking the local path trajectory meeting the vehicle dynamics requirement as a feasible trajectory; 7, performing cost evaluation on the feasible trajectory by using a multi-target cost function to obtain an optimal trajectory; 8, the optimal track is issued to a control module to control the commercial vehicle to run. The method has the effect that the optimal local path planning is realized.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and particularly to a local path planning method for a pure electric autonomous commercial vehicle in a park logistics scenario. Background Art

[0002] With the rapid development of autonomous driving technology, the application of autonomous driving in commercial vehicles in closed scenarios is becoming more and more extensive. Among them, the closed park logistics scenario provides good conditions for the use of autonomous commercial logistics vehicles, and can carry out transportation all day long, improving work efficiency, eliminating traffic safety caused by fatigue driving, and having economic value. Aiming at the difficulty of path planning for autonomous logistics vehicles at each cargo delivery or receiving point in the park logistics scenario, such as the dynamic changes of workers and cargo stacking, unstructured T-shaped narrow warehouse entrances, unstructured herringbone slope entrances, structured L-shaped road steep slope entrances and other scenarios, as Figure 2 shown. Existing local path planning methods include artificial potential field method, rapid random number, Lattice and other methods.

[0003] Disadvantages of the prior art: Traditional local path planning algorithms cannot cope with the influence of vehicle parameters such as large variation range of the total mass of commercial vehicles, large vehicle size, and large turning radius in practical applications, and the warehousing scenarios in closed parks are complex, there are problems of local path planning under the constraints of road parameters such as slope, road width variation and curvature, and actual application scenarios such as the parking position of the target point. Summary of the Invention

[0004] A local path planning method for a pure electric autonomous commercial vehicle in a park logistics scenario provided by the present invention realizes optimal local path planning, and meets the restrictions on the vehicle in the local path planning in the park, especially in each parking point scenario.

[0005] To achieve the above object, a key point of a local path planning method for a pure electric autonomous commercial vehicle in a park logistics scenario provided by the present invention is to include the following steps:

[0006] Step 1: Construct a load-centroid offset model of the commercial vehicle;

[0007] Step 2: According to the vehicle kinematic model, calculate the lateral acceleration limit, longitudinal acceleration limit, braking distance and turning radius limit of the commercial vehicle under the current load;

[0008] Step 3: Perform global path planning according to the starting point coordinates and ending point coordinates of the commercial vehicle to obtain a global reference path, and convert the Cartesian coordinates of the vehicle and obstacles on the global reference path into Frenet coordinates to reduce the planning complexity;

[0009] Step 4: Horizontally and longitudinally dynamically sample the global reference path in the Frenet coordinate system;

[0010] Step 5: Generate the L(s) planning function and the s(t) planning function according to the sampling state, perform trajectory fitting through the L(s) planning function and the s(t) planning function, and generate at least two local path trajectories;

[0011] Step 6: Conduct a feasibility assessment on each of the local path trajectories, determine whether the trajectory meets the vehicle dynamics requirements, perform trajectory feasibility screening, and use the local path trajectories that meet the vehicle dynamics requirements as feasible trajectories;

[0012] Step 7: Construct a multi-objective cost function, use the multi-objective cost function to evaluate the cost of the feasible trajectories, dynamically allocate the weights of the cost function, and then convert the optimal trajectory obtained from the evaluation from the Frenet coordinate system to the Cartesian coordinate system;

[0013] Step 8: Send the optimal trajectory in the Cartesian coordinate system to the control module, and the control module controls the commercial vehicle to drive according to the optimal trajectory.

[0014] Through the above design, for the characteristics of pure electric autonomous commercial vehicles and the road characteristics such as road gradients and curvatures in the logistics park scenario, an improved Lattice algorithm considering scenario road gradients, widths, lengths, curvatures and parking space constraints, as well as vehicle dimensions, turning radii, and the influence of the change in the total vehicle mass on the center of gravity, which affects the steering and braking performance of commercial vehicles in multiple dimensions, is adopted to achieve optimal local path planning, meeting the requirements for local path planning of pure electric commercial vehicles in the park, especially the restrictions on vehicles in each parking point scenario (gradients, curves, narrow roads).

[0015] As an optimization: In the above Step 1, when the commercial vehicle is unloaded, the position of the center of mass is determined by the vehicle's own structure and obtained through design parameters or actual measurements:

[0016] r empty =(x empty ,y empty ,z empty )

[0017] where (x empty ,y empty ,z empty ) represents the coordinates of the center of mass of the unloaded vehicle, and r empty represents the vector of the center of mass of the unloaded vehicle;

[0018] Obtain the mass m of each area of the commercial vehicle through the pressure sensor array at the bottom of the cargo box of the commercial vehicle i(t), a pressure sensor collects the mass of one area, and a real-time calculation vehicle centroid estimation model is established using the pressure sensor data:

[0019]

[0020] r total (r) = (x total , y tota l, z total )

[0021] Among them, m i (t) represents the mass of the i-th area at time t, m0 represents the vehicle's unloaded mass, r i (t) represents the centroid vector of the i-th area block, r total (r) is the centroid vector of the total vehicle mass after loading, (x total , y total , z total ) are the centroid coordinates of the vehicle after loading, and n represents the total number of area blocks.

[0022] As an optimization: in the step 2, regarding the influence of the vehicle centroid offset caused by the vehicle's load change on vehicle dynamics, where the centroid height z total affects the roll threshold, and the lateral acceleration limit is:

[0023] According to the dynamic constraints, the longitudinal and lateral maximum accelerations under various vehicle total masses are calculated, and the expression is:

[0024]

[0025] Among them, a long,max represents the longitudinal maximum acceleration, a lat,max represents the lateral maximum acceleration, T max represents the maximum motor torque, v represents the vehicle speed, m represents the vehicle total mass, r represents the tire rolling radius, L t represents the track width, z total represents the distance of the vehicle centroid from the ground Z-axis under the total mass, and g represents the acceleration due to gravity;

[0026] F resistance represents the total resistance, including air resistance, rolling resistance, and gradient resistance, and the expression is:

[0027]

[0028] Among them, ρ represents the air density, C d represents the drag coefficient, A represents the frontal projected area of the vehicle head, θ represents the gradient, f r represents the rolling resistance;

[0029] Centroid lateral offset ytotal Cause changes in the inherent steering characteristics of the vehicle and changes in the turning radius, with the expression as follows:

[0030]

[0031] where k max represents the maximum turning radius, L represents the wheelbase, C αf represents the front wheel slip ratio, C αr represents the rear wheel slip ratio, L r represents the distance from the center of mass to the rear axle, L f represents the distance from the center of mass to the front axle.

[0032] The offset of the center of mass changes L r and L f , thus affecting k max .

[0033] As a preference: In the said step 4, in order to solve the influence of the change in the total mass of the commercial vehicle on the steering, braking, and acceleration performance of the vehicle, as well as the path safety and feasibility of the local path planning under the changes in the curvature, gradient, and width of the road, perform longitudinal and lateral dynamic sampling on the global reference path in the Frenet coordinate system, specifically as follows:

[0034] (1) Sampling in the longitudinal s direction:

[0035] The initial sampling interval Δs0 is based on the current speed v and the time step T, with the expression as:

[0036] Δs0 = vT

[0037] The dynamic adjustment formula, comprehensively adjusting the sampling interval Δs by the load mass influence factor α(m), the road curvature influence factor β(c), and the road gradient influence factor γ(θ), with the expression as follows:

[0038]

[0039] Ensure that Δs is within the reasonable range [Δs min , Δs max ;

[0040] Generate a sequence of sampling points, generate longitudinal sampling points s0, s1, s2... by incrementing Δs, and generate candidate local path trajectories in combination with the lateral offset;

[0041] (2) Sampling in the lateral d direction

[0042] Lane constraint: The lateral offset is restricted within the lane d ∈ [d left , d right ;

[0043] Curvature constraint: Lateral acceleration limit to avoid sideslip:

[0044]

[0045] where w vehicle is the vehicle width, μ is the road adhesion coefficient, k road is the road curvature, d max is the maximum lateral displacement, d left represents the distance from the lane center to the left lane line, d right represents the distance from the lane center to the right lane line.

[0046] Preferably, the calculation expression of the load mass influence factor α(m) is as follows:

[0047]

[0048] where m0 is the unloaded mass of the vehicle; an increase in the load mass m will extend the braking distance. When m > m0, the load mass influence factor α(m) increases, and the sampling interval needs to be reduced;

[0049] The calculation expression of the road curvature influence factor β(c) is as follows:

[0050] β(c) = 1 + K c |C|

[0051] where K c is the curvature sensitivity coefficient, which controls the adjustment intensity of the curvature on the interval; C represents the curvature, with the unit of rad / m. The larger the curvature C, the denser the sampling points;

[0052] The calculation expression of the road slope influence factor γ(θ) is as follows:

[0053] γ(θ) = 1 + K θ |sinθ|

[0054] where K θ is the slope sensitivity coefficient, θ is the slope angle, and the slope θ affects the braking performance. The braking distance increases when going downhill.

[0055] Preferably, in the step 5, according to the sampling state, the L(s) planning function and the s(t) planning function are generated as follows:

[0056] In the Lattice algorithm, a trajectory is generated based on the Frenet coordinate system, and the vehicle state is described by longitudinal (s) and lateral (d) components:

[0057] Longitudinal state:

[0058] where s t represents the position, represents the speed, represents acceleration;

[0059] Lateral state:

[0060] where d s represents the lateral offset, represents the lateral offset velocity, represents the lateral offset acceleration;

[0061] (1) Lateral trajectory generation

[0062] The relationship between the lateral displacement L(s) and the longitudinal displacement s, that is, the expression of the L(s) planning function is:

[0063] L(s) = a0 + a1s + a2s 2 + a3s 3 + a4s 4 + a5s 5

[0064] where a0, a1, a2, a3, a4, a5 are lateral displacement coefficients, and a0 to a5 are solved by a linear equation;

[0065] Boundary conditions:

[0066] Starting point constraint, s = 0:

[0067] L(0) = d current

[0068]

[0069] where d current represents the lateral offset under the starting point, represents the lateral offset velocity under the starting point, represents the lateral offset acceleration under the starting point;

[0070] End point constraint, s = s T :

[0071] L(s T ) = d target

[0072] L′(s T ) = 0

[0073] L″(s T ) = 0

[0074] where d target represents the lateral offset distance under the end state;

[0075] (2) Longitudinal velocity planning

[0076] The longitudinal displacement s(t) uses a quartic polynomial to satisfy the continuity of velocity and acceleration. The expression of the s(t) planning function is as follows:

[0077] s(t) = b0 + b1t + b2t 2 + b3t 3 + b4t 4

[0078] where b0, b1, b2, b3, and b4 are longitudinal displacement coefficients;

[0079] The planning start point is t = 0, and the end point is t = T:

[0080] Initial state:

[0081] s(0) = 0

[0082] v(0) = v current

[0083] a(0) = a current

[0084] End state:

[0085]

[0086] where v current represents the velocity in the initial state, a current represents the acceleration in the initial state, and v target represents the velocity in the end state.

[0087] In the Lattice algorithm, the generation of the lateral displacement function L(s) and the longitudinal displacement function s(t) is achieved through polynomial parameterization. The generation of these two functions needs to satisfy the dynamic constraints and boundary conditions of the vehicle, such as the position, velocity, and acceleration at the start and end points.

[0088] Preferably: In step 6, the feasibility of each local path trajectory is evaluated to determine whether the trajectory meets the vehicle dynamics requirements, and trajectory feasibility screening is performed as follows:

[0089] Longitudinal acceleration limit:

[0090] |a long | ≤ a long,max

[0091] Lateral acceleration limit:

[0092] |a lat | ≤ a lat,max

[0093] Steering curvature constraint:

[0094] κ(s) ≤ κ max

[0095] Obstacle collision detection:

[0096] The expression of the safety distance model (which expands as the vehicle's load mass increases) is as follows:

[0097] r safe = r base + fΔm

[0098] where r safe represents the safety distance, r base represents the basic safety distance when the vehicle is unloaded, f represents the adjustment coefficient, Δm represents the vehicle's load mass, and Δm = m - m0;

[0099] The collision detection formula is as follows:

[0100]

[0101] ‖P ego (t) - P obs (t)‖2 > r safe

[0102] where a long represents the longitudinal acceleration, a long,max represents the maximum longitudinal acceleration, a lat represents the lateral acceleration, a lat,max represents the maximum lateral acceleration, κ(s) represents the turning radius at time s, κ max represents the maximum steering curvature, P ego (t) represents the position coordinates of the vehicle at time t, P obs (t) represents the position coordinates of the obstacle at time t, ‖‖2 represents the L2 norm, and [0, t horizon represents the detection time range.

[0103] Preferably: in the said step 7, for the commercial vehicle scenario, a weighted multi-objective cost function is designed to calculate the cost of each feasible path, considering the smoothness, safety, and followability of the trajectory. The expression of the multi-objective cost function is as follows:

[0104] J = w1J smooth + w2J obstacle + w3J longi

[0105] where w1, w2, and w3 are weight coefficients that need to be adjusted according to the scenario; J smooth is the smoothness cost, J obstacle is the obstacle cost, and J longi is the cost of tracking the reference trajectory;

[0106] (1) The smooth cost J smooth The expression is:

[0107] J smooth = J lat_jerk + J long_jerk

[0108] Reduce rapid acceleration and sharp turning:

[0109] Lateral acceleration J lat_jerk Minimize:

[0110]

[0111] Longitudinal acceleration constraint J long_jerk (The greater the total vehicle mass, the higher the jitter cost):

[0112]

[0113] Where m0 is the unladen mass of the vehicle;

[0114] (2) The obstacle cost J obstacle The expression is:

[0115] J obstacle = J brake + J lat

[0116] Dynamic braking distance J brake Constraint:

[0117]

[0118] d obs (s i ) represents the distance d i from the trajectory point s obs to the nearest obstacle; when the total vehicle mass m increases, the braking distance required increases proportionally ;

[0119] Lateral stability J lat Constraint:

[0120]

[0121] R(s i ) is the road curvature radius at the trajectory point s i ; v(s i ) is the vehicle speed at the trajectory point s i ; μ is the road friction coefficient;

[0122] Allowable lateral acceleration threshold: The greater the total vehicle mass m, the weaker the vehicle's anti-rollover ability, and the threshold is according to Reduce;

[0123] Actual lateral acceleration: Calculated from the trajectory curvature radius R(s i ) and vehicle speed v(s i ), reflecting the centrifugal force during turning;

[0124] (3) The cost J of tracking the reference trajectory longi The expression is:

[0125] J longi = J speed + J dis

[0126]

[0127] Among them, J speed represents the speed deviation cost, and J dis represents the sum of the lateral deviation distances between the planned path and the reference path. v ref represents the vehicle reference speed, and v evaluate represents the trajectory speed planned by the vehicle. L(s i ) represents the lateral deviation between the trajectory point s i in the Frenet coordinate system and the reference trajectory.

[0128] Advantages of the present invention:

[0129] 1. To solve the influence of the change in the total mass of commercial vehicles on the steering, braking, and acceleration performance of vehicles, as well as the path safety and feasibility of local path planning under the changes in curvature, slope, and width of the road, dynamic sampling optimization is performed on the longitudinal and lateral directions in the Frenet coordinate system. ① Dynamically adjust the spacing of sampling points and the smoothness requirements of the path according to the influence of the load mass change on the steering and braking performance; ② The change in the curvature of the road will affect the steering demand and stability of the vehicle. In the Lattice algorithm, increase the density of sampling points in the section with a larger curvature to more finely control the steering and path smoothness of the vehicle.

[0130] 2. For the commercial vehicle scenario, design a weighted multi-objective cost function to calculate the cost of each alternative path, considering the smoothness, safety, and followability of the trajectory under the characteristics of the change in the total mass of commercial vehicles.

[0131] 3. The improved Lattice planner can adapt to the change state of the total mass of the vehicle and the environmental change during the actual use of commercial vehicles, improve the trajectory safety in complex scenarios such as park logistics, and can be extended to various complex scenarios such as ports and mines. Brief description of the drawings

[0132] Figure 1 It is a schematic flowchart of the present invention;

[0133] Figure 2 (a) Schematic diagram of the operation scenario of an autonomous commercial vehicle in a logistics park in the embodiment;

[0134] Figure 2 (b) Schematic diagram of non-structured herringbone slope warehousing in the embodiment;

[0135] Figure 2 (c) Schematic diagram of non-structured T-shaped narrow warehouse entrance in the embodiment;

[0136] Figure 2 (d) Schematic diagram of structured L-shaped road steep slope warehousing in the embodiment;

[0137] Figure 3 It is a schematic diagram of horizontal and vertical scatter sampling in the Frenet coordinate system in the embodiment. Detailed implementation manners

[0138] The present invention will be further described in detail below with reference to the accompanying drawings and specific examples. The following embodiments or drawings are used to illustrate the present invention, but not to limit the scope of the present invention.

[0139] As Figure 1 shown: A local path planning method for a pure electric autonomous commercial vehicle in a park logistics scenario includes the following steps:

[0140] Step 1: Construct a load-centroid offset model of the commercial vehicle;

[0141] Step 2: According to the vehicle kinematic model, calculate the lateral acceleration limit, longitudinal acceleration limit, braking distance and turning radius limit of the commercial vehicle under the current load;

[0142] Step 3: Perform global path planning based on the starting point coordinates and ending point coordinates of the commercial vehicle to obtain a global reference path, and convert the Cartesian coordinates of the vehicle and obstacles on the global reference path into the Frenet coordinate system to reduce the planning complexity;

[0143] Step 4: Perform horizontal and vertical dynamic sampling on the global reference path in the Frenet coordinate system;

[0144] Step 5: Generate an L(s) planning function and an s(t) planning function according to the sampling state, and perform trajectory fitting through the L(s) planning function and the s(t) planning function to generate at least two local path trajectories;

[0145] Step 6: Perform a feasibility evaluation on each of the local path trajectories, determine whether the trajectory meets the vehicle dynamics requirements, perform trajectory feasibility screening, and use the local path trajectories that meet the vehicle dynamics requirements as feasible trajectories;

[0146] Step 7: Construct a multi-objective cost function, use the multi-objective cost function to evaluate the cost of the feasible trajectory, dynamically allocate the weights of the cost function, and then convert the optimal trajectory obtained by the evaluation from the Frenet coordinate system to the Cartesian coordinate system;

[0147] Step 8: Send the optimal trajectory in the Cartesian coordinate system to the control module, and the control module controls the commercial vehicle to drive according to the optimal trajectory.

[0148] In Step 1, when the commercial vehicle is unloaded, the position of the center of mass is determined by the vehicle's own structure and can be obtained through design parameters or actual measurements:

[0149] r empty =(x empty ,y empty ,z empty )

[0150] where (x empty ,y empty ,z empty ) represents the coordinates of the center of mass of the unloaded vehicle, and r empty represents the center of mass vector of the unloaded vehicle;

[0151] Obtain the mass m i (t) of each area of the commercial vehicle through the pressure sensor array at the bottom of the cargo box of the commercial vehicle. One pressure sensor corresponds to collecting the mass of one area, and use the pressure sensor data to establish a real-time calculation model for estimating the center of mass of the whole vehicle:

[0152]

[0153] r total (r)=(x total ,y tota l,z total )

[0154] where m i (t) represents the mass of the i-th area at time t, m0 represents the unloaded mass of the vehicle, r i (t) represents the center of mass vector of the i-th area block, r total (r) represents the center of mass vector of the total mass of the vehicle after loading, and (x total ,y total ,z total ) represents the coordinates of the center of mass of the vehicle after loading, and n represents the total number of area blocks.

[0155] In Step 2, regarding the influence of the vehicle's center of mass offset caused by the vehicle's load change on the vehicle dynamics, where the center of mass height z total affects the roll threshold, and the lateral acceleration limit is:

[0156] According to the dynamic constraints, the longitudinal and lateral maximum accelerations under various vehicle gross masses are calculated, and the expression is as follows:

[0157]

[0158] Among them, a long,max represents the longitudinal maximum acceleration, a lat,max represents the lateral maximum acceleration, T max represents the maximum torque of the motor, v represents the vehicle speed, m represents the vehicle gross mass, r represents the tire rolling radius, L t represents the track width, z total represents the distance of the vehicle's center of mass from the ground Z-axis under the gross mass, and g represents the acceleration due to gravity;

[0159] F resistance represents the total resistance, including air resistance, rolling resistance, and gradient resistance, and the expression is as follows:

[0160]

[0161] Among them, ρ represents the air density, C d represents the drag coefficient, A represents the frontal projected area of the vehicle head, θ represents the gradient, f r represents the rolling resistance;

[0162] The lateral offset y of the center of mass total causes changes in the vehicle's inherent steering characteristics and turning radius, and the expressions are as follows:

[0163]

[0164] Among them, k max represents the maximum turning radius, L represents the wheelbase, C αf represents the front wheel slip ratio, C αr represents the rear wheel slip ratio, L r represents the distance from the center of mass to the rear axle, L f represents the distance from the center of mass to the front axle.

[0165] The offset of the center of mass changes L r and L f , thereby affecting k max .

[0166] In step 4, in order to solve the influence of the change in the commercial vehicle's gross mass on the vehicle's steering, braking, and acceleration performance, as well as the path safety and feasibility of local path planning under the changes in the curvature, gradient, and width of the road, longitudinal and lateral dynamic sampling is performed on the global reference path in the Frenet coordinate system, as Figure 3 shown, specifically as follows:

[0167] (1) Sampling in the longitudinal s direction:

[0168] The initial sampling interval Δs0 is based on the current speed v and the time step T, and the expression is:

[0169] Δs0 = vT

[0170] The dynamic adjustment formula adjusts the sampling interval Δs by comprehensively considering the load mass influence factor α(m), the road curvature influence factor β(c), and the road slope influence factor γ(θ). The expression is as follows:

[0171]

[0172] Ensure that Δs is within a reasonable range [Δs min , Δs max ;

[0173] Generate a sequence of sampling points, generate longitudinal sampling points s0, s1, s2... in increasing order of Δs, and generate candidate local path trajectories in combination with the lateral offset;

[0174] (2) Sampling in the lateral d direction

[0175] Lane constraint: The lateral offset is restricted within the lane d ∈ [d left , d right ;

[0176] Curvature constraint: Lateral acceleration limit to avoid skidding:

[0177]

[0178] Where, w vehicle is the vehicle width, μ is the road adhesion coefficient, k road is the road curvature, d max is the maximum lateral displacement, d left represents the distance from the lane center to the left lane line, d right represents the distance from the lane center to the right lane line.

[0179] The calculation expression of the load mass influence factor α(m) is as follows:

[0180]

[0181] Where, m0 is the unloaded mass of the vehicle; an increase in the load mass m will extend the braking distance. When m > m0, the load mass influence factor α(m) increases, and the sampling interval needs to be reduced;

[0182] The calculation expression of the road curvature influence factor β(c) is as follows:

[0183] β(c) = 1 + K c |C|

[0184] Among them, K c is the curvature sensitivity coefficient, which controls the adjustment intensity of the curvature on the interval; C represents the curvature, with the unit of rad / m. The larger the curvature C, the denser the sampling points;

[0185] The calculation expression of the road slope influence factor γ(θ) is as follows:

[0186] γ(θ) = 1 + K θ |sinθ|

[0187] Among them, K θ is the slope sensitivity coefficient, θ is the slope angle, and the slope θ affects the braking performance, and the braking distance increases when going downhill.

[0188] In the said step 5, according to the sampling state, the L(s) planning function and the s(t) planning function are generated, specifically as follows:

[0189] In the Lattice algorithm, a trajectory is generated based on the Frenet coordinate system, and the vehicle state is described by longitudinal (s) and lateral (d) components:

[0190] Longitudinal state:

[0191] Among them, s t represents the position, represents the speed, represents the acceleration;

[0192] Lateral state:

[0193] Among them, d s represents the lateral offset, represents the lateral offset speed, represents the lateral offset acceleration;

[0194] (1) Lateral trajectory generation

[0195] The relationship between the lateral displacement L(s) and the longitudinal displacement s, that is, the expression of the L(s) planning function is:

[0196] L(s) = a0 + a1s + a2s 2 + a3s 3 + a4s 4 + a5s 5

[0197] Among them, a0, a1, a2, a3, a4, a5 are the lateral displacement coefficients;

[0198] Boundary conditions:

[0199] Starting point constraint, s = 0:

[0200] L(0) = d current

[0201]

[0202] where d current represents the lateral offset under the starting point, represents the lateral offset speed under the starting point, represents the lateral offset acceleration under the starting point;

[0203] End constraint, s = s T :

[0204] L(s T ) = d target

[0205] L′(s T ) = 0

[0206] L″(s T ) = 0

[0207] where d target represents the lateral offset distance in the end state;

[0208] (2) Longitudinal velocity planning

[0209] The longitudinal displacement s(t) uses a quartic polynomial to satisfy the continuity of velocity and acceleration. The expression of the s(t) planning function is:

[0210] s(t) = b0 + b1t + b2t 2 + b3t 3 + b4t 4

[0211] where b0, b1, b2, b3, b4 are longitudinal displacement coefficients;

[0212] The starting point of the planning is t = 0, and the end point is t = T:

[0213] Initial state:

[0214] s(0) = 0

[0215] v(0) = v current

[0216] a(0) = a current

[0217] End state:

[0218]

[0219] where v current represents the velocity in the initial state, acurrent represents the acceleration in the initial state, v target represents the velocity in the end state.

[0220] In the Lattice algorithm, the generation of the lateral displacement function L(s) and the longitudinal displacement function s(t) is achieved through polynomial parameterization. The generation of these two functions needs to satisfy the vehicle's dynamic constraints and boundary conditions, such as the positions, velocities, and accelerations at the starting and ending points.

[0221] In step 6, the feasibility of each local path trajectory is evaluated to determine whether the trajectory meets the vehicle dynamics requirements, and trajectory feasibility screening is performed as follows:

[0222] Longitudinal acceleration limit:

[0223] |a long | ≤ a long,max

[0224] Lateral acceleration limit:

[0225] |a lat | ≤ a lat,max

[0226] Steering curvature constraint:

[0227] κ(s) ≤ κ max

[0228] Obstacle collision detection:

[0229] The expression of the safety distance model (which expands as the vehicle's load mass increases) is as follows:

[0230] r safe = r base + fΔm

[0231] where r safe represents the safety distance, r base represents the basic safety distance when the vehicle is unloaded, f represents the adjustment coefficient, Δm represents the vehicle's load mass, and Δm = m - m0;

[0232] The collision detection formula is as follows:

[0233]

[0234] ‖P ego (t) - P obs (t)‖2 > r safe

[0235] where a long represents the longitudinal acceleration, a long,max represents the maximum longitudinal acceleration, a latDenotes the lateral acceleration, \(a\). lat,max Denotes the maximum lateral acceleration, \(\kappa(s)\) denotes the turning radius at time \(s\), \(\kappa\). max Denotes the maximum steering curvature, \(P\). ego \((t)\) denotes the position coordinates of the vehicle at time \(t\), \(P\). obs \((t)\) denotes the position coordinates of the obstacle at time \(t\), \(\|\cdot\|_2\) denotes the \(L_2\) norm, \([0,t horizon denotes the detection time range.

[0236] In step 7, for the commercial vehicle scenario, a weighted multi-objective cost function is designed to calculate the cost of each feasible path, considering the smoothness, safety, and followability of the trajectory. The multi-objective cost function expression is as follows:

[0237] \(J = w_1J smooth + w_2J obstacle + w_3J longi

[0238] where \(w_1\), \(w_2\), \(w_3\) are weight coefficients that need to be adjusted according to the scenario; \(J smooth is the smoothness cost, \(J obstacle is the obstacle cost, \(J longi is the cost of tracking the reference trajectory;

[0239] (1) The smoothness cost \(J smooth The expression is:

[0240] \(J smooth = J lat_jerk + J long_jerk

[0241] Reduce sudden acceleration and sharp turning:

[0242] Lateral acceleration \(J lat_jerk Minimize:

[0243]

[0244] Longitudinal acceleration constraint \(J long_jerk (The larger the total vehicle mass, the higher the jitter cost):

[0245]

[0246] where \(m_0\) is the unladen mass of the vehicle;

[0247] (2) The obstacle cost \(J obstacle The expression is:

[0248] \(J obstacle = J brake + J lat

[0249] Dynamic braking distance J brake Constraint:

[0250]

[0251] d obs (s i ) represents the distance d from the trajectory point s i to the nearest obstacle; when the total vehicle mass m increases, the braking distance required increases proportionally obs ; grows;

[0252] Lateral stability J lat Constraint:

[0253]

[0254] R(s i ) is the road curvature radius at the trajectory point s i ; v(s i ) is the vehicle speed at the trajectory point s i ; μ is the road friction coefficient;

[0255] Allowable lateral acceleration threshold: The greater the total vehicle mass m, the weaker the vehicle's anti-rollover ability, and the threshold decreases according to ;

[0256] Actual lateral acceleration: Calculated from the trajectory curvature radius R(s i ) and vehicle speed v(s i ), reflecting the centrifugal force during turning;

[0257] (3) The cost J of tracking the reference trajectory longi The expression is:

[0258] J longi = J speed + J dis

[0259]

[0260] where J speed represents the speed deviation cost, J dis represents the sum of the lateral deviation distances between the planned path and the reference path, v ref represents the vehicle reference speed, v evaluate represents the vehicle planned trajectory speed, L(s i ) represents the lateral deviation from the reference trajectory at the trajectory point s i in the Frenet coordinate system.

[0261] The above are only the preferred embodiments of the present invention and are not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and changes. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims

1. A local path planning method for a pure electric self-driving commercial vehicle in a park logistics scenario, characterized in that: The following steps are involved: Step 1: Construct a load-center-of-mass offset model for commercial vehicles; Step 2: Calculate the lateral acceleration limit, longitudinal acceleration limit, braking distance and turning radius limit of the commercial vehicle under the current load according to the vehicle kinematic model; Step 3: Perform global path planning based on the starting point coordinates and the end point coordinates of the commercial vehicle to obtain a global reference path, and convert the Cartesian coordinate system of the vehicle and obstacles on the global reference path into a Frenet coordinate system; Step 4: Performing horizontal and vertical dynamic sampling on the global reference path in the Frenet coordinate system; Step 5: Generate L(s) planning function and s(t) planning function according to the sampling state, perform trajectory fitting through L(s) planning function and s(t) planning function, and generate at least two local path trajectories; Step 6: Perform feasibility assessment on each of the local path trajectories to determine whether the trajectory meets the vehicle dynamics requirements, perform trajectory feasibility screening, and take the local path trajectories that meet the vehicle dynamics requirements as feasible trajectories; Step 7: construct a multi-objective cost function, use the multi-objective cost function to evaluate the cost of the feasible trajectory, dynamically assign the cost function weight, and then convert the optimal trajectory obtained by the evaluation from the Frenet coordinate system to the Cartesian coordinate system; Step 8: Send the optimal trajectory in the Cartesian coordinate system to the control module, and the control module controls the commercial vehicle to travel according to the optimal trajectory.

2. The local path planning method for a pure electric self-driving commercial vehicle in a park logistics scenario according to claim 1 is characterized by: In step 1, when the commercial vehicle is unloaded, the center of mass position is determined by the vehicle structure itself and is obtained through design parameters or actual measurements: r empty =(x empty ,y empty ,z empty ) Among them, (x empty ,y empty ,z empty ) represents the coordinates of the center of mass of the unloaded vehicle, r empty represents the vehicle's unloaded center of mass vector; The mass m of each area of ​​the commercial vehicle is obtained by using a pressure sensor array at the bottom of the commercial vehicle cargo box i (t), using the pressure sensor data to establish a real-time vehicle center of mass estimation model: r total (r)=(x total ,y total ,z total ) Among them, m i (t) represents the mass of the ith region at time t, m0 represents the unloaded mass of the vehicle, r i (t) represents the centroid vector of the i-th region block, r total (r) The center of mass vector of the vehicle after loading, (x total ,y total ,z total ) is the center of mass coordinate of the vehicle after loading, and n is the total number of area blocks.

3. The local path planning method for a pure electric self-driving commercial vehicle in a park logistics scenario according to claim 1 is characterized by: In step 2, the influence of the vehicle center of mass displacement caused by the vehicle load change on the vehicle dynamics is considered, wherein the center of mass height z total The lateral acceleration limit affecting the roll threshold is: According to the dynamic constraints, the maximum lateral and longitudinal accelerations under various vehicle gross masses are calculated, and the expressions are: Among them, a long,max represents the maximum longitudinal acceleration, a lat,max represents the maximum lateral acceleration, T max represents the maximum torque of the motor, v represents the vehicle speed; m represents the total mass of the vehicle, i.e. the unloaded mass of the vehicle + the mass of the cargo; r represents the rolling radius of the tire, L t Indicates wheelbase, z total It represents the distance between the center of mass of the vehicle and the ground on the Z axis under the total mass, and g represents the acceleration due to gravity; F resistance Represents the total resistance, including air resistance, rolling resistance and slope resistance, and the expression is: Where ρ represents the air density, C d represents the drag coefficient, A represents the front projection area of ​​the vehicle, θ represents the slope, and f r Indicates rolling resistance; Lateral displacement of center of mass y total This causes the vehicle's inherent steering characteristics and turning radius to change, and the expression is as follows: Among them, k max represents the maximum turning radius, L represents the wheelbase, C represents αf represents the front wheel slip rate, C αr Represents the rear wheel slip rate, L r Indicates the distance from the center of mass to the rear axle, L f Represents the distance from the center of mass to the front axle.

4. The local path planning method for a pure electric self-driving commercial vehicle in a park logistics scenario according to claim 1 is characterized by: In step 4, the global reference path is dynamically sampled horizontally and vertically in the Frenet coordinate system, as follows: (1) Sampling in the longitudinal s direction: The initial sampling interval Δs0 is based on the current velocity v and the time step T, and is expressed as: Δs0=vT Dynamic adjustment formula, comprehensive load mass influence factor α(m), road curvature influence factor β(c) and road slope influence factor γ(θ) to adjust the sampling interval Δs, the expression is as follows: Ensure that Δs is within a reasonable range [Δs min ,Δs max ]Inside; Generate a sampling point sequence, generate longitudinal sampling points s0, s1, s2…incrementally according to Δs, and generate candidate local path trajectories in combination with lateral offsets; (2) Sampling in the lateral d direction Lane constraint: The lateral deviation is limited to within the lane d∈[d left , d right ]; Curvature Constraint: Lateral Acceleration Limit: Among them, w vehicle is the vehicle width, μ is the road adhesion coefficient, k road is the road curvature, d max is the maximum lateral displacement, d left Indicates the distance from the center of the lane to the left lane line, d right Indicates the distance from the center of the lane to the right lane line.

5. The local path planning method for a pure electric self-driving commercial vehicle in a park logistics scenario according to claim 4 is characterized by: The calculation expression of the load mass influence factor α(m) is as follows: Among them, m0 is the unloaded mass of the vehicle; The calculation expression of the road curvature influence factor β(c) is as follows: β(c)=1+K c |C| Among them, K c is the curvature sensitivity coefficient; C represents the curvature, the unit is rad / m; The calculation expression of the road slope influence factor γ(θ) is as follows: γ(θ)=1+K θ |sinθ| Among them, K θ is the slope sensitivity coefficient, and θ is the slope angle.

6. The local path planning method for a pure electric self-driving commercial vehicle in a park logistics scenario according to claim 1 is characterized by: In step 5, according to the sampling state, the L(s) planning function and the s(t) planning function are generated, as follows: In the Lattice algorithm, the trajectory is generated based on the Frenet coordinate system, and the vehicle state is described by the longitudinal (s) and lateral (d) components: Portrait state: Among them, s t Indicates location, Indicates speed, represents acceleration; Horizontal state: Among them, d s Indicates the lateral offset, represents the lateral displacement velocity, represents the lateral deviation acceleration; (1) Lateral trajectory generation The relationship between the lateral displacement L(s) and the longitudinal displacement s, that is, the expression of the L(s) planning function is: L(s)=a0+a1s+a2s 2 +a3s 3 +a4s 4 +a5s 5 Among them, a0, a1, a2, a3, a4, and a5 are lateral displacement coefficients; Boundary conditions: Starting point constraint, s=0: L(0)=d current Among them, d current Indicates the lateral offset below the starting point, represents the lateral offset velocity below the starting point, represents the lateral offset acceleration below the starting point; End point constraint, s=s T : L(s T )=d target L′(s T )=0 L″(s T )=0 Among them, d target Indicates the lateral offset distance at the end state; (2) Longitudinal speed planning The longitudinal displacement s(t) uses a quartic polynomial to satisfy the continuity of velocity and acceleration. The planning function expression of s(t) is: s(t)=b0+b1t+b2t 2 +b3t 3 +b4t 4 Among them, v0, b1, b2, b3, b4 are longitudinal displacement coefficients; The planning starting point is t=0 and the end point is t=T: Initial state: s(0)＝0 v(0)=v current a(0)=a current End state: Among them, v current represents the velocity in the initial state, a current represents the acceleration in the initial state, v target Indicates the speed at the end state.

7. The local path planning method for a pure electric self-driving commercial vehicle in a park logistics scenario according to claim 1 is characterized by: In step 6, the feasibility of each local path trajectory is evaluated to determine whether the trajectory meets the vehicle dynamics requirements, and the trajectory feasibility screening is performed. The process is as follows: Longitudinal acceleration limit: |a long |≤a long,max Lateral acceleration limit: |a lat |≤a lat,max Turning curvature constraint: κ(s)≤κ max Obstacle collision detection: The safety distance model expression is as follows: r safe =r base +fΔm Among them, r safe Indicates the safety distance, r base It represents the basic safety distance when empty, f represents the adjustment factor, Δm represents the vehicle cargo mass, Δm=m-m0; The collision detection formula is as follows: ‖P ego (t)-P obs (t)‖2>r safe Among them, a long represents the longitudinal acceleration, a long,max represents the maximum longitudinal acceleration, a lat is the lateral acceleration, a lat,max represents the maximum lateral acceleration, κ(s) represents the turning radius at time s, and κ max represents the maximum turning curvature, P ego (t) represents the position coordinates of the vehicle at time t, P obs (t) represents the obstacle position coordinate at time t, ‖‖2 represents the L2 norm, [0,t horizon ] indicates the detection time range.

8. The local path planning method for a pure electric self-driving commercial vehicle in a park logistics scenario according to claim 1 is characterized by: In step 7, for the commercial vehicle scenario, a weighted multi-objective cost function is designed to calculate the cost of each feasible path, taking into account the smoothness, safety and followability of the trajectory with the reference trajectory. The expression of the multi-objective cost function is as follows: J=w1J smooth +w2J obstacle +w3J longi Among them, w1, w2, w3 are weight coefficients, J smooth is the smoothing cost, J obstacle is the obstacle cost, J longi Cost for tracking the reference trajectory; (1) The smoothing cost J smooth The expression is: I smooth =J lat_jerk +J long_jerk Reduce sudden acceleration and sharp turns: Lateral acceleration J lat_jerk Minimize: Longitudinal acceleration constraint J long_jerk : Among them, m0 is the unloaded mass of the vehicle; (2) The obstacle cost J obstacle The expression is: I obstacle =J brake +J lat Dynamic braking distance J brake constraint: d obs (s i ) represents the trajectory point s i The distance to the nearest obstacle d obs ; When the total mass of the vehicle m increases, the braking distance required increases in proportion increase; Lateral stability lat constraint: R(s i ) is the point on the trajectory s i The radius of curvature of the road; v(s i ) is the point on the trajectory s i The vehicle speed, μ is the road friction coefficient; Allowable lateral acceleration threshold: The greater the vehicle's total mass m, the weaker the vehicle's ability to resist rollover. reduce; Actual lateral acceleration: from the trajectory curvature radius R (s i ) and vehicle speed v(s i ) calculation, reflecting the centrifugal force during turning; (3) The tracking reference trajectory cost J longi The expression is: I longi =J speed +J dis Among them, J speed represents the speed deviation cost, J dis represents the sum of the lateral deviation distances between the planned path and the reference path, v ref represents the vehicle reference speed, v evaluate represents the trajectory speed of the vehicle planning, L(s i ) represents the trajectory point s in the Frenet coordinate system i The lateral deviation from the reference trajectory.

Images