Map construction method fusing laser radar inertial navigation satellite navigation multi-modal data

Abstract

The invention discloses a map construction method fusing laser radar inertial navigation satellite navigation multi-modal data, and relates to the technical field of vehicle autonomous positioning and high-precision map construction. The method comprises the steps of obtaining multi-source original data of a laser radar point cloud, and performing preprocessing; sequentially constructing a laser speedometer constraint, an inertia pre-integration constraint, a loopback detection constraint and a global satellite navigation system constraint based on the preprocessed data, and constructing a factor graph optimization framework according to each constraint; in the loopback detection constraint, a point cloud descriptor construction method based on vertical layering of a cylindrical coordinate system is adopted, and a low-dimensional global descriptor with discrimination capability is formed by performing space division on a point cloud and extracting local height features; a delay switching strategy between a laser inertial system and a satellite inertial system is adopted in the factor graph optimization framework; and carrying out positioning and mapping service based on the factor graph optimization framework. According to the method, the precision and reliability of map construction in a complex environment can be remarkably improved.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle autonomous positioning and high-precision map construction, and in particular to a map construction method integrating laser radar, inertial navigation, and satellite navigation multimodal data. Background Art

[0002] With the rapid development of intelligent transportation systems and autonomous driving technologies, high-precision map construction has become a key technology for autonomous vehicle navigation. Traditional map construction methods primarily rely on single sensors and suffer from numerous limitations. While global satellite navigation systems can provide global location information, signal obstruction and multipath effects severely degrade positioning accuracy in complex environments such as urban canyons and tunnels, making them inadequate for high-precision mapping. LiDAR, as an active sensor, can acquire three-dimensional geometric information about the environment and offers high ranging accuracy and angular resolution. However, when used alone, it is susceptible to interference from dynamic objects and suffers from reduced matching performance in feature-sparse environments. Inertial measurement units (IMUs) can provide high-frequency attitude and motion information, but their measurements are subject to drift errors that accumulate over time. Long-term, standalone use can significantly degrade positioning accuracy.

[0003] Existing multi-sensor fusion mapping methods suffer from the following major problems: First, sensor fusion strategies are not rational enough, failing to fully leverage the complementary advantages of each sensor under different environmental conditions; second, loop closure detection algorithms are inefficient, making it difficult to handle real-time mapping requirements in large-scale environments; third, there is a lack of effective data quality assessment mechanisms, making it impossible to adaptively select the optimal sensor combination; and fourth, the factor graph optimization framework is not well designed, failing to effectively constrain long-term drift errors. These problems seriously restrict the practical application of high-precision map construction technology in complex urban environments. Summary of the Invention

[0004] The purpose of the present invention is to provide a map construction method that integrates multimodal data of lidar, inertial navigation, and satellite navigation, aiming to solve or improve at least one of the above-mentioned technical problems.

[0005] To achieve the above object, the present invention provides the following solutions:

[0006] A map construction method integrating multimodal data of lidar, inertial navigation and satellite navigation, comprising:

[0007] Acquire multi-source raw data of the lidar point cloud and perform preprocessing to obtain preprocessed data; the preprocessing includes dedistortion processing and motion compensation processing;

[0008] Based on the preprocessed data, laser odometry constraints, inertial pre-integration constraints, loop detection constraints, and global satellite navigation system constraints are sequentially constructed, and a factor graph optimization framework is constructed based on each constraint. The loop detection constraint adopts a point cloud descriptor construction method based on vertical stratification of a cylindrical coordinate system, which forms a low-dimensional but discriminative global descriptor by spatially partitioning the point cloud and extracting local height features. The factor graph optimization framework adopts a delayed switching strategy between the laser inertial system and the satellite inertial system, and performs a smooth transition based on a measure of the difference in position information between the two systems.

[0009] Based on the factor graph optimization framework, positioning and mapping services are provided to achieve globally consistent map construction.

[0010] Optionally, the multi-source raw data of the laser radar point cloud is obtained and preprocessed to obtain preprocessed data, and the specific process includes:

[0011] First, the multi-source original data is subjected to dedistortion processing:

[0012] The angular velocity and linear velocity information of the inertial measurement unit are used to calculate the motion trajectory during the lidar scan. Timestamp compensation is performed on each laser point. The radar is assumed to move uniformly within the cycle. The interpolation method is used to calculate the pose change of each point. All laser points are uniformly transformed to the same time base to eliminate the influence of motion distortion. The pose change formula is:

[0013]

[0014] Among them, p i is the original coordinate of the i-th point, is the coordinate after distortion, t k-1,i is the acquisition time of the i-th point, t k is the starting time of the kth frame, t k-1 is the start time of the k-1th frame, T k-1 is the radar pose transformation matrix at the start of the k-1th frame;

[0015] Establish a unified time base system to time-align data from different sensors. This time alignment uses a combination of hardware time synchronization and software interpolation to ensure temporal consistency of sensor data. It also enables effective fusion of sensor data at different frequencies through timestamp matching and linear interpolation.

[0016] The raw data of the inertial measurement unit is low-pass filtered to remove high-frequency noise and outliers, and the positioning data of the global satellite navigation system is validated to eliminate abnormal data with poor positioning accuracy. The lidar point cloud is also downsampled and outliers are removed.

