Lane centerline generation method based on Kalman filter algorithm

Abstract

The present invention relates to a lane centerline generation method based on a Kalman filter algorithm, comprising: (1) clearing or recursively calculating lane centerline scattered points based on acquired vehicle-mounted data; (2) determining lane validity in combination with current road features or lane lines on a certain side to obtain lane line quality; (3) obtaining observed quantities, predicted quantities, and noise data of road features based on a Kalman filter algorithm, and updating and optimizing perception of lane lines on both sides; (4) interpolating the x-value of a lane line on one side to obtain the y-value of the lane line on the other side, performing x-value alignment and lateral offset compensation, and obtaining the lane centerline; (5) smoothing and publishing the obtained lane centerline to provide a planned trajectory line. The present invention also relates to a corresponding device, a processor, and a storage medium thereof. The lane centerline generation method based on a Kalman filter algorithm of the present invention greatly improves the stability and robustness of the lane centering function.

Description

Technical Field

[0001] The present invention relates to the field of intelligent driving, and in particular to the field of lane centerline generation, and specifically refers to a lane centerline generation method, device, processor and computer-readable storage medium thereof based on a Kalman filter algorithm. Background Art

[0002] Currently, in the process of developing the lane centering function, it is necessary to process the perceived lane line to obtain the lane center line, that is, to generate the lane center line. The generated trajectory line needs to be handed over to the downstream control module to control the vehicle so that the vehicle drives near the center of the lane.

[0003] Existing traditional lane centerline generation methods simply add and fuse the perceived lane lines on both sides, or perform some simple filtering on the generated lane centerline. If the perceived lane line detection results have low frequency, lateral jumps, inconsistent lengths, or even frame drops, it is difficult to generate a continuous and smooth lane centerline between frames, causing the downstream control module to control the vehicle to be off-center or even deviate. Summary of the Invention

[0004] The purpose of the present invention is to overcome the shortcomings of the above-mentioned prior art and provide a lane centerline generation method based on the Kalman filter algorithm.

[0005] In order to achieve the above objectives, the lane centerline generation method based on the Kalman filter algorithm of the present invention is as follows:

[0006] The lane centerline generation method based on the Kalman filter algorithm has the following main features:

[0007] (1) Based on the currently acquired vehicle data, the lane centerline scattered points are cleared or recursively processed;

[0008] (2) Determine lane validity based on current road features or lane lines on a certain side to obtain lane line quality;

[0009] (3) Based on the Kalman filter algorithm, the observed, predicted and noise data of road characteristics are obtained, and then the lane lines on both sides are updated and the perception is optimized;

[0010] (4) Based on the x-value interpolation of the lane line on one side, the y-value of the lane line on the other side is obtained, and the x-value alignment and lateral offset compensation are performed to obtain the lane centerline;

[0011] (5) Smooth the obtained lane centerline and publish the data to provide the planned trajectory line.

[0012] Preferably, the step (1) is as follows: obtaining vehicle computer data, and if no prior data exists, directly proceeding to step (2); if the current vehicle computer retains historical frame data, it is necessary to recursively extrapolate the lane line prior information to the vehicle coordinate system of the current frame based on the vehicle kinematic model. The specific processing steps are as follows:

[0013] (1.1) Calculate the relative displacement between two frames: Based on the data from the vehicle speed sensor and yaw rate sensor, calculate the displacement change of the vehicle in the x and y directions by integration ( , ) and heading angle change ;

[0014] (1.2) Coordinate rotation and translation: Lane line scatter sequence in the history frame The calculation formula for coordinate transformation is as follows:

[0015] ;

[0016] in, Represents the lane line scatter points in the current frame's ego-vehicle coordinate system.

[0017] Preferably, the step (2) is: judging whether the lane is valid based on a threshold value according to the lane line confidence data input by vehicle perception, and the specific steps include:

[0018] (2.1) Set the default width threshold range according to the road type [ , ],in represents the minimum width threshold, Indicates the maximum width threshold; if the current road has a certain curvature , based on the real-time curvature Correction threshold: , , and then determine whether the current lane width meets the width threshold requirement in the following way:

[0019] Lane Valid = ;

[0020] in Indicates the lane width at the current vehicle position;

[0021] (2.2) Based on current vehicle speed Query and obtain the minimum lane length threshold , and then determine whether the effective perception length of the current lane line meets the minimum length threshold Require;

[0022] (2.3) Intercept three scattered points that represent the lane line characteristics, including the starting point: the lane line scattered point closest to the vehicle ; End point: the farthest effective point of the lane line ; Midpoint: the longitudinal midpoint of the lane line , for these three scattered points ( , ), k is one of the scattered points, and the points 1m before and after it are taken ( , )and( , ), calculate the heading angle of each scattered point :

[0023] ;

[0024] For these three scattered heading angles Take the average value to get the average heading angle of the lane line , calculate the average heading angle difference between the left and right lane lines , based on the average heading angle difference Whether it is within the threshold is used to determine the consistency of the heading angles of the lane lines on both sides.

[0025] Preferably, the step (3) specifically includes the following steps:

