Map construction method, device, medium and system based on multiple sensors

Abstract

The application discloses a map construction method and device based on multi-sensor fusion, a medium and a system. The method comprises the following steps: obtaining current frame multi-source sensing data through a multi-mode sensing device of a handheld SLAM surveying and mapping instrument and pre-processing the current frame multi-source sensing data; the multi-source sensing data comprises laser radar point cloud data, RGB image data, IMU data and RTK positioning data; a three-mode tightly coupled joint optimization objective function is optimized and solved according to the current frame multi-source sensing data to obtain current frame pose parameters; the mileage data of a mileage meter, the current frame multi-source sensing data and the current frame pose parameters are fused based on a Kalman filtering algorithm to obtain a current frame pose, and the current frame pose and the laser radar point cloud data are superimposed to obtain a three-dimensional map. The application can greatly improve the precision of map construction.

Description

Technical Field

[0001] This invention relates to SLAM mapping scanners, and more particularly to map building methods, apparatus, media, and systems based on multi-sensor fusion. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a core technology in surveying engineering, autonomous robot navigation, and digital twins, used to simultaneously estimate the pose of a vehicle and construct a 3D map of the environment in unknown environments. With technological advancements, handheld SLAM scanners have gained widespread attention in fields such as architectural surveying, topographical exploration, and cultural heritage digitization. However, current handheld SLAM scanners generally employ single-modal sensors for map construction. Single-modal sensors suffer from several drawbacks: purely visual SLAM scanners are prone to degradation and failure in environments with weak textures and strong lighting variations; purely LiDAR SLAM scanners experience significant accuracy drops in geometrically degraded scenarios such as long corridors; and SLAM scanners using only IMU sensors inevitably suffer from cumulative drift. These limitations restrict their application in complex environments, hindering the development of maps and thus limiting the application scenarios of handheld SLAM scanners. Summary of the Invention

[0003] To overcome the shortcomings of existing technologies, one of the objectives of this invention is to provide a map building method based on multi-sensor fusion, so as to solve the problem of scene limitations in map building of existing handheld SLAM mapping scanners with single-modal sensors.

[0004] The second objective of this invention is to provide a map building device based on multi-sensor fusion to solve the problem of scene limitations in existing handheld SLAM mapping scanners with single-modal sensors during map building.

[0005] A third objective of this invention is to provide a computer-readable storage medium to address the problem of scene limitations in existing handheld SLAM mapping scanners with single-modal sensors during map building.

[0006] The fourth objective of this invention is to provide a handheld SLAM mapping and scanning control system to solve the problem of scene limitations in existing single-modal sensor handheld SLAM mapping scanners during map building.

[0007] One of the objectives of this invention is achieved through the following technical solution: A map building method based on multi-sensor fusion, the map building method comprising: Data acquisition steps: Acquire multi-source sensor data of the current frame using a handheld SLAM mapping scanner multi-mode sensing device and preprocess the current frame multi-source sensor data; the multi-source sensor data includes lidar point cloud data, RGB image data, IMU data and RTK positioning data; Pose optimization steps: Based on the multi-source sensor data of the current frame, optimize and solve the pre-constructed three-modal tightly coupled joint optimization objective function in the system to obtain the pose parameters of the current frame; Pose fusion steps: Based on the Kalman filter algorithm, the odometer mileage data, the current frame multi-source sensor data and the current frame pose parameters are fused to obtain the current frame pose; Map building steps: Overlay the current frame pose and LiDAR point cloud data to obtain a 3D map.

[0008] Furthermore, the pose fusion step includes: a local fusion step: based on the Kalman filter algorithm, the odometer data, IMU data and the pose parameters of the current frame are locally fused in a preset local region to obtain a locally fused pose; the local region refers to a local time window centered on the current keyframe; Global fusion step: Based on the Kalman filter algorithm, the local fused pose and RTK positioning data are fused to obtain the global fused pose, and then the current frame pose is obtained based on the global fused pose; the current frame pose includes the attitude, position, velocity, accelerometer bias and gyroscope bias of the current frame.

[0009] Furthermore, after obtaining the global fusion pose in the global fusion step, the following steps are also included: a keyframe determination step: determining whether the current frame is a keyframe; if so, recording the current frame as the current keyframe and storing it in the historical keyframe database, and then performing a loop closure detection step; if not, then performing a map construction step. The loop closure detection step involves performing loop closure detection on historical keyframes in the system's historical keyframe database based on the current keyframe, and optimizing the global fusion pose based on the loop closure detection results.

