A dynamics-based method for sensing external forces at the end of a rope-driven flexible continuous robot

Abstract

The present invention provides a method for sensing external force at the end of a rope-driven flexible continuous robot based on dynamics, which belongs to the field of intelligent sensing of rope-driven continuous robots. The specific steps are as follows: first, establish a unit formula of various components; second, establish a dynamic DAEs model for sensing external force of a rope-driven flexible continuous robot; then, respectively measure and obtain the driving length of the rope and the actuating force of the rope driving point; then, use the measured feedback of the rope driving length and the actuating force of the driving point to solve the DAEs model to obtain the current external force at the end; finally, update the system state variables, and execute the above measurement and solution in a loop to complete the entire sensing task. The sensing method of the present invention does not need to be equipped with a large number of complex sensor devices such as vision, inertial navigation, and fiber grating. It only needs to measure the actuating length and actuating force information of the driving rope, which can enable the robot to better adapt to a narrow and unstructured working environment; it can sense and estimate the time-varying external force at the end of the robot in real time and accurately.

Description

Technical Field

[0001] The invention belongs to the field of intelligent perception of a rope-driven flexible continuous robot, and relates to a method for sensing external force at the end of a rope-driven flexible continuous robot based on dynamics. Background Art

[0002] Flexible continuous robots have gradually shown their unique advantages in applications in narrow and restricted unstructured space scenarios due to their slender body structure, smooth continuous deformation and excellent environmental adaptability. In such scenarios, intelligent perception is an important technical guarantee for continuous robots to achieve goals such as obstacle avoidance, fine control, and human-computer interaction. This paper studies the problem of external force perception at the end of a rope-driven flexible continuous robot.

[0003] At present, direct measurement is a common method for sensing external forces in robots. This method measures external forces by placing sensors directly on the surface of the robot, without the need for a complex modeling process, and is relatively simple to implement. For example, the Chinese patent [CN 116276961 A] provides a multi-source fusion sensing system and a sensing method for a space dexterous manipulator, which uses sensors such as vision to realize the perception of the manipulator's own and external information. However, due to its slender structural form, the rope-driven flexible continuous robot is difficult to carry traditional sensing equipment such as torque, vision, and inertial navigation. In recent years, small tactile sensors and flexible electronic devices have shown great application potential in the field of external force perception of continuous robots, but these precise and advanced sensors usually have high requirements for environmental conditions such as temperature, humidity, and corrosiveness, and the preparation process is complicated and cumbersome, and the cost is high. In addition, these sensors also face problems with system stability and reliability, and it is currently difficult to achieve effective integration with rope-driven continuous robots.

[0004] To overcome the above problems, some scholars have adopted an indirect method to estimate the external load of the rope-driven continuous robot. This method aims to use external devices or embedded devices to measure the robot's configuration, and then estimate the external load through calculation and deduction. However, this indirect measurement method also faces many problems in practical applications. On the one hand, for external configuration measurement equipment, it usually needs to be arranged in a larger space and is difficult to use in a confined and unstructured space. On the other hand, for embedded sensor devices such as fiber Bragg gratings, it may sacrifice the robot's movement flexibility, and such devices are easily damaged after repeated use, resulting in reduced accuracy. To solve the above two problems, the Chinese patent [CN 115890673 A] provides a rope-driven flexible manipulator external force sensing method based on a force (or torque) balance equation model, which effectively reduces the dependence of the rope-driven flexible manipulator sensing system on sensors. However, the perception model established by this method has not yet taken into account the elastic deformation of the various components of the robotic arm and the slack of the driving rope, which will affect the accuracy of external force perception; in addition, the DH coordinate system description method adopted by this method is suitable for robotic arms with rigid joints in series, but it is difficult to extend to continuous robots with flexible joint connections, such as the one designed in the present invention.

[0005] Therefore, there is an urgent need to further study a method for external force perception of a rope-driven flexible continuous robot that has less reliance on sensor devices, high precision and a wide range of applications, in order to improve its intelligence level. Summary of the invention

[0006] In response to the above technical problems, the present invention proposes a method for sensing external force at the end of a rope-driven flexible continuous robot based on dynamics. Based on the position finite element and complementary theory, the method establishes the dynamic differential algebraic equation of the rope-driven flexible continuous robot system. It only needs to embed a tension sensor at the base end of the rope-driven flexible continuous robot without changing the structure of the rope-driven flexible continuous robot. By relying on the feedback of the actuating force and actuating length information driven by the rope, the change of the interaction force between the end of the rope-driven flexible continuous robot and the outside world can be accurately and efficiently sensed. The proposed method has low dependence on sensor devices and does not change the main structural design of the rope-driven flexible continuous robot, which provides a new idea for solving the problem of weak perception ability of the rope-driven flexible continuous robot.

[0007] In order to achieve the above object, the technical means adopted by the present invention are as follows:

[0008] A method for sensing external force at the end of a rope-driven flexible continuous robot based on dynamics is provided. The method for sensing external force at the end of a rope-driven flexible continuous robot is as follows: first, according to the basic component types contained in the rope-driven flexible continuous robot, unit formulas of various components of the rope-driven flexible continuous robot are established; second, motion constraints and actuation constraints of the driving rope are introduced, and the actuation force and driving length of the rope are used as inputs of the dynamics system, and the external force at the end of the rope-driven flexible continuous robot, the internal force of the body structure and the position information are used as outputs. The unit formulas of various components are combined to establish dynamic differential-algebraic equations (Differential-AlgebraicEquations, A dynamic DAEs model system is constructed, that is, a dynamic DAEs model suitable for external force perception of a rope-driven flexible continuous robot is constructed; then, the encoder of the servo motor is used to measure the driving length of the rope, and the tension sensor embedded in the base end of the rope-driven flexible continuous robot is used to measure the actuation force of the rope driving point; then, the measured rope driving length and actuation force are fed back to the dynamic DAEs model, and the generalized-α method is used to solve the dynamic DAEs model to estimate the current terminal external force of the rope-driven flexible continuous robot; finally, the dynamic DAEs model state variables are updated, and the above measurement and solution are performed cyclically until the perception task is completed. It has the following steps:

[0009] The first step is to establish the unit formula of various components of the rope-driven flexible continuous robot based on position finite element and complementarity theory.

[0010] like Figure 2 As shown in FIG. 1 , the rope-driven flexible continuous robot includes four types of components: rods, springs, rings, and active drive ropes. The dynamic unit formulas of each component are derived below.

[0011] Step 1.1, for the rod element: its generalized coordinates It can be represented by the unit node position in the global coordinate system:

[0012] (1)

[0013] in, and is the position coordinate vector of the nodes at both ends of the unit; , , and , , They are and Coordinate components in the Cartesian global coordinate system. The superscript T indicates the transpose operation of a vector or matrix.

[0014] Then the position coordinates of any point on the rod element are It is expressed as:

[0015] (2)

[0016] in, C x = [ x 1 − x ] ⊗ I 3 , represents the mapping matrix between the generalized coordinates of the rod element and the coordinates of any point of the rod element; x ∈ [ 0 , 1 ] is the natural coordinate; I 3 is the three-dimensional identity matrix; is the Kronecker product.

[0017] Assuming that the bar element satisfies the small strain assumption, its generalized internal force vector for:

[0018] f r = [ − f r ' l ^ r T f r ' l ^ r T ] (3)

[0019] in, is the axial unit direction vector of the rod element, is the magnitude of the elastic internal force, that is:

[0020] (4)

[0021] in, is the elastic modulus of the rod element, is the cross-sectional area of ​​the rod element, is the initial length of the bar element, is the rod unit length.

[0022] Furthermore, the linear density of the rod element is defined as , then the kinetic energy of the rod element It is expressed as:

[0023] (5)

[0024] in, represents the mass of the bar element; represents the generalized velocity of the rod element; represents the generalized velocity of any point on the rod element; Represents the length element in the natural coordinate system.

[0025] According to the above expression (5), the generalized mass matrix of the rod element can be obtained for:

[0026] M r = m r ∫ 0 1 C x T C x d x = m r 6 [ 2 I 3 I 3 I 3 2 I 3 ] (6)

[0027] So far, the generalized coordinates (shown in formula (1)), generalized internal forces (shown in formula (3)) and generalized mass matrix (shown in formula (6)) of the rod element have been derived.

[0028] Step 1.2: For the spring element, it can be regarded as a rod element without pressure stiffness. The derivation process of its element formula is the same as that of the rod element. When subjected to pressure, let the generalized internal force vector in formula (3) be =0.

[0029] Step 1.3, for the ring unit, its generalized coordinates It can be described by the vertex position of the inscribed equilateral triangle of the ring, that is:

[0030] q t = [ R 1 T R 2 T R 3 T ] T = [ x 1 , y 1 , z 1 , x 2 , y 2 , z 2 , x 3 , y 3 , z 3 ] T (7)

[0031] in, (i = 1, 2, 3) represents the coordinate vectors of the three vertices of the inscribed equilateral triangle of the ring; (i = 1, 2, 3) represents the coordinate components in the Cartesian global coordinate system.

[0032] Any point on the ring The generalized coordinates of It can be described as:

[0033] (8)

[0034] in, represents the generalized position coordinates of the center of the ring, Represents the center of the ring The vector to any point on the ring can be expressed as:

[0035] O ' N i ⃗ = 2 3 [ c o s ( i ) ⋅ s 1 + c o s ( i + 2 π 3 ) ⋅ s 2 + c o s ( i + 4 π 3 ) ⋅ s 3 ] (9)

[0036] (10)

[0037] in, express The angle with the x direction of the global coordinate system; Indicates the center of a ring The vector pointing to the i-th vertex of the equilateral triangle inscribed in the torus.

[0038] The exact load-deformation (stress-strain) curve on the ring is theoretically nonlinear, but it can be approximately linearized under the assumption of small deformation. Therefore, the internal force vectors of the three equivalent rod elements that form the equilateral triangle inscribed in the ring can be expressed as , and (Formulas (3) and (4)) are used to assemble the finite element to obtain the approximate equivalent generalized internal force vector of the ring element.

[0039] f t = [ ( − f t ( 1 ) l ^ t ( 1 ) + f t ( 3 ) l ^ t ( 3 ) ) T , ( f t ( 1 ) l ^ t ( 1 ) − f t ( 2 ) l ^ t ( 2 ) ) T , ( − f t ( 3 ) l ^ t ( 3 ) + f t ( 2 ) l ^ t ( 2 ) ) T ] T (11)

[0040] in, , and are the unit direction vectors of the equivalent rod unit inscribed in the ring, It can be specifically expressed as:

[0041] (12)

[0042] in, is the Young's modulus of the inscribed equivalent rod element, is the cross-sectional area of ​​the equivalent rod element, is the initial length of the equivalent bar element in the stress-free state, represents the length of the kth equivalent rod unit after elastic deformation. Therefore, acting on any point on the ring The concentrated force on the Get the effect on the node , and The equivalent concentrated force on It can be expressed as:

[0043] C i = ∂ R i ∂ q t = [ ∂ R i ∂ R 1 ∂ R i ∂ R 2 ∂ R i ∂ R 3 ] (13)

[0044] in,

[0045] ∂ R i ∂ R 1 = [ 4 9 c o s ( i ) − 2 9 ( c o s ( i + 2 π 3 ) + c o s ( i + 4 π 3 ) ) + 1 3 ] ⋅ I 3 ∂ R i ∂ R 2 = [ 4 9 c o s ( i + 2 π 3 ) − 2 9 ( c o s ( i ) + c o s ( i + 4 π 3 ) ) + 1 3 ] ⋅ I 3 ∂ R i ∂ R 3 = [ 4 9 c o s ( i + 4 π 3 ) − 2 9 ( c o s ( i + 2 π 3 ) + c o s ( i ) ) + 1 3 ] ⋅ I 3 (14)

[0046] Further ignoring the rotation effect of the ring itself, the generalized mass matrix can be derived based on the kinetic energy of the ring unit for:

[0047] (15)

[0048] in, represents the linear density of the ring; represents the radius of the ring; Represents a 9-dimensional unit vector.

[0049] So far, the generalized coordinates (shown in formula (7)), generalized internal forces (shown in formula (11)) and generalized mass matrix (shown in formula (15)) of the ring element have been derived.

[0050] Step 1.4, for the active drive rope unit: it can be regarded as a super unit connected in series with a series of rod units. Consider that the active drive rope passes through n-1 pulleys, n segments, and a total of n+1 nodes. Then, the generalized coordinates of the drive rope unit are for:

[0051] q c = [ R 1 T R 2 T ⋯ R n T R n + 1 T ] T = [ x 1 , y 1 , z 1 , ⋯ , x n + 1 , y n + 1 , z n + 1 ] T (16)

[0052] Its generalized internal force vector With the generalized mass matrix The internal force vectors of all rope segments can be and the mass matrix Finite element assembly is performed to obtain:

[0053] f c = [ T c ( 1 ) l ^ c ( 1 ) T , − T c ( 1 ) ( l ^ c ( 1 ) − l ^ c ( 2 ) ) T , ⋯ , − T c ( n ) ( l ^ c ( n − 1 ) − l ^ c ( n ) ) T , − T c ( n ) l ^ c ( n ) T ] T (17)

[0054] M c = ∪ k = 1 n M c ( k ) = [ 2 M 1 M 1 M 1 2 M 1 + 2 M 2 M 2 M 2 ⋯ … ⋯ 2 M n − 1 + 2 M n M n M n 2 M n ] (18)

[0055] in, is the unit direction vector of the i-th segment of the driving rope, i=1, 2, …, n; , and are the total length of the current driving rope and the length of the i-th section, respectively, i=1, 2,…, n.

[0056] Step 1.5: For the active drive rope described in step 1.4, further consider the friction problem between the active drive rope and the threading hole contact point. The details are as follows:

[0057] like Figure 3 As shown in the figure, due to the friction between the active drive rope and the threading hole contact point, this will cause the internal force of each section of the drive rope to be different. According to the capstan equation, the internal force of the active drive rope on both sides of the kth ring unit is and The following relations are satisfied:

[0058] (19)

[0059] in, It is the driving force of the motor end of the driving rope; is the total number of ring units; To drive the rope and The friction coefficient of the rings depends on the material parameters and the relative sliding speed; is the included angle of the driving ropes on both sides of the ring; is the force magnitude of the driving point connected to the motor. Formula (19) can be further rewritten as:

[0060] (20)

[0061] in, For the Rope force on a disk Relative to the motor end force The attenuation coefficient.

[0062] Step 1.6: For the active drive rope described in step 1.4, further consider the possible relaxation problem during the movement of the rope-driven flexible continuous robot. The details are as follows:

[0063] like Figure 3 As shown, the relaxation factor variable is introduced :

[0064] (twenty one)

[0065] in, , and are the initial length, deformation and current length of the active drive rope. , actively drive the rope to relax, at this time the driving force at the motor end ;when When the rope is tightened, Then according to the complementarity theory, and Satisfies the linear complementary relationship, that is:

[0066] (twenty two)

[0067] For the linear complementarity problem shown in formula (22), the Fischer-Burmeister (abbreviated as: FB) function can be used to further transform it into a nonlinear complementarity problem for solution:

[0068] (twenty three)

[0069] in, It is the FB function; is a set constant, which is taken as 1 in the present invention; Indicates a constant small amount.

[0070] So far, the generalized coordinates (shown in formula (16)), generalized internal forces (shown in formula (17)) and generalized mass matrix (shown in formula (18)) of the actively driven rope unit considering friction and relaxation have been derived.

[0071] In the second step, constraints are introduced to establish the dynamic DAEs model of the rope-driven flexible continuous robot system.

[0072] Step 2.1, first, refer to the multi-body dynamics modeling theory and use the driving length of the rope as the control input to establish the motion constraint equation of the system. Compared with rope tension control, this method of using rope displacement as the control quantity is more conducive to the realization of the motor, because for most commercial motors, it is difficult to accurately output control force or torque after the reducer, but the motor's built-in encoder can directly measure the displacement and is not affected by the rope material and the dynamic characteristics of the rope-driven flexible continuous robot. Then, the movement of each driving rope must meet the following constraints:

[0073] (twenty four)

[0074] in, represents the constraint function of the active drive rope; is the actual measured actuation length of the active drive rope end; is the desired control input.

[0075] Step 2.2, then, consider the unknown force in three-dimensional space borne by the end point of the rope-driven flexible continuous robot , the actuating force of the rope segment between the first ring and the motor actuation point. Three unknown quantities will be added to the rope-driven flexible continuous robot motion system. In order to realize the perception and estimation of the external force magnitude of the unknown end point, the present invention will introduce the following actuating force constraint conditions into the dynamic model of rope-driven flexible continuous robot perception by measuring the real tension of the three driving ropes: :

[0076] (25)

[0077] in, It is the real measured feedback value of the power at the motor end of the driving rope; for the current moment; Indicates the estimated value of the driving force of the rope motor at the current moment.

[0078] Step 2.3, finally, the generalized internal forces and generalized mass matrices of the component units of the rope-driven flexible continuous robot established in the first step are assembled by finite element method, and the constraints of formula (24) and formula (25) are considered to establish the dynamic DAEs model for external force perception at the end of the rope-driven flexible continuous machine with rope friction and relaxation:

[0079] (26)

[0080] in, is the generalized coordinate vector of the rope-driven flexible continuous robot; is the generalized velocity vector of the rope-driven flexible continuum robot; is the generalized acceleration vector of the rope-driven flexible continuum robot; is the external force vector acting on the end point of the rope-driven flexible continuous robot; is the generalized mass matrix of the system; is the generalized internal force of the system, including the internal force and gravity of each unit; is the number of nodes. is the motion constraint condition for the driving rope, as shown in formula (24); is the Lagrange multiplier; is the number of driving ropes. is the force constraint condition of the driving rope, as shown in formula (25); is the number of driving ropes whose tension is measured, and . is the nonlinear complementary constraint equation for rope relaxation, as shown in formula (23).

[0081] The perception model shown in the above formula (26) takes into account the friction and relaxation non-smooth effects of the driving rope, and is a dynamic DAEs model for external force perception at the end of a rope-driven flexible continuous robot.

[0082] The third step is to measure feedback and obtain the current driving state information of the active drive rope.

[0083] like Figure 4 Before using the second step of the dynamic differential algebraic equations model (26) to perform external force perception calculations at the end of the rope-driven flexible continuous robot, it is necessary to measure and collect the current actuation force and actuation length of the driving rope. Specifically, one end of the tension sensor is connected to the active driving rope, and the other end is connected to the servo motor. When the servo motor receives the desired rope driving length command and executes it, its internal encoder collects the current actuation length of the active driving rope, and the actuation length of the current driving point of the active driving rope is synchronously collected through the tension sensor, and the collected data is fed back to the dynamic DAEs model. Furthermore, by solving the dynamic DAEs model, the external force vector f at the end of the rope-driven flexible continuous robot at the current moment can be obtained. ex Here, it should be pointed out that the dynamic DAEs model shown in formula (26) can not only realize the external force perception of the end of the rope-driven flexible continuous robot, but also obtain the configuration and structural internal force information of the rope-driven flexible continuous robot, thus realizing the multimodal perception of the rope-driven flexible continuous robot with very few sensor equipment requirements.

[0084] The fourth step is to solve the dynamic DAEs model of external force perception.

[0085] Using the current actuation length of the active drive rope and the actuation length of the current driving point of the active drive rope measured in the third step, the external force at the end of the rope-driven flexible continuous robot is obtained by solving the dynamic DAEs model shown in formula (26).

[0086] Step 4.1: First, the generalized-α method is used to numerically discretize the dynamic DAEs model shown in formula (26). Moment-Dynamics DAEs Model for:

[0087] g ( x n + 1 ) = [ M q ¨ n + 1 + F q n + 1 T l n + 1 − ( f in ( q n + 1 , q ˙ n + 1 ,( T M ) n + 1 ) + ( f ex ) n + 1 ) F ( q n + 1 , t ) ϒ ( ( T M ) n + 1 ) P ( q n + 1 ,( T M ) n + 1 ) ] = 0 (27)

[0088] in, , and Respectively represent t n+1 Generalized coordinates, generalized velocity and generalized acceleration of the moment-rope-driven flexible continuous robot; Indicates t n+1 Lagrange multiplier at time; Indicates t n+1 The general internal force of the moment; Indicates t n+1 The driving force of the rope motor end at all times; the unknown variables of (27) are x n + 1 = [ q n + 1 , q ˙ n + 1 , q ¨ n + 1 , l n + 1 , ( T M ) n + 1 , ( f ex ) n + 1 ] T ; , and Sequentially represents t n+1 The motion constraints of the driving rope, the force constraints of the driving rope and the nonlinear complementary constraint equations of the rope slack at all times; and and They are:

[0089] (28)

[0090] (29)

[0091] in, is the integration step length, is an auxiliary variable of the generalized-α method, and the vector parameter , The following relationship is satisfied:

[0092] (30)

[0093] in, represents the initial value of the auxiliary variable of the generalized-α method; are algorithm parameters, and:

[0094] (31)

[0095] in, r ∈ [ 0 1] is the spectral radius of the generalized-α method.

[0096] Step 4.2, then, at each time step, use x ^ n + 1 = [ q n + 1 , l n + 1 , ( T M ) n + 1 , ( f ex ) n + 1 ] T As the quantity to be determined, the classic Newton-Raphson algorithm is used to iteratively solve the problem. The iterative format is as follows:

[0097] (32)

[0098] in, is the imbalance vector obtained according to the value of the current iteration step, is the iteration increment value, is the Jacobian matrix of the current iteration. , that is, the residual converges to the specified range (in the invention Specifically, 1×10 -6 ), the estimated external force vector at that moment can be obtained. The specific expression is:

[0099] J n + 1 ( k ) = [ ∂ ( M q ¨ n + 1 ) ∂ q n + 1 + ∂ ( F q n + 1 T l n + 1 ) ∂ q n + 1 − ∂ f n + 1 ∂ q n + 1 F q n + 1 T − ∂ f n + 1 ∂ ( T M ) n + 1 − ∂ f n + 1 ∂ ( f ex ) n + 1 F q n + 1 0 0 0 0 0 ∂ ( ϒ ( ( T M ) n + 1 ) ) ∂ ( T M ) n + 1 0 ∂ ( P ( q n + 1 , ( T M ) n + 1 ) ) ∂ q n + 1 0 ∂ ( P ( q n + 1 , ( T M ) n + 1 ) ) ∂ ( T M ) n + 1 0 ] (33)

[0100] in, After finding the increment value of the current iteration step, the unknown variables of the dynamic DAEs model can be updated as follows:

[0101] (34)

[0102] in, and are the process quantities to be solved, , .

[0103] At this point, complete t n+1 The task of sensing external force at the end of a rope-driven flexible continuous robot.

[0104] The fifth step is to determine whether the external force estimation at all discrete moments has been completed. If it has been completed, the calculation ends; if not, the unknown variables of the system shown in formula (34) are updated, and the third and fourth steps are returned to execute, and the cycle is repeated until the entire perception task is completed.

[0105] The beneficial effects of the present invention are:

[0106] (1) The present invention provides a method for sensing external force at the end of a rope-driven flexible continuous robot based on dynamics. Compared with the traditional force sensing strategy of a rope-driven flexible continuous robot, it does not need to carry a large number of complex sensors such as vision, inertial navigation, and fiber grating, and only needs to measure the actuation length and actuation force information of the driving rope. This new sensing strategy has low dependence on sensors and does not change the main structural design of the rope-driven flexible continuous robot, which can make the rope-driven flexible continuous robot better adapt to narrow and unstructured working environments.

[0107] (2) The present invention establishes a dynamic DAEs model for external force perception of a rope-driven flexible continuous robot. Compared with static or kinematic models, it can more accurately restore the deformation and motion laws of flexible structures (such as springs, elastic rods, ropes, etc.) in a real rope-driven flexible continuous robot system because it takes into account the inertial effect of component movement. It can also perceive and estimate the time-varying external force at the end of the rope-driven flexible continuous robot in real time. In addition, the established dynamic DAEs model takes into account the friction and relaxation of the driving rope, further improving the perception accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0108] Figure 1 It is a specific flow chart for implementing the present invention.

