Flight attitude stability control method and system for unmanned aerial vehicle with arms

Abstract

The invention provides a flight attitude stability control method for an unmanned aerial vehicle with arms, and the method comprises the steps: S1, building an attitude kinematic model of the unmanned aerial vehicle according to an unmanned aerial vehicle model; s2, according to the attitude kinematics model of the unmanned aerial vehicle, establishing an attitude kinematics model of the unmanned aerial vehicle with the arm; and S3, establishing an adaptive control model, adjusting parameters of the attitude kinematics model of the unmanned aerial vehicle with the arm in real time, and performing attitude control on the unmanned aerial vehicle with the arm. Mechanical arm interference is introduced into the unmanned aerial vehicle attitude control system, the environment interaction capability of the unmanned aerial vehicle with the arm is greatly improved, and the real-time performance and stability of the system are enhanced.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle flight control, and in particular to a flight attitude stabilization control method and system for an unmanned aerial vehicle with arms. Background Art

[0002] Drones are a type of sports platform that has been widely used in recent years. They can be applied to a variety of mission scenarios such as information collection, environmental monitoring, and intelligent interaction. However, drones themselves have a simple structure and are light in weight, making it difficult to carry more equipment. Therefore, they are only suitable for some tasks such as information data collection, and it is difficult to achieve more complex environmental interaction tasks. In addition, the environmental interaction equipment newly added to drones is often independent of the two control systems of the drones, and there is little interaction between the two, making it difficult to achieve true collaborative operation. Moreover, since the model of the drone itself is complex and it is an under-actuated system, its control requires a more accurate mathematical model to achieve stable control. The introduction of additional equipment will cause the model parameters of the system itself to change, and the interaction with the environment will cause the system parameters to change during the operation, which is more prone to instability and difficult to adapt to the changing needs of the actual application scenario environment. The applicability is poor.

[0003] At present, common control algorithms for UAVs include PID, ADRC, sliding mode control, and robust control. However, in common control algorithms, the control law needs to be adjusted according to the specific model. After achieving a good control effect, if the system parameters change to a certain extent, it is difficult to achieve a satisfactory control effect. In order to achieve timely compensation for the periodic interference caused by the introduction of the robotic arm, a fixed-time convergence observer is introduced to ensure that the upper limit of the convergence time is controllable, and at the same time, a second-order nonlinear anti-disturbance controller is combined to achieve high-precision compensation control of the system.

[0004] The Chinese patent document with publication number CN116880535A discloses an improved second-order self-disturbance rejection control method for attitude control of a quad-rotor UAV. Compared with the prior art, the beneficial effects of the invention are: faster response speed, the optimized linear extended state observer transfer function algorithm greatly improves the response speed of the control system; the control system is more stable, and the amplitude change of the optimized linear extended state observer transfer function algorithm is smoother during the control system response process; high control accuracy, by optimizing the control algorithm, the phase lag is significantly reduced, and the accuracy and stability of the control are improved; the parameters are easy to adjust, and the invention introduces an adaptive control method for real-time adjustment of the PD link control parameters of the PD controller, which can compensate for the tracking error caused by the controller parameter deviation and disturbance, which not only simplifies the parameter setting process, but also improves the control accuracy of the control system and enhances the anti-disturbance performance of the controller. The invention cannot solve the problem of how to use the optimized adaptive control model for attitude control of a UAV carrying a robotic arm.

[0005] Therefore, a method and system for stabilizing the flight attitude of an unmanned aerial vehicle with arms are needed. Summary of the invention

[0006] In view of the defects in the prior art, the object of the present invention is to provide a flight attitude stabilization control method and system for an unmanned aerial vehicle with arms.

[0007] A flight attitude stabilization control method for an arm-mounted drone provided by the present invention comprises:

[0008] Step S1: Establishing the attitude kinematics model of the UAV according to the UAV model;

[0009] Step S2: establishing an attitude kinematics model of the UAV with arms according to the attitude kinematics model of the UAV;

[0010] Step S3: Establish an adaptive control model, adjust the parameters of the attitude kinematics model of the UAV with arms in real time, and perform attitude control on the UAV with arms.

[0011] Preferably, the step S1 comprises:

[0012] Step S1.1: Based on the drone model, establish the drone position and attitude model:

[0013]

[0014]

[0015] ψ represents the yaw angle of the drone; φ represents the roll angle of the drone; θ represents the pitch angle of the drone; J xx represents the moment of inertia of the drone around the x-axis, J yy represents the moment of inertia of the drone around the y-axis, J zz represents the moment of inertia of the drone around the z-axis; τ x Represents the x-axis direction vector of the input drone signal; τ y Represents the y-axis direction vector of the input drone signal; τ z Represents the z-axis direction vector of the input drone signal;

[0016] Step S1.2: According to the position equation and attitude model of the UAV, establish the attitude kinematic model of the UAV:

[0017]

[0018] Among them, δ is the Euler angle vector of the UAV, expressed as δ = [φ, θ, ψ]; τf represents the moment of inertia of the UAV; τ dRepresents the external disturbance to the UAV; the matrices N and C are the parameter matrices of the UAV's attitude kinematics model, which are:

[0019]

[0020] c 11 =0

[0021]

[0022] Among them, I x ,I y ,I z Represents the inertia matrix of the drone body, S represents sin; C represents cos.

[0023] Preferably, step S2 comprises:

[0024] Step S2.1: Add a robotic arm to the drone model;

[0025] Step S2.2: According to the attitude kinematics model of the UAV, the attitude kinematics model of the UAV with arms is established as follows:

[0026]

[0027] Among them, τ extra Represents the total disturbance after the robotic arm is introduced into the UAV model.

[0028] Preferably, step S3 comprises:

[0029] Step S3.1: Connect the tracking differentiator, the control law unit and the error feedback unit according to a preset rule to establish an adaptive control model for adjusting the attitude of the attitude kinematic model of the UAV with arms;

