Rotor wing flight mechanical arm sliding mode control method

Abstract

The invention relates to a rotor wing flight mechanical arm sliding mode control method. A controller design method comprises the following steps: step 1, establishing a rotor wing flight mechanical arm system dynamics model; 2, a rotor wing flight mechanical arm system kinematics model is established; 3, in consideration of the influence of the time-varying external environment and the model uncertainty on the rotor wing flight mechanical arm system in the process of picking up and releasing the target object in the air, rewriting a dynamic model of the rotor wing flight mechanical arm system, and converting the dynamic model into a state space form; step 4, designing a fractional order sliding mode surface function to change the speed and the sliding mode value when the system state reaches the sliding mode surface so as to avoid impact and system buffeting; and 5, a fast terminal sliding mode reaching law is adopted to ensure the control precision of the controller and reduce buffeting, an adaptive law is introduced into the reaching law to adjust the control gain and improve the tracking performance, and an interference estimator based on an RBF neural network is designed.

Description

Technical Field

[0001] The invention relates to a sliding mode control method for a rotor flight mechanical arm. Background Art

[0002] A rotorcraft manipulator is a flying robot composed of a single rotorcraft drone and one or more manipulators. The robot system integrates the advantages of rotorcraft drones and manipulator systems, and has more complete functions than single-type robots. This composite robot system consisting of a mobile platform and a manipulator has been used for many years in underwater and land applications, such as disaster relief, underwater resource exploration and development, etc. The rotorcraft manipulator is a complex system that is nonlinear, strongly coupled, and significantly affected by external interference, and has high requirements for the robustness of the controller. At present, many control methods have been applied to the rotorcraft manipulator system, such as PID control, sliding mode control, adaptive control, fuzzy control, delay estimation control, model predictive control, and their hybrid control. Among them, sliding mode control has a simple structure, fast response speed, can overcome the uncertainty of the system, has strong robustness to external interference and unmodeled dynamics, and has good control effect on the control of nonlinear systems. It is widely used in rotorcraft manipulators.

[0003] Conventional sliding mode control in rotorcraft manipulator systems is highly robust to system nonlinearity and uncertainty, and can respond quickly to system control inputs. However, there are problems such as high-frequency jitter caused by frequent switching, over / under regulation of the system due to inappropriate switching gain, and the inability to guarantee control accuracy under unknown external interference. Summary of the invention

[0004] Aiming at the trajectory tracking control problem of a rotorcraft manipulator under external environmental interference (such as wind, noise, etc.), the present invention provides a sliding mode control method for a rotorcraft manipulator with a faster system response speed, which can eliminate static errors, reduce system chattering, and improve anti-interference ability and tracking performance. The technical solution is as follows:

[0005] A sliding mode control method for a rotor flight manipulator, wherein the controller design includes the following steps:

[0006] Step 1, establishing a dynamic model of the rotor flight manipulator system;

[0007] Step 2, establishing a kinematic model of the rotor flight manipulator system;

[0008] Step 3, considering that the rotorcraft flight manipulator system is affected by the time-varying external environment and model uncertainty during the process of picking up and releasing the target object in the air, the dynamic model of the rotorcraft flight manipulator system is rewritten and converted into the form of state space;

[0009] Step 4: Design a fractional-order sliding surface function to change the speed and sliding value when the system state reaches the sliding surface to avoid shock and system chattering.

[0010] Step 5: Use the fast terminal sliding mode reaching law to ensure the control accuracy of the controller and reduce jitter. Introduce the adaptive law into the reaching law to adjust the control gain and improve the tracking performance. Design the disturbance estimator based on RBF neural network to estimate and compensate for the disturbance of external environment and model uncertainty.

[0011] Step 1 is as follows:

[0012] According to the rotor flight manipulator model, the three reference coordinate systems Σ I ,Σ B and Σ i They are defined as the earth coordinate system, the body coordinate system and the robot arm link coordinate system, i = 1, 2, 3 represents the serial number of each robot arm link, and all reference coordinate systems are located at the geometric center of the corresponding rigid body; the coordinate variable contains the coordinate system of the quadrotor in the coordinate system Σ I The position p in the T , Euler angle And the joint angle of each link η=[η1,η2,η3] T , the vector of coordinate variables is expressed as:

