A fusion navigation method of geomagnetic vector and INS based on multidimensional constraint factor graph

Abstract

The present invention proposes a geomagnetic vector and INS fusion navigation method based on a multidimensional constraint factor graph, comprising the following steps: step 1, constructing an offline geomagnetic fingerprint library; step 2, constructing a position and velocity model; step 3, obtaining the acceleration vector and angular velocity vector parts according to the inertial sensor, and introducing a multidimensional constraint term; step 4, constructing an objective function, and then reconstructing the output of step 1-step 3 according to the process of updating the variables in the objective function according to the position to obtain a differential model and an overall factor graph model; step 5, using the differential model of the objective function, using momentum and second-order moment estimation to update the parameters of the overall factor graph model; step 6, repeating step 5 for each moment coordinate in the navigation history sequence in a sliding window manner to obtain the optimal estimated navigation trajectory; step 7, applying the optimized overall factor graph model to update the predicted position. Higher positioning accuracy and stronger anti-interference performance can be achieved.

Description

Technical Field

[0001] The present invention relates to the field of navigation and positioning technology, and in particular to a geomagnetic vector and INS fusion navigation method based on a multi-dimensional constraint factor graph. Background Art

[0002] The Earth's magnetic field is generated by electric currents inside the Earth. Its shape is similar to a huge magnetic dipole, with magnetic field lines flowing from the South Pole to the North Pole. The strength and direction of the Earth's magnetic field vary depending on the geographical location. It is usually weaker near the equator and stronger near the poles. The difference in its strength in different geographical locations and its relative stability in time and space make geomagnetic navigation a positioning method with broad application prospects in the field of navigation. Most of the current geomagnetic matching methods are based on the idea of ​​fingerprint matching, which collects the geomagnetic field of the target area and then matches the geomagnetic data measured in real time by navigation with it. In addition, current geomagnetic positioning is often combined with other positioning technologies to form a multi-source fusion positioning technology. For example, combining geomagnetic sensors with inertial navigation systems can overcome the characteristics of inertial navigation systems accumulating errors over time and improve overall positioning accuracy.

[0003] At present, the navigation technology that integrates geomagnetism and other sensors still has problems such as low geomagnetic information, high trajectory calculation failure rate, and low efficiency in correcting abnormal sensors. There are also studies on increasing the amount of information carried by geomagnetic information by increasing the dimension of geomagnetic information, but the correction ability for abnormal sensors is weak and the gradient diverges.

[0004] The navigation system of multi-sensor data fusion can be summarized into three major systems:

[0005] The first is the Kalman filter based on the recursive process and its derived algorithms. However, the calculation complexity for high-dimensional systems will increase significantly. At the same time, the Kalman filter is highly dependent on fixed system models and noise parameters, and may diverge when the initial state estimation is inaccurate or the error is large.

[0006] Faced with highly nonlinear systems and possible non-Gaussian distribution noise models in complex environments, the particle filter algorithm based on the Monte Carlo method approximates the probability distribution by using multiple randomly sampled particles and estimates the system state by weighted averaging of particles. In this process, as time goes by, the weights of most particles will gradually decrease, resulting in reduced estimation accuracy.

[0007] In order to overcome the disadvantages of the two widely used combined navigation algorithms mentioned above, the factor graph model has become increasingly popular in recent years. Its undirected structure based on the traditional Bayesian network allows different sensor data to be flexibly added to the graph as factors without special assumptions. The optimization idea based on the maximum a posteriori estimation allows the factor graph to globally optimize the entire state sequence. In long-term navigation tasks, it can simultaneously consider current and historical data and perform global consistency optimization to reduce cumulative errors. However, this global optimization idea that aims to minimize the objective function requires a lot of computing resources. When dealing with high-dimensional state spaces and large sensor errors, this idea often leads to matching failures, gradient divergence of the overall objective function, and abnormal solution time, affecting the accuracy of positioning. Summary of the invention

[0008] The purpose of the present invention is to propose a geomagnetic vector and I NS fusion navigation method based on a multidimensional constraint factor graph to address the shortcomings of the prior art, and to use a factor graph modeling method that performs constraints in a cross-time step manner to ensure that the optimization of each epoch in the iterative process will take into account the influence of global attitude and velocity, while reducing the influence of outliers on the overall navigation sequence residuals through gradient association in the face of random anomalous geomagnetic areas.

[0009] In order to solve the above technical problems, the technical solution of the present invention is:

[0010] A geomagnetic vector and INS fusion navigation method based on a multi-dimensional constraint factor graph comprises the following steps:

