Multi-source information fusion outdoor inspection scene three-dimensional color geometric model reconstruction method

Abstract

The invention belongs to the technical field of robotics and computer vision, and relates to a multi-source information fusion outdoor inspection scene three-dimensional color geometric model reconstruction method, which comprises the following steps: (1) inertial measurement unit IMU internal reference calibration initialization, (2) laser radar and inertial measurement unit IMU combined external reference calibration, (3) state estimation, (4) using an i-Octree structure to store an updated map, and (5) performing multi-source information fusion. (5) carrying out loopback detection to solve the mileage drift problem; (6) carrying out data fusion of the laser radar and the color camera; and (7) carrying out 3D global positioning. The method has the advantages that the information of the laser radar and the information of the color camera are fused, the one-to-one mapping relation between each space point in the three-dimensional point cloud and a certain pixel point in the color image is established, visual information is more completely described, and the visual information is more accurately and accurately positioned by using a 3D global positioning method. And high-precision reconstruction of the three-dimensional color geometric model is realized.

Description

Technical Field

[0001] The invention relates to a method for reconstructing a three-dimensional color geometric model of an outdoor inspection scene by fusing multi-source information, and belongs to the technical field of robotics and computer vision. Background Art

[0002] Traditional security equipment can only work in fixed directions and fixed areas. Security tasks for large-area scenes have problems such as high deployment difficulty, high equipment cost, monitoring blind spots, and high risk of data leakage. Intelligent security robots can conduct autonomous patrols, and one of the core issues is the construction of patrol scene models. In the field of autonomous driving, high-precision map construction is the cornerstone for achieving correct vehicle decision-making and control, which is different from human driving because humans can rely on their highly accurate and generalized perception capabilities to drive vehicles, even without high-precision maps. However, current environmental perception algorithms cannot reach the level of humans, especially in complex urban traffic and extreme weather conditions, which may cause the algorithm to fail. High-precision maps can provide rich and beyond-line-of-sight information to help autonomous vehicles maintain the correct driving path under various conditions.

[0003] In recent years, the research on security robots has made great progress, and has achieved fruitful theoretical results in obstacle avoidance, path planning, autonomous navigation and human-computer interaction. However, most of these research results are oriented to indoor structured environments, such as homes, shopping malls, warehouses, etc. Outdoor environments such as campuses, factories and communities are key places for people's daily activities and are also key areas for security protection. However, the outdoor environment has a complex structure, strong dynamics, and a wide variety of physical properties and types of targets to be sensed. In particular, intelligent perception technology for outdoor unstructured environments such as campuses, factories and communities has always been a research difficulty in this field and has not been well solved so far. Summary of the invention

[0004] In order to overcome the shortcomings of the prior art, the purpose of the present invention is to provide a method for reconstructing a three-dimensional color geometric model of an outdoor inspection scene by fusing multi-source information. The method of the present invention is to fuse information collected by multiple sensors, respectively construct a global point cloud map and a local color point cloud map of the outdoor inspection scene, and then construct a global color point cloud map through global positioning technology to achieve reconstruction of the three-dimensional color geometric model of the outdoor inspection scene.

[0005] In order to achieve the above-mentioned invention object, the technical solution adopted by the present invention is: a method for reconstructing a three-dimensional color geometric model of an outdoor inspection scene by multi-source information fusion, comprising the following steps:

[0006] Step 1, inertial measurement unit IMU internal parameter calibration initialization, IMU internal parameters include IMU initial zero bias, measurement noise and initial gravity vector, which are important input parameters of the initial state and affect the accuracy and reliability of the filter. Use the static initialization method to get the initial value of the IMU internal parameter and directly add it to the program operation. The so-called static initialization is to calculate the IMU internal parameter when the moving subject is in a stationary state. Since the moving subject is not moving, the tiny measurement noise is usually ignored. It is believed that the IMU's gyroscope measurement value has only zero bias, and the IMU's accelerometer measurement value has only zero bias and gravity acceleration. It specifically includes the following sub-steps:

[0007] (a) At the beginning of recording the corresponding data set, the device is placed still for a period of time to wait for the static initialization calculation to succeed until it is less than the manually set threshold. The static initialization time is set to 10 seconds in the program;

[0008] (b) Find the mean of all IMU measurements during the static initialization time. Assume that the mean values ​​of the gyroscope and accelerometer measurements are and ;

[0009] (c) The relationship between the IMU measurement value and the true value is given by the formula Describe:

[0010]

[0011] in, and They represent the angular velocity and acceleration of the moving subject respectively, yes The abbreviation of the motion main system is usually the transformation from the motion main system to the world system. To express, Represents the acceleration due to gravity. Since the motion of the moving subject on the surface is affected by gravity, the accelerometer measurement value will be affected by the additional acceleration due to gravity. and They are the real acceleration and real angular velocity of the moving subject in the world system. The world system is usually converted from the longitude and latitude coordinate system to the UTM (Universal Transverse Mercator Grid System) coordinate system. The axis is usually parallel to the center of the earth, according to the formula It can be seen that when the moving subject is stationary, the true angular velocity and true acceleration are both zero, and the zero bias of the gyroscope is considered to be , the initial posture of the moving subject is positioned as the posture of the world system, that is, ,matrix represents the identity matrix, so the static measurement value of the accelerometer is . Relative to the acceleration due to gravity , It is considered to be a negligible amount, so it can be directly considered The direction is the acceleration due to gravity direction, and query the magnitude of gravity acceleration through the local longitude and latitude, or directly use 9.8 as the magnitude of gravity acceleration;

