Cooperative positioning method fusing land-based navigation and inter-machine distance measurement information

Abstract

The invention belongs to the technical field of target positioning, and particularly relates to a land-based navigation and inter-machine distance measurement fused cooperative positioning method in a satellite denial environment. The method is mainly applied to unmanned aerial vehicle group cooperative positioning, especially in an environment in which satellite signals are blocked or interfered. According to the scheme, measurement data of the angle of arrival (AOA) and the time of arrival (TOA) are provided through a plurality of base stations, and the positions of the unmanned aerial vehicles are globally optimized by adopting a multi-dimensional scale (MDS) algorithm and Prushing analysis in combination with measurement data of the relative distance between the unmanned aerial vehicles. According to the method, the positioning precision is improved, the robustness and the real-time performance of the system are enhanced, and the method is especially suitable for unmanned aerial vehicle group positioning requirements in a complex environment. The method provides an efficient and reliable solution for high-precision unmanned aerial vehicle group cooperative positioning in a satellite denial environment, and has a wide application prospect.

Description

Technical Field

[0001] The present invention belongs to the technical field of target positioning, and in particular relates to an unmanned aerial vehicle (UAV) collaborative positioning algorithm integrating land-based navigation and inter-aircraft ranging information under satellite navigation denial conditions. Background Art

[0002] With the rapid development of drone technology, drone group collaboration has been widely used in many fields, such as environmental monitoring, disaster relief, military reconnaissance, etc. The collaborative positioning technology of drone groups is particularly important in an environment without GPS or satellite signal interference. In order to achieve high-precision positioning, many studies have realized the positioning of drones based on the measurement guidance information of ground base stations. This method has achieved certain research results, but there are still some key technical bottlenecks:

[0003] (1) Positioning methods based on land-based guidance are often affected by noise and measurement errors. In practical applications, due to the uncertainty of sensor accuracy, environmental complexity, and transmission signals, there are certain errors in land-based measurements under long-distance measurement conditions. These errors will lead to deviations in positioning results, especially in multi-base station and multi-UAV positioning scenarios, where the errors may be amplified and affect positioning accuracy.

[0004] (2) Cooperation between drones is not fully utilized. When positioning a drone group, existing technologies often only focus on the measurement data between a single drone and the base station. Although drone groups can improve positioning accuracy by exchanging relative distance information with each other, existing positioning algorithms usually fail to make full use of this collaborative information, resulting in the positioning accuracy of the system not being effectively improved.

[0005] Therefore, although the positioning method based on land-based guidance information in satellite-denied environments has made some progress in some cases, existing research has not fully utilized the collaborative information between drone groups to improve positioning accuracy. Summary of the invention

[0006] In view of the above problems, the present invention proposes a collaborative positioning method using MDS (multidimensional scaling) algorithm and Proucker analysis. By comprehensively considering the AOA and TOA measurement information of the land-based base station and the relative distance data between the drones, the MDS algorithm and Proucker analysis are used to optimize the positioning results, thereby significantly improving the positioning accuracy in the satellite denial environment. Through this method, the impact of measurement errors on positioning accuracy can be effectively reduced, and the collaborative information between drone groups can be used to improve positioning accuracy and system robustness. This method not only improves positioning accuracy, but also reduces computational complexity, and has higher real-time and adaptability.

[0007] The technical solution adopted by the present invention is:

[0008] A collaborative positioning method integrating land-based navigation and inter-machine ranging information. The positioning system includes a base station and a UAV. The position of the base station is set to bs = (x0, y0), and the initial position of the UAV is ms. i =(x i ,y i ), the speed is V i =(v xi ,v yi ); The collaborative positioning method comprises the following steps:

[0009] S1. At each moment, the base station measures the drone multiple times to obtain the distance between the base station and the drone. and angle At the same time, the distance between drones is measured to obtain the distance measurement information between drones. Wherein, the superscript m represents the measured value, j represents the jth measurement, the total number of measurements is defined as M, and the interval between each measurement is T;

[0010] S2. Use the measurement data of the base station to make a preliminary estimate of the drone's position, and define the linear equation of the measurement information between the base station and the drone as:

[0011] G toa YZ0=Y toa ,G aoa YZ0=Y aoa

[0012] Among them, Z0 is the position estimation vector, G toa and G aoa are matrices related to TOA and AOA measurements, respectively, toa and Y aoa is the corresponding measurement result, the total number of drones is i, the total number of multiple measurement points is M, and each measurement time is represented by j;

[0013] The parametric form of the linear equation is as follows:

[0014] G j_toa =[G T 1j_toa …G T ij_toa ] T

[0015] G ij_TOA =[-2x0 -2y0 -2x0(j-1)T -2y0(j-1)T 1 ((j-1)T) 2 2(j-1)T]

[0016] Y j =[YT ij_toa …Y T ij_toa ] T ,

[0017] G j =[G T 1j_aoa …G T ij_aoa ] T

[0018] Y j =[Y T ij_aoa …Y T ij_aoa ] T ,

[0019] The least square method is used to make a preliminary position estimate, and G = [G toa ; G aoa ],Y=[Y toa ; Y aoa ]:

[0020] Z0=(G T G) -1 G T Y

[0021] S3. Use the inter-machine distance measurement information to locate the drone:

[0022] Define the number of drones as n, and use the measured inter-drone distance information to get the distance square matrix D as:

[0023]

[0024] Construct the centralization matrix J:

[0025]

[0026] Among them, I n is the n×n identity matrix, 1 n is an n×1 column vector of all ones;

[0027] Construct the inner product matrix B:

[0028]

[0029] Perform eigenvalue decomposition on matrix B to obtain the eigenvalue matrix E and eigenvector matrix V:

[0030] BV=EV

[0031] Sort the eigenvalues ​​and select the first two largest eigenvalues ​​and their corresponding eigenvectors to calculate the drone position:

[0032]

[0033] Among them, V(:,1:2) are the first two eigenvectors, is the square root of the first two largest eigenvalues, and the final estimated drone position Z mds is an n×2 matrix;

[0034] S4: Fuse the preliminary estimated position signal with the UAV position to achieve collaborative positioning and obtain the final UAV positioning information, including:

[0035] Add Z0 obtained from S2 and Z obtained from S3 mds Defined as input matrices X and Y respectively, X and Y are centered, that is, the centroid of each matrix is ​​moved to the origin:

[0036]

[0037] Scale X' and Y' to unit norm respectively, and scale Y' and X' to the same scale by the scaling factor so that their norms are equal:

[0038]

[0039] In order to align Y' and X', it is necessary to solve the rotation matrix T so that the transformed Y' is closest to X'. The matrix T can be calculated by singular value decomposition (SVD):

[0040] First find the auxiliary matrix A:

[0041] A=X′ T Y′

[0042] Then decompose A by SVD:

[0043] A=UΣV T

[0044] Among them, Σ is a diagonal matrix, U and V are orthogonal matrices. The rotation matrix T is:

[0045] T=VU T

[0046] Using the rotation matrix T, the transformed Y' is closest to X', and the transformed Y is Y'·T, and the final positioning result Z can be obtained:

[0047] Z=b·Y′·T+c

[0048] Where b is the scaling factor, T is the rotation matrix and c is the translation vector.

[0049] The beneficial effects of the present invention are as follows: the present invention significantly improves the positioning accuracy of a group of drones in a satellite denial or signal interference environment by fusing the angle of arrival (AOA) and time of arrival (TOA) information of the base station and the distance measurement data between drones. A short-term multiple measurement model is adopted, combined with the noise characteristics of multiple measurements, to optimize the measurement data and effectively reduce the impact of measurement errors. The position information of the drone group is globally optimized through the MDS algorithm and Prucker analysis, making full use of the collaborative information between drones, which not only improves the overall positioning accuracy, but also effectively reduces the impact of the measurement error of a single drone on the system positioning results. In addition, while improving the accuracy, the present invention also enhances the robustness of the system, enabling it to maintain high-precision positioning in complex environments and be more adaptable. Overall, the present invention improves the computational efficiency and real-time performance of the positioning system by optimizing the calculation process and algorithm implementation, and can adapt to the collaborative positioning needs of larger drone groups. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 A model diagram of a collaborative positioning method that integrates land-based navigation and inter-aircraft ranging at a certain moment;

[0051] Figure 2 This is the core flow chart of the algorithm;

[0052] Figure 3 It is the positioning accuracy of the fusion of land-based navigation and inter-machine ranging under different numbers of short-time measurement points. DETAILED DESCRIPTION

[0053] The technical solution of the present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0054] The present invention provides a collaborative positioning method in a satellite-denied environment that integrates land-based navigation and inter-machine ranging information. The method uses the measurement data of multiple base stations on UAVs, including arrival time (TOA) and arrival angle (AOA) information, combined with the relative distance data between UAVs, and uses the MDS algorithm and Prucker analysis to perform global position optimization. By integrating this information, not only can accurate positioning be performed in a satellite-denied or interference environment, but also the positioning accuracy can be optimized through multiple measurements, thereby improving the robustness and real-time performance of the system.

