Multi-source Fusion Localization Method for Underground Pipeline Robots Based on Factor Graph Weight Adaptation

Abstract

A multi-source fusion positioning method for underground pipeline network robots based on factor graph weight adaptation, which is a combined positioning method for inertial navigation positioning, extremely low frequency positioning, radio frequency positioning, and multi-source sensors of underground pipeline network maps. First, according to the INS / UWB / ELF measurement and error models, INS / UWB / ELF factor nodes and their cost functions are constructed. Through the measurement errors at each moment, the weights of each sensor in the factor graph are dynamically updated and adjusted, and a sliding window constraint is applied to reduce the dimension of the navigation prior information. According to the optimal estimation sequence of the preliminary fusion, a hidden Markov model is established and solved using the Viterbi algorithm for map matching to obtain the multi-source fusion positioning result, realizing high-precision positioning of the underground pipeline network robot underground.

Description

Technical Field

[0001] The present invention belongs to the field of underground robot positioning, and more specifically, it is a multi-source fusion positioning method for underground pipe network robots based on factor graph weight adaptation. Background Art

[0002] In recent years, as the urban above-ground space has gradually become saturated, the expansion of urban underground space has become a trend. Urban underground pipe corridors are the most important infrastructure to ensure the normal operation of urban life.

[0003] At present, the operation and management of the urban underground pipe corridor system mostly still remain in the manual stage, with low automation and poor adaptability of the system. Therefore, underground pipe corridor inspection robots can effectively avoid the defects of manual inspection. However, restricted by factors such as the underground environment, geographical space, drainage conditions, and weak sensing signals, conventional navigation methods are not applicable, and the autonomous positioning and navigation decision-making of robots in the underground environment have become an urgent problem to be solved.

[0004] At present, the following main solutions exist for the positioning technology of underground pipe corridor robots:

[0005] 1. Positioning technology based on extremely low frequency (ELF) electromagnetic waves. Aiming at the shielding problem of conventional electromagnetic signals by metal pipe walls and rock and soil environments, it has strong penetration and good perception of the underground environment. The transmission power required for extremely low frequency signals is relatively high, and the magnetic field signal decays rapidly, with a small detection range.

[0006] 2. Positioning technology based on ultra-wideband (UWB).

[0007] In the areas where the underground pipe corridor is connected to the ground surface and where UWB positioning modules can be installed, there are positioning methods based on fingerprints with RSS and CSI characteristics. The positioning accuracy of fingerprint positioning based on RSS is greatly affected by non-line-of-sight, and the fingerprint positioning with CSI as the characteristic has better positioning accuracy in non-line-of-sight situations.

[0008] 3. Positioning technology based on inertial navigation.

[0009] First of all, the characteristics of high output frequency and immunity to environmental interference of MEMS-IMU have the technical advantage of quickly capturing auxiliary positioning signals. Due to the long initialization time of conventional absolute positioning during the operation of the robot and the problem that high-frequency output of positioning results cannot be achieved.

[0010] Secondly, the accuracy and reliability of the robot attitude measurement system mainly depend on MEMS-IMU devices. Inertial devices are vulnerable to factors such as temperature and vibration during the operation of the robot, which can change the calibration parameters, resulting in a decrease in the accuracy of inertial devices; when each sensor assisting the MEMS-IMU in attitude measurement provides observation data, due to the changing operating environment of the robot, there are problems of failure or quality degradation. Therefore, how to make full use of inertial navigation information to improve the real-time performance, continuity, and high-frequency output of the absolute positioning and attitude measurement system urgently needs to be solved.

[0011] 4. Positioning technology based on pipeline network maps.

[0012] Map matching positioning determines the position of the robot based on a known environmental map. First, an observation sensor is used to observe the current environment and generate a local map, and then the local map is feature-matched with the known global environmental map. Finally, the position and orientation of the robot in the known environmental map are determined according to the matching results. The pipeline network map can be used for map matching or map constraint to improve the positioning accuracy.

[0013] The current existing technologies are as follows:

[0014] Publication number: CN112577496A, Title: "A Multi-source Fusion Positioning Method Based on Adaptive Weight Selection". The method steps are as follows: obtain initial nodes according to the initial data of the sensor; generate initial position variable nodes and initial error variable nodes from the initial nodes; the inertial navigation factor expands the position variable nodes, and the error factor expands the error variable nodes, and the error variable nodes correct the inertial navigation factor; the output results of the satellite navigation factor and the visual odometer factor are data-fused with the positioning results in the position variable nodes according to different weights. The present invention applies an adaptive weight selection factor graph model to multi-source fusion navigation positioning, realizes the process of adaptive weight selection for sensor factors in a relatively simple way, can troubleshoot and reconnect satellite positioning information by dynamically adjusting the weights, and reduces the positioning error to a certain extent.

