阅读说明：本技术 一种基于tdoa和fdoa的无人机高精度定位方法 (High-precision positioning method for unmanned aerial vehicle based on TDOA and FDOA ) 是由 熊海良 侯强 朱维红 许玉丹 王宏蕊 庄众 任美婷 于 2020-10-21 设计创作，主要内容包括：本发明公开了一种基于TDOA和FDOA的无人机高精度定位方法,包括以下步骤：利用空间中的接收站接收TDOA测量值和FDOA测量值,并将其等效为距离差测量值和距离差变化率测量值；对接收站的位置坐标进行联合估计构建待估计矢量；构建对应距离差测量值、距离差变化率测量值以及接收站位置测量值和待估计矢量之间的表达式；求出代价误差函数的非线性加权最小二乘优化模型；泰勒级数联合迭代求出待估计矢量的最优闭式解；当满足迭代终止条件得到目标的位置和速度。本发明的方法将接收站的位置坐标进行联合迭代估计,降低了接收站位置误差对定位精度的影响；利用定位误差修正提供更加精确的迭代初值,在保证收敛性和提高收敛速度的同时,提高目标定位精度。(The invention discloses a high-precision positioning method of an unmanned aerial vehicle based on TDOA and FDOA, which comprises the following steps: receiving, with a receiving station in space, TDOA measurements and FDOA measurements equivalent to range difference measurements and range difference rate of change measurements; performing joint estimation on the position coordinates of the receiving station to construct a vector to be estimated; constructing corresponding measured values of range difference, measured values of range difference change rate and expressions between the measured values of the receiving station position and the vector to be estimated; solving a nonlinear weighted least square optimization model of the cost error function; solving the optimal closed-form solution of the vector to be estimated by Taylor series combined iteration; and obtaining the position and the speed of the target when the iteration termination condition is met. The method of the invention carries out joint iterative estimation on the position coordinates of the receiving station, thereby reducing the influence of the position error of the receiving station on the positioning precision; and a more accurate iteration initial value is provided by using positioning error correction, and the target positioning precision is improved while the convergence is ensured and the convergence speed is improved.)

1. A high-precision positioning method for an unmanned aerial vehicle based on TDOA and FDOA is characterized by comprising the following steps:

step 1: equating the TDOA measurements and the FDOA measurements received by the receiving stations to range difference measurements and range difference rate measurements using K receiving stations in space;

step 2: performing joint estimation on the position coordinates of the receiving station, and constructing a vector eta to be estimated, which comprises an unknown real target position and speed vector and an unknown real coordinate vector of the receiving station;

and step 3: constructing corresponding range difference measurement values, range difference change rate measurement values and expressions between the receiving station position measurement values and the vector eta to be estimated;

and 4, step 4: defining a cost error function of the expression in the step 3, and solving a nonlinear weighted least square optimization model of the cost error function;

and 5: setting the expression in step 3 at the initial estimation value eta_aUsing a first order Taylor series expansion and carrying into step 4The nonlinear weighted least square optimization model is used for solving the optimal closed solution of the vector eta to be estimated;

step 6: setting the condition of stopping iteration, 0 < epsilon < 1, repeating the step 5;

and 7: and when the iteration termination condition is met, terminating the iteration to obtain the position and the speed of the target.

2. The method for positioning unmanned aerial vehicle with high precision based on TDOA and FDOA as claimed in claim 1, wherein the specific method in step 1 is as follows:

with K receiving stations, the true position of the ith receiving station isTrue velocity ofTrue position of the target is q⁰＝[x⁰,y⁰,z⁰]^TThe true speed of the target isThe true distance between the target and the ith receiving station is expressed as:

let 1 st receiving stationFor a reference receiving station, the difference between the true distance from the target to the ith receiving station and the reference receiving station is:

wherein r is_i ⁰To representTrue distance, r, between target and ith receiving station₁ ⁰Representing the real distance between the target and the reference receiving station, and c is the propagation speed of the electromagnetic wave;

then, respectively obtaining derivatives of the two ends of the formula (1) for the time t to obtain the true distance change rate between the target and the ith receiving station as follows:

wherein, the symbol (·)^TRepresenting a transpose of a matrix or vector;

the true velocity difference between the ith and reference receiving stations is:

wherein the content of the first and second substances,is the true rate of change of range between the target and the reference receiving station, f₀Is the frequency of the electromagnetic wave signal and,is the true FDOA value between the ith receiving station and the reference receiving station;

the TDOA measurement is equivalent to a range difference measurement, which is expressed as:wherein the content of the first and second substances,representing the true difference in distance, Δ t, between the target arriving at the ith receiving station and arriving at the reference receiving station_i1Represents the time difference measurement error between the target arriving at the ith receiving station and arriving at the reference receiving station, namely the TDOA measurement error;

assuming that the range difference measurements follow a Gaussian distribution with a mean value of 0Written in vector form:wherein the content of the first and second substances,n＝[n₂₁,n₃₁,...,n_K1]^Tn denotes a vector of measurement error components of the distance difference, r_K1Representing a measured value of a distance difference between the target arriving at the kth receiving station and arriving at the reference receiving station,representing the true distance difference between the target arriving at the Kth receiving station and the reference receiving station, n_K1Representing the error between the measured value of the distance difference between the target arriving at the Kth receiving station and the reference receiving station and the true value;

similarly, the FDOA measured value is equivalent to a measured value of the rate of change of the range difference, which is expressed asWherein the content of the first and second substances,representing the true speed difference, Δ f, between the ith and the reference receiving stations_i1A measurement error representing a frequency difference between the target arrival ith receiving station and the reference receiving station, i.e., an FDOA measurement error;

assuming that the measurements of the rate of change of range difference follow a gaussian distribution with a mean value of 0 and are uncorrelated with the TDOA measurements; handleWriting to vector shapesFormula (II)Wherein the content of the first and second substances, a vector of measurement errors representing the rate of change of the range difference,representing the true velocity difference between the kth receiving station and the reference receiving station,error of the measured value and the true value representing the speed difference between the kth receiving station and the reference receiving station.

3. The method for positioning unmanned aerial vehicle with high precision based on TDOA and FDOA as claimed in claim 2, wherein the specific method in step 2 is as follows:

the measured position of the receiving station is expressed as:wherein p is_i＝[x_i,y_i,z_i]^TIndicating the measured position of the receiving station, Δ p_iIndicating errors in the measured and actual positions of the receiving station,representing the true location of the receiving station;

construction of a vector to be estimatedWherein q is^0TThe true position representing the object is transposed,the true velocity representing the target is transposed,indicating that the true position of the kth receiving station is transposed.

4. The method for positioning unmanned aerial vehicle with high precision based on TDOA and FDOA as claimed in claim 3, wherein the specific method in step 3 is as follows:

constructing corresponding measured values of the range difference, measured values of the range difference change rate and expressions between the measured values of the receiving station position and the vector eta to be estimated:

Γ＝Θ-g(η) (5)

wherein the content of the first and second substances,from the difference of distance measurementsMeasured value of rate of change of range differenceAnd the measured value p of the position of the receiving station_iThe components of the composition are as follows,to representTaking out the transpose and transferring the transpose,to representTaking out the transpose and transferring the transpose,representing the measured value p of the position of the Kth receiving station_KTaking and transposing;

representing the measurement error vector, n, corresponding to Θ^TRepresenting the distance difference measurement error n by taking the transpose,the measurement error representing the rate of change of the range difference is transposed,the measured value error of the K-th receiving station position is expressed and transposed;

the first K-1 elements of g (η) are expressed by the measured TDOADetermining that the Kth element to the 2K-2 element are expressed by the measurement FDOAIt is determined that the last 2K-1 th to 5K-2 th elements are determined by the receiving station position coordinates, i.e. p_i＝[x_i,y_i,z_i]^T；

Assuming that Γ mean is 0, the covariance matrix is Q, the form of Q is:

wherein Q is_tCovariance matrix, Q, representing TDOA measurement noise_fCovariance matrix, Q, representing FDOA measurement noise_pCovariance matrix, O, representing errors in position measurements of the receiving station_m×nAn all-zero matrix representing all elements of the m x n dimension as zeros.

5. The method for positioning unmanned aerial vehicle with high precision based on TDOA and FDOA as claimed in claim 4, wherein the specific method in step 4 is as follows:

the cost error function of equation (5) is:

J(η)＝(Θ-g(η))^TQ^-1(Θ-g(η))

the nonlinear weighted least squares optimization model is:

η＝arg min(Θ-g(η))^TQ^-1(Θ-g(η)) (6)

6. the method for positioning unmanned aerial vehicle with high precision based on TDOA and FDOA as claimed in claim 5, wherein the specific method in step 5 is as follows:

let eta be_aIs an initial estimate of η, by g (η) at η_aThe first order taylor expansion of (a) solves equation (5), expanding equation (5) into the form:

wherein the content of the first and second substances,is g (η) at η_aThe jacobian matrix of (a) is of the form:

let r be [ r ]_21,a,…,r_K1,a]^T，r_K1,aRepresenting the initial estimate as eta_aThe measured value of the distance difference between the arrival of the target at the kth receiving station and the arrival at the reference receiving station,the expression represents the initial estimate value of η_aThe true distance difference between the arrival of the target at the kth receiving station and the arrival at the reference receiving station,the measurement position of the K-th receiving station is expressed to be transposed;

thenExpressed in the following form:

substituting equation (7) into equation (6) to obtain a linear weighted least squares optimization model of the (l + 1) th iteration result:

obviously, the above equation is a quadratic optimization problem with respect to the target solution, so there is an optimal closed-form solution:

7. the method for high-precision positioning of unmanned aerial vehicle based on TDOA and FDOA as claimed in claim 6, wherein the specific method in step 7 is as follows:

when it is satisfied withThe iteration is terminated, and the convergence value is obtained at the terminationIs noted as eta_bEta at the end of iteration_bThe first three elements of the target form a position vector of the target, and the 4 th to 6 th elements form a velocity vector of the target, namely the position and the velocity of the target can be obtained.

8. The method for high-precision positioning of UAVs based on TDOA and FDOA of claim 6, wherein η in step 5_aThe value of (d) is obtained by a two-step method of positioning error correction.

9. The method for high-precision positioning of unmanned aerial vehicle based on TDOA and FDOA as recited in claim 8, wherein the two-step method for positioning error correction specifically comprises:

in a first step, a quantity to be estimated is definedDerived using a weighted least squares methodAn estimated value of (d);

and secondly, estimating errors of the target position and speed estimation output in the first step, and obtaining final initial estimation of the target position and speed after correction.

10. The method for high-precision positioning of unmanned aerial vehicle based on TDOA and FDOA as recited in claim 9, further comprising the following steps:

defining a quantity to be estimatedSuppose q^0T,r₁ ⁰,Are uncorrelated and are obtained by using a weighted least squares methodIs determined by the estimated value of (c),the estimation equation of (a) is:

wherein ξ₁A vector representing the components of the measurement error term,

obtained according to formula (11)Weighted least squares estimation of (c):

in the above formula, W₁Is defined as:

θ₁the covariance matrix of the estimation error is:

the second step is that:

estimating the error Δ q and of the target position and velocity estimates output in the first stepObtaining final estimation of the target position and speed after correction;

in the first step q⁰Is determined by the estimated value of (c),in the first stepAn estimated value of (d);

using a first order Taylor expansion, r₁ ⁰Andin thatAndunfolding:

wherein the content of the first and second substances,a and B are each independentlyIn thatAndand (3) a gradient matrix of a first-order Taylor series expansion term is formed, and the expressions of A and B are respectively as follows, ignoring the Taylor expansion terms with more than two orders:

the estimation equation of the second step is established as follows:

in the formula: xi₂Vector representing the composition of the measurement error term, Δ θ₁Is composed ofAnd theta₁A difference of (d);

the second step to be estimated parameter can be obtained from the equation (23)The weighted least squares solution of (c) is:

wherein the weighting matrix is

Correction of θ using equation (28)₁Updating the estimated values of the target position and velocity information:

θ₃＝[θ₁(1:3)，θ₁(5:7)]^T-θ₂。

Technical Field

The invention relates to an unmanned aerial vehicle positioning method, in particular to an unmanned aerial vehicle high-precision positioning method based on TDOA and FDOA.

Background

