Asymmetric flexible robot motion control method and system based on cc-prb model

Abstract

The application discloses a non-symmetrical flexible robot motion control method and system based on a CC-PRB model, and the method comprises the following steps: constructing a continuum robot with structural asymmetry, and obtaining a target end position offset; performing CC-PRB forward kinematics estimation on the target end position offset to obtain a current end pose; performing inverse kinematics iteration to obtain optimal curvature parameters of the continuum robot; performing motor pulse calculation and friction compensation on a total length change of a guide line to obtain absolute motor pulse numbers corresponding to the guide line of the continuum robot; setting a safety threshold, estimating an end contact force through a sensorless method to obtain a three-dimensional contact reaction force of the end of the continuum robot, and driving a closed-loop stepping motor to complete motion. The application can improve the end position control precision of the continuum robot under a large bending angle. The application can be widely applied to the technical field of robot motion control.

Description

Technical Field

[0001] This invention relates to the field of robot motion control technology, and in particular to an asymmetric flexible robot motion control method and system based on the CC-PRB model. Background Technology

[0002] Continuum robots consist of a flexible, elastic skeleton and multiple guide lines, and are widely used in surgeries involving narrow cavities (such as endotracheal intubation and gastrointestinal endoscopy). The current mainstream modeling method is the segmented constant curvature model (PCC / CC), which assumes that each segment of the robot is uniformly curved along the arc length and maps the changes in guide line length to the end-effector pose solely through pure geometric relationships.

[0003] However, the existing CC model has the following shortcomings, for example: 1) Symmetry axis assumption: Existing methods all assume that the bending axes of adjacent joints are orthogonal (90° / 0°) or parallel, resulting in a uniform distribution of the workspace and an inability to concentrate bending capacity in a specific direction (such as the direction of lifting the epiglottis in endotracheal intubation).

[0004] 2) S-shaped bending problem: When the 8 joint angles are solved as independent variables, the inverse kinematic solution is prone to an S-shaped configuration with adjacent joints bending in opposite directions, which violates the physical constraints of the spring hinge structure.

[0005] 3) Spring compression not included in the model: The tension of the guide line causes axial compression of the spring, making the actual length of the joint less than the free length. Existing methods do not model this, resulting in a systematic deviation in the height of the end in the Z direction. Summary of the Invention

[0006] To address the aforementioned technical problems, the present invention aims to provide an asymmetric flexible robot motion control method and system based on the CC-PRB model, which can improve the end-effector position control accuracy of a continuum robot under large bending angles.

[0007] The first technical solution adopted in this invention is: an asymmetric flexible robot motion control method based on the CC-PRB model, comprising the following steps: Construct a structurally asymmetric continuum robot and obtain the target end-effector position offset of the continuum robot; The total length change of the guide line of the continuum robot is determined, and the CC-PRB positive kinematics estimation is performed on the target end position offset of the continuum robot to obtain the current end pose of the continuum robot. The optimal curvature parameters of the continuum robot are obtained by performing inverse kinematics iterative solution on the current end-effector pose. Based on the optimal curvature parameters of the continuum robot, motor pulse calculation and friction compensation are performed on the total length change of the guide line of the continuum robot to obtain the absolute number of motor pulses corresponding to the guide line of the continuum robot. A safety threshold is set based on the absolute pulse count of the motor corresponding to the guide line of the continuum robot. The end contact force is estimated without sensors to obtain the three-dimensional contact reaction force at the end of the continuum robot, which drives the closed-loop stepper motor to complete the motion.

[0008] Furthermore, the step of constructing a structurally asymmetric continuum robot and obtaining the target end-effector position offset of the continuum robot specifically includes: Construct a continuum robot with structural asymmetry, wherein the structural asymmetry refers to the staggered arrangement of the bending axes of the continuum robot, with odd-numbered joints bending around a 90° direction axis and even-numbered joints bending around a 30° direction axis; The image of the target object is extracted using a continuum robot vision system with structural asymmetry, and the center pixel deviation of the target object image is obtained. The image center pixel deviation of the target object is converted into the target end position offset of the continuum robot.

[0009] Furthermore, the step of performing CC-PRB positive kinematic estimation on the target end-effector position offset of the continuum robot to obtain the current end-effector pose of the continuum robot specifically includes: The target end position offset is converted into a two-dimensional global curvature vector of the continuum robot and then subjected to joint angle projection processing to determine the rotation angle of each joint of the continuum robot. Define the unit vector of the joint bending axis of the continuum robot and the position vector of the fixed point of the guide line on the isolation disk, and derive the axial displacement of the guide line to obtain the total length change of the guide line of the continuum robot. The spring compression is calculated based on the total change in the length of the guide wire of the continuum robot, and the actual length of each joint of the continuum robot is obtained. By combining the rotation angles of each joint of the continuum robot with the actual length of each joint, the current end-effector pose of the continuum robot is obtained through chain-like product of homogeneous transformation matrices.

[0010] Furthermore, the step of defining the unit vector of the joint bending axis of the continuum robot and the position vector of the fixed point of the guide line on the isolation disk, and deriving the axial displacement of the guide line to obtain the total length change of the guide line of the continuum robot, specifically includes: Construct a coordinate system for the continuum robot, and define the unit vector of the bending axis of the j-th joint and the position vector of the fixed point of the i-th guide line on the isolation disk; Based on the fundamental kinematic principle of rigid body rotation about an axis, the displacement vector of the fixed point of the guide line is determined according to the unit vector of the bending axis of the j-th joint and the position vector of the fixed point of the i-th guide line on the isolation plate. Perform a cross product operation between the unit vector of the bending axis of the j-th joint and the position vector of the fixed point of the i-th guide line on the isolation plate to determine the effective rotation radius of the guide line; By integrating the displacement vector of the fixed point of the guide line, a finite bending angle is obtained. Combined with the effective rotation radius of the guide line, a geometric projection is performed to obtain the change in the length of the guide line at a single joint of the continuum robot. The total change in guideline length of the continuous robot is obtained by superimposing the changes in the length of the guidelines of all the single joints.

