Indoor and outdoor unmanned aerial vehicle continuous autonomous positioning method and system
Through the RTK module and lidar camera system on the drone, combined with historical image feature matching and octree global map, the problem of positioning accuracy and repeated flight consistency of the drone in indoor and outdoor environments is solved, and the satellite signal is realized High-precision continuous autonomous positioning in rejected environments.
Patent Information
- Application Number
- CN202411165955.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-23
- Publication Date
- 2024-11-29
AI Technical Summary
The autonomous positioning of drones in open scenes mainly relies on satellite or ground base station signals. After entering a building or indoor environment, the signal is blocked, resulting in large positioning errors or failures. The existing self-motion estimation methods cannot guarantee the accuracy and smooth flight of repeated flight missions.
The RTK module on the drone is used to obtain global positioning information, combine lidar and RGB cameras to extract feature points through laser point cloud projection images, and use historical image feature matching and nearest neighbor feature matching retrieval of the octree global map to construct observation errors Estimate the position and optimize the global map through RTK global positioning information to achieve continuous autonomous positioning of the drone.
High-precision positioning of drones is realized in an environment where satellite and ground base station signal denial, ensuring the positioning accuracy and smooth flight of drones when repeatedly flying between buildings, and improving the real-timeness of positioning algorithms through dual-thread design.
Smart Images