[0030] Step S3.2: Collect the initial signal of the attitude kinematics model of the UAV with arms as the input signal of the adaptive control model, input the input signal into the tracking differentiator, and perform signal conversion; specifically:

[0031] fh=fhan(τ1(t)-τ(t),τ2(t),r,h0)

[0032] τ1(t+T)=τ1(t)+hτ2(t)

[0033] τ2(t+T)=τ2(t)+h*fh

[0034] Among them, the initial signal includes the Euler angle vector, moment of inertia and model parameters of the attitude kinematics model of the UAV with arms; r is the velocity factor of the function; τ1(t) represents the tracking signal of the input signal τ(t); τ2(t) represents the approximate differential of the input signal τ(t); τ(t) represents the input signal at time t; T represents the sampling period; h0 is the filtering factor; h is the sampling time, and fhan is the fastest tracking function;

[0035] Step S3.3: The converted signal is transmitted to the control law unit, the signal is further processed, and the output signal u of the control law unit is sent to the attitude kinematics model of the UAV with arms, and the attitude of the attitude kinematics model of the UAV with arms is adjusted; specifically:

[0036]

[0037] Among them, β1 and β2 are the gain coefficients of the tracking signal of the input and the observation value of its differential signal respectively; r1, r2, σ1, σ2 are the parameters of the adaptive control model; fal is a nonlinear function;

[0038] Step S3.4: After the posture of the posture kinematic model of the robot with arms is adjusted, the output signal of the robot with arms is y(t), an error feedback unit is set, y(t) is sent to the error feedback unit, and the output signal z of the error feedback unit is fed back to the designated unit of the adaptive control model to perform closed-loop real-time feedback control on the posture of the drone with arms model;

[0039] Among them, the error feedback unit is designed as:

[0040]

[0041] Wherein, e(t) is the tracking error of the attitude kinematics model of the UAV with arms; z1(t) and z2(t) are the tracking signal of the output signal of the error feedback unit and the derivative of the output signal of the error feedback unit, respectively; z3 is the disturbance estimate; α1 and α2 are the nonlinear parameters of the attitude kinematics model of the UAV with arms, respectively; σ is the filter coefficient; β 01 , β 02 and β 03 All are parameters, the function fal is:

[0042]

[0043] Wherein, e represents the error between the input and output signals; the value of α is set in the interval (0, 1).

[0044] Preferably, it includes:

[0045] In the adaptive control model, a fixed-time convergence observer is introduced to compensate for disturbances. Specifically:

[0046] According to the attitude kinematics model of the UAV with arms, the disturbance after the robotic arm is introduced into the UAV model is:

[0047]

[0048] According to the disturbance, the state variables are designed for:

[0049]

[0050] A fixed-time convergence observer is introduced into the adaptive control model to compensate for disturbances and converge the error signal within a preset time. The fixed-time convergence observer is set as:

[0051]

[0052] in, The state vector of the observer is:

[0053]

[0054] Parameters satisfy: α 01 >1,α 02 <1, and k1, k2, a1, a2, a3 are all positive gains, β represents the preset parameters;

[0055] Error signal e w for:

[0056] The error signal converges when the error signal e w When it converges to 0, the observation error of the observer Converge to 0 within the preset time, convergence time T i The upper limit T i-max for:

[0057]

[0058] A flight attitude stabilization control system for an arm-mounted drone provided by the present invention comprises:

[0059] Module M1: Establish the attitude kinematics model of the UAV based on the UAV model;

[0060] Module M2: Establish the attitude kinematics model of the UAV with arms based on the attitude kinematics model of the UAV;

[0061] Module M3: Establish an adaptive control model, adjust the parameters of the attitude kinematic model of the UAV with arms in real time, and perform attitude control on the UAV with arms.

[0062] Preferably, the module M1 comprises:

[0063] Module M1.1: Establish the position and attitude model of the drone based on the drone model:

[0064]

[0065] ψ represents the yaw angle of the drone; φ represents the roll angle of the drone; θ represents the pitch angle of the drone; J xx represents the moment of inertia of the drone around the x-axis, J yy represents the moment of inertia of the drone around the y-axis, J zz represents the moment of inertia of the drone around the z-axis; τ x Represents the x-axis direction vector of the input drone signal; τ y Represents the y-axis direction vector of the input drone signal; τ z Represents the z-axis direction vector of the input drone signal;

[0066] Module M1.2: Establish the attitude kinematics model of the UAV based on the position equation and attitude model of the UAV:

[0067]

[0068] Among them, δ is the Euler angle vector of the drone, expressed as δ = [φ, θ, ψ]; τ f Represents the moment of inertia of the drone; τ d Represents the external disturbance to the UAV; the matrices N and C are the parameter matrices of the UAV's attitude kinematics model, which are:

[0069]

[0070] c 11 =0

[0071]

[0072] Among them, I x ,I y ,I z Represents the inertia matrix of the drone body, S represents sin; C represents cos.

[0073] Preferably, module M2 comprises:

[0074] Module M2.1: Add a robotic arm to the drone model;

[0075] Module M2.2: Based on the attitude kinematics model of the UAV, the attitude kinematics model of the UAV with arms is established as follows:

[0076]

[0077] Among them, τ extra Represents the total disturbance after the robotic arm is introduced into the UAV model.

[0078] Preferably, the module M3 includes:

[0079] Module M3.1: Connect the tracking differentiator, control law unit and error feedback unit according to preset rules to establish an adaptive control model for adjusting the attitude of the attitude kinematic model of the UAV with arms;

[0080] Module M3.2: Collect the initial signal of the attitude kinematics model of the UAV with arms as the input signal of the adaptive control model, input the input signal into the tracking differentiator for signal conversion; specifically:

[0081] fh=fhan(τ1(t)-τ(t),τ2(t),r,h0)

[0082] τ1(t+T)=τ1(t)+hτ2(t)

[0083] τ2(t+T)=τ2(t)+h*fh

[0084] Among them, the initial signal includes the Euler angle vector, moment of inertia and model parameters of the attitude kinematics model of the UAV with arms; r is the velocity factor of the function; τ1(t) represents the tracking signal of the input signal τ(t); τ2(t) represents the approximate differential of the input signal τ(t); τ(t) represents the input signal at time t; T represents the sampling period; h0 is the filtering factor; h is the sampling time, and fhan is the fastest tracking function;

[0085] Module M3.3: The converted signal is transmitted to the control law unit, the signal is further processed, and the output signal u of the control law unit is sent to the attitude kinematics model of the UAV with arms, and the attitude of the attitude kinematics model of the UAV with arms is adjusted; specifically:

[0086]

[0087] Among them, β1 and β2 are the gain coefficients of the tracking signal of the input and the observation value of its differential signal respectively; r1, r2, σ1, σ2 are the parameters of the adaptive control model; fal is a nonlinear function;

[0088] Module M3.4: After the posture of the kinematic model of the robot with arms is adjusted, the output signal of the robot with arms is y(t). An error feedback unit is set, y(t) is sent to the error feedback unit, and the output signal z of the error feedback unit is fed back to the designated unit of the adaptive control model to perform closed-loop real-time feedback control on the posture of the drone with arms model.

[0089] Among them, the error feedback unit is designed as:

[0090]

[0091] Wherein, e(t) is the tracking error of the attitude kinematics model of the UAV with arms; z1(t) and z2(t) are the tracking signal of the output signal of the error feedback unit and the derivative of the output signal of the error feedback unit, respectively; z3 is the disturbance estimate; α1 and α2 are the nonlinear parameters of the attitude kinematics model of the UAV with arms, respectively; σ is the filter coefficient; β 01 , β 02 and β 03 All are parameters, the function fal is:

[0092]

[0093] Wherein, e represents the error between the input and output signals; the value of α is set in the interval (0, 1).

[0094] Preferably, it includes:

[0095] In the adaptive control model, a fixed-time convergence observer is introduced to compensate for disturbances. Specifically:

[0096] According to the attitude kinematics model of the UAV with arms, the disturbance after the robotic arm is introduced into the UAV model is:

[0097]

[0098] According to the disturbance, the state variables are designed for:

[0099]

[0100] A fixed-time convergence observer is introduced into the adaptive control model to compensate for disturbances and converge the error signal within a preset time. The fixed-time convergence observer is set as:

[0101]

[0102] in, The state vector of the observer is:

[0103]

[0104] Parameters satisfy: α 01 >1,α 02 <1, and k1, k2, a1, a2, a3 are all positive gains, β represents the preset parameters;

[0105] Error signal e w for:

[0106] The error signal converges when the error signal e w When it converges to 0, the observation error of the observer Converge to 0 within the preset time, convergence time T i The upper limit T i-max for:

[0107]

[0108] Compared with the prior art, the present invention has the following beneficial effects:

[0109] 1. The flight attitude stabilization control method of the UAV with arms provided by the present invention combines a fixed time convergence observer with a second-order nonlinear anti-disturbance controller, which effectively improves the disturbance observation capability and control effect of the UAV system with periodic disturbances;

[0110] 2. The flight attitude stabilization control method of the UAV with arms provided by the present invention effectively solves the influence of external disturbances and parameter perturbations and periodic disturbances caused by the mechanical arm on the system stability, and improves the anti-interference ability of the UAV with arms system;

[0111] 3. In the adaptive control model, the present invention adopts a combination of tracking differentiator, control law unit and error feedback unit. This design can quickly and accurately process the attitude signal of the UAV and accurately adjust the attitude through a real-time feedback mechanism. This not only improves the response speed of the control, but also enhances the robustness of the system;

[0112] 4. The present invention further introduces a fixed-time convergence observer in the adaptive control model to compensate for disturbances. The observer can quickly converge to zero error within a preset time, thereby achieving accurate estimation and compensation of disturbances. BRIEF DESCRIPTION OF THE DRAWINGS

[0113] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:

[0114] Figure 1 This is a flow chart of the flight attitude stabilization control method of the UAV with arms of the present invention;

[0115] Figure 2This is a structural principle diagram of the flight attitude stabilization control method of an armed drone in step 3 of Example 1 of the present invention;

[0116] Figure 3 This is a control effect diagram of Example 1 of the present invention;

[0117] Figure 4 This is a control effect diagram of Example 1 of the present invention;

[0118] Figure 5 The control effect diagram of three controllers under different interferences in Example 1 of the present invention;

[0119] Figure 6 This is a control effect diagram of three controllers under different interferences in Example 1 of the present invention. DETAILED DESCRIPTION

[0120] The present invention is described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those of ordinary skill in the art, several changes and improvements can also be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0121] The present invention provides a flight attitude stabilization control method for an arm-mounted UAV, which realizes attitude stabilization control of the arm-mounted UAV in a stationary state and during flight. Among them, due to the introduction of an additional mechanical arm, the improved attitude stabilization control method is responsible for stabilizing the attitude of the UAV in the flight process and in a stationary state, so as to balance the interference of the mechanical arm to the system during the movement process. The present invention introduces the interference of the mechanical arm into the attitude control system of the UAV, greatly improving the environmental interaction capability of the arm-mounted UAV and enhancing the real-time performance and stability of the system.

[0122] A flight attitude stabilization control method for an arm-mounted drone provided by the present invention comprises:

[0123] Step S1: Establishing the attitude kinematics model of the UAV according to the UAV model;

[0124] Step S2: establishing an attitude kinematics model of the UAV with arms according to the attitude kinematics model of the UAV;

