Vehicle positioning method based on Pareto and sensor fusion

Abstract

The invention discloses a vehicle positioning method based on Pareto and sensor fusion, and the method comprises the steps: building a dead reckoning model based on vehicle kinematics according to the vehicle driving data through obtaining the induction data, collected by a plurality of sensors, of an autonomous driving vehicle; the point cloud data of the 3D laser radar is recorded on a brightness frame generation module for brightness frame extraction to obtain a brightness map, a plurality of reference points of the brightness map are obtained, and a positioning model of the autonomous driving vehicle is constructed according to the plurality of reference points; the dead reckoning model and the positioning model are subjected to fusion estimation to obtain a position estimation result, the position estimation result is input into a Pareto positioning model for optimization analysis to obtain a vehicle positioning result, multiple standards can be optimized in real time by adopting multiple sensors and a Pareto optimization algorithm, no linear processing is needed, and the positioning accuracy is improved. Flexible and robust positioning is realized, and the reliability of autonomous vehicle positioning estimation is improved.

Description

Technical Field

[0001] The invention belongs to the technical field of vehicle positioning, and in particular relates to a vehicle positioning method based on Pareto and sensor fusion. Background Art

[0002] With the development of sensing technology, the safety and operating efficiency of automated driving systems in typical urban environments have been greatly improved. In-depth research on the development of various automotive technologies requires improving the safety of transportation systems, reducing travel time, and improving energy efficiency. Accurate, real-time, and reliable positioning is one of the most critical functions in autonomous driving operations and is the basis for many other core applications, including target tracking, trajectory prediction, motion planning, and control.

[0003] However, in autonomous driving, a range of sensor modalities are used to support positioning, including global navigation satellite systems (GNSS), inertial measurement units (IMUs), cameras, lidars, and radars. Each sensor forms part of a rich stream, but often their performance is affected by weather conditions such as heavy rain, fog, or snow, which can reduce the quality and reliability of sensor data. For example, GNSS suffers from signal attenuation and multipath errors when operating in challenging environments such as urban canyons or forests; on the other hand, lidar systems suffer from beam scattering under rainy conditions, further leading to lower target detection accuracy. These environmental parameters introduce variations in sensor data, thereby increasing the computational burden of real-time processing while reducing the overall positioning accuracy. Therefore, it is urgent to provide a vehicle positioning method based on Pareto and sensor fusion to solve the above-mentioned technical problems. Summary of the invention

[0004] In view of this, the present invention provides a vehicle positioning method based on Pareto and sensor fusion. The use of multi-sensors and Pareto optimization algorithm can optimize multiple standards in real time, including sensor accuracy, computational load and robustness to environmental changes, thereby improving the reliability of positioning estimation. The following technical solutions are specifically used to achieve this.

[0005] The present invention provides a vehicle positioning method based on Pareto and sensor fusion, comprising the following steps: Acquire sensing data collected by multiple sensors of the autonomous driving vehicle, wherein the sensing data includes vehicle driving data, point cloud data of 3D laser radar, GNSS signals and preset map data, and the vehicle driving data includes wheel speed and yaw rate; A dead reckoning model based on vehicle kinematics is constructed according to the vehicle driving data, and the point cloud data of the 3D laser radar is recorded on a brightness frame generation module to perform brightness frame extraction to obtain a brightness map; Acquire multiple reference points of the brightness map, build a positioning model of the autonomous driving vehicle according to the multiple reference points, and fuse the dead reckoning model with the positioning model to obtain a position estimation result; The position estimation result is input into the Pareto positioning model for optimization analysis to obtain the vehicle positioning result.

[0006] As a preferred embodiment of the above technical solution, a dead reckoning model based on vehicle kinematics is constructed according to the vehicle driving data, and the point cloud data of the 3D laser radar is recorded on a brightness frame generation module to perform brightness frame extraction to obtain a brightness map, including: The coordinates corresponding to the point cloud data of the 3D laser radar are transformed into navigation coordinates. The transformation expression is: (1) in, are the coordinates of the midpoint in the luminance frame, is the translation vector from the lidar frame to the navigation frame, is the rotation matrix between the lidar coordinates and the navigation coordinates, the rotation matrix The definition expression is: (2) in, , and is the heading, pitch and roll information, and ; After there is a point cloud in the navigation frame, the brightness frames are accumulated together to form a bright spot cloud map.

[0007] As a preferred embodiment of the above technical solution, obtaining multiple reference points of the brightness map and constructing a positioning model of the autonomous driving vehicle according to the multiple reference points includes: Set the plane where the autonomous vehicle is located to , ,in, , For discrete-time indices, the estimated position is based on a set of fixed reference points or distance measurements at known locations. , and ,in ; The distance between the position node and each reference point in time is expressed as , observation distance yes Noise measurement value, distance measurement value Additive zero-mean Gaussian noise The corresponding expression is: (3) The variance of the noise is Related to distance, modeled as: (4) in, and is a known constant, It is the distance is the baseline variance of the noise when , sets the initial level of the noise variance and represents the minimum noise variance, is a constant and is used to control the noise variance as the distance increases with the increase of .

[0008] As a preferred embodiment of the above technical solution, the position estimation result is input into a Pareto positioning model for optimization analysis to obtain a vehicle positioning result, including: Determining ideal sensor fusion parameters using error state constrained Pareto algorithm and , and strike a balance between minimizing position and heading errors in the estimates and managing noise variance; The error state vector It can be expanded to represent the individual errors of the east and north position, velocity and heading, and the corresponding expressions are: (10) in, and is the true eastern and northern position, and True east and north speeds, is the true heading, with a corresponding estimated value , , , and , the error state covariance matrix is ​​defined as: (11) The total error is expressed as the weighted sum Q of the individual errors using a weighting matrix: (12) in, To determine the diagonal matrix of the relative importance of each error component, (13) The noise covariance matrix associated with the measurement noise vector that affects dead reckoning and range measurements is expressed as: (14) Among them, the variances of distance measurement noise, velocity noise and heading noise are respectively expressed as , and express.

[0009] As a preferred embodiment of the above technical solution, the error state constrained Pareto algorithm estimates the position and heading errors in the dead reckoning model and obtains error measurement values ​​of the east and north positions, speeds and headings. The estimated errors of the east and north positions, speeds and headings are expressed in matrix form as follows: (15) in, Denotes the error state vector, and the Pareto optimization problem is to minimize the estimation error and noise variance, subject to the sensor fusion parameters and The corresponding objective function is expressed as: Make obedient and (16) Where Q is the weighting matrix of the error components, R is the noise covariance matrix associated with the sensor noise, is a regularization parameter that controls the trade-off between minimizing the estimation error and managing the noise variance, represents the maximum allowable error to ensure that the constraints are met; the sensor fusion parameters are selected using the optimization framework and , reducing positioning and heading errors while controlling the overall noise variance.

[0010] As a preferred embodiment of the above technical solution, the constrained optimization problem of the error state constrained Pareto algorithm is estimated to balance the estimation error and the sensor noise, including: Initial estimate: Determines an estimate based on the dead reckoning model and measured distances , , , and ,The initial estimate will provide a benchmark for calculating the error and determining the fusion parameters; Error propagation: Use formula (15) to propagate the estimated error Estimated as true state The difference from the initial estimate , propagate the difference and calculate the covariance matrix in formula (11) , to quantify the errors from different sources; Noise analysis: The noise covariance matrix of formula (14) is calculated by analyzing the sensor characteristics ; Optimization analysis: Solve the Pareto optimization problem in formula (15) to determine the fusion parameters and , minimize the weighted sum of the estimation error and the noise variance, and keep and , showing that Lagrange multipliers are useful for managing equality constraints in nonlinear optimization problems ; Update estimate: After getting the optimal fusion parameters and After that, update the location Estimation, speed and heading , the corrected best estimate includes both range and dead reckoning data.

