A drone sphere sensing and capturing device and method

Abstract

The present invention relates to the field of drone capture of flying objects, and discloses a drone sphere perception and capture device and method. The biggest difference between this drone device and method and other drones is that it is equipped with a depth camera and an onboard host, and the control instructions are all calculated by an intelligent method for sensing and capturing spheres deployed on the onboard host. The intelligent method includes three modules: sphere recognition based on HSV segmentation, sphere trajectory prediction based on a drag coefficient optimization physical model, and drone trajectory planning based on optimal control. The present invention solves the problem that the existing drone sphere capture system relies on external motion capture equipment and is therefore unable to achieve sphere perception and capture in other scenarios. The drone of the present invention has autonomous perception capabilities, and can achieve sphere recognition, trajectory prediction and drone trajectory planning by relying solely on its own sensors, so that the drone can quickly perceive and capture spheres in any scenario.

Description

Technical Field

[0001] The present invention belongs to the technical field of drone flying object capture, and in particular relates to a drone sphere sensing and capturing device and method. Background Art

[0002] Quadcopters are highly agile, typically boasting a high thrust-to-weight ratio and possessing large angular acceleration, enabling them to perform complex and highly maneuverable tasks, such as aerial acrobatics and intercepting flying objects. Using drones to catch and play with balls is particularly entertaining. Currently, the drone ball-catching system developed by ETH Zurich is one of the world's most renowned drone juggling systems, capable of quickly identifying and capturing balls. However, the perception of such systems relies on external motion capture equipment, which must be deployed in a fixed location at considerable cost. The drone itself lacks positioning and perception capabilities, making it difficult to capture balls in other scenarios, and the cost of equipment migration is also high. Summary of the Invention

[0003] To address the existing problem of drone ball-catching systems relying on external motion capture equipment and thus being unable to sense and capture balls in other scenarios, a drone ball-sensing and capturing device and method are proposed. The device features autonomous sensing capabilities, allowing the drone to identify balls, predict their trajectories, and plan their trajectory solely through its onboard sensors. This allows the drone to quickly sense and capture balls in any scenario. The specific technical solution is as follows:

[0004] A device for sensing and capturing spheres of an unmanned aerial vehicle (UAV), comprising an onboard host, which is a cross-shaped bracket structure. The upper middle surface of the cross-shaped bracket is provided with a flight control system and a depth camera. The flight control system is connected to the onboard host via a serial port. Each bracket end is provided with an electric speed regulator, whose signal input line is connected to the flight control system. A brushless motor is extended from each bracket end. The output end of the electric speed regulator is connected to the brushless motor via a wire. A propeller is installed on the rotor shaft of the brushless motor. A net bag frame is fixedly mounted on the frame. The onboard host obtains and calculates sensor information from the depth camera, and sends control quantity information of the UAV obtained by calculating an intelligent method for sensing and capturing spheres to the flight control system via a serial port. The flight control system controls the electric speed regulator, which controls the position and speed of the brushless motor to control the movement of the UAV.

[0005] Furthermore, the airborne host includes three modules: a sphere recognition module, a sphere trajectory prediction module, and a UAV trajectory planning module.

[0006] The present invention also discloses a method for sensing and capturing a sphere by a drone, comprising the following steps:

[0007] Step 1: sphere identification;

[0008] Step 2: sphere trajectory prediction;

[0009] Step 3: UAV trajectory planning.

[0010] Furthermore, the step 1 includes the following steps:

[0011] Step 20: Obtain the RGB image and depth information from the depth camera;

[0012] Step 21, preprocessing the RGB image;

[0013] Step 22, HSV color image segmentation;

[0014] Step 23, morphological filtering;

[0015] Step 24: binarization, contour detection, and extraction of the contour-selected area in the image through bitwise AND operation;

[0016] Step 25: Perform Hough circle detection on the image and extract the minimum covering circle of each contour detected in step 24. If the area difference between the detected Hough circle and the minimum covering circle does not exceed the threshold, and the center of the Hough circle is within the contour, then the Hough circle is taken as the outer contour of the target sphere, and its center is recorded as (x h ,y h ), the radius is denoted as r h ; If not satisfied, skip and enter the next loop;

[0017] Step 26, obtain the center of the circle (x h ,y h ) at a depth value z;

[0018] Step 27, align the center of the circle (x h ,y h ) Perform camera internal parameter conversion to obtain the target sphere plane center coordinates (x, y) in the camera coordinate system;

[0019] Step 28: Send the observed coordinates (x, y, z) of the identified target sphere center and the time t of sending the message to the sphere trajectory prediction module;

