Unmanned aerial vehicle accurate landing control method and device based on visual guidance

Abstract

The invention discloses an unmanned aerial vehicle precise landing control method and device based on visual guidance. The method comprises the following steps: setting a landing identifier; controlling the unmanned aerial vehicle to detect the position and judging whether the unmanned aerial vehicle arrives at a landing area according to the position; after reaching the landing area, controlling the unmanned aerial vehicle to descend to a preset height, collecting an environment image by the unmanned aerial vehicle, identifying a landing identifier according to the environment image and verifying, and determining a target landing identifier; calculating an included angle between the unmanned aerial vehicle and the target landing identification direction according to the environment image; calculating a horizontal distance between the unmanned aerial vehicle and the target landing identifier according to the target landing identifier; and fusing the included angle with the yaw angle of the unmanned aerial vehicle by using a non-destructive Kalman filter, fusing the horizontal distance with the horizontal speed of the unmanned aerial vehicle, and controlling the flight state of the unmanned aerial vehicle, so that the unmanned aerial vehicle flies to the position right above the target landing identifier and lands to the position where the target landing identifier is located. The method can improve the landing accuracy of the unmanned aerial vehicle.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicles (UAVs), and in particular to a method and device for controlling the precise landing of an UAV based on vision guidance. Background Art

[0002] With the rapid development of science and technology, drones are widely used in both military and civilian fields. To achieve automated drone operations, precise landing has received widespread attention. To achieve precise takeoff and landing at a fixed location, high landing accuracy is required. Currently, the most common landing method is guided landing based on satellite positioning. However, the positioning accuracy of standard single-point satellite positioning mode is typically in the meter range, which is difficult to meet the requirements for precise landing. RTK mode is also affected by factors such as the environment and base station distribution. In some cases, it is difficult to achieve a fixed solution, which cannot meet the requirements for stable and precise landing. To address the problem of insufficient satellite positioning accuracy, vision guidance is also a commonly used assisted landing method. For example, patent CN114200948B discloses a vision-assisted autonomous landing method for drones, which includes the following steps: Step S1: Design a landing marker; Select two different types of QR codes and nest them to form the landing marker; Step S2: Image capture; The drone first captures an image of the landing marker using a fixed downward-looking camera; Step S3: Obtain the coordinates of the marker's center; Utilize a detection algorithm to obtain the coordinates of the landing marker's center on the image; Step S4: Attitude compensation; Attitude compensation is used to eliminate attitude changes during the drone's translation; Step S5: Target prediction; Considering the real-time changes in the landing marker's posture, a Kalman filter is introduced to predict the target's next position on the image; Step S6: Landing control; The predicted coordinates are directly used as controller input to achieve drone landing. This method requires multiple rotation matrix calculations, and based on the extended Kalman filter, the Jacobian matrix needs to be calculated, making the method complex and computationally intensive. This method is controlled based on the image plane, requiring specific control rates for cameras with different focal lengths and resolutions, resulting in poor adaptability. Errors in image calculations can significantly affect landing accuracy. Summary of the Invention

[0003] The present invention provides a method and device for precise landing control of an unmanned aerial vehicle (UAV) based on vision guidance, which reduces the amount of calculation while ensuring landing accuracy.

[0004] A method for precise landing control of a UAV based on vision guidance, comprising:

[0005] Set up landing signs;

[0006] Control the drone to detect its own position and determine whether it has reached the landing area based on its own position;

[0007] After arriving at the landing area, the drone is controlled to descend to a preset height, the drone collects environmental images, identifies the landing mark based on the environmental images and verifies it to determine the target landing mark;

[0008] Calculating the angle between the drone and the target landing mark direction based on the environmental image;

[0009] Calculate the horizontal distance between the UAV and the target landing mark according to the target landing mark;

[0010] Using a lossless Kalman filter, the angle is integrated with the yaw angle of the drone, the horizontal distance is integrated with the horizontal speed of the drone, and the flight state of the drone is controlled so that the drone flies directly above the target landing mark;

[0011] Control the drone to land vertically to the target landing mark.

[0012] Furthermore, the landing mark includes a high-altitude guidance mark and a near-ground guidance mark; the size of the near-ground guidance mark is smaller than the size of the high-altitude guidance mark.

[0013] Furthermore, the landing mark is a QR code;

[0014] Identifying and verifying the landing mark according to the environmental image to determine the target landing mark includes:

[0015] Performing grayscale processing and binarization processing on the environment image to extract edge contours;

[0016] Performing polygonal approximation on the edge contour to screen a quadrilateral area;

[0017] Convert the quadrilateral area into a square to obtain the landing mark;

[0018] The code in the landing mark is identified, and the target landing mark is determined according to the identification result.

[0019] Furthermore, the high-altitude guidance mark and the near-ground guidance mark have different codes; and determining the target landing mark according to the recognition result includes:

[0020] Comparing the code in the landing mark with the pre-stored preset high-altitude guidance code and the pre-stored preset ground guidance code, determining the landing mark corresponding to the code consistent with the preset high-altitude guidance code as the target high-altitude guidance mark, and determining the landing mark corresponding to the code consistent with the preset ground guidance code as the target ground guidance mark;

[0021] The target high-altitude guidance mark or the target near-ground guidance mark is selected as the target landing mark.

[0022] Furthermore, calculating the angle between the drone and the target landing mark direction according to the environmental image includes:

[0023] Establishing an image coordinate system according to the arrangement direction of pixels of the environment image;

[0024] The vertical axis in the image coordinate system is used as the direction of the drone, and the side of the target landing mark closest to the vertical axis in the image coordinate system is used as the target landing mark direction. The angle between the drone and the target landing mark direction is calculated based on the pixel coordinates of the two end points of the side of the target landing mark closest to the vertical axis in the image coordinate system.

[0025] Furthermore, the angle between the UAV and the target landing mark is the arc tangent of the slope of the side of the target landing mark closest to the vertical axis in the image coordinate system.

