A trajectory planning method and system based on guide point smoothing and a storage medium

Abstract

The application discloses a kind of trajectory planning methods based on guide point smoothing, system and storage medium, it includes: obtaining last time guide point, current time reference line and vehicle running state information;Calculate the lateral deviation of vehicle relative to reference line, if reference line switching is detected, then the deviation is compensated based on the reference offset between reference line before and after switching;Trend accumulation processing is carried out to lateral deviation, and effective moving intention is determined, and lateral movement increment is calculated according to intention and real-time vehicle speed;Based on the increment, the last time guide point is iteratively updated, the guide point after smoothing in current time is obtained and target trajectory is generated;The application eliminates trajectory jump by reference line switching compensation, filters noise by trend accumulation, limits lateral change rate by vehicle speed adaptive increment, solves the problems of trajectory jitter, reference line switching jump and low safety under high-speed lane changing of commercial vehicle, improves trajectory smoothness and stability.

Description

Technical Field

[0001] This application relates to the field of autonomous driving technology, specifically to a trajectory planning method, system, and storage medium based on guide point smoothing. Background Technology

[0002] With the rapid development of autonomous driving technology, automatic lane changing functionality for commercial vehicles (such as heavy-duty trucks and logistics vans) has become a research hotspot. Compared to passenger cars, commercial vehicles have significant characteristics such as greater mass, higher center of gravity, and greater inertia. These physical characteristics make commercial vehicles extremely sensitive to the smoothness and continuity of their trajectories.

[0003] Existing trajectory planning methods typically employ a guide point mechanism to generate lane-change trajectories. However, the following problems have been observed in practical applications: (1) Existing methods mostly update the guide point directly based on single-frame sensing data. Due to the noise in the sensor, the fluctuation of single-frame data will cause the guide point to jitter at high frequency. For commercial vehicles with large inertia, this jitter will cause the vehicle body to shake violently, affecting cargo safety and passenger comfort.

[0004] (2) During lane changing, the vehicle’s main reference line will switch from the original lane to the target lane. The existing technology lacks a reference compensation for the instant of reference line switching, which causes the guide point coordinate system to jump, generating a discontinuous trajectory and forcing the vehicle’s steering wheel to suddenly correct. Summary of the Invention

[0005] This application provides a trajectory planning method, system, and storage medium based on guide point smoothing, which can solve the technical problems of trajectory jitter and reference line switching jumps in commercial vehicles existing in the prior art.

[0006] In a first aspect, embodiments of this application provide a trajectory planning method based on guide point smoothing, which includes: Obtain the previous guide point, the current reference line, and vehicle driving status information; Calculate the lateral deviation of the vehicle relative to the reference line at the current time; wherein, if a change in the reference line is detected, the lateral deviation is compensated based on the reference offset between the historical reference line before the change and the current reference line after the change; The lateral deviation is processed by trend accumulation, and when the accumulated value exceeds a preset threshold, a valid movement intention is determined. The lateral movement increment is calculated based on the valid movement intention and the real-time vehicle speed in the vehicle driving status information. The guide point at the previous moment is iteratively updated based on the lateral movement increment to obtain the smoothed guide point at the current moment; then the target trajectory is generated based on the smoothed guide point at the current moment.

[0007] Preferably, before obtaining the previous time-point guide point, the following steps are also included: Initialize the vehicle's path filter and trend manager, and reset the left and right lateral movement intentions and the initial movement speed of the guide point; Initialize the initial attitude and coordinate parameters of the guide point based on the current reference line and the candidate lane-changing reference line; The current reference line is saved as a historical reference line for reference line switching detection and deviation compensation calculation in the next planning cycle.

[0008] Preferably, the lateral deviation is compensated based on the reference offset between the historical reference line before the switch and the reference line at the current time after the switch, specifically including: Obtain the vehicle's current longitudinal position; At the current longitudinal position of the vehicle, the first lateral coordinate value of the historical reference line and the second lateral coordinate value of the reference line at the current moment are obtained respectively. The difference between the first horizontal coordinate value and the second horizontal coordinate value is used as the reference offset; The lateral deviation is superimposed on the reference offset to obtain the compensated lateral deviation.

[0009] Preferably, the lateral deviation is processed by trend accumulation, and a valid movement intention is determined when the accumulated value exceeds a preset threshold, specifically including: Obtain the trend direction of the previous moment; The current deviation direction is determined based on the positive or negative sign of the lateral deviation, and the trend accumulation value is obtained by combining the trend direction at the previous moment and a preset rule. The preset rule is: if the current deviation direction is consistent with the trend direction at the previous moment, the trend accumulation value is increased; if the current deviation direction is inconsistent with the trend direction at the previous moment, the trend accumulation value is decreased or cleared to zero, and the trend direction at the previous moment is updated to the current deviation direction. If the cumulative trend value is greater than the preset threshold, then a valid movement intention is determined; otherwise, no valid movement intention is determined.

