CN114973036B - Unmanned aerial vehicle three-dimensional positioning method based on GNSS/inertial navigation/wireless base station fusion - Google Patents

Abstract

The invention provides an unmanned aerial vehicle three-dimensional positioning method based on GNSS/inertial navigation/wireless base station fusion, which converts satellite navigation, inertial navigation and wireless navigation information carried by an unmanned aerial vehicle into different information probability models based on an information geometric theory, and solves the problem of different information formats of heterogeneous navigation sources during fusion. In addition, by utilizing the correlation between the navigation source information probability and the positioning accuracy of the unmanned aerial vehicle, the positioning result is obtained by fusing a plurality of probability density functions through calculating the navigation source information accuracy probability function, so that the fusion speed and stability of the three-dimensional positioning of the unmanned aerial vehicle can be effectively improved, errors can be effectively restrained under the condition that the navigation information is lost, and the three-dimensional positioning of the unmanned aerial vehicle is realized.

Description

Three-dimensional positioning method of UAV based on GNSS/inertial navigation/wireless base station fusion

Technical Field

The present invention relates to the field of fusion positioning technology, and specifically to a three-dimensional positioning method for unmanned aerial vehicles based on GNSS/inertial navigation/wireless base station fusion assisted by an information geometry model.

Background Art

Due to their small size, flexible maneuverability, wide application scenarios and low cost, drones provide strong support for smart city construction in the fields of express logistics, smart agriculture, emergency rescue, remote sensing and mapping.

In order to achieve high-precision navigation and positioning, drones are usually equipped with multiple navigation sources such as satellite navigation, inertial navigation, radio navigation, and visual navigation. However, the above navigation sources all have certain application limitations. For example, in scenes such as dense vegetation and urban canyons, the navigation source signal of satellite navigation is weak or highly distorted, resulting in poor positioning information accuracy, which will have a catastrophic impact on drone flight; due to its inherent defects, the positioning error of inertial navigation is a cumulative error accumulated over time, which needs to be calibrated through other navigation source information, and the positioning result is poor in stability; due to terrain restrictions and limited ground coverage of base stations, the navigation source signal of radio positioning is easily blocked, resulting in signal loss; visual navigation has a large amount of image processing in a dynamic environment and poor real-time performance. It can be seen that a single type of navigation source cannot meet the real-time positioning needs of drones, and the high maneuverability of drones leads to high requirements for the real-time and stability of navigation positioning. Therefore, how to achieve the integration of various types of navigation information and improve the real-time and reliability of the entire drone navigation and positioning system is a hot issue in drone applications.

The Bayesian method is an earlier information fusion method. It combines the associated probability distribution of each target into a joint posterior distribution function, continuously updates the likelihood function of the assumed joint distribution, and finally fuses the data through the maximum or minimum of the likelihood function. D-S evidence theory is a generalized Bayesian reasoning method. It uses probability intervals and uncertainty intervals to obtain the likelihood function of the hypothesis under multiple evidences, and defines levels for partial data and uncertain events, so as to objectively describe uncertain events. Non-Bayesian estimation methods mainly include the least squares (LS) estimation method and the maximum likelihood estimation (ML) method. However, this type of method cannot meet the real-time positioning of drones under high mobility characteristics, and the stability of positioning accuracy is also poor.

The fusion positioning method currently used by UAVs is mainly developed from vehicle navigation fusion and can be divided into two categories: one is positioning data fusion represented by Kalman filtering, and the other is intelligent fusion represented by neural networks. The computational complexity of Kalman filtering increases greatly with the number of fusion sources, while neural networks require a large amount of real-time data training, which is not suitable for the high real-time requirements of UAV three-dimensional positioning.

Therefore, how to realize UAV positioning in unknown, complex, and dynamic environments, and how to maximize the real-time performance of the algorithm and reduce the amount of calculation still needs further research. At present, information geometry theory has attracted more and more attention as another method to solve nonlinear random problems. The essence of information geometry is the study of the intrinsic geometric properties of probability distribution manifolds. It uses differential geometry methods to geometricize the basic problems in probability theory and information theory. Information geometry methods have made a lot of progress in statistical signal processing, parameter estimation and filtering, but how to use information geometry distance to realize the information fusion of multiple navigation sources of UAVs is still blank.

Summary of the invention

