Trajectory tracking control method and device for quad-rotor unmanned aerial vehicle based on motor f-pwm model

Abstract

The application discloses a kind of four rotor unmanned aerial vehicle trajectory tracking control method and equipment based on motor f-PWM model, first consider system internal uncertainty and external unknown disturbance, based on Newton-Euler method establishes the dynamics model of the position and attitude of four rotor unmanned aerial vehicle;Then the f-PWM model of hollow cup DC motor is mapped into PWM domain under the position and attitude subsystem of four rotor unmanned aerial vehicle;Finally, based on the output of position loop horizontal channel controller, the tracking target attitude of attitude subsystem is solved, and the attitude tracking controller of attitude loop is designed using superhelix control method in PWM domain.The application improves the accuracy of four rotor unmanned aerial vehicle trajectory tracking, and improves the signal conversion efficiency of control signal from controller to actuator.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of flight control of quadrotor unmanned aerial vehicles, and particularly relates to a trajectory tracking control method and device for a quadrotor unmanned aerial vehicle based on a motor f-PWM model. BACKGROUND

[0002] With the rapid development of unmanned aerial vehicle technology, quadrotor unmanned aerial vehicles have been widely used in various fields due to their flexibility and stability. By integrating sensors such as IMU, GPS, and vision sensors, unmanned aerial vehicles can obtain real-time position information and attitude data to monitor and adjust the flight state. Position and attitude tracking control, as one of the key technologies for autonomous flight of unmanned aerial vehicles, is crucial for improving their navigation accuracy and flight stability. However, tracking position and attitude faces many challenges, including interference from dynamic environments, sensor noise, and real-time algorithm. When the quadrotor unmanned aerial vehicle moves quickly or is affected by wind, the estimation accuracy of position and attitude is easily affected, leading to unstable flight. On the other hand, the most important thing for attitude tracking control is fast tracking to ensure that the unmanned aerial vehicle can quickly respond to environmental changes and maintain stable flight. Traditional PID control methods often cannot meet the high-precision requirements in these situations, often facing slow response, poor anti-interference ability, and other problems, limiting the performance of unmanned aerial vehicles.

[0003] In recent years, super-spiral control algorithm in second-order sliding mode control has gradually attracted attention. This algorithm not only has finite-time characteristics, which can quickly converge to the target state in a short time, but also can effectively suppress external disturbances, thereby achieving fast and stable attitude tracking in complex environments. Compared with traditional control methods, the super-spiral algorithm significantly improves the control effect of quadrotor unmanned aerial vehicles in position and attitude tracking, enabling the unmanned aerial vehicle to maintain high-precision flight state in a dynamically changing environment.

[0004] In addition, the f-PWM model of the DC motor is used for controller design, further enhancing the practicality of the control strategy. By designing the controller output as a PWM signal, it can be directly applied to motor control, achieving more efficient energy transfer. This design method based on the motor model in the PWM domain makes the controller better fit the actual engineering needs, improving the adaptability and reliability of the control system.

[0005] However, current research on quadrotor unmanned aerial vehicle attitude tracking based on super-spiral control algorithm is still limited, especially in the promotion and optimization of practical applications. Therefore, in-depth exploration of the technical development in this field not only promotes the innovation of unmanned aerial vehicle attitude control, but also provides new possibilities for its application in complex tasks. Through the study of this control algorithm and its combination with the motor model, it is expected to bring more extensive application prospects to the control method of quadrotor unmanned aerial vehicles and help the further development of unmanned aerial vehicle technology. SUMMARY

[0006] The present application is aimed at the problem of long transition process of position stability and large steady-state error of traditional double closed-loop PID control algorithm in unmanned aerial vehicle, and provides a quadrotor unmanned aerial vehicle trajectory tracking control method and equipment based on motor f-PWM model. First, the internal uncertainty and external unknown disturbance of the system are considered, and the dynamics model of the position and attitude of the quadrotor unmanned aerial vehicle is established based on the Newton-Euler method. Then, the position and attitude subsystem of the quadrotor unmanned aerial vehicle is mapped to the PWM domain based on the f-PWM model of the hollow cup DC motor. Finally, based on the output of the position loop horizontal channel controller, the tracking target attitude of the attitude subsystem is calculated, and the super-spiral control method is used in the PWM domain to design the attitude tracking controller of the attitude loop. The present application improves the accuracy of trajectory tracking of the quadrotor unmanned aerial vehicle and improves the signal conversion efficiency from the controller to the actuator.

