Quadrotor unmanned aerial vehicle trajectory tracking control method and device based on motor f-PWM model

Abstract

The invention discloses a quadrotor unmanned aerial vehicle trajectory tracking control method and device based on a motor f-PWM model, and the method comprises the steps: firstly considering the internal uncertainty and external unknown interference of a system, and building a kinetic model of the position and attitude of a quadrotor unmanned aerial vehicle based on a Newton-Euler method; then mapping the position and attitude subsystem of the quadrotor unmanned aerial vehicle to a PWM domain based on an f-PWM model of a hollow cup direct current motor; and finally, based on the output of the position loop horizontal channel controller, the tracking target attitude of the attitude subsystem is solved, and an attitude tracking controller of the attitude loop is designed by adopting a superhelix control method in a PWM (Pulse-Width Modulation) domain. According to the invention, the trajectory tracking accuracy of the quad-rotor unmanned aerial vehicle is improved, and the signal conversion efficiency of the control signal from the controller to the execution mechanism is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of flight control of a quad-rotor unmanned aerial vehicle, and in particular relates to a quad-rotor unmanned aerial vehicle trajectory tracking control method and equipment based on a motor f-PWM model. Background Art

[0002] With the rapid development of UAV technology, quad-rotor UAVs have been widely used in many fields due to their flexibility and stability. By integrating sensors (such as IMU, GPS, visual sensors, etc.), UAVs can obtain position information and attitude data in real time to monitor and adjust the flight status. Position and attitude tracking control, as one of the key technologies for autonomous flight of UAVs, is crucial to improving their navigation accuracy and flight stability. However, position and attitude tracking faces many challenges, including interference from dynamic environments, sensor noise, and real-time algorithms. When quad-rotor UAVs move quickly or are affected by wind, the estimation accuracy of position and attitude is easily affected, resulting in unstable flight. On the other hand, what attitude tracking control needs most is fast tracking to ensure that the UAV can respond quickly to environmental changes and maintain stable flight. Traditional PID control methods often fail to meet the requirements of high precision in these cases, and often face problems such as slow response and poor anti-interference ability, which limits the performance of UAVs.

[0003] In recent years, the super-helical control algorithm in second-order sliding mode control has gradually attracted attention. This algorithm not only has a limited time characteristic and can quickly converge to the target state in a short time, but also can effectively suppress external interference, thereby achieving fast and stable attitude tracking in complex environments. Compared with traditional control methods, the super-helical algorithm significantly improves the control effect of quadrotor drones in position and attitude tracking, allowing drones to maintain high-precision flight status in dynamically changing environments.

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

[0005] However, the current research on attitude tracking of quadrotor drones based on super-helical control algorithms is still relatively limited, especially in terms of promotion and optimization in practical applications. Therefore, in-depth exploration of the technological development in this field can not only promote the innovation of drone attitude control, but also provide 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 broader application prospects to the control methods of quadrotor drones and promote the further development of drone technology. Summary of the invention

[0006] The present invention is aimed at the problem that the traditional double closed-loop PID control algorithm has a long transition process of drone position stability and a large steady-state error when moving quickly. It provides a quad-rotor drone trajectory tracking control method and equipment based on the motor f-PWM model. First, the internal uncertainty of the system and the external unknown interference are considered, and the dynamic model of the position and attitude of the quad-rotor drone is established based on the Newton-Euler method; then, the position and attitude subsystem of the quad-rotor drone 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 horizontal channel controller of the position loop, the tracking target attitude of the attitude subsystem is solved, and the super-helical control method is used in the PWM domain to design the attitude tracking controller of the attitude loop. The present invention improves the accuracy of the trajectory tracking of the quad-rotor drone and improves the signal conversion efficiency of the control signal from the controller to the actuator.

[0007] In order to achieve the above object, the technical solution adopted by the present invention is: a quad-rotor drone trajectory tracking control method based on a motor f-PWM model, characterized in that it includes the following steps:

[0008] S1. Establishing a quadrotor drone model: establishing a dynamic model of the position and attitude of the quadrotor drone based on the Newton-Euler method, wherein the model divides the quadrotor drone into a position subsystem and an attitude subsystem, so that the position tracking controller in the position subsystem is decoupled from the attitude tracking controller in the attitude subsystem;