[0026] Furthermore, calculating the horizontal distance between the UAV and the target landing mark according to the target landing mark includes:

[0027] Establishing an image coordinate system according to the arrangement direction of pixels of the environment image;

[0028] Calculating the side length of the target landing mark in the environment image;

[0029] Calculating a proportional coefficient according to the side length of the target landing mark in the environment image and the actual physical side length;

[0030] The horizontal distance between the UAV and the target landing mark is calculated based on the central pixel coordinates of the target landing mark, the central pixel coordinates of the environment image, and the scale coefficient.

[0031] Furthermore, the horizontal distance between the UAV and the target landing mark includes the horizontal distance in the x direction and the horizontal distance in the y direction;

[0032] The horizontal distance in the x-direction is the ratio of the difference between the horizontal coordinate of the center pixel of the target landing mark and the horizontal coordinate of the center pixel of the environment image to the proportional coefficient;

[0033] The horizontal distance in the y direction is the ratio of the difference between the vertical coordinate of the center pixel of the target landing mark and the vertical coordinate of the center pixel of the environment image to the proportional coefficient.

[0034] Furthermore, a lossless Kalman filter is used to fuse the angle with the yaw angle of the UAV, and to fuse the horizontal distance with the horizontal speed of the UAV, and to control the flight state of the UAV, including:

[0035] Initializing the covariance matrix, the process noise matrix, the measurement noise matrix, and the state variables, wherein the initialized state variables include the angle, the horizontal distance between the UAV and the target landing mark in the x direction, and the horizontal distance between the UAV and the target landing mark in the y direction;

[0036] Generate a sigma point according to the state quantity;

[0037] Perform state prediction and covariance matrix prediction based on the horizontal speed, yaw angle and sigma point output by the drone itself to obtain the predicted state quantity and predicted covariance matrix;

[0038] The distance between the drone and the ground is detected by ground detection equipment. The measurement variables are established based on the horizontal distance in the x-direction and y-direction between the drone nose and the target landing mark, the distance between the drone and the ground, and the rotation matrix from the camera coordinate system to the world coordinate system.

[0039] Substitute the sigma point into the observation model for calculation, and determine whether to update the measurement variable based on the calculation result;

[0040] After determining to update the measured variable, updating the Kalman gain, updating the covariance matrix according to the predicted covariance matrix and the updated Kalman gain, and calculating the state estimate according to the predicted state quantity and the updated Kalman gain;

[0041] The flight state of the UAV is controlled according to the state estimation value.

[0042] Furthermore, the sigma point is substituted into the observation model for calculation, and whether to update the measurement variable is determined according to the calculation result, including:

[0043] Substitute the sigma point into the observation model for propagation, and calculate and obtain the predicted measurement sigma point;

[0044] Calculating a predicted measurement mean based on the predicted measurement sigma point;

[0045] Calculating a measurement covariance matrix and a state-measurement cross covariance matrix based on the predicted measurement mean;

[0046] Calculating measurement residuals based on the current measurement variables and the predicted measurement mean;

[0047] Calculating a residual weighted norm based on the measurement residual and the measurement covariance matrix;

[0048] The residual weighted norm is compared with a preset fault detection value. If the residual weighted norm is smaller than the preset fault detection value, it is determined to update the measurement variable; otherwise, the measurement variable update in the current calculation cycle is abandoned.

[0049] A vision-guided UAV precision landing control device applied to the above method comprises:

[0050] The position detection module is used to control the drone to detect its own position and determine whether it has reached the landing area based on its own position;

[0051] A target determination module is used to control the drone to descend to a preset height after reaching the landing area, wherein the drone collects environmental images, identifies and verifies the landing mark based on the environmental images, and determines the target landing mark;

[0052] An angle calculation module, used to calculate the angle between the drone and the target landing mark direction based on the environmental image;

[0053] A distance calculation module is used to calculate the horizontal distance between the UAV and the target landing mark according to the target landing mark;

[0054] a data fusion module, configured to fuse the angle with the yaw angle of the UAV and fuse the horizontal distance with the horizontal speed of the UAV using a lossless Kalman filter;

[0055] The landing control module is used to control the flight state of the UAV so that the UAV flies to the top of the target landing mark and controls the UAV to land vertically to the location of the target landing mark.

[0056] Furthermore, the landing mark includes a high-altitude guidance mark and a near-ground guidance mark; the size of the near-ground guidance mark is smaller than the size of the high-altitude guidance mark.

[0057] Furthermore, the landing mark is a QR code;

[0058] The target determination module identifies and verifies the landing mark according to the environment image to determine the target landing mark, including:

[0059] Performing grayscale processing and binarization processing on the environment image to extract edge contours;

[0060] Performing polygonal approximation on the edge contour to screen a quadrilateral area;

[0061] Convert the quadrilateral area into a square to obtain the landing mark;

[0062] The code in the landing mark is identified, and the target landing mark is determined according to the identification result.

[0063] Furthermore, the high-altitude guidance mark and the near-ground guidance mark have different codes; the target determination module determines the target landing mark according to the recognition result, including:

[0064] Comparing the code in the landing mark with the pre-stored preset high-altitude guidance code and the pre-stored preset ground guidance code, determining the landing mark corresponding to the code consistent with the preset high-altitude guidance code as the target high-altitude guidance mark, and determining the landing mark corresponding to the code consistent with the preset ground guidance code as the target ground guidance mark;

[0065] The target high-altitude guidance mark or the target near-ground guidance mark is selected as the target landing mark.

[0066] Furthermore, the angle calculation module calculates the angle between the drone and the target landing mark direction according to the environmental image, including:

[0067] Establishing an image coordinate system according to the arrangement direction of pixels of the environment image;

[0068] The vertical axis in the image coordinate system is used as the direction of the drone, and the side of the target landing mark closest to the vertical axis in the image coordinate system is used as the target landing mark direction. The angle between the drone and the target landing mark direction is calculated based on the pixel coordinates of the two end points of the side of the target landing mark closest to the vertical axis in the image coordinate system.

