Construction robot positioning and mapping method in long tunnel environment

Abstract

The invention discloses a construction robot positioning and mapping method in a long tunnel environment, and relates to the technical field of computer vision, and the method comprises the steps that a robot obtains data information in the long tunnel environment through a carried sensor, and the data information comprises IMU information, an RGB image and a depth image; the method comprises the following steps: calculating IMU pre-integration according to IMU information; carrying out adaptive ORB point feature extraction and EDlines line feature extraction and optimization on an RGB image; constructing a deep learning network to carry out feature matching on extracted point-line features to obtain a matching result; constructing a point-line feature re-projection error model based on the matching result; preliminarily estimating an inter-frame pose based on the depth image and a point-line feature re-projection error model; the method comprises the following steps: selecting a key frame and detecting an identification code based on a depth image and a preliminarily estimated inter-frame pose, constructing a factor graph model based on the key frame, an identification code detection result and IMU pre-integration, updating global positioning based on a factor graph optimization result, and constructing a global map.

Description

Technical Field

[0001] The present invention belongs to the field of visual synchronous positioning and mapping, and computer vision technology, and in particular relates to a construction robot positioning and mapping method in a long tunnel environment. Background Art

[0002] With the rapid advancement of urbanization and the vigorous development of transportation infrastructure construction, the importance of tunnels in modern cities has become increasingly prominent, and long tunnels are the key link in urban transportation networks. Long tunnels are not only an important carrier of urban rail transportation, ensuring the efficient flow of personnel and materials, but also a key channel for the transportation of construction materials, playing a vital role in urban construction and development.

[0003] The safety and reliability of long tunnels are directly related to the stable operation of urban transportation systems and the smooth progress of construction projects. In the construction and maintenance of long tunnels, the application of construction robots is particularly important. Construction robots focus on the construction of long tunnels and can efficiently and accurately complete construction tasks such as drilling and material handling in complex tunnel environments, greatly improving construction efficiency and quality, and ensuring that long tunnels can better assume key functions such as rail transportation and construction material transportation.

[0004] However, traditional manual construction is time-consuming and difficult, and the efficiency of identifying abnormal working conditions in the tunnel and the working conditions of the equipment during construction is low, and the accuracy is difficult to guarantee. At the same time, the track-type construction robot system is not flexible enough in deployment, the equipment is inefficient, and the maintenance cost is high. Therefore, it is of great practical significance to develop a method that enables construction robots to perform real-time localization and mapping (SLAM) in long tunnel environments. Summary of the invention

[0005] In order to solve the above technical problems, the present invention proposes a construction robot positioning and mapping method in a long tunnel environment, which improves the automation level and construction accuracy of the robot in tunnel construction.

[0006] To achieve the above objectives, the present invention provides a construction robot positioning and mapping method in a long tunnel environment, comprising:

[0007] The robot obtains data information in the long tunnel environment through the sensors it carries, and the data information includes IMU information, RGB images, and depth images;

[0008] Calculate the IMU pre-integration according to the IMU information, perform adaptive ORB point feature extraction and EDlines line feature extraction and optimization on the RGB image, build a deep learning network to perform feature matching on the extracted point and line features to obtain matching results, build a point and line feature reprojection error model based on the matching results, and preliminarily estimate the inter-frame pose based on the depth image and the point and line feature reprojection error model;

[0009] Based on the depth image and the preliminary estimated inter-frame pose, key frames are selected, identification codes are detected, a factor graph model is constructed based on the key frames, identification code detection results and the IMU pre-integration, the factor graph is optimized to obtain a factor graph optimization result, global positioning is updated based on the factor graph optimization result, and a global map is constructed.

[0010] Technical effect of the invention: The invention discloses a method for positioning and mapping a construction robot in a long tunnel environment. By fusing visual information with inertial information, high-precision positioning and mapping can be achieved in a long tunnel, and stable and precise operation can be maintained even in the case of changing lighting, complex environment or lack of external positioning signals. The invention overcomes the challenges of traditional SLAM algorithms and other positioning methods in tunnel environments due to problems such as dust influence, uneven lighting intensity, lack of features and inertial drift, and ensures efficient, real-time and precise positioning and mapping of construction robots in complex tunnel environments. The invention greatly improves the automation level and construction accuracy of robots in tunnel construction, and meets the special needs of tunnel engineering. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0012] Figure 1 A schematic diagram of a flow chart of a method for positioning and mapping a construction robot in a long tunnel environment according to an embodiment of the present invention;

[0013] Figure 2 A schematic diagram of a flow chart of a method for extracting line features optimized according to an embodiment of the present invention;

[0014] Figure 3 This is an overall architecture diagram of a point-line feature synchronous matching network PLFG according to an embodiment of the present invention;

[0015] Figure 4 This is a schematic diagram of the preprocessing network structure in the point-line feature synchronous matching network according to an embodiment of the present invention;

[0016] Figure 5 A schematic diagram of a point feature matching network structure in a point-line feature synchronous matching network according to an embodiment of the present invention;

[0017] Figure 6 This is a schematic diagram of the line feature matching network structure in the point-line feature synchronous matching network according to an embodiment of the present invention;

[0018] Figure 7 This is a schematic diagram of a feature fusion network structure in a point-line feature synchronous matching network according to an embodiment of the present invention;

[0019] Figure 8 This is a schematic diagram of the expression of the multi-layer perceptron MLP model according to an embodiment of the present invention;

[0020] Fig. 9 A schematic diagram of coordinate transformation between a camera coordinate system and a real-world coordinate system according to an embodiment of the present invention;

[0021] Fig.10 This is a schematic diagram of the point and line feature reprojection error according to an embodiment of the present invention;

[0022] Fig.11 This is a schematic diagram of a sliding window tightly coupled optimization factor graph model according to an embodiment of the present invention;

[0023] Fig.12 This is a scene diagram of an experiment for extracting point and line features of a long tunnel according to an embodiment of the present invention, wherein (a) is a previous key frame image captured by a camera, and (b) is a subsequent key frame image captured by the camera;

