Nuclear industry complex environment multi-sensor fusion point cloud reconstruction method and system

Abstract

The invention discloses a nuclear industry indoor complex environment point cloud reconstruction method and a nuclear industry indoor complex environment point cloud reconstruction system based on multi-sensor fusion, which are fused with multi-sensing information such as images, laser radar point cloud, IMU (Inertial Measurement Unit) and the like. A Visual nuclear industry complex environment multi-sensor fusion point cloud reconstruction method and a Visual nuclear industry complex environment multi-sensor fusion point cloud reconstruction system Inertial nuclear industry complex environment multi-sensor fusion point cloud reconstruction method and a Visual nuclear industry complex environment multi-sensor fusion point cloud reconstruction system Odometer are introduced into a system framework, and VIO information is output; a laser radar inertial odometer (LIO, Lidar nuclear industry complex environment multi-sensor fusion point cloud reconstruction method, a system Inertial nuclear industry complex environment multi-sensor fusion point cloud reconstruction method and a system Odometer) system are used for outputting LIO information; the distribution weight of the VIO and the distribution weight of the LIO are adjusted according to the failure condition of the odometer system in the environment, a loose coupling graph optimization method is used for jointly optimizing VIO and LIO weighting strategies, and a coupling pose is generated. Coupling poses meeting key frame conditions are selected, radar point clouds corresponding to the coupling poses are converted into a world coordinate system, a global point cloud map is formed, and indoor three-dimensional point cloud reconstruction is achieved. According to the method, the camera, the laser radar and the IMU sensor are fused, the situation that a single sensor fails can be dealt with, the point cloud reconstruction precision of the nuclear industry indoor complex environment is improved, and the application prospect of the whole industry is supplemented.

Description

Technical Field

[0001] The present invention relates to the field of indoor point cloud reconstruction in nuclear industry, and in particular to a method and system for reconstructing point cloud in complex environment of nuclear industry by fusion of multiple sensors. Background Art

[0002] Multi-sensor fusion point cloud reconstruction method and system for complex environment in nuclear industry The indoor point cloud model of nuclear industry can provide detailed environmental perception information for robots, so that robots can accurately identify and locate objects in the environment, plan safe and efficient paths in complex environments, and avoid collisions and dangerous areas; secondly, through high-precision point cloud models, robotic arms can perform complex operation tasks and avoid the risks caused by misoperation to the greatest extent; complex lighting conditions make it difficult for pure visual sensors to capture clear features for matching and thus complete point cloud reconstruction; in addition, the high reflectivity of indoor metal facilities in the nuclear industry makes it easy for lidar to produce errors during the measurement process, affecting the reconstruction accuracy; therefore, combining multi-sensor fusion technology to develop an accurate and efficient point cloud reconstruction system is crucial to improving the perception ability and overall work efficiency of robots in the nuclear industry.

[0003] Existing point cloud reconstruction methods can be roughly divided into two categories:

[0004] 1) Point cloud reconstruction method based on VIO-SLAM; feature point detection algorithm is used to extract feature points in the image and match feature points between consecutive frames; the camera pose is calculated using the PnP algorithm and optimization method through the matched feature points; the point cloud map of the environment is gradually generated using the camera pose and the depth information of the feature points.

[0005] 2) Point cloud reconstruction method based on LIO-SLAM; by extracting features such as planes, edges and corners from the point cloud, the newly collected point cloud is matched with the local map using the point cloud registration algorithm to determine the position and posture of the robot; combining the lidar point cloud and IMU data, global consistency adjustment is performed through methods such as Kalman filter or factor graph optimization to generate an accurate global point cloud map.

[0006] Multi-sensor fusion point cloud reconstruction method and system for complex environment in nuclear industry The point cloud reconstruction method process based on VIO-SLAM is as follows Figure 1 As shown in the figure; firstly, a continuous image sequence in the environment is obtained by using a monocular, binocular or RGB-D camera, feature points in the image are extracted by a feature point detection algorithm, and feature points are matched between consecutive frames; the camera posture is calculated by using the PnP algorithm and optimization method through the matched feature points; the point cloud map of the environment is gradually generated by using the camera posture and the depth information of the feature points; combined with the global optimization method, the accuracy and consistency of the point cloud reconstruction map are further improved.

