An aerial docking system and method for unmanned aerial vehicles based on magnetic vector fields

Abstract

This invention provides an aerial docking method for unmanned aerial vehicles (UAVs) based on magnetic vector fields. The method includes establishing a dynamic model of the UAV; measuring and fitting a mathematical model of the magnetic vector field using a magnetic vector field measurement and simulation device; designing a docking process; calculating new expected torque and force based on the initial expected force and torque and the magnetic vector field mathematical model; compensating for the expected difference using QP compensation to obtain the final expected thrust of the UAV; and controlling the UAV motors based on the final expected thrust of the UAV using a throttle-thrust relationship identification model. This invention achieves high-precision docking and long-term aerial loiter without the need for high-precision sensors or complex mechanical structures. By deploying the fitted magnetic vector field mathematical model into the UAV docking system, this invention achieves UAV docking through electromagnetic force. Furthermore, it effectively integrates the relevant factors introduced by the magnetic vector field, resulting in a more robust docking system.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned aerial vehicles, and particularly relates to an unmanned aerial vehicle air docking system and method based on a magnetic vector field. BACKGROUND

[0002] With the rapid development of unmanned aerial vehicle technology, unmanned aerial vehicles are increasingly widely applied in civilian and military fields, and the ability requirements for their task execution are also improved, and with the application of lightweight, low-power sensor technology and on-board computers, unmanned aerial vehicles can obtain higher computing power, perform more and more complex tasks, and also can realize more powerful control methods.

[0003] At present, unmanned aerial vehicles can reliably complete tasks such as aerial video shooting, autonomous monitoring, object delivery and construction site inspection. When the unmanned aerial vehicle lands or stays in a certain place, in order to improve the accuracy of its positioning, high-level sensors or combined sensors are needed to obtain accurate positions, and these sensors are extremely expensive. SUMMARY

[0004] In view of the deficiencies of the prior art, the present application provides an unmanned aerial vehicle air docking system and method based on a magnetic vector field, which can realize high-precision docking and long-time aerial stay of unmanned aerial vehicles.

[0005] The technical scheme of the present application is as follows: an unmanned aerial vehicle air docking method based on a magnetic vector field, comprising the following steps:

[0006] S1), establishing a dynamic model of the unmanned aerial vehicle;

[0007] S2), measuring and fitting a magnetic vector field mathematical model by using a magnetic vector field measurement simulation device;

[0008] S3), designing a docking process and using a Kalman filtering algorithm to fuse position data in the air docking process of the unmanned aerial vehicle;

[0009] S4), calculating new expected moments and forces according to initial expected forces and moments and the magnetic vector field mathematical model;

[0010] S5), compensating the expected difference by using QP compensation to obtain the final expected pulling force of the unmanned aerial vehicle;

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

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

[0013] Preferably, in step S1), the expression of the dynamic model of the unmanned aerial vehicle is:

[0014] where P = (x, y, z) is the position of the aerial vehicle in the world coordinate system; R is the representation of the attitude of the body coordinate system with respect to the world coordinate system in x y z T is the linear velocity of the aerial vehicle in the world coordinate system; m is the mass of the UAV; g is the gravity acceleration in the world coordinate system; F B and M B represent the force and moment generated by the actuators in the body coordinate system; T em and M em represent the electromagnetic force and moment generated by the electromagnetic system in the body coordinate system; I represents the inertia matrix; w = [w x y z T is the angular velocity of the aerial vehicle in the body coordinate system; is the derivative of the position P is the skew-symmetric matrix; is the derivative of the attitude angle R; is the derivative of the angular velocity w is the derivative of the linear velocity v T represents the transpose processing.

[0015] As preferred, in step S3), the position data fusion of the aerial docking point of the UAV is performed by using the Kalman filtering algorithm, which specifically includes the following steps:

[0016] S31), the permanent magnet device is arranged at the docking point position; and assuming that the UAV approaches the docking point from the take-off point A, when the D435 camera of the UAV first detects the two-dimensional code target at the docking point position, the UAV immediately stops moving, and the position is set as position B;

[0017] S32), the B position state is maintained, the D435 camera of the UAV is used to continuously measure the two-dimensional code target for a period of time, and an intermediate coordinate system: {O m m y m} is established;