[0024] Fig.13 This is a point feature extraction effect diagram of an embodiment of the present invention;

[0025] Fig.14 This is a line feature extraction effect diagram of an embodiment of the present invention;

[0026] Fig.15 This is a diagram showing the effect of synchronous matching of point and line features according to an embodiment of the present invention;

[0027] Fig.16 An experimental scene diagram for constructing a three-dimensional point cloud map of a long tunnel according to an embodiment of the present invention;

[0028] Fig.17 A three-dimensional point cloud map of a long tunnel according to an embodiment of the present invention;

[0029] Fig.18 It is a schematic diagram of the entire long tunnel mapping process of the construction robot according to an embodiment of the present invention. DETAILED DESCRIPTION

[0030] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0031] It should be noted that the steps shown in the flowcharts of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and that, although a logical order is shown in the flowcharts, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0032] like Figure 1 As shown, this embodiment provides a construction robot positioning and mapping method in a long tunnel environment, including:

[0033] The robot obtains data information in the long tunnel environment through the sensors it carries, and the data information includes IMU information, RGB images, and depth images;

[0034] Calculate IMU pre-integration based on IMU information, perform adaptive ORB point feature extraction and EDlines line feature extraction and optimization on RGB images, build a deep learning network to match the extracted point and line features, obtain matching results, build a point and line feature reprojection error model based on the matching results, and preliminarily estimate the inter-frame pose based on the depth image and the point and line feature reprojection error model;

[0035] Based on the depth image and the preliminary estimation of the inter-frame pose, key frames are selected and identification codes are detected. A factor graph model is constructed based on the key frames, identification code detection results and IMU pre-integration. The factor graph is optimized to obtain the factor graph optimization result. Based on the factor graph optimization result, the global positioning is updated and a global map is constructed.

[0036] Furthermore, the adaptive ORB point feature extraction of RGB image information includes:

[0037] Point feature extraction is a key link in visual SLAM. The ORB detector can quickly extract pixels with obvious brightness differences in the image as key points. However, long tunnels have some challenges such as uneven lighting, repeated textures, and dust particles, which often lead to a small number of extracted point features and uneven distribution, which can easily lead to subsequent image matching failures and affect the accurate estimation of camera pose and position. For example, on the lining surface of a long tunnel, some areas have rich textures, and the traditional ORB-SLAM3 algorithm may extract a large number of concentrated feature points here; while in the smoother areas inside the tunnel with less obvious textures, such as some parts of the side walls, the number of feature points is seriously insufficient. Inspired by the idea of ​​"Gini coefficient" in economics, an adaptive threshold calculation method is proposed. This method can dynamically adjust the threshold according to the needs of different scenarios and extract a sufficient number of evenly distributed feature points to improve tracking performance.

[0038] In order to measure the uneven distribution of gray values ​​in a local image, the Gini coefficient G is introduced, and its calculation formula is: ,

[0039] in, and It represents the grayscale value of the i-th and j-th pixels in the local image, and n is the number of all pixels in the image. In a long tunnel environment, the grayscale distribution in different areas varies greatly. The Gini coefficient can accurately quantify this difference and provide a basis for subsequent threshold adjustment. For example, in the area where the light changes significantly at the entrance of the tunnel, the grayscale value fluctuates greatly, and the Gini coefficient can reflect this unevenness.

[0040] Calculate the mean gray value of the local image Gray value standard deviation , the formula is as follows: , Reflects the overall gray level of the local image, n is the number of all pixels in the image, and Respectively represent the horizontal and vertical positions of the pixel in the image. In a long tunnel, due to factors such as lighting conditions and tunnel wall material, the grayscale mean values ​​of different areas are different. , It reflects the degree of dispersion of grayscale values ​​relative to the mean and is used to evaluate the richness of image texture. In long tunnels, the standard deviation will be larger where the texture of the tunnel wall changes. The final adaptive threshold calculation formula is as follows:

[0041] ,in, is the balance coefficient, is the maximum imbalance, It is a uniformity factor, which reflects the uniformity of the distribution of feature points in the local image. The larger the uniformity factor, the more uniform the distribution of feature points. In a long tunnel environment, for areas with uneven distribution of feature points, this factor can be used to adjust the threshold to make the feature points more evenly distributed. In the bends of the tunnel, the illumination and texture changes are complex, and this factor can optimize the distribution of feature points. is the contrast factor, which reflects the grayscale contrast in the local image area. The larger the contrast factor, the richer the texture of the image and the more feature points. The marks or cracks on the tunnel wall are areas with rich textures, and the contrast factor can ensure that enough feature points are extracted. To determine the balance coefficient Reasonable values ​​under different external environmental conditions, in the long tunnel multiple lighting scene experimental environment, set different lighting intensity, uniformity, angle and other conditions, collect a large number of image samples, calculate the Gini coefficient, grayscale mean and standard deviation of each image, and use different The following experience was obtained: in areas with sufficient light, The value of is set to 0.2, which effectively prevents the number of feature points from being too large due to the high threshold, thereby avoiding the introduction of too many noise points; in dim areas, the value is set to 0.8, which is more effective in extracting a sufficient number of relatively evenly distributed feature points in low-contrast and fuzzy texture images, thereby compensating for the adverse effects of insufficient illumination on feature point extraction; in areas with strong illumination and obvious illumination gradients, the grayscale value distribution shows great imbalance, The value selection needs to be considered comprehensively. It is necessary to effectively suppress the excessive reduction of the threshold caused by the excessive standard deviation in the area of ​​sudden changes in illumination, so as to avoid extracting too many invalid feature points. On the other hand, it is also necessary to ensure that an appropriate number of evenly distributed feature points can be accurately extracted in areas with relatively normal illumination and texture. Based on this situation, The value range of can be set between 0.4 and 0.6. By analyzing the grayscale distribution of local images, this method can more flexibly extract feature points in long tunnel environments and overcome the problem of uneven distribution of feature points in traditional ORB algorithms in low-texture and strong-light areas.