[0026] (3.1) Obtaining the observations in the Kalman filter based on the cubic polynomial coefficients Starting from the rear of the vehicle in the ego-vehicle coordinate system, a sparse point sequence is selected at a preset interval. A piecewise spline curve is constructed based on the cubic polynomial coefficients, enforcing continuity constraints. A dense point sequence is generated by interpolation at a preset interval within each interval. This serves as the Kalman filter observation of the left and right lane lines, achieving smooth expression of lane line features and data densification.

[0027] (3.2) Based on the lane validity and historical frame data, the prediction quantity is generated through four scenarios: lane half-width and lateral offset calculation, curvature compensation and historical data fusion, cubic spline interpolation and historical frame heading angle mean fusion ;

[0028] (3.3) Generate left and right lane line scattered point sequences based on the Kalman filter algorithm: Dynamically calibrate the observation noise matrix based on the performance of the perception hardware and the process noise matrix ;State transition matrix based on constant velocity model Calculate the prior error covariance of the current frame ; and combined with the observation matrix Calculate Kalman gain , and finally fused the predicted amount and observations , output the optimal scattered point sequence of the left and right lane lines, providing highly robust input for lane centerline generation;

[0029] (3.4) Complete the calculation of the current frame, convert the data structure for data release, and prepare a predicted value for the calculation of the next frame.

[0030] More preferably, the step (3.1) is specifically as follows:

[0031] Starting from the rear position of the vehicle in the vehicle coordinate system, a scatter point is selected every preset interval until the end to obtain a sparse point sequence , According to the lane line cubic polynomial coefficient and Calculate and obtain; then perform cubic spline interpolation densification, and the adjacent sparse points and Construct a cubic polynomial as follows:

[0032] ;

[0033] The sparse points satisfy position continuity, first-order derivative continuity, and second-order derivative continuity. The formula is as follows:

[0034] ;

[0035] Interpolate between adjacent sparse points at preset intervals to obtain a dense point sequence , as the observations of the left lane line and the right lane line , to ensure that the scatter points are smooth and accurate.

[0036] More preferably, the step (3.2) is specifically as follows: based on whether the lane line determined in step (2) is valid and whether there is historical frame data, the following scene distinctions are made:

[0037] (3.2.1) Scenario 1: Lane markings are valid but no historical frame data exists;

[0038] Define the lateral distance from the vehicle to the left lane line as , the lateral distance to the right lane line is , then the lane half-width is , according to the lane half-width Lateral offset from the vehicle to the lane centerline Calculate forecast amount ;

[0039] (3.2.2) Scenario 2: Lane markings are valid and historical frame data exists;

[0040] Define road curvature Compensation for lateral offset caused by ,in Indicates the vehicle speed, Represents the time interval between frames, and the optimal estimate of the fusion history frame Compensation for lateral offset Get forecast amount ;

[0041] (3.2.3) Scenario 3: The lane marking is invalid and no historical frame data exists;

[0042] According to the sparse point sequence Perform cubic spline interpolation to obtain a dense point sequence , as a predictor ;

[0043] (3.2.4) Scenario 4: Lane markings are invalid but historical frame data exists;

[0044] Calculate the average heading angle of the first three frames of history , fusion of historical frames optimal estimation and the mean heading angle Get forecast amount ,in Indicates the vehicle speed, Indicates the time interval between frames.

[0045] Preferably, the step (3.3) of generating the scattered point sequence of the left and right lane lines based on the Kalman filter algorithm includes the following steps:

[0046] (3.3.1) Prior error covariance calculation;

[0047] According to the state transfer matrix and the covariance of the posterior error of the previous frame , calculate the current frame prior error covariance ,in is the posterior error covariance of the Kalman filter output of the previous frame, the initial frame According to the uncertainty of the initial state of the lane line, it is set as a diagonal matrix; the state transfer matrix A constant velocity model is used, specifically:

[0048] ;

[0049] in, is the time interval between adjacent frames;

[0050] (3.3.2) Kalman gain calculation;

[0051] By prior error covariance , observation matrix and observation noise , calculate the Kalman gain , where the observation matrix Designed for , used to extract position observations from the state vector;

[0052] (3.3.3) Optimal state estimation;

[0053] The predicted amount obtained in the fusion step (3.3) , the observations obtained in step (3.2) and Kalman gain Calculate the Kalman optimal value of the left and right lane lines in the vehicle coordinate system in the following way: :

[0054] ;

[0055] Among them, Kalman optimal quantity Indicates the scattered points of the lane lines on the left and right sides Value, from which we can conclude that the scattered point sequences of the lane lines on the left and right sides are and ;

[0056] (3.3.4) Posterior Error Covariance Update: Update the error covariance as follows Provides the basis for the next frame calculation:

[0057] ;

[0058] Among them, P t is the posterior error covariance, is the identity matrix.

[0059] Preferably, the step (4) is to dynamically select the reference lane line according to the effective sensing length of the left and right lane lines when obtaining the lane center line, which is divided into the following four scenarios:

[0060] (4.1) Scenario 1: The left lane line dominates;

[0061] When the effective sensing length of the left lane line exceeds the preset length but the right lane line is less than the preset length, the lane centerline is directly generated by lateral offset based on the left lane line. The formula is as follows:

[0062] ;

[0063] The scatter point sequence of the left lane line is , the scattered point sequence of the right lane line is , The center line of the lane sequence, Half the width of the lane;

