Unmanned aerial vehicle air docking system and method based on magnetic vector field

Abstract

The invention provides an unmanned aerial vehicle air docking method based on a magnetic vector field. The method comprises the following steps: establishing a kinetic model of an unmanned aerial vehicle; measuring and fitting by using a magnetic vector field measurement simulation device to obtain a magnetic vector field mathematical model; designing a docking process; calculating new expected moment and force according to the initial expected force and moment and a magnetic vector field mathematical model; performing compensation processing on the expected difference by using QP compensation to obtain a final expected pulling force of the unmanned aerial vehicle; and according to the final expected pulling force of the unmanned aerial vehicle, a motor of the unmanned aerial vehicle is controlled through the accelerator and thrust relation identification model. According to the invention, high-precision butt joint and long-time air stay can be realized without adopting a high-precision sensor or a complex mechanical structure; according to the method, the fitted magnetic vector field mathematical model is deployed into the unmanned aerial vehicle docking system, so that docking of the unmanned aerial vehicle is realized through electromagnetic force; and related factors brought by introduction of a magnetic vector field can be effectively fused, so that a complete docking system has higher robustness.

Description

Technical Field

[0001] The invention relates to the technical field of unmanned aerial vehicles, and in particular to a system and method for unmanned aerial vehicle aerial docking based on a magnetic vector field. Background Art

[0002] With the rapid development of drone technology, drones are being used more and more widely in civil and military fields, and the requirements for their ability to perform tasks are also increasing. At the same time, with the application of lightweight, low-power sensor technology and onboard computers, drones can obtain higher computing power, perform increasingly complex tasks, and implement more powerful control methods.

[0003] Currently, drones can also reliably complete tasks such as aerial photography, autonomous surveillance, object delivery, and construction site inspection. When a drone lands or stays in a certain place, in order to improve its positioning accuracy, it is necessary to use high-level sensors or combined sensors to obtain the precise location, and these sensors are extremely expensive. Summary of the invention

[0004] In view of the deficiencies in the prior art, the present invention provides a UAV aerial docking system and method based on a magnetic vector field, which can achieve unmanned high-precision docking and long-term aerial stay.

[0005] The technical solution of the present invention is: a method for aerial docking of a UAV based on a magnetic vector field, comprising the following steps:

[0006] S1), establish the dynamic model of the UAV;

[0007] S2), using a magnetic vector field measurement simulation device to measure and fit a magnetic vector field mathematical model;

[0008] S3), design the docking process and use the Kalman filter algorithm to fuse the position data during the aerial docking of the UAV;

[0009] S4), calculating new desired torque and force based on the initial desired force and torque and the magnetic vector field mathematical model;

[0010] S5), using QP compensation to compensate for the expected difference, and obtain the final expected pulling force of the UAV;

[0011] S6), the actuator controls the UAV motor according to the final expected pulling force of the UAV through the throttle and thrust relationship identification model.

[0012] Preferably, in step S1), the expression of the dynamic model of the UAV is:

[0013]

[0014] Where P = (x, y, z) is the position of the aircraft in the world coordinate system; R is the attitude of the body coordinate system relative to the world coordinate system. The following expression: v = [v x ,v y ,v z ] T is the linear velocity of the aircraft in the world coordinate system; m is the mass of the drone; g is the gravitational acceleration in the world coordinate system; F B and M B They represent the force and torque generated by the actuator in the body coordinate system respectively; T em and M em They represent the electromagnetic force and torque generated by the electromagnetic system in the body coordinate system respectively; I represents the inertia matrix; w = [w x ,w y ,w z ] T is the angular velocity of the aircraft in the body coordinate system; For position P Derivation; is an antisymmetric matrix; To find the derivative of the attitude angle R; is the diagonal velocity w Derivation; Line speed v Derivative; T represents transpose.

[0015] Preferably, in step S3), the Kalman filter algorithm is used to fuse the position data of the aerial docking point of the UAV, which specifically includes the following steps:

[0016] S31), setting the permanent magnetic device at the docking point; and assuming that the drone starts from the take-off point A and approaches the docking point, when the D435 camera of the drone detects the QR code target at the docking point for the first time, the drone immediately stops moving and sets the position to position B;

