Path planning method and system for long-wheelbase heavy-duty car

Abstract

The invention belongs to the technical field of vehicle control, and particularly relates to a long-wheelbase-oriented heavy-duty vehicle path planning method and system, and the method comprises the steps: carrying out the environment perception and fusion modeling based on multi-source sensor data and a high-precision map; calculating the inner wheel difference compensation amount according to the vehicle wheelbase and the current steering angle, and compensating the transverse offset under the Frenet coordinate system; according to the method, the transverse offset is dynamically corrected through the inner wheel difference compensation algorithm designed in the step S2, and the scraping risk caused by inner wheel track deviation during lane changing and turning of the long-wheelbase vehicle is effectively avoided; meanwhile, the rollover risk index is introduced as the dynamic constraint of the path curvature, the rollover possibility of the high-centroid vehicle during high-speed turning or emergency obstacle avoidance is limited from the dynamics level, and the accident rate is remarkably reduced through double guarantee.

Description

Technical Field

[0001] This invention belongs to the field of vehicle control technology, specifically relating to a path planning method and system for heavy-duty vehicles with long wheelbases. Background Technology

[0002] In the high-speed trunk logistics transportation sector, heavy-duty commercial vehicles play an irreplaceable role due to their superior load-bearing capacity. However, to achieve this function, these vehicles generally possess physical characteristics such as long wheelbases and high centers of gravity. While these characteristics improve transportation efficiency, they also present significant challenges to intelligent driving path planning. Specifically, a long wheelbase leads to a significant inner wheel difference when the vehicle changes lanes or turns. If the planned path does not take this factor into account, it can easily cause the middle and rear of the vehicle to scrape and collide with vehicles in adjacent lanes or the road edge. On the other hand, a high center of gravity severely reduces the vehicle's lateral stability. When cornering at high speeds or performing emergency obstacle avoidance maneuvers, the risk of rollover increases significantly, posing a great threat to driving safety.

[0003] Currently, most traditional local path planning methods are designed for ordinary passenger vehicles, and their optimization objectives often focus on passenger comfort or the shortest path distance, failing to fully consider the aforementioned physical constraints unique to heavy commercial vehicles. For example, patent CN113804207A proposes a path planning method based on the Frenet coordinate system entitled "Vehicle Path Planning Method, System, Device and Storage Medium." While this method can achieve path search, its search strategy fails to establish an adaptive association with the dynamic information of obstacles (such as distance and speed) and completely ignores inner wheel difference compensation and rollover risk constraints. Similarly, patent CN113819917A (Autonomous Driving Path Planning Method, Device, Equipment and Storage Medium) uses a state grid algorithm for path planning, but its sampling point generation method is fixed and random, lacking targeting for high-risk areas, and similarly fails to incorporate the vehicle's long wheelbase and high center of gravity characteristics as core safety factors into the planning model.

[0004] Therefore, there is a significant flaw in the existing technology: the lack of a path planning method specifically designed for the long wheelbase and high center of gravity characteristics of heavy commercial vehicles. As a result, the planned path may be smooth and feasible in theory, but in actual application, it may harbor the dual safety hazards of scratches and rollovers. Summary of the Invention

[0005] To address the aforementioned shortcomings of existing technologies, this invention provides a method, system, terminal, and storage medium for path planning of heavy-duty vehicles with long wheelbases.

[0006] In a first aspect, the present invention provides a path planning method for heavy-duty vehicles with long wheelbases, comprising: S1. Environmental perception and fusion modeling based on multi-source sensor data and high-precision maps; S2. Calculate the inner wheel difference compensation amount based on the vehicle wheelbase and current steering angle, and compensate for the lateral offset in the Frenet coordinate system; S3. Based on the compensated lateral offset, a number of smooth candidate paths that satisfy the start and end point positions, heading and curvature boundary conditions are generated using a fifth-order polynomial curve. S4. Calculate the rollover risk index based on vehicle dynamics parameters and real-time path curvature, and use the rollover risk index as a dynamic constraint on path curvature to select a subset of candidate paths that meet the rollover safety constraints. S5. Perform collision detection on each path in the candidate path subset, and select the path with no collision and the lowest rollover risk index as the optimal path output.

[0007] Further improvements to this technical solution include step S1, which includes: S11. Acquire three-dimensional point cloud data of the surrounding environment through lidar, detect the distance and speed information of obstacles through millimeter-wave radar, capture traffic signals and road signs through high-definition cameras, and detect nearby obstacles through ultrasonic sensors to form a multi-source heterogeneous raw dataset. S12. Denoise and filter preprocessing are performed on the multi-source heterogeneous original dataset. The Kalman filter algorithm is used to perform spatiotemporal alignment and data association on the preprocessed data to obtain the fused obstacle state estimation results and road environment information. S13. Match and fuse the fused obstacle state estimation results and road environment information with the road topology and lane geometry information provided by the map to construct a rasterized or topological environment model containing obstacle location, road boundary and traffic semantic information.

[0008] Further improvements to this technical solution include step S2, which includes: S21. Obtain the vehicle's wheelbase parameter L and current steering angle δ, and calculate the inner wheel difference compensation amount Δd based on the vehicle's steering geometry to correct the lateral position deviation caused by the difference in the inner wheel trajectory during the steering process of long-wheelbase heavy vehicles; wherein, the calculation formula for the inner wheel difference compensation amount Δd is: ; in, This indicates the change in heading angle of the end point of the path relative to the starting point; S22. In the Frenet coordinate system, the inner wheel difference compensation amount Δd is superimposed on the original lateral offset D determined according to the road boundary in the environmental model to generate the compensated lateral offset target amount. ; Target amount of lateral offset after compensation The calculation formula is: ; S23, with the compensated lateral offset target amount As the endpoint constraint, a fifth-order polynomial curve is used to generate candidate paths.

