A vision-based autonomous tracking and landing method for dynamic platform UAV

Abstract

The present invention discloses a method for autonomous tracking and landing of a dynamic platform UAV based on vision, which relates to the field of autonomous landing technology of UAV, and aims to solve the problem of large positioning deviation and low landing accuracy caused by nonlinear motion and excessive interference in the dynamic situation of non-uniform linear motion of the landing platform. The key points of the technical solution are to use AprilTag as a landing mark, detect and locate the landing platform through vision, and obtain the coordinates of the landing platform in the body coordinate system through coordinate conversion. The relative speed of the landing platform is estimated using the coordinates of the two moments before and after, and the abnormal speed data caused by the detection error is eliminated by the local weighted K nearest neighbor algorithm. The observed data is used as the input of the attenuated memory volume Kalman filter for trajectory prediction, and the UAV is controlled to track the landing platform. In addition, during the landing process, the height of the UAV is adaptively estimated in real time by integrating the camera, laser rangefinder and the barometer data of the UAV, and the rotor is stopped in time within the landing height, and the electromagnet is turned on to adsorb the landing platform to prevent the UAV from falling from the landing platform and being damaged. This method performs well in velocity estimation, trajectory prediction and landing accuracy. The average error of trajectory prediction is 28% higher than that of the standard volumetric Kalman filter method, and the average landing deviation can be controlled within 12 cm.

Description

Technical Field

[0001] The present application relates to the technical field of autonomous landing of unmanned aerial vehicles, and in particular to a vision-based autonomous tracking and landing method for a dynamic platform unmanned aerial vehicle. Background Art

[0002] In recent years, air-ground collaboration has attracted widespread attention from the academic community because of its ability to perform smarter tasks with higher efficiency, flexibility, and robustness in complex and diverse environments. Using ground unmanned vehicles to charge drones or transport drones to target areas and recover drones after completing the mission effectively makes up for the shortcomings of poor drone endurance. The ability of drones to quickly and accurately land on ground platforms for charging in a limited space is a prerequisite for ensuring multiple autonomous flights of drones. Therefore, how to stably and accurately land at a designated location, that is, to achieve autonomous landing of drones, is of great significance to improving the autonomy of drones. However, the common autonomous landing methods for drones currently have two defects: first, most solutions require the ground platform to park at a pre-selected position and remain stationary or at a constant speed, which results in low landing efficiency and makes it difficult to ensure uniform motion due to environmental influences; second, the commonly used navigation methods are based on the Global Positioning System (GPS) or Inertial Navigation System (INS). When using the INS, positioning errors will accumulate over time, making it impossible to guarantee landing accuracy and affecting the flight safety of the drone. As for GPS, the positioning accuracy of commercial-grade GPS can only reach the meter level, which cannot meet the navigation and landing requirements in complex environments such as densely built cities. When there are too many obstructions, the GPS signal may even be lost, resulting in landing failure. Summary of the invention

[0003] The present application provides a vision-based dynamic platform UAV autonomous tracking and landing method, the technical purpose of which is to improve the landing accuracy of the UAV.

[0004] The above technical objectives of this application are achieved through the following technical solutions:

[0005] A vision-based autonomous tracking and landing method for a dynamic platform UAV, comprising:

[0006] Step 1: Obtain the GPS information of the landing platform on the unmanned vehicle and perform preliminary positioning. The drone flies to the vicinity of the landing platform based on the preliminary positioning and then starts the visual guidance program;

[0007] Step 2: The drone detects whether the landing platform is in its field of view. If so, it goes to step 3, otherwise it returns to step 1.

[0008] Step 3: Detect the logo of the landing platform, perform image processing on the logo to obtain the coordinates of the landing platform in the pixel coordinate system, and convert the coordinates in the pixel coordinate system into the coordinates in the body coordinate system;

[0009] Step 4: Estimate the speed of the landing platform relative to the drone based on the drone body coordinates and time interval at the previous and next moments, and use the local weighted K-nearest neighbor algorithm to eliminate abnormal speed data to obtain the estimated speed;

