A multi-sensor fusion positioning method applicable to building scenarios

Abstract

The present invention relates to a multi-sensor fusion positioning method applicable to construction scenarios. The multi-sensor fusion positioning technology with lidar, inertial measurement unit and wheel encoder as the core can achieve centimeter-level high-precision positioning of the robot in complex construction scenarios and improve the quality of construction operations. Applying the multi-sensor fusion positioning technology to construction scenarios can ensure the stability and accuracy of high-precision positioning and provide effective positioning perception data for the construction operations of construction robots.

Description

Technical Field

[0001] The present invention relates to the technical field of building construction, and particularly to a multi-sensor fusion positioning method applicable to building scenarios. Background Art

[0002] A mobile robot refers to an integrated hardware system that has environmental perception, dynamic decision-making and planning, and control capabilities in a complex working environment. Mobile robots have greater flexibility and mobility, and can replace humans to engage in dangerous, harsh, and general mechanical repetitive tasks, playing an important role in scenarios such as industry, agriculture, and service.

[0003] According to the core sensor, robot positioning technologies can be divided into lidar-based positioning technology, vision camera-based positioning technology, and GPS-based positioning technology. In the complex environment of a construction site, the accuracy of a vision camera is difficult to guarantee due to unstable light conditions, and traditional GPS positioning methods cannot work properly due to unstable signals. Summary of the Invention

[0004] The purpose of the present invention is to provide a multi-sensor fusion positioning method applicable to building scenarios to solve the problems encountered in the above background art.

[0005] To achieve the above purpose, the technical solution of the present invention is as follows:

[0006] A multi-sensor fusion positioning method applicable to building scenarios includes the following steps:

[0007] S1. Install a lidar, an IMU sensor, and a wheel encoder in the chassis of the mobile robot. Place the robot in a room with a flat ground and four closed sides. The mobile robot randomly moves the chassis to collect the output information of the lidar, IMU, and wheel encoder.

[0008] Use the inter-frame matching method to register the lidar data to obtain the lidar odometry information; use the IMU pre-integration method to obtain the IMU odometry information; sample and integrate the wheel encoder to obtain the chassis odometry information.

[0009] S2. Perform time synchronization processing on the collected data and perform spatial calibration processing on the collected data.

[0010] When performing time synchronization processing on the collected data, use the lidar odometry data as the time reference to synchronize the chassis odometry and IMU odometry information; for the chassis odometry, find the two frames of data closest to the time stamp of the i-th lidar frame before and after, and perform linear interpolation according to the time difference from the i-th frame of data to obtain the chassis odometry data synchronized with the i-th frame; perform the same interpolation synchronization operation for the IMU odometry information.

[0011] When performing spatial calibration processing on the collected data, the hand-eye calibration method AX = XB is adopted, where A represents the data of one sensor, B represents the data of another sensor, and X is the external parameter matrix between the two sensors. The external parameter matrix X usually consists of translation and rotation components and has a total of six degrees of freedom, that is, six sets of equations need to be solved.

[0012] Furthermore, when solving the six sets of equations, an overdetermined system of equations is formed, and a residual model of argmin|AX = XB| is established. The external parameters of the two sensors are found by minimizing the residual through an optimization method, and the spatio-temporal synchronization of the original data is completed after pairwise calibration.

[0013] S3. Perform real-time projection correction on the sensor data of the lidar, IMU sensor, and wheel encoder.

[0014] According to the spatio-temporal calibration results, the IMU and lidar data are converted to the chassis coordinate system to complete the unification of coordinate system data. After that, the data for fusion processing are all the converted data; according to the inclination data in the IMU, the chassis odometer and lidar data are corrected. The data are decomposed in the three-dimensional space, with the horizontal plane as the reference, and the data are projected onto the horizontal plane to complete the data correction.

[0015] S4. Obtain the plane scan data of the surrounding environment through a single-line lidar, and obtain the displacement information of the robot according to the tightly coupled odometry method; perform ICP registration using the laser inter-frame matching method to obtain the laser odometry data; and obtain the front-end laser submap according to the laser odometry data.

