Point-line feature fusion-based transformer substation inspection robot positioning and mapping method

Abstract

The invention discloses a transformer substation inspection robot positioning and mapping method based on point-line feature fusion. According to the method, a binocular camera is used for collecting point-line features in an image frame, and high-quality long-line features are quickly obtained based on an adaptive criterion optimized EDLines algorithm; the spatial depth of point-line features is obtained through triangularization of a binocular camera, and redundant line features are filtered based on a length response threshold strategy of the spatial depth of a transformer substation. Based on a point-line pole feature pole plane constraint method, IMU information and visual information are combined, gyroscope zero offset is independently estimated and optimized, IMU pre-integration information is updated, and a more accurate initial posture of the inspection robot is obtained. A point-line feature dictionary based on a transformer substation scene and a large-scale equipment target detection model in a transformer substation are made, closed-loop detection is performed through transformer substation equipment road signs and point-line features, and rapid relocation correction is performed on sliding windows of a current key frame and a previous fixed key frame number. And finally, the pose of the substation inspection robot and a substation scene map are optimized through global BA, and the problem of large-scale scene error drift accumulation is solved.

Description

Technical Field

[0001] The present invention relates to the field of SLAM in complex scenes combining vision and inertial fusion of point and line features, and is a method for stable mapping and precise positioning of a substation inspection robot in a complex environment of a substation. Background Art

[0002] With the continuous and rapid development of the economy and society, my country's dependence on power equipment is increasing. As an important part of the modern power system and the future smart grid, the substation is responsible for the voltage increase and decrease regulation and the distribution of electric energy in the power network. The stability and reliability of its operation directly determine the stability of the power supply system. Therefore, it is necessary to regularly inspect the equipment in the substation and eliminate safety hazards as soon as possible to ensure the safe and stable operation of the power network.

[0003] Traditional inspection methods usually require manual intervention. On the one hand, substations are large in area and have diverse equipment, so manual inspections are time-consuming and labor-intensive. On the other hand, manual live-line operations have certain safety risks. Therefore, it has become a trend to introduce robot technology to achieve automatic inspections of substations. The substation inspection robot is a complex system that integrates a mobile platform, multiple sensors, a positioning and navigation system, a data processing and decision-making system, and an autonomous control system to achieve accurate, efficient, and safe inspection operations, effectively avoiding a series of problems faced by manual inspections.

[0004] Trackless navigation technology is the premise for autonomous positioning and real-time understanding of equipment status. Simultaneous Localization and Mapping (SLAM), as the core technology of robot autonomous navigation, has gradually become one of the research hotspots of trackless inspection robots in substations. Today, SLAM research mainly focuses on laser SLAM, visual SLAM and multi-sensor fusion SLAM. Laser SLAM solutions can be divided into single-line laser radar or multi-line laser radar. Single-line laser radar lacks most scene information due to its 2D dimension limitation; multi-line laser radar complements the lack of single-line laser radar in dimension, but its large point cloud load and high cost limit its application in substations. Visual SLAM uses the image information captured by the camera to estimate the position and posture of the robot in real time, and at the same time builds a three-dimensional map of the environment. It has super scene recognition capabilities and can obtain rich external texture information. However, among the existing visual algorithms, point feature-based algorithms such as ORB-SLAM series and Vins-Fusion have certain spatial limitations and are far from describing the complex structure of the substation. Moreover, the perception aliasing will affect the mapping and positioning of similar scenes in the substation. The sparse map constructed by the algorithm in the complex outdoor large-scale scene of the substation has low accuracy and poor system initialization robustness. Line feature-based algorithms such as PL-SLAM and PL-VIO have long line feature detection and matching time and weak geometric constraints, and low point-line feature fusion efficiency, which seriously affects the efficiency of substation inspection and map construction accuracy. In addition, the illumination in the outdoor environment may vary greatly, which affects the quality of camera perception and image feature extraction. The smooth surface and metal structure of the equipment in the artificial structure scene outside the substation are prone to interference from unreliable textures. Although the inertial measurement unit (IMU) provides high-frequency updates of the equipment's motion status, which makes up for the disadvantage that the camera is difficult to extract reliable feature points when moving rapidly, its inaccurate posture makes it impossible to remove the influence of gravity, and the continuous accumulation of errors will affect the construction of the global map. Summary of the invention

[0005] In view of the above challenges and problems, the present invention provides a substation inspection robot positioning and mapping method based on point-line feature fusion to improve the positioning accuracy and robustness of the substation inspection robot in large-scale substation scenarios. The present invention uses a binocular camera to collect point-line features in image frames, and quickly obtains high-quality long-line features based on the EDLines algorithm optimized by adaptive criteria; the spatial depth of the point-line features is obtained by triangulation of the binocular camera, and the redundant line features are filtered based on the length response threshold strategy of the substation spatial depth. Based on the method of point-line polar feature polar plane constraint, the IMU information and visual information are combined to independently estimate and optimize the gyroscope zero bias, and the IMU pre-integration information is updated to obtain a more accurate initial posture of the inspection robot. A point-line feature dictionary based on the substation scene and a target detection model for large-scale equipment in the substation are made, and closed-loop detection is performed through the substation equipment landmarks and point-line features. The current key frame and the sliding window of the previous fixed number of key frames are quickly relocated and corrected. Finally, the substation inspection robot posture and substation scene map are optimized through global BA, which solves the problem of error drift accumulation in large-scale scenes. The key point of the present invention is to improve the line feature extraction method, to more quickly build a more spatial semantic map through substation equipment landmarks and point and line features, to improve the visual-inertial joint initial optimization method and closed-loop correction method, to compensate for and optimize the inaccurate mapping and unstable positioning in complex substation scenes.

[0006] The substation inspection robot positioning and mapping method based on point-line feature fusion includes the following steps:

[0007] Step 1: Obtain binocular image data, extract point features and line features respectively, and match them.

[0008] Step 2: Calculate the initial posture of the substation inspection robot in pure vision based on the point-line features; through the constraints of the point-line features projected on the polar plane, decouple and optimize the inspection robot's own posture to obtain a more robust and high-precision map initialization result.

[0009] Step 3: Train the closed-loop word dictionary and medium-to-large equipment target detection model for point and line features in the substation scene. During the inspection process of the substation inspection robot, the semantic information of the equipment in the scene and the point and line feature descriptors are extracted for closed-loop detection, the weighted similarity score of the point and line features is optimized, and the key frame with the highest similarity score in the key frame database is found as the optimal closed-loop key frame to complete the closed-loop detection.