The scientific technology of the world develops rapidly nowadays, and the form of war also changes fundamentally. The war under modern high-tech conditions is a multi-dimensional integrated war which is based on information technology and relates to the fields of sea, land, air, sky, electricity and the like. Electronic warfare is the dominant one, and accurate positioning and tracking of the radiation source is an important function of modern electronic warfare systems. In the military field, the accurate estimation of the state information of the radiation source is beneficial to the acquisition of battlefield information and the use of an accurate guided weapon, and can provide powerful support for hitting enemy targets; in the civil field, accurate navigation and positioning service can be provided for the society. Therefore, the precise positioning and tracking of the radiation source has wide application prospect in military and civil fields.

Target location can be divided into active location and passive location depending on whether the receiving station sends electromagnetic wave signals to the target. The active positioning method is that a positioning system transmits electromagnetic waves outwards and receives echo signals from a target. And the target is detected, positioned and tracked through processing and analyzing the echo signals. It has the advantages of all weather and high precision. However, since the frequency band of the electromagnetic waves radiated outwards is fixed, the electromagnetic waves are easy to be found and tracked by enemies, and then the electromagnetic waves are hit by targeted electronic interference and accurate guided weapons, so that the positioning performance of the target is reduced, and the safety of a positioning system is endangered.

The passive positioning method is a technology in which a positioning system does not transmit an electromagnetic wave signal to a target, and determines the position of the target only by using radiation information of a target radiation source. Passive positioning systems position a target radiation source by measuring the rate of change of direction angle and phase of the signal from the source to the receiving station, or by measuring the time difference between the arrival of the signal at multiple receiving stations. When there is relative motion between the radiation source target and the receiving stations, the frequency difference between the signals arriving at the multiple receiving stations can also be used to locate the target.

The positioning system combining different measurement information can integrate the advantages of different measurement information, enhance the adaptability to the signal types of the radiation sources, improve the positioning precision of the radiation sources to a certain extent and reduce the number of receiving stations. Modern positioning systems are typically mounted on drones, satellites, ships, and other mobile platforms. In fact, random errors often exist in the positioning of the mobile platform, and the positioning accuracy of the radiation source based on the time difference and the frequency difference is very sensitive to the position of the receiving station, so that the statistical information of the random errors of the position of the receiving station needs to be considered to improve the positioning accuracy of the radiation source.

Disclosure of Invention

In order to solve the technical problems, the invention provides a high-precision positioning method of an unmanned aerial vehicle based on TDOA and FDOA, which combines the TDOA and FDOA positioning technologies for use and takes the position error of a receiving station into account when parameter estimation is carried out, thereby improving the positioning performance of a moving target and achieving the aim of accurately positioning the moving target.

In order to achieve the purpose, the technical scheme of the invention is as follows:

a high-precision positioning method for an unmanned aerial vehicle based on TDOA and FDOA comprises the following steps:

step 1: equating the TDOA measurements and the FDOA measurements received by the receiving stations to range difference measurements and range difference rate measurements using K receiving stations in space;

step 2: performing joint estimation on the position coordinates of the receiving station, and constructing a vector eta to be estimated, which comprises an unknown real target position and speed vector and an unknown real coordinate vector of the receiving station;

and step 3: constructing corresponding range difference measurement values, range difference change rate measurement values and expressions between the receiving station position measurement values and the vector eta to be estimated;

and 4, step 4: defining a cost error function of the expression in the step 3, and solving a nonlinear weighted least square optimization model of the cost error function;

and 5: setting the expression in step 3 at an initial value eta_aPerforming first-order Taylor series expansion and substituting the first-order Taylor series expansion into the nonlinear weighted least square optimization model in the step 4 to obtain an optimal closed solution of the vector eta to be estimated;

step 6: setting the condition of stopping iteration, 0 < epsilon < 1, repeating the step 5;

and 7: and when the iteration termination condition is met, terminating the iteration to obtain the position and the speed of the target.

In the scheme, the specific method of the step 1 is as follows:

with K receiving stations, the true position of the ith receiving station isTrue velocity ofTrue position of the target is q⁰＝[x⁰,y⁰,z⁰]^TThe true speed of the target isThe true distance between the target and the ith receiving station is expressed as:

let 1 st receiving stationFor a reference receiving station, the difference between the true distance from the target to the ith receiving station and the reference receiving station is:

wherein r is_i ⁰Representing the true distance, r, between the target and the i-th receiving station₁ ⁰Representing target and reference receptionThe true distance between stations, c is the electromagnetic wave propagation speed;

then, respectively obtaining derivatives of the two ends of the formula (1) for the time t to obtain the true distance change rate between the target and the ith receiving station as follows:

wherein, the symbol (·)^TRepresenting a transpose of a matrix or vector;

the true velocity difference between the ith and reference receiving stations is:

wherein the content of the first and second substances,is the true rate of change of range between the target and the reference receiving station, f₀Is the frequency of the electromagnetic wave signal and,is the true FDOA value between the ith receiving station and the reference receiving station;

the TDOA measurement is equivalent to a range difference measurement, which is expressed as:k, wherein,representing the true difference in distance, Δ t, between the target arriving at the ith receiving station and arriving at the reference receiving station_i1Represents the time difference measurement error between the target arriving at the ith receiving station and arriving at the reference receiving station, namely the TDOA measurement error;

assuming that the range difference measurements follow a Gaussian distribution with a mean value of 0i-2, 3, …, K is written in vector form:wherein the content of the first and second substances,n＝[n₂₁,n₃₁,...,n_K1]^Tn denotes a vector of measurement error components of the distance difference, r_K1Representing a measured value of a distance difference between the target arriving at the kth receiving station and arriving at the reference receiving station,representing the true distance difference between the target arriving at the Kth receiving station and the reference receiving station, n_K1Representing the error between the measured value of the distance difference between the target arriving at the Kth receiving station and the reference receiving station and the true value;

similarly, the FDOA measured value is equivalent to a measured value of the rate of change of the range difference, which is expressed asK, wherein,representing the true speed difference, Δ f, between the ith and the reference receiving stations_i1A measurement error representing a frequency difference between the target arrival ith receiving station and the reference receiving station, i.e., an FDOA measurement error;

assuming that the measurements of the rate of change of range difference follow a gaussian distribution with a mean value of 0 and are uncorrelated with the TDOA measurements; handleK is written in vector form as 2,3Wherein the content of the first and second substances, a vector of measurement errors representing the rate of change of the range difference,representing the true velocity difference between the kth receiving station and the reference receiving station,error of the measured value and the true value representing the speed difference between the kth receiving station and the reference receiving station.

In the scheme, the specific method of the step 2 is as follows:

the measured position of the receiving station is expressed as:wherein p is_i＝[x_i,y_i,z_i]^TIndicating the measured position of the receiving station, Δ p_iIndicating errors in the measured and actual positions of the receiving station,representing the true location of the receiving station;

construction of a vector to be estimatedWherein q is^0TThe true position representing the object is transposed,the true velocity representing the target is transposed,indicating that the true position of the kth receiving station is transposed.

In the above scheme, the specific method of step 3 is as follows:

constructing corresponding measured values of the range difference, measured values of the range difference change rate and expressions between the measured values of the receiving station position and the vector eta to be estimated:

Γ＝Θ-g(η) (5)

wherein the content of the first and second substances,from the difference of distance measurementsMeasured value of rate of change of range differenceAnd the measured value p of the position of the receiving station_iThe components of the composition are as follows,to representTaking out the transpose and transferring the transpose,to representTaking out the transpose and transferring the transpose,representing the measured value p of the position of the Kth receiving station_KTaking and transposing;

representing the measurement error vector, n, corresponding to Θ^TRepresenting the distance difference measurement error n by taking the transpose,the measurement error representing the rate of change of the range difference is transposed,the measured value error of the K-th receiving station position is expressed and transposed;

the first K-1 elements of g (η) are expressed by the measured TDOAK, K is determined by the measurement FDOA expression for the K-th element through the 2K-2-th elementK, the last 2K-1 to 5K-2 elements are determined by the receiving station position coordinates, i.e., p_i＝[x_i,y_i,z_i]^T；

Assuming that Γ mean is 0, the covariance matrix is Q, the form of Q is:

wherein Q is_tCovariance matrix, Q, representing TDOA measurement noise_fCovariance matrix, Q, representing FDOA measurement noise_pCovariance matrix, O, representing errors in position measurements of the receiving station_m×nAn all-zero matrix representing all elements of the m x n dimension as zeros.

In the above scheme, the specific method of step 4 is as follows:

the cost error function of equation (5) is:

J(η)＝(Θ-g(η))^TQ^-1(Θ-g(η))

the nonlinear weighted least squares optimization model is:

η＝arg min(Θ-g(η))^TQ^-1(Θ-g(η)) (6)

