A self-adaptive weighted positioning method based on BIM multi-sensor fusion and electronic equipment

Abstract

The present application relates to the positioning and navigation technical field of steel bar binding robot, specifically to a kind of adaptive weighted positioning method based on BIM multi-sensor fusion and electronic equipment, comprising the following steps: S1, obtains two-dimensional grid map;S2, data acquisition;S3, laser data processing;S4, laser point cloud screening and transmission;S5, generate error term;S6, calculate sensor weight;S7, smooth weight change;S8, preliminary positioning;S9, optimization positioning result;The BIM model of the building is exported and two-dimensional grid map is generated, the map is imported into ROS as the environment reference of laser radar positioning, the positioning accuracy of the robot is improved;The present application combines a variety of sensors to construct a multi-sensor fusion model, dynamically evaluates the real-time data quality of each sensor to adjust the data weight of each sensor, effectively improves the positioning accuracy and stability of the present application in complex environment changes, expands the use range of steel bar binding robot, and has broad application prospect.

Description

TECHNICAL FIELD

[0001] The present application relates to the positioning and navigation technology field of steel bar binding robots, in particular to a self-adaptive weighted positioning method based on BIM multi-sensor fusion and an electronic device. BACKGROUND

[0002] In the construction industry, accurate positioning is crucial for steel bar binding robots to perform tasks; traditional positioning methods usually rely on a single sensor, such as an inertial measurement unit (IMU), a laser radar, etc., however, with the rapid development of technology, the positioning accuracy using a single sensor is easily affected by environmental interference or its own limitations, resulting in positioning error accumulation, signal blocking, and reliability problems caused by environmental changes; for example, laser radar is prone to measurement errors when encountering glass or reflective surfaces, visual sensors are prone to failure in low light or complex visual environments, and wheel speed odometry can cause position estimation errors when the wheels slip; in order to obtain a more accurate and stable positioning system, the prior art provides a multi-sensor fusion positioning method that fuses the pose information obtained by IMU, laser radar, etc. to determine the position of the robot.

[0003] The existing multi-sensor fusion positioning method usually relies on a pre-constructed map for positioning, but in the indoor environment of a building, the accuracy and update of the map are often insufficient to cope with complex building structures; with the widespread application of building information modeling (BIM), BIM models, as a digital expression of building information, can provide accurate building geometry and topology information, providing a new data source for indoor positioning; if BIM is combined with the multi-sensor fusion positioning method, the accuracy and update deficiencies of traditional multi-sensor fusion positioning methods in the indoor environment of a building can be addressed. SUMMARY

[0004] The purpose of the present application is to provide a self-adaptive weighted positioning method based on BIM multi-sensor fusion that can effectively improve the positioning accuracy and stability of steel bar binding robots in dynamic construction environments.

[0005] To achieve the above purpose, the present application adopts the following technical solutions:

[0006] A self-adaptive weighted positioning method based on BIM multi-sensor fusion, comprising the following steps:

[0007] S1, obtaining a two-dimensional grid map containing building frame structure and steel bar arrangement information;

[0008] S2, collecting data: collecting real-time data of an inertial measurement unit (IMU), a laser radar, a visual sensor, and a wheel speed odometry, and synchronizing the time stamps;

[0009] S3, laser data processing: based on the data information collected in step S2, coordinate transformation is performed; the initial pose change is calculated using IMU and wheel odometry, and the point cloud data of the laser radar is corrected for distortion and noise is removed;

[0010] S4, laser point cloud screening and transmission: determine whether the robot is currently moving in a longitudinal or lateral mode, select the laser radar point cloud data within a specific angle range according to different motion types, remove invalid or redundant point cloud information, and transmit the point cloud data to the system in real time for environment matching in the Cartographer algorithm;

[0011] S5, generate error term: evaluate the quality of real-time data of each sensor and generate respective error terms;

[0012] S6, calculate sensor weight: regularize the error terms to calculate the initial weight of each sensor data;

[0013] S7, smooth weight change: smooth the weight change according to the exponential weighted moving average method;

[0014] S8, preliminary positioning: weight and fuse the data of each sensor according to the adaptive weight to generate a preliminary positioning result;

[0015] S9, optimize positioning results: use extended Kalman filter to optimize the preliminary positioning results to obtain the final positioning results.

