Trajectory tracking method and system of quad-rotor unmanned aerial vehicle under attitude limitation

Abstract

The invention discloses a trajectory tracking method and system under attitude limitation of a quad-rotor unmanned aerial vehicle, and the method comprises the steps: obtaining a kinetic model of the quad-rotor unmanned aerial vehicle, and obtaining a first state-space equation set and a second state-space equation set according to the kinetic model; designing an expansion state observer used for observing system state quantity and expansion state quantity according to the first state space equation set and the second state space equation set; designing a trajectory tracking controller according to the extended state observer; and acquiring an ideal value of attitude tracking, designing an auxiliary system according to the trajectory tracking controller, and performing attitude limitation on the ideal value according to a preset attitude range through the auxiliary system. The method can effectively limit the inclination angle attitude of the body of the quadrotor unmanned aerial vehicle in the flight process, enables the monitored target equipment to be always in the effective visual range of the camera below the body, improves the robustness of target positioning, achieves the autonomous and safe inspection of the target equipment, and can be widely applied to the technical field of intelligent robots.

Description

Technical Field

[0001] The present application relates to the field of intelligent robot technology, and in particular to a trajectory tracking method and system for a quad-rotor unmanned aerial vehicle (UAV) under attitude constraints. Background Art

[0002] Robotics technology has advanced rapidly in recent years, particularly the development and application of quadcopter drones, which have begun to be used in the safety monitoring of electromechanical equipment. However, due to the inherent structural design of current quadcopter drones and the performance of their visual information collection devices, such as cameras, the inclination angles generated during flight can cause the target device to exceed the effective recognition range of the camera, making it impossible to continuously identify and locate the target object, effectively preventing autonomous safety inspections. Furthermore, existing safety inspection quadcopter drones still require manual control by personnel or the installation of expensive gimbal equipment. Summary of the Invention

[0003] In order to solve the above technical problems, the purpose of the present invention is to provide a trajectory tracking method and system for a quadrotor drone under attitude constraints, which can ensure that the target device is always within the effective recognition range of the camera.

[0004] To achieve the above objectives, one aspect of an embodiment of the present application provides a trajectory tracking method for a quadrotor drone under attitude constraints, comprising the following steps:

[0005] Obtaining a dynamic model of the quadrotor drone, and obtaining a first state-space equation group and a second state-space equation group according to the dynamic model;

[0006] Designing an extended state observer based on the first state space equation group and the second state space equation group, wherein the extended state observer is used to observe system state quantities and extended state quantities;

[0007] Designing a trajectory tracking controller based on the extended state observer;

[0008] An ideal value for posture tracking is obtained, an auxiliary system is designed according to the trajectory tracking controller, and the auxiliary system is used to perform posture restrictions on the ideal value according to a preset posture range.

[0009] In some embodiments, obtaining the dynamic model of the quadrotor drone specifically includes:

[0010] Obtaining the mass, moment of inertia, linear velocity conversion matrix, angular velocity conversion matrix, force input, and torque input of the quadrotor drone;

[0011] Obtaining the dynamic model according to the mass, the moment of inertia, the linear velocity conversion matrix, the angular velocity conversion matrix, the force input, and the torque input;

[0012] The kinetic model is:

[0013]

[0014] Where ξ represents the three-axis translation position and ξ = [x, y, z] T , v represents the three-axis translation linear velocity and v=[v x , v y , v z ] T , η represents the three-axis Euler angle and η=[φ,θ,ψ] T , ω represents the three-axis angular velocity and ω=[p,q,r] T , m represents the mass, g represents the acceleration due to gravity, R t represents the linear velocity conversion matrix, R r represents the angular velocity conversion matrix, d ξ represents the three-axis translation disturbance torque, d η represents the three-axis rotation interference torque, J represents the moment of inertia, u ξ represents the force input, u η represents the torque input.

[0015] In some embodiments, obtaining the first state-space equations and the second state-space equations according to the dynamic model specifically includes:

[0016] Define state space variables, disturbance state variables, virtual control input variables, state space input variables and state change matrix;

[0017] Converting the dynamic model from a second-order differential equation to a first-order differential equation according to the state-space variables, the disturbance state variables, the virtual control input variables, the state-space input variables, and the state change matrix to obtain the first state-space equation group and the second state-space equation group;

[0018] The first state space equations are:

[0019]

[0020] The second state space equations are:

[0021]

[0022] Wherein, x1, x2, x3, x4, x5 and x6 represent the state space variables, d1, d2, d3, d4 and d5 represent the interference state variables, g1, g2, g3, g4 and g5 represent the state change matrix, represents the virtual control input variable, and represents the state-space input variables.

[0023] In some embodiments, designing an extended state observer based on the first state-space equations and the second state-space equations specifically includes:

[0024] Performing a linear transformation on the first state space equation group to obtain a first state feedback equation group, and performing a linear transformation on the second state space equation group to obtain a second state feedback equation group;

