Method and system for positioning unmanned aerial vehicle in GPS degradation scene

Abstract

The invention discloses an unmanned aerial vehicle positioning method and system in a GPS degradation scene, and belongs to the technical field of unmanned aerial vehicle positioning, and the method comprises the steps: calibrating an internal parameter matrix, and obtaining an initial external parameter matrix; real-time image data and point cloud data are obtained through a multi-sensor fusion algorithm, and two-dimensional feature points in the image data and three-dimensional feature points in the point cloud data are extracted; updating an external parameter matrix in real time based on an extended Kalman filtering algorithm in combination with observation data of the two-dimensional feature points and the three-dimensional feature points; and calculating the three-dimensional position of the visual sensor in the laser radar coordinate system through coordinate transformation to obtain real-time positioning result data of the unmanned aerial vehicle relative to the unmanned ship. According to the invention, through multi-sensor fusion and the extended Kalman filtering algorithm, the external parameter matrix between the visual sensor and the laser radar is updated in real time, the positioning precision and real-time performance are effectively improved, accurate positioning of the unmanned aerial vehicle can be realized in a GPS degradation scene, and the method is suitable for unmanned aerial vehicle positioning requirements in a dynamic environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of UAV positioning, and particularly relates to a positioning method and system for UAVs in GPS degradation scenarios. Background Art

[0002] Generally, GPS positioning is used for UAV positioning. However, in harsh environments, GPS (Global Positioning System) may fail due to external interference, and a stable information acquisition method for the actual position is needed. The cooperative positioning technology of UAVs and unmanned boats originated from the need for high-precision positioning in complex marine environments. UAVs and unmanned boats have unique advantages in fields such as marine detection, maritime patrol, and emergency rescue. The positioning technology of UAVs and unmanned boats is one of the core technologies for realizing their autonomous navigation, mission execution, and cooperative operation.

[0003] The cooperative positioning technology of UAVs and unmanned boats mainly includes satellite positioning (GNSS / GPS), visual positioning, lidar positioning, ultra-wideband positioning (UWB), and multi-sensor fusion positioning. Satellite positioning is the basic positioning technology for UAVs and unmanned boats, which realizes high-precision absolute positioning by receiving satellite signals; visual positioning uses visual sensors carried by UAVs or unmanned boats to realize positioning through image processing and computer vision algorithms; lidar positioning obtains distance information in the environment by emitting laser beams and measuring the reflection time; ultra-wideband positioning realizes high-precision positioning by sending nanosecond-level extremely narrow pulses, especially suitable for indoor or satellite signal-limited environments; multi-sensor fusion positioning is to improve the robustness and accuracy of positioning.

[0004] In some complex environments such as urban canyons with high-rise buildings, mountainous areas, or forests, GPS signals will be interfered with, and even the signals may completely fail. In areas where GPS signals cannot cover, UAVs or unmanned boats equipped with GPS cannot work either. In the case of electromagnetic interference or intentional interference, signal acquisition is also a relatively serious problem. These problems will reduce the working efficiency and positioning accuracy in the cooperative positioning of UAVs and unmanned boats. In summary, in the current cooperative positioning of UAVs and unmanned boats, position information may be lost due to complex environments, interference of GPS signals, and natural interference such as geomagnetism, and further optimization is needed to maintain the stability of position information acquisition. Summary of the Invention

[0005] The present invention provides a positioning method and system for UAVs in GPS degradation scenarios, aiming to solve the problem that in the current cooperative positioning of UAVs and unmanned boats, position information may be lost due to complex environments, interference of GPS signals, and natural interference such as geomagnetism, and further optimization is needed to maintain the stability of position information acquisition.

