Anti-interference control algorithm for flying mechanical arm coupling

CN118981835APending Publication Date: 2024-11-19SOUTHWEAT UNIV OF SCI & TECH
View PDF 0 Cites 2 Cited by

Patent Information

Application Number
CN202411057705.7
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-02
Publication Date
2024-11-19

AI Technical Summary

Technical Problem

[0004]本发明的目的在于针对现有技术中的上述不足,提供一种飞行机械臂耦合的抗干扰控制算法,以解决现有飞行器控制方法在该设备作业时出现姿态不平稳、易受外部干扰、飞行姿态控制精度低的问题

Benefits of technology

[0050] This invention divides the overall control of the flying mechanical arm into two parts: the aircraft and the mechanical arm, and adopts a separated control strategy; since the connection between the four-rotor aircraft and the mechanical arm is strongly coupled, the movement of one individual is regarded as a response to the other. Therefore, it is necessary to robustly control the aircraft and the manipulator, observe and estimate the disturbance caused by the work of other mechanisms within the system, and perform equivalent compensation. The so-called separated control method is to separate the two main components of the rotor flying robot arm and control them independently to improve the anti-interference ability of the flying robot arm and achieve the purpose of accurately controlling the attitude and grabbing the target.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure BDA0004976914610000031
    Figure BDA0004976914610000031
  • Figure BDA0004976914610000038
    Figure BDA0004976914610000038
  • Figure BDA0004976914610000041
    Figure BDA0004976914610000041
Patent Text Reader

Abstract

The invention discloses an anti-interference control algorithm for flying mechanical arm coupling. The anti-interference control algorithm comprises the steps that an outer ring controller and an inner ring controller are initialized; receiving input expected position information, and calculating an expected attitude angle of the aircraft; inputting the received expected attitude angle into an improved fuzzy active disturbance rejection controller to generate a control law, generating a control signal based on the control law, and transmitting the control signal to a control distributor; the control distributor converts the received control quantity signal and outputs the received control quantity signal into a corresponding motor control signal; the motor controller controls the four brushless motors to rotate at different speeds. After the expected pose of the aircraft is input, an improved fuzzy active-disturbance-rejection attitude controller is designed to suppress and counteract external disturbance and internal disturbance of the system, so that an aircraft platform is stabilized, the attitude of the unmanned aerial vehicle is controlled, and a stable platform is provided for follow-up action operation of a mechanical arm.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the technical field of aircraft, and in particular relates to an anti-interference control algorithm for flight manipulator coupling. Background Art

[0002] In recent years, with the maturity of relevant technologies and the improvement of supporting industrial chains, small multi-rotor UAVs and related flight platforms have achieved rapid development. The application fields have gradually expanded from professional aerial photography to many industries such as power line inspection, agricultural plant protection, emergency search and rescue, showing broad application prospects. They will surely become important products that serve the national economic development and open up new growth in high-tech industries in the future. However, constrained by technological development, the existing multi-rotor flight platforms are still limited to application scenarios that cannot physically contact the external environment, which greatly limits the actual application scope of UAVs. Therefore, conquering the flexible flight and dexterous aerial operation technology of UAVs will enable UAVs to have breakthrough aerial operation capabilities like birds hunting and nesting, greatly expand the application field of UAVs, solve the common problem of existing UAVs that they are "visible but not touchable", and realize the leap from flight platform to flight operation platform. It is one of the frontier issues that urgently need to be solved to drive the continuous development of related technologies.

[0003] At the same time, multi-rotor flight platforms usually rely on the blades with fixed rotation axis to generate aerodynamic force to control the posture and position of the fuselage. They have the inherent characteristics of under-actuation, strong coupling, high nonlinear randomness and uncertainty, and severe external disturbances. They are typical objects of long-term research in the control field. Multi-degree-of-freedom manipulators have many control quantities and complex kinematic and dynamic relationships. They are also hot research objects in the control field. When the flight operation platform realizes dexterous operations in the air, it is necessary to combine the manipulator with the flight platform to become a foundation-free floating manipulator system that has physical contact and interaction with the external environment. This provides a very challenging and complex research object for related control theory research, which has important theoretical research value and practical value. Summary of the invention

[0004] The purpose of the present invention is to provide an anti-interference control algorithm for flight manipulator coupling in order to solve the problems of unstable attitude, susceptibility to external interference and low flight attitude control accuracy in the existing aircraft control method when the device is operating.

[0005] In order to achieve the above object, the technical solution adopted by the present invention is:

[0006] An anti-interference control algorithm for flight manipulator coupling includes the following steps:

[0007] S1, initialize the outer loop controller and inner loop controller of the aircraft, the outer loop controller is the position controller, and the inner loop controller is the attitude controller;

[0008] S2, the position controller receives the input desired position information, calculates the desired attitude angle of the aircraft, and outputs the attitude angle to the attitude controller;

[0009] S3, the attitude controller inputs the attitude angle into the improved fuzzy anti-disturbance controller to generate a control law, generates a control signal based on the control law, and transmits the control signal to the control distributor;

[0010] S4, the control distributor converts the received control quantity signal and outputs it into a corresponding motor control signal;

[0011] S5. The motor controller receives the motor control signal, converts it into a corresponding motor PWM speed control signal to output, so as to control the four brushless motors to operate at different speeds, and then controls the attitude of the aircraft platform to provide a stable platform for the robot arm control.

[0012] Preferably, the improved fuzzy ADRC attitude control strategy in S3 includes:

[0013] S301, constructing an attitude model of the aircraft;

[0014]

[0015] in, is the second-order derivative of the aircraft’s flight altitude; is the second-order derivative of the vehicle’s roll angle; is the second-order derivative of the aircraft pitch angle; is the second-order derivative of the aircraft's yaw angle; g is the acceleration of gravity; m is the mass of the aircraft; φ is the aircraft's roll angle; θ is the aircraft's pitch angle; J x , J y , J z are the moments of inertia of the body around their respective coordinate axes; is the first-order derivative of the aircraft pitch angle; is the first-order derivative of the aircraft yaw angle; U 1 is the vertical motion control quantity; U 2 is the rolling motion control quantity; U 3 is the pitch motion control quantity; U 4 is the yaw motion control quantity; J is the motor moment of inertia;