[0011] Step 1: uniformly collect geomagnetic data of each axis in the navigation space according to the navigation coordinate system, and expand the discrete magnetic field intensity in the geomagnetic data to obtain an offline geomagnetic fingerprint library;

[0012] Step 2: construct the system state of the combined navigation according to the position of the pedestrian in the navigation coordinate system at a certain moment in accordance with the northeast sky, and then construct the position and speed model according to the system state;

[0013] Step 3: Obtain the acceleration vector and angular velocity vector through the inertial sensor, then build a pre-integration model based on the acceleration vector and angular velocity vector, and introduce multi-dimensional constraint items;

[0014] Step 4: First, construct the objective function, and then reconstruct the outputs of steps 1 to 3 by updating the variables in the objective function according to the position to obtain the differential model and the overall factor graph model;

[0015] Step 5: Using the reconstructed differential model, the momentum and second-order moment estimation are used to update the parameters of the overall factor graph model;

[0016] Step 6: Repeat step 5 for each moment coordinate in the navigation history sequence in a sliding window manner to obtain the optimal estimated navigation trajectory;

[0017] Step 7: Apply the optimized overall factor graph model to update the predicted position.

[0018] Preferably, in step 1, the method for collecting geomagnetic data is: first determine the location p of a certain point in the region k =(x k ,y k ), the matching area is a square with four vertex coordinates as follows:

[0019]

[0020] in, Indicates rounding down, m1, m2, m3, and m4 indicate the point (x k ,y k ) are the coordinates of the four vertices of the rectangle after rounding up and down. The intermediate matrix P is calculated based on the partial derivatives and cross partial derivatives of the magnetic field intensity in the X and Y directions at the same position:

[0021]

[0022] Where g(·) represents the magnetic field strength at the current point, g x (·) represents the geomagnetic partial derivative of the current point in the x direction, g y (·) represents the geomagnetic derivative of the current point in the y direction, g xy (·) represents the composite partial derivative of the current point in the x and y directions. The coefficient matrices on both sides are provided by the cubic convolution interpolation algorithm and are substituted as known parameters.

[0023] Preferably, in step 1, continuous expansion is performed by interpolation, and the expression is as follows:

[0024]

[0025] The geomagnetic interpolation sequence of the navigation prediction sequence, the east-to-north derivatives and the acquired geomagnetic data are stored for subsequent gradient descent operations.

[0026] In the above technical solution, an offline geomagnetic fingerprint library is established in the navigation coordinate system. Using a three-axis magnetometer, the directions of the magnetometer are aligned with the navigation coordinate system, and the geomagnetic data of multiple time periods in a day are collected with a resolution of one meter and averaged in different time periods. The collection area should be a rectangular area to ensure reasonable smoothing of the map boundary value.

[0027] The data is collected while walking continuously in the navigation area. The acceleration process of pedestrians walking continuously is modeled as uniform motion per unit time. The measured values ​​of each sensor during the motion process are recorded (three-axis magnetometer, three-axis accelerometer, three-axis angular velocity meter). The original navigation sequence of the inertial device is solved using the INS information by rectangular integration, which is used as the original control group constructed by the residual term and the control group of the algorithm predicted path. In this process, the data is smoothed according to a certain sampling interval per unit time as the data sampling value per unit time to avoid abnormal data at the sampling point.

[0028] Preferably, in step 2, the system state at time k can be expressed as:

[0029] X k =[E k N k U k ]

[0030] Among them, E k ,N k ,U k They represent the east, north and celestial position components of the pedestrian in the navigation coordinate system at time k respectively.

[0031] Preferably, in step 2, assuming that the pedestrian is walking at a normal speed, a position and speed model is constructed according to the rectangular integration principle of the sampling time, which can be expressed as the following formula:

[0032]

[0033] Among them, v x,k ,v y,k ,v z,k represents the current carrier coordinate system velocity accumulated based on the acceleration of each axis, and v E,k ,v N,k ,v U,k It is the projection of the carrier coordinate system velocity in the northeast celestial coordinate system.

[0034] Preferably, in step 3, the acceleration vector mathematical model is as follows:

[0035] a b,k =[a x,k a y,k a z,k ] T

[0036] Among them, a x,k 、a y,k 、a z,k They represent the acceleration of each axis in the carrier coordinate system at time k. The acceleration vector measurement function can be described as:

[0037]

[0038] u k =[u x,k ,u y,k ,u z,k ] T

[0039] Among them, L acc (X k ) indicates that the acceleration vector measurement function is the system prediction value, u k represents the error correction term for the triaxial accelerometer, δ acc Represents the constraint boundary of the three-axis accelerometer;