[0011] Furthermore, the step of calculating the spring compression based on the total change in the length of the guide wires of the continuum robot to obtain the actual length of each joint of the continuum robot specifically includes: The axial compressive force applied by the spring is determined based on the total change in the guide line length of the continuum robot. By introducing the position weights of each joint and combining them with the axial compressive force applied by the springs, the theoretical and actual net lengths of the springs in each joint are calculated to obtain the actual lengths of each joint of the continuum robot. The position weights of each joint are used to quantify the differences in the compression distribution of joints at different positions.

[0012] Furthermore, the step of performing inverse kinematics iterative solution on the current end-effector pose of the continuum robot to obtain the optimal curvature parameters of the continuum robot specifically includes: Load global control parameters, which include numerical difference step size, maximum single-step step size, convergence threshold, maximum number of iterations, and damping regularization weights. Set the two-dimensional global curvature vector of the continuum robot as the initial curvature parameter; The initial curvature parameters are input into the forward kinematics model for estimation to obtain the curvature parameter increments; The curvature parameters are repeatedly incrementally input into the positive kinematics model for estimation until the global control parameters are satisfied, thus obtaining the optimal curvature parameters for the continuum robot.

[0013] Furthermore, the step of inputting the initial curvature parameters into the forward kinematics model for estimation to obtain the curvature parameter increments specifically includes: The initial curvature parameters are input into the forward kinematics model for estimation to obtain the current actual position of the end effector of the continuum robot. Calculate the difference between the current end-effector pose and the actual current end-effector position of the continuous robot to determine the 3D end-effector position error vector of the continuous robot. By applying a perturbation to the initial curvature parameters, the end-effector position of the continuum robot after perturbation is obtained; A numerical Jacobian matrix is ​​constructed by calculating the difference between the end-effector position of the continuum robot after perturbation and the current actual end-effector position of the continuum robot. The numerical Jacobian matrix and the 3D end-effector position error vector of the continuum robot are substituted into the preset damped least squares closed-form update formula to calculate the curvature parameter increment.

[0014] Furthermore, the step of calculating the total length change of the guide wire of the continuum robot based on the optimal curvature parameters of the continuum robot using motor pulses and friction compensation to obtain the absolute number of motor pulses corresponding to the guide wire of the continuum robot specifically includes: The total change in the length of the guide wire of the continuous robot is converted into the theoretical amount of wire pulled at the motor end of the continuous robot. Considering the frictional loss of the guide wire of the continuum robot in the channel of the isolation disk, the Capstan friction equation is used and combined with the optimal curvature parameters of the continuum robot to compensate for the theoretical wire pulling amount at the motor end of the continuum robot under different working conditions, and the theoretical wire pulling amount after frictional loss compensation is determined. The theoretical draw length after friction loss compensation is converted into pulse counts to obtain the absolute number of motor pulses corresponding to the guide wire of the continuum robot.

[0015] Furthermore, the step of setting a safety threshold based on the absolute pulse count of the motor corresponding to the guide line of the continuum robot, estimating the end-effector contact force without sensors to obtain the three-dimensional contact reaction force at the end of the continuum robot, and driving the closed-loop stepper motor to complete the motion specifically includes: Load the pre-calibrated system core parameters, which include the spring torsional stiffness matrix, the lead wire-joint matrix, and the safety threshold. The safety threshold is determined by the absolute number of motor pulses corresponding to the guide wire of the continuum robot. The actual drive current value of each motor of the continuous robot is determined based on the rotation angle of each joint of the continuous robot. Based on the PRB quasi-static equilibrium principle, the relationship between the spring restoring torque and the guide wire tension torque is determined by combining the spring torsional stiffness matrix and the guide wire-joint matrix. The theoretical equivalent tension vector of the guide wire of the continuum robot in non-contact condition is obtained by inverse solution of the least squares method. The actual tension of the current guide line of the continuous robot is calculated by using the calibration mapping relationship between the actual driving current value of each motor of the continuous robot and the theoretical equivalent tension vector of the guide line of the continuous robot when there is no contact. The difference in drive current between each motor of the continuum robot is determined by the difference between the theoretical equivalent tension vector of the guide line of the continuum robot under non-contact conditions and the actual tension of the current guide line of the continuum robot. Considering the safety threshold, the difference in drive current between each motor of the continuum robot is calculated by inverse kinematics to determine the three-dimensional contact reaction force at the end of the continuum robot, which drives the closed-loop stepper motor to complete the motion.

[0016] The second technical solution adopted in this invention is: an asymmetric flexible robot motion control system based on the CC-PRB model, comprising: The first module is used to construct a structurally asymmetric continuum robot and obtain the target end-effector position offset of the continuum robot. The second module is used to determine the total length change of the guide wire of the continuum robot and to perform CC-PRB positive kinematic estimation on the target end position offset of the continuum robot to obtain the current end pose of the continuum robot. The third module is used to perform inverse kinematics iterative solution on the current end pose of the continuum robot to obtain the optimal curvature parameters of the continuum robot. The fourth module is used to calculate motor pulses and perform friction compensation on the total length change of the guide line of the continuum robot based on the optimal curvature parameters of the continuum robot, so as to obtain the absolute number of motor pulses corresponding to the guide line of the continuum robot. The fifth module is used to set a safety threshold based on the absolute pulse count of the motor corresponding to the guide line of the continuum robot, estimate the end contact force without sensors, obtain the three-dimensional contact reaction force at the end of the continuum robot, and drive the closed-loop stepper motor to complete the motion.

