Extensible laser mapping method based on prior topological information

Abstract

The application discloses an extensible laser mapping method based on prior topological information and belongs to the technical field of multi-sensor fusion of unmanned vehicles. The method comprises the following steps: S1, multi-sensor data acquisition; S2, data preprocessing; S3, radar odometry; S3.1, IMU pre-integration; S3.2, radar point cloud matching; S3.3, pose fusion; S3.4, map construction; S4, map updating and expansion based on topological information; S4.1, dynamic loading module; S4.2, map association; S4.3, map updating evaluation and map expansion and fusion. The method realizes an incremental expansion, globally consistent and resource-efficient laser mapping system by tightly coupling multi-sensor data, embedding prior topological constraints and implementing dynamic resource management, and provides key mapping technical support for long-term autonomous navigation in a dynamic environment when the operation area is expanded.

Description

Technical Field

[0001] This invention relates to the field of multi-sensor fusion technology for autonomous vehicles, specifically a scalable laser mapping method based on prior topological information. Background Technology

[0002] In the fields of autonomous driving and robotics, Simultaneous Localization and Mapping (SLAM) is a key component for achieving autonomous navigation. In autonomous driving systems, SLAM provides crucial technical support for scenarios requiring high-precision positioning, such as parking. In robotics applications, SLAM, with its relatively low implementation cost and high positioning accuracy, has become an important part of robot navigation systems. By building environmental maps in real time and combining them with its own localization, SLAM provides reliable map and location information for subsequent modules such as environmental perception and path planning.

[0003] Currently, SLAM technology mainly employs two implementation schemes: vision-based and laser-based. Vision-based schemes can provide rich texture and color information, enabling SLAM feature extraction and supporting color-based object perception tasks; however, their robustness is weak in complex lighting environments such as low light and strong light. Nevertheless, laser-based SLAM technology remains the mainstream choice for autonomous vehicles and robotic systems requiring stable mapping and localization.

[0004] Methods for LiDAR point cloud matching have matured. Classical methods like ICP and NDT, which match all points between points in the target cloud, suffer from low computational efficiency. However, the feature point matching method employed by LOAM has achieved significant progress in improving both accuracy and computational efficiency in LiDAR point cloud matching. Regarding improvements in accuracy and computational efficiency for LiDAR point cloud matching, F-LOAM proposes replacing automatic differentiation in the matching optimization process with analytical differentiation, thus enhancing computational efficiency. LEGO-LOAM extracts ground feature points by determining whether the curvature values ​​of the current point cloud and surrounding point clouds meet the planar point extraction criteria, constraining point cloud matching and improving SLAM accuracy in the field of ground mobile robots (in this invention, unmanned vehicles). The feature matching method proposed by LOAM provides an algorithmic foundation for subsequent high-precision positioning using LiDAR SLAM.

[0005] To improve the robustness of LiDAR SLAM, multi-sensor fusion SLAM schemes have been developed beyond localization based solely on LiDAR point cloud matching. Fusion methods are mainly divided into two types: optimization-based and filtering-based. Among filtering methods, FAST-LIO, with its high-speed fusion advantage using the Error State Kalman Filter (ESKF), has become the algorithmic foundation for filtering-based fusion schemes. Building upon the FAST-LIO algorithm, FAST-LIO2 directly matches the original point cloud with the map and uses a KD-tree for efficient and rapid management of the real-time point cloud map.

[0006] In optimization-based fusion methods, LIO-SAM uses radar point cloud matching factors and IMU prediction factors as edges for graph optimization, performs backend optimization based on the GTSAM optimizer, and introduces loop closure detection edges, making the entire system have a relatively robust backend. Because the frontend of LIO-SAM is highly dependent on the accuracy of radar point cloud matching, existing technologies dynamically adjust the size of surface features to adapt to different environmental features and utilize adaptive surface elements to construct maps and perform registration, thus optimizing the registration process. Existing technologies use triangular descriptors to construct triangular bag-of-words structures to achieve real-time location recognition and six-DOF loop closure pose estimation. To establish SLAM accuracy in large-scene environments, existing technologies incorporate the absolute positioning obtained from RTK observations as an optimization factor into the graph optimization fusion, enabling high-precision SLAM in open environments. Existing technologies improve the quality of paired points by reducing the number of points to be registered and increasing the number of iteration targets, thereby improving positioning accuracy.

[0007] Current prior map building methods, whether based on filtering (FAST-LIO series) or optimization (LIO-SAM series), directly project each frame of the point cloud during mapping onto the global coordinate system to obtain a holistic point cloud map. These methods have limited map expansion capabilities; as the operating range of autonomous vehicles expands, all areas need to be rebuilt, making it difficult to support long-term, scalable mapping tasks. Summary of the Invention

[0008] To address or partially address the problems existing in related technologies, this invention provides a scalable laser mapping method based on prior topological information. This method achieves seamless stitching and low-memory expansion in multi-segment mapping scenarios by fusing prior topological constraints and combining laser-inertial odometry (LIO) with a dynamic map loading mechanism.

[0009] To achieve the above objectives, the technical solution of the present invention is as follows: A scalable laser mapping method based on prior topological information is provided, the innovation of which lies in the following steps: S1. Multi-sensor data acquisition: Build a data acquisition platform based on unmanned vehicles to acquire three-dimensional point cloud data, and obtain three-axis angular velocity and three-axis force information through IMU to establish a rigid transformation matrix from IMU to LiDAR coordinate system; S2. Data preprocessing: Perform zero-bias estimation and gravity separation on the raw IMU data, and perform motion distortion compensation on the LiDAR point cloud; S3, Radar Odometry: Construct IMU pre-integration factors and radar odometry factors, and perform pose fusion. Construct a global point cloud map based on the fused pose estimation. S4. Map Update Extension Based on Topology Information S4.1 Dynamic Loading Module: Dynamically loads the local sub-map corresponding to adjacent keyframe nodes; S4.2 Map Association: Register the current frame point cloud with the dynamically loaded local sub-map to construct map association constraints; S4.3 Map Update Evaluation and Map Expansion and Integration: Evaluate the matching quality between the newly created map and the existing map to determine whether to expand the map; after meeting the evaluation conditions, integrate the newly created map data into the global map to complete the incremental expansion.

[0010] Preferably, step S1 includes the following steps: S1.1 Acquiring 3D point cloud data: A rotating LiDAR is installed on the top of the unmanned vehicle as the main sensor. The ranging module is driven by an internal motor to perform a 360° omnidirectional high-speed rotation scan around the Z-axis to acquire 3D point cloud data, so as to form a continuous 3D point cloud coverage of the surrounding environment. The scanning frequency is 10Hz. S1.2 Acquisition of triaxial angular velocity and force information: A nine-axis MEMS inertial sensor is selected as a high-frequency inertial measurement sensor (IMU) to output the triaxial angular velocity and triaxial force information of the carrier at a frequency of not less than 400Hz. S1.3 Rigid Constraints: The rotating lidar and high-frequency inertial measurement unit (IMU) are rigidly mounted on the unmanned vehicle via metal brackets, and the different sensors are calibrated with external parameters.