[0013] q=[p T ,Φ T ,η T ] T (1)

[0014] From Σ B to Σ I The rotation matrix It is expressed as follows:

[0015]

[0016] Where C and S are the abbreviations of trigonometric functions cos(.) and sin(.) respectively; and They are defined as the quadrotor in the reference coordinate system Σ I and Σ B The speeds, ω and ω b is the quadrotor in the reference coordinate system Σ I and Σ B The angular velocity in has the following relationship:

[0017]

[0018] ω=[ω x ω y ω z ]T (4)

[0019]

[0020] The Euler angular velocity can be converted to the following through the transformation matrix T:

[0021]

[0022] Relative to the body coordinate system Σ B The position in is represented by It is related to the reference coordinate system Σ i The position p i There are the following relationships:

[0023]

[0024] The acceleration of the i-th link and angular acceleration The formula is:

[0025]

[0026] Among them J t,i and J r,i is the Jacobian matrix; from formulas (9), (10) and (11), the reference coordinate system Σ i Relative to the Earth coordinate system Σ I The acceleration and angular acceleration are expressed as:

[0027]

[0028] The operator S(·) transforms a vector into an antisymmetric matrix, rewriting equations (3), (7), (12) and (13) as follows:

[0029]

[0030] in is a 3×3 unit matrix, 03 is a 3×3 zero matrix, B t,b , B r,b , B t,i and B r,i is the defined matrix, and B r,b =[03 T 03], Combining equations (14)-(17), the derivatives of the state variables in the earth coordinate system are Establish a connection with the rotor flight manipulator system state and the speed and angular velocity of each of its links.

[0031] Step 2 is as follows:

[0032] The Euler-Lagrange equations for establishing a rotorcraft flight manipulator system are used:

[0033]

[0034] in Kinetic energy of the rotor flight manipulator system and potential energy The Lagrangian function of the rotor flight manipulator system is τ, ext is the external disturbance of the rotor flight manipulator system, kinetic energy and its components are calculated as follows:

[0035]

[0036] Where m represents the mass matrix, I represents the inertia matrix, subscript b represents the body regarded as a rigid body, and subscript i represents the serial number of each robot arm link. Represents the reference frame Σ I To the body coordinate system Σ B The rotation matrix, potential energy The calculation is as follows:

[0037]

[0038] where e3 = [0 0 1] T , g is the acceleration due to gravity;

[0039] Substituting (19), (20) and (23) into (18), we can obtain the kinematic model of the rotorcraft flight manipulator system:

[0040]

[0041] Where M(q) is the inertia matrix, represents the Coriolis matrix, G(q) represents the gravity matrix; using formulas (14)-(17), the inertia matrix M(q) is calculated as:

[0042]

[0043] The Coriolis matrix is ​​derived by computing each element of the kth row and jth column:

[0044]

[0045] Where m αβ represents the αth row and βth column elements of the inertia matrix M(q); the gravity matrix G(q) is calculated using the following partial derivatives:

[0046]

[0047] Step 3 is as follows:

[0048] Considering that the rotorcraft flight manipulator system is affected by the time-varying external environment and model uncertainty during the process of picking up and releasing the target object in the air, the kinematic model (24) of the rotorcraft flight manipulator system is rewritten as follows:

[0049]

[0050] in represents the interference of external environment and model uncertainty, and They represent the deviation values ​​of inertia matrix, Coriolis matrix and gravity matrix between the derived model and the actual model respectively;

[0051] Define x1 = q and The rewritten dynamic model (28) is transformed into the following state space form:

[0052]

[0053] Step 4 is as follows:

[0054] definition The desired position of the rotorcraft manipulator system in the Earth coordinate system, is the desired velocity, then the trajectory tracking error and its derivative are expressed as follows:

[0055]

[0056] Design the following fractional-order sliding surface function:

[0057]

