Inertial-based multi-UAV collaborative positioning method in satellite-denied environment

Abstract

The present invention is an inertial-based multi-UAV collaborative positioning method in a satellite-denied environment. It includes the following steps: (1) The slave node periodically reads the internal and external heterogeneous sensor information of the collaborative network at time k; (2) The information of the airborne inertial gyroscope and accelerometer is used to predict the slave attitude quaternion, speed and position information at the next moment; (3) A factor graph dynamic graph optimization model, heterogeneous heterogeneous factor nodes and a global optimization cost function are constructed; (4) A global optimization cost function of the slave state facing the sliding window is constructed; (5) Based on the constructed dynamic graph optimization model and the global optimization cost function, the Gauss-Newton nonlinear least squares method is used to iteratively optimize and estimate the slave node state in the sliding window to obtain the global optimal navigation solution of the slave state. The present invention can effectively improve the navigation and positioning performance of clustered UAVs in a satellite-denied environment.

Description

Technical Field

[0001] The present invention belongs to the field of collaborative navigation of clustered UAVs, and in particular relates to an inertial-based collaborative positioning method for multiple UAVs in a satellite-denied environment. Background Art

[0002] As an independent and autonomous intelligent entity, UAV (Unmanned Aerial Vehicle) can complete tasks such as power line inspection and cargo transportation in complex environments, and has been widely used. Single UAVs are limited by multiple factors such as load, volume, and mass, and problems such as poor autonomy, insufficient task execution capability, and low damage resistance are gradually exposed. Under the guidance of intelligent collaborative strategies, a drone swarm composed of multiple drones can give full play to the advantages of group resources and quantity, effectively improve the efficiency and success rate of task execution in complex environments, and complete tasks that a single drone cannot complete.

[0003] Under the condition that satellite signals are available, swarm drones currently mainly use inertial / satellite combined navigation systems to provide them with accurate navigation information. This navigation and positioning method shows a strong dependence on satellite navigation systems. Considering that the flight environment of swarm drones is often affected by factors such as electromagnetic interference and electromagnetic suppression, satellite signals may be unavailable due to interference from the external environment. Affected by inertial sensor errors, relying solely on inertial navigation systems, the positioning performance of drones for a long time is difficult to meet actual application requirements. Summary of the invention

[0004] The purpose of the present invention is to provide an inertial-based multi-UAV collaborative positioning method in a satellite-denied environment. By making full use of the UAV's onboard endogenous visual sensor information, radio altimeter information, and group exogenous relative distance perception information as the UAV state endogenous observation constraints, a global optimization graph model and cost function of the UAV state facing the sliding window are constructed; the global optimization estimation of the UAV state in the sliding window is realized by the nonlinear least squares method, and the global optimal navigation solution of the UAV state is obtained; the method proposed by the present invention can realize the high-precision navigation and positioning solution of the cluster UAV in a satellite-denied environment, and meet the precise navigation and positioning requirements of the UAV in a complex interference environment.

[0005] The technical solution to achieve the purpose of the present invention is: an inertial-based multi-UAV collaborative positioning method in a satellite denial environment, comprising the following steps:

[0006] Step (1): The slave node periodically reads the heterogeneous sensor information inside and outside the collaborative network at time k;

[0007] Step (2): using the information from the onboard inertial gyroscope and accelerometer to predict the slave's attitude quaternion, velocity and position information at the next moment;

[0008] Step (3): using the internal and external heterogeneous observation navigation sensor information of the collaborative network obtained by the slave, and according to the probabilistic dependency relationship between the slave state and the internal and external observation sensor information, construct a factor graph dynamic graph optimization model, heterogeneous heterogeneous factor nodes and a global optimization cost function;

[0009] Step (4): construct a global optimization cost function for the slave state facing the sliding window;

[0010] Step (5): Based on the constructed dynamic graph optimization model and the global optimization cost function, the Gauss-Newton nonlinear least squares method is used to iteratively optimize and estimate the slave node states in the sliding window to obtain the global optimal navigation solution of the slave states.

[0011] Furthermore, the information read in step (1) includes: endogenous inertial gyroscope information Accelerometer information Where i = 1, 2, 3, ..., NM is the number of slave nodes in the cluster collaborative network, N is the number of drone nodes in the cluster collaborative network, and M is the number of long-distance nodes with known positions in the collaborative network; the relative position constraint information of the visual base at adjacent moments Δp = [ΔxΔyΔz], where Δx, Δy, Δz are the relative position constraints of the drone at adjacent moments obtained by the visual sensor through the point cloud processing algorithm; the radio altimeter sensor information h i ; and external relative distance perception information d i =[d 1 …d M ], where d 1 ...d M Represents the distance observation information between slave node i and the leader node.

[0012] Furthermore, step (2) specifically includes the following steps:

[0013] Step (21): Slave attitude quaternion prediction equation