[0007] To achieve the above purpose, the technical scheme adopted by the present application is as follows: a quadrotor unmanned aerial vehicle trajectory tracking control method based on motor f-PWM model, characterized in that it comprises the following steps:

[0008] S1, establishing a quadrotor unmanned aerial vehicle model: based on the Newton-Euler method, the dynamics model of the position and attitude of the quadrotor unmanned aerial vehicle is established, the model divides the quadrotor unmanned aerial vehicle into a position subsystem and an attitude subsystem, so that the position tracking controller in the position subsystem and the attitude tracking controller in the attitude subsystem are decoupled;

[0009] S2, pwm mapping: the f-PWM model of the hollow cup DC motor is established, and the attitude subsystem of the quadrotor unmanned aerial vehicle is mapped to the PWM domain;

[0010] S3, position tracking controller and attitude tracking controller design: the three-axis position tracking controller of the position loop is composed of a PD controller combined with an ESO (extended state observer), based on the output of the position loop horizontal channel controller, the expected target attitude of the attitude subsystem is calculated, and the super-spiral control method is used in the PWM domain to design the attitude tracking controller of the attitude loop; the three inputs of the attitude tracking controller and one input of the vertical channel of the position controller form four inputs of the quadrotor unmanned aerial vehicle, realizing the tracking control of the trajectory.

[0011] As an improvement to the present invention, the dynamic model of the position and attitude of the quadcopter UAV is specifically as follows:

[0012]

[0013] Where: r d =[x d y d z d ] T x represents the desired position of the quadcopter drone in the global coordinate system. d y d z d These represent the desired positions along the X, Y, and Z axes, respectively. This indicates the positional error of a quadcopter drone; This represents the velocity of the quadcopter drone in the global coordinate system. φ represents the velocity along the X, Y, and Z axes, respectively; g is the acceleration due to gravity, and η = [φ θ ψ]. T denoted by Euler angles in the global coordinate system for a quadcopter UAV, where φ, θ, and ψ represent roll angle, pitch angle, and yaw angle, respectively. f i Let f be the lift generated by each rotor, i∈{1,2,3,4}. p =f1+f2+f3+f4 represents the total lift provided by the four rotors of the quadcopter UAV to be designed in the body coordinate system, M2=[0 0 1] T , This represents the lumped disturbance experienced by a quadcopter UAV in a translational model in the global coordinate system. These represent the lumped disturbances experienced along the X, Y, and Z axes, respectively. Due to unknown external interference experienced by the quadcopter drone, k x k y k z For unknown speed interference coefficient; η d =[φ d θ d ψ d ] T Let φ represent the expected Euler angles of the quadcopter UAV in the global coordinate system. d θ d ψ d η represents the desired roll angle, desired pitch angle, and desired yaw angle, respectively; e =η-η d This indicates the attitude error of a quadcopter drone; This represents the angular velocity of the quadcopter drone in the body coordinate system, where These are the angular velocities of rotation about the X, Y, and Z axes, respectively; J = diag{J xx J yy J zz} represents the inertial matrix of a quadcopter drone, J xx J yy J zz >0; Represents ω b The antisymmetric matrix; This represents the torque of the quadcopter drone in the body coordinate system, where These are the torques about the X, Y, and Z axes, respectively. l x l y l z They are respectively The corresponding lever arm, f b =[f1 f2 f3 f4] T This represents the lift generated by the four propellers of a quadcopter drone. U = [f0 △f φ △f θ △f ψ ] T These represent the four input signals of the quadcopter UAV system, which respectively represent the lift required by each rotor to keep the aircraft hovering, the lift in the roll direction that varies from U1, the lift in the pitch direction that varies from U1, and the lift in the yaw direction that varies from U1.

[0014] As another improvement of the present invention, step S2 specifically includes the following steps:

[0015] S21: The relationship between the input PWM and the output thrust of each hollow cup DC motor is established using the following quadratic function:

[0016]

[0017] Where a and b are constants, i∈{1,2,3,4}, p i f is the PWM input to the i-th hollow cup DC motor. i The thrust output by the i-th hollow cup DC motor;