[0009] S2, PWM mapping: Establish the f-PWM model of the hollow cup DC motor and map the attitude subsystem of the quadrotor drone to the PWM domain;

[0010] S3. Design of position tracking controller and attitude tracking controller: The three-axis position tracking controller of the position loop is composed of a PD controller combined with an ESO (expanded state observer). Based on the output of the horizontal channel controller of the position loop, the expected target attitude of the attitude subsystem is calculated. The super-helical 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 constitute the four-way input of the quadrotor UAV to realize trajectory tracking control.

[0011] As an improvement of the present invention, the dynamic model of the position and attitude of the quadrotor drone is specifically:

[0012]

[0013] Where: r d =[x d y d z d ] T represents the desired position of the quadrotor drone in the global coordinate system, x d ,y d 、z d are the desired positions of the X, Y, and Z axes respectively; Represents the position error of the quadrotor drone; represents the speed of the quadrotor drone in the global coordinate system, are the velocities of the X, Y, and Z axes respectively; g is the acceleration due to gravity, η=[φ θ ψ] T represents the Euler angle of the quadrotor drone in the global coordinate system, where φ, θ, and ψ represent the roll angle, pitch angle, and yaw angle, respectively; f i is the lift generated by each rotor, i∈{1,2,3,4}, f p =f1+f2+f3+f4 is the total lift provided by the four rotors of the quadcopter to be designed in the body coordinate system, M2 = [0 0 1] T , It represents the lumped disturbance of the quadrotor UAV in the translational model in the global coordinate system. Respectively represent the total interference received on the X, Y, and Z axes, where is the external unknown interference to the quadrotor drone, k x , k y , k z is the unknown speed interference coefficient; η d =[φ d θ d ψ d ] T represents the expected Euler angle of the quadrotor drone in the global coordinate system, where φ d ,θ d , d They represent the desired roll angle, the desired pitch angle and the desired yaw angle respectively; η e =η-η d Represents the attitude error of the quadrotor drone; represents the angular velocity of the quadrotor drone in the body coordinate system, where are the angular velocities of rotation around the X, Y, and Z axes respectively; J = diag{J xx J yy J zz} represents the inertia matrix of the quadrotor drone, J xx , J yy , J zz >0; Represents ω b The antisymmetric matrix of ; Represents the torque of the quadrotor drone in the body coordinate system, where are the moments about the X, Y, and Z axes respectively; l x , l y , l z They are The corresponding force arm, f b =[f1 f2 f3 f4] T Indicates the lift generated by the four propellers of a quadcopter drone; U=[f0 △f φ △f θ △f ψ ] T Represents the four input signals of the quadcopter UAV system, which respectively represent the lift that each rotor needs to provide when the aircraft maintains hovering, the lift that changes in the roll angle direction based on U1, the lift that changes in the pitch angle direction based on U1, and the lift that changes in the yaw angle direction based on U1.

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

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

[0016]

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

[0018] S22: Expand the part of the dynamic model of the position and attitude of the quadrotor drone in step S1 that represents the conversion relationship from the rotational angular velocity in the body coordinate system to the Euler angle to obtain:

[0019]

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

[0021]

[0022] in

[0023] S23: Get the attitude dynamics subsystem of the quadrotor drone in the PWM domain

[0024]

[0025] in,

[0026]

[0027] D φ , D θ , D ψ are the total disturbances inside the attitude subsystem of the quadrotor drone, △p φ , △p θ , △p ψ They are PWM signals used to adjust the roll angle, pitch angle and yaw angle respectively.

[0028] As another improvement of the present invention, in 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 posture of the posture subsystem is η d =[φ d θ d ψ d ] T , where ψ d Set to 0, φ d =m(u x sinψ-u y cosψ) / f p ,θ d =m(u x cosψ+u y sinψ) / f p ;

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

[0030]

[0031] Where y = sgn(x) is the sign function; They 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 roll angle direction attitude tracking controller gain, λ θ >0 is the pitch angle direction attitude tracking controller gain, λ ψ >0 is the attitude tracking controller gain in the yaw angle direction; e φ 、e θ 、e ψ They 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. They represent the sliding surface in the rolling angle direction, the sliding surface in the pitch angle direction, and the sliding surface in the yaw angle direction respectively; All by φ d ,θ d After being differentiated respectively, the signals are obtained through a second-order low-pass digital filter.

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