[0014] The slave predicts the attitude quaternion using the following formula:

[0015]

[0016] In the formula is the attitude quaternion of the slave at time k; ΔT is the discrete sampling period of the airborne inertial navigation system;

[0017] is the attitude quaternion of the slave at time k-1; the superscript T represents the transpose of the matrix; It is expressed as:

[0018]

[0019] in Calculated by the following formula:

[0020]

[0021] is the component of the angular velocity of the gyro system of the slave node i relative to the navigation system on the three axes of the system; is the component of the gyroscope's zero bias on the three axes of the machine system at time k;

[0022] Step (22): Slave speed prediction equation

[0023] The slave uses the following formula to predict the speed at the next moment:

[0024]

[0025] In the formula in is the component of the acceleration of the slave-to-machine system relative to the navigation system at time k on the X, Y, and Z sides of the slave-to-machine system;

[0026] in is the component of the slave accelerometer zero bias on the X, Y, and Z axes of the machine system at time k; g = [00g] T , g is the acceleration due to gravity; in is the component of the linear velocity of the slave system relative to the navigation system at time k on the X, Y, and Z axes of the navigation system;

[0027] in is the component of the linear velocity of the slave system relative to the navigation system at time k-1 on the X, Y, and Z axes of the navigation system; is the attitude conversion matrix from the machine system of node i to the navigation system, expressed as:

[0028]

[0029] in:

[0030]

[0031]

[0032] Step (23): Slave position prediction equation

[0033] The slave uses the following formula to predict the position at the next moment:

[0034]

[0035] In the formula is the position of slave node k in the navigation coordinate system at the moment; is the position of the slave node k-1 in the navigation coordinate system at time;

[0036] Step (24): Slave gyro bias and accelerometer bias Prediction equation

[0037]

[0038] Furthermore, step (3) specifically includes the following steps:

[0039] Step (31): Inertia pre-integration factor node construction

[0040] The state of the slave node is predicted by the airborne inertial navigation system: the IMU factor node in the factor graph optimization model is constructed using the actual output value and state prediction value of the inertial navigation system, which is expressed as:

[0041]

[0042] In the formula For the IMU factor node in the graph optimization model, is the actual measurement value of the inertial navigation system, h imu (x k-1 ) is the one-step prediction function of the slave state, Q k is the system noise matrix;

[0043] In the adjacent sampling cycles of the data link, the actual measured value of inertial pre-integration is calculated by the following formula:

[0044]

[0045] In the formula Relative constraints on attitude, velocity and position for inertial pre-integration of adjacent sampling periods of data link relative ranging; is the actual measurement value of the gyroscope at time k, R k is the UAV attitude transformation matrix at time k, Measure the noise for the gyroscope and accelerometer separately;

[0046] The difference between the actual measured value of inertia pre-integration and the state predicted value is used to construct the inertia pre-integration factor node in the graph optimization model. The corresponding residual constraint term is shown in the following formula:

[0047]

[0048] In the formula They are the slave inertia pre-integrated attitude, velocity, position and zero bias residual constraint terms respectively; are gyroscope and accelerometer t k The constant zero bias at time, are gyroscope and accelerometer t k+1 Constant zero bias at time;

[0049] Step (32): Construction of visual base relative position constraint factor node

[0050] The relative positions of slaves at adjacent moments obtained by the visual sensor are used as observation constraint information to construct the corresponding visual basis relative position constraint factor nodes and iteratively optimize and estimate the slave states.

[0051] Step (33): Radio altimeter factor node construction

[0052] Using radio altimeter information as the slave state observation constraint, the radio altimeter factor node in the factor graph optimization model is constructed to iteratively optimize the slave state.

[0053] Step (34): Data link relative distance observation factor node construction

[0054] When the slave nodes in the cluster collaborative network are time synchronized with the leader nodes, the slave airborne data link terminal obtains the distance observation information between the slave node and the leader node in real time through TOA. The relative distance observation factor node in the factor graph optimization model is constructed by utilizing the probabilistic dependency relationship between the relative distance observation and the slave state, and the slave node state is iteratively optimized and estimated.

[0055] Further, step (32) is specifically as follows: by using the visual sensor image point cloud processing algorithm, the relative position constraints of the three axes X, Y, and Z of the slave at adjacent moments are obtained as follows:

[0056] Δp visual =[Δx visual ,Δy visual ,Δz visual ] (11)

[0057] Where Δx visual ,Δy visual ,Δz visual is the relative displacement of the slave machine's X, Y, and Z axes at adjacent moments obtained by the visual sensor through the image point cloud processing algorithm; using the relative displacement of the three axes of the visual base slave machine node, the relative distance displacement of the slave machine at adjacent moments is further expressed as:

[0058]