[0125] Step S3: Establish an adaptive control model, adjust the parameters of the attitude kinematics model of the UAV with arms in real time, and perform attitude control on the UAV with arms.

[0126] Specifically, the step S1 includes:

[0127] Step S1.1: Based on the drone model, establish the drone position and attitude model:

[0128]

[0129] ψ represents the yaw angle of the drone; φ represents the roll angle of the drone; θ represents the pitch angle of the drone; J xx represents the moment of inertia of the drone around the x-axis, J yy represents the moment of inertia of the drone around the y-axis, J zz represents the moment of inertia of the drone around the z-axis; τ x Represents the x-axis direction vector of the input drone signal; τ y Represents the y-axis direction vector of the input drone signal; τ z Represents the z-axis direction vector of the input drone signal;

[0130] Step S1.2: According to the position equation and attitude model of the UAV, establish the attitude kinematic model of the UAV:

[0131]

[0132] Among them, δ is the Euler angle vector of the drone, expressed as δ = [φ, θ, ψ]; τ f Represents the moment of inertia of the drone; τ d Represents the external disturbance to the UAV; the matrices N and C are the parameter matrices of the UAV's attitude kinematics model, which are:

[0133]

[0134] c 11 =0

[0135]

[0136] Among them, I x ,I y ,I z Represents the inertia matrix of the drone body, S represents sin; C represents cos.

[0137] Specifically, step S2 includes:

[0138] Step S2.1: Add a robotic arm to the drone model;

[0139] Step S2.2: According to the attitude kinematics model of the UAV, the attitude kinematics model of the UAV with arms is established as follows:

[0140]

[0141] Among them, τ extra Represents the total disturbance after the robotic arm is introduced into the UAV model.

[0142] Specifically, step S3 includes:

[0143] Step S3.1: Connect the tracking differentiator, the control law unit and the error feedback unit according to a preset rule to establish an adaptive control model for adjusting the attitude of the attitude kinematic model of the UAV with arms;

[0144] Step S3.2: Collect the initial signal of the attitude kinematics model of the UAV with arms as the input signal of the adaptive control model, input the input signal into the tracking differentiator, and perform signal conversion; specifically:

[0145] fh=fhan(τ1(t)-τ(t),τ2(t),r,h0)

[0146] τ1(t+T)=τ1(t)+hτ2(t)

[0147] τ2(t+T)=τ2(t)+h*fh

[0148] Among them, the initial signal includes the Euler angle vector, moment of inertia and model parameters of the attitude kinematics model of the UAV with arms; r is the velocity factor of the function; τ1(t) represents the tracking signal of the input signal τ(t); τ2(t) represents the approximate differential of the input signal τ(t); τ(t) represents the input signal at time t; T represents the sampling period; h0 is the filtering factor; h is the sampling time, and fhan is the fastest tracking function;

[0149] Step S3.3: The converted signal is transmitted to the control law unit, the signal is further processed, and the output signal u of the control law unit is sent to the attitude kinematics model of the UAV with arms, and the attitude of the attitude kinematics model of the UAV with arms is adjusted; specifically:

[0150]

[0151] Among them, β1 and β2 are the gain coefficients of the tracking signal of the input and the observation value of its differential signal respectively; r1, r2, σ1, σ2 are the parameters of the adaptive control model; fal is a nonlinear function;

[0152] Step S3.4: After the posture of the posture kinematic model of the robot with arms is adjusted, the output signal of the robot with arms is y(t), an error feedback unit is set, y(t) is sent to the error feedback unit, and the output signal z of the error feedback unit is fed back to the designated unit of the adaptive control model to perform closed-loop real-time feedback control on the posture of the drone with arms model;

[0153] Among them, the error feedback unit is designed as:

[0154]

[0155] Wherein, e(t) is the tracking error of the attitude kinematics model of the UAV with arms; z1(t) and z2(t) are the tracking signal of the output signal of the error feedback unit and the derivative of the output signal of the error feedback unit, respectively; z3 is the disturbance estimate; α1 and α2 are the nonlinear parameters of the attitude kinematics model of the UAV with arms, respectively; σ is the filter coefficient; β 01 , β 02 and β 03 All are parameters, the function fal is:

[0156]

[0157] Wherein, e represents the error between the input and output signals; the value of α is set in the interval (0, 1).

[0158] Specifically, they include:

[0159] In the adaptive control model, a fixed-time convergence observer is introduced to compensate for disturbances. Specifically:

[0160] According to the attitude kinematics model of the UAV with arms, the disturbance after the robotic arm is introduced into the UAV model is:

[0161]

[0162] According to the disturbance, the state variables are designed for:

[0163]

[0164] A fixed-time convergence observer is introduced into the adaptive control model to compensate for disturbances and converge the error signal within a preset time. The fixed-time convergence observer is set as:

[0165]

[0166] in, The state vector of the observer is:

[0167]

[0168] Parameters satisfy: α 01 >1,α 02 <1, and k1, k2, a1, a2, a3 are all positive gains, β represents the preset parameters;

[0169] Error signal e w for:

[0170] The error signal converges when the error signal e w When it converges to 0, the observation error of the observer Converge to 0 within the preset time, convergence time T i The upper limit T i-max for:

[0171]

[0172] Example 1

[0173] The present invention provides a flight attitude stabilization control method for an arm-mounted UAV, which mainly includes the following four steps: S1: establishing a mathematical model based on the hardware structure and motion characteristics of a quad-rotor UAV; S2: establishing a mathematical model of an arm-mounted UAV; S3: designing an attitude control algorithm for an arm-mounted UAV; S4: conducting a simulation experiment to verify the control effect. The specific implementation is as follows:

[0174] Step 1: Establish a mathematical model based on the hardware structure and motion characteristics of the quadrotor drone.

