A joint positioning and rotation estimation method for UAV based on LED and PD array

Abstract

The present invention discloses a method for joint positioning and rotation estimation of unmanned aerial vehicles based on LED and PD (Photodiode) arrays. The method collects incident light signals from different LEDs by carrying multiple photodiodes with different normal vectors on the unmanned aerial vehicle, and obtains the coordinates and rotation information of the unmanned aerial vehicle using an eigenvalue decomposition algorithm based on the covariance matrix of the light direction vector. Traditional consumer-grade unmanned aerial vehicles generally use GPS and barometers for positioning, but in indoor scenes, satellite signals will be severely attenuated and distorted by the walls of buildings. Therefore, the present invention proposes that in indoor scenes, LED and PD arrays can be used to quickly locate unmanned aerial vehicles. Compared with existing unmanned aerial vehicle indoor positioning methods based on visual sensors, ultrasonic sensors, laser rangefinders, UWB positioning, etc., its advantages are: low algorithm complexity, low hardware cost, small size, low power consumption, high response rate, and high accuracy.

Description

Technical Field

[0001] The invention relates to the technical field of indoor wireless optical positioning, and in particular to an indoor unmanned aerial vehicle joint positioning and rotation estimation method based on LED and PD (Photodiode) array. Background Art

[0002] As modern advanced equipment, drones play an important role in the civil and military fields. The high-precision positioning of drones is the guarantee of their safe flight and subsequent autonomous mission execution. Traditional drones generally use GPS positioning solutions, but because satellite signals are severely attenuated and distorted by building walls, they cannot provide accurate positioning in indoor scenes.

[0003] Existing indoor drone positioning solutions mainly include UWB positioning and various sensors (such as ultrasound, lidar, cameras, etc.) on drones that can achieve positioning. Although these devices have high accuracy and can reach the centimeter level, they generally have problems such as high hardware cost, large size, and high power consumption. Indoor drones are generally small in size and have limited load capacity. The large device size and power consumption will pose a serious challenge to the flight and endurance of the drone. In addition, some solutions cannot obtain the rotation information of the drone and need to be assisted by other sensors (such as gyroscopes) to measure, which will introduce additional noise and hardware costs. Summary of the invention

[0004] Purpose of the invention: In order to overcome the shortcomings of the above-mentioned existing solutions, the present invention provides a method for joint positioning and rotation estimation of drones based on LED and PD arrays, which reduces hardware costs and power consumption while meeting the requirements of positioning accuracy, and can obtain the three-dimensional coordinates and rotation information of indoor drones without relying on auxiliary devices such as gyroscopes.

[0005] The method for joint positioning and rotation estimation of a UAV based on LED and PD arrays of the present invention comprises the following process:

[0006] 1. Deploy K LEDs at different locations and measure their three-dimensional coordinates, recorded as coordinate vector l k ,k=1,2,3,...,K。

[0007] 2. The drone is equipped with a receiver consisting of M photodiodes (PDs), whose center coordinates are marked as u.

[0008] 3. Establish a relative coordinate system with the center of the receiver as the origin. The normalized light direction vector of the kth LED in the absolute coordinate system and the relative coordinate system can be expressed as r k and r′ k :

[0009]

[0010]

[0011] where G is the 3×3 rotation matrix of the receiver relative to the absolute coordinate system.

[0012] 4. Based on the photocurrent measurement obtained by the receiver Calculate the normalized light direction vector of the kth LED in the relative coordinate system

[0013] 5. According to Estimate the receiver 3D coordinates u * , that is, solve the following optimization problem:

[0014]

[0015] stu∈Φ(u)

[0016] Where tr(X) is the trace of the matrix X, and abs(X) is the matrix of the absolute values ​​of each element of X, that is, {abs(X)} m,n =|{(X)} m,n |; is the light direction vector measured in 4, r k (u) will r k is expressed as a function of u; U(u) and are all 3×3 matrices, whose columns are [r1(u),...,r K (u)][r1(u),...,r K (u)] T and is the ordered normalized eigenvector of ; Φ(u) defines an optional region of interest. This optimization problem can be solved by gradient descent.

[0017] 6. Based on the intermediate result of 5, the estimated value of the rotation matrix G can be further obtained * :

[0018]

[0019] Where sign{X} is the matrix that takes the signs of the elements in X, and diag{X} is the diagonal matrix with all the diagonal elements in X.

[0020] Beneficial effects:

[0021] The hardware device used in the present invention is low-cost and does not rely on devices other than the photoelectric sensor array such as a gyroscope, and can simultaneously estimate the three-dimensional coordinates and rotation information of the drone. The computational complexity of the method increases only linearly with the number of LEDs, and can achieve centimeter-level positioning accuracy, and the rotation estimation is reliable. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 This is a schematic diagram of the system of the UAV joint positioning and rotation estimation method based on LED and PD array proposed by the present invention (taking a system using 4 LEDs and 4 PDs as an example);

[0023] Figure 2 A is a special case where the method fails;

[0024] Figure 3 B is the special case where the method fails;

[0025] Figure 4 Flowchart of the coordinate estimation algorithm. DETAILED DESCRIPTION

[0026] The present invention will be further described below in conjunction with the accompanying drawings.

[0027] 1. Deploy K LEDs at different locations, such as Figure 1 As shown, measure its three-dimensional coordinates and record them as coordinate vector l k ,k=1,2,3,...,K。

[0028] 2. The drone is equipped with a receiver consisting of M photodiodes (PDs), such as Figure 1 As shown, v′ m is the normal vector of the mth PD, and the receiver center coordinate is denoted by u. The photodiodes on the receiver should be close enough so that they share the normalized light direction vector about one LED.

