Graph optimization GNSS and IMU integrated navigation method and system

Abstract

The invention discloses a graph optimization GNSS (Global Navigation Satellite System) and IMU (Inertial Measurement Unit) integrated navigation method and a graph optimization GNSS and IMU integrated navigation system. The method comprises the following steps: acquiring first target data, and performing first preprocessing on the first target data, establishing a first graph optimization model according to the first target data after the first preprocessing; and solving the first graph optimization model, and determining the navigation state according to a solving result. Through a graph optimization technology, IMU and GNSS data can be effectively fused, and the stability and precision of a navigation system can be kept even under the condition that GNSS signals are interrupted or the number of satellites is insufficient.

Description

Technical Field

[0001] The present invention relates to the technical field of wire state monitoring, and in particular to a graph-optimized GNSS and IMU combined navigation method and system. Background Art

[0002] Traditional GNSS / IMU integrated navigation methods mainly rely on Kalman filtering (KF) technology. Kalman filtering fuses the high-frequency, short-term precision motion sensor data provided by IMU with the low-frequency, long-term precision positioning data provided by GNSS through recursive prediction and update steps. However, Kalman filtering is prone to estimation error accumulation and system performance degradation when facing GNSS signal interruption or insufficient number of satellites. In addition, Kalman filtering is sensitive to outliers (such as pseudorange errors caused by multipath effects), which further limits its application effect in complex environments.

[0003] In recent years, graph optimization (GO) methods have gradually become a powerful tool to solve the above problems. Graph optimization represents the state and observation data of the navigation system as nodes and edges in a graph, and uses batch optimization technology to simultaneously consider observation information from multiple times and multiple satellites, thereby improving the global consistency and robustness of the system. However, traditional graph optimization methods still face the challenges of reduced positioning accuracy and insufficient system stability when facing GNSS signal loss or satellite occlusion.

[0004] In the field of wire and line dancing monitoring, accurate positioning and attitude estimation are crucial for real-time monitoring of wire status, disaster prevention, and maintaining the stable operation of power systems. Under the influence of external factors such as wind and temperature changes, wires may produce complex dancing patterns, which places higher requirements on the real-time and accuracy of the navigation system. In such applications, the traditional GNSS / IMU combined navigation system will have a large interference problem due to the long noise duration, and the anti-noise ability is weak, making it difficult to maintain high-precision and stable positioning performance, affecting the reliability and effectiveness of wire dancing monitoring. Summary of the invention

[0005] In view of the above existing problems, the present invention is proposed.

[0006] Therefore, the present invention provides a graph-optimized GNSS and IMU combined navigation method and system, which can solve the problems mentioned in the background technology.

[0007] In order to solve the above technical problems, the present invention provides the following technical solutions:

[0008] In a first aspect, the present invention provides a graph-optimized GNSS and IMU combined navigation method, comprising: acquiring first target data, and performing a first preprocessing on the first target data, the first preprocessing at least including calculating a first alternative constraint; establishing a first graph optimization model based on the first target data after the first preprocessing; solving the first graph optimization model, and determining a navigation state based on the solution result.

[0009] As a preferred solution of the graph optimization GNSS and IMU integrated navigation method described in the present invention, wherein: the first graph optimization model includes:

[0010] The first graph optimization model includes a first objective function and a first constraint condition;

[0011] The first objective function is an arbitrary function for solving the total cost;

[0012] The first constraint condition is the first substitution constraint.

[0013] As a preferred solution of the graph optimization GNSS and IMU integrated navigation method described in the present invention, wherein: the first graph optimization model is composed of a plurality of nodes and a plurality of factors;

[0014] The several factors constitute the first objective function;

[0015] The plurality of nodes at least include state vectors of the receiver at different times and position vectors of the satellite.

[0016] As a preferred solution of the graph optimization GNSS and IMU integrated navigation method described in the present invention, wherein: solving the first graph optimization model includes:

[0017] Solving the first graph optimization model to obtain a first increment;

[0018] Preset a first threshold value, and determine the relationship between the first increment and the first threshold value;

[0019] The solution is completed based on the judgment result.

[0020] As a preferred solution of the graph-optimized GNSS and IMU integrated navigation method of the present invention, wherein: the first objective function at least includes an IMU factor, a GNSS pseudorange factor and a virtual pseudorange factor;

[0021] The IMU factor connects the receiver states at adjacent moments;

[0022] The GNSS pseudorange factor links the receiver state to the satellite position at the corresponding moment;

[0023] The virtual pseudorange factor links the receiver state to the predicted satellite positions.

[0024] As a preferred solution of the graph optimized GNSS and IMU integrated navigation method of the present invention, the acquiring of first target data and performing first preprocessing on the first target data comprises:

[0025] The first target data at least includes pseudo-range measurement values ​​of a plurality of satellites corresponding to each moment to the target conductor and their change rates;

[0026] The first preprocessing includes predicting the virtual pseudorange at the next moment based on the pseudorange and pseudorange change rate at the previous moment;

[0027] Virtual pseudoranges provide a first substitute constraint when real pseudoranges are not available.

[0028] As a preferred solution of the graph optimization GNSS and IMU integrated navigation method described in the present invention, the first objective function also includes:

[0029] The IMU factor is the IMU error function at a certain moment;

[0030] The GNSS pseudorange factor is a GNSS pseudorange error function of a number of satellites at a certain moment;

[0031] The virtual pseudorange factor is a virtual pseudorange error function of a number of satellites at a certain moment.

[0032] In a second aspect, the present invention provides a graph-optimized GNSS and IMU integrated navigation system, comprising:

[0033] A preprocessing module, configured to obtain first target data and perform first preprocessing on the first target data, wherein the first preprocessing at least includes calculating a first substitution constraint;

[0034] A model building module, used for building a first graph optimization model according to the first target data after the first preprocessing;

[0035] A solution module is used to solve the first graph optimization model and determine the navigation state according to the solution result.

[0036] In a third aspect, the present invention provides an electronic device, comprising:

[0037] Memory and processor;

[0038] The memory is used to store computer executable instructions, and the processor is used to execute the computer executable instructions. When the computer executable instructions are executed by the processor, the steps of the method are implemented.

[0039] In a fourth aspect, the present invention provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program implements the steps of the method described above when executed by a processor.

[0040] Compared with the prior art, the beneficial effects of the present invention are as follows: the present invention proposes a graph-optimized GNSS and IMU combined navigation method and system, obtains first target data, and performs a first preprocessing on the first target data, wherein the first preprocessing at least includes calculating a first substitution constraint; establishes a first graph optimization model according to the first target data after the first preprocessing; solves the first graph optimization model, and determines the navigation state according to the solution result. Through the graph optimization technology, the present invention can effectively fuse IMU and GNSS data, and maintain the stability and accuracy of the navigation system even when the GNSS signal is interrupted or the number of satellites is insufficient. The system of the present invention has stronger robustness when facing outliers, such as pseudorange errors caused by multipath effects, and reduces sensitivity to outliers. In the field of wire line galloping monitoring, the method and system provided by the present invention can provide accurate positioning and attitude estimation, which is helpful to monitor the wire state in real time, prevent disasters, and maintain the stable operation of the power system. The graph-optimized GNSS and IMU combined navigation method and system of the present invention improve the application effect in complex environments through the improvement of the optimization algorithm, especially in the line galloping monitoring scene with strong electromagnetic interference, and has better anti-noise ability. The system of the present invention is flexible in design and can adapt to navigation requirements under different environments and conditions, providing new ideas and solutions for the development of navigation technology. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other accompanying drawings can be obtained based on these accompanying drawings without paying creative labor.

[0042] Figure 1 A method flow chart of a method and system for graph-optimized GNSS and IMU integrated navigation provided by one embodiment of the present invention;

[0043] Figure 2 A detailed and implementable flow chart of a graph-optimized GNSS and IMU integrated navigation method and system provided for one embodiment of the present invention. DETAILED DESCRIPTION

[0044] In order to make the above-mentioned purposes, features and advantages of the present invention more obvious and easy to understand, the specific implementation methods of the present invention are described in detail below in conjunction with the drawings of the specification. Obviously, the described embodiments are part of the embodiments of the present invention, but not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary persons in the art without creative work should fall within the scope of protection of the present invention.

[0045] Example 1

[0046] Reference Figure 1-Figure 2 , which is the first embodiment of the present invention, provides a graph-optimized GNSS and IMU combined navigation method and system, including:

[0047] There are some problems in the existing related technologies. For example, GNSS signals are easily blocked in urban canyons, tunnels or indoor environments, resulting in reduced positioning accuracy. Although IMU (inertial measurement unit) can provide continuous motion information, its errors will accumulate over time, affecting the accuracy of long-term navigation. In addition, when GNSS and IMU data are fused, traditional methods often cannot fully consider the dynamic changes of various error sources, resulting in limited navigation performance.

[0048] This application provides a method that can effectively solve the above-mentioned problems. Next, we will combine multiple embodiments to explain in detail how to implement the image optimization GNSS and IMU combined navigation method;

[0049] Figure 1 A flowchart of a method and system for optimizing GNSS and IMU combined navigation is shown, including:

[0050] S101, acquiring first target data, and performing first preprocessing on the first target data, wherein the first preprocessing at least includes calculating a first substitution constraint;