[0010] Step 5: According to the coordinates of the landing platform's logo in the body coordinate system, the motion trajectory of the landing platform is predicted using the decaying memory volumetric Kalman filter algorithm;

[0011] Step 6: According to the estimated speed of the drone and the motion trajectory of the landing platform, control the drone to fly toward the center of the landing platform;

[0012] Step 7: Fuse the multi-sensor information to obtain the height information of the UAV from the landing platform. If the landing height is reached, execute step 8; otherwise, descend 0.2 meters and return to step 2;

[0013] Step 8: The drone's rotor stops, and the electromagnet is turned on to attract the landing platform to complete the landing.

[0014] The beneficial effects of the present application are as follows: the vision-based dynamic platform UAV autonomous tracking and landing method described in the present application gets rid of the restrictions on the landing platform position and speed of the original UAV landing scheme, does not require the ground platform to be parked at a pre-selected position and remain stationary or move at a constant speed, is less affected by the environment, and has high landing efficiency; it uses visual guidance, which is more accurate than GPS and INS, and will not affect the landing accuracy due to signal weakening caused by surrounding occlusion. The present application optimizes speed estimation through a local weighted K nearest neighbor algorithm, uses a decaying memory volumetric Kalman filter for trajectory prediction, and adaptively fuses multi-source sensor information to estimate the height of the UAV. It has good performance in speed estimation, trajectory prediction, and landing accuracy. The average error of trajectory prediction is 28% higher than that of the standard volumetric Kalman filter method, and the average landing offset can be controlled within 12 cm. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 This is a flow chart of a vision-based autonomous tracking and landing method for a dynamic platform drone in an embodiment of the present application;

[0016] Figure 2a A diagram showing the conversion relationship between a pixel coordinate system and an image coordinate system in an embodiment of the present application;

[0017] Figure 2b A diagram showing the conversion relationship between the image coordinate system and the camera coordinate system in the embodiment of the present application;

[0018] Figure 3 This is a comparison diagram of speed estimation effects in the embodiments of the present application;

[0019] Figure 4 This is a trajectory prediction error comparison diagram in the embodiment of the present application;

[0020] Figure 5 This is a diagram showing the landing accuracy effect in an embodiment of the present application. DETAILED DESCRIPTION

[0021] The technical solution of the present application will be described in detail below with reference to the accompanying drawings.

[0022] This application takes a six-rotor drone performing an autonomous landing mission as an example. The camera is fixed to the drone vertically downward, with the centers 30 cm apart. The x-axis direction of the camera is the same as the y-axis direction of the drone. An AprilTag logo consisting of an outer white border and an 8×8 grid containing binary information inside is affixed to the center of the landing platform.

[0023] The process of the vision-based dynamic platform drone autonomous tracking and landing method described in this application is as follows: Figure 1 As shown, specifically including:

[0024] Step 1: Obtain the GPS information of the landing platform on the unmanned vehicle and perform preliminary positioning. The drone flies to the vicinity of the landing platform based on the preliminary positioning and then starts the visual guidance program.

[0025] First, the landing platform uses its own GPS to obtain its own longitude and latitude. The UAV obtains the platform's longitude and latitude information through communication between the landing platform and the UAV. After the UAV flies to the corresponding longitude and latitude position, it starts the visual guidance program.

[0026] Step 2: The drone detects whether the landing platform is in its field of view. If so, proceed to step 3; otherwise, return to step 1.

[0027] Specifically, the mark attached to the landing platform is detected. If the mark is within the field of view of the camera, the subsequent landing process is entered to further process and respond to the mark information. If the mark is outside the field of view of the camera, the drone rises to expand the field of view and starts timing. If the mark is found within the set time range, the subsequent landing process is entered. If it times out, it returns to the previous step and communicates with the landing platform again to obtain the longitude and latitude information.

[0028] Step 3: Detect the logo of the landing platform, perform image processing on the logo to obtain the coordinates of the landing platform in the pixel coordinate system, and convert the coordinates in the pixel coordinate system into coordinates in the body coordinate system.

