Non-singular fast terminal sliding mode-based fixed time impedance control method for linear drive continuous mechanical arm

Abstract

The invention discloses a linear drive continuous mechanical arm fixed time impedance control method based on a non-singular fast terminal sliding mode, and the method comprises the steps: firstly, building a dynamic model of a linear drive continuous mechanical arm; secondly, designing an impedance model based on expected inertia, damping and rigidity, and representing an impedance error; and finally, a linear drive continuous type mechanical arm fixed time impedance control system is designed, the control system comprises a fixed time disturbance observer for estimating external unknown disturbance and unmodeled dynamics and a singular avoiding piecewise function for eliminating control law singularity, and an impedance control law is designed through a non-singular fast terminal sliding mode method. The linear drive continuous mechanical arm can achieve stable transition between free motion and a contact environment, and has high-precision and high-robustness compliant control capacity.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of compliant control of serial manipulators, and particularly relates to a fixed-time impedance control method for wire-driven serial manipulators based on a non-singular fast terminal sliding mode. BACKGROUND

[0002] Wire-driven serial manipulators are widely used in space in-orbit maintenance, narrow slit detection, minimally invasive surgery, rescue exploration and other tasks due to their lightweight structure, high compliance and flexible operation in narrow and unstructured space. Compared with traditional joint manipulators, wire-driven serial manipulators realize flexible motion through flexible driving force transmission and segmented continuous bending, and can perform complex operations in limited space, which is an important research direction in the field of robot technology at home and abroad.

[0003] However, when wire-driven serial manipulators perform tasks, especially in the process of interacting with the environment, the control of the contact force between the end effector and the outside world is greatly increased. Excessive contact force can damage the manipulator system or the work object, and in severe cases, it will lead to task failure; too small contact force cannot guarantee stable contact, resulting in unstable operation or failure to achieve the expected operation accuracy. In tasks that require precise control of the operating force (such as spacecraft in-orbit assembly, medical surgery, and precision detection), relying solely on position control often cannot meet the requirements.

[0004] Existing research shows that impedance control is an effective method to realize the compliant interaction of serial manipulators with the environment, and impedance control combined with a dynamics model can improve the response speed, accuracy and stability of the system. However, the traditional impedance control has the following shortcomings: first, the fixed-time convergence characteristics of the controller are not considered, making it difficult to guarantee fast and stable operation under task time constraints; second, when there are complex external disturbances, drive rope friction or model unmodeled dynamics, the control performance will decrease significantly; finally, the singularity problem of the control law may occur in the terminal sliding mode control method, affecting the stability and realizability of the system.

[0005] Therefore, how to realize the fixed-time impedance control of wire-driven serial manipulators under the condition of external disturbances and model uncertainties, eliminate the singularity of the control law, and improve the compliance and contact force control accuracy is a technical problem that needs to be solved at present. SUMMARY

[0006] In view of the above problems, the purpose of the present application is to provide a non-singular fast terminal sliding mode based on line drive continuous type robot arm fixed time impedance control method, to provide a convergence time predictable, small steady state error, strong anti-disturbance ability of non-singular fast terminal sliding mode based on line drive continuous type robot arm fixed time impedance control method. The method can realize smooth switching between free motion and contact environment, and ensure that the robot arm has high precision and strong robustness in different operation modes.

[0007] The specific technical scheme for achieving the purpose of the present application is:

[0008] A non-singular fast terminal sliding mode based on line drive continuous type robot arm fixed time impedance control method, comprising the following steps:

[0009] Step 1, according to the segmented constant curvature assumption, the dynamics model of the line drive continuous type robot arm is established, and the state space equation is derived, and the lumped uncertainty term and external disturbance existing in the model are defined;

[0010] Step 2, based on the expected inertia matrix, damping matrix and stiffness matrix, the expected impedance model in the joint space of the continuous type robot arm is established, and the impedance error is represented;

[0011] Step 3, design a fixed time disturbance observer to observe the external unknown disturbance of the line drive continuous type robot arm and the unmodeled dynamics of the system;

[0012] Step 4, design a singularity avoidance segmented function to eliminate the singularity problem generated in the derivation process of the virtual control law in the non-singular fast terminal sliding mode control;

