Trajectory tracking control method of rope-driven flexible continuous robot based on sliding mode impedance

Abstract

The invention discloses a trajectory tracking control method of a rope-driven flexible continuous robot based on sliding mode impedance. The trajectory tracking control method comprises the following steps: S1, obtaining a kinetic model and giving an expected reference trajectory; s2, introducing a quaternion to eliminate an error, and obtaining a tracking error of the working tail end of the robot; s3, designing a sliding mode, selecting an approaching rate, and enabling a position tracking error to reach the sliding mode; designing a control law of a robot trajectory tracking controller according to the robot model and a trajectory tracking control target, and determining control parameters; s4, introducing an impedance model to form a sliding mode impedance system; and S5, analyzing the stability and convergence of the sliding mode impedance system, adjusting the control parameters of the controller, and outputting a trajectory tracking result. The sliding-mode impedance controller is designed for the rope-driven flexible continuous robot, and on the basis that a sliding-mode control scheme is used for guaranteeing the robust performance of the system, the impedance scheme is combined to achieve compliant control.

Description

Technical Field

[0001] The invention relates to the technical field of robot system trajectory tracking, and in particular to a trajectory tracking control method of a rope-driven flexible continuous robot based on sliding mode impedance. Background Art

[0002] In the application scenarios of rope-driven flexible continuous robots that require highly precise and detailed control, accurate and fast control solutions are very much needed to ensure that continuous robots can be further developed in practical applications. Therefore, it is crucial to obtain reliable and stable control solutions. However, due to its high nonlinearity after modeling and the elastic properties brought by the material, this undoubtedly brings challenges to its related robust control design. Sliding mode control, as a strong robust nonlinear control method, is an excellent control solution for continuous robots, both in terms of strong robust performance and fast real-time performance. Sliding mode control is essentially a variable structure method. Its main method is to control the state of the system to enter the sliding surface by introducing a specific area called the sliding surface. Once the state enters the sliding surface, the dynamic performance of the system will show strong robustness.

[0003] At present, for most industrial robot application scenarios, the working environment of the robot is required to be known in advance and accurately modeled. However, for rescue robots and medical robots, it is difficult to meet the assumption that the working environment can be accurately modeled in advance, because they often work in unstructured environments. In other words, the working environment of such robots is often unstructured and uncertain. Rope-driven flexible continuous robots are often used in the fields of rescue and medical robots. They not only need to frequently contact unknown working environments, but also need to frequently interact with humans, or need to achieve various objects. In such scenarios, force control of continuous robots is required, and compliant control is a good solution for force control. In addition, impedance control is a feasible method to achieve compliant behavior. Hogan first introduced the concept of impedance control in 1984, which can now be considered a classic control method in robotics. One of the core of impedance control is that the controller should be able to adjust the mechanical impedance of the robot. Ahmad designed a multi-surface sliding mode control of continuum robots with mismatched uncertainties. It uses sliding mode control to deal with the uncertainty of the robot system. If there is an external force, the controller may fail. Summary of the invention

[0004] In order to solve the technical problems existing in the prior art, the present invention provides a trajectory tracking control method of a rope-driven flexible continuous robot based on sliding mode impedance. An impedance model is introduced on the basis of a sliding mode controller to ensure the robust performance of the system, and flexible control is achieved in combination with an impedance scheme.

[0005] The present invention is achieved by at least one of the following technical solutions.

[0006] The trajectory tracking control method of a rope-driven flexible continuous robot based on sliding mode impedance includes the following steps:

[0007] S1. Obtaining a dynamic model and an expected reference trajectory of the robot according to the structure of the rope-driven flexible continuous robot;

[0008] S2, introduce quaternion to eliminate truncation error, obtain the actual position of the robot's working end, feed it back to the controller, and make a difference with the expected reference trajectory in step S1 to obtain the tracking error of the robot's working end position;

[0009] S3. Design the sliding mode according to the position tracking error information, and select the approach rate to make the position tracking error reach the sliding mode; design the control law of the robot trajectory tracking controller according to the robot's dynamic model and expected trajectory, and determine the control parameters;

[0010] S4. Introduce the impedance model to form a sliding mode impedance system; when there is no external force, the robot's working end tracks the original desired trajectory; when there is an external force, the impedance model generates a new reference trajectory and tracks the new reference trajectory to comply with the external force until the external force disappears and then returns to tracking the original reference trajectory;

[0011] S5. Analyze the stability and convergence of the sliding mode impedance system, adjust the control parameters of the trajectory tracking controller, and output the trajectory tracking results.

[0012] Furthermore, in step S1, the structure of the rope-driven flexible continuous robot includes an elastic main rod, a plurality of support plates connected in series through the main rod, and four ropes that are parallel and symmetrical to each other in pairs, and the four ropes are connected to the main rod through a series of support plates.

[0013] Furthermore, in step S1, the dynamic model of the robot is as follows:

[0014]

[0015] Where s∈[0,L] is the spatial position variable, i.e., the arc length of the main rod, L is the length of the main rod, t∈[0,+∞] is the time variable, p(s,t) is the centerline position of the robot main rod, R(s,t) is the rotation matrix of the robot main rod, u(s,t) is the curvature vector of the rod in the local coordinate system, v(s,t) is the rate of change of the rod position relative to the arc length, q(s,t) is the velocity of the rod in the local coordinate system, and ω(s,t) is the angular velocity of the rod in the local coordinate system;