[0064] (4.2) Scenario 2: The right lane line dominates;

[0065] When the effective perception length of the right lane line exceeds the preset length and the left side is insufficient, the right dominant scene is processed symmetrically, and the y sequence of the lane center line for:

[0066] ;

[0067] (4.3) Scenario 3: Symmetric interpolation processing;

[0068] When the effective sensing length of the lane lines on both sides exceeds the preset length, the left lane line Sequence as the basis, forcibly change the right lane line Sequence aligned to the left Sequence, through linear interpolation algorithm, in each Generate the corresponding right lane line at the position Sequence, find satisfaction The adjacent right scatter index of and , the formula is as follows:

[0069] ;

[0070] in Represents the original scattered point index of the right lane line, Indicates the adjacent index in the right lane line scatter sequence The next scatter point of The subsequent scattered points of the right lane line after interpolation are , the lane lines on the left and right sides After sequence alignment, the y sequence of the lane centerline for:

[0071] ;

[0072] Therefore, the lane centerline scattered point sequence based on the Kalman filter algorithm is .

[0073] The lane centerline generation device based on the Kalman filter algorithm has the following main features:

[0074] a processor configured to execute computer-executable instructions;

[0075] The memory stores one or more computer-executable instructions. When the computer-executable instructions are executed by the processor, the steps of generating the lane center line based on the Kalman filter algorithm are implemented.

[0076] The lane centerline generation processor based on the Kalman filter algorithm has the main feature that the processor is configured to execute computer-executable instructions. When the computer-executable instructions are executed by the processor, the steps of generating the lane centerline based on the Kalman filter algorithm described above are implemented.

[0077] The main feature of the computer-readable storage medium is that a computer program is stored thereon, and the computer program can be executed by a processor to implement the above-mentioned steps of generating the lane centerline based on the Kalman filter algorithm.

[0078] The lane centerline generation method, device, processor and computer-readable storage medium based on the Kalman filter algorithm of the present invention greatly improve the stability and robustness of the lane centering function compared with the existing traditional lane centerline generation method, and can achieve adaptivity. When the lane line is perceived to have large changes, including tidal lanes, blocked lanes and lane line jumping, this technical solution can adaptively generate a stable lane centerline. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 This is a flow chart of the lane centerline generation method based on the Kalman filter algorithm of the present invention. DETAILED DESCRIPTION

[0080] In order to more clearly describe the technical content of the present invention, further description is given below in conjunction with specific embodiments.

[0081] Before describing in detail embodiments according to the present invention, it should be noted that, hereinafter, the terms "comprises," "comprising," or any other variations thereof are intended to cover non-exclusive inclusion, whereby a process, method, article, or apparatus comprising a list of elements includes not only those elements, but also other elements not expressly listed or inherent to such process, method, article, or apparatus.

[0082] See also Figure 1 As shown, the lane centerline generation method based on the Kalman filter algorithm, wherein the method comprises the following steps:

[0083] (1) Based on the currently acquired vehicle data, the lane centerline scattered points are cleared or recursively processed;

[0084] (2) Determine lane validity based on current road features or lane lines on a certain side to obtain lane line quality;

[0085] (3) Based on the Kalman filter algorithm, the observed, predicted and noise data of road characteristics are obtained, and then the lane lines on both sides are updated and the perception is optimized;

[0086] (4) Based on the x-value interpolation of the lane line on one side, the y-value of the lane line on the other side is obtained, and the x-value alignment and lateral offset compensation are performed to obtain the lane centerline;

[0087] (5) Smooth the obtained lane centerline and publish the data to provide the planned trajectory line.

[0088] As a preferred embodiment of the present invention, the step (1) is specifically as follows:

[0089] Obtain vehicle data. If no prior data exists, proceed directly to step (2). If the current vehicle retains historical frame data, the lane line prior information must be recursively transferred to the current frame's ego vehicle coordinate system based on the vehicle kinematic model. The processing steps are as follows:

[0090] (1.1) Calculate the relative displacement between two frames: Based on the data from the vehicle speed sensor and yaw rate sensor, calculate the displacement change of the vehicle in the x and y directions by integration ( , ) and heading angle change ;

[0091] (1.2) Coordinate rotation and translation: Lane line scatter sequence in the history frame The calculation formula for coordinate transformation is as follows:

[0092] ;

[0093] in, Indicates the lane line scatter points in the current frame's ego vehicle coordinate system. If the steering wheel angle change rate exceeds the threshold (calibrated value), lateral acceleration rate of change exceeds (calibrated value), the ADAS function is turned off, the vehicle ahead cuts in, or the driver takes over (such as braking or turning on the turn signal), the lane centerline prior data is cleared.

[0094] As a preferred embodiment of the present invention, the step (2) is: judging whether the lane is valid based on a threshold value according to the lane line confidence data input by vehicle perception, and the specific steps include:

[0095] (2.1) Set the default width threshold range according to the road type [ , ],in represents the minimum width threshold, Indicates the maximum width threshold. For highways, the width threshold range is generally [2.5, 5.0]m. For urban roads, the width threshold range is [2.0, 5.5]m. If the current road has a certain curvature , based on the real-time curvature Correction threshold: , , curvature The larger the value, the greater the lane width fluctuation range allowed for the curve. Then, it is determined whether the current lane width meets the width threshold requirement:

[0096] Lane Valid = ;

[0097] in Indicates the lane width at the current vehicle position;

[0098] (2.2) Based on vehicle speed Query and obtain the minimum lane length threshold , design the following speed threshold correspondence relationship, if the speed Less than 30kph, minimum length threshold =30m; if the vehicle speed Between [30, 60] kph, minimum length threshold =50m, if the vehicle speed Over 60kph, minimum length threshold =80m, and then determine whether the effective perception length of the current lane line meets the minimum length threshold Require;

[0099] (2.3) Intercept three scattered points that represent the lane line characteristics, including the starting point: the lane line scattered point closest to the vehicle , End point: the farthest effective point of the lane line , midpoint: the longitudinal midpoint of the lane line , for these three scattered points ( , ), take the points 1m before and after ( , )and( , ), calculate the heading angle of each scattered point :

[0100] ;

[0101] For these three scattered heading angles Take the average value to get the average heading angle of the lane line , calculate the average heading angle difference between the left and right lane lines , based on the average heading angle difference Whether it is within the threshold is used to determine the consistency of the heading angles of the lane lines on both sides.

[0102] As a preferred embodiment of the present invention, the step (3) specifically includes the following steps:

[0103] (3.1) Obtaining the observations in the Kalman filter based on the cubic polynomial coefficients Starting from x = -4m (behind the rear of the vehicle) in the ego-vehicle coordinate system, a sparse point sequence is selected at 12m intervals. A piecewise spline curve is constructed based on the cubic polynomial coefficients, enforcing continuity constraints. Within each interval, a dense point sequence is interpolated at 1.5m intervals to serve as the Kalman filter observations for the left and right lane lines, achieving smooth expression of lane line features and data densification.

[0104] (3.2) Based on the lane validity and historical frame data, the prediction quantity is generated through four scenarios: lane half-width and lateral offset calculation, curvature compensation and historical data fusion, cubic spline interpolation, and historical frame heading angle mean fusion. ;

[0105] (3.3) Generate left and right lane line scattered point sequences based on the Kalman filter algorithm: Dynamically calibrate the observation noise matrix based on the performance of the perception hardware and the process noise matrix ;State transition matrix based on constant velocity model Calculate the current frame prior error covariance ; Combined with the observation matrix Calculate Kalman gain , realize the weight distribution of proximal dependent observation and distal dependent prediction; finally, the fusion prediction and observations , output the optimal scattered point sequence of the left and right lane lines, providing highly robust input for lane centerline generation;

[0106] (3.4) Complete the calculation of the current frame, convert the data structure for data release, and prepare a predicted value for the calculation of the next frame.

[0107] As a preferred embodiment of the present invention, the step (3.1) is specifically as follows:

[0108] From the vehicle coordinate system = -4m (behind the rear of the vehicle), select a scattered point every 12m, until =56m ends, obtaining the sparse point sequence ,in =-4m, 8m, 20m, 32m, 44m, 56m, According to the lane line cubic polynomial coefficient and Calculated; then perform cubic spline interpolation densification, and the adjacent sparse points and Construct a cubic polynomial:

[0109] ;

[0110] The sparse points satisfy position continuity, first-order derivative continuity, and second-order derivative continuity. The formula is as follows:

[0111] ;

[0112] Interpolate between adjacent sparse points at 1.5m intervals to obtain a dense point sequence , as the observations of the left lane line and the right lane line , to ensure that the scatter points are smooth and accurate.

[0113] As a preferred embodiment of the present invention, the step (3.2) is specifically as follows: based on whether the lane line determined in step (2) is valid and whether there is historical frame data, the following scene distinctions are made:

[0114] (3.2.1) Scenario 1: Lane markings are valid but no historical frame data exists;

[0115] Define the lateral distance from the vehicle to the left lane line as , the lateral distance to the right lane line is , then the lane half-width is , according to the lane half-width Lateral offset from the vehicle to the lane centerline Calculate forecast amount ;

[0116] (3.2.2) Scenario 2: Lane markings are valid and historical frame data exists;

[0117] Define road curvature Compensation for lateral offset caused by ,in Indicates the vehicle speed, Represents the time interval between frames, and the optimal estimate of the fusion history frame Compensation for lateral offset Get forecast amount ;

[0118] (3.2.3) Scenario 3: The lane marking is invalid and no historical frame data exists;

[0119] According to the sparse point sequence Perform cubic spline interpolation to obtain a dense point sequence , as a predictor ;

[0120] (3.2.4) Scenario 4: Lane markings are invalid but historical frame data exists;

[0121] Calculate the average heading angle of the first three frames of history , fusion of historical frames optimal estimation and the mean heading angle Get forecast amount ,in Indicates the vehicle speed, Indicates the time interval between frames.

[0122] As a preferred embodiment of the present invention, the step (3.3) is specifically as follows:

[0123] Observation noise matrix The lane detection performance is calibrated offline based on the perception hardware (such as cameras and radars). The credibility of the measured values ​​is obtained through statistical analysis of historical perception data. The process noise matrix According to the longitudinal distance in the vehicle coordinate system The value is dynamically adjusted using a preset noise lookup table, where The larger the value (farther away from the vehicle position), the greater the process noise. The larger the value, the more confidence the lane line prediction result has attenuated at the far end. The generation of the lane line scattered point sequence on the left and right sides based on the Kalman filter algorithm is divided into the following steps:

