Lane center line generation method based on Kalman filtering algorithm

Abstract

The invention relates to a lane center line generation method based on a Kalman filtering algorithm, and the method comprises the steps: (1) carrying out the clearing or recursion of scattered points of a lane center line based on obtained vehicle data; (2) lane validity judgment is carried out in combination with current road features or a lane line on one side, and lane line quality is obtained; (3) obtaining observed quantity, pre-measured quantity and noise data of road characteristics based on a Kalman filtering algorithm, and carrying out updating and perceptual optimization on lane lines on two sides; (4) obtaining a y value of a lane line on the other side based on x value interpolation of the lane line on one side, and performing x value alignment and lateral offset compensation to obtain a lane center line; and (5) carrying out smoothing processing and data publishing on the obtained lane center line, and providing a planning trajectory. The invention further relates to a corresponding device, a processor and a storage medium thereof. By adopting the lane center line generation method based on the Kalman filtering algorithm, the stability and robustness of a lane centering function are greatly improved.

Description

Technical Field

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

[0002] Currently, during the development of the lane centering function, it is necessary to process the perceived lane lines to obtain the lane center line, that is, the generation of 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 travels near the center of the lane.

[0003] The existing traditional methods for generating lane center lines only add and fuse the perceived lane lines on both sides, or perform some simple filtering on the generated lane center line. If the detection results of the perceived lane lines have a low frequency, lateral jitter, inconsistent lengths, or even frame drops, it is very difficult to generate a continuous and smooth lane center line between frames, resulting in the downstream control module not centering or even deviating when controlling the vehicle. Summary of the Invention

[0004] The object of the present invention is to overcome the above-mentioned disadvantages of the prior art and provide a method for generating a lane center line based on the Kalman filtering algorithm.

[0005] In order to achieve the above object, the method for generating a lane center line based on the Kalman filtering algorithm of the present invention is as follows: The method for generating a lane center line based on the Kalman filtering algorithm is mainly characterized in that the method includes the following steps: (1) Based on the vehicle data obtained currently, clear or recursively process the lane center line scatter points; (2) Combine the current road characteristics or a certain side lane line to judge the lane validity and obtain the lane line quality; (3) Based on the Kalman filtering algorithm, obtain the observed value, predicted value and noise data of the road characteristics, and then update and perceptually optimize the lane lines on both sides; (4) Interpolate the y value of the other side lane line based on the x value of one side lane line, perform x value alignment and lateral offset compensation to obtain the lane center line; (5) Smoothly process and publish the obtained lane center line data to provide a planned trajectory line.

[0006] Preferably, step (1) is as follows: Obtain the vehicle data. If there is no prior data, directly enter step (2); if the current vehicle retains historical frame data, it is necessary to recursively deduce the lane line prior information to the ego-vehicle coordinate system of the current frame based on the vehicle kinematic model. The specific processing steps are as follows: (1.1) Calculate the relative displacement between two frames: Based on the data from the vehicle speed sensor and the yaw rate sensor, calculate the displacement changes of the vehicle in the x and y directions ( , ) and the change in the heading angle ; (1.2) Coordinate rotation and translation: For the sequence of lane line scatter points in the historical frame , the calculation formula for coordinate transformation is as follows: ; Among them, represents the lane line scatter points in the vehicle coordinate system of the current frame.

[0007] Preferably, step (2) is: Based on the lane line confidence data input by vehicle perception, determine whether the lane is valid based on a threshold. The specific steps include: (2.1) Set the default width threshold range according to the road type , , where represents the minimum width threshold, represents the maximum width threshold; if there is a certain curvature in the current road, then correct the threshold based on the real-time curvature : , , and then determine whether the current lane width meets the width threshold requirement in the following way: Lane valid = ; Among them represents the lane width at the current vehicle position; (2.2) Query and obtain the minimum length threshold of the lane line based on the current vehicle speed , and then determine whether the effective perception length of the current lane line meets the minimum length threshold requirement; (2.3) Intercept three scatter points representing the lane line features, including the starting point: the lane line scatter point closest to the vehicle ; the ending point: the farthest valid point of the lane line ; the midpoint: the longitudinal midpoint of the lane line . For these three scatter points ( , ), where k is one of the scatter points, take the points 1m before and after each of them ( , ) and ( , ), and calculate the heading angle of each scatter point: ; For these three scatter heading angles Take the average value to obtain the average heading angle of this lane line , calculate the difference between the average heading angles of the left and right lane lines , based on the difference between the average heading angles Determine whether it is within the threshold to judge the consistency of the heading angles of the two side lane lines