[0016] Obtain the plane scan data of the surrounding environment through a single-line lidar, obtain the displacement information of the robot according to the tightly coupled method of odometry and IMU information. Using the displacement information of the robot between two frames of lidar data as the initial value, perform ICP registration on the front and rear frame laser data using the laser inter-frame matching method to obtain the laser odometry data. According to this odometry data, key frames are selected at certain intervals, and the laser data are superimposed to obtain the front-end laser submap.

[0017] Furthermore, the front-end laser submap contains laser data with a fixed number of frames. When the submap is filled, several frames of data with relatively close spatial distances in the submap are selected, and laser ICP registration is performed again to achieve submap loop closure. Using the position information of the laser in the submap as nodes and the odometry and loop closure information as edge constraints, nonlinear optimization is performed to correct the displacement relationship between the nodes.

[0018] S5. Obtain the submap information output by the front end, convert the submap to the prior map coordinate system according to the initial positioning, and perform feature extraction; obtain the displacement information of the robot according to the tightly coupled method of odometry and IMU information.

[0019] Using the displacement information of the robot as the prior initial value, the submap data as the source point cloud, and the map data as the target point cloud, obtain the semantic information of the point cloud, calculate the curvature and normal vector information of the submap and the global map point cloud, and classify the data into planes, internal and external corners, and dynamic obstacles according to the curvature distribution.

[0020] S6. According to the data of the source point cloud and the target point cloud of the data, use the inter-frame laser matching method to perform weighted laser ICP registration on the two frames of data; obtain laser odometry information, laser submap localization information, chassis odometry information, and IMU information.

[0021] S7. Add the laser odometry information, laser submap localization information, chassis odometry information, and IMU information of a fixed length to the information queue; use the residual term equation to construct a window optimization model;

[0022] S8. Perform marginalization processing on the tail of the data queue according to the single-sided constraint and the double-sided constraint, and obtain the marginalization prior factor through calculation.

[0023] The factors corresponding to the residuals of the submap matching and the optimization variables are the map prior information. One factor constrains one pose, which is a single-sided constraint; the residuals of the laser odometry and the optimization quantity, the residuals of the chassis odometry and the optimization quantity, and the residuals of the IMU pre-integrated odometry and the optimization variables. The factors corresponding to these three items are the odometry information. One factor constrains two adjacent poses, which is a double-sided constraint; the marginalization processing formula is AX = b, which is decomposed into the following form:

[0024]

[0025] where X i is the edge frame data, X j is the latest frame of data. Through the Schur complement transformation, an equation related only to X j is obtained:

[0026]

[0027] S9. According to the marginalization prior factor, use the optimization method to solve the system state information to obtain the optimal estimate of the current state; add the latest frame of data, and continue to repeat steps S7 to S8, continuously update the old frames, add new frames, and realize the forward sliding of the window; fuse the localization information of multiple sensors, and output the real-time position and attitude information of the robot.

[0028] Compared with the prior art, the beneficial effects of the present invention are as follows: The multi-sensor fusion positioning technology with lidar, inertial measurement unit and wheel encoder as the core can achieve centimeter-level high-precision positioning of the robot in complex building scenarios, improving the quality of construction operations. Applying the multi-sensor fusion positioning technology to building scenarios can ensure the stability and accuracy of high-precision positioning, providing effective positioning perception data for the construction operations of construction robots. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] The disclosure of the present invention will be described with reference to the accompanying drawings. It should be understood that the drawings are only for illustrative purposes and are not intended to limit the scope of protection of the present invention. In the drawings, the same reference numerals are used to refer to the same components. Among them:

[0030] Figure 1 is the fusion positioning flowchart of the present invention;

[0031] Figure 2 is the schematic diagram of the external parameter calibration of the sensor in the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0032] In order to make the technical means, creative features, achieved purposes and functions of the present invention easy to understand, the present invention will be further described in detail below with reference to the accompanying drawings. These drawings are all simplified schematic diagrams, only illustrating the basic structure of the present invention in a schematic manner, so they only show the relevant components of the present invention.