[0015] The solution it adopts is to construct a weight function based on the position difference between the positioning result in the position variable node of the factor graph and the positioning result of the satellite navigation factor to complete the method of adaptive weight selection. The solution we adopt is to first calculate using a dynamic weight function based on the residual between the sensor measurement value and the predicted value, and then process the weights of each sensor at each moment using a clustering algorithm. The clustering center point is the weight, which can effectively improve the positioning accuracy.

[0016] Publication No.: CN111337020A, Title: "Factor Graph Fusion Positioning Method Incorporating Robust Estimation", which includes: obtaining sensor measurement information; determining a multi-dimensional state quantity composed of position, velocity, attitude, and deviation quantity based on the measurement information, constructing a state space model, and thus establishing a combined navigation system model; constructing a test statistic according to the prediction residual vector of the navigation system model to determine whether there are abnormal observations; if so, introducing an exponentially decaying adaptive factor to automatically adjust the observation noise; if not, constructing a factor graph model; defining the measurement information as the factor nodes of the factor graph, defining the state quantity information of the navigation system model as the variable nodes of the factor graph, constructing a system framework diagram for multi-source information fusion based on the factor graph, and thus constructing a factor graph model; inferring the factor graph model, and obtaining positioning information according to the inference result. To achieve the advantage of improving positioning accuracy.

[0017] The solution it adopts is to construct a test statistic according to the prediction residual vector of the navigation system model to determine whether there are abnormal observations. If so, introducing an exponentially decaying adaptive factor to automatically adjust the observation noise; if not, constructing a factor graph model. Abnormal observations are judged by the confidence level of the statistic. In our patent, a factor graph model is directly constructed, and the processing of abnormal observations is realized by according to the dynamic weight function and clustering algorithm, isolating the sensor factor at that moment, and using other sensor factors for fusion, with less error in the complex environment where underground sensor signals are vulnerable to interference.

[0018] Publication No.: CN111780755A, Title: "A Multi-Source Fusion Navigation Method Based on Factor Graph and Observability Analysis", the method includes the following steps: constructing a multi-source fusion navigation system based on an inertial navigation / auxiliary sensor combined navigation model, obtaining a combined navigation robust Kalman sub-filter with inertial navigation as the core and two or more of satellite, vision, and odometer as auxiliary sensors; based on the navigation solution results of each combined navigation robust Kalman sub-filter, measuring the observability of the state variables of each combined navigation robust Kalman sub-filter; adopting an incremental factor graph architecture, online selecting the best factors to participate in the fusion according to the credibility evaluation of multi-source combined navigation factors, and automatically adjusting the weights of information distribution, so as to realize cross-scene multi-source fusion navigation. The present invention can realize the adaptive fusion of multi-sensors and safe and reliable navigation positioning, and improve the accuracy and reliability of the fusion of multi-source navigation information such as inertial / satellite / vision.

[0019] The adopted solution is to construct Kalman sub - filters for integrated navigation such as inertial / satellite and inertial / vision. By designing an adaptive robust Kalman filtering algorithm, the filtering algorithm is optimized and the influence of uncertain factors is reduced. Then, based on the observability analysis of the dynamic time - varying system, the credibility of integrated navigation factors such as inertial / satellite and inertial / vision is measured online. However, in our patent, Kalman filtering is not used. By determining weights according to the dynamic weight function and clustering algorithm, an ELF\UWB\INS\map factor graph model is constructed, and the complexity of the system is lower.

[0020] In existing factor graph algorithms, multi - source information fusion mostly relies on initial weight assignment. However, in the underground environment, each sub - navigation system is affected by the environment and its own accuracy, resulting in abnormal observation values. The factor graph performs multi - source fusion on the abnormal values, leading to a decrease in the accuracy of integrated navigation. The present invention proposes a multi - source fusion positioning method for underground pipeline network robots based on factor graph weight adaption. The weight of each sensor is adjusted in real - time according to the dynamic weight function, and sensors with abnormal observation values are down - weighted or even isolated. The effective fusion of integrated navigation information is achieved, the robustness of the factor graph algorithm is enhanced, and thus the navigation accuracy is improved.

