Construction site-oriented multi-modal adaptive weight SLAM method and system

Abstract

The invention discloses a multi-mode adaptive weight SLAM method and system for a construction site. The method comprises the steps that a mapping model is established through sensor joint calibration, laser radar, camera and IMU data are synchronously collected through hardware, and dynamic filtering, distortion compensation and feature extraction are carried out on point clouds and images; the voxel entropy, the angular resolution and the feature density are calculated in real time to evaluate the degradation degree of each mode, and a visual residual error is dynamically adjusted by adopting an adaptive weight and frame skipping mechanism to ensure reasonable information injection in a degradation scene; iMU pre-integration, LiDAR point-surface residual errors and weighted visual reprojection residual errors are sequentially fused in an incremental Kalman sliding window frame, an automatic repositioning strategy is matched, and a high-precision pose and dense color point cloud map is output. According to the method, calibration, synchronization, degradation detection, sliding window optimization, map release and relocation are integrated, and the mapping precision and robustness can be remarkably improved under the complex working conditions of dust shielding, illumination variation, dynamic obstacles and the like.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of three-dimensional perception of construction sites, and particularly relates to a multi-modal adaptive weight SLAM method and system for construction sites, which is suitable for real-time positioning and three-dimensional environment perception under complex environment of construction sites. BACKGROUND

[0002] With the rapid development of building industrialization and smart construction sites, the demand for real-time three-dimensional perception and high-precision positioning of construction sites is increasingly prominent. Under the complex environmental conditions of construction sites, traditional mapping methods based on a single sensor cannot meet the dual requirements of precision and robustness. Visual SLAM is prone to feature extraction failure and tracking loss in scenes with severe changes in light or lack of texture, and laser SLAM may have attitude singularity and scale drift when geometric constraints degenerate. In order to break through the above bottlenecks, in recent years, the academic field has proposed a laser point cloud and visual image fusion SLAM framework, which optimizes laser geometric residuals and image re-projection residuals jointly at the factor graph level to realize information complementation.

[0003] Current laser point cloud and visual image fusion SLAM methods have achieved good results in conventional urban and indoor scenes, but still face robustness challenges in construction sites with dust, repeated structures, and numerous dynamic obstacles. Existing fusion algorithms generally use fixed residual weight or linear weighting strategy to process dynamic targets, which has poor filtering effect. When one modality fails due to environmental degradation, noise information is still injected into the optimization process with high weight, which not only amplifies the error, but also may cause map distortion. In view of these pain points, the present application focuses on the dust shielding, repeated reinforcement formwork, and high dynamic mechanical operation conditions specific to construction scenes, and proposes a multi-modal adaptive weight SLAM method and system for construction sites, thereby significantly improving mapping accuracy and system stability while maintaining real-time calculation. SUMMARY

[0004] The application provides a multi-modal adaptive weight SLAM method and system for construction sites, which significantly improves mapping accuracy and robustness under complex conditions such as dust shielding, light changes, and dynamic obstacles.

[0005] Technical scheme: The multi-modal adaptive weight SLAM method for construction sites provided by the application comprises the following steps:

[0006] (1) jointly calibrate the laser radar and the camera to establish a mapping model between the three-dimensional laser point cloud and the two-dimensional visual image;

[0007] (2) Hardware trigger is used to realize time synchronization between laser radar and camera. After the external parameter calibration, three-dimensional laser point cloud and corresponding two-dimensional visual image frames of the construction site are synchronously collected;

[0008] (3) After obtaining the time-synchronized data, voxel-level motion consistency detection is performed on the point cloud to extract and remove the points of dynamic objects. In the image end, dynamic pixel regions are extracted using optical flow and semantic segmentation;

[0009] (4) For the laser point cloud, motion distortion compensation is performed in combination with the IMU integral result, voxel downsampling is performed to uniformize the spatial distribution, and edge points and plane points are divided based on curvature estimation. For the visual image, lens distortion and contrast balance are completed, the dynamic pixel region extracted in step (3) is ignored, FAST corner points are detected and ORB descriptors are calculated, and the area of invalid region caused by dust or occlusion in the image is counted to obtain the occlusion rate;

[0010] (5) For the same frame of point cloud and image data collected in step (2), the voxel entropy and angular resolution are calculated to measure the degree of geometric degradation of the point cloud; the feature point density, local contrast distribution and occlusion rate are used to quantify the visual degradation degree; and the point cloud degradation score and the visual degradation score are obtained respectively;

[0011] (6) According to the visual degradation score and the point cloud degradation score of each frame obtained in (5), the original visual weight is calculated. The weight is improved when the visual feature is obvious and the point cloud is degraded, weakened when the visual is moderately degraded, and skipped when the visual is severely degraded;

[0012] (7) In the incremental Kalman sequential update framework, the IMU pre-integral factor is used for state prediction, the LiDAR point surface residual factor is linearized and Kalman updated in sequence, and the visual re-projection residual factor is updated in sequence; all updates are accumulated into the current state and covariance. When the number of frames exceeds the sliding window depth, the earliest frame is edge-eliminated by Schur complement, the prior is generated and the old frame is removed, and the history constraint and size are kept constant;

[0013] (8) After optimization, high-precision pose estimation and dense color point cloud map are released. When it is detected that multiple consecutive frames are in severe degradation and the tracking credibility is lower than the threshold, automatic repositioning is triggered.

[0014] Further, the mapping model between the three-dimensional laser point cloud and the two-dimensional visual image in step (1) is:

[0015]

[0016]

[0017] In the formula, (u, v) is the pixel point coordinate in the pixel coordinate system; Rl T l Let C be the external parameters of the lidar in the world coordinate system, where α, β, and γ are the roll angle, pitch angle, and yaw angle, respectively; X C Y C Z R represents the coordinates of the lidar in the world coordinate system during image acquisition. lc T lc f is the relative extrinsic parameter obtained from the joint calibration of the lidar and camera. x f y Let c be the focal length of the camera along the x-axis and y-axis in the pixel coordinate system. x c y Let x be the coordinates of the camera's optical center in the pixel coordinate system. w y w , z w ) represents the coordinates of a point in the world coordinate system.

[0018] Furthermore, the implementation process of step (3) is as follows:

[0019] (3.1) In the point cloud, the current frame and the previous frame point cloud are divided into voxels. The point density and centroid offset of the voxel blocks at the same position are compared. If the change exceeds the set threshold, it is marked as a dynamic region. The corresponding point set is removed from the point cloud to remove the dynamic object. The remaining point cloud is filtered for connectivity, and static points connected to the main structure are retained.

[0020] (3.2) At the image end, calculate the pixel displacement field between consecutive image frames and identify regions with abrupt displacement changes; use a pre-trained semantic segmentation network to detect dynamic targets in the image, fuse the optical flow abnormal region with the semantic segmentation mask, and obtain the final dynamic mask.

[0021] Furthermore, the implementation process of step (4) is as follows:

[0022] (4.1) Read the IMU angular velocity and acceleration data synchronized with the point cloud time, calculate the relative motion within the scanning cycle through pre-integration, and project each laser point within the scanning cycle back to the radar scanning trigger time based on linear interpolation to eliminate the point cloud trailing caused by sensor motion.

[0023] (4.2) The distortion-corrected point cloud is downsampled using a uniform voxel grid with a fixed leaf length; the local surface curvature is calculated based on neighborhood PCA:

[0024] Curvature = λ3 / (λ1+λ2+λ3))

