BIM (Building Information Modeling) multi-sensor fusion-based adaptive weighted positioning method and electronic equipment

Abstract

The invention relates to the technical field of positioning and navigation of reinforcing steel bar binding robots, in particular to a BIM multi-sensor fusion-based adaptive weighted positioning method and electronic equipment, and the method comprises the following steps: S1, obtaining a two-dimensional grid map; s2, collecting data; s3, laser data processing; s4, laser point cloud screening and transmission; s5, generating an error term; s6, calculating the weight of the sensor; s7, smoothing weight change; s8, performing preliminary positioning; s9, optimizing a positioning result; according to the invention, the BIM model of the building is exported and the two-dimensional grid map is generated, and the map is imported into the ROS as an environment reference for laser radar positioning, so that the positioning precision of the robot is improved; the multi-sensor fusion model is constructed by combining various sensors, the real-time data quality of each sensor is dynamically evaluated to adjust the data weight of each sensor, the positioning precision and stability of the method in complex environment changes are effectively improved, the application range of the steel bar binding robot is expanded, and the method has wide application prospects.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning and navigation of a steel bar binding robot, and specifically to an adaptive weighted positioning method and electronic equipment based on BIM multi-sensor fusion. Background Art

[0002] In the construction industry, accurate positioning is crucial for rebar tying robots to perform tasks; traditional positioning methods usually rely on a single sensor, such as an inertial measurement unit (IMU), lidar, etc. However, with the rapid development of technology, the positioning accuracy when using a single sensor is easily affected by environmental interference or its own limitations, resulting in reliability problems caused by accumulated positioning errors, signal blocking, and environmental changes; for example, lidar 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 odometers can cause inaccurate position estimation 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 integrates the posture information obtained by sensors such as IMU and lidar to determine the robot's position.

[0003] Existing multi-sensor fusion positioning methods usually rely on pre-built maps for positioning, but in the indoor environment of buildings, the accuracy and update of maps are often not enough 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 multi-sensor fusion positioning methods, it can make up for the lack of accuracy and update of traditional multi-sensor fusion positioning methods in the indoor environment of buildings. Summary of the invention

[0004] The object of the present invention is to provide an adaptive weighted positioning method and electronic equipment based on BIM multi-sensor fusion, which can effectively improve the positioning accuracy and stability of a steel bar tying robot in a dynamic construction environment.

[0005] Based on the above purpose, the present invention adopts the following technical solution:

[0006] An adaptive weighted positioning method based on BIM multi-sensor fusion includes the following steps:

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

[0008] S2, data collection: collect real-time data from the inertial measurement unit IMU, lidar, visual sensor and wheel speed odometer, and synchronize and align the timestamps;

[0009] S3, laser data processing: coordinate transformation is performed according to the data information collected in step S2; the initial posture change is calculated using the IMU and the wheel speed odometer, and the point cloud data of the laser radar is distorted and noise is removed;

[0010] S4, laser point cloud screening and transmission: Determine whether the robot is currently in longitudinal or lateral movement mode, select the 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 environment matching in the Cartographer algorithm;

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

[0012] S6. Calculate sensor weights: Regularize the error terms and calculate the initial weights of each sensor data;

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

[0014] S8, preliminary positioning: weighted fusion of the data of each sensor according to the b adaptation weight to generate a preliminary positioning result;

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

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

[0017] The point cloud data is converted from the LiDAR coordinate system to the global coordinate system through coordinate transformation to ensure consistency with the 2D grid map coordinates;

[0018] The amount of point cloud data is reduced by voxel grid filtering, which divides the point cloud data into voxels of fixed size, and each voxel is replaced by the centroid of all points in the voxel, thereby reducing the amount of point cloud data;

[0019] Remove outliers through statistical filtering, calculate the number of neighboring points of each point, and remove points whose number of neighboring points is less than the set threshold;

[0020] Remove noise points through radius filtering, and remove noise points based on the number of neighboring points within the radius;

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

[0022] Conditional filtering is performed 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] Among them, θ(t) is the posture at time t, ω(t) is the angular velocity;