[0010] Step 4: When it is detected that the current keyframe has a corresponding optimal closed-loop keyframe, the relative posture relationship between the current keyframe and the optimal closed-loop keyframe is calculated through optimized fast repositioning, and closed-loop correction is performed through the nonlinear global optimization of the back-end to update and optimize the posture of all inspection robots in the current map and the complex outdoor three-dimensional map of the substation.

[0011] The beneficial effects of the present invention are as follows:

[0012] The method described in the present invention proposes a substation inspection robot positioning and mapping method based on the fusion of point and line features for complex substation operation and maintenance scenarios, combining the advantages of point features and line features to effectively obtain more structural information of the substation scene; speeding up the line feature extraction speed, removing broken line features and redundant short line features, ensuring the accuracy of the map; and optimizing the map initialization method to improve the robustness of the system; in the face of the cumulative errors brought by large-scale outdoor scenes, the closed-loop detection and correction method of point and line features and substation equipment landmarks greatly reduces the errors; the scheme can obtain a more structured and more stably positioned navigation map, making navigation in the substation scene faster and more reliable, providing strong support for operation and maintenance personnel.

[0013] Innovation 1: Add line features to visual information, improve the EDLines line detection algorithm through adaptive merging criteria, and remove redundant line features based on the substation spatial depth response threshold strategy. In the complex operation and maintenance environment of substations, line features can effectively transmit structural information compared to point features; based on the EDLines algorithm, improve its related parameters, and merge the fracture line features according to the adaptive merging criteria; according to the binocular camera, obtain the substation spatial depth through left and right eye triangulation, propose the length response threshold strategy of the substation spatial depth, screen out redundant line features, and reduce the calculation burden of line feature optimization in the back-end.

[0014] Innovation 2: Based on the polar plane constraint method of point and line features, the map initialization method is optimized to obtain a more accurate initial posture of the inspection robot. By combining visual and IMU information, adding constraints on the projection of point and line features on the polar plane, independently estimating and optimizing the gyroscope zero bias; updating the IMU pre-integration information, decoupling optimization to obtain the accurate initial posture of the inspection robot, optimizing the map initialization in the complex outdoor operation and maintenance environment of the substation, and improving its robustness.

[0015] Innovation 3: Add spatial constraints through point-line feature dictionary and substation equipment landmarks to optimize closed-loop detection and correction methods. Based on the substation image dataset, point-line feature descriptors are used to generate point-line feature dictionaries, and target detection models for large and medium-sized equipment in substations are trained. Closed-loop detection is performed on the map based on the point-line feature dictionary and substation equipment landmarks, and a more appropriate point-line weighted similarity score method is used to evaluate the optimal closed-loop keyframe. The residual optimization function constructed by the sliding window is added with the substation equipment landmark residual, and local map optimization is performed through rapid relocation. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 The figure is a flow chart of a method according to an embodiment of the present invention. DETAILED DESCRIPTION

[0017] The technical solution of the present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0018] The present invention provides a method for positioning and mapping a substation inspection robot based on point-line feature fusion. Figure 1 As shown, the following steps are included:

[0019] Step 1: During the inspection process, the substation inspection robot uses a binocular camera to obtain left and right target image frames in real time as the input of the visual thread. The visual thread preprocesses the acquired image frames, obtains point features and line features respectively, matches them, and stores the image frames in the image frame database. At the same time, the first image frame is set as the initial key frame, and every 1s or when the number of point and line feature pairs matched by the new image frame is less than 1 / 4, the new image frame is set as the key frame, and the key frame is stored in the key frame database. Among them, the EDLines algorithm is optimized by the adaptive merging principle for line feature extraction, and redundant short line features are screened out by the length response threshold strategy based on spatial depth. The specific operations of the visual thread are as follows:

[0020] Step 1.1: Grayscale and histogram equalization are performed on the left and right image frames obtained by the binocular camera. The Shi-Tomasi algorithm is used to obtain the point features of each image frame, and the feature points are homogenized using a quadtree to remove redundant point features. According to the known left and right parallax of the binocular camera, the point features of the left and right image frames are triangulated to obtain spatial point features with absolute scale. The BRIEF descriptor is used to describe the pixel information around the point features. The Hamming distance between the BRIEF descriptors in the adjacent left image frames is compared to perform rough similarity matching, and the RANSAC progressive sample consistency (PROSAC) algorithm is used to eliminate the wrong matching point feature matching pairs.

[0021] Step 1.2: For the left-eye image frame acquired by the binocular camera, extract line features through the optimized EDLines algorithm, use the LBD descriptor to describe the local appearance of the line features, and use KNN to match the line features. The specific steps include:

[0022] Step 1.2.1: Image preprocessing: Grayscale the left-eye image frame collected by the binocular camera, and then perform bilateral filtering and denoising on the grayscale image to obtain the preprocessed image. Compared with Gaussian filtering, bilateral filtering uses pixel similarity weighting method to denoise while retaining the edge details of the image, preparing for the subsequent line feature extraction.

[0023] Step 1.2.2: Line feature extraction based on the optimized EDLines algorithm:

[0024] The image gradient function is obtained by the grayscale value of the preprocessed image. Since the image is equivalent to a two-dimensional discrete function, the grayscale approximation of the image gradient function is calculated using the Sobel discrete difference operator based on the difference method to obtain the gradient value and gradient direction at the pixel point (x, y).

[0025] The image gradient function is used to calculate the image gradient of the preprocessed image to obtain a gradient map. The entire gradient map is traversed to filter out pixel points whose pixel gradient values ​​are greater than the gradient threshold compared to the adjacent pixel points, to obtain an image area with more edge details. The preferred gradient threshold is TH. gradient =5.22. For the obtained new image area, the pixel points whose gradient values ​​are greater than the anchor point threshold compared with the gradient values ​​of the adjacent pixel points are selected in the gradient direction as anchor points. The anchor point threshold is preferably TH anchor =3. For anchor points, according to the surrounding pixels of the current anchor point, find the pixel points (possibly anchor points) with the same gradient direction as the current anchor point as the extended pixel points. If the gradient direction of the next pixel point on the gradient direction of the current anchor point is inconsistent with the gradient direction of the current anchor point, detect the two adjacent pixels of the next pixel point. Finally, when all the extended directions are extracted, it means that the current anchor point connection is completed. For the remaining unconnected anchor points, repeat the extension operation to connect the anchor points until all the anchor points are connected into line edges. The above pixel extension method reduces the search amount required for anchor point extension and speeds up the extraction of image line edges.