[0013] Step 5, construct a non-singular fast terminal sliding surface, combine the disturbance estimation value in step 3 and the singularity avoidance segmented function in step 4, and design a sliding mode control law τ;

[0014] Step 6, the state space equation in step 1, the impedance model in step 2 and the control law in step 5 are used to generate the control input in the drive space, and the drive rope tension is calculated according to the drive rope constraint, so as to realize the contact force compliant control of the line drive continuous type robot arm.

[0015] Compared with the prior art, the present application has the following advantages:

[0016] (1) The fixed time impedance control system proposed in the present application combines the non-singular fast terminal sliding mode method, which can avoid the singularity problem in sliding mode control and ensure that the impedance error converges in fixed time, thereby improving the response speed and convergence performance of the control system;

[0017] (2) This scheme introduces a fixed-time disturbance observer, which effectively estimates and compensates for external disturbances and unmodeled dynamics, significantly enhancing the robustness of the control system, while achieving smooth switching between free motion and contact environment without mode switching control, avoiding system instability;

[0018] (3) The control framework based on the desired impedance model in this scheme can maintain high compliance and high precision contact force control when the end effector interacts with the environment, greatly improving the safety and control accuracy of the operation.

[0019] The application will be further described below with reference to the specific embodiments. BRIEF DESCRIPTION OF DRAWINGS

[0020] Figure 1 It is a fixed-time impedance control architecture for a non-singular fast terminal sliding mode based linear drive continuous robot arm. DETAILED DESCRIPTION

[0021] EMBODIMENTS

[0022] The technical solutions in the embodiments of the application will be clearly and completely described below with reference to the drawings in the embodiments of the application. The described embodiments are only some of the embodiments of the application, rather than all the embodiments. Based on the embodiments in the application, all other embodiments obtained by those skilled in the art without creative work fall within the scope of protection of the application.

[0023] As shown in the present application and claims, unless the context clearly indicates otherwise, the words "one", "an", "a", and / or "the" do not mean to specify a single number, but can also include a plurality. Generally, the terms "comprising" and "including" only indicate the inclusion of the steps and elements explicitly identified, and these steps and elements do not constitute an exclusive list, and the method or device can also include other steps or elements.

[0024] Unless otherwise specifically stated, the relative arrangement of parts and steps, numerical expressions, and numerical values set forth in these embodiments do not limit the scope of the application. At the same time, it should be understood that the sizes of the various parts shown in the drawings are not drawn in proportion to the actual proportions. The technology, methods and devices known to those skilled in the relevant art can not be discussed in detail, but under appropriate circumstances, the technology, methods and devices should be considered as part of the authorized specification. In all examples shown and discussed here, any specific value should be interpreted as merely exemplary, and not as a limitation. Therefore, other examples of exemplary embodiments can have different values. It should be noted that similar reference numbers and letters represent similar items in the following drawings, so once an item is defined in one drawing, it does not need to be further discussed in subsequent drawings.

[0025] In combination Figure 1 , a non-singular fast terminal sliding mode based impedance control method for cable-driven continuum manipulators, comprising the steps of:

[0026] Step 1, establish the dynamics model of the cable-driven continuum manipulator according to the piecewise constant curvature assumption, and derive its state space equation, and define the lumped uncertainty and external disturbance existing in the model:

[0027]

[0028] where, is the inertia matrix of the continuum manipulator, is the joint angle variable, θ i and is the bending angle and rotation angle of the i-th section, is the Coriolis force and centripetal force matrix, G(q) is the potential energy term, τ d represents the uncertain external disturbance, is the control input, J Lq is the velocity Jacobian matrix of the continuum manipulator from the driving space to the joint space, is the contact torque equivalent to the joint space, J Xq is the Jacobian matrix from the task space to the joint space, f e = [f ex f ey f ez ] T is the contact force between the manipulator end effector and the environment;

[0029] Since simplifying assumptions (such as piecewise constant curvature assumption) are made when modeling the dynamics of the continuum manipulator, unmodeled dynamics exist in the model, which can be represented as:

[0030]

[0031] where, M0(q)、 and G0(q) are the nominal values of the inertia matrix, Coriolis force and centripetal force matrix, and potential energy term of the cable-driven continuum manipulator, respectively. Δ (q)、 and G Δ (q) represent the uncertainty caused by unmodeled dynamics, respectively.