[0018] S22: Expanding the part representing the transformation relationship from rotational angular velocity to Euler angles in the body coordinate system in the dynamic model of the position and attitude of the quadcopter UAV in step S1 yields:

[0019]

[0020] Using the small angle assumption, we can obtain

[0021]

[0022] wherein

[0023] S23: obtaining a quadrotor unmanned aerial vehicle attitude dynamics subsystem under a PWM domain

[0024]

[0025] wherein,

[0026]

[0027] D φ , D θ , D ψ are four-rotor unmanned aerial vehicle attitude subsystem internal lumped disturbances, respectively, △p φ , △p θ , △p ψ are PWM signals for adjusting the roll angle, the pitch angle and the yaw angle, respectively.

[0028] As another improvement of the present application, in the step S3, the position loop controller responsible for position control is composed of a PD controller combined with an ESO, and the output of the position loop controller is u o =[u x u y u z ] T , the expected target attitude of the attitude subsystem is η d =[φ d θ d ψ d ] T , wherein ψ d is set to 0, φ d =m(u x sinψ-u y cosψ) / f p , and θ d =m(u x cosψ+u y sinψ) / f p ;

[0029] The △p φ , △p θ , △p ψ are designed to be the controller The attitude tracking controller of the attitude loop under the PWM domain is:

[0030]

[0031] wherein y=sgn(x) is a sign function; respectively represent the attitude tracking control quantity in the roll angle direction, the attitude tracking control quantity in the pitch angle direction, and the attitude tracking control quantity in the yaw angle direction; λ φ >0 is a roll angle direction attitude tracking controller gain, λ θ >0 is a pitch angle direction attitude tracking controller gain, λ ψ >0 is a yaw angle direction attitude tracking controller gain;e φ , e θ , e ψ respectively represent the angle tracking error of the roll angle, the angle tracking error of the pitch angle, and the angle control error of the yaw angle, respectively represent the sliding surface in the roll angle direction, the sliding surface in the pitch angle direction, and the sliding surface in the yaw angle direction; are all obtained by differentiating φ d , θ d respectively and then passing through a second-order low-pass digital filter.

[0032] As another improvement of the application, the obtained The second-order low-pass digital filter used is specifically:

[0033]

[0034] Wherein, k∈Z + ;△t is a control period; diff k is a real-time difference signal; is an intermediate variable required for filtering, and α1, α2, β0, β1, β2 are filter parameters to be designed.

[0035] In order to achieve the above-mentioned purpose, the technical scheme adopted by the application is: a computer readable storage medium, which stores a computer program, the computer program is executed by a processor, and the trajectory tracking control method of the quad-rotor unmanned aerial vehicle based on the motor f-PWM model is realized.

[0036] In order to achieve the above-mentioned purpose, the technical scheme adopted by the application is: a computer device, comprising

[0037] a memory for storing instructions;

[0038] a processor for executing the instructions, so that the computer device executes the trajectory tracking control method of the quad-rotor unmanned aerial vehicle based on the motor f-PWM model.

[0039] Compared with the prior art, the application has the following technical advantages and effects:

[0040] (1) In the aspect of position tracking control, the super-spiral controller is applied to the attitude tracking control of the quad-rotor unmanned aerial vehicle under the control scheme of the traditional ESO combined with the PD controller, compared with the general PID attitude tracking control scheme, the trajectory tracking error of the quad-rotor unmanned aerial vehicle is significantly reduced, and the method has universality.

[0041] (2) The controller design method based on the f-PWM model takes PWM as the output of the controller, can be directly input to the motor, and simplifies the signal conversion process from the controller to the actuator.

[0042] (3) The technical idea of the f-PWM model is suitable for other technical fields which hope to simplify part of the control process, and has wide application prospect. BRIEF DESCRIPTION OF DRAWINGS

[0043] Figure 1 It is a step flow chart of the method of the application;

[0044] Figure 2 It is a schematic diagram of an X-shaped quad-rotor unmanned aerial vehicle and its body coordinate system in step S1 of embodiment 1 of the application;

[0045] Figure 3 It is the working interval of the hollow cup DC motor △p φ , △p θ , △p ψ used in embodiment 1 of the application, using a linear function instead of a quadratic function effect diagram;

[0046] Figure 4 It is a control block diagram of the double-closed-loop system of the position loop and the attitude loop of the quad-rotor unmanned aerial vehicle;

