Unmanned aerial vehicle integrated navigation method based on visual geographic positioning and odometer / IMU fusion

Abstract

The invention discloses an unmanned aerial vehicle integrated navigation method based on visual geographic positioning and odometer / IMU fusion, and relates to unmanned aerial vehicle positioning. Comprising the steps of defining a coordinate system and converting the coordinate system; establishing a remote sensing image database; a step of visual geographic positioning; a relative motion compensation step; and a factor graph fusion positioning step. Extracting a remote sensing image of a corresponding area from the remote sensing image database, and matching the remote sensing image with the image shot by the unmanned aerial vehicle to obtain visual geographic positioning; converting a laser radar (or visual) odometer coordinate system into a visual geographic positioning coordinate system; compensating the obtained visual geographic positioning according to the relative motion of a laser radar (or visual) odometer; a factor graph optimization method is adopted to fuse visual geographic positioning, a speedometer and an IMU to obtain optimal pose estimation. The problem that the unmanned aerial vehicle does not depend on GNSS for positioning is solved, the reliability of navigation positioning of the unmanned aerial vehicle is improved, economy and reliability are achieved, and precision is guaranteed.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle (UAV) positioning, and in particular to a UAV combined navigation method based on the fusion of visual geographic positioning and odometer / IMU. Background Art

[0002] With the continuous development of drone technology, drones have been increasingly used in intelligence reconnaissance, search and rescue, land surveying and environmental monitoring, etc. Accurate positioning of drones is essential for the effective execution of these functions. At present, drone positioning technology mainly uses satellite navigation system GNSS, visual odometer positioning and lidar odometer positioning. Although GNSS can provide high positioning accuracy, in actual flight missions, due to the influence of unpredictable factors such as high-rise building occlusion or electromagnetic interference, GNSS signals are easily interfered or lost, thus affecting drone positioning. Visual odometers and lidar odometers can provide relatively accurate drone positioning in a short time and short distance, but as the drone flight time and flight distance increase, both visual odometers and lidar odometers will inevitably experience positioning drift.

[0003] Visual navigation and positioning technology based on visual geolocation and remote sensing images can solve the above problems. As a passive sensor, the camera obtains information by receiving ground signals without relying on external systems, thus avoiding the shortcomings associated with GNSS. And because the positioning information obtained by the visual geolocation system through image matching is absolute positioning information, it solves the positioning drift of the visual odometer and the lidar odometer over a long period of time and long distance. However, since the positioning of visual geolocation is discrete positioning, in order to achieve continuous positioning of the drone, it is also necessary to fuse the positioning information of other sensors to achieve continuous positioning output. Summary of the invention

[0004] The purpose of the present invention is to address the problem of GNSS positioning failure of drones in complex scenarios and the inevitable positioning drift when using visual odometers and lidar odometers alone. The present invention provides a drone positioning method based on visual geolocation, lidar (or visual) odometer and IMU fusion. The method first converts the positioning information obtained by the lidar (or visual) odometer into a visual geolocation coordinate system through coordinate system conversion; then constructs a remote sensing image database; then matches the pictures taken by the drone with the remote sensing image database to obtain visual geolocation; finally, the visual geolocation and the converted lidar (or visual) odometer positioning information are fused with the IMU using a factor graph optimization method to obtain continuous positioning information.

[0005] A UAV integrated navigation method based on visual geolocation and odometer / IMU fusion includes the following steps:

[0006] 1) A method for establishing a remote sensing image database: Using a two-stage remote sensing image segmentation method based on Nona-Tree, the original remote sensing image is segmented into image blocks of 1024×1024 pixels, and further divided into sub-image blocks of 512×512 pixels to form an extended remote sensing image database;

[0007] 2) A visual geolocation method: A two-stage multi-scale scene matching method based on graph neural network SuperPoint & SuperGLUE is used to realize UAV visual geolocation;

[0008] 3) A method for relative motion compensation of visual geolocation: using the lidar odometer to calculate the relative displacement between the start timestamp of visual geolocation and the current timestamp, and to compensate the visual geolocation result;

[0009] 4) A method for fusing visual geolocation, odometer and IMU based on factor graph: define the state sequence, including position, orientation, accelerometer random walk noise and gyroscope random walk noise; construct visual geolocation factor residual, odometer factor residual and IMU pre-integration factor residual; construct factor graph optimization objective function, use iSAM2 optimization library for state estimation, and obtain the positioning result of integrated navigation.