[0025] In the formula, λ1 is the distribution variance of the neighborhood point cloud along the main direction, λ2 is the distribution variance of the neighborhood point cloud along the secondary direction, and λ3 is the distribution variance of the neighborhood point cloud along the normal direction; the edge points and planar points are divided; at the same time, points with long distance or strong reflection noise are removed, and KD-Tree radius search is combined to accelerate the neighborhood calculation.

[0026] (4.3) Based on the offline calibrated camera intrinsic parameters, perform subpixel-level distortion correction on the image acquired in step (2), apply contrast-limited adaptive histogram equalization, process the image in blocks and limit local gain to suppress overexposure or underexposure caused by dust.

[0027] (4.4) Detect dynamic threshold FAST corner points on the processed image and generate ORB descriptors; identify unusable areas by setting grayscale thresholds; calculate the occlusion rate occ by counting the proportion of pixels in these unusable areas to the total number of pixels.

[0028] Furthermore, the curvature of the edge point is greater than 0.1 and λ1 / λ2≥3.0, λ1 / λ3≥3.0; the curvature of the plane point is less than 0.05 and λ2 / λ1≤0.30, λ3 / λ1≤0.10.

[0029] Furthermore, the implementation process of step (5) is as follows:

[0030] (5.1) Using the downsampled point cloud from step (4.2) as input, the point cloud space is divided into a voxel grid, and the information entropy H of the point density distribution within each voxel is calculated. pc :

[0031]

[0032] In the formula, N represents the total number of voxels in the point cloud spatial partition, and p i Let n be the probability that the number of points within the i-th voxel represents the total number of points in the point cloud. i This represents the number of laser points within the i-th voxel;

[0033] (5.2) Project the point onto the unit sphere, find the nearest neighbor angle between the sampling direction vectors, and take the average to obtain the average angular resolution R. θ ;

[0034] (5.3) The voxel entropy H PC and average angular resolution R θ After linear normalization, the orientation is made consistent, and the linear combination results in a single point cloud degradation score D. L :

[0035] D L =αH norm +βR norm

[0036] Here, α and β are two weighting coefficients, determined by offline testing;

[0037] (5.4) Input the image processed in step (4.3), and calculate the effective feature point density ρ by the ratio of the number of effective FAST corner points obtained in (4.4) to the total number of pixels in the image. feat ;

[0038] (5.5) Calculate the standard deviation of luminance σ within an 8×8 sliding window. l Then, iterate through the image and take the global mean to obtain the local contrast C. img ;

[0039] (5.6) The effective feature point density, brightness standard deviation, and occlusion rate (occ) obtained in (4.4) are linearly normalized and their directions are aligned. They are then linearly combined to form a single image degradation score D. V :

[0040] D V =γρ norm +δσ norm +εocc norm

[0041] Where γ, δ, and ε are three weighting coefficients determined by offline testing, and the generated D V ∈[0,1] and point cloud degradation score D L Maintain the same dimensions.

[0042] Furthermore, the implementation process of step (6) is as follows:

[0043] (6.1) Based on the visual degradation score D calculated for each frame in step (5) V With point cloud degradation score D L Calculate the uncropped visual weights using the following formula.

[0044]

[0045] Where η is the compensation coefficient for visual degradation of point cloud;

[0046] (6.2) will Clip to interval [λ] min [1], to ensure minimum visual retention

[0047]

[0048] (6.3) If Below the frame skipping threshold λ skipIf the number of features in the current image is less than the set minimum value, the visual observation of this frame will be skipped and no residual factor will be generated. A maximum of 4 consecutive frame skips are allowed. After exceeding this limit, one frame of visual observation will be forcibly retained to reconstruct the constraints.

[0049] (6.4) Apply a first-order low-pass filter to the weights after each frame cropping to suppress weight abrupt changes caused by degradation score fluctuations and reduce nonlinear optimization oscillations:

[0050]

[0051] Where α is the filter coefficient.

[0052] Furthermore, the implementation process of step (7) is as follows:

[0053] (7.1) Let the state vector of the i-th keyframe within the sliding window be:

[0054]

[0055] Among them, R i t represents the three-dimensional rotation of the camera or lidar coordinate system relative to the world coordinate system in the i-th frame; i v represents the position coordinates of the camera in the world coordinate system in the i-th frame; i This represents the linear velocity vector of the camera relative to the world coordinate system at frame i. This indicates the zero bias of the IMU angular velocity sensor; This represents the zero bias of the IMU accelerometer; the keyframe sliding window has a fixed length of N, and edge detection is performed when it exceeds the limit.

[0056] (7.2) Using the raw angular velocity and acceleration of the IMU between adjacent keyframes, the rotation, velocity and displacement increments are obtained through pre-integration. The incremental motion model is then used to recalculate the state X of the previous frame according to Kalman prediction. i-1 Perform state propagation, synchronously linearize the Jacobian, update the covariance matrix, and form a prior estimate for the i-th frame;

[0057] (7.3) For each point cloud feature point p k Fit plane parameters to the local map, transform the point from the current frame coordinate system to the world coordinate system, and calculate the distance to its corresponding plane as the residual using the following formula:

[0058]

[0059] Where, p k R represents the coordinates of the k-th LiDAR feature point in the radar coordinate system of the current frame. i With t i These are the rotation matrix and translation vector for the i-th frame, respectively; n kThe unit normal vector corresponding to the planar feature is obtained by fitting the neighborhood point set of the local map using PCA; d k The offset of the plane equation defines the specific position of the plane in the world coordinate system;

[0060] Perform first-order linearization on the point cloud residuals and calculate their relationship with respect to the state variable X. i Jacobian matrix:

[0061]

[0062] The gain matrix is ​​calculated using the classic Kalman measurement update formula, the state increment is updated and the covariance is corrected, and each LiDAR geometric factor is fused into the state estimate one by one as it arrives.

[0063] (7.4) For the m-th map point in the i-th frame Calculate the reprojection error:

[0064]

