Unmanned aerial vehicle cluster cooperative positioning method based on dynamic performance evaluation

Abstract

The application discloses a kind of unmanned aerial vehicle cluster cooperative positioning methods based on dynamic performance evaluation, each unmanned aerial vehicle uses the output information of airborne inertial sensor to complete system state quantity update, in short time content maintains absolute positioning accuracy;Then between each unmanned aerial vehicle is completed inter-aircraft ranging and cooperative message transmission using data link, the coupling relationship between ranging error, prior position error information, space geometry configuration is comprehensively considered, and all measurement information is used to construct weighted cooperative accuracy factor to be used to represent cooperative positioning performance lower bound;By successively eliminating the observation matrix of each adjacent unmanned aerial vehicle, the contribution degree of each adjacent unmanned aerial vehicle is calculated respectively, and the measurement information with high contribution degree is screened out to participate in cooperative positioning calculation.The application can reduce the calculation complexity of navigation system and the communication frequency of data link, improve the real-time performance and accuracy of cooperative positioning, and can be used for high-precision navigation and positioning of unmanned aerial vehicle cluster under satellite denial and complex environment.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of unmanned system cluster navigation and positioning, and particularly relates to a method for cooperative positioning of a UAV cluster based on dynamic performance evaluation. BACKGROUND

[0002] In recent years, single unmanned aerial vehicle (UAV) is difficult to adapt to various operation task requirements in complex environments such as cities and hills due to the limitation of the carrier platform. A cluster composed of multiple UAVs can effectively exert the performance of the UAV cluster through information interaction and cooperative operation between the carriers, and the high-precision absolute positioning information is one of the prerequisites for the UAV cluster to perform tasks.

[0003] In the current navigation and positioning system of the UAV cluster, each UAV is configured with an inertial sensor. The inertial navigation has strong anti-interference capability, and the use environment is not limited by the region. The inertial navigation position solution result can be continuously output in the whole environment at all times. Therefore, the inertial sensor is an important component in the existing integrated navigation mode. However, the positioning error of the inertial navigation will continuously accumulate over time, and the high-precision inertial device has high cost and large size. Due to the load limitation, a small UAV can only carry a micro-electro-mechanical system (MEMS) inertial sensor with low precision. Therefore, the global navigation satellite system (GNSS) and inertial data fusion are needed to complete the positioning error correction and compensation. However, the GNSS external navigation information is easy to be interfered and cheated. Therefore, the cooperative positioning through inter-aircraft ranging is an effective method for the navigation of the UAV cluster, which can maintain the overall positioning accuracy of the cluster and reduce the hardware cost of the navigation sensor.

[0004] The cooperative positioning of the UAV cluster is similar to the cooperative positioning of the wireless sensor network. Through information sharing and inter-aircraft ranging between the UAVs, the distributed positioning solution can be completed by using the geometric constraint relationship. The accuracy of the cooperative positioning depends on the quality and quantity of the observation information. In theory, when the filter based on the minimization of the mean square error is used for data fusion, the optimal estimation can be obtained by combining the motion model of the UAV itself after obtaining the measurement information of different qualities. However, the increase of the communication overhead between the UAVs and the increase of the positioning solution complexity can easily cause the real-time performance to be poor. Therefore, it is of great scientific and application value to establish a dynamic performance evaluation system for the cooperative positioning of the UAV cluster, select the measurement information with high contribution to the positioning of the UAV itself, and improve the positioning accuracy of the UAV cluster. SUMMARY

[0005] The application aims to solve the problem of high-precision navigation and positioning of the existing UAV cluster in the satellite denial and complex environment. The application provides a method for cooperative positioning of a UAV cluster based on dynamic performance evaluation.

[0006] Technical solution: A kind of unmanned aerial vehicle cluster cooperative positioning method based on dynamic performance evaluation provided by the application, comprising the following steps:

[0007] (1) initialize the inertial navigation system state quantity of each unmanned aerial vehicle, set the state estimation initial value in Kalman filter, covariance matrix initial value;

[0008] (2) using the high-frequency output of airborne MEMS inertial sensor, the acceleration and angular velocity in the three orthogonal axis directions of unmanned aerial vehicle coordinate system are used to complete one-step prediction calculation of unmanned aerial vehicle attitude, velocity and position three state quantities;