[0008] Preferably, step (3) specifically includes the following steps (3.1) Obtain the observation quantity in the Kalman filter based on the coefficients of the cubic polynomial : Starting from the position behind the vehicle tail in the vehicle coordinate system, select a sparse point sequence at a preset interval. Based on the coefficients of the cubic polynomial, construct a piecewise spline curve, force it to satisfy the continuity constraint, and interpolate at a preset interval within each interval to generate a dense point sequence as the Kalman filter observation quantity of the left and right lane lines, realizing the smooth expression of the lane line features and data densification (3.2) Generate the predicted value through four types of scenarios: calculation of the lane half-width and lateral offset, curvature compensation and historical data fusion, cubic spline interpolation, and fusion of the average historical frame heading angle, based on the lane line validity and historical frame data ; (3.3) Generate the scatter point sequences of the left and right lane lines based on the Kalman filter algorithm: Dynamically calibrate the observation noise matrix and the process noise matrix based on the performance of the perception hardware Calculate the prior error covariance of the current frame based on the state transition matrix of the constant velocity model ; And calculate the Kalman gain in combination with the observation matrix , and finally fuse the predicted value and the observation quantity , output the optimal scatter point sequences of the left and right lane lines, providing a highly robust input for the generation of the lane center line (3.4) Complete the calculation of the current frame, convert the data structure for data publishing, and prepare a predicted value for the calculation of the next frame

[0009] More preferably, step (3.1) is specifically Starting from the position behind the vehicle tail in the vehicle coordinate system, select a scatter point at every preset interval number until the end to obtain a sparse point sequence , Obtained according to the coefficients of the cubic polynomial of the lane line and ; Then perform cubic spline interpolation for densification. For adjacent sparse points and construct a cubic polynomial in the following manner ; Among them, the sparse points satisfy position continuity, first derivative continuity, and second derivative continuity, and the formula is as follows: ; Interpolate at a preset interval between adjacent sparse points to obtain a dense point sequence , which is used as the observation quantity of the left and right lane lines , so as to ensure that the scattered points are smooth and accurate.

[0010] More preferably, the step (3.2) is specifically: based on whether the lane line judged in step (2) is valid and whether there is historical frame data, the following scene classification is performed: Scene 1: The lane line is valid but there is no historical frame data; Define the lateral distance from the vehicle to the left lane line as , and the lateral distance to the right lane line as , then the lane half-width is . According to the lane half-width and the lateral offset from the vehicle to the lane center line, calculate the prediction value ; Scene 2: The lane line is valid and there is historical frame data; Define the lateral offset compensation amount caused by the road curvature , where represents the vehicle speed, represents the frame interval time, fuse the optimal estimate of the historical frame and the lateral offset compensation amount to obtain the prediction value ; Scene 3: The lane line is invalid and there is no historical frame data; Perform cubic spline interpolation on the sparse point sequence to obtain the dense point sequence , which is used as the prediction value ; Scene 4: The lane line is invalid but there is historical frame data; Calculate the average heading angle of the previous three historical frames, fuse the optimal estimate of the historical frame and the average heading angle to obtain the prediction value , where represents the vehicle speed, represents the frame interval time.

[0011] Preferably, step (3.3) of generating the scattered point sequences of the left and right lane lines based on the Kalman filter algorithm includes the following steps: (3.3.1) Calculation of the prior error covariance; According to the state transition matrix and the posterior error covariance of the previous frame , calculate the prior error covariance of the current frame , where is the posterior error covariance output by the Kalman filter of the previous frame, and the initial frame is set as a diagonal matrix according to the initial state uncertainty of the lane line; the state transition matrix adopts a constant velocity model, specifically: ; where, is the time interval between adjacent frames; (3.3.2) Calculation of the Kalman gain; Through the prior error covariance , the observation matrix and the observation noise , calculate the Kalman gain , where the observation matrix is designed as , and is used to extract the position observation from the state vector; (3.3.3) Optimal state estimation; Fuse the predicted value obtained in step (3.3), the observed value obtained in step (3.2), and the Kalman gain , and calculate the Kalman optimal quantity corresponding to the left and right lane lines in the vehicle coordinate system according to the following method : ; where, the Kalman optimal quantity represents the value in the scattered points of the left and right lane lines, and the scattered point sequences of the left and right lane lines are respectively and ; (3.3.4) Update of the posterior error covariance: Update the error covariance to provide a basis for the calculation of the next frame: ; where, P t is the posterior error covariance, is the identity matrix.

