Campus logistics vehicle path planning method and system based on improved bidirectional A* algorithm

Abstract

The invention provides a campus logistics vehicle path planning method and system based on an improved bidirectional A * algorithm. The method comprises the following steps: constructing a grid map containing static obstacles, road information and tidal people flow data; performing global path planning by adopting an improved bidirectional A * algorithm; based on a vehicle state, a historical speed, a path curvature and a dynamic obstacle, establishing a speed prediction model fusing a current curvature and a future curvature, and performing speed prediction in combination with enhanced Kalman filtering; under a Frenet coordinate system, constructing a local path optimization model and generating a local reference path based on the global path, the obstacle distribution, the vehicle size and the centroid height, and constructing a speed optimization model and generating an optimal speed profile based on the local reference path and the vehicle body quality; and performing iterative optimization according to the local path optimization result and the speed optimization result, and outputting a final path to be called by a vehicle control system. According to the invention, the distribution efficiency and the driving safety of the campus logistics vehicle in a complex dynamic environment can be obviously improved.

Description

Technical Field

[0001] The present invention relates to the field of intelligent logistics and autonomous driving navigation technology, and in particular to a campus logistics vehicle path planning method and system based on an improved bidirectional A* algorithm. Background Art

[0002] With the continued expansion of universities and the rapid development of e-commerce, the demand for on-campus logistics and distribution has increased significantly, and intelligent logistics vehicles have gradually become the primary means of transportation for campus distribution. Campus environments are characterized by complex spatial structures, diverse obstacle types (static obstacles such as buildings, landscaping, and railings), narrow and variable roads, dense intersections, and significant and dynamic tidal flow distribution, which greatly increases the difficulty of path planning.

[0003] In addition, campus logistics vehicles have special physical characteristics that are significantly different from traditional vehicles in actual operation. On the one hand, with the diversification of distribution tasks, logistics vehicles often need to transport large, special-shaped or collective materials, such as furniture, teaching materials, instruments and equipment, etc. In order to meet transportation needs, the vehicle may temporarily add cargo compartments, hang external cargo or open the tailgate, resulting in dynamic changes in the vehicle's overall dimensions (length, width, height). On the other hand, the vehicle's load mass and center of gravity height will also change in real time with the cargo loading situation. These factors not only affect the vehicle's traffic capacity and obstacle avoidance strategies in scenarios such as narrow roads, curves, and intersections, but also put forward higher dynamic modeling and real-time adjustment requirements for path planning and speed planning.

[0004] The existing campus logistics vehicle path planning method has the following core defects:

[0005] Global path planning level:

[0006] Traditional A* algorithms have low search efficiency in large-scale, complex environments, making them difficult to meet real-time requirements. While the bidirectional A* algorithm offers some efficiency improvements, it fails to account for dynamic boundary buffers, causing paths to run close to road edges, posing a safety hazard. Furthermore, existing algorithms generally fail to incorporate dynamic environmental information, such as tidal crowd flow, into path cost modeling, leading to irrational path selection during peak hours and low traffic efficiency.

[0007] Local path-speed coordination and multi-constraint fusion level:

[0008] Existing local planning methods generally separate path generation from speed planning, lack a collaborative optimization mechanism, and insufficiently consider the real-time avoidance of dynamic obstacles and the integration of multiple constraints on vehicle physical characteristics (such as load, center of mass, and geometric size changes). As a result, in scenarios such as complex pedestrian flows, dynamic obstacles, and large-scale transportation, it is difficult to balance path feasibility, speed smoothness, and obstacle avoidance safety, affecting the overall safety and efficiency of logistics vehicle operations.

[0009] In summary, existing campus logistics vehicle routing methods struggle to balance global efficiency, local safety, path-speed collaborative optimization, and the multi-source dynamic environment and multi-dimensional physical characteristics of vehicles. A novel campus logistics vehicle routing solution that integrates dynamic environment perception, multi-parameter vehicle modeling, and global-local collaborative optimization is urgently needed to meet the urgent needs of intelligent campus logistics distribution for efficiency, safety, and intelligence. Summary of the Invention

[0010] To address the above technical issues, the present invention aims to provide a campus logistics vehicle path planning method and system based on an improved bidirectional A* algorithm. This invention effectively addresses the challenges of balancing global efficiency, local safety, path-speed coordinated optimization, multi-source dynamic environments, and the multi-dimensional physical characteristics of vehicles.

[0011] To achieve the aforementioned purpose, the specific solutions adopted by the present invention are as follows:

[0012] The present invention provides a campus logistics vehicle path planning method based on an improved bidirectional A* algorithm, comprising:

[0013] S1. Obtain vehicle size, center of mass height, vehicle status, historical speed, dynamic obstacle information, and pedestrian flow information;

[0014] S2, constructing a raster map containing static obstacles, road information, tidal crowd flow data, and tidal crowd flow risk field;

[0015] S3, using the improved bidirectional A* algorithm for global path planning. After the global path is generated, spline curve fitting is used for curvature smoothing optimization;

[0016] S4. Based on vehicle status, historical speed, path curvature, and dynamic obstacles, a speed prediction model is established that integrates current and future curvatures. This model uses an enhanced Kalman filter to perform speed prediction and dynamically adjusts speed limits based on the spatiotemporal risk field.

[0017] S5. In the Frenet coordinate system, based on the global path, obstacle distribution, vehicle size, and center of mass height parameters, a local path optimization model is constructed to generate a local reference path.

[0018] S6. In the Frenet coordinate system, a speed optimization model is constructed based on the local reference path and vehicle mass to generate the optimal speed profile;

[0019] S7. Perform iterative optimization based on the local path optimization results and the speed optimization results;

[0020] S8. Integrate the iterative optimization results and output the final path for the vehicle control system to call.

[0021] Furthermore, the method for constructing the tidal flow risk field in step S2 is:

[0022] R crowd (x,y,t)=ρ(x,y,t)·W t (t)·W s (x,y) (1)

[0023] Where R crowd (x, y, t) represents the risk value of the crowd flow at the spatial location (x, y) on campus at time t; ρ(x, y, t) represents the real-time crowd flow density at the spatial location (x, y) on campus at time t; W t (t) is the time weight coefficient; W s (x,y) is the spatial weight coefficient.

[0024] Furthermore, in step S3, the total cost function of the improved bidirectional A* algorithm is:

[0025] F(n)=G(n)+H′(n) (2)

[0026] Where G(n) is the actual cumulative cost from the starting point to node n, and H′(n) is the improved heuristic function, which is defined as follows:

[0027] H′(n)=H geo (n)+α c r crowd (n,t)+α b Φ buffer (n,t) (3)

[0028] Where H geo (n) is the geometric distance estimate from node n to the target node, R crowd (n,t) is the tidal flow risk field value of node n at time t, α c , α b is the weight coefficient, Φ buffer (n,t) is the penalty term of node n for the adaptive boundary buffer, Φ buffer (n,t) is defined as:

[0029]

[0030] Where k buffer is the buffer penalty coefficient, d edge (n) is the distance from node n to the road boundary, r buffer (n,t) is the width of the adaptive boundary buffer, r buffer (n,t)=r0+λ1R crowd (n,t)+λ2v(n), λ1 and λ2 are adaptive buffer adjustment parameters, and v(n) is the speed of node n.

[0031] Furthermore, in step S4, the speed prediction formula is:

[0032]

[0033] Where, ∈ is a small constant to prevent the denominator from being zero, R(x,y,t) is the space-time risk field, λ obs is the maximum weight / intensity of the space-time risk field on speed suppression, a t is the acceleration, w t is the process noise;

[0034]

[0035] Where m is the number of pre-response steps, γ is the pre-response intensity coefficient, is the curvature of the i-th step.

[0036] Furthermore, in step S4, the fusion mechanism of the current curvature and the future curvature is:

[0037] By interpolating the curvature lookup table of the global path, the curvature sequence of multiple time steps in the future is obtained. Where n is the number of look-ahead steps;

[0038] The effective curvature is obtained by fusing the current curvature and the future curvature according to the time decay weight:

[0039]

[0040] Where, the time decay weight ω i =e -λ·i , λ is the attenuation coefficient;

[0041] Covariance matrix of process noise:

[0042]

[0043] Among them, α is the current curvature influence coefficient, and β is the maximum curvature influence coefficient in the future;

[0044] Furthermore, the speed upper and lower limits are adjusted dynamically according to the spatiotemporal risk field, including:

[0045] Constructing a space-time risk field:

[0046]

[0047] Where η is the balance coefficient between curvature and collision risk, κ(x,y) is the curvature of the current position, and d min (x, y, t) is the distance between the current position and the nearest obstacle;

[0048] Dynamically adjust the speed upper and lower limits based on the spatiotemporal risk field:

[0049] v min =max(0,v current -δR(p t ,t)) (10)

[0050]

[0051] Where, v min is the minimum speed allowed at the current moment, v max is the maximum speed allowed at the current moment, v current is the actual speed of the vehicle, v limit is the theoretical speed limit; R(p t ,t) is the current position p t and the risk field value at time t, δ and θ are adjustment coefficients.

[0052] Furthermore, step S5 includes:

[0053] In the Frenet coordinate system, sampling is performed at equal intervals along the longitudinal direction of the reference line, and multiple points are discretely constructed into a two-dimensional node grid in the transverse direction. Each pair of adjacent nodes is connected by a quintic polynomial in the transverse trajectory, and dense sampling is performed on the trajectory to calculate the transverse first-order, second-order, and third-order derivatives, the reference line offset, and the distance to the obstacle. The path optimization cost function is:

[0054]

[0055] Where, l(s i ) is the lateral position of the i-th sampling point, l′(s i ),l″(s i ),l″′(s i ) are its first, second and third order derivatives, d ij is the distance from the i-th point to the h-th obstacle, w is the vehicle width, L is the vehicle length, h is the vehicle center of mass height, w1, w3, w4, w5 are weight coefficients, d ij is the Euclidean distance from the i-th sampling point to the j-th obstacle, σ is the attenuation coefficient of the distance penalty, and w 2,0 、w 2,1 , is the second-order derivative weight benchmark term and the highly correlated term, w 6,0 、w 6,1 is the safety distance penalty weight benchmark term and the highly correlated term, d i,min is the distance from point i to the nearest obstacle, d0 is the basic safety distance, k1, k2, and k3 are the gain coefficients of the center of mass height, vehicle width, and length to the safety distance, respectively. + Represents the ReLU operation;

[0056] The objective function of quadratic programming smooth optimization is:

[0057]

[0058] Where, l i is the horizontal position of the i-th discrete point, l c,i The reference path output by the path optimization cost function, is the lateral deviation penalty, w2(l i′ ) 2 is the heading angle change penalty, w3(l″ i ) 2 is the curvature penalty, w4(l″′ i ) 2 is the curvature change rate penalty, w5(l i -l c,i ) 2 To penalize the reference line, J end is the terminal state deviation cost;

[0059] The constraints of formula (12) and formula (13) are:

[0060] l min (s c )+r safe ≤l c ≤l max (s c )-r safe (14)

[0061] Where, L is the vehicle length; w is the vehicle width;

[0062] s i is the longitudinal coordinate of the vehicle center in the Frenet coordinate system; l(s i ) is the lateral coordinate of the vehicle center in the Frenet coordinate system; l min (s c ), l max (s c ):respectively in s c The lower and upper boundaries of the feasible region of the position; r safe For safety margin.

[0063] Furthermore, step S6 includes:

[0064] The ST graph is constructed based on the longitudinal arc length s of the path and the planning time t. For each sampling point, the total cost is recursively calculated using dynamic programming. The dynamic programming objective function is:

[0065]

[0066] Where, v i is the velocity of point i, a i is the acceleration of point i, v pred.i is the predicted speed of point i output by the speed prediction model, Φ obs is the obstacle avoidance cost function, m is the vehicle mass, w pred is the speed deviation weight, w a is the acceleration penalty weight, w obs Penalty weight for obstacle avoidance;

[0067] After the initial velocity profile is obtained by dynamic programming, the optimal velocity profile is generated by quadratic programming. The objective function of quadratic programming is:

[0068]

[0069] Where, v i is the velocity of point i, a i is the acceleration of point i, v ref is the road speed limit or recommended speed, is the reference speed of point i output in the dynamic planning stage, m is the vehicle mass, w a is the acceleration penalty weight, w v is the speed deviation, w ref_dp is the dynamic programming reference speed tracking weight, w j Penalty weight for acceleration rate;

[0070] To achieve the aforementioned purpose, the present invention also adopts the following specific solutions:

[0071] The present invention provides a campus logistics vehicle path planning system based on an improved bidirectional A* algorithm, comprising:

[0072] Environmental perception and map interface module, used to obtain real-time information such as obstacles, roads, center of mass height, and tidal flow of people;

