A tightly coupled SLAM method based on coding dimensionality fusion

Abstract

The present invention relates to the field of intelligent positioning and navigation technology in mines, solving the problem of underground navigation positioning in unstructured and GNSS-inoperable areas, and in particular to a tightly coupled SLAM method based on coded dimensionality fusion, comprising the following steps: S1, subscribing to the raw point cloud data collected by the laser radar and the raw data collected by the IMU; S2, calculating the motion compensation information of the current frame laser point cloud based on the raw point cloud data collected by the laser radar and the raw data collected by the IMU; S3, pre-integrating the IMU information based on the raw data collected by the IMU, and transmitting the IMU pre-integrated information. The present invention achieves high-precision positioning in coal mines, while balancing accuracy and computational complexity, improving the accuracy and real-time performance of positioning and mapping of mine shafts and tunnels, and to a certain extent realizing precise perception and positioning technology for unmanned driving underground.

Description

Technical Field

[0001] The present invention relates to the technical field of mine intelligent positioning and navigation, and in particular to a tightly coupled SLAM method based on coding dimensionality fusion. Background Art

[0002] Coal mine tunnels, mining faces, and other working areas are typically characterized by unstructured environments, and GNSS technology cannot be directly applied underground. This leads to frequent mining accidents during coal mining, and there is an urgent need for mechanized replacement of personnel, automated reduction of manpower, and improved mine intelligence. Building an autonomous positioning system solution suitable for unmanned coal mines can solve these problems. However, an autonomous positioning system suitable for unmanned coal mines must first address issues such as precise positioning and posture perception for underground unmanned driving. Rapidly achieving precise perception and positioning technology for underground unmanned driving that integrates multiple information such as vision, inertial navigation, and laser is key to achieving local autonomy for underground unmanned driving. Therefore, SLAM methods that integrate vision, lidar, and inertial measurement units are extremely important. Summary of the Invention

[0003] In response to the shortcomings of the existing technology, the present invention provides a tightly coupled SLAM method based on coding dimensionality fusion, which solves the problem of unstructured underground navigation and positioning where GNSS cannot function, and to a certain extent realizes the precise perception and positioning technology of underground unmanned driving.

[0004] To solve the above technical problems, the present invention provides the following technical solutions: a tightly coupled SLAM method based on coding dimensionality fusion, comprising the following steps:

[0005] S1. Subscribe to the raw point cloud data collected by the lidar and the raw data collected by the IMU;

[0006] S2. Calculate motion compensation information of the current frame laser point cloud based on the original point cloud data collected by the lidar and the current frame laser point cloud in the original data collected by the IMU;

[0007] S3. Pre-integrate the IMU information based on the raw data collected by the IMU and propagate the IMU pre-integrated information;

[0008] S4, acquiring the current frame through the camera, performing frame-to-frame optical flow tracking, and encoding some of the coordinates into 5D coordinates;

[0009] S5. Perform dimensionality upgrade on the encoded 5D coordinates through a neural network, and determine feature points in the dimensionality upgrade result;

[0010] S6. Update the LIO subsystem using the motion compensation information of the point cloud and the IMU pre-integration information to obtain LIO subsystem information;

[0011] S7. Update the VIO subsystem with the feature points after dimension elevation and the IMU pre-integration information to obtain the VIO subsystem information;

[0012] S8. Incorporate the IMU information, LIO subsystem information, and VIO subsystem information into the global map update.

[0013] Furthermore, in step S2, based on the current-frame lidar point cloud in the original point cloud data collected by the lidar and the original data collected by the IMU, calculate the motion compensation information for the current-frame lidar point cloud. The specific process includes the following steps:<00001,00><00001,01>S21. Using the lidar as a reference, find the IMU data that includes the start and end times of a frame of lidar points;<00001,02><00001,03>S22. Integrate the above IMU data. Taking the first IMU as the reference coordinate, integrate all the IMU data in sequence, and each IMU obtains a corresponding pose;<00001,04><0,00001,05>S23. Calculate the pose of the lidar at the end time relative to the first IMU;<00001,06><00001,07>S24. Process all the points in the current-frame lidar point cloud according to steps S21 - S23, calculate the pose of the lidar at the end time relative to the first IMU, then transform it to the lidar coordinate system at the last time to obtain the motion compensation information for the current-frame lidar point cloud, and finally repackage it into a frame of lidar data and publish it.<00001,08><00001,09>Furthermore, in step S23, calculating the pose of the lidar at the end time relative to the first IMU, the specific process includes the following:<00001,10><00001,11>If the odometer queue is exactly synchronized with the lidar data, assuming the times of the i-th and j-th data are t e ,