[0011] As a preferred embodiment of the above technical solution, the Pareto front is used to visualize the solution of the Pareto optimization problem in formula (16), where the points on the curve correspond to different methods of weighing the minimization of estimation error and the management of noise variance. The points on the Pareto front are the optimal solutions and correspond to the minimum overall error, satisfying the restrictions on estimation accuracy and noise variance.

[0012] As a preferred embodiment of the above technical solution, the position estimation result is input into a Pareto positioning model for optimization analysis to obtain a vehicle positioning result, including: The state space model is used to analyze the stability of the fusion estimation process. The state vector at time It is expressed as: (17) A discrete-time linear system can be used to model the evolution of this state vector: (18) in, is the state transition matrix, which defines the state from time arrive The evolution of The control input The input matrix mapped to the state, is process noise, and the default process noise is a zero-mean Gaussian random vector with a covariance matrix , the measurement model relates the state vector to the observed measurement value, and the corresponding expression is: (19) in, is the measurement matrix, To measure noise, preset is a zero-mean Gaussian distribution with covariance, and by analyzing the state transfer matrix to test the characteristic value of; If all eigenvalues ​​are inside the unit circle, the system is stable; If any eigenvalue is outside the unit circle radius, the localization process is unstable.

[0013] As a preferred embodiment of the above technical solution, according to the state transfer matrix The eigenvalue analysis of the process estimates the stability of the process. When all eigenvalues ​​of are located on the unit circle of the complex plane, the system is stable and the corresponding expression is: (20) If any eigenvalue is greater than 1, the system is unstable.

[0014] As a preferred embodiment of the above technical solution, a positive definite function of the Lyapunov candidate function is used Stability analysis is performed on the stability, and the state vector is defined as: (twenty one) Among them, P is a positive definite matrix. As time goes by, the decrease of Lyapunov function indicates the stability of the system. The corresponding expression is: (twenty two) Substituting the state space equation into the Lyapunov function yields: (twenty three) in, If is a negative value, then: (twenty four) Formula (24) is the Lyapunov stability criterion. If there exists a positive definite matrix that satisfies formula (24), then stability exists.

[0015] The present invention provides a vehicle positioning method based on Pareto and sensor fusion, which collects sensing data of an autonomous driving vehicle by acquiring multiple sensors, constructs a dead reckoning model based on vehicle kinematics according to the vehicle driving data, records the point cloud data of the 3D laser radar on a brightness frame generation module to perform brightness frame extraction to obtain a brightness map, acquires multiple reference points of the brightness map, and constructs a positioning model of the autonomous driving vehicle according to the multiple reference points, fuses the dead reckoning model and the positioning model to obtain a position estimation result, inputs the position estimation result into a Pareto positioning model to perform optimization analysis to obtain a vehicle positioning result, adopts multiple sensors and a Pareto optimization algorithm to optimize multiple standards in real time without any linearization processing, realizes flexible and robust positioning, and improves the reliability of autonomous vehicle positioning estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0017] Figure 1 A flow chart of a vehicle positioning method based on Pareto and sensor fusion provided by the present invention. DETAILED DESCRIPTION

[0018] Embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and cannot be understood as limiting the present invention.

[0019] Using Pareto-based multi-objective optimization, unlike traditional single-objective optimization methods that focus on maximizing accuracy, Pareto optimization allows for simultaneous trade-offs between multiple competing objectives, such as accuracy, computational efficiency, and robustness to environmental disturbances. In the context of positioning, Pareto optimization has shown its ability to reduce the mean and variance of positioning errors. As a result, a more balanced and adaptable solution is applicable to a larger range of operating conditions and performs efficiently. These multi-objective methods have particular advantages in urban scenarios, where multiple criteria including sensor accuracy, computational load, and robustness to environmental changes need to be optimized in real time in complex and dynamic scenarios; they are particularly useful in autonomous driving in complex urban scenarios, where complex scenarios, such as urban environments, require multiple dynamic considerations in real time to provide reliable positioning estimates. By combining Pareto optimization, multi-sensor fusion systems now provide a more flexible and adaptable solution to solving positioning problems, especially under challenging environmental conditions.

[0020] See also Figure 1 The present invention provides a vehicle positioning method based on Pareto and sensor fusion, comprising the following steps: S1: Acquire sensing data collected by multiple sensors of the autonomous driving vehicle, wherein the sensing data includes vehicle driving data, point cloud data of 3D laser radar, GNSS signal and preset map data, and the vehicle driving data includes wheel speed and yaw rate; S2: constructing a dead reckoning model based on vehicle kinematics according to the vehicle driving data, and recording the point cloud data of the 3D laser radar on a brightness frame generation module to perform brightness frame extraction to obtain a brightness map; S3: Acquire multiple reference points of the brightness map, build a positioning model of the autonomous driving vehicle according to the multiple reference points, and fuse the dead reckoning model with the positioning model to obtain a position estimation result; S4: Inputting the position estimation result into the Pareto positioning model for optimization analysis to obtain the vehicle positioning result.

[0021] In this embodiment, the sensory inputs used for the overall integrated positioning based on inertial lidar include wheel speed (wheel speed), yaw rate, point cloud data of 3D lidar sensor, low-cost global navigation satellite system (GNSS) signal and Google map (preset map). Wheel speed and yaw rate are necessary for designing a dead reckoning model based on vehicle kinematics, which is used to estimate the heading and position of the vehicle. At the same time, the point cloud data collected by the 3D lidar (LiDAR) sensor will be processed by brightness frame extraction in a dedicated brightness frame generation module. The brightness frame to be extracted will be used in two ways: to create a light map for offline tasks, and to process the current point cloud frame offline through a map matching algorithm based on normal distribution transform (NDT), the latter of which allows the conversion of point cloud data, which helps to provide better positioning. The position and heading estimation errors are input into the positioning model through the dead reckoning model. On the one hand, these estimates will be used as measurements in the error state-based Pareto optimization framework, which aims to estimate the position and heading errors inherent in the dead reckoning model; on the other hand, the results will be crucially used to make position and heading estimation corrections in the dead reckoning model, thereby improving positioning reliability. Among them, one of the most important issues is that 3D LiDAR sensors produce a large number of points, which may generate computational load during the operation of the mapping matching algorithm when these points are converted into point clouds. In addition, vegetation changes with the seasons and maps may need to be updated from time to time, which complicates the entire positioning process. These challenges can be addressed by reducing the density of the map and reducing the impact of vegetation changes. Low-cost GNSS position data can be provided together with Google Maps to remove vegetation in the point cloud frame and the generated map.

[0022] It should be noted that the sensor information includes wheel speed and yaw rate, point cloud data from 3D LiDAR, signals from low-cost global navigation satellite systems, and Google Maps. The odometry model based on vehicle kinematics is established using wheel speed and yaw rate to estimate the vehicle's heading and position. At the same time, the point cloud from the 3D LiDAR is registered to the brightness generation module for creating a brightness frame. Based on the generated brightness frame, a light map is created offline. In the error-state-based Pareto optimization model, the difference between the position and heading estimates of the dead reckoning model and the position and heading estimates of the map matching algorithm is used as a measurement value for estimating the position and heading errors of the dead reckoning model, and the position and heading of the dead reckoning model are corrected using the estimated position and heading errors of the dead reckoning model.