[0012] (d) Then substitute the gravity acceleration and subtract the effect of gravity acceleration from the mean value of the accelerometer measurement to get the accelerometer bias. Describe:

[0013]

[0014] Finally, the IMU zero bias is assumed to be constant and the IMU measurement noise variance is estimated;

[0015] Step 2: Joint external parameter calibration of LiDAR and IMU. External parameter calibration between LiDAR and IMU includes rotational external parameter, translational external parameter and time offset. Rotational external parameter and translational external parameter are caused by the inconsistency of sensor coordinate system, while time offset is caused by the inevitable transmission and processing delay before receiving data between LiDAR and IMU modules. It specifically includes the following sub-steps:

[0016] (a) The data preprocessing part mainly uses a non-causal zero-phase low-pass filter to filter out high-frequency noise, including IMU measurement noise and angular velocity and linear velocity noise in the radar odometer. Since the IMU data frequency is usually higher than the radar data frequency, the IMU data is linearly interpolated based on the time of the radar odometer to obtain the IMU data and radar data at the same time;

[0017] (b) Initialize the time offset, ignore the gyro bias, and obtain the characteristic that the angular velocity is independent of the external parameters. Use zero-center cross-correlation to quantify the similarity between the angular velocities of the radar odometer and the IMU to obtain the initial value of the time offset.

[0018] (c) Refine the time offset and calculate the residual time offset that is smaller than the resolution. At this time, the gyroscope zero bias and rotation extrinsic parameters need to be considered. Assuming that the angular acceleration in the residual time is constant, the rotation extrinsic parameters, the gyroscope zero bias, and the residual time offset are calibrated at the same time.

[0019] (d) Calibrate the remaining parameters, including the translation extrinsic parameter, the initial gravity vector, and the accelerometer bias, through the acceleration relationship between the two externally fixed coordinate systems;

[0020] Step 3: Perform state estimation. In order to facilitate the derivation of the formula, the manifold calculation symbol is introduced. for dimensional manifold, through two encapsulation operators and To build from The local neighborhood on to its tangent space The bidirectional mapping of It is to combine the elements on the manifold with the elements of the tangent space to obtain new elements on the manifold; It maps two elements on the manifold to the tangent space to represent the "difference" between them. The specific definition is given by the formula Describe:

[0021]

[0022] in, A special orthogonal group representing three-dimensional rotations (all elements are is an orthogonal matrix with a determinant of +1), express A rotation matrix in the group, represents a vector in a Lie algebra, represents the exponential mapping from Lie algebra to Lie group, which is used to convert Mapping back superior, yes The inverse operation of The elements on are mapped to the Lie algebra, and represents a normal vector;

[0023] Based on definition and Operator, set The index of the IMU measurement value is obtained by using a zero-order holder in the IMU sampling period. At, through the formula Discretize:

[0024]

[0025] In the formula, the first frame based on IMU is used as the global coordinate system (denoted as ), , Represents the position and attitude of the IMU in the global coordinate system, represents the gravity vector in the global coordinate system, , Represents the measurement value of IMU, , express , The measurement noise, , represents the IMU bias, represents the state quantity of the 24-dimensional system, represents the input control quantity, Represents the amount of noise. The IMU data and lidar data are tightly coupled by iterative extended Kalman filtering, which includes the following sub-steps:

[0026] (a) The state quantity is predicted by forward propagation of IMU data, using the formula is described, where the noise Set to zero, Represent the true value, propagated value, updated value, and the error between the true value and its estimated value, respectively. Represents the index of the measurement value immediately following the current measurement value.

[0027]

[0028] in, Indicates noise The covariance matrix of and The calculation is done by the formula Describe,

[0029]

[0030] The prediction process is continuously executed until the end of a frame of point cloud data. The prediction state at this time is recorded as , the corresponding covariance is , Indicates the moment;

[0031] (b) Use IMU data to dedistort the point cloud and move all points in each frame to the end moment for distortion removal;

[0032] (c) Residual calculation, search in the map managed by i-Octree The nearest point is then found, and a normal vector and centroid are fitted. A local facet patch is fitted using the normal vector and centroid, and the measurement equation is expanded by a first-order Taylor series to obtain the corresponding Jacobian matrix and residual.

[0033] (d) Iterative update, using the predicted state obtained in substep (a) and covariance For unknown status A prior Gaussian distribution is imposed, The covariance of the following error states is expressed by the formula Describe:

[0034]

[0035] in, For the Moment The state after the Kalman filter update is: is the error, express about The partial differential at , continuously perform iterative updates of the state quantity, and calculate the Kalman iteration state and Kalman gain , until the convergence condition is met , is the convergence threshold, and the final optimal state is obtained and covariance estimation , then transform the lidar points of each frame into the global coordinate system, and insert the transformed lidar points into the global map managed by i-Octree;