[0010] Preferably, the lateral movement increment is calculated based on the valid movement intention and the real-time vehicle speed in the vehicle driving status information, specifically including: Calculate the basic lateral movement increment based on the valid movement intention; Obtain the lateral movement increment of the previous moment, and calculate the difference between the basic lateral movement increment and the lateral movement increment of the previous moment; A rate of change constraint threshold is determined based on the real-time vehicle speed; wherein the rate of change constraint threshold is negatively correlated with the real-time vehicle speed; if the absolute value of the difference is greater than the rate of change constraint threshold, the difference is restricted to the rate of change constraint threshold to obtain the basic lateral movement increment after the rate of change constraint. Obtain the predicted heading angle of the vehicle, calculate the heading deviation between the predicted heading angle of the vehicle and the heading angle of the reference line, and calculate the lateral compensation amount based on the heading deviation; The basic lateral movement increment constrained by the rate of change is superimposed with the lateral compensation amount to obtain a candidate lateral movement increment, which is then used as the lateral movement increment.

[0011] Preferably, calculating the lateral movement increment based on the effective movement intention and the real-time vehicle speed further includes: Based on the real-time vehicle speed query, a preset vehicle speed-limit mapping table is used to obtain the dynamic offset limit value at the current real-time vehicle speed. The vehicle speed-limit mapping table is configured such that the higher the vehicle speed, the smaller the dynamic offset limit value. The absolute value of the candidate lateral movement increment is compared with the dynamic offset limit; If the absolute value of the candidate lateral movement increment is greater than the dynamic offset limit, then the lateral movement increment is limited to the dynamic offset limit. If the absolute value of the candidate lateral movement increment is less than or equal to the dynamic offset limit, then the lateral movement increment is determined as the candidate lateral movement increment.

[0012] Preferably, the guide point at the previous time step is iteratively updated based on the lateral movement increment, specifically including: Obtain the previous horizontal coordinate and the previous vertical coordinate of the guide point at the previous time step; Add the previous longitudinal coordinate to the preset longitudinal step size to obtain the current longitudinal coordinate; Add the horizontal coordinate of the previous moment to the horizontal movement increment to obtain the horizontal coordinate of the current moment; The smoothed guide point for the current time is determined based on the vertical coordinate and the horizontal coordinate of the current time.

[0013] Preferably, generating the target trajectory based on the smoothed guide point at the current moment specifically includes: Using the smooth guide points that have been iteratively updated as trajectory anchor points, a continuous and smooth target trajectory is generated by a fifth-order polynomial fitting method.

[0014] Secondly, a trajectory planning system based on guide point smoothing is provided, which includes: The information acquisition module is used to obtain the previous guide point, the current reference line, and vehicle driving status information. The deviation compensation module is used to calculate the lateral deviation of the vehicle relative to the reference line at the current time. If a change in the reference line is detected, the lateral deviation is compensated based on the reference offset between the historical reference line before the change and the reference line at the current time after the change, so as to obtain the compensated lateral deviation. The intent determination module is used to perform trend accumulation processing on the compensated lateral deviation, and when the accumulated value exceeds a preset threshold, it determines the valid movement intent and calculates the lateral movement increment based on the valid movement intent and the real-time vehicle speed in the vehicle driving status information. The trajectory generation module is used to iteratively update the guide point of the previous moment based on the lateral movement increment to obtain the smoothed guide point of the current moment; and then generate the target trajectory based on the smoothed guide point of the current moment.

[0015] Thirdly, a computer-readable storage medium is provided, on which a trajectory planning program based on guide point smoothing is stored, wherein when the trajectory planning program based on guide point smoothing is executed by a processor, the steps of the above-described trajectory planning method based on guide point smoothing are implemented.

[0016] The beneficial effects of the technical solutions provided in this application include: First, by compensating for lateral deviation based on the reference offset between the historical reference line before the switch and the reference line at the current moment after the switch, seamless alignment of the old and new reference line coordinate systems is achieved. This eliminates abrupt changes in guide point positions and trajectory discontinuities caused by reference line switching, avoiding abrupt jumps in vehicle control commands. Second, by performing trend accumulation processing on the compensated lateral deviation and using a preset threshold to filter valid movement intentions, combined with real-time vehicle speed to calculate the lateral movement increment, this multi-frame temporal filtering mechanism effectively isolates single-frame data fluctuations caused by sensor noise, preventing vehicle body swaying due to large inertia in commercial vehicles. Simultaneously, adaptive vehicle speed calculation ensures tighter lateral movement permissions at high speeds, improving dynamic stability. Finally, iterative updates of the guide point at the previous moment based on the lateral movement increment achieve a smooth transition of the guide point position, resulting in a high-order continuity in the final generated target trajectory. In summary, this effectively solves the problems of trajectory jitter, reference line switching jumps, and low high-speed safety in existing technologies for commercial vehicles, significantly improving the robustness, ride comfort, and cargo safety of autonomous driving systems. It is particularly suitable for high-speed lane-changing scenarios for commercial vehicles with high centers of gravity and large masses. Attached Figure Description

[0017] Figure 1 This is a flowchart illustrating the trajectory planning method based on guide point smoothing in this application; Figure 2 This is a schematic diagram of the guide points for the lane-changing trajectory based on the guide point smoothing trajectory planning method of this application; Figure 3 This is a detailed flowchart of the lateral deviation compensation steps in this application; Figure 4 This is a schematic diagram illustrating the data flow between the trajectory planning system based on guide point smoothing and the vehicle behavior planning subsystem and speed planning subsystem of this application. Detailed Implementation

