Trajectory tracking control method for rope-driven flexible continuum robot based on sliding mode impedance

Abstract

The application discloses a trajectory tracking control method based on a sliding mode impedance for a rope-driven flexible continuous robot, and comprises the following steps: S1, a dynamic model is acquired, and a desired reference trajectory is given; S2, a quaternion is introduced to eliminate an error, and a tracking error of a robot working end is acquired; S3, a sliding mode is designed, a reaching rate is selected, and a position tracking error is caused to reach the sliding mode; a control law of a robot trajectory tracking controller is designed according to a robot model and a control target of trajectory tracking, and control parameters are determined; S4, an impedance model is introduced to form a sliding mode impedance system; and S5, stability and convergence of the sliding mode impedance system are analyzed, the control parameters of the controller are adjusted, and a trajectory tracking result is output. The application designs a sliding mode impedance controller for the rope-driven flexible continuous robot, and combines an impedance scheme to realize flexible control on the basis of guaranteeing robust performance of the system by using a sliding mode control scheme.

Description

Technical Field

[0001] This invention relates to the field of trajectory tracking technology for robot systems, and in particular to a trajectory tracking control method for a rope-driven flexible continuous robot based on sliding mode impedance. Background Technology

[0002] In applications requiring highly precise and meticulous control, rope-driven flexible continuous robots necessitate accurate and rapid control schemes to ensure their continued development in practical applications. Therefore, obtaining a reliable and stable control scheme is crucial. However, the inherent high nonlinearity of continuous robots after modeling, along with the elastic properties of the materials, undoubtedly presents challenges to the design of robust control systems. Sliding mode control, as a robust nonlinear control method, offers excellent control for continuous robots due to its strong robustness and rapid real-time performance. Essentially a variable structure method, sliding mode control primarily involves introducing a specific region called a sliding surface to control the system's state. Once the state enters the sliding surface, the system's dynamic performance exhibits strong robustness.

[0003] Currently, for most industrial applications of robotics, the working environment of the robot is required to be known in advance and accurately modeled. However, for rescue and medical robots, it is difficult to meet the assumption of accurately modeling the working environment in advance, because they often operate in unstructured environments. In other words, the working environment of these robots is often unstructured and unpredictable. Rope-driven flexible continuous robots are commonly used in rescue and medical robotics. They not only frequently encounter unknown working environments but also need to frequently interact with humans, or perform tasks such as grasping various objects. In such scenarios, requirements are placed on the force control of continuous robots, and compliant control is a good solution for force control. Furthermore, impedance control is a feasible method to achieve compliant behavior. Hogan first introduced the concept of impedance control in 1984, and it is now considered a classic control method in robotics. One of the core aspects 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 for continuum robots with mismatched uncertainties. This control addresses the uncertainties of the robot system through sliding mode control, which may fail if external forces are applied. Summary of the Invention

[0004] To address the technical problems existing in the prior art, this invention provides a trajectory tracking control method for a rope-driven flexible continuous robot based on sliding mode impedance. By introducing an impedance model on the basis of the sliding mode controller, the robust performance of the system is ensured, and compliant control is achieved by combining the impedance scheme.

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

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

[0007] S1. Obtain the dynamic model and desired reference trajectory of the robot based on the structure of the rope-driven flexible continuous robot;

[0008] S2. Introduce quaternions to eliminate truncation error, obtain the actual position of the robot's end effector, feed it back to the controller, and subtract it from the expected reference trajectory in step S1 to obtain the tracking error of the robot's end effector position.

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

[0010] S4. Introduce an impedance model to form a sliding mode impedance system; when there is no external force, the robot's end effector 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 adapt to the external force, until the external force disappears and it will resume 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, multiple support plates connected in series through the main rod, and four ropes that are parallel and symmetrical to each other in pairs. The four ropes are connected to the main rod through a series of support plates.