[0069] Furthermore, the angle between the UAV and the target landing mark is the arc tangent of the slope of the side of the target landing mark closest to the vertical axis in the image coordinate system.

[0070] Furthermore, the distance calculation module calculates the horizontal distance between the UAV and the target landing mark according to the target landing mark, including:

[0071] Establishing an image coordinate system according to the arrangement direction of pixels of the environment image;

[0072] Calculating the side length of the target landing mark in the environment image;

[0073] Calculating a proportional coefficient according to the side length of the target landing mark in the environment image and the actual physical side length;

[0074] The horizontal distance between the UAV and the target landing mark is calculated based on the central pixel coordinates of the target landing mark, the central pixel coordinates of the environment image, and the scale coefficient.

[0075] Furthermore, the horizontal distance between the UAV and the target landing mark includes the horizontal distance in the x direction and the horizontal distance in the y direction;

[0076] The horizontal distance in the x-direction is the ratio of the difference between the horizontal coordinate of the center pixel of the target landing mark and the horizontal coordinate of the center pixel of the environment image to the proportional coefficient;

[0077] The horizontal distance in the y direction is the ratio of the difference between the vertical coordinate of the center pixel of the target landing mark and the vertical coordinate of the center pixel of the environment image to the proportional coefficient.

[0078] Furthermore, the data fusion module utilizes a lossless Kalman filter to fuse the angle with the yaw angle of the UAV, and to fuse the horizontal distance with the horizontal speed of the UAV, including:

[0079] Initializing the covariance matrix, the process noise matrix, the measurement noise matrix, and the state variables, wherein the initialized state variables include the angle, the horizontal distance between the UAV and the target landing mark in the x direction, and the horizontal distance between the UAV and the target landing mark in the y direction;

[0080] Generate a sigma point according to the state quantity;

[0081] Perform state prediction and covariance matrix prediction based on the horizontal speed, yaw angle and sigma point output by the drone itself to obtain the predicted state quantity and predicted covariance matrix;

[0082] The distance between the drone and the ground is detected by ground detection equipment. The measurement variables are established based on the horizontal distance in the x-direction and y-direction between the drone nose and the target landing mark, the distance between the drone and the ground, and the rotation matrix from the camera coordinate system to the world coordinate system.

[0083] Substitute the sigma point into the observation model for calculation, and determine whether to update the measurement variable based on the calculation result;

[0084] After determining to update the measured variable, updating the Kalman gain, updating the covariance matrix according to the predicted covariance matrix and the updated Kalman gain, and calculating the state estimate according to the predicted state quantity and the updated Kalman gain;

[0085] The landing control module is used to control the flight state of the UAV according to the state estimation value.

[0086] Furthermore, the data fusion module substitutes the sigma point into the observation model for calculation, and determines whether to update the measurement variable according to the calculation result, including:

[0087] Substitute the sigma point into the observation model for propagation, and calculate and obtain the predicted measurement sigma point;

[0088] Calculating a predicted measurement mean based on the predicted measurement sigma point;

[0089] Calculating a measurement covariance matrix and a state-measurement cross covariance matrix based on the predicted measurement mean;

[0090] Calculating measurement residuals based on the current measurement variables and the predicted measurement mean;

[0091] Calculating a residual weighted norm based on the measurement residual and the measurement covariance matrix;

[0092] The residual weighted norm is compared with a preset fault detection value. If the residual weighted norm is smaller than the preset fault detection value, it is determined to update the measurement variable; otherwise, the measurement variable update in the current calculation cycle is abandoned.

[0093] The method and device for controlling the precise landing of a UAV based on vision guidance provided by the present invention have at least the following beneficial effects:

[0094] (1) In the image-based angle and distance calculation process, there is no need to calculate the Jacobian matrix or perform intrinsic calibration of the camera, which effectively reduces the computational complexity of the entire algorithm, avoids the error caused by camera calibration, and improves the accuracy of landing control;

[0095] (2) Flight control is performed based on the position deviation output from the body coordinate system, eliminating the need to design different control rates for different drones due to different cameras, resulting in better adaptability;

[0096] (3) Flight control is performed based on lossless Kalman filtering. The data obtained by image calculation is used as the observation value, and abnormality judgment is performed through the residual. This can effectively isolate the abnormality of the observation value in the Kalman filter and enhance the robustness of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0097] Figure 1 This is a flowchart of an embodiment of the vision-guided UAV precise landing control method provided by the present invention.

[0098] Figure 2 This is a schematic diagram of an embodiment of a landing mark in the vision-guided UAV precision landing control method provided by the present invention.

[0099] Figure 3 This is a flowchart of an embodiment of determining the target landing mark in the vision-guided UAV precision landing control method provided by the present invention.

[0100] Figure 4 This is a flowchart of an embodiment of calculating the angle between the drone nose and the target landing mark direction in the vision-guided drone precision landing control method provided by the present invention.

[0101] Figure 5 This is a schematic diagram of an embodiment of the image coordinate system in the vision-guided UAV precision landing control method provided by the present invention.

[0102] Figure 6This is a flowchart of an embodiment of calculating horizontal distance in the vision-guided UAV precision landing control method provided by the present invention.

[0103] Figure 7 This is a flowchart of an embodiment of controlling the flight state of a drone in the vision-guided drone precision landing control method provided by the present invention.

[0104] Figure 8 This is a flowchart of an embodiment of determining the update of measurement variables in the vision-guided UAV precision landing control method provided by the present invention.

[0105] Figure 9 This is a flow chart of an embodiment of the vision-guided UAV precision landing control device provided by the present invention. DETAILED DESCRIPTION

[0106] In order to better understand the above technical solution, the above technical solution will be described in detail below with reference to the accompanying drawings and specific implementation methods.

[0107] refer to Figure 1 In some embodiments, a method for precise landing control of a UAV based on vision guidance is provided, comprising:

[0108] S1. Set landing mark;

[0109] S2: Control the drone to detect its own position and determine whether it has reached the landing area based on its own position;

