Automatic driving longitudinal control method based on self-adaptive preview and dynamic gradient compensation

Abstract

The invention relates to the technical field of automatic driving control, in particular to an automatic driving longitudinal control method based on self-adaptive preview and dynamic gradient compensation, which comprises the following steps of: calculating an index value of the position of a vehicle relative to a nearest track point of a reference track through an interpolation algorithm; calculating a corresponding reference trajectory curvature value according to the calculated index value, and calculating a preview distance; determining a preview point index, extracting speed values of two points, and calculating a feed-forward acceleration in combination with a preview distance; calculating a feedback acceleration according to the speed error, adding the feedback acceleration and the feed-forward acceleration, performing amplitude limiting to obtain a feed-feedback composite control acceleration value, and generating a new acceleration command value; and selecting a slope angle for gradient compensation, performing acceleration gradient compensation to obtain a final acceleration command value, and circulating the steps until the vehicle safely arrives at the end point. The overall precision, robustness and adaptability of the longitudinal control system are improved, and the method is suitable for various road structures and driving scenes and has remarkable engineering application value.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving control technology, and in particular to an autonomous driving longitudinal control method based on adaptive preview and dynamic slope compensation. Background Art

[0002] With the rapid development of autonomous driving technology, longitudinal control technology in autonomous driving systems has become a critical component for achieving safe and efficient driving. Existing longitudinal control technologies typically employ a PID feedback control method based on a fixed preview distance. This method obtains the target speed value by setting a fixed-length preview window and calculates the acceleration command using a proportional-integral-derivative control algorithm. However, these methods suffer from flaws in the preview mechanism when used in complex road scenarios. The fixed preview distance cannot adapt to changes in road curvature, and the lack of curvature-sensitive parameters leads to a mismatch between the preview point selection and vehicle dynamics, ultimately reducing speed control accuracy in curved conditions. Slope compensation is also problematic, with a single slope data source resulting in inaccurate slope perception and a lack of multi-source fusion between the vehicle's own sensor data and the planned trajectory elevation data. Furthermore, the control architecture suffers from flaws, including insufficient coupling between the feedforward and feedback control components and a feedforward term that fails to account for the velocity change rate at the preview point, resulting in increased acceleration command overshoot under sudden speed changes.

[0003] Therefore, the existing longitudinal control methods have obvious deficiencies in terms of adaptive preview mechanism, slope perception accuracy and control architecture design. There is an urgent need for a longitudinal control method that can dynamically adjust the preview distance and combine trajectory and posture information to perform adaptive slope compensation to improve the accuracy and stability of control. Summary of the Invention

[0004] In response to the above-mentioned technical deficiencies, the present invention provides an autonomous driving longitudinal control method based on adaptive preview and dynamic slope compensation, so as to dynamically adjust the preview distance and perform adaptive slope compensation in combination with trajectory and posture information.

[0005] The present invention is achieved through the following technical solutions: A method for longitudinal control of an autonomous driving vehicle based on adaptive preview and dynamic slope compensation is provided, the method comprising the following steps: Step S10: Receive reference trajectory point data sent by the planning layer, receive vehicle chassis feedback data, and receive vehicle positioning data, and interpolate and calculate the index value of the vehicle's own position relative to the nearest trajectory point of the reference trajectory based on the received data; Step S20: Obtaining a corresponding reference trajectory curvature value based on the calculated index value of the closest point on the reference trajectory. The curvature of the corresponding reference trajectory point is calculated by fitting a three-point circle to the received reference trajectory. The preview distance is calculated by comprehensively considering the curvature of the closest point on the current reference trajectory, the current vehicle speed, and the front and rear wheelbase of the vehicle. The preview distance is dynamically adapted to the vehicle speed and curvature. The calculation of the preview distance is positively correlated with the vehicle speed and negatively correlated with the curvature. Step S30: Starting from the calculated index of the closest point on the reference trajectory, the distances between the trajectory points are accumulated and compared with the calculated preview distance to obtain the index value of the preview point on the reference trajectory. The velocities of the closest point and the preview point are obtained from the calculated index values on the reference trajectory, respectively, and combined with the preview distance to calculate the feedforward acceleration. Step S40: Comparing the velocity of the nearest point on the reference trajectory with the current vehicle velocity to obtain a velocity error, and changing the sign of the velocity error value in combination with the vehicle's travel direction. The velocity error noise is filtered and a feedback acceleration value is obtained through PID calculation. The feedforward acceleration and feedback acceleration are added together, and after limiting the acceleration magnitude, a feedforward-feedback composite control acceleration value is obtained. Error compensation is performed on the calculated composite control acceleration value, and the composite control acceleration value is compared with the current actual acceleration value. The error is filtered, and the control instruction is dynamically corrected based on the error value, ultimately generating a new acceleration command value. Step S50: Two slope angle calculation methods are considered: one is to directly calculate the slope angle from the vehicle posture, and the other is to calculate the slope angle from the elevation difference of the trajectory points. The slope angle used for slope compensation is dynamically selected based on the set slope source strategy. In the adaptive mode, the trajectory-calculated slope angle is used at high speeds, and the original slope angle is used at low speeds. Acceleration slope compensation is performed on the new acceleration command value. The slope-compensated acceleration value is then smoothed and filtered to obtain the final acceleration command control output value. This process is repeated repeatedly to output the corresponding acceleration control command value until the vehicle safely reaches the destination.

