A method for global positioning of a drone in a GNSS denial environment

Abstract

The application discloses a kind of for GNSS denial environment unmanned plane global positioning method, combine airborne camera, inertial measurement unit, barometer and magnetometer and so on Variety of sensors, the shortcoming that visual odometry cannot realize WGS-84 global positioning is improved, on the basis of unmanned plane local position, attitude angle of visual positioning and inertial measurement unit positioning fusion, further based on heading angle global attitude angle global attitude angle fusion estimation, based on coordinate conversion, altitude global position fusion estimation, obtain global position, attitude angle, and send to flight control system, realize self-positioning control.This provides more reliable pose information for the positioning of unmanned aerial vehicle in denial environment, and realizes the stable flight of unmanned aerial vehicle in denial environment.

Description

Technical Field

[0001] This invention belongs to the field of multi-rotor UAV positioning technology in GNSS-denied environments, and more specifically, relates to a global positioning method for UAVs in GNSS-denied environments. Background Technology

[0002] With the increasing maturity of drone technology, it has been applied to various fields, such as search and rescue and inspection. Accurate position estimation is a prerequisite for drones to perform their missions smoothly, and traditional positioning and navigation technologies, represented by the Global Navigation Satellite System (GNSS), have been well applied in drone navigation systems.

[0003] GNSS denied environments refer to situations where satellite signals are interfered with or blocked, preventing GNSS positioning equipment from outputting navigation parameters correctly. Because GNSS signals are easily interfered with and blocked by buildings, mountains, forests, and other areas, drone flight is restricted. Navigation technology based on multi-source data fusion relies on multiple sensors, such as vision and IMU, to acquire information about the surrounding environment and uses information fusion technology to obtain the drone's positioning information, thereby reducing the drone's dependence on GNSS and showing promising application prospects in the civilian sector.

[0004] like Figure 1 As shown, Chinese invention patent application CN112179338A, published on January 5, 2021, discloses a method for low-altitude UAV self-localization based on vision and inertial navigation fusion. This method leverages the characteristics of a monocular camera and an IMU module to achieve complementary sensor data. While monocular camera-based localization can adapt to most scenarios, it cannot solve scale issues and struggles to establish matching relationships in weak-texture or fast-moving scenes, easily leading to tracking loss. Pure IMU can accurately reflect dynamic changes in short timeframes (milliseconds), but its cumulative error increases over long periods (seconds). Therefore, by fusing visual and inertial navigation information in the initialization, local optimization, and global optimization stages of the localization process, the advantages of different sensors can be complemented, ultimately enhancing the applicability and accuracy of the localization system and providing reliable technical support for UAV self-localization in low-altitude battlefield environments. However, this method is only suitable for low-altitude environments.

[0005] Currently, drone positioning technology has the following problems:

[0006] (1) Since UAVs usually need to rely on GNSS on the UAV to achieve positioning, it is difficult to achieve positioning and flight in a denied environment;

[0007] (2), the existing SLAM-based visual positioning system can only realize relative positioning in a local coordinate system, and cannot realize positioning in a global coordinate system;

[0008] (3), the existing unmanned aerial vehicle positioning method has poor real-time performance in on-board platform operation, which limits the flight maneuverability of the unmanned aerial vehicle.

[0009] Therefore, it is necessary to propose a visual-based global positioning method for unmanned aerial vehicles, which is used for positioning in a GNSS denial environment. SUMMARY

[0010] The purpose of the present application is to overcome the shortcomings of the prior art, and to provide a global positioning method for unmanned aerial vehicles in a GNSS denial environment, which provides more reliable pose information for unmanned aerial vehicle positioning in a denial environment, and realizes stable flight of the unmanned aerial vehicle in a denial environment.

[0011] To achieve the above-mentioned purpose of the application, the global positioning method for unmanned aerial vehicles in a GNSS denial environment comprises the following steps:

[0012] (1), first, initialize, input the longitude (lon takeoff , latitude (lat takeoff ) and WGS84 coordinates of the target point of the unmanned aerial vehicle take-off, then calculate the heading angle according to the magnetic field vector output by the magnetometer, and obtain the stable heading angle yaw mag ;

[0013] |yaw current -yaw last |≤ θ (1)

[0014] Wherein, yaw current is the heading angle calculated in the current cycle, yaw last is the heading angle calculated in the last cycle, and θ is a threshold value in radians. The cycle is calculated until the condition of formula (1) is met, and the magnetometer heading angle yaw mag =yaw current is obtained.

[0015] Then, the unmanned aerial vehicle takes off.

[0016] (2), calculate the local position and attitude angle of the unmanned aerial vehicle based on the visual information of the on-board camera

[0017] 2.1), first, the airborne camera collects the current image data, obtains the image key points through the FAST (Features from Accelerated Segment Test) corner detection algorithm, describes the image key points based on the BRIEF (Binary Robust Independent Elementary Features) descriptor, and obtains the feature points;

[0018] 2.2), then, the matching relationship of the feature points of the last frame image and the current frame image is calculated, the rotation matrix between the kth frame and the k+1th frame in the camera coordinate system is obtained translation vector

[0019] The basic matrix E is constructed:

[0020]

[0021] Point Point For the matching feature point pair i, i=1, 2, …, 8, the two matching feature points are point located in the kth frame, point located in the k+1th frame, respectively represented as wherein, is the pixel horizontal coordinate and the pixel vertical coordinate of point on the kth frame, is the pixel horizontal coordinate and the pixel vertical coordinate of point on the k+1th frame; the coordinates thereof on the normalized plane are respectively:

[0022]

[0023] Based on the eight-point method, the following formula is solved:

[0024]

[0025] The basic matrix E is obtained;

[0026] Based on singular value decomposition, the rotation matrix translation vector

[0027] 2.3), then, according to the rotation matrix translation vector the depth of all feature points is calculated that is, the following formula is solved:

[0028]

[0029] the kth frame point the k+1 frame point depth of N is the number of feature point matching point pairs;

[0030] 2.4), finally, the UAV rotation matrix translation vector

[0031] for the k+1 frame point The non-homogeneous coordinates in the world coordinate system are obtained according to the following formula

[0032]

[0033] wherein:

[0034]

[0035]

[0036] wherein, is the rotation matrix from the 0th frame to the k+1 frame, is the translation vector from the 0th frame to the k+1 frame, then the point the homogeneous coordinates in the world coordinate system

[0037]

[0038] define the rotation matrix from the camera coordinate system to the world coordinate system

[0039]

[0040] translation vector

[0041]

[0042] Based on singular value decomposition, the rotation matrix is solved by the following formula translation vector

[0043]

[0044] wherein, the translation vector represents the local position of the UAV calculated based on the onboard camera vision information, and the rotation matrix is the local attitude angle of the UAV calculated based on the onboard camera vision information;

[0045] (3) Based on the acceleration and angular velocity of the inertial measurement unit, the local position and attitude angle of the UAV are calculated

[0046] 3.1), calculate the position and velocity of the k+1 frame by integrating the inertial measurement unit:

[0047]

[0048] where, is the position of the k+1 frame in the world coordinate system, is the position of the k frame in the world coordinate system, is the velocity of the k frame in the world coordinate system, is the velocity of the k+1 frame in the world coordinate system, Δt k is the time interval between the k frame and the k+1 frame, is the rotation matrix of the inertial measurement unit in the world coordinate system, is the acceleration information output by the inertial measurement unit accelerometer, g w = [0, 0, g] T is the acceleration of gravity;

[0049] 3.2), calculate the quaternion of the unmanned aerial vehicle:

[0050]

[0051] where, is the quaternion of the k+1 frame in the world coordinate system, is the quaternion of the k frame in the world coordinate system, is the angular velocity information output by the inertial measurement unit, is the skew-symmetric matrix of ;