[0042] Furthermore, EDlines line feature extraction and optimization of RGB image information include:

[0043] Line features are added on the basis of the original point features as visual features. Line features are of great significance for improving the positioning accuracy of tunnel scenes. However, traditional line feature extraction methods have defects in long tunnel scenes. For example, in the weak texture area of ​​the long tunnel, although the EDLines line feature extraction method can extract line features, it will produce a large number of invalid line features. These invalid line features not only increase the time cost of subsequent feature matching, but also because of their low quality, they are not conducive to stable tracking, which in turn has a negative impact on the positioning effect in the long tunnel environment. In areas of long tunnels with uneven lighting or smoother side walls, the large number of short line segments and line segments with messy directions extracted by the EDLines algorithm will interfere with the subsequent positioning and mapping process.

[0044] First, the line features extracted by the EDLines algorithm are processed to remove those whose length is less than the preset threshold. In a long tunnel environment, a line segment that is too short often cannot provide accurate position and direction information, and may be the result of noise or mis-extraction. For example, tiny line segments generated by light reflection on the tunnel wall. By setting a reasonable length threshold , these interfering line segments can be effectively removed and the amount of subsequent calculations can be reduced.

[0045] For the remaining line segments, consider their merging conditions. Assume that the two line feature segments to be merged are and Its endpoints are represented as , and , The direction vector of each line segment is expressed as:

[0046] ,

[0047] The merging condition of line segments is:

[0048] ,

[0049] in, is the angle between the two line segments, is the minimum distance between any endpoints of two line segments, and is the direction vector of line segment a and line segment b, is the preset angle threshold, is the preset endpoint distance threshold. In a long tunnel environment, due to the structural characteristics and lighting factors, the consistency of the line segment direction and endpoint distance is crucial for accurately extracting valid line features. At the bend of the tunnel, the angle change of adjacent line segments should be relatively small, and the endpoint distance should also be within a reasonable range. The angle between any two line segments is less than the threshold And line segment Any endpoint to the line segment The minimum distance between any endpoints is less than the threshold , you can merge line segments. The length of the merged line segment is the length of the longer line segment, and the merged direction is the direction of the angle bisector of the two line segments. In this way, line segments with similar local directions and dense density can be merged into long line segments, improving the quality and representativeness of line features.

[0050] If the merged line segments still meet the merge conditions, continue to merge until all line segments no longer meet the conditions. The specific operation is as follows: Use the DBSCA algorithm to cluster the line segments, set the parameters including the long line segment set , Aggregation Radius and the minimum number of clusters MinPts. Clustering can group line segments with similar directions and positions into a group for subsequent merging. Get the number of clusters in the clustering result, and then merge the line segments in each cluster. For the line segments in each cluster, first calculate the angle between the two line segments and the distance between all the endpoints of the two line segments to determine whether the merging conditions are met. Merge the two line segments that meet the conditions, and sort out the length and direction of the merged line segments. Repeat the above steps until there are no more line segments that meet the merging conditions, such as Figure 2As shown. Through the above optimization method, the number of invalid line features can be effectively reduced, the quality of valid line features can be improved, and the stability and positioning accuracy of the SLAM system can be improved. This has significant advantages for robot positioning and mapping in long tunnel environments, especially in applications in low-texture areas, and can provide more efficient feature extraction and matching effects.

[0051] Furthermore, a deep learning network is constructed to perform feature matching on the extracted point and line features, and the matching results obtained include:

[0052] In the feature matching process, due to the complexity of the long tunnel environment, the accuracy of feature matching is seriously problematic. The PLFG model can effectively solve this problem and improve the robustness and efficiency of matching. The present invention proposes a parallel network combining Transformer and GNN neural networks for point-line feature synchronous matching. The network is divided into three parts: initialization network, point-line feature matching, and fusion. Figures 3 to 7 shown.

[0053] The specific structure of the initialization network includes: input layer, convolution layer, pooling layer, encoding layer, and finally outputs rich descriptors and feature information. The feature information of the input image is first reduced in dimension by a 3x3 convolution layer. Then it enters the pooling layer, which can effectively aggregate local area information and reduce redundant feature dimensions, making the features more compact and easy to handle while retaining key information. Finally, the processed features are learned by learning a position encoding PE with a multi-layer perceptron (MLP) p and direction encoding PE e , generating a spatial descriptor for each key point , generate edge descriptors for each line endpoint , and get the location information of the point at the same time MLP consists of an input layer, multiple hidden layers, and an output layer. Its structure is as follows Figure 8 As shown, the mathematical expression is:

[0054] ,

[0055] in is the hidden layer output, and the parameters include the connection weights and biases between each layer, that is, is the weight from the input layer to the hidden layer, is the weight from the hidden layer to the output layer, , is the bias, and G is the activation function. MLP takes information about a point or line segment as input, such as the coordinates of the point , Confidence As well as the endpoint coordinates and endpoint offset of the line segment , , Line Score The hidden layer processes and transforms these input information to generate accurate spatial descriptors d and edge descriptors. , thereby encoding the position and direction:

[0056] .

[0057] The specific structure of the point feature matching network includes: input layer, Transformer layer, matching assignment layer, confidence layer, and soft assignment processing output layer.

[0058] Each Transformer layer contains an attention mechanism and a cross-attention mechanism. The self-attention mechanism allows the model to focus on the relationship between different key points within each image, thereby capturing feature dependencies within the image. In the cross-attention mechanism, a connection is established between two images, allowing the network to learn the correspondence between key points in different images.

[0059] The match assignment layer accepts the descriptors of the two images processed by the Transformer layer as input, and constructs a match assignment matrix by calculating the similarity score and matchability score between the descriptors. The purpose of this layer is to determine the matching possibility between the key points in the two images, providing a basis for subsequent matching screening.

[0060] The confidence layer calculates confidence based on the descriptor at each Transformer layer to evaluate the confidence or reliability of key points. This confidence information plays a role in the model's early stopping and point pruning strategies, helping the model to dynamically adjust computing resources during training or inference to improve efficiency.