[0018] S33), the data continuously measured in step S32) is averaged to obtain the real distance (L, W, H) of the UAV and the docking point in the B position state;

[0019] S34), during the docking process, the Kalman filtering algorithm is used to perform position data fusion on the position .​​​​​​​​​​​​

[0020] As preferred, in step S34), the expression of position data fusion of position C using Kalman filter algorithm is:

[0021]

[0022]

[0023]

[0024] wherein, represents the position data of the target obtained from T265; represents the coordinates of the UAV fed back by the T265 camera in the intermediate coordinate system; represents the target position data measured by the D435; represents the transformation matrix from the image coordinate system to the intermediate coordinate system; D435 D435 D435 represents the target position in the D435 camera coordinate system; P target represents the position after fusion; Kk represents the Kalman gain.

[0025] As preferred, 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, specifically including the following steps:

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

[0027]

[0028] wherein, [F1, F2, F3, F4] respectively represent the tension of the four motors of the UAV; respectively represent the components of the initial expected force F B and torque M B on the three axes xyz;

[0029] S42), considering the effect of electromagnetic force on the tension F i ′(i=1, 2, 3, 4) of the four motors of the UAV, the new expected torque and force ; that is:

[0030] ​​

[0031] where [M emx ,M emy ,M emz ] and [T emx ,T emy ,T emz ] are the components of the moment T em and the electromagnetic force M em in the three axes respectively; [F1', F2', F3', F4'] are the tensions of the four motors after the amplitude limiting processing;

[0032] S43), since the electromagnetic device is installed at the position of distance h from the center of mass of the UAV and across the axis, the expression of the electromagnetic moment is obtained as:

[0033]

[0034] As preferred, in step S5), the desired difference is compensated by using the QP compensator to obtain the final desired tension of the UAV, specifically including the following steps:

[0035] S51), the new desired moment and force are subtracted from the initial desired force F B and moment M B given by the upper controller; the desired difference ΔT is obtained:

[0036]

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

[0038]

[0039] where ΔF represents the desired difference of the tension;

[0040] S53), the constraint in step S52) is relaxed by introducing the slack variable s; it is obtained as:

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

[0042] where Δτ represents the equality constraint after introducing the slack variable s;

[0043] S54), ΔF is limited to obtain the inequality constraint:

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

[0045] where F min , F maxrespectively represent the minimum and maximum pulling force provided by the set actuator;

[0046] S55), constructing a target function to make the actuator not reach the saturation state; that is:

[0047]

[0048] In the formula, U and V are weight matrices;

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

[0050] F exp =F'+AF;

[0051]

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

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

[0054] 1. The present application does not need to use high-precision sensors and does not need complex mechanical structure to realize high-precision docking and long-time air stay.

[0055] 2. The present application deploys the fitted magnetic vector field mathematical model to the unmanned aerial vehicle docking system, so as to realize the docking of the unmanned aerial vehicle through magnetic force. Moreover, the present application can effectively fuse the related factors brought by the magnetic vector field, so that the complete docking system has higher robustness. BRIEF DESCRIPTION OF DRAWINGS

[0056] Figure 1 is the flow framework diagram of the method of the present application;

[0057] Figure 2 is the flow diagram of the docking method of the present application;

[0058] Figure 3 is the structural schematic diagram of the unmanned aerial vehicle of the present application;

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

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

[0061] Figure 6 is the structural schematic diagram of the electromagnetic device of the present application;

[0062] In the diagram, 11-base; 12-first pressure sensor; 13-second pressure sensor; 14-third pressure sensor; 15-fixed end; 16-moving end; 17-permanent magnet 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 Implementation

[0065] The specific embodiments of the present invention will be further described below with reference to the accompanying drawings:

[0066] Example 1

[0067] like Figure 1 As shown, this embodiment provides an aerial docking method for unmanned aerial vehicles based on magnetic vector fields, including the following steps:

[0068] S1) Establish a dynamic model of the UAV;

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

[0070]