[0017] S32), maintain the B position state, use the drone's D435 camera to continuously measure the QR code target for a period of time, and establish an intermediate coordinate system: m ,x m ,x y ,z m};

[0018] S33), taking the average of the data continuously measured in step S32), and obtaining the real distance (L, W, H) between the drone and the docking point in the B position state;

[0019] S34) During the docking process, the Kalman filter algorithm is used to locate the Perform location data fusion.

[0020] Preferably, in step S34), the expression for performing position data fusion on position C using the Kalman filter algorithm is:

[0021]

[0022]

[0023]

[0024] In the formula, Indicates that the position data of the target is obtained from T265; Indicates the coordinates of the drone fed back by the T265 camera in the intermediate coordinate system; Indicates the target position data measured by D435; Represents the transformation matrix from the image coordinate system to the intermediate coordinate system; (x D435 ,y D435 ,z D435 ) represents the target position in the D435 camera coordinate system; P target represents the position of the target after fusion; Kk represents the Kalman gain.

[0025] Preferably, in step S4), the new expected torque and force are calculated according to the initial expected force and torque and the magnetic vector field mathematical model, which specifically includes the following steps:

[0026] S41), before entering the control distribution layer, the initial expected force F given by the upper controller B and moment M B Decompose into three axes xyz and according to the control efficiency matrix A 6×4 Calculate the pulling force F of the motor i (i=1,2,3,4); that is:

[0027]

[0028] In the formula, [F1, F2, F3, F4] represent the pulling force of the four motors of the drone respectively; They represent the initial expected force F B and moment M B Components on the three axes xyz;

[0029] S42), consider the pulling force F of the four motors of the drone by electromagnetic force i ′(i=1,2,3,4) to obtain the new desired torque and force Right now:

[0030]

[0031] In the formula, [M emx ,M emy ,M emz ]、[T emx ,T emy ,T emz ] are torque T em and electromagnetic force M em Components on three axes; [F1′, F2′, F3′, F4′] are the pulling forces of the four motors obtained after limiting processing;

[0032] S43), since the electromagnetic device is installed at a distance h from the center of mass of the drone and passes through the axis, the expression of the electromagnetic torque is obtained as:

[0033]

[0034] Preferably, in step S5), the expected difference is compensated by using a QP compensator to obtain the final expected pulling force of the drone, which specifically includes the following steps:

[0035] S51), the new expected torque and force Subtract the initial desired force F given by the upper controller B and moment M B ; Get the expected difference ΔT:

[0036]

[0037] S52), since the expected difference ΔT satisfies the control efficiency model, the expression of the equality constraint in the QP problem is obtained as follows:

[0038]

[0039] In the formula, ΔF represents the expected difference in tension;

[0040] S53), by introducing the slack variable s to relax the constraints in step S52); obtain:

[0041] Δτ=A 6×4 ΔF+s;

[0042] In the formula, Δτ represents the equality constraint that introduces the slack variable s;

[0043] S54), restrict ΔF and obtain the inequality constraint:

[0044] F min -F′≤ΔF≤F max -F′;

[0045] In the formula, F min 、F maxThey represent the minimum and maximum pulling forces provided by the set actuator respectively;

[0046] S55), construct an objective function so that the actuator does not reach saturation; that is:

[0047]

[0048] Where U and V are weight matrices;

[0049] S56), output expected pulling force F exp for:

[0050] F exp =F′+ΔF;

[0051]

[0052] The motor is controlled through the throttle and thrust relationship identification model.

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

[0054] 1. The present invention can achieve high-precision docking and long-term air stay without the use of high-precision sensors or complex mechanical structures;

[0055] 2. The present invention deploys the fitted magnetic vector field mathematical model into the UAV docking system, thereby achieving the docking of the UAV through magnetic force; and the present invention can effectively integrate the relevant factors brought by the introduction of the magnetic vector field, so that the complete docking system has higher robustness. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 It is a flow chart of the method of the present invention;

[0057] Figure 2 It is a schematic diagram of the process of docking of the method of the present invention;

[0058] Figure 3 It is a schematic diagram of the structure of the UAV of the present invention;

[0059] Figure 4 It is a structural schematic diagram of the magnetic vector field measurement simulation device of the present invention;

