Unmanned aerial vehicle grabbing and positioning method on offshore mobile platform

Abstract

The invention relates to an unmanned aerial vehicle grabbing and positioning method on an offshore mobile platform, and the method comprises the steps: firstly building an imaging model and an RT expression between a control point under a target coordinate system of an unmanned aerial vehicle and an imaging coordinate under a visual camera coordinate system, and setting a plurality of mark point target circles on the unmanned aerial vehicle; then n coplanar and non-collinear mark point target circles are selected as control points and combined with the forming model to solve RT, meanwhile, a problem is converted into a nonlinear optimization solving problem, and an LM algorithm is utilized to optimize and solve the optimal iteration step length h to improve the calculation precision, so that when the unmanned aerial vehicle lands on an unmanned aerial vehicle carrying platform, the calculation precision of the unmanned aerial vehicle is improved. And the visual camera performs image acquisition on the mark point target circle on the unmanned aerial vehicle, and the pose data of the unmanned aerial vehicle can be solved through the steps to guide the grabbing mechanism to complete grabbing. The method has the advantages of being high in vision measurement precision, stable in measurement result and the like, and the unmanned aerial vehicle grabbing and recycling requirements of an offshore mobile platform can be met.

Description

Technical Field

[0001] The invention relates to the field of unmanned aerial vehicles, and in particular to a method for grasping and positioning unmanned aerial vehicles on a marine mobile platform. Background Art

[0002] Drones are widely used in the ocean, with the characteristics of remote monitoring and reconnaissance, rapid deployment and flexible maneuverability, all-weather operation and reduced personnel risks. The existing technology mainly tests and optimizes the performance of drone systems under different sea conditions through experiments to ensure that drones can be efficiently recovered under various wave conditions, especially to verify the reliability and stability of the system in a simulated wave environment to ensure the safety and accuracy of the recovery process.

[0003] In the process of drone capture and recovery, the traditional method is to calibrate the camera based on the known reference objects of the external structure, such as control lines or control points, and based on the scene information, using mathematical transformation formulas and optimization, and then realize the spatial positioning and target tracking of the drone through the camera recognition of the cooperation mark on the drone, and finally guide the capture device to adjust the posture to complete the drone capture and recovery operation. However, the above method has high requirements for scene information and camera movement, and the offshore mobile platform is usually in a turbulent state caused by waves, and when encountering bad weather or other emergencies, its scene information is more complicated. The above method often cannot meet the requirements of drone recovery.

[0004] The patent with the authorization announcement number CN110044212B discloses a method for capturing and recovering a rotary-wing UAV based on visual measurement information. When the UAV enters the imaging range, the binocular vision system collects the image information of the objects in its imaging area, processes and analyzes the image information for judgment. When the UAV enters the clear imaging range, the binocular vision system uses a high-speed industrial camera to collect and store the image information of the UAV every Δt. After image processing, the marking points on the UAV image are identified, and then the pose vector of the UAV in the image is obtained through algorithm processing, thereby realizing the spatial positioning and target tracking of the UAV. Finally, when the UAV is captured, the corresponding pose of the capture device is adjusted through segmented trajectory fitting and target prediction. As mentioned above, this method has high requirements for scene information and camera motion, and its control accuracy under complex sea conditions needs to be further improved.

[0005] The patent with authorization announcement number CN114200948B discloses a method for autonomous landing of UAV based on vision assistance. The method is to achieve autonomous landing of UAV by designing a landing mark and capturing the landing mark image through the downward-looking camera carried by the UAV itself to adjust the posture, but the method does not take into account the capture and recovery of the UAV. Summary of the invention

[0006] The purpose of the present invention is to provide a method for grasping and positioning a UAV on an offshore mobile platform, which utilizes a visual camera to identify a target circle of marker points on the UAV, and then obtains a translation matrix T and a rotation matrix R through a forming model composed of point coordinates in the UAV target coordinate system and imaging coordinates of corresponding points in the visual camera coordinate system, thereby obtaining UAV posture data to realize UAV grasping and recovery control. In addition, the present invention utilizes the LM algorithm to calculate the optimal iteration step size, which has the advantages of high visual measurement accuracy and stable measurement results, thereby ensuring the UAV grasping control accuracy and meeting the UAV grasping requirements of the offshore mobile platform.