[0110] S3. After arriving at the landing area, the drone is controlled to descend to a preset height, the drone collects an environmental image, identifies and verifies the landing mark based on the environmental image, and determines the target landing mark;

[0111] S4. Calculating the angle between the UAV and the target landing mark direction based on the environmental image;

[0112] S5. Calculate the horizontal distance between the UAV and the target landing mark according to the target landing mark;

[0113] S6. Using a lossless Kalman filter, fuse the included angle with the yaw angle of the drone, fuse the horizontal distance with the horizontal speed of the drone, and control the flight state of the drone so that the drone flies directly above the target landing mark.

[0114] S7. Control the UAV to land vertically to the location of the target landing mark.

[0115] Further, in step S1, refer to Figure 2In some embodiments, the landing mark includes a high-altitude guidance mark A and a near-ground guidance mark B; the size of the near-ground guidance mark B is smaller than that of the high-altitude guidance mark A.

[0116] Among them, the high-altitude guidance mark A is used for landing guidance when the UAV is far away from the landing site, and the low-ground guidance mark B is used for landing guidance when the UAV is close to the landing site.

[0117] Specifically, multiple drones can be parked in the drone landing area. Each drone has a corresponding docking position pre-planned, and a landing mark is set at the corresponding docking position. Each landing mark has a different code and corresponds to a different drone. The drone identifies and determines the target landing mark corresponding to itself through the code of the landing mark.

[0118] Furthermore, in step S2, the drone detects its own position, compares its own position coordinates with the position coordinates of the preset landing area, and determines whether the drone has reached the landing area based on the comparison result.

[0119] In some embodiments, the landing mark is a QR code.

[0120] refer to Figure 3 In step S3, after the UAV reaches the landing area, the UAV is controlled to descend to a preset height, and an environmental image is collected. The landing mark is identified and verified based on the environmental image to determine the target landing mark, including:

[0121] S31, performing grayscale processing and binarization processing on the environment image to extract edge contours;

[0122] S32, performing polygonal approximation on the edge contour to screen quadrilateral areas;

[0123] S33, converting the quadrilateral area into a square to obtain the landing mark;

[0124] S34. Identify the code in the landing mark, and determine the target landing mark according to the identification result.

[0125] Furthermore, in step S34, the codes of the high-altitude guidance mark and the near-ground guidance mark are different; determining the target landing mark according to the recognition result includes:

[0126] Comparing the code in the landing mark with the pre-stored preset high-altitude guidance code and the pre-stored preset ground guidance code, determining the landing mark corresponding to the code consistent with the preset high-altitude guidance code as the target high-altitude guidance mark, and determining the landing mark corresponding to the code consistent with the preset ground guidance code as the target ground guidance mark;

[0127] The target high-altitude guidance mark or the target near-ground guidance mark is selected as the target landing mark.

[0128] Specifically, the landing area may have multiple landing marks for guiding multiple drones to land precisely at the preset landing location. Therefore, a drone may recognize multiple landing marks in the environmental image. Therefore, it is necessary to further determine whether it is its own landing mark based on the code of the landing mark, compare the code in the landing mark with the pre-stored preset high-altitude guidance code and preset near-ground guidance code, and determine the landing mark corresponding to the code consistent with the preset high-altitude guidance code as the target high-altitude guidance mark, and determine the landing mark corresponding to the code consistent with the preset near-ground guidance code as the target near-ground guidance mark. If only the complete target high-altitude guidance mark is recognized, the target high-altitude guidance mark is used as the target landing mark. If only the complete target near-ground guidance mark is recognized, the target near-ground guidance mark is used as the target landing mark. If both the complete target high-altitude guidance mark and the target near-ground guidance mark are recognized, the target near-ground guidance mark is used as the target landing mark.

[0129] Further, refer to Figure 4 In step S4, the angle between the UAV and the target landing mark direction is calculated based on the environmental image, including:

[0130] S41, establishing an image coordinate system according to the arrangement direction of pixels of the environment image;

[0131] S42. Use the longitudinal axis in the image coordinate system as the direction of the UAV, and the side of the target landing mark closest to the longitudinal axis in the image coordinate system as the target landing mark direction. Calculate the angle between the UAV and the target landing mark direction based on the pixel coordinates of the two endpoints of the side of the target landing mark closest to the longitudinal axis in the image coordinate system.

[0132] Specifically, in step S41 , an image coordinate system is established with one corner of the environment image as the origin, preferably the upper left corner, the horizontal pixel direction in the environment image as the horizontal axis, and the vertical pixel direction as the vertical axis.

[0133] In step S42, according to the setting of the drone camera, its direction is the vertical axis in the image coordinate system, and the side of the target landing mark closest to the vertical axis in the image coordinate system is used as the target landing mark direction. For example, Figure 5 , ABCD is the target landing mark, among which AD is the side closest to the vertical axis in the image coordinate system. Therefore, the AD side is used as the target landing mark direction. The angle between the drone head and the target landing mark direction is the angle between the AD side and the vertical axis. Therefore, the slope of the AD side is the angle between the drone head and the target landing mark direction.

[0134] Specifically, the angle between the UAV and the target landing mark is the arc tangent of the slope of the side of the target landing mark closest to the vertical axis in the image coordinate system.

[0135] For example, Figure 5 In the example, the inverse tangent value of the slope of the AD edge can be calculated based on the pixel coordinates of the two ends of the AD edge, that is, the pixel coordinates of point A and point D. The calculation formula is as follows:

[0136] α=arctan(-(u d -u a ) / (v d -v a )); (1)

[0137] Among them, α is the angle between AD side and the vertical axis, u d ,v d are the pixel horizontal and vertical coordinates of point D, u a 、v a are the pixel horizontal and vertical coordinates of point A respectively.

[0138] Further, refer to Figure 6 In step S5, the horizontal distance between the UAV and the target landing mark is calculated according to the target landing mark, including:

[0139] S51, establishing an image coordinate system according to the arrangement direction of pixels of the environment image;

[0140] S52, calculating the side length of the target landing mark in the environment image;

[0141] S53, calculating a proportionality coefficient according to the side length of the target landing mark in the environment image and the actual physical side length;

[0142] S54: Calculate the horizontal distance between the UAV and the target landing mark according to the central pixel coordinates of the target landing mark, the central pixel coordinates of the environment image, and the scale factor.

[0143] Specifically, in step S51 , an image coordinate system is established with a corner of the environment image as the origin, preferably the upper left corner, the horizontal pixel direction in the environment image as the horizontal axis, and the vertical pixel direction as the vertical axis.

[0144] In step S53, the proportional coefficient is the ratio of the side length of the target landing mark in the environment image to the actual physical side length, specifically:

[0145] k=a / A; (2)

[0146] Wherein, k represents the proportional coefficient, a represents the side length of the target landing mark in the environment image, and A represents the actual physical side length of the target landing mark.

[0147] Furthermore, in step S54, the horizontal distance between the UAV and the target landing mark includes the horizontal distance in the x direction and the horizontal distance in the y direction;

[0148] The horizontal distance in the x-direction is the ratio of the difference between the horizontal coordinate of the center pixel of the target landing mark and the horizontal coordinate of the center pixel of the environment image to the proportional coefficient;

[0149] The horizontal distance in the y direction is the ratio of the difference between the vertical coordinate of the center pixel of the target landing mark and the vertical coordinate of the center pixel of the environment image to the proportional coefficient.

[0150] Specifically, the horizontal distance in the x-direction and the horizontal distance in the y-direction between the drone and the target landing mark are calculated using the following formula:

[0151]

[0152] Among them, x c Indicates the horizontal distance between the drone and the target landing mark in the x direction, y c represents the horizontal distance between the UAV and the target landing mark in the y direction, (x, y) represents the horizontal and vertical coordinates of the center pixel of the target landing mark, (x0, y0) represents the horizontal and vertical coordinates of the center pixel of the environment image, and k represents the scale factor.

[0153] Further, refer to Figure 7 In step S6, a lossless Kalman filter is used to fuse the angle with the yaw angle of the UAV, and the horizontal distance with the horizontal speed of the UAV, and the flight state of the UAV is controlled, including:

[0154] S61, initializing the covariance matrix, the process noise matrix, the measurement noise matrix, and the state quantity, where the initialized state quantity includes the angle, the horizontal distance between the UAV and the target landing mark in the x direction, and the horizontal distance between the UAV and the target landing mark in the y direction;

[0155] S62, generating a sigma point according to the state quantity;

[0156] S63, performing state prediction and covariance matrix prediction based on the horizontal speed, yaw angle and sigma point output by the drone itself to obtain a predicted state quantity and a predicted covariance matrix;

[0157] S64, detecting the distance between the UAV and the ground using a ground detection device, and establishing measurement variables based on the horizontal distance in the x-direction and the horizontal distance in the y-direction between the UAV nose and the target landing mark, the distance between the UAV and the ground, and the rotation matrix from the camera coordinate system to the world coordinate system;

[0158] S65, substituting the sigma point into the observation model for calculation, and determining whether to update the measurement variable according to the calculation result;

[0159] S66: After determining to update the measured variable, update the Kalman gain, update the covariance matrix according to the predicted covariance matrix and the updated Kalman gain, and calculate the state estimate according to the predicted state quantity and the updated Kalman gain;

[0160] S67: Control the flight state of the UAV according to the state estimation value.

[0161] Specifically, in step S61, the covariance matrix P, the process noise matrix Q, the measurement noise matrix R and the state quantity x are initialized, where the state quantity x = [θpnpe], θ represents the yaw angle of the UAV, pn represents the distance from the UAV to the target landing mark in the north direction, pe represents the distance from the UAV to the target landing mark in the east direction, and the initialized state quantities include the angle between the UAV and the target landing mark, the horizontal distance between the UAV and the target landing mark in the x direction, and the horizontal distance between the UAV and the target landing mark in the x direction.

[0162] In step S62 , a sigma point is generated according to the state quantity. In this embodiment, the dimension n of the state quantity x is 3, and 2n+1 sigma points, ie, seven sigma points, are generated.

[0163] Specifically, at each time step k, the current state quantity is estimated k-1 , and the covariance matrix P k-1 , generate 7 sigma points as follows:

[0164] X k-1 =[x k -1,x k-1 -γL :,1 ,x k-1 +γL :,1 ,x k-1 -γL :,2 ,x k-1 -γL :,2 ,x k-1 -γL :,3 ,x k-1 -γL :,3 ]; (4)

[0165] in, λ is the adjustment parameter, which is adjusted according to the actual situation. :,i represents the i-th column of L, and P represents the covariance matrix.

[0166] Furthermore, in step S63, the state quantity x and the covariance matrix are predicted:

[0167] The drone can output its own horizontal speed v n ,v e , yaw angle δ. If the time interval between two moments is △t, the state quantity prediction at time k can be expressed as

[0168]

[0169] in, represents the i-th sigma point, δ represents the yaw angle of the drone, v n Indicates the speed of the drone in the north direction, v e represents the speed of the UAV in the east direction, △t represents the time interval between time k and time k-1, and x k|k-1 represents the predicted state quantity at time k, is the mean weight;

[0170] The prediction of the covariance matrix P can be expressed as:

[0171]

[0172]

[0173] Among them, P k|k-1 represents the prediction covariance matrix at time k, represents the covariance weight, Q represents the process noise matrix, and λ is the adjustment parameter.

[0174] Furthermore, in step S64, the distance between the drone and the ground is detected by the ground detection equipment. According to the horizontal distance in the x-direction and the horizontal distance in the y-direction between the drone head and the target landing mark, the distance between the drone and the ground, and the rotation matrix from the camera coordinate system to the world coordinate system, the measurement variables are established, including:

[0175] Calculate the distance between the drone and the landing point in the world coordinate system based on the horizontal distance in the x-direction and the y-direction between the drone's nose and the target landing mark, the distance between the drone and the ground, and the rotation matrix from the camera coordinate system to the world coordinate system.

[0176] The distance between the UAV and the landing point in the world coordinate system is used as the measurement variable.

[0177] The specific calculation formula is as follows:

[0178]

[0179] Among them, x w 、y w 、z w are the x-, y-, and z-direction distances from the drone to the landing point in the world coordinate system, and d is the distance from the drone to the ground. is the rotation matrix from the camera coordinate system to the world coordinate system, x c Indicates the horizontal distance between the drone nose and the target landing mark in the x direction, y c Indicates the horizontal distance in the y direction between the drone nose and the target landing mark.

[0180] Further, refer to Figure 8 In step S65, the sigma point is substituted into the observation model for calculation, and whether to update the measurement variable is determined according to the calculation result, including:

[0181] S65a, substituting the sigma point into the observation model for propagation, and calculating and obtaining the predicted measurement sigma point;

[0182] S65b, calculating and obtaining a predicted measurement mean value based on the predicted measurement sigma point;

[0183] S65c, calculating a measurement covariance matrix and a state-measurement cross covariance matrix according to the predicted measurement mean;

[0184] S65d, calculating the measurement residual according to the current measurement variable and the predicted measurement mean;

[0185] S65e, calculating a residual weighted norm according to the measurement residual and the measurement covariance matrix;

[0186] S65f, comparing the residual weighted norm with a preset fault detection value. If the residual weighted norm is smaller than the preset fault detection value, it is determined to update the measurement variables; otherwise, the measurement variable update in the current calculation cycle is abandoned.

[0187] Specifically, in step S65a, the sigma point is substituted into the observation model for propagation, as shown below:

[0188]

[0189] in, represents the i-th sigma point, Represents the predicted measurement sigma point.

[0190] Furthermore, in step S65b, the predicted measurement mean is as follows:

[0191]

[0192] Among them, z k|k-1 Represents the predicted measurement mean.

[0193] Furthermore, in step S65c, the measurement covariance matrix and the state-measurement cross covariance matrix are calculated using the following formulas:

[0194]

[0195] Among them, S k represents the measurement covariance matrix, represents the covariance weight, Indicates the predicted measurement sigma point, z k|k-1 represents the predicted measurement mean, R represents the measurement noise matrix, C k represents the state-measurement cross-covariance matrix, Indicates the i-th sigma point, x k|k-1 Represents the predicted state quantity at time k.

[0196] Furthermore, in step S65d, the measurement residual is calculated according to the following formula:

[0197] r=z k -z k|k-1 ; (14)

[0198] Among them, r is the measurement residual, z k is the current measured variable, z k|k-1 Represents the predicted measurement mean.

[0199] Furthermore, in step S65e, the residual weighted norm is calculated according to the following formula:

[0200]

[0201] Among them, E represents the residual weighted norm, r represents the measurement residual, S k represents the measurement covariance matrix.

[0202] Furthermore, in step S65f, the residual weighted norm is compared with a preset fault detection value. If the residual weighted norm is smaller than the preset fault detection value, it means that the current measured variable z k If it is reasonable, the measurement update can be performed; otherwise, the current measurement variable update in this cycle is abandoned and the next calculation cycle is directly entered.

[0203] Furthermore, in step S66, the Kalman gain is updated according to the measurement covariance matrix and the state-measurement cross covariance matrix:

[0204] Kk =C k S k -1 ; (16)

[0205] Among them, K k represents the Kalman gain, C k represents the state-measurement cross covariance matrix, S k represents the measurement covariance matrix.

[0206] The updated covariance matrix is ​​as follows:

[0207] P k =P k|k-1 -K k S k K k -1 ; (17)

[0208] Among them, P k represents the updated covariance matrix, P k|k-1 represents the prediction covariance matrix at time k, S k represents the measurement covariance matrix, K k represents the Kalman gain.

[0209] The state estimate is calculated according to the following formula:

[0210] x k =x k|k-1 +K k r; (18)

[0211] Among them, x k is the state estimate, K k represents the Kalman gain, r represents the measurement residual, x k|k-1 Represents the predicted state quantity at time k.

[0212] Furthermore, in step S67, after obtaining the state estimation value, the flight state of the UAV is controlled according to the state estimation value.

[0213] Specifically, in step S7, according to the calculated state estimation value, the drone is controlled to fly to directly above the target landing mark, and then the drone is controlled to land vertically to the location of the target landing mark.

[0214] refer to Figure 9 In some embodiments, a vision-guided UAV precision landing control device applied to the above method is provided, comprising:

[0215] Position detection module 201, used to control the drone to detect its own position and determine whether it has reached the landing area based on its own position;

[0216] The target determination module 202 is used to control the drone to descend to a preset height after reaching the landing area, the drone collects environmental images, identifies the landing mark based on the environmental images, verifies the landing mark, and determines the target landing mark;

[0217] Angle calculation module 203, used to calculate the angle between the drone and the target landing mark direction based on the environmental image;

[0218] The distance calculation module 204 is used to calculate the horizontal distance between the UAV and the target landing mark according to the target landing mark;

[0219] A data fusion module 205 is configured to fuse the angle with the yaw angle of the UAV and fuse the horizontal distance with the horizontal speed of the UAV using a lossless Kalman filter;

[0220] The landing control module 206 is used to control the flight state of the UAV so that the UAV flies to the top of the target landing mark and controls the UAV to land vertically at the location of the target landing mark.

[0221] Furthermore, the landing mark includes a high-altitude guidance mark and a near-ground guidance mark; the size of the near-ground guidance mark is smaller than the size of the high-altitude guidance mark.

[0222] Furthermore, the landing mark is a QR code;

[0223] The target determination module 202 identifies the landing mark according to the environment image and verifies it to determine the target landing mark, including:

[0224] Performing grayscale processing and binarization processing on the environment image to extract edge contours;