[0021] ,

[0020] , e , s , s , , , s and t e , then solve for the poses p s and p e at times t s and p e ;

[0020] If the above situation does not exist, that is, there is no corresponding pose at times t s and t e , then calculate the poses at m times by linear interpolation of the odometer: Assume there is a pose at time (l, k), and l < s < k, then obtain the poses p s and p e in this case;

[0021] At times t s and te During the time period, a total of m poses {p s ,p s+1 ,…,p s+m-2 ,p e}, and then perform linear interpolation on the pose corresponding to each LiDAR point in the known m+2 poses.

[0022] Furthermore, in step S3, the IMU information is pre-integrated based on the raw data collected by the IMU, and the IMU pre-integrated information is propagated. The specific process includes the following steps:

[0023] S31, receiving laser odometry data from the raw data collected by the IMU, unifying the laser odometry data into a specified format, and saving the timestamp of the current laser odometry data;

[0024] S32, remove the IMU incremental data whose timestamp is earlier than the laser odometry timestamp in the IMU queue;

[0025] S33, calculating the relative pose transformation between the IMU odometers corresponding to the start and end times in the IMU queue;

[0026] S34, combining the data previously received from the laser odometry and the data obtained from the relative pose transformation previously calculated and publishing them;

[0027] S35. Subscribe to the raw data collected by the IMU and add it to the queue. Use the result obtained after the previous moment optimization as the starting value of the integration, perform integral calculation on the data in the IMU queue, and publish the IMU increment.

[0028] S36. Initialize the pre-integrator using the current optimized state, and calculate the IMU data after the current frame to obtain MU pre-integration information;

[0029] S37. Propagate the MU pre-integration information.

[0030] Furthermore, in step S4, the current frame is acquired by the camera, and frame-to-frame optical flow tracking is performed, and some of the coordinates are encoded into 5D coordinates. The specific process includes the following steps:

[0031] S41, subscribe to the current frame information collected by the camera;

[0032] S42, performing frame-by-frame optical flow tracking;

[0033] S43. Convert the input 5D coordinates by using the position encoding.

[0034] Furthermore, in step S42, frame-by-frame optical flow tracking is performed, and the specific process includes the following steps:

[0035] S421. Build a Gaussian pyramid for each frame, with the largest scale image at the top and the original image at the bottom.

[0036] S422. Estimate the position of the next frame starting from the top layer, use it as the initial position of the next layer, search downward along the pyramid, and repeat the estimation action until the bottom layer of the pyramid is reached.

[0037] Furthermore, in step S43, the input 5D coordinates are converted by using the position code, and the specific process includes the following steps:

[0038] S431, allowing the camera ray to pass through the scene to sample a set of 3D points;

[0039] S432, inputting a set of spatial points and their corresponding two-dimensional viewing directions into a neural network to generate a set of output colors and volume densities;

[0040] S433. Use classic voxel rendering methods to accumulate these colors and volume densities into a 2D image.

[0041] Furthermore, in step S6, the LIO subsystem is updated using the point cloud motion compensation information and the IMU pre-integration information to obtain LIO subsystem information. The specific process includes the following steps:

[0042] S61, for the motion compensation information scan of the incoming point cloud, the motion distortion caused by continuous movement within the frame is compensated by IMU back propagation;

[0043] S62, using the error state iterative Kalman filter (ESIKF) of the motion compensation information of the point cloud to minimize the point-to-plane residual to estimate the state of the LIO subsystem;

[0044] S63. In the converged state, the scanned points are appended to the global map, and the corresponding voxels are marked as activated or deactivated, completing the update of the LIO subsystem.

[0045] Furthermore, in step S7, the VIO subsystem is updated using the dimensionally upgraded feature points and the IMU pre-integrated information to obtain VIO subsystem information. The specific process includes the following steps:

[0046] S71. Between frames, map points are tracked using the Lucas-Kanade optical flow method, and the system state is estimated by minimizing the projection error of the map points by optimizing the system state with the Perspective-n-Point method. The system estimate is then iterated using the ESIKF framework, which is equivalent to the Gauss-Newton method.

[0047] S72, between the frame and the global map, projecting the tracked points in the global map into the current image frame, and then minimizing the photometric error between the frame and the map of these points to further refine the state estimate of the system;

[0048] S73. After the VIO update from the frame to the global map, the precise pose of the current image frame can be obtained. Then, by linearly interpolating the RGB colors of the pixels corresponding to the map points on the current image frame and the adjacent pixels, texture rendering is performed to update the color of the points in the global map using the converged state estimate and the original input image.