[0010] In step 1), the method for establishing a remote sensing image database comprises the following specific steps:

[0011] (1.1) Using a two-stage remote sensing image segmentation method based on Nona-Tree;

[0012] (1.2) Segment the original remote sensing image into 1024×1024 pixel remote sensing images using a step size of 1024 pixels and no overlap;

[0013] (1.3) Expand a set of 4×4 remote sensing images using the Nona-Tree method;

[0014] (1.4) After creating the extended remote sensing image database with 1024×1024 pixels, each of these map blocks is further divided into remote sensing images of 512×512 pixels;

[0015] (1.5) Each group of 4×4 remote sensing images (size is 512×512 pixels) is expanded using the Nona-Tree method to obtain the final remote sensing image database.

[0016] In step 2), the visual geolocation method comprises the following two stages:

[0017] (2.1) The first stage realizes multi-scale image matching of remote sensing images and UAV images at i groups of different resolutions, and splices the matching results of the three groups of images. The specific steps are as follows:

[0018] (2.1.1) According to the initial positioning and fusion positioning, the corresponding remote sensing image is retrieved from the remote sensing image database, and the image taken by the camera on the drone is obtained, and the two images are synchronously scaled to i groups of resolutions of different scales [ ],[ ] represents i groups of different scaling scales for the length and width of the original image; the value range of i can be 1 to 5;

[0019] (2.1.2) The scaled i groups of images are sent to the graph neural network for image matching, and the pixel coordinate sets of i groups of image matching points at different scales are obtained [ , ];

[0020] (2.1.3) Map the pixel coordinates of the matching point pairs obtained at different scales back to the original image. The mapping formula is:

[0021]

[0022] in, represents the matched keypoints in the image mapped back to its original size, Represents the matched key points in the image mapped back to the original size Axis coordinates and Axis coordinates, and Respectively represent the Axis coordinates and Axis coordinates; After the pixel coordinate sets of the three sets of matching point pairs of different scales are mapped back to the original image, the three sets are spliced ​​to obtain the key point pair matching results of the first stage [ , ].

[0023] (2.2) The second stage implements clustering and cropping of the most relevant areas of the remote sensing image and the drone image, performs image matching again, and splices them with the image matching results of the first stage. Finally, the homography matrix and the positioning of the drone are calculated based on the matching results. The specific steps are as follows:

[0024] (2.2.1) According to the mutually matched key point pairs obtained in the first stage [ , ], the DBSCAN method is used to cluster and obtain the area with the highest concentration of key point pairs, that is, the area where the UAV image and the satellite image are most similar. The area is framed with a bbox and the bbox area is cropped to obtain the most locally relevant UAV image and satellite image;

[0025] (2.2.2) The locally most relevant UAV image and satellite image are sent to the graph neural network again for image matching to obtain the second image matching pixel coordinate set [ , ], and map the second image matching pixel coordinate set back to the original image to obtain the key point pair matching result of the second stage [ , ];

[0026] (2.2.3) Match the image results of the first stage with those of the second stage [ , ]and[ , ] to obtain the final key point matching results of the UAV image and the satellite image. , ];

[0027] (2.2.4) According to the key point matching results [ , ] Calculate the change relationship from the pixel plane of the drone image to the pixel plane of the satellite map block, that is, the homography matrix , the homography matrix is ​​calculated by the robust method based on RANSAC. According to experience, the RANSAC reprojection error is set to 5 and the number of iterations is set to 1000. After obtaining the homography matrix from the pixel coordinate system of the drone image to the pixel coordinate system of the remote sensing image, the pixel coordinates of the drone on the satellite image can be directly calculated;

[0028] (2.2.5) The latitude and longitude coordinates of the drone can be calculated from the pixel coordinates of the drone on the satellite image using the following formula:

[0029]

[0030]

[0031] in,( , ) represents the latitude and longitude coordinates of the upper left corner of the satellite map, ( , ) represents the latitude and longitude coordinates of the lower right corner of the remote sensing image, , Indicates the width and height of the satellite map. , Represents the pixel coordinates of the drone on the remote sensing image.

[0032] In step 3), the method for relative motion compensation of visual geolocation comprises the following specific steps:

[0033] (3.1) Use the LiDAR odometer to calculate the relative displacement between the start timestamp of visual geolocation and the current timestamp, and compensate for visual geolocation: The visual geolocation module receives the image Frame, Save LiDAR odometry positioning of frames ; After obtaining the output of the visual geolocation module Frame, based on the positioning information output by the visual geolocation module Timestamp in turn up Frame-Saved LiDAR Odometry Positioning , based on the current LiDAR odometry positioning of frames , the method to calculate the relative displacement is as follows:

[0034]

[0035] (3.2) Calculate the visual geolocation of the current frame:

[0036]

[0037] in, The visual geolocation module outputs the location of the key frame of the image received by the visual geolocation module, Represents the output positioning of the visual geolocation module in the current frame.

[0038] In step 4), the method of visual geographic positioning, odometer and IMU based on factor graph fusion comprises the following specific steps:

[0039] (4.1) The state sequence is defined as follows:

[0040]

[0041]

[0042] The state sequence from frame i to frame j includes the motion state of the nth frame consisting of four state variables: position ,direction , accelerometer random walk noise and gyroscope random walk noise ;

[0043] (4.2) Constructing the visual geolocation factor residual as follows:

[0044]

[0045] in, represents the observations for visual geolocation, Indicates the position after visual geolocation compensation;

[0046] (4.3) Construct odometry factor residual and IMU pre-integration factor residual ,in, represents the odometer observation, Represents the observation of IMU;

[0047] (4.4) The factor graph optimization objective function is constructed as follows:

[0048]

[0049] Among them, argmin{.} is the marginalized prior residual term, IMU pre-integration factor residual term, lidar odometer factor residual term, and visual geographic positioning factor residual term (the subscripts are various covariance matrices); in order to solve the above optimization problems, the iSAM2 optimization library is used for state estimation to obtain the positioning result of the integrated navigation.

[0050] The present invention aims at the situation that the current UAVs are highly dependent on GNSS for positioning, and the UAVs cannot be positioned and cannot operate normally due to the loss of GNSS under special conditions. The present invention provides a UAV positioning method based on visual geolocation, laser radar (or visual) odometer and IMU fusion suitable for small and medium-sized UAVs. First, the remote sensing image of the corresponding area is taken from the remote sensing image database, and the visual geolocation is obtained by matching with the image taken by the UAV; then the laser radar (or visual) odometer coordinate system is converted to the visual geolocation coordinate system; then the obtained visual geolocation is compensated for relative motion according to the laser radar (or visual) odometer; finally, the factor graph optimization method is used to fuse the visual geolocation, odometer and IMU to obtain the optimal pose estimation.

[0051] The present invention achieves high-precision positioning of UAVs in complex scenarios by integrating data from multiple sensors such as visual geolocation, LiDAR (or visual) odometer and IMU. Visual geolocation technology uses high-resolution remote sensing images and advanced image matching algorithms to provide accurate location information; while LiDAR (or visual) odometer and IMU provide data directly related to the motion state of the UAV, further enhancing the accuracy and stability of positioning. The multi-sensor fusion method significantly improves the positioning accuracy of UAVs in complex environments. The addition of LiDAR (or visual) odometer and IMU provides additional motion information for UAVs, enhancing the robustness and reliability of the system. Even when the GNSS signal is completely lost, the UAV can achieve continuous and stable positioning through this solution.

[0052] The present invention can be applied to a variety of complex scenes and terrain conditions, including urban canyons, mountainous areas, forests, etc. Visual geolocation technology is not affected by terrain and weather, and can perform accurate measurements in various environments; while the laser radar (or visual) odometer and IMU provide data directly related to the motion state of the drone, allowing the drone to better adapt to complex environments. Through the present invention, the drone can plan the optimal path based on real-time location information and mission requirements to improve flight efficiency. At the same time, high-precision positioning information also helps the drone to detect and avoid obstacles in time during flight, reduce collision risks, and improve flight safety.

[0053] The present invention has the characteristics of being fast, stable and reliable, solves the problem that the UAV does not rely on GNSS for positioning, improves the reliability of UAV navigation and positioning, and provides an economical, reliable and precision-guaranteed UAV combined navigation method. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 It is a schematic diagram of the overall structure of an embodiment of the present invention.

[0055] Figure 2 This is a schematic diagram of Nona-Tree.

[0056] Figure 3 Schematic diagram of visual geolocation.