[0033]

[0034] Where k∈Z + ;△t is the control period;diff k It is a real-time differential signal; are the intermediate variables required for filtering, and α1, α2, β0, β1, and β2 are the filtering parameters to be designed.

[0035] In order to achieve the above-mentioned purpose, the technical solution also adopted by the present invention is: a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, it implements the trajectory tracking control method of a four-rotor drone based on the motor f-PWM model as described in any one of claims 1-5.

[0036] In order to achieve the above object, the present invention also adopts a technical solution: a computer device, comprising

[0037] A memory for storing instructions;

[0038] A processor is used to execute the instructions so that the computer device executes the trajectory tracking control method of a quad-rotor unmanned aerial vehicle based on a motor f-PWM model as described in any one of claims 1 to 5.

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

[0040] (1) In terms of position tracking control, the traditional ESO combined with PD controller control scheme is adopted, and the super-helical controller is applied to the attitude tracking control of the quadrotor UAV. Compared with the general PID attitude tracking control scheme, the trajectory tracking error of the quadrotor UAV is significantly reduced, and it has universality.

[0041] (2) The controller design method based on the f-PWM model uses 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.

[0042] (3) The technical concept of the f-PWM model proposed in the present invention is applicable to other technical fields that hope to simplify part of the control process, and has broad application prospects. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 is a flow chart of the steps of the method of the present invention;

[0044] Figure 2 This is a schematic diagram of an X-shaped quad-rotor drone and its body coordinate system in step S1 of embodiment 1 of the present invention;

[0045] Figure 3 △p is the coreless DC motor used in Example 1 of the present invention φ , △p θ , △p ψ In the working range, the effect diagram is that a linear function is used instead of a quadratic function;

[0046] Figure 4 This is the control block diagram of the dual closed-loop system of the position loop and attitude loop of the quadrotor drone;

[0047] Figure 5 This is a comparison chart of the position control error of the quadcopter UAV hovering experiment when the attitude loop uses different controllers in the test example of the present invention, where:

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

[0049] (b) Using a super-helical controller for the attitude loop;

[0050] Figure 6 Schematic diagram of the three-dimensional reference trajectory used in the trajectory tracking experiment of the quad-rotor drone in the test example of the present invention;

[0051] Figure 7 This is a comparison chart of the position control error of the quadrotor UAV trajectory tracking experiment under different controllers used in the attitude loop in the test example of the present invention, where:

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

[0053] (b) Using a super-helical controller for the attitude loop;

[0054] Figure 8 In the test example of the present invention, the angle control error e in the tracking experiment is obtained when the attitude loop of the quad-rotor drone adopts a super-helical controller. φ With sliding surface s φ Schematic diagram of convergence over time, where (a) is e φ , (b) is s φ ;

[0055] Fig. 9 This is a three-dimensional comparison diagram of the expected trajectory and the actual trajectory in the tracking experiment when the attitude loop of the quadrotor drone adopts a super-helical controller in the test example of the present invention. DETAILED DESCRIPTION

[0056] The present invention will be further explained below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention.

[0057] Example 1

[0058] The trajectory tracking control method of quadrotor UAV based on motor f-PWM model, such as Figure 1 As shown, the following steps are included:

[0059] Step S1: Considering the internal uncertainty of the system and the external unknown interference, a dynamic model of the position and attitude of the quadrotor drone is established based on the Newton-Euler method. The model divides the quadrotor drone into a position subsystem and an attitude subsystem, so that the design of the position tracking controller is decoupled from the design of the attitude tracking controller as much as possible, simplifying the controller design as a whole.

[0060] The dynamic model of the position and attitude of the quadrotor drone is:

[0061]

