An open road high-precision positioning method based on multi-source fusion

Abstract

The application discloses an open road high-precision positioning method based on multi-source fusion, and the method is: dynamic adaptive laser point cloud feature extraction, multi-source fusion positioning based on a local factor graph, and map full life cycle maintenance based on dynamic feature optimization; through the dynamic feature extraction scheme based on the space degree, the feature extraction is more robust, and the efficiency and effectiveness of the feature extraction in a complex environment are effectively improved; the multi-layer factor graph architecture is adopted, the pose output precision and the robustness of the positioning system in a complex scene are improved, and the positioning system is suitable for a complex scene with high positioning precision requirements; through the map updating mechanism, the map maintenance of the positioning system in the full life cycle is realized, the positioning error failure caused by environmental changes is effectively solved, and the long-term effectiveness of scene elements is improved.

Description

Technical Field

[0001] This invention belongs to the field of autonomous driving and intelligent transportation technology, specifically involving a dynamic adaptive multi-source fusion positioning method based on LiDAR. This method integrates dynamic environmental perception, multi-sensor fusion positioning and map lifecycle management technologies, and is suitable for high-precision real-time positioning in complex scenarios such as urban roads, parks, and overpasses. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) technology, as a core technology in the fields of autonomous driving and mobile robotics, directly impacts the safety and reliability of intelligent systems through its environmental adaptability and positioning accuracy. Current technologies face three main technical bottlenecks in practical road applications:

[0003] (a) Environmental adaptability defects of feature extraction mechanisms

[0004] Traditional laser SLAM systems often employ fixed-parameter feature extraction strategies, such as edge-plane feature classification methods based on curvature thresholds. These static feature extraction schemes exhibit significant limitations in dynamic traffic scenarios: First, fixed thresholds struggle to adapt to fluctuations in point cloud reflectivity caused by lighting conditions and weather changes in the road environment, leading to increased misclassification rates (experimental data shows a misclassification rate of 28.6% in rainy and foggy weather); second, unstructured point cloud features generated by dynamic obstacles (such as vehicles and pedestrians) are easily misidentified as valid environmental features, resulting in cumulative positioning errors during the backend optimization process.

[0005] (II) The challenge of spatiotemporal consistency in multi-source sensor fusion

[0006] Existing fusion positioning systems mostly use extended Kalman filtering or single factor graph frameworks for multi-sensor data fusion, which makes it difficult to effectively coordinate the differences in spatiotemporal characteristics of different sensors: 1) There is a timing mismatch between the high-frequency output (≥100Hz) of the inertial measurement unit (IMU) and the mid-frequency sampling (10-20Hz) of the lidar; 2) GNSS signals are affected by multipath effects in complex scenarios such as urban canyons, resulting in low-frequency jump errors; 3) The odometer error of the wheel speed meter on wet and slippery roads can reach 15% / s.

[0007] (iii) Insufficient map maintenance efficiency in dynamic environments

[0008] Traditional offline mapping methods cannot meet the long-term positioning requirements of open road scenarios: 1) Semi-dynamic objects such as construction barriers and temporary roadblocks cause inconsistencies in mapping data across multiple trips; 2) Changes in point cloud structure caused by seasonal vegetation changes will lead to a decrease in positioning accuracy.

[0009] Therefore, there is an urgent need to build a multi-source fusion positioning system with environmental adaptability. Through the collaborative innovation of dynamic feature extraction mechanism, multi-level fusion architecture and intelligent map maintenance technology, we can break through the existing technical bottlenecks and provide accurate and reliable positioning solutions for autonomous driving systems.

[0010] A search revealed that Chinese invention patent CN117490683A discloses a localization and mapping method for underground tunnels using a multi-sensor fusion algorithm, comprising the following steps: Step 1: Based on a prior high-precision point cloud map, obtain the tunnel's contour projection to construct a global map; Step 2: Initialize the localization system. The vehicle departs from the tunnel's starting intersection and receives a UWB signal located at the intersection to obtain an estimated position of the vehicle at the tunnel intersection; Step 3: Finally, use a fusion algorithm of LiDAR, IMU, and UWB to perform real-time pose estimation of the vehicle.

[0011] This application differs from the aforementioned prior art in the following ways:

[0012] 1. The aforementioned comparative documents obtain real-time pose estimation of unmanned vehicles by fusing sensor data such as LiDAR, IMU and UWB. This application focuses not only on multi-sensor fusion algorithms, but also on the implementation of a series of solutions such as data preprocessing, adaptive feature extraction, dynamic object removal, and long-term map maintenance. The two are fundamentally different in terms of implementation technology and problem solving.

[0013] 2. The aforementioned comparative documents mainly provide solutions for the positioning of unmanned vehicles in underground mines, primarily targeting enclosed scenarios within mines to address the degradation of lidar. This application, on the other hand, targets complex outdoor scenarios such as open roads, addressing the positioning offset problem caused by GNSS jumps due to multipath effects or obstruction in urban areas. It not only meets the positioning needs of indoor scenarios but also addresses the positioning needs of environments such as company parks, open roads, and outdoor parks. The two applications differ fundamentally in their application scenarios and research objectives. Summary of the Invention