[0018] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present application, and not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present application.

[0019] To make the objectives, technical solutions, and advantages of this application clearer, the embodiments of this application will be described in further detail below with reference to the accompanying drawings.

[0020] Example 1 This application provides a trajectory planning method based on guide point smoothing, applied to the autonomous driving control system of commercial vehicles. This embodiment uses a scenario where the vehicle changes lanes to the right during cruising at a speed between 60 km / h and 90 km / h as an example for detailed explanation.

[0021] like Figure 1 and Figure 2 As shown, the trajectory planning method based on guide point smoothing mainly includes the following steps: Step 100: Obtain the previous time guide point, the current time reference line, and vehicle driving status information; Step 200: Calculate the lateral deviation of the vehicle relative to the reference line at the current time; wherein, if a change in the reference line is detected, the lateral deviation is compensated based on the reference offset between the historical reference line before the change and the reference line at the current time after the change, and the compensated lateral deviation is obtained. Step 300: Perform trend accumulation processing on the lateral deviation, and when the accumulated value exceeds the preset threshold, determine the valid movement intention, and calculate the lateral movement increment based on the valid movement intention and the real-time vehicle speed in the vehicle driving status information. Step 400: Iteratively update the guide point of the previous time step based on the horizontal movement increment to obtain the smoothed guide point of the current time step; then generate the target trajectory based on the smoothed guide point of the current time step.

[0022] The above steps first compensate for the lateral deviation by adjusting the baseline offset between the historical reference line before the switch and the reference line at the current moment after the switch. This achieves seamless alignment of the old and new reference line coordinate systems, eliminating abrupt changes in guide point positions and trajectory discontinuities caused by reference line switching, and avoiding abrupt changes in vehicle control commands. Secondly, by performing trend accumulation processing on the compensated lateral deviation and using a preset threshold to filter valid movement intentions, combined with real-time vehicle speed calculation of the lateral movement increment, this multi-frame temporal filtering mechanism effectively isolates single-frame data fluctuations caused by sensor noise, preventing vehicle body swaying due to large inertia in commercial vehicles. Simultaneously, adaptive speed calculation ensures tighter lateral movement permissions at high speeds, improving dynamic stability. Finally, iterative updates of the guide point at the previous moment are performed based on the lateral movement increment, achieving a smooth transition of the guide point position and ensuring high-order continuity in the final generated target trajectory. In summary, this technology effectively solves the problems of lane-changing trajectory jitter, reference line switching jumps, and low high-speed safety in existing technologies for commercial vehicles, and significantly improves the robustness, ride comfort, and cargo safety of autonomous driving systems. It is particularly suitable for high-speed lane-changing scenarios for commercial vehicles with high center of gravity and large mass.

[0023] The following is a detailed explanation of each step.

[0024] Step 100: Obtain the previous time step guide point, the current time step reference line, and vehicle driving status information. In this step, the basic data required for planning is first obtained. To ensure the stability of the iterative calculation, after entering the high-speed lane-changing planning state, an initialization operation must be performed first, specifically including: Step 100-1: Initialize the vehicle's path filter and trend manager, specifically including: Reset the horizontal movement intention and the initial movement speed of the guide point to zero. The initial attitude and coordinate parameters of the guide point are initialized based on the current reference line and the candidate track change reference line; the current reference line is saved as a historical reference line for reference line switching detection and deviation compensation calculation in the next planning cycle.

[0025] In this embodiment, the path filter and trend manager are initialized, and the default parameters are set as follows: laziness threshold laziness_=3.0; trend increment per frame trend_move_velocity_=1.0; maximum trend value max_trend_=6.0; rightward trend counter negative_=0; lateral increment of leading point l_inc=0; and upper limit of lateral movement speed leading_point_move_max_velocity_=-0.2 (negative value indicates rightward movement).

[0026] This step clarifies the source of the historical reference line, which is used for reference line switching detection and deviation compensation calculation in the next planning cycle. By clearing the state variables, sudden changes at system startup are avoided, providing a clean initial state for subsequent iterative calculations and ensuring the robustness of the planning system.

[0027] Step 100-2: Obtain vehicle driving status information. This specifically includes obtaining the vehicle's real-time speed, current position, and heading angle. In this embodiment, the vehicle speed is within the high-speed range of 60km / h-90km / h.

[0028] Step 200: Calculate the lateral deviation of the vehicle relative to the current reference line. If a reference line switch is detected, the lateral deviation is compensated based on the baseline offset between the historical reference line before the switch and the current reference line after the switch, resulting in the compensated lateral deviation. In this step, the system calculates the deviation of the planned target relative to the road center and compensates for it when the reference line switches to eliminate coordinate system jumps. (Reference) Figure 3 As shown, step 200 specifically includes the following steps: Step 200-1: Obtain the current longitudinal position of the vehicle.