[0062] Where: r w =[x w y w z w ] T Indicates the position of the quadrotor drone in the global coordinate system, x w ,y w 、z w They are the X, Y, and Z axis positions respectively; represents the speed of the quadrotor drone in the global coordinate system, are the speeds of the X, Y, and Z axes respectively; m is the mass of the quadrotor drone, represents the total lift generated by the four rotors in the global coordinate system, F g =[00mg] T represents the gravity of the quadcopter in the global coordinate system, g is the local gravity acceleration, F a =Kv represents the air resistance of the quadrotor UAV during translation 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 quadrotor drone; Represents the disturbance vector of the translational motion of the quadrotor drone in the global coordinate system; η=[φ θ ψ] T represents the Euler angle of the quadrotor drone in the global coordinate system, where φ, θ, and ψ represent the roll angle, pitch angle, and yaw angle, respectively; represents the angular velocity of the quadrotor drone in the body coordinate system, where are the angular velocities of rotation around the X, Y, and Z axes respectively; J = diag{J xx J yy J zz} represents the inertia matrix of the quadrotor drone, J xx , J yy , J zz >0; Represents ω b The antisymmetric matrix of ; Represents the torque of the quadrotor drone in the body coordinate system, where are the moments about the X, Y, and Z axes respectively.

[0063] Figure 2 It is a schematic diagram of the X-type quadrotor drone and its body coordinate system, where OX b Y b Z b Indicates the body coordinate system, 1, 2, 3, 4 are the rotor numbers, OX w Y w Z w is the global coordinate system; according to Figure 2 The rotor numbers of the quadcopter shown are M4 and f b It can be expressed as: l x , l y , l z They are The corresponding force arm, f b =[f1f2 f3 f4] T , represents the lift generated by the four propellers of the quadcopter; U=[f0△f φ △f θ △f ψ ] T The four input signals of the quadrotor UAV system are the total lift provided by the four rotors, the lift in the roll angle direction that varies based on f0, the lift in the pitch angle direction that varies based on f0, and the lift in the yaw angle direction that varies based on f0. The input reference signal of the attitude loop is η d =[φ d θ d ψ d ] T , where φ d ,θ d , d They represent the desired roll angle, the desired pitch angle and the desired yaw angle, respectively, ψ d It is set to 0 externally; to facilitate the design of the position loop controller, a small angle assumption is adopted. When the quadrotor drone is near the balance point, sinφ≈φ, cosφ≈1, sinθ≈θ, cosθ≈1. Let the virtual control quantity of the position loop be:

[0064]

[0065] By calculation, the desired roll angle can be expressed as φ d =m(u x sinψ-u y cosψ) / f p , the desired pitch angle can be expressed as θ d =m(u x cosψ+u y sinψ) / f p , f i is the lift generated by each rotor, i∈{1,2,3,4}, f p =f1+f2+f3+f4 is the total lift provided by the four rotors of the quadcopter to be designed in the body coordinate system. x ,u y It consists of a PD controller combined with an ESO.

[0066] In practical applications, the expected position of the quadrotor drone in the global coordinate system is generally given; d =[x d y d z d ] T , x d ,y d 、z d are the expected positions of the X, Y, and Z axes respectively; this is used to construct a comprehensive position and attitude model of the quadrotor drone:

[0067]

[0068] in, M2=[0 0 1] T , It represents the lumped disturbance of the quadrotor UAV in the translational model in the global coordinate system. Respectively represent the total interference received on the X, Y, and Z axes, where is the external unknown interference to the quadrotor drone, k x , k y , k z is the unknown speed interference coefficient; η e =η-η d Represents the attitude error of the quadrotor drone.

[0069] Step S2: Establish the f-PWM model of the coreless DC motor and map the attitude subsystem of the quadrotor drone to the PWM domain. Map the design of the position and attitude subsystem controller of the quadrotor drone to the PWM domain, and use 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 coreless brushless DC motor used in the experiment in this embodiment, when the PWM is limited between 0 and 1, according to the experimental data, the PWM applied by the drone to each DC motor and the lift generated by the propeller rotation satisfy the quadratic function relationship. Therefore, the following quadratic function is used to establish the p input to each coreless DC motor: i The output thrust f i The relationship between

[0071]

[0072] Among them, i∈{1,2,3,4}, through parameter identification, we can get a=0.091492681,b=0.067673604. i The PWM that is actually output by the final drive motor is calculated by the driver according to the controller output of the attitude loop.

[0073] Expanding the part of the dynamic model of the position and attitude of the quadrotor drone in step S1 that represents the conversion relationship from the rotational angular velocity in the body coordinate system to the Euler angle can obtain