[0124] (3.3.1) Prior error covariance calculation;

[0125] According to the state transfer matrix and the covariance of the posterior error of the previous frame , calculate the current frame prior error covariance ,in is the posterior error covariance of the Kalman filter output of the previous frame, the initial frame According to the uncertainty of the initial state of the lane line, it is set as a diagonal matrix; the state transfer matrix In this method, a constant velocity model is adopted, specifically:

[0126] ;

[0127] in, The time interval between adjacent frames reflects the change of lane line status (position and speed) over time;

[0128] (3.3.2) Kalman gain calculation;

[0129] By prior error covariance , observation matrix and observation noise , calculate the Kalman gain , where the observation matrix Designed for , used to extract position observations from the state vector;

[0130] (3.3.3) Optimal state estimation;

[0131] The predicted amount obtained in the fusion step (3.3) , the observations obtained in step (3.2) and Kalman gain , calculate the Kalman optimal quantity corresponding to the lane lines on the left and right sides in the vehicle coordinate system as follows:

[0132] ;

[0133] Among them, Kalman optimal quantity It can represent the scattered points of the lane lines on the left and right sides Value, from which we can conclude that the scattered point sequences of the lane lines on the left and right sides are and ;

[0134] (3.3.4) Posterior Error Covariance Update: Update the error covariance as follows Provides the basis for the next frame calculation:

[0135] ;

[0136] in is the identity matrix. This step ensures that the error covariance gradually converges with the iterative process.

[0137] As a preferred embodiment of the present invention, step (4) is to dynamically select the reference lane line according to the effective sensing length of the left and right lane lines (with a threshold of 30m as the judgment condition) when obtaining the lane center line, which is divided into the following four scenarios:

[0138] (4.1) Scenario 1: The left lane line dominates;

[0139] When the effective sensing length of the left lane line exceeds 30m (calibrable) but the right side is less than 30m, the left lane line is used as the reference and the lane centerline is directly generated by lateral offset. The formula is as follows:

[0140] ;

[0141] The scatter point sequence of the left lane line is , the scattered point sequence of the right lane line is , The center line of the lane sequence, Half the width of the lane;

[0142] (4.2) Scenario 2: The right lane line dominates;

[0143] When the effective perception length of the right lane line exceeds 30m (calibrable) and the left side is less than 30m, the right dominant scene is processed symmetrically, and the y sequence of the lane centerline is for:

[0144] ;

[0145] (4.3) Scenario 3: Symmetric interpolation processing;

[0146] When the effective sensing length of the lane lines on both sides exceeds 30m (calibrable), the left lane line Sequence as the basis, forcibly change the right lane line Sequence aligned to the left Sequence, through linear interpolation algorithm, in each Generate the corresponding right lane line at the position Sequence, the formula is as follows:

[0147] ;

[0148] in , is the original scattered point sequence of the right lane line, and the scattered point sequence of the right lane line after interpolation is , the lane lines on the left and right sides After sequence alignment, the y sequence of the lane centerline for:

[0149] ;

[0150] Therefore, the lane centerline scattered point sequence based on the Kalman filter algorithm is .

[0151] The technical solution will be further described in detail below. Figure 1 The specific processing steps are as follows:

[0152] 1) Clear and recurse lane centerline scattered points. If no prior data exists, proceed directly to the next step. If historical frame data is retained, the lane centerline prior information needs to be recursively transferred to the current frame's ego vehicle coordinate system. If a function is turned off, the preceding vehicle cuts in, or the turn signal is activated, the lane centerline prior data is cleared.

[0153] 2) Determining the validity of a road feature or lane marking on one side. This judgment involves: determining a threshold based on the perceived lane line confidence level and different traffic scenarios; calculating whether the lane width meets the corresponding threshold; obtaining the corresponding threshold from a speed table lookup to determine whether the lane line length on one side is within the threshold; and intercepting three scattered points representing the lane line characteristics (starting point, midpoint, and end point), calculating the average heading angle of these scattered points, obtaining a consistency threshold from a table lookup based on the lane line curvature information, and determining whether the average heading angle error of the lane lines on both sides is within the threshold. Only lane lines that have passed the validity judgment can enter the filtering operation. This operation not only determines the lane line quality but also prevents noise from contaminating the generated lane centerline.

[0154] 3) Update and optimize the perceived lane lines based on the Kalman filter algorithm. The filtering and optimization methods used in this technical solution include:

[0155] Obtaining Observations in Kalman Filtering Based on Cubic Polynomial Coefficients In one embodiment, the x-values ​​in the vehicle coordinate system are sparsely sampled, starting from -4 and selecting a point every 12 points up to 56, for a total of 6 points. Then, a densification process is performed, and cubic spline interpolation is performed every 1.5 between two points to ensure smooth and accurate scatter points.