[0016] Preferably, the specific process of step S3 laser data processing includes:

[0017] Convert the point cloud data from the laser radar coordinate system to the global coordinate system through coordinate transformation to ensure consistency with the two-dimensional grid map coordinates;

[0018] Reduce the amount of point cloud data by voxel grid filtering, divide the point cloud data into fixed size voxels, and replace each voxel with the center of gravity of all points in the voxel to reduce the amount of point cloud data;

[0019] Remove outliers by statistical filtering, calculate the number of neighborhood points for each point, and remove points with a neighborhood point number less than a set threshold;

[0020] Remove noise points by radius filtering based on the number of neighborhood points within a radius range;

[0021] Perform height filtering to filter out points with heights outside a reasonable range, such as ground points or points with abnormal heights;

[0022] Perform conditional filtering to remove points that do not meet the conditions.

[0023] The IMU provides angular velocity and acceleration information, and the attitude change is obtained by integration:

[0024]

[0025] Where θ(t) is the attitude at time t, and ω(t) is the angular velocity;

[0026] Wheel speed odometry provides information about the robot's position changes on a plane. Using this pose change information, it calculates the robot's pose (translation and rotation) changes relative to its initial position.

[0027] Δx=υ x ·Δt, Δy=υ y ·Δt

[0028] Where Δx and Δy are the displacements in the x and y directions, respectively, and υ x and υ y It is the robot's velocity component;

[0029] The estimated initial pose information is applied to laser point cloud distortion correction:

[0030] p corr =R(θ)·p raw +t

[0031] Where, p corr For the corrected point cloud, p raw R(θ) represents the original point cloud, R(θ) is the rotation matrix, and t is the pose translation.

[0032] Preferably, the specific process of laser point cloud screening and transmission in step S4 includes:

[0033] The scanning angle of the LiDAR is dynamically adjusted according to the robot's current motion type to ensure that the collected point cloud data covers the key areas of the robot's forward path.

[0034] Subscribe to LiDAR data and convert it to Cartesian coordinate system data;

[0035] Subscribe to odometry data and calculate the direction of movement based on position changes over a period of time. If the robot mainly moves along the X-axis, it is considered to be moving longitudinally; if it mainly moves along the Y-axis, it is considered to be moving laterally.

[0036] During longitudinal movement, point cloud data points within a 90-degree range (45° to 135° and 225° to 315°) in front of and behind the LiDAR are prioritized. During lateral movement, point cloud data points within a 90-degree range (315° to 45° and 135° to 225°) to the left and right of the LiDAR are prioritized. The preprocessed and corrected point cloud data is input into the Cartographer algorithm, matched with a static 2D grid map, to calculate the robot's pose in the global coordinate system.

[0037] Preferably, the specific process of generating the error term in step S5 includes:

[0038] The noise level of the IMU gyroscope output is evaluated, and the linearity, bias, and scale factor error of the IMU accelerometer under different motion states are measured to generate error terms.

[0039] The positioning accuracy is evaluated by analyzing the density and consistency of the point cloud data acquired by the lidar, and the measurement error is estimated by statistically analyzing the noise level in the lidar point cloud, thus generating an error term.

[0040] Visual sensor error terms are generated based on multiple image quality metrics;

[0041] The wheel speed odometer error term is generated by comparing the difference between the expected travel distance and the actual travel distance. If slippage is detected, the error term takes a very large value. The speed calculated by the wheel speed odometer is compared with the speed provided by the IMU. If the difference exceeds a preset threshold, slippage is considered to have occurred.

[0042] Preferably, the specific process of calculating the sensor weights in step S6 includes:

[0043] Regularization of error terms: To ensure that error terms are compared on the same scale, each error term is normalized.

[0044]

[0045] Where max(E) and min(E) are the minimum and maximum values ​​of all sensor error terms, respectively. Let the regularized error terms be denoted as ; and the regularized error terms be denoted as .

[0046] Preferably, the initial weights are calculated based on the regularized error term:

[0047]

[0048] Among them, W i This represents the initial weight of each error term. Let W represent the sum of all regularized error terms; the initial weights of each error term are denoted as W.IMU W Lidar W Vision and W Odometry .

[0049] Preferably, the specific process of smoothing the weight changes in step S7 includes:

[0050] The exponentially weighted moving average (EWMA) method is used to smooth out weight changes in order to reduce drastic weight fluctuations caused by sensor error variations.