[0029] After a series of image processing, including image grayscale, image thresholding, contour extraction, etc., the coordinates (u0, v0) of the logo center in the pixel coordinate system are obtained.

[0030] like Figure 2a As shown, the coordinates are converted from the pixel coordinate system (P) to the image coordinate system (I), expressed as:

[0031]

[0032] Among them, d x and d y Represents the length of the unit pixel in the x and y directions; (u, v) represents the coordinates of the logo center in the image coordinate system.

[0033] like Figure 2b As shown, the coordinates are transformed from the image coordinate system to the camera coordinate system (C), expressed as:

[0034]

[0035] Where f represents the focal length of the camera; subscript C represents the coordinates in the camera coordinate system.

[0036] Convert the coordinates from the camera coordinate system to the body coordinate system (B), expressed as:

[0037]

[0038] The subscript B represents the coordinates in the body coordinate system.

[0039] Finally, the conversion formula from pixel coordinate system to body coordinate system is as follows:

[0040]

[0041] Step 4: Estimate the speed of the landing platform relative to the drone based on the drone body coordinates and time interval at the previous and next moments, and use the local weighted K-nearest neighbor algorithm to eliminate abnormal speed data to obtain the estimated speed.

[0042] Specifically, step 4 includes:

[0043] Step 41: Obtain the coordinates of the drone body in the two frames of images at two moments in time, and perform differential calculation on the coordinates of the drone body to obtain the speed of the landing platform relative to the drone, which is expressed as:

[0044]

[0045] in, It represents the speed of the landing platform relative to the drone on the x-axis in the body coordinate system at time t; It represents the speed of the landing platform relative to the drone on the y-axis in the body coordinate system at time t; Represents the x-axis coordinate of the drone body in the body coordinate system at time t; Represents the x-axis coordinate of the drone body in the body coordinate system at time t-1; Represents the UAV body coordinate of the y-axis in the body coordinate system at time t; represents the UAV body coordinate of the y-axis in the body coordinate system at time t-1; Δt represents the time interval between time t and time t-1;

[0046] Step 42: When the relative speed of the landing platform is large, the position of the target between adjacent frames may change greatly. At this time, the speed estimation will produce a large error, making the calculated relative speed too large or too small, which will affect the tracking of the drone. The estimated speed anomaly belongs to the context anomaly in time series data. Based on KNN, a local weighted anomaly detection algorithm and dynamic threshold are proposed to detect and eliminate abnormal data.

[0047] Specifically, the local weighted K-nearest neighbor algorithm is used to remove abnormal speed data, including: comparing the weighted average distance with the dynamic threshold. If the weighted average distance is greater than the dynamic threshold, it means that the estimated speed is an abnormal speed, and the abnormal speed is removed. Then, the estimated speed at the previous moment is used to replace the abnormal speed. Specifically, it is expressed as:

[0048]

[0049] in, Represents the weighted average distance between the current sample data and the data points at the previous K moments; Represents the weighted average of the data points at the previous K moments; ω t-i represents the weight at time ti; λ represents the decay rate; T dyn represents the current threshold, that is, the dynamic threshold; α represents the dynamic coefficient.

[0050] Local weighted KNN performs anomaly detection based on distance. It calculates the weighted average distance between a sample and its nearest K samples, and compares the calculated weighted average distance with the dynamic threshold (the product of the weighted average of the nearest K samples and the dynamic coefficient). If the weighted average distance is greater than the threshold, the corresponding sample is considered abnormal, and then the abnormal speed is replaced by the estimated speed value of the previous moment.

[0051] Step 5: According to the coordinates of the landing platform's logo in the body coordinate system, the decaying memory volumetric Kalman filter algorithm is used to predict the motion trajectory of the landing platform.

[0052] Aiming at the limitation of camera frame rate and the problem of sudden target loss, a position estimation method based on decaying memory volumetric Kalman filter is proposed. The process includes prediction step, update step and estimation step. The prerequisites are as follows: the state vector Covariance matrix P k , process noise Q and measurement noise R. In this application, an attenuation factor b is introduced to process Q and R, which is expressed as:

[0053]

[0054] Specifically, the step 5 includes:

[0055] Step 51: In the prediction step, the volume points are first calculated, which is expressed as:

[0056]

[0057] Among them, P k represents the covariance matrix; represents the state vector; represents the volume point; n represents the dimension of the state vector; represents the volume point set, [I] = [I n ,-I n ];

[0058] Then the state of the volume point is predicted, which is expressed as:

[0059] in, represents the volume point through which the output propagates through the state equation;

[0060] The target state vector and error covariance matrix are predicted according to the volume point state, which can be expressed as:

[0061]

[0062] in, represents the predicted value of the target state vector; P k+1|k represents the predicted value of the error covariance matrix; Q represents the process noise, b represents the attenuation factor;

[0063] Step 52: In the update step, the volume point is calculated according to the target state vector of the prediction step and the predicted value of the error covariance matrix, which is expressed as:

[0064]

[0065] The volume point measurement is predicted based on the volume point, expressed as: in, represents the volume point where the output propagates through the measurement equation;

[0066] The target measurement is predicted based on the volume point measurement, expressed as:

[0067]

[0068] in, represents the predicted value of the next measurement step;

[0069] The measurement error covariance, measurement cross-covariance moment and Kalman gain matrix are calculated and expressed as:

[0070]

[0071] in, represents the error covariance matrix, represents the cross-covariance matrix prediction value, K k+1 represents the Kalman gain matrix; R represents the measurement noise,

[0072] Step 53: In the estimation step, the state vector and the error covariance matrix are first estimated based on the Kalman gain matrix and the predicted value of the target state vector, which is expressed as:

[0073]

[0074] in, represents the state vector at the current moment, P k+1 Represents the error covariance matrix at the current moment;

[0075] Step 54: Repeat steps 51 to 54 according to the state vector and error covariance matrix at the current moment until the motion trajectory prediction of the landing platform is finally completed.

[0076] Step 6: Based on the estimated speed of the drone and the motion trajectory of the landing platform, control the drone to fly toward the center of the landing platform.

[0077] After obtaining the estimated speed of the moving target, the speed control command of the drone is output. The speed of the drone consists of two parts: the estimated speed of the dynamic target and the ratio of the target's offset relative to the center of the camera. By increasing the speed of the target to keep the drone and the landing platform relatively still, the problem is simplified to a static landing, and then the offset of the target is used to control the drone to gradually approach the target, expressed as: v U =v p +μ*d; where subscript U represents the drone, subscript p represents the landing platform, and μ is in units of seconds. -1 d is the scale factor, and d is the horizontal distance between the landing platform and the UAV in the body coordinate system.

[0078] Step 7: Fuse the multi-sensor information to obtain the height information of the drone from the landing platform. If the landing height is reached, execute step 8; otherwise, descend 0.2 meters and return to step 2.

[0079] During the landing process, in order to ensure timely stopping of the motor and starting of the electromagnet, the altitude information is particularly important. This application proposes an adaptive weighted fusion algorithm that considers the characteristics of the sensor itself and the actual measurement data, and realizes the fusion of information from multiple sensors such as cameras, laser rangefinders and barometers to detect altitude.

[0080] The weight consists of two parts: fixed weight and measurement weight. The fixed weight is determined by the characteristics of the sensor itself. The higher the sensor accuracy, the greater the fixed weight ratio. At the same time, it also meets the following requirements:

[0081]

[0082] Among them, the subscript q = 1, 2, 3 corresponds to three types of sensors, Represents the sensor fixed weight.

[0083] The measurement weight is calculated from the variance of the actual measurement data and is expressed as:

[0084]

[0085] Among them, the subscript q = 1, 2, 3 corresponds to three types of sensors, represents the variance of each sensor data, represents the kth last data of the qth sensor, Represents the sensor measurement weight.

[0086] Finally, the fixed weights and the measured weights are combined to obtain the final weight of each sensor, which is expressed as:

[0087]