[0006] Preferably, the step of calculating the index value of the vehicle's own position relative to the nearest trajectory point of the reference trajectory by an interpolation algorithm in step S10 includes receiving the reference trajectory point posture data and the vehicle's current posture data, traversing the trajectory points and searching for the nearest point index based on the square of the Euclidean distance and the heading error, when the nearest point is found, obtaining the next closest point index, selecting the closest point before and after the nearest point as the next closest point, calculating the nearest point posture information by linear interpolation and returning the nearest point index value, and when the nearest point is not found, outputting that the nearest point is not found.

[0007] Preferably, in step S20, the corresponding reference trajectory curvature value is calculated by three-point circle fitting, and the calculation formula is shown in formula (1): (1) Where (x1, y1), (x2, y2), and (x3, y3) are three points on the reference trajectory, curvature is the calculated curvature value, den is the product of the lengths of the three sides formed by the three points, and area is twice the directed area of the triangle formed by the three points on the reference trajectory.

[0008] Preferably, in step S20, the preview distance is calculated by combining the calculated curvature value, the current vehicle speed, and the front and rear wheelbase of the vehicle, so that the preview distance dynamically adapts to the vehicle speed and curvature. The calculation of the preview distance is positively correlated with the vehicle speed and negatively correlated with the curvature. The calculation formula of the preview distance is shown in formula (2): (2) Where L is the vehicle's front and rear wheelbase, speed_ratio is the vehicle's current speed gain coefficient, vx_abs is the absolute value of the vehicle's current speed, curvature_abs is the absolute value of the curvature of the current reference trajectory point, curvature_gain is the curvature gain coefficient, and ld is the comprehensively calculated preview distance.

[0009] Preferably, in step S30, the trajectory distance is accumulated starting from the nearest point index of the reference trajectory, compared with the preview distance to determine the preview point index, the velocity values of the two points are extracted, and the feedforward acceleration is calculated in combination with the preview distance. The steps include obtaining the preview distance, the reference trajectory point and the nearest point index value, traversing the trajectory points starting from the nearest point in a loop and accumulating the distances between the trajectory points, when the accumulated distance is not less than the preview distance, selecting the current point as the preview point and returning the preview point index value, when the trajectory point traversal is completed, selecting the last point as the preview point and returning the preview point index value, when the trajectory point traversal is not completed, continuing to traverse the trajectory points in a loop and accumulating the distances between the trajectory points, and looping through the above steps until the preview point is determined; extracting the velocity values of the two points is to extract the velocity values of the nearest point and the preview point, and calculating the feedforward acceleration in combination with the preview distance. The calculation formula is shown in formula (3): (3) Where vx_pre is the velocity of the preview point, vx_nearest is the velocity of the nearest point, ld is the preview distance, and ff_acc is the feedforward acceleration value.

[0010] Preferably, in step S40, the speed error is obtained by comparing the speed of the nearest point of the reference trajectory with the current speed of the vehicle. The speed error is obtained by comparing the speed of the nearest point of the reference trajectory with the current speed of the vehicle, and the sign of the speed error value is determined in combination with the vehicle's driving direction. The feedback acceleration value is obtained by filtering the speed error noise and performing PID calculation. The speed error calculation formula is shown in formula (4): (4) Where vx_error is the calculated speed error value, vx_sign is the sign value of the vehicle's driving direction, which is 1 when the vehicle is moving forward and -1 when it is moving backward, vx_nearest is the speed value of the nearest point of the vehicle's reference trajectory, and vx_current is the current speed value of the vehicle. The discrete form of the PID calculation formula is shown in Equation (5): (5) Among them, kp, ki, and kd are proportional, integral, and differential gains respectively, e[n] is the speed error at the current moment, e[n-1] is the speed error at the previous moment, Δt is the control period, and u[n] is the control quantity output at the current moment n. The feedforward acceleration and feedback acceleration are added together, and the feedforward-feedback composite control acceleration value is obtained after the acceleration size is limited. The feedforward-feedback composite control acceleration value is error compensated, and the composite control acceleration value is compared with the current acceleration value of the vehicle. The error is filtered and the control instruction is dynamically corrected according to the error value. Finally, a new acceleration command value is generated. The error compensation dynamic correction formula is shown in Equation (6): (6) Where acc_cur is the current acceleration value of the vehicle, acc_ctrl is the calculated feedforward-feedback composite control acceleration value, acc_error is the acceleration error value, fb_gain is the acceleration feedback gain value, and acc_comp is the acceleration command value after error compensation correction.