[0017] The beneficial effects of the method and system of this invention are as follows: This invention constructs a structurally asymmetric continuum robot, obtains the target end-effector position offset of the continuum robot, and uniformly parameterizes the angles of the eight asymmetric joints using two global curvature parameters. Simultaneously, it physically eliminates S-shaped curvature through a monotonically increasing scaling factor, making it suitable for kinematic modeling of continuum robots with arbitrary intersecting bending axis angles. Furthermore, it determines the total length change of the guide wire of the continuum robot and performs CC-PRB forward kinematic estimation on the target end-effector position offset to obtain the current end-effector pose. The guide wire tension is mapped to the spring compression of each joint with position weights, correcting the actual joint length in the HTM chain and improving the Z-direction accuracy of the end-effector under large bending angles. Finally, it performs inverse kinematic iteration on the current end-effector pose of the continuum robot to obtain... The optimal curvature parameters of the continuum robot are obtained, enabling rapid solution of damped least-squares inverse kinematics and meeting real-time control requirements. Based on the optimal curvature parameters of the continuum robot, motor pulse calculation and friction compensation are performed on the total length change of the guide line of the continuum robot to obtain the absolute number of motor pulses corresponding to the guide line. Sensorless end contact force estimation is achieved through static equilibrium equations to construct an active safety protection mechanism. Finally, a safety threshold is set according to the absolute number of motor pulses corresponding to the guide line of the continuum robot. The end contact force is estimated without sensors to obtain the three-dimensional contact reaction force at the end of the continuum robot, driving the closed-loop stepper motor to complete the motion. This can introduce position increment scaling, physically eliminate S-shaped curvature, and couple spring compression statics into forward kinematics, significantly improving the end position accuracy under large bending angles. Attached Figure Description

[0018] Figure 1 This is a flowchart of the steps of the asymmetric flexible robot motion control method based on the CC-PRB model of the present invention; Figure 2 This is a structural block diagram of the asymmetric flexible robot motion control system based on the CC-PRB model of this invention; Figure 3 This is a schematic diagram of the complete closed-loop process of the CC-PRB control algorithm provided in a specific embodiment of the present invention; Figure 4 This is a schematic diagram of the inverse kinematic damping least squares iterative solution provided in a specific embodiment of the present invention; Figure 5 This is a schematic diagram of the sensorless force estimation and active safety protection decision-making process provided in a specific embodiment of the present invention. Detailed Implementation

[0019] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments. The step numbers in the following embodiments are only for ease of explanation and do not limit the order of the steps. The execution order of each step in the embodiments can be adapted according to the understanding of those skilled in the art.

[0020] Reference Figure 1 and Figure 3 This invention provides a motion control method for an asymmetric flexible robot based on the CC-PRB model, the method comprising the following steps: S100. Construct a continuum robot with structural asymmetry and obtain the target end position offset of the continuum robot. Specifically, a structurally asymmetric continuum robot is constructed, wherein the structural asymmetry refers to the staggered arrangement of the bending axes of the continuum robot, with odd-numbered joints bending around a 90° direction axis and even-numbered joints bending around a 30° direction axis; an image of a target object is extracted through a vision system of the structurally asymmetric continuum robot, and the pixel deviation of the center of the target object's image is obtained; the pixel deviation of the center of the target object's image is converted into the target end position offset of the continuum robot.

[0021] In this embodiment, the continuum robot targeted by the present invention includes: 1) Proximal passive section: 200mm long stainless steel threaded rigid pipe, which only transmits axial thrust and does not participate in bending; 2) Distal active section: It consists of 9 isolation discs (each disc is 4mm thick) and 8 compression springs (each with a free net length of 7mm) alternatingly, with a total length of 92mm; 3) End plate: The 9th isolation plate (4mm) is used to fix the endoscope camera; 4) Drive guide wires: Three Kevlar guide wires are evenly distributed at 120° intervals (arranged at angles of 180°, 60°, and 300° respectively), with an off-axis radius of... .

[0022] Therefore, the total length of the active segment is .

[0023] The structural asymmetry stems from the staggered arrangement of the bending axes: odd-numbered joints (joints 1, 3, 5, and 7) bend around a 90° axis, while even-numbered joints (joints 2, 4, 6, and 8) bend around a 30° axis, with the two axes differing by 60°. This non-orthogonal design results in an elliptical working space, increasing the maximum extension radius in the epiglottic lifting direction (270°) by 27.7% (42.8 mm vs. 33.5 mm) compared to an orthogonal design (90° / 0°), while maintaining a relatively constant total working space area. right ).

[0024] Furthermore, the current position of the motor encoder is read and converted into the actual length of the guide wire. Read the motor drive current for subsequent contact force estimation.

[0025] Identify glottis / target markers using the YOLO vision system and extract image center deviation. (pixels); Convert image deviation to target end position offset as follows:

[0026]

[0027]

[0028] In the above formula, , Indicates image center deviation. This represents the curvature control amount in the X direction of the robot. This represents the curvature control amount in the robot's Y-direction. This indicates the visual servo proportional gain.