[0011] Preferably, step S2 includes the following steps: S2.1. Initialization of IMU raw data: During the initialization phase, the observations within the static window are used to perform zero bias estimation and subtraction of the gyroscope and accelerometer. At the same time, by analyzing the acceleration vector distribution at the initial moment, the gravitational acceleration component is separated from the raw measurement, thereby establishing the initial horizontal attitude reference in the global coordinate system. S2.2. Motion distortion compensation for LiDAR point clouds: Acquire IMU measurement data aligned with the point cloud scanning cycle time, and use a pre-calibrated rigid transformation matrix to map the original laser point cloud to the IMU coordinate system to achieve spatial dimension alignment between sensors. Considering the non-instantaneous imaging characteristics of rotating radar, the high-frequency IMU pre-integration trajectory within the point cloud scanning cycle of the frame is retrieved in real time. The precise pose increment of each laser point at the moment of emission is calculated by linear interpolation, and based on this, all discrete points are compensated to the reference pose at the beginning of the frame to eliminate point cloud trailing and geometric stretching caused by carrier motion, and restore the distorted original scanning data to a distortion-free point cloud frame with true physical proportions.

[0012] Preferably, the specific process of the radar odometer in step S3 is as follows: S3.1 Constructing the IMU pre-integration factor: The angular velocity and acceleration measurement models for the IMU are defined as follows: in and These are the raw angular velocity and acceleration measurements of the IMU at time t. It refers to the angular velocity of the actual motion. This refers to the gyroscope's zero bias. This refers to the white noise of the gyroscope. The accuracy of the measurement is affected by zero bias. and white noise The impact; It refers to the acceleration of actual motion. It is the rotation matrix of the autonomous vehicle in the global coordinate system. It is gravitational acceleration. This refers to the zero bias of the accelerometer. This refers to the white noise of the accelerometer; Based on IMU measurement data, the motion state of the unmanned vehicle is estimated using a trajectory inference algorithm. At any moment, the speed of the driverless car ,Location and rotation matrix The following kinematic equations are used for calculation: in, Represents the velocity at time t. Indicates the time increment; Indicates the position at time t; Using pre-integration techniques, based on IMU measurement data between adjacent keyframes, the calculation is directly performed from time [time value missing]. At the time The relative increments of the motion state, specifically, the pre-integral increments of rotation, velocity, and position. , and Defined as follows: in, Indicates time The rotation matrix, It is a rotation matrix transpose, Indicates time speed, express The speed of time; express Location at any given moment express The position at that moment; Indicates from Time's up The time interval between moments; express The rotation matrix at time step; Based on the above pre-integral increment, construct the IMU pre-integral constraints: , in, Represents the IMU pre-integration residual vector. Represents rotational residuals. Represents the velocity residual. Indicates the positional residual. Indicates zero-biased residual; Define the pre-integral residual: in, Indicates from Time's up Rotational residuals at time intervals Show from Time's up The velocity residual at any given moment Show from Time's up Position residual at any given moment; S3.2 Constructing Radar Odometry Factors: Based on the distortion-free point cloud, extract edge points and planar points from the point cloud features to construct a local map corresponding to the current lidar frame; For point cloud matching of edge points, the five nearest neighbor edge points are found in the local map, and then the mean and covariance matrix of this point set are calculated. If the five nearest points satisfy the line feature condition, the distance from the current edge point to the line is calculated, and the formula for calculating the point-line residual is as follows: in, For dot product operation, This is a cross product operation; The residual between edge points and points / lines in the local map; It is a unit vector; The radar odometer pose is to be determined. This is the current edge point; Indicates distance in a local map The center of the set of the five nearest edge points; This represents the eigenvector corresponding to the largest eigenvalue; It is the initial state estimate for point cloud matching; For point cloud matching of planar points, retrieve the distance from the current planar point in the local map. The five nearest neighbors are analyzed, and their spatial distribution is examined to determine if they form a plane. If a plane is formed, the eigenvector corresponding to the smallest eigenvalue is taken as the normal vector of the plane. The formula for calculating the point-to-surface residual is: in, This represents the point-to-surface residual from a point on the plane to the local map. Indicates the geometric center of the extracted points on the local map; Minimize point-line residuals using the LOAM method and point-to-surface residuals The optimization process updates the correspondence based on the current pose estimate in each iteration and repeats the solution until convergence. The final solution yields the state estimate obtained through point cloud matching in the global coordinate system. Then the first The formula for the residual of the secondary radar odometer is: in, This represents the pose to be solved at the current moment; S3.3 Pose fusion to obtain the optimal pose estimate: An incremental optimization framework based on factor graphs is adopted to discretize the continuous motion trajectory of the autonomous vehicle into a series of state nodes x0, x1, ... x2 to be solved. n Between adjacent state nodes, the system constructs two types of constraint factors with different physical characteristics in parallel: one is the IMU pre-integration factor, which locks the relative motion trend of the carrier in a short time by integrating the raw IMU measurement data generated by high-frequency sampling; the other is the radar odometry factor, i.e., the LO factor, which provides absolute pose correction constraints by geometrically matching the current radar keyframe features with the local map using Scan-to-Map Matching. By fusing measurement data from the IMU and radar, a robust radar inertial odometry (LIO) is constructed. The optimization equation for the radar inertial odometry is as follows: in, This represents the optimal state estimate. Represents the set of all state nodes. Indicates from Time's up The IMU pre-integrated residual vector at time step; After establishing the above factor graph model, the system calls the incremental smoothing algorithm to perform nonlinear least squares optimization on all state nodes in the graph. This process achieves the optimal estimation of the carrier's position, velocity, and attitude by minimizing the weighted sum of the radar residual and the IMU integral residual. S3.4 Constructing a global point cloud map: After each state update of the factor graph, the feature point cloud of the current keyframe, including edge points and planar points, is transformed to the global world coordinate system using the optimized latest pose after motion compensation. As the autonomous vehicle trajectory extends, the feature set of historical keyframes is voxelized downsampled and spatially indexed for optimization, ultimately producing a map containing road topology information and a high-precision global point cloud.

[0013] Preferably, in step S4.1, the specific process of dynamic loading is as follows: After obtaining a rough vehicle pose through map association, this estimated pose is used as input to the dynamic loading module during each localization update. The dynamic loading module retrieves the poses of nearby keyframe nodes from the prior topology map in the form of a sliding window, extracts point cloud features of the key pose point from the map, and uses these point cloud features to construct a local sub-map for feature matching for subsequent feature matching. The local sub-map data loaded by the dynamic loading module is continuously refreshed as the autonomous vehicle moves. When the vehicle leaves the current window range, the point cloud features of the corresponding old key pose points are unloaded from memory, and the point cloud features of the new key pose points are reloaded to refresh the local sub-map used for feature matching.

