A Factor Graph-Based Adaptive Covariance Integrated Navigation Method for LTE / IMU

Abstract

This invention relates to an LTE / IMU integrated navigation method using an adaptive covariance factor graph. This method models the navigation problem as a global maximum a posteriori estimation problem by tightly coupling and integrating the factor graph, achieving optimal estimation of navigation pose information. The main factor nodes include IMU factors and pseudorange factors. The cost function consists of the difference between the observed and estimated quantities, enabling time-varying analysis and correction of state error and pseudorange error values. Simultaneously, a probability transfer model for sensor-acquired information is constructed, and a detailed optimal estimation solution is provided. Furthermore, addressing the problem of the covariance matrix not changing over time in traditional factor graph integrated navigation methods, this invention proposes an adaptive covariance method, which can effectively improve positioning accuracy.

Description

Technical Field

[0001] This invention designs a positioning system based on adaptive covariance and tight coupling of LTE / IMU multi-sensor fusion. Background Technology

[0002] Kalman filtering (KF) and its variants are commonly used to integrate multi-sensor measurements for localization. These filter-based sensor ensemble methods estimate the optimal posterior probability of the state based on the current measurement and the estimate from the previous time step, failing to fully utilize historical states and observations prior to the previous time step. Furthermore, using extended Kalman filtering (EKF) to solve typical nonlinear system problems like integrated localization approximates the dynamics and observation equations of the nonlinear system by linearizing it at each time step. However, for highly nonlinear systems, a single linearization may not provide a sufficiently accurate state estimate, thus requiring multiple iterations to progressively approach the optimal state estimate.

[0003] Factor Graph Optimization (FGO) can effectively address these issues. It is a graph optimization method used for state estimation and parameter optimization. In FGO, all sensor measurements are treated as relevant state variables (i.e., factors) in a factor graph, used to construct a function that maximizes the problem. Furthermore, it utilizes all historical information to estimate the optimal state set, thereby increasing its resistance to outliers and achieving better performance. Summary of the Invention

[0004] To address the problems of high nonlinearity in positioning systems and poor positioning accuracy and robustness when using LTE as a positioning reference signal in existing technologies, this invention proposes an LTE / IMU integrated navigation method based on adaptive covariance factor graphs. This method enables dynamic position estimation in integrated navigation and effectively combines LTE and IMU signals, thereby improving the accuracy, robustness, and availability of the positioning system.

[0005] In this invention, the vehicle's position information is estimated by using LTE signals to estimate the distance between the base station and the vehicle. Pre-integration is performed using IMU (Inertial Measurement Unit) measurements, and the vehicle's pose information is then combined with LTE signals to perform Maximum a Posteriori (MAP) estimation, ultimately obtaining the most accurate pose estimate. Furthermore, the covariance matrix is ​​adaptively adjusted to make the final result closer to the optimal solution. This invention is a study of tightly coupled LTE / IMU multi-sensor positioning based on factor graph optimization. A factor graph model is established, and each node is analyzed and designed, with the entire optimization process presented. Finally, actual test data demonstrates the feasibility of this invention.

[0006] Technical solution of the present invention:

[0007] A multi-sensor tightly coupled positioning method for LTE / IMU based on factor graph optimization employs multiple sensors and achieves adaptive adjustment of the covariance matrix; it includes the following steps:

[0008] Step 1: Construct a tightly coupled factor graph model;

[0009] Construct a tightly coupled factor graph model, calculate the cost function, and derive the maximum a posteriori estimation problem of the system state variables;

[0010] The tightly coupled factor graph includes two types of nodes: variable nodes and factor nodes. Variable nodes are used to represent state variables or parameters that need to be estimated, while factor nodes represent constraints or observations. The connection between variable nodes and factor nodes represents the relationship between variables and constraints.

[0011] Step 2: Calculate each node of the factor graph to transform the maximum a posteriori estimation problem of the system state variables into an NLSP problem;

[0012] Step 3: Adaptively adjust the covariance matrix in the cost function to more accurately reflect the current measurement noise and system uncertainty;

[0013] Step 4: Solve the NLSP problem under adaptive covariance in Step 3 using a nonlinear least squares problem to obtain an estimate of the position at the next time step.

