Collaborative navigation positioning method for dynamically and adaptively adjusting precision factor

Abstract

The invention belongs to the technical field of navigation and positioning in a denial environment, and discloses a collaborative navigation and positioning method for dynamically and adaptively adjusting a precision factor. The method comprises the following steps: S1, a non-denial area reference source unmanned aerial vehicle receives a satellite navigation signal to realize high-precision positioning, and controls the unmanned aerial vehicle to maneuver and fly in a specific area according to self positioning information and movement speed and direction; s2, the reference source unmanned aerial vehicle in the non-denial area broadcasts own position information to other reference source unmanned aerial vehicles and the unmanned aerial vehicle in the denial area through a radio link; s3, obtaining position information of each reference source unmanned aerial vehicle by the denial area unmanned aerial vehicle, and calculating distance information relative to the reference source according to the arrival time; s4, the unmanned aerial vehicle in the denial area corrects the inertial navigation error according to the distance information relative to the reference source to obtain corrected absolute navigation positioning information; s5, the unmanned aerial vehicle in the denial area broadcasts the corrected absolute navigation positioning information through a wireless link; and S6, the non-denial area reference source unmanned aerial vehicle calculates the geometric distribution precision factor under the station center coordinate system of the denial area unmanned aerial vehicle according to the corrected absolute navigation positioning information of the denial area unmanned aerial vehicle, and adjusts the movement speed and direction of the non-denial area reference source unmanned aerial vehicle according to the geometric distribution precision factor.

Description

Technical Field

[0001] The present invention belongs to the technical field of navigation and positioning in a denied environment, and in particular relates to a collaborative navigation and positioning method with dynamic adaptive adjustment of precision factor. Background Art

[0002] In a denied environment, satellite navigation is not possible, and navigation and positioning solutions can only be relied upon by inertial navigation equipment, the accuracy of which cannot directly meet the absolute and relative navigation and positioning accuracy requirements of the UAV.

[0003] The unmanned aerial vehicle to be positioned can obtain accurate distance information between the aircraft through relative ranging of the inter-aircraft radio link, and receive the positioning information of other unmanned aerial vehicles, and combine with inertial navigation to realize relative navigation positioning between the aircraft. With this method, since the unmanned aerial vehicle cannot obtain the absolute navigation information source, its absolute navigation positioning error will still accumulate and diverge over time, and cannot meet the absolute navigation positioning accuracy requirements. Usually, the ground non-denied anchor nodes that can perform satellite navigation positioning or have accurate absolute positions are used as high-precision positioning information sources, and coordinated navigation and positioning are performed with unmanned aerial vehicles in the denied area to provide accurate absolute navigation positioning information for the unmanned aerial vehicles in the denied area. However, since the precision factor of the spatial geometric distribution formed by the anchor node and the unmanned aerial vehicle in the denied area is affected by the distance and orientation between the unmanned aerial vehicle and the ground anchor node, when the unmanned aerial vehicle is far away from the ground anchor node or close to the anchor node, the spatial geometric distribution deteriorates, and the influence of the precision factor on the positioning error gradually increases, which can even cause the positioning error to increase at a rate of ten times, and the anchor node needs to be deployed and planned in advance, and the deployment cost is high.

[0004] In recent years, the Beidou satellite navigation system has completed its networking and realized global satellite navigation services. However, how to provide UAVs with high-precision navigation and positioning capabilities in a denied environment is one of the problems that domestic relevant research institutions urgently need to solve. Therefore, it is very important to study a method to improve the navigation accuracy of UAVs in a denied environment. Summary of the invention

[0005] The purpose of the present invention is to propose a collaborative navigation and positioning method with dynamic adaptive adjustment of precision factor. The UAV in the denied area uses a radio link to measure the distance with the reference source of the UAV in the non-denied area, and receives the navigation and positioning information of the reference source. The distance information and the altitude value output by the barometer are used to realize the fusion of the combined navigation information of inertial navigation / radio ranging / barometric altimeter, suppress the divergence of inertial navigation errors, and obtain higher-precision absolute and relative positioning information. At the same time, the reference source UAV in the non-denied area calculates the observation vector and precision factor formed by the UAV in the denied area and dynamically adjusts its own position, thereby reducing the spatial geometric distribution precision factor, further improving the navigation and positioning accuracy of the UAV in the denied area, and realizing high-precision navigation and positioning in the UAV denied environment. In addition, this method uses the UAV in the non-denied area as a reference source, and does not require the deployment of fixed nodes in advance, saving the cost of deploying anchor points.

[0006] The technical solution of the present invention:

[0007] A collaborative navigation positioning method with dynamic adaptive adjustment of precision factor, the steps are as follows:

[0008] S1: The reference source drone in the non-denied zone receives satellite navigation signals to achieve high-precision positioning, and controls its own maneuvering flight in a specific area based on its own positioning information and movement speed and direction;

[0009] S2: The reference source drone in the non-denied area broadcasts its own position information to other reference source drones and the denied area drones through a radio link;

[0010] S3: The UAV in the denied area obtains the position information of each reference source UAV, and calculates the distance information relative to the reference source based on the arrival time;

[0011] S4: The UAV in the denied area corrects the inertial navigation error according to the distance information relative to the reference source to obtain the corrected absolute navigation positioning information;

[0012] S5: The UAV in the denied zone broadcasts the corrected absolute navigation positioning information through the wireless link;

[0013] S6: The reference source UAV in the non-denied area calculates the geometric distribution precision factor in the station center coordinate system of the UAV in the denied area based on the corrected absolute navigation positioning information of the UAV in the denied area, and adjusts its own movement speed and direction based on the geometric distribution precision factor.

[0014] Further, in S4, the specific correction process is as follows:

[0015] S41: Establish the state equation of the inertial navigation / radio ranging / barometric altimeter integrated navigation system;