[0014] Preferably, the specific process of step S4.2 is as follows: S4.2.1 First, use the same registration method as step S3.2 to register the current frame point cloud with the dynamically loaded local sub-map; S4.2.2. Estimate the position of the autonomous vehicle in the global map using LIO odometry; query the k key pose points closest to this position in the existing map topology information; S4.2.3 Construct a local sub-map based on the point cloud features of these key frame nodes obtained from prior topological retrieval; S4.2.4. Call the point cloud registration method in S4.21 to match the current frame point cloud features with the local sub-map, including minimizing the point-line + point-area residuals, aligning the current frame features with the local sub-map, and obtaining the first point in the prior topological information. pose of each keyframe constraint; S4.2.5. Using both real-time observation data from sensors and the states matched online based on prior topological information as constraints, construct the graph optimization constraints as follows: in, This indicates that LIO estimates the pose. This represents the matching constraints between the current frame's point cloud and topology information; S4.2.6 Output the matching constraints between the current frame point cloud and topology information to the backend graph optimizer.

[0015] Preferably, step S4.3, map update evaluation and map extension fusion, includes: S4.31 Calculate the geometric consistency index: After registration in step S4.2.4, two sets of residuals are obtained: point-to-line residuals and point-to-area residuals. Using the k-nearest neighbor rule of the kd-tree in both the new and existing maps, the point in the existing map that is closest to each point in the new map is searched. Within the overlapping area, for points in the new map... Using a kd-tree to search for its nearest neighbor in the existing map The average nearest neighbor distance is defined as: in, This represents the average nearest neighbor distance between the newly created map and the existing map, where m is the number of coordinate points of the trajectory point; like If the value is less than the threshold, proceed to the next step of judgment; S4.32 Calculate the consistency index of matching residuals: Construct a normalized evaluation quantity for matching residuals. : in, and Define the point-line / point-area residuals obtained from radar point cloud matching calculation in step S3.2; when When the residual is less than the preset residual threshold, the matching of the current real-time point cloud data with the point cloud features of the prior map is considered stable and repeatable. S4.33, Calculate the pose consistency index: To prevent the autonomous vehicle's pose from shifting when a mismatch occurs, a pose correction range is defined. : when When the position is less than the preset pose threshold, the reliability of the matching optimization is considered high; if If the value is too large, it indicates that the autonomous vehicle pose obtained from the feature matching calculation is unreliable. In this case, the system prohibits fusion to avoid ghosting in the global map.

[0016] Preferably, if Less than the corresponding threshold, and Less than the corresponding threshold, and If the value is less than the corresponding threshold, the fusion condition is met, and the fusion flag Fuse=1 is set to determine whether the new map can be merged with the original map; otherwise, the fusion flag Fuse=0 and fusion is not performed.

[0017] This invention provides a scalable laser mapping method based on prior topological information, which has the following advantages: (1) By tightly coupling multi-sensor data, embedding prior topological constraints, and implementing dynamic resource management, this invention realizes an incrementally expandable, globally consistent, and resource-efficient laser mapping system, providing key mapping technology support for long-term autonomous navigation in dynamic environments when the operating area expands.

[0018] (2) The present invention sets up a dynamic loading module and a sliding window management mechanism. The dynamic loading module only maintains the active sub-maps in the sliding window around the current pose, dynamically loads the data of neighboring topological nodes, and unloads them in time after the vehicle leaves, thereby reducing memory usage and computational load.

[0019] (3) In order to ensure the quality and consistency of the expanded map, the map update judgment module of this invention performs a quantitative evaluation of the matching degree between the newly created sub-map and the existing map, thereby avoiding mismatch or low-quality data from polluting the global map and ensuring the robustness and reliability of the map expansion process.

[0020] (4) The method of the present invention supports the continuous construction of new areas on the basis of existing maps, effectively suppresses cumulative drift, and improves the construction of large-scale scenes. Figure 1 Consistency and long-term availability provide a reliable map building solution for long-term autonomous navigation in dynamically changing environments. Attached Figure Description

[0021] Figure 1 This is a schematic diagram of the overall framework of the mapping method of the present invention.

[0022] Figure 2 This is a schematic diagram of point cloud matching of edge points in this invention.

[0023] Figure 3 This is a schematic diagram of point cloud matching of planar points according to the present invention.

[0024] Figure 4 This is a schematic diagram of the factor graph-based incremental optimization framework of the present invention.

[0025] Figure 5 This is a schematic diagram of the point cloud matching method based on prior topological information of the present invention.

[0026] Figure 6 This diagram illustrates the process of merging and expanding the newly created map and the existing map in this invention.

[0027] Figure 7 This is a schematic diagram of the experimental platform in Embodiment 1 of the present invention.

[0028] Figure 8 This is a test map of scenario one in Embodiment 1 of the present invention.

[0029] Figure 9 This is the test map for scenario two in Embodiment 1 of the present invention.

[0030] Figure 10 This is a rendering of the scene in Embodiment 1 of the present invention.

[0031] Figure 11 This is a rendering of the second scenario in Embodiment 1 of the present invention.

[0032] Figure 12 This is a schematic diagram of the mapping trajectory extraction for scenario one in Embodiment 1 of the present invention.

[0033] Figure 13 This is a schematic diagram of the mapping accuracy in scenario one of Embodiment 1 of the present invention.

[0034] Figure 14 This is a schematic diagram of the mapping accuracy in scenario two of Embodiment 1 of the present invention.

[0035] Figure 15This is a flowchart of the LIO-SAM process.

[0036] Figure 16 This is a flowchart of the FAST-LIO process. Detailed Implementation

[0037] The present invention will be further described in detail below with reference to specific embodiments and accompanying drawings, but the scope of protection of the present invention is not limited to the content described.

[0038] A schematic diagram of the overall framework of the mapping method of the present invention is shown below. Figure 1 As shown.

[0039] This invention provides a scalable laser mapping method based on prior topological information, comprising the following steps: S1, Multi-sensor data acquisition A data acquisition platform based on autonomous vehicles is established. The core perception module of the data acquisition platform adopts a tightly coupled LiDAR-IMU architecture, specifically including: S1.1. Acquiring 3D point cloud data: A rotating LiDAR is used as the main observation sensor. A 16-line mechanical rotating LiDAR is installed on the top of the unmanned vehicle. The rotating LiDAR drives the ranging module to perform a 360° omnidirectional high-speed rotation scan around the Z-axis through an internal motor (the scanning frequency is set to 10Hz) to form a continuous 3D point cloud coverage of the surrounding environment.

[0040] S1.2. Acquisition of triaxial angular velocity and force information: A nine-axis MEMS inertial sensor is selected as a high-frequency inertial measurement sensor (IMU). This sensor outputs the triaxial angular velocity and triaxial force information of the carrier at a frequency of not less than 400Hz.

[0041] S1.3. Rigid Constraints: To ensure the uniformity of multiple sensors in the autonomous vehicle space, different sensors are rigidly mounted on the autonomous vehicle using metal brackets. Precise extrinsic parameter calibration is performed between different sensors to establish a rigid transformation matrix from the IMU coordinate system to the lidar coordinate system, and this matrix is ​​considered constant during the data acquisition process.