[0040] The speed of each historical navigation sequence is defined, and the speed vector mathematical model is as follows:

[0041] v k =[v x,k v y,k v z,k ] T

[0042] Among them, v x,k 、v y,k 、v z,k They represent the speed of each axis in the carrier coordinate system at time k. The velocity vector measurement function can be described as:

[0043]

[0044] Among them, L vel (X k ) represents the velocity vector function, It represents the constraints of all acceleration vector constraints on the velocity measurement function at the current moment since the start of the navigation sequence. The overall velocity vector function is the predicted value of the velocity at the current moment.

[0045] The mathematical model of angular velocity vector is as follows:

[0046] ω b,k =[ω x,k ω y,k ω z,k ] T

[0047] Among them, ω x,k ,ω y,k ,ω z,k They represent the angular velocity of each axis in the carrier coordinate system at time k;

[0048] The constraints on the angular velocity vector across time steps are the same as the constraints on the acceleration and are described as follows:

[0049]

[0050] Among them, L gyr (X k ) represents the angular velocity vector, ω b,k represents the angular velocity vector of the navigation carrier rotating around the x, y, and z axes, w k , δ gyr Represents the constraint boundary of the three-axis angular velocity meter; the corresponding heading angle measurement function of the navigation carrier is described as follows:

[0051]

[0052] Among them, L yaw (X k ) indicates that the heading angle measurement function is the predicted value of the heading angle at the current moment.

[0053] In the above technical solution, the measurement values ​​collected in the previous step and averaged at a certain sampling frequency are independently modeled. The three-dimensional angular velocity vector of the gyroscope measurement value and the three-dimensional acceleration vector of the accelerometer measurement value are modeled as measurement vectors, and the corresponding measurement function containing the predicted value is the superposition of the measurement vector and the three-dimensional constraint condition, and the measurement function of each sensor is constrained within a reasonable range. The measurement function of the geomagnetic factor is modeled as a cubic convolution interpolation of the offline geomagnetic fingerprint library as a measurement function containing the predicted value. The above modeling can be approximated to conform to the Gaussian probability distribution in the combined navigation algorithm based on the factor graph model, thereby establishing an objective function based on the maximum a posteriori probability, and in the subsequent steps, the constraint terms are fully utilized.

[0054] Preferably, in step 4, the overall factor graph model is expressed as follows:

[0055]

[0056] in, represent the acceleration factor, angular velocity factor and geomagnetic factor respectively,

[0057] Preferably, the objective function of the overall factor graph model is:

[0058]

[0059] Among them, σ gyr ,σ acc Represent the standard deviation of the gyroscope and accelerometer respectively; G(w,u x ,u y ) represents the composite function corresponding to the geomagnetic factors including all constraints, represents the predicted heading angle, They respectively represent the predicted values ​​of the geomagnetic data of each axis in the northeast celestial coordinate system, and n represents the length of a complete navigation sequence.

[0060] Preferably, the reconstruction method of the differential model is:

[0061] First, for any angular velocity constraint in the objective function, that is, w k Differentiate, where k is any value in a sequence of length n:

[0062]

[0063] For G(w,u x ,u y ) cannot be solved directly. It is necessary to establish the association between the objective function and the gradient of each sensor used to collect geomagnetic data, so that the gradient is transferred from the output sensor level back to the objective function level, and the chain derivation rule is used to solve the gradient of each layer;

[0064] Then, for any constraint term of the x-axis or y-axis acceleration in the objective function, that is, u x,k ,u y,k Differentiate, where k is any value in a sequence of length n:

[0065]

[0066] Build an association between the objective function and each sensor gradient.

[0067] In the above technical solution, the variables in the objective function are reconstructed into a differential model according to the position cumulative update process to reduce the nonlinearity of the objective function. At the same time, the chain derivation rule of complex functions is used to establish the association between the directions of the geomagnetic fingerprint library and the constraints across time steps. Calculate the contribution of each parameter to the loss function (objective function), transfer the calculation error layer by layer, and finally update the parameters through the gradient. The application of the above logic in this method is to first prepare the gradient matrix of the fingerprint library in the east and north directions of the navigation coordinate system, then rotate the coordinates of the predicted value at the current moment to the carrier coordinate system, and establish a coordinate overall update iteration model. The model contains multi-dimensional constraint factors to ensure that the sensor data and position update points are processed in a reasonable range, and prepare the derivatives of each parameter and measurement function to the constraint terms.