[0016] S42: Establishing measurement equations of integrated navigation system;

[0017] S43: According to the state equation and the measurement equation, an EKF filtering method is used to perform information fusion to obtain an error state update of the inertial navigation;

[0018] S44: Correcting the error of the inertial navigation using the error state update amount of the inertial navigation to obtain corrected absolute navigation positioning information.

[0019] Further, in S41, the process of establishing the state equation of the integrated navigation system is as follows:

[0020] Setting state variables, including state variables of the inertial navigation part and state variables of the radio ranging clock error part;

[0021] State variable X of the inertial navigation part I It is composed of conventional inertial navigation parameter error and inertial navigation sensor error:

[0022]

[0023] Among them, φ N ,φ E ,φ D Denotes the attitude error components in the north, east and ground directions, δV N , δV E , δV D Indicates the velocity error components in the north, east and ground directions; Indicates latitude error, longitude error and altitude error; δb gx ,δb gy ,δb gz represents the gyro drift, δb ax ,δb ay ,δb az Indicates accelerometer drift;

[0024] Radio ranging clock error state variable X(t) R Includes: distance error δt equivalent to clock error u and the distance rate error δt equivalent to the clock frequency error ru ;

[0025] The state equation of the inertial navigation system is established as:

[0026]

[0027] F I (t) is the state transfer matrix; W(t) I is the system noise; G I is the noise driving matrix;

[0028] Formulate the radio link clock error differential equation:

[0029]

[0030] where w tu and w tru are the driving white noise of the distance error equivalent to the clock error and the distance rate error equivalent to the clock frequency error, β tru is the inverse of the correlation time of the first-order Markov process;

[0031] The radio link ranging state equation is obtained based on the radio link clock error differential equation:

[0032]

[0033]

[0034] The state equation of the integrated navigation system is as follows:

[0035]

[0036] The state equation of the integrated navigation system is discretized to obtain the state equation of the integrated navigation system in discrete state:

[0037]

[0038] Among them, the state transfer matrix T s is the inertial navigation update time, W k-1 is the system noise sequence; at the same time, W k-1 Satisfaction: E[W k-1 ]=0, Γ k-1 is the system noise driving matrix, and the system noise variance matrix Q k-1 is a non-negative positive definite matrix.

[0039] Further, the integrated navigation system measurement equation includes: a measurement equation formed by a difference between a relative distance calculated by an inertial navigation system and a distance measured by a radio link (i.e., a distance difference),

[0040] The measurement equation is composed of the difference between the rate of change of the distance relative to the reference source calculated based on the inertial navigation position and the rate of change of the distance relative to the reference source measured through the radio link (i.e., the rate of change of the distance difference),

[0041] The measurement equation is composed of the difference between the barometer altitude and the inertial navigation altitude.

[0042] Furthermore, the measurement equation established based on the difference between the relative distance calculated by the inertial navigation and the distance measured by the radio link (i.e., the distance difference) is as follows:

[0043] The position information S obtained by inertial navigation at time k I(k) and the location information S of the reference source drone j J (k) relative distance p Ij (k) is:

[0044]

[0045] The coordinates in the inertial navigation earth-centered earth-fixed rectangular coordinate system (with the earth's center of mass as the origin, the X axis pointing to the intersection of the earth's equatorial plane and the Greenwich meridian plane, the Z axis pointing to the North Pole of the earth, and the Y axis determined by the right-hand coordinate system) are S I (k) = [x I ,y I , z I ] T , the coordinates in the Earth-fixed rectangular coordinate system of the reference source are S J (k) = [x sj ,y sj , z sj ] T ;

[0046] The expression of the inertial navigation calculation relative to the reference source drone is:

[0047] P Ij =r j -e jx δx-e jy δy-e jz δ z

[0048] Among them, δx, δy, and δz are the deviations between the inertial navigation solution position and the actual position in the Earth-centered Earth-fixed rectangular coordinate system, e jx 、e jy 、e jz is the component of the unit observation vector of the reference source UAV relative to the UAV to be positioned in the Earth-centered Earth-fixed rectangular coordinate system, r j is the real relative distance from the reference source to the UAV to be located;

[0049]

[0050] The distance expression relative to the reference source drone obtained by the radio link is:

[0051] p Rj =r j +δt u +v pj

[0052] Among them, v pj To measure noise;

[0053] Subtracting the distance expression of the inertial navigation from the distance expression measured by the radio link can give the distance difference δpj for:

[0054] δp j =-e jx δx-e jy δy-e jz δz-δt u -v pj

[0055] The measurement equation of the distance difference is:

[0056] Z pk =H pk X k +V p

[0057] in:

[0058]

[0059] Where L is latitude, λ is longitude, h is height, R N is the radius of curvature of the angular circle of the reference ellipsoid, and e is the eccentricity of the ellipsoid.

[0060] Furthermore, the process of establishing the measurement equation consisting of the difference between the distance change rate relative to the reference source obtained by inertial navigation position calculation and the distance change rate relative to the reference source obtained by radio link measurement (i.e., the distance difference change rate) is as follows:

[0061] The position information S obtained by inertial navigation from time k-1 to time k I and the location information S of the reference source UAV j J The relative distance change rate for:

[0062]

[0063] The expression of the distance change rate calculated by inertial navigation is:

[0064]

[0065] in, is the actual relative distance change rate from the reference source to the UAV to be located, is the deviation between the position change rate calculated by the inertial navigation and the real position change rate in the earth-centered earth-fixed rectangular coordinate system;

[0066] The expression of the distance change rate relative to the reference source drone obtained by the radio link is:

[0067]

[0068] Among them, v mj To measure noise;

[0069] Subtract the distance change rate expression of the inertial navigation from the distance change rate expression measured by the radio link to obtain the distance difference change rate for:

[0070]

