A sliding mode control method for a force feedback manipulator

Abstract

This invention discloses a sliding mode control method for a force feedback manipulator, comprising the following steps: Step 1, establishing a kinematic model and a dynamic model; Step 2, determining the state-space expression of the force feedback system; Step 3, designing the sliding surface; Step 4, performing stability analysis of the force feedback system; Step 5, determining the conditions that the sliding mode control law should satisfy; Step 6, controlling the force feedback manipulator. Compared with existing force feedback manipulator control methods, this invention adopts a sliding mode control method, which can dynamically change according to the current state of the system to achieve the system moving according to a preset sliding mode trajectory. It has good adaptability and robustness, can reduce the interference of disturbance factors on the dynamic model, and achieve precise control. Furthermore, the sliding mode control method only requires adjusting two parameters, making adjustment more convenient and simple, and improving the design efficiency of the control method.

Description

Technical Field

[0001] This invention relates to a sliding mode control method for a force feedback robot. Background Technology

[0002] Force feedback gloves are a type of robotic hand that most closely resembles the human hand; they are widely used in many fields, and have shown excellent performance in the rehabilitation of patients with hand dysfunction caused by chronic diseases, disabilities, and movement disorders.

[0003] However, the existing control methods for force feedback manipulators have the following drawbacks: First, disturbance factors significantly interfere with the system dynamics model, resulting in low control accuracy; second, there are many control parameters, leading to low design efficiency of the control method. Summary of the Invention

[0004] The purpose of this invention is to provide a sliding mode control method for a force feedback robot to solve the problems mentioned in the background art.

[0005] To achieve the above objectives, the present invention provides the following technical solution: a sliding mode control method for a force feedback manipulator, comprising the following steps: Step 1, establishing a kinematic model and a dynamic model; Step 2, determining the state-space expression of the force feedback system; Step 3, designing the sliding surface; Step 4, performing stability analysis of the force feedback system; Step 5, determining the conditions that the sliding mode control law should satisfy; Step 6, controlling the force feedback manipulator;

[0006] In step one above, the slider on the guide rail in the force feedback manipulator platform is taken as the controlled object, and the kinematic model of the single finger drive part of the force feedback manipulator and the dynamic model of the force feedback manipulator are built.

[0007] In step two above, the state-space expression of the force feedback manipulator is determined;

[0008] In step three above, a force feedback sliding mode control law is designed.

[0009] In step four above, the stability of the force feedback system is analyzed and the controller parameters are solved.

[0010] In step five above, the conditions that the sliding mode control law should satisfy are determined so that the system satisfies the reachability of the sliding state;

[0011] In step six above, the calculated control quantity is applied to the force feedback manipulator system to achieve precise control.

[0012] Preferably, in step one, the theoretical model of the spring tension in the kinematic model of the single-finger drive part of the force feedback manipulator satisfies:

[0013] f(t) = kx

[0014] In the formula, k is the spring constant (N / mm), and x is the slider displacement (mm);

[0015] We can obtain:

[0016]

[0017] Since the motor is driven by the track, the displacement of the motor (mm) can be obtained:

[0018]

[0019] In the formula, v is the linear velocity of the motor (mm / s);

[0020] We can obtain:

[0021]

[0022] Differentiating both sides of the equation, we get:

[0023]

[0024] If tracking a sinusoidal signal:

[0025] f(t) = sint

[0026] Therefore, the speed control quantity for the DC motor can be obtained:

[0027]

[0028] The dynamic model of the force feedback manipulator is described as follows:

[0029]

[0030] Preferably, in step two, the specific steps are: taking the state variable x1 = x, Therefore, the state equation of the force feedback system is:

[0031]

[0032] Therefore, the state-space expression of the force feedback system is:

[0033]

[0034]

[0035] In the formula, x1 is the displacement of the slider (mm), x2 is the sliding speed of the slider (mm / s), c is the spring damping coefficient (N*s / mm), k is the spring constant (N / mm), m is the mass including the slider (kg), T is the motor output torque (N / mm), n is the motor reduction ratio, R is the radius of the synchronous pulley (mm), and c0 is the slider friction coefficient (N / kg). This is the error that exists between the simulated real force feedback system and the force feedback model, and this error will also be considered as part of the total disturbance.

[0036] Preferably, in step three, specifically: the sliding mode function in the force feedback sliding mode control algorithm is designed as follows:

[0037]

[0038] In the formula, e is the tracking error between the preset position and the actual position;

[0039] Define Lyapunov functions as follows:

[0040]

[0041] and

[0042]

[0043] So

[0044]

[0045] To ensure the stability of the force feedback system, it is necessary to ensure...

[0046]

[0047] Therefore, the sliding mode control law can be designed as follows:

[0048]

[0049] Preferably, in step four, specifically: in designing the sliding mode control law, sgn(s) is a sign function, combined with the upper limit of disturbance D of the force feedback system. Then, when s takes both positive and negative values, u satisfies the inequality constraint requirements; and when... When t→∞, s≡0. According to the LaSalle invariance principle, the closed-loop system controlled by the designed sliding mode control law is asymptotically stable. When t→∞, s→0, and the convergence speed of s depends on θ, that is, adjusting the magnitude of θ can adjust the control speed.

[0050] Preferably, in step five, specifically: when s = 0, The convergence result is e = e(0)exp(-αt), therefore, as t→∞, the system tracking error exponential converges to e→0, and x(t)→x is automatically realized. d (t), The rate at which the system error converges to zero depends on α; therefore, in an ideal, finite time, once the force feedback system is controlled to... After this plane, its state variables On the phase plane, it will automatically "slide" back to the origin without requiring additional control.

[0051] Compared with the prior art, the beneficial effects of the present invention are as follows: Compared with the existing force feedback manipulator control method, the present invention adopts the sliding mode control method, which can dynamically change according to the current state of the system to realize the system moving according to the preset sliding mode trajectory. It has good adaptability and robustness, can reduce the interference of disturbance factors on the dynamic model, and achieve the effect of precise control. Moreover, the sliding mode control method only requires the adjustment of two parameters, which is more convenient and simple to adjust, and can improve the design efficiency of the control method. Attached Figure Description

[0052] Figure 1 This is a flowchart of the method of the present invention;

[0053] Figure 2 This is a dynamic model diagram of a force feedback manipulator control system.

[0054] Figure 3 Wiring diagram for simulation experiment of sliding mode variable structure control algorithm of force feedback system;

[0055] Figure 4 The effect diagram of adjusting α control in the sliding mode control method in the simulation experiment;

[0056] Figure 5 The diagram shows the effect of adjusting θ control in the sliding mode control method during the simulation experiment.

[0057] Figure 6 This is an experimental platform for force feedback robotic arms. Detailed Implementation

[0058] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0059] Please see Figure 1-6The present invention provides an embodiment of a sliding mode control method for a force feedback manipulator, comprising the following steps: Step 1, establishing a kinematic model and a dynamic model; Step 2, determining the state-space expression of the force feedback system; Step 3, designing the sliding surface; Step 4, performing stability analysis of the force feedback system; Step 5, determining the conditions that the sliding mode control law should satisfy; Step 6, controlling the force feedback manipulator.

[0060] In step one above, the slider on the guide rail of the force feedback manipulator platform is taken as the controlled object, and the kinematic model of the single-finger drive part and the dynamic model of the force feedback manipulator are built; the theoretical model of the spring tension satisfies:

[0061] f(t) = kx

[0062] In the formula, k is the spring constant (N / mm), and x is the slider displacement (mm);

[0063] We can obtain:

[0064]

[0065] Since the motor is driven by the track, the displacement of the motor (mm) can be obtained:

[0066]

[0067] In the formula, v is the linear velocity of the motor (mm / s);

[0068] We can obtain:

[0069]

[0070] Differentiating both sides of the equation, we get:

[0071]

[0072] If tracking a sinusoidal signal:

[0073] f(t) = sint

[0074] Therefore, the speed control quantity for the DC motor can be obtained:

[0075]

[0076] The dynamic model of the force feedback manipulator is described as follows:

[0077]

[0078] In step two above, the state-space expression of the force feedback manipulator is determined; specifically, the state variable x1 = x is taken. Therefore, the state equation of the force feedback system is:

[0079]

[0080] Therefore, the state-space expression of the force feedback system is:

[0081]

[0082] In the formula, x1 is the displacement of the slider (mm), x2 is the sliding speed of the slider (mm / s), c is the spring damping coefficient (N*s / mm), k is the spring constant (N / mm), m is the mass including the slider (kg), T is the motor output torque (N / mm), n is the motor reduction ratio, R is the radius of the synchronous pulley (mm), and c0 is the slider friction coefficient (N / kg). This is the error that exists between the simulated real force feedback system and the force feedback model, and this error will also be considered as part of the total disturbance;

[0083] In step three above, the force feedback sliding mode control law is designed; specifically, the sliding mode function in the force feedback sliding mode control algorithm is designed as follows:

[0084]

[0085] In the formula, e is the tracking error between the preset position and the actual position;

[0086] Define Lyapunov functions as follows:

[0087]

[0088] and

[0089]

[0090] So

[0091]

[0092] To ensure the stability of the force feedback system, it is necessary to ensure...

[0093]