[0009] (3) each unmanned aerial vehicle broadcasts its own information through data link, and also receives the data link cooperative information of adjacent unmanned aerial vehicles;

[0010] (4) the unmanned aerial vehicle receives the measurement information of n adjacent unmanned aerial vehicles, judges whether the number of adjacent unmanned aerial vehicles is greater than the preset threshold, when the condition is met, enter (5), when the condition is not met, directly enter step (7);

[0011] (5) considering the coupling relationship between ranging error, prior position error information and space geometry configuration, a weighted cooperative accuracy factor is constructed with all measurement information to represent the lower bound of cooperative positioning performance;

[0012] (6) by excluding the observation matrix of each adjacent unmanned aerial vehicle in turn, the contribution degree of each adjacent unmanned aerial vehicle is calculated, and the measurement information with high contribution degree is selected to participate in cooperative positioning calculation;

[0013] (7) the system measurement update is completed by using Kalman filter, the inertial navigation positioning error is corrected, the cooperative positioning result in strapdown inertial navigation solution platform is output, and the related information is encoded and transmitted to data communication equipment, and the next cycle is started from step (2), until the unmanned aerial vehicle ends the positioning calculation.

[0014] Further, the unmanned aerial vehicle inertial navigation system state quantity in step (1) is:

[0015]

[0016] wherein, is the platform error angle of inertial navigation system in east direction, north direction and sky direction, δv=[δv E δv N δv U ] is the velocity error of unmanned aerial vehicle in east direction, north direction and sky direction, δp=[δL δλ δh] is the latitude, longitude and height error of unmanned aerial vehicle, ε b =[ε bx ε by ε bz] is the constant drift error of the gyro, ε r = [ε rx ε ry ε rz ] is the first-order Markov drift error of the gyro, is the first-order Markov state quantity of the accelerometer;

[0017] According to the position, velocity, attitude, and inertial sensor characteristics of the starting time of the UAV, the state quantity initial value X (0) and the system noise W (0) , the covariance matrix initial value P (0) is set according to the uncertainty of the state quantity initial value, so as to ensure the stability and convergence speed of the filter.

[0018] Further, the one-step prediction of the three state quantities in step (2) is solved according to the differential equation of the inertial error, and a discrete form of the state transition equation is constructed, and the mathematical expression is:

[0019] X (k) = Φ (k|k-1) X (k-1) + G (k-1) W (k-1)

[0020] Wherein, Φ (k|k-1) is the error state transition matrix, G (k-1) is the system noise matrix, and W (k-1) is the system noise.

[0021] Further, the step (3) is implemented as follows:

[0022] The UAV outputs three types of navigation positioning information: position p = [L λ h], velocity v = [v E v N v U ], and attitude a = [φ θ γ]; meanwhile, each UAV encodes its own number, timestamp, three-dimensional position, and prior position error information and transmits them to the data communication equipment for broadcasting.

[0023] Further, the step (5) is implemented as follows:

[0024] (51) Construct a cooperative positioning error propagation model:

[0025] The position of the UAV in the geocentric coordinate system is x = (x, y, z), and it is assumed that n adjacent UAVs can establish data communication with it, and their positions are x ri = (x ri , y ri , z ri ), and the real distance between the adjacent UAV and the UAV is di ||x-x ri ||, considering the prior position error δx of the adjacent UAV ri and its own position error δx, the measured distance d i is represented as:

[0026] d i ′=||x+δx-(x ri +δx ri )||+ε i

[0027] where δx is the position correction vector [δx, δy, δz] T , δx ri is the prior position error of the i-th adjacent UAV, ε i is the ranging error;

[0028] (52) Geometric representation of ranging information:

[0029] After linearizing the above formula and ignoring high-order terms, we get:

[0030]

[0031] The i-th element of the ranging residual vector v is v i = d i ′- d i , and the linear approximation is obtained:

[0032]

[0033] where,

[0034] Express the ranging residual of all adjacent UAVs in vector form:

[0035] v = Gδx + Jδx ri + ε

[0036] where G is a geometric matrix with dimensions n x 3; J is a position error influence matrix of adjacent UAVs with dimensions n x 3n, and ε is a ranging error vector;

[0037] (53) Objective function solution:

[0038] By introducing a weight matrix W, we get the regression estimate under different variance observations. The weight matrix W is as follows:

[0039]

[0040] where, is the variance of the total error of the adjacent UAVs; construct the objective function:

[0041] min δx (v-Gδx) T W(v-Gδx)

[0042] Generally, G is full rank, which guarantees the uniqueness and stability of the solution. The derivative of the objective function with respect to δx is set to zero to obtain:

[0043] δx=(G T WG) -1 G T Wv

[0044] (54)System error term definition: The covariance matrix of δx directly reflects the distribution of the estimation error, which is defined as:

[0045] Cov(δx)=E[(δx-E[δx])(δx-E[δx]) T ]

[0046] Where E[·] is the mean or expectation, and Cov(·) is the covariance.

[0047] The ranging error ε i satisfies the normal distribution, with a mean of zero and a variance that is positively correlated with the distance size. Each ranging error is independent of each other. The prior position error δx ri of each adjacent UAV is independent of each other.

[0048] From the cooperative positioning error propagation model, the covariance matrix of the ranging residual error vector v is:

[0049] Cov(v)=JCov(δx r )J T +Cov(ε)

[0050] Where Cov(δx r ) is composed of Cov(δx ri ), which reflects the distribution of the position error of each adjacent UAV, and Cov(δx ri ) is Cov(ε) is the covariance of the ranging error. According to the ranging error model, the variance is positively correlated with the distance size, and according to the ranging error model , we get where k is the proportionality coefficient.

[0051] (55)Lower bound of cooperative positioning performance WCDOP:

[0052] Substitute Cov(v) into Cov(δx) and expand and solve to get:

[0053] Cov(δx)=(G T WG)-1 [G T WJCov(δx r )J T WG

[0054] +σ 2 G T WRWG](G T WG) -1

[0055] Position covariance Cov(δx) contains the contribution of all influencing factors, in which the diagonal elements directly quantify the positioning accuracy of the unknown UAV in three-axis direction. WCDOP is defined as:

[0056] WCDOP = trace(Cov(δx))

[0057] WCDOP describes the uncertainty of the positioning solution, which provides the basis for collaborative positioning performance optimization.

[0058] Further, the contribution degree of each adjacent UAV in step (6) is calculated as follows:

[0059] First, the position information of all adjacent UAVs received is converted to the ECEF coordinate system using the rotation matrix, and it is judged whether the adjacent UAVs are collinear with the unknown UAV or the included angle formed is less than the set threshold. If the above conditions are met, the adjacent UAV with larger sum of prior position error and ranging error needs to be removed; then the observation vector g i of each UAV is removed in turn, and the contribution degree of all UAVs to the collaborative positioning solution is calculated, and the change value of WCDOP is defined as:

[0060]

[0061] According to the contribution degree of each adjacent UAV, a dynamic performance evaluation system is established, and the measurement information with high contribution degree is selected to participate in the collaborative positioning solution.

[0062] Further, the system measurement update method in step (7) is as follows:

[0063] The conversion matrix is used to establish a unified dimension of position error in two coordinate systems:

[0064]

[0065] Where, R N is the radius of curvature of the circle, and f is the earth flattening;

[0066] When the measurement information of the four adjacent UAVs selected is used for positioning solution, the measurement equation of the collaborative positioning system is as follows:

[0067]

[0068] wherein, V (k) is the measurement noise of the unmanned aerial vehicle participating in the cooperative positioning solution.

[0069] Further, the preset threshold value in step (4) is 4.

[0070] Further, the number of the measurement information with high contribution degree in step (6) is greater than or equal to a preset threshold value.

[0071] Beneficial effects: Compared with the prior art, the beneficial effects of the present application: based on the cooperative positioning using inter-aircraft ranging, the present application comprehensively analyzes the influence of ranging error, prior position error information and spatial geometric configuration on the cooperative positioning performance; according to the principle of solving the unknown unmanned aerial vehicle position by the closed-form analytical algorithm, the lower bound WCDOP of the cooperative positioning performance is constructed, considering the coupling relationship between the influencing factors; the contribution degree of each adjacent unmanned aerial vehicle is calculated and a dynamic performance evaluation system is established, the measurement information with high contribution degree is selected to participate in the cooperative positioning solution, and finally the absolute position information of the unmanned aerial vehicle is output; the present application can reduce the calculation complexity of the navigation system and the communication frequency of the data link, improve the real-time performance and accuracy of the cooperative positioning, and can be used for high-precision navigation and positioning of unmanned aerial vehicle cluster in satellite denial and complex environment. BRIEF DESCRIPTION OF DRAWINGS

