A concentric composite continuum robot leader-follower control method and system

Abstract

The application provides a kind of concentric composite continuum robot leadership following control method and system, its method includes: establishing outer layer mapping and inner layer mapping, the end pose of concentric composite continuum robot is calculated, and center line discrete space coordinate point set is generated;With end pose approximation target pose and end position error minimization as optimization goal, with the obstacle avoidance constraint between discrete space coordinate point set and obstacle set and the physical limit constraint of configuration space as constraint condition, the optimal configuration parameter of terminal state is solved;With optimal configuration parameter as terminal boundary condition, obtain geometric path, and generate expected configuration sequence;Real-time acquisition current actual configuration vector, with expected configuration sequence as target, with the tracking error of current actual configuration vector and expected configuration vector to build stability constraint optimization problem and solve in real time, then update configuration parameter until convergence.The application realizes efficient, robust and universal leadership following motion control.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and in particular to a leader-follower control method and system for a concentric composite continuum robot. Background Technology

[0002] Concentric Composite Continuum Robots (CCCRs) combine the structural advantages of tendon-driven continuum robots and concentric tube robots. They retain the inherent flexibility and continuous bending capabilities of continuum robots while enhancing overall rigidity and load-bearing capacity through concentric nesting structures. Leveraging their unique morphological features, these robots can navigate flexibly through winding anatomical structures or narrow, unstructured spaces, exhibiting dexterity and accessibility unmatched by traditional rigid-body robots in constrained environments such as minimally invasive surgery. However, achieving safe and efficient motion control in these complex environments requires addressing two fundamental issues: firstly, how to accurately describe the robot's kinematic behavior; and secondly, how to plan motion paths that satisfy environmental constraints.

[0003] Kinematic modeling of continuum robots typically employs the piecewise constant curvature assumption, discretizing the continuum into multiple virtual joints and establishing a mapping from joint space to operational space through the product of homogeneous transformation matrices. Regarding the mapping from configuration space to joint space, tendon-driven continuum robots require establishing an analytical relationship between tendon displacement and joint bending angle. Existing research on kinematic modeling of single-tube continuum robots is relatively mature, but kinematic modeling for concentric composite structures still faces challenges. Due to the interlayer contact friction and constraint differences between the inner and outer tubes, the bending motion of the outer continuum will act on the inner continuum through contact force coupling, causing the actual deformation of the inner tube to deviate from the configuration expected by its own driving input. This coupling effect renders traditional single-tube kinematic models ineffective in concentric composite structures, making it difficult to accurately calculate the robot's overall shape and end effector pose at any given time.

[0004] In terms of motion planning, the leader-follower motion strategy is an effective means for continuum robots to achieve safe navigation in confined environments. The core idea of ​​this strategy is to enable the robot body to accurately reproduce the proven safe path of its front end, thereby reaching the target area while minimizing the risk of collisions with the environment. Existing research on leader-follower motion planning mainly falls into the following two categories: Leader-following motion planning methods based on kinematic models typically require prior settings for the robot's geometric parameters and motion paths. For single-tube continuum robots, although configuration sequences satisfying leader-following conditions can be planned by restating them using forward kinematic models, path errors and end-effector errors increase significantly when the path is described by arbitrary 3D curves. For concentric tube robots, leader-following motion can only be achieved under specific conditions of tube pre-bending rate and deployment sequence. Existing methods lack universality in handling arbitrary tip navigation tasks and are difficult to meet the needs of complex and variable surgical environments.

[0005] Control strategies based on optimization and sampling have, to some extent, expanded the applicability of leader-follower motion. While methods that frame shape control as a nonlinear optimization problem and transform it into quadratic programming through kinematic linearization are suitable for arbitrary tip navigation tasks, they are highly dependent on robot shape information, and their computational efficiency needs further improvement. Sampling-based motion planning methods, although capable of jointly considering robot configuration and base pose, still struggle in height-constrained environments with sharp curvature or bifurcated paths. Furthermore, achieving precise control of multi-segment tendon-driven continuum robots is inherently a highly challenging task due to their infinite degrees of freedom, nonlinear behavior, and the coupling characteristics of tendon actuation.

[0006] Therefore, there is an urgent need for a new technical solution that can fully leverage the structural advantages of concentric composite continuum robots while achieving efficient, robust, and universal leader-follower motion control. Summary of the Invention