[0094] Therefore, the sliding mode control law can be designed as follows:

[0095]

[0096] In step four above, the stability of the force feedback system is analyzed, and the controller parameters are solved. Specifically, in the design of the sliding mode control law, sgn(s) is the sign function. Combined with the upper limit of disturbance D of the force feedback system, u satisfies the inequality constraint requirements regardless of whether s is positive or negative. When t→∞, s≡0. According to the LaSalle invariance principle, the closed-loop system controlled by the designed sliding mode control law is asymptotically stable. When t→∞, s→0, and the convergence speed of s depends on θ, that is, adjusting the size of θ can adjust the control speed.

[0097] In step five above, the conditions that the sliding mode control law should satisfy are determined so that the system satisfies the reachability of the sliding state; specifically, when s = 0, The convergence result is e = e(0)exp(-αt), therefore, as t→∞, the system tracking error exponential converges to e→0, and x(t)→x is automatically realized. d (t), The rate at which the system error converges to zero depends on α; therefore, in an ideal, finite time, once the force feedback system is controlled to... After this plane, its state variables On the phase plane, it will automatically "slide" back to the origin without requiring additional control;

[0098] In step six above, the calculated control quantity is applied to the force feedback manipulator system to achieve precise control.

[0099] The force feedback manipulator is controlled using the method proposed in the above embodiments. A slider on the guide rail of the force feedback manipulator platform is taken as the controlled object. The actual position information of the slider is obtained through a displacement sensor and used as the input to the system feedback loop. The controller sets the preset position information of the slider, calculates the deviation value and the rate of change of the deviation value between the preset position and the actual position, and uses this as the system input. The designed sliding mode control algorithm calculates the control output quantity, which is then input into the force feedback control system to control the movement of the slider on the guide rail. The control flowchart of the sliding mode control algorithm is shown below. Figure 1 As shown; the specific steps for calculating the control output using the sliding mode control algorithm are as follows:

[0100] 1) The control model of the slider is established using state-space representation, as shown below:

[0101]

[0102] In the formula, x1 is the displacement of the slider (mm), x2 is the sliding speed of the slider (mm / s), c is the spring damping coefficient (N*s / mm), k is the spring elastic coefficient (N / mm), m is the mass including the slider (kg), T is the motor output torque (N / mm), n is the motor reduction ratio, R is the radius of the synchronous pulley (mm), and c0 is the slider friction resistance coefficient (N / kg), which is generally between 0.01 and 0.05. This is the error that exists between the simulated real force feedback system and the force feedback model, and this error will also be considered as part of the total disturbance;

[0103] To simplify the dynamic model without losing generality, the actuator of the controller adopts a motor-spring damping model; the parameters of the simulation model of the force feedback manipulator control system are shown in Table 1.

[0104] The values ​​of parameters c and c0 in Table 1 are rough estimates based on the equipment model; substituting the parameters of the simulation model of the force feedback manipulator control system into the state-space expression, we can obtain:

[0105]

[0106]

[0107] 2) Based on the control model in step 1, the sliding surface is designed as follows:

[0108]

[0109] Among them, parameter c must satisfy the Hurwitz condition, that is, parameter c > 0;

[0110] In order to achieve the control objective:

[0111] x(t)→x d (t),

[0112] Therefore, the tracking error and its derivative are:

[0113] e(t) = x(t) - x d (t),

[0114] Where, x d (t) represents the ideal displacement signal of the force feedback system;

[0115] Define Lyapunov functions as follows:

[0116]

[0117] and

[0118]

[0119] So

[0120]

[0121] To ensure the stability of the force feedback system, it is necessary to ensure...

[0122]

[0123] The sliding mode control law can then be designed as follows:

[0124]

[0125] In the formula, e is the tracking error between the preset position and the actual position;

[0126] 3) To verify the force feedback control effect of the sliding mode controller, a sliding mode control law was designed and a system was built. Figure 3 The simulation diagram shown is obtained through simulation. Figure 4 , Figure 5 The simulation results are shown. Comparing the simulation results with the experimental results, it can be found that for this step signal, the convergence speed of the SMC control algorithm is related to the α parameter, and the larger the α parameter, the faster the SMC converges to a steady state. However, when α is greater than a certain threshold, further increases in α do not significantly increase the convergence speed of the control system. The convergence speed of the SMC control algorithm also depends on the θ parameter, and the larger the θ parameter, the faster the SMC converges to a steady state. However, when θ is greater than a certain threshold, further increases in θ do not significantly increase the convergence speed. Therefore, the simulation results correctly verify the results of the SMC control algorithm formula derivation.