[0057] Figure 4 This is a real picture of the drone of Example 1.

[0058] Figure 5 This is a test scene diagram of Example 1.

[0059] Figure 6 This is a diagram of the experimental results of Example 1. DETAILED DESCRIPTION

[0060] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the following embodiments will further illustrate the present invention in conjunction with the accompanying drawings. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. On the contrary, the present invention covers any substitution, modification, equivalent method and scheme made on the essence and scope of the present invention as defined by the claims.

[0061] The present invention provides an embodiment of a UAV integrated navigation method based on visual geographic positioning and odometer / IMU fusion, which includes the following five parts. The overall structure can be seen in Figure 1 .

[0062] (1) Coordinate system definition and coordinate system conversion

[0063] The camera is installed in the center of the drone, pointing vertically downward, and the visual geolocation coordinate system is defined as , the laser radar (or visual) odometer output positioning coordinate system is defined as . 3D pose of the drone express The position coordinates of the drone at this moment in the visual geopositioning coordinate system.

[0064] Define LiDAR (or visual) odometry in The output position of the moment is:

[0065]

[0066] Define the visual geolocation module in The output position of the moment is:

[0067]

[0068] Since the laser radar (or visual) odometry and visual geolocation modules only exist in the output location The rigid rotation about the axis, so the rotation matrix is ​​defined as:

[0069]

[0070] In the formula, Represents the angle between the LiDAR (or visual) odometry and the visual geolocation module coordinate system.

[0071] The rotation matrix from the odometer positioning coordinate system to the visual geolocation coordinate system can be obtained by solving the following least squares method: :

[0072]

[0073] In the formula, For LiDAR (or visual) odometry Output positioning at the moment; For the visual geolocation module Output positioning at the moment; represents the L2 norm;

[0074] (2) Construction of remote sensing image database

[0075] In order to achieve large-scale visual geolocation in an environment with limited GPS signals, it is hoped that the remote sensing image database can cover as large an area as possible. A two-stage remote sensing image segmentation method based on Nona-Tree is used to create a remote sensing image database with extensive coverage and efficiency. First, the original remote sensing image is segmented into multiple 1024×1024 pixel remote sensing images using a step size of 1024 pixels and no overlap to ensure that each image block contains sufficient information for subsequent visual geolocation; then, a set of 4×4 1024×1024 pixel remote sensing images is expanded using the Nona-Tree method to create an expanded remote sensing image database with 1024×1024 pixels. Each of these map blocks is further divided into 512×512 pixel remote sensing images. After segmentation, each set of 4×4 512×512 pixel remote sensing images is expanded using the Nona-Tree method to obtain the final remote sensing image database. The details of the Nona-Tree image segmentation method can be found in Figure 2 By expanding and re-segmenting the Nona-Tree method, image information can be used more effectively, improving the coverage and retrieval accuracy of the image database. At the same time, smaller image blocks are also convenient for subsequent image matching and feature extraction.

[0076] (3) Visual Geolocation Module

[0077] The visual geolocation module uses a two-stage multi-scale image matching network based on the graph neural network SuperPoint & SuperGLUE to achieve multi-scale image matching of remote sensing images and drone images. Its specific structure can be found in Figure 3 .

[0078] 1) The first stage: realize multi-scale image matching of remote sensing images and UAV images at three different resolutions, and stitch the matching results of the three sets of images;

[0079] Take out the remote sensing images corresponding to the initial positioning and fusion positioning from the remote sensing image database, and obtain the images taken by the camera on the drone, and scale the two images synchronously to i groups of resolutions of different scales [ ],in,[ ] represents the scaling of the length and width of the original image; the value of i ranges from 1 to 5; in this embodiment, the value of i is 3, and the two images are synchronously scaled to three different resolutions. ],[ ],[ ].

[0080] The scaled i groups of images are sent to the graph neural network SuperPoint & SuperGLUE for image matching, and the pixel coordinates of the i groups of image matching points at different scales are obtained. , ],[ , ] respectively represent the pixel coordinate sets of the matching point pairs detected on the drone image and the remote sensing image. In this embodiment, the three groups of scaled images are respectively sent to the graph neural network SuperPoint & SuperGLUE for image matching, and three groups of image matching point pair pixel coordinate sets at different scales are obtained, which are respectively represented as [ , ],[ , ],[ , ].

