A Cooperative Navigation Method for Cluster UAVs Driven by Objectives

Abstract

The present invention discloses a method for cooperative navigation of cluster unmanned aerial vehicles driven by a target, comprising the following steps: obtaining the position information and ranging information of a leading node, and based on the position information and ranging information, using the maximum vector tetrahedron volume method to perform a spatial geometric configuration on slave nodes; obtaining the GDOP values of the spatial geometric configuration and performing a sorting process, and based on the smallest GDOP value, using the least squares method to iterate the spatial geometric configuration to obtain an optimal spatial geometric configuration; based on the optimal spatial geometric configuration and in combination with Kalman filtering, correcting the positions of the slave nodes to achieve cooperative navigation of the cluster unmanned aerial vehicles. The present invention realizes high-precision cooperative navigation of cluster unmanned aerial vehicles in the whole process driven by a target under the architecture of master-slave cooperative navigation of cluster unmanned aerial vehicles.

Description

Technical Field

[0001] The present invention belongs to the technical field of cooperative navigation, and particularly relates to a cooperative navigation method for a swarm of unmanned aerial vehicles (UAVs) driven by a target. Background Art

[0002] Swarm UAVs have many advantages such as strong combat capabilities, high system survival rate, and low attack cost. With the increasing variety of UAVs, the mission fields and mission types of swarm UAVs have been continuously expanded, and they have gradually been involved in various fields such as airspace confrontation, defense, surveillance, and reconnaissance. High-precision navigation information is the key information for realizing the navigation control of UAV swarms. When UAV swarms face the actual battlefield environment with high antagonism, high uncertainty, and high dynamics, formulating a scientific and clear target-driven strategy is the key in the research of UAV swarm technology under multi-task objectives.

[0003] Current research scholars have carried out a large amount of research work on the control problem of swarm UAVs driven by a single target. For example, in the research on "Multi-UAV Cooperative Attack Strategy", it is proposed that the multi-UAV cooperative attack strategy is studied from two parts: multi-UAV cooperative air combat and multi-UAV cooperative ground attack, and combat models in different environments are established respectively. In the research on "Research on Heterogeneous UAV Formation Defense and Evaluation Strategy", it is proposed that the genetic algorithm is applied to the defense deployment problem of UAV swarms. For enemy aircraft formations of different scales and different formations, the UAV formations on our side are optimized, and the effectiveness of the algorithm is verified by drawing loss curves of various battle situations. In the research on "Research on Patrol UAV Formation Trajectory Tracking Control", a patrol method for patrol UAV formations is proposed. Aiming at the saturation input phenomenon in the UAV formation process, the leader-follower control algorithm is used to control the UAV formation to effectively achieve the purpose of formation control.

[0004] Analysis of the above existing literature shows that current research scholars have carried out a large amount of research work on the control problem of swarm UAVs in a single-task target scenario. However, considering the complex combat environment, no literature comprehensively considers the situation when UAV swarms are driven by multi-task objectives. In addition, the real-time topological changes of the formation configuration during the execution of UAV swarm mission objectives will affect the navigation and positioning errors of UAVs, thereby affecting the success rate of UAV swarm mission execution.

[0005] Therefore, it is urgent to propose a cooperative navigation method for swarm UAVs driven by a target to achieve high-precision cooperative navigation of swarm UAVs throughout the whole process under multi-target drive. Summary of the Invention

[0006] The object of the present invention is to provide a collaborative navigation method for a swarm of unmanned aerial vehicles (UAVs) driven by objectives. In the case of a swarm of UAVs being driven by multiple objectives such as attack mission objectives, avoidance mission objectives, inspection mission objectives, etc., the least squares method is adopted to complete the iteration of the configurations of the swarm of UAVs under different objective drives. At the same time, on the premise of not considering the clock non-synchronization error between UAVs, based on the relative ranging method between UAVs, multiple leading UAVs in the optimal spatial geometric configuration are selected in real time through the minimum geometric dilution of precision (GDOP) criterion to perform collaborative positioning calculation on slave node UAVs. On this basis, an extended Kalman filter is designed to reduce the positioning error during the process of UAVs executing different mission objectives, so as to solve the problems existing in the above-mentioned prior art.

[0007] To achieve the above object, the present invention provides a collaborative navigation method for a swarm of UAVs driven by objectives, including the following steps:

[0008] Obtain the position information and ranging information of the leading UAV node. Based on the position information and ranging information, use the maximum vector tetrahedron volume method to perform a spatial geometric configuration on the slave node UAVs;

[0009] Obtain the GDOP value of the spatial geometric configuration and perform sorting processing. Based on the minimum GDOP value, use the least squares method to iterate the spatial geometric configuration to obtain the optimal spatial geometric configuration;

[0010] Based on the optimal spatial geometric configuration and combined with Kalman filtering, correct the positions of the slave node UAVs to achieve the collaborative navigation of the swarm of UAVs.

[0011] Optionally, the process of obtaining the position information and ranging information of the leading UAV node includes: obtaining the position information of the leading UAV based on the on-board navigation system, and broadcasting the position information to the slave node UAVs through a data link; the slave node UAVs receive the position information and measure the relative distance to the leading UAV node to obtain the ranging information.

[0012] Optionally, the process of using the maximum vector tetrahedron volume method to perform a spatial geometric configuration on the slave node UAVs includes: obtaining the three-dimensional relative distance between the leading UAV node and the slave node UAVs, performing a first-order linearized Taylor expansion on the three-dimensional relative distance, and then obtaining the collaborative positioning error equation set of the slave node UAVs; performing a least squares method processing on the collaborative positioning error equation set to obtain the error covariance equation; combining the relative ranging error between the leading UAV node and the slave node UAVs with the error covariance equation to obtain the spatial geometric configuration between the slave node UAVs and the leading UAV node.

[0013] Optionally, the process of obtaining the optimal spatial geometric configuration includes: when the GDOP value of the spatial geometric configuration is the smallest, obtaining the node coordinates of the leading node and the slave nodes as well as the coordinates of each base station, and then obtaining the distance equations from the node coordinates to each base station; performing iterative difference operations on the distance equations and solving them using the least squares method to obtain the collaborative positioning solution of the leading node to the slave nodes, and then obtaining the optimal spatial geometric configuration.

[0014] Optionally, the process of correcting the position of the slave nodes in combination with Kalman filtering includes: based on the optimal spatial geometric configuration, obtaining a preferred leading node; based on the preferred leading node, constructing a relative ranging error observation model and a UAV cluster state recurrence model for the slave nodes, and then using the extended Kalman filter to perform time update and measurement update on the slave nodes to achieve position correction of the slave nodes.

[0015] Optionally, the process of constructing a relative ranging error observation model for the slave nodes includes: constructing a pseudorange equation between the leading node and the slave nodes and performing first-order linearized Taylor expansion, and then constructing the Jacobian matrix of the UAV cluster based on the relative ranging method, and constructing a relative ranging error observation model for the slave nodes based on the Jacobian matrix.

[0016] Optionally, the process of constructing a UAV cluster state recurrence model includes: obtaining the navigation output parameter errors of the inertial navigation system carried by the UAVs and establishing an 18-dimensional state vector, and then constructing a system state transition matrix and a system noise matrix, and constructing a UAV cluster state recurrence model based on the 18-dimensional state vector, the system state transition matrix and the system noise matrix.

[0017] The technical effects of the present invention are as follows:

[0018] Based on the iterative least squares method, the present invention constructs a target-driven configuration change model for cluster UAVs. On this basis, a collaborative navigation node optimization strategy based on the minimum geometric dilution of precision (GDOP) criterion is established; and based on the optimal spatial configuration under the target-driven strategy, by designing a collaborative navigation filter based on relative distance constraints, the online estimation and compensation of the UAV state under the target-driven change of cluster UAVs are realized, so as to achieve the purpose of improving the positioning accuracy of cluster UAVs. The present invention realizes high-precision collaborative navigation of cluster UAVs in the whole process under target drive in the architecture of master-slave collaborative navigation of cluster UAVs. Description of the Drawings

[0019] The drawings constituting a part of this application are used to provide a further understanding of this application. The schematic embodiments of this application and their descriptions are used to explain this application and do not constitute an improper limitation to this application. In the drawings:

[0020] Figure 1 Schematic diagram of the target configuration of the swarm drones in the embodiment of the present invention;

[0021] Figure 2 Schematic diagram of the target configuration of the swarm drones avoiding the target in the embodiment of the present invention;

