A construction robot positioning and mapping method in long tunnel environments

Abstract

The present invention discloses a construction robot positioning and mapping method in a long tunnel environment, which relates to the field of computer vision technology. The method comprises: the robot obtains data information in the long tunnel environment through an onboard sensor, the data information including IMU information, RGB image and depth image; IMU pre-integration is calculated according to the IMU information, adaptive ORB point feature extraction and EDlines line feature extraction and optimization are performed on the RGB image, a deep learning network is constructed to perform feature matching on the extracted point and line features to obtain matching results, a point and line feature reprojection error model is constructed based on the matching results, and inter-frame pose is preliminarily estimated based on the depth image and the point and line feature reprojection error model; key frames are selected based on the depth image and the preliminarily estimated inter-frame pose, identification code detection is performed, a factor graph model is constructed based on the key frames, identification code detection results and IMU pre-integration, global positioning is updated based on the factor graph optimization result, and a global map is constructed.

Description

Technical Field

[0001] The present invention belongs to the fields 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 booming development of transportation infrastructure, tunnels are becoming increasingly important in modern cities, and long tunnels are a crucial link in urban transportation networks. Long tunnels are not only important vehicles for urban rail transit, ensuring the efficient flow of people and materials, but also serve as key channels 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 linked to the stable operation of urban transportation systems and the smooth progress of construction projects. The application of construction robots is particularly important in the construction and maintenance of long tunnels. Specializing in the construction of long tunnels, construction robots can efficiently and accurately complete tasks such as drilling and material handling in complex tunnel environments, significantly improving construction efficiency and quality, ensuring that long tunnels can better fulfill key functions such as rail transportation and construction material transportation.

[0004] However, traditional manual construction is time-consuming and difficult, with low efficiency and difficulty in accurately identifying abnormal tunnel conditions and the working status of equipment during construction. Furthermore, track-mounted construction robot systems lack flexibility in deployment, resulting in low equipment utilization and high maintenance costs. Therefore, developing a method that enables construction robots to perform real-time localization and mapping (SLAM) in long tunnel environments is of great practical significance. Summary of the Invention

[0005] 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 robot's automation level and construction accuracy in tunnel construction.

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

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

[0008] Calculate IMU pre-integration based on 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 present invention discloses a method for positioning and mapping a construction robot in a long tunnel environment. By fusing visual information and inertial information, high-precision positioning and mapping can be achieved in a long tunnel, and stable and accurate operation can be maintained even in the case of changing lighting, complex environment or lack of external positioning signals. The present invention overcomes the challenges brought by 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, ensuring efficient, real-time and accurate positioning and mapping of construction robots in complex tunnel environments. The present invention greatly improves the automation level and construction accuracy of robots in tunnel construction, meeting the special needs of tunnel engineering. BRIEF DESCRIPTION OF THE DRAWINGS

[0011] The accompanying drawings, which constitute part of this application, are intended to provide a further understanding of this application. The exemplary embodiments and descriptions of this application are intended to explain this application and do not constitute an improper limitation on this application. In the accompanying drawings:

[0012] Figure 1 This is 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 This is a schematic diagram of the process of optimizing the line feature extraction method according to an embodiment of the present invention;

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

[0015] Figure 4 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 Schematic diagram of the point feature matching network structure in the 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 Schematic diagram of the feature fusion network structure in the point-line feature synchronous matching network according to an embodiment of the present invention;

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

[0020] Figure 9 Schematic diagram of coordinate transformation between the camera coordinate system and the real-world coordinate system according to an embodiment of the present invention;

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

[0022] Figure 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] Figure 12 This is an experimental scene diagram of extracting point and line features of a long tunnel according to an embodiment of the present invention, where (a) is the previous key frame image captured by the camera, and (b) is the next key frame image captured by the camera;

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

[0025] Figure 14 This is a diagram showing the effect of line feature extraction according to an embodiment of the present invention;

[0026] Figure 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] Figure 16 An experimental scenario diagram for constructing a 3D point cloud map of a long tunnel according to an embodiment of the present invention;