[0029] 3. Establish a relative coordinate system with the center of the receiver as the origin. The normalized light direction vector of the kth LED in the absolute coordinate system and the relative coordinate system can be expressed as r k and r′ k :

[0030]

[0031]

[0032] Where G is the 3×3 rotation matrix of the receiver relative to the absolute coordinate system. The purpose of this method is to use the actual measurement Estimate u and G.

[0033] 4. When the drone is flying, the photocurrent measurement value obtained by the receiver Calculate the normalized light direction vector of the kth LED in the relative coordinate system The specific calculation method is as follows:

[0034]

[0035] Among them, V PD =[v′1,...,v′ M ] T It is an M×3 matrix consisting of PD normal vectors.

[0036] 5. According to Estimate the receiver 3D coordinates u * , that is, solve the following optimization problem:

[0037]

[0038] stu∈Φ(u)

[0039] Where tr(X) represents the trace of the matrix X, and abs(X) is the matrix that takes the absolute values ​​of each element of X, that is, {abs(X)} m,n =|{(X)} m,n |; is the light direction vector measured in 4, r k (u) will r k is expressed as a function of u; U(u) and are all 3×3 matrices, whose columns are [r1(u),...,r K (u)][r1(u),...,r K (u)] T and is the ordered normalized eigenvector of ; Φ(u) defines an optional region of interest.

[0040] This optimization problem can be solved by the gradient descent method. The algorithm flow chart is as follows Figure 4 As shown, the specific steps are: first initialize a smaller value ε and the maximum number of iterations N th , guess a position u * ; when g(u * )≤K-ε, using the guessed position as the starting point to continuously update u using the gradient descent method * , if the number of iterations exceeds N th Then exit. Finally, output u at this time * That is, the estimated position of the drone.

[0041] 6. According to the position estimation result of 5, calculate the corresponding rotation matrix G * :

[0042]

[0043] Where sign{X} is the matrix that takes the signs of the elements in X, and diag{X} is the diagonal matrix with all the diagonal elements in X.

[0044] 7. It should be noted that the method will fail when the LED deployment in 1 is in the following two situations:

[0045] 1) When the LED coordinates l k When collinear, the method will fail because there are infinitely many u * and G * Satisfy g(u * )=K. An example of this situation is Figure 2 As shown, both u1 and u2 can maximize the objective function.

[0046] 2) When have the same eigenvalues, the method fails because There are infinitely many solutions, and the objective function may not reach its maximum value. An example of this situation is Figure 3 As shown, the LEDs are arranged in an equally spaced ring, and the receiver located directly below the symmetric center of the LED will have a high positioning error.

[0047] Therefore, when initially deploying the LEDs, try to avoid having them be collinear or symmetrical.

Claims

1. A method for joint positioning and rotation estimation of unmanned aerial vehicles based on LED and PD array, characterized in that: The steps include: Step 1: deploy K LEDs at different locations and measure their three-dimensional coordinates, recorded as coordinate vector l k ,k=1,2,3,...,K; Step 2: The drone is equipped with a receiver consisting of M PDs, whose center coordinates are marked as u; Step 3: Establish a relative coordinate system with the center of the receiver as the origin. The normalized light direction vector of the kth LED in the absolute coordinate system and the relative coordinate system are expressed as r k and r k ′: Where G is the 3×3 rotation matrix of the receiver relative to the absolute coordinate system; Step 4: Based on the photocurrent measurement value obtained by the receiver Calculate the normalized light direction vector of the kth LED in the relative coordinate system Step 5, according to Estimate the receiver 3D coordinates u * , that is, solve the following optimization problem: stu∈Φ(u) Where tr(X) is the trace of the matrix X, and abs(X) is the matrix of the absolute values ​​of each element of X, that is, {abs(X)} m,n =|{(X)} m,n |; is the light direction vector measured in step 4, r k (u) will r k is expressed as a function of u; U(u) and are all 3×3 matrices, whose columns are [r1(u),...,r K (u)][r1(u),...,r K (u)] T and The ordered normalized eigenvector of ; Φ(u) defines an optional region of interest; Step 6: Based on the intermediate result of step 5, further obtain the estimated value G of the rotation matrix * : Where sign{X} is the matrix that takes the signs of the elements in X, and diag{X} is the diagonal matrix with all the diagonal elements in X.

2. The method for joint positioning and rotation estimation of unmanned aerial vehicles based on LED and PD array according to claim 1, characterized in that: The light source LEDs should be placed in an asymmetrical manner, avoiding their being collinear as much as possible.

3. The method for joint positioning and rotation estimation of unmanned aerial vehicles based on LED and PD array according to claim 1, characterized in that: The PDs onboard the drone should be close enough so that they share a normalized light direction vector about a certain LED.

4. The method for joint positioning and rotation estimation of unmanned aerial vehicles based on LED and PD array according to claim 1, characterized in that: The three-dimensional coordinates of the light source LED are known, and the photocurrent measurement value of the PD is known. The light direction vector is calculated through the photocurrent measurement value, and then the three-dimensional coordinates and rotation information of the drone are estimated.

5. The method for joint positioning and rotation estimation of unmanned aerial vehicles based on LED and PD array according to claim 1, characterized in that: In step 4, the normalized light direction vector in the relative coordinate system is calculated by the following formula: Among them, V PD =[v′1,...,v′ M ] T is an M×3 matrix, v′ m are the normal vectors of the mth PD, which are the intrinsic parameters of the receiver.

6. The method for joint positioning and rotation estimation of unmanned aerial vehicles based on LED and PD array according to claim 1, characterized in that: In step 5, the optimization problem is solved by gradient descent, and the objective function value is compared with its theoretical maximum to avoid local optimal values.

Images