Active-disturbance-rejection sliding-mode composite control system of wrist rehabilitation robot

Abstract

The invention provides an active-disturbance-rejection sliding mode compound control system of a wrist rehabilitation robot, and relates to the technical field of rehabilitation robot control, the wrist rehabilitation robot adopts a folding air bag as a driving element, and the control system comprises a tracking differentiator used for tracking and differentiating an input target angle signal; the extended state observer is used for estimating the system state and the total disturbance in real time; the sliding mode controller is used for designing a nonlinear control rate; the dead-zone nonlinear inverse function control module is used for compensating the nonlinear control rate of the sliding mode controller and eliminating dead-zone nonlinearity of the high-speed switch valve; and the self-adaptive combination valve comprises two high-speed switch valves, a control unit and a pressure sensor and is used for executing the control instruction and monitoring the pressure in the folding air bag. The problems of compressibility of air in the air bag, dead zone nonlinearity of a gas control element, nonlinearity of gas flow and the like can be solved, and high-precision trajectory tracking control over the wrist rehabilitation robot is achieved.

Description

Technical Field

[0001] The invention relates to the technical field of rehabilitation robot control, and in particular to an anti-disturbance sliding mode composite control system of a wrist rehabilitation robot. Background Art

[0002] Wrist dysfunction is a common movement disorder, and repeated physical training is a conventional method to restore wrist movement function. However, providing long-term, repeated physical training for a large number of people who need rehabilitation requires a large number of full-time technicians, and the lack of medical resources restricts the rehabilitation process. To address this problem, researchers have proposed a solution for robot-assisted rehabilitation, which can provide users with more precise and repetitive training with minimal supervision.

[0003] Over the past few decades, researchers have developed a variety of robotic devices for wrist rehabilitation training. These devices include single-degree-of-freedom and multi-degree-of-freedom wrist rehabilitation robots that can provide a variety of movements such as palmar flexion / dorsiflexion, abduction / adduction. Most of these devices are motor-driven and belong to rigid exoskeleton structures with high control accuracy, strong rigidity, and small allowable misalignment. However, if this structure exceeds the user's maximum tolerance angle during exercise, it is easy to cause wrist injury.

[0004] In order to improve the flexibility of rehabilitation devices, some researchers have begun to use pneumatic drive devices, such as pneumatic artificial muscles. Compared with motor drive, pneumatic drive devices are more compact, lighter and more convenient to use. However, due to the compressibility of the air inside the pneumatic system, the pressure-angle-force nonlinearity of the airbag itself, the dead zone nonlinearity of the gas control element and the nonlinearity of the gas flow through the control element, these nonlinear factors are coupled with each other and jointly affect the dynamic characteristics of the pneumatic system. Traditional linear control methods (such as PID control) are difficult to effectively handle these complex nonlinearities and uncertainties, and cannot achieve accurate trajectory tracking control. Summary of the invention

[0005] In view of this, the present invention proposes an auto-disturbance rejection sliding mode composite control system for a wrist rehabilitation robot. By combining the advantages of auto-disturbance rejection control and sliding mode control, a control strategy that can effectively estimate and compensate for the total disturbance of the system is designed. The system aims to overcome the problems of air compressibility inside the airbag, dead zone nonlinearity of the gas control element, and gas flow nonlinearity, and realize high-precision trajectory tracking control of the wrist rehabilitation robot.

[0006] The technical solution of the present invention is implemented as follows: The present invention provides an anti-disturbance sliding mode composite control system for a wrist rehabilitation robot, the wrist rehabilitation robot adopts a folding airbag as a driving element, and the control system includes:

[0007] A tracking differentiator, used for tracking and differentiating an input target angle signal;

[0008] Extended state observer for real-time estimation of system state and total disturbance;

[0009] Sliding mode controller for designing nonlinear control rates;

[0010] Dead zone nonlinear inverse function control module, used to compensate for the nonlinear control rate of the sliding mode controller and eliminate the dead zone nonlinearity of the high-speed switch valve;

[0011] The adaptive combination valve includes two high-speed switching valves, a control unit and a pressure sensor, which are used to execute control instructions and monitor the pressure in the folding airbag.

[0012] On the basis of the above technical solution, preferably, the mathematical model of the high-speed switching valve is:

[0013]

[0014] Where f(u) is the effective opening area coefficient of the valve, u is the input duty cycle, and u s is the dead-band duty cycle.

[0015] Based on the above technical solution, preferably, the energy conservation equation of the folded airbag is:

[0016]

[0017] Where P q is the internal pressure of the folded airbag, q m1 is the net mass flow rate in the airbag cavity, q m1 =q m2 -q m3 , where q m2 is the mass flow rate of inflating the folded airbag, q m3 is the mass flow rate of the folded airbag deflation, K is the air specific heat ratio, R is the ideal gas constant, V q is the volume of the folded airbag, T u Absolute temperature of the gas source.

[0018] On the basis of the above technical solution, preferably, the dynamic model of the folding airbag driven wrist rehabilitation robot is:

[0019]

