A method and system for navigation of a high-altitude unmanned aerial vehicle based on vision and laser ranging

Abstract

The application provides a mid-high altitude unmanned aerial vehicle navigation method and system based on vision and laser ranging, a monocular image is combined with laser ranging to determine a key frame, accurate real scene scale information is acquired, feature tracking is carried out on a current frame and the key frame, relative horizontal displacement and covariance are calculated according to the position of a laser ranging point in the key frame and the feature tracking position of the laser ranging point in the current frame in the feature tracking, and combined with unmanned aerial vehicle POS data; relative heading angle and heading angle covariance are calculated according to the relative rotation angle of a feature block of the key frame and a feature block of the current frame after feature tracking; and the motion state estimation of the unmanned aerial vehicle is realized by combining the calculated results with an inner filtering algorithm of a flight control system. The method can be used in real-time operation on a micro unmanned aerial vehicle platform with limited processor computing power and limited camera resolution, and has the advantages of lightness, robustness and practicability.

Description

Technical Field

[0001] This invention belongs to the field of computer vision, and in particular relates to a navigation method and system for medium- and high-altitude unmanned aerial vehicles based on vision and laser ranging. Background Technology

[0002] The statements in this section are merely background information related to the present invention and do not necessarily constitute prior art.

[0003] Unmanned aerial vehicles (UAVs) possess flexibility and high stealth capabilities, making them highly suitable for reconnaissance missions in battlefield environments. Typically, micro-UAVs employing low-cost inertial navigation sensors achieve integrated navigation by fusing data from multiple sensors, including GNSS, IMU, magnetic compass, and barometer, due to the accuracy limitations of IMU sensors. However, GNSS signals are susceptible to interference in certain situations, such as obstruction by tall buildings or mountains, multipath propagation effects, and radio frequency interference from similar radio frequency bands. When GNSS is interfered with, the inherently high noise and low accuracy of low-cost inertial navigation devices can lead to significant cumulative errors in positioning data within a short period. In such cases, additional environmental perception methods are needed to enhance observational information and improve positioning accuracy.

[0004] In recent years, with the development of semiconductor technology and optical processes, visual sensor and lens technologies have become increasingly mature. When the GNSS signal of a drone is interfered with, the visual system deployed on the drone can theoretically process the images captured by the camera through a processor, and use visual odometry technology in SLAM (Simultaneous Localization and Mapping), with the assistance of sensors such as inertial navigation, to calculate the drone's motion information, providing flight control with navigation information such as speed and position, and enabling the drone to navigate and position for flight.

[0005] To address navigation challenges under GNSS interference, academia and industry have conducted extensive research on SLAM technology, particularly visual odometry. To fully leverage the advantages of multi-sensor fusion in UAVs, recent studies have focused on fusing visual, inertial navigation, and laser sensors to achieve more robust positioning. Taking visual-inertial odometry as an example, data fusion frameworks can be categorized into loosely coupled and tightly coupled methods. Loosely coupled methods involve separate state estimations from visual and inertial sensors, followed by fusion based on these estimates. This approach is computationally simpler but has lower accuracy. Tightly coupled methods, on the other hand, fuse observations from visual and inertial sensors using a unified filtering / optimization framework to directly obtain a single state estimate. This approach is computationally more complex but offers higher accuracy. However, in high-altitude UAV flight scenarios, we believe that most existing odometry methods suffer from the following limitations:

[0006] (1) Odometry initialization has limitations: When using visual-inertial navigation odometry in mid-to-high altitude scenarios, UAVs may encounter fog, fog, or feature-scarce scenarios, leading to prolonged lack of observation information and failure. Therefore, the function of re-initializing in mid-to-high altitude scenarios is required. The usual initialization method requires applying sufficient translational motion to the UAV. However, for multi-rotor UAV platforms, it is necessary to frequently perform stationary hovering tasks, but the translational motion conditions for initialization cannot be met during hovering.

[0007] (2) Lack of real-scale observation: In mid-to-high altitude scenarios, binocular vision systems will degenerate into monocular systems. Some monocular vision-inertial navigation odometry methods obtain scale observation information through IMU pre-integration methods. However, this method requires the UAV to perform a certain translational motion, and the vibration of the UAV platform will also affect the accuracy of the IMU, resulting in a large error in scale.

[0008] (3) Computing power limitations: UAV platforms, especially micro-UAV platforms used for reconnaissance, have significant limitations on endurance and payload, thus limiting the computing power of the computing unit. Most visual-inertial odometry methods require feature extraction, matching, and filtering operations at the front end, and feature extraction and matching are time-consuming. Furthermore, for visual-inertial odometry methods that are tightly coupled with inertial navigation, iterative optimization calculations of batch observation information are usually required at the back end, which also consumes a lot of time. Summary of the Invention