[0175] According to Newton's second law and Euler's equation, the linear velocity and angular velocity of the quadrotor drone system can be expressed as:

[0176]

[0177] F=-Ue+mgR T e

[0178] Among them, m is the overall weight of the quadrotor drone; v is the linear velocity of the drone in three-dimensional coordinates; Ω is the angular velocity of the quadrotor drone; J is the rotational inertia matrix of the drone; τ is the control torque vector of the drone; R is the transformation matrix between the drone body coordinates and the three-dimensional world coordinates; sk(Ω) represents the antisymmetric matrix of the angular velocity of the drone; F is the resultant force of the lift and gravity generated by the drone; U represents the lift generated by the drone; e represents the error signal; R T represents the transposed matrix of matrix R; g represents the gravitational acceleration.

[0179] After sorting, the position equation and attitude equation of the quadrotor drone can be written as:

[0180]

[0181] More generally, considering external disturbances, the attitude kinematics model of the quadrotor drone can be written as:

[0182]

[0183] Among them, δ is the Euler angle vector of the system, which can be expressed as δ = [φ, θ, ψ], which represent the roll angle, pitch angle and yaw angle of the drone respectively; τ f With τ dThey represent the moment of inertia and external disturbance of the UAV respectively; the matrices N and C are the parameter matrices of the system, which can be expressed as:

[0184]

[0185] c 11 =0

[0186]

[0187] Step 2: Establish a mathematical model of the drone with arms.

[0188] The ideal quadrotor drone attitude control equation is:

[0189]

[0190] After the introduction of the robotic arm, the system parameters change and additional disturbances are introduced. The equation can be rewritten as:

[0191]

[0192] Among them, C ext It indicates the impact of the introduction of the robotic arm on the system parameters, including the change of weight, the change of the center of gravity, the change of the moment of inertia, etc.; τ ext It indicates the additional impact on the system caused by the swing of the robotic arm due to inertia during flight.

[0193] To simplify the modeling and control process, all disturbance terms are moved to the right side of the equation and regarded as a total disturbance, which can be written as:

[0194]

[0195] Among them, τ extra represents the parameter perturbation of the system introduced by the manipulator and all other disturbances. Therefore, the designed attitude stabilization algorithm needs to extra Make compensation.

[0196] Step 3: Design an attitude control algorithm for the UAV with arms.

[0197] The initialization link of the controller is the tracking differentiator (TD), which is defined as follows:

[0198] fh=fhan(τ1(t)-τ(t),τ2(t),r,h0)

[0199] τ1(t+T)=τ1(t)+hτ2(t)

[0200] τ2(t+T)=τ2(t)+h*fh

[0201] This link is used to quickly obtain the tracking signal and its differential signal from the input signal, while trying to avoid the generation of overshoot, solving the contradiction between fast and overshoot response, and facilitating the smooth transition and fast and smooth operation of the system. Among them, r is the speed factor of the function, which mainly affects the tracking speed of the input signal τ; h0 is the filter factor that determines the anti-interference ability of this link; h is the sampling time, and fhan is the fastest tracking function of this component, and its specific form is as follows:

[0202] d=rh

[0203] d0=hd

[0204] y=τ1+hτ2

[0205]

[0206] The error feedback link of the controller is the extended state observer (ESO), which is defined as follows:

[0207]

[0208] Among them, e(t) is the tracking error of the system, z1 and z2 are the derivatives of the tracking signal and the system output signal respectively, and z3 is the disturbance estimate, which is the estimated value obtained by comprehensively considering the internal influences such as the changes in the internal parameters of the system and all external disturbances. It is regarded as an extended state in this observer. α1 and α2 are the nonlinear parameters of the system respectively, and σ is the filter coefficient of the system. β 01 , β 02 and β 03 is a set of parameters used to determine the state tracking capability of the system and the ESO's ability to estimate the overall disturbance of the system. A new function fal is defined in the formula, which is a nonlinear function with the specific form:

[0209]

[0210] Among them, α determines the order of the function, and σ determines the size of the linear interval of the function. In order to achieve small gain for large errors and large gain for small errors, the value of α is usually set to the interval (0, 1).

[0211] To achieve nonlinear compensation of the system, the control law of the controller is set to a nonlinear control law (NLSEF), which is defined as follows:

[0212]

[0213] Among them, β1 and β2 are the gain coefficients of the ESO observation value of the tracking signal of the input and its derivative signal respectively; r1, r2, σ1 and σ2 are a set of parameters, which are used to avoid unstable phenomena such as high-frequency oscillation in the system.

[0214] In order to realize the rapid observation and convergence of the system to the error, a fixed time convergence observer (FTCO) is introduced on the basis of the original controller. Its specific design form is:

[0215] The disturbance of the system can be expressed as:

[0216]

[0217] For this disturbance, the designed state variables are:

[0218]

[0219] Γ(Θ1)=N(Θ1)Θ2

[0220]

[0221] Further we can get

[0222]

[0223] Therefore, the fixed-time observer is designed as:

[0224]

[0225] in, is the state vector of the observer, which consists of the following form:

[0226]

[0227] To ensure the convergence of the observer, the parameters should satisfy: 01 >1,α 02 <1, and k1, k2, a1, a2, a3 are all positive gains.

[0228] Error signal e w for:

[0229] When the error e w When it converges to 0, the observation error of the observer It will converge to 0 within a fixed time, and the upper limit of its convergence time is not affected by the initial value of the error. The upper limit is:

[0230]

[0231] Step 4: Perform simulation test through simulink.

[0232] The physical parameters of the quad-rotor drone selected in the present invention are:

[0233] The mass of the drone is 10kg and the acceleration due to gravity is 9.8m / s2 The moment of inertia is set to: I x =0.5kg / m 2 ,I y =1kg / m 2 ,I z =0.3kg / m 2 In this experiment, both the drone and the robotic arm were treated as strictly rigid bodies, which means that their shapes did not change during the flight.