[0156] Whether the prediction quantity in Kalman filter is effectively obtained based on road characteristics : If the road features are valid but there is no historical frame data, first use the sum of the distance between the vehicle and the lane line and the lane centerline as the lane half-width, and use the lane half-width minus the distance between the vehicle and the lane centerline as the predicted value If the road features are valid and there is historical frame data, the lateral error caused by the road features plus the historical frame data is used as the prediction amount. If the road features are invalid and there is no historical frame data, then the cubic spline interpolation is performed directly based on 6 points. If the road features are invalid and there is historical frame data, then the optimal value of the previous frame is used to obtain the predicted value based on the second-order derivative or the average value of the yaw angle at the first three points. ;

[0157] Acquisition of observation noise and process noise: The observation noise R is based on the performance calibration of the perceived lane line, and the process noise Q is based on the x value in the vehicle coordinate system. The larger the x value, the larger the process noise Q. The state transfer matrix F and the observation matrix H are both unit matrices, and the Kalman gain is calculated. As the x value in the vehicle coordinate system increases, it presents an inverted triangle characteristic. Based on the obtained observation , predicted amount , and the Kalman gain Calculate the Kalman optimal value corresponding to the x value in the vehicle coordinate system :

[0158] ;

[0159] ;

[0160] ;

[0161] Completes calculations for the current frame, converts data structures for data distribution, and prepares a prediction for the next frame.

[0162] 4) Interpolate the y value of the lane marking on one side based on the x value of the other side: When obtaining the lane centerline, the x and y values ​​in the vehicle coordinate system are not simply added together. Instead, the x values ​​of the lane markings on both sides are aligned, the corresponding y values ​​are linearly interpolated, and then the y values ​​are added based on the same x value. If one lane marking on one side is shorter, the lane centerline is obtained by applying a lateral offset based on the lane width based on the longer lane marking.

[0163] 5) Complete the acquisition of lane centerlines and perform smoothing for data publishing to provide trajectories for downstream control modules.

[0164] Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, segment or portion of code comprising one or more executable instructions for implementing the steps of a specific logical function or process, and the scope of the preferred embodiments of the present invention includes alternative implementations in which functions may be performed out of the order shown or discussed, including performing functions in a substantially simultaneous manner or in the reverse order depending on the functions involved, which should be understood by those skilled in the art to which the embodiments of the present invention pertain.

[0165] It should be understood that each part of the present invention can be implemented by hardware, software, firmware or a combination thereof. In the above embodiments, multiple steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution device.

[0166] Those skilled in the art will understand that all or part of the steps of the method for implementing the above-mentioned embodiment can be completed by instructing the relevant hardware through a program, and the program can be stored in a computer-readable storage medium. When the program is executed, it includes one of the steps of the method embodiment or a combination thereof.

[0167] The storage medium mentioned above can be a read-only memory, a magnetic disk or an optical disk, etc.

[0168] Throughout this specification, references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "embodiment" indicate that the specific features, structures, materials, or characteristics described in conjunction with that embodiment or example are included in at least one embodiment or example of the present invention. In this specification, schematic representations of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.

[0169] Although the embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and are not to be construed as limitations on the present invention. A person skilled in the art may change, modify, replace and modify the above embodiments within the scope of the present invention.

[0170] In practical application, this technical solution has the following technical effects:

[0171] (1) The lane centerline generation method based on the Kalman filter algorithm in this technical solution greatly improves the stability and robustness of the lane centering function compared to existing traditional lane centerline generation methods. It can achieve self-adaptation. When the lane line undergoes significant changes, including tidal lanes, blocked lanes, and lane line jumps, this algorithm can adaptively generate a stable lane centerline.

[0172] (2) In this technical solution, the validity of the road features and lane lines input by perception is judged, and lane lines that do not meet the judgment conditions are warned and pre-processed. This can be fed back to perception improvement and prevent data contamination. At the same time, in order to ensure the continuity of the lane centerline result output, historical frame data is used for data recursion, and finally a low-contamination, continuous and smooth trajectory line is output, which is more friendly to the downstream control module.

[0173] (3) In this technical solution, after sparse value acquisition based on cubic polynomial, the points are densely processed based on cubic spline interpolation between scattered points. This ensures that the measured values ​​are consistent with the upstream input and can also make the observed values ​​have good continuity and smoothness;

[0174] (4) In this technical solution, the road features input from upstream or the saved historical data are used as the prediction quantity, and the process noise is obtained by looking up the table based on the x value in the vehicle coordinate system. On the one hand, multiple data can be integrated to improve the lane line confidence. On the other hand, the result of the previous frame can be recursively transferred to the current frame to maintain the continuity between frames. At the same time, because the calculated Kalman gain presents an inverted triangle shape, the lane line close to the vehicle will not jump significantly due to the jump of the perception data, and the lane line far from the vehicle is more consistent with the perception result, ensuring the stability of the lane centering function.

[0175] (5) In this technical solution, when obtaining the lane centerline coordinates, the x values ​​of the lane lines on both sides need to be aligned to ensure the accuracy of the results. At the same time, when the lane line on one side is temporarily lost due to occlusion or other reasons, the centerline is generated by offsetting the lane line on the other side by a certain distance, thus effectively ensuring robustness.

[0176] In this specification, the present invention has been described with reference to specific embodiments thereof. However, it will be apparent that various modifications and variations may be made without departing from the spirit and scope of the present invention. Accordingly, the specification and drawings are to be regarded as illustrative rather than restrictive.

Claims

