Rotor flight mechanical arm trajectory tracking control method

Abstract

The present application relates to a kind of rotor flight mechanical arm trajectory tracking control method, comprising the following steps: step one: in the earth coordinate system, according to rigid kinematics law obtains rotor flight mechanical arm system dynamics model;Step two: with integrated total disturbance as the estimation result, improve supercoil extended state observer, for disturbance compensation and estimation;Step three: based on the preset performance control design error conversion function, for controller design;Step four: design rotor flight mechanical arm position controller and attitude controller based on dynamic surface control.

Description

Technical Field

[0001] This invention belongs to the field of automatic control technology for aircraft, and relates to a trajectory tracking control method for a rotorcraft robotic arm. Background Technology

[0002] Quadrotor drones, due to their simple structure, high flexibility, and good maneuverability, are widely used in rescue missions and fire prevention. Combining a robotic arm with a drone allows for contact-based interaction and perception with the environment, enabling complex tasks such as aerial grabbing and supply delivery. Controlling a rotor-flying robotic arm system requires considering the interaction forces between the robotic arm and the rotor platform. Furthermore, since the rotor-flying robotic arm platform needs to contact the external environment and target objects, it is necessary to enhance the system's robustness against external interference while ensuring control accuracy. Currently, while commonly used quadrotor drone controllers have good performance, their application to rotor-flying robotic arms is less effective. For example, back-stepping controllers are prone to differential explosion due to the nonlinearity and high order of interference from the robotic arm and external disturbances, severely impacting controller performance. Dr. Swaroop proposed a Dynamic Surface Controller (DSC) to solve the problem of dimensional explosion in back-stepping design. Its design concept uses a first-order low-pass filter to filter the virtual control law, replacing complex differential calculations.

[0003] DSC avoids analytical derivation of virtual control, overcoming the "computational explosion" problem. However, existing methods only consider slowly changing robotic arm disturbances and external disturbances, while neglecting the stability of the UAV platform. Extended State Observer (ESO) can estimate and compensate for external disturbances to the control system in real time without precise model information, but it makes limited use of velocity and even acceleration information measured by sensors such as IMU and optical flow, and cannot efficiently estimate the system's strong nonlinear signals. Summary of the Invention

[0004] This invention overcomes the shortcomings of existing parameter controllers in considering robotic arm disturbances, external disturbances, and stability, and provides a trajectory tracking control method for a rotorcraft robotic arm. This invention designs an improved superspiral extended state observer and proposes a dynamic surface controller based on a preset performance function. This method, based on dynamic surface control, utilizes the concept and techniques of defined performance to introduce a smooth, bounded, and monotonically decreasing performance function to ensure the transient and steady-state error response and convergence speed of the rotorcraft robotic arm platform. An extended state observer is designed based on a superspiral algorithm to reduce the impact of strongly nonlinear external disturbances on the system. Simultaneously, an adaptive mass estimation method is designed to reduce disturbances during object grasping. The technical solution is as follows:

[0005] A method for trajectory tracking and control of a rotorcraft robotic arm includes the following steps:

[0006] Step 1: Obtain the dynamic model of the rotor-flying robotic arm system in the Earth coordinate system according to the laws of rigid body kinematics;

[0007] Step 2: Using the integrated total disturbance as the estimation result, improve the superspiral extended state observer to compensate for and estimate the disturbance;

[0008] Step 3: Design an error transformation function based on preset performance control for controller design;

[0009] Step 4: Design the position controller and attitude controller for the rotorcraft flying robot based on dynamic surface control.

[0010] Furthermore, the method for step one is as follows:

[0011] The Earth coordinate system is defined as Σ I ={X I ,Y I Z I The body coordinate system is defined as Σ. B ={X B ,Y B Z B};The rotorcraft flying robotic arm system relative to the Earth coordinate system Σ I Position and velocity are defined as p = [x, y, x] T and v = [v x ,v y ,v z ] T Its attitude is represented by Φ = [φ, θ, ψ] T Indicated as roll angle, pitch angle, and yaw angle, respectively, from Σ B To Σ I The rotation matrix is I R B It is expressed as follows:

[0012]

[0013] Where C and S represent the abbreviations for the trigonometric functions cos(.) and sin(.), respectively;

[0014] In the Earth coordinate system Σ I The dynamic model of the rotor-flying robotic arm system is obtained based on the laws of rigid body kinematics:

[0015]

[0016] Among them, the total weight of the m-rotor flying robotic arm system, F m,x F m,y Fm,z M m,φ M m,θ M m,ψ The forces and torques representing the interaction between the robotic arm and the drone, mc q,n (n = x, y, z, φ, θ, ψ) represents the centroid offset term, g represents gravitational acceleration, and I x ,I y ,I z d represents the moment of inertia. e,n (n=x,y,z,φ,θ,ψ) represents the external disturbances experienced by the UAV. U1,U2,U3,U4 represent the control inputs of the UAV propellers. U1 is the total lift in the z-direction of the propellers, U2 and U3 are the rotational torques of the front and rear channels and the left and right channels, respectively, and U4 is the rotational torque in the fuselage direction. The thrust and torque are generated by the four propellers. The relationship between the thrust or torque and the propeller speed is expressed as:

[0017]

[0018] Among them, c T and c M These represent the thrust coefficient and torque coefficient, respectively; l represents the distance from the propeller to the center of the rotor-flying robotic arm system; ω i i = 1, ..., 4 represents the speed of each motor.

[0019] Furthermore, the method for step two is as follows:

[0020] Treating uncertainty and environmental disturbance as a single integrated disturbance, the dynamic model of the rotorcraft flying manipulator system in Equation (2) is simplified as follows:

[0021]

[0022] in, U x =SψSφ+CψSθCφ and U y =-CψSφ+SψSθ Cφ is an intermediate variable used to calculate the set roll and pitch angles, h i Let |h| be the derivative of the total perturbation, and assume that |h| i |<κ i This means that the total disturbance is bounded;

[0023] An improved superspiral extended state observer, based on the integrated total perturbation as the estimation result, is constructed as follows:

[0024]

[0025] in, Here, is the `softsign` function, used to improve the chattering phenomenon in the original superspiral extended state observer, where λ is a given positive constant. and Representing x respectively i,2 The estimate and d i The estimate, and To estimate the error, β i,1 and β i,2 This is the observer gain.

[0026] Furthermore, the method for step three is as follows:

[0027] Error transformation follows the concept of preset performance control, with a preset performance function η. i (t) is defined as:

[0028]

[0029] Here η i,0 >η i,∞ , χ i The set positive constant; in order to meet the preset motion control performance, the tracking error satisfies the following inequality:

[0030]

[0031] in, k i and e is a set positive constant. i,1 =x i,1 -x i,d To track errors, Let x be the set trajectory, where x d ,y d ,z d and It is given, and φ d and θ d The calculation is performed using the following formula:

[0032]

[0033] To ensure that the error remains within acceptable limits, the controller error transfer function is designed as follows:

[0034]

[0035] For conversion error, A function that is strictly monotonically increasing is defined as:

[0036]

[0037] It has the following properties:

[0038] (1)

[0039] (2)

[0040] Conversion error Written as:

[0041]

[0042] in ω i (t) is abbreviated as ω i , will η i (t) is abbreviated as η i Further calculate the derivative of the conversion error. for:

[0043]

[0044] in It is a bounded variable.

[0045] Furthermore, step four employs a cascade control method, as follows:

[0046] 1) Position controller

[0047] The dynamic equation for the position of the UAV can be simplified to the following form using formula (4):

[0048]

[0049] Define the first error integral surface as The Lyapunov function selected is:

[0050]

[0051] Its derivative is calculated as follows:

[0052]