[0073] The global path planning module is used to construct grid maps and tidal crowd risk fields. It uses an improved bidirectional A* algorithm for global path planning. After the global path is generated, spline curve fitting is used for curvature smoothing optimization.

[0074] The speed prediction module is used to establish a speed prediction model that integrates current and future curvatures based on vehicle status, historical speed, path curvature, and dynamic obstacles, and combines it with enhanced Kalman filtering for speed prediction;

[0075] The local path optimization module is used to build a local path optimization model and generate a local reference path based on the global path, obstacle distribution, vehicle size, and center of mass height parameters in the Frenet coordinate system;

[0076] The speed optimization module is used to build a speed optimization model based on the local reference path and vehicle mass in the Frenet coordinate system, and dynamically adjust the speed upper and lower limits according to the spatiotemporal risk field to generate the optimal speed profile;

[0077] The path-speed decoupling complementary optimization module is used to perform multiple rounds of iterative optimization based on the local path optimization results and speed optimization results;

[0078] The trajectory fusion and output module is used to fuse the iterative optimization results and output the final path for the vehicle control system to call.

[0079] The beneficial effects of the present invention are:

[0080] (1) This paper introduces boundary buffers and tidal crowd flow effects into the traditional bidirectional A* algorithm, achieving efficient global path search in complex campus road environments. By setting up boundary buffers, the safety redundancy of paths is effectively improved, reducing the risk of collisions caused by road boundaries or obstacles. The introduction of tidal crowd flow effects enables path selection to dynamically avoid crowded areas, significantly enhancing the adaptability and safety of path planning.

[0081] (2) During the path and speed decoupling optimization phase, the path optimization component incorporates the vehicle's center of mass height and geometric dimensions (including width and length) into the modeling of constraints and cost functions for the first time, enabling adaptive path planning for different vehicle sizes and center of gravity characteristics. This mechanism dynamically adjusts the lateral feasible space and safety distance thresholds, automatically increasing penalties for paths with large curvature or large lateral acceleration for vehicles with high center of mass or variable geometric dimensions, guiding the path optimization to select a smoother and safer trajectory, further improving the feasibility and passability of the path.

[0082] In the speed optimization phase, this invention incorporates vehicle mass into the speed optimization model and cost function. By weighting acceleration and jerk with vehicle mass, the vehicle's dynamic characteristics and safety limits are fully reflected during acceleration, deceleration, and speed changes. This effectively avoids sudden acceleration or deceleration caused by mass changes, improving the smoothness, comfort, and safety of the logistics vehicle's ride.

[0083] Compared to existing technologies, this invention not only improves path search efficiency and dynamic obstacle avoidance capabilities, but also achieves collaborative optimization by incorporating Kalman filtering, overcoming the limitations of traditional methods that fail to consider key factors such as vehicle geometry, center of mass height, mass, and tidal crowd flow. Through these innovative improvements, this invention can significantly improve the delivery efficiency and driving safety of campus logistics vehicles in complex and dynamic environments, reduce logistics operating costs, and provide a new, efficient, safe, and practical technical solution for campus logistics vehicle path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0084] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for describing the embodiments or the prior art.

[0085] Figure 1 This is a flow chart of the campus logistics vehicle path planning method based on the improved bidirectional A* algorithm of the present invention;

[0086] Figure 2 This is a data interaction flow chart of the campus logistics vehicle path planning method based on the improved bidirectional A* algorithm of the present invention;

[0087] Figure 3 This is a working principle diagram of the speed prediction model in the campus logistics vehicle path planning method based on the improved bidirectional A* algorithm of the present invention;

[0088] Figure 4 Schematic diagram of the feasible region boundary in the present invention.

[0089] Numbers in the figure: 1-path after dynamic planning in the path planning stage, 2-path after quadratic planning in the path planning stage, 3-vehicle. DETAILED DESCRIPTION

[0090] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions of the present invention will be clearly and completely described below in conjunction with specific embodiments and drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Therefore, the detailed description of the embodiments of the present invention provided in the drawings below is not intended to limit the scope of protection claimed.

[0091] This invention systematically solves the technical defects of existing campus logistics vehicle path planning in complex dynamic environments through multi-dimensional innovation. The overall concept includes the following aspects:

[0092] 1. Global path planning optimization

[0093] This paper proposes an improved bidirectional A* algorithm to achieve intelligent global routing for campus logistics vehicles. During the path search process, this algorithm dynamically optimizes the traditional heuristic function by comprehensively considering multiple factors such as geographic distance, tidal crowd risk, and road boundary buffers. Specifically, the optimized heuristic evaluation function consists of three parts:

[0094] The first item is the geographical distance evaluation, which represents the shortest spatial distance from the current node to the target node;

[0095] The second item is the tidal crowd risk item, which reflects the crowd density and risk level in different areas of the campus at different times. This risk field is obtained by integrating historical crowd trajectories, course schedules, campus activities, and real-time perception data using a spatiotemporal prediction model. Its value changes dynamically with spatial location and time.

[0096] The third factor is the adaptive boundary buffer factor, which measures the safe distance between the path and the road boundary. The adaptive boundary buffer width is adjusted based on factors such as the basic buffering requirements, the local pedestrian risk level, and the current vehicle speed, thereby balancing traffic efficiency and safety.

[0097] Among them, the weights of various parameters can be flexibly configured according to actual needs to achieve the optimal balance between path safety and efficiency.

[0098] 2. Speed ​​prediction for local planning

[0099] In the local path optimization stage, the present invention proposes a speed prediction model that integrates path curvature. This model is based on the Kalman filter method and integrates the vehicle's current state, historical speed, path curvature changes (including consideration of future multi-step forward curvature), and the movement trend of dynamic obstacles to achieve real-time dynamic prediction of the vehicle's future speed. The state transition equation in the model will be adaptively adjusted according to the change in effective curvature, making the speed prediction of the vehicle more accurate in different curves and obstacle environments. The effective curvature parameter is normalized to reflect the impact of path curvature on the lateral movement of the vehicle, effectively improving the adaptability and robustness of the model.

[0100] 3. Path-speed decoupling and complementary optimization

[0101] The present invention realizes decoupling and complementary optimization of path and speed in the Frenet coordinate system.