[0029] Step 200-2: Calculate the lateral deviation and reference offset, that is, at the current longitudinal position of the vehicle, obtain the first lateral coordinate value of the historical reference line and the second lateral coordinate value of the reference line at the current moment. Step 200-3: Use the difference between the first horizontal coordinate value and the second horizontal coordinate value as the reference offset; Step 200-4: Add the lateral deviation to the reference offset to obtain the compensated lateral deviation.

[0030] In this embodiment, the guide point information at the current moment is obtained based on the previously generated optimal trajectory. The guide point at the current moment and the guide point at the previous moment are projected onto the reference line at the previous moment, and the lateral deviation l_diff_ between the current guide point and the guide point at the previous moment is calculated. The lateral deviation l_diff_ref (i.e., the reference offset) between the reference line at the previous moment and the reference line at the current moment is calculated. The reference line reference deviation is superimposed on the lateral deviation l_diff_ to obtain the final lateral deviation l_diff.

[0031] Since lane changing is essentially a translation of the reference line, without compensation, the position of the guide point relative to the new reference line will suddenly change. In this embodiment, by superimposing the reference offset, it is equivalent to mathematically aligning the zero point of the new reference line with the zero point of the old reference line, ensuring the continuity of deviation calculation, avoiding the controller from outputting abrupt steering commands due to sudden error changes, and solving the trajectory discontinuity problem caused by reference line switching.

[0032] Step 300: Perform trend accumulation processing on the compensated lateral deviation, and when the accumulated value exceeds a preset threshold, determine the valid movement intention, and calculate the lateral movement increment based on the valid movement intention and the real-time vehicle speed in the vehicle driving status information. In this step, the system filters noise through multi-frame trend judgment and calculates a safe movement increment in combination with vehicle speed, specifically including the following steps: Step 300-1, Trend Accumulation and Valid Intent Determination: That is, to obtain the trend direction of the previous moment; The current deviation direction is determined by the sign of the lateral deviation, and the cumulative trend value is obtained by combining the trend direction of the previous moment and the preset rules. The default rule is: if the current deviation direction is consistent with the trend direction of the previous moment, the trend accumulation value is increased; if the current deviation direction is inconsistent with the trend direction of the previous moment, the trend accumulation value is decreased or cleared to zero, and the trend direction of the previous moment is updated to the current deviation direction. If the cumulative trend value is greater than the preset threshold, then a valid movement intention is determined; otherwise, no valid movement intention is determined.

[0033] In this embodiment, the lane change is to the right. When l_diff < -0.2, it is determined that the vehicle has a tendency to move to the right, and negative_ = negative_ + 1; then, when there is a first tendency to move to the right, negative_ is 1, but negative_ is less than laziness_ (3.0), so it is considered an invalid intention, trend = 0; only when negative_ accumulates to be greater than laziness_ is it considered a valid intention, trend = 1.

[0034] This embodiment constructs a trend accumulation and inertia threshold anti-shake mechanism. Commercial vehicles have high inertia and are sensitive to high-frequency noise. Perception errors in a single frame may lead to micro-management of the guide point. This step utilizes multi-frame temporal information, and only takes action when the deviation persists and exceeds the "preset threshold". It is equivalent to a temporal filter with a dead zone, which effectively filters out the jitter caused by sensor noise and prevents repeated direction jumps.

[0035] Step 300-2, Calculation of smoothing increment of guide point (calculation of lateral movement increment), specifically includes: Calculate the base lateral movement increment based on valid movement intentions; The rate of change constraint threshold is determined based on the real-time vehicle speed; the rate of change constraint threshold is negatively correlated with the real-time vehicle speed; if the absolute value of the difference is greater than the rate of change constraint threshold, the difference is restricted to the rate of change constraint threshold to obtain the basic lateral movement increment after the rate of change constraint. Obtain the predicted heading angle of the vehicle, calculate the heading deviation between the predicted heading angle of the vehicle and the heading angle of the reference line, and calculate the lateral compensation based on the heading deviation. The basic lateral movement increment constrained by the rate of change is superimposed with the lateral compensation amount to obtain the candidate lateral movement increment, and the candidate lateral movement increment is used as the lateral movement increment.

[0036] Step 300-2-1, Calculating the basic lateral movement increment includes: Determine the status of valid movement intentions. Read the valid movement intention flag (e.g., trend) output in step 300-1. If trend=0 (invalid intention), set the basic lateral movement increment to zero and keep the guide point stationary; if trend=1 (valid intention), proceed to the next step of calculation. In this embodiment, when negative_ (rightward movement trend counter) accumulates to greater than laziness_, trend=1 is determined.

[0037] Step 300-2-2: Determine the direction sign of movement. Based on the lane-changing direction indicated by the valid movement intention, determine the positive or negative sign of the basic lateral movement increment. In this embodiment, it is a right lane change. Left is defined as positive, and right as negative. When a right lane change intention is determined, the direction sign is negative (-1).

[0038] Step 300-2-3: Determine the basic step size value. Based on the current real-time vehicle speed, query the preset speed-step size mapping table to obtain the basic step size value at the current vehicle speed. The basic step size value is negatively correlated with the real-time vehicle speed. In this embodiment, the vehicle speed is in the range of 60km / h-90km / h. For this high-speed condition, the basic step size value is set to 0.01 meters / frame. If the vehicle speed is higher, this value should be smaller (e.g., 0.005) to meet the high-speed stability requirements.

