Hovering control method and system for small unmanned helicopter with mechanical arm

Abstract

The invention provides a hovering control method and a hovering control system for a small unmanned helicopter with a mechanical arm, and provides a position-attitude double-loop composite control strategy integrating a disturbance observer and an extended state observer. Wherein a sliding mode controller is provided in a position loop, and a nonlinear disturbance observer is designed; besides, in consideration of the influence of continuous operation of the mechanical arm on the gravity center and rotational inertia of the whole system under wind disturbance, a nonlinear expansion state observer is designed in an attitude ring, and a backstepping sliding mode controller is provided to stabilize the hovering attitude of the helicopter. According to the hovering control method and control system for the small unmanned helicopter with the mechanical arm, the position error is smaller than or equal to 2% under disturbance of equivalent wind power of three levels, and the sliding mode controller is high in control precision, high in robustness and obvious in tremor suppression effect; the maximum amplitude of the hovering attitude angle is smaller than or equal to 0.1 rad, and high-robustness control can be achieved by combining a backstepping sliding mode controller with an improved extended state observer.

Description

Technical Field

[0001] The present invention relates to the field of hovering control of unmanned helicopters, and in particular to a hovering control method and a control system for a small unmanned helicopter with a mechanical arm. Background Art

[0002] Unmanned helicopters, capable of hovering, vertical ascent and descent, and boasting high flight speeds, are widely used in both civilian and defense sectors. With the continued advancement of drone research in recent years, unmanned aerial manipulators (UAMs) have been designed to enable complex aerial operations. These robots, equipped with robotic arms, are designed to perform high-altitude operations. However, how to achieve hovering operations with UAMs despite wind disturbances has been a key research topic.

[0003] Although helicopters have advantages over multi-rotor drones such as faster flight speed and long-distance transportation, research on UAM using helicopters as mobile platforms is also limited due to the more complex structure and control of helicopters.

[0004] In summary, the flight control problem of unmanned helicopters in complex wind disturbance environments needs to be further studied. Proposing a hovering control method and control system for a small unmanned helicopter with a robotic arm that can resist complex wind disturbances has become an urgent problem to be solved. Summary of the Invention

[0005] In view of this, the present invention provides a hovering control method and control system for a small unmanned helicopter with a mechanical arm to solve the problems existing in the prior art.

[0006] On one hand, the present invention provides a hovering control method for a small unmanned helicopter with a mechanical arm, comprising:

[0007] S1: Build a helicopter model;

[0008] S2: Design a position-attitude dual-loop composite control strategy that integrates a nonlinear disturbance observer and a nonlinear extended state detector. The position loop uses a nonlinear disturbance observer to realize wind disturbance estimation and realizes position control through a sliding mode controller. The attitude loop uses a nonlinear extended state detector to observe state uncertainty and uses a backstepping sliding mode controller to realize attitude stabilization control.

[0009] Preferably, in S1, the helicopter model is as follows:

[0010]

[0011] Where m is the mass of the helicopter, g is the acceleration due to gravity, and P j (j=x,y,z) is the position of the helicopter in the inertial coordinate system, Ix ,I y ,I z Represents the axis around the body x b ,y b ,z b The moment of inertia of rotation in the direction of mr is the main rotor thrust, H mr The vertical height from the main rotor hub to the center of gravity of the helicopter, K β The stiffness coefficient of the main rotor, T tr is the thrust generated by the tail rotor, H tr D is the vertical distance from the tail rotor hub to the helicopter's center of gravity. tr is the distance from the tail rotor hub to the helicopter's center of gravity at the rear, a and b are the longitudinal and lateral flapping angles respectively, n Pj (j=x,y,z) represents the force disturbance in each direction of the body axis, n Ij (j=x, y, z) represents the torque in each direction of the body axis, and the disturbance function is differentiable and bounded.

[0012] Further preferably, S2 includes:

[0013] S21: For the position control of the Z axis of the inertial coordinate system, the sliding mode control law of the Z axis of the inertial coordinate system is designed by the sliding mode control method according to the dynamic equation:

[0014]

[0015] Among them, the coefficient k z =0.9, β z =0.6 and q z =0.7 are all positive numbers, n pz Indicates the body axis z b The force perturbation in the direction is:

[0016]

[0017] in, is a positive constant;

[0018] In the sliding mode control law of the Z axis, the sliding surface is The exponential reaching law is -k z f(s z )-q z s z , the error about the height is e z =P zr -P z , where P z and P zr are the actual position of the height and the expected position of the height respectively;

[0019] The steering angle δ of the servo that controls the total pitch angle of the main rotor blades is calculated based on the main rotor thrust on the Z axis. col The formula is as follows:

[0020] δ col =-(θ col +0.0553) / 0.4095;

[0021] Among them, the main rotor collective pitch angle θ col The calculation formula is:

[0022] θ col ≈(-T mr / 0.867+v i,mr -w a -au a +bv a ) / 27;

[0023] Among them, v i,mr is the induced speed generated by the main rotor rotation, a and b are the longitudinal and lateral flapping angles respectively, w a The helicopter is about the body axis z b Translational velocity in the direction, u a The x axis of the helicopter b The translational velocity in the direction, v a The y axis of the helicopter b Translational velocity in the direction;

[0024] For the position control of the horizontal X-axis of the inertial coordinate system, the sliding mode control law of the horizontal X-axis of the inertial coordinate system is designed by the sliding mode control method according to the dynamic equation:

[0025]

[0026] Among them, the coefficient k x =2,β x =2.5 and q x =1.6 are both positive numbers. In the sliding mode control law of the X axis, the sliding mode surface is The exponential reaching law is -k x f(s x )-q x s x , the position error about the horizontal X-axis of the inertial coordinate system is e x =P xr -P x , where P z and P zr are the actual position and expected position of the horizontal X-axis of the inertial coordinate system respectively;