[0025] Obtain a first state transformation matrix according to the first state feedback equation group, and obtain a second state transformation matrix according to the second state feedback equation group;

[0026] Obtain a first measurement value feedback matrix according to the first state feedback equation group and the first state transformation matrix, and obtain a second measurement value feedback matrix according to the second state feedback equation group and the second state transformation matrix;

[0027] Obtain a first system function according to the first state-space equations, and obtain a second system function according to the second state-space equations;

[0028] Obtaining the extended state observer according to the first measurement value feedback matrix, the second measurement value feedback matrix, the first system function, and the second system function;

[0029] The extended state observer is:

[0030]

[0031] Among them, (L 11 , L 12 ) represents the first measurement value feedback matrix, (L 21 , L 22 ) represents the second measurement value feedback matrix, represents the first system function, represents the second system function, C1 and C2 represent the system observation matrix, D1 and D2 represent the system interference distribution matrix, y 1m and y 2m Indicates the measurement feedback value, represents the system state quantity and and represents the extended state quantity and col(·) represents a column vector.

[0032] In some embodiments, designing a trajectory tracking controller based on the extended state observer specifically includes:

[0033] Obtaining a state space vector and an interference state vector according to the extended state observer;

[0034] Define a virtual ideal tracking trajectory vector, an actual ideal tracking trajectory vector, a gain matrix, and a first-order filter bandwidth coefficient matrix;

[0035] The trajectory tracking controller is obtained according to the state space vector, the interference state vector, the virtual ideal tracking trajectory vector, the actual ideal tracking trajectory vector, the gain matrix and the first-order filter bandwidth coefficient matrix.

[0036] In some embodiments, obtaining an ideal value for posture tracking and designing an auxiliary system based on the trajectory tracking controller specifically includes:

[0037] Obtaining the virtual control input variable according to the trajectory tracking controller, and then using the virtual control input variable as the ideal value;

[0038] designing the auxiliary system according to the virtual control input variable and the saturation function;

[0039] The auxiliary systems are:

[0040]

[0041] in, represents the auxiliary system, e2 represents the tracking error of the trajectory tracking controller and represents the virtual control input variable, sat(·) represents the saturation function, and κ and δ are both positive real numbers.

[0042] In some embodiments, the auxiliary system performs posture restriction on the ideal value according to a preset posture range, specifically:

[0043] When the ideal value exceeds the preset posture range, the virtual control input variable is updated by the auxiliary system so that the ideal value is within the preset posture range.

[0044] To achieve the above objectives, another aspect of the present application provides a trajectory tracking system for a quadrotor drone under attitude constraints, comprising:

[0045] A dynamic model acquisition module is used to obtain a dynamic model of the quadrotor UAV and obtain a first state space equation group and a second state space equation group according to the dynamic model;

[0046] An extended state observer design module, configured to design an extended state observer based on the first state space equations and the second state space equations, wherein the extended state observer is configured to observe system state quantities and extended state quantities;

[0047] A trajectory tracking controller design module, used to design a trajectory tracking controller based on the extended state observer;

[0048] The posture limiting module is used to obtain an ideal value for posture tracking, design an auxiliary system according to the trajectory tracking controller, and then use the auxiliary system to perform posture limiting on the ideal value according to a preset posture range.

[0049] To achieve the above-mentioned purpose, another aspect of an embodiment of the present application proposes an electronic device, which includes a memory, a processor, a program stored on the memory and executable on the processor, and a data bus for realizing connection and communication between the processor and the memory. When the program is executed by the processor, the trajectory tracking method under attitude constraints of the quadrotor drone as described above is realized.

[0050] To achieve the above-mentioned purpose, another aspect of an embodiment of the present application proposes a storage medium, which is a computer-readable storage medium for computer-readable storage, and the storage medium stores one or more programs, and the one or more programs can be executed by one or more processors to implement the trajectory tracking method under attitude constraints of the quadrotor drone as described above.

[0051] The beneficial effects of the present invention are as follows: the trajectory tracking method and system of the quadrotor drone under attitude constraints of the present invention first obtains the dynamic model of the quadrotor drone, obtains the first state space equation group and the second state space equation group according to the dynamic model, and then designs an extended state observer according to the first state space equation group and the second state space equation group, then designs a trajectory tracking controller according to the extended state observer, and finally obtains the ideal value of attitude tracking, designs an auxiliary system according to the trajectory tracking controller, and then performs attitude constraints on the ideal value according to a preset attitude range through the auxiliary system. The ideal value of attitude tracking dominated by the design of the auxiliary system of the present invention can effectively limit the tilt angle attitude of the quadrotor drone during flight, so that the monitored target device is always within the effective visual range of the camera under the fuselage, improves the robustness of target positioning, and thus realizes autonomous safety inspection of the target device. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following introduction is made to the drawings required for use in the embodiments of the present invention. It should be understood that the drawings introduced below are only for the convenience of clearly describing some embodiments of the technical solutions of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without any creative work.