[0013] Furthermore, in step S1, the robot's dynamic model 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's main rod, R(s,t) is the rotation matrix of the robot's 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's 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] Where the subscripts s and t represent the first derivatives of the variable with respect to space and time, respectively, i.e., p s (s,t) and p t (s,t) represents the first derivative of the robot's main link's centerline position with respect to space and time, respectively, R. s (s,t) and R t (s,t) represents the first derivatives of the rotation matrix of the robot's main link with respect to space and time, respectively, u s (s,t) and u t (s,t) are the first 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 derivatives of the rate of change of the rod's position with respect to arc length with respect to space and time, respectively, q s (s,t) and q t (s,t) are the first derivatives of the rod's velocity in the local coordinate system with respect to space and time, respectively, ω. s (s,t) and ω t (s,t) are the first derivatives of the rod's angular velocity in the local coordinate system with respect to space and time, respectively; the superscript ^ denotes a mapping from a three-dimensional vector space to its antisymmetric matrix. Let u(s,t) be the antisymmetric matrix. Let v be 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), and u(s,t) is the initial value of u(s,t). For v s Initial values ​​of (s,t) For u s The initial value of (s,t) is given, where the subscript ext indicates the external load and tendon indicates the action from the rope, i.e., f. ext For external load distribution force, l ext f is the distributed torque of the external load. tendon For the distributed force of the rope, l tendon The distributed torque of the rope is defined as the force and torque applied per unit arc length s, respectively, as l = l ext +l tendon and f = fext +f tendon ρ is the mass density of the robot's main link, A is the cross-sectional area of ​​the robot's main link, J is the moment of inertia matrix, and K... se K is the shear and tensile stiffness matrix. bt Here are the bending and torsional stiffness matrices.

[0017] Furthermore, in step S1, the model of the robot's four ropes is as follows:

[0018] In the global coordinate system, suppose the path of the i-th rope is represented as p. i (s,t) satisfy the following relation:

[0019]

[0020] in This represents the offset of the i-th rope from the center of the cross-section of the robot's main rod in the local coordinate system.

[0021]

[0022] Where x i (s) and y i (s) are the offset functions of the i-th rope on the x-axis and the y-axis from the center of the cross section of the robot's main rod, respectively;

[0023] Assuming internal force and p i The directions (s,t) are tangent. Ignoring friction and inertia, the distributed force f of the rope... tendon and distributed torque l tendon This is obtained through the following relationship:

[0024]

[0025] Where τ i It is the tension on the i-th rope, and n is the number of ropes. For p is The antisymmetric matrix of (s,t), for The antisymmetric matrix, in space for p i Taking the first and second partial derivatives of (s,t) yields:

[0026]

[0027] In the formula p is (s,t) is p i The first derivative of (s,t) with respect to space, p iss (s,t) is p i (s,t) is the second derivative with respect to space; for The first derivative with respect to space, for The second derivative with respect to space; v(s,t) is the rate of change of the rod's position with respect to the arc length, v s (s,t) is the first derivative of the rate of change of the rod position with respect to the arc length with respect to space; Let u(s,t) be the antisymmetric matrix. For u s The antisymmetric matrix of (s,t).

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