[0072] Figure 1 is the architecture diagram of the unmanned aerial vehicle cluster cooperative positioning system based on dynamic performance evaluation constructed by the present application;

[0073] Figure 2 is the flowchart of the present application;

[0074] Figure 3 is the initial position distribution diagram of the unmanned aerial vehicle cluster constructed by the present application;

[0075] Figure 4 is the change surface diagram of the unmanned aerial vehicle cooperative positioning performance under the influence of different factors, wherein (a) is the positioning performance change surface diagram after ignoring the influence of the position error of the adjacent unmanned aerial vehicle, (b) is the positioning performance change surface diagram after considering the influence of the position error of the adjacent unmanned aerial vehicle; (c) is the positioning performance change surface diagram of the unmanned aerial vehicle No. 2 with smaller prior position error; (d) is the positioning performance change surface diagram of the unmanned aerial vehicle No. 2 with larger prior position error;

[0076] Figure 5 is the flight trajectory diagram of the unmanned aerial vehicle cluster constructed by the present application;

[0077] Figure 6It is a three-axis positioning error comparison chart of the method and configuration optimal screening method, the closest screening method of the application;

[0078] Figure 7 It is an error cumulative distribution comparison chart of the method and configuration optimal screening method, the closest screening method of the application. DETAILED DESCRIPTION

[0079] The application will be further described in detail below with reference to the accompanying drawings.

[0080] As shown in Figure 1 , Figure 2 , the application provides a UAV cluster cooperative positioning method based on dynamic performance evaluation. Each UAV uses the output information of the on-board inertial sensor to complete system state quantity updating, and maintains absolute positioning accuracy in a short time. Then, each UAV uses the data link to complete inter-aircraft ranging and message transmission, and establishes a dynamic performance evaluation system according to the cooperative messages received from adjacent UAVs. The contribution degree of each adjacent UAV is calculated, and the measurement information with high contribution degree is selected to participate in cooperative positioning calculation, so as to reduce the system calculation complexity and communication frequency while ensuring the overall positioning accuracy of the cluster. The high-precision navigation and positioning problem of the existing UAV cluster in satellite denial and complex environment is solved. The specific implementation is as follows:

[0081] Step 1: initialize the inertial navigation system state quantity of each UAV, set the initial value of state estimation and the initial value of covariance matrix in the Kalman filter.

[0082] The inertial navigation system state quantity of the UAV is:

[0083]

[0084] Among them, is the platform error angle of the inertial navigation system in the east direction, north direction and sky direction, δv=[δv E δv N δv U ] is the velocity error of the UAV in the east direction, north direction and sky direction, δp=[δL δλ δh] is the latitude, longitude and height error of the UAV, ε b =[ε bx ε by ε bz ] is the constant drift error of the gyroscope, ε r =[ε rx ε ry ε rz ] is the first-order Markov drift error of the gyroscope, is the first-order Markov state quantity of the accelerometer.

[0085] According to the starting time of the position, speed, attitude, inertia sensor characteristic setting state quantity initial value X of unmanned aerial vehicle (0) And system noise W (0) , covariance matrix initial value P (0) Need to set according to the uncertainty of the initial state, so as to ensure the stability and convergence speed of the filter.

[0086] Step 2: using the high frequency output of onboard MEMS inertial sensor unmanned aerial vehicle itself coordinate system three orthogonal axis direction acceleration and angular velocity, complete unmanned aerial vehicle attitude, speed, position three state quantity one step prediction calculation.

[0087] System state quantity one step prediction can be solved according to the differential equation of inertia error, the discrete form of state transition equation is constructed, the mathematical expression is:

[0088] X (k) =Φ (k|k-1) X (k-1) +G (k-1) W (k-1)

[0089] Where, Φ (k|k-1) For error state transition matrix, G (k-1) For system noise matrix, W (k-1) For system noise.

[0090] Step 3: each unmanned aerial vehicle broadcasts its own number, three-dimensional position, prior position error information through data link, and also receives the data link coordination information of adjacent unmanned aerial vehicles.

