CN112529936B - A Monocular Sparse Optical Flow Algorithm for Outdoor UAVs - Google Patents

Abstract

The invention relates to the technical field of target tracking, in particular to a monocular sparse optical flow algorithm for an outdoor unmanned aerial vehicle, which comprises the following steps of: s1: extracting and tracking characteristic points of two adjacent frames of gray images, and marking the characteristic points as an original sample point set; s2: removing invalid characteristic points in the original sample point set to obtain a sampling sample point set, and obtaining an optimal model H through the sampling sample point set _best The method comprises the steps of carrying out a first treatment on the surface of the S3: according to the optimal model H _best Obtaining the speed of the unmanned aerial vehicle under an image coordinate system; s4: the speed in the image coordinate system is converted to the speed in the camera coordinate system. According to the scheme, invalid characteristic points in the original sample point set are removed, so that the influence of the characteristic points generated by the shadow of the unmanned aerial vehicle in the flight process can be eliminated, and the speed estimation of the unmanned aerial vehicle in the outdoor flight is more accurate.

Description

A Monocular Sparse Optical Flow Algorithm for Outdoor UAVs

technical field

The invention relates to the technical field of target tracking, and more particularly relates to a monocular sparse optical flow algorithm for outdoor drones.

Background technique

The concept of optical flow was first proposed by James J.Gibson in the 1940s, which refers to the speed of pattern motion in time-varying images. Because when an object is moving, the brightness pattern of its corresponding point on the image is also moving. The apparent movement of the brightness of this image is the optical flow. Since it contains the information of the target movement, it can be used by the observer. Determine the movement of the target.

Optical flow is often used in motion image analysis. It is the instantaneous speed of the pixel movement of the spatial moving object on the observation imaging plane, and the grayscale image of the previous frame and the adjacent two frames is found by using the change of pixels in the image sequence in the time domain and the correlation between adjacent frames. The corresponding relationship between the current frames in the frame can be used to calculate the motion information of the object between adjacent frames. The optical flow algorithm can be divided into dense optical flow and sparse optical flow in terms of solution scale. Dense optical flow needs to be solved for each pixel on the image, and the amount of calculation is large; while sparse optical flow only focuses on the feature points on the image such as corner points or edge points, and the amount of calculation is small, so it is suitable for embedded devices. It is mostly used in the UAV flight control system to implement calculations to obtain the speed of the UAV itself. The classic sparse optical flow algorithm was first proposed by Lucas and Kanade in 1981, so it is also called L-K optical flow. The L-K optical flow algorithm needs to satisfy three basic assumptions: the first one is the assumption of constant brightness. The assumption of constant brightness states that the brightness of points representing the same object in two consecutive frames remains constant. The second is the time-continuity assumption. The temporal continuity hypothesis points out that in two consecutive frames of pictures, the displacement of the object is relatively small. The third is the spatial consistency assumption. The spatial consistency assumption states that adjacent points in the graph belong to the same surface and have similar motions. Then use the least square method to solve the basic optical flow equation for all pixels in the neighborhood.

Chinese patent CN106989744A discloses a method for obtaining optical flow velocity, which adopts the method of Shi-Tomasi corner point detection for each frame of grayscale image, selects 100 feature points with the most obvious texture information, and selects 3* The pixel window of 3 is used as a pixel unit, and the pixel window position in the previous frame grayscale image in the two adjacent grayscale images is used as the initial position of the pixel window in the next frame grayscale image to establish a search area, using Lucas -Kanade reverse multiplication algorithm, using a five-layer optical flow pyramid model, using the least squares method, by making the pixel window of the previous frame in two adjacent grayscale images search for gray in the search area of the grayscale image of the next frame The minimum degree difference and the minimum position of the pixel window of the next frame are obtained, and the distance difference between the pixel windows of the two frames is the optical flow vector, and the optical flow velocity is obtained through the difference.

However, when the UAV is flying outdoors, it will produce shadows under the sunlight. When the edge points and corner points of the UAV’s own shadow are selected as feature points, it cannot correctly reflect the movement speed of the UAV relative to the bottom surface. . Therefore, this method is not suitable for outdoor UAV flight control.

Contents of the invention

