Unmanned aerial vehicle formation control method based on UWB anchor point position optimization

Abstract

The invention belongs to the field of control, and particularly relates to an unmanned aerial vehicle formation control method based on UWB anchor point position optimization. Aiming at the problems of inaccurate positioning and formation keeping caused by obstacle shielding and sensor measurement errors when a sensor network is formed by an unmanned aerial vehicle group carrying anchor points and ground target positioning and formation control are carried out by using a multilateral measurement method, the invention provides a UWB anchor point position optimization method to control the formation of the unmanned aerial vehicles carrying the anchor points; a multilateral ranging method is used for fusion positioning, so that the unmanned aerial vehicles are cognitive and unified to keep formation stability; providing an evaluation parameter reflecting the geometric position quality of the anchor node of the unmanned aerial vehicle, and evaluating the position distribution condition of the unmanned aerial vehicle. According to the method for optimizing the positions of the unmanned aerial vehicles through UWB anchor point arrangement, the formation and the flight height of the multiple unmanned aerial vehicles are limited, the positioning precision is improved, meanwhile, guidance is provided for a control strategy, and the overall calculation amount is reduced.

Description

Technical Field

[0001] The present invention relates to the field of control, and in particular to a UAV formation control method based on UWB anchor point position optimization. Background Art

[0002] With the widespread application of UAV systems in civil and military fields, their adaptability in complex obstacle environments has been continuously improved. Positioning and tracking of ground moving targets has become a key problem and hot direction in the research and practical application of UAV systems. In the research of multi-UAV formation control, common methods include:

[0003] 1. Leader-Follower Model

[0004] This mode selects one drone as the leader and the others as followers. The leader is responsible for planning the flight path and tracking the path, while transmitting its status information to the followers. This method is easy to operate and has a direct control method. However, its stability is highly dependent on the performance of the leader. Once the leader fails, the entire formation may lose control.

[0005] 2. Virtual structure method

[0006] In this method, the formation is regarded as a virtual rigid structure, and all drones are controlled as nodes in the structure. By precisely controlling the position of each node, the movement of the entire formation can be managed. This method reduces the dependence on a single drone, but as a centralized control strategy, it faces the problems of high communication burden and insufficient robustness.

[0007] 3. Consistency Theory

[0008] The consistency theory aims to make the states of each individual in the system (such as position and speed) gradually converge over time through information exchange between intelligent agents. This control method is favored for its distributed architecture and low communication requirements. However, the design of its control strategy and the stability analysis process are relatively complex, which increases the difficulty of implementation.

[0009] The above methods each have their own advantages and disadvantages and are suitable for different scenarios; however, the above methods are only proposed for formation control, without considering the overall tracking problem and lack of consideration for positioning problems.

[0010] Based on the close connection between positioning and control, the present invention adds positioning factors to the generation of drone group trajectories. Cameras, radars, and UWB are all electromagnetic wave sensors, and have similar characteristics when used in wireless sensor networks. The present invention uses UWB for multi-machine data fusion positioning. Commonly used UWB fusion positioning methods include geometric methods such as three-sided ranging positioning and multilateral ranging positioning. The position points estimated by geometric methods are often discontinuous, and the output results need to be filtered and smoothed. The measurement value of the geometric data fusion input filter is the target position point, which can be regarded as a linear positioning method. This method has a fast convergence speed and a small computational burden; in contrast, the state fusion method using nonlinear inputs such as extended Kalman filtering and unscented Kalman filtering, the input filter is the distance measured by the sensor, and the position is obtained through position calculation and filtered. The nonlinear state fusion method has a strong anti-noise ability and is more advantageous than the linear positioning method in dealing with interference such as non-line-of-sight measurement, but the amount of calculation is large, and the disadvantage will gradually expand with the expansion of the network. In summary, the present invention selects the multilateral ranging method for fusion positioning, constructs a fully connected network to increase the robustness of the fusion effect, and the fusion result is processed by Kalman filtering to obtain a smooth output result. Summary of the invention

[0011] 1. Technical issues to be solved