in the above scheme, the specific method of step 5 is as follows:

let eta be_aIs an initial estimate of η, by g (η) at η_aThe first order taylor expansion of (a) solves equation (5), expanding equation (5) into the form:

wherein the content of the first and second substances,is g (η) at η_aThe jacobian matrix of (a) is of the form:

let r be [ r ]_21,a,...,r_K1,a]^T，r_K1,aRepresenting the initial estimate as eta_aThe measured value of the distance difference between the arrival of the target at the kth receiving station and the arrival at the reference receiving station,the expression represents the initial estimate value of η_aThe true distance difference between the arrival of the target at the kth receiving station and the arrival at the reference receiving station,indicating that the measurement position of the K-th receiving station is transposed.

ThenExpressed in the following form:

substituting equation (7) into equation (6) to obtain a linear weighted least squares optimization model of the (l + 1) th iteration result:

obviously, the above equation is a quadratic optimization problem with respect to the target solution, so there is an optimal closed-form solution:

in the above scheme, the specific method of step 7 is as follows:

when it is satisfied withThe iteration is terminated, and the convergence value is obtained at the terminationIs noted as eta_bEta at the end of iteration_bThe first three elements of the target form a position vector of the target, and the 4 th to 6 th elements form a velocity vector of the target, namely the position and the velocity of the target can be obtained.

In a further technical scheme, eta in the step 5_aThe value of (d) is obtained by a two-step method of positioning error correction.

In a further technical solution, the two-step method for correcting the positioning error specifically includes:

in a first step, a quantity to be estimated is definedDerived using a weighted least squares methodAn estimated value of (d);

and secondly, estimating errors of the target position and speed estimation output in the first step, and obtaining final initial estimation of the target position and speed after correction.

In a further technical scheme, the first step:

defining a quantity to be estimatedSuppose q^0T,Are uncorrelated and are obtained by using a weighted least squares methodIs determined by the estimated value of (c),the estimation equation of (a) is:

wherein ξ₁A vector representing the components of the measurement error term,

obtained according to formula (11)Weighted least squares estimation of (c):

in the above formula, W₁Is defined as:

θ₁the covariance matrix of the estimation error is:

the second step is that:

estimating the error Δ q and of the target position and velocity estimates output in the first stepObtaining final estimation of the target position and speed after correction;

in the first step q⁰Is determined by the estimated value of (c),in the first stepAn estimated value of (d);

using a first order Taylor expansion, r₁ ⁰Andin thatAndunfolding:

wherein the content of the first and second substances,a and B are each independentlyIn thatAndand (3) a gradient matrix of a first-order Taylor series expansion term is formed, and the expressions of A and B are respectively as follows, ignoring the Taylor expansion terms with more than two orders:

the estimation equation of the second step is established as follows:

in the formula: xi₂Vector representing the composition of the measurement error term, Δ θ₁Is composed ofAnd theta₁A difference of (d);

the second step to be estimated parameter can be obtained from the equation (23)The weighted least squares solution of (c) is:

wherein the weighting matrix is

Correction of θ using equation (28)₁Updating the estimated values of the target position and velocity information:

θ₃＝[θ₁(1:3)，θ₁(5:7)]^T-θ₂。

through the technical scheme, the high-precision positioning method of the unmanned aerial vehicle based on the TDOA and the FDOA has the following beneficial effects:

1. the method and the device perform Taylor series joint iterative estimation on the position coordinates of the receiving station while estimating the target position and speed, thereby reducing the influence of the position error of the receiving station on the positioning precision.

2. The TDOA and FDOA measured values are simultaneously used for estimation, the positioning method has no time difference ambiguity problem, and the defects of the TDOA positioning method can be effectively overcome.

3. The invention provides a more accurate iteration initial value by utilizing a two-step method of positioning error correction, and improves the target positioning precision while ensuring the convergence and improving the convergence speed.

Drawings

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below.

FIG. 1 is a schematic flow chart of a TDOA and FDOA-based high-precision positioning method for an UAV disclosed in the embodiments of the present invention;

fig. 2 is a schematic diagram of a relationship between an unmanned aerial vehicle and a receiving station disclosed in the embodiment of the present invention.

Detailed Description

The technical solution in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention.