[0029] S200. Determine the total length change of the guide line of the continuum robot, and perform CC-PRB positive kinematic estimation on the target end position offset of the continuum robot to obtain the current end pose of the continuum robot. S210. Convert the target end position offset into a two-dimensional global curvature vector of the continuum robot and perform joint angle projection processing to determine the joint angles of the continuum robot. In this embodiment, a two-dimensional global curvature vector is defined. ,in , These represent the equivalent bending curvature of the robot in the horizontal X and Y directions (unit: rad). The entire bending state of the robot is completely described by only these two parameters, reducing the 8-DOF problem to a 2-DOF solution, significantly reducing computational complexity.

[0030] No. The rotation angles of the joints (j = 1, 2, ..., 8) The global curvature parameter is calculated using the following formula, the expression of which is:

[0031] In the above formula, This represents the rotation angles of each joint in a continuum robot. , Represents the global curvature parameter. Indicates the first The bending axis angle of each joint This represents the projection coefficient of the bending axis in the X direction. This represents the projection coefficient of the bending axis in the Y direction. This indicates the position-incrementing scaling factor.

[0032] It should be noted that the scaling factor ( hour , hour Based on the following physical observations: Due to the constraint effect of the 200mm passive segment on the proximal active joint, the proximal joint ( Near the end joint (where the actual bending angle is smallest) The bending angle is maximized. Incremental scaling ensures that all joint angles have the same sign as the global curvature parameter, physically eliminating the S-shaped configuration of adjacent joints bending in opposite directions, a configuration that is impossible in spring hinge structures but frequently occurs in independent solution schemes.

[0033] S220. Define the unit vector of the joint bending axis of the continuum robot and the position vector of the fixed point of the guide line on the isolation disk, and derive the axial displacement of the guide line to obtain the total length change of the guide line of the continuum robot. Specifically, a coordinate system for the continuum robot is constructed, defining the unit vector of the bending axis of the j-th joint and the position vector of the fixed point of the i-th guide line on the isolation disk. Based on the fundamental kinematic principle of rigid body rotation about an axis, the displacement vector of the fixed point of the guide line is determined according to the unit vector of the bending axis of the j-th joint and the position vector of the fixed point of the i-th guide line on the isolation disk. The effective rotation radius of the guide line is determined by performing a cross product operation on the unit vector of the bending axis of the j-th joint and the position vector of the fixed point of the i-th guide line on the isolation disk. The finite bending angle is obtained by integrating the displacement vector of the fixed point of the guide line, and geometric projection is performed in combination with the effective rotation radius of the guide line to obtain the change in length of the guide line of a single joint of the continuum robot. The changes in length of the guide lines of all single joints of the continuum robot are superimposed to obtain the total change in length of the guide lines of the continuum robot.

[0034] In this embodiment, firstly, let the first... The unit vector of the bending axis of each joint is:

[0035] Guide lines The fixed position vector on the isolation disk is ( The off-axis radius of the guide line. (for the arrangement angle)

[0036] When joints Around the axis Turned at a tiny angle At that time, the displacement vector of the fixed point of the guide line is:

[0037] Calculate the cross product Its expression is:

[0038] The above formula gives the tangential displacement of the guide wire fixing point in the tangential plane of the disk. The axial length change of the guide wire (along the robot axis Z direction) is determined by the guide wire offset distance. With bending angle The geometric projection along the axial direction determines the correct formula:

[0039] in, The item indicates when the leader line Located in the direction of the bending axis ( When the angle between the guide line and the bending axis is 0°, cos=1, and the line length shortens to the maximum; when the guide line is perpendicular to the bending axis (angle 90°), cos=0, and the line length remains unchanged. The negative sign indicates that the line shortens (tension increases) when bending towards the guide line.

[0040] The contributions of the eight joints are summed to obtain the guide line. The total change in length is expressed as follows:

[0041] Write it in matrix form (leader-joint geometry mapping matrix) Its expression is:

[0042]

[0043] In the above formula, Indicates the leader line The total change in length, This represents the lead-joint geometry mapping matrix. Indicates only the first When the first joint is bent, the second joint... The change in the length of a single joint caused by the root guide line.

[0044] S230. Calculate the spring compression based on the total change in the length of the guide wire of the continuum robot to obtain the actual length of each joint of the continuum robot. Specifically, the axial compressive force applied by the spring is determined based on the total length change of the guide line of the continuum robot; the position weights of each joint are introduced, and the theoretical and actual net lengths of the springs of each joint are calculated in combination with the axial compressive force applied by the springs to obtain the actual lengths of each joint of the continuum robot. The position weights of each joint are used to quantify the differences in the compression distribution of joints at different positions.

[0045] In this embodiment, only the taut guide wire ( An axial compressive force is then applied to the spring. The total tension is defined as:

[0046] in, The operation retains negative values ​​(for taut lines) and clears positive values ​​(for slack lines) to zero, because slack lines do not generate compressive force.

[0047] Because the proximal joint bears the cumulative tension of the guide lines from all the joints above it, its compression is greater than that of the distal joint, thus a positional weight is introduced. This distribution can be described by the following expression:

[0048]

[0049] In the above formula, Indicates the free net length of the spring. Indicates the spring compression coefficient. Indicates the total tension. This indicates the total number of joints.

[0050] The actual total length of each joint consists of two parts: a rigid disc (incompressible, 4mm) and an elastic spring (net length after compression), expressed as follows:

[0051] In the above formula, Indicates the first The actual net length of the spring after compression at each joint. Indicates the first The actual total length of each joint This indicates the thickness of the rigid isolation disc.

[0052] S240. Combining the rotation angles of each joint of the continuum robot with the actual lengths of each joint of the continuum robot, the current end-effector pose of the continuum robot is obtained by chaining homogeneous transformation matrices.

[0053] In this embodiment, 8 joint angles are known. and the actual length of each joint The end-effector pose is obtained by chain rule integration of homogeneous transformation matrices:

[0054] The transformation matrices are defined as follows: 1) Passive segment (pure Z-direction translation, no rotation):

[0055]

[0056] 2) No. One spring joint (Rodrigues rotation + local Z-axis translation):

[0057] Rotation matrix Calculated using Rodrigues' formula, representing the angle around the bending axis. Rotation horn:

[0058] in The antisymmetric matrix (cross product matrix) of the bending axial quantity:

[0059] 3) The 9th disk at the top (rigid translation, no rotation):

[0060]

[0061] The end position coordinates are extracted from the first three elements of the fourth column of the transformation matrix, and its expression is:

[0062] In the above formula, This represents the total homogeneous transformation matrix from the base coordinate system to the end coordinate system.

[0063] S300. Perform inverse kinematics iterative solution on the current end pose of the continuum robot to obtain the optimal curvature parameters of the continuum robot. S310. Load global control parameters, which include numerical difference step size, maximum single-step step size, convergence threshold, maximum number of iterations, and damping regularization weight. S320. Set the two-dimensional global curvature vector of the continuum robot as the initial curvature parameter; S330. Input the initial curvature parameters into the forward kinematics model for estimation to obtain the curvature parameter increments; Specifically, the initial curvature parameters are input into the forward kinematics model for estimation to obtain the current actual position of the end effector of the continuum robot; the difference between the current end effector pose and the current actual position of the end effector of the continuum robot is calculated to determine the 3D end effector position error vector of the continuum robot; a perturbation is applied to the initial curvature parameters to obtain the perturbed end effector position of the continuum robot; the difference between the perturbed end effector position and the current actual position of the end effector of the continuum robot is calculated to construct a numerical Jacobian matrix; the numerical Jacobian matrix and the 3D end effector position error vector of the continuum robot are substituted into a preset damped least squares closed-form update formula for calculation to obtain the curvature parameter increment.

[0064] S340. Based on the incremental input of curvature parameters to the positive kinematics model estimation, until the global control parameters are satisfied, the optimal curvature parameters of the continuum robot are obtained.

[0065] In this embodiment, given the target end position Find the curvature parameters that make the positive kinematic output equal to the target. .

[0066] The IK solution employs regularized least squares, simultaneously minimizing the end-position error and the curvature parameter amplitude (to prevent damage to the spring from large-angle bending). The objective function is constructed as follows:

[0067] In the above formula, Indicates the 3D end-position error. Represents the numerical Jacobian matrix. Indicates the damping regularization weight. This represents the increment of the curvature parameter.

[0068] Taking the derivative of the objective function and setting it to zero, we obtain the curvature parameter update for each iteration, expressed as:

[0069] in, Since it is a 2×2 positive definite matrix, the computational cost of directly inverting it is extremely small. This represents the gradient of the end-point error in the curvature parameter space, indicating the direction in which adjustment should be made. To reduce errors; This represents a regularization term that pulls the curvature parameter back to zero to prevent excessive bending angles from damaging the spring.

[0070] like Figure 4 The diagram illustrates the core iterative mechanism of damped least squares IK. Each iteration requires only two FK calls (one for each of the two iterations). and By applying a small perturbation, a 3×2 numerical Jacobian matrix can be calculated, and the system of 2D linear equations can be solved. The parameter update is obtained with extremely low computational cost (<1ms / iteration). The red loop on the left represents the non-converged iteration path, and the green arrow on the right represents the exit path when convergence occurs.

[0071] S400. Based on the optimal curvature parameters of the continuum robot, motor pulse calculation and friction compensation are performed on the total length change of the guide line of the continuum robot to obtain the absolute number of motor pulses corresponding to the guide line of the continuum robot. S410. Convert the total length change of the guide wire of the continuous robot into the theoretical wire pulling amount at the motor end of the continuous robot. In this embodiment, the expression for the theoretical required number of lines is:

[0072] In the above formula, Indicates the first The theoretical required amount of wire for the root guide wire based on the motor's perspective. The inverse kinematics solution yields the first... Change in the total length of the root guide line.

[0073] S420. Considering the friction loss of the guide wire of the continuous robot in the isolation disk channel, the theoretical wire pulling amount at the motor end of the continuous robot is compensated for under different working conditions by adopting the Capstan friction equation and combining it with the optimal curvature parameters of the continuous robot, and the theoretical wire pulling amount after friction loss compensation is determined. In this embodiment, the Capstan friction equation is used for compensation, and the equivalent bending angle is:

[0074]

[0075] In the above formula, This represents the Capstan friction compensation coefficient. This represents the coefficient of dynamic friction between the guide wire and the isolation disc channel. This represents the magnitude of the displacement of the end effector relative to the initial axis in the XY plane. This represents the effective length of a single segment in a continuum robot. This represents the equivalent angle of the robot's overall bending.

[0076] When retracting the line ( The motor actively pulls the cable (requiring overcoming complete friction), and its expression is:

[0077] When laying out the line ( (Spring assists in wire feeding, compensation is halved), its expression is:

[0078] In the above formula, Indicates the number after friction compensation The actual amount of wire required to be sent from the root guide wire.

[0079] S430. Calculate the pulse count of the theoretical wire pulling amount after friction loss compensation to obtain the absolute number of motor pulses corresponding to the guide wire of the continuum robot.

[0080] In this embodiment, the absolute pulse count is converted and a lower limit protection is set, the expression of which is:

[0081] In the above formula, Indicates the first The final absolute pulse count of the motor corresponding to the root guide wire. This indicates the number of initial preload bias pulses in the system. This function represents the floor function, which converts the calculated result into an integer number of pulses. This represents the step pitch coefficient of the stepper motor. This indicates a lower boundary safety protection measure, which forcibly limits the number of pulses to non-negative to prevent the motor from pushing the wire in reverse and damaging the mechanical structure.

[0082] S500: Set a safety threshold based on the absolute pulse count of the motor corresponding to the guide line of the continuum robot, estimate the end contact force without sensors, obtain the three-dimensional contact reaction force at the end of the continuum robot, and drive the closed-loop stepper motor to complete the motion.

[0083] S510. Load the pre-calibrated system core parameters, which include the spring torsional stiffness matrix, the lead wire-joint matrix, and the safety threshold. The safety threshold is determined by the absolute number of motor pulses corresponding to the guide wire of the continuum robot. S520. Determine the actual drive current value of each motor of the continuous robot based on the joint rotation angle of each joint of the continuous robot. S530. Based on the PRB quasi-static static equilibrium principle, the relationship between the spring restoring torque and the guide wire tension torque is determined by combining the spring torsional stiffness matrix and the guide wire-joint matrix. The theoretical equivalent tension vector of the guide wire of the continuum robot in non-contact condition is obtained by inverse solution of the least squares method. In this embodiment, PRB static analysis shows that, under quasi-static equilibrium, the spring restoring torque is equal to the guide line tension torque, and its expression is:

[0084] In the above formula, This represents the torsional stiffness matrix of the spring. Indicates the current joint angle. This represents the transpose of the lead-joint matrix. This represents the estimated tension values ​​of the three guide wires.

[0085] By inversely solving the tension torque of the three guide wires, the equivalent tension vector of the three guide wires is expressed as follows:

[0086] In the above formula, This represents the estimated tension values ​​of the three guide wires.

[0087] S540. Calculate the actual tension of the current guide line of the continuous robot by means of the calibration mapping relationship between the actual driving current value of each motor of the continuous robot and the theoretical equivalent tension vector of the guide line of the continuous robot when there is no contact. S550. Determine the difference in drive current of each motor of the continuous robot based on the difference between the theoretical equivalent tension vector of the guide line of the continuous robot when there is no contact and the actual tension of the current guide line of the continuous robot. S560. Considering the safety threshold, the difference in drive current between each motor of the continuous robot is calculated by inverse kinematics to determine the three-dimensional contact reaction force at the end of the continuous robot, and drive the closed-loop stepper motor to complete the motion.

[0088] In this embodiment, when the robot comes into contact with the airway wall, the static balance is disrupted, resulting in a residual between the actual joint angle and the theoretical joint angle. Utilizing the principle of a static virtual sensor, and by monitoring the actual drive current of the motor (ZDTX42S supports current reading), the end-effector contact reaction force is calculated as follows:

[0089] The estimated contact force is compared with the safety threshold to trigger a graded safety protection response, as shown in Table 1.

[0090] Table 1. Triggering Graded Safety Protection Response Data Table

[0091] like Figure 5 As shown, this illustrates the complete decision chain from motor current to tiered safety protection. The first layer ( The tension of the guide wire is estimated using the PRB static equilibrium equation. The second layer ( The estimated end contact force is obtained by correcting the motor current residual and compared with two threshold levels: if it exceeds the hard threshold... Triggering emergency retreat (to prevent airway perforation), exceeding the soft threshold Speed ​​reduction alarm (prompting operator intervention). Third-level check pulse lower limit. This is to prevent control failure caused by insufficient preload.

[0092] In summary, the embodiments of the present invention can accurately describe the end-effector pose of an 8-joint asymmetric curved axis continuum robot using only two global curvature parameters, and achieve safe and compliant closed-loop motion control and sensorless end-effector force estimation through spring compression static coupling and Capstan friction compensation.

[0093] The purpose of this invention is to: 1) Establish an accurate guide line displacement model applicable to asymmetric structures with 90° / 30° staggered bending axes to eliminate sector drive blind zones; 2) By using a position-incrementing scaling factor, the eight joint angles are compressed into two global curvature parameters, physically eliminating the S-shaped bend; 3) Incorporate the axial compression static coupling of the spring into the forward kinematics to correct the actual joint length and improve the accuracy of global modeling; 4) Achieve fast solution of damped least squares inverse kinematics to meet real-time control requirements (≥10Hz); 5) Sensorless end contact force estimation is achieved through static equilibrium equations to construct an active safety protection mechanism.

[0094] Therefore, the embodiments of the present invention have the following advantages compared with the prior art: 1) Compared to the standard CC model: it introduces incremental position scaling, physically eliminates S-shaped curvature, and couples spring compression statics into forward kinematics, significantly improving end-position accuracy under large curvature angles; 2) Compared to the independent 8-DOF PRB model: the solution dimension is compressed from 8 to 2, each step of IK only requires 2 FK calls, and a single iteration is <1ms, which meets the requirements of real-time control; 3) Compared with the virtual work principle energy functional method: the method of this invention does not require solving partial differential equations or numerical integration, the formula structure is simple, and all are matrix algebra operations, which is easy to implement in embedded systems and machine learning frameworks; 4) The sensorless force estimation mechanism avoids the engineering challenge of installing physical force sensors in narrow airways with a diameter of <10mm, and achieves real-time monitoring and safety protection of airway wall contact force through pure algorithmic means.