[0007] The objective of the present invention is achieved through the following technical solutions:

[0008] A method for grasping and positioning a UAV on an offshore mobile platform comprises the following steps:

[0009] Step 1: Establish the measurement reference coordinate system, the drone target coordinate system and the visual camera coordinate system;

[0010] Step 2: Establish an imaging model and RT expression between a control point (X, Y, Z) in the drone target coordinate system and the imaging coordinates (u, v) in the visual camera coordinate system, where R is the rotation matrix of the measurement reference coordinate system and the visual camera coordinate system, and T is the translation matrix of the measurement reference coordinate system and the visual camera coordinate system.

[0011] Step 3: A plurality of target circles of marker points are set on the drone, and n coplanar and non-colinear target circles of marker points are selected as control points and RT is solved in combination with the forming model, where n≥4;

[0012] Step 4: Convert the problem into a nonlinear optimization problem, and use the LM algorithm to optimize the optimal iteration step h to improve the calculation accuracy;

[0013] Step 5: When the movement frequency of the drone platform reaches the set frequency, the drone lands on the drone platform, and then the visual camera collects images of the target circle of the marker points on the drone, and solves the drone posture data through the above steps to guide the grasping mechanism to complete the grasping.

[0014] In step 2, the forming model is shown in the following formula (1):

[0015]

[0016] In the above formula (1), fu and fv are the focal ratios of the visual camera, u0 and v0 are the principal point coordinates of the visual camera, fu, fv, u0 and v0 constitute the camera intrinsic parameter matrix, and s is the scale coefficient;

[0017] RT is shown in the following formula (2):

[0018]

[0019] In the above formula (2), R includes r ij , i = 0, 1, 2, j = 0, 1, 2, T includes tx, ty, tz;

[0020] In step 3, for the above equation (1), let the Z-direction component of the spatial coordinate on the right side of the equation be 0, and we get:

[0021]

[0022] In the above formula (4), t0, t1, and t2 constitute the translation matrix T;

[0023] Let the product of the camera intrinsic parameter matrix containing fu, fv, u0 and v0 and the RT matrix be expressed as:

[0024]

[0025] In the above formula (5), h8 = 1.0, and the remaining elements in the H matrix are the unknown quantities to be solved;

[0026] Substituting the above formula (5) into formula (4) and rearranging it, we get:

[0027]

[0028] The above formula (6) is a control point in the UAV target coordinate system, and at least four control points are required to complete the solution process:

[0029] Assuming that the sequence numbers of the n control points are 1, 2, 3…n, n ≥ 4, the above formula (6) is expanded to:

[0030]

[0031] make:

[0032]

[0033] Then the above formula (7) can be written as:

[0034] B·L=C (8);

[0035] According to the above formula (8), we can get:

[0036] L(B T B) -1 ·B T ·C (9);

[0037] So far, all the elements of the H matrix have been solved, including (X1, Y1)…(X n , Y n) are the coordinate values ​​of each control point in the drone target coordinate system, (u1, v1)…(u n , v n ) is the imaging coordinate value of each control point in the visual camera coordinate;

[0038] Move the camera intrinsic parameter matrix on the right side of equation (5) to the left side of the equal sign to obtain the following equation (10):

[0039]

[0040] Both ends of the above formula (10) are 3×3 matrices, and the elements correspond one to one. The solution is:

[0041]

[0042] r2=r0×r1

[0043]

[0044] In the above formula (11), r0 and r1 are used to satisfy the constraint condition that the vector modulus is 1.0 to calculate the value of the proportional coefficient s, and then r0, r1, r2, t are obtained, that is, RT in the above formula (2).

[0045] In step 3, the proportionality coefficient s is solved as follows:

[0046] make:

[0047]

[0048] Then we have:

[0049] r0=sA -1 H0

[0050] r1=sA -1 H1;

