A tension-optimization-based trajectory tracking method and device for a wire-driven continuum robot

Abstract

The present application belongs to the related technical field of continuum robot trajectory tracking, and discloses a wire transmission continuum robot trajectory tracking method and device based on tension optimization, comprising the following steps: step one, constructing the forward kinematics equation from the base to the end of the continuum robot according to the coordinate transformation relationship of the continuum robot; step two, constructing the statics equation of the continuum robot according to the force condition of the continuum robot; step three, integrating the equality constraints and inequality constraints in physics and geometry of the robot, taking the minimization of the transmission wire tension as the optimization target, and constructing a nonlinear optimization problem based on the kinematics equation and the statics equation of the robot; step four, solving the nonlinear optimization problem by using a nonlinear optimization solver to obtain the transmission wire tension under the current trajectory tracking end target point and the joint angle of the robot in the current state, and then completing the trajectory tracking. The present application can safely and efficiently complete the trajectory tracking task.

Description

Technical Field

[0001] This invention belongs to the technical field of trajectory tracking for continuum robots, and more specifically, relates to a method and device for trajectory tracking of wire-driven continuum robots based on tension optimization. Background Technology

[0002] Continuum robots are robots composed of a flexible continuum structure and a flexible cable (such as nickel-titanium alloy wire, steel cable, etc.) transmission system, characterized by flexibility, deformability, and high degrees of freedom. By controlling the flexible cables, they can flexibly change their posture in complex environments and confined spaces, thus performing delicate operations. Therefore, they have good application potential in fields such as medical surgery and space exploration. However, compared with traditional rigid robots, the flexible structure and cable-driven characteristics of continuum robots also make their modeling and control more complex, requiring consideration of nonlinearity and coupling effects, which poses challenges to control algorithms and system design.

[0003] Currently, kinematic models based on constant curvature or equal angle assumptions are commonly used for trajectory tracking problems of wire-driven continuum robots. These models are computationally simple and have good applicability when the robot is not under external force load. However, when the robot is subjected to external loads, deformation occurs due to its flexibility. In this case, continuing to use the above assumptions to calculate the robot's end-effector pose will introduce large errors, thus hindering precise control of the robot's end-effector position. To account for the influence of external forces, a mechanical model can be introduced into the model. One approach is to introduce a dynamic model, but due to the consideration of acceleration terms such as inertial forces and Coriolis forces, the complexity of the model and the solution time increase significantly, making it difficult to use in practical trajectory tracking. Another approach is to introduce a static model. Considering that the robot's fine manipulation does not require high speed, this model reasonably ignores the influence of dynamic acceleration terms. Its advantage is that it can accurately simulate the robot's state information and has a faster calculation speed, making it more likely to be applied to trajectory tracking. However, simply using inverse statics may result in underdetermined nonlinear equations, leading to multiple solutions. Trajectory tracking calculations based solely on kinematics may be ineffective if other constraints are not met, particularly the tension of the drive wire. Excessive or insufficient tension in the drive wire can reduce the robot's controllability and safety. Therefore, it is meaningful to consider how to perform trajectory tracking on the robot using a mechanical model under external loads and to reasonably optimize the tension of the drive wire. Summary of the Invention

[0004] In view of the above-mentioned defects or improvement needs of the existing technology, the present invention provides a method and device for trajectory tracking of a wire-driven continuum robot based on tension optimization, which is used to solve the problem of reduced controllability and safety of trajectory tracking caused by the lack of reasonable tension optimization in the existing technology.

[0005] To achieve the above objectives, according to one aspect of the present invention, a method for trajectory tracking of a wire-driven continuum robot based on tension optimization is provided, the method comprising the following steps:

[0006] Step 1: Construct the forward kinematic equations of the continuous robot from the base to the end effector based on the coordinate transformation relationship of the continuous robot;

[0007] Step 2: Construct the static equations of the continuum robot based on the forces acting on it.

[0008] Step 3: Integrate the equality and inequality constraints in the robot's physics and geometry, take minimizing the tension of the transmission wire as the optimization objective, and construct a nonlinear optimization problem based on the robot's kinematic equations and effort equations;