[0036] Step 4. Use the i-Octree structure to save and update the map. i-Octree does not retain all feature nodes, but only the nodes near the current radar, which reduces the data size. Each pose update dynamically adds and deletes nodes. When deleting a node, the i-Octree structure first checks whether the node is located in the given space. All nodes in the given space are directly deleted without searching for nodes in it, which significantly reduces the deletion time. In the iterative update, nodes are continuously recursively inserted and removed, and i-Octree is dynamically updated. In the residual calculation, the KNN nearest neighborhood algorithm is used to find the node in the tree. The distance between the nearest neighbor point and the target point, when the distance is less than the threshold, is regarded as an internal point and used for state estimation;

[0037] Step 5: Perform loop detection to solve the mileage drift problem and ensure the consistency of map construction. For continuous lidar frame data, select key frames with the same time interval and save them into the historical key frame queue and cyclically update the key frame factor graph. When it is subsequently detected that the trajectory distance difference between the current key frame and the historical key frame is greater than the threshold and the relative posture difference is less than the threshold, start loop detection, add the relevant key frames to the graph optimization algorithm, use the GTSAM library to perform graph optimization operations to correct the algorithm posture calculation results, and obtain the trajectory map of the moving subject and the 3D global point cloud map of the outdoor inspection scene through steps 2, 3, 4, and 5.

[0038] Step 6: Fusing the laser radar and color camera data, including the following sub-steps:

[0039] (a) Camera intrinsic calibration: Use the camera to collect 30 images of calibration plate data, and use MATLAB's calib toolbox to manually extract corner points from each calibration plate image data in the order of upper left, lower left, lower right, and upper right to obtain the camera intrinsic parameters;

[0040] (b) Extract edge features in the environment. First, divide the laser point cloud into multiple cubes with the same side length. Then use the random sampling consensus algorithm (RANSAC) to extract planes in each cube space. For spaces with more than 2 planes, calculate the intersection lines between these planes as point cloud edge features. This ensures that the extracted edges are depth-continuous. Then, project these edge features onto a 2D plane using the camera intrinsic parameters solved in the previous step. At the same time, use the Canny algorithm to extract edge features in the image.

[0041] (c) Match the extracted edge features, sample some points in the extracted edge, then project the points onto the image based on the currently estimated extrinsic parameters, and then find the nearest pixel in the edge of the image through the KD tree. point, and the direction vector of the radar edge is also projected onto the image to determine whether it is consistent with this point. The straight lines formed by the points are parallel to each other to filter out the false matches that are not parallel at all;

[0042] (d) External parameter optimization: The measurement noise of the image edge is the pixel position error. The measurement noise of the radar edge is divided into the beam direction error and the depth measurement error. The distance between corresponding points and the directional angle deviation between them are used as residual values ​​to optimize the calibration results and obtain the final calibration results. All laser points in each frame of the 3D point cloud of the outdoor inspection scene are projected using the obtained external parameters, and the corresponding projected pixel points are found. The laser points are colored to obtain local 3D color point cloud data.

[0043] Step 7, 3D global positioning. The global point cloud map is obtained through step 5, but it does not contain color information. The local 3D color point cloud map to be globally positioned is obtained through step 6. The current gravity direction is obtained by IMU measurement. Through 3D global positioning, the local 3D color point cloud map can be matched to the 6 degrees of freedom in the global map. ;in, It refers to translation along the X axis, that is, moving left and right. refers to translation along the Y axis, that is, moving forward and backward. It refers to translation along the Z axis, that is, moving up and down. It is the rotation around the X axis, also called the roll angle. It is the rotation around the Y axis, also called the pitch angle. It is the rotation around the Z axis, also called the yaw angle, which includes the following sub-steps:

[0044] (a) Initialize the pose and coordinate conversion, initialize the initial pose, map data, and point cloud data to be registered, and convert the lidar point cloud into the map coordinate system;

[0045] (b) Matching degree calculation and search space division: Calculate the matching degree between the laser point cloud and the map, and divide the search space into multiple regions by accumulating the probability values ​​of the grid where the laser point is located to form tree nodes, where the root node corresponds to the largest region;

[0046] (c) Greedy search and regional optimization: Calculate and determine the optimal matching degree of each region, i.e., the upper bound, and use the greedy scoring method to calculate the score. The score is improved by expanding the maximum probability value of the grid in a larger area, further dividing the possible area, and discarding the unmatched areas, focusing on the search for the optimal area;

[0047] (d) Solve for the optimal degree of freedom, complete the search within the optimal area, solve the robot's optimal 6-DOF posture, match the scanned local map with the 3D global map, and construct a 3D color geometric model of the outdoor inspection scene.