[0020] In the formula, θ f is the rotation angle of the wrist rehabilitation robot, r is the distance from the center of the folded airbag to the wrist joint rotation axis, S is the contact area between the folded airbag and the base plate, K is the air specific heat ratio, R is the ideal gas constant, T is the absolute temperature, q mis the mass flow rate of the gas in the folding airbag, J is the moment of inertia of the joint rotor, f(u) is the effective opening area coefficient of the valve, b v is the joint damping coefficient, m is the mass of the hand support board, g is the acceleration of gravity, F h is the force exerted on the wrist rehabilitation robot when the wrist interacts with the wrist rehabilitation robot, b0 is a constant term, a1, a2, a3 are fitting relationship coefficients, which are used to fit the volume V of the folded airbag q The rotation angle θ of the wrist rehabilitation robot f Functional relationship:

[0021] make and:

[0022]

[0023] Then the state space equation of the control system is obtained as:

[0024]

[0025] Among them, is regarded as an unknown nonlinear term of the control system.

[0026] On the basis of the above technical solution, preferably, the tracking differentiator is designed as follows:

[0027]

[0028] fHan(θ r1 -θ d ,θ r2 ,r0,h0) The specific expression is as follows:

[0029]

[0030] in:

[0031]

[0032] Where: r0 is the speed factor of the tracking differentiator. The larger r0 is, the faster the tracking speed is. h0 is the filter factor of the differential tracker. θ d is the input target angle signal of the tracking differentiator; θ r1 is the input tracking signal of the tracking differentiator; θ r2 is θ r1 The differential signal.

[0033] On the basis of the above technical solution, preferably, the unknown nonlinear term of the control system is As the state variable of the extended state observer, let The derivative of Right now The third-order extended state observer is designed as follows:

[0034]

[0035] Where ε1 is the angle observation value Output Angle The error, and They are and The estimated value of β 01 , β 02 and β 03 are the gain coefficients of the extended state observer respectively; fal(ε i ,a i ,δ) function is defined as follows:

[0036]

[0037] in, i=1,2,3.

[0038] Based on the above technical solution, preferably, the nonlinear control rate of the sliding mode controller is as follows:

[0039]

[0040] In the formula, f u is the sliding mode control rate; s is the sliding mode surface; e1 and e2 are respectively generated by the tracking differentiator Signal and the estimate produced by the extended state observer The error between η>0 and 0

[0041]

[0042] Where x represents the system state, which is used to represent the distance from the system state point to the sliding surface, and α is a variable coefficient. is the variable gain term of the approach rate, and c is the control parameter used to control the smooth continuity of sigmoid(s).

[0043] On the basis of the above technical solution, preferably, the dead zone nonlinear inverse function control module takes the effective opening area coefficient f(u) of the inflation and deflation valve output by the sliding mode controller as input, and calculates the effective opening area coefficient f(u) of the inflation and deflation valve output by f -1 (f(u)) is used as the input duty cycle command u of the high-speed on-off valve for charging and discharging to eliminate the dead zone nonlinearity of the high-speed on-off valve. The expression of the control rate u is: ​

[0044]

[0045] When f(u)>0, the control rate u1=u is obtained, which is used as the input duty cycle of the high-speed switching valve to control the inflation flow of the folding airbag; when f(u)<0, the control rate u2=-u is obtained, which is used as the input duty cycle of the high-speed switching valve to control the deflation flow of the folding airbag.

[0046] On the basis of the above technical solution, preferably, the control system further includes:

[0047] Sine wave function generation module, used to generate target angle signal:

[0048] θ d =A sin(πft-0.5π)+B

[0049] Among them, θ d is the target angle signal, A=25 is the amplitude of the sine wave, f=0.025Hz is the frequency of the sine wave, and B=35 is the offset of the sine wave angle;

[0050] Minimum acceleration model generation module, used to generate the optimal wrist motion curve:

[0051]

[0052] θ0=10,θ1=10when t∈[nT,1 / 4T+nT)

[0053] θ0=10,θ1=50when t∈[1 / 4T+T,1 / 2T+nT)

[0054] θ0=50,θ1=50when t∈[1 / 2T+nT,3 / 4T+nT)

[0055] θ0=50,θ1=10when t∈[3 / 4T+nT,T+nT)

[0056] Among them, θ0 is the initial position of the joint movement, θ1 is the stop position of the joint movement, T is the period of the optimal control curve of the wrist movement, n is the number of times the wrist movement control curve is repeated, t is the time value, d is the interval time, t1 is the stop time of the joint movement, and t0 is the start time of the joint movement.

[0057] On the basis of the above technical solution, preferably, the operation process of the control system is:

[0058] The S1 tracking differentiator receives the target angle signal and generates a smooth input tracking signal and its differential signal;

[0059] The S2 extended state observer estimates the system state and total disturbance based on the system input and output;

[0060] The S3 sliding mode controller designs nonlinear control rates based on state estimation and disturbance estimation;

[0061] The nonlinear control rate of the S4 dead zone nonlinear inverse function control module is compensated to eliminate the dead zone nonlinearity of the high-speed switching valve;

[0062] The S5 adaptive combination valve executes the compensated control command to adjust the gas flow in the folding airbag;

[0063] The S6 encoder records the rotation angle of the wrist joint in real time and feeds it back to the control system to form a closed-loop control.

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

[0065] (1) By combining the advantages of anti-disturbance control and sliding mode control, the complex nonlinear problems and uncertainties existing in the folding airbag-driven wrist rehabilitation robot are effectively overcome. The system can estimate and compensate the total disturbance of the system in real time, and at the same time use nonlinear sliding mode control to deal with the nonlinear characteristics of the folding airbag, model inaccuracy and external load disturbance, thereby achieving high-precision trajectory tracking control of the wrist rehabilitation robot;