[0234] Set the convergence time T1 to 0.5 seconds. Figure 3 and Figure 4 It can be concluded that the fixed-time convergent observer can utilize both the control quantity generated by the controller and the output of the UAV system to estimate the total disturbance of the system within a finite time.

[0235] After verifying the observation capability of FTCO, it is combined with an active disturbance rejection controller to show its disturbance rejection capability and compare it with the traditional active disturbance rejection controller and PID controller. Figure 5 and 6 The control effects of the three controllers under different disturbances are shown. It can be found that ADRC combined with FTCO has good anti-interference ability when facing disturbances and can complete convergence in the fastest time.

[0236] The present invention also provides a flight attitude stabilization control system for an arm-mounted drone. The flight attitude stabilization control system for the arm-mounted drone can be implemented by executing the process steps of the flight attitude stabilization control method for the arm-mounted drone, that is, those skilled in the art can understand the flight attitude stabilization control method for the arm-mounted drone as a preferred implementation of the flight attitude stabilization control system for the arm-mounted drone.

[0237] A flight attitude stabilization control system for an arm-mounted UAV provided by the present invention includes: module M1: establishing an attitude kinematic model of the UAV according to the UAV model; module M2: establishing an attitude kinematic model of the arm-mounted UAV according to the attitude kinematic model of the UAV; module M3: establishing an adaptive control model, adjusting the parameters of the attitude kinematic model of the arm-mounted UAV in real time, and performing attitude control on the arm-mounted UAV.

[0238] Specifically, the module M1 includes: Module M1.1: Establishing the position and attitude model of the drone according to the drone model:

[0239]

[0240] ψ represents the yaw angle of the drone; φ represents the roll angle of the drone; θ represents the pitch angle of the drone; J xx represents the moment of inertia of the drone around the x-axis, Jyy represents the moment of inertia of the drone around the y-axis, J zz represents the moment of inertia of the drone around the z-axis; τ x Represents the x-axis direction vector of the input drone signal; τ y Represents the y-axis direction vector of the input drone signal; τ z Represents the z-axis direction vector of the input drone signal; Module M1.2: Establish the attitude kinematic model of the drone based on the position equation and attitude model of the drone:

[0241]

[0242] Among them, δ is the Euler angle vector of the drone, expressed as δ = [φ, θ, ψ]; τ f Represents the moment of inertia of the drone; τ d Represents the external disturbance to the UAV; the matrices N and C are the parameter matrices of the UAV's attitude kinematics model, which are:

[0243]

[0244] c 11 =0

[0245]

[0246] Among them, I x ,I y ,I z Represents the inertia matrix of the drone body, S represents sin; C represents cos.

[0247] Specifically, module M2 includes: module M2.1: adding a mechanical arm to the drone model; module M2.2: establishing a posture kinematics model of the drone with an arm according to the posture kinematics model of the drone, which is:

[0248]

[0249] Among them, τ extra Represents the total disturbance after the robotic arm is introduced into the UAV model.

[0250] Specifically, the module M3 includes: module M3.1: connecting the tracking differentiator, the control law unit and the error feedback unit according to preset rules to establish an adaptive control model for adjusting the attitude of the attitude kinematic model of the UAV with arms; module M3.2: collecting the initial signal of the attitude kinematic model of the UAV with arms as the input signal of the adaptive control model, inputting the input signal into the tracking differentiator, and performing signal conversion; specifically:

[0251] fh=fhan(τ1(t)-τ(t),τ2(t),r,h0)

[0252] τ1(t+T)=τ1(t)+hτ2(t)

[0253] τ2(t+T)=τ2(t)+h*fh

[0254] Among them, the initial signal includes the Euler angle vector, moment of inertia and model parameters of the attitude kinematics model of the UAV with arms; r is the speed factor of the function; τ1(t) represents the tracking signal of the input signal τ(t); τ2(t) represents the approximate differential of the input signal τ(t); τ(t) represents the input signal at time t; T represents the sampling period; h0 is the filter factor; h is the sampling time, and fhan is the fastest tracking function; module M3.3: the converted signal is passed to the control law unit, the signal is further processed, and the output signal u of the control law unit is sent to the attitude kinematics model of the UAV with arms to adjust the attitude of the attitude kinematics model of the UAV with arms; specifically:

[0255]

[0256] Among them, β1 and β2 are the gain coefficients of the tracking signal of the input quantity and the observation value of its differential signal respectively; r1, r2, σ1, σ2 are the parameters of the adaptive control model; fal is a nonlinear function; Module M3.4: After the posture of the posture kinematic model of the robot with arms is adjusted, the output signal of the robot with arms is y(t), an error feedback unit is set, y(t) is sent to the error feedback unit, and the output signal z of the error feedback unit is fed back to the designated unit of the adaptive control model to perform closed-loop real-time feedback control on the posture of the UAV model with arms;

[0257] Among them, the error feedback unit is designed as:

[0258]

[0259] Wherein, e(t) is the tracking error of the attitude kinematics model of the UAV with arms; z1(t) and z2(t) are the tracking signal of the output signal of the error feedback unit and the derivative of the output signal of the error feedback unit, respectively; z3 is the disturbance estimate; α1 and α2 are the nonlinear parameters of the attitude kinematics model of the UAV with arms, respectively; σ is the filter coefficient; β 01 , β 02 and β 03 All are parameters, the function fal is:

[0260]

[0261] Wherein, e represents the error between the input and output signals; the value of α is set in the interval (0, 1).

[0262] Specifically, it includes: In the adaptive control model, a fixed time convergence observer is introduced to compensate for the disturbance. Specifically, according to the attitude kinematics model of the UAV with arms, the disturbance after the robotic arm is introduced into the UAV model is:

[0263]