[0012] In view of the deficiencies in the prior art, the present invention provides a UAV formation control method based on UWB anchor point position optimization, which solves the problems raised in the above background technology.

[0013] (II) Technical solution

[0014] In order to achieve the above-mentioned purpose, the present invention specifically adopts the following technical solutions:

[0015] A UAV formation control method based on UWB anchor point position optimization specifically includes the following steps:

[0016] S1: Control the formation of drones carrying anchor points by using the UWB anchor point position optimization method;

[0017] S2: Use the multi-lateral ranging method for fusion positioning, so that multiple drones can unify their cognition to maintain the stability of the formation;

[0018] S3: Use relative position relationship to maintain the formation shape and convert the similarity error between the current formation and the target formation into a differentiable function;

[0019] S4: Propose an evaluation parameter that reflects the quality of the geometric configuration of the UAV anchor nodes and evaluates the UAV position distribution.

[0020] Furthermore, the problem of inaccurate measurement data of a single drone in S1 due to field of view occlusion and mutual interference between drone anchor nodes can be optimized by changing the drone formation arrangement, where each anchor point cooperates with each other to form the maximum field of view. The factor alpha affecting vision is defined as:

[0021]

[0022] α is the solid angle, which measures the field of view from the ground target node to the formation. r is the distance from a certain drone to the target. The drone formation is distributed on the same sphere with a radius of r, and the maximum field of view can be obtained by maximizing alpha.

[0023] Furthermore, in S2, multi-lateral ranging is used in combination with least squares solution for fusion positioning. Taking 4 anchor point drones as an example, the positioning formula is:

[0024]

[0025] Among them, d represents the distance measured by each anchor drone, the three-dimensional coordinates with subscripts 1 to 4 represent the positions of each drone, and the three-dimensional coordinates of t in the table below are the target positions that need to be located. The process of determining the target position is to solve the overdetermined equation. An approximate solution can be found through methods such as the least squares method to minimize the error; the fully connected network reaches a consensus on the position of the target, further preparing for formation control. At the same time, increasing the number of anchor drones can enhance the stability of positioning, so as to obtain as much measurement data of drones as possible for positioning.

[0026] Furthermore, in S3, the relative position relationship is used to maintain the formation shape and the formation position error is quantified as a function, and the relative position relationship is specified as In order to evaluate the deviation from the expected formation, a differentiable formation similarity error metric is proposed, which is formulated as follows:

[0027]

[0028] tr{·} represents the trace of the matrix, L is the symmetric normalized Laplacian operator of the current group formation, L des is the corresponding part of the expected formation. The present invention uses the Frobenius norm in the distance metric ‖·‖ F , as a graph representation matrix, L contains information about the graph structure. This makes f s Only the geometry of the formation is considered, without being affected by scaling, translation, or rotation. In addition, f s It is a dimensionless value that only reflects the error in the similarity of the formation shape.

[0029] Furthermore, the geometric configuration quality evaluation parameter of the drone anchor node in S4, the geometric dilution of precision (GDOP) is defined as follows:

[0030]

[0031] where e is considered as the measurement noise of the UWB sensor node, σ e is the variance, c represents the speed of light, and the covariance matrix of the unbiased estimate is assumed to be Cov(u).

[0032] The present invention proposes a UAV formation control method based on UWB anchor point position optimization, which controls the formation of UAVs carrying anchor points by using the UWB anchor point position optimization method; uses a multilateral ranging method for fusion positioning so that multiple UAVs have unified cognition to maintain the stability of the formation; uses relative position relationship to maintain the formation shape and converts the similarity error between the current formation and the target formation into a differentiable function; and proposes an evaluation parameter that reflects the geometric position quality of the UAV anchor node to evaluate the position distribution of the UAVs.

[0033] (III) Beneficial effects

[0034] Compared with the prior art, the present invention provides a UAV formation control method based on UWB anchor point position optimization, which has the following beneficial effects:

[0035] 1. Aiming at the problems of inaccurate positioning and formation maintenance when a UAV formation tracks a target, the present invention uses a UWB anchor point arrangement method to optimize the position control of the UAV, achieves the purpose of combining control with positioning, and proposes an evaluation index for the control effect.

[0036] 2. To address the problem of inconsistent position recognition during UAV formation flight, the present invention uses relative positions to restrict the formation arrangement relationship and forms a function form that is easy to calculate.

[0037] 3. In order to solve the problem that the amount of calculation for multi-UAV positioning increases with the number of sorties, the present invention proposes a multilateral ranging method for fusion positioning to ensure that symmetrical position data participate in the fusion, which can maintain the accuracy of the fusion data while taking into account a smaller amount of calculation. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 A flow chart of a UAV formation control method based on UWB anchor point position optimization provided by an embodiment of the present invention;

[0039] Figure 2 Causes of the position arrangement and positioning errors of the drones provided in the embodiments of the present invention;

[0040] Figure 3The relationship between each anchor point and the visual factor alpha influencing the embodiment of the present invention in 2D;

[0041] Figure 4 The formation arrangement of three anchor points determined based on the best alpha in the 3D case according to the embodiment of the present invention;

[0042] Figure 5 The overall flow chart of fusion positioning for the multilateral ranging method;

[0043] Figure 6 Schematic diagram of data fusion for fusion positioning using the multilateral ranging method. DETAILED DESCRIPTION

[0044] 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.

[0045] Example

[0046] like Figure 1-6 As shown in the figure, the present invention provides a UAV formation control method based on UWB anchor point position optimization, which uses the UWB anchor point arrangement method to optimize the position control of the UAV, and uses the relative position relationship to limit the formation arrangement. The optimized UAV anchor points are accurately positioned using the multilateral ranging method to achieve an effective combination of positioning and control. Finally, the geometric precision loss index is used to evaluate the control effect of the UAV formation.

[0047] S1: Control the formation of drones carrying anchor points by using the UWB anchor point position optimization method;

[0048] S2: Use the multi-lateral ranging method for fusion positioning, so that multiple drones can unify their cognition to maintain the stability of the formation;

[0049] S3: Use relative position relationship to maintain the formation shape and convert the similarity error between the current formation and the target formation into a differentiable function;

[0050] S4: Propose an evaluation parameter that reflects the quality of the geometric configuration of the UAV anchor nodes and evaluates the UAV position distribution.

[0051] S101, controlling the formation of UAVs carrying anchor points using the UWB anchor point position optimization method;

[0052] Figure 2The figure shows the arrangement of drone positions and the causes of positioning errors, but it only considers two drones carrying anchors. In this figure, the green and red circles are ideal measurement ranges without noise, and the black dotted circle represents the actual measurement of the W2 drone affected by noise. The actual target position is at the intersection of the red and green circles, but under the influence of noise, the target point is located at the intersection of the red and dotted circles. Observation Figure 2 The error between the two intersection points (a) and (b) shows that the positioning error is related to the distance between the two drones and the angle α (a factor affecting vision) between the two circle centers and their intersection points. The larger α is, the smaller the error distance between the two intersection points will be. Figure 3 In the 2D case, the relationship between the three anchor points and the visual factor alpha is shown. In order to maintain the maximum alpha, this example proposes that the distance between two consecutive waypoints is equal, so that an equilateral triangle trajectory is formed for the three waypoints. When a drone obtains the first distance measurement value at A, the target node is located somewhere in the circle with A as the center and a range of r. In this way, the final drone formation arrangement design is as follows Figure 3 As shown in (b), the formation is arranged with A as the center, and three drones are evenly distributed on the circle in an equilateral triangle. This embodiment is a multi-drone tracking ground target, which requires multi-drone formation control under 3D conditions. Multilateral positioning requires the coordination of four drones. According to the principle of maximum visual factor alpha, the optimal formation arrangement method is as follows Figure 4 Tetrahedron formed.

[0053] S102, using the multi-lateral ranging method for fusion positioning, so that multiple drones can have unified cognition to maintain the stability of the formation;