[0007] To address the aforementioned problems in the prior art, this invention provides a leader-follower control method and system for concentric composite continuum robots, achieving efficient, robust, and universal leader-follower motion control.

[0008] To achieve the above objectives, the technical solution adopted by the present invention is as follows: In a first aspect, the present invention provides a leader-follower control method for a concentric composite continuum robot, comprising: Establish the mutual mapping relationship between the displacement of the driving tendon of the outer continuum and the outer configuration parameters, and the mutual mapping relationship between the displacement of the driving tendon of the inner continuum and the inner configuration parameters. Calculate the end-effector pose of the concentric composite continuum robot based on the outer and inner configuration parameters, and generate a discrete spatial coordinate point set for the centerline. The mutual mapping relationship between the displacement of the driving tendon of the inner continuum and the inner configuration parameters includes the coupling compensation amount caused by the bending of the outer continuum. With the goal of minimizing the end-effector pose and approximating the target pose, and with the obstacle avoidance constraints between the discrete spatial coordinate point set and the obstacle set and the physical constraint of the configuration space as the constraint conditions, the optimal configuration parameters of the endpoint state are solved. Using the optimal configuration parameters as the endpoint boundary conditions, a collision-free geometric path that satisfies curvature constraints is generated from the initial pose to the endpoint configuration. The geometric path is then smoothed and optimized, mapped to the joint space, and the desired configuration sequence is generated. The current actual configuration vector of the concentric composite continuum robot is collected in real time. Taking the desired configuration sequence as the target, a stability constraint optimization problem is constructed based on the tracking error between the current actual configuration vector and the corresponding desired configuration vector in the desired configuration sequence, and solved in real time. The configuration parameters are updated according to the optimal configuration update amount obtained by the solution. This process is repeated until the current actual configuration converges to the desired configuration sequence.

[0009] The beneficial effects of this invention are as follows: By introducing a coupling compensation amount caused by the bending of the outer continuum into the driving mapping of the inner continuum, the nonlinear coupling effect between the inner and outer tubes is accurately characterized, overcoming the defect of the failure of the traditional single-tube kinematic model in concentric composite structures; by minimizing the end position error as the objective and solving the optimal configuration at the endpoint in combination with obstacle avoidance constraints, and then generating a collision-free smooth path that satisfies curvature constraints using this configuration as the boundary and mapping it to the desired configuration sequence, global optimal path planning under any three-dimensional path is realized, improving the universality of the method; by collecting the actual configuration in real time and constructing an optimization problem with stability constraints using the tracking error for online solution, combined with a cyclic convergence mechanism, the strict convergence of the robot configuration to the desired trajectory is ensured, realizing efficient, robust and universal leader-follower motion control.

[0010] Optionally, the mapping function between the inner continuum-driven tendon displacement and the inner layer configuration parameters is: ; ; In the formula, This represents the total change in the driving length of the inner tendon. This refers to the length increment caused by the bending of the inner continuum itself. This is the compensation amount for inner layer coupling caused by the bending of the outer continuum. r represents the change in the i-th tendon of the inner layer. I Let θ be the radius of the inner tendon distribution circle. O The outer bending angle, O ψ is the azimuth angle of the curved plane. i Let be the position angle of the i-th tendon.

[0011] Optionally, the step of constructing a stability-constrained optimization problem based on the tracking error between the current actual configuration vector and the corresponding desired configuration vector in the desired configuration sequence includes: With desired configuration sequence ∈C is the target configuration, and the control Lyapunov function V( Let ) be a quadratic function as follows: ; In the formula, Let M be the current actual configuration vector, and M be a symmetric positive definite weight matrix used to decouple the motion flexibility of the inner continuum and the outer continuum. The optimal update amount of the configuration parameters within a period is controlled by the following expression: ; ; ; In the formula, This represents the optimal update amount of the configuration parameters within the current control cycle. Let λ be the desired configuration at the next time step, and λ be the convergence coefficient. This is the actual configuration vector at the next moment. and These are the lower and upper bounds of the configuration space, respectively.

[0012] Optionally, generating a collision-free geometric path from the initial pose to the final configuration, satisfying curvature constraints, includes: Path search is performed using an improved fast expanding random tree algorithm, wherein the node cost function of the improved fast expanding random tree algorithm is... Defined as: ; In the formula, w1 and w c These are the weighting coefficients for path length and curvature magnitude, respectively, d(q) n ,q r ) is the new node q n With its parent node q r Euclidean distance between them, κ(q) n ,q r ) represents the equivalent curvature of the path segment, κ. max This represents the maximum allowable bending curvature for the robot.