[0007] The shortcomings of the point cloud reconstruction method based on VIO-SLAM are mainly the following two points:

[0008] 1) VIO-SLAM is very sensitive to lighting changes. In dark, reflective and weak-textured environments, the camera’s imaging quality is seriously affected, resulting in difficulties in feature point detection and matching, and poor reconstruction results.

[0009] 2) The artificially formulated feature extraction and feature matching algorithms have high computational complexity, especially in high-resolution images and large-scale environments, which can easily lead to a decrease in the real-time performance of the system.

[0010] Point cloud reconstruction method based on LIO-SLAM; use lidar to obtain laser point cloud data of the environment, and IMU to provide inertial data; perform distortion correction on lidar point cloud data based on IMU information; extract features such as planes, edges, and corners from the point cloud, and match the newly collected point cloud with the local map through a point cloud registration algorithm to determine the position and posture of the robot; combine lidar point cloud and IMU data, and perform global consistency adjustment through methods such as Kalman filter or factor graph optimization to reduce cumulative errors and generate an accurate global point cloud map.

[0011] The shortcomings of the point cloud reconstruction method based on LIO-SLAM are mainly the following two points:

[0012] 1) In the point cloud reconstruction method based on LIO-SLAM, the mirror reflection of the laser radar in the indoor environment is too high, and the echo energy may be small, resulting in measurement errors in the point cloud.

[0013] 2) In the point cloud reconstruction method based on LIO-SLAM, the amount of laser radar point cloud data is large, reaching hundreds of thousands to millions per second, with high processing and storage requirements, requiring powerful computing power and efficient data processing algorithms. Summary of the invention

[0014] The technical problem to be solved by the present invention is to provide a method and system for reconstructing point cloud of complex nuclear industry indoor environment based on multi-sensor fusion in view of the shortcomings of the existing technology, so as to quickly and accurately perform point cloud modeling.

[0015] In order to solve the above technical problems, the technical solution adopted by the present invention is: a method for reconstructing multi-sensor fusion point cloud in complex environment of nuclear industry, comprising the following steps:

[0016] A multi-sensor fusion point cloud reconstruction method for complex nuclear industry environments comprises the following steps: (1) integrating a line feature extraction module into a visual inertial odometer system to obtain VIO information; using a laser radar inertial odometer system to obtain LIO information; performing failure judgment on the VIO and LIO according to speed limit, angle limit and displacement limit, and assigning weights to the VIO and LIO according to the failure conditions.

[0017] (2) The VIO, LIO and weight information are jointly optimized using loosely coupled graph optimization to obtain a coupled pose, and the radar point cloud frame corresponding to the coupled pose that meets the requirements is selected as a key frame using the key frame standard, and the coupled pose that does not meet the key frame standard is eliminated. The point cloud corresponding to the key frame pose is converted to the world coordinate system and added to the global point cloud map.

[0018] The method of the present invention not only adds a mid-line feature extraction module to the VIO system, but also jointly optimizes the pose through loosely coupled graph optimization, thereby ensuring that the SLAM system can work stably when a single sensor fails, thereby improving the stability of the multi-sensor SLAM system and the accuracy of point cloud modeling.

[0019] The specific implementation process of step 1) includes:

[0020] 1) Input images, lidar point clouds, and IMU data;

[0021] 2) Obtain LIO information through the lidar inertial odometer system;

[0022] 3) extracting line features from the image, integrating the line features into the feature extraction module of the visual inertial odometer system, and jointly estimating the pose with the point features to obtain VIO information;

[0023] 4) Using failure conditions such as speed, angle and displacement limits to judge the failure of the laser radar inertial odometer information and the visual inertial odometer information, and assigning weights to the laser inertial odometer and the visual inertial odometer;