[0016] The subscripts s and t represent the first-order derivatives of the variable with respect to space and time, respectively, that is, p s (s,t) and p t (s, t) are the first-order derivatives of the centerline position of the robot main rod with respect to space and time, respectively, R s (s,t) and R t (s, t) is the first-order derivative of the robot's main rod rotation matrix with respect to space and time, respectively, u s (s,t) and u t (s, t) are the first-order derivatives of the curvature vector in the local coordinate system with respect to space and time, respectively, v s (s,t) and v t (s, t) are the first-order derivatives of the rate of change of the rod position with respect to the arc length with respect to space and time, respectively, q s (s,t) and q t (s, t) are the first-order derivatives of the velocity of the rod in the local coordinate system with respect to space and time, respectively, ω s (s,t) and ω t (s, t) are the first-order derivatives of the angular velocity of the rod in the local coordinate system with respect to space and time respectively; the superscript ^ represents the mapping from a three-dimensional vector space to its antisymmetric matrix, is the antisymmetric matrix of u(s,t), is the antisymmetric matrix of ω(s,t); the superscript * indicates the initial value of the variable, v * (s,t) is the initial value of v(s,t), u(s,t) is the initial value of u(s,t), v s The initial value of (s,t), for u s The initial value of (s, t), the subscript ext represents the external load, and tendon represents the action from the rope, that is, f ext is the external load distribution force, l ext is the external load distribution moment, f tendon is the distributed force of the rope, l tendon is the distributed moment of the rope, and the force and moment applied per unit arc length s are defined as l = l ext +l tendon and f = fext +f tendon , ρ is the mass density of the robot main rod, A is the cross-sectional area of ​​the robot main rod, J is the moment of inertia matrix, K se is the shear and tension stiffness matrix, K bt are the bending and torsional stiffness matrices.

[0017] Furthermore, in step S1, the models of the four ropes of the robot are as follows:

[0018] In the global coordinate system, assume that the path of the i-th rope is represented by p i (s, t), satisfying the following relationship:

[0019]

[0020] in It represents the offset of the i-th rope from the cross-sectional center of the robot main rod in the local coordinate system:

[0021]

[0022] where x i (s) and y i (s) are the offset functions of the cross-sectional center of the i-th rope and the robot main pole on the x-axis and y-axis, respectively;

[0023] Assume that the internal force is related to p i The direction of (s, t) is tangent. When friction and inertia are neglected, the distributed force f of the rope is tendon and the distributed moment l tendon It is obtained through the following relationship:

[0024]

[0025] where τ i is the tension on the ith rope, n is the number of ropes, For p is The antisymmetric matrix of (s,t), for The antisymmetric matrix of p in space i Taking the first and second order partial derivatives of (s, t) we can get:

[0026]

[0027] Where p is (s,t) is p i The first-order derivative of (s, t) with respect to space, p iss (s,t) is p i The second-order derivative of (s,t) with respect to space; for The first derivative with respect to space is for The second-order derivative with respect to space; v(s,t) is the rate of change of the rod position with respect to the arc length, v s (s,t) is the first-order derivative of the rate of change of the rod position with respect to the arc length with respect to space; is the antisymmetric matrix of u(s,t), for u s The antisymmetric matrix of (s,t).

[0028] Furthermore, in step S2, the quaternion h is introduced as:

[0029] h=h1+h2i x +h3j y +h4k z (6);

[0030] Where h1 is the real part of the quaternion h, i x ,j y ,k z is the imaginary unit, h2, h3, h4 are the coefficients of the corresponding imaginary unit; the first-order derivative of the quaternion h with respect to space h s for:

[0031]

[0032] where u(s,t)=[u1 u2 u3] T , u1 u2 u3 are the curvature components of u(s,t) on the x, y, and z axes respectively;

[0033] The rotation matrix R(s,t) is expressed as:

[0034]

[0035] Furthermore, in step S2, the actual position p(L, t) of the robot end is obtained by a high-speed camera installed around the working environment. The actual position p of the end of the rope-driven flexible continuous robot is x and the expected reference trajectory x d The tracking error e is:

[0036] e=x d -p x (8);

[0037] in(·) x Represents the value of a vector on the x-axis, that is, p x is the value of p(L,t) on the x-axis; the control target is the actual position p of the end of the rope-driven flexible continuous robot x Can track the desired reference trajectory x d, that is, the tracking error e eventually converges to 0.

[0038] Furthermore, in step S3, considering the external disturbances to the robot system, for the system on the x-axis, the first state variable is set to x1=p x , the second state variable is The state equation of the following robot system is obtained:

[0039]

[0040] in is the first-order derivative of x1 with respect to time, is the first-order derivative of x2 with respect to time, where p x is the position of the end of the rod on the x-axis, For p x The first derivative with respect to time is For p x For the second derivative of time, substitute formula (1) to formula (5) into formula (10) to obtain:

[0041]

[0042] where n s is the first-order derivative of the internal force of the rod with respect to space and time, d is the disturbance received by the end of the rope-driven flexible continuous robot, and F e is the external force acting on the robot system in the impedance model; subscript (·) x represents the value of a vector on the x-axis, R is the rotation matrix of the robot main rod, R = R (h), R t is the first-order derivative of the robot's main rod's rotation matrix with respect to time, q is the rod's velocity in the local coordinate system, and q t is the first derivative of the velocity of the rod in the local coordinate system with respect to time, is the antisymmetric matrix of ω(s,t); ω(s,t) is the angular velocity of the rod in the local coordinate system, ρ is the mass density of the robot main rod, A is the cross-sectional area of ​​the robot main rod, K se is the shear and tension stiffness matrix, K bt are the bending and torsional stiffness matrices; v s The initial value of v s is the rate of change of the rod position with respect to the arc length, is the antisymmetric matrix of u(s,t), u s (s, t) is the first-order derivative of the curvature vector in the local coordinate system with respect to space, f ext is the external load distribution force, f tendon is the distributed force of the rope;