[0014] The constraints of the factor graph are composed of cost functions. By modeling the factor graph as a global maximum a posteriori estimation problem, the solution that minimizes each local cost function is obtained, thereby achieving the optimal estimation of navigation pose information.

[0015] The beneficial effects of this invention are:

[0016] (1) Improve positioning accuracy and stability: By constructing a tightly coupled factor graph model and using IMU and LTE as positioning reference signals, a new solution is provided for the navigation system, which is more reliable and stable.

[0017] (2) Enhance the adaptability of the positioning system: In the process of state transition of the factor graph, the covariance matrix is ​​adaptively adjusted to make the system more adaptable to changes in the external environment and enhance its robustness. Attached Figure Description

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

[0019] Figure 2 This is an LTE / IMU factor graph model according to an embodiment of the present invention;

[0020] Figure 3 This is a comparison chart of algorithm performance under different noise levels in embodiments of the present invention. Detailed Implementation

[0021] The following specific embodiments illustrate the implementation of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification, but the features of this invention are not limited to these embodiments. To provide a deep understanding of the present invention, many specific details will be included in the following description. The present invention may also be implemented without using these details. Furthermore, to avoid confusion or obscuring the focus of the present invention, some specific details will be omitted in the description. It should be noted that, unless otherwise specified, the embodiments and features in the embodiments of the present invention can be combined with each other.

[0022] To make the objectives, technical solutions, and advantages of the present invention clearer, the embodiments of the present invention will be described in further detail below with reference to the accompanying drawings.

[0023] A factor graph-based integrated navigation method for LTE / IMU based on adaptive covariance includes the following steps, such as... Figure 1 :

[0024] Step 1: Construct a tightly coupled factor graph model.

[0025] like Figure 2 This is an LTE / IMU factor graph model according to an embodiment of the present invention;

[0026] In factor graphs, there are generally two types of nodes: variable nodes and factor nodes. Variable nodes represent state variables or parameters that need to be estimated, while factor nodes represent constraints or observations. The connections between variable nodes and factor nodes represent the relationships between variables and constraints.

[0027] The variable nodes of this invention include: pseudorange error variable, pose variable, velocity variable, and state error variable.

[0028] The factor nodes include: prior pose factor, prior velocity factor and prior error factor, INS factor node and pseudorange factor node.

[0029] Define the local function corresponding to each factor node. ,make Indicates the first Each subsystem, then at time have ,in It is a set of measured values.

[0030] The measurement variables are divided into two parts: IMU measurements and LTE pseudorange measurements.

[0031] The IMU state variables at each moment are divided into three parts: the pose information and velocity information of the carrier at the current moment, and the IMU error information, i.e. The pose state variable is defined as follows: This represents the attitude and position information of the carrier in the positioning coordinate system. The velocity variable is defined as... , representing the three-dimensional velocity of the carrier in the positioning coordinate system. The state error variable is defined as... , representing the three-dimensional error values ​​of the accelerometer and gyroscope in the carrier coordinate system, is used to eliminate error bias during state transitions. The clock error corresponding to LTE pseudorange is defined as... .

[0032] In a localization system, the local function can be replaced by a cost function. This invention represents the current sensor measurement value. Compared with the predicted value at the previous time point The difference between them is taken as a residual term and used as input to the cost function. Under the condition of Gaussian noise distribution, the corresponding MAP problem is described as follows:

[0033] (1)

[0034] in Let be the covariance matrix of the noise. ,in , representing the square of the Mahalanobis distance. Substituting... In the above MAP problem, after taking the negative natural logarithm of both sides, the MAP problem of the system state variables can be transformed into a nonlinear least squares problem (NLSP).

[0035] (2)

[0036] Based on the above theory, this invention designs a tightly coupled combined positioning method that utilizes multiple sensors, including an IMU and an LTE sensor, for measurement. The tight coupling means that the IMU measurement values ​​are directly fused with pseudorange information extracted from the LTE signal. While the LTE positioning system cannot operate independently and output a single positioning result, it can be fused with the IMU to correct the pose information calculated by the INS. This method integrates the IMU measurement values ​​to obtain estimated position and velocity values, and uses the LTE signal to estimate the pseudorange between the vehicle and the base station, thereby constraining the position.