[0010] Furthermore, the loop closure detection step specifically includes: Step 1: Extract image feature points from the RGB image data of the current keyframe, and quantize the image feature points into bag-of-words vectors using a pre-trained DBoW2 dictionary; Step 2: Based on the bag-of-words vector of the current keyframe and the bag-of-words vector of each keyframe in the historical keyframe database, candidate loop-closing frames are selected from the historical keyframe database. Step 3: Verify each candidate loopback frame based on geometric consistency verification to derive the loopback frame from the candidate loopback frames; Step 4: Based on the global fusion pose of the current keyframe and the global fusion pose of the loopback frame, perform ICP registration on the LiDAR point cloud data of the current keyframe and the LiDAR point cloud data of the loopback frame. Then, based on the ICP registration result, obtain the relative pose of the current keyframe relative to the loopback frame, and then obtain the pose constraint of the loopback frame based on the relative pose. Step 5: Add the loop closure frame pose constraints to the constructed global factor graph, and optimize the global factor graph using GTSAM to output the globally optimal historical keyframe pose sequence. Then, optimize the global fused pose based on the globally optimal historical keyframe pose sequence.

[0011] Furthermore, the pose optimization step specifically includes: The residual calculation steps are as follows: the point-to-surface distance residual is obtained from the LiDAR point cloud data of the current frame, the visual photometric residual is obtained from the RGB image data of the current frame, and the RTK global pose residual is obtained from the RTK positioning data of the current frame. Joint optimization steps: Determine the pose parameters and construct an error state vector based on the pose parameters. Combine the point-to-surface distance residual, visual photometric residual, and RTK global pose residual to construct a three-modal tightly coupled joint optimization objective function. Then, optimize the three-modal tightly coupled joint optimization objective function based on the iterative error state Kalman filter to obtain the pose parameters of the current frame.

[0012] Furthermore, the formula for the three-modal tightly coupled joint optimization objective function is as follows: ; in, Let be the error state vector, and ; , , , , , respectively, are the rotation matrix, translation vector, carrier velocity vector, acceleration bias vector, and gyroscope bias vector; , , , , These are rotational error, positional error, velocity error, accelerometer bias error, and gyroscope bias error, respectively. For Huber kernel function; , , These are the Huber function thresholds for point-to-surface distance residuals, visual photometric residuals, and RTK global position residuals, respectively. For the first The point-to-surface distance residual of feature points in a LiDAR point cloud data set. For the first The visual luminance residual of each pixel involved in the calculation This represents the global position residual for RTK. The point-to-plane distance residual is obtained by searching for the nearest neighbor plane for each feature point in the lidar point cloud data of the current frame. The formula for the point-to-plane distance residual is: ; In the formula, The first [frame] of the current frame of lidar point cloud data The point-to-surface distance residual of each feature point.

[0013] For the first The unit normal vector of the nearest neighbor plane of each feature point.

[0014] For the first The coordinates of any point on the nearest neighbor plane of each feature point.

[0015] For the first Coordinates of each feature point in the lidar coordinate system It is the unit normal vector of the nearest neighbor plane; This refers to the pose of the lidar in the world coordinate system. The visual luminance residual is obtained by comparing the luminance difference at corresponding pixel positions between the RGB image data of the current frame and the RGB image data of the reference frame. The formula for the visual luminance residual is: ; In the formula, , These are the grayscale matrices of the RGB image data of the current frame and the grayscale matrices of the RGB image data of the reference frame, respectively. , , and , respectively, are the corresponding pixels of the RGB image data of the current frame and the corresponding pixels of the RGB image data of the reference frame; , and are the pixel points of the RGB image data of the current frame, respectively. The interpolated grayscale value at the location, and the pixel points of the RGB image data of the reference frame. Interpolated grayscale value at the location; The RTK global position residual measures the deviation between the position component of the current frame and the RTK measured position. The formula for the RTK global position residual is: ; The position component in the current pose; This refers to the actual RTK position output by the RTK device.

[0016] Furthermore, the preprocessing of the current frame multi-source sensing data specifically includes: laser point cloud motion distortion correction preprocessing, IMU pre-integration processing, and RTK coordinate transformation preprocessing; Before the data acquisition step, the method further includes: calibrating the parameters of the LiDAR, RGB camera, IMU sensor and RTK device of the handheld SLAM mapping scanner; the parameter calibration includes RGB camera intrinsic parameter calibration, RGB camera-IMU sensor extrinsic parameter calibration, LiDAR-IMU sensor extrinsic parameter calibration, LiDAR-RGB camera extrinsic parameter calibration and RTK antenna offset measurement.

[0017] The second objective of this invention is achieved by the following technical solution: A map building apparatus based on multi-sensor fusion includes a memory and a processor. The memory stores a map building program that runs on the processor. The map building program is a computer program. When the processor executes the map building program, it implements the steps of a map building method based on multi-sensor fusion as one of the objectives of this invention.

[0018] The third objective of this invention is achieved by the following technical solution: A computer-readable storage medium having a map-building program stored thereon, the map-building program being a computer program, which, when executed by a processor, implements the steps of a multi-sensor fusion-based map-building method as one of the objectives of this invention.

[0019] The fourth objective of this invention is achieved by the following technical solution: A handheld SLAM mapping and scanning control system includes a sensor module, a clock synchronization controller, and an edge computing platform. The sensor module includes a LiDAR, an RGB camera, an IMU sensor, and an RTK device. The clock synchronization controller is electrically connected to the LiDAR, RGB camera, IMU sensor, and RTK device, and is used to receive PP signals from the RTK device and generate a clock synchronization signal, which is then synchronously sent to the LiDAR, RGB camera, and IMU sensor. The edge computing platform is electrically connected to the LiDAR, RGB camera, IMU sensor, and RTK device in the sensor module, and is used to execute the steps of a multi-sensor fusion-based map construction method as used in one of the objectives of this invention.