[0014] To overcome the shortcomings of the existing technologies, this invention provides a multi-source fusion positioning system and implementation method for open road scenarios. Through the synergistic effect of an environment-adaptive feature extraction mechanism, a multi-level factor graph fusion architecture, and a full lifecycle map maintenance system, it effectively solves key technical problems such as positioning drift, inaccurate multi-source data fusion, and map reliability degradation in dynamic environments.

[0015] This invention is achieved through the following technical solutions:

[0016] A high-precision positioning method for open roads based on multi-source fusion is proposed, comprising the following steps: S1. Dynamic adaptive laser point cloud feature extraction: By receiving raw data from point cloud, GNSS, IMU, and wheel speedometer, GNSS quality assessment, point cloud distortion correction and feature extraction, and IMU and wheel speedometer deviation estimation are performed; S2. Multi-source fusion positioning based on local factor maps: The values ​​of IMU and wheel speedometer are received for trajectory estimation, and high-frequency fusion pose estimation is performed by fusing laser odometry and GNSS odometry; S3. Map lifecycle maintenance based on dynamic feature optimization: A priori map is constructed, and dynamic scene updates and maintenance are achieved through intelligent fusion of multi-source data and an incremental update mechanism. Further, the specific method for step S1 is as follows:

[0017] S11, GNSS quality assessment and data screening

[0018] A multi-level criterion-based filtering mechanism based on satellite visibility and positioning status is constructed to filter GNSS data;

[0019] S12, Point Cloud Distortion Correction

[0020] A spatiotemporal interpolation compensation algorithm based on odometry data derived from IMU and wheel speedometer tracks is used to eliminate motion distortion in laser point clouds;

[0021] S13, Adaptive Point Cloud Feature Extraction

[0022] First, ground point cloud separation based on dynamic dual thresholds is performed. The point cloud is projected onto the XOY plane and divided into different grids in a 1*1 grid format. Ground points are extracted according to the dynamic dual threshold ground point extraction strategy. For each grid point, if the height difference between the point and the lowest point in that grid is greater than threshold h1, or if the height difference between the point and the lowest points of the surrounding 8 grids is greater than threshold h2, it is judged as a non-ground point; as follows:

[0023] ;

[0024] Among them, h k h is the height of point k. min h is the lowest height of the current grid cell. neimin It is the lowest height of the nearest neighboring grid cell to the current point;

[0025] The RANSAC algorithm is used to fit the plane of the ground points, downsample the ground points, calculate the normal vector, impose normal vector constraints, save the ground points that meet the conditions, and update the normal vector.

[0026] An adaptive feature extraction algorithm based on spatial degree is adopted to calculate the openness of the space where the vehicle is currently located. The feature extraction scheme is dynamically adjusted according to the spatial degree, which is defined as follows:

[0027] ;

[0028] in, For the spatial degree of the k-th frame, This represents the average distance of the preprocessed k-frame laser points from the origin. Take 0.8, Set to 0.2; S14, Multi-sensor calibration correction

[0029] For the IMU, a calibration method based on static conditions is adopted: during the system initialization phase, the device is kept still for 10 seconds, and the data from the gyroscope and accelerometer are recorded and processed as zero bias. The measured values ​​are then corrected during operation.

[0030] For wheel speed meters, a dynamic calibration scheme is adopted: select a straight road section with a length greater than 100 meters, maintain a vehicle speed v=40km / h, synchronously record the wheel speed meter output and GNSS mileage, and calculate the wheel speed meter correction factor.

[0031] ;

[0032] in, GNSS track length The length of the wheel speed gauge trajectory.

[0033] Furthermore, the specific method for step S2 is as follows: S21, Construction of the trajectory estimation model

[0034] A recursive localization model based on kinematic constraints is established and defined as follows:

[0035] set up The pose state at time is ,in This is the position in the northeast celestial coordinate system. The heading angle is determined by the IMU angular velocity ω and the wheel speed gauge linear velocity v. Given the sampling period, derive the pose prediction at time k:

[0036] ;

[0037] S22, Laser Odometer

[0038] Inter-frame registration is performed using extracted feature points from two consecutive point clouds to calculate laser odometry. A model for point cloud transformation is constructed. Line feature point groups obtained through projection are rotated and translated to their corresponding similar positions in the newly entered point cloud while maintaining the relationships between feature points, resulting in one set of relationships. The same applies to surface feature points, yielding a second set of relationships. For line features, the minimum straight-line distance to neighboring points is calculated; for surface features, the minimum planar distance to neighboring points is calculated.

[0039] ;

[0040] ;

[0041] in, and These represent the distances from a point to a line and from a point to a surface, respectively. express Feature points in a frame The coordinates; and express The coordinates of two points on the corresponding polyline in the previous frame; , , This represents the coordinates of three points on the corresponding plane in the previous frame;

[0042] ;

[0043] ;

[0044] The above equation is solved using the nonlinear optimization Levenberg-Marquardt method to obtain the optimal parameter estimate, thereby obtaining laser odometry data; S23, GNSS odometry

[0045] A local coordinate system is established using the preprocessed GNSS data. GNSS odometry can be obtained by calculating the position of the latest GNSS data within this local coordinate system. (S24, Multi-source fusion positioning)