[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 h1 represents the imaginary unit, h2, h3, h4 are the coefficients of the corresponding imaginary unit; h1 is the first derivative of the quaternion h with respect to space. s for:

[0031]

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

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

[0034]

[0035] Further, in step S2, the actual position p(L,t) of the robot's end effector is obtained using high-speed cameras installed around the working environment. This is the actual position p of the end effector of the rope-driven flexible continuous robot. x With the expected reference trajectory x d The tracking error e is:

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

[0037] in(·) x p represents the value of a vector on the x-axis. x Let p(L,t) be the value of p(L,t) on the x-axis; the control objective is the actual position p of the end effector of the tethered flexible continuous robot. x Able to track the desired reference trajectory x dThat is, the tracking error e eventually converges to 0.

[0038] Furthermore, in step S3, considering the external disturbances experienced by the robot system, for the system on the x-axis, let the first state variable be x1 = p. x The second state variable is The following state equations for the robot system are obtained:

[0039]

[0040] in Let x1 be the first derivative with respect to time. Let p be the first derivative of x² with respect to time. x Let x be the position of the end of the rod on the x-axis. For p x The first derivative with respect to time, For p x Substituting equations (1) to (5) into equation (10) with respect to the second derivative with respect to time, we obtain:

[0041]

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

[0043] To achieve the control objective described above, 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 out of the four ropes. Therefore, the distributed force f of the ropes... tendon The relationship with the tension of the two ropes is expressed as:

[0044]

[0045] Where α i As an intermediate quantity, α p =[α1 α2] is the midpoint between the two ropes on the positive and negative sides of the x-axis. Consider the two ropes on opposite sides of the x-axis as equivalent to a single rope capable of generating negative tension, where τ1 and τ2 are the tensions on the two ropes.

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

[0047]

[0048] Where e is the actual position of the end effector p of the rope-driven flexible continuous robot. x With the expected reference trajectory x d Tracking error, The first derivative of the tracking error with respect to time, For the desired reference trajectory x d The second derivative with respect to time, λ is an adjustable parameter that satisfies λ > 0; α1 is an intermediate quantity on one side of the positive x-axis, T c For control input, ρ is the mass density of the robot's main link, A is the cross-sectional area of ​​the robot's main link, and d... x The disturbance experienced by the end effector 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. The control law of the other side is obtained due to the symmetry of the entire rope-driven flexible continuous robot.

[0049] To ensure that the tracking error e at the robot's end-effector eventually converges to zero, 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 dynamic equation of the impedance model is:

[0053]

[0054] Where F e In the impedance model, represents the external force acting on the robot system; M, B, and K represent the system's inertial, damping, and stiffness characteristics, respectively; x0, Let x represent the position, velocity, and acceleration of the original tracking trajectory, respectively. d , and These represent the displacement, velocity, and acceleration of the newly generated trajectory of the system under the action of external forces, respectively.

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

[0056] S51. Select the Lyapunov function V:

[0057]

[0058] Where s m It is a sliding surface;

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

[0060]

[0061] in, For the sliding surface s m Relative to the first derivative over time, The first derivative of the tracking error with respect to time, For the desired reference trajectory x d The second derivative with respect to time, ρ is the mass density of the robot's main link, A is the cross-sectional area of ​​the robot's main link, and n is the second derivative with respect to time. s f is the first derivative of the internal force of the rod with respect to space and time. ext For external load distribution force, F e In the impedance model, α1 represents the external force acting on the robot system; the subscript x indicates the value of a vector on the x-axis, α1 is the intermediate quantity on the positive x-axis side, and T... c To control the input, d x Let λ be the disturbance experienced by the end effector of the tethered flexible continuous robot along the x-axis; λ is an adjustable parameter that satisfies λ>0;

[0062] Substituting the control law, we get:

[0063]

[0064] Where k > 0, k and ε are adjustable positive parameters, s m Define the sliding surface. For dx The upper bound value, i.e. This holds true for all adjustable parameters, ε > |d|. x |, We get:

[0065]

[0066] From the above derivation, V≥0 and Furthermore, V is bounded as time t→∞; according to Lyapunov's stability theory and Barbalat's lemma, V is bounded as time t→∞, and s m →0, e→0 and The asymptotic stability of the rope-driven flexible continuous robot system is ensured by the sliding mode impedance system.

[0067] S53. Combining the control law and the analysis of steps S51 and S52, select adjustable parameters. The external disturbance parameter is assumed to be: the disturbance experienced by the end effector of the rope-driven flexible continuous robot. Set the inertial characteristics M, damping characteristics B, and stiffness characteristics 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 robot's working end effector to move. The actual position of the working end effector of the rope-driven flexible continuous robot is obtained by measuring 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 external force, thus achieving the effect of compliant control.

[0071] 2. This invention introduces an impedance model on the basis of the sliding mode controller, which can generate a new reference trajectory according to the external force, thus achieving the effect of compliant control. Attached Figure Description

[0072] Figure 1 This is a flowchart of a trajectory tracking control method for a rope-driven flexible continuous robot based on sliding mode impedance according to the present invention.

[0073] Figure 2 This is a schematic diagram of the structure of the rope-driven flexible continuous robot in an embodiment of the present invention;

[0074] Figure 3 This 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 This is a schematic diagram of trajectory tracking under external force conditions in an embodiment of the present invention;

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

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

[0078] The present invention will be further described in detail below with reference to the embodiments and accompanying drawings, but the embodiments of the present invention are not limited thereto.

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

[0080] S1. Obtain the dynamic model and desired reference trajectory of the robot based on the structure of the rope-driven flexible continuous robot;

[0081] S2. Introduce quaternions to eliminate truncation error; obtain the actual position of the robot's end effector, feed it back to the controller, and subtract it from the expected reference trajectory in step S1 to obtain the tracking error of the robot's end effector position;

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

[0083] S4. Introduce an impedance model to form a sliding mode impedance system; when there is no external force, the robot's end effector 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 adapt to the external force, until the external force disappears and it will resume 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, one segment of a multi-segment rope-driven flexible continuous robot is taken as the research object. The rope-driven continuous robot mainly consists of a central elastic main rod 1, support disks 2 threaded on the elastic main rod 1, and four parallel and symmetrical drive ropes 3. These ropes 3 are connected to the elastic main rod 1 through a series of support disks 2. The mass of the support disks 2 and ropes 3 is negligible, and the friction between the support disks 2 and ropes 3 is also negligible.

[0087] This invention uses Cosserat link theory to model the robot's main link and rope separately, and then couples the two models to obtain the robot's dynamic model.

[0088] S12. The robot's dynamic model is as follows:

[0089]

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

[0091] Where the subscripts s and t represent the first derivatives of the variable with respect to space and time, respectively, i.e., p s (s,t) and p t (s,t) represents the first derivative of the robot's main link's centerline position with respect to space and time, respectively, R. s (s,t) and R t (s,t) represents the first derivatives of the rotation matrix of the robot's main link with respect to space and time, respectively, u s (s,t) and u t (s,t) are the first 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 derivatives of the rate of change of the rod's position with respect to arc length with respect to space and time, respectively, q s (s,t) and q t (s,t) are the first derivatives of the rod's velocity in the local coordinate system with respect to space and time, respectively, ω. s (s,t) and ω t (s,t) are the first derivatives of the rod's angular velocity in the local coordinate system with respect to space and time, respectively; the superscript ^ denotes a mapping from a three-dimensional vector space to its antisymmetric matrix. Let u(s,t) be the antisymmetric matrix. Let v be 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), and u(s,t) is the initial value of u(s,t). For v s Initial values ​​of (s,t) For u sThe initial value of (s,t) is given, where the subscript ext indicates the external load and tendon indicates the action from the rope, i.e., f. ext For external load distribution force, l ext f is the distributed torque of the external load. tendon For the distributed force of the rope, l tendon The distributed torque of the rope is defined as the force and torque applied per unit arc length s, respectively, as l = l ext +l tendon and f = f ext +f tendon ρ is the mass density of the robot's main link, A is the cross-sectional area of ​​the robot's main link, J is the moment of inertia matrix, and K... se K is the shear and tensile stiffness matrix. bt Here are the bending and torsional stiffness matrices;

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