[0016] S302. Design an improved fuzzy active disturbance rejection controller based on fuzzy logic control strategy;

[0017] The improved fuzzy anti-disturbance controller includes: tracking differentiator, extended state observer, nonlinear error feedback control law and fuzzy logic control part;

[0018] S303, using an improved fuzzy anti-disturbance controller to compensate for the total disturbance of the aircraft and rewrite the attitude model of the aircraft.

[0019] Preferably, the tracking differentiator is used to smooth the sudden change part of the input signal and extract the differential signal of the input signal, and filter the input signal at the same time, and its discretization expression is:

[0020]

[0021] Among them, v 1 To track the input signal, v d To track the expected input signal; v 2 、f h They are v 1 The first and second order derivatives of; T is the discrete sampling time; f han () is the fastest tracking function; k is the current time.

[0022] Preferably, the discretization expression of the extended state observer is:

[0023]

[0024] Among them, u is the input signal; y() is the output signal; z 1 (),z 2 () are the estimated values ​​of the internal state of the aircraft system and the comprehensive disturbance of the system by the extended state observer; z 3 is the state expanded by the observer, which means the estimation of the total disturbance; β 01 (),β 02 (),β 03() is the error feedback gain, which affects the convergence speed of ESO; α 1 ,α 2 is the power of the nonlinear part of the fal() function; δ is the interval width of the fal linear part; b is the control channel gain; T() is the discrete sampling time; e() is the difference between the estimated value of the internal system state of the aircraft and the output value of the controlled object;

[0025] Improve the fal() function to get the improved fal * ()function:

[0026]

[0027] Where x is fal * () function input; α is the improved fal * () is the power of the nonlinear part of the function; sign() is the sign function.

[0028] Preferably, the nonlinear error feedback control law controls the aircraft system and dynamically compensates for the total disturbance of the system according to the error between the output tracking signal generated by the tracking differentiator and its differential signal and the state variable estimate observed by the extended state observer. The discretized expression of the nonlinear error feedback control law is:

[0029]

[0030] Among them, e 1 () is the difference between the tracking input signal and the estimated value of the internal state of the aircraft system observed by the extended state observer; e 2 () is the difference between the differential signal of the tracking input signal and the estimated value of the comprehensive disturbance to the system; u 0 () is the feedback control law; β 1 , β 2 are the nonlinear feedback proportional factor and differential gain respectively; α 3 , α 4 For improved fal * () The power of the nonlinear part of the function; δ 0 For improved fal * ()The width of the interval of the linear part of the function.

[0031] Preferably, the fuzzy logic control part includes: taking the error signal e 1 and its differential signal e 2 As input, β is controlled by fuzzy control rules. 1 and β 2 Make corrections in real time.

[0032] Preferably, rewriting the attitude model of the aircraft in S303 includes:

[0033]

[0034] Among them, f 1 、f 2 、f 3 、f 4 is the internal disturbance of the aircraft system; w 1 、w 2 、w 3 、w 4 is the external disturbance of each channel;

[0035] Preferably, the robot arm control strategy includes:

[0036] T1, initialize the robot arm posture state and controller;

[0037] T2, input the desired motion position coordinates of the end of the robot arm and the current position coordinates of the end of the robot arm detected by the sensor, compare and subtract the desired motion position coordinates of the end of the robot arm and the current position coordinates of the end of the robot arm to obtain the motion position coordinate difference;

[0038] T3, input the motion position coordinate difference into the adaptive sliding mode controller to generate the manipulator control law, and input the manipulator control law into the disturbance observer;

[0039] T4, the disturbance observer estimates the total disturbance value of the internal and external disturbances of the observation system equivalently, and uses the disturbance compensation algorithm to compensate the equivalent estimated total disturbance value to generate a control law, which specifies the generation of a control signal;

[0040] T5. The control signal is converted into a motor control signal, and the motor control signal is input into the driver. The driver controls the motor to drive the robot arm to move, thereby realizing precise position control of the end of the robot arm.

[0041] Preferably, the control law of the adaptive sliding mode controller in T3 is designed as:

[0042]

[0043] Its adaptive law is:

[0044]

[0045] Among them, u is the robot arm control signal; is the second-order derivative of the desired end-of-arm motion position; is the estimated value of θ, θ is the unknown moment of inertia of the system manipulator; is the position tracking error; c is a constant, c>0; s is the defined sliding mode function; η is the coefficient; sign(s) is the sign function; k s is a coefficient greater than 0; is the first-order derivative of the estimated value of θ; is the mapping adaptive algorithm; γ is a constant; θ max is the maximum unknown moment of inertia of the system robot; is the input of the mapping adaptive algorithm.

[0046] Preferably, the control law generated by the disturbance observer in T4 is:

[0047]

[0048] Where u(t) is the control law signal; μ>0 is a constant; L is the transfer coefficient matrix from the state vector x(t) to the target output signal y(t); D is the coefficient matrix, and (LD) + (LD)=I l , I lis the identity matrix of order l; is the estimated value of y(t); A is the coefficient matrix; θ∈R m For optional vectors; LAL + is the pseudo-inverse of matrix A; s is the integrand in the integral function.

[0049] The anti-interference control algorithm for flight manipulator coupling provided by the present invention has the following beneficial effects:

[0050] The present invention divides the overall control of the flying manipulator into two parts: the aircraft and the manipulator, and adopts a separate control strategy; since the connection between the aircraft and the manipulator of the quadcopter is strongly coupled, the movement of one individual is regarded as a perturbation to the other individual, so it is necessary to perform robust control on the aircraft and the manipulator, observe and estimate the disturbance caused by the operation of other mechanisms within the system, and perform equivalent compensation. The so-called separate control method is to separate the two main components of the rotor flight manipulator and control them independently, improve the anti-interference ability of the flying manipulator, and achieve the purpose of accurately controlling the posture and grasping the target object.