[0020] Compared with the prior art, the beneficial effects of the present invention are as follows: This invention combines LiDAR, IMU sensor, RGB camera and RTK device to achieve three-modal tight coupling optimization, and combines RTK constraints to optimize pose parameters. At the same time, it combines Kalman filtering to optimize global pose, which greatly improves the accuracy of map construction. Attached Figure Description

[0021] Figure 1 A flowchart of the map construction method based on multi-sensor fusion provided by the present invention. Detailed Implementation

[0022] The present invention will now be further described in conjunction with the accompanying drawings and specific embodiments. It should be noted that, without conflict, the various embodiments or technical features described below can be arbitrarily combined to form new embodiments.

[0023] Example 1

[0024] This invention provides a map construction method based on multi-sensor fusion, which combines a handheld SLAM mapping scanner to achieve high-precision 3D reconstruction. It improves the accuracy of 3D reconstruction by integrating LiDAR, RGB cameras, IMU sensors, and RTK devices to achieve multi-sensor fusion. Specifically, this invention provides a preferred embodiment of a map construction method based on multi-sensor fusion, such as... Figure 1 As shown, it includes: Step S1: Acquire multi-source sensor data of the current frame using the multi-mode sensing device of a handheld SLAM mapping scanner and preprocess the multi-source sensor data of the current frame.

[0025] Specifically, the present invention is applied to a handheld SLAM mapping scanner, which is equipped with a LiDAR, an RGB camera, an IMU sensor, and an RTK device. When the handheld SLAM mapping scanner scans, it simultaneously acquires multi-source sensor data of the current frame based on the above-mentioned devices. The multi-source sensor data includes LiDAR point cloud data from the LiDAR, RGB image data from the RGB camera, IMU data from the IMU sensor, and RTK positioning data from the RTK device.

[0026] Preferably, the lidar in this embodiment is a 16-line mechanically rotating lidar. This lidar features a zero-blind-spot design, a ranging range of 0.05m to 80m, a ranging accuracy of ±1cm, an accuracy better than ±0.5cm, 360° omnidirectional horizontal field of view, ±15° vertical field of view, and a horizontal resolution of 0.2°@600rpm. Furthermore, this lidar supports GPS PPS+NMEA time synchronization, receiving a clock synchronization signal via an M8 aviation connector to achieve clock synchronization of multi-source sensor data. More specifically, the lidar point cloud data includes six parameters: x, y, z, intensity, ring, and timestamp; where x, y, and z are the three-dimensional coordinates of a point in the point cloud data, intensity is the reflection intensity of the point, ring is the scan line number of the point, and timestamp is the scan timestamp of the point.

[0027] The RGB camera uses a CMOS image sensor with a maximum resolution of 3280×2464 pixels, a horizontal field of view of 62.2°, an aperture of F / 2.0, and supports external trigger mode. It can also obtain a clock synchronization signal through the FSTROBE signal line to achieve clock synchronization.

[0028] The IMU sensor can be equipped with a 9-axis AHRS module, which integrates a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer, and can also provide a magnetometer-assisted initial heading angle.

[0029] The RTK device supports five systems across all frequency bands: BDS / GPS / GLONASS / Galileo / QZSS. With a dual-antenna baseline of 1m, the heading accuracy is <0.2°, and with a baseline of 30cm, it is <0.4°. The RTK device integrates dual-antenna orientation functionality; a dual-antenna baseline >10cm is sufficient to calculate the heading angle, directly replacing a magnetometer for heading initialization of handheld SLAM mapping scanners.

[0030] Preferably, the present invention further preprocesses the acquired multi-source sensing data of the current frame. Specifically, the preprocessing includes laser point cloud motion distortion correction preprocessing, IMU pre-integration processing, and RTK coordinate transformation preprocessing.

[0031] Because the carrier moves continuously during the mechanical rotation scanning process of the lidar, the points collected at different times in a frame of point cloud will be distorted. Therefore, the continuous pose between adjacent frames can be obtained through IMU data to perform motion compensation for each point in the lidar point cloud.

[0032] IMU pre-integration processing refers to pre-integrating IMU data within the time interval of adjacent keyframes to obtain the pose increment, velocity increment, and rotation increment between adjacent keyframes. A keyframe is a frame selected by the system at fixed time intervals or when the displacement exceeds a set threshold and stored in the system to form a keyframe database. Each keyframe includes a keyframe ID, timestamp, and multi-source sensor data.

[0033] RTK coordinate transformation preprocessing refers to converting RTK positioning data to a local Cartesian coordinate system. RTK positioning data output by RTK devices is in WGS84 latitude and longitude coordinates; therefore, it needs to be converted to a local Cartesian coordinate system for subsequent calculations.

