CN118031963A - Underground positioning method and system based on comprehensive dotted line characteristics - Google Patents

Abstract

The present invention discloses an underground positioning method and system based on comprehensive point and line features. In the visual odometer part based on point and line features, firstly, the original image of the binocular camera is subjected to dedistortion and binocular correction operations, so that the epipolar lines of the two images are on the same horizontal line. Then, the image is subjected to point and line feature extraction and matching, and after completion, the point and line reprojection error function of the system is constructed and the pose estimation is performed; then, the pose is further optimized using a local optimization thread; in order to eliminate the accumulated error, a loop detection operation is performed, and after the loop is detected, a loop correction is performed, and a global pose graph optimization is performed on the pose. The present invention can have a higher positioning accuracy in underground scenes with dim light and changing light, and because the more time-consuming point and line feature processing algorithm is improved, the present invention avoids the problem that the traditional visual positioning method based on point and line features is more time-consuming.

Description

A method and system for underground positioning based on comprehensive point and line features

Technical Field

The invention belongs to the technical field of robot positioning, and in particular relates to an underground positioning method and system based on comprehensive point and line features.

Background technique

With the development of science and technology, equipment in mines is becoming more and more intelligent, and firefighting, rescue, and inspection robots are increasingly widely used. The basic premise for these robots to complete their tasks is to accurately determine their own positions in real time. The positioning solutions used in indoor environments include ultra-wideband (UWB) technology and simultaneous localization and mapping (SLAM) technology. Although UWB technology is accurate enough, it is very expensive and has insufficient coverage.

SLAM technology is currently a widely used positioning solution with the advantages of high precision, low cost and the ability to work in a wide range of scenarios. However, traditional SLAM algorithms mainly rely on point features. In underground scenes with dim light or changing light, there will be problems such as failure to extract sufficiently typical features and feature matching failure, resulting in poor positioning results.

Summary of the invention

The technical problem to be solved by the present invention is to provide an underground positioning method and system based on comprehensive point and line features in view of the deficiencies in the above-mentioned prior art, so as to solve the technical problem of low positioning accuracy of traditional point feature-based SLAM algorithms in underground scenes with dim light and changing light.

The present invention adopts the following technical solutions:

A downhole positioning method based on comprehensive point and line features comprises the following steps:

S1, performing dedistortion and binocular correction operations on the two original images of the binocular camera, so that the epipolar lines of the two processed images are on the same horizontal line;

S2, using an improved point and line feature processing algorithm to extract and match point and line features of the two images obtained in step S1;

S3, deriving a reprojection error function of point and line features based on the features obtained in step S2;

S4, using the Levenberg-Marquardt method to solve the reprojection error function obtained in step S3 to obtain the camera pose to be estimated, and using a local optimization thread to optimize the obtained camera pose to obtain an optimized pose;

S5, judging whether the current frame is a key frame according to the posture obtained in S4, if the current frame is a key frame, performing loop detection according to the similarity between the two frames of images, and correcting the detected loop to obtain the posture that needs to be corrected;

S6. Use the posture that needs to be corrected obtained in step S5 to globally optimize the posture graph, output the posture of the image frame, and obtain the positioning trajectory.

Preferably, step S2 is specifically:

S201, using a GPU to accelerate the feature extraction process, using an improved optical flow matching strategy to perform bidirectional and circular operations respectively, to obtain image features;

S202. Use the straight line segment detection algorithm to perform line segment detection and restore the damaged line segments. Then, construct the point-line invariant based on the point features, and complete the line segment matching based on the existing feature points. If the number of matching line segments based on the point-line invariant is insufficient, calculate the LBD descriptor for the line segments that do not meet the point-line invariant construction conditions and perform line segment matching to obtain the matched features.

More preferably, in step S201, the bidirectional operation is specifically:

When performing point feature tracking on images I ₁ and I ₂ , use the LK optical flow method to track features from image I ₁ to image I ₂ ; check the tracking results, retain the correctly tracked point features, and remove the incorrectly tracked ones; use the LK optical flow method to track the retained point features from image I ₂ to image I ₁ ; check the tracking results again; use the retained point features as the matching point pairs between image I ₁ and image I ₂ .

More preferably, in step S201, the ring operation is specifically:

Previous left eye image With the current frame left eye image/> Using bidirectional optical flow, /> The point feature set is The set of matching points obtained by tracking is recorded as/> For the current frame left eye image/> With the current frame right eye image/> Using bidirectional optical flow, /> The point feature set is/> The set of matching points obtained by tracking is recorded as x _temp1 ; for the right eye image of the current frame/> With the previous frame right eye image/> Using bidirectional optical flow, /> The point feature set is x _temp1 , and the matching point set obtained by tracking is recorded as x _temp2 ; the previous frame right eye image/> The original point feature set is/> Through the previous frame right eye image/> The temporary point feature set is x _temp2 , and the features in x _temp2 are detected, and the features falling into /> The surrounding of the corresponding feature; the right image of the current frame The temporary point feature set is x _temp1 , the previous frame right eye image/> The temporary point feature set is x _temp2 . According to the correspondence between x _temp1 and x _temp2 , the features removed from x _temp2 are removed from x _temp1 , and the remaining features are retained and recorded as />

More preferably, step S202 is specifically:

S2021, eliminating line segments whose length is less than a set threshold, comprehensively evaluating the angle of the main direction of the line segment, the distance between the midpoints of the two line segments, the distance between the endpoints of the line segment, and the ratio of the endpoint distance to the average length of the two line segments, and merging the line segments;

S2022. For the projection of points and lines on the same plane in space, the distance ratio between two point features and line features in the previous and next frames remains unchanged, which is called point-line invariant. The homography matrix is solved by direct linear transformation method using 4 pairs of matching feature points.

S2023. When the angle of the main direction of the line segment is less than or equal to the set threshold, and there are at least two matching point features in the line segment support domain, the corresponding point-line invariant is calculated. For line segments l ₁ and l ₂ , the final similarity Sim(l ₁ ,l ₂ ) is the maximum value of AffSim(l ₁ ,l ₂ ). If Sim(l ₁ ,l ₂ )>0.95, the line segment is matched successfully.

Preferably, in step S3, the reprojection error function F is specifically:

Among them, p _l and I _l represent the point and line feature sets respectively, ρ _p and ρ _l represent the Huber robust kernel functions of point and line features respectively, Σ _j and Σ _k represent the Gaussian distribution covariance matrix of point and line features, and/> They represent the reprojection errors of the j-th point feature and the k-th line feature respectively.

Preferably, in step S4, the obtained camera pose is optimized using a local optimization thread as follows:

A local map is constructed using the currently processed keyframe, the keyframes connected to the current frame in the co-visual map, and the landmark spatial points and lines observed by the keyframes. The camera pose and spatial point and line positions are used as state variables to be optimized. The reprojection error function between the spatial points and lines in the local map and the keyframes where the corresponding points and lines can be observed is used as a constraint condition. The sparsity of the graph model is used to accelerate the solution to obtain the optimized camera pose.

Preferably, in step S5, the key frame selection principle is as follows:

The number of ordinary frames between the current frame and the previous key frame is greater than or equal to 20; greater than or equal to 55 point features and 26 line features are tracked; the ratio of the point and line features observed jointly by the current frame and the previous key frame to the point and line features observed in the current frame is less than 0.7.

Preferably, in step S5, the detected loop is corrected by:

The new pose of the current frame is determined by calculating the relative pose of the current frame and the loop frame, as well as the pose of the loop frame; the pose of the key frames around the current frame is updated by the relative pose of the current frame with the surrounding key frames before the current frame is updated; after the pose is updated, the landmarks related to the key frame are updated according to the relative position of each landmark and the key frame that generated the landmark; the landmarks observed in the loop frame and its adjacent key frames are projected into the current frame and its adjacent key frames, and the landmarks of the current frame are updated based on the historical data.

In a second aspect, an embodiment of the present invention provides a downhole positioning system based on comprehensive point and line features, comprising:

The preprocessing module performs dedistortion and binocular correction operations on the two original images of the binocular camera so that the epipolar lines of the two processed images are on the same horizontal line;

The extraction module uses an improved point and line feature processing algorithm to extract and match point and line features of the two images obtained by the preprocessing module;

A calculation module, which derives a reprojection error function of point and line features based on the features obtained by the extraction module;

The optimization module uses the Levenberg-Marquardt method to solve the reprojection error function obtained by the calculation module to obtain the camera pose to be estimated, and uses the local optimization thread to optimize the obtained camera pose to obtain the optimized pose; determines whether it is a key frame according to the pose, and if the current frame is a key frame, performs loop detection according to the similarity between the two frames, and corrects the detected loop to obtain the pose that needs to be corrected;

The positioning module uses the posture that needs to be corrected obtained by the optimization module to globally optimize the posture graph, output the posture of the image frame, and obtain the positioning trajectory.

Compared with the prior art, the present invention has at least the following beneficial effects:

A method for underground positioning based on comprehensive point and line features improves the poor positioning performance in dark scenes compared to the traditional SLAM algorithm that only relies on point features. On the basis of the original algorithm, a visual positioning algorithm with comprehensive point and line features is designed by introducing line features and image preprocessing algorithms. In view of the problem that point feature extraction becomes more difficult in dark scenes, the present invention uses an image preprocessing algorithm with low computational complexity to process image frames that need to be enhanced, and introduces relatively rich line features in actual scenes; in view of the problem that the introduction of line features increases system time consumption, the present invention designs an improved optical flow method and a line segment matching algorithm based on point and line invariants, which improves the problem that the feature matching algorithm relying on descriptors is relatively time-consuming.

Furthermore, there are many line features in actual scenes, and the combination of point and line features can reduce the difficulty of feature extraction and solve the problem of low positioning accuracy or even positioning failure.

Furthermore, the purpose of the bidirectional optical flow method is to obtain the motion information of objects in the scene by observing the movement of pixels in the image between two frames. Optical flow represents the displacement of each pixel in the image over time, that is, the motion trajectory of the object. The bidirectional optical flow method takes into account the optical flow information between two frames and can capture the motion characteristics of the object more comprehensively.

Furthermore, since the ring-shaped arrangement can reduce errors caused by noise or anomalies to a certain extent, the robustness of the system can be improved.

Furthermore, by setting S202, unrecognizable parts of the line segment can be eliminated, and removing outliers can improve the accuracy of the system and further improve the speed of line segment matching.