[0047] Figure 5 It is a comparison diagram of position control errors of the quad-rotor unmanned aerial vehicle in hovering experiments under different controllers of the attitude loop in the test example of the application, wherein,

[0048] (a) is the PID controller of the attitude loop;

[0049] (b) is the super-spiral controller of the attitude loop;

[0050] Figure 6 It is a schematic diagram of the three-dimensional reference trajectory used in the trajectory tracking experiment of the quad-rotor unmanned aerial vehicle in the test example of the application;

[0051] Figure 7 It is a comparison diagram of position control errors of the quad-rotor unmanned aerial vehicle in trajectory tracking experiments under different controllers of the attitude loop in the test example of the application, wherein,

[0052] (a) is the PID controller of the attitude loop;

[0053] (b) the super-spiral controller is used for the attitude loop;

[0054] Figure 8 In the test example of the present application, the super-spiral controller is used for the attitude loop of the quadrotor unmanned aerial vehicle, and the angle control error e φ and the sliding surface s φ are shown in the following figures. φ (a) is e φ , and (b) is s w .

[0055] Figure 9 In the test example of the present application, the super-spiral controller is used for the attitude loop of the quadrotor unmanned aerial vehicle, and the three-dimensional comparison diagram of the expected trajectory and the actual trajectory in the tracking experiment is shown in the following figure. DETAILED DESCRIPTION

[0056] The present application will be further illustrated in conjunction with the accompanying drawings and specific embodiments, and it should be understood that the following specific embodiments are only used to illustrate the present application and not to limit the scope of the present application.

[0057] Embodiment 1

[0058] The trajectory tracking control method of the quadrotor unmanned aerial vehicle based on the motor f-PWM model, as shown in the following figure, includes the following steps: Figure 1

[0059] Step S1: considering the internal uncertainty and external unknown disturbance of the system, the dynamics model of the position and attitude of the quadrotor unmanned aerial vehicle is established based on the Newton-Euler method. The model divides the quadrotor unmanned aerial vehicle into a position subsystem and an attitude subsystem, so that the design of the position tracking controller and the design of the attitude tracking controller are as decoupled as possible, and the overall controller design is simplified.

[0060] The dynamics model of the position and attitude of the quadrotor unmanned aerial vehicle is as follows:

[0061]

[0062] wherein r w = [x w y w z w ] T represents the position of the quadrotor unmanned aerial vehicle in the global coordinate system, x w , y w , and z w are the positions of the X, Y, and Z axes, respectively; represents the velocity of the quadrotor unmanned aerial vehicle in the global coordinate system, are the velocities of the X, Y, and Z axes, respectively; m is the mass of the quadrotor unmanned aerial vehicle, represents the total lift generated by the four rotors in the global coordinate system, F g = [00mg] T F represents the gravitational force acting on the quadcopter drone in the global coordinate system, where g is the local gravitational acceleration and F is the gravitational force. a =Kv represents the air resistance experienced by the quadcopter UAV during translational motion in the global coordinate system, K = diag{k x k y k z}, k x ,k y ,k z >0 indicates the drag coefficient of a quadcopter drone; This represents the disturbance vector experienced by the quadcopter UAV during its translational motion in the global coordinate system; η = [φ θ ψ] T denoted by Euler angles in the global coordinate system for a quadcopter UAV, where φ, θ, and ψ represent roll angle, pitch angle, and yaw angle, respectively. This represents the angular velocity of the quadcopter drone in the body coordinate system, where These are the angular velocities of rotation about the X, Y, and Z axes, respectively; J = diag{J xx J yy J zz} represents the inertial matrix of a quadcopter drone, J xx J yy J zz >0; Represents ω b The antisymmetric matrix; This represents the torque of the quadcopter drone in the body coordinate system, where These are the torques about the X, Y, and Z axes, respectively.