[0068] Preferably, the parameter updating expression is as follows:

[0069]

[0070] in, represents the second-order data estimate of the angular velocity constraint term; k represents the current moment, k-1 represents the previous moment; β2 represents the decay rate of the variance term.

[0071] As a preference, a fast gradient descent algorithm is used, and the gradients of each parameter prepared in the previous step are integrated into the overall gradient of the objective function using a sliding window, and then the current moment momentum of the gradient and the exponentially weighted moving average of the gradient are calculated, and the deviation is corrected using the decay rate of the variance to ensure that the momentum and second order are not too small in the early stage of the model iteration, resulting in a slow convergence speed and affecting the estimation accuracy of the initial position. After all navigation sequences are predicted, a complete and unique prediction sequence is obtained.

[0072] The present invention has the following characteristics and beneficial effects:

[0073] By adopting the above technical solution, low-cost inertial sensors and geomagnetic dynamometers can be used to achieve long-term, high-precision, lightweight integrated navigation with strong anti-interference ability and fast solution speed. Compared with the existing geomagnetic inertial device integrated navigation, under the same earth magnetic field and inertial device error model conditions, it can achieve higher positioning accuracy and stronger anti-interference performance, and solve the matching failure problem existing at this stage.

[0074] Compared with the prior art, the present invention has the following advantages:

[0075] (1) Add multi-dimensional sensor rationality constraints, limit the angular velocity and acceleration vector, and maintain the pedestrian posture and velocity projection of the northeast sky at a high confidence level at each moment when updating the position. From the perspective of the sensor, the stability of the overall system is improved. This probabilistic constraint solves the problem of divergent solution trajectories and lengthy solution time caused by sensor errors in traditional factor graphs in inertial integrated navigation.

[0076] (2) Compared with the traditional kinematic model, the small acceleration model of pedestrians' daily walking uses rectangular time integration to calculate the velocity. When the sampling frequency matches the time step during position update, the impact of acceleration deviation on positioning accuracy is reduced. Subsequent experimental verification found that this velocity processing method improves the accuracy compared with trapezoidal integration.

[0077] (3) Introducing cross-time step constraints, the cumulative characteristics of inertial devices indicate that the position update per unit time is affected by all previous rationality constraints. In the gradient descent process, all attitude matrices and accelerations between the current predicted position and the index value are used in a sliding window manner to perform gradient descent on the current predicted position, thus achieving the descent of multiple residual values ​​at one moment.

[0078] (4) The original objective function is established with the original sensor data through the chain rule of differentiation. While avoiding the difficulty of nonlinear function optimization, the objective function is sensitive to slight changes in the gradient and the update space is reduced. In the optimization stage, the fast gradient descent algorithm is combined with the sliding window algorithm to convert the large number of complex Jacobian matrix solutions and matrix sparsification in the cause subgraph model into second-order moment estimation and momentum update of one-dimensional data. The introduced sliding window algorithm performs historical smoothing on the gradient, considers the influence of historical data and local gradient changes in a fixed time period from the multi-dimensional perspective of algorithm optimization and model construction, and makes the navigation solution converge quickly. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0080] Figure 1 Block diagram of the geomagnetic vector and I NS fusion navigation system based on multi-dimensional constraint factor graph.

[0081] Figure 2 The overall principle diagram of interpolation after offline geomagnetic vector collection in the embodiment.

[0082] Figure 3 A schematic diagram of the structure of a multidimensional constraint factor graph in an embodiment.

[0083] Figure 4 Result diagram of predicted experimental path.

[0084] Figure 5 Probability distribution diagram of cumulative error of prediction experiment. DETAILED DESCRIPTION

[0085] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other.

[0086] In the description of the present invention, it should be understood that the terms "center", "longitudinal", "lateral", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside" and the like indicate positions or positional relationships based on the positions or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as limiting the present invention. In addition, the terms "first", "second", and the like are only used for descriptive purposes, and cannot be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, features defined as "first", "second", and the like may explicitly or implicitly include one or more of the features. In the description of the present invention, unless otherwise specified, "multiple" means two or more.

[0087] In the description of the present invention, it should be noted that, unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, or it can be the internal communication of two components. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood by specific circumstances.

[0088] The present invention provides a geomagnetic vector and I NS fusion navigation method based on a multidimensional constraint factor graph, which uses the Dayila precision measurement to screen outliers of inertial sensors and constrain the measured values ​​of the inertial sensors within a reasonable range, so as to avoid neglecting the local rationality of sensor data due to the correction of sensor errors when major sensor errors occur.