[0026] For the extracted image line edges, based on the minimum straight line length and the maximum mean square line fitting error, the number of false alarms (NFA) is used to fit the image line edges into appropriate line segments and retain them in the line segment set.

[0027] After generating a line segment set containing all the line segments of the image, sort them in descending order by length, and filter the line segments by angle and spatial distance. First, calculate the angle between the current line segment and the direction vector of the remaining line segments in the line segment set, and filter out line segments with too large angles:

[0028]

[0029] Among them, l1 and l2 represent the two line segments to be compared, v1 and v2 represent the direction vectors of l1 and l2, and θ min It represents the screening threshold between the characteristic angles of the comparison line, and 0.2rad is preferably taken here;

[0030] Secondly, the line segments with too large horizontal and vertical distances are filtered out respectively. As long as there is a pair of endpoints of the current line segment and another line segment that meet the following conditions, the other line segment is the pre-merged line segment of the current line segment; compared with the calculation of Euclidean distance, this screening saves calculation overhead:

[0031] |x 1i -x 2j | <d min

[0032] |y 1i -y 2j | <d min

[0033] i,j∈[1,2]

[0034] Among them, x 1i and x 2j are the horizontal positions of the two end points of line segment 1 and line segment 2, respectively, 1i and 2j are the vertical positions of the two end points of line segment 1 and line segment 2, respectively. min It is the screening threshold for comparing the distance between the endpoints of line features, and 30 is preferably selected here.

[0035] After the segment set is traversed and screened by the above method, the pre-merged segment can be merged with the corresponding segment only if it meets the adaptive merging criteria. First, the Euclidean distance between the endpoint pair of the pre-merged segment and the corresponding segment is less than d min ; Secondly, the angle threshold is adaptively changed according to the length of the pre-merged line segment, so that the longer pre-merged line segment has a stricter angle threshold, and the angle threshold sigmoid function based on the adaptive length is established. The specific calculation method is as follows:

[0036]

[0037] Among them, k and a are the steepness and translation position of the sigmoid function, respectively. Here, it is preferred that k is 2.5 and a is 2, so that the adaptive angle threshold has a stricter restriction on the merging of pre-merged long line segments; n is the normalized length of the pre-merged line segment.

[0038] When the pre-merged segment meets the above adaptive merging criteria, find the farthest two endpoints of the current segment and the pre-merged segment in the horizontal direction of the current segment and use them as the endpoints of the merged segment; if the farthest two endpoints are both endpoints of the current segment, ignore the pre-merged segment. After selecting new endpoints, connect the two endpoints to merge the two lines into a new segment, replace the original corresponding segment, and delete the pre-merged segment from the segment set. Repeat the above operation for the segments in the segment set in order of length until there are no pre-merged segments that can be merged in the segment set.

[0039] Step 1.2.3: Adopt the length response threshold strategy based on the substation spatial depth to filter out redundant short-line features:

[0040] Considering that the depth of the offline features in the large outdoor scene of the substation is different, and there are short line features when the EDLines algorithm extracts the image frame of the large substation scene, in order to ensure the reliability of the line features and the computational cost of the system, a length response threshold strategy based on spatial depth is used to set the threshold L according to the spatial depth of the line features and the image size. min To filter out redundant short-line features, the formula is as follows:

[0041]

[0042] Among them, min(W I ,H I ) represents the smaller value of the width and height of the input image; z is the spatial depth of the line feature, b is the left and right eye baselines of the binocular camera, and η is the ratio factor, and η=0.125 is preferably set here.

[0043] Step 1.2.4: Based on the LBD descriptor, use KNN to match line features:

[0044] According to the screened line features, the support area of ​​each line feature is divided into pixel strips, and the strip descriptor is calculated. The LBD descriptor is constructed by the mean vector and standard deviation vector of the strip descriptor. By calculating the Hamming distance of the LBD descriptors of two line features in adjacent frames, a kd tree is constructed and the optimal line feature pair is obtained through the nearest neighbor search.

[0045] Step 2: According to the point feature pairs and line feature pairs matched by the left and right eyes of the initial key frame in step 1, the initial posture of the substation inspection robot in the pure vision case is calculated through the essential matrix. According to the required number of point and line features, continuous key frames are selected from the initial key frame to form a continuous key frame set. Based on the first-order Taylor expansion of the direction vectors of the point and line feature pairs on the polar plane of the left and right eyes of the adjacent key frames and the pre-integration of the IMU rotation part, the minimum eigenvalue optimization problem of the point and line features on the polar plane is constructed by combining visual and inertial information to obtain the initial gyroscope zero bias, set up the prior and IMU residual functions to obtain the optimal estimation of the key frame speed, gravity direction and IMU zero bias, and finally decouple and optimize the posture of the inspection robot itself in turn to obtain a more robust and high-precision map initialization result.

[0046] Step 2.1: Obtain the initial posture of the substation inspection robot under pure vision:

[0047] According to the point feature pairs successfully matched in the initial key frame obtained in step 1, the image coordinates of the successfully matched point feature pairs are first converted into normalized coordinates, and 8 pairs of point features are randomly selected to construct a linear equation system to estimate the essential matrix. Then, the singular value matrix obtained by SVD decomposition is used to restore the camera's rotation matrix and translation vector, that is, the initial posture of the substation inspection robot under pure vision conditions.

[0048] Step 2.2: IMU preprocessing and gyroscope bias construction:

[0049] Since the state propagation of the IMU model is in the form of pre-integration, but the amount of pre-integration calculation is too large, the partial derivative form of the first-order Taylor expansion is used to adjust the pre-integration. and its first-order partial derivative for:

[0050]

[0051] Among them, b k and b k+1 are the kth frame and k+1th frame of the IMU system, respectively, t k and t k+1 are the timestamps of the kth frame and the k+1th frame respectively, Ω(ω) is the angular velocity matrix, is the IMU angular velocity measurement value at the tth moment, ω x ,ω y ,ω z They are the x, y, and z axis components of the IMU angular velocity vector, is the rotation change from time t to the kth frame of the IMU system, and are the gyro bias and the increment of gyro bias in the kth frame of IMU, and b ω is the gyroscope bias, is the Jacobian matrix of the pre-integrated rotation term to the gyroscope bias.

[0052] For point features in three-dimensional space, connect them to the center of the left and right eyes of the camera in the kth and k+1th frames to obtain the direction vectors and For line features in three-dimensional space, the direction vector f from the camera optical center to the line feature is obtained by cross-producting the normal vector n of the Plücker coordinates and the line direction vector d. l :