[0074]

[0075] Using the small angle assumption, we can get

[0076]

[0077] in

[0078] Therefore, the attitude dynamics subsystem of the quadrotor UAV in the PWM domain is obtained.

[0079]

[0080] in,

[0081]

[0082] D φ , D θ , D ψ can be regarded as the internal lumped disturbance of the attitude subsystem of the quadrotor drone, △p φ , △p θ , △p ψ They are PWM signals used to adjust the roll angle, pitch angle and yaw angle respectively.

[0083] In the actual flight process of the quadcopter, the △p used for attitude adjustment is φ , △p θ , △p ψ The value is generally less than the total p input to the DC motor. i One twentieth of the p value, a linear function can be used to approximate the input p value of each coreless DC motor. i The output thrust f i The relationship between , the linear function is:

[0084] f i =kp i

[0085] Where k = 0.071. Figure 3 In this embodiment, the hollow cup DC motor △p used in the experiment φ , △p θ , △p ψ In the working range, the effect of using a linear function instead of a quadratic function is shown in the figure. It can be seen that the two curves are almost completely overlapped. In addition, since the stm32f405 microcontroller uses a 16-bit register to store the PWM used to drive the motor, △p φ , △p θ , △p ψ The final value ranges from 0 to 65535. After normalization, we have

[0086]

[0087] Step S3: Based on the output of the horizontal channel controller of the position loop, the desired target attitude of the attitude subsystem is solved, and the attitude tracking controller of the attitude loop is designed by adopting the super-helical control method in the PWM domain.

[0088] The position loop controller responsible for position control is composed of a PD controller combined with an ESO. The output of the position loop controller is u o =[u x u y u z ] T , where u z The input signal f0 of the integrated model of the position and attitude of the quadrotor drone in step S1 is corresponding to the input signal f0 of the integrated model of the position and attitude of the quadrotor drone in step S1. The expected target attitude of the attitude subsystem is η d =[φ d θ d ψ d ] T , where ψ d Set to 0, φ d =m(u x sinψ-u y cosψ) / f p ,θ d =m(u x cosψ+u y sinψ) / f p .

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

[0090]

[0091] in They 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 roll angle direction attitude tracking controller gain, λ θ >0 is the pitch angle direction attitude tracking controller gain, λ ψ >0 is the attitude tracking controller gain in the yaw angle direction; in practical applications, the controller parameters should be set as large as possible without obvious body vibration during the trajectory tracking of the quadcopter. φ 、e θ 、e ψThey represent the angle tracking error of the roll angle, the angle tracking error of the pitch angle, and the angle tracking error of the yaw angle, respectively. They represent the sliding surface in the rolling angle direction, the sliding surface in the pitch angle direction, and the sliding surface in the yaw angle direction respectively. All by φ d ,θ d After being differentiated, they are filtered through a second-order low-pass digital filter to obtain For example, the designed second-order low-pass digital filter is as follows

[0092]

[0093] Where k∈Z + ;△t is the control period;diff k It is a real-time differential signal, and backward differential is used here; is the intermediate variable required for filtering, α1, α2, β0, β1, β2 are the filter parameters to be designed, let f c is the cut-off frequency, f s is the attitude loop control frequency, then β1=2β0,β2=β0,

[0094] The attitude tracking controller of the attitude loop can be mapped back from the PWM domain to the physical level, and then:

[0095]

[0096] u φ 、u θ 、u ψ It can correspond to the three input signals of the comprehensive model of the position and attitude of the quadrotor drone in step S1: △f φ , △f θ , △f ψ Combined with the position tracking controller, a complete quadrotor drone position loop and attitude loop double closed-loop system control block diagram is formed, as shown in Figure 4 shown.

[0097] Test Case

[0098] In order to verify the effectiveness of the proposed quadrotor UAV trajectory tracking control method based on the DC motor f-PWM model, the Crazyfl ie UAV was selected as the experimental platform, and the UAV experimental system was built with the help of the motion capture system to verify the algorithm. The controllers of the quadrotor position loop all use the PD combined with ESO interference compensation solution, and the controllers of the attitude loop use the cascade PID (P+PI) controller and the super-helical controller solution to complete the 10-second fixed-height hovering task and trajectory tracking task. In terms of the selection of attitude loop controller parameters in the PWM domain, let λ φ =λ θ =λ ψ =15. 10 seconds fixed height hovering:

[0099] Figure 5 (a)(b) shows the change of position error over time when the attitude loop uses PID controller and super-helical controller respectively. The results show that there is actually not much difference in the control effect of the two schemes for the height. The main improvement is in the control effect of the horizontal position. When the attitude loop uses the PID controller, after the system enters the steady state, the horizontal position error can barely be maintained within 0.005m, and a small part still exceeds 0.005m. When the attitude loop uses the super-helical controller, the horizontal position error can be completely stably maintained within 0.003m.

[0100] Trajectory tracking:

[0101] Given seven reference points in the global coordinate system, the Minimum Snap method is used to plan a smooth trajectory consisting of six segments of fifth-order polynomials passing through all reference points, such as Figure 6 shown.

[0102] First, the quadcopter is ordered to hover at a fixed altitude at the center point of the three-dimensional eight-shaped figure. After 5 seconds, it begins to track the trajectory, circles the eight-shaped figure, and then returns to the starting point. The time limit for circling a figure of eight is 13 seconds, at which time the average speed is about 0.2m / s. Figure 7 (a)(b) shows the change of trajectory tracking position error over time when the attitude loop uses PID controller and super helical controller respectively. The results show that the main improvement is also the control effect of the horizontal position. Compared with the attitude loop using PID controller, when the attitude loop uses super helical controller, there is a smaller steady-state error after the system enters steady state.

[0103] Taking the roll angle φ as an example, Figure 8 (a) and (b) show the sliding surface s when the attitude loop of the quadrotor drone uses a super-helical controller during the trajectory tracking experiment. φ Convergence of the rolling angle tracking error eφ The results show that the sliding surface s φ It can quickly converge to 0 and remain near 0. Fig. 9 The comparison diagram of the expected trajectory and the actual trajectory is shown in three-dimensional space. The above results show that the trajectory tracking control method of the quad-rotor drone based on the motor f-PWM model designed in the present invention is effective.

[0104] It should be noted that the above content only illustrates the technical idea of ​​the present invention and cannot be used to limit the protection scope of the present invention. For ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications all fall within the protection scope of the claims of the present invention.

Claims

1. A quadrotor UAV trajectory tracking control method based on a motor f-PWM model, characterized in that: The steps include: S1. Establishing a quadrotor drone model: Establishing a dynamic model of the position and attitude of a quadrotor drone based on the Newton-Euler method. The model divides the quadrotor drone into a position subsystem and an attitude subsystem, so that the position tracking controller in the position subsystem is decoupled from the attitude tracking controller in the attitude subsystem. S2, PWM mapping: Establish the f-PWM model of the hollow cup DC motor and map the attitude subsystem of the quadrotor drone to the PWM domain; S3. Design of position tracking controller and attitude tracking controller: The three-axis position tracking controller of the position loop is composed of a PD controller combined with an expanded state observer. Based on the output of the horizontal channel controller of the position loop, the desired target attitude of the attitude subsystem is solved. The superhelical control method is adopted 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 constitute the four-way input of the quadrotor UAV to realize trajectory tracking control.