[0052] where, represents the local position of the unmanned aerial vehicle, represents the local attitude angle;

[0053] (4), fuse the position and attitude angle calculated based on the on-board camera visual information and the acceleration and angular velocity calculated by the inertial measurement unit

[0054] 4.1), first fuse the translation vector based on the on-board camera visual information and the translation vector based on the inertial measurement unit to obtain the fused local translation vector p local :

[0055]

[0056] where, α1, β1 are weight scalars, the fused local translation vector p local represents the fused position;

[0057] 4.2), then convert the rotation matrix to Euler angles:

[0058]

[0059] Convert the UAV quaternion to Euler angles:

[0060]

[0061] 4.3), fuse the onboard camera Euler angles with the inertial measurement unit Euler angles to obtain the fused Euler angles [pitch local , roll local , yaw local ] T :

[0062]

[0063] wherein a2, b2 are weight scalars, and the fused Euler angles represent the fused attitude angles;

[0064] (5), read the current air pressure of the UAV based on the barometer, and calculate the current altitude h abs

[0065]

[0066] wherein P0 is the standard atmospheric pressure, taking the value 101.325 kPa, P is the actual measured atmospheric pressure of the UAV, with the unit of kPa, and T is the actual measured temperature, with the unit of °C;

[0067] (6), global position and attitude angle fusion estimation

[0068] 6.1), convert the fused attitude angles to obtain the global attitude angles [pitch, roll, yaw] T in the geographic coordinate system:

[0069]

[0070] 6.2), convert the fused local translation vector p local from the right front upper coordinate system to the ENU coordinate system position [e, n, u] T :

[0071]

[0072] wherein the coordinate origin in the ENU coordinate system is the takeoff position of the UAV;

[0073] 6.3), convert the UAV position [e, n, u] TConvert the UAV position in the ECEF coordinate system [x ecef ,y ecef ,z ecef ] T :

[0074]

[0075] 6.4), convert the UAV position in the ECEF coordinate system [x ecef ,y ecef ,z ecef ] T to the WGS84 coordinate system:

[0076]

[0077] Where alt, lat, lon are the height, latitude, longitude respectively, e is the eccentricity of the ellipsoid:

[0078]

[0079] C is the radius of curvature of the reference ellipsoid:

[0080]

[0081] a is the length of the major axis of the earth, b is the length of the minor axis of the earth, ξ is the height anomaly value, is the difference between the quasi-geoid and the geodetic height reference ellipsoid:

[0082] α3, β3 are weight scalars, h abs0 is the altitude of the UAV takeoff position;

[0083] (7), send the global position and attitude angle to the flight control system

[0084] The latitude lat, longitude lon, height alt, pitch angle pitch, roll angle roll, heading angle yaw and speed of the global pose are sent to the flight control system to realize self-positioning control.

[0085] The purpose of the application is achieved:

[0086] The application is a UAV global positioning method for GNSS denial environment, which combines various sensors such as airborne camera, inertial measurement unit, barometer and magnetometer, improves the shortcomings of visual odometer that cannot realize WGS-84 global positioning, and further estimates the global attitude angle based on the heading angle on the basis of fusion of the local position and attitude angle of the UAV in visual positioning and inertial measurement unit positioning, and estimates the global position based on coordinate conversion and altitude, so as to obtain the global position and attitude angle and send them to the flight control system, so as to realize self-positioning control. Thus, more reliable position information is provided for UAV positioning in denial environment, and stable flight of the UAV in denial environment is realized. BRIEF DESCRIPTION OF DRAWINGS

[0087] Figure 1 is a flowchart of a low-altitude UAV self-positioning method based on fusion of vision and inertial navigation in the prior art;

[0088] Figure 2 is a flowchart of a specific embodiment of the UAV global positioning method for GNSS denial environment of the application;