[0053] Figure 1 A flowchart of the steps of a trajectory tracking method under attitude constraints for a quadrotor drone provided by an embodiment of the present invention;

[0054] Figure 2 A schematic structural diagram of a trajectory tracking system under attitude constraints for a quadrotor drone provided by an embodiment of the present invention;

[0055] Figure 3 A schematic structural diagram of a security monitoring quadrotor drone system provided by an embodiment of the present invention;

[0056] Figure 4 A schematic diagram of the hardware structure of an electronic device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0057] In order to make the purpose, technical solutions and advantages of the present application clearer, the present application is further described in detail below with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are only used to explain the present application and are not intended to limit the present application. When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the embodiments of the present application. They are merely examples of devices and methods consistent with some aspects of the embodiments of the present application as detailed in the appended claims.

[0058] It will be understood that the terms "first", "second", etc. used in this application may be used herein to describe various concepts, but unless otherwise specified, these concepts are not limited by these terms. These terms are only used to distinguish one concept from another. For example, without departing from the scope of the embodiments of the present application, the first information may also be referred to as the second information, and similarly, the second information may also be referred to as the first information. Depending on the context, the words "if" and "if" as used herein may be interpreted as "at the time of" or "when" or "in response to determining".

[0059] The terms "at least one", "plurality", "each", "any", etc. used in this application include "at least one", "two" or more, "plurality" or "each", "any" or "any one", "each" or "any one" in the context of the present invention, and "at least one" or "at least one" includes one, two or more, "plurality" or "any one" includes two or more, "each" or "each one" in the context of the present invention, and "any" or "any one

[0060] Robotics technology has advanced rapidly in recent years, particularly the development and application of quadcopter drones, which have begun to be used in the safety monitoring of electromechanical equipment. However, due to the inherent structural design of current quadcopter drones and the performance of their visual information collection devices, such as cameras, the inclination angles generated during flight can cause the target device to exceed the effective recognition range of the camera, making it impossible to continuously identify and locate the target object, effectively preventing autonomous safety inspections. Furthermore, existing safety inspection quadcopter drones still require manual control by personnel or the installation of expensive gimbal equipment.

[0061] To this end, an embodiment of the present invention proposes a trajectory tracking method for a quadrotor drone under attitude constraints, which first obtains a dynamic model of the quadrotor drone, obtains a first state-space equation group and a second state-space equation group based on the dynamic model, and then designs an extended state observer based on the first state-space equation group and the second state-space equation group. Then, a trajectory tracking controller is designed based on the extended state observer, and finally, an ideal value for attitude tracking is obtained, an auxiliary system is designed based on the trajectory tracking controller, and then the ideal value is attitude-constrained according to a preset attitude range by the auxiliary system. The ideal value for attitude tracking dominated by the auxiliary system designed by the present invention can effectively limit the tilt angle attitude of the quadrotor drone during flight, so that the monitored target device is always within the effective visual range of the camera under the fuselage, thereby improving the robustness of target positioning, thereby realizing autonomous safety inspection of the target device.

[0062] Reference Figure 1 , Figure 1 The present invention provides a flow chart of the steps of a method for tracking a trajectory of a quadrotor drone under attitude constraints. The present invention provides a method for tracking a trajectory of a quadrotor drone under attitude constraints, and the method includes steps S101 to S104:

[0063] S101, obtaining a dynamic model of the quadrotor drone, and obtaining a first state-space equation group and a second state-space equation group according to the dynamic model;

[0064] As an optional implementation, the step of obtaining the dynamic model of the quadrotor drone can be specifically divided into the following steps S1011 and S1012:

[0065] S1011. Obtain the mass, moment of inertia, linear velocity conversion matrix, angular velocity conversion matrix, force input, and torque input of the quadrotor drone:

[0066] S1012. Obtain a dynamic model based on mass, moment of inertia, linear velocity conversion matrix, angular velocity conversion matrix, force input, and torque input;

[0067] The kinetic model is:

[0068]

[0069] Where ξ represents the three-axis translation position and ξ = [x, y, z] T , v represents the three-axis translation linear velocity and v=[v x , v y , v z ] T , η represents the three-axis Euler angle and η=[φ,θ,ψ] T , ω represents the three-axis angular velocity and ω=[p,q,r] T , m represents mass, g represents acceleration due to gravity, R t Represents the linear velocity conversion matrix, R r represents the angular velocity conversion matrix, d ξ represents the three-axis translation disturbance torque, d η represents the three-axis rotation interference torque, J represents the moment of inertia, u ξ Represents force input, u η Indicates torque input.

[0070] Specifically, according to the quadrotor UAV system, its dynamic model is obtained as shown in the following formula:

[0071]