[0063] Figure 2 This is a schematic diagram of the X-type quadcopter UAV and its body coordinate system, where OX b Y b Z b This indicates the body coordinate system, where 1, 2, 3, and 4 are rotor designations, OX. w Y w Z w Using the global coordinate system; according to Figure 2 The rotor designations shown for the quadcopter UAV are M4 and f. b It can be represented as: l x l y l z They are respectively The corresponding lever arm, f b =[f1 f2 f3f4] T This indicates the lift generated by the four propellers of the quadcopter drone; U = [f0 △f φ △f θ △f ψ ] T represent four input signals of quadrotor unmanned aerial vehicle system, respectively represent total lift provided by four rotors, lift varied from f0 in roll angle direction, lift varied from f0 in pitch angle direction, lift varied from f0 in yaw angle direction. Input reference signal of attitude loop is η d = [φ d θ d ψ d ] T , where φ d , θ d , ψ d respectively represent expected roll angle, expected pitch angle and expected yaw angle, ψ d is set to 0 by external; for the convenience of designing position loop controller, small angle assumption is adopted, when quadrotor unmanned aerial vehicle is near equilibrium point, sinφ≈φ, cosφ≈1, sinθ≈θ, cosθ≈1, at this time Let position loop virtual control quantity be:

[0064]

[0065] Through calculation, expected roll angle can be expressed as φ d =m(u x sinψ-u y cosψ) / f p , expected pitch angle can be expressed as θ d =m(u x cosψ+u y sinψ) / f p , f i is lift generated by each rotor, i∈{1,2,3,4}, f p =f1+f2+f3+f4 is total lift provided by four rotors of quadrotor unmanned aerial vehicle to be designed under body coordinate system. u x , u y is composed of PD controller combined with ESO.

[0066] In practical application, generally expected position of quadrotor unmanned aerial vehicle under global coordinate system is given; r d =[x d y d z d ] T , x d , y d , z d respectively are expected positions of X, Y, Z axes; with this, position and attitude comprehensive model of quadrotor unmanned aerial vehicle is constructed:

[0067]

[0068] wherein, M2=[0 0 1] T , denotes the lumped disturbance received by the quad-rotor UAV in the translational model in the global coordinate system, denote the lumped disturbance received by the quad-rotor UAV in the X, Y, Z axis respectively, wherein is the external unknown disturbance received by the quad-rotor UAV, k x , k y , k z is the unknown velocity disturbance coefficient; η e = η- η d denotes the attitude error of the quad-rotor UAV.

[0069] Step S2: establishing the f-PWM model of the hollow cup DC motor and mapping the attitude subsystem of the quad-rotor UAV to the PWM domain. Mapping the design of the position and attitude subsystem controller of the quad-rotor UAV to the PWM domain, taking PWM as the output of the controller, which can be directly input to the motor, simplifying the signal conversion process from the controller to the actuator.

[0070] For the hollow cup brush DC motor used in the experiment in the embodiment, when the PWM is limited to 0-1, according to the experimental data, the PWM applied to each DC motor by the UAV and the lift generated by the rotation of the propeller driven by the DC motor satisfy a quadratic function relationship, therefore a monomial quadratic function is adopted to establish the relationship between the input p i and the output thrust f i of each hollow cup DC motor

[0071]

[0072] wherein, i ∈ {1, 2, 3, 4}, and a = 0.091492681, b = 0.067673604 can be obtained through parameter identification. p i is the real output PWM of the final driving motor, which is calculated according to the output of the attitude loop controller by the driving program.

[0073] The part of the dynamics model of the position and attitude of the quad-rotor UAV in step S1 representing the rotation angle velocity in the body coordinate system to the Euler angle conversion relationship can be expanded to obtain

[0074]

[0075] Using the small angle assumption, the following can be obtained

[0076]

[0077] wherein

[0078] Thus the attitude dynamics subsystem of the quadrotor under PWM domain is obtained.

[0079]

[0080] wherein,

[0081]

[0082] D φ , D θ , D ψ may be regarded as the internal lumped disturbance of the attitude subsystem of the quadrotor, respectively. φ , △p θ , △p ψ are PWM signals for adjusting the roll angle, the pitch angle and the yaw angle, respectively.

[0083] In the actual flight process of the quadrotor, the values of △p φ , △p θ , △p ψ are generally less than one twentieth of the total p i input to the DC motor, and a linear function can be used to approximate the relationship between the input p i and the output thrust f i of the hollow cup DC motor, and the linear function is:

[0084] f i = kp i

[0085] wherein k = 0.071. Figure 3 is the effect diagram of using a linear function instead of a quadratic function in the working range of △p φ , △p θ , △p ψ of the hollow cup DC motor used in the experiment in this embodiment, and it can be seen that the two curves almost completely coincide. In addition, since the stm32f405 single-chip microcomputer uses a 16-bit register to store the PWM used to drive the motor, the final values of △p φ , △p θ , △p ψ are all in the range of 0-65535, and after normalization, it has