[0020] Repeat steps 20-28 to achieve real-time recognition of the sphere.

[0021] Furthermore, the step 2 includes the following steps:

[0022] Step 30: Construct a spherical dynamics model; add wind resistance to the free fall equation and optimize the dynamics equation;

[0023] Step 31: Obtaining the initial velocity of the sphere; using the position of the sphere in the two frames before and after the visual sampling to calculate the initial velocity of the sphere;

[0024] Step 32: Sphere pose estimation based on Kalman filter; Estimate the sphere pose using Kalman filter;

[0025] Step 33: Predict the trajectory of the sphere based on the physical model; calculate the position of the sphere at the next time t according to the sphere dynamics model;

[0026] Step 34: Drag coefficient optimization: While estimating the sphere's pose based on the Kalman filter and predicting its trajectory based on the physical model, the drag coefficient is optimized using a nonlinear least squares method.

[0027] Steps 32, 33, and 34 are completed between one frame and the next frame of sphere recognition sampling. During each cycle, the read sphere coordinate points and the drag coefficient are optimized. When the error value is less than the threshold, the final predicted sphere coordinate p at time t is obtained. f =(x f ,y f ,z f ) T , and send it to the UAV trajectory planning module.

[0028] Furthermore, the step 3 includes the following steps:

[0029] Step 40: Construct a UAV dynamics model; based on the current general modeling method for UAVs, the UAV is modeled as a rigid body with six degrees of freedom;

[0030] Step 41: Formulate UAV trajectory planning as an optimal control problem; use jerk To describe the trajectory x(t) of the UAV about time t, and construct the UAV trajectory planning as an optimal boundary value problem, namely OBVP problem;

[0031] Step 42: Solve the optimal control problem; use the Pontryagin minimum principle to solve the OBVP problem;

[0032] Step 43: Convert the optimal control problem into a polynomial extremum problem and solve it;

[0033] Step 44: Execute the optimal control quantity; send the calculated control quantity to the flight controller via serial communication, and the flight controller executes it, thereby enabling the drone to capture the sphere.

[0034] Furthermore, the step 1 includes the following specific steps:

[0035] Step 20, pre-processing the RGB image includes: Gaussian filtering, color space conversion: converting the BGR format to the HSV format;

[0036] Step 22, performing HSV color image segmentation according to the set color range of the desired target sphere;

[0037] Step 23, performing morphological filtering on the segmented image, including three operations of erosion, dilation, and corrosion;

[0038] Step 24, performing binarization processing on the image after morphological filtering, performing contour detection on the processed binary image, and extracting the contour selected area in the image through bitwise AND operation;

[0039] Furthermore, the step 2 includes the following specific steps:

[0040] Step 30: Construct a spherical dynamics model: Assume that the state of the sphere in three-dimensional space is x=(p x ,p y ,p z ,v x ,v y ,v z ) T , without considering the spin of the sphere, the dynamic model of the sphere under the influence of wind resistance is:

[0041]

[0042] In formula (a), g is the acceleration due to gravity, k is D is the resistance coefficient, satisfying Where m is the mass of the sphere, C D is the air resistance coefficient, ρ a is the air density, A is the cross-sectional area of ​​the sphere, and ||v|| in a continuous flight is regarded as a fixed value related to the initial motion speed of the trajectory, that is,

[0043] K=f(v0)=k D ||v|| (b)

[0044] Since equation (a) is linearly simplified, the error of simplifying the ||v|| variation is reduced by increasing the degree of freedom of K variation. Different treatments are performed on the three directions of x, y, and z. x ,K y ,K z Therefore, the dynamic model of the sphere is further simplified as follows:

[0045]

[0046] Formula (c) is the final optimization result of the spherical dynamics model, which is a first-order linear differential form that is independently decoupled in three directions. If the initial state of the ball is given by x0 = (p x0 ,py0 ,p z0 ,v x0 ,v y0 ,v z0 ) T , then the continuous motion model of the sphere's position and velocity changing with time t can be solved as follows:

[0047]

[0048]

[0049] The discrete motion model of the sphere can be obtained by equations (d) and (e):

[0050]

[0051]

[0052] In equations (f) and (g), p(i) is the position of the sphere when it moves to the i-th point, v(i) is the speed of the sphere when it moves to the i-th point, and ΔT is the visual observation sampling period;

[0053] Step 31: Obtaining the initial velocity of the sphere: From formulas (d) to (g), we can see that the initial velocity of the sphere is required when using the discrete motion model and continuous motion model of the sphere for pose estimation and trajectory prediction. Assume that the position of the sphere in the first frame of the visual sampling is p1 = (x1, y1, z1) T , the position of the sphere in the second frame image is p2 = (x2, y2, z2) T , then the initial velocity is:

[0054]

[0055] In formula (h), v0 is taken as the velocity at position p2, that is, (p2, v0) is taken as the initial state (p0, v0) of the sphere. Step 32: Sphere pose estimation based on Kalman filtering: From formulas (f) and (g), we can see that the discrete motion model of the sphere can be rewritten as:

[0056]

[0057] According to the state transfer equation of formula (i), the Kalman filter is used to estimate the position of the sphere center coordinate observation value (x, y, z) given in the sphere recognition step 28, and the optimal estimated position p is obtained. m =(x m ,y m ,z m )T, and use the optimal estimated position for the next step of sphere trajectory prediction;

[0058] Step 33: Prediction of sphere trajectory based on physical model: According to formulas (f) and (g), the sphere coordinates p at time t can be directly obtained through the continuous motion model of the sphere. f =(x f ,y f ,z f ) T , the coordinates are in the actual site coordinate system;

[0059] Step 34: Drag coefficient optimization: While estimating the sphere's pose based on the Kalman filter and predicting its trajectory based on the physical model, the drag coefficient K is optimized using the nonlinear least squares method. x ,K y ,K z , construct the square term of the position error in the xyz direction, recorded as:

[0060]

[0061] Where i is the i-th coordinate point of the sphere, and n represents the number of coordinate points selected for the nonlinear least squares method. The drag coefficient optimization objective function for each cycle is as follows:

[0062]

[0063] The Ceres nonlinear optimization library is used to call the nonlinear least squares solver to solve the above equation.

[0064] Furthermore, step 3 includes the following specific steps:

[0065] Step 40: Construct a UAV dynamics model: Based on the current general modeling method for UAVs, the UAV is modeled as a rigid body with six degrees of freedom, which are the position (x1, x2, x3) and angular velocity (w1, w2, w3) in the three-dimensional Cartesian coordinate system. Its dynamics equation is:

[0066]

[0067]

[0068] Where g is the acceleration due to gravity, R is the rotation matrix, e3 = (0, 0, 1), is the skew-symmetric matrix form of angular velocity:

[0069]

[0070] Step 41: Formulate UAV Trajectory Planning as an Optimal Control Problem: Using Jerk To describe the trajectory x(t) of the UAV about time t, the force f input to the control system is:

[0071]

[0072] Derivative (1) and substitute into (m) to obtain (o), derivative (n) and substitute into (1) to obtain (p);

[0073]

[0074]

[0075] From formula (0) and formula (p), we can deduce:

[0076]

[0077] Equations (n) and (q) show that the control force f and angular velocity w of the drone can be calculated from the jerk j of the drone. The objective function of the optimal control problem can be expressed as:

[0078]

[0079] Where i represents the number of axes in the coordinate system. For the convenience of calculation, the variables of the three axes of the drone are calculated separately, and the axis number i is omitted in the following. The state of the control system is s = (p, v, a), which includes position p, velocity v, and acceleration a. The drone is initially stationary, and the initial state is s0 = (p0, v0, a0) = (0, 0, 0). In order for the drone to capture the sphere, the position of the sphere at a certain moment must be the same as the position of the drone. According to the sphere trajectory prediction module, the position p of the ball at time t is obtained. f , then the terminal state s of the UAV motion f =(p f ,v f ,a f ), so far, the UAV trajectory planning is constructed as an optimal boundary value problem: OBVP problem;

[0080] Step 42: Solve the optimal control problem. There are mature mathematical methods for solving the OBVP problem. First, introduce the co-state λ = (λ1,λ2,λ3) and define the Hamiltonian equation:

[0081] H(s,j,λ)=j 2 +λ T f(s,j)=j 2 +λ1v+λ2a+λ3j (s)

[0082] According to the minimum principle:

[0083]

[0084] The asterisk * indicates the optimal quantity. Introducing constant coefficients α, β, and γ, we can obtain:

[0085]

[0086] The optimal input is:

[0087]

[0088] Integrating formula (v) yields:

[0089]

[0090] Then after time T from the initial state, the optimal state of the terminal state is s * (T)=s f , combined with the constraints of the minimum principle The solution is:

[0091]

[0092] Step 43: Convert the optimal control problem into a polynomial extremum problem and solve it. Substitute equation (v) into equation (o) to obtain:

[0093]

[0094] Substitute equation (x) into equation (y). From equation (x), we know that the values ​​of α, β, and γ are only related to T. Then J in equation (y) is a function that is only related to T. Use Ferrari's method to find the extreme value of equation (y) and the corresponding T.