In view of the problems that the existing multi-source fusion positioning methods have slow fusion speed, poor positioning stability, and are difficult to achieve rapid positioning of UAVs in unknown, complex, and dynamic environments, the present invention proposes a GNSS/inertial navigation/wireless base station fusion UAV three-dimensional positioning method based on information geometry. This method is based on the theory of information geometry and creatively converts the satellite navigation, inertial navigation, and radio navigation information carried by the UAV into different information probability models, solving the problem of different formats of heterogeneous navigation source information during fusion. In addition, by utilizing the correlation between the probability of UAV navigation source information and positioning accuracy, by calculating the probability function of the navigation source information accuracy, and fusing multiple probability density functions to obtain the positioning result, the fusion speed and stability of the UAV three-dimensional positioning can be effectively improved. In the case of navigation information loss, the error can still be effectively suppressed to achieve the three-dimensional positioning of the UAV.

The technical solution of the present invention is:

A three-dimensional positioning method for unmanned aerial vehicles based on GNSS/inertial navigation/wireless base station fusion includes the following steps:

Step 1: Use several distributed navigation sources on the UAV to solve the positioning information and obtain their respective positioning results; map the positioning results of each distributed navigation source to a probability density function that conforms to its own distribution, and obtain the initialized correlation matrix R ₍₀₎ ; the satellite navigation source and the radio navigation source obey the Gaussian probability distribution model, and the inertial navigation source obeys the Poisson probability distribution model;

Step 2: According to the information fusion criterion of minimizing the square sum of matrix KL divergence under multi-source information fusion, iterate the correlation matrix Obtain R that satisfies the minimum value of the objective function; where N represents the number of navigation sources involved in the fusion on the UAV, h is the iteration step, KLD() represents the matrix KL divergence, _Rk represents the covariance matrix of the kth navigation source positioning result, and R _(i) represents the i-th iteration result of the covariance matrix R; repeat the iterative process, and when ||R _(i+1) -R _(i) ||＜δ is satisfied, the iteration is jumped out, and the iterative result of the covariance matrix R is obtained;

Step 3: After determining the minimum covariance matrix R of the objective function, calculate the optimal weight factor ω _k by Obtain the fusion result of the mean of multiple navigation sources; where μ is the final position vector of the positioning target.

Furthermore, in step 1, the three-dimensional probability density function is decomposed into three parallel two-dimensional probability density function representations; wherein the Gaussian probability distribution model is:

in Correlation coefficient

The Poisson probability distribution model is:

Among them, δ ₁ , δ ₂ , δ ₃ , δ ₄ , δ ₅ , δ ₆ are setting parameters.

Furthermore, the weight factors are calculated by a non-iterative fast covariance fusion algorithm.

Furthermore, the weight factor

Wherein ε _k =tr(R _k ), tr(·) represents the trace of the matrix.

Beneficial Effects

The present invention converts the satellite navigation, inertial navigation and radio navigation information carried by the UAV into different information probability models through the information geometry theory, and uses mathematical derivation to fit the probability density functions of different types of navigation sources to realize the design of multi-source fusion positioning algorithm based on information geometry in three-dimensional space.

The present invention utilizes the correlation between the probability of the navigation source information of the UAV and the positioning accuracy to convert different types of navigation source information into probability functions, which has excellent convergence speed and low computational complexity, and can meet the needs of three-dimensional real-time positioning of the UAV. In addition, when the navigation source information is lost or a sudden error occurs, the method proposed by the present invention can make full use of the distributed different types of navigation source information carried by the UAV to achieve rapid fusion, effectively improve the positioning accuracy and stability of the UAV, have strong error suppression capabilities, and achieve rapid and stable fusion.

Additional aspects and advantages of the present invention will be given in part in the following description and in part will be obvious from the following description, or will be learned through practice of the present invention.

BRIEF DESCRIPTION OF THE DRAWINGS

The above and/or additional aspects and advantages of the present invention will become apparent and easily understood from the description of the embodiments in conjunction with the following drawings, in which:

Fig. 1 is a schematic diagram of a system model of the present invention;

In Figure 1, A represents a drone. The drone node can receive satellite navigation, inertial navigation, and radio navigation signal information, and the drone positioning can be achieved by fusing multiple navigation source information. However, in urban environments, streets often divide the surrounding dense and tall buildings, forming an urban canyon effect. In this environment, various navigation sources of drones will be blocked or interfered. In Figure 1, the blue link represents the ranging communication link between the base station and the drone, and the yellow link represents the ranging communication link between the satellite and the drone. Due to the limitations of terrain and the ground coverage of the base station, the drone can sometimes receive the base station signal, and the drone may not be able to receive the base station signal for a period of time, depending on whether the navigation source signal is blocked. The drone receives various navigation source signals that change with the area of movement.

Figure 2 is a probability density diagram of each navigation source;

(a) is the navigation source signal that follows Gaussian distribution