[0051] In the above formula, H0 is the matrix composed of h0, h3, and h6, H1 is the matrix composed of h1, h4, and h7, and r0 and r1 satisfy the constraint that the vector modulus is 1.0, then:

[0052] ||r0||=|||sA -1 H0||=1

[0053] ||r1||=l||sA -1 H1||=1;

[0054] Then we get:

[0055]

[0056] In step 4, the problem is transformed into a nonlinear optimization problem:

[0057] The general form of the nonlinear least squares problem is shown in equation (12):

[0058]

[0059] x * =aegmin x {F(x)} (12);

[0060] In the above formula (12), f: R n →R m , m→n, n represents the number of independent variables, m represents the number of equations, x * represents the optimal solution;

[0061] The control point reprojection error is used as f(x), as shown in equation (13):

[0062]

[0063] In the above formula (13), (u′ i ,v′ i ) represents the imaging coordinates of the ith control point calculated according to the updated value of the current optimization variable in each iteration, (u i ,v i ) represents the actual imaging coordinate of the ith control point, and m represents the number of control points;

[0064] The optimal iteration step length h in step 4 is:

[0065] Taylor expansion of f in the above formula (12) yields:

[0066] f(x+h)=f(x)+J(x)h+O(||h|| 2 ) (14);

[0067] In the above formula (14), h represents the step size, O(||h|| 2 ) is the Taylor expansion term, J∈R m×n is the Jacobian matrix;

[0068] make Where “≡” means constant equality, then:

[0069]

[0070] In the above formula (15), f = f(x), J = J(x), and it can be seen from the above formula (15) that the process of determining the iteration step size h is transformed into the process of searching for the minimum value of L(h), where:

[0071] L′(h)=J T f+J T Jh;

[0072] When L(h) reaches an extreme value, L ′ (h) = 0, and then the optimal step length h is obtained.

[0073] In step 4, a damping term is introduced to limit the amplitude of h during the solution of the step length h:

[0074]

[0075] In the above formula (16), h lm represents the step size after LM algorithm optimization, argmin represents the parameter value that makes the function value reach the minimum or minimum, and μ represents the damping coefficient;

[0076] Taking the derivative of equation (16), we get:

[0077]

[0078] Let the derivative of the above formula (17) be 0 and we get:

[0079] (J T J+μI)h lm =-J T f, μ ≥ 0 (18);

[0080] In the above formula (18), I is the unit matrix, and then the steepest descent method or Gauss-Newton method is used to solve the problem according to the value of μ.

[0081] The advantages and positive effects of the present invention are:

[0082] 1. The present invention utilizes a visual camera to identify a target circle of a marker point on a UAV, and then obtains a translation matrix T and a rotation matrix R through a forming model formed by the point coordinates in the UAV target coordinate system and the imaging coordinates of the corresponding points in the visual camera coordinate system, thereby obtaining the coordinates of the target circle of the marker point in the measurement reference coordinate system, that is, obtaining the UAV posture data to realize the UAV grasping and recovery control.

[0083] 2. The present invention uses the reprojection error of the control point (marker point target circle) as f(x) to transform the problem into a nonlinear optimization problem, and uses the LM algorithm to calculate the optimal iteration step size h to improve the calculation accuracy, thereby ensuring the UAV grasping control accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0084] Figure 1 A schematic flow chart of the method of the present invention,

[0085] Figure 2 A schematic diagram of a grabbing mechanism using the method of the present invention is shown in FIG.

[0086] Figure 3 for Figure 2Top view of the grabbing mechanism.

[0087] Figure 4 for Figure 3 Aerial view of drone,

[0088] Figure 5 for Figure 2 Schematic diagram of the installation position of the visual camera on the gripping mechanism.

[0089] Figure 6 This is a comparison chart of the measurement accuracy between the method of the present invention and the existing Zhang Zhengyou method.

[0090] Among them, 1 is a UAV, 101 is a target circle of a marking point, 2 is a grasping mechanism, 201 is a flange, 202 is a mobile truss, 3 is a UAV platform, and 4 is a visual camera. DETAILED DESCRIPTION

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