[0093] In the global coordinate system, suppose the path of the i-th rope is represented as p. i (s,t) satisfy the following relation:

[0094]

[0095] in This represents the offset of the i-th rope from the center of the cross-section of the robot's main rod in the local coordinate system.

[0096]

[0097] Where x i (s) and y i (s) are the offset functions of the i-th rope on the x-axis and the y-axis relative to the center of the cross-section of the robot's main rod, respectively.

[0098] Assuming internal force and p i The directions (s,t) are tangent. Ignoring friction and inertia, the distributed force f of the rope... tendon and distributed torque l tendon This is obtained through the following relationship:

[0099]

[0100] Where τ i It is the tension on the i-th rope, and n is the number of ropes. For p is The antisymmetric matrix of (s,t), for The antisymmetric matrix, in space for p i Taking the first and second partial derivatives of (s,t) yields:

[0101]

[0102] In the formula p is (s,t) is p i The first derivative of (s,t) with respect to space, p iss (s,t) is p i (s,t) is the second derivative with respect to space; for The first derivative with respect to space, for The second derivative with respect to space; v(s,t) is the rate of change of the rod's position with respect to the arc length, v s (s,t) is the first derivative of the rate of change of the rod position with respect to the arc length with respect to space; Let u(s,t) be the antisymmetric matrix. For u s The antisymmetric matrix of (s,t).