[0053] Assume a first-order inertial element satisfies z i,2 =x i,2 -x i,2v γ i =x i,2v -α i , where z i,2 Represents the physical quantity x i,2 The actual value and the expected value, i.e., the virtual control quantity x i,2v The error, γ i Represents the physical quantity z i,2Expected value x i,2v With the input α of the first-order inertial element i The error between them is:

[0054]

[0055] The input quantity α of the first-order inertial element i Set to:

[0056]

[0057] Among them, c i,1 Since it is a set positive parameter, therefore:

[0058]

[0059] use Let m represent the estimated total mass of the drone. The second Lyapunov function selected is:

[0060]

[0061] The derivative of V2 is calculated as follows:

[0062]

[0063] The control input is designed as follows:

[0064]

[0065] Among them, c i,2 Estimate the mass using the set positive parameters. The adaptive law is:

[0066]

[0067] Therefore, formula (34) is recalculated as follows:

[0068]

[0069] By designing parameter c i,1 and c i,2 To keep the dynamic equation (23) of the UAV position stable;

[0070] 2) Attitude controller

[0071] The non-dynamic equations of the UAV attitude can be simplified to the following form using formula (4):

[0072]

[0073] Define the first error integral surface as The Lyapunov function selected is:

[0074]

[0075] Its derivative is then calculated as:

[0076]

[0077] Assume a first-order inertial element satisfies z i,2 =x i,2 -x i,2v γ i =x i,2v -α i ;z i,2 Represents the physical quantity x i,2 The actual value and the expected value, i.e., the virtual control quantity x i,2v The error, γ i Represents the physical quantity z i,2 Expected value x i,2v With the input α of the first-order inertial element i The error between them is:

[0078]

[0079] The input quantity α of the first-order inertial element i Set to:

[0080]

[0081] Among them, c i,1 Since it is a set positive parameter, therefore:

[0082]

[0083] The second Lyapunov function selected is:

[0084]

[0085] Its derivative is calculated as:

[0086]

[0087] The input to the attitude controller is designed as follows:

[0088]

[0089] By designing parameter c i,1 and c i,2 The dynamic equation (24) of the UAV attitude is kept stable. Attached Figure Description

[0090] Figure 1 flow chart

[0091] Figure 2 Rotary-wing flying robotic arm structure diagram

[0092] Figure 3 Control circuit diagram of a quadcopter with robotic arm Detailed Implementation

[0093] The present invention will now be described in conjunction with the accompanying drawings and embodiments.

[0094] The specific implementation steps of this invention are as follows:

[0095] Step 1: Dynamic Modeling of the Rotary Wing Manipulator System