[0027] For the position control of the horizontal Y-axis of the inertial coordinate system, the sliding mode control law of the horizontal Y-axis of the inertial coordinate system is designed according to the dynamic equation:

[0028]

[0029] Among them, the coefficient k y =2.1,β y =2.6 and q y =1.6 are both positive numbers. In the sliding mode control law of the Y axis, the sliding mode surface is The exponential reaching law is -k y f(s y )-q y s y , the position error about the horizontal Y axis of the inertial coordinate system is e y =P yr -P y , where P y and P yr are the actual position and expected position of the horizontal Y axis of the inertial coordinate system respectively;

[0030] According to the control law U x and U y , the formula for calculating the longitudinal and lateral flapping angles of the main rotor is as follows:

[0031]

[0032] Among them, K β The stiffness coefficient of the main rotor, H mr The vertical height from the main rotor hub to the helicopter's center of gravity, T tr The pull generated by the tail rotor, H tr It is the vertical distance from the tail rotor hub to the helicopter's center of gravity. is the roll angular acceleration, is the pitch angular acceleration;

[0033] Then, the longitudinal cyclic pitch angle θ of the main rotor blade of the helicopter is calculated using the following formula: lon The steering gear angle δ lon and controls the lateral cyclic pitch angle θ of the helicopter main rotor blades lat The steering gear angle δ lat :

[0034]

[0035] The longitudinal cyclic pitch angle θ of the helicopter main rotor blade lon and the lateral periodic pitch angle θ lat The calculation formula is as follows:

[0036]

[0037] Where p is the roll angular velocity, q is the pitch angular velocity;

[0038] S22: Design of nonlinear disturbance observer;

[0039] The nonlinear disturbance observer of the inertial coordinate system Z axis is designed as follows:

[0040]

[0041] Where, Represents the estimated disturbance value in the Z-axis direction of the inertial coordinate system, Z z is the internal state of the observer, l(w a ) is the designed helicopter about the body axis z b The translational velocity w a The observer gain function, T mr is the sliding mode control law, g is the acceleration of gravity, and we have:

[0042]

[0043] Among them, in the nonlinear disturbance observer z1=2.8 and z2=0.2 are both positive constants;

[0044] The nonlinear disturbance observer in the X-axis direction of the inertial coordinate system is designed as follows:

[0045]

[0046] Where, Indicates the estimated disturbance value in the X-axis direction of the inertial coordinate system, Z x is the internal state of the observer, l(u a ) is the observer gain function, U x is the control law of the horizontal X-axis direction of the inertial coordinate system, u a The x axis of the helicopter b Translational velocity in the direction;

[0047] The nonlinear disturbance observer designed Among them, x1=2 and x2=0.12 are both positive constants;

[0048] The nonlinear disturbance observer in the Y-axis direction of the inertial coordinate system is designed as follows:

[0049]

[0050] Where, Indicates the estimated disturbance value in the Y-axis direction of the inertial coordinate system, Z y is the internal state of the observer, l(va ) is the observer gain function, U y is the control law of the horizontal Y-axis direction of the inertial coordinate system, v a The y axis of the helicopter b Translational velocity in the direction;

[0051] Design of nonlinear disturbance observer Among them, y1=2.1 and y2=0.1 are both positive constants;

[0052] S23: Determine the moment of inertia of the robotic arm by analyzing its motion t The relationship between the center of gravity OM of the helicopter-mounted robotic arm and the change in the robotic arm joint angle is:

[0053]

[0054] Among them, m t is the total mass of the UAM system, I0=J is the inertia tensor matrix of the helicopter itself, yes The third-order sequential principal minor of ; By OC i3×1 Constructed antisymmetric matrix, m i is the mass of the connecting rod,

[0055] J i =diag{j ix ,j iy ,j iz} is the inertia tensor matrix of each connecting rod;

[0056] S24: According to the dynamic equations of helicopter torque Design a posture controller where ω = (p, q, r) T is the angular velocity vector in the body coordinate system, U ω ∈R 3×1 is the control variable of attitude control, T∈R 3×1 is the column vector containing the perturbation, C∈R 3×3 is the coefficient matrix about the change of the overall center of gravity, which is:

[0057]

[0058] Where N mr is a constant, n Ij (j=x,y,z) represents the torque disturbance in each axis direction, M mr With L mr The torque generated by the main rotor is on the body axis x b and y b The component in the direction, T tr The pull generated by the tail rotor, Htr D is the vertical distance from the tail rotor hub to the helicopter's center of gravity. tr It is the distance from the tail rotor hub to the horizontal rear center of gravity of the helicopter.

[0059] The backstepping sliding mode control method is used to stabilize the helicopter's hovering attitude: Assume the attitude angle of the helicopter is have Let the angle error be e ξ =ξ d -ξ, where ξ d is the desired attitude angle, and the time derivative of the angle error is Regarding the subsystem of angle ξ, let the first Lyapunov function be And the time derivative is Regarding the subsystem of angular velocity ω, the integral sliding surface Among them, λ=diag{5,8,3} is a constant coefficient, is the virtual control quantity, k ξ =diag{4,11,10} is a constant coefficient, and according to the exponential approach law, Calculation, the control quantity of attitude control is:

[0060]

[0061] Where k1 and k2 are diagonal matrices with constant coefficients whose elements are greater than zero, where k1 = diag{22, 42, 30} and k2 = diag{27, 27, 40}.

[0062] In the inner loop command generator, according to the thrust T generated by the tail rotor tr Solve for the tail rotor's collective pitch angle θ ped and its steering gear angle δ ped , the formula is as follows:

[0063]

[0064] Among them, D tr is the distance from the tail rotor hub to the helicopter's center of gravity at the rear horizontal position, v i,tr is the induced speed generated by the tail rotor, r is the yaw angular velocity;