[0089] Figure 3 is a principle block diagram of a specific embodiment of the UAV global positioning method for GNSS denial environment of the application;

[0090] Figure 4 is a schematic diagram of the true value of the positioning result and the positioning value. DETAILED DESCRIPTION

[0091] The specific embodiments of the application will be described below in conjunction with the accompanying drawings, so that those skilled in the art can better understand the application. It should be particularly noted that in the following description, when the detailed description of known functions and designs may obscure the main content of the application, these descriptions will be omitted here.

[0092] Figure 2 is a flowchart of a specific embodiment of the UAV global positioning method for GNSS denial environment of the application.

[0093] In this embodiment, Figure 2 As shown in the figure, the UAV global positioning method for GNSS denial environment of the application includes the following steps:

[0094] Step S1: initialization and calculation of heading angle, then take off

[0095] First, initialization is performed, and the takeoff latitude and longitude (lon takeoff ,lat takeoff), and then the heading angle is calculated according to the magnetic field vector output by the magnetometer. Since the fusion speed of the magnetometer is slow and unstable in the initialization process, the formula (1) is used to obtain the stable heading angle yaw mag ;

[0096] |yaw current -yaw last |≤θ (1)

[0097] Wherein, yaw current is the heading angle calculated in the current cycle, yaw last is the heading angle calculated in the last cycle, and θ is the threshold value in radian. The cycle is calculated until the condition of formula (1) is met, and the magnetometer heading angle yaw mag is obtained. current .

[0098] Then, the unmanned aerial vehicle takes off;

[0099] Step S2: calculating the local position and attitude angle of the unmanned aerial vehicle based on the visual information of the on-board camera

[0100] Step S2.1: first, the on-board camera collects current image data, obtains image key points through the FAST (Features from Accelerated Segment Test) corner detection algorithm, describes the image key points based on the BRIEF (Binary Robust Independent Elementary Features) descriptor, and obtains feature points.

[0101] Step S2.2: then, the matching relationship of the feature points of the last frame image and the current frame image is calculated, and the rotation matrix translation vector

[0102] point point is the two matching feature points of the feature point matching point pair i, i = 1, 2, …, 8, and point is located in the kth frame, and point is located in the k+1th frame, and is respectively represented as wherein, is the pixel horizontal coordinate and pixel vertical coordinate of point on the kth frame, is the pixel horizontal coordinate and pixel vertical coordinate of point on the k+1th frame. There is the following relationship:

[0103]

[0104] Where K is the intrinsic parameter matrix of the UAV's onboard camera (3×3); for for Antisymmetric matrix:

[0105]

[0106] This invention uses an eight-point method to solve for the rotation matrix. Translation vector

[0107] Construct the fundamental matrix E:

[0108]

[0109] point point Their coordinates on the normalized plane are as follows:

[0110]

[0111] Thus, the point point The relationship can be simplified to:

[0112]

[0113] Thus, based on the eight-point method, the following formula can be solved:

[0114]

[0115] The fundamental matrix E is obtained;

[0116] Based on Singular Value Decomposition (SVD), the rotation matrix between the k-th frame and the (k+1)-th frame is obtained through the fundamental matrix E. Translation vector

[0117] Step S2.3: Then, according to the rotation matrix... Translation vector Calculate the depth of all feature points That is, solve the following formula:

[0118]

[0119] Get the k-th frame point frame (k+1) depth N is the number of feature point matching pairs.

[0120] Step S2.4: Finally, calculate the UAV rotation matrix. Translation vector

[0121] For frame k+1 The non-homogeneous coordinates in the world coordinate system can be obtained using the following formula.

[0122]

[0123] in:

[0124]

[0125]

[0126] In the formula, This is the rotation matrix from frame 0 to frame (k+1). Let be the translation vector from frame 0 to frame (k+1), then the point homogeneous coordinates in the world coordinate system

[0127]

[0128] Define the rotation matrix from the camera coordinate system to the world coordinate system.