[0048] The beneficial effects of the present invention are: a method for reconstructing a three-dimensional color geometric model of an outdoor inspection scene by multi-source information fusion, comprising the following steps: (1) initialization of the internal calibration of the inertial measurement unit IMU, (2) joint external calibration of the laser radar and the inertial measurement unit IMU, (3) state estimation, (4) using the i-Octree structure to save and update the map, (5) performing loop detection to solve the mileage drift problem, (6) performing data fusion of the laser radar and the color camera, and (7) 3D global positioning. Compared with the prior art, the present invention has the following advantages: First, the laser radar and IMU data are fused in a tightly coupled manner, thereby improving the accuracy of positioning and map construction. Second, the i-Octree structure is used to dynamically save and update the map to improve the running speed. Third, the loop detection mechanism and closed-loop optimization function are integrated, which helps to reduce the accumulated error in long-term operation and improve the global consistency of the map. Fourth, by fusing the information from the lidar and color camera, a one-to-one mapping relationship is established between each spatial point in the three-dimensional point cloud and a pixel point in the color image, thereby more completely describing the visual information. Using the 3D global positioning method, a high-precision three-dimensional color geometric model reconstruction method is achieved. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 It is a flow chart of the method steps of the present invention.

[0050] Figure 2 It is a flowchart of the mapping algorithm steps of the present invention.

[0051] Figure 3This is the IMU and LiDAR initialization calibration flow chart in the present invention.

[0052] Figure 4 It is a flow chart of the state estimation, the core part of the mapping algorithm in the present invention.

[0053] Figure 5 It is a 3D global point cloud map of the outdoor inspection scene of the present invention.

[0054] Figure 6 It is a trajectory diagram of the moving subject of the present invention.

[0055] Figure 7 It is a local three-dimensional color point cloud data map of the present invention.

[0056] Among them, (a) is the image data map taken by the camera, and (b) is the fused three-dimensional color point cloud data map.

[0057] Figure 8 It is a three-dimensional color geometric model diagram of an outdoor inspection scene of the present invention. DETAILED DESCRIPTION

[0058] The present invention will be further described below in conjunction with the accompanying drawings.

[0059] like Figure 1 As shown, a method for reconstructing a three-dimensional color geometric model of an outdoor inspection scene by fusion of multi-source information includes the following steps:

[0060] Step 1, inertial measurement unit IMU internal parameter calibration initialization, IMU internal parameters include IMU initial zero bias, measurement noise and initial gravity vector, which are important input parameters of the initial state and affect the accuracy and reliability of the filter. Use the static initialization method to get the initial value of the IMU internal parameter and directly add it to the program operation. The so-called static initialization is to calculate the IMU internal parameter when the moving subject is in a stationary state. Since the moving subject is not moving, the tiny measurement noise is usually ignored. It is believed that the IMU's gyroscope measurement value has only zero bias, and the IMU's accelerometer measurement value has only zero bias and gravity acceleration. It specifically includes the following sub-steps:

[0061] (a) At the beginning of recording the corresponding data set, the device is placed still for a period of time to wait for the static initialization calculation to succeed until it is less than the manually set threshold. The static initialization time is set to 10 seconds in the program;

[0062] (b) Find the mean of all IMU measurements during the static initialization time. Assume that the mean values ​​of the gyroscope and accelerometer measurements are and ;

[0063] (c) The relationship between the IMU measurement value and the true value is given by the formula Describe:

[0064]

[0065] in, and They represent the angular velocity and acceleration of the moving subject respectively, yes The abbreviation of the motion main system is usually the transformation from the motion main system to the world system. To express, Represents the acceleration due to gravity. Since the motion of the moving subject on the surface is affected by gravity, the accelerometer measurement value will be affected by the additional acceleration due to gravity. and They are the real acceleration and real angular velocity of the moving subject in the world system. The world system is usually converted from the longitude and latitude coordinate system to the UTM (Universal Transverse Mercator Grid System) coordinate system. The axis is usually parallel to the center of the earth, according to the formula It can be seen that when the moving subject is stationary, the true angular velocity and true acceleration are both zero, and the zero bias of the gyroscope is considered to be , the initial posture of the moving subject is positioned as the posture of the world system, that is, ,matrix represents the identity matrix, so the static measurement value of the accelerometer is . Relative to the acceleration due to gravity , It is considered to be a negligible amount, so it can be directly considered The direction is the acceleration due to gravity direction, and query the magnitude of gravity acceleration through the local longitude and latitude, or directly use 9.8 as the magnitude of gravity acceleration;

[0066] (d) Then substitute the gravity acceleration and subtract the effect of gravity acceleration from the mean value of the accelerometer measurement to get the accelerometer bias. Describe:

[0067]

[0068] Finally, the IMU zero bias is assumed to be constant and the IMU measurement noise variance is estimated;

[0069] Step 2: Joint external parameter calibration of LiDAR and IMU. The external parameter calibration between LiDAR and IMU is as follows: Figure 3As shown, it includes rotation extrinsic parameters, translation extrinsic parameters and time offset. The rotation extrinsic parameters and translation extrinsic parameters are caused by the inconsistency of the sensor coordinate system, while the time offset is caused by the inevitable transmission and processing delays of the radar and IMU modules before receiving data. It specifically includes the following sub-steps:

[0070] (a) The data preprocessing part mainly uses a non-causal zero-phase low-pass filter to filter out high-frequency noise, including IMU measurement noise and angular velocity and linear velocity noise in the radar odometer. Since the IMU data frequency is usually higher than the radar data frequency, the IMU data is linearly interpolated based on the time of the radar odometer to obtain the IMU data and radar data at the same time;

[0071] (b) Initialize the time offset, ignore the gyro bias, and obtain the characteristic that the angular velocity is independent of the external parameters. Use zero-center cross-correlation to quantify the similarity between the angular velocities of the radar odometer and the IMU to obtain the initial value of the time offset.

[0072] (c) Refine the time offset and calculate the residual time offset that is smaller than the resolution. At this time, the gyroscope zero bias and rotation extrinsic parameters need to be considered. Assuming that the angular acceleration in the residual time is constant, the rotation extrinsic parameters, gyroscope zero bias and residual time offset are calibrated at the same time;

[0073] (d) Calibrate the remaining parameters through the acceleration relationship between the two fixed external coordinate systems, including the translation external parameter, the initial gravity vector and the accelerometer zero bias. After the initialization calibration is completed, the mapping algorithm can be performed. The specific process is as follows: Figure 2 shown.

[0074] Step 3: Perform state estimation. In order to facilitate the derivation of the formula, the manifold calculation symbol is introduced. for dimensional manifold, through two encapsulation operators and To build from The local neighborhood on to its tangent space The bidirectional mapping of It is to combine the elements on the manifold with the elements of the tangent space to obtain new elements on the manifold; It maps two elements on the manifold to the tangent space to represent the "difference" between them. The specific definition is given by the formula Describe:

[0075]

[0076] in, A special orthogonal group representing three-dimensional rotations (all elements are is an orthogonal matrix with a determinant of +1), express A rotation matrix in the group, represents a vector in a Lie algebra, represents the exponential mapping from Lie algebra to Lie group, which is used to convert Mapping back superior, yes The inverse operation of The elements on are mapped to the Lie algebra, and represents a normal vector;

[0077] Based on definition and Operator, set The index of the IMU measurement value is obtained by using a zero-order holder in the IMU sampling period. At, through the formula Discretize:

[0078]

[0079] In the formula, the first frame based on IMU is used as the global coordinate system (denoted as ), , Represents the position and attitude of the IMU in the global coordinate system, represents the gravity vector in the global coordinate system, , Represents the measurement value of IMU, , express , The measurement noise, , represents the IMU bias, represents the state quantity of the 24-dimensional system, represents the input control quantity, Represents the amount of noise. The IMU data and lidar data are tightly coupled by iterative extended Kalman filtering. The process is as follows Figure 4 As shown, it specifically includes the following sub-steps:

[0080] (a) The state quantity is predicted by forward propagation of IMU data, using the formula is described, where the noise Set to zero, Represent the true value, propagated value, updated value, and the error between the true value and its estimated value, respectively. Represents the index of the measurement value immediately following the current measurement value.

[0081]

[0082] in, Indicates noise The covariance matrix of and The calculation is done by the formula Describe,

[0083]

[0084] The prediction process is continuously executed until the end of a frame of point cloud data. The prediction state at this time is recorded as , the corresponding covariance is , Indicates the moment;

[0085] (b) Use IMU data to dedistort the point cloud and move all points in each frame to the end moment for distortion removal;

[0086] (c) Residual calculation, search in the map managed by i-Octree The nearest point is then found, and a normal vector and centroid are fitted. A local facet patch is fitted using the normal vector and centroid, and the measurement equation is expanded by a first-order Taylor series to obtain the corresponding Jacobian matrix and residual.

[0087] (d) Iterative update, using the predicted state obtained in substep (a) and covariance For unknown status A prior Gaussian distribution is imposed, The covariance of the following error states is expressed by the formula Describe:

[0088]

[0089] in, For the Moment The state after the Kalman filter update is: is the error, express about The partial differential at , continuously perform iterative updates of the state quantity, and calculate the Kalman iteration state and Kalman gain , until the convergence condition is met , is the convergence threshold, and the final optimal state is obtained and covariance estimation , then transform the lidar points of each frame into the global coordinate system, and insert the transformed lidar points into the global map managed by i-Octree;

[0090] Step 4. Use the i-Octree structure to save and update the map. i-Octree does not retain all feature nodes, but only the nodes near the current radar, which reduces the data size. Each pose update dynamically adds and deletes nodes. When deleting a node, the i-Octree structure first checks whether the node is located in the given space. All nodes in the given space are directly deleted without searching for nodes in it, which significantly reduces the deletion time. In the iterative update, nodes are continuously recursively inserted and removed, and i-Octree is dynamically updated. In the residual calculation, the KNN nearest neighborhood algorithm is used to find the node in the tree. The distance between the nearest neighbor point and the target point, when the distance is less than the threshold, is regarded as an internal point and used for state estimation;