[0071] The measurement equation of the distance difference change rate is obtained as follows:

[0072]

[0073] in:

[0074]

[0075] Furthermore, the measurement equation formed by the difference between the barometer altitude and the inertial navigation altitude is established as follows:

[0076] The height h measured by inertial navigation I for:

[0077] h I =h+δh

[0078] Among them, h is the true height, δh is the inertial navigation height error;

[0079] The altitude measured by the barometer is:

[0080] h B =h+v h

[0081] where v h To measure the noise, we can subtract the above equations (18) and (19) to obtain:

[0082] δh B =h B -h I =δh+v h

[0083] The measurement equation of the height difference change rate is obtained as follows:

[0084] Z hk =H hk X k +V h (t)

[0085] in, .

[0086] Further, in S43, the information fusion process is as follows:

[0087] According to the state equation of the integrated navigation system in the discrete state, the error state equation of the inertial navigation at time k is predicted and obtained.

[0088] For the estimated bias covariance matrix P k Make predictions,

[0089] According to the inertial navigation solution at time k, the position S I (k) and the position information of the reference source UAV j, calculate the relative distance and distance change rate obtained by the inertial navigation; calculate the distance difference, distance difference change rate and height difference based on the distance and distance change rate relative to the reference source UAV obtained by the radio link at time k, and the barometer height value; calculate the H in the measurement equation k Measurement matrix;

[0090] Perform measurement updates and calculate gain matrix Get the inertial guidance error state update Update the estimated error covariance matrix Subtract the state quantity after filtering and updating from the position and velocity of the inertial navigation The error correction has been completed.

[0091] Further, in S6, the specific steps are as follows:

[0092] S61: Calculate the observation vector of each reference source relative to the UAV in the denied area. Take two reference sources as an example. The positions of reference source UAV 1 and reference source UAV 2 in the Earth-centered Earth-fixed coordinate system are [X e1 Y e1 Z e1 ] T 、[X e2 Y e2 Z e2 ] T , the position of the drone in the denied zone is [X e0 Y e0 Z e0 ] T , the observation vector of the reference source UAV 1 relative to the denied zone UAV in the earth-centered earth-fixed rectangular coordinate system [ΔX e1 ΔY e1 ΔZ e1 ] T for:

[0093]

[0094] Similarly, the observation vector [ΔX e2 ΔY e2 ΔZ e2 ] T, the coordinates of the reference source drone 1 in the station center coordinate system with the drone in the denied area as the origin (with the drone's center of mass as the coordinate origin, the X axis pointing to the east, the Y axis pointing to the north, and the Z axis pointing to the sky) [ΔX E1 ΔY N1 ΔZ U1 ] T for:

[0095]

[0096] Among them, the coordinate transformation matrix for:

[0097]

[0098] Similarly, the coordinates of the reference source drone 2 in the station center coordinate system with the drone in the denied area as the origin [ΔX E2 ΔY N2 ΔZ U2 ] T , since the height is already given by the barometer observation, we only need to consider the impact of the horizontal precision factor on the positioning accuracy of the UAV in the denied area. The observation vectors of reference source 1 relative to the UAV in the denied area are The observation vector of reference source 2 relative to the UAV in the denied zone is

[0099] S62: Calculate the azimuth of each reference source relative to the UAV in the denied area; the azimuth α1 of the reference source UAV 1 relative to the UAV in the denied area in the station center coordinate system with the origin as the origin is defined as the angle from the north direction to the projection direction of the observation vector in the horizontal plane in a clockwise direction, α1 is α1=tan -1 (ΔX E1 / ΔY N1 ); Similarly, the azimuth angle α2 of the reference source UAV 2 relative to the denied zone UAV in the station center coordinate system with the origin as α2 = tan -1 (ΔX E2 / ΔY N2 );

[0100] S63: Calculate the weight coefficient matrix of each reference source relative to the UAV in the denied area; the geometric matrix G of each reference source relative to the UAV in the denied area is:

[0101]

[0102] The weight coefficient matrix H is:

[0103]

[0104] S63: Calculate horizontal position dilution of precision If HDOP>T hdop , Thdop is the threshold, then steps S64 and S65 are performed; otherwise, the maneuvering position of the reference source remains unchanged;

[0105] S64: Calculate a reference source motion vector according to the azimuth angle and the observation vector;

[0106] The relative angle difference of the reference source is calculated as Δα = |α1-α2|:

[0107] c) If Δα < 90° and the reference source is located in the maneuvering area, the motion vector of the reference source 1 in the station center system with the UAV in the denied area as the origin is given as:

[0108] V1=[cosα1 -sinα1] T

[0109] The motion vector of reference source 2 is V2 = [-cosα2 sinα2] T ;

[0110] d) If Δα>90° and the reference source is located in the maneuvering area, the motion vector of the reference source 1 in the station center system with the UAV in the denied area as the origin is given as:

[0111] V1=[-cosα1 -sinα1] T

[0112] The motion vector of reference source 2 is V2 = [cosα2 - sinα2] T

[0113] S65: The reference source adjusts the maneuvering position of the reference source according to the motion vector, and controls the maneuvering position of the reference source within the flight area.

[0114] Beneficial effects of the present invention:

[0115] (1) Collaborative navigation and positioning method based on moving reference source in denied environment. UAVs in non-denied areas provide location information and ranging information to UAVs in denied areas through radio links. There is no need to deploy ground reference anchor nodes in advance, which saves deployment cost and time. At the same time, UAVs in denied areas use a collaborative navigation and positioning method that combines inertial navigation / radio ranging / barometric altimeter, which suppresses the accumulation and divergence of inertial navigation errors and improves the accuracy of absolute geographic location.