(b) is the navigation source signal that obeys Poisson distribution;

Among them, navigation sources represented by satellite navigation and radio navigation obey the Gaussian probability distribution model, and navigation sources represented by inertial navigation obey the Poisson probability distribution model, which is consistent with the characteristic that their positioning errors accumulate over time.

DETAILED DESCRIPTION

In order to solve the problems that the existing multi-source fusion positioning methods have slow fusion speed and poor positioning stability, and are difficult to achieve rapid positioning of UAVs in unknown, complex and dynamic environments, the present invention provides a GNSS/inertial navigation/wireless base station fusion UAV three-dimensional positioning method based on information geometry. This multi-source fusion UAV three-dimensional positioning method based on information geometry has low calculation complexity, fast fusion speed and high stability, and can effectively solve the problem of difficult three-dimensional positioning of high-dynamic targets represented by UAVs.

The GNSS/inertial navigation/wireless base station fusion unmanned aerial vehicle three-dimensional positioning method based on information geometry proposed in the present invention combines information geometry theory with the information fusion process, so that the fusion process of probability information that cannot be directly performed in Euclidean space can be realized on the statistical manifold. Considering the large amount of calculation for directly solving the position of the three-dimensional unmanned aerial vehicle node, the present invention adopts the method of decomposing the three-dimensional space into three parallel two-dimensional spaces, which not only improves the positioning accuracy of the unmanned aerial vehicle, but also ensures the demand for real-time positioning of the unmanned aerial vehicle. In order to obtain the fused navigation positioning information, it is first necessary to establish a probability statistical model of the positioning information of each navigation source. The navigation sources studied in the present invention are mainly divided into the following two categories: one is a Gaussian probability distribution model represented by satellite navigation and radio navigation, and the other is a Poisson probability distribution model represented by inertial navigation, which conforms to the characteristics of its positioning error accumulation over time. Both probability distribution models are navigation sources with known parameters. The variance of the probability distribution of the corresponding positioning can be calculated according to their parameters, and the parameter analytical formula of the corresponding navigation source positioning can be obtained by substituting them. Through the multi-source fusion algorithm based on information geometry, the distance is replaced by the K-L divergence, and the lower bound of the fusion objective function is obtained, and finally the real-time positioning result of the unmanned aerial vehicle node is obtained.

The specific drone node positioning process includes the following three steps:

(1) Initialization

In the GNSS, inertial navigation, and wireless base station navigation and positioning systems of the navigation and positioning module, the positioning information received by each system is solved to obtain the positioning coordinates. The positioning information of each navigation and positioning system is mapped to conform to the respective distribution probability density functions to obtain the initialized correlation matrix R ₍₀₎ .

(2) Loop Iteration

According to the information fusion criterion of minimizing the square sum of matrix KL divergence under multi-source information fusion, the correlation matrix is iterated Obtain R that satisfies the minimum value of the objective function. Where N represents the total number of navigation sources to be fused for the drone node, h is the iteration step, KLD() represents the matrix KL divergence, _Rk represents the covariance matrix of the kth navigation source positioning position, R _(i) represents the i-th iteration result of the covariance matrix R, repeat the iterative process, and when ||R _(i+1) -R _(i) ||＜δ is satisfied, the iteration is jumped out, and the iterative result of the covariance matrix R is obtained.

(3) Calculate the weights and obtain the final positioning result

After determining the minimum covariance matrix R of the objective function, calculate the optimal weight factor ω _k by The fusion result of the mean of multiple navigation sources is obtained; where μ is the final position vector of the positioning target, and R is the covariance matrix of the final positioning target position vector.

Embodiments of the present invention are described in detail below. The embodiments are exemplary and intended to be used to explain the present invention, but should not be construed as limiting the present invention.

Assume that the distributed navigation sources on UAV A include satellite navigation, inertial navigation and wireless base station navigation, with a total of N navigation sources, and their observation data ^x∈Rm . At the i-th navigation source, the measured value of x is recorded as z _i , then the local posterior probability density of x is p _i (x|z _i ), i＝1,…,N, and the correlation between local navigation sources is unknown. The ultimate goal is to merge all these probability densities into a single probability density, recorded as Will Recorded as Let p _i (x|z _i ),i＝1,…,N be denoted as p _i ,i＝1,…,N. If p _i ,i＝1,…,N all belong to the same probability distribution family S＝{p(x；ξ)}, then an optimization criterion is used to select the optimal point on S In the framework of information geometry, the distribution family S is regarded as a Riemannian manifold. Under this condition, the information-related geodesic distance and information fusion theory criterion can be obtained:

Here d refers to the Riemann distance between two points on the manifold. According to formula (1), we can get The objective function of is the sum of the squared distances between a probability density on S and all locally known probability densities, which can be intuitively interpreted as finding the most similar local probability density in S.

Next, each probability density function is discussed in detail. Considering the large amount of computation required to directly solve the three-dimensional UAV node position, the present invention adopts a method of decomposing the three-dimensional space into three parallel two-dimensional spaces, which not only improves the UAV positioning accuracy, but also ensures the real-time positioning requirements of the UAV.

On the XOY plane, assuming that F(x,y) is the joint distribution function of the two-dimensional continuous random variable (X,Y), if there exists a non-negative function f(x,y), for any real number x,y:

Then f(x,y) is called the joint probability density function of the two-dimensional continuous random variables (X,Y).

Assuming that in the R region, x1＜x＜x2, y1＜y＜y2, then the probability distribution in the region is:

And F(x,y) satisfies the following in the entire domain:

The probability density function of a P-dimensional random variable X = (X ₁ , X ₂ , …, _XP ) ^T is:

X is called P-dimensional normal distribution, denoted as X~ _NP (μ,∑). The covariance matrix is∑＝Cov(X), and the mathematical expectation vector isμ＝( _μ1 , _μ2 ,…, _μP ) ^T . The symbol|| represents the determinant of the matrix, (·) ^-1 represents the inverse of the matrix, and (·) ^T represents the transpose of the matrix.

Defined by the probability density function of the multidimensional Gaussian distribution, the probability density function of the two-dimensional Gaussian distribution Z _Gauss = f _Gauss (x, y) is:

Then Z = f(x, y) is said to satisfy the two-dimensional Gaussian distribution, and Correlation coefficient

According to formula (6), the probability density function of the two-dimensional Gaussian distribution in the XOZ and YOZ planes can also be obtained:

in Correlation coefficient

in Correlation coefficient

The following is a discussion on Poisson distribution. According to the generalized bivariate Poisson distribution proposed by Berkhout & Plug. On the XOY plane, assuming that (X, Y) is a random variable that follows a univariate Poisson distribution with parameters δ _i (i = 1, 2), the probability density function of the vector (X, Y) Z _Poiss = f _Poiss (x, y) is equal to:

Under these conditions,

lnδ ₁ =m′β ₁ (10)

lnδ ₂ =m′β ₂ +ηx (11)

Where β ₁ , β ₂ and η are parameters, and parameters δ ₁ and δ ₂ will change with the change of regression covariate m′, which represents the observed heterogeneity between individuals. When η = 0, variables X and Y are independent. The generalized linear model of formula (10) is applicable to the response variable X, and the model of formula (11) is applicable to the response variable Y. The resolution of these models can not only highlight the independence between variables X and Y, but also highlight the impact of factor m′ on these same variables. The bivariate Poisson distribution has the following characteristics:

where c ₂ is the intercept of equation (11).

Equation (14) shows that variable Y is overdispersed. Equation (15) confirms that variables X and Y are independent if and only if η = 0. The covariance is negative, zero, or positive, depending on whether η is negative, zero, or positive.

According to formula (6), the probability density function of the two-dimensional Poisson distribution in the XOZ and YOZ planes can also be obtained:

Among them, δ ₃ , δ ₄ , δ ₅ , and δ ₆ are parameters.

Since the Riemann distance in equation (1) is difficult to calculate, KLD is used to measure the difference between the two probability distributions p(x|θ ₁ ) and p(x|θ ₂ ):

Considering the complex Gaussian vector distribution N(0,R) with mean 0 and correlation matrix R, the distribution expression is:

The probability distribution family S = {p(x|R)|R∈H(n)} parameterized by the correlation matrix R∈H(n), where H(n) is an n×n dimensional Hermitian positive definite matrix space. According to information geometry theory, under certain topological and differential structures, S can form a manifold with R-dimensional natural coordinates. Because the coordinate R of the manifold S is the correlation matrix, S can also be called a matrix manifold.

Since the zero-mean Gaussian vector distribution has a dual structure, that is, the manifold has two mutually dual coordinate systems, and the two can be transformed into each other by the Legendre transformation of the potential function. Then the natural coordinate R and the dual coordinate ^Rω have the following Legendre transformation relationship:

in represents the gradient operator. Substituting the potential function of the zero-mean complex Gaussian distribution, we can get the KL divergence from R ₁ to R ₂ on the manifold:

where tr(·) represents the trace of the matrix.