[0060] Figure 5 It is a structural schematic diagram of the permanent magnet device of the present invention;

[0061] Figure 6 It is a schematic diagram of the structure of the electromagnetic device of the present invention;

[0062] In the figure, 11-base; 12-first compression and tension sensor; 13-second compression and tension sensor; 14-third compression and tension sensor; 15-fixed end; 16-movable end; 17-permanent magnetic device; 18-electromagnetic device; 19-lateral moving mechanism; 20-longitudinal moving mechanism;

[0063] 171-permanent magnet housing; 172-permanent magnet;

[0064] 181 - electromagnetic housing; 182 - electromagnet. DETAILED DESCRIPTION

[0065] The specific implementation of the present invention will be further described below in conjunction with the accompanying drawings:

[0066] Example 1

[0067] like Figure 1 As shown, this embodiment provides a method for aerial docking of a UAV based on a magnetic vector field, comprising the following steps:

[0068] S1), establish the dynamic model of the UAV;

[0069] The expression of the dynamic model of the UAV constructed in this embodiment is:

[0070]

[0071] Where P = (x, y, z) is the position of the aircraft in the world coordinate system; R is the attitude of the body coordinate system relative to the world coordinate system. The following expression: v = [v x ,v y ,v z ] T is the linear velocity of the aircraft in the world coordinate system; m is the mass of the drone; g is the gravitational acceleration in the world coordinate system; F B and M B They represent the force and torque generated by the actuator in the body coordinate system respectively; T em and M em They represent the electromagnetic force and torque generated by the electromagnetic system in the body coordinate system respectively; I represents the inertia matrix; w = [w x ,w y ,w z ] T is the angular velocity of the aircraft in the body coordinate system; For position P Derivation; is an antisymmetric matrix; To find the derivative of the attitude angle R; is the diagonal velocity w Derivation; Line speedv Derivative; T represents transpose.

[0072] S2), using a magnetic vector field measurement simulation device to measure, fit and construct a magnetic vector field mathematical model;

[0073] In this embodiment, Figure 4 As shown, the magnetic vector field measurement simulation device includes a base 11, and a first compression and tension sensor 12, a second compression and tension sensor 13, a third compression and tension sensor 14, and a fixed end 15 and a movable end 16 arranged on the base 11; the fixed end 15 is provided with a permanent magnetic device 17, the movable end 16 is provided with an electromagnetic device 18, and the movable end 16 is driven by a transverse moving mechanism 19 and a longitudinal moving mechanism 20, and the longitudinal moving mechanism 20 is arranged on the transverse moving mechanism 19.

[0074] In this embodiment, the first compression and tension force sensor 12, the second compression and tension force sensor 13, and the third compression and tension force sensor 14 are respectively arranged at the lower end, the rear end, and one side of the fixed end 15, and corresponding force transmission parts are arranged between the first compression and tension force sensor 12, the second compression and tension force sensor 13, and the third compression and tension force sensor 14 and the fixed end 15.

[0075] This embodiment adjusts the distance between the electromagnetic device 18 and the permanent magnetic device 17 by controlling the lateral moving mechanism 19 and the longitudinal moving mechanism 20, thereby simulating the distance between the electromagnetic device 18 of the drone and the permanent magnetic device 17 of the docking point. This embodiment realizes the measurement of forces of different axes through the deformation of the first compression and tension sensor 12, the second compression and tension sensor 13, and the third compression and tension sensor 14. This embodiment obtains the mathematical model of the magnetic vector field based on data fitting; and then integrates the mathematical model of the magnetic vector field into the drone docking system. The expression of the mathematical model of the magnetic vector field of this embodiment is:

[0076]

[0077] In the formula, p1~p 18 They respectively represent the parameters after least square fitting; x, y, z respectively represent the distance between the permanent magnet device and the electromagnet device on the x, y, z axis.

[0078] In this embodiment, Figure 5 As shown, the permanent magnet device 17 includes a permanent magnet housing 171 and a permanent magnet 172 disposed in the permanent magnet housing 171; the permanent magnet 172 is a N35 type permanent magnet.

