Joint self-localization and attitude estimation method, storage medium and device based on UAV

Abstract

This invention discloses a joint self-localization and attitude estimation method, storage medium, and device based on an unmanned aerial vehicle (UAV). The joint self-localization and attitude estimation method includes: a UAV equipped with an array of sensors receiving signals from K anchor node radiation sources with known positions, and estimating the azimuth angles of the K anchor node radiation sources; based on the azimuth angle estimates and the position coordinates of each anchor node radiation source, constructing an optimization function with UAV position and attitude as variables using the least squares criterion; eliminating the UAV position variable in the optimization function using inequality relations to construct an optimization function with attitude as the only variable; and obtaining the optimal UAV attitude and its own position by using a one-dimensional search of the optimization function with attitude as the variable. This invention achieves high-precision attitude angle determination without additional payload, simultaneously realizing self-localization and attitude angle determination, fitting navigation denial scenarios, with low operating costs, and significant engineering application value.

Description

Technical Field

[0001] This invention belongs to the field of passive positioning technology, specifically relating to a method, storage medium, and device for joint self-localization and attitude estimation based on unmanned aerial vehicles (UAVs). Background Art

[0002] With the development of navigation satellite technology, the Global Positioning System (GPS) and the BeiDou Navigation Satellite System have become indispensable parts of daily life and engineering, providing inexpensive and high-precision positioning and navigation for hundreds of millions of people. However, satellite signals may not be able to penetrate obstacles in harsh environments (such as urban canyons, tree canopies, and overpasses), thus providing navigation information with large errors or even erroneous results. Furthermore, in environments with significant electromagnetic interference, satellite signals may be unable to provide positioning due to interference, leading to satellite navigation rejection.

[0003] Most current azimuth-based positioning technologies do not consider sensor attitude issues in engineering environments. In fact, the attitude of a UAV is not static but can change with variations in the sampling observation points. Under additional loads, this attitude angle can be relatively easily obtained through physical means. However, due to limitations in the theory and technology of high-energy-density batteries, existing payload UAVs are limited by their battery capacity and power, lacking high payload capacity. Excessive loads would shorten flight time and reduce the UAV's flexibility. Furthermore, most existing azimuth-based positioning technologies are within the framework of passive positioning and have not been extended to self-localization. Determining both position and attitude simultaneously using anchor node radiation sources at known locations remains an unconsidered engineering problem when navigation is denied. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this invention provides a joint self-localization and attitude estimation method, storage medium, and device based on unmanned aerial vehicles (UAVs). This joint self-localization and attitude estimation method solves navigation denial by using anchor nodes to achieve high-precision self-localization and attitude estimation. The final result can be obtained with only one-dimensional search, which has low complexity and significant engineering application value.

[0005] To achieve the above objectives, the present invention adopts the following technical solution: a joint self-localization and attitude estimation method based on unmanned aerial vehicles (UAVs), specifically including the following steps:

[0006] Step S1: The UAV equipped with array sensors receives signals from K anchor node radiation sources with known locations and estimates the azimuth angles of the K anchor node radiation sources.

[0007] Step S2: Based on the azimuth estimate and the position coordinates of the radiation sources at each anchor node, construct an optimization function with the UAV position and attitude as variables using the least squares criterion;

[0008] Step S3: Eliminate the UAV position variable in the optimization function using inequality relations, and construct an optimization function with attitude as the only variable;

[0009] Step S4: Use a one-dimensional search to find the optimal UAV attitude and the UAV's own position using an optimization function with attitude as the variable.

[0010] Furthermore, in step S1, the signal received by the UAV from the radiation source of the anchor node with a known location is as follows:

[0011] x(t)=a(θ k ,ζ)s k (t)+n(t)