[0021] Aiming at the problem of autonomous and precise navigation of urban underground pipeline network robots, this patent proposes a multi - source fusion positioning method for underground pipeline network robots based on factor graph weight adaption, which can effectively solve the problem of poor positioning accuracy in the underground environment. Summary of the Invention

[0022] To solve the above - mentioned technical problems, the present invention proposes a multi - source fusion positioning method for underground pipeline network robots based on factor graph weight adaption, which can effectively solve the problem of poor positioning accuracy in the underground environment. It integrates a combined positioning method of inertial navigation positioning, extremely low frequency positioning, radio frequency positioning, and underground pipeline network maps, and can achieve high - precision positioning of underground pipeline network robots.

[0023] To achieve the above object, the technical solution adopted by the present invention is:

[0024] The present invention provides a multi - source fusion positioning method for underground pipeline network robots based on factor graph weight adaption, which is characterized by including the following steps:

[0025] S1: System initialization, set the initial parameters of the INS, UWB, ELF, and map navigation subsystems, determine the sliding window size s, establish the INS\UWB\ELF measurement and error models, construct the INS\UWB\ELF factor nodes and their cost functions, and establish a multi - source fusion positioning model based on factor graph weight adaption;

[0026] S2: When new measurement information is input, determine whether it meets the sliding window range: if it exceeds the window range, slide the window forward or generate a new window, and use the state quantity and observation quantity distribution estimated in the previous window as the prior estimate of the current window; if it meets the sliding window size, directly proceed to S3;

[0027] S3: Add corresponding factors, dynamically adjust the weights of corresponding factors according to the measurement information results of different sensors, isolate the invalid factors, perform incremental update processing, further update and fuse the navigation information, and iterate to obtain the optimal estimate;

[0028] S4: Output the state estimation value of each state node in the sliding window, use the final estimation value of the covariance matrix and mean vector of each system as the initial value of each parameter in the next sliding window, and move the sliding window backward k nodes;

[0029] S5: Determine whether there is new measurement information input, if yes, repeat step S2, otherwise end the update;

[0030] S6: Based on the optimal estimation value sequence obtained above, a hidden Markov model is established to match the map data according to the observation cost and the conversion cost. When an optimal estimation value sequence is matched using the HMM, the Viterbi algorithm is used to backtrack the matching process, calculate the matching results, and obtain the multi-source fusion positioning results.

[0031] As a further improvement of the present invention, in step S1, the INS factor node and its cost function:

[0032] According to the NE carrier coordinate system, a multi-source fusion positioning model based on factor graph is established. In the factor graph, squares represent factor nodes and circles represent variable nodes.

[0033] Let the state variable at time k be X k

[0034]

[0035] Where: P = [xyz] T is the carrier position vector; v = [v x v y v z ] T is the carrier velocity vector; q 0 =[q 1 q 2 q 3 q 4 ] is the carrier attitude quaternion vector; is the random walk term of the accelerometer; is the gyroscope random walk term;

[0036] Its cost function is:

[0037]

[0038] x k ,x k+1 are the state variables at times k and k + 1, is the INS deviation measurement noise covariance, and h(x k ,x k+1 ) is the INS measurement result.

[0039] As a further improvement of the present invention, in step S1, the UWB factor node and its cost function are specifically:

[0040] Establish a UWB measurement model and construct a UWB factor node and its cost function;

[0041] Use the channel state information CSI as a feature to establish a UWB fingerprint database and construct a UWB factor node;

[0042] This positioning method is divided into two stages: an offline stage and an online stage. In the offline stage, first collect the CSI data amplitude to establish the CSI original fingerprint database, and then use the sparse autoencoder SAE to compress the input CSI data to the feature layer, and the output feature layer is further connected to the 1D-CNN model for training and testing;

[0043] CSI = [|CSI 1,1,1 |, |CSI 1,1,2 |,... |CSI 1,1,q |,... |CSI 1,2,1 |,... |CSI m,n,q |]

[0044] where m is the number of transmitter antennas, n is the number of receiver antennas, and q is the number of channels, i.e., the number of subcarriers;

[0045] In the online stage, the underground robot decodes by matching the received CSI measurement value, obtains the positioning result according to the regression model, inputs the measured coordinates of UWB into the factor graph, and constructs a UWB factor node;

