A UAV indoor positioning method based on monocular vision

Abstract

The present invention discloses a method for indoor positioning of unmanned aerial vehicles based on monocular vision, which belongs to the field of computer vision technology, including a unmanned aerial vehicle posture estimation module and a mapping module, which is suitable for tracking and self-positioning of unmanned aerial vehicle flight trajectories in indoor GPS-deficient environments. The main idea is: through the image frames taken by the unmanned aerial vehicle's airborne camera, the visual SLAM algorithm is used to estimate the posture of the unmanned aerial vehicle's airborne camera in real time, and at the same time, a sparse point cloud map of the current environment is established. Pose estimation part: combining the feature point method and the direct method, first track the feature points, and then use the direct method to perform block matching according to the key points to estimate the camera posture. Mapping part: first determine whether the current image is a key frame, if so, extract features for it, and then initialize a depth filter to perform depth estimation of the feature points. Otherwise, update the depth filter, and then determine whether the depth distribution of the feature point converges, if so, add the 3D point of this feature to the atlas.

Description

Technical Field

[0001] The invention belongs to the field of computer vision, and specifically designs an indoor unmanned aerial vehicle self-positioning method. Background Art

[0002] In recent years, drone technology has developed rapidly. In the military, the emergence of drones has changed the face of modern warfare. On the other hand, drones represented by brands such as DJI have also received unprecedented attention in the civilian field.

[0003] Taking civilian consumer drones as an example, it is well known that drones mainly rely on global positioning systems represented by GPS to achieve their own positioning functions. However, it is difficult to locate indoors where GPS signals are missing. On the other hand, traditional indoor positioning technology mainly uses Bluetooth, WIFI, base station positioning and other methods to integrate into a positioning system. However, this type of method is greatly affected by the environment and often requires the deployment of multiple devices. In addition, this method can only obtain distance information, and cannot know the posture of the device in space.

[0004] Visual SLAM provides a solution for the positioning of drones in environments without GPS signals, which is of great significance for the self-positioning of drones. It is mainly divided into two functions: positioning and mapping. Positioning mainly calculates the position and posture of the drone in the current environment based on the image input by the camera, so as to obtain information in the environment; the role of mapping is to establish the corresponding relationship between the surrounding environment and space. Summary of the invention

[0005] The purpose of the present invention is to provide a UAV indoor positioning method based on monocular vision, which can effectively solve the current problem of UAV positioning in an environment where GPS signals are missing, such as indoors.

[0006] The present invention provides the following technical solutions:

[0007] A method for indoor positioning of a UAV based on monocular vision, comprising:

[0008] The pose estimation module combines the feature point method and the direct method to first track the feature points and then use the direct method to perform block matching based on the key points to estimate the camera pose.

[0009] The mapping module is used to build a sparse map of the surrounding environment in real time. It estimates the depth of feature points in the keyframe by building a deep filter and maps the converged feature points to the atlas.

[0010] A method for indoor positioning of a drone based on monocular vision comprises the following steps:

[0011] Step 1, using the first two frames of the camera to initialize the system;

[0012] Step 2: Based on the first two frames of the camera image extracted in step 1, the camera pose relative to the previous frame image is obtained by minimizing the photometric error between the projection positions of the corresponding 3D points between the two images;

[0013] Step 3: Predict the position of the feature block in the reference frame in the current frame using the initial camera pose obtained in step 2. Due to the presence of noise, there is a certain error between the estimated 3D point and camera pose and the true value. The solution is to optimize the predicted position of the feature block by optimizing the photometric error between the corresponding feature block in the current frame and the previous key frame.

[0014] Step 4: Using the feature block prediction position that has been optimized in step 3, the camera pose and feature block position are optimized by minimizing the photometric error between the projection positions of the corresponding 3D points between the two frames of images again. The optimization items here can be selected according to the actual situation.

[0015] Step 5: Based on steps 1 to 4, the pose change of the drone's onboard camera can be obtained. The next task is to build a map. First, add the current image to a frame sequence, and then determine whether the current frame meets the key frame screening conditions of the system. If it is a key frame, go to step 6, otherwise jump to step 8;

[0016] Step 6: Extract FSAT features from the current key frame. To avoid the phenomenon of "clustering" of FAST corner points, a non-maximum suppression method is used to retain only the corner points with the maximum response value in a certain area.

[0017] Step 7: Based on the FAST feature points extracted in step 6, a depth filter is initialized, and the initial value is set to the average depth of the 3D points corresponding to the feature blocks in the current key frame, and then the matching points are determined by using the epipolar line search method;

[0018] Step 8: When the current frame is a non-key frame, update the depth filter initialized by the previous key frame and determine whether the feature point has converged. If so, add the 3D point corresponding to the feature point to the map for subsequent camera pose estimation.

[0019] Preferably, the drone airborne camera pose estimation algorithm comprises the following steps:

[0020] S1: Use the first two frames of the camera to initialize the system. First, when the image just comes in, obtain its pyramid image, 5 layers, and a ratio of 2. The purpose of constructing the image pyramid is to obtain images of different resolutions by downsampling at different levels. Then detect the FAST feature points and edge features. Starting from the first image, use the optical flow method to continuously track the feature points, calculate the initial position of the camera, and perform triangulation initialization at the same time. The triangulation formula is: s1x1=s2Rx2+t, where: x1 x2 is the normalized coordinates of the two feature points, s1 s2 represents the depth of the two feature points to be solved, and Rt represents the rotation and translation of the camera;

[0021] S2: Use the direct method to minimize the image block reprojection residual to obtain the pose, which is done by continuously optimizing the pose T k,k-1 Minimize the residual loss function: Where: u represents the pixel coordinate, δI(T,u)=I k (π(T.π -1 (u,d u )))-I k-1 (u), where: I k represents the intensity image acquired at time step k, d u represents the depth value of the image coordinates, and π(.) represents the camera projection model. This nonlinear least squares problem can be solved iteratively using the Gauss-Newton method, and the iterative increment ξ (Lie algebra) of the pose can be calculated using the following equation: T Jξ=-J T δI(0). The Jacobian matrix is ​​the derivative of the image residual with respect to the Lie algebra, which can be obtained by the chain derivation rule: In the process of continuous iterative optimization, the camera's posture is constantly corrected;

[0022] S3: Since the estimated camera pose is biased, the predicted feature block position is biased. To solve this problem, the Gauss-Newton method can be used to optimize the predicted position of this feature block. The specific method is to reconstruct a residual and optimize the feature prediction position: The optimization variable here is the pixel position, A i is a reversible matrix representing an affine transformation. After this step, the predicted position of the feature point is optimized;

[0023] S4: The pose of the camera in the current frame can be obtained through the inter-frame matching of S2, but this frame-to-frame pose estimation method will inevitably lead to cumulative errors, resulting in drift. Therefore, the pose of the current frame is further constrained by the established map model. Because the predicted position of the feature block is optimized in S3, the direct method is used again to construct a pure motion BA problem to further optimize the camera pose. The specific approach is to minimize the reprojection error established in the previous feature alignment step:

[0024] Preferably, the drone mapping algorithm comprises the following steps:

[0025] S1: First, determine whether the current frame is a key frame. If so, perform FAST feature extraction on the current key frame. In order to avoid the "clustering" of FAST corner points, the non-maximum suppression method is used to retain only the corner points with the maximum response value in a certain area. Then initialize a depth filter, set the initial value to the average depth of the 3D points corresponding to the feature blocks in the current key frame, and then use the polar line search method to determine the matching points;

[0026] S2: If the current frame is not a key frame, update the depth filter and map the converged feature points to the atlas.

[0027] Beneficial Effects

[0028] The beneficial effect of the present invention is that the present invention first uses the first two frames of images from the drone's onboard camera to initialize the system, and then performs pose initialization through image alignment based on a sparse model. Since there is a certain deviation between the estimated values ​​and actual values ​​of the three-dimensional points and camera poses, minimizing the photometric error between the corresponding feature blocks in the current frame and the previous key frame can further optimize the 2D position of each feature block, and finally, through these optimized feature points, the direct method is used again to optimize the camera pose. The advantage of this is that the cumulative error caused by inter-frame pose estimation is eliminated. At the same time, the mapping module determines the depth value of the matching feature point by means of depth filtering and polar line search, converts it to world coordinates and adds it to the atlas, solving the positioning problem of drones in environments where GPS signals are missing, such as indoors. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 It is a schematic diagram of the process of the present invention;

[0030] Figure 2 This is a schematic diagram of the FAST corner point model in the present invention; DETAILED DESCRIPTION

[0031] The present invention will be further described below in conjunction with the accompanying drawings:

[0032] The present invention provides a method for indoor positioning of a UAV based on monocular vision. Specifically, the indoor positioning system of the UAV comprises: a posture estimation module of a UAV-mounted camera and a mapping module.

[0033] like Figure 1 As shown, a method for indoor positioning of a UAV based on monocular vision comprises the following steps:

[0034] Step 1, using the first two frames of the camera to initialize the system;

[0035] Step 2: Based on the first two frames of the camera image extracted in step 1, the camera pose relative to the previous frame image is obtained by minimizing the photometric error between the projection positions of the corresponding 3D points between the two images;

[0036] Step 3: Predict the position of the feature block in the reference frame in the current frame using the initial camera pose obtained in step 2. Due to the presence of noise, there is a certain error between the estimated 3D point and camera pose and the true value. The solution is to optimize the predicted position of the feature block by optimizing the photometric error between the corresponding feature block in the current frame and the previous key frame.

[0037] Step 4: Using the feature block prediction position that has been optimized in step 3, the camera pose and feature block position are optimized by minimizing the photometric error between the projection positions of the corresponding 3D points between the two frames of images again. The optimization items here can be selected according to the actual situation.