[0012] Among them, s k (t) represents the signal radiated by the k-th anchor node radiation source and reaches the UAV after propagation, where n(t) represents zero-mean complex Gaussian noise, and a(θ) k ,ζ) denotes a linear manifold, j represents the imaginary unit, θ k This represents the azimuth angle of the radiation source at the k-th anchor node. [u kx ,u ky ] represents the position coordinates of the radiation source at the k-th anchor node, p = [p x ,p y ] represents the current actual position coordinates of the UAV, ζ represents the UAV attitude, λ is the signal wavelength, d is the element spacing, and M represents the number of elements.

[0013] Furthermore, the process of estimating the azimuth angles of the radiation sources at the K anchor nodes in step S1 is as follows:

[0014] Step S1.1: Calculate the covariance matrix of the signal received by the UAV from the anchor node radiation source with a known location. in, H represents the sign of the mean, and H represents the conjugate transpose.

[0015] Step S1.2: Adjust the covariance matrix. Perform eigenvalue decomposition. Where Λ represents the eigenvalue matrix, Λ=diag{λ1,...,λ M}, the diagonal elements of Λ are λ1,...,λ M Covariance matrix eigenvalues, λ1≥...≥λ M ; Represents the characteristic matrix, e1,...,e M For λ1,...,λ MOne-to-one correspondence of eigenvectors;

[0016] Step S1.3: Take the eigenvectors corresponding to the first K eigenvalues ​​to form the signal subspace E. s =[e1,…,e K ], take the signal subspace E s The first M-1 rows and the last M-1 rows respectively form matrix E x and E y ;

[0017] Step S1.4: For the matrix Eigenvalue decomposition yields the eigenmatrix E, which is then decomposed into K×K submatrices:

[0018]

[0019] Step S1.5: Calculate the auxiliary matrix And find the eigenvalues ​​of the auxiliary matrix. Obtain the estimated azimuth angle value

[0020] Further, step S2 includes the following sub-steps:

[0021] Step S2.1: Based on the azimuth angle under ideal conditions Based on the definition of the tangent function, the estimated azimuth angle has the following approximate relationship with the UAV's position:

[0022]

[0023] in, Let ζ represent the estimated azimuth angle, ζ represent the UAV attitude, and p = [p x ,p y [u] represents the current actual position coordinates of the drone. kx ,u ky [] represents the position coordinates of the radiation source at the k-th anchor node;

[0024] Step S2.2: Based on the approximate relationship between the estimated azimuth angle and the UAV position, construct K equations to obtain Ap≈b.

[0025] Where A is a K×2 matrix, b is a K×1 matrix, represented by a stack of all constant terms from the K equations.

[0026] Step 2.3: Construct an optimization function with the UAV's position and attitude as variables using the least squares criterion.

[0027] Where ||| represents the Euclidean norm.

[0028] Furthermore, the specific process of step 3 is as follows: based on the inequality relationship ||Ap-b|| 2 ≥||A(A T A) -1 A T bb|| 2 Eliminating the UAV position variable from the optimization function yields an optimization function with state as the variable:

[0029]

[0030] Furthermore, the optimal UAV attitude in step 4 Optimal UAV self-position

[0031] Furthermore, the present invention also provides a computer-readable storage medium storing a computer program that causes a computer to execute the aforementioned UAV-based joint self-localization and attitude estimation method.

[0032] Furthermore, the present invention also provides an electronic device, including: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, it implements the aforementioned UAV-based joint self-localization and attitude estimation method.

[0033] Compared with existing technologies, the present invention has the following advantages: The joint self-localization and attitude estimation method of the present invention considers the position and attitude variables of the UAV simultaneously, thus enabling simultaneous estimation of its own position and attitude. Compared with traditional methods, it can additionally obtain its own attitude information, which is more in line with actual engineering scenarios. At the same time, the present invention can perform simultaneous position and attitude estimation using a single UAV equipped with an array sensor, reducing the load on the UAV and establishing the UAV's own attitude without additional physical peripherals. In addition, the present invention uses one-dimensional search to obtain the optimal attitude and the UAV's own position, which greatly reduces the difficulty of UAV position and attitude estimation. Attached Figure Description

[0034] Figure 1 This is a flowchart of the joint self-localization and attitude estimation method based on UAVs according to the present invention;

[0035] Figure 2 This is a diagram of the positioning scene described in this invention;

[0036] Figure 3 This is a diagram showing the self-localization performance of the UAV-based joint self-localization and attitude estimation method of the present invention as a function of azimuth estimation error;

[0037] Figure 4This is a graph showing the attitude estimation performance of the UAV-based joint self-localization and attitude estimation method of this invention as the azimuth estimation error changes. Detailed Implementation

[0038] The technical solution of the present invention will be further explained and described below with reference to the accompanying drawings.

[0039] like Figure 1 This is a flowchart of the joint self-localization and attitude estimation method based on UAVs according to the present invention. The joint self-localization and attitude estimation method specifically includes the following steps:

[0040] Step S1: The UAV equipped with array sensors receives signals from K anchor node radiation sources with known positions and estimates the azimuth angles of the K anchor node radiation sources to provide prior information for the subsequent covariance matrix. The estimation is performed using a method with lower azimuth angle estimation complexity, which greatly reduces the computational complexity.

[0041] like Figure 2 Based on the positioning scenario, in this invention, the signal received by the UAV from the radiation source of the anchor node with a known location is as follows:

[0042] x(t)=a(θ k ,ζ)s k (t)+n(t)

[0043] Among them, s k (t) represents the signal radiated by the k-th anchor node radiation source and reaches the UAV after propagation, where n(t) represents zero-mean complex Gaussian noise, and a(θ) k ,ζ) denotes a linear manifold, j represents the imaginary unit, θ k This represents the azimuth angle of the radiation source at the k-th anchor node. [u kx ,u ky ] represents the position coordinates of the radiation source at the k-th anchor node, p = [p x ,p y ] represents the current actual position coordinates of the UAV, ζ represents the UAV attitude, λ is the signal wavelength, d is the element spacing, and M represents the number of elements.