[0091] Unmanned aerial vehicle can output position p = [L λ h], velocity v = [v E v N V U ], attitude a = [φ θ γ] three kinds of navigation positioning information output, at the same time, each unmanned aerial vehicle encodes its own number, time stamp, three-dimensional position, prior position error information and transmits it to the data communication equipment for broadcast.

[0092] Step 4: unmanned aerial vehicle receives the measurement information of n adjacent unmanned aerial vehicles, judges whether the number of adjacent unmanned aerial vehicles is greater than 4, when the condition is met, all measurement information is used to construct weighted collaborative accuracy factor (WCDOP) to represent the lower bound of collaborative positioning performance; When the condition is not met, the Kalman filter is used to complete the system measurement update.

[0093] Step 5: build collaborative positioning dynamic performance evaluation system: consider the coupling relationship between ranging error, prior position error information and space geometry configuration, use all measurement information to construct WCDOP to represent the lower bound of collaborative positioning performance.

[0094] (5.1) System model construction:

[0095] The position of the UAV in the Earth-Centered Earth-Fixed (ECEF) coordinate system is x = (x, y, z). Assuming that there are n adjacent UAVs that can establish data communication with it, their positions are x ri = (x ri , y ri , z ri ), the real distance between the adjacent UAV and the UAV is d i = ||x-x ri ||, considering the prior position error δx ri of the adjacent UAV and the position error δx of itself, the measured distance d′ i is expressed as:

[0096] d′ i = ||x+δx-(x ri +δx ri )||+ε i

[0097] where δx is the position correction vector [δx, δy, δz] T , δx ri is the prior position error of the i-th adjacent UAV, and ε i is the ranging error.

[0098] (5.2) Geometric representation of ranging information:

[0099] After linearizing the above equation and ignoring high-order terms, we get:

[0100]

[0101] The i-th element of the ranging residual vector v is v i = d i ′-d i , and the linear approximation can be obtained as:

[0102]

[0103] where,

[0104] Express the ranging residual of all adjacent UAVs as a vector:

[0105] v = Gδx + Jδx ri + ε

[0106] where G is a geometric matrix with dimensions n x 3; J is a position error influence matrix of adjacent UAVs with dimensions n x 3n, and ε is a ranging error vector.

[0107] (5.3) Objective function solution:

[0108] The regression estimation under different variance observation data is obtained by introducing the weight matrix W, and the weight matrix W is as follows:

[0109]

[0110] wherein, is the variance of the total error of the adjacent UAVs.

[0111] The objective function is constructed:

[0112] min δx (v-Gδx) T W(v-Gδx)

[0113] Generally, G is full rank, which can guarantee the uniqueness and stability of the solution. The derivative of the objective function with respect to δx is obtained and set to zero:

[0114] δx=(G T WG) -1 G T Wv

[0115] The system error term is defined in equation (5.4):

[0116] The covariance matrix of δx directly reflects the distribution of the estimation error, which is defined as:

[0117] Cov(δx)=E[(δx-E[δx])(δx-E[δx]) T ]

[0118] Wherein, E[·] is the average or expected value, and Cov(·) is the covariance.

[0119] The ranging error ε i satisfies the normal distribution, the mean is zero, the variance is positively correlated with the distance size, and each ranging error is independent of each other; the prior position error δx ri of each adjacent UAV is independent of each other.

[0120] According to the system error propagation model, the covariance matrix of the ranging residual error vector v is:

[0121] Cov(v)=JCov(δx r )J T +Cov(ε)

[0122] Wherein, Cov(δx r ) is composed of Cov(δx ri ) reflecting the distribution of the position error of each adjacent UAV, and Cov(δx ri ) is Cov(ε) is the covariance of ranging error, whose variance is positively correlated with the distance according to the ranging error model

[0123] (5.5) WCDOP solution:

[0124] Substitute Cov(v) into Cov(δx) and expand and solve it:

[0125] Cov(δx) = (G T WG) -1 [G T WJCov(δx r )J T WG

[0126] +σ 2 G T WRWG](G T WG) -1

[0127] The position covariance Cov(δx) contains the contribution of all factors, in which the diagonal elements directly quantify the positioning accuracy of the unknown UAV in three-axis directions. WCDOP is defined as:

[0128] WCDOP = trace(Cov(δx))