[0012] Preferably, in step (4) of obtaining the lane center line, the reference lane line is dynamically selected according to the effective sensing lengths of the left and right lane lines, which is divided into the following four scenarios: (4.1) Scenario 1: The left lane line is dominant; When the effective sensing length of the left lane line exceeds the preset length while the right one is insufficient, the left lane line is taken as the reference, and the lane center line is directly generated by lateral offset. The formula is as follows: ; where the scatter point sequence of the left lane line is , and the scatter point sequence of the right lane line is , is the sequence of the lane center line, is the lane half-width; (4.2) Scenario 2: The right lane line is dominant; When the effective sensing length of the right lane line exceeds the preset length while the left one is insufficient, the right-dominant scenario is symmetrically processed. The y sequence of the lane center line is: ; (4.3) Scenario 3: Symmetric interpolation processing; When the effective sensing lengths of both the left and right lane lines exceed the preset length, taking the sequence of the left lane line as the reference, the sequence of the right lane line is forced to be aligned with the left sequence. Through the linear interpolation algorithm, the corresponding position of the right lane line is generated for each sequence, and the adjacent right scatter point indices and and satisfying ; where represents the original scatter point index of the right lane line, represents the next scatter point adjacent to the index in the scatter point sequence of the right lane line, that is, the successor scatter point of . The scatter point sequence of the right lane line after interpolation processing is . After aligning the sequences of the left and right lane lines, the y sequence of the lane center line is: ; Thus, the scatter point sequence of the lane center line based on the Kalman filter algorithm is .

[0013] The lane centerline generation device based on the Kalman filter algorithm is mainly characterized in that the device includes: A processor configured to execute computer-executable instructions; A memory storing one or more computer-executable instructions, which, when executed by the processor, implement the steps of the above-mentioned lane centerline generation based on the Kalman filter algorithm.

[0014] The lane centerline generation processor based on the Kalman filter algorithm is mainly characterized in that the processor is configured to execute computer-executable instructions, which, when executed by the processor, implement the steps of the above-mentioned lane centerline generation based on the Kalman filter algorithm.

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

[0016] By adopting the lane centerline generation method, device, processor and computer-readable storage medium based on the Kalman filter algorithm of the present invention, compared with the existing traditional lane centerline generation methods, the stability and robustness of the lane centering function are greatly improved, and adaptability can be achieved. When there are large changes in the perceived lane lines, including the appearance of tidal lanes, blocked lanes, and lane line jumps, etc., the technical solution of the present invention can adaptively generate stable lane centerlines. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 It is a flowchart of the lane centerline generation method based on the Kalman filter algorithm of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0018] In order to be able to more clearly describe the technical content of the present invention, the following will be further described in conjunction with specific embodiments.

[0019] Before detailing the embodiments according to the present invention, it should be noted that, hereinafter, the terms "comprising", "including" or any other variant are intended to cover non-exclusive inclusion, such that a process, method, article or device comprising a series of elements not only includes those elements but also includes other elements not expressly listed, or elements inherent to such process, method, article or device.

[0020] Please refer to Figure 1 As shown, the lane centerline generation method based on the Kalman filter algorithm, wherein the method includes the following steps: (1)Based on the currently obtained in-vehicle data, clear or recursively process the lane centerline scatter points; (2)Combine the current road characteristics or a certain lane line to judge the lane validity and obtain the lane line quality; (3)Based on the Kalman filter algorithm, obtain the observed values, predicted values, and noise data of the road characteristics, and then update and optimize the perception of the lane lines on both sides; (4)Interpolate the y value of the other lane line based on the x value of one side lane line, perform x value alignment and lateral offset compensation to obtain the lane centerline; (5)Smoothly process and publish the obtained lane centerline data to provide a planned trajectory line.

[0021] As a preferred embodiment of the present invention, step (1) is specifically as follows: Obtain the in-vehicle data. If there is no prior data, directly enter step (2); if the current in-vehicle retains historical frame data, the prior information of the lane line needs to be recursively pushed to the ego vehicle coordinate system of the current frame based on the vehicle kinematic model. The processing steps are as follows: (1.1)Calculate the relative displacement between two frames: Based on the data of the vehicle speed sensor and the yaw rate sensor, calculate the displacement change amounts of the vehicle in the x and y directions ( , ) and the heading angle change amount ; (1.2)Coordinate rotation and translation: For the lane line scatter point sequence in the historical frame, the calculation formula for coordinate transformation is as follows: ; Among them, represents the lane line scatter points in the ego vehicle coordinate system of the current frame. If the steering wheel angle change rate exceeds the threshold (calibration value), the lateral acceleration change rate exceeds (calibration value), the ADAS function is turned off, a vehicle cuts in front, or driver takeover is detected (such as stepping on the brake or turning on the turn signal), the prior data of the lane centerline is cleared.