[0092] like Figure 1 As shown, the method of the present invention comprises the following steps:

[0093] Step 1: Establish the measurement reference coordinate system, UAV target coordinate system and visual camera coordinate system.

[0094] like Figures 2 to 5 As shown, the device of the present invention comprises a drone 1, a gripping mechanism 2 and a drone platform 3, wherein the drone 1 is arranged on the drone platform 3, and as shown in FIG. Figure 5 As shown, the visual camera 4 is arranged on the gripping mechanism 2, wherein the gripping mechanism 2 has the control capability of three degrees of freedom, and performs precise translation control in the three directions of X, Y, and Z respectively. Through the coordinated control of these three directions, the mechanical claw of the gripping mechanism 2 can be flexibly moved in a complex space environment, and the drone platform 3 can achieve a swing angle change of ±10° in roll and ±5° in pitch, and the swing frequency is stably maintained at a level not less than 0.1 Hz to match the turbulence of the waves. The gripping mechanism 2 and the drone platform 3 are both well-known technologies in the art and are commercially available products.

[0095] In this embodiment, the measurement reference coordinate system O B -X B Y B Z B The establishment of Figure 3 and Figure 5 As shown, it is established at the center of the flange 201 at the lower end of the mobile truss 202 of the grasping mechanism 2, and the coordinate system X B The direction is consistent with the forward and backward movement direction of the truss, and the backward movement direction is positive. B The direction is consistent with the horizontal movement direction of the truss, and the left movement direction is positive, ZB Direction follows the right-hand rule.

[0096] In this embodiment, the drone target coordinate system O A -X A Y A Z A The establishment of Figure 4 As shown, the drone is provided with a plurality of target circles 101 with marking points, and the origin of the drone target coordinate system O A The distance between the closest mark point target circle 101 is L, X A and X B The direction is parallel and backward is positive, Y A and Y B The direction is parallel and to the left, Z A In addition, when the center of the flange 201 at the lower end of the truss of the grabbing mechanism 2 is aligned with the target coordinate system O of the drone, A When the gripping mechanism 2 and the manipulator fixtures on both sides of the lower end are parallel to the central axis of the drone, the object to be measured can be accurately grasped. Figure 5 As shown, the length of the visual camera support arm is equal to L, and the design principle of each mark point target circle 101 is: when the mechanical claw on the lower side of the grasping mechanism 2 accurately grasps the object to be measured, the mark point target circle 101 needs to appear in the middle of the field of view of the visual camera 4 without being blocked.

[0097] Visual camera coordinate system O C -X C Y C Z C The establishment of Figure 5 As shown, the visual camera 4 is fixed on the mobile truss 202 of the grasping mechanism 2, and the origin O C Built on Vision Camera 4, X C and X B The direction is parallel and backward is positive, Y C and Y B The direction is parallel and to the left, Z C Direction follows the right-hand rule.

[0098] The relationship between the above coordinate systems is: the measurement reference coordinate system obtains the visual camera coordinate system through translation (translation matrix T) and rotation (rotation matrix R), and the imaging coordinate relationship between the points in the drone target coordinate system and the corresponding points in the visual camera coordinate system is shown in the forming model in the following steps. The method of the present invention is to set N control points (i.e., the target circle 101 of the marker point) in the drone target coordinate system (i.e., the scene), and form corresponding N feature points after imaging by the visual camera 4, and then use the camera model internal parameter matrix, the imaging coordinates of the N feature points and the corresponding target model parameters (i.e., the three-dimensional coordinates of the N control points in the drone target coordinate system) to solve RT, and then obtain the coordinates of the feature points in the visual camera coordinate system in the measurement reference coordinate system through RT, and send them to the device control system for controlling the grasping mechanism 2 to move and grasp the drone 1.

[0099] Step 2: Establish the imaging model and RT expression between a control point (X, Y, Z) in the UAV target coordinate system and the imaging coordinates (u, v) in the visual camera coordinate system.

[0100] The molding model is shown in the following formula (1):

[0101]