[0095] Reference Figure 2 An asymmetric flexible robot motion control system based on the CC-PRB model includes: The first module 201 is used to construct a structurally asymmetric continuum robot and obtain the target end position offset of the continuum robot. The second module 202 is used to determine the total length change of the guide line of the continuum robot and to perform CC-PRB positive kinematic estimation on the target end position offset of the continuum robot to obtain the current end pose of the continuum robot. The third module 203 is used to perform inverse kinematics iterative solution on the current end pose of the continuum robot to obtain the optimal curvature parameters of the continuum robot. The fourth module 204 is used to calculate motor pulses and perform friction compensation on the total length change of the guide line of the continuum robot based on the optimal curvature parameters of the continuum robot, so as to obtain the absolute number of motor pulses corresponding to the guide line of the continuum robot. The fifth module is used to set a safety threshold based on the absolute pulse count of the motor corresponding to the guide line of the continuum robot, estimate the end contact force without sensors, obtain the three-dimensional contact reaction force at the end of the continuum robot, and drive the closed-loop stepper motor to complete the motion.

[0096] The content of the above method embodiments is applicable to this system embodiment. The specific functions implemented in this system embodiment are the same as those in the above method embodiments, and the beneficial effects achieved are also the same as those achieved in the above method embodiments.

[0097] The preferred embodiments of the present application have been described above with reference to the accompanying drawings, but this is not intended to limit the scope of the embodiments of the present application. Any modifications, equivalent substitutions, and improvements made by those skilled in the art without departing from the scope and substance of the embodiments of the present application shall be within the scope of the embodiments of the present application.

