Underground mine positioning and mapping trajectory optimization method based on downhole control point constraint

Abstract

The application discloses a kind of underground mine positioning and mapping trajectory optimization methods based on underground control point constraint, belong to positioning and mapping field, this method includes respectively acquiring GNSS data, laser data, IMU data and image data;According to GNSS data, laser data, IMU data and image data, the initialization of laser radar-vision-inertial odometry system and GNSS initialization are completed;Based on RANSAC algorithm, the point cloud features of the laid underground control points are extracted in real time, and the global coordinate constraint factor of nonlinear least squares optimization is generated;Relative attitude solution is carried out according to global coordinate constraint factor, and underground mine positioning and mapping trajectory optimization is completed.The application solves the problem of accumulated error overrun and point cloud mispositioning layering caused by pose drift in special environment of underground mine.

Description

Technical Field

[0001] This invention belongs to the field of positioning and mapping, and particularly relates to a method for optimizing the positioning and mapping trajectory of underground mines based on underground control point constraints. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a technology that allows a moving object, with an uncertain position, to create a map consistent with its environment using its own sensors, while simultaneously determining its own position on the map. In recent years, with the rapid development of technologies such as the Internet, artificial intelligence, and big data, and the further improvement of computer data processing capabilities, SLAM technology has made significant progress, especially in real-time 3D reconstruction and localization in unknown environments. Initially, the SLAM framework combined a single measurement sensor, a primary camera, or a LiDAR sensor to construct a state and a 3D map—a technique known as visual SLAM. It utilizes cost-effective CMOS sensors and lenses to establish accurate data associations, thereby achieving a certain level of localization accuracy. Rich color information further enriches semantic perception. Utilizing this enhanced scene understanding, deep learning methods are used for robust feature extraction and dynamic object filtering. However, visual SLAM lacks direct depth measurement, thus requiring simultaneous optimization of map points through operations such as triangulation or depth filtering. This approach typically limits the accuracy and density of map construction, the varying measurement noise at different scales, sensitivity to lighting changes, and the impact of textureless environments on data association. Compared to visual SLAM, LiDAR SLAM technology utilizes LiDAR sensors to directly obtain accurate depth measurements, providing higher accuracy and efficiency in localization and map building. However, while the point cloud maps reconstructed by LiDAR SLAM are detailed, they lack color information; localization accuracy often deteriorates in environments with insufficient geometric constraints, such as narrow tunnels, single walls, and extended walls. Typically, existing SLAM technologies, relying solely on a single sensor in environments lacking structure or texture, cannot provide accurate and robust pose estimation. Therefore, the fusion of commonly used sensors such as LiDAR, cameras, and IMUs is gaining increasing importance. However, SLAM systems employing multi-sensor fusion still face the following challenges in mapping:

[0003] ① Efficiency in processing large amounts of point cloud data and high-resolution images;

[0004] ② In environments lacking structure or texture, the LiDAR inertial odometry (LIO) subsystem and the visual inertial odometry (VIO) subsystem extract very limited feature points from visual and LiDAR data, respectively, leading to reduced accuracy.

[0005] To ensure the accuracy of the reconstructed color point cloud and achieve pixel-level precision in pose estimation, it is necessary to synchronize hardware, perform rigorous pre-calibration of external parameters between LiDAR and camera, accurately recover exposure time, and implement a fusion strategy that can achieve pixel-level precision in real time.

[0006] Accuracy analysis of the collected tunnel point clouds revealed that existing multi-sensor fusion positioning and mapping algorithms often encounter problems such as excessive cumulative errors and misaligned point cloud layering due to pose drift in the special environments of underground mines with no GNSS signals, low lighting, and missing structural features. Figure 1 and Figure 2 As shown in the figure, in the field of mineral resources, existing 3D laser scanning equipment has difficulty obtaining complete color point clouds with realistic textures in underground tunnels. Currently, most of the data is black and white point cloud data that reflects 3D spatial information, and even after color point cloud rendering, it still cannot intuitively reflect the current status information of the tunnel. Summary of the Invention

[0007] To address the aforementioned shortcomings in existing technologies, this invention provides an underground mine positioning and mapping trajectory optimization method based on underground control point constraints, which solves the problem of cumulative error exceeding limits and point cloud misalignment and layering caused by pose drift in the special environment of underground mines.

[0008] To achieve the aforementioned objectives, the technical solution adopted by this invention is: a method for optimizing the positioning and mapping trajectory of underground mines based on underground control point constraints, comprising:

[0009] Acquire GNSS data, laser data, IMU data, and image data respectively;

[0010] The initialization of the lidar-vision-inertial odometry system and the GNSS initialization are completed based on GNSS data, laser data, IMU data, and image data.

[0011] The point cloud features of the deployed downhole control points are extracted in real time based on the RANSAC algorithm, and a global coordinate constraint factor is generated by nonlinear least squares optimization.

[0012] The relative attitude is calculated based on the global coordinate constraint factor to complete the positioning and mapping trajectory optimization of the underground mine.

[0013] Furthermore, the initialization of the lidar-vision-inertial odometry system and the GNSS initialization are completed based on GNSS data, laser data, IMU data, and image data, specifically as follows:

[0014] GNSS data, laser data, IMU data, and image data are preprocessed, and the preprocessed GNSS data, laser data, IMU data, and image data are time-aligned to obtain time-aligned multi-sensor data;

[0015] Based on time-aligned multi-sensor data, the extrinsic parameters of each sensor are calibrated to obtain the laser-IMU extrinsic parameter matrix, camera-IMU extrinsic parameter matrix, and GNSS-IMU extrinsic parameter matrix.

[0016] Based on time-aligned multi-sensor data, GNSS data is defiltered to obtain filtered GNSS data.

[0017] Laser feature points are obtained by extracting point cloud features from laser data.