The invention provides a high-precision positioning method of an unmanned aerial vehicle based on TDOA and FDOA, which specifically comprises the following steps as shown in FIG. 1:

step 1: equating the TDOA measurements and FDOA measurements received by the receiving stations to range difference measurements and range difference rate of change measurements using the K receiving stations in the space shown in fig. 2;

with K receiving stations, the true position of the ith receiving station isTrue velocity ofTrue position of the target is q⁰＝[x⁰,y⁰,z⁰]^TThe true speed of the target isThe true distance between the target and the ith receiving station is expressed as:

let 1 st receiving stationFor a reference receiving station, the difference between the true distance from the target to the ith receiving station and the reference receiving station is:

wherein r is_i ⁰Representing the true distance, r, between the target and the i-th receiving station₁ ⁰Representing the real distance between the target and the reference receiving station, and c is the propagation speed of the electromagnetic wave;

then, respectively obtaining derivatives of the two ends of the formula (1) for the time t to obtain the true distance change rate between the target and the ith receiving station as follows:

wherein, the symbol (·)^TRepresenting a transpose of a matrix or vector;

the true velocity difference between the ith and reference receiving stations is:

wherein the content of the first and second substances,is aimed atReference to the true rate of change of distance between receiving stations, f₀Is the frequency of the electromagnetic wave signal and,is the true FDOA value between the ith receiving station and the reference receiving station;

the TDOA measurement is equivalent to a range difference measurement, which is expressed as:k, wherein,representing the true difference in distance, Δ t, between the target arriving at the ith receiving station and arriving at the reference receiving station_i1Represents the time difference measurement error between the target arriving at the ith receiving station and arriving at the reference receiving station, namely the TDOA measurement error;

assuming that the range difference measurements follow a Gaussian distribution with a mean value of 0i 2, 3.. K writes in vector form:wherein the content of the first and second substances,n＝[n₂₁,n₃₁,...,n_K1]^Tn denotes a vector of measurement error components of the distance difference, r_K1Representing a measured value of a distance difference between the target arriving at the kth receiving station and arriving at the reference receiving station,representing the true distance difference between the target arriving at the Kth receiving station and the reference receiving station, n_K1Representing the error between the measured value of the distance difference between the target arriving at the Kth receiving station and the reference receiving station and the true value;

likewise, the FDOA measurements are equated to distancesA measure of the rate of change of the dispersion, a measure of the rate of change of the range difference being expressed asK, wherein,representing the true speed difference, Δ f, between the ith and the reference receiving stations_i1A measurement error representing a frequency difference between the target arrival ith receiving station and the reference receiving station, i.e., a measurement error of the FDOA; assuming that the measurements of the rate of change of range difference follow a gaussian distribution with a mean value of 0 and are uncorrelated with the TDOA measurements; handleK is written in vector form as 2,3Wherein the content of the first and second substances, a vector of measurement errors representing the rate of change of the range difference,

representing the true velocity difference between the kth receiving station and the reference receiving station,error of the measured value and the true value representing the speed difference between the kth receiving station and the reference receiving station.

Step 2: performing joint estimation on the position coordinates of the receiving station, and constructing a vector eta to be estimated, which comprises an unknown real target position and speed vector and an unknown real coordinate vector of the receiving station;

the measured position of the receiving station is expressed as:wherein p is_i＝[x_i,y_i,z_i]^TIndicating the measured position of the receiving station, Δ p_iIndicating errors in the measured and actual positions of the receiving station,representing the true location of the receiving station;

construction of a vector to be estimatedWherein q is^0TThe true position representing the object is transposed,the true velocity representing the target is transposed,indicating that the true position of the kth receiving station is transposed.

And step 3: constructing corresponding measured values of the range difference, measured values of the range difference change rate and expressions between the measured values of the receiving station position and the vector eta to be estimated:

Γ＝Θ-g(η) (5)

wherein the content of the first and second substances,from the difference of distance measurementsMeasured value of rate of change of range differenceAnd the measured value p of the position of the receiving station_iThe components of the composition are as follows,to representTaking out the transpose and transferring the transpose,to representTaking out the transpose and transferring the transpose,representing the measured value p of the position of the Kth receiving station_KTaking and transposing;

representing the measurement error vector, n, corresponding to Θ^TRepresenting the distance difference measurement error n by taking the transpose,the measurement error representing the rate of change of the range difference is transposed,the measured value error of the K-th receiving station position is expressed and transposed;