[0009] Further improvements to this technical solution include step S3, which includes: S31. In the Frenet coordinate system, define the longitudinal coordinate s and the lateral coordinate d of the path, where s is the distance traveled along the road reference line and d is the lateral offset distance perpendicular to the reference line; simultaneously determine the longitudinal endpoint coordinate of the path. , which is the total length of the path from the starting point to the end point along the reference line, calculated from the current vehicle position and the target driving position; S32. Based on the vertical coordinate s and the horizontal coordinate d, establish a fifth-order polynomial path model, the expression of which is: ; in, The coefficients of the fifth-degree polynomial; This represents the horizontal coordinate value corresponding to the vertical coordinate s; S33. Set the start and end boundary conditions for the path. The start boundary condition is: the position of the start point. Heading angle curvature The endpoint boundary condition is: the location of the endpoint. Target heading angle Target curvature Substitute the above six boundary conditions into the fifth-degree polynomial expression in step S32 to construct a system of linear equations and solve for the coefficients. By adjusting the starting heading angle Final heading angle Given the range of values, repeat the coefficient calculation process to generate at least three smooth candidate paths that satisfy the start and end point positions, headings, and curvature boundary conditions.

[0010] Further improvements to this technical solution include step S4, which includes: S41. Calculate the rollover risk threshold based on the vehicle center of gravity height and wheel track parameters in vehicle dynamics. Side rollover risk threshold The calculation formula is: ; in, It is the acceleration due to gravity; The height of the vehicle's center of gravity; The track width of a vehicle is the lateral distance between the center points of the left and right wheels that touch down. S42. For each candidate path generated in step S3, calculate the curvature of each point on the path based on its fifth-degree polynomial expression. curvature The calculation formula is: ; in, Lateral displacement For longitudinal displacement The first derivative of represents the change in the heading angle of the path; Lateral displacement For longitudinal displacement The second derivative; S43. Based on the vehicle's current speed and the curvature of the path, dynamically calculate the real-time rollover risk index for each candidate path. Side rollover risk index The calculation formula is: ; in, This represents the vehicle's current speed. S44. Calculate the maximum real-time rollover risk index for each candidate path. with rollover risk threshold Compare and filter out all that meet the requirements. Candidate paths are selected based on the conditions, forming a subset of candidate paths that satisfy the rollover safety constraints.

[0011] Further improvements to this technical solution include step S5, which includes: S51. Based on the environment model constructed in step S1, establish a vehicle geometric model represented by a vehicle outline polygon and an obstacle geometric model represented by a minimum circumcircle or polygon; and calculate the minimum safe distance between the vehicle and the obstacle. Minimum safe distance The calculation formula is: ; in, This represents the total response time of the path planning and control system. The maximum safe deceleration for the vehicle; S52. For each path in the subset of candidate paths that meet the rollover safety constraints selected in step S4, based on the vehicle geometry model established in step S51, calculate the minimum distance between the geometric envelope of the vehicle and the geometric model of the obstacle when the vehicle travels along the path. and at the preset safe distance Compare; if the condition is met for all points on the path. If so, the path is determined to be a collision-free path; S53. For all paths determined to be collision-free, construct a comprehensive evaluation function J for quantitative evaluation, and select the path with the best evaluation result as the final output path. Further improvements to this technical solution include the following: the expression for the comprehensive evaluation function J is: ; in, The square integral of the path curvature is used to evaluate path smoothness and comfort. The predicted route travel time; The weighting coefficients for each evaluation index are used; finally, the path that minimizes the value of the comprehensive evaluation function J is selected as the optimal path output.

[0012] Secondly, the present invention provides a path planning system for heavy-duty vehicles with long wheelbases, comprising: The environmental perception module is used for environmental perception and fusion modeling based on multi-source sensor data and high-precision maps. The inner wheel difference compensation module is used to calculate the inner wheel difference compensation amount based on the vehicle wheelbase and the current steering angle, and to compensate for the lateral offset in the Frenet coordinate system. The path planning module is used to generate multiple smooth candidate paths that satisfy the origin and destination positions, headings and curvature boundary conditions based on the compensated lateral offset using a fifth-order polynomial curve. The rollover risk assessment module is used to calculate the rollover risk index based on vehicle dynamics parameters and real-time path curvature, and to use the rollover risk index as a dynamic constraint on path curvature to select a subset of candidate paths that meet the rollover safety constraints. The optimal path decision module is used to perform collision detection on each path in the candidate path subset and select the path with no collision and the lowest rollover risk index as the optimal path output.

[0013] Thirdly, the present invention provides a terminal, comprising: Processor, memory, among which, This memory is used to store computer programs. The processor is used to retrieve and run the computer program from memory, causing the terminal to perform the terminal method described above.

[0014] Fourthly, the present invention provides a computer storage medium storing instructions that, when executed on a computer, cause the computer to perform the methods described in the above aspects.

[0015] The beneficial effects of this invention are as follows: This invention uses the inner wheel difference compensation algorithm designed in step S2 to dynamically correct the lateral offset, effectively avoiding the risk of scraping caused by the deviation of the inner wheel trajectory when long wheelbase vehicles change lanes or turn. At the same time, the rollover risk index is introduced as a dynamic constraint on the path curvature, which limits the possibility of rollover of high center of gravity vehicles when cornering at high speed or avoiding obstacles in emergency situations from a dynamic perspective. The dual protection significantly reduces the accident rate.

[0016] In this invention, step S3 uses a fifth-order polynomial curve for path fitting, which can accurately ensure the continuity of the first derivative (heading angle) and second derivative (curvature) of the path. This means that when the vehicle follows the path, the changes in steering wheel angle and lateral acceleration are continuous and smooth, avoiding the curvature abrupt changes that may be caused by traditional segmented paths or low-order curves. This significantly improves driving comfort, reduces impact wear on the chassis and actuators, and is beneficial to the stability of vehicle control.