[0081] Since the image has been scaled, the pixel coordinate sets of the matching point pairs obtained at different scales need to be remapped back to the original image size. The remapping formula is:

[0082]

[0083] in, represents the matched keypoints in the image remapped back to its original size, Represents the matched key points in the image mapped back to the original size Axis coordinates and Axis coordinates, and Respectively represent the Axis coordinates and Axis coordinates

[0084] After the pixel coordinate sets of the three sets of matching point pairs of different scales are mapped back to the original image, the three sets are concatenated to obtain the key point pair matching results of the first stage [ , ].

[0085] 2) The second stage: clustering and cropping the most relevant areas of the remote sensing image and the drone image, performing image matching again, and splicing them with the image matching results of the first stage. Finally, the homography matrix and the positioning of the drone are calculated based on the matching results.

[0086] According to the mutually matched key point pairs obtained in the first stage [ , ], the DBSCAN method (density-based clustering algorithm) is used to cluster the area with the highest concentration of key point pairs, that is, the area where the UAV image and the satellite image are most similar. The area is framed with a bounding box bbox and the bbox area is cropped to obtain the most locally relevant UAV image (UAV image) and satellite image.

[0087] Afterwards, the cropped local most relevant UAV image and satellite image are sent to the graph neural network again for image matching, and the second image matching pixel coordinate set is obtained [ , ], and map the second image matching pixel coordinate set back to the original image to obtain the key point pair matching result of the second stage [ , ].

[0088] Finally, the image matching results of the first stage and the second stage are [ , ]and[ , ] to obtain the final key point matching results of the UAV image and the satellite image. , ].

[0089] After obtaining the final feature point matching result [ , ], the change relationship from the pixel plane of the drone image to the pixel plane of the satellite map block is calculated according to the key point matching results, that is, the homography matrix between the pixel plane of the drone image and the satellite map block is calculated, where the calculation method of the homography matrix H is:

[0090]

[0091] in, represents the value of the homography matrix H, represents the key points matched on the drone, Represents the key points matched on the satellite image.

[0092] The homography matrix is ​​calculated using a robust method based on RANSAC, the RANSAC reprojection error is set to 5, and the number of iterations is set to 1000. After obtaining the homography matrix from the pixel coordinate system of the drone image to the pixel coordinate system of the remote sensing image, the pixel coordinates of the drone on the satellite image can be directly calculated. Since each stored remote sensing image block contains the latitude and longitude coordinates of the upper left and lower right ends, the latitude and longitude coordinates of the drone can be calculated from the pixel coordinates of the drone on the satellite image using the following formula:

[0093]

[0094]

[0095] in,( , ) represents the latitude and longitude coordinates of the upper left corner of the satellite map, ( , ) represents the latitude and longitude coordinates of the lower right corner of the remote sensing image, , Indicates the width and height of the satellite map. , Represents the pixel coordinates of the drone on the remote sensing image.

[0096] Through the above two-stage method, the matching of UAV images and satellite remote sensing images and the positioning of UAVs are effectively achieved. In the first stage, candidate key point pairs are obtained through preliminary matching. In the second stage, the matching accuracy is improved through clustering, cropping and re-matching. Finally, the homography matrix and the longitude and latitude coordinates of the UAV are calculated.

[0097] (4) Relative motion compensation

[0098] When visual geolocation is used to calculate the absolute positioning of the drone, the drone continues to fly, resulting in a time lag between the start timestamp of visual geolocation and the current timestamp. To solve this problem, the lidar odometry is used to calculate the relative displacement between the start timestamp of visual geolocation and the current timestamp to compensate for the visual geolocation.

[0099] The visual geolocation module receives the image Frame, Save LiDAR odometry positioning of frames After obtaining the output of the visual geolocation module Frame, based on the positioning information output by the visual geolocation module Timestamp in turn up Frame-Saved LiDAR Odometry Positioning , based on the current LiDAR odometry positioning of frames , the method to calculate the relative displacement is as follows:

[0100]

[0101] The visual geolocation of the current frame is then calculated:

[0102]

[0103] in, The visual geolocation module outputs the location of the key frame of the image received by the visual geolocation module, Represents the output positioning of the visual geolocation module in the current frame.

[0104] (5) Factor graph optimization fusion positioning

[0105] In order to improve the accuracy and stability of drone positioning, the factor graph optimization method is used to integrate visual geolocation, lidar odometer positioning and IMU. For the optimization sequence, the state sequence is defined as follows:

[0106]

[0107]

[0108] The state sequence from frame i to frame j includes the motion state of the nth frame consisting of four state variables: position ,direction , accelerometer random walk noise and gyroscope random walk noise .

[0109] IMU pre-integration factor residual The construction is as follows:

[0110]

[0111] in, represents the IMU observation, The Jacobian matrix representing the pre-integrated terms of gyroscope velocity, position, and attitude, The Jacobian matrix representing the accelerometer velocity and position pre-integration terms, Represents the external parameters of IMU, Represents the random walk error of the gyroscope and accelerometer.

[0112] LiDAR odometer LIO factor residual The build is as follows:

[0113]

[0114] in, represents the odometer observation, Indicates the change in the position and attitude of the LIO. Similarly, the visual odometry factor can be constructed.

[0115] Visual geolocation factor residuals The build is as follows:

[0116]

[0117] in, represents the observations for visual geolocation, Represents the position after visual geolocation compensation.

[0118] The factor graph optimization objective function is constructed as follows:

[0119]

[0120] In argmin{.}, there are the marginalized prior residual term, the IMU pre-integration factor residual term, the lidar odometer factor residual term, and the visual geolocation factor residual term (the subscripts are various covariance matrices).

[0121] A specific embodiment is given below.

[0122] Embodiment 1: The present invention is based on the positioning method of UAV visual geographic positioning, laser radar odometer and IMU, which is used for navigation and positioning of UAV in outdoor environment under GNSS denial. The specific implementation operation process is as follows.

[0123] Step 1: Use the Wingsun-X4 drone as the experimental platform. Figure 4 As shown, the equipment, sensors and parameters used are shown in Table 1.

[0124] Table 1

[0125]

[0126] Step 2: According to the method and algorithm in the invention content, use NVIDIA Jetson Orin NX as the host computer of the drone (Linux system and ROS environment) and follow the following steps to implement programming.

[0127] Step 3: Define the laser radar odometer coordinate system, the drone initial coordinate system, and the visual geolocation coordinate system;

[0128] Step 4: Start the LiDAR odometer, which outputs the positioning in the LiDAR odometer coordinate system at a frequency of 20 Hz. At the same time, the LiDAR coordinate system is converted to the world coordinate system based on the initial positioning.

[0129] Step 5: Start the visual geolocation module. The steps of the visual geolocation module are as follows:

[0130] Step 5.1: Based on the initial positioning information of the drone, obtain the remote sensing map of the corresponding area from the remote sensing image database , sent to the visual geolocation module, the image acquired by the camera at the current moment Feed into the visual geolocation module

[0131] Step 5.2: Save the current laser radar odometer positioning information

[0132] Step 5.3: Send the remote sensing image to the visual geolocation module and drone camera images Send it to the first stage of the two-stage multi-scale image matching network to obtain the key point pair matching results of the first stage [ , ]

[0133] Step 5.4: Match the key points according to the first stage. , ], the DBSCAN method is used to cluster the key point pairs to obtain the area with the highest degree of mutual matching, and the area is cropped in the UAV image and remote sensing image to obtain the local remote sensing image and local drone camera images ;

[0134] Step 5.5: Local remote sensing image and local drone camera images The key point pair matching results of the second stage are obtained by sending it to the second stage of the image matching network. , ];

[0135] Step 5.6: Match the image results of the first stage with those of the second stage [ , ]and[ , ] to obtain the final key point matching result of the drone camera image and the remote sensing image [ , ];

[0136] Step 5.7: According to the key point matching results of the final drone camera image and remote sensing image [ , ], calculate the homography matrix between the drone camera image and the remote sensing image ;

[0137] Step 5.8: According to the homography matrix Calculate the latitude and longitude of the drone in the remote sensing image and output the latitude and longitude positioning ;

[0138] Step 6: According to the positioning information of the lidar odometer saved by the visual geolocation module And the positioning information of the lidar odometer at the current moment , calculate the output location of the visual geolocation module at the current moment ;

[0139] Step 7: The current laser radar odometer positioning information , the output location of the visual geolocation module at the current moment And the IMU uses the factor graph optimization method to perform fusion to obtain the fused UAV positioning;

[0140] Test environment such as Figure 5 As shown, the drone first flies in a straight line to the specified location, and then flies in a rectangular shape to collect data. The flight scene is mainly an outdoor park scene.