[0049] Furthermore, in step S8, the IMU information, LIO subsystem information, and VIO subsystem information are added to the global map update. The specific process includes:

[0050] Extract feature points: extract key points in the image and find pixels with certain features in the image;

[0051] Matching feature points: Based on the obtained key point positions, calculate the descriptors of the feature points and construct feature descriptors. Then, compare the descriptors of the feature points with the descriptors to achieve feature matching.

[0052] The LIO system is responsible for building the geometric structure of the map, and the VIO subsystem is responsible for rendering the texture of the map, that is, the color information of the point cloud.

[0053] By means of the above technical solution, the present invention provides a tightly coupled SLAM method based on coding dimensionality fusion, which has at least the following beneficial effects:

[0054] 1. This invention uses lidar, cameras, and IMUs to achieve high-precision positioning in coal mines and improve the posture perception of the underground environment, thereby achieving high-precision positioning in coal mines. At the same time, it balances accuracy and computational complexity, improves the accuracy and real-time performance of positioning and mapping of mine shafts and tunnels, and effectively solves the problem of underground positioning and mapping difficulties in unstructured environments where GNSS cannot function.

[0055] 2. The present invention uses a method of tight coupling of vision, lidar and IMU. In the unstructured environment characteristics of mine shafts, vision and lidar cannot collect enough feature point clouds. IMU is used for pre-integration to make up for the defect of insufficient feature point clouds, and then the feature extraction of point clouds is performed on vision and lidar, so that the entire system remains stable.

[0056] 3. The present invention uses a method of tight coupling of vision, lidar and IMU to calculate the original point cloud of vision and lidar together with IMU data to improve the accuracy of the entire system in unstructured environments.

[0057] 4. The present invention performs visual coding dimensionality upgrading to obtain more and more detailed visual feature point information by increasing the dimension, thereby ensuring the accuracy and real-time performance of the system.

[0058] 5. The present invention uses visual coding to increase the dimension, maps 5D coordinates to high-dimensional space, maps the network input to high-dimensional space to expand the distance between points, allows the network to learn more high-frequency information, and ensures the accuracy and real-time performance of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0060] Figure 1 Flowchart of the steps of the tightly coupled SLAM method of the present invention;

[0061] Figure 2 This is a flow chart of the principle of the tightly coupled SLAM method of the present invention;

[0062] Figure 3 This is a schematic diagram of laser radar motion compensation according to the present invention;

[0063] Figure 4 This is a schematic diagram of IMU pre-integration data transmission in the present invention;

[0064] Figure 5 This is a schematic diagram of the visual coordinate dimension-upgraded data transmission of the present invention;

[0065] Figure 6 This is a schematic diagram of the LIO subsystem update of the present invention;

[0066] Figure 7 This is a schematic diagram of the VIO subsystem update of the present invention;

[0067] Figure 8 Schematic diagram of the principle of PnP reprojection error of the present invention;

[0068] Figure 9 This is the experimental result diagram of the tightly coupled SLAM method of the present invention. DETAILED DESCRIPTION

[0069] To make the above-mentioned objectives, features, and advantages of the present invention more clearly understood, the present invention is further described below in detail with reference to the accompanying drawings and specific embodiments. This will enable a full understanding of how this application uses technical means to solve technical problems and achieve technical effects, and to implement the invention accordingly.

[0070] Those skilled in the art will appreciate that all or part of the steps in the above-mentioned embodiment methods can be accomplished by instructing the relevant hardware through a program. Therefore, the present application may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Furthermore, the present application may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0071] Please refer to Figures 1-9 , shows a specific implementation method of this embodiment. This embodiment fully considers the unstructured environmental characteristics of mine shafts and tunnels and the factors that GNSS technology cannot be directly used. Due to the unstructured environmental characteristics of mine shafts and tunnels, under certain circumstances, the underground light environment is unstable, and vision and lidar cannot collect enough feature point clouds. However, IMU does not rely on external information and exists independently of other sensors. Therefore, IMU can perform pre-integration when there are not enough feature point clouds to make up for the defect of not having enough feature point clouds, and then extract the features of the point cloud for the lidar, so that the entire system remains stable.

[0072] This embodiment uses a tightly coupled approach with vision, LiDAR, and IMU, combining raw point clouds from vision and LiDAR with IMU data to improve the accuracy of the entire system in unstructured environments. Furthermore, this embodiment performs dimensionality upscaling on visual encoding, acquiring more detailed visual feature point information by increasing the dimensionality, thus ensuring the accuracy and real-time performance of the system.