[0046] A local factor graph optimization model is constructed to output high-precision positioning results, and the optimal pose is estimated by minimizing the residual function.

[0047] Furthermore, the specific method of step S3 is as follows: S31, Spatiotemporal joint initialization to construct an adaptive initialization model based on sensor state;

[0048] If a usable GNSS is found after data filtering, the laser is synchronized with the GNSS data in time, and the laser is initialized by interpolating the GNSS observation value at the corresponding time. If no usable GNSS is found, the laser is initialized using the odometry calculated from the flight track, in the same way as above.

[0049] The interpolation formula is as follows:

[0050] ;

[0051] ;

[0052] in, For the pose change of the GNSS odometry from time t1 to time t2, For the GNSS odometry pose at time to, for Laser odometry pose initialized by GNSS at all times; S32, dynamic extraction of keyframes, establishing a dual-threshold keyframe selection strategy.

[0053] After extracting keyframes, they are added to the keyframe set. Keyframes within 3 meters are selected, and feature points within them are used to construct a local map. The local map is then downsampled using a voxel grid with a resolution of 0.1 to preserve feature information. S33: Dynamic object filtering and static map generation. Feature points are extracted from keyframes and superimposed on the laser point cloud according to the optimized pose to construct a global map. The global map and keyframe point cloud are converted into a depth map. Dynamic object detection is performed based on depth changes, and non-ground dynamic points are filtered out to obtain a static prior map. S34: Map lifecycle maintenance and updates. Dynamic scene updates and maintenance are achieved through intelligent fusion of multi-source data and an incremental update mechanism.

[0054] Furthermore, the adaptive feature extraction algorithm in step S13 is as follows: When the spatial degree is less than the threshold, a curvature-based feature extraction scheme is adopted:

[0055] The laser sector is evenly divided into six segments. The curvature of each point in each laser line is calculated and sorted by size. The curvature calculation formula is as follows:

[0056] ;

[0057] in for The curvature of a point;

[0058] The 20 largest points are selected as corner points and the 100 smallest points are selected as planar points. After selecting the feature points, the five nearest points are marked and no further processing is performed, thus completing the curvature-based feature extraction.

[0059] When the spatial degree is higher than the threshold, a feature extraction scheme based on spatial features is adopted:

[0060] In the kd-tree, find the nearest neighbor (k=10) of each non-ground point. Perform principal component analysis based on the k nearest neighbors to calculate the PCA parameter. Then, perform eigenvalue decomposition on the covariance matrix formed by these k points to obtain three eigenvalues. Define linearity, flatness, and curvature. ;

[0061] For all non-ground points, they are distinguished according to linearity and flatness, and divided into line features and surface features. According to their direction vectors, line features are further subdivided into column features and beam features, and surface features are further subdivided into elevation features and planar features.

[0062] Furthermore, the method for establishing the local coordinate system in step S23 is as follows: Use the open-source third-party library utm_convert to convert the longitude, latitude, and elevation in the WGS84 coordinate system into the ENU local coordinate system, and take the starting position as the origin of the local coordinate system.

[0063] Furthermore, the local factor map in step S24 includes track estimation factor, laser odometry factor, and GNSS odometry factor;

[0064] The trajectory extrapolation factor is used to provide an initial pose for laser odometry optimization using trajectory extrapolation, and its residual function is as follows:

[0065] ;

[0066] in, For the pose of node k, The pose calculated from the flight path;

[0067] The laser odometry factor uses laser odometry as an inter-frame constraint, and its residual function is as follows:

[0068] ;

[0069] in, For the pose transformation estimated by the laser odometry, log() is the Lie algebra mapping;

[0070] The GNSS odometry factor uses selected GNSS as a constraint to correct laser drift, and its residual function is:

[0071] ;

[0072] in, For the pose of node k, The pose of the GNSS odometer;

[0073] The function to minimize the residual is as follows:

[0074] .

[0075] Further, the specific method of step S34 is as follows: S341. Using the location reference during map construction, overlay different maps according to a unified spatial reference; S342. Divide the overlaid map into several grids, convert the old and new maps into depth maps according to the point cloud information based on the grids, calculate the depth difference between the old and new maps within the same grid, and identify grids exceeding a threshold as changed grids. Filter out changed grids that exist in the old map but not in the new map, and retain changed grids that are not in the old map but appear in the new map. This is used for map maintenance and updating to obtain an updated map. Further, map maintenance and updating includes filtering out disappeared areas and merging newly added areas;

[0076] The filtering of the disappearing region is performed as follows:

[0077] ;

[0078] in, and The corresponding numbers for the old and new maps are respectively Block grid, The grid to be filtered out;

[0079] The newly added regions are merged using a weighted merging update, as follows:

[0080] ;

[0081] ;

[0082] in, To add a new grid, For updated map information, , As a confidence weight.

[0083] Furthermore, it also includes step S4: based on the prior map, perform frame matching and principal factor map optimization, the specific methods of which are as follows:

[0084] S41. Inter-frame registration under prior map constraints