[0013] Optionally, the smoothing optimization of the geometric path includes: Variational optimization is used for smoothing, and the optimized smooth path is represented as a discrete point sequence P={x1,x2,…,x}. M The original path point is The optimization problem of the variational method is: ; ; In the formula, , x is the curvature smoothing weighting coefficient. i x i-1 and x i+1 For smooth path points, , and For the original path point, κ(x) i (x) represents the smooth path point. i curvature, κ max The maximum bending curvature allowed for the robot; Map the smoothed and optimized geometric path to joint space and solve for the configuration sequence: ; ; In the formula, x k p is the reference trajectory point j Let q be the feature point after discretizing the robot's centerline. start q goal For the initial pose and the final pose.

[0014] Optionally, the mapping function between the outer continuum-driven tendon displacement and the outer configuration parameters is: ; In the formula, Δl O R is the amount of change in the length of the driving tendon. O θ is the radius of the outer tendon distribution circle. O The outer bending angle, O ψ1, ψ2, and ψ3 are the azimuth angles of the curved plane, and the position angles of the 1st, 2nd, and 3rd tendons, respectively.

[0015] Optionally, the obstacle avoidance constraint function is: ; In the formula, c k r is the center position of the spherical obstacle. k Let r be the radius of the spherical obstacle, and r0 be the safety redundancy distance.

[0016] Optionally, calculating the end-effector pose of the concentric composite continuum robot based on the outer layer configuration parameters and the inner layer configuration parameters includes: Using the piecewise constant curvature assumption, the outer continuum and the inner continuum are discretized into multiple virtual joints, and the homogeneous transformation matrix of each virtual joint is established. The homogeneous transformation matrices of each virtual joint are multiplied together to obtain the transformation matrix of the end of the outer continuum relative to the base. Using the pose of the inner continuum extending from the end of the outer continuum as the initial reference, a transformation matrix is ​​established relative to the end of the outer continuum. Multiply the transformation matrix of the outer continuum end relative to the base with the transformation matrix of the inner continuum end relative to the outer continuum end to obtain the complete transformation matrix of the robot end effector relative to the base; The position vectors of the origins of each coordinate system are recursively extracted from the transformation matrix sequence by the coordinate extraction operator, and the extracted position vectors of the origins of each coordinate system constitute the set of discrete spatial coordinate points of the centerline.

[0017] Optionally, after updating the configuration parameters according to the optimal configuration update amount obtained by solving, the method further includes: When the optimization problem fails to be solved, it automatically reverts to a preset heuristic strategy for fault tolerance correction. The heuristic strategy is to recursively calculate the current actual configuration vector along the direction of the desired configuration sequence with a fixed step size.

[0018] In a second aspect, the present invention provides a concentric composite continuum robot leader-follower control system, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the concentric composite continuum robot leader-follower control method of the first aspect.

[0019] The technical effects of the concentric composite continuum robot leader-follower control system provided in the second aspect are described in the relevant description of the concentric composite continuum robot leader-follower control method provided in the first aspect. Attached Figure Description

[0020] Figure 1 This is a schematic diagram of the main process of a leader-follower control method for a concentric composite continuum robot according to an embodiment of the present invention; Figure 2 This is a schematic diagram illustrating the obstacle avoidance and following effect according to an embodiment of the present invention; Figure 3 This is a schematic diagram of the optimal configuration planning involved in an embodiment of the present invention; Figure 4 This is a schematic diagram of the structure of a concentric composite continuum robot leader-follower control system according to an embodiment of the present invention.

[0021] Explanation of reference numerals in the attached figures: 1: A leader-follower control system for a concentric composite continuum robot; 2: Processor; 3: Memory. Detailed Implementation

[0022] To better understand the above technical solutions, exemplary embodiments of the present invention will be described in more detail below with reference to the accompanying drawings. Although exemplary embodiments of the present invention are shown in the drawings, it should be understood that the present invention can be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that the present invention can be understood more clearly and thoroughly, and that the scope of the present invention can be fully conveyed to those skilled in the art.

[0023] Example 1 In the existing technology, the motion control of concentric composite continuum robots has problems such as model failure caused by the coupling effect between the inner and outer tubes, large planning errors under arbitrary three-dimensional paths, and tracking control that lacks strict stability guarantees.