In order to overcome the deficiencies in the above-mentioned prior art, the present invention provides a monocular sparse optical flow algorithm for outdoor drones, which can eliminate the influence of its own shadow in the speed recognition process of the drone, and improve the speed of the drone. Estimation accuracy.

In this technical solution, a monocular sparse optical flow algorithm for outdoor drones is provided, including the following steps:

S1: Extract and track the feature points of two adjacent grayscale images, which are recorded as the original sample point set;

S2: Eliminate invalid feature points in the original sample point set to obtain a sampling sample point set, and obtain the optimal model H _best by sampling the sample point set;

S3: Obtain the speed of the UAV in the image coordinate system according to the optimal model H _best ;

S4: Convert the speed in the image coordinate system to the speed in the camera coordinate system.

In this solution, by eliminating invalid feature points in the original sample point set, the influence of feature points generated by the UAV’s own shadow during flight can be eliminated, making the speed estimation of the UAV more accurate when flying outdoors.

Preferably, in the above-mentioned step S1, the relative motion equations of the feature points of two adjacent frames are expressed as:

p' ^[i] = Hp ^[i] ,

Among them, p' ^[i] = [x' ^[i] , y' ^[i] , 1] is the i-th feature point of the current frame in two adjacent grayscale images, p ^[i] = [x ^[i] , y ^[i] , 1] is the i-th feature point of the previous frame in two adjacent gray-scale images, H is the model with the most feature points in the gray-scale image, and is the homography matrix, h ₁₃ and h ₂₃ respectively Indicates the translation of the drone in the x-direction and y-direction in the camera coordinate system. Solve h ₁₃ and h ₂₃ to get the speed of the UAV in the image coordinates.

Preferably, in the above step S2, the optimal model H _best is obtained by using the random sampling consensus algorithm, which specifically includes the following steps:

S21: Randomly search for _mi pairs of sample points in the original sample point set;

S22: Use m _i to obtain model H for sample points;

S23: Calculate the intra-office point set I _i according to the model H, and obtain the current optimal intra-office point set I _best according to the intra-office point set I _i ;

S24: Update the model H according to the current optimal intra-office point set I _best , and record it as the current optimal model H _best ;

S25: Calculate the error sum J _sum of all sample points in the original sample point set to the current optimal model H _best , and judge whether J _sum is smaller than the total model error threshold E, if so, output the current optimal model H _best , if not, then Go to step S26;

S26: Set the maximum number of cycles K, execute step S21 to step S25 cyclically, obtain and output the current optimal model H _best .

Preferably, in the above-mentioned step S22, the model H is obtained by using a homogeneous equation system, and the specific formula is:

Among them, (x ^[n] , y ^[n] ) and (x′ ^[n] , y′ ^[n ]) are the nth group of sample pairs, and x ^[1] is the previous frame of two adjacent grayscale images The x-axis position of the first feature point in the image, y ^[1] is the y-axis position of the first feature point in the image of the previous frame in two adjacent grayscale images, and x ^[n] is the two adjacent gray-scale images The x-axis position of the mth feature point in the previous frame in the frame grayscale image, and y ^[1] is the y-axis position of the nth feature point in the previous frame in the two adjacent grayscale images, x' ⁽¹⁾ is the x-axis position of the first feature point in the current frame in the two adjacent grayscale images, and y' ^{(1) is} the first feature point in the current frame in the two adjacent grayscale images The y-axis position in the image, x' ^[n] is the x-axis position of the mth feature point in the image in the current frame in the two adjacent grayscale images, and y' ^[n] is the x-axis position in the two adjacent grayscale images The y-axis position of the nth feature point in the image in the previous frame.

Preferably, h ₃₃ in the above-mentioned model H is set to 1, since model H is calculated using a homogeneous equation.

Preferably, the aforementioned m is greater than or equal to 4, which is set because after h ₃₃ is set to 1, the model H still has 8 unknowns, and 8 points are needed to solve it.

Preferably, the above step S23 specifically includes the following steps:

S231: set the maximum number of cycles N;

S232: Calculate the error J ^[n] between all sample pairs in the original sample point set and the model H. The specific formula is:

J ^[n] ＝||p ^[n] '-Hp ^[n] || ₂ ;