[0044] The process of estimating the azimuth angle of the radiation sources at the K anchor nodes in step S1 is as follows:

[0045] Step S1.1: Calculate the covariance matrix of the signal received by the UAV from the anchor node radiation source with a known location. in, H represents the sign of the mean, and H represents the conjugate transpose.

[0046] Step S1.2: Adjust the covariance matrix. Perform eigenvalue decomposition. Where Λ represents the eigenvalue matrix, Λ=diag{λ1,...,λ M}, the diagonal elements of Λ are λ1,...,λ M Covariance matrix eigenvalues, λ1≥...≥ M ; Represents the characteristic matrix, e1,...,e M For λ1,...,λ M One-to-one correspondence of eigenvectors;

[0047] Step S1.3: Take the eigenvectors corresponding to the first K eigenvalues ​​to form the signal subspace E. s =[e1,...,e K ], take the signal subspace E s The first M-1 rows and the last M-1 rows respectively form matrix E x and E y ;

[0048] Step S1.4: For the matrix Eigenvalue decomposition yields the eigenmatrix E, which is then decomposed into K×K submatrices:

[0049]

[0050] Step S1.5: Calculate the auxiliary matrix And find the eigenvalues ​​of the auxiliary matrix. Obtain the estimated azimuth angle value

[0051] Step S2: Based on the azimuth estimate and the position coordinates of the radiation sources at each anchor node, construct an optimization function with the UAV's position and attitude as variables using the least squares criterion. The least squares criterion has quasi-linear characteristics, and applying this criterion can reduce the difficulty of the solution; specifically, it includes the following sub-steps:

[0052] Step S2.1: Based on the azimuth angle under ideal conditions Based on the definition of the tangent function, the estimated azimuth angle has the following approximate relationship with the UAV's position:

[0053]