[0102] In terms of path (SL) optimization, a smooth and safe obstacle avoidance path is generated through quintic polynomial trajectories and dynamic programming. The vehicle center of mass height parameter is introduced into the path cost function. The center of mass height will significantly affect the vehicle's maximum safe turning speed and allowable turning radius, thereby determining the stability of the vehicle when turning. The higher the center of mass height, the greater the risk of rollover when turning at high speeds. Therefore, the dynamic stability constraints brought by the center of mass height must be fully considered during the path generation process. At the same time, the vehicle size (such as width and length) will directly affect the vehicle's passable space in narrow sections or sharp bends, and the obstacle avoidance space is dynamically adjusted accordingly during path planning.

[0103] In terms of speed (ST) optimization, the vehicle's speed profile is dynamically adjusted based on the aforementioned speed prediction results. The speed optimization objective function specifically incorporates the effect of mass on acceleration changes (i.e., jerk). This allows for smoother speed changes under real-world conditions, such as load variations or large-item transport. This reduces safety hazards associated with sudden acceleration or deceleration, improving operational smoothness and safety.

[0104] Through the multi-dimensional innovations of global path optimization, local speed prediction, and decoupling and complementary optimization, the present invention significantly improves the path safety, speed smoothness, and overall operating efficiency of campus logistics vehicles in complex dynamic environments, meeting the actual needs of intelligent campus logistics distribution for efficient and safe path planning. The specific plan is as follows.

[0105] See also Figure 1-Figure 3 The present invention provides a campus logistics vehicle path planning based on an improved bidirectional A* algorithm, including:

[0106] S1. Obtain vehicle size, center of mass height, vehicle status, and historical speed, and use on-board sensors to collect dynamic obstacle information and pedestrian flow information.

[0107] Specifically, vehicle dimensions (vehicle width W, vehicle length L) are obtained by measuring them using external sensors such as camera visual recognition before departure, detecting feature points of the vehicle's outer contour, obtaining width and length parameters, and inputting them into the local path optimization model in real time.

[0108] Vehicle center of mass height (h) and mass (m) acquisition: The mass is acquired through four-wheel load sensors and the vertical load changes are measured under vehicle acceleration and deceleration conditions. Combined with the dynamic model, vehicle geometry parameters and acceleration information, the center of mass height is dynamically inferred to adapt to different loads and vehicle model changes.

[0109] The above parameters are input into the optimization model in real time, and the relevant constraints and cost functions in path optimization and speed optimization are dynamically adjusted to achieve adaptive optimization for different vehicle sizes, loads and center of gravity characteristics.

[0110] S2. Construct a raster map containing static obstacles, road information, and tidal crowd flow data.

[0111] Specifically, by analyzing historical crowd flow data, course schedules, event information, and real-time perception data, we predict the crowd flow density in different time periods and use spatiotemporal prediction modeling to obtain the tidal crowd flow risk field R. crowd (x, y, t), and mark road attributes and safety thresholds on the map to provide a dynamic environment basis for subsequent path and speed planning.

[0112] Furthermore, the formula for the tidal flow risk field is:

[0113] Rcrowd (x,y,t)=ρ(x,y,t)·W t (t)·W s (x,y) (1)

[0114] Where R crowd (x, y, t) represents the pedestrian risk value at the spatial location (x, y) on campus at time t. A higher value indicates a greater potential risk to logistics vehicles at that spatial point. ρ(x, y, t) represents the real-time pedestrian density (in people / square meter) at the spatial location (x, y) on campus at time t. This can be collected through devices such as cameras and sensors, or predicted based on historical data (e.g., density peaks during tidal periods such as get out of class time and meal times). W t (t) is the time weight coefficient, which reflects the impact of campus tidal patterns on risk. The value is 1.2-1.5 during peak hours (such as 11:30-13:00 and 17:30-19:00), 0.8-1.0 during off-peak hours, and 0.5-0.7 during low-peak hours (such as late at night); W s (x,y) is the spatial weight coefficient, which reflects the impact of the inherent attributes of the region on the risk. The values ​​of congested areas such as main roads, entrances to teaching buildings, and areas around canteens are 1.1-1.3, and the values ​​of green belts, open spaces, etc. are 0.6-0.9.

[0115] S3, using the improved bidirectional A* algorithm for global path planning. After the global path is generated, spline curve fitting is used for curvature smoothing optimization;

[0116] Furthermore, an improved bidirectional A* algorithm is used for global path search. The total cost function of the traditional A* algorithm is:

[0117] F(n)=G(n)+H(n)

[0118] Among them, F(n) is the total cost of node n, G(n) is the actual cost from the starting point to node n, and H(n) is the heuristic estimated cost function from node n to the end point.

[0119] This paper proposes a bidirectional A* path search method that integrates the dynamic risk field of tidal human flow and adaptive boundary buffer. Its total cost function is:

[0120] F(n)=G(n)+H′(n) (2)

[0121] Where G(n) is the actual cumulative cost from the starting point to node n, and H′(n) is the improved heuristic function, as follows:

[0122] H′(n)=H geo (n)+α c R crowd (n,t)+αb Φ buffer (n,t) (3)

[0123] Where H geo (n) is the geometric distance estimate from node n to the target node, R crowd (n,t) is the tidal flow risk field value of node n at time t, the coordinates of node n are (x,y), Φ buffer (n,t) is the penalty term of node n regarding the adaptive boundary buffer, α c , α b are the weight parameters of the tidal flow risk field and the adaptive boundary buffer, respectively, which can be dynamically adjusted through historical data or online learning. buffer (n,t) is defined as:

[0124]

[0125] Where k buffer is the adaptive boundary buffer penalty coefficient, d edge (n) is the distance from node n to the road boundary, r buffer (n,t) is the adaptive boundary buffer width.

[0126] Furthermore, the adaptive boundary buffer width r buffer (n, t) According to the basic buffer width r0, the local tidal flow risk field value R crowd (n, t), estimated speed v(n) and other factors are adjusted dynamically, r buffer (n,t)=r0+λ1R crowd (n,t)+λ2v(n), where λ1 and λ2 are the adaptive buffer adjustment parameters.

[0127] Furthermore, the estimated speed v(n) is determined based on factors such as road type, historical operating data, speed limits, and environmental risks, reflecting the vehicle's expected speed on different road sections. This speed is used to dynamically adjust the width of the adaptive boundary buffer during the global path planning phase, improving the planned path's adaptability to actual operational safety requirements.