Wherein, p ^[n] ' is the nth feature point of the previous frame in two adjacent grayscale images, and p ^[n] is the nth feature point of the current frame in the adjacent two grayscale images;

S233: Determine whether J ^[n] is smaller than the error threshold e, if so, classify the nth feature point into the optimal intra-office point set I _best , if not, classify the n-th feature point into the out-of-field point set to remove;

S234: Repeat step S232 to step S233 to obtain intra-office point set I _i ;

S235: Obtain the quantities S _i and S _best of all elements in the intra-office point set I _i and the optimal intra-office point set I _best respectively, and judge whether S _i is smaller than S _best , if so, make I _best = I _best , if not, then Let I _best = I _i ;

Among them, i is the number of loop execution times, i≤N.

Preferably, since the optical flow camera of the UAV is generally installed at the bottom, and the lens is vertically downward, when the UAV is flying at medium and low speeds, it always maintains this small pitch angle and roll angle, so it is considered that the camera coordinate system is The speed of is the speed of the UAV relative to the lower surface, and the specific formula converted in the above step S4 is:

v _c = Kv _p ,

Among them, v _p is the speed of the UAV in the image coordinate system, v _c is the speed of the UAV in the camera coordinate system, and K is the internal parameter matrix of the camera.

In order to improve the reliability of the speed estimation of the UAV, a step S5 is also included: using Kalman filter fusion to estimate the optimal speed of the UAV.

Preferably, the above step S5 specifically includes the following steps:

S51: UAVs generally carry an inertial measurement unit. At this time, the acceleration is obtained according to the onboard inertial measurement unit of the UAV, and the UAV velocity v _i is obtained by integrating the acceleration;

S52: Use the velocity model in the prediction stage of the Kalman filter, and use the _vc obtained in step S4 in the update stage of the Kalman filter to obtain the optimal velocity estimate v of the UAV.

Compared with the prior art, the beneficial effect is: the present invention eliminates the invalid feature points in the original sample point set by using the random sampling consensus algorithm, which can eliminate the influence of the feature points produced by the UAV's own shadow during the flight process , which makes the speed estimation of UAVs more accurate when flying outdoors; in addition, after calculating the speed of UAVs, the inertial measurement unit is used to construct the speed model of UAVs, and the fusion of UAV speeds and speed models is estimated through Kalman filtering. The speed of man-machine further improves the reliability and accuracy of speed estimation, which is conducive to the better application of UAV flight control system.

Description of drawings

Fig. 1 is the schematic flow chart of the monocular sparse optical flow algorithm that the present invention is used for outdoor UAV;

Fig. 2 is a schematic diagram of extracting and tracking feature points in the monocular sparse optical flow algorithm used for outdoor drones in the present invention, wherein the feature points generated by the shadow of the drone are inside the rectangular frame;

Figure 3 is the visualization effect diagram calculated by using the classic optical flow algorithm on Figure 2;

Fig. 4 is a visualization effect diagram calculated on Fig. 2 by using the monocular sparse optical flow algorithm for outdoor drones of the present invention.

Detailed ways

The accompanying drawings are for illustrative purposes only, and should not be construed as limitations on this patent; in order to better illustrate this embodiment, certain components in the accompanying drawings will be omitted, enlarged or reduced, and do not represent the size of the actual product; for those skilled in the art It is understandable that some well-known structures and descriptions thereof may be omitted in the drawings. The positional relationship described in the drawings is for illustrative purposes only, and should not be construed as a limitation on this patent.

In the drawings of the embodiments of the present invention, the same or similar symbols correspond to the same or similar components; The orientation or positional relationship indicated by "long" and "short" are based on the orientation or positional relationship shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the referred device or element must have a specific Orientation, construction and operation in a specific orientation, so the terms describing the positional relationship in the drawings are for illustrative purposes only, and should not be construed as limitations on this patent. For those of ordinary skill in the art, it can be understood according to specific circumstances The specific meaning of the above terms.

Below by specific embodiment, in conjunction with accompanying drawing, the technical solution of the present invention is described in further detail:

Example 1

Figure 1 is an embodiment of a monocular sparse optical flow algorithm for outdoor drones, including the following steps:

S1: Use the Shi-Tomasi corner detection method to extract and track the feature points of two adjacent grayscale images, which are recorded as the original sample point set; of course, other methods can be used to detect feature points in the grayscale image, which will not be done here limited;

S2: Eliminate invalid feature points in the original sample point set to obtain a sampling sample point set, and obtain the optimal model H _best by sampling the sample point set;

S3: Obtain the speed of the UAV in the image coordinate system according to the optimal model H _best ;

S4: Convert the speed in the image coordinate system to the speed in the camera coordinate system.

In step S1 in the present embodiment, the relative motion equations of the feature points of two adjacent frames are expressed as:

p' ^[i] = Hp ^[i] ,

Among them, p' ^[i] = [x' ^[i] , y' ^[i] , 1] is the i-th feature point of the current frame in two adjacent grayscale images, p ^[i] = [x ^[i] , y ^[i] , 1] is the i-th feature point of the previous frame in two adjacent grayscale images, H is the model with the most feature points in the grayscale image, and is a homography matrix. Due to the pitch of the UAV Angle and roll angle are close to zero, so h ₁₂ , h ₂₂ , h ₁₃ , h ₂₃ are all close to 0, so h ₁₃ and h ₂₃ represent the translation amount of the drone in the x direction and y direction in the camera coordinate system respectively , solve the h ₁₃ and h ₂₃ to get the speed of the UAV in the image coordinates.

In step S2 in this embodiment, the optimal model H _best is obtained by using the random sampling consensus algorithm, which specifically includes the following steps:

S21: Randomly search for _mi pairs of sample points in the original sample point set;

S22: Use m _i to obtain model H for sample points;

S23: Calculate the intra-office point set I _i according to the model H, and obtain the current optimal intra-office point set I _best according to the intra-office point set I _i ;

S24: Update the model H according to the current optimal intra-office point set I _best , and record it as the current optimal model H _best ;

S25: Calculate the error sum J _sum of all sample points in the original sample point set to the current optimal model H _best , and judge whether J _sum is smaller than the total model error threshold E, if so, output the current optimal model H _best , if not, then Go to step S26;

S26: Set the maximum number of cycles K, execute step S21 to step S25 cyclically, obtain and output the current optimal model H _best .

In the step S22 among the present embodiment, utilize homogeneous equations to obtain model H, concrete formula is:

Among them, (x ^[n] , y ^[n] ) and (x′ ^[n ], y′ ^[n ]) are the nth group of sample pairs, and x ^[1] is the previous frame of two adjacent grayscale images The x-axis position of the first feature point in the image, y ^[1] is the y-axis position of the first feature point in the image of the previous frame in two adjacent grayscale images, and x ^[n] is the two adjacent gray-scale images The x-axis position of the mth feature point in the previous frame in the frame grayscale image, and y ^[1] is the y-axis position of the nth feature point in the previous frame in the two adjacent grayscale images, x' ⁽¹⁾ is the x-axis position of the first feature point of the current frame in the two adjacent grayscale images, and y' ^(1] is the first feature point of the current frame in the two adjacent grayscale images In the y-axis position in the image, x' ^[n] is the x-axis position of the mth feature point in the image in the current frame of the two adjacent grayscale images, and y' ^[n] is the two adjacent grayscale images The y-axis position of the nth feature point in the image in the previous frame.

h ₃₃ in the model H in this embodiment is set to 1, because the model H is calculated using a homogeneous equation.

In this embodiment, m is greater than or equal to 4. This setting is because after h ₃₃ is set to 1, the model H still has 8 unknowns, and 8 points are needed to solve it, that is, at least 4 pairs of sample points are required.

In step S23 in this embodiment, specifically include the following steps:

S231: set the maximum number of cycles N;

S232: Calculate the error J ^[n] between all sample pairs in the original sample point set and the model H. The specific formula is:

J ^[n] ＝||p ^[n] '-Hp ^[n] || ₂ ;

Wherein, p ^[n] ' is the nth feature point of the previous frame in two adjacent grayscale images, and p ^[n] is the nth feature point of the current frame in the adjacent two grayscale images;

S233: Determine whether J ^[n] is smaller than the error threshold e, if so, classify the nth feature point into the optimal intra-office point set I _best , if not, classify the n-th feature point into the out-of-field point set to remove;