[0024] From the above process, it can be seen that the present invention extracts and matches features based on point and line features, thereby improving the accuracy of VIO information of the visual inertial odometer system in a feature-less environment; multi-sensor fusion point cloud reconstruction method and system for complex nuclear industry environments

[0025] The line features in step 3) are extracted by the LSD algorithm. As shown in the figure, the LSD algorithm extraction process is as follows:

[0026] Calculate the image gradient to identify edge information, and use the gradient direction and amplitude to detect line segments;

[0027]

[0028]

[0029] According to the gradient direction, adjacent pixels are clustered into initial line segment candidate regions with similar directions;

[0030] |θ(x i,i )-(x j,j )<∈

[0031] The candidate region is verified to ensure that it is a straight line segment, and the region that does not meet the conditions is filtered using geometric constraints.

[0032] Linear regression is used to accurately locate the verified line segments, and the sub-pixel positions of the line segments are calculated to improve the detection accuracy. Finally, the straight line segments are generated and their parameters are output.

[0033]

[0034]

[0035] The specific implementation method of the speed limit in step 4) is: if the inter-frame speed of the two system odometers is greater than the threshold value v1, the speed difference between the two is greater than the threshold value v2, and the odometer data with a smaller speed and not stationary is selected as the output value; the specific implementation method of the angle limit is: when the roll angle and pitch angle of the lidar inertial odometer change too much, reduce the weight of the lidar inertial odometer; when the pitch angle is close to 90°, the camera is facing the top (there is light on the top, and the overexposure is serious), reduce the weight of the visual inertial odometer; the specific implementation method of the displacement limit is: when moving a long distance along the height direction of the lidar, the height direction estimation result of the laser odometer is inaccurate (in the indoor environment, the height direction lacks significant features, and the lidar is prone to degradation when moving along the height direction), reduce the weight of the lidar inertial odometer.

[0036] The specific implementation process of step 2) includes:

[0037] 1) performing loosely coupled graph optimization on the VIO, LIO and weight information to obtain a coupled pose;

[0038] 2) Using the key frame standard, the coupling poses that meet the requirements are selected as key frame poses, and the coupling poses that do not meet the key frame standard are eliminated;

[0039] 3) Convert the lidar point cloud corresponding to the key frame pose to the world coordinate system to form a global point cloud map.

[0040] This paper introduces loosely coupled graph optimization to jointly optimize LIO and VIO, which increases the robustness and accuracy of the odometer system. The coupled pose in graph optimization is expressed as follows:

[0041]

[0042] Among them, x i and x j represents the position; h(x i,j ) represents the position x i In place x j The predicted measurement value of is the information matrix, which represents the uncertainty of the measurement; is the measurement value of the visual odometry; is the measurement value of the lidar odometry.

[0043] In step 1), various observation data are connected to the optimization variables through corresponding constraints to form a graph structure. The constructed graph optimization structure is as follows:

[0044] The position fixity constraint connects the visual and laser odometry observations to the position optimization variables, ensuring that the position optimization variables are consistent with the observations.

[0045]

[0046] The angle fixity constraint connects the visual and laser odometry observation data to the angle optimization variable, ensuring that the angle optimization variable is consistent with the observation data.

[0047]

[0048] Speed ​​physical constraints constrain speed optimization variables according to the physical model to ensure that the speed optimization variables conform to the physical model.

[0049]

[0050] The angular velocity physical constraint constrains the angular velocity optimization variables according to the physical model to ensure that the angular velocity optimization variables conform to the physical model.

[0051]

[0052] The present invention utilizes the key frame standard to eliminate a large number of redundant similar postures, obtain representative postures, and reduce the information redundancy problem of the system when it is stationary or at a low speed.

[0053] The specific implementation process of point cloud reconstruction includes: obtaining the coupled pose through loosely coupled graph optimization, selecting the key frame pose in the coupled pose, and projecting the point cloud into the world coordinate system according to the key frame pose to form a point cloud model.