[0042] S2, Data Preprocessing S2.1. Initialization of Raw IMU Data: To address the noise and time-dependent bias issues present in the raw IMU data, the initialization phase utilizes observations within a static window to estimate and subtract biases from the gyroscope and accelerometer data, thereby improving measurement accuracy. Simultaneously, by analyzing the initial acceleration vector distribution, the gravitational acceleration component is separated from the raw measurements, thus establishing an initial horizontal attitude reference in the global coordinate system.

[0043] S2.2. Motion Distortion Compensation for LiDAR Point Clouds: Given the significant point cloud motion distortion generated by rotating radar during vehicle movement, a time synchronization mechanism is established to acquire IMU measurement data aligned with the point cloud scanning cycle time. A pre-calibrated rigid transformation matrix is ​​used to map the original laser point cloud to the IMU coordinate system, achieving spatial dimensional alignment between sensors. Considering the non-instantaneous imaging characteristic of rotating radar, the system continuously retrieves the high-frequency IMU pre-integration trajectory within the point cloud scanning cycle of that frame. Linear interpolation is used to calculate the precise pose increment of each laser point at the moment of emission. Based on this, all discrete points are compensated to the reference pose at the start of the frame, eliminating point cloud trailing and geometric stretching caused by vehicle motion, and restoring the distorted original scan data to a distortion-free point cloud frame with true physical proportions.

[0044] S3, Radar Odometer S3.1, IMU pre-integration In addition to motion constraints provided by point cloud matching based on geometric features, the system also incorporates IMU data for fusion to obtain higher-frequency motion observations that are unaffected by environmental textures. IMUs can directly sense the angular velocity and linear acceleration of the carrier using inertial measurement principles. Through pre-integration processing, they can provide continuous and relatively accurate pose change estimates in a short time, effectively compensating for the shortcomings of lidar in fast-moving or feature-deficient scenarios.

[0045] The angular velocity and acceleration measurement models for the IMU are defined as follows: in and These are the raw angular velocity and acceleration measurements of the IMU at time t. It refers to the angular velocity of the actual motion. This refers to the gyroscope (angular velocity) having zero bias. This refers to the white noise of the gyroscope. The accuracy of the measurement is affected by zero bias. and white noise The impact. It refers to the acceleration of actual motion. It is the rotation matrix of the autonomous vehicle in the global coordinate system. It is gravitational acceleration. This refers to the zero bias of the accelerometer (accelerometer). This refers to the white noise of the accelerometer.

[0046] Based on IMU measurement data, the motion state of the unmanned vehicle is estimated using a trajectory extrapolation algorithm. At any moment, the speed of the driverless car ,Location and rotation attitude (matrix) The following kinematic equations can be used for calculation: in, Represents the velocity at time t. Indicates the time increment; This indicates the position at time t.

[0047] To further efficiently integrate IMU measurement information and avoid repetitive integration calculations during the optimization process, this invention employs a pre-integration technique. Based on IMU measurement data between adjacent keyframes, it directly calculates the relative increment of the motion state from time i to time j. Specifically, it calculates the changes in rotation, velocity, and position. , , Define as follows in, Indicates time The rotation matrix, express The rotation matrix at time step; Indicates time speed, express The speed of time; Indicates time Location, express The position at that moment; Indicates from Time's up The time interval between moments; It is a rotation matrix The transpose of .

[0048] Based on the above pre-integral increments, IMU pre-integral constraints can be constructed: , in, Represents the IMU pre-integration residual vector. Represents rotational residuals. Represents the velocity residual. Indicates the positional residual. This indicates zero-biased residuals.

[0049] Define the pre-integral residual: in, Indicates from Time's up Rotational residuals at time intervals Show from Time's up The velocity residual at any given moment Show from Time's up Position residual at any given moment.

[0050] S3.2, Radar Point Cloud Matching The core of radar inertial odometry lies in comparing radar point cloud information scanned by the autonomous vehicle at different locations to accurately determine the vehicle's pose changes. Point cloud matching in radar inertial odometry provides motion constraints for the autonomous vehicle based on the environmental geometry. LiDAR point cloud registration is performed in a global coordinate system. After distortion correction of the radar point cloud data, a feature extraction strategy using the LOAM framework is employed to extract point cloud features. Edge points and planar points from the extracted point cloud features are used to construct a local map corresponding to the current LiDAR frame. During the motion state determination phase, the point cloud matching module aligns the current frame with the local map.

[0051] For point cloud matching of edge points, the five nearest neighbors of each edge point are found in the local map, and then the mean and covariance matrix of this point set are calculated. For example... Figure 2 As shown, if the five nearest points satisfy the line characteristic condition, then calculate the distance from the current edge point to the line. The formula for calculating the point-line residual is as follows: in, For dot product operation, This is a cross product operation; The residual between edge points and points / lines in the local map; It is a unit vector; The radar odometer pose is to be determined. This is the current edge point; Indicates distance in a local map The center of the set of the five nearest edge points; This represents the eigenvector corresponding to the largest eigenvalue; It is the initial state estimate for point cloud matching.

[0052] Point cloud matching of planar points follows a similar principle; the algorithm retrieves points in the local map that are far from the current planar point. The five nearest neighbors are analyzed, and their spatial distribution is used to determine whether a plane is formed. If a plane is formed, then... Figure 3 As shown, the eigenvector corresponding to the smallest eigenvalue is taken as the normal vector of the plane. Based on this, the formula for calculating the point-area residual is as follows: in, This represents the point-to-surface residual from a point on the plane to the local map. This indicates the geometric center of the extracted points on the local map.

[0053] Minimize point-line residuals using the LOAM method and point-to-surface residuals The optimization process updates the correspondence based on the current pose estimate in each iteration and repeats the solution until convergence. The final solution yields the state estimate obtained through point cloud matching in the global coordinate system. The residual of the radar odometer for the i-th iteration is given by the following formula: in, Indicates the first Secondary radar odometer residual This represents the pose to be solved.

[0054] S3.3, Pose Fusion An incremental optimization framework based on factor graphs is adopted, and the principle diagram is as follows: Figure 4 As shown, the continuous motion trajectory of the unmanned vehicle is discretized into a series of state nodes (x0, x1, ... x2) to be solved. n Between adjacent state nodes, the system constructs two types of constraint factors with different physical characteristics in parallel: one is the IMU pre-integration factor (red circular node in the figure), which locks the relative motion trend of the carrier in a short time by integrating the raw IMU measurement data generated by high-frequency sampling; the other is the radar odometry factor (green square node in the figure, i.e., LO factor), which provides absolute pose correction constraints by geometrically matching the current radar keyframe features with the local map (Scan-to-Map Matching). This incremental optimization framework transforms sensor observations of different frequencies and characteristics into unified mathematical constraints, laying the foundation for tight coupling of multi-source data.

[0055] The factor map not only utilizes the short-term, high-frequency characteristics of the IMU to provide accurate initial guesses for radar matching, but also effectively suppresses the cumulative drift of the IMU by leveraging the geometric observations of the radar. Through the combined effect of these multi-dimensional factors, the system can achieve spatiotemporally aligned pose output in complex motion environments, ensuring the closed-loop consistency and global accuracy of the mapped trajectory in long-distance, large-scale scenarios. By fusing the measurement data from the IMU and radar, a robust radar inertial odometry (LIO) is constructed. The optimization equation for the radar inertial odometry is as follows: in, This represents the optimal state estimate. Represents the set of all state nodes. Indicates from Time's up The IMU pre-integration residual vector at time t.