[0054] The overall flow chart of multi-lateral ranging method for fusion positioning is as follows: Figure 5 As shown in the figure, it is divided into three steps: measurement information collection, multi-lateral positioning solution and Kalman filtering. Multi-lateral ranging information collection uses the time difference measurement method (TOA) and combines the bilateral two-way ranging method to reduce the influence of factors such as clocks to obtain accurate distance information input.

[0055] The process of multi-lateral distance measurement to calculate the target position information is as follows. Figure 6 As shown:

[0056] Based on the labels of known anchor positions, the target point of unknown position can be determined by calculating the intersection of three circles in two dimensions and the intersection of at least four spheres in three dimensions. However, the deviation caused by the inevitable noise in the wireless channel will affect the ranging measurement in practice, causing the distances of each measurement to intersect in a smaller range rather than a clear point. The main causes of noise in UWB wireless channels include non-line-of-sight interference, multipath effects, hardware limitations, etc. Due to these measurement noises, the circles in practice rarely intersect at a single location, and what is usually obtained is an estimated location area. Assuming a constant deviation, we can conclude that if we increase the number of anchor nodes in the system, the estimated area will become smaller. We can assume that the anchor drone position X in 3D i =[x i ,y i ,z i ] T , target label position X t =[x t ,y t ,z t ] T , where i = 1, 2, 3, 4 are the IDs of known anchor points. The general spherical equation for true range multilateration using the measured distance between the anchor point and the tag (i.e. the radius of the sphere) is as follows:

[0057]

[0058] Among them, d i is anchor point X i and label X t The present invention uses the actual range of four anchor points to perform multilateral positioning, and the formula is derived as follows:

[0059]

[0060] Subtracting (10) from (11) and simplifying yields:

[0061]

[0062] Similarly, subtracting (10) from (12) and (13) can yield the following matrix-represented equation system:

[0063] Ax=b (15)

[0064] in,

[0065]

[0066] The elements of the matrix A in (15) come from the reference coordinates of the anchor nodes, which are the known coordinate values ​​of the system. All elements in the vector b are also known to the system, namely the measured distance range and the coordinates of the anchor points. Therefore, the vector x is the only unknown item in (15) and is also the target position to be estimated in the system. If the square matrix A T A is reversible, we can regard equation (15) as a least squares problem. The process of solving the target position point is to solve the problem of overdetermined equations.

[0067] S103, maintaining the formation shape using the relative position relationship and converting the similarity error between the current formation and the target formation into a differentiable function;

[0068] After completing the formation position arrangement design and the multi-lateral positioning target process, the drone formation is controlled. The four drone anchor points use relative positions to form a regular tetrahedron formation. The four drone group configuration is represented by an undirected graph Modeling, where is a set of vertices, is the set of edges. In the example, vertex i represents a vertex with position vector The i-th drone of . Connect the vertices and vertices Edge Indicates that drones i and j can measure the geometric distance between each other. In our work, each robot can obtain all robots {p1,...,p i ,...,p N} location information; therefore, Figure Then, we pass

[0069] A ij =w ij =||p i -p j || 2 (16)

[0070]

[0071] Determine the configuration diagram The adjacency matrix of Sum degree matrix Among them, the non-negative edge weight w ij is the square distance between the i-th UAV and the j-th UAV, ‖·‖ represents the Euclidean norm. The corresponding Laplace matrix is:

[0072] L=DA (18)

[0073] Using the above matrix, Figure The symmetric normalized Laplacian matrix of is defined as:

[0074] L=D -1 / 2 LD -1 / 2 =ID -1 / 2 AD -1 / 2 (19)

[0075] in is the identity matrix. L contains information that is invariant to scale, translation, and rotation.

[0076] Finally, we use graph theory to describe the desired tetrahedral shape. By specifying the positions The desired shape is independent of the coordinate system, as long as the relative positions are provided. To evaluate the deviation from the desired formation, we propose a differentiable formation similarity error metric.

[0077]