[0011] Preferably, in step S50, the slope angle for slope compensation is dynamically selected based on a set slope source strategy. Two slope angle calculation methods are considered, including directly calculating the slope angle from the vehicle's posture and calculating the slope angle from the elevation difference of trajectory points. An adaptive speed threshold is set, the slope source is initially set, and a determination is made as to whether the vehicle is in adaptive mode. If the vehicle is in adaptive mode, a determination is made as to whether the current vehicle speed is greater than the adaptive speed threshold. If the current vehicle speed is greater than the adaptive speed threshold, the slope angle is calculated from the elevation difference of trajectory points. If the current vehicle speed is less than or equal to the adaptive speed threshold, the slope angle is directly calculated from the vehicle's posture. If the vehicle is not in adaptive mode, the slope angle is calculated based on the determined slope source. Furthermore, when the adaptive mode is selected, the slope angle is calculated using the trajectory at high speeds and the original slope angle at low speeds.

[0012] In addition, to achieve the above-mentioned objectives, the present invention further proposes an autonomous driving longitudinal control system based on adaptive preview and dynamic slope compensation, wherein the autonomous driving longitudinal control system based on adaptive preview and dynamic slope compensation comprises: Data acquisition and nearest trajectory point index value calculation module: used to obtain reference trajectory point data, chassis feedback data and vehicle positioning data, and calculate the index value of the vehicle's own position relative to the nearest trajectory point of the reference trajectory through interpolation algorithm; Reference trajectory curvature value and preview distance calculation module: This module is used to obtain the curvature value of the nearest point based on the calculated index value and the reference curvature value calculated by three-point circle fitting. The preview distance is calculated based on the current vehicle speed and the front and rear wheelbase of the vehicle. This allows the preview distance to dynamically adapt to the vehicle speed and curvature. The calculation of the preview distance is positively correlated with the vehicle speed and negatively correlated with the curvature. Feedforward acceleration calculation module: used to accumulate the trajectory distance starting from the closest point index of the reference trajectory, compare it with the preview distance to determine the preview point index, extract the speed value of the closest point of the reference trajectory and the preview point speed value, and calculate the feedforward acceleration based on the preview distance; The composite control acceleration calculation and new acceleration command value generation module is used to compare the speed value of the nearest point of the reference trajectory with the current vehicle speed to obtain the speed error. The calculation sign is determined according to the driving direction. The feedback acceleration is calculated through noise filtering and PID, and then added to the feedforward acceleration and limited to obtain the feedforward-feedback composite control acceleration value. The new acceleration command value is then generated through error compensation correction. Slope angle calculation and final acceleration command value generation module: It is used to dynamically select the slope angle for slope compensation based on the set slope source strategy, perform acceleration slope compensation based on the new acceleration command value, and obtain the final acceleration command control output value through smoothing filtering. The above steps are continuously repeated to output the corresponding acceleration command value until the vehicle reaches the destination safely.

[0013] In addition, to achieve the above-mentioned purpose, the present invention also proposes an autonomous driving longitudinal control device based on adaptive preview and dynamic slope compensation, the device including: a memory, a processor, and programs such as an optical crystal defect detection algorithm stored in the memory and runnable on the processor, the programs such as the optical crystal defect detection algorithm are steps for implementing the autonomous driving longitudinal control method based on adaptive preview and dynamic slope compensation as described above.

[0014] In addition, to achieve the above-mentioned purpose, the present invention also provides a computer program product, which includes programs such as an optical crystal defect detection algorithm. When the programs such as the optical crystal defect detection algorithm are executed by a processor, the autonomous driving longitudinal control method based on adaptive preview and dynamic slope compensation as described above is implemented.

[0015] The advantages and effects of the present invention are: This invention dynamically calculates the preview distance through an adaptive preview mechanism based on road curvature and vehicle speed, and obtains the target speed and position of future path points. This enables the control system to perceive path changes in advance, enhancing its responsiveness to complex road scenarios and effectively reducing tracking error and control delay. This method combines target acceleration calculation (feedforward) based on the preview point with PID feedback adjustment of the vehicle's actual speed error, giving the system improved response speed and anti-interference capabilities during dynamic acceleration and deceleration. Compared to traditional pure feedback methods, it can significantly reduce overshoot and system oscillation. Based on the current vehicle speed and slope source selection mechanism, this method integrates the target slope information calculated in the trajectory with the actual vehicle pitch angle, dynamically selects an appropriate slope compensation path, and combines a low-pass filter to reduce attitude measurement noise, improving slope perception accuracy and control output smoothness. This method is particularly suitable for conditions such as hill starts and long descent control.

[0016] In summary, the present invention can effectively overcome the shortcomings of existing autonomous driving longitudinal control methods in terms of unreasonable preview mechanism, inaccurate slope processing, and poor control structure. It improves the overall accuracy, robustness, and adaptability of the longitudinal control system, is applicable to a variety of road structures and driving scenarios, and has significant engineering application value. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0018] Figure 1 This is a flow chart of the autonomous driving longitudinal control method based on adaptive preview and dynamic slope compensation of the present invention.

[0019] Figure 2 This is a flowchart of calculating the nearest point of a reference trajectory based on the current status of the vehicle in one embodiment of the present invention.

[0020] Figure 3 The flowchart of calculating the preview point and its index value according to the preview distance and the nearest point index value in one embodiment of the present invention.

[0021] Figure 4 This is a flow chart of dynamically selecting a slope angle for slope compensation according to a slope source strategy in one embodiment of the present invention.

[0022] Figure 5 Schematic diagram of the structure of the autonomous driving longitudinal control system based on adaptive preview and dynamic slope compensation of the present invention.