[0066] (2) A tracking differentiator is used to track and differentiate the input target angle signal to generate a smooth input tracking signal and its differential signal, which effectively avoids the high-frequency noise that may be introduced by the traditional differentiator and improves the system's anti-interference ability and control accuracy;

[0067] (3) The designed extended state observer can estimate the system state and total disturbance in real time, normalize the system's "unmodeled dynamics" and "unknown external disturbance" effects into the system's total disturbance, and provide real-time estimation and dynamic compensation, thereby improving the system's adaptability to external disturbances and parameter changes;

[0068] (4) The proposed nonlinear sliding mode controller designs a new reaching law and uses the sigmoid function instead of the traditional sign function, which effectively reduces the system chattering phenomenon and improves the system stability and comfort;

[0069] (5) The designed dead zone nonlinear inverse function control module effectively eliminates the dead zone nonlinearity of the high-speed switching valve by compensating for the nonlinear control rate output by the sliding mode controller, thereby improving the control accuracy and response speed of the system;

[0070] (6) The adaptive combination valve is used to execute control instructions and monitor the pressure inside the folding airbag, which realizes precise control of the gas flow and improves the dynamic response performance and stability of the system;

[0071] (7) The provided sine wave function generation module and minimum acceleration model generation module can generate target angle signals that conform to the motion characteristics of the human body, making the motion trajectory of the wrist rehabilitation robot closer to natural human motion, thereby improving the effect and comfort of rehabilitation training. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0073] Figure 1 It is a framework diagram of the control system of the present invention;

[0074] Figure 2 It is a schematic diagram of a curve of the fitted folding airbag gas and the wrist joint rotation angle of the present invention;

[0075] Figure 3 A curve diagram showing the relationship between the input duty ratio and the valve port effective area coefficient of the high-speed switch valve of the present invention;

[0076] Figure 4 It is a schematic diagram of the structure of the active disturbance rejection sliding mode composite controller of the present invention;

[0077] Figure 5 It is the sigmoid function curve diagram of the present invention;

[0078] Figure 6 It is a curve diagram of the control rate u after eliminating the nonlinearity of the dead zone of the present invention;

[0079] Figure 7 This is a comparison chart of the experimental results of the sinusoidal wave trajectory tracking control of the control system and the PID controller in Example 1 of the present invention;

[0080] Figure 8 This is a schematic diagram of estimating angular velocity and system disturbance when the control system is running in Example 1 of the present invention;

[0081] Fig. 9 This is a comparison chart of wrist optimal trajectory tracking control experimental results of the control system and the PID controller in Example 2 of the present invention;

[0082] Fig.10 It is a schematic diagram of estimating angular velocity and system disturbance when the control system of Example 2 of the present invention is running. DETAILED DESCRIPTION

[0083] The following will be combined with the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0084] like Figure 1 As shown, the present invention provides an anti-disturbance sliding mode composite control system of a wrist rehabilitation robot, the wrist rehabilitation robot adopts a folding airbag as a driving element, and the control system includes:

[0085] A tracking differentiator, used for tracking and differentiating an input target angle signal;

[0086] Extended state observer for real-time estimation of system state and total disturbance;

[0087] Sliding mode controller for designing nonlinear control rates;

[0088] Dead zone nonlinear inverse function control module, used to compensate for the nonlinear control rate of the sliding mode controller and eliminate the dead zone nonlinearity of the high-speed switch valve;

[0089] The adaptive combination valve includes two high-speed switching valves, a control unit and a pressure sensor, which are used to execute control instructions and monitor the pressure in the folding airbag.

[0090] Specifically, the present invention proposes an active anti-disturbance sliding mode composite controller ADRSMC, which normalizes the effects of "unmodeled dynamics" and "unknown external disturbances" into the total disturbance of the system, and combines them by an expanded state observer to provide real-time estimation and dynamic compensation. At the same time, a nonlinear sliding mode control rate is used to overcome the influence of factors such as strong nonlinearity of the folding airbag, inaccurate model and the existence of external load disturbance on the system.

[0091] Specifically, the wrist rehabilitation robot of the present invention adopts a folding airbag as a driving element. The robot body is mainly composed of a hand support, a base, an encoder and a forearm support. When the wrist rehabilitation robot is operating, the forearm of the arm is placed on the forearm support, the wrist is placed on the hand support, and the wrist is fixed with a bandage. The folding airbag is inflated and deflated to drive the wrist to rotate along the axis. The encoder records the rotation angle. The movable forearm support is designed to adapt to different forearm sizes.

[0092] The control system of the present invention mainly combines the self-disturbance rejection sliding mode composite controller and the adaptive combination valve to control the inflation and deflation flow of the folding airbag. The combination valve integrates two high-speed switch valves, a control unit and a pressure sensor. The pressure sensor monitors the internal pressure of the folding airbag. The predefined data and pressure data are communicated with the host computer through the control unit. The air inlet of the high-speed switch valve 1 (HSV1) is connected to the air compressor, and the exhaust port is connected to the airbag. When HSV1 is opened, compressed air enters the folding airbag, the volume of the folding airbag chamber increases, and the angle θ f By adjusting the duty cycle of the PWM wave input to HSV1, the gas flow entering the folded airbag can be adjusted, thereby achieving the rotation angle θ f The air inlet of HSV2 is connected to the airbag. When HSV is opened, the gas inside the folded airbag is discharged into the atmosphere, the volume of the folded airbag chamber becomes smaller, and the turning angle θ f Reduce, similarly, to achieve the rotation angle θ f Reduce acceleration control. Use low noise air compressor to provide power for wrist rehabilitation robot.