[0085] The current laser frame is downsampled using a voxel grid with a resolution of 0.1, and then matched with the prior map for pose estimation. Based on line features and surface features, point-line residuals and point-surface residuals between the current frame and the prior map are constructed respectively. The residuals are then optimized using Gauss-Newton iteration to obtain the optimal pose estimate for the current laser odometry.

[0086] S42, Principal Factor Chart Optimization

[0087] The frame matching results are added to the factor graph as inter-frame constraints and fused with the high-frequency localization results output by the fusion localization part to obtain the final pose estimate; the factor graph includes prior map factors and fusion localization factors.

[0088] The residual function of the prior map factor is:

[0089] ;

[0090] in, For the pose transformation estimated by the laser odometry, log() is the Lie algebra mapping;

[0091] The residual function of the fused positioning factor is:

[0092] ;

[0093] in, For the pose of node k, The pose calculated for the local factor map;

[0094] Optimal pose estimation is achieved by minimizing the residual function:

[0095] ;

[0096] The final pose is obtained.

[0097] Compared with the prior art, the technical advantages of the present invention are as follows:

[0098] 1. By adopting a spatial-degree-based dynamic feature extraction scheme, feature extraction becomes more robust, effectively improving the efficiency and effectiveness of feature extraction in complex environments;

[0099] 2. The multi-layer factor graph architecture improves the pose output accuracy and robustness of the positioning system in complex scenarios, making it suitable for complex scenarios with high positioning accuracy requirements.

[0100] 3. Through the map update mechanism, map maintenance is achieved throughout the entire lifecycle of the positioning system, effectively solving the problem of positioning error failure caused by environmental changes and improving the long-term effectiveness of scene elements. Attached Figure Description

[0101] Figure 1 This is a flowchart of the algorithm of the present invention;

[0102] Figure 2 This is a diagram showing the distortion correction effect of the present invention;

[0103] Figure 3 This is a diagram illustrating the ground point extraction effect of the present invention.

[0104] Figure 4This is a diagram illustrating the curvature-based feature extraction effect of the present invention.

[0105] Figure 5 This is a diagram illustrating the feature extraction effect based on spatial features according to the present invention.

[0106] Figure 6 The original point cloud image without removing dynamic objects;

[0107] Figure 7 Point cloud after dynamic object removal;

[0108] Figure 8 This is a display image showing the overlay of the old and new maps;

[0109] Figure 9 Update the resulting image on the map;

[0110] Figure 10 This is for merging the positioning results. Detailed Implementation

[0111] The technical solutions of various embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0112] In the description of this invention, it should be noted that, unless otherwise explicitly specified and limited, the terms "installation," "connection," and "linking" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection, an electrical connection, or a connection that allows for communication; they can refer to a direct connection or an indirect connection through an intermediate medium; they can refer to the internal communication of two components or the interaction between two components. Those skilled in the art can understand the specific meaning of the above terms in this invention according to the specific circumstances.

[0113] In this invention, unless otherwise explicitly specified and limited, "above" or "below" the second feature can include direct contact between the first and second features, or contact between the first and second features through another feature between them. Furthermore, "above," "over," and "on top" of the second feature includes the first feature directly above or diagonally above the second feature, or simply indicates that the first feature is at a higher horizontal level than the second feature. "Below," "below," and "under" the second feature includes the first feature directly below or diagonally below the second feature, or simply indicates that the first feature is at a lower horizontal level than the second feature.

[0114] It will be understood by those skilled in the art that, unless otherwise defined, all terms used herein (including technical and scientific terms) have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. It should also be understood that terms such as those defined in general dictionaries should be understood to have the same meaning as in the context of the prior art, and should not be interpreted in an idealized or overly formal sense unless defined as herein.

[0115] This invention is a high-precision positioning method for open roads based on multi-source fusion. The method is as follows: S1, Dynamic Adaptive Laser Point Cloud Feature Extraction: By receiving raw data from point cloud, GNSS, IMU, and wheel speedometer, GNSS quality assessment, point cloud distortion correction and feature extraction, and IMU and wheel speedometer deviation estimation are completed. The specific method is as follows:

[0116] S11, GNSS quality assessment and data screening

[0117] A multi-level criterion-based filtering mechanism based on satellite visibility and positioning status is constructed. The satellite number threshold criterion is implemented: GNSS observation data with less than 10 visible satellites are discarded; the positioning status criterion is implemented: only the observation values ​​of the RTK fixed solution are retained to filter the GNSS data.

[0118] S12, Point Cloud Distortion Correction

[0119] A spatiotemporal interpolation compensation algorithm based on odometry data derived from IMU and wheel velocity meter tracks is used to eliminate motion distortion in laser point clouds. First, the start and end times of the current laser frame are obtained. The pose at the start and end times of the laser frame is obtained by interpolation in the odometry derived from the track. For each laser point, the corresponding IMU odometry pose is obtained by interpolation using the laser point's timestamp. The relative transformation is calculated with the initial pose. Each point is then projected onto the start time of the laser frame to complete distortion correction.

[0120] ;

[0121] Where Pi_des is the corrected laser point position, Lit0 is the IMU odometry pose at the initial moment of the laser frame where the laser point is located, Liti is the IMU odometry pose at the moment the laser point is located, and Pi is the laser point position before correction. The distortion correction effect is shown in the attached figure. Figure 2 As shown;

