Control Method of Amphibious Quadrotor UAV Based on Geometric Attitude Conversion Algorithm

Abstract

The present invention provides a control method for an amphibious quadrotor UAV based on a geometric attitude conversion algorithm, including an outer position loop, a geometric attitude conversion algorithm, and an inner attitude loop. The outer position loop calculates the desired thrust value Td and the desired thrust direction nd of the airframe according to the current position ξ1 and the current speed vI of the airframe along the desired tracking trajectory. The geometric attitude conversion algorithm calculates the desired attitude qd based on the current thrust direction n of the airframe, the current attitude q, and the desired thrust direction nd. The inner attitude loop calculates the desired torque τ2 based on the current angular velocity v2 of the airframe, the current attitude q, and the desired attitude qd. The desired thrust value Td and the desired torque τ2 are then output to the motors of the amphibious quadrotor UAV. This method can overcome the influence of buoyancy in water on the force state and motion state of the amphibious quadrotor UAV, and can achieve trajectory tracking in the air and in water without switching the control structure, but only by changing the control parameters.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicle control, and particularly to the control technology of an amphibious quadrotor unmanned aerial vehicle, especially a control method for an amphibious quadrotor unmanned aerial vehicle based on a geometric attitude conversion algorithm. Background Art

[0002] The cross-aqua-air movement of the amphibious quadrotor unmanned aerial vehicle in the vertical plane is as Figure 2 shown. When the airframe is flying in the air, the main external forces acting on it are gravity and air resistance. The thrust has to overcome gravity, and its direction does not necessarily point to the direction of motion. When submerging and navigating in water, it is also affected by buoyancy. Usually, by appropriately designing the counterweight, the buoyancy is approximately equal to the gravity, and the direction of its thrust has to point to the direction of motion. Therefore, the processes of changing the motion states in the air and in water are also different. How to overcome the influence of different forces and motion forms in the air and in water and design a trajectory control method for the amphibious quadrotor unmanned aerial vehicle is a difficult problem faced by theoretical research.

[0003] The main problems and deficiencies of the existing methods are as follows: First, the existing methods require separately designing two controllers with different control structures, one for air flight control and the other for underwater submerging and navigating control, to perform the water-air switching work. The control structure switching process is likely to induce the airframe to shake. Second, the amphibious quadrotor unmanned aerial vehicle is affected by buoyancy during underwater movement. Therefore, the force states and motion forms of the airframe in the air and in water are different, and it is difficult for the traditional quadrotor flight control method to achieve underwater trajectory tracking. Summary of the Invention

[0004] The purpose of the present invention is to provide a control method for an amphibious quadrotor unmanned aerial vehicle based on a geometric attitude conversion algorithm, which solves the problems of the above-mentioned control structure switching and different force states and motion forms of the airframe in the air and in water. The present invention realizes trajectory tracking in the air and underwater with a single control structure. The geometric attitude conversion algorithm adopted by the present invention can extract the desired attitude from the desired thrust direction, and the calculation amount is small.

[0005] The above object of the present invention is achieved by the following technical solutions:

[0006] A control method for an amphibious quadrotor unmanned aerial vehicle based on a geometric attitude conversion algorithm. The control structure adopted includes an outer position loop, a geometric attitude conversion algorithm, and an inner attitude loop. The outer position loop realizes position tracking. According to the position point information of the desired trajectory , the current position ξ1 of the airframe, and the current speed v I , the desired thrust value T d for the airframe to track the desired trajectory and the desired thrust direction n d are calculated.

[0007] The geometric attitude conversion algorithm can overcome the influence of the differences in the force states and motion forms of the airframe in the air and water, and calculate the desired attitude q according to the current thrust direction n of the airframe, the current attitude q, and the desired thrust direction n d to obtain the desired attitude q d , and can achieve the motion control of the amphibious quadrotor UAV in the air and water without switching the control structure.