[0051] W i (t+1) =αW i (t) +(1-α)W i (t-1)

[0052] Where α is the smoothing coefficient, and its value ranges from 0 to 1.

[0053] Preferably, the specific process of preliminary positioning in step S8 includes:

[0054] The data from each sensor are weighted and fused according to adaptive weights to generate preliminary localization results:

[0055]

[0056] in, For the initial pose estimation after multi-sensor data fusion, P i This represents the data from the i-th sensor.

[0057] Preferably, the specific process of optimizing the positioning results in step S9 includes:

[0058] Define grid cells: Cells represent the spacing and spatial location of reinforcing bars at millimeter-level resolution;

[0059] Obstacle marking: Treating columns, beams, etc., as obstacles;

[0060] Reinforcing bar passable area: Mark the area for tying operations;

[0061] The two-dimensional raster map can be represented as:

[0062]

[0063] S91. Initialize the state vector, covariance matrix, and noise covariance:

[0064] State vector: Set the initial state vector X0, which contains preliminary positioning results, such as position and velocity information;

[0065] X0 = [x, y, θ, υ, ω] T

[0066] Where x and y are the robot's position coordinates, θ is the attitude angle, υ is the linear velocity, and ω is the angular velocity; these are taken from the initial positioning results during initialization.

[0067] Covariance matrix: The initial covariance matrix P0 represents the uncertainty of the initial state. The variances of each parameter can be estimated based on the system characteristics.

[0068]

[0069] The process noise covariance matrix and the measurement noise covariance matrix are set according to the noise type respectively:

[0070] Process noise covariance matrix Q: Considering the noise characteristics of robot motion, such as the measurement errors of wheel speed odometer and IMU;

[0071] Measurement noise covariance matrix R: reflects the measurement accuracy and noise level of lidar point clouds, vision sensors, etc.

[0072] S92. Perform state prediction and covariance prediction:

[0073] State prediction: Using the system's state transition model f, predict the state at the next moment.

[0074]