[0073] Please refer to Figure 1 and Figure 2 This embodiment proposes a tightly coupled SLAM method based on coding dimensionality fusion, which includes the following steps:

[0074] S1. Subscribe to the raw point cloud data collected by the lidar and the raw data collected by the IMU;

[0075] S2. Calculate motion compensation information of the current frame laser point cloud based on the original point cloud data collected by the lidar and the current frame laser point cloud in the original data collected by the IMU;

[0076] For the laser point cloud of the current frame, the motion compensation information of the point cloud is calculated as follows: the relative motion of the laser radar relative to the initial moment is calculated at each laser beam reception moment, the laser point coordinates are multiplied by the coordinate system conversion relationship, the position and posture of the laser point at each moment are obtained, and then converted to the coordinate system of the laser point at the initial moment.

[0077] Please refer to Figure 3, in step S2, according to the current frame of lidar point cloud in the original point cloud data collected by the lidar and the original data collected by the IMU, calculate the motion compensation information of the current frame of lidar point cloud. The specific process includes the following steps:

[0078] S21. Taking the lidar as a reference, find the IMU data including the start and end times of a frame of points of the lidar;

[0079] S22. Integrate the above IMU data. Taking the first IMU as the reference coordinate, integrate all the IMU data in sequence, and each IMU obtains a corresponding pose;

[0080] S23. Calculate the pose of the lidar at the end time relative to the first IMU.

[0081] In step S23, to calculate the pose of the lidar at the end time relative to the first IMU, the specific process includes the following:

[0082] If the odometer queue is exactly synchronized with the lidar data, assuming the times of the i-th and j-th data are t s and t e respectively, then solve the poses p s and p e at times t s and t e ;

[0083] The poses p s and p e at times t s and t e are respectively:

[0084] p s = OdomList[i]

[0085] p e = OdomList[j]

[0086] If the above situation does not exist, that is, there is no corresponding pose at times t s and t e , then calculate the poses at m times by linear interpolation of the odometer: Assume there is a pose at time (l, k), and l < s < k, then obtain the poses p s and p e in the above situation;

[0087] The formulas for the poses p s and p e are as follows:

[0088] p l = OdomList[l]

[0089] p k =OdomList[k]

[0090]

[0091] In t s and t e During the time period, a total of m poses {p s ,p s+1 ,…,p s+m-2 ,p e}, and then perform linear interpolation on the pose corresponding to each LiDAR point in the known m+2 poses.

[0092] x' i =(p x ,p y )

[0093]

[0094] angle=a tan2(p y ,p x )

[0095] This posture is in the IMU coordinate system and needs to be converted into the posture of the lidar coordinate system relative to the first IMU coordinate system.

[0096] S24. Process all points in the laser point cloud of the current frame according to steps S21-S23, calculate the posture of the laser radar relative to the first IMU at the end moment, and then convert it to the laser radar coordinate system at the last moment to obtain the motion compensation information of the laser point cloud of the current frame, and finally repackage it into a frame of laser data and publish it.

[0097] S3. Pre-integrate the IMU information based on the raw data collected by the IMU, and propagate the IMU pre-integrated information. According to the change increment from the laser odometry at the previous moment to the IMU odometry at the current moment, calculate the IMU odometry at the current moment based on the change increment, and display the trajectory of the local IMU odometry through rviz.

[0098] Please refer to Figure 4 In step S3, the IMU information is pre-integrated according to the raw data collected by the IMU, and the IMU pre-integrated information is propagated. The specific process includes the following steps:

[0099] S31, receiving laser odometry data from the raw data collected by the IMU, unifying the laser odometry data into a specified format, and saving the timestamp of the current laser odometry data;

[0100] S32, remove the IMU incremental data whose timestamp is earlier than the laser odometry timestamp in the IMU queue;

[0101] S33. Calculate the relative pose transformation between the IMU odometers corresponding to the start and end times in the IMU queue. The calculation method is the same as step S2 and will not be repeated here.

[0102] S34. Combine and publish the data previously received from the laser odometry and the data obtained by the relative pose transformation previously calculated. The data previously received from the laser odometry is the original data collected by the IMU, and the data obtained by the relative pose transformation previously calculated is the laser data repackaged into a frame calculated in step S2, that is, the motion compensation information of the laser point cloud of the current frame.

[0103] S35. Subscribe to the raw data collected by the IMU and add it to the queue. Use the result obtained after the previous moment optimization as the starting value of the integration, perform integral calculation on the data in the IMU queue, and publish the IMU increment.