[0037] Step 2: Calculate each node of the factor graph to transform the maximum a posteriori estimation problem of the system state variables into an NLSP problem.

[0038] (1) Prior factors

[0039] The prior factors include: prior pose factor, prior velocity factor, and prior error factor. The factor graph at each time step begins with the prior factors, which represent the maximum a posteriori estimate of the state value from the previous time step and are input into the variable nodes, including the state variables from step 1. The variable nodes at the current time step are based on the optimized result of the factor graph and serve as the prior factors for the next time step. As time progresses and new sensor data is input, the factor graph continuously extends, updating the values ​​of the prior factors.

[0040] (2) INS factor nodes

[0041] INS factor nodes represent the difference between IMU predicted values ​​and measured values, and the connection time... With time The state variable nodes. The IMU prediction values ​​are obtained from the state transition equations of the INS system, which can be expressed as: . This indicates that the expression follows a pattern with a mean of 0 and a covariance matrix of . The Gaussian distribution. A state transition matrix can be used to represent this.

[0042] (3)

[0043] in, It is the direction cosine matrix from the positioning system to the vehicle system, representing the direction of the vector in the positioning system within the vehicle system. Generally speaking, It is composed of attitude angles (including roll angle, pitch angle, and yaw angle), representing the attitude of the vehicle system relative to the positioning system. Specifically, it is represented as...

[0044] (4)

[0045] It is the antisymmetric matrix of the IMU angular accelerometer, composed of the direction cosine matrix and the gyroscope zero bias. Multiplication yields the result, expressed as

[0046] (5)

[0047] The gyroscope data needs to be preprocessed by subtracting the zero bias. The purpose of this matrix is ​​to apply a compensation to the gyroscope measurements to counteract the effect of the zero bias, thereby improving the overall performance of the INS.

[0048] The conversion of the positioning coordinate system from meters to latitude and longitude is represented as:

[0049] (6)

[0050] in It represents the radius of curvature of the meridian, that is, the radius of curvature of the north-south motion. It represents the radius of curvature of the normal, that is, the radius of curvature of east-west motion. It is the latitude of the current location.

[0051] The effect of gravitational acceleration caused by altitude is eliminated, expressed as

[0052] (7)

[0053] It is the polar radius in the geocentric inertial frame of reference. It is gravity at the local location. By... Introducing this into the system model makes it more suitable for applications in different geographical locations.

[0054] and This is the inverse of the correlation time of the gyroscope and accelerometer, used to eliminate the influence of time on their deviation. By dynamically compensating for sensor measurements, changes in deviation within the system can be better tracked. If they do not change with time or the expected value of their time derivatives is zero, it indicates that the deviation is relatively stable on a certain time scale, and can therefore be set to zero.

[0055] And IMU in The measured value at time is These are the measurements from the accelerometer and gyroscope in the carrier coordinate system, respectively. The measurements need to be transformed from the carrier coordinate system to the positioning coordinate system using a direction cosine matrix before calculation. Since the IMU's output frequency is higher than other sensors, to reduce computational load, only the INS factor nodes are updated during time intervals without other measurement inputs; global optimization is not performed. For IMU update cycles, in each cycle, only the IMU measurements are pre-integrated. Assuming from... Time's up When no other measurements are input, the changes in velocity and attitude angles in the carrier coordinate system are obtained by integrating the difference between the IMU measurements and the IMU state error variable:

[0056] (8)

[0057] The changes in velocity and attitude angles obtained from the pre-integration above can be used to further estimate the state of the carrier in the positioning coordinate system.

[0058] (9)

[0059] (10)

[0060] (11)

[0061] Factor plot in the 1st When updating, the cost function of the INS factor node is set to the state transition estimate. The difference between the state obtained through IMU pre-integration and the state obtained through IMU pre-integration. Let represent the estimated values ​​of the first 9 dimensions of the INS state variables—attitude, velocity, and position—after the state transition. Then, this local cost function is specifically expressed as:

[0062] (12)

[0063] Therefore, the constraint condition for the INS factor node is:

[0064] (13)

[0065] (3) Pseudo-distance factor nodes

[0066] The pseudorange factor nodes are composed of the difference between the LTE measured pseudorange values ​​and the predicted values. The LTE measured values ​​represent the distance values ​​from different base stations to the receiver, and are therefore related to the location variables in the positioning state variables. This invention assumes that three base stations are currently observed, so a pseudorange factor is set for each base station, representing the pseudorange information associated with that base station. The cost functions corresponding to these pseudorange factors are used to constrain the state variables of the positioning system. If the number of observed base stations changes in practical applications, the system can flexibly adapt to different situations by simply increasing or decreasing the number of pseudorange factors according to the actual situation, exhibiting plug-and-play characteristics.

[0067] LTE observations This represents the distance values ​​to the three base stations. This represents the coordinates of the i-th base station. The predicted value can be expressed as

[0068] (14)

[0069] express The fourth to sixth dimensions represent the predicted values ​​for the carrier's location. Indicates the predicted location With the i-th base station The distance between them. And a pseudorange error variable is added to the factor plot to account for the receiver clock offset error. Modeling is performed. Clock offset represents the amount of deviation of the receiver clock relative to the reference clock. It is the speed of light. , representing the positional offset between the i-th base station and the carrier due to clock skew. The LTE factor local cost function is specifically expressed as: Therefore, the constraint condition for the LTE factor is:

[0070] (15)

[0071] Finally, to transform the maximum a posteriori estimation problem of the system state variables into an NLSP problem, combining the above expressions, the least squares problems corresponding to the two observations are jointly expressed as:

[0072] (16)

[0073] Step 3: Adaptively adjust the covariance matrix in the cost function to more accurately reflect the current measurement noise and system uncertainty.

[0074] In fusion algorithms, factor graph optimization methods typically set the covariance matrix to a constant value. However, in reality, an accurate covariance matrix is ​​difficult to obtain and may change with time and environment. To address this issue, this invention proposes an Adaptive Factor Graph Optimization (AFGO) technique. This technique aims to dynamically adjust the covariance matrix in the cost function based on real-time data and environmental changes, making it more accurately reflect current measurement noise and system uncertainties. Through this adaptive adjustment, AFGO can improve the robustness and performance of the positioning system, thereby better adapting to complex real-world positioning scenarios.

[0075] The adaptive covariance steps are as follows:

[0076] (1) Transform the MAP problem into Here, we assume the covariance matrix is... The covariance matrix is ​​a known quantity. If the covariance matrix is ​​unknown, the above problem can be transformed into... At the same time, Make an estimate.

[0077] (2) Based on Bayesian analysis, a multivariate problem with a Gaussian distribution is modeled. The covariance matrix can be used to model this problem. The model is based on the Normal Inverse Wishart (NIW) distribution, which describes the prior distribution of the covariance matrix of a multivariate normally distributed sample. It is characterized as... And the probability density function is Time can be represented as

[0078] (17)

[0079] in For the dimension of covariance, Represents the determinant of a matrix. The scaling matrix, As for degrees of freedom, these two usually need to be set based on prior knowledge or using empirical values. Let the distribution be an n-variable Gramma distribution. Let the probability of the above maximum a posteriori problem be expressed as:

[0080] (18)

[0081] According to Bayes' theorem, the above equation can be transformed into .because and They are all irrelevant, therefore we can conclude that... ,in Let be the likelihood function, and its expression is:

[0082] (19)

[0083] in This indicates measurement noise.

[0084] (3) To Simultaneously, estimation is performed. After integrating the above expressions, taking the negative natural logarithm of both sides of the maximum a posteriori problem probability form yields the solution.

[0085] (20)

[0086] The right side of the above equation is... Taking the derivative and finding it equal to zero, we obtain the inverse estimate of the covariance as follows: .

[0087] (4) Finally, the covariance matrix is ​​obtained and correlated with the current measurement value and the covariance matrix of the previous time step to achieve adaptive adjustment. According to the definition of the NIW distribution, It can be set based on prior knowledge, and can be defined through the noise propagation equation, assuming... This is the process noise matrix corresponding to the sensor. Represented as