[0128] Specifically, the tidal flow risk area R crowd (x, y, t) is obtained by integrating historical crowd flow trajectories, course schedules, event information and real-time perception data, and adopting spatiotemporal prediction modeling. Its value changes dynamically with time t and spatial position (x, y), which is used to reflect the crowd density and risk level in different areas at different time periods. Based on historical data and real-time perception when triggering replanning, the tidal crowd flow risk field value R of node n at time t is predicted. crowd (n,t).

[0129] Furthermore, after the global path is generated, spline curve fitting is used to perform curvature smoothing optimization.

[0130] S4. Based on vehicle status, historical speed, path curvature, and dynamic obstacles, a speed prediction model that integrates current and future curvatures is established. Enhanced Kalman filtering is used for speed prediction, and the upper and lower speed limits are dynamically adjusted according to the spatiotemporal risk field.

[0131] Specifically, based on the vehicle's current position, driving direction, historical speed, and surrounding dynamic obstacle information obtained by on-board sensors, a speed prediction model that integrates current curvature and future curvature is established. The enhanced Kalman filter method is combined with the dynamic obstacle motion model to predict the speed within a period of time in the future. The speed prediction formula is:

[0132]

[0133] In the formula, 1-Δt·f(k eff )-Δt·h(R(x,y,t)) is the state transfer equation, which integrates the spatiotemporal risk field and path curvature; ∈ is a small constant to prevent the denominator from being zero; R(x,y,t) is the space-time risk field; λ obs is the maximum weight / intensity of space-time inhibition of speed; a t is the acceleration, w t is the process noise;

[0134]

[0135] Where m is the number of pre-response steps, γ is the pre-response intensity coefficient, is the curvature of the i-th step.

[0136] Furthermore, in step S4, the fusion mechanism of the current curvature and the future curvature is:

[0137] By interpolating the curvature lookup table of the global path, the curvature sequence of multiple time steps in the future is obtained. Where n is the number of look-ahead steps;

[0138] The effective curvature is obtained by fusing the current curvature and the future curvature according to the time decay weight:

[0139]

[0140] Where, the time decay weight ω i =e -λ·i , λ is the attenuation coefficient;

[0141] Covariance matrix of process noise:

[0142]

[0143] Among them, α is the current curvature influence coefficient, and β is the maximum curvature influence coefficient in the future;

[0144] Furthermore, the speed upper and lower limits are adjusted dynamically according to the spatiotemporal risk field, including:

[0145] Constructing a space-time risk field:

[0146]

[0147] Where η is the balance coefficient between curvature and collision risk, k(x,y) is the curvature of the current position, and d min (x, y, t) is the distance between the current position and the nearest obstacle;

[0148] Dynamically adjust the speed upper and lower limits based on the spatiotemporal risk field:

[0149] v min =max(0,v current -δR(p t ,t)) (10)

[0150]

[0151] Where, v min is the minimum speed allowed at the current moment, v max is the maximum speed allowed at the current moment, v current is the actual speed of the vehicle, v limit is the theoretical speed limit; R(p t ,t) is the current position p t and the risk field value at time t, δ and θ are adjustment coefficients.

[0152] Furthermore, a Kalman filter measurement update is performed after the velocity prediction:

[0153] After speed prediction, use the sensor to measure the speed z t Do measurement update:

[0154]

[0155] P t =(1-K t )P t|t-1

[0156] The speed prediction model workflow is as follows:

[0157] a. Input vehicle status, path curvature, obstacle information, and vehicle mass.

[0158] b. Curvature sequence acquisition: According to the vehicle's current position and forward distance, the current curvature κ is obtained from the curvature lookup table t and future curvature sequences

[0159] c. Effective curvature calculation: Fusion of current and future curvatures according to time decay weight to obtain effective curvature κ eff .

[0160] d. State prediction: using dynamically adjusted state transition equations and process noise covariance matrix Perform Kalman filter prediction.

[0161] e. Control input correction: Adjust the control input according to the future curvature sequence to achieve pre-response deceleration / acceleration.

[0162] f. Measurement update: Fuse sensor measurement values ​​and perform measurement update.

[0163] S5. In the Frenet coordinate system, based on the global path, obstacle distribution, vehicle size, and center of mass height parameters, a local path optimization model is constructed to generate a local reference path.

[0164] S6. In the Frenet coordinate system, a speed optimization model is constructed based on the local reference path and vehicle mass to generate the optimal speed profile.

[0165] Furthermore, a path-velocity decoupling method in the Frenet coordinate system is employed. A global path is first generated using an improved bidirectional A* algorithm and mapped to a reference line trajectory via Frenet coordinate transformation. Subsequently, local path optimization (SL optimization) and velocity optimization (ST optimization) are performed sequentially to achieve effective avoidance of dynamic obstacles. The optimal velocity profile is then generated by combining vehicle mass and size parameters.

[0166] Specifically, step S5 includes: in the Frenet coordinate system, sampling is performed at equal intervals along the reference line in the longitudinal direction, and multiple points are discretely constructed in the transverse direction to form a two-dimensional node grid. Each pair of adjacent nodes is connected to the transverse trajectory using a quintic polynomial, and dense sampling is performed on the trajectory. The transverse first-order, second-order, and third-order derivatives, the reference line offset, and the distance to the obstacle are calculated. The path optimization cost function is:

[0167]

[0168] Where, l(s i ) is the lateral position of the i-th sampling point, l′(s i ),l″(s i ),l″′(s i ) are its first, second and third order derivatives, d ijis the distance from the i-th point to the j-th obstacle, w is the vehicle width, L is the vehicle length, h is the vehicle center of mass height, w1, w3, w4, w5 are weight coefficients, d ij is the Euclidean distance from the i-th sampling point to the j-th obstacle, σ is the attenuation coefficient of the distance penalty, and w 2,0 、w 2,1 , is the second-order derivative weight benchmark term and the highly correlated term, w 6,0 、w 6,1 is the safety distance penalty weight benchmark term and the highly correlated term, d i,min is the distance from point i to the nearest obstacle, d0 is the basic safety distance, k1, k2, and k3 are the gain coefficients of the center of mass height, vehicle width, and length to the safety distance, respectively. + Represents the ReLU operation.

[0169] The objective function of quadratic programming smooth optimization is:

[0170]