[0056] After establishing the aforementioned factor graph model, the system calls an incremental smoothing algorithm (such as iSAM2) to perform nonlinear least squares optimization on all state nodes in the graph. This process achieves optimal estimation of the carrier's position, velocity, and attitude (Pose) by minimizing the weighted sum of the radar residual and the IMU integral residual.

[0057] S3.4 Map Building After each state update of the factor graph, the system uses the optimized latest pose to transform the motion-compensated feature point cloud (edge ​​points and planar points) of the current keyframe to the global world coordinate system. As the autonomous vehicle's trajectory extends, the system performs voxelization downsampling and spatial indexing optimization on the feature set of historical keyframes, ultimately producing a map containing road topology information and a high-precision global point cloud.

[0058] S4. Map Update Extension Based on Topology Information S4.1 Dynamic Loading Module After obtaining a rough vehicle pose through map association, this estimated pose is used as input to the dynamic loading module during each localization update. The dynamic loading module can retrieve the poses of nearby keyframe nodes from the prior topology map in the form of a sliding window, and extract point cloud features of the key pose points from the map. These point cloud features are used to construct a local sub-map for feature matching and subsequent feature matching.

[0059] The prior map data loaded by the dynamic loading module is continuously refreshed as the autonomous vehicle moves. When the vehicle leaves the current window range, the point cloud features of the corresponding old key pose points are unloaded from memory, and the point cloud features of the new key pose points are reloaded to refresh the sub-map used for feature matching.

[0060] Because performing full access and visualization of the complete global map during runtime is costly and resource-intensive, this method sets a maximum active keyframe, requiring only a portion of the map near the current location, thus ensuring: (1) Memory usage does not increase linearly with the expansion of the global map; (2) Furthermore, the computational cost of feature matching almost only increases with the increase of the sliding window; (3) It matches the extended mapping and saves system memory and computing load during the mapping process of extremely large scenes.

[0061] S4.2 Map Association First, registration is performed using the scan-to-map registration method based on the ICP iterative framework described in step S3.2. Unlike traditional point-to-point ICP, this method, after establishing nearest neighbor correspondence, constructs point-to-line constraints for edge features and point-to-surface constraints for planar features, and solves the radar odometry pose by minimizing the nonlinear least squares optimization of the point-line / point-surface residuals.

[0062] After completing the initial mapping, the system outputs a Global Map and a point cloud feature containing all the key pose points the autonomous vehicle made during the initial map construction, along with the corresponding radar point cloud data for each key pose point. As the autonomous vehicle's operating range expands, the previously constructed map often cannot cover the new operational area; therefore, incremental map expansion based on existing map data is necessary.

[0063] The map association module is used to establish geometric constraints between the point cloud data scanned in real time by the unmanned vehicle and the prior map topology information during the extended mapping process. In the area where the two intersect, the system retrieves key pose points in the neighborhood based on the prior topology and performs geometric matching between the real-time scan and its associated local sub-map, thereby aligning the point cloud data scanned in real time by the unmanned vehicle with the prior global map.

[0064] The map association module is implemented through dynamic matching. The input of this module is a local sub-map of point cloud features associated with key pose points constructed by the dynamic loading module and the LiDAR point cloud features obtained by the current unmanned vehicle in real time scanning. The output is the pose association result between the current position of the unmanned vehicle and the prior local sub-map and the corresponding map association constraint factors, which are used for backend graph optimization.

[0065] Its process is as follows Figure 5As shown, the localization process first uses LIO odometry to estimate the autonomous vehicle's position on the global map. Then, the system queries the existing map topology information to find the k closest key pose points to that position. Based on this, a local sub-map is constructed using the point cloud features of neighboring keyframe nodes obtained from prior topology retrieval. The scan-to-map registration based on the aforementioned ICP iterative framework (including minimizing point-line and point-surface residuals) is then used to align the current frame features with the local sub-map, obtaining pose constraints which are then input into the backend graph optimization.

[0066] During the optimization process, both real-time observation data from sensors and online matching states based on prior topology information are used as constraints. Prior information is managed using a sliding window, and prior map data is managed by a dynamic loading module using a sliding window. Keyframe features outside the window are unloaded promptly to control memory usage. Backend graph optimization edge-processes state nodes outside the window when necessary to maintain the real-time performance of incremental optimization. The final graph optimization constraints are: in, It is a radar inertial odometry constraint. This represents the pose estimated by LIO. This represents the matching constraints between the current frame's point cloud and topology information. The first in the prior topological information The pose of each keyframe.

[0067] S4.3 Map Update Evaluation and Map Expansion and Integration After dynamically loading the region of interest from the prior topology information, the map update judgment module matches the current frame point cloud with the corresponding region, thereby establishing the constraint relationship between the prior information and the real-time state.

[0068] During map expansion, if the newly created map is directly merged with the existing map without sufficient alignment, mismatches will occur, affecting the overall mapping quality. Therefore, to ensure a smooth transition of the new map to the existing map, this method uses a map update evaluation module to calculate the degree of matching between the new and existing maps, determining whether to merge. A high degree of matching indicates good geometric consistency, allowing for a smooth map expansion.

[0069] First, after the scan-to-map registration in step S4.2, two types of residual sets are obtained: point-to-line residuals and point-to-area residuals. In both the newly created map and the existing map, such as... Figure 6 As shown, using the k-nearest neighbor rule of a kd-tree, we search for the point in the original map that is closest to each point in the new map. Within the overlapping region, we search for points in the new map. Using a kd-tree to search for its nearest neighbor in the existing map The average nearest neighbor distance is defined as: in, This represents the average nearest neighbor distance between the newly created map and the existing map. The number of coordinate points of the trajectory points.

[0070] like If the distance is less than the threshold, it indicates a good stitching effect between the newly created map and the existing map, allowing for further evaluation. Relying solely on the average distance to evaluate map updates is insufficient to distinguish between accidental matching and true matching of factual point cloud data. Therefore, this method constructs a normalized evaluation metric for the matching residuals. : in, and This defines the point-line / point-area residuals obtained from the radar point cloud matching calculation in step S3.2. When... When the residual value is less than a preset residual threshold, the matching between the current real-time point cloud data and the point cloud features of the prior map can be considered stable and repeatable, rather than accidental alignment. To avoid excessive pose shift of the autonomous vehicle when mismatches occur, this method defines a pose correction magnitude. : when When the position is less than the preset pose threshold, the reliability of the matching optimization is considered high; if If the value is too large, it indicates that the autonomous vehicle pose obtained from the feature matching calculation is unreliable. In this case, the system prohibits fusion to prevent ghosting from appearing on the global map.