[0093] When the volume of the airbag increases, the rotation angle of the wrist joint increases; when the volume decreases, the rotation angle decreases, and the two are in a positive proportional relationship. Due to the irregular shape of the folded airbag itself and the compressibility of air, it is difficult to directly measure the volume of the airbag, and there is no relevant theory to establish its mathematical modeling. An indirect measurement scheme is proposed and a test platform is built to establish a mathematical model of the volume of the folded airbag chamber and the rotation angle of the wrist joint. The test platform mainly includes: an air tank, an airbag, a stop valve 1, a stop valve 2, a pressure sensor, an angle sensor, and its data acquisition module and a computer that provides a human-computer interaction interface. The test process is as follows: 1) Open stop valve 1, inflate the air tank to the pressure of P1, close stop valve 1, and collect the pressure of the air tank after the temperature stabilizes; 2) Open stop valve 2, and the gas of a certain pressure in the air tank enters the airbag. After the temperature stabilizes, record the readings of the pressure sensor and the angle sensor; 3) After the recording is completed, open the deflation valve to restore the initial posture of the wrist joint support; 4) Take different values ​​of the inflation pressure P1 in step 1, and repeat the process 1)-3).

[0094] According to the isothermal process: the Boyle-Martin law, when a certain mass of a gas remains at a constant temperature, the product of pressure and volume is a constant:

[0095] P1V1=P2V2 (1)

[0096] Where P1 is the pressure of the air tank, P2 is the pressure of the airbag, and V1 is the volume of the air tank and the connected air pipe. With the help of formula (1), V2 (the total volume of the airbag, air tank and the connected air pipe) can be calculated, and thus the airbag volume V can be calculated. q .

[0097] The above test was repeated several times, and the rotation angle of the wrist rehabilitation device collected and the airbag volume after calculation were fitted with a quadratic function to obtain the functional relationship:

[0098]

[0099] Among them, a1, a2, a3 are the coefficients of the fitting relationship:

[0100]

[0101] The curve diagram of the fitted airbag volume and wrist rotation angle is as follows: Figure 2 As shown, the fitting curve can well characterize V q With θ f The relationship between.

[0102] High-speed on-off valves are usually controlled by PWM waves. The duty cycle represents the proportion of the duration of the high level in a PWM wave cycle. Adjusting the duty cycle can adjust the opening time of the high-speed on-off valve, thereby adjusting the gas flow through the high-speed on-off valve. When the duration of the PWM wave high-level signal is less than the opening time of the high-speed on-off valve, the high-speed on-off valve switches to close before it is fully opened, and appears to be closed to the outside. The valve effective opening area coefficient f(u) = 0. When the input duty cycle is greater than the dead zone duty cycle u of the high-speed on-off valve s When u=1, the valve is in a fully open state, f(u)=1.

[0103] The control system of the present invention adopts two-way high-speed switch valves to control the inflation and deflation flow rates respectively. Figure 3 The first quadrant curve is the relationship curve between the control rate u (input duty cycle) and the effective opening area coefficient of the inflation valve, and the third quadrant curve is the relationship curve between the control rate u and the effective opening area coefficient f(u) of the deflation valve. The relationship between the control rate u and the effective opening area coefficient f(u) of the system inflation and deflation is as follows:

[0104]

[0105] The Sanville formula is used to approximate the gas mass flow rate through the high-speed switching valve port:

[0106] q m =Q m f(u) (4)

[0107] in:

[0108]

[0109] Where: A vis the maximum effective valve port area; K = 1.4 is the air specific heat ratio, R = 287 N·m / kg·k is the ideal gas constant, T u is the absolute temperature of the air source. When the folded airbag is inflated, p u is the upstream pressure of the high-speed switching valve port, that is, the pressure of the gas source, p d is the pressure downstream of the high-speed switch valve port, which is equal to the internal pressure of the folding airbag; when the folding airbag is deflated, p u is the internal pressure of the folded airbag, p d It is the downstream pressure of the high-speed valve port, which is equal to atmospheric pressure.

[0110] The inflation and deflation process of the folding airbag complies with the thermodynamic energy conservation equation, and its pressure change can be characterized as:

[0111]

[0112] Where P q is the internal pressure of the folded airbag, q m1 is the net mass flow rate in the airbag cavity, q m1 =q m2 -q m3 , where q m2 is the mass flow rate of inflating the folded airbag, q m3 The mass flow rate for deflation of the folded airbag.

[0113] Through relevant mechanical and physical theories, the dynamic model of the airbag-driven wrist joint device can be described as:

[0114]

[0115] Where τ is the driving torque of the wrist joint, S is the contact area between the folding airbag and the base plate, r is the distance from the center point of the folding airbag to the rotation axis of the wrist joint, J is the moment of inertia of the joint wheel, and b is v is the joint damping coefficient, θ f is the rotation angle of the wrist rehabilitation robot, m is the mass of the hand support board, g is the gravitational acceleration, F h It is the force applied to the wrist rehabilitation robot when the wrist interacts with the wrist rehabilitation robot.