[0053]

[0054] Among them, n is the normal vector of the polar plane formed by the line feature and the camera optical center, and d is the direction vector of the line feature.

[0055] Connect the line features to the center points of the left and right eyes of the camera in the kth and k+1th frames to obtain the direction vectors and The two vectors form different polar planes with the baselines of adjacent frames. Through the rotation matrix between the IMU coordinate system and the camera coordinate system After converting the IMU coordinate system where the gyroscope zero bias is located into the left and right eye coordinate systems of the camera, a coordinate system based on the direction vector and The gyroscope zero bias b is optimized by minimizing the eigenvalue model ω , the formula is as follows:

[0056]

[0057] in, It is a set of continuous key frames in the initialization phase with IMU measurement information. The continuous key frame set is composed of continuous key frames selected from the initial key frame according to the number of point and line features required. L M k,k+l and R M k,k+l is a positive semidefinite matrix, λ min is the minimum eigenvalue of the semi-positive definite matrix, which is the product of the matrix formed by the polar plane constraint relationship of the left and right cameras and its transposed matrix, N L and N R are the total number of point features matched by the left and right eyes respectively, M L and M R are the total number of line features matched by the left and right eyes respectively, and are the left and right eye centers of the kth frame and the i-th point feature p i The direction vector between and are the left and right eye centers of the kth frame and the jth line feature l i The direction vector between and They are the left camera coordinate system c L and the right camera coordinate system c R The transformation matrix between the IMU coordinate system b.

[0058] Step 2.3: IMU state variable optimization:

[0059] The preliminary gyroscope bias estimate is obtained through step 2.2, and the optimal estimate of the IMU state variables is further obtained. That is, the key frame velocity, gravity vector, gyroscope bias and accelerometer bias b a , the formula is as follows:

[0060]

[0061] in, is the speed of each of the N+1 key frames in the world coordinate system, is the gravity vector in the camera coordinate system.

[0062] Through IMU pre-integration and The residual function is constructed based on the state variables in order to optimize the state variables. The formula is as follows:

[0063]

[0064] Among them, r p is the IMU bias prior residual, is the residual of the change in IMU measurement information between the kth frame and the k+1th frame, Σ p is the covariance of the IMU zero-bias prior, is the covariance of the change in IMU measurement information between the kth frame and the k+1th frame.

[0065] Step 2.4: Gradually optimize the posture of the substation inspection robot through decoupling:

[0066] While obtaining the optimal IMU state variables in step 2.3, the residual of the pre-integrated rotation part is Get the rotation matrix between the optimized world coordinate system and the kth frame of the current IMU system Then update the rotation matrix of each image key frame in the left and right eyes:

[0067]

[0068] in, and are the rotation matrices of the left camera and the right camera from the kth frame to the world coordinate system, and They are the fixed rotation matrices from the left camera and the right camera to the IMU coordinate system.

[0069] For each point and line feature extracted from the key frame of the image, the least squares method is used to construct an error model such as point and line reprojection to optimize the translation vector The formula is as follows:

[0070]

[0071] Among them, X i is the i-th 3D point feature in the world coordinate system, x i Yes X i The two-dimensional point features projected on the left-eye k-th frame image plane, m j are the homogeneous coordinates of the midpoint of the j-th 2D line feature, It is the projected two-dimensional line feature obtained by transforming the j-th three-dimensional line feature in the camera coordinate system to the k-th image plane of the left eye. and is the coefficient of the projected two-dimensional line feature, n xand n l are the number of point features and line features respectively, π is the projection function, ρ is the Huber kernel function that only processes visual outliers, and Σ is the covariance of the residual function.

[0072] Step 3: Train the closed-loop word dictionary and medium-to-large equipment target detection model for point and line features in the substation scene. During the inspection process of the substation inspection robot, the semantic information of the equipment in the scene and the point and line feature descriptors are extracted for closed-loop detection, the weighted similarity score of the point and line features is optimized, and the key frame with the highest similarity score in the key frame database is found as the optimal closed-loop key frame to complete the closed-loop detection.

[0073] Step 3.1: Offline training of point and line feature dictionary in substation scenario:

[0074] To obtain a sufficient number of substation image datasets, it is necessary to cover a collection of images taken under different lighting conditions, different substation scenes, different weather conditions, and different seasons. First, all the above training images are traversed, and Shi-Tomasi point features and EDLines line features are extracted for each image, and the BRIEF descriptor and 256-dimensional binary LBD descriptor are used to quantify the feature information. The third-party library DBoW2 library of the bag of words model is used to establish a dictionary tree of point features and line features, and the number of branches of the dictionary tree is uniformly set to K = 10 and the depth L = 5. The descriptors of the extracted point features and line features are clustered into 10 sets using K-means, that is, all feature samples are clustered into 10 categories, and clustered down 4 times in turn, and a point and line feature dictionary is generated offline. Finally, the last layer of the dictionary nodes are called words, and TF-IDF (frequency-inverse document frequency) is used to assign a certain weight according to the frequency of each word in the training set.

[0075] Step 3.2: Extracting semantic information of substation equipment:

[0076] The images of medium and large equipment are selected from the substation image dataset as the substation equipment dataset, and the data is annotated. Considering the real-time requirements of the SLAM system, the lightweight YOLOv5s model is selected as the medium and large equipment target detection model, and only the key frames are detected to meet the real-time performance requirements of SLAM. In the large-scale substation scene, in order to reasonably utilize the limited visual information processing resources, the attention module CA (coordinate attention) is selected to be inserted into the backbone network (backbone) convolution layer of the lightweight YOLOv5s model. This module introduces coordinate information to obtain the precise positioning information of the substation equipment and enhances the target position information capture ability of the medium and large equipment target detection model. After training the substation equipment dataset with the medium and large equipment target detection model, the medium and large equipment target detection model is built into the industrial computer of the inspection robot to obtain the semantic information of the substation equipment in real time. Due to the possibility of false detection, only when the medium and large equipment target detection model detects the spatial position information of the device in three consecutive key frames, it is mapped as a landmark to the map with point and line features.

[0077] Step 3.3: Get closed-loop frames through closed-loop detection:

[0078] After the map initialization is completed, the closed-loop detection thread is started to perform closed-loop detection on the current keyframe obtained in real time during the inspection process. If the current keyframe contains a substation equipment landmark, the most recently mapped landmark is identified and compared with the landmark in the keyframe in the keyframe database, and the spatial proximity between them is calculated to determine whether the keyframe in the keyframe database is a closed-loop candidate keyframe:

[0079]

[0080] Among them l cur is the two-dimensional projection position of the landmark of the current key frame, is the landmark set in the keyframe in the keyframe database, Landmark collection is the 3D position of the i-th landmark in , and ||·||2 is the 2-norm.

[0081] At the same time, based on the point features and line features in the current key frame obtained by the visual thread, the point and line feature dictionary obtained in step 3.1 is used to compare the descriptors of the point and line features online with the cluster centers of each intermediate node of the tree structure in the dictionary (4 times), so as to search for the word position where the descriptor is located, and compare the key frames in the key frame database, find the key frames whose common words between the two frames exceed 80% of the words in the current key frame, and store them in the closed-loop candidate key frame group, and convert the point and line features of the current key frame into bag-of-words vectors of point features and line features respectively. By comparing the bag-of-words vectors between the current key frame and all closed-loop candidate key frames in the closed-loop candidate key frame group, the image similarity score based on point features and line features is obtained. In the similarity score, the two criteria of feature scale and distribution degree are used to weight the similarity scores of the two features, and the normalized feature scale I is calculated. p ,I l and distribution degree D p , D l , the formula is as follows:

[0082]

[0083] Among them, N p 、N l and N max are the number of point features, line features and all features, D p , D l and D max They are the distribution values ​​of point features, line features and all features respectively.

[0084] Based on the distance weighting, dispersion is added to the denominator of the similarity score as a penalty term to reduce the impact of high dispersion on the score. The formula is as follows:

[0085]

[0086] where w p and w l is the weight coefficient of point features and line features. Point features are more important than line features. Here, w is preferred. p Take 0.6, w l Take 0.4.

[0087] According to the weighted similarity score, the closed-loop candidate keyframe with the highest similarity score with the current keyframe is found as the closed-loop keyframe, but the closed-loop keyframe needs to be checked for temporal consistency: the closed-loop keyframe cannot be the keyframe in the sliding window where the current keyframe is located; and a spatial coherence check is also required: the similarity score between the closed-loop keyframe and all other keyframes in the current sliding window needs to be greater than the set similarity threshold, which is set to 0.75 times the similarity score between the closed-loop keyframe and the current keyframe. Only when the above two conditions are met can the closed-loop keyframe be determined as the optimal closed-loop keyframe.

[0088] Step 4: When it is detected that the current keyframe has a corresponding optimal closed-loop keyframe, the relative posture relationship between the current keyframe and the closed-loop keyframe is calculated through optimized fast repositioning, and closed-loop correction is performed through the nonlinear global optimization of the back-end to update and optimize the posture of all inspection robots in the current map and the complex outdoor three-dimensional map of the substation.

[0089] Step 4.1: Calculate the relative pose relationship between the current frame and the closed-loop frame using the optimized fast relocalization method:

[0090] When the optimal closed-loop keyframe is detected, the point-line features of the optimal closed-loop keyframe and the current keyframe are first used for rough matching based on the descriptor. The homography matrix of high-quality point-line pairs is calculated using the PROSAC algorithm. The point-line projection residual is calculated to determine the inliers and outliers, and the incorrectly matched point-line features are removed. The pose and point-line features of the optimal closed-loop keyframe are used as closed-loop constraints and added to the overall residual function of the back-end nonlinear optimization. The pose of the optimal closed-loop keyframe is optimized through a sliding window, so as to calculate the relative pose relationship between the current keyframe and the optimal closed-loop keyframe v. The residual function formula is expressed as follows:

[0091]

[0092] Among them, I prior 、e imu 、e point and e line They are the marginalized prior residual, IMU residual, point feature residual and line feature residual, e loop is the residual error of the closed-loop term, e ldmk is the landmark residual, i.e., the landmark spatial proximity, is the set of point-line features in the optimal closed-loop keyframe, and are the measured values ​​of point and line features respectively, and are the rotation quaternion and spatial position of the optimal closed-loop keyframe pose, respectively.

[0093] Through the multi-view constraints of the above sliding window, the tightly coupled optimized pose is obtained, so as to calculate the accumulated offset error, and then the pose of the key frame in the sliding window is corrected, and all frames in the sliding window are aligned with the past pose to complete fast repositioning.

[0094] Step 4.2: Global closed-loop optimization:

[0095] When the keyframe matching the optimal closed-loop keyframe slides out of the sliding window, closed-loop optimization is performed on all image frames in the image frame database. Considering that the gravity vector has been optimized in step 2.3, the pitch and roll angles in the rotation quaternion can be observed based on the gravity vector, so only the position q needs to be optimized in the global closed-loop optimization. w The global BA function is used to construct the overall residual objective function of the position and yaw angle of the sequence frame and the optimal closed-loop key frame, and the Ceres library is called to solve the optimization problem. The optimal posture of each global frame is obtained, and the spatial position of its 3D points and 3D lines is optimized accordingly, and finally a complex outdoor 3D map of the substation that can be used for stable navigation is obtained.

[0096] The above contents are further detailed descriptions of the present invention in combination with specific / preferred embodiments, and it cannot be determined that the specific implementation of the present invention is limited to these descriptions. For ordinary technicians in the technical field to which the present invention belongs, they can also make several substitutions or modifications to these described embodiments without departing from the concept of the present invention, and these substitutions or modifications should be regarded as belonging to the protection scope of the present invention.

[0097] Parts of the present invention that are not described in detail belong to the well-known technologies of those skilled in the art.

Claims

1. A substation inspection robot positioning and mapping method based on point-line feature fusion, characterized in that: The following steps are involved: Step 1: Obtain binocular image data, extract point features and line features respectively, and match them; Step 2: Calculate the initial posture of the substation inspection robot in pure vision based on the point-line features; decouple and optimize the posture of the inspection robot itself through the constraints of the point-line features projected on the polar plane to obtain a more robust and high-precision map initialization result; Step 3: Train the closed-loop word dictionary and medium-to-large equipment target detection model of point and line features in the substation scene; during the inspection process of the substation inspection robot, extract the semantic information of the equipment in the scene and the point and line feature descriptors for closed-loop detection, optimize the weighted similarity score of the point and line features, find the key frame with the highest similarity score in the key frame database as the optimal closed-loop key frame, and complete the closed-loop detection; Step 4: When it is detected that the current keyframe has a corresponding optimal closed-loop keyframe, the relative posture relationship between the current keyframe and the optimal closed-loop keyframe is calculated through optimized fast repositioning, and closed-loop correction is performed through the nonlinear global optimization of the back-end to update and optimize the posture of all inspection robots in the current map and the complex outdoor three-dimensional map of the substation.