[0051] After the desired aircraft posture is input, the present invention designs an improved fuzzy ADRC to suppress and offset the external and internal disturbances of the system, thereby stabilizing the aircraft platform, controlling the attitude of the drone, and providing a stable platform for the subsequent operation of the manipulator. Compared with the traditional ADRC, there are many problems in the controller design that require manual adjustment of parameters, and the use of fal * Function construction extended state observer When the deviation is large, the observer gain will increase sharply, resulting in a decrease in the anti-interference ability of the system. The aircraft part controller design of the present invention incorporates fuzzy logic control strategy into the nonlinear error feedback part to reduce the difficulty of parameter setting; an improved fal * Function, false * The function limits the amplitude to 1 when the error is large, that is, it limits the size of the system gain under large error conditions, thereby increasing the system's anti-interference ability.

[0052] The robot arm partial control of the present invention adopts adaptive sliding mode control and a continuum robot arm anti-interference control algorithm based on disturbance observer. When considering the influence of measurement noise, the traditional disturbance observer has the problem of reduced control performance. The anti-interference control algorithm based on disturbance observer adopted by the present invention measures the signal equal to the sum of the output of the control object and the measurement noise, estimates the internal parameter disturbance and external interference of the system through the interference estimation algorithm, constructs the control law, suppresses the interference from the inside and outside of the system during the operation of the robot arm grasping the target object, and realizes the accurate grasping of the target object. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1This is a control flow chart of the quadrotor aircraft of the present invention.

[0054] Figure 2 This is a flow chart of the robot arm control of the present invention.

[0055] Figure 3 This is an improved fuzzy anti-disturbance controller of the present invention.

[0056] Figure 4 This is a block diagram of the attitude angle control of the quadrotor aircraft of the present invention.

[0057] Figure 5 This is a control block diagram of the disturbance observer of the present invention.

[0058] Figure 6 This is a control block diagram of the robot arm of the present invention. DETAILED DESCRIPTION

[0059] The specific implementation modes of the present invention are described below so that those skilled in the art can understand the present invention. However, it should be clear that the present invention is not limited to the scope of the specific implementation modes. For those of ordinary skill in the art, as long as various changes are within the spirit and scope of the present invention as defined and determined by the attached claims, these changes are obvious, and all inventions and creations utilizing the concept of the present invention are protected.

[0060] Example 1

[0061] This embodiment provides an anti-interference control algorithm for flight manipulator coupling, referring to Figure 1 In this embodiment, an improved fuzzy anti-disturbance attitude controller is designed to suppress and offset the external disturbance and internal disturbance of the system, so as to stabilize the aircraft platform and control the attitude of the UAV, which specifically includes the following steps:

[0062] Step S1: After the aircraft takes off, the flight control instructions in the onboard computer are executed to initialize the inner and outer loop controllers, where the outer loop controller is the position controller and the inner loop controller is the attitude controller.

[0063] Step S2, the position controller receives the input desired position information, calculates the desired attitude angle of the aircraft, and outputs the attitude angle to the attitude controller;

[0064] Specifically, when the aircraft is flying in the air, it uses a camera to identify and locate the target object, and obtains the desired position information of the aircraft through coordinate position conversion, which is transmitted to the onboard computer program position controller through the communication bus. The position controller receives the input position information, calculates the desired attitude angle data of the aircraft, and inputs the attitude angle data into the inner attitude control loop.

[0065] Step S3: After the inner loop attitude control receives the attitude angle data output by the outer loop, the attitude angle data is input into the improved fuzzy anti-disturbance controller to generate a control law, which in turn controls the generation of a control signal and transmits it to the control distributor via a communication bus.

[0066] The attitude control strategy of the improved fuzzy ADRC in this embodiment includes:

[0067] Step S301, constructing an attitude model of a quadrotor aircraft;

[0068] Considering the nonlinear characteristics of the quadrotor, the following assumptions are made when establishing the attitude model: (1) the quadrotor is a rigid body; (2) the mass and moment of inertia of the quadrotor are constant; (3) the geometric center of the quadrotor is consistent with the center of mass. Based on the structure and body coordinate system of the quadrotor, the attitude model of the quadrotor is established as shown in the following formula.

[0069]

[0070] in, is the second-order derivative of the aircraft’s flight altitude; is the second-order derivative of the vehicle’s roll angle; is the second-order derivative of the aircraft pitch angle; is the second-order derivative of the aircraft's yaw angle; g is the acceleration of gravity; m is the mass of the aircraft; φ is the aircraft's roll angle; θ is the aircraft's pitch angle; J x , J y , J z are the moments of inertia of the body around their respective coordinate axes; for; U 1 is the vertical motion control quantity; U 2 is the rolling motion control quantity; U 3 is the pitch motion control quantity; U 4 is the yaw motion control quantity; J is the motor moment of inertia.

[0071] in:

[0072]

[0073] Where, d is the distance from the center of the quadrotor to the motor; c T is the propeller thrust coefficient; c M is the propeller torque coefficient; ω i (i=1,2,3,4) is the motor speed.

[0074] Step S302: designing an improved fuzzy active disturbance rejection controller based on fuzzy logic control strategy;

[0075] Since the quadcopter UAV system is an underactuated system, there are internal uncertainties. At the same time, the UAV is also easily disturbed by external unknown factors during flight, such as interfering wind, atmospheric pressure, temperature, etc. There are always internal uncertainties in the UAV system, such as some internal parameters cannot be determined and the dynamic part that is not modeled, etc. At the same time, the UAV is also easily disturbed by external unknown factors during flight, such as interfering wind, atmospheric pressure, temperature, etc. The use of an active disturbance rejection controller does not rely on the precise mathematical model of the controlled object, and can estimate and compensate for the internal and external disturbances of the system model. When the control object encounters uncertain disturbances or parameter changes, it can also achieve better control effects and has strong robustness. For the traditional active disturbance rejection controller (ADRC), there are many problems that need to be manually adjusted in the design of the controller, and when the error amount is large when the extended state observer is constructed by using the fal function, the gain of the observer will increase sharply, resulting in a decrease in the anti-interference ability of the system. The design of the aircraft part controller of the present invention incorporates fuzzy logic control strategy in the nonlinear error feedback part to reduce the difficulty of parameter setting; an improved fal function is used. * Function, false * The function limits the amplitude to 1 when the error is large, that is, it limits the size of the system gain under large error conditions, thereby increasing the system's anti-interference ability.