[0127] 4) To verify the effectiveness of the sliding mode control algorithm in the experiment, an experimental platform for the force feedback manipulator was built based on the selection of experimental equipment and relevant simulation models. Figure 6 As shown, force feedback passive motion rehabilitation experiments and force feedback active motion rehabilitation experiments were completed based on this experimental platform.

[0128] 5) The simulation results show that in a strong disturbance environment, the sliding mode control algorithm enables the controller to reach a stable value faster under the premise of anti-disturbance, and the step tracking curve is smoother.

[0129] Table 1. Parameters of the simulation model of the force feedback manipulator control system

[0130]

[0131]

[0132] Based on the above, the advantages of this invention are that the sliding mode control method adopted in this invention has strong adaptability and robustness, and can perform force feedback control without relying on a specific system model. It can predict, estimate and suppress environmental disturbances of the force feedback manipulator system in real time, reduce the impact of disturbances on the system, and achieve precise control. Furthermore, the sliding mode control method can make the control system stable by designing the sliding surface, and is not prone to overshoot. At the same time, the sliding mode control method only requires adjusting two parameters, which is convenient and simple to adjust, and has the advantage of convenient tuning. This invention can precisely control the force feedback manipulator and solve the problems of poor coordination during movement and anti-interference control of the system during rehabilitation for patients with hand dysfunction.

[0133] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the invention can be implemented in other specific forms without departing from its spirit or essential characteristics. Therefore, the embodiments should be considered in all respects as exemplary and non-limiting, and the scope of the invention is defined by the appended claims rather than the foregoing description. Thus, all variations falling within the meaning and scope of equivalents of the claims are intended to be included within the present invention. No reference numerals in the claims should be construed as limiting the scope of the claims.

Claims

1. A sliding mode control method for a force feedback robot, comprising the following steps: Step 1: Establish kinematic and dynamic models; Step 2: Determine the state-space expression of the force feedback system; Step 3: Design the sliding surface; Step 4: Perform stability analysis of the force feedback system; Step 5: Determine the conditions that the sliding control law should satisfy; Step 6: Control the force feedback manipulator; Its characteristics are: In step one above, the slider on the guide rail in the force feedback manipulator platform is taken as the controlled object, and the kinematic model of the single finger drive part of the force feedback manipulator and the dynamic model of the force feedback manipulator are built. The dynamic model of the force feedback manipulator is described as follows: ； in, The spring constant is... For slider displacement, This is the spring damping coefficient. For the mass including the slider, For the motor output torque, This is the motor reduction ratio. The radius of the timing pulley, The coefficient of friction of the slider. It represents the error that exists between simulating a real force feedback system and a force feedback model; In step two above, the state-space expression of the force feedback manipulator is determined; In step three above, the force feedback sliding mode control law is designed; specifically, the sliding mode function in the force feedback sliding mode control algorithm is designed as follows: ； In the formula, This represents the tracking error between the preset position and the actual position. Define Lyapunov functions as follows: ； and ； So ； To ensure the stability of the force feedback system, it is necessary to ensure ， Therefore, the sliding mode control law is designed as follows: ;in, This represents the upper limit of interference for a force feedback system. In step four above, the stability of the force feedback system is analyzed and the controller parameters are solved. In step five above, the conditions that the sliding mode control law should satisfy are determined so that the system satisfies the reachability of the sliding state; In step six above, the calculated control quantity is applied to the force feedback manipulator system to achieve precise control.

2. The sliding mode control method for a force feedback robot according to claim 1, characterized in that: In step one, the theoretical model of the spring tension in the kinematic model of the single-finger drive part of the force feedback manipulator satisfies: ， In the formula The spring constant is... This represents the slider displacement; We can obtain: ， Furthermore, because the motor is driven by the track, the displacement of the motor is obtained: ， In the formula The linear velocity of the motor rotation; We can obtain: ， Differentiating both sides of the equation, we get: ， If tracking a sinusoidal signal: ， Therefore, the speed control quantity for the DC motor is obtained as follows: ， This leads to the dynamic model of the force feedback manipulator.

3. The sliding mode control method for a force feedback robot according to claim 1, characterized in that: In step four, specifically, the design of the sliding mode control law involves... The sign function is related to the upper limit of disturbance in the force feedback system. Combination, then when Taking both positive and negative values, All satisfy the inequality constraints; and when hour, According to the LaSalle invariance principle, the closed-loop system controlled by the designed sliding mode control law is asymptotically stable when... hour, ,and Convergence speed depends on That is, regulation The size and speed can be adjusted and controlled.

4. The sliding mode control method for a force feedback robot according to claim 1, characterized in that: In step five, specifically: when hour, The convergence result is ,therefore At that time, the system tracking error converges exponentially. It can be done automatically. The rate at which the systematic error converges to zero depends on .

Images