Claims

1. An asymmetric flexible robot motion control method based on the CC-PRB model, characterized in that, Includes the following steps: Construct a structurally asymmetric continuum robot and obtain the target end-effector position offset of the continuum robot; The total length change of the guide line of the continuum robot is determined, and the CC-PRB positive kinematics estimation is performed on the target end position offset of the continuum robot to obtain the current end pose of the continuum robot. The optimal curvature parameters of the continuum robot are obtained by performing inverse kinematics iterative solution on the current end-effector pose. Based on the optimal curvature parameters of the continuum robot, motor pulse calculation and friction compensation are performed on the total length change of the guide line of the continuum robot to obtain the absolute number of motor pulses corresponding to the guide line of the continuum robot. A safety threshold is set based on the absolute pulse count of the motor corresponding to the guide line of the continuum robot. The end contact force is estimated without sensors to obtain the three-dimensional contact reaction force at the end of the continuum robot, which drives the closed-loop stepper motor to complete the motion.

2. The asymmetric flexible robot motion control method based on the CC-PRB model according to claim 1, characterized in that, The step of constructing a continuum robot with an asymmetric structure and obtaining the target end-effector position offset of the continuum robot specifically includes: Construct a continuum robot with structural asymmetry, wherein the structural asymmetry refers to the staggered arrangement of the bending axes of the continuum robot, with odd-numbered joints bending around a 90° direction axis and even-numbered joints bending around a 30° direction axis; The image of the target object is extracted using a continuum robot vision system with structural asymmetry, and the center pixel deviation of the target object image is obtained. The image center pixel deviation of the target object is converted into the target end position offset of the continuum robot.