[0032] Therefore, the system model can be represented as:

[0033]

[0034] where, u d is the lumped equivalent uncertainty of the system, represented as

[0035]

[0036] Let x1 = q, The state-space equation of the linearly driven continuous robotic arm is:

[0037]

[0038] The following assumptions are made:

[0039] 1) Generalized angular position q and angular velocity of a linearly driven continuous robotic arm system Both can be measured.

[0040] 2) External disturbance τ d The derivative of the system and the system itself are bounded, and the model uncertainty of the system is M. Δ (q) and G Δ Since (q) is bounded, the generalized lumped disturbance d and its derivative are both bounded, i.e., ||d|| < c1. Both c1 and c2 are unknown positive numbers.

[0041] 3) Contact force f e Force can be measured by installing a force sensor on an online drive continuous robotic arm.

[0042] The lumped external disturbance is then:

[0043] d = M0 -1 (x1)u d

[0044] Among them, M0(q), G0(q) and M are the nominal quantities of the inertia matrix, Coriolis force and centripetal force matrix, and potential energy term of the linearly driven continuous manipulator, respectively. Δ (q) and G Δ (q) represents the uncertainty caused by the unmodeled dynamics;

[0045] Step 2: Based on the desired inertia matrix, damping matrix, and stiffness matrix, establish the desired impedance model in the joint space of the continuous robotic arm to characterize the impedance error.

[0046]

[0047] Where q represents the actual value of the generalized state of the robotic arm system, q d M represents the desired trajectory in joint space. d =diag(M d1 M d2 M d3 Md4 )C d = diag(C d1 ,C d2 ,C d3 ,C d4 )K d = diag(K d1 ,K d2 ,K d3 ,K d4 ) respectively denote the desired inertia matrix, damping matrix and stiffness matrix, τ e is the contact force in the task space equivalent to the contact torque in the joint space;

[0048] Define the impedance error e z as

[0049]

[0050] Design the following second-order low-pass filter

[0051]

[0052] where q e denotes the deviation of the desired trajectory.

[0053] Define the error vector e = q - q r and the reference trajectory q r = q d + q e , e z can be further expressed as

[0054]

[0055] When the impedance error e z converges to the origin, the manipulator system will satisfy the desired impedance model shown in equation (6). In combination with equation (9), e→0 and e z →0 are equivalent in the low-frequency range.

[0056] Step 3, design a fixed-time disturbance observer to observe the external unknown disturbance of the serial manipulator and the unmodeled dynamics of the system:

[0057]

[0058] where, and denote the estimated values of the system state x2 and the lumped external disturbance d, is the state quantity of the observer, δ ∈ (0, 1) denotes the amplification factor, g1 > 0, g2 > 0, α ∈ (1, 1.5) and β ∈ (0.5, 1) are to be designed parameters, and satisfy For simplicity of expression, for x = [x1 x2... x n ] T ∈R n and α ≥ 0, define |x| α = [ |x1| α |x2| α ... |x n | α ] T , sig α (x) = [ |x1| α sign(x1) |x2| α sign(x2)... |x n | α sign(x n )] T where sign(·) denotes the sign function.

[0059] Step 4, design an anti-singularity segmentation function to eliminate the singularity problem caused by the derivation of the virtual control law in the non-singular fast terminal sliding mode control:

[0060]

[0061] where, is the independent variable of the anti-singularity function, ε a is a very small positive number, and 0 < β < 1 is a parameter to be designed for the controller;

[0062] Step 5, construct a non-singular fast terminal sliding surface, combine the disturbance estimation value in step 3 and the anti-singularity segmentation function in step 4, and design the sliding mode control law τ:

[0063] Design the sliding surface of the non-singular terminal sliding mode:

[0064]

[0065] where s = [s1 s2 s3 s4] T , k1 > 0, k2 > 0, and α > 1 are controller gains to be designed, and g(e) represents the use of an anti-singularity segmentation function for each element of the error e;

[0066] Taking the derivative of the above equation gives

[0067]

[0068] Since is

[0069]

[0070] Set the equivalent control term of the control law:

[0071]

[0072] Design the approaching term of the control law:

[0073] τ sw = -M0[λsig α (s)+μsig β (s)+γsign(s)] (16)

[0074] where λ>0, μ>0 and γ>0 are the controller gains to be designed; o

[0075] Obtain the sliding mode control law of the nonsingular fast terminal:

[0076] τ=τ eq +τ sw (17)

[0077] Step 6, generate the control input of the driving space according to the state space equation in step 1, the impedance model in step 2 and the control law in step 5, and calculate the driving rope tension according to the driving rope constraint to realize the contact force compliant control of the wire-driven continuum manipulator:

[0078]

[0079] where, represents the velocity Jacobian matrix of the continuum manipulator from the driving space to the joint space the null space of is the vector modified according to the minimum value in the calculation result of is the generalized inverse of

[0080] According to the designed actual control law, the fast and accurate control of the contact force between the wire-driven continuum manipulator end effector and the environment can be realized.

[0081] ​​​In summary, the application aims at a wire-driven serial manipulator, and designs a fixed-time impedance control method based on a non-singular fast terminal sliding mode. The method uses a fixed-time disturbance observer to effectively estimate and compensate for external disturbances and unmodeled dynamics, and can significantly improve the robustness of the control system. By constructing a non-singular fast terminal sliding mode method and introducing a non-singular fast terminal sliding mode method, the impedance control law can avoid singularity and ensure that the impedance error converges in a fixed time. The control framework based on the desired impedance model can smoothly switch between free motion and contact environment, and maintain the high compliance of the end effector. The fixed-time impedance control method based on the non-singular fast terminal sliding mode of the wire-driven serial manipulator can make the impedance error converge quickly, effectively overcome the influence of external disturbances and model uncertainties, and improve the precision and compliance performance of the contact force control.

[0082] The application also provides a fixed-time impedance control system for a wire-driven serial manipulator based on a non-singular fast terminal sliding mode, comprising the following modules:

[0083] A dynamics model construction module is used to establish a dynamics model of the wire-driven serial manipulator according to the piecewise constant curvature assumption, derive the state space equation thereof, and define the lumped uncertainty and external disturbances existing in the model;

[0084] A desired impedance model module is used to establish a desired impedance model in the joint space of the serial manipulator based on a desired inertia matrix, a damping matrix and a stiffness matrix, and characterize the impedance error;

[0085] A fixed-time disturbance observer module is used to design a fixed-time disturbance observer to observe the external unknown disturbances and unmodeled dynamics of the wire-driven serial manipulator;

[0086] A non-singular piecewise function module is used to design a non-singular piecewise function to eliminate the singularity problem of the virtual control law in the non-singular fast terminal sliding mode control during derivation;

[0087] A sliding mode control law module is used to construct a non-singular fast terminal sliding surface, combine the disturbance estimation value and the non-singular piecewise function, and design a sliding mode control law τ;

[0088] A contact force compliance control module is used to generate control input in the drive space by using the state space equation, the impedance model and the control law, and calculate the drive rope tension according to the drive rope constraint, so as to realize contact force compliance control of the wire-driven serial manipulator.

[0089] The application also provides a computer device, which comprises a memory, a processor and a computer program stored in the memory and executable on the processor, and the processor realizes the following steps when executing the computer program:

[0090] Step 1, the dynamics model of the line-driven continuous manipulator is established according to the piecewise constant curvature assumption, and the state space equation is derived, and the lumped uncertainty and external disturbance existing in the model are defined;

[0091] Step 2, based on the expected inertia matrix, damping matrix and stiffness matrix, an expected impedance model in the joint space of the continuous manipulator is established to represent the impedance error;

[0092] Step 3, a fixed-time disturbance observer is designed to observe the external unknown disturbance of the line-driven continuous manipulator and the unmodeled dynamics of the system;

[0093] Step 4, a singularity avoidance piecewise function is designed to eliminate the singularity problem caused by the derivative of the virtual control law in the nonsingular fast terminal sliding mode control;

[0094] Step 5, a nonsingular fast terminal sliding surface is constructed, and the sliding mode control law τ is designed by combining the disturbance estimation value in step 3 and the singularity avoidance piecewise function in step 4;