[0039] Step 300-2-4: Calculate the basic lateral movement increment. Multiply the basic step size value by the sign of the movement direction to obtain the basic lateral movement increment. Based on the above parameters, the theoretical value of the calculated basic lateral movement increment is -0.01. This value represents the ideal single-frame movement amount under no inertial constraints.

[0040] In this embodiment, when trend=1, the lateral increment l_inc of the guide point is calculated. A gradual acceleration strategy is used to update the guide point position.

[0041] Each time, increment by -0.01, then l_inc = l_inc - 0.01; l_inc <leading_point_move_max_velocity, l_inc=leading_point_move_max_velocity_ (i.e. -0.2).

[0042] The step size of -0.01 and the upper limit of -0.2 here are adaptive speed parameters set according to the vehicle speed range of 60km / h-90km / h. The higher the vehicle speed, the smaller the absolute value.

[0043] This embodiment implements dual speed adaptive protection. At high speeds, it limits the rate of change of increments to prevent sudden changes in vehicle dynamics; combined with the predicted vehicle front attitude, it optimizes the smoothness of the trajectory during the initial lane change.

[0044] Step 300-3, adaptive speed limiting, specifically includes: Based on the real-time vehicle speed query, the preset vehicle speed-limit mapping table is used to obtain the dynamic offset limit value at the current real-time vehicle speed. The vehicle speed-limit mapping table is configured such that the higher the vehicle speed, the smaller the dynamic offset limit value. Compare the absolute value of the candidate lateral movement increment with the dynamic offset limit; If the absolute value of the candidate lateral movement increment is greater than the dynamic offset limit, then the lateral movement increment is limited to the dynamic offset limit. If the absolute value of a candidate lateral movement increment is less than or equal to the dynamic offset limit, then the lateral movement increment is determined as a candidate lateral movement increment.

[0045] In this embodiment, a maximum value for the lateral movement increment of the guide point is determined based on different vehicle speeds. When the candidate lateral movement increment exceeds the limit, the final output lateral movement increment equals the limit value. This achieves tightening of the lateral change rate at high speeds and retaining adjustment margin at low speeds, limiting the lateral change rate during lane changes from the source, avoiding excessively abrupt or sudden lane changes, significantly improving vehicle lateral tracking performance and driving stability, and reducing the rollover risk of commercial vehicles during high-speed lane changes.

[0046] Step 400: Iteratively update the guide point from the previous time step based on the lateral movement increment to obtain the smoothed guide point at the current time step; then generate the target trajectory based on the smoothed guide point at the current time step. In this step, the system applies the calculated increment to the guide point coordinates and generates the final executable trajectory, specifically including the following steps: Step 400-1, iterative update of the guiding point, specifically including: Obtain the previous horizontal coordinate and the previous vertical coordinate of the guide point at the previous time step; Add the previous longitudinal coordinate to the preset longitudinal step size to obtain the current longitudinal coordinate; add the previous lateral coordinate to the lateral movement increment to obtain the current lateral coordinate. The smoothed guide point for the current time is determined based on the current vertical coordinate and the current horizontal coordinate.

[0047] In this embodiment, the lateral coordinate of the current guiding point is determined based on the lateral coordinate of the guiding point at the previous moment and the calculated lateral movement increment (e.g., current lateral coordinate = previous lateral coordinate + lateral movement increment). The longitudinal coordinate of the guiding point is determined based on the longitudinal coordinate of the guiding point at the previous moment and the sum of a preset longitudinal step size. This step clarifies the specific mathematical logic of the iterative update. By advancing the longitudinal step size and superimposing the lateral increment, the continuity of the guiding point in both the time and spatial domains is ensured. Compared to directly generating the target point, this incremental iteration method naturally possesses low-pass filtering characteristics, further guaranteeing the smoothness of the guiding point trajectory.

[0048] This step clarifies the specific mathematical logic of "iterative updates." By using a method of "vertical step-by-step advancement + horizontal incremental superposition," the continuity of the guiding points in both the time and spatial domains is ensured. Compared to directly generating target points, this incremental iteration method naturally possesses low-pass filtering characteristics, further guaranteeing the smoothness of the guiding point trajectory.

[0049] Step 400-2: Smooth trajectory generation output, specifically including: Using the smooth guide points that have been iteratively updated as trajectory anchor points, a continuous and smooth target trajectory is generated by a fifth-order polynomial fitting method.

[0050] In this embodiment, the smooth guide point after iterative update is used as the trajectory anchor point, and the industry-standard fifth-order polynomial fitting method is used to generate a continuous and smooth high-speed lane-changing trajectory, thus completing the overall trajectory planning output.

[0051] Since the key input guiding points have undergone the smoothing and iterative processing of the aforementioned steps, the generated target trajectory naturally possesses high-order continuity, that is, it has continuous position, velocity, and acceleration, which facilitates tracking by the underlying controller and reduces wear on the actuator.