[0095] Step 44: Execute the optimal control variable: Using the above T, substitute into formula (x) to obtain α, β, and γ. Substitute into formula (v) to obtain the optimal acceleration function with respect to time t. According to formula (n) and formula (q), the control force f and angular velocity w can be calculated. They are sent to the flight control via serial communication, and the flight control executes, thereby enabling the drone to capture the sphere.

[0096] The beneficial effects of the present invention are:

[0097] (1) The present invention does not rely on external motion capture equipment, but only relies on the sensors and onboard computing units carried by the drone itself to achieve sphere recognition, trajectory prediction and drone trajectory planning.

[0098] (2) The present invention can compensate for sensor errors and optimize the drag coefficient of the sphere in real time, thereby reducing the impact of uncertainty on sphere recognition and trajectory prediction.

[0099] (3) The present invention can take the UAV dynamics into account during trajectory planning, thereby capturing the sphere quickly and accurately. BRIEF DESCRIPTION OF THE DRAWINGS

[0100] Figure 1 It is a schematic diagram of the drone sphere sensing and capturing device of the present invention.

[0101] Figure 2 It is a schematic diagram of the overall process of the drone sphere sensing and capturing method of the present invention.

[0102] Figure 3 It is a schematic diagram of the sphere recognition process of the present invention.

[0103] Figure 4 It is a schematic diagram of the sphere trajectory prediction process of the present invention.

[0104] Figure 5 It is a schematic diagram of the UAV trajectory planning process of the present invention.

[0105] Explanation of the marks in the figure: 1. Propeller; 2. Brushless motor; 3. Electronic speed controller; 4. Flight controller; 5. Airborne host; 6. Depth camera; 7. Net bag rack; 8. Rack; 10. Sphere recognition module; 11. Sphere trajectory prediction module; 12. UAV trajectory planning module. DETAILED DESCRIPTION

[0106] In order to make the purpose, technical solutions and advantages of the present invention clearer, the technical solutions of this patent are further described in detail below with reference to the accompanying drawings and specific implementation steps.

[0107] like Figure 1 As shown, the drone sphere sensing and capture device of the present invention includes an onboard host 5. The onboard host 5 is a cross-shaped bracket structure. The upper surface of the middle of the cross-shaped bracket is equipped with a flight controller 4 and a depth camera 6. The depth camera 6 is used to transmit images and point cloud data in real time. The flight controller 4 is connected to the onboard host 5 via a serial port. Each bracket end has an electric speed controller 3, and the signal input line of the electric speed controller 3 is connected to the flight controller 4. A brushless motor 2 is extended to each bracket end, and the output end of the electric speed controller 3 is connected to the brushless motor 2 via a wire. A propeller 1 is installed on the rotor shaft of the brushless motor 2. A net bag frame 7 is fixedly mounted on the frame 8. The onboard host 5 obtains and calculates the sensor information of the depth camera 6, and then sends the drone control quantity information calculated by the intelligent method of sensing and capturing the sphere to the flight controller 4 via the serial port. The flight controller 4 controls the electric speed controller 3, and the electric speed controller 3 controls the position and speed of the brushless motor 2, thereby controlling the movement of the drone.

[0108] like Figure 2 As shown, the drone sphere sensing and capturing method of the present invention includes three modules: a sphere recognition module 10 , a sphere trajectory prediction module 11 , and a drone trajectory planning module 12 , all of which are deployed in the airborne host 5 .

[0109] Example 1: Start the drone and hover it in the air. The operator faces 3-5 meters in front of the drone's depth camera, so that the operator is within the depth camera's field of view. The operator holds a ball and throws it forward.

[0110] The onboard host 5 obtains the sensor information of the depth camera 6 and calculates, first, the sphere recognition 10 is performed, such as Figure 3 As shown, it includes the following calculation methods:

[0111] Step 20, obtaining the RGB image and depth information of the depth camera 6;

[0112] Step 21, preprocessing the RGB image, including: Gaussian filtering, color space conversion (BGR format to HSV format);

[0113] Step 22, performing HSV color image segmentation according to the set color range of the desired target sphere;

[0114] Step 23, performing morphological filtering on the segmented image, including three operations of erosion, dilation, and corrosion;

[0115] Step 24, performing binarization processing on the image after morphological filtering, performing contour detection on the processed binary image, and extracting the contour selected area in the image through bitwise AND operation;

[0116] Step 25: Perform Hough circle detection on the image and extract the minimum covering circle of each contour detected in step 24. If the area difference between the detected Hough circle and the minimum covering circle does not exceed the threshold, and the center of the Hough circle is within the contour, then the Hough circle is taken as the outer contour of the target sphere, and its center is recorded as (x h ,y h ), the radius is denoted as r h ; If not satisfied, skip and enter the next loop;