[0122] S13, Adaptive Point Cloud Feature Extraction

[0123] First, ground point cloud separation based on dynamic dual thresholds is performed. The point cloud is projected onto the XOY plane and divided into different grids in a 1*1 grid format. Ground points are extracted according to the dynamic dual threshold ground point extraction strategy. For each grid point, if the height difference between the point and the lowest point in that grid is greater than threshold h1, or if the height difference between the point and the lowest points of the surrounding 8 grids is greater than threshold h2, it is judged as a non-ground point; as follows:

[0124] ;

[0125] Among them, h k h is the height of point k. min h is the lowest height of the current grid cell. neimin It is the lowest height of the nearest neighboring grid cell to the current point;

[0126] The RANSAC algorithm is used to perform plane fitting on ground points. Ground points are downsampled, and normal vectors are calculated. Normal vector constraints are applied, and ground points that meet the conditions are saved and their normal vectors are updated. The ground point extraction results are shown in the attached figure. Figure 3 As shown;

[0127] An adaptive feature extraction algorithm based on spatial degree is adopted to calculate the openness of the space where the vehicle is currently located. The feature extraction scheme is dynamically adjusted according to the spatial degree, which is defined as follows:

[0128] ;

[0129] in, For the spatial degree of the k-th frame, This represents the average distance of the preprocessed k-frame laser points from the origin.

[0130] Take 0.8, Take 0.2;

[0131] The adaptive feature extraction algorithm specifically employs a curvature-based feature extraction scheme when the spatial degree is less than a threshold.

[0132] The laser sector is evenly divided into six segments. The curvature of each point in each laser line is calculated and sorted by size. The curvature calculation formula is as follows:

[0133] ;

[0134] in for The curvature of a point;

[0135] The 20 largest points are selected as corner points, and the 100 smallest points are selected as planar points. After selecting the feature points, the five nearest points are marked and no further processing is performed. This completes the curvature-based feature extraction. The result is shown in the attached figure. Figure 4 As shown;

[0136] When the spatial degree is higher than the threshold, a feature extraction scheme based on spatial features is adopted:

[0137] In the kd-tree, find the nearest neighbor (k=10) of each non-ground point. Perform principal component analysis based on the k nearest neighbors to calculate the PCA parameter. Then, perform eigenvalue decomposition on the covariance matrix formed by these k points to obtain three eigenvalues. Define linearity, flatness, and curvature. , , ;

[0138] For all non-ground points, they are distinguished based on linearity and flatness, categorized into line features and surface features. Based on their direction vectors, line features are further subdivided into column features and beam features, and surface features are subdivided into elevation features and planar features. The results are shown in the attached figure. Figure 5 As shown; S14, Multi-sensor calibration correction

[0139] For the IMU, a calibration method based on static conditions is adopted: during the system initialization phase, the device is kept still for 10 seconds, and the data from the gyroscope and accelerometer are recorded and processed as zero bias. The measured values ​​are then corrected during operation.

[0140] For wheel speed meters, a dynamic calibration scheme is adopted: select a straight road section with a length greater than 100 meters, maintain a vehicle speed v=40km / h, synchronously record the wheel speed meter output and GNSS mileage, and calculate the wheel speed meter correction factor.

[0141] ;

[0142] in, GNSS track length S2. Based on the values ​​of the multi-source fusion positioning receiver IMU and wheel speedometer, the trajectory is calculated using the values ​​of the local factor map. High-frequency fusion pose estimation is then performed by fusing laser odometry and GNSS odometry. The specific method is as follows:

[0143] S21, Construction of trajectory estimation model

[0144] A recursive localization model based on kinematic constraints is established and defined as follows:

[0145] set up The pose state at time is ,in This is the position in the northeast celestial coordinate system. The heading angle is determined by the IMU angular velocity ω and the wheel speed gauge linear velocity v. Given the sampling period, derive the pose prediction at time k:

[0146] ;

[0147] S22, Laser Odometer

[0148] Inter-frame registration is performed using extracted feature points from two consecutive point clouds to calculate laser odometry. A model for point cloud transformation is constructed. Line feature point groups obtained through projection are rotated and translated to their corresponding similar positions in the newly entered point cloud while maintaining the relationships between feature points, resulting in one set of relationships. The same applies to surface feature points, yielding a second set of relationships. For line features, the minimum straight-line distance to neighboring points is calculated; for surface features, the minimum planar distance to neighboring points is calculated.

[0149] ;

[0150] ;

[0151] in, and These represent the distances from a point to a line and from a point to a surface, respectively. express Feature points in a frame The coordinates; and express The coordinates of two points on the corresponding polyline in the previous frame; , , This represents the coordinates of three points on the corresponding plane in the previous frame;

[0152] ;

[0153] ;

[0154] The above equation is solved using the nonlinear optimization Levenberg-Marquardt method to obtain the optimal parameter estimate, thereby obtaining laser odometry data; S23, GNSS odometry

[0155] A local coordinate system is established from the preprocessed GNSS data. The GNSS odometry can be obtained by calculating the position of the latest GNSS data in the local coordinate system.