[0129] WCDOP describes the uncertainty of the positioning solution, which can provide a basis for cooperative positioning performance optimization.

[0130] Step 6: Calculate the contribution of each neighboring UAV

[0131] Convert the received position information of all neighboring UAVs to ECEF coordinate system using rotation matrix, judge whether there is collinearity between the neighboring UAV and the unknown UAV or the included angle is less than the set threshold, if the above conditions are met, the neighboring UAV with larger sum of prior position error and ranging error needs to be removed. Then remove the observation vector g i of each UAV in turn, calculate the contribution of all UAVs to the cooperative positioning solution, and the change value of WCDOP is defined as:

[0132]

[0133] According to the contribution of each neighboring UAV, a dynamic performance evaluation system is established, and the measurement information with high contribution is selected to participate in the cooperative positioning solution. ​

[0134] The measurement information of the four high-contribution adjacent unmanned vehicles after screening is used for positioning calculation, and the measurement equation of the cooperative positioning system is as follows:

[0135]

[0136] Wherein, V (k) is the measurement noise of the unmanned vehicle participating in the cooperative positioning calculation.

[0137] Since the system measurement model is constructed based on the geocentric coordinate system, the state quantity in the inertial navigation system is in the northeast geographical system, and the conversion matrix is used to establish the unified dimension of the position error in the two coordinate systems

[0138]

[0139] Wherein, R N is the curvature radius of the prime vertical circle, and f is the earth flattening.

[0140] Figure 3 is the initial position distribution map of the unmanned vehicle cluster constructed by the application, wherein the cross symbol represents an unknown unmanned vehicle, and the round symbol represents an adjacent unmanned vehicle. Figure 4 is the change surface map of the cooperative positioning performance of the unmanned vehicle of the application under different influence factors. It is assumed that Figure 3 No. 1 and No. 3 are reference unmanned vehicles with high-precision position information, No. 2, No. 4 and No. 5 have large position errors, and the positioning performance change simulation results of the unknown unmanned vehicle on the plane with a height of 25 m are given. Among them, Figure 4 (a) is the positioning performance change surface map after ignoring the influence factor of the position error of the adjacent unmanned vehicle, Figure 4 (b) is the positioning performance change surface map considering the influence factor of the position error of the adjacent unmanned vehicle. Since No. 2 unmanned vehicle has a better elevation angle compared to others, only from the spatial geometric configuration, the unmanned vehicle has a greater contribution to the cooperative positioning performance improvement, Figure 4 (c) is the positioning performance change surface map of No. 2 unmanned vehicle with small prior position error, and Figure 4 (d) is the positioning performance change surface map of No. 2 unmanned vehicle with large prior position error. From Figure 5 The results show that the dynamic performance evaluation WCDOP reasonably represents the coupling relationship between the position error and the three influence factors, and can provide a basis for the screening of high-contribution measurement information.

[0141] Figure 6 is the flight trajectory map of the unmanned vehicle cluster constructed by the application, and the six unmanned vehicles maneuver according to the preset trajectory. Figure 7Is the three-axis positioning error comparison chart of the method and the configuration optimal screening method, the nearest distance screening method of the present application; wherein the curve marked with an asterisk is the screening method based on dynamic performance evaluation in the present application, the curve marked with a square marker is the configuration optimal screening method, and the curve marked with a circle marker is the nearest distance screening method. The method proposed in the present application improves the cooperative positioning accuracy of the unmanned aerial vehicle by more than 15%, reduces the dependence of the unmanned aerial vehicle cluster cooperative navigation and positioning on the data link communication large bandwidth and high frequency, and reduces the calculation complexity while ensuring the positioning accuracy.

[0142] ​ Is the error cumulative distribution comparison chart of the method and the configuration optimal screening method, the nearest distance screening method of the present application; wherein the curve marked with an asterisk is the positioning error cumulative distribution curve based on dynamic performance evaluation in the present application, the curve marked with a pentagram marker is the positioning error cumulative distribution curve of the configuration optimal screening method, and the curve marked with a diamond marker is the positioning error cumulative distribution curve of the nearest distance screening method. The method proposed in the present application improves the overall positioning accuracy of the unmanned aerial vehicle cluster, and can be used for high-precision navigation and positioning of the unmanned aerial vehicle cluster under satellite denial and complex environment.