[0022] Figure 3 Schematic diagram of the target configuration of the swarm drones for inspection in the embodiment of the present invention;

[0023] Figure 4 Flowchart of the target-driven strategy of the swarm drones in the embodiment of the present invention;

[0024] Figure 5 Schematic diagram of the optimal spatial configuration of slave node 1 in the embodiment of the present invention;

[0025] Figure 6 Flowchart of the design of the cooperative navigation algorithm for the swarm drones based on target driving in the embodiment of the present invention;

[0026] Figure 7 GDOP value of slave node 1 for real-time settlement with cooperative nodes under the target-driven task in the embodiment of the present invention;

[0027] Figure 8 Schematic diagram of the three-dimensional position error curve of slave node 1 in the single configuration and the optimal spatial configuration in the embodiment of the present invention. Detailed implementation manner

[0028] It should be noted that, without conflict, the embodiments in the present application and the features in the embodiments may be combined with each other. The present application will be described in detail below with reference to the drawings and in combination with the embodiments.

[0029] It should be noted that the steps shown in the flowchart of the drawings may be executed in a computer system such as a set of computer-executable instructions, and although the logical order is shown in the flowchart, in some cases, the steps shown or described may be executed in a different order than here.

[0030] Embodiment 1

[0031] Behaviors such as attacking, evading, and patrolling in a drone swarm often show a high degree of consistency with biological swarms. In the division of labor and cooperation behaviors of wolf packs during hunting, the collective escape behaviors of fish schools when in danger, and the information transmission and search behaviors of ants during foraging, each individual in a biological swarm has the ability of autonomous decision-making and information exchange. Through continuous evolution, the swarm finally shows cooperation, stability, and the ability to adapt to complex environments at the macroscopic level, which closely matches the requirements of distributed and collaborative target tracking in a drone swarm. Therefore, this embodiment combines the advantages of different biological community cooperation methods, forms a mapping relationship with the behaviors of swarm drones driven by different targets, and conducts relevant research work.

[0032] Through the research and summary of the predation strategies of wolf packs, the evasion strategies of fish schools, and the foraging strategies of ant colonies, it is concluded that the implementation of biological swarm strategies highly depends on the decisions of leaders and the reasonable formation configurations of biological communities. Based on this research, when this embodiment conducts research on the collaborative navigation of swarm drones driven by targets, the research focus is placed on whether the drones can quickly and accurately generate corresponding target-driven configurations under the master-slave collaborative navigation architecture of swarm drones, and whether high-precision navigation and positioning can be ensured during the configuration generation process of swarm drones.

[0033] The research on the target-driven strategy of swarm drones adopts the traditional master-slave collaborative navigation architecture, where the leader node is equipped with a MEMS (Micro-Electro-Mechanical Systems) inertial measurement unit, a satellite receiver, and a data link communication device, and the slave nodes are equipped with MEMS inertial measurement units and data link communication devices. As Figure 1 shown, the configuration of the attack formation of swarm drones adopts a regular spatial triangle configuration; as Figure 2 shown, the evasion configuration of swarm drones adopts a regular sphere configuration; as Figure 3 shown, the patrol configuration of swarm drones adopts a spatial vector straight line. Among them, Figures 1-3 the arrow direction indicates the flight direction of the swarm drones.

[0034] This embodiment conducts research on the cooperative navigation of cluster UAVs driven by a target from the perspective of improving the positioning accuracy during the configuration iteration process of cluster UAVs. During the execution of target tasks by a UAV formation, the real-time topological changes of the formation configuration will be faced, and configuration optimization is a key technology to fully utilize the advantages of UAV clusters. In the research on the cooperative navigation of cluster UAVs driven by a target, this embodiment adopts the maximum volume of the tetrahedron with the largest deviation method, referring to the concept of the GDOP value in satellite navigation. When the volume of the spatial vector tetrahedron formed by 4 leading UAVs and slave nodes in the UAV cluster is the largest, that is, when the geometric dilution of precision GDOP is the smallest, the optimal geometric configuration of the leading UAVs is obtained, which can effectively reduce the positioning error of the slave nodes. Therefore, based on the minimum GDOP criterion, this embodiment conducts real-time configuration optimization on the leading nodes during the least squares iteration process of the cluster UAV configuration, and at the same time designs a cooperative navigation filter for the UAV cluster to achieve the cooperative navigation of the cluster UAVs.