[0091] Step 5: Perform loop detection to solve the mileage drift problem and ensure the consistency of map construction. For continuous lidar frame data, select key frames with the same time interval and save them into the historical key frame queue and cyclically update the key frame factor graph. When it is subsequently detected that the trajectory distance difference between the current key frame and the historical key frame is greater than the threshold and the relative posture difference is less than the threshold, start loop detection, add the relevant key frames to the graph optimization algorithm, and use the GTSAM library to perform graph optimization operations to correct the algorithm posture calculation results. Through steps 2, 3, 4, and 5, obtain the trajectory map of the moving subject and the 3D global point cloud map of the outdoor inspection scene, such as Figure 6 and Figure 5 shown.

[0092] Step 6: Fusing the laser radar and color camera data, including the following sub-steps:

[0093] (a) Camera intrinsic calibration: Use the camera to collect 30 images of calibration plate data, and use MATLAB's calib toolbox to manually extract corner points from each calibration plate image data in the order of upper left, lower left, lower right, and upper right to obtain the camera intrinsic parameters;

[0094] (b) Extract edge features in the environment. First, divide the laser point cloud into multiple cubes with the same side length. Then use the random sampling consensus algorithm (RANSAC) to extract planes in each cube space. For spaces with more than 2 planes, calculate the intersection lines between these planes as point cloud edge features. This ensures that the extracted edges are depth-continuous. Then, project these edge features onto a 2D plane using the camera intrinsic parameters solved in the previous step. At the same time, use the Canny algorithm to extract edge features in the image.

[0095] (c) Match the extracted edge features, sample some points in the extracted edge, then project the points onto the image based on the currently estimated extrinsic parameters, and then find the nearest pixel in the edge of the image through the KD tree. point, and the direction vector of the radar edge is also projected onto the image to determine whether it is consistent with this point. The straight lines formed by the points are parallel to each other to filter out the false matches that are not parallel at all;

[0096] (d) External parameter optimization. The measurement noise of the image edge is the pixel position error. The measurement noise of the radar edge is divided into the beam direction error and the depth measurement error. The distance between the corresponding points and the directional angle deviation between them are used as the residual value to optimize the calibration result to obtain the final calibration result. For each frame of the 3D point cloud of the outdoor inspection scene, all laser points are projected by the obtained external parameters, and the corresponding projection pixel points are found. The laser points are colored to obtain the local 3D color point cloud data, such as Figure 7 shown.

[0097] Step 7, 3D global positioning. The global point cloud map is obtained through step 5, but it does not contain color information. The local 3D color point cloud map to be globally positioned is obtained through step 6. The current gravity direction is obtained by IMU measurement. Through 3D global positioning, the local 3D color point cloud map can be matched to the 6 degrees of freedom in the global map. ;in, It refers to translation along the X axis, that is, moving left and right. refers to translation along the Y axis, that is, moving forward and backward. It refers to translation along the Z axis, that is, moving up and down. It is the rotation around the X axis, also called the roll angle. It is the rotation around the Y axis, also called the pitch angle. It is the rotation around the Z axis, also called the yaw angle, which includes the following sub-steps:

[0098] (a) Initialize the pose and coordinate conversion, initialize the initial pose, map data, and point cloud data to be registered, and convert the lidar point cloud into the map coordinate system;

[0099] (b) Matching degree calculation and search space division: Calculate the matching degree between the laser point cloud and the map, and divide the search space into multiple regions by accumulating the probability values ​​of the grid where the laser point is located to form tree nodes, where the root node corresponds to the largest region;

[0100] (c) Greedy search and regional optimization: Calculate and determine the optimal matching degree of each region, i.e., the upper bound, and use the greedy scoring method to calculate the score. The score is improved by expanding the maximum probability value of the grid in a larger area, further dividing the possible area, and discarding the unmatched areas, focusing on the search for the optimal area;

[0101] (d) Solve the optimal degree of freedom, complete the search in the optimal area, solve the optimal 6-DOF posture of the robot, match the scanned local map with the 3D global map, and construct a 3D color geometric model of the outdoor inspection scene, such as Figure 8 shown.

Claims