[0006] To achieve the above object, the present invention adopts the following technical solutions: The present invention provides a positioning method for an unmanned aerial vehicle (UAV) in a GPS degradation scenario, comprising the following steps: S1. Calibrate the internal parameter matrix of the vision sensor and obtain the initial external parameter matrix between the vision sensor and the lidar; the initial external parameter matrix includes a rotation matrix and a translation vector; S2. Real-time collect the image data of the vision sensor and the point cloud data of the lidar through a multi-sensor fusion algorithm, and extract the two-dimensional feature points in the image data and the three-dimensional feature points in the point cloud data; S3. Based on the extended Kalman filter algorithm, use the initial external parameter matrix as the state vector, and combine the observation data of the two-dimensional feature points and the three-dimensional feature points to update the external parameter matrix between the vision sensor and the lidar in real time; S4. Based on the external parameter matrix between the vision sensor and the lidar, calculate the three-dimensional position of the vision sensor in the lidar coordinate system through coordinate transformation to obtain the real-time positioning result data of the UAV relative to the unmanned ship.

[0007] In some embodiments, in S1, the vision sensor and the lidar include: the vision sensor is installed on the UAV, and the lidar is installed on the unmanned ship; or, the lidar is installed on the UAV, and the vision sensor is installed on the unmanned ship.

[0008] Further, in S1, the vision sensor includes a camera, and the scanning direction of the lidar is synchronized with the moving direction of the unmanned ship.

[0009] In some embodiments, in S1, calibrating the internal parameter matrix of the vision sensor includes the following steps S11. Determine the focal length parameter, principal point coordinates, and distortion coefficient of the vision sensor; S12. Construct the following mathematical expression of the internal parameter matrix of formula (1): (1); where, ( ) is the focal length, ( ) is the principal point.

[0010] In some embodiments, in S2, the multi-sensor fusion algorithm further includes: S21. Convert the data collected by the UAV and the unmanned ship to a unified world coordinate system; S22. Perform time synchronization correction on the data collected by the UAV and the unmanned ship.

[0011] In some embodiments, in S3, the extended Kalman filter algorithm includes: S31. Convert the initial external parameter matrix into the state vector of the following formula (2): (2); Among them, 、 、 are rotation vectors, 、 、 are translation vectors, is the external parameter to be estimated; S32. Predict the current state vector and covariance matrix through the state transition model; S33. Project the three-dimensional point cloud of the lidar onto the two-dimensional image plane of the vision sensor, calculate the observation residual and linearize the observation model; S34. Update the state vector and covariance matrix according to the observation residual and the Jacobian matrix to obtain the updated external parameter matrix.

[0012] Furthermore, in S34, the observation residual includes: projecting the three-dimensional point cloud of the lidar onto the image plane of the vision sensor through the current external parameter matrix and matching it with the two-dimensional feature points to obtain the projection error.

[0013] In some embodiments, in S3, the update frequency of the external parameter matrix is consistent with the acquisition frame rate of the image data of the vision sensor.

[0014] In some embodiments, in S4, the external parameter matrix of the following formula (3) represents the transformation from the lidar coordinate system to the camera coordinate system: (3); Among them, is a 3×3 rotation matrix, is a 3×1 translation vector, is the three-dimensional coordinate of the point in the camera coordinate system, is the three-dimensional coordinate of the point in the lidar coordinate system.

[0015] The present invention also provides a positioning system for an unmanned aerial vehicle in a GPS degradation scenario. The system includes a parameter calibration module, a data acquisition and feature extraction module, an external parameter update module, and a positioning calculation module, where: The parameter calibration module is used to calibrate the internal parameter matrix of the vision sensor and obtain the initial external parameter matrix between the vision sensor and the lidar; The data acquisition and feature extraction module is used to collect the image data of the vision sensor and the point cloud data of the lidar in real time through a multi-sensor fusion algorithm, and extract the two-dimensional feature points in the image data and the three-dimensional feature points in the point cloud data; The external parameter update module is used to update the external parameter matrix between the vision sensor and the lidar in real time based on the extended Kalman filter algorithm, with the initial external parameter matrix as the state vector, in combination with the observation data of two-dimensional feature points and three-dimensional feature points. The positioning calculation module is used to calculate the three-dimensional position of the vision sensor in the lidar coordinate system based on the external parameter matrix between the vision sensor and the lidar, and obtain the real-time positioning result data of the UAV relative to the unmanned ship.