[0043] In order to achieve the above control objectives, since it only considers the tracking problem on the x-axis, it is only necessary to control the two ropes on the x-axis side of the four ropes. Then the distributed force f of the rope is tendon The relationship with the tension of the two ropes is expressed as:

[0044]

[0045] where α i is the intermediate quantity, α p =[α1 α2] is the middle value of the two ropes on the positive and negative sides of the x-axis, The two ropes on both sides of the x-axis are regarded as equivalent to one rope that can achieve negative tension. τ1 and τ2 are the tensions on the two ropes.

[0046] Furthermore, in step S3, according to the control target of the robot system state, the sliding surface s is defined. m and sliding surface s m First derivative in time for:

[0047]

[0048] Where e is the actual position p of the end of the rope-driven flexible continuous robot x and the expected reference trajectory x d The tracking error, is the first-order derivative of the tracking error with respect to time, is the expected reference trajectory x d The second-order derivative with respect to time, λ is an adjustable parameter and satisfies λ>0; α1 is the intermediate quantity on the positive x-axis side, T c is the control input, ρ is the mass density of the robot main rod, A is the cross-sectional area of ​​the robot main rod, and d x is the disturbance on the x-axis experienced by the end of the rope-driven flexible continuous robot; the rope of the rope-driven flexible continuous robot is set on the positive and negative sides of the x-axis and y-axis. When designing the controller, only the control law of the positive side is considered separately, and the control law of the other side is obtained due to the symmetric relationship in the entire rope-driven flexible continuous robot;

[0049] In order to make the tracking error e of the robot's working end position converge to 0, the control law is:

[0050]

[0051] in k and ε are adjustable positive parameters, and sgn(.) is the sign function.

[0052] Furthermore, in step S4, the kinetic equation of the impedance model is:

[0053]

[0054] where F e is the external force acting on the robot system in the impedance model; M, B, and K represent the inertia, damping, and stiffness characteristics of the system, respectively; x0, Respectively represent the position, velocity and acceleration of the original tracking trajectory, x d , and They respectively represent the displacement, velocity and acceleration of the newly generated trajectory of the system under the action of external force.

[0055] Further, step S5 includes the following steps:

[0056] S51. Select the Lyapunov function V:

[0057]

[0058] where s m is the sliding surface;

[0059] S52. Find the first-order derivative of the Lyapunov function get:

[0060]

[0061] in, Sliding surface s m The first-order derivative with respect to time is is the first-order derivative of the tracking error with respect to time, is the expected reference trajectory x d The second-order derivative with respect to time, ρ is the mass density of the robot main rod, A is the cross-sectional area of ​​the robot main rod, n s is the first-order derivative of the rod's internal force with respect to space and time, f ext is the external load distribution force, F e is the external force acting on the robot system in the impedance model; the subscript x represents the value of a vector on the x-axis, α1 is the middle value on the positive x-axis side, T c is the control input, d x is the disturbance on the x-axis experienced by the end of the rope-driven flexible continuous robot; λ is an adjustable parameter and satisfies λ>0;

[0062] Substituting the control law into the input, we get:

[0063]

[0064] Where k>0, k and ε are adjustable positive parameters, s m is the sliding surface, and is dx The upper bound value of Always holds true. For the adjustable parameter ε, take ε>|d x |, we get:

[0065]

[0066] From the above deduction, V≥0 and And when time t→∞, V is bounded; According to Lyapunov stability theory and Barbalat lemma, when time t→∞, V is bounded, and s m →0, e→0, and Under the action of sliding mode impedance system, the asymptotic stability of the rope-driven flexible continuous robot system is guaranteed.

[0067] S53, combining the control law and the analysis of step S51 and step S52, select adjustable parameters, and the external disturbance parameters are assumed to be: the disturbance received by the end of the rope-driven flexible continuous robot Set the inertia characteristic M, damping characteristic B and stiffness characteristic K of the impedance model;

[0068] S54. The rope-driven flexible continuous robot outputs rope tension under the action of the sliding mode impedance system to drive the working end of the robot to move. The actual position of the working end of the rope-driven flexible continuous robot is measured by the sensor, and the position tracking error is calculated.

[0069] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0070] 1. The sliding mode impedance controller of the present invention has better dynamic performance and tracking accuracy, and can generate a new reference trajectory according to the external force, thus achieving a compliant control effect.

[0071] 2. The present invention introduces an impedance model based on the sliding mode controller, which can generate a new reference trajectory according to the external force, thereby achieving a compliant control effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] Figure 1 It is a flow chart of a trajectory tracking control method of a rope-driven flexible continuous robot based on sliding mode impedance of the present invention;

[0073] Figure 2 is a schematic structural diagram of a rope-driven flexible continuous robot according to an embodiment of the present invention;

[0074] Figure 3 is a diagram showing the change of the magnitude of the external force over time in an embodiment of the present invention;

[0075] Figure 4 is a schematic diagram of trajectory tracking in the presence of external force in an embodiment of the present invention;

[0076] Figure 5 is a schematic diagram of the output of a sliding mode impedance controller in an embodiment of the present invention;