[0102] In the above formula (1), fu and fv are the focal ratios of the visual camera 4, which are the ratios of the camera imaging focal length to the size of the imaging target pixel; u0 and v0 are the principal point coordinates of the visual camera 4, and fu and fv as well as u0 and v0 need to be obtained in advance through camera intrinsic calibration and form the camera intrinsic matrix in the above formula (1), which is a well-known technology in the art. In addition, in the above formula (1), s is a proportional coefficient.

[0103] The RT expression is shown in the following formula (2):

[0104]

[0105] In the above formula (2), R is the rotation matrix (r ij , i = 0, 1, 2, j = 0, 1, 2), which is used to describe the rotation direction of the coordinate axis of the measurement reference coordinate system relative to the coordinate axis of the visual camera coordinate system, T is the translation matrix (tx, ty, tz), which is used to describe the position of a point in the measurement reference coordinate system (such as the origin of the coordinate system) in the visual camera coordinate system.

[0106] In addition, in the above formula (2), the R matrix contains 9 elements, but there are only three relatively independent variables (ax, ay, az), ax, ay, az represent the rotation angles around the x-axis, y-axis, and z-axis respectively, and the corresponding relationship between ax, ay, az and R is as follows:

[0107] R=Rz·Ry·Rx

[0108]

[0109] Rx, Ry, and Rz represent the offset rotation angles of any point in the drone target coordinate system in the XYZ direction of the measurement reference coordinate system.

[0110] Step 3: Solve RT.

[0111] exist Figure 4 Select n coplanar but non-collinear marker points from all points of the marker point target circle 101 shown in the figure, where n≥4. For the above equation (1), let the Z-direction component of the spatial coordinates on the right side of the equation be 0 (the three-dimensional coordinates of the coplanar points are all 0 in the Z direction), and we can get:

[0112]

[0113] In the above formula (4), t0, t1, and t2 constitute the translation matrix T, which, like tx, ty, and tz, represents the position of a point in space in the visual camera coordinate system.

[0114] Let the product of the camera intrinsic parameter matrix containing fu, fv, u0 and v0 and the RT matrix be expressed as:

[0115]

[0116] In the above formula (5), h8=1.0, and the remaining elements in the H matrix are the unknown quantities to be solved. h8 can be set to any number without affecting the formula relationship. It is set to 1.0 only for the convenience of calculation. If it is set to other numbers, the solution result is just scaled according to h8.

[0117] Substituting the above formula (5) into formula (4) and rearranging it, we can get:

[0118]

[0119] The above formula (6) is a control point in the drone target coordinate system. The present invention requires at least 4 control points (four marker point target circles 101) to complete the solution process, specifically:

[0120] Assuming that the serial numbers of the n marker target circles 101 are 1, 2, 3 ... n, n ≥ 4, the above formula (6) can be expanded to:

[0121]

[0122] make:

[0123]

[0124] Then the above formula (7) can be written as:

[0125] B·L=C (8);

[0126] Solving the least squares linear equations yields:

[0127] L=(B T B) -1 ·B T ·C (9);

[0128] At this point, all elements of the H matrix have been solved (i.e., L), where the least squares linear equation system is a well-known technique in the field of mathematics, (X1, Y1)…(X n , Y n ) are the coordinate values ​​of each control point (marker point target circle 101) in the drone target coordinate system, (u1, v1)…(u n , v n ) is the imaging coordinate value of each control point in the visual camera coordinate system. The above values ​​can be obtained through the visual camera system.

[0129] The camera intrinsic parameter matrix on the right side of the above equation (4) is moved to the left side of the equal sign to obtain the following equation (10). Since h8=1.0 is set in the process of solving the H matrix, there will be a difference in the proportional coefficient s at both ends of the following equation (10):

[0130]

[0131] Both ends of the above formula (10) are 3×3 matrices, and the elements correspond one to one. The solution is:

[0132]

[0133] r2=r0×r1

[0134]

[0135] The value of the proportional coefficient s can be calculated by using r0 and r1 to satisfy the constraint that the vector modulus is 1.0, as follows:

[0136] make:

[0137]

[0138] Then we have:

[0139] ||r0||=||sA -1 H0||=1

[0140] ||r1||=||sA -1 H1||=1;

[0141] In the above formula, H0 is the matrix composed of h0, h3, and h6, and H1 is the matrix composed of h1, h4, and h7, and then we get:

[0142]

[0143] Then, according to the above formula (11), the translation amount t and the value of the third column vector r2 of the rotation matrix can be further obtained. r0, r1, r2, t, that is, RT.

[0144] Step 4: Convert the problem into a nonlinear optimization problem, and use the LM algorithm to optimize the optimal iteration step size h to improve the calculation accuracy.

[0145] Step 4.1: First, transform the problem into a nonlinear optimization problem, specifically:

[0146] The general form of the nonlinear least squares problem is shown in equation (12):

[0147]

[0148] x * =argmin x {F(x)} (12);

[0149] In the above formula (12), f: R n →R m , m>n, n represents the number of independent variables, m represents the number of equations, x * Represents the optimal solution.

[0150] The present invention uses the control point (marker point target circle 101) reprojection error as f(x), as shown in the following formula (13):

[0151]

[0152] In the above formula (13), (u′ i ,v′ i ) represents the imaging coordinates of the ith control point calculated according to the updated value of the current optimization variable in each iteration, which can be calculated by the above equations (2) and (3). i ,v i ) represents the actual imaging coordinates of the ith control point, and m represents the number of control points.

[0153] Step 4.2: Use the LM algorithm to calculate the optimal iteration step size h to improve the calculation accuracy.

[0154] The optimization solution in this step is mainly to determine the update step size (including direction and size) of the optimization variable in each iteration process, specifically:

[0155] Taylor expansion of f in equation (12) yields:

[0156] f(x+h)=f(x)+J(x)h+O(‖h‖2 ) (14);

[0157] In the above formula (14), h represents the step size, O(‖h‖ 2 ) is the Taylor expansion term, J∈R m×n is the Jacobian matrix, that is:

[0158]

[0159] make Where “≡” means constant equality, then:

[0160]

[0161] In the above formula (15), f = f(x), J = J(x), and it can be seen from the above formula (15) that the process of determining the iteration step size h is transformed into the process of searching for the minimum value of L(h):

[0162] L′(h)=J T f+J T Jh, L″(h)=J T J;

[0163] When L(h) reaches an extreme value, L′(h)=0, and the optimal step length h is obtained.

[0164] In addition, the LM optimization algorithm of the present invention introduces a damping term in the process of solving the step length h, as shown in the following equation (16), the purpose of which is to limit the amplitude of h:

[0165]

[0166] In the above formula (16), h lm It represents the step size after LM algorithm optimization, argmin represents the parameter value that makes the function value reach the minimum or minimum, and μ represents the damping coefficient.

[0167] Taking the derivative of equation (16), we get:

[0168]

[0169] Let the derivative of the above formula (17) be 0, we can get:

[0170] (J T J+μI)h lm =-J T f, μ ≥ 0 (18);

[0171] In the above formula (18), I represents the unit matrix;

[0172] For the above formula (18), when m>0, (J T J+μI) is a positive definite symmetric matrix, which can ensure that hlm It is moving in the direction of the objective function decreasing; when μ is very large, the following equation (19) is obtained, and the solution is obtained by the steepest descent method. When μ is very small, the following equation (20) is obtained, where h gn is the original step size, and the solution is obtained using the Gauss-Newton method.

[0173]

[0174] The steepest descent method has a fast convergence speed when the initial value deviates far from the optimal solution, but when the optimization variable is close to the optimal solution, the convergence speed will decrease a lot, and at this time, the Gauss-Newton method can obtain a good convergence effect. The LM optimization algorithm of the present invention combines the steepest descent method with the Gauss-Newton method, and takes the advantages of the two to complement each other. The steepest descent method and the Gauss-Newton method are well-known technologies in the art, and can be found in Chapter 5 of "Nonlinear Optimization Theory and Method" (4th edition), compiled by Wang Yiju and Xiu Naihua.