[0065] Among them, z im =[u im ,v im ] T , where is the pixel coordinate of the m-th feature point actually observed in the i-th frame; For the predicted pixel position, Let m be the m-th 3D map point in the world coordinate system; [R i , t i [] represents the extrinsic parameter of the i-th frame; K is the camera intrinsic parameter matrix; (·) represents the perspective projection operation;

[0066] The visual residual is linearized to the first order, and its relationship with the state variable X is calculated. i Jacobian matrix:

[0067]

[0068] Introducing visual weights Standard visual noise covariance R v Scaling is performed to dynamically adjust the weight of visual information in backend optimization:

[0069]

[0070] The algorithm uses the classic Kalman measurement update formula to calculate the gain matrix, update the state increment and correct the covariance. After predicting the IMU and LiDAR updates, the algorithm injects visual information with dynamic weights in sequence.

[0071] (7.5) Integrate the IMU pre-integration residual, the LiDAR geometric residual, and the weighted visual reprojection residual into a weighted least squares objective function:

[0072]

[0073] Among them, X i r is the state vector of the i-th frame; imu，ij W is the IMU pre-integration residual from frame i to frame j. imu Its covariance; r l,k Let w be the weight of the k-th LiDAR geometric residual. l Reflects LiDAR noise level; r v,im For the m-th visual reprojection residual of the i-th frame, To achieve adaptive visual weights, corresponding covariance weight matrices or scalars are applied to all residual terms, and a unified factor graph approach is used for solving, thus realizing tight coupling between IMU, LiDAR, and vision.

[0074] (7.6) In each optimization iteration, the algorithm estimates X based on the current state. i As the expansion point, recalculate all residual terms and their Jacobian matrices; before each linearization, update the visual factor weights based on the latest sensor degradation. Incremental Gauss-Newton linearization and updates are performed on LiDAR and visual factors;

[0075] (7.7) When the number of sliding window frames exceeds the preset length N, the oldest frame is removed and its information is retained as prior information to prevent the loss of historical constraints.

[0076] Furthermore, the implementation process of step (8) is as follows:

[0077] (8.1) After the optimization iteration converges, rotate and translate the current keyframe by R. i , t i Linear velocity v i and IMU bias The final estimation result is published to downstream modules via ROS TF and callback interface; the visual degradation score D obtained in this optimization is used. V LiDAR Degradation Score D L and final visual weight The data is fed back to the front-end filtering and weight calculation module for data preprocessing and adaptive weight updates in the next frame.

[0078] (8.2) Based on the current pose, fuse the static point cloud of the current frame with the corresponding RGB image projection to obtain a color point cloud fragment, and incrementally fuse the fragment into the global map;

[0079] (8.3) Maintain an observation sliding window of length , and accumulate statistics on the severe degradation flag and tracking confidence score of each frame; when the number of “severe degradation” frames in the window exceeds the preset threshold continuously and the average tracking confidence score is lower than the threshold, the tracking is considered unstable and relocation is required.

[0080] (8.4) After the relocation condition is triggered, the regular sliding window update is paused. Several potential candidate poses are quickly retrieved from the previously saved keyframe library using visual and LiDAR features to generate a preliminary pose guess list.

[0081] (8.5) For each candidate initial pose, load the corresponding map points and the current frame observation data, and perform a local optimization: perform incremental Gauss-Newton iteration on the visual reprojection residual and LiDAR point-area residual to fine-tune the candidate pose; verify the overall residual and tracking frame number after optimization, and select the candidate with the smallest residual and the highest tracking quality as the final relocalization result.

[0082] (8.6) After selecting the best relocation pose, replace the earliest frame state in the sliding window with the pose, reinitialize the sliding window based on the new pose, resume the IMU prediction and sequential measurement update process, and exit the relocation mode.

[0083] (8.7) After successful relocalization, the new degradation score, visual weight and relocalization status are fed back to the front-end dynamic filtering and weight calculation module to ensure that the preprocessing and weight adaptation of the next frame are based on the latest global reference, thereby realizing the transition from relocalization to normal fusion in extreme degradation scenarios.

[0084] The multimodal adaptive weighted SLAM system for construction sites described in this invention includes the following modules:

[0085] The calibration module is used to jointly calibrate the lidar and camera, outputting the rotation and translation matrix between the image and point cloud acquired by the sensor with a fixed relative position, and establishing a mapping model between the three-dimensional lidar point cloud and the two-dimensional visual image.

[0086] The synchronization module uses an external STM32 as a hardware trigger unit to drive the LiDAR, camera and IMU with hardware trigger signals to achieve precise clock synchronization and send the synchronized raw data to each preprocessing module.

[0087] The dynamic target filtering module removes moving object points based on multi-frame voxel consistency and inter-scan isolation detection in the point cloud; and filters out dynamic pixels by fusing optical flow and deep learning instance segmentation in the vision end.

[0088] The point cloud preprocessing module performs back projection based on the IMU pre-integration at the acquisition time of each laser point, performs motion distortion compensation for each laser point, and then extracts edge points and planar points on the compensated point cloud and removes noise.

[0089] The image preprocessing module performs subpixel-level distortion correction and CLAHE contrast equalization on each frame of the image, detects FAST corner points with dynamic thresholds in blocks and generates ORB descriptors, calculates the proportion of invalid pixel areas caused by dust or occlusion, and outputs the image occlusion rate to provide visual indicators for degradation detection.

[0090] The degradation detection module calculates the voxel entropy and average angular resolution of each frame to quantify the degree of geometric degradation of LiDAR, and calculates the visual degradation score of the image based on the effective feature point density, local contrast and occlusion rate.

[0091] The adaptive weight calculation module combines the degree of geometric degradation and the visual degradation score to calculate the original visual weights. After cropping, it generates smooth weights through a first-order low-pass filter and automatically determines whether to skip frames based on the threshold.

[0092] The sliding window optimization module uses the IMU pre-integration factor to perform state prediction and covariance propagation on the latest frame; it linearizes each LiDAR point-area residual factor in the order of arrival and performs Kalman measurement update; it applies weighted Kalman update to the visual reprojection residual factor of the adaptive frame selection in sequence; when the number of key frames exceeds the window depth, it generates a prior for the oldest frame and removes the old frame.

[0093] The map building and publishing module accepts the optimal pose obtained after sliding window optimization, fuses static point cloud with color image information to generate a high-precision dense color point cloud map, and outputs real-time mapping results to downstream systems through a visualization interface or message publishing mechanism.

[0094] The relocalization module continuously monitors the degradation score and tracking confidence across multiple consecutive frames. When severe degradation is detected and the confidence score falls below a set threshold, it triggers global subgraph or loop closure detection and restarts the localization process.

[0095] Beneficial effects: Compared with the prior art, the beneficial effects of the present invention are as follows:

[0096] 1. This invention effectively eliminates moving object interference by combining multi-frame voxel consistency testing at the radar end with a dynamic target filtering method based on optical flow and semantic segmentation at the visual end. This allows the back-end SLAM optimization to rely solely on the static structure, improving positioning stability and accuracy; and significantly enhancing robustness in dynamic environments.

[0097] 2. This invention introduces a dual-modal degradation evaluation mechanism of laser and vision for the first time, and designs an adaptive adjustment strategy for visual weights. This enables automatic enhancement of visual constraints when the laser geometry is sparse, automatic weakening of visual constraints and even frame skipping when visual information degrades. At the same time, a first-order low-pass filter is introduced to smooth weight changes, effectively avoiding optimization oscillations caused by drastic weight fluctuations, and realizing the complementary advantages of laser and vision sensors.

[0098] 3. This invention transforms the sliding window batch optimization of the original LiDAR-Inertial-Visual (radar-inertial measurement unit-camera fusion) method into an incremental Kalman sequential update process of "IMU prediction → LiDAR measurement update → visual measurement update → old frame edge-out", which does not require large-scale sparse matrix decomposition and effectively reduces peak computation and memory requirements.

[0099] 4. This invention integrates an automatic relocation mechanism, which can quickly restore positioning in continuous and severely degraded scenarios, forming a complete perception closed-loop feedback system, thereby improving the reliability and applicability of the system in complex engineering environments. Attached Figure Description

[0100] Figure 1 This is a flowchart of a multimodal adaptive weighted SLAM method for construction sites;

[0101] Figure 2 It is the coordinate system involved in the mapping model between 3D laser point clouds and 2D visual images;

[0102] Figure 3 This is a schematic diagram illustrating the principle of dynamic target filtering implemented in this invention;

[0103] Figure 4 This is a schematic diagram illustrating the principle of adaptive weight adjustment for dual-modal degradation assessment proposed in this invention.

[0104] Figure 5 This is a rendering of the map created using the present invention. Detailed Implementation

[0105] The present invention will now be described in further detail with reference to the accompanying drawings:

[0106] This invention provides a multimodal adaptive weighted SLAM method for construction sites, such as... Figure 1 As shown, it includes the following steps:

[0107] Step 1: Perform joint calibration of the lidar and camera, and establish a mapping model between the 3D lidar point cloud and the 2D visual image.

[0108] 3D laser point cloud data is obtained through LiDAR scanning. Solid-state LiDAR is recommended, as it allows for denser point cloud data under fixed conditions. 2D visual images are obtained through camera capture.

[0109] The joint calibration method for LiDAR and camera requires setting up a checkerboard calibration board, adjusting the position of the calibration board, and acquiring multiple sets of synchronous LiDAR point cloud and camera image data. Sub-pixel coordinates of checkerboard corner points are extracted from the camera images, and the coordinates of the intersection points of the checkerboard point cloud are extracted from the corresponding LiDAR point cloud by establishing a checkerboard model and fitting the model. The PnP algorithm is used to calculate the initial rotation and translation relationship between the LiDAR and camera. Optimization methods are used to jointly optimize multiple sets of data to obtain a more accurate LiDAR-camera extrinsic parameter matrix.

[0110] For example, in this embodiment, a solid-state lidar is selected to collect point cloud data, with a field of view of 70.4° × 77.2°. A focal length (f) is selected. x f y An industrial camera with coordinates (4397.696171, 4398.796641) has an optical center (c). x c y The coordinates are (1998.921593, 1569.022796), and the image resolution is 4024×3036 pixels. The relative positions of the lidar and the industrial camera are fixed. Point clouds and images at the same location are collected from the calibration board and recorded as a set of data. 6-8 sets of data are collected at different locations, and the extrinsic parameter matrix from the lidar to the camera is calculated.

[0111] After obtaining the camera intrinsic parameter matrix and the sensor extrinsic parameter matrix, a mapping model between the 3D laser point cloud and the 2D visual image is established. The mapping model is as follows:

[0112]

[0113] In the formula, (u, v) are the pixel coordinates in the pixel coordinate system; R l T l Let C be the external parameters of the lidar in the world coordinate system, where α, β, and γ are the roll angle, pitch angle, and yaw angle, respectively; X C Y C Z R represents the coordinates of the lidar in the world coordinate system during image acquisition. lc T lc f is the relative extrinsic parameter obtained from the joint calibration of the lidar and camera. x f y Let c be the focal length of the camera along the x-axis and y-axis in the pixel coordinate system. x c y Let x be the coordinates of the camera's optical center in the pixel coordinate system.w y w , z w ) represents the coordinates of a point in the world coordinate system.

[0114] In this embodiment, the mapping model between the three-dimensional laser point cloud and the two-dimensional visual image is as follows: Figure 2 As shown, the calculated radar-camera extrinsic parameter matrix is:

[0115]

[0116] The camera's intrinsic parameter matrix is:

[0117]

[0118] Step 2: Achieve strict time synchronization between the LiDAR and the camera through hardware triggering. After completing the external parameter calibration, synchronously acquire the three-dimensional laser point cloud and the corresponding two-dimensional visual image frames of the construction site.

[0119] In this embodiment, the STM32 microcontroller is designed as an external clock master module, internally configured with a high-precision timer to output high-precision synchronization trigger signals to the LiDAR and industrial camera, achieving time synchronization acquisition. The STM32 establishes a communication channel with the master control platform (NVIDIA Jetson Orin nano in this embodiment) via a USB serial port, sending the corresponding trigger timestamp to the master control in real time each time a trigger event occurs. The master control runs a specially designed timestamp parsing node in the ROS system, accurately associating the received trigger time with the acquired image and point cloud data, assigning each a unified timestamp.

[0120] Step 3: After obtaining the time-synchronized data, perform voxel-level motion consistency detection on the multi-frame point cloud to extract and remove points of dynamic objects; at the image level, extract dynamic pixel regions using optical flow and semantic segmentation. The process for filtering out dynamic objects is as follows: Figure 3 As shown, it specifically includes:

[0121] (3.1) In the point cloud, a voxel-level motion consistency detection method is adopted to divide the point cloud of the current frame and the previous frame into voxels. The point density and centroid offset of the voxel blocks at the same position are compared. If the change exceeds the set threshold, it is marked as a dynamic region. The corresponding point set is removed from the point cloud to remove the dynamic object. The remaining point cloud is filtered for connectivity, and static points connected to the main structure are retained.

[0122] (3.2) At the image end, dynamic regions are detected by combining optical flow estimation and semantic segmentation. The pixel displacement field between consecutive image frames is calculated to identify regions with abrupt displacement changes; dynamic targets in the image are detected using a pre-trained semantic segmentation network, and the optical flow anomaly regions are fused with the semantic segmentation mask to obtain the final dynamic mask.

[0123] In this embodiment, to ensure real-time mapping, the pre-trained semantic segmentation network employs a lightweight YOLO detection model to identify targets as personnel and mainstream construction machinery and equipment.

[0124] Step 4: For the laser point cloud, perform motion distortion compensation based on IMU integration results, perform voxel downsampling to homogenize the spatial distribution, and divide edge points and planar points based on curvature estimation; for the visual image, complete lens distortion correction and contrast equalization, ignore the dynamic pixel regions extracted in Step 3, detect FAST corner points and calculate ORB descriptors, and calculate the area of ​​invalid regions caused by dust or occlusion in the image to obtain the occlusion rate. Specifically, this includes:

[0125] (4.1) Read the IMU angular velocity and acceleration data synchronized with the point cloud time, calculate the relative motion within the scanning cycle through pre-integration, and project each laser point within the scanning cycle back to the radar scanning trigger moment based on linear interpolation to eliminate the point cloud trailing caused by sensor motion.

[0126] (4.2) The distortion-corrected point cloud is downsampled using a uniform voxel grid with a fixed leaf length. Local surface curvature is calculated based on neighborhood PCA:

[0127] Curvature = λ3 / (λ1+λ2+λ3))