[0171] Where, l i is the horizontal position of the i-th discrete point; l c,i is the reference path output by the path optimization cost function; w1, w2, w3, w4, w5 are weight coefficients; As a penalty for lateral deviation, the trajectory is kept as close to the reference line (such as the lane centerline) as possible to prevent the trajectory from deviating too far; w2(l i′ ) 2 The penalty for heading angle change controls the slope of the trajectory to prevent the vehicle trajectory from being too steep and ensure smooth driving; w3(l″ i ) 2 Curvature penalty controls the curvature of the trajectory, suppresses sharp turns, and ensures drivability and comfort; w4(l″′ i ) 2 Penalty for curvature change rate, ensuring smooth curvature change and reducing the danger of "sudden turn"; w5(l i -l c,i ) 2 To penalize the reference line, the trajectory is further constrained to be close to the reference trajectory; J end It is the endpoint state penalty, which has special requirements for the endpoint, such as the deviation penalty of the endpoint position, heading angle, curvature, etc. from the expected value.

[0172] Furthermore, the constraints of formula (12) and formula (13) are:

[0173] l min (s c )+r safe ≤l c ≤l max (sc )-r safe (14)

[0174] Where, Respectively represent the longitudinal coordinates of the front and rear boundaries of the vehicle in the Frenet coordinate system (front: back: ); Respectively represent the lateral coordinates of the left and right boundaries of the vehicle in the Frenet coordinate system (left: right: ); L is the vehicle length, which is obtained by visual sensor detection before departure; w is the vehicle width, which is obtained by visual sensor detection before departure; s i is the longitudinal coordinate of the vehicle center in the Frenet coordinate system; l(s i ) is the lateral coordinate of the vehicle center in the Frenet coordinate system. min (s c ), l max (s c ):respectively in s c The lower and upper boundaries of the feasible region of the location, obtained by the map or perception system; safe is the safety margin used to prevent the vehicle from colliding with the boundary of the feasible region.

[0175] Specifically, for the path planning of the vehicle in the Frenet coordinate system, the four corner points of the vehicle must always be within the feasible region of the road. Let the position of the vehicle center in the Frenet coordinate system be (s i ,l(s i ), the vehicle length is L and the width is w, then the coordinates of the four corner points of the vehicle can be expressed as:

[0176] Front left corner:

[0177] Front right corner:

[0178] Rear left corner:

[0179] Rear right corner:

[0180] In order to ensure that all four corner points of the vehicle are within the feasible region, the following constraints need to be imposed on each corner point:

[0181] l min (s c )+r safe ≤l c ≤l max (s c )-r safe

[0182] The constraints of the four corner points of the vehicle can be expanded into the following four groups:

[0183]

[0184] like Figure 4 As shown, the upper and lower lines represent the boundaries of the feasible region, label 1 represents the path of vehicle 3 after dynamic planning in the path planning stage, and label 2 represents the path of the vehicle after secondary planning in the path planning stage.

[0185] Furthermore, step S6 includes:

[0186] After obtaining the local reference path, the ST graph is constructed based on the longitudinal arc length s of the path and the planning time t. For each sampling point, the total cost is recursively calculated using dynamic programming. The objective function of dynamic programming (DP planning) is:

[0187]

[0188] Among them, v i is the velocity of point i; a i is the acceleration of point i; v pred.i is the predicted speed of point i output by the speed prediction model; Φ obs is the obstacle avoidance cost function, indicating whether the point is near an obstacle or has a collision risk (if it is close to the obstacle, the penalty is large); m is the vehicle mass, w pred is the speed deviation weight, which is used to track the priority of the predicted speed, w a is the acceleration penalty weight, which is used to suppress large acceleration to ensure comfort and safety; obs Obstacle avoidance penalty weight, used to prevent collision with obstacles;

[0189] After obtaining the preliminary velocity profile in the dynamic programming stage, quadratic programming (QP programming) is used to generate the optimal velocity profile to further improve the smoothness, safety, and controllability of the velocity trajectory. The quadratic programming objective function is:

[0190]

[0191] Where, v i is the velocity of point i; a i is the acceleration of point i; v ref Speed ​​limits or recommended speeds for roads, such as those set by traffic regulations or maps; is the reference speed of point i output in the dynamic planning stage; m is the vehicle mass; w a is the acceleration penalty weight, w v is the speed deviation weight, which is used to encourage people to stay close to the speed limit or recommended speed; ref_dpw is the dynamic programming reference speed tracking weight, which is used to ensure that the quadratic programming result does not deviate from the local reference path output in the dynamic programming stage; j Penalize the acceleration rate weight to further improve comfort and smoothness;

[0192] The constraints include: upper and lower speed limits: dynamically adjusted by the space-time risk field R(x, y, t); physical constraints on acceleration and acceleration rate; and collision safety constraints: ensuring that the trajectory on the ST diagram does not enter the area occupied by obstacles.

[0193] S7. Perform iterative optimization based on the local path optimization results and the speed optimization results.

[0194] Specifically, based on the local path and speed optimization results, multiple rounds of iterative optimization are performed to improve the overall feasibility and robustness of the trajectory.

[0195] S8. Integrate the iterative optimization results and output the final path for the vehicle control system to call.

[0196] Specifically, the iterative optimization results are integrated to generate the final path, including the spatial position, velocity, acceleration and other information of each discrete trajectory point, for real-time tracking by the vehicle control system.

[0197] In general, the overall process of path and speed optimization of the present invention includes the following steps:

[0198] a) Obtain the vehicle's current location, historical status, global reference path, and environmental perception information (including obstacles, tidal crowd flow, vehicle size, and center of mass height), and construct a grid map and tidal crowd flow risk field;

[0199] b) Use the improved bidirectional A* algorithm to perform global path planning on the grid map and output a smooth reference path;

[0200] c) In the Frenet coordinate system, based on the global path, obstacle distribution, vehicle size, center of mass height and other parameters, SL (lateral-longitudinal) local path dynamic planning and QP smoothing optimization are performed to generate a collision-free and controllable local reference path;

[0201] d) Based on the local reference path, the spatiotemporal risk field, and the impact of vehicle dynamics, a speed prediction model is used to recursively predict vehicle speed at several moments in the future, and the upper and lower speed limits are dynamically adjusted;

[0202] e) On the ST diagram, a combined optimization method of dynamic optimization and quadratic optimization is used to generate the optimal speed profile that takes into account dynamic feasibility, safety, comfort and traffic efficiency;

[0203] f) Based on the local path and speed optimization results, multiple rounds of iterative optimization are performed to improve the overall feasibility and robustness of the trajectory;

[0204] g) Fuse the iterative optimization output results to generate the final feasible trajectory, including the spatial position, velocity, acceleration and other information of each discrete trajectory point, for real-time tracking by the vehicle control system.

[0205] On the other hand, the present invention provides a campus logistics vehicle path planning system based on an improved bidirectional A* algorithm, comprising:

[0206] Environmental perception and map interface module: real-time acquisition of information such as obstacles, roads, centroid height, and tidal flow of people;

[0207] The global path planning module generates a global reference path based on the improved bidirectional A* algorithm;

[0208] Speed ​​prediction module, which integrates vehicle status, path curvature, and dynamic obstacles to predict speed in real time based on the Kalman filter method;

[0209] The local path optimization module is used to dynamically adjust and optimize the local path in the Frenet coordinate system by combining obstacle information, center of mass height, and vehicle size information;

[0210] The speed optimization module is used to optimize the speed profile by combining dynamic programming and quadratic programming in the Frenet coordinate system to generate the optimal speed profile;

[0211] The path-speed decoupling complementary optimization module is used to perform multiple rounds of alternating iterations based on the local path optimization results and the speed optimization results;

[0212] The trajectory fusion and output module is used to fuse the local path and speed optimization results and output the final path for the vehicle control system to call.

[0213] Among them, the global path planning module, local path optimization module, speed prediction module, speed planning module, path-speed decoupling complementary optimization module, trajectory fusion and output module, and environmental perception and map interface module work together to achieve multi-factor dynamic adaptive joint optimization of path and speed.

[0214] The beneficial effects of the present invention are as follows: (1) By introducing boundary buffers and tidal crowd flow effects into the traditional bidirectional A* algorithm, the present invention achieves efficient global path search in a complex campus road environment. By setting a boundary buffer, the safety redundancy of the path is effectively improved, and the collision risk caused by road boundaries or obstacles is reduced; by introducing tidal crowd flow effects, the path selection can dynamically avoid crowded areas, significantly enhancing the adaptability and safety of path planning.

[0215] (2) During the path and speed decoupling optimization phase, the path optimization component incorporates the vehicle's center of mass height and geometric dimensions (including width and length) into the modeling of constraints and cost functions for the first time, enabling adaptive path planning for different vehicle sizes and center of gravity characteristics. This mechanism dynamically adjusts the lateral feasible space and safety distance thresholds, automatically increasing penalties for paths with large curvature or large lateral acceleration for vehicles with high center of mass or variable geometric dimensions, guiding the path optimization to select a smoother and safer trajectory, further improving the feasibility and passability of the path.

[0216] (3) In the speed optimization part, the vehicle mass factor is introduced into the speed optimization model and cost function. By weighting the acceleration and acceleration change rate (jerk) with the vehicle mass, the vehicle can fully demonstrate its own dynamic characteristics and safety limits during acceleration, deceleration, and speed change, effectively avoiding sudden acceleration or deceleration caused by mass changes, and improving the smoothness, comfort, and safety of the logistics vehicle.

[0217] Compared to existing technologies, this invention not only improves path search efficiency and dynamic obstacle avoidance capabilities, but also achieves coordinated optimization of path and speed, overcoming the limitations of traditional methods that fail to consider key factors such as vehicle geometry, center of mass height, mass, and tidal flow of people. Through these innovative improvements, this invention can significantly improve the delivery efficiency and driving safety of campus logistics vehicles in complex and dynamic environments, reduce logistics operating costs, and provide a new, efficient, safe, and practical technical solution for campus logistics vehicle path planning.

[0218] The above description is merely a preferred embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Based on the embodiments of the present invention, any other embodiments obtained by ordinary technicians in this field without creative work, such as making formal modifications to the technical solutions described in the following embodiments within the inspiration of the present invention, or making equivalent replacements for some of the technical features therein, shall fall within the scope of protection of this patent.

Claims

1. A campus logistics vehicle path planning method based on an improved bidirectional A* algorithm is characterized by: include: S1. Obtain vehicle size, center of mass height, vehicle status, historical speed, dynamic obstacle information, and pedestrian flow information; S2, constructing a raster map containing static obstacles, road information, tidal crowd flow data, and tidal crowd flow risk field; S3, using the improved bidirectional A* algorithm for global path planning. After the global path is generated, spline curve fitting is used for curvature smoothing optimization; S4. Based on vehicle status, historical speed, path curvature, and dynamic obstacle information, a speed prediction model is established that integrates current and future curvatures. This model is combined with an enhanced Kalman filter for speed prediction, and the speed upper and lower limits are dynamically adjusted according to the spatiotemporal risk field. S5. In the Frenet coordinate system, based on the global path, obstacle distribution, vehicle size, and center of mass height parameters, a local path optimization model is constructed to generate a local reference path. S6. Build a speed optimization model based on the local reference path and vehicle mass to generate an optimal speed profile; S7. Perform iterative optimization based on the local path optimization results and the speed optimization results; S8. Integrate the iterative optimization results and output the final path for the vehicle control system to call.

2. The campus logistics vehicle path planning method based on the improved bidirectional A* algorithm according to claim 1 is characterized in that: The method for constructing the tidal flow risk field in step S2 is: R crowd (x,y,t)=ρ(x,y,t)·W t (t)·W s (x,y) (1) Where R crowd (x, y, t) represents the risk value of the crowd flow at the spatial location (x, y) on campus at time t; ρ(x, y, t) represents the real-time crowd flow density at the spatial location (x, y) on campus at time t; W t (t) is the time weight coefficient; W s (x,y) is the spatial weight coefficient.

3. The campus logistics vehicle path planning method based on the improved bidirectional A* algorithm according to claim 2 is characterized in that: In step S3, the total cost function of the improved bidirectional A* algorithm is: F(n)=G(n)+H′(n) (2) Where G(n) is the actual cumulative cost from the starting point to node n, and H′(n) is the improved heuristic function, as follows: H′(n)=H geo (n)+a c R crowd (n,t)+a b F buffer (n,t) (3) Where H geo (n) is the geometric distance estimate from node n to the target node, R crowd (n,t) is the tidal flow risk field value of node n at time t, α c , α b is the weight coefficient, Φ buffer (n,t) is the penalty term of node n for the adaptive boundary buffer, Φ buffer (n,t) is defined as: Where k buffer is the buffer penalty coefficient, d edge (n) is the distance from node n to the road boundary, r buffer (n,t) is the width of the adaptive boundary buffer, r buffer (n,t)=r0+λ1R crowd (n,t)+λ2v(n), where λ1 and λ2 are the adaptive boundary buffer adjustment parameters, and v(n) is the estimated speed of node n.