[0024] In this embodiment, an accurate kinematic model is established by introducing coupling compensation in the driving mapping of the inner continuum. The desired configuration sequence is obtained by combining endpoint optimal configuration search and smooth path generation. Based on real-time configuration feedback, an optimization problem with stability constraints is constructed and solved online, thereby achieving efficient, robust, and universal leader-follower motion control. See the discussion below for details.

[0025] Please refer to Figures 1 to 3 A leader-follower control method for a concentric composite continuum robot, comprising: Step S1: Establish the mutual mapping relationship between the displacement of the outer continuum driving tendon and the outer configuration parameters, as well as the mutual mapping relationship between the displacement of the inner continuum driving tendon and the inner configuration parameters. Calculate the end-effector pose of the concentric composite continuum robot based on the outer and inner configuration parameters, and generate a discrete spatial coordinate point set for the centerline. The mutual mapping relationship between the displacement of the inner continuum driving tendon and the inner configuration parameters includes the coupling compensation amount caused by the bending of the outer continuum.

[0026] In this embodiment, the system first receives user input parameters such as the target pose, the set of environmental obstacles, the initial pose, and the physical limit parameters of the robot body. Then, for both the outer and inner continuums, a mapping relationship is constructed between the displacement of the driving tendons and the configuration parameters.

[0027] For the inner continuum, considering its coupling effect with the outer continuum, a coupling compensation amount is introduced. The mapping relationship function between the tendon displacement driven by the inner continuum and the inner continuum configuration parameters in step S1 is as follows: ; ; In the formula, This represents the total change in the driving length of the inner tendon. This refers to the length increment caused by the bending of the inner continuum itself. This is the compensation amount for inner layer coupling caused by the bending of the outer continuum. r represents the change in the i-th tendon of the inner layer. I Let θ be the radius of the inner tendon distribution circle. O The outer bending angle, O ψ is the azimuth angle of the curved plane. i Let be the position angle of the i-th tendon.

[0028] For the outer continuum, its configuration parameters are: At this point, the mapping function between the tendon displacement driven by the outer continuum and the outer configuration parameters in step S1 is: ; In the formula, Δl O R is the amount of change in the length of the driving tendon. O θ is the radius of the outer tendon distribution circle. O The outer bending angle, O ψ1, ψ2, and ψ3 are the azimuth angles of the curved plane, and the position angles of the 1st, 2nd, and 3rd tendons, respectively.

[0029] In this embodiment, the kinematic model from configuration space to operational space is based on the Piecewise Constant Curvature (PCC) assumption and coordinate transformation. Step S1 calculates the end-effector pose of the concentric composite continuum robot based on the outer and inner configuration parameters, including: Step S11: Using the piecewise constant curvature assumption, the outer and inner continuums are discretized into multiple virtual joints, and the homogeneous transformation matrix of each virtual joint is established. The homogeneous transformation matrices of each virtual joint are multiplied together to obtain the transformation matrix of the end of the outer continuum relative to the base.

[0030] For the outer continuum, it is discretized into n o There are 1 virtual joint, and the homogeneous transformation matrix of a single virtual joint is: ; θ; ; In the formula, h o For discrete joint step size, is the length of the outer continuum.

[0031] At this point, the transformation matrix of the outer continuum end relative to the base is obtained as follows: ; In the formula, This is the base transformation matrix that includes the feed displacement.

[0032] Step S12: Using the pose of the inner continuum extending from the end of the outer continuum as the initial reference, establish the transformation matrix of the end of the inner continuum relative to the end of the outer continuum.

[0033] Step S13: Multiply the transformation matrix of the outer continuum end relative to the base with the transformation matrix of the inner continuum end relative to the outer continuum end to obtain the complete transformation matrix of the robot end effector relative to the base.

[0034] For the inner continuum, the pose of its extended outer end is used as the initial reference. The same modeling logic as the outer layer is adopted to obtain the transformation matrix of the inner end relative to the outer end. Finally, the complete transformation matrix of the robot's end effector relative to the base is: .

[0035] Step S14: Recursively extract the position vectors of the origins of each level of the coordinate system from the transformation matrix sequence using the coordinate extraction operator. The extracted position vectors of the origins of each level of the coordinate system constitute the set of discrete spatial coordinate points of the centerline.

[0036] Among them, the coordinate extraction operator is defined. By recursively extracting the transformation matrix sequence, the position vectors of the origins of each coordinate system are obtained: ; At this point, the discrete spatial coordinate point set of the robot body is generated. It is used for collision detection and trajectory planning in the second part.