[0033] According to the technical solution of the present invention, without changing the essence of the present invention, those of ordinary skill in the art can propose various structural ways and implementation ways that can be mutually replaced. Therefore, the following detailed embodiments and the accompanying drawings are only exemplary descriptions of the technical solution of the present invention, and should not be regarded as the whole of the present invention or as a limitation or restriction on the technical solution of the present invention.

[0034] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0035] As Figure 1 shown, a multi-sensor fusion positioning method applicable to building scenarios includes the following steps:

[0036] S1. Install a lidar, an IMU sensor and a wheel encoder in the chassis of the mobile robot, place the robot in a room with a flat ground and four closed sides, and randomly move the chassis of the mobile robot to collect the output information of the lidar, IMU and wheel encoder.

[0037] LiDAR is an optoelectronic ranging sensor that obtains the position profile information of a target by emitting a laser beam towards the target and then receiving the reflected signal. It features a long detection range and high precision, generally reaching about 2 cm, and is not affected by light changes. IMU (Inertial Measurement Unit) is an inertial component that usually includes an accelerometer, a gyroscope, and a magnetometer. It can feedback its own motion state information, including three-dimensional acceleration, angle, angular velocity, etc., and can obtain the three-dimensional motion information of an object in real time at a high sampling rate. A wheel encoder is an angular displacement sensor that converts the displacement on a code disk into an electrical signal and obtains the odometer information of the robot's motion through integration. It determines the change in the robot's pose by detecting the arc degree that the robot's wheels rotate in a certain period of time, and can obtain relative displacement and motion.

[0038] When collecting the output information of LiDAR, IMU, and wheel encoder, the frame-to-frame matching method is used to register the laser data to obtain the laser odometer information; the IMU pre-integration method is used to obtain the IMU odometer information; the wheel encoder is sampled and integrated to obtain the chassis odometer information.

[0039] S2. Perform time synchronization processing on the collected data and perform spatial calibration processing on the collected data.

[0040] When performing time synchronization processing on the collected data, the laser odometer data is used as the time reference to synchronize the chassis odometer and IMU odometer information. For the chassis odometer, find the two frames of data closest to the time stamp of the i-th frame of LiDAR, and perform linear interpolation according to the time difference from the i-th frame of data to obtain the chassis odometer data synchronized with the i-th frame. The same interpolation synchronization operation as the chassis odometer is adopted for the IMU odometer information, and the same applies to the chassis odometer information.

[0041] It should be noted that direct linear interpolation can be used for interpolation of positions, and spherical interpolation should be used for interpolation of quaternions. After the processing is completed, the time stamp alignment operation of several sensors is completed.

[0042] Please refer to Figure 2 , when performing spatial calibration processing on the collected data, the hand-eye calibration method AX = XB is adopted, where A represents the data of one sensor, B represents the data of another sensor, and X is the external parameter matrix between the two sensors. The external parameter matrix X usually consists of translation and rotation components, with a total of six degrees of freedom, that is, six groups of equations need to be solved.

[0043] Further, when solving the six groups of equations, the amount of odometer data is much greater than 6, forming an overdetermined system of equations. A residual model of argmin|AX = XB| is established, and the residual is minimized through an optimization method to find the extrinsic parameters of the two sensors. After pairwise calibration, the spatio-temporal synchronization of the original data is completed.

[0044] S3. Perform real-time projection correction on the sensor data of the lidar, IMU sensor, and wheel encoder.

[0045] According to the spatio-temporal calibration results, convert the IMU and lidar data to the chassis coordinate system to complete the unification of coordinate system data. After that, the data for fusion processing are all the converted data. According to the inclination data in the IMU, perform projection correction on the chassis odometer and lidar data, decompose the data in three-dimensional space, and use the horizontal plane as the reference to project the data onto the horizontal plane to complete the data correction.

[0046] S4. Obtain the plane scan data of the surrounding environment through a single-line lidar, and obtain the displacement information of the robot according to the tightly coupled odometer method; use the laser inter-frame matching method for ICP registration to obtain the laser odometer data; and obtain the front-end laser submap according to the laser odometer data.