[0104] Subscribe to IMU increments for pose estimation. When new IMU data is passed in, the current pose is estimated based on the previous IMU integral data and the current data.

[0105] Specifically, the formulas for IMU angular velocity and acceleration are defined as follows:

[0106] Angular velocity:

[0107] Acceleration:

[0108] The IMU observation source data at time t is and and and will be affected by white noise n t and IMU bias b t The influence of Matrix is the transformation matrix from the world coordinate system to the robot coordinate system. The gravity constant vector g belongs to the world coordinate system w.

[0109] S36. Initialize the pre-integrator using the current optimized state, and calculate the IMU data after the current frame to obtain MU pre-integration information;

[0110] The current optimized state should be understood as: adding the IMU factor and performing integral calculations between two frames of IMU data; adding a priori factors and using the currently received laser odometry as the priori factors; assigning initial values ​​to the variable nodes, performing optimization, and updating the state before the current optimized state.

[0111] The optimizer resets every time it receives 100 frames of laser odometry data. Initialization differs from resetting in that the current velocity and pose remain consistent with those obtained from the previous optimization, and the noise model uses the edge distribution of the previously optimized model. This is a conventional technique and will not be described in detail here.

[0112] For example, the robot's motion is inferred by the IMU measurement, and the robot's position P at time t+Δt is t+Δt , speed v t+Δt and rotate R t+Δt The calculation formula is as follows:

[0113]

[0114]

[0115]

[0116] in, Assume that the robot's angular velocity and acceleration remain constant during the integration process.

[0117] The relative motion between different timestamps is obtained by IMU pre-integration. The position change Δp between the mth and nth moments obtained by pre-integration is mn , speed change Δv mn and rotation change ΔR mn The calculation formula is as follows:

[0118]

[0119]

[0120]

[0121] S37. Propagate the MU pre-integration information.

[0122] Taking into account the unstructured environmental characteristics of mine shafts, under certain circumstances, the lidar cannot collect enough feature point clouds. The IMU can perform pre-integration when there are not enough feature point clouds, making up for the defect of not having enough feature point clouds and improving the accuracy of the system in unstructured environments.

[0123] S4, acquiring the current frame through the camera, performing frame-to-frame optical flow tracking, and encoding some of the coordinates into 5D coordinates;

[0124] In step S4, the current frame is acquired by the camera, and frame-to-frame optical flow tracking is performed, and some of the coordinates are encoded into 5D coordinates. The specific process includes the following steps:

[0125] S41, subscribe to the current frame information collected by the camera;

[0126] S42, performing frame-by-frame optical flow tracking;

[0127] In step S42, frame-by-frame optical flow tracking is performed, and the specific process includes the following steps:

[0128] S421. Build a Gaussian pyramid for each frame, with the largest scale image at the top and the original image at the bottom.

[0129] S422. Estimate the position of the next frame starting from the top layer, use it as the initial position of the next layer, search downward along the pyramid, and repeat the estimation action until the bottom layer of the pyramid is reached.

[0130] S43, converting the input 5D coordinates by using a position encoding;

[0131] Specifically, the continuous scene is represented as a 5D vector-valued function whose input is the 3D position x = (x, y, z) and the 2D viewing direction Its output is the emission color c = (r, g, b) and the volume density σ. The direction is represented as a 3D Cartesian unit vector d. An MLP network FΘ: (x, d) is used to approximate this continuous 5D scene representation (c, σ) and optimize its weights θ to map from each input 5D coordinate to its corresponding volume density and directional emission color.

[0132] FΘ:(x,d)→(c,σ)

[0133] In step S43, the input 5D coordinates are converted by using the position encoding, and the specific process includes the following steps:

[0134] S431, allowing the camera ray to pass through the scene to sample a set of 3D points;

[0135] S432, inputting a set of spatial points and their corresponding two-dimensional viewing directions into a neural network to generate a set of output colors and volume densities;

[0136] S433. Use classic voxel rendering methods to accumulate these colors and volume densities into a 2D image.

[0137] S5. The coded 5D coordinates are upgraded through a neural network, and feature points are determined in the upgraded results.

[0138] Please refer to the specific process Figure 5 :

[0139] The position encoding is performed using sin and cos methods, and the encoded 5D coordinate pose is first normalized to [-1, 1].

[0140] Use sin and cos to encode the position, increasing the 3D position vector to 10 dimensions and the 2D direction vector to 4 dimensions;