[0079] like Figure 6As shown, the electromagnet device 18 includes an electromagnet housing 181 and an electromagnet 182 disposed in the electromagnet housing 181. The electromagnet 182 is composed of an iron core and a coil, and is connected to a wireless switch module to control the current.

[0080] In the actual docking process, Figure 2 and 3 As shown, the UAV is provided with a corresponding electromagnet device 18, and a corresponding permanent magnet device 17 is provided at the docking point.

[0081] In this embodiment, the lateral moving mechanism 19 includes a lateral fixing frame and a lateral adjusting screw arranged on the lateral fixing frame, a lateral sliding block is arranged on the lateral adjusting screw, and a hand crank handle is also arranged at the end of the lateral adjusting screw.

[0082] The structure of the longitudinal moving mechanism 20 is the same as that of the transverse moving mechanism 19 , and the longitudinal fixed frame of the longitudinal moving mechanism 20 is arranged on the transverse slider of the transverse moving mechanism 19 , and the moving end 16 is arranged on the longitudinal slider of the longitudinal moving mechanism 20 .

[0083] S3), design the docking process and use the Kalman filter algorithm to fuse the position data during the aerial docking of the UAV; Figure 2 As shown, the specific steps include:

[0084] S31), the permanent magnetic device 17 is set at the docking point; and assuming that the drone starts from the take-off point A and approaches the docking point, when the D435 camera of the drone detects the QR code target at the docking point for the first time, the drone immediately stops moving and sets the position to position B;

[0085] S32), maintain the B position state, use the drone's D435 camera to continuously measure the QR code target for a period of time, and establish an intermediate coordinate system: m ,x m ,x y ,z m};

[0086] S33), taking the average of the data continuously measured in step S32), and obtaining the real distance (L, W, H) between the drone and the docking point in the B position state;

[0087] S34) During the docking process, the Kalman filter algorithm is used to locate the Perform position data fusion, the expression of position data fusion is:

[0088]

[0089]

[0090]

[0091] In the formula, Indicates that the position data of the target is obtained from T265; Indicates the coordinates of the drone fed back by the T265 camera in the intermediate coordinate system; Indicates the target position data measured by D435; Represents the transformation matrix from the image coordinate system to the intermediate coordinate system; (x D435 ,y D435 ,z D435 ) represents the target position in the D435 camera coordinate system; P target Indicates the distance from the target after fusion; K k represents the Kalman gain.

[0092] S4), calculating new expected torque and force according to the initial expected force and torque and the magnetic vector field mathematical model; specifically comprising the following steps:

[0093] S41), before entering the control distribution layer, the initial expected force F given by the upper controller B and moment M B Decomposed into three axes xyz, the upper controller in this embodiment includes a position controller and a posture controller; and according to the control efficiency matrix A 6×4 Calculate the pulling force F of the motor i (i=1,2,3,4); that is:

[0094]

[0095] In the formula, [F1, F2, F3, F4] represent the pulling force of the four motors of the drone respectively; They represent the initial expected force F B and moment M B Components on the three axes xyz;

[0096] S42), consider the pulling force F of the electromagnetic force on the four unmanned motors i (i=1,2,3,4) to obtain the new desired torque and force Right now:

[0097]

[0098] In the formula, [M emx ,M emy ,M emz ]、[T emx ,T emy ,T emz ] are torque T emand electromagnetic force M em Components on the three axes xyz; [F1′, F2′, F3′, F4′] represent the tension of the four motors after limiting processing;

[0099] S43), since the electromagnetic device 18 is installed at a distance h from the center of mass of the drone and passes through the axis, the electromagnetic torque is expressed as:

[0100]

[0101] S5), using QP compensation to compensate for the expected difference, to obtain the final expected pulling force of the drone; specifically including the following steps:

[0102] S51), the new expected torque and force Subtract the initial desired force F given by the upper controller B and moment M B ; Get the expected difference ΔT:

[0103]

[0104] S52), since the expected difference ΔT satisfies the control efficiency model, the expression of the equality constraint in the QP problem is obtained as follows:

[0105]

[0106] In the formula, ΔF represents the expected difference in tension;

[0107] S53), by introducing the slack variable s to relax the constraints in step S52); obtain:

[0108] Δτ=A 6×4 ΔF+s;