[0034] In addition, to ensure the synchronization of multi-source sensor data, the present invention also implements hardware-level clock synchronization through a microcontroller chip set in the system. Specifically, the microcontroller chip receives the PPD signal output by the RTK device and generates three clock synchronization signals, which are respectively input to the lidar, IMU sensor and RGB camera, thereby realizing the clock synchronization of multi-source sensors.

[0035] In addition, when the handheld SLAM mapping scanner is started, this invention also calibrates the multi-source sensor devices to achieve data consistency among the multi-source data sensors. The calibration of the multi-source sensor devices includes RGB camera intrinsic parameter calibration, RGB camera-IMU sensor extrinsic parameter calibration, LiDAR-IMU sensor extrinsic parameter calibration, LiDAR-RGB camera extrinsic parameter calibration, and antenna offset measurement.

[0036] Step S2: Based on the multi-source sensor data of the current frame, optimize and solve the pre-constructed three-modal tightly coupled joint optimization objective function in the system to obtain the pose parameters of the current frame.

[0037] Pose parameters include attitude, position, velocity, accelerometer bias, and gyroscope bias. These pose parameters can be represented by an error state vector, specifically: (1).

[0038] This is the error state vector.

[0039] For rotation matrix, It is a translation vector. For velocity vectors, For acceleration bias vector, Gyroscope bias vector.

[0040] For rotational error, For positional error, For speed error, For accelerometer bias error, This represents the gyroscope bias error. and It is possible Parameterization determines the residual distance between the point and area of ​​the lidar. This refers to the pose of the lidar in the world coordinate system. , Furthermore, the visual luminosity residual is determined through the projection matrix; The odometer speed observation equation is constrained by a local Kalman filter; , The prior covariance of the IMU and the integral is used to optimize the Kalman filter.

[0041] This invention constructs a three-modal tightly coupled joint optimization objective function by weighting and jointly minimizing the residuals of LiDAR, RGB camera, and RTK device using a Huber kernel function. The Huber kernel function... It can be represented as: (2).

[0042] In the formula: The threshold for the Huber kernel function. For the residual, when the absolute value of the residual is... Less than At that time, Huber kernel function Equivalent to the least squares criterion, it maintains high sensitivity to small residuals; when the absolute value of the residuals... Exceed At that time, Huber kernel function The residuals grow linearly, with lower weights given to abnormally large residuals, thus achieving robust optimization. By setting a corresponding residual threshold for each residual, the distinction between "normal residuals" and "abnormal residuals" is determined. For example, if the threshold is too large, robustness is lost; if it is too small, it is excessive and valid observations are discarded.

[0043] The objective function for the three-modal tightly coupled joint optimization is: (3).

[0044] The threshold value for the Huber function of the point-to-surface distance residual. The Huber function threshold for the visual photometric residual. The threshold for the Huber function of the RTK global position residual.

[0045] For the first The point-to-surface distance residual of feature points in a LiDAR point cloud data set. For the first The visual luminance residual of each pixel involved in the calculation This represents the global position residual for RTK.

[0046] By analyzing the error state vector Iterative minimization is performed to solve the objective function of the three-modal tightly coupled joint optimization, and then the pose parameters of the current frame are obtained.

[0047] More specifically, the point-to-surface distance residual is derived by searching for the nearest neighbor plane for each feature point in the LiDAR point cloud data of the current frame. The nearest neighbor plane is obtained by searching within the local map voxel grid. Nearest neighbor search is performed, and principal component analysis (PCA) is used to estimate the unit normal vector of the obtained nearest neighbors. Specifically, the point-to-surface distance residual can be expressed by the following formula: (4).

[0048] In the formula, The first [frame] of the current frame of lidar point cloud data The point-to-surface distance residual of each feature point.

[0049] For the first The unit normal vector of the nearest neighbor plane of each feature point.

[0050] For the first The coordinates of any point on the nearest neighbor plane of each feature point.

[0051] For the first The coordinates of each feature point in the lidar coordinate system. Furthermore, all coordinates used in this invention are in world coordinates.

[0052] The visual luminance residual is calculated by comparing the luminance difference at corresponding pixel locations between the current frame's RGB image data and the reference frame's RGB image data. The visual luminance residual is expressed by the following formula: (5).

[0053] In the formula, , These are the grayscale matrices of the RGB image data of the current frame and the grayscale matrices of the RGB image data of the reference frame, respectively.

[0054] , These are the corresponding pixels in the RGB image data of the current frame and the corresponding pixels in the RGB image data of the reference frame, respectively.

[0055] , and are the pixel points of the RGB image data of the current frame, respectively. The interpolated grayscale value at the location, and the pixel points of the RGB image data of the reference frame. The interpolated grayscale value at that location.

[0056] The reference frame is dynamically updated at regular keyframe intervals during system operation.

[0057] In addition, grayscale values ​​are obtained by projecting 3D map points onto the imaging plane of the RGB camera based on the pose of the current frame, thus yielding the pixel values. , And use bilinear interpolation to obtain the grayscale value with sub-pixel precision; at the same time, when A pixel may participate in residual calculation if it falls within the valid area of ​​the RGB image data of the reference frame; otherwise, it may not.