2. The substation inspection robot positioning and mapping method based on point-line feature fusion according to claim 1 is characterized in that: Step 1: During the inspection process, the substation inspection robot uses a binocular camera to obtain left and right target image frames in real time as input of the visual thread. The visual thread pre-processes the obtained image frames, obtains point features and line features respectively, matches them, and stores the image frames in the image frame database; At the same time, the first image frame is set as the initial key frame, and every 1 second or when the number of point-line feature pairs matched by the new image frame is less than 1 / 4, the new image frame is set as the key frame, and the key frame is stored in the key frame database; the adaptive merging principle is used to optimize the EDLines algorithm for line feature extraction, and the redundant short line features are screened out through the length response threshold strategy based on spatial depth.

3. The substation inspection robot positioning and mapping method based on point-line feature fusion according to claim 2 is characterized in that: The specific operations of the visual thread are as follows: Step 1.1: The left and right image frames acquired by the binocular camera are grayed and histogram equalized. The Shi-Tomasi algorithm is used to obtain the point features of each image frame, and the quadtree is used to achieve feature point homogenization and remove redundant point features. According to the known left and right parallax of the binocular camera, the point features of the left and right image frames are triangulated to obtain spatial point features with absolute scale. The BRIEF descriptor is used to describe the pixel information around the point features, and the Hamming distance between the BRIEF descriptors in the adjacent image frames of the left eye is compared to perform rough similarity matching, and the RANSAC progressive sample consistency algorithm is used to eliminate the wrong matching point feature matching pairs. Step 1.2: For the left-eye image frame acquired by the binocular camera, extract line features through the optimized EDLines algorithm, use the LBD descriptor to describe the local appearance of the line features, and use KNN to match the line features; specifically, the following steps are included: Step 1.2.1: Image preprocessing: grayscale the left-eye image frame collected by the binocular camera, and then perform bilateral filtering and noise reduction on the grayscale image to obtain the preprocessed image; Step 1.2.2: Line feature extraction based on the optimized EDLines algorithm: The image gradient function is obtained by the grayscale value of the preprocessed image. Since the image is equivalent to a two-dimensional discrete function, the grayscale approximation of the image gradient function is calculated based on the difference method using the Sobel discrete difference operator to obtain the gradient value and gradient direction at the pixel point (x, y); The image gradient of the preprocessed image is calculated by obtaining the image gradient function to obtain the gradient map, and the entire gradient map is traversed to screen out the pixel points whose pixel gradient values ​​are greater than the gradient threshold compared with the gradient values ​​of the adjacent pixel points, so as to obtain the image area with more edge details; for the obtained new image area, the pixel points whose gradient values ​​are greater than the anchor point threshold compared with the gradient values ​​of the adjacent pixel points are screened out in the gradient direction as anchor points; for the anchor points, according to the surrounding pixel points of the current anchor point, the pixel points with the same gradient direction as the current anchor point are found as the extended pixel points, if the gradient direction of the next pixel point in the gradient direction of the current anchor point is inconsistent with the gradient direction of the current anchor point, the two adjacent pixel points of the next pixel point are detected, and finally, when all the extension directions are extracted, it means that the current anchor point connection is completed; for the remaining unconnected anchor points, the extension operation is repeated to connect the anchor points until all the anchor points are connected into line edges; For the extracted image line edges, based on the minimum straight line length and the maximum mean square line fitting error, the number of falsely detected line segments NFA is used to fit the image line edges into line segments and retain them in the line segment set; After generating a line segment set containing all the line segments of the image, arrange them in descending order by length, and filter the line segments by angle and spatial distance. First, calculate the angle between the current line segment and the direction vector of the remaining line segments in the line segment set, and filter out line segments with too large angles: Among them, l1 and l2 represent the two line segments to be compared, v1 and v2 represent the direction vectors of l1 and l2, and θ min Indicates the screening threshold between the angles of the comparison line features; Secondly, the line segments with too large horizontal and vertical distances are filtered out respectively. As long as there is a pair of endpoints of the current line segment and another line segment that meet the following conditions, the other line segment is the pre-merged line segment of the current line segment: |x 1i -x 2j |<d min |and 1i -and 2j | <d min i,j∈[1,2] Among them, x 1i and x 2j are the horizontal positions of the two end points of line segment 1 and line segment 2, respectively, 1i and 2j are the vertical positions of the two end points of line segment 1 and line segment 2, respectively. min The filter threshold for comparing the distance between the endpoints of line features After the segment set is traversed and screened by the above method, the pre-merged segment can be merged with the corresponding segment only if it meets the adaptive merging criteria; first, the Euclidean distance between the pre-merged segment and the corresponding segment endpoint pair is less than d min ; Secondly, the angle threshold is adaptively changed according to the length of the pre-merged line segment, and the angle threshold sigmoid function based on the adaptive length is established. The specific calculation method is as follows: Among them, k and a are the curve steepness and translation position of the sigmoid function respectively; n is the normalized length of the pre-merged line segment; When the pre-merged line segment meets the above adaptive merging criteria, based on the horizontal direction of the current line segment, find the two farthest endpoints of the current line segment and the pre-merged line segment, and use them as the endpoints of the merged line segment; if the two farthest endpoints are both endpoints of the current line segment, ignore the pre-merged line segment; after selecting new endpoints, connect the two endpoints to merge the two lines into a new line segment, and replace the original corresponding line segment, and delete the pre-merged line segment from the line segment set; repeat the above operations in sequence according to the length of the line segments in the line segment set until there are no pre-merged line segments that can be merged in the line segment set; Step 1.2.3: Adopt the length response threshold strategy based on the substation spatial depth to filter out redundant short-line features: Using the length response threshold strategy based on spatial depth, the threshold L is established according to the spatial depth of the line feature and the image size. min To filter out redundant short-line features, the formula is as follows: Among them, min(W I ,H I ) represents the smaller value of the width and height of the input image; z is the spatial depth of the line feature, b is the left and right eye baselines of the binocular camera, and η is the ratio factor; Step 1.2.4: Based on the LBD descriptor, use KNN to match line features: According to the screened line features, the support area of ​​each line feature is divided into pixel strips, and the strip descriptor is calculated. The LBD descriptor is constructed by the mean vector and standard deviation vector of the strip descriptor. By calculating the Hamming distance of the LBD descriptors of pairwise line features in adjacent frames, a kd tree is constructed and the optimal line feature pair is obtained through nearest neighbor search.