[0017] This invention introduces a rollover risk index (RTI) dynamic constraint mechanism in step S4, which quantifies vehicle dynamic parameters (center of gravity height, wheelbase) and real-time status (vehicle speed, path curvature) into calculable safety thresholds. This proactively filters out high-risk paths with excessive curvature that may lead to rollover during the path generation stage, greatly reducing the probability of rollover for high-center-of-gravity vehicles during high-speed cornering or emergency obstacle avoidance.

[0018] Step S5 of this invention does not simply aim for "no collisions" as the final goal. Instead, it constructs a comprehensive evaluation function J, incorporating multiple performance indicators such as maximum rollover risk, the square integral of path curvature (smoothness), and travel time (efficiency) into a unified framework for weighted evaluation. This multi-objective optimization strategy ensures that the system ultimately selects a high-quality path that achieves the best balance between comfort and efficiency while maintaining a safety baseline. This overcomes the limitations of existing technologies that often focus on only a single indicator, demonstrating a higher level of intelligent decision-making capability. Attached Figure Description

[0019] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0020] Figure 1 This is a schematic flowchart illustrating a method according to an embodiment of the present invention.

[0021] Figure 2 This is a schematic block diagram of a system according to an embodiment of the present invention.

[0022] Figure 3This is a schematic diagram of the structure of a terminal provided in an embodiment of the present invention. Detailed Implementation

[0023] To make the objectives, features, and advantages of this invention more apparent and understandable, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings of the specific embodiments. Obviously, the embodiments described below are only some embodiments of this invention, and not all embodiments. Based on the embodiments in this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0024] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. The terminology used herein in the description of the invention is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention.

[0025] Figure 1 This is a schematic flowchart illustrating a path planning method for heavy-duty vehicles with long wheelbases provided by the present invention. Figure 1 The implementing entity can be a path planning system for heavy-duty vehicles with long wheelbases. Depending on different requirements, the order of steps in this flowchart can be changed, and some steps can be omitted.

[0026] like Figure 1 As shown, the method includes: S1. Environmental perception and fusion modeling based on multi-source sensor data and high-precision maps; S2. Calculate the inner wheel difference compensation amount based on the vehicle wheelbase and current steering angle, and compensate for the lateral offset in the Frenet coordinate system; S3. Based on the compensated lateral offset, a number of smooth candidate paths that satisfy the start and end point positions, heading and curvature boundary conditions are generated using a fifth-order polynomial curve. S4. Calculate the rollover risk index based on vehicle dynamics parameters and real-time path curvature, and use the rollover risk index as a dynamic constraint on path curvature to select a subset of candidate paths that meet the rollover safety constraints. S5. Perform collision detection on each path in the candidate path subset, and select the path with no collision and the lowest rollover risk index as the optimal path output.

[0027] To facilitate understanding of the present invention, the following description further illustrates the path planning method for long-wheelbase heavy-duty vehicles provided by the present invention, based on the principle of the path planning method for long-wheelbase heavy-duty vehicles and in conjunction with the path planning process for long-wheelbase heavy-duty vehicles in the embodiments.

[0028] First, step S1 includes: S11. Acquire three-dimensional point cloud data of the surrounding environment through lidar, detect the distance and speed information of obstacles through millimeter-wave radar, capture traffic signals and road signs through high-definition cameras, and detect nearby obstacles through ultrasonic sensors to form a multi-source heterogeneous raw dataset. S12. Denoise and filter preprocessing are performed on the multi-source heterogeneous original dataset. The Kalman filter algorithm is used to perform spatiotemporal alignment and data association on the preprocessed data to obtain the fused obstacle state estimation results and road environment information. S13. Match and fuse the fused obstacle state estimation results and road environment information with the road topology and lane geometry information provided by the map to construct a rasterized or topological environment model containing obstacle location, road boundary and traffic semantic information.

[0029] Specifically, multi-source sensor data acquisition: Raw environmental data is collected in parallel using lidar, millimeter-wave radar, high-definition cameras, and ultrasonic sensors deployed on heavy-duty vehicles.

[0030] LiDAR: A 128-line mechanical LiDAR is selected, with a horizontal field of view of 360° and a vertical field of view of [missing information]. The point cloud output frequency is 10Hz, ranging from 20° to +15°. It is mounted on the center of the vehicle's roof, with its coordinate system origin defined as the projection of the vehicle's rear axle center onto the ground. The acquired raw point cloud data is stored in .pcd format, with each point containing three-dimensional coordinates. Including reflection intensity, lidar is used to accurately identify the outline of obstacles.

[0031] Millimeter-wave radar: Utilizing a 77GHz forward-facing long-range radar, with a detection range of 0.5m to 250m, a range resolution of 0.5m, a velocity resolution of 0.1m / s, and a refresh rate of 20Hz. Mounted in the center of the vehicle's front bumper, it outputs a target list, with each target including its relative distance. Azimuth Relative velocity In addition to target width estimation, millimeter-wave radar is used to detect the relative distance and radial velocity of obstacles, effectively compensating for the limitations of visual sensors in adverse weather conditions.

[0032] High-definition camera: A 4K resolution (3840×2160) camera with a frame rate of 30fps and HDR functionality is used. Mounted on the top inner side of the windshield, it receives vehicle yaw rate signals via a CAN bus to achieve image stabilization. The acquired image data is processed in real-time using the YOLOv8 algorithm to output traffic light status, lane line types, and obstacle categories. The high-definition camera is used to capture semantic information such as lane lines and traffic signs.

[0033] Ultrasonic sensors: 12 ultrasonic probes are used, installed in the front, rear, left, and right directions of the vehicle, with a detection range of 0.2m to 5m and an update frequency of 50Hz. Used for low-speed, close-range obstacle detection, outputting binary obstacle markers and approximate distances to prevent low-speed collisions.