[0225] Performing polygonal approximation on the edge contour to screen a quadrilateral area;

[0226] Convert the quadrilateral area into a square to obtain the landing mark;

[0227] The code in the landing mark is identified, and the target landing mark is determined according to the identification result.

[0228] Furthermore, the codes of the high-altitude guidance mark and the near-ground guidance mark are different; the target determination module 202 determines the target landing mark according to the recognition result, including:

[0229] Comparing the code in the landing mark with the pre-stored preset high-altitude guidance code and the pre-stored preset ground guidance code, determining the landing mark corresponding to the code consistent with the preset high-altitude guidance code as the target high-altitude guidance mark, and determining the landing mark corresponding to the code consistent with the preset ground guidance code as the target ground guidance mark;

[0230] The target high-altitude guidance mark or the target near-ground guidance mark is selected as the target landing mark.

[0231] Furthermore, the angle calculation module 203 calculates the angle between the drone and the target landing mark direction according to the environmental image, including:

[0232] Establishing an image coordinate system according to the arrangement direction of pixels of the environment image;

[0233] The longitudinal axis in the image coordinate system is used as the direction of the drone, and the side of the target landing mark closest to the longitudinal axis in the image coordinate system is used as the target landing mark direction. The angle between the drone head and the target landing mark direction is calculated based on the pixel coordinates of the two end points of the side of the target landing mark closest to the longitudinal axis in the image coordinate system.

[0234] Furthermore, the angle between the nose of the UAV and the direction of the target landing mark is the arc tangent value of the slope of the side of the target landing mark closest to the vertical axis in the image coordinate system.

[0235] Furthermore, the distance calculation module 204 calculates the horizontal distance between the UAV and the target landing mark according to the target landing mark, including:

[0236] Establishing an image coordinate system according to the arrangement direction of pixels of the environment image;

[0237] Calculating the side length of the target landing mark in the environment image;

[0238] Calculating a proportional coefficient according to the side length of the target landing mark in the environment image and the actual physical side length;

[0239] The horizontal distance between the UAV and the target landing mark is calculated based on the central pixel coordinates of the target landing mark, the central pixel coordinates of the environment image, and the scale coefficient.

[0240] Furthermore, the horizontal distance between the UAV and the target landing mark includes the horizontal distance in the x direction and the horizontal distance in the y direction;

[0241] The horizontal distance in the x-direction is the ratio of the difference between the horizontal coordinate of the center pixel of the target landing mark and the horizontal coordinate of the center pixel of the environment image to the proportional coefficient;

[0242] The horizontal distance in the y direction is the ratio of the difference between the vertical coordinate of the center pixel of the target landing mark and the vertical coordinate of the center pixel of the environment image to the proportional coefficient.

[0243] Furthermore, the data fusion module 205 utilizes a lossless Kalman filter to fuse the angle with the yaw angle of the drone, including:

[0244] Initializing the covariance matrix, the process noise matrix, the measurement noise matrix, and the state variables, wherein the initialized state variables include the angle, the horizontal distance between the UAV and the target landing mark in the x direction, and the horizontal distance between the UAV and the target landing mark in the y direction;

[0245] Generate a sigma point according to the state quantity;

[0246] Perform state prediction and covariance matrix prediction based on the horizontal speed, yaw angle and sigma point output by the drone itself to obtain the predicted state quantity and predicted covariance matrix;

[0247] The distance between the drone and the ground is detected by ground detection equipment. The measurement variables are established based on the horizontal distance in the x-direction and y-direction between the drone nose and the target landing mark, the distance between the drone and the ground, and the rotation matrix from the camera coordinate system to the world coordinate system.

[0248] Substitute the sigma point into the observation model for calculation, and determine whether to update the measurement variable based on the calculation result;

[0249] After determining to update the measured variables, updating the Kalman gain, updating the covariance matrix according to the predicted covariance matrix and the updated Kalman gain, and calculating the state estimate according to the predicted state quantity;

[0250] The landing control module 206 is used to control the flight state of the UAV according to the state estimation value.

[0251] Furthermore, the data fusion module 205 substitutes the sigma point into the observation model for calculation, and determines whether to update the measurement variables according to the calculation result, including:

[0252] Substitute the sigma point into the observation model for propagation, and calculate and obtain the predicted measurement sigma point;

[0253] Calculating a predicted measurement mean based on the predicted measurement sigma point;

[0254] Calculating a measurement covariance matrix and a state-measurement cross covariance matrix based on the predicted measurement mean;

[0255] Calculating measurement residuals based on the current measurement variables and the predicted measurement mean;

[0256] Calculating a residual weighted norm based on the measurement residual and the measurement covariance matrix;

[0257] The residual weighted norm is compared with a preset fault detection value. If the residual weighted norm is smaller than the preset fault detection value, it is determined to update the measurement variable; otherwise, the measurement variable update in the current calculation cycle is abandoned.

[0258] The above embodiments provide a method and device for controlling the precise landing of a UAV based on vision guidance, which have at least the following beneficial effects:

[0259] (1) In the image-based angle and distance calculation process, there is no need to calculate the Jacobian matrix or perform intrinsic calibration of the camera, which effectively reduces the computational complexity of the entire algorithm, avoids the error caused by camera calibration, and improves the accuracy of landing control;

[0260] (2) Flight control is performed based on the position deviation output from the body coordinate system, eliminating the need to design different control rates for different cameras, resulting in better adaptability;

[0261] (3) Flight control is performed based on lossless Kalman filtering. The data obtained by image calculation is used as the observation value, and abnormality judgment is performed through the residual. This can effectively isolate the abnormality of the observation value in the Kalman filter and enhance the robustness of the system.

[0262] Although preferred embodiments of the present invention have been described, those skilled in the art may make additional changes and modifications to these embodiments once they are aware of the basic inventive concepts. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the invention. Obviously, those skilled in the art may make various changes and modifications to the present invention without departing from the spirit and scope of the invention. Thus, the present invention is intended to include such changes and modifications as fall within the scope of the claims and their equivalents.