[0141] Specifically, the following formula is used to upgrade the 3-dimensional position vector to 10 dimensions and the 2-dimensional direction vector to 4 dimensions.

[0142] γ(p)=(sin(2 0 πp),cos(2 0 πp),…,sin(2 L-1 πp),cos(2 L-1 πp)).

[0143] Redefine FΘ as Here γ is a mapping from R to a higher-dimensional space R2L, and FΘ is still a simple regular MLP, which is a fully connected deep neural network.

[0144] Considering that the tight coupling method of lidar and IMU is used in mine tunnels to improve the accuracy of the entire system in unstructured environments, but this will increase the amount of computation, this embodiment maps 5D coordinates to a high-dimensional space. Mapping the network input to the high-dimensional space increases the distance between points and allows the network to learn more high-frequency information.

[0145] S6. Update the LIO subsystem using the motion compensation information of the point cloud and the IMU pre-integration information to obtain the LIO subsystem information. Directly use the point cloud map of the LIO system and update the system status by observing the map texture (i.e., the RGB color of the point cloud).

[0146] Please refer to Figure 6 In step S6, the LIO subsystem is updated using the motion compensation information of the point cloud and the IMU pre-integration information to obtain the LIO subsystem information. The specific process includes the following steps:

[0147] S61, for the motion compensation information scan of the incoming point cloud, the motion distortion caused by continuous movement within the frame is compensated by IMU back propagation;

[0148] S62, using the error state iterative Kalman filter (ESIKF) of the motion compensation information of the point cloud to minimize the point-to-plane residual to estimate the state of the LIO subsystem;

[0149] S63. In the converged state, the scanned points are appended to the global map, and the corresponding voxels are marked as activated or deactivated, completing the update of the LIO subsystem. Accordingly, the accumulated 3D points in the global map form a geometric structure, which is also used to provide depth for the VIO subsystem.

[0150] S7, updating the VIO subsystem through the dimension-upgraded feature points and IMU pre-integration information to obtain VIO subsystem information;

[0151] Please refer to Figure 7 In step S7, the VIO subsystem is updated using the dimensionally upgraded feature points and IMU pre-integrated information to obtain VIO subsystem information. The specific process includes the following steps:

[0152] S71. Between frames, map points are tracked using the Lucas-Kanade optical flow method, and the system state is estimated by minimizing the projection error of the map points by optimizing the system state using Perspective-n-Point (PnP). The system estimate is then iterated using the ESIKF framework, which is equivalent to the Gauss-Newton method (GN).

[0153] Please refer to Figure 8 , PnP reprojection error calculation:

[0154] In order to calculate the point s in the image frame I k To solve the reprojection error in , we need to transform point s into the camera coordinate system and solve the current state and camera projection model of each ESIKF iteration.

[0155] Frame-to-frame VIO ESIKF updates are performed based on the PnP reprojection error.

[0156] X k The observation distribution of is combined with the prior distribution of IMU propagation to obtain the maximum a posteriori estimate MAP of the error state, which is formulated as follows:

[0157]

[0158] The error state is updated by the following formula:

[0159]

[0160] The above formula is iterated until convergence. The iterative Kalman filter method is equivalent to the GN Gauss-Newton optimization method.

[0161] S72, between the frame and the global map, project the tracked points in the global map into the current image frame, and then minimize the photometric error between the frame and the map for these points, further refining the system state estimate, thereby more accurately estimating the system state. Similarly, the system estimate is iteratively solved using the ESIKF framework;

[0162] First, perform frame-to-map luminosity update;

[0163] The formula for the photometric error from frame to map is as follows:

[0164]

[0165] Among them G s is the color stored in the global map, γ s is the current image I k The color observed in the prediction point s in the current image frame I k The coordinates on Then linear interpolation is performed to obtain the RGB colors of adjacent pixels.

[0166] Then perform frame-to-map VIO ESIKF update;

[0167] The maximum a posteriori (MAP) estimation formula is the same as that used for the maximum a posteriori estimation of the error state above, and then the state update is performed:

[0168]

[0169] Finally, ESIKF is used to iteratively update the frame-to-map VIO until convergence.

[0170] S73. After the VIO update from the frame to the global map, the precise pose of the current image frame can be obtained. Then, by linearly interpolating the RGB colors of the pixels corresponding to the map points on the current image frame and the adjacent pixels, texture rendering is performed to update the color of the points in the global map using the converged state estimate and the original input image.

[0171] In step S73, the specific process includes the following steps:

[0172] S731, rendering the texture of the global map;