[0117] Step 26, obtain the center of the circle (x h ,y h ) at a depth value z;

[0118] Step 27, align the center of the circle (x h ,y h ) Perform camera internal parameter conversion to obtain the target sphere plane center coordinates (x, y) in the camera coordinate system;

[0119] Step 28: Send the observed coordinate values ​​(x, y, z) of the identified target sphere center and the time t of sending the message to the sphere trajectory prediction module 11.

[0120] Repeat steps 20-28 to achieve real-time recognition of the sphere.

[0121] Then, the ball trajectory prediction 11 is performed, which can more accurately estimate the real-time ball position obtained from the ball recognition 10 module and predict the ball's future trajectory while taking into account external interference such as wind resistance. Figure 4 As shown, the trajectory prediction method mainly includes the following calculation methods:

[0122] Step 30: Construct a spherical dynamics model. Assume that the state of the sphere in three-dimensional space is x=(p x ,p y ,p z ,v x ,v y ,v z ) T , without considering the spin of the sphere, the dynamic model of the sphere under the influence of wind resistance is:

[0123]

[0124] In formula (a), g is the acceleration due to gravity, k is D is the resistance coefficient, satisfying Where m is the mass of the sphere, C D is the air resistance coefficient, ρ a is the air density, and A is the cross-sectional area of ​​the sphere. In order to reduce the nonlinear and isotropic coupling effects caused by ||v||, ||v|| in a continuous flight is regarded as a fixed value related to the initial motion speed of the trajectory, that is,

[0125] K=f(v0)=k D ||v|| (b)

[0126] Since equation (a) is linearly simplified, the error of simplifying the ||v|| variation is reduced by increasing the degree of freedom of K variation. Different treatments are performed on the three directions of x, y, and z. x ,K y ,K z Therefore, the dynamic model of the sphere is further simplified as follows:

[0127]

[0128] Formula (c) is the final optimization result of the spherical dynamics model, which is a first-order linear differential form that is independently decoupled in three directions. If the initial state of the ball is given by x0 = (p x0 ,p y0 ,p z0 ,v x0 ,v y0 ,v z0 ) T , then the continuous motion model of the sphere's position and velocity changing with time t can be solved as follows:

[0129]

[0130]

[0131] The discrete motion model of the sphere can be obtained by equations (d) and (e):

[0132]

[0133]

[0134] In equations (f) and (g), p(i) is the position of the sphere when it moves to the i-th point, v(i) is the velocity of the sphere when it moves to the i-th point, and ΔT is the visual observation sampling period.

[0135] Step 31: Obtain the initial velocity of the sphere. From formulas (d) to (g), we can see that the initial velocity of the sphere is required when using the discrete motion model and continuous motion model of the sphere for pose estimation and trajectory prediction. Assume that the position of the sphere in the first frame of the visual sampling is p1 = (x1, y1, z1) T , the position of the sphere in the second frame image is p2 = (x2, y2, z2) T , then the initial velocity is:

[0136]

[0137] In formula (h), v0 is taken as the velocity at position p2, that is, (p2, v0) is taken as the initial state (p0, v0) of the sphere.

[0138] Step 32: Estimation of sphere pose based on Kalman filter. From formulas (f) and (g), the discrete motion model of the sphere can be rewritten as:

[0139]

[0140] In order to reduce the sensor error and obtain a more accurate sphere position, according to the state transfer equation of formula (i), the Kalman filter is used to estimate the sphere center coordinate observation value (x, y, z) given in step 28 of sphere recognition 10, and the optimal estimated position p is obtained. m =(x m ,y m ,z m ) T , and the optimal estimated position is used for the next step of sphere trajectory prediction.

[0141] Step 33: Prediction of sphere trajectory based on physical model. According to formulas (f) and (g), the sphere coordinates p at time t can be directly obtained through the continuous motion model of the sphere. f =(xf ,y f ,z f ) T , which is in the actual site coordinate system.

[0142] Step 34: Drag coefficient optimization. While the sphere pose estimation 32 based on Kalman filter and the sphere trajectory prediction 33 based on physical model are being performed, the drag coefficient K is optimized using nonlinear least squares method. x ,K y ,K z , construct the square term of the position error in the xyz direction, recorded as:

[0143]

[0144] Where i is the i-th coordinate point of the sphere, and n represents the number of coordinate points selected for the nonlinear least squares method. The drag coefficient optimization objective function for each cycle is as follows:

[0145]

[0146] The above equation can be solved by using the Ceres nonlinear optimization library and calling the nonlinear least squares solver.

[0147] Steps 32, 33, and 34 are completed between one frame and the next frame of ball recognition 10. During each cycle, the read ball coordinates and drag coefficient are optimized, which can accurately predict the ball's motion trajectory. When the error value in equation (j) is less than the threshold, the final predicted ball coordinates p at time t are obtained. f =(x f ,y f ,z f ) T , and send it to the UAV trajectory planning 12 module.

[0148] Finally, the UAV trajectory planning 12 is performed and the control quantity is executed. The sphere coordinates obtained by the sphere trajectory prediction 11 module are used as the terminal state of the UAV motion planning. Figure 5 As shown, it includes the following calculation methods:

[0149] Step 40: Construct the UAV dynamics model. According to the current general modeling method of UAVs, the UAV is modeled as a rigid body with six degrees of freedom, namely the position (x1, x2, x3) and angular velocity (w1, w2, w3) in the three-dimensional Cartesian coordinate system. Its dynamics equation is:

[0150]

[0151]

[0152] Where g is the acceleration due to gravity, R is the rotation matrix, e3 = (0, 0, 1), is the skew-symmetric matrix form of angular velocity:

[0153]

[0154] Step 41: Formulate the UAV trajectory planning as an optimal control problem. Use jerk To describe the trajectory x(t) of the UAV about time t, the force f input to the control system is:

[0155]

[0156] Taking the derivative of equation (1) and substituting it into equation (m) gives equation (o). Taking the derivative of equation (n) and substituting it into equation (1) gives equation (p).

[0157]

[0158]

[0159] From formula (0) and formula (p), we can deduce:

[0160]

[0161] Equations (n) and (q) show that the control force f and angular velocity w of the drone can be calculated from the drone's acceleration j. The objective function of the optimal control problem can be expressed as:

[0162]

[0163] Where i represents the number of axes in the coordinate system. For the convenience of calculation, the variables of the three axes of the drone are calculated separately, and the axis number i is omitted in the following description. The state quantity of the control system is s = (p, v, a), which includes position p, velocity v, and acceleration a. The drone is stationary at the initial moment, and the initial state is s0 = (p0, v0, a0) = (0, 0, 0). In order for the drone to capture the sphere, it is necessary to ensure that the position of the sphere at a certain moment is the same as the position of the drone. According to the sphere trajectory prediction module (11), the position p of the ball at time t is obtained f , then the terminal state s of the UAV motion f =(p f ,v f ,a f ). At this point, the UAV trajectory planning is constructed as an optimal boundary value problem (OBVP problem).

[0164] Step 42: Solve the optimal control problem. There are mature mathematical methods for solving the OBVP problem. First, introduce the co-state λ = (λ1,λ2,λ3) and define the Hamiltonian equation:

[0165] H(s,j,λ)=j 2 +λ T f(s,j)=j 2 +λ1v+λ2a+λ3j (s)

[0166] According to the minimum principle:

[0167]

[0168] The asterisk * indicates the optimal quantity. Introducing constant coefficients α, β, and γ, we can obtain:

[0169]

[0170] The optimal input is:

[0171]

[0172] Integrating formula (v) yields:

[0173]

[0174] Then after time T from the initial state, the optimal state of the terminal state is s * (T)=s f , combined with the constraints of the minimum principle The solution is:

[0175]

[0176] Step 43: Convert the optimal control problem into a polynomial extremum problem and solve it. Substituting equation (v) into equation (o), we get:

[0177]

[0178] Substituting formula (x) into formula (y), we know from formula (x) that the values ​​of α, β, and γ are only related to T. Then J in formula (y) is a function that is only related to T. The extreme value of formula (y) and the corresponding T can be obtained using the Ferrari method.

[0179] Step 44: Execute the optimal control variable. Using the above T, substitute into equation (x) to obtain α, β, and γ. Substituting into equation (v) to obtain the optimal jerk as a function of time t, the control force f and angular velocity w are calculated using equations (n) and (q). These are sent to the flight controller 4 via serial communication, which executes the command, allowing the drone to capture the sphere.

[0180] The present invention is not limited to the above embodiments. Within the knowledge of those skilled in the art, various modifications can be made without departing from the spirit of the present invention. For example, a single module from sphere recognition, sphere trajectory prediction, or drone motion planning can be used for other tasks.

Claims