1. A lane centerline generation method based on Kalman filter algorithm, characterized in that: The method comprises the following steps: (1) Based on the currently acquired vehicle data, the lane centerline scattered points are cleared or recursively processed. Specifically, the vehicle data is obtained. If there is no prior data, go directly to step (2); if the current vehicle data is retained, the lane line prior information is recursively transferred to the current frame's ego vehicle coordinate system based on the vehicle kinematic model. If the steering wheel angle change rate exceeds the threshold, the lateral acceleration change rate exceeds the calibration value, the ADAS function is turned off, the preceding vehicle cuts in, or the driver is detected to take over, the lane centerline prior data is cleared. (2) Determine lane validity based on the current road characteristics or lane markings on a certain side to obtain lane marking quality, including: setting a default width threshold range based on the road type, and if the current road has a certain curvature, correcting the threshold based on the real-time curvature; (3) Based on the Kalman filter algorithm, the observation, prediction and noise data of road features are obtained, and then the lane lines on both sides are updated and the perception is optimized. Specifically, the observation in the Kalman filter is obtained based on the cubic polynomial coefficients; based on the lane line validity and historical frame data, the prediction is generated through four scenarios: lane half-width and lateral offset calculation, curvature compensation and historical data fusion, cubic spline interpolation and historical frame heading angle mean fusion; and the observation noise matrix and process noise matrix are dynamically calibrated based on the performance of the perception hardware; (4) Based on the x-value interpolation of the lane line on one side, the y-value of the lane line on the other side is obtained, and the x-value alignment and lateral offset compensation are performed to obtain the lane centerline. Specifically, when obtaining the lane centerline, the reference lane line is dynamically selected based on the effective sensing length of the left and right lane lines, which is divided into the following four scenarios: (4.1) Scenario 1: The left lane line dominates; When the effective sensing length of the left lane line exceeds the preset length but the right lane line is less than the preset length, the lane centerline is directly generated by lateral offset based on the left lane line. The formula is as follows: ； The scatter point sequence of the left lane line is , the scattered point sequence of the right lane line is , The center line of the lane sequence, Half the width of the lane; (4.2) Scenario 2: The right lane line dominates; When the effective perception length of the right lane line exceeds the preset length and the left side is insufficient, the right dominant scene is processed symmetrically, and the y sequence of the lane center line for: ； (4.3) Scenario 3: Symmetric interpolation processing; When the effective sensing length of the lane lines on both sides exceeds the preset length, the left lane line Sequence as the basis, forcibly change the right lane line Sequence aligned to the left Sequence, through linear interpolation algorithm, in each Generate the corresponding right lane line at the position Sequence, find satisfaction The adjacent right scatter index of and , the formula is as follows: ； in Represents the original scattered point index of the right lane line, Indicates the adjacent index in the right lane line scatter sequence The next scatter point of The subsequent scattered points of the right lane line after interpolation are , the lane lines on the left and right sides After sequence alignment, the y sequence of the lane centerline for: ； Therefore, the lane centerline scattered point sequence based on the Kalman filter algorithm is ; (5) Smooth the obtained lane centerline and publish the data to provide the planned trajectory line.

2. The lane centerline generation method based on the Kalman filter algorithm according to claim 1, characterized in that: The specific processing steps of step (1) are as follows: (1.1) Calculate the relative displacement between two frames: Based on the data from the vehicle speed sensor and yaw rate sensor, calculate the displacement change of the vehicle in the x and y directions by integration ( , ) and heading angle change ; (1.2) Coordinate rotation and translation: Lane line scatter sequence in the history frame The calculation formula for coordinate transformation is as follows: ； in, Represents the lane line scatter points in the current frame's ego-vehicle coordinate system.

3. The lane centerline generation method based on the Kalman filter algorithm according to claim 2, characterized in that: The step (2) is to determine whether the lane is valid based on the threshold according to the lane line confidence data input by the vehicle perception. The specific steps include: (2.1) Set the default width threshold range according to the road type [ , ],in represents the minimum width threshold, Indicates the maximum width threshold; if the current road has a certain curvature , based on the real-time curvature Correction threshold: , , and then determine whether the current lane width meets the width threshold requirement in the following way: Lane Valid = ; in Indicates the lane width at the current vehicle position; (2.2) Based on current vehicle speed Query and obtain the minimum lane length threshold , and then determine whether the effective perception length of the current lane line meets the minimum length threshold Require; (2.3) Intercept three scattered points that represent the lane line characteristics, including the starting point: the lane line scattered point closest to the vehicle ; End point: the farthest effective point of the lane line ; Midpoint: the longitudinal midpoint of the lane line , for these three scattered points ( , ), k is one of the scattered points, and the points 1m before and after it are taken ( , )and( , ), calculate the heading angle of each scattered point : ； For these three scattered heading angles Take the average value to get the average heading angle of the lane line , calculate the average heading angle difference between the left and right lane lines , based on the average heading angle difference Whether it is within the threshold is used to determine the consistency of the heading angles of the lane lines on both sides.

