UAV 3D Position Estimation Method Based on UAV Depth Estimation

Abstract

The present invention provides a method for estimating the three-dimensional position of a drone based on drone depth estimation, which relates to the technical field of drone positioning. The present invention installs a tunnel light at intervals in a mine tunnel, mounts a camera on a drone and sets a displacement sensor. During the flight of the drone, the camera is controlled to capture the tunnel light in real time to obtain image information, and the movement distance information of the drone is obtained through the displacement sensor. By establishing a world coordinate system, an aircraft coordinate system, a camera coordinate system and a tunnel light coordinate system, the translation vectors of the two images in the world coordinate system are derived from the front and back frames captured by the camera in combination with the movement distance information of the drone. The estimated depth of the tunnel light relative to the camera is obtained from the translation vector, thereby calculating the three-dimensional position of the drone, and realizing accurate positioning of the drone in the mine.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle (UAV) positioning, and in particular to a method for estimating the three-dimensional position of a UAV based on UAV depth estimation. Background Art

[0002] Currently, underground coal mine inspections still face numerous challenges. While many domestic coal mines still rely on traditional manual inspections, this approach is susceptible to human factors, such as inspectors' experience, skills, work attitude, and health, which can reduce inspection reliability. Furthermore, the underground coal mine environment is complex and dangerous, filled with hazardous gases, posing a serious threat to inspectors' safety. When emergencies occur underground, especially in remote or unmanned areas, manual inspections have relatively limited response and emergency response capabilities, and remote command and dispatch are also challenging. Therefore, to improve the reliability, efficiency, safety, and controllability of inspections, the use of drones for automated underground coal mine inspections is becoming a growing trend. Inspection drones not only perform inspections stably and continuously, but also operate in hazardous environments, reducing personnel safety risks. Through intelligent means, they improve inspection efficiency and accuracy. With continuous technological advancements and decreasing costs, drones are expected to be more widely used in the coal mining industry, providing strong support for safe production and efficient management.

[0003] Accurate positioning of drones underground in mines is essential for reliable inspections. Currently, drone positioning in mines faces several major challenges. First, satellite positioning signals are severely attenuated by the time they reach the mine floor, making them impractical for positioning drones underground. Second, the geological conditions underground in coal mines are complex and varied. Obstacles and rough tunnel walls severely impact signal transmission and reception, as well as reflection and scattering, creating a multipath effect for underground signals. Finally, the complex electromagnetic environment significantly interferes with signal propagation, leading to large errors or even failure in positioning accuracy, and high power consumption. Considering these challenges, ultra-wideband (UWB) technology has become the mainstream method for positioning in mines. UWB does not use traditional carrier waves to transmit data, but instead utilizes impulse pulses with widths reaching nanoseconds (ns) or even picoseconds (ps). In the frequency domain, UWB's bandwidth far exceeds that of typical narrowband and broadband technologies, enabling information transmission across a very wide frequency range. This enables high data rates and lower power spectral density in mines, as well as centimeter-level or even millimeter-level positioning accuracy. However, due to the high cost of underground wiring, current UWB coverage in mines is limited to establishing a one-dimensional linear map of the mine, lacking the ability to perform three-dimensional positioning of drones.

[0004] Based on this, the present invention is proposed. Summary of the Invention

[0005] The purpose of the present invention is to provide a method for estimating the three-dimensional position of a drone based on drone depth estimation.

[0006] To achieve the above object, the present invention provides the following technical solutions:

[0007] The method for estimating the three-dimensional position of a UAV based on UAV depth estimation includes the following steps:

[0008] (1) Install a tunnel light at intervals in the mine tunnel;

[0009] (2) A camera is mounted on the drone and a displacement sensor is set. During the flight of the drone, the camera is controlled to capture the lane lights in real time to obtain image information, and the movement distance information of the drone is obtained through the displacement sensor;

[0010] (3) Establish the world coordinate system, aircraft coordinate system, camera coordinate system and lane light coordinate system;

[0011] (4) Derivation of the translation vectors of the two images in the world coordinate system from the two frames taken by the camera and the moving distance information of the drone;

[0012] (5) The estimated depth of the roadway light relative to the camera is obtained from the translation vector, thereby calculating the three-dimensional position of the UAV.

[0013] The present invention provides a preferred solution, in which, in step 4, deduction and approximate calculation are performed in combination with the UAV mechanism model.

[0014] The present invention provides a preferred solution, in step 4, the estimated depth of the tunnel light relative to the camera is obtained from the translation vector based on the pinhole imaging model and triangulation method, thereby calculating the three-dimensional position of the drone.

[0015] Compared with the existing technology, the above technical solution has the following advantages:

[0016] The present invention can only establish a one-dimensional linear map of the mine through the coverage of UWB in the mine at the current stage, and lacks the ability to perform three-dimensional positioning of the UAV.

[0017] (1) The present invention installs a tunnel light at intervals in the mine tunnel, mounts a camera on a drone and sets a displacement sensor. During the flight of the drone, the camera is controlled to shoot the tunnel light in real time to obtain image information, and the movement distance information of the drone is obtained through the displacement sensor. By establishing a world coordinate system, an aircraft coordinate system, a camera coordinate system and a tunnel light coordinate system, the translation vectors of the two frames in the world coordinate system are derived from the front and back images taken by the camera and the movement distance information of the drone. The estimated depth of the tunnel light relative to the camera is obtained from the translation vector, and the three-dimensional position of the drone is calculated, thereby realizing accurate positioning of the drone in the mine.

[0018] (2) Since the essence of the depth estimation of the UAV is the position estimation of the UAV, it can be obtained by taking two frames of images before and after the camera and deriving the translation vector in the world coordinate system. The present invention approximately calculates the position of the UAV in the world coordinate system through the UAV mechanism model, and obtains an approximate real translation vector in the world coordinate system that is magnified at a real scale.

[0019] (3) Based on the pinhole imaging model and triangulation method, the present invention obtains the estimated depth of the tunnel light relative to the drone camera from the approximate true translation vector obtained from the camera, thereby calculating the three-dimensional position of the drone, and further realizing the precise positioning of the three-dimensional position of the drone in the mine.

[0020] (4) The present invention only needs to use a monocular camera instead of a binocular camera, an RGB-D depth camera or an optical flow sensor to perform three-dimensional modeling, and solves the problem that the above sensors are basically unusable in mines. In addition, the monocular camera used in the present invention has the advantages of light weight, fast calculation speed and low power compared with RGB-D cameras and optical flow sensors, and is suitable for being carried on drones in mines with load and power limitations. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are merely embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying any creative work.

[0022] Figure 1 A diagram describing a mine system provided for a specific embodiment of the present invention;

[0023] Figure 2 A schematic diagram of a lane map definition provided by a specific embodiment of the present invention;

[0024] Figure 3 The coordinate system definition of a UAV in a mine provided by a specific embodiment of the present invention;

[0025] Figure 4 A schematic diagram of camera depth estimation provided by a specific embodiment of the present invention.

[0026] Figure numerals: ① is the UWB base station, ② is the drone, ③ is the tunnel light, and ④ is the ground cabin. DETAILED DESCRIPTION

[0027] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0028] The method for estimating the three-dimensional position of a drone based on drone depth estimation in this embodiment includes the following steps:

[0029] Step 1: Install a tunnel light at every interval in the mine tunnel, and install a UWB base station at every end. The drone communicates with the UWB base station during flight and continuously updates the distance information, such as Figure 1 As shown in the figure, UWB can obtain the one-dimensional position information of the UAV in the tunnel through the base station deployed in the tunnel and the tag installed on the UAV body. The mine tunnel map is defined as follows: Figure 2 shown.

[0030] Step 2: Mount a camera and set a displacement sensor (in this embodiment, an IMU) on the drone, and set a lidar. The lidar is installed on the drone with a field of view toward the top of the tunnel. During the flight of the drone, the camera is controlled to capture the tunnel lights in real time to obtain image information, and the displacement sensor is used to obtain the movement distance information of the drone;

[0031] In this embodiment, the drone also obtains displacement information in the body coordinate system based on its own sensor, that is, the displacement sensor (in this embodiment, IMU is used), such as the depth distance z from the target object. e , and the distance y from the top of the roadway e Etc., so as to obtain the relative position relationship between the drone and the fixed object The drone first obtains its own displacement information through the IMU, and estimates the depth distance information from the mine lamp through the monocular camera. The y-axis coordinate y in the world coordinate system w Obtained by a lidar mounted on a drone with its field of view toward the top of the tunnel.

[0032] Step 3: Establish the world coordinate system, aircraft coordinate system, camera coordinate system and tunnel light coordinate system, that is, define the coordinate system of the UAV in the mine, such as Figure 3 As shown in the figure, in the world coordinate system, the flight direction toward the front of the tunnel is defined as the x-axis, and the direction from the drone to the top of the mine tunnel is defined as the y-axis. The relative positions of the aircraft coordinate system and the camera coordinate system are known, so the problem is transformed into the positional relationship between the camera coordinate system and the world coordinate system. Since both the x-axis and y-axis coordinates can be measured by sensors, the problem of drone positioning in mines can be simplified to estimating the depth axis coordinate.