[0008] The inner attitude loop is used to control the attitude of the airframe. The inner attitude loop calculates the desired torque τ according to the current angular velocity v2 of the airframe, the current attitude q, and the desired attitude q d . The desired thrust value T d and the desired torque τ are then output to the motors of the amphibious quadrotor UAV to achieve the trajectory tracking of its motion in the air and water.

[0009] The control method of the amphibious quadrotor UAV based on the geometric attitude conversion algorithm according to the present invention includes the following steps:

[0010] Step 1: Set the parameters of the desired trajectory ;

[0011] Step 2: Calculate the magnitude T of the desired thrust d and the thrust direction n d according to the motion mathematical model;

[0012] Step 3: The geometric attitude conversion algorithm deduces the desired attitude q d ;

[0013] Step 4: The attitude controller calculates the desired torque τ2(τ x , τ y , τ z );

[0014] Step 5: Adjust the position ξ1 of the airframe.

[0015] The geometric attitude conversion algorithm in Step 3 deduces the desired attitude q d , specifically: The geometric attitude conversion algorithm calculates the desired attitude q through two-axis angle rotations according to the desired thrust direction n d , the current thrust direction n of the airframe, and the current attitude q d .

[0016] The geometric attitude conversion algorithm is proposed based on the quaternion coordinate conversion and the rotation axis-angle coordinate conversion method. The specific steps are as follows:

[0017] Step 3.1: Calculate the first rotation q1

[0018] From n to n dAccording to the rotation axis-angle conversion method, the rotation axis angle can be defined as the vector axis u and the rotation angle β, which are calculated by the following formulas respectively:

[0019]

[0020] β=atan2(u·(n×n d ),n·n d )

[0021] Define the unit quaternion q1 from the rotation axis angle

[0022]

[0023] q1 represents the rotation from the current thrust direction n to the desired thrust direction n d There is

[0024]

[0025] In the formula, represents quaternion multiplication.

[0026] Step 3.2: Calculate the transitional attitude q′ d

[0027] According to the definition of unit quaternion multiplication, the transitional attitude q′ based on the unit quaternion is obtained d as

[0028]

[0029] After one rotation, the current thrust direction n reaches the desired thrust direction n along the path l1 d , and this path is shorter than any other path connecting the endpoints of the two vectors. Along the path l1, that is, from the current attitude q to the transitional attitude q′ d is the shortest path. The transitional attitude q′ d can ensure the desired thrust direction n d , but the body yaw angle is not necessarily 0, and it still needs to be rotated once more around the desired thrust direction to ensure that the body yaw angle approaches 0.

[0030] Step 3.3: Calculate the second rotation q2

[0031] From q′ d the corresponding rotation matrix R d (q′ d ) can be obtained. Let n bx n bz be the unit vectors of the OX d axis and the OZ B axis in the body coordinate system corresponding to q′ B , then there is n bx =Rd (q′ d )e1, n bz = n d = R d (q′ d )e3. If the OX axis of the body coordinate system corresponding to q′ d is rotated around the OZ axis to the XOZ plane of the inertial coordinate system, the yaw angle can be ensured to be 0. Let OX′ in the XOZ plane B be obtained by rotating the OX axis. Then OX′ is perpendicular to the OZ axis, and the unit vector n′ in the direction of the OX′ axis can be obtained B axis to the XOZ plane of the inertial coordinate system, the yaw angle can be ensured to be 0. Let OX′ in the XOZ plane E O E Z E plane. Then OX′ E O E Z E plane is obtained by rotating the OX axis. Then OX′ B is perpendicular to the OZ axis B axis, and the unit vector n′ in the direction of the OX′ axis can be obtained B . Therefore, to ensure that the yaw angle approaches 0, it is necessary to rotate by β′ = atan2(n B ·(n B × n′ bx ), n d · n′ bz ·(n bx × n′ bx ), n bx · n′ bx ). There is

[0032]

[0033] q2 represents the rotation from n bx to n′ bx . There is

[0034]

[0035] Step 3.4: Calculate the desired attitude q d

[0036] Therefore, the desired attitude q that simultaneously ensures the desired thrust direction n d and the desired yaw angle of 0 is d as follows