[0116] Substituting equations (2), (4) and (5) into equation (6), the dynamic model of the folding airbag-driven wrist rehabilitation robot can be obtained as follows:

[0117]

[0118] The transformation of formula (7) is:

[0119]

[0120] Pick and:

[0121]

[0122] Then the state space equation of the control system is obtained as:

[0123]

[0124] Among them, is regarded as an unknown nonlinear term of the control system.

[0125] The structure of the active disturbance rejection sliding mode composite controller provided by the present invention is as follows: Figure 4 As shown, it includes a tracking differentiator, an extended state observer, a sliding mode controller, and a dead zone nonlinear inverse function control module.

[0126] The tracking differentiator TD is a nonlinear dynamic link for reasonably extracting the differential signal. The input signal θ d After the tracking differentiator, an input tracking signal θ can be obtained. r1 and a differential signal θ r2 The tracking differentiator is designed as follows:

[0127]

[0128] fhan(θ r1 -θ d ,θ r2 ,r0,h0) The specific expression is as follows:

[0129]

[0130] in:

[0131]

[0132] Where: r0 is the speed factor of the tracking differentiator. The larger r0 is, the faster the tracking speed is, which is determined by the needs of the transition process and the limit capacity of the system; h0 is the filter factor of the differential tracker. The value of h0 needs to comprehensively consider the filtering effect and the limit tracking speed of the system; θ d is the input target angle signal of the tracking differentiator; θ r1 is the input tracking signal of the tracking differentiator; θ r2 is θ r1 The differential signal.

[0133] The extended state observer (ESO) is used to estimate the state variables and unknown nonlinear terms. In formula (10), As the state variable of the expansion state machine, The derivative of Right now Then formula (10) is rewritten as:

[0134]

[0135] The three-order ESO design is as follows:

[0136]

[0137] Where ε1 is the angle observation value Output Angle The error, and They are and The estimated value of β 01 , β 02 and β 03 are the gain coefficients of the extended state observer respectively; fal(ε i ,a i ,δ) function is defined as follows:

[0138]

[0139] in, i=1,2,3.

[0140] Tracking the θ generated by the differentiator r1 ,θ r2 Signal and ESO generated estimates This results in errors e1 and e2.

[0141]

[0142] In order to achieve rapid convergence of the system state within a limited time, the terminal sliding mode function is selected as follows:

[0143]

[0144] η>0, and 0

[0145] Derivative of the sliding surface s:

[0146]

[0147] The present invention adds a system state variable x to represent the distance from the system state point to the sliding surface, and the proposed convergence rate is as follows:

[0148]

[0149] Where x represents the system state, which is used to represent the distance from the system state point to the sliding surface, and α is a variable coefficient.​

[0150] like Figure 5 As shown in Figure 1, when the system trajectory approaches the sliding mode surface, the variable gain term |x| / |x|+α of this approach rate gradually decreases and eventually converges to zero. In addition, λ1|x| / |x|+α is always less than λ1, which can further suppress the sliding mode chattering.

[0151] The traditional exponential reaching law uses the sign(s) function, which requires the controller to have an infinite switching frequency. However, it is impossible to achieve an infinite switching frequency in practice, so the actual sliding mode motion state cannot accurately reach the pre-designed sliding surface, and it travels back and forth on the sliding surface, thus generating jitter. The approach rate used in the present invention uses the sigmoid(s) function, and the parameter c determines the smooth continuity of the sigmoid(s) function. The sliding mode reaching law uses the smooth characteristics of the sigmoid(s) function to improve the system jitter caused by the high switching frequency of the traditional exponential reaching law.

[0152] The sliding mode control rate f can be obtained by combining equations (16) and (17): u for:

[0153]

[0154] In order to illustrate the stability of the designed sliding mode controller, the Lyapunov function is selected:

[0155]

[0156] Taking the derivative of formula (19), we can get:

[0157]

[0158] It can be seen that the system meets the reachability condition, the controller is stable, and the system can converge to the sliding surface (s=0) from any point outside the sliding surface (s≠0) within a finite time, that is, the system state tracking error can always converge to zero within a finite time.

[0159] Due to the inherent time characteristics of the opening / closing of high-speed switching valves, there are problems such as Figure 3 The typical dead zone nonlinearity shown in the figure, and the dead zone time consistency of different valves is good. The present invention sets a dead zone nonlinear inverse function control module, takes the effective opening area coefficient f(u) of the inflation and deflation valve output by the sliding mode controller as input, and uses f -1 (f(u)) is used as the input duty cycle command u for the high-speed on / off valve control of the gas charging and discharging to eliminate the dead zone nonlinearity of the high-speed on / off valve, such as Figure 6 As shown. The expression of control rate u is:

[0160]

[0161] When f(u)>0, the control rate u1=u is obtained, which is used as the input duty cycle of the high-speed switching valve to control the inflation flow of the folding airbag; when f(u)<0, the control rate u2=-u is obtained, which is used as the input duty cycle of the high-speed switching valve to control the deflation flow of the folding airbag.

[0162] Specifically, the control system of the present invention further includes:

[0163] Sine wave function generation module, used to generate target angle signal:

[0164] θ d =A sin(πft-0.5π)+B (22)