[0071] In the formula, P = (x, y, z) represents the position of the aircraft in the world coordinate system; R represents the attitude of the body coordinate system relative to the world coordinate system. The following is the representation; v = [v x ,v y ,v z ] T Let m be the linear velocity of the aircraft in the world coordinate system; m be the mass of the UAV; g be the gravitational acceleration in the world coordinate system; F be the linear velocity of the aircraft in the world coordinate system. B and M B T represents the force and torque generated by the actuator in the body coordinate system, respectively. em and M em These 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 This represents the angular velocity of the aircraft in the body coordinate system. For position P Differentiate; To oppose the formation; To differentiate with respect to attitude angle R; For angular velocity w Differentiate; For linear speedv Differentiate; T denotes transpose.

[0072] S2) Measure and fit the magnetic vector field mathematical model using a magnetic vector field measurement and simulation device;

[0073] In this embodiment, as Figure 4 As shown, the magnetic vector field measurement simulation device includes a base 11, a first pressure-tension sensor 12, a second pressure-tension sensor 13, a third pressure-tension sensor 14 disposed on the base 11, a fixed end 15, and a moving end 16; a permanent magnet device 17 is disposed on the fixed end 15, and an electromagnetic device 18 is disposed on the moving end 16, and the moving end 16 is driven by a horizontal moving mechanism 19 and a vertical moving mechanism 20, the vertical moving mechanism 20 being disposed on the horizontal moving mechanism 19.

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

[0075] This embodiment controls the lateral movement mechanism 19 and the longitudinal movement mechanism 20 to adjust the distance between the electromagnetic device 18 and the permanent magnet device 17, thereby simulating the distance between the electromagnetic device 18 of the UAV and the permanent magnet device 17 at the docking point. This embodiment uses the deformation of the first pressure-tension sensor 12, the second pressure-tension sensor 13, and the third pressure-tension sensor 14 to measure forces along different axes. This embodiment obtains a magnetic vector field mathematical model based on data fitting; then, the magnetic vector field mathematical model is integrated into the UAV docking system. The expression for the magnetic vector field mathematical model in this embodiment is:

[0076]

[0077] In the formula, p1~p 18 ...

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

[0079] like Figure 6As shown, the electromagnet device 18 includes an electromagnet shell 181 and an electromagnet 182 arranged in the electromagnet shell 181, the electromagnet 182 is composed of a core and a coil, and the wireless switch module can control the current.

[0080] In the actual docking process, as shown in Figure 2 and 3 The unmanned aerial vehicle is provided with a corresponding electromagnet device 18, and the docking point position is provided with a corresponding permanent magnet device 17.

[0081] In this embodiment, the transverse movement mechanism 19 includes a transverse fixed frame and a transverse adjusting screw rod arranged on the transverse fixed frame, the transverse adjusting screw rod is provided with a transverse sliding block, and the end of the transverse adjusting screw rod is further provided with a hand wheel handle.

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

[0083] S3), design the docking process and use Kalman filtering algorithm to fuse the position data in the aerial docking process of the unmanned aerial vehicle; as shown in Figure 2 , specifically comprising the following steps:

[0084] S31), the permanent magnet device 17 is arranged at the docking point position; and it is assumed that the unmanned aerial vehicle starts from the take-off point A and approaches the docking point, when the D435 camera of the unmanned aerial vehicle detects the two-dimensional code target of the parking point position for the first time, the unmanned aerial vehicle immediately stops moving, and the position is set as position B;

[0085] S32), keep the B position state, use the D435 camera of the unmanned aerial vehicle to continuously measure the two-dimensional code target for a period of time, and at the same time, establish an intermediate coordinate system: {O m ,x m ,x y ,z m};

[0086] S33), the data measured continuously in step S32) is averaged to obtain the real distance (L, W, H) between the unmanned aerial vehicle and the docking point in the B position state;

[0087] S34), in the docking process, the Kalman filtering algorithm is used to fuse the position The expression of the fusion of the position data is:

[0088]

[0089]

[0090]

[0091] wherein, represents obtaining the position data of the target from T265; represents the coordinates of the UAV fed back by the T265 camera in the intermediate coordinate system; represents the target position data measured by the 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 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 including the following steps:

[0093] S41), before entering the control allocation layer, decomposing the initial expected force F B and torque M B given by the upper controller to three axes xyz, the upper controller in the embodiment includes a position controller and an attitude controller; and calculating the pulling force F 6×4 of the motor (i=1, 2, 3, 4) according to the control efficiency matrix A i ; that is:

[0094]

[0095] wherein, [F1, F2, F3, F4] respectively represent the pulling force of the four motors of the UAV; respectively represent the components of the initial expected force F B and torque M B on three axes xyz;

[0096] S42), considering the effect of electromagnetic force on the pulling force F i of the four motors of the UAV (i=1, 2, 3, 4), obtaining new expected torque and force ; that is:

[0097]

[0098] wherein, [M emx , M emy , M emz ], [T emx , T emy , T emz ] are torque T emand electromagnetic force M em Components on three axes xyz; [F1', F2', F3', F4'] represent the four motor limited tension forces respectively;

[0099] S43), because the electromagnetic device 18 is installed at the position of distance h from the center of mass of the UAV and across the axis, the expression of the electromagnetic torque is obtained as:

[0100]

[0101] S5), the desired difference is compensated by using QP compensation to obtain the final desired tension of the UAV; Specifically, it includes the following steps:

[0102] S51), the new desired torque and force Subtract the initial desired force F B and torque M B ; Get the desired difference ΔT:

[0103]

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

[0105]

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

[0107] S53), the constraint in step S52) is relaxed by introducing a relaxation variable s; Get:

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

[0109] In the formula, Δτ represents the equality constraint of the introduced relaxation variable s;

[0110] S54), limit ΔF to obtain inequality constraints:

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

[0112] In the formula, F min , F max respectively represent the minimum and maximum tension provided by the set actuator; F' is the motor tension after limiting;

[0113] S55), construct the objective function to make the actuator not reach the saturation state; That is:

[0114]

[0115] In the formula, U and V are weight matrices;

[0116] S56), Output the final expected pull force F of the drone. exp for:

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

[0118]

[0119] S6), the actuator is based on the final desired pull F of the drone. exp The drone motor is controlled by identifying the relationship between throttle and thrust.

[0120] Example 2

[0121] like Figure 1 As shown, this embodiment provides an aerial docking system for unmanned aerial vehicles based on magnetic vector fields, including a main controller, an onboard computer, and actuators; 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 and attitude controller work closely together to ensure that the UAV 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 UAV.

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

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

[0127] The desired torque and force calculation unit is used to calculate the initial desired force F of the UAV. B and torque M B The calculation of the new desired torque and force of the UAV is based on the electromagnetic force exerted on the four motors of the UAV, Fi (i = 1, 2, 3, 4).

[0128] The compensation unit is used to compensate for the expected difference using QP compensation to obtain the final expected thrust of the UAV, and to control the UAV using the final expected thrust of the UAV.

[0129] The executor is used for controlling the unmanned aerial vehicle motor through the throttle and thrust relationship identification model according to the final desired tension of the unmanned aerial vehicle.

[0130] The above embodiments and descriptions are only illustrative of the principles and the best mode of the present application, and various changes and improvements can be made to the present application without departing from the spirit and the scope of the present application, and these changes and improvements all fall within the scope of the present application.

Claims

1. A magnetic vector field based method for aerial docking of unmanned aerial vehicles, characterized in that, The method comprises the following steps: S1), establishing a dynamic model of the unmanned aerial vehicle; S2), measuring and fitting a magnetic vector field mathematical model by using a magnetic vector field measurement simulation device; S3), designing a docking process and fusing position data of the aerial docking process of the unmanned aerial vehicle by using a Kalman filtering algorithm; specifically comprising the following steps: S31), setting a permanent magnet device at a docking point position; and assuming that the unmanned aerial vehicle starts from a takeoff point A and approaches the docking point, when a D435 camera of the unmanned aerial vehicle first detects a two-dimensional code target at the docking point position, the unmanned aerial vehicle immediately stops moving, and sets the position as a position B; S32), keep B position state, use the D435 camera of unmanned aerial vehicle to measure the two-dimensional code target continuously for a period of time, and establish an intermediate coordinate system at the same time: ; S33), taking the average of the data continuously measured in step S32) to obtain the real distance between the UAV and the docking point in the B position state ; S34), in the docking process, using Kalman filter algorithm to position C position data fusion; its expression is: ； ； ； wherein, represents obtaining position data of the target from the T265 camera; represents the coordinate of the UAV in the intermediate coordinate system fed back by the T265 camera; represents the position data of the target measured by the D435 camera; represents the transformation matrix from the image coordinate system to the intermediate coordinate system; represents the position of the target in the D435 camera coordinate system; represents the position after fusion; represents the Kalman gain; S4), calculating new expected moments and forces according to initial expected forces and moments and the magnetic vector field mathematical model; S5), compensating an expected difference by using QP compensation to obtain final expected pulling forces of the unmanned aerial vehicle; S6), controlling the motor of the unmanned aerial vehicle by the actuator according to the final expected pulling forces of the unmanned aerial vehicle through a throttle and thrust relationship identification model.