[0076] The improved fuzzy anti-disturbance controller designed by the present invention consists of four parts: tracking differentiator, extended state observer, nonlinear error feedback control law and fuzzy logic control part; Figure 3 , the following will describe the four parts in detail:

[0077] (1) Tracking Differentiator (TD)

[0078] Arranging a transition process for the system input can smoothly input the sudden change part of the signal and extract the differential signal of the input signal. It also has the function of filtering. Its discretization expression is:

[0079]

[0080] Among them, v 1 To track the input signal, v d To track the expected input signal; v 2 、f h They are v 1 The first and second order derivatives of; T is the discrete sampling time; f han () is the fastest tracking function; k is the current time;

[0081] f han (x 1 ,x 2 ,r,h) is the fastest tracking function, and its expression is:

[0082]

[0083] Among them, r is the speed factor that determines the tracking speed; h is the filter factor, d 0 is the intermediate variable of the fastest tracking function; a 0 is the intermediate variable of the fastest tracking function; x 1 ,x 2 f han ()The fastest tracking function has two input variables.

[0084] (2) Extended State Observer (ESO)

[0085] This observer is used to reflect the impact of the unknown part of the model and the external uncertainty disturbance on the controlled object. ESO estimates the total disturbance composed of the internal uncertainty of the system and the external disturbance in real time by designing a state quantity, and dynamically compensates it in the subsequent control, thereby turning the controlled object into a common integral series type object. The input signal of ESO is the input u and output y of the controlled object, and the output signal z 1 ,z 2 ,z 3 They represent the ESO's estimated values ​​of the system's internal state and the comprehensive disturbance to the system, respectively. 3 It represents the state expanded by the observer and the estimation of the total disturbance. ESO is discretized as shown below:

[0086]

[0087] Among them, β 01 ,β 02 ,β 03 is the error feedback gain, which affects the convergence speed of ESO; α 1 ,α 2 Is the power of the nonlinear part of the fal function, generally taken as α 1 =0.5,α 2 =0.25; δ is the width of the linear part of the fal function; b is the control channel gain, which determines the range of variation of the total disturbance estimate. The fal function is a traditional nonlinear function. However, when the deviation is large when the fal function is used to construct the extended state observer, the gain of the observer will increase sharply, resulting in a decrease in the system's anti-interference ability.

[0088] The present invention adopts an improved fal * Function, false * The function limits the amplitude to 1 when the error is large, that is, it limits the size of the system gain under large error conditions, thereby increasing the system's anti-interference ability. Based on this, the proposed improved fal * The function is:

[0089]

[0090] (3) Nonlinear Error Feedback Control Law (NLSEF)

[0091] The nonlinear error feedback control law module in the ADRC is a nonlinear control independent of the controlled object. It controls the system and dynamically compensates for the total disturbance of the system according to the error between the output tracking signal generated by the tracking differentiator and its differential signal and the state variable estimate observed by the extended state observer. The NLSEF discretization expression is:

[0092]

[0093] Among them, e 1 () is the difference between the tracking input signal and the estimated value of the internal state of the aircraft system observed by the extended state observer; e 2 () is the difference between the differential signal of the tracking input signal and the estimated value of the comprehensive disturbance to the system; u 0 () is the feedback control law; β 1 , β 2 are the nonlinear feedback proportional factor and differential gain respectively; α 3 , α 4 For improved fal * () The power of the nonlinear part of the function; δ 0 For improved fal * ()The width of the interval of the linear part of the function.

[0094] (4) Fuzzy logic control part;

[0095] After analyzing the above-mentioned ADRC principle, it is found that there are many parameters that need to be adjusted in the controller design process. In order to reduce the difficulty of parameter setting, the fuzzy logic control strategy is integrated into the nonlinear error feedback part, so that the system can automatically adjust the nonlinear feedback proportional factor β 1 and the differential gain β 2 In the controller, the error signal e 1 and its differential signal e 2 As input, β is controlled by fuzzy control rules. 1 and β 2 Real-time correction is performed to meet the requirements of different time periods. In the controller, the input of the fuzzy variable is e 1 and e 2 , the output is β 1 and β 2At the same time, five language sets {NB, NS, ZO, PS, PB} are defined to fuzzify the input and output quantities, and then fuzzy rules are established. According to the established fuzzy rules, fuzzy relations are solved and fuzzy reasoning is performed, and finally the control quantity is defuzzified.

[0096] Step S303, constructing a block diagram of the attitude fuzzy anti-disturbance control of the quadrotor aircraft;

[0097] According to the attitude model of the quadrotor aircraft (Equation (1)), the attitude angles of the quadrotor aircraft (roll angle φ, pitch angle θ, yaw angle ψ) are coupled with each other. The self-disturbance rejection control can solve this problem very well. The mutual influence between different attitude angle channels is regarded as the internal disturbance of the system, and the external disturbance caused by the environment is combined as the total disturbance of the system. Then each channel estimates the total disturbance in real time and compensates for it, thereby realizing decoupling control. The roll angle φ, pitch angle θ, yaw angle ψ and the height z of the aircraft are controlled by the improved fuzzy self-disturbance rejection controller. Each channel is changed from the original nonlinear and uncertain object to an "integral series" linear system. The control structure of the entire system is as follows: Figure 4 shown.

[0098] Among them, φ d ,θ d ,ψ d ,z d are the expected values ​​of roll angle, pitch angle, yaw angle and altitude respectively; u i (i=1,2,3,4) are the control variables of roll angle, pitch angle, yaw angle and altitude; M i (i=1,2,3,4) are the control signals of the four motors of the quadrotor.

[0099] Therefore, the attitude model (1) can be rewritten into the form corresponding to the nonlinear active disturbance rejection control theory, as shown in the following formula:

[0100]

[0101] Where: f i (i=1,2,3,4) is the internal disturbance of the system; w i (i=1,2,3,4) is the external disturbance of each channel;

[0102]