[0047] Specifically, obtain the plane scan data of the surrounding environment through a single-line lidar, obtain the displacement information of the robot according to the tightly coupled method of odometer and IMU information. Use the displacement information of the robot between two frames of lidar data as the initial value, and use the laser inter-frame matching method to perform ICP registration on the front and rear frame laser data to obtain the laser odometer data. According to this odometer data, key frames are selected at certain intervals, and the laser data is superimposed to obtain the front-end laser submap.

[0048] Further, the front-end laser submap contains laser data with a fixed number of frames. When the submap is filled, several frames of data with relatively close spatial distances in the submap are selected, and laser ICP registration is performed again to achieve submap loop closure. Using the position information of the laser in the submap as nodes and the odometer and loop closure information as edge constraints, perform nonlinear optimization to correct the displacement relationship between the nodes. This can reduce the cumulative error caused by laser registration and obtain globally consistent laser key frame data inside the submap.

[0049] S5. Obtain the submap information output by the front end, convert the submap to the prior map coordinate system according to the initial positioning, and perform feature extraction; obtain the displacement information of the robot according to the tightly coupled method of odometer and IMU information.

[0050] Further, using the displacement information of the robot as the prior initial value, the submap data as the source point cloud, and the map data as the target point cloud. Due to the particularity of the indoor building scene, walls and other are fixed obstacles with relatively smooth surface features, while construction workers, scaffolding, etc. are dynamic obstacles, which have a certain impact on the accuracy of laser ICP registration.

[0051] Obtain the semantic information of the point cloud, calculate the curvature and normal vector information of the submap and the global map point cloud, and divide the data into planes, internal and external corners, and dynamic obstacles according to the curvature distribution.

[0052] If the curvature is small and the change is relatively gentle, it is marked as a plane point; if two planes intersect and the points at the intersection where the normal vectors are perpendicular to each other are marked as internal and external corner points; the remaining points with obvious curvature changes are marked as dynamic obstacle points. This classification method is relatively easy to implement and can meet the positioning requirements with high real-time requirements, so it is applicable to construction operations.

[0053] S6. According to the source point cloud and target point cloud data of the data, use the laser inter-frame matching method to perform weighted laser ICP registration on the two-frame data. Among them, plane points and internal and external corner points are given higher weight coefficients, and the weight of dynamic obstacle points is smaller, and the positioning information based on lidar can be obtained. By performing weighted laser ICP registration, laser odometer information, laser submap positioning information, chassis odometer information, and IMU information are obtained.

[0054] Next, perform positioning data fusion. Since the multi-sensor fusion of the current information is affected by the data quality of the current frame, in the construction site scenario, the abnormal occurrence frequency of sensor information is relatively high, which is likely to cause the decline of the positioning output accuracy and stability. The present invention adopts the input of a multi-sensor fusion module based on a sliding window, which can effectively improve the positioning stability in a high-dynamic harsh environment and obtain high-precision positioning data at the same time.

[0055] S7. Add the laser odometer information, laser submap positioning information, chassis odometer information, and IMU information of a fixed length to the information queue; use the residual term equation to construct a window optimization model.

[0056] When constructing the window optimization model, represent the optimization form as J T Q -1 JΔx = -J T ∑r, where r represents the residual term, J represents the Jacobian matrix of the residual with respect to the state quantity, and Q -1 is the noise matrix. In the positioning system, the residual terms include: the residual of the submap matching result and the optimization quantity, the residual of the laser odometer and the optimization quantity, the residual of the chassis odometer and the optimization quantity, the residual of the IMU pre-integrated odometer and the optimization variable, and the residual term brought by the marginalized prior factor.

[0057] S8. Marginalize the tail of the data queue according to unilateral constraints and bilateral constraints, and obtain the marginal prior factor through calculation.