[0009] Step four: Use a nonlinear optimization solver to solve the nonlinear optimization problem to obtain the target point at the end of the current trajectory tracking. global p end The system calculates the tension of the transmission wire and the joint angles of the robot in its current state to complete trajectory tracking.

[0010] Furthermore, the forward kinematic equations are:

[0011]

[0012] In the formula, Let {end} be the homogeneous coordinate vector representation of the robot's end point in the end-point coordinate system {end}, and let (0,0,0,1) be a constant vector. T , The homogeneous transformation matrix of the robot's end-effector coordinate system {end} in the global coordinate system {global} is given by the parameters. Let n be the joint angle of the robot, and n be the number of joint angles, which is related to the structure of the robot. The homogeneous coordinate vector representation of the robot's end point in the global coordinate system {global} is specifically as follows: global x end , global y end , global z end ,1) T The first three components of the vector are the robot's three coordinate components in Cartesian space. In trajectory tracking tasks, the end point of the robot's trajectory is taken as...global p end Substitute.

[0013] Furthermore, the static equation is:

[0014] global F act +Σ global F(q)=0 (2)

[0015] global M act +Σ global M(q)=0 (3)

[0016] In the formula, This represents the vector representation of the tension in the robot's drive wire in the global coordinate system {global}. This represents the vector representation of the forces acting on the robot in the global coordinate system {global}, excluding the tension in the transmission wire. This represents the vector representation of the tension torque of the robot's drive wire in the global coordinate system {global}. This is the vector representation of the torques acting on the robot in the global coordinate system {global}, excluding the torque of the transmission wire tension. The summation sign indicates that the robot is in static equilibrium, and the resultant force and resultant torque are 0.

[0017] Furthermore, the tension F of the transmission wire act Since the joint angles q of the robot are unknown, they are treated as optimization variables and solved together in the optimization problem. The objective of the optimization problem is to minimize the sum of squares of the L2 norm of the tension in the transmission wire during trajectory tracking, and the corresponding formula is:

[0018]

[0019] For the optimization variable F act The following inequality constraints are applied to q:

[0020] F act,min ≤F act ≤F act,max (5)

[0021] q min ≤q≤q max (6)

[0022] In the formula, F act,min and F act,max These represent the minimum and maximum values ​​of the transmission wire tension, respectively. Setting the minimum value prevents the transmission wire driving the robot's movement from becoming slack, while setting the maximum value prevents the transmission wire from breaking due to excessive tension; q min and q maxThese represent the minimum and maximum values ​​of the robot's joint angles, respectively. The limitations on joint angles are related to the robot's structural design parameters.

[0023] Furthermore, the nonlinear optimization problem is expressed as:

[0024]

[0025] Among them, (a) to (c) are equality constraints, and (d) to (e) are inequality constraints; let Equation (7) simplifies to a standard nonlinear optimization problem:

[0026]

[0027] Furthermore, by solving the nonlinear optimization problem using a nonlinear optimization solver, the target point at the end of the current trajectory tracking is obtained. global p end The optimized tension of the transmission wire is used as input to control the robot to track the target point; simultaneously... global p end Update to the next target point, list similar optimization problems, and repeatedly use the nonlinear optimization solver to solve them until the trajectory tracking reaches the endpoint, at which point the entire trajectory tracking process ends.

[0028] Furthermore, the Jacobian matrix and Hessian matrix are provided for the nonlinear solver, and the corresponding formulas are:

[0029]

[0030] In the formula, J f,x B is the Jacobian matrix of the objective function f(x) with respect to x. f,x J is the Hessian matrix of the objective function f(x) with respect to x. h,x J is the Jacobian matrix of the equality constraint vector h(x) with respect to x. g,x It is the Jacobian matrix of the inequality constraint vector g(x) with respect to x.

[0031] The present invention also provides a wire-driven continuum robot trajectory tracking system based on tension optimization. The system includes a memory and a processor. The memory stores a computer program, and the processor executes the computer program to perform the wire-driven continuum robot trajectory tracking method based on tension optimization as described above.

[0032] The present invention also provides a computer-readable storage medium storing machine-executable instructions, which, when invoked and executed by a processor, cause the processor to implement the tension-optimized wire-driven continuum robot trajectory tracking method described above.