3. The asymmetric flexible robot motion control method based on the CC-PRB model according to claim 2, characterized in that, The step of performing CC-PRB positive kinematic estimation on the target end-effector position offset of the continuum robot to obtain the current end-effector pose of the continuum robot specifically includes: The target end-effector position offset is converted into a two-dimensional global curvature vector of the continuum robot and then subjected to joint angle projection processing to determine the rotation angle of each joint of the continuum robot. Define the unit vector of the joint bending axis of the continuum robot and the position vector of the fixed point of the guide line on the isolation disk, and derive the axial displacement of the guide line to obtain the total length change of the guide line of the continuum robot. The spring compression is calculated based on the total change in the length of the guide wire of the continuum robot, and the actual length of each joint of the continuum robot is obtained. By combining the rotation angles of each joint of the continuum robot with the actual length of each joint, the current end-effector pose of the continuum robot is obtained through chain-like product of homogeneous transformation matrices.

4. The asymmetric flexible robot motion control method based on the CC-PRB model according to claim 3, characterized in that, The step of defining the unit vector of the joint bending axis of the continuum robot and the position vector of the fixed point of the guide line on the isolation disk, and deriving the axial displacement of the guide line to obtain the total length change of the guide line of the continuum robot, specifically includes: Construct a coordinate system for the continuum robot, and define the unit vector of the bending axis of the j-th joint and the position vector of the fixed point of the i-th guide line on the isolation disk; Based on the fundamental kinematic principle of rigid body rotation about an axis, the displacement vector of the fixed point of the guide line is determined according to the unit vector of the bending axis of the j-th joint and the position vector of the fixed point of the i-th guide line on the isolation plate. Perform a cross product operation between the unit vector of the bending axis of the j-th joint and the position vector of the fixed point of the i-th guide line on the isolation plate to determine the effective rotation radius of the guide line; By integrating the displacement vector of the fixed point of the guide line, a finite bending angle is obtained. Combined with the effective rotation radius of the guide line, a geometric projection is performed to obtain the change in the length of the guide line at a single joint of the continuum robot. The total change in guideline length of the continuous robot is obtained by superimposing the changes in the length of the guidelines of all the single joints.