[0018] Pre-integrate the IMU data to obtain the IMU pre-integrated value and the zero bias initial value;

[0019] Corner detection and optical flow tracing are performed on the image data to obtain visual feature points;

[0020] Initialize the lidar-vision-inertial odometry system: complete the lidar-IMU initialization based on the lidar feature points, IMU pre-integration, and zero bias initial value to obtain the initial state of the lidar-IMU; complete the vision-IMU initialization based on the vision feature points and the initial state of the lidar-IMU to obtain the IMU pose and the world coordinate system gravity vector;

[0021] Determine whether the IMU pose and the world coordinate system gravity vector have converged. If so, perform GNSS initialization; otherwise, return to perform lidar-vision-inertial odometry system initialization.

[0022] Furthermore, the GNSS initialization is as follows: based on the filtered GNSS data, GNSS anchor point calculation and heading angle alignment are performed to obtain the initial pose of global anchoring, thus completing the GNSS initialization.

[0023] Furthermore, the expression for the global coordinate constraint factor is:

[0024]

[0025]

[0026] in, This is the global coordinate constraint factor; Number of control points; For the world coordinate system The coordinates of the control points; This is the rotation correction matrix; The first for pose prediction Coordinates of control points; This is the translation correction vector; It is an L2 norm; For the first The optimized rotation matrix for each control point; For the first Rotation matrix before optimization of each control point; For the first Optimized translation correction vector for each control point; For the first Translation correction vector of each control point before optimization; The prior residual vector; For the first The attitude parameters in quaternion form after optimization of each control point; It is a quaternion cross product; For the first The inverse of the attitude parameter expression in quaternion form before optimization of each control point; To convert the results of quaternion-related operations into a three-dimensional vector form.

[0027] Furthermore, the step of performing relative attitude calculation based on global coordinate constraint factors to optimize the underground mine positioning and mapping trajectory specifically involves: introducing global coordinate constraint factors into the factor map of the lidar-vision-inertial odometry system, and performing relative attitude calculation based on the factor map with introduced global coordinate constraint factors to optimize the underground mine positioning and mapping trajectory; the expression for the factor map with introduced global coordinate constraint factors is:

[0028]

[0029] in, Factor plot for introducing global coordinate constraint factors; This is the visual reprojection error factor; for The weights; for The weights; For IMU pre-integration factor; for The weights; The point-to-surface registration factor for the laser point cloud; for The weights; This is the global coordinate constraint factor; for The weights; This is the lap-loop constraint factor; This is a transpose.

[0030] Furthermore, the expression for the visual reprojection error factor is:

[0031]

[0032]

[0033] in, The coordinates of the object points in the optimized map; The coordinates of the object point on the map; for The z-component; To take only the x and y components of the vector as the residual; The image plane coordinates of the visual feature points; This is the rotation matrix from the camera coordinate system to the carrier coordinate system; For the world coordinate system to the 1st Rotation matrix in the carrier coordinate system under the frame; For the first The rotation matrix from the carrier coordinate system to the world coordinate system in the frame; This is the rotation matrix from the carrier coordinate system to the camera coordinate system; Inverse depth; For the first Object coordinates in the optimized map at frame rate; This is the translation vector from the camera coordinate system to the vehicle coordinate system; For the first Translation vector from the carrier coordinate system to the world coordinate system in the frame; For the first Translation vector from the carrier coordinate system to the world coordinate system in the frame; The x-coordinate of the image plane of the visual feature point; The vertical coordinate of the image plane of the visual feature point is denoted by .

[0034] Furthermore, the expression for the IMU pre-integration factor is:

[0035]

[0036]

[0037]

[0038] in, To the world coordinate system The rotation matrix of the body coordinate system at any given time; for The position vector of the IMU in the world coordinate system at any given time; for The position vector of the IMU in the world coordinate system at any given time; This is the gravitational acceleration vector in the world coordinate system; for time; The current reference frame; for The velocity vector of the IMU in the world coordinate system at any given moment; The result of pre-integration calculation arrive At any given moment, the position integral based on the body coordinate system; for The velocity vector of the IMU in the world coordinate system at any given moment; The result of pre-integration calculation arrive At any given moment, the velocity integral based on the body coordinate system; for The inverse of the rotation quaternion from the body coordinate system to the world coordinate system at any given moment; It is a quaternion cross product; for The rotation quaternion from the body coordinate system to the world coordinate system at any given time; The result of pre-integration calculation arrive At any given moment, the attitude integral based on the body coordinate system; To convert the results of quaternion-related operations into a three-dimensional vector form; for Accelerometer deviation at any given time; for Accelerometer deviation at any given time; for Gyroscope deviation at any given moment; for Gyroscope deviation at any given moment; for Time body coordinate system to The rotation matrix of the body coordinate system at any given time; For IMU in Acceleration measured at any given time; For IMU accelerometer in Timing deviation; For time; This is an operation to convert an angular velocity vector into an antisymmetric matrix; For IMU in Angular velocity measured at any given time; For IMU gyroscope in Timing deviation; The result of pre-integration calculation Time's up Attitude integral based on the body coordinate system at any given moment; It is the antisymmetric matrix form of angular velocity; This is the angular velocity matrix; Angular velocity; The x-component of the angular velocity; The y-component of the angular velocity; Let z be the z-component of the angular velocity.

[0039] Furthermore, the expression for the laser point cloud point-to-surface registration factor is:

[0040]

[0041] in, In the world coordinate system, this is the unit normal vector of the plane corresponding to the point cloud; for The rotation matrix from the body coordinate system to the world coordinate system at any given time; The rotation matrix from the laser radar to the carrier coordinate system; In the lidar coordinate system, the first The coordinates of each laser point; This is the translation vector from the laser radar to the carrier coordinate system; for The position vector of the IMU in the world coordinate system at any given time; for The position vector of the IMU in the world coordinate system at any given time.