[0058] RTK global position residual measures the deviation between the position components of the current pose and the RTK measured position. The RTK global position residual can be expressed by the following formula: (6).

[0059] This represents the position component in the current pose.

[0060] This refers to the actual RTK position output by the RTK device.

[0061] In addition, the present invention also includes an RTK information matrix, which is derived from the RTK quality factor in the NMEA message. It is written in the form of a diagonal matrix into the focus block corresponding to RTK in the observation noise covariance matrix of the Kalman filter algorithm. It does not appear explicitly in the objective function formula, but is implicitly weighted through the observation noise covariance matrix.

[0062] This invention employs an Iterative Error State Kalman Filter (IESKF) to solve the joint optimization objective function. Specifically, the solution process involves taking the partial derivatives of the three types of residuals with respect to the error state vector in each iteration of the IESKF, obtaining various Jacobian matrix blocks. These Jacobian matrix blocks are then concatenated to form a joint observation Jacobian matrix. Huber weights are applied to each residual, and the error state increment is calculated using the Kalman update formula. The state is then updated on the Lie group until the error state increment norm is reached. When the number of iterations is less than the set threshold or the maximum number of iterations is reached, convergence is achieved, and the error state increment is obtained. Then, the pose parameters of the current frame are obtained based on the error state increment.

[0063] The formula for a single IESKF iteration update is: (7).

[0064] in, This is the error state vector.

[0065] To jointly observe the Jacobian matrix, the error state increment is adjusted at the current linearized point in each iteration using the point-to-surface distance residual, visual photometric residual, and RTK global position residual in the objective function. Taking the partial derivative yields the corresponding Jacobian matrix block, and then concatenating the three Jacobian matrix blocks gives the Jacobian matrix. .

[0066] The weighted residual vector is specifically obtained by concatenating the point-to-surface distance residual, visual luminosity residual, and RTK global position residual after weighting them according to Huber.

[0067] The prior covariance matrix is ​​obtained by pre-integrating and propagating the prior covariance from the previous time step using the IMU. The specific propagation formula is as follows: (8).

[0068] In the formula: The state transition Jacobian matrix; Input the Jacobian matrix for noise; This is the continuous-time noise spectral density matrix of the IMU, which includes the random walk and Gaussian white noise density parameters of the accelerometer and gyroscope.

[0069] The noise covariance matrix is ​​obtained by weighting using the Huber kernel function.

[0070] This is the Kalman gain matrix.

[0071] More preferably, the present invention also uses the yaw angle obtained by the RTK device during system initialization. The yaw component is directly assigned to the state vector and used as a weak constraint in subsequent optimization (by adjusting the yaw residual). - (Added to the observation equation) to continuously correct yaw drift. Specifically, the yaw angle of the RTK dual-line orientation output is applied to the system through the following two mechanisms: (1) Initialization assignment mechanism: When the system starts, the yaw angle is directly assigned to the yaw component of the rotation part in the initial state vector. The yaw component can be composed of the 3z-axis component of the rotation error and the yaw angle of the current linearization point, specifically: ,in, The yaw angle of the current linearized point. This represents the z-axis component of the rotational error.

[0072] (2) Continuous correction mechanism: In each subsequent iteration of IESKF, the bias residuals are added to the observation equations in the form of weak constraints. The way the bias residuals are added to the observation equations is as follows: (3) Incorporating into the joint Jacobian matrix The last line will Incorporating residual vector The last position will be the yaw measurement noise variance. Write the last diagonal element of the observation noise covariance matrix. Here, "weak constraint" means: yaw noise variance. Much larger than the location noise variance In other words, the weight of the yaw constraint in the information matrix is ​​much lower than that of the position constraint, ensuring that yaw correction is only effective when a large deviation accumulates, thus avoiding negative interference with the high-precision rotation estimation of laser and vision systems. That is, when solving the three-modal tightly coupled joint optimization objective function, the yaw residual is considered... The z-vector, along with the three types of residuals, is incorporated into the IESKF and solved collaboratively using the same Kalman update formula.

[0073] Step S3: Based on the Kalman filter algorithm, fuse the odometer data, the current frame multi-source sensor data, and the current frame pose parameters to obtain the current frame pose.

[0074] This invention employs dual Kalman filters to achieve the fusion of the current pose. Specifically, it includes: firstly, performing local fusion based on the Kalman filter algorithm, odometer data, IMU data, and current frame pose parameters to obtain a local fused pose; then, globally fusion of the local fused pose and RTK positioning data based on the Kalman filter to obtain a global fused pose; and finally, deriving the current frame pose parameters based on the global fused pose.

[0075] The pose parameters of the current frame include the current frame's position, velocity, attitude, accelerometer bias, and gyroscope bias. The handheld SLAM mapping scanner is localized by calculating the pose of the current frame, thereby enabling map construction.