[0017] Optionally, based on the preprocessed data, laser odometry constraints, inertial pre-integration constraints, loop detection constraints, and global satellite navigation system constraints are sequentially constructed, and a factor graph optimization framework is constructed according to each constraint. The specific process includes:

[0018] Based on the preprocessed data, a laser odometry system for inter-frame registration is constructed and laser odometry information is solved to determine laser odometry constraints; the laser odometry information includes relative pose estimation data;

[0019] Based on the laser odometry information, a pre-integration calculation is performed according to the kinematic model of the inertial measurement unit, a tightly coupled laser inertial odometry is constructed, and the inertial measurement unit odometry information is solved to determine the inertial pre-integration constraints; the tightly coupled laser inertial odometry includes an optimized pre-integrator and an inertial measurement unit pre-integrator;

[0020] The Cartesian coordinate system point cloud data collected by the lidar is converted into a cylindrical coordinate system, and the point cloud is divided into three-dimensional space in the cylindrical coordinate system. The feature matrix in the three-dimensional space is averaged in the height direction to obtain a two-dimensional descriptor matrix. Position alignment is performed based on the two-dimensional descriptor matrix and cosine similarity to determine loop detection constraints;

[0021] Constructing a multi-dimensional signal quality indicator. When the signal quality of the global satellite navigation system meets the multi-dimensional signal quality indicator, using the positioning information of the global satellite navigation system as a trustworthy constraint and constructing a global satellite navigation system constraint.

[0022] A factor graph optimization framework is constructed according to the laser odometry constraint, the inertial pre-integration constraint, the loop detection constraint and the global satellite navigation system constraint, and a delayed switching strategy is adopted to smoothly switch between the laser inertial system and the satellite inertial system.

[0023] Optionally, based on the pre-processed data, a laser odometry system for inter-frame registration is constructed and laser odometry information is solved. The specific process includes:

[0024] Based on the preprocessed data, the laser radar point cloud is rearranged according to the line bundle, the local geometric features of each point are calculated, and the point cloud is classified into two categories: corner point features and plane point features using a curvature calculation method; wherein the corner point features correspond to edge structures in the environment and have larger curvature values; and the plane point features correspond to plane structures in the environment and have smaller curvature values.

[0025] For different types of feature points, corresponding registration optimization problems are constructed: for corner features, a point-to-line registration method is used, and for planar point features, a point-to-surface registration method is used. By minimizing the geometric constraint error, the relative pose transformation between frames is solved:

[0026] Define the plane as n(n,d), where n is the unit length normal and d is the intercept. For a point p outside the plane, i For example, the distance from a point to a plane is n T p i +d, construct the point and the nearest neighbor q i The error function of the plane where it is located is solved, and the derivative formula with respect to R and T is solved; where R is the rotation matrix and T is the translation vector;

[0027] Define the straight line as p = ξλ + p0, where ξ is the direction vector of unit length, p0 is a point on the straight line, and λ is a scalar parameter. i For example, using the definition of cross product, taking the length from point to line as the error function, we construct the formula for optimizing the cross product vector and the derivative formula with respect to R and T.

[0028] Based on the calculation results of the relative pose transformation between frames, the Gauss-Newton method or the Levenberg-Marquardt method is used to solve the nonlinear least squares optimization problem, and the rotation matrix and translation vector between frames are iteratively calculated. The laser odometry information is obtained through multiple iterative optimizations.

[0029] Optionally, based on the laser odometry information, a pre-integration calculation is performed according to a kinematic model of an inertial measurement unit to construct a tightly coupled laser inertial odometry and solve the inertial measurement unit odometry information. The specific process includes:

[0030] Establish the kinematic model of the inertial measurement unit, consider the bias and noise characteristics of the gyroscope and accelerometer, and assume that the state vector of the inertial measurement unit at time t is: x = (R, v, p, b g ,b a ), where R is the rotation matrix, v is the velocity vector, p is the position vector, and b g and b a are the bias vectors of the gyroscope and accelerometer respectively;

[0031] A pre-integration calculation is performed on the inertial measurement data from time i to time j to obtain relative motion information; where i, j∈[1,t], and the pre-integration calculation process is independent of the current state estimate, and no recalculation is required when the state changes; the pre-integration process includes two types of pre-integrators, namely an optimized pre-integrator and an inertial measurement unit pre-integrator; based on the optimized pre-integrator and the inertial measurement unit pre-integrator, a tightly coupled laser inertial odometry is constructed and the inertial measurement unit odometry information is solved.

[0032] Optionally, after receiving the first frame of lidar odometer information, the optimization pre-integrator is first initialized, and then the posture information provided by the lidar odometer is added to the factor graph as a priori factor, and the speed prior factor and the inertial measurement unit bias prior factor are continued to be added. In order to respond to the change of the zero-position deviation in real time, when the number of factors in the factor graph exceeds a preset threshold, the factor graph is reset: the cached posture, speed and inertial measurement unit bias at the last critical moment are added as priori factors to the newly created factor graph; for the original inertial measurement unit data in the optimization queue with a timestamp before the current lidar odometer, the inertial measurement unit pre-integrator is called to perform integration processing; then, the inertial measurement unit factor is constructed based on the integration result; finally, the inertial measurement unit factor is jointly optimized with the current lidar odometer data to obtain the latest zero-position deviation estimate;