5. The asymmetric flexible robot motion control method based on the CC-PRB model according to claim 4, characterized in that, The step of calculating the spring compression based on the total length change of the guide wires of the continuum robot to obtain the actual length of each joint of the continuum robot specifically includes: The axial compressive force applied by the spring is determined based on the total change in the length of the guide wire of the continuum robot. By introducing the position weights of each joint and combining them with the axial compressive force applied by the springs, the theoretical and actual net lengths of the springs in each joint are calculated to obtain the actual lengths of each joint of the continuum robot. The position weights of each joint are used to quantify the differences in the compression distribution of joints at different positions.

6. The asymmetric flexible robot motion control method based on the CC-PRB model according to claim 5, characterized in that, The step of performing inverse kinematics iteration to solve the current end-effector pose of the continuum robot to obtain the optimal curvature parameters of the continuum robot specifically includes: Load global control parameters, which include numerical difference step size, maximum single-step step size, convergence threshold, maximum number of iterations, and damping regularization weights. Set the two-dimensional global curvature vector of the continuum robot as the initial curvature parameter; The initial curvature parameters are input into the forward kinematics model for estimation to obtain the curvature parameter increments; The curvature parameters are repeatedly incrementally input into the positive kinematics model for estimation until the global control parameters are satisfied, thus obtaining the optimal curvature parameters for the continuum robot.

7. The asymmetric flexible robot motion control method based on the CC-PRB model according to claim 6, characterized in that, The step of inputting the initial curvature parameters into the forward kinematics model for estimation to obtain the curvature parameter increments specifically includes: The initial curvature parameters are input into the forward kinematics model for estimation to obtain the current actual position of the end effector of the continuum robot. Calculate the difference between the current end-effector pose and the actual current end-effector position of the continuous robot to determine the 3D end-effector position error vector of the continuous robot. By applying a perturbation to the initial curvature parameters, the end-effector position of the continuum robot after perturbation is obtained; A numerical Jacobian matrix is ​​constructed by calculating the difference between the end-effector position of the continuum robot after perturbation and the current actual end-effector position of the continuum robot. The numerical Jacobian matrix and the 3D end-effector position error vector of the continuum robot are substituted into the preset damped least squares closed-form update formula to calculate the curvature parameter increment.

8. The asymmetric flexible robot motion control method based on the CC-PRB model according to claim 7, characterized in that, The step of calculating the total length change of the guide wire of the continuum robot based on the optimal curvature parameters of the continuum robot and performing motor pulse calculation and friction compensation to obtain the absolute number of motor pulses corresponding to the guide wire of the continuum robot specifically includes: The total change in the length of the guide wire of the continuous robot is converted into the theoretical amount of wire pulled at the motor end of the continuous robot. Considering the frictional loss of the guide wire of the continuum robot in the channel of the isolation disk, the Capstan friction equation is used and combined with the optimal curvature parameters of the continuum robot to compensate for the theoretical wire pulling amount at the motor end of the continuum robot under different working conditions, and the theoretical wire pulling amount after frictional loss compensation is determined. The theoretical draw length after friction loss compensation is converted into pulse counts to obtain the absolute number of motor pulses corresponding to the guide wire of the continuum robot.

9. The asymmetric flexible robot motion control method based on the CC-PRB model according to claim 8, characterized in that, The step of setting a safety threshold based on the absolute pulse count of the motor corresponding to the guide line of the continuum robot, estimating the end-effector contact force without sensors to obtain the three-dimensional contact reaction force at the end of the continuum robot, and driving the closed-loop stepper motor to complete the motion specifically includes: Load the pre-calibrated system core parameters, which include the spring torsional stiffness matrix, the lead wire-joint matrix, and the safety threshold. The safety threshold is determined by the absolute number of motor pulses corresponding to the guide wire of the continuum robot. The actual drive current value of each motor of the continuous robot is determined based on the rotation angle of each joint of the continuous robot. Based on the PRB quasi-static equilibrium principle, the relationship between the spring restoring torque and the guide wire tension torque is determined by combining the spring torsional stiffness matrix and the guide wire-joint matrix. The theoretical equivalent tension vector of the guide wire of the continuum robot in non-contact condition is obtained by inverse solution of the least squares method. The actual tension of the current guide line of the continuous robot is calculated by using the calibration mapping relationship between the actual driving current value of each motor of the continuous robot and the theoretical equivalent tension vector of the guide line of the continuous robot when there is no contact. The difference in drive current between each motor of the continuum robot is determined by the difference between the theoretical equivalent tension vector of the guide line of the continuum robot under non-contact conditions and the actual tension of the current guide line of the continuum robot. Considering the safety threshold, the difference in drive current between each motor of the continuum robot is calculated by inverse kinematics to determine the three-dimensional contact reaction force at the end of the continuum robot, which drives the closed-loop stepper motor to complete the motion.

10. An asymmetric flexible robot motion control system based on the CC-PRB model, characterized in that, Includes the following modules: The first module is used to construct a structurally asymmetric continuum robot and obtain the target end-effector position offset of the continuum robot. The second module is used to determine the total length change of the guide wire of the continuum robot and to perform CC-PRB positive kinematic estimation on the target end position offset of the continuum robot to obtain the current end pose of the continuum robot. The third module is used to perform inverse kinematics iterative solution on the current end pose of the continuum robot to obtain the optimal curvature parameters of the continuum robot. The fourth module is used to calculate motor pulses and perform friction compensation on the total length change of the guide line of the continuum robot based on the optimal curvature parameters of the continuum robot, so as to obtain the absolute number of motor pulses corresponding to the guide line of the continuum robot. The fifth module is used to set a safety threshold based on the absolute pulse count of the motor corresponding to the guide line of the continuum robot, estimate the end contact force without sensors, obtain the three-dimensional contact reaction force at the end of the continuum robot, and drive the closed-loop stepper motor to complete the motion.

Images