[0059] In the formula is the relative distance displacement of the slave at adjacent moments obtained by the visual sensor; the actual measured value and state prediction value of the relative distance displacement of the slave obtained by the visual sensor are used to construct the visual basis relative position constraint factor node in the factor graph optimization model, which is expressed as:

[0060]

[0061] Where f visual (x k ,x k+1 ) is the relative position constraint factor node of the visual base, R visual h is the visual sensor measurement noise matrix; distance (x k ,x k+1 ) is the relative displacement calculation function of the slave vision sensor at adjacent moments in the sampling period.

[0062] Furthermore, step (33) is specifically as follows:

[0063] Using the actual measured values ​​of the radio altimeter information and the predicted values ​​of the slave state, the radio altimeter factor node in the factor graph optimization model is constructed as follows:

[0064]

[0065] Where f height (x k-1 ) is the radio altimeter factor node in the factor graph optimization model, is the actual measurement value of the radio altimeter, R height Measure the noise matrix for the radio altimeter.

[0066] Furthermore, step (34) is specifically as follows:

[0067] Using the actual measured value of the relative distance of the data link and the predicted value of the slave state, the relative distance observation factor node in the graph optimization model is constructed, which is expressed as:

[0068]

[0069] Where x i ,x j are the states of the slave node and the leader node, respectively, distance (x i ,x j ) is the relative distance observation factor node between the slave node and the leader node in the graph optimization model, is the actual measured value of the relative distance between the slave and the leader node obtained by the data link system terminal, R distance The noise matrix for data link relative range measurement;

[0070] By utilizing the internal and external observation constraints in the cluster collaborative network, a graph optimization model of the inertial-based multi-UAV collaborative positioning method in a satellite denied environment is constructed.

[0071] Furthermore, step (4) is specifically as follows:

[0072] By saving the historical moment slave nodes in real time to obtain the internal and external source observation constraint information, a global optimization cost function of the slave state from the historical moment to the current moment for the sliding window is constructed; the slave state in the sliding window is globally optimized and estimated by the Gauss-Newton nonlinear least squares method to further improve the navigation and positioning performance of the slave nodes in the satellite denial environment; the global optimization cost function of the slave node state in the sliding window is constructed as follows:

[0073]

[0074] In the formula is the maximum a posteriori probability estimate of the slave state, is the inertia pre-integration residual constraint term of slave node i, and L is the sliding window length.

[0075] Furthermore, step (5) is specifically as follows:

[0076] Inertial-based multi-UAV collaborative positioning method in satellite denial environment, the system state quantity is expressed as:

[0077] x i ={p i ,v i ,φ i ,β i ,i=1,2,...NM} (17)

[0078] Where x i is the system state of the cluster collaborative network slave node i, p i =[x i y i z i ] T is the position of slave node i; v i =[v x v y z z ] T is the speed of slave node i; is the attitude of slave node i, including pitch, roll and heading; Zero bias of gyroscope and accelerometer of slave node i;

[0079] The slave state increment is iteratively optimized by Gauss-Newton nonlinear least squares method to obtain the global optimal navigation solution of the slave state.

[0080]

[0081] Where Δx is the slave state increment; A k b is the Jacobian matrix of the internal and external observations of the cluster collaborative network; b k is the residual matrix of endogenous and exogenous observations.

[0082] In response to the demand for high-precision navigation and positioning of cluster UAVs in a satellite-denied environment, the present invention discloses an inertial-based multi-UAV collaborative positioning method in a satellite-denied environment. The present invention adopts a plug-and-play factor graph optimization estimation architecture for heterogeneous and heterogeneous navigation information sources, which can realize the topological collaborative optimization estimation of the state of slave nodes in the cluster collaborative network, and is more suitable for cluster UAV networks with dynamic topology changes driven by tasks; the present invention abandons the traditional Kalman filter estimation architecture, which generally only uses the internal and external source observations of the collaborative network at the current moment to estimate the system state quantity, and fully preserves and utilizes the historical moment observation constraint information, constructs a global optimization cost function for the internal and external source observation constraints of the slave state from the historical moment to the current moment for the sliding window, and iteratively optimizes and estimates the system state quantity, which can obtain a better estimation accuracy and positioning performance of the slave state.

[0083] Compared with the prior art, the present invention has the following beneficial effects:

[0084] 1) The present invention gets rid of the reliance of traditional swarm UAV navigation and positioning methods on satellite navigation systems, and can obtain good positioning accuracy even in satellite-denied environments, laying a foundation for swarm UAVs to better perform tasks in complex environments.

[0085] 2) The method proposed in the present invention gets rid of the defect of the traditional fixed Kalman filter architecture, which requires prior modeling based on the quantity and characteristics of internal and external source observation information in the collaborative network. It can realize plug-and-play topological collaborative optimization estimation of slave status according to the dynamic changes of collaborative network navigation information sources.