[0037]

[0038] The beneficial effects of the present invention are as follows:

[0039] (1) The control scheme designed by the geometric attitude conversion algorithm of the present invention realizes the trajectory tracking of the quadrotor layout body in the air or underwater with a separate control structure.

[0040] ​​​(2) The geometric attitude conversion algorithm adopted by the present invention can extract the desired attitude from the desired thrust direction.

[0041] (3) The geometric attitude conversion algorithm adopted by the present invention can ensure that the yaw angle of the aircraft body approaches 0 during the trajectory tracking process.

[0042] (4) The geometric attitude conversion algorithm adopted by the present invention relies on the unit quaternion calculation, and the calculation amount is small. Brief Description of the Drawings

[0043] The drawings described herein are used to provide a further understanding of the present invention and form a part of this application. The schematic examples and descriptions of the present invention are used to explain the present invention and do not constitute an improper limitation to the present invention.

[0044] Figure 1 is the abstract drawing of the present invention;

[0045] Figure 2 is the schematic diagram of the movement of the water-air amphibious quadrotor UAV in the vertical plane;

[0046] Figure 3 is the simplified schematic diagram of the inertial coordinate system and the aircraft body coordinate system adopted by the present invention;

[0047] Figure 4 is the first axis-angle rotation schematic diagram of the geometric attitude conversion algorithm of the present invention;

[0048] Figure 5 is the movement trajectory diagram of the water-air amphibious quadrotor UAV tracking a helix in water;

[0049] Figure 6 is the movement trajectory diagram of the water-air amphibious quadrotor UAV tracking a fixed point in water;

[0050] Figure 7 is the movement trajectory diagram of the water-air amphibious quadrotor UAV tracking a helix in the air;

[0051] Figure 8 is the movement trajectory diagram of the water-air amphibious quadrotor UAV tracking a fixed point in the air. Detailed Embodiment

[0052] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention. To make the above objects, features, and advantages of the present invention more obvious and understandable, the present invention will be further described in detail below in conjunction with the drawings and specific embodiments.

[0053] See Figures 1 to 8 As shown, the present invention provides a control method for an amphibious quadrotor UAV based on a geometric attitude conversion algorithm. The control structure of this method includes an outer position loop, a geometric attitude conversion algorithm, and an inner attitude loop. The outer position loop calculates the desired thrust value T and the desired thrust direction n I for the airframe to track the desired trajectory based on the position point information of the desired trajectory d , the current position ξ1 of the airframe, and the current speed v d . The geometric attitude conversion algorithm processes the current thrust direction n, the current attitude q, and the desired thrust direction n d of the airframe to obtain the desired attitude q d . The inner attitude loop calculates the desired torque τ based on the current angular velocity v2, the current attitude q, and the desired attitude q d of the airframe. The desired thrust value T d and the desired torque τ are then output to the motors of the amphibious quadrotor UAV to achieve trajectory tracking during its movement in the air and water. The geometric attitude conversion algorithm can overcome the influence of the differences in the force states and motion forms of the airframe in the air and water, and can achieve the motion control of the amphibious quadrotor UAV in the air and water without switching the control structure.

[0054] For the convenience of description, the following terms are defined. In the present invention, the airframe refers to the airframe of the amphibious quadrotor UAV. Thrust refers to the lift generated by the motors of the amphibious quadrotor UAV driving the rotors. The desired maneuvering trajectory refers to a pre-set motion trajectory that the airframe needs to track. The vertical direction refers to the direction perpendicular to the ground and opposite to gravity. The shortest path refers to the path l1 passed by the end point of the current thrust direction n when the current thrust direction n rotates once with the starting point of the current thrust direction n as the rotation center and passes through the desired thrust direction n d at the minimum rotation angle β, as shown in Figure 4 .

[0055] Embodiment:

[0056] The amphibious quadrotor UAV used in this embodiment is a cross-shaped quadrotor UAV, as shown in the appendix Figure 3 . The airframe plane refers to the plane formed by the center of the amphibious quadrotor UAV and the centers of the 4 motors. The thrust provided by the rotors is always perpendicular to the airframe plane and towards the upper part of the airframe plane. The inertial coordinate system OX E Y E Z E and the airframe coordinate system O B X B Y B Z B both satisfy the right-hand coordinate system criterion. The inertial coordinate system O E X E YE Z E : O E Designate a fixed point on the ground, O E Z E The axis points to the center of the earth, O F X E , O E Y E , O E Z E Are mutually perpendicular to each other. The body coordinate system O B X B Y B Z B : O B Is the center of the quadrotor body, O B X B Is the direction along the line from the body center O B To the center of the No. 1 rotor motor; O B Y B Is the direction along the line from the body center O B To the center of the No. 4 rotor motor; O B Z B Is the direction passing through the body center O B , And perpendicular to the body facing downwards.

[0057] The control structure adopted in this embodiment includes an outer position loop, a geometric attitude conversion algorithm and an inner attitude loop. The outer position loop realizes position tracking, according to the desired trajectory Of the position point information, the current position ξ1 of the body and the current speed v I , Calculate the desired thrust value T d And the desired thrust direction n d . The geometric attitude conversion algorithm processes according to the current thrust direction n of the body, the current attitude q, and the desired thrust direction n d To obtain the desired attitude q d . The inner attitude loop calculates the desired torque τ according to the current angular velocity v2 of the body, the current attitude q, and the desired attitude q d . The desired thrust value T d And the desired torque τ are then output to the motors of the water-air amphibious quadrotor UAV to realize the trajectory tracking of its movement in the air and water.

[0058] The control structure includes an outer position loop, a geometric attitude conversion algorithm and an inner attitude loop. The outer position loop is used to control the body position, and the inner attitude loop is used to control the body attitude.

[0059] The geometric attitude conversion algorithm can overcome the influence of the differences in the force states and motion forms of the body in the air and water, and processes according to the current thrust direction n of the body, the current attitude q, and the desired thrust direction n d To obtain the desired attitude qd , the motion control of the amphibious quadrotor UAV in the air and water can be achieved without switching the control structure.

[0060] The specific steps of this embodiment are as follows:

[0061] Step 1: Set the parameters of the desired trajectory .

[0062] The position information of the body that needs to track the desired trajectory is set as ξ 1d , and ξ 1d is continuously differentiable of the first and second orders, which are the first derivative (i.e., velocity) and the second derivative (i.e., acceleration) of the position respectively.

[0063] Step 2: Calculate the magnitude T of the desired thrust d and the thrust direction n d .

[0064] An inertial coordinate system and a body coordinate system are established to describe the six-degree-of-freedom motion of the body, as Figure 3 shown. The influence of the motor and the propeller arm is ignored in the modeling, and the body is regarded as a homogeneous sphere. The mathematical model of the motion in the air / water can be simplified as:

[0065]