[0033] In summary, compared with the prior art, the trajectory tracking method and equipment for wire-driven continuum robots based on tension optimization provided by this invention have the following advantages:

[0034] 1. This method comprehensively considers the static equations and kinematic equations of robot motion, as well as the physical inequality constraints of transmission wire tension and joint variables. It uses minimizing the transmission wire tension as the optimization objective to construct a nonlinear optimization problem for continuum robot trajectory tracking, thereby safely and efficiently completing the trajectory tracking task. Furthermore, by comprehensively considering geometric, mechanical, and physical structural constraints, it overcomes the limitations of traditional kinematics, which only considers geometric analysis and leads to unreasonable tension calculations.

[0035] 2. This invention takes into account static equations, which improves the accuracy of robot model calculations and can also address the problem of trajectory tracking of continuum robots under external force loads.

[0036] 3. The solution obtained by this invention can not only obtain the tension of the transmission wire, ensuring the controllability of the robot, but also infer the joint angles of the robot, thereby deducing the shape of the robot. Attached Figure Description

[0037] Figure 1 This is a flowchart of a wire-driven continuum robot trajectory tracking method based on tension optimization provided by the present invention;

[0038] Figure 2 This is a model diagram of the continuum robot used in the embodiments of the present invention;

[0039] Figure 3 The diagram shows the trajectory tracking implementation results under no-load conditions at the end of the robot according to an embodiment of the present invention. (a) shows the initial configuration of the robot and the trajectory curve of the tracking, (b) shows the comparison between the actual coordinates and the expected coordinates of the three coordinates in the Cartesian space of the robot trajectory tracking, and (c) shows the transmission wire tension diagram of the robot trajectory tracking process optimization.

[0040] Figure 4 The diagram shows the implementation results of the trajectory tracking method of the end effector under a 40g load in this embodiment of the invention. (a) is the initial configuration of the robot and the trajectory curve of the tracking, (b) is a comparison diagram of the actual coordinates and expected coordinates of the three coordinates of the robot trajectory tracking in Cartesian space, and (c) is the transmission wire tension diagram of the robot trajectory tracking process optimization.

[0041] In all the figures, the same reference numerals are used to denote the same elements or structures, wherein: 201 - distal part of the continuum robot, 202 - proximal part of the continuum robot, 203 - transmission wire. Detailed Implementation

[0042] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0043] This invention provides a trajectory tracking method for a wire-driven continuum robot based on tension optimization. The method first constructs the robot's forward kinematic equations from the base to the end effector based on the robot's coordinate transformation, and then constructs the robot's static equations based on the robot's force balance. Next, it integrates the physical and geometric equality and inequality constraints related to the robot, using minimizing the tension of the transmission wire as the optimization objective, and constructs a nonlinear optimization problem. Then, it solves this nonlinear optimization problem using a nonlinear optimization solver. Finally, it terminates the optimization iteration based on whether the convergence condition is met, obtaining the final optimized tension and the robot's joint angles in this state.

[0044] Specifically, the trajectory tracking method mainly includes the following steps:

[0045] Step 1: Construct the forward kinematic equations of the continuum robot from the base to the end effector based on the coordinate transformation relationship of the continuum robot:

[0046]

[0047] In the formula, Let {end} be the homogeneous coordinate vector representation of the robot's end point in the end-point coordinate system {end}, and let (0,0,0,1) be a constant vector. T ,

[0048] The homogeneous transformation matrix of the robot's end-effector coordinate system {end} in the global coordinate system {global} is given by the parameters. Let n be the joint angle of the robot, and n be the number of joint angles, which is related to the structure of the robot. The homogeneous coordinate vector representation of the robot's end point in the global coordinate system {global} is specifically as follows: global x end , global y end , global z end ,1) T The first three components of the vector are the robot's three coordinate components in Cartesian space. In trajectory tracking tasks, the end point of the robot's trajectory can be used as... global p endSubstitute.

[0049] Step 2: Construct the static equations of the continuum robot based on the forces acting on it:

[0050] global F act +Σ global F(q)=0 (2)

[0051] global M act +Σ global M(q)=0 (3)