[0143] The above-described embodiments only express the embodiments of the present application, which are described in detail and specifically, but should not be understood as limiting the scope of the patent. It should be noted that for ordinary skilled in the art, without departing from the concept of the present application, a number of modifications and improvements can be made, which are all within the scope of the present application. Therefore, the scope of the patent of the present application should be subject to the appended claims.

Claims

1. A method for cooperative positioning of a UAV swarm based on dynamic performance evaluation, characterized in that, The method comprises the following steps: (1) initializing the state quantity of the inertial navigation system of each unmanned aerial vehicle, setting the initial value of state estimation and the initial value of covariance matrix in the Kalman filter; (2) using the high-frequency output of the airborne MEMS inertial sensor in the three orthogonal axis directions of the unmanned aerial vehicle coordinate system to complete one-step prediction calculation of the three state quantities of the unmanned aerial vehicle, namely attitude, velocity and position; (3) each unmanned aerial vehicle broadcasts its own information through the data link, and also receives the data link cooperative information of adjacent unmanned aerial vehicles; (4) the unmanned aerial vehicle receives the measurement information of n adjacent unmanned aerial vehicles, judges whether the number of adjacent unmanned aerial vehicles is greater than a preset threshold, and when the condition is met, enters step (5), and when the condition is not met, directly enters step (7); (5) considering the coupling relationship between the ranging error, the prior position error information and the spatial geometric configuration, a weighted cooperative accuracy factor is constructed by using all the measurement information to represent the lower bound of the cooperative positioning performance; (6) the contribution degree of each adjacent unmanned aerial vehicle is calculated by sequentially eliminating the observation matrix of each adjacent unmanned aerial vehicle, and the measurement information with high contribution degree is selected to participate in the cooperative positioning solution; (7) the system measurement update is completed by using the Kalman filter to correct the inertial navigation positioning error, and the cooperative positioning result in the strapdown inertial navigation solution platform is output, and the related information is encoded and transmitted to the data communication equipment, and the next cycle is started from step (2), until the unmanned aerial vehicle completes the positioning solution; The contribution degree calculation method of each adjacent unmanned aerial vehicle in step (6) is as follows: Firstly, the position information of all adjacent UAVs received is converted to the ECEF coordinate system using a rotation matrix, and it is determined whether the adjacent UAVs are collinear with the unknown UAV or form an angle less than a set threshold. If the above conditions are met, the adjacent UAV with a larger sum of prior position error and ranging error needs to be removed; then the observation vector g i of each UAV is removed in turn, and the contribution of all UAVs to the cooperative positioning solution is calculated. The change value of WCDOP is defined as: Wherein, G is a geometric matrix, and W is a weight matrix; According to the contribution degree of each adjacent unmanned aerial vehicle, a dynamic performance evaluation system is established, and the measurement information with high contribution degree is selected to participate in the cooperative positioning solution; The system measurement update method in step (7) is as follows: Utilizing conversion matrices Establishing the unified dimension of position error in two coordinate systems: where R N is the meridian radius of curvature, f is the flattening of the Earth; L is the latitude, λ is the longitude, and h is the height. When the measurement information of the four adjacent unmanned aerial vehicles selected is used for positioning solution, the cooperative positioning system measurement equation is as follows: where V (k) is the measurement noise of the UAV participating in the cooperative positioning solution.

2. The method of claim 1, wherein, The state quantity of the unmanned aerial vehicle inertial navigation system in step (1) is: wherein, is the platform error angle of the inertial navigation system in the east, north, and sky directions, is the velocity error of the unmanned aerial vehicle in the east, north, and sky directions, δp = [δL δλ δh] is the latitude, longitude, and height error of the unmanned aerial vehicle, ε b = [ε bx ε by ε bz ] is the gyro constant drift error, ε r = [ε rx ε ry ε rz ] is the gyro first-order Markov drift error, is the accelerometer first-order Markov state quantity; According to the position, speed, attitude, inertia sensor characteristic setting state quantity initial value X of the starting time of the unmanned aerial vehicle (0) And system noise W (0) Covariance matrix initial value P (0) Need to set according to the uncertainty of the initial state quantity, so as to ensure the stability and convergence speed of the filter.