[0116] (2) Dynamic adaptive adjustment of spatial geometric distribution precision factor. The reference source drone calculates the precision factor in the drone station center coordinate system in the denied area. When the horizontal position precision factor exceeds the set threshold, the motion vector is calculated based on the observation vector and azimuth, and the maneuvering position is dynamically adjusted to obtain a better spatial geometric distribution and reduce the precision factor. Without adding more reference source drones, the navigation and positioning accuracy of the drone can be significantly improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0117] Figure 1 The overall architecture diagram of the collaborative navigation and positioning system based on the moving reference source in a denied environment;

[0118] Figure 2 A flow chart of a collaborative navigation and positioning method with dynamic adaptive adjustment of precision factor;

[0119] Figure 3 It is a principle block diagram of the collaborative navigation and positioning method based on a moving reference source;

[0120] Figure 4 This is a flow chart of the method for dynamic adaptive adjustment of precision factor;

[0121] Figure 5 Schematic diagram of the collaborative navigation method with dynamic and adaptive adjustment of precision factor. DETAILED DESCRIPTION

[0122] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0123] (1) Figure 1 As shown in the figure, the overall architecture diagram of the collaborative navigation and positioning system based on the moving reference source in a denied environment includes the reference source UAV in the non-denied area, the UAV to be positioned in the denied area, and the radio link between the UAVs.

[0124] As a reference source for collaborative positioning of UAVs in the denied zone, the non-denied zone UAV can maneuver within the set area, achieve high-precision satellite navigation positioning, possess high-precision absolute geographic location information, and broadcast its absolute geographic location to other UAVs via radio links.

[0125] The UAV to be located in the denied zone maneuvers in several specific areas, receives the location information broadcast by the reference source UAV through the radio link, and obtains the distance between the aircraft by measuring the information transmission time. It combines the barometric altitude measured by its own barometric altimeter and the navigation parameter information such as speed, position and attitude calculated by the inertial navigation to achieve the tight combination of inertial navigation / radio ranging / barometric altimeter navigation information fusion, thereby improving the absolute navigation positioning accuracy of the inertial navigation.

[0126] The UAV in the denied area broadcasts its own position information through radio links, and the reference source UAV in the non-denied area obtains the position information of other reference sources and UAVs in the denied area through radio links, and dynamically adjusts its own maneuvering position, so that the UAV in the denied area can obtain a better spatial geometric distribution, reduce the geometric distribution precision factor, and improve the combined navigation and positioning accuracy of UAVs in the denied area.

[0127] A collaborative navigation and positioning system design method based on a moving reference source in a denied environment adopts UAVs in non-denied areas as moving reference sources, and provides absolute geographic location information and ranging information to UAVs in denied areas through radio links. There is no need to deploy ground reference anchor nodes in advance, which saves deployment cost and time, and increases the flexibility of system deployment.

[0128] (2) The implementation process of the present invention is shown in Figure 2 The collaborative navigation positioning method with dynamic adaptive adjustment of precision factor includes the following steps:

[0129] S1: The reference source drone in the non-denied zone can receive satellite navigation signals to achieve high-precision satellite navigation, and control its own maneuvering flight in a specific location within a specific area based on the location information and movement speed and direction;

[0130] S2: The reference source drone in the non-denied area broadcasts its own position information to other reference source drones and the denied area drones through a radio link;

[0131] S3: The UAV to be located in the denied area obtains the location information of each reference source UAV through the radio communication terminal, and calculates the distance information relative to the reference source according to the arrival time;

[0132] S4: The unmanned aerial vehicle to be positioned in the denied area uses the combined navigation information fusion processing of inertial navigation / radio ranging / barometric altimeter to correct the inertial navigation error and obtain more accurate absolute navigation positioning information, such as Figure 3 The principle block diagram of the collaborative navigation positioning method based on the moving reference source is shown in the figure. The specific implementation steps are as follows:

[0133] S41: Establish the state equation of the inertial navigation / radio ranging / barometric altimeter integrated navigation system. The system state variables include the inertial navigation part and the radio ranging clock error part, where the inertial navigation part state variable X I It is composed of conventional inertial navigation parameter error and inertial navigation sensor error:

[0134]

[0135] Among them, φ N ,φ E ,φ D Denotes the attitude error components in the north, east and ground directions, δV N, δV E , δV D Indicates the velocity error components in the north, east and ground directions; Indicates latitude error, longitude error and altitude error; δb gx ,δb gy ,δb gz represents the gyro drift, δb ax ,δb ay ,δb az Represents the accelerometer drift. Simplifying the random error output of the accelerometer and gyroscope into Gaussian white noise, the accelerometer drift model is:

[0136] δb a =δb a0 +η b (1)

[0137] Among them, δb a0 is the accelerometer bias, η b is the white noise deviation of the accelerometer random error.

[0138] The gyroscope drift model is:

[0139] b g =δb g0 +η g (2)

[0140] Among them, δb g0 is the gyroscope bias, η g is the gyroscope random error white noise deviation.

[0141] The state equation of the inertial navigation system is:

[0142]

[0143] in:

[0144]

[0145] Among them, (F n ) 9×9 is the conventional error matrix of inertial navigation, F m =0 6×6 , state transfer matrix F S for:

[0146]

[0147] in is the attitude change matrix in the inertial navigation solution, and the noise driving matrix G I for:

[0148]

[0149] The random error of the inertial navigation sensor is the amount of system noise:

[0150] W(t) I =[w gx ,w gy ,w gz ,w ax ,w ay ,w az ] T

[0151] Among them, w gx 、w gy 、w gz The white noise representing the drift of the gyroscope points to the three coordinate axes in the carrier system; w ax 、w ay 、w az The components of white noise representing acceleration drift pointing in the directions of the three coordinate axes in the carrier system.

[0152] The radio link clock error constitutes the 2D state X of the system R , which is equivalent to the clock error, the distance error δt u and the distance rate error δt equivalent to the clock frequency error ru The error differential equation is:

[0153]

[0154] where w tu and w tru are the driving white noise of the distance error equivalent to the clock error and the distance rate error equivalent to the clock frequency error, β tru is the inverse of the correlation time of the first-order Markov process. The radio link ranging state equation is obtained as:

[0155]

[0156] in:

[0157]

[0158] The final state equation is:

[0159]

[0160] In t k At this moment, the continuous state equation of formula (6) is discretized to obtain:

[0161]

[0162] Among them, the state transfer matrix Ts W is the inertial navigation update time. k-1 is the system noise sequence; at the same time, W k-1 Satisfaction: E[W k-1 ]=0, Γ k-1 is the system noise driving matrix, and the system noise variance matrix Q k-1 is non-negative definite.

[0163] S42: The selected measurement includes the difference between the relative distance and the radio link measurement distance calculated by the UAV to be positioned in the denial zone based on its own inertial navigation solution and the reference source UAV position information, the rate of change of the distance difference, and the difference between the barometer height measurement and the inertial navigation height. Each measurement equation is established according to the measurement components;

[0164] S43: Establishing a measurement equation consisting of the difference between the relative distance calculated by the inertial navigation and the distance measured by the radio link in the integrated navigation measurement equation. The specific implementation steps are as follows:

[0165] The position information S obtained by inertial navigation at time k I (k) and the location information S of the reference source drone j J (k) relative distance p Ij (k) is:

[0166]

[0167] Among them, the coordinates solved by the inertial navigation in the earth-centered earth-fixed rectangular coordinate system are S I (k) = [x I ,y I , z I ] T , the coordinates in the Earth-fixed rectangular coordinate system of the reference source are S J (k) = [x sj ,y sj , z sj ] T The distance expression of inertial navigation is:

[0168] p Ij =r j -e jx δx-e jy δy-e jz δz (9)

[0169] Among them, δx, δy, and δz are the deviations between the inertial navigation solution position and the actual position in the Earth-centered Earth-fixed rectangular coordinate system, e jx 、e jy 、e jz is the component of the unit observation vector of the reference source UAV relative to the UAV to be positioned in the Earth-centered Earth-fixed rectangular coordinate system, rj is the actual relative distance from the reference source to the UAV to be located.

[0170]

[0171] The distance expression relative to the reference source drone obtained by the radio link is:

[0172] p Rj =r j +δt u +v pj (10)

[0173] Among them, v pj To measure noise.

[0174] Subtracting the distance expression of the inertial navigation from the distance expression measured by the radio link can give the distance difference δp j for:

[0175] δp j =-e jx δx-e jy δy-e jz δz-δt u -v pj (11)

[0176] The measurement equation of the distance difference is:

[0177] Z pk =H pk X k +V p (12)

[0178] in:

[0179]

[0180] Where L is latitude, λ is longitude, h is height, R N is the radius of curvature of the angular circle of the reference ellipsoid, and e is the eccentricity of the ellipsoid.

[0181] S44: Establishing a measurement equation consisting of the distance difference change rate in the combined navigation measurement equation. The specific implementation steps are as follows:

[0182] The position information S obtained by inertial navigation from time k-1 to time k I and the location information S of the reference source UAV j J The relative distance change rate for:

[0183]

[0184] The expression of the distance change rate calculated by inertial navigation is:

[0185]

[0186] in, is the actual relative distance change rate from the reference source to the UAV to be located, is the deviation between the position change rate calculated by the inertial navigation and the actual position change rate in the Earth-centered Earth-fixed rectangular coordinate system.

[0187] The expression of the distance change rate relative to the reference source drone obtained by the radio link is:

[0188]

[0189] Among them, v mj To measure noise.

[0190] The distance difference change rate can be obtained by subtracting the distance change rate expression of the inertial navigation from the distance change rate expression measured by the radio link. for:

[0191]

[0192] The measurement equation of the distance difference change rate is obtained as follows:

[0193]

[0194] in:

[0195]

[0196] S45: Establish a measurement equation consisting of the difference between the barometer altitude and the inertial navigation altitude. The specific implementation steps are as follows:

[0197] The height h measured by inertial navigation I for:

[0198] h I =h+δh (19)

[0199] Among them, h is the true height and δh is the inertial navigation height error.

[0200] The altitude measured by the barometer is:

[0201] h B =h+v h (20)

[0202] where v h To measure the noise, we can subtract the above equations (18) and (19) to obtain:

[0203] δh B =h B -hI =δh+v h (twenty one)

[0204] The measurement equation of the height difference change rate is obtained as follows:

[0205] Z hk =H hk X k +V h (t) (22)

[0206] in,

[0207] S46: According to S43, S44, and S45, the total measurement equation of the integrated navigation system is obtained as follows:

[0208]

[0209] Among them, H k is the measurement matrix, V k is the measurement noise sequence, V k Satisfaction: E[V k ]=0, Measurement noise variance matrix R k Zhengding.

[0210] S47: According to the state equation and measurement equation, the EKF filtering method is used to perform inertial navigation / radio ranging / barometric altimeter information fusion. The specific steps are as follows:

[0211] According to formula (7), the error state equation of the inertial navigation at time k is predicted, and we get

[0212] For the estimated bias covariance matrix P k Make predictions,

[0213] According to the inertial navigation solution at time k, the position S I (k) and the position information of the reference source UAV j are combined with equations (8) and (14) to calculate the relative distance and distance change rate obtained by the inertial navigation. Based on the distance and distance change rate relative to the reference source UAV obtained by the radio link at time k, as well as the barometer height value, the distance difference, distance difference change rate and height difference are calculated according to equations (9) (10) (11), (15) (16) (17) and (19) (20) (21). According to equations (12) (18) (22), H in the measurement equation of equation (23) is calculated. k Measurement matrix.