[0086] 3) The method proposed in the present invention does not require the assistance of ground station information. The long-distance node in the collaborative network is equivalent to a mobile base station, and the system has good topology reconstruction. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] Figure 1 Schematic diagram of the time series of heterogeneous navigation sensors in a collaborative network.

[0088] Figure 2 Graph model for inertial-based multi-UAV collaborative optimization in satellite-denied environments.

[0089] Figure 3 It is a schematic diagram of the process flow of the multi-UAV collaborative positioning method of the present invention.

[0090] Figure 4 It is an outdoor test verification platform for the method of the present invention.

[0091] Figure 5 This is the simulation analysis result of the present invention.

[0092] Figure 6 This is the outdoor test analysis result of the present invention. DETAILED DESCRIPTION

[0093] The present invention is further described in detail below in conjunction with the accompanying drawings.

[0094] The invention discloses an inertial-based multi-UAV collaborative positioning method in a satellite denial environment, and belongs to the field of cluster UAV collaborative navigation. When the cluster UAV collaborative network is time synchronized, can communicate with each other, and can perceive relative distances, according to the probability dependence between the UAV state and the internal and external source observations of the collaborative network, the relative position constraints of adjacent moments obtained by the onboard internal source visual sensor, the height observation constraints obtained by the radio altimeter, and the external relative distance perception observation constraints are used to construct a global optimization dynamic graph model and cost function of the UAV state, and establish heterogeneous and heterogeneous internal and external source observation factor nodes. The UAV state increment in the cost function is iteratively optimized and estimated by the Gauss-Newton nonlinear least squares method, and the global optimal navigation solution of the UAV state can be obtained. In order to improve the optimization estimation accuracy and positioning performance of the UAV state in the satellite denial environment, the concept of sliding window is adopted in this paper. By saving the internal and external source observation constraint information of the UAV at the historical moment of the collaborative network in real time, a global optimization cost function of the UAV state from the historical moment to the current moment facing the sliding window is constructed. The UAV state in the sliding window is globally iteratively optimized and estimated by the nonlinear least squares method, and the global optimal navigation solution of the UAV state can be obtained. The method disclosed by the invention can effectively improve the navigation and positioning performance of cluster UAVs in a satellite denial environment.

[0095] The present invention is directed to a cluster UAV cooperative network composed of N UAVs, wherein M UAVs are leader nodes, flying in the area where satellite signals are available, equipped with low-precision inertial sensors, single-point mode satellite receivers and data link terminal equipment, and high-precision location information is obtained in real time through the INS / GPS loose combined navigation system. The others are all slave nodes that need to perform collaborative positioning and solving, flying in the area where satellite signals are denied, equipped with low-precision inertial sensors, visual sensors, radio altimeters and data link terminal equipment. During collaborative positioning and solving, the cluster UAV nodes achieve collaborative network time synchronization through the data link precision alignment method. The leader node transmits its own high-precision location information to the slave node in real time through the data link terminal. While the slave node receives the high-precision location information of the leader node, it uses the radio signal arrival time (Time of Arrival, TOA) method to measure the distance observation information between the leader node in real time. The slave nodes utilize the high-precision position information of the leader aircraft, the airborne endogenous inertial sensor information, the visual sensor information, the radio altimeter information, and the exogenous relative distance perception information between UAVs, and design a collaborative positioning algorithm based on factor graph optimization to achieve high-precision collaborative positioning solution for the slave nodes in a satellite denial environment.

[0096] The inertial-based multi-UAV collaborative positioning method in a satellite denial environment includes the following steps:

[0097] Step 1: The slave node periodically reads the information of heterogeneous sensors inside and outside the collaborative network at time k, including the information of the endogenous inertial gyroscope. Accelerometer information Where i = 1, 2, 3, ..., NM is the number of slave nodes in the cluster collaborative network, N is the number of drone nodes in the cluster collaborative network, M is the number of long-distance nodes with known positions in the collaborative network, the relative position constraint information of the visual base at adjacent moments Δp = [Δx Δy Δz], where Δx, Δy, Δz are the relative position constraints of drones at adjacent moments obtained by the visual sensor through the point cloud processing algorithm, and the radio altimeter sensor information h i , and external relative distance perception information d i =[d 1 …d M ], where d 1 ...d M Represents the distance observation information between slave node i and the leader node.

[0098] Step 2: Use the onboard inertial gyroscope and accelerometer information to predict the slave's attitude quaternion, velocity and position information at the next moment;

[0099] 1) Slave attitude quaternion prediction equation

[0100] The slave predicts the attitude quaternion using the following formula:

[0101]

[0102] In the formula is the attitude quaternion of the slave at time k; ΔT is the discrete sampling period of the airborne inertial navigation system; is the attitude quaternion of the slave at time k-1; the superscript T represents the transpose of the matrix; It can be expressed as:

[0103]

[0104] in Calculated by the following formula:

[0105]