[0023] It should be understood that by acquiring sensing data of the autonomous vehicle collected by multiple sensors, a dead reckoning model based on vehicle kinematics is constructed according to the vehicle driving data, and the point cloud data of the 3D lidar is recorded on a brightness frame generation module to extract the brightness frame to obtain a brightness map, multiple reference points of the brightness map are obtained, and a positioning model of the autonomous vehicle is constructed according to the multiple reference points, the dead reckoning model and the positioning model are fused and estimated to obtain a position estimation result, and the position estimation result is input into a Pareto positioning model for optimization analysis to obtain a vehicle positioning result. The use of multiple sensors and a Pareto optimization algorithm can optimize multiple criteria in real time without any linearization processing, thereby achieving flexible and robust positioning and improving the reliability of autonomous vehicle positioning estimation.

[0024] Optionally, a dead reckoning model based on vehicle kinematics is constructed according to the vehicle driving data, and the point cloud data of the 3D laser radar is recorded on a brightness frame generation module to perform brightness frame extraction to obtain a brightness map, including: The coordinates corresponding to the point cloud data of the 3D laser radar are transformed into navigation coordinates. The transformation expression is: (1) in, are the coordinates of the midpoint in the luminance frame, is the translation vector from the lidar frame to the navigation frame, is the rotation matrix between the lidar coordinates and the navigation coordinates, the rotation matrix The definition expression is: (2) in, , and is the heading, pitch and roll information, and ; After there is a point cloud in the navigation frame, the brightness frames are accumulated together to form a bright spot cloud map.

[0025] In this embodiment, obtaining a plurality of reference points of the brightness map and constructing a positioning model of the autonomous driving vehicle according to the plurality of reference points includes: Set the plane where the autonomous vehicle is located to , ,in, , For discrete-time indices, the estimated position is based on a set of fixed reference points or distance measurements at known locations. , and ,in ; The distance between the position node and each reference point in time is expressed as , observation distance yes Noise measurement value, distance measurement value Additive zero-mean Gaussian noise The corresponding expression is: (3) The variance of the noise is Related to distance, modeled as: (4) in, and is a known constant, It is the distance is the baseline variance of the noise when , sets the initial level of the noise variance and represents the minimum noise variance, is a constant and is used to control the noise variance as the distance increases with the increase of .

[0026] It should be noted that the bright LiDAR frame is called "brightness frame" and is based on the road boundary information obtained using the mapping service. Based on the rough data given by Google Maps, the vegetation and road boundary information on the left and right sides of the autonomous vehicle can be segmented. Each category is considered to be constrained by the boundary close to the road and depicts the edge of the vegetation or other unwanted features; the leftmost and rightmost boundaries, the boundary area contains these features. This boundary information is given in the form of a list of four discrete points that define the respective boundaries. The LiDAR points are then converted to UTM coordinates (east and north) based on the current position and direction of the autonomous vehicle reported by the GNSS. For each translated LiDAR point, the nearest lateral position point is found from the four boundary lists. This area is the area surrounded by the two points on the left and right boundary tables, and these LiDAR points are then marked as deleted from the original list, resulting in a "brighter" but relevant LiDAR point frame. It is worth noting that the prior information from the predefined map (Google Maps) is used to segment non-road areas and vegetation, while low-cost GNSS is used to reduce the point cloud data of the LiDAR frame. Among them, the conversion from World Geodetic System (WGS) coordinates to Universal Transverse Mercator (UTM) coordinates and then from UTM to LiDAR coordinates is pre-calculated, which will prevent the conversion calculation from being repeated in each iteration and greatly reduce the computational overhead. The algorithm saves these conversions in advance and avoids recalculating them for each LiDAR frame. In addition, by using some spatial data structures, such as KD trees or R trees, boundary points can be found efficiently for comparison, which can reduce the linear complexity of selecting boundary points to logarithmic, thereby improving the speed and accuracy of checking whether LiDAR points are between road boundaries; finally, LiDAR points benefit from batch processing, performing conversion and deletion operations on point sets, which minimizes iteration overhead and allows the algorithm to take advantage of modern hardware to process multiple points at a time, which not only reduces the number of operations but also greatly speeds up execution.

[0027] The conversion of 3D LiDAR coordinates to navigation coordinates is achieved using an Applanix-LVX system with RTK correction installed on a high-end vehicle. This precise transformation is used to convert the point cloud data of the brightness frame into navigation coordinates. These converted point clouds are superimposed together to form a bright spot cloud map. In other words, the light frames are accumulated together to form a light point cloud map. Since the vegetation that changes with the seasons has been removed from the frame, on the one hand, the density of the map can be reduced, and on the other hand, since the vegetation is changing, the map will not necessarily be updated. For the former, the advantage is that based on the light frame with reduced density, the computational load of the map matching algorithm can be significantly reduced; for the latter, since there is no vegetation changing in both the map and the frame, the positioning accuracy can be improved.

[0028] Optionally, the position estimation result is input into a Pareto positioning model for optimization analysis to obtain a vehicle positioning result, including: Determining ideal sensor fusion parameters using error state constrained Pareto algorithm and , and strike a balance between minimizing position and heading errors in the estimates and managing noise variance; The error state vector It can be expanded to represent the individual errors of the east and north position, velocity and heading, and the corresponding expressions are: (10) in, and is the true eastern and northern position, and True east and north speeds, is the true heading, with a corresponding estimated value , , , and , the error state covariance matrix is ​​defined as: (11) The total error is expressed as the weighted sum Q of the individual errors using a weighting matrix: (12) in, To determine the diagonal matrix of the relative importance of each error component, (13) The noise covariance matrix associated with the measurement noise vector that affects dead reckoning and range measurements is expressed as: (14) Among them, the variances of distance measurement noise, velocity noise and heading noise are respectively expressed as , and express.

[0029] In this embodiment, the error state constrained Pareto algorithm aims to determine the ideal fusion parameters and strike a balance between minimizing the position and heading errors in the estimation and managing the noise variance. The purpose of the algorithm is to reduce the mean square error of the estimation while keeping the estimation error within the allowable range and taking into account the uncertainty caused by sensor noise. The fusion process aims to reduce the overall error while controlling the variation and optimizing the position and heading prediction in the presence of sensor noise.

[0030] Optionally, the error state constrained Pareto algorithm estimates the position and heading errors in the dead reckoning model and obtains error measurements of the east and north positions, velocities, and headings. The estimated errors of the east and north positions, velocities, and headings are expressed in matrix form as: (15) in, Denotes the error state vector, and the Pareto optimization problem is to minimize the estimation error and noise variance, subject to the sensor fusion parameters and The corresponding objective function is expressed as: Make obedient and (16) Where Q is the weighting matrix of the error components, R is the noise covariance matrix associated with the sensor noise, is a regularization parameter that controls the trade-off between minimizing the estimation error and managing the noise variance, represents the maximum allowable error to ensure that the constraints are met; the sensor fusion parameters are selected using the optimization framework and , reducing positioning and heading errors while controlling the overall noise variance.