[0088] (twenty one)

[0089] Adaptive adjustment of covariance can be expressed as

[0090] (twenty two)

[0091] Step 4: Solve the NLSP problem under adaptive covariance in Step 3 using a nonlinear least squares problem to obtain an estimate of the position at the next time step.

[0092] After constructing the factor graph model and adaptively designing the covariance, a solution method for the final NLSP problem in step 2 is presented. This invention uses the Gauss-Newton iterative optimization algorithm to solve the nonlinear least squares problem. It is based on the linearization idea, solving the problem by performing a linear approximation in each iteration. The nonlinear model is linearly approximated using Taylor expansion, and then the equation is iterated to correct the regression function in order to find the optimal state value that minimizes the objective residual. Below, we will solve the aforementioned NLSP. The steps for solving the nonlinear least squares problem are explained below:

[0093] (1) Construct well After solving the least squares problem, the error term is... A first-order Taylor expansion is performed at the point where...

[0094] (twenty three)

[0095] in, , which represents the system increment. Let k be the measurement function at time k, representing the mapping relationship between the state vector and the observed values. It is a nonlinear function, therefore The Jacobian matrix is ​​used to represent the approximate linearized partial derivative matrix obtained by taking the partial derivative of the observation with respect to the state variables in each dimension.

[0096] (2) The above NLSP problem is then transformed into finding the solution through multiple iterations. , making For this linear least squares problem, each subsystem can be... Mahalanobis distance Converting to the form of the L2 norm, i.e.

[0097] (twenty four)

[0098] Therefore, there is ,in , representing the product of the inverse of the square root of the covariance and the Jacobian matrix. This represents the product of the inverse of the square root of the covariance and the residuals; therefore, there are variables. The problem of finding the extremum of can be expressed as the solution obtained by differentiating and setting it to zero as

[0099] (25)

[0100] (3) Update the solution for the next time step as follows

[0101] (26)

[0102] A simulation platform was built using MATLAB. The positioning results based on AFGO, FGO, and EKF algorithms were compared for different standard deviations of LTE positioning pseudoranges. Figure 3 As shown.

[0103] It is evident that the EKF algorithm, due to its simple model assumptions and reliance on prior values ​​from the previous time step, exhibits relatively low positioning accuracy. In contrast, the FGO algorithm considers more nonlinear factors, thus demonstrating better performance. The AFGO algorithm further incorporates uncertainties in dynamic environments, resulting in superior robustness. Furthermore, as the pseudorange standard deviation increases, the positioning error growth rate of the AFGO algorithm is observed to be lower than that of the FGO algorithm. This indicates that in noisier environments, the AFGO algorithm adapts better to environmental changes, thereby producing more stable and accurate positioning results.

[0104] While the present invention has been illustrated and described with reference to certain preferred embodiments, those skilled in the art should understand that the above description is a further detailed explanation of the invention in conjunction with specific embodiments, and should not be construed as limiting the specific implementation of the invention to these descriptions. Various changes in form and detail can be made by those skilled in the art, including several simple deductions or substitutions, without departing from the spirit and scope of the invention.

Claims