[0026] The wheel speed odometer provides the position change of the robot on the plane. Using the posture change information, the posture (translation and rotation) change of the robot relative to the initial position is calculated:

[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 is the velocity component of the robot;

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

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

[0031] Among them, p corr is the corrected point cloud, p raw is the original point cloud, R(θ) is the rotation matrix, and t is the position translation.

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

[0033] Dynamically adjust the scanning angle of the LiDAR 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 the lidar data and convert it into Cartesian coordinate system data;

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

[0036] When moving vertically, the focus is on selecting point cloud data points within the 90-degree range of the front and rear of the laser radar (45 degrees to 135 degrees and 225 degrees to 315 degrees). When moving horizontally, the focus is on selecting point cloud data points within the 90-degree range of the left and right of the laser radar (315 degrees to 45 degrees and 135 degrees to 225 degrees). The pre-processed and corrected point cloud data is input into the Cartographer algorithm, matched with the static two-dimensional grid map, and the robot's position and posture in the global coordinate system are calculated;

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

[0038] Evaluate the noise level of the IMU gyroscope output and measure the linearity, bias and scale factor errors of the IMU accelerometer under different motion states 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 noise level in the LiDAR point cloud is statistically analyzed to estimate its measurement error and generate an error term.

[0040] generating visual sensor error terms based on multiple image quality metrics;

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

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

[0043] Regularize the error terms: In order to make the error terms comparable on the same scale, each error term is normalized:

[0044]

[0045] Among them, max(E) and min(E) are the minimum and maximum values ​​of all sensor error terms, respectively. represents the regularized error term; the regularized error terms are

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

[0047]

[0048] Among them, W i represents the initial weight of each error term, represents the sum of the regularized error terms; the initial weights of each error term are recorded as WIMU , W Lidar , W Vision and W Odometry .

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

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

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

[0052] Among them, α is the smoothing coefficient, and its value range is 0-1.

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

[0054] The data of each sensor is weighted and fused according to the adaptive weights to generate a preliminary positioning result:

[0055]

[0056] in, is the initial pose estimation after multi-sensor data fusion, P i is the data of the i-th sensor.

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

[0058] Define grid cells: cells represent the spacing and spatial position of reinforcement bars with millimeter resolution;

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

[0060] Steel bar accessible area: mark the area where the binding operation path is located;

[0061] The two-dimensional grid map can be expressed as:

[0062]

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

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

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

[0066] Among them, x, y are the robot position coordinates, θ is the attitude angle, υ is the linear velocity, and ω is the angular velocity; it is taken from the initial positioning result during initialization;

[0067] Covariance matrix: The initial covariance matrix P0 is set to represent the uncertainty of the initial state. The variance of each parameter can be estimated based on the system characteristics:

[0068]

[0069] Set the process noise covariance matrix and measurement noise covariance matrix according to the noise type:

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

[0071] Measurement noise covariance matrix R: reflects the measurement accuracy and noise level of lidar point cloud, visual sensor, etc.;

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

[0073] State prediction: Use the system's state transition model f to predict the state at the next moment:

[0074]

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

[0076] Covariance prediction: Use the Jacobian matrix F to update the covariance matrix:

[0077]

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

[0079] S93, Update Phase:

[0080] Calculate the Kalman gain: Use the prediction covariance matrix and the Jacobian matrix H of the measurement model to calculate the Kalman gain Kk :

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

[0082] in, 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 measurement value z k Update the state estimate:

[0084]

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

[0086] Update the covariance matrix:

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

[0088] S94, iterate and output the final positioning result: repeat steps S92 and S93, and as the time step k increases, combine the real-time sensor data to update the positioning, and recursively execute the prediction and update stages; after several iterations, the final state vector and the covariance matrix P k It is the positioning result after the extended Kalman filter optimization.

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

[0090] The beneficial effects of the present invention include:

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

[0092] The present invention fuses data from multiple traditional sensors, including lidar, IMU, visual sensor, and wheel speed odometer, significantly improving positioning accuracy. Compared with the traditional single sensor positioning method, the present invention fuses data from multiple sensors, making the positioning process more stable and reliable and effectively reducing positioning errors.

[0093] The present invention can maintain precise positioning in complex indoor environments, especially in conditions of insufficient lighting or unclear visual features. By dynamically adjusting the data weights of each sensor, even in a dark environment where the lidar or visual sensor fails, the rebar tying robot can still be effectively positioned through the IMU and wheel speed odometer, thereby enhancing the adaptability of the system in dynamic environments, enhancing the robustness of the system, greatly expanding the scope of use of the present invention, and being able to effectively respond to the challenges brought about by environmental changes. BRIEF DESCRIPTION OF THE DRAWINGS

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

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

[0096] Figure 3 This is the laser radar point cloud data distortion correction process of the present invention. DETAILED DESCRIPTION

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

[0098] S1. Obtaining a two-dimensional grid map containing building frame structure and reinforcement layout information, 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 the building frame structure and reinforcement layout information of the building.

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

[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: geometry and spatial distribution of columns, beams and walls;

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

[0104] Regional segmentation: Extract local information of the robot’s current workspace.

[0105] Among them, the steel bar node coordinate set can be expressed 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] Among them, the information of the reinforcement is usually stored in IEC.ReinforcingBar, and each coordinate point corresponds to a position node on the reinforcement mesh.

[0108] S13, generating a two-dimensional grid map: generating a corresponding two-dimensional grid map according to the building frame structure and steel bar arrangement information extracted in step S2; the two-dimensional grid map represents the building frame structure and steel bar arrangement range in the form of cells, including:

[0109] Define grid cells: cells represent the spacing and spatial position of reinforcement bars with millimeter resolution;

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

[0111] Rebar passable area: mark the area where the binding operation path is located.

[0112]

[0113] After obtaining the two-dimensional grid map, the robot is positioned: Figure 2As shown, the two-dimensional grid map obtained in step S13 is imported into the robot operating system (ROS), and the data of various sensors set on the robot are collected, and the current position of the robot is located by an adaptive weighted positioning method of 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 a corresponding yaml configuration file); the map_server node of ROS is used to load the map file into the ROS environment, so that the robot can access the map information in real time for subsequent lidar positioning.