[0052] In the formula, This represents the vector representation of the tension in the robot's drive wire in the global coordinate system {global}. This is a vector representation of the forces acting on a robot in the global coordinate system {global}, excluding the tension in the transmission wire, including but not limited to external forces, elastic forces, gravity, etc. This represents the vector representation of the tension torque of the robot's drive wire in the global coordinate system {global}. This is a vector representation of the torques acting on the robot in the global coordinate system {global}, excluding the torque of the transmission wire tension. This includes, but is not limited to, external force torques, elastic torques, and gravitational torques. The summation sign indicates that the robot is in static equilibrium, and the resultant force and resultant torque are 0.

[0053] For static equations, if the force vectors and moment vectors are not in the same coordinate system, they need to be transformed to be described in the same coordinate system. This requires the use of the coordinate transformation method in step one.

[0054] Step 3: Integrate the equality and inequality constraints in the robot's physics and geometry, take minimizing the tension of the transmission wire as the optimization objective, and construct a nonlinear optimization problem based on the robot's kinematic equations and effort equations.

[0055] In actual robot trajectory tracking, the tension F of the transmission wire act Since the joint angles q of the robot are unknown, they are treated as optimization variables and solved together in the optimization problem. The objective of the optimization problem is to minimize the sum of the squares of the L2 norm of the tension in the transmission wire during trajectory tracking, in order to avoid the risk of the transmission wire breaking due to excessive tension during robot movement.

[0056]

[0057] For the optimization variable F act The following inequality constraints are applied to q:

[0058] F act,min ≤Fact ≤F act,max (5)

[0059] q min ≤q≤q max (6)

[0060] In the formula, F act,min and F act,max These represent the minimum and maximum values ​​of the transmission wire tension, respectively. Setting the minimum value prevents the transmission wire driving the robot's movement from becoming slack, while setting the maximum value prevents the transmission wire from breaking due to excessive tension; q min and q max These represent the minimum and maximum values ​​of the robot's joint angles, respectively. The limitations on joint angles are related to the robot's structural design parameters.

[0061] The nonlinear optimization problem for trajectory tracking of the continuum robot is expressed as:

[0062]

[0063] Among them, (a) to (c) are equality constraints, and (d) to (e) are inequality constraints; let Equation (7) simplifies to a standard nonlinear optimization problem:

[0064]

[0065] Step four: Use a nonlinear optimization solver to solve the nonlinear optimization problem to obtain the target point at the end of the current trajectory tracking. global p end The system calculates the tension of the transmission wire and the joint angles of the robot in its current state to complete trajectory tracking.

[0066] Specifically, the nonlinear optimization problem is solved using a nonlinear optimization solver to obtain the target point at the end of the current trajectory tracking. global p end The optimized tension of the transmission wire is used as input to control the robot to track the target point; simultaneously... global p end Update to the next target point, list similar optimization problems, and repeatedly use the nonlinear optimization solver to solve them until the trajectory tracking reaches the endpoint, at which point the entire trajectory tracking process ends.

[0067] For nonlinear solvers, providing the Jacobian and Hessian matrices of the exact model is beneficial for improving the accuracy and convergence speed of the solution, specifically including:

[0068]

[0069] In the formula, J f,xB is the Jacobian matrix of the objective function f(x) with respect to x. f,x J is the Hessian matrix of the objective function f(x) with respect to x. h,x J is the Jacobian matrix of the equality constraint vector h(x) with respect to x. g,x It is the Jacobian matrix of the inequality constraint vector g(x) with respect to x.

[0070] The joint variables obtained can be used to solve the position and orientation of the robot end effector. The joint variables can be substituted into formula (1) to obtain the solution.

[0071] The present invention also provides a wire-driven continuum robot trajectory tracking system based on tension optimization. The system includes a memory and a processor. The memory stores a computer program, and the processor executes the computer program to perform the wire-driven continuum robot trajectory tracking method based on tension optimization as described above.

[0072] The present invention also provides a computer-readable storage medium storing machine-executable instructions, which, when invoked and executed by a processor, cause the processor to implement the tension-optimized wire-driven continuum robot trajectory tracking method described above.

[0073] The present invention will be further described in detail below with reference to specific embodiments.

