A Multi-Source Navigation Information Fusion Method Based on Online Incremental Scale Factor Map

Abstract

This invention discloses a multi-source navigation information fusion method based on online incremental scaling factor graphs. This method constructs a factor graph using an inertial navigation system as the core sensor. Addressing the efficiency issue of large-scale factor graph computation, it introduces a dynamically changing sliding window method to achieve incremental updates of the factor graph. Using the circular probability error calculated from the covariance matrix during fusion as a benchmark, the size of the sliding window during factor graph optimization is decreased or increased when a pre-set accuracy threshold is higher or lower than a certain proportion of the circular probability error. This achieves an adaptive sliding window method based on navigation accuracy. Compared to existing factor graph multi-sensor information fusion methods, this method improves the optimization efficiency of traditional factor graphs while ensuring the robustness of the optimization results.

Description

Technical Field

[0001] This invention belongs to the field of multi-source information navigation, and more specifically, it relates to a multi-source navigation information fusion method based on online incremental scale factor graphs. Background Technology

[0002] Currently, multi-sensor information fusion technology has been widely applied in the navigation field. The factor graph framework can combine sensors with different update rates and errors for application. Based on the influence of measurement information on navigation state variables, sensors can be abstracted into measurement factors. Furthermore, the factor graph-based multi-source information fusion navigation method can conveniently process data from asynchronous heterogeneous sensors. After receiving the sensor output data, it expands factor nodes and quickly and effectively updates the system state according to the system's state equation and measurement equation, realizing comprehensive processing of multi-sensor data. This effectively solves problems such as various nonlinear parts that are difficult to linearize, the inability to obtain an accurate system model, or information asynchrony during multi-information filtering in multi-source information fusion systems during optimization.

[0003] Traditional factor graph fusion algorithms suffer from drift due to sensor failure in special scenarios such as underground parking lots, open outdoor spaces, and areas with satellite signal interference. Furthermore, due to their inherent structure, global optimization of traditional factor graphs results in excessive computational load and low computational efficiency.

[0004] This invention provides a multi-source navigation information fusion method based on online incremental scaling factor graphs. Compared to traditional factor graph fusion methods, this method uses an inertial navigation system as the core sensor and incorporates other auxiliary sensors to cope with application environments in various complex scenarios. Furthermore, addressing the issue of reduced optimization efficiency in the later stages of traditional factor graph optimization due to the increased graph size, this method introduces a dynamic sliding window method with adaptively changing window size to achieve incremental updates of the factor graph. Using the circular error calculated from the covariance matrix during fusion as a benchmark, the size of the factor graph optimization window is decreased or increased when a pre-set accuracy threshold is higher or lower than a certain proportion of the circular error, thus achieving a dynamic sliding window method that adapts to navigation accuracy. This method utilizes a sliding window to structurally scale the traditional factor graph and replaces the global optimization of the traditional factor graph with an incremental approach, significantly improving the optimization efficiency of factor graph navigation methods while ensuring navigation accuracy and robustness. Summary of the Invention

[0005] This invention provides a multi-source navigation information fusion method based on online incremental scale factor graphs. The technical problem it solves is: to realize multi-scenario applications by utilizing inertial navigation and other auxiliary sensors, and to realize iterative optimization of factor graphs by using a dynamic sliding window method, thereby significantly improving the computational efficiency of factor graph optimization while ensuring the optimization accuracy and robustness of the algorithm.

[0006] To achieve the above objectives, the present invention employs the following technical solution:

[0007] A multi-source navigation information fusion method based on online incremental scaling factor graphs is proposed. This method collects measurement information from inertial navigation and other auxiliary sensors, constructs corresponding sensor factor nodes for each, and implements incremental optimization of the integrated navigation system using a dynamic sliding window method. Then, the sensor factor nodes are fused and optimized using the factor graph. Using this information fusion method for integrated navigation can effectively reduce the computational cost of factor graph optimization and improve robustness. The steps are as follows:

[0008] Step 1: Collect angular velocity and acceleration output data from the inertial navigation system;

[0009] Step 2: Acquire position, velocity, and attitude data from the auxiliary sensors;

[0010] Step 3 involves constructing a factor map using the inertial navigation system and auxiliary sensors;

[0011] Step 4 introduces a dynamic sliding window method that can adapt to the size of the estimation accuracy window to achieve incremental updates of the factor graph. The circular probability error calculated by the covariance matrix during fusion is used as the standard. When the preset accuracy threshold is higher or lower than a certain proportion of the circular probability error, the size of the factor graph optimization window is reduced or increased to ensure optimization accuracy while improving the optimization efficiency of the traditional factor graph.

[0012] Step 5 involves incremental sliding optimization of the factor graph based on the determined sliding window size. This process handles discarded nodes and newly added nodes on the Bayesian network during sliding, thereby achieving a dynamic sliding window factor graph optimization method that adapts to the changing window size based on navigation accuracy.

[0013] Preferably, step 1 includes the following steps:

[0014] Step 1.1 Collect accelerometer measurement data of the inertial navigation system in the carrier. The output data of the accelerometer is shown in formula (1):

[0015]

[0016] Among them, f n The specific force output by the accelerometer of the inertial navigation system. Let ω be the acceleration of the carrier relative to the Earth. ie Let ω be the angular velocity of Earth's rotation. en V is the angular velocity of the carrier relative to the Earth. en Let g be the velocity of the carrier relative to the Earth, and g be the gravitational acceleration of the Earth at the location of the carrier.

[0017] Step 1.2 Collect gyroscope measurement data of the inertial navigation system in the carrier. The output data of the gyroscope is shown in formula (2):

[0018]

[0019] in, The projection of the angular velocity of the gyroscope of the carrier system relative to the inertial frame onto the carrier system. Let be the projection of the angular velocity of the geographic frame relative to the inertial frame onto the carrying frame. Let be the projection of the angular velocity of the navigation frame relative to the geographic frame onto the vehicle frame. The projection of the angular velocity of the carrying system relative to the geographic system onto the carrying system;