[0071] like Less than the corresponding threshold, and Less than the corresponding threshold, and If the value is less than the corresponding threshold, set the fusion flag Fuse=1 to determine if the newly created map can be merged with the original map; otherwise, set the fusion flag Fuse=0 and do not perform fusion.

[0072] The map update evaluation module of this method integrates three indicators: geometric consistency, matching residual consistency, and pose consistency. Only when all three indicators simultaneously meet their respective set conditions will the map update evaluation module output an instruction to merge and expand the new map with the original global map, thus avoiding the writing of erroneous data into the global map. Subsequently, if Fuse=1, the real-time autonomous vehicle pose, corner points, and planar point features extracted from the point cloud for each frame after matching are added to the global map, realizing a point cloud map expansion function suitable for autonomous driving.

[0073] Map expansion begins at the front-end perception layer: LiDAR provides relative motion estimation by extracting features and matching them with the local map; IMU provides high-frequency pose compensation through pre-integration. The two are deeply integrated to construct a high-precision radar inertial odometry (LIO) that estimates the vehicle's initial position in the prior map.

[0074] The core mechanism of map expansion lies in the introduction and utilization of prior topological information. First, using the current pose estimated by LIO, neighboring keyframe nodes are queried from the prior topological information obtained in the initial mapping. The topological information corresponding to these keyframe nodes is then associated to create a local prior sub-map. Features are extracted from the point cloud data obtained during real-time scanning during map expansion. Map association constraints are established by matching the real-time scanned point cloud features with the points cloud features extracted from this local prior sub-map. These constraints, along with LIO factors, are input into the backend graph optimizer to expand the existing global map, thereby enabling the addition of the new expanded map to the global map at both the geometric and topological levels.

[0075] Example 1 (Verification Experiment) 1. Experimental preparation To verify the effectiveness and accuracy of the method of this invention, a real-vehicle road test was conducted on the Ackermann steering intelligent vehicle platform. The experimental platform is as follows: Figure 7 As shown, this experimental setup is equipped with a 16-line lidar and a real-time differential positioning system (RTK) on the top front of the vehicle, as well as various sensors including an IMU, ultrasonic radar, and a camera positioned above the center point of the rear axle. All sensor data is connected to an industrial control computer and is used for communication and synchronous acquisition based on the Robot Operating System (ROS).