[0089] In the geomagnetic matching link, a geomagnetic fingerprint library is established in the northeast celestial coordinate system. In the process of constructing the factor graph objective function, a deep fusion relationship is established between the geomagnetic fingerprint and the angular velocity vector and the acceleration vector. In the matching process, the fusion relationship is used to associate the geomagnetic fingerprint with the regional constraints across time steps, thereby suppressing the influence of abnormal geomagnetic measurements on the residual values ​​of the navigation sequence.

[0090] Cubic convolution interpolation is used when constructing the geomagnetic fingerprint library. The feature prediction value corresponding to the target point is obtained through the influence factor of the feature values ​​of 16 fingerprint collection points on the target point, so as to improve the resolution of the geomagnetic fingerprint library from the perspective of image processing.

[0091] Therefore, the present invention can improve the overall positioning accuracy and stability of the navigation system and reduce the uncertainty existing at the system level.

[0092] like Figure 1As shown, the specific steps are as follows:

[0093] Step 1: Build an offline geomagnetic fingerprint library. Collect geomagnetic data of each axis evenly in the navigation space through various sensors according to the navigation coordinate system in advance, and then expand the discrete magnetic field intensity into a continuous offline geomagnetic fingerprint library by interpolation.

[0094] First, determine the location of a certain point in the area. The matching area is a square with four vertex coordinates as follows:

[0095]

[0096] The intermediate matrix P is calculated based on the partial derivatives and cross partial derivatives of the magnetic field intensity in the X and Y directions at the same position:

[0097]

[0098] Where g(·) represents the magnetic field strength at the current point, g x (·) represents the geomagnetic partial derivative of the current point in the x direction, g y (·) represents the geomagnetic derivative of the current point in the y direction, g xy (·) represents the composite partial derivative of the current point in the x and y directions. The coefficient matrices on both sides are provided by the cubic convolution interpolation algorithm and are substituted as known parameters.

[0099] Further, such as Figure 2 As shown in the figure, the continuous expansion is carried out, and the output interpolation function is a one-dimensional magnetic scalar. The following magnetic vector measurement function can be obtained by interpolating the geomagnetic map in each direction, that is, the predicted value of the geomagnetic part in the factor graph model:

[0100]

[0101] in, represents the interpolation function output by the geomagnetic fingerprint library, L magV Represents the magnetic vector function. The partial derivatives of the magnetic field intensity at the same point in the two coordinate axis directions are:

[0102]

[0103] The geomagnetic interpolation sequence of the navigation prediction sequence, the east-to-north derivatives and the geomagnetic fingerprint library of each axis are stored for subsequent gradient descent operations.

[0104] Step 2: Construct the system state of the integrated navigation. The system state at time k can be expressed as:

[0105] X k =[E k N k U k ] (6)

[0106] Among them, E k ,N k ,U k They respectively represent the position of the pedestrian in the navigation coordinate system at time k that conforms to the northeast sky.

[0107] Furthermore, assuming that the pedestrian is walking at a normal speed, the position and speed model is constructed according to the rectangular integration principle of the sampling time, which can be expressed as the following formula:

[0108]

[0109] Among them, v x,k ,v y,k ,v z,k represents the current carrier coordinate system velocity accumulated based on the acceleration of each axis, and v E,k ,v N,k ,v U,k It is the projection of the carrier coordinate system velocity in the northeast celestial coordinate system.

[0110] In this embodiment, the sampling frequency is set to 1 second to simplify the formula and parameters. The speed and posture matrix can be described by the following formula:

[0111]

[0112] Among them, a x,i ,a y,i ,a z,i is the acceleration value measured by each axis at time i, It is the attitude matrix from the carrier coordinate system to the northeast sky coordinate system.

[0113] It should be noted that in the formula, because in most cases the navigation scenario does not need to consider the impact of the pitch angle and roll angle on the attitude matrix, only the impact of the heading angle is considered, that is, the rotation of the gyroscope z-axis. The heading angle is expressed as follows:

[0114]

[0115] Step 3: Independently model the inertial device measurement function and error terms, that is, model the acceleration vector, angular velocity vector and multi-dimensional constraint terms.

[0116] The acceleration term is described as follows: (11)

[0118]

[0119] u k =[u x,k ,u y,k ,u z,k ]T (13)

[0120] Among them, L acc (X k ) The acceleration vector measurement function is the system prediction value, a b,k Represents the measured value of the system acceleration vector. k represents the error correction term for the triaxial accelerometer, δ acc is the constraint boundary of the acceleration sensor parameters of each axis. In this case, its value is three times the standard deviation of the sensor data. After normalization, it is equal to δ acc Multiplication keeps the error in the measured value within a reasonable range.