[0020] Preferably, step 2 includes the following steps:

[0021] Step 2.1 Acquire the position p output by the auxiliary sensor on the carrier. assi speed v assi ,attitude Information, specifically the information collected, is shown in formula (3):

[0022]

[0023] Where, p east ,p north ,p up These represent the eastward, northward, and celestial positions in the northeast-sky coordinate system obtained by the auxiliary sensor, respectively. east ,v north ,v up denoted as eastward velocity, northward velocity, and celestial velocity in the northeast-sky coordinate system obtained by auxiliary sensors, respectively; r, p, and y represent the roll angle, pitch angle, and yaw angle obtained by auxiliary sensors, respectively.

[0024] Preferably, step 3 includes the following steps:

[0025] Step 3.1 Based on the angular velocity and acceleration measurements of the inertial navigation system acquired in Step 1, construct the IMU pre-integration factor and update it for one pre-integration cycle t. k ~t k+1 Measurement data within t kPre-integration is performed under the carrier system at time t, and the calculation formulas for position, velocity, and attitude increments are shown in (4):

[0026]

[0027] in, and Represent the coordinate system of the carrier at time t to t k The rotation matrix and rotation rate matrix of the carrier coordinate system at any given time, f t b and These represent the accelerometer and gyroscope measurements at the current time t, respectively. and These represent the deviations of the accelerometer and gyroscope at the current time t, respectively.

[0028] Based on the dead reckoning principle of INS, t is obtained. k+1 The position, velocity, and attitude of the carrier at any given time are shown in formula (5):

[0029]

[0030] in, and They represent t respectively k The position, velocity, and attitude of the carrier in the navigation coordinate system at any given moment, g n For t k+1 Gravity vector in the navigation coordinate system at any given time Indicates t k The rotation matrix from the carrier coordinate system to the navigation coordinate system at any given time.

[0031] The IMU pre-integration factor is thus obtained as shown in formula (6):

[0032]

[0033] Where, x k+1 and x k They represent t respectively k+1 Time and t k The position, velocity, and attitude state variables at time α k Indicates the inertial navigation device at t k The deviation variable at time, z k Indicates t k The inertial device measurement value at time t, h(x) k ,α k ,z k ) represents t obtained from the state equation k+1 The predicted navigation state values ​​at time x, compared with the current estimated values. k+1The difference is the error function that needs to be minimized, represented by the IMU pre-integration factor, and d(·) is the cost function.

[0034] Step 3.2 Based on the auxiliary sensor measurement data described above, and utilizing the velocity, position, and attitude information provided by the auxiliary sensor, construct the auxiliary sensor factor, and its measurement equation. As shown in formula (7):

[0035]

[0036] Among them, h assi (·) is t k Measurement model of the time-assisted sensor, n assi The measurement noise of the auxiliary sensor is considered. The auxiliary sensor factor, constructed using the auxiliary sensor, is shown in formula (8):

[0037]

[0038] Where d[·] is the cost function and err(·) is the error function.

[0039] Preferably, step 4 includes the following steps:

[0040] Step 4.1 First, to balance computational complexity and navigation accuracy, the desired accuracy in the horizontal x and y directions is pre-set as D = (d x ,d y Let the deviation of the maximum circular probability error be (δ). x ,δ y Then, the circular probability error is calculated with the desired accuracy as the center and the deviation as the radius, at 50% and 95%, as shown in formula (9):

[0041]

[0042] Then, circles C1 and C2 are drawn with the expected accuracy D as the center and the circular probability error CEP and CEP95 as the radii. These circles are used as the basis for increasing or decreasing the size of the sliding window in the future, as shown in formula (10):

[0043]

[0044] Step 4.2 involves constructing a factor graph based on step 3. To obtain higher optimization accuracy, a certain number of factor nodes are first accumulated for optimization, denoted as pri_accu. The number of factor nodes pri_accu is then optimized once, and the resulting value is used as the prior value first_prior for the subsequent dynamic sliding window method.

[0045] Then, using the prior value first_prior, the number of factor nodes that meet the preset sliding window size SW_first is added to the sliding window to perform multi-sensor factor graph optimization.

[0046] The weight matrix is ​​obtained by using the inverse Ω of the optimized covariance matrix to calculate the mean deviation in the x and y directions. Ω is shown in formula (11):

[0047]

[0048] Where θ is the heading angle, Ω xx ,Ω yy ,Ω θθ These are the variances in the x, y, and θ directions, respectively, Ω xy ,Ω xθ ,Ω yθ These are the covariances of x and y, x and θ, and y and θ, respectively. These are the transposes of the covariances of x and y, x and θ, and y and θ, respectively, and the weights w between two factor nodes i and j. ij Expressed using formula (12):

[0049]

[0050] Where det(·) is the determinant of the matrix, the weight matrix W between factor nodes is obtained using formula (12), and the average weights of the x and y two-dimensional planes in this stage are obtained using formula (13).

[0051]

[0052] Where R is a diagonal matrix with r as its diagonal. i =∑ j w ij The diagonal values ​​r1 and r2 are the mean deviations. z is the current measurement value, and λ is the set coefficient.

[0053] Step 4.3 When the mean deviation If the error is within 50% circular error, the optimized result has high accuracy; if the sliding window size is larger than the preset minimum sliding window size SW, then... min Then, it is necessary to keep the number of sensor factor nodes behind the sliding window unchanged, while removing the sensor factor nodes in front of the sliding window, as shown in formula (14):

[0054]

[0055] Where SW is the current size of the sliding window. Considering that if the accuracy is too high, formula (14) needs to be calculated repeatedly, which increases the computational complexity of the algorithm, the sliding window size SW_lose is reduced once when the accuracy threshold is more than 50% of CEP to reduce the computational complexity, as shown in formula (15).

[0056]

[0057] Combining formulas (14) and (15), the overall formula for the decrease in the sliding window size is shown in formula (16):

[0058]

[0059] As the sliding window moves forward and removes factor nodes at the edge of the sliding window, to prevent information loss during the removal of edge factor nodes, edge factor nodes are transformed into prior information through edgeification; a factor node θ is removed from the factor graph. i This is equivalent to marginalizing factor nodes θ from the joint probability density function. i From the perspective of probability density, the marginalization factor node θ i This is also equivalent to the factor node θ i Integrating, the calculation formula is shown in equation (17):

[0060]

[0061] Where Θ represents the set of state variables, and p(·) represents the joint probability density function. Formula (17) represents the marginalization process, for the final state node θ n The integral only needs to be obtained by taking θ from the end of the product of the joint probability distributions. n Discarding the formula, the calculation formula is shown in formula (18):

[0062]

[0063] Step 4.4 When the mean deviation If the error exceeds 95% circular error, it indicates that the accuracy of the optimized result is low; if the sliding window size is smaller than the preset maximum sliding window size SW at this time... max Then, the nodes in front of the sliding window do not need to be removed, but only new sensor factor nodes need to be added to the sliding window, as shown in formula (19):

[0064]

[0065] Considering that if the accuracy is too low, the above formula needs to be calculated repeatedly, which increases the computational complexity of the algorithm; therefore, when the accuracy threshold is higher than 50% of CEP95, the sliding window size SW_increase is increased once to reduce the computational complexity, as shown in formula (20):

[0066]

[0067] Combining formulas (19) and (20), the general formula for the increase of the sliding window size is shown in formula (21):

[0068]

[0069] Step 4.5 When the mean deviation obtained above Beyond 50% circular error, within 95% circular error, or when the sliding window size reaches the preset minimum size SW. min Or the largest size SW max Then it will no longer decrease or increase, as shown in formula (22):

[0070]

[0071] Preferably, step 5 includes the following steps:

[0072] Step 5.1 Obtain the specific size SW of the sliding window based on the above equation, and update the factor graph model through incremental inference: The cost function of the factor graph within the sliding window is divided into two types. If it is a priori factor, the cost function of the factor graph is as shown in equation (23).

[0073]

[0074] in, For prior information on position, velocity, and attitude, This refers to the zero-bias drift prior information for the gyroscope and acceleration.

[0075] If it is not a priori factor, then the cost function of the factor graph is as shown in equation (24):

[0076]

[0077] in, The position, velocity, and attitude increments Δp obtained for each IMU pre-integration factor within a certain time interval within the sliding window. i ,Δv i ,Δr i and corresponding gyroscope and accelerometer zero bias increment SW represents the size of the sliding window, and N represents the number of auxiliary sensors other than the inertial navigation system within the corresponding time period. The cost function for other auxiliary sensors;

[0078] Step 5.2 The cost function is decomposed using a factor graph model so that each factor node corresponds to a sensor. The simplified maximum a posteriori estimate of the navigation state is obtained as shown in formula (25).

[0079]

[0080] Among them, f i (·) represents the local function corresponding to the factor node, X i Let X be the set of state variables corresponding to the sensor factor nodes, and let X be the set of all state variable nodes of the navigation system.

[0081] The posterior probability density function p(X|Z) of the navigation state variables and the measurements of each sensor is proportional to the product of all factor nodes, thus formula (26) holds:

[0082]

[0083] Where X is the set of state variables, and Z is the set of sensor measurements. Σ is the square of the Mahalanobis distance. i Let z be the corresponding covariance matrix. i It is the factor node θ i The measured values ​​at each point. Therefore, the problem of fusing the data from the various sensors is to solve for the maximum a posteriori estimate. According to formula (26), the maximum a posteriori estimate of the system state variables is essentially the solution of the following nonlinear least squares formula, as shown in formula (27):

[0084]

[0085] The factor node fusion problem within the sliding window can be transformed into formula (28) based on the Jacobian matrix:

[0086]

[0087] Where ΔX=[ΔX1,...,ΔX SW [ represents the increment of the navigation state within the sliding window] For the current optimized estimate The residuals obtained from the following observations The Jacobian matrix containing all factor nodes within the sliding window is shown in Equation (29):

[0088]

[0089] in, Represents the state variable X i Jacobian matrix for X j The partial derivatives, Represents measurement information Z i Jacobian matrix for X j The partial derivatives of .

[0090] Compared with the prior art, the present invention, employing the above technical solution, has the following technical effects:

[0091] This invention offers superior performance compared to traditional algorithms in terms of real-time performance and robustness. Because it replaces traditional global factor graph optimization with a dynamic sliding window factor graph incremental smoothing optimization method that adapts to changing window size, it significantly improves the computational efficiency of graph optimization, thus ensuring real-time performance even when processing large-scale data from multiple sensors. Furthermore, this invention uses an auxiliary sensor and inertial navigation to form a combined navigation system for factor graph fusion, enhancing the algorithm's robustness in various complex scenarios. Attached Figure Description

[0092] Figure 1 This is a flowchart of the method of the present invention;

[0093] Figure 2 Optimized runtime for factor plots with different sliding window sizes on the same data;

[0094] Figure 3 To optimize and compare the results with the true values ​​using factor plots with different sliding window size values ​​under the same data;

[0095] Figure 4 To compare the optimization results with the true values ​​using factor plots with different sliding window sizes under the same data in two dimensions;

[0096] Figure 5 The results of the optimization using different sliding window size factor plots and the three-dimensional error comparison of the true value are presented under the same data.

[0097] Figure 6 Comparison of the optimized results of this algorithm, GNSS data, and ground truth data tracks in the northeast-northeast coordinate system;

[0098] Figure 7 Comparison of the optimization results of this algorithm with the three-dimensional position error of GNSS in the northeast celestial coordinate system;

[0099] Figure 8 This is a graph showing the dynamic change of the sliding window size during the optimization process of this algorithm;

[0100] Figure 9 To compare the runtime and RMSE error of the three axes (north, south, east, and west) between this algorithm and the traditional ISAM optimization algorithm. Detailed Implementation

[0101] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings. Examples of the embodiments are shown in the accompanying drawings. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention.

[0102] This invention is a multi-source navigation information fusion method based on online incremental scale factor graphs, and its process is as follows: Figure 1 As shown, it includes the following steps:

[0103] Step 1: Acquire the angular velocity and acceleration output data of the inertial navigation system. Step 1 specifically includes the following steps:

[0104] Step 1.1: Collect accelerometer measurement data from the inertial navigation system in the carrier. The accelerometer output data is shown in the following formula:

[0105]

[0106] Among them, f n The specific force output by the accelerometer of the inertial navigation system. Let ω be the acceleration of the carrier relative to the Earth. ie Let ω be the angular velocity of Earth's rotation. en V is the angular velocity of the carrier relative to the Earth. en Let g be the velocity of the carrier relative to the Earth, and g be the gravitational acceleration of the Earth at the location of the carrier.

[0107] Step 1.2: Collect gyroscope measurement data from the inertial navigation system in the carrier. The gyroscope output data is shown in the formula:

[0108]

[0109] in, The projection of the angular velocity of the gyroscope of the carrier system relative to the inertial frame onto the carrier system. Let be the projection of the angular velocity of the geographic frame relative to the inertial frame onto the carrying frame. Let be the projection of the angular velocity of the navigation frame relative to the geographic frame onto the vehicle frame. The projection of the angular velocity of the carrying system relative to the geographic system onto the carrying system;

[0110] Step 2: Acquire position, velocity, and attitude data from the auxiliary sensors. Step 2 specifically includes the following steps:

[0111] Step 2.1: Collect the position p output by the auxiliary sensor on the carrier. assi speed v assi ,attitude The specific information collected is shown in the formula:

[0112] p assi =[p east ,p north ,p up ]

[0113] v assi =[v east ,v north ,v up ]

[0114]

[0115] Where, p east ,p north ,p up These represent the eastward, northward, and celestial positions in the northeast-sky coordinate system obtained by the auxiliary sensor, respectively. east ,v north ,v up denoted as eastward velocity, northward velocity, and celestial velocity in the northeast-sky coordinate system obtained by auxiliary sensors, respectively; r, p, and y represent the roll angle, pitch angle, and yaw angle obtained by auxiliary sensors, respectively.

[0116] Step 3 involves constructing a factor map using the inertial navigation system and auxiliary sensors. The specific steps of step 3 include the following:

[0117] Step 3.1: Based on the angular velocity and acceleration measurements of the inertial navigation system acquired in Step 1, construct the IMU pre-integration factor and update it for one pre-integration cycle t. k ~t k+1 Measurement data within t k Pre-integration is performed under the vehicle system at time t, and the formulas for calculating the position, velocity, and attitude increments are shown below:

[0118]

[0119] in, and Represent the coordinate system of the carrier at time t to t k The rotation matrix and rotation rate matrix of the carrier coordinate system at any given time, f t b and These represent the accelerometer and gyroscope measurements at the current time t, respectively. and These represent the deviations of the accelerometer and gyroscope at the current time t, respectively.

[0120] Based on the dead reckoning principle of INS, t is obtained. k+1The carrier's position, velocity, and attitude at any given moment are shown in the formula:

[0121]

[0122] in, and They represent t respectively k The position, velocity, and attitude of the carrier in the navigation coordinate system at any given moment, g n For t k+1 Gravity vector in the navigation coordinate system at any given time Indicates t k The rotation matrix from the carrier coordinate system to the navigation coordinate system at any given time.

[0123] The IMU pre-integration factor is thus obtained as shown in the formula:

[0124]

[0125] Where, x k+1 and x k They represent t respectively k+1 Time and t k The position, velocity, and attitude state variables at time α k Indicates the inertial navigation device at t k The deviation variable at time, z k Indicates t k The inertial device measurement value at time t, h(x) k ,α k ,z k ) represents t obtained from the state equation k+1 The predicted navigation state values ​​at time x, compared with the current estimated values. k+1 The difference is the error function that needs to be minimized, represented by the IMU pre-integration factor, and d(·) is the cost function.

[0126] Step 3.2: Based on the auxiliary sensor measurement data described above, and utilizing the velocity, position, and attitude information provided by the auxiliary sensor, construct the auxiliary sensor factor, and its measurement equation. As shown in the formula:

[0127]

[0128] Among them, h assi (·) is t k Measurement model of the time-assisted sensor, n assi The measurement noise is denoted by the auxiliary sensor. The auxiliary sensor factor, constructed using the auxiliary sensor, is shown in the formula:

[0129]

[0130] Where d[·] is the cost function and err(·) is the error function.

[0131] Step 4 introduces a dynamic sliding window method that adapts to the size of the estimation accuracy window to achieve incremental updates of the factor graph. Using the circular probability error calculated from the covariance matrix during fusion as the standard, when a pre-set accuracy threshold is higher or lower than a certain proportion of the circular probability error, the size of the factor graph optimization window is reduced or increased to ensure optimization accuracy while improving the optimization efficiency of traditional factor graphs. The specific steps of Step 4 include the following:

[0132] Step 4.1: First, to balance computational complexity and navigation accuracy, the desired accuracy in the horizontal x and y directions is pre-set as D = (d x ,d y Let the deviation of the maximum circular probability error be (δ). x ,δ y Then, calculate the circular probability error with the desired accuracy as the center and the deviation as the radius of 50% and 95%, as shown in the formula:

[0133] CEP = 0.589 (δ) x +δ y )

[0134] CEP95=1.2272(δ x +δ y )

[0135] Then, circles C1 and C2 are drawn with the expected accuracy D as the center and the circular probability error CEP and CEP95 as the radii. These circles are used as the basis for increasing or decreasing the size of the sliding window in the future, as shown in formula (10):

[0136]

[0137] Step 4.2: Construct the factor graph according to Step 3. In order to obtain higher optimization accuracy, first accumulate a certain number of factor nodes for optimization, that is, the number of factor nodes is denoted as pri_accu; optimize the number of factor nodes pri_accu once, and the obtained value is used as the prior value first_prior of the subsequent dynamic sliding window method.

[0138] Then, using the prior value first_prior, the number of factor nodes that meet the preset sliding window size SW_first is added to the sliding window to perform multi-sensor factor graph optimization.

[0139] The weight matrix is ​​obtained by using the inverse Ω of the optimized covariance matrix to calculate the mean deviation in the x and y directions. Ω is shown in the formula:

[0140]

[0141] Where θ is the heading angle, Ω xx ,Ω yy ,Ω θθ These are the variances in the x, y, and θ directions, respectively, Ω xy ,Ω xθ ,Ω yθ These are the covariances of x and y, x and θ, and y and θ, respectively. These are the transposes of the covariances of x and y, x and θ, and y and θ, respectively, and the weights w between two factor nodes i and j. ij Expressed using a formula:

[0142]

[0143] Where det(·) is the determinant of the matrix, the weight matrix W between factor nodes is obtained using the above formula, and the mean weights of the x and y two-dimensional planes in this stage are obtained using the following formula.

[0144]

[0145] Where R is a diagonal matrix with r as its diagonal. i =∑ j w ij The diagonal values ​​r1 and r2 are the mean deviations. z is the current measurement value, and λ is the set coefficient.

[0146] Step 4.3, when the mean deviation If the error is within 50% circular error, the optimized result has high accuracy; if the sliding window size is larger than the preset minimum sliding window size SW, then... min Therefore, it is necessary to keep the number of sensor factor nodes behind the sliding window unchanged, while removing the sensor factor nodes in front of the sliding window, as shown in the formula:

[0147]

[0148] Where SW is the current size of the sliding window. Considering that if the accuracy is too high, formula (14) needs to be calculated repeatedly, which increases the computational complexity of the algorithm, the sliding window size SW_lose is reduced once when the accuracy threshold is more than 50% of CEP to reduce the computational complexity, as shown in the formula:

[0149]

[0150] Combining the two formulas above, we can obtain the general formula for the sliding window size reduction as follows:

[0151]

[0152] As the sliding window moves forward and removes factor nodes at the edge of the sliding window, to prevent information loss during the removal of edge factor nodes, edge factor nodes are transformed into prior information through edgeification; a factor node θ is removed from the factor graph. i This is equivalent to marginalizing factor nodes θ from the joint probability density function. i From the perspective of probability density, the marginalization factor node θ i This is also equivalent to the factor node θ i The integral is calculated using the formula shown below:

[0153]

[0154] Where Θ represents the set of state variables, and p(·) represents the joint probability density function. Formula (17) represents the marginalization process, for the final state node θ n The integral only needs to be obtained by taking θ from the end of the product of the joint probability distributions. n Discarding the formula, the calculation is as follows:

[0155]

[0156] Step 4.4, when the mean deviation If the error exceeds 95% circular error, it indicates that the accuracy of the optimized result is low; if the sliding window size is smaller than the preset maximum sliding window size SW at this time... max In this case, the nodes in front of the sliding window do not need to be removed; instead, new sensor factor nodes only need to be added to the sliding window, as shown in the formula:

[0157]

[0158] Considering that if the accuracy is too low, the above formula needs to be calculated repeatedly, increasing the computational complexity of the algorithm; therefore, when the accuracy threshold is higher than 50% of CEP95, the sliding window size SW_increase is increased once to reduce computational complexity, i.e., the formula is:

[0159]

[0160] Combining the two formulas above, we can obtain the general formula for increasing the sliding window size as follows:

[0161]

[0162] Step 4.5, when the average deviation obtained above... Beyond 50% circular error, within 95% circular error, or when the sliding window size reaches the preset minimum size SW.min Or the largest size SW max When it stops decreasing or increasing, that is, the formula is:

[0163] SW remains unchanged

[0164] Step 5: Based on the determined sliding window size, perform incremental sliding optimization of the factor graph. This involves processing discarded factor nodes and newly added factor nodes on the Bayesian network during sliding, thus achieving a dynamic sliding window factor graph optimization method that adapts to the changing window size based on navigation accuracy. The specific steps of step 5 include the following:

[0165] Step 5.1: Obtain the specific size SW of the sliding window based on the above equation, and update the factor graph model through incremental inference. The cost function of the factor graph within the sliding window is of two types. If it is a priori factors, the cost function of the factor graph is as shown in the equation:

[0166]

[0167] in, For prior information on position, velocity, and attitude, This refers to the zero-bias drift prior information for the gyroscope and acceleration.

[0168] If it is not a priori factor, then the cost function of the factor graph is as follows:

[0169]

[0170] in, The position, velocity, and attitude increments Δp obtained for each IMU pre-integration factor within a certain time interval within the sliding window. i ,Δv i ,Δr i and corresponding gyroscope and accelerometer zero bias increment SW represents the size of the sliding window, and N represents the number of auxiliary sensors other than the inertial navigation system within the corresponding time period. This is the cost function for other auxiliary sensors.

[0171] Step 5.2: Decompose the cost function using a factor graph model so that each factor node corresponds to a sensor, thus obtaining the simplified maximum a posteriori estimate of the navigation state as shown in the formula:

[0172]

[0173] Among them, f i (·) represents the local function corresponding to the factor node, X iLet X be the set of state variables corresponding to the sensor factor nodes, and let X be the set of all state variable nodes of the navigation system.

[0174] The posterior probability density function p(X|Z) of the navigation state variables and the measurements of each sensor is proportional to the product of all factor nodes, i.e., the formula is:

[0175]

[0176] Where X is the set of state variables, and Z is the set of sensor measurements. Σ is the square of the Mahalanobis distance. i Let z be the corresponding covariance matrix. i It is the factor node θ i The measured values ​​at each point. Therefore, the problem of fusing the data from the various sensors is to solve for the maximum a posteriori estimate. According to formula (26), the maximum a posteriori estimate of the system state variables is essentially solving the following nonlinear least squares formula, as shown in the formula:

[0177]

[0178] The factor node fusion problem within the sliding window can be transformed into the following formula based on the Jacobian matrix:

[0179]

[0180] Where ΔX=[ΔX1,...,ΔX SW [ represents the increment of the navigation state within the sliding window] For the current optimized estimate The residuals obtained from the following observations The Jacobian matrix contains all factor nodes within the sliding window, as shown in the formula:

[0181]

[0182] in, Represents the state variable X i Jacobian matrix for X j The partial derivatives, Represents measurement information Z i Jacobian matrix for X j The partial derivatives of .

[0183] Example:

[0184] The implementation example is based on the following simulation environment: using inertial navigation as the core sensor and GNSS as the auxiliary sensor, the dynamic sliding window factor graph optimization algorithm under inertial navigation and GNSS integrated navigation is verified using inertial navigation data and GNSS data obtained through both actual sensor data and simulation data.

[0185] The algorithm is used to optimize factor graphs under a dynamic sliding window as an example. Validation results using actual sensor data are shown below. Figure 2 As shown in Figures 3, 4, and 5, the optimized runtime using different sliding window size factor plots is as follows: Figure 2 As shown, the results of optimization using different sliding window size factor plots and the 3D comparison with the ground truth are as follows: Figure 3 As shown, the results of the two-dimensional comparison between the optimization using different sliding window size factor plots and the true values ​​are as follows: Figure 4 As shown, the results of optimization using different sliding window size factor plots and the comparison of the true 3D error are as follows: Figure 5 As shown.

[0186] The algorithm is used to optimize factor graphs under a dynamic sliding window as an example. The validation results using simulation data are as follows: Figure 6 As shown in Figures 7, 8, and 9. The simulation data uses a pre-defined 300-second vehicle trajectory. From seconds 51 to 110, the GNSS northeast-sky position error is set to 10 meters, 10 meters, and 2 meters; from seconds 161 to 200, the error is set to 30 meters, 30 meters, and 4 meters; and from seconds 241 to 270, the error is set to 50 meters, 50 meters, and 5 meters. A comparison of the tracks of this algorithm, GNSS data, and ground truth data in the northeast-sky coordinate system is shown below. Figure 6 As shown, the three-axis errors of this algorithm and GNSS are compared in the northeast-northeast coordinate system. Figure 7 As shown, the sliding window size changes during algorithm optimization. Figure 8 As shown, the comparison results between this algorithm and the traditional ISAM algorithm are as follows: Figure 9 As shown.

[0187] The algorithm first accumulates a certain number of factor nodes for the first optimization to increase the accuracy of the initial values. Then, a sliding window is used to control the number of factor graph nodes added in subsequent optimization processes. Figure 2 As can be seen, with the same data, the optimization time varies depending on the size of the sliding window, and the larger the sliding window, the longer the optimization time. From Figure 3 and Figure 4 As can be seen, regardless of whether it's a three-dimensional or two-dimensional coordinate system, the smaller the sliding window size, the greater the error between the estimated trajectory and the true GNSS value. From the above... Figure 2 As can be seen in sections 3 and 4, the larger the sliding window size, the longer the optimization time, and the more accurate the optimization, which is consistent with the theoretical part of this patent. Figure 5 The relationship between the sliding window size and the factor plot optimization accuracy is clearly shown in the x, y, and z directions, further supporting the above conclusion. From Figure 6 and Figure 7It can be seen that this algorithm can adaptively adjust the size of the sliding window to achieve a balance between optimization time and optimization accuracy when faced with GNSS noise set in the simulation data. Figure 8 This demonstrates how the sliding window size adapts to different time periods. Figure 9 Comparing this algorithm with the traditional ISAM algorithm, it can be seen that the algorithm has a shorter running time than the traditional method without the sliding window algorithm, and the root mean square error of the three axes of the Northeast Sky is comparable to or even lower than that of the traditional method, which demonstrates the effectiveness of the theory of this patent.

[0188] Those skilled in the art will understand that, unless specifically stated otherwise, the singular forms “a,” “an,” “the,” and “the” used herein may also include the plural forms. It should be further understood that the term “comprising” as used in this specification means the presence of the stated features, integers, steps, operations, elements, and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or groups thereof. It should be understood that when we say an element is “connected” or “coupled” to another element, it can be directly connected or coupled to the other element, or there may be intermediate elements. Furthermore, “connected” or “coupled” as used herein can include wireless connections or couplings. The term “and / or” as used herein includes any and all combinations of one or more of the associated listed items.

[0189] It will be understood by those skilled in the art that, unless otherwise defined, all terms used herein (including technical and scientific terms) have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. It should also be understood that terms such as those defined in general dictionaries should be understood to have the same meaning as in the context of the prior art, and should not be interpreted in an idealized or overly formal sense unless defined as herein.

[0190] It will be understood by those skilled in the art that this invention may relate to apparatus for performing one or more of the operations described in this application. The apparatus may be specifically designed and manufactured for the desired purpose, or may include known devices in general-purpose computers with programs stored therein that can be selectively activated or reconfigured. Such computer programs may be stored in a device (e.g., computer)-readable medium or in any type of medium suitable for storing electronic instructions and coupled to a bus, including but not limited to any type of disk (including floppy disks, hard disks, optical disks, CD-ROMs, and magneto-optical disks), random access memory (RAM), read-only memory (ROM), electrically programmable ROM, electrically erasable ROM (EPROM), electrically erasable programmable ROM (EEPROM), flash memory, magnetic cards, or optical cards. A readable medium includes any mechanism for storing or transmitting information in a form readable by a device (e.g., computer). For example, readable media include random access memory (RAM), read-only memory (ROM), disk storage media, optical storage media, flash memory devices, signals propagated in electrical, optical, acoustic, or other forms (e.g., carrier waves, infrared signals, digital signals), etc.

[0191] Those skilled in the art will understand that each box in these structure diagrams and / or block diagrams and / or flow diagrams, as well as combinations of boxes in these structure diagrams and / or block diagrams and / or flow diagrams, can be implemented using computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing method to generate a machine, thereby creating, through execution by the processor of the computer or other programmable data processing method, methods specified in the boxes of the structure diagrams and / or block diagrams and / or flow diagrams.

[0192] Those skilled in the art will understand that the steps, measures, and schemes in the various operations, methods, and processes discussed in this invention can be alternated, modified, combined, or deleted. Furthermore, other steps, measures, and schemes in the various operations, methods, and processes discussed in this invention can also be alternated, modified, rearranged, decomposed, combined, or deleted. Furthermore, steps, measures, and schemes in the prior art that are similar to those disclosed in this invention can also be alternated, modified, rearranged, decomposed, combined, or deleted.

[0193] The above embodiments are merely illustrative of the technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. Any modifications made to the technical solutions based on the technical concept proposed in this invention shall fall within the scope of protection of this invention.

Claims

1. A multi-source navigation information fusion method based on online incremental scale factor graphs, characterized in that, The method includes the following steps: Step 1: Collect the angular velocity and acceleration output data of the inertial navigation system in the carrier; Step 2: Collect position, velocity, and attitude data from the auxiliary sensors in the carrier; Step 3: Construct a factor map based on the inertial navigation system and auxiliary sensors; Step 4: Introduce a dynamic sliding window method that adapts to the size of the estimation accuracy window to achieve incremental updates of the factor graph. The circular probability error calculated from the covariance matrix during fusion is used as the standard. When the preset accuracy threshold is higher or lower than a certain proportion of the circular probability error, the size of the factor graph sliding window is reduced or increased. Step 5: Based on the determined sliding window size, perform incremental sliding optimization of the factor graph, and process the factor nodes that are dropped and the newly added factor nodes during sliding on the Bayesian network. The implementation process of step 1 is as follows: Step 1.1: Collect accelerometer measurement data of the inertial navigation system in the carrier. The output data of the accelerometer is shown in formula (1): ； in, The specific force output by the accelerometer of the inertial navigation system. The acceleration of the carrier relative to the Earth. The angular velocity of Earth's rotation. The angular velocity of the carrier relative to the Earth. The speed of the carrier relative to the Earth. This represents the gravitational acceleration of the Earth at the location of the carrier. Step 1.2: Collect gyroscope measurement data from the inertial navigation system in the carrier. The output data of the gyroscope is shown in formula (2): ； in, The projection of the angular velocity of the gyroscope of the carrier system relative to the inertial frame onto the carrier system. Let be the projection of the angular velocity of the geographic frame relative to the inertial frame onto the carrying frame. Let be the projection of the angular velocity of the navigation frame relative to the geographic frame onto the vehicle frame. The projection of the angular velocity of the carrying system relative to the geographic system onto the carrying system; The implementation process of step 2 is as follows: Step 2.1: Collect the position output from the auxiliary sensors on the carrier. ,speed ,attitude Information, specifically the information collected, is shown in formula (3): ； in, These represent the eastward, northward, and celestial positions in the northeast-sky coordinate system obtained by auxiliary sensing, respectively. These represent the eastward velocity, northward velocity, and celestial velocity in the northeast-sky coordinate system obtained by the auxiliary sensor, respectively. These represent the roll angle, pitch angle, and yaw angle obtained from auxiliary sensor data acquisition, respectively.

2. The multi-source navigation information fusion method based on online incremental scale factor graphs according to claim 1, characterized in that... The implementation process of step 3 is as follows: Step 3.1: Based on the angular velocity and acceleration measurements of the inertial navigation system acquired in Step 1, construct the IMU pre-integration factor and update it for one pre-integration cycle. Measurement data within Pre-integration is performed under the time-based system to obtain the position increment. Speed ​​increment attitude increment The calculation formula is shown in (4): ； in, and They represent the current time. The carrier coordinate system to The rotation matrix and rotation rate matrix of the carrier coordinate system at any given time. and They represent the current time. The measurements from the accelerometer and gyroscope, and They represent the current time. The deviation of the accelerometer and gyroscope; Based on the dead reckoning principle of INS, we obtain The position, velocity, and attitude of the carrier at any given time are shown in formula (5): ； in, , and They represent The position, velocity, and attitude of the carrier in the navigation coordinate system at any given moment. for Gravity vector in the navigation coordinate system at any given time express The rotation matrix from the carrier coordinate system to the navigation coordinate system at any given time; The IMU pre-integration factor is thus obtained as shown in formula (6): ； in, and They represent Time and Position, velocity, and attitude state variables at any given moment. Indicating inertial navigation devices in Deviation variable at time, express The inertial device measurement value at time _____. The expression obtained from the state equation The predicted values ​​of navigation state variables at time t, compared with the current estimates. The difference is the error function that needs to be minimized, represented by the IMU pre-integration factor. The cost function; Step 3.2: Construct the auxiliary sensor factor and its measurement equation. As shown in formula (7): ； in, yes The measurement model of the constant-time auxiliary sensor, To account for the measurement noise of the auxiliary sensor, the auxiliary sensor factor constructed using the auxiliary sensor is shown in formula (8): ； in, Let cost function be This is the error function.

3. The multi-source navigation information fusion method based on online incremental scale factor graphs according to claim 2, characterized in that... The implementation process of step 4 is as follows: Step 4.1: First, in order to balance computational complexity and navigation accuracy, the horizontal position is preset. and The expected accuracy in the direction is Let the deviation of the maximum circular probability error be... Then, the circular probability error with the desired accuracy as the center and the deviation as the radius of 50% and 95% is calculated, as shown in formula (9): ； Then, with the desired accuracy Centered on the circle, the circular probability error and Draw a circle with radius 1. This serves as the basis for subsequently increasing or decreasing the size of the sliding window, as shown in formula (10): ； Step 4.2: Construct a factor graph based on Step 3. To improve optimization accuracy, first accumulate a certain number of factor nodes for optimization, that is, denoted as the number of factor nodes. ; Number of factor nodes First, perform an optimization, and use the resulting value as the prior value for the subsequent dynamic sliding window method. ; Then, use prior values. To fit the preset sliding window size The number of factor nodes is added to the sliding window for multi-sensor factor graph optimization; Based on the inverse of the optimized covariance matrix To obtain the weight matrix for calculation Mean deviation in direction ; As shown in formula (11): ； in, It is the heading angle. They are Variance in direction, They are and , and , and covariance, They are and , and , and The transpose of the covariance, two factor nodes Weights between Expressed using formula (12): ； in, Given the determinant of the matrix, the weight matrix between factor nodes is obtained using formula (12). Then, the value of this stage is obtained through formula (13). and Weighted mean of a two-dimensional plane : ； in, It is a diagonal matrix and its diagonal is its diagonal and That is, the mean deviation. , The current measurement value, The set coefficient; Step 4.3: When the mean deviation If the error is within 50% circular error, the optimized result has high accuracy; if the sliding window size is larger than the preset minimum sliding window size... Then, it is necessary to keep the number of factor nodes behind the sliding window unchanged, while removing the factor nodes in front of the sliding window, as shown in formula (14): ； in, Given the current sliding window size, considering that if the precision is too high, formula (14) needs to be calculated repeatedly, which increases the computational complexity of the algorithm; therefore, a precision threshold is set to be higher than the current size. When the value is above 50%, reduce the sliding window size all at once. To reduce computational complexity, as shown in formula (15): ； Combining formulas (14) and (15), the overall formula for the decrease in the sliding window size is shown in formula (16): ； As the sliding window moves forward and removes factor nodes at the edge of the sliding window, to prevent information loss during the removal of edge factor nodes, edge factor nodes are transformed into prior information through edgeification; a factor node is then removed from the factor graph. This is equivalent to marginalizing factor nodes from the joint probability density function. From the perspective of probability density, marginalization factor nodes This is also equivalent to the factor node Integrating, the calculation formula is shown in equation (17): ； in, Represents a set of state variables. The joint probability density function is represented by equation (17), which represents the marginalization process for the final state node. The integral only needs to be obtained from the end of the product of the joint probability distributions. Discarding the formula, the calculation formula is shown in formula (18): ； Step 4.4: When the mean deviation If the error exceeds 95% circular error, it indicates that the optimized result has low accuracy; if the sliding window size is smaller than the preset maximum sliding window size at this time. Then, the nodes in front of the sliding window do not need to be removed, but only new factor nodes need to be added to the sliding window, as shown in formula (19): ； Considering that if the precision is too low, the above formula needs to be calculated repeatedly, increasing the computational complexity of the algorithm; therefore, a precision threshold is set to be higher than... When the value is above 50% of 95, increase the sliding window size all at once. To reduce computational complexity, as shown in formula (20): ； Combining formulas (19) and (20), the general formula for the increase of the sliding window size is shown in formula (21): ； Step 4.5: When the average deviation obtained above... Beyond 50% circular error probability, within 95% circular error probability, or when the sliding window size reaches the preset minimum size. Or maximum size Then it will no longer decrease or increase, as shown in formula (22): (22)。 4. The multi-source navigation information fusion method based on online incremental scale factor graphs according to claim 3, characterized in that... The implementation process of step 5 is as follows: Step 5.1: Obtain the specific dimensions of the sliding window based on the above equation. The factor graph model is updated through incremental inference: the cost function of the factor graph within the sliding window is of two types. If it is a priori factor, the cost function of the factor graph is as shown in equation (23). ； in, For prior information on position, velocity, and attitude, This refers to the zero-bias drift prior information for the gyroscope and acceleration. If it is not a priori factor, then the cost function of the factor graph is as shown in equation (24): ； in, The position, velocity, and attitude increments obtained for each IMU pre-integration factor within a certain time interval within the sliding window. and corresponding gyroscope and accelerometer zero bias increment , The size of the sliding window. This refers to the number of auxiliary sensors other than inertial navigation systems within a given time period. The cost function for other auxiliary sensors; Step 5.2 The cost function is decomposed using the factor graph so that each factor node corresponds to a sensor, thus obtaining the simplified maximum a posteriori estimate of the navigation state as shown in Equation (25): ； in, For the local function corresponding to the factor node, This is the set of state variables corresponding to the sensor factor nodes. It is the set of all state variable nodes in the navigation system; The posterior probability density function of navigation state variables and various sensor measurements Proportional to the product of all factor nodes, we have formula (26): ； in, It is a set of state variables. It is a set of sensor measurements. The square of the Mahalanobis distance. The corresponding covariance matrix is... It is a factor node The measurement values ​​at each location; therefore, the fusion problem of the various sensors is to solve for the maximum a posteriori estimate. According to formula (26), the maximum a posteriori estimate of the system state variables is essentially the solution of the following nonlinear least squares formula, as shown in formula (27): ； The factor node fusion problem within the sliding window can be transformed into formula (28) based on the Jacobian matrix: ； in, This represents the increment of the navigation state within the sliding window. For the current optimized estimate The residuals obtained from the following observations The Jacobian matrix containing all factor nodes within the sliding window is shown in Equation (29): (29)； in, Represents state variables Jacobian matrix pair The partial derivatives, Indicates measurement information Jacobian matrix pair The partial derivatives of .

Images