[0054] in, Let ζ represent the estimated azimuth angle, ζ represent the UAV attitude, and p = [p x ,p y [u] represents the current actual position coordinates of the drone. kx ,u ky [] represents the position coordinates of the radiation source at the k-th anchor node;

[0055] Step S2.2: Based on the approximate relationship between the estimated azimuth angle and the UAV position, construct K equations to obtain Ap≈b.

[0056] Where A is a K×2 matrix, b is a K×1 matrix, represented by a stack of all constant terms from the K equations.

[0057] Step 2.3: Construct an optimization function with the UAV's position and attitude as variables using the least squares criterion.

[0058] Where |||| represents the Euclidean norm.

[0059] Step S3: Eliminate the UAV position variable in the optimization function using inequality relations, and construct an optimization function with attitude as the only variable, thus reducing the dimensionality of the optimization process; specifically, first optimize the UAV position variable p, since the least squares solution is... Therefore, for any UAV attitude ζ, to minimize the optimization function, it is only necessary to let p = (A T A) -1 A T Therefore, the following inequality relationship is derived: b. Based on the inequality ||Ap-b|| 2 ≥||A(A T A) -1 A T bb|| 2 Eliminating the UAV position variable from the optimization function yields an optimization function with state as the variable:

[0060] Step S4: Utilize a one-dimensional search to find the optimal UAV attitude and its own position using an optimization function with attitude as the variable, thereby reducing the solution complexity; specifically, based on inequality relations... The optimization function with UAV position and attitude as variables is transformed into a one-dimensional optimization problem, and its optimal solution is obtained through one-dimensional search. Note that... Where n is a positive integer, It is a periodic function with a period of π. The search only needs to be performed in the interval [0,π]. For each search point, the optimal function value is calculated, and the search point that minimizes the optimal function value is taken as the optimal pose. Optimal UAV self-position

[0061] The joint self-localization and attitude estimation method based on UAVs of this invention was simulated. The simulation parameters were set as follows: the positions of the three anchor radiation sources were

[1050] . T

[3020] T , and

[400] T The location coordinates of the drone are

[00] . T The unit is meters (m); the attitude angle of the UAV is 40°.

[0062] Figure 3 The diagram shows the self-localization performance of the UAV-based joint self-localization and attitude estimation method of this invention as the azimuth estimation error changes. It can be seen that as the azimuth estimation error increases, the self-localization error of this invention also increases, but it always has a low self-localization error and has the best self-localization performance. Figure 4 The graph shows the attitude estimation performance of the UAV-based joint self-localization and attitude estimation method of this invention as the azimuth estimation error changes. It can be seen that as the azimuth estimation error increases, the attitude estimation error of this invention also increases, but it always has a low attitude estimation error and has the best attitude estimation performance.

[0063] In one embodiment of the present invention, a computer-readable storage medium is also provided, storing a computer program that enables a computer to execute the aforementioned UAV-based joint self-localization and attitude estimation method.

[0064] In another technical solution of the present invention, an electronic device is also provided, including: a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the aforementioned UAV-based joint self-localization and attitude estimation method.

[0065] In the embodiments disclosed in this application, a computer storage medium may be a tangible medium that may contain or store programs for use by or in conjunction with an instruction execution system, apparatus, or device. The computer storage medium may include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination of the foregoing. More specific examples of computer storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.

[0066] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed in this application can be implemented in electronic hardware or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0067] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any transformations or substitutions that can be conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A joint self-localization and attitude estimation method based on unmanned aerial vehicles (UAVs), characterized in that, The specific steps include: Step S1: The UAV equipped with array sensors receives signals from K anchor node radiation sources with known locations and estimates the azimuth angles of the K anchor node radiation sources. Step S2: Based on the azimuth estimate and the position coordinates of the radiation sources at each anchor node, construct an optimization function with the UAV's position and attitude as variables using the least squares criterion; including the following sub-steps: Step S2.1: Based on the azimuth angle under ideal conditions Based on the definition of the tangent function, the estimated azimuth angle has the following approximate relationship with the UAV's position: in, Let ζ represent the estimated azimuth angle, ζ represent the UAV attitude, and p = [p x ,p y [u] represents the current actual position coordinates of the drone. kx ,u ky [] represents the position coordinates of the radiation source at the k-th anchor node; Step S2.2: Based on the approximate relationship between the estimated azimuth angle and the UAV position, construct K equations to obtain Ap≈b. Where A is a K×2 matrix, b is a K×1 matrix, represented by a stack of all constant terms from the K equations. Step 2.3: Construct an optimization function with the UAV's position and attitude as variables using the least squares criterion. Where || represents the Euclidean norm; Step S3: Eliminate the UAV position variable in the optimization function using inequality relations, and construct an optimization function with attitude as the only variable; Step S4: Use a one-dimensional search to find the optimal UAV attitude and the UAV's own position using an optimization function with attitude as the variable.

2. The method for joint self-localization and attitude estimation based on unmanned aerial vehicles according to claim 1, characterized in that, In step S1, the signal received by the UAV from the known location of the anchor node radiation source is as follows: x(t)=a(θ k ,g)s k (t)+n(t) Among them, s k (t) represents the signal radiated by the k-th anchor node radiation source and reaches the UAV after propagation, where n(t) represents zero-mean complex Gaussian noise, and a(θ) k ,ζ) denotes a linear manifold, j represents the imaginary unit, θ k This represents the azimuth angle of the radiation source at the k-th anchor node. [u kx ,u ky ] represents the position coordinates of the radiation source at the k-th anchor node, p = [p x ,p y ] represents the current actual position coordinates of the UAV, ζ represents the UAV attitude, λ is the signal wavelength, d is the element spacing, and M represents the number of elements.