4. The substation inspection robot positioning and mapping method based on point-line feature fusion according to claim 1 is characterized in that: Step 2: According to the point feature pairs and line feature pairs matched by the left and right eyes of the initial key frame in step 1, the initial posture of the substation inspection robot under pure vision is calculated through the essential matrix; according to the required number of point and line features, continuous key frames are selected from the initial key frame to form a continuous key frame set, based on the first-order Taylor expansion of the direction vectors of the point and line feature pairs on the left and right eye polar planes of adjacent key frames and the pre-integration of the IMU rotation part, the minimum eigenvalue optimization problem of the point and line features on the polar plane is constructed by combining visual and inertial information to obtain the initial gyroscope zero bias, establish the prior and IMU residual functions to obtain the optimal estimation of the key frame velocity, gravity direction and IMU zero bias, and finally decouple and optimize the inspection robot's own posture in turn to obtain a more robust and high-precision map initialization result.

5. The method for positioning and mapping a substation inspection robot based on point-line feature fusion according to claim 4 is characterized in that: Step 2 specifically includes the following steps: Step 2.1: Obtain the initial posture of the substation inspection robot under pure vision: According to the point feature pairs successfully matched in the initial key frame obtained in step 1, the image coordinates of the successfully matched point feature pairs are first converted into normalized coordinates, and 8 pairs of point features are randomly selected to construct a linear equation system to estimate the essential matrix. Then, the singular value matrix obtained by SVD decomposition is used to restore the rotation matrix and translation vector of the camera, that is, the initial posture of the substation inspection robot under pure vision conditions. Step 2.2: IMU preprocessing and gyroscope bias construction: Since the state propagation of the IMU model is in the form of pre-integration, the partial derivative form of the first-order Taylor expansion is used to adjust the pre-integration; the pre-integration of the rotation part between the k frame and the k+1 frame and its first-order partial derivative for: Among them, b k and b k+1 are the kth frame and k+1th frame of the IMU system, respectively, t k and t k+1 are the timestamps of the kth frame and the k+1th frame respectively, Ω(ω) is the angular velocity matrix, is the IMU angular velocity measurement value at the tth moment, ω x ,ω y ,ω z They are the x, y, and z axis components of the IMU angular velocity vector, is the rotation change from time t to the kth frame of the IMU system, and are the gyro bias and the increment of gyro bias in the kth frame of IMU, and b ω is the gyroscope bias, is the Jacobian matrix of the pre-integrated rotation term to the gyroscope bias; For point features in three-dimensional space, connect them to the center of the left and right eyes of the camera in the kth and k+1th frames to obtain the direction vectors and For line features in three-dimensional space, the direction vector f from the camera optical center to the line feature is obtained by cross-producting the normal vector n of the Plücker coordinates and the line direction vector d. l : Where n is the normal vector of the polar plane formed by the line feature and the camera optical center, and d is the direction vector of the line feature; Connect the line features to the center points of the left and right eyes of the camera in the kth and k+1th frames to obtain the direction vectors and The two vectors form different polar planes with the baselines of adjacent frames; through the rotation matrix between the IMU coordinate system and the camera coordinate system After converting the IMU coordinate system where the gyroscope zero bias is located into the left and right eye coordinate systems of the camera, a coordinate system based on the direction vector and The gyroscope zero bias b is optimized by minimizing the eigenvalue model ω , the formula is as follows: in, It is a set of continuous key frames in the initialization phase with IMU measurement information. The continuous key frame set is composed of continuous key frames selected from the initial key frame according to the number of point and line features required. L M k,k+l and R M k,k+l is a positive semidefinite matrix, λ min is the minimum eigenvalue of the semi-positive definite matrix, which is the product of the matrix formed by the polar plane constraint relationship of the left and right cameras and its transposed matrix, N L and N R are the total number of point features matched by the left and right eyes respectively, M L and M R are the total number of line features matched by the left and right eyes respectively, and are the left and right eye centers of the kth frame and the i-th point feature p i The direction vector between and are the left and right eye centers of the kth frame and the jth line feature l i The direction vector between and They are the left camera coordinate system c L and the right camera coordinate system c R The transformation matrix between the IMU coordinate system b; Step 2.3: IMU state variable optimization: The preliminary gyroscope bias estimate is obtained through step 2.2, and the optimal estimate of the IMU state variables is further obtained. That is, the key frame velocity, gravity vector, gyroscope bias and accelerometer bias b a , the formula is as follows: in, is the speed of each of the N+1 key frames in the world coordinate system, is the gravity vector in the camera coordinate system; Through IMU pre-integration and The residual function is constructed based on the state variables in order to optimize the state variables. The formula is as follows: Among them, r p is the IMU bias prior residual, is the residual of the change in IMU measurement information between the kth frame and the k+1th frame, Σ p is the covariance of the IMU zero-bias prior, is the covariance of the change in IMU measurement information between the kth frame and the k+1th frame; Step 2.4: Gradually optimize the posture of the substation inspection robot through decoupling: While obtaining the optimal IMU state variables in step 2.3, the residual of the pre-integrated rotation part is Get the rotation matrix between the optimized world coordinate system and the kth frame of the current IMU system Then update the rotation matrix of each image key frame in the left and right eyes: in, and are the rotation matrices of the left camera and the right camera from the kth frame to the world coordinate system, and They are the fixed rotation matrices from the left camera and the right camera to the IMU coordinate system; For each point and line feature extracted from the key frame of the image, the least squares method is used to construct an error model such as point and line reprojection to optimize the translation vector The formula is as follows: Among them, X i is the i-th 3D point feature in the world coordinate system, x i Yes X i The two-dimensional point features projected on the left-eye k-th frame image plane, m j are the homogeneous coordinates of the midpoint of the j-th 2D line feature, It is the projected two-dimensional line feature obtained by transforming the j-th three-dimensional line feature in the camera coordinate system to the k-th image plane of the left eye. and is the coefficient of the projected two-dimensional line feature, n x and n l are the number of point features and line features respectively, π is the projection function, ρ is the Huber kernel function that only processes visual outliers, and Σ is the covariance of the residual function.