From the perspective of information geometry, the distance function is replaced by KL divergence, which provides a lower limit for the minimum value of the objective function in equation (1). The matrix KL divergence is used to solve the minimum value to achieve the optimization of UAV fusion positioning. The covariance matrix R _k of N probability distributions, k = 1, ..., N, is fused, so the problem becomes to find R that satisfies the minimum value of the objective function:

Taking the derivative of the above formula, we can get

Therefore, there is

R _(i+1) =R _(i) -hf′(R _(i) ) (24)

h is the iteration step length.

After determining the minimum covariance matrix R of the objective function, the mean fusion result u can be obtained by formula (25):

Where μ _k represents the positioning coordinate vector of the kth navigation subsystem, R _k represents the covariance matrix of the kth navigation subsystem positioning position, and ω _k is the weight factor. Calculating the optimal weight factor ω _k requires repeated iterations, which is a huge amount of calculation. In order to avoid such a large amount of calculation, this algorithm uses the non-iterative fast covariance fusion algorithm (Fast Covariance Intersection, FCI). The non-iterative fusion weights ω ₁ ,ω ₂ ,…ω _N satisfy ω ₁ +ω ₂ +…+ω _N ＝1, and the linear constraint can be generalized as:

tr(R _k )ω _k -tr(R _k+1 )ω _k+1 ＝0,k＝1,2,…,N-1 (26)

Where tr() represents the trace of the matrix. Let ε _k = tr(R _k ), and derive the fusion weights ω ₁ ,ω ₂ ,…ω _N from equation (26):

This embodiment utilizes the correlation between the probability of drone navigation source information and positioning accuracy to convert different types of navigation source information into probability functions, which has excellent convergence speed and low computational complexity and can meet the needs of drone three-dimensional real-time positioning.

Although the embodiments of the present invention have been shown and described above, it is to be understood that the above embodiments are illustrative and are not to be construed as limitations on the present invention. A person skilled in the art may change, modify, substitute and modify the above embodiments within the scope of the present invention without departing from the principles and purpose of the present invention.

Claims (4)

1. A unmanned aerial vehicle three-dimensional positioning method based on GNSS/inertial navigation/wireless base station fusion is characterized by comprising the following steps: the method comprises the following steps:

Step 1: positioning information is resolved by utilizing a plurality of distributed navigation sources on the unmanned aerial vehicle, and respective positioning results are obtained respectively; mapping the positioning results of each distributed navigation source into probability density functions conforming to respective distribution respectively to obtain an initialized correlation matrix R ₍₀₎; the satellite navigation source and the radio navigation source obey a Gaussian probability distribution model, and the inertial navigation source obeys a poisson probability distribution model;

step 2: according to the matrix K-L divergence sum minimization information fusion criterion under multi-source information fusion, iterating the related matrix Obtaining R meeting the minimum value of the objective function; wherein N represents the number of navigation sources participating in fusion on the unmanned aerial vehicle, h is an iteration step length, KLD () represents the matrix K-L divergence, R _k represents the covariance matrix of the kth navigation source positioning result, and R _(i) represents the ith iteration result of the covariance matrix R; repeating the iterative process, and obtaining a covariance matrix R iterative result when meeting the requirement of R _(i+1)-R_(i) < delta jumping out of the iteration;

Step 3: after determining the minimum covariance matrix R of the objective function, calculating the optimal weight factor omega _k by Obtaining a fusion result of a plurality of navigation source mean values; where μ is the resulting position vector for locating the target.

2. The unmanned aerial vehicle three-dimensional positioning method based on GNSS/inertial navigation/wireless base station fusion according to claim 1, wherein the unmanned aerial vehicle three-dimensional positioning method is characterized by comprising the following steps of: in the step 1, decomposing a three-dimensional probability density function into three parallel two-dimensional probability density function representations; wherein the gaussian probability distribution model is:

Wherein the method comprises the steps of Correlation coefficient

The poisson probability distribution model is:

Wherein δ ₁,δ₂,δ₃,δ₄,δ₅,δ₆ is a setting parameter.

3. The unmanned aerial vehicle three-dimensional positioning method based on GNSS/inertial navigation/wireless base station fusion according to claim 1, wherein the unmanned aerial vehicle three-dimensional positioning method is characterized by comprising the following steps of: the weight factors are calculated by a non-iterative fast covariance fusion algorithm.

4. The unmanned aerial vehicle three-dimensional positioning method based on GNSS/inertial navigation/wireless base station fusion according to claim 3, wherein the unmanned aerial vehicle three-dimensional positioning method is characterized by comprising the following steps of: weighting factor

Where ε _k＝tr(R_k), tr (. Cndot.) represents the trace of the matrix.