[0109] In the formula, Δτ represents the equality constraint that introduces the slack variable s;

[0110] S54), restrict ΔF and obtain the inequality constraint:

[0111] F min -F′≤ΔF≤F max -F′;

[0112] In the formula, F min 、F max They represent the minimum and maximum pulling forces provided by the set actuator respectively; F′ is the motor pulling force obtained after limiting processing;

[0113] S55), construct an objective function so that the actuator does not reach saturation; that is:

[0114]

[0115] Where U and V are weight matrices;

[0116] S56), output the final expected pulling force F of the drone exp for:

[0117] F exp =F′+ΔF;

[0118]

[0119] S6), the actuator is based on the final expected pulling force F of the drone exp ,The UAV motor is controlled through the throttle and thrust relationship identification model.

[0120] Example 2

[0121] like Figure 1 As shown, this embodiment provides a UAV aerial docking system based on magnetic vector field, including a main controller, an onboard computer, and an actuator; the main controller includes a position controller, an attitude controller, and a control distributor; the onboard computer includes a fusion unit, a desired torque and force calculation unit, and a compensation unit;

[0122] The position controller works closely with the attitude controller to ensure that the drone remains stable when executing position change commands.

[0123] The position controller is used to adjust the speed and direction to achieve the desired position;

[0124] The attitude controller is used to adjust the tilt angle (pitch, roll and yaw) of the drone;

[0125] The control distributor is used to ensure that the flight control commands from the upper layer are correctly and effectively converted into specific control inputs for each controllable actuator of the UAV;

[0126] The fusion unit is used to fuse the position data of the cameras D435 and T265 by using a Kalman filter algorithm;

[0127] The desired moment and force calculation unit is used to calculate the initial desired force F of the drone. B and moment M B , and the electromagnetic force on the four motors of the drone to calculate the new expected torque and force of the drone

[0128] The compensation unit is used to compensate the expected difference by using QP compensation to obtain the final expected pulling force of the UAV, and control the UAV by the final expected pulling force of the UAV;

[0129] The actuator is used to control the UAV motor according to the final expected pulling force of the UAV through the throttle and thrust relationship identification model.

[0130] The above embodiments and descriptions are only for illustrating the principles and best embodiments of the present invention. Without departing from the spirit and scope of the present invention, the present invention may be subject to various changes and improvements, all of which fall within the scope of the present invention to be protected.

Claims

1. A method for aerial docking of unmanned aerial vehicles based on magnetic vector field, characterized in that: The following steps are involved: S1), establish the dynamic model of the UAV; S2), using a magnetic vector field measurement simulation device to measure and fit a magnetic vector field mathematical model; S3), design the docking process and use the Kalman filter algorithm to fuse the position data during the aerial docking of the UAV; S4), calculating new desired torque and force based on the initial desired force and torque and the magnetic vector field mathematical model; S5), using QP compensation to compensate for the expected difference, and obtain the final expected pulling force of the UAV; S6), the actuator controls the UAV motor according to the final expected pulling force of the UAV through the throttle and thrust relationship identification model.

2. The method for aerial docking of unmanned aerial vehicles based on magnetic vector field according to claim 1, characterized in that: In step S1), the expression of the dynamic model of the drone is: Where P = (x, y, z) is the position of the aircraft in the world coordinate system; R is the attitude of the body coordinate system relative to the world coordinate system. The following expression: v = [v x ,v y ,v z ] T is the linear velocity of the aircraft in the world coordinate system; m is the mass of the drone; g is the gravitational acceleration in the world coordinate system; F B and M B They represent the force and torque generated by the actuator in the body coordinate system respectively; T em and M em They represent the electromagnetic force and torque generated by the electromagnetic system in the body coordinate system respectively; I represents the inertia matrix; w = [w x ,w y ,w z ] T is the angular velocity of the aircraft in the body coordinate system; For position P Derivation; is an antisymmetric matrix; To find the derivative of the attitude angle R; is the diagonal velocity w Derivation; Line speed v Derivative; T represents transpose.