[0077] Figure 6 is a diagram showing the deformation of the rod in the xz plane over time in an embodiment of the present invention; DETAILED DESCRIPTION

[0078] The present invention is further described in detail below in conjunction with embodiments and drawings, but the embodiments of the present invention are not limited thereto.

[0079] like Figure 1 As shown, in this embodiment, a trajectory tracking control method of a rope-driven flexible continuous robot based on sliding mode impedance includes the following steps:

[0080] S1. Obtaining a dynamic model and an expected reference trajectory of the robot according to the structure of the rope-driven flexible continuous robot;

[0081] S2, introduce quaternion to eliminate truncation error; obtain the actual position of the robot's working end, feed it back to the controller, and make a difference with the expected reference trajectory in step S1 to obtain the tracking error of the robot's working end position;

[0082] S3. Design a sliding mode based on the position tracking error information, and select a convergence rate to make the position tracking error reach the sliding mode; design a control law of the robot trajectory tracking controller based on the robot's dynamic model and the desired trajectory, and determine the control parameters;

[0083] S4. Introduce the impedance model to form a sliding mode impedance system; when there is no external force, the robot's working end tracks the original desired trajectory; when there is an external force, the impedance model generates a new reference trajectory and tracks the new reference trajectory to comply with the external force until the external force disappears and then returns to tracking the original reference trajectory;

[0084] S5. Analyze the stability and convergence of the sliding mode impedance system, adjust the control parameters of the trajectory tracking controller, and output the trajectory tracking results.

[0085] Specifically, in this embodiment, the specific process of step S1 is as follows:

[0086] S11. The structure of the rope-driven flexible continuous robot is as follows Figure 2 As shown, a segment of a multi-segment rope-driven flexible continuous robot is taken as the research object. The rope-driven continuous robot is mainly composed of a central elastic main rod 1, a support plate 2 passing through the elastic main rod 1, and four driving ropes 3 that are parallel and symmetrical to each other. These ropes 3 are connected to the elastic main rod 1 through a series of support plates 2. The mass of the support plate 2 and the rope 3 can be ignored, and the friction between the support plate 2 and the rope 3 can also be ignored.

[0087] The present invention adopts Cosserat rod theory to respectively model the robot main rod and rope, and then couples the two models to obtain the dynamic model of the robot.

[0088] S12. The dynamic model of the robot is as follows:

[0089]

[0090] Where s∈[0,L] is the spatial position variable (i.e., the arc length of the main rod), t∈[0,+∞] is the time variable, p(s,t) is the centerline position of the robot main rod, R(s,t) is the rotation matrix of the robot main rod, u(s,t) is the curvature vector of the rod in the local coordinate system, v(s,t) is the rate of change of the rod position relative to the arc length, q(s,t) is the velocity of the rod in the local coordinate system, and ω(s,t) is the angular velocity of the rod in the local coordinate system;

[0091] The subscripts s and t represent the first-order derivatives of the variable with respect to space and time, respectively, that is, p s (s,t) and p t (s, t) are the first-order derivatives of the centerline position of the robot main rod with respect to space and time, respectively, R s (s,t) and R t (s, t) is the first-order derivative of the robot's main rod rotation matrix with respect to space and time, respectively, u s (s,t) and u t (s, t) are the first-order derivatives of the curvature vector in the local coordinate system with respect to space and time, respectively, v s (s,t) and v t (s, t) are the first-order derivatives of the rate of change of the rod position with respect to the arc length with respect to space and time, respectively, q s (s,t) and q t (s, t) are the first-order derivatives of the velocity of the rod in the local coordinate system with respect to space and time, respectively, ω s (s,t) and ω t (s, t) are the first-order derivatives of the angular velocity of the rod in the local coordinate system with respect to space and time respectively; the superscript ^ represents the mapping from a three-dimensional vector space to its antisymmetric matrix, is the antisymmetric matrix of u(s,t), is the antisymmetric matrix of ω(s,t); the superscript * indicates the initial value of the variable, v * (s,t) is the initial value of v(s,t), u(s,t) is the initial value of u(s,t), v s The initial value of (s,t), for u sThe initial value of (s, t), the subscript ext represents the external load, and tendon represents the action from the rope, that is, f ext is the external load distribution force, l ext is the external load distribution moment, f tendon is the distributed force of the rope, l tendon is the distributed moment of the rope, and the force and moment applied per unit arc length s are defined as l = l ext +l tendon and f = f ext +f tendon , ρ is the mass density of the robot main rod, A is the cross-sectional area of ​​the robot main rod, J is the moment of inertia matrix, K se is the shear and tension stiffness matrix, K bt are the bending and torsional stiffness matrices;

[0092] S13. The model of the four ropes of the robot is as follows:

[0093] In the global coordinate system, assume that the path of the i-th rope is represented by p i (s, t), satisfying the following relationship:

[0094]

[0095] in It represents the offset of the i-th rope from the cross-sectional center of the robot main rod in the local coordinate system:

[0096]

[0097] where x i (s) and y i (s) are the offset functions of the cross-sectional center of the ith rope and the robot main pole on the x-axis and y-axis, respectively.

[0098] Assume that the internal force is related to p i The direction of (s, t) is tangent. When friction and inertia are neglected, the distributed force f of the rope is tendon and the distributed moment l tendon It is obtained through the following relationship:

[0099]

[0100] where τ i is the tension on the ith rope, n is the number of ropes, For p is The antisymmetric matrix of (s,t), for The antisymmetric matrix of p in space i Taking the first and second order partial derivatives of (s, t) we can get:

[0101]

[0102] Where p is (s,t) is p i The first-order derivative of (s, t) with respect to space, p iss (s,t) is p i The second-order derivative of (s,t) with respect to space; for The first derivative with respect to space is for The second-order derivative with respect to space; v(s,t) is the rate of change of the rod position with respect to the arc length, v s (s,t) is the first-order derivative of the rate of change of the rod position with respect to the arc length with respect to space; is the antisymmetric matrix of u(s,t), for u s The antisymmetric matrix of (s,t).

[0103] As an embodiment, a reference motion trajectory x0 is given, and x0=0.1sin(π / 3t).

[0104] Specifically, in this embodiment, the specific process of step S2 is as follows:

[0105] S21. Introduce the quaternion h as follows:

[0106] h=h1+h2i+h3j+h4k (6);

[0107] Where h1 is the real part of the quaternion h, i, j, k are imaginary units, and h2, h3, h4 are the coefficients of the corresponding imaginary units. The first-order derivative of the quaternion h with respect to space is h s for:

[0108]

[0109] where u(s,t)=[u1 u2 u3] T , u1 u2 u3 are the curvature components of u(s,t) on the x, y, and z axes respectively.

[0110] Then the rotation matrix R(s,t) can be expressed as:

[0111]

[0112] S22, the actual position p(L, t) of the robot end is obtained by installing high-speed cameras around the working environment. The actual position p of the end of the rope-driven flexible continuous robot is x and the expected reference trajectory x d The tracking error e is:

[0113] e=x d -p x (9);

[0114] in(·) x Represents the value of a vector on the x-axis, that is, p x is the value of p(L,t) on the x-axis. The control target is the actual position p of the end of the rope-driven flexible continuous robot. x Can track the desired reference trajectory x d , that is, the tracking error e eventually converges to 0.

[0115] Specifically, in this embodiment, the specific process of step S3 is as follows:

[0116] S31. Consider the external disturbances to the robot system. For the system on the x-axis, let the state variable 1 be x1=p x , state variable 2 is The state equation of the following robot system is obtained:

[0117]

[0118] in is the first-order derivative of x1 with respect to time, is the first-order derivative of x2 with respect to time. x is the position of the end of the rod on the x-axis, For p x The first derivative with respect to time is For p x For the second derivative of time, substitute formula (1) to formula (5) into formula (10) to obtain:

[0119]

[0120] where n s is the first-order derivative of the internal force of the rod with respect to space and time, d is the disturbance received by the end of the rope-driven flexible continuous robot, and F e is the external force acting on the robot system in the impedance model; subscript (·) x represents the value of a vector on the x-axis, R is the rotation matrix of the robot main rod (R = R (h)), R t is the first-order derivative of the robot's main rod's rotation matrix with respect to time, q is the rod's velocity in the local coordinate system, and q t is the first derivative of the velocity of the rod in the local coordinate system with respect to time, is the antisymmetric matrix of ω(s,t); ω(s,t) is the angular velocity of the rod in the local coordinate system, ρ is the mass density of the robot main rod, A is the cross-sectional area of ​​the robot main rod, K seis the shear and tension stiffness matrix, K bt are the bending and torsional stiffness matrices; v s The initial value of v s is the rate of change of the rod position with respect to the arc length, is the antisymmetric matrix of u(s,t), u s (s, t) is the first-order derivative of the curvature vector in the local coordinate system with respect to space, f ext is the external load distribution force, f tendon is the distributed force of the rope.

[0121] In order to achieve the above control objectives, since it only considers the tracking problem on the x-axis, it is only necessary to control the two ropes on the x-axis side of the four ropes. Then the distributed force f of the rope is tendon The relationship with the tension of the two ropes is expressed as:

[0122]

[0123] where α i is the intermediate quantity, α p =[α1 α2] is the middle value of the two ropes on the positive and negative sides of the x-axis, The two ropes on both sides of the x-axis are regarded as equivalent to one rope that can achieve negative tension. τ1 and τ2 are the tensions on the two ropes.

[0124] S32, then define the sliding surface s according to the control target of the robot system state m and sliding surface s m First derivative in time for:

[0125]

[0126] Where e is the actual position p of the end of the rope-driven flexible continuous robot x and the expected reference trajectory x d The tracking error, is the first-order derivative of the tracking error with respect to time, is the expected reference trajectory x d The second-order derivative with respect to time, λ is an adjustable parameter and satisfies λ>0; α1 is the intermediate quantity on the positive x-axis side, T c is the control input, ρ is the mass density of the robot main rod, A is the cross-sectional area of ​​the robot main rod, and d x is the disturbance received by the end of the rope-driven flexible continuous robot on the x-axis; the rope of the rope-driven flexible continuous robot is set on the positive and negative sides of the x-axis and y-axis. When designing the controller, only the control law of the positive side is considered separately, and the control law of the other side is obtained due to the symmetrical relationship in the entire rope-driven flexible continuous robot.

[0127] In order to make the tracking error e of the robot's working end position converge to 0, the control law is:

[0128]

[0129] in k and ε are adjustable positive parameters, and sgn(.) is the sign function.

[0130] Specifically, in this embodiment, the specific process of step S4 is as follows:

[0131] Adding an impedance model can give the robot a certain controllable compliance at the level of the control algorithm. This flexibility is like a spring fixed to the end of the robot, with a controllable stiffness coefficient. Although such a spring does not exist in physical space, this function can be realized by the control algorithm. The dynamic equation of the impedance model is as follows:

[0132]

[0133] Among them, M, B, and K are three adjustable parameters in the impedance model that affect the impedance characteristics of the system, representing the inertia characteristics, damping characteristics, and stiffness characteristics of the system, respectively. The inertia parameter M of the system is related to the structure and material of the system and is generally not adjustable; the damping parameter B of the system affects the dynamic characteristics of the system; the stiffness parameter K of the system reflects whether the system is flexible or rigid when interacting with the environment. e is the external force acting on the robot system in the impedance model, such as Figure 3 As shown. Among them, x0, Respectively represent the position, velocity and acceleration of the original tracking trajectory. d , and They respectively represent the displacement, velocity and acceleration of the newly generated trajectory of the system under the action of external force.

[0134] Specifically, in this embodiment, the specific process of step S5 is as follows:

[0135] S51. Select the Lyapunov function V:

[0136]

[0137] where s m The sliding surface.

[0138] S52. Find the first-order derivative of the Lyapunov function get:

[0139]

[0140] in, Sliding surface s m The first-order derivative with respect to time is is the first-order derivative of the tracking error with respect to time, is the expected reference trajectory x d The second-order derivative with respect to time, ρ is the mass density of the robot main rod, A is the cross-sectional area of ​​the robot main rod, n s is the first-order derivative of the rod's internal force with respect to space and time, f ext is the external load distribution force, F e is the external force acting on the robot system in the impedance model; the subscript x represents the value of a vector on the x-axis, α1 is the middle value on the positive x-axis side, T c is the control input, d x is the disturbance on the x-axis experienced by the end of the rope-driven flexible continuous robot; λ is an adjustable parameter and satisfies λ>0.

[0141] Substituting the control law into the input, we get:

[0142]

[0143] Where k>0, k and ε are adjustable positive parameters, s m is the sliding surface, and is d x The upper bound value of Always holds true. For the adjustable parameter ε, take ε>|d x |, we get:

[0144]

[0145] From the above deduction, V≥0 and And when time t→∞, V is bounded; According to Lyapunov stability theory and Barbalat lemma, when time t→∞, V is bounded, and s m →0, e→0, and Therefore, it can be concluded that under the action of the sliding mode impedance controller, the asymptotic stability of the rope-driven flexible continuous robot system can be guaranteed.

[0146] S53, combining the control law and the analysis of step S51 and step S52, as an embodiment, the adjustable parameters are selected as λ=12, k=10, ε=3. The external disturbance parameters are assumed to be: The parameters of the impedance model are set as: M = diag (10, 10, 10), B = diag (50, 50, 50), K = diag (150, 150, 150).

[0147] S54. The rope-driven flexible continuous robot outputs rope tension under the action of the sliding mode impedance controller to drive the working end of the robot to move. The actual position of the working end of the rope-driven flexible continuous robot is measured by the sensor, and the position tracking error is calculated. Figure 4 The actual tracking trajectory, the new reference trajectory, and the original reference trajectory are shown in the presence of the above external forces. It can be seen that when the external force is not 0, the sliding mode impedance controller will generate a new reference trajectory based on the external force, and then the controller will track the new reference trajectory to comply with the external force to achieve the purpose of compliant control. After the external force disappears, the reference trajectory and the original reference trajectory return to an equal relationship, and the controller will re-track the original reference trajectory.

[0148] Figure 5 is the controller output of the sliding mode impedance controller, which can further help understand how the sliding mode impedance controller works. Figure 6 The deformation of the rod on the xz plane over time under the control of the sliding mode impedance controller shows that its deformation state is in line with expectations, and the entire rod complies with the external force F. e The size and direction of the movement are adjusted accordingly, and the new desired trajectory is tracked correctly, achieving a smooth control effect.

[0149] The above embodiments are preferred implementation modes of the present invention, but the implementation modes of the present invention are not limited to the above embodiments. Any other changes, modifications, substitutions, combinations, and simplifications that do not deviate from the spirit and principles of the present invention should be equivalent replacement methods and are included in the protection scope of the present invention.

Claims

1. A trajectory tracking control method for a rope-driven flexible continuous robot based on sliding mode impedance, characterized in that: The following steps are involved: S1. Obtaining a dynamic model and an expected reference trajectory of the robot according to the structure of the rope-driven flexible continuous robot; S2, introduce quaternion to eliminate truncation error, obtain the actual position of the robot's working end, feed it back to the controller, and make a difference with the expected reference trajectory in step S1 to obtain the tracking error of the robot's working end position; S3. Design the sliding mode according to the position tracking error information and select the approach rate so that the position tracking error reaches the sliding mode; According to the robot's dynamic model and expected trajectory, design the control law of the robot's trajectory tracking controller and determine the control parameters; S4. Introduce the impedance model to form a sliding mode impedance system; when there is no external force, the robot's working end tracks the original desired trajectory; when there is an external force, the impedance model generates a new reference trajectory and tracks the new reference trajectory to comply with the external force until the external force disappears and then returns to tracking the original reference trajectory; S5. Analyze the stability and convergence of the sliding mode impedance system, adjust the control parameters of the trajectory tracking controller, and output the trajectory tracking results.

2. The trajectory tracking control method of a rope-driven flexible continuous robot based on sliding mode impedance according to claim 1 is characterized in that: In step S1, the structure of the rope-driven flexible continuous robot includes an elastic main rod, a plurality of support plates connected in series through the main rod, and four ropes that are parallel and symmetrical to each other. The four ropes are connected to the main rod through a series of support plates.

