Method for simultaneous positioning estimation of unmanned aerial vehicle and target based on TOA sensor

Abstract

The invention discloses an unmanned aerial vehicle and target simultaneous positioning estimation method based on a TOA sensor. The method comprises the following steps: acquiring arrival time measurement information, wherein the arrival time measurement information comprises first measurement time of a sensor relative to a target, second measurement information of the sensor relative to an anchor point and third measurement information of mutual observation between the sensors; constructing a maximum likelihood estimation problem for the sensor and the target based on the arrival time measurement information; and for the maximum likelihood estimation problem, positioning a sensor position and a target position by using an iterative search algorithm. According to the method, the reference marker with absolute position information is introduced, simultaneous positioning of the sensor and the target is expanded to arrival time measurement under the three-dimensional condition, and the positioning estimation precision of the unmanned aerial vehicle and the target is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of target positioning estimation, and more particularly to a UAV and target simultaneous localization and mapping method based on TOA sensors. BACKGROUND

[0002] The main function of TOA (Time of Arrival) measurement is to achieve high-precision positioning of targets. By measuring the time of arrival of signals, the position of the target can be accurately measured and tracked. TOA measurement has the advantages of high precision and accuracy, and is not affected by signal strength, reflection and interference, so it performs well in complex environments. In addition, TOA measurement does not require complex hardware devices, only precise clock synchronization and multiple receivers are needed to achieve high-precision positioning. TOA measurement is widely used in UAV control, communication systems, navigation and positioning systems, etc. For example, the Global Navigation Satellite System (GNSS) uses the TOA of satellite signals to determine the position of the user. In addition, TOA measurement is also used in intelligent transportation systems, indoor positioning, radar systems and communication systems. In indoor positioning, TOA measurement can be used to achieve accurate indoor navigation and location services. In summary, TOA measurement is a high-precision target positioning technology that determines the position of a target by measuring the propagation time of a signal. TOA measurement has high precision, is not affected by interference and has a wide range of application scenarios.

[0003] TOA positioning mainly has two categories, namely the positioning method based on elliptical TOA measurement and the positioning method based on circular TOA measurement. The main difference between the two methods is that in the former application scenario, the signal transmitter and the signal receiver are separated, while in the latter application scenario, the signal transmitter and the receiver are integrated. Since the relationship between TOA measurement and target position is nonlinear, and measurement noise is inevitable, different nonlinear estimation algorithms need to be applied.

[0004] In the prior art, the Maximum Likelihood Estimator (MLE) is a commonly used method in target positioning estimation, which is used to estimate parameters or target states. The core idea of the Maximum Likelihood Estimator is to find the parameter value that makes the observed data appear with the highest probability under a given probability model. It assumes that the observed data is independently drawn from a known probability distribution, and determines the parameter value by maximizing the likelihood function. The likelihood function is a function of the parameter, which represents the likelihood of the observed data occurring. The Maximum Likelihood Estimator seeks the parameter value that maximizes the likelihood function, which is the estimated target state or parameter. The Maximum Likelihood Estimator has a wide range of applications in target positioning. For example, in wireless sensor network positioning, it is used to estimate the position of the target. In machine learning, the Maximum Likelihood Estimator is often used to estimate the parameters of a model, such as the regression coefficients in linear regression. In radar positioning, it is used to estimate the distance and direction of the target. The Maximum Likelihood Estimator is mathematically rigorous and has a good theoretical foundation; in the case of large samples, the estimation results of the Maximum Likelihood Estimator usually have asymptotic normality and effectiveness; the Maximum Likelihood Estimator can be well applied to various types of probability models and distributions. In summary, the Maximum Likelihood Estimator is a powerful method for target positioning estimation, which estimates the parameter value or target state by maximizing the likelihood function, and is widely used in various fields, including statistics, machine learning, and sensor networks.