[0042] Furthermore, the expression for the loop constraint factor is:

[0043]

[0044] in, For the first The carrier coordinate system under the frame to the first Rotation of the carrier coordinate system within the frame; In the world coordinate system, the first The inverse of the position vector in the frame's carrier coordinate system; In the world coordinate system, the first The inverse of the position vector in the frame's carrier coordinate system; For the world coordinate system to the 1st The inverse of the rotation matrix of the frame's carrier coordinate system; In the world coordinate system, the first The position vector of the origin of the carrier coordinate system of the frame; In the world coordinate system, the first The position vector of the origin of the carrier coordinate system of the frame; The first one obtained by loop closure detection The carrier coordinate system of the frame to the first Translation constraints of the frame's carrier coordinate system; To convert the results of quaternion-related operations into a three-dimensional vector form; It is a quaternion cross product.

[0045] Furthermore, the optimization objective of the mapping trajectory is:

[0046]

[0047] in, These are trajectory parameters; In order to make smallest The possible values ​​of ; Factor graph for introducing global coordinate constraint factors.

[0048] The beneficial effects of this invention are as follows: This invention not only fuses laser point clouds with image features, but also solves the problems of excessive cumulative errors and misaligned point cloud layering in underground mine positioning and mapping. Specifically, firstly, a total station is used to accurately measure a small set of underground control points in the underground tunnels to establish a global benchmark in the CGCS2000 coordinate system; then, based on the RANSAC algorithm, the point cloud features of the underground control points are extracted in real time to generate a nonlinear least squares optimized global coordinate constraint factor; finally, the coordinate constraints of the underground control points are introduced into the factor graph model of the odometer for dynamic real-time solution, thereby effectively ensuring high-precision positioning performance of long-distance trajectories after long-term operation. Attached Figure Description

[0049] Figure 1 This is the original point cloud accuracy map.

[0050] Figure 2 This is a schematic diagram of multi-layer errors in point clouds.

[0051] Figure 3 This is a flowchart of the method of the present invention.

[0052] Figure 4 This is a schematic diagram of a laser-vision-inertial odometer that integrates downhole control point constraints in an embodiment of the present invention.

[0053] Figure 5 This is a schematic diagram illustrating the first application effect of the image feature fusion algorithm for mine roadways and mining equipment in this embodiment of the invention.

[0054] Figure 6 This is a schematic diagram illustrating the second application effect of the image feature fusion algorithm for mine roadways and mining equipment in this embodiment of the invention.

[0055] Figure 7 This is a schematic diagram comparing the registration results in an embodiment of the present invention.

[0056] Figure 8 This is a schematic diagram showing the local effects before and after registration in an embodiment of the present invention.

[0057] Figure 9 This is a schematic diagram illustrating the overlay effect of the registered point cloud and downhole control points in an embodiment of the present invention. Detailed Implementation

[0058] The specific embodiments of the present invention are described below to enable those skilled in the art to understand the present invention. However, it should be understood that the present invention is not limited to the scope of the specific embodiments. For those skilled in the art, various changes are obvious as long as they are within the spirit and scope of the present invention as defined and determined by the appended claims. All inventions utilizing the concept of the present invention are protected.

[0059] Example 1

[0060] like Figure 3 As shown, in one embodiment of the present invention, a method for optimizing the location and mapping trajectory of an underground mine based on underground control point constraints includes:

[0061] Acquire GNSS data, laser data, IMU data, and image data respectively;

[0062] The initialization of the lidar-vision-inertial odometry system and the GNSS initialization are completed based on GNSS data, laser data, IMU data, and image data.

[0063] The point cloud features of the deployed downhole control points are extracted in real time based on the RANSAC algorithm, and a global coordinate constraint factor is generated by nonlinear least squares optimization.

[0064] The relative attitude is calculated based on the global coordinate constraint factor to complete the positioning and mapping trajectory optimization of the underground mine.

[0065] This invention not only fuses laser point clouds with image features but also solves the problems of excessive cumulative errors and misaligned point cloud layering in underground mine positioning and mapping. Specifically, firstly, a total station is used to accurately measure a small set of underground control points in the underground tunnels to establish a global benchmark in the CGCS2000 coordinate system; then, the point cloud features of the underground control points are extracted in real time based on the RANSAC algorithm to generate a nonlinear least squares optimized global coordinate constraint factor; finally, the coordinate constraints of the underground control points are introduced into the factor graph model of the odometer for dynamic real-time solution, thereby effectively ensuring high-precision positioning performance of long-distance trajectories after long-term operation.

[0066] The positioning and mapping platform based on multi-sensor data fusion collects various sensor data, including laser data, image data, IMU data, and GNSS data, as the core input to the algorithm. Through preprocessing, initialization, and optimization processes, it improves the relative positioning and mapping accuracy in underground tunnel environments. Figure 3 As shown.

[0067] In surface environments, GNSS measurement data, due to its high accuracy and global positioning capabilities, is used for initial alignment with the CGCS2000 coordinate system. However, in downhole environments, weak GNSS signals can render the data ineffective due to signal reception problems, failing to establish effective global constraints. Furthermore, positioning and mapping based solely on laser-vision-inertial navigation (LAC) results in significant accumulated errors over extended periods. To further improve the accuracy of the scanned point cloud in the global coordinate system, global constraints from downhole control points were added to the architecture. Firstly, control points with known absolute coordinates provide geometric reference features for relative attitude calculations, making the odometer more robust and enabling more accurate estimation of motion states. Secondly, the global coordinate constraints provided by the deployed control points introduce a global constraint factor into the relative attitude calculation, limiting error accumulation and further improving the accuracy of the global attitude.

[0068] To achieve effective fusion and positioning of multi-sensor data, a factor graph optimization method is used to estimate the state of the sensors. The odometry part is divided into laser-assisted visual-inertial odometry and laser-visual-inertial odometry with fusion control point constraints. A loop closure detection module is also added to the system.