[0075] in, It is the predicted state at time k, u k-1 At time k-1, the control input linear velocity is ω. k-1 At time k-1, the control input angular velocity is θ. k-1 It is the heading angle of the previous state (relative to the x-axis), x k-1 and y k-1 Cos(θ) represents the position of the previous state in the plane. k-1 ) and sin(θ k-1 These are the x-component and y-component of the heading angle, respectively.

[0076] Covariance prediction: Update the covariance matrix using the Jacobian matrix F:

[0077]

[0078] in, is the Jacobian matrix of the state transition model, and Q is the covariance matrix of the process noise;

[0079] S93, Update Phase:

[0080] Calculate the Kalman gain: Using the prediction covariance matrix and the Jacobian matrix H of the measurement model, calculate the Kalman gain K.k :

[0081] K k =P k|k-1 H T k(H k P k|k-1 H T k +R) -1

[0082] in, R is the Jacobian matrix of the measurement model, and R is the covariance matrix of the observation noise.

[0083] State update: using Kalman gain and actual measured value z k Updated state estimate:

[0084]

[0085] Where h is the measurement model function, It is a state vector;

[0086] Update the covariance matrix:

[0087] P k =(IK k H k )P k|k-1

[0088] S94. Iterate and output the final localization result: Repeat steps S92 and S93. As the time step k increases, the localization is updated by combining real-time sensor data. The prediction and update stages are executed recursively. After several iterations, the final state vector is obtained. The covariance matrix P k It is the localization result after optimization by extended Kalman filter.

[0089] An electronic device includes a memory and a processor, wherein a computer program is stored in the memory, and the processor executes the computer program to implement any step of any of the above-described adaptive weighted positioning methods based on BIM multi-sensor fusion.

[0090] The beneficial effects of this invention include:

[0091] This invention provides high-precision map information by effectively utilizing the two-dimensional grid map generated from the BIM model, reducing the complexity of the traditional map construction and updating process, thereby simplifying the implementation process and effectively improving the overall usability and practicality of the system.

[0092] This invention fuses data from multiple traditional sensors, including lidar, IMU, visual sensors, and wheel speed odometers, significantly improving positioning accuracy. Compared to traditional single-sensor positioning methods, this invention, by fusing data from multiple sensors, makes the positioning process more stable and reliable, effectively reducing positioning errors.

[0093] This invention maintains accurate positioning even in complex indoor environments, especially in situations with insufficient lighting or unclear visual features. By dynamically adjusting the data weights of each sensor, even in dark environments where lidar or visual sensors fail, it can still effectively locate the rebar tying robot using an IMU and wheel speed odometer. This enhances the system's adaptability to dynamic environments, strengthens its robustness, and greatly expands its application scope, effectively addressing the challenges posed by environmental changes. Attached Figure Description

[0094] Figure 1 This is a schematic diagram of the overall process of the present invention;

[0095] Figure 2 This is a flowchart illustrating the steps of the multi-sensor fusion adaptive weighted fusion localization method of the present invention;

[0096] Figure 3 This describes the process for correcting distortion in point cloud data from a lidar system, as described in this invention. Detailed Implementation

[0097] The following is a further explanation of the present invention in conjunction with specific embodiments, such as... Figure 1 As shown, an adaptive weighted positioning method based on BIM multi-sensor fusion includes the following steps:

[0098] S1. Obtain a two-dimensional raster map containing information on the building frame structure and steel reinforcement layout. The process includes:

[0099] S11. Export IFC file: Use mainstream BIM software (such as Revit, Archicad, etc.) as the modeling platform and export IFC file from the Building Information Modeling (BIM) file. The IFC file includes information on the building's frame structure and reinforcement layout.

[0100] S12. Parse the IFC file: Parse the IFC file exported in step S1 to extract the building's frame structure and steel reinforcement layout information; use an IFC file parsing tool or library (in this embodiment, Python is used, employing the Ifcopenshell library) to decode the exported IFC file to extract the building structure information.

[0101] During the parsing process, each geometric object is separated to extract valid building geometry and attribute data; the building information extracted from the IFC file includes:

[0102] Frame structure: the geometry and spatial distribution of columns, beams, and walls;

[0103] Reinforcement information: node coordinates, reinforcement spacing, diameter;

[0104] Region segmentation: Extracting local information of the robot's current working area.

[0105] The set of coordinates of the reinforcement nodes can be represented as:

[0106] RebarNode(x,y,z)={N i |N i ∈IFC.ReinforcingBar}={(x1,y1,z1),(x2,y2,z2),…,(x n ,y n ,z n )}

[0107] The information about the reinforcing bars is usually stored in IEC.ReinforcingBar, with each coordinate point corresponding to a location node on the reinforcing bar mesh.

[0108] S13. Generate a 2D raster map: Based on the building frame structure and rebar layout information extracted in step S2, generate a corresponding 2D raster map; the 2D raster map represents the building frame structure and rebar layout range in cell format, including:

[0109] Define grid cells: Cells represent the spacing and spatial location of reinforcing bars at millimeter-level resolution;

[0110] Obstacle marking: Treating columns, beams, etc., as obstacles;

[0111] Reinforcing bar passable area: Mark the area for binding operations.

[0112]

[0113] After obtaining the 2D grid map, the robot is localized: such as Figure 2As shown, the two-dimensional grid map obtained in step S13 is imported into the Robot Operating System (ROS), and data from various sensors installed on the robot are collected. The robot's current position is located using an adaptive weighted localization method based on multi-sensor fusion. The generated two-dimensional grid map is converted from the format in the BIM parsing software to a ROS-compatible format (such as a pgm image file and its corresponding yaml configuration file). The map file is loaded into the ROS environment using the map_server node of ROS, enabling the robot to access map information in real time for subsequent LiDAR localization.

[0114] S2. Data Acquisition: Acquire real-time data from the inertial measurement unit (IMU), lidar, vision sensor, and wheel speed odometer, and synchronize and align the timestamps.

[0115] S3. Laser data processing: The collected laser data is preprocessed. Based on the data information collected in step S2, coordinate transformation is performed to convert the point cloud data from the lidar coordinate system to the global coordinate system to ensure consistency with the coordinates of the two-dimensional grid map.

[0116] Voxel mesh filtering reduces the amount of point cloud data by dividing the point cloud data into voxels of a fixed size, and replacing each voxel with the centroid of all points within the voxel, thereby reducing the amount of point cloud data.

[0117] Outliers are removed by statistical filtering. The number of neighboring points for each point is calculated, and points whose number of neighboring points is less than a set threshold are removed.

[0118] Noise points are removed by radius filtering, based on the number of neighboring points within the radius range;

[0119] Perform height filtering to filter out points whose height is outside the reasonable range, such as ground points or points with abnormal height;

[0120] Perform conditional filtering to remove points that do not meet the conditions.

[0121] The IMU provides angular velocity and acceleration information, and the attitude change is obtained by integration:

[0122]

[0123] Where θ(t) is the attitude at time t, and ω(t) is the angular velocity;

[0124] Wheel speed odometry provides information about the robot's position changes on a plane. Using this pose change information, it calculates the robot's pose (translation and rotation) changes relative to its initial position.

[0125] Δx=υ x ·Δt, Δy=υ y ·Δt

[0126] Where Δx and Δy are the displacements in the x and y directions, respectively, and υ x and υ y It is the robot's velocity component;

[0127] The estimated initial pose information is applied to laser point cloud distortion correction:

[0128] p corr =R(θ)·P raw +t

[0129] Where, p corr For the corrected point cloud, p raw R(θ) represents the original point cloud, R(θ) is the rotation matrix, and t is the pose translation.

[0130] S4. Laser Point Cloud Screening and Transmission: Determine whether the robot is currently in longitudinal or lateral movement mode, and select laser radar point cloud data within a specific angle range according to different movement types; dynamically adjust the scanning angle of the laser radar according to the robot's current movement type to ensure that the collected point cloud data covers the key areas of the robot's forward path;

[0131] First, subscribe to LiDAR data and convert it to Cartesian coordinate system data:

[0132] x i =r i ·cos(θ i )

[0133] y i =r i sin(θ) i )