[0109] Figure 2 It is a model diagram of the rope-driven flexible continuous robot used in the present invention, wherein: the left figure is the rope-driven flexible continuous robot, and the Ⅰ, Ⅱ, Ⅲ, Ⅳ, Ⅴ, Ⅵ marked on the top represent 6 active driving ropes; the top right side is the active driving rope unit, the left figure in the middle part of the right side is the rod unit, the right figure is the spring unit, and the bottom right side is the ring unit.

[0110] Figure 3 It is the process of building a dynamic model for external force perception.

[0111] Figure 4 Schematic diagram of friction analysis of the active drive rope.

[0112] Figure 5 Schematic diagram of the slack analysis of the actively driven rope.

[0113] Figure 6 This is a schematic diagram of the static steady load perception experiment achieved by weight loading at the end of a rope-driven flexible continuous robot.

[0114] Figure 7 It is a comparison curve between the perceived estimated value and the actual measured value of the external force under different static constant loads;

[0115] Figure 8 is the external force perception error of different static steady loads.

[0116] Figure 9 This is a schematic diagram of the time-varying load sensing experiment achieved by a three-axis force sensor at the end of a rope-driven flexible continuous robot.

[0117] Figure 10 It is the real-time external force perception time curve.

[0118] Figure 11 It is the error time history curve of real-time external force perception. DETAILED DESCRIPTION