[0023] Figure 6 This is a schematic block diagram of the structure of the autonomous driving longitudinal control electronic device based on adaptive preview and dynamic slope compensation of the present invention. DETAILED DESCRIPTION

[0024] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0025] like Figure 1 As shown, in one embodiment of the present invention, an autonomous driving longitudinal control method based on adaptive preview and dynamic slope compensation includes the following steps: Step S10: Receive reference trajectory point data sent by the planning layer, receive vehicle chassis feedback data, and receive the vehicle's own positioning data, and interpolate the received data to calculate the index value of the vehicle's own position relative to the nearest trajectory point of the reference trajectory.

[0026] Specifically, the step of calculating the index value of the vehicle's own position relative to the nearest track point of the reference track by the interpolation algorithm in step S10 is as follows: Figure 2 As shown, it includes receiving the reference trajectory point pose data and the vehicle's current pose data, traversing the trajectory points and finding the nearest point index based on the Euclidean distance squared and the heading error. When the nearest point is found, the next closest point index is obtained, and the point with a shorter distance before and after the nearest point is selected as the next closest point. The nearest point pose information is calculated by linear interpolation and the nearest point index value is returned. When the nearest point is not found, the nearest point is outputted as not found.

[0027] Step S20: Obtain the corresponding reference trajectory curvature value based on the calculated index value of the nearest point of the reference trajectory. The corresponding reference trajectory point curvature is calculated by fitting a three-point circle to the received reference trajectory. The preview distance is calculated by comprehensively considering the curvature of the nearest point of the current reference trajectory, the current vehicle speed, and the front and rear wheelbases of the vehicle. The preview distance is dynamically adapted to the vehicle speed and curvature. The calculation of the preview distance is positively correlated with the vehicle speed and negatively correlated with the curvature.

[0028] Specifically, in step S20, the corresponding reference trajectory curvature value is calculated by three-point circle fitting, and the calculation formula is shown in formula (1): (1) Where (x1, y1), (x2, y2), and (x3, y3) are three points on the reference trajectory, curvature is the calculated curvature value, den is the product of the lengths of the three sides formed by the three points, and area is twice the directed area of the triangle formed by the three points on the reference trajectory.

[0029] Specifically, in step S20, the preview distance is calculated by combining the calculated curvature value, the current vehicle speed, and the front and rear wheelbase of the vehicle, so that the preview distance dynamically adapts to the vehicle speed and curvature. The calculation of the preview distance is positively correlated with the vehicle speed and negatively correlated with the curvature. The calculation formula of the preview distance is shown in formula (2): (2) Where L is the vehicle's front and rear wheelbase, speed_ratio is the vehicle's current speed gain coefficient, vx_abs is the absolute value of the vehicle's current speed, curvature_abs is the absolute value of the curvature of the current reference trajectory point, curvature_gain is the curvature gain coefficient, and ld is the comprehensively calculated preview distance.

[0030] Step S30: Starting from the calculated index of the closest point on the reference trajectory, the distances between trajectory points are accumulated and compared with the calculated preview distance to obtain the index value of the preview point on the reference trajectory. The speed values of the closest point and the preview point on the reference trajectory are obtained from the calculated index values, respectively, and the feedforward acceleration is calculated in combination with the preview distance.

[0031] Specifically, in step S30, the track distance is accumulated starting from the nearest point index of the reference track, and the step of comparing it with the preview distance to determine the preview point index is as follows: Figure 3 As shown, it includes obtaining the preview distance, reference trajectory point and the index value of the nearest point, traversing the trajectory points in a loop starting from the nearest point and accumulating the distance between the trajectory points. When the accumulated distance is not less than the preview distance, the current point is selected as the preview point and the preview point index value is returned. When the trajectory point traversal is completed, the last point is selected as the preview point and the preview point index value is returned. When the trajectory point traversal is not completed, the trajectory points are continued to be traversed in a loop and the distance between the trajectory points is accumulated. The above steps are repeated until the preview point is determined; the velocity values of the two points are extracted, and the feedforward acceleration is calculated in combination with the preview distance. The formula for extracting the velocity values of the two points is to extract the velocity values of the nearest point and the preview point, and the feedforward acceleration is calculated in combination with the preview distance as shown in formula (3): (3) Where vx_pre is the velocity of the preview point, vx_nearest is the velocity of the nearest point, ld is the preview distance, and ff_acc is the feedforward acceleration value.

[0032] Step S40: A speed error is obtained by comparing the speed of the nearest point on the reference trajectory with the current speed of the vehicle. The sign of the speed error value is changed in combination with the vehicle's driving direction. The speed error noise is filtered and a feedback acceleration value is obtained through PID calculation. The feedforward acceleration and feedback acceleration are added together, and the feedforward-feedback composite control acceleration value is obtained after limiting the acceleration magnitude. Error compensation is performed on the calculated composite control acceleration value, and the composite control acceleration value is compared with the current actual acceleration value. The error is filtered and the control instruction is dynamically corrected according to the error value to finally generate a new acceleration command value.