[0052] In summary, step 200 eliminates the abrupt change in guide point position caused by the switching of the reference line coordinate system, ensuring the continuity of the trajectory in space. The underlying principle is as follows: Existing technologies typically switch the reference line directly during lane changes, causing a sudden, abrupt change in the lateral deviation of the guide point relative to the new reference line. The controller may misinterpret this as a large error, leading to drastic steering wheel adjustments. In step 200 of this application, upon detecting a reference line switch, the reference offset between the "historical reference line before the switch" and the "reference line at the current moment after the switch" is calculated, and this offset is added to the lateral deviation for compensation. This is equivalent to mathematically aligning the coordinate system zeros of the old and new reference lines, ensuring the continuity of the compensated lateral deviation at the moment of switch, thereby eliminating trajectory discontinuities and sudden steering wheel changes at the source.

[0053] Step 300 filters out single-frame data fluctuations caused by sensor noise, preventing vehicle swaying due to high inertia and improving passenger comfort and cargo safety. The underlying principle is as follows: Existing technologies often calculate movement directly based on single-frame deviation, which is susceptible to noise interference. This application introduces a trend accumulation processing mechanism in step 300. Instead of acting immediately based on single-frame deviation, it determines valid movement intentions by comparing the accumulated trend value with a preset threshold. Only when the deviation direction is consistent for multiple consecutive frames and the accumulated value exceeds the threshold is a valid intention confirmed. This temporal filtering mechanism eliminates high-frequency noise, ensuring that the guide point moves only under confirmed stable intentions, thus solving the trajectory jitter problem.

[0054] Steps 300 and 400 implement smooth iterative updates of the guide point and dynamically constrain the lateral movement range based on vehicle speed to prevent excessive lateral acceleration during high-speed lane changes, which could lead to vehicle instability or rollover. The solution principle is as follows: Step 400 employs an iterative update method, obtaining the current point based on the "previous time step guide point" plus the "lateral movement increment," rather than directly generating the target point. This incremental update inherently possesses low-pass filtering characteristics, ensuring a smooth transition of the guide point trajectory in the time domain. Step 300, when calculating the lateral movement increment, incorporates real-time vehicle speed for rate-of-change constraints and amplitude limits (the higher the speed, the smaller the increment limit). This aligns with the dynamic characteristics of commercial vehicles (sensitive to lateral forces at high speeds), limiting the lateral change rate during lane-changing from the source, preventing excessively abrupt movements, and achieving adaptive safety constraints across all vehicle speed conditions.

[0055] The above application utilizes the compensation in step 200, the intention filtering and velocity constraint in step 300, and the time iteration update in step 400. The three processes work synergistically, specifically manifested as follows: Input terminal (step 200): ensures the continuity of the deviation signal; Processing end (step 300): ensures the stability and security of control commands; Output (step 400): ensures the smoothness of the execution trajectory.

[0056] The final generated target trajectory is fitted based on smoothed guide points, naturally possessing high-order continuity. This method is particularly suitable for high-speed lane-changing scenarios of commercial vehicles with high center of gravity, large mass, and high inertia, significantly improving the robustness, comfort, and safety of autonomous driving systems.

[0057] Example 2 This application provides a trajectory planning system based on guide point smoothing, which includes: The information acquisition module is used to obtain the previous guide point, the current reference line, and vehicle driving status information. The deviation compensation module is used to calculate the lateral deviation of the vehicle relative to the reference line at the current time. If a change in the reference line is detected, the lateral deviation is compensated based on the reference offset between the historical reference line before the change and the reference line at the current time after the change, and the compensated lateral deviation is obtained. The intent determination module is used to perform trend accumulation processing on the compensated lateral deviation, and when the accumulated value exceeds a preset threshold, it determines the valid movement intent and calculates the lateral movement increment based on the valid movement intent and the real-time vehicle speed in the vehicle driving status information. The trajectory generation module is used to iteratively update the guide point of the previous time step based on the lateral movement increment to obtain the smoothed guide point of the current time step; then, the target trajectory is generated based on the smoothed guide point of the current time step.

[0058] The functions of each module in the above-mentioned trajectory planning system based on guide point smoothing correspond to the steps in the above-mentioned trajectory planning method embodiment based on guide point smoothing, and their functions and implementation processes will not be described in detail here.

[0059] refer to Figure 4 As shown, the modules on it are part of the motion planning subsystem within the overall vehicle planning system. The motion planning subsystem receives information from the behavior planning subsystem and finally outputs it to the speed planning subsystem.

[0060] Example 3 This application provides a trajectory planning device based on guide point smoothing. The trajectory planning device based on guide point smoothing can be a device with data processing function, such as an on-board chip, ECU, or server.

[0061] In this embodiment, the trajectory planning device based on guide point smoothing may include a processor, a memory, a communication interface, and a communication bus.

[0062] The communication bus can be of any type and is used to interconnect the processor, memory, and communication interface.

[0063] The communication interface includes input / output (I / O) interfaces, physical interfaces, and logical interfaces used for interconnecting internal components of the guide point-based smoothing trajectory planning device, as well as interfaces used for interconnecting the guide point-based smoothing trajectory planning device with other devices (such as other computing devices or user equipment). Physical interfaces can be Ethernet interfaces, fiber optic interfaces, ATM interfaces, etc.; user equipment can be displays, keyboards, etc.