[0103] This embodiment takes the pitch angle θ as an example to design a second-order improved fuzzy anti-disturbance attitude controller.

[0104] (1) Tracking Differentiator (TD)

[0105] The outer loop position control solver calculates the expected pitch angle θd , is input into the improved fuzzy anti-disturbance control tracking differentiator (TD) of the pitch angle of the inner loop attitude controller. After TD, the tracking signal θ of the expected value is output. 1 and its derivative signal θ 2 , the TD expression is as follows:

[0106]

[0107] Where T is the discrete sampling time; r is the speed factor that determines the tracking speed; f han (θ 1 (k)-θ d (k),θ 2 (k), r, h) is the fastest tracking function; h is the filter factor.

[0108] (2) Extended State Observer (ESO)

[0109] First calculate the error e between the expected value and the actual value of the pitch angle θ tracking θ , then design the state quantity z θ1 To observe the pitch angle error; z θ2 To observe the differential signal of the error and the state quantity expanded by the design; θ3 The total disturbance of the observation system. Then the extended state observer is designed as:

[0110]

[0111] Among them, β θ01 , β θ02 , β θ03 is the pitch angle error feedback gain, which affects the convergence speed of ESO; α 1 , α 2 It is an improved fal * The power of the nonlinear part of the function, usually α 1 =0.5,α 2 =0.25; 6 is the improved fal * The width of the interval of the linear part of the function; b is the control channel gain, which determines the range of variation of the total disturbance estimate.

[0112] (3) Nonlinear Error Feedback Control Law (NLSEF)

[0113] The nonlinear error feedback law module in the ADRC is a nonlinear controller independent of the controlled object. According to the output tracking signal θ generated by the tracking differentiator, 1 and its derivative signal θ 2 The state variable estimate z observed by the extended state observer θ1 、z θ2The error between them is then used to control the system and dynamically compensate for the total disturbance of the system. The discretization expression of NLSEF is:

[0114]

[0115] Among them, β θ1 , β θ2 They are the nonlinear feedback proportional factor and differential gain respectively; the improved fal * The function is a nonlinear function; α 3 , α 4 It is an improved fal * The power of the nonlinear part of the function, usually α 3 =0.5,α 4 =0.25;δ 0 It is the width of the linear part of the fal* function.

[0116] (4) Fuzzy logic control part

[0117] In actual pitch angle control, the regulation rule of self-disturbance rejection control using an onboard computer or other computer is as follows:

[0118] u θ0 (k) = β θ1 fal*(e θ1 (k), α 3 , δ 0 )+β θ2 fal*(e θ2 (k), α 4 , δ 0 ) (12)

[0119] Among them, β θ1 , β θ2 They are the nonlinear feedback proportional factor and differential gain, i.e. β θ1 is the proportionality coefficient, β θ2 is the differential coefficient, e θ1 (k), e θ2 (k) is the error and the rate of change of the error.

[0120] The purpose of the fuzzy logic control designed in this invention is to make the parameter β θ1 , β θ2 With the deviation e θ1 (k) and the deviation change rate e θ2 (k) changes. Therefore, the relationship between them is established first. According to actual engineering experience, the parameter β θ1 , β θ2 In different θ1 (k), e θ2 The adjustment principles under (k) are:

[0121] (1) When the error e θ1 When (k) is large, it means that the absolute value of the error is large. Regardless of the trend of the error, the controller's β should be considered. θ1 Take a larger value to improve the rapidity of response; in order to suppress the overshoot of the system, β θ2 A larger value should be used.

[0122] (2) When the error e θ1 (k) When the size is medium, in order to reduce the overshoot of the system and ensure a certain response speed, β θ1 Should be appropriately reduced, while β θ2 The value should be moderate.

[0123] (3) When the error e θ1 When (k) is small, in order to reduce the steady-state error, β θ1 and β θ2 It should be larger to avoid the output response oscillating around the set value and to consider the anti-interference performance of the system. θ2 The value must be appropriate.

[0124] The following fuzzy control rule table is established for e θ1 , e θ2 The parameter setting fuzzy control tables are established respectively, as shown in Table 1 and Table 2.

[0125] Table 1-β θ1 Adjustment rules table

[0126]

[0127]

[0128] Table 2-β θ2 Adjustment rules table

[0129]

[0130] The fuzzy control subset is e θ1 、e θ2 ={NB,NS,ZO,PS,PB}, let e θ1 、e θ2 and β θ1 , β θ2 All obey normal distribution, so the membership of each fuzzy subset can be obtained. According to the membership assignment table of each fuzzy subset and the fuzzy control model of each parameter;

[0131] Apply fuzzy synthetic reasoning to design β θ1 , β θ2Fuzzy matrix table, find out the correction parameters and substitute them into the calculation formula:

[0132]

[0133] That is, the adjusted β θ1 , β θ2 Combining the ADRC parameter setting principle and the ADRC formula, an improved fuzzy active disturbance rejection pitch angle controller can be obtained.

[0134] The designs of the fuzzy anti-disturbance controllers for the three channels of roll angle φ, yaw angle ψ and aircraft height z are similar to the design of the above-mentioned pitch angle θ controller, so they will not be described in detail.

[0135] Step S4: After receiving the control quantity signal, the control distributor converts and outputs the corresponding motor control signal.

[0136] Step S5: After receiving the control signal, the motor controller converts and outputs the corresponding motor PWM speed control signal to control the four brushless motors to have different speeds. The four brushless motors have different speeds, which in turn controls the attitude angle of the aircraft platform, providing a stable platform for the robot arm below to accurately grasp the target object.

[0137] Example 2

[0138] This embodiment is based on the stable aircraft platform provided in Embodiment 1, and the manipulator control strategy based on the anti-interference control algorithm of adaptive sliding mode control and interference observer. Figure 2 and Figure 6 , which includes:

[0139] Step T1, initialize the robot arm posture state and all controller parameters;

[0140] Step T2, input the desired motion position coordinates of the end of the robot arm and the current position coordinates of the end of the robot arm detected by the sensor, compare and subtract the desired motion position coordinates of the end of the robot arm and the current position coordinates of the end of the robot arm to obtain the motion position coordinate difference;