[0054] Accordingly, the present invention also provides a point cloud reconstruction system for complex indoor environments in the nuclear industry based on multi-sensor fusion, comprising a computer device; the computer device is programmed or configured to be used for the steps of the method described in the present invention.

[0055] Compared with the prior art, the present invention has the following beneficial effects:

[0056] 1. Line features are added to the feature extraction module of the VIO system, which improves the robustness and accuracy of the VIO system's visual odometer in feature-deficient environments.

[0057] 2. Use a loosely coupled graph optimization system to jointly optimize VIO and LIO, so that they can still work normally when any sub-odometer system fails in the indoor environment of the nuclear industry, thereby improving the stability of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 This is a flow chart of the point cloud reconstruction method based on VIO-SLAM;

[0059] Figure 2 This is a flow chart of the point cloud reconstruction method based on LIO-SLAM;

[0060] Figure 3 This is a schematic diagram of the architecture of the point cloud reconstruction system for complex indoor environments in the nuclear industry based on multi-sensor fusion;

[0061] Figure 4 It is a schematic diagram of line feature extraction;

[0062] Figure 5 It is a schematic diagram of loose coupling method;

[0063] Figure 6 It is a schematic diagram of the graph optimization method;

[0064] Figure 7 This is a schematic diagram of the point cloud reconstruction effect;

[0065] Figure 8 This is a schematic diagram of the core process of the present invention. DETAILED DESCRIPTION

[0066] The following is a detailed description of the specific implementation of the present invention in conjunction with the accompanying drawings: The system architecture of the present invention is as follows: Figure 3As shown. Images, lidar point clouds and inertial information are obtained through cameras, lidars and IMUs respectively, and line feature extraction modules are integrated into the visual inertial odometer system to obtain VIO information; LIO information is obtained using the lidar inertial odometer system; failure judgments are made on the VIO and LIO according to speed limits, angle limits and displacement limits, and weights are assigned to the VIO and LIO according to the failure conditions. The VIO, LIO and weight information are jointly optimized using loosely coupled graph optimization to obtain the coupled pose, and the radar point cloud frames corresponding to the coupled poses that meet the requirements are selected as key frames using the key frame standard, and the coupled poses that do not meet the key frame standard are eliminated. The point clouds corresponding to the key frame poses are converted to the world coordinate system and added to the global point cloud map.

[0067] The line feature extraction process in the VIO system feature extraction module is as follows: Figure 4 As shown, the main method steps of this module are as follows:

[0068] 1) Input image data

[0069] The image data of the present invention is obtained by a binocular camera, and the image under the relevant path is obtained by simulating the running trajectory of the robot indoors.

[0070] 2) Calculate the gradient of the image to identify edge information and use the gradient direction and amplitude to perform line segment detection.

[0071]

[0072]

[0073] 3) According to the gradient direction, adjacent pixels are clustered into initial line segment candidate regions with similar directions.

[0074] |θ(x i,i )-(x j,j )<∈

[0075] 4) Verify the candidate area to ensure that it is a straight line segment, and use geometric constraints to filter out areas that do not meet the conditions.

[0076] 5) Accurately locate the verified line segments through linear regression and calculate the sub-pixel position of the line segments to improve the detection accuracy. Finally, the straight line segments are generated and their parameters are output.

[0077]

[0078]

[0079] The loose coupling method used in the present invention is as follows: Figure 5As shown in the figure, the loose coupling method is to obtain the final pose output of each system through the VIO and LIO, and then optimize the final pose through the graph optimization criterion, so as to ensure that the SLAM system can work stably when a single sensor fails, thereby improving the stability of the multi-sensor SLAM system and the accuracy of point cloud modeling.

[0080] The graph optimization method used in the present invention is shown in Figure 6. The laser odometer and the visual odometer are combined with weight information to perform graph optimization to obtain a coupled pose. The main components of this module are:

[0081] Various observation data (visual and laser odometer) are connected to the optimization variables through corresponding constraints to form a graph structure. The coupled pose in graph optimization is expressed as follows:

[0082]