Furthermore, the gap between the spatial points and lines in the local map and the observable point and line key frames is measured by setting the reprojection error function F, and the positioning is solved based on this constraint.

Furthermore, the main components of the local optimization thread are: selecting key frames, maintaining local maps, and adjusting postures. This process can eliminate cumulative errors and improve positioning accuracy to a certain extent by optimizing the camera postures and spatial points and lines in the local map.

Furthermore, by setting key frames to represent the scene, the number of image frames that need to be processed is reduced. By focusing only on key frames, the algorithm can reduce the amount of calculation and improve the real-time performance of the system.

Furthermore, loop correction detection can identify scenes that the system has visited before, but due to errors in sensors and algorithms, there may be some deviations in the previously recorded locations. Through loop correction, the system can correct these errors, maintain global consistency, and improve positioning accuracy. Improve map consistency: Loop correction helps maintain the consistency of the constructed map. In the absence of loop correction, the system may have different parts of the map that do not match. Through loop correction, these parts can be aligned to build a more consistent map.

It can be understood that the beneficial effects of the second aspect mentioned above can be found in the relevant description of the first aspect mentioned above, and will not be repeated here.

In summary, the present invention provides more comprehensive and multi-dimensional information by integrating point and line features, so that high-precision positioning can be maintained even in low-light conditions. Compared with traditional SLAM algorithms based on point features, this method is more adaptable and can cope with the challenges of light changes. The advantage of this invention is that it provides more powerful tools, making it possible to achieve high-precision positioning in low-light and ever-changing underground scenes.

The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments.

BRIEF DESCRIPTION OF THE DRAWINGS

Figure 1 is a scene diagram of the MH_04_difficult sequence, where (a) is the MH_03_medium sequence and (b) is the MH_04_difficult sequence;

Fig. 2 is a basic framework diagram of the method;

Figure 3 is a diagram of the optical flow matching strategy;

FIG4 is a diagram of the line feature matching process based on point-line invariants;

FIG5 is a schematic diagram of line segment merging;

Fig. 6 is a schematic diagram of a point-line invariant;

FIG7 is a schematic diagram of line segment matching effect;

FIG8 is a schematic diagram of the reprojection error of line features;

Figure 9 shows the original image feature extraction and matching;

Figure 10 shows feature extraction and matching after image preprocessing;

Figure 11 is a trajectory diagram on the MH_04_difficult sequence;

Figure 12 is a graph of the APE error.

Detailed ways

The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

In the description of the present invention, it should be understood that the terms “include” and “comprises” indicate the presence of described features, wholes, steps, operations, elements and/or components, but do not exclude the presence or addition of one or more other features, wholes, steps, operations, elements, components and/or collections thereof.

It should also be understood that the terms used in the present specification are only for the purpose of describing specific embodiments and are not intended to limit the present invention. As used in the present specification and the appended claims, the singular forms "a", "an" and "the" are intended to include plural forms unless the context clearly indicates otherwise.

It should be further understood that the term "and/or" used in the present specification and the appended claims refers to any combination of one or more of the associated listed items and all possible combinations, and includes these combinations. For example, A and/or B can represent: A exists alone, A and B exist at the same time, and B exists alone. In addition, the character "/" in the present invention generally indicates that the associated objects are in an "or" relationship.

It should be understood that, although the terms first, second, third, etc. may be used to describe preset ranges, etc. in the embodiments of the present invention, these preset ranges should not be limited to these terms. These terms are only used to distinguish preset ranges from each other. For example, without departing from the scope of the embodiments of the present invention, the first preset range may also be referred to as the second preset range, and similarly, the second preset range may also be referred to as the first preset range.

The word "if" as used herein may be interpreted as "at the time of" or "when" or "in response to determining" or "in response to detecting", depending on the context. Similarly, the phrases "if it is determined" or "if (stated condition or event) is detected" may be interpreted as "when it is determined" or "in response to determining" or "when detecting (stated condition or event)" or "in response to detecting (stated condition or event)", depending on the context.

Various structural schematic diagrams of the embodiments disclosed in the present invention are shown in the accompanying drawings. These figures are not drawn to scale, and some details are magnified and some details may be omitted for the purpose of clear expression. The shapes of various regions and layers shown in the figures and the relative sizes and positional relationships therebetween are only exemplary, and may deviate in practice due to manufacturing tolerances or technical limitations, and those skilled in the art may further design regions/layers with different shapes, sizes, and relative positions according to actual needs.

The present invention provides an underground positioning method based on comprehensive point and line features, which has high positioning accuracy in underground scenes with dim light or changing light. In addition, due to the improvement of the more time-consuming point and line feature processing algorithm, the present invention avoids the problem of the traditional visual positioning method based on point and line features being more time-consuming.

Please refer to Figure 2. The present invention provides an underground positioning method based on comprehensive point and line features, including a visual odometer based on point and line features, i.e., a front-end thread, and a loop detection and optimization part; in the visual odometer part based on point and line features, firstly, the original image of the binocular camera is subjected to dedistortion and binocular correction operations, so that the epipolar lines of the two images are on the same horizontal line. Then, the image is subjected to point and line feature extraction and matching, after which the point and line reprojection error function of the system is constructed and pose estimation is performed; then, the pose is further optimized using a local optimization thread; in order to eliminate accumulated errors, a loop detection operation is performed, loop correction is performed after the loop is detected, and a global pose graph optimization is performed on the pose. The specific steps are as follows:

S1, read the original image of the binocular camera, and perform dedistortion and binocular correction operations;

S2, using improved point and line feature processing algorithm;

S201. Considering the large amount of parallel computing in the traditional LK optical flow method, GPU is used for accelerated computing. This idea is implemented in OpenCV through CUDA.

Please refer to FIG3 . In order to improve the accuracy of feature tracking, the present invention designs an improved optical flow matching strategy. The bidirectional matching strategy means that when point feature tracking is performed on images I ₁ and I ₂ , the following steps are performed:

As shown by the arrows from left to right, feature tracking is performed from image I ₁ to image I ₂ using the LK optical flow method;

Check the tracking results of the first step, keep the correctly tracked point features, and remove the incorrectly tracked ones;

The point features retained in the second step are transferred from image I ₂ to image I ₁ using the LK optical flow method, as shown by the arrow from right to left in Figure 3;

Check the tracking results in step 3 and perform similar operations as in step 2;

The retained point features are used as matching point pairs between image I ₁ and image I ₂ .

The ring operation in the matching strategy is as follows:

Previous left eye image With the current frame left eye image/> Using bidirectional optical flow, /> The point feature set is The set of matching points obtained by tracking is recorded as/>

For the left image of the current frame With the current frame right eye image/> Using bidirectional optical flow, /> The point feature set is/> The set of matching points obtained by tracking is recorded as x _temp1 ;

Right eye image of the current frame With the previous frame right eye image/> Using bidirectional optical flow, /> The point feature set is x _temp1 , and the matching point set obtained by tracking is recorded as x _temp2 ;

Previous right eye image The original point feature set is/> Obtained from the previous step /> The temporary point feature set is x _temp2 , and the features in x _temp2 are detected to determine whether they can fall into /> The surrounding of the corresponding feature is retained if possible, otherwise it is removed;

Current frame right eye image The temporary point feature set is x _temp1 , the previous frame right eye image/> The temporary point feature set is x _temp2 . According to the correspondence between x _temp1 and x _temp2 , the features removed from x _temp2 in the previous step are removed from x _temp1 , and the remaining features are retained and recorded as />

To address the problem that feature tracking is prone to failure in fast-motion scenes, the optical flow points are predicted through the motion of the previous frame to provide a better initial value for optical flow tracking.

S202, designing a line feature matching method based on point-line invariants for line feature matching between adjacent frames and left and right eye images;

First, the LSD (Line Segment Detector) algorithm is used to detect line segments and restore damaged line segments. Then, point-line invariants are constructed based on point features, and line segment matching is completed based on existing feature points. If the number of matching line segments based on point-line invariants is insufficient in some scenarios, the LBD descriptor is calculated for the line segments that do not meet the point-line invariant construction conditions and line segment matching is performed. This can effectively improve the speed and accuracy of line segment matching.

Please refer to Figure 4, the specific steps are as follows:

S2021, Line feature preprocessing

When performing line segment detection, the LSD algorithm will identify a continuous line segment as multiple segments. To address this problem, the present invention performs line segment merging processing. First, line segments that are too short are eliminated, and then the angle of the main direction of the line segment, the distance between the midpoints of the two line segments, the distance between the endpoints of the line segment, and the ratio of the endpoint distance to the average length of the two line segments are comprehensively evaluated. The effect is shown in Figure 5.

Set a threshold to remove line segments whose length is lower than the threshold. Line segments that are too short may be noise or insignificant. Removing them helps improve the robustness of the system. Calculate the main direction angle of each line segment. The coordinate information of the two endpoints of the line segment can be used to calculate the direction of the line segment. The main direction angle is helpful for subsequent segment merging judgment. The difference in direction angles can be used to evaluate whether two line segments tend to be parallel. Calculate the distance between the midpoints of the two line segments. If the midpoints of the two line segments are close, it may indicate that they are adjacent in space and may be merged. Calculate the distance between the two endpoints of each line segment. This distance can be used to evaluate the length of the line segment, and it can also be used to determine whether the line segments tend to be parallel. The ratio of the distance between the endpoints to the average length of the two line segments is used as a judgment factor. If this ratio is small, it means that the distance between the endpoints of the two line segments is relatively small, which may indicate that they are on the same straight line.

By comprehensively considering the above factors, a comprehensive evaluation index can be obtained, which can be used to decide whether to merge two line segments. The whole process aims to improve the accuracy and reliability of line segment detection.

S2022, Point-Line Invariants

For the projection of points and lines on the same plane in space, the ratio of the distance between the two point features and the line features in the previous and next frames remains unchanged, which is called the point-line invariant. The homography matrix is solved by direct linear transformation using 4 pairs of matching feature points. The premise for constructing point-line invariants is that the points and lines are located in the same plane. This paper believes that the points and lines located in the support domain of the line segment meet this requirement. The support domain of the line segment uses the distance between the point and the line segment as the determination principle, which is mainly divided into the distance of the point along the parallel line segment and the perpendicular line segment. Specifically, it refers to the distance between the point and the perpendicular bisector of the line segment and the distance between the point and the line segment. The requirements are less than 0.5 times the length of the line segment and less than 2 times the length of the line segment, respectively.