[0061] The location information of key points in the image and spatial descriptors As input to the point feature matching network, for each local feature in the image, it is compared with a state are associated to represent the current state of the feature. Initially represented as a space descriptor , and then the state is updated in each layer. Each layer consists of a continuous combination of a self-attention unit and a cross-attention unit, and a multi-layer perceptron updates the state based on the information aggregated by the image:

[0062] ,

[0063] in, represents the current feature of node i, represents the characteristics of node i after update, Indicates connection, is a multi-head attention mechanism applied to point features:

[0064] ,

[0065] in is a projection matrix, is an image and images point and Point The score is calculated differently in self-attention and cross-attention units.

[0066] In the self-attention unit, the image Pay attention to every point All points in the current state are transformed by different linear transformations Decompose into key vector and query vector and , defining point and Point The attention score between is:

[0067] ,

[0068] in is the relative position rotation encoding matrix, which is achieved by projecting the two-dimensional coordinates onto the learned basis vectors and rotating in multiple two-dimensional subspaces, specifically:

[0069] ,

[0070] Each submatrix represents the rotation on the two-dimensional subspace, are the learned basis vectors.

[0071] By converting the relative positions of feature points into rotation angles, this encoding method can preserve the relative geometric relationship of feature points in perspective projection. Specifically, the rotation encoding matrix performs angular rotations through multiple two-dimensional subspaces to ensure that the position encoding remains consistent at different spatial scales. This encoding method preserves the relative geometric relationship of feature points in perspective projection, improves matching accuracy, and avoids the loss of position information in multi-layer networks.

[0072] In the cross attention unit, Each point in the image follows the other images For all points in , a key vector is calculated for each element, but no query is performed. The attention score is expressed as:

[0073] .

[0074] in, Indicates that for the image arrive and from the image arrive The information is transferred, and the similarity only needs to be calculated once. In terms of correspondence prediction, the distribution is predicted given the updated state of any layer. After the features of each layer are updated, the current feature state is used to calculate the correspondence and decide whether to stop the calculation early. By calculating the similarity score matrix between the points of the two images and matching score :

[0075] ,

[0076] ,

[0077] in, is the similarity score matrix, is the state of the i-th local feature in image A after network update, is the state of the jth local feature in image B after network update, is the index set of local features in image A, is the index set of local features in image B, is the state of the i-th local feature in the image after network update;

[0078] Combined to get the point matching matrix:

[0079] ,

[0080] in, is the point matching matrix, is the matching score of image A, is the matching score of image B;

[0081] When two points are both predicted to be matchable and their similarity is higher than any other point in the two images, the pair is considered Produce a corresponding relationship.

[0082] Adaptive depth and width mechanism is used to reduce the amount of computation. The network reduces the number of layers (depth) and pre-prunes unmatched points (width) according to the complexity of key points in the input image pair. The network backbone enhances the input descriptor. If the image has rich texture and small appearance changes, the inference can be stopped when the early layer prediction is accurate. Specifically, the confidence of the feature of each layer is calculated. :

[0083] .

[0084] Setting the confidence threshold (Decreases with the number of layers) If > , and exceeds the preset ratio , the prediction of the current layer is considered to be reliable enough and the reasoning is stopped. At the same time, the point features with high confidence but low matching are detected And remove it to reduce the amount of subsequent calculations, is the matching score threshold.

[0085] The specific structure of the line feature matching network includes: input layer, GNN layer, line information layer, and soft assignment processing output layer.

[0086] The GNN layer contains self-attention and back-propagation mechanisms to capture the relationship between the endpoints of the line segments.

[0087] The line information layer focuses on processing line segment related information and updating the relationship between endpoints and line segments.

[0088] The network receives line segments, endpoints and their descriptors in the image, where the line segment endpoints are nodes and the line segments are edges. The network structure consists of 6 identical layers, each containing a self-attention unit and a line information transfer module. Each layer contains a self-attention unit and a line information transfer module. In each layer iteration, the nodes aggregate global information through the self-attention unit, and at the same time, the line information transfer module transfers local information between the nodes connecting the line segments, thereby updating the edge descriptors. The feature update process of the self-attention unit is similar to point feature matching, except that the multi-head attention mechanism using online features is optimized for line features, especially when dealing with long line segments and large changes in perspective:

[0089] ,

[0090] Among them, the key vector , query vector Sum value vector It is a node feature and In the self-attention layer, and From the same image.

[0091] Nodes aggregate global information through self-attention units, allowing The endpoints of a line are connected to their neighboring endpoints using local edge connectivity , and find the same type of connection in another image. At the same time, the line information transmission module transmits local information between the nodes of the connecting line segment to update the edge descriptor. The formula is:

[0092] ,

[0093] in, represents the current feature of node i, represents the characteristics of node i after update, Is the line endpoint The total number of adjacent endpoints, Indicates connection, It is the edge descriptor of the line segment. Normalization can prevent the imbalance caused by the different numbers of neighboring nodes during the feature update process and ensure the stable update of node features.

[0094] The line information passing mechanism enables line endpoints to use local edge connectivity to find the same connection in the other image and update the aggregated endpoint features and edge descriptor information. , and , ; Construct the matching matrix of the line segment as follows:

[0095] ,

[0096] Represents the line segment in image A The starting point feature and the line segment in image B The other three expressions are the same. By comparing the two matching methods, the order of the line segment endpoints can be avoided to affect the matching results. Regardless of the order of the line segment endpoints, the best match can be found.

[0097] Add a virtual row and virtual column to the last row and column of the point and line matching matrix. These rows and columns are filled with a learnable parameter to represent those unmatched points and lines. and Apply softmax to all rows and columns of , and then take the geometric mean to get the final matching matrix:

[0098] ,

[0099] The matching information in two directions (from image A to image B and from image B to image A) is comprehensively considered, and the comprehensive information of the relative matching degree of point and line features in the two images is integrated. The value of the matrix element is between 0 and 1, and has a clear probabilistic meaning, that is, it represents the confidence level of the point and line feature matching. The closer the element value is to 1, the higher the confidence level of the corresponding point and line feature matching; conversely, the closer the element value is to 0, the lower the confidence level.