[0173] First, all points in all activated voxels are retrieved. Assume that there are n points in total, denoted as ζ={P1,…,P n}. If you click P s Falling in the current image frame, the observed color γ is obtained by linearly interpolating the RGB color values ​​of adjacent pixels on the current image frame. s and covariance Σ nγs Through Bayesian updating, the color of the newly observed point on the image is compared with the existing color value c recorded in the map s Fusion is performed to obtain the updated color value and c s The covariance of is calculated using the following formula:

[0174]

[0175]

[0176]

[0177] S732, VIO subsystem tracking point update;

[0178] After texture rendering is completed, we update the tracked point set P: if the PnP reprojection error or photometric error calculated for a point in point set P is large, it will be deleted from the set of tracking points; if the point is projected onto the current image frame and there are no other tracking points nearby (for example, the radius is set to 50 pixels), it will be added to the point set P.

[0179] The VIO subsystem estimates the state of the system by minimizing the frame-to-frame PnP reprojection error and minimizing the photometric error from frame to global map, and renders the texture of the global map.

[0180] S8. Add IMU information, LIO subsystem information, and VIO subsystem information to the global map update. The LIO system is responsible for building the geometric structure of the map, and the VIO subsystem is responsible for rendering the map texture, that is, the color information of the point cloud.

[0181] In step S8, the IMU information, LIO subsystem information, and VIO subsystem information are added to the global map update. The specific process includes:

[0182] Extract feature points: extract key points in the image and find pixels with certain features in the image;

[0183] Matching feature points: Based on the obtained key point positions, calculate the descriptors of the feature points and construct feature descriptors. Then, compare the descriptors of the feature points with the descriptors to achieve feature matching.

[0184] The LIO system is responsible for building the geometric structure of the map, and the VIO subsystem is responsible for rendering the texture of the map, that is, the color information of the point cloud.

[0185] Please refer to the final experimental results Figure 9 , which is a graph showing the experimental results of the tightly coupled SLAM method based on coded dimensionality increase and fusion of lidar, camera and IMU under the unstructured characteristics of mine shafts in this embodiment.

[0186] The above embodiments provide a detailed introduction to the present invention. Specific examples are used herein to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core ideas. At the same time, for those skilled in the art, according to the ideas of the present invention, there may be changes in the specific implementation methods and application scopes. In summary, the contents of this specification should not be understood as limiting the present invention.

Claims

1. A tightly coupled SLAM method based on coding dimensionality fusion, characterized in that: The following steps are involved: S1. Subscribe to the raw point cloud data collected by the lidar and the raw data collected by the IMU; S2. Calculate motion compensation information of the current frame laser point cloud based on the original point cloud data collected by the lidar and the current frame laser point cloud in the original data collected by the IMU; S3. Pre-integrate the IMU information based on the raw data collected by the IMU and propagate the IMU pre-integrated information; S4. The current frame is acquired through the camera, and optical flow tracking is performed frame by frame. Part of the coordinates are encoded into 5D coordinates, including: S41, subscribe to the current frame information collected by the camera; S42, performing frame-by-frame optical flow tracking; S43, converting the input 5D coordinates by using the position encoding, including: S431, allowing the camera ray to pass through the scene to sample a set of 3D points; S432, inputting a set of spatial points and their corresponding two-dimensional viewing directions into a neural network to generate a set of output colors and volume densities; S433, using classic voxel rendering methods to accumulate these colors and volume densities into a 2D image; S5. Perform dimensionality upgrade on the encoded 5D coordinates through a neural network, and determine feature points in the dimensionality upgrade result; S6. Update the LIO subsystem using the motion compensation information of the point cloud and the IMU pre-integration information to obtain LIO subsystem information; S7, updating the VIO subsystem through the dimension-upgraded feature points and IMU pre-integration information to obtain VIO subsystem information; S8. Add IMU information, LIO subsystem information, and VIO subsystem information to the global map update.

2. The tightly coupled SLAM method according to claim 1, wherein: In step S2, the motion compensation information of the current frame laser point cloud is calculated based on the original point cloud data collected by the lidar and the current frame laser point cloud in the original data collected by the IMU. The specific process includes the following steps: S21. Using the laser radar as a reference, find the IMU data of the start and end times of the points that contain one frame of the laser radar; S22, integrating the above IMU data, taking the first IMU as the reference coordinate, integrating all IMU data in sequence, and obtaining a corresponding pose for each IMU; S23, calculating the attitude of the lidar relative to the first IMU at the end time; S24. Process all points in the laser point cloud of the current frame according to steps S21-S23, calculate the posture of the laser radar relative to the first IMU at the end moment, and then convert it to the laser radar coordinate system at the last moment to obtain the motion compensation information of the laser point cloud of the current frame, and finally repackage it into a frame of laser data and publish it.