[0058] The factor corresponding to the subgraph matching and the residual of the optimization variable is the map prior information. One factor constrains one pose, which is a unilateral constraint. The residuals of the laser odometer and the optimization quantity, the residuals of the chassis odometer and the optimization quantity, and the residuals of the IMU pre-integrated odometer and the optimization variable. The factors corresponding to these three items are odometer information. One factor constrains two adjacent poses, which is a bilateral constraint.

[0059] The marginalization processing formula is AX = b, which is decomposed into the following form:

[0060]

[0061] Among them, X i is the edge frame data, and X j is the latest frame of data. Through the Schur complement transformation, an equation related only to X j is obtained:

[0062] It is possible to obtain the marginal prior factor without relying on the edge frame data.

[0063] S9. According to the marginal prior factor, use the optimization method to solve the system state information and obtain the optimal estimate of the current state. Add the latest frame of data, and continue to repeat steps S7 to S8 to continuously update the old frames, add new frames, and realize the forward sliding of the window. Integrate the positioning information of multiple sensors, output the real-time position and attitude information of the robot, and realize the high-precision positioning function of the construction mobile robot operation.

[0064] Multi-sensor fusion refers to calibrating and aligning the time and space of different sensors, realizing data complementarity according to corresponding technologies, and outputting more accurate and high-frequency three-dimensional pose information of the robot. In the construction scenario, the robot construction operation needs to achieve centimeter-level high-precision positioning. However, the challenges are that the environment is relatively complex and changeable, the ground conditions are usually poor, it is easy to slip, and the environmental information is unstable, which is a high-dynamic scenario.

[0065] The multi-sensor fusion positioning technology with lidar, inertial measurement unit and wheel encoder as the core of the present invention can realize centimeter-level high-precision positioning of the robot in a complex construction scenario and improve the quality of construction operations. Applying the multi-sensor fusion positioning technology to the construction scenario can ensure the stability and accuracy of high-precision positioning, and provide effective positioning perception data for the construction robot operation.

[0066] The specific embodiments described above further elaborate on the purpose, technical solutions, and beneficial effects of the present invention. It should be understood that the above description is only the specific embodiments of the present invention and is not used to limit the protection scope of the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention shall be included within the protection scope of the present invention.

Claims

1. A multi-sensor fusion positioning method applicable to building scenarios, characterized in that, Including the following steps: S1. Install a lidar, an IMU sensor, and a wheel encoder in the chassis of the mobile robot. Place the robot in a room with a flat ground and enclosed on all four sides. The mobile robot randomly moves the chassis to collect the output information of the lidar, IMU, and wheel encoder. S2. Perform time synchronization processing on the collected data and perform spatial calibration processing on the collected data. S3. Perform real-time projection correction on the sensor data of the lidar, IMU sensor, and wheel encoder. S4. Obtain the planar scan data of the surrounding environment through a single-line lidar, and obtain the displacement information of the robot according to the tightly coupled odometry method; perform ICP registration using the laser inter-frame matching method to obtain the laser odometry data; and obtain the front-end laser sub-map according to the laser odometry data. Obtain the planar scan data of the surrounding environment through a single-line lidar, and obtain the displacement information of the robot according to the tightly coupled method of odometry and IMU information. Use the displacement information of the robot between two frames of lidar data as the initial value, and perform ICP registration on the front and rear frame laser data using the laser inter-frame matching method to obtain the laser odometry data. According to the odometry data, select key frames at a certain interval, perform superposition processing on the laser data, and obtain the front-end laser sub-map. The front-end laser sub-map contains a fixed number of frames of laser data. When the sub-map is filled, select several frames of data with relatively close spatial distances in the sub-map, perform laser ICP registration again to achieve sub-map loop closure. Use the position information of the laser in the sub-map as nodes, and the odometry and loop closure information as edge constraints to perform non-linear optimization to correct the displacement relationship between nodes. S7. Obtain the sub-map information output by the front end, convert the sub-map to the prior map coordinate system according to the initial positioning, and perform feature extraction; obtain the displacement information of the robot according to the tightly coupled method of odometry and IMU information. S8. According to the source point cloud and target point cloud data of the data, perform weighted laser ICP registration on the two frames of data using the laser inter-frame matching method; obtain laser odometry information, laser sub-map positioning information, chassis odometry information, and IMU information. S9. Add the laser odometry information, laser sub-map positioning information, chassis odometry information, and IMU information of a fixed length to the information queue; construct a window optimization model using the residual term equation. S10. Perform marginalization processing on the tail of the data queue according to the unilateral constraint and bilateral constraint, and obtain the marginalization prior factor through calculation. S11. According to the marginalization prior factor, use the optimization method to solve the system state information to obtain the optimal estimate of the current state; add the latest frame of data, continue to repeat steps S7 to S8, continuously update the old frames, add new frames, and realize the forward sliding of the window; fuse the positioning information of multiple sensors and output the real-time position and attitude information of the robot.