[0051] In the embodiment of the present application, obtaining first target data and performing first preprocessing on the first target data includes:

[0052] The first target data at least includes pseudo-range measurement values ​​of a plurality of satellites corresponding to each moment to the target conductor and their change rates;

[0053] The first preprocessing includes predicting the virtual pseudorange at the next moment based on the pseudorange and pseudorange change rate at the previous moment;

[0054] Virtual pseudoranges provide a first substitute constraint when real pseudoranges are not available.

[0055] In an optional embodiment, the first target data may also include measurements of an accelerometer and a gyroscope of an IMU. These measurements are used to estimate the motion state of the target, including speed and attitude information. Through the IMU data, the dynamic changes of the target can be modeled, so that the continuity and accuracy of navigation can be maintained when the GNSS signal is lost or inaccurate.

[0056] In an optional embodiment, the first preprocessing may further include filtering the IMU data to reduce the influence of noise and error. The filtering algorithm may be a Kalman filter, an extended Kalman filter or a particle filter, etc., to adapt to different application scenarios and accuracy requirements. Through filtering, the reliability of the IMU data can be improved, thereby improving the performance of the entire navigation system.

[0057] In an optional embodiment, the first preprocessing may further include correcting the multipath effect of the satellite signal to ensure the accuracy of the pseudorange measurement value.

[0058] It should be noted that the correction of multipath effects usually involves complex signal processing techniques, such as spatial correlation analysis or time series analysis. These preprocessing steps work together to provide high-quality data input for subsequent map optimization processing, ensuring the stability and accuracy of the navigation system.

[0059] In an optional embodiment, based on the pseudorange and pseudorange change rate at the previous moment, the prediction of the virtual pseudorange at the next moment can be achieved by establishing a dynamic model. The dynamic model can predict the future position of the target based on its motion characteristics, such as speed and acceleration. This prediction method can reduce the dependence on real-time GNSS signals, so that continuous navigation information can still be provided in the event of temporary signal loss.

[0060] It should be noted that the prediction model can be combined with historical data and statistical information to improve the accuracy of the prediction. In practical applications, a Kalman filter can be used to integrate the prediction model and observation data to optimize the navigation solution process. In this way, the integrated navigation system can maintain high positioning accuracy and reliability even when the GNSS signal is unstable or interrupted.

[0061] In the embodiment of the present application, at each time k, the pseudo-range measurement value of the i-th satellite is collected and its rate of change Based on the pseudorange and pseudorange change rate of the previous moment, the virtual pseudorange of the next moment is predicted Virtual pseudorange when true pseudorange is not available providing a first alternative constraint;

[0062] It should be noted that obtaining the first target data and performing a first preprocessing on the first target data, the first preprocessing at least including calculating the first alternative constraint can significantly improve the robustness of the system. Through the first preprocessing, the pseudorange measurement error caused by signal shielding or interference can be effectively reduced, thereby improving the overall performance of the navigation system. In addition, the first preprocessing also includes filtering and noise suppression of the data to ensure that more accurate and reliable data can be used in the subsequent graph optimization process. Such processing not only enhances the anti-interference ability of the system, but also provides high-quality input data for the graph optimization algorithm, so that the continuity and accuracy of navigation can be maintained when the GNSS signal is unstable or interrupted.

[0063] S102, establishing a first graph optimization model according to the first target data after the first preprocessing;

[0064] In an optional embodiment, the first graph optimization model can be represented by a factor graph, where nodes represent state variables and edges represent observation constraints. In the factor graph, each factor node can be represented as a factor function, which maps the connected variable nodes to a non-negative value that represents the compatibility between the observed data and the model prediction. By minimizing the product of the factor function, the optimal state estimate can be solved.

[0065] In an optional embodiment, the first graph optimization model may include multiple constraints, such as kinematic constraints, observation constraints, and prior knowledge constraints, to ensure the accuracy and reliability of the navigation solution. In practical applications, the construction of the first graph optimization model needs to consider the balance between computational efficiency and accuracy to adapt to different application scenarios and real-time requirements.

[0066] In an embodiment of the present application, the first graph optimization model includes:

[0067] The first graph optimization model includes a first objective function and a first constraint condition;

[0068] The first objective function is an arbitrary function that solves the total cost;

[0069] The first constraint is the first alternative constraint.

[0070] In an optional embodiment, the first objective function may use the least square method to minimize the difference between the observed data and the model prediction. In this way, the accuracy of the navigation solution can be ensured while reducing the amount of calculation.

[0071] In an optional embodiment, the first objective function may also include one or more regularization terms to prevent overfitting and enhance the generalization ability of the model.