[0128] In the formula, λ1 is the distribution variance of the neighborhood point cloud along the main direction (maximum extension direction), λ2 is the distribution variance of the neighborhood point cloud along the secondary direction (extension in the plane perpendicular to the main direction), and λ3 is the distribution variance of the neighborhood point cloud along the normal direction (thickness direction).

[0129] We divide edge points (curvature > 0.1 and λ1 / λ2 ≥ 3.0, λ1 / λ3 ≥ 3.0) into planar points (curvature < 0.05 and λ2 / λ1 ≤ 0.30, λ3 / λ1 ≤ 0.10); at the same time, we remove out-of-range or strong reflection noise points, and combine KD-Tree radius search to accelerate neighborhood calculation.

[0130] (4.3) Based on the offline calibrated camera intrinsic parameters, perform subpixel-level distortion correction on the image acquired in step (2), apply Limit Contrast Adaptive Histogram Equalization (CLAHE), process the image in blocks (32×32 pixels) and limit the local gain to ≤3.0 to suppress overexposure / underexposure caused by dust.

[0131] (4.4) Detect dynamic threshold FAST corner points on the processed image and generate ORB descriptors. Identify invalid regions through threshold segmentation and calculate the occlusion rate occ by statistically analyzing the proportion of invalid pixels.

[0132] Step 5: For the point cloud and image data acquired in Step 2, calculate voxel entropy and angular resolution to measure the degree of geometric degradation of the point cloud; quantify the degree of visual degradation using feature point density, local contrast distribution, and occlusion rate; obtain the point cloud degradation score and the visual degradation score respectively. Specifically, this includes:

[0133] (5.1) Using the downsampled point cloud from (4.2) as input, the point cloud space is divided into 0.1m... 3 Voxel grid, calculate the information entropy H of the point density distribution within each voxel. pc :

[0134]

[0135] In the formula, N represents the total number of voxels in the point cloud spatial partition (voxel size is 0.1m). 3 (Based on the typical object dimensions in the construction scenario), p i Let n be the probability that the number of points within the i-th voxel represents the total number of points in the point cloud. i This represents the number of laser points within the i-th voxel.