[0034] All sensor data are synchronized with timestamps via the CAN bus to form a multi-source heterogeneous raw dataset, ensuring spatiotemporal consistency of the data.

[0035] Time synchronization mechanism: Utilizing the PTP (Precision Time Protocol), and based on the PPS signal of the LiDAR, the sampling times of the millimeter-wave radar, camera, and ultrasonic sensor are unified to the GPS timestamp, achieving a time synchronization accuracy better than 1ms. Each sensor's data is timestamped immediately after acquisition. .

[0036] In step S12, the Kalman filter algorithm is used to achieve data fusion, specifically including the following interrelated formulas: The state prediction equation is: ; in, This represents the prior state estimation vector at time k (including obstacle position and velocity). Represents the state transition matrix; Let represent the posterior state estimate vector at time k-1; Represents the control input matrix; This represents the control input vector (e.g., vehicle acceleration). The covariance prediction equation is: ; in, Let represent the prior estimate covariance matrix at time k; Let represent the posterior estimated covariance matrix at time k-1; Represents the process noise covariance matrix; The Kalman gain equation is: ; in, This represents the Kalman gain matrix at time k, used to balance the predicted and observed values; Represents the observation matrix; Represents the observation noise covariance matrix; The state update equation is: ; in, Let represent the posterior state estimation vector at time k; Let represent the posterior state estimation vector at time k; The covariance update equation is: ; in, Let represent the posterior estimated covariance matrix at time k; Represents the identity matrix.

[0037] Through iterative calculation using the above formula, the final output is the fused obstacle state estimation results (such as position and velocity covariance) and road environment information, effectively improving data accuracy and robustness.

[0038] In step S13, the transformation formula for converting the fused sensor data to the map coordinate system is as follows: ; in, Indicates the position and heading angle in the map coordinate system; Indicates the position and heading angle in the sensor coordinate system; This indicates the vehicle's position in the map coordinate system; This represents the vehicle's heading angle in the map coordinate system. After transformation, road topology information (e.g., lane width, curvature) and dynamic obstacle locations (e.g., trucks, cars) are semantically fused to generate a rasterized environment model (0.1m × 0.1m resolution) or a topological model (with lanes as nodes). This model is updated in real time and includes obstacle probability distribution, road boundaries, and semantic labels for traffic signs, providing structured environmental priors for path planning.

[0039] Rasterized environment model construction: A local raster map of 200m × 200m is constructed centered on the vehicle's current location, with a resolution of 0.5m. Each raster stores the following information: Occupancy probability Updated by log-probability fusion after projection from LiDAR point cloud: ; in, This represents the prior probability that a single grid cell is occupied by an obstacle before any sensor measurement data is fused. ; To represent the likelihood probability that the grid cell containing the endpoint of the measurement beam is indeed occupied by an obstacle when the sensor detects an echo at the endpoint of its measurement beam, Weight Adjust dynamically based on sensor confidence level.

[0040] Obstacle category (dynamic / static / unknown): determined by the output state of the fused Kalman filter; dynamic obstacles have an additional velocity vector. .

[0041] Road boundaries: The 3D point series of lane lines are extracted from the high-precision map, rasterized after spline interpolation, and marked as impassable areas.

[0042] Traffic semantic information: The traffic light status (red / yellow / green) identified by the camera is mapped to the corresponding grid, serving as a hard constraint for subsequent path planning.

[0043] Topological environment model construction: In highway scenarios, a topological map can be built in parallel. Lane centerlines are used as nodes, and lane connections are used as edges. Each node stores attributes such as lane width, speed limit, and curvature. Dynamic obstacles are attached to the nearest lane node, enabling fast lookup.

[0044] Secondly, step S2 includes: S21. Obtain the vehicle's wheelbase parameter L and current steering angle δ, and calculate the inner wheel difference compensation amount Δd based on the vehicle's steering geometry to correct the lateral position deviation caused by the difference in the inner wheel trajectory during the steering process of long-wheelbase heavy vehicles; wherein, the calculation formula for the inner wheel difference compensation amount Δd is: ; in, This indicates the change in heading angle of the end point of the path relative to the starting point; S22. In the Frenet coordinate system, the inner wheel difference compensation amount Δd is superimposed on the original lateral offset D determined according to the road boundary in the environmental model to generate the compensated lateral offset target amount. ; Target amount of lateral offset after compensation The calculation formula is: ; S23, with the compensated lateral offset target amount As a constraint on the endpoint, a quintic polynomial curve is used to generate candidate paths to ensure that the path avoids the risk of scraping from vehicles in adjacent lanes or the road edge when changing lanes or turning.

[0045] Wheelbase parameter L acquisition: The wheelbase L of a heavy-duty vehicle is the straight-line distance between the center of the front axle and the center of the rear axle (unit: m). This parameter is an inherent attribute of the vehicle from the factory and is read from the vehicle's ECU (electronic control unit) through the onboard T-Box (remote information processing terminal) at a reading frequency of 1Hz (since the wheelbase is a fixed value, high-frequency acquisition is not required). After reading, it is verified with the wheelbase parameter registered on the vehicle registration certificate. If the deviation exceeds ±0.05m, it is judged as data abnormal, and the value registered on the vehicle registration certificate is used as the final L to ensure the accuracy of the parameter.

[0046] Current steering angle δ acquisition: The real-time steering angle is acquired by the steering angle sensor (model: Bosch EPS steering angle sensor) mounted on the vehicle steering system. The sensor has a sampling frequency of 100Hz and a measurement range of -45° to +45° (corresponding to the steering limit of heavy vehicles). The output signal is a CAN bus message. The acquired steering angle data is low-pass filtered (cutoff frequency 5Hz) to remove high-frequency noise caused by mechanical vibration of the steering system, and the smoothed current steering angle δ is obtained.