1. A method for reconstructing a three-dimensional color geometric model of an outdoor inspection scene by fusion of multi-source information, characterized in that: The following steps are involved: Step 1, inertial measurement unit IMU internal parameter calibration initialization, IMU internal parameters include IMU initial zero bias, measurement noise and initial gravity vector, which are important input parameters of the initial state and affect the accuracy and reliability of the filter. Use the static initialization method to get the initial value of the IMU internal parameter and directly add it to the program operation. The so-called static initialization is to calculate the IMU internal parameter when the moving subject is in a stationary state. Since the moving subject is not moving, the tiny measurement noise is usually ignored. It is believed that the IMU's gyroscope measurement value has only zero bias, and the IMU's accelerometer measurement value has only zero bias and gravity acceleration. It specifically includes the following sub-steps: (a) At the beginning of recording the corresponding data set, the device is placed still for a period of time to wait for the static initialization calculation to succeed until it is less than the manually set threshold. The static initialization time is set to 10 seconds in the program; (b) Find the mean of all IMU measurements during the static initialization time. Assume that the mean values ​​of the gyroscope and accelerometer measurements are and ; (c) The relationship between the IMU measurement value and the true value is given by the formula Describe: ； in, and They represent the angular velocity and acceleration of the moving subject respectively, yes The abbreviation of the motion main system is usually the transformation from the motion main system to the world system. To express, Represents the acceleration due to gravity. Since the motion of the moving subject on the surface is affected by gravity, the accelerometer measurement value will be affected by the additional acceleration due to gravity. and They are the real acceleration and real angular velocity of the moving subject in the world system. The world system is usually converted from the longitude and latitude coordinate system to the UTM (Universal Transverse Mercator Grid System) coordinate system. The axis is usually parallel to the center of the earth, according to the formula It can be seen that when the moving subject is stationary, the true angular velocity and true acceleration are both zero, and the zero bias of the gyroscope is considered to be , the initial posture of the moving subject is positioned as the posture of the world system, that is, ,matrix represents the identity matrix, so the static measurement value of the accelerometer is . Relative to the acceleration due to gravity , It is considered to be a negligible amount, so it can be directly considered The direction is the acceleration due to gravity direction, and query the magnitude of gravity acceleration through the local longitude and latitude, or directly use 9.8 as the magnitude of gravity acceleration; (d) Then substitute the gravity acceleration and subtract the effect of gravity acceleration from the mean value of the accelerometer measurement to get the accelerometer bias. Describe: ； Finally, the IMU zero bias is assumed to be constant and the IMU measurement noise variance is estimated; Step 2: Joint external parameter calibration of LiDAR and IMU. External parameter calibration between LiDAR and IMU includes rotational external parameter, translational external parameter and time offset. Rotational external parameter and translational external parameter are caused by the inconsistency of sensor coordinate system, while time offset is caused by the inevitable transmission and processing delay before receiving data between LiDAR and IMU modules. It specifically includes the following sub-steps: (a) The data preprocessing part mainly uses a non-causal zero-phase low-pass filter to filter out high-frequency noise, including IMU measurement noise and angular velocity and linear velocity noise in the radar odometer. Since the IMU data frequency is usually higher than the radar data frequency, the IMU data is linearly interpolated based on the time of the radar odometer to obtain the IMU data and radar data at the same time; (b) Initialize the time offset, ignore the gyro bias, and obtain the characteristic that the angular velocity is independent of the external parameters. Use zero-center cross-correlation to quantify the similarity between the angular velocities of the radar odometer and the IMU to obtain the initial value of the time offset. (c) Refine the time offset and calculate the residual time offset that is smaller than the resolution. At this time, the gyroscope zero bias and rotation extrinsic parameters need to be considered. Assuming that the angular acceleration in the residual time is constant, the rotation extrinsic parameters, gyroscope zero bias and residual time offset are calibrated at the same time; (d) Calibrate the remaining parameters, including the translation extrinsic parameter, the initial gravity vector, and the accelerometer bias, through the acceleration relationship between the two externally fixed coordinate systems; Step 3: Perform state estimation. In order to facilitate the derivation of the formula, the manifold calculation symbol is introduced. for dimensional manifold, through two encapsulation operators and To build from The local neighborhood on to its tangent space The bidirectional mapping of It is to combine the elements on the manifold with the elements of the tangent space to obtain new elements on the manifold; It maps two elements on the manifold to the tangent space to represent the "difference" between them. The specific definition is given by the formula Describe: ； in, A special orthogonal group representing three-dimensional rotations (all elements are is an orthogonal matrix with a determinant of +1), express A rotation matrix in the group, represents a vector in a Lie algebra, represents the exponential mapping from Lie algebra to Lie group, which is used to convert Mapping back superior, yes The inverse operation of The elements on are mapped to the Lie algebra, and represents a normal vector; Based on definition and Operator, set The index of the IMU measurement value is obtained by using a zero-order holder in the IMU sampling period. At, through the formula Discretize: ； In the formula, the first frame based on IMU is used as the global coordinate system (denoted as ), , Represents the position and attitude of the IMU in the global coordinate system, represents the gravity vector in the global coordinate system, , Represents the measurement value of IMU, , express , The measurement noise, , represents the IMU bias, represents the state quantity of the 24-dimensional system, represents the input control quantity, Represents the amount of noise. The IMU data and lidar data are tightly coupled by iterative extended Kalman filtering, which includes the following sub-steps: (a) The state quantity is predicted by forward propagation of IMU data, using the formula is described, where the noise Set to zero, Represent the true value, propagated value, updated value, and the error between the true value and its estimated value, respectively. Represents the index of the measurement value immediately following the current measurement value. ； in, Indicates noise The covariance matrix of and The calculation is done by the formula Describe: ； The prediction process is continuously executed until the end of a frame of point cloud data. The prediction state at this time is recorded as , the corresponding covariance is , Indicates the moment; (b) Use IMU data to dedistort the point cloud and move all points in each frame to the end moment for distortion removal; (c) Residual calculation, search in the map managed by i-Octree The nearest point is then found, and a normal vector and centroid are fitted. A local facet patch is fitted using the normal vector and centroid, and the measurement equation is expanded by a first-order Taylor series to obtain the corresponding Jacobian matrix and residual. (d) Iterative update, using the predicted state obtained in substep (a) and covariance For unknown status A prior Gaussian distribution is imposed, The covariance of the following error states is expressed by the formula Describe: ； in, For the Moment The state after the Kalman filter update is: is the error, express about The partial differential at , continuously perform iterative updates of the state quantity, and calculate the Kalman iteration state and Kalman gain , until the convergence condition is met , is the convergence threshold, and the final optimal state is obtained and covariance estimation , then transform the lidar points of each frame into the global coordinate system, and insert the transformed lidar points into the global map managed by i-Octree; Step 4. Use the i-Octree structure to save and update the map. The i-Octree does not retain all feature nodes, but only the nodes near the current radar, which reduces the data size. Each pose update dynamically adds and deletes nodes. When deleting a node, the i-Octree structure first checks whether the node is located in the given space. All nodes in the given space are directly deleted without searching for nodes in it, which significantly reduces the deletion time. In the iterative update, nodes are continuously recursively inserted and removed, and the i-Octree is dynamically updated. In the residual calculation, the KNN nearest neighborhood algorithm is used to find the node in the tree. The distance between the nearest neighbor point and the target point, when the distance is less than the threshold, is regarded as an internal point and used for state estimation; Step 5: Perform loop detection to solve the mileage drift problem and ensure the consistency of map construction. For continuous lidar frame data, select key frames with the same time interval and save them into the historical key frame queue and cyclically update the key frame factor graph. When it is subsequently detected that the trajectory distance difference between the current key frame and the historical key frame is greater than the threshold and the relative posture difference is less than the threshold, start loop detection, add the relevant key frames to the graph optimization algorithm, use the GTSAM library to perform graph optimization operations to correct the algorithm posture calculation results, and obtain the trajectory map of the moving subject and the 3D global point cloud map of the outdoor inspection scene through steps 2, 3, 4, and 5. Step 6: Fusing the laser radar and color camera data, including the following sub-steps: (a) Camera intrinsic calibration: Use the camera to collect 30 images of calibration plate data, and use MATLAB's calib toolbox to manually extract corner points from each calibration plate image data in the order of upper left, lower left, lower right, and upper right to obtain the camera intrinsic parameters; (b) Extract edge features in the environment. First, divide the laser point cloud into multiple cubes with the same side length. Then use the random sampling consensus algorithm (RANSAC) to extract planes in each cube space. For spaces with more than 2 planes, calculate the intersection lines between these planes as point cloud edge features. This ensures that the extracted edges are depth-continuous. Then, project these edge features onto a 2D plane using the camera intrinsic parameters solved in the previous step. At the same time, use the Canny algorithm to extract edge features in the image. (c) Match the extracted edge features, sample some points in the extracted edge, then project the points onto the image based on the currently estimated extrinsic parameters, and then find the nearest pixel in the edge of the image through the KD tree. point, and the direction vector of the radar edge is also projected onto the image to determine whether it is consistent with this point. The straight lines formed by the points are parallel to each other to filter out the false matches that are not parallel at all; (d) External parameter optimization: The measurement noise of the image edge is the pixel position error. The measurement noise of the radar edge is divided into the beam direction error and the depth measurement error. The distance between corresponding points and the directional angle deviation between them are used as residual values ​​to optimize the calibration results and obtain the final calibration results. All laser points in each frame of the 3D point cloud of the outdoor inspection scene are projected using the obtained external parameters, and the corresponding projected pixel points are found. The laser points are colored to obtain local 3D color point cloud data. Step 7, 3D global positioning. The global point cloud map is obtained through step 5, but it does not contain color information. The local 3D color point cloud map to be globally positioned is obtained through step 6. The current gravity direction is obtained by IMU measurement. Through 3D global positioning, the local 3D color point cloud map can be matched to the 6 degrees of freedom in the global map. ;in, It refers to translation along the X axis, that is, moving left and right. refers to translation along the Y axis, that is, moving forward and backward. It refers to translation along the Z axis, that is, moving up and down. It is the rotation around the X axis, also called the roll angle. It is the rotation around the Y axis, also called the pitch angle. It is the rotation around the Z axis, also called the yaw angle, which includes the following sub-steps: (a) Initialize the pose and coordinate conversion, initialize the initial pose, map data, and point cloud data to be registered, and convert the lidar point cloud into the map coordinate system; (b) Matching degree calculation and search space division: Calculate the matching degree between the laser point cloud and the map, and divide the search space into multiple regions by accumulating the probability values ​​of the grid where the laser point is located to form tree nodes, where the root node corresponds to the largest region; (c) Greedy search and regional optimization: Calculate and determine the optimal matching degree of each region, i.e., the upper bound, and use the greedy scoring method to calculate the score. The score is improved by expanding the maximum probability value of the grid in a larger area, further dividing the possible area, and discarding the unmatched areas, focusing on the search for the optimal area; (d) Solve for the optimal degree of freedom, complete the search within the optimal area, solve the robot's optimal 6-DOF posture, match the scanned local map with the 3D global map, and construct a 3D color geometric model of the outdoor inspection scene.

Images