2. The multi-sensor fusion positioning method applicable to building scenarios according to claim 1, characterized in that: In step S1, perform registration on the laser data using the inter-frame matching method to obtain the laser odometry information; obtain the IMU odometry information using the IMU pre-integration method; sample and integrate the wheel encoder to obtain the chassis odometry information.

3. The multi-sensor fusion positioning method applicable to building scenarios according to claim 1, characterized in that: In step S2, when performing time synchronization processing on the collected data, the laser odometer data is used as the time reference to synchronize the chassis odometer and IMU odometer information; for the chassis odometer, find the two frames of data closest to the i-th frame lidar timestamp before and after, and perform linear interpolation based on the time difference from the i-th frame data to obtain the chassis odometer data synchronized with the i-th frame; the same interpolation synchronization operation is adopted for the IMU odometer information.

4. The multi-sensor fusion positioning method applicable to building scenarios according to claim 1, characterized in that: In step S2, when performing spatial calibration processing on the collected data, the hand-eye calibration method AX = XB is used, where A represents the data of a certain sensor, B represents the data of another sensor, and X is the extrinsic parameter matrix between the two sensors. The extrinsic parameter matrix X consists of translation and rotation components and has a total of six degrees of freedom, that is, six sets of equations need to be solved.

5. The multi-sensor fusion positioning method applicable to building scenarios according to claim 4, characterized in that: When solving the six sets of equations, an overdetermined system of equations is formed, and a residual model of argmin|AX = XB| is established. The residual is minimized through an optimization method to find the extrinsic parameters of the two sensors. After pairwise calibration, the spatio-temporal synchronization of the original data is completed.

6. The multi-sensor fusion positioning method applicable to building scenarios according to claim 1, characterized in that: In step S3, according to the spatio-temporal calibration result, the IMU and lidar data are converted to the chassis coordinate system to complete the unification of coordinate system data. After that, the data for fusion processing are all the converted data; according to the tilt angle data in the IMU, the chassis odometer and lidar data are corrected. The data is decomposed in three-dimensional space, and with the horizontal plane as the reference, the data is projected onto the horizontal plane to complete the data correction.

7. The multi-sensor fusion positioning method applicable to building scenarios according to claim 1, characterized in that: In step S5, using the displacement information of the robot as the prior initial value, the submap data as the source point cloud, and the map data as the target point cloud, the semantic information of the point cloud is obtained, and the curvature and normal vector information of the submap and global map point clouds are calculated. And according to the curvature distribution, the data is divided into planes, internal and external corners, and dynamic obstacles.

8. The multi-sensor fusion positioning method applicable to building scenarios according to claim 1, characterized in that: In step S8, the factors corresponding to the residuals of submap matching and optimization variables are the map prior information. One factor constrains one pose, which is a unilateral constraint; the residuals of the laser odometer and the optimization quantity, the residuals of the chassis odometer and the optimization quantity, and the residuals of the IMU pre-integrated odometer and the optimization variables. The factors corresponding to these three items are the odometer information. One factor constrains two adjacent poses, which is a bilateral constraint; the marginalization processing formula is AX = b, which is disassembled into the following form: ； Among them, X i is edge frame data, and X j is the latest frame data. Through Schur complement transformation, an equation related only to X j is obtained: 。

Images