[0047] Obtaining the change in heading angle θ: Based on the gridded environment model constructed in step S1 and the vehicle positioning system, determine the heading angle of the path starting point (the current position of the vehicle). Heading angle relative to the end of the path (the position after the target lane change / turn) The change in heading angle θ is the difference between the two, i.e. (Take the absolute value, as inner wheel difference compensation is only related to steering amplitude and not steering direction). During calculation, ensure that θ is within the range of 0 to π / 2 rad. Then according to Value selection (to avoid excessive compensation in extreme steering scenarios).

[0048] Frenet coordinate system construction: The Frenet coordinate system is established with the road reference line (such as the lane center line) in the environment model of step S1 as the vertical axis s (the direction of travel along the reference line is positive), and the horizontal axis d (the direction of lateral offset) perpendicular to the reference line and pointing to the left of the lane. The origin of the coordinate system is the projection point of the path starting point on the reference line. The unit of the vertical axis s is m, and the unit of the horizontal axis d is m.

[0049] The initial lateral offset D is determined as follows: D is the lateral offset (in meters) at the end of the path without inner wheel difference compensation, determined based on the road boundary information (e.g., lane width, obstacle position) provided by the environmental model in step S1. For example, if a vehicle needs to change lanes from the current lane (3.75m wide) to the left lane, and the lateral distance between the center line of the current lane and the center line of the left lane is 3.75m, then the initial lateral offset D = 3.75m (target is the center line of the left lane). If the lateral distance between the edge of the left lane and the center line of the current lane is 1.875m (half the lane width), and there are no obstacles within 1m of the edge of the left lane, then D can be 1.5m (with a safety margin of 0.375m). The value of D must meet the following requirements. ( (For the target lane width), ensure that the original offset is within the safe range of the lane.

[0050] The inner wheel difference compensation amount Δd calculated in step S21 is superimposed on the original lateral offset amount D to generate the compensated lateral offset target amount. The calculation formula is: The compensation direction is determined collaboratively based on the steering direction and the offset direction. If the vehicle turns left And it needs to be shifted to the left. or turn right And it needs to be shifted to the right ( ),but Take a positive value (same direction compensation), that is ; If the vehicle turns left And it needs to be shifted to the right. or turn right And it needs to be shifted to the left ( ),but Take the negative value (reverse compensation), that is ; Verification is required after compensation. Is it within the safe range of the target lane? If (Exceeding the right-hand safety boundary of the lane), then adjust to ;like (Exceeding the left safety boundary of the lane), then adjust to This ensures that the offset target amount after compensation conforms to the road boundary constraints.

[0051] This invention introduces a dynamic compensation model based on steering geometry and path curvature, and for the first time explicitly models the rear axle trajectory of heavy vehicles at the path planning level. This fundamentally solves the problem of "front wheel safety, rear wheel collision" caused by neglecting the inner wheel difference in traditional methods. The formula introduces the change in heading angle θ, so that the compensation amount depends not only on the steering angle, but also on the overall curvature of the path, avoiding over-compensation on paths with small curvature and improving the rationality of the path.

[0052] Next, step S3 includes: S31. In the Frenet coordinate system, define the longitudinal coordinate s and the lateral coordinate d of the path, where s is the distance traveled along the road reference line and d is the lateral offset distance perpendicular to the reference line; simultaneously determine the longitudinal endpoint coordinate of the path. , which is the total length of the path from the starting point to the end point along the reference line, calculated from the current vehicle position and the target driving position; S32. Based on the vertical coordinate s and the horizontal coordinate d, establish a fifth-order polynomial path model, the expression of which is: ; in, The coefficients of the fifth-degree polynomial; This represents the horizontal coordinate value corresponding to the vertical coordinate s; S33. Set the start and end boundary conditions for the path. The start boundary condition is: the position of the start point. Heading angle curvature The endpoint boundary condition is: the location of the endpoint. Target heading angle Target curvature Substitute the above six boundary conditions into the fifth-degree polynomial expression in step S32 to construct a system of linear equations and solve for the coefficients. By adjusting the starting heading angle Final heading angle Given the range of values, repeat the coefficient calculation process to generate at least three smooth candidate paths that satisfy the start and end point positions, headings, and curvature boundary conditions.

[0053] The first derivative (heading) and second derivative (curvature) of the fifth-order polynomial path are continuous, which avoids the curvature abrupt change problem of the traditional cubic polynomial path; the vehicle steering wheel does not need to be adjusted frequently, effectively suppressing the body roll (the core risk of high center of gravity vehicles), protecting the stability of the cargo, reducing the driver's operating intensity, and avoiding the risk of rollover caused by curvature abrupt change.

[0054] The endpoint position constraint directly reuses step S2. This ensures that the path avoids the risk of inner wheel difference scraping from the source; the starting and ending headings and curvature constraints are consistent with the driving characteristics of heavy vehicles (such as the starting / ending curvature being 0), to avoid the path being out of sync with the actual state of the vehicle; the Frenet coordinate system is consistent with step S2, and no additional coordinate transformation is required.

[0055] By adjusting At least three candidate paths are generated, covering different trajectories such as "left-leaning, center-leaning, and right-leaning", which can be adapted to sub-scenarios such as "slow vehicle on the left side of the right lane", "no vehicle on the right lane", and "guardrail on the right side of the right lane". Subsequent steps S4 (rollover risk screening) and S5 (collision detection) can select the global optimal solution from multiple paths, avoiding the "local optimal trap" of a single path, and adapting to complex traffic scenarios such as highways and urban expressways.

[0056] Additionally, step S4 includes: S41. Calculate the rollover risk threshold based on the vehicle center of gravity height and wheel track parameters in vehicle dynamics. Side rollover risk threshold The calculation formula is: ; in, It is the acceleration due to gravity; The height of the vehicle's center of gravity; The track width of a vehicle is the lateral distance between the center points of the left and right wheels that touch down. S42. For each candidate path generated in step S3, calculate the curvature of each point on the path based on its fifth-degree polynomial expression. curvature The calculation formula is: ; in, Lateral displacement For longitudinal displacement The first derivative of represents the change in the heading angle of the path; Lateral displacement For longitudinal displacement The second derivative; S43. Based on the vehicle's current speed and the curvature of the path, dynamically calculate the real-time rollover risk index for each candidate path. Side rollover risk index The calculation formula is: ; in, This represents the vehicle's current speed. S44. Calculate the maximum real-time rollover risk index for each candidate path. with rollover risk threshold Compare and filter out all that meet the requirements. Candidate paths are selected based on the conditions, forming a subset of candidate paths that satisfy the rollover safety constraints.

[0057] By side rollover risk threshold With real-time risk index The quantitative calculation transforms the qualitative problem of heavy-duty vehicles' "high center of gravity and easy rollover" into a quantitative judgment, avoiding the shortcomings of traditional path planning that "only considers smoothness and ignores rollover risk." For example, path factors... Those exceeding the threshold are eliminated, thus avoiding the risk of rollover during high-speed cornering from the source and adapting to the dynamic characteristics of heavy-duty vehicles.

[0058] The rollover risk threshold is dynamically adjusted according to the center of gravity height and wheelbase (e.g., when load changes cause h to increase). (Reduced, with stricter constraints) The risk index updates in real time with speed and curvature (e.g., as vehicle speed increases). Increase the risk warning in a timely manner; the screening rules ensure safety (removing paths that exceed the threshold) while also taking feasibility into account (selecting the lowest risk path in extreme scenarios), avoiding the problem of no available paths caused by a "one-size-fits-all" approach, and adapting to complex traffic scenarios.

[0059] The input reuses the candidate path multinomial model from step S3 (without rebuilding the path), and the output provides "risk priority" annotations for collision detection in step S5 (e.g., prioritizing detection of low-risk collisions). The path is defined, reducing the amount of subsequent calculations; at the same time, the high-risk section marking can help the vehicle reduce speed in advance (such as the speed reduction prompt triggered in step S44), realizing the linkage of "path planning-risk warning-control execution" and improving the overall safety of the intelligent driving system.

[0060] Finally, step S5 includes: S51. Based on the environment model constructed in step S1, establish a vehicle geometric model represented by a vehicle outline polygon and an obstacle geometric model represented by a minimum circumcircle or polygon; and calculate the minimum safe distance between the vehicle and the obstacle. Minimum safe distance The calculation formula is: ; in, This represents the total response time of the path planning and control system. The maximum safe deceleration for the vehicle; S52. For each path in the subset of candidate paths that meet the rollover safety constraints selected in step S4, based on the vehicle geometry model established in step S51, calculate the minimum distance between the geometric envelope of the vehicle and the geometric model of the obstacle when the vehicle travels along the path. and at the preset safe distance Compare; if the condition is met for all points on the path. If so, the path is determined to be a collision-free path; S53. For all paths determined to be collision-free, construct a comprehensive evaluation function J for quantitative evaluation, and select the path with the best evaluation result as the final output path. Furthermore, the expression for the comprehensive evaluation function J is: ; in, The square integral of the path curvature is used to evaluate path smoothness and comfort. The predicted route travel time; The weighting coefficients for each evaluation indicator are set according to the priority of driving safety, comfort, and efficiency; finally, the path that minimizes the comprehensive evaluation function J value is selected as the optimal path output.

[0061] The evaluation function integrates smoothness (curvature integral), efficiency (driving time), and safety (rollover risk), and flexibly adjusts the priority through weighting coefficients: improving safety-first scenarios (e.g., highways). Improvement in efficiency-priority scenarios (such as urban expressways) This ensures that the output path is "globally optimal" rather than "optimal for a single metric." For example, a path may have the best smoothness, but it may have a high risk of tipping over and would be eliminated in a safety-first scenario, thus avoiding safety hazards caused by selecting a path based on a single metric.

[0062] The input terminal reuses the environmental model (obstacle information) from step S1 and the rollover risk index from step S4. This eliminates the need for repeated data collection, reducing computational load; the optimal path at the output end is directly used for vehicle control execution (e.g., steering, braking), and the smoothness index of the evaluation function can assist the control layer in optimizing the rate of change of steering angle, achieving "planning-control" linkage and improving driving stability.

[0063] In some embodiments, the long-wheelbase heavy-duty vehicle routing system 200 may include multiple functional modules composed of computer program segments. The computer programs for each program segment in the long-wheelbase heavy-duty vehicle routing system 200 may be stored in the memory of a computer device and executed by at least one processor to perform (see details). Figure 1 (Description) Functionality for path planning of heavy-duty vehicles with long wheelbases.

[0064] In this embodiment, the long-wheelbase heavy-duty vehicle path planning system 200 can be divided into multiple functional modules according to its functions, such as... Figure 2 As shown. The functional modules may include: an environmental perception module 210, an inner wheel difference compensation module 220, a path planning module 230, a rollover risk assessment module 240, and an optimal path decision module 250. The module referred to in this invention is a series of computer program segments that can be executed by at least one processor and perform a fixed function, and are stored in memory. In this embodiment, the functions of each module will be described in detail in subsequent embodiments.

[0065] Specifically, the environmental perception module 210 is used for environmental perception and fusion modeling based on multi-source sensor data and high-precision maps; the inner wheel difference compensation module 220 is used to calculate the inner wheel difference compensation amount based on the vehicle wheelbase and current steering angle, and to compensate for the lateral offset in the Frenet coordinate system; the path planning module 230 is used to generate multiple smooth candidate paths that meet the boundary conditions of the start and end point positions, headings and curvatures based on the compensated lateral offset using a fifth-order polynomial curve; the rollover risk assessment module 240 is used to calculate the rollover risk index based on vehicle dynamic parameters and real-time path curvature, and to use the rollover risk index as a dynamic constraint condition for the path curvature to select a subset of candidate paths that meet the rollover safety constraints; the optimal path decision module 250 is used to perform collision detection on each path in the candidate path subset, and to select the path with no collision and the smallest rollover risk index as the optimal path output.

[0066] Figure 3 This is a schematic diagram of a terminal 300 provided in an embodiment of the present invention. The terminal 300 can be used to execute the heavy-duty vehicle path planning method for long wheelbase provided in the embodiment of the present invention.

[0067] The terminal 300 may include a processor 310, a memory 320, and a communication module 330. These components communicate via one or more buses. Those skilled in the art will understand that the server structure shown in the figure does not constitute a limitation of the present invention. It may be a bus topology or a star topology, and may include more or fewer components than shown, or combine certain components, or have different component arrangements.

[0068] The memory 320 can be used to store the execution instructions of the processor 310. The memory 320 can be implemented by any type of volatile or non-volatile memory terminal or a combination thereof, such as static random access memory (SRAM), electrically erasable programmable read-only memory (EEPROM), erasable programmable read-only memory (EPROM), programmable read-only memory (PROM), read-only memory (ROM), magnetic storage, flash memory, magnetic disk, or optical disk. When the execution instructions in the memory 320 are executed by the processor 310, the terminal 300 is able to perform some or all of the steps in the above method embodiments.

[0069] The processor 310 serves as the control center of the storage terminal, connecting various parts of the electronic terminal via various interfaces and lines. It executes software programs and / or modules stored in the memory 320, and calls data stored in the memory to perform various functions of the electronic terminal and / or process data. The processor can be composed of integrated circuits (ICs), such as a single packaged IC or multiple packaged ICs with the same or different functions connected together. For example, the processor 310 may consist only of a central processing unit (CPU). In this embodiment of the invention, the CPU may have a single processing core or include multiple processing cores.

[0070] The communication module 330 is used to establish a communication channel, enabling the storage terminal to communicate with other terminals. It receives user data sent by other terminals or sends user data to other terminals.

[0071] The present invention also provides a computer storage medium, wherein the computer storage medium may store a program, which, when executed, may include some or all of the steps provided in the embodiments of the present invention. The storage medium may be a magnetic disk, an optical disk, read-only memory (ROM), or random access memory (RAM), etc.

[0072] Those skilled in the art will clearly understand that the techniques in the embodiments of the present invention can be implemented using software plus necessary general-purpose hardware platforms. Based on this understanding, the technical solutions in the embodiments of the present invention, or the parts that contribute to the prior art, can be embodied in the form of a software product. This computer software product is stored in a storage medium such as a USB flash drive, mobile hard drive, read-only memory (ROM), random access memory (RAM), magnetic disk, or optical disk, or other media capable of storing program code. It includes several instructions to cause a computer terminal (which may be a personal computer, server, or a second terminal, network terminal, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention.

[0073] The same or similar parts between the various embodiments in this specification can be referred to mutually. In particular, the terminal embodiments are basically similar to the method embodiments, so the description is relatively simple, and the relevant parts can be referred to the description in the method embodiments.

[0074] In the embodiments provided by this invention, it should be understood that the disclosed systems and methods can be implemented in other ways. For example, the system embodiments described above are merely illustrative. For instance, the division of modules is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple modules or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between systems or modules may be electrical, mechanical, or other forms.

[0075] The modules described as separate components may or may not be physically separate. The components shown as modules may or may not be physical modules; that is, they may be located in one place or distributed across multiple network modules. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs.

[0076] In addition, the functional modules in the various embodiments of the present invention can be integrated into one processing module, or each module can exist physically separately, or two or more modules can be integrated into one module.

[0077] Although the present invention has been described in detail with reference to the accompanying drawings and preferred embodiments, the present invention is not limited thereto. Various equivalent modifications or substitutions can be made to the embodiments of the present invention by those skilled in the art without departing from the spirit and essence of the invention, and such modifications or substitutions should all be within the scope of the present invention. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should also be covered within the protection scope of the present invention.

Claims

1. A path planning method for heavy-duty vehicles with long wheelbases, characterized in that, include: S1. Environmental perception and fusion modeling based on multi-source sensor data and high-precision maps; S2. Calculate the inner wheel difference compensation amount based on the vehicle wheelbase and current steering angle, and compensate for the lateral offset in the Frenet coordinate system; S3. Based on the compensated lateral offset, a number of smooth candidate paths that satisfy the start and end point positions, heading and curvature boundary conditions are generated using a fifth-order polynomial curve. S4. Calculate the rollover risk index based on vehicle dynamics parameters and real-time path curvature, and use the rollover risk index as a dynamic constraint on path curvature to select a subset of candidate paths that meet the rollover safety constraints. S5. Perform collision detection on each path in the candidate path subset, and select the path with no collision and the lowest rollover risk index as the optimal path output.

2. The path planning method for heavy-duty vehicles with long wheelbases according to claim 1, characterized in that, Step S1 includes: S11. Acquire three-dimensional point cloud data of the surrounding environment through lidar, detect the distance and speed information of obstacles through millimeter-wave radar, capture traffic signals and road signs through high-definition cameras, and detect nearby obstacles through ultrasonic sensors to form a multi-source heterogeneous raw dataset. S12. Denoise and filter preprocessing are performed on the multi-source heterogeneous original dataset. The Kalman filter algorithm is used to perform spatiotemporal alignment and data association on the preprocessed data to obtain the fused obstacle state estimation results and road environment information. S13. Match and fuse the fused obstacle state estimation results and road environment information with the road topology and lane geometry information provided by the map to construct a rasterized or topological environment model containing obstacle location, road boundary and traffic semantic information.

3. The path planning method for heavy-duty vehicles with long wheelbases according to claim 2, characterized in that, Step S2 includes: S21. Obtain the vehicle's wheelbase parameter L and current steering angle δ, and calculate the inner wheel difference compensation amount Δd based on the vehicle's steering geometry to correct the lateral position deviation caused by the difference in the inner wheel trajectory during the steering process of long-wheelbase heavy vehicles; wherein, the calculation formula for the inner wheel difference compensation amount Δd is: ； in, This indicates the change in heading angle of the end point of the path relative to the starting point; S22. In the Frenet coordinate system, the inner wheel difference compensation amount Δd is superimposed on the original lateral offset D determined according to the road boundary in the environmental model to generate the compensated lateral offset target amount. ; Target amount of lateral offset after compensation The calculation formula is: ； S23, with the compensated lateral offset target amount As the endpoint constraint, a fifth-order polynomial curve is used to generate candidate paths.

4. The path planning method for long-wheelbase heavy-duty vehicles according to claim 3, characterized in that, Step S3 includes: S31. In the Frenet coordinate system, define the longitudinal coordinate s and the lateral coordinate d of the path, where s is the distance traveled along the road reference line and d is the lateral offset distance perpendicular to the reference line; simultaneously determine the longitudinal endpoint coordinate of the path. , which is the total length of the path from the starting point to the end point along the reference line, calculated from the current vehicle position and the target driving position; S32. Based on the vertical coordinate s and the horizontal coordinate d, establish a fifth-order polynomial path model, the expression of which is: ； in, The coefficients of the fifth-degree polynomial; This represents the horizontal coordinate value corresponding to the vertical coordinate s; S33. Set the start and end boundary conditions for the path. The start boundary condition is: the position of the start point. Heading angle curvature The endpoint boundary condition is: the location of the endpoint. Target heading angle Target curvature Substitute the above six boundary conditions into the fifth-degree polynomial expression in step S32 to construct a system of linear equations and solve for the coefficients. By adjusting the starting heading angle Final heading angle Given the range of values, repeat the coefficient calculation process to generate at least three smooth candidate paths that satisfy the start and end point positions, headings, and curvature boundary conditions.

5. The path planning method for heavy-duty vehicles with long wheelbases according to claim 4, characterized in that, Step S4 includes: S41. Calculate the rollover risk threshold based on the vehicle center of gravity height and wheel track parameters in vehicle dynamics. Side rollover risk threshold The calculation formula is: ； in, It is the acceleration due to gravity; The height of the vehicle's center of gravity; The track width of a vehicle is the lateral distance between the center points of the left and right wheels that touch down. S42. For each candidate path generated in step S3, calculate the curvature of each point on the path based on its fifth-degree polynomial expression. curvature The calculation formula is: ； in, Lateral displacement For longitudinal displacement The first derivative of represents the change in the heading angle of the path; Lateral displacement For longitudinal displacement The second derivative; S43. Based on the vehicle's current speed and the curvature of the path, dynamically calculate the real-time rollover risk index for each candidate path. Side rollover risk index The calculation formula is: ； in, This represents the vehicle's current speed. S44. Calculate the maximum real-time rollover risk index for each candidate path. with rollover risk threshold Compare and filter out all that meet the requirements. Candidate paths are selected based on the conditions, forming a subset of candidate paths that satisfy the rollover safety constraints.

6. The path planning method for heavy-duty vehicles with long wheelbases according to claim 5, characterized in that, Step S5 includes: S51. Based on the environment model constructed in step S1, establish a vehicle geometric model represented by a vehicle outline polygon and an obstacle geometric model represented by a minimum circumcircle or polygon; and calculate the minimum safe distance between the vehicle and the obstacle. Minimum safe distance The calculation formula is: ； in, This represents the total response time of the path planning and control system. The maximum safe deceleration for the vehicle; S52. For each path in the subset of candidate paths that meet the rollover safety constraints selected in step S4, based on the vehicle geometry model established in step S51, calculate the minimum distance between the geometric envelope of the vehicle and the geometric model of the obstacle when the vehicle travels along the path. and at the preset safe distance Compare; if the condition is met for all points on the path. If so, the path is determined to be a collision-free path; S53. For all paths determined to be collision-free, construct a comprehensive evaluation function J for quantitative evaluation, and select the path with the best evaluation result as the final output path.

7. The path planning method for heavy-duty vehicles with long wheelbases according to claim 6, characterized in that, The expression for the comprehensive evaluation function J is: ； in, The square integral of the path curvature is used to evaluate path smoothness and comfort. The predicted route travel time; The weighting coefficients for each evaluation index are used; finally, the path that minimizes the value of the comprehensive evaluation function J is selected as the optimal path output.

8. A path planning system for heavy-duty vehicles with long wheelbases, characterized in that, include: The environmental perception module is used for environmental perception and fusion modeling based on multi-source sensor data and high-precision maps. The inner wheel difference compensation module is used to calculate the inner wheel difference compensation amount based on the vehicle wheelbase and the current steering angle, and to compensate for the lateral offset in the Frenet coordinate system. The path planning module is used to generate multiple smooth candidate paths that satisfy the origin and destination positions, headings and curvature boundary conditions based on the compensated lateral offset using a fifth-order polynomial curve. The rollover risk assessment module is used to calculate the rollover risk index based on vehicle dynamics parameters and real-time path curvature, and to use the rollover risk index as a dynamic constraint on path curvature to select a subset of candidate paths that meet the rollover safety constraints. The optimal path decision module is used to perform collision detection on each path in the candidate path subset and select the path with no collision and the lowest rollover risk index as the optimal path output.

9. A terminal, characterized in that, include: processor; Memory used to store the processor's execution instructions; The processor is configured to perform the method of any one of claims 1-7.

10. A computer-readable storage medium storing a computer program, characterized in that, When the program is executed by the processor, it implements the method as described in any one of claims 1-7.

Images