[0016] Compared with the prior art, the positioning method and system of a UAV in a GPS degradation scenario of the present invention have the following beneficial effects: The positioning method of a UAV in a GPS degradation scenario of the present invention aims at the positioning of a UAV in a GPS degradation scenario, and uses the fusion of images and lidar to position the UAV relative to the unmanned ship. The extended Kalman filter algorithm is used to estimate the external parameter matrix between the vision sensor and the lidar in real time, and the external parameter matrix is corrected and estimated through prediction, observation, linearization and update. Coordinate transformation is performed according to the obtained external parameter matrix to obtain the coordinates of the vision sensor relative to the lidar. The present invention is based on the map construction and positioning of a UAV in a GPS degradation scenario. A vision sensor and a lidar are installed on the UAV and the unmanned ship, and through multi-sensor fusion, the external parameter matrix between the vision sensor and the lidar is continuously updated to perform the real-time positioning of the UAV relative to the unmanned ship. The present invention calibrates the internal parameter matrix of the vision sensor on the UAV, and then uses the calibration principle of the external parameter matrix to position the position of the UAV relative to the unmanned ship. The calibration of the external parameter matrix adopts the principles of computer vision and multi-sensor fusion, and will not cause the loss of position information due to environmental changes and natural interferences such as geomagnetism, and can better maintain the stability of position information acquisition. And through the fusion of the vision sensor and the lidar, the precise positioning of the UAV can be realized in a GPS degradation scenario, expanding the application range of the UAV and promising to improve the applicability of the UAV. Description of the Drawings

[0017] The accompanying drawings in the specification are used to provide a further understanding of the present invention, and constitute a part of the present invention. The schematic embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation to the present invention.

[0018] Figure 1 It is a schematic flow chart of the positioning method of a UAV in a GPS degradation scenario of the present invention; Figure 2 It is a schematic diagram of the input data relationship for updating the external parameter matrix between the vision sensor and the lidar based on the extended Kalman filter algorithm in the positioning method of a UAV in a GPS degradation scenario of the present invention. Detailed Embodiments

[0019] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Apparently, the described embodiments are some, but not all, of the embodiments of the present invention. Components of the embodiments of the present invention usually described and illustrated in the accompanying drawings here can be arranged and designed in various different configurations.

[0020] Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the claimed present invention, but merely represents selected embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts fall within the scope of protection of the present invention.

[0021] It should be noted that like reference numerals and letters denote like items in the following drawings. Therefore, once an item is defined in one drawing, it does not need to be further defined and explained in subsequent drawings.

[0022] In the description of the embodiments of the present invention, it should be noted that if terms such as "upper", "lower", "horizontal", "inner", etc. indicate orientations or positional relationships based on the orientations or positional relationships shown in the drawings, or the orientations or positional relationships in which the inventive product is customarily placed during use, it is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of the present invention. In addition, terms such as "first", "second", etc. are only used for descriptive distinction and cannot be construed as indicating or implying relative importance.

[0023] In addition, if the term "horizontal" appears, it does not mean that the component is required to be absolutely horizontal, but it can be slightly inclined. For example, "horizontal" only means that its direction is more horizontal relative to "vertical", and does not mean that the structure must be completely horizontal, but it can be slightly inclined.

[0024] In the description of the embodiments of the present invention, it should also be noted that unless otherwise clearly specified and limited, if terms such as "set", "installed", "connected", "connected" are understood in a broad sense. For example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be directly connected or indirectly connected through an intermediate medium, and it can be the communication inside two components. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific situations.

[0025] How to achieve high-precision and real-time positioning of an unmanned aerial vehicle (UAV) in a complex environment and other interference factors to improve the service stability of the UAV.