[0065] Servo control signal δ lat and δ lon The calculation formulas are:

[0066]

[0067] Among them, the longitudinal periodic pitch angle θ of the helicopter main rotor blade is lon and the lateral periodic pitch angle θ lat The calculation formula is:

[0068]

[0069] Where p is the roll angular velocity, q is the pitch angular velocity;

[0070] S25: According to the dynamic equations of helicopter torque Construct a nonlinear extended state observer:

[0071]

[0072] Among them, Z1 represents the state observation of the attitude angle ξ, e o Represents the observation error of the attitude angle, Z2 represents the state observation of the attitude angular velocity ω, Z3 represents the observation value of the total internal and external disturbance f in the torque equation, a1=a2=a3=0.5, δ1=δ2=δ3=2, β1=diag{15,36,67}, β2=diag{15,55,35}, β3=diag{26,90,62}, fal in the extended state observer expression * is an improved nonlinear function, specifically expressed as:

[0073]

[0074] The present invention also provides a small unmanned helicopter hovering control system with a mechanical arm, which is used to execute the above-mentioned small unmanned helicopter hovering control method with a mechanical arm.

[0075] The hovering control method and control system for a small unmanned helicopter with a robotic arm provided by the present invention take into account the impact of external interference on the system. An improved disturbance observer can compensate for the impact of interference, thereby improving the system's anti-interference capability. A sliding mode controller and a backstepping sliding mode controller for variable parameter models are proposed in the position loop and attitude loop, respectively, to achieve higher system control robustness. An improved extended state observer is proposed to effectively resolve the state uncertainty problem caused by wind disturbance and robotic arm motion.

[0076] The hovering control method and control system for a small unmanned helicopter with a robotic arm provided by the present invention have a position error of ≤2% under disturbances with an equivalent wind force of level 3. Furthermore, the sliding mode controller has high control accuracy, strong robustness, and significant vibration suppression effect. The maximum amplitude of the hovering attitude angle is ≤0.1 rad, and a backstepping sliding mode controller combined with an improved extended state observer can achieve highly robust control. BRIEF DESCRIPTION OF THE DRAWINGS

[0077] Figure 1 A schematic diagram of a small unmanned helicopter with a mechanical arm according to the present invention;

[0078] Figure 2Schematic diagram of the body coordinate system and inertial (ground) coordinate system of the small unmanned helicopter of the present invention;

[0079] Figure 3 Position-attitude control block diagram designed for the present invention;

[0080] Figure 4 This is the change curve of each joint of the airborne three-joint robotic arm of the present invention, simulating the real situation in which the robotic arm performs continuous aerial operation tasks;

[0081] Figure 5 The observation value of the external wind disturbance by the improved nonlinear disturbance observer of the present invention;

[0082] Figure 6 is the position control error curve based on the improved sliding mode controller (SMC) in the position loop of the present invention;

[0083] Figure 7 The steering gear angle curve for manipulating the UAM position change obtained by calculation in the present invention;

[0084] Figure 8 This is the hovering attitude curve of the UAM when only the backstepping sliding mode controller is used in a windless environment;

[0085] Figure 9 The hovering attitude curve of the UAM under the motion of the onboard manipulator according to the embodiment of the present invention is obtained by combining the backstepping sliding mode controller with the improved nonlinear extended state observer;

[0086] Figure 10 The embodiment of the present invention calculates the servo angle change curve that ensures the UAM hovering posture. DETAILED DESCRIPTION

[0087] The present invention will be further described below with reference to specific embodiments.

[0088] like Figures 1 to 3 As shown, the present invention provides a hovering control method for a small unmanned helicopter system with a mechanical arm, comprising the following steps:

[0089] S1: Establish a helicopter model, wherein the helicopter model is as follows:

[0090]

[0091] Where m = 0.94 kg is the mass of the helicopter, g = 9.81 N / kg is the acceleration of gravity, and P j (j=x,y,z) is the position of the helicopter in the inertial coordinate system, I x ,I y ,I z Represents the axis around the body xb ,y b ,z b The moment of inertia of rotation in the direction of mr is the main rotor thrust, H mr =0.077m is the vertical height from the main rotor hub to the helicopter's center of gravity,

[0092] K β =45.96N·m is the stiffness coefficient of the main rotor, T tr is the thrust generated by the tail rotor, H tr =0.012m is the vertical distance from the tail rotor hub to the helicopter's center of gravity, D tr =0.320m is the distance from the tail rotor hub to the helicopter's center of gravity at the rear, a and b are the longitudinal and lateral flapping angles respectively, n Pj (j=x,y,z) represents the force disturbance in each direction of the body axis, n Ij (j = x, y, z) represents the torque in each direction of the body axis, and the disturbance function is differentiable and bounded;

[0093] The derivation process of the above model is as follows:

[0094] First, according to the Newton-Euler method, the dynamic equation of the small unmanned helicopter is established:

[0095]

[0096] Where, J = diag{I x ,I y ,I z} is the helicopter inertia tensor matrix, I x =0.0650kg·m 2 , I y＝ 0.1382kg·m 2 , I z =0.2105kg·m 2 ,ω=(p,q,r) T is the angular velocity vector in the body coordinate system; F b and M b denote the aerodynamic force vector and aerodynamic moment vector respectively;

[0097] in,

[0098] Where, X mr ,Y mr ,Z mr They represent the components of the thrust generated by the main rotor in the direction of the fuselage axis, L mr ,M mr ,N mrThey represent the components of the torque generated by the main rotor in the direction of the fuselage axis; Y tr ,L tr ,N tr Indicates the force and torque of the tail rotor; when the helicopter is in low-speed flight or hovering, ignore the resistance of the fuselage and the force and torque generated by the vertical fin tail;

[0099] Define F bg is the projection vector of gravity in the body coordinate system, which is:

[0100]

[0101] In addition, the main rotor thrust T mr The calculation formula is:

[0102]

[0103] Where, v i,mr The induced speed generated by the main rotor rotation, Ω mr =165rad / s is the main rotor speed,

[0104] R mr = 0.248m, the radius of the main rotor blade, ρ = 1.29kg / m 3 represents the air density, b mr =2 is the number of blades, c mr =0.03m is the blade chord C lα,mr =4.411rad -1 is the slope of the lift curve of the main rotor blade, w bl,mr is the net vertical airspeed of the main rotor rotation;

[0105] The components of the thrust generated by the main rotor in the direction of the fuselage axis are:

[0106]

[0107] Each component of force controls the pitch, roll, and lift motion of the helicopter; the component of the torque generated by the main rotor in the direction of the fuselage axis is:

[0108]

[0109] Among them, H mr = 0.077m is the vertical height from the main rotor hub to the helicopter's center of gravity, K β =45.96N·m is the stiffness coefficient of the main rotor, which is used to describe the difficulty of blade deformation. mr The total power of the motor that controls the rotation of the helicopter rotor. When the helicopter load is constant, the total power of the motor and the main rotor speed are both fixed values.

[0110] The thrust T generated by the tail rotor tr The formula is:

[0111]

[0112] Where, v i,tr is the induced speed generated by the tail rotor, Ω tr is the tail rotor speed, R tr =0.051m is the radius of the tail rotor blade, b tr =2 is the number of tail rotor blades, c tr =0.016m is the tail rotor blade chord length, C lα,tr =2.729rad -1 is the lift curve slope of the tail rotor, w bl,tr is the net vertical airspeed produced by the tail rotor rotation;

[0113] In the body coordinate system, the tail rotor only produces the y b Axial tensile force Y tr =-T tr The flapping angle caused by the rotation of the tail rotor is not considered here. The pulling force produces torques in two directions, namely the torque L caused by the vertical upward position deviation of the tail rotor hub in the direction of the helicopter's center of gravity and the torque L caused by the vertical upward position deviation of the tail rotor hub in the direction of the helicopter's center of gravity. tr and z in the body coordinate system b The moment N on the axis that offsets the main rotor tr :

[0114]

[0115] Where H tr D is the vertical distance from the tail rotor hub to the helicopter's center of gravity. tr is the distance from the tail rotor hub to the helicopter's center of gravity at the rear horizontally;

[0116] The main rotor blade flapping dynamics equation is:

[0117]

[0118] Among them, γ m is the blade Locke number, I β =3.75×10 -4 kg·m 2 is the moment of inertia of the main rotor blades rotating around the main axis, Ω mr is the main rotor speed, a and b are the longitudinal and lateral flapping angles respectively, K β =45.96N·m is the stiffness coefficient of the main rotor, p is the roll angular velocity, q is the pitch angular velocity, θ lat and θ lon are the lateral cyclic pitch angle and longitudinal cyclic pitch angle of the main rotor blade respectively;

[0119] In summary, the kinetic equation can be expressed in detail as:

[0120]

[0121] When the helicopter is hovering, the roll angle, pitch angle and flap angle are all close to 0°, so:

[0122] Sina = a, sinb = b, sinφ = φ, sinθ = θ, and cosa = cosb = cosφ = cosθ = 1. The above formula can be further simplified:

[0123]

[0124] Assume the position disturbance function n p* and attitude disturbance function n I* The helicopter attitude angle has the relationship:

[0125] Where φ, θ, and ψ are the pitch angle, roll angle, and yaw angle of the helicopter, respectively; p, q, and r are the angular velocity vectors in the body coordinate system;

[0126] S2: According to Figure 3 As shown in the system control block diagram, a position-attitude dual-loop composite control strategy is designed that integrates the nonlinear disturbance observer (NDOB) and the nonlinear extended state observer (NESO).

[0127] The position loop is used to ensure the stability of the helicopter's hovering position, and the attitude loop is used to ensure the stability of the helicopter's hovering attitude when the onboard manipulator continues to move. A sliding mode controller is proposed for the position loop. Due to external interference, a non-determined observable (NDOB) is designed to compensate for external interference of the drone based on the error between actual and observed conditions. A backstepping sliding mode controller is proposed for the attitude loop to achieve higher attitude control robustness, and a non-determined observable (NESO) is designed to address state uncertainty.

[0128] When the onboard manipulator moves, the center of mass OM and moment of inertia I of the helicopter as a whole t The position loop ensures the stable hovering position of a small unmanned helicopter equipped with a robotic arm. Wind disturbance estimation is achieved using the NDOB, and position control is achieved through a sliding mode controller. The attitude loop ensures the stable hovering attitude of the unmanned helicopter when the onboard robotic arm moves. NESO is used to observe state uncertainty, and a backstepping sliding mode controller is used to achieve attitude stability control.

[0129] S21: For the position control of the Z axis of the inertial coordinate system, the sliding mode control law of the Z axis of the inertial coordinate system is designed by the sliding mode control method according to the dynamic equation:

[0130]

[0131] Among them, the coefficient k z =0.9, β z =0.6 and q z =0.7 are all positive numbers, n pz Indicates the body axis z b The force perturbation in the direction is:

[0132]

[0133] in, is a positive constant; it is not difficult to conclude that: -1≤f(x)≤|f(x)|≤1, f(x) is continuously differentiable and monotonically increasing;

[0134] In the sliding mode control law of the Z axis, the sliding surface is The exponential reaching law is -k z f(s z )-q z s z , the error about the height is e z =P zr -P z , where P z and P zr are the actual position of the height and the expected position of the height respectively;

[0135] The method for calculating the steering angle of the servo that controls the change of the main rotor blade collective pitch angle based on the main rotor thrust on the Z axis is:

[0136] δ col =-(θ col +0.0553) / 0.4095;

[0137] The calculation formula of the main rotor collective pitch angle is:

[0138] θ col ≈(-T mr / 0.867+v i,mr -w a -au a +bv a ) / 27;

[0139] Among them, v i,mr is the induced speed generated by the main rotor rotation, a and b are the longitudinal and lateral flapping angles respectively, w a The helicopter is about the body axis z b Translational velocity in the direction, ua The x axis of the helicopter b The translational velocity in the direction, v a The y axis of the helicopter b Directional velocity.

[0140] For the position control of the horizontal X-axis of the inertial coordinate system, the sliding mode control law of the horizontal X-axis of the inertial coordinate system is designed by the sliding mode control method according to the dynamic equation:

[0141]

[0142] Among them, the coefficient k x =2,β x =2.5 and q x =1.6 are both positive numbers. In the sliding mode control law of the X axis, the sliding mode surface is The exponential reaching law is -k x f(s x )-q x s x , the position error about the horizontal X-axis of the inertial coordinate system is e x =P xr -P x , where P z and P zr are the actual position and expected position of the horizontal X-axis of the inertial coordinate system respectively.

[0143] For the position control of the horizontal Y-axis of the inertial coordinate system, the sliding mode control law of the horizontal Y-axis of the inertial coordinate system is designed according to the dynamic equation:

[0144]

[0145] Among them, the coefficient k y =2.1,β y =2.6 and q y =1.6 are both positive numbers. In the sliding mode control law of the Y axis, the sliding mode surface is The exponential reaching law is -k y f(s y )-q y s y , the position error about the horizontal Y axis of the inertial coordinate system is e y =P yr -P y , where P y and P yr are the actual position and expected position of the horizontal Y axis of the inertial coordinate system respectively;

[0146] According to the control law U x and U y, the formula for calculating the longitudinal and lateral flapping angles of the main rotor is as follows:

[0147]

[0148] Among them, K β =45.96N·m is the rigidity coefficient of the main rotor, H mr = 0.077m is the vertical height from the main rotor hub to the helicopter's center of gravity, T tr The pull generated by the tail rotor, H tr =0.012m is the vertical distance from the tail rotor hub to the helicopter's center of gravity. is the roll angular acceleration, is the pitch angular acceleration;

[0149] Then the longitudinal cyclic pitch angle (θ) of the helicopter main rotor blade is calculated using the following formula: lon ) of the steering gear angle (δ lon ) and controls the lateral cyclic pitch angle (θ lat ) of the steering gear angle (δ lat ):

[0150] δ lon =(θ lon +0.0028) / 0.2008

[0151] δ lat =(θ lat +0.0034) / 0.2056

[0152] The longitudinal cyclic pitch angle (θ lon ) and the lateral periodic pitch angle (θ lat ) is calculated as follows:

[0153]

[0154] Where p is the roll angular velocity, q is the pitch angular velocity;

[0155] S22: Design of nonlinear disturbance observer;

[0156] The nonlinear disturbance observer of the inertial coordinate system Z axis is designed as follows:

[0157]

[0158] Where, Represents the estimated disturbance value in the Z-axis direction of the inertial coordinate system, Z z is the internal state of the observer, l(w a ) is the designed helicopter about the body axis z b The translational velocity wa The observer gain function, T mr is the sliding mode control law, g is the acceleration of gravity, and we have:

[0159]

[0160] Moreover, the designed nonlinear disturbance observer Among them, z1=2.8 and z2=0.2 are both positive constants;

[0161] The nonlinear disturbance observer in the X-axis direction of the inertial coordinate system is designed as follows:

[0162]

[0163] Where, Indicates the estimated disturbance value in the X-axis direction of the inertial coordinate system, Z x is the internal state of the observer, l(u a ) is the observer gain function, U x is the control law of the horizontal X-axis direction of the inertial coordinate system, u a The x axis of the helicopter b Translational velocity in the direction;

[0164] The nonlinear disturbance observer designed Among them, x1=2 and x2=0.12 are both positive constants;

[0165] The nonlinear disturbance observer in the Y-axis direction of the inertial coordinate system is designed as follows:

[0166]

[0167] Where, Indicates the estimated disturbance value in the Y-axis direction of the inertial coordinate system, Z y is the internal state of the observer, l(v a ) is the observer gain function, U y is the control law of the horizontal Y-axis direction of the inertial coordinate system, v a The y axis of the helicopter b Translational velocity in the direction;

[0168] Design of nonlinear disturbance observer Among them, y1=2.1 and y2=0.1 are both positive constants;

[0169] S23: According to Figure 4 The curves of the joint angles of the helicopter-mounted robotic arm are shown, simulating the actual situation where the robotic arm performs a continuous aerial operation task. The moment of inertia I of the robotic arm is determined by analyzing the motion of the robotic arm. tThe relationship between the center of gravity OM of the helicopter-mounted robotic arm and the change in the robotic arm joint angle is:

[0170]

[0171] Among them, m t is the total mass of the UAM system, I0=J is the inertia tensor matrix of the helicopter itself, yes The third-order sequential principal minor of ; By OC i3×1 Constructed antisymmetric matrix, m i is the mass of the connecting rod,

[0172] J i =diag{j ix ,j iy ,j iz} is the inertia tensor matrix of each connecting rod;

[0173] The construction principle is as follows:

[0174] Assume a column vector ζ=[x,y,z] T ,but

[0175] S24: According to the dynamic equations of helicopter torque Design a posture controller where ω = (p, q, r) T is the angular velocity vector in the body coordinate system, U ω ∈R 3×1 is the control variable of attitude control, T∈R 3×1 is the column vector containing the perturbation, C∈R 3×3 is the coefficient matrix about the change of the overall center of gravity, which is:

[0176]