[0035] As Figure 4 shown, this embodiment provides a cooperative navigation method for cluster UAVs driven by a target. First, obtain the position information and ranging information of the leading nodes. Based on the position information and ranging information, use the maximum vector tetrahedron volume method to perform spatial geometric configuration on the slave nodes; obtain the GDOP values of the spatial geometric configuration and perform sorting processing. Based on the smallest GDOP value, use the least squares method to iterate the spatial geometric configuration to obtain the optimal spatial geometric configuration; based on the optimal spatial geometric configuration and combined with Kalman filtering, correct the positions of the slave nodes to achieve the cooperative navigation of the cluster UAVs.

[0036] Obtain the optimal spatial geometric configuration of cluster UAVs driven by a target:

[0037] Assume that there are N leading nodes in the cooperative network of the UAV cluster, flying within the satellite signal coverage area, and k slave nodes to be solved for cooperative positioning. The leading UAVs solve their own navigation results through the on-board navigation system, and at the same time broadcast their own position information to the slave nodes through the data link. The slave nodes receive the position information of the leading nodes in real time and realize the relative distance measurement with the leading nodes based on the TOA (time of arrival) method. According to the position information and ranging information of the leading UAVs, use the maximum vector tetrahedron volume method to simultaneously realize the real-time optimization of the spatial configuration of the slave nodes. The tetrahedron of the spatial configuration of slave node 1 in the cooperative network of the UAV cluster is as Figure 5 shown.

[0038] The calculated value of the three-dimensional relative distance between the leading and slave nodes can be expressed as:

[0039]

[0040] where, (x si , y si,z si ) is the three - dimensional position of the leading node, and (x, y, z) is the three - dimensional position of the slave node. Performing a first - order linearized Taylor expansion on equation (1), in the case of N leading nodes, the error equations for the collaborative positioning of the slave node are as follows:

[0041] R - P = G u ·X u (2)

[0042] where R=(r1 r2 … r n ) is the spatial geometric distance from the slave node to the leading node; P=(ρ1 ρ2... ρ n ) is the three - dimensional relative distance measurement value from the slave node to the leading node; G u is the direction cosine matrix from the slave node to the leading node, which can be expressed as:

[0043]

[0044] where X u =[δx δy δz] T , and (δx, δy, δz) are the position errors of the inertial navigation system on the slave node aircraft.

[0045] From equation (2), using the least - squares method, we can obtain:

[0046]

[0047] The error covariance equation can be expressed as:

[0048]

[0049] Assume that the relative ranging error between the slave node and each leading node is a white - noise model and they are independent of each other:

[0050] cov(δ(R - P)) = σ 2 I (6)

[0051] Substituting equation (6) into equation (5), we can get:

[0052]

[0053] Let in the above formula

[0054]

[0055] Then the GDOP error coefficient of the spatial geometric configuration between the slave node and the leading node can be expressed as:

[0056]

[0057] To obtain four leading aircraft with the optimal spatial geometric configuration of the slave nodes in real time, it is necessary to calculate the GDOP (Geometric Dilution of Precision) values of all spatial configurations at the current moment in real time. The spatial geometric configuration with the minimum GDOP value at the current moment is used as the preferred leading node result:

[0058] GDOP m = min(GDOP) (10)

[0059] Iterative process of cluster UAV configuration based on target-driven strategy:

[0060] Denote the position coordinates of the UAV node as (x, y, z), and solve the distance d between two UAV nodes in the collaborative network ij (i ∈ (1, N - 1), j ∈ (i + 1, N)):

[0061]

[0062] For the convenience of performing iterative least squares, in this embodiment, the three-dimensional coordinates of the reference target points of the UAV nodes after the iterative completion of the target configuration of the cluster UAVs are given, denoted as At the same time, calculate the distances between each UAV node and other nodes after the iterative target configuration, denoted as

[0063] Bring the calculated values of the distances between two UAV nodes {d 12 ,..., d ij ,..., d N-1N} and into Equation (12) one by one for iterative residual calculation:

[0064]

[0065]