[0022] As a preferred embodiment of the present invention, step (2) is: Based on the lane line confidence data input by vehicle perception, judge whether the lane is valid based on a threshold. The specific steps include: (2.1)Set the default width threshold range , according to the road type, where represents the minimum width threshold, Denote the maximum width threshold. For highways, the general width threshold range is [2.5, 5.0] m. For urban roads, the width threshold range is [2.0, 5.5] m. If there is a certain curvature in the current road , then based on the real-time curvature Correct the threshold: , , the larger the curvature , the larger the allowable lane width fluctuation range for the curve. Then, determine whether the current lane width meets the width threshold requirement: Lane valid = ; where denotes the lane width at the current vehicle position; (2.2) Based on the vehicle speed Query and obtain the minimum length threshold of the lane line , design the following corresponding relationship between vehicle speed thresholds. If the vehicle speed is less than 30 kph, the minimum length threshold = 30 m; if the vehicle speed is between [30, 60] kph, the minimum length threshold = 50 m. If the vehicle speed exceeds 60 kph, the minimum length threshold = 80 m. Then, determine whether the effective perception length of the current lane line meets the minimum length threshold requirement; (2.3) Intercept three scatter points representing the lane line features, including the starting point: the scatter point of the lane line closest to the vehicle , the ending point: the farthest valid point of the lane line , the midpoint: the longitudinal midpoint of the lane line . For these three scatter points ( , ), respectively take the points 1 m before and after each of them ( , ) and ( , ), and calculate the heading angle of each scatter point: ; Take the average value of the heading angles of these three scatter points to obtain the average heading angle of the lane line , calculate the difference between the average heading angles of the left and right lane lines, and based on whether the difference is within the threshold, judge the consistency of the heading angles of the two side lane lines.

[0023] As a preferred embodiment of the present invention, step (3) specifically includes the following steps: (3.1) Obtain the observed quantity in the Kalman filter based on the coefficients of the cubic polynomial : Starting from x = -4m (behind the vehicle tail) in the vehicle coordinate system, select a sparse point sequence at intervals of 12m. Based on the coefficients of the cubic polynomial, construct a piecewise spline curve, and force it to satisfy the continuity constraint. Interpolate at intervals of 1.5m within each interval to generate a dense point sequence, which is used as the observed quantity of the Kalman filter for the left and right lane lines, realizing the smooth expression of lane line features and data densification; (3.2) Generate predicted values through four types of scenarios: calculation of lane half-width and lateral offset, curvature compensation and historical data fusion, cubic spline interpolation, and fusion of the average heading angle of historical frames, based on the lane line validity and historical frame data ; (3.3) Generate the scattered point sequences of the left and right lane lines based on the Kalman filter algorithm: Dynamically calibrate the observation noise matrix and the process noise matrix based on the performance of the perception hardware; Calculate the prior error covariance of the current frame based on the state transition matrix of the constant velocity model; Calculate the Kalman gain in combination with the observation matrix , realizing the weight assignment of proximal dependence on observation and distal dependence on prediction; Finally, fuse the predicted value and the observed quantity , and output the optimal scattered point sequences of the left and right lane lines, providing a highly robust input for the generation of the lane center line; (3.4) Complete the calculation of the current frame, convert the data structure for data publication, and prepare a predicted value for the calculation of the next frame.

[0024] As a preferred embodiment of the present invention, step (3.1) is specifically: Starting from = -4m (behind the vehicle tail) in the vehicle coordinate system, select a scattered point every 12m until = 56m to obtain the sparse point sequence , where = -4m, 8m, 20m, 32m, 44m, 56m, is calculated according to the coefficients of the cubic polynomial of the lane line and ; Then, perform cubic spline interpolation for densification. For adjacent sparse points and , construct a cubic polynomial: ; Among them, the sparse points satisfy position continuity, first derivative continuity, and second derivative continuity, and the formula is as follows: ; Interpolate at an interval of 1.5 m between adjacent sparse points to obtain a dense point sequence , which is used as the observation quantity of the left lane line and the right lane line , so as to ensure that the scattered points are smooth and accurate.

[0025] As a preferred embodiment of the present invention, the step (3.2) is specifically as follows: Based on whether the lane line judged in step (2) is valid and whether there is historical frame data, the following scene classification is carried out: (3.2.1) Scene 1: The lane line is valid but there is no historical frame data; Define the lateral distance from the vehicle to the left lane line as , and the lateral distance to the right lane line as , then the lane half-width is . According to the lane half-width and the lateral offset from the vehicle to the lane center line, calculate the prediction quantity ; (3.2.2) Scene 2: The lane line is valid and there is historical frame data; Define the lateral offset compensation amount caused by the road curvature , where represents the vehicle speed, represents the frame interval time, and fuse the optimal estimate of the historical frame and the lateral offset compensation amount to obtain the prediction quantity ; (3.2.3) Scene 3: The lane line is invalid and there is no historical frame data; Perform cubic spline interpolation on the sparse point sequence to obtain the dense point sequence , which is used as the prediction quantity ; (3.2.4) Scene 4: The lane line is invalid but there is historical frame data; Calculate the average heading angle of the previous three historical frames, and fuse the optimal estimate of the historical frame and the average heading angle to obtain the prediction quantity , where represents the vehicle speed, represents the frame interval time.