[0177] Where N mr is a constant, n Ij (j=x,y,z) represents the torque disturbance in each axis direction, M mr With L mr The torque generated by the main rotor is on the body axis x b and y b The component in the direction, T tr The pull generated by the tail rotor, H tr D is the vertical distance from the tail rotor hub to the helicopter's center of gravity. tr It is the distance from the tail rotor hub to the horizontal rear center of gravity of the helicopter.

[0178] Therefore, according to the dynamic equation of the helicopter torque, the backstepping sliding mode control method is used to stabilize the helicopter's hovering attitude: Assume that the attitude angle of the helicopter is have Let the angle error be e ξ =ξ d -ξ, where ξ d is the desired attitude angle, and the time derivative of the angle error is Regarding the subsystem of angle ξ, let the first Lyapunov function be And the time derivative is Regarding the subsystem of angular velocity ω, the integral sliding surface Among them, λ=diag{5,8,3} is a constant coefficient, is the virtual control quantity, k ξ =diag{4,11,10} is a constant coefficient, and according to the exponential approach law, Calculate, therefore, the attitude control quantity is:

[0179]

[0180] Wherein, k1, k2 (where k1 = diag{22, 42, 30}, k2 = diag{27, 27, 40}) are diagonal matrices with constant coefficients and all elements are greater than zero;

[0181] In the inner loop command generator, according to the thrust T generated by the tail rotor tr Solve for the tail rotor's collective pitch angle θ ped and its steering gear angle δ ped , the formula is as follows:

[0182]

[0183] Among them, D tr is the distance from the tail rotor hub to the helicopter's center of gravity at the rear horizontal position, v i,tr is the induced speed generated by the tail rotor, r is the yaw angular velocity;

[0184] Servo control signal δ lat and δ lon The solutions are the same as before, which are:

[0185]

[0186] Among them, the longitudinal cyclic pitch angle of the helicopter main rotor blade (θ lon ) and the lateral periodic pitch angle (θ lat ) is calculated as follows:

[0187]

[0188] Where p is the roll angular velocity and q is the pitch angular velocity.

[0189] S25: According to the dynamic equations of helicopter torque Construct a nonlinear extended state observer:

[0190]

[0191] Among them, Z1 represents the state observation of the attitude angle ξ, e o Represents the observation error of the attitude angle, Z2 represents the state observation of the attitude angular velocity ω, Z3 represents the observation value of the total internal and external disturbance f in the torque equation, a1=a2=a3=0.5, δ1=δ2=δ3=2, β1=diag{15,36,67}, β2=diag{15,55,35}, β3=diag{26,90,62}, fal in the extended state observer expression * is an improved nonlinear function, specifically expressed as:

[0192]

[0193] The present invention also provides a small unmanned helicopter hovering control system with a mechanical arm, which is used to execute the above-mentioned small unmanned helicopter hovering control method with a mechanical arm.

[0194] The stability of the control system is proved below:

[0195] Regarding position loop control, let the Lyapunov function for height change be where s z is the sliding surface controlled by the Z-axis direction of the inertial coordinate system, V z Positive definite, the time derivative is:

[0196]

[0197] If and only if s z = 0, the equality holds, where the coefficient k z and β z are all positive numbers, e z is the height error, T mr represents the main rotor thrust, n pz Indicates the body axis z b The force disturbance in the direction, g is the acceleration of gravity, so, Negative setting, Z direction position control system is stable.

[0198] For the control law of the X-axis direction of the inertial coordinate system, let the Lyapunov function be Among them, s x is the sliding surface controlled in the X-axis direction of the inertial coordinate system, Vx Positive definite, the time derivative is:

[0199]

[0200] If and only if S x = 0, where k x and β x are all positive numbers, U x is the control law of the horizontal X-axis direction of the inertial coordinate system, n px Indicates the body axis x b The force disturbance in the direction, e x is the position error of the horizontal X-axis of the inertial coordinate system, so Negative setting, X-direction position control system is stable.

[0201] By the same token, it can be proved that the control system of the Y-axis direction of the inertial coordinate system is also stable. The proof is complete.

[0202] Regarding attitude loop control, let the Lyapunov function of the second-order system be According to the previous analysis, when the second subsystem about angular velocity ω is stable, the first subsystem about attitude angle ξ is stable. V2 is positive definite for the second subsystem, and:

[0203]

[0204] Among them, s is the controlled sliding surface α is the virtual control quantity, I t is the inertia tensor matrix, C is the coefficient matrix about the change of the overall center of gravity, U ω is the control quantity, T is the column vector containing the disturbance, λ>0 is a constant coefficient, and the second subsystem is stable, so If and only if s=0, the equality holds, that is, V is positive definite. Negative definiteness, the stability of the system is proved.

[0205] Figure 5 The nonlinear disturbance observer used in this invention observes the external wind disturbance. In the figure, (a), (b), and (c) are the wind disturbance observation values ​​of the X-axis, Y-axis, and Z-axis, respectively. (d) is a schematic diagram of the effect of turbulence on torque. Figure 5 It can be seen that the error between the disturbance observer's disturbance estimation and the actual external disturbance gradually converges to 0, which means that the designed disturbance observer can accurately estimate the existing external disturbance, and the improved disturbance observer has a more accurate disturbance estimation than the ordinary linear disturbance observer.

[0206] Figure 6The position control error curve based on the improved sliding mode controller (SMC) in the position loop of the present invention is shown in the figure. In the figure, (a) (b) (c) are the position errors in the X, Y and Z directions respectively. Figure 6 It can be seen that the improved sliding mode controller with position loop design has higher control accuracy, stronger robustness, and better suppression effect on fuselage vibration;

[0207] Figure 7 The servo angle curves for controlling the UAM position change obtained by calculation in the present invention are shown in Figures (a), (b), and (c), respectively, for controlling the longitudinal cyclic pitch, the lateral cyclic pitch, and the main blade collective pitch angle. Figure 7 It can be seen that for the UAM hovering position control, the control signal curve of the servo is smoother under the improved sliding mode controller;