2. The magnetic vector field based aerial docking method for UAVs as claimed in claim 1, wherein: In step S1), the expression of the dynamic model of the unmanned aerial vehicle is: ； wherein is the position of the UAV in the world coordinate system; is the representation of the attitude of the body coordinate system relative to the world coordinate system in ; is the linear velocity of the UAV in the world coordinate system; is the mass of the UAV; is the gravitational acceleration in the world coordinate system; and represent the force and torque generated by the actuators in the body coordinate system, respectively; and represent the electromagnetic force and torque generated by the electromagnetic system in the body coordinate system, respectively; represents the inertia matrix; is the angular velocity of the UAV in the body coordinate system; is the derivative of the position ; is the skew-symmetric matrix; is the derivative of the attitude angle ; is the derivative of the angular velocity ; is the derivative of the linear velocity ; and T represents the transpose process.

3. The magnetic vector field based UAV aerial docking method of claim 2, wherein: In step S2), the expression of the magnetic vector field mathematical model is: ； In the formula, respectively represent the parameters after fitting by least square method; , , respectively represent the distance between the permanent magnet device and the electromagnet device on the x, y, z axes.

4. The magnetic vector field based UAV air docking method of claim 3, wherein: In step S4), the initial desired force given by the upper controller is decomposed into three axes xyz and the four motor tensions are calculated without the influence of electromagnetic force according to the control efficiency matrix and the moment ; that is: T1 = (F1 + F2 + F3 + F4) / 4 T2 = (F1 - F2 + F3 - F4) / 4 T3 = (F1 - F2 - F3 + F4) / 4 T4 = (F1 + F2 - F3 - F4) / 4 ； In the formula, respectively represent the pulling forces of the four motors of the unmanned aerial vehicle, , respectively represent the initial desired forces and the components of the moment on the three axes xyz.

5. The magnetic vector field based UAV air docking method of claim 4, wherein: In step S4), the electromagnetic force exerted on the four motors of the drone is considered. The effect of this is to obtain a new desired torque and force. ;Right now: ； wherein, , are the components of the moment of force and the electromagnetic force on the three axes, respectively; are the four motor tensions obtained after the amplitude limiting process, respectively.

6. The unmanned aerial vehicle mid-air docking method based on magnetic vector field of claim 5, wherein: In step S4), since the electromagnetic device is installed at a distance h from the center of mass of the UAV, and the distance is... Therefore, the electromagnetic torque can be expressed as: 。 7. The magnetic vector field based UAV air docking method of claim 6, wherein: In step S5) the new desired force and force moment are subtracted from the initial desired force and force moment given by the upper controller and moment ; the desired difference is obtained 。 8. The magnetic vector field based UAV air docking method of claim 7, wherein: In step S5), the final expected pulling forces of the unmanned aerial vehicle are obtained by compensating the expected difference by using QP compensation, specifically comprising the following steps: S51), due to the desired difference Satisfying the control efficiency model, the expression of the equality constraint in the QP problem is thus obtained as: ； In the formula, represents the difference in tension expectation; S52), relaxing the constraint in step S51) by introducing a relaxation variable s; obtaining: ； wherein denotes the equality constraint introducing the slack variable s; S53), to restricting, resulting in inequality constraints: ； wherein, , respectively represent the minimum and maximum pulling force provided by the set actuators; is the motor pulling force after the clipping process; S54), constructing an objective function, that is, ； In the formula, U and V are weight matrices. S55), output the final desired pulling force of the four motors is: ； 。

Images