[0096] The Earth coordinate system is defined as Σ I ={X I ,Y I Z I The body coordinate system is defined as Σ. B ={X B ,Y B Z B The rotorcraft robotic arm system relative to the Earth coordinate system Σ I Position and velocity are defined as p = [x, y, x] T and v = [v x ,v y ,v z ] T Its attitude is represented by Φ = [φ, θ, ψ] T Indicated as roll angle, pitch angle, and yaw angle, respectively, from Σ B To Σ I The rotation matrix is I R B It is expressed as follows:

[0097]

[0098] Here, C and S represent the abbreviations for the trigonometric functions cos(.) and sin(.), respectively.

[0099] The rotorcraft's flight arm is affected by model uncertainties and environmental disturbances. Model uncertainties include uncertain terms, center of gravity shifts, and coupled interaction forces / torques caused by the arm's motion. In the Earth coordinate system Σ I Based on the laws of rigid body kinematics, the dynamic model of the rotorcraft flying robot system can be obtained as follows:

[0100]

[0101] Among them, the total weight of the m-rotor flying robotic arm system, F m,xF m,y F m,z M m,φ M m,θ M m,ψ The forces and torques representing the interaction between the robotic arm and the drone, mc q,n (n = x, y, z, φ, θ, ψ) represents the centroid offset term, g represents gravitational acceleration, and I x ,I y ,I z d represents the moment of inertia. e,n (n=x,y,z,φ,θ,ψ) represents the external disturbances experienced by the UAV. U1,U2,U3,U4 represent the control inputs of the UAV propellers. U1 is the total lift in the z-direction of the propellers, U2 and U3 are the rotational torques of the front and rear channels and the left and right channels, respectively, and U4 is the rotational torque in the fuselage direction. (Thrust and torque are generated by the four propellers. The relationship between thrust / torque and propeller speed is expressed as:)

[0102]

[0103] Among them, c T and c M These represent the thrust coefficient and torque coefficient, respectively; l represents the distance from the propeller to the center of the rotor-flying robotic arm system; ω i (i = 1, ..., 4 represents the speed of each motor.)

[0104] Step 2: Design an improved superspiral extended state observer for compensating for and estimating disturbances.

[0105] This invention treats uncertainty and environmental disturbance as an integrated total disturbance, and the dynamic model in formula (2) can be simplified as follows:

[0106]

[0107] in, U x =SψSφ+CψSθCφ and U y =-CψSφ+SψSθ Cφ is an intermediate variable used to calculate the set roll and pitch angles, h i Let |h| be the derivative of the total perturbation, and assume that |h| i |<κ i This means that the total disturbance is bounded.

[0108] An improved superspiral extended state observer, based on the integrated total perturbation as the estimation result, is constructed as follows:

[0109]

[0110] in, Here, is the `softsign` function, used to improve the chattering phenomenon in the original superspiral extended state observer, where λ is a given positive constant. and Representing x respectively i,2 The estimate and d i The estimate, and To estimate the error, β i,1 and β i,2 Let be the observer gain. Subtracting equation (4) from equation (5), we obtain the error equation:

[0111]

[0112] Introduce a variable Then |σ i |<1. Will Substituting into the error equation (6), we get:

[0113]

[0114] definition δ i,2 =ε i,2 The following conclusions can be drawn:

[0115] (1) In a finite time, if δ i,1 →0,δ i,2 →0, then ε i,1 →0,ε i,2 →0.

[0116] (2)|δ i,1 | 2 =|ε i,1 |,sign(δ i,1 ) = sign(ε i,1 ).

[0117] δ i,1 and δ i,1 The derivative is:

[0118]

[0119] Substituting (8) into (7) yields:

[0120]

[0121] Considering δ i,2 =ε i,2 q i =1-|σ i |>0,(9) is rewritten as:

[0122]

[0123] Define the following matrix to construct a quadratic-like Lyapunov function for stability analysis:

[0124]

[0125]

[0126]

[0127]

[0128] Among them, v i =d i sign(ε i,1 Define the following class of quadratic Lyapunov functions:

[0129]

[0130] Taking its derivative, we get:

[0131]

[0132] Q i =-((A) i +M i ) T P i +P i (A i +M i Substituting matrices (11)-(14) into the equations, we get:

[0133]

[0134] To make Q i Minimum eigenvalue λ i,min >0, only the following conditions need to be met:

[0135]

[0136] Therefore, the Lyapunov function The following inequalities must be satisfied:

[0137]

[0138] According to Lyapunov's stability theorem, in a finite time interval δ i,1 →0,δ i,2 →0, meaning the estimation error ε i,1 →0,ε i,2→0 means that the estimation error of the total disturbance can converge to 0 in a finite amount of time.

[0139] Step 3: Design an error transformation function based on a preset performance control design for controller design.

[0140] Error transformation follows the concept of preset performance control, with a preset performance function η. i (t) is defined as:

[0141]

[0142] Here η i,0 >η i,∞ , χ i This is a set positive constant. To meet the preset motion control performance, the tracking error should satisfy the following inequality:

[0143]

[0144] in, k i and e is a set positive constant. i,1 =x i,1 -x i,d To track errors, Let x be the set trajectory, where x d ,y d ,z d and It is given, and φ d and θ d The calculation needs to be performed using the following formula:

[0145]

[0146] To ensure that the error remains within acceptable limits, the controller error transfer function is designed as follows:

[0147]

[0148] For conversion error, A function that is strictly monotonically increasing is defined as:

[0149]

[0150] It has the following properties:

[0151] (3)

[0152] (4)

[0153] Conversion error Written as:

[0154]

[0155] in ω i (t) is abbreviated as ω i , will η i (t) is abbreviated as η i Further calculate the derivative of the conversion error. for:

[0156]

[0157] in It is a bounded variable.

[0158] Step 4: Design of the position controller and attitude controller for the rotorcraft flying arm

[0159] The rotorcraft robotic arm adopts a cascade control scheme.

[0160] 1) Design of position controller

[0161] The non-dynamic equation for the UAV's position can be simplified to the following form using formula (4):

[0162]

[0163] Define the first error integral surface as The Lyapunov function selected is:

[0164]

[0165] Its derivative is calculated as follows:

[0166]

[0167] Assume a first-order inertial element satisfies z i,2 =x i,2 -x i,2v γ i =x i,2v -α i , where z i,2 Represents the physical quantity x i,2 The actual value and the expected value, i.e., the virtual control quantity x i,2v The error, γ i Represents the physical quantity z i,2 Expected value x i,2v With the input α of the first-order inertial element i The error between them is:

[0168]

[0169] The input quantity α of the first-order inertial element i Set to:

[0170]

[0171] Among them, c i,1 Since it is a set positive parameter, therefore:

[0172]

[0173] use Let m represent the estimated total mass of the drone. The second Lyapunov function selected is:

[0174]

[0175] The derivative of V2 is calculated as follows:

[0176]

[0177] The control input is designed as follows:

[0178]

[0179] Among them, c i,2 Estimate the mass using the set positive parameters. The adaptive law is:

[0180]

[0181] Formula (34) is recalculated as follows:

[0182]

[0183] By designing parameter c i,1 and c i,2 The dynamic equation (27) for the position of the UAV is kept stable.

[0184] 2) Design of the attitude controller

[0185] The non-dynamic equations of the UAV attitude can be simplified to the following form using formula (4):

[0186]

[0187] Define the first error integral surface as The Lyapunov function selected is:

[0188]

[0189] Its derivative is then calculated as:

[0190]

[0191] Assume a first-order inertial element satisfies z i,2 =x i,2 -x i,2v γ i =x i,2v -α i z i,2 Represents the physical quantity x i,2 The actual value and the expected value, i.e., the virtual control quantity x i,2v The error, γ i Represents the physical quantity z i,2 Expected value x i,2v With the input α of the first-order inertial element i The error between them is:

[0192]

[0193] The input quantity α of the first-order inertial element i Set to:

[0194]

[0195] Among them, c i,1 Since it is a set positive parameter, therefore:

[0196]

[0197] The second Lyapunov function selected is:

[0198]

[0199] Its derivative is calculated as:

[0200]

[0201] Therefore, the input of the attitude controller is designed as follows:

[0202]

[0203] Therefore, by designing parameter c i,1 and c i,2 The dynamic equation (38) of the UAV attitude is kept stable.

Claims

1. A method for trajectory tracking and control of a rotorcraft, comprising the following steps: Step 1: Obtain the dynamic model of the rotorcraft flying robot system in the Earth coordinate system according to the laws of rigid body kinematics, as follows: The earth coordinate system is defined as , the body coordinate system is defined as ; the position and velocity of the rotor flight manipulator system relative to the earth coordinate system are defined as and , the attitude is represented by , which is the roll angle, the pitch angle and the yaw angle, and the rotation matrix from to is represented by . （1） in, C and S denote the abbreviations for the trigonometric functions cos (.) and sin (.) respectively; In the earth coordinate system Next, the dynamics model of the rotor flight manipulator system is obtained according to the rigid body kinematics law: （2） in, m The total weight of the rotorcraft flight arm system Represents the interaction force and torque between the robotic arm and the drone. Indicates the centroid offset term. g Represents gravitational acceleration. Indicates the moment of inertia. This indicates external interference experienced by the drone. The control inputs representing the drone's propellers. The total lift in the z-direction of the propeller is [value missing]. , respectively, represent the rotational torque of the front and rear channels and the left and right channels; , represents the rotational torque in the fuselage direction. Thrust and torque are generated by the four propellers. The relationship between thrust or torque and propeller speed is expressed as: （3） in, and These represent the thrust coefficient and the torque coefficient, respectively. l This represents the distance from the propeller to the center of the rotorcraft arm system. This represents the rotational speed of each motor; Step 2: Using the integrated total disturbance as the estimation result, improve the superspiral extended state observer for compensation and disturbance estimation, as follows: Treating uncertainty and environmental disturbance as a single integrated total disturbance, the dynamic model of the rotorcraft flying manipulator system in Equation (2) is simplified as follows: （4） in, , , , ， ， ， ， and These are intermediate variables used to calculate the set roll and pitch angles. Let be the derivative of the total perturbation, and assume... This means that the total disturbance is bounded; An improved superspiral extended state observer, based on the integrated total perturbation as the estimation result, is constructed as follows: （5） in, for softsign This function is used to improve the chattering phenomenon in the original superspiral extended state observer. Given a positive constant, and Represent The estimate and The estimate, and To estimate the error, and To obtain the observer gain, subtract equation (5) from equation (4) to get the error equation: （6）； Step 3: Design an error transformation function based on preset performance control for controller design, as follows: Error transformation follows the principle of preset performance control, with a preset performance function. Defined as: （20） here , The set positive constant; in order to meet the preset motion control performance, the tracking error satisfies the following inequality: （21） in, and For the set positive numbers, To track errors, For the set trajectory, where and It is given, and and The calculation is performed using the following formula: （22） To ensure that the error remains within acceptable limits, the controller error transfer function is designed as follows: （23） For conversion error, For a strictly monotonically increasing function, it is defined as: （24） It has the following properties: Conversion error Written as: （25） in ,Will Abbreviated as ,Will Abbreviated as Further calculate the derivative of the conversion error. for （26） in , is a bounded variable; Step 4: Design the position controller and attitude controller for the rotorcraft flying robot based on dynamic surface control.

2. The trajectory tracking and control method for a rotorcraft flying robotic arm according to claim 1, characterized in that, Step four uses a cascade control method, as follows: 1) Design of position controller The dynamic equation for the position of the UAV can be simplified to the following form using formula (4): （27） Define the first error integral surface as The Lyapunov function selected is: （28） Its derivative is calculated as follows: （29） Assume a first-order inertial element satisfies , , ,in, Representing physical quantities The actual value and the expected value, i.e., the virtual control quantity The error, Representing physical quantities Expected value Input quantity of first-order inertial element The error between them, （30） Input quantity of the first-order inertial element Set as 31） in, Since it is a set positive parameter, therefore （32） use Indicates the total mass of the drone The estimated value, then The second Lyapunov function selected is: （33） The derivative is calculated as follows: （34） Design the control input as （35） in, Estimate the mass using the set positive parameters. The adaptive law is （36） Therefore, formula (34) is recalculated as （37） Through design parameters and To keep the dynamic equation (27) of the UAV position stable; 2) Design of the attitude controller The non-dynamic equations of the UAV attitude can be simplified to the following form using formula (4): （38） Define the first error integral surface as The Lyapunov function selected is: （39） Its derivative is then calculated as （40） Assume a first-order inertial element satisfies , , ; Representing physical quantities The actual value and the expected value, i.e., the virtual control quantity The error, Representing physical quantities Expected value Input quantity of first-order inertial element The error between them is: （41） Input quantity of the first-order inertial element Set as （42） in, Since it is a set positive parameter, therefore （43） The second Lyapunov function selected is: （44） Its derivative is calculated as: （45） Design the input of the attitude controller as （46） Through design parameters and The dynamic equation (38) of the UAV attitude is kept stable.

Images