[0175] In addition, the Taylor second-order expansion L(h) is used in the above formula (20) to approximate F(x+h). For the same h, when the optimization variable is far away from the extreme point x in the current iteration process, * When L(h) approximates F(x+h) to a low degree, we hope that the iterative process will be closer to the steepest descent method, so we can increase the value of μ appropriately. In the current iterative process, when the optimization variable is close to the extreme point x * When L(h) approximates F(x+h) to a high degree, we hope that the iterative process will be closer to the Gauss-Newton method, so the value of μ can be appropriately reduced.

[0176] The following formula (21) is used in the LM algorithm of the present invention to describe the approximation degree of ΔF and ΔL, where ρ represents the gain ratio.

[0177]

[0178] △F=F(x)-F(x+h)

[0179] ΔL=L(0)-L(h) (21);

[0180] Step 5: When the movement frequency of the drone platform 3 reaches the set frequency, the drone 1 lands on the drone platform 3, and then the visual camera 4 collects images of the target circle 101 on the drone 1, and solves the drone posture data through the above steps to guide the grasping mechanism 2 to complete the grasping. Specifically:

[0181] The visual camera 4 acquires the image of the target circle 101 on the drone 1 and obtains the coordinate value (X, Y, Z) of the target circle 101 in the drone's own coordinate system and the imaging coordinate (u, v) in the visual camera coordinate system through the software system. Then, the H matrix is ​​obtained according to the above formula (9), and the RT is obtained according to the above formula (11). Finally, the imaging coordinate (u, v) in the visual camera coordinate system is converted into the coordinate in the measurement reference coordinate system through RT and sent to the equipment control system for controlling the grasping mechanism 2 to move and grasp the drone 1.

[0182] In addition, the present invention uses the reprojection error of the control point (marker point target circle 101) as f(x) to transform the problem into a nonlinear optimization problem, and uses the LM algorithm to calculate the optimal iteration step size h to improve the calculation accuracy, such as Figure 6 As shown, the accuracy of the calibration method of the present invention is significantly better than the Zhang Zhengyou calibration method commonly used in the prior art.

Claims

1. A method for grabbing and positioning a drone on an offshore mobile platform, characterized in that: The steps include: Step 1: Establish the measurement reference coordinate system, the drone target coordinate system and the visual camera coordinate system; Step 2: Establish an imaging model and RT expression between a control point (X, Y, Z) in the drone target coordinate system and the imaging coordinates (u, v) in the visual camera coordinate system, where R is the rotation matrix of the measurement reference coordinate system and the visual camera coordinate system, and T is the translation matrix of the measurement reference coordinate system and the visual camera coordinate system. Step 3: A plurality of target circles of marker points are set on the drone, and n coplanar and non-colinear target circles of marker points are selected as control points and RT is solved in combination with the forming model, where n≥4; Step 4: Convert the problem into a nonlinear optimization problem, and use the LM algorithm to optimize the optimal iteration step h to improve the calculation accuracy; Step 5: When the movement frequency of the drone platform reaches the set frequency, the drone lands on the drone platform, and then the visual camera collects images of the target circle of the marker points on the drone, and solves the drone posture data through the above steps to guide the grasping mechanism to complete the grasping.

2. The method for grasping and positioning a UAV on a mobile offshore platform according to claim 1, characterized in that: In step 2, the forming model is shown in the following formula (1): In the above formula (1), fu and fv are the focal ratios of the visual camera, u0 and v0 are the principal point coordinates of the visual camera, fu, fv, u0 and v0 constitute the camera intrinsic parameter matrix, and s is the scale coefficient; RT is shown in the following formula (2): In the above formula (2), R includes r ij , i=0, 1, 2, j=0, 1, 2, T includes tx, ty, tz.