[0121] In this embodiment, three times the standard deviation of each axis sensor is selected as a constraint. When updating the position, in order to reduce the complexity of the calculation, the speed of each historical navigation sequence is defined, which is described as follows:

[0122] v k =[v x,k v y,k v z,k ] T (14)

[0123]

[0124] Among them, L vel (X k ) represents the velocity vector function. It represents the constraints of all acceleration vector constraints on the velocity measurement function at the current moment since the start of the navigation sequence; the overall velocity vector function is the predicted value of the velocity at the current moment.

[0125] The constraints on the angular velocity vector across time steps are the same as the constraints on the acceleration and are described as follows:

[0126]

[0127] Among them, L gyr (X k ) represents the angular velocity vector, ω b,k Represents the angular velocity vector of the navigation carrier rotating around the x, y, and z axes. k , δ gyr The meaning expressed is similar to the definition of the acceleration term constraint. To reduce the computational complexity, the heading angle at each moment is defined as follows:

[0128]

[0129] Among them, L yaw (X k) represents the heading angle measurement function, that is, the predicted value of the heading angle, which can be determined by the angular velocity vector and the constraint term at all historical moments.

[0130] Step 4: Reconstruct the predicted values ​​obtained in steps 1, 2, and 3 and map them to the names of the sensor's measurement parameters one by one, and then reconstruct the overall factor graph model for the geomagnetic and inertial factors, which is described as follows:

[0131]

[0132] in, Respectively represent the predicted values ​​of heading angle, velocity vector, geomagnetic vector, angular velocity vector, and acceleration vector. Represents the predicted navigation sequence.

[0133] The update process of the predicted value of the navigation position is the same as formula (7), which is described in more detail as follows (the length of the navigation sequence is n):

[0134]

[0135] in, They represent the predicted values ​​of the east, north, and celestial coordinates at the current moment in the navigation coordinate system respectively. e0, n0, and u0 indicate that the initial position is a known position in this case.

[0136] The overall multidimensional constraint factor graph model can be described as follows through the position solution model:

[0137]

[0138] in, They represent acceleration factor, angular velocity factor and geomagnetic factor respectively. As described in formula (22), the geomagnetic prediction factor is rotated to the carrier coordinate system through the attitude matrix. Through this reverse process, the amount of information carried by the geomagnetic measurement values ​​of each axis in the carrier coordinate system is increased, which can greatly improve the solution success rate when the amount of information carried by the geomagnetic is small.

[0139] Therefore, the objective function can be described as:

[0140]

[0141] Among them, σ gyr ,σ acc Represent the standard deviation of the gyroscope and accelerometer respectively. In this case, the sensor devices with the same specifications are used, so the standard deviation of the same type can be set to the same parameter. For sensors of the same type but different specifications, the parameters here need to be reset as required; G(w,u x ,u y) represents the composite function corresponding to the geomagnetic factors containing all constraints. This transforms the factor graph model into an objective function optimization problem.

[0142] Step 5: For any angular velocity constraint in the objective function, that is, w k Differentiate, where k is any value in a sequence of length n:

[0143]

[0144] The derivative of the geomagnetic component cannot be solved directly. It is necessary to establish the association between the objective function and the gradient of each sensor, so that the gradient is transmitted from the output sensor level back to the objective function level, and the chain derivative rule is used to solve the gradient of each layer.

[0145] The gradient change of the pedestrian posture matrix determined by the heading angle:

[0146]

[0147] Gradient changes in all directions of the three-dimensional geomagnetic map:

[0148]

[0149] Coordinates of each direction for constraint w k The gradient relationship between:

[0150]

[0151] Combining the geomagnetic prediction values ​​in each direction with the gradients of each parameter, we can obtain:

[0152]

[0153]

[0154] Step 6: For any constraint term of the x-axis or y-axis acceleration in the objective function, that is, u x,k ,u y,k Differentiate, where k is any value in a sequence of length n:

[0155]

[0156] Similarly, the association of the objective function to each sensor gradient is established.

[0157] Establishing the gradient association between the position coordinates and the acceleration constraint term can be described as:

[0158]

[0159] Establish the gradient association between geomagnetic interpolation and constraint terms:

[0160] To u x,k :

[0161]

[0162] To u y,k :

[0163]

[0164] Combining the geomagnetic prediction values ​​in each direction with the gradients of each parameter, we can obtain:

[0165]

[0166]