[0129]

[0130] Translation vector

[0131]

[0132] structure Augmented matrix:

[0133]

[0134] Thus, based on singular value decomposition, the rotation matrix can be solved using the following formula. Translation vector

[0135]

[0136] Among them, the translation vector The rotation matrix represents the local position of the drone calculated based on visual information from the onboard camera. The local attitude angle of the drone is calculated based on visual information from the airborne camera.

[0137] Step S3: Calculate the local position and attitude angle of the UAV based on the acceleration and angular velocity of the inertial measurement unit

[0138] Step S3.1: Calculate the position and velocity of the k+1 frame by integrating the inertial measurement unit:

[0139]

[0140] wherein, is the position of the k+1 frame in the world coordinate system, is the position of the k frame in the world coordinate system, is the velocity of the k frame in the world coordinate system, is the velocity of the k+1 frame in the world coordinate system, Δt k is the time interval between the k frame and the k+1 frame, is the rotation matrix of the inertial measurement unit in the world coordinate system, is the acceleration information output by the accelerometer of the inertial measurement unit, g w = [0, 0, g] T is the acceleration of gravity;

[0141] Step S3.2: Calculate the quaternion of the UAV:

[0142]

[0143] wherein, is the quaternion of the k+1 frame in the world coordinate system, is the quaternion of the k frame in the world coordinate system, is the angular velocity information output by the inertial measurement unit, is the skew-symmetric matrix of ; and

[0144] wherein, represents the local position of the UAV, represents the local attitude angle.

[0145] Step S4: Fuse the position and attitude angle calculated based on the visual information of the onboard camera and the acceleration and angular velocity of the inertial measurement unit

[0146] Step S4.1: First, fuse the translation vector based on the visual information of the onboard camera with the translation vector based on the inertial measurement unit to obtain the fused local translation vector p local :

[0147]

[0148] wherein, α1, β1 are weight scalars, and the fused local translation vector plocal represents the fused position;

[0149] Step S4.2: Then the rotation matrix is converted into Euler angles:

[0150]

[0151] The quaternion of the UAV is converted into Euler angles:

[0152]

[0153] Step S4.3: The onboard camera Euler angles are fused with the inertial measurement unit Euler angles to obtain the fused Euler angles [pitch local , roll local , yaw local ] T :

[0154]

[0155] wherein α2, β2 are weight scalars, and the fused Euler angles represent the fused attitude angle.

[0156] Step S5: The current air pressure of the UAV is read based on the barometer, and the current altitude h abs

[0157]

[0158] wherein P0 is the standard atmospheric pressure, taking the value 101.325 kPa, P is the actual measured atmospheric pressure of the UAV, in kPa, and T is the actual measured temperature, in ℃.

[0159] In this embodiment, in order to more accurately measure the current air pressure of the UAV, the measured air pressure P is corrected based on the current flight speed calculated by the UAV IMU, the current wind speed calculated based on the difference between the current attitude angle and the attitude angle motor speed of the UAV, thereby improving the altitude measurement accuracy.

[0160] Step S6: Global position, attitude angle fusion estimation

[0161] Step S6.1: The fused attitude angle is converted into a geographic coordinate system to obtain the global attitude angle [pitch, roll, yaw] T :

[0162]

[0163] Step S6.2: The fused local translation vector p local is converted from the right front upper coordinate system to the ENU coordinate system position [e, n, u]T :

[0164]

[0165] Wherein, the coordinate origin in the ENU coordinate system is the UAV take-off position;

[0166] Step S6.3: convert the UAV position [e, n, u] in the ENU coordinate system into the UAV position [x T ,y ecef ,z ecef ] in the ECEF coordinate system; ecef T :

[0167]

[0168] Step S6.4: convert the UAV position [x ecef ,y ecef ,z ecef ] in the ECEF coordinate system into the WGS84 coordinate system: T

[0169]