[0214] Perform measurement updates and calculate gain matrix Get the inertial guidance error state update Update the estimated error covariance matrix Subtract the state quantity after filtering and updating from the position and velocity of the inertial navigation That is, the error correction is completed. After filtering and correcting the inertial navigation, it is necessary to Set to zero, that is,

[0215] An information fusion method for the inertial navigation / radio ranging / barometric altimeter combined navigation system based on a moving reference source in a denied environment is proposed. The state equations of the inertial navigation error and the radio link clock error and clock drift are established. The difference between the relative distance calculated by the inertial navigation and the distance measured by the radio link, the rate of change of the distance difference, and the difference between the barometer height measured and the inertial navigation height are used as measurement information to correct the inertial navigation error and improve the absolute and relative navigation positioning accuracy in the denied area.

[0216] S5: The UAV located in the denied area broadcasts its own position information obtained by the inertial navigation in step S4 through a wireless link, and the reference source UAV receives the position information of the UAV in the denied area and the position information of other reference sources.

[0217] S6: Each reference source calculates the geometric distribution precision factor of all reference sources in the station center coordinate system of the unmanned aerial vehicle to be positioned, and adaptively adjusts its own maneuvering position. Figure 4 The principle block diagram of the precision factor dynamic adaptive adjustment method is shown in the figure. The specific steps are as follows:

[0218] S61: Calculate the observation vector of each reference source relative to the UAV in the denied area. Here, two reference sources are taken as an example. The positions of reference source UAV 1 and reference source UAV 2 in the Earth-centered Earth-fixed coordinate system are [X e1 Y e1 Z e1 ] T 、[X e2 Y e2 Z e2 ] T , the position of the drone in the denied zone is [X e0 Y e0 Z e0 ] T , the observation vector of the reference source UAV 1 relative to the denied zone UAV in the earth-centered earth-fixed rectangular coordinate system [ΔX e1 ΔY e1 ΔZ e1 ] T for:

[0219]

[0220] Similarly, the observation vector [ΔX e2 ΔYe2 ΔZ e2 ] T The coordinates of the reference source UAV 1 in the station center coordinate system with the UAV in the denied area as the origin [ΔX E1 ΔY N1 ΔZ U1 ] T for:

[0221]

[0222] Among them, the coordinate transformation matrix for:

[0223]

[0224] Similarly, the coordinates of the reference source drone 2 in the station center coordinate system with the drone in the denied area as the origin [ΔX E2 ΔY N2 ΔZ U2 ] T Since the height is already given by the barometer observation, we only need to consider the impact of the horizontal precision factor on the positioning accuracy of the UAV in the denied area. The observation vectors of reference source 1 relative to the UAV in the denied area are The observation vector of reference source 2 relative to the UAV in the denied zone is

[0225] S62: Calculate the azimuth of each reference source relative to the UAV in the denied area. The azimuth α1 of the reference source UAV 1 relative to the UAV in the denied area in the station center coordinate system with the origin as the origin is defined as the angle from the north direction to the projection direction of the observation vector in the horizontal plane in a clockwise direction, α1 is α1 = tan -1 (ΔX E1 / ΔY N1 ). Similarly, the azimuth angle α2 of the reference source UAV 2 relative to the denied zone UAV in the station center coordinate system with the origin as α2 = tan -1 (ΔX E2 / ΔY N2 ).

[0226] S63: Calculate the weight coefficient matrix of each reference source relative to the UAV in the denied area. The geometric matrix G of each reference source relative to the UAV in the denied area is:

[0227]

[0228] The weight coefficient matrix H is:

[0229]

[0230] S63: Calculate the horizontal position precision factor of each reference source relative to the UAV in the denied area, and determine whether it is greater than the set threshold. Calculate the horizontal position precision factor According to engineering experience, the horizontal position precision factor threshold is set to T hdop =4, if HDOP>T hdop , then proceed to steps S64 and S65; otherwise, the reference source maneuvering position remains unchanged.

[0231] S64: Calculate the reference source motion vector according to the azimuth angle and the observation vector. For n reference source drones, assuming that the relative azimuth angle differences between adjacent reference sources are equal, when the azimuth angle difference between adjacent observation vectors is Therefore, for a system with two reference sources, when the angle between the reference sources is 90°, the horizontal position precision factor is the smallest. Figure 5 It can be seen that the relative angle difference of the reference source is calculated as Δα=|α1-α2|:

[0232] e) If Δα<90° and the reference source is located in the maneuvering area, the motion vector of the reference source 1 in the station center system with the UAV in the denied area as the origin is given as:

[0233] V1=[cosα1 -sinα1] T (29)

[0234] The motion vector of reference source 2 is V2 = [-cosα2 sinα2] T .

[0235] f) If Δα>90° and the reference source is located in the maneuvering area, the motion vector of the reference source 1 in the station center system with the UAV in the denied area as the origin is given as:

[0236] V1=[-cosα1 -sinα1] T (30)

[0237] The motion vector of reference source 2 is V2 = [cosα2 - sinα2] T .

[0238] S65: The reference source adjusts the maneuvering position of the reference source according to the motion vector, and controls the maneuvering position of the reference source within the flight area.

[0239] A collaborative navigation method with dynamic and adaptive adjustment of the precision factor of spatial geometric distribution is proposed. The reference source UAV calculates the precision factor in the UAV station center coordinate system in the denied area. When the horizontal position precision factor exceeds the set domain value, the motion vector is calculated based on the observation vector and azimuth, and its own maneuvering position is dynamically adjusted to obtain a better spatial geometric distribution and reduce the precision factor. Without adding reference source UAVs, the spatial geometric distribution of the reference source is optimized to improve the navigation and positioning accuracy of the UAV to be located.