[0026] As Figure 1 and Figure 2 shown, the present invention provides a positioning method for an unmanned aerial vehicle (UAV) in a GPS degradation scenario, comprising the following steps: S1. Calibrate the internal parameter matrix of the vision sensor and obtain the initial external parameter matrix between the vision sensor and the lidar; the initial external parameter matrix includes a rotation matrix and a translation vector; S2. Real-time collect the image data of the vision sensor and the point cloud data of the lidar through a multi-sensor fusion algorithm, and extract the two-dimensional feature points in the image data and the three-dimensional feature points in the point cloud data; S3. Based on the extended Kalman filter algorithm, use the initial external parameter matrix as the state vector, and combine the observation data of the two-dimensional feature points and the three-dimensional feature points to real-time update the external parameter matrix between the vision sensor and the lidar; S4. Based on the external parameter matrix between the vision sensor and the lidar, calculate the three-dimensional position of the vision sensor in the lidar coordinate system through coordinate transformation to obtain the real-time positioning result data of the UAV relative to the unmanned ship.

[0027] Through the fusion of the vision sensor and the lidar, the present invention can achieve precise positioning of the UAV in a GPS degradation scenario, expand the application range of the UAV, and real-time update the external parameter matrix between the vision sensor and the lidar through the multi-sensor fusion algorithm and the extended Kalman filter algorithm, which can effectively improve the positioning accuracy and reduce error accumulation. Moreover, the present invention adopts the extended Kalman filter algorithm to be able to real-time process the sensor data and dynamically update the external parameter matrix to ensure the real-time nature of the positioning result, which is applicable to the positioning requirements of the UAV in a dynamic environment. Through the collaborative work of the vision sensor and the lidar, combining the two-dimensional image feature points and the three-dimensional point cloud feature points, it can make full use of the advantages of different sensors, improve the environmental perception and positioning ability of the UAV, and enhance the scene applicability of the UAV.

[0028] The present invention also provides a positioning system for an unmanned aerial vehicle (UAV) in a GPS degradation scenario. The system includes a parameter calibration module, a data collection and feature extraction module, an external parameter update module, and a positioning calculation module, wherein: The parameter calibration module is used to calibrate the internal parameter matrix of the vision sensor and obtain the initial external parameter matrix between the vision sensor and the lidar; The data collection and feature extraction module is used to real-time collect the image data of the vision sensor and the point cloud data of the lidar through a multi-sensor fusion algorithm, and extract the two-dimensional feature points in the image data and the three-dimensional feature points in the point cloud data; The external parameter update module is used to, based on the extended Kalman filter algorithm, use the initial external parameter matrix as the state vector, and combine the observation data of the two-dimensional feature points and the three-dimensional feature points to real-time update the external parameter matrix between the vision sensor and the lidar; The positioning calculation module is used to calculate the three-dimensional position of the vision sensor in the lidar coordinate system through coordinate transformation based on the external parameter matrix between the vision sensor and the lidar, and obtain the real-time positioning result data of the UAV relative to the unmanned ship.

[0029] The system of the present invention is used to implement the method of the present invention. The system can achieve efficient operation and function division, is convenient for expansion and maintenance, and provides a carrier for the operation of the UAV.

[0030] The following further details a positioning method and system for a UAV in a GPS degradation scenario of the present invention through specific embodiments.

[0031] The design of the present invention first needs to calibrate the internal parameter matrix of the vision sensor on the UAV, and then use the calibration principle of the external parameter matrix to position the position of the UAV relative to the unmanned ship. The designed external parameter matrix calibration adopts the principles of computer vision and multi-sensor fusion, and will not cause the loss of position information due to environmental changes and natural interferences such as geomagnetism. The following two solutions are proposed for application in the present invention: Solution 1: Install a vision sensor on the UAV and a lidar on the unmanned ship.

[0032] Solution 2: Install a lidar on the UAV and a vision sensor on the unmanned ship.

[0033] The installation positions of the above two sensors are different. By flexibly configuring the installation positions of the vision sensor and the lidar (UAV or unmanned ship), different mission requirements and application scenarios can be adapted. The same algorithm can meet the conditions during actual application. As an embodiment, the following mainly focuses on Solution 1.

[0034] Combining the data of the vision sensor and the lidar, high-precision environmental perception and positioning are realized through a multi-sensor fusion algorithm. The vision sensor takes pictures during flight, and the lidar continuously scans the surrounding environment on the ground. The external parameter matrix is obtained by fusing the information of the pictures and the lidar for position positioning.