[0074] Please see Figure 1 The trajectory tracking method for a wire-driven continuum robot based on tension optimization provided in this embodiment of the invention mainly includes the following steps:

[0075] S1 is the input of the initial tension and structural parameters of the transmission wire.

[0076] S2, establish the kinematic equations of the continuum robot based on the coordinate transformation relationship.

[0077] S3. Establish the static equations of the continuum robot based on the mechanical equilibrium relationship.

[0078] S4 is a nonlinear optimization problem constructed by combining the tension of the transmission wire and the constraints of the mechanical structure.

[0079] S5 uses a nonlinear optimization solver to optimize the tension of the transmission wire, which can also solve joint variables. It determines whether the optimization meets the convergence condition. If it does, proceed to S6; otherwise, repeat S5 to continue solving.

[0080] S6, record the obtained optimization variables: including transmission wire tension and joint variables.

[0081] S7 determines whether the entire trajectory tracking solution has been completed. If not, it returns to S2 to optimize the solution for the next point in the trajectory tracking. Otherwise, it outputs the result sequence of the entire trajectory tracking optimization process, including the transmission wire tension and joint variables.

[0082] In one implementation, please refer to Figure 2 The continuous robot used to verify this method consists of two parts: a distal part 201 and a proximal part 202. Both parts are driven by six drive wires 203 made of nickel-titanium alloy. The distal part 201 is driven by four drive wires, and the proximal part 202 is driven by two drive wires.

[0083] In one implementation, please refer to Figure 3 and Figure 4 The robot's relevant parameters are as follows: the joint diameter is 4mm, the total length is 30mm, the Young's modulus of the central skeleton is 0.55GPa, the maximum bending angle of each joint of the robot is limited to ±18°, and the range of transmission wire tension is limited to 0.5N~50N.

[0084] Figure 3 The initial tension of the transmission wire in the simulation is [3.22N, 0.5N, 0.5N, 35.6N, 0.5N, 0.5N]. The robot does not apply any load. Figure 4 The initial tension of the transmission wire in the simulation was [10.9N, 0.5N, 0.5N, 32.3N, 0.5N, 0.5N], and a load of 40g was applied to the end of the robot. Figure 3 and Figure 4 The simulation trajectory tracking is the same circular trajectory.

[0085] In another implementation, the specific steps are as follows:

[0086] Step 1: Based on the robot's structural characteristics, establish the robot's kinematic equations using coordinate transformation relationships, and establish the robot's joint space q and end effector Cartesian space p. end The mapping between them. The constraints of the kinematic equations ensure the consistency between the Cartesian coordinates of the robot's end point and the target coordinates of the trajectory tracking.

[0087] Step 2: Based on the force and torque balance of the robot, obtain the robot's static equations. Simultaneously, use the coordinate transformation from Step 1 to establish the robot's joint space q and driving space force F. act Mapping within the same coordinate system. The constraints of the static equations ensure that the robot's state is feasible and stable. Simultaneously, the initial values ​​of each joint variable in the robot's joint space can be calculated based on the initial tension and end-effector load of the robot's drivewire.

[0088] Step 3: Combining the equality constraints of the equations described in Step 1 and Step 2, as well as the inequality constraints on the magnitude of joint variables and transmission wire tension, a nonlinear problem is constructed. The optimization objective is to minimize the sum of squares of the L2 norm of the transmission wire tension, with the aim of minimizing the tension of the robot's transmission wire while ensuring the trajectory tracking task.

[0089] Step 4: The obtained nonlinear optimization problem is solved using a nonlinear solver to obtain the optimal search direction Δx and the corresponding step size coefficient α. k Combining the x obtained in the previous step k-1 Update the current optimization variable x k =x k-1 +α k Δx, when ||x k -x k-1 When the value is less than the tolerance, the iteration terminates, and the optimal solution x for the current trajectory tracking target point is obtained. * .

[0090] Step 5: Using the next trajectory tracking point as the target, repeat steps 1-4 to continue optimizing the new tension. The entire trajectory tracking task is completed, and the optimized tension of the transmission wire throughout the entire process is obtained.

[0091] As an optional embodiment, the method can simultaneously obtain the robot's joint variables q, thereby obtaining the robot's configuration.