[0033] Specifically, in step S40, the speed error is obtained by comparing the speed of the nearest point of the reference trajectory with the current speed of the vehicle. The speed error is obtained by comparing the speed of the nearest point of the reference trajectory with the current speed of the vehicle, and the sign of the speed error value is determined in combination with the vehicle's driving direction. The feedback acceleration value is obtained by filtering the speed error noise and performing PID calculation. The speed error calculation formula is shown in formula (4): (4) Where vx_error is the calculated speed error value, vx_sign is the sign value of the vehicle's driving direction, which is 1 when the vehicle is moving forward and -1 when it is moving backward, vx_nearest is the speed value of the nearest point of the vehicle's reference trajectory, and vx_current is the current speed value of the vehicle. The discrete form of the PID calculation formula is shown in Equation (5): (5) Among them, kp, ki, and kd are proportional, integral, and differential gains respectively, e[n] is the speed error at the current moment, e[n-1] is the speed error at the previous moment, Δt is the control period, and u[n] is the control quantity output at the current moment n. The feedforward acceleration and feedback acceleration are added together, and the feedforward-feedback composite control acceleration value is obtained after the acceleration size is limited. The feedforward-feedback composite control acceleration value is error compensated, and the composite control acceleration value is compared with the current acceleration value of the vehicle. The error is filtered and the control instruction is dynamically corrected according to the error value. Finally, a new acceleration command value is generated. The error compensation dynamic correction formula is shown in Equation (6): (6) Where acc_cur is the current acceleration value of the vehicle, acc_ctrl is the calculated feedforward-feedback composite control acceleration value, acc_error is the acceleration error value, fb_gain is the acceleration feedback gain value, and acc_comp is the acceleration command value after error compensation correction.

[0034] Step S50: Two slope angle calculation methods are considered: one is to directly calculate the slope angle from the vehicle posture, and the other is to calculate the slope angle from the elevation difference of the trajectory points. The slope angle used for slope compensation is dynamically selected based on the set slope source strategy. In the adaptive mode, the trajectory-calculated slope angle is used at high speeds, and the original slope angle is used at low speeds. Acceleration slope compensation is performed on the new acceleration command value. The slope-compensated acceleration value is then smoothed and filtered to obtain the final acceleration command control output value. This process is repeated repeatedly to output the corresponding acceleration control command value until the vehicle safely reaches the destination.

[0035] Specifically, in step S50, the slope angle for slope compensation is dynamically selected according to the set slope source strategy. Two slope angle calculation methods are considered, including directly calculating the slope angle from the vehicle posture and calculating the slope angle from the elevation difference of the trajectory points. The adaptive speed threshold is set. The specific process is as follows: Figure 4 As shown, it includes initially setting the slope source, judging whether the vehicle is in adaptive mode, judging whether the current vehicle speed is greater than the adaptive speed threshold when the vehicle is in adaptive mode, and when the current vehicle speed is greater than the adaptive speed threshold, selecting the elevation difference of the trajectory point to calculate the slope angle, and when the current vehicle speed is less than or equal to the adaptive speed threshold, directly calculating the slope angle from the vehicle posture; when the vehicle is not in adaptive mode, calculating the slope angle according to the determined slope source, and when the adaptive mode is selected, using the trajectory to calculate the slope angle at high speed and using the original slope angle at low speed.

[0036] In addition, if Figure 5 As shown, in one embodiment of the present invention, an autonomous driving longitudinal control system based on adaptive preview and dynamic slope compensation is proposed. The autonomous driving longitudinal control system based on adaptive preview and dynamic slope compensation includes: Data acquisition and nearest trajectory point index value calculation module: used to obtain reference trajectory point data, chassis feedback data and vehicle positioning data, and calculate the index value of the vehicle's own position relative to the nearest trajectory point of the reference trajectory through interpolation algorithm; Reference trajectory curvature value and preview distance calculation module: This module is used to obtain the curvature value of the nearest point based on the calculated index value and the reference curvature value calculated by three-point circle fitting. The preview distance is calculated based on the current vehicle speed and the front and rear wheelbase of the vehicle. This allows the preview distance to dynamically adapt to the vehicle speed and curvature. The calculation of the preview distance is positively correlated with the vehicle speed and negatively correlated with the curvature. Feedforward acceleration calculation module: used to accumulate the trajectory distance starting from the closest point index of the reference trajectory, compare it with the preview distance to determine the preview point index, extract the speed value of the closest point of the reference trajectory and the preview point speed value, and calculate the feedforward acceleration based on the preview distance; The composite control acceleration calculation and new acceleration command value generation module is used to compare the speed value of the nearest point of the reference trajectory with the current vehicle speed to obtain the speed error. The calculation sign is determined according to the driving direction. The feedback acceleration is calculated through noise filtering and PID, and then added to the feedforward acceleration and limited to obtain the feedforward-feedback composite control acceleration value. The new acceleration command value is then generated through error compensation correction. Slope angle calculation and final acceleration command value generation module: It is used to dynamically select the slope angle for slope compensation based on the set slope source strategy, perform acceleration slope compensation based on the new acceleration command value, and obtain the final acceleration command control output value through smoothing filtering. The above steps are continuously repeated to output the corresponding acceleration command value until the vehicle reaches the destination safely.