[0035] Based on the map construction of the UAV in a GPS degradation scenario, real-time data is continuously provided during the flight of the UAV, and the map of this area is constructed in cooperation with the sensors on the unmanned ship. For the positioning of the UAV in a GPS degradation scenario, first calibrate the internal parameter matrix and distortion coefficient of the vision sensor; check the sensor status and select a suitable calibration scenario; the UAV and the unmanned ship collect data for multi-view and multi-position synchronous sampling; convert the data collected by the UAV and the unmanned ship to a unified world coordinate system for time correction; use the calibration algorithm to calculate the relative pose between the sensors; verify the calibration result and calculate the reprojection error; finally, perform real-time positioning and navigation.

[0036] The intrinsic matrix of the camera is only related to the camera itself and depends on the internal parameters of the camera, including the focal length ( ), the principal point ( ), and the distortion coefficients. The focal length is the ability of the lens to focus light onto the image sensor and affects the scaling ratio of the image. The principal point is the intersection of the optical axis and the image plane (usually close to the center of the image). The distortion parameters are mainly divided into radial distortion and tangential distortion. Among them, radial distortion is caused by the lens quality, and the light is more bent at places far from the center of the lens than at places close to the center; tangential distortion is caused by the lens itself being uneven with the camera sensor plane or the image plane. By calibrating the intrinsic matrix of the vision sensor (including the focal length, principal point coordinates, and distortion coefficients), the internal parameters of the camera can be accurately described, providing reliable basic data for the subsequent calculation of the extrinsic matrix. The mathematical form of the intrinsic matrix is generally the following formula (1): (1); The extrinsic matrix represents the transformation from the lidar coordinate system to the camera coordinate system: (2); is a 3×3 rotation matrix, is a 3×1 translation vector, is the three-dimensional coordinate of the point in the camera coordinate system, is the three-dimensional coordinate of the point in the lidar coordinate system.

[0037] As Figure 2 shown, the present invention calculates the extrinsic matrices of the camera and the lidar in real time based on the Extended Kalman Filter (EKF) method. It is necessary to first obtain the initial pose transformation matrix from the camera to the lidar and the intrinsic matrix of the camera. In the real-time update stage, the real-time update inputs of the image feature points and the lidar feature points are input. The data stream inputs include the camera image stream and the lidar point cloud. The Extended Kalman Filter algorithm can dynamically adjust the extrinsic matrix through state vector prediction, observation residual calculation, and state update, ensuring the real-time performance and accuracy of the positioning result.

[0038] The difference between the Extended Kalman Filter (EKF) and the Standard Kalman Filter (KF) is that EKF performs local linearization on the non-linear model through first-order Taylor expansion and calculates the Jacobian matrix to replace the linear transformation matrix in KF.

[0039] The core principles of EKF include the state space model and the prediction and update process. The state space model includes the state vector and the observation vector. The observation vector is the 2D feature point coordinates in the image, which is a 2N-dimensional vector in practical applications. The state vector is the extrinsic parameter to be estimated, which is a 6-dimensional vector in practical applications: (3); Among them, , , are rotation vectors, , , are translation vectors, is the external parameter to be estimated.

[0040] The prediction and update process includes a prediction stage and an update stage. The prediction stage predicts the current state and covariance according to the state transition model. The update stage needs to calculate the Kalman gain : (4); Among them, is the predicted covariance, is the observation Jacobian matrix, is the observation noise covariance.

[0041] Combine the observed value to correct the predicted value: (5); (6); Among them, is the updated state vector, is the state vector in the prediction stage, is the observed data, is the observation model function, is the updated covariance, is the identity matrix.

[0042] Obtain the external parameter matrix of the vision sensor and the lidar. The format of the external parameter matrix is as follows: (7); Among them, is a 3×3 rotation matrix, is a 3×1 translation vector, is the external parameter matrix.