[0005] In recent years, scholars have begun to focus on the problem of simultaneous localization of sensors and targets, and have solved various positioning problems using different types of sensors through the use of different estimation methods. Recent research has solved the problem of simultaneous localization of sensors and targets through the use of Direction of Arrival (AOA) sensors. However, in general, TOA measurements can provide better accuracy than AOA measurements, but the problem of simultaneous localization of sensors and targets based on TOA measurements has not been studied. In addition, existing research has shown that TOA measurements from different observation locations can have different effects on the final estimation performance. Therefore, it is important to optimize the distribution of sensors to obtain more effective measurements. For example, the trace of the inverse of the Fisher Information Matrix (FIM) is commonly used as a cost function to guide mobile sensors. This approach is also known as the A-optimal criterion.

[0006] In summary, the prior art currently only considers the problem of simultaneous localization of AOA sensors and targets, as well as the problem of relative positioning of TOA sensors, and further research is needed on the scheme of simultaneous localization of sensors and targets based on TOA. SUMMARY

[0007] The purpose of the present application is to overcome the above-mentioned defects of the prior art and provide a method for simultaneous localization and estimation of unmanned aerial vehicles and targets based on TOA sensors. The method comprises the following steps:

[0008] Acquire arrival time measurement information, which includes a first measurement time of the sensor relative to the target, a second measurement information of the sensor relative to the anchor point, and a third measurement information of mutual observation between the sensors;

[0009] Based on the arrival time measurement information, a maximum likelihood estimation problem is constructed for the sensor and the target;

[0010] For the maximum likelihood estimation problem, an iterative search algorithm is used to locate the sensor position and the target position.

[0011] Compared with existing technologies, the advantages of this invention are that, to solve the problem of simultaneous localization of sensors and targets based on Time of Arrival (TOA) in 3D information interference environments, it provides a sensor 3D target localization method based on TOA. This method can locate the sensor's own position and the absolute position information of the target even when the sensor's own position is unknown due to external interference or other reasons. This invention can be used in positioning systems for unmanned aerial vehicles, unmanned vehicles, and other unmanned systems, as well as fixed indoor positioning devices, improving positioning accuracy and solving the positioning problem in information-denied environments.

[0012] Other features and advantages of the invention will become clear from the following detailed description of exemplary embodiments of the invention with reference to the accompanying drawings. Attached Figure Description

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

[0014] Figure 1 This is a flowchart of a method for simultaneous localization estimation of a UAV and a target based on a TOA sensor according to an embodiment of the present invention;

[0015] Figure 2 This is a geometric schematic diagram of simultaneous positioning of a sensor and a target according to an embodiment of the present invention. Detailed Implementation

[0016] 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, numerical expressions, and values ​​of the components and steps set forth in these embodiments do not limit the scope of the invention.

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

[0018] Techniques, methods, and equipment known to those skilled in the art may not be discussed in detail, but where appropriate, such techniques, methods, and equipment should be considered part of the specification.

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

[0020] It should be noted that similar labels and letters in the following figures indicate similar items; therefore, once an item is defined in one figure, it does not need to be discussed further in subsequent figures.

[0021] In summary, this invention explores the limitations of the TOA (Transit-of-Area) localization problem from the perspectives of equation solution properties and geometric properties, such as the intrinsic fuzziness of TOA localization and the minimum conditions required for TOA localization due to this fuzziness. Furthermore, a maximum likelihood estimator is used to design a corresponding localization estimation algorithm. This invention enables simultaneous localization of the sensor itself and the target in 3D application scenarios using a set of integrated transceiver TOA sensors that do not require synchronization.

[0022] See Figure 1 As shown, the provided method for simultaneous localization estimation of UAVs and targets based on TOA sensors includes the following steps:

[0023] Step S1: Obtain the arrival time measurement information of the sensor relative to the target and relative to the anchor point, as well as the arrival time measurement information observed between the sensors.

[0024] For example, using K transceiver sensors r whose locations are unknown. k ∈R d To locate a static target p∈R d There are M anchor points b in the scene whose positions are known. m ∈R d Used for assisted positioning. Unless otherwise specified, the subscript ranges for sensors and anchors are k = 1, 2, ..., K and m = 1, 2, ..., M, respectively. Neither the target nor the anchors emit signals. Each measurement from each sensor can obtain 1, M, and K-1 TOA measurements from the target, anchors, and other sensors, respectively. For simplicity, it is assumed that each sensor can accurately identify the source of the received TOA signal to correctly associate the TOA signal with the target, anchors, and sensors. TOA signals can be acquired at different times without synchronization. Figure 2 The diagram illustrates the principle of target localization. The measurement obtained by sensor k relative to the target, even with noise, can be represented as:

[0025]

[0026] The noisy measurement obtained relative to the anchor point m can be expressed as:

[0027]

[0028] The measurement with noise obtained relative to another sensor j can be expressed as:

[0029]

[0030] in This is the ideal distance after noise removal. They are independent Gaussian noises, and they satisfy... This holds true under normal circumstances. The power factor 2 originates from the round-trip propagation of the signal, r. k b represents the absolute position of the k-th sensor. m r represents the absolute position of the m-th anchor point. j This represents the absolute position of the j-th sensor. Let represent the variance of the measurement noise of the k-th sensor when measuring the target position. This represents the variance of the measurement noise of the k-th sensor when measuring the m-th anchor point. Let represent the variance of the measurement noise of the k-th sensor when measuring the position of the j-th sensor. This represents the true value of the measured distance between the k-th sensor and the target (without measurement noise). This indicates that sensor k obtains a noisy measurement relative to target p, with the unit being distance (meters / m). Note that when two sensors measure each other, the two measurements are identical and can be averaged to reduce error.

[0031] To address the ambiguity issues related to reflection, translation, and rotation, and to ensure that the positioning problem described in 3D Euclidean space has a definite solution, based on the isometry property in Euclidean space, at least three non-collinear known position anchors and three non-collinear unknown position sensors are needed to solve the 2D problem; while for the 3D problem, at least four non-coplanar known position anchors and four non-coplanar unknown position sensors are needed.

[0032] Step S2: Based on the obtained arrival time measurement information, construct the maximum likelihood estimation problem for sensors and targets with unknown locations.

[0033] Positioning based on circular TOA measurement is an estimation problem, and its accuracy is affected by the quality of the measurement, the distribution of noise, the number of sensors and anchor points, and the geometry of the positioning. Positioning is a difficult nonlinear problem to solve. In the scenario of this invention, the problem becomes even more complex because the sensor positions are also unknown.

[0034] In one embodiment, a maximum likelihood estimator (MLE) is used. The unknown parameters in the problem include r1, r2, ..., r... K p, and so on, are combined to form an unknown vector. Define a measurement function:

[0035] f(ξ)=[||r1-b1||,…,||r1-b M ||,…,||r k -b1||,…,||r k -b M ||,…,||r K -b1||,…,||r K

[0036] -b M ||,||r1-r2||,…,||r K-1 -r K ||,||r1-p||,…,||r K -P||] T

[0037] The measurement function has N = KM + K(K-1) / 2 + K elements, each of which represents the distance between the sensor and the object being measured. When the position variable ξ is the true position of the sensor and the target, each term of f(ξ) represents the true distance between the sensor and the object being measured. When the position variable ξ is the estimated position of the sensor and the target, each term of f(ξ) represents the distance between the sensor and the object being measured based on the estimated position.

[0038] The measurement noise follows a Gaussian distribution. The likelihood function for the TOA measurement is shown below:

[0039]

[0040] in:

[0041]

[0042] It is an N×1 measurement vector:

[0043]

[0044] It is the covariance matrix of the measurement noise, |·| represents the determinant of the matrix, and s represents a measurement vector that collects all the measurements in the problem, which can be better computed.

[0045] The maximum likelihood (ML) estimate of an unknown location, denoted as This is obtained by maximizing the log-likelihood function ln p(s|ξ) with respect to ξ, which is equivalent to:

[0046]

[0047] Here, J ML (ξ) is the ML cost function that is equal to:

[0048]

[0049] Step S3: For the constructed maximum likelihood estimation problem, use an iterative search algorithm to locate the sensor position and the target position.

[0050] Minimize J on ξ ML (ξ) is a nonlinear least squares problem without a closed-form solution. To obtain numerical results, iterative search algorithms can be used, such as Gauss-Newton, quasi-Newton, and Niall-Mead simplex algorithms. In one embodiment, the Gauss-Newton algorithm is chosen, and the equation for the iterative solution is:

[0051]