[0064] Memory can be various types of storage media, such as random access memory (RAM), read-only memory (ROM), non-volatile RAM (NVRAM), flash memory, optical storage, hard disk, programmable ROM (PROM), erasable PROM (EPROM), electrically erasable PROM (EEPROM), etc.

[0065] The processor can be a general-purpose processor, which can call a trajectory planning program based on guide point smoothing stored in memory and execute the trajectory planning method based on guide point smoothing provided in the embodiments of this application. For example, the general-purpose processor can be a central processing unit (CPU). The method executed when the trajectory planning program based on guide point smoothing is called can be referred to in the various embodiments of the trajectory planning method based on guide point smoothing in this application, and will not be repeated here.

[0066] Example 4 This application also provides a computer-readable storage medium.

[0067] This application discloses a computer-readable storage medium storing a trajectory planning program based on guide point smoothing. When executed by a processor, the trajectory planning program based on guide point smoothing implements the steps of the trajectory planning method based on guide point smoothing as described above. The medium can be any medium capable of storing program code, such as a USB flash drive, external hard drive, read-only memory (ROM), random access memory (RAM), magnetic disk, or optical disk.

[0068] The method implemented when the trajectory planning program based on guide point smoothing is executed can be referred to in various embodiments of the trajectory planning method based on guide point smoothing in this application, and will not be repeated here.

[0069] It should be noted that the sequence numbers of the embodiments in this application are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments.

[0070] The terms "comprising" and "having," and any variations thereof, in the specification, claims, and accompanying drawings of this application are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or apparatus that includes a series of steps or units is not limited to the listed steps or units, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to such process, method, product, or apparatus. The terms "first," "second," and "third," etc., are used to distinguish different objects, etc., and do not indicate a sequence, nor do they limit "first," "second," and "third" to different types.

[0071] In the description of the embodiments of this application, terms such as "exemplary," "for example," or "for instance" are used to indicate examples, illustrations, or explanations. Any embodiment or design described as "exemplary," "for example," or "for instance" in the embodiments of this application should not be construed as being more preferred or advantageous than other embodiments or designs. Specifically, the use of terms such as "exemplary," "for example," or "for instance" is intended to present the relevant concepts in a concrete manner.

[0072] In the description of the embodiments of this application, unless otherwise stated, " / " means "or". For example, A / B can mean A or B. The "and / or" in the text is merely a description of the relationship between related objects, indicating that there can be three relationships. For example, A and / or B can mean: A exists alone, A and B exist simultaneously, and B exists alone. In addition, in the description of the embodiments of this application, "multiple" means two or more.

[0073] In some processes described in the embodiments of this application, multiple operations or steps are included in a specific order. However, it should be understood that these operations or steps may not be executed in the order they appear in the embodiments of this application, or they may be executed in parallel. The sequence number of the operation is only used to distinguish different operations, and the sequence number itself does not represent any execution order. In addition, these processes may include more or fewer operations, and these operations or steps may be executed sequentially or in parallel, and these operations or steps may be combined.

[0074] Through the above description of the embodiments, those skilled in the art can clearly understand that the methods of the above embodiments can be implemented by means of software plus necessary general-purpose hardware platforms. Of course, they can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of this application, in essence, or the part that contributes 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 ROM / RAM, magnetic disk, optical disk) as described above, and includes several instructions to cause a terminal device to execute the methods described in the various embodiments of this application.

[0075] The above are merely preferred embodiments of this application and do not limit the patent scope of this application. Any equivalent structural or procedural transformations made using the content of this application's specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of this application.

Claims

1. A trajectory planning method based on guide point smoothing, characterized in that, It includes: Obtain the previous guide point, the current reference line, and vehicle driving status information; Calculate the lateral deviation of the vehicle relative to the reference line at the current time; wherein, if a change in the reference line is detected, the lateral deviation is compensated based on the reference offset between the historical reference line before the change and the current reference line after the change; The lateral deviation is processed by trend accumulation, and when the accumulated value exceeds a preset threshold, a valid movement intention is determined. The lateral movement increment is calculated based on the valid movement intention and the real-time vehicle speed in the vehicle driving status information. The guide point at the previous moment is iteratively updated based on the lateral movement increment to obtain the smoothed guide point at the current moment; then the target trajectory is generated based on the smoothed guide point at the current moment.

2. The trajectory planning method based on guide point smoothing as described in claim 1, characterized in that, Before obtaining the previous time-point guide, the following steps are also included: Initialize the vehicle's path filter and trend manager, and reset the left and right lateral movement intentions and the initial movement speed of the guide point; Initialize the initial attitude and coordinate parameters of the guide point based on the current reference line and the candidate lane-changing reference line; The current reference line is saved as a historical reference line for reference line switching detection and deviation compensation calculation in the next planning cycle.