[0043] Calculate the relative position of the vision sensor with respect to the lidar according to the obtained external parameter matrix, and coordinate transformation derivation is required. Based on the transformation from the lidar coordinate system to the vision sensor coordinate system in formula (2), find the position of the camera origin in the lidar coordinate system (i.e., ): (8); The coordinates of the vision sensor relative to the lidar can be obtained , realizing unmanned aerial vehicle map construction and positioning in a GPS degraded scenario.

[0044] The external parameter calibration implements a real-time external parameter calibration algorithm based on the extended Kalman filter. This algorithm combines 2D image feature points and 3D lidar point clouds to continuously update the relative position (external parameters) between the vision sensor and the lidar, so as to improve the positioning accuracy. Specifically: It takes the internal parameter matrix of the vision sensor and the initial external parameters between the vision sensor and the lidar as inputs. It mainly includes four steps: initialization, state transition, calibration update, and output. In the first step, configure the parameters of the extended Kalman filter (state transition matrix, observation noise, etc.), and convert the initial external parameters into the state vector of the EKF. In the second step, convert the state vector (rotation and translation information) of the extended Kalman filter into an external parameter matrix (rotation matrix and translation vector). In the third step, according to the state transition model of the extended Kalman filter, predict the next state; then project the 3D lidar point cloud onto the 2D image, calculate the observation residual; use numerical methods to calculate the Jacobian matrix for linearizing the observation model; update the state and covariance matrix of the extended Kalman filter to fuse the new observation information. In the fourth step, initialize the external parameter calibrator; process the real-time data stream, including obtaining images and lidar point clouds, and extracting feature points; call the calibration update method to output the updated external parameter matrix.

[0045] Positioning mainly transforms the above-calibrated external parameter matrix to calculate the relative position of the vision sensor with respect to the lidar. Taking the calibrated external parameter matrix as the input, the output is the three-dimensional coordinates of the vision sensor with respect to the lidar. Its main logic is as follows: After each external parameter calibration update, calculate and output the position of the camera in the lidar coordinate system by operating on the transpose of the rotation matrix and the translation vector.

[0046] In summary, for the positioning of an unmanned aerial vehicle (UAV) in a GPS-degraded scenario, the present invention uses the fusion of images and lidar to locate the position of the UAV relative to an unmanned ship. The extended Kalman filter algorithm is used to estimate the external parameter matrix between the vision sensor and the lidar in real time, and the external parameter matrix is corrected and estimated through prediction, observation, linearization, and update. Coordinate transformation is performed based on the obtained external parameter matrix to obtain the coordinates of the vision sensor relative to the lidar. The present invention can be applied to relatively complex environments and can achieve real-time and accurate positioning of the UAV.

[0047] Finally, it should be noted that: The above description is only a preferred embodiment of the present invention and does not impose any form of limitation on the present invention; any person skilled in the art can smoothly implement the present invention according to the instructions and the above description. Slight modifications, decorations, and equivalent changes made using the technical content disclosed above are all equivalent embodiments of the present invention; at the same time, any equivalent changes, modifications, and evolutions made to the above embodiments based on the essential technology of the present invention still fall within the protection scope of the technical solution of the present invention.

Claims

1. A method for positioning a drone in a GPS-degraded scenario, characterized in that: The steps include: S1. Calibrate the intrinsic parameter matrix of the visual sensor and obtain the initial extrinsic parameter matrix between the visual sensor and the laser radar; the initial extrinsic parameter matrix includes a rotation matrix and a translation vector; S2, collect image data of visual sensors and point cloud data of laser radar in real time through multi-sensor fusion algorithm, extract two-dimensional feature points in image data and three-dimensional feature points in point cloud data; S3, based on the extended Kalman filter algorithm, using the initial external parameter matrix as the state vector, combined with the observation data of the two-dimensional feature points and the three-dimensional feature points, the external parameter matrix between the visual sensor and the laser radar is updated in real time; S4. Based on the external parameter matrix between the visual sensor and the laser radar, the three-dimensional position of the visual sensor in the laser radar coordinate system is calculated through coordinate transformation to obtain the real-time positioning result data of the UAV relative to the unmanned ship.