2. The quadrotor drone trajectory tracking control method based on the motor f-PWM model as claimed in claim 1, characterized in that: The dynamic model of the position and attitude of the quadrotor drone is specifically: Where: r d =[x d y d z d ] T represents the desired position of the quadrotor drone in the global coordinate system, x d ,y d 、z d are the desired positions of the X, Y, and Z axes respectively; Represents the position error of the quadrotor drone; represents the speed of the quadrotor drone in the global coordinate system, are the velocities of the X, Y, and Z axes respectively; g is the acceleration due to gravity, η=[φθψ] T represents the Euler angle of the quadrotor drone in the global coordinate system, where φ, θ, and ψ represent the roll angle, pitch angle, and yaw angle, respectively; f i is the lift generated by each rotor, i∈{1,2,3,4}, f p =f1+f2+f3+f4 is the total lift provided by the four rotors of the quadcopter to be designed in the body coordinate system, M2=[001] T , It represents the lumped disturbance of the quadrotor UAV in the translational model in the global coordinate system. Respectively represent the total interference received on the X, Y, and Z axes, where is the external unknown interference to the quadrotor drone, k x , k y , k z is the unknown speed interference coefficient; η d =[φ d θ d ψ d ] T represents the expected Euler angle of the quadrotor drone in the global coordinate system, where φ d ,θ d , d They represent the desired roll angle, the desired pitch angle and the desired yaw angle respectively; η e =η-η d Represents the attitude error of the quadrotor drone; represents the angular velocity of the quadrotor drone in the body coordinate system, where are the angular velocities of rotation around the X, Y, and Z axes respectively; J = diag{J xx J yy J zz } represents the inertia matrix of the quadrotor drone, J xx , J yy , J zz >0; Represents ω b The antisymmetric matrix of ; Represents the torque of the quadrotor drone in the body coordinate system, where are the moments about the X, Y, and Z axes respectively; l x , l y , l z They are The corresponding force arm, f b =[f1 f2 f3 f4] T Indicates the lift generated by the four propellers of a quadcopter drone; U=[f0 Δf φ Δf θ Δf ψ ] T Represents the four input signals of the quadcopter UAV system, which respectively represent the lift that each rotor needs to provide when the aircraft maintains hovering, the lift that changes in the roll angle direction based on U1, the lift that changes in the pitch angle direction based on U1, and the lift that changes in the yaw angle direction based on U1.

3. The quadrotor drone trajectory tracking control method based on the motor f-PWM model as claimed in claim 2, characterized in that: The step S2 specifically includes the following steps: S21: The relationship between the PWM input and the output thrust of each coreless DC motor is established using the following quadratic function: Among them, a and b are constants, i∈{1,2,3,4}, p i is the PWM input to the i-th coreless DC motor, f i is the thrust output by the i-th coreless DC motor; S22: Expand the part of the dynamic model of the position and attitude of the quadrotor drone in step S1 that represents the conversion relationship from the rotational angular velocity in the body coordinate system to the Euler angle to obtain: Using the small angle assumption, we can get in S23: Get the attitude dynamics subsystem of the quadrotor drone in the PWM domain in, D φ , D θ , D ψ are the total disturbances inside the attitude subsystem of the quadrotor drone, Δp φ , Δp θ , Δp ψ They are PWM signals used to adjust the roll angle, pitch angle and yaw angle respectively.

4. The quadrotor drone trajectory tracking control method based on the motor f-PWM model as claimed in claim 3, characterized in that: In 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 posture of the posture subsystem is η d =[φ d θ d ψ d ] T , where ψ d Set to 0, φ d =m(u x sinψ-u y cosψ) / f p ,θ d =m(u x cosψ+u y sinψ) / f p ; Design Δp φ , Δp θ , Δp ψ Controller The attitude tracking controller of the attitude loop in the PWM domain is: Where y = sgn(x) is the sign function; They 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 roll angle direction attitude tracking controller gain, λ θ >0 is the pitch angle direction attitude tracking controller gain, λ ψ >0 is the attitude tracking controller gain in the yaw angle direction; e φ 、e θ 、e ψ They 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. They represent the sliding surface in the rolling angle direction, the sliding surface in the pitch angle direction, and the sliding surface in the yaw angle direction respectively; All by φ d ,θ d After being differentiated respectively, the signals are obtained through a second-order low-pass digital filter.

5. The quadrotor drone trajectory tracking control method based on the motor f-PWM model as claimed in claim 4, characterized in that: Said to obtain The second-order low-pass digital filter used is specifically: Where k∈Z + ; Δt is the control period; diff k It is a real-time differential signal; are the intermediate variables required for filtering, and α1, α2, β0, β1, and β2 are the filtering parameters to be designed.

6. A computer-readable storage medium, characterized in that: A computer program is stored thereon, and when the computer program is executed by a processor, a trajectory tracking control method for a quad-rotor unmanned aerial vehicle based on a motor f-PWM model as described in any one of claims 1 to 5 is implemented.

7. A computer device, characterized in that: include A memory for storing instructions; A processor is used to execute the instructions so that the computer device executes the trajectory tracking control method of a quad-rotor unmanned aerial vehicle based on a motor f-PWM model as described in any one of claims 1 to 5.

Images