[0072] It should be noted that these regularization terms can be constraints based on prior knowledge, such as limits on velocity or acceleration, to ensure the physical rationality of the solution. By adjusting the weights of these regularization terms, the best balance can be found between the accuracy and stability of the navigation solution.

[0073] In an optional embodiment, the first objective function may also include one or more dynamic noise terms, which can reflect the dynamic characteristic changes of the system in actual operation. By introducing dynamic noise terms, the model can better adapt to environmental changes and improve the navigation solution performance under complex dynamic conditions.

[0074] In an optional embodiment, the introduction of dynamic noise terms can also help the model identify and correct abnormal data caused by sensor errors or external interference, thereby improving the robustness of the overall navigation system.

[0075] In the embodiment of the present application, the first graph optimization model is composed of a number of nodes and a number of factors;

[0076] Several factors constitute the first objective function;

[0077] Several nodes include at least the state vectors of the receiver at different times and the position vectors of the satellite.

[0078] In the embodiment of the present application, the first objective function includes at least an IMU factor, a GNSS pseudorange factor, and a virtual pseudorange factor;

[0079] The IMU factor connects the receiver states at adjacent moments;

[0080] The GNSS pseudorange factor links the receiver state to the satellite position at the corresponding moment;

[0081] The virtual pseudorange factor links the receiver state to the predicted satellite positions.

[0082] In the embodiment of the present application, the first objective function also includes:

[0083] The IMU factor is the IMU error function at a certain moment;

[0084] The GNSS pseudorange factor is the GNSS pseudorange error function of several satellites at a certain moment;

[0085] The virtual pseudorange factor is the virtual pseudorange error function of several satellites at a certain moment.

[0086] Exemplarily, the nodes and factors of the graph optimization model are constructed, where the nodes include the state vectors x of the receiver at different times k and the satellite's position vector P k sat, the factors include IMU factor, GNSS pseudorange factor and virtual pseudorange factor. The IMU factor connects the receiver status at adjacent moments, the GNSS pseudorange factor connects the receiver status with the satellite position at the corresponding moment, and the virtual pseudorange factor connects the receiver status with the predicted satellite position;

[0087] Furthermore, the IMU factor is calculated, which is the IMU error function e at time k. IMU,k ;

[0088] Furthermore, the GNSS pseudorange factor is calculated, which is the GNSS pseudorange error function e of the ith satellite at time k. GNSS,k,i ;

[0089] Furthermore, the virtual pseudorange factor is calculated, which is the virtual pseudorange error function e of the i-th satellite at time k. VC,k,i ;

[0090] Furthermore, the total cost function is constructed by using the IMU factor, GNSS pseudorange factor, and virtual pseudorange factor;

[0091] e IMU,k =x k -h(x k-1 ,c k-1 ,z IMU,k-1 )

[0092] Among them, x k represents the state vector of the IMU at time k,

[0093] Where h(·) represents the IMU state transfer function; x k-1 represents the state vector at time k-1; c k-1 represents the IMU deviation at time k-1; z IMU,k-1 represents the IMU measurement value at time k-1; p k represents the position vector at time k; v k represents the velocity vector at time k; R k represents the IMU attitude rotation matrix at time k; b a,k represents the accelerometer deviation at time k; b ω,k represents the gyroscope bias at time k;

[0094] Furthermore, h(·) has the following specific form:

[0095]

[0096] v k =v k-1 +R k-1 (a k-1 -ba,k-1 )Δt

[0097] R k =R k-1 exp((ω k-1 -b ω,k-1 )Δt)

[0098]

[0099] Where Δt represents the time interval between consecutive moments; exp(·) represents the exponential mapping from the rotation vector to the rotation matrix; and The process noise of the accelerometer and gyroscope bias, a k-1 represents the acceleration measured by the IMU at time k-1; ω k-1 Represents the angular velocity measured by the IMU at time k-1.

[0100]

[0101] in, represents the Euclidean distance between the receiver position and the i-th satellite position, b k represents the receiver clock bias at time k.

[0102]

[0103] in, The predicted position vector of the i-th satellite at time k.

[0104] Furthermore, the total cost function is constructed as follows,

[0105]

[0106] Where E represents the total cost function, Σ IMU,k represents the covariance matrix of the IMU error function, Σ GNSS,k,i represents the covariance matrix of the GNSS pseudorange error function, Σ VC,k,i Represents the covariance matrix of the virtual pseudorange error function.

[0107] It should be noted that establishing the first graph optimization model based on the first target data after the first preprocessing improves the accuracy of data processing, and the error state of the sensor can be estimated more accurately through the graph optimization model. The computational efficiency is optimized, and the first graph optimization model can quickly converge to the optimal solution, reducing the computational time. The robustness of the system is enhanced, and stable navigation performance can be maintained even in complex environments. Through the iterative process of the graph optimization model, data from different sensors can be effectively fused, improving the overall performance of the navigation system.