[0165] Among them, θ d is the target angle signal, A=25 is the amplitude of the sine wave, f=0.025Hz is the frequency of the sine wave, and B=35 is the offset of the sine wave angle.

[0166] The minimum acceleration model generation module is used to generate the optimal wrist motion curve. The minimum acceleration model satisfies the following mathematical rules:

[0167]

[0168] Where t0 is the start time of joint movement, θ0 is the initial position of joint movement, t1 is the stop time of joint movement, θ1 is the stop position of joint movement, is the joint angular acceleration, which is the third-order derivative of the joint angle with respect to time.

[0169] Combined with the minimum acceleration model, the optimal joint angle curve is obtained through variational conversion:

[0170]

[0171] Keep a certain delay time of 1 / 4T at the lowest position and the highest position respectively, and finally get the optimal wrist motion control curve law as follows:

[0172]

[0173] Among them, θ0 is the initial position of the joint movement, θ1 is the stop position of the joint movement, T is the period of the optimal control curve of the wrist movement, n is the number of times the wrist movement control curve is repeated, t is the time value, d is the interval time, t1 is the stop time of the joint movement, and t0 is the start time of the joint movement.

[0174] The operation process of the control system provided by the present invention is:

[0175] The S1 tracking differentiator receives the target angle signal and generates a smooth input tracking signal and its differential signal;

[0176] The S2 extended state observer estimates the system state and total disturbance based on the system input and output;

[0177] The S3 sliding mode controller designs nonlinear control rates based on state estimation and disturbance estimation;

[0178] The nonlinear control rate of the S4 dead zone nonlinear inverse function control module is compensated to eliminate the dead zone nonlinearity of the high-speed switching valve;

[0179] The S5 adaptive combination valve executes the compensated control command to adjust the gas flow in the folding airbag;

[0180] The S6 encoder records the rotation angle of the wrist joint in real time and feeds it back to the control system to form a closed-loop control.

[0181] Specifically, in order to verify the effect of the active disturbance rejection sliding mode composite control system proposed in the present invention, a comparative experiment was carried out on the active disturbance rejection sliding mode composite controller ADRSMC of the present invention and the traditional PID controller. A total of two groups of experiments were carried out to form two embodiments. In the two groups of experiments, the parameters of the ADRSMC and PID controller are shown in Tables 1 and 2.

[0182] Table 1ADRSMC parameters

[0183]

[0184]

[0185] Table 2 PID parameters

[0186] kp ki kd 0.1 0.5 0.001

[0187] Example 1

[0188] The first group of experiments is the sine wave trajectory tracking control experiment. The experimental results of ADRSMC and PID control are shown in Figure 7 As shown, Figure 7 (a) is the experimental curve of ADRSMC trajectory tracking control when there is no load. Figure 7 (b) is the error between ADRSMC and PID control when there is no load. Figure 7 (c) is the experimental curve of PID trajectory tracking control when there is no load. Figure 7 (d) is the experimental curve of ADRSMC trajectory tracking control when there is load force. Figure 7 (e) is the experimental curve ADRSMC of trajectory tracking control and PID control error. Figure 7 (f) in the figure is the experimental curve of PID trajectory tracking control when there is load force. The results of ESO estimation of angular velocity and disturbance in the experiment are shown in Figure 8As shown, Figure 8 (a) in the equation is the angular velocity estimation. Figure 8 (b) in the figure is the perturbation estimate.

[0189] Depend on Figure 7 and Figure 8 It can be seen that no matter there is no load or there is load, the actual output θf zero load and θf load can quickly and accurately reach the command input θd.

[0190] The output of the tracking differentiator θr1 accurately tracks the given θd, the rotation angle observation value It is almost consistent with θf zero load and θf load. Figure 8 In (a), it can be seen that the ESO in ADRSMC is sensitive to the angular velocity observation, whether it is without load or with load. The angular velocity θr2 output by the TD process can always be tracked. Figure 8 (b) shows the nonlinear uncertainty when there is no load and when there is a load force. The results show that the observed disturbance value It changes with the load, which is consistent with the actual situation.

[0191] In the initial stage of trajectory tracking, the ADRSMC tracking error is the largest (1.69°) without load and 1.03° with load; the corresponding errors of PID are 7.29° and 2.51° respectively. The maximum error of ADRSMC in both test scenarios is smaller than that of PID, because the former introduces a tracking differentiator TD to output a transition signal θr1, which better matches the limit capacity of the system in the initial stage.

[0192] Figure 7 Middle (a), Figure 7 (d) and Figure 7 Middle (c), Figure 7 By comparing (f) in the figure, it can be seen that when there is no load and with load, the response time of ADRSMC to reach a stable state is shorter than that of PID as a controller. After ADRSMC reaches a stable state, the root mean square error (RSME) and mean absolute error (MAE) are smaller than those of PID, as shown in Table 3.

[0193] Table 3 Sine wave trajectory tracking control performance table

[0194]