[0141] Test results such as Figure 6 As shown in the figure, red represents the true value of positioning, yellow represents the positioning of the visual geolocation module, and blue represents the fused positioning. It can be seen that the positioning of the visual geolocation module is not much different from the true value of positioning, and the fused positioning further enhances the positioning accuracy and real-time performance of the drone.

[0142] The above embodiments are only preferred embodiments of the present invention and cannot be considered to limit the scope of the present invention. All equivalent changes and improvements made within the scope of the present invention should still fall within the scope of the present invention.

Claims

1. A UAV integrated navigation method based on visual geolocation and odometer / IMU fusion, characterized in that The following steps are involved: 1) A method for establishing a remote sensing image database: Using a two-stage remote sensing image segmentation method based on Nona-Tree, the original remote sensing image is segmented into image blocks of 1024×1024 pixels, and further divided into sub-image blocks of 512×512 pixels to form an extended remote sensing image database; 2) A visual geolocation method: A two-stage multi-scale scene matching method based on graph neural network SuperPoint & SuperGLUE is used to realize UAV visual geolocation. In the first stage, candidate key point pairs are obtained through preliminary matching. In the second stage, the matching accuracy is improved through clustering, cropping and re-matching, and finally the homography matrix and the longitude and latitude coordinates of the UAV are calculated. 3) A method for relative motion compensation of visual geolocation: using the lidar odometer to calculate the relative displacement between the start timestamp of visual geolocation and the current timestamp, and to compensate the visual geolocation result; 4) A method for fusing visual geolocation, odometer and IMU based on factor graph: define the state sequence, including position, orientation, accelerometer random walk noise and gyroscope random walk noise; construct visual geolocation factor residual, odometer factor residual and IMU pre-integration factor residual; construct factor graph optimization objective function, use iSAM2 optimization library for state estimation, and obtain the positioning result of integrated navigation.

2. A UAV integrated navigation method based on visual geographic positioning and odometer / IMU fusion as claimed in claim 1, characterized in that In step 1), the method for establishing a remote sensing image database comprises the following specific steps: (1.1) Using a two-stage remote sensing image segmentation method based on Nona-Tree; (1.2) Segment the original remote sensing image into 1024×1024 pixel remote sensing images using a step size of 1024 pixels and no overlap; (1.3) Expand a set of 4×4 remote sensing images using the Nona-Tree method; (1.4) After creating the extended remote sensing image database with 1024×1024 pixels, each of these map blocks is further divided into remote sensing images of 512×512 pixels; (1.5) Each group of 4×4 remote sensing images is expanded using the Nona-Tree method to obtain the final remote sensing image database; the size of each remote sensing image is 512×512 pixels.

3. A UAV integrated navigation method based on visual geographic positioning and odometer / IMU fusion as claimed in claim 1, characterized in that In step 2), the visual geolocation method comprises the following two stages: (2.1) In the first stage, multi-scale image matching of remote sensing images and UAV images at i different resolutions is achieved, and i groups of image matching results are stitched together; (2.2) The second stage implements clustering and cropping of the most relevant areas of the remote sensing image and the UAV image, performs image matching again, and splices them with the image matching results of the first stage. Finally, the homography matrix and the positioning of the UAV are calculated based on the matching results.

4. A UAV integrated navigation method based on visual geographic positioning and odometer / IMU fusion as claimed in claim 3, characterized in that In step (2.1), the first stage includes the following specific steps: (2.1.1) According to the initial positioning and fusion positioning, the corresponding remote sensing image is retrieved from the remote sensing image database, and the image taken by the camera on the drone is obtained, and the two images are synchronously scaled to i groups of resolutions of different scales [ ]; (2.1.2) The scaled i groups of images are sent to the graph neural network for image matching, and the pixel coordinate sets of i groups of image matching points at different scales are obtained [ , ];in,[ , ] represent the pixel coordinate sets of matching point pairs detected on the UAV image and the remote sensing image respectively; (2.1.3) Map the pixel coordinates of the matching point pairs obtained at different scales back to the original image. The mapping formula is: in, represents the matched keypoints in the image mapped back to its original size, Represents the matched key points in the image mapped back to the original size Axis coordinates and Axis coordinates, and Respectively represent the Axis coordinates and Axis coordinates; After the pixel coordinate sets of i groups of matching point pairs of different scales are mapped back to the original image, the i groups are concatenated to obtain the key point pair matching results of the first stage [ , ].