[0136] (5.2) Project the point onto the unit sphere, find the nearest neighbor angle between the sampling direction vectors, and take the average to obtain the average angular resolution R. θ .

[0137] (5.3) The voxel entropy H is calculated according to the following formula. PC (point cloud distribution uniformity) and average angular resolution R θ After linear normalization, the orientation is made consistent, and the linear combination results in a single point cloud degradation score D. L :

[0138] D L =αH norm +βR norm

[0139] Here, α and β are two weighting coefficients, determined by offline testing.

[0140] (5.4) Input the image processed in (4.3), and calculate the effective feature point density ρ by the ratio of the number of effective FAST corner points obtained in (4.4) to the total number of pixels in the image. feat .

[0141] (5.5) Calculate the standard deviation of luminance σ within an 8×8 sliding window. l Then, iterate through the image and take the global mean to obtain the local contrast C. img .

[0142] (5.6) The effective feature point density, brightness standard deviation, and occlusion rate (occ) obtained in (4.4) are linearly normalized and their directions are aligned. They are then linearly combined to form a single image degradation score D. V :

[0143] D V =γρ norm +δσ norm +εocc norm

[0144] Where γ, δ, and ε are three weighting coefficients determined by offline testing, and the generated D V ∈[0,1] and point cloud degradation score D L Maintain the same dimensions.

[0145] Step 6: Calculate the original visual weights based on the visual degradation score and point cloud degradation score obtained in Step 5 for each frame. These weights are increased when visual features are significant but the point cloud is degraded, weakened when visual degradation is moderate, and skipped when visual degradation is severe. All weight changes are processed by a first-order low-pass filter to ensure a smooth transition and avoid oscillations in the back-end optimization process. Specifically, this includes:

[0146] (6.1) Based on the visual degradation score D calculated for each frame in step (5) V With point cloud degradation score D L Calculate the uncropped visual weights using the following formula.

[0147]

[0148] Where η is the compensation coefficient for visual degradation of point cloud.

[0149] (6.2) will Clip to interval [λ] min [1], to ensure minimum visual retention

[0150]

[0151] (6.3) If Below the frame skipping threshold λ skip If the number of features in the current image is less than the set minimum value, the visual observation of this frame will be skipped and no residual factor will be generated. A maximum of 4 consecutive frame skips are allowed. After exceeding this limit, one frame of visual observation will be forcibly retained to reconstruct the constraints.

[0152] (6.4) Apply a first-order low-pass filter to the weights after each frame cropping to suppress weight abrupt changes caused by degradation score fluctuations and reduce nonlinear optimization oscillations:

[0153]

[0154] Here, α is the filtering coefficient, which is typically between 0.8 and 0.95. Considering environmental factors, α is set to 0.8 here.

[0155] Step 7: Within the incremental Kalman sequential update framework, state prediction is performed using the IMU pre-integration factor. For each LiDAR point-area residual factor, sequential linearization and Kalman updates are performed. For the visual reprojection residual factors of adaptive frame selection, sequential updates are also performed. All updates are accumulated in the current state and covariance. When the number of frames exceeds the sliding window depth, Schur complement marginalization is performed on the earliest frame to generate priors and remove old frames, maintaining historical constraints and scale constrained. Figure 4 As shown, it specifically includes:

[0156] (7.1) Let the state vector of the i-th keyframe within the sliding window be:

[0157]

[0158] Among them, R i t represents the three-dimensional rotation of the camera (or lidar) coordinate system relative to the world coordinate system in the i-th frame; i v represents the position coordinates of the camera in the world coordinate system in the i-th frame; i This represents the linear velocity vector of the camera relative to the world coordinate system at frame i. This indicates the zero bias of the IMU gyroscope (angular velocity sensor); This represents the zero bias of the IMU accelerometer. The keyframe sliding window has a fixed length of N; edge clearing is performed when the window exceeds this length. The window length is also fixed at 5 bits to match the device's computing performance.

[0159] (7.2) Using the raw angular velocity and acceleration of the IMU between adjacent keyframes, the rotation, velocity and displacement increments are obtained through pre-integration. Following the Kalman prediction steps, the incremental motion model is used to evaluate the state X of the previous frame. i-1 Perform state propagation, synchronously linearize the Jacobian, update the covariance matrix, and form a prior estimate for the i-th frame.

[0160] (7.3) For each point cloud feature point p selected in (4.2) k Fit plane parameters to the local map, transform the point from the current frame coordinate system to the world coordinate system, and calculate the distance to its corresponding plane as the residual using the following formula:

[0161]

[0162] Where, p k R represents the coordinates of the k-th LiDAR feature point in the radar coordinate system of the current frame. iWith t i These are the rotation matrix and translation vector of the nth frame, respectively; k The unit normal vector corresponding to the planar feature is obtained by fitting the neighborhood point set of the local map using PCA; d k The offset of the plane equation defines the specific position of the plane in the world coordinate system.

[0163] Perform first-order linearization on the point cloud residuals and calculate their relationship with respect to the state variable X. i Jacobian matrix:

[0164]

[0165] The gain matrix is ​​calculated using the classic Kalman measurement update formula, the state increment is updated and the covariance is corrected, and each LiDAR geometric factor is integrated into the state estimate one by one as it arrives.

[0166] (7.4) For the m-th map point in the i-th frame Calculate the reprojection error:

[0167]

[0168] Where z im =[u im v im ] T , where is the pixel coordinate of the m-th feature point actually observed in the i-th frame; For the predicted pixel position, Let m be the m-th 3D map point in the world coordinate system; [R i , t i ] is the extrinsic parameter of the i-th frame; K is the camera intrinsic parameter matrix; (·) represents the perspective projection operation.

[0169] The visual residual is linearized to the first order, and its relationship with the state variable X is calculated. i Jacobian matrix:

[0170]

[0171] Introducing the visual weights calculated using smoothing in (6) Standard visual noise covariance R v Scaling is performed to dynamically adjust the weight of visual information in backend optimization:

[0172]

[0173] The algorithm uses the classic Kalman measurement update formula to calculate the gain matrix, update the state increment and correct the covariance. After predicting IMU and LiDAR updates, the algorithm injects visual information with dynamic weights in sequence.

[0174] (7.5) Integrate the IMU pre-integration residual, the LiDAR geometric residual, and the weighted visual reprojection residual into a weighted least squares objective function:

[0175]

[0176] Among them, X i r is the state vector of the i-th frame; imu，ij W is the IMU pre-integration residual from frame i to frame j. imu Its covariance; r l，k Let w be the weight of the k-th LiDAR geometric residual. l Reflects LiDAR noise level; r v，im For the m-th visual reprojection residual of the i-th frame, For adaptive visual weights, this function applies a corresponding covariance weight matrix or scalar to each residual term and solves the problem using a unified factor graph approach, achieving tight coupling between IMU, LiDAR, and vision.

[0177] (7.6) In each optimization iteration, the algorithm estimates X based on the current state. i As the expansion point, recalculate all residual terms and their Jacobian matrices. Before each linearization, update the visual factor weights based on the latest sensor degradation. Incremental Gauss-Newton linearization and updates are performed on LiDAR and visual factors.

[0178] (7.7) When the number of sliding window frames exceeds the preset length of 5, the oldest frame needs to be removed and its information retained as prior information to prevent the loss of historical constraints.

[0179] Step 8: After optimization, publish high-precision pose estimation and dense color point cloud map; when multiple consecutive frames are detected to be in severe degradation and the tracking confidence is below the threshold, trigger automatic relocalization.