[0092] Those skilled in the art will readily understand that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A trajectory tracking method for a wire-driven continuum robot based on tension optimization, characterized in that, The method includes the following steps: Step 1: Construct the forward kinematic equations of the continuum robot from the base to the end effector based on the coordinate transformation relationship of the continuum robot; Step 2: Construct the static equations of the continuum robot based on the forces acting on it. Step 3: Integrate the equality and inequality constraints in the robot's physics and geometry, take minimizing the tension of the transmission wire as the optimization objective, and construct a nonlinear optimization problem based on the robot's kinematic equations and effort equations; Step four: Use a nonlinear optimization solver to solve the nonlinear optimization problem to obtain the target point at the end of the current trajectory tracking. The system calculates the tension in the transmission wire and the joint angles of the robot in its current state to complete trajectory tracking. tension of the transmission wire and the joint angle of the robot Since these variables are unknown, they are treated as optimization variables and solved together in the optimization problem. The objective of the optimization problem is to minimize the sum of squares of the L2 norm of the tension in the transmission wire during trajectory tracking. The corresponding formula is: （4） For optimization variables and The following inequality constraints are applied: （5） （6） In the formula, and These represent the minimum and maximum values ​​of the transmission wire tension, respectively. The minimum value is set to prevent the transmission wire that drives the robot from becoming loose, while the maximum value is set to prevent the transmission wire from breaking due to excessive tension. and These represent the minimum and maximum values ​​of the robot's joint angles, respectively. The limitations on joint angles are related to the robot's structural design parameters. The nonlinear optimization problem is represented as: （7） Among them, (a)~(c) are equality constraints, and (d)~(e) are inequality constraints; Let the robot end point be in the end coordinate system The homogeneous coordinate vector representation below is a constant vector. , For the robot end-effector coordinate system In the global coordinate system The homogeneous transformation matrix representation under the following parameters For the robot's joint angles, The number of joint angles is related to the robot's structure; The robot end point in the global coordinate system The homogeneous coordinate vector representation below is specifically as follows: The first three components of the vector are the robot's three coordinate components in Cartesian space. In trajectory tracking tasks, the end point of the robot's trajectory is taken as... Substitute; For the tension of the transmission wire in the robot in the global coordinate system Vector representation below, For the forces on the robot other than the tension in the transmission wire, in the global coordinate system Vector representation of the forces acting on the object; The torque of the wire pulling force of the robot in the global coordinate system Vector representation below, For the torques of the robot other than the tension torque of the drive wire in the global coordinate system The vector representation of the torque acting on the robot; the summation sign indicates that the robot is in static equilibrium, and the resultant force and resultant torque are... ;make Equation (7) is simplified to a standard nonlinear optimization problem: （8） In the formula, The equality constraint vector; This is the inequality constraint vector.

2. The trajectory tracking method for a wire-driven continuum robot based on tension optimization as described in claim 1, characterized in that: The nonlinear optimization problem is solved using a nonlinear optimization solver to obtain the target point at the end of the current trajectory tracking. The optimized tension of the transmission wire is used as input to control the robot to track the target point; simultaneously... Update to the next target point, list similar optimization problems, and repeatedly use the nonlinear optimization solver to solve them until the trajectory tracking reaches the endpoint, at which point the entire trajectory tracking process ends.

3. The trajectory tracking method for a wire-driven continuum robot based on tension optimization as described in claim 2, characterized in that: The Jacobian and Hessian matrices are provided for the nonlinear solver, and the corresponding formulas are: （9） In the formula, It is the objective function about Jacobian matrix, It is the objective function about The Hessian matrix, It is an equality constraint vector about Jacobian matrix, Inequality constraint vector about The Jacobian matrix.

4. A trajectory tracking system for a wire-driven continuum robot based on tension optimization, characterized in that: The system includes a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, it performs the trajectory tracking method for a wire-driven continuum robot based on tension optimization as described in any one of claims 1-3.

5. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores machine-executable instructions, which, when invoked and executed by a processor, cause the processor to implement the tension-optimized wire-driven continuum robot trajectory tracking method according to any one of claims 1-3.

Images