Unmanned aerial vehicle three-dimensional position estimation method based on unmanned aerial vehicle depth estimation
By installing tunnel lights and drone camera sensors underground in coal mines, combining the world coordinate system and tunnel light coordinate system, deducing the translation vector to calculate the three-dimensional position of the drone, solving the problem of three-dimensional positioning of the drone underground in coal mines, and achieving accurate positioning and Reduces equipment costs and power consumption.
Patent Information
- Application Number
- CN202411055425.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-02
- Publication Date
- 2024-11-12
- Estimated Expiration
- 2044-08-02
AI Technical Summary
It is difficult to accurately position the drone underground in coal mines. The existing UWB technology can only realize one-dimensional linear maps, lack three-dimensional positioning capabilities, and traditional sensors are limited in use in mines.
Install tunnel lights in mine tunnels, and mount cameras and displacement sensors on the drone. Image information and displacement sensor data are captured through the camera, combined with the world coordinate system and the tunnel light coordinate system, and deduce the translation vector to calculate the three-dimensionality of the drone The position is accurately positioned using a monocular camera, pinhole imaging model and triangulation method.
It realizes accurate three-dimensional positioning of underground drones of coal mines, reduces power consumption and equipment costs, and is suitable for underground mine applications with load and power limitations.
Smart Images