[0134] Where, r i Let θ be the distance to the i-th point. i The angle of the lidar at that point.

[0135] Secondly, by subscribing to odometer data and recording the current location information compared to the previous moment, the location change is calculated.

[0136] Δx=x c -x p

[0137] Δy=y c -y p

[0138] Where Δx and Δy represent the robot's displacements along the x-axis and y-axis, respectively, x c With y c This represents the robot's position on the x-axis and y-axis at the previous moment, x pWith y p This indicates the robot's current position on the x-axis and y-axis.

[0139] The direction of motion is determined by comparing the absolute values ​​of position changes: if |Δx| > |Δy|, the motion type is longitudinal; otherwise, the motion type is lateral.

[0140] Select the key scanning angle range of the LiDAR based on the type of motion:

[0141] When moving longitudinally (primarily along the X-axis): Front: 45° to 135°; Rear: 225° to 315°;

[0142] When moving laterally (primarily along the Y-axis): Left side: 315° to 45°; Right side: 135° to 225°.

[0143] Within the angle range selected based on the motion type, valid point cloud data points are filtered out; for each point of the LiDAR, the angle is calculated, and it is determined whether it falls within the valid range.

[0144] For vertical movement:

[0145] θ i ∈[45°, 135°] or θ i ∈[225°, 315°]

[0146] For lateral movement:

[0147] θ i ∈[315°, 45°] or θ i ∈[135°, 225°]

[0148] By traversing the LiDAR data points, points that meet the conditions are collected and output to ensure coverage of the key areas of the robot's forward path; the preprocessed and corrected point cloud data is input into the Cartographer algorithm, which matches it with the static two-dimensional grid map to calculate the robot's pose in the global coordinate system.

[0149] S5. Generate error terms: Evaluate the quality of real-time data from each sensor and generate their respective error terms.

[0150] First, the noise level of the IMU gyroscope output is evaluated, and the linearity, bias, and scale factor error of the IMU accelerometer under different motion states are measured to generate error terms. The weight ω of each error term is dynamically calculated based on environmental characteristics or sensor status. niose ω bias ω linearity In order to obtain a more reasonable overall error estimate.

[0151] The IMU error term is:

[0152]

[0153] Where, σ 2 noise The standard deviation of noise level, σ 2 bias σ represents the standard deviation of zero bias (bias). 2 linearity θ represents the standard deviation of the linearity error. i , and φ i Let be the noise level, zero bias, and linearity error for the i-th iteration, respectively.

[0154] Then, a visual sensor error term is generated based on multiple image quality metrics, which typically include brightness and sharpness, etc.

[0155]