[0046] The input layer of the SAE has m * n * q neurons, and the two hidden layers have l and q neurons respectively. The three-layer one-dimensional convolutional neural network 1D-CNN includes a convolutional layer, a batch normalization layer, an activation layer, a pooling layer, a fully connected FC layer, and a softmax output layer, and the activation function is the ReLU function;

[0047] Its cost function is:

[0048]

[0049] where N is the total number of verification samples, y(x) is the expected output, j represents the number of layers in the network, and h j (x) is the activation vector output by the network when x is the input.

[0050] As a further improvement of the present invention, in the step S1, the ELF factor node and its cost function:

[0051] Establish an ELF measurement model, construct an INS factor node and its cost function, convert the magnetic signal into voltage through a sensor array, and then perform non-linear calculation through a particle swarm quasi-Newton hybrid algorithm;

[0052] Its cost function is:

[0053]

[0054] where min is the minimum value function, u i is the voltage observation value, and u i is the voltage state variable.

[0055] As a further improvement of the present invention, in the step S3, the ELF factor node and its cost function:

[0056] The multi-source fusion navigation algorithm fusion framework based on the factor graph is based on the INS factor. As time updates, the INS is pre-integrated and optimized with a sliding window. When the navigation system receives the measurement information of the ELF and UWB sensors, the corresponding sensor factor nodes are added to the factor graph fusion framework of the navigation system, and the relevant variable nodes are updated according to their corresponding sensor measurement equations and their corresponding cost functions;

[0057] Its cost function is:

[0058]

[0059] where i represents the factors of various navigation systems at this moment, argmin represents the variable value when the objective function takes the minimum value, K(d i ) is the dynamic weight function, and h i (X i ) is the measurement function of different navigation systems, and z i is the actual measurement value obtained by various navigation systems.

[0060] As a further improvement of the present invention, in the step S3, the steps for adaptively adjusting the dynamic weights of the factor graph based on the residuals are as follows:

[0061]

[0062] where K(di ) is the dynamic weight function, Δd 2 represents the square of the residual between the measured value and the predicted value of the sensor, called the credible distance, and the parameter θ is the coefficient;

[0063] Cluster the weights K(d i ) of each sensor at each moment using the HDBSCAN algorithm. The center point of the cluster is the weight. Each newly added discrete point that is not clustered determines that the sensor factor node fails at this moment, and directly isolates this factor without using this factor for fusion;

[0064] Normalize the weights K(d i ) of all factors, and then perform subsequent optimal estimation.

[0065] As a further improvement of the present invention, the isolation of the failed factor in step S3 is as follows:

[0066] Perform real-time determination on the sensor factor node. If the newly added point is not clustered, isolate the determined failed sensor factor; for the INS factor, if it is not clustered for consecutive t moments, it is determined that the cumulative error of the INS is large and the INS positioning fails, and the INS factor is isolated.

[0067] As a further improvement of the present invention, the establishment of the hidden Markov model in step S6 is specifically as follows;

[0068] For each calculated optimal estimation point, first determine a set of candidate road segments. If it is found that the calculated optimal estimation point is very close to a certain road segment, assign a lower cost value to this road segment. Then calculate the weights for the edges connecting each pair of adjacent vertices in the Markov chain. Finally, find the maximum likelihood path on the Markov chain;

[0069] C Match = C O + C T

[0070] Among them, C Match represents the total cost of map matching, C O represents the observation cost of map matching, and C T represents the transformation cost of map matching.

[0071] As a further improvement of the present invention, the observation cost in step S6 is as follows:

[0072] (1) Calculate the distance from the optimal estimation point to the projection point of the matching road segment. The greater the distance, the greater the cost;

[0073] (2) Calculate the direction difference between the forward direction of the optimal estimation point and the direction of the road segment. The greater the direction difference, the greater the cost;

[0074] (3) The values of the weighted average distance cost and the direction cost are used as the final observation cost;

[0075] (4) If the accuracy of the optimal estimated value is low, the distance and direction costs are correspondingly reduced;

[0076] (5) When the speed of the optimal estimated value is low, the direction cost needs to be reduced.

[0077] As a further improvement of the present invention, the conversion cost in step S6 is as follows:

[0078] The conversion cost is the cost from the point corresponding to a certain section in the previous point to the point corresponding to a certain section in the next point during the process from the observation point of the previous optimal estimated value to the next observation point. The closer the section distance is to the distance between the two observation points, the smaller the cost.