[0066] If the iterative residual satisfies the minimum residual threshold set by Equation (13) it proves that the cluster UAVs have completed the configuration iteration under the corresponding target drive. If the iterative residual does not satisfy the threshold of Equation (13), the three-dimensional coordinate values of the cluster UAVs in the initial configuration are added with the position iteration step size, and the process of Equations (11) - (13) is repeated until the set iterative minimum residual threshold is satisfied That is, the target-driven task is completed, and the iterative flight track of the cluster UAVs under the target drive is recorded for subsequent filtering settlement.

[0067] Design of collaborative navigation filter for cluster UAVs based on target-driven strategy:

[0068] Flowchart of the design of collaborative navigation algorithm for cluster UAVs based on target drive is asFigure 6 As shown in the figure. First, based on the collaborative network clock synchronization, the slave node uses the on-board data link communication system to identify the device IDs of different master nodes in real time, complete relative ranging, and share navigation information. The on-board navigation computer combines its own inertial navigation information and relative ranging information to complete the conversion of the space coordinate system. Secondly, according to the space configuration optimization method proposed in this embodiment, four master nodes that form the optimal space configuration with the slave node at different times are selected in real time. On this basis, an extended Kalman filter is designed to finally realize the online estimation and real-time compensation of the system state variables, so as to achieve the purpose of improving the positioning accuracy of the slave node.

[0069] State equation model:

[0070] The system state variables of the UAV swarm collaborative positioning algorithm based on space configuration optimization are:

[0071]

[0072]

[0073] Among them, is the platform error angle of the on-board inertial navigation system of the slave node; δv E , δv N , δv U are the velocity errors of the on-board inertial navigation system of the slave node; δL, δλ, δh are the position errors of the on-board inertial navigation system of the slave node; ε bx , ε by , ε bz are the random constants of the three-axis gyroscope; ε rx , ε ry , ε rz are the random noises of the first-order Markov process of the three-axis gyroscope; ▽ x , ▽ y , ▽ z are the random noises of the first-order Markov process of the three-axis accelerometer.

[0074] By using the error differential equation of the slave node inertial navigation system, the state recurrence equation of the collaborative positioning algorithm is constructed as:

[0075]

[0076] Among them, F(t) is the system state transition matrix, G(t) is the system noise matrix; W(t) is the white noise of the gyroscope and accelerometer.

[0077]

[0078] F N is the system matrix corresponding to the platform error angle, velocity error, and position error:

[0079]

[0080]

[0081] Among them, is the attitude matrix from the body coordinate system to the navigation coordinate system, T gx , T gy , T gz are the time constants related to the three-axis gyroscope, T ax , T ay , T az are the time constants related to the three-axis accelerometer.

[0082] Observation equation model:

[0083] The calculated value of the three-dimensional relative distance between the slave node and the leader node in the optimal spatial geometric configuration can be expressed as:

[0084]

[0085] Among them, (x s , y s , z s ) is the position solved by the on-board inertial navigation system of the slave node, and (x mi , y mi , z mi ) is the three-dimensional position of the leader node. First-order Taylor expansion of Equation (19) and ignoring the high-order terms gives:

[0086]

[0087] The direction cosines of the calculated value of the relative distance between UAV nodes in the x, y, and z axes can be expressed as:

[0088]

[0089]

[0090]

[0091]

[0092] The measured value of the relative distance between the leader and slave nodes obtained by the slave node through the data link communication system in the optimal spatial configuration can be expressed as:

[0093] ρ Dj = r j - v ρj (25)

[0094] Among them, r j is the true value of the relative distance between the leader and slave nodes, v ρjis the white noise of the relative ranging error of the airborne data link system.

[0095] Taking the difference between Equation (20) and Equation (25) gives:

[0096] δρ i = ρ smi - ρ Dj = e i1 δx + e i2 δy + e i3 δz + v ρj (26)

[0097] Taking i = 1, 2, 3, the relative ranging observation equation under the data link communication system can be expressed as:

[0098]

[0099] In this embodiment, the navigation coordinate system is selected as the east, north, and celestial geographic coordinate system. Therefore, it is necessary to convert the inertial navigation position error in the Earth-Centered Earth Fixed (ECEF) coordinate system to the geographic coordinate system. The mathematical expression of the spatial transformation relationship between the two coordinate systems is:

[0100]

[0101] where δL, δλ, δh represent the latitude, longitude, and altitude position errors of the slave node; L, λ, h are the latitude, longitude, and altitude values of the slave node solved. R N = R e (1 + f(sinL) 2 ), R e = 6378137m, f = 1 / 298.257.