[0141] Step T3, input the motion position coordinate difference into the adaptive sliding mode controller to generate the manipulator control law, and input the manipulator control law into the disturbance observer;

[0142] Among them, the adaptive sliding mode controller is designed as:

[0143] The robotic arm system is briefly described as:

[0144]

[0145] Among them, x 1 、x 2represents the position and speed of the end of the robot; J is the unknown moment of inertia of the system and is a constant greater than zero; △ is the total uncertainty including interference and model uncertainty; u(t) is the control signal. Taking θ = J, equation (15) is rewritten as:

[0146]

[0147] Assume the following:

[0148] ① The uncertain parameter θ is bounded:

[0149] 0<θ min ≤θ≤θ max

[0150] ②△ is bounded, expressed as:

[0151] |△|≤D

[0152] The sliding mode function is defined as:

[0153]

[0154] Among them, x 1 ,x 2 ,x d It is represented by the actual position of the current end of the robot arm, the current movement speed of the end of the robot arm and the expected movement position instruction of the end of the robot arm; e = x 1 -x d is the position tracking error, c>0.

[0155]

[0156] like is the estimated value of θ, and the Lyapunov function is defined as:

[0157]

[0158] in,

[0159] but:

[0160]

[0161] The control law is designed as:

[0162]

[0163] Among them, u is the robot arm control signal; is the second-order derivative of the desired end-of-arm motion position; is the estimated value of θ, θ is the unknown moment of inertia of the system manipulator; is the position tracking error; c is a constant, c>0; s is the defined sliding mode function; η is the coefficient; sign(s) is the sign function; k s is a coefficient greater than 0; is the first derivative of the estimate of θ.

[0164]

[0165] Take the adaptive law as

[0166]

[0167] but:

[0168]

[0169] Since if and only if s = 0, when When s≡0. According to the LaSalle invariance principle, the closed-loop system is asymptotically stable, that is, when t→∞, s→0; the system convergence speed depends on k s .

[0170] To prevent Too large, causing the control input signal u(t) to be too large or In this case, it is necessary to design an adaptive law Changes in [θ min θ max ] range, a mapping adaptive algorithm is used to modify formula (23) as follows:

[0171]

[0172] in, is the first-order derivative of the estimated value of θ; is the mapping adaptive algorithm; γ is a constant; θ max is the maximum unknown moment of inertia of the system robot; is the input of the mapping adaptive algorithm.

[0173] even though If it exceeds the maximum value and there is a trend of continued increase, that is, Then take The value remains unchanged, that is when If it exceeds the minimum value and there is a trend of continuing to decrease, that is, Then take The value remains unchanged, that is This mapping algorithm can ensure To ensure

[0174] Step T4, the disturbance observer performs equivalent estimation of the total disturbance value of the internal and external disturbances of the observation system, and compensates the equivalent estimated total disturbance value by using the disturbance compensation algorithm to generate a control law, which specifies the generation of a control signal;

[0175] Among them, this embodiment is a continuum manipulator anti-interference control algorithm based on a disturbance observer, which specifically includes the following contents:

[0176] The basic idea of ​​the disturbance observer is to equate the difference between the actual model and the ideal model output caused by external disturbances and model parameter changes to the total disturbance, that is, to observe the equivalent disturbance, introduce an equal amount of compensation in the control, and achieve complete suppression of the disturbance. The control block diagram of the disturbance observer is shown in the figure. Figure 5 shown.

[0177] Figure 5 Medium G p (s) is the transfer function of the object, is the equivalent disturbance, d is the observed disturbance, and u is the control input. As can be seen from the figure, the estimated value of the equivalent disturbance is for:

[0178]

[0179] The above results show that the above control method can accurately estimate the interference and take compensation measures to reduce or even eliminate the interference. However, for the actual physical system, its implementation has the following problems:

[0180] (1) Usually, G p The relative order of (s) is not 0, and its inverse G p -1 (s) physically difficult to achieve;

[0181] (2) Object G p The exact mathematical model of (s) is not available;

[0182] (3) Considering the influence of measurement noise, the control performance of the above method will decrease.

[0183] In view of the shortcomings of the above-mentioned traditional disturbance observer, this embodiment proposes a continuum manipulator anti-interference control algorithm based on the disturbance observer. The anti-interference control algorithm estimates the interference caused by external interference and model parameter changes, and uses the disturbance compensation algorithm to compensate the estimated disturbance value to achieve the purpose of suppressing interference.

[0184] The control algorithm considers a nonlinear controlled object with vector input and output signals. The signal available for measurement is the sum of the output of the controlled object and the measurement noise, both of which have the same dimension and are independent of the measurement noise and disturbance sources. Parameter disturbances and external disturbances may be present in any equation of the target model. The conditions for calculating the algorithm parameters are obtained in the form of linear matrix inequalities (LMI) solvability.

[0185] The following symbols are used in this algorithm: R is the set of real numbers; I l The order is The identity matrix of is a matrix of dimension (m-1)×m, which is obtained by removing the m-order identity matrix OK n×l is a zero matrix of dimension n×l; A + is the pseudo-inverse matrix of A; E j =[0, ..., 0, 1, 0, ..., 0] T is a vector of corresponding dimension, whose j-th component is equal to 1 and the rest are 0; |·| and ||·|| represent the Euclidean norm of the vector and the matrix norm consistent with it, respectively; p=d / dt is the differential operator.

[0186] The following is a description of the above issues:

[0187] A controlled object, whose model is described by the following equation:

[0188]

[0189] y(t)=Lx(t) (28)

[0190] z(t)=y(t)+ζ(t) (29)