[0052] Where i = 0, 1, ..., represents the iteration number, and J(i) is... Let e(ξ) be the N×d(K+1) Jacobian matrix with respect to ξ, and let e(ξ) be the vector of measurement error.

[0053]

[0054] in, It is an M×d matrix, k=1,…,K. It is particularly important to note that a mutual observation term between sensors is intentionally included in the algorithm, corresponding to J. k In the expression Items that occur simultaneously with other sensors This represents the estimated position of the k-th sensor. This represents the estimated location of the target. The initial estimate for the Gauss-Newton iteration is randomly set within the region where the target and sensor are expected to be located. The initial value is solved multiple times, and the one with the minimum ML cost is selected as the final solution. For details of the proposed MLE algorithm, see Algorithm 1.

[0055]

[0056] In summary, existing studies on simultaneous localization of sensors and targets only address solutions based on AOA sensors, and existing sub-localization problems based on TOA sensors only consider relative position localization. This invention focuses on localization based on circular TOA measurements. By introducing markers in three-dimensional space to cooperate with the TOA sensor, it achieves both sensor self-localization and target localization, thus improving localization efficiency. Furthermore, this invention improves the maximum likelihood estimation method by introducing mutual sensor observation, thereby enhancing the localization estimation accuracy of the sensor-carrying UAV and the target. In addition, simulation experiments have verified that this invention can be effectively applied in various scenarios, such as UAV navigation and localization.

[0057] This invention can be a system, method, and / or computer program product. A computer program product may include a computer-readable storage medium having computer-readable program instructions loaded thereon for causing a processor to implement various aspects of the invention.

[0058] Computer-readable storage media can be tangible devices capable of holding and storing instructions for use by an instruction execution device. Computer-readable storage media can be, for example, but not limited to, electrical storage devices, magnetic storage devices, optical storage devices, electromagnetic storage devices, semiconductor storage devices, or any suitable combination thereof. More specific examples (a non-exhaustive list) of computer-readable storage media include: portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), static random access memory (SRAM), portable compact disc read-only memory (CD-ROM), digital multifunction disc (DVD), memory sticks, floppy disks, mechanical encoding devices, such as punch cards or recessed protrusions storing instructions thereon, and any suitable combination thereof. The computer-readable storage media used herein are not to be construed as transient signals themselves, such as radio waves or other freely propagating electromagnetic waves, electromagnetic waves propagating through waveguides or other transmission media (e.g., light pulses through fiber optic cables), or electrical signals transmitted through wires.

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

[0060] The computer program instructions used to perform the operations of this invention may 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, etc., and conventional procedural programming languages ​​such as "C" or similar languages. The computer-readable program instructions may be executed entirely on the user's computer, partially on the user's computer, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving a remote computer, the remote computer may 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 may be connected to an external computer (e.g., via the Internet using an Internet service provider). In some embodiments, electronic circuitry, such as programmable logic circuitry, field-programmable gate arrays (FPGAs), or programmable logic arrays (PLAs), is personalized by utilizing state information from the computer-readable program instructions. This electronic circuitry can execute the computer-readable program instructions to implement various aspects of the invention.

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

[0062] 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 apparatus to produce a machine such that, when executed by the processor of the computer or other programmable data processing apparatus, they create means for implementing the functions / actions specified in one or more blocks of the flowchart and / or block diagram. These computer-readable program instructions can also be stored in a computer-readable storage medium that causes a computer, programmable data processing apparatus, and / or other device to operate in a particular manner; thus, the computer-readable medium storing the instructions comprises an article of manufacture that includes instructions for implementing aspects of the functions / actions specified in one or more blocks of the flowchart and / or block diagram.

[0063] Computer-readable program instructions may also be loaded onto a computer, other programmable data processing apparatus, or other device to cause a series of operational steps to be 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 perform the functions / actions specified in one or more boxes of a flowchart and / or block diagram.

[0064] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of the present invention. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of an instruction containing one or more executable instructions for implementing a specified logical function. In some alternative implementations, the functions marked in the blocks may occur in a different order than those marked in the drawings. For example, two consecutive blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, can be implemented using a dedicated hardware-based system that performs the specified function or action, or using a combination of dedicated hardware and computer instructions. It will be known to those skilled in the art that implementation in hardware, implementation in software, and implementation using a combination of software and hardware are equivalent.