Claims

1. A method for precise landing control of an unmanned aerial vehicle based on vision guidance, characterized in that: include: Set up landing signs; Control the drone to detect its own position and determine whether it has reached the landing area based on its own position; After arriving at the landing area, the drone is controlled to descend to a preset height, the drone collects environmental images, identifies the landing mark based on the environmental images and verifies it to determine the target landing mark; Calculating the angle between the drone and the target landing mark direction based on the environmental image; Calculate the horizontal distance between the UAV and the target landing mark according to the target landing mark; Using a lossless Kalman filter, the angle is integrated with the yaw angle of the drone, the horizontal distance is integrated with the horizontal speed of the drone, and the flight state of the drone is controlled so that the drone flies directly above the target landing mark; Control the drone to land vertically to the target landing mark.

2. The method according to claim 1, characterized in that The landing mark includes a high-altitude guidance mark and a near-ground guidance mark; the size of the near-ground guidance mark is smaller than that of the high-altitude guidance mark.

3. The method according to claim 2, characterized in that The landing mark is a QR code; Identifying and verifying the landing mark according to the environmental image to determine the target landing mark includes: Performing grayscale processing and binarization processing on the environment image to extract edge contours; Performing polygonal approximation on the edge contour to screen a quadrilateral area; Convert the quadrilateral area into a square to obtain the landing mark; The code in the landing mark is identified, and the target landing mark is determined according to the identification result.

4. The method according to claim 3, characterized in that The codes of the high-altitude guidance mark and the near-ground guidance mark are different; Determine the target landing mark based on the recognition results, including: Comparing the code in the landing mark with the pre-stored preset high-altitude guidance code and the pre-stored preset ground guidance code, determining the landing mark corresponding to the code consistent with the preset high-altitude guidance code as the target high-altitude guidance mark, and determining the landing mark corresponding to the code consistent with the preset ground guidance code as the target ground guidance mark; The target high-altitude guidance mark or the target near-ground guidance mark is selected as the target landing mark.

5. The method according to claim 1, wherein Calculating the angle between the UAV and the target landing mark direction according to the environmental image, including: Establishing an image coordinate system according to the arrangement direction of pixels of the environment image; The vertical axis in the image coordinate system is used as the direction of the drone, and the side of the target landing mark closest to the vertical axis in the image coordinate system is used as the target landing mark direction. The angle between the drone and the target landing mark direction is calculated based on the pixel coordinates of the two end points of the side of the target landing mark closest to the vertical axis in the image coordinate system.

6. The method according to claim 5, characterized in that The angle between the UAV and the target landing mark is the arc tangent of the slope of the side of the target landing mark closest to the vertical axis in the image coordinate system.

7. The method according to claim 1, characterized in that Calculating the horizontal distance between the UAV and the target landing mark according to the target landing mark includes: Establishing an image coordinate system according to the arrangement direction of pixels of the environment image; Calculating the side length of the target landing mark in the environment image; Calculating a proportional coefficient according to the side length of the target landing mark in the environment image and the actual physical side length; The horizontal distance between the UAV and the target landing mark is calculated based on the central pixel coordinates of the target landing mark, the central pixel coordinates of the environment image, and the scale coefficient.

8. The method according to claim 7, characterized in that The horizontal distance between the UAV and the target landing mark includes the horizontal distance in the x direction and the horizontal distance in the y direction; The horizontal distance in the x-direction is the ratio of the difference between the horizontal coordinate of the center pixel of the target landing mark and the horizontal coordinate of the center pixel of the environment image to the proportional coefficient; The horizontal distance in the y direction is the ratio of the difference between the vertical coordinate of the center pixel of the target landing mark and the vertical coordinate of the center pixel of the environment image to the proportional coefficient.

9. The method according to claim 1, characterized in that The angle is integrated with the yaw angle of the UAV by using a lossless Kalman filter, the horizontal distance is integrated with the horizontal speed of the UAV, and the flight state of the UAV is controlled, including: Initializing the covariance matrix, the process noise matrix, the measurement noise matrix, and the state variables, wherein the initialized state variables include the angle, the horizontal distance between the UAV and the target landing mark in the x direction, and the horizontal distance between the UAV and the target landing mark in the y direction; Generate a sigma point according to the state quantity; Perform state prediction and covariance matrix prediction based on the horizontal speed, yaw angle and sigma point output by the drone itself to obtain the predicted state quantity and predicted covariance matrix; The distance between the drone and the ground is detected by ground detection equipment. The measurement variables are established based on the horizontal distance in the x-direction and y-direction between the drone and the target landing mark, the distance between the drone and the ground, and the rotation matrix from the camera coordinate system to the world coordinate system. Substitute the sigma point into the observation model for calculation, and determine whether to update the measurement variable based on the calculation result; After determining to update the measured variable, updating the Kalman gain, updating the covariance matrix according to the predicted covariance matrix and the updated Kalman gain, and calculating the state estimate according to the predicted state quantity and the updated Kalman gain; The flight state of the UAV is controlled according to the state estimation value.

10. A vision-guided UAV precision landing control device applied to the method according to any one of claims 1 to 9, characterized in that: include: The position detection module is used to control the drone to detect its own position and determine whether it has reached the landing area based on its own position; A target determination module is used to control the drone to descend to a preset height after reaching the landing area, wherein the drone collects environmental images, identifies and verifies the landing mark based on the environmental images, and determines the target landing mark; An angle calculation module, used to calculate the angle between the drone and the target landing mark direction based on the environmental image; A distance calculation module is used to calculate the horizontal distance between the UAV and the target landing mark according to the target landing mark; a data fusion module, configured to fuse the angle with the yaw angle of the UAV and fuse the horizontal distance with the horizontal speed of the UAV using a lossless Kalman filter; The landing control module is used to control the flight state of the UAV so that the UAV flies to the top of the target landing mark and controls the UAV to land vertically to the location of the target landing mark.

Images