[0066] In the formula, G is the gravity received by the body, and B is the buoyancy received by the body. ξ1 = (x, y, z) represents the current position of the center of mass of the body; v I = [v x , v y , v z T is the velocity of the body in the inertial coordinate system; M 11 = (m + m a )I3, m represents the mass of the body, m a represents the added mass of the body moving in the medium, I3 represents the third-order identity matrix, f a = RC d I3R -1 v I represents the resistance received during the motion in the medium, C d is the resistance coefficient, B and G respectively represent the buoyancy and gravity of the body in the medium, e3 = [0, 0, 1] T . R is the rotation matrix from the body coordinate system to the inertial coordinate system. q = [η q1 q2 q3] T = [η ε T T is the unit quaternion, which is used to represent the attitude of the body, v2 = [p, q, r] T represents the angular velocity of the body, M22 =(J + J a )I3, where J represents the moment of inertia of the body, and J a represents the additional rotational mass in motion in the medium. There is v2×(Jv2) = 0, and f b = C m I3v2 represents the resistance moment suffered during motion in the medium, and C m represents the resistance moment coefficient.

[0067] The control input variables are τ1 = [0, 0, -T d T ; τ2 = [τ x , τ y , τ z T . τ x , τ y , τ z are respectively the rolling moment about the X B axis, the pitching moment about the Y B axis, and the yaw moment about the Z B axis.

[0068] For motion in water, the buoyancy is equal to the gravity B = G, and the empirical formulas for the added mass m a and the added moment of inertia J a can be expressed as where r is the radius of the sphere and ρ is the density of water.

[0069] For motion in air, the buoyancy B = 0, and the added mass m a and the added moment of inertia J a can be neglected, m a = 0, J a = 0.

[0070] The position controller of the outer position loop first calculates the desired thrust value T 1d and the desired thrust direction n ξ1, v I based on ξ d and d .

[0071]

[0072] where k1 and k2 are positive numbers to be set.

[0073] Step 3: The geometric attitude conversion algorithm derives the desired attitude q d .

[0074] The geometric attitude conversion algorithm proposed in this embodiment is based on the quaternion coordinate conversion and the rotation axis - angle coordinate conversion method. According to the desired thrust direction n d ​​, the current thrust direction n and current posture q of the body, calculate the expected posture q d .

[0075] By the attached Figure 4 It can be seen that the thrust direction n rotates around the vector axis u by an angle β, and the forward thrust direction n can reach n along the path l1. d , and this path is shorter than any other path connecting the two vector endpoints. The vector axis u and the rotation angle β are calculated by the following formulas:

[0076]

[0077] β=atan2(u·(n×n d ), n·n d )

[0078] The unit quaternion q1 is defined by the rotation axis angle

[0079]

[0080] q1 represents the current thrust direction n to the desired thrust direction n d The rotation of

[0081]

[0082] Where, Represents quaternion multiplication.

[0083] According to the definition of unit quaternion multiplication, the transition posture q′ based on the unit quaternion is obtained d for

[0084]

[0085] After one rotation, the current thrust direction n is reached along the path l1 from the desired thrust direction n d , this path is shorter than any other path connecting the two vector endpoints, along the path l1, that is, the current posture q to the transition posture q' d The shortest path of transition posture q′ d Able to ensure the desired thrust direction n d However, the yaw angle of the aircraft is not necessarily 0. It needs to be rotated again around the desired thrust direction to ensure that the yaw angle of the aircraft is close to 0.

[0086] By q′ d The corresponding rotation matrix R can be obtained d (q′ d ), let n bx n bz q′ d The corresponding body coordinate system OX BAxis and OZ B The unit vector of the axis, then n bx =R d (q′ d )e1、n bz =n d =R d (q′ d )e3. If q′ d The corresponding body coordinate system OX B Axis around OZ B The X axis rotates to the inertial coordinate system E O E Z E In the plane, the yaw angle can be guaranteed to be 0. Let X E O E Z E In-plane OX′ B For OX B The axis is rotated, then OX′ B Perpendicular to OZ B axis, and then we can get OX' B Unit vector n′ in the direction of the axis bx Therefore, in order to ensure that the yaw angle is close to 0, it is necessary to rotate around n d Rotation β′=atan2(n bz ·(n bx ×n′ bx ), n bx ·n′ bx ),have

[0087]

[0088] q2 represents n bx to n′ bx The rotation of

[0089]

[0090] Thus, the desired thrust direction n is guaranteed at the same time d and the desired attitude q with the desired yaw angle of 0 d for

[0091]

[0092] Step 4: The attitude controller calculates the desired torque τ2(τ x , τ y , τ z ).

[0093] The attitude controller is based on the desired attitude q d , the current posture q of the body, the current angular velocity v2 of the body, the expected torque τ2(τ x , τy , τ z ) is calculated as follows:

[0094]

[0095] In the formula, K d , K p are positive numbers to be set, and sgn represents the sign function.

[0096] Step 5: Adjust the position ξ1 of the airframe.

[0097] The motor of the airframe adjusts the position ξ1 and attitude q of the airframe according to the desired thrust T d and the desired torque vector τ2(τ x , τ y , τ z ), and loop through Steps 2 to 5 to make the airframe track the desired trajectory.

[0098] Example simulation results and analysis:

[0099] To verify the effectiveness and practicality of the method, the following trajectory tracking simulation experiments in water and air are carried out using this method. Set the airframe mass to m = 4.188 kg, and the airframe moment of inertia to I xx = I yy = I zz = 1.68×10 -2 , and the airframe radius is 0.1 m. The experiment is carried out through the Simulink algorithm in Matlab software.

[0100] (1) Underwater trajectory tracking

[0101] The water-air amphibious quadrotor UAV is set to be neutrally stable, that is, the gravity of the airframe is equal to the buoyancy in water, and the acting points of the buoyancy and gravity are the same. The initial position of the water-air amphibious quadrotor UAV is set to (0, 0, 0). Then, the tracking effect of the water-air amphibious quadrotor UAV underwater is simulated.

[0102] The tracking desired trajectory is a helix: Set the desired trajectory to be a helix rising along the Z-axis, the period of the helix is 20 s, and the radius is 2 m. The relevant parameters of the position controller and the attitude controller are set to k1 = 5, k2 = 10, K d = 5, K p = 200, the simulation time is set to 25 s, and the sampling interval is 5 s. The simulation results are as Figure 5 shown. In water, the thrust direction of the airframe points to the tangent direction of the motion trajectory. This method can track the position trajectory well, and the maximum tracking error is about 0.05 m.

[0103] Tracking the desired trajectory as a fixed point: The desired position is set directly below the initial position as (0, 0, 3). The relevant parameters of the position controller and the attitude controller are set as k1 = 5, k2 = 10, K d = 20, K p = 50. Figure 5 In the figure, the horizontal line of the symbol represents the position of the body plane, and the vertical line represents the thrust direction. It means that the thrust direction of the body is exactly upward. The sampling times are 0s, 1s, 2.5s, 5s, 10s, 20s. The simulation results are as Figure 6 shown. In water, the thrust direction of the body points to the tangent direction of the motion trajectory. Since the gravity is equal to the buoyancy, when the body reaches the desired position (i.e., the fixed point), there is no requirement for the final attitude of the body. This method has a good tracking effect on the fixed point.

[0104] (2) Air trajectory tracking

[0105] The desired trajectories are respectively set as a helix and a fixed point, and the tracking effect of the water-air amphibious quadrotor UAV in the air is simulated. The controller parameters are set as k1 = 5, k2 = 10, K d = 5, K p = 200.

[0106] Tracking the desired trajectory as a helix: The period of the desired helix trajectory is 2π, and the radius is 2m. The simulation results are as Figure 7 shown, indicating that the body has a good tracking effect on the set helix trajectory.

[0107] Tracking the desired trajectory as a fixed point: The initial position is set as (0, 0, -10), and the desired position is the fixed point (0, 0, 0). The sampling times are 0s, 0.25s, 0.5s, 0.75s, 1s, 1.5s, 2s, 3s, 4s, 5s. The simulation results are as Figure 8 shown. When the initial position error is large, the water-air amphibious quadrotor UAV can first fly upside down, accelerate through thrust and gravity, and then turn over to reach the desired position, with the thrust balancing the gravity.

[0108] The above are only the preferred examples of the present invention and are not used to limit the present invention. For those skilled in the art, the present invention can have various changes and modifications. Any modifications, equivalent replacements, improvements, etc. made to the present invention shall be included within the protection scope of the present invention.

Claims

1. A control method for an amphibious quadrotor UAV based on a geometric attitude conversion algorithm, characterized in that: The adopted control structure includes an outer position loop, a geometric attitude conversion algorithm, and an inner attitude loop. The outer position loop realizes position tracking. According to the position point information of the desired trajectory , the current position ξ1 of the airframe, and the current speed v I , the desired thrust value T d and the desired thrust direction n d for the airframe to track the desired trajectory are calculated; The geometric attitude conversion algorithm can overcome the influence of the differences in the force states and motion forms of the airframe in the air and water, and according to the current thrust direction n of the airframe, the current attitude q, and the desired thrust direction n d process to obtain the desired attitude q d , and can achieve the motion control of the amphibious quadrotor UAV in the air and water without switching the control structure; The geometric attitude conversion algorithm is proposed based on the quaternion coordinate conversion and the rotation axis-angle coordinate conversion method. The specific steps are as follows: Step 3.1: Calculate the first rotation q1 From n to n d According to the rotation axis angle conversion method from n to n, the rotation axis angle can be defined as the vector axis u and the rotation angle β, which are calculated by the following formulas respectively: β = atan2(u · (n × n d ), n · n d ) Define the unit quaternion q1 by the rotation axis-angle q1 represents the rotation from the current thrust direction n to the desired thrust direction n d and there is wherein, represents quaternion multiplication; Step 3.2: Calculate the transitional attitude q′ d According to the definition of unit quaternion multiplication, the transitional attitude q' based on unit quaternion is obtained d is After one rotation, the current thrust direction n reaches the desired thrust direction n along the path l1 d , which is shorter than any other path connecting the endpoints of the two vectors. Along the path l1, it is also the shortest path from the current attitude q to the transitional attitude q′ d ; the transitional attitude q′ d can ensure the desired thrust direction n d , but the body yaw angle is not necessarily 0. It is still necessary to rotate once more around the desired thrust direction to ensure that the body yaw angle approaches 0; Step 3.3: Calculate the second rotation q2 By q′ d The corresponding rotation matrix R can be obtained d (q′ d ), let n bx n bz q′ d The corresponding body coordinate system OX B Axis and OZ B The unit vector of the axis, then n bx =R d (q′ d )e1、n bz =n d =R d (q′ d )e3; If q′ d The corresponding body coordinate system OX B Axis around OZ B The X axis rotates to the inertial coordinate system E O E Z E In the plane, the yaw angle can be guaranteed to be 0; let X E O E Z E In-plane OX′ B For OX B The axis is rotated, then OX′ B Perpendicular to OZ B axis, and then we can get OX' B Unit vector n′ in the direction of the axis bx Therefore, in order to ensure that the yaw angle approaches 0, it is necessary to rotate around n d Rotation β′=atan2(n bz ·(n bx ×n′ bx ),n bx ·n′ bx ),have q2 represents the rotation from n bx to n' bx and there is Step 3.4: Calculate the desired attitude q d Therefore, while ensuring the desired thrust direction n d and the desired attitude q with a desired yaw angle of 0 d is 2. The control method of the water-air amphibious quadrotor UAV based on the geometric attitude conversion algorithm according to claim 1, characterized in that: The described inner attitude loop is used to control the body attitude. The inner attitude loop calculates the desired torque τ based on the current angular velocity v2 of the body, the current attitude q, and the desired attitude q d and the desired thrust value T d and the desired torque τ are then output to the motors of the amphibious quadrotor UAV to achieve trajectory tracking during its movement in the air and water.

3. The control method of the water-air amphibious quadrotor UAV based on the geometric attitude conversion algorithm according to claim 1 or 2, characterized in that: It includes the following steps: Step 1, set the parameters of the desired trajectory ; Step 2: Calculate the magnitude T of the desired thrust and the thrust direction n according to the kinetic model d d ;​ Step 3: Derive the expected attitude q using the geometric attitude conversion algorithm d ; Step 4, the attitude controller calculates the desired torque τ2(τ x , τ y , τ z ); Step 5: Adjust the position ξ1 of the fuselage.

4. The control method of the water-air amphibious quadrotor UAV based on the geometric attitude conversion algorithm according to claim 3, wherein: Derivation of the desired attitude q using the geometric attitude transformation algorithm described in Step 3 d , specifically: The geometric attitude transformation algorithm calculates the desired attitude q through two-axis angle rotations based on the desired thrust direction n d , the current thrust direction n of the airframe, and the current attitude q d .

Images