Figure CN118941648A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of unmanned aerial vehicle positioning, and in particular to a method for estimating the three-dimensional position of an unmanned aerial vehicle based on unmanned aerial vehicle depth estimation. Background Art
[0002] At present, the inspection tasks in coal mines still face many challenges. Although many domestic coal mines still rely on traditional manual inspection methods, this method is easily affected by human factors, such as the experience, skills, work attitude and health status of the inspectors, which may reduce the reliability of the inspection. In addition, the underground environment of coal mines is complex and dangerous, filled with harmful gases, which poses a serious threat to the safety of inspectors. When an emergency occurs in a coal mine, especially in remote or unmanned areas, the response and emergency handling capabilities of manual inspections are relatively weak, and remote command and dispatch are also difficult. Therefore, from the perspective of improving the reliability, efficiency, safety and controllability of inspections, the use of drones for automatic inspections in coal mines is gradually becoming a development trend. Inspection drones can not only perform inspection tasks stably and continuously, but also work in dangerous environments, reduce personnel safety risks, and improve inspection efficiency and accuracy through intelligent means. With the continuous advancement of technology and the gradual reduction of costs, drones are expected to be more widely used in the coal mining industry, providing strong support for safe production and efficient management of coal mines.
[0003] The precise positioning of UAVs in mines is the basis for reliable inspections. At present, the positioning of UAVs in mines faces several major difficulties. First, since the satellite positioning signal has been severely attenuated when it passes through the coal mine layer and reaches the underground, satellite positioning cannot be applied to the positioning of UAVs in mines. Secondly, the geological conditions in coal mines are complex and changeable. Obstacles and rough tunnel walls will have a serious impact on the transmission and reception, reflection and scattering of signals, forming a multipath effect of underground signals. Finally, the complex electromagnetic environment will cause serious interference to the propagation of signals, resulting in large positioning accuracy errors or even failure, and high power consumption. Considering the above difficulties, ultra-wideband (UWB) technology has become the mainstream method for positioning in mines at this stage. UWB does not use traditional carriers to transmit data, but uses impulse pulses with a width of nanoseconds (ns) or even picoseconds (ps) to transmit information. In the frequency domain, the bandwidth of UWB far exceeds the general narrowband and broadband, which enables UWB to transmit information in a very wide frequency band, thereby achieving high data transmission rate and lower power spectrum density in mines, as well as centimeter-level or even millimeter-level positioning accuracy. However, due to the high cost of underground wiring in mines, the current coverage of UWB in mines can only establish a one-dimensional linear map of the mine, and lacks 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 the depth estimation of the UAV comprises the following steps:
[0008] (1) Install a tunnel light at a certain interval 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) Obtain the estimated depth of the roadway light relative to the camera 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 the step four, the estimated depth of the lane light relative to the camera is obtained from the translation vector based on the pinhole imaging model and triangulation method, so as to calculate the three-dimensional position of the drone.
[0015] Compared with the prior art, 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 a certain interval 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 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. In addition, a world coordinate system, an aircraft coordinate system, a camera coordinate system and a tunnel light coordinate system are established. The translation vectors of the two images taken by the camera in the world coordinate system are derived 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, 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 real translation vector obtained from the camera, thereby calculating the three-dimensional position of the drone, and further realizing the accurate 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 drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.
[0022] Figure 1 A mine system description diagram provided for a specific implementation of the present invention;
[0023] Figure 2 A schematic diagram of a lane map definition provided by a specific implementation method of the present invention;
[0024] Figure 3 A coordinate system definition of a drone 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 implementation manner of the present invention.
[0026] Figure numerals: ① is the UWB base station, ② is the drone, ③ is the lane light, and ④ is the ground support cabin. DETAILED DESCRIPTION
[0027] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. 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 creative work 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 a certain distance in the mine tunnel, and install a UWB base station at each 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 drone in the tunnel through the base station deployed in the tunnel and the tag installed on the drone body. The mine tunnel map is defined as follows: Figure 2 shown.
[0030] Step 2: Mount a camera on the drone and set a displacement sensor (IMU is used in this embodiment), and set a laser radar. The laser radar 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 movement distance information of the drone is obtained through the displacement sensor;
[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, an IMU is used), such as the depth distance z from the target object. e , and the distance y from the top of the tunnel 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 drone 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 position of the aircraft coordinate system and the camera coordinate system is 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 coordinate and the y-axis coordinate can be measured by sensors, the positioning problem of drones in mines can be simplified to the problem of 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 taken by the camera and the moving distance information of the drone. This embodiment mainly combines the drone mechanism model for derivation and approximate calculation, and specifically includes the following steps:
[0034] The essence of the depth estimation of the drone is the position estimation of the drone, which is done 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 in 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^ Yes i i-1 The antisymmetric matrix of .
[0037] Considering that formula (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. 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 Expressed as the drone position, Θ = [φ,θ,ψ] T Expressed as the UAV Euler angle, n i Expressed as the motor speed of the i-th propeller, c T It represents the lift coefficient of the propeller, and m represents the mass of the drone. The actual displacement of the drone can be calculated through the drone mechanism model, and then the measured displacement can be corrected from the real scale.
[0042] The relative position of the drone and the onboard camera is fixed, and the camera is defined to take two frames of images 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 obtain the approximate real translation vector f in the world coordinate system that is magnified in real proportion. i i-1 :
[0045]
[0046] Step 5: Obtain the estimated depth of the lane lights relative to the camera from the translation vector, thereby calculating the three-dimensional position of the drone. This embodiment mainly obtains the estimated depth of the lane lights relative to the camera from the translation vector based on the pinhole imaging model and triangulation method, thereby calculating the three-dimensional position of the drone. This embodiment adopts the pinhole imaging method: it is simple and easy to do, and the imaging speed is fast. The triangulation method is adopted: it is easy to calculate, has high accuracy, and can still work when the light is poor.
[0047] This embodiment is based on the pinhole imaging model and the 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 the homogeneous transformation matrix It can be seen that the three-dimensional position of the drone is 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 is 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 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 . And 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, we can know that:
[0052]
[0053] Therefore:
[0054]
[0055] Finally, the depth information is obtained by the least squares 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 , the three-dimensional position information of the drone can be estimated by the known signal light coordinates and the lane width and height information This allows the drone to accurately dock at the logistics warehouse in the alley.
[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 the present application. This article uses specific examples to illustrate the principles and implementation methods of the present application. The description of the above embodiments is only used to help understand the method of the present application and its core idea. It should be pointed out that for ordinary technicians in this technical field, without departing from the principles of the present application, several improvements and modifications can be made to the present application, and these improvements and modifications also fall within the scope of protection of the claims of the present 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 of the drone, control the camera to capture the lane lights in real time to obtain image information, and obtain the movement distance information of the drone through the displacement sensor. Step 3: Establish the world coordinate system, aircraft coordinate system, camera coordinate system and lane light coordinate system; Step 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; 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.
2. The method for estimating the three-dimensional position of a drone based on drone depth estimation according to claim 1, characterized in that: 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 1, characterized in that: In the step 5, the estimated depth of the lane light relative to the camera is obtained from the translation vector based on the pinhole imaging model and triangulation, so as to calculate the three-dimensional position of the UAV.
4. The method for estimating the three-dimensional position of a drone based on drone depth estimation according to claim 2, characterized in that: In step 4, the derivation and calculation are carried out in combination with the UAV mechanism model, which includes the following: The relative position of the drone and the camera is fixed, and the camera is defined to take two frames of images 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 is expressed as in Capture 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: Using this as a constraint, we get the approximate real translation vector f in the world coordinate system that is magnified at the real scale. i i-1 :
5. The method for estimating the three-dimensional position of a drone based on drone depth estimation according to claim 3, characterized in that: 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, so as to calculate the three-dimensional position of the drone, which includes the following: It can be seen that the three-dimensional position of the drone is 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 Equivalence, transforming the problem into a solution c P i problem.
6. The method for estimating the three-dimensional position of a drone based on drone depth estimation according to claim 5, characterized in that: 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 The three-dimensional coordinates in the camera coordinate system c P i The relationship expression is: Among them, K represents the intrinsic parameter matrix of the camera, 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.
7. The method for estimating the three-dimensional position of a drone based on drone depth estimation according to claim 5, characterized in that: The K is obtained by Zhang Zhengyou calibration method.
8. The method for estimating the three-dimensional position of a drone based on drone depth estimation according to claim 5, characterized in that: The p i Obtained by the YOLO-v5 target detection algorithm.
9. The method for estimating the three-dimensional position of a drone based on drone depth estimation according to claim 6, characterized in that: 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: 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 , and 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, we can know that: Therefore: Finally, the depth information is obtained by the least squares method [ c z i , c z i-1 ] T : in, 10. The method for estimating the three-dimensional position of a drone based on drone depth estimation according to claim 6, characterized in that: According to the depth information, we can further obtain the other two-dimensional coordinate information in the world coordinate system: 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 , the three-dimensional position information of the UAV can be estimated by the known coordinates of the lane lights and the lane width and height information e c .
Citation Information
Patent Citations
Monocular vision ranging method based on edge point information in image
CN105809687A
Simultaneous localization and mapping (SLAM) method for unmanned aerial vehicle based on mixed vision odometers and multi-scale map
CN109029417A
Small unmanned aerial vehicle visual positioning method
CN111288989A
Navigation method and device, electronic equipment and storage medium
CN113465600A
Monocular vision-based auxiliary inertial positioning method
CN115752442A