[0156] Where, σ 2 brightness σ represents the standard deviation of brightness. 2 sharpness The standard deviation of sharpness; u i and υ i ω represents the brightness and sharpness errors of the i-th iteration, respectively. brightness ω sharpness The weights for the brightness and sharpness error terms calculated based on environmental characteristics.

[0157] Next, a wheel speed odometer error term is generated by comparing the difference between the expected travel distance and the actual travel distance. If slippage is detected, the error term takes a very large value. The speed calculated by the wheel speed odometer is compared with the speed provided by the IMU. A threshold is preset to determine whether the difference is significant, so as to decide whether to record it as an error. If the difference exceeds the preset threshold, slippage is considered to have occurred.

[0158]

[0159] Where, d actual d represents the actual distance traveled. expected Indicates the expected distance to move; u i and υ i These represent the brightness and sharpness errors for the i-th iteration, respectively.

[0160] S6. Calculate sensor weights: Regularize the error term and calculate the initial weights of each sensor data.

[0161] First, regularization is performed on the error terms: To ensure that the error terms are compared on the same scale, each error term is normalized:

[0162]

[0163] Where max(E) and mmin(E) are the minimum and maximum values ​​of all sensor error terms, respectively. Let the regularized error terms be denoted as ; and the regularized error terms be denoted as .

[0164] Then, the initial weights are calculated based on the regularized error term:

[0165]

[0166] Among them, W i This represents the initial weight of each error term. This represents the sum of all regularized error terms.

[0167] For example, the initial weight calculation process for an IMU sensor is as follows:

[0168]

[0169] After calculation, the initial weights of each error term are denoted as W. IMU W Lidar W Vision and W Odometry .

[0170] S7. Smooth weight changes: Smooth weight changes using the exponentially weighted moving average method.

[0171] The exponentially weighted moving average (EWMA) method is used to smooth out weight changes in order to reduce drastic weight fluctuations caused by sensor error variations.

[0172] W i (t+1) =αW i (t) +(1-α)W i (t-1)

[0173] Where α is the smoothing coefficient, and its value ranges from 0 to 1.

[0174] S8. Preliminary localization: The data from each sensor are weighted and fused according to adaptive weights to generate preliminary localization results.

[0175]

[0176] in, For the initial pose estimation after multi-sensor data fusion, P i This represents the data from the i-th sensor.

[0177] S9. Optimize the positioning results: Use extended Kalman filtering to optimize the preliminary positioning results and obtain the final positioning results.

[0178] S91. Initialize the state vector, covariance matrix, and noise covariance:

[0179] State vector: Set the initial state vector X0, which contains preliminary positioning results, such as position and velocity information;

[0180] X0 = [x, y, θ, υ, ω] T

[0181] Where x and y are the robot's position coordinates, θ is the attitude angle, υ is the linear velocity, and ω is the angular velocity; these are taken from the initial positioning results during initialization.

[0182] Covariance matrix: The initial covariance matrix P0 represents the uncertainty of the initial state. The variances of each parameter can be estimated based on the system characteristics.

[0183]

[0184] The process noise covariance matrix and the measurement noise covariance matrix are set according to the noise type respectively:

[0185] Process noise covariance matrix Q: Considering the noise characteristics of robot motion, such as the measurement errors of wheel speed odometer and IMU;

[0186] Measurement noise covariance matrix R: reflects the measurement accuracy and noise level of lidar point clouds, vision sensors, etc.

[0187] S92. Perform state prediction and covariance prediction:

[0188] State prediction: Using the system's state transition model f, predict the state at the next moment.

[0189]