[0009] To overcome the shortcomings of the prior art, this invention provides a navigation method and system for medium- and high-altitude unmanned aerial vehicles (UAVs) based on vision and laser ranging. Monocular images are combined with laser ranging and feature tracking to perform relevant calculations for UAV navigation. This invention has low computational requirements, is robust and practical, and can adapt to flight missions such as flight and hovering of small multi-rotor UAV platforms at medium and high altitudes.

[0010] To achieve the above objectives, a first aspect of the present invention provides a mid-to-high altitude unmanned aerial vehicle (UAV) navigation method based on vision and laser ranging, comprising:

[0011] In monocular images acquired by UAVs, key frames are determined based on the acquisition time of laser ranging, and feature tracking of the current frame is performed based on motion compensation according to the key frames.

[0012] Based on the UAV POS data, the position of the laser ranging point in the keyframe, and the feature tracking position of the laser ranging point in the current frame, calculate the horizontal displacement and horizontal displacement covariance between the current frame and the keyframe.

[0013] Based on the UAV POS data, the feature blocks of the key frame, and the relative rotation angle of the feature blocks of the current frame after corresponding feature tracking, calculate the heading offset and heading offset covariance between the current frame and the key frame.

[0014] The obtained horizontal displacement and horizontal displacement covariance information, as well as heading offset and heading offset covariance information, are used as the observation information of the flight control Kalman filter framework. The inertial navigation and visual observation information are fused by the flight control Kalman filter algorithm to obtain the UAV navigation information.

[0015] A second aspect of the present invention provides a mid-to-high altitude unmanned aerial vehicle (UAV) navigation system based on vision and laser ranging, comprising:

[0016] The feature tracking module is configured to: determine key frames in the monocular image acquired by the UAV based on the laser ranging acquisition time, and perform feature tracking on the current frame based on motion compensation according to the key frames;

[0017] The displacement calculation module is configured to calculate the horizontal displacement and horizontal displacement covariance between the current frame and the key frame based on the UAV POS data, the position of the laser ranging point in the key frame, and the feature tracking position of the laser ranging point in the current frame.

[0018] The heading calculation module is configured to: calculate the heading offset and heading offset covariance between the current frame and the key frame based on the UAV POS data, the feature blocks of the key frame, and the relative rotation angle of the feature blocks of the current frame after corresponding feature tracking;

[0019] Navigation module: The obtained horizontal displacement and horizontal displacement covariance information, heading offset and heading offset covariance information are used as the observation information of the flight control Kalman filter framework. The inertial navigation and visual observation information are fused through the flight control Kalman filter algorithm to obtain the UAV navigation information.

[0020] A third aspect of the present invention provides a computer device comprising: a processor, a memory, and a bus, wherein the memory stores machine-readable instructions executable by the processor, and when the computer device is running, the processor communicates with the memory via the bus, and when the machine-readable instructions are executed by the processor, a mid-to-high altitude unmanned aerial vehicle navigation method based on vision and laser ranging is executed.

[0021] A fourth aspect of the present invention provides a computer-readable storage medium storing a computer program that, when executed by a processor, performs a mid-to-high altitude unmanned aerial vehicle (UAV) navigation method based on vision and laser ranging.

[0022] The above one or more technical solutions have the following beneficial effects:

[0023] In this invention, monocular images are combined with laser ranging to determine keyframes, obtaining accurate real-world scene scale information. Feature tracking is performed on the current frame and keyframes. Based on the position of the laser ranging point in the keyframe and its feature-tracked position in the current frame, and combined with UAV POS data, the relative horizontal displacement and covariance are calculated. Based on the feature blocks of the keyframes and the relative rotation angle of the corresponding feature blocks in the current frame after feature tracking, the relative heading angle and heading angle covariance are calculated. The calculated results are then combined with a filtering algorithm within the flight control system to achieve UAV motion state estimation. This method can run in real-time on micro-UAV platforms with limited processor computing power and camera resolution, offering advantages such as lightweight design, robustness, and practicality.

[0024] In this invention, odometer calculation does not require initialization and is suitable for multi-rotor UAV platforms performing hovering tasks; the feature tracking method has higher robustness compared to general feature point extraction and matching methods and can be applied to lower resolution cameras and scenes with less texture.

[0025] Advantages of additional aspects of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0026] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0027] Figure 1 This is a schematic diagram of the system framework in Embodiment 1 of the present invention.

[0028] Figure 2 This is a schematic diagram of motion compensation between consecutive frames in Embodiment 1 of the present invention;

[0029] Figure 3 This is a schematic diagram of the translation calculation between the current frame and the key frame in Embodiment 1 of the present invention;

[0030] Figure 4 This is a schematic diagram of the heading calculation between the current frame and the key frame in Embodiment 1 of the present invention;

[0031] Figure 5 This is a schematic diagram of the workflow in Embodiment 1 of the present invention. Detailed Implementation