[0208] Figure 8 The hovering attitude curve of the UAM when the present invention only uses the backstepping sliding mode controller in a windless environment is shown in Figure 2. Figure 8 It can be seen that the control effect of the backstepping sliding mode controller in the attitude loop is more robust than that of the simple backstepping controller;

[0209] Figure 9 The hovering attitude curve of the UAM is obtained by combining the backstepping sliding mode controller and the improved nonlinear extended state observer under the motion of the airborne manipulator in the embodiment of the present invention. Figure 9 It can be seen that after combining with the nonlinear extended state observer (NESO), the overall control effect of the attitude loop is better and the attitude variation amplitude is smaller;

[0210] Figure 10 The servo angle variation curve for ensuring the hovering attitude of the UAM is calculated for the embodiment of the present invention. In the figure, (a) (b) (c) are the servo angle variation curves for controlling the lateral cyclic pitch, the servo angle variation curves for controlling the longitudinal cyclic pitch, and the servo angle variation curves for controlling the tail rotor blade collective pitch angle, respectively. Figure 10 It can be seen that under the influence of turbulence, the servo control signal curve that maintains the stable hovering posture of the small unmanned helicopter with a robotic arm is obtained.

[0211] Those skilled in the art will appreciate that the embodiments of the present application can be provided as methods, systems or computer program products. Therefore, the application can adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the application can adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code. The scheme in the embodiment of the present application can be implemented in various computer languages, for example, object-oriented programming language Java and literal translation scripting language JavaScript, etc.

[0212] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the steps in the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0213] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0214] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0215] Although the preferred embodiments of the present application have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present application.

[0216] Obviously, those skilled in the art may make various changes and modifications to this application without departing from the spirit and scope of this application. Thus, if these modifications and variations of this application fall within the scope of the claims of this application and their equivalents, this application is intended to include these modifications and variations.

Claims

1. A hovering control method for a small unmanned helicopter with a robotic arm, characterized in that: include: S1: Build a helicopter model; S2: Design a position-attitude dual-loop composite control strategy that integrates a nonlinear disturbance observer and a nonlinear extended state detector. The position loop uses a nonlinear disturbance observer to realize wind disturbance estimation and realizes position control through a sliding mode controller. The attitude loop uses a nonlinear extended state detector to observe state uncertainty and uses a backstepping sliding mode controller to realize attitude stabilization control.

2. The hovering control method for a small unmanned helicopter with a robotic arm according to claim 1 is characterized in that: In S1, the helicopter model is as follows: Where m is the mass of the helicopter, g is the acceleration due to gravity, and P j (j=x,y,z) is the position of the helicopter in the inertial coordinate system, I x ,I y ,I z Represents the axis around the body x b ,y b ,z b The moment of inertia of rotation in the direction of mr is the main rotor thrust, H mr The vertical height from the main rotor hub to the center of gravity of the helicopter, K β The stiffness coefficient of the main rotor, T tr is the thrust generated by the tail rotor, H tr D is the vertical distance from the tail rotor hub to the helicopter's center of gravity. tr is the distance from the tail rotor hub to the helicopter's center of gravity at the rear, a and b are the longitudinal and lateral flapping angles respectively, n Pj (j=x,y,z) represents the force disturbance in each direction of the body axis, n Ij (j=x, y, z) represents the torque in each direction of the body axis, and the disturbance function is differentiable and bounded.