[0100] The fusion network consists of a fully connected layer network. The matching matrix of point and line features is flattened and passed as input to the fully connected layer. By learning appropriate weights, the network automatically fuses the matching information of point features and line features and outputs the point feature matching score. and line feature matching score and the fusion results.

[0101] During the entire training process, a distributed training strategy is adopted. First, the point feature matching network and the line feature matching network are pre-trained. In order to evaluate the matching performance of the model, a training loss function is designed to measure the difference between the point and line feature matching results predicted by the model and the actual matching. For both the point feature matching network and the line feature matching network, a negative log-likelihood loss function based on matching probability is used to calculate the error between the predicted matching matrix and the ground truth. The specific loss function form is:

[0102] ,

[0103] ,

[0104] Where T is the set of ground truth values ​​and all medium representations have the same annotation. is the indicator function, when When it is a true match =1, otherwise = 0. The loss function is optimized by using the Adam optimization algorithm to calculate the gradient and update the network parameters until the model converges.

[0105] The fusion network is then trained using supervised learning. The loss function of the fusion network is defined by comparing the difference between the fusion matching results and the individual matching point and line feature results. The goal is to optimize the parameters of the fully connected layer by minimizing the loss function. The loss function of the fusion network is as follows:

[0106] ,

[0107] in, As a hyperparameter, it is set to 0.4 in the experiment for weak texture scenes, which shows that the role of offline features in this scene is more significant and contributes more to the final matching results.

[0108] Furthermore, constructing a point and line feature reprojection error model based on the matching result includes:

[0109] Calculating the relative pose between the current frame and the previous frame based on the matching result;

[0110] Based on the relative position and posture, calculating the projection pixel coordinates of the point and line features on the current frame image;

[0111] By calculating the pixel distance between the projection point and the actual observation point, the point feature reprojection error is obtained;

[0112] Orthogonalization parameters are used to describe the projection process, and the line feature reprojection error is obtained by calculating the orthogonal distances from the two endpoints of the line segment on the image to the projection line.

[0113] Based on the point feature reprojection error and the line feature reprojection error, a point and line feature reprojection error model is constructed.

[0114] Specifically, based on the point-line feature matching results, the relative pose between the current frame and the previous frame is first calculated. In the straight section and curve of the tunnel, the motion state of the construction robot changes differently. Calculating the relative pose through accurate point-line feature matching can better adapt to these different scenarios.

[0115] The inverse depth method is used to represent point features in three-dimensional space. This representation method has advantages when dealing with feature points at different distances in long tunnels, and can more effectively describe the spatial position relationship of point features. Subsequently, the point features in 3D space are projected onto the image plane of the current frame. In long tunnels, due to factors such as refraction and reflection of light and changes in camera viewing angle, the projection calculation of point features needs to consider a variety of complex situations. Through accurate camera models and calculated relative poses, such as Fig. 9 As shown, the projection pixel coordinates of the 3D point feature on the current frame image are calculated. , ,Then, by calculating the pixel distance between the projection point and the actual observation point, the point feature reprojection error is obtained, such as Fig.10 As shown:

[0116] ,

[0117] in, represents the camera coordinate system, , is the actual observed pixel coordinate of the point feature on the image. In a long tunnel environment, accurate point feature reprojection error calculation helps to timely detect deviations in pose estimation and make corrections.

[0118] For line features, the Plücker coordinate representation is used to project the line features in the camera coordinate system onto the image. In long tunnels, accurate projection of line features (such as the edge of the tunnel wall, track lines, circuit lines, wall markings, etc.) is also important for pose estimation. Due to the long and narrow space of long tunnels, the projection changes of line features at different positions and angles are complex. The Plücker coordinate representation can better describe the geometric characteristics of line features. At the same time, orthogonalization parameters are used to describe the projection process. By calculating the orthogonal distance from the two endpoints of the line segment on the image to the projection line, the line feature reprojection error is obtained, such as Fig.10 As shown:

[0119] ,

[0120] in, , are the homogeneous coordinates of the two endpoints of the line segment on the image, is the projection line equation of the line feature on the image. In the complex environment of a long tunnel, this line feature reprojection error calculation method can accurately reflect the matching error of the line feature and provide an important basis for pose estimation.

[0121] Further, calculating the IMU pre-integral according to the IMU information includes:

[0122] The IMU at the front end is mainly responsible for receiving measurement data and participating in pose estimation. However, due to the many interference factors in the tunnel environment, such as bumps and vibrations, IMU measurement data is easily affected by noise, especially in long tunnels. In order to improve the accuracy of pose estimation, we integrate IMU data into the sliding window optimization process at the back end, thereby effectively reducing noise interference and improving positioning accuracy.

[0123] According to the definition of the pre-integration term, at two moments and , the pre-integral term is calculated from the system state. Assume and Separately for the moment and The rotation matrix of and is the position vector at the corresponding moment, and is the velocity vector, is the time interval between two moments, is the acceleration due to gravity. Then the following three pre-integral terms can be calculated:

[0124] ,

[0125] Among them, the rotation pre-integration term Describes the relationship between the system's rotation changes from time to time. In a long tunnel environment, when the robot turns or adjusts its posture, an accurate rotation pre-integration term helps to more accurately track the robot's direction changes, thereby improving the accuracy of pose estimation. Position pre-integration term The position, speed, gravity, and time interval are comprehensively considered. In long tunnels, due to the undulating terrain and the movement of the robot, the position pre-integration term can better reflect the displacement of the robot in space. Especially in the process of long-distance driving, the accurate estimation of position change is crucial to improve the overall positioning accuracy. It reflects the change relationship of speed between two moments. In a long tunnel environment, the speed change of the robot will be affected by many factors, such as the stability of the motor drive, track friction, etc. The speed pre-integration term can help accurately capture these changes, thereby providing an important basis for pose estimation. These pre-integration terms are independent of the IMU measurement data and are usually calculated based on the measurement results of other sensors such as vision, so they are called ideal pre-integration terms.