[0114] S2, data collection: collect real-time data from inertial measurement unit (IMU), lidar, visual sensor and wheel speed odometer, and synchronize and align timestamps;

[0115] S3, laser data processing: pre-process the collected laser data, perform coordinate transformation according to the data information collected in step S2, and convert the point cloud data from the laser radar coordinate system to the global coordinate system to ensure consistency with the two-dimensional grid map coordinates.

[0116] The amount of point cloud data is reduced by voxel grid filtering, which divides the point cloud data into voxels of fixed size, and each voxel is replaced by the centroid of all points in the voxel, thereby reducing the amount of point cloud data;

[0117] Remove outliers through statistical filtering, calculate the number of neighboring points of each point, and remove points whose number of neighboring points is less than the set threshold;

[0118] Remove noise points through radius filtering, and remove noise points based on the number of neighboring points within the radius;

[0119] Perform height filtering to filter out points whose heights are not within a reasonable range, such as ground points or points with abnormal heights;

[0120] Conditional filtering is performed 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] Among them, θ(t) is the posture at time t, ω(t) is the angular velocity;

[0124] The wheel speed odometer provides the position change of the robot on the plane. Using the posture change information, the posture (translation and rotation) change of the robot relative to the initial position is calculated:

[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 is the velocity component of the robot;

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

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

[0129] Among them, p corr is the corrected point cloud, p raw is the original point cloud, R(θ) is the rotation matrix, and t is the position 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 laser radar scanning angle according to the current movement type of the robot to ensure that the collected point cloud data covers the key areas of the robot's forward path;

[0131] First, subscribe to the lidar data and convert it to Cartesian coordinates:

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

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

[0134] Among them, r i is the distance to the i-th point, θ i is the lidar angle at that point.

[0135] Secondly, subscribe to the odometer data and calculate the position change by recording the current position information and the position information at the previous moment:

[0136] Δx=x c -x p

[0137] Δy=y c -y p

[0138] Among them, Δx and Δy represent the displacement of the robot on the x-axis and y-axis respectively. c With y c Indicates the position of the robot on the x-axis and y-axis at the previous moment, x pWith y p Indicates the current position of the robot on the x-axis and y-axis.

[0139] By comparing the absolute value of the position change, the direction of movement is determined: if |Δx|>|Δy|, the movement type is longitudinal; otherwise, the movement type is transverse.

[0140] Depending on the type of motion, select the critical scanning angle range of the LiDAR:

[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] Filter out valid point cloud data points within the angle range selected according to the motion type; for each point of the lidar, calculate the angle and determine 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, the qualified points are collected and output to ensure that the key areas of the robot's forward path are covered; the preprocessed and corrected point cloud data is input into the Cartographer algorithm, matched with the static two-dimensional grid map, and the robot's position and posture in the global coordinate system are calculated.

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

[0150] First, the noise level of the IMU gyroscope output is evaluated, and the linearity, zero bias, and scale factor errors of the IMU accelerometer under different motion states are measured to generate error terms; among them, the weight ω of each error term is dynamically calculated according to the environmental characteristics or sensor state niose ,ω bias ,ω linearity , in order to obtain a more reasonable overall error estimate.

[0151] The IMU error term is:

[0152]

[0153] Among them, σ 2 noise represents the standard deviation of the noise level, σ 2 bias Represents the standard deviation of the zero bias, σ 2 linearity Represents the standard deviation of linearity error; θ i , and φ i are the noise level, zero bias and linearity error of the ith time respectively.

[0154] The visual sensor error term is then generated based on multiple image quality indicators, which usually include brightness and clarity:

[0155]

[0156] Among them, σ 2 brightness represents the standard deviation of brightness, σ 2 sharpness Indicates the standard deviation of clarity; u i and i are the brightness and clarity errors of the i-th time, ω brightness ,ω sharpness is the weight of the brightness and clarity error terms calculated based on environmental characteristics.

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

[0158]

[0159] Among them, d actual Indicates the actual moving distance, d expected Indicates the expected moving distance; u i and i are the brightness and clarity errors of the i-th time respectively.

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

[0161] First, the error terms are regularized: In order to make the error terms comparable on the same scale, each error term is normalized:

[0162]

[0163] Among them, max(E) and mmin(E) are the minimum and maximum values ​​of all sensor error terms, respectively. represents the regularized error term; the regularized error terms are

[0164] Then calculate the initial weights based on the regularized error term:

[0165]

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

[0167] For example, the initial weight calculation process of the IMU sensor is:

[0168]

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

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

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

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

[0173] Among them, α is the smoothing coefficient, and its value range is 0-1.

[0174] S8, preliminary positioning: weighted fusion of the data from each sensor according to the adaptive weight to generate a preliminary positioning result.

[0175]

[0176] in, is the initial pose estimation after multi-sensor data fusion, P i is the data of the i-th sensor.

[0177] S9. Optimize positioning results: Use extended Kalman filtering to optimize the preliminary positioning results to 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 the preliminary positioning results, such as position and speed information;

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

[0181] Among them, x, y are the robot position coordinates, θ is the attitude angle, υ is the linear velocity, and ω is the angular velocity; it is taken from the initial positioning result during initialization;

[0182] Covariance matrix: The initial covariance matrix P0 is set to represent the uncertainty of the initial state. The variance of each parameter can be estimated based on the system characteristics:

[0183]

[0184] Set the process noise covariance matrix and measurement noise covariance matrix according to the noise type:

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

[0186] Measurement noise covariance matrix R: reflects the measurement accuracy and noise level of lidar point cloud, visual sensor, etc.;

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

[0188] State prediction: Use the system's state transition model f to predict the state at the next moment:

[0189]

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

[0191] Covariance prediction: Use the Jacobian matrix F to update the covariance matrix:

[0192]

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

[0194] S93, Update Phase:

[0195] Calculate the Kalman gain: Use the prediction covariance matrix and the Jacobian matrix H of the measurement model to 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, 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 measurement value z k Update the state estimate:

[0199]

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

[0201] Update the covariance matrix:

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

[0203] S94, iterate and output the final positioning result: repeat steps S92 and S93, and as the time step k increases, combine the real-time sensor data to update the positioning, and recursively execute the prediction and update stages; after several iterations, the final state vector and the covariance matrix P k It is the positioning result after the extended Kalman filter optimization.

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

[0205] The above description is only a further explanation of the present invention in combination with specific embodiments. All descriptions do not limit the scope of protection of the present invention. Any changes or replacement schemes that can be easily thought of by any technician in this field within the technical scope disclosed by the present invention should be included in the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be based on the scope of protection of the claims.

Claims

1. An adaptive weighted positioning method based on BIM multi-sensor fusion, characterized in that: The following steps are involved: S1, obtaining a two-dimensional grid map containing building frame structure and steel bar arrangement information; S2, data collection: collect real-time data from the inertial measurement unit IMU, lidar, visual sensor and wheel speed odometer, and synchronize and align the timestamps; S3, laser data processing: according to the data information collected in step S2S2, the point cloud data is converted from the laser radar coordinate system to the global coordinate system through coordinate transformation to ensure that it is consistent with the two-dimensional grid map coordinates obtained in step S1; the initial posture change is calculated using the IMU and the wheel speed odometer, and the point cloud data of the laser radar is distorted and noise is removed; S4, laser point cloud screening and transmission: Determine whether the robot is currently in longitudinal or lateral movement mode, select the 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 environment matching in the Cartographer algorithm; S5, generate error terms: evaluate the quality of real-time data of each sensor and generate respective error terms; S6. Calculate sensor weights: Regularize the error terms and calculate the initial weights of each sensor data; S7, smooth weight changes: smooth weight changes according to the exponentially weighted moving average method; S8, preliminary positioning: weighted fusion of the data of each sensor according to the adaptive weight to generate a preliminary positioning result; S9. Optimize positioning results: Use extended Kalman filtering to optimize the preliminary positioning results, obtain and output the final positioning results.

2. The adaptive weighted positioning method based on BIM multi-sensor fusion according to claim 2 is characterized in that: The specific process of the laser data processing in step S3 includes: The amount of point cloud data is reduced by voxel grid filtering, which divides the point cloud data into voxels of fixed size, and each voxel is replaced by the centroid of all points in the voxel, thereby reducing the amount of point cloud data; Remove outliers through statistical filtering, calculate the number of neighboring points of each point, and remove points whose number of neighboring points is less than the set threshold; Remove noise points through radius filtering, and remove noise points based on the number of neighboring points within the radius; Perform height filtering to filter out points whose heights are not within a reasonable range, such as ground points or points with abnormal heights; 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: Among them, θ(t) is the posture at time t, ω(t) is the angular velocity; The wheel speed odometer provides the position change of the robot on the plane. Using the posture change information, the posture (translation and rotation) change of the robot relative to the initial position is calculated: Δx=v x ·Δt,Δy=v y ·Δt where Δx and Δy are the displacements in the x and y directions respectively, and v x and v y is the velocity component of the robot; The estimated initial pose information is applied to the laser point cloud distortion correction: p corr =R(θ)·p raw +t Among them, p corr is the corrected point cloud, p raw is the original point cloud, R(θ) is the rotation matrix, and t is the position translation.

3. The adaptive weighted positioning method based on BIM multi-sensor fusion according to claim 3 is characterized in that: The specific process of laser point cloud screening and transmission in step S4 includes: Dynamically adjust the scanning angle of the LiDAR 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 the lidar data and convert it into Cartesian coordinate system data; Subscribe to odometer data and calculate the direction of movement based on the position change over a period of time; if the robot moves mainly along the X axis, it is considered to be moving longitudinally; if it moves mainly along the Y axis, it is considered to be moving laterally; When moving vertically, the focus is on selecting point cloud data points within the 90-degree range of the front and rear of the laser radar (45 degrees to 135 degrees and 225 degrees to 315 degrees); when moving horizontally, the focus is on selecting point cloud data points within the 90-degree range of the left and right of the laser radar (315 degrees to 45 degrees and 135 degrees to 225 degrees); the preprocessed and corrected point cloud data is input into the Cartographer algorithm, matched with the static two-dimensional grid map, and the position and posture of the robot in the global coordinate system are calculated.

4. The adaptive weighted positioning method based on BIM multi-sensor fusion according to claim 4 is characterized in that: The specific process of generating the error term in step S5 includes: Evaluate the noise level of the IMU gyroscope output and measure the linearity, bias and scale factor errors of the IMU accelerometer under different motion states 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 noise level in the LiDAR point cloud is statistically analyzed to estimate its measurement error and generate an error term. generating visual sensor error terms based on multiple image quality metrics; The wheel speed odometer error term is generated by comparing the difference between the expected moving distance and the actual moving distance. If slippage is detected, the error term takes a maximum value. The speed calculated by the wheel speed odometer is compared with the speed provided by the IMU. If the difference exceeds the preset threshold, slippage is considered to have occurred.

5. The adaptive weighted positioning method based on BIM multi-sensor fusion according to claim 5 is characterized in that: The specific process of calculating the sensor weight in step S6 includes: Regularize the error terms: In order to make the error terms comparable on the same scale, each error term is normalized: Among them, max(E) and min(E) are the minimum and maximum values ​​of all sensor error terms, respectively. represents the regularized error term; the regularized error terms are Calculate the initial weights based on the regularized error term: Among them, W i represents the initial weight of each error term, represents the sum of the regularized error terms; the initial weights of each error term are recorded as W IMU , W Lidar , W Vision and W Odometry .

6. The adaptive weighted positioning method based on BIM multi-sensor fusion according to claim 6 is characterized in that: The specific process of step S7 smoothing weight change includes: The exponentially weighted moving average (EWMA) method is used to smooth weight changes to reduce drastic weight changes caused by sensor error fluctuations: W i (t+1) =αW i (t) +(1-α)W i (t-1) Among them, α is the smoothing coefficient, and its value range is 0-1.

7. The adaptive weighted positioning method based on BIM multi-sensor fusion according to claim 7 is characterized in that: The specific process of the preliminary positioning in step S8 includes: The data of each sensor is weighted and fused according to the adaptive weights to generate a preliminary positioning result: in, is the initial pose estimation after multi-sensor data fusion, P i is the data of the ith sensor.

8. The adaptive weighted positioning method based on BIM multi-sensor fusion according to claim 8 is characterized in that: The specific process of optimizing the positioning result in step S9 includes: S91. Initialize the state vector, covariance matrix, and noise covariance: State vector: Set the initial state vector X0, which contains the preliminary positioning results, such as position and speed information; Covariance matrix: The initial covariance matrix P0 is set to represent the uncertainty of the initial state. The variance of each parameter can be estimated based on the system characteristics. Set the process noise covariance matrix and the measurement noise covariance matrix according to the noise type; Process noise covariance matrix Q: takes into account the noise characteristics in the robot motion, such as the measurement errors of the wheel speed odometer and IMU; Measurement noise covariance matrix R: reflects the measurement accuracy and noise level of lidar point cloud, visual sensor, etc.; S92. Perform state prediction and covariance prediction: State prediction: Use the system's state transition model f to predict the state at the next moment; Covariance prediction: Use the Jacobian matrix F to update the covariance matrix; S93, Update Phase: Calculate the Kalman gain: Use the prediction covariance matrix and the Jacobian matrix H of the measurement model to calculate the Kalman gain K k ; State update: using Kalman gain and actual measurement value z k Update the state estimate and update the covariance matrix; S94, iterate and output the final positioning result: repeat steps S92 and S93, and as the time step k increases, combine the real-time sensor data to update the positioning, and recursively execute the prediction and update stages; after several iterations, the final state vector and the covariance matrix P k It is the positioning result after the extended Kalman filter optimization.

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

Images