3. The hovering control method of a small unmanned helicopter with a robotic arm according to claim 1 is characterized by: S2 includes: S21: For the position control of the Z axis of the inertial coordinate system, the sliding mode control law of the Z axis of the inertial coordinate system is designed by the sliding mode control method according to the dynamic equation: Among them, the coefficient k z =0.9, β z =0.6 and q z =0.7 are all positive numbers, n pz Indicates the body axis z b The force perturbation in the direction is: in, is a positive constant; In the sliding mode control law of the Z axis, the sliding surface is The exponential reaching law is -k z f(s z )-q z s z , the error about the height is e z =P zr -P z , where P z and P zr are the actual position of the height and the expected position of the height respectively; The steering angle δ of the servo that controls the total pitch angle of the main rotor blades is calculated based on the main rotor thrust on the Z axis. col The formula is as follows: d col =-(θ col +0.0553) / 0.4095; Among them, the main rotor collective pitch angle θ col The calculation formula is: θ col ≈(-T mr / 0.867+v i,mr -w a -At a +bv a ) / 27; Among them, v i,mr is the induced speed generated by the main rotor rotation, a and b are the longitudinal and lateral flapping angles respectively, w a The helicopter is about the body axis z b Translational velocity in the direction, u a The x axis of the helicopter b The translational velocity in the direction, v a The y axis of the helicopter b Translational velocity in the direction; For the position control of the horizontal X-axis of the inertial coordinate system, the sliding mode control law of the horizontal X-axis of the inertial coordinate system is designed by the sliding mode control method according to the dynamic equation: Among them, the coefficient k x =2,β x =2.5 and q x =1.6 are both positive numbers. In the sliding mode control law of the X axis, the sliding mode surface is The exponential reaching law is -k x f(s x )-q x s x , the position error about the horizontal X-axis of the inertial coordinate system is e x =P xr -P x , where P z and P zr are the actual position and expected position of the horizontal X-axis of the inertial coordinate system respectively; For the position control of the horizontal Y-axis of the inertial coordinate system, the sliding mode control law of the horizontal Y-axis of the inertial coordinate system is designed according to the dynamic equation: Among them, the coefficient k y =2.1,β y =2.6 and q y =1.6 are both positive numbers. In the sliding mode control law of the Y axis, the sliding mode surface is The exponential reaching law is -k y f(s y )-q y s y , the position error about the horizontal Y axis of the inertial coordinate system is e y =P yr -P y , where P y and P yr are the actual position and expected position of the horizontal Y axis of the inertial coordinate system respectively; According to the control law U x and U y , the formula for calculating the longitudinal and lateral flapping angles of the main rotor is as follows: Among them, K β The stiffness coefficient of the main rotor, H mr The vertical height from the main rotor hub to the helicopter's center of gravity, T tr The pull generated by the tail rotor, H tr It is the vertical distance from the tail rotor hub to the helicopter's center of gravity. is the roll angular acceleration, is the pitch angular acceleration; Then, the longitudinal cyclic pitch angle θ of the main rotor blade of the helicopter is calculated using the following formula: lon The steering gear angle δ lon and controls the lateral cyclic pitch angle θ of the helicopter main rotor blades lat The steering gear angle δ lat : The longitudinal cyclic pitch angle θ of the helicopter main rotor blade lon and the lateral periodic pitch angle θ lat The calculation formula is as follows: Where p is the roll angular velocity, q is the pitch angular velocity; S22: Design of nonlinear disturbance observer; The nonlinear disturbance observer of the inertial coordinate system Z axis is designed as follows: Where, Represents the estimated disturbance value in the Z-axis direction of the inertial coordinate system, Z z is the internal state of the observer, l(w a ) is the designed helicopter about the body axis z b The translational velocity w a The observer gain function, T mr is the sliding mode control law, g is the acceleration of gravity, and we have: Among them, in the nonlinear disturbance observer z1=2.8 and z2=0.2 are both positive constants; The nonlinear disturbance observer in the X-axis direction of the inertial coordinate system is designed as follows: Where, Indicates the estimated disturbance value in the X-axis direction of the inertial coordinate system, Z x is the internal state of the observer, l(u a ) is the observer gain function, U x is the control law of the horizontal X-axis direction of the inertial coordinate system, u a The x axis of the helicopter b Translational velocity in the direction; The nonlinear disturbance observer designed Among them, x1=2 and x2=0.12 are both positive constants; The nonlinear disturbance observer in the Y-axis direction of the inertial coordinate system is designed as follows: Where, Indicates the estimated disturbance value in the Y-axis direction of the inertial coordinate system, Z y is the internal state of the observer, l(v a ) is the observer gain function, U y is the control law of the horizontal Y-axis direction of the inertial coordinate system, v a The y axis of the helicopter b Translational velocity in the direction; Design of nonlinear disturbance observer Among them, y1=2.1 and y2=0.1 are both positive constants; S23: Determine the moment of inertia of the robotic arm by analyzing its motion t The relationship between the center of gravity OM of the helicopter-mounted robotic arm and the change in the robotic arm joint angle is: Among them, m t is the total mass of the UAM system, I0=J is the inertia tensor matrix of the helicopter itself, yes The third-order sequential principal minor of ; By OC i3×1 Constructed antisymmetric matrix, m i is the mass of the connecting rod, J i =diag{j ix ,j iy ,j iz } is the inertia tensor matrix of each connecting rod; S24: According to the dynamic equations of helicopter torque Design a posture controller where ω = (p, q, r) T is the angular velocity vector in the body coordinate system, U ω ∈R 3×1 is the control variable of attitude control, T∈R 3×1 is the column vector containing the perturbation, C∈R 3×3 is the coefficient matrix about the change of the overall center of gravity, which is: Where N mr is a constant, n Ij (j=x,y,z) represents the torque disturbance in each axis direction, M mr With L mr The torque generated by the main rotor is on the body axis x b and y b The component in the direction, T tr The pull generated by the tail rotor, H tr D is the vertical distance from the tail rotor hub to the helicopter's center of gravity. tr It is the distance from the tail rotor hub to the horizontal rear center of gravity of the helicopter. The backstepping sliding mode control method is used to stabilize the helicopter's hovering attitude: Assume the attitude angle of the helicopter is have Let the angle error be e ξ =ξ d -ξ, where ξ d is the desired attitude angle, and the time derivative of the angle error is Regarding the subsystem of angle ξ, let the first Lyapunov function be And the time derivative is Regarding the subsystem of angular velocity ω, the integral sliding surface Among them, λ=diag{5,8,3} is a constant coefficient, is the virtual control quantity, k ξ =diag{4,11,10} is a constant coefficient, and according to the exponential approach law, Calculation, the control quantity of attitude control is: Where k1 and k2 are diagonal matrices with constant coefficients whose elements are greater than zero, and k1 = diag{22,42,30}. k2=diag{27,27,40}; In the inner loop command generator, according to the thrust T generated by the tail rotor tr Solve for the tail rotor's collective pitch angle θ ped and its steering gear angle δ ped , the formula is as follows: Among them, D tr is the distance from the tail rotor hub to the helicopter's center of gravity at the rear horizontal position, v i,tr is the induced speed generated by the tail rotor, r is the yaw angular velocity; Servo control signal δ lat and δ lon The calculation formulas are: Among them, the longitudinal periodic pitch angle θ of the helicopter main rotor blade is lon and the lateral periodic pitch angle θ lat The calculation formula is: Where p is the roll angular velocity, q is the pitch angular velocity; S25: According to the dynamic equations of helicopter torque Construct a nonlinear extended state observer: Among them, Z1 represents the state observation of the attitude angle ξ, e o Represents the observation error of the attitude angle, Z2 represents the state observation of the attitude angular velocity ω, Z3 represents the observation value of the total internal and external disturbance f in the torque equation, a1=a2=a3=0.5, δ1=δ2=δ3=2, β1=diag{15,36,67}, β2=diag{15,55,35}, β3=diag{26,90,62}, fal in the extended state observer expression * is an improved nonlinear function, specifically expressed as:

4. A small unmanned helicopter hovering control system with a robotic arm, characterized in that: A hovering control method for a small unmanned helicopter with a robotic arm is used to execute any one of claims 1 to 3.

Images