Please refer to Figure 6, which consists of a line segment located on the same plane and two feature points nearby. Assume that the spatial points P ₁ , P ₂ and the spatial line segment L are located on the plane β, and X ₁ , X ₂ , l ₁ and Y ₁ , Y ₂ , l ₂ are the projections of the spatial points P ₁ , P ₂ and the spatial line segment L on the previous and next frames.

For line segments l ₁ and l ₂ , the homography matrix is used to describe their mapping relationship. Assume that the mapping relationship is H, the coefficient vectors of the straight line where line segments l ₁ and l ₂ are located are p and q, and the homogeneous coordinates corresponding to points X ₁ , X ₂ and Y ₁ , Y ₂ are X ₁ , X ₂ and Y ₁ , Y ₂ .

The feature points and feature lines satisfy the following relationship:

q＝Hp (1)

_Yi = _HXi , i = 1, 2 (2)

remember:

Substituting equation (1) and equation (2) into equation (4), we get:

D(X ₁ ,X ₂ ,l ₁ )＝D(Y ₁ ,Y ₂ ,l ₂ ) (5)

From formula (5), we can see that for the projection of points and lines on the same plane in space, the distance ratio between two point features and line features in the previous and next frames is constant, and it is called point-line invariant. The homography matrix can be solved by direct linear transformation method using 4 pairs of matching feature points.

S2023, line segment matching

Directly calculating the point-line invariants for all line segments for matching will result in a waste of computing resources. This paper sets a range for the angle of the main direction of the line segment. If the angle is larger than the range, the next step of matching will not be performed, thus reducing the matching time.

For the line segments that meet the angle restrictions, verify whether they meet the conditions for constructing point-line invariants, that is, there are at least two matching point features in the line segment support domain. If they meet the conditions, calculate their point-line invariants and define the invariant error of the two line segments as:

AffSim(l ₁ ,l ₂ )=exp(−|D(X _i ,X _j ,l ₁ )−D(Y _i ,Y _j ,l ₂ )|), i,j∈[1,n]

Among them, (X _i ,X _j ) are two feature points in the support domain of line segment l ₁ , (Y _i ,Y _j ) are two feature points in the support domain of line segment l ₂ , (X _i ,Y _i ) is a pair of matching feature points, and n represents the number of matching feature points in the support domain of the line segment.

For line segments l ₁ and l ₂ , their final similarity Sim(l ₁ ,l ₂ ) is the maximum value of AffSim(l ₁ ,l ₂ ). If Sim(l ₁ ,l ₂ )>0.95, the line segments are matched successfully.

Directly calculating the point-line invariants for all line segments for matching will result in a waste of computing resources. A range is set for the angle of the main direction of the line segment. If the angle is larger than the range, the next step of matching is not performed, thus reducing the matching time.

For the line segments that meet the angle restriction, verify whether they meet the conditions for constructing point-line invariants, that is, there are at least two matching point features in the line segment support domain. If they meet the conditions, calculate their point-line invariants and define the invariant errors of the two line segments _l1 and _l2 as:

AffSim(l ₁ ,l ₂ )＝exp(−|D(X _i ,X _j ,l ₁ )−D(Y _i ,Y _j ,l ₂ )|),i,j∈[1,n] (6)

Among them, (X _i ,X _j ) are two feature points in the support domain of line segment l ₁ , (Y _i ,Y _j ) are two feature points in the support domain of line segment l ₂ , (X _i ,Y _i ) is a pair of matching feature points, and n represents the number of matching feature points in the support domain of the line segment.

For line segments l ₁ and l ₂ , their final similarity Sim(l ₁ ,l ₂ ) is the maximum value of AffSim(l ₁ ,l ₂ ). If Sim(l ₁ ,l ₂ )>0.95, the line segments are matched successfully. The overall matching effect is shown in FIG7 .

S3. Derivation of the reprojection error function of point and line features

S301. Parameterization method of point and line features

Point features are relatively simpler than line features and can be represented in three-dimensional space using Euclidean coordinates. The present invention uses Plücker coordinates and orthogonal methods to represent straight lines.

S302, reprojection error function of point and line features

The point and line error function is constructed using the reprojection error. The first projection refers to the feature formed by projecting the spatial point and line onto the image plane. Reprojection refers to reprojection using the estimated three-dimensional space landmarks and camera pose. The reprojection error refers to the difference between the estimated pixel point during reprojection and the pixel point obtained by the first projection. The reprojection error function of the comprehensive point and line features can be constructed as shown in formula (7):

Among them, p _l and I _l represent the point and line feature sets respectively, ρ _p and ρ _l represent the Huber robust kernel functions of point and line features respectively, Σ _j and Σ _k represent the Gaussian distribution covariance matrix of point and line features, and/> They represent the reprojection errors of the j-th point feature and the k-th line feature respectively.

S4. The error function is solved by using the Levenberg-Marquardt method (LM) to obtain the camera pose to be estimated, and the camera pose is optimized using a local optimization thread;