[0031] In this embodiment, the goal of the error state constraint Pareto algorithm is to estimate the position and heading errors in the dead reckoning model and obtain the error measurements of the east and north positions, speed and heading. By optimizing the fusion parameters, the error state constraints are ensured to be met while minimizing the estimation error and noise variance, and maintaining a balance between the two competing goals. The fusion parameters are selected using the optimization framework to reduce the positioning and heading errors while controlling the overall noise variance. The error constraint makes the estimation error not exceed a certain maximum value, so that the estimated mean square error (MSE) and the uncertainty caused by minimizing sensor noise are balanced, thereby ensuring the robust positioning of the autonomous vehicle.

[0032] Optionally, a constrained optimization problem of an error-state constrained Pareto algorithm is estimated to balance estimation error and sensor noise, including: Initial estimate: Determines an estimate based on the dead reckoning model and measured distances , , , and ,The initial estimate will provide a benchmark for calculating the error and determining the fusion parameters; Error propagation: Use formula (15) to propagate the estimated error Estimated as true state The difference from the initial estimate , propagate the difference and calculate the covariance matrix in formula (11) , to quantify the errors from different sources, including the necessary steps of how errors from measurement noise propagate through the system; Noise analysis: The noise covariance matrix of formula (14) is calculated by analyzing the sensor characteristics ,This matrix illustrates the uncertainty level of the sensor input and is a key factor in the noise of the entire optimization process; Optimization analysis: Solve the Pareto optimization problem in formula (15) to determine the fusion parameters and , minimize the weighted sum of the estimation error and the noise variance, and keep and , showing that Lagrange multipliers are useful for managing equality constraints in nonlinear optimization problems ,This step minimizes the weighted sum of the estimation error and the noise variance.,Since both the objective function and the constraints are nonlinear,,constrained nonlinear optimization techniques are used - Lagrange multipliers are used to provide the,optimal fusion parameters; Update estimate: After getting the optimal fusion parameters and After that, update the location Estimation, speed and heading ,The corrected optimal estimate includes both range and dead reckoning data, minimizing the inaccuracy of position and heading while accounting for noise.

[0033] In this embodiment, the solution of the Pareto optimization problem in formula (16) is graphically displayed using the Pareto front, where the points on the curve correspond to different methods of weighing the minimization of estimation error and the management of noise variance. The points on the Pareto front are the optimal solutions and correspond to the minimum overall error, satisfying the constraints on estimation accuracy and noise variance. For the constrained optimization problem of the error state constrained Pareto method, the estimation and refinement of the position, speed and heading of the autonomous driving vehicle are analyzed in detail using multiple steps, while balancing the estimation error and sensor noise. With the help of the Pareto front, the solution of the Pareto optimization problem described in (16) can be graphically displayed, where the points on the curve correspond to different methods of weighing the minimization of estimation error and the management of noise variance. The Pareto front shows the various trade-offs between these competing objectives and provides a range of solutions. This point on the Pareto front is considered to be the optimal solution, which corresponds to the minimum overall error, while satisfying the constraints on estimation accuracy and noise variance. Therefore, the error state constrained Pareto method systematically and robustly improves the accuracy and reliability of vehicle positioning. The estimator systematically fuses the dead reckoning results with the odometry measurements, effectively leveraging their respective strengths to constrain the accumulation of position errors described by Eq. The uncertainty caused by the noise modeled in Eq. (14) is maintained in a controlled manner, achieving this goal by seeking the best trade-off between reducing the estimation error and mitigating the impact of the noise variance, so that the error (described by the error state vector in Eq. (15)) is kept within an acceptable range. When applications require high-precision positioning, such as modeling sensor noise patterns in autonomous car navigation, the error state constrained Pareto improves the reliability and performance of positioning systems that are susceptible to noise in the real world by effectively controlling the estimation errors and uncertainties introduced by noisy sensors.

[0034] Optionally, the position estimation result is input into a Pareto positioning model for optimization analysis to obtain a vehicle positioning result, including: The state space model is used to analyze the stability of the fusion estimation process. The state vector at time It is expressed as: (17) A discrete-time linear system can be used to model the evolution of this state vector: (18) in, is the state transition matrix, which defines the state from time arrive The evolution of The control input The input matrix mapped to the state, is process noise, and the default process noise is a zero-mean Gaussian random vector with a covariance matrix , the measurement model relates the state vector to the observed measurement value, and the corresponding expression is: (19) in, is the measurement matrix, To measure noise, preset is a zero-mean Gaussian distribution with covariance, and by analyzing the state transfer matrix to test the characteristic value of; If all eigenvalues ​​are inside the unit circle, the system is stable; If any eigenvalue is outside the unit circle radius, the localization process is unstable.

[0035] In this embodiment, according to the state transfer matrix The eigenvalue analysis of the process estimates the stability of the process. When all eigenvalues ​​of are located on the unit circle of the complex plane, the system is stable and the corresponding expression is: (20) If any eigenvalue is greater than 1, the system is unstable.

[0036] Positive definite functions using Lyapunov candidate functions Stability analysis is performed on the stability, and the state vector is defined as: (twenty one) Among them, P is a positive definite matrix. As time goes by, the decrease of Lyapunov function indicates the stability of the system. The corresponding expression is: (twenty two) Substituting the state space equation into the Lyapunov function yields: (twenty three) in, If is a negative value, then: (twenty four) Formula (24) is the Lyapunov stability criterion. If there exists a positive definite matrix that satisfies formula (24), then stability exists.

[0037] It is important to note that in the navigation of autonomous vehicles, proving the stability of the estimation process is crucial to ensure that the positioning remains accurate and reliable over time. Stability analysis studies the growth of small perturbations or estimation errors and determines whether they decrease over time, remain bounded, or grow indefinitely. Stability can be checked by analyzing the eigenvalues ​​of the state transfer matrix. If all eigenvalues ​​are within the unit circle, the system is stable, which basically means that any estimation error will decay over time. On the other hand, if any eigenvalue is outside the unit circle radius, the disturbance will be amplified and may cause instability in the localization process. In addition, analyzing the covariance matrices will be useful as they provide information about the process noise and measurement noise on the stability of the state estimate, enabling a robust fusion estimation process, while providing a stable positioning estimate, enhancing the vehicle's ability to localize in a dynamic environment and operate reliably under changing conditions.

[0038] Specifically, multiple data acquisition systems were used to capture point cloud data for high-precision mapping and localization. The HDL-32E 3D LiDAR sensor mounted on the vehicle provided the required high-frequency point cloud data at 20 Hz, while the ROS-developed algorithms processed this data in real time on an industrial-grade PC. The vehicle pose updates were localized in response at 5Hz, while the ground truth was determined by the integrated GNSS and IMU system, Planar LVX. For large-scale data collection, the RIEGL VMX-2HA mobile laser scanning system was used, which provided very accurate 3D data with an accuracy of 8 mm and a precision of 5 mm, and could obtain up to 1.1 million measurements per second due to its dual scanning mechanism. Fixed GPS stations also allowed the establishment of control points to enhance georeferencing, allowing the data to be precisely aligned with real-world coordinates. The MLS system reached speeds of up to 60 km / h during operation, achieving a point density of approximately 7500-8500 points / m2. Due to the very high point density, even the finest details can be represented in the data, while also obtaining the granularity required for mapping and subsequent analysis. Four different independent MLS datasets were used and provided in LAS file format. Dataset 1 is a 30-meter-long urban road corridor segment. The number of points in the study area is 1,827,963, with a density of 8,500 points / m²; Dataset 2 is a length of 176.5 m for other road segments, with 9,850,578 points respectively. These datasets span different sections of the highway, each with different positioning and mapping challenges. In these datasets, the high level of point density allows thorough and detailed scanning of road markings, curbs, and even building facades, which can easily capture details and provide robust analysis for applications in autonomous navigation.