[0076] This study selected a 16-line LiDAR and IMU as the main data sources to acquire precise vehicle position information in dynamic environments. To obtain reliable trajectory ground truth for evaluating algorithm performance, the system integrated a high-precision RTK positioning system provided by Sinan Navigation Company, with horizontal positioning accuracy down to the centimeter level. RTK integrates satellite navigation and ground base station observations, achieving high-precision pose acquisition and effectively improving the reliability and usability of experimental data. Data acquisition was based on the Ubuntu 18.04 system with Linux architecture, the robot operating system used was ROS Melodic version, and real-time data reading was performed using NVIDIA Jetson Orin series processing units. Detailed parameters of each device are shown in Table 1. Table 1 2. Test Scenario To verify the adaptability and scalability of the method of this invention under different terrains and environmental structures, this experiment conducted field data collection in a real road environment. The unmanned vehicle operated along multiple pre-set trajectories, and the data collection area covered various representative mapping scenarios, including densely built-up areas, open roads, and shady forest areas. Figure 8 and 9 As shown, the experiment set up two test scenarios with different scales and spatial structures: Scenario 1 ( Figure 8 Scene 1 is a mixed area approximately 700 m long and 300 m wide, containing three discontinuous acquisition trajectories; Scene 2 ( Figure 9 The area is approximately 300 m × 150 m, containing two acquisition trajectories. During data acquisition, sensors such as LiDAR, IMU, and RTK are strictly time-synchronized to record the vehicle's pose, motion state, and 3D point cloud information of the surrounding environment in real time, forming a multi-dimensional, time-aligned dataset that fuses LiDAR point clouds, inertial measurement, and centimeter-level positioning ground truth. The experiment focuses on designing trajectory intersection and overlap areas to verify the ability of the method of this invention to stitch and fuse multiple trajectory data during map expansion, thereby evaluating its effectiveness in maintaining map continuity, consistency, and seamless expansion in large-scale scenarios.

[0077] 3. Experiment with mapping results Figure 10 and Figure 11 The mapping performance of the scalable laser mapping method based on the prior topological information of this invention is shown. Figure 10 and Figure 11 They correspond to as Figure 8 and Figure 9 The mapping results of the two test scenarios demonstrate that the sub-map stitching achieves map expansion. During the new map construction process, when the location of the new mapping task is still within the previously constructed map, the real-time map and the prior map are matched via a sliding window using a dynamic loading module. The mapping results show that the newly added map is associated with the original map, followed by incremental map expansion. The map texture transitions naturally, without feature conflicts or boundary blurring issues caused by multiple mapping iterations. This result verifies the effectiveness of the sub-map stitching strategy based on topological information. Through GNSS initial positioning, topological vertex matching, and graph optimization constraints, the global consistency of maps constructed from data collected at different times and on different trajectories is ensured.

[0078] The method of this invention can achieve stable and reliable map construction in both scenarios, possessing not only excellent environmental feature capture capabilities but also... Figure 1 Furthermore, through topological information constraints and dynamic loading mechanisms, it can achieve seamless expansion across scenarios, providing effective technical support for the practical application of large-scale laser SLAM.

[0079] To verify the stability and accuracy of positioning during the mapping process, this method employs a verification approach that matches point clouds with the completed map. For example... Figure 12 As shown, this method uses real-time output lidar point clouds to perform dynamic feature matching with a pre-built high-precision prior map, and extracts the trajectory points of the map by matching the trajectory. The white and red are point cloud maps after stitching together multiple map segments, and the colored (lower left) is the real-time matching point cloud for verifying the map trajectory.

[0080] Using centimeter-level high-precision positioning data provided by the RTK system as the real trajectory reference, the degree of agreement between the spatial coordinates of the algorithm trajectory and the real trajectory directly reflects the mapping accuracy. The smaller the deviation between the two, the stronger the positioning stability and the more accurate the mapping result.

[0081] Figure 13 and Figure 14 Scene 1 was shown. Figure 13 ) and Scene 2 ( Figure 14 The mapping accuracy of each algorithm is as follows.

[0082] To further highlight the mapping accuracy advantage of the proposed algorithm, the trajectory verification results of this method are compared with those of LIO-SAM (flowchart shown in Figure 1). Figure 15 (as shown), LOAM, LEGO-LOAM, FAST-LIO (flowchart shown) Figure 16 (As shown) A comparison is made between mainstream laser SLAM mapping algorithms. All algorithms were tested on the same experimental platform and the same dataset. The accuracy of different mapping algorithms was compared and analyzed. This method uses the root mean square error (RMSE) to evaluate the mapping accuracy. The RMSE calculation formula is: Where m is the number of coordinate points of the trajectory points. The final mapping accuracy of each method is shown in Table 2. Table 2 The comparison of the localization of the mapped trajectories shows that, in both scenario one and scenario two, the map trajectory established by this method is closest to the actual trajectory in terms of accuracy. Following closely are LIO-SAM and FAST-LIO, whose mapping accuracy is basically the same, with little difference in performance. The LOAM algorithm, which uses pure LiDAR mapping, has a larger mapping error. The trajectory comparison demonstrates that, thanks to the proposed real-time mapping and topological information sliding window matching, the map can be expanded while reducing mapping errors through multi-segment mapping.

[0083] Compared with LOAM's pure LiDAR mapping, the mapping accuracy of the algorithm in this invention and other multi-sensor fusion methods is more advantageous. The main reason is that the multi-sensor fusion mapping method can, to a certain extent, constrain and correct the matching error and inverse constraints of the radar point cloud. Ultimately, this results in LOAM's mapping errors of 56.237 meters and 5.007 meters in Scene 1 and Scene 2, respectively, which are relatively large.

[0084] Multi-sensor fusion mapping can achieve high mapping accuracy within a small area. However, due to sensor measurement noise and algorithm limitations, error accumulation is inevitable in large-scale mapping. Therefore, in the LIO-SAM and FAST-LIO mapping processes, the initial mapping and elevation are high, with a high degree of overlap with the actual trajectory. However, the mapping accuracy begins to decline in the later stages, mainly due to error accumulation. While the overall positioning accuracy is improved compared to LOAM, the mapping error is relatively high. In scenarios one and two, the mapping errors of LIO-SAM are 9.631 meters and 3.146 meters, respectively, while those of FAST-LIO are 10.816 meters and 2.265 meters, respectively. The LIO-SAM and FAST-LIO workflow frameworks are as follows: Figure 15 and 16 As shown.

[0085] The proposed method uses real-time newly added maps and prior topological information to stitch together multiple map segments. This allows for map expansion and improves mapping efficiency. Furthermore, multi-session mapping effectively eliminates accumulated errors.

[0086] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A scalable laser mapping method based on prior topological information, characterized in that: Includes the following steps: S1. Multi-sensor data acquisition: Build a data acquisition platform based on unmanned vehicles to acquire three-dimensional point cloud data, and obtain three-axis angular velocity and three-axis force information through IMU. Establish a rigid transformation matrix from IMU to LiDAR coordinate system to provide raw input for subsequent data preprocessing and state estimation. S2. Data Preprocessing: Based on the data collected in S1, zero-bias estimation and gravity separation are performed on the raw IMU data, and motion distortion compensation is performed on the LiDAR point cloud to improve data quality and provide denoising and alignment input for subsequent state estimation. S3, Radar Odometry: Based on the data preprocessed in step S2, IMU pre-integration factor and radar odometry factor are constructed, and pose fusion is performed. Based on the fused pose estimation, a global point cloud map is constructed, and a priori topology structure that can be used for subsequent map expansion is generated. S4. Map update extension based on topology information: S4.1 Dynamic Loading Module: Dynamically loads the local sub-maps corresponding to adjacent keyframe nodes, providing local matching objects for map expansion; S4.2 Map Association: Register the current frame point cloud with the dynamically loaded local sub-map to construct map association constraints and provide prior associations for map optimization; S4.3 Map Update Evaluation and Map Expansion and Integration: Evaluate the matching quality between the newly created map and the existing map to determine whether to expand the map; after meeting the evaluation conditions, integrate the newly created map data into the global map to complete the incremental expansion.

2. The scalable laser mapping method based on prior topological information according to claim 1, characterized in that: Step S1 includes the following steps: S1.1 Acquiring 3D point cloud data: A rotating LiDAR is installed on the top of the unmanned vehicle as the main sensor. The ranging module is driven by an internal motor to perform a 360° omnidirectional high-speed rotation scan around the Z-axis to acquire 3D point cloud data, so as to form a continuous 3D point cloud coverage of the surrounding environment. The scanning frequency is 10Hz. S1.2 Acquisition of triaxial angular velocity and force information: A nine-axis MEMS inertial sensor is selected as a high-frequency inertial measurement sensor (IMU) to output the triaxial angular velocity and triaxial force information of the carrier at a frequency of not less than 400Hz. S1.3 Rigid Constraints: The rotating lidar and high-frequency inertial measurement unit (IMU) are rigidly mounted on the unmanned vehicle via a metal bracket. External parameter calibration is performed between different sensors to establish a rigid transformation matrix from the IMU to the LiDAR coordinate system.

3. The scalable laser mapping method based on prior topological information according to claim 1, characterized in that: Step S2 includes the following steps: S2.

1. Initialization of IMU raw data: During the initialization phase, the observations within the static window are used to perform zero bias estimation and subtraction of the gyroscope and accelerometer. At the same time, by analyzing the acceleration vector distribution at the initial moment, the gravitational acceleration component is separated from the raw measurement, thereby establishing the initial horizontal attitude reference in the global coordinate system. S2.

2. Motion distortion compensation for LiDAR point clouds: Acquire IMU measurement data aligned with the point cloud scanning cycle time, and use a pre-calibrated rigid transformation matrix to map the original laser point cloud to the IMU coordinate system to achieve spatial dimension alignment between sensors. Considering the non-instantaneous imaging characteristics of rotating radar, the high-frequency IMU pre-integration trajectory within the point cloud scanning cycle of the frame is retrieved in real time. The precise pose increment of each laser point at the moment of emission is calculated by linear interpolation, and based on this, all discrete points are compensated to the reference pose at the beginning of the frame to eliminate point cloud trailing and geometric stretching caused by carrier motion, and restore the distorted original scanning data to a distortion-free point cloud frame with true physical proportions.

4. The scalable laser mapping method based on prior topological information according to claim 1, characterized in that: The specific process of the radar odometer in step S3 is as follows: S3.1 Constructing the IMU pre-integration factor: The angular velocity and acceleration measurement models for the IMU are defined as follows: in and These are the raw angular velocity and acceleration measurements of the IMU at time t. It refers to the angular velocity of the actual motion. This refers to the gyroscope's zero bias. This refers to the white noise of the gyroscope. The accuracy of the measurement is affected by zero bias. and white noise The impact; It refers to the acceleration of actual motion. It is the rotation matrix of the autonomous vehicle in the global coordinate system. It is gravitational acceleration. This refers to the zero bias of the accelerometer. This refers to the white noise of the accelerometer; Based on IMU measurement data, the motion state of the unmanned vehicle is estimated using a trajectory inference algorithm. At any moment, the speed of the driverless car ,Location and rotation matrix The following kinematic equations are used for calculation: in, Represents the velocity at time t. Indicates the time increment; Indicates the position at time t; Using pre-integration techniques, based on IMU measurement data between adjacent keyframes, the calculation is directly performed from time [time value missing]. At the time The relative increments of the motion state, specifically, the pre-integral increments of rotation, velocity, and position. , and Defined as follows: in, Indicates time The rotation matrix, It is a rotation matrix transpose, Indicates time speed, express The speed of time; express Location at any given moment express The position at that moment; Indicates from Time's up The time interval between moments; express The rotation matrix at time step; Based on the above pre-integral increment, construct the IMU pre-integral constraints: ， in, Represents the IMU pre-integration residual vector. Represents rotational residuals. Represents the velocity residual. Indicates the positional residual. Indicates zero-biased residual; Define the pre-integral residual: in, Indicates from Time's up Rotational residuals at time intervals Show from Time's up The velocity residual at any given moment Show from Time's up Position residual at any given moment; S3.2 Constructing Radar Odometry Factors: Based on the distortion-free point cloud, extract edge points and planar points from the point cloud features to construct a local map corresponding to the current lidar frame; For point cloud matching of edge points, the five nearest neighbor edge points are found in the local map, and then the mean and covariance matrix of this point set are calculated. If the five nearest points satisfy the line feature condition, the distance from the current edge point to the line is calculated, and the formula for calculating the point-line residual is as follows: in, For dot product operation, This is a cross product operation; The residual between edge points and points / lines in the local map; It is a unit vector; The radar odometer pose is to be determined. This is the current edge point; Indicates distance in a local map The center of the set of the five nearest edge points; This represents the eigenvector corresponding to the largest eigenvalue; It is the initial state estimate for point cloud matching; For point cloud matching of planar points, retrieve the distance from the current planar point in the local map. The five nearest neighbors are analyzed, and their spatial distribution is examined to determine if they form a plane. If a plane is formed, the eigenvector corresponding to the smallest eigenvalue is taken as the normal vector of the plane. The formula for calculating the point-to-surface residual is: in, This represents the point-to-surface residual from a point on the plane to the local map. Indicates the geometric center of the extracted points on the local map; Minimize point-line residuals using the LOAM method and point-to-surface residuals The optimization process updates the correspondence based on the current pose estimate in each iteration and repeats the solution until convergence. The final solution yields the state estimate obtained through point cloud matching in the global coordinate system. Then the first The formula for the residual of the secondary radar odometer is: in, This represents the pose to be solved at the current moment; S3.3 Pose fusion to obtain the optimal pose estimate: An incremental optimization framework based on factor graphs is adopted to discretize the continuous motion trajectory of the autonomous vehicle into a series of state nodes x0, x1, ... x2 to be solved. n Between adjacent state nodes, the system constructs two types of constraint factors with different physical characteristics in parallel: one is the IMU pre-integration factor, which locks the relative motion trend of the carrier in a short time by integrating the raw IMU measurement data generated by high-frequency sampling; the other is the radar odometry factor, i.e., the LO factor, which provides absolute pose correction constraints by geometrically matching the current radar keyframe features with the local map using Scan-to-Map Matching. By fusing measurement data from the IMU and radar, a robust radar inertial odometry (LIO) is constructed. The optimization equation for the radar inertial odometry is as follows: in, This represents the optimal state estimate. Represents the set of all state nodes. Indicates from Time's up The IMU pre-integrated residual vector at time step; After establishing the above factor graph model, the system calls the incremental smoothing algorithm to perform nonlinear least squares optimization on all state nodes in the graph. This process achieves the optimal estimation of the carrier's position, velocity, and attitude by minimizing the weighted sum of the radar residual and the IMU integral residual. S3.4 Constructing a global point cloud map: After each state update of the factor graph, the feature point cloud of the current keyframe, including edge points and planar points, is transformed to the global world coordinate system using the optimized latest pose after motion compensation. As the autonomous vehicle trajectory extends, the feature set of historical keyframes is voxelized downsampled and spatially indexed for optimization, ultimately producing a map containing road topology information and a high-precision global point cloud.

5. A scalable laser mapping method based on prior topological information according to claim 1, characterized in that: In step S4.1, the specific process of dynamic loading is as follows: After obtaining a rough vehicle pose through map association, this estimated pose is used as input to the dynamic loading module during each localization update. The dynamic loading module retrieves the poses of nearby keyframe nodes from the prior topology map in the form of a sliding window, extracts point cloud features of the key pose point from the map, and uses these point cloud features to construct a local sub-map for feature matching for subsequent feature matching. The local sub-map data loaded by the dynamic loading module is continuously refreshed as the autonomous vehicle moves. When the vehicle leaves the current window range, the point cloud features of the corresponding old key pose points are unloaded from memory, and the point cloud features of the new key pose points are reloaded to refresh the local sub-map used for feature matching.

6. The scalable laser mapping method based on prior topological information according to claim 4, characterized in that: The specific process of step S4.2 is as follows: S4.2.1 First, use the same registration method as step S3.2 to register the current frame point cloud with the dynamically loaded local sub-map; S4.2.

2. Estimate the position of the autonomous vehicle in the global map using LIO odometry; query the k key pose points closest to this position in the existing map topology information; S4.2.3 Construct a local sub-map based on the point cloud features of these key frame nodes obtained from prior topological retrieval; S4.2.

4. Call the point cloud registration method in S4.21 to match the current frame point cloud features with the local sub-map, including minimizing the point-line + point-area residuals, aligning the current frame features with the local sub-map, and obtaining the first point in the prior topological information. pose of each keyframe constraint; S4.2.

5. Using both real-time observation data from sensors and the states matched online based on prior topological information as constraints, construct the graph optimization constraints as follows: in, This indicates that LIO estimates the pose. This represents the matching constraints between the current frame's point cloud and topology information; S4.2.6 Output the matching constraints between the current frame point cloud and topology information to the backend graph optimizer.

7. A scalable laser mapping method based on prior topological information according to claim 6, characterized in that: Step S4.3, map update evaluation and map extension fusion, includes: S4.31 Calculate the geometric consistency index: After registration in step S4.2.4, two sets of residuals are obtained: point-to-line residuals and point-to-area residuals. Using the k-nearest neighbor rule of the kd-tree in both the new and existing maps, the point in the existing map that is closest to each point in the new map is searched. Within the overlapping area, for points in the new map... Using a kd-tree to search for its nearest neighbor in the existing map The average nearest neighbor distance is defined as: in, This represents the average nearest neighbor distance between the newly created map and the existing map, where m is the number of coordinate points of the trajectory point; like If the value is less than the threshold, proceed to the next step of judgment; S4.32 Calculate the consistency index of matching residuals: Construct a normalized evaluation quantity for matching residuals. : in, and Define the point-line / point-area residuals obtained from radar point cloud matching calculation in step S3.2; when When the residual is less than the preset residual threshold, the matching of the current real-time point cloud data with the point cloud features of the prior map is considered stable and repeatable. S4.33, Calculate the pose consistency index: To prevent the autonomous vehicle's pose from shifting when a mismatch occurs, a pose correction range is defined. : when When the position is less than the preset pose threshold, the reliability of the matching optimization is considered high; if... If the value is too large, it indicates that the autonomous vehicle pose obtained from the feature matching calculation is unreliable. In this case, the system prohibits fusion to avoid ghosting in the global map.

8. A scalable laser mapping method based on prior topological information according to claim 7, characterized in that: like Less than the corresponding threshold, and Less than the corresponding threshold, and If the value is less than the corresponding threshold, the fusion condition is met, and the fusion flag Fuse=1 is set to determine whether the new map can be merged with the original map; otherwise, the fusion flag Fuse=0 and fusion is not performed.

Images