[0088] Among them, ω q represents the final weight of the sensor, α and β are the coefficients corresponding to the fixed weight and the measurement weight respectively.

[0089] Step 8: The drone's rotor stops, and the electromagnet is turned on to attract the landing platform to complete the landing.

[0090] After falling below the landing height, the drone's rotor motor stops and the electromagnet is turned on to attract the landing platform to prevent the drone from falling from the landing platform and causing damage. At this point, the drone's autonomous landing process ends.

[0091] In order to verify the superiority of the method of this application, the method is compared with the difference-only method (without using K-nearest neighbors) in the velocity estimation experiment, and the estimation results are recorded; in the trajectory prediction experiment, it is compared with the standard CKF method, and the estimation errors of the two are recorded; in the landing experiment, the deviation between the drone and the center of the platform is recorded.

[0092] Speed ​​estimation effect comparison Figure 3 As shown in the figure, it can be seen that the local weighted KNN anomaly detection eliminates the abnormal speed data that increases or decreases sharply, the estimated curve is smoother, and the tracking effect is improved; the trajectory prediction error is as follows Figure 4 As shown in the figure, it can be seen that during the landing process, the trajectory predicted by AMCKF is closer to the actual trajectory, and the prediction error is smaller than that of the standard CKF in most of the time. According to statistics, the average error of AMCKF is 0.0109, the root mean square error is 0.01215, the average error of CKF is 0.01515, and the root mean square error is 0.01505. The method of the present invention has an improvement of 28% in average error and 17% in root mean square error compared with the standard CKF algorithm, and has higher accuracy. The landing accuracy is shown in Figure 5 As shown, it can be seen that the deviation of the drone in the x and y directions in each experiment is less than 12 cm. After statistics, the landing deviations in the x and y directions are 7.85 cm and 7.05 cm respectively. In summary, the method of the present invention has improvements in speed estimation, trajectory prediction and height estimation, which ultimately helps to improve landing accuracy.

Claims

1. A vision-based dynamic platform UAV autonomous tracking and landing method, characterized in that: include: Step 1: Obtain the GPS information of the landing platform on the unmanned vehicle and perform preliminary positioning. The drone flies to the vicinity of the landing platform based on the preliminary positioning and then starts the visual guidance program; Step 2: The drone detects whether the landing platform is in its field of view. If so, it goes to step 3, otherwise it returns to step 1. Step 3: Detect the logo of the landing platform, perform image processing on the logo to obtain the coordinates of the landing platform in the pixel coordinate system, and convert the coordinates in the pixel coordinate system into the coordinates in the body coordinate system; Step 4: Estimate the speed of the landing platform relative to the drone based on the drone body coordinates and time interval at the previous and next two moments, and use the local weighted K-nearest neighbor algorithm to eliminate abnormal speed data to obtain the estimated speed of the landing platform; Step 5: According to the coordinates of the landing platform's logo in the body coordinate system, the motion trajectory of the landing platform is predicted using the decaying memory volumetric Kalman filter algorithm; Step 6: According to the estimated speed and motion trajectory of the landing platform, control the drone to fly toward the center of the landing platform; Step 7: Fuse the multi-sensor information to obtain the height information of the UAV from the landing platform. If the landing height is reached, execute step 8; otherwise, descend 0.2 meters and return to step 2; Step 8: The drone's rotor stops, and the electromagnet is turned on to attract the landing platform to complete the landing.

2. The method according to claim 1, characterized in that In the step 3, the logo of the landing platform is detected by a camera connected to the drone, and the logo is grayed, thresholded, and contour extracted to obtain the coordinates (u0, v0) of the center of the logo in the pixel coordinate system; wherein the x-axis direction of the camera is the same as the y-axis direction of the drone; The coordinates in the pixel coordinate system are converted to the coordinates in the body coordinate system, expressed as: Among them, d x and d y Represents the length of the unit pixel in the x and y directions; (u, v) represents the coordinates of the logo center in the image coordinate system; f represents the focal length of the camera; subscript C represents the coordinates in the camera coordinate system; subscript B represents the coordinates in the body coordinate system.