[0039] Specifically, real-time performance is the most critical issue in autonomous vehicle positioning. The rotation rate of the Velodyne LiDAR and the update frequency of the algorithm ensure that the vehicle's posture is generated at a frequency of 5Hz, thereby achieving continuous and responsive positioning. The use of industrial PCs to handle the computational workload brought by real-time processing of large data sets ensures the accuracy and speed of the algorithm. A powerful data acquisition and positioning system is proposed, which integrates high-performance LiDAR, precision GNSS / IMU and a powerful data processing platform. These data sets are very valuable for further verification and continuous improvement of positioning algorithms in challenging real-world scenarios.

[0040] The tests were conducted on a loop of approximately 2.7 km, with this part of the 0.5 km section of the track being specifically chosen for analysis as it faces a complex environment with a highly complex built environment and diverse vegetation. The performance of the brightness map was considered from the identification of 5 invariant features from 1D, 2D and 3D structures. The impact of the environment on the accuracy of the algorithm was analyzed in detail, considering Areas 1 and 2, where there are buildings and vegetation. The tests were conducted under heavy rain and fog conditions, which are common in the region. The harsh conditions introduced a lot of variability in the LiDAR point cloud, mainly due to the scattering effect caused by raindrops and the reduced visibility in foggy conditions. The density of raindrops as well as the diffusion effect of light in fog require frequent updates and adaptations of the map to achieve accurate real-time positioning. In fact, the ability to consider and adapt to such environmental changes is an important consideration when evaluating algorithms.

[0041] To simulate real driving conditions, a shuttle bus was driven along a ring road in a normal pattern of acceleration, deceleration and steering. During the test, the longitudinal speed was kept between 0 and 25 km / h by following the speed limit of the ring road. Therefore, the focus of the test was on the robustness of the map matching algorithm in adverse weather conditions, especially rain, which affects the data acquisition of the lidar sensor and the processing required to correctly localize the vehicle. The algorithm was initially executed in clear weather to provide a baseline for the actual performance of localization, and this result has been used as a reference for further evaluation. Subsequent experiments have been completed to test the combination of rainfall with lidar scanning and feature extraction. The test also explored how weather affects the ability to distinguish between static features (such as buildings) and dynamic environmental features (such as vegetation), given the nature of their appearance when it rains. This part of the evaluation is crucial to determine whether economically efficient navigation can be relied upon in changing weather conditions, especially in areas where heavy rainfall is expected.

[0042] The proposed brightness map generation was tested under adverse weather conditions, when the weather was clear and when it rained heavily. Two types of point cloud maps were created: a dense map that includes data from all available LiDAR frames and a lightmap that retains only invariant features such as buildings and road surfaces, while transient objects like vegetation are filtered out. The dense map contains all the information from the LiDAR points and provides a complete representation of the environment. It is worth noting that the lightmap retains less point density because sensitive points due to weather conditions are removed. Vegetation, characterized by areas 1 and 2, shows a greater reduction in point density in the brightness map compared to the dense map, which is particularly welcome during heavy rain, as this condition tends to make the LiDAR data worse due to scattering and reflection effects. This is because the brightness map, by removing these weather-sensitive points, presents fewer artefacts and provides a clearer view of the campus infrastructure.

[0043] As a result, the size of the point cloud map is reduced from 118 MB for the dense map to 48.3 MB for the light map, which indicates a 59% reduction in density. In addition, although vegetation changes require updating the dense map at a seasonal level, the coarse positioning light map generated based on a low-cost GNSS receiver is robust to seasonal and weather changes. This allows filtering out different vegetation points, making it denser in the map and reducing the update frequency, making it more efficient, reliable and robust to use this map matching algorithm in very adverse weather conditions (such as heavy rain).

[0044] Specifically, dense LiDAR frames are processed to effectively filter out vegetation points, thereby reducing the point cloud density. This filtering step is critical because during heavy rain, the ability of LiDAR to capture accurate data is severely affected. LiDAR scans acquired under heavy rain conditions appear relatively sparse, darker, and have an increased presence of noise points compared to LiDAR scans collected during clear weather. It is worth noting that these noise points are mainly generated by raindrops falling off the laser beam and only account for 1.22% of the total points received per scan, which is a negligible amount. Under heavy rain conditions, the number of measured points decreases, the intensity values ​​decrease, and some noise points are randomly present in the scan due to laser reflections from raindrops. Table 1 summarizes the statistics for three additional rainfall conditions in the environment, indicating that on average, 23% of the points are lost in a scan cycle. However, even after losing these points, the impact of noise points due to reflected raindrops is very small, which again strengthens the strength of the filtering method under weather conditions.

[0045] Table 1 Evaluation of rainfall impact

[0046] The reduction in the number of reception points affects not only the number of features to be extracted that are relevant for a good localization, but also their quality. The accumulated ground reflectance features for about 200 scans during the localization test. In contrast, it can be observed that some areas do not have any ground reflectance features during rainfall, while others show no change. This is due to the different permeability of surface materials, which is why some areas, such as grass on the roadside, which easily allows groundwater to enter the soil, reflect less changes during rainfall. Table 2 gives the number of features corresponding to each scene. As the rainfall intensity increases, the number of observable ground objects decreases significantly. However, the features that are less affected by rainfall change very little. Although falling raindrops can absorb some lidar points, it is unlikely that they will absorb all the points that hit the surface.

[0047] Table 2 Characteristic observations under different weather conditions

[0048] After preprocessing, an integrated localization algorithm was implemented using the light map and dense map generated under heavy rain conditions, denoting the brightness frame and bright point cloud map as LL, the brightness frame and dense point cloud map as LD, and the dense frame and dense point cloud map as DD. The brightness frame image can be accurately registered on the brightness frame image and on the dense map, similar to the registration of dense frame images on dense maps. In response to the current fair weather conditions, the LL method has the shortest processing time, averaging 107.5 ms, while the LD and DD methods have higher processing times, 136.1 ms and 243.8 ms, respectively. The processing time of all methods increases from light rain to heavy rain, with the DD method reaching a critical peak of 284.7 ms during heavy rain. This behavior changes beneficially when the integrated Pareto method is used: the efficiency is greatly improved across methods and conditions. When the DD method is considered for heavy rain, the processing time for heavy rain is reduced from 284.7 ms to 104.4 ms. The optimization highlights the efficiency of the Pareto method in finding the right balance between computational efficiency and accuracy; thus, it may be applicable for real-time map matching in adverse weather conditions.

[0049] Table 3 summarizes the comparison of processing time under different environmental conditions and map matching methods, highlighting the efficiency of individual and combined Pareto methods. The data shows that the brightness frame and brightness map (LL) method has the shortest processing time in good weather conditions, with an average of 107.5 ms and an RMS of 32.1 ms. In contrast, the brightness frame and dense map (LD) method has an average processing time of 136.1 ms and an RMS of 47.4 ms, while the dense frame and dense map (DD) method has the longest processing time, with an average processing time of 243.8 ms and an RMS of 65.3 ms. When the weather gets worse, the processing time of each method increases. During heavy rain, the LL method has an average of 125.2 ms with a standard deviation of 37.39 ms. On the other hand, the average time for the LD and DD methods under the same weather conditions is 155.5 ms and 284.7 ms, and the RMS value is 54.3 ms and 76.2 ms, respectively. This increase is due to the increased complexity and noise brought by bad weather, which will eventually affect the frame quality and map matching accuracy.