3. The tightly coupled SLAM method according to claim 2, wherein: In step S23, the attitude of the lidar relative to the first IMU at the end time is calculated. The specific process includes the following steps: If the odometer queue is synchronized with the laser data, assuming that the time of the i-th and j-th data are and , then solve and The posture of the moment and ; If the above situation does not exist, and If there is no corresponding pose at the moment, the pose at m moments is calculated by linear interpolation of the odometer: Always have a posture, and , then the pose in the above case is obtained and ; exist and During the time period, a total of m poses are taken , and then perform linear interpolation on the pose corresponding to each point of the lidar in the known m+2 poses.

4. The tightly coupled SLAM method according to claim 1, wherein: In step S3, the IMU information is pre-integrated based on the raw data collected by the IMU, and the IMU pre-integrated information is propagated. The specific process includes the following steps: S31, receiving laser odometry data from the raw data collected by the IMU, unifying the laser odometry data into a specified format, and saving the timestamp of the current laser odometry data; S32, remove the IMU incremental data whose timestamp is earlier than the laser odometry timestamp in the IMU queue; S33, calculating the relative pose transformation between the IMU odometers corresponding to the start and end times in the IMU queue; S34, combining the data previously received from the laser odometry and the data obtained from the relative pose transformation previously calculated and publishing them; S35. Subscribe to the raw data collected by the IMU and add it to the queue. Use the result obtained after the previous moment optimization as the starting value of the integration, perform integral calculation on the data in the IMU queue, and publish the IMU increment. S36. Initialize the pre-integrator using the current optimized state, and calculate the IMU data after the current frame to obtain MU pre-integration information; S37. Propagate the MU pre-integration information.

5. The tightly coupled SLAM method according to claim 1, wherein: In step S42, frame-by-frame optical flow tracking is performed, and the specific process includes the following steps: S421. Build a Gaussian pyramid for each frame, with the largest scale image at the top and the original image at the bottom. S422. Estimate the position of the next frame starting from the top layer, use it as the initial position of the next layer, search downward along the pyramid, and repeat the estimation action until the bottom layer of the pyramid is reached.

6. The tightly coupled SLAM method according to claim 1, wherein: In step S6, the LIO subsystem is updated using the point cloud motion compensation information and the IMU pre-integration information to obtain the LIO subsystem information. The specific process includes the following steps: S61, for the motion compensation information scan of the incoming point cloud, the motion distortion caused by continuous movement within the frame is compensated by IMU back propagation; S62, using the error state iterative Kalman filter (ESIKF) of the motion compensation information of the point cloud to minimize the point-to-plane residual to estimate the state of the LIO subsystem; S63. In the converged state, the scanned points are appended to the global map, and the corresponding voxels are marked as activated or deactivated, completing the update of the LIO subsystem.

7. The tightly coupled SLAM method according to claim 1, wherein: In step S7, the VIO subsystem is updated using the dimensionally upgraded feature points and the IMU pre-integrated information to obtain VIO subsystem information. The specific process includes the following steps: S71. Between frames, map points are tracked using the Lucas-Kanade optical flow method, and the system state is estimated by minimizing the projection error of the map points by optimizing the system state with the Perspective-n-Point method. The system estimate is then iterated using the ESIKF framework, which is equivalent to the Gauss-Newton method. S72, between the frame and the global map, projecting the tracked points in the global map into the current image frame, and then minimizing the photometric error between the frame and the map of these points to further refine the state estimate of the system; S73. After the VIO update from the frame to the global map, the precise pose of the current image frame can be obtained. Then, by linearly interpolating the RGB colors of the pixels corresponding to the map points on the current image frame and the adjacent pixels, texture rendering is performed to update the color of the points in the global map using the converged state estimate and the original input image.

8. The tightly coupled SLAM method according to claim 1, wherein: In step S8, the IMU information, LIO subsystem information, and VIO subsystem information are added to the global map update. The specific process includes: Extract feature points: extract key points in the image and find pixels with certain features in the image; Matching feature points: Based on the obtained key point positions, calculate the descriptors of the feature points and construct feature descriptors. Then, compare the descriptors of the feature points with the descriptors to achieve feature matching. The LIO system is responsible for building the geometric structure of the map, and the VIO subsystem is responsible for rendering the texture of the map, that is, the color information of the point cloud.

Images