[0119] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0120] by Figure 2 The rope-driven flexible continuous robot shown in the figure is the research object. Figure 1 The implementation steps of the process are used to verify the method involved in the present invention. Figure 2 The design concept of the rope-driven flexible continuous robot shown is derived from the tensegrity structure, which is composed of rod and ring pressure-bearing components, spring tension components and actively driven ropes. It can achieve a large range of contraction and continuous bending movements. The overall performance is a complex rigid-flexible coupling nonlinear system, which has the representative characteristics of a rope-driven flexible continuous robot. This kind of rope-driven flexible continuous robot is difficult to carry complex sensor devices such as vision, inertial navigation, and fiber grating due to its slender body structure, and its perception ability is relatively weak. In order to solve this problem, the present invention proposes a dynamics-based external force perception method for a rope-driven flexible continuous robot. Figure 3 As shown in the figure, the proposed method can realize the real-time perception and size estimation of the external force at the end of the rope-driven flexible continuous robot by establishing a dynamic DAEs model of external force perception of the rope-driven flexible continuous robot, only by measuring the state information of the driving rope, and without destroying the main body structure of the rope-driven flexible continuous robot itself. Using the method of the present invention, the static constant load perception and dynamic time-varying load perception of the end of the rope-driven flexible continuous robot are used as examples for experimental verification.