[0072] Where ξ represents the three-axis translation position and ξ = [x, y, z] T , v represents the three-axis translation linear velocity and v=[v x , v y , v z ] T , η represents the three-axis Euler angle and η=[φ,θ,ψ] T , ω represents the three-axis angular velocity and ω=[p,q,r] T , represents the mass of the quadrotor drone, J=[J x , J y , J z ] T represents the moment of inertia of the quadrotor drone, g represents the acceleration due to gravity, u ξ =[0, 0, τ f ] T and u η =[τ p , τ q , τ r ] T Represent the force input and torque input of the system, R t and R rThey represent the linear velocity conversion matrix and angular velocity conversion matrix respectively, as shown in the following formulas:

[0073]

[0074] Among them, the abbreviation S (·) 、C (·) 、T (·) and Se (·) represent the trigonometric functions sin(·), cos(·), tan(·), and sec(·), respectively; φ, θ, and ψ represent the roll, pitch, and yaw angles, respectively.

[0075] As an optional implementation, obtaining the first state-space equations and the second state-space equations according to the dynamics model can be specifically divided into the following steps S1013 and S1014:

[0076] S1013. Define state space variables, disturbance state variables, virtual control input variables, state space input variables, and state change matrix;

[0077] S1014. Convert the dynamic model from a second-order differential equation to a first-order differential equation based on the state-space variables, the disturbance state variables, the virtual control input variables, the state-space input variables, and the state change matrix to obtain a first state-space equation group and a second state-space equation group.

[0078] The first state space equations are:

[0079]

[0080] The second state space equations are:

[0081]

[0082] Among them, x1, x2, x3, x4, x5 and x6 represent state space variables, d1, d2, d3, d4 and d5 represent interference state variables, g1, g2, g3, g4 and g5 represent state change matrices, represents the virtual control input variable, and Represents the state-space input variables.

[0083] Specifically, the dynamic model of the quadrotor drone is rewritten as a subsystem of two state-space equations, namely the first state-space equation group and the second state-space equation group:

[0084]

[0085] Among them, vectors x1, x2, x3, x4, x5 and x6 represent state space variables, d1, d2, d3, d4 and d5 represent interference state variables, and matrices g1, g2, g3, g4 and g5 represent state change matrices. represents the virtual control input variable (i.e., the ideal value of the attitude constraint), and represents the state space input variables. The specific definitions of the above variables and matrices are shown in the following formulas:

[0086]

[0087] S102, designing an extended state observer based on the first state space equation group and the second state space equation group, wherein the extended state observer is used to observe the system state quantity and the extended state quantity;

[0088] In some optional embodiments, an extended state observer is designed based on the quadrotor UAV system to observe the system state quantity and the extended state quantity. The system state quantity includes the three-axis translation position ξ=[x,y,z] T 、Three-axis translation linear velocity v=[v x , v y , v z ] T 、Three-axis Euler angle η=[φ,θ,ψ] T And the three-axis rotation angular velocity ω=[p,q,r] T , the expanded state quantity includes three-axis translation interference torque, three-axis rotation interference torque d ξ =[d x , d y , d z ] T .

[0089] As an optional implementation, the step of designing an extended state observer based on the first state-space equations and the second state-space equations can be specifically divided into the following steps S1021 to S1025:

[0090] S1021. Perform a linear transformation on the first state space equation group to obtain a first state feedback equation group, and perform a linear transformation on the second state space equation group to obtain a second state feedback equation group;

[0091] Specifically, according to the first state space equation group and the second state space equation group, the first state feedback equation group and the second state feedback equation group are designed as shown below:

[0092]

[0093] Among them, z1, z2, z3, z4, z5 and z6 represent the new state space variables after linear transformation, and the function vectors in the transformation formula are f1=x2, And f5=g4x6.

[0094] S1022. Obtain a first state transformation matrix according to the first state feedback equation group, and obtain a second state transformation matrix according to the second state feedback equation group;

[0095] Specifically, according to the first state feedback equation group and the second state feedback equation group, the first state transformation matrix S1 and the second state transformation matrix S2 are obtained as shown below:

[0096]

[0097] The element vector is defined as as well as I is the identity matrix, vector J φθ It is defined as follows:

[0098]

[0099] S1023. Obtain a first measurement value feedback matrix according to the first state feedback equation group and the first state transformation matrix, and obtain a second measurement value feedback matrix according to the second state feedback equation group and the second state transformation matrix;

[0100] Specifically, according to the first state feedback equations and the first state transformation matrix, the first measurement value feedback matrix of the extended state observer is designed, and according to the second state feedback equations and the second state transformation matrix, the second measurement value feedback matrix of the extended state observer is designed. 11 , L 12 ) and the second measurement value feedback matrix (L 21 , L 22 ) are shown as follows:

[0101]

[0102] Wherein, K1, K2, K3, and K4 represent the extended state observer feedback gain coefficient matrix, W1 and W2 represent the extended state observer convergence bandwidth matrix, w1 and w2 represent the extended state observer convergence bandwidth coefficients, and W1 and W2 are shown in the following formulas:

[0103]

[0104] W2=diag(1,1,W2,W2)

[0105] S1024. Obtain a first system function according to the first state-space equations, and obtain a second system function according to the second state-space equations;

[0106] Specifically, the first system function is The second system function is Here, col(·) represents a column vector.

[0107] S1025. Obtain an extended state observer according to the first measurement value feedback matrix, the second measurement value feedback matrix, the first system function, and the second system function;

[0108] The extended state observer is:

[0109]

[0110] Among them, (L 11 , L 12 ) represents the first measurement value feedback matrix, (L 21 , L 22 ) represents the second measurement value feedback matrix, represents the first system function, represents the second system function, C1 and C2 represent the system observation matrix, D1 and D2 represent the system interference distribution matrix, y 1m and y 2m Indicates the measurement feedback value, represents the system state and and Represents the extended state quantity and col(·) represents a column vector.

[0111] S103, designing a trajectory tracking controller based on the extended state observer;

[0112] As an optional implementation, the step of designing a trajectory tracking controller based on the extended state observer can be specifically divided into the following steps S1031 to S1033:

[0113] S1031. Obtain a state space vector and an interference state vector according to an extended state observer;

[0114] S1032, defining a virtual ideal tracking trajectory vector, an actual ideal tracking trajectory vector, a gain matrix, and a first-order filter bandwidth coefficient matrix;

[0115] S1033 , obtaining a trajectory tracking controller according to the state space vector, the interference state vector, the virtual ideal tracking trajectory vector, the actual ideal tracking trajectory vector, the gain matrix, and the first-order filter bandwidth coefficient matrix.

[0116] Specifically, the state space vector is obtained according to the state of the extended state observer designed above. and the interference state vector Define the virtual ideal tracking trajectory vector as (x 2d 、x 3d 、x 4d 、x 6d ), define the actual ideal tracking trajectory vector as x 1d =[x d ,y d ] T and x 5d =[ψ d , z d ] T , define the dynamic surface controller gain matrix as (A1, A2, A3, A4, A5, A6), the first-order filter bandwidth coefficient matrix as (σ1, σ2, σ3, σ4), and the obtained trajectory tracking controller includes the first trajectory tracking controller and the second trajectory tracking controller. The first trajectory tracking controller is shown in the following formula:

[0117]

[0118] The second trajectory tracking controller is shown in the following formula:

[0119]

[0120] S104 , obtaining an ideal value for posture tracking, designing an auxiliary system based on a trajectory tracking controller, and then using the auxiliary system to impose posture restrictions on the ideal value according to a preset posture range.

[0121] As an optional implementation, the step of obtaining the ideal value of posture tracking and designing an auxiliary system based on the trajectory tracking controller can be specifically divided into the following steps S1041 and S1042:

[0122] S1041. Obtain a virtual control input variable according to the trajectory tracking controller, and then use the virtual control input variable as an ideal value;

[0123] S1042. Design an auxiliary system based on virtual control input variables and saturation functions;

[0124] The auxiliary systems are:

[0125]

[0126] in, represents the virtual control input variable, represents the auxiliary system, e2 represents the tracking error of the trajectory tracking controller and sat(·) represents the saturation function, and both k and δ are positive real numbers.

[0127] Specifically, based on the trajectory tracking controller designed above, the virtual control input variable of the position control is The saturation function sat(·) is introduced at the ideal value of posture tracking, as shown in the following formula:

[0128]

[0129] For the error caused by the introduction of the saturation function, the design auxiliary system ζ(∈) is shown as follows:

[0130]

[0131] Where e2 represents the tracking error of the second trajectory tracking controller and

[0132] As an optional implementation, the step of limiting the posture of the ideal value according to the preset posture range by the auxiliary system can be specifically divided into the following steps S1043:

[0133] S1043. When the ideal value exceeds the preset posture range, the virtual control input variable is updated through the auxiliary system so that the ideal value is within the preset posture range.

[0134] It should be noted that the embodiment of the present invention reversely compensates for the difference beyond the preset posture range through the designed auxiliary system, so that the function achieved is: when the ideal value of posture tracking does not exceed the preset posture range, the auxiliary system has no effect on the controller; when the ideal value of posture tracking exceeds the preset posture range, the auxiliary system dominates the ideal value of posture tracking to limit it within the preset posture range, that is, the virtual control input variable in the first trajectory tracking controller is Redesign it as follows:

[0135]

[0136] Where λ is a positive real number.

[0137] The above describes the trajectory tracking method under attitude constraints of a quadrotor drone according to an embodiment of the present invention. It can be recognized that, compared to the equipment safety inspection method in the prior art, the embodiment of the present invention performs saturation constraints on the virtual control input variable of position control (i.e., the ideal value of attitude tracking) based on a saturation function, and designs an additional auxiliary system. When the ideal value of attitude tracking exceeds the preset attitude range, the auxiliary system directs the ideal value of attitude tracking to be limited within the preset range, thereby effectively limiting the inclination angle of the quadrotor drone during flight, so that the monitored target device is always within the effective visual range of the camera under the fuselage, improving the robustness of target positioning, and realizing autonomous safety inspection of the target device.

