Visual inertia fusion aircraft autonomous navigation method
By employing a visual-inertial fusion method, utilizing camera calibration, gradient orientation histogram matching, and deep learning networks, the navigation accuracy problem of UAVs in GPS-free environments was solved, achieving high-precision autonomous navigation and inertial error suppression.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-12-06
- Publication Date
- 2026-03-31
AI Technical Summary
Unmanned aerial vehicles (UAVs) struggle to achieve high-precision autonomous navigation in environments without GPS or at night. Existing SLAM technology suffers from position divergence in open environments, and the accumulation of errors in inertial navigation systems cannot meet the requirements for long-range navigation.
A visual-inertial fusion method is adopted, which uses camera correction, gradient orientation histogram matching, end-to-end deep learning network and pose observation fusion filtering algorithm to achieve image registration and inertial navigation data fusion, and obtain accurate position and heading information.
It achieves high-precision registration between infrared or visible light aerial images and satellite visible light reference images, effectively suppressing inertial navigation errors and improving navigation accuracy and stability.
Smart Images
Figure CN121761876A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of visual navigation, specifically relating to a visual-inertial fusion method for autonomous navigation of aircraft. Background Technology
[0002] Drone navigation typically employs a Global Navigation Satellite System (GNSS) or a fusion of GNSS and an Inertial Navigation System (INS). However, satellite signals can be interfered with, spoofed, or even blocked. In such cases, because INS position and velocity errors accumulate, relying solely on the inertial navigation system generally cannot meet navigation requirements, especially for MEMS (Micro-Electro-Mechanical System) inertial navigation systems widely used in small and medium-sized drones.
[0003] In recent years, SLAM (Simultaneous Localization and Mapping) technology, based on the fusion of vision, lidar, and inertial navigation, has been extensively studied and widely applied to autonomous navigation in robots, unmanned vehicles, and drones. However, due to the accumulation of errors in visual odometry, SLAM also suffers from position divergence. To address this issue, SLAM technology overcomes position error accumulation through closed-loop path detection and optimization, but closed-loop path conditions do not exist in many airborne navigation applications. In practice, autonomous navigation based on SLAM technology is mostly used in indoor or enclosed environments. In outdoor, open environments, SLAM technology is typically used to support autonomous navigation under conditions of short-term GPS signal loss, and cannot support long-range autonomous navigation and global positioning. Summary of the Invention
[0004] Purpose of the invention: To address the problem of unmanned aerial vehicles (UAVs) struggling to navigate in environments without GPS or at night, a visual-inertial fusion method for autonomous navigation of UAVs is proposed.
[0005] Technical solution: A visual-inertial fusion autonomous navigation method for aircraft, comprising the following steps: Step 1: Correct the real-time image based on the camera intrinsic parameters, the altitude provided by the altimeter, and the attitude information provided by the inertial navigation system, and convert it into a frontal downward view image; Step 2: Calculate the displacement relationship between the real-time downward view and the reference view; Step 3: Use an end-to-end deep learning network to further estimate the homography relationship between the real-time image and the reference image to obtain accurate position and heading information; Step 4: Based on the displacement relationship obtained in Step 2 and the precise position and heading information obtained in Step 3, the final position and heading estimates are obtained by fusing them together.
[0006] Furthermore, in step 1, the process is as follows: A visual-inertial fusion autonomous navigation method for aircraft, characterized by the following steps: In the reference state of the inertial navigation coordinate system, the X-axis points due east, indicating the pitch of the UAV, denoted as . x The Y-axis points due south, indicating that the drone is rolling, denoted as... y The Z-axis, pointing towards the ground along the plumb line, indicates the yaw of the UAV, denoted as . z The following formula gives the rotation matrix of the UAV from the reference state at any time. R Calculation steps:
[0007]
[0008]
[0009]
[0010] Real-time images captured by the drone are denoted as A The corrected bottom view image is denoted as The camera intrinsic parameter matrix is denoted as K The following formula gives the calculation steps for transforming a real-time image into a frontal downward view image:
[0011] The altitude data provided by the altimeter is recorded as H The focal length of the camera lens is denoted as f The camera pixel size is denoted as du Then the object plane resolution of the real-time image Res Calculated using the following formula:
[0012] The object plane resolution of the reference image is denoted as BRes Calculate the scaling factor Ra Scaling the bottom-view image by a scaling factor. Ra Calculated using the following formula:
[0013] The above steps are used to correct the real-time images captured by the drone into a frontal downward view image and to calculate the coordinates of the bottom point.
[0014] Furthermore, in step 2, the main direction of the gradient direction histogram is used as the matching feature; The calculation process for the principal direction of HOG is as follows:
[0015] In the above formula, This represents the HOG eigenvector, which statistically analyzes the gradient intensity in each direction. It is the direction of the strongest gradient in the HOG feature vector, and is used as the principal direction; Assumption It is the main direction angle of the real-time HOG image. It is the HOG principal direction angle of the corresponding window on the reference image, and the following formula gives the cosine and... The calculation process.
[0016]
[0017] The reference drawing size is The corrected real-time image size is By using cosine similarity to calculate the principal direction and performing template matching, the displacement relationship between the real-time image and the reference image in the downward view is obtained.
[0018] Furthermore, during the downward-looking correction process, the position of the aircraft's lower point on the real-time image is calculated. Based on the image matching results, the position of the aircraft on the reference image is calculated. Then, based on the geographic coordinates of the reference image, the high-frequency horizontal position information of the aircraft is obtained.
[0019] Furthermore, in step 3, the points in the reference image are mapped to the world coordinate system, and the 2D points in the real-time image are established to correspond with the 3D points in the world coordinate system. Then, the PNP pose calculation method is used to calculate the precise position and attitude information of the camera. Since the installation relationship between the camera and the aircraft is known, the heading and position information of the aircraft are obtained.
[0020] Furthermore, in step 3, the specific process is as follows: Reference image I and real-time image Divided into multiple sub-regions, the truth-valued monography transformation between the real-time image and the reference image is denoted as... The homography transformation of the sub-region estimation is as follows: loss function L The calculation formula is as follows:
[0021] In the above formula, The number of partitioned regions. sub-region Upper control point set, These are the image coordinates of the control points; Subregion Translation relative to the top left corner of the original image; the matching network obtains After estimating the affine transformation of each sub-region, the correspondence between all control points between the real-time image and the reference image is obtained. Then, the global homography transformation is estimated using the RANSAC algorithm. This allows for precise matching, resulting in accurate position and heading information.
[0022] Furthermore, the specific process in step 4 is as follows: attitude error Speed error Position error Gyroscope deviation and accelerometer deviation Defined as a state vector as follows:
[0023] This indicates the location obtained from image matching. Let represent the position obtained from the inertial navigation system. Then, the position error of the inertial navigation system is:
[0024]
[0025] This represents the heading angle obtained from image matching. Let represent the heading angle obtained from the inertial navigation system. Then, the heading error of the inertial navigation system is:
[0026] The error in the above formula is the Euler angle error, while the state vector... The heading error in the equation is the mathematical platform error; using Let represent the yaw, pitch, and roll angles of the aircraft, respectively. Then, the coordinate transformation matrix from the body coordinate system to the navigation coordinate system is... It is expressed as follows:
[0027] use Indicates the first OK If the elements of the column are given, then the observation equation for the heading is expressed as:
[0028] Based on the above formulas, the measurement model for data fusion is obtained as follows:
[0029] Using the obtained measurement model, the high-frequency horizontal position information obtained in step 2 and the precise position and heading information obtained in step 3 are fused to obtain the final position and heading estimates. An electronic device includes: a processor and a memory; The memory stores instructions that the processor can execute; When the processor is configured to execute the instructions, the electronic device enables a visual-inertial fusion aircraft autonomous navigation method.
[0030] A readable storage medium comprising: software instructions; When the software instructions are executed in the electronic device, the electronic device enables the electronic device to implement a visual-inertial fusion aircraft autonomous navigation method.
[0031] Beneficial effects: This invention enables the registration of infrared or visible light aerial images with satellite visible light reference images, and achieves autonomous positioning and heading correction based on the registration results. The heterogeneous image matching method of this invention consists of fast image matching based on structural features and precise registration based on an end-to-end learning network. The former can provide position correction of approximately 20Hz, while the latter can provide high-precision position and heading correction. This invention uses a pose fusion filtering method to fuse the position and orientation information obtained from image matching with inertial navigation, which can effectively suppress the divergence of inertial navigation errors and ultimately improve navigation accuracy. Attached Figure Description
[0032] Figure 1 Overall framework diagram of an airborne inertial navigation position and heading correction method based on heterogeneous image matching.
[0033] Figure 2 Ground point localization based on fast heterogeneous image matching.
[0034] Figure 3 HOG principal direction extraction under heterogeneous image differences and noise interference conditions.
[0035] Figure 4 Aircraft positioning and orientation based on PNP calculation.
[0036] Figure 5 An end-to-end deep learning matching network based on block-based strategy and parameter sharing.
[0037] Figure 6 Results of fast heterogeneous image and precise heterogeneous image registration. Detailed Implementation
[0038] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present application, and not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative effort should fall within the scope of protection of the present application.
[0039] In this application, the terms "upper," "lower," "left," "right," "front," "rear," "top," "bottom," "inner," "outer," "middle," "vertical," "horizontal," "lateral," and "longitudinal" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. These terms are primarily for the purpose of better describing this application and its embodiments, and are not intended to limit the indicated device, element, or component to having a specific orientation, or to be constructed and operated in a specific orientation.
[0040] Furthermore, the terms "installation," "setup," "equipped with," "connection," "linking," and "socketing" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral structure; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium, or an internal connection between two devices, components, or parts. Those skilled in the art can understand the specific meaning of these terms in this application based on the specific circumstances.
[0041] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.
[0042] The technical solution of this invention will be further described below with reference to specific algorithm implementations and accompanying drawings. The overall framework of the visual-inertial fusion autonomous navigation method for aircraft proposed in this invention is as follows: Figure 1 As shown, the specific steps are as follows: Step 1: Correct the real-time image based on the camera intrinsic parameters, the altitude provided by the altimeter, and the attitude information provided by the inertial navigation system, and convert it into a frontal downward view image.
[0043] In the reference state of the inertial navigation coordinate system, the X-axis points due east, indicating the pitch of the UAV, denoted as . x The Y-axis points due south, indicating that the drone is rolling, denoted as... y The Z-axis, pointing towards the ground along the plumb line, indicates the yaw of the UAV, denoted as . zThe following formula gives the rotation matrix of the UAV from the reference state at any time. R Calculation steps:
[0044]
[0045]
[0046]
[0047] Real-time images captured by the drone are denoted as A The corrected bottom view image is denoted as The camera intrinsic parameter matrix is denoted as K The following formula gives the calculation steps for transforming a real-time image into a frontal downward view image:
[0048] The altitude data provided by the altimeter is recorded as H The focal length of the camera lens is denoted as f The camera pixel size is denoted as du Then the object plane resolution of the real-time image Res Calculated using the following formula:
[0049] The object plane resolution of the reference image is denoted as BRes Calculate the scaling factor Ra Scaling the bottom-view image by a scaling factor. Ra Calculated using the following formula:
[0050] The above steps correct real-time images captured by the drone into a top-down view image and calculate the coordinates of the bottom point. After image scaling, the object plane resolution of the top-down view image is ensured to be consistent with the object plane resolution of the reference image. The original real-time image is shown below. Figure 2 As shown in (a), the corrected real-time image and the ground point are as follows: Figure 2 As shown in (b).
[0051] Step 2: The displacement relationship between the real-time image and the reference image is quickly calculated using the proposed fast heterogeneous image matching method based on dense structural features; This invention uses the principal direction of the Histogram of Oriented Gradients (HOG) as the matching feature. The HOG principal direction is obtained by statistically analyzing the gradient distribution of a local region and selecting the direction with the strongest gradient. Principal direction extraction can effectively improve the feature's adaptability to differences in heterogeneous images and noise interference, such as... Figure 3As shown, noise interference and differences in heterogeneous images cause significant changes in the HOG feature vector, with noticeable alterations in the gradient strength of each direction, but its principal direction remains unchanged. The following equation illustrates the calculation process for extracting the principal direction of the HOG feature vector.
[0052]
[0053] In the above formula, This represents the HOG eigenvector, which statistically analyzes the gradient intensity in each direction. It is the direction of the strongest gradient in the HOG feature vector, i.e., the principal direction.
[0054] Assumption It is the main direction angle of the real-time HOG image. It is the HOG principal direction angle of the corresponding window on the reference image, and the following formula gives the cosine and... The calculation process.
[0055]
[0056] The reference drawing size is The corrected real-time image size is By using cosine similarity to calculate the principal direction and performing template matching, the displacement relationship between the real-time image and the reference image in the downward view is obtained.
[0057] Since the position of the aircraft's lower point on the real-time image can be calculated during the downward-looking correction process, the aircraft's position on the reference image can be calculated based on the image matching results. Then, based on the geographic coordinates of the reference image, the aircraft's high-frequency horizontal position information can be obtained, such as... Figure 2 As shown in (c).
[0058] Step 3: Based on the results of Step 2, use an end-to-end deep learning network to further accurately estimate the homography relationship between the real-time image and the reference image; Points in the reference image can be mapped to the world coordinate system. A correspondence is established between 2D points in the real-time image and 3D points in the world coordinate system. Therefore, the PNP pose calculation method can be used to calculate the camera's precise position and attitude information, such as... Figure 4 As shown, since the installation relationship between the camera and the aircraft is known, the aircraft's heading and position information can be obtained.
[0059] This invention applies a block-based strategy to an end-to-end deep learning matching algorithm based on a two-stream symmetric network and improves the network's loss function, effectively enhancing matching accuracy. Figure 5 As shown.
[0060] Reference image I and real-time image Divided into multiple sub-regions, the truth-valued monography transformation between the real-time image and the reference image is denoted as... The homography transformation of the sub-region estimation is as follows: loss function L The calculation formula is as follows:
[0061] In the above formula, The number of partitioned regions. sub-region Upper control point set, These are the image coordinates of the control points. Subregion A translation relative to the top left corner of the original image. The matching network obtains... After estimating the affine transformation of each sub-region, the correspondence between all control points in the real-time image and the reference image can be obtained. Then, the RANSAC algorithm is used to estimate the global homography transformation. This allows for precise matching, resulting in accurate position and heading information.
[0062] The results of step 2, the fast heterogeneous image matching method, and step 3, the accurate estimation of homography between the real-time image and the reference image, are as follows: Figure 6 As shown. Figure 6 Figures (a) and (c) show the results of the fast heterogeneous image matching method. Figure 6 Figures (b) and (d) show the results of accurately estimating the homography transformation between the real-time image and the reference image. By comparison, it can be seen that the method of accurately estimating the homography transformation between the real-time image and the reference image can better align the ground objects in the real-time image and the reference image.
[0063] Step 4: Use a pose-based fusion filtering algorithm to fuse the high-frequency horizontal position information obtained in Step 2 and the position and heading information obtained in Step 3 with the inertial navigation data to obtain a more accurate position and heading estimate.
[0064] attitude error Speed error Position error Gyroscope deviation and accelerometer deviation Defined as a state vector as follows:
[0065] This indicates the location obtained from image matching. Let represent the position obtained from the inertial navigation system. Then, the position error of the inertial navigation system is:
[0066]
[0067] This represents the heading angle obtained from image matching. Let represent the heading angle obtained from the inertial navigation system. Then, the heading error of the inertial navigation system is:
[0068] The error in the above formula is the Euler angle error, while the state vector... The heading error in the equation is the mathematical platform error. (Using...) Let represent the yaw, pitch, and roll angles of the aircraft, respectively. Then, the coordinate transformation matrix from the body coordinate system to the navigation coordinate system is... It is expressed as follows:
[0069] use Indicates the first OK If the elements of the column are given, then the observation equation for the heading can be expressed as:
[0070] Based on the above formulas, the measurement model for data fusion is obtained as follows:
[0071] Using the obtained measurement model, the high-frequency horizontal position information obtained in step 2 and the position and heading information obtained in step 3 are fused with the inertial navigation system data to obtain a more accurate position and heading estimate.
[0072] The above description is merely a specific embodiment of the present invention, providing a detailed description of the invention. Parts not covered herein are conventional techniques. However, the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention. The scope of protection of the present invention should be determined by the scope of the claims.
Claims
1. A visual-inertial fusion aircraft autonomous navigation method, characterized in that: The steps are as follows: Step 1: According to the camera intrinsic parameters, the height provided by the altimeter and the attitude information provided by the inertial navigation, the real-time image is corrected and converted into a direct downward view image; Step 2: The displacement relationship between the direct downward view real-time image and the reference image is calculated; Step 3: The end-to-end deep learning network is used to further accurately estimate the homographic transformation relationship between the real-time image and the reference image, and the accurate position and heading information is obtained; Step 4: The final position and heading estimation is obtained by fusing the displacement relationship obtained in step 2 and the accurate position and heading information obtained in step 3.
2. The method of claim 1, wherein: In step 1, the process is as follows: The visual-inertial fusion aircraft autonomous navigation method comprises the following steps: The X axis points to the east in the reference state of the inertial navigation coordinate system, represents the pitch of the UAV, and is denoted as x ; the Y axis points to the south, represents the roll of the UAV, and is denoted as y ; the Z axis points to the ground along the plumb line direction, represents the yaw of the UAV, and is denoted as z ; the following formula gives the rotation matrix of the UAV at any time to the reference state R The calculation steps are as follows: The real-time image taken by the UAV is denoted as A , the corrected nadir view image is denoted as , the camera intrinsic matrix is denoted as K , and the following equation gives the calculation steps of transforming the real-time image into the nadir view image: The height data provided by the altimeter is denoted as H The camera lens focal length is denoted as f The camera pixel size is denoted as du The object plane resolution of the real-time image is then calculated by Res the following equation: The object plane resolution of the reference image is denoted by BRes The scaling factor Ra The down view image is scaled with the scaling factor Ra is calculated by The real-time image taken by the unmanned aerial vehicle is corrected into a direct downward view image and the nadir point coordinates are calculated through the above steps.
3. The method of claim 2, wherein: In step 2, the main direction of the gradient direction histogram is used as the matching feature; The calculation process of the main direction of HOG is as follows: In the above formula, represents the HOG feature vector, and the gradient intensity of each direction is counted; is the strongest direction of the gradient in the HOG feature vector, and is taken as the main direction; Assume is the real-time image HOG dominant direction angle, is the HOG dominant direction angle of the corresponding window on the reference image, the following equation gives the calculation process of cos and .
4. The reference image size is , the real-time image size after correction is , the displacement relationship between the real-time image and the reference image is obtained by using cosine similarity to calculate the main direction and template matching.
5. The method of claim 3, wherein: In the direct downward view correction process, the position of the nadir point on the real-time image is calculated, the position of the aircraft on the reference image is calculated according to the image matching result, and the high-frequency horizontal position information of the aircraft is obtained according to the geographic coordinates of the reference image.
6. The method of claim 4, wherein: In step 3, the points in the reference image are mapped to the world coordinate system, the 2D points in the real-time image are corresponded to the 3D points in the world coordinate system, and then the PNP pose solving method is used to calculate the accurate position and attitude information of the camera. Since the installation relationship between the camera and the aircraft is known, the heading and position information of the aircraft is obtained.
7. The method of claim 5, wherein: In step 3, the specific process is as follows: The reference image I and the real-time image are divided into multiple sub-regions, and the ground truth homography transformation between the real-time image and the reference image is denoted as , the estimated homography transformation of the sub-region is , and the loss function is L The calculation formula is as follows: In the above formula, is the number of block regions, is the sub-region is the set of control points, is the image coordinates of the control points; is the sub-region is the translation relative to the upper left corner of the original image; obtained by the matching network After the affine transformation estimation of the sub-region is obtained, the corresponding relationship between all control points in the real-time image and the reference image is obtained, and the global homographic transformation is estimated using the RANSAC algorithm , so as to realize accurate matching and obtain accurate position and heading information.
8. The method of claim 6, wherein: In step 4, the specific process is as follows: pose error , velocity error , position error , gyroscope bias , and accelerometer bias are defined as the state vector as follows: represents a position obtained from image matching, represents a position obtained from an inertial navigation system, the position error of the inertial navigation being: denotes the heading angle obtained from image matching, denotes the heading angle obtained from the inertial navigation system, the heading error of the inertial navigation is The error in the above equation is the Euler angle error, while the heading error in the state vector is the mathematical platform error; let denote the heading angle, pitch angle, and roll angle of the aircraft, respectively, then the coordinate transformation matrix from the body coordinate system to the navigation coordinate system is given by Using denotes the row column of elements, then the observation equation for the heading is given by: Combining the above formula, the measurement model of data fusion is obtained as: Using the obtained measurement model, the high-frequency horizontal position information obtained in step 2 and the accurate position and heading information obtained in step 3 are fused to obtain the final position and heading estimation.
9. An electronic device, comprising: It comprises: A processor and a memory; The memory stores instructions executable by the processor; The processor is configured to execute the instructions, so that the electronic device implements the method of any one of claims 1-7.
10. A readable storage medium, characterized by, The readable storage medium comprises software instructions; When the software instructions run in the electronic device, the electronic device implements the method of any one of claims 1-7.