[0156] The method for establishing a local coordinate system is as follows:

[0157] The open-source third-party library utm_convert is used to convert longitude, latitude, and elevation in the WGS84 coordinate system to the ENU local coordinate system, with the starting point location as the origin of the local coordinate system; S24, multi-source fusion positioning.

[0158] A local factor graph optimization model is constructed to output high-precision positioning results, and the optimal pose is estimated by minimizing the residual function.

[0159] The local factor map in step S24 includes the track estimation factor, the laser odometry factor, and the GNSS odometry factor;

[0160] The trajectory extrapolation factor is used to provide an initial pose for laser odometry optimization using trajectory extrapolation, and its residual function is as follows:

[0161] ;

[0162] in, For the pose of node k, The pose calculated from the flight path;

[0163] The laser odometry factor uses laser odometry as an inter-frame constraint, and its residual function is as follows:

[0164] ;

[0165] in, For the pose transformation estimated by the laser odometry, log() is the Lie algebra mapping;

[0166] The GNSS odometry factor uses selected GNSS as a constraint to correct laser drift, and its residual function is:

[0167] ;

[0168] in, For the pose of node k, The pose of the GNSS odometer;

[0169] The function to minimize the residual is as follows:

[0170] S3. A priori map is constructed based on dynamic feature optimization throughout its entire lifecycle. This is achieved through intelligent fusion of multi-source data and an incremental update mechanism, enabling dynamic scene updates and maintenance. The specific methods are as follows:

[0171] S31. Spatiotemporal joint initialization constructs an adaptive initialization model based on sensor states;

[0172] If a usable GNSS is found after data filtering, the laser is synchronized with the GNSS data in time, and the laser is initialized by interpolating the GNSS observation value at the corresponding time. If no usable GNSS is found, the laser is initialized using the odometry calculated from the flight track, in the same way as above.

[0173] The interpolation formula is as follows:

[0174] ;

[0175] ;

[0176] in, For the pose change of the GNSS odometry from time t1 to time t2, For the GNSS odometry pose at time to, for Laser odometry pose initialized by GNSS at all times; S32, dynamic extraction of keyframes, establishing a dual-threshold keyframe selection strategy.

[0177] After extracting keyframes, they are added to a keyframe set. Keyframes within a 3-meter radius are selected, and feature points within them are used to construct a local map. The local map is then downsampled using a voxel grid with a resolution of 0.1 to preserve feature information. S33: Dynamic Object Filtering and Static Map Generation. Feature points from the extracted keyframes are overlaid onto the laser point cloud according to the optimized pose to construct a global map. The global map and keyframe point cloud are converted into a depth map. Dynamic object detection is performed based on depth changes, filtering out non-ground dynamic points to obtain a static prior map. The results are shown in the attached figure. Figure 6 , 7 As shown; S34, Map lifecycle maintenance and updates are achieved through multi-source data intelligent fusion and incremental update mechanisms to update and maintain dynamic scenes. The specific methods are as follows: S341, Using the location benchmark at the time of map construction, different maps are overlaid according to a unified spatial benchmark;

[0178] S342. Divide the overlaid map into several grids. Convert the old and new maps into depth maps based on point cloud information according to the grids. Calculate the depth difference between the old and new maps within the same grid. If the difference exceeds a threshold, it is identified as a changed grid. Filter out changed grids that exist in the old map but not in the new map, and retain changed grids that are not in the old map but appear in the new map. Perform map maintenance and updates in this way to obtain the updated map, as shown in the attached figure. Figure 8 , 9 As shown. Furthermore, map maintenance and updates include filtering out disappearing areas and merging newly added areas;

[0179] The filtering of the disappearing region is performed as follows: ;

[0180] in, and These are the v-th grid cells corresponding to the old and new maps, respectively. The grid to be filtered out;

[0181] The newly added regions are merged using a weighted merging update, as follows: ;

[0182] ;

[0183] in, To add a new grid, For updated map information, , As a confidence weight.

[0184] S4. Based on the prior map, perform frame matching and principal factor map optimization. The specific method is as follows:

[0185] S41. Inter-frame registration under prior map constraints

[0186] The current laser frame is downsampled using a voxel grid with a resolution of 0.1, and then matched with the prior map for pose estimation. Based on line features and surface features, point-line residuals and point-surface residuals between the current frame and the prior map are constructed respectively. The residuals are then optimized using Gauss-Newton iteration to obtain the optimal pose estimate for the current laser odometry.

[0187] S42, Principal Factor Chart Optimization

[0188] The frame matching results are added to the factor graph as inter-frame constraints and fused with the high-frequency localization results output by the fusion localization part to obtain the final pose estimate; the factor graph includes prior map factors and fusion localization factors.

[0189] The residual function of the prior map factor is:

[0190] ;

[0191] in, For the pose transformation estimated by the laser odometry, log() is the Lie algebra mapping;

[0192] The residual function of the fused positioning factor is:

[0193] ;

[0194] in, For the pose of node k, The pose calculated for the local factor map;

[0195] Optimal pose estimation is achieved by minimizing the residual function:

[0196] ;