The obtained camera pose is obtained by relying on the information between two adjacent frames. As the system runs, the reliability of the results gradually decreases. In order to achieve higher positioning accuracy, the present invention draws on the co-viewing idea of the ORBSLAM2 algorithm, uses the key frames currently processed, the key frames connected to the current frame in the co-viewing, and the landmark spatial points and lines observed by these key frames to construct a local map, and uses the camera pose and spatial point and line positions as state variables to be optimized. The reprojection error function between the spatial points and lines in the local map and the key frames that can observe the points and lines is used as a constraint condition, and the sparsity of the graph model is used to accelerate the solution. By optimizing the camera pose and spatial points and lines in the local map, the cumulative error can be eliminated to a certain extent and the positioning accuracy can be improved.

S5. If the current frame is a key frame, loop detection is performed and the detected loop is corrected;

The principles for selecting key frames in the present invention are: there are no less than 20 ordinary frames between the current frame and the previous key frame; no less than 55 point features and 26 line features are tracked; the ratio of the point and line features observed jointly by the current frame and the previous key frame to the point and line features observed in the current frame is less than 0.7.

The present invention uses the bag-of-words model to take the similarity between two frames of images as the basis for loop detection. Generally, the similarity between two frames can be judged based on the bag-of-words model. The bag-of-words model is composed of word nodes, and the word nodes are composed of descriptors of image points and line features. The word vector corresponding to each frame of the image is obtained through the model, and then the word vectors of the two frames are compared to determine their similarity.

The main purpose of the correction is to update the drifted pose of the current frame to more accurate data. The new pose of the current frame can be obtained by calculating the relative pose of the current frame and the loop frame and the pose of the loop frame. At the same time, the key frames around the current frame can be updated with their relative poses to the surrounding key frames before the current frame is updated. After the pose is updated, the landmarks related to these key frames also need to be updated. For each landmark, its relative position with the key frame that generated it has not changed, and the landmark points and lines can be updated based on this feature. The landmarks observed in the current frame also need to be updated. The landmarks observed in the loop frame and its adjacent key frames can be projected into the current frame and its adjacent key frames. It is generally believed that historical data is more reliable, so the historical data is used as a basis to update the landmarks of the current frame.

S6. Optimize the pose graph, output the pose of the image frame, and obtain the positioning trajectory.

In order to obtain higher positioning accuracy, global optimization is required after loop detection. As the system continues to run, the scale of the variables to be estimated becomes larger and larger, which will cause the problem of reduced computational efficiency and increased time consumption. In fact, after local optimization, the spatial point and line coordinates tend to converge, and there is little need for back-end optimization again. Therefore, the present invention constructs a posture graph model that only optimizes the camera posture.

The following steps are performed to locate the trajectory based on the pose graph model:

1) Obtain the pose graph;

2) Set trajectory constraints: In the pose graph, nodes represent the pose of the camera, and edges represent the relative motion relationship between adjacent nodes. These constraints are provided by sensors, which can be visual feature matching, IMU measurement, etc. These constraints connect the poses at different times and form the basis of the trajectory.

3) Nonlinear optimization: Use nonlinear optimization algorithms to minimize the error terms of all constraints in the pose graph. This process adjusts the camera's pose so that the entire pose graph is more consistent with actual observations, thereby obtaining a more accurate trajectory estimate.

4) Trajectory Estimation: After the optimization is completed, the pose information of the camera at different times can be extracted from the pose graph to obtain an estimate of the entire trajectory. This trajectory estimate can be used for positioning, navigation or other applications. The pose graph is a graph structure in which nodes represent the poses of the camera and edges represent the constraints between two poses.

Such a model is used to minimize the estimation error of the camera at different times or positions, thereby improving the accuracy of the SLAM system.

In yet another embodiment of the present invention, a downhole positioning system based on comprehensive point and line features is provided, which can be used to implement the above-mentioned downhole positioning method based on comprehensive point and line features. Specifically, the downhole positioning system based on comprehensive point and line features includes a preprocessing module, an extraction module, a calculation module, an optimization module and a positioning module.

The preprocessing module performs dedistortion and binocular correction operations on the two original images of the binocular camera, so that the epipolar lines of the two processed images are on the same horizontal line;

The extraction module uses an improved point and line feature processing algorithm to extract and match point and line features of the two images obtained by the preprocessing module;

A calculation module, which derives a reprojection error function of point and line features based on the features obtained by the extraction module;

The optimization module uses the Levenberg-Marquardt method to solve the reprojection error function obtained by the calculation module to obtain the camera pose to be estimated, and uses the local optimization thread to optimize the obtained camera pose to obtain the optimized pose; determines whether it is a key frame according to the pose, and if the current frame is a key frame, performs loop detection according to the similarity between the two frames, and corrects the detected loop to obtain the pose that needs to be corrected;

The positioning module uses the posture that needs to be corrected obtained by the optimization module to globally optimize the posture graph, output the posture of the image frame, and obtain the positioning trajectory.

In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all of the embodiments. The components of the embodiments of the present invention described and shown in the drawings here can usually be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present invention provided in the drawings is not intended to limit the scope of the claimed invention, but merely represents selected embodiments of the present invention. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

The method of the present invention is tested in the MH_04_difficult sequence of the internationally recognized EuRoC data set. The MH_04_difficult sequence contains some environments with dim light, changing light, and poor texture, which can simulate the underground environment. A fragment of the sequence is shown in Figure 1.