1. A positioning method based on factor graph optimization for tightly coupled LTE / IMU multi-sensor systems, characterized in that, Includes the following steps: Step 1: Construct a tightly coupled factor graph model; Construct a tightly coupled factor graph model, calculate the cost function, and derive the maximum a posteriori estimation problem of the system state variables; Step 2: Calculate each node of the factor graph to transform the maximum a posteriori estimation problem of the system state variables into a nonlinear least squares NLSP problem; Step 3: Adaptively adjust the covariance matrix in the cost function to more accurately reflect the current measurement noise and system uncertainty; Step 4: Solve the NLSP problem under adaptive covariance in Step 3 using a nonlinear least squares problem to obtain an estimate of the position at the next time step; Step 1 is as follows: The tightly coupled factor graph includes two types of nodes: variable nodes and factor nodes. Variable nodes are used to represent state variables or parameters that need to be estimated, while factor nodes represent constraints or observations. The connection between variable nodes and factor nodes represents the relationship between variables and constraints. The variable nodes of this method include: pseudorange error variable, pose variable, velocity variable, and state error variable; Factor nodes include: prior pose factor, prior velocity factor and prior error factor, INS factor node and pseudorange factor node; Define the local function corresponding to each factor node. ,make Indicates the first Each subsystem, then at time have , Let represent the probability, where A set of measured values; The measurement variables are divided into two parts: IMU measurement values ​​and LTE pseudorange measurement values. The IMU state variables at each moment are divided into three parts: the pose information and velocity information of the carrier at the current moment, and the IMU error information, i.e. The pose state variable is defined as follows: This represents the attitude and position information of the carrier in the positioning coordinate system; the velocity variable is defined as... , representing the three-dimensional velocity of the carrier in the positioning coordinate system; the state error variable is defined as The error values ​​for the accelerometer and gyroscope in three dimensions within the carrier coordinate system are used to eliminate error bias during state transitions; the clock error corresponding to the LTE pseudorange is defined as... ; In a localization system, the local function uses a cost function. Indicates; the current sensor measurement value Compared with the predicted value at the previous time point The difference between them is taken as a residual term and used as input to the cost function; under the condition of Gaussian noise distribution, the corresponding maximum a posteriori (MAP) estimation problem is described as follows: in Let be the covariance matrix of the noise; make ,in , representing the square of the Mahalanobis distance, substitute into In the above MAP problem, after taking the negative natural logarithm of both sides, the MAP problem of the system state variables can be transformed into a nonlinear least squares problem (NLSP). ； In step 3, the adaptive covariance steps are as follows: (1) Transform the MAP problem into Here, we assume the covariance matrix is... The variables are known; if the covariance matrix is ​​unknown, the above problem can be transformed into... At the same time, Make an estimate; (2) Model the multivariate problem with Gaussian distribution based on Bayesian analysis; (3) To Moment Simultaneously estimate; solve for in, For the dimension of covariance, Represents the determinant of a matrix. The scaling matrix, For degrees of freedom, the right side of the above equation is relative to Taking the derivative and finding it equal to zero, we obtain the inverse estimate of the covariance as follows: ; (4) Finally, the covariance matrix is ​​obtained and correlated with the current measurement value and the covariance matrix of the previous time step to achieve adaptive adjustment; according to the definition of the normal inverse Wissaud NIW distribution, It can be set based on prior knowledge, and can be defined through the noise propagation equation, assuming... This is the process noise matrix corresponding to the sensor. Expressed as Adaptive adjustment of covariance can be expressed as 。 2. The positioning method for LTE / IMU multi-sensor tight coupling based on factor graph optimization as described in claim 1, characterized in that, In step 2, the NLSP problem is: in, The local cost function of the IMU is specifically expressed as: in, This represents the estimated values ​​of the first 9 dimensions of the INS state variables—attitude, velocity, and position—after the state transition. Represents the observed attitude, velocity, and position at time k; The local cost function for the LTE factor is specifically expressed as: in This represents the estimated pseudorange of the i-th LTE base station. This represents the observed value of the pseudorange of the corresponding base station.

3. The positioning method for LTE / IMU multi-sensor tight coupling based on factor graph optimization as described in claim 1, characterized in that, In step 4, the steps for solving the nonlinear least squares problem are as follows: (1) Construct well After solving the least squares problem, the error term is... A first-order Taylor expansion is performed at the point where... in, , representing the system increment; Let k be the measurement function at time k, representing the mapping relationship between the state vector and the observed values. It is a nonlinear function, therefore The Jacobian matrix is ​​used to represent the approximate linearized partial derivative matrix obtained by taking the partial derivative of the observation with respect to the state variables in each dimension. (2) The above NLSP problem is then transformed into finding the solution through multiple iterations. , making For this linear least squares problem, each subsystem can be... Mahalanobis distance Converting to the form of the L2 norm, i.e. Therefore, there is ,in , representing the product of the inverse of the square root of the covariance and the Jacobian matrix. This represents the product of the inverse of the square root of the covariance and the residuals; therefore, there are variables. The problem of finding the extremum of can be expressed as the solution obtained by differentiating and setting it to zero as (3) Update the solution for the next time step as follows 。

Images