[0190] in, It is the predicted state at time k, u k-1 At time k-1, the control input linear velocity is ω. k-1 At time k-1, the control input angular velocity is θ. k-1 It is the heading angle of the previous state (relative to the x-axis), x k-1 and y k-1 Let cos(θ) be the position of the previous state in the plane. k-1 ) and sin(θ k-1 These are the x-component and y-component of the heading angle, respectively.

[0191] Covariance prediction: Update the covariance matrix using the Jacobian matrix F:

[0192]

[0193] in, is the Jacobian matrix of the state transition model, and Q is the covariance matrix of the process noise;

[0194] S93, Update Phase:

[0195] Calculate the Kalman gain: Using the prediction covariance matrix and the Jacobian matrix H of the measurement model, calculate the Kalman gain K. k :

[0196] K k =P k|k-1 H T k (H k P k|k-1 H T k +R) -1

[0197] in, R is the Jacobian matrix of the measurement model, and R is the covariance matrix of the observation noise.

[0198] State update: using Kalman gain and actual measured value z k Updated state estimate:

[0199]

[0200] Where h is the measurement model function, It is a state vector;

[0201] Update the covariance matrix:

[0202] P k =(IK k H k )P k|k-1

[0203] S94. Iterate and output the final localization result: Repeat steps S92 and S93. As the time step k increases, the localization is updated by combining real-time sensor data. The prediction and update stages are executed recursively. After several iterations, the final state vector is obtained. The covariance matrix P k It is the localization result after optimization by extended Kalman filter.

[0204] Finally, the robot localization result obtained in step S94 is output, and the real-time sensor data and localization result are stored for subsequent optimization of the weight update mechanism.

[0205] The above description is merely a further explanation of the present invention in conjunction with specific embodiments. All descriptions made do not imply any limitation on the scope of protection of the present invention. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. An adaptive weighted positioning method based on BIM multi-sensor fusion, characterized in that, Includes the following steps: S1. Obtain a two-dimensional grid map containing information on the building frame structure and steel reinforcement layout; S2. Data Acquisition: Acquire real-time data from the inertial measurement unit (IMU), lidar, vision sensor, and wheel speed odometer, and synchronize and align the timestamps. S3. Laser Data Processing: Based on the data information collected in step S2, the point cloud data is transformed from the lidar coordinate system to the global coordinate system through coordinate transformation to ensure consistency with the coordinates of the two-dimensional grid map obtained in step S1; the initial pose change is calculated using the IMU and wheel speed odometer, and distortion correction and noise removal are performed on the lidar point cloud data. S4. Laser point cloud screening and transmission: Determine whether the robot is currently in longitudinal or lateral movement mode, select laser radar point cloud data within a specific angle range according to different movement types, eliminate invalid or redundant point cloud information, and transmit the point cloud data to the system in real time for environmental matching in the Cartographer algorithm. S5. Generate error terms: Evaluate the quality of real-time data from each sensor and generate their respective error terms; S6. Calculate sensor weights: Regularize the error term and calculate the initial weights of each sensor data. S7. Smoothing weight changes: Smoothing weight changes using the exponentially weighted moving average method; S8. Preliminary localization: The data from each sensor are weighted and fused according to adaptive weights to generate a preliminary localization result; S9. Optimize positioning results: Optimize the preliminary positioning results using extended Kalman filtering to obtain and output the final positioning results; The specific process of calculating the sensor weights in step S6 includes: The error terms are regularized: to ensure that the error terms are compared on the same scale, each error term is normalized. ， in, and These are the minimum and maximum values ​​among all sensor error terms, respectively. Let the regularized error terms be denoted as ; and the regularized error terms be denoted as . ; Calculate the initial weights based on the regularized error term: ， in, This represents the initial weight of each error term. Let represent the sum of all regularized error terms; the initial weights of each error term are denoted as . , , and .

2. The adaptive weighted positioning method based on BIM multi-sensor fusion according to claim 1, characterized in that: The specific process of laser data processing in step S3 includes: Voxel mesh filtering reduces the amount of point cloud data by dividing the point cloud data into voxels of a fixed size, and replacing each voxel with the centroid of all points within the voxel, thereby reducing the amount of point cloud data. Outliers are removed by statistical filtering. The number of neighboring points for each point is calculated, and points whose number of neighboring points is less than a set threshold are removed. Noise points are removed by radius filtering, based on the number of neighboring points within the radius range; Perform height filtering to filter out points whose height is outside the reasonable range, such as ground points or points with abnormal height; Perform conditional filtering to remove points that do not meet the conditions; The IMU provides angular velocity and acceleration information, and the attitude change is obtained by integration: ， in, Let the pose be at time t. Angular velocity; Wheel speed odometry provides information about the robot's position changes on a plane. Using this pose change information, it calculates the robot's translational and rotational changes relative to its initial position. , in, and These are the displacements in the x and y directions, respectively. and It is the robot's velocity component; The estimated initial pose information is applied to laser point cloud distortion correction: ， in, The corrected point cloud, The original point cloud, For rotation matrix, This represents the pose translation.

3. The adaptive weighted positioning method based on BIM multi-sensor fusion according to claim 2, characterized in that: The specific process of laser point cloud screening and transmission in step S4 includes: The scanning angle of the LiDAR is dynamically adjusted according to the robot's current motion type to ensure that the collected point cloud data covers the key areas of the robot's forward path. Subscribe to LiDAR data and convert it to Cartesian coordinate system data; Subscribe to odometry data and calculate the direction of movement based on position changes over a period of time; if the robot mainly moves along the X-axis, it is considered to be moving longitudinally; if it mainly moves along the Y-axis, it is considered to be moving laterally. During longitudinal movement, point cloud data points within a 90-degree range in front and behind the LiDAR, specifically those between 45 and 135 degrees and between 225 and 315 degrees, are selected. During lateral movement, point cloud data points within a 90-degree range to the left and right of the LiDAR, specifically those between 315 and 45 degrees and between 135 and 225 degrees, are selected. The preprocessed and corrected point cloud data is then input into the Cartographer algorithm, matched with a static 2D grid map, to calculate the robot's pose in the global coordinate system.

4. The adaptive weighted positioning method based on BIM multi-sensor fusion according to claim 3, characterized in that: The specific process of generating the error term in step S5 includes: The noise level of the IMU gyroscope output is evaluated, and the linearity, bias, and scale factor error of the IMU accelerometer under different motion states are measured to generate error terms. The positioning accuracy is evaluated by analyzing the density and consistency of the point cloud data acquired by the lidar, and the measurement error is estimated by statistically analyzing the noise level in the lidar point cloud, thus generating an error term. Visual sensor error terms are generated based on multiple indicators of image quality; The wheel speed odometer error term is generated by comparing the difference between the expected travel distance and the actual travel distance. If slippage is detected, the error term takes a very large value. The speed calculated by the wheel speed odometer is compared with the speed provided by the IMU. If the difference exceeds a preset threshold, slippage is considered to have occurred.

5. The adaptive weighted positioning method based on BIM multi-sensor fusion according to claim 4, characterized in that: The specific process of smoothing weight changes in step S7 includes: The exponentially weighted moving average (EWMA) method is used to smooth out weight changes and reduce drastic weight fluctuations caused by sensor error variations. ， in, This is a smoothing coefficient, with a value range of 0-1.

6. The adaptive weighted positioning method based on BIM multi-sensor fusion according to claim 5, characterized in that: The specific process of preliminary positioning in step S8 includes: The data from each sensor are weighted and fused according to adaptive weights to generate preliminary localization results: ， in, For initial pose estimation after multi-sensor data fusion, This represents the data from the i-th sensor.

7. The adaptive weighted positioning method based on BIM multi-sensor fusion according to claim 6, characterized in that: The specific process of optimizing the positioning results in step S9 includes: S91. Initialize the state vector, covariance matrix, and noise covariance: State vector: Set the initial state vector It contains preliminary positioning results, including position and velocity information; Covariance matrix: Set the initial covariance matrix This represents the uncertainty of the initial state; the variance of each parameter can be estimated based on the system characteristics. The process noise covariance matrix and the measurement noise covariance matrix are set according to the noise type, respectively; Process noise covariance matrix Consider the noise characteristics of the robot's motion, including the measurement errors of the wheel speed odometer and IMU; Measurement noise covariance matrix This reflects the measurement accuracy and noise level of lidar point clouds and visual sensors. S92. Perform state prediction and covariance prediction: State prediction: using the system's state transition model To predict the state at the next moment; Covariance prediction: using the Jacobian matrix Update the covariance matrix; S93, Update Phase: Calculating Kalman gain: using the prediction covariance matrix and the Jacobian matrix of the measurement model. Calculate Kalman gain ; State update: using Kalman gain and actual measurements Update the state estimate and the covariance matrix; S94. Iterate and output the final localization result: Repeat steps S92 and S93. As the time step k increases, the localization is updated by combining real-time sensor data. The prediction and update stages are executed recursively. After several iterations, the final state vector is obtained. Covariance Matrix It is the localization result after optimization by extended Kalman filter.

8. An electronic device, comprising a memory and a processor, wherein the memory stores a computer program, characterized in that: When the processor executes the computer program, it implements any step in the adaptive weighted positioning method based on BIM multi-sensor fusion as described in any one of claims 1-7.

Images