The performance of this method is evaluated by using Absolute Pose Error (APE) as an evaluation indicator of the accuracy of the positioning algorithm. APE evaluates the positioning accuracy of the algorithm by calculating the distance between the estimated pose and the true pose.

The calculation formula of APE is:

APE _i = _Ti (T _i ′) ^-1 (8)

Among them, _Ti′∈SE (3) represents the estimated pose at time i, and _Ti∈SE (3) represents the true pose at time i.

After calculating the APE at all times, we can calculate its root mean square error (RMSE). The RMSE is used to evaluate the positioning accuracy of the algorithm on the entire trajectory. The calculation formula is as follows:

In the above formula, trans(APE) represents the translation part of APE.

The effect of line feature extraction and matching of the original image in the MH_04_difficult sequence is shown in Figure 9, and it can be found that the extracted features are relatively few. The effect of feature extraction and matching after image enhancement is shown in Figure 10, and it can be found that after image enhancement, the effect of feature extraction and matching is greatly improved.

The positioning accuracy of the method of the present invention and the ORBSLAM2 algorithm in the MH_04_difficult sequence is compared, as shown in Figures 11 and 12, where PL-SLAM represents the result of the method of the present invention. The information in the figure is summarized as shown in Table 1.

Table 1 Comparison of positioning accuracy (meters)

In the MH_04_difficult sequence, due to the insufficient texture and insufficient point features, after the introduction of line features, the positioning accuracy of the PL-SLAM algorithm is significantly improved compared with the ORBSLAM2 algorithm. The maximum APE error of the PL-SLAM algorithm is about 0.106 meters, which is 0.048 meters less than that of the ORBSLAM2 algorithm, and the APE root mean square error is 0.043 meters, which is 50.6% less than that of the ORBSLAM2 algorithm. The maximum APE error of the present invention is less than 0.1 meters, and the APE root mean square error is only 0.043 meters. In the scene with a motion range of about 6 meters wide and a distance of 58.6 meters, the positioning accuracy is quite high.

In summary, the present invention provides an underground positioning method and system based on comprehensive point and line features, which solves the problem that the traditional algorithm has insufficient positioning accuracy or even fails to locate in scenes with low light. On the basis of the traditional algorithm, a visual positioning algorithm based on comprehensive point and line features is designed by introducing line features and image preprocessing algorithms. The results show that the algorithm in this paper has obvious improvements and has higher positioning accuracy.

Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code.

The present application is described with reference to the flowchart and/or block diagram of the method, device (system) and computer program product according to the embodiment of the present application. It should be understood that each process and/or box in the flowchart and/or block diagram, and the combination of the process and/or box in the flowchart and/or block diagram can be realized by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device produce a device for realizing the function specified in one process or multiple processes in the flowchart and/or one box or multiple boxes in the block diagram.

These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable memory produce a manufactured product including an instruction device that implements the functions specified in one or more processes in the flowchart and/or one or more boxes in the block diagram.

These computer program instructions may also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, whereby the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in one or more processes in the flowchart and/or one or more boxes in the block diagram.

The above contents are only for explaining the technical idea of the present invention and cannot be used to limit the protection scope of the present invention. Any changes made on the basis of the technical solution in accordance with the technical idea proposed by the present invention shall fall within the protection scope of the claims of the present invention.

Claims (10)