[0050] The processing time of the integrated Pareto method was reduced under all conditions and methods, with the average time for the LL method under heavy rain conditions reduced from 125.2 ms to 70.8 ms, the LD method reduced from 155.5 ms to 81.5 ms, and the DD method reduced from 284.7 ms to 104.4 ms. The same pattern was maintained regardless of the weather changes. For example, in good weather conditions, it can be reduced to 60.8 ms using the LL method, 71.5 ms using the LD method, and 89.3 ms using the DD method. This is not only accurate, but also proves that the method used in this paper has high processing efficiency. Overall, the above data shows that the brightness frame reduces the calculation time by more than 43.4%. It also shows that the RMS value of both the LL and LD methods has a slight reduction. Although the utilization of the light map (brightness map) does not significantly reduce the processing time, it does effectively reduce the map size. At present, the method known as the integrated Pareto is effective in optimizing real-time map matching systems, especially in difficult conditions, because it balances computational performance and accuracy.

[0051] Table 3 Comparison of processing time

[0052] Table 4 shows the comparison of localization estimation errors for different settings in the map matching algorithm under different weather conditions, using three different methods: (1) light frame and light map (LL), (2) light frame and dense map (LD), and (3) dense frame and dense map (DD). For this comparison, the error metrics will include AME and RMSE. Since the localization performance is evaluated as a balance under different environmental conditions, the integrated Pareto method is applied to optimize the trade-off between these error metrics.

[0053] Table 4 Comparison of positioning estimation errors

[0054] As mentioned above, a comprehensive analysis of the error performance in various sensor fusion methods has been conducted, and clear patterns of error distribution are observed in the odometry, dead reckoning, and proposed Pareto sensor fusion methods. In the LL case, the proposed Pareto sensor fusion has a lower cumulative error distribution compared to odometry and dead reckoning, indicating that this method has better performance in minimizing the absolute position error. For the LD scenario, although the error level increases compared to LL, the error level in the empirical cumulative distribution function (CDF) of the simulated error data is also more favorable for the proposed Pareto sensor fusion. With further reduction in the error rate, the DD method extends the advantage of the proposed Pareto sensor fusion, giving it the lowest cumulative error among all methods. Further confirmation of these observations is obtained from the values ​​of the root mean square error, as the proposed Pareto sensor fusion produces the smallest RMSE value in each case, which confirms its ability to produce accurate and reliable position estimates. These results once again strengthen the strength of the proposed Pareto sensor fusion and can effectively improve the error performance of different magnitudes of errors and complexities in sensor data.

[0055] In good weather conditions, the localization error is lowest for brightness frames vs. brightness maps, so this setting provides a fair baseline for position accuracy: it balances computational efficiency with reduced frame and map density to avoid overfitting. However, the dense setting, which provides detailed environmental features, does not provide a significant improvement in accuracy under good conditions. As weather conditions worsen, localization performance deteriorates for all configurations. Moderate and heavy rain lead to a large increase in position error, with more significant error increases shown in the denser configurations (brightness frames vs. dense maps and dense frames vs. dense maps). This is likely because the lidar sensor interacts more with rain, i.e., transient environmental features such as raindrops and wet surfaces introduce noise.

[0056] The brightness frames with brightness maps are fairly stable in light rain conditions, with low localization errors; even in moderate and heavy rain conditions, this configuration suffers some accuracy loss. Still, it performs better than denser ones that are resistant to ambient noise. While dense maps may provide benefits in ideal conditions, more lightweight approaches (especially brightness frames with brightness maps) may provide more robust localization performance in adverse weather conditions (such as heavy rain), as noise can compromise the integrity of dense point clouds.

[0057] The east and north position errors of the three selected frame and map accuracy configurations: Light frame with Light map (LL), Lightframe with Dense map (LD), and Dense frame with Dense map (DD). The results are given for all four different weather conditions: sunny, light rain, moderate rain, and heavy rain. The change in the configuration's east position error over time reflects the fluctuation of positioning accuracy when the environment changes. (1) LL (Light Frame with Light Map) configuration, where the LL (Light Frame with Light Map) configuration performs stably under good weathering conditions, with an east position error generally less than 0.5 meters. It performs roughly the same in light rain weather. However, in light rain and rainy conditions, its east position error begins to deteriorate, sometimes exceeding 0.75 meters. Although the LL configuration is robust under benign conditions, its sparse map cannot capture and compensate for dynamic environmental changes (such as raindrops and wet surfaces), resulting in large position accuracy deviations. (2) LD (Light Frame with Dense Map) configuration, where the LD configuration has a slightly higher error under adverse weather conditions. The performance is relatively stable on sunny days, with a position error of around -0.25 to 0.25 m. Light and heavy rain cause large fluctuations in power and position errors. In most heavy rain conditions, the positioning error exceeds 0.5 m. The increased map accuracy captures transient features such as raindrops and reflections that the positioning algorithm cannot evenly filter out, thereby reducing accuracy. (3) DD (Dense Frame with Dense Map) configuration, where the maximum position error values ​​under different weather conditions are for the DD configuration. In good weather conditions, the errors are very close to the values ​​of the other two configurations, mostly less than 0.5 m. However, as the rain starts to get heavier, these position errors become unstable, with most exceeding 0.75 m and approaching 1 m in heavy rain. The combination of dense frame and dense map greatly amplifies environmental noise and captures too many transient or irrelevant features, such as artifacts caused by rain. This will lead to overfitting and a serious loss of positioning accuracy.

[0058] The comparative analysis shows that the LL configuration has the most stable performance, as the position error is constant and lower compared to other configurations, whether it is sunny or light rain. This means that the frame and map density in LL optimally balance the representation of the environment to resist noise, and is therefore the most reliable for general use, especially in adverse conditions. In contrast, the LD and DD configurations can provide finer details of environmental identification, but are more susceptible to environmental noise, especially when it is raining heavily outside. In the dense configuration, redundant data points are introduced, adding a lot of noise and reducing the positioning accuracy, which means that too much detail in the environment model is negative in dynamic and noisy conditions, because the positioning system cannot filter out useful information from the noise. It can be seen that LL has the best performance in any weather conditions, especially in highly disturbing conditions such as heavy rain. Although the denser LD and DD can capture more details, they are also more susceptible to noise, which can lead to significant errors in weather degradation. The frame and map accuracy are appropriately selected to achieve the best positioning accuracy, at least in the presence of environmental noise, especially in highly dynamic and challenging outdoor conditions.

[0059] By comparing the different analyses, it can be noted for all configurations that rain significantly increases the localization errors, where the severity of these errors increases with the intensity of rain. However, the most consistent performance is provided by the LL configuration, as the error deviations are smaller in both sunny and rainy conditions. The lightweight frame and map settings are more effective in handling noisy and dynamic environments, without capturing irrelevant details that interfere with localization accuracy. However, in heavy rain conditions, the LD setting is more sensitive to rain, while the dense map provides more details of the environment. It may be useful in ideal conditions, but it introduces more noise in bad weather, leading to larger localization errors. This leads to the highest position error in all weather types in the DD configuration, which uses both dense frames and dense maps. Such a configuration, especially in rainy conditions, captures too much noise, which can prevent the system from maintaining a good localization, making it the least suitable configuration for use in challenging environments. Therefore, the LL configuration is the most robust and reliable in localization performance under different weather conditions, especially in rainy conditions. The denser configurations, whether involving dense maps or frames, or both, are more prone to noise and position errors, especially in rainy environmental conditions. These results bring back to the question of optimizing the localization system by balancing environmental detail and noise immunity, so the brightness frame and map settings may perform better when dealing with dynamic and challenging outdoor conditions, which are often accompanied by rain or other types of environmental interference.