[0079] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0080] 1. By fusing the extremely low frequency (ELF) signal positioning, inertial navigation system (INS) positioning, ultra-wideband (UWB) positioning, and the prior information of the underground pipeline network map with a factor graph, the present invention effectively improves the positioning accuracy in the underground environment with strong signal shielding.

[0081] 2. The present invention introduces a factor graph weight adaptive function based on residuals, and through the judgment of its credibility, realizes the plug-and-play of each factor and the adaptive adjustment of weights.

[0082] 3. The present invention improves the UWB fingerprint positioning network based on CSI, and through the training and recognition of the SAE network and the 1D-CNN network, improves the positioning accuracy in the case of UWB non-line-of-sight. Description of the Drawings

[0083] Figure 1 Flowchart of the multi-source fusion positioning method for the underground pipeline network robot;

[0084] Figure 2 Principle diagram of the factor graph improved by dynamic weight adaptive adjustment based on residuals;

[0085] Figure 3 UWB fingerprint positioning network based on CSI. Detailed Embodiment

[0086] The following further describes the present invention in detail in conjunction with the drawings and the specific embodiments:

[0087] The overall process of the present invention is as Figure 1 shown, the principle diagram of the factor graph improved by dynamic weight adaptive adjustment based on residuals is as Figure 2 shown, and the UWB fingerprint positioning network based on CSI is asFigure 3 As shown in the figure, the present invention provides a multi-source fusion positioning method for an underground pipeline network robot based on factor graph weight adaption, which is characterized by the following steps:

[0088] The present invention designs a multi-source fusion positioning method for an underground pipeline network robot based on factor graph weight adaption, which combines inertial navigation positioning, extremely low frequency positioning, radio frequency positioning, and combined positioning methods of underground pipeline network maps, and can achieve high-precision positioning of underground pipeline network robots.

[0089] S1: System initialization, set the initial parameters of the INS, UWB, ELF, and map navigation subsystems, determine the sliding window size s, establish the measurement and error models of INS / UWB / ELF, construct the factor nodes of INS / UWB / ELF and their cost functions, and establish a multi-source fusion positioning model based on factor graph weight adaption;

[0090] S2: When new measurement information is input, determine whether it meets the sliding window range: when it exceeds the window range, slide the window forward or generate a new window, and use the state quantity and observation distribution estimated in the previous window as the prior estimate of the current window; when it meets the sliding window size, directly proceed to S3;

[0091] S3: Add corresponding factors, dynamically adjust the weights of the corresponding factors according to the measurement information results of different sensors, perform incremental update processing, further update and fuse the navigation information, and iteratively obtain the optimal estimated value;

[0092] S4: Output the state estimated values of each state node within the sliding window, use the final estimated values of the covariance matrix and mean vector of each system as the initial values of each parameter within the next sliding window, and move the sliding window backward by k nodes;

[0093] S5: Determine whether new measurement information is input. If so, repeat step S2; otherwise, end the update;

[0094] S6: According to the optimal estimated value sequence obtained above, establish a hidden Markov model, and use the Viterbi algorithm to perform map matching calculation to obtain the multi-source fusion positioning result.

[0095] According to the multi-source fusion positioning method for an underground pipeline network robot based on factor graph weight adaption described in claim 1, it is characterized in that: in the step S1, the INS / UWB / ELF factor nodes and their cost functions:

[0096] According to the north-east-down carrier coordinate system, establish a multi-source fusion positioning model based on factor graph. In the factor graph, squares represent factor nodes and circles represent variable nodes;

[0097] Let the state variable at time k be X k

[0098]