[0058] where k i =diag(k i1 ,…,k i9 ),i=1,2,k i1 >0,0<λ1,λ2<1, is a fractional order function, defined as follows

[0059]

[0060] is a fractional function, where Γ(np) is the factorial, Γ(np) = (n-p+1)!, then the derivative of the fractional sliding surface function is calculated as follows:

[0061]

[0062] From formula (29), formula (33) is further expressed as

[0063]

[0064] Step 5 is as follows

[0065] According to the fractional-order sliding surface function, the equivalent control law of the rotor flight manipulator system is obtained:

[0066]

[0067] where u eq is the equivalent control input, let The equivalent control law is simplified as follows:

[0069]

[0070] In order to ensure that the second-layer sliding surface of the system has a global fast convergence speed, the fast terminal sliding surface is designed with the following reaching law:

[0071]

[0072] Among them, α1 is the set parameter, is the adaptive parameter, sig(s) ρ =|s| ρ sgn(s) and thus the switching control law can be designed as:

[0073]

[0074] Adaptive parameters The adaptive law is:

[0075]

[0076] in For the adaptive parameters, RBF neural network is used to approximate the external loop interference and the dynamic uncertainty term F(x1) of the system. As the input of the neural network, the designed disturbance control law is:

[0077]

[0078] in:

[0079]

[0080] c j and b j is the set parameter, adaptive parameter The adaptive law is:

[0081]

[0082] Among them B W and k W is the adaptive parameter of the RBF neural network estimator, so the overall control law is designed as follows:

[0083]

[0084] The present invention designs a controller through fractional-order sliding mode control, fast terminal sliding mode control, adaptive control and RBF neural network. The speed and sliding mode value when the system state reaches the sliding mode surface are changed through the fractional-order sliding mode surface to avoid causing a large impact and system chattering. The memory and heredity of the fractional-order calculus operator are used to reduce the switching probability in the approaching stage and improve the continuity of the system control behavior. The fast terminal sliding mode surface is used to ensure that the system reaches the sliding mode surface within a finite time, so that the system state converges rapidly to an equilibrium state within a finite time. The adaptive law is used to adjust the control gain and improve the tracking performance. The interference estimator based on the RBF neural network is designed to perform online dynamic estimation and compensation for the interference of the external environment and model uncertainty, thereby reducing the influence of the interference on the composite system. BRIEF DESCRIPTION OF THE DRAWINGS

[0085] Figure 1 Controller Design Process

[0086] Figure 2 Rotor flight manipulator system model

[0087] Figure 3 Rotor flight manipulator control system structure DETAILED DESCRIPTION

[0088] The present invention is described below in conjunction with the accompanying drawings and embodiments.

[0089] The present invention firstly models the dynamics and kinematics of the rotor flight manipulator system as a whole, and then designs a fractional-order sliding surface according to the UAV dynamics equation to achieve a faster response speed of the control system and eliminate static errors. A fast terminal sliding mode reaching law is used to ensure the control accuracy of the controller and reduce chattering. An adaptive law is introduced into the reaching law to adjust the control gain and improve the tracking performance. Finally, an interference estimator based on RBF neural network is designed to estimate and compensate for the interference of the external environment and model uncertainty, so that the system can obtain a good trajectory tracking effect under the influence of dynamic uncertainty and time-varying external interference. The flow chart is as follows Figure 1 shown.

[0090] 1. Establish the dynamics and kinematics model of the rotor flight manipulator system

[0091] (1) Kinetic model

[0092] According to the rotor flight manipulator model, the three reference coordinate systems Σ I ,Σ B and Σ i They are defined as the earth coordinate system, the body coordinate system, and the robot arm link coordinate system. i = 1, 2, 3 represents the serial number of each robot arm link. All reference coordinate systems are located at the geometric center of the corresponding rigid body. The coordinate variables include the coordinate system of the quadrotor in the coordinate system Σ I The position p in the T , Euler angle And the joint angle of each link η=[η1,η2,η3] T , the vector of coordinate variables is expressed as:

[0093] q=[p T ,Φ T ,η T ] T (1)

[0094] From Σ B to Σ I The rotation matrix It is expressed as follows:

[0095]

[0096] Where C and S are the abbreviations of trigonometric functions cos(.) and sin(.). and They are defined as the quadrotor in the reference coordinate system Σ I and Σ B The speeds, ω and ω b is the quadrotor in the reference coordinate system Σ I and Σ B The angular velocity in has the following relationship:

[0097]

[0098] ω=[ω x ω y ω z ] T (4)

[0099]

[0100] The Euler angular velocity can be converted to the following through the transformation matrix T:

[0101]

[0102] Relative to the body coordinate system Σ B The position in is represented by It is related to the reference coordinate system Σ i The position p i There are the following relationships:

[0103]

[0104] The acceleration of the i-th link and angular acceleration The formula is:

[0105]

[0106] Among them J t,i and J r,i is the Jacobian matrix. According to formulas (9), (10) and (11), the reference coordinate system Σ i Relative to the Earth coordinate system Σ I The acceleration and angular acceleration are expressed as:

[0107]

[0108] The operator S(·) transforms a vector into an antisymmetric matrix, rewriting equations (3), (7), (12) and (13) as follows:

[0109]

[0110] in is a 3×3 unit matrix, 03 is a 3×3 zero matrix, B t,b , B r,b , B t,i and B r,i is the defined matrix, and

[0111] B r,b =[03T 03],

[0112] Combining equations (14)-(17), the derivatives of the state variables in the earth coordinate system are Establish a connection with the rotor flight manipulator system state and the speed and angular velocity of each of its links.

[0113] (2) Kinematic model

[0114] The kinematic model of the rotor flight manipulator system is established using the following Euler-Lagrange equation:

[0115]

[0116] in Kinetic energy of the rotor flight manipulator system and potential energy The Lagrangian function of the rotor flight manipulator system is τ, ext is the external disturbance of the rotor flight manipulator system, kinetic energy and its components are calculated as follows:

[0117]

[0118] Where m represents the mass matrix, I represents the inertia matrix, subscript b represents the body regarded as a rigid body, and subscript i represents the serial number of each robot arm link. Represents the reference frame Σ I To the body coordinate system Σ B The rotation matrix, potential energy The calculation is as follows:

[0119]

[0120] where e3 = [0 0 1] T , g is the acceleration due to gravity.

[0121] Substituting (19), (20) and (23) into (18), we obtain:

[0122]

[0123] Where M(q) is the inertia matrix, represents the Coriolis matrix, and G(q) represents the gravity matrix. Using formulas (14)-(17), the inertia matrix M(q) is calculated as:

[0124]

[0125] The Coriolis matrix is ​​derived by computing each element of the kth row and jth column:

[0126]

[0127] Where m αβ represents the αth row and βth column element of the inertia matrix M(q). The gravity matrix G(q) is calculated using the following partial derivatives:

[0128]

[0129] 2. Design of trajectory tracking controller for rotor flight manipulator system

[0130] Considering that the rotorcraft flight manipulator system is affected by the time-varying external environment and model uncertainty during the process of picking up and releasing the target object in the air, the dynamic model (24) of the rotorcraft flight manipulator system is rewritten as follows:

[0131]

[0132] in represents the interference of external environment and model uncertainty, and They represent the deviation values ​​of inertia matrix, Coriolis matrix and gravity matrix between the derived model and the actual model respectively.

[0133] Define x1 = q and Then the kinematic model (28) is transformed into the following state space form:

[0134]

[0135] definition The desired position of the rotorcraft manipulator system in the Earth coordinate system, is the desired velocity, then the trajectory tracking error and its derivative are expressed as follows:

[0136]

[0137] Design the following fractional-order sliding surface function:

[0138]

[0139] where k i =diag(k i1 ,…,k i9 ),i=1,2,k i1 >0,0<λ1,λ2<1, is a fractional order function, defined as follows

[0140]

[0141] is a fractional function, where Γ(np) is the factorial, Γ(np) = (n-p+1)!, then the derivative of the fractional sliding surface function is calculated as follows:

[0142]

[0143] From formula (29), formula (33) can be further expressed as