[0121] First, in order to verify the terminal external force perception method of the rope-driven flexible continuous robot proposed in this invention, a perception experimental platform was built. The experimental platform consists of a rope-driven flexible continuous robot, a servo motor drive module, a motion capture system for sensing result verification, and a control end computer. Among them, the rope-driven flexible continuous robot is 0.7m long and contains 14 ring units, 39 rod units, 39 spring units, and 6 active drive ropes. The active drive ropes can be divided into two groups, one group of active drive ropes runs from the base to the end, and the other group of active drive ropes runs from the base to the eighth ring. The drive module is composed of 6 servo motors (SV-X6MN002A-N6PA hcfa, China) and 6 S-type sensors (AT8301 AUTODA, China) evenly arranged in a ring, and adopts a PLC (Programmable Logic Controller) industrial automation motor control system. The PLC control system adopts the UDP (User Data Protocol) protocol and transmits and receives information with the computer's MATLAB through serial communication. It can achieve precise control of the displacement of the drive rope while obtaining the actual tension of the drive rope in real time.

[0122] It should be noted here that: in the process of collecting tension data, the Smooth script function in MATLAB can be used to filter out the data noise of the tension sensor, so as to smooth the measurement data. In addition, due to the error in the material parameters between the physical prototype of the rope-driven flexible continuous robot and the numerical perception model, the present invention uses the Bayesian optimization method to identify the elastic modulus parameters of the spring and rope unit of the physical prototype of the rope-driven flexible continuous robot; and the geometric model parameters are directly measured according to the physical prototype.

[0123] After parameter identification and correction, the specific model parameters of the rope-driven flexible continuous robot prototype are: the radius of the ring is 35 mm, the mass is 7.47×10 -3 kg; the elastic modulus of the compression rod is 84 GPa, the initial length is 60.6 mm, and the mass is 7.3×10 -4 kg, and the cross-sectional area is 7.07×10 -6 m 2 ; The Young's modulus of the spring is 0.5 GPa, the initial length is 32.2 mm, and the cross-sectional area is 3.14×10 -6 m 2 The elastic modulus of the active drive rope is 39 GPa, the initial lengths are 0.7 m and 0.4 m, and the cross-sectional area is 7.85×10 -7 m 2 .