[0240] The above is only a specific embodiment of the present invention, and the present invention is described in detail. The unexplained part is a conventional technology. However, the protection scope of the present invention is not limited to this. Any changes or substitutions that can be easily thought of by any technician familiar with the technical field within the technical scope disclosed by the present invention should be included in the protection scope of the present invention. The protection scope of the present invention shall be based on the protection scope of the claims.

Claims

1. A collaborative navigation and positioning method with dynamic adaptive adjustment of precision factor, characterized by: Here are the steps: S1: The reference source drone in the non-denied zone receives satellite navigation signals to achieve high-precision positioning, and controls its own maneuvering flight in a specific area based on its own positioning information and movement speed and direction; S2: The reference source drone in the non-denied area broadcasts its own position information to other reference source drones and the denied area drones through a radio link; S3: The UAV in the denied area obtains the position information of each reference source UAV, and calculates the distance information relative to the reference source based on the arrival time; S4: The UAV in the denied area corrects the inertial navigation error according to the distance information relative to the reference source to obtain the corrected absolute navigation positioning information; S5: The UAV in the denied zone broadcasts the corrected absolute navigation positioning information through the wireless link; S6: The reference source UAV in the non-denied area calculates the geometric distribution precision factor in the station center coordinate system of the UAV in the denied area based on the corrected absolute navigation positioning information of the UAV in the denied area, and adjusts its own movement speed and direction based on the geometric distribution precision factor.

2. The method according to claim 1, characterized in that: In S4, the specific correction process is as follows: S41: Establish the state equation of the inertial navigation / radio ranging / barometric altimeter integrated navigation system; S42: Establishing measurement equations of integrated navigation system; S43: According to the state equation and the measurement equation, an extended Kalman filter (EKF) filtering method is used to perform information fusion to obtain an error state update of the inertial navigation; S44: Correct the accumulated error of the inertial navigation using the error state update amount of the inertial navigation to obtain more accurate absolute navigation positioning information after correction.

3. The method according to claim 2, characterized in that: In S41, the process of establishing the state equation of the integrated navigation system is as follows: Setting state variables, including inertial navigation state variables and radio ranging clock error state variables; State variable X of the inertial navigation part I It is composed of conventional inertial navigation parameter error and inertial navigation sensor error: Among them, φ N ,φ E ,φ D Denotes the attitude error components in the north, east and ground directions, δV N , δV E , δV D Indicates the velocity error components in the north, east and ground directions; δλ, δh represent latitude error, longitude error and altitude error; δb gx ,δb gy ,δb gz represents the gyro drift, δb ax ,δb ay ,δb az Indicates accelerometer drift; Radio ranging clock error state variable X(t) R Includes: distance error δt equivalent to clock error u and the distance rate error δt equivalent to the clock frequency error ru ; The state equation of the inertial navigation system is established as: F I (t) is the state transfer matrix; W(t) I is the system noise; G I is the noise driving matrix; Establish the radio ranging clock error differential equation: where w tu and w tru are the driving white noise of the distance error equivalent to the clock error and the driving white noise of the distance rate error equivalent to the clock frequency error, β tru is the inverse of the correlation time of the first-order Markov process; According to the radio ranging clock error differential equation, the radio ranging clock error state equation is obtained: The state equation of the integrated navigation system is as follows: The state equation of the integrated navigation system is discretized to obtain the state equation of the integrated navigation system in discrete state: Among them, the state transfer matrix T s is the inertial navigation update time, W k-1 is the system noise sequence; at the same time, W k-1 Satisfaction: E[W k-1 ]=0, Γ k-1 is the system noise driving matrix, and the system noise variance matrix Q k-1 is a non-negative positive definite matrix.

4. The method according to claim 3, characterized in that: The measurement equation of the integrated navigation system includes: a measurement equation composed of the difference between the relative distance calculated by the inertial navigation and the distance measured by the radio link (i.e., the distance difference), The measurement equation is composed of the difference between the rate of change of the distance relative to the reference source calculated based on the inertial navigation position and the rate of change of the distance relative to the reference source measured through the radio link (i.e., the rate of change of the distance difference), The measurement equation is composed of the difference between the barometer altitude and the inertial navigation altitude.

5. The method according to claim 4, characterized in that: The process of establishing the measurement equation consisting of the difference between the relative distance calculated by the inertial navigation and the distance measured by the radio link is as follows: The position information S obtained by inertial navigation at time k I (k) and the location information S of the reference source drone j J (k) relative distance p ij (k) is: The coordinates in the inertial navigation earth-centered earth-fixed rectangular coordinate system (with the earth's center of mass as the origin, the X axis pointing to the intersection of the earth's equatorial plane and the Greenwich meridian plane, the Z axis pointing to the North Pole of the earth, and the Y axis determined by the right-hand coordinate system) are S I (k) = [x I ,y I , z I ] T , the coordinates in the Earth-fixed rectangular coordinate system of the reference source are S J (k) = [x sj ,y sj , z sj ] T ; The relative distance expression of the UAV relative to the reference source calculated by the inertial navigation is: p 1j =η-e jx δx-e jy dy-e jz δz Among them, δx, δy, and δz are the deviations between the inertial navigation solution position and the actual position in the Earth-centered Earth-fixed rectangular coordinate system, e jx 、e jy 、e jz is the component of the unit observation vector of the reference source UAV relative to the UAV to be positioned in the Earth-centered Earth-fixed rectangular coordinate system, r j is the real relative distance from the reference source to the UAV to be located; The distance expression relative to the reference source drone obtained by radio link measurement is: p Rj =r j +δt u +v pj Among them, v pj To measure noise; Subtracting the relative distance expression calculated by the inertial navigation from the distance expression measured by the radio link can give the distance difference δp j for: δp j =-e jx δx-e jy δy-e jz δz-δt u -v pj The measurement equation of the distance difference is: Z pk =H pk X k +V p in: Where L is latitude, λ is longitude, h is height, R N is the radius of curvature of the angular circle of the reference ellipsoid, and e is the eccentricity of the ellipsoid.