[0138] Reference Figure 2 The embodiment of the present invention further provides a trajectory tracking system for a quadrotor drone under attitude constraints, comprising:

[0139] A dynamic model acquisition module is used to obtain the dynamic model of the quadrotor UAV and obtain the first state space equation group and the second state space equation group according to the dynamic model;

[0140] An extended state observer design module is used to design an extended state observer based on the first state space equation group and the second state space equation group, wherein the extended state observer is used to observe the system state quantity and the extended state quantity;

[0141] Trajectory tracking controller design module, used to design trajectory tracking controller based on extended state observer;

[0142] The attitude limitation module is used to obtain the ideal value of attitude tracking, design the auxiliary system according to the trajectory tracking controller, and then use the auxiliary system to limit the attitude of the ideal value according to the preset attitude range.

[0143] The contents of the above-mentioned embodiments of the trajectory tracking method under attitude constraints of a quadrotor drone are all applicable to the embodiments of the trajectory tracking system under attitude constraints of this quadrotor drone. The functions specifically implemented by the embodiments of the trajectory tracking system under attitude constraints of this quadrotor drone are the same as those in the above-mentioned embodiments of the trajectory tracking method under attitude constraints of a quadrotor drone, and the beneficial effects achieved are also the same as those achieved by the above-mentioned embodiments of the trajectory tracking method under attitude constraints of a quadrotor drone.

[0144] Furthermore, the embodiment of the present invention also provides a security monitoring quadrotor drone system for realizing a trajectory tracking method under attitude constraints of a quadrotor drone, such as Figure 3The figure shows a schematic block diagram of a security monitoring quadrotor drone system. The system includes a remote terminal 101 and a quadrotor drone platform 201. The remote terminal 101 includes a human-machine interaction host computer 10 and a wireless communication module base station 11. The quadrotor drone platform 201 includes a wireless communication module mobile terminal 21, a visual positioning module 22, a security monitoring module 23, a quadrotor flight controller 24, a drive module 25, and a power module 26. The visual positioning module 22 includes a stereo camera 221 and a visual data processing module 222; the security monitoring module 23 includes a sensor 231 and a data acquisition device 232; the drive module 25 includes a brushless motor speed regulator 251 and a brushless motor 252; and the power module 26 includes a step-down module 261, a current and voltage measurement module 262, and a battery 263.

[0145] An embodiment of the present invention further provides an electronic device comprising: a memory, a processor, a program stored in the memory and executable on the processor, and a data bus for communicating between the processor and the memory. When the program is executed by the processor, the aforementioned method for trajectory tracking under attitude constraints for a quadrotor drone is implemented. The electronic device can be any intelligent terminal, including a tablet computer and an in-vehicle computer.

[0146] like Figure 4 FIG2 is a schematic diagram of the hardware structure of the electronic device provided by the embodiment of the present invention, referring to FIG2 Figure 4 , an embodiment of the present invention provides an electronic device, including:

[0147] The processor 1001 may be implemented as a general-purpose CPU (Central Processing Unit), a microprocessor, an application-specific integrated circuit (ASIC), or one or more integrated circuits, and is configured to execute relevant programs to implement the technical solutions provided by the embodiments of the present invention.

[0148] The memory 1002 can be implemented in the form of a read-only memory (ROM), a static storage device, a dynamic storage device, or a random access memory (RAM). The memory 1002 can store an operating system and other application programs. When the technical solutions provided in the embodiments of this specification are implemented through software or firmware, the relevant program codes are stored in the memory 1002 and are called by the processor 1001 to execute the trajectory tracking method under attitude constraints for a quadrotor drone according to the embodiment of the present invention.

[0149] Input / output interface 1003, used to implement information input and output;

[0150] Communication interface 1004, used to implement communication interaction between this device and other devices, which can be achieved through wired means (such as USB, network cable, etc.) or wireless means (such as mobile network, WiFi, Bluetooth, etc.);

[0151] Bus 1005 , which transmits information between various components of the device (e.g., processor 1001 , memory 1002 , input / output interface 1003 , and communication interface 1004 );

[0152] The processor 1001 , the memory 1002 , the input / output interface 1003 and the communication interface 1004 are connected to each other in communication within the device via a bus 1005 .

[0153] An embodiment of the present invention also provides a storage medium, which is a computer-readable storage medium used for computer-readable storage. The storage medium stores one or more programs, and the one or more programs can be executed by one or more processors to implement the above-mentioned trajectory tracking method under attitude constraints of the quadrotor drone.