1. A method for sensing and capturing a sphere of a drone using a sphere sensing and capturing device for a drone, wherein the sphere sensing and capturing device for a drone comprises an onboard host (5), wherein the onboard host (5) is a cross-shaped bracket structure, wherein a flight control (4) and a depth camera (6) are provided on the middle upper surface of the cross-shaped bracket, wherein the flight control (4) is connected to the onboard host (5) via a serial port, wherein the onboard host (5) comprises three modules: a sphere recognition module (10), a sphere trajectory prediction module (11), and a drone trajectory planning module (12); wherein each bracket end has an electric regulator (3), wherein a signal input line of the electric regulator (3) is connected to the flight control ( 4) connection, each bracket end is extended and connected with a brushless motor (2), the output end of the electric regulator (3) is connected to the brushless motor (2) through a wire, a propeller (1) is installed on the rotor shaft of the brushless motor (2), and the net bag frame (7) is fixedly installed on the frame (8), the airborne host (5) obtains and calculates the sensor information of the depth camera (6), and sends the control quantity information of the drone calculated by the intelligent method of sensing and capturing the sphere to the flight control (4) through the serial port, and the flight control (4) controls the electric regulator (3), and the electric regulator (3) controls the position and speed of the brushless motor (2) to control the movement of the drone; it is characterized in that The method comprises the following steps: Step 1: sphere identification; Step 2: sphere trajectory prediction; The step 2 includes the following specific steps: Step 30: Construct a spherical dynamics model: Assume that the state of the sphere in three-dimensional space is x=(p x ,p y ,p z ,v x ,v y ,v z ) T , without considering the spin of the sphere, the dynamic model of the sphere under the influence of wind resistance is: In formula (a), g is the acceleration due to gravity, kD is the drag coefficient, and it satisfies Where m is the mass of the sphere, C D is the air resistance coefficient, ρ a is the air density, A is the cross-sectional area of ​​the sphere, and ‖v‖ in a continuous flight is regarded as a fixed value related to the initial motion speed of the trajectory, that is, K=f(v0)=k D ∥v∥(b) Since the linear simplification of equation (a) is done, the error of simplification of ‖v‖ is reduced by increasing the freedom of K change. Different treatments are made for the three directions of x, y, and z. x ,K y ,K z Therefore, the dynamic model of the sphere is further simplified as follows: Formula (c) is the final optimization result of the spherical dynamics model, which is a first-order linear differential form that is independently decoupled in three directions. If the initial state of the ball is given by x0 = (p x0 ,p y0 ,p z0 ,v x0 ,v y0 ,v z0 ) T , then the continuous motion model of the sphere's position and velocity changing with time t can be solved as follows: The discrete motion model of the sphere can be obtained by equations (d) and (e): In equations (f) and (g), p(i) is the position of the sphere when it moves to the i-th point, v(i) is the speed of the sphere when it moves to the i-th point, and ΔT is the visual observation sampling period; Step 31: Obtaining the initial velocity of the sphere: From formulas (d) to (g), we can see that the initial velocity of the sphere is required when using the discrete motion model and continuous motion model of the sphere for pose estimation and trajectory prediction. Assume that the position of the sphere in the first frame of the visual sampling is p1 = (x1, y1, z1) T , the position of the sphere in the second frame image is p2 = (x2, y2, z2) T , then the initial velocity is: In formula (h), v0 is taken as the velocity at position p2, that is, (p2, v0) is taken as the initial state of the sphere (p0, v0); Step 32: Sphere pose estimation based on Kalman filtering: From formulas (f) and (g), the discrete motion model of the sphere can be rewritten as: According to the state transfer equation of formula (i), the Kalman filter is used to estimate the position of the sphere center coordinate observation value (x, y, z) given in the sphere recognition step 28, and the optimal estimated position p is obtained. m =(x m ,y m ,z m ) T , and use the optimal estimated position for the next step of sphere trajectory prediction; Step 33: Prediction of sphere trajectory based on physical model: According to formulas (f) and (g), the sphere coordinates p at time t can be directly obtained through the continuous motion model of the sphere. f =(x f ,y f ,z f ) T , the coordinates are in the actual site coordinate system; Step 34: Drag coefficient optimization: While estimating the sphere’s pose based on the Kalman filter and predicting its trajectory based on the physical model, the drag coefficients Kx, Ky, and Kz are optimized using the nonlinear least squares method to construct the squared term of the position error in the xyz direction, which is expressed as: Where i is the i-th coordinate point of the sphere, n represents the number of coordinate points selected for the nonlinear least squares method, and the drag coefficient optimization objective function in each cycle is as follows: Use Ceres nonlinear optimization library and call nonlinear least squares solver to solve the above equation; Step 3: UAV trajectory planning; The step 3 includes the following specific steps: Step 40: Construct a UAV dynamics model: Based on the current general modeling method for UAVs, the UAV is modeled as a rigid body with six degrees of freedom, which are the position (x1, x2, x3) and angular velocity (w1, w2, w3) in the three-dimensional Cartesian coordinate system. Its dynamics equation is: Where g is the acceleration due to gravity, R is the rotation matrix, e3 = (0, 0, 1), is the skew-symmetric matrix form of angular velocity: Step 41: Formulate UAV Trajectory Planning as an Optimal Control Problem: Using Jerk To describe the trajectory x(t) of the UAV about time t, the force f input to the control system is: Derivative (1) and substitute into (m) to obtain (o), derivative (n) and substitute into (1) to obtain (p); From formula (0) and formula (p), we can deduce: Equations (n) and (q) show that the control force f and angular velocity w of the drone can be calculated from the jerk j of the drone. The objective function of the optimal control problem can be expressed as: Where i represents the number of axes in the coordinate system. For the convenience of calculation, the variables of the three axes of the drone are calculated separately, and the axis number i is omitted in the following. The state of the control system is s = (p, v, a), which includes position p, velocity v, and acceleration a. The drone is initially stationary, and the initial state is s0 = (p0, v0, a0) = (0, 0, 0). In order for the drone to capture the sphere, the position of the sphere at a certain moment must be the same as the position of the drone. According to the sphere trajectory prediction module, the position p of the ball at time t is obtained. f , then the terminal state s of the UAV motion f =(p f ,v f ,a f ), so far, the UAV trajectory planning is constructed as an optimal boundary value problem: OBVP problem; Step 42: Solve the optimal control problem. There are mature mathematical methods for solving the OBVP problem. First, introduce the co-state λ = (λ1,λ2,λ3) and define the Hamiltonian equation: H(s,j,λ)=j 2 +λ T f(s,j)=j 2 +λ1v+λ2a+λ3j (s) According to the minimum principle: The asterisk * indicates the optimal quantity. By introducing the constant coefficients α, β, and γ, we can obtain: The optimal input is: Integrating formula (v) yields: Then after time T from the initial state, the optimal state of the terminal state is s * (T)=s f , combined with the constraints of the minimum principle The solution is: Step 43: Convert the optimal control problem into a polynomial extremum problem and solve it. Substitute equation (v) into equation (o) to obtain: Substitute equation (x) into equation (y). From equation (x), we know that the values ​​of α, β, and γ are only related to T. Then J in equation (y) is a function that is only related to T. Use Ferrari's method to find the extreme value of equation (y) and the corresponding T. Step 44: Execute the optimal control variable: Using the above T, substitute into formula (x) to obtain α, β, and γ. Substitute into formula (v) to obtain the optimal acceleration function with respect to time t. According to formula (n) and formula (q), the control force f and angular velocity w can be calculated. They are sent to the flight control via serial communication, and the flight control executes, thereby enabling the drone to capture the sphere.