[0086]

[0087] Step S3: based on the output of the position loop level channel controller, calculate the desired target attitude of the attitude subsystem, and design the attitude tracking controller of the attitude loop in the PWM domain using the super-spiral control method.

[0088] The position loop controller responsible for position control is composed of a PD controller combined with an ESO, and the output of the position loop controller is u o = [u x u y u z ] T , wherein u z corresponds to the input signal f0 of the position and attitude integrated model of the quad-rotor unmanned aerial vehicle in step S1. The desired target attitude of the attitude subsystem is η d = [φ d θ d ψ d ] T , wherein ψ d is set to 0, φ d = m (u x sinψ - u y cosψ) / f p , and θ d = m (u x cosψ + u y sinψ) / f p .

[0089] Design △p φ , △p θ , and △p ψ as the controller The attitude tracking controller of the attitude loop in the PWM domain is:

[0090]

[0091] wherein respectively represent the attitude tracking control amount in the roll angle direction, the attitude tracking control amount in the pitch angle direction, and the attitude tracking control amount in the yaw angle direction; λ φ > 0 is the gain of the attitude tracking controller in the roll angle direction, λ θ > 0 is the gain of the attitude tracking controller in the pitch angle direction, λ ψ > 0 is the gain of the attitude tracking controller in the yaw angle direction; in actual application, the controller parameters should be set as large as possible on the premise that no obvious body jitter occurs during the trajectory tracking of the quad-rotor unmanned aerial vehicle.e φ , e θ , e ψrespectively represent the angle tracking error of roll angle, the angle tracking error of pitch angle, the angle tracking error of yaw angle, respectively represent the sliding surface of roll angle direction, the sliding surface of pitch angle direction, the sliding surface of yaw angle direction. are all obtained by second-order low-pass digital filtering after differentiating d , θ d respectively, and taking as an example, the designed second-order low-pass digital filter is as follows

[0092]

[0093] wherein, k∈Z + ;△t is a control period; diff k is a real-time difference signal, and backward difference is adopted herein; is an intermediate variable required for filtering, and α1, α2, β0, β1, β2 are filtering parameters to be designed, and f c is a cutoff frequency, f s is a posture loop control frequency, and thus β1=2β0, β2=β0,

[0094] The posture tracking controller of the posture loop can be reflected back to the physical layer from the PWM domain, and thus

[0095]

[0096] u φ , u θ , u ψ may correspond to three input signals of the position and posture comprehensive model of the quad-rotor unmanned aerial vehicle in step S1:△f φ , △f θ , △f ψ . In combination with the position tracking controller, a complete quad-rotor unmanned aerial vehicle position loop and posture loop double-closed-loop system control block diagram is formed, as shown in Figure 4 .

[0097] Test Example

[0098] To verify the effectiveness of the trajectory tracking control method of the quadrotor unmanned aerial vehicle based on the f-PWM model of the DC motor, a Crazyflie unmanned aerial vehicle is selected as an experimental platform, and a motion capture system is used to build an unmanned aerial vehicle experimental system to verify the algorithm. The position loop controller of the quadrotor unmanned aerial vehicle adopts a PD combined with ESO disturbance compensation scheme, and the attitude loop controller adopts a cascade PID (P+PI) controller and a super-spiral controller scheme to complete a 10-second constant-height hovering task and a trajectory tracking task. In the PWM domain, the attitude loop controller parameters are selected as λ φ θ ψ = 15. 10-second constant-height hovering:

[0099] Figure 5 (a) (b) show the position error changes with time in the case of using a PID controller and a super-spiral controller in the attitude loop, respectively. The results show that there is actually no big difference in the control effect of the height for the two schemes, and the main improvement is in the control effect of the horizontal position. When the PID controller is used in the attitude loop, the horizontal position error can be basically maintained within 0.005 m after the system enters the steady state, and a small part still exceeds 0.005 m. When the super-spiral controller is used in the attitude loop, the horizontal position error can be completely stabilized and maintained within 0.003 m.

[0100] Trajectory tracking:

[0101] Seven reference points in the global coordinate system are given, and a smooth trajectory composed of six five-order polynomials is planned by using the Minimum Snap method, as shown in Figure 6 .