[0144]

[0145] So the equivalent control law of the system is as follows:

[0146]

[0147] where u eq is the equivalent control input, let The equivalent control law is simplified as follows:

[0149]

[0150] In order to ensure that the second-layer sliding surface of the system has a global fast convergence speed, the fast terminal sliding surface is designed with the following reaching law:

[0151]

[0152] Among them, α1 is the set parameter, is the adaptive parameter, sig(s) ρ =|s| ρ sgn(s) and thus the switching control law can be designed as:

[0153]

[0154] Adaptive parameters The adaptive law is:

[0155]

[0156] in For the adaptive parameters, RBF neural network is used to approximate the external loop interference and the dynamic uncertainty term F(x1) of the system. As the input of the neural network, the designed disturbance control law is:

[0157]

[0158] in:

[0159]

[0160] c j and b j is the set parameter, adaptive parameter The adaptive law is:

[0161]

[0162] Among them B W and k W is the adaptive parameter of the RBF neural network estimator, so the overall control law is designed as follows:

[0163]

[0164] The controller designed by the present invention is mainly composed of a fractional sliding surface, a fast terminal sliding surface, an adaptive law and an interference estimator based on an RBF neural network. The fractional sliding surface is used to achieve a faster response speed of the control system and eliminate static errors, the fast terminal sliding surface is used to ensure the control accuracy of the controller and reduce chattering, the adaptive law is used to adjust the control gain and improve the tracking performance, and the interference estimator based on the RBF neural network estimates and compensates for the interference of the external environment and model uncertainty, thereby reducing the influence of the interference on the composite system. The controller based on the above components will ensure that the rotor flight manipulator system obtains a good trajectory tracking effect under the influence of dynamic uncertainty and time-varying external interference, thereby completing a precise control task.

Claims

1. A sliding mode control method for a rotor flight manipulator, wherein the controller design comprises the following steps: Step 1, establishing a dynamic model of the rotor flight manipulator system; Step 2, establishing a kinematic model of the rotor flight manipulator system; Step 3, considering that the rotorcraft flight manipulator system is affected by the time-varying external environment and model uncertainty during the process of picking up and releasing the target object in the air, the dynamic model of the rotorcraft flight manipulator system is rewritten and converted into the form of state space; Step 4: Design a fractional-order sliding surface function to change the speed and sliding value when the system state reaches the sliding surface to avoid shock and system chattering. Step 5: Use the fast terminal sliding mode reaching law to ensure the control accuracy of the controller and reduce jitter. Introduce the adaptive law into the reaching law to adjust the control gain and improve the tracking performance. Design the disturbance estimator based on RBF neural network to estimate and compensate for the disturbance of external environment and model uncertainty.

2. The sliding mode control method of a rotor flight manipulator according to claim 1, characterized in that: Step 1 is as follows: According to the rotor flight manipulator model, the three reference coordinate systems Σ I ,Σ B and Σ i They are defined as the earth coordinate system, the body coordinate system and the robot arm link coordinate system, i = 1, 2, 3 represents the serial number of each robot arm link, and all reference coordinate systems are located at the geometric center of the corresponding rigid body; the coordinate variable includes the coordinate system of the quadrotor in the coordinate system Σ I The position p in the T , Euler angle And the joint angle of each link η=[η1,η2,η3] T , the vector of coordinate variables is expressed as: q=[p T ,F T ,or T ] T (1) From Σ B to Σ I The rotation matrix It is expressed as follows: Where C and S are the abbreviations of trigonometric functions cos(.) and sin(.) respectively; and They are defined as the quadrotor in the reference coordinate system Σ I and Σ B The speeds, ω and ω b is the quadrotor in the reference coordinate system Σ I and Σ B The angular velocity in has the following relationship: ω=[ω x oh y oh z ] T (4) The Euler angular velocity can be converted to the following through the transformation matrix T: Relative to the body coordinate system Σ B The position in is represented by It is related to the reference coordinate system Σ i The position p i There are the following relationships: The acceleration of the i-th link and angular acceleration The formula is: Among them J t,i and J r,i is the Jacobian matrix; from formulas (9), (10) and (11), the reference coordinate system Σ i Relative to the Earth coordinate system Σ I The acceleration and angular acceleration are expressed as: The operator S(·) transforms a vector into an antisymmetric matrix, rewriting equations (3), (7), (12) and (13) as follows: in is a 3×3 unit matrix, 03 is a 3×3 zero matrix, B t,b , B r,b , B t,i and B r,i is the defined matrix, and B r,b =[03 T 03], Combining equations (14)-(17), the derivatives of the state variables in the earth coordinate system are Establish a connection with the rotor flight manipulator system state and the speed and angular velocity of each of its links.