[0099] Where: P = [x y z] T is the carrier position vector; v = [v x v y v z T is the carrier velocity vector; q 0 = [q 1 q 2 q 3 q 4 is the carrier attitude quaternion vector; is the accelerometer random walk term; is the gyroscope random walk term;

[0100] Its cost function is:

[0101]

[0102] is the INS deviation measurement noise covariance, h(x k , x k+1 ) is the INS measurement result;

[0103] Establish a UWB measurement model, and construct a UWB factor node and its cost function;

[0104] Use the channel state information CSI as a feature to establish a UWB fingerprint database and construct a UWB factor node;

[0105] This positioning method is divided into two stages: the offline stage and the online stage. In the offline stage, first collect the CSI data amplitude to establish the CSI original fingerprint database, and then use the sparse autoencoder SAE to compress the input CSI data to the feature layer, and the output feature layer is further connected to the 1D-CNN model for training and testing;

[0106] CSI = [|CSI 1,1,1 |, |CSI 1,1,2 |,... |CSI 1,1,q |,... |CSI 1,2,1 , |... |CSI m,n,q |]

[0107] Where m is the number of transmitter antennas, n is the number of receiver antennas, and q is the number of channels, i.e., the number of subcarriers;

[0108] In the online stage, the underground robot decodes by matching the received CSI measurement value, obtains the positioning result according to the regression model, inputs the measured coordinates of UWB into the factor graph, and constructs a UWB factor node;​

[0109] The SAE input layer has m*n*q neurons, and the two hidden layers have l and q neurons respectively. The three-layer one-dimensional convolutional neural network 1D-CNN includes a convolutional layer, a batch normalization layer, an activation layer, a pooling layer, a fully connected FC layer, and a softmax output layer. The activation function is the ReLU function;

[0110] Its cost function is:

[0111]

[0112] where N is the total number of validation samples, y(x) is the expected output, j represents the layer number in the network, and h j (x) is the activation vector output by the network when x is the input;

[0113] An ELF measurement model is established, the INS factor nodes and their cost functions are constructed, the magnetic signal is converted into voltage through a sensor array, and then non-linear solution is carried out through a particle swarm quasi-Newton hybrid algorithm;

[0114] Its cost function is:

[0115]

[0116] According to the method for multi-source fusion positioning of an underground pipe network robot based on factor graph weight adaption described in claim 1, it is characterized in that: in step S3, the principle of the factor graph is as follows:

[0117] The multi-source fusion navigation algorithm fusion framework based on the factor graph is based on the INS factor. As time updates, the INS is pre-integrated and optimized with a sliding window. When the navigation system receives the measurement information of the ELF and UWB sensors, the corresponding sensor factor nodes are added to the factor graph fusion framework of the navigation system, and the relevant variable nodes are updated according to their corresponding sensor measurement equations and their corresponding cost functions;

[0118] Its cost function is:

[0119]

[0120] where h i (X i ) is the measurement function of different navigation systems, and z i is the actual measurement value obtained by various navigation systems.

[0121] According to the method for multi-source fusion positioning of an underground pipe network robot based on factor graph weight adaption described in claim 1, it is characterized in that: in step S3, the steps for adaptively adjusting the dynamic weight of the factor graph based on residuals are as follows:

[0122]

[0123] Among them, K(d i ) is the dynamic weight function, and Δd 2 represents the square of the residual between the measured value and the predicted value of the sensor, which is called the confidence distance, and the parameter θ is a coefficient;

[0124] Cluster the weights K(d i ) of each sensor at each moment using the HDBSCAN algorithm. The clustering center point is the weight. For each newly added discrete point that has not been clustered, it is determined that the sensor factor node fails at this moment, and this factor is directly isolated and not used for fusion. For the INS factor, if it has not been clustered for t consecutive moments, it is determined that the cumulative error of the INS is large and the INS positioning fails, and the INS factor is isolated;

[0125] Normalize the weights K(d i ) of all factors, and then perform subsequent optimal estimation.

[0126] According to the multi-source fusion positioning method for an underground pipeline network robot with adaptive factor graph weights described in claim 1, it is characterized in that: in step S6, according to the obtained optimal estimation value sequence, a hidden Markov model is established, and the map data is matched according to the observation cost and the transition cost. After a sequence of optimal estimation values is matched using HMM, the Viterbi algorithm is used to backtrack the matching process, calculate the matching result, and obtain the multi-source fusion positioning result;

[0127] For each calculated optimal estimation value point, first determine a set of candidate road segments. If it is found that the calculated optimal estimation value point is very close to a certain road segment, assign a lower cost value to this road segment. Then calculate the weights for the edges connecting each pair of adjacent vertices in the Markov chain. Finally, find the maximum likelihood path on the Markov chain.

[0128] The observation cost is as follows:

[0129] (1) Calculate the distance from the optimal estimation value point to the projection point of the matching road segment. The greater the distance, the greater the cost;

[0130] (2) Calculate the direction difference between the forward direction of the optimal estimation value point and the direction of the road segment. The greater the direction difference, the greater the cost;

[0131] (3) The weighted average of the distance cost and the direction cost values is used as the final observation cost;

[0132] (4) If the accuracy of the optimal estimation value is low, the distance and direction costs are reduced accordingly;

[0133] (5) When the speed of the optimal estimation value is low, the direction cost needs to be reduced;

[0134] The conversion cost refers to the cost for the hidden sequence, i.e., the points on the matched road section, from a certain road section corresponding to the previous point to a certain road section corresponding to the next point during the process from the previous optimal estimated value observation point to the next observation point. The closer the road section distance is to the distance between the two observation points, the smaller the cost.

[0135] C Match = C O + C T

[0136] The above are only the preferred embodiments of the present invention, and do not impose any other form of limitation on the present invention. Any modification or equivalent change made according to the technical essence of the present invention still falls within the scope claimed by the present invention.

Claims

1. Factor Graph Weight Adaptive Multi-Source Fusion Localization Method for Underground Pipeline Robots Characterized in that It includes the following steps: S1: System initialization, set the initial parameters of INS, UWB, ELF, and map navigation subsystems, determine the sliding window size s, establish INS\UWB\ELF measurement and error models, construct INS\UWB\ELF factor nodes and their cost functions, and establish a multi-source fusion localization model based on factor graph weight adaptation; S2: When new measurement information is input, determine whether it meets the sliding window range: when it exceeds the window range, slide the window forward or generate a new window, and use the state quantity and observation distribution estimated within the previous window as the prior estimate of the current window; when it meets the sliding window size, directly proceed to S3; S3: Add corresponding factors, dynamically adjust the weights of the corresponding factors according to the measurement information results of different sensors, isolate the invalid factors, perform incremental update processing, further update and fuse the navigation information, and iteratively obtain the optimal estimated value; S4: Output the state estimated values of each state node within the sliding window, use the final estimated values of the covariance matrix and mean vector of each system as the initial values of each parameter within the next sliding window, and move the sliding window backward by k nodes; S5: Determine whether there is new measurement information input. If so, repeat step S2; otherwise, end the update; S6: According to the optimal estimated value sequence obtained above, establish a hidden Markov model, match the map data according to the observation cost and transition cost. After a sequence of optimal estimated values is matched using HMM, use the Viterbi algorithm to backtrack the matching process, calculate the matching result, and obtain the multi-source fusion localization result.

2. The factor graph weight adaptive multi-source fusion localization method for underground pipeline robots according to claim 1 Characterized in that: In step S1, the INS factor node and its cost function: Based on the north-east-down vehicle coordinate system, establish a multi-source fusion localization model based on factor graph. In the factor graph, squares represent factor nodes and circles represent variable nodes; Let the state variable at time k be X k Where: P = [x y z] T is the carrier position vector; v = [v x v y v z T is the carrier velocity vector; q 0 = [q 1 q 2 q 3 q 4 is the carrier attitude quaternion vector; is the accelerometer random walk term; is the gyroscope random walk term;​ Its cost function is: x k , x k+1 are the state variables at times k and k + 1, is the INS deviation measurement noise covariance, h(x k , x k+1 ) is the INS measurement result.

3. The factor graph weight adaptive multi-source fusion localization method for underground pipeline robots according to claim 1 Characterized in that: In step S1, the UWB factor node and its cost function are specifically: Establish a UWB measurement model, construct a UWB factor node and its cost function; Use the channel state information CSI as a feature to establish a UWB fingerprint database and construct a UWB factor node; This localization method is divided into two stages: the offline stage and the online stage. In the offline stage, first collect the CSI data amplitude to establish the CSI original fingerprint database, and then use the sparse autoencoder SAE to compress the input CSI data to the feature layer. The output feature layer is further connected to a 1D-CNN model for training and testing; CSI = [|CSI 1,1,1 |,|CSI 1,1,2 |,…|CSI 1,1,q |,…|CSI 1,2,1 |,...|CSI m,n,q |] Where m is the number of transmitter antennas, n is the number of receiver antennas, and q is the number of channels, i.e., the number of subcarriers; In the online stage, the underground robot decodes by matching the received CSI measurement value, obtains the localization result according to the regression model, inputs the measured coordinates of UWB into the factor graph, and constructs a UWB factor node; The SAE input layer has m*n*q neurons, and the two hidden layers have l and q neurons respectively. The three-layer one-dimensional convolutional neural network 1D-CNN includes a convolutional layer, a batch normalization layer, an activation layer, a pooling layer, a fully connected FC layer, and a softmax output layer. The activation function is the ReLU function; Its cost function is: where N is the total number of validation samples, y(x) is the desired output, j represents the number of layers in the network, and h j (x) is the activation vector output by the network when x is the input.

4. The method for multi-source fusion positioning of an underground pipeline network robot based on factor graph weight adaption according to claim 1, characterized in that: In the step S1, the ELF factor node and its cost function: An ELF measurement model is established, an INS factor node and its cost function are constructed. Through a sensor array, the magnetic signal is converted into voltage, and then a particle swarm quasi-Newton hybrid algorithm is used for non-linear calculation; Its cost function is: where min is the minimum value function, is the voltage observation value, u i is the voltage state variable.

5. The method for multi-source fusion positioning of an underground pipeline network robot based on factor graph weight adaption according to claim 1, characterized in that: In the step S3, the corresponding sensor factor node is added to the factor graph fusion framework of the navigation system: The factor graph fusion framework of the multi-source fusion navigation algorithm based on the factor graph is based on the INS factor. As time updates, the INS is pre-integrated and optimized with a sliding window. When the navigation system receives the measurement information of the ELF and UWB sensors, the corresponding sensor factor nodes are added to the factor graph fusion framework of the navigation system, and the relevant variable nodes are updated according to their corresponding sensor measurement equations and their corresponding cost functions; Its cost function is: where \(i\) represents the factor of various navigation systems at the current moment, \(\text{argmin}\) represents the variable value when the objective function takes the minimum value, \(K(d i )\) is the dynamic weight function, \(h i (X i )\) is the measurement function of different navigation systems, and \(z i \) is the actual measurement value obtained from various navigation systems.

6. The method for multi-source fusion positioning of an underground pipeline network robot based on factor graph weight adaption according to claim 1, characterized in that: In the step S3, the steps for adaptively adjusting the dynamic weight of the factor graph based on the residual are as follows: Among them, K(d i ) is the dynamic weight function, and Δd 2 represents the square of the residual between the measured value and the predicted value of the sensor, which is called the credible distance, and the parameter θ is a coefficient; Cluster the weights K(d i ) of each sensor at each moment using the HDBSCAN algorithm. The clustering center point is the weight. For each newly added discrete point that has not been clustered, it is determined that the sensor factor node fails at this moment, and this factor is directly isolated and not used for fusion; Normalize the weights K(d i ) of all factors, and then perform subsequent optimal estimation.

7. The method for multi-source fusion positioning of an underground pipeline network robot based on factor graph weight adaption according to claim 1, characterized in that: The isolation of failed factors in the step S3 is as follows: The sensor factor nodes are judged in real time. If the newly added points are not clustered, the sensor factor nodes judged to have failed are isolated; for the INS factor, if it is not clustered for consecutive t time instants, it is judged that the cumulative error of the INS is large and the INS positioning fails, and the INS factor is isolated.

8. The method for multi-source fusion positioning of an underground pipeline network robot based on factor graph weight adaption according to claim 1, characterized in that: The establishment of the hidden Markov model in the step S6 is specifically as follows; For each calculated optimal estimated value point, a set of candidate road segments is first determined. If it is found that the calculated optimal estimated value point is very close to a certain road segment, a lower cost value is assigned to this road segment. Then the weights of the edges connecting each pair of adjacent vertices in the Markov chain are calculated. Finally, the maximum likelihood path is found on the Markov chain; C Match = C O + C T Among them, C Match represents the total cost of map matching, C O represents the observation cost of map matching, C T represents the transformation cost of map matching.

9. The method for multi-source fusion positioning of an underground pipeline network robot based on factor graph weight adaption according to claim 1, characterized in that: The observation cost in the step S6 is as follows: (1) Calculate the distance from the optimal estimated value point to the projection point of the matching road segment. The greater the distance, the greater the cost; (2) Calculate the direction difference between the advancing direction of the optimal estimated value point and the direction of the road section. The greater the direction difference, the greater the cost. (3) The weighted average of the distance cost and the direction cost is used as the final observation cost. (4) If the accuracy of the optimal estimated value is low, the distance and direction costs are correspondingly reduced. (5) When the speed of the optimal estimated value is low, the direction cost needs to be reduced.

10. The multi-source fusion positioning method for an underground pipeline network robot based on factor graph weight adaption according to claim 1, characterized in that: the conversion cost in step S6 is as follows: The conversion cost is the cost from a certain road section corresponding to the previous point to a certain road section corresponding to the next point during the process from the observation point of the previous optimal estimated value to the next observation point. The closer the road section distance is to the distance between the two observation points, the smaller the cost.

Images