[0102] Substituting Equation (28) into Equation (27) gives the observation equation of the cluster UAV system based on target driving:

[0103] Z ρ = δρ = H ρ X i + V ρ (29)

[0104] where H ρ = [0 3×6 H ρ1 0 3×9 ,

[0105]

[0106]

[0107] Kalman filter design:

[0108] After the state equation and the observation equation are established, the process of time update and measurement update by Kalman filter is as follows:

[0109]

[0110]

[0111]

[0112]

[0113]

[0114] In the above formula, is the estimated state quantity from time t - 1 to time t, and Φ t,t-1 is the state transition matrix from time t - 1 to time t, is the estimated state quantity of the previous step, is the estimated state quantity of this step, and K t is the filtering gain of the system at time t, Z t is the measured quantity at time t, and its value changes in real time according to H in formula (29) ρ is the measurement matrix of the system at time t, and its value changes in real time according to Z in formula (29) t is the measurement matrix of the system at time t, and its value changes in real time according to Z in formula (29) ρ is the mean square error corresponding to t / t-1 For the corresponding mean square error, R t is the measurement noise matrix of the system at time t, P t-1 is the mean square error of the previous step, Γ t,t-1 is the system input matrix from time t - 1 to time t, P t / t For the corresponding mean square error, I is the identity matrix, and the superscript T represents transpose.

[0115] Finally, the simulation analysis part of the cooperative navigation algorithm is carried out according to the established Kalman filter time update and measurement update equations.

[0116] Simulation analysis

[0117] To verify the feasibility and effectiveness of the algorithm, this embodiment verified and analyzed the algorithm through simulation. A total of 5 leading aircraft and 3 following aircraft were set in the simulation. All UAV nodes fitted the flight tracks under different target drives through the iterative least squares method for subsequent filtering and solution procedures. In the following simulation, the traditional single configuration for comparison refers to the spatial configuration composed of 1 following aircraft node to be located and 4 leading aircraft in the cooperative network. It should be noted that once the 4 leading aircraft nodes selected for cooperative solution are chosen, they will not be replaced during the cooperative positioning of the following aircraft nodes. The selection criteria for the 4 leading aircraft nodes only need to ensure that the initial flight track directions are different from each other and different from the following aircraft node.

[0118] The PC (personal computer) used in this embodiment for algorithm verification is equipped with an AMD Ryzen 5 5600G with Radeon Graphics, a 3.90 GHz CPU, and 16 GB of on-board RAM. All algorithm debugging was carried out in MATLAB 2019b.

[0119] During the simulation, the sampling frequency of the airborne inertial navigation system is 50 HZ, and the sampling frequencies of the leading aircraft's airborne satellite receivers and data links are 1 HZ. The performance parameter settings of the airborne sensors in the simulation are shown in Table 1. The entire simulation duration is 345 s.

[0120] Table 1

[0121]

[0122] This embodiment also gives the initial flight parameters of some leading aircraft and following aircraft node 1 in the simulation. The relevant parameter settings are given in Table 2, and the initial parameter settings of the remaining leading aircraft and following aircraft nodes are similar to the relevant parameter setting modes in Table 2.

[0123] Table 2

[0124]

[0125] In Table 2, the UAV node position parameters are set in the order of longitude, latitude, and altitude; the UAV node attitude angle parameters are set in the order of roll angle, pitch angle, and heading angle.

[0126] Table 3 shows the root mean square (RMSE) of the three-dimensional position error of the following aircraft nodes under the single configuration and the optimal spatial geometric configuration:

[0127] Table 3

[0128]

[0129] As can be seen from the data in Table 3, compared with the traditional single configuration method, the proposed method can significantly improve the positioning accuracy of the slave nodes. The position accuracy of the slave nodes is improved by about 28%, thus verifying the effectiveness of the method proposed in this embodiment.

[0130] To verify the optimal strategy for the real-time spatial configuration of the slave nodes proposed in this embodiment, taking the slave node 1 as an example, Figure 7 the GDOP value corresponding to the optimal spatial geometric configuration formed by the slave node 1 and the master in real time is given.

[0131] To more intuitively compare and analyze the performance of the algorithm proposed in this embodiment. Figure 8 The position error curves of the slave node 1 obtained by using the single configuration and the method proposed in this embodiment are given. As Figure 8 can be seen, the method proposed in this embodiment takes into account the spatial configuration of the slave nodes and the cooperative nodes, and the slave nodes can obtain better positioning accuracy than the single configuration. To quantitatively compare and analyze the performance of the algorithm proposed in this embodiment with respect to the traditional single-configuration cooperative positioning algorithm, Table 3 gives the root mean squared error (RMSE) of the three-dimensional position error of the slave node 1 under the two algorithms.

[0132] This example effectively realizes the complex control of the swarm of UAVs when receiving multi-target driving. At the same time, it can accurately estimate the position of the slave nodes by combining the dynamic topology structure of the swarm of UAVs in a complex flight environment.

[0133] The above is only a preferred specific embodiment of the present application, but the protection scope of the present application is not limited thereto. Any changes or substitutions that can be easily thought of by those skilled in the art within the technical scope disclosed in the present application should be covered by the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A collaborative navigation method for cluster drones driven by a target, characterized in that, Including the following steps: Obtain the position information and ranging information of the leading node. Based on the position information and ranging information, use the maximum vector tetrahedron volume method to perform spatial geometric configuration on the slave nodes; Obtain the GDOP value of the spatial geometric configuration and perform sorting processing. Based on the minimum GDOP value, use the least squares method to iterate the spatial geometric configuration to obtain the optimal spatial geometric configuration; Based on the optimal spatial geometric configuration and combined with Kalman filtering, correct the positions of the slave nodes to achieve cooperative navigation of the cluster UAVs; The process of obtaining the optimal spatial geometric configuration includes: when the GDOP value of the spatial geometric configuration is the smallest, obtain the node coordinates of the leading node and the slave nodes and the coordinates of each base station, and then obtain the distance equations from the node coordinates to each base station; perform iterative difference on the distance equations and solve them using the least squares method to obtain the cooperative positioning solution of the leading node to the slave nodes, and then obtain the optimal spatial geometric configuration.

2. The cooperative navigation method for cluster UAVs driven by the target according to claim 1, characterized in that The process of obtaining the position information and ranging information of the leading node includes: obtain the position information of the leading node based on the airborne navigation system and broadcast the position information to the slave nodes through the data link; the slave nodes receive the position information and measure the relative distance to the leading node to obtain the ranging information.

3. The cooperative navigation method for cluster UAVs driven by the target according to claim 1, characterized in that The process of performing spatial geometric configuration on the slave nodes using the maximum vector tetrahedron volume method includes: obtain the three-dimensional relative distance between the leading node and the slave nodes, perform first-order linearized Taylor expansion on the three-dimensional relative distance, and then obtain the cooperative positioning error equation set of the slave nodes; perform least squares method processing on the cooperative positioning error equation set to obtain the error covariance equation; combine the relative ranging error between the leading node and the slave nodes with the error covariance equation to obtain the spatial geometric configuration between the slave nodes and the leading node.

4. The cooperative navigation method for cluster UAVs driven by the target according to claim 1, characterized in that The process of correcting the positions of the slave nodes in combination with Kalman filtering includes: based on the optimal spatial geometric configuration, obtain the preferred leading node; based on the preferred leading node, construct the relative ranging error observation model of the slave nodes and the state recurrence model of the UAV cluster, and then use the extended Kalman filter to perform time update and measurement update on the slave nodes to achieve position correction of the slave nodes.

5. The cooperative navigation method for cluster UAVs driven by the target according to claim 4, characterized in that The process of constructing the relative ranging error observation model of the slave nodes includes: construct the pseudorange equation between the leading node and the slave nodes and perform first-order linearized Taylor expansion, and then construct the Jacobian matrix of the UAV cluster based on the relative ranging method. Based on the Jacobian matrix, construct the relative ranging error observation model of the slave nodes.

6. The target-driven cooperative navigation method for a cluster of unmanned aerial vehicles according to claim 4, wherein The process of constructing the state recurrence model of the unmanned aerial vehicle cluster includes: obtaining the navigation output parameter error of the inertial navigation system carried by the unmanned aerial vehicle and establishing an 18-dimensional state vector, and then constructing the system state transition matrix and the system noise matrix, and constructing the state recurrence model of the unmanned aerial vehicle cluster based on the 18-dimensional state vector, the system state transition matrix and the system noise matrix.

Images