[0032] It should be noted that the following detailed descriptions are exemplary and intended to provide further illustration of the invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0033] It should be noted that the terminology used herein is for the purpose of describing particular implementations only and is not intended to limit the exemplary implementations of the present invention.

[0034] Where there is no conflict, the embodiments and features in the embodiments of the present invention can be combined with each other.

[0035] Example 1

[0036] This embodiment discloses a mid-to-high altitude UAV navigation method based on vision and laser ranging, including:

[0037] In monocular images acquired by UAVs, key frames are determined based on the acquisition time of laser ranging, and feature tracking of the current frame is performed based on motion compensation according to the key frames.

[0038] Based on the UAV POS data, the position of the laser ranging point in the keyframe, and the feature tracking position of the laser ranging point in the current frame, calculate the horizontal displacement and horizontal displacement covariance between the current frame and the keyframe.

[0039] Based on the UAV POS data, the feature blocks of the key frame, and the relative rotation angle of the feature blocks of the current frame after corresponding feature tracking, calculate the heading offset and heading offset covariance between the current frame and the key frame.

[0040] The obtained horizontal displacement and horizontal displacement covariance information, as well as heading offset and heading offset covariance information, are used as the observation information of the flight control Kalman filter framework. The inertial navigation and visual observation information are fused by the flight control Kalman filter algorithm to obtain the UAV navigation information.

[0041] The overall approach of the vision- and laser ranging-based mid-to-high altitude UAV navigation method proposed in this embodiment is as follows: Utilizing the attitude and barometer altitude information provided by the UAV and combined with laser ranging values, feature tracking of a monocular image sequence is used to calculate the 3-DOF odometry information (relative displacement, heading, and covariance) at real scale between two adjacent frames. Furthermore, the calculated odometry information is sent to the flight control system, where the position and heading observation information are fused using the flight control system's Kalman filter framework. To this end, an odometry framework with two real-time running threads is designed. The first thread includes keyframe selection, feature tracking, and calculation of relative displacement and covariance. During the calculation process, the feature extraction / matching process is replaced by feature block tracking. A suitable strategy is used to select keyframes, and feature blocks within these keyframes are directly tracked, significantly improving computational efficiency. Simultaneously, based on the principle of direct geolocation, the relative displacement between frames is calculated to provide position observation information. The second thread, based on the feature tracking results of the first thread (i.e., the feature blocks of the keyframes and the current frame), calculates the heading offset of the feature blocks using homography, polar coordinate transformation, and template matching methods, providing heading observation information.

[0042] In mid- / high-altitude scenarios, the binocular camera of a UAV's visual navigation system degenerates into a monocular camera due to limitations in baseline length and resolution. Since monocular cameras lack true spatial scale information, a small, lightweight single-point ranging LiDAR can be used to recover the scale. Because the UAV is far from the ground, LiDAR ranging is less affected by vegetation interference, and the LiDAR ranging values ​​can be considered accurate. The following reasonable assumptions are made:

[0043] (1) Local homography assumption, that is, the area near the laser projection point is a plane;

[0044] (2) The vast majority of surface features are static features.

[0045] Based on the above assumptions, theoretically, image tracking methods can be directly used to track feature blocks in the vicinity of the laser projection point, thereby replacing feature extraction and matching operations. This improves computational efficiency. Furthermore, we believe that feature block-based tracking methods, compared to feature point extraction and matching, are more adaptable to lower image resolutions and scenarios with scarcer textures, exhibiting higher robustness. Robustness can be further enhanced by leveraging UAV attitude information. Therefore, this method proposes using a monocular camera for feature extraction and tracking, integrating other sensors such as ranging lidar and barometers to construct a lightweight odometer. To facilitate the explanation of the principle, it is assumed that the monocular camera intrinsic parameters, laser-camera extrinsic parameters, and camera-aircraft extrinsic parameters in this embodiment have been calibrated, and that image, laser, aircraft attitude, and barometer data are synchronized. Additionally, due to the mid-to-high altitude scenario, the translational amounts in the laser-camera extrinsic parameters and camera-aircraft extrinsic parameters are ignored.

[0046] To achieve the goal of estimating (positioning) motion state under GNSS, the method in this embodiment adopts the following technical solution:

[0047] For motion state estimation, this embodiment employs a mechanism based on keyframe feature tracking. In keyframes, feature blocks are constructed centered on laser projection points. Using synchronized attitude and laser ranging information, the spatial 3D coordinates of the features are calculated using a direct geolocation method. In non-keyframes, robust tracking of features is performed using POS information, and new 3D coordinates are calculated using synchronized attitude and barometer altitude information, thereby calculating the displacement relative to the keyframe. Furthermore, based on the feature blocks of the keyframe and the current frame, feature matching in polar coordinates is performed to calculate the relative heading, thus enabling odometer calculation.

[0048] In this embodiment, a mid-to-high altitude UAV navigation method based on vision and laser ranging specifically includes:

[0049] S1. In the initial stage, key frames are selected using appropriate strategies, and feature tracking is performed between the current frame and the key frames. At the same time, motion compensation between frames is used to improve the robustness of feature tracking.

[0050] S2. Based on feature tracking, laser ranging, and POS information, calculate the displacement and covariance between the current frame and the key frame;

[0051] S3. Based on feature tracking, laser ranging, and POS information, calculate the heading offset and covariance between the current frame and the key frame;

[0052] S4. Data fusion with the UAV flight control terminal to obtain real-time navigation information.

[0053] Specifically, this method, based on reasonable assumptions about mid-to-high altitude scenarios, combines information from a monocular camera with information from a single-point ranging radar and UAV POS information (attitude, barometer altitude). On one hand, in the monocular image sequence, keyframes are selected using an appropriate selection strategy, and feature block selection is performed within these keyframes. Then, in subsequent non-keyframes, robust tracking of the feature blocks is performed based on inter-frame motion compensation and correlation filtering methods to calculate the horizontal displacement and covariance between the current frame and the keyframes. On the other hand, based on the feature tracking results of the current frame and the keyframes, regions of interest are selected for image processing, and the relative heading and covariance between the current frame and the keyframes are calculated to obtain three-degree-of-freedom odometry information.

[0054] In this embodiment, the feature tracking method includes:

[0055] First, in the keyframe, using the known extrinsic matrix and UAV POS information, the projection point of the laser measurement point on the image is calculated, and the NED coordinates of the laser measurement point are calculated using a direct geolocation method.

[0056]

[0057]

[0058] Where (u_l_ref, v_l_ref) are the pixel coordinates of the projection point of the laser measurement point on the image. Here, represents the NED coordinates of the laser measurement point within the keyframe, where the 'ref' suffix indicates a keyframe; the subscript 'i' represents the image coordinate system, 'l' represents the laser coordinate system, 'c' represents the camera coordinate system, 'b' represents the UAV body coordinate system, and 'n' represents the NED coordinate system. 'D' is the laser range value, 'K' is the camera intrinsic parameter, and 's' is the scale factor. For laser-camera extrinsic parameters, For camera-body external parameters; The rotation matrix from the airframe to the NED can be obtained from the pitch, roll, and yaw values ​​in the UAV's POS information.

[0059] In the keyframe, a fixed-size image block is selected as the feature block, centered on the image projection point of the laser measurement point. The tracking initialization method of the MOSSE correlation filtering algorithm is adopted. In the keyframe, a square feature block of appropriate pixel size (such as 100*100 pixels) is selected centered on the feature center point, i.e., the image projection point of the laser measurement point. The frequency domain information of the feature block is extracted to obtain the initial correlation filter. During the tracking process, the tracking algorithm tracks the feature block frame by frame. The center point of the feature block in the image coordinate system is the feature center.

[0060] In each subsequent non-keyframe image, the coordinates of the feature centers calculated in the NED system from the previous frame are normalized into vectors, which serve as the feature prior NED vectors. Combining the POS information of the current image and the camera's intra-camera parameters, the projected pixel coordinates of the feature prior NED vectors in the current frame are calculated.

[0061]

[0062] in, Let be the feature center vector of the previous frame in the NED system, i.e., the feature prior vector, s be the scale factor, and (u_p_curr, v_p_curr) be the projected pixel coordinates of the feature prior NED vector in the current frame. Using these projected pixel coordinates as the center, an image of a certain size (e.g., 200*200 pixels) is extracted as the ROI (Region of Interest) image, which is then used as the motion-compensated image. Motion compensation can compensate for rapid changes in pose, achieving an effect similar to image stabilization, thereby significantly improving the robustness of tracking. A schematic diagram of motion pose compensation is shown below. Figure 2 As shown, the projected pixel coordinates of the feature prior NED vector in the current frame are based on the NED vector calculated in the previous frame. Combined with the pose of the current frame, they are directly projected onto the image pixel coordinates of the current frame. These are used as prior pixel coordinates for motion compensation (similar to image stabilization) to improve tracking robustness.

[0063] Then, in the motion-compensated image, the MOSSE correlation filtering algorithm is also used. In the frequency domain, the current image is correlated with the correlation filter to obtain a response map. The position of the maximum response in the response map is then calculated as the new position after tracking, and the correlation filter is updated to complete feature tracking. The image tracking algorithm can directly obtain the center point and size of the tracking box. The new feature center after tracking refers to the pixel coordinates of the center point of the tracking box in the current frame, obtained through inter-frame tracking.

[0064] Finally, calculate the vector of the new feature center in the NED system after tracking in the current frame, and use this vector as the feature prior NED vector for the next frame:

[0065]