[0154] The memory, as a non-transient computer-readable storage medium, can be used to store non-transient software programs and non-transient computer executable programs. In addition, the memory may include a high-speed random access memory and may also include a non-transient memory, such as at least one disk storage device, a flash memory device, or other non-transient solid-state storage device. In some embodiments, the memory may optionally include a memory remotely arranged relative to the processor, and these remote memories may be connected to the processor via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0155] The embodiment of the present invention also discloses a computer program product or computer program, which includes computer instructions stored in a computer-readable storage medium. A processor of a computer device can read the computer instructions from the computer-readable storage medium, and the processor executes the computer instructions, so that the computer device performs Figure 1 The method shown.

[0156] In some optional embodiments, the function / operation mentioned in the block diagram may not occur in the order mentioned in the operation diagram. For example, depending on the function / operation involved, the two boxes shown in succession can actually be executed substantially simultaneously or the above-mentioned boxes can sometimes be executed in reverse order. In addition, the embodiment presented and described in the flow chart of the present invention is provided in an exemplary manner for the purpose of providing a more comprehensive understanding of the technology. The disclosed method is not limited to the operation and logic flow presented herein. Optional embodiments are contemplated in which the order of the various operations is changed and the sub-operations described as a part of a larger operation are performed independently.

[0157] In addition, although the present invention is described in the context of functional modules, it should be understood that, unless otherwise stated, one or more of the above-mentioned functions and / or features can be integrated into a single physical device and / or software module, or one or more functions and / or features can be implemented in separate physical devices or software modules. It is also understood that a detailed discussion of the actual implementation of each module is not necessary for understanding the present invention. More specifically, given the properties, functions, and internal relationships of the various functional modules in the devices disclosed herein, the actual implementation of the module will be understood within the routine skills of an engineer. Therefore, a person skilled in the art can implement the present invention set forth in the claims using ordinary skills without undue experimentation. It is also understood that the specific concepts disclosed are merely illustrative and are not intended to limit the scope of the present invention, which is determined by the full scope of the appended claims and their equivalents.

[0158] If the above functions are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the above methods of each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, and other media that can store program code.

[0159] The logic and / or steps represented in the flowcharts or otherwise described herein, for example, can be considered as an ordered list of executable instructions for implementing the logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (e.g., a computer-based system, a system including a processor, or other system that can fetch and execute instructions from an instruction execution system, apparatus, or device). For purposes of this specification, a "computer-readable medium" can be any device that can contain, store, communicate, propagate, or transport a program for use by, or in conjunction with, an instruction execution system, apparatus, or device.

[0160] It should be understood that various parts of the present invention can be implemented using hardware, software, firmware, or a combination thereof. In the above-described embodiments, multiple steps or methods can be implemented using software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented using hardware, as in another embodiment, any one of the following technologies known in the art or a combination thereof can be used: a discrete logic circuit having a logic gate circuit for implementing a logic function on a data signal, an application-specific integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0161] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions, and variations may be made to the embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the claims and their equivalents.

[0162] The above is a specific description of the preferred implementation of the present invention, but the present invention is not limited to the embodiments. Those skilled in the art can make various equivalent modifications or substitutions without violating the spirit of the present invention. These equivalent modifications or substitutions are all included in the scope defined by the claims of this application.

Claims

1. A trajectory tracking method for a quad-rotor drone under attitude constraints, characterized in that: The following steps are involved: Obtaining a dynamic model of the quadrotor drone, and obtaining a first state space equation group and a second state space equation group according to the dynamic model; Designing an extended state observer according to the first state space equation group and the second state space equation group, wherein the extended state observer is used to observe system state quantities and extended state quantities; Designing a trajectory tracking controller based on the extended state observer; An ideal value for posture tracking is obtained, an auxiliary system is designed according to the trajectory tracking controller, and then the ideal value is posture-limited according to a preset posture range by the auxiliary system.

2. The method for tracking a trajectory of a quad-rotor drone under attitude constraints according to claim 1, characterized in that: The obtaining of the dynamic model of the quadrotor drone specifically includes: Obtaining the mass, moment of inertia, linear velocity conversion matrix, angular velocity conversion matrix, force input, and torque input of the quadrotor drone; Obtaining the dynamic model according to the mass, the moment of inertia, the linear velocity conversion matrix, the angular velocity conversion matrix, the force input, and the torque input; The kinetic model is: Where ξ represents the three-axis translation position and ξ=[x,y,z] T , v represents the linear velocity of the three-axis translation and v=[v x ,v y ,v z ] T , η represents the three-axis Euler angle and η=[φ,θ,ψ] T , ω represents the three-axis angular velocity and ω=[p,q,r] T , m represents the mass, g represents the acceleration due to gravity, R t represents the linear velocity conversion matrix, R r represents the angular velocity conversion matrix, d ξ represents the three-axis translation disturbance torque, d η represents the three-axis rotation disturbance torque, J represents the moment of inertia, u ξ represents the force input, u η represents the torque input.