[0108] S103, solving the first graph optimization model, and determining the navigation state according to the solution result.

[0109] In an optional embodiment, the first graph optimization model can be solved by a graph-based optimization algorithm, such as the least squares method or the gradient descent method. Through iterative calculation, the optimal solution is gradually approached to obtain an estimated value of the sensor error state. During the solution process, appropriate convergence conditions can be set, such as an error threshold or an upper limit on the number of iterations, to ensure the efficiency and accuracy of the calculation. After the solution is completed, the navigation state can be accurately determined based on the obtained error state estimate, thereby achieving high-precision positioning and navigation.

[0110] In an optional embodiment, the Kalman filter algorithm can also be used to solve the first graph optimization model. The algorithm estimates the system state by establishing a state space model, combining sensor data and a prediction model. At each time step, the Kalman filter will first make a prediction and then update it based on the new observation data to obtain a more accurate state estimate. This method is particularly suitable for dynamic systems and can effectively handle noise and uncertainty, thereby improving the stability and accuracy of the navigation system. In addition, the Kalman filter has a high computational efficiency and is suitable for real-time or near real-time navigation applications. In this way, the performance of the navigation system can be further improved to ensure navigation accuracy in various complex environments.

[0111] In an optional embodiment, the particle filter algorithm can also be used to solve the first graph optimization model. The algorithm represents the probability distribution through a series of random samples (particles), and each particle represents a possible state of the system. During the filtering process, the particles are resampled according to the observed data and the system model to reflect the latest state information. The particle filter is particularly suitable for systems with nonlinear and non-Gaussian noise, and can provide a more flexible and robust navigation solution. Through particle filtering, complex dynamic environments can be effectively handled, and the adaptability and accuracy of the navigation system can be improved. In addition, the parallel processing capability of the particle filter makes it have good scalability on multi-core processors, further enhancing the ability of real-time processing.

[0112] In an embodiment of the present application, solving the first graph optimization model includes:

[0113] Solving the first graph optimization model to obtain a first increment;

[0114] Preset a first threshold value, and determine the relationship between the first increment and the first threshold value;

[0115] The solution is completed based on the judgment result.

[0116] Exemplarily, Taylor expansion is performed on the total cost function to linearize the total cost function, and a Jacobian matrix J and a residual vector e are obtained;

[0117] Calculate the weighted Jacobian matrix J based on the Jacobian matrix J and the residual vector e w and the weighted residual vector e w ;

[0118] Constructing the weighted increment equation

[0119] Use the conjugate gradient method to solve the state variable increment Δx;

[0120] Check whether the norm of the state variable increment Δx is less than the preset threshold. If the norm of the state variable increment Δx is less than the preset value, update the state vector x←x+Δx, otherwise re-collect data and repeat the overall steps.

[0121] Weighted Jacobian matrix J w The calculation method is:

[0122] J w =Σ -1 / 2 J

[0123] Weighted residual vector e w The calculation method is:

[0124] -1 / 2

[0125] e w =Σe

[0126] in, Σ -1 / 2 is the inverse square root of the covariance matrix Σ.

[0127] In summary, the present invention proposes a graph-optimized GNSS and IMU combined navigation method, obtains first target data, and performs a first preprocessing on the first target data, the first preprocessing at least includes calculating a first substitution constraint; establishing a first graph optimization model according to the first target data after the first preprocessing; solving the first graph optimization model, and determining the navigation state according to the solution result. Through the graph optimization technology, the present invention can effectively fuse IMU and GNSS data, and maintain the stability and accuracy of the navigation system even when the GNSS signal is interrupted or the number of satellites is insufficient. The system of the present invention has stronger robustness when facing outliers, such as pseudorange errors caused by multipath effects, and reduces sensitivity to outliers. In the field of wire line dancing monitoring, the method and system provided by the present invention can provide accurate positioning and attitude estimation, which is helpful to monitor the wire state in real time, prevent disasters, and maintain the stable operation of the power system. The graph-optimized GNSS and IMU combined navigation method and system of the present invention improve the application effect in complex environments through the improvement of the optimization algorithm, especially in the line dancing monitoring scene with strong electromagnetic interference, and has better anti-noise ability. The system of the present invention is flexible in design and can adapt to navigation requirements under different environments and conditions, providing new ideas and solutions for the development of navigation technology.

[0128] Example 2

[0129] In a preferred embodiment, the following specific method steps can be designed according to the above embodiment, such as: Figure 2 As shown:

[0130] Step 1: At each time k, collect the pseudorange measurement value of the i-th satellite and its rate of change Based on the pseudorange and pseudorange change rate of the previous moment, the virtual pseudorange of the next moment is predicted Virtual pseudorange when true pseudorange is not available Provide alternative constraints;

[0131] Step 2: Construct the nodes and factors of the graph optimization model. The nodes include the state vector x of the receiver at different times. k and the satellite's position vector The factors include IMU factors, GNSS pseudorange factors and virtual pseudorange factors. The IMU factors connect the receiver states at adjacent moments, the GNSS pseudorange factors connect the receiver states with the satellite positions at the corresponding moments, and the virtual pseudorange factors connect the receiver states with the predicted satellite positions.

[0132] Step 3: Calculate the IMU factor, which is the IMU error function e at time k IMU,k ;

[0133] Step 4: Calculate the GNSS pseudorange factor, which is the GNSS pseudorange error function e of the ith satellite at time k. GNSS,k,i ;

[0134] Step 5: Calculate the virtual pseudorange factor, which is the virtual pseudorange error function e of the i-th satellite at time k. VC,k,i ;

[0135] Step 6: Construct a total cost function through the IMU factor, the GNSS pseudorange factor and the virtual pseudorange factor;

[0136] Step 7: Perform Taylor expansion on the total cost function in step 6 to linearize the total cost function and obtain the Jacobian matrix J and the residual vector e;

[0137] Step 8: Calculate the weighted Jacobian matrix J based on the Jacobian matrix J and the residual vector e w and the weighted residual vector e w ;

[0138] Step 9: Construct weighted increment equation

[0139] Step 10, using the conjugate gradient method to solve the state variable increment Δx;

[0140] Step 11, check whether the norm of the state variable increment Δx is less than a preset threshold value, if the norm of the state variable increment Δx is less than the preset value, jump to step 12, otherwise jump to step 1;

[0141] Step 12: Update the state vector x←x+Δx.

[0142] Specifically,

[0143]

[0144] in, represents the predicted virtual pseudorange, represents the pseudorange measurement value of the i-th satellite at time k, represents the pseudorange change rate of the ith satellite at time k, Δt represents the time interval between consecutive moments, represents the position vector of the i-th satellite at time k+1, represents the position vector of the receiver at time k, and v represents the velocity vector of the receiver.

[0145] Specifically,

[0146] e IMU,k =x k -h(x k-1 ,c k-1 ,z IMU,k-1 )

[0147] Among them, x k represents the state vector of the IMU at time k,

[0148] h(·) represents the IMU state transfer function; x k-1 represents the state vector at time k-1; c k-1 represents the IMU deviation at time k-1; z IMU,k-1 represents the IMU measurement value at time k-1; p k represents the position vector at time k; v k represents the velocity vector at time k; R k represents the IMU attitude rotation matrix at time k; b a,k represents the accelerometer deviation at time k; b ω,k represents the gyroscope bias at time k;

[0149] In an optional embodiment, h(·) has the following specific form:

[0150]

[0151] v k =v k-1 +R k-1 (a k-1 -b a,k-1 )Δt

[0152] R k =R k-1 exp((ω k-1 -b ω,k-1 )Δt)

[0153]

[0154] Δt represents the time interval between consecutive moments; exp(·) represents the exponential mapping of the rotation vector to the rotation matrix; and The process noise of the accelerometer and gyroscope bias respectively; a k-1 represents the acceleration measured by the IMU at time k-1; ω k-1 Represents the angular velocity measured by the IMU at time k-1.

[0155] Specifically,

[0156]

[0157] in, represents the Euclidean distance between the receiver position and the i-th satellite position, b k represents the receiver clock bias at time k.

[0158] Specifically,

[0159]

[0160] in, The predicted position vector of the i-th satellite at time k.

[0161] Specifically, the total cost function is constructed by the following method,

[0162]

[0163] Where E represents the total cost function, Σ IMU,k represents the covariance matrix of the IMU error function, Σ GNSS,k,i represents the covariance matrix of the GNSS pseudorange error function, Σ VC,k,i Represents the covariance matrix of the virtual pseudorange error function.

[0164] Specifically, the weighted Jacobian matrix J w The calculation method is:

[0165] J w =Σ -1 / 2 J

[0166] Weighted residual vector e w The calculation method is:

[0167] -1 / 2

[0168] e w =Σe

[0169] in, Σ -1 / 2 is the inverse square root of the covariance matrix Σ.