[0033] The inertial measurement unit pre-integrator uses the received latest zero-position deviation estimate value in combination with the original data to perform an integration operation and finally outputs the inertial measurement unit odometer information.

[0034] Optionally, the Cartesian coordinate point cloud data collected by the laser radar is converted into a cylindrical coordinate system, and the point cloud is divided into three-dimensional space in the cylindrical coordinate system. The feature matrix in the three-dimensional space is averaged in the height direction to obtain a two-dimensional descriptor matrix, and position alignment is performed based on the two-dimensional descriptor matrix and cosine similarity. The specific process includes:

[0035] Convert the Cartesian coordinate point cloud data collected by the lidar into a cylindrical coordinate system to improve the rotation invariance of the descriptor. For each point (x, y, z) in the point cloud, construct a cylindrical coordinate system.

[0036] The point cloud is divided into three-dimensional space in the cylindrical coordinate system, and is divided into N along the radial, angle and height directions respectively. r radial intervals, N θ Angle interval and N Z height layers, each spatial unit (a, b, c) corresponds to a three-dimensional grid, where a∈[1,N r ],b∈[1,N θ ],c∈[1,N Z ];

[0037] For each spatial unit, the maximum height value of all points in the area is extracted as the feature representation:

[0038] H(a,b,c)=max{z|(r,θ,z)∈cell(a,b,c)}

[0039] Among them, r represents radial data and θ represents angle;

[0040] The three-dimensional feature matrix is ​​averaged in the height direction to obtain a two-dimensional descriptor matrix:

[0041]

[0042] Expand the two-dimensional descriptor matrix into a one-dimensional feature vector, and use cosine similarity to measure the similarity between different frames. For the descriptor vector d of the current frame q and the descriptor vector d of the historical frame h , similarity calculation formula S(P q ,P h )for:

[0043]

[0044] A hierarchical navigable small-world graph algorithm is used to perform approximate nearest neighbor search to retrieve several candidate frames with the highest similarity.

[0045] The retrieved candidate frames are geometrically consistent and the results of false matches are eliminated. The credibility of the candidate frames is evaluated by calculating the spatial distribution consistency of the matching point pairs. The vector directions of all matching point pairs are calculated, their variance distribution is analyzed, and the frame with the highest geometric consistency is selected as the final loop matching result.

[0046] Optionally, a multi-dimensional signal quality indicator is constructed. When the signal quality of the global satellite navigation system meets the multi-dimensional signal quality indicator, the positioning information of the global satellite navigation system is used as a trust constraint. The specific process includes:

[0047] Establish a multi-dimensional signal quality assessment system and dynamically adjust the weight of each indicator according to different environmental conditions; the multi-dimensional signal quality indicators include horizontal geometric dilution of precision, vertical geometric dilution of precision, number of visible satellites, signal-to-noise ratio, and positioning status;

[0048] Design a threshold judgment mechanism. When the signal quality index meets the preset conditions, the positioning results of the global satellite navigation system are used as a trust constraint. When the signal quality does not meet the preset conditions, it switches to a positioning mode based on the fusion of laser odometry and inertial measurement unit.

[0049] Consider the IMU as a motion model, the GNSS observation as an observation model, and set the sliding window IMU state to X = [x0, x1, ..., x n ], where x = [p, v, R, b g ,b a ,g], R is the rotation matrix, v is the velocity vector, p is the position vector, b g and b a are the bias vectors of the gyroscope and accelerometer respectively;

[0050] GNSS information and IMU information of different frequencies are time-aligned through soft synchronization. There will be several IMU information frames between two GNSS information frames. The IMU pre-integration factor is added as a priori factor, and the maximum a posteriori estimation problem is constructed:

[0051]

[0052] Among them, r IMU and r GNSS is the residual of IMU pre-integration factor and GNSS factor, n represents the number of IMU pre-integration factors, and m represents the number of GNSS factors; and represents the covariance, Represents the residuals of rotation T, linear velocity v, position p, gyroscope and accelerometer bias, represents the GNSS observation factor, and X represents the optimization variable.

[0053] According to the specific embodiments provided by the present invention, the present invention discloses the following technical effects:

[0054] The present invention discloses a map construction method that integrates multimodal data of laser radar inertial navigation satellite navigation. The method comprises obtaining multi-source raw data of laser radar point cloud and performing preprocessing; constructing laser odometry constraints, inertial pre-integration constraints, loop detection constraints and global satellite navigation system constraints in sequence based on the preprocessed data, and constructing a factor graph optimization framework according to each constraint; in the loop detection constraint, a point cloud descriptor construction method based on vertical stratification of cylindrical coordinate system is adopted, and a low-dimensional but discriminative global descriptor is formed by spatially dividing the point cloud and extracting local height features; in the factor graph optimization framework, a delayed switching strategy between the laser inertial system and the satellite inertial system is adopted; and positioning and mapping services are performed based on the factor graph optimization framework. The present invention can realize efficient loop detection and adopts a multi-constraint factor graph optimization framework, which significantly improves the accuracy and reliability of map construction in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0056] Figure 1 Schematic diagram of a multi-sensor data acquisition platform in this embodiment;

[0057] Figure 2 This is a flowchart of the laser radar point cloud dedistortion process in this embodiment;