[0026] As a preferred embodiment of the present invention, the step (3.3) is specifically as follows: Observation noise matrix According to the detection performance of lane lines by perception hardware (such as cameras, radars), perform offline calibration. Through statistical analysis of historical perception data, obtain the credibility reflecting the measured values; Process noise matrix According to the longitudinal distance in the vehicle's own coordinate system Value for dynamic adjustment, implemented using a preset noise lookup table, where The larger the value (farther from the vehicle's own position), the larger the process noise The larger the value, to reflect the confidence attenuation of the lane line prediction result at the far end. The generation of the scatter point sequences of the left and right lane lines based on the Kalman filter algorithm is divided into the following steps: (3.3.1) Calculation of the prior error covariance; According to the state transition matrix And the posterior error covariance of the previous frame , calculate the prior error covariance of the current frame , where Is the posterior error covariance output by the Kalman filter of the previous frame, and the initial frame Is set as a diagonal matrix according to the initial state uncertainty of the lane line; State transition matrix Adopts a constant velocity model in this method, specifically: ; Among them, Is the time interval between adjacent frames, reflecting the change law of the lane line state (position and speed) over time; (3.3.2) Calculation of the Kalman gain; Through the prior error covariance , observation matrix And observation noise , calculate the Kalman gain , where the observation matrix Is designed as , used to extract the position observation from the state vector; (3.3.3) Optimal state estimation; Fuse the predicted value obtained in step (3.3) , the observed value obtained in step (3.2) And the Kalman gain , calculate the Kalman optimal values corresponding to the left and right lane lines in the vehicle's own coordinate system As follows: ; Among them, the Kalman optimal value Can represent the Values, from which the scatter point sequences of the left and right lane lines are respectively and ; (3.3.4)Posterior error covariance update: Update the error covariance in the following manner to provide a basis for the calculation of the next frame: ; where is the identity matrix, and this step ensures that the error covariance converges gradually with the iterative process.

[0027] As a preferred embodiment of the present invention, in step (4) for obtaining the lane center line, when the left and right lane lines have effective sensing lengths (judged by a threshold of 30m), the reference lane line is dynamically selected, which is divided into the following four scenarios: (4.1)Scenario 1: The left lane line is dominant; When the effective sensing length of the left lane line exceeds 30m (calibratable) while the right side is insufficient, taking the left lane line as the reference, the lane center line is directly generated by lateral offset, and the formula is as follows: ; where the scatter point sequence of the left lane line is , and the scatter point sequence of the right lane line is , is the sequence of the lane center line, is the lane half-width; (4.2)Scenario 2: The right lane line is dominant; When the effective sensing length of the right lane line exceeds 30m (calibratable) while the left side is insufficient, the right-dominant scenario is symmetrically processed, and the y sequence of the lane center line is: ; (4.3)Scenario 3: Symmetric interpolation processing; When the effective sensing lengths of the left and right lane lines both exceed 30m (calibratable), taking the sequence of the left lane line as the reference, the sequence of the right lane line is forced to be aligned with the sequence of the left side. Through the linear interpolation algorithm, the corresponding sequence of the right lane line is generated at each position, and the formula is as follows: ; where , is the original scatter point sequence of the right lane line, and the scatter point sequence of the right lane line after interpolation processing is , aligning the left and right lane lines After sequence alignment, the y sequence of the lane center line is as follows: ; Thus, the scatter point sequence of the lane center line based on the Kalman filtering algorithm is .

[0028] The following will further elaborate on this technical solution in detail. As Figure 1 shown, the specific processing steps are as follows: 1) Clearing and recursion of the scatter points of the lane center line. If there is no prior data, directly proceed to the next step; if historical frame data is retained, the prior information of the lane center line needs to be recursively transferred to the ego vehicle coordinate system of the current frame. If operations such as function shutdown, lead vehicle insertion, or turn signal activation occur, the prior data of the lane center line is cleared.

[0029] 2) Determine whether the road feature or a certain lane line on one side is valid. This determination criterion includes: determining the threshold based on the lane line confidence level input by perception and different traffic scenarios; calculating whether the lane width meets the corresponding threshold; obtaining the corresponding threshold by looking up the table based on the vehicle speed and determining whether the length of a certain lane line on one side is within the threshold; intercepting three scatter points (starting point, midpoint, and ending point) representing the lane line features, calculating the average heading angle of these scatter points, and obtaining the consistency threshold by looking up the table based on the lane line curvature information to determine whether the average heading angle error between the two lane lines is within the threshold. Only the lane lines that pass the validity judgment can enter the filtering operation. This operation can not only judge the quality of the lane lines but also avoid the generation quality of the lane center line being contaminated by miscellaneous points.