[0167] Step 7: It is necessary to use the differential model reconstructed by the objective function and update the parameters using momentum and second-order moment estimation, which is described as follows:

[0168]

[0169] in, represents the second-order estimate of the angular velocity constraint term; k represents the current moment, k-1 represents the previous moment; β2 represents the decay rate of the variance term, which is set to 0.9 in this embodiment; α represents the learning rate, which is set to 0.001 in this case; ε represents a constant value to prevent the denominator from being 0 during the update.

[0170] Specifically, for the gradient descent using the second-order moment estimation method, in order to solve the problems of adaptive learning rate being too sensitive, missing the optimal point with a large update step size, and local extreme value traps, a fine fixed small step size adjustment of the first-order momentum is performed to control the convergence speed, which is described as follows:

[0171]

[0172] Among them, lr is a fixed learning rate, which is set to 0.00001 in this case. The update of other constraints is similar:

[0173]

[0174]

[0175] Step 8: Repeat steps 5, 6, and 7 for each moment coordinate in the navigation history sequence in a sliding window manner to obtain the optimal solution data. This embodiment is different from the traditional sliding window algorithm. It only uses window design without considering smoothing. Excessive smoothing makes it difficult to capture changes within the window, so the window size is set to 30 and no repeated processing is performed. After the processing is completed, the predicted position is finally updated according to the method in step 4.

[0176] Depend on Figure 4 and Figure 5 It can be proved that the path prediction accuracy of this embodiment is very high, and the original maximum error of the inertial device solution data of 7-8 meters is optimized to within one meter.

[0177] The embodiments of the present invention are described in detail above with reference to the accompanying drawings, but the present invention is not limited to the described embodiments. For those skilled in the art, various changes, modifications, substitutions and variations of these embodiments including components are made without departing from the principles and spirit of the present invention, and still fall within the scope of protection of the present invention.

Claims

1. A geomagnetic vector and INS fusion navigation method based on a multidimensional constraint factor graph, characterized in that: The steps include: Step 1: uniformly collect geomagnetic data of each axis in the navigation space according to the navigation coordinate system, and expand the discrete magnetic field intensity in the geomagnetic data to obtain an offline geomagnetic fingerprint library; Step 2: construct the system state of the combined navigation according to the position of the pedestrian in the navigation coordinate system at a certain moment in accordance with the northeast sky, and then construct the position and speed model according to the system state; Step 3: Obtain the acceleration vector and angular velocity vector through the inertial sensor, then construct the acceleration vector model and angular velocity vector model, and introduce multi-dimensional constraint items to obtain the velocity prediction value and heading angle prediction value; Step 4: First, construct the objective function, and then map the outputs of steps 1 to 3 to the speed prediction values ​​and the heading angle prediction values ​​one by one according to the process of updating the variables in the objective function according to the position, and then reconstruct them to obtain the differential model and the overall factor graph model; Step 5: Using the reconstructed differential model, the momentum and second-order moment estimation are used to update the parameters of the overall factor graph model; Step 6: Repeat step 5 for each moment coordinate in the navigation history sequence in a sliding window manner to obtain the optimal estimated navigation trajectory; Step 7: Apply the optimized overall factor graph model to update the predicted position.

2. The geomagnetic vector and INS fusion navigation method based on multidimensional constraint factor graph according to claim 1 is characterized in that: In step 1, the method for collecting geomagnetic data is: first determine the location p of a certain point in the area k =(x k ,y k ), the matching area is a square with four vertex coordinates as follows: in, Indicates rounding down, m1, m2, m3, and m4 indicate the point (x k ,y k ) The coordinates of the four vertices of the rectangle are rounded up and down respectively; The intermediate matrix P is calculated based on the partial derivatives and cross partial derivatives of the magnetic field intensity in the X and Y directions at the same position: Where g(·) represents the magnetic field strength at the current point, g x (·) represents the geomagnetic partial derivative of the current point in the x direction, g y (·) represents the geomagnetic derivative of the current point in the y direction, g xy (·) represents the composite partial derivative of the current point in the x and y directions. The coefficient matrices on both sides are provided by the cubic convolution interpolation algorithm and are substituted as known parameters.

3. The geomagnetic vector and INS fusion navigation method based on multidimensional constraint factor graph according to claim 2 is characterized in that: In step 1, continuous expansion is performed by interpolation, and the expression is as follows: The geomagnetic interpolation sequence of the navigation prediction sequence, the east-to-north derivatives and the acquired geomagnetic data are stored for subsequent gradient descent operations.

