A method for dynamic target estimation of UAV in information denial environment

Abstract

The present invention discloses a method for estimating dynamic targets of unmanned aerial vehicles (UAVs) in an information denial environment. The method comprises: obtaining three-dimensional angle information, the three-dimensional angle information including the angle of arrival from the UAV to a first anchor point, the angle of arrival from the UAV to a second anchor point, and the angle of arrival from the UAV to the target; filtering the three-dimensional angle information with an extended Kalman filter to obtain estimated UAV state information and corresponding state covariance information, the UAV state information being used to characterize the position and velocity of the UAV and the position and velocity of the target; and constructing a loss function using the UAV state information and the state covariance information to optimize the flight path of the UAV and achieve observation of the target. The present invention utilizes two anchor point information with known absolute positions to achieve position tracking of the UAV and the target.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicle (UAV) technology, and more particularly to a method for estimating dynamic targets of UAVs in an information denial environment. Background Art

[0002] Target tracking technology is widely used in various applications, such as aerial photography, public safety, and humanitarian search and rescue. Estimating the target's position and velocity from noisy measurements obtained by various sensors is a widely researched area. Unmanned aerial vehicles (UAVs) can be used for target tracking due to their excellent maneuverability when equipped with angle-of-arrival (AOA) sensors. By solving a trigonometric problem and combining it with a filter estimator, the position and velocity of both the UAV and the target can be estimated from noisy AOA measurements. Furthermore, researchers have proposed various estimation algorithms to estimate target states using nonlinear AOA measurements. For example, the maximum likelihood estimator (MLE) and pseudo-linear estimator (PLE), as classic batch filters, have been used to estimate target position and velocity from AOA target measurements with Gaussian noise. Kalman-based methods, considered as recursive filters, are more commonly used in the field of target tracking. For AOA target tracking, the extended Kalman filter (EKF), unscented Kalman filter (UKF), pseudo-linear Kalman filter (PLKF), and cubic Kalman filter (CKF) have all been used to varying degrees.

[0003] Existing research has shown that optimizing the UAV's flight path to make the information gathered from measurements more effective can significantly improve target tracking performance. To improve target estimation accuracy, a cost function should be designed for UAV path optimization. Common cost functions include the estimated covariance matrix and the Fisher Information Matrix (FIM). Various algorithms can be used to optimize cost functions, including gradient-based, exhaustive search, and learning-based methods.

[0004] In the prior art, the location of a drone is assumed to be accurately obtained using external information, such as the Global Positioning System (GPS). However, drones may operate in environments where external signals are missing (or so-called information denial environments), such as indoor spaces and interference areas. In these areas, the drone's external signals are missing and its position cannot be obtained. Currently, target positioning without knowing its own position has attracted a lot of interest. For example, in simultaneous localization and mapping (SLAM) applications, additional information about surrounding anchor points (such as the location of nearby buildings) is added to obtain the absolute target position. In wireless communications, target positioning through multiple anchor points (such as base station positioning) is already relatively mature, but since it is basically performed through the time difference of arrival (TDOA) method, the required number of anchor points must be greater than or equal to 3 or other prior knowledge must be introduced to uniquely determine the target position.

[0005] After analysis, it is found that in the existing technology, whether it is a target estimation scheme based solely on time difference of arrival (TDOA) or angle of arrival (AOA), it can only obtain the distance and angle of the target relative to the drone, but cannot obtain its absolute position. Summary of the Invention

[0006] The purpose of the present invention is to overcome the above-mentioned defects of the prior art and provide a method for estimating dynamic targets of unmanned aerial vehicles in an information denial environment, the method comprising:

[0007] Acquire three-dimensional angle information, the three-dimensional angle information including an arrival angle from the drone to the first anchor point, an arrival angle from the drone to the second anchor point, and an arrival angle from the drone to the target;

[0008] The three-dimensional angle information is filtered using an extended Kalman filter to obtain estimated drone state information and corresponding state covariance information, wherein the drone state information is used to characterize the position and velocity of the drone and the position and velocity of the target;

[0009] The loss function is constructed using the drone state information and the state covariance information to optimize the flight path of the drone at subsequent moments and achieve observation of the target.