3. The method for aerial docking of unmanned aerial vehicles based on magnetic vector field according to claim 1, characterized in that: In step S2), the expression of the magnetic vector field mathematical model is: In the formula, p1~p 18 They respectively represent the parameters after least square fitting; x, y, z respectively represent the distance between the permanent magnet device and the electromagnet device on the x, y, z axis.

4. The method for aerial docking of a UAV based on a magnetic vector field according to claim 1, characterized in that: In step S3), the Kalman filter algorithm is used to fuse the position data of the aerial docking point of the UAV, which specifically includes the following steps: S31), setting the permanent magnetic device at the docking point; and assuming that the drone starts from the take-off point A and approaches the docking point, when the D435 camera of the drone detects the QR code target at the docking point for the first time, the drone immediately stops moving and sets the position to position B; S32), maintain the B position state, use the drone's D435 camera to continuously measure the QR code target for a period of time, and establish an intermediate coordinate system: m ,x m ,x y ,z m }; S33), taking the average of the data continuously measured in step S32), and obtaining the real distance (L, W, H) between the drone and the docking point in the B position state; S34) During the docking process, the Kalman filter algorithm is used to locate the Perform location data fusion.

5. The method for aerial docking of unmanned aerial vehicles based on magnetic vector field according to claim 4, characterized in that: In step S34), the expression for position data fusion of position C using Kalman filter algorithm is: In the formula, Indicates the position data of the target obtained from the T265 camera; Indicates the coordinates of the drone fed back by the T265 camera in the intermediate coordinate system; Indicates the target position data measured by D435; Represents the transformation matrix from the image coordinate system to the intermediate coordinate system; (x D435 ,y D435 ,z D435 ) represents the target position in the D435 camera coordinate system; P target Indicates the distance from the target after fusion; K k represents the Kalman gain.

6. The method for aerial docking of unmanned aerial vehicles based on magnetic vector field according to claim 1, characterized in that: In step S4), before entering the control allocation layer, the initial desired force F given by the upper controller is B and moment M B Decompose into three axes xyz and according to the control efficiency matrix A 6×4 Calculate the pulling force F of the four motors without the influence of electromagnetic force i (i=1,2,3,4); that is: In the formula, [F1, F2, F3, F4] represent the pulling force of the four motors of the drone, They represent the initial expected force F B and moment M B Components on the three axes xyz.

7. The method for aerial docking of unmanned aerial vehicles based on magnetic vector field according to claim 6, characterized in that: In step S4), consider the pulling force F of the four motors of the drone i (i=1,2,3,4) to obtain the new desired torque and force Right now: In the formula, [M emx ,M emy ,M emz ]、[T emx ,T emy ,T emz ] are torque T em and electromagnetic force M em The components on the three axes; [F1′, F′2, F′3, F′4] are the four motor pulling forces obtained after limiting processing.

8. The method for aerial docking of unmanned aerial vehicles based on magnetic vector field according to claim 7, characterized in that: In step S4), since the electromagnetic device is installed at a distance h from the center of mass of the drone, and the Z B axis, therefore, the expression of electromagnetic torque is:

9. The method for aerial docking of unmanned aerial vehicles based on magnetic vector field according to claim 8, characterized in that: In step S5), the new desired torque and force Subtract the initial desired force F given by the upper controller B and moment M B ; Get the expected difference ΔT:

10. The method for aerial docking of unmanned aerial vehicles based on magnetic vector field according to claim 9, characterized in that: In step S5), the expected difference is compensated by using QP compensation to obtain the final expected pulling force of the drone, which specifically includes the following steps: S51), since the expected difference ΔT satisfies the control efficiency model, the expression of the equality constraint in the QP problem is obtained as follows: In the formula, ΔF represents the expected difference in tension; S52), by introducing the slack variable s to relax the constraints in step S51); obtain: Δτ=A 6×4 ΔF+s; In the formula, Δτ represents the equality constraint that introduces the slack variable s; S53), restrict ΔF and obtain the inequality constraint: F min -F′≤ΔF≤F max -F′; In the formula, F min 、F max They represent the minimum and maximum pulling forces provided by the set actuator respectively; F′ is the motor pulling force obtained after limiting processing; S54), construct the objective function, that is: Where U and V are weight matrices; S55), output the final expected pulling force F of the four motors exp for: F exp =F′+ΔF;

Images