[0102] First, the quadrotor unmanned aerial vehicle is made to hover at the center point of the three-dimensional eight-character, and after 5 seconds, it starts to track the trajectory to complete the eight-character and then returns to the starting point. The time for tracking one eight-character is limited to 13 seconds, and the average speed is about 0.2 m / s at this time. Figure 7 (a) (b) show the trajectory tracking position error changes with time in the case of using a PID controller and a super-spiral controller in the attitude loop, respectively. The results show that the main improvement is also the control effect of the horizontal position. Compared with the case of using a PID controller in the attitude loop, when a super-spiral controller is used in the attitude loop, there is a smaller steady-state error after the system enters the steady state.

[0103] Taking the roll angle φ as an example, Figure 8 (a) (b) respectively show the convergence of the sliding surface s φ and the roll angle tracking error e​​φ The convergence condition of the sliding mode surface s φ The result shows that the sliding mode surface s Figure 9 The comparison chart of the expected trajectory and the actual trajectory in the three-dimensional space is shown. The results show that the trajectory tracking control method of the four-rotor unmanned aerial vehicle based on the motor f-PWM model designed by the application is effective.

[0104] It should be noted that the above content only illustrates the technical idea of the application and cannot limit the protection scope of the application. For ordinary skilled persons in the art, without departing from the principles of the application, a number of improvements and refinements can be made, which fall within the protection scope of the claims of the application.

Claims

1. A method for trajectory tracking control of a quadrotor unmanned aerial vehicle based on a motor f-PWM model, characterized in that, The method comprises the following steps: S1, establishing a quadrotor unmanned aerial vehicle model: a dynamic model of position and attitude of the quadrotor unmanned aerial vehicle is established based on Newton-Euler method, the model divides the quadrotor unmanned aerial vehicle into a position subsystem and an attitude subsystem, so that a position tracking controller in the position subsystem is decoupled from an attitude tracking controller in the attitude subsystem; S2, pwm mapping: an f-PWM model of the hollow cup DC motor is established, and the attitude subsystem of the quadrotor unmanned aerial vehicle is mapped into a PWM domain; S3, position tracking controller and attitude tracking controller design: the three-axis position tracking controller of the position loop is composed of a PD controller combined with an extended state observer, based on the output of the position loop horizontal channel controller, the expected target attitude of the attitude subsystem is calculated, and an attitude tracking controller of the attitude loop is designed in the PWM domain by using a super-spiral control method; the four inputs of the quadrotor unmanned aerial vehicle are composed of three inputs of the attitude tracking controller and one input of the vertical channel of the position controller, so that the trajectory tracking control is realized. 2.The motor f-PWM model based quadrotor UAV trajectory tracking control method of claim 1, wherein: The dynamic model of position and attitude of the quadrotor unmanned aerial vehicle is specifically: wherein: r d = [x d y d z d ] T represents the desired position of the quad-rotor UAV in the global coordinate system, x d , y d , z d are the desired positions of the X, Y, Z axes, respectively; represents the position error of the quad-rotor UAV; represents the velocity of the quad-rotor UAV in the global coordinate system, are the velocities of the X, Y, Z axes, respectively; g is the gravity acceleration, and η = [φ θ ψ] T represents the Euler angles of the quad-rotor UAV in the global coordinate system, wherein φ, θ, ψ represent the roll angle, the pitch angle, and the yaw angle, respectively; f i is the lift generated by each rotor, i ∈ {1, 2, 3, 4}, and f p = f1+ f2+ f3+ f4is the total lift provided by the four rotors of the quad-rotor UAV to be designed in the body coordinate system, M2= [001] T , represents the lumped disturbance received by the quad-rotor UAV in the global coordinate system in the translational model, represent the lumped disturbances received by the X, Y, Z axes, respectively, wherein is the external unknown disturbance received by the quad-rotor UAV, k x , k y , k z are unknown velocity disturbance coefficients; η d = [φ d θ d ψ d ] T represents the desired Euler angles of the quad-rotor UAV in the global coordinate system, wherein φ d , θ d , ψ d represent the desired roll angle, the desired pitch angle, and the desired yaw angle, respectively; η e = η - η d represents the attitude error of the quad-rotor UAV; represents the angular velocity of the quad-rotor UAV in the body coordinate system, wherein are the angular velocities of rotation around the X, Y, Z axes, respectively; J = diag{J xx J yy J zz} represents the inertia matrix of the quad-rotor UAV, J xx , J yy , J zz > 0; denotes the skew-symmetric matrix of ω b ; denotes the moment of the quadrotor UAV in the body frame, where are the moments about the X, Y, Z axes respectively; l x , l y , l z are the force arms corresponding to f respectively, f b = [f1 f2 f3 f4] T denotes the lift generated by the four propellers of the quadrotor UAV respectively; U = [f0 Δf φ Δf θ Δf ψ ] T denotes the four input signals of the quadrotor UAV system, which respectively represent the lift required to be provided by each rotor when the aircraft is kept hovering, the lift varied in the roll angle direction on the basis of U1, the lift varied in the pitch angle direction on the basis of U1, and the lift varied in the yaw angle direction on the basis of U1.