5. A UAV integrated navigation method based on visual geographic positioning and odometer / IMU fusion as claimed in claim 3, characterized in that In step (2.2), the second stage includes the following specific steps: (2.2.1) According to the mutually matched key point pairs obtained in the first stage [ , ], the DBSCAN method is used to cluster and obtain the area with the highest concentration of key point pairs, that is, the area where the UAV image and the satellite image are most similar. The area is framed with a bbox and the bbox area is cropped to obtain the most locally relevant UAV image and satellite image; (2.2.2) The locally most relevant UAV image and satellite image are sent to the graph neural network again for image matching to obtain the second image matching pixel coordinate set [ , ], and map the second image matching pixel coordinate set back to the original image to obtain the key point pair matching result of the second stage [ , ]; (2.2.3) Match the image results of the first stage with those of the second stage [ , ]and[ , ] to obtain the final key point matching results of the UAV image and the satellite image. , ]; (2.2.4) According to the key point matching results [ , ] Calculate the change relationship from the pixel plane of the drone image to the pixel plane of the satellite map block, that is, the homography matrix , the homography matrix is ​​calculated using a robust method based on RANSAC. If the RANSAC reprojection error of a point is less than the set threshold, the point is considered to be a correctly matched point; it is iterated multiple times, and each time the candidate homography matrix with the largest number of correctly matched points is recorded as the final homography matrix; after obtaining the homography matrix from the pixel coordinate system of the drone image to the pixel coordinate system of the remote sensing image, the pixel coordinates of the drone on the satellite image are directly calculated; (2.2.5) The latitude and longitude coordinates of the drone can be calculated from the pixel coordinates of the drone on the satellite image using the following formula: in,( , ) represents the latitude and longitude coordinates of the upper left corner of the satellite map, ( , ) represents the latitude and longitude coordinates of the lower right corner of the remote sensing image, , Indicates the width and height of the satellite map. , Represents the pixel coordinates of the drone on the remote sensing image.

6. A UAV integrated navigation method based on visual geographic positioning and odometer / IMU fusion as claimed in claim 5, characterized in that In step (2.2.4), the threshold is set to 5 and the number of iterations is set to 1000.

7. The method for integrated navigation of unmanned aerial vehicles based on visual geographic positioning and odometer / IMU fusion as claimed in claim 1, characterized in that In step 3), the method for relative motion compensation of visual geolocation comprises the following specific steps: (3.1) Use the LiDAR odometer to calculate the relative displacement between the start timestamp of visual geolocation and the current timestamp, and compensate for visual geolocation: The visual geolocation module receives the image Frame, Save LiDAR odometry positioning of frames ; After obtaining the output of the visual geolocation module Frame, based on the positioning information output by the visual geolocation module Timestamp in turn up Frame-Saved LiDAR Odometry Positioning , based on the current LiDAR odometry positioning of frames , the method to calculate the relative displacement is as follows: (3.2) Calculate the visual geolocation of the current frame: in, The visual geolocation module outputs the location of the key frame of the image received by the visual geolocation module, Represents the output positioning of the visual geolocation module in the current frame.

8. The method for integrated navigation of unmanned aerial vehicles based on visual geographic positioning and odometer / IMU fusion as claimed in claim 1, characterized in that In step 4), the method of visual geolocation, odometer and IMU based on factor graph fusion comprises the following specific steps: (4.1) The state sequence is defined as follows: The state sequence from frame i to frame j includes the motion state of the nth frame consisting of four state variables: position ,direction , accelerometer random walk noise and gyroscope random walk noise ; (4.2) Constructing the visual geolocation factor residual as follows: in, represents the observations for visual geolocation, Indicates the position after visual geolocation compensation; (4.3) Construct odometry factor residual and IMU pre-integration factor residual ,in, represents the odometer observation, Represents the observation of IMU; (4.4) The factor graph optimization objective function is constructed as follows: Among them, argmin{.} is the marginalized prior residual term, IMU pre-integration factor residual term, lidar odometer factor residual term, and visual geographic positioning factor residual term, and the subscripts are various covariance matrices; the iSAM2 optimization library is used for state estimation to obtain the positioning result of the integrated navigation.

Images