[0106] is the component of the angular velocity of the gyro system of the slave node i relative to the navigation system on the three axes of the system; is the component of the gyroscope's zero bias on the three axes of the machine system at time k.

[0107] 2) Slave speed prediction equation

[0108] The slave uses the following formula to predict the speed at the next moment:

[0109]

[0110] In the formula in is the component of the acceleration of the slave-to-machine system relative to the navigation system at time k on the X, Y, and Z sides of the slave-to-machine system; in is the component of the slave accelerometer zero bias on the X, Y, and Z axes of the machine system at time k; g = [00g] T , g is the acceleration due to gravity; in is the component of the linear velocity of the slave system relative to the navigation system at time k on the X, Y, and Z axes of the navigation system; in is the component of the linear velocity of the slave system relative to the navigation system at time k-1 on the X, Y, and Z axes of the navigation system; is the attitude conversion matrix from the machine system of node i to the navigation system, which can be expressed as:

[0111]

[0112] in:

[0113]

[0114] 3) Slave position prediction equation

[0115] The slave uses the following formula to predict the position at the next moment:

[0116]

[0117] In the formula is the position of slave node k in the navigation coordinate system at the moment; is the position of the slave node k-1 in the navigation coordinate system at time;

[0118] 4) Slave gyro bias and accelerometer bias Prediction equation

[0119]

[0120] Step 3: Using the internal and external heterogeneous observation navigation sensor information obtained by the slave in the collaborative network, a factor graph optimization dynamic graph model, heterogeneous heterogeneous factor nodes and a global optimization cost function are constructed according to the probabilistic dependency relationship between the slave state and the internal and external observation sensor information.

[0121] 1) Inertia pre-integration factor node construction

[0122] The state of the slave node is generally predicted by the airborne inertial navigation system in one step. Using the actual output value and state prediction value of the inertial navigation system, the IMU (Inertial Measurement Unit, IMU) factor node in the factor graph optimization model can be constructed, which can be expressed as:

[0123]

[0124] In the formula For the IMU factor node in the graph optimization model, is the actual measurement value of the inertial navigation system, h imu (x k-1 ) is the one-step prediction function of the slave state, Q k is the system noise matrix.

[0125] In actual engineering applications, the sampling frequency of the inertial navigation system is generally higher than that of navigation sensors such as data link relative ranging and radio altimeters. The sampling frequency of inertial sensors is generally 200Hz, while the sampling frequencies of data link relative ranging and radio altimeters are 10Hz respectively. Figure 1 As shown. Figure 1It can be seen that in the adjacent sampling periods of the collaborative network exogenous observation, more IMU factor nodes are added to the graph optimization model for iterative optimization estimation of the UAV state, which increases the computational complexity of the system and reduces the computational efficiency of the system to a certain extent. In order to reduce the computational complexity of the system and improve the iterative optimization efficiency of the system, this paper adopts the inertial pre-integration technology.

[0126] In the adjacent sampling cycles of the data link, the actual measured value of inertial pre-integration can be calculated by the following formula:

[0127]

[0128] In the formula Relative constraints on attitude, velocity and position for inertial pre-integration of adjacent sampling periods of data link relative ranging; is the actual measurement value of the gyroscope at time k, R k is the UAV attitude transformation matrix at time k, The noise is measured separately for the gyroscope and accelerometer.

[0129] The difference between the actual measured value of inertia pre-integration and the state predicted value is used to construct the inertia pre-integration factor node in the graph optimization model. The corresponding residual constraint term is shown in the following formula:

[0130]

[0131] In the formula They are respectively the slave inertia pre-integration attitude, velocity, position and zero bias residual constraints. are gyroscope and accelerometer t k The constant zero bias at time, are gyroscope and accelerometer t k+1 A constant zero bias at time.

[0132] 2) Construction of visual base relative position constraint factor node

[0133] In a satellite signal denial environment, visual sensors, as one of the navigation sensors commonly carried by drones, can obtain high-precision relative position constraint information of slaves at adjacent moments through image point cloud processing algorithms. The relative positions of slaves at adjacent moments obtained by visual sensors are used as observation constraint information to construct corresponding visual basis relative position constraint factor nodes, and the slave states are iteratively optimized and estimated, which can effectively improve the slave state optimization estimation accuracy and positioning performance.

[0134] Through the visual sensor image point cloud processing algorithm, the relative position constraints of the slave's X, Y, and Z axes at adjacent moments can be expressed as:

[0135] Δp visual =[Δx visual ,Δyvisual ,Δz visual ] (11)

[0136] Where Δx visual ,Δy visual ,Δz visual is the relative displacement of the slave machine's X, Y, and Z axes at adjacent moments obtained by the visual sensor through the image point cloud processing algorithm. Using the relative displacement of the three axes of the visual base slave machine node, the relative distance displacement of the slave machine at adjacent moments can be further expressed as:

[0137]