[0037] Step S2: With the goal of minimizing the end-effector pose and approximating the target pose, and with the constraints of obstacle avoidance between the discrete spatial coordinate point set and the obstacle set, and the physical constraints of the configuration space as constraints, solve for the optimal configuration parameters of the final state.

[0038] Reference Figure 2 θ O θ is the outer layer bending angle. I X is the inner layer bending angle. b2 Y b2 Z b Let {b} be the coordinate axes of the base coordinate system, X k2 Y k2 Z kLet T be the coordinate axis of the coordinate system {k} at the k-th virtual joint. k1 T k2 is the homogeneous transformation matrix. In this embodiment, the configuration planning method for the concentric composite continuum robot mainly achieves follow-the-leader (FTL) motion of the robot in complex and constrained environments through a two-stage solution strategy, minimizing the sweep space and improving surgical safety.

[0039] Specifically, for the target pose, an optimization model is established with the goal of minimizing the end-effector position error, and conditions including obstacle avoidance constraints and physical limitations in the configuration space. The main approach is to use a genetic algorithm (GA) to perform a global search of the robot's configuration parameters to solve the optimization model and obtain the steady-state optimal configuration parameters when the robot reaches the target point. The optimization objective is defined as minimizing the position error of the end effector, and the obstacle avoidance requirement is transformed into a hard constraint. Its mathematical model is as follows: ; ; ; Among them, P tip P represents the position of the robot's end effector. target For the target location, O env Let r0 be the set of environmental obstacles, and Θ be the safety redundancy distance. min and Θ max These represent the lower and upper bounds of the configuration space, respectively. Then, regarding obstacle avoidance requirements, the obstacle avoidance constraints between the robot body and environmental obstacles are defined using the N feature points S discretized from the CCCR centerline above, ensuring they satisfy the Euclidean distance inequality constraint. Therefore, the obstacle avoidance constraint function in step S2 is: ; In the formula, c k r is the center position of the spherical obstacle. k Let r be the radius of the spherical obstacle, and r0 be the safety redundancy distance.

[0040] It should be noted that for non-spherical obstacles, a similar bounding box or distance field mapping method is used to determine the safety threshold.

[0041] Step S3: Using the optimal configuration parameters as the endpoint boundary conditions, generate a collision-free geometric path from the initial pose to the endpoint configuration that satisfies curvature constraints. Perform smooth optimization on the geometric path and map it to the joint space to generate the desired configuration sequence.

[0042] Among them, based on optimal configuration parameters As the endpoint boundary condition, a continuous transition path and corresponding configuration sequence from the initial pose to the endpoint configuration are planned. This embodiment uses an improved rapidly-exploring random tree (RRT) that incorporates curvature constraints. The algorithm is used to solve the geometric path. In step S3, a collision-free geometric path that satisfies curvature constraints is generated from the initial pose to the final configuration, including: An improved fast expanding random tree algorithm is used for path search, and the node cost function of the improved fast expanding random tree algorithm is improved. Defined as: ; In the formula, w1 and w c These are the weighting coefficients for path length and curvature magnitude, respectively, d(q) n ,q r ) is the new node q n With its parent node q r Euclidean distance between them, κ(q) n ,q r ) represents the equivalent curvature of the path segment, κ. max This represents the maximum allowable bending curvature for the robot.

[0043] To further improve the smoothness of the path and meet the requirements of FTL motion, step S3 involves smoothing the geometric path, including: Step S31: Perform smoothing optimization using the variational method. The optimized smooth path is represented as a discrete point sequence P={x1,x2,…,x}. M The original path point is The optimization problem of the variational method is: ; ; In the formula, , x is the curvature smoothing weighting coefficient. i x i-1 and x i+1 For smooth path points, , and For the original path point, κ(x) i (x) represents the smooth path point. i curvature, κ max This represents the maximum allowable bending curvature for the robot.

[0044] Step S32: Map the smoothed and optimized geometric path to the joint space and solve for the configuration sequence: ; ; In the formula, x k p is the reference trajectory point j Let q be the feature point after discretizing the robot's centerline. start qgoal represents the initial pose and the final pose.