3. The trajectory planning method based on guide point smoothing as described in claim 1, characterized in that, The lateral deviation is compensated based on the reference offset between the historical reference line before the switch and the current reference line after the switch, specifically including: Obtain the vehicle's current longitudinal position; At the current longitudinal position of the vehicle, the first lateral coordinate value of the historical reference line and the second lateral coordinate value of the reference line at the current moment are obtained respectively. The difference between the first horizontal coordinate value and the second horizontal coordinate value is used as the reference offset; The lateral deviation is superimposed on the reference offset to obtain the compensated lateral deviation.

4. The trajectory planning method based on guide point smoothing as described in claim 1, characterized in that, Lateral deviations are processed by trend accumulation, and when the accumulated value exceeds a preset threshold, a valid movement intention is determined, specifically including: Obtain the trend direction of the previous moment; The current deviation direction is determined based on the positive or negative sign of the lateral deviation, and the trend accumulation value is obtained by combining the trend direction of the previous moment and a preset rule. The preset rule is: if the current deviation direction is consistent with the trend direction of the previous moment, the trend accumulation value is increased; if the current deviation direction is inconsistent with the trend direction of the previous moment, the trend accumulation value is decreased or cleared to zero, and the trend direction of the previous moment is updated to the current deviation direction. If the cumulative trend value is greater than the preset threshold, then a valid movement intention is determined; otherwise, no valid movement intention is determined.

5. The trajectory planning method based on guide point smoothing as described in claim 4, characterized in that, The lateral movement increment is calculated based on the valid movement intention and the real-time vehicle speed in the vehicle driving status information, specifically including: Calculate the basic lateral movement increment based on the valid movement intention; Obtain the lateral movement increment of the previous moment, and calculate the difference between the basic lateral movement increment and the lateral movement increment of the previous moment; A rate of change constraint threshold is determined based on the real-time vehicle speed; wherein the rate of change constraint threshold is negatively correlated with the real-time vehicle speed; if the absolute value of the difference is greater than the rate of change constraint threshold, the difference is restricted to the rate of change constraint threshold to obtain the basic lateral movement increment after the rate of change constraint. Obtain the predicted heading angle of the vehicle, calculate the heading deviation between the predicted heading angle of the vehicle and the heading angle of the reference line, and calculate the lateral compensation amount based on the heading deviation; The basic lateral movement increment constrained by the rate of change is superimposed with the lateral compensation amount to obtain a candidate lateral movement increment, which is then used as the lateral movement increment.

6. The trajectory planning method based on guide point smoothing as described in claim 5, characterized in that, Calculating the lateral movement increment based on the effective movement intention and the real-time vehicle speed also includes: Based on the real-time vehicle speed query, a preset vehicle speed-limit mapping table is used to obtain the dynamic offset limit value at the current real-time vehicle speed. The vehicle speed-limit mapping table is configured such that the higher the vehicle speed, the smaller the dynamic offset limit value. The absolute value of the candidate lateral movement increment is compared with the dynamic offset limit; If the absolute value of the candidate lateral movement increment is greater than the dynamic offset limit, then the lateral movement increment is limited to the dynamic offset limit. If the absolute value of the candidate lateral movement increment is less than or equal to the dynamic offset limit, then the lateral movement increment is determined as the candidate lateral movement increment.

7. The trajectory planning method based on guide point smoothing as described in claim 1, characterized in that, The guide point at the previous time step is iteratively updated based on the aforementioned lateral movement increment, specifically including: Obtain the previous horizontal coordinate and the previous vertical coordinate of the guide point at the previous time step; Add the previous longitudinal coordinate to the preset longitudinal step size to obtain the current longitudinal coordinate; Add the horizontal coordinate of the previous moment to the horizontal movement increment to obtain the horizontal coordinate of the current moment; The smoothed guide point for the current time is determined based on the vertical coordinate and the horizontal coordinate of the current time.

8. The trajectory planning method based on guide point smoothing as described in claim 1, characterized in that, Generate the target trajectory based on the smoothed guide point at the current moment, specifically including: Using the smooth guide points that have been iteratively updated as trajectory anchor points, a continuous and smooth target trajectory is generated by a fifth-order polynomial fitting method.

9. A trajectory planning system based on guide point smoothing, characterized in that, It includes: The information acquisition module is used to obtain the previous guide point, the current reference line, and vehicle driving status information. The deviation compensation module is used to calculate the lateral deviation of the vehicle relative to the reference line at the current time. If a change in the reference line is detected, the lateral deviation is compensated based on the reference offset between the historical reference line before the change and the reference line at the current time after the change, so as to obtain the compensated lateral deviation. The intent determination module is used to perform trend accumulation processing on the compensated lateral deviation, and when the accumulated value exceeds a preset threshold, it determines the valid movement intent and calculates the lateral movement increment based on the valid movement intent and the real-time vehicle speed in the vehicle driving status information. The trajectory generation module is used to iteratively update the guide point of the previous moment based on the lateral movement increment to obtain the smoothed guide point of the current moment. Then, the target trajectory is generated based on the smoothed guide point at the current moment.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a trajectory planning program based on guide point smoothing, wherein when the trajectory planning program based on guide point smoothing is executed by a processor, it implements the steps of the trajectory planning method based on guide point smoothing as described in any one of claims 1 to 8.

Images