[0065] The various embodiments of the present invention have been described above. These descriptions are exemplary and not exhaustive, and are 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 chosen to best explain the principles, practical application, or technical improvements to the embodiments in the market, or to enable others skilled in the art to understand the embodiments disclosed herein. The scope of the invention is defined by the appended claims.

Claims

1. A method for simultaneous localization and mapping of a target by unmanned aerial vehicles based on TOA sensors, comprising the steps of: obtaining time of arrival measurements, the time of arrival measurements comprising first measurements of sensors relative to a target, second measurements of sensors relative to anchor points, and third measurements of sensors relative to each other; constructing a maximum likelihood estimation problem for the sensors and the target based on the time of arrival measurements; solving the maximum likelihood estimation problem for sensor positions and target positions using an iterative search algorithm.

2. The method of claim 1, wherein, The time of arrival measurements comprise: the first measurements of sensor k relative to the target with noise are denoted as: the second measurements of sensor k relative to anchor point m with noise are denoted as: the third measurements of sensor k relative to sensor j with noise are denoted as: wherein, is the ideal distance without noise, is an independent Gaussian noise satisfying r k represents the absolute position of the kth sensor, b m represents the absolute position of the mth anchor point, r j represents the absolute position of the jth sensor, represents the variance of the measurement noise of the kth sensor when measuring the target position, represents the variance of the measurement noise of the kth sensor when measuring the mth anchor point, represents the variance of the measurement noise of the kth sensor when measuring the jth sensor position, represents the true value of the measured distance between the kth sensor and the target, represents the measurement with noise obtained by the sensor k relative to the target p.

3. The method of claim 2, wherein, the maximum likelihood estimation problem is denoted as: where: e(ξ) = s - f(ξ) f(ξ) = [||r1 - b1||,..., ||r1 - b M ||,..., ||r k 1 - b1||,..., ||r k 1 - b M ||,..., ||r K 1 - b1||,..., ||r K 1 - b M ||, ||r1 - r2||,..., ||r K-1 1 - r K ||, ||r1 - p||,..., ||r K 1 - p||] T where J ML (ξ) is the maximum likelihood cost function, e represents the error between the true and estimated positions, p represents the absolute position of the target, ξ = [r1,..., r k ,p] T denotes all unknown positions in the localization problem, f(ξ) is a predefined measurement function, where each term represents the distance of a sensor to a measured object, denotes the distance measured by the kth sensor to the mth anchor point, denotes the distance measured by the kth sensor to the jth sensor, denotes the distance measured by the kth sensor to the target, k = 1, 2,..., K is the index of the sensor, K is the number of sensors, m = 1, 2,..., M is the index of the anchor point, M is the number of anchor points.

4. The method of claim 3, wherein, the maximum likelihood estimation problem is solved using a Gauss-Newton iterative search algorithm, the equation of the iterative solution is denoted as: where J(i) is The N x d(K + 1) Jacobian matrix with respect to ξ is denoted as: where i is the index of the iteration number, e(ξ) is the vector of measurement errors, is a M x d matrix, with denotes the estimated position of the target by the kth sensor, denotes the estimated position of the kth sensor, denotes the estimated position of the target.

5. The method of claim 4, wherein, the initial estimate of the Gauss-Newton iterative search algorithm is randomly set in the area where the target and sensors are expected to be located, the initial value is solved repeatedly for multiple times to obtain the final solution of the maximum likelihood estimation problem.

6. The method of claim 1, wherein, The iterative search algorithm comprises Gauss-Newton, quasi-Newton, and Nelder-Mead simplex algorithm.

7. The method of claim 1, wherein, The sensors are integrated transceivers.

8. The method of claim 1, wherein, The number of sensors is greater than or equal to 3, and the number of anchor points is greater than or equal to 3.

9. A computer readable storage medium having stored thereon a computer program, wherein, The computer program is executed by a processor to implement the steps of the method according to any one of claims 1 to 8.

10. A computer device comprising a memory and a processor, having stored on the memory a computer program capable of running on the processor, characterized in that, The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 8.

Images