[0124] Embodiment 1: Terminal static constant load perception:

[0125] (1) Step 1: Based on the position finite element, the dynamic unit formula of each component of the rope-driven flexible continuous robot is established, including four types of components: rod, spring, ring and active drive rope. Furthermore, for the active drive rope: on the one hand, the winch friction model is adopted to establish the internal force expression of the active drive rope considering friction (Formulas (19) and (20)), and all unit mass matrices and internal force vectors are integrated and assembled to form the dynamic differential equation (the first term of Formula (26)) through the finite element assembly technology; on the other hand, based on the complementary theory, the relaxation complementary equation of the active drive rope is established (Formula (22)), and the FB function is used to further transform it into a nonlinear complementary equation (Formula (23)).

[0126] (2) Step 2: Use the driving length of the active driving rope as the control input to establish the motion constraint equation of the system (Formula (24)), and introduce the force constraint condition (Formula (25)) into the dynamic model of the perception of the rope-driven flexible continuous robot. Then, the component units established in step 1 are assembled by finite element method, and the constraints (Formula (24) and (25)) are established in parallel to establish the dynamic DAEs model of the external force perception of the end of the rope-driven flexible continuous robot including rope friction and relaxation (Formula (26)).

[0127] (3) Step 3: Figure 3 As shown in the figure, the current driving state of the rope-driven flexible continuous robot is measured. Specifically, the tension sensor is used to measure the tension of the three active driving ropes ( Figure 2 The current actuation force of the ropes numbered Ⅰ, Ⅲ and Ⅴ in the figure is measured, and the current actuation length of the active drive rope is collected using the encoder inside the servo motor. Further, the measured data is fed back to the dynamic DAEs model of external force perception. (4) Step 4, using the current drive state information measured and fed back in step 3, the generalized-α method is used to solve the dynamic DAEs model, and the current terminal external force of the rope-driven flexible continuous robot can be obtained.

[0128] (5) Step 5: Determine whether the entire perception task is completed. If it is completed, end the calculation; if not, update the unknown state variables of the system (Formula (34)), and return to execute steps 3 and 4, and repeat this cycle until the entire perception task is completed.

[0129] like Figure 6As shown in the figure, the implementation steps of the above-mentioned sensing method are used to conduct a static steady load sensing experiment at the end of a rope-driven flexible continuous robot. The prototype of the rope-driven flexible continuous robot is deformed by the driving rope and remains stationary after reaching a certain position. At this time, a weight with a known mass is suspended at the end of the rope-driven flexible continuous robot. The experiment is divided into 8 groups in total. Each group of experiments is given a set of discrete driving rope lengths. The mass of the weights applied in the 8 groups of experiments increases linearly in increments of 10g. Considering the limited load capacity of the rope-driven flexible continuous robot, the maximum mass of the suspended weight in the experiment is 90g.

[0130] like Figure 7 As shown in , the perceived estimated external load curve is consistent with the actual external load curve. The perceived error is as follows Figure 8 As shown in the figure, the maximum relative error is 10.88%, the average relative error is 5.21%, the maximum absolute error is 3.52g, and the average absolute error is 2.20g. And from the curve of the figure, it can be seen that the external load perception error of the small mass weight is relatively large. The reason may be that the measurement accuracy of the tension sensor is limited, resulting in a large relative error of the small mass weight, but the absolute error is relatively stable. The above experimental results show that the proposed external force perception method can effectively and accurately perceive the static and constant external load at the end of the rope-driven flexible continuous robot.

[0131] Example 2: Dynamic time-varying load perception at the terminal:

[0132] Similarly, perform steps 1 to 5 above to conduct an experimental verification of the dynamic time-varying external force perception of the end of the rope-driven flexible continuous robot.

[0133] like Figure 9 As shown in the figure, a lightweight thin wire (negligible mass) is tied to the end of the rope-driven flexible continuous robot and connected to a three-axis force sensor (LX-SZ-160 Lixin, Hefei, China). The specific process of the experiment is as follows: at the initial moment, the rope-driven flexible continuous robot is in a stationary state, while the connecting rope is in a relaxed state; the motor is started and the driving rope is tightened to cause the rope-driven flexible continuous robot to deform in space; during the movement, the connecting rope changes from a relaxed state to a taut state and continues to maintain the tension state, and the end of the rope-driven flexible continuous robot is subjected to an external tensile force that varies in direction and size. Subsequently, when the rope-driven flexible continuous robot performs spatial movement, when the end approaches the three-axis force sensor, the rope changes from a taut state to a slack state; when it moves away from the three-axis force sensor, the rope changes from a slack state to a taut state. During the entire movement process, four switchings of the tension and relaxation of the connecting rope are achieved. Figure 10 and Figure 11 The terminal external force perception time curve and the corresponding perception error curve of the rope-driven flexible continuous robot during movement are given respectively.

[0134] like Figure 10 As shown in the figure, during the tensioning of the connecting rope (loading process), the rope-driven flexible continuous robot moves relatively smoothly, and the external force perception curve fluctuates slightly; during the unloading process, the rope-driven flexible continuous robot shakes more obviously, and the external force perception curve fluctuates relatively more. The sudden change of the external force perception curve observed in the dynamic weight experiment will occur when switching between the moving and static states. Figure 11 As shown in the figure, the average relative error of the perceived force size is 9.97% in the X-axis direction, 9.54% in the Y-axis direction, and 10.84% ​​in the Z-axis direction. The above experimental results show that the proposed external force perception algorithm can accurately estimate the dynamic time-varying external force at the end of the rope-driven flexible continuous robot in real time.

[0135] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for sensing external force at the end of a rope-driven flexible continuous robot based on dynamics, characterized in that: The method for sensing external force at the end of a rope-driven flexible continuous robot is as follows: first, according to the basic component types contained in the rope-driven flexible continuous robot, a unit formula of various components of the rope-driven flexible continuous robot is established; second, the motion constraint conditions and actuation force constraint conditions of the driving rope are introduced, and the actuation force and driving length of the rope are used as the input of the dynamic system, and the external force at the end of the rope-driven flexible continuous robot, the internal force of the body structure and the position information are used as the output, and a dynamic DAEs model suitable for external force sensing of the rope-driven flexible continuous robot is established by combining unit formulas of various components; second, the driving length of the rope and the actuation force of the rope driving point are measured, and the measured rope driving length and actuation force are fed back to the dynamic DAEs model to estimate the current external force at the end of the rope-driven flexible continuous robot; finally, the state variables of the dynamic DAEs model are updated, and the above steps are executed repeatedly until the sensing task is completed; specifically, the following steps are included: The first step is to establish the unit formula of various components of the rope-driven flexible continuous robot; the rope-driven flexible continuous robot contains four types of components: rods, springs, rings and active drive ropes; the specific calculation method of the dynamic unit formula of each component is as follows: Step 1.1, for the rod unit: its generalized coordinate q r It is represented by the unit node position in the global coordinate system: Among them, R k and R k+1 is the position coordinate vector of the nodes at both ends of the unit; x k ,y k ,z k and x k+1 ,y k+1 ,z k+1 R k and R k+1 Coordinate components in the Cartesian global coordinate system; the superscript T indicates the transpose operation of a vector or matrix; Then the position coordinates R of any point on the rod element are ξ It is expressed as: R ξ =ξR k +(1-ξ)R k+1 =C ξ what r (2) in, It represents the mapping matrix between the generalized coordinates of the rod unit and the coordinates of any point of the rod unit, ξ∈[0,1] is the natural coordinate, I3 is the three-dimensional unit matrix, is the Kronecker product; Then its generalized internal force vector f r for: in, is the axial unit direction vector of the rod element, f r ' is the magnitude of the elastic internal force, that is: f r ' Among them, E r is the elastic modulus of the rod element, A r is the cross-sectional area of ​​the rod element, l r0 is the initial length of the bar element, l r is the rod unit length; Define the linear density of the rod element as ρ r =m r / l r , then the kinetic energy of the rod unit is T rod It is expressed as: Among them, m r represents the mass of the bar element; represents the generalized velocity of the rod element; represents the generalized velocity of any point on the rod element; dξ represents the length element in the natural coordinate system; According to formula (5), the generalized mass matrix M of the rod element is obtained: r for: Step 1.2: For the spring element, it is regarded as a rod element without pressure stiffness. The derivation process of its element formula is the same as that of the rod element. When subjected to pressure, let the generalized internal force vector f in formula (3) be r =0; Step 1.3, for the ring unit, its generalized coordinate q t The vertex position description of the inscribed equilateral triangle of the ring is used, that is: Among them, R i Represents the coordinate vectors of the three vertices of the inscribed equilateral triangle of the ring; x i ,y i ,z i Represents the coordinate components in the Cartesian global coordinate system; i = 1, 2, 3; Any point N on the ring θ The generalized coordinates R θ for: Among them, R O' represents the generalized position coordinates of the center of the ring, Represents the vector from the center of the ring O' to any point on the ring; The internal force vectors f of the three equivalent rod elements forming the equilateral triangle inscribed in the ring are t (1) , f t (2) and f t (3) Finite element assembly is performed to obtain the approximate equivalent generalized internal force vector of the ring element; in, and are the unit direction vectors of the equivalent rod unit inscribed in the ring, f t (k) Specifically expressed as: Among them, E t is the Young's modulus of the inscribed equivalent rod element, A t is the cross-sectional area of ​​the equivalent rod element, l t0 is the initial length of the equivalent bar element in the stress-free state, represents the length of the kth equivalent rod element after elastic deformation; Therefore, acting on any point N on the ring θ The concentrated force on θ Obtain the equivalent concentrated force, C, acting on nodes N1, N2 and N3 θ It is expressed as: in, The generalized mass matrix M is derived based on the kinetic energy of the ring unit: t for: Among them, ρ represents the linear density of the ring; r represents the radius of the ring; I9 represents the 9-dimensional unit vector; Step 1.4, for the active drive rope unit: it is regarded as a super unit of a series of rod units connected in series. Consider that the active drive rope passes through n-1 pulleys, n segments, and a total of n+1 nodes; then, the generalized coordinates q of the drive rope unit are c for: Its generalized internal force vector f c With the generalized mass matrix M c The internal force vectors of all rope segments are and the mass matrix Finite element assembly is performed to obtain: in, is the unit direction vector of the i-th segment of the driving rope, i = 1, 2, …, n; l c and are the total length of the current driving rope and the length of the i-th section, i = 1, 2, ..., n; Step 1.5, for the active drive rope described in step 1.4, consider the friction problem between the active drive rope and the threading hole contact point; the details are as follows: According to the winch equation, the internal forces of the active driving ropes on both sides of the kth ring unit are and The following relations are satisfied: Among them, T M The driving force of the motor end of the driving rope; n t is the total number of ring units; is the friction coefficient between the driving rope and the kth ring, which depends on the material parameters and the relative sliding speed; is the included angle of the driving ropes on both sides of the ring; is the force magnitude of the driving point connected to the motor; Formula (19) is rewritten as: in, is the rope force on the kth disk Relative to the motor end force T M The attenuation coefficient of Step 1.6, for the active drive rope described in step 1.4, consider the possible relaxation problem during the movement of the rope-driven flexible continuous robot; the details are as follows: Introduce the relaxation factor variable η: the=l c0 -l c +Δl c (21) Among them, l c0 , Δl c and l c are the initial length, deformation and current length of the active drive rope respectively; when η≥0, the active drive rope is relaxed, and the actuating force T at the drive motor end is M =0; when η=0, the rope is tensioned, T M ≥0; then according to the complementary theory, η and T M Satisfies the linear complementary relationship, that is: η≥0,T M ≥0,ηT M =0 (22) For the linear complementarity problem shown in formula (22), it is transformed into a nonlinear complementarity problem for solution: Among them, Ψ(q c ,T M ) is the FB function; χ is the set constant; ε represents a constant small quantity; The second step is to introduce constraint conditions and establish the dynamic DAEs model of the rope-driven flexible continuous robot system; Step 2.1, based on the multi-body dynamics modeling theory, the motion constraint equation of the system is established with the driving length of the rope as the control input; the motion of each driving rope satisfies the following constraints: Φ(q,t)=d(q c ,t)+u(t)=0 (24) Where Φ(q,t) represents the constraint function of the active drive rope; d(q c ,t) is the actual measured actuation length at the end of the active drive rope; u(t) is the desired control input; Step 2.2: Consider the unknown forces in three-dimensional space borne by the end point of the rope-driven flexible continuous robot Three unknown quantities are added to the motion system of the rope-driven flexible continuous robot, and the following force constraint condition Υ(T M ): in, is the actual measured feedback value of the power at the end of the rope drive motor; t is the current moment; T M (t) represents the estimated value of the driving force of the rope motor at the current moment; Step 2.3, finite element assembly of the generalized internal forces and generalized mass matrices of each component unit of the rope-driven flexible continuous robot established in the first step, and considering the constraints of formula (24) and formula (25), a dynamic DAEs model for external force perception at the end of the rope-driven flexible continuous machine with rope friction and relaxation is established: in, is the generalized coordinate vector of the rope-driven flexible continuous robot; is the generalized velocity vector of the rope-driven flexible continuum robot; is the generalized acceleration vector of the rope-driven flexible continuum robot; is the external force vector acting on the end point of the rope-driven flexible continuous robot; is the generalized mass matrix of the system; is the generalized internal force of the system, including the internal force and gravity of each unit, n n is the number of nodes; is the motion constraint condition of the driving rope, as shown in formula (24); is the Lagrange multiplier, n c is the number of driving ropes; is the force constraint condition of the driving rope, as shown in formula (25), p is the number of driving ropes whose tension is measured, and p≤n c ; is the nonlinear complementary constraint equation for rope relaxation; The third step is to measure feedback and obtain the current driving state information of the active driving rope; One end of the tension sensor is connected to the active drive rope, and the other end is connected to the servo motor. The current actuation length of the active drive rope is collected through the servo motor, and the actuation length of the current driving point of the active drive rope is synchronously collected through the tension sensor, and the collected data is fed back to the dynamic DAEs model; The fourth step is to solve the kinetic DAEs model; Using the current actuation length of the active drive rope and the actuation length of the current driving point of the active drive rope measured in the third step, the external force at the end of the rope-driven flexible continuous robot is obtained by solving the dynamic DAEs model shown in formula (26); Step 4.1, using the generalized-α method, the kinetic DAEs model shown in formula (26) is numerically discretized at t n+1 Moment dynamics DAEs model equation g(x n+1 )for: Among them, q n+1 , and Respectively represent t n+1 The generalized coordinates, generalized velocity and generalized acceleration of the rope-driven flexible continuous robot at the moment λ n+1 Indicates t n+1 Lagrange multiplier at time; f in Indicates t n+1 The generalized internal force at a given moment; (T M ) n+1 Indicates t n+1 The force at the motor end of the rope that drives it at all times; Φ(q n+1 ,t)、Υ((T M ) n+1 ) and Ψ(q n+1 ,(T M ) n+1 ) represents t n+1 The motion constraints of the driving rope, the force constraints of the driving rope and the nonlinear complementary constraint equations of the rope slack at all times; and q n+1 and They are: Where h is the integration step length; a n 、a n+1 is a vector parameter, where a n is an auxiliary variable of the generalized-α method, and the vector parameter a n 、a n+1 The following relationship is satisfied: Among them, α m ,α f ,β,γ are parameters; a0 represents the initial value of the auxiliary variable of the generalized-α method; Step 4.2: At each time step, use As the quantity to be determined, it is solved iteratively. The format of the iteration is as follows: in, is the imbalance vector obtained according to the value of the current iteration step, is the iteration increment value, is the Jacobian matrix of the current iteration; when That is, the residual converges to the specified range, and the estimated external force vector at that moment can be obtained; After finding the increment value of the current iteration step, the unknown variables of the dynamic DAEs model are updated as follows: in, and are the process quantities to be solved, At this point, complete t n+1 The task of sensing the external force at the end of a rope-driven flexible continuous robot; The fifth step is to determine whether the external force estimation at all discrete moments has been completed; if it has been completed, the calculation is terminated; if not, the unknown variables of the system are updated as shown in formula (34), and the third and fourth steps are returned to execute, and the cycle is repeated until the entire perception task is completed.

2. According to the method of a dynamics-based rope-driven flexible continuous robot terminal external force sensing method of claim 1, it is characterized in that: In step 1.3, Specifically expressed as: Among them, θ represents The angle with the x direction of the global coordinate system; s i Represents the vector from the center of the ring O' to the i-th vertex of the equilateral triangle inscribed in the ring.

3. The method for sensing external force at the end of a rope-driven flexible continuous robot based on dynamics according to claim 1, characterized in that: In the fourth step, while solving the dynamic DAEs model to sense the external force at the end of the rope-driven flexible continuous robot, the configuration and structural internal force information of the rope-driven flexible continuous robot can also be obtained.

4. The method for sensing external force at the end of a rope-driven flexible continuous robot based on dynamics according to claim 1, characterized in that: In step 4.1, α m ,α f ,β,γ satisfy the following relationship: where ρ∈[0 1] is the spectral radius of the generalized-α method.

5. The method for sensing external force at the end of a rope-driven flexible continuous robot based on dynamics according to claim 1, characterized in that: In step 4.2, The specific expression is: in, 6. The method for sensing external force at the end of a rope-driven flexible continuous robot based on dynamics according to claim 1, characterized in that: In step 4.2, the specified range tol is specifically 1×10 -6 .

Images