[0138] In the formula is the relative distance displacement of the slave at adjacent moments obtained by the visual sensor. Using the actual measured value and state prediction value of the relative distance displacement of the slave obtained by the visual sensor, the visual base relative position constraint factor node in the factor graph optimization model can be constructed, which can be expressed as:

[0139]

[0140] Where f visual (x k ,x k+1 ) is the relative position constraint factor node of the visual base, R visual h is the visual sensor measurement noise matrix. distance (x k ,x k+1 ) is the relative displacement calculation function of the slave vision sensor at adjacent moments in the sampling period.

[0141] 3) Radio altimeter factor node construction

[0142] The airborne radio altimeter can measure the altitude information of the slave in real time. The radio altimeter information is used as the observation constraint of the slave state. The radio altimeter factor node in the factor graph optimization model is constructed to iteratively optimize the slave state, which can effectively improve the slave state optimization estimation accuracy and positioning accuracy.

[0143] Using the actual measured values ​​of the radio altimeter information and the predicted values ​​of the slave state, the radio altimeter factor node in the factor graph optimization model is constructed as follows:

[0144]

[0145] Where f height (x k-1 ) is the radio altimeter factor node in the factor graph optimization model, is the actual measurement value of the radio altimeter, R height Measure the noise matrix for the radio altimeter.

[0146] 4) Data link relative distance observation factor node construction

[0147] When the slave nodes and the leader nodes of the cluster cooperative network are synchronized, the slave airborne data link terminal can obtain the distance observation information between the slave node and the leader node in real time through TOA. By using the probabilistic dependency between relative distance observation and slave state, the relative distance observation factor node in the factor graph optimization model is constructed, and the slave node state is iteratively optimized and estimated, which can further improve the accuracy of slave state optimization estimation.

[0148] Using the actual measured value of the relative distance of the data link and the predicted value of the slave state, the relative distance observation factor node in the graph optimization model is constructed, which can be expressed as:

[0149]

[0150] Where x i ,x j are the states of the slave node and the leader node, respectively, distance (x i ,x j ) is the relative distance observation factor node between the slave node and the leader node in the graph optimization model, is the actual measured value of the relative distance between the slave and the leader node obtained by the data link system terminal, R distance is the data link relative distance measurement noise matrix.

[0151] By using the internal and external observation constraints in the cluster collaborative network, an optimization model of the inertial-based multi-UAV collaborative positioning method in a satellite denial environment is constructed. See the attachment Figure 2 shown.

[0152] Step 4: Construct a global optimization cost function for the slave state of the sliding window

[0153] In order to improve the accuracy of slave state optimization estimation and positioning performance in a satellite denial environment, the present invention adopts the concept of sliding windows. By saving the internal and external source observation constraint information of the slave nodes at historical moments in real time, a global optimization cost function of the slave state from the historical moment to the current moment of the sliding window is constructed. The global optimization estimation of the slave state in the sliding window can be performed by Gauss-Newton nonlinear least squares method, which can further improve the navigation and positioning performance of the slave node in a satellite denial environment. The global optimization cost function of the slave node state in the sliding window can be constructed as:

[0154]

[0155] In the formula is the maximum a posteriori probability estimate of the slave state, is the inertia pre-integration residual constraint term of slave node i, and L is the sliding window length.

[0156] Step 5: Based on the constructed dynamic graph optimization model and global optimization cost function, the Gauss-Newton nonlinear least squares method is used to iteratively optimize and estimate the slave node states in the sliding window to obtain the global optimal navigation solution for the slave states.

[0157] Inertial-based multi-UAV collaborative positioning method in satellite denial environment, the system state can be expressed as:

[0158] x i ={p i ,v i ,φ i ,β i ,i=1,2,...NM} (17)

[0159] Where x i is the system state of the cluster collaborative network slave node i, p i =[x i y i z i ] T is the position of slave node i; v i =[v x v y z z ] T is the speed of slave node i; is the attitude of slave node i, including pitch, roll and heading; Zero bias of gyroscope and accelerometer of slave node i.

[0160] The global optimal navigation solution of the slave state can be obtained by iteratively optimizing the slave state increment through the Gauss-Newton nonlinear least squares method.

[0161]

[0162] Where Δx is the slave state increment; A k b is the Jacobian matrix of the internal and external observations of the cluster collaborative network; b k is the residual matrix of endogenous and exogenous observations.

[0163] The schematic diagram of the process of the method proposed by the present invention is shown in the attachment Figure 3 shown.

[0164] Step 6: Using the method proposed in this invention, the collaborative positioning performance of the slave aircraft in a satellite denial environment is verified through simulation analysis and outdoor multi-UAV collaborative positioning test. The test platform and verification analysis results are shown in the attachment. Figure 4 , Figure 5 and Figure 6 shown.