[0180] (8.1) After the optimization iteration converges, rotate and translate the current keyframe by R. i , t i Linear velocity v i and IMU bias The final estimation result is published to downstream modules via ROS TF and callback interface; the visual degradation score D obtained in this optimization is used. V LiDAR Degradation Score D L and final visual weight Feedback is sent to the front-end filtering and weight calculation modules for data preprocessing and adaptive weight updates in the next frame.

[0181] (8.2) Based on the current pose, fuse the static point cloud of the current frame with the corresponding RGB image projection to obtain a colored point cloud fragment, and incrementally fuse the fragment into the global map.

[0182] (8.3) Maintain an observation sliding window of length M, and accumulate statistics on the severe degradation flag and tracking confidence score for each frame; when the number of "severely degraded" frames in the window continuously exceeds a preset threshold, and the average tracking confidence score is lower than the threshold, tracking instability is identified, and relocation is required. In this embodiment, M is set to 7. If the system has more than 4 frames of poor quality within approximately 7 consecutive frames, and the overall tracking score is low, it can be identified as unstable, triggering relocation.

[0183] (8.4) After the relocation condition is triggered, the regular sliding window update is paused. Several potential candidate poses are quickly retrieved from the previously saved keyframe library using visual and LiDAR features to generate a preliminary pose guess list.

[0184] (8.5) For each candidate initial pose, load the corresponding map points and the current frame observation data, and perform a local optimization: perform incremental Gauss-Newton iteration on the visual reprojection residual and LiDAR point-area residual to fine-tune the candidate pose; verify the overall residual and tracking frame number after optimization, and select the candidate with the smallest residual and the highest tracking quality as the final relocalization result.

[0185] (8.6) After selecting the best relocation pose, replace the earliest frame state in the sliding window with the pose, reinitialize the sliding window based on the new pose, resume the IMU prediction and sequential measurement update process, and exit the relocation mode.

[0186] (8.7) After successful relocalization, the new degradation score, visual weight and relocalization status are fed back to the front-end dynamic filtering and weight calculation module to ensure that the preprocessing and weight adaptation of the next frame are based on the latest global reference, thereby realizing the transition from relocalization to normal fusion in extreme degradation scenarios.

[0187] This invention also provides a multimodal adaptive weighted SLAM system for construction sites, comprising:

[0188] The calibration module is used to jointly calibrate the lidar and camera, outputting the rotation and translation matrix between the image and point cloud acquired by the sensor with a fixed relative position, and establishing a mapping model between the three-dimensional lidar point cloud and the two-dimensional visual image.

[0189] The synchronization module uses an external STM32 as a hardware trigger unit to drive the LiDAR, camera, and IMU with hardware trigger signals to achieve precise clock synchronization and send the synchronized raw data to each preprocessing module.

[0190] The dynamic target filtering module removes moving object points based on multi-frame voxel consistency and inter-scan isolation detection in the point cloud; and filters out dynamic pixels by fusing optical flow and deep learning instance segmentation dynamic masks in the vision end.

[0191] The point cloud preprocessing module performs back projection based on the IMU pre-integration at the acquisition time of each laser point, performs motion distortion compensation for each laser point, and then extracts edge points and planar points on the compensated point cloud and removes noise.

[0192] The image preprocessing module performs subpixel-level distortion correction and CLAHE contrast equalization on each frame of the image, detects FAST corner points with dynamic thresholds in blocks and generates ORB descriptors, calculates the proportion of invalid pixel areas caused by dust or occlusion, and outputs the image occlusion rate to provide visual indicators for degradation detection.

[0193] The degradation detection module calculates the voxel entropy and average angular resolution for each frame to quantify the degree of geometric degradation of LiDAR. L The visual degradation score D is calculated for the image based on the effective feature point density, local contrast, and occlusion rate. V .

[0194] Adaptive weight calculation module, integrating D L With D V Calculate the original visual weights After cropping, smooth weights are generated using a first-order low-pass filter. The system automatically determines whether to skip frames based on a threshold.

[0195] The sliding window optimization module uses IMU pre-integration factors to perform state prediction and covariance propagation on the latest frame; it linearizes each LiDAR point-area residual factor in the order of arrival and performs Kalman measurement update; it sequentially applies weighted Kalman update to the visual reprojection residual factors of adaptive frame selection; when the number of key frames exceeds the window depth N, the module generates a priori for the oldest frame and removes old frames.

[0196] The map building and publishing module accepts the optimal pose obtained after sliding window optimization, fuses static point cloud with color image information to generate a high-precision dense color point cloud map, and outputs the real-time mapping results to downstream systems through a visualization interface or message publishing mechanism.

[0197] The relocalization module continuously monitors the degradation score and tracking confidence of multiple consecutive frames. Once severe degradation is detected and the confidence falls below the set threshold, it triggers global subgraph or loop closure detection and restarts the localization process.

[0198] The system may also include a cloud synchronization module, which caches the latest N frames of key data locally and uses a 5G network to upload the compressed ROS topic message stream to the cloud server, enabling real-time cloud backup of collected data, poses and maps, suitable for multi-terminal collaborative mapping.

[0199] In addition, the system may include a construction progress verification module. After the system completes the construction of a 3D point cloud map of the construction site, it aligns the real-time acquired site point cloud to the project BIM model coordinate system. It then uses the component list and classification numbers provided by the BIM model to perform component-level matching of the point cloud. For units not detected in the point cloud or areas with coverage below a threshold, the module marks them as "incomplete" or "pending verification." Subsequently, the system calculates the completed percentage based on component volume, area, or number of nodes, providing a quantitative assessment of the actual construction progress.

[0200] The validation of this invention on the Newton cusp dataset is as follows: Figure 5 As shown, a high-quality colored point cloud was obtained.

[0201] It should be understood that the embodiments and descriptions above are only the principles, main features and advantages of the present invention. Various changes and modifications can be made to the present invention without departing from the spirit and scope of the invention, and all such changes and modifications fall within the protection scope of the present invention.

Claims

1. A multimodal adaptive weighted SLAM method for construction sites, characterized in that, Includes the following steps: (1) Perform joint calibration of lidar and camera to establish a mapping model between three-dimensional lidar point cloud and two-dimensional visual image; (2) The time synchronization between the lidar and the camera is achieved by hardware triggering. After the external parameter calibration is completed, the three-dimensional laser point cloud and the corresponding two-dimensional visual image frame of the construction site are collected synchronously. (3) After obtaining the time-synchronized data, voxel-level motion consistency detection is performed on the multi-frame point cloud in the point cloud to extract and remove the points of dynamic objects; dynamic pixel regions are extracted at the image end using optical flow and semantic segmentation. (4) For the laser point cloud, motion distortion compensation is performed by combining the IMU integration results, voxel downsampling is performed to homogenize the spatial distribution, and edge points and planar points are divided based on curvature estimation. For visual images, lens distortion correction and contrast equalization are completed, the dynamic pixel regions extracted in step (3) are ignored, FAST corner points are detected and ORB descriptors are calculated, and the area of ​​invalid regions caused by dust or occlusion in the image is counted to obtain the occlusion rate. (5) For the point cloud and image data collected in step (2) in the same frame, calculate the voxel entropy and angular resolution to measure the degree of geometric degradation of the point cloud; quantify the degree of visual degradation by feature point density, local contrast distribution and occlusion rate. Point cloud degradation score and visual degradation score were obtained respectively; (6) Calculate the original visual weight based on the visual degradation score and point cloud degradation score of each frame obtained in (5). The weight is increased when the visual features are obvious but the point cloud is degraded, weakened when the visual degradation is moderate, and skipped when the visual degradation is severe. (7) Within the incremental Kalman sequential update framework, state prediction is performed using the IMU pre-integration factor. The LiDAR point-area residual factor is linearized and Kalman updated sequentially for each LiDAR point-area residual factor. The visual reprojection residual factor of adaptive frame selection is updated sequentially. All updates are accumulated into the current state and covariance. When the number of frames exceeds the sliding window depth, Schur marginalization is performed on the earliest frame to generate priors and remove old frames, keeping the historical constraints and scale constant. (8) After optimization, release high-precision pose estimation and dense color point cloud map; when multiple consecutive frames are detected to be in severe degradation and the tracking confidence is lower than the threshold, trigger automatic relocalization.