6. The substation inspection robot positioning and mapping method based on point-line feature fusion according to claim 1 is characterized in that: Step 3: Step 3.1: Offline training of point and line feature dictionary in substation scenario: To obtain a sufficient number of substation image datasets, it is necessary to cover a collection of images taken under different lighting conditions, different substation scenes, different weather conditions, and different seasons. First, traverse all the above training images, extract Shi-Tomasi point features and EDLines line features for each image, and use BRIEF descriptor and 256-dimensional binary LBD descriptor to quantify feature information respectively. Use the third-party library DBoW2 library of the bag of words model to establish a dictionary tree of point features and line features, uniformly set the number of branches of the dictionary tree K = 10 and the depth L = 5, and use K-means to cluster the extracted point features and line features into 10 sets, that is, cluster all feature samples into 10 categories, cluster them down 4 times in turn, and generate a point and line feature dictionary offline. Finally, the nodes in the last layer of the dictionary are called words, and TF-IDF is used to assign a certain weight to each word according to its frequency in the training set. Step 3.2: Extracting semantic information of substation equipment: Select medium and large equipment images from the substation image dataset as the substation equipment dataset, and perform data annotation; Considering the real-time requirements of the SLAM system, select the lightweight YOLOv5s model as the medium and large equipment target detection model, and only perform target detection on key frames to meet the real-time performance requirements of SLAM; In the large-scale substation scene, in order to rationally utilize limited visual information processing resources, select the attention module CA to insert into the backbone network convolution layer of the lightweight YOLOv5s model. This module introduces coordinate information to obtain the precise positioning information of the substation equipment and enhances the target position information capture capability of the medium and large equipment target detection model; After training the substation equipment dataset with the medium and large equipment target detection model, the medium and large equipment target detection model is built into the industrial computer of the inspection robot, so as to obtain the semantic information of the substation equipment in real time; Due to the possibility of false detection, only when the medium and large equipment target detection model detects the spatial position information of the device in three consecutive key frames, it will be mapped as a landmark to the map with point and line features; Step 3.3: Get closed-loop frames through closed-loop detection: After the map initialization is completed, the closed-loop detection thread is started to perform closed-loop detection on the current key frame obtained in real time during the inspection process; if the current key frame has a substation equipment landmark, the most recently mapped landmark is identified and compared with the landmark in the key frame in the key frame database, and the spatial proximity between them is calculated to determine whether the key frame in the key frame database is a closed-loop candidate key frame: Among them I cur is the two-dimensional projection position of the landmark of the current key frame, is the landmark set in the keyframe in the keyframe database, Landmark collection The 3D position of the i-th landmark in , ||·||2 is the 2-norm; At the same time, based on the point features and line features in the current key frame obtained by the visual thread, the point and line feature dictionary obtained in step 3.1 is used to compare the descriptors of the point and line features online with the cluster center of each intermediate node of the tree structure in the dictionary, so as to search for the word position of the descriptor, and compare the key frames in the key frame database, find the key frames whose common words between the two frames exceed 80% of the words in the current key frame, and store them in the closed-loop candidate key frame group, and convert the point and line features of the current key frame into bag-of-words vectors of point features and line features respectively; by comparing the bag-of-words vectors between the current key frame and all closed-loop candidate key frames in the closed-loop candidate key frame group, the image similarity score based on point features and line features is obtained; in the similarity score, the two criteria of feature scale and distribution degree are adopted to weight the similarity scores of the two features, and the normalized feature scale I is calculated. p ,I l and distribution degree D p , D l , the formula is as follows: Among them, N p 、N l and N max are the number of point features, line features and all features, D p , D l and D max They are the distribution values ​​of point features, line features and all features respectively; Based on the distance weighting, dispersion is added to the denominator of the similarity score as a penalty term to reduce the impact of high dispersion on the score. The formula is as follows: where w p and w l is the weight coefficient of point features and line features; According to the weighted similarity score, find the closed-loop candidate keyframe with the highest similarity score with the current keyframe as the closed-loop keyframe, but the closed-loop keyframe needs to undergo a temporal consistency check: the closed-loop keyframe cannot be a keyframe in the sliding window where the current keyframe is located; and a spatial coherence check is also required: the similarity score between the closed-loop keyframe and all other keyframes in the current sliding window needs to be greater than the set similarity threshold, and the similarity threshold is set to 0.75 times the similarity score between the closed-loop keyframe and the current keyframe; only when the above two conditions are met can the closed-loop keyframe be determined as the optimal closed-loop keyframe.

7. The substation inspection robot positioning and mapping method based on point-line feature fusion according to claim 6 is characterized in that: Step 4: Step 4.1: Calculate the relative pose relationship between the current frame and the closed-loop frame using the optimized fast relocalization method: When the optimal closed-loop keyframe is detected, the point-line features of the optimal closed-loop keyframe and the current keyframe are firstly roughly matched based on the descriptor, and the homography matrix of the high-quality point-line pairs is calculated by the PROSAC algorithm. The point-line projection residual is calculated to determine the inliers and outliers, and the incorrectly matched point-line features are eliminated; the pose and point-line features of the optimal closed-loop keyframe are used as closed-loop constraints and added to the overall residual function of the back-end nonlinear optimization. The pose of the optimal closed-loop keyframe is optimized by sliding the window, so as to calculate the relative pose relationship between the current keyframe and the optimal closed-loop keyframe v. The residual function formula is expressed as follows: Among them, I prior 、e imu 、e point and e line They are the marginalized prior residual, IMU residual, point feature residual and line feature residual, e loop is the residual error of the closed-loop term, e ldmk is the landmark residual, i.e., the landmark spatial proximity, is the set of point-line features in the optimal closed-loop keyframe, and are the measured values ​​of point and line features respectively, and are the rotation quaternion and spatial position of the optimal closed-loop keyframe pose, respectively; Through the multi-view constraints of the sliding window above, the tightly coupled optimized pose is obtained, so as to calculate the accumulated offset error, and then correct the pose of the key frame in the sliding window, align all frames in the sliding window with the past pose, and complete fast repositioning; Step 4.2: Global closed-loop optimization: When the key frame matching the optimal closed-loop key frame slides out of the sliding window, closed-loop optimization is performed on all image frames in the image frame database; in global closed-loop optimization, only the position q w and yaw angle ψ; the overall residual objective function of the position and yaw angle of the sequence frame and the optimal closed-loop key frame is constructed through the global BA function, and the Ceres library is called to solve the optimization problem, and the optimal posture of each global frame is obtained, and the spatial position of its 3D points and 3D lines is optimized accordingly, and finally a complex outdoor 3D map of the substation that can be used for stable navigation is obtained.

Images