[0191] Where x(t)=[x 1 (t), x 2 (t), ..., x n (t)] T is the state vector; u(t)∈R l is the control signal; y(t)∈R m is the unmeasurable target output signal (m≥2); z(t)∈R m is a measurable signal; ζ(t) = [ζ 1 (t),ζ 2 (t), ..., ζ m (t)] T is bounded measurement noise. The unknown function ψ(x)∈R l and The following conditions are met: is a bounded function, and X 1 >0,α 1 >0 and α 2 > 0. Given a matrix A∈R n×n , B∈R n×l , D∈R n×l and And A is a Hurwitz matrix, (LD) + (LD)=I l , (LD) + (LB) = kI l , k∈R. Unknown coefficient c 0 belongs to a known interval, that is, c 0 ∈[c min , c max ], and c min +k>0. In the following proposition, condition (LD) must be met + (LD)=I l , (LD) + (LB) = kI l To form the model required for the closed-loop system, and then obtain the linear matrix inequality. It can be seen that the condition c is satisfied min +k>0 will ensure the boundedness of the control signal. From a practical point of view, the condition c min +k>0 is equivalent to providing negative feedback in a closed system.

[0192] Assume that there is a vector No. components, and satisfy the following relationship:

[0193]

[0194] Among them, i∈{1,..,m} and g∈{1,..,i-1,i+1,...,m}.

[0195] Necessity of condition (30): In order to compensate for independent disturbances and measurement noise simultaneously, at least two independent measurements of the target output signal are required. No. The limit value of one component is smaller than the limit value of other components. First, estimate the signal It is used to measure noise The part that does not include the i-th component. The accuracy of the estimate will depend on the vector Middle The value of the component, which is why condition (30) needs to be satisfied. By using the estimated value of the actual output signal of the controlled object, we can construct the estimated value of the disturbance and then compensate for it.

[0196] Assumptions is a bounded signal, and |ζ i (t)|≤χ 2 and Indicates that, where χ 2 and χ 3 Is a normal number.

[0197] A control algorithm needs to be developed to meet the target conditions

[0198]

[0199] Where δ>0 will be defined later.

[0200] Interference Estimation Algorithm

[0201] in:

[0202]

[0203] Considering the controlled object equations (27) and (28), formula (29) is rewritten as follows:

[0204]

[0205] Eliminate the i-th equation in formula (34). Formula (34) is multiplied on the left by get:

[0206]

[0207] in, The interference model can be expressed as:

[0208]

[0209] Define new symbols:

[0210]

[0211] Introduce an algorithm to estimate the vector Its form is:

[0212]

[0213] in, yes The estimated vector of γ∈R m-1 is a vector of choices.

[0214] To evaluate the performance of formula (13), consider the error:

[0215]

[0216] Differentiating the above formula gives the following form:

[0217]

[0218] From formula (39), we can see that the value of the error e depends on ζ i (t), so it is necessary to satisfy condition (30). The value of the error e depends on f and u. By selecting and controlling u, the effect of the disturbance f on Estimated quality impact.

[0219] Disturbance compensation algorithm

[0220] Through the algorithm formula (37), the algorithm can estimate the disturbance signal part ζ(t) as the signal use To complete the information about the signal y(t). is the estimated value of y(t). Considering formula (34) and formula (37), we introduce The expression is of the form:

[0221]

[0222] Differentiate equation (40) in time, taking into account equations (27), (28), and (36), and write the result as:

[0223]

[0224] Using the structure of formula (41), the estimated value of the disturbance f is introduced Its form is:

[0225]

[0226] Among them, α(p) is a scalar differential operator, v(t)∈R l It is the auxiliary control action required to generate the disturbance compensation signal.

[0227] Introduce an auxiliary control action:

[0228]

[0229] Substituting formula (42) into formula (43), we can obtain:

[0230]

[0231] set up Where μ>0 is a sufficiently small number, and the formula (44) is integrated over t:

[0232]

[0233] To compensate for the disturbance, the control law can be expressed in the form of u(t) = v(t). However, the performance of the control law does not depend on the initial conditions. and v(0). Therefore, the control law can be expressed as:

[0234]

[0235] Where u(t) is the control law signal; μ>0 is a constant; L is the transfer coefficient matrix from the state vector x(t) to the target output signal y(t); D is the coefficient matrix, and (LD) + (LD)=I l , I l is the identity matrix of order l; is the estimated value of y(t); A is the coefficient matrix; θ∈R m For optional vectors; LAL + is the pseudo-inverse of matrix A; s is the integrand in the integral function.

[0236] Consider the control system consisting of the target model (27), (28), the disturbance estimation algorithm (37) and the control law (46). For a given number β>0 and μ>0, there exists a coefficient τ 1 >0,τ 2 >0, ρ>0 and matrix P>0, so that the linear matrix inequality (LMI) holds. Algorithms (35), (37), (40) and (46) ensure that the objective condition (31) is satisfied, where

[0237]

[0238] Among them, λ min (P) is the smallest eigenvalue of the matrix P.

[0239] Step T5: convert the control signal into a motor control signal, input the motor control signal into a driver, and the driver controls the motor to drive the robot arm to move, thereby achieving precise position control of the end of the robot arm.

[0240] Although the specific implementation of the invention is described in detail in conjunction with the drawings, it should not be understood as limiting the scope of protection of this patent. Within the scope described in the claims, various modifications and variations that can be made by those skilled in the art without creative work still fall within the scope of protection of this patent.

Claims

1. An anti-interference control algorithm for flight manipulator coupling, characterized in that: The following steps are involved: S1, initialize the outer loop controller and inner loop controller of the aircraft, the outer loop controller is the position controller, and the inner loop controller is the attitude controller; S2, the position controller receives the input desired position information, calculates the desired attitude angle of the aircraft, and outputs the attitude angle to the attitude controller; S3, the attitude controller inputs the attitude angle into the improved fuzzy anti-disturbance controller to generate a control law, generates a control signal based on the control law, and transmits the control signal to the control distributor; S4, the control distributor converts the received control quantity signal and outputs it into a corresponding motor control signal; S5. The motor controller receives the motor control signal, converts it into a corresponding motor PWM speed control signal to output, so as to control the four brushless motors to operate at different speeds, and then controls the attitude of the aircraft platform to provide a stable platform for the robot arm control.