[0060] It should be noted that these results provide important insights into the performance and optimization possibilities of sensor fusion methods in terms of processing time and position estimation errors. A very effective strategy to remove vegetation from LiDAR point clouds to reduce point cloud density and improve computational efficiency is this reduction, especially for light frame configurations, significantly reduces the processing load that the map matching algorithm needs to perform. Although vegetation removes some low-quality features, it does not significantly affect the localization accuracy because structural type features (such as buildings) dominate the map matching process. These features are more stable and invariant, therefore, they provide a more reliable reference for localization.

[0061] Computational results show that using lighter maps alone provides limited benefit in terms of increased processing time, while merging it with light frame generation can reduce computational requirements and appear to shrink further in severe weather conditions. This is evident from the examples that have been demonstrated using moderate and heavy rain, where processing times are significantly lower than those using dense frames and maps. As autonomous vehicle systems further evolve, computational complexity increases. When sensor fusion systems will start to integrate many features and functions, the optimization of each subsystem will become more important. By simplifying the map to reduce the computational load, in addition to enhancing real-time processing capabilities, the proposed method also opens new avenues for deploying complex localization and map matching algorithms in challenging dynamic environments.

[0062] Localization in rain-affected environments is one of the most challenging tasks for any autonomous driving localization system, given the rain-induced variability in sensor data. LiDAR point cloud data exhibits high variability in rain conditions, as each beam scatters and attenuates the signal. These effects degrade the accuracy of map matching algorithms, especially those that rely on dense point clouds representing transient environmental features such as vegetation. These features like vegetation are very sensitive to rain, thus increasing the variability of point-to-point localization performance during moderate and heavy rain. The proposed Pareto sensor fusion approach achieves optimal map generation by judiciously excluding these rain-sensitive features. As a result, the reference map will be very stable and reliable, as it filters out points associated with vegetation, which tend to fluctuate continuously with weather conditions and seasons. This reduction in point cloud density will increase the efficiency of the process and reduce the adverse effects of rain on LiDAR-based map matching.

[0063] By integrating brightness frames and lightmaps to make the system more robust in rain-affected environments, this setup is well balanced in light or moderate rainfall as it minimizes the computational load without compromising localization accuracy. When the rain is heavy, the additional contribution of temporal removal of the point cloud becomes more important. While dense point clouds can have higher spatial resolution, the increased distortion of the LiDAR signals makes them more susceptible to noise and errors in heavy rainfall conditions. The presented lightweight map will be more suitable for these conditions and therefore focuses on keeping features that are more stable and not affected by rain, such as buildings and road boundaries, which can serve as reliable markers for localization.

[0064] Therefore, rain-affected environments need to be further processed in a more automated and adaptive manner. In practice, this will involve manually removing rain-sensitive features such as vegetation through tools such as Google Maps combined with coarse GNSS data. Automating this using machine learning and computer vision will greatly improve accuracy and efficiency. For example, deep learning models can be used to identify and exclude real-time elements in the lidar point cloud that are affected by rain. This will provide more consistent lidar performance under different weather conditions without the need for human intervention. Such adaptive filtering can also further enhance the system's ability to maintain high positioning accuracy under heavy rainfall conditions, thereby improving the reliability of autonomous vehicle navigation in rain-affected environments. In addition, integrating data from weather forecasts or rainfall sensors into the positioning framework can dynamically adjust the filtering process of the point cloud according to real-time weather conditions. Although the proposed Pareto sensor fusion method has greatly improved the positioning performance under rain-affected conditions, there is still a lot of room for optimization. Automating the process of removing rain-sensitive features and adaptive filtering techniques using real-time environmental data are very promising for further enhancing the robustness and accuracy of future autonomous positioning systems.

[0065] The result is stronger if GNSS data is also considered in map matching in good signal conditions to provide large-scale resilience to dead reckoning errors that grow larger over time. This hybrid approach, which fuses GNSS with lidar and inertial sensors in a consensus framework, has the potential to further extend the system’s operating range by improving its adaptability in real-world conditions. The Pareto-optimized localization framework optimally balances accuracy and computational efficiency in different conditions. This multi-objective approach excels at providing high adaptability and accuracy, especially in complex urban environments. Combining Pareto optimization with lightweight maps and sensor fusion enhances the system’s resilience and accuracy, making it suitable for real-time, weather-variable localization for autonomous vehicle applications. The Pareto-optimized localization approach shows large improvements in all metrics, with a 40-45% reduction in RMSE (from 0.0965m to 0.0566m in heavy rain), significantly improving localization flexibility and accuracy over traditional methods. By using a brightness map that retains only essential invariant features, the approach reduces point cloud density by 59%, reducing computational load and improving performance in adverse weather conditions. At the same time, the Pareto optimized brightness frame and brightness map (LL) method reduced the processing time from 107.5 ms to 60.8 ms under sunny conditions and from 284.7 ms to 104.4 ms under heavy rain conditions, proving the efficiency and accuracy of the method. The real-time application of the Pareto method in dynamic and climate change environments is effective, while pursuing higher positioning accuracy and higher processing efficiency.

[0066] In all examples shown and described herein, any specific values ​​should be interpreted as merely exemplary and not as limiting, and thus other examples of the exemplary embodiments may have different values.

[0067] It should be noted that similar reference numerals and letters denote similar items in the following drawings, and therefore, once an item is defined in one drawing, further definition and explanation thereof is not required in subsequent drawings.

[0068] The above-mentioned embodiments only express several implementation methods of the present invention, and the description thereof is relatively specific and detailed, but it cannot be understood as limiting the scope of the present invention. It should be pointed out that for ordinary technicians in this field, several modifications and improvements can be made without departing from the concept of the present invention, which all belong to the protection scope of the present invention.

Claims

1. A vehicle positioning method based on Pareto and sensor fusion, characterized in that: The following steps are involved: Acquire sensing data collected by multiple sensors of the autonomous driving vehicle, wherein the sensing data includes vehicle driving data, point cloud data of 3D laser radar, GNSS signals and preset map data, and the vehicle driving data includes wheel speed and yaw rate; A dead reckoning model based on vehicle kinematics is constructed according to the vehicle driving data, and the point cloud data of the 3D laser radar is recorded on a brightness frame generation module to perform brightness frame extraction to obtain a brightness map; Acquire multiple reference points of the brightness map, build a positioning model of the autonomous driving vehicle according to the multiple reference points, and fuse the dead reckoning model with the positioning model to obtain a position estimation result; The position estimation result is input into the Pareto positioning model for optimization analysis to obtain the vehicle positioning result.

2. The vehicle positioning method based on Pareto and sensor fusion according to claim 1, characterized in that: A dead reckoning model based on vehicle kinematics is constructed according to the vehicle driving data, and the point cloud data of the 3D laser radar is recorded on a brightness frame generation module to perform brightness frame extraction to obtain a brightness map, including: The coordinates corresponding to the point cloud data of the 3D laser radar are transformed into navigation coordinates. The transformation expression is: （1） in, are the coordinates of the midpoint in the luminance frame, is the translation vector from the lidar frame to the navigation frame, is the rotation matrix between the lidar coordinates and the navigation coordinates, the rotation matrix The definition expression is: （2） in, , and is the heading, pitch and roll information, and ; After there is a point cloud in the navigation frame, the brightness frames are accumulated together to form a bright spot cloud map.