[0066] Where (u_t_curr, v_t_curr) are the new feature center pixel coordinates after tracking. The NED vector of the feature center in the current frame also serves as the prior NED vector for the next frame. During feature tracking, for each non-key frame, the operations of feature center NED vector projection, ROI extraction, image tracking, and feature center NED vector update are performed iteratively.

[0067] In this embodiment, a suitable keyframe selection strategy includes:

[0068] 1) In the initial stage, the laser ranging data is first evaluated. If the distance is within a suitable range, the drone's flight altitude is considered appropriate and meets the scenario requirements. At this point, based on the image and laser ranging acquisition control signals, an image frame captured synchronously with the laser acquisition is selected as the first keyframe, and the calculation of visual odometry information is initiated.

[0069] 2) During the visual odometry calculation process, keyframes are updated using three strategies:

[0070] a. By using the tracking quality of features, i.e. the PSR (Peak Sidelobe Ratio) parameter, when it is below a fixed threshold, it means that tracking is about to be lost. At this time, the latest acquired image frame that was captured synchronously with the laser acquisition is selected as the new key frame.

[0071] b. Calculate the disparity of the current frame relative to the previous keyframe. Based on the principle of disparity, define the ratio of the horizontal relative displacement between the current frame and the keyframe to the height component of the feature center in the keyframe NED frame:

[0072]

[0073] in, In the keyframe, the horizontal component of the NED coordinates of the feature center; This represents the horizontal component of the NED coordinates of the feature center in the current frame. The modulus of the difference between the two is the horizontal displacement distance of the current frame relative to the previous keyframe. (Z_ref) n This refers to the height component of the NED coordinates of the feature center in the keyframe. When the proportion exceeds a threshold, it means that the feature parallax is too large. In this case, the most recently acquired image frame that was captured synchronously with the laser acquisition is selected as the new keyframe.

[0074] c. The pose of the current frame relative to the keyframe. If the pitch or roll angle of the current frame relative to the keyframe exceeds the threshold, it means that the tracked features are about to move out of the field of view and be lost. In this case, the latest image frame acquired and captured synchronously with the laser acquisition is selected as the new keyframe.

[0075] In this embodiment, the method for calculating the horizontal displacement and covariance between the current frame and the keyframe includes:

[0076] First, in the keyframe, the projection point of the laser measurement point on the image is calculated, and the NED coordinates of the laser measurement point are calculated by using the direct geolocation method and referring to the feature tracking method steps.

[0077] Then, the tracking is initialized in the key frame by using the correlation filtering tracking method; and in the subsequent non-key frame image sequence, the motion is compensated and the features are tracked by referring to the feature tracking method steps, and the pixel coordinate information of the feature center is obtained in real time.

[0078] By combining the drone's attitude, camera intrinsic parameters, and barometer altitude information, calculate the relative coordinates of the feature center in the current frame in the NED system:

[0079]

[0080]

[0081] in, The coordinates of the feature center in the current frame in the camera coordinate system. The coordinates of the feature center in the current frame in the NED coordinate system are given, with a superscript '" indicating a lack of true scale information; These are the pixel coordinates of the feature center obtained through the feature tracking step.

[0082] In solving At that time, due to With only 2 degrees of freedom, the solution lacks true scale. To obtain true scale information, this embodiment uses the barometer height change in the current frame and keyframe, superimposed with the Z_ref component in the keyframe, to solve for the true scale height component information in the NED system in the current frame:

[0083] (Z_curr) n =(Z_ref) n +Δheight

[0084] Where Δheight represents the change in barometer altitude, (Z_ref) n This refers to the height component in the NED system within the keyframe; (Z_curr) nThis represents the height component at the true scale in the NED system for the current frame. (Z_curr) is the height component at the true scale. n For reference, the scaling factor is calculated based on the height component as follows:

[0085]

[0086] Among them, (Z_curr′) n The solution obtained previously The height component in the coordinate system, with odometry_ratio being the calculated scaling factor. This relates to the scale-deficient NED coordinates. Multiplying by a scaling factor yields the NED coordinates of the target at true scale.

[0087] Considering that altitude changes can be observed via a barometer at the flight control unit, and that the odometer calculation in this method also relies on barometer altitude, this method does not repeat the observation information for altitude displacement, but only provides the observation information for horizontal displacement. Ultimately... This refers to the horizontal displacement of the current frame relative to the keyframe in the NED coordinate system. Adding this displacement to the total displacement at the keyframe acquisition time gives the current horizontal displacement of the UAV.

[0088] Additionally, the covariance of the displacement information from the odometer is calculated. During odometer calculation, errors may arise from laser ranging, barometer readings, and attitude angles. Since the odometer calculation uses a direct geolocation method, in mid-to-high altitude scenarios, the positioning accuracy of this method is primarily affected by attitude errors, and these errors increase approximately linearly with the target slant distance w. Therefore, for mid-to-high altitude scenarios, we consider the main errors to originate from the pitch and roll angles of the POS (Position Point), which increase linearly with altitude, while ignoring laser ranging and barometer errors. Furthermore, we assume that the two horizontal displacement odometers are independent. Assuming the prior attitude angle error is Δθ, the displacement... The covariance can be approximated as:

[0089]

[0090] Where h is the drone's altitude relative to the ground, which is the NED coordinate of the laser measurement point in the keyframe. The altitude component Z_ref; ω is the first adjustable scaling factor, which can be adjusted within the range of 1-4 during application. This displacement covariance matrix, as the covariance of displacement observation information, is fused into the Kalman filter framework of the flight control unit. A schematic diagram for calculating horizontal displacement is shown below. Figure 3 As shown.

[0091] Furthermore, methods for calculating the heading and covariance between the current frame and the keyframe include:

[0092] First, to calculate the homography matrix from the orthophoto viewpoint, a virtual orthophoto camera coordinate system o is defined. This coordinate system shares the same origin as the camera coordinate system, but the camera's optical axis aligns with the direction of gravity. The camera coordinate system can be obtained by transforming the UAV's roll and pitch angles. Based on the camera's intrinsic parameters and the pitch and roll attitude in the POS, the homography matrices H_curr and H_ref from the current frame and keyframe to their respective orthophoto views are calculated.

[0093]

[0094]

[0095] in, Let be the rotation matrix from the current frame's camera coordinate system to the orthographic projection viewpoint. This is the rotation matrix from the keyframe camera coordinate system to the orthophoto view. To facilitate the calculation of the relative heading between the current frame and the keyframe, only the pitch and roll angles are involved in the calculation of the rotation matrix from the current frame / keyframe to the orthophoto view, and the heading angle is set to 0.

[0096] Then, based on the homography matrix, the pixels in the feature blocks of the current frame and keyframes are mapped to the orthophoto viewpoint respectively:

[0097] M_curr′=H_curr*M_curr

[0098] M_ref′=H_ref*M_ref

[0099] Where M_curr and M_ref are the pixel coordinates of the feature blocks in the current frame and keyframe, respectively, and M_curr′ and M_ref′ are the pixel coordinates mapped to the orthographic view. According to the above formula, the new feature blocks under the orthographic view can be obtained, and the feature centers of the current frame and keyframe can be calculated to be mapped to the new feature centers under the orthographic view.

[0100] Then, for the orthorectified feature blocks of the current frame and the keyframe, using their respective feature centers as center points (x′, y′), the centers of the orthorectified feature blocks of the current frame and the keyframe are transformed from Cartesian coordinates (x, y) to polar coordinates (r, θ) through polar transformation:

[0101]

[0102]

[0103] In polar coordinates, a sub-region is extracted from the keyframe feature block as a template image, with its center at (r, θ), where r is the x-coordinate and θ is the y-coordinate. Using a template matching method, the matching position (r′, θ′) of the template image center in the current frame is calculated. (θ′-θ) is the relative rotation angle between the two feature blocks, which is the relative heading angle between the current frame and the keyframe.

[0104] Finally, the variance information of the heading is calculated based on the matching similarity obtained from template matching and the error of the x-coordinate of the matching position in polar coordinates. Let sim be the matching similarity, located in the interval (0,1]; |r′-r| be the absolute value of the x-coordinate error of the matching position in polar coordinates.

[0105] We assume that the standard deviation of the heading calculation is inversely proportional to the matching similarity and directly proportional to the absolute value of the x-coordinate error in polar coordinates. We define the currently observed heading angle yaw as the sum of the heading angle of the keyframe and (θ′-θ), and its variance is related to the matching similarity and the absolute value of the x-coordinate error in polar coordinates as follows:

[0106]

[0107] Where k is the second adjustable proportional coefficient, which can be adjusted within the range of 1 to 4 in practical applications; MAX_VAR is a fixed value, and T is a threshold. When |r′-r| is higher than this threshold, it indicates that the matching result is unreliable, and at this time, the heading variance is set to the maximum value MAX_VAR. A schematic diagram for calculating the heading is shown below. Figure 4 As shown.

[0108] Furthermore, the calculated displacement, heading, and covariance information are sent to the flight control unit via a communication interface. This information serves as odometry observation information, and within a common EKF framework (such as the PIXHAWK open-source flight control system) or UKF framework, it is used to update the posterior state estimates and covariance of the displacement and heading values.