[0170] Wherein, alt, lat, and Ion are the altitude, latitude, and longitude, respectively, and e is the eccentricity of the ellipsoid:

[0171]

[0172] C is the radius of curvature of the reference ellipsoid:

[0173]

[0174] a is the length of the major axis of the Earth, b is the length of the minor axis of the Earth, ξ is the height anomaly value, and N is the difference between the quasi-geoid and the geodetic height reference ellipsoid;

[0175] α3, β3 are weight scalars, which can be automatically adjusted according to the current speed of the UAV and the wind speed in the specific implementation process, and h abs0 is the altitude of the UAV take-off position.

[0176] Step S7: send the global position and attitude angle to the flight control system

[0177] The latitude lat, longitude Ion, altitude alt, pitch angle pitch, roll angle roll, heading angle yaw, and speed of the global pose are sent to the flight control system to realize self-positioning control.

[0178] Figure 3 ​​​This is a principle block diagram of a specific implementation of the UAV global positioning method for GNSS denied environments according to the present invention.

[0179] like Figure 3 As shown, the hardware in this invention includes an unmanned aerial vehicle (UAV) test platform comprising an airborne camera, an inertial measurement unit (IMU), a barometer, a magnetometer, an airborne computer, an embedded flight control system, an electronic speed controller, and rotor motors. The airborne camera acquires current image data, the IMU acquires the UAV's angular velocity and acceleration, the barometer acquires air pressure, and the magnetometer acquires the UAV's current magnetic field vector. The airborne computer processes multiple sensor signals in real time to achieve real-time UAV positioning. The UAV test platform is used to execute positioning tasks and conduct experimental verification. During positioning, the airborne computer tracks feature points based on the airborne camera, integrates data based on the IMU, obtains the fused local position and attitude angles, calculates altitude based on the barometer, calculates and processes the heading angle based on the magnetometer, and fuses the local pose to achieve global pose estimation and position control for the UAV. Finally, the data is input to the embedded flight control system to achieve autonomous flight control. This invention enables real-time global positioning and stable flight of the UAV in the Coordinate System of World Geodetic Coordinates (WGS-84) even without GNSS signals.

[0180] In this embodiment, the UAV test platform is described using the FRD coordinate system, where the X-axis points towards the UAV's nose, the Y-axis points towards the right side of the UAV, and the Z-axis points towards the bottom of the UAV. The magnetometer, IMU, and barometer are mounted on the UAV's center plate, located at the geometric center of the UAV's XY plane. The coordinate system for the magnetometer and IMU is such that the X-axis points forward of the UAV, the Y-axis points to the right side of the UAV, and the Z-axis points downwards. The airborne camera is mounted below the UAV, with its imaging plane perpendicular to the UAV's Z-axis and looking downwards at the ground. The airborne computer is mounted on the upper part of the UAV.

[0181] In this embodiment, the magnetometer module uses anisotropic magnetoresistive materials to detect the magnitude of magnetic induction intensity in space, and then calculates the direction of the magnetic field.

[0182] The IMU integrates a 3-axis MEMS gyroscope and a 3-axis MEMS accelerometer. It uses three 16-bit ADCs for the gyroscope and accelerometer to convert the measured analog quantities into output digital quantities.

[0183] The barometer module features a high-resolution barometric pressure sensor with an IIC bus interface, including a high-linearity pressure sensor and an ultra-low-power 24-bit analog-to-digital converter with a resolution of up to 10 cm.

[0184] Data from the magnetometer, barometer, and IMU sensors are first transmitted to the UAV flight control system via IIC, and then communicate with the onboard computer via the RS232 serial port's MAVROS protocol.

[0185] In the embodiment, the on-board computer unit adopts NUC of Intel Company, and the computing power and performance of the on-board computer unit guarantee the positioning of the unmanned aerial vehicle.