[0264] According to the disturbance, the state variables are designed for:

[0265]

[0266] A fixed-time convergence observer is introduced into the adaptive control model to compensate for disturbances and converge the error signal within a preset time. The fixed-time convergence observer is set as:

[0267]

[0268] in, The state vector of the observer is:

[0269]

[0270] Parameters satisfy: α 01 >1,α 02 <1, and k1, k2, a1, a2, a3 are all positive gains, β represents the preset parameters;

[0271] Error signal e w for:

[0272] The error signal converges when the error signal e w When it converges to 0, the observation error of the observer Converge to 0 within the preset time, convergence time T i The upper limit T i-max for:

[0273]

[0274] Those skilled in the art know that, in addition to realizing the system and its various devices, modules, and units provided by the present invention in a purely computer-readable program code, it is entirely possible to realize the same functions in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system and its various devices, modules, and units provided by the present invention can be considered as a hardware component, and the devices, modules, and units included therein for realizing various functions can also be regarded as structures within the hardware component; the devices, modules, and units for realizing various functions can also be regarded as both software modules for realizing the method and structures within the hardware component.

[0275] The above describes the specific embodiments of the present invention. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essence of the present invention. In the absence of conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.

Claims

1. A flight attitude stabilization control method for an arm-mounted drone, characterized in that: include: Step S1: Establishing the attitude kinematics model of the UAV according to the UAV model; Step S2: establishing an attitude kinematics model of the UAV with arms according to the attitude kinematics model of the UAV; Step S3: Establish an adaptive control model, adjust the parameters of the attitude kinematics model of the UAV with arms in real time, and perform attitude control on the UAV with arms.

2. The flight attitude stabilization control method of the UAV with arms according to claim 1 is characterized in that: The step S1 comprises: Step S1.1: Based on the drone model, establish the drone position and attitude model: ψ represents the yaw angle of the drone; φ represents the roll angle of the drone; θ represents the pitch angle of the drone; J xx represents the moment of inertia of the drone around the x-axis, J yy represents the moment of inertia of the drone around the y-axis, J zz represents the moment of inertia of the drone around the z-axis; τ x Represents the x-axis direction vector of the input drone signal; τ y Represents the y-axis direction vector of the input drone signal; τ z Represents the z-axis direction vector of the input drone signal; Step S1.2: According to the position equation and attitude model of the UAV, establish the attitude kinematic model of the UAV: Among them, δ is the Euler angle vector of the drone, expressed as δ = [φ, θ, ψ]; τ f Represents the moment of inertia of the drone; τ d Represents the external disturbance to the UAV; the matrices N and C are the parameter matrices of the UAV's attitude kinematics model, which are: c 11 ＝0 Among them, I x ,I y ,I z Represents the inertia matrix of the drone body, S represents sin; C represents cos.

3. The flight attitude stabilization control method of the UAV with arms according to claim 1 is characterized in that: Step S2 includes: Step S2.1: Add a robotic arm to the drone model; Step S2.2: According to the attitude kinematics model of the UAV, the attitude kinematics model of the UAV with arms is established as follows: Among them, τ extra Represents the total disturbance after the robotic arm is introduced into the UAV model.

4. The flight attitude stabilization control method of the UAV with arms according to claim 1, characterized in that: The step S3 comprises: Step S3.1: Connect the tracking differentiator, the control law unit and the error feedback unit according to a preset rule to establish an adaptive control model for adjusting the attitude of the attitude kinematic model of the UAV with arms; Step S3.2: Collect the initial signal of the attitude kinematics model of the UAV with arms as the input signal of the adaptive control model, input the input signal into the tracking differentiator, and perform signal conversion; specifically: fh=fhan(τ1(t)-τ(t),τ2(t),r,h0) τ1(t+T)=τ1(t)+hτ2(t) τ2(t+T)=τ2(t)+h*fh Among them, the initial signal includes the Euler angle vector, moment of inertia and model parameters of the attitude kinematics model of the UAV with arms; r is the velocity factor of the function; τ1(t) represents the tracking signal of the input signal τ(t); τ2(t) represents the approximate differential of the input signal τ(t); τ(t) represents the input signal at time t; T represents the sampling period; h0 is the filtering factor; h is the sampling time, and fhan is the fastest tracking function; Step S3.3: The converted signal is transmitted to the control law unit, the signal is further processed, and the output signal u of the control law unit is sent to the attitude kinematics model of the UAV with arms, and the attitude of the attitude kinematics model of the UAV with arms is adjusted; specifically: Among them, β1 and β2 are the gain coefficients of the tracking signal of the input and the observation value of its differential signal respectively; r1, r2, σ1, σ2 are the parameters of the adaptive control model; fal is a nonlinear function; Step S3.4: After the posture of the posture kinematic model of the robot with arms is adjusted, the output signal of the robot with arms is y(t), an error feedback unit is set, y(t) is sent to the error feedback unit, and the output signal z of the error feedback unit is fed back to the designated unit of the adaptive control model to perform closed-loop real-time feedback control on the posture of the drone with arms model; Among them, the error feedback unit is designed as: Wherein, e(t) is the tracking error of the attitude kinematics model of the UAV with arms; z1(t) and z2(t) are the tracking signal of the output signal of the error feedback unit and the derivative of the output signal of the error feedback unit, respectively; z3 is the disturbance estimate; α1 and α2 are the nonlinear parameters of the attitude kinematics model of the UAV with arms, respectively; σ is the filter coefficient; β 01 , β 02 and β 03 All are parameters, the function fal is: Wherein, e represents the error between the input and output signals; the value of α is set in the interval (0, 1).