[0109] This invention relies on a monocular camera, combined with UAV POS information and single-point ranging radar measurements, and employs image feature tracking and image matching methods to achieve 3-DoF (horizontal displacement, heading angle) motion state estimation for UAVs in mid-to-high altitude scenarios. During the calculation process, keyframes and their feature blocks are first selected using an appropriate strategy. Then, the calculation is divided into two threads. The first thread tracks the feature blocks of the keyframes in real time and calculates the UAV's horizontal displacement and covariance by combining laser ranging values, UAV attitude, and barometer changes. The second thread, based on the feature tracking results of the first thread, extracts the regions of interest from the keyframes and the current frame, and calculates the UAV's heading offset and variance using image matching methods. The motion state and covariance information calculated by the two threads are sent to the flight controller, where data is loosely coupled using a common EKF / UKF method to achieve high frame rate motion state estimation. Based on the high-altitude flight scenario of UAVs, this method constructs a visual odometry calculation framework on reasonable assumptions, and optimizes the computational efficiency and robustness of feature matching and tracking methods. This enables the method to run in real time on micro-UAV platforms with limited processor computing power and camera resolution, and has the advantages of being lightweight, robust and practical.

[0110] Example 2

[0111] The purpose of this embodiment is to provide a mid-to-high altitude unmanned aerial vehicle (UAV) navigation system based on vision and laser ranging, including:

[0112] The feature tracking module is configured to: determine key frames in the monocular image acquired by the UAV based on the laser ranging acquisition time, and perform feature tracking on the current frame based on motion compensation according to the key frames;

[0113] The displacement calculation module is configured to calculate the horizontal displacement and horizontal displacement covariance between the current frame and the key frame based on the UAV POS data, the position of the laser ranging point in the key frame, and the feature tracking position of the laser ranging point in the current frame.

[0114] The heading calculation module is configured to: calculate the heading offset and heading offset covariance between the current frame and the key frame based on the UAV POS data, the feature blocks of the key frame, and the relative rotation angle of the feature blocks of the current frame after corresponding feature tracking;

[0115] Navigation module: The obtained horizontal displacement and horizontal displacement covariance information, heading offset and heading offset covariance information are used as the observation information of the flight control Kalman filter framework. The inertial navigation and visual observation information are fused through the flight control Kalman filter algorithm to obtain the UAV navigation information.

[0116] Example 3

[0117] The purpose of this embodiment is to provide a computing device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the steps of the above-described method.

[0118] Example 4

[0119] The purpose of this embodiment is to provide a computer-readable storage medium.

[0120] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, performs the steps of the above method.

[0121] The steps and methods involved in the apparatuses of Embodiments 2, 3, and 4 above correspond to those in Embodiment 1. For specific implementation details, please refer to the relevant description section of Embodiment 1. The term "computer-readable storage medium" should be understood as a single medium or multiple media including one or more instruction sets; it should also be understood as including any medium capable of storing, encoding, or carrying an instruction set for execution by a processor and enabling the processor to perform any of the methods in this invention.

[0122] Those skilled in the art will understand that the modules or steps of the present invention described above can be implemented using general-purpose computer devices. Optionally, they can be implemented using computer-executable program code, thereby allowing them to be stored in a storage device for execution by a computer device, or they can be fabricated as separate integrated circuit modules, or multiple modules or steps can be fabricated as a single integrated circuit module. The present invention is not limited to any particular combination of hardware and software.

[0123] While the specific embodiments of the present invention have been described above in conjunction with the accompanying drawings, this is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art without creative effort based on the technical solutions of the present invention are still within the scope of protection of the present invention.

Claims

1. A navigation method for medium- and high-altitude unmanned aerial vehicles (UAVs) based on vision and laser ranging, characterized in that, include: In monocular images acquired by UAVs, key frames are determined based on the acquisition time of laser ranging, and feature tracking of the current frame is performed based on motion compensation according to the key frames. Based on the UAV POS data, the position of the laser ranging point in the keyframe, and the feature tracking position of the laser ranging point in the current frame, calculate the horizontal displacement and horizontal displacement covariance between the current frame and the keyframe. Based on the UAV POS data, the feature blocks of the key frame, and the relative rotation angle of the feature blocks of the current frame after corresponding feature tracking, calculate the heading offset and heading offset covariance between the current frame and the key frame. The obtained horizontal displacement and horizontal displacement covariance information, as well as heading offset and heading offset covariance information, are used as the observation information of the flight control Kalman filter framework. The inertial navigation and visual observation information are fused by the flight control Kalman filter algorithm to obtain the UAV navigation information.

2. The mid-to-high altitude UAV navigation method based on vision and laser ranging as described in claim 1, characterized in that, In monocular images, keyframes are determined based on laser ranging. Specifically, it is determined whether the laser ranging data meets the conditions. If the conditions are met, the image frame captured synchronously with the laser ranging acquisition is selected as the keyframe.

3. The mid-to-high altitude UAV navigation method based on vision and laser ranging as described in claim 2, characterized in that, The keyframe update strategy is as follows: If the peak sidelobe ratio parameter is less than a fixed threshold, the latest image frame that was captured synchronously with the laser ranging acquisition is selected as the new key frame. Alternatively, if the ratio of the horizontal relative displacement between the current frame and the key frame and the drone's altitude above the ground both exceed the corresponding thresholds, then the latest image frame acquired and captured synchronously with the laser ranging acquisition is selected as the new key frame. Alternatively, if the pitch or roll angle of the current frame relative to the keyframe exceeds the corresponding threshold, then the latest acquired image frame that was captured synchronously with the laser ranging acquisition is selected as the new keyframe.