[0037] The autonomous driving longitudinal control system based on adaptive preview and dynamic slope compensation provided in this application adopts the autonomous driving longitudinal control method based on adaptive preview and dynamic slope compensation in the above-mentioned embodiments, and can solve the technical problems of existing autonomous driving longitudinal control methods such as unreasonable preview mechanism, inaccurate slope processing, and poor control structure. Compared with the existing technology, the beneficial effects of the autonomous driving longitudinal control system based on adaptive preview and dynamic slope compensation provided in this application are the same as the beneficial effects of the autonomous driving longitudinal control method based on adaptive preview and dynamic slope compensation provided in the above-mentioned embodiments. The other technical features of the autonomous driving longitudinal control system based on adaptive preview and dynamic slope compensation are the same as those disclosed in the above-mentioned embodiments and are not further described here.

[0038] The present application provides an autonomous driving longitudinal control device based on adaptive preview and dynamic slope compensation, wherein the autonomous driving longitudinal control device based on adaptive preview and dynamic slope compensation includes: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the autonomous driving longitudinal control method based on adaptive preview and dynamic slope compensation in the above-mentioned embodiment one.

[0039] like Figure 6In one embodiment of the present invention, a schematic diagram of the structure of an autonomous driving longitudinal control device based on adaptive preview and dynamic slope compensation suitable for implementing the embodiments of the present application is shown. The autonomous driving longitudinal control device based on adaptive preview and dynamic slope compensation in the embodiments of the present application may include, but is not limited to, mobile terminals such as mobile phones, laptop computers, digital broadcast receivers, PDAs (Personal Digital Assistants), PADs (Portable Application Descriptions), PMPs (Portable Media Players), and fixed terminals such as digital TVs and desktop computers. Figure 6 The autonomous driving longitudinal control device based on adaptive preview and dynamic slope compensation shown is only an example and should not bring any limitation to the functions and scope of use of the embodiments of the present application.

[0040] Figure 6 The illustrated autonomous driving longitudinal control device based on adaptive preview and dynamic slope compensation may include a processor 1001 (e.g., a central processing unit, graphics processing unit, etc.), which can perform various appropriate actions and processes based on programs stored in a read-only memory (ROM) 1002 or programs loaded from a storage device 1003 into a machine-readable storage medium (RAM) 1004. RAM 1004 also stores various programs and data required for the operation of the autonomous driving longitudinal control device based on adaptive preview and dynamic slope compensation. Processor 1001, ROM 1002, and RAM 1004 are interconnected via a bus 1005. An input / output (I / O) interface 1006 is also connected to the bus. Typically, the following systems may be connected to the I / O interface 1006: input devices 1007 including, for example, a touchscreen, touchpad, keyboard, mouse, image sensor, microphone, accelerometer, gyroscope, etc.; output devices 1008 including, for example, a liquid crystal display (LCD), speaker, vibrator, etc.; storage system 1003 including, for example, a magnetic tape, hard disk, etc.; and a communication unit 1009. The communication unit 1009 may allow the autonomous driving longitudinal control device based on adaptive preview and dynamic slope compensation to communicate wirelessly or wired with other devices to exchange data. While the figure shows an autonomous driving longitudinal control device based on adaptive preview and dynamic slope compensation with various systems, it should be understood that implementation or presence of all the illustrated systems is not required. More or fewer systems may alternatively be implemented or present.

[0041] In particular, according to the embodiments disclosed in the present application, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, the embodiments disclosed in the present application include a computer program product comprising a computer program carried on a computer-readable medium, the computer program comprising program code for executing the method shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from a network via a communication unit, or installed from a storage device 1003, or installed from a ROM 1002. When the computer program is executed by the processor 1001, the above-mentioned functions defined in the method of the embodiments disclosed in the present application are performed.

[0042] The autonomous driving longitudinal control device based on adaptive preview and dynamic slope compensation provided in this application adopts the autonomous driving longitudinal control method based on adaptive preview and dynamic slope compensation in the above-mentioned embodiment, which can solve the technical problems of existing autonomous driving longitudinal control methods such as unreasonable preview mechanism, inaccurate slope processing, and poor control structure. Compared with the existing technology, the beneficial effects of the autonomous driving longitudinal control device based on adaptive preview and dynamic slope compensation provided in this application are the same as the beneficial effects of the autonomous driving longitudinal control method based on adaptive preview and dynamic slope compensation provided in the above-mentioned embodiment. The other technical features of the autonomous driving longitudinal control device based on adaptive preview and dynamic slope compensation are the same as those disclosed in the above-mentioned embodiment and are not further described here.

[0043] The various parts disclosed in this application can be implemented using hardware, software, firmware, or a combination thereof. In the description of the above embodiments, specific features, structures, materials, or characteristics can be combined in any appropriate manner in any one or more embodiments or examples.

[0044] The present application also provides a computer program product, including a computer program, which, when executed by a processor, implements the steps of the above-mentioned autonomous driving longitudinal control method based on adaptive preview and dynamic slope compensation.

[0045] The computer program product provided in this application addresses the technical issues inherent in existing autonomous driving longitudinal control methods, including inadequate preview mechanisms, inaccurate slope handling, and poor control structures. Compared to existing technologies, the computer program product provided in this application offers the same beneficial effects as the autonomous driving longitudinal control method based on adaptive preview and dynamic slope compensation provided in the aforementioned embodiments, and will not be further elaborated here.