3. The vehicle positioning method based on Pareto and sensor fusion according to claim 1, characterized in that: Acquiring a plurality of reference points of the brightness map and constructing a positioning model of the autonomous driving vehicle according to the plurality of reference points includes: Set the plane where the autonomous vehicle is located to , ,in, , For discrete-time indices, the estimated position is based on a set of fixed reference points or distance measurements at known locations. , and ,in ; The distance between the position node and each reference point in time is expressed as , observation distance yes Noise measurement value, distance measurement value Additive zero-mean Gaussian noise The corresponding expression is: （3） Among them, the variance of the noise Related to distance, modeled as: （4） in, and is a known constant, It is the distance is the baseline variance of the noise when , sets the initial level of the noise variance and represents the minimum noise variance, is a constant and is used to control the noise variance as the distance increases with the increase of .

4. The vehicle positioning method based on Pareto and sensor fusion according to claim 3 is characterized in that: The dead reckoning model and the positioning model are fused and estimated to obtain a position estimation result, including: By fusing the two estimates, the autonomous vehicle is estimated at time The two estimated values ​​include the distance measured using the weighted least squares algorithm and the dead reckoning using the speed and direction measurements. The fused estimate is recorded as , the corresponding expression is: （5） （6） The estimation problem is solved by using the speed and direction readings to update the measured distance to estimate the vehicle's The temporal position, where the estimated It is determined by the output of two estimators, including an estimator based on ranging and a dead reckoning estimator. The expression of position estimation is: （7） in, is the position estimate from the measured distance, is the dead-reckoning position estimate, and is satisfied The sensor fusion parameters are estimated based on the measured distance and dead reckoning fusion And provide The corresponding expressions for the eastern and northern positions in time are: （8） （9） in, Indicated in Estimated eastern position in time, Indicated in Estimated northern position in time, Indicated in Sensor fusion parameters for estimating the east position of the measured distance in time, Indicated in Sensor fusion parameters for dead reckoning east position estimate in time, Indicated in Sensor fusion parameters for estimating the north position of the measured distance in time, Indicated in Sensor fusion parameters for dead reckoning north position estimate in time.

5. The vehicle positioning method based on Pareto and sensor fusion according to claim 4, characterized in that: The position estimation result is input into the Pareto positioning model for optimization analysis to obtain the vehicle positioning result, including: Determining ideal sensor fusion parameters using error state constrained Pareto algorithm and , and strike a balance between minimizing position and heading errors in the estimates and managing noise variance; The error state vector It can be expanded to represent the individual errors of the east and north position, velocity and heading, and the corresponding expressions are: （10） in, and is the true east and north position, and True east and north speeds, is the true heading, with a corresponding estimated value , , , and , the error state covariance matrix is ​​defined as: （11） The total error is expressed as the weighted sum Q of the individual errors using a weighting matrix: （12） in, To determine the diagonal matrix of the relative importance of each error component, （13） The noise covariance matrix associated with the measurement noise vector that affects dead reckoning and range measurements is expressed as: （14） Among them, the variances of distance measurement noise, velocity noise and heading noise are respectively expressed as , and express.

6. The vehicle positioning method based on Pareto and sensor fusion according to claim 5, characterized in that: The error state constrained Pareto algorithm estimates the position and heading errors in the dead reckoning model and obtains the error measurements of the east and north positions, speeds, and headings. The estimated errors of the east and north positions, speeds, and headings are expressed in matrix form as follows: （15） in, Denotes the error state vector, and the Pareto optimization problem is to minimize the estimation error and noise variance, subject to the sensor fusion parameters and The corresponding objective function is expressed as: Make obedient and (16) Where Q is the weighting matrix of the error components, R is the noise covariance matrix associated with the sensor noise, is a regularization parameter that controls the trade-off between minimizing the estimation error and managing the noise variance, represents the maximum allowable error to ensure that the constraints are met; the sensor fusion parameters are selected using the optimization framework and , reducing positioning and heading errors while controlling the overall noise variance.

7. The vehicle positioning method based on Pareto and sensor fusion according to claim 6, characterized in that: The constrained optimization problem of the error-state constrained Pareto algorithm is estimated to balance the estimation error and sensor noise, including: Initial estimate: Determines an estimate based on the dead reckoning model and measured distances , , , and ,The initial estimate will provide a benchmark for calculating the error and determining the fusion parameters; Error propagation: Use formula (15) to propagate the estimated error Estimated as true state The difference from the initial estimate , propagate the difference and calculate the covariance matrix in formula (11) , to quantify the errors from different sources; Noise analysis: The noise covariance matrix of formula (14) is calculated by analyzing the sensor characteristics ; Optimization analysis: Solve the Pareto optimization problem in formula (15) to determine the fusion parameters and , minimize the weighted sum of the estimation error and the noise variance, and keep and , showing that Lagrange multipliers are useful for managing equality constraints in nonlinear optimization problems ; Update estimation: After obtaining the optimal fusion parameters and After that, update the location Estimation, speed and heading , the corrected best estimate includes both range and dead reckoning data; The Pareto front is used to visualize the solution of the Pareto optimization problem in formula (16), where the points on the curve correspond to different methods of weighing the minimization of estimation error and the management of noise variance. The points on the Pareto front are the optimal solutions and correspond to the minimum overall error, satisfying the constraints on estimation accuracy and noise variance.

8. The vehicle positioning method based on Pareto and sensor fusion according to claim 1, characterized in that: The position estimation result is input into the Pareto positioning model for optimization analysis to obtain the vehicle positioning result, including: The state space model is used to analyze the stability of the fusion estimation process. The state vector at time It is expressed as: （17） A discrete-time linear system can be used to model the evolution of this state vector: （18） in, is the state transition matrix, which defines the state from time arrive The evolution of The control input The input matrix mapped to the state, is process noise, and the default process noise is a zero-mean Gaussian random vector with a covariance matrix , the measurement model relates the state vector to the observed measurement value, and the corresponding expression is: （19） in, is the measurement matrix, To measure noise, preset is a zero-mean Gaussian distribution with covariance, and by analyzing the state transfer matrix to test the characteristic value of; If all eigenvalues ​​are inside the unit circle, the system is stable; If any eigenvalue is outside the unit circle radius, the localization process is unstable.

9. The vehicle positioning method based on Pareto and sensor fusion according to claim 8, characterized in that: According to the state transfer matrix The eigenvalue analysis of the process estimates the stability of the process. When all eigenvalues ​​of are located on the unit circle of the complex plane, the system is stable and the corresponding expression is: （20） If any eigenvalue is greater than 1, the system is unstable.

10. The vehicle positioning method based on Pareto and sensor fusion according to claim 9, characterized in that: Positive definite functions using Lyapunov candidate functions Stability analysis is performed on the stability, and the state vector is defined as: （21） Among them, P is a positive definite matrix. As time goes by, the decrease of Lyapunov function indicates the stability of the system. The corresponding expression is: （22） Substituting the state space equation into the Lyapunov function yields: （23） in, If is a negative value, then: （24） Formula (24) is the Lyapunov stability criterion. If there exists a positive definite matrix that satisfies formula (24), then stability exists.

Images