3. The method according to claim 2, characterized in that The step 4 comprises: Step 41: Obtain the coordinates of the drone body in the two frames of images at two moments in time, and perform differential calculation on the coordinates of the drone body to obtain the speed of the landing platform relative to the drone, which is expressed as: in, It represents the speed of the landing platform relative to the drone on the x-axis in the body coordinate system at time t; It represents the speed of the landing platform relative to the drone on the y-axis in the body coordinate system at time t; Represents the x-axis coordinate of the drone body in the body coordinate system at time t; Represents the x-axis coordinate of the drone body in the body coordinate system at time t-1; Represents the UAV body coordinate of the y-axis in the body coordinate system at time t; represents the UAV body coordinate of the y-axis in the body coordinate system at time t-1; Δt represents the time interval between time t and time t-1; Step 42: using the local weighted K-nearest neighbor algorithm to remove abnormal speed data, including: comparing the weighted average distance with the dynamic threshold, if the weighted average distance is greater than the dynamic threshold, it means that the estimated speed is an abnormal speed, the abnormal speed is removed, and then the estimated speed at the previous moment is used to replace the abnormal speed, which is specifically expressed as: in, Represents the weighted average distance between the current sample data and the data points at the previous K moments; Represents the weighted average of the data points at the previous K moments; ω t-i represents the weight at time ti; λ represents the decay rate; T dyn represents the current threshold, that is, the dynamic threshold; α represents the dynamic coefficient.

4. The method according to claim 3, characterized in that The step 5 comprises: Step 51: In the prediction step, the volume points are first calculated, which is expressed as: Among them, P k represents the covariance matrix; represents the state vector; represents the volume point; n represents the dimension of the state vector; represents the volume point set, [I] = [I n ,-I n ]; Then the state of the volume point is predicted, which is expressed as: in, represents the volume point through which the output propagates through the state equation; The target state vector and error covariance matrix are predicted according to the volume point state, which can be expressed as: in, represents the predicted value of the target state vector; P k+1|k represents the predicted value of the error covariance matrix; Q represents the process noise, b represents the attenuation factor; Step 52: In the update step, the volume point is calculated according to the target state vector of the prediction step and the predicted value of the error covariance matrix, which is expressed as: The volume point measurement is predicted based on the volume point, expressed as: in, represents the volume point where the output propagates through the measurement equation; The target measurement is predicted based on the volume point measurement, expressed as: in, represents the predicted value of the next measurement step; The measurement error covariance, measurement cross-covariance matrix and Kalman gain matrix are calculated and expressed as: in, represents the error covariance matrix, represents the cross-covariance matrix prediction value, K k+1 represents the Kalman gain matrix; R represents the measurement noise, Step 53: In the estimation step, the state vector and the error covariance matrix are first estimated based on the Kalman gain matrix and the predicted value of the target state vector, which is expressed as: in, represents the state vector at the current moment, P k+1 Represents the error covariance matrix at the current moment; Step 54: Repeat steps 51 to 54 according to the state vector and error covariance matrix at the current moment until the motion trajectory prediction of the landing platform is finally completed.

5. The method according to claim 4, characterized in that In step 6, the offset of the landing platform is used to control the drone to gradually approach the landing platform, which is expressed as: U =v p +μ*d; where subscript U represents the UAV, subscript p represents the landing platform, and μ represents the unit of s -1 The scale factor of d represents the horizontal distance between the landing platform and the UAV in the body coordinate system.

6. The method according to claim 5, characterized in that In step 7, the fixed weights of the multiple sensors satisfy: Among them, the subscript q = 1, 2, 3 corresponds to three types of sensors, represents the fixed weight of the sensor; The measurement weight of multiple sensors is expressed as: Among them, the subscript q = 1, 2, 3 corresponds to three types of sensors, Indicates the kth last data of the qth sensor; represents the variance of each sensor data, Represents the measurement weight of the sensor; The final weight of each sensor is expressed as: Among them, ω q represents the final weight of the sensor, α and β are the coefficients corresponding to the fixed weight and the measurement weight respectively.

Images