[0170] It should be noted that in the line sway monitoring scenario of this application, due to the high electromagnetic noise, GNSS often finds it difficult to stably provide high-precision position information. Although IMU can provide continuous position and attitude updates in a short period of time, the cumulative error of IMU, especially the drift phenomenon, will become more and more obvious over time. Therefore, it is necessary to combine IMU and GNSS to achieve high-precision and continuous position information updates. However, the existing combined navigation method does not distinguish between the weight processing of measurement signals in scenarios with large and small electromagnetic noise, resulting in poor system robustness.

[0171] In order to solve these problems, the present invention proposes a graph optimization method based on weighted least squares and introduces virtual pseudorange as a new constraint mechanism. Specifically, when the GNSS pseudorange cannot be obtained normally, the system predicts the virtual pseudorange at the next moment based on the pseudorange and pseudorange change rate at the previous moment as a substitute constraint for the GNSS pseudorange. This mechanism effectively solves the problem of position information interruption caused by limited GNSS signals, thereby improving the robustness and continuity of the system.

[0172] In an optional embodiment, the virtual pseudorange predicts the pseudorange at a future time through known motion information. When the GNSS signal is not available, the virtual pseudorange can be used as a constraint to prevent the system from drifting. In addition to the virtual pseudorange, the present invention combines the IMU and GNSS pseudorange together through a graph optimization framework. The graph optimization method gradually optimizes the system state by constructing nodes and edges in the graph and utilizing the constraint relationship between adjacent nodes. The graph optimization model of the invention includes the state vector of the receiver at different times and the position vector of the satellite. The IMU factor, GNSS pseudorange factor and virtual pseudorange factor respectively describe the error of the IMU, the GNSS pseudorange error and the virtual pseudorange error, and these factors together constitute the key constraints in the graph optimization model.

[0173] In an optional embodiment, the IMU factor connects the receiver states at adjacent moments and describes the state change of the inertial measurement unit. The GNSS pseudorange factor connects the position states of the receiver and the satellite and reflects the deviation of the pseudorange measurement. The virtual pseudorange factor provides compensation through the predicted pseudorange when the GNSS signal is unavailable to ensure the continuity of the graph optimization process.

[0174] In an optional embodiment, in order to improve the system's error resistance, a total cost function is constructed, which integrates the error terms of IMU, GNSS pseudorange and virtual pseudorange, and controls the influence of different error terms on the system solution results through the weight matrix. In order to optimize the state variables, the conjugate gradient method is used to solve the weighted increment equation, which significantly speeds up the solution of large-scale optimization problems. The application of the conjugate gradient method not only effectively reduces the computational complexity, but also ensures that the method can achieve efficient solution with less computing resources in practical applications.

[0175] In an optional embodiment, during the solution process, the system updates the state variables through multiple iterations. At the end of each round of iteration, check whether the norm of the state variable increment is less than a preset threshold. If the norm of the increment is less than the preset value, the system solution process will stop, which means that the navigation system has reached a stable state. If the increment exceeds the threshold, the system will continue to iterate and optimize until the expected accuracy is achieved. Such an iterative process ensures that the system maintains stability in the process of gradually approaching the optimal solution, and improves the system's anti-interference ability through weighted error constraints.

[0176] Example 3

[0177] This embodiment also provides a graph-optimized GNSS and IMU integrated navigation system, including:

[0178] A preprocessing module, used for acquiring first target data and performing first preprocessing on the first target data, wherein the first preprocessing at least includes calculating a first substitution constraint;

[0179] A model building module, used to build a first graph optimization model according to the first target data after the first preprocessing;

[0180] The solution module is used to solve the first graph optimization model and determine the navigation state according to the solution result.

[0181] The above-mentioned unit modules may be embedded in or independent of a processor in a computer device in the form of hardware, or may be stored in a memory in a computer device in the form of software, so that the processor can call and execute operations corresponding to the above-mentioned modules.

[0182] This embodiment also provides an electronic device, which includes a processor, a memory, a communication interface, a display screen and an input device connected through a system bus. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The communication interface of the computer device is used to communicate with an external terminal in a wired or wireless manner, and the wireless manner can be achieved through WIFI, an operator network, NFC (near field communication) or other technologies. When the computer program is executed by the processor, a busbar pressure loss load transfer analysis method is implemented. The display screen of the computer device can be a liquid crystal display screen or an electronic ink display screen, and the input device of the computer device can be a touch layer covered on the display screen, or a key, trackball or touchpad set on the computer device housing, or an external keyboard, touchpad or mouse, etc.

[0183] This embodiment also provides a computer-readable storage medium on which a computer program is stored. When the program is executed by a processor, the method proposed in the above embodiment is implemented.

[0184] The storage medium proposed in this embodiment and the method proposed in the above embodiment belong to the same inventive concept. The technical details not fully described in this embodiment can be referred to the above embodiment, and this embodiment has the same beneficial effects as the above embodiment.