4. The lane centerline generation method based on the Kalman filter algorithm according to claim 1, characterized in that: The step (3) specifically includes the following steps: (3.1) Obtaining the observations in the Kalman filter based on the cubic polynomial coefficients Starting from the rear of the vehicle in the ego-vehicle coordinate system, a sparse point sequence is selected at a preset interval. A piecewise spline curve is constructed based on the cubic polynomial coefficients, enforcing continuity constraints. A dense point sequence is generated by interpolation at a preset interval within each interval. This serves as the Kalman filter observation of the left and right lane lines, achieving smooth expression of lane line features and data densification. (3.2) Based on the lane validity and historical frame data, the prediction quantity is generated through four scenarios: lane half-width and lateral offset calculation, curvature compensation and historical data fusion, cubic spline interpolation and historical frame heading angle mean fusion ; (3.3) Generate left and right lane line scattered point sequences based on the Kalman filter algorithm: Dynamically calibrate the observation noise matrix based on the performance of the perception hardware and the process noise matrix ;State transition matrix based on constant velocity model Calculate the prior error covariance of the current frame ; and combined with the observation matrix Calculate Kalman gain , and finally fusion prediction and observations , output the optimal scattered point sequence of the left and right lane lines, providing highly robust input for lane centerline generation; (3.4) Complete the calculation of the current frame, convert the data structure for data release, and prepare a predicted value for the calculation of the next frame.

5. The lane centerline generation method based on the Kalman filter algorithm according to claim 4 is characterized in that: The steps (3.1) are specifically as follows: Starting from the rear position of the vehicle in the vehicle coordinate system, a scatter point is selected every preset interval until the end to obtain a sparse point sequence , According to the lane line cubic polynomial coefficient and Calculate and obtain; then perform cubic spline interpolation densification, and the adjacent sparse points and Construct a cubic polynomial as follows: ； The sparse points satisfy position continuity, first-order derivative continuity, and second-order derivative continuity. The formula is as follows: ； Interpolate between adjacent sparse points at preset intervals to obtain a dense point sequence , as the observations of the left lane line and the right lane line , to ensure that the scatter points are smooth and accurate.

6. The lane centerline generation method based on the Kalman filter algorithm according to claim 5, characterized in that: The step (3.2) is specifically as follows: based on whether the lane line determined in step (2) is valid and whether there is historical frame data, the following scene distinctions are made: (3.2.1) Scenario 1: Lane markings are valid but no historical frame data exists; Define the lateral distance from the vehicle to the left lane line as , the lateral distance to the right lane line is , then the lane half-width is , according to the lane half-width Lateral offset from the vehicle to the lane centerline Calculate forecast amount ; (3.2.2) Scenario 2: Lane markings are valid and historical frame data exists; Define road curvature Compensation for lateral offset caused by ,in Indicates the vehicle speed, Represents the time interval between frames, and the optimal estimate of the fusion history frame Compensation for lateral offset Get forecast amount ; (3.2.3) Scenario 3: The lane marking is invalid and no historical frame data exists; According to the sparse point sequence Perform cubic spline interpolation to obtain a dense point sequence , as a predictor ; (3.2.4) Scenario 4: Lane markings are invalid but historical frame data exists; Calculate the average heading angle of the first three frames of history , fusion of historical frames optimal estimation and the mean heading angle Get forecast amount ,in Indicates the vehicle speed, Indicates the time interval between frames.

7. The lane centerline generation method based on the Kalman filter algorithm according to claim 6, characterized in that: The step (3.3) of generating the scattered point sequence of the left and right lane lines based on the Kalman filter algorithm includes the following steps: (3.3.1) Prior error covariance calculation; According to the state transfer matrix and the covariance of the posterior error of the previous frame , calculate the current frame prior error covariance ,in is the posterior error covariance of the Kalman filter output of the previous frame, the initial frame According to the uncertainty of the initial state of the lane line, it is set as a diagonal matrix; the state transfer matrix A constant velocity model is used, specifically: ； in, is the time interval between adjacent frames; (3.3.2) Kalman gain calculation; By prior error covariance , observation matrix and observation noise , calculate the Kalman gain , where the observation matrix Designed for , used to extract position observations from the state vector; (3.3.3) Optimal state estimation; The predicted amount obtained in the fusion step (3.3) , the observations obtained in step (3.2) and Kalman gain Calculate the Kalman optimal value of the left and right lane lines in the vehicle coordinate system in the following way: : ； Among them, Kalman optimal quantity Indicates the scattered points of the lane lines on the left and right sides Value, from which we can conclude that the scattered point sequences of the lane lines on the left and right sides are and ; (3.3.4) Posterior Error Covariance Update: Update the error covariance as follows Provides the basis for the next frame calculation: ； Among them, P t is the posterior error covariance, is the identity matrix.

8. A lane centerline generation device based on Kalman filter algorithm, characterized in that: The device comprises: a processor configured to execute computer-executable instructions; A memory storing one or more computer-executable instructions, wherein when the computer-executable instructions are executed by the processor, the steps of generating a lane centerline based on a Kalman filter algorithm according to any one of claims 1 to 7 are implemented.

9. A lane centerline generation processor based on Kalman filter algorithm, characterized in that: The processor is configured to execute computer-executable instructions. When the computer-executable instructions are executed by the processor, the steps of generating a lane centerline based on a Kalman filter algorithm according to any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium, characterized in that A computer program is stored thereon, and the computer program can be executed by a processor to implement the steps of generating a lane centerline based on a Kalman filter algorithm as described in any one of claims 1 to 7.

Images