6. The method according to claim 5, characterized in that: The process of establishing the measurement equation consisting of the difference between the rate of change of the distance relative to the reference source calculated based on the inertial navigation position and the rate of change of the distance relative to the reference source measured through the radio link (i.e., the rate of change of the distance difference) is as follows: The position information S obtained by inertial navigation from time k-1 to time k I and the location information s of the reference source UAV j J The relative distance change rate for: The expression of the distance change rate calculated by inertial navigation is: in, is the actual relative distance change rate from the reference source to the UAV to be located, is the deviation between the position change rate obtained by inertial navigation solution and the actual position change rate in the Earth-centered Earth-fixed rectangular coordinate system; The expression of the distance change rate relative to the reference source drone obtained by the radio link is: Among them, v mj To measure noise; Subtract the distance change rate expression of the inertial navigation from the distance change rate expression measured by the radio link to obtain the distance difference change rate for: The measurement equation of the distance difference change rate is obtained as follows: Z p·k =H pk X k +V p in:

7. The method according to claim 6, characterized in that: The process of establishing the measurement equation consisting of the difference between the barometer altitude and the inertial navigation altitude is as follows: The height h measured by inertial navigation I for: h I =h+δh Among them, h is the true height, δh is the inertial navigation height error; The altitude measured by the barometer is: h B =h+v k where v h To measure the noise, we can subtract the above equations (18) and (19) to obtain: δh B =h B -h I =δh+v h The measurement equation of the height difference change rate is obtained as follows: Z hk =H hk X k +V h (t) in, 8. The method according to claim 7, characterized in that: In S43, the information fusion process is as follows: According to the state equation of the integrated navigation system in the discrete state, the error state equation of the inertial navigation at time k is predicted and obtained. For the estimated bias covariance matrix P k Make predictions, According to the inertial navigation solution at time k, the position S 1 (k) and the position information of the reference source UAV i, calculate the relative distance and distance change rate obtained by the inertial navigation; calculate the distance difference, distance difference change rate and height difference based on the distance and distance change rate relative to the reference source UAV obtained by the radio link at time k, and the barometer height value; calculate the H in the measurement equation k Measurement matrix; Perform measurement updates and calculate gain matrix Get the inertial guidance error state update Update the estimated error covariance matrix Subtract the state quantity after filtering and updating from the position and velocity of the inertial navigation The error correction has been completed.

9. The method according to claim 8, characterized in that: In S6, the specific steps are as follows: S61: Calculate the observation vector of each reference source relative to the UAV in the denied area. Take two reference sources as an example. The positions of reference source UAV 1 and reference source UAV 2 in the Earth-centered Earth-fixed rectangular coordinate system are [X e1 Y e1 Z e1 ] T 、[X e2 Y e2 Z e2 ] T , the position of the drone in the denied zone is [X e0 Y e0 Z e0 ] T , the observation vector of the reference source UAV 1 relative to the denied zone UAV in the earth-centered earth-fixed rectangular coordinate system [ΔX e1 ΔY e1 ΔZ e1 ] T for: Similarly, the observation vector [ΔX e2 ΔY e2 ΔZ e2 ] T , the coordinates of the reference source drone 1 in the station center coordinate system with the drone in the denied area as the origin (with the drone's center of mass as the coordinate origin, the X axis pointing to the east, the Y axis pointing to the north, and the Z axis pointing to the sky) [ΔX E1 ΔY N1 ΔZ U1 ] T for: Among them, the coordinate transformation matrix for: Similarly, the coordinates of the reference source drone 2 in the station center coordinate system with the drone in the denied area as the origin [ΔX E2 ΔY N2 ΔZ U2 ] T , since the height is already given by the barometer observation, we only need to consider the impact of the horizontal precision factor on the positioning accuracy of the UAV in the denied area. The observation vectors of reference source 1 relative to the UAV in the denied area are The observation vector of reference source 2 relative to the UAV in the denied zone is S62: Calculate the azimuth of each reference source relative to the UAV in the denied area; the azimuth α1 of the reference source UAV 1 relative to the UAV in the denied area in the station center coordinate system with the origin as the origin is defined as the angle from the north direction to the projection direction of the observation vector in the horizontal plane in a clockwise direction, α1 is α1=tan -1 (ΔX E1 / ΔY N1 ); Similarly, the azimuth angle α2 of the reference source UAV 2 relative to the denied zone UAV in the station center coordinate system with the origin as α2 = tan -1 (ΔX E2 / ΔY N2 ); S63: Calculate the weight coefficient matrix of each reference source relative to the UAV in the denied area; the geometric matrix G of each reference source relative to the UAV in the denied area is: The weight coefficient matrix H is: S63: Calculate horizontal position dilution of precision If HDOP>T hdop , T hdop is the threshold, then steps S64 and S65 are performed; otherwise, the maneuvering position of the reference source remains unchanged; S64: Calculate a reference source motion vector according to the azimuth angle and the observation vector; The relative angle difference of the reference source is calculated as Δα = |α1-α2|: a) If Δα < 90° and the reference source is located in the maneuvering area, the motion vector of the reference source 1 in the station center system with the UAV in the denied area as the origin is given as: V1=[cosα1 -sinα1] T The motion vector of reference source 2 is V2 = [-cosα2 sinα2] T ; b) If Δα>90° and the reference source is located in the maneuvering area, the motion vector of the reference source 1 in the station center system with the UAV in the denied area as the origin is given as: V1=[-cosα1 -sinα1] T The motion vector of reference source 2 is V2 = [cosα2 - sinα2] T S65: The reference source adjusts the maneuvering position of the reference source according to the motion vector, and controls the maneuvering position of the reference source within the flight area.

Images