[0083] Among them, x i and x j represents the position; h(x i,j ) represents the position x i In place x j The predicted measurement value of is the information matrix, which represents the uncertainty of the measurement; is the measurement value of the visual odometry; is the measurement value of the lidar odometry.

[0084] The position fixity constraint (1) connects the visual and laser odometry observation data to the position optimization variables, ensuring that the position optimization variables are consistent with the observation data.

[0085]

[0086] The angle fixity constraint (2) connects the visual and laser odometry observation data to the angle optimization variable, ensuring that the angle optimization variable is consistent with the observation data.

[0087]

[0088] Speed ​​physical constraints (3) constrain the speed optimization variables according to the physical model to ensure that the speed optimization variables conform to the physical model.

[0089]

[0090] Angular velocity physical constraints (4) constrain the angular velocity optimization variables according to the physical model to ensure that the angular velocity optimization variables conform to the physical model.

[0091]

[0092] The key frame standard is used to select the coupling poses that meet the requirements as the key frame poses, and the detailed method for eliminating the coupling poses that do not meet the key frame standard is as follows:

[0093] Calculate the quaternion and translation vector of the current frame and the previous key frame. When the quaternion variable or translation vector is greater than the set threshold, it is identified as a key frame.

[0094]

[0095]

[0096] Add the point cloud corresponding to the key frame pose to the world coordinate system, filter the world coordinate system map and finally realize point cloud modeling

[0097] The core process of the present invention is as follows Figure 7 shown.

[0098] The present invention builds a system platform based on 64-bit Ubuntu20.04 and Ros1, uses C++ programming language to write system programs, and the hardware equipment uses 32-line laser radar, binocular depth camera and 9-axis IMU.

[0099] Comparison of existing methods

[0100] The present invention will be compared with two existing methods to verify the accuracy of point cloud modeling of the present invention. The two existing methods are ORB-SLAM and LIO-SAM, and all methods are tested on the same data set. Figure 7 The reconstruction effect diagram of each method. The receiving slot reconstructed by LIO-SAM is seriously missing, and no parameter quantification comparison evaluation is performed. Table 1 is the size comparison of the measurement results of each method and the actual measurement results. Table 2 is the error distribution of each method compared with the true value. Compared with the existing methods, the present invention integrates the line feature module and uses loosely coupled graph optimization to fuse multimodal data. It has better robustness and stability in the overall odometer, and is therefore superior to the existing methods in terms of point cloud modeling accuracy.

[0101] Table 1 Size comparison of existing methods

[0102]

[0103] Table 2 Comparison of errors of existing methods

Claims

1. A multi-sensor fusion point cloud reconstruction method for complex nuclear industry environments, characterized by: The following steps are involved: (1) A line feature extraction module is integrated into the visual inertial odometry system to obtain VIO information; a lidar inertial odometry system is used to obtain LIO information; a failure judgment is performed on the VIO and LIO according to speed limit, angle limit and displacement limit, and weights are assigned to the VIO and LIO according to the failure conditions. (2) The VIO, LIO and weight information are jointly optimized using loosely coupled graph optimization to obtain a coupled pose, and the radar point cloud frame corresponding to the coupled pose that meets the requirements is selected as a key frame using the key frame standard, and the coupled pose that does not meet the key frame standard is eliminated. The point cloud corresponding to the key frame pose is converted to the world coordinate system and added to the global point cloud map.

2. According to claim 1, a multi-sensor fusion point cloud reconstruction method for complex nuclear industry environments is characterized by: The specific implementation process of step (1) includes: 1) Input images, lidar point clouds, and IMU data; 2) Obtain LIO information through the lidar inertial odometer system; 3) extracting line features from the image, integrating the line features into the feature extraction module of the visual inertial odometry system, and jointly estimating the pose with the point features to obtain VIO information; 4) Using failure conditions such as speed, angle and displacement limits to judge the failure of the laser radar inertial odometer information and the visual inertial odometer information, assigning weights to the laser inertial odometer and the visual inertial odometer to obtain VIO and LIO weight information.