[0095] Step 6, the control input in the driving space is generated from the state space equation in step 1, the impedance model in step 2 and the control law in step 5, and the driving rope tension is calculated according to the driving rope constraint to realize the contact force compliant control of the line-driven continuous manipulator.

[0096] A computer storage medium has a computer program stored thereon, and the computer program is executed by a processor to implement the following steps:

[0097] Step 1, the dynamics model of the line-driven continuous manipulator is established according to the piecewise constant curvature assumption, and the state space equation is derived, and the lumped uncertainty and external disturbance existing in the model are defined;

[0098] Step 2, based on the expected inertia matrix, damping matrix and stiffness matrix, an expected impedance model in the joint space of the continuous manipulator is established to represent the impedance error;

[0099] Step 3, a fixed-time disturbance observer is designed to observe the external unknown disturbance of the line-driven continuous manipulator and the unmodeled dynamics of the system;

[0100] Step 4, a singularity avoidance piecewise function is designed to eliminate the singularity problem caused by the derivative of the virtual control law in the nonsingular fast terminal sliding mode control;

[0101] Step 5, a nonsingular fast terminal sliding surface is constructed, and the sliding mode control law τ is designed by combining the disturbance estimation value in step 3 and the singularity avoidance piecewise function in step 4;

[0102] Step 6, generate the control input of the driving space according to the state space equation in step 1, the impedance model in step 2 and the control law in step 5, and calculate the driving rope tension according to the driving rope constraint, so as to realize the contact force compliant control of the wire-driven continuum manipulator.

[0103] The above-described embodiments only express several implementation manners of the present application, and the description is relatively specific and detailed, but it should not be understood as a limitation on the patent scope of the present application. It should be pointed out that, for ordinary skilled persons in the art, several modifications and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the patent protection scope of the present application should be subject to the appended claims.

Claims

1. A non-singular fast terminal sliding mode based line drive continuum manipulator fixed time impedance control method, characterized in that, The method comprises the following steps: Step 1, a dynamics model of the wire-driven continuum manipulator is established according to a piecewise constant curvature assumption, and a state space equation thereof is derived, and a lumped uncertainty term and an external disturbance existing in the model are defined; Step 2, an expected impedance model in a joint space of the continuum manipulator is established based on an expected inertia matrix, a damping matrix and a stiffness matrix, and impedance errors are represented; Step 3, a fixed-time disturbance observer is designed to observe external unknown disturbances and unmodeled dynamics of the wire-driven continuum manipulator; Step 4, a singularity-avoiding piecewise function is designed to eliminate a singularity problem caused by derivation of a virtual control law in the non-singular fast terminal sliding mode control; Step 5, a non-singular fast terminal sliding surface is constructed, and a sliding mode control law τ is designed in combination with the disturbance estimation value in Step 3 and the singularity-avoiding piecewise function in Step 4; Step 6, control input in a driving space is generated from the state space equation in Step 1, the impedance model in Step 2 and the control law in Step 5, and driving rope tension is calculated according to driving rope constraints, so that contact force compliant control of the wire-driven continuum manipulator is realized.

2. The non-singular fast terminal sliding mode based linear drive continuum manipulator fixed-time impedance control method of claim 1, wherein, The dynamics model of the wire-driven continuum manipulator in Step 1 is: wherein, is the inertia matrix of the serial manipulator, is the joint angle variable, θ i and is the bending angle and rotation angle of the i-th joint, is the Coriolis and centripetal force matrix, G(q) is the potential energy term, τ d represents the uncertain external disturbance, is the control input, J Lq is the velocity Jacobian matrix of the serial manipulator from the driving space to the joint space, is the equivalent contact torque to the joint space, J Xq is the Jacobian matrix from the task space to the joint space, f e = [f ex f ey f ez ] T is the contact force between the manipulator end effector and the environment; Let x1 = q, The state space equation of the wire-driven continuum manipulator is The lumped uncertainty term is: The lumped external disturbance is: d = M0 -1 (x1)u d Among them, M0(q), G0(q) and M are the nominal quantities of the inertia matrix, Coriolis force and centripetal force matrix, and potential energy term of the linearly driven continuous manipulator, respectively. Δ (q) and G Δ (q) represents the uncertainty caused by the unmodeled dynamics.