[0078] tr{·} represents the trace of the matrix, L is the symmetric normalized Laplacian operator of the current group formation, L des is the corresponding part of the expected formation. The present invention uses the Frobenius norm in the distance metric ‖·‖ F , as a graph representation matrix, L contains information about the graph structure. This makes f s Only the geometry of the formation is considered, without being affected by scaling, translation, or rotation. In addition, f s It is a dimensionless value that only reflects the error in the similarity of the formation shape.

[0079] S104, propose an evaluation parameter reflecting the geometric configuration quality of the drone anchor node to evaluate the drone position distribution.

[0080] In the process of multi-UAV formation flight, in order to quantitatively analyze the positioning performance under different anchor point UAV geometric configurations, the geometric dilution of precision (GDOP) index is introduced. It reflects the quality of the geometric configuration of the anchor node and can observe the pros and cons of the UAV formation for positioning performance in real time. Here, it is assumed that the unbiased estimated covariance matrix is ​​Cov(u). GDOP can be defined as:

[0081]

[0082] where e is considered as the measurement noise of the UWB sensor node, σ e is the variance, c represents the speed of light, and the covariance matrix of the unbiased estimate is assumed to be Cov(u).

Claims

1. A UAV formation control method based on UWB anchor point position optimization, characterized in that: The control method comprises: S1: Control the formation of drones carrying anchor points by using the UWB anchor point position optimization method; S2: Use the multi-lateral ranging method for fusion positioning, so that multiple drones can unify their cognition to maintain the stability of the formation; S3: Use relative position relationship to maintain the formation shape and convert the similarity error between the current formation and the target formation into a differentiable function; S4: Propose an evaluation parameter that reflects the quality of the geometric configuration of the UAV anchor nodes and evaluates the UAV position distribution.

2. The method for controlling a UAV formation based on UWB anchor point position optimization according to claim 1, characterized in that: In S1, each anchor point cooperates with each other to form the maximum field of view, reducing the NLOS situation of UWB detection and increasing the accuracy of fusion data. The factor alpha that affects vision is defined as: α is the solid angle, which measures the field of view from the ground target node to the formation. r is the distance from a certain drone to the target. The drone formation is distributed on the same sphere with a radius of r, and the maximum field of view can be obtained by maximizing alpha.

3. The method for controlling a UAV formation based on UWB anchor point position optimization according to claim 1, characterized in that: In S2, multiple drones form a fully connected network and use the multilateral ranging method for fusion positioning. Taking 4 anchor drones as an example, the positioning formula is: Among them, d represents the distance measured by each anchor point drone, the three-dimensional coordinates with subscripts 1 to 4 represent the position of each drone, and the three-dimensional coordinates with subscripts t are the target positions that need to be located. The process of determining the target position is to solve the overdetermined equation. An approximate solution can be found by the least squares method and other methods to minimize the error.

4. The method for controlling a UAV formation based on UWB anchor point position optimization according to claim 1, characterized in that: In S3, the relative position relationship is used to maintain the formation shape and the formation position error is quantified as a function. The relative position relationship is specified as In order to evaluate the deviation from the expected formation, a differentiable formation similarity error metric is proposed, which is formulated as follows: tr{·} represents the trace of the matrix, L is the symmetric normalized Laplacian operator of the current group formation, L des is the corresponding part of the expected formation; the present invention uses the Frobenius norm in the distance metric ‖·‖ F , as a graph representation matrix, L contains information about the graph structure; this makes f s Only the geometry of the formation is considered, without being affected by scaling, translation or rotation; f s It is a dimensionless value that only reflects the error in the similarity of the formation shape.

5. The method for controlling a UAV formation based on UWB anchor point position optimization according to claim 1, characterized in that: In S4, the evaluation parameters of the geometric configuration quality of the drone anchor nodes are used to evaluate the arrangement of the drone positions to reduce the formation control error. The Geometric Dilution of Precision (GDOP) is defined as follows: Where e represents the measurement noise of the UWB sensor node, σ e is the variance, c represents the speed of light, and the covariance matrix of the unbiased estimate is assumed to be Cov(u).

Images