[0069] This invention adopts a vision-inertial odometry design similar to LVI-SAM. In the vision front-end, a corner detector is used to detect easily trackable corners, and the Kanade-Lucas-Tomasi optical flow method is employed for texture tracking of these corners. In this invention, the system's state vector... Defined as:

[0070]

[0071] in It is the rotation matrix from the carrier coordinate system to the world coordinate system. It is a translation vector. It is the velocity vector and It refers to the acceleration bias of the IMU sensor and the gyroscope bias.

[0072] Most optimization-based visual-inertial odometry systems typically require sufficient IMU motion excitation during initialization to estimate scale information. However, visual-inertial odometry often fails under conditions of uniform motion or small acceleration because the IMU is not observable in terms of scale information.

[0073] LiDAR sensors have a natural advantage in distance measurement, as their distance measurements already contain scale information. Therefore, to improve the initialization robustness of visual-inertial odometry (VIO), LiDAR measurement information can be directly incorporated, making the scale of the VIOS more comparable and accelerating the convergence speed of the initialization optimization. Specifically, the LiDAR-inertial odometry is initialized first, after which it acquires the system state. and its associated system bias Then (where k is the initialization window length), all image keyframes within the initialization window length are linearly interpolated and correlated with the system state, providing good initial values ​​for the visual-inertial odometry initialization. With the assistance of such laser scale information, the visual-inertial odometry becomes more robust and converges faster.

[0074] Besides providing scale information to the vision subsystem, LiDAR's laser scanning points can also provide depth priors for visual feature points, and can also eliminate the need for triangulation of visual feature points. To correctly assign depth values ​​to feature points, this invention first projects the visual feature points and LiDAR scanning points on the image onto a unit ellipsoid projection, and then performs a two-dimensional KD-tree query on the detected feature points to find the three nearest neighbors that do not exceed the distance limit. The depth prior is obtained by interpolating the triangular facets formed by the laser points. Then, the validity of the interpolated feature points is checked, especially those at geometric edges, where depth abrupt changes are prone to occur. By comparing the depth difference between the nearest neighbor LiDAR depth points of the interpolated feature points, it is determined whether the depth prior of the feature point is abrupt, and unstable depth prior values ​​are discarded.

[0075] To further optimize the sensor pose, it is necessary to simultaneously optimize the poses of all associated camera sensors using feature points in the scene; this process is also known as bundle adjustment. The image plane coordinates of the visual feature points... coordinates of the object point on the map The residual model between them can be established as follows:

[0076]

[0077] in and It is the extrinsic parameter between the camera coordinate system and the carrier coordinate system. and It is the first The transformation from the carrier coordinate system to the world coordinate system in the frame. This means that only the first two terms of the vector are taken as the residual. To optimize the sensor pose, only the inverse depth in the residual model needs to be considered. Camera attitude parameters and If the inverse depth of a feature point is the laser prior value, then optimization is not necessary.

[0078] The IMU pre-integration factor is calculated using the median method based on the current reference frame of the IMU raw data. By accumulating points, you can obtain the following points:

[0079]

[0080] The three integral terms correspond to the translation integral term, the velocity integral term, and the rotation integral term, respectively, where:

[0081]

[0082] The integral term can then be used to establish the following residual model with respect to the sensor attitude:

[0083]

[0084] in It is a quaternion form of attitude parameter expression, while This represents quaternion multiplication.

[0085] The laser-inertial initialization assumes that the sensors are stationary during initial startup. This initialization first uses the gravitational acceleration vector measured by the IMU under stationary conditions to align the system's gravity direction. Then, it initializes the IMU's acceleration and acceleration bias using the inter-frame matching results of the lidar under stationary conditions. Finally, after the laser-inertial odometry initialization is completed, the estimated IMU bias and system attitude are input into the vision-inertial initialization module, completing the overall laser-vision-inertial navigation initialization.

[0086] like Figure 4 As shown, in the laser odometry section, an overall pose graph is maintained internally by the system. This pose graph includes five constraints: IMU pre-integration constraints, visual odometry constraints, lidar odometry constraints, loop closure constraints, and downhole control point constraints. These constraints work together to optimize the trajectory. The following section describes how visual constraints are jointly optimized with other constraints:

[0087] Laser odometry constraints are pose constraints obtained by registering the laser-scanned point cloud of the current frame with the historical scanned point cloud map. The accuracy and robustness of point cloud registration place high demands on the initial pose values, especially in degenerate scenarios with insufficient geometric constraints. Therefore, the visual-inertial odometry component can provide the system with initial pose values ​​for inter-frame point cloud matching, enhancing the system's perception of texture features in areas with fewer geometric constraints, thereby improving the overall robustness and relative accuracy of the system. Simultaneously, the laser odometry system also integrates a degradation detection mechanism; if significant degradation occurs in the scene, laser odometry optimization is not performed on the current frame.

[0088] Laser odometry optimizes sensor attitude primarily using a point-to-plane geometric constraint model from the current frame to the historical map. This model searches the historical map for points in the current frame that are compatible with the current frame. The five nearest neighbors are identified, and the normal vector of the thin plane formed by these five points is obtained through PCA analysis or least squares method. and a point on that plane Then the residual model of the constraint factor can be expressed as:

[0089]

[0090] in and This is the extrinsic parameter matrix of the laser-radar sensor reaching the carrier coordinate system. Optimizing the sensor attitude only requires adjusting the residual model... and The Jacobian matrix can be obtained by finding it.