[0197] Received as attached Figure 10 The final pose shown.

[0198] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the technical solutions of the embodiments of the present invention.

Claims

1. A high-precision positioning method for open roads based on multi-source fusion, characterized in that, The method is as follows: S1. Dynamic adaptive laser point cloud feature extraction: By receiving raw data from point cloud, GNSS, IMU, and wheel speed meter, GNSS quality assessment, point cloud distortion correction and feature extraction, and IMU and wheel speed meter deviation estimation are completed; The specific method of step S1 is as follows: S11, GNSS quality assessment and data screening; A multi-level criterion-based filtering mechanism based on satellite visibility and positioning status is constructed to filter GNSS data; S12, Point cloud distortion correction; A spatiotemporal interpolation compensation algorithm based on odometry data derived from IMU and wheel speedometer tracks is used to eliminate motion distortion in laser point clouds; S13, Adaptive point cloud feature extraction; First, ground point cloud separation based on dynamic dual thresholds is performed. The point cloud is projected onto the XOY plane and divided into different grids in a 1*1 grid format. Ground points are extracted according to the dynamic dual threshold ground point extraction strategy. For each grid point, if the height difference between the point and the lowest point in that grid is greater than threshold h1, or if the height difference between the point and the lowest points of the surrounding 8 grids is greater than threshold h2, it is judged as a non-ground point; as follows: ； Among them, h k h is the height of point k. min h is the lowest height of the current grid cell. neimin It is the lowest height of the nearest neighboring grid cell to the current point; The RANSAC algorithm is used to fit the plane of the ground points, downsample the ground points, calculate the normal vector, impose normal vector constraints, save the ground points that meet the conditions, and update the normal vector. An adaptive feature extraction algorithm based on spatial degree is adopted to calculate the openness of the space where the vehicle is currently located. The feature extraction scheme is dynamically adjusted according to the spatial degree, which is defined as follows: ； in, For the spatial degree of the k-th frame, This represents the average distance of the preprocessed k-frame laser points from the origin. Take 0.8, Set to 0.2; S14, Multi-sensor calibration correction; For the IMU, a calibration method based on static conditions is adopted: during the system initialization phase, the device is kept still for 10 seconds, and the data from the gyroscope and accelerometer are recorded and processed as zero bias. The measured values ​​are then corrected during operation. For wheel speed meters, a dynamic calibration scheme is adopted: select a straight road section with a length greater than 100 meters, maintain a vehicle speed v=40km / h, synchronously record the wheel speed meter output and GNSS mileage, and calculate the wheel speed meter correction factor. ； in, GNSS track length The length of the wheel speed gauge trajectory; S2. Based on the values ​​of multi-source fusion positioning receiver IMU and wheel speed meter based on local factor map, the trajectory is calculated, and the laser odometry and GNSS odometry are fused to perform high-frequency fusion pose estimation. The specific method of step S2 is as follows: S21, Construction of trajectory estimation model; A recursive localization model based on kinematic constraints is established and defined as follows: set up The pose state at time is ,in This is the position in the northeast celestial coordinate system. The heading angle is determined by the IMU angular velocity ω and the wheel speed gauge linear velocity v. Given the sampling period, derive the pose prediction at time k: ； S22, Laser Odometer; Inter-frame registration is performed using extracted feature points from two consecutive point clouds to calculate laser odometry. A model for point cloud transformation is constructed. Line feature point groups obtained through projection are rotated and translated to their corresponding similar positions in the newly entered point cloud while maintaining the relationships between feature points, resulting in one set of relationships. The same applies to surface feature points, yielding a second set of relationships. For line features, the minimum straight-line distance to neighboring points is calculated; for surface features, the minimum planar distance to neighboring points is calculated. ； ； in, and These represent the distances from a point to a line and from a point to a surface, respectively. express Feature points in a frame The coordinates; and express The coordinates of two points on the corresponding polyline in the previous frame; , and This represents the coordinates of three points on the corresponding plane in the previous frame; ； ； The above equation is solved using the nonlinear optimization Levenberg-Marquardt method to obtain the optimal parameter estimate, thereby obtaining laser odometry data; S23, GNSS odometry; A local coordinate system is established from the preprocessed GNSS data. The GNSS odometry can be obtained by calculating the position of the latest GNSS data in the local coordinate system; S24, multi-source fusion positioning; Construct a local factor graph optimization model to output high-precision positioning results, and estimate the optimal pose by minimizing the residual function; S3, Map lifecycle maintenance based on dynamic feature optimization; Construct a prior map and realize the dynamic scene update and maintenance through intelligent fusion of multi-source data and incremental update mechanism; The specific method of step S3 is as follows: S31, Spatiotemporal joint initialization; Construct an adaptive initialization model based on sensor state; If a usable GNSS is found after data filtering, the laser is synchronized with the GNSS data in time, and the laser is initialized by interpolating the GNSS observation value at the corresponding time. If no usable GNSS is found, the laser is initialized using the odometry calculated from the flight track, in the same way as above. The interpolation formula is as follows: ； ； in, For the pose change of the GNSS odometry from time t1 to time t2, For the GNSS odometry pose at time to, for Laser odometry pose initialized by GNSS; S32, dynamic extraction of key frames, and establishment of a dual-threshold key frame selection strategy; After extracting keyframes, they are added to a keyframe set. Keyframes within 3 meters are selected, and feature points within them are used to construct a local map. The local map is then downsampled using a voxel grid with a resolution of 0.1 to preserve feature information. S33: Dynamic Object Filtering and Static Map Generation; Feature points are extracted from keyframes and overlaid on the laser point cloud according to the optimized pose to construct a global map. The global map and keyframe point cloud are converted into a depth map. Dynamic object detection is performed based on depth changes, filtering out non-ground dynamic points to obtain a static prior map. S34: Map Lifecycle Maintenance and Updates; Dynamic scene updates and maintenance are achieved through intelligent fusion of multi-source data and an incremental update mechanism. Step S4: Based on the prior map localization, perform frame matching and principal factor map optimization. The specific method is as follows: S41. Inter-frame registration under prior map constraints; The current laser frame is downsampled using a voxel grid with a resolution of 0.1, and then matched with the prior map for pose estimation. Based on line features and surface features, point-line residuals and point-surface residuals between the current frame and the prior map are constructed respectively. The residuals are then optimized using Gauss-Newton iteration to obtain the optimal pose estimate for the current laser odometry. S42. Principal Factor Plot Optimization; The frame matching results are added to the factor graph as inter-frame constraints and fused with the high-frequency localization results output by the fusion localization part to obtain the final pose estimate; the factor graph includes prior map factors and fusion localization factors. The residual function of the prior map factor is: ； in, For the pose transformation estimated by the laser odometry, log() is the Lie algebra mapping; The residual function of the fused positioning factor is: ； in, For the pose of node k, The pose calculated for the local factor map; Optimal pose estimation is achieved by minimizing the residual function: ； The final pose is obtained.