[0046] Obviously, those skilled in the art may make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if such changes and modifications fall within the scope of the claims and their equivalents, the present invention is intended to include such changes and modifications.

Claims

1. An autonomous driving longitudinal control method based on adaptive preview and dynamic slope compensation, characterized in that: The method comprises: Step S10: Obtain reference trajectory point data, chassis feedback data, and vehicle positioning data, and calculate the index value of the vehicle's own position relative to the nearest trajectory point on the reference trajectory through an interpolation algorithm; Step S20: Calculate the curvature value of the nearest point based on the calculated index value and the reference curvature value calculated by three-point circle fitting, and calculate the preview distance based on the current vehicle speed and the front and rear wheelbase of the vehicle; Step S30: Starting from the closest point index of the reference trajectory, the trajectory distance is accumulated, and compared with the preview distance to determine the preview point index, and the velocity value of the closest point of the reference trajectory and the preview point velocity value are extracted, and the feedforward acceleration is calculated in combination with the preview distance; Step S40: Compare the velocity value of the nearest point on the reference trajectory with the current vehicle velocity to obtain a velocity error. The calculation sign is determined according to the driving direction. The feedback acceleration is calculated through noise filtering and PID, and is added to the feedforward acceleration and limited to obtain a feedforward-feedback composite control acceleration value. The new acceleration command value is then generated through error compensation correction. Step S50: Dynamically select the slope angle for slope compensation according to the set slope source strategy, perform acceleration slope compensation for the new acceleration command value, obtain the final acceleration command control output value through smoothing filtering, and continuously loop the above steps to output the corresponding acceleration command value until the vehicle reaches the destination safely.

2. The autonomous driving longitudinal control method based on adaptive preview and dynamic slope compensation according to claim 1, characterized in that: The step of calculating the index value of the nearest trajectory point of the vehicle's own position relative to the reference trajectory by an interpolation algorithm in step S10 includes receiving the reference trajectory point posture data and the vehicle's current posture data, traversing the trajectory points and searching for the nearest point index based on the square of the Euclidean distance and the heading error, when the nearest point is found, obtaining the next closest point index, selecting the closest point before and after the nearest point as the next closest point, calculating the nearest point posture information by linear interpolation and returning the nearest point index value, and when the nearest point is not found, outputting that the nearest point was not found.

3. The autonomous driving longitudinal control method based on adaptive preview and dynamic slope compensation according to claim 1, characterized in that: In step S20, the corresponding reference trajectory curvature value is calculated by three-point circle fitting, and the calculation formula is shown in formula (1): （1） Where (x1, y1), (x2, y2), and (x3, y3) are three points on the reference trajectory, curvature is the calculated curvature value, den is the product of the lengths of the three sides formed by the three points, and area is twice the directed area of the triangle formed by the three points on the reference trajectory.

4. The method for autonomous driving longitudinal control based on adaptive preview and dynamic slope compensation according to claim 1, characterized in that: In step S20, the preview distance is calculated by combining the calculated curvature value, the current vehicle speed, and the front and rear wheelbase of the vehicle, so that the preview distance dynamically adapts to the vehicle speed and curvature. The calculation of the preview distance is positively correlated with the vehicle speed and negatively correlated with the curvature. The calculation formula of the preview distance is shown in formula (2): （2） Where L is the vehicle's front and rear wheelbase, speed_ratio is the vehicle's current speed gain coefficient, vx_abs is the absolute value of the vehicle's current speed, curvature_abs is the absolute value of the curvature of the current reference trajectory point, curvature_gain is the curvature gain coefficient, and ld is the comprehensively calculated preview distance.

5. The method for autonomous driving longitudinal control based on adaptive preview and dynamic slope compensation according to claim 1, characterized in that: In step S30, the trajectory distance is accumulated starting from the nearest point index of the reference trajectory, compared with the preview distance to determine the preview point index, the velocity values of the two points are extracted, and the feedforward acceleration is calculated in combination with the preview distance. The steps include obtaining the preview distance, the reference trajectory point and the nearest point index value, traversing the trajectory points starting from the nearest point and accumulating the distances between the trajectory points, when the accumulated distance is not less than the preview distance, selecting the current point as the preview point and returning the preview point index value, when the trajectory point traversal is completed, selecting the last point as the preview point and returning the preview point index value, when the trajectory point traversal is not completed, continuing to traverse the trajectory points in a loop and accumulating the distances between the trajectory points, and looping through the above steps until the preview point is determined; extracting the velocity value of the nearest point of the reference trajectory and the velocity value of the preview point, and calculating the feedforward acceleration in combination with the preview distance. The calculation formula is shown in formula (3): （3） Where vx_pre is the velocity of the preview point, vx_nearest is the velocity of the nearest point, ld is the preview distance, and ff_acc is the feedforward acceleration value.