[0186] In the embodiment, the unmanned aerial vehicle test platform adopts a four-rotor unmanned aerial vehicle with an axle distance of 680 mm, and the unmanned aerial vehicle mainly comprises an unmanned aerial vehicle frame, a propeller, a flight control system, a motor, an electronic speed controller, a power battery, an on-board computer, an on-board camera, a remote control transmitter and a receiver.

[0187] After testing, the maximum take-off weight of the unmanned aerial vehicle is 7 kg, the load weight is 3 kg, the endurance of the unmanned aerial vehicle in an empty state is 30 minutes, the endurance of the unmanned aerial vehicle in a full load state is 15 minutes, and the maximum flight speed is 4 m / s.

[0188] In the positioning test, firstly, the longitude and latitude information of the take-off position, the flight height information and the longitude and latitude information of the target point are inputted, the unmanned aerial vehicle takes off and automatically reaches the target point in sequence. During the test, the GNSS is installed on the unmanned aerial vehicle to calculate the true value of the current longitude and latitude of the unmanned aerial vehicle, as shown in FIG. 6, the solid line is the true value of the unmanned aerial vehicle measured by the GNSS, and the spaced dotted line is the real-time positioning value of the application. After testing, the unmanned aerial vehicle can achieve a positioning frequency of 30 Hz and a real-time positioning accuracy of 5.83 m during flight. Figure 4

[0189] Although the above describes the specific embodiments of the application in order to facilitate the understanding of the application by the person skilled in the art, it should be clear that the application is not limited to the scope of the specific embodiments, and for the person skilled in the art, it is obvious that various changes are within the spirit and scope of the application defined and determined by the appended claims, and all the application creations using the concept of the application are included in the protection.​

Claims