3. The sliding mode control method of a rotor flight manipulator according to claim 2, characterized in that: Step 2 is as follows: The Euler-Lagrange equations for establishing a rotorcraft flight manipulator system are used: in Kinetic energy of the rotor flight manipulator system and potential energy The Lagrangian function of the rotor flight manipulator system is τ, ext is the external disturbance of the rotor flight manipulator system, kinetic energy and its components are calculated as follows: Where m represents the mass matrix, I represents the inertia matrix, subscript b represents the body regarded as a rigid body, and subscript i represents the serial number of each robot arm link. Represents the reference frame Σ I To the body coordinate system Σ B The rotation matrix, potential energy The calculation is as follows: Where e3 = [001] T , g is the acceleration due to gravity; Substituting (19), (20) and (23) into (18), we can obtain the kinematic model of the rotorcraft flight manipulator system: Where M(q) is the inertia matrix, represents the Coriolis matrix, G(q) represents the gravity matrix; Using formulas (14)-(17), the inertia matrix M(q) is calculated as: The Coriolis matrix is ​​derived by computing each element of the kth row and jth column: Where m αβ represents the αth row and βth column elements of the inertia matrix M(q); the gravity matrix G(q) is calculated using the following partial derivatives:

4. The sliding mode control method of a rotor flight manipulator according to claim 3, characterized in that: Step 3 is as follows: Considering that the rotorcraft manipulator system is affected by the time-varying external environment and model uncertainty during the process of picking up and releasing the target object in the air, the kinematic model (24) of the rotorcraft manipulator system is rewritten as follows: in represents the interference of external environment and model uncertainty, and They represent the deviation values ​​of inertia matrix, Coriolis matrix and gravity matrix between the derived model and the actual model respectively; Define x1 = q and The rewritten dynamic model (28) is transformed into the following state space form:

5. The sliding mode control method of a rotor flight manipulator according to claim 4, characterized in that: Step 4 is as follows: definition The desired position of the rotorcraft manipulator system in the Earth coordinate system, is the desired velocity, then the trajectory tracking error and its derivative are expressed as follows: Design the following fractional-order sliding surface function: where k i =diag(k i1 ,…,k i9 ),i=1,2,k i1 >0,0<λ1,λ2<1, is a fractional order function, defined as follows is a fractional function, where Γ(np) is the factorial, Γ(np) = (n-p+1)!, then the derivative of the fractional sliding surface function is calculated as follows: From formula (29), formula (33) is further expressed as 6. The sliding mode control method of a rotor flight manipulator according to claim 5, characterized in that: Step 5 is as follows: According to the fractional-order sliding surface function, the equivalent control law of the rotor flight manipulator system is obtained: where u eq is the equivalent control input, let The equivalent control law is simplified as follows: In order to ensure that the second-layer sliding surface of the system has a global fast convergence speed, the fast terminal sliding surface is designed with the following reaching law: Among them, α1 is the set parameter, is the adaptive parameter, sig(s) ρ =|s| ρ sgn(s) and thus the switching control law can be designed as: Adaptive parameters The adaptive law is: in For the adaptive parameters, RBF neural network is used to approximate the external loop interference and the dynamic uncertainty term F(x1) of the system. As the input of the neural network, the designed disturbance control law is: in: c j and b j is the set parameter, adaptive parameter The adaptive law is: Among them B W and k W is the adaptive parameter of the RBF neural network estimator, so the overall control law is designed as follows:

Images