4. The campus logistics vehicle path planning method based on the improved bidirectional A* algorithm according to claim 1 is characterized in that: In step S4, the speed prediction formula is: Where, ∈ is a small constant to prevent the denominator from being zero, R(x,y,t) is the space-time risk field, λ obs is the maximum weight / intensity of the space-time risk field on speed suppression, a t is the acceleration, w t is the process noise; Where m is the number of pre-response steps, γ is the pre-response intensity coefficient, is the curvature of the i-th step.

5. The campus logistics vehicle path planning method based on the improved bidirectional A* algorithm according to claim 4 is characterized in that: In step S4, the fusion mechanism of the current curvature and the future curvature is: By interpolating the curvature lookup table of the global path, the curvature sequence of multiple time steps in the future is obtained. Where n is the number of look-ahead steps; The effective curvature is obtained by fusing the current curvature and the future curvature according to the time decay weight: Where, the time decay weight ω i =e -λ·i , θ is the attenuation coefficient; Covariance matrix of process noise: Where α is the current curvature influence coefficient, and β is the maximum curvature influence coefficient in the future.

6. The campus logistics vehicle path planning method based on the improved bidirectional A* algorithm according to claim 4 is characterized in that: In step S4, dynamically adjusting the speed upper and lower limits according to the spatiotemporal risk field includes: Constructing a space-time risk field: Where η is the balance coefficient between curvature and collision risk, κ(x,y) is the curvature of the current position, and d min (x, y, t) is the distance between the current position and the nearest obstacle; Dynamically adjust the speed upper and lower limits based on the spatiotemporal risk field: v min =max(0,v current -δR(p t ,t)) (10) Where, v min is the minimum speed allowed at the current moment, v max is the maximum speed allowed at the current moment, v current is the actual speed of the vehicle, v limit is the theoretical speed limit; R(p t ,t) is the current position p t and the risk field value at time t, δ and θ are adjustment coefficients.

7. The campus logistics vehicle path planning method based on the improved bidirectional A* algorithm according to claim 1 is characterized in that: Step S5 includes: In the Frenet coordinate system, sampling is performed at equal intervals along the longitudinal direction of the reference line, and multiple points are discretely constructed into a two-dimensional node grid in the transverse direction. Each pair of adjacent nodes is connected by a quintic polynomial in the transverse trajectory, and dense sampling is performed on the trajectory to calculate the transverse first-order, second-order, and third-order derivatives, the reference line offset, and the distance to the obstacle. The path optimization cost function is: Where, l(s i ) is the lateral position of the i-th sampling point, l′(s i ),l″(s i ),l″′(s i ) are its first, second and third order derivatives, d ij is the distance from the i-th point to the j-th obstacle, w is the vehicle width, L is the vehicle length, h is the vehicle center of mass height, w1, w3, w4, w5 are weight coefficients, d ij is the Euclidean distance from the i-th sampling point to the j-th obstacle, σ is the attenuation coefficient of the distance penalty, and w 2,0 、w 2,1 is the second-order derivative weight benchmark term and the highly correlated term, w 6,0 、w 6,1 is the safety distance penalty weight benchmark term and the highly correlated term, d i,min is the distance from point i to the nearest obstacle, d0 is the basic safety distance, k1, k2, and k3 are the gain coefficients of the center of mass height, vehicle width, and length to the safety distance, respectively. + Represents the ReLU operation; The objective function of quadratic programming smooth optimization is: Where, l i is the horizontal position of the i-th discrete point, l c,i The reference path output by the path optimization cost function, is the lateral deviation penalty, w2(l i′ ) 2 is the heading angle change penalty, w3(l″ i ) 2 is the curvature penalty, w4(l″′ i ) 2 is the curvature change rate penalty, w5(l i -l c,i ) 2 To penalize the reference line, J end Penalty for the terminal state; The constraints of formula (12) and formula (13) are: l min (s c )+r safe ≤l c ≤l max (s c )-r safe (14) In formula, L is the vehicle length; w is the vehicle width; s i is the longitudinal coordinate of the vehicle center in the Frenet coordinate system; l(s i ) is the lateral coordinate of the vehicle center in the Frenet coordinate system; l min (s c ), l max (s c ):respectively in s c The lower and upper boundaries of the feasible region of the position; r safe For safety margin.

8. The campus logistics vehicle path planning method based on the improved bidirectional A* algorithm according to claim 1 is characterized in that: Step S6 includes: The ST graph is constructed based on the longitudinal arc length s of the path and the planning time t. For each sampling point, the total cost is recursively calculated using dynamic programming. The dynamic programming objective function is: Where, v i is the velocity of point i, a i is the acceleration of point i, v pred.i is the predicted speed of point i output by the speed prediction model, Φ obs is the obstacle avoidance cost function, m is the vehicle mass, w pred is the speed deviation weight, w a is the acceleration penalty weight, w obs Penalty weight for obstacle avoidance; After obtaining the preliminary velocity profile in the dynamic programming stage, the optimal velocity profile is generated using quadratic programming. The quadratic programming objective function is: Where, v i is the velocity of point i, a i is the acceleration of point i, v ref is the road speed limit or recommended speed, is the reference speed of point i output in the dynamic planning stage, m is the vehicle mass, w a is the acceleration penalty weight, w v is the speed deviation, w ref_dp is the dynamic programming reference speed tracking weight, w j Penalty weight for acceleration rate.

9. Campus logistics vehicle path planning system based on improved bidirectional A* algorithm, characterized by: include: Environmental perception and map interface module, used to obtain real-time information on obstacles, roads, center of mass height, and tidal crowd flow; The global path planning module is used to perform global path planning using an improved bidirectional A* algorithm. After the global path is generated, spline curve fitting is used to perform curvature smoothing optimization. The speed prediction module is used to establish a speed prediction model that integrates current and future curvatures based on vehicle status, historical speed, path curvature, and dynamic obstacles. It uses enhanced Kalman filtering to perform speed prediction and dynamically adjusts speed limits based on the spatiotemporal risk field. The local path optimization module is used to build a local path optimization model and generate a local reference path based on the global path, obstacle distribution, vehicle size, and center of mass height parameters in the Frenet coordinate system; The speed optimization module is used to build a speed optimization model based on the local reference path and vehicle body mass in the Frenet coordinate system to generate the optimal speed profile; The path-speed decoupling complementary optimization module is used to perform multiple rounds of iterative optimization based on the local path optimization results and speed optimization results; The trajectory fusion and output module is used to fuse the iterative optimization results and output the final path for the vehicle control system to call.

Images