3. The method for multi-sensor fusion point cloud reconstruction in complex nuclear industry environment according to claim 2 is characterized by: The line features in step 3) are extracted by LSD algorithm, and the LSD algorithm extraction process is as follows: First, the image gradient is calculated to identify edge information, and line segment detection is performed using the gradient direction and amplitude; According to the gradient direction, adjacent pixels are clustered into initial line segment candidate regions with similar directions; |θ(x i ,y i )-θ(x j ,y j )|<∈ Verifying the candidate area to ensure that it is a straight line segment, and filtering out areas that do not meet the conditions using geometric constraints; The verified line segments are accurately located by linear regression, and the sub-pixel positions of the line segments are calculated to improve the detection accuracy; finally, straight line segments are generated.

4. The method for multi-sensor fusion point cloud reconstruction in complex nuclear industry environment according to claim 2 is characterized by: The specific implementation method of the speed limit in step 4) is: if the inter-frame speed of the two system odometers is greater than the threshold value v1, the speed difference between the two is greater than the threshold value v2, and the odometer data with a smaller speed and not stationary is selected as the output value; the specific implementation method of the angle limit is: when the roll angle and pitch angle of the lidar inertial odometer change too much, the weight of the lidar inertial odometer is reduced; when the pitch angle is close to 90°, the camera is facing the top, and the weight of the visual inertial odometer is reduced; the specific implementation method of the displacement limit is: when moving a long distance along the height direction of the lidar, the height direction estimation result of the laser odometer is inaccurate, and the weight of the lidar inertial odometer is reduced.

5. The method for multi-sensor fusion point cloud reconstruction in complex nuclear industry environment according to claim 1, characterized in that: The specific implementation process of step (2) includes: 5) The VIO and LIO are combined with weight information to perform loosely coupled graph optimization to obtain a coupled pose; 6) Using the key frame standard, select the coupling poses that meet the requirements as the key frame poses, and eliminate the coupling poses that do not meet the key frame standard; 7) Convert the lidar point cloud corresponding to the key frame pose to the world coordinate system to form a global point cloud map.

6. The method for multi-sensor fusion point cloud reconstruction in complex nuclear industry environment according to claim 2 is characterized by: The loose coupling method in step 4) is to obtain the respective system pose outputs through the VIO and LIO, and then optimize the final pose through the graph optimization criterion.

7. The method for multi-sensor fusion point cloud reconstruction in complex nuclear industry environment according to claim 5, characterized in that: The specific implementation of the graph optimization method in step 5) is as follows: various observation data are connected to the optimization variables through corresponding constraints to form a graph structure; the coupled pose in the graph optimization is expressed as follows: Among them, x i and x j represents the position; h(x i ,x j ) represents the position x i In place x j The predicted measurement value of is the information matrix, which represents the uncertainty of the measurement; is the measurement value of the visual odometry; is the measurement value of the lidar odometry; The position fixity constraint connects the visual and laser odometer observation data with the position optimization variable to ensure that the position optimization variable is consistent with the observation data; The angle fixation constraint connects the visual and laser odometer observation data with the angle optimization variable to ensure that the angle optimization variable is consistent with the observation data; Speed ​​physical constraints constrain speed optimization variables according to the physical model to ensure that the speed optimization variables conform to the physical model; The angular velocity physical constraint constrains the angular velocity optimization variables according to the physical model to ensure that the angular velocity optimization variables conform to the physical model; 8. The method for multi-sensor fusion point cloud reconstruction in complex nuclear industry environment according to claim 5 is characterized by: The specific implementation method of the key frame standard in step 6) is: calculating the quaternion and translation vector of the current frame and the previous key frame, and when the quaternion variable or the translation vector is greater than a set threshold, it is identified as a key frame; 9. A multi-sensor fusion point cloud reconstruction system for complex nuclear industry environments, characterized by: The system comprises a computer device; the computer device is programmed or configured to execute the steps of the method according to any one of claims 1 to 8.

Images