[0126] The actual IMU measurement data will be affected by noise, and the resulting pre-integration term is called the measurement pre-integration term. , , To measure the pre-integration term, , , is the approximate value of the ideal pre-integration term considering the influence of noise, and are the deviations of the gyroscope and accelerometer, respectively, , , , , is the corresponding partial derivative, then the measurement pre-integration term is expressed as:

[0127] ,

[0128] in, Inside is an exponential mapping function, which is used to convert Lie algebra into Lie group to consider the influence of gyroscope bias on the rotation pre-integration term. In a long tunnel environment, gyroscope bias may be caused by factors such as magnetic field interference and long-term operation. Accurately describing its influence on the rotation pre-integration term is crucial to improving the accuracy of pose estimation. It reflects the combined effect of accelerometer and gyroscope bias on the velocity pre-integral term. In long tunnels, during the acceleration and deceleration of the robot, accelerometer bias may lead to velocity estimation errors. By accurately calculating the bias effect in the velocity pre-integral term, the velocity estimation can be effectively corrected, thereby improving the accuracy of pose estimation. In a long tunnel environment, small errors in position may accumulate as the robot moves. By considering the deviation in the measurement pre-integration term, this cumulative error can be effectively suppressed and the positioning accuracy can be improved.

[0129] According to the IMU measurement model, the following three error terms are defined:

[0130] ,

[0131] Among them, the rotation error term In is a logarithmic mapping function that converts the Lie group into a Lie algebra, which is used to measure the difference between the measured pre-integral term and the ideal pre-integral term in terms of rotation. In a long tunnel, the posture changes of the robot at the bend are relatively complex, and the rotation error term can accurately reflect this difference and provide a basis for subsequent optimization. Speed ​​error term Directly calculate the error between the ideal value of the velocity pre-integral term and the measured value, and adjust the pose estimation in time to adapt to the actual motion state changes of the robot. Position error term Measures the error between the ideal value of the position pre-integration term and the measured value. In the long-distance positioning process of long tunnels, accurate evaluation of the position error plays a key role in correcting the pose estimation and improving the positioning accuracy.

[0132] These three error terms together constitute the IMU pre-integration residual: ,In the long tunnel environment, the IMU pre-integration residual comprehensively reflects the deviation between the ,IMU measurement data and the ideal state. By processing the residual in the ,backend optimization process, the IMU data is effectively utilized to improve the ,precision of pose estimation, thereby improving the ,performance of the entire positioning and mapping system.

[0133] Further, preliminarily estimating the inter-frame pose based on the depth image and the point-line feature reprojection error model includes: for each feature point or feature line, calculating the error between the reprojected position of the feature point or feature line in the current estimated pose and the position in the actual image;

[0134] The nonlinear least squares method is used to minimize the reprojection error of all feature points and feature lines to obtain a preliminary estimate of the inter-frame pose. Specifically, depth information helps to recover the positions of points and line segments in three-dimensional space and provide accurate geometric constraints. The core of pose estimation is to minimize the reprojection error. For each feature point or feature line, the error between its reprojected position under the current estimated pose and its position in the actual image is calculated. The camera pose is adjusted by the nonlinear least squares method to minimize the reprojection error of all feature points and feature lines, thereby obtaining an accurate preliminary pose estimate.

[0135] Furthermore, the key frame selection includes: the key frame selection is based on when the camera displacement exceeds a certain threshold, indicating that the camera has sufficient displacement; the viewing angle changes greatly, indicating that the scene seen by the camera has changed significantly; the current frame contains new valid feature points, which can enhance the description ability of the map.

[0136] The selection of key frames can also be refined to consider a variety of factors, as follows:

[0137] 1. Posture changes:

[0138] The system determines whether a new keyframe is needed based on the posture change between adjacent frames. If the posture change between the current frame and the previous frame exceeds a certain threshold, the system will mark the current frame as a keyframe. Specifically, it can be based on the following conditions:

[0139] 1. The displacement change is greater than the set threshold.

[0140] 2. The rotation angle change is greater than the set threshold.

[0141] 2. Visual feature quality:

[0142] Consider the number and distribution of feature points and lines. In a long tunnel environment, evaluate the number, quality and distribution of ORB and EDlines features in the current frame. If there are enough feature points and lines (such as enough corner points, structural lines, edges, etc.) and they are evenly distributed in the current frame, it will be considered a suitable keyframe.

[0143] 3. Changes in camera perspective:

[0144] Determine whether the current frame has sufficient perspective change relative to the existing key frame. If so, use this frame as the key frame.

[0145] 4. Feature matching quality:

[0146] If the current frame can be well matched with the feature points and lines in the existing map, the system may consider the current frame to be a suitable keyframe. On the contrary, if the match is not good, the frame may not be selected as a keyframe.

[0147] 5. Key frame interval control:

[0148] In a single-directional environment such as a long tunnel, although the displacement between each frame may be small, the system will have a certain interval to decide whether to add a new keyframe.

[0149] 6. Local map update:

[0150] In a long tunnel environment, the local map is updated regularly to improve system stability by optimizing the pose of existing keyframes. If the current frame contributes more to the existing map, the frame will be selected as a keyframe.

[0151] Identification code detection includes:

[0152] The RGB image is subjected to two-dimensional identification code recognition, the two-dimensional identification codes placed along the tunnel are identified, and relative pose estimation is calculated.

[0153] Specifically, an identification code is set every n meters in the entire long tunnel. These two-dimensional identification codes are unique and their contents contain absolute location information. When the robot is moving, it uses its RGB-D camera to read the identification code information and obtain the absolute coordinates of the current position. The construction robot uses its own positioning system to estimate the current position. The absolute position provided by the identifier By comparison, the position error , Based on this error, the robot's local position estimate is corrected and the robot's current position estimate is updated.