1. A downhole positioning method based on comprehensive point and line features, characterized in that it comprises the following steps: S1, performing dedistortion and binocular correction operations on the two original images of the binocular camera, so that the epipolar lines of the two processed images are on the same horizontal line; S2, using an improved point and line feature processing algorithm to extract and match point and line features of the two images obtained in step S1; S3, deriving a reprojection error function of point and line features based on the features obtained in step S2; S4, using the Levenberg-Marquardt method to solve the reprojection error function obtained in step S3 to obtain the camera pose to be estimated, and using the local optimization thread to optimize the obtained camera pose to obtain an optimized pose; S5, judging whether the current frame is a key frame according to the posture obtained in S4, if the current frame is a key frame, performing loop detection according to the similarity between the two frames of images, and correcting the detected loop to obtain the posture that needs to be corrected; S6. Use the posture that needs to be corrected obtained in step S5 to globally optimize the posture graph, output the posture of the image frame, and obtain the positioning trajectory. 2. The downhole positioning method based on comprehensive point and line features according to claim 1, characterized in that step S2 specifically comprises: S201, using a GPU to accelerate the feature extraction process, using an improved optical flow matching strategy to perform bidirectional and circular operations respectively, to obtain image features; S202. Use the straight line segment detection algorithm to perform line segment detection and restore the damaged line segments. Then, construct the point-line invariant based on the point features, and complete the line segment matching based on the existing feature points. If the number of matching line segments based on the point-line invariant is insufficient, calculate the LBD descriptor for the line segments that do not meet the point-line invariant construction conditions and perform line segment matching to obtain the matched features. 3. The downhole positioning method based on comprehensive point and line features according to claim 2 is characterized in that in step S201, the bidirectional operation is specifically: When performing point feature tracking on images I ₁ and I ₂ , use the LK optical flow method to track features from image I ₁ to image I ₂ ; check the tracking results, retain the correctly tracked point features, and remove the incorrectly tracked ones; use the LK optical flow method to track the retained point features from image I ₂ to image I ₁ ; check the tracking results again; use the retained point features as the matching point pairs between image I ₁ and image I ₂ . 4. The downhole positioning method based on comprehensive point and line features according to claim 2 is characterized in that, in step S201, the circular operation is specifically: Previous left eye image With the current frame left eye image/> Using bidirectional optical flow, /> The point feature set is/> The set of matching points obtained by tracking is recorded as/> For the current frame left eye image/> With the current frame right eye image/> Using bidirectional optical flow, /> The point feature set is/> The set of matching points obtained by tracking is recorded as x _temp1 ; for the right eye image of the current frame/> With the previous frame right eye image/> Using bidirectional optical flow, /> The point feature set is x _temp1 , and the matching point set obtained by tracking is recorded as x _temp2 ; the previous frame right eye image/> The original point feature set is/> Through the previous frame right eye image/> The temporary point feature set is x _temp2 , and the features in x _temp2 are detected, and the features falling into /> The surrounding of the corresponding feature; the right image of the current frame The temporary point feature set is x _temp1 , the previous frame right eye image/> The temporary point feature set is x _temp2 . According to the correspondence between x _temp1 and x _temp2 , the features removed from x _temp2 are removed from x _temp1 , and the remaining features are retained and recorded as /> 5. The downhole positioning method based on comprehensive point and line features according to claim 2, characterized in that step S202 specifically comprises: S2021, eliminating line segments whose length is less than a set threshold, comprehensively evaluating the angle of the main direction of the line segment, the distance between the midpoints of the two line segments, the distance between the endpoints of the line segment, and the ratio of the endpoint distance to the average length of the two line segments, and merging the line segments; S2022. For the projection of points and lines on the same plane in space, the distance ratio between two point features and line features in the previous and next frames remains unchanged, which is called point-line invariant. The homography matrix is solved by direct linear transformation method using 4 pairs of matching feature points. S2023. When the angle of the main direction of the line segment is less than or equal to the set threshold, and there are at least two matching point features in the line segment support domain, the corresponding point-line invariant is calculated. For line segments l ₁ and l ₂ , the final similarity Sim(l ₁ ,l ₂ ) is the maximum value of AffSim(l ₁ ,l ₂ ). If Sim(l ₁ ,l ₂ )>0.95, the line segment is matched successfully. 6. The downhole positioning method based on comprehensive point and line features according to claim 1, characterized in that in step S3, the reprojection error function F is specifically: Among them, p _l and I _l represent the point and line feature sets respectively, ρ _p and ρ _l represent the Huber robust kernel functions of point and line features respectively, Σ _j and Σ _k represent the Gaussian distribution covariance matrix of point and line features, and/> They represent the reprojection errors of the j-th point feature and the k-th line feature respectively. 7. The downhole positioning method based on comprehensive point and line features according to claim 1 is characterized in that, in step S4, the obtained camera pose is optimized using a local optimization thread as follows: A local map is constructed using the currently processed keyframe, the keyframes connected to the current frame in the co-visual map, and the landmark spatial points and lines observed by the keyframes. The camera pose and spatial point and line positions are used as state variables to be optimized. The reprojection error function between the spatial points and lines in the local map and the keyframes where the corresponding points and lines can be observed is used as a constraint condition. The sparsity of the graph model is used to accelerate the solution to obtain the optimized camera pose. 8. The downhole positioning method based on comprehensive point and line features according to claim 1 is characterized in that, in step S5, the key frame selection principle is as follows: The number of ordinary frames between the current frame and the previous key frame is greater than or equal to 20; greater than or equal to 55 point features and 26 line features are tracked; the ratio of the point and line features observed jointly by the current frame and the previous key frame to the point and line features observed in the current frame is less than 0.7. 9. The downhole positioning method based on comprehensive point and line features according to claim 1 is characterized in that in step S5, the detected loop is corrected specifically by: The new pose of the current frame is determined by calculating the relative pose of the current frame and the loop frame, as well as the pose of the loop frame; the pose of the key frames around the current frame is updated by the relative pose of the current frame with the surrounding key frames before the current frame is updated; after the pose is updated, the landmarks related to the key frame are updated according to the relative position of each landmark and the key frame that generated the landmark; the landmarks observed in the loop frame and its adjacent key frames are projected into the current frame and its adjacent key frames, and the landmarks of the current frame are updated based on the historical data. 10. A downhole positioning system based on comprehensive point and line features, characterized by comprising: The preprocessing module performs dedistortion and binocular correction operations on the two original images of the binocular camera so that the epipolar lines of the two processed images are on the same horizontal line; The extraction module uses an improved point and line feature processing algorithm to extract and match point and line features of the two images obtained by the preprocessing module; A calculation module, which derives a reprojection error function of point and line features based on the features obtained by the extraction module; The optimization module uses the Levenberg-Marquardt method to solve the reprojection error function obtained by the calculation module to obtain the camera pose to be estimated, and uses the local optimization thread to optimize the obtained camera pose to obtain the optimized pose; determines whether it is a key frame according to the pose, and if the current frame is a key frame, performs loop detection according to the similarity between the two frames, and corrects the detected loop to obtain the pose that needs to be corrected; The positioning module uses the posture that needs to be corrected obtained by the optimization module to globally optimize the posture graph, output the posture of the image frame, and obtain the positioning trajectory.