[0010] Compared with the existing technology, the advantage of the present invention is that it provides a method for autonomous target tracking of drones based on azimuth (angle, which can be obtained by a PTZ camera) and anchor points. By introducing anchor points with known absolute positions, the method can provide the drone with the absolute position information of itself and the target, and track the target in an environment where external signals (GPS, RTK, etc.) are lost, providing a more robust solution for the positioning and tracking of the drone itself and the target. The present invention can be applied to the positioning systems of various unmanned aerial vehicles, such as multi-rotor drones and fixed-wing drones. It can serve as a redundant means of self-positioning and can also provide the absolute geographic coordinates of the target based on the azimuth angle, thereby improving positioning speed and accuracy.

[0011] Further features and advantages of the present invention will become apparent from the following detailed description of exemplary embodiments of the present invention with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0012] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and, together with the description, serve to explain the principles of the invention.

[0013] Figure 1 is a geometric diagram of target tracking according to one embodiment of the present invention;

[0014] Figure 2is a flow chart of a method for estimating a dynamic target of a UAV in an information denial environment according to an embodiment of the present invention;

[0015] Figure 3 is a flow chart of an extended Kalman filter according to one embodiment of the present invention;

[0016] Figure 4 is a schematic diagram of constructing a loss function according to one embodiment of the present invention;

[0017] Figure 5 FIG. 4 is a process diagram of a path optimization method according to an embodiment of the present invention. DETAILED DESCRIPTION

[0018] Various exemplary embodiments of the present invention will now be described in detail with reference to the accompanying drawings. It should be noted that unless otherwise specifically stated, the relative arrangement of components and steps, numerical expressions and numerical values set forth in these embodiments do not limit the scope of the present invention.

[0019] The following description of at least one exemplary embodiment is merely illustrative in nature and is in no way intended to limit the invention, its application, or uses.

[0020] Technologies, methods, and equipment known to ordinary technicians in the relevant art may not be discussed in detail, but where appropriate, the technologies, methods, and equipment should be considered part of the specification.

[0021] In all examples shown and discussed herein, any specific values should be interpreted as merely exemplary and not limiting. Therefore, other examples of the exemplary embodiments may have different values.

[0022] It should be noted that like reference numerals and letters refer to like items in the following figures, and therefore, once an item is defined in one figure, it need not be further discussed in subsequent figures.

[0023] The present invention provides a method for autonomous target positioning of UAV in an environment without external signals, which mainly includes two parts: target filtering estimation and UAV path optimization. The method introduces two stationary markers with known absolute positions, and uses the three-dimensional angle information collected by the angle of arrival (AOA) sensor (including the arrival angle θ between the UAV and the anchor point 1) to calculate the target location. ub1 , the arrival angle θ between the drone and anchor point 2 ub2 , the arrival angle θ between the UAV and the moving target up) as input. After a period of continuous observation, the position and velocity errors of the moving target and the drone itself can be converged to a low range, ultimately outputting the position and velocity of both the drone and the target. Because sensor position affects the filter estimator's observation of the target, a path optimization algorithm is introduced after filtering to improve the efficiency and accuracy of target positioning. Furthermore, for the drone's safety, no-fly zones of varying sizes are set around targets and landmarks during path planning.

[0024] See also Figure 1 The target tracking geometry diagram shows the positional relationship between the moving target, the drone, anchor 1 (anchor1) and anchor 2 (anchor2) at time k, where anchor 1 and anchor 2 are landmarks with known absolute positions. For example, the coordinates of anchor 1 are b1 = [x b1 ,y b1 ] T , the coordinates of anchor point 2 are b2=[x b2 ,y b2 ] T , T represents transposition; the coordinates of the moving target at time k are p k =[x pk ,y pk ] T , the coordinates are unknown, the position of the drone at time k is marked as u k (Unknown). k is a discrete time identifier, and the discrete time interval is set to M.