the first K-1 elements of g (η) are expressed by the measured TDOAK, K is determined by the measurement FDOA expression for the K-th element through the 2K-2-th elementK, the last 2K-1 to 5K-2 elements are determined by the receiving station position coordinates, i.e., p_i＝[x_i,y_i,z_i]^T；

Assuming that Γ mean is 0, the covariance matrix is Q, the form of Q is:

wherein Q is_tCovariance matrix, Q, representing TDOA measurement noise_fCovariance matrix, Q, representing FDOA measurement noise_pCovariance matrix, O, representing errors in position measurements of the receiving station_m×nAn all-zero matrix representing all elements of the m x n dimension as zeros.

And 4, step 4: defining a cost error function of the expression in the step 3, and solving a nonlinear weighted least square optimization model of the cost error function;

the cost error function of equation (5) is:

J(η)＝(Θ-g(η))^TQ^-1(Θ-g(η))

the nonlinear weighted least squares optimization model is:

η＝arg min(Θ-g(η))^TQ^-1(Θ-g(η)) (6)

and 5: setting the expression in step 3 at an initial value eta_aPerforming first-order Taylor series expansion and substituting the first-order Taylor series expansion into the nonlinear weighted least square optimization model in the step 4 to obtain an optimal closed solution of the vector eta to be estimated;

let eta be_aIs an initial estimate of η, by g (η) at η_aThe first order taylor expansion of (a) solves equation (5), expanding equation (5) into the form:

wherein the content of the first and second substances,is g (η) at η_aThe jacobian matrix of (a) is of the form:

let r be [ r ]_21,a,...,r_K1,a]^T，r_K1,aRepresenting the initial estimate as eta_aThe measured value of the distance difference between the arrival of the target at the kth receiving station and the arrival at the reference receiving station,the expression represents the initial estimate value of η_aThe true distance difference between the arrival of the target at the kth receiving station and the arrival at the reference receiving station,indicating that the measurement position of the K-th receiving station is transposed.

ThenExpressed in the following form:

substituting equation (7) into equation (6) to obtain a linear weighted least squares optimization model of the (l + 1) th iteration result:

obviously, the above equation is a quadratic optimization problem with respect to the target solution, so there is an optimal closed-form solution:

wherein eta is_aThe value of (a) is obtained by a two-step method of positioning error correction, which is specifically as follows:

the first step is as follows:

defining a quantity to be estimatedSuppose q^0T,r₁ ⁰,Are uncorrelated and are obtained by using a weighted least squares methodIs determined by the estimated value of (c),the estimation equation of (a) is:

wherein ξ₁A vector representing the components of the measurement error term,

obtained according to formula (11)Weighting ofLeast square estimation:

in the above formula, W₁Is defined as:

θ₁the covariance matrix of the estimation error is:

the second step is that:

estimating the error Δ q and of the target position and velocity estimates output in the first stepObtaining final estimation of the target position and speed after correction;

in the first step q⁰Is determined by the estimated value of (c),in the first stepAn estimated value of (d);

using a first order Taylor expansion, r₁ ⁰Andin thatAndunfolding:

wherein the content of the first and second substances,a and B are each independentlyIn thatAndand (3) a gradient matrix of a first-order Taylor series expansion term is formed, and the expressions of A and B are respectively as follows, ignoring the Taylor expansion terms with more than two orders:

the estimation equation of the second step is established as follows:

in the formula: xi₂Representing measurement error termsVector of composition, Δ θ₁Is composed ofAnd theta₁A difference of (d);

the second step to be estimated parameter can be obtained from the equation (23)The weighted least squares solution of (c) is:

wherein the weighting matrix is

Correction of θ using equation (28)₁Updating the estimated values of the target position and velocity information:

θ₃＝[θ₁(1:3)，θ₁(5:7)]^T-θ₂。

step 6: setting the end iteration condition epsilon, 0 < epsilon < 1, and repeating the step 5.

And 7: when it is satisfied withThe iteration is terminated, and the convergence value is obtained at the terminationIs noted as eta_bEta at the end of iteration_bThe first three elements of the target form a position vector of the target, and the 4 th to 6 th elements form a velocity vector of the target, namely the position and the velocity of the target can be obtained.

The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.