2. The multimodal adaptive weighted SLAM method for construction sites according to claim 1, characterized in that, The mapping model between the three-dimensional laser point cloud and the two-dimensional visual image mentioned in step (1) is as follows: In the formula, (u, v) are the pixel coordinates in the pixel coordinate system; R l T l Let C be the external parameters of the lidar in the world coordinate system, where α, β, and γ are the roll angle, pitch angle, and yaw angle, respectively; X C Y C Z R represents the coordinates of the lidar in the world coordinate system during image acquisition. lc T lc f is the relative extrinsic parameter obtained from the joint calibration of the lidar and camera. x f y Let c be the focal length of the camera along the x-axis and y-axis in the pixel coordinate system. x c y Let x be the coordinates of the camera's optical center in the pixel coordinate system. w y w , z w ) represents the coordinates of a point in the world coordinate system.

3. The multimodal adaptive weighted SLAM method for construction sites according to claim 1, characterized in that, The implementation process of step (3) is as follows: (3.1) In the point cloud, the current frame and the previous frame point cloud are divided into voxels. The point density and centroid offset of the voxel blocks at the same position are compared. If the change exceeds the set threshold, it is marked as a dynamic region. The corresponding point set is removed from the point cloud to remove the dynamic object. The remaining point cloud is filtered for connectivity, and static points connected to the main structure are retained. (3.2) At the image end, calculate the pixel displacement field between consecutive image frames and identify regions with abrupt displacement changes; use a pre-trained semantic segmentation network to detect dynamic targets in the image, fuse the optical flow abnormal region with the semantic segmentation mask, and obtain the final dynamic mask.

4. The multimodal adaptive weighted SLAM method for construction sites according to claim 1, characterized in that, The implementation process of step (4) is as follows: (4.1) Read the IMU angular velocity and acceleration data synchronized with the point cloud time, calculate the relative motion within the scanning cycle through pre-integration, and project each laser point within the scanning cycle back to the radar scanning trigger time based on linear interpolation to eliminate the point cloud trailing caused by sensor motion. (4.2) The distortion-corrected point cloud is downsampled using a uniform voxel grid with a fixed leaf length; the local surface curvature is calculated based on neighborhood PCA: Curvature = λ3 / (λ1+λ2+λ3)) In the formula, λ1 is the distribution variance of the neighborhood point cloud along the main direction, λ2 is the distribution variance of the neighborhood point cloud along the secondary direction, and λ3 is the distribution variance of the neighborhood point cloud along the normal direction; the edge points and planar points are divided; at the same time, points with long distance or strong reflection noise are removed, and KD-Tree radius search is combined to accelerate the neighborhood calculation. (4.3) Based on the offline calibrated camera intrinsic parameters, perform subpixel-level distortion correction on the image acquired in step (2), apply contrast-limited adaptive histogram equalization, process the image in blocks and limit local gain to suppress overexposure or underexposure caused by dust. (4.4) Detect dynamic threshold FAST corner points on the processed image and generate ORB descriptors; identify unusable areas by setting grayscale thresholds; The occlusion rate (occupancy rate) is calculated by counting the proportion of pixels in these unusable areas to the total number of pixels.

5. The multimodal adaptive weighted SLAM method for construction sites according to claim 4, characterized in that, The curvature of the edge point is greater than 0.1 and λ1 / λ2≥3.0, λ1 / λ3≥3.0; the curvature of the plane point is less than 0.05 and λ2 / λ1≤0.30, λ3 / λ1≤0.

10.

6. The multimodal adaptive weighted SLAM method for construction sites according to claim 1, characterized in that, The implementation process of step (5) is as follows: (5.1) Using the downsampled point cloud from step (4.2) as input, the point cloud space is divided into a voxel grid, and the information entropy H of the point density distribution within each voxel is calculated. pc : In the formula, N represents the total number of voxels in the point cloud spatial partition, and p i Let n be the probability that the number of points within the i-th voxel represents the total number of points in the point cloud. i This represents the number of laser points within the i-th voxel; (5.2) Project the point onto the unit sphere, find the nearest neighbor angle between the sampling direction vectors, and take the average to obtain the average angular resolution R. θ ; (5.3) The voxel entropy H PC and average angular resolution R θ After linear normalization, the orientation is made consistent, and the linear combination results in a single point cloud degradation score D. L : D L =αH norm +βR norm Here, α and β are two weighting coefficients, determined by offline testing; (5.4) Input the image processed in step (4.3), and calculate the effective feature point density ρ by the ratio of the number of effective FAST corner points obtained in (4.4) to the total number of pixels in the image. feat ; (5.5) Calculate the standard deviation of luminance σ within an 8×8 sliding window. l Then, iterate through the image and take the global mean to obtain the local contrast C. img ; (5.6) The effective feature point density, brightness standard deviation, and occlusion rate (occ) obtained in (4.4) are linearly normalized and their directions are aligned. They are then linearly combined to form a single image degradation score D. V : D V =g norm +ds norm +εocc norm Where γ, δ, and ε are three weighting coefficients determined by offline testing, and the generated D V ∈[0,1] and point cloud degradation score D L Maintain the same dimensions.

7. The multimodal adaptive weighted SLAM method for construction sites according to claim 1, characterized in that, The implementation process of step (6) is as follows: (6.1) Based on the visual degradation score D calculated for each frame in step (5) V With point cloud degradation score D L Calculate the uncropped visual weights using the following formula. Where η is the compensation coefficient for visual degradation of point cloud; (6.2) will Clip to interval [λ] min [1], ensuring minimum visual retention (6.3) If Below the frame skipping threshold λ skip If the number of features in the current image is less than the set minimum value, the visual observation of this frame will be skipped and no residual factor will be generated. A maximum of 4 consecutive frame skips are allowed. After exceeding this limit, one frame of visual observation will be forcibly retained to reconstruct the constraints. (6.4) Apply a first-order low-pass filter to the weights after each frame cropping to suppress weight abrupt changes caused by degradation score fluctuations and reduce nonlinear optimization oscillations: Where α is the filter coefficient.