3. The method for tracking a trajectory of a quad-rotor drone under attitude constraints according to claim 1, characterized in that: The step of obtaining the first state space equation group and the second state space equation group according to the dynamic model specifically includes: Define state space variables, disturbance state variables, virtual control input variables, state space input variables and state change matrix; According to the state space variables, the disturbance state variables, the virtual control input variables, the state space input variables and the state change matrix, the dynamic model is converted from a second-order differential equation to a first-order differential equation to obtain the first state space equation group and the second state space equation group; The first state space equations are: The second state space equations are: Wherein, x1, x2, x3, x4, x5 and x6 represent the state space variables, d1, d2, d3, d4 and d5 represent the interference state variables, g1, g2, g3, g4 and g5 represent the state change matrix, represents the virtual control input variable, and represents the state-space input variables.

4. The method for tracking a trajectory of a quad-rotor drone under attitude constraints according to claim 1, characterized in that: The designing of an extended state observer according to the first state space equation group and the second state space equation group specifically includes: Performing a linear transformation on the first state space equation group to obtain a first state feedback equation group, and performing a linear transformation on the second state space equation group to obtain a second state feedback equation group; Obtain a first state transformation matrix according to the first state feedback equation group, and obtain a second state transformation matrix according to the second state feedback equation group; Obtain a first measurement value feedback matrix according to the first state feedback equation group and the first state transformation matrix, and obtain a second measurement value feedback matrix according to the second state feedback equation group and the second state transformation matrix; Obtain a first system function according to the first state-space equation group, and obtain a second system function according to the second state-space equation group; Obtaining the extended state observer according to the first measurement value feedback matrix, the second measurement value feedback matrix, the first system function, and the second system function; The extended state observer is: Among them, (L 11 , L 12 ) represents the first measurement value feedback matrix, (L 21 , L 22 ) represents the second measurement value feedback matrix, represents the first system function, represents the second system function, C1 and C2 represent system observation matrices, D1 and D2 represent system interference distribution matrices, y 1m and 2m Indicates the measurement feedback value, represents the state quantity of the system and and represents the extended state quantity and col(·) represents a column vector.

5. The method for tracking a trajectory of a quad-rotor drone under attitude constraints according to claim 1, characterized in that: The designing of a trajectory tracking controller according to the extended state observer specifically includes: Obtaining a state space vector and an interference state vector according to the extended state observer; Define a virtual ideal tracking trajectory vector, an actual ideal tracking trajectory vector, a gain matrix, and a first-order filter bandwidth coefficient matrix; The trajectory tracking controller is obtained according to the state space vector, the interference state vector, the virtual ideal tracking trajectory vector, the actual ideal tracking trajectory vector, the gain matrix and the first-order filter bandwidth coefficient matrix.

6. The method for tracking a trajectory of a quad-rotor drone under attitude constraints according to claim 3, characterized in that: The step of obtaining an ideal value for posture tracking and designing an auxiliary system according to the trajectory tracking controller specifically includes: Obtaining the virtual control input variable according to the trajectory tracking controller, and then using the virtual control input variable as the ideal value; designing the auxiliary system according to the virtual control input variable and the saturation function; The auxiliary systems are: in, represents the auxiliary system, e2 represents the tracking error of the trajectory tracking controller and represents the virtual control input variable, sat(·) represents the saturation function, and κ and δ are both positive real numbers.

7. The method for tracking a trajectory of a quad-rotor drone under attitude constraints according to claim 6, characterized in that: The auxiliary system is used to limit the posture of the ideal value according to the preset posture range, specifically: When the ideal value exceeds the preset posture range, the virtual control input variable is updated by the auxiliary system so that the ideal value is within the preset posture range.

8. A trajectory tracking system under attitude constraints for a quad-rotor drone, characterized in that: include: A dynamics model acquisition module, used to acquire a dynamics model of the quadrotor drone, and obtain a first state space equation group and a second state space equation group according to the dynamics model; An extended state observer design module, used to design an extended state observer according to the first state space equation group and the second state space equation group, wherein the extended state observer is used to observe system state quantities and extended state quantities; A trajectory tracking controller design module, used to design a trajectory tracking controller according to the extended state observer; The posture limiting module is used to obtain an ideal value for posture tracking, design an auxiliary system according to the trajectory tracking controller, and then perform posture limiting on the ideal value according to a preset posture range through the auxiliary system.

9. An electronic device, characterized in that: The electronic device includes a memory, a processor, a program stored in the memory and executable on the processor, and a data bus for realizing connection and communication between the processor and the memory. When the program is executed by the processor, the steps of the trajectory tracking method under attitude constraints of a quadrotor drone as described in any one of claims 1 to 7 are realized.

10. A storage medium, the storage medium being a computer-readable storage medium, used for computer-readable storage, characterized in that: The storage medium stores one or more programs, and the one or more programs can be executed by one or more processors to implement the steps of the trajectory tracking method under attitude constraints of a quadrotor drone as described in any one of claims 1 to 7.

Images