6. The method for autonomous driving longitudinal control based on adaptive preview and dynamic slope compensation according to claim 1, characterized in that: In step S40, the speed error is obtained by comparing the speed of the nearest point of the reference trajectory with the current speed of the vehicle. The speed error is obtained by comparing the speed of the nearest point of the reference trajectory with the current speed of the vehicle, and the sign of the speed error value is determined in combination with the vehicle's driving direction. The feedback acceleration value is obtained by filtering the speed error noise and performing PID calculation. The speed error calculation formula is shown in formula (4): （4） Where vx_error is the calculated speed error value, vx_sign is the sign value of the vehicle's driving direction, which is 1 when the vehicle is moving forward and -1 when it is moving backward, vx_nearest is the speed value of the nearest point of the vehicle's reference trajectory, and vx_current is the current speed value of the vehicle. The discrete form of the PID calculation formula is shown in Equation (5): （5） Among them, kp, ki, and kd are proportional, integral, and differential gains respectively, e[n] is the speed error at the current moment, e[n-1] is the speed error at the previous moment, Δt is the control period, and u[n] is the control quantity output at the current moment n. The feedforward acceleration and feedback acceleration are added together, and the feedforward-feedback composite control acceleration value is obtained after the acceleration size is limited. The feedforward-feedback composite control acceleration value is error compensated, and the composite control acceleration value is compared with the current acceleration value of the vehicle. The error is filtered and the control instruction is dynamically corrected according to the error value. Finally, a new acceleration command value is generated. The error compensation dynamic correction formula is shown in Equation (6): （6） Where acc_cur is the current acceleration value of the vehicle, acc_ctrl is the calculated feedforward-feedback composite control acceleration value, acc_error is the acceleration error value, fb_gain is the acceleration feedback gain value, and acc_comp is the acceleration command value after error compensation correction.

7. The method for autonomous driving longitudinal control based on adaptive preview and dynamic slope compensation according to claim 1, characterized in that: In step S50, a slope angle for slope compensation is dynamically selected based on a set slope source strategy. Two slope angle calculation methods are considered, including directly calculating the slope angle from the vehicle posture and calculating the slope angle from the elevation difference of trajectory points. An adaptive speed threshold is set, the slope source is initially set, and it is determined whether the vehicle is in adaptive mode. When the vehicle is in adaptive mode, it is determined whether the current vehicle speed is greater than the adaptive speed threshold. When the current vehicle speed is greater than the adaptive speed threshold, the slope angle is calculated using the elevation difference of the trajectory points. When the current vehicle speed is less than or equal to the adaptive speed threshold, the slope angle is directly calculated from the vehicle posture. When the vehicle is not in adaptive mode, the slope angle is calculated based on the determined slope source.

8. An autonomous driving longitudinal control system based on adaptive preview and dynamic slope compensation, characterized by: The autonomous driving longitudinal control system based on adaptive preview and dynamic slope compensation includes: Data acquisition and nearest trajectory point index value calculation module: used to obtain reference trajectory point data, chassis feedback data and vehicle positioning data, and calculate the index value of the vehicle's own position relative to the nearest trajectory point of the reference trajectory through interpolation algorithm; Reference trajectory curvature value and preview distance calculation module: used to obtain the curvature value of the nearest point based on the calculated index value and the reference curvature value calculated by three-point circle fitting, and calculate the preview distance based on the current vehicle speed and the front and rear wheelbase of the vehicle; Feedforward acceleration calculation module: used to accumulate the trajectory distance starting from the closest point index of the reference trajectory, compare it with the preview distance to determine the preview point index, extract the speed value of the closest point of the reference trajectory and the preview point speed value, and calculate the feedforward acceleration based on the preview distance; The composite control acceleration calculation and new acceleration command value generation module is used to compare the speed value of the nearest point of the reference trajectory with the current vehicle speed to obtain the speed error. The calculation sign is determined according to the driving direction. The feedback acceleration is calculated through noise filtering and PID, and then added to the feedforward acceleration and limited to obtain the feedforward-feedback composite control acceleration value. The new acceleration command value is then generated through error compensation correction. Slope angle calculation and final acceleration command value generation module: It is used to dynamically select the slope angle for slope compensation based on the set slope source strategy, perform acceleration slope compensation based on the new acceleration command value, and obtain the final acceleration command control output value through smoothing filtering. The above steps are continuously repeated to output the corresponding acceleration command value until the vehicle reaches the destination safely.

9. Autonomous driving longitudinal control device based on adaptive preview and dynamic slope compensation, characterized by: The autonomous driving longitudinal control device based on adaptive preview and dynamic slope compensation includes: A memory, a processor, and an autonomous driving longitudinal control program based on adaptive preview and dynamic slope compensation stored in the memory and executable on the processor, wherein the autonomous driving longitudinal control program based on adaptive preview and dynamic slope compensation, when executed by the processor, implements the autonomous driving longitudinal control method based on adaptive preview and dynamic slope compensation as described in any one of claims 1 to 7.

10. A computer program product, characterized in that The computer program product includes an autonomous driving longitudinal control program based on adaptive preview and dynamic slope compensation. When the autonomous driving longitudinal control program based on adaptive preview and dynamic slope compensation is executed by a processor, it implements the autonomous driving longitudinal control method based on adaptive preview and dynamic slope compensation as described in any one of claims 1 to 7.

Images