4. The mid-to-high altitude UAV navigation method based on vision and laser ranging as described in claim 1, characterized in that, Feature tracking of the current frame based on motion compensation using keyframes specifically includes: In the keyframe, a fixed-size image block is selected as the feature block, centered on the image projection point of the laser measurement point. The feature block is tracked to obtain the feature center. In non-key frames, the coordinates of the feature center of the previous frame in the NED system are normalized into a vector and used as the feature prior NED vector. Based on the prior feature NED vector, the POS data of the current frame, and the camera's intra-frame parameters, calculate the projected pixel coordinates of the prior feature NED vector in the current frame. The region of interest is selected centered on the projected pixel coordinates of the calculated feature prior NED vector in the current frame, and used as the motion-compensated image. The tracking filtering algorithm is applied to the motion-compensated image to obtain the feature center of the current frame after tracking; The midline of the feature tracked in the current frame is used as the prior NED vector of the feature in the next frame, and this process is repeated cyclically.

5. The mid-to-high altitude UAV navigation method based on vision and laser ranging as described in claim 1, characterized in that, Based on the UAV POS data, the position of the laser ranging point in the keyframe, and the feature tracking position of the laser ranging point in the current frame, the horizontal displacement and horizontal displacement covariance between the current frame and the keyframe are calculated, specifically: Based on the tracked position information, camera parameters, and barometer altitude, calculate the relative coordinates of the feature center in the current frame in the NED system; Based on the relative coordinates of the feature center in the current frame and the relative coordinates of the feature center in the key frame in the NED system, the horizontal displacement between the current frame and the key frame is obtained. The horizontal displacement covariance is obtained based on the prior attitude angle error.

6. The mid-to-high altitude UAV navigation method based on vision and laser ranging as described in claim 1, characterized in that, Based on the UAV POS data, the feature blocks of the keyframes, and the relative rotation angle of the feature blocks of the current frame after corresponding feature tracking, the heading offset and heading offset covariance between the current frame and the keyframes are calculated, specifically: Based on the camera parameters and the pitch and roll attitudes in the UAV POS data, the homography matrices of the current frame and keyframe to their respective orthophoto views are calculated. Based on the pixel coordinates of the feature blocks in the current frame and key frame, and the homography matrix of the current frame and key frame to their respective orthophoto views, the pixel coordinates of the feature blocks in the current frame and key frame are mapped to the pixel coordinates of the orthophoto view. Perform polar transformation on the feature blocks in the current frame and keyframe from the orthophoto perspective, with their respective feature centers as the center points; The heading offset between the current frame and the keyframe is obtained from the polar coordinates after polar transformation.

7. The mid-to-high altitude UAV navigation method based on vision and laser ranging as described in claim 6, characterized in that, In polar coordinates, a sub-region is extracted from the feature block of the keyframe as a template image. Based on the center of the template image, the matching polar coordinates of the center of the template image in the current frame are calculated using the template matching method. The heading offset between the current frame and the keyframe is obtained based on the matching polar coordinates. The variance information of the heading is calculated based on the matching similarity obtained from template matching and the error of the matching position x-coordinate in polar coordinates.

8. A mid-to-high altitude unmanned aerial vehicle (UAV) navigation system based on vision and laser ranging, characterized in that, include: The feature tracking module is configured to: determine key frames in the monocular image acquired by the UAV based on the laser ranging acquisition time, and perform feature tracking on the current frame based on motion compensation according to the key frames; The displacement calculation module is configured to calculate the horizontal displacement and horizontal displacement covariance between the current frame and the key frame based on the UAV POS data, the position of the laser ranging point in the key frame, and the feature tracking position of the laser ranging point in the current frame. The heading calculation module is configured to: calculate the heading offset and heading offset covariance between the current frame and the key frame based on the UAV POS data, the feature blocks of the key frame, and the relative rotation angle of the feature blocks of the current frame after corresponding feature tracking; Navigation module: The obtained horizontal displacement and horizontal displacement covariance information, heading offset and heading offset covariance information are used as the observation information of the flight control Kalman filter framework. The inertial navigation and visual observation information are fused through the flight control Kalman filter algorithm to obtain the UAV navigation information.

9. A computer device, characterized in that, include: The system includes a processor, a memory, and a bus. The memory stores machine-readable instructions executable by the processor. When the computer device is running, the processor communicates with the memory via the bus. When the machine-readable instructions are executed by the processor, they perform a mid-to-high altitude UAV navigation method based on vision and laser ranging as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program, which, when executed by a processor, performs a mid-to-high altitude unmanned aerial vehicle navigation method based on vision and laser ranging as described in any one of claims 1 to 7.

Images