[0058] Figure 3 Schematic diagram of the optimized pre-integrator and inertial pre-integrator in this embodiment;

[0059] Figure 4 Schematic diagram of the cylindrical coordinate system point cloud space division in this embodiment;

[0060] Figure 5 Schematic diagram of a global satellite navigation system filter in this embodiment;

[0061] Figure 6 This is a structural diagram of the satellite inertial odometry factor diagram in this embodiment;

[0062] Figure 7 This is the multi-constraint factor graph optimization structure diagram in this embodiment DETAILED DESCRIPTION

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

[0064] The purpose of the present invention is to provide a map construction method that integrates multimodal data of lidar, inertial navigation, and satellite navigation, aiming to solve or improve at least one of the above-mentioned technical problems.

[0065] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0066] like Figure 1-Figure 7 As shown, the present invention provides a map construction method that integrates multimodal data of lidar inertial navigation satellite navigation, including:

[0067] S1, multi-sensor data preprocessing and synchronization:

[0068] Raw multi-sensor data requires unified preprocessing and time synchronization due to varying sampling frequencies, inconsistent timestamps, and sensor noise. First, the LiDAR point cloud data is dedistorted to eliminate the effects of vehicle motion on point cloud acquisition. Point cloud distortion caused by vehicle motion during LiDAR data acquisition is compensated for by high-frequency motion information provided by the inertial measurement unit.

[0069] S1.1 LiDAR motion distortion compensation:

[0070] The angular velocity and linear velocity information of the inertial measurement unit are used to calculate the motion trajectory during the LiDAR scan and perform timestamp compensation on each laser point. Assuming that the motion of the LiDAR is uniform within the cycle, the interpolation method can be used to calculate the pose change of each point. The transformation formula is as follows (1). Where, p i is the original coordinate of the i-th point, is the coordinate after distortion, t k-1,i is the acquisition time of the point, t k is the starting time of the kth frame, t k-1 is the start time of the k-1th frame.

[0071]

[0072] Based on the uniform motion assumption, the coordinate transformation matrix of the point at the start of scanning is calculated, and all laser points are uniformly transformed to the same time base to eliminate the influence of motion distortion.

[0073] S1.2 Multi-sensor time synchronization:

[0074] Establish a unified time base system to time-align data from different sensors. A combination of hardware time synchronization and software interpolation ensures temporal consistency across sensor data. Effective fusion of sensor data at different frequencies is achieved through timestamp matching and linear interpolation.

[0075] S1.3 Sensor data filtering and preprocessing:

[0076] Low-pass filtering is performed on the raw data from the inertial measurement unit to remove high-frequency noise and outliers. The global satellite navigation system's positioning data is validated to remove outliers with poor positioning accuracy. The lidar point cloud is downsampled and outliers are removed to improve the computational efficiency of subsequent processing.

[0077] S2, laser odometry construction and feature extraction:

[0078] Based on pre-processed LiDAR point cloud data, a laser odometry system with inter-frame registration is constructed. By extracting the geometric features of the point cloud, accurate registration between adjacent frames is achieved, resulting in high-frequency relative pose estimation.

[0079] S2.1 Point cloud feature classification and extraction:

[0080] The LiDAR point cloud is rearranged according to the line bundle, and the local geometric features of each point are calculated. Curvature calculation is used to classify the point cloud into two categories: corner features and planar features. Corner features correspond to edge structures in the environment and have larger curvature values; planar features correspond to planar structures in the environment and have smaller curvature values.

[0081]

[0082] S2.2 Point-to-line and point-to-surface registration optimization:

[0083] For different types of feature points, we construct corresponding registration optimization problems. For corner features, we use a point-to-line registration method; for planar features, we use a point-to-surface registration method. By minimizing the geometric constraint error, we solve the relative pose transformation between frames.

[0084] Define the plane as n(n,d), where n is the unit length normal and d is the intercept. For a point p outside the plane i For example, the distance from the point to the plane is n T p i +d. This point can be constructed with its nearest neighbor q i The error function of the plane is given by formula (3), and then the derivatives with respect to R and T are obtained as formula (4).

[0085] e i =n T (Rq i +t)+d(3)

[0086]

[0087] Define a straight line as p = ξλ + p0, where ξ is a direction vector of unit length, p0 is a point on the straight line, and λ is a scalar parameter in the parametric equation of the straight line, used to describe the position of a point on the straight line (different values ​​of t correspond to different points on the straight line). i For example, using the definition of cross product, the length from the point to the line is used as the error function, and the cross product vector can be optimized as shown in formula (5), and the derivative with respect to R and t is formula (6).

[0088] e i =d×(Rq i +t-p0)(5)

[0089]

[0090] S2.3 Iterative closest point optimization solution:

[0091] The Gauss-Newton method or the Levenberg-Marquardt method is used to solve the nonlinear least squares optimization problem, iteratively calculating the rotation matrix and translation vector between frames. Through multiple iterations of optimization, accurate laser odometry information is obtained.

[0092] S3, inertia pre-integration constraint construction:

[0093] In order to effectively utilize the high-frequency measurement information of the inertial measurement unit and avoid processing a large amount of inertial data during the optimization process, the pre-integration technology is used to accumulate the continuous inertial measurements.

[0094] S3.1 Inertial measurement model establishment:

[0095] Establish the kinematic model of the inertial measurement unit, taking into account the bias and noise characteristics of the gyroscope and accelerometer. Assume that the state vector of the inertial measurement unit at time t is: x = (R, v, p, b g ,b a ) Where R is the rotation matrix, v is the velocity vector, p is the position vector, b g and b a are the bias vectors of the gyroscope and accelerometer, respectively.

[0096] S3.2 Pre-integration calculation process:

[0097] For the inertial measurement data from time i to time j, the cumulative integral calculation is performed to obtain the relative motion information. The pre-integration calculation process is independent of the current state estimate and does not need to be recalculated when the state changes, which greatly improves the calculation efficiency. The pre-integrated rotation, velocity and displacement observations are defined as:

[0098]

[0099]

[0100] where δφ ij ,δv ij ,δp ij Pre-integrate the noise term for the inertial measurement unit.

[0101] S3.3 tightly coupled laser inertial odometry:

[0102] This module designs two types of pre-integrators: optimized pre-integrator and inertial measurement unit pre-integrator (structure see Figure 2 Because the zero-point bias of the IMU drifts over time during operation, the module requires real-time zero-point bias estimates. Therefore, the optimization pre-integrator maintains an optimization queue and estimates the latest zero-point bias using observation data provided by the lidar odometry. The IMU pre-integrator maintains an IMU data queue and integrates the latest estimated zero-point bias with the received raw data to output the IMU odometry.

[0103] The optimization pre-integrator workflow is as follows: After receiving the first frame of lidar odometry, the factor graph and its optimizer are initialized. Next, the pose information provided by the lidar odometry is added to the factor graph as a prior factor, followed by velocity and IMU bias prior factors. To respond to changes in the zero-bias in real time, the factor graph is reset when the number of factors in the factor graph exceeds a preset threshold. The cached pose, velocity, and IMU bias at the previous critical moment are added as prior factors to the newly created factor graph. For the raw IMU data in the optimization queue with a timestamp before the current lidar odometry frame, the IMU pre-integrator is called to perform integration. An IMU factor is then constructed based on this integration result. Finally, this IMU factor is added to the factor graph along with the current lidar odometry data for joint optimization, resulting in the latest zero-bias estimate, which is then passed to the IMU pre-integrator.

[0104] The IMU pre-integrator uses the latest received zero-bias estimate and integrates it with the raw data to perform an integration operation, ultimately outputting high-precision IMU odometry information. This mechanism enables tightly coupled odometry between the LiDAR and IMU.

[0105] S4, loop closure detection based on cylindrical coordinate system:

[0106] To detect whether a vehicle has returned to a previously visited location, an efficient loop closure detection algorithm was designed. This algorithm constructs a compact global descriptor of the point cloud based on cylindrical coordinates and achieves fast location recognition through similarity calculation.

[0107] S4.1 Cylindrical coordinate system point cloud transformation:

[0108] The Cartesian coordinate system point cloud data collected by the lidar is converted into a cylindrical coordinate system to improve the rotation invariance of the descriptor. For each point (x, y, z) in the point cloud, its cylindrical coordinate system is represented as:

[0109]

[0110] z=z(12)

[0111] The cylindrical coordinate system has natural rotation invariance, making scene recognition more robust under different vehicle headings.

[0112] S4.2 Space division and descriptor construction:

[0113] The point cloud is divided into three-dimensional space in the cylindrical coordinate system, and is divided into N along the radial, angle and height directions respectively. r radial intervals, N θ Angle interval and N ZEach spatial unit (a, b, c) corresponds to a three-dimensional grid, where a∈[1,N r ],b∈[1,N θ ],c∈[1,N Z ].

[0114] For each spatial unit, the maximum height value of all points in the area is extracted as the feature representation:

[0115]

[0116] In order to reduce the computational complexity, the three-dimensional feature matrix is ​​averaged in the height direction to obtain a two-dimensional descriptor matrix:

[0117]

[0118] S4.3 Similarity calculation and candidate frame retrieval:

[0119] Expand the two-dimensional descriptor matrix into a one-dimensional feature vector and use cosine similarity to measure the similarity between different frames. q and the descriptor vector d of the historical frame h , and its similarity is calculated as:

[0120]

[0121] A hierarchical navigable small-world graph algorithm is used to achieve efficient approximate nearest neighbor search and quickly retrieve several candidate frames with the highest similarity.

[0122] S4.4 Geometric consistency verification:

[0123] Verify the geometric consistency of the retrieved candidate frames to eliminate false matches. Evaluate the credibility of the candidate frames by calculating the spatial distribution consistency of the matching point pairs. Calculate the vector directions of all matching point pairs, analyze their variance distribution, and select the frame with the highest geometric consistency as the final loopback matching result.

[0124] S5, Global Satellite Navigation System Signal Quality Assessment:

[0125] To address the problem of large fluctuations in global satellite navigation system signal quality in complex urban environments, a real-time signal quality assessment and screening mechanism has been designed to ensure that only high-quality positioning information is involved in the map construction process.

[0126] S5.1 Multi-dimensional signal quality indicators:

[0127] Establish a comprehensive signal quality assessment system, using multiple indicators such as horizontal geometric dilution of precision, vertical geometric dilution of precision, number of visible satellites, signal-to-noise ratio, and positioning status for comprehensive evaluation. Dynamically adjust the weight of each indicator based on different environmental conditions to achieve adaptive quality assessment.

[0128] S5.2 Signal screening strategy:

[0129] A threshold judgment mechanism is designed. When the signal quality index meets the preset conditions, the positioning results of the global satellite navigation system are added as a trustworthy constraint into the optimization framework. When the signal quality is poor, the system automatically switches to a positioning mode based on the fusion of laser odometry and inertial measurement unit.

[0130] S5.3 Satellite Inertial Odometry:

[0131] Sliding window factor graph optimization regards IMU as a motion model and GNSS observation as an observation model, and sets the sliding window IMU state to X = [x0, x1, ..., x n ], where x = [p, v, R, b g ,b a ,g], GNSS and IMU messages of different frequencies are time-aligned through soft synchronization. There will be several IMU messages between two GNSS frames. Here, the IMU pre-integration factor is added as a priori factor and used to calculate the zero bias estimate. The maximum a posteriori estimation problem is constructed as shown in the formula.

[0132]

[0133] where r IMU and r GNSS is the residual of the IMU pre-integration factor and the GNSS factor, n represents the number of IMU pre-integration factors, and m represents the number of GNSS factors. and represents the covariance, Represents the residuals of rotation R, linear velocity v, position p, gyroscope and accelerometer bias, represents the GNSS observation factor, and X represents the optimization variable.

[0134] S6, Multi-constrained Factor Graph Optimization:

[0135] A factor graph optimization framework with multiple constraint types is constructed to unify the laser odometry constraints, inertial pre-integration constraints, global satellite navigation system constraints, and loop detection constraints into a single optimization problem to achieve globally consistent map construction.

[0136] S6.1 Switching between laser inertial odometer and satellite inertial odometer:

[0137] Achieve smooth switching between multi-sensor systems, avoiding positioning jumps during the switching process. A delayed switching strategy is used. When the global satellite navigation system signal recovers well, it is first used as a constraint to correct the accumulated error of the laser inertial odometry. The system switching is then performed after the position estimates of the two systems converge.

[0138] S6.2 Factor graph model construction:

[0139] A graph structure model consisting of state nodes and factor nodes is established. State nodes represent the vehicle's posture state at different times, including parameters such as position, attitude, velocity, and inertial sensor bias. Factor nodes represent different types of constraint relationships and connect corresponding state nodes.

[0140] S6.3 Multiple types of constraint factors:

[0141] The laser odometry factor connects pose nodes at adjacent moments, providing inter-frame relative motion constraints. The inertial pre-integration factor connects adjacent keyframes, providing kinematic constraints and bias estimates. The global navigation satellite system factor provides absolute position constraints to suppress long-term drift. The loop detection factor connects non-adjacent historical pose nodes, providing global consistency constraints.

[0142] S6.4 Joint Optimization Solution:

[0143] A nonlinear least-squares optimization method is used to solve the factor graph optimization problem. The overall objective function is constructed as the weighted sum of squared residuals of each type of constraint, and the optimal state estimate is obtained through an iterative optimization algorithm. The uncertainty of each constraint is considered during the optimization process, and the corresponding covariance matrix is ​​used for weighting.

[0144] S6.5 sliding window optimization:

[0145] To maintain real-time performance, a sliding window strategy is used to limit the number of optimization variables. The most recent keyframe is retained for optimization, while earlier historical states are marginalized and converted into prior constraints. This preserves the constraints of historical information while controlling computational complexity.

[0146] Through the collaborative work of the above steps, the present invention realizes the construction of high-precision maps that integrate multimodal data from lidar, inertial measurement units, and global satellite navigation systems. It can provide stable and reliable positioning and mapping services in complex urban environments, and provides important technical support for the autonomous navigation of intelligent vehicles.

[0147] The various embodiments in this specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the various embodiments can be referenced to each other.

[0148] This document uses specific examples to illustrate the principles and implementation methods of the present invention. The above examples are only intended to help understand the core concept of the present invention. At the same time, those skilled in the art will find that the specific implementation methods and application scopes may vary based on the concept of the present invention. In summary, the contents of this specification should not be construed as limiting the present invention.

Claims

1. A map construction method integrating multimodal data of lidar, inertial navigation and satellite navigation, characterized in that: include: Acquire multi-source raw data of the lidar point cloud and perform preprocessing to obtain preprocessed data; The preprocessing includes dedistortion processing and motion compensation processing; Based on the preprocessed data, laser odometry constraints, inertial pre-integration constraints, loop detection constraints, and global satellite navigation system constraints are sequentially constructed, and a factor graph optimization framework is constructed based on each constraint. The loop detection constraint adopts a point cloud descriptor construction method based on vertical stratification of a cylindrical coordinate system, which forms a low-dimensional but discriminative global descriptor by spatially partitioning the point cloud and extracting local height features. The factor graph optimization framework adopts a delayed switching strategy between the laser inertial system and the satellite inertial system, and performs a smooth transition based on a measure of the difference in position information between the two systems. Based on the factor graph optimization framework, positioning and mapping services are provided to achieve globally consistent map construction.

2. The map construction method according to claim 1, characterized in that: The multi-source raw data of the laser radar point cloud is obtained and preprocessed to obtain preprocessed data. The specific process includes: First, the multi-source original data is subjected to dedistortion processing: The angular velocity and linear velocity information of the inertial measurement unit are used to calculate the motion trajectory during the lidar scan. Timestamp compensation is performed on each laser point. The radar is assumed to move uniformly within the cycle. The interpolation method is used to calculate the pose change of each point. All laser points are uniformly transformed to the same time base to eliminate the influence of motion distortion. The pose change formula is: Among them, p i is the original coordinate of the i-th point, is the coordinate after distortion, t k-1,i is the acquisition time of the i-th point, t k is the starting time of the kth frame, t k-1 is the start time of the k-1th frame, T k-1 is the radar pose transformation matrix at the start of the k-1th frame; Establish a unified time base system to time-align data from different sensors. This time alignment uses a combination of hardware time synchronization and software interpolation to ensure temporal consistency of sensor data. It also enables effective fusion of sensor data at different frequencies through timestamp matching and linear interpolation. The raw data of the inertial measurement unit is low-pass filtered to remove high-frequency noise and outliers, and the positioning data of the global satellite navigation system is validated to eliminate abnormal data with poor positioning accuracy. The lidar point cloud is also downsampled and outliers are removed.

3. The map construction method according to claim 1, characterized in that: Based on the preprocessed data, laser odometry constraints, inertial pre-integration constraints, loop detection constraints, and global satellite navigation system constraints are sequentially constructed, and a factor graph optimization framework is constructed according to each constraint. The specific process includes: Based on the preprocessed data, a laser odometry system for inter-frame registration is constructed and laser odometry information is solved to determine laser odometry constraints; the laser odometry information includes relative pose estimation data; Based on the laser odometry information, a pre-integration calculation is performed according to the kinematic model of the inertial measurement unit, a tightly coupled laser inertial odometry is constructed, and the inertial measurement unit odometry information is solved to determine the inertial pre-integration constraints; the tightly coupled laser inertial odometry includes an optimized pre-integrator and an inertial measurement unit pre-integrator; The Cartesian coordinate system point cloud data collected by the lidar is converted into a cylindrical coordinate system, and the point cloud is divided into three-dimensional space in the cylindrical coordinate system. The feature matrix in the three-dimensional space is averaged in the height direction to obtain a two-dimensional descriptor matrix. Position alignment is performed based on the two-dimensional descriptor matrix and cosine similarity to determine loop detection constraints; Constructing a multi-dimensional signal quality indicator. When the signal quality of the global satellite navigation system meets the multi-dimensional signal quality indicator, using the positioning information of the global satellite navigation system as a trustworthy constraint and constructing a global satellite navigation system constraint. A factor graph optimization framework is constructed according to the laser odometry constraint, the inertial pre-integration constraint, the loop detection constraint and the global satellite navigation system constraint, and a delayed switching strategy is adopted to smoothly switch between the laser inertial system and the satellite inertial system.

4. The map construction method according to claim 3, characterized in that: Based on the pre-processed data, a laser odometry system with inter-frame registration is constructed and the laser odometry information is solved. The specific process includes: Based on the preprocessed data, the laser radar point cloud is rearranged according to the line bundle, the local geometric features of each point are calculated, and the point cloud is classified into two categories: corner point features and plane point features using a curvature calculation method; wherein the corner point features correspond to edge structures in the environment and have larger curvature values; and the plane point features correspond to plane structures in the environment and have smaller curvature values. For different types of feature points, corresponding registration optimization problems are constructed: for corner features, a point-to-line registration method is used, and for planar point features, a point-to-surface registration method is used. By minimizing the geometric constraint error, the relative pose transformation between frames is solved: Define the plane as n(n,d), where n is the unit length normal and d is the intercept. For a point p outside the plane, i For example, the distance from a point to a plane is n T p i +d, construct the point and the nearest neighbor q i The error function of the plane where it is located is solved, and the derivative formula with respect to R and T is solved; where R is the rotation matrix and T is the translation vector; Define the straight line as p = ξλ + p0, where ξ is the direction vector of unit length, p0 is a point on the straight line, and λ is a scalar parameter. i For example, using the definition of cross product, taking the length from point to line as the error function, we construct the formula for optimizing the cross product vector and the derivative formula with respect to R and T. Based on the calculation results of the relative pose transformation between frames, the Gauss-Newton method or the Levenberg-Marquardt method is used to solve the nonlinear least squares optimization problem, and the rotation matrix and translation vector between frames are iteratively calculated. The laser odometry information is obtained through multiple iterative optimizations.

5. The map construction method of fusing laser radar inertial navigation satellite navigation multimodal data according to claim 3 is characterized in that: Based on the laser odometry information, a pre-integration calculation is performed according to the kinematic model of the inertial measurement unit to construct a tightly coupled laser inertial odometry and solve the inertial measurement unit odometry information. The specific process includes: Establish the kinematic model of the inertial measurement unit, consider the bias and noise characteristics of the gyroscope and accelerometer, and assume that the state vector of the inertial measurement unit at time t is: x = (R, v, p, b g ,b a ), where R is the rotation matrix, v is the velocity vector, p is the position vector, and b g and b a are the bias vectors of the gyroscope and accelerometer respectively; A pre-integration calculation is performed on the inertial measurement data from time i to time j to obtain relative motion information; where i, j∈[1,t], and the pre-integration calculation process is independent of the current state estimate, and no recalculation is required when the state changes; the pre-integration process includes two types of pre-integrators, namely an optimized pre-integrator and an inertial measurement unit pre-integrator; based on the optimized pre-integrator and the inertial measurement unit pre-integrator, a tightly coupled laser inertial odometry is constructed and the inertial measurement unit odometry information is solved.

6. The map construction method according to claim 5, characterized in that: After receiving the first frame of lidar odometer information, the optimization pre-integrator is first initialized, and then the posture information provided by the lidar odometer is added as a priori factor to the factor graph, and the speed prior factor and the inertial measurement unit bias prior factor are continued to be added. In order to respond to the change of the zero-position deviation in real time, when the number of factors in the factor graph exceeds the preset threshold, the factor graph is reset: the cached posture, speed and inertial measurement unit bias at the last critical moment are added as priori factors to the newly created factor graph; for the original inertial measurement unit data in the optimization queue with a timestamp before the current lidar odometer, the inertial measurement unit pre-integrator is called to perform integration processing; then, the inertial measurement unit factor is constructed based on the integration result; finally, the inertial measurement unit factor is jointly optimized with the current lidar odometer data to obtain the latest zero-position deviation estimate; The inertial measurement unit pre-integrator uses the received latest zero-position deviation estimate value in combination with the original data to perform an integration operation and finally outputs the inertial measurement unit odometer information.

7. The map construction method of fusing laser radar inertial navigation satellite navigation multimodal data according to claim 3 is characterized in that: The Cartesian coordinate system point cloud data collected by the lidar is converted into a cylindrical coordinate system. The point cloud is divided into three-dimensional space in the cylindrical coordinate system. The feature matrix in the three-dimensional space is averaged in the height direction to obtain a two-dimensional descriptor matrix. Position registration is performed based on the two-dimensional descriptor matrix and cosine similarity. The specific process includes: Convert the Cartesian coordinate point cloud data collected by the lidar into a cylindrical coordinate system to improve the rotation invariance of the descriptor. For each point (x, y, z) in the point cloud, construct a cylindrical coordinate system. The point cloud is divided into three-dimensional space in the cylindrical coordinate system, and is divided into N along the radial, angle and height directions respectively. r radial intervals, N θ Angle interval and N Z height layers, each spatial unit (a, b, c) corresponds to a three-dimensional grid, where a∈[1,N r ],b∈[1,N θ ],c∈[1,N Z ]; For each spatial unit, the maximum height value of all points in the area is extracted as the feature representation: H(a,b,c)=max{z|(r,θ,z)∈cell(a,b,c)} Among them, r represents radial data and θ represents angle; The three-dimensional feature matrix is ​​averaged in the height direction to obtain a two-dimensional descriptor matrix: Expand the two-dimensional descriptor matrix into a one-dimensional feature vector, and use cosine similarity to measure the similarity between different frames. For the descriptor vector d of the current frame q and the descriptor vector d of the historical frame h , similarity calculation formula S(P q ,P h )for: A hierarchical navigable small-world graph algorithm is used to perform approximate nearest neighbor search to retrieve several candidate frames with the highest similarity. The retrieved candidate frames are geometrically consistent and the results of false matches are eliminated. The credibility of the candidate frames is evaluated by calculating the consistency of the spatial distribution of the matching point pairs. The vector directions of all matching point pairs are calculated, their variance distribution is analyzed, and the frame with the highest geometric consistency is selected as the final loop matching result.

8. The map construction method of fusing laser radar inertial navigation satellite navigation multimodal data according to claim 3 is characterized in that: A multi-dimensional signal quality indicator is constructed. When the signal quality of the global satellite navigation system meets the multi-dimensional signal quality indicator, the positioning information of the global satellite navigation system is used as a trust constraint. The specific process includes: Establish a multi-dimensional signal quality assessment system and dynamically adjust the weight of each indicator according to different environmental conditions; the multi-dimensional signal quality indicators include horizontal geometric dilution of precision, vertical geometric dilution of precision, number of visible satellites, signal-to-noise ratio, and positioning status; A threshold judgment mechanism is designed. When the signal quality index meets the preset conditions, the positioning results of the global satellite navigation system are used as a trust constraint. When the signal quality does not meet the preset conditions, the positioning mode is switched to the positioning mode based on the fusion of laser odometry and inertial measurement unit. Consider the IMU as a motion model, the GNSS observation as an observation model, and set the sliding window IMU state to X = [x0, x1, ..., x n ], where x = [p, v, R, b g ,b a ,g], R is the rotation matrix, v is the velocity vector, p is the position vector, b g and b a are the bias vectors of the gyroscope and accelerometer respectively; GNSS information and IMU information of different frequencies are time-aligned through soft synchronization. There will be several IMU information frames between two GNSS information frames. The IMU pre-integration factor is added as a priori factor, and the maximum a posteriori estimation problem is constructed: Among them, r IMU and r GNSS is the residual of IMU pre-integration factor and GNSS factor, n represents the number of IMU pre-integration factors, and m represents the number of GNSS factors; and represents the covariance, Represents the residuals of rotation T, linear velocity v, position p, gyroscope and accelerometer bias, represents the GNSS observation factor, and X represents the optimization variable.

Images