8. The multimodal adaptive weighted SLAM method for construction sites according to claim 1, characterized in that, The implementation process of step (7) is as follows: (7.1) Let the state vector of the i-th keyframe within the sliding window be: Among them, R i t represents the three-dimensional rotation of the camera or lidar coordinate system relative to the world coordinate system in the i-th frame; i v represents the position coordinates of the camera in the world coordinate system in the i-th frame; i This represents the linear velocity vector of the camera relative to the world coordinate system at frame i. This indicates the zero bias of the IMU angular velocity sensor; This represents the zero bias of the IMU accelerometer; the keyframe sliding window has a fixed length of N, and edge detection is performed when it exceeds the limit. (7.2) Using the raw angular velocity and acceleration of the IMU between adjacent keyframes, the rotation, velocity and displacement increments are obtained through pre-integration. The incremental motion model is then used to recalculate the state X of the previous frame according to Kalman prediction. i-1 Perform state propagation, synchronously linearize the Jacobian, update the covariance matrix, and form a prior estimate for the i-th frame; (7.3) For each point cloud feature point p k Fit plane parameters to the local map, transform the point from the current frame coordinate system to the world coordinate system, and calculate the distance to its corresponding plane as the residual using the following formula: Where, p k R represents the coordinates of the k-th LiDAR feature point in the radar coordinate system of the current frame. i With t i These are the rotation matrix and translation vector for the i-th frame, respectively; n k The unit normal vector corresponding to the planar feature is obtained by fitting the neighborhood point set of the local map using PCA; d k The offset of the plane equation defines the specific position of the plane in the world coordinate system; Perform first-order linearization on the point cloud residuals and calculate their relationship with respect to the state variable X. i Jacobian matrix: The gain matrix is ​​calculated using the classic Kalman measurement update formula, the state increment is updated and the covariance is corrected, and each LiDAR geometric factor is fused into the state estimate one by one as it arrives. (7.4) For the m-th map point in the i-th frame Calculate the reprojection error: Where z im =[u im ,v im ] T , where is the pixel coordinate of the m-th feature point actually observed in the i-th frame; For the predicted pixel position, Let m be the m-th 3D map point in the world coordinate system; [R i , t i [] represents the extrinsic parameter of the i-th frame; K is the camera intrinsic parameter matrix; (·) represents the perspective projection operation; The visual residual is linearized to the first order, and its relationship with the state variable X is calculated. i Jacobian matrix: Introducing visual weights Standard visual noise covariance R v Scaling is performed to dynamically adjust the weight of visual information in backend optimization: The algorithm uses the classic Kalman measurement update formula to calculate the gain matrix, update the state increment and correct the covariance. After predicting the IMU and LiDAR updates, the algorithm injects visual information with dynamic weights in sequence. (7.5) Integrate the IMU pre-integration residual, the LiDAR geometric residual, and the weighted visual reprojection residual into a weighted least squares objective function: Among them, X i r is the state vector of the i-th frame; imu，ij W is the IMU pre-integration residual from frame i to frame j. imu Its covariance; r l,k Let w be the weight of the k-th LiDAR geometric residual. l Reflects LiDAR noise level; r v,im For the m-th visual reprojection residual of the i-th frame, To achieve adaptive visual weights, corresponding covariance weight matrices or scalars are applied to all residual terms, and a unified factor graph approach is used for solving, thus realizing tight coupling between IMU, LiDAR, and vision. (7.6) In each optimization iteration, the algorithm estimates X based on the current state. i As the expansion point, recalculate all residual terms and their Jacobian matrices; before each linearization, update the visual factor weights based on the latest sensor degradation. Incremental Gauss-Newton linearization and updates are performed on LiDAR and visual factors; (7.7) When the number of sliding window frames exceeds the preset length N, the oldest frame is removed and its information is retained as prior information to prevent the loss of historical constraints.

9. The multimodal adaptive weighted SLAM method for construction sites according to claim 1, characterized in that, The implementation process of step (8) is as follows: (8.1) After the optimization iteration converges, rotate and translate the current keyframe by R. i ,t i Linear velocity v i and IMU bias The final estimation result is published to downstream modules via ROS TF and callback interface; the visual degradation score D obtained in this optimization is used. V LiDAR Degradation Score D L and final visual weight The data is fed back to the front-end filtering and weight calculation module for data preprocessing and adaptive weight updates in the next frame. (8.2) Based on the current pose, fuse the static point cloud of the current frame with the corresponding RGB image projection to obtain a color point cloud fragment, and incrementally fuse the fragment into the global map; ( 8.3) Maintain an observation sliding window of length and perform cumulative statistics on the severe degradation flag and tracking confidence score for each frame; When the number of "severely degraded" frames in the window exceeds the preset threshold continuously, and the average tracking confidence is lower than the threshold, the tracking is considered unstable and relocation is required. (8.4) After the relocation condition is triggered, the regular sliding window update is paused. Several potential candidate poses are quickly retrieved from the previously saved keyframe library using visual and LiDAR features to generate a preliminary pose guess list. (8.5) For each candidate initial pose, load the corresponding map points and the current frame observation data, and perform a local optimization: perform incremental Gauss-Newton iteration on the visual reprojection residual and LiDAR point-area residual to fine-tune the candidate pose; verify the overall residual and tracking frame number after optimization, and select the candidate with the smallest residual and the highest tracking quality as the final relocalization result. (8.6) After selecting the best relocation pose, replace the earliest frame state in the sliding window with the pose, reinitialize the sliding window based on the new pose, resume the IMU prediction and sequential measurement update process, and exit the relocation mode. (8.7) After successful relocalization, the new degradation score, visual weight and relocalization status are fed back to the front-end dynamic filtering and weight calculation module to ensure that the preprocessing and weight adaptation of the next frame are based on the latest global reference, thereby realizing the transition from relocalization to normal fusion in extreme degradation scenarios.

10. A multimodal adaptive weighted SLAM system for construction sites, employing the method described in any one of claims 1 to 9, comprising the following modules: The calibration module is used to jointly calibrate the lidar and camera, outputting the rotation and translation matrix between the image and point cloud acquired by the sensor with a fixed relative position, and establishing a mapping model between the three-dimensional lidar point cloud and the two-dimensional visual image. The synchronization module uses an external STM32 as a hardware trigger unit to drive the LiDAR, camera and IMU with hardware trigger signals to achieve precise clock synchronization and send the synchronized raw data to each preprocessing module. The dynamic target filtering module removes moving object points based on multi-frame voxel consistency and inter-scan isolation detection in the point cloud; and filters out dynamic pixels by fusing optical flow and deep learning instance segmentation in the vision end. The point cloud preprocessing module performs back projection based on the IMU pre-integration at the acquisition time of each laser point, performs motion distortion compensation for each laser point, and then extracts edge points and planar points on the compensated point cloud and removes noise. The image preprocessing module performs subpixel-level distortion correction and CLAHE contrast equalization on each frame of the image, detects FAST corner points with dynamic thresholds in blocks and generates ORB descriptors, calculates the proportion of invalid pixel areas caused by dust or occlusion, and outputs the image occlusion rate to provide visual indicators for degradation detection. The degradation detection module calculates the voxel entropy and average angular resolution of each frame to quantify the degree of geometric degradation of LiDAR, and calculates the visual degradation score of the image based on the effective feature point density, local contrast and occlusion rate. The adaptive weight calculation module combines the degree of geometric degradation and the visual degradation score to calculate the original visual weights. After cropping, it generates smooth weights through a first-order low-pass filter and automatically determines whether to skip frames based on the threshold. The sliding window optimization module uses the IMU pre-integration factor to perform state prediction and covariance propagation on the latest frame; it linearizes each LiDAR point-area residual factor in the order of arrival and performs Kalman measurement update; it applies weighted Kalman update to the visual reprojection residual factor of the adaptive frame selection in sequence; when the number of key frames exceeds the window depth, it generates a prior for the oldest frame and removes the old frame. The map building and publishing module accepts the optimal pose obtained after sliding window optimization, fuses static point cloud with color image information to generate a high-precision dense color point cloud map, and outputs real-time mapping results to downstream systems through a visualization interface or message publishing mechanism. The relocalization module continuously monitors the degradation score and tracking confidence across multiple consecutive frames. When severe degradation is detected and the confidence score falls below a set threshold, it triggers global subgraph or loop closure detection and restarts the localization process.

Images