[0028] Figure 17 This is a three-dimensional point cloud map of a long tunnel according to an embodiment of the present invention;

[0029] Figure 18 This is a schematic diagram of the entire long tunnel mapping process of a 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 of the embodiments in this 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 acquires data information in the long tunnel environment through the sensors it carries, including 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 perform feature matching on 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 preliminary estimated 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, adaptive ORB point feature extraction of RGB image information includes:

[0037] Point feature extraction is a key step in visual SLAM. The ORB detector can quickly extract pixels with significant brightness differences in an image as keypoints. However, long tunnels present challenges such as uneven lighting, repetitive textures, and dust particles. These factors often result in 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 are rich in texture, and the traditional ORB-SLAM3 algorithm may extract a large number of concentrated feature points in these areas. However, in smoother, less textured areas within the tunnel, such as certain parts of the sidewalls, the number of feature points is severely insufficient. Inspired by the concept of the "Gini coefficient" in economics, this paper proposes an adaptive threshold calculation method. This method dynamically adjusts the threshold according to the needs of different scenarios to extract a sufficient number of evenly distributed feature points, thereby improving 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 represents the grayscale values ​​of the i-th and j-th pixels in a local image, where n is the total number of pixels in the local image. In long tunnels, grayscale distributions vary significantly across different regions. The Gini coefficient accurately quantifies these differences, providing a basis for subsequent threshold adjustments. For example, at tunnel entrances, where light levels vary significantly, grayscale values ​​fluctuate significantly, 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 local image, and Indicates 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 ​​in 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, where the texture of the tunnel wall changes, the standard deviation will be larger. The final adaptive threshold calculation formula is as follows:

[0041] ,in, is the balance coefficient, is the maximum imbalance, This is a uniformity factor that reflects the uniformity of feature point distribution within a local image. A larger uniformity factor indicates a more even distribution of feature points. In long tunnels, where feature point distribution is uneven, this factor can be used to adjust the threshold for a more even distribution. This factor can also be used to optimize feature point distribution in tunnel curves, where lighting and texture vary intricately. The contrast factor reflects the grayscale contrast within the local image area. The larger the contrast factor, the richer the texture of the image and the more feature points there are. The markings or cracks on the tunnel wall are areas with rich textures, and the contrast factor can ensure that enough feature points are extracted. In order to obtain reasonable values ​​under different external environmental conditions, we set different lighting intensity, uniformity, angle and other conditions in the long tunnel lighting scene experimental environment, collected a large number of image samples, calculated the Gini coefficient, grayscale mean and standard deviation of each image, and used 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 feature points with relatively uniform distribution in low-contrast and fuzzy texture images, thereby compensating for the adverse effects of insufficient lighting on feature point extraction; in areas with strong lighting and obvious lighting gradients, the grayscale value distribution shows a great imbalance. The selection of the value needs to be comprehensively considered. It is necessary to effectively suppress the excessive reduction of the threshold due to the high standard deviation in the area of ​​sudden changes in illumination, thereby avoiding the extraction of 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, overcoming the problem of uneven distribution of feature points in low-texture and strong lighting areas caused by the traditional ORB algorithm.

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

[0043] Line features are added to the existing point-based visual features. Line features are crucial for improving positioning accuracy in tunnel scenarios. However, traditional line feature extraction methods have limitations in long tunnels. For example, in weakly textured areas of long tunnels, the EDLines line feature extraction method can extract line features, but it produces a large number of invalid line features. These invalid line features not only increase the time cost of subsequent feature matching, but also, due to their low quality, hinder stable tracking, negatively impacting positioning in long tunnel environments. In areas of long tunnels with uneven lighting or smooth sidewalls, the EDLines algorithm extracts a large number of short and erratically oriented line segments, which interfere with subsequent positioning and mapping.

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