[0091] Because GNSS signals are weak in underground mine tunnels, effective global constraints cannot be formed. Furthermore, positioning and mapping based solely on laser-vision-inertial navigation will accumulate significant errors over long periods. To further improve the accuracy of the scanned point cloud in the global coordinate system, this invention proposes deploying a small number of control points underground. These control points provide global coordinate constraints, adding global constraints to the factor graph optimization. Specifically, before data acquisition and processing, the coordinates of the measured control points need to be... As control data input into the system (coordinate data converted from CGCS2000 to the carrier's local world coordinate system), during the scanning process, targets deployed within the scene are extracted, and target points are fitted and incorporated into the optimization. Due to the high reflectivity of the target material, a coarse extraction is first performed based on point cloud intensity information. Then, based on the extracted target point cloud, the RANSAC method is used to fit the target center. Subsequently, the nearest neighbor query is performed using the target center's coordinates in the world coordinate system and the control point coordinates in the control database to obtain the control point coordinates in real time. The residual model between the fitted center point and control point coordinates can be expressed as:

[0092]

[0093] The correction information can be directly obtained from this formula through SVD decomposition. and This information is then incorporated as a constraint into the factor graph optimization, and its residual model can be expressed as:

[0094]

[0095] The quaternion form With rotation matrix form These are different parameter forms representing the attitude of the same sensor.

[0096] This invention uses loop closure detection based on Euclidean distance. The backend starts a new thread to perform distance detection on the pose of the current frame. When the odometry pose and the historical pose meet a distance threshold in Euclidean distance, the current frame is matched and optimized with the historical map to obtain the relative pose. The problem of this pose graph optimization can then be expressed as:

[0097]

[0098] The residual model requires obtaining the corresponding Jacobian matrix for all poses involved in the optimization.

[0099] Thus far, all optimization factors of the multi-sensor fusion localization and mapping algorithm that integrates global constraints of downhole control points have been introduced. The optimization problem that this system needs to solve can be expressed as follows:

[0100]

[0101] in The weights between the various factors are represented by , and this nonlinear optimization problem is optimized using the Levenberg-Marquardt method.

[0102] GNSS systems typically calculate receiver coordinates in the Earth-Center Earth-Fixed (ECEF) coordinate system, while laser-vision-inertial systems typically calculate coordinates in the local world frame (LOF) coordinate system. This requires coordinate transformation and alignment between the ECEF and LEF coordinate systems.

[0103] In this embodiment, the ECEF coordinate system selected is the CGCS2000 coordinate system, and the coordinates of the aforementioned control points are also in the CGCS2000 coordinate system. The coordinates of the GNSS system are calculated using RTK technology. To transform the geocentric-fixed coordinate system to the local world coordinate system, it is first necessary to transform the geocentric-fixed coordinate system to the East-North-Upcoordinates (ENU) coordinate system. This transformation relationship can be expressed by the following formula:

[0104]

[0105] in It is the rotation matrix for transforming from the East-North-Sky coordinate system to the Earth-centered Earth-fixed coordinate system. L It's longitude. B It's latitude. It is the corresponding translation vector, that is, the receiver coordinates measured in the geocentric coordinate system. The coordinates of the point in the ECEF coordinate system These are the coordinates of a point in the ENU coordinate system.

[0106] Since the local world coordinate system is based on the coordinate system of the first frame of the carrier during data acquisition, although this coordinate system is aligned with gravity, its relationship with the yaw angle of the East-North-Sky coordinate system is still unknown. Therefore, it is still necessary to determine the yaw angle. Perform the calculation. Given the heading angle, alignment between the local world coordinate system and the East-North-Sky coordinate system can be achieved using the following formula:

[0107]

[0108] The above transformation relationship is reversible, that is, it can be transformed from the geocentric coordinate system to the local world coordinate system. At this point, the coordinate system of the GNSS system is unified with the coordinate system of the laser-vision-inertial system.

[0109] Using the trajectory optimization algorithm described above, multi-source sensor data, including LiDAR point clouds, image data, and IMU inertial navigation measurement data, are fused. First, the inverse depth parameterization method in the LVI-SAM framework is adopted. A dense depth map is generated through neighborhood plane fitting and interpolation of image feature points, establishing the geometric correspondence between the LiDAR point cloud and image pixels. Then, a residual model of the image plane coordinates of image feature points and the object-side coordinates of the map is established using bundle adjustment. This effectively fuses the point cloud data with image features. The fused true-color point cloud model is detailed below. Figure 5 (Figures a and c show the color rendering point cloud effect, while figures b and d show the effect of algorithm application.) Figure 6As shown, the fused true-color point cloud model not only possesses detailed 3D geometric information but also boasts more realistic and richer color textures, making target features more prominent and the scene more lifelike.

[0110] Based on nine high-precision control points deployed underground, a global constraint factor graph model was constructed. Point cloud registration was performed using a point-to-surface geometric constraint model. Details of the registration results before and after registration, as well as the alignment of the trajectory with the control points before and after trajectory optimization, are available in [link to documentation]. Figure 7 to Figure 9 As shown in the figure, the proposed factor graph optimization algorithm for fusing control point constraint factors significantly improves the accuracy of the point cloud in global coordinates, and the coordinates of the registered point cloud with the same-name feature points are well superimposed with the coordinates of the downhole control points.

[0111] Example 2

[0112] This invention includes the following steps:

[0113] Acquire GNSS data, laser data, IMU data, and image data respectively;

[0114] The initialization of the lidar-vision-inertial odometry system and the GNSS initialization are completed based on GNSS data, laser data, IMU data, and image data.

[0115] The point cloud features of the deployed downhole control points are extracted in real time based on the RANSAC algorithm, and a global coordinate constraint factor is generated by nonlinear least squares optimization.

[0116] The relative attitude is calculated based on the global coordinate constraint factor to complete the positioning and mapping trajectory optimization of the underground mine.

[0117] The initialization of the lidar-vision-inertial odometry system and GNSS initialization are completed based on GNSS data, laser data, IMU data, and image data, specifically as follows:

[0118] GNSS data, laser data, IMU data, and image data are preprocessed, and the preprocessed GNSS data, laser data, IMU data, and image data are time-aligned to obtain time-aligned multi-sensor data;

[0119] Based on time-aligned multi-sensor data, the extrinsic parameters of each sensor are calibrated to obtain the laser-IMU extrinsic parameter matrix, camera-IMU extrinsic parameter matrix, and GNSS-IMU extrinsic parameter matrix.

[0120] Based on time-aligned multi-sensor data, GNSS data is defiltered to obtain filtered GNSS data.

[0121] Laser feature points are obtained by extracting point cloud features from laser data.

[0122] Pre-integrate the IMU data to obtain the IMU pre-integrated value and the zero bias initial value;

[0123] Corner detection and optical flow tracing are performed on the image data to obtain visual feature points;

[0124] Initialize the lidar-vision-inertial odometry system: complete the lidar-IMU initialization based on the lidar feature points, IMU pre-integration, and zero bias initial value to obtain the initial state of the lidar-IMU; complete the vision-IMU initialization based on the vision feature points and the initial state of the lidar-IMU to obtain the IMU pose and the world coordinate system gravity vector;

[0125] Determine whether the IMU pose and the world coordinate system gravity vector have converged. If so, perform GNSS initialization; otherwise, return to perform lidar-vision-inertial odometry system initialization.

[0126] The GNSS initialization is as follows: GNSS anchor point calculation and heading angle alignment are performed based on the filtered GNSS data to obtain the initial pose of global anchoring, thus completing the GNSS initialization.

[0127] In this embodiment, the initial state of the laser-IMU includes the initial pose of the first IMU, the first IMU zero bias, and the gravity vector of the world coordinate system. Specifically, obtaining the initial state of the laser-IMU involves:

[0128] By matching the laser feature points of adjacent frames, the inter-frame pose transformation is obtained;

[0129] Based on the inter-frame pose transformation, IMU pre-integral value, and initial zero bias value, the initial pose and zero bias of the IMU are optimized to obtain the first initial pose and the first zero bias of the IMU.

[0130] Based on the first IMU's zero bias, the IMU acceleration after removing the zero bias is obtained;

[0131] Based on the initial pose of the first IMU and the IMU acceleration after removing the zero bias, the gravity direction is fitted to obtain the gravity vector in the world coordinate system.

[0132] The initial state after visual-IMU alignment includes the second camera-IMU extrinsic parameter matrix, the second visual feature depth, and the initial pose of the second IMU; specifically, obtaining the initial state after visual-IMU alignment involves:

[0133] The laser data is transformed to the camera coordinate system using the laser-camera extrinsic parameter matrix to obtain the laser point cloud in the camera coordinate system;

[0134] Based on camera intrinsic parameters, the laser point cloud in the camera coordinate system is projected onto the pixel plane to obtain the physical depth of the laser point cloud;

[0135] The physical depth of the laser point cloud is assigned to the visual feature points to obtain the first visual feature depth;

[0136] By utilizing reprojection error and IMU pre-integration constraints, the extrinsic matrix of the first camera-IMU, the depth of the first visual feature, and the initial pose of the first IMU are optimized to obtain the extrinsic matrix of the second camera-IMU, the depth of the second visual feature, and the initial pose of the second IMU.

[0137] The expression for the global coordinate constraint factor is:

[0138]

[0139]

[0140] in, This is the global coordinate constraint factor; Number of control points; For the world coordinate system The coordinates of the control points; This is the rotation correction matrix; The first for pose prediction Coordinates of control points; This is the translation correction vector; It is an L2 norm; For the first The optimized rotation matrix for each control point; For the first Rotation matrix before optimization of each control point; For the first Optimized translation correction vector for each control point; For the first Translation correction vector of each control point before optimization; The prior residual vector; For the first The attitude parameters in quaternion form after optimization of each control point; It is a quaternion cross product; For the first The inverse of the attitude parameter expression in quaternion form before optimization of each control point; To convert the results of quaternion-related operations into a three-dimensional vector form.

[0141] The process of calculating relative attitude based on global coordinate constraint factors to optimize the positioning and mapping trajectory of the underground mine involves: incorporating global coordinate constraint factors into the factor map of the LiDAR-Vision-Inertial Odometry system; and performing relative attitude calculation based on the factor map with global coordinate constraint factors to optimize the positioning and mapping trajectory of the underground mine. The expression for the factor map with global coordinate constraint factors is as follows:

[0142]

[0143] in, Factor plot for introducing global coordinate constraint factors; This is the visual reprojection error factor; for The weights; for The weights; For IMU pre-integration factor; for The weights; The point-to-surface registration factor for the laser point cloud; for The weights; This is the global coordinate constraint factor; for The weights; This is the lap-loop constraint factor; This is a transpose.

[0144] The expression for the visual reprojection error factor is:

[0145]

[0146]

[0147] in, The coordinates of the object points in the optimized map; The coordinates of the object point on the map; for The z-component; To take only the x and y components of the vector as the residual; The image plane coordinates of the visual feature points; This is the rotation matrix from the camera coordinate system to the carrier coordinate system; For the world coordinate system to the 1st Rotation matrix in the carrier coordinate system under the frame; For the first The rotation matrix from the carrier coordinate system to the world coordinate system in the frame; This is the rotation matrix from the carrier coordinate system to the camera coordinate system; Inverse depth; For the first Object coordinates in the optimized map at frame rate; This is the translation vector from the camera coordinate system to the vehicle coordinate system; For the first Translation vector from the carrier coordinate system to the world coordinate system in the frame; For the first Translation vector from the carrier coordinate system to the world coordinate system in the frame; The x-coordinate of the image plane of the visual feature point; The vertical coordinate of the image plane of the visual feature point is denoted by .

[0148] The expression for the IMU pre-integration factor is:

[0149]

[0150]

[0151]

[0152] in, To the world coordinate system The rotation matrix of the body coordinate system at any given time; for The position vector of the IMU in the world coordinate system at any given time; for The position vector of the IMU in the world coordinate system at any given time; This is the gravitational acceleration vector in the world coordinate system; for time; The current reference frame; for The velocity vector of the IMU in the world coordinate system at any given moment; The result of pre-integration calculation arrive At any given moment, the position integral based on the body coordinate system; for The velocity vector of the IMU in the world coordinate system at any given moment; The result of pre-integration calculation arrive At any given moment, the velocity integral based on the body coordinate system; for The inverse of the rotation quaternion from the body coordinate system to the world coordinate system at any given moment; It is a quaternion cross product; for The rotation quaternion from the body coordinate system to the world coordinate system at any given time; The result of pre-integration calculation arrive At any given moment, the attitude integral based on the body coordinate system; To convert the results of quaternion-related operations into a three-dimensional vector form; for Accelerometer deviation at any given time; for Accelerometer deviation at any given time; for Gyroscope deviation at any given moment; for Gyroscope deviation at any given moment; for Time body coordinate system to The rotation matrix of the body coordinate system at any given time; For IMU in Acceleration measured at any given time; For IMU accelerometer in Timing deviation; For time; This is an operation to convert an angular velocity vector into an antisymmetric matrix; For IMU in Angular velocity measured at any given time; For IMU gyroscope in Timing deviation; The result of pre-integration calculation Time's up Attitude integral based on the body coordinate system at any given moment; It is the antisymmetric matrix form of angular velocity; This is the angular velocity matrix; Angular velocity; The x-component of the angular velocity; The y-component of the angular velocity; Let z be the z-component of the angular velocity.

[0153] The expression for the laser point cloud point-to-surface registration factor is:

[0154]

[0155] in, In the world coordinate system, this is the unit normal vector of the plane corresponding to the point cloud; for The rotation matrix from the body coordinate system to the world coordinate system at any given time; The rotation matrix from the laser radar to the carrier coordinate system; In the lidar coordinate system, the first The coordinates of each laser point; This is the translation vector from the laser radar to the carrier coordinate system; for The position vector of the IMU in the world coordinate system at any given time; for The position vector of the IMU in the world coordinate system at any given time.

[0156] The expression for the loop constraint factor is:

[0157]

[0158] in, For the first The carrier coordinate system under the frame to the first Rotation of the carrier coordinate system within the frame; In the world coordinate system, the first The inverse of the position vector in the frame's carrier coordinate system; In the world coordinate system, the first The inverse of the position vector in the frame's carrier coordinate system; For the world coordinate system to the 1st The inverse of the rotation matrix of the frame's carrier coordinate system; In the world coordinate system, the first The position vector of the origin of the carrier coordinate system of the frame; In the world coordinate system, the first The position vector of the origin of the carrier coordinate system of the frame; The first one obtained by loop closure detection The carrier coordinate system of the frame to the first Translation constraints of the frame's carrier coordinate system; To convert the results of quaternion-related operations into a three-dimensional vector form; It is a quaternion cross product.

[0159] The optimization objective of the mapping trajectory is:

[0160]

[0161] in, These are trajectory parameters; In order to make smallest The possible values ​​of ; Factor graph for introducing global coordinate constraint factors.

Claims

1. A method for optimizing the positioning and mapping trajectory of underground mines based on underground control point constraints, characterized in that, include: GNSS data, laser data, IMU data, and image data were acquired separately. The initialization of the lidar-vision-inertial odometry system and the GNSS initialization are completed based on GNSS data, laser data, IMU data, and image data. The point cloud features of the deployed downhole control points are extracted in real time based on the RANSAC algorithm, and a global coordinate constraint factor is generated by nonlinear least squares optimization; the expression of the global coordinate constraint factor is: in, This is the global coordinate constraint factor; Number of control points; For the world coordinate system The coordinates of the control points; This is the rotation correction matrix; The first for pose prediction Coordinates of control points; This is the translation correction vector; It is an L2 norm; For the first The optimized rotation matrix for each control point; For the first Rotation matrix before optimization of each control point; For the first Optimized translation correction vector for each control point; For the first Translation correction vector of each control point before optimization; The prior residual vector; For the first The attitude parameters in quaternion form after optimization of each control point; It is a quaternion cross product; For the first The inverse of the attitude parameter expression in quaternion form before optimization of each control point; To convert the results of quaternion-related operations into a three-dimensional vector form; The relative attitude is calculated based on global coordinate constraint factors to optimize the positioning and mapping trajectory of the underground mine. Specifically, this involves incorporating global coordinate constraint factors into the factor graph of the LiDAR-Vision-Inertial Odometry system, and then performing relative attitude calculations based on this factor graph to optimize the positioning and mapping trajectory of the underground mine. The expression for the factor graph incorporating global coordinate constraint factors is as follows: in, Factor plot for introducing global coordinate constraint factors; This is the visual reprojection error factor; for The weights; for The weights; For IMU pre-integration factor; for The weights; The point-to-surface registration factor for the laser point cloud; for The weights; This is the global coordinate constraint factor; for The weights; This is the lap-loop constraint factor; This is a transpose.

2. The method for optimizing the location and mapping trajectory of underground mines based on underground control point constraints according to claim 1, characterized in that, The initialization of the lidar-vision-inertial odometry system and GNSS initialization are completed based on GNSS data, laser data, IMU data, and image data, specifically as follows: GNSS data, laser data, IMU data, and image data are preprocessed, and the preprocessed GNSS data, laser data, IMU data, and image data are time-aligned to obtain time-aligned multi-sensor data; Based on time-aligned multi-sensor data, the extrinsic parameters of each sensor are calibrated to obtain the laser-IMU extrinsic parameter matrix, camera-IMU extrinsic parameter matrix, and GNSS-IMU extrinsic parameter matrix. Based on time-aligned multi-sensor data, GNSS data is defiltered to obtain filtered GNSS data. Laser feature points are obtained by extracting point cloud features from laser data. Pre-integrate the IMU data to obtain the IMU pre-integrated value and the zero bias initial value; Corner detection and optical flow tracing are performed on the image data to obtain visual feature points; Initialize the lidar-vision-inertial odometry system: complete the lidar-IMU initialization based on the lidar feature points, IMU pre-integration, and zero bias initial value to obtain the initial state of the lidar-IMU; complete the vision-IMU initialization based on the vision feature points and the initial state of the lidar-IMU to obtain the IMU pose and the world coordinate system gravity vector; Determine whether the IMU pose and the world coordinate system gravity vector have converged. If so, perform GNSS initialization; otherwise, return to perform lidar-vision-inertial odometry system initialization.

3. The method for optimizing the positioning and mapping trajectory of underground mines based on underground control point constraints according to claim 2, characterized in that, The GNSS initialization is as follows: GNSS anchor point calculation and heading angle alignment are performed based on the filtered GNSS data to obtain the initial pose of global anchoring, thus completing the GNSS initialization.

4. The method for optimizing the location and mapping trajectory of underground mines based on underground control point constraints according to claim 1, characterized in that, The expression for the visual reprojection error factor is: in, The coordinates of the object points in the optimized map; The coordinates of the object point on the map; for The z-component; To take only the x and y components of the vector as the residual; The image plane coordinates of the visual feature points; This is the rotation matrix from the camera coordinate system to the carrier coordinate system; For the world coordinate system to the 1st Rotation matrix in the carrier coordinate system under the frame; For the first The rotation matrix from the carrier coordinate system to the world coordinate system in the frame; This is the rotation matrix from the carrier coordinate system to the camera coordinate system; Inverse depth; For the first Object coordinates in the optimized map at frame rate; This is the translation vector from the camera coordinate system to the vehicle coordinate system; For the first Translation vector from the carrier coordinate system to the world coordinate system in the frame; For the first Translation vector from the carrier coordinate system to the world coordinate system in the frame; The x-coordinate of the image plane of the visual feature point; The vertical coordinate of the image plane of the visual feature point is denoted by .

5. The method for optimizing the positioning and mapping trajectory of underground mines based on underground control point constraints according to claim 1, characterized in that, The expression for the IMU pre-integration factor is: in, To the world coordinate system The rotation matrix of the body coordinate system at any given time; for The position vector of the IMU in the world coordinate system at any given time; for The position vector of the IMU in the world coordinate system at any given time; This is the gravitational acceleration vector in the world coordinate system; for time; The current reference frame; for The velocity vector of the IMU in the world coordinate system at any given moment; The result of pre-integration calculation arrive At any given moment, the position integral based on the body coordinate system; for The velocity vector of the IMU in the world coordinate system at any given moment; The result of pre-integration calculation arrive At any given moment, the velocity integral based on the body coordinate system; for The inverse of the rotation quaternion from the body coordinate system to the world coordinate system at any given moment; It is a quaternion cross product; for The rotation quaternion from the body coordinate system to the world coordinate system at any given time; The result of pre-integration calculation arrive At any given moment, the attitude integral based on the body coordinate system; To convert the results of quaternion-related operations into a three-dimensional vector form; for Accelerometer deviation at any given time; for Accelerometer deviation at any given time; for Gyroscope deviation at any given moment; for Gyroscope deviation at any given moment; for Time body coordinate system to The rotation matrix of the body coordinate system at any given time; For IMU in Acceleration measured at any given time; For IMU accelerometer in Timing deviation; For time; This is an operation to convert an angular velocity vector into an antisymmetric matrix; For IMU in Angular velocity measured at any given time; For IMU gyroscope in Timing deviation; The result of pre-integration calculation Time's up Attitude integral based on the body coordinate system at any given moment; It is the antisymmetric matrix form of angular velocity; This is the angular velocity matrix; Angular velocity; The x-component of the angular velocity; The y-component of the angular velocity; Let z be the z-component of the angular velocity.

6. The method for optimizing the positioning and mapping trajectory of underground mines based on underground control point constraints according to claim 1, characterized in that, The expression for the laser point cloud point-to-surface registration factor is: in, In the world coordinate system, this is the unit normal vector of the plane corresponding to the point cloud; for The rotation matrix from the body coordinate system to the world coordinate system at any given time; The rotation matrix from the laser radar to the carrier coordinate system; In the lidar coordinate system, the first The coordinates of each laser point; This is the translation vector from the laser radar to the carrier coordinate system; for The position vector of the IMU in the world coordinate system at any given time; for The position vector of the IMU in the world coordinate system at any given time.

7. The method for optimizing the location and mapping trajectory of underground mines based on underground control point constraints according to claim 1, characterized in that, The expression for the loop constraint factor is: in, For the first The carrier coordinate system under the frame to the first Rotation of the carrier coordinate system within the frame; In the world coordinate system, the first The inverse of the position vector in the frame's carrier coordinate system; In the world coordinate system, the first The inverse of the position vector in the frame's carrier coordinate system; For the world coordinate system to the 1st The inverse of the rotation matrix of the frame's carrier coordinate system; In the world coordinate system, the first The position vector of the origin of the frame's carrier coordinate system; In the world coordinate system, the first The position vector of the origin of the frame's carrier coordinate system; The first one obtained by loop closure detection The carrier coordinate system of the frame to the first Translation constraints of the frame's carrier coordinate system; To convert the results of quaternion-related operations into a three-dimensional vector form; It is a quaternion cross product.

8. The method for optimizing the location and mapping trajectory of underground mines based on underground control point constraints according to claim 1, characterized in that, The optimization objective of the mapping trajectory is: in, These are trajectory parameters; In order to make smallest The value of ; Factor graph for introducing global coordinate constraint factors.

Images