[0025] See also Figure 2 As shown, the provided method for estimating dynamic targets of UAVs in an information denial environment mainly includes: step S110, obtaining AOA sensor measurements; step S120, extending the Kalman filter; step S130, a path optimization algorithm based on gradient descent; step S140, determining whether it is within the no-fly distance range; if so, performing path replanning (step S150); and then moving in a specified direction at a fixed speed (step S160).

[0026] The following section describes the extended Kalman filter and gradient descent optimization algorithm. A recursive Kalman filter can be used as the filter. Because the sensor measurements and the model have a nonlinear relationship, a nonlinear filtering approach is ultimately performed based on the extended Kalman filter.

[0027] See also Figure 3 The extended Kalman filtering process shown in Figure 1 mainly includes the following steps:

[0028] Step S210 , predicting the prior state and covariance based on the state transition model and the initial state.

[0029] X k|k-1 =FXk-1|k-1 +m k (1)

[0030] P k|k-1 =FX k-1|k-1 F T +Q k (2)

[0031] Among them, X k|k-1 is based on the k-1 moment state X k-1|k-1 Prior estimate of; F is the state transfer matrix; P k|k-1 For state X k|k-1 The covariance matrix of the state X k|k-1 The degree of uncertainty; m k is the process noise, which is used to quantify the systematic errors that are not fully considered, for example, m k is an independent zero-mean additive Gaussian white noise, that is, m k ~N(0,Q k ), Q k is the system error m k The covariance matrix of

[0032] The state matrix X is composed of the position and velocity of the drone itself and the position and velocity of the target, for example, in, Represents x uk The derivative of time (speed), specifically, x uk is the position of the drone on the x-axis, is the velocity component of the drone on the x-axis, y uk is the position of the drone on the y-axis, is the velocity component of the drone on the y-axis, x pk is the position of the target on the x-axis, is the velocity component of the target on the x-axis, y pk is the target's position on the y-axis, is the target's velocity component on the y-axis.

[0033] State X at time K-1 k-1|k-1 Transfer to the prior state X through the state transfer matrix F k|k-1 , where the state transfer is assumed to be a constant velocity model, so the state transfer matrix is Where Fi is That is, P k+1 =P k +V k M.

[0034] Step S220: Calculate the Jacobian matrix based on the current state and measurement.

[0035] H k=Jacobian(X k|k-1 , Z k ) (2)

[0036] Among them, Z k is the three-dimensional angle information observed by the AOA sensor at time k, that is, Z k =[θ b1k ,θ b2k ,θ pk ] T , H k is the measurement matrix at time k, which is a Jacobian matrix with 3 rows and 8 columns, expressed as:

[0037]

[0038] Among them, d ub1 =||u k -b1||2,d ub2 =||u k -b2||2,d up =||u k -p k ||2.

[0039] Step S230: Calculate the estimated state residual according to the measurement model.

[0040]

[0041] in, is the residual, which represents the error between measurement and estimation; the function h(·) is a nonlinear measurement function that transforms data from the state space to the measurement space, which is specifically expanded as follows:

[0042]

[0043] Among them, n k Represents the sensor measurement noise. For example, it is an independent zero-mean additive Gaussian white noise, that is, n k ~N(0,R k ).

[0044] Step S240: Calculate the Kalman gain.

[0045]

[0046]

[0047] Among them, R k The noise level is used to characterize the noise of the sensor measurement data. For example, it can be obtained from the sensor specification sheet. k To simplify the writing of intermediate variables; K kis the Kalman gain at time k.

[0048] Step S250: Update the posterior estimate and covariance.

[0049]

[0050] P k|k =(IK k H k )P k|k-1 (9)

[0051] Among them, X k|k is the posterior estimated state; P k|k is the posterior covariance matrix; I is the 8*8 identity matrix.

[0052] For the path optimization algorithm, it takes the current state as input and predicts the direction of the drone that can effectively reduce the loss function (or cost function) through gradient descent, so that the drone can obtain the best observation path when moving along this direction.

[0053] Figure 4 It is a schematic diagram of constructing the loss function, which takes the current state X k|k And a small displacement vector δ is input, which is passed into the copy of the original filter to complete the one-step prediction, thereby obtaining the state covariance matrix obtained by moving the small displacement δ, and finally outputting the trace of the covariance matrix.