Figure CN119043326A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the technical field of unmanned aerial vehicles, and in particular relates to a method and system for continuous autonomous positioning of indoor and outdoor unmanned aerial vehicles. Background Art
[0002] In drone package delivery, rescue, inspection and other tasks, drones are usually required to be able to autonomously locate in open scenes and to travel between buildings. Among them, the autonomous positioning of drones in open scenes mainly relies on satellite or ground base station signals to achieve positioning, such as using GPS (Global Positioning System) or RTK (Real-time kinematic); after entering a building or indoor environment, the signal is blocked, resulting in excessive errors in the GPS / RTK positioning results or positioning failure. At this time, a drone self-motion estimation method is needed to determine the drone's posture in real time. Therefore, the self-motion estimation algorithm of visual sensors or lidar sensors is widely used. This type of self-motion estimation algorithm can perform real-time positioning based on the drone's surrounding environment information. However, there are two difficulties in drone positioning through vision or lidar odometers: 1. The self-motion estimation algorithm of vision or lidar can only provide the relative position of the drone. Affected by the starting position of the drone and the accumulated error of motion estimation, its positioning result cannot guarantee the robot to repeat the flight mission. 2. When the drone is flying at high altitude, the surrounding environment information is missing and it can only be positioned through GPS / RTK information. As the drone descends, there is a possibility that GPS / RTK will gradually become ineffective due to the influence of buildings. In order to ensure a smooth flight process, the self-motion estimation algorithm needs to be initialized quickly and the reference system of the positioning result must be aligned with the reference system of the GPS / RTK positioning. Summary of the invention
[0003] In response to the shortcomings of the existing technology, this application proposes a method and system for continuous autonomous positioning of indoor and outdoor drones, which can not only realize drone positioning in the absence of high-altitude environmental characteristics, but also realize drone positioning in building environments where satellite and ground base station signals are denied, and can smoothly switch between satellite positioning and positioning based on self-motion estimation.
[0004] In the first aspect, the present application proposes a method for continuous autonomous positioning of indoor and outdoor drones, comprising:
[0005] Use the RTK module on the drone to obtain RTK global positioning information;
[0006] Use the laser radar on the drone to obtain the laser point cloud;
[0007] Using an RGB camera on the drone to acquire an image, wherein the camera viewing directions of the laser radar and the RGB camera are vertically downward relative to the drone, and the laser radar and the RGB camera have overlapping fields of view;
[0008] In the positioning thread, the laser point cloud is projected onto the image, the feature points in the image after the laser point cloud projection are extracted, and the depth of each feature point is calculated. The matching feature points corresponding to the feature points are retrieved through historical image feature matching and nearest neighbor feature matching of the octree global map to obtain successfully matched feature point pairs. According to the successfully matched feature point pairs, the first observation error constructed using the depth of each feature point is used to estimate the drone's pose, and the drone's estimated pose is obtained.
[0009] In the map building thread, new feature points are observed based on the estimated position and posture of the drone, and the new feature points are converted to the world coordinate system. The new feature points converted to the world coordinate system are placed in the global map to obtain a global map after updating the map elements. Based on the global map after updating the map elements and the second observation error constructed using the RTK global positioning information, the global map is optimized to obtain an updated global map.
[0010] When the next drone positioning is performed, the updated global map is used as the global map in the positioning thread.
[0011] The step of projecting the laser point cloud onto the image, extracting feature points in the image after the laser point cloud projection, and calculating the depth of each feature point includes:
[0012] Projecting the laser point cloud onto the image, and selecting a target feature point in the image after the laser point cloud is projected;
[0013] Obtain n points closest to the target feature point, and form a first point cloud with the target feature point and the n points;
[0014] In the camera coordinate system, calculating the expectation and covariance of the first point cloud, where the expectation of the first point cloud is a three-dimensional vector;
[0015] Calculate the eigenvalue of the covariance of the first point cloud and take the eigenvector with the smallest eigenvalue;
[0016] Specify a plane, a point on the plane is the expectation of the first point cloud, and the direction of the plane is the eigenvector with the smallest eigenvalue;
[0017] Calculate the intersection of the back-projected ray of the pixel point on the image after the laser point cloud is projected and the plane, and use the intersection as a feature point in the image after the laser point cloud is projected;
[0018] Calculate the position coordinates of the feature point in the camera coordinate system according to the first point cloud and the feature vector with the smallest eigenvalue;
[0019] In the camera coordinate system, the z-axis direction is defined as the front of the drone's flight, and the expected component of the first point cloud in the z-axis direction is used as the depth of each feature point.
[0020] The process of retrieving matching feature points corresponding to the feature points through historical image feature matching and nearest neighbor feature matching of the octree global map to obtain successfully matched feature point pairs includes:
[0021] Step S42.1: Encode the global map using an octree, and save the encoded feature points, the spatial coordinates of the feature points, and descriptor information, wherein the descriptor information includes: a brief descriptor consisting of 256 Boolean values or a superpoint descriptor consisting of 128 floating point values;
[0022] Step S42.2: extract feature points Pi from the current image and the current laser point cloud;
[0023] Step S42.3: Calculate the descriptor distances between the feature point Pi and the feature points of the historical image respectively, sort the feature points of the historical image according to the descriptor distances from small to large, obtain the feature points with the smallest and second smallest descriptor distances on the historical image, and record them as the optimal matching point h1 and the second optimal matching point h2 respectively;
[0024] Step S42.4: Perform saliency detection on the optimal matching point and the suboptimal matching point using the descriptor information. If the saliency detection requirement is met, go to step S42.5;
[0025] Step S42.5: Calculate the descriptor distance between the optimal matching point h1 and all feature points on the current image, and sort the feature points of the current image from small to large according to the descriptor distance, and obtain the matching point P1 with the smallest descriptor distance between the optimal matching point h1 and the current image;
[0026] Step S42.6: Check the consistency of feature point Pi and matching point P1. If the consistency requirement is met, that is, Pi is equal to P1, then the matching feature point corresponding to feature point Pi is h1. Take h1 as the historical image matching feature point, and go to step S42.2 to obtain the next feature point matching result.
[0027] Step S42.7: If the feature point Pi does not meet the significance requirement or consistency requirement, find the feature point n1 closest to the three-dimensional coordinates of the feature point Pi in the global map through octree search, compare the descriptor distance between the two features, and when the descriptor distance is less than the preset value, the matching feature point corresponding to the feature point Pi is n1, and n1 is used as the nearest neighbor matching feature point;
[0028] Step S42.8: The feature point Pi is matched with the historical image matching feature point h1 or the feature point Pi is matched with the nearest neighbor matching feature point n1 to form a successfully matched feature point pair.
[0029] The method of estimating the posture of the UAV by using the first observation error constructed by the depth of each feature point according to the successfully matched feature point pairs to obtain the estimated posture of the UAV includes:
[0030] The first observation error is constructed using the depth of each feature point;
[0031] According to the first observation error and the minimum eigenvalue of the covariance of the first point cloud, a first least squares problem is constructed and solved to obtain the estimated position and pose of the drone.
[0032] The first observation error is constructed by using the depth of each feature point, and the calculation formula is as follows:
[0033]
[0034] Among them, r p is the first observation error of the feature point, (u, v) is the coordinate of the feature point on the two-dimensional image, that is, the coordinate of the feature point Pi on the image, d is the depth of the feature point, f x is the calculated focal length of the camera in the x-axis direction, f y is the calculated focal length of the camera in the y-axis direction, c x is the calculated principal point on the camera x-axis, c y is the calculated principal point on the camera's y axis, (x c ,y c ,z c ) T is the three-dimensional coordinate of the feature point in the UAV coordinate system.
[0035] According to the minimum eigenvalue of the covariance of the observation error and the first point cloud, a least squares problem is constructed and solved to obtain the estimated posture of the drone. The calculation formula is as follows:
[0036]
[0037] in, is the optimal estimation result of the camera's estimated pose. The estimated pose of the drone at the current moment is obtained based on the optimal estimation result of the camera's estimated pose. is the axis-angle expression of the rotation matrix, The exponential mapping is the rotation transformation matrix from the world coordinate system to the drone coordinate system, is the translation transformation vector from the world coordinate system to the drone coordinate system, is the optimal estimated coordinate of the feature point in the world coordinate system, that is, the coordinate of the historical image matching feature point h1 or the coordinate of the nearest neighbor matching feature point n1, k min is the minimum eigenvalue of the covariance of the first point cloud, r p is the first observation error of the feature point.
[0038] The method of completing global map optimization according to the global map after the map elements are updated and the second observation error constructed using the RTK global positioning information to obtain an updated global map includes:
[0039] The second observation error constructed using RTK global positioning information;
[0040] According to the first observation error, the second observation error and the minimum eigenvalue of the covariance of the global map after updating the map elements, a second least squares problem is constructed and solved to complete the global map optimization and obtain an updated global map.
[0041] The second observation error constructed using the RTK global positioning information is calculated as follows:
[0042]
[0043] Among them, R gc is the rotation transformation matrix from the drone coordinate system to the world coordinate system calculated by RTK, t gc is the translation transformation vector from the drone coordinate system to the world coordinate system calculated by RTK, R cw is the rotation transformation matrix from the world coordinate system to the drone coordinate system calculated by the global map, t cw is the translation transformation vector from the world coordinate system to the drone coordinate system calculated through the global map, r g is the second observation error.
[0044] The second least squares problem is constructed according to the first observation error, the second observation error and the minimum eigenvalue of the covariance of the global map after updating the map elements. The calculation formula is as follows:
[0045]
[0046] in, is the optimal estimation result of the UAV’s estimated pose, is the axis-angle representation of the rotation matrix, is the translation transformation matrix from the world coordinate system to the drone coordinate system, is the optimal estimated coordinate of the feature point in the world coordinate system, that is, the coordinate of the historical image matching feature point h1 or the coordinate of the nearest neighbor matching feature point n1, r p is the first observation error of the feature point, rg is the second observation error of the feature point, ρ1(·) is the HuberLoss robust kernel function, and the expression of ρ1(·) is as follows:
[0047]
[0048] ρ2(·) is the ArctanLoss robust kernel function, and the expression of ρ2(·) is as follows:
[0049] ρ2(x)=arctan(x).
[0050] The method for continuous autonomous positioning of indoor and outdoor drones also includes:
[0051] Obtain the RTK global positioning information of the time period t before a certain moment and the continuous pose estimation results of the UAV based on the global map;
[0052] According to the RTK global positioning information and the continuous pose estimation result of the UAV based on the global map, the rotation and translation of the RTK trajectory within the time period t and the rotation and translation of the UAV trajectory based on the global map are calculated;
[0053] Align the RTK trajectory with the UAV trajectory to obtain the RTK trajectory points and the corresponding UAV trajectory points;
[0054] If the sum of the distances between the RTK track point and the UAV track point is less than the set threshold, the RTK global positioning information is valid, and the global map is updated using the first observation error and the second observation error;
[0055] If the sum of the distances between the RTK track point and the odometer track point is greater than or equal to the set threshold, the RTK global positioning information is invalid and only the first observation error is used to update the global map.
[0056] In a second aspect, the present application proposes an indoor and outdoor drone continuous autonomous positioning system, including: a global positioning module, a point cloud acquisition module, an image acquisition module, a positioning thread module, a mapping thread module and an update map output module, wherein the global positioning module, the point cloud acquisition module, and the image acquisition module are connected to the positioning thread module, the positioning thread module is connected to the mapping thread module, and the mapping thread module is connected to the update map output module;
[0057] The global positioning module is used to obtain RTK global positioning information using the RTK module on the drone;
[0058] Point cloud acquisition module, used to obtain laser point cloud using laser radar on the drone;
[0059] An image acquisition module, for using an RGB camera on the drone, wherein, to acquire an image, the camera viewing directions of the laser radar and the RGB camera are vertically downward relative to the drone, and the laser radar and the RGB camera have overlapping fields of view;
[0060] A positioning thread module is used to project the laser point cloud onto an image, extract feature points in the image after the laser point cloud is projected, and calculate the depth of each feature point. The matching feature points corresponding to the feature points are retrieved through historical image feature matching and nearest neighbor feature matching of the octree global map to obtain successfully matched feature point pairs. According to the successfully matched feature point pairs, the first observation error constructed using the depth of each feature point is used to estimate the posture of the UAV, thereby obtaining the estimated posture of the UAV.
[0061] A mapping thread module is used to obtain new feature points according to the estimated position and posture of the drone in the mapping thread, convert the new feature points to the world coordinate system, and put the new feature points converted to the world coordinate system into the global map to obtain a global map after updating the map elements, and complete the global map optimization according to the global map after updating the map elements and the second observation error constructed by using the RTK global positioning information to obtain an updated global map;
[0062] The map output module is updated to use the updated global map as the global map in the positioning thread when performing the next UAV positioning.
[0063] Beneficial effects:
[0064] This application proposes a method and system for continuous autonomous positioning of indoor and outdoor drones. On the one hand, it introduces laser radar-assisted visual odometer positioning, which can still output the current drone position in real time when satellite positioning fails due to building obstruction; on the other hand, based on limited RTK global positioning information, the drone has high repeated positioning accuracy when flying repeatedly between buildings; the real-time performance of the positioning algorithm is ensured by a dual-thread design. BRIEF DESCRIPTION OF THE DRAWINGS
[0065] Figure 1 A flow chart of a method for continuous autonomous positioning of indoor and outdoor drones according to an embodiment of the present application;
[0066] Figure 2 Schematic diagram of the installation of the RTK module, laser radar and RGB camera in the embodiment of the present application;
[0067] Figure 3 Feature point extraction flow chart of the embodiment of the present application;
[0068] Figure 4 Schematic diagram of laser point cloud projection on an image according to an embodiment of the present application;
[0069] Figure 5 The depth reliability weighted corner point extraction process of the embodiment of the present application;
[0070] Figure 6 Schematic diagram of the historical feature matching process in the embodiment of the present application;
[0071] Figure 7 Optimization schematic diagram of monocular visual odometer assisted by area array laser radar in an embodiment of the present application;
[0072] Figure 8 Schematic diagram of the octree structure of an embodiment of the present application;
[0073] Fig. 9 Schematic diagram of the existing system execution process;
[0074] Fig.10 Schematic diagram of the dual-thread system execution process of the embodiment of the present application;
[0075] Fig.11 A schematic diagram of the delay verification method flow in an embodiment of the present application;
[0076] Fig.12 A block diagram of an indoor and outdoor drone continuous autonomous positioning system according to an embodiment of the present application. DETAILED DESCRIPTION
[0077] The specific implementation of the present application is further described in detail below in conjunction with the drawings and examples.
[0078] Since there are two difficulties in drone positioning through vision or lidar odometer, there are some solutions in the existing technology, such as:
[0079] Chinese patent number "202111642896.X An absolute positioning method integrating visual inertial odometer and intermittent RTK" proposes to use the positioning results of VIO and the observation information of RTK to construct rolling optimization, and finally use the ceres optimizer to solve the optimal state estimation of the drone. At the same time, in view of the RTK intermittent problem, a calibration event smoothing scheme is designed to ensure the smooth flight of the drone. The paper "Zhang J, Singh S.INS Assisted Monocular Visual Odometry for Aerial Vehicles[J]. Springer Tracts in Advanced Robotics, 2015, 105: 183-197. DOI: 10.1007 / 978-3-319-07488-7_13." uses a downward-mounted camera and altimeter to assist the GPS positioning results, uses a filtering method to combine the camera odometer positioning results and the GPS positioning results, uses visual motion estimation to reduce the jump of GPS positioning, and uses the GPS positioning results to compensate for the cumulative error caused by visual motion estimation. The paper "Cao S, Lu X, Shen S. GVINS: Tightly Coupled GNSS–Visual–Inertial Fusion for Smooth and Consistent State Estimation [J]. IEEE Transactions on Robotics: A publication of the IEEE Robotics and Automation Society, 2022 (4): 38." uses the method of tightly coupling satellite raw data with VIO to achieve complementarity between VIO and GNSS. On the one hand, GNSS data is used to reduce the system's cumulative error. On the other hand, the tightly coupled system is more robust and can still be fused and solved even when there are less than 4 GNSS receiving satellites. The invention patent "201811306455.0 A SLAM method combining GPS and radar odometer" uses GPS combined with laser radar odometer to achieve positioning. When GPS is reliable, the position and posture obtained by GPS are used as the final positioning result. When GPS positioning is unreliable, the historical GPS information is used to optimize the laser odometer position and posture to obtain the final positioning result. This achieves autonomous positioning from with to without GPS. The invention patent "202210929221.1 Visual fusion lidar SLAM positioning and navigation method and its drone system" combines the laser odometry, visual odometry, and GPS positioning results, and determines the final fused positioning result by comparing the errors of multiple positioning results, thereby improving the adaptability of the drone positioning system in different environments.
[0080] However, there are also some difficult problems in the existing technology, such as:
[0081] The VIO fusion GPS solutions proposed in the Chinese patent number "202111642896.X An absolute positioning method integrating visual inertial odometer and intermittent RTK" and the paper "Cao S, Lu X, Shen S. GVINS: Tightly Coupled GNSS–Visual–Inertial Fusion for Smooth and Consistent State Estimation[J]. IEEE Transactions on Robotics: A publication of the IEEE Robotics and Automation Society, 2022(4):38." are both based on the normal operation of VIO. However, when the drone is flying at high altitude, RTK positioning is usually effective but VIO is invalid. When the drone enters a building complex from high altitude, the RTK signal is gradually affected. The above method does not have the ability to quickly initialize VIO and align the VIO positioning results with the GPS positioning results. The paper "Zhang J, Singh S.INS Assisted Monocular Visual Odometry for AerialVehicles[J].Springer Tracts in Advanced Robotics,2015,105:183-197.DOI:10.1007 / 978-3-319-07488-7_13." uses an altimeter to assist the visual odometer system to obtain a scaled motion estimation method. Although it can quickly calculate inter-frame motion, it assumes that the ground area observed by the camera during flight is relatively flat, and the use scenario is limited. Compared with the visual odometer, the invention patent "201811306455.0 A SLAM method combining GPS and radar odometer" can be initialized faster, which is conducive to the smooth switching from GPS positioning to odometer positioning. However, for the use of multi-line laser radar in urban environments, due to the single side facade structure of the building, the laser radar observation is prone to degradation; similarly, for the array laser radar, due to the limited observation field size, the observation degradation problem is prone to occur. The invention patent "202210929221.1 Visual fusion lidar SLAM positioning and navigation method and its drone system" obtains the optimal positioning result by combining multiple positioning modes and has stronger environmental adaptability. However, the pose estimation of a large amount of sensor information increases the computational overhead, which is not conducive to deployment on mobile devices such as drones.
[0082] Based on the above problems, this application proposes a method and system for continuous autonomous positioning of indoor and outdoor drones, which can be used for drone package delivery, high-rise rescue, inspection and other tasks. The key technologies to achieve this function include two aspects: 1. Use the RTK module to obtain globally consistent drone positioning results, and ensure that the drone can repeatedly and consistently complete the positioning task during flight without building an environmental map. 2. Design a visual odometer based on sparse point features of the image, and use the point cloud obtained by the area array laser radar to achieve depth estimation of single-frame image point features, thereby realizing the initialization of the monocular visual odometer. Further, in the subsequent inter-frame motion estimation process of the image, the depth estimation results of the image features are used to improve the positioning accuracy and ensure that the scale of the monocular visual odometer is consistent with the actual scale.
[0083] Embodiment 1:
[0084] This embodiment proposes a method for continuous autonomous positioning of indoor and outdoor drones. Figure 1 As shown, including:
[0085] Step S1: Use the RTK module on the drone to obtain RTK global positioning information;
[0086] Step S2: using the laser radar on the drone to obtain the laser point cloud;
[0087] Step S3: using an RGB (Red Green Blue) camera on the drone to acquire an image, wherein the camera observation directions of the laser radar and the RGB camera are vertically downward relative to the drone, and the laser radar and the RGB camera have overlapping fields of view;
[0088] In this embodiment, the RTK module, laser radar and RGB camera are used to realize the positioning of the drone, wherein the laser radar selects a planar laser radar, such as Figure 2 As shown, the observation directions of the array lidar and RGB camera are vertically downward relative to the drone to ensure that the two have a large overlapping field of view.
[0089] Step S4: In the positioning thread, the laser point cloud is projected onto the image, feature points in the image after the laser point cloud projection are extracted, and the depth of each feature point is calculated. The matching feature points corresponding to the feature points are retrieved through historical image feature matching and nearest neighbor feature matching of the octree global map to obtain successfully matched feature point pairs. According to the successfully matched feature point pairs, the first observation error constructed by the depth of each feature point is used to estimate the pose of the UAV, and the estimated pose of the UAV is obtained.
[0090] Step S5: In the map construction thread, new feature points are observed based on the estimated position and posture of the drone, the new feature points are converted to the world coordinate system, and the new feature points converted to the world coordinate system are put into the global map to obtain a global map after updating the map elements. According to the global map after updating the map elements and the second observation error constructed by using the RTK global positioning information, the global map optimization is completed to obtain an updated global map;
[0091] Step S6: When performing the next drone positioning, the updated global map is used as the global map in the positioning thread.
[0092] In step S4, the laser point cloud is projected onto the image, feature points in the image after the laser point cloud projection are extracted, and the depth of each feature point is calculated, such as Figure 3 Shown include:
[0093] Step S41.1: Projecting the laser point cloud onto the image, and selecting a target feature point in the image after the laser point cloud is projected;
[0094] Step S41.2: obtaining n points closest to the target feature point, and forming a first point cloud with the target feature point and the n points;
[0095] Step S41.3: In the camera coordinate system, the expectation and covariance of the first point cloud are calculated. The expectation of the first point cloud is a three-dimensional vector, and the calculation formula is as follows:
[0096]
[0097] Among them, μ is the expectation of the first point cloud, which is a three-dimensional vector used to represent the three-dimensional coordinates of the feature points on the image in the camera coordinate system, p is the three-dimensional point coordinates collected by the array laser radar in the camera coordinate system, σ 2 is a three-dimensional square matrix, is the covariance of the first point cloud, and n is the number of laser radar point clouds around the feature point on the image. After obtaining the plane composed of the laser radar point cloud, the plane and the feature point exist as follows Figure 4 The relationship shown in the figure is as follows, where the circular points are the three-dimensional points collected by the laser radar and the diamond points are the feature points. The relationship between the feature points and the plane is calculated as follows:
[0098]
[0099] in, and are both unit vectors, Through the covariance σ 2 Calculate the eigenvector with the smallest eigenvalue to obtain, is the back-projection vector of the feature point on the image, s is the scale factor, and by solving The value of is the position of the feature point in the camera coordinate system.
[0100] Step S41.4: Calculate the eigenvalue of the covariance of the first point cloud, and take the eigenvector with the smallest eigenvalue;
[0101] Step S41.5: specify a plane, a point on the plane is the expectation of the first point cloud, and the direction of the plane is the eigenvector with the smallest eigenvalue;
[0102] In terms of point feature selection on images, traditional point feature extraction methods such as ORB (Oriented FAST and Rotated BRIEF, ORB algorithm combines FAST key point detection algorithm and BRIEF feature extraction algorithm), SIFT (Scale-Invariant Feature Transform, scale-invariant feature transformation), etc., do not necessarily have 3D points collected by the array laser radar around the strong corner features on the image, making it difficult to accurately calculate the feature depth. Therefore, when extracting features, the depth reliability index k is introduced min The significance of the weighted feature points is used to select features that can accurately calculate the depth and correctly match. The depth reliability index is defined as the covariance σ 2 The minimum eigenvalue of 2 is semi-positive definite, so its minimum eigenvalue must not be less than 0. The basis is to make a plane assumption about the laser radar point cloud around the feature point: when the point cloud is very close to the feature point, the covariance σ 2 All eigenvalues are small, and when the point cloud is truly on a plane, the covariance σ 2 The minimum eigenvalue is still small, and the accurate coordinates of the feature points can be calculated at this time; on the contrary, when the minimum eigenvalue is large, it means that the laser radar point is far away from the feature or the area where the feature is located is not a plane, and the coordinates of the feature points cannot be accurately calculated at this time. It can be seen that σ 2 The minimum eigenvalue can reflect whether the three-dimensional coordinate calculation of the current feature point is accurate or not.
[0103] Take FAST corner extraction as an example. Figure 5 As shown in the figure, after completing the extraction of candidate corner points on the conventional image, the three-dimensional coordinates and covariance corresponding to the corner points are calculated by formulas (1) and (2). Then, the candidate corner points are screened according to the minimum eigenvalue of the covariance, so as to obtain feature points that are significant on the image and can calculate reliable depth. Among them, the depth of the feature point is obtained by the expected μ calculated by formula (1). Usually, the camera coordinate system defines the z-axis direction as the front, so the value of the feature point depth d is equal to the component of the vector μ in the z-axis direction.
[0104] Step S41.6: Calculate the intersection of the back-projected ray of the pixel point on the image after the laser point cloud projection and the plane, and use the intersection as the feature point in the image after the laser point cloud projection;
[0105] Step S41.7: Calculate the position coordinates of the feature point in the camera coordinate system according to the first point cloud and the feature vector with the smallest eigenvalue;
[0106] Step S41.8: In the camera coordinate system, define the z-axis direction as the front, and use the expected component of the first point cloud in the z-axis direction as the depth of each feature point.
[0107] In this embodiment, the use of the area array laser radar is mainly to obtain the depth information of the point features on the image. Compared with the general RGBD camera (RGBD, Red Green Blue Depth), the area array laser radar can detect the depth of farther points, but due to the limitations of the radar observation field and resolution, the point cloud obtained by the area array laser radar cannot accurately describe the depth of each pixel on the image. In this embodiment, the coordinates of the feature points in three-dimensional space are obtained by plane fitting, such as Figure 3 As shown, first, project the laser point cloud onto the image, select a feature point on the image as the target feature point, obtain the n point clouds closest to the target feature point on the image, and calculate the expectation and covariance of the first point cloud in the camera coordinate system for the first point cloud composed of the n point clouds and the target feature point, calculate the eigenvector with the smallest eigenvalue of the covariance eigenvalue of the first point cloud, specify a plane, a point on the plane as the expectation of the first point cloud, the plane direction as the eigenvector, and calculate the intersection of the back-projection ray of the pixel point on the image and the plane. The above method can be used to calculate the expectation and covariance of the point cloud, and the position coordinates of the feature point in the camera coordinate system can be calculated based on the first point cloud and the eigenvector with the smallest eigenvalue; at the same time, the component of the expectation of the first point cloud in the z-axis direction is used as the depth of each feature point.
[0108] In step S4, the matching feature points corresponding to the feature points are retrieved through the historical image feature matching and the nearest neighbor feature matching of the octree global map to obtain the successfully matched feature point pairs, including:
[0109] Step S42.1: Encode the global map using an octree, and save the encoded feature points, the spatial coordinates of the feature points, and descriptor information, wherein the descriptor information includes: a brief descriptor consisting of 256 Boolean values or a superpoint descriptor consisting of 128 floating point values;
[0110] Step S42.2: extract feature points Pi from the current image and the current laser point cloud;
[0111] Step S42.3: Calculate the descriptor distances between the feature point Pi and the feature points of the historical image respectively, sort the feature points of the historical image according to the descriptor distances from small to large, obtain the feature points with the smallest and second smallest descriptor distances on the historical image, and record them as the optimal matching point h1 and the second optimal matching point h2 respectively;
[0112] Step S42.4: Perform saliency detection on the optimal matching point and the suboptimal matching point using the descriptor information. If the saliency detection requirement is met, go to step S42.5;
[0113] Step S42.5: Calculate the descriptor distance between the optimal matching point h1 and all feature points on the current image, and sort the feature points of the current image from small to large according to the descriptor distance, and obtain the matching point P1 with the smallest descriptor distance between the optimal matching point h1 and the current image;
[0114] Step S42.6: Check the consistency of feature point Pi and matching point P1. If the consistency requirement is met, that is, Pi is equal to P1, then the matching feature point corresponding to feature point Pi is h1. Take h1 as the historical image matching feature point, and go to step S42.2 to obtain the next feature point matching result.
[0115] Step S42.7: If the feature point Pi does not meet the significance requirement or consistency requirement, find the feature point n1 closest to the three-dimensional coordinates of the feature point Pi in the global map through octree search, compare the descriptor distance between the two features, and when the descriptor distance is less than the preset value, the matching feature point corresponding to the feature point Pi is n1, and n1 is used as the nearest neighbor matching feature point;
[0116] Step S42.8: The feature point Pi is matched with the historical image matching feature point h1 or the feature point Pi is matched with the nearest neighbor matching feature point n1 to form a successfully matched feature point pair.
[0117] In this embodiment, after the feature point extraction is completed, the descriptor information corresponding to the feature point is extracted on the image by conventional methods. The descriptor information extraction method uses the general brief descriptor information or superpoint descriptor information, and then the descriptor is used to match the feature points. Feature matching is the process of matching the feature points of the current frame with the historical feature points in the map. Among them, the frame information is RTK global positioning information, laser point cloud, image, and estimated pose of the drone. The number of images depends on the processor performance and computing efficiency requirements. The smaller the number of images, the faster the calculation speed, but the lower the pose estimation accuracy. For the application of continuous positioning of drones, two matching methods are designed: historical image feature matching and nearest neighbor feature matching of the octree global map.
[0118] The historical image feature matching is achieved by comparing the descriptor distance between the current image feature points and the historical image feature points. The similarity evaluation index used by the brief descriptor information is the Hamming distance, and the similarity evaluation index used by the superpoint descriptor information is the Euclidean distance. The smaller the distance corresponding to the descriptor information, the higher the similarity of the feature points. Furthermore, in order to reduce the occurrence of mismatching, the significance check and cross-matching check are designed, such as Figure 6 As shown in the figure, D(p i ,h j ) function represents the feature point p i and h j The descriptor distance between them.
[0119] The nearest neighbor feature matching of the octree global map utilizes the continuous flight characteristics of the UAV to estimate the approximate UAV posture. Through the nearest neighbor search, multiple candidate nearest neighbor feature points of the current point feature can be directly obtained from the historical point features. Then, the descriptor distance between the current feature and the candidate nearest neighbor feature is compared, and the map feature point with the highest similarity is selected to complete the matching.
[0120] In step S4, the first observation error constructed by using the depth of each feature point is used to estimate the pose of the drone according to the successfully matched feature point pairs to obtain the estimated pose of the drone, including:
[0121] Step S43.1: Use the depth of each feature point to construct the first observation error, and the calculation formula is as follows:
[0122]
[0123] Among them, r p is the first observation error of the feature point, (u, v) is the coordinate of the feature point on the two-dimensional image, that is, the coordinate of the feature point Pi on the image, d is the depth of the feature point, obtained by the expected μ calculated by formula (1). Usually, the camera coordinate system defines the z-axis direction as the front, so the value of d is equal to the component of the vector μ in the z-axis direction, where the camera coordinate system is a three-dimensional rectangular coordinate system with the focal center of the camera as the origin and the optical axis as the z-axis. x is the calculated focal length of the camera in the x-axis direction, f y is the calculated focal length of the camera in the y-axis direction, c x is the calculated principal point on the camera x-axis, c y is the calculated principal point on the camera's y axis, (x c ,y c ,z c ) T is the three-dimensional coordinate of the feature point in the drone coordinate system, and the calculation formula is as follows:
[0124]
[0125] Among them, (x c ,y c ,z c ) T is the three-dimensional coordinate of the feature point in the world coordinate system, R cw is the rotation transformation matrix from the world coordinate system to the drone coordinate system, t cw It is the translation transformation vector from the world coordinate system to the drone coordinate system.
[0126] In this embodiment, a monocular visual odometer is assisted by an array laser radar, and the current observation features and camera pose are estimated based on a graph optimization method. The optimization content is as follows: Figure 7 As shown in , the frame information is the image continuously collected by the camera. The number of images depends on the processor performance and computing efficiency requirements. The smaller the number of images, the faster the calculation speed, but the lower the pose estimation accuracy. The observation error is shown in the calculation formula (4). Compared with the reprojection error, the observation error that introduces the feature depth information can obtain a scaled global map and pose estimation result.
[0127] Step S43.2: According to the minimum eigenvalue of the observation error and the covariance of the first point cloud, a least squares problem is constructed and solved to obtain the estimated position and posture of the drone. The calculation formula is as follows:
[0128]
[0129] in, is the optimal estimation result of the camera's estimated pose. Since the camera is fixed on the drone, the drone's pose can be obtained from the camera's pose through rigid transformation. To facilitate subsequent calculations, the drone's reference system is set to be consistent with the camera's reference system, that is, the camera's pose is equal to the drone's pose. is the axis-angle representation of the rotation matrix, The exponential mapping is the rotation transformation matrix from the world coordinate system to the drone coordinate system, is the translation transformation vector from the world coordinate system to the drone coordinate system, is the optimal estimated coordinate of the feature point in the world coordinate system, that is, the coordinate of the historical image matching feature point h1 or the coordinate of the nearest neighbor matching feature point n1, k min is the minimum eigenvalue of the covariance of the first point cloud, r p is the first observation error of the feature point. Parameter k min is the minimum eigenvalue of the covariance obtained by feature extraction, which is used to adjust the weights of different feature observation errors. Its function is shown in formula (7). Formula (7) is transformed from formula (6). p =(r p1,r p2 ,r p3 ) T , observation error r p3 Related to the measured depth r p3 =dz c , when the depth measurement of a feature point is unreliable, a larger k min Value makes becomes smaller, so the observation error r of the depth of this feature point p3 Suppressed.
[0130]
[0131] For the least squares problem, that is, formula (6), the Gauss-Newton or Levenburg-Marquadt method can be used to solve it. In order to make the solution accurate and fast, it is necessary to linearize the observation error during the iteration process, that is, solve the Jacobian of the observation error r for the drone's posture and coordinate points respectively. The results are shown in formulas (8), (9), (10):
[0132]
[0133] The symbol ·^ is an operator for finding the antisymmetric matrix of a vector, as shown in equation (11).
[0134]
[0135] Among them, p w =(p x ,p y ,p z ) T represents the feature points in the world coordinate system, represents the drone's position, is the axis-angle representation of the rotation matrix, Exponential mapping is the rotation transformation matrix from the world coordinate system to the drone coordinate system, t cw It is the translation transformation vector from the world coordinate system to the drone coordinate system.
[0136] In step S5, the global map optimization is completed according to the global map after the map elements are updated and the second observation error constructed by using the RTK global positioning information to obtain an updated global map, including:
[0137] Step S5.1: The second observation error constructed using the global positioning information is calculated as follows:
[0138]
[0139] Among them, R gcis the rotation transformation matrix from the drone coordinate system to the world coordinate system calculated by RTK, t gc is the translation transformation vector from the drone coordinate system to the world coordinate system calculated by RTK, R cw is the rotation transformation matrix from the world coordinate system to the drone coordinate system calculated by the global map, t cw is the translation transformation vector from the world coordinate system to the drone coordinate system calculated through the global map, r g is the second observation error.
[0140] Step S5.2: construct a second least squares problem and solve it according to the first observation error, the second observation error and the minimum eigenvalue of the covariance of the global map after updating the map elements, complete the global map optimization, and obtain the updated global map.
[0141] Wherein, the second least squares problem is constructed according to the first observation error, the second observation error and the minimum eigenvalue of the covariance of the global map after updating the map elements, and the calculation formula is as follows:
[0142]
[0143] in, is the optimal estimation result of the UAV’s estimated pose, is the axis-angle representation of the rotation matrix, The exponential mapping is the rotation transformation matrix from the world coordinate system to the drone coordinate system, is the translation transformation matrix from the world coordinate system to the drone coordinate system, is the optimal estimated coordinate of the feature point in the world coordinate system, that is, the coordinate of the historical image matching feature point h1 or the coordinate of the nearest neighbor matching feature point n1, r p is the first observation error of the feature point, r g is the second observation error of the feature point, ρ1(·) is the HuberLoss robust kernel function. When the first observation error of the feature point is large, this feature is considered difficult to accurately extract or match and track. Therefore, based on the HuberLoss characteristic ρ1(·), the influence of this feature on the overall least squares loss is reduced. The expression of ρ1(·) is shown in (14):
[0144]
[0145] ρ2(·) is the robust kernel function of ArctanLoss, which plays the same role as HuberLoss. The difference is that due to the influence of the nearest neighbor search range of feature matching, the range of the first observation error value is limited, while the second observation error can be very large. The arctan function is used to ensure that the overall least squares loss will not be affected by a global positioning result with a large deviation. The expression of ρ2(·) is shown in (15).
[0146] ρ2(x)=arctan(x) (15)
[0147] In this embodiment, the visual odometer assisted by the array laser radar can ensure that the positioning results are real-time and effective in complex environments. However, due to the initial posture deviation of the drone and the influence of the accumulated errors in the positioning process, the simple visual combined with laser radar posture estimation system cannot guarantee the consistency of the long-term and cyclic flight path of the drone. Among them, the odometer is used to obtain continuous and uninterrupted drone posture, and RTK as a global positioning module does not have the problem of error accumulation. However, during use, the RTK positioning results are not real-time and effective due to the influence of weather and building obstructions. Therefore, based on the visual combined with laser radar posture estimation and RTK characteristics, we add the observation error factor of the RTK positioning result to the graph optimization problem.
[0148] Since the RTK signal is not available during the entire flight of the drone, (R gc ,t gc ) represents the positioning result of a single RTK reception, and multiple RTK positioning results are used to reduce the cumulative error of the odometer. g When calculating the Jacobian of the drone's posture, the axis angle is used to simplify the calculation Indicates rotation, Get r g The Jacobian of the UAV posture is shown in formula (16), where: The function is the right-multiplied BCH approximate function.
[0149]
[0150] In this embodiment, first, the system map includes the posture information of the drone's continuous positioning, the global map composed of the image feature points calculated when the drone completes the posture estimation at different times, and the descriptor information of the feature points on each image. The frame information is stored in a linked list in chronological order; the map point information is stored in an octree structure due to the large number of points, such as Figure 8As shown, the purpose of using octree to encode feature points in this embodiment is to improve the efficiency of feature point search, that is, through the octree map, all feature points near a certain coordinate can be quickly found, which is conducive to the nearest neighbor matching of the feature. The difference is that for each feature point data, not only the coordinates of the feature point in space are saved, but also the corresponding descriptor information of the feature point on the image is saved, so that it can be used for nearest neighbor feature matching.
[0151] The continuous execution process of the system is as follows Fig. 9 As shown, Fig. 9 It includes two data collection and calculation processes. Among them, process ① feature extraction method is the feature extraction method of step S4 in this embodiment; process ② feature matching process is based on the feature points of the current frame, and the process of obtaining feature point pairs through historical image feature matching and nearest neighbor feature matching of the octree global map. Process ③ The current pose estimation method is completed by solving the least squares estimation problem of the first observation error; process ④ updating map elements is the process of converting the newly observed feature points of the current frame to the world coordinate system after the pose estimation of the current frame is completed, and then putting them into the octree-encoded global map; process ⑤ is based on the new observation optimization map, combined with the global positioning information obtained by the RTK module, the least squares optimization problem combining the first observation error and the second observation error is constructed and solved to complete the global map optimization.
[0152] On this basis, in order to improve the system computing efficiency, this embodiment designs a dual-thread mode, which reduces the waiting time of modules in the serial computing mode by stacking different tasks, thereby improving the overall efficiency of the system. Fig.10 As shown. Fig. 9 It can be found that the dual-thread mode has a greater data throughput and can receive more data, publish positioning and map results in a shorter time.
[0153] The method for continuous autonomous positioning of indoor and outdoor drones also includes:
[0154] Obtain the RTK global positioning information of the time period t before a certain moment and the continuous pose estimation results of the UAV based on the global map;
[0155] According to the RTK global positioning information and the continuous pose estimation result of the UAV based on the global map, the rotation and translation of the RTK trajectory within the time period t and the rotation and translation of the UAV trajectory based on the global map are calculated;
[0156] Align the RTK trajectory with the UAV trajectory to obtain the RTK trajectory points and the corresponding UAV trajectory points;
[0157] If the sum of the distances between the RTK track point and the UAV track point is less than the set threshold, the RTK global positioning information is valid, and the global map is updated using the first observation error and the second observation error;
[0158] If the sum of the distances between the RTK track point and the odometer track point is greater than or equal to the set threshold, the RTK global positioning information is invalid and only the first observation error is used to update the global map.
[0159] In this embodiment, in order to ensure that the RTK global positioning information obtained by the RTK module is reliable, this embodiment uses a delayed verification method to determine whether the RTK positioning result of the current road section is available. First, the RTK positioning results and odometer positioning results of the t time period before a certain moment are obtained, and the t value is generally taken as 10 to 20 seconds. Then, by calculating the rotation and translation of the RTK trajectory and the odometer trajectory within the t time period, the RTK trajectory is aligned with the odometer trajectory. Finally, whether the RTK is valid is determined based on the sum of the distances between the RTK trajectory points and the odometer trajectory points. This is based on the fact that the motion estimation results of the odometer in a short period of time are reliable, and the continuous pose estimation results based on the successfully matched feature point pairs in the current time period are used as a reference. When the RTK trajectory is consistent with the odometer trajectory, the RTK positioning result is considered correct, such as Fig.11 As shown in the figure, the dotted line is the odometer positioning track, the broken line is the RTK positioning track, and the sum of the distances between the RTK positioning point and the odometer positioning point in the statistical area is taken as the consistency error between RTK and odometer. When the consistency error is greater than the preset parameter, the RTK positioning result of the current segment is considered invalid. Figure 8 The RTK positioning result in area ① has a smaller consistency error than that calculated in area ②, and the RTK information in area ① is more accurate and reliable.
[0160] The present application proposes a method and system for continuous autonomous positioning of indoor and outdoor drones, which uses a planar array laser radar to assist in depth estimation of image feature points, thereby quickly initializing image features. In the feature extraction process, features with more accurate depth estimation are selected to improve the accuracy of feature point matching. A feature observation error function that includes both reprojection error and depth error is constructed, and an error function weighted by depth reliability index is proposed. A dual-thread mapping and positioning mode based on RTK is proposed to improve data throughput. Whether the current RTK information is valid is estimated based on a delay verification method. This embodiment is used for the building environment positioning task of drones. On the one hand, a visual odometer system assisted by a planar array laser radar is introduced, which can still output the current drone posture in real time when the satellite positioning fails due to building occlusion; on the other hand, based on the global positioning information obtained by a limited RTK module, the drone has a high repeat positioning accuracy when flying repeatedly between buildings. In terms of the system, the real-time performance of the positioning algorithm is ensured by a dual-thread design; and a delay consistency verification method is proposed to ensure that the RTK information used by the system is valid, so that the system has stronger robustness.
[0161] Embodiment 2:
[0162] This embodiment proposes a continuous autonomous positioning system for indoor and outdoor drones. Fig.12 As shown, it includes: a full global positioning module, a point cloud acquisition module, an image acquisition module, a positioning thread module, a mapping thread module and an update map output module, wherein the global positioning module, the point cloud acquisition module, the image acquisition module are connected to the positioning thread module, the positioning thread module is connected to the mapping thread module, and the mapping thread module is connected to the update map output module;
[0163] The global positioning module is used to obtain RTK global positioning information using the RTK module on the drone;
[0164] Point cloud acquisition module, used to obtain laser point cloud using laser radar on the drone;
[0165] An image acquisition module, for using an RGB camera on the drone, wherein, to acquire an image, the camera viewing directions of the laser radar and the RGB camera are vertically downward relative to the drone, and the laser radar and the RGB camera have overlapping fields of view;
[0166] A positioning thread module is used to project the laser point cloud onto an image, extract feature points in the image after the laser point cloud is projected, and calculate the depth of each feature point. The matching feature points corresponding to the feature points are retrieved through historical image feature matching and nearest neighbor feature matching of the octree global map to obtain successfully matched feature point pairs. According to the successfully matched feature point pairs, the first observation error constructed using the depth of each feature point is used to estimate the posture of the UAV, thereby obtaining the estimated posture of the UAV.
[0167] A mapping thread module is used to obtain new feature points according to the estimated position and posture of the drone in the mapping thread, convert the new feature points to the world coordinate system, and put the new feature points converted to the world coordinate system into the global map to obtain a global map after updating the map elements, and complete the global map optimization according to the global map after updating the map elements and the second observation error constructed by using the RTK global positioning information to obtain an updated global map;
[0168] The map output module is updated to use the updated global map as the global map in the positioning thread when performing the next UAV positioning.
[0169] The various embodiments in the present application are described in a progressive manner, and the same or similar parts between the various embodiments can be referenced to each other, and each embodiment focuses on the differences from other embodiments.
[0170] The protection scope of the present application is not limited to the above-mentioned embodiments. Obviously, those skilled in the art can make various changes and modifications to the present disclosure without departing from the scope and spirit of the present disclosure. If these changes and modifications fall within the scope of the claims of the present disclosure and their equivalents, the intention of the present disclosure also includes these changes and modifications.
Claims
1. A method for continuous autonomous positioning of indoor and outdoor drones, characterized in that: include: Use the RTK module on the drone to obtain RTK global positioning information; Use the laser radar on the drone to obtain the laser point cloud; Using an RGB camera on the drone to acquire an image, wherein the camera viewing directions of the laser radar and the RGB camera are vertically downward relative to the drone, and the laser radar and the RGB camera have overlapping fields of view; In the positioning thread, the laser point cloud is projected onto the image, the feature points in the image after the laser point cloud projection are extracted, and the depth of each feature point is calculated. The matching feature points corresponding to the feature points are retrieved through historical image feature matching and nearest neighbor feature matching of the octree global map to obtain successfully matched feature point pairs. According to the successfully matched feature point pairs, the first observation error constructed using the depth of each feature point is used to estimate the drone's pose, and the drone's estimated pose is obtained. In the map building thread, new feature points are observed based on the estimated position and posture of the drone, and the new feature points are converted to the world coordinate system. The new feature points converted to the world coordinate system are placed in the global map to obtain a global map after updating the map elements. Based on the global map after updating the map elements and the second observation error constructed using the RTK global positioning information, the global map is optimized to obtain an updated global map. When the next drone positioning is performed, the updated global map is used as the global map in the positioning thread.
2. The indoor and outdoor UAV continuous autonomous positioning method according to claim 1 is characterized in that: The step of projecting the laser point cloud onto the image, extracting feature points in the image after the laser point cloud projection, and calculating the depth of each feature point includes: Projecting the laser point cloud onto the image, and selecting a target feature point in the image after the laser point cloud is projected; Obtain n points closest to the target feature point, and form a first point cloud with the target feature point and the n points; In the camera coordinate system, calculating the expectation and covariance of the first point cloud, where the expectation of the first point cloud is a three-dimensional vector; Calculate the eigenvalue of the covariance of the first point cloud and take the eigenvector with the smallest eigenvalue; Specify a plane, a point on the plane is the expectation of the first point cloud, and the direction of the plane is the eigenvector with the smallest eigenvalue; Calculate the intersection of the back-projected ray of the pixel point on the image after the laser point cloud is projected and the plane, and use the intersection as a feature point in the image after the laser point cloud is projected; Calculate the position coordinates of the feature point in the camera coordinate system according to the first point cloud and the feature vector with the smallest eigenvalue; In the camera coordinate system, the z-axis direction is defined as the front of the drone's flight, and the expected component of the first point cloud in the z-axis direction is used as the depth of each feature point.
3. The indoor and outdoor UAV continuous autonomous positioning method according to claim 1, characterized in that: The process of retrieving matching feature points corresponding to the feature points through historical image feature matching and nearest neighbor feature matching of the octree global map to obtain successfully matched feature point pairs includes: Step S42.1: Encode the global map using an octree, and save the encoded feature points, the spatial coordinates of the feature points, and descriptor information, wherein the descriptor information includes: a brief descriptor consisting of 256 Boolean values or a superpoint descriptor consisting of 128 floating point values; Step S42.2: extract feature points Pi from the current image and the current laser point cloud; Step S42.3: Calculate the descriptor distances between the feature point Pi and the feature points of the historical image respectively, sort the feature points of the historical image according to the descriptor distances from small to large, obtain the feature points with the smallest and second smallest descriptor distances on the historical image, and record them as the optimal matching point h1 and the second optimal matching point h2 respectively; Step S42.4: Use the descriptor information to perform saliency detection on the optimal matching point and the suboptimal matching point. If the saliency detection requirement is met, go to step S42.5; Step S42.5: Calculate the descriptor distance between the optimal matching point h1 and all feature points on the current image, and sort the feature points of the current image from small to large according to the descriptor distance, and obtain the matching point P1 with the smallest descriptor distance between the optimal matching point h1 and the current image; Step S42.6: Check the consistency of feature point Pi and matching point P1. If the consistency requirement is met, that is, Pi is equal to P1, then the matching feature point corresponding to feature point Pi is h1. Take h1 as the historical image matching feature point, and go to step S42.2 to obtain the next feature point matching result. Step S42.7: If the feature point Pi does not meet the significance requirement or consistency requirement, find the feature point n1 closest to the three-dimensional coordinates of the feature point Pi in the global map through octree search, compare the descriptor distance between the two features, and when the descriptor distance is less than the preset value, the matching feature point corresponding to the feature point Pi is n1, and n1 is used as the nearest neighbor matching feature point; Step S42.8: The feature point Pi is matched with the historical image matching feature point h1 or the feature point Pi is matched with the nearest neighbor matching feature point n1 to form a successfully matched feature point pair.
4. The indoor and outdoor UAV continuous autonomous positioning method according to claim 3 is characterized in that: The method of estimating the posture of the UAV by using the first observation error constructed by the depth of each feature point according to the successfully matched feature point pairs to obtain the estimated posture of the UAV includes: The first observation error is constructed using the depth of each feature point; According to the first observation error and the minimum eigenvalue of the covariance of the first point cloud, a first least squares problem is constructed and solved to obtain the estimated position and pose of the drone.
5. The indoor and outdoor UAV continuous autonomous positioning method according to claim 4, characterized in that: The first observation error is constructed by using the depth of each feature point, and the calculation formula is as follows: Among them, r p is the first observation error of the feature point, (u, v) is the coordinate of the feature point on the two-dimensional image, that is, the coordinate of the feature point Pi on the image, d is the depth of the feature point, f x is the calculated focal length of the camera in the x-axis direction, f y is the calculated focal length of the camera in the y-axis direction, c x is the calculated principal point on the camera x-axis, c y is the calculated principal point on the camera's y axis, (x c ,y c ,z c ) T is the three-dimensional coordinate of the feature point in the UAV coordinate system.
6. The indoor and outdoor UAV continuous autonomous positioning method according to claim 4, characterized in that: According to the minimum eigenvalue of the covariance of the observation error and the first point cloud, a least squares problem is constructed and solved to obtain the estimated posture of the drone. The calculation formula is as follows: in, is the optimal estimation result of the camera's estimated pose. The estimated pose of the drone at the current moment is obtained based on the optimal estimation result of the camera's estimated pose. is the axis-angle expression of the rotation matrix, Exponential mapping is the rotation transformation matrix from the world coordinate system to the drone coordinate system, is the translation transformation vector from the world coordinate system to the drone coordinate system, is the optimal estimated coordinate of the feature point in the world coordinate system, that is, the coordinate of the historical image matching feature point h1 or the coordinate of the nearest neighbor matching feature point n1, that is, the coordinate of the nearest neighbor matching feature point n1, k min is the minimum eigenvalue of the covariance of the first point cloud, r p is the first observation error of the feature point.
7. The indoor and outdoor UAV continuous autonomous positioning method according to claim 1, characterized in that: The method of completing global map optimization according to the global map after the map elements are updated and the second observation error constructed using the RTK global positioning information to obtain an updated global map includes: The second observation error constructed using RTK global positioning information; According to the first observation error, the second observation error and the minimum eigenvalue of the covariance of the global map after updating the map elements, a second least squares problem is constructed and solved to complete the global map optimization and obtain an updated global map.
8. The indoor and outdoor UAV continuous autonomous positioning method according to claim 7, characterized in that: The second observation error constructed using the RTK global positioning information is calculated as follows: Among them, R gc is the rotation transformation matrix from the drone coordinate system to the world coordinate system calculated by RTK, t gc is the translation transformation from the UAV coordinate system to the world coordinate system calculated by RTK, R cw is the rotation transformation matrix from the world coordinate system to the drone coordinate system calculated by the global map, t cw is the translation transformation vector from the world coordinate system to the drone coordinate system calculated through the global map, r g is the second observation error; The second least squares problem is constructed according to the first observation error, the second observation error and the minimum eigenvalue of the covariance of the global map after updating the map elements. The calculation formula is as follows: in, is the optimal estimation result of the UAV’s estimated pose, is the axis-angle representation of the rotation matrix, is the translation transformation matrix from the world coordinate system to the drone coordinate system, is the optimal estimated coordinate of the feature point in the world coordinate system, that is, the coordinate of the historical image matching feature point h1 or the coordinate of the nearest neighbor matching feature point n1, r p is the first observation error of the feature point, r g is the second observation error of the feature point, ρ1(·) is the HuberLoss robust kernel function, and the expression of ρ1(·) is as follows: ρ2(·) is the ArctanLoss robust kernel function, and the expression of ρ2(·) is as follows: ρ2(x)=arctan(x).
9. The indoor and outdoor UAV continuous autonomous positioning method according to claim 1, characterized in that: The method for continuous autonomous positioning of indoor and outdoor drones also includes: Obtain the RTK global positioning information of the time period t before a certain moment and the continuous pose estimation results of the UAV based on the global map; According to the RTK global positioning information and the continuous pose estimation result of the UAV based on the global map, the rotation and translation of the RTK trajectory within the time period t and the rotation and translation of the UAV trajectory based on the global map are calculated; Align the RTK trajectory with the UAV trajectory to obtain the RTK trajectory points and the corresponding UAV trajectory points; If the sum of the distances between the RTK track point and the UAV track point is less than the set threshold, the RTK global positioning information is valid, and the global map is updated using the first observation error and the second observation error; If the sum of the distances between the RTK track point and the odometer track point is greater than or equal to the set threshold, the RTK global positioning information is invalid and only the first observation error is used to update the global map.
10. A continuous autonomous positioning system for indoor and outdoor drones, characterized in that: include: A global positioning module, a point cloud acquisition module, an image acquisition module, a positioning thread module, a mapping thread module and an update map output module, wherein the global positioning module, the point cloud acquisition module and the image acquisition module are connected to the positioning thread module, the positioning thread module is connected to the mapping thread module, and the mapping thread module is connected to the update map output module; The global positioning module is used to obtain RTK global positioning information using the RTK module on the drone; Point cloud acquisition module, used to obtain laser point cloud using laser radar on the drone; An image acquisition module, for using an RGB camera on the drone, wherein, to acquire an image, the camera viewing directions of the laser radar and the RGB camera are vertically downward relative to the drone, and the laser radar and the RGB camera have overlapping fields of view; A positioning thread module is used to project the laser point cloud onto an image, extract feature points in the image after the laser point cloud is projected, and calculate the depth of each feature point. The matching feature points corresponding to the feature points are retrieved through historical image feature matching and nearest neighbor feature matching of the octree global map to obtain successfully matched feature point pairs. According to the successfully matched feature point pairs, the first observation error constructed using the depth of each feature point is used to estimate the posture of the UAV, thereby obtaining the estimated posture of the UAV. A mapping thread module is used to obtain new feature points according to the estimated position and posture of the drone in the mapping thread, convert the new feature points to the world coordinate system, and put the new feature points converted to the world coordinate system into the global map to obtain a global map after updating the map elements, and complete the global map optimization according to the global map after updating the map elements and the second observation error constructed by using the RTK global positioning information to obtain an updated global map; The map output module is updated to use the updated global map as the global map in the positioning thread when performing the next UAV positioning.
Citation Information
Cited By
Four-rotor unmanned aerial vehicle seamless positioning method based on multi-source fusion
CN120779443A