3. The motor f-PWM model based quadrotor UAV trajectory tracking control method of claim 2, wherein: The step S2 specifically comprises the following steps: S21: a one-dimensional quadratic function is used to establish the relationship between the PWM input of each hollow cup DC motor and the thrust output as follows: wherein a, b are constants, i∈{1, 2, 3, 4}, p i is the PWM input to the i-th hollow cup DC motor, f i is the thrust output of the i-th hollow cup DC motor; S22: the part of the dynamic model of position and attitude of the quadrotor unmanned aerial vehicle in step S1, which represents the rotation angular velocity in the body coordinate system to the Euler angle conversion relationship, is expanded to obtain: By using the small angle assumption, the following can be obtained wherein S23: the attitude dynamics subsystem of the quadrotor unmanned aerial vehicle in the PWM domain is obtained Wherein, D φ , D θ , D ψ are the internal lumped disturbances of the quadrotor attitude subsystem, respectively φ , Δp θ , Δp ψ are the PWM signals used to adjust the roll, pitch and yaw angles, respectively.

4. The motor f-PWM model based quadrotor UAV trajectory tracking control method of claim 3, wherein: The position loop controller responsible for position control in step S3 is composed of a PD controller combined with an ESO, and the output of the position loop controller is u o = [u x u y u z ] T , and the expected target attitude of the attitude subsystem is η d = [φ d θ d ψ d ] T , wherein ψ d is set to 0, φ d = m(u x sinψ-u y cosψ) / f p , and θ d = m(u x cosψ+u y sinψ) / f p ; Design Δp φ , Δp θ , Δp ψ respectively, the controller The attitude tracking controller of the attitude loop under the PWM domain is: where y = sgn(x) is a sign function; respectively represent the attitude tracking control quantity in the roll angle direction, the attitude tracking control quantity in the pitch angle direction, and the attitude tracking control quantity in the yaw angle direction; λ φ > 0 is a roll angle direction attitude tracking controller gain, λ θ > 0 is a pitch angle direction attitude tracking controller gain, λ ψ > 0 is a yaw angle direction attitude tracking controller gain; e φ , e θ , e ψ respectively represent the angle tracking error of the roll angle, the angle tracking error of the pitch angle, and the angle control error of the yaw angle, respectively represent the sliding mode surface in the roll angle direction, the sliding mode surface in the pitch angle direction, and the sliding mode surface in the yaw angle direction; are all obtained by differentiating φ d , θ d , and then passing through a second-order low-pass digital filter.

5. The motor f-PWM model based quadrotor UAV trajectory tracking control method of claim 4, wherein: The obtained The second-order low-pass digital filter used is specifically: wherein k∈Z + ; Δt is a control period; diff k is a real-time difference signal; is an intermediate variable required for filtering, and α1, α2, β0, β1, β2 are filter parameters to be designed.

6. A computer-readable storage medium, characterized in that: The computer program is stored on the computer program, and the computer program is executed by the processor to realize the trajectory tracking control method of the quadrotor unmanned aerial vehicle based on the motor f-PWM model according to any one of claims 1-5.

7. A computer device, characterized by: The computer program is stored on the computer program, and the computer program is executed by the processor to realize the trajectory tracking control method of the quadrotor unmanned aerial vehicle based on the motor f-PWM model according to any one of claims 1-5. The computer program is stored on the computer program, and the computer program is executed by the processor to realize the trajectory tracking control method of the quadrotor unmanned aerial vehicle based on the motor f-PWM model according to any one of claims 1-5. ​

Images