[0054] Combine Figure 4 As shown, constructing the loss function specifically includes:

[0055] X k|k-1 =FX k-1|k-1 +m k (10)

[0056] P k|k-1 =FX k-1|k-1 F T +Q k (11)

[0057] X k+1|k，δ =X k+1|k +δ (12)

[0058] Among them, δ is a small displacement vector; X k+1|k，δ For state X k+1|k The state after a small displacement.

[0059] Then, according to X k+1lk，δ Calculate H k+1 .

[0060] calculate:

[0061] calculate:

[0062] Calculation: P k+1|k+1 =(IK k+1 H k+1) P k+1|k (15)

[0063] Calculation: J(X` k+1|k+1 )=tr(P k+1|k+1 ) (16)

[0064] Where tr(·) is the matrix trace; J(·) is the cost value after a small shift.

[0065] In one embodiment, assuming the drone moves in a 2D plane, its movement has two degrees of freedom. Therefore, it can be decomposed into two components along the x-axis and y-axis, which can be further divided into positive and negative directions. After combining, four possible movement directions are obtained: (d, 0), (-d, 0), (0, d), (0, -d)]. By moving in these four directions, the cost value of each direction is obtained, thereby obtaining a measure of the quality of observation in each direction.

[0066] Figure 5 This is a schematic diagram of the path optimization process. One branch is used to calculate the cost of the drone flying along the x-axis, and the other branch is used to calculate the cost of the drone flying along the y-axis. The specific steps include:

[0067] Calculate the cost J of moving along the positive direction of the x-axis using δ = [d, 0] as the parameter xp , where d is a smaller scalar representing the step size;

[0068] Calculate the cost J of moving along the negative direction of the x-axis using δ = [-d, 0] as the parameter xn ;

[0069] Calculate the total cost of the drone flying along the x-axis

[0070] Calculate the cost J of moving along the positive direction of the y axis using δ = [0, d] as the parameter yp ;

[0071] Calculate the cost J of moving along the negative direction of the y axis using δ = [0, -d] as the parameter yn ;

[0072] Calculate the total cost of the drone flying along the y-axis

[0073] Calculate the total cost: J = [J x , J y ];

[0074] Calculate the optimized direction at time k, expressed as:

[0075]

[0076] Among them, ||·||2 is the L2 regularization, and v represents the constant flight speed of the drone.

[0077] After completing path planning, the closer the drone gets to a landmark or target, the more accurate the information it obtains. This can cause the drone to continuously approach the landmark or target until it overlaps and collides with it. Therefore, after completing path planning, it is necessary to set a no-fly zone based on the position of the drone and the landmark or target to prevent the drone from flying too close and causing danger.

[0078] In summary, the present invention introduces markers (one or more) in conjunction with the AOA sensor to complete the acquisition of the drone's own coordinates, and also completes the acquisition of the target coordinates by introducing markers (one or more) in conjunction with the AOA sensor. In addition, the gradient descent of the cost function is completed by random perturbation to optimize the flight path of the drone and obtain the best observation of the moving target. The present invention only requires, for example, a PTZ camera to measure the azimuth between the drone and the anchor point and the target. The absolute position information of the drone and the target can be obtained without any other information. The position tracking of the drone and the target can be completed by using at least two anchor point information with known absolute positions. It has been verified that the present invention improves the efficiency and accuracy of target tracking based on AOA.

[0079] The present invention may be a system, a method and / or a computer program product. The computer program product may include a computer-readable storage medium carrying computer-readable program instructions for causing a processor to implement various aspects of the present invention.

[0080] Computer-readable storage medium can be a tangible device that can keep and store the instructions used by the instruction execution device.Computer-readable storage medium can be, for example, but not limited to, an electrical storage device, a magnetic storage device, an optical storage device, an electromagnetic storage device, a semiconductor storage device or any suitable combination thereof.More specific examples (non-exhaustive list) of computer-readable storage medium include: a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), a static random access memory (SRAM), a portable compact disc read-only memory (CD-ROM), a digital versatile disk (DVD), a memory stick, a floppy disk, a mechanical encoding device, for example, a punch card or a convex structure in a groove having instructions stored thereon, and any suitable combination thereof.Computer-readable storage medium used herein is not interpreted as a transient signal itself, such as radio waves or other freely propagating electromagnetic waves, electromagnetic waves propagated by waveguides or other transmission media (for example, light pulses by fiber optic cables), or electrical signals transmitted by wires.

[0081] The computer-readable program instructions described herein can be downloaded from a computer-readable storage medium to each computing / processing device, or downloaded to an external computer or external storage device via a network, such as the Internet, a local area network, a wide area network, and / or a wireless network. The network can include copper transmission cables, fiber optic transmission, wireless transmission, routers, firewalls, switches, gateway computers, and / or edge servers. The network adapter card or network interface in each computing / processing device receives the computer-readable program instructions from the network and forwards the computer-readable program instructions to be stored in the computer-readable storage medium in each computing / processing device.

[0082] The computer program instructions for performing the operation of the present invention can be assembly instructions, instruction set architecture (ISA) instructions, machine instructions, machine-dependent instructions, microcode, firmware instructions, state setting data, or source code or object code written in any combination of one or more programming languages, including object-oriented programming languages such as Smalltalk, C++, Python, and conventional procedural programming languages such as "C" language or similar programming languages. The computer readable program instructions can be executed entirely on the user's computer, partially on the user's computer, as an independent software package, partially on the user's computer, partially on a remote computer, or completely on a remote computer or server. In the case of a remote computer, the remote computer can be connected to the user's computer via any type of network, including a local area network (LAN) or a wide area network (WAN), or can be connected to an external computer (e.g., using an Internet service provider to connect via the Internet). In some embodiments, an electronic circuit, such as a programmable logic circuit, a field programmable gate array (FPGA), or a programmable logic array (PLA), is personalized by utilizing the state information of the computer readable program instructions, and the electronic circuit can execute the computer readable program instructions, thereby realizing various aspects of the present invention.

[0083] Various aspects of the present invention are described herein with reference to flowcharts and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the present invention. It should be understood that each block of the flowcharts and / or block diagrams, and combinations of blocks in the flowcharts and / or block diagrams, can be implemented by computer-readable program instructions.

[0084] These computer-readable program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing device, thereby producing a machine, so that when these instructions are executed by the processor of the computer or other programmable data processing device, a device is generated that implements the functions / actions specified in one or more blocks in the flowchart and / or block diagram. These computer-readable program instructions can also be stored in a computer-readable storage medium, where these instructions cause the computer, programmable data processing device, and / or other device to operate in a specific manner. Thus, the computer-readable medium storing the instructions comprises an article of manufacture that includes instructions for implementing various aspects of the functions / actions specified in one or more blocks in the flowchart and / or block diagram.

[0085] Computer-readable program instructions may also be loaded onto a computer, other programmable data processing apparatus, or other device so that a series of operational steps are performed on the computer, other programmable data processing apparatus, or other device to produce a computer-implemented process, thereby causing the instructions executed on the computer, other programmable data processing apparatus, or other device to implement the functions / actions specified in one or more blocks in the flowchart and / or block diagram.

[0086] The flowcharts and block diagrams in the accompanying drawings show the possible implementation architecture, functions and operations of the systems, methods and computer program products according to multiple embodiments of the present invention. In this regard, each box in the flowchart or block diagram can represent a module, program segment or part of an instruction, and the module, program segment or part of the instruction contains one or more executable instructions for implementing the specified logical function. In some alternative implementations, the functions marked in the box can also occur in an order different from that marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flowchart, and the combination of boxes in the block diagram and / or flowchart, can be implemented by a dedicated hardware-based system that performs the specified function or action, or can be implemented by a combination of dedicated hardware and computer instructions. It is well known to those skilled in the art that implementation by hardware, implementation by software, and implementation by a combination of software and hardware are all equivalent.

[0087] While various embodiments of the present invention have been described above, the foregoing description is intended to be illustrative, non-exhaustive, and not limited to the disclosed embodiments. Many modifications and variations will be apparent to those skilled in the art without departing from the scope and spirit of the described embodiments. The terminology used herein is selected to best explain the principles of the embodiments, their practical applications, or technological improvements in the marketplace, or to enable others skilled in the art to understand the embodiments disclosed herein. The scope of the present invention is defined by the appended claims.

Claims

1. A method for estimating dynamic targets of unmanned aerial vehicles in an information denial environment, comprising the following steps: Acquire three-dimensional angle information, the three-dimensional angle information including an arrival angle from the drone to the first anchor point, an arrival angle from the drone to the second anchor point, and an arrival angle from the drone to the target; The three-dimensional angle information is filtered using an extended Kalman filter to obtain estimated drone state information and corresponding state covariance information, wherein the drone state information is used to characterize the position and velocity of the drone and the position and velocity of the target; Constructing a loss function using the drone state information and the state covariance information to optimize the drone's flight path and achieve target observation; Among them, for the three-dimensional angle information, it is beneficial to filter with an extended Kalman filter to obtain estimated drone state information and corresponding state covariance information including: Predict the drone’s prior state and covariance, expressed as: in, Based on the k-1 moment state A priori estimate of Status The covariance matrix of is the process noise, yes The covariance matrix of , is the position of the drone on the x-axis, is the velocity component of the drone on the x-axis, is the position of the drone on the y-axis, is the velocity component of the drone on the y-axis, is the position of the target on the x-axis, is the velocity component of the target on the x-axis, is the target's position on the y-axis, is the velocity component of the target on the y-axis, F is the state transfer matrix; Calculate the Jacobian matrix , expressed as: in, is the three-dimensional angle information observed at time k, is a Jacobian matrix with 3 rows and 8 columns ； Calculate estimated state residuals , expressed as: in, is the residual, Expressed as: = Calculate the Kalman gain, expressed as: in, To measure noise, is an intermediate variable, is the Kalman gain at time k; Update the posterior estimate and covariance as the estimated drone state information and the corresponding state covariance information, expressed as: in, is the posterior estimated state, is the posterior covariance matrix, I is the 8*8 identity matrix, is the x-axis coordinate of the first anchor point, is the y-axis coordinate of the first anchor point, is the x-axis coordinate of the second anchor point, is the y-axis coordinate of the second anchor point, is the x-axis coordinate of the target at time k, is the y-axis coordinate of the target at time k, , , , , , is the measurement noise of the sensor.

2. The method according to claim 1, characterized in that The loss function is constructed according to the following steps: Computational Status After a small displacement The subsequent state is represented as: ； according to calculate ; calculate ; calculate calculate ; calculate ; in, To get the matrix trace, is the cost after a small displacement.

3. The method according to claim 2, characterized in that Constructing a loss function using the drone state information and the state covariance information to optimize the drone's flight path includes: by Calculate the cost value of the drone moving along the positive direction of the x-axis for the parameter ; by Calculate the cost value of the drone moving along the negative direction of the x-axis for the parameter ; Calculate the total cost of the drone along the x-axis ; by Calculate the cost value of the drone moving along the positive direction of the y-axis for the parameter ; by Calculate the cost value of the drone moving along the negative direction of the y axis for the parameter ; Calculate the total cost of the drone along the y-axis Calculating total cost ; The calculated total cost value is used to obtain the direction of the optimized k-th moment, which is expressed as: in, is L2 regularization, Indicates the step size.

4. The method according to claim 1, wherein The three-dimensional angle information is measured and obtained using an arrival angle sensor.

5. The method according to claim 1, wherein Also includes: When optimizing the drone's flight path, set no-fly zones around targets and landmarks.

6. The method according to claim 1, characterized in that The drone is an unmanned aerial vehicle, including a multi-rotor drone or a fixed-wing drone.

7. The method according to claim 1, characterized in that Also includes: The absolute geographic coordinates of the target are determined based on the arrival angle of the drone to the target.

8. A computer-readable storage medium having a computer program stored thereon, wherein: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

9. A computer device comprising a memory and a processor, wherein a computer program capable of being executed on the processor is stored in the memory, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 7 are implemented.

Images