[0154] Furthermore, a factor graph model is constructed based on the key frame, the identification code detection result and the IMU pre-integration, and the factor graph is optimized to obtain the factor graph optimization result, including:

[0155] To ensure the real-time performance of the system in a long tunnel environment, the backend uses a factor graph optimization method based on a sliding window strategy to tightly couple the visual odometer data and the IMU data. The adaptive weight function is used to process the visual odometer factor, IMU factor, and identification code detection factor as factor nodes, and the robot motion posture is used as a variable node to construct a factor graph framework, such as Fig.11 shown.

[0156] In weakly textured scenes such as long tunnels, sliding window optimization has obvious advantages over traditional global map optimization. By limiting the window size, the system can focus on processing the most relevant and latest frame information for the current robot state. When the robot is driving along the long tunnel, the sliding window can focus on the recently observed area around the robot, effectively controlling the computational complexity. This is because the environmental characteristics of most areas in the long tunnel change relatively slowly, and there is no need to optimize the global information every time, thereby improving the real-time performance of the optimization.

[0157] The state variables contained in the sliding window are expressed as follows:

[0158] ,

[0159] in, are all variables in the sliding window, Represents the state variables corresponding to the 𝑖th key frame in the sliding window, including the position of the carrier coordinate system relative to the world coordinate system ,speed ,attitude and bias estimates for the accelerometer and gyroscope and ; Representative Inverse depth values ​​of map points. In long tunnels, accurate estimation of inverse depth values ​​helps improve the accuracy of map construction. represent Orthogonal parameter description of space line segments; are the size of the sliding window and the number of map points and line segments, respectively; Represents the identification code position state variable.

[0160] In the sliding window factor graph optimization framework, the loss function is used to optimize all state variables within the sliding window. In order to improve the optimization effect, the matching information obtained in the point-line feature matching network (PLFG) is combined to convert the elements in the matching matrix into matching scores of point-line features. , and based on the score Dynamically adjust the weights of point features and line features in the visual odometer. Specifically, this embodiment sets the score confidences of point features and line features as and , and calculate their values ​​in the normalized form of the matching score. When the features of the two frames are completely matched, their scores are set to , the calculation formula is as follows:

[0161] ,

[0162] ,

[0163] In order to further enhance the accuracy of the optimization framework for the robot's posture, the significant influence of the identification code factor on posture correction is also considered. The identification code can provide more reliable absolute posture constraints, and its weight is set as follows according to the situation:

[0164] ,

[0165] At the same time, the weight of the IMU factor is associated with the score confidence of the point and line features through dynamic adjustment. When , the weight of the IMU factor is set as follows:

[0166] ,

[0167] In addition, the point and line feature residuals constitute the visual odometry factor, and its weight coefficient is adjusted according to the different score confidences:

[0168] ,

[0169] ,

[0170] in, Indicates the relative contribution of the point feature in the confidence of the two features, Similarly. This weight distribution strategy ensures that the optimization process can fully utilize the advantages of various types of observation information: under the premise of ensuring the accuracy of the identification code factor, when the matching degree of the visual features is high, the visual odometer factor weight is dominant; in the case of poor visual feature matching, the IMU factor will provide a more reliable constraint, thereby ensuring the robustness and accuracy of the optimization results.

[0171] The present invention realizes the adaptive weighting of visual odometer factor, identification code factor and IMU factor, so that the sliding window factor graph optimization can more efficiently handle the multi-source data fusion problem in weak texture environments such as long tunnels, and significantly improves the system's pose estimation accuracy and real-time performance.

[0172] Integrating these weights into the loss function of the sliding window optimization, the new loss function is as follows:

[0173] ,

[0174] in, It is the key frame set of all point features in the sliding window; It is the key frame set of all line features in the sliding window; and is the error and Jacobian matrix of the identification code detection; is the set of IMU pre-integrated measurements within the sliding window; is the map point reprojection error constraint between map point 𝑗 and the 𝑗th keyframe in the sliding window, is the covariance matrix of the corresponding map point observations; is the line segment reprojection error constraint between line segment 𝑘 and the 𝑖th keyframe in the sliding window, is the covariance matrix of the corresponding line segment observation; is the IMU pre-integration residual constraint between two adjacent key frames, is the covariance matrix corresponding to the IMU pre-integrated measurement; , , are the robust Huber kernel functions of point features, line features, and IMU pre-integration respectively. The frame is marginalized. The state prior of the frame is retained for the pose optimization of the next frame. In a long tunnel environment, this processing method can continuously update the system state, allowing the robot to continuously obtain more accurate pose estimates during movement while maintaining the efficiency of the optimization process.

[0175] Furthermore, the global positioning is updated and the global map is constructed.

[0176] From the perspective of the entire long tunnel, by continuously eliminating the errors on each path, the global position estimation is finally optimized. By integrating the robot pose estimation and depth image data, the robot can perform high-precision 3D point cloud reconstruction at each position point. Through the continuous accumulation of dense point cloud data, the 3D point cloud map of the entire tunnel is gradually constructed. The specific long tunnel map construction process is as follows: Fig.18 shown.

[0177] In order to verify the method of the present invention, a dataset was prepared using an actual long tunnel construction scene to verify the specific scene of the feature matching experiment. Fig.12 As shown in the figure, the specific scenario of verifying the map construction experiment is as follows Fig.16 The adaptive threshold ORB feature point extraction in the present invention can obtain a large number of feature points with uniform distribution in the long tunnel with weak texture. The specific effect is as follows Fig.13 As shown in the figure, the EDlines extraction result is optimized in the present invention, and a long line segment that is conducive to positioning is extracted in a long tunnel with weak texture and similar structure. The specific effect is as follows: Fig.14 In the present invention, point-line feature synchronous matching is performed on the extracted point-line features in a long tunnel with weak texture. The specific effect is as follows Fig.15 The specific indicators are shown in Table 1. The present invention constructs a three-dimensional point cloud map of an actual long tunnel scene, and the specific effects are as follows Fig.17 shown.