4. The geomagnetic vector and INS fusion navigation method based on multidimensional constraint factor graph according to claim 1 is characterized in that: In step 2, the system state at time k can be expressed as: X k =[E k N k U k ] Among them, E k ,N k ,U k They represent the east, north and celestial position components of the pedestrian in the navigation coordinate system at time k respectively.

5. The geomagnetic vector and INS fusion navigation method based on multidimensional constraint factor graph according to claim 4 is characterized in that: In step 2, assuming that the pedestrian is walking at a normal speed, the position and speed model is constructed according to the rectangular integration principle of the sampling time, which can be expressed as the following formula: Among them, v x,k ,v y,k ,v z,k represents the current carrier coordinate system velocity accumulated based on the acceleration of each axis, and v E,k ,v N,k ,v U,k It is the projection of the carrier coordinate system velocity in the northeast celestial coordinate system.

6. The geomagnetic vector and INS fusion navigation method based on multidimensional constraint factor graph according to claim 5 is characterized in that: In step 3, the acceleration vector mathematical model is as follows: a b,k =[a x,k a y,k a z,k ] T Among them, a x,k 、a y,k 、a z,k They represent the acceleration of each axis in the carrier coordinate system at time k; The acceleration vector measurement function can be described as: in k =[in x,k ,in y,k ,in z,k ] T Among them, L acc (X k ) indicates that the acceleration vector measurement function is the system prediction value, u k represents the error correction term for the triaxial accelerometer, δ acc Represents the constraint boundary of the three-axis accelerometer; The speed of each historical navigation sequence is defined, and the speed vector mathematical model is as follows: v k =[v x,k v y,k v z,k ] T Among them, v x,k 、v y,k 、v z,k They represent the speed of each axis in the carrier coordinate system at time k; The velocity vector measurement function can be described as: Among them, L vel (X k ) represents the velocity vector function, It represents the constraints of all acceleration vector constraints on the velocity measurement function at the current moment since the start of the navigation sequence. The overall velocity vector function is the predicted value of the velocity at the current moment. The mathematical model of angular velocity vector is as follows: oh b,k =[ω x,k oh y,k oh z,k ] T Among them, ω x,k ,ω y,k ,ω z,k They represent the angular velocity of each axis in the carrier coordinate system at time k; The constraints on the angular velocity vector across time steps are the same as the constraints on the acceleration and are described as follows: Among them, L gyr (X k ) represents the angular velocity vector, ω b,k represents the angular velocity vector of the navigation carrier rotating around the x, y, and z axes, w k , δ gyr represents the constraint boundary of the three-axis angular velocity meter; The heading angle measurement function corresponding to the navigation carrier is described as follows: Among them, L yaw (X k ) indicates that the heading angle measurement function is the predicted value of the heading angle at the current moment.

7. The geomagnetic vector and INS fusion navigation method based on multidimensional constraint factor graph according to claim 6 is characterized in that: In step 4, the overall factor graph model is expressed as follows: Among them, among them, They represent acceleration factor, angular velocity factor and geomagnetic factor respectively.

8. The geomagnetic vector and INS fusion navigation method based on multidimensional constraint factor graph according to claim 7 is characterized in that: The objective function of the overall factor graph model is: Among them, σ gyr ,σ acc Represent the standard deviation of the gyroscope and accelerometer respectively; G(w,u x ,u y ) represents the composite function corresponding to the geomagnetic factors including all constraints, represents the predicted heading angle, They respectively represent the predicted values ​​of the geomagnetic data of each axis in the northeast celestial coordinate system, and n represents the length of a complete navigation sequence.

9. The geomagnetic vector and INS fusion navigation method based on multidimensional constraint factor graph according to claim 8, characterized in that: The reconstruction method of the differential model is: First, for any angular velocity constraint in the objective function, that is, w k Differentiate, where k is any value in a sequence of length n: For G(w,u x ,u y ) cannot be solved directly. It is necessary to establish the association between the objective function and the gradient of each sensor used to collect geomagnetic data, so that the gradient is transferred from the output sensor level back to the objective function level, and the chain derivation rule is used to solve the gradient of each layer; Then, for any constraint term of the x-axis or y-axis acceleration in the objective function, that is, u x,k ,u y,k Differentiate, where k is any value in a sequence of length n: Build an association between the objective function and each sensor gradient.

10. The geomagnetic vector and INS fusion navigation method based on multidimensional constraint factor graph according to claim 9, characterized in that: The parameter update expression is as follows: in, represents the second-order data estimate of the angular velocity constraint term; k represents the current moment, k-1 represents the previous moment; β2 represents the decay rate of the variance term.

Images