[0055] The specific implementation steps of the present invention are summarized as follows:

[0056] Step 1: Environmental parameter setting: Set parameters such as base station location, number of drones, initial position, speed, number of measurement points, and measurement error standard deviation.

[0057] Step 2: Generate multiple measurement data: Measure the distance and angle between the base station and the drone multiple times to simulate the measurement noise in reality. Generate noisy distance and angle measurement results through these measurement data.

[0058] Step 3: Position estimation: The AOA and TOA information measured by the base station are used to make a preliminary estimate of the position of the drone. Then, the distance measurement data between drones is combined to perform collaborative positioning through the MDS algorithm.

[0059] Step 4: Optimization and evaluation: Use Prucker analysis to fuse the relative positions of multiple UAVs and ground-based information, calculate the positioning results of the two, and evaluate them through the root mean square error (RMSE).

[0060] Example

[0061] This embodiment uses the Matlab simulation platform to carry out experiments.

[0062] The specific implementation method of the present invention performs collaborative positioning of a group of drones in a single land-based base station environment, utilizes the AOA and TOA information of the base station and the relative distance information between drones, performs multiple measurements and optimizes the global position in combination with the MDS algorithm.

[0063] In this example, the environment parameters are first set, including the base station position and the initial position and speed of the drone. Assume that there is one base station and four drones are randomly distributed in a two-dimensional plane. The position of the base station is fixed, and the initial position and speed of all drones are randomly generated.

[0064] Step 1: Environment parameter settings

[0065] Base station location: Set a base station, whose location is the coordinate origin bs = [0,0].

[0066] Drone initial position and speed: The initial positions of the four drones are randomly generated in the range of [1000,3000], and the speed of each drone is randomly generated in the range of -15 to 15 m / s.

[0067] Noise settings: Define the measurement error standard deviation: Base station distance measurement error σ r1 =2m, angle error σ θ =0.1 degrees, and the measurement error between drones is σr2=1m.

[0068] Step 2: Generate measurement data

[0069] For the distance and angle between each drone and the base station, the system generates distance and angle data through multiple measurements. The time measurement interval is 0.5s, the number of multiple measurements is set to 3-8 times, and Gaussian noise is added to these data to simulate the actual measurement process. The measurement data is generated through the following steps:

[0070] (1) Calculation of true distance and angle:

[0071] The actual distance and angle between each drone and the base station at each measurement time are calculated through geometric relationships. The distance is calculated using the Euclidean distance:

[0072] r true (t)=||ms j (t)-bs i ||

[0073] The angle is calculated by the coordinate difference between the base station and the drone:

[0074]

[0075] (2) Generation of noisy measurement data:

[0076] Based on the real measurement values, random noise is added to generate noisy measurement data:

[0077] r(t)=r true (t)+∈ r ,θ(t)=θ true (t)+∈ θ

[0078] in, and Represents measurement error.

[0079] Step 3: Distance measurement between drones

[0080] The relative distance between drones is calculated dynamically by their position changes. For any pair of drones, the relative distance at each moment is calculated and the error is added:

[0081] d ij (t)=||ms i (t)-ms j (t)||

[0082] After adding the error, we get the relative distance with noise:

[0083]

[0084] in, is the distance measurement error.

[0085] Step 4: Preliminary location estimate

[0086] The least squares method is used to estimate the position of each drone using the AOA and TOA data from the base station. The distance and angle values ​​of each measurement point form a linear equation. Based on these measurement data, the least squares method is used to obtain a preliminary position estimate Z0. The solved positions are: [2942.9, 2462.4], [1469.1, 1498.9], [2639, 1260.8], [1048.6, 1966.6].

[0087] Step 5: MDS algorithm for multi-UAV relative positioning optimization

[0088] Based on the relative distance information between drones, the MDS algorithm is used to optimize the global position of the drones. The specific steps are as follows:

[0089] (1) Constructing the distance matrix: For each pair of UAVs ij, the distance matrix is ​​constructed by calculating their relative positions.

[0090] (2) Perform eigenvalue decomposition: Perform eigenvalue decomposition on the distance matrix and extract the first two largest eigenvalues ​​and the corresponding eigenvectors.

[0091] (3) Position estimation: Based on the maximum eigenvalue and eigenvector, the position of the drone is estimated. The results are: [-1011.1, 512.16], [593.9, -204.66], [-519.27, -637.03], [936.5, 329.53].