[0178] Table 1 Experimental data of point-line feature matching

[0179] The above are only preferred specific implementations of the present application, but the protection scope of the present application is not limited thereto. Any changes or substitutions that can be easily thought of by a person skilled in the art within the technical scope disclosed in the present application should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.

Claims

1. A method for positioning and mapping a construction robot in a long tunnel environment, characterized in that: include: The robot obtains data information in the long tunnel environment through the sensors it carries, and the data information includes IMU information, RGB images, and depth images; Calculate the IMU pre-integration according to the IMU information, perform adaptive ORB point feature extraction and EDlines line feature extraction and optimization on the RGB image, build a deep learning network to perform feature matching on the extracted point and line features to obtain matching results, build a point and line feature reprojection error model based on the matching results, and preliminarily estimate the inter-frame pose based on the depth image and the point and line feature reprojection error model; Based on the depth image and the preliminary estimated inter-frame pose, key frames are selected, identification codes are detected, a factor graph model is constructed based on the key frames, identification code detection results and the IMU pre-integration, the factor graph is optimized to obtain a factor graph optimization result, global positioning is updated based on the factor graph optimization result, and a global map is constructed.

2. The method for positioning and mapping a construction robot in a long tunnel environment as claimed in claim 1, characterized in that: The adaptive ORB point feature extraction of the RGB image comprises: The Gini coefficient is introduced according to the RGB image to calculate the unevenness of the gray value distribution of the local image; By calculating the gray value mean and standard deviation of the local image, the overall gray level of the local image is obtained; Based on the Gini coefficient, the uniformity factor, the contrast factor and the balance coefficient, an adaptive threshold is calculated and obtained, and point features are extracted based on the adaptive threshold.

3. The method for positioning and mapping a construction robot in a long tunnel environment as claimed in claim 1, characterized in that: Extracting and optimizing EDlines features from the RGB image includes: Extract line features from the RGB image using EDlines, and remove line segments smaller than a preset threshold; The merging conditions are set, and the removed line segments are iteratively merged according to the merging conditions until all line segments no longer meet the merging conditions, thus completing the optimization of line features.

4. The method for positioning and mapping a construction robot in a long tunnel environment as claimed in claim 1, characterized in that: Constructing a deep learning network to perform feature matching on the extracted point and line features, and obtaining the matching results includes: wherein the deep learning network includes an initialization network, a point matching network, a line matching network, and a fusion network, and the process of obtaining the matching results includes: The input image enters the initialization network and is processed by a 3×3 convolution layer for dimensionality reduction. After dimensionality reduction, it enters a 2×2 pooling layer to aggregate local information to obtain feature information after reducing redundant features. In the encoding layer, the position and direction of the point features and line features are encoded respectively by a multi-layer perceptron MLP to obtain the spatial descriptor of the key points and the edge descriptor of the line endpoints. The key points and spatial descriptors enter the point matching network, and the context information is updated in the Transformer layer through the self-attention mechanism and the cross-attention mechanism to enrich the spatial descriptor, calculate the similarity and matching score of the descriptors, and obtain the point matching matrix; The line endpoints and edge descriptors enter the line matching network, and in the GNN layer, the information is transmitted between the endpoints and the line segments through the self-attention mechanism and the line information layer, the edge descriptor representation is optimized, and the line matching matrix is ​​obtained; The fusion network flattens the point matching matrix and the line matching matrix through a fully connected layer, automatically fuses the matching information of point features and line features through learning weights, and obtains point feature matching scores, line feature matching scores and fusion results.

5. The method for positioning and mapping a construction robot in a long tunnel environment as claimed in claim 1, characterized in that: Constructing a point and line feature reprojection error model based on the matching result includes: Based on the matching result, calculating the relative pose between the current frame and the previous frame; Based on the relative position and posture, calculating the projection pixel coordinates of the point and line features on the current frame image; By calculating the pixel distance between the projection point and the actual observation point, the point feature reprojection error is obtained; Orthogonalization parameters are used to describe the projection process, and the line feature reprojection error is obtained by calculating the orthogonal distances from the two endpoints of the line segment on the image to the projection line. Based on the point feature reprojection error and the line feature reprojection error, a point and line feature reprojection error model is constructed.

6. The method for positioning and mapping a construction robot in a long tunnel environment as claimed in claim 5, characterized in that: Preliminarily estimating the inter-frame pose based on the depth image and the point-line feature reprojection error model includes: for each feature point or feature line, calculating the error between the reprojected position of the feature point or feature line in the current estimated pose and the position in the actual image; The reprojection errors of all feature points and feature lines are minimized through the nonlinear least squares method to obtain a preliminary estimate of the inter-frame pose.

7. The method for positioning and mapping a construction robot in a long tunnel environment as claimed in claim 1, characterized in that: Identification code detection includes: The RGB image is subjected to two-dimensional identification code recognition, the two-dimensional identification codes placed along the tunnel are identified, and relative pose estimation is calculated.

8. The method for positioning and mapping a construction robot in a long tunnel environment as claimed in claim 1, characterized in that: Constructing a factor graph model based on the key frame, the identification code detection result and the IMU pre-integration includes: formulating a visual odometry factor according to the point feature reprojection error and the line feature reprojection error, calculating a confidence by matching scores of point and line features, and dynamically adjusting the weights of point and line features by using the confidence; Formulate an IMU pre-integration factor according to the IMU information, and formulate an identification code detection factor according to the identification code for relative pose estimation; Dynamically set the identification code detection factor according to whether the identification code is successfully detected; The robot's motion posture is used as a variable node, and the adaptively weighted visual odometry factor, IMU pre-integration factor and identification code detection factor are used as factor nodes to construct a factor graph model.

9. The method for positioning and mapping a construction robot in a long tunnel environment as claimed in claim 1, characterized in that: Building a global map includes: Performing global positioning based on the selected key frames and combining the factor graph optimization result; By integrating robot pose estimation, key frame information and depth image information, the robot performs high-precision 3D point cloud reconstruction at each location to build a global map.

Images