[0033] Step 4: Derivation of the translation vectors of the two images in the world coordinate system from the two frames captured by the camera and the distance the drone has moved. This embodiment mainly uses the drone mechanism model for derivation and approximate calculation, specifically including the following steps:

[0034] The essence of the depth estimation of the UAV is the position estimation of the UAV, which is achieved by taking two frames of images C before and after the camera. i and C i-1 The derived translation vector f in the world coordinate system i i-1 This embodiment uses the random sample consensus (RANSAC) algorithm to remove incorrect matching points from a given feature point set and solve the camera's essential matrix E:

[0035]

[0036] in, and is the matching feature point and The homogeneous coordinates of . By the expression of the essential matrix It can be seen that the essential matrix can be decomposed to obtain f i i-1 and where f i i-1^ is f i i-1 The antisymmetric matrix of .

[0037] Considering that Equation (1) is valid when multiplied by any non-zero scale factor, and is an orthogonal matrix, we can know that:

[0038]

[0039] Therefore, it is impossible to obtain the accurate translation vector by decomposing E, and only the proportional relationship within the vector can be obtained. However, the method of this embodiment takes into account that the position of the drone in the world coordinate system can be approximately calculated by the drone mechanism model, as shown in the following formula:

[0040]

[0041] Where P = [x, y, z] T Denoted as the drone position, Θ = [φ,θ,ψ] T Expressed as the Euler angle of the drone, n i Expressed as the motor speed of the i-th propeller, c T represents the propeller's lift coefficient, and m represents the mass of the drone. The actual displacement of the drone can be calculated using the drone's mechanical model, allowing the measured displacement to be corrected based on the actual scale.

[0042] The relative position of the drone and the onboard camera is fixed, and the camera is defined to take two frames of image C i and C i-1 When the position coordinates of the roadway lights in the aircraft coordinate system are b f i =[ b x i b y i b z i ] T and b f i-1 =[ b x i-1 b y i-1 b z i-1 ] T , then the translation vector of the drone can be expressed as in Then take image C i and C i-1 The moving distance of the UAV obtained by using the incremental IMU during this period is expressed by the following formula:

[0043]

[0044] in, b a i-1 Represents the acceleration output value of the IMU at time i-1. Using this as a constraint, we can get the approximate true translation vector f in the world coordinate system that is magnified in real proportion. i i-1 :

[0045]

[0046] Step 5: Determine the estimated depth of the roadway light relative to the camera from the translation vector to calculate the drone's 3D position. This embodiment primarily relies on a pinhole imaging model and triangulation to determine the estimated depth of the roadway light relative to the camera from the translation vector. This embodiment utilizes the pinhole imaging method, which is simple to implement and offers fast imaging speed. It also utilizes triangulation, which is computationally simple, highly accurate, and works even in low-light conditions.

[0047] This embodiment is based on the pinhole imaging model and triangulation method to obtain the approximate real translation vector f from the camera i i-1 Get the estimated depth of the roadway light relative to the drone camera [ c z i , c z i-1 ] T , and thus calculate the three-dimensional position of the UAV f l c . And by the definition of homogeneous transformation matrix It can be seen that the three-dimensional position of the UAV f l c The coordinates of the geometric center point of the roadway light in the camera coordinate system of the i-th frame c P i =[ c x i , c y i , c z i ] T Equivalent, so the problem can be transformed into a solution c P i problem.

[0048] The pixel coordinate point p of the image taken by the camera is obtained through the pinhole imaging model i =[u i ,v i ] T and the three-dimensional coordinates in the camera coordinate system c P i The relationship expression is:

[0049]

[0050] Where K represents the intrinsic parameter matrix of the camera, which can be obtained by Zhang Zhengyou calibration method, l u , l v They represent the focal length of the camera along the U and V axes respectively, and [u0,v0] T It represents the coordinate information of the intersection of the axis and the image plane in the pixel coordinate system. i Obtained by the YOLO-v5 target detection algorithm.

[0051] The essence of triangulation is to calculate the position coordinates of the same spatial point at different times. Therefore, according to formula (6), the coordinates of point P in the camera coordinate system of the i-th frame are c P i =[ c x i , c y i , c z i ] T ,have c P i = c z i K - 1 p i , the coordinates of point P in the camera coordinate system of the i-1th frame are c P i-1 =[ c x i-1 , c y i-1 , c z i-1 ] T ,have c P i-1 = c z i-1 K -1 p i-1 The coordinate change relationship between the camera's front and back frames is: in and f i i-1 is the external parameter of the second camera in the camera coordinate system of the first camera, so:

[0052]

[0053] Therefore:

[0054]

[0055] Finally, the depth information is obtained by the least square method [ c z i , c z i-1 ] T :

[0056]

[0057] in,

[0058] Thus, the other two-dimensional coordinate information in the world coordinate system can be obtained:

[0059]

[0060] From the above formula, we can know that the coordinates of the geometric center point of the lane light in the camera coordinate system of the i-th frame are c P i =[ c x i , c y i , c z i ] T , by knowing the coordinates of the traffic lights and the width and height of the lane, the three-dimensional position information of the drone can be estimated This allows the drone to dock accurately in the logistics warehouse in the tunnel.

[0061] Each embodiment in this specification focuses on the differences from other embodiments, and the same or similar parts between the embodiments can be referenced to each other.

[0062] The above is a detailed introduction to the drone three-dimensional position estimation method based on drone depth estimation provided by this application. This article uses specific examples to illustrate the principles and implementation methods of this application. The description of the above embodiments is only used to help understand the method of this application and its core idea. It should be pointed out that for ordinary technicians in this technical field, without departing from the principles of this application, several improvements and modifications can be made to this application, and these improvements and modifications also fall within the scope of protection of the claims of this application.

Claims

1. A method for estimating the three-dimensional position of a UAV based on UAV depth estimation, characterized in that: The steps include: Step 1: Install a tunnel light at a certain distance in the mine tunnel; Step 2: Mount a camera on the drone and set a displacement sensor. During the flight, the camera is controlled to capture the lane lights in real time to obtain image information, and the displacement sensor is used to obtain the movement distance of the drone. Step 3: Establish the world coordinate system, aircraft coordinate system, camera coordinate system and lane light coordinate system; Step 4: Derived the translation vectors of the two images in the world coordinate system from the two frames captured by the camera and the distance the drone moved. Step 5: Obtain the estimated depth of the roadway light relative to the camera from the translation vector to calculate the three-dimensional position of the drone; In step 5, the estimated depth of the roadway light relative to the camera is obtained from the translation vector based on the pinhole imaging model and triangulation method, thereby calculating the three-dimensional position of the drone; In step 5, the estimated depth of the roadway light relative to the camera is obtained from the translation vector based on the pinhole imaging model and triangulation method, thereby calculating the three-dimensional position of the drone. Specifically, the following is the definition of the homogeneous transformation matrix: It can be seen that the three-dimensional position of the drone The coordinates of the geometric center point of the roadway light in the camera coordinate system of the i-th frame Equivalence, transforming the problem into a solution Problems; Get the pixel coordinates of the camera image through the pinhole imaging model The relationship between the three-dimensional coordinates and the camera coordinate system is expressed as follows: (6) in, represents the intrinsic parameter matrix of the camera, , Respectively represent the camera focal length along the U and V axes, and It represents the coordinate information of the intersection of the axis and the image plane in the pixel coordinate system; Based on the depth information, we can further obtain the other two-dimensional coordinate information in the world coordinate system: (10) From the above formula, we can know that the coordinates of the geometric center point of the lane light in the camera coordinate system of the i-th frame are , the three-dimensional position information of the UAV can be estimated by knowing the coordinates of the lane lights and the lane width and height information .

2. The method for estimating the three-dimensional position of a drone based on drone depth estimation according to claim 1, wherein: In the step 4, deduction and approximate calculation are performed in combination with the UAV mechanism model.

3. The method for estimating the three-dimensional position of a drone based on drone depth estimation according to claim 2, wherein: In step 4, the derivation and calculation are carried out in combination with the UAV mechanism model, including the following: The relative position of the drone and the camera is fixed, and the camera is defined to capture two frames of images. and When the position coordinates of the roadway lights in the aircraft coordinate system are and , then the translation vector of the UAV is expressed as ,in , , ; Capture images and The moving distance of the UAV obtained by using the incremental IMU during this period is expressed by the following formula: (4) Using this as a constraint, we get the approximate true translation vector in the world coordinate system that is magnified at the true scale. : (5)。 4. The method for estimating the three-dimensional position of a drone based on drone depth estimation according to claim 1, wherein: described Obtained by Zhang Zhengyou calibration method.

5. The method for estimating the three-dimensional position of a drone based on drone depth estimation according to claim 1, wherein: described Obtained by the YOLO-v5 target detection algorithm.

6. The method for estimating the three-dimensional position of a drone based on drone depth estimation according to claim 1, wherein: The essence of the triangulation method is to calculate the position coordinates of the same spatial point at different times. Therefore, according to formula (6), the coordinates of point P in the camera coordinate system of the i-th frame are ,have , the coordinates of point P in the camera coordinate system of the i-1th frame are ,have , and the coordinate change relationship between the two frames before and after the camera is ,in and is the external parameter of the second camera in the camera coordinate system of the first camera, so: (7) Therefore: (8) Finally, the depth information is obtained by the least squares method : (9) in, .

Images