[0030] 3) Update and optimize the perceived lane lines according to the Kalman filtering algorithm. The filtering and optimization methods adopted in this technical solution include: Obtain the observed quantity in the Kalman filter based on the cubic polynomial coefficients : In a specific embodiment, sparsely sample the x value in the ego vehicle coordinate system, starting from -4, selecting one point every 12 until 56, a total of 6 points are selected, and then perform densification processing. Perform cubic spline interpolation every 1.5 between two points to ensure that the scatter points are smooth and accurate; Obtain the predicted quantity in the Kalman filter based on whether the road feature is valid : If the road feature is valid but there is no historical frame data, first use the sum of the distance from the ego vehicle to the lane line and the distance from the ego vehicle to the lane center line as the lane half-width, and use the lane half-width minus the length of the ego vehicle from the lane center line as the predicted quantity ; if the road feature is valid and there is historical frame data, then use the lateral error caused by the road feature plus the historical frame data as the predicted quantity ; If the road feature is invalid and there is no historical frame data, cubic spline interpolation is directly performed based on 6 points; if the road feature is invalid and there is historical frame data, the predicted value is obtained using the optimal result of the previous frame based on the second derivative continuity or the average yaw angle at the first three points ; Obtaining the observation noise and process noise: The observation noise R is calibrated based on the performance of the perceived lane lines, and the process noise Q is obtained by looking up a table based on the x value in the vehicle's coordinate system. The larger the x value, the larger the process noise Q. The state transition matrix F and the observation matrix H are both identity matrices, and the Kalman gain is calculated Showing an inverted triangle characteristic as the x value in the vehicle's coordinate system increases, based on the obtained observed quantity , the predicted value , and the Kalman gain Calculate the Kalman optimal quantity corresponding to the x value in the vehicle's coordinate system : ; ; ; Complete the calculation of the current frame, convert the data structure for data publishing, and prepare a predicted value for the next frame's calculation.

[0031] 4) Interpolate the y value of the other side lane line based on the x value of a certain side lane line: When obtaining the lane centerline, it is not simply adding the x value and y value in the vehicle's coordinate system, but aligning the x values of the two side lane lines and linearly interpolating the corresponding y values, and then adding the y values based on the same x value. If one side lane line is shorter, the lane centerline is obtained by making a corresponding lateral offset based on the longer lane line according to the lane width; 5) Complete the acquisition of the lane centerline, perform smoothing processing for data publishing, and provide a trajectory for the downstream control module.

[0032] Any process or method description shown in the flowchart or described in other ways herein can be understood as representing a module, segment, or part of code including one or more executable instructions for implementing a specific logical function or process, and the scope of the preferred embodiments of the present invention includes additional implementations, where the functions may be executed in a way that is not shown or discussed, including in a substantially simultaneous manner according to the involved functions or in the reverse order, which should be understood by those skilled in the technical field to which the embodiments of the present invention belong.

[0033] It should be understood that the various parts 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.

[0034] Those of ordinary skill in the art can understand that all or part of the steps carried by the method of implementing the above embodiments can be completed by instructing relevant hardware through a program. The program can be stored in a computer-readable storage medium. When the program is executed, it includes one or a combination of the steps of the method embodiments.

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

[0036] In the description of this specification, the description with reference to terms such as "one embodiment", "some embodiments", "example", "specific example", or "embodiment" means that the specific features, structures, materials, or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described can be combined in a suitable manner in any one or more embodiments or examples.

[0037] Although the embodiments of the present invention have been shown and described above, it can be understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those of ordinary skill in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present invention.