[0076] Local fusion refers to the process of using the pose parameters of the current frame as the estimated quantities, the pre-integration results of IMU data as the prediction input for system state transition, and the velocity constraints of odometry and IESKF pose constraints (position constraints and rotation constraints) as the observations within a set local region to complete Kalman prediction and update, and then output the local fused pose.

[0077] The speed constraint of the odometer is the speed prediction output by the odometer, which is provided to the Kalman filter as an external observation.

[0078] IESKF pose constraints include IESKF position constraints and IESKF rotation constraints. The IESKF position constraint is the current frame position component output by IESKF, which serves as the position observation of the local Kalman filter. The IESKF rotation constraint is the attitude observation obtained by converting the current frame rotation matrix output by IESKF into a rotation vector.

[0079] The local region refers to a local time window centered on the current keyframe, which is usually the time period between two adjacent keyframes. Within this window, RTK positioning data is not introduced, and only IMU data and odometer mileage data are used for local observation to ensure the real-time and continuity of high-frequency output.

[0080] Global fused pose refers to using the local fused pose as the predictor and the EMU 3D coordinates of the RTK positioning data as the observation, correcting the position components according to Kalman gain, and then outputting the global fused pose. The update period of the global fusion is synchronized with the RTK output frequency (10 Hz). During a brief period of RTK signal loss, the global Kalman filter enters pure prediction mode, maintaining the output with the local fused pose. The prediction and update of the global fused pose are similar to those of the local fused pose, and will not be described in detail here.

[0081] Step S4: Overlay the current frame pose and laser point cloud data to generate a 3D map.

[0082] To further eliminate the unavoidable global cumulative drift in large-scale mapping, this invention requires further evaluation of the current frame. Specifically, it determines whether the current frame is a keyframe. If so, the current frame is stored in the historical keyframe database, and loop closure detection is triggered; otherwise, step S4 is executed to directly generate a 3D map. Specifically, the current frame is considered a keyframe if the time interval between the current frame and the previous historical keyframe is greater than a preset interval, or if the displacement between the current frame and the previous historical keyframe exceeds a preset displacement.

[0083] More preferably, the loop closure detection mechanism corrects the global pose graph through GTSAM factor graph optimization. Specifically, the loop closure detection process includes: Step 1: Extract image feature points from the RGB image data of the current keyframe, and quantize the feature points into bag-of-words vectors using a pre-trained DBoW2 dictionary.

[0084] Step 2: Based on the bag-of-words vector of the current keyframe and the bag-of-words vector of each keyframe in the historical keyframe database, candidate loop-closing frames are selected from the historical keyframe database.

[0085] Each keyframe in the historical keyframe database also includes a global fused pose and its DBoW2 bag-of-words vector. The historical keyframe database is maintained in a sliding window format, with a maximum number of keyframes set, and indexed by position to support fast nearest neighbor lookups. Candidate loop-closure frames are selected from the historical keyframe database by calculating the similarity between the bag-of-words vector of the current keyframe and the bag-of-words vectors of each keyframe in the historical keyframe database.

[0086] Step 3: Verify the candidate loopback frames to derive the loopback frames from the candidate loopback frames.

[0087] By performing RANSAC geometric consistency verification on candidate loop closure frames and using BRIEF descriptors of image feature points for feature matching, false loop closure frames that are similar in appearance but different in location are eliminated, and then several loop closure frames are selected from the candidate loop closure frames.

[0088] Step 4: Perform iterative nearest-point fine registration on the loop-loop frame based on the lidar point cloud data of the current key frame to achieve registration of the loop-loop frame, and estimate the relative pose based on the registered loop-loop frame.

[0089] Specifically, the point cloud data of the current keyframe is used as the source point cloud, and the point cloud data of each loopback frame is used as the target point cloud. Based on the ICP algorithm, the point cloud data of the current keyframe is matched and registered with the point cloud data of each loopback frame, and then the registered transformation matrix is ​​output, which is the relative pose of the current frame with respect to the loopback frame, so as to obtain the pose constraint of the loopback frame.

[0090] Step 5: Add the loopback frame pose constraints to the constructed global factor graph, and optimize the global factor graph using GTSAM to output the globally optimal historical keyframe pose sequence. Then, optimize the global fused pose based on the globally optimal historical keyframe pose sequence. GTSAM, short for Georgia Tech Smoothing and Mapping Library, specifically optimizes the global factor graph using the iSAM2 incremental smoothing algorithm.

[0091] During map construction, coordinate transformation is performed based on the optimized pose parameters of the current frame and the LiDAR point cloud data to transform the point cloud of the current frame to the world coordinate system and then overlay it to generate a global 3D map. The global 3D map is dynamically managed using a voxel grid based on a KD-tree index, with the voxel resolution set to 0.05m by default to achieve a balance between storage usage and map accuracy.

[0092] To verify the effectiveness and advancement of the multi-sensor fusion map construction method proposed in this invention, systematic experimental evaluations were conducted in multiple test scenarios, and the results were compared with existing representative methods, as shown in Table 1:

[0093] Table 1 The test sequence covers the following five scenarios: indoor corridor (Seq01), outdoor open space (Seq02), indoor long corridor with geometric degradation (Seq03), low-light environment (Seq04), and scene with dynamic pedestrians (Seq05). As shown in Table 1, the ATE (Absolute Trajectory Error) of this invention is superior to currently popular SLAM map building methods in all test scenarios; and the processing time is only about 42 ms / frame, demonstrating good real-time performance. Among them, FAST-LIVO2 is an open-source LiDAR-inertial-vision fusion localization and mapping system; LIO-SAM is a tightly coupled LiDAR inertial odometry system; LiORF is an improved SLAM system based on LIO-SAM; and ORB-SLAM3 is an open-source visual SLAM system released by the Robotics Perception Laboratory of the University of Zaragoza, Spain.

[0094] Example 2 The map building device based on multi-sensor fusion includes a memory and a processor. The memory stores a map building program that runs on the processor. The map building program is a computer program. When the processor executes the map building program, it implements the steps of the map building method based on multi-sensor fusion as provided in Embodiment 1.

[0095] Example 3 A computer-readable storage medium storing a map building program thereon, the map building program being a computer program, which, when executed by a processor, implements the steps of the map building method based on multi-sensor fusion as provided in Embodiment 1.

[0096] Example 4 A handheld SLAM mapping and scanning control system includes a sensor module, a clock synchronization controller, and an edge computing platform. The sensor module includes a LiDAR, an RGB camera, an IMU sensor, and an RTK device. The clock synchronization controller is electrically connected to the LiDAR, RGB camera, IMU sensor, and RTK device, and is used to receive PP signals from the RTK device and generate a clock synchronization signal, which is then synchronously sent to the LiDAR, RGB camera, and IMU sensor. The edge computing platform is electrically connected to the LiDAR, RGB camera, IMU sensor, and RTK device in the sensor module, and is used to execute the steps of the map construction method based on multi-mode sensors provided in Embodiment 1.

[0097] The above embodiments are merely preferred embodiments of the present invention and should not be construed as limiting the scope of protection of the present invention. Any non-substantial changes and substitutions made by those skilled in the art based on the present invention shall fall within the scope of protection claimed by the present invention.

Claims

1. A map construction method based on multi-sensor fusion, characterized in that, The map construction method includes: Data acquisition steps: Acquire multi-source sensor data of the current frame using a handheld SLAM mapping scanner multi-mode sensing device and preprocess the current frame multi-source sensor data; the multi-source sensor data includes lidar point cloud data, RGB image data, IMU data and RTK positioning data; Pose optimization steps: Based on the multi-source sensor data of the current frame, optimize and solve the pre-constructed three-modal tightly coupled joint optimization objective function in the system to obtain the pose parameters of the current frame; Pose fusion steps: Based on the Kalman filter algorithm, the odometer mileage data, the current frame multi-source sensor data and the current frame pose parameters are fused to obtain the current frame pose; Map building steps: Overlay the current frame pose and LiDAR point cloud data to obtain a 3D map.

2. The map construction method based on multi-sensor fusion according to claim 1, characterized in that, The pose fusion step includes: a local fusion step: based on the Kalman filter algorithm, the odometer mileage data, IMU data and the pose parameters of the current frame are locally fused in a preset local region to obtain the local fused pose; the local region refers to a local time window centered on the current keyframe; Global fusion step: Based on the Kalman filter algorithm, the local fused pose and RTK positioning data are fused to obtain the global fused pose, and then the current frame pose is obtained based on the global fused pose; the current frame pose includes the attitude, position, velocity, accelerometer bias and gyroscope bias of the current frame.

3. The map construction method based on multi-sensor fusion according to claim 2, characterized in that, The global fusion step, after obtaining the global fusion pose, also includes: a keyframe determination step: determining whether the current frame is a keyframe. If so, the current frame is recorded as the current keyframe and stored in the historical keyframe database, and then the loop closure detection step is executed; if not, the map construction step is executed. The loop closure detection step involves performing loop closure detection on historical keyframes in the system's historical keyframe database based on the current keyframe, and optimizing the global fusion pose based on the loop closure detection results.

4. The map construction method based on multi-sensor fusion according to claim 3, characterized in that, The loop closure detection step specifically includes: Step 1: Extract image feature points from the RGB image data of the current keyframe, and quantize the image feature points into bag-of-words vectors using a pre-trained DBoW2 dictionary; Step 2: Based on the bag-of-words vector of the current keyframe and the bag-of-words vector of each keyframe in the historical keyframe database, candidate loop-closing frames are selected from the historical keyframe database. Step 3: Verify each candidate loopback frame based on geometric consistency verification to derive the loopback frame from the candidate loopback frames; Step 4: Based on the global fusion pose of the current keyframe and the global fusion pose of the loopback frame, perform ICP registration on the LiDAR point cloud data of the current keyframe and the LiDAR point cloud data of the loopback frame. Then, based on the ICP registration result, obtain the relative pose of the current keyframe relative to the loopback frame, and then obtain the pose constraint of the loopback frame based on the relative pose. Step 5: Add the loop closure frame pose constraints to the constructed global factor graph, and optimize the global factor graph using GTSAM to output the globally optimal historical keyframe pose sequence. Then, optimize the global fused pose based on the globally optimal historical keyframe pose sequence.