2. The high-precision positioning method for open roads based on multi-source fusion according to claim 1, characterized in that, The adaptive feature extraction algorithm in step S13 specifically involves: when the spatial degree is less than a threshold, a curvature-based feature extraction scheme is adopted. The laser sector is evenly divided into six segments. The curvature of each point in each laser line is calculated and sorted by size. The curvature calculation formula is as follows: ； in for The curvature of a point; The 20 largest points are selected as corner points and the 100 smallest points are selected as planar points. After selecting the feature points, the five nearest points are marked and no further processing is performed, thus completing the curvature-based feature extraction. When the spatial degree is higher than the threshold, a feature extraction scheme based on spatial features is adopted: In the kd-tree, find the nearest neighbor (k=10) of each non-ground point. Perform principal component analysis based on the k nearest neighbors to calculate the PCA parameter. Then, perform eigenvalue decomposition on the covariance matrix formed by these k points to obtain three eigenvalues. Define linearity, flatness, and curvature. ; For all non-ground points, they are distinguished according to linearity and flatness, and divided into line features and surface features. According to their direction vectors, line features are further subdivided into column features and beam features, and surface features are further subdivided into elevation features and planar features.

3. The high-precision positioning method for open roads based on multi-source fusion according to claim 2, characterized in that, The method for establishing the local coordinate system in step S23 is as follows: Use the open-source third-party library utm_convert to convert the longitude, latitude, and elevation in the WGS84 coordinate system into the ENU local coordinate system, and take the starting position as the origin of the local coordinate system.

4. The high-precision positioning method for open roads based on multi-source fusion according to claim 3, characterized in that, The local factor map in step S24 includes track estimation factor, laser odometry factor and GNSS odometry factor. The trajectory extrapolation factor is used to provide an initial pose for laser odometry optimization using trajectory extrapolation, and its residual function is as follows: ； in, For the pose of node k, The pose calculated from the flight path; The laser odometry factor uses laser odometry as an inter-frame constraint, and its residual function is as follows: ； in, For the pose transformation estimated by the laser odometry, log() is the Lie algebra mapping; The GNSS odometry factor uses selected GNSS as a constraint to correct laser drift, and its residual function is: ； in, For the pose of node k, The pose of the GNSS odometer; The function to minimize the residual is as follows: 。 5. The high-precision positioning method for open roads based on multi-source fusion according to claim 4, characterized in that, The specific method of step S34 is as follows: S341. Using the location reference during map construction, overlay different maps according to a unified spatial reference; S342. Divide the overlaid map into several grids, convert the old and new maps into depth maps according to the point cloud information based on the grids, calculate the depth difference between the old and new maps within the same grid, and if it exceeds the threshold, it is identified as a changed grid. Filter out the changed grids that exist in the old map but not in the new map, and retain the changed grids that do not exist in the old map but appear in the new map. In this way, map maintenance and updates are performed to obtain the updated map.

6. The high-precision positioning method for open roads based on multi-source fusion according to claim 5, characterized in that, The map maintenance and updates include filtering out disappearing areas and merging newly added areas; The filtering of the disappearing region is performed as follows: ； in, and The corresponding numbers for the old and new maps are respectively Block grid, The grid to be filtered out; The newly added regions are merged using a weighted merging update, as follows: ； ； in, To add a new grid, For updated map information, , As a confidence weight.

Images