[0038] In practical applications, the present technical solution has the following technical effects: (1) The method for generating a lane centerline based on the Kalman filtering algorithm in the present technical solution greatly improves the stability and robustness of the lane centering function compared with the existing traditional methods for generating lane centerlines. It can achieve self-adaptation. When there are large changes in the perceived lane lines, including the emergence of tidal lanes, blocked lanes, and lane line jumps, etc., this algorithm can adaptively generate a stable lane centerline; (2) In the present technical solution, the effectiveness of the perceived input road features and lane lines is judged, and warnings and preprocessing are performed on the lane lines that do not meet the judgment conditions. It can not only feedback to the perception for improvement, but also prevent data pollution. At the same time, for the continuity of the lane centerline result output, historical frame data is used for data recursion, and finally a trajectory line with a low pollution rate, continuity, and smoothness is output, which is more friendly to the downstream control module; (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, which not only ensures that the measured value conforms to the upstream input, but also makes the observed value have good continuity and smoothness; (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 according to the x value in the vehicle coordinate system. On the one hand, multiple data can be integrated to improve the lane line confidence, and 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 away from the vehicle is more consistent with the perception result, ensuring the stability of the lane centering function. (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 lane line on the other side is offset laterally by a certain distance to generate the centerline, thereby effectively ensuring robustness.

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

Claims

1. A method for generating a lane center line based on the Kalman filtering algorithm, characterized in that The method described above includes the following steps: (1) Based on the currently obtained in-vehicle data, clear or recursively process the lane centerline scatter points; (2) Combine the current road characteristics or a certain side lane line to judge the lane validity and obtain the lane line quality; (3) Based on the Kalman filter algorithm, obtain the observed values, predicted values, and noise data of the road characteristics, and then update and perceptually optimize the lane lines on both sides; (4) Interpolate the y value of the other side lane line based on the x value of one side lane line, perform x value alignment and lateral offset compensation to obtain the lane centerline; (5) Smoothly process the obtained lane centerline and publish the data to provide a planned trajectory line.

2. The lane centerline generation method based on the Kalman filter algorithm according to claim 1, wherein The step (1) is as follows: Obtain the in-vehicle data. If there is no prior data, directly enter step (2); if the current in-vehicle retains historical frame data, the prior information of the lane line needs to be recursively pushed to the ego vehicle coordinate system of the current frame based on the vehicle kinematic model. The specific processing steps are as follows: (1.1) Calculate the relative displacement between two frames: Based on the data from the vehicle speed sensor and the yaw rate sensor, calculate the displacement changes in the x and y directions of the vehicle through integration ( , ) and the change in the heading angle ; (1.2) Coordinate rotation and translation: For the lane line scatter point sequence in the historical frame The calculation formula for coordinate transformation is as follows: ； Among them, represents the lane line scatter points in the vehicle coordinate system of the current frame.

3. The method for generating a lane center line based on the Kalman filtering algorithm according to claim 2, characterized in that The step (2) is as follows: Based on the lane line confidence data input by vehicle perception, judge whether the lane is valid based on a threshold. The specific steps include: (2.1) Set the default width threshold range according to the road type , , where represents the minimum width threshold, represents the maximum width threshold; if there is a certain curvature in the current road , then based on the real-time curvature correct the threshold: , , and then judge whether the current lane width meets the width threshold requirement in the following way: Lane valid = ; wherein represents the lane width at the current vehicle position; (2.2) Based on the current vehicle speed Query and obtain the minimum length threshold of the lane line Then, determine whether the effective perception length of the current lane line meets the minimum length threshold Requirement; (2.3) Intercept three scatter points representing the characteristics of the lane line, including the starting point: the scatter point of the lane line closest to the host vehicle ; the end point: the farthest valid point of the lane line ; the midpoint: the longitudinal midpoint of the lane line , for these three scatter points ( , ), where k is one of the scatter points, take the points 1m before and after it respectively ( , ) and ( , ), and calculate the heading angle of each scatter point : ； For these three scatter heading angles Take the average value to obtain the average heading angle of this lane line , calculate the difference in the average heading angles of the left and right lane lines , based on the difference in the average heading angles Whether it is within the threshold value to judge the consistency of the heading angles of the lane lines on both sides.

4. The lane centerline generation method based on the Kalman filtering algorithm according to claim 1, wherein, The step (3) specifically includes the following steps: (3.1) Obtaining the observation quantity in the Kalman filter based on the coefficients of the cubic polynomial : Starting from the rear of the vehicle body in the vehicle coordinate system, a sparse point sequence is selected at a preset interval. A piecewise spline curve is constructed based on the coefficients of the cubic polynomial, and the continuity constraint is forcibly satisfied. Dense point sequences are interpolated at a preset interval within each interval as the Kalman filter observation quantities of the left and right lane lines, realizing the smooth expression of lane line features and data densification; (3.2) Generate predicted values through four types of scenarios: calculation of lane half-width and lateral offset, curvature compensation and historical data fusion, cubic spline interpolation, and historical frame heading angle mean fusion, based on lane line validity and historical frame data ; (3.3) Generate left and right lane line scattered point sequence based on Kalman filter algorithm: Dynamically calibrate the observation noise matrix based on the performance of the perception hardware and the process noise matrix ;State transfer 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 the fusion prediction With the observed quantity , output the optimal scattered point sequence of the left and right lane lines, providing a highly robust input for lane centerline generation; (3.4) Complete the calculation of the current frame, convert the data structure for data publishing, and prepare a predicted value for the calculation of the next frame at the same time.

5. The lane centerline generation method based on the Kalman filtering algorithm according to claim 4, characterized in that The step (3.1) is specifically: Starting from the position behind the rear of the vehicle in the vehicle coordinate system, select a scattered point at every preset interval number until the end to obtain a sparse point sequence , Calculated according to the coefficients of the cubic polynomial of the lane line and ; Then perform cubic spline interpolation densification, and construct a cubic polynomial for adjacent sparse points and in the following way: ； Among them, the sparse points satisfy position continuity, first derivative continuity, and second derivative continuity. The formula is as follows: ； Interpolate at a preset interval between adjacent sparse points to obtain a dense point sequence as the observation values of the left and right lane lines to ensure that the scattered points are smooth and accurate 6. The method for generating a lane center line based on the Kalman filtering algorithm according to claim 5, wherein The step (3.2) is specifically: Based on whether the lane line judged in step (2) is valid and whether there is historical frame data, perform the following scene differentiations: (3.2.1) Scene 1: The lane line is valid but there is no historical frame data; Define the lateral distance from the vehicle to the left lane line as , and the lateral distance to the right lane line as . Then the lane half-width is . According to the lane half-width and the lateral offset from the vehicle to the lane center line calculate the predicted quantity ; (3.2.2) Scene 2: The lane line is valid and there is historical frame data; Define road curvature Lateral offset compensation caused , where represents the vehicle speed, represents the inter-frame time interval, and fuses the optimal estimate of historical frames and the lateral offset compensation to obtain the predicted value ; (3.2.3) Scene 3: The lane line is invalid and there is no historical frame data; According to the sparse point sequence Perform cubic spline interpolation to obtain a dense point sequence , which is regarded as the predicted value ; (3.2.4) Scene 4: The lane line is invalid but there is historical frame data; Calculate the average heading angle of the first three historical frames , fuse the optimal estimation of historical frames with the average heading angle to obtain the prediction quantity , where represents the vehicle speed of the host vehicle, represents 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) for generating the scatter point sequences of the left and right lane lines based on the Kalman filter algorithm includes the following steps: (3.3.1) Calculate the prior error covariance; According to the state transition matrix and the posterior error covariance of the previous frame , calculate the prior error covariance of the current frame , where is the posterior error covariance output by the Kalman filter of the previous frame, and the initial frame is set as a diagonal matrix according to the initial state uncertainty of the lane line; the state transition matrix adopts a constant velocity model, specifically: ； Among them, is the time interval between adjacent frames; (3.3.2) Calculate the Kalman gain; Through the prior error covariance , the observation matrix and the observation noise , calculate the Kalman gain , where the observation matrix is designed as for extracting the position observation from the state vector; (3.3.3) Optimal state estimation; The predicted quantity obtained in the fusion step (3.3) , the observed quantity obtained in step (3.2) and the Kalman gain , calculate the Kalman optimal quantities corresponding to the left and right lane lines in the vehicle coordinate system in the following manner : ； Among them, the Kalman optimal quantity represents the value among the scattered points of the lane lines on the left and right sides. From this, the scattered point sequences of the lane lines on the left and right sides are respectively and ; (3.3.4) Posterior error covariance update: Update the error covariance in the following manner to provide a basis for the calculation of the next frame: ； where P t is the posterior error covariance, is the identity matrix.

8. The method for generating a lane centerline based on the Kalman filtering algorithm according to claim 7, wherein When the step (4) is to obtain the lane centerline, the reference lane line is dynamically selected according to the effective perception lengths of the left and right lane lines, which is divided into the following four scenarios: (4.1) Scene 1: The left lane line is dominant; When the effective perception length of the left lane line exceeds the preset length while the right side is insufficient, use the left lane line as the reference, and directly generate the lane centerline through lateral offset. The formula is as follows: ； Among them, the scatter point sequence of the left lane line is , and the scatter point sequence of the right lane line is , is the sequence of the lane center line, is the lane half-width; (4.2) Scene 2: The right lane line is dominant; When the effective sensing length of the right lane line exceeds the preset length while the left side is insufficient, symmetrically process the right-dominated scenario, and the y sequence of the lane center line is as follows: ； (4.3) Scene 3: Symmetric interpolation processing; When the effective sensing lengths of the left and right lane lines both exceed the preset length, using the left lane line sequence as a reference, force the right lane line sequence to be aligned with the left sequence. Through the linear interpolation algorithm, at each position, generate the corresponding sequence of the right lane line, find the adjacent right scatter point indices that satisfy and , and the formula is as follows: ； Among them represents the original scatter point index of the right lane line represents the next scatter point adjacent to the index in the scatter point sequence of the right lane line, that is the successor scatter point of After interpolation processing, the scatter point sequence of the right lane line is , after aligning the sequences of the left and right lane lines the y sequence of the lane center line is as follows: ； Thus, the lane centerline scatter point sequence based on the Kalman filter algorithm is .

9. A lane center line generation device based on the Kalman filtering algorithm, characterized in that, The device includes: A processor configured to execute computer-executable instructions; A memory storing one or more computer-executable instructions, and when the computer-executable instructions are executed by the processor, the steps of generating the lane centerline based on the Kalman filter algorithm as described in any one of claims 1 to 8 are implemented.

10. A lane centerline generation processor based on the Kalman filtering algorithm, characterized in that, The described processor is configured to execute computer-executable instructions, and when the computer-executable instructions are executed by the processor, the steps of generating a lane center line based on the Kalman filtering algorithm described in any one of claims 1 to 8 are implemented.

11. 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 center line based on the Kalman filtering algorithm described in any one of claims 1 to 8.

Images