[0103] As one example, 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 the imaginary units, and h2, h3, h4 are the coefficients of the corresponding imaginary units. The first derivative of the quaternion h with respect to space is hi. s for:

[0108]

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

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

[0111]

[0112] S22. Obtain the actual position p(L,t) of the robot's end effector using high-speed cameras installed around the working environment. (This refers to the actual position p of the end effector of a rope-driven flexible continuous robot.) x With the expected reference trajectory x d The tracking error e is:

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

[0114] in(·) x p represents the value of a vector on the x-axis. x Let p(L,t) be the value of p(L,t) on the x-axis. The control objective is the actual position p of the end effector of the tethered flexible continuous robot. x Able to 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. Considering the external disturbances experienced by the robot system, for the system on the x-axis, let state variable 1 be x1 = p. x State variable 2 is The following state equations for the robot system are obtained:

[0117]

[0118] in Let x1 be the first derivative with respect to time. Let p be the first derivative of x² with respect to time. x Let x be the position of the end of the rod on the x-axis. For p x The first derivative with respect to time, For p x Substituting equations (1) to (5) into equation (10) with respect to the second derivative with respect to time, we obtain:

[0119]

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

[0121] To achieve the control objective described above, 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 out of the four ropes. Therefore, the distributed force f of the ropes... tendon The relationship with the tension of the two ropes is expressed as:

[0122]

[0123] Where α i As an intermediate quantity, α p =[α1 α2] is the midpoint between the two ropes on the positive and negative sides of the x-axis. Consider the two ropes on opposite sides of the x-axis as equivalent to a single rope capable of generating negative tension, where τ1 and τ2 are the tensions on the two ropes.

[0124] S32. Next, based on the control objectives of the robot system state, define the sliding surface s. m and sliding surface s m First derivative in time for:

[0125]

[0126] Where e is the actual position of the end effector p of the rope-driven flexible continuous robot. x With the expected reference trajectory x d Tracking error, The first derivative of the tracking error with respect to time, For the desired reference trajectory x d The second derivative with respect to time, λ is an adjustable parameter that satisfies λ > 0; α1 is an intermediate quantity on one side of the positive x-axis, T c For control input, ρ is the mass density of the robot's main link, A is the cross-sectional area of ​​the robot's main link, and d... x The disturbance experienced by the end effector of the rope-driven flexible continuous robot on the x-axis is described. 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. The control law of the other side is obtained due to the symmetry of the entire rope-driven flexible continuous robot.

[0127] To ensure that the tracking error e at the robot's end-effector eventually converges to zero, 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 impart a degree of controllable compliance to the robot at the control algorithm level. This compliance is like a spring fixed to the robot's end effector, with a controllable stiffness coefficient. Although such a spring does not exist in physical space, this functionality can be achieved using control algorithms. The dynamic equations of the impedance model are as follows:

[0132]

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