2. The method according to claim 1, characterized in that The step 1 comprises the following steps: Step 20: Obtain the RGB image and depth information from the depth camera; Step 21, preprocessing the RGB image; Step 22, HSV color image segmentation; Step 23, morphological filtering; Step 24: binarization, contour detection, and extraction of the contour-selected area in the image through bitwise AND operation; Step 25: Perform Hough circle detection on the image and extract the minimum covering circle of each contour detected in step 24. If the area difference between the detected Hough circle and the minimum covering circle does not exceed the threshold, and the center of the Hough circle is within the contour, then the Hough circle is taken as the outer contour of the target sphere, and its center is recorded as (x h ,y h ), The radius is denoted as r h ; If not satisfied, skip and enter the next loop; Step 26, obtain the center of the circle (x h ,y h ) at a depth value z; Step 27, align the center of the circle (x h ,y h ) Perform camera internal parameter conversion to obtain the target sphere plane center coordinates (x, y) in the camera coordinate system; Step 28: Send the observed coordinates (x, y, z) of the identified target sphere center and the time t of sending the message to the sphere trajectory prediction module; Repeat steps 20-28 to achieve real-time recognition of the sphere.

3. The method according to claim 1, characterized in that The step 1 includes the following specific steps: Step 20, pre-processing the RGB image includes: Gaussian filtering, color space conversion: converting the BGR format to the HSV format; Step 22, performing HSV color image segmentation according to the set color range of the desired target sphere; Step 23, performing morphological filtering on the segmented image, including three operations of erosion, dilation, and corrosion; Step 24 , binarize the image after morphological filtering, perform contour detection on the processed binary image, and extract the contour-selected area in the image through a bitwise AND operation.

Images