S234: Repeat step S232 to step S233 to obtain intra-office point set I _i ;

S235: Obtain the numbers S _i and s _best of all elements in the intra-office point set I _i and the optimal intra-office point set I _best respectively, and judge whether S _i is smaller than S _best , if so, make I _best = I _best , if not, then Let I _best = I _i ;

Among them, i is the number of loop execution times, i≤N.

In this embodiment, since the optical flow camera of the UAV is generally installed at the bottom, and the lens is vertically downward, when the UAV is flying at medium and low speeds, the pitch angle and roll angle of the UAV are considered to be zero, so the camera is considered The speed under the coordinate system is the speed of the UAV relative to the lower surface. The specific formula converted in step S4 is:

v _c = Kv _p ,

Among them, v _p is the speed of the UAV in the image coordinate system, v _c is the speed of the UAV in the camera coordinate system, and K is the internal parameter matrix of the camera.

Example 2

The only difference between this embodiment and Embodiment 1 is that step S5 is further included: using Kalman filter fusion to estimate the optimal speed of the UAV. This further improves the reliability of the drone's velocity estimate.

Since the UAV generally carries an inertial measurement unit, step S5 in this embodiment specifically includes the following steps:

S51: Obtain the acceleration according to the onboard inertial measurement unit of the drone, and obtain the velocity v _i of the drone by integrating the acceleration;

S52: Use the velocity model in the prediction stage of the Kalman filter, and use the _vc obtained in step S4 in the update stage of the Kalman filter to obtain the optimal velocity estimate v of the UAV.

As shown in Figures 2 to 4, use the monocular sparse optical flow algorithm for outdoor drones of the present invention to process the pictures acquired by the optical flow camera of the drone, wherein the original sample point set is processed by using the random sampling consensus algorithm Invalid feature points, that is, the feature points produced by the shadow of the drone in the grayscale image acquired by the optical flow camera, can be eliminated, that is, the feature points produced by the shadow of the drone during its flight can be eliminated, and its influence can be avoided. This makes the speed estimation of the UAV more accurate when flying outdoors.

Apparently, the above-mentioned embodiments of the present invention are only examples for clearly illustrating the present invention, rather than limiting the implementation of the present invention. For those of ordinary skill in the art, other changes or changes in different forms can be made on the basis of the above description. It is not necessary and impossible to exhaustively list all the implementation manners here. All modifications, equivalent replacements and improvements made within the spirit and principles of the present invention shall be included within the protection scope of the claims of the present invention.

Claims (7)

1. A monocular sparse optical flow algorithm for an outdoor unmanned aerial vehicle, comprising the steps of:

s1: extracting and tracking characteristic points of two adjacent frames of gray images, and marking the characteristic points as an original sample point set;

s2: removing invalid characteristic points in the original sample point set to obtain a sampling sample point set, and obtaining an optimal model H through the sampling sample point set _best The method comprises the steps of carrying out a first treatment on the surface of the Obtaining an optimal model H by using a random sampling consistency algorithm _best The method specifically comprises the following steps:

s21: randomly finding m in the original sample point set _i Sample points;

s22: by m _i Obtaining a model H for the sample points; the homogeneous equation set is utilized to calculate a model H, and the specific formula is as follows:

wherein, (x) ^[n] ,y ^[n] ) And (x' ^[n] ,y′ ^[n] ) For the nth group of sample pairs, x ^[1] For the position of the first characteristic point of the previous frame in the adjacent two frames of gray level images in the x axis and y ^[1] For the position of the first characteristic point of the previous frame in the adjacent two frames of gray level images in the y axis of the image, x ^[n] For the position of the x-axis and y of the m-th feature point of the previous frame in the adjacent two frames of gray level images in the image ^[n] For the position of the nth characteristic point of the previous frame in the adjacent two frames of gray images in the y axis of the image, x' ⁽¹⁾ For the x-axis position, y of the first characteristic point of the current frame in the image in two adjacent frames of gray scale images ^′(1] For the position of the first characteristic point of the current frame in the two adjacent frames of gray images in the y axis of the image, x' ^[n] For the position of the m-th characteristic point of the current frame in the adjacent two frames of gray images in the x-axis of the image, y' ^[n] Is the first frame in two adjacent frames of gray level imagesThe y-axis positions of the n feature points in the image;

s23: calculating according to the model H to obtain an intra-local point set I _i And according to the intra-office point set I _i Obtaining a current optimal local point set I _best The method comprises the steps of carrying out a first treatment on the surface of the The method specifically comprises the following steps:

s231: setting the maximum cycle number N;

s232: calculating the error J between all pairs of samples in the original sample point set and the model H ^[n] The specific formula is as follows:

J ^[n] ＝||p ^[n] ′-Hp ^[n] || ₂ ；

wherein p is ^[n] ' is the nth characteristic point of the previous frame in two adjacent frames of gray level images, p ^[n] The n-th feature point of the current frame in the gray level images of two adjacent frames;

s233: judgment of J ^[n] If the error threshold value is smaller than the error threshold value e, if so, the nth characteristic point is classified into an optimal local point set I _best If not, classifying the nth characteristic point into an outlier set for eliminating;

s234: repeatedly executing the steps S232 to S233 to obtain the local point set I _i ；

S235: respectively find out the point set I in the office _i And an optimal local Point set I _best All element numbers S in (3) _i And S is _best Judgment of s _i Whether or not it is smaller than s _best If yes, let I _best ＝I _best If not, make I _best ＝I _i ；

Wherein i is the number of times of cyclic execution, i is less than or equal to N;

s24: according to the current optimal local point set I _best Updating the model H and recording the model H as a current optimal model H _best ；

S25: calculating the current optimal model H of all sample points in the original sample point set _best Error sum J of (2) _sum Judgment of J _sum Whether the error threshold value is smaller than the total model error threshold value E, if so, outputting a current optimal model H _best If not, go to step S26;

s26: setting the maximum number of loops K, and circularly executing the steps S21 to S25 to obtainAnd outputs the current optimal model H _best ；

S3: according to the optimal model H _best Obtaining the speed of the unmanned aerial vehicle under an image coordinate system;

s4: the speed in the image coordinate system is converted to the speed in the camera coordinate system.

2. The monocular sparse optical flow algorithm for an outdoor unmanned aerial vehicle of claim 1, wherein the step S1 represents the relative motion equation of the feature points of two adjacent frames as:

p′ ^[i] ＝Hp ^[i] ,

wherein p' ^[i] ＝[x′ ^[i] ,y′ ^[i] ,1]For the ith feature point, p of the current frame in two adjacent frames of gray level images ^[i] ＝[x ^[i] ,y ^[i] ,1]The i-th characteristic point of the previous frame in two adjacent frames of gray images is represented by H, the model of the most characteristic points in the gray images is represented by homography matrix, and H ₁₃ And h ₂₃ Respectively represent the translation amount of the unmanned aerial vehicle in the x direction and the y direction in the image coordinate system.

3. The monocular sparse optical flow algorithm for an outdoor drone of claim 1, wherein H of H in the model ₃₃ Set to 1.

4. The monocular sparse optical flow algorithm for an outdoor unmanned aerial vehicle of claim 1, wherein m in step S21 is 4 or more.

5. The monocular sparse optical flow algorithm for an outdoor drone of claim 1, wherein the specific formula of the conversion in step S4 is:

v _c ＝v _p ，

wherein v is _p For the speed of the unmanned aerial vehicle in the image coordinate system, v _c The speed of the unmanned aerial vehicle under the camera coordinate system is represented by K, which is an internal reference matrix of the camera.

6. The monocular sparse optical flow algorithm for an outdoor drone of claim 5, further comprising step S5: and estimating the optimal speed of the unmanned aerial vehicle by using Kalman filtering fusion.

7. The monocular sparse optical flow algorithm for an outdoor drone of claim 6, wherein step S5 specifically comprises the steps of:

s51: the acceleration is obtained according to the unmanned aerial vehicle-mounted inertial measurement unit, and the unmanned aerial vehicle speed v is obtained by integrating the acceleration _i ；

S52: using the velocity model in the prediction stage of Kalman filtering, and using v obtained in the step S4 _c And the updating stage is used for Kalman filtering to obtain the optimal speed estimation v of the unmanned aerial vehicle.