1. A global positioning method for unmanned aerial vehicles (UAVs) in GNSS-denied environments, characterized in that, Includes the following steps: (1), First, initialization, input unmanned aerial vehicle take-off latitude and longitude (lon takeoff , lat takeoff ), the target point WGS84 coordinates, and then according to the magnetic field vector output by the magnetometer to calculate the heading angle, get a stable heading angle yaw mag ; | yaw current -yaw last |≤θ (1) where yaw current is the heading angle calculated for the current cycle, yaw last is the heading angle calculated for the previous cycle, θ is a threshold value in radians, and the cycle is calculated until the condition of equation (1) is met, obtaining the magnetometer heading angle yaw mag = yaw current ; Then, the drone took off; (2) Calculate the local position and attitude angle of the UAV based on the visual information of the airborne camera. 2.1) First, the airborne camera acquires current image data, obtains key points of the image through the FAST (Features from Accelerated Segment Test) corner detection algorithm, and describes the key points of the image based on the BRIEF (Binary Robust Independent Elementary Features) descriptor to obtain feature points; 2.2) Then, calculate the feature point matching relationship between the previous frame and the current frame, and then obtain the rotation matrix between the k-th frame and the (k+1)-th frame in the camera coordinate system. Translation vector Construct the fundamental matrix E: point point For feature point matching, two matching feature points are given for each pair i, i = 1, 2, ..., 8. Located in the k-th frame, point Located in the (k+1)th frame, respectively represented as in, For the k-th frame point The pixel x-coordinate and pixel y-coordinate, For the (k+1)th frame point The pixel's x-coordinate and y-coordinate; their coordinates on the normalized plane are respectively: Solve the following formula using the eight-point method: The fundamental matrix E is obtained; Based on singular value decomposition, the rotation matrix between the k-th frame and the (k+1)-th frame is obtained through the fundamental matrix E. Translation vector 2.3) Then, according to the rotation matrix Translation vector Calculate the depth of all feature points That is, solve the following formula: Get the k-th frame point frame (k+1) depth N is the number of feature point matching pairs; 2.4) Finally, calculate the UAV rotation matrix. Translation vector For frame k+1 The non-homogeneous coordinates in the world coordinate system can be obtained using the following formula. in: In the formula, This is the rotation matrix from frame 0 to frame (k+1). Let be the translation vector from frame 0 to frame (k+1), then the point homogeneous coordinates in the world coordinate system Define the rotation matrix from the camera coordinate system to the world coordinate system. Translation vector Based on singular value decomposition, the rotation matrix can be solved using the following formula. Translation vector Among them, the translation vector The rotation matrix represents the local position of the drone calculated based on visual information from the onboard camera. The local attitude angles of the drone calculated based on visual information from the airborne camera; (3) Calculate the local position and attitude angle of the UAV based on the acceleration and angular velocity of the inertial measurement unit. 3.1) Calculate the position and velocity of the (k+1)th frame by integrating using the inertial measurement unit: in, This represents the position of the (k+1)th frame in the world coordinate system. Let k be the position in the world coordinate system. Let the velocity be the velocity of the k-th frame in the world coordinate system. Let Δt be the velocity in the (k+1)th frame in the world coordinate system. k The time interval between frame k and frame (k+1) is... This is the rotation matrix of the inertial measurement unit in the world coordinate system. The acceleration information output by the accelerometer of the inertial measurement unit, g w =[0,0,g] T It is the acceleration due to gravity; 3.2) Calculate the quaternion of the UAV: in, Let be the quaternion of the (k+1)th frame in the world coordinate system. Let be the quaternion of the k-th frame in the world coordinate system. The angular velocity information output by the inertial measurement unit. for The antisymmetric matrix; in, Indicates the local location of the drone. Indicates the local attitude angle; (4) The position and attitude angles are calculated by integrating the visual information from the airborne camera with the acceleration and angular velocity from the inertial measurement unit. 4.1) First, the translation vector is obtained based on the visual information from the airborne camera. Translation vector obtained from inertial measurement unit The fusion yields the fused local translation vector p. local : Wherein, α1, β1 are weight scalars, the fused local translation vector p local Indicates the fused position; 4.2) Then rotate the matrix Convert to Euler angles: Converting drone-based quaternions to Euler angles: 4.3), fuse the onboard camera Euler angles with the inertial measurement unit Euler angles to get the fused Euler angles [pitch local , roll local , yaw local ] T : Where α2 and β2 are weight scalars, and the fused Euler angles represent the fused attitude angles; (5) Based on the barometer to read the current pressure of the UAV, calculate the current altitude h abs Where P0 is the standard atmospheric pressure, with a value of 101.325 kPa, P is the actual atmospheric pressure measured by the UAV, in kPa, and T is the actual measured temperature, in °C. (6) Global position and attitude angle fusion estimation 6.1) Convert the fused attitude angles to the geographic coordinate system to obtain the global attitude angles [pitch, roll, yaw]. T : 6.2) The fused local translation vector p local Transform the position from the upper right coordinate system to the ENU coordinate system [e,n,u] T : in, The origin of the ENU coordinate system is the takeoff position of the UAV. 6.3) Position the UAV in the ENU coordinate system [e,n,u] T Convert the UAV position to the ECEF coordinate system [x] ecef ,y ecef ,z ecef ] T : 6.4) Position the UAV in the ECEF coordinate system [x ecef ,y ecef ,z ecef ] T Transform to WGS84 coordinate system: Where alt, lat, and lon represent altitude, latitude, and longitude, respectively, and e is the ellipsoidal eccentricity: C is the radius of curvature of the reference ellipsoid. a is the length of the Earth's semi-major axis, b is the length of the Earth's semi-minor axis, ξ is the geoid undulation, which is the difference in elevation between the geoid and the geodetic reference ellipsoid. α3 and β3 are weight scalars, h abs0 The altitude of the drone's takeoff location; (7) Send global position and attitude angle to the flight control system Global pose parameters: latitude (lat), longitude (lon), altitude (alt), pitch, roll, yaw, and velocity. The signal is sent to the flight control system to achieve self-positioning control.

Images