[0165] Depend on Figure 5 It can be seen from the simulation analysis results that the position error of the slave aircraft in a satellite denial environment is basically kept within 2m by using the method proposed in the present invention, indicating that the method proposed in the present invention can effectively improve the navigation and positioning performance of the slave aircraft in a satellite denial environment.

[0166] Based on the simulation analysis, the UAV test platform Figure 6 The positioning error curve of the slave in the outdoor environment is given. It can be seen from the positioning error curve that the three-axis positioning error of the slave is basically kept within 2m, which is consistent with the simulation analysis results, further verifying the feasibility and effectiveness of the method proposed in the present invention.

Claims

1. An inertial-based multi-UAV collaborative positioning method in a satellite-denied environment, characterized in that: The steps include: Step (1): The slave node periodically reads the heterogeneous sensor information inside and outside the collaborative network at time k; Step (2): using the information from the onboard inertial gyroscope and accelerometer to predict the slave's attitude quaternion, velocity and position information at the next moment; Step (3): using the internal and external heterogeneous observation navigation sensor information of the collaborative network obtained by the slave, and according to the probabilistic dependency relationship between the slave state and the internal and external observation sensor information, construct a factor graph dynamic graph optimization model, heterogeneous heterogeneous factor nodes and a global optimization cost function; Step (4): construct a global optimization cost function for the slave state facing the sliding window; Step (5): Based on the constructed dynamic graph optimization model and the global optimization cost function, the Gauss-Newton nonlinear least squares method is used to iteratively optimize and estimate the slave node states in the sliding window to obtain the global optimal navigation solution of the slave states.

2. The method according to claim 1, characterized in that The information read in step (1) includes: endogenous inertial gyroscope information Accelerometer information Where i = 1, 2, 3, ..., NM is the number of slave nodes in the cluster collaborative network, N is the number of drone nodes in the cluster collaborative network, and M is the number of long-distance nodes with known positions in the collaborative network; the relative position constraint information of the visual base at adjacent moments Δp = [ΔxΔyΔz], where Δx, Δy, Δz are the relative position constraints of the drone at adjacent moments obtained by the visual sensor through the point cloud processing algorithm; the radio altimeter sensor information h i ; and external relative distance perception information d i =[d1…d M ], where d1...d M Represents the distance observation information between slave node i and the leader node.

3. The method according to claim 2, characterized in that Step (2) specifically includes the following steps: Step (21): Slave attitude quaternion prediction equation The slave predicts the attitude quaternion using the following formula: In the formula is the attitude quaternion of the slave at time k; ΔT is the discrete sampling period of the airborne inertial navigation system; is the attitude quaternion of the slave at time k-1; the superscript T represents the transpose of the matrix; It is expressed as: in Calculated by the following formula: is the component of the angular velocity of the gyro system of the slave node i relative to the navigation system on the three axes of the system; is the component of the gyroscope's zero bias on the three axes of the machine system at time k; Step (22): Slave speed prediction equation The slave uses the following formula to predict the speed at the next moment: In the formula in is the component of the acceleration of the slave-to-machine system relative to the navigation system at time k on the X, Y, and Z sides of the slave-to-machine system; in is the component of the slave accelerometer zero bias on the X, Y, and Z axes of the machine system at time k; g = [00g] T , g is the acceleration due to gravity; in is the component of the linear velocity of the slave system relative to the navigation system at time k on the X, Y, and Z axes of the navigation system; in is the component of the linear velocity of the slave system relative to the navigation system at time k-1 on the X, Y, and Z axes of the navigation system; is the attitude conversion matrix from the machine system of node i to the navigation system, expressed as: in: Step (23): Slave position prediction equation The slave uses the following formula to predict the position at the next moment: In the formula is the position of slave node k in the navigation coordinate system at the moment; is the position of the slave node k-1 in the navigation coordinate system at time; Step (24): Slave gyro bias and accelerometer bias Prediction equation 4. The method according to claim 3, characterized in that Step (3) specifically includes the following steps: Step (31): Inertia pre-integration factor node construction The state of the slave node is predicted by the airborne inertial navigation system: the IMU factor node in the factor graph optimization model is constructed using the actual output value and state prediction value of the inertial navigation system, which is expressed as: In the formula For the IMU factor node in the graph optimization model, is the actual measurement value of the inertial navigation system, h imu (x k-1 ) is the one-step prediction function of the slave state, Q k is the system noise matrix; In the adjacent sampling cycles of the data link, the actual measured value of inertial pre-integration is calculated by the following formula: In the formula Relative constraints on attitude, velocity and position for inertial pre-integration of adjacent sampling periods of data link relative ranging; is the actual measurement value of the gyroscope at time k, R k is the UAV attitude transformation matrix at time k, Measure the noise for the gyroscope and accelerometer separately; The difference between the actual measured value of inertia pre-integration and the state predicted value is used to construct the inertia pre-integration factor node in the graph optimization model. The corresponding residual constraint term is shown in the following formula: In the formula They are the slave inertia pre-integrated attitude, velocity, position and zero bias residual constraint terms respectively; are gyroscope and accelerometer t k The constant zero bias at time, are gyroscope and accelerometer t k+1 Constant zero bias at time; Step (32): Construction of visual base relative position constraint factor node The relative positions of slaves at adjacent moments obtained by the visual sensor are used as observation constraint information to construct the corresponding visual basis relative position constraint factor nodes and iteratively optimize and estimate the slave states. Step (33): Radio altimeter factor node construction Using radio altimeter information as the slave state observation constraint, the radio altimeter factor node in the factor graph optimization model is constructed to iteratively optimize the slave state. Step (34): Data link relative distance observation factor node construction When the slave nodes in the cluster collaborative network are time synchronized with the leader nodes, the slave airborne data link terminal obtains the distance observation information between the slave node and the leader node in real time through TOA. The relative distance observation factor node in the factor graph optimization model is constructed by utilizing the probabilistic dependency relationship between the relative distance observation and the slave state, and the slave node state is iteratively optimized and estimated.