[0092] Step 6: Calculate the absolute position using Prucker analysis

[0093] Based on the MDS results of the inter-machine distance measurement and the preliminary positioning results of the land-based positioning, information fusion and position alignment are achieved through centralization and rotation matrix calculation. The final position estimate is: [2938.1, 2465.6], [14731499.3], [2639.7, 1253.9], [1048.9, 1969.9]. It is closest to the actual simulated position [2938, 2468.7], [1471.9, 1495.8], [2641.6, 1253.3], [1047.5, 1966.7], and the effect is relatively ideal.

[0094] The accuracy of the above scheme is evaluated:

[0095] The positioning accuracy is evaluated by calculating the root mean square error (RMSE) between the estimated position and the true position. The RMSE calculation formula is as follows:

[0096]

[0097] in, is the estimated position, ms i is the actual location.

[0098] Based on the above experimental scene settings, the experimental results under different measurement points M were statistically analyzed. The results of 5000 Monte Carlo experiments showed that the positioning accuracy was significantly improved with the increase of the number of measurement points. In particular, after adding the inter-machine ranging information through the MDS algorithm and the Prucker analysis algorithm, the positioning accuracy was significantly improved compared with the single-station AOA+TOA method. This reflects the effectiveness of the method of the present invention.

Claims

1. A collaborative positioning method integrating land-based navigation and inter-machine ranging information. The positioning system includes a base station and a UAV. The position of the base station is set to bs = (x0, y0), and the initial position of the UAV is ms i =(x i ,y i ), the speed is V i =(v xi ,v yi ); characterized in that, The collaborative positioning method comprises the following steps: S1. At each moment, the base station measures the drone multiple times to obtain the distance between the base station and the drone. and angle At the same time, the distance between drones is measured to obtain the distance measurement information between drones. Wherein, the superscript m represents the measured value, j represents the jth measurement, the total number of measurements is defined as M, and the interval between each measurement is T; S2. Use the measurement data of the base station to make a preliminary estimate of the drone's position, and define the linear equation of the measurement information between the base station and the drone as: G toa YZ0=Y toa ,G aoa YZ0=Y aoa Among them, Z0 is the position estimation vector, G toa and G aoa are matrices related to TOA and AOA measurements, respectively, toa and Y aoa is the corresponding measurement result, the total number of drones is i, the total number of multiple measurement points is M, and each measurement time is represented by j; The parametric form of the linear equation is as follows: G ij_TOA =[-2x0-2y0-2x0(j-1)T-2y0(j-1)T 1((j-1)T) 2 2(j-1)T] Define G = [G toa ; G aoa ],Y=[Y toa ; Y aoa ], and make a preliminary position estimate by the least squares method: Z0=(G T G) -1 G T Y S3. Use the inter-machine distance measurement information to locate the drone: Define the number of drones as n, and use the measured inter-drone distance information to get the distance square matrix D as: Construct the centralization matrix J: Among them, I n is the n×n identity matrix, 1 n is an n×1 column vector of all ones; Construct the inner product matrix B: Perform eigenvalue decomposition on matrix B to obtain the eigenvalue matrix E and eigenvector matrix V: BV=EV Sort the eigenvalues ​​and select the first two largest eigenvalues ​​and their corresponding eigenvectors to calculate the drone position: Among them, V(:,1:2) are the first two eigenvectors, is the square root of the first two largest eigenvalues, and the final estimated drone position Z mds is an n×2 matrix; S4: Fuse the preliminary estimated position signal with the UAV position to achieve collaborative positioning and obtain the final UAV positioning information, including: Add Z0 obtained from S2 and Z obtained from S3 mds Defined as input matrices X and Y respectively, X and Y are centered, that is, the centroid of each matrix is ​​moved to the origin: Scale X' and Y' to unit norm respectively, and scale Y' and X' to the same scale by the scaling factor so that their norms are equal: In order to align Y' and X', the rotation matrix T needs to be solved so that the transformed Y' is closest to X'; the matrix T is calculated by singular value decomposition (SVD): First find the auxiliary matrix A: A=X ′T AND' Then decompose A by SVD: A=UΣV T Among them, Σ is a diagonal matrix, U and V are orthogonal matrices, and the rotation matrix T is: T=VU T Using the rotation matrix T, the transformed Y' is closest to X', and the transformed Y is Y'·T, and the final positioning result Z is obtained: Z=b·Y′·T+c Where b is the scaling factor, T is the rotation matrix and c is the translation vector.

Images