[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 influence of structural characteristics and lighting factors, the consistency of line segment direction and endpoint distance is crucial for accurately extracting valid line features. At the bend of the tunnel, the angle change between 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 segment is the length of the longer segment, and the merged direction is the direction of the angle bisector of the two segments. This allows you to merge closely spaced line segments with similar local directions into long segments, improving the quality and representativeness of line features.

[0050] If the merged segments still meet the merge conditions, continue merging until all segments no longer meet the conditions. The specific operation is as follows: Use the DBSCA algorithm to cluster the segments, set the parameters including the long segment set , aggregation radius And the minimum number of clusters MinPts. Clustering can group line segments with similar directions and positions into a group, which is convenient 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. The two line segments that meet the conditions are merged, and the length and direction of the merged line segments are sorted. Repeat the above steps until there are no more line segments that meet the merging conditions, such as Figure 2This optimization method effectively reduces the number of invalid line features and improves the quality of valid line features, thereby enhancing the stability and positioning accuracy of the SLAM system. This has significant advantages for robot positioning and mapping in long tunnel environments, especially in low-texture areas, providing more efficient feature extraction and matching.

[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 a serious problem. The PLFG model can effectively solve this problem and improve the robustness and efficiency of matching. This paper proposes a parallel network combining Transformer and GNN neural networks for point and line feature synchronous matching. The network is divided into three parts: initialization network, point and line feature matching and fusion. Figures 3 to 7 shown.

[0053] The specific structure of the initialized 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 process while retaining key information. Finally, the processed features are learned by learning a positional encoding PE with a multi-layer perceptron (MLP). p and direction code 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 the layers, 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, 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 includes both an attention mechanism and a cross-attention mechanism. The self-attention mechanism allows the model to focus on the relationship between different keypoints within each image, thereby capturing feature dependencies within the image. In the cross-attention mechanism, a connection is established between two images, enabling the network to learn the correspondence between keypoints in different images.

[0059] The match assignment layer takes 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 likelihood of matching between key points in the two images, providing a basis for subsequent match screening.

[0060] The confidence layer calculates confidence based on descriptors at each Transformer layer to assess 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 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 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 calculation method of this score is different 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 Decomposed 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 them 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 preserves the relative geometric relationships of feature points in perspective projection. Specifically, the rotation encoding matrix performs angular rotations through multiple two-dimensional subspaces, ensuring that the position encoding remains consistent across different spatial scales. This encoding method preserves the relative geometric relationships of feature points in perspective projection, improving matching accuracy while avoiding 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 transmission only requires calculating the similarity once. In terms of correspondence prediction, the distribution is predicted given the updated state of any layer. After the feature of each layer is 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 j-th 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 points in the two images, the pair is considered matched. Generate corresponding relationship.

[0082] Adaptive depth and width mechanisms are used to reduce computational effort. The network reduces the number of layers (depth) and pre-prunes unmatched points (width) based on 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, inference can be stopped when the early layer predictions are accurate. Specifically, the point feature confidence is calculated for each layer. :

[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 eliminate 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 mechanism and back-propagation mechanism to capture the relationship between the endpoints of the line segment.

[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 from the image. Line segment endpoints are nodes, and line segments are edges. The network structure consists of six identical layers, each containing a self-attention unit and a line information transfer module. Each layer contains one self-attention unit and one line information transfer module. During each iteration, nodes aggregate global information through the self-attention unit, while the line information transfer module transfers local information between nodes connecting line segments, thereby updating edge descriptors. The feature update process of the self-attention unit is similar to that of point feature matching, except that the multi-head attention mechanism applied to online features is optimized for line features, especially when dealing with long line segments and large changes in viewpoint:

[0089] ,

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

[0091] Nodes aggregate global information through self-attention units, allowing A line endpoint uses local edge connectivity to its set of neighboring endpoints , 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 adjacent nodes during the feature update process and ensure the stable update of node features.

[0094] The line information transfer mechanism enables line endpoints to find the same connection in another image using local edge connectivity, 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 expressed in the same way. By comparing the two matching methods, the order of the line segment endpoints can be prevented from affecting the matching results. Regardless of the order of the line segment endpoints, the optimal 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. Apply softmax to all rows and columns of , and then take the geometric mean to get the final matching matrix:

[0098] ,

[0099] This matrix comprehensively considers matching information from both directions (from image A to image B and from image B to image A), integrating information about the relative matching degree of point and line features in the two images. This ensures that the matrix elements are between 0 and 1 and have a clear probabilistic meaning, representing the confidence level of the point and line feature match. The closer the element value is to 1, the higher the confidence level of the corresponding point and line feature match; 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. 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 real 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 results of the individual matching point and line features. The goal is to optimize the parameters of the fully connected layer by minimizing this 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 results 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 projected pixel coordinates of the point and line features on the current frame image;

[0111] By calculating the pixel distance between the projected 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 between the two endpoints of the line segment on the image and the projected 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. The construction robot's motion changes differently in straight sections and curves within a tunnel. Calculating the relative pose through precise point-line feature matching allows for better adaptation to these diverse scenarios.

[0115] The inverse depth method is used to represent point features in three-dimensional space. This representation method has advantages when processing feature points of 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 perspective, the projection calculation of point features needs to consider a variety of complex situations. Through accurate camera models and calculated relative poses, such as Figure 9 As shown, calculate the projection pixel coordinates of the 3D point feature on the current frame image 、 ,Then, by calculating the pixel distance between the projection point and the actual observation point, the point feature reprojection error is obtained, as Figure 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 promptly detect and correct deviations in pose estimation.

[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, driving 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 use of the Plücker coordinate representation can better describe the geometric characteristics of line features. At the same time, the orthogonalization parameters are used to describe the projection process. By calculating the orthogonal distance from the two end points of the line segment on the image to the projection line, the line feature reprojection error is obtained, such as Figure 10 As shown:

[0119] ,

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

[0121] Furthermore, calculating the IMU pre-integral based on the IMU information includes:

[0122] The front-end IMU is primarily responsible for receiving measurement data and participating in pose estimation. However, due to numerous interfering factors in tunnel environments, such as bumps and vibrations, IMU measurement data is susceptible to noise, especially in long tunnels. To improve pose estimation accuracy, we integrate IMU data into the back-end sliding window optimization process, effectively reducing noise interference and improving positioning accuracy.

[0123] According to the definition of the pre-integration term, at two moments and , calculate the pre-integral term from the system state. Let and Separate moments 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 moment to moment. In a long tunnel environment, when the robot turns or adjusts its posture, accurate rotation pre-integration helps to more accurately track the robot's direction changes, thereby improving the accuracy of pose estimation. Position pre-integration The position, velocity, gravity, and time interval are comprehensively considered. In long tunnels, due to the undulating terrain and the movement of the robot, the position pre-integral term can better reflect the displacement of the robot in space. Especially in the process of long-distance driving, the accurate estimation of position changes is crucial to improving the overall positioning accuracy. This term reflects the change in velocity between two moments. In long tunnels, the robot's velocity is affected by a variety of factors, such as motor drive stability and track friction. Velocity pre-integration can accurately capture these changes, providing an important basis for pose estimation. These pre-integration terms are independent of IMU measurement data and are typically calculated based on measurements from other sensors, such as vision. Therefore, 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 biases of the gyroscope and accelerometer, respectively, 、 、 、 、 is the corresponding partial derivative, then the measurement pre-integration term is expressed as:

[0127] ,

[0128] in, in is an exponential mapping function used to convert Lie algebras into Lie groups to account for the impact of gyroscope bias on the rotation pre-integration term. In long tunnel environments, gyroscope bias can be caused by factors such as magnetic field interference and long-term operation. Accurately describing its impact on the rotation pre-integration term is crucial to improving pose estimation accuracy. This reflects the combined impact of accelerometer and gyroscope bias on the velocity pre-integration term. In long tunnels, accelerometer bias can lead to velocity estimation errors during the robot's acceleration and deceleration. By accurately calculating the bias effect in the velocity pre-integration term, the velocity estimate 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 and is used to measure the difference in rotation between the measured pre-integral term and the ideal pre-integral term. In a long tunnel, the robot's posture changes at the bend are relatively complex. The rotation error term can accurately reflect this difference and provide a basis for subsequent optimization. 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. During long-distance positioning in long tunnels, accurate assessment of position error plays a key role in correcting pose estimation and improving positioning accuracy.

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

[0133] Furthermore, preliminarily estimating the inter-frame pose based on the depth image and the point and 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] Using nonlinear least squares, the reprojection error of all feature points and feature lines is minimized to obtain a preliminary estimate of the inter-frame pose. Specifically, depth information helps recover the positions of points and line segments in three-dimensional space, providing 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 in the current estimated pose and its position in the actual image is calculated. The camera pose is adjusted using nonlinear least squares to minimize the reprojection error of all feature points and feature lines, thereby obtaining an accurate preliminary pose estimate.

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

[0136] The selection of keyframes 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 pose changes between adjacent frames. If the pose 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 quantity, quality, and distribution of ORB and EDlines features in the current frame. If the current frame contains sufficient feature points and lines (such as corners, structural lines, edges, and other features) and is evenly distributed, it is considered a suitable keyframe.

[0143] 3. Camera perspective changes:

[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. Keyframe 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 long tunnel environments, local maps are updated regularly to improve system stability by optimizing the poses of existing keyframes. If the current frame contributes significantly to the existing map, it will be selected as a keyframe.

[0151] Identification code detection includes:

[0152] Perform two-dimensional identification code recognition on the RGB image, identify the two-dimensional identification codes placed along the tunnel, and calculate relative pose estimation.

[0153] Specifically, an identification code is set every n meters in the entire long tunnel. These two-dimensional identification codes are unique and contain absolute location information. When the robot is moving, it uses its RGB-D camera to read the identification code information to obtain the absolute coordinates of the current position. The construction robot will use its own positioning system to estimate the current position. The absolute position provided by the identifier For 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 adopts a factor graph optimization method based on a sliding window strategy to tightly couple the visual odometry data and the IMU data. The adaptive weight function is used to process the visual odometry 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 Figure 11 shown.

[0156] In weakly textured scenes like long tunnels, sliding window optimization offers significant advantages over traditional global map optimization. By limiting the window size, the system can focus on processing the most relevant and recent frames of information for the current robot state. As the robot navigates the tunnel, the sliding window can focus on the recently observed area around the robot, effectively controlling computational complexity. This is because the environmental characteristics of most areas in long tunnels change relatively slowly, eliminating the need to optimize global information every time, thus improving the real-time nature 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, specifically 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 segments; are the size of the sliding window and the number of map points and line segments, respectively; Indicates 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 odometry. Specifically, this embodiment sets the score confidence of point features and line features to be and , calculate their values ​​in the normalized form of matching scores. When the features of 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 residuals of point and line features constitute the visual odometry factor, and its weight coefficient is adjusted according to the different score confidence levels:

[0168] ,

[0169] ,

[0170] in, Indicates the relative contribution of point features in the confidence of 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 that the identification code factor ensures accuracy, when the visual feature matching degree is high, the visual odometry factor has a higher weight; when the visual feature matching is poor, 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 odometry 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, significantly improving 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 set of key frames where all point features exist in the sliding window; It is the set of key frames where all line features exist in the sliding window; and is the error and Jacobian matrix of the identification code detection; is the set of IMU pre-integrated measurement values ​​within the sliding window; is the map point reprojection error constraint between the map point 𝑗 and the 𝑗th keyframe in the sliding window, is the covariance matrix of the corresponding map point observations; is the segment reprojection error constraint between 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 each frame is retained for pose optimization in the next frame. In long tunnel environments, 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 a 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 fusing the robot pose estimation with the 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 process of long tunnel map construction is as follows: Figure 18 shown.

[0177] In order to verify the method of the present invention, a dataset was created using an actual long tunnel construction scene to verify the specific scene of the feature matching experiment. Figure 12 As shown in the figure, the specific scenario of verifying the map construction experiment is as follows Figure 16 The adaptive threshold ORB feature point extraction in the present invention can extract a large number of feature points with uniform distribution in the long tunnel with weak texture. The specific effect is as follows Figure 13 As shown in the figure, the present invention optimizes the EDlines extraction results, and extracts long line segments that are conducive to positioning in long tunnels with weak textures and similar structures. The specific effect is as follows: Figure 14 The present invention synchronously matches the point and line features, and synchronously matches the extracted point and line features in a long tunnel with weak texture. The specific effect is as follows Figure 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 Figure 17 shown.

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

[0179] The above are merely preferred embodiments of the present application, but the scope of protection of the present application is not limited thereto. Any changes or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in this application should be included in the scope of protection of the present application. Therefore, the scope of protection of the present application should be based on the scope of protection 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 acquires data information in the long tunnel environment through the sensors it carries, including IMU information, RGB images, and depth images; Calculate IMU pre-integration based on 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; Performing adaptive ORB point feature extraction on the RGB image includes: 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 mean and standard deviation of the grayscale values ​​of the local image, the overall grayscale level of the local image is obtained; Calculating an adaptive threshold based on the Gini coefficient, the uniformity factor, the contrast factor, and the balance coefficient, and extracting point features based on the adaptive threshold; Introducing the Gini coefficient G, the calculation formula is: ； in, and 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 local image; Calculate the mean gray value of the local image Gray value standard deviation , the formula is as follows: ； ； in, Reflects the overall gray level of the local image, n is the number of all pixels in the local image, and Respectively represent the horizontal and vertical positions of the pixel in the image; Get the adaptive threshold, the calculation formula is: ； in, is the balance coefficient, is the maximum imbalance, is the uniformity factor, is the contrast factor; 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 according to claim 1, wherein: 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; Set the merging conditions, and iteratively merge the removed line segments in combination with the merging conditions until all line segments no longer meet the merging conditions, thus completing the optimization of line features.

3. The method for positioning and mapping a construction robot in a long tunnel environment according to claim 1, wherein: Constructing a deep learning network to perform feature matching on the extracted point and line features to obtain a matching result 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 result includes: The input image enters the initialization network and undergoes dimensionality reduction processing through a 3×3 convolutional layer. After dimensionality reduction processing, 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 through a multi-layer perceptron (MLP) to obtain spatial descriptors of key points and edge descriptors of line endpoints. The spatial descriptors of the key points 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 descriptors, calculate the similarity and matching score of the descriptors, and obtain the point matching matrix; The edge descriptors of the line endpoints enter the line matching network, and in the GNN layer, the self-attention mechanism and the line information layer are used to transfer information between the endpoints and the line segments, optimize the edge descriptor representation, and obtain the line matching matrix. The fusion network flattens the point matching matrix and the line matching matrix through a fully connected layer, and automatically fuses the matching information of point features and line features by learning weights to obtain point feature matching scores, line feature matching scores and fusion results.

4. The method for positioning and mapping a construction robot in a long tunnel environment according to claim 1, wherein: Constructing a point and line feature reprojection error model based on the matching results includes: Based on the matching results, calculating the relative pose between the current frame and the previous frame; Based on the relative position and posture, calculating the projected pixel coordinates of the point and line features on the current frame image; By calculating the pixel distance between the projected 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 between the two endpoints of the line segment on the image and the projected line. Based on the point feature reprojection error and the line feature reprojection error, a point and line feature reprojection error model is constructed.

5. The method for positioning and mapping a construction robot in a long tunnel environment according to claim 4, wherein: Preliminarily estimating the inter-frame pose based on the depth image and the point and 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 error of all feature points and feature lines is minimized by nonlinear least squares method to obtain a preliminary estimate of the inter-frame pose.

6. The method for positioning and mapping a construction robot in a long tunnel environment according to claim 1, wherein: Identification code detection includes: Perform two-dimensional identification code recognition on the RGB image, identify the two-dimensional identification codes placed along the tunnel, and calculate relative pose estimation.

7. The method for positioning and mapping a construction robot in a long tunnel environment according to claim 4, wherein: Constructing a factor graph model based on the keyframe, 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 level through a matching score of the point and line features, and dynamically adjusting the weights of the point and line features using the confidence level; Formulate an IMU pre-integration factor based on the IMU information, and formulate an identification code detection factor based on the relative pose estimation of the identification code; Dynamically set the identification code detection factor based on 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.

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

Images