[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 It is a sliding surface.

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

[0139]

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

[0141] Substituting the control law, we get:

[0142]

[0143] Where k > 0, k and ε are adjustable positive parameters, s m Define the sliding surface. For d x The upper bound value, i.e. This holds true for all adjustable parameters, ε > |d|. x |, We get:

[0144]

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

[0146] S53. Combining the control law and the analysis of steps S51 and S52, as an example, 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 follows: 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 a sliding mode impedance controller to drive the robot's working end effector to move. The actual position of the working end effector of the rope-driven flexible continuous robot is measured by sensors, and the position tracking error is calculated. Figure 4 This demonstrates the actual tracking trajectory, the new reference trajectory, and the original reference trajectory under the presence of the aforementioned external forces. It shows that when the external force is not zero, the sliding mode impedance controller generates a new reference trajectory based on the applied force. The controller then tracks this new reference trajectory to conform to the external force, achieving compliant control. After the external force disappears, the reference trajectory and the original reference trajectory regain their equal relationship, and the controller resumes tracking the original reference trajectory.

[0148] Figure 5 The controller output of the sliding mode impedance controller can further help in understanding how the sliding mode impedance controller works. Figure 6 The figure shows the deformation of the rod in the xz plane over time under the control of the sliding mode impedance controller. It can be seen that its deformation state is as expected, and the entire rod conforms to the external force F. e The size and direction of the changes were monitored, and the new desired trajectory was eventually correctly tracked, achieving the effect of compliant control.

[0149] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and shall be included within the protection scope of the present invention.

Claims

1. A trajectory tracking control method for a rope-driven flexible continuum robot based on sliding mode impedance, characterized by, The method comprises the following steps: S1, obtaining a dynamics model of the robot and a desired reference trajectory according to the structure of the rope-driven flexible continuous robot; S2, introducing a quaternion to eliminate truncation error, obtaining an actual position of the robot working end, feeding back to the controller, and subtracting the desired reference trajectory in step S1 to obtain a tracking error of the robot working end position; S3, designing a sliding mode according to the tracking error information of the position, and selecting an approach rate to make the position tracking error reach the sliding mode; According to the dynamics model and the desired trajectory of the robot, a control law of the robot trajectory tracking controller is designed, and the control parameters are determined; S4, introducing an impedance model to form a sliding mode impedance system; when no external force exists, the robot working end tracks the original desired trajectory; when an external force exists, the impedance model generates a new reference trajectory and tracks the new reference trajectory to comply with the external force, and after the external force disappears, it will return to track the original reference trajectory; S5, analyzing the stability and convergence of the sliding mode impedance system, adjusting the control parameters of the trajectory tracking controller, and outputting the trajectory tracking result, comprising the following steps: S51, selecting a Lyapunov function : (1)； wherein is a slide surface; S52, first derivative of Lyapunov function , we get: (2)； wherein, is the sliding surface is the first order derivative of the tracking error with respect to time, is the first order derivative of the tracking error with respect to time, is the desired reference trajectory is the second order derivative of the tracking error with respect to time, is the mass density of the robot main rod, is the cross-sectional area of the robot main rod, is the first order derivative of the internal force of the rod with respect to space and time, is the external load distribution force, is the external force acting on the robot system in the impedance model; subscript x denotes the value of a vector on axis, is the positive axis side intermediate quantity, is the control input, is the disturbance on the rope-driven flexible continuum robot end in x axis; is an adjustable parameter, and satisfies ; Substitute the control law into to obtain: (3)； where , and are adjustable positive parameters, is a sliding surface defined by is an upper bound of , i.e. always holds for adjustable parameters , take , we get: (4)； From the above derivation and , and when time , is bounded; according to Lyapunov stability theory and Barbalat lemma, it is obtained that when time , is bounded, while , and , under the action of the sliding mode impedance system, the asymptotic stability of the rope-driven flexible continuum robot system is ensured; S53, in combination with the control law and the analysis of step S51 and step S52, select the adjustable parameters, the external disturbance parameters are assumed that the disturbance received by the end of the rope-driven flexible continuous robot , set the inertia characteristics of the impedance model , the damping characteristics and the stiffness characteristics ; S54, the rope-driven flexible continuous robot outputs the rope tension under the action of the sliding mode impedance system, drives the robot working end to move, measures the actual position of the robot working end of the rope-driven flexible continuous robot through the sensor, and calculates the position tracking error.

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

3. The trajectory tracking control method based on sliding mode impedance for a rope-driven flexible continuum robot according to claim 2, wherein, In step S1, the dynamics model of the robot is as follows: (5)； wherein is a spatial position variable, i.e. the main rod arc length, L is the length of the main rod, is a time variable, is the center line position of the robot main rod, is the rotation matrix of the robot main rod, is the curvature vector of the rod in the local coordinate system, is the rate of change of the rod position with respect to the arc length, is the velocity of the rod in the local coordinate system, is the angular velocity of the rod in the local coordinate system; where the subscript and denote the first order derivative of the variable with respect to space and time, respectively, i.e. and are the first order derivative of the centerline position of the robot's main rod with respect to space and time, respectively, and are the first order derivative of the rotation matrix of the robot's main rod with respect to space and time, respectively, and are the first order derivative of the curvature vector in the local coordinate system with respect to space and time, respectively, and are the first order derivative of the rate of change of the rod position with respect to arc length with respect to space and time, respectively, and are the first order derivative of the velocity of the rod in the local coordinate system with respect to space and time, respectively, and are the first order derivative of the angular velocity of the rod in the local coordinate system with respect to space and time, respectively; the superscript denotes the mapping of a three-dimensional vector space to its skew-symmetric matrix, is the skew-symmetric matrix of , is the skew-symmetric matrix of ; the superscript denotes the initial value of the variable, is the initial value of , is the initial value of ; the subscript denotes the external load, denotes the action from the rope, i.e. is the external load distributed force, is the external load distributed moment, is the distributed force of the rope, is the distributed moment of the rope, per unit arc length the force and moment applied are defined as and , is the mass density of the robot's main rod, is the cross-sectional area of the robot's main rod, is the moment of inertia matrix, is the shear and tensile stiffness matrix, is the bending and torsional stiffness matrix.

4. The trajectory tracking control method based on sliding mode impedance for a rope-driven flexible continuum robot according to claim 3, wherein, In step S1, the model of the four ropes of the robot is as follows: In the global coordinate system, assume that the first The path of the root string is represented as satisfies the following relationship: (6)； wherein represents the offset of the root cord from the cross-sectional center of the robot main shaft in the local coordinate system: (7)； wherein and are respectively x the offset of the root of the rope from the cross-sectional center of the robot main rod as a function of the offset of the root of the rope from the cross-sectional center of the robot main rod as a function of Assuming that the internal force is tangent to the direction of the rope, and neglecting friction and inertia, the distributed force and distributed moment are obtained by the following relations: (8)； wherein is the first is the tension on the root rope, n is the number of ropes, is the anti-symmetric matrix of is the anti-symmetric matrix of is the anti-symmetric matrix of is the anti-symmetric matrix of The first and second order partial derivatives of the spatial function (9)； wherein is the first derivative with respect to space, is the second derivative with respect to space; is the first derivative with respect to space, is the second derivative with respect to space; is the rate of change of the rod position with respect to arc length, is the first derivative with respect to space of the rate of change of the rod position with respect to arc length; is the skew-symmetric matrix of is the skew-symmetric matrix of 5.The trajectory tracking control method based on sliding mode impedance for a rope-driven flexible continuum robot according to claim 4, wherein, In step S2, the quaternion is introduced is: (10)； wherein is the real part of the quaternion is the imaginary unit, is the imaginary unit, is the coefficient corresponding to the imaginary unit; the quaternion is the first derivative of the space is: (11)； wherein , are respectively in the curvature component of the axis; Rotation matrix is represented as: (12)。 6.The trajectory tracking control method based on sliding mode impedance for a rope-driven flexible continuum robot according to claim 5, wherein, In step S2, the actual position of the robot end is obtained by means of high-speed cameras installed around the work environment , the tracking error of the actual position of the rope-driven flexible continuous robot end and the desired reference trajectory is : (13)； wherein represents the value of a vector on axis, i.e. is the value on axis; the control target is the actual position of the end of the rope-driven flexible continuous robot can track the desired reference trajectory , i.e. the tracking error converges to 0 eventually.

7. The trajectory tracking control method based on sliding mode impedance for a rope-driven flexible continuum robot according to claim 6, wherein, In step S3, considering the external disturbance to the robot system, for x the system on the axis, let the first state variable be , and the second state variable be , the following state equation of the robot system is obtained: (14)； wherein is the first derivative with respect to time, is the first derivative with respect to time, wherein is the position of the end of the rod in the axis, is the first derivative with respect to time, is the second derivative with respect to time, substituting equations (5) through (9) into equation (14) gives: (15)； where is the first derivative of the internal force of the rod with respect to space and time, is the disturbance experienced by the end of the rope-driven flexible continuum robot, is the external force acting on the robot system in the impedance model; subscript denotes the value of a vector on axis, is the rotation matrix of the robot main rod, , is the first derivative of the rotation matrix of the robot main rod with respect to time, is the velocity of the rod in the local coordinate system, is the first derivative of the velocity of the rod in the local coordinate system with respect to time, is the skew-symmetric matrix of ; is the angular velocity of the rod in the local coordinate system, is the mass density of the robot main rod, is the cross-sectional area of the robot main rod, is the shear and tensile stiffness matrix, is the bending and torsional stiffness matrix; is the initial value of , is the rate of change of the rod position with respect to the arc length, is the skew-symmetric matrix of , is the first derivative of the curvature vector in the local coordinate system with respect to space, is the external load distribution force, is the distribution force of the rope; To achieve the above control objectives, since it only considers the control of the tracking problem on the axis, only two of the four ropes on the axis side need to be controlled, and the distribution force of the rope is related to the tension of the two ropes. (16)； wherein is the intermediate quantity, is is the intermediate quantity of the two ropes on either side of the axis, is considered relative to is the result of the two ropes on either side of the axis being equivalent to one rope that can achieve negative tension, , is the tension on the two ropes.

8. The trajectory tracking control method based on sliding mode impedance for a rope-driven flexible continuum robot according to claim 7, wherein, In step S3, a sliding surface is defined in accordance with the control target of the robot system state and the sliding surface is a first order derivative in time is (17)； wherein is the actual position of the rope-driven flexible continuum robot's end-effector, is the tracking error from the desired reference trajectory, is the first derivative of the tracking error with respect to time, is the second derivative of the desired reference trajectory with respect to time, is the second derivative of the desired reference trajectory with respect to time, is a tunable parameter, and satisfies ; is a positive is the control input, is the mass density of the robot's main rod, is the cross-sectional area of the robot's main rod, is the disturbance experienced by the rope-driven flexible continuum robot's end-effector in x the z-axis; The ropes of the rope-driven flexible continuum robot are arranged in the axis and the positive and negative sides of the axis, only the control law of the positive side is considered when designing the controller, and the control law of the other side is obtained due to the symmetrical relationship in the whole rope-driven flexible continuum robot. To track the position of the robot work end Converges to zero eventually, the control law is (18)； wherein , and are adjustable positive parameters, is a sign function.

9. The trajectory tracking control method based on sliding mode impedance for a rope-driven flexible continuum robot according to claim 8, wherein, In step S4, the dynamics equation of the impedance model is: (19)； wherein is the external force acting on the robot system in the impedance model; , , represent the inertia, damping and stiffness characteristics of the system, respectively; , , represent the position, velocity and acceleration of the original tracking trajectory, respectively, , and represent the displacement, velocity and acceleration of the newly generated trajectory of the system under the external force, respectively.

Images