5. The flight attitude stabilization control method of the UAV with arms according to claim 4 is characterized in that: include: In the adaptive control model, a fixed-time convergence observer is introduced to compensate for disturbances. Specifically: According to the attitude kinematics model of the UAV with arms, the disturbance after the robotic arm is introduced into the UAV model is: According to the disturbance, the state variables are designed for: A fixed-time convergence observer is introduced into the adaptive control model to compensate for disturbances and converge the error signal within a preset time. The fixed-time convergence observer is set as: in, The state vector of the observer is: Parameters satisfy: α 01 >1,α 02 <1, and k1, k2, a1, a2, a3 are all positive gains, β represents the preset parameters; Error signal e w for: The error signal converges when the error signal e w When it converges to 0, the observation error of the observer Converge to 0 within the preset time, convergence time T i The upper limit T i-max for:

6. A flight attitude stabilization control system for an unmanned aerial vehicle with arms, characterized in that: include: Module M1: Establish the attitude kinematics model of the UAV based on the UAV model; Module M2: Establish the attitude kinematics model of the UAV with arms based on the attitude kinematics model of the UAV; Module M3: Establish an adaptive control model, adjust the parameters of the attitude kinematic model of the UAV with arms in real time, and perform attitude control on the UAV with arms.

7. The flight attitude stabilization control system of the UAV with arms according to claim 6, characterized in that: The module M1 comprises: Module M1.1: Establish the position and attitude model of the drone based on the drone model: ψ represents the yaw angle of the drone; φ represents the roll angle of the drone; θ represents the pitch angle of the drone; J xx represents the moment of inertia of the drone around the x-axis, J yy represents the moment of inertia of the drone around the y-axis, J zz represents the moment of inertia of the drone around the z-axis; τ x Represents the x-axis direction vector of the input drone signal; τ y Represents the y-axis direction vector of the input drone signal; τ z Represents the z-axis direction vector of the input drone signal; Module M1.2: Establish the attitude kinematics model of the UAV based on the position equation and attitude model of the UAV: Among them, δ is the Euler angle vector of the drone, expressed as δ = [φ, θ, ψ]; τ f Represents the moment of inertia of the drone; τ d Represents the external disturbance to the UAV; the matrices N and C are the parameter matrices of the UAV's attitude kinematics model, which are: c 11 ＝0 Among them, I x ,I y ,I z Represents the inertia matrix of the drone body, S represents sin; C represents cos.

8. The flight attitude stabilization control system of the UAV with arms according to claim 6, characterized in that: Module M2 includes: Module M2.1: Add a robotic arm to the drone model; Module M2.2: Based on the attitude kinematics model of the UAV, the attitude kinematics model of the UAV with arms is established as follows: Among them, τ extra Represents the total disturbance after the robotic arm is introduced into the UAV model.

9. The flight attitude stabilization control system of the UAV with arms according to claim 6, characterized in that: The module M3 comprises: Module M3.1: Connect the tracking differentiator, control law unit and error feedback unit according to preset rules to establish an adaptive control model for adjusting the attitude of the attitude kinematic model of the UAV with arms; Module M3.2: Collect the initial signal of the attitude kinematics model of the UAV with arms as the input signal of the adaptive control model, input the input signal into the tracking differentiator for signal conversion; specifically: fh=fhan(τ1(t)-τ(t),τ2(t),r,h0) τ1(t+T)=τ1(t)+hτ2(t) τ2(t+T)=τ2(t)+h*fh Among them, the initial signal includes the Euler angle vector, moment of inertia and model parameters of the attitude kinematics model of the UAV with arms; r is the velocity factor of the function; τ1(t) represents the tracking signal of the input signal τ(t); τ2(t) represents the approximate differential of the input signal τ(t); τ(t) represents the input signal at time t; T represents the sampling period; h0 is the filtering factor; h is the sampling time, and fhan is the fastest tracking function; Module M3.3: The converted signal is transmitted to the control law unit, the signal is further processed, and the output signal u of the control law unit is sent to the attitude kinematics model of the UAV with arms, and the attitude of the attitude kinematics model of the UAV with arms is adjusted; specifically: Among them, β1 and β2 are the gain coefficients of the tracking signal of the input and the observation value of its differential signal respectively; r1, r2, σ1, σ2 are the parameters of the adaptive control model; fal is a nonlinear function; Module M3.4: After the posture of the kinematic model of the robot with arms is adjusted, the output signal of the robot with arms is y(t). An error feedback unit is set, y(t) is sent to the error feedback unit, and the output signal z of the error feedback unit is fed back to the designated unit of the adaptive control model to perform closed-loop real-time feedback control on the posture of the drone with arms model. Among them, the error feedback unit is designed as: Wherein, e(t) is the tracking error of the attitude kinematics model of the UAV with arms; z1(t) and z2(t) are the tracking signal of the output signal of the error feedback unit and the derivative of the output signal of the error feedback unit, respectively; z3 is the disturbance estimate; α1 and α2 are the nonlinear parameters of the attitude kinematics model of the UAV with arms, respectively; σ is the filter coefficient; β 01 , β 02 and β 03 All are parameters, the function fal is: Wherein, e represents the error between the input and output signals; the value of α is set in the interval (0, 1).

10. The flight attitude stabilization control system of the UAV with arms according to claim 9, characterized in that: include: In the adaptive control model, a fixed-time convergence observer is introduced to compensate for disturbances. Specifically: According to the attitude kinematics model of the UAV with arms, the disturbance after the robotic arm is introduced into the UAV model is: According to the disturbance, the state variables are designed for: A fixed-time convergence observer is introduced into the adaptive control model to compensate for disturbances and converge the error signal within a preset time. The fixed-time convergence observer is set as: in, The state vector of the observer is: Parameters satisfy: α 01 >1,α 02 <1, and k1, k2, a1, a2, a3 are all positive gains, β represents the preset parameters; Error signal e w for: The error signal converges when the error signal e w When it converges to 0, the observation error of the observer Converge to 0 within the preset time, convergence time T i The upper limit T i-max for:

Images