3. The trajectory tracking control method of a rope-driven flexible continuous robot based on sliding mode impedance according to claim 1 is characterized in that: In step S1, the dynamic model of the robot is as follows: Where s∈[0,L] is the spatial position variable, i.e., the arc length of the main rod, L is the length of the main rod, t∈[0,+∞] is the time variable, p(s,t) is the centerline position of the robot main rod, R(s,t) is the rotation matrix of the robot main rod, u(s,t) is the curvature vector of the rod in the local coordinate system, v(s,t) is the rate of change of the rod position relative to the arc length, q(s,t) is the velocity of the rod in the local coordinate system, and ω(s,t) is the angular velocity of the rod in the local coordinate system; The subscripts s and t represent the first-order derivatives of the variable with respect to space and time, respectively, that is, p s (s,t) and p t (s, t) are the first-order derivatives of the centerline position of the robot main rod with respect to space and time, respectively, R s (s,t) and R t (s, t) is the first-order derivative of the robot's main rod rotation matrix with respect to space and time, respectively, u s (s,t) and u t (s, t) are the first-order derivatives of the curvature vector in the local coordinate system with respect to space and time, respectively, v s (s,t) and v t (s, t) are the first-order derivatives of the rate of change of the rod position with respect to the arc length with respect to space and time, respectively, q s (s,t) and q t (s, t) are the first-order derivatives of the velocity of the rod in the local coordinate system with respect to space and time, respectively, ω s (s,t) and ω t (s, t) are the first-order derivatives of the angular velocity of the rod in the local coordinate system with respect to space and time respectively; the superscript ^ represents the mapping from a three-dimensional vector space to its antisymmetric matrix, is the antisymmetric matrix of u(s,t), is the antisymmetric matrix of ω(s,t); the superscript * indicates the initial value of the variable, v * (s,t) is the initial value of v(s,t), u(s,t) is the initial value of u(s,t), v s The initial value of (s,t), for u s The initial value of (s, t), the subscript ext represents the external load, and tendon represents the action from the rope, that is, f ext is the external load distribution force, l ext is the external load distribution moment, f tendon is the distributed force of the rope, l tendon is the distributed moment of the rope, and the force and moment applied per unit arc length s are defined as l = l ext +l tendon and f = f ext +f tendon , ρ is the mass density of the robot main rod, A is the cross-sectional area of ​​the robot main rod, J is the moment of inertia matrix, K se is the shear and tension stiffness matrix, K bt are the bending and torsional stiffness matrices.

4. The trajectory tracking control method of a rope-driven flexible continuous robot based on sliding mode impedance according to claim 1 is characterized in that: In step S1, the models of the four ropes of the robot are as follows: In the global coordinate system, assume that the path of the i-th rope is represented by p i (s, t), satisfying the following relationship: in It represents the offset of the i-th rope from the cross-sectional center of the robot main rod in the local coordinate system: where x i (s) and y i (s) are the offset functions of the cross-sectional center of the i-th rope and the robot main pole on the x-axis and y-axis, respectively; Assume that the internal force is related to p i The direction of (s, t) is tangent. When friction and inertia are neglected, the distributed force f of the rope is tendon and the distributed moment l tendon It is obtained through the following relationship: where τ i is the tension on the ith rope, n is the number of ropes, For p is The antisymmetric matrix of (s,t), for The antisymmetric matrix of p in space i Taking the first and second order partial derivatives of (s, t) we can get: Where p is (s,t) is p i The first-order derivative of (s, t) with respect to space, p iss (s,t) is p i The second-order derivative of (s,t) with respect to space; for The first derivative with respect to space is for The second derivative with respect to space; v(s,t) is the rate of change of the rod position with respect to the arc length, v s (s,t) is the first-order derivative of the rate of change of the rod position with respect to the arc length with respect to space; is the antisymmetric matrix of u(s,t), for u s The antisymmetric matrix of (s,t).

5. The trajectory tracking control method of a rope-driven flexible continuous robot based on sliding mode impedance according to claim 1 is characterized in that: In step S2, the quaternion h is introduced as: h=h1+h2i x +h3j y +h4k z (6); Where h1 is the real part of the quaternion h, i x ,j y ,k z is the imaginary unit, h2, h3, h4 are the coefficients of the corresponding imaginary unit; the first-order derivative of the quaternion h with respect to space h s for: where u(s,t)=[u1 u2 u3] T , u1 u2 u3 are the curvature components of u(s,t) on the x, y, and z axes respectively; The rotation matrix R(s,t) is expressed as:

6. The trajectory tracking control method of a rope-driven flexible continuous robot based on sliding mode impedance according to claim 1 is characterized in that: In step S2, the actual position p(L, t) of the robot end is obtained by high-speed cameras installed around the working environment. The actual position p x and the expected reference trajectory x d The tracking error e is: and=x d -p x (9); in(·) x Represents the value of a vector on the x-axis, that is, p x is the value of p(L,t) on the x-axis; the control target is the actual position p of the end of the rope-driven flexible continuous robot x Can track the desired reference trajectory x d , that is, the tracking error e eventually converges to 0.