[0195] When PID is used as a controller, under no-load conditions, the output curve will experience high-frequency vibration when tracking the sine wave command (see Figure 7In (c), when there is load force, PID is used as the controller, and the high-frequency vibration is weakened (see Figure 7 (f)); when ADRSMC is used as the controller, no high-frequency vibration occurs when there is no load or load force; at the peak of the sine wave curve, the output signal can quickly track the given signal and can adapt well to the changes in control conditions (see Figure 7 (a) and Figure 7 (d)). When there is no load or load force, the output signal of PID has a certain delay compared with the given signal, and the controller parameters cannot adapt to the change of the target value (see Figure 7 (c) and Figure 7 (f)). The sinusoidal command tracking experiment shows that the control effect of ADRSMC is better than that of PID in terms of response time, stability and accuracy.

[0196] Example 2

[0197] The second group of experiments is the trajectory tracking control experiment of the optimal wrist motion curve. The experimental results are as follows Fig. 9 As shown, Fig. 9 (a) is the experimental curve of optimal trajectory tracking control of ADRSMC wrist when there is no load. Fig. 9 (b) is the error between ADRSMC and PID control when there is no load. Fig. 9 (c) is the experimental curve of the optimal trajectory tracking control of the PID wrist when there is no load. Fig. 9 (d) is the experimental curve of the optimal trajectory tracking control of the ADRSMC wrist when there is load force. Fig. 9 (e) is the experimental curve ADRSMC and PID control error of wrist optimal trajectory tracking control. Fig. 9 (f) in the figure is the experimental curve of the optimal trajectory tracking control of the PID wrist when there is a load force. The results of the ESO estimation of the angular velocity and disturbance in the experiment are shown in Fig.10 As shown, Fig.10 (a) in the equation is the angular velocity estimation. Fig.10 (b) in the figure is the perturbation estimate.

[0198] The root mean square error (RSME) and mean absolute error (MAE) of the ADRSMC and PID controllers under given signals are shown in Table 4.

[0199] Table 4 Wrist optimal curve trajectory tracking control performance

[0200]

[0201]

[0202] from Fig. 9 , Fig.10As can be seen from Table 4, the results of Example 2 are similar to those of Example 1. ADRSMC is superior to PID in terms of tracking time to reach a stable state, stability, and accuracy.

[0203] In summary, the present invention provides a wrist rehabilitation robot with a folding airbag as a driving element. Since a flexible material is used as a driver, the overall device is light in weight and small in size. When a person's wrist feels uncomfortable with the current bending angle, the airbag is easily compressed, allowing the person's wrist to maintain a certain position deviation from the instruction value, so that the subject can maintain a better sense of comfort when participating in rehabilitation training. The manufacturing cost of the pleated folding airbag is lower than that of pneumatic muscles and motors, and the price of the entire device is relatively low.

[0204] Since the flexible material of the folded airbag is used as the driver, its easy compressibility and the influence of the nonlinearity of the control element make the control difficult. The embodiment provided by the present invention shows that ADRSMC can overcome the influence of nonlinearity, and the overall control accuracy is within ±1°, achieving a good control effect. At the peak of the sine wave, when the subject participates in rehabilitation training, the ADRSMC tracking trajectory fluctuates to a certain extent. Here, when the human wrist joint moves to the extreme angle, a nonlinear resistance interference will be generated, which affects the tracking accuracy of the rehabilitation device. ADRSMC estimates the magnitude of the interference force in real time through ESO and compensates it in the SMC controller, thereby improving the control accuracy of the peak position. In the process of wrist optimal curve trajectory tracking, when ADRSMC is used as a controller without load test, when the instruction angle is less than 10°, the tracking accuracy with load is significantly improved compared with that without load. The main reason is that the internal pressure of the airbag is small when there is no load, and the deflation rate is lower than the speed required by the instruction; when the subject participates in rehabilitation training, the airbag will be compressed to form pressure, which will accelerate the deflation rate, thereby improving the control accuracy in this process. In subsequent work, the deflation valve can be changed to a negative pressure pump to increase the deflation rate of the system, so as to obtain better angle control accuracy.

[0205] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. An anti-disturbance sliding mode composite control system for a wrist rehabilitation robot, characterized in that: The wrist rehabilitation robot uses a folded airbag as a driving element, and the control system includes: A tracking differentiator, used for tracking and differentiating an input target angle signal; Extended state observer for real-time estimation of system state and total disturbance; Sliding mode controller for designing nonlinear control rates; Dead zone nonlinear inverse function control module, used to compensate for the nonlinear control rate of the sliding mode controller and eliminate the dead zone nonlinearity of the high-speed switch valve; The adaptive combination valve includes two high-speed switching valves, a control unit and a pressure sensor, which are used to execute control instructions and monitor the pressure in the folding airbag.

2. The anti-disturbance sliding mode composite control system of a wrist rehabilitation robot according to claim 1, characterized in that: The mathematical model of the high-speed switching valve is: Where f(u) is the effective opening area coefficient of the valve, u is the input duty cycle, and u s is the dead-band duty cycle.

3. The anti-disturbance sliding mode composite control system of a wrist rehabilitation robot according to claim 1, characterized in that: The energy conservation equation of the folded airbag is: Where P q is the internal pressure of the folded airbag, q m1 is the net mass flow rate in the airbag cavity, q m1 =q m2 -q m3 , where q m2 is the mass flow rate of inflating the folded airbag, q m3 is the mass flow rate of the folded airbag deflation, K is the air specific heat ratio, R is the ideal gas constant, V q is the volume of the folded airbag, T u For temperature.

4. The anti-disturbance sliding mode composite control system of a wrist rehabilitation robot according to claim 1, characterized in that: The dynamic model of the folding airbag-driven wrist rehabilitation robot is: In the formula, θ f is the rotation angle of the wrist rehabilitation robot, r is the distance from the center of the folded airbag to the wrist joint rotation axis, S is the contact area between the folded airbag and the base plate, K is the air specific heat ratio, R is the ideal gas constant, T is the temperature, q m is the mass flow rate of the gas in the folding airbag, J is the moment of inertia of the joint rotor, f(u) is the effective opening area coefficient of the valve, b v is the joint damping coefficient, m is the mass of the hand support board, g is the acceleration of gravity, F h is the force exerted on the wrist rehabilitation robot when the wrist interacts with the wrist rehabilitation robot, b0 is a constant term, a1, a2, a3 are fitting relationship coefficients, which are used to fit the volume V of the folded airbag q The rotation angle θ of the wrist rehabilitation robot f Functional relationship: make and: Then the state space equation of the control system is obtained as: Among them, is regarded as an unknown nonlinear term of the control system.

5. The anti-disturbance sliding mode composite control system of a wrist rehabilitation robot as claimed in claim 4, characterized in that: The tracking differentiator is designed as follows: fhan(θ r1 -θ d ,θ r2 ,r0,h0) The specific expression is as follows: in: Where: r0 is the speed factor of the tracking differentiator. The larger r0 is, the faster the tracking speed is. h0 is the filter factor of the differential tracker. θ d is the input target angle signal of the tracking differentiator; θ r1 is the input tracking signal of the tracking differentiator; θ r2 is θ r1 The differential signal.

6. The anti-disturbance sliding mode composite control system of a wrist rehabilitation robot as claimed in claim 5, characterized in that: The unknown nonlinear terms of the control system As the state variable of the extended state observer, let The derivative of Right now The third-order extended state observer is designed as follows: Where ε1 is the angle observation value Output Angle The error, and They are and The estimated value of β 01 , β 02 and β 03 are the gain coefficients of the extended state observer respectively; fal(ε i ,a i ,δ) function is defined as follows: in, i=1,2,3.

7. The anti-disturbance sliding mode composite control system of a wrist rehabilitation robot as claimed in claim 6, characterized in that: The nonlinear control rate of the sliding mode controller is as follows: where f u is the sliding mode control law; s is the sliding mode surface; e1 and e2 are the errors between the θ r1 , θ r2 signals generated by the tracking differentiator and the estimated values generated by the extended state observer; η > 0, and 0 < q / p < 1, where p and q are both positive numbers; λ1 and λ2 are the coefficients of the reaching law, and the reaching law is as follows: Where x represents the system state, which is used to represent the distance from the system state point to the sliding surface, and α is a variable coefficient. is the variable gain term of the approach rate, and c is the control parameter used to control the smooth continuity of sigmoid(s).

8. The anti-disturbance sliding mode composite control system of a wrist rehabilitation robot as claimed in claim 2, characterized in that: The dead zone nonlinear inverse function control module takes the effective opening area coefficient f(u) of the inflation and deflation valve output by the sliding mode controller as input, and -1 (f(u)) is used as the input duty cycle command u of the high-speed on-off valve for charging and discharging to eliminate the dead zone nonlinearity of the high-speed on-off valve. The expression of the control rate u is: When f(u)>0, the control rate u1=u is obtained, which is used as the input duty cycle of the high-speed switching valve to control the inflation flow of the folding airbag; when f(u)<0, the control rate u2=-u is obtained, which is used as the input duty cycle of the high-speed switching valve to control the deflation flow of the folding airbag.

9. The anti-disturbance sliding mode composite control system of a wrist rehabilitation robot as claimed in claim 1, characterized in that: The control system further comprises: Sine wave function generation module, used to generate target angle signal: i d =A sin(πft-0.5π)+B Among them, θ d is the target angle signal, A=25 is the amplitude of the sine wave, f=0.025Hz is the frequency of the sine wave, and B=35 is the offset of the sine wave angle; Minimum acceleration model generation module, used to generate the optimal wrist motion curve: θ0=10,θ1=10when t∈[nT,1 / 4T+nT) θ0=10,θ1=50when t∈[1 / 4T+T,1 / 2T+nT) θ0=50,θ1=50when t∈[1 / 2T+nT,3 / 4T+nT) θ0=50,θ1=10when t∈[3 / 4T+nT,T+nT) Among them, θ0 is the initial position of the joint movement, θ1 is the stop position of the joint movement, T is the period of the optimal control curve of the wrist movement, n is the number of times the wrist movement control curve is repeated, t is the time value, d is the interval time, t1 is the stop time of the joint movement, and t0 is the start time of the joint movement.

10. The anti-disturbance sliding mode composite control system of a wrist rehabilitation robot according to claim 1, characterized in that: The operation process of the control system is as follows: The S1 tracking differentiator receives the target angle signal and generates a smooth input tracking signal and its differential signal; The S2 extended state observer estimates the system state and total disturbance based on the system input and output; The S3 sliding mode controller designs nonlinear control rates based on state estimation and disturbance estimation; The nonlinear control rate of the S4 dead zone nonlinear inverse function control module is compensated to eliminate the dead zone nonlinearity of the high-speed switching valve; The S5 adaptive combination valve executes the compensated control command to adjust the gas flow in the folding airbag; The S6 encoder records the rotation angle of the wrist joint in real time and feeds it back to the control system to form a closed-loop control.

Images