3. The method of claim 1, wherein, The one-step prediction of the three state quantities in step (2) is solved according to the inertial error differential equation, and the discrete form of the state transition equation is constructed, and the mathematical expression is: X (k) = Φ (k|k-1) X (k-1) + G (k-1) W (k-1) where Φ (k|k-1) is the error state transition matrix, G (k-1) is the system noise matrix, and W (k-1) is the system noise.

4. The method of claim 1, wherein, The implementation process of step (3) is as follows: The UAV outputs position p = [L λ h], velocity Attitude a = [φ θ γ] These three kinds of navigation positioning information; At the same time, each UAV encodes its own number, timestamp, three-dimensional position, and prior position error information and transmits them to the data communication device for broadcasting.

5. The method of claim 1, wherein, The implementation process of step (5) is as follows: (51) constructing a cooperative positioning error propagation model: The position of the unmanned aerial vehicle in the geocentric and fixed coordinate system is x=(x, y, z). It is assumed that n adjacent unmanned aerial vehicles can establish data communication with the unmanned aerial vehicle, and the positions of the n adjacent unmanned aerial vehicles are x ri =(x ri ,y ri ,z ri ), the real distance between the adjacent unmanned aerial vehicle and the unmanned aerial vehicle is d i =||x-x ri ||, the prior position error δx ri of the adjacent unmanned aerial vehicle and the position error δx of the unmanned aerial vehicle are considered, and the measured distance d′ i is represented as: d' i =||x+δx-(x ri +δx ri )||+ε i wherein δx is a position correction vector [δx, δy, δz] T , δx ri is an i-th neighboring UAV prior position error, ε i is a ranging error; (52) geometric representation of ranging information: After linearizing the above formula and ignoring the high-order terms, the following formula is obtained: The i-th row element of the range residual vector v is v i = d' i - d i Substituting the linear approximation into the equation gives: wherein, The ranging residual error of all adjacent unmanned aerial vehicles is expressed in the form of a vector: v = Gdx + Jdx ri + ε Wherein, G is a geometric matrix, and the dimension is n x 3; J is a position error influence matrix of adjacent unmanned aerial vehicles, and the dimension is n x 3n; and ε is a ranging error vector; (53) solving the objective function: By introducing the weight matrix W, the regression estimation under different variance observation data is obtained, and the weight matrix W is as follows: wherein, is the variance of the total error of the adjacent drones; a target function is constructed: min δx (ν-Gδx) T W(ν-Gδx) G is full, which can ensure the uniqueness and stability of the solution, and the derivative of the objective function with respect to δx is zero: δx = (G T WG) -1 G T Wν (54) system error term definition: the covariance matrix of δx directly reflects the distribution of the estimation error, and is defined as: Cov(δx) = E[(δx - E[δx])(δx - E[δx]) T ] Wherein, E[·] is the average value or expectation value, and Cov(·) is the covariance. Ranging error ε i satisfies normal distribution, the mean is zero, the variance is positively correlated with the distance size, and each ranging error is independent of each other; each adjacent UAV prior position error δx ri is independent of each other; According to the cooperative positioning error propagation model, the covariance matrix of the ranging residual vector v is: Cov(v) = J Cov(δx r )J T +Cov(ε) wherein Cov(δx r ) is composed of Cov(δx ri ) reflecting the position error distribution of each adjacent UAV, and Cov(δx ri ) is Cov(ε) is the covariance of the ranging error. According to the ranging error model, the variance is positively correlated with the distance size, and according to the ranging error model Cov(ε) = kR2 wherein k is a proportional coefficient; (55)Solution of WCDOP: Substitute Cov(v) into Cov(δx) and expand and solve it to obtain: Cov(δx) = (G T WG) -1 [G T WJCov(δx r )J T WG+σ 2 G T WRWG](G T WG) -1 The position covariance Cov(δx) contains the contributions of all influencing factors, wherein the diagonal elements directly quantify the positioning accuracy of the unknown UAV in the three-axis direction. WCDOP is defined as: WCDOP=trace(Cov(δx)) WCDOP describes the uncertainty of the positioning solution, and provides a basis for cooperative positioning performance optimization.

6. The method of claim 1, wherein, The preset threshold in step (4) is 4.

7. The method of claim 1, wherein, The number of high-contribution measurement information in step (6) is greater than or equal to a preset threshold.

Images