7. A trajectory tracking control method for a rope-driven flexible continuous robot based on sliding mode impedance according to any one of claims 1 to 6, characterized in that: In step S3, considering the external disturbances to the robot system, for the system on the x-axis, the first state variable is x1=p x , the second state variable is The state equation of the following robot system is obtained: in is the first-order derivative of x1 with respect to time, is the first-order derivative of x2 with respect to time, where p x is the position of the end of the rod on the x-axis, For p x The first derivative with respect to time is For p x For the second derivative of time, substitute formula (1) to formula (5) into formula (10) to obtain: where n s is the first-order derivative of the internal force of the rod with respect to space and time, d is the disturbance received by the end of the rope-driven flexible continuous robot, and F e is the external force acting on the robot system in the impedance model; subscript (·) x represents the value of a vector on the x-axis, R is the rotation matrix of the robot main rod, R = R (h), R t is the first-order derivative of the robot's main rod's rotation matrix with respect to time, q is the rod's velocity in the local coordinate system, and q t is the first derivative of the velocity of the rod in the local coordinate system with respect to time, is the antisymmetric matrix of ω(s,t); ω(s,t) is the angular velocity of the rod in the local coordinate system, ρ is the mass density of the robot main rod, A is the cross-sectional area of ​​the robot main rod, K se is the shear and tension stiffness matrix, K bt are the bending and torsional stiffness matrices; v s The initial value of v s is the rate of change of the rod position with respect to the arc length, is the antisymmetric matrix of u(s,t), u s (s, t) is the first-order derivative of the curvature vector in the local coordinate system with respect to space, f ext is the external load distribution force, f tendon is the distributed force of the rope; In order to achieve the above control objectives, since it only considers the tracking problem on the x-axis, it is only necessary to control the two ropes on the x-axis side of the four ropes, so the distributed force f of the rope is tendon The relationship with the tension of the two ropes is expressed as: where α i is the intermediate quantity, α p =[α1 α2] is the middle value of the two ropes on the positive and negative sides of the x-axis, The two ropes on both sides of the x-axis are regarded as equivalent to one rope that can achieve negative tension. τ1 and τ2 are the tensions on the two ropes.

8. The trajectory tracking control method of a rope-driven flexible continuous robot based on sliding mode impedance according to claim 1 is characterized in that: In step S3, the sliding surface s is defined according to the control target of the robot system state. m and sliding surface s m First derivative in time for: Where e is the actual position p of the end of the rope-driven flexible continuous robot x and the expected reference trajectory x d The tracking error, is the first-order derivative of the tracking error with respect to time, is the expected reference trajectory x d The second-order derivative with respect to time, λ is an adjustable parameter and satisfies λ>0; α1 is the intermediate quantity on the positive x-axis side, T c is the control input, ρ is the mass density of the robot main rod, A is the cross-sectional area of ​​the robot main rod, and d x is the disturbance on the x-axis experienced by the end of the rope-driven flexible continuous robot; the rope of the rope-driven flexible continuous robot is set on the positive and negative sides of the x-axis and y-axis. When designing the controller, only the control law of the positive side is considered separately, and the control law of the other side is obtained due to the symmetric relationship in the entire rope-driven flexible continuous robot; In order to make the tracking error e of the robot's working end position converge to 0, the control law is: in k and ε are adjustable positive parameters, and sgn(.) is the sign function.

9. The trajectory tracking control method of a rope-driven flexible continuous robot based on sliding mode impedance according to claim 1 is characterized in that: In step S4, the kinetic equation of the impedance model is: where F e is the external force acting on the robot system in the impedance model; M, B, and K represent the inertia, damping, and stiffness characteristics of the system, respectively; x0, Respectively represent the position, velocity and acceleration of the original tracking trajectory, x d , and They respectively represent the displacement, velocity and acceleration of the newly generated trajectory of the system under the action of external force.

10. The trajectory tracking control method of a rope-driven flexible continuous robot based on sliding mode impedance according to claim 1, characterized in that: Step S5 includes the following steps: S51. Select the Lyapunov function V: where s m is the sliding surface; S52. Find the first-order derivative of the Lyapunov function get: in, Sliding surface s m The first-order derivative with respect to time is is the first-order derivative of the tracking error with respect to time, is the expected reference trajectory x d The second-order derivative with respect to time, ρ is the mass density of the robot main rod, A is the cross-sectional area of ​​the robot main rod, n s is the first-order derivative of the rod's internal force with respect to space and time, f ext is the external load distribution force, F e is the external force acting on the robot system in the impedance model; the subscript x represents the value of a vector on the x-axis, α1 is the middle value on the positive x-axis side, T c is the control input, d x is the disturbance on the x-axis experienced by the end of the rope-driven flexible continuous robot; λ is an adjustable parameter and satisfies λ>0; Substituting the control law into the input, we get: Where k>0, k and ε are adjustable positive parameters, s m is the sliding surface, and is d x The upper bound value of Always holds true. For the adjustable parameter ε, take ε>|d x |, we get: From the above deduction, V≥0 and And when time t→∞, V is bounded; According to Lyapunov stability theory and Barbalat lemma, when time t→∞, V is bounded, and s m →0, e→0, and Under the action of sliding mode impedance system, the asymptotic stability of the rope-driven flexible continuous robot system is guaranteed. S53, combining the control law and the analysis of step S51 and step S52, select adjustable parameters, and the external disturbance parameters are assumed to be: the disturbance received by the end of the rope-driven flexible continuous robot Set the inertia characteristic M, damping characteristic B and stiffness characteristic K of the impedance model; S54. The rope-driven flexible continuous robot outputs rope tension under the action of the sliding mode impedance system to drive the working end of the robot to move. The actual position of the working end of the rope-driven flexible continuous robot is measured by the sensor, and the position tracking error is calculated.

Images