[0038] Step 5: Based on steps 1 to 4, the pose change of the drone's onboard camera can be obtained. The next task is to build a map. First, add the current image to a frame sequence, and then determine whether the current frame meets the key frame screening conditions of the system. If it is a key frame, go to step 6, otherwise jump to step 8;

[0039] Step 6: Extract FSAT features from the current key frame. To avoid the phenomenon of "clustering" of FAST corner points, a non-maximum suppression method is used to retain only the corner points with the maximum response value in a certain area.

[0040] Step 7: Based on the FAST feature points extracted in step 6, a depth filter is initialized, and the initial value is set to the average depth of the 3D points corresponding to the feature blocks in the current key frame, and then the matching points are determined by using the epipolar line search method;

[0041] Step 8: When the current frame is a non-key frame, update the depth filter initialized by the previous key frame and determine whether the feature point has converged. If so, add the 3D point corresponding to the feature point to the map for subsequent camera pose estimation.

Claims

1. A UAV indoor positioning method based on monocular vision, characterized in that: include; Step 1, using the first two frames of the camera to initialize the system; Step 2: Based on the first two frames of the camera image extracted in step 1, the camera pose relative to the previous frame image is obtained by minimizing the photometric error between the projection positions of the corresponding 3D points between the two images; Step 3: Predict the position of the feature block in the reference frame in the current frame using the initial camera pose obtained in step 2. Due to the presence of noise, there is a certain error between the estimated 3D point and camera pose and the true value. The solution is to optimize the predicted position of the feature block by optimizing the photometric error between the corresponding feature block in the current frame and the previous key frame. Step 4: Using the predicted position of the feature block that has been optimized in step 3, the camera pose and the position of the feature block are optimized by minimizing the photometric error between the projection positions of the corresponding 3D points between the two frames of images again; Step 5: Based on steps 1 to 4, the pose change of the drone's airborne camera can be obtained. The next task is to build a map. First, add the current image to a frame sequence, and then determine whether the current frame meets the key frame screening conditions of the system. If it is a key frame, go to step 6, otherwise jump to step 8. Step 6: Extract FSAT features from the current key frame. To avoid the "clustering" of FAST corner points, a non-maximum suppression method is used to retain only the corner points with the maximum response value in a certain area. Step 7: Based on the FAST feature points extracted in step 6, a depth filter is initialized, and the initial value is set to the average depth of the 3D points corresponding to the feature blocks in the current key frame, and then the matching points are determined by using the epipolar line search method; Step 8: When the current frame is a non-key frame, update the depth filter initialized by the previous key frame and determine whether the feature point has converged. If so, add the 3D point corresponding to the feature point to the map for subsequent camera pose estimation.

2. A method for indoor positioning of a UAV based on monocular vision as claimed in claim 1, characterized in that: The step 1 is specifically as follows: S1: When the image just comes in, its pyramid image is obtained, with 5 layers and a scale of 2. The purpose of constructing the image pyramid is to obtain images of different resolutions by downsampling at different levels, which solves the weakness of FSAT corner points that do not have scale; S2: Detect FAST feature points and edge features; if the number of feature points in the middle of the image exceeds 50, take this image as the first key frame; S3: Then process the continuous images after the first one to initialize the triangle with the first one; starting from the first image, use the optical flow method to continuously track the feature points and calculate the initial position of the camera; The triangulation formula is: s1x1=s2Rx2+t, where: x1 x2 is the normalized coordinates of the two feature points, s1 s2 represents the depth of the two feature points to be solved, and R t represents the rotation and translation of the camera.

3. A method for indoor positioning of a UAV based on monocular vision as claimed in claim 1, characterized in that: The step 2 is specifically as follows: S1: Take the previous frame as the reference frame and the pose of the previous frame as the initial pose of the current frame; S2: Combine all the tiles on the reference frame with the map points and project them onto the pyramid image of the current frame. On the pyramid image of the current frame, start from the highest layer and calculate layer by layer to the lower layers, each time inheriting the optimization result of the previous one. S3: The residual to be optimized is the brightness residual of the block of the feature point on the reference frame and the block at the position projected on the current frame. The specific formula is: δI(T,u)=I k (π(T.π -1 (u,d u )))-I k-1 (u), where: I k represents the intensity image collected at time step k, u represents the pixel coordinate, d u Represents the depth value of the image coordinates, and π(.) represents the camera projection model.

4. A method for indoor positioning of a UAV based on monocular vision as claimed in claim 1, characterized in that: The step 6 is specifically as follows: S1: Select pixel p in the image, assuming its brightness is I p ; S2: Set a threshold T to I p 20%; S3: Taking pixel p as the center, select 16 pixels on a circle with a radius of 3, as shown in Figure 2; S4: If there are N consecutive points on the selected circle with brightness greater than I p +T or less than I p -T, then pixel p can be considered as a feature point; S5: In order to prevent corner points from "clustering", the non-maximum suppression method is used to retain only the corner points with the maximum response value in a certain area; S6: Repeat the above steps.

Images