2. The method for positioning a UAV in a GPS degradation scenario according to claim 1, characterized in that: In S1, the visual sensor and the laser radar include: the visual sensor is installed on a drone, and the laser radar is installed on an unmanned ship; or, the laser radar is installed on a drone, and the visual sensor is installed on an unmanned ship.

3. The method for positioning a UAV in a GPS degradation scenario according to claim 2, characterized in that: In S1, the visual sensor includes a camera, and the scanning direction of the laser radar is synchronized with the moving direction of the unmanned ship.

4. The method for positioning a UAV in a GPS-degraded scenario according to claim 1, characterized in that: In S1, calibrating the intrinsic parameter matrix of the visual sensor includes the following steps: S11, determining the focal length parameters, principal point coordinates and distortion coefficient of the visual sensor; S12. Construct the following formula (1): The mathematical expression of the internal parameter matrix is: （1）； in,( ) is the focal length, ( ) as the main point.

5. The method for positioning a UAV in a GPS-degraded scenario according to claim 1, characterized in that: In S2, the multi-sensor fusion algorithm further includes: S21, converting the data collected by the drone and the unmanned ship into a unified world coordinate system; S22. Perform time synchronization correction on the collected data of the UAV and the unmanned ship.

6. The method for positioning a UAV in a GPS-degraded scenario according to claim 1, characterized in that: In S3, the extended Kalman filter algorithm includes: S31, convert the initial external parameter matrix into the state vector of the following formula (3): （3）； in, , , is the rotation vector, , , is the translation vector, is the external parameter to be estimated; S32, predicting the current state vector and covariance matrix through the state transition model; S33, projecting the three-dimensional point cloud of the laser radar onto the two-dimensional image plane of the visual sensor, calculating the observation residual and linearizing the observation model; S34. Update the state vector and the covariance matrix according to the observed residual and the Jacobian matrix to obtain an updated extrinsic parameter matrix.

7. The method for positioning a UAV in a GPS-degraded scenario according to claim 6, characterized in that: In the S34, the observation residual includes: projecting the three-dimensional point cloud of the laser radar to the image plane of the visual sensor through the current external parameter matrix, and matching it with the two-dimensional feature points to obtain the projection error.

8. The method for positioning a UAV in a GPS-degraded scenario according to claim 1, characterized in that: In S3, the updating frequency of the extrinsic parameter matrix is ​​kept consistent with the image data acquisition frame rate of the visual sensor.

9. The method for positioning a UAV in a GPS-degraded scenario according to claim 1, characterized in that: In S4, the external parameter matrix of the following formula (2) represents the transformation from the laser radar coordinate system to the camera coordinate system: （2）； in, is a 3×3 rotation matrix, is a 3×1 translation vector, is the three-dimensional coordinate of the point in the camera coordinate system, is the three-dimensional coordinate of the point in the laser radar coordinate system.

10. The system based on the method for positioning a UAV in a GPS degradation scenario according to any one of claims 1 to 9, characterized in that: The system includes a parameter calibration module, a data acquisition and feature extraction module, an external parameter update module and a positioning calculation module, wherein: The parameter calibration module is used to calibrate the internal parameter matrix of the visual sensor and obtain the initial external parameter matrix between the visual sensor and the laser radar; The data acquisition and feature extraction module is used to collect the image data of the visual sensor and the point cloud data of the laser radar in real time through the multi-sensor fusion algorithm, and extract the two-dimensional feature points in the image data and the three-dimensional feature points in the point cloud data; The external parameter update module is used to update the external parameter matrix between the visual sensor and the lidar in real time based on the extended Kalman filter algorithm, using the initial external parameter matrix as the state vector and combining the observation data of the two-dimensional feature points and the three-dimensional feature points; The positioning calculation module is used to calculate the three-dimensional position of the visual sensor in the laser radar coordinate system through coordinate transformation based on the external parameter matrix between the visual sensor and the laser radar, and obtain the real-time positioning result data of the UAV relative to the unmanned ship.

Images