2. The anti-interference control algorithm for flight manipulator coupling according to claim 1 is characterized in that: The improved fuzzy ADRC attitude control strategy in S3 includes: S301, constructing an attitude model of the aircraft; in, is the second-order derivative of the aircraft’s flight altitude; is the second-order derivative of the aircraft’s roll angle; is the second-order derivative of the aircraft pitch angle; is the second-order derivative of the aircraft's yaw angle; g is the acceleration of gravity; m is the aircraft's mass; φ is the aircraft's roll angle; θ is the aircraft's pitch angle; J x , J y , J z are the moments of inertia of the body around their respective coordinate axes; is the first-order derivative of the aircraft pitch angle; is the first-order derivative of the aircraft's yaw angle; U1 is the vertical motion control quantity; U2 is the roll motion control quantity; U3 is the pitch motion control quantity; U4 is the yaw motion control quantity; J is the motor's moment of inertia; S302. Design an improved fuzzy active disturbance rejection controller based on fuzzy logic control strategy; The improved fuzzy anti-disturbance controller includes: tracking differentiator, extended state observer, nonlinear error feedback control law and fuzzy logic control part; S303, using an improved fuzzy anti-disturbance controller to compensate for the total disturbance of the aircraft and rewrite the attitude model of the aircraft.

3. The anti-interference control algorithm for flight manipulator coupling according to claim 2 is characterized in that: The tracking differentiator is used to smooth the sudden change part of the input signal and extract the differential signal of the input signal, and filter the input signal at the same time. Its discretization expression is: Among them, v1 is the tracking input signal, v d is the expected signal for tracking input; v2, f h are the first and second order derivatives of v1 respectively; T is the discrete sampling time; f han () is the fastest tracking function; k is the current time.

4. The anti-interference control algorithm for flight manipulator coupling according to claim 3 is characterized in that: The discretization expression of the extended state observer is: Among them, u is the input signal; y() is the output signal; z1(), z2() are the estimated values ​​of the internal state of the aircraft system and the comprehensive disturbance of the system by the extended state observer; z3 is the state expanded by the observer, which means the estimation of the total disturbance; β 01 (),β 02 (),β 03() is the error feedback gain, which affects the convergence speed of ESO; α1 and α2 are the powers of the nonlinear part of the fal() function; δ is the interval width of the fal linear part; b is the control channel gain; T() is the discrete sampling time; e() is the difference between the estimated value of the internal system state of the aircraft and the output value of the controlled object; Improve the fal() function to get the improved fal * ()function: Where x is fal * () function input; α is the improved fal * () is the power of the nonlinear part of the function; sign() is the sign function.

5. The anti-interference control algorithm for flight manipulator coupling according to claim 4 is characterized in that: The nonlinear error feedback control law controls the aircraft system and dynamically compensates for the total disturbance of the system according to the error between the output tracking signal generated by the tracking differentiator and its differential signal and the state variable estimation observed by the extended state observer. The discretized expression of the nonlinear error feedback control law is: Among them, e1() is the difference between the tracking input signal and the estimated value of the internal state of the aircraft system observed by the extended state observer; e2() is the difference between the differential signal of the tracking input signal and the estimated value of the comprehensive disturbance received by the system; u0() is the feedback control law; β1 and β2 are the nonlinear feedback proportional factor and the differential gain respectively; α3 and α4 are the improved fal * () The power of the nonlinear part of the function; δ0 is the improved fal * ()The width of the interval of the linear part of the function.

6. The anti-interference control algorithm for flight manipulator coupling according to claim 5, characterized in that: The fuzzy logic control part includes: taking the error signal e1 and its differential signal e2 as input, and using fuzzy control rules to perform real-time correction on β1 and β2.

7. The anti-interference control algorithm for flight manipulator coupling according to claim 2, characterized in that: The step of rewriting the attitude model of the aircraft in S303 includes: Among them, f1, f2, f3, and f4 are the internal disturbances of the aircraft system; w1, w2, w3, and w4 are the external disturbances of each channel; 8. The anti-interference control algorithm for flight manipulator coupling according to claim 1, characterized in that: The robot control strategy includes: T1, initialize the robot arm posture state and controller; T2, input the desired motion position coordinates of the end of the robot arm and the current position coordinates of the end of the robot arm detected by the sensor, compare and subtract the desired motion position coordinates of the end of the robot arm and the current position coordinates of the end of the robot arm to obtain the motion position coordinate difference; T3, input the motion position coordinate difference into the adaptive sliding mode controller to generate the manipulator control law, and input the manipulator control law into the disturbance observer; T4, the disturbance observer estimates the total disturbance value of the internal and external disturbances of the observation system equivalently, and uses the disturbance compensation algorithm to compensate the equivalent estimated total disturbance value to generate a control law, which specifies the generation of a control signal; T5. The control signal is converted into a motor control signal, and the motor control signal is input into the driver. The driver controls the motor to drive the robot arm to move, thereby realizing precise position control of the end of the robot arm.

9. The anti-interference control algorithm for flight manipulator coupling according to claim 8, characterized in that: The control law of the adaptive sliding mode controller in T3 is designed as follows: Its adaptive law is: Among them, u is the robot arm control signal; is the second-order derivative of the desired end-of-arm motion position; is the estimated value of θ, θ is the unknown moment of inertia of the system manipulator; is the position tracking error; c is a constant, c>0; s is the defined sliding mode function; η is the coefficient; sign(s) is the sign function; k s is a coefficient greater than 0; is the first-order derivative of the estimated value of θ; is the mapping adaptive algorithm; γ is a constant; θ max is the maximum unknown moment of inertia of the system robot; is the input of the mapping adaptive algorithm.

10. The anti-interference control algorithm for flight manipulator coupling according to claim 8, characterized in that: The control law generated by the disturbance observer in T4 is: Where u(t) is the control law signal; μ>0 is a constant; L is the transfer coefficient matrix from the state vector x(t) to the target output signal y(t); D is the coefficient matrix, and (LD) + (LD)=I l , I l is the identity matrix of order l; is the estimated value of y(t); A is the coefficient matrix; θ∈R m For optional vectors; LAL + is the pseudo-inverse of matrix A; s is the integrand in the integral function.

Citation Information

Cited By

  • Automatic equipment control method and system based on low-power-consumption rocker

    CN119536120A

  • Automation equipment control method and system based on low-power joystick

    CN119536120B