[0045] Therefore, by minimizing the distance between the two, solving this multi-objective optimization model can generate a safe, continuous, and leader-follower optimal motion configuration sequence. (See details...) Figure 3 This is the optimal configuration planning state diagram of this embodiment, where {b}, {e}, and {k} are the base coordinate system, end-effector coordinate system, and virtual joint coordinate system, respectively, and θ O θ is the outer layer bending angle. I c is the inner layer bending angle. k1 c k2 r represents the center position of the spherical obstacle. k1 r k2 R is the radius of the spherical obstacle. r The r0 mentioned above represents the safety redundancy distance, and d represents the actual distance between the robot's centerline feature point and the obstacle.

[0046] Step S4: Real-time acquisition of the current actual configuration vector of the concentric composite continuum robot. Taking the desired configuration sequence as the target, construct a stability constraint optimization problem based on the tracking error between the current actual configuration vector and the corresponding desired configuration vector in the desired configuration sequence, and solve it in real time. Update the configuration parameters according to the optimal configuration update amount obtained from the solution, and repeat the process until the current actual configuration converges to the desired configuration sequence.

[0047] This embodiment provides a leader-follower motion control method based on Control Lyapunov Function (CLF) and Quadratic Programming (QP). This method transforms FTL control into a nonlinear time-varying optimization problem with stability constraints, achieving high-precision and highly stable configuration tracking while ensuring the robot strictly adheres to environmental constraints.

[0048] Specifically, step S4 constructs a stability-constrained optimization problem based on the tracking error between the current actual configuration vector and the corresponding desired configuration vector in the desired configuration sequence, including: Step S41, with the desired configuration sequence ∈C is the target configuration, and the control Lyapunov function V( Let ) be a quadratic function as follows: ; In the formula, M is the current actual configuration vector, and M is a symmetric positive definite weight matrix used to decouple the motion flexibility of the inner continuum and the outer continuum.

[0049] Step S42: Control the optimal update amount of configuration parameters within the cycle using the following expression: ; ; ; In the formula, This represents the optimal update amount of the configuration parameters within the current control cycle. Let λ be the desired configuration at the next time step, and λ be the convergence coefficient. This is the actual configuration vector at the next moment. and These are the lower and upper bounds of the configuration space, respectively.

[0050] Step S42 involves obtaining the optimal update expression by solving a quadratic programming optimization problem with hard constraints within a controlled Lyapunov framework. The first inequality is the Lyapunov exponential decay condition, ensuring that the system's generalized energy decreases strictly to achieve asymptotic stability. The second inequality is the physical driving limit constraint of the configuration space. By solving this QP optimization problem in real time, the optimal configuration increment is obtained, enabling the robot to achieve FTL motion following while simultaneously satisfying both the physical constraints of the robot body and the kinematic stability criteria.

[0051] In this embodiment, after updating the configuration parameters according to the obtained optimal configuration update amount in step S4, the method further includes: When the optimization problem fails to be solved, it automatically reverts to the preset heuristic strategy for fault tolerance correction. The heuristic strategy is to recursively calculate the current actual configuration vector along the direction of the desired configuration sequence with a fixed step size.

[0052] In summary, the various embodiments of the present invention have the following technical effects: First, by introducing the coupling compensation amount caused by the bending of the outer continuum into the driving mapping of the inner continuum, the nonlinear coupling effect between the inner and outer tubes is accurately characterized, overcoming the defect of the failure of the traditional single tube kinematic model in concentric composite structures, providing a more reliable geometric basis for the accurate calculation of the overall shape of the robot, and thus improving the system's tracking accuracy and robustness of the target path.

[0053] Second, by minimizing the end-effector position error and solving for the optimal end-effector configuration in conjunction with obstacle avoidance constraints, and then using this configuration as the boundary, an improved fast extended random tree algorithm with curvature constraints combined with variational methods is used to smoothly optimize and generate a collision-free geometric path that satisfies the curvature constraints. This path is then mapped to the desired configuration sequence. This not only ensures that the generated geometric path strictly meets the robot's physical curvature constraints, but also achieves the leader-follower motion characteristics, minimizing the sweep space during the motion process. This effectively solves the problem of global obstacle avoidance and smooth transition for continuum robots in complex and confined environments, and significantly improves the operational safety in narrow environments.

[0054] Third, by collecting actual configurations in real time and constructing a quadratic programming optimization problem with controllable exponential decay of Lyapunov functions based on tracking errors, and combining it with a cyclic convergence mechanism and a heuristic fault-tolerant correction strategy when the solution fails, the asymptotic convergence of the robot configuration to the desired trajectory is ensured, achieving high-precision and high-stability configuration tracking in complex and confined spaces.