3. The method for joint self-localization and attitude estimation based on unmanned aerial vehicles according to claim 2, characterized in that, The process of estimating the azimuth angle of the radiation sources at the K anchor nodes in step S1 is as follows: Step S1.1: Calculate the covariance matrix of the signal received by the UAV from the anchor node radiation source with a known location. Where E represents the mean sign and H represents the conjugate transpose; Step S1.2: Adjust the covariance matrix. Perform eigenvalue decomposition. Where Λ represents the eigenvalue matrix, Λ=diag{λ1,…,λ M }, the diagonal elements of Λ are λ1,…,λ M Covariance matrix eigenvalues, λ1≥…≥λ M ; Represents the characteristic matrix, e1,…,e M For λ1,…,λ M One-to-one correspondence of eigenvectors; Step S1.3: Take the eigenvectors corresponding to the first K eigenvalues ​​to form the signal subspace E. s =[e1,…,e K ], take the signal subspace E s The first M-1 rows and the last M-1 rows respectively form matrix E x and E y ; Step S1.4: For the matrix Eigenvalue decomposition yields the eigenmatrix E, which is then decomposed into K×K submatrices: Step S1.5: Calculate the auxiliary matrix And find the eigenvalues ​​λ of the auxiliary matrix. k The estimated azimuth angle is obtained.

4. The method for joint self-localization and attitude estimation based on unmanned aerial vehicles according to claim 1, characterized in that, The specific process of step 3 is as follows: Based on the inequality relationship ||Ap-b|| 2 ≥||A(A T A) -1 A T bb|| 2 Eliminating the UAV position variable from the optimization function yields an optimization function with state as the variable: P(ζ)=b T b-b T A(A T A) -1 A T b。 5. The method for joint self-localization and attitude estimation based on unmanned aerial vehicles according to claim 4, characterized in that, Optimal UAV attitude in step 4 Optimal UAV self-position 6. A computer-readable storage medium storing a computer program, characterized in that, The computer program causes the computer to execute the UAV-based joint self-localization and attitude estimation method as described in any one of claims 1-5.

7. An electronic device, characterized in that, include: The memory, the processor, and the computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, it implements the UAV-based joint self-localization and attitude estimation method as described in any one of claims 1-5.

Images