3. The non-singular fast terminal sliding mode based linear drive continuum manipulator fixed-time impedance control method of claim 1, wherein, The expected impedance model in Step 2 is specifically: where M d = diag(M d1 ,M d2 ,M d3 ,M d4 ), C d = diag(C d1 ,C d2 ,C d3 ,C d4 ) and K d = diag(K d1 ,K d2 ,K d3 ,K d4 ) represent the desired inertia matrix, damping matrix and stiffness matrix, respectively, q represents the actual value of the generalized state of the robotic arm system, q d represents the desired trajectory in joint space, τ e is the contact force in task space equivalent to the contact torque in joint space.

4. The non-singular fast terminal sliding mode based linear drive continuum manipulator fixed-time impedance control method of claim 2, wherein, The fixed-time disturbance observer in Step 3 is: wherein and denotes the estimation value of the system state x2and the lumped external disturbance d, is the state quantity of the observer, δ∈(0, 1) denotes an amplification factor, g1>0, g2>0, α∈(1, 1.5) and β∈(0.5, 1) are to-be-designed parameters, and satisfy For simplicity of expression, for x=[x1x2...x n ] T ∈R n and α≥0, define |x α =[|x1| α |x2| α ... |x n | α ] T , sig α (x)=[|x1| α sign(x1) |x2| α sign(x2)... |x n | α sign(x n )] T , wherein sign(·) denotes a sign function.

5. The non-singular fast terminal sliding mode based linear drive continuum manipulator fixed-time impedance control method of claim 2, wherein, The singularity-avoiding piecewise function in Step 4 is: wherein, to avoid singular function arguments, ε a is a small positive number and 0 < β < 1 is a parameter of the controller to be designed.

6. The non-singular fast terminal sliding mode based linear drive continuum manipulator fixed-time impedance control method of claim 2, wherein, The sliding mode control law is: A sliding surface of the non-singular terminal sliding mode is designed: where s = [s1s2s3s4] T k1> 0, k2> 0, a > 1 are the controller gains to be designed, g(e) represents the use of a piecewise function avoiding singularity for each element of the error e; An equivalent control term of the control law is set: An approaching term of the control law is designed: τ sw = -M0[λsig α (s) + μsig β (s) + γsign(s)] where λ > 0, μ > 0 and γ > c o K is the gain of the controller to be designed; A non-singular fast terminal sliding mode control law is obtained: τ = τ eq + τ sw .

7. The non-singular fast terminal sliding mode based linear drive continuum manipulator fixed-time impedance control method of claim 2, wherein, The driving rope tension in Step 6 is: wherein, denotes the null space of the velocity Jacobian matrix from the continuous robot arm drive space to the joint space is a vector that is modified according to the minimum value in the calculation result is the generalized inverse of .​​ 8. A non-singular fast terminal sliding mode based linear drive continuum manipulator fixed time impedance control system, characterized in that, The method comprises the following modules: A dynamics model construction module is used to establish a dynamics model of the wire-driven continuum manipulator according to a piecewise constant curvature assumption, and to derive a state space equation thereof, and to define a lumped uncertainty term and an external disturbance existing in the model; An expected impedance model module is used to establish an expected impedance model in a joint space of the continuum manipulator based on an expected inertia matrix, a damping matrix and a stiffness matrix, and to represent impedance errors; A fixed-time disturbance observer module is used to design a fixed-time disturbance observer to observe external unknown disturbances and unmodeled dynamics of the wire-driven continuum manipulator; A singularity-avoiding piecewise function module is used to design a singularity-avoiding piecewise function to eliminate a singularity problem caused by derivation of a virtual control law in the non-singular fast terminal sliding mode control; A sliding mode control law module is used to construct a non-singular fast terminal sliding surface, and to design a sliding mode control law τ in combination with a disturbance estimation value and a singularity-avoiding piecewise function; A contact force compliant control module is used to generate control input in a driving space from a state space equation, an impedance model and a control law, and to calculate driving rope tension according to driving rope constraints, so that contact force compliant control of the wire-driven continuum manipulator is realized.

9. A computer device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, The processor executes the computer program to realize the steps of the method in any one of claims 1-7.

10. A computer storable medium having stored thereon a computer program, characterized in that The computer program is executed by the processor to realize the steps of the method in any one of claims 1-7.

Images