[0055] In summary, the method and system provided by this invention fully leverage the structural advantages of concentric composite continuum robots while achieving efficient, robust, and universal leader-follower motion control.

[0056] Example 2 Please refer to Figure 4 A concentric composite continuum robot leader-follower control system 1 includes a memory 3, a processor 2, and a computer program stored in the memory 3 and executable on the processor 2. When the processor 2 executes the computer program, it implements the steps in the above embodiment 1.

[0057] Since the systems / devices described in the above embodiments of the present invention are systems / devices used to implement the methods of the above embodiments of the present invention, those skilled in the art can understand the specific structure and modifications of the systems / devices based on the methods described in the above embodiments of the present invention, and therefore will not be repeated here. All systems / devices used in the methods of the above embodiments of the present invention fall within the scope of protection of the present invention.

[0058] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, apparatus, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0059] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (devices), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, as well as combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions.

[0060] It should be noted that any reference numerals placed between parentheses in the claims should not be construed as limiting the claims. The word "comprising" does not exclude the presence of components or steps not listed in the claims. The word "a" or "an" preceding a component does not exclude the presence of a plurality of such components. The invention can be implemented by means of hardware comprising several different components and by means of a suitably programmed computer. In claims that enumerate several means, several of these means may be embodied by the same hardware. The use of the terms first, second, third, etc., is merely for convenience of expression and does not indicate any order. These terms can be understood as part of the component names.

[0061] Furthermore, it should be noted that in the description of this specification, the terms "one embodiment," "some embodiments," "embodiment," "example," "specific example," or "some examples," etc., refer to specific features, structures, materials, or characteristics described in connection with that embodiment or example, which are included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Furthermore, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0062] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the claims should be interpreted to include both the preferred embodiments and all changes and modifications falling within the scope of the invention.

[0063] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, then this invention should also include these modifications and variations.

Claims

1. A leader-follower control method for a concentric composite continuum robot, characterized in that, include: Establish the mutual mapping relationship between the displacement of the driving tendon of the outer continuum and the outer configuration parameters, and the mutual mapping relationship between the displacement of the driving tendon of the inner continuum and the inner configuration parameters. Calculate the end-effector pose of the concentric composite continuum robot based on the outer and inner configuration parameters, and generate a discrete spatial coordinate point set for the centerline. The mutual mapping relationship between the displacement of the driving tendon of the inner continuum and the inner configuration parameters includes the coupling compensation amount caused by the bending of the outer continuum. With the goal of minimizing the end-effector pose and approximating the target pose, and with the obstacle avoidance constraints between the discrete spatial coordinate point set and the obstacle set and the physical constraint of the configuration space as the constraint conditions, the optimal configuration parameters of the endpoint state are solved. Using the optimal configuration parameters as the endpoint boundary conditions, a collision-free geometric path that satisfies curvature constraints is generated from the initial pose to the endpoint configuration. The geometric path is then smoothed and optimized, mapped to the joint space, and the desired configuration sequence is generated. The current actual configuration vector of the concentric composite continuum robot is collected in real time. Taking the desired configuration sequence as the target, a stability constraint optimization problem is constructed based on the tracking error between the current actual configuration vector and the corresponding desired configuration vector in the desired configuration sequence, and solved in real time. The configuration parameters are updated according to the optimal configuration update amount obtained by the solution. This process is repeated until the current actual configuration converges to the desired configuration sequence.

2. The leader-follower control method for a concentric composite continuum robot according to claim 1, characterized in that, The mapping function between the tendon displacement driven by the inner continuum and the inner layer configuration parameters is as follows: ； ； In the formula, This represents the total change in the driving length of the inner tendon. This refers to the length increment caused by the bending of the inner continuum itself. This is the compensation amount for inner layer coupling caused by the bending of the outer continuum. r represents the change in the i-th tendon of the inner layer. I Let θ be the radius of the inner tendon distribution circle. O The outer bending angle, O ψ is the azimuth angle of the curved plane. i Let be the position angle of the i-th tendon.