5. The map construction method based on multi-sensor fusion according to claim 1, characterized in that, The pose optimization steps specifically include: The residual calculation steps are as follows: the point-to-surface distance residual is obtained from the LiDAR point cloud data of the current frame, the visual photometric residual is obtained from the RGB image data of the current frame, and the RTK global pose residual is obtained from the RTK positioning data of the current frame. Joint optimization steps: Determine the pose parameters and construct an error state vector based on the pose parameters. Combine the point-to-surface distance residual, visual photometric residual, and RTK global pose residual to construct a three-modal tightly coupled joint optimization objective function. Then, optimize the three-modal tightly coupled joint optimization objective function based on the iterative error state Kalman filter to obtain the pose parameters of the current frame.

6. The map construction method based on multi-sensor fusion according to claim 5, characterized in that, The formula for the three-modal tightly coupled joint optimization objective function is as follows: ； in, Let be the error state vector, and ; , , , , , respectively, are the rotation matrix, translation vector, carrier velocity vector, acceleration bias vector, and gyroscope bias vector; , , , , These are rotation error, position error, velocity error, accelerometer bias error, and gyroscope bias error, respectively. For Huber kernel function; , , These are the Huber function thresholds for point-to-surface distance residuals, visual photometric residuals, and RTK global position residuals, respectively. For the first The point-to-surface distance residual of feature points in a LiDAR point cloud data set. For the first The visual luminance residual of each pixel involved in the calculation. This represents the global position residual for RTK. The point-to-plane distance residual is obtained by searching for the nearest neighbor plane for each feature point in the lidar point cloud data of the current frame. The formula for the point-to-plane distance residual is: ； In the formula, The first [frame] of the current frame of lidar point cloud data The point-to-surface distance residual of each feature point. For the first The unit normal vector of the nearest neighbor plane of each feature point. For the first The coordinates of any point on the nearest neighbor plane of each feature point. For the first Coordinates of each feature point in the lidar coordinate system It is the unit normal vector of the nearest neighbor plane; The pose of the lidar in the world coordinate system; The visual luminance residual is obtained by comparing the luminance difference at corresponding pixel positions between the RGB image data of the current frame and the RGB image data of the reference frame. The formula for the visual luminance residual is: ； In the formula, , , , are the grayscale matrices of the RGB image data of the current frame and the grayscale matrices of the RGB image data of the reference frame, respectively; , , and , respectively, are the corresponding pixels of the RGB image data of the current frame and the corresponding pixels of the RGB image data of the reference frame; , and are the pixel points of the RGB image data of the current frame, respectively. The interpolated grayscale value at the location, and the pixel points of the RGB image data of the reference frame. Interpolated grayscale value at the location; The RTK global position residual measures the deviation between the position component of the current frame and the RTK measured position. The formula for the RTK global position residual is: ； The position component in the current pose; This refers to the actual RTK position output by the RTK device.

7. The map construction method based on multi-sensor fusion according to claim 1, characterized in that, The preprocessing of the current frame multi-source sensor data specifically includes: laser point cloud motion distortion correction preprocessing, IMU pre-integration processing, and RTK coordinate transformation preprocessing; Before the data acquisition step, the method further includes: calibrating the parameters of the LiDAR, RGB camera, IMU sensor and RTK device of the handheld SLAM mapping scanner; the parameter calibration includes RGB camera intrinsic parameter calibration, RGB camera-IMU sensor extrinsic parameter calibration, LiDAR-IMU sensor extrinsic parameter calibration, LiDAR-RGB camera extrinsic parameter calibration and RTK antenna offset measurement.

8. A map building apparatus based on multi-sensor fusion, comprising a memory and a processor, wherein the memory stores a map building program that runs on the processor, characterized in that, The map building program is a computer program, and when the processor executes the map building program, it implements the steps of the map building method based on multi-sensor fusion as described in any one of claims 1-8.

9. A computer-readable storage medium having a map-building program stored thereon, characterized in that, The map building program is a computer program, and when executed by a processor, it implements the steps of the map building method based on multi-sensor fusion as described in any one of claims 1-8.

10. A handheld SLAM mapping and scanning control system, characterized in that, The system includes a sensor module, a clock synchronization controller, and an edge computing platform. The sensor module comprises a LiDAR, an RGB camera, an IMU sensor, and an RTK device. The clock synchronization controller is electrically connected to the LiDAR, RGB camera, IMU sensor, and RTK device, and is used to receive PP signals from the RTK device and generate a clock synchronization signal, which is then synchronously sent to the LiDAR, RGB camera, and IMU sensor. The edge computing platform is electrically connected to the LiDAR, RGB camera, IMU sensor, and RTK device in the sensor module, and is used to execute the steps of the map construction method based on multi-sensor fusion as described in any one of claims 1-7.

Images