[0185] Through the above description of the implementation methods, the technicians in the relevant field can clearly understand that the present invention can be implemented by means of software and necessary general hardware, and of course can also be implemented by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art can be embodied in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, such as a computer floppy disk, read-only memory (ReadOnly, Memory, ROM), random access memory (RandomAccess Memory, RAM), flash memory (FLASH), hard disk or optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, server, or network device, etc.) to perform the methods of various embodiments of the present invention.

[0186] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention may be modified or replaced by equivalents without departing from the spirit and scope of the technical solutions of the present invention, which should all be included in the scope of the claims of the present invention.

[0187] Those skilled in the art will appreciate that the embodiments of the present application can be provided as methods, systems, or computer program products. Therefore, the present application can adopt the form of complete hardware embodiments, complete software embodiments, or embodiments in combination with software and hardware. Moreover, the present application can adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code. The scheme in the embodiments of the present application can be implemented in various computer languages, for example, object-oriented programming language Java and literal scripting language JavaScript, etc.

[0188] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0189] These computer program instructions may also be stored in a computer readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture including an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0190] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process in the computer or other programmable device. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0191] Although the preferred embodiments of the present application have been described, those skilled in the art may make other changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications falling within the scope of the present application.

[0192] Obviously, those skilled in the art can make various changes and modifications to the present application without departing from the spirit and scope of the present application. Thus, if these modifications and variations of the present application fall within the scope of the claims of the present application and their equivalents, the present application is also intended to include these modifications and variations.

Claims

1. A graph-optimized GNSS and IMU combined navigation method, characterized in that: include: Acquire first target data, and perform first preprocessing on the first target data, wherein the first preprocessing at least includes calculating a first substitution constraint; Establishing a first graph optimization model according to the first target data after the first preprocessing; The first graph optimization model is solved, and the navigation state is determined according to the solution result.

2. The graph-optimized GNSS and IMU integrated navigation method according to claim 1, characterized in that: The first graph optimization model comprises: The first graph optimization model includes a first objective function and a first constraint condition; The first objective function is an arbitrary function for solving the total cost; The first constraint condition is the first substitution constraint.

3. The graph-optimized GNSS and IMU integrated navigation method according to claim 2, characterized in that: The first graph optimization model is composed of a number of nodes and a number of factors; The several factors constitute the first objective function; The plurality of nodes at least include state vectors of the receiver at different times and position vectors of the satellite.

4. The graph-optimized GNSS and IMU integrated navigation method as claimed in claim 3, characterized in that: The solving the first graph optimization model comprises: Solving the first graph optimization model to obtain a first increment; Preset a first threshold value, and determine the relationship between the first increment and the first threshold value; The solution is completed based on the judgment result.

5. The graph-optimized GNSS and IMU integrated navigation method according to claim 4, characterized in that: The first objective function includes at least an IMU factor, a GNSS pseudorange factor and a virtual pseudorange factor; The IMU factor connects the receiver states at adjacent moments; The GNSS pseudorange factor links the receiver state to the satellite position at the corresponding moment; The virtual pseudorange factor links the receiver state to the predicted satellite positions.

6. The graph-optimized GNSS and IMU integrated navigation method according to claim 5, characterized in that: The acquiring of first target data and performing first preprocessing on the first target data includes: The first target data at least includes pseudo-range measurement values ​​of a plurality of satellites corresponding to each moment to the target conductor and their change rates; The first preprocessing includes predicting the virtual pseudorange at the next moment based on the pseudorange and pseudorange change rate at the previous moment; Virtual pseudoranges provide a first substitute constraint when real pseudoranges are not available.

7. The map-optimized GNSS and IMU integrated navigation method according to claim 6, characterized in that: The first objective function also includes: The IMU factor is the IMU error function at a certain moment; The GNSS pseudorange factor is a GNSS pseudorange error function of a number of satellites at a certain moment; The virtual pseudorange factor is a virtual pseudorange error function of a number of satellites at a certain moment.

8. A graph-optimized GNSS and IMU integrated navigation system, characterized in that: include: A preprocessing module, configured to obtain first target data and perform first preprocessing on the first target data, wherein the first preprocessing at least includes calculating a first substitution constraint; A model building module, used for building a first graph optimization model according to the first target data after the first preprocessing; A solution module is used to solve the first graph optimization model and determine the navigation state according to the solution result.

9. An electronic device, comprising: Memory and processor; The memory is used to store computer executable instructions, and the processor is used to execute the computer executable instructions. When the computer executable instructions are executed by the processor, the steps of any one of the methods of claims 1 to 7 are implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images