3. The leader-follower control method for a concentric composite continuum robot according to claim 1, characterized in that, The stability-constrained optimization problem constructed using the tracking error between the current actual configuration vector and the corresponding desired configuration vector in the desired configuration sequence includes: With desired configuration sequence ∈C is the target configuration, and the control Lyapunov function V( Let ) be a quadratic function as follows: ； In the formula, Let M be the current actual configuration vector, and M be a symmetric positive definite weight matrix used to decouple the motion flexibility of the inner continuum and the outer continuum. The optimal update amount of the configuration parameters within a period is controlled by the following expression: ； ； ； In the formula, This represents the optimal update amount of the configuration parameters within the current control cycle. Let λ be the desired configuration at the next time step, and λ be the convergence coefficient. This is the actual configuration vector at the next moment. and These are the lower and upper bounds of the configuration space, respectively.

4. The leader-follower control method for a concentric composite continuum robot according to claim 1, characterized in that, Generating a collision-free geometric path from the initial pose to the final configuration, satisfying curvature constraints, includes: Path search is performed using an improved fast expanding random tree algorithm, wherein the node cost function of the improved fast expanding random tree algorithm is... Defined as: ； In the formula, w1 and w c These are the weighting coefficients for path length and curvature magnitude, respectively, d(q) n ,q r ) is the new node q n With its parent node q r Euclidean distance between them, κ(q) n ,q r ) represents the equivalent curvature of the path segment, κ. max This represents the maximum allowable bending curvature for the robot.

5. The leader-follower control method for a concentric composite continuum robot according to claim 1, characterized in that, The smoothing optimization of the geometric path includes: Variational optimization is used for smoothing, and the optimized smooth path is represented as a discrete point sequence P={x1,x2,…,x}. M The original path point is The optimization problem of the variational method is: ； ； In the formula, , x is the curvature smoothing weighting coefficient. i x i-1 and x i+1 For smooth path points, , and For the original path point, κ(x) i (x) represents the smooth path point. i curvature, κ max The maximum bending curvature allowed for the robot; Map the smoothed and optimized geometric path to joint space and solve for the configuration sequence: ； ； In the formula, x k p is the reference trajectory point j Let q be the feature point after discretizing the robot's centerline. start q goal For the initial pose and the final pose.

6. The leader-follower control method for a concentric composite continuum robot according to claim 1, characterized in that, The mapping function between the tendon displacement driven by the outer continuum and the outer configuration parameters is as follows: ； In the formula, Δl O R is the amount of change in the length of the driving tendon. O θ is the radius of the outer tendon distribution circle. O The outer bending angle, O ψ1, ψ2, and ψ3 are the azimuth angles of the curved plane, and the position angles of the 1st, 2nd, and 3rd tendons, respectively.

7. The leader-follower control method for a concentric composite continuum robot according to claim 1, characterized in that, The obstacle avoidance constraint function is: ； In the formula, c k r is the center position of the spherical obstacle. k Let r be the radius of the spherical obstacle, and r0 be the safety redundancy distance.

8. A leader-follower control method for a concentric composite continuum robot according to any one of claims 1 to 7, characterized in that, The calculation of the end-effector pose of the concentric composite continuum robot based on the outer layer configuration parameters and the inner layer configuration parameters includes: Using the piecewise constant curvature assumption, the outer continuum and the inner continuum are discretized into multiple virtual joints, and the homogeneous transformation matrix of each virtual joint is established. The homogeneous transformation matrices of each virtual joint are multiplied together to obtain the transformation matrix of the end of the outer continuum relative to the base. Using the pose of the inner continuum extending from the end of the outer continuum as the initial reference, a transformation matrix is ​​established relative to the end of the outer continuum. Multiply the transformation matrix of the outer continuum end relative to the base with the transformation matrix of the inner continuum end relative to the outer continuum end to obtain the complete transformation matrix of the robot end effector relative to the base; The position vectors of the origins of each coordinate system are recursively extracted from the transformation matrix sequence by the coordinate extraction operator, and the extracted position vectors of the origins of each coordinate system constitute the set of discrete spatial coordinate points of the centerline.

9. A leader-follower control method for a concentric composite continuum robot according to any one of claims 1 to 7, characterized in that, After updating the configuration parameters based on the optimal configuration update amount obtained from the solution, the process further includes: When the optimization problem fails to be solved, it automatically reverts to a preset heuristic strategy for fault tolerance correction. The heuristic strategy is to recursively calculate the current actual configuration vector along the direction of the desired configuration sequence with a fixed step size.

10. A concentric composite continuum robot leader-follower control system, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements a leader-follower control method for a concentric composite continuum robot as described in any one of claims 1 to 9.

Images