5. The method according to claim 4, characterized in that Step (32) is specifically as follows: by using the visual sensor image point cloud processing algorithm, the relative position constraints of the slave machine's X, Y, and Z axes at adjacent moments are obtained as follows: Δp visual =[Δx visual ,Δy visual ,Δz visual ] (11) Where Δx visual ,Δy visual ,Δz visual is the relative displacement of the slave machine's X, Y, and Z axes at adjacent moments obtained by the visual sensor through the image point cloud processing algorithm; using the relative displacement of the three axes of the visual base slave machine node, the relative distance displacement of the slave machine at adjacent moments is further expressed as: In the formula The relative distance displacement of the slave at adjacent moments obtained by the visual sensor; The actual measured value and state prediction value of the relative distance displacement of the slave obtained by the visual sensor are used to construct the visual base relative position constraint factor node in the factor graph optimization model, which is expressed as: Where f visual (x k ,x k+1 ) is the relative position constraint factor node of the visual base, R visual h is the visual sensor measurement noise matrix; distance (x k ,x k+1 ) is the relative displacement calculation function of the slave vision sensor at adjacent moments in the sampling period.

6. The method according to claim 5, characterized in that Step (33) is specifically: Using the actual measured values ​​of the radio altimeter information and the predicted values ​​of the slave state, the radio altimeter factor node in the factor graph optimization model is constructed as follows: Where f height (x k-1 ) is the radio altimeter factor node in the factor graph optimization model, is the actual measurement value of the radio altimeter, R height Measure the noise matrix for the radio altimeter.

7. The method according to claim 6, characterized in that Step (34) is specifically: Using the actual measured value of the relative distance of the data link and the predicted value of the slave state, the relative distance observation factor node in the graph optimization model is constructed, which is expressed as: Where x i ,x j are the states of the slave node and the leader node, respectively, distance (x i ,x j ) is the relative distance observation factor node between the slave node and the leader node in the graph optimization model, is the actual measured value of the relative distance between the slave and the leader node obtained by the data link system terminal, R distance The noise matrix for data link relative range measurement; By utilizing the internal and external observation constraints in the cluster collaborative network, a graph optimization model of the inertial-based multi-UAV collaborative positioning method in a satellite denied environment is constructed.

8. The method according to claim 7, characterized in that Step (4) is specifically as follows: By saving the historical moment slave nodes in real time to obtain the internal and external source observation constraint information, a global optimization cost function of the slave state from the historical moment to the current moment for the sliding window is constructed; the slave state in the sliding window is globally optimized and estimated by the Gauss-Newton nonlinear least squares method to further improve the navigation and positioning performance of the slave nodes in the satellite denial environment; the global optimization cost function of the slave node state in the sliding window is constructed as follows: In the formula is the maximum a posteriori probability estimate of the slave state, is the inertia pre-integration residual constraint term of slave node i, and L is the sliding window length.

9. The method according to claim 8, characterized in that Step (5) is specifically as follows: Inertial-based multi-UAV collaborative positioning method in satellite denial environment, the system state quantity is expressed as: x i ={p i ,v i ,f i ,b i ,i=1,2,...NM} (17) Where x i is the system state of the cluster collaborative network slave node i, p i =[x i y i z i ] T is the position of slave node i; v i =[v x v y z z ] T is the speed of slave node i; is the attitude of slave node i, including pitch, roll and heading; Zero bias of gyroscope and accelerometer of slave node i; The slave state increment is iteratively optimized by Gauss-Newton nonlinear least squares method to obtain the global optimal navigation solution of the slave state. Where Δx is the slave state increment; A k b is the Jacobian matrix of the internal and external observations of the cluster collaborative network; b k is the residual matrix of endogenous and exogenous observations.

Images