3. The method for grasping and positioning a UAV on a mobile offshore platform according to claim 2, characterized in that: In step 3, for the above equation (1), let the Z-direction component of the spatial coordinate on the right side of the equation be 0, and we get: In the above formula (4), t0, t1, and t2 constitute the translation matrix T; Let the product of the camera intrinsic parameter matrix containing fu, fv, u0 and v0 and the RT matrix be expressed as: In the above formula (5), h8 = 1.0, and the remaining elements in the H matrix are the unknown quantities to be solved; Substituting the above formula (5) into formula (4) and rearranging it, we get: The above formula (6) is a control point in the UAV target coordinate system, and at least four control points are required to complete the solution process: Assuming that the sequence numbers of the n control points are 1, 2, 3…n, n ≥ 4, the above formula (6) is expanded to: make: Then the above formula (7) can be written as: B·L=C (8); According to the above formula (8), we can get: L=(B T ·B) -1 ·B T ·C (9); So far, all the elements of the H matrix have been solved, including (X1, Y1)…(X n , Y n ) are the coordinate values ​​of each control point in the drone target coordinate system, (u1, v1)…(u n , v n ) is the imaging coordinate value of each control point in the visual camera coordinate; Move the camera intrinsic parameter matrix on the right side of equation (5) to the left side of the equal sign to obtain the following equation (10): Both ends of the above formula (10) are 3×3 matrices, and the elements correspond one to one. The solution is: r2＝r0×r1 In the above formula (11), r0 and r1 are used to satisfy the constraint condition that the vector modulus is 1.0 to calculate the value of the proportional coefficient s, and then r0, r1, r2, t are obtained, that is, RT in the above formula (2).

4. The method for grasping and positioning a UAV on a mobile offshore platform according to claim 3, characterized in that: In step 3, the proportionality coefficient s is solved as follows: make: Then we have: r0=sA -1 H0 r1=sA -1 H1; In the above formula, H0 is the matrix composed of h0, h3, and h6, H1 is the matrix composed of h1, h4, and h7, and r0 and r1 satisfy the constraint that the vector modulus is 1.0, then: ||r0||=||||sA -1 H0||=1 ||r1||=||sA -1 H1||=1; Then we get:

5. The method for grasping and positioning a UAV on an offshore mobile platform according to claim 1, characterized in that: In step 4, the problem is transformed into a nonlinear optimization problem: The general form of the nonlinear least squares problem is shown in equation (12): x * =argmin x {F(x)} (12); In the above formula (12), f: R n →R m , m>n, n represents the number of independent variables, m represents the number of equations, x * represents the optimal solution; The control point reprojection error is used as f(x), as shown in equation (13): In the above formula (13), (u′ i ,v′ i ) represents the imaging coordinates of the ith control point calculated according to the updated value of the current optimization variable in each iteration, (u i ,v i ) represents the actual imaging coordinates of the ith control point, and m represents the number of control points.

6. The method for grasping and positioning a UAV on a mobile offshore platform according to claim 5, characterized in that: The optimal iteration step length h in step 4 is: Taylor expansion of f in the above formula (12) yields: f(x+h)=f(x)+J(x)h+O(||h|| 2 ) (14); In the above formula (14), h represents the step size, O(||h|| 2 ) is the Taylor expansion term, J∈R m×n is the Jacobian matrix; make Where "≡" means constant equality, then: In the above formula (15), f = f(x), J = J(x), and it can be seen from the above formula (15) that the process of determining the iteration step size h is transformed into the process of searching for the minimum value of L(h), where: L′(h)=J T f+J T Jh; When L(h) reaches an extreme value, L′(h)=0, and the optimal step length h is obtained.

7. The method for grasping and positioning a UAV on an offshore mobile platform according to claim 6, characterized in that: In step 4, a damping term is introduced to limit the amplitude of h during the solution of the step length h: In the above formula (16), h lm represents the step size after LM algorithm optimization, argmin represents the parameter value that makes the function value reach the minimum or minimum, and μ represents the damping coefficient; Taking the derivative of equation (16), we get: Let the derivative of the above formula (17) be 0 and we get: (J T J+μI)h lm =-J T f,μ≥0 (18); In the above formula (18), I is the unit matrix, and then the steepest descent method or Gauss-Newton method is used to solve the problem according to the value of μ.

Images