A multi-contact force sensing device and method for continuum robots based on fiber Bragg gratings

Abstract

This invention provides a multi-contact force sensing device and method for a continuum robot based on fiber optic gratings (FBGs). The device includes multiple sequentially connected unit bodies, each comprising a rigid segment and a flexible segment. A positioning block is mounted on the rigid segment of each unit body. A central cavity is formed in the center of the positioning block, and driving cavities are formed around the periphery of the positioning block. Multiple driving cavities are spaced apart circumferentially along the positioning block. A multi-core optical fiber with multiple FBG sensors is disposed within the central cavity, and a single-core optical fiber etched with multiple FBG sensors is disposed within each driving cavity. By reconstructing the tension along the entire driving filament using FBG sensors, explicit modeling of the friction between the driving filament and the driving channel is eliminated, overcoming the errors and excessive computational resource consumption caused by modeling. This method can simultaneously estimate the magnitude of multiple contact points and contact forces on the continuum robot body, providing significant assurance for the safe intervention of surgical robots.

Description

Technical Field

[0001] This invention relates to the field of force sensing technology for rope robots, and more specifically, to a multi-contact-point force sensing device and method for a continuum robot based on fiber optic gratings. Background Technology

[0002] Continuum robots have attracted increasing attention in minimally invasive surgery due to their dexterity and flexibility. Their compliant skeletons can naturally adapt to environmental obstacles, enabling them to reach deep into human cavities via winding paths. To minimize friction and tissue damage between the robot and the body's cavities, real-time sensing of the robot's interaction with the environment is crucial for safe in vivo navigation. This interaction information sensing improves human-robot interaction, ensuring safe and intuitive operation. Enhancing the force sensing of continuum robots is essential for providing detailed contact force feedback and safe operation, especially for distal lesions surrounded by intricate anatomical structures. However, achieving multi-point force sensing within the robot's small scale has remained a challenging research problem.

[0003] A Chinese patent application with publication number CN112917468B discloses a method and device for sensing the end effector force of a rope-driven flexible manipulator. The method includes establishing a coordinate system for the manipulator's links; establishing dynamic equilibrium equations and torque equilibrium equations for the end effector and the center block, and calculating the initial end effector force; establishing dynamic equilibrium equations and torque equilibrium equations for the two reciprocal arm segments, and calculating the end effector force at this time; establishing dynamic equilibrium equations and torque equilibrium equations for the multiple reciprocal arm segments, and iteratively calculating the end effector force; determining whether the error between the two calculated end effector forces is lower than a threshold, or whether the next iteration number is greater than the number of arm segments, and if so, stopping the iteration.

[0004] Existing rope-driven flexible robotic arms require the establishment of complex dynamic equilibrium equations and torque equilibrium equations. The frictional force between the drive wire and the cavity needs to be characterized by a model. The establishment of too many ideal models makes it difficult to guarantee the accuracy of force estimation. Moreover, this method can only estimate the force at the end of the robot and cannot estimate the force at other parts of the robot. Summary of the Invention

[0005] To address the shortcomings of existing technologies, the present invention aims to provide a multi-contact force sensing device and method for a continuum robot based on fiber optic gratings.

[0006] According to the present invention, a multi-contact force sensing device for a continuum robot based on fiber optic gratings includes multiple sequentially connected unit bodies. Each unit body includes a rigid segment and a flexible segment. A positioning block is mounted on the rigid segment of the unit body. A central cavity is formed in the middle of the positioning block, and a driving cavity is formed on the periphery of the positioning block. Multiple driving cavities are formed at intervals along the circumference of the positioning block. A multi-core optical fiber with multiple FBG sensors is disposed in the central cavity, and a single-core optical fiber with multiple FBG sensors is disposed in each driving cavity.

[0007] Preferably, the unit body is a nickel-titanium tube with a symmetrical groove structure. The unit body includes two rigid sections and two flexible sections. The two flexible sections are disposed between the two rigid sections and are arranged opposite to each other.

[0008] Preferably, in the cross-section of the bendable section of the unit body, the angle between the beam of the bendable section and the adjacent drive cavity is 45°.

[0009] Preferably, the positioning block is in the shape of a circular plate and is coaxially installed in the rigid section of the unit body. The driving cavity is formed by the positioning block and the inner wall of the rigid section of the unit body, and four driving cavities are formed at equal intervals along the axial direction of the positioning block.

[0010] Preferably, the end of the multi-core optical fiber is fixed to the end of the robot, and the end of any one of the single-core optical fibers is fixed to the end of the robot.

[0011] According to the present invention, a multi-contact-point force sensing method for a continuum robot based on a fiber Bragg grating is provided. The sensing method includes the following steps:

[0012] Step S1: Calculate the tension and bending curvature of each drive wire on each unit body based on the measurement values ​​of the FBG sensor on the multi-core optical fiber and the FBG sensor on the single-core optical fiber.

[0013] Step S2: Interpolate the curvature calculated in step S1 to reconstruct the three-dimensional shape of the robot and obtain the pose of each unit.

[0014] Step S3: Based on the curvature and three-dimensional shape, make a preliminary estimate of the number and location of contact points;

[0015] Step S4: Using the beam theory model, construct the moment balance equation of the unit based on the number of stress points, three-dimensional shape, unit bending curvature, and unit driving wire tension;

[0016] And / or, using the beam theory model, the moment balance equation of the unit is constructed based on the number of force points, three-dimensional shape, unit bending curvature and unit driving wire tension. If the number of external forces is greater than one, the distance between two adjacent forces is determined. If the distance between the two forces is greater than the length of a single unit, the moment balance equation is constructed using the units between the two forces, providing additional constraint equations.

[0017] Step S5: Solve for the location of the contact point and the magnitude of the force based on the equations constructed in step S4.

[0018] Preferably, it includes contact estimation:

[0019] The relationship between the curvature and bending moment of the j-th unit cell of the robot relative to its own local coordinate system {Sj} along the y and z directions is as follows:

[0020]

[0021] in, y M j(Sj) (j = 1, 2, ..., n) and z M j(Sj) For bending moment, y κ j and z κ j For curvature, y EI j and z EI j Let n be the bending stiffness, and n be the number of unit cells in the continuum robot.

[0022] The robot's base coordinate system {R} and the shape sensor's base coordinate system {S} are located at the root of the robot. The local coordinate system of the j-th unit is located at the root of that unit. {S1} represents the local coordinate system of the 1-th unit. The relationship between the driving wire tension and bending moment of the j-th unit relative to {Sj} is as follows:

[0023]

[0024] in, and These are the torques of the j-th unit along the y and z directions, respectively, generated by the tension of the driving wire; T ij (i = 1, 2, 3, 4) is the tension of the i-th single-core optical fiber in the j-th unit; r is the distance from the robot's central axis to the drive cavity;

[0025] If an external force acts on the robot body, the force F and its position P at the contact point can be transformed from {R} to {Sj};

[0026]

[0027]

[0028] Among them, F (Sj) and P (Sj) These represent the force and position of the contact force relative to {Sj}, respectively. and Let {Sj} be the rotation matrix and position vector relative to {S}, respectively. It is the rotation matrix of {S} relative to {R}; the bending moment of the j-th element relative to {Sj} under the action of external force is:

[0029]

[0030] Therefore, the total torque of the j-th unit along the y-direction {Sj} y M j(Sj) and total torque in the z direction z M j(Sj) for:

[0031]

[0032] in, and yes The values ​​in the y and z directions can be obtained using equations (1), (2), and (6); if If the values ​​in all three directions are 0, it means that no external force is acting on the robot body; if the number of contact points of the continuous robot is greater than 1, then according to equation (6), the torque of all units between the root of the robot and the first contact point near the proximal end can be obtained as follows:

[0033]

[0034] Where q is the number of contact points. and The torques along the y and z directions generated by the j-th element under the action of the k-th external force, respectively;

[0035] If an external force is applied to a certain position of the robot body, the curvature change before and after the force is applied will be different; the number of contact points and their related positions can be estimated in advance, as follows:

[0036] Interpolation techniques are used to calculate the continuous curvature and drive wire tension on the robot body. m points are taken at equal intervals along the robot's length, and a local coordinate system is established for each point. The local coordinate system of the i-th point is {CSi}. Then, the torques at the i-th point along the y and z directions of {CSi} are respectively... and Calculate the torque difference between any two adjacent points. Since torque has a linear relationship with force and distance, therefore and The changes can be used to estimate the quantity of external forces and their associated locations;

[0037] The Bishop algorithm framework is used to estimate the robot's 3D pose. After the robot's shape is reconstructed, three 2D cubic splines are used to fit the robot's shape on the xoy, xoz, and yoz planes, respectively. Since the point of application of the external force on the robot is on the robot body, these three 2D cubic spline functions can be used to constrain the estimated contact point positions, making the optimization process more robust, as shown below:

[0038]

[0039] Where P = [x, y, z] represents the position of the external force, and t ij (i = 1, 2, 3; j = 1, 2, 3, 4) are the fitting parameters of the cubic spline function.

[0040] Preferably, it includes calculating the driving wire tension:

[0041] Using the robot's end effector as a reference, the result is derived gradually towards the root:

[0042]

[0043] N ij =m-round(L ij / d f (10)

[0044] Among them, L ij is the length of the i-th single-core fiber from the robot's end effector to the j-th unit cell, p is the number of discrete points between two adjacent unit cells, m is the number of FBG sensors in each single-core fiber, and d f N is the distance between two adjacent FBG sensors in a single-core optical fiber. ij This is the serial number of the FBG sensor for the i-th single-core optical fiber. Finally, the serial number N... ij The FBG sensor is assigned to the j-th unit cell to calculate the tension of the drive filament of that unit cell; when an optical fiber is pulled, the tension T of the i-th single-core optical fiber in the j-th unit cell is... ij for:

[0045] T ij =ε ij E f A f (11)

[0046] Where, ε ij It is the Nth ij Strain of one FBG sensor; Ef and A f These are the Young's modulus and cross-sectional area of ​​the optical fiber, respectively; if the temperature change is negligible, then T ij It can be calculated as follows:

[0047]

[0048] Wherein, λ(N) ij ) and λ0(N ij ) are the Nth fiber. ij The wavelength and initial wavelength of each FBG sensor, S ε is the fiber strain sensitivity coefficient.

[0049] Preferably, the calibration method is included: calibration is performed by applying a known three-dimensional force to the continuum robot, and the stiffness of the robot can be calibrated using equations (1) and (6), as follows:

[0050]

[0051] in, and It relies on wavelength readings from four single-core optical fibers. y κ j and z κ j Wavelength readings depend on MCF. and Based on the known three-dimensional force, the parameter calibration process for a single-core optical fiber mainly involves fixing weights of different masses at the end of the fiber and converting the wavelength change of the fiber optic FBG sensor into tensile force. If the temperature change during the calibration process is negligible, then the parameters of the optical fiber are:

[0052]

[0053] Where T is the gravity of the weight, and Δλ and λ0 are the wavelength change and initial wavelength of the FBG sensor.

[0054] Preferably, it includes external force calculation: According to equation (7), 3q units can establish 6q equations. The unit between every two adjacent force points is used to construct the solution equation. Based on the curvature of the robot, the contact position of the external forces can be estimated. If the distance between two adjacent external forces along the continuous robot body is greater than ζ, then the unit between these two adjacent forces is used to construct equation (7). If the continuous robot is subjected to q external forces, the optimization objective function for force solution can be written as:

[0055]

[0056] in,

[0057]

[0058]

[0059] 1 g(k) = t 11 x k 3 +t 12 x k 2 +t 13 x k +t 14 -y k

[0060] 2 g(k) = t 21 x k 3 +t 22 x k 2 +t 23 x k +t 24 -z k

[0061] 3 g(k) = t 31 y k 3 +t 32 y k 2 +t 33 y k +t 34 -z k

[0062]

[0063]

[0064] h is the total number of units from the root of the robot to the nearest first contact point. If h is greater than 3q, then h is set to 3q. If the distance between any two adjacent contact points is greater than ζ, then ψ includes all units between the two adjacent contact points. u represents the u-th external force from the root to the end of the continuum robot. P k =[x k ,y k ,z k ] T Let {k} be the position of the k-th external force relative to {R}. F k It is the kth external force relative to {R}; η1, η2, η3 are weighting coefficients; and Let P be the torque along the y and z directions generated by the k-th external force acting on the i-th unit; the constraints of the optimization process are the minimum and maximum values ​​of the external force and the contact position, denoted as P. min <P k <P max F min <F k <F max ;P min and P max These represent the minimum and maximum values ​​at the contact positions, respectively; F min and F max These represent the minimum and maximum values ​​of the external force, respectively. Finally, an optimization algorithm can be used to solve the above optimization problem and calculate... and In the optimization process, the contact position calculated using curvature will be used as... The initial value.

[0065] Compared with the prior art, the present invention has the following beneficial effects:

[0066] 1. This invention reconstructs the tension distribution along the entire drive filament in real time by monitoring the wavelength changes of an FBG sensor on a single-core optical fiber. It establishes a beam-based mechanical model that considers segmental differences, multiple drive filaments, and the interaction of external forces. Simultaneously, it senses the position and magnitude of multiple contact forces on the continuum robot and reconstructs the tension along the entire drive filament using an FBG sensor. This eliminates the need for explicit modeling of the friction between the drive filament and the drive channel, overcoming the problems of errors and excessive computational resource consumption caused by modeling. This method can simultaneously estimate the magnitude of multiple contact points and contact forces on the continuum robot body, providing a significant guarantee for the safe intervention of surgical robots.

[0067] 2. This invention achieves force sensing at multiple contact points in a continuum robot with active spatial bending capability while maintaining the robot's small scale, simplifying the force estimation process and further increasing the accuracy of the estimation. Attached Figure Description

[0068] Other features, objects, and advantages of the present invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings:

[0069] Figure 1 This invention mainly embodies the overall configuration diagram of the continuum robot;

[0070] Figure 2 for Figure 1 A schematic diagram of the cross-section of AA;

[0071] Figure 3 for Figure 1 A schematic diagram of the cross-section of BB;

[0072] Figure 4 This is a schematic diagram illustrating the overall structure of the unit body in this invention;

[0073] Figure 5 This invention primarily illustrates the overall bending moment and force mechanism of a continuum robot;

[0074] Figure 6 This invention primarily illustrates the bending moment and force mechanism of the cross-section of a continuum robot;

[0075] Figure 7 This invention mainly embodies the multi-contact point force sensing flowchart;

[0076] Figure 8 This is a schematic diagram illustrating the bending moment and driving wire tension, which are the main features of this invention.

[0077] The figure shows: 1. Unit body; 2. Rigid section 2; 3. Flexible section; 4. Central cavity; 5. Drive cavity; 6. Positioning block; 7. Multi-core optical fiber; 8. Single-core optical fiber. Detailed Implementation

[0078] The present invention will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make several changes and improvements without departing from the concept of the present invention. These all fall within the protection scope of the present invention.

[0079] like Figure 1 , Figure 2 , Figure 3 , Figure 4 , Figure 5 as well as Figure 6 As shown, a multi-contact force sensing method for a continuum robot based on fiber optic gratings according to the present invention includes multiple sequentially connected unit bodies 1, each unit body 1 including a rigid segment 2 and a flexible segment 3. A positioning block 6 is mounted on the rigid segment 2 of the unit body 1. A central cavity 4 is formed in the center of the positioning block 6, and driving cavities 5 are formed on the periphery of the positioning block 6. Multiple driving cavities 5 are formed at intervals along the circumference of the positioning block 6. A multi-core optical fiber 7 with multiple FBG sensors is disposed within the central cavity 4, and a single-core optical fiber 8 engraved with multiple FBG sensors is disposed within each driving cavity 5.

[0080] Furthermore, unit 1 is a nickel-titanium tube with a symmetrical grooved structure. Unit 1 includes two rigid sections 2 and two flexible sections 3. The two flexible sections 3 are both disposed between the two rigid sections 2 and are arranged opposite to each other. Positioning block 6 is circular and coaxially mounted in the rigid section 2 of unit 1. Driving cavity 5 is formed by the cooperation of positioning block 6 and the inner wall of rigid section 2 of unit 1, and four driving cavities 5 are formed at equal intervals along the axial direction of positioning block 6. The end of multi-core optical fiber 7 is fixed to the end of the robot, and the end of any single-core optical fiber 8 is also fixed to the end of the robot.

[0081] Specifically, the main body of the continuum robot is formed by laser cutting of a nickel-titanium tube, resulting in a symmetrical slotted structure. In this slotted structure, the robot body consists of multiple identical unit cells 1, each including two rigid segments 2 and two flexible segments 3. When the positioning block 6 is assembled into the slotted nickel-titanium tube, four drive channels 5 and a central channel 4 are formed. A single-core optical fiber 8, etched with multiple FBG sensors, is disposed in each drive channel 5 to drive the robot and sense the distribution of driving force. The ends of the four single-core optical fibers 8 are fixed to the end of the robot. When the single-core optical fiber 8 is used to drive the robot to bend, the FBG sensors of the fiber can slide along the drive channel 5. A multi-core optical fiber 7 (MCF) with multiple FBG sensors is disposed in the central channel 4 of the robot to measure the bending curvature of the robot, then reconstruct the shape of the robot and estimate the pose of each unit cell 1. The ends and roots of the MCF are fixed to the end and root of the robot, respectively. In the cross-section of the robot's bendable segment 3, the angle between the beam of the bendable segment 3 and the adjacent drive cavity 5 is 45°.

[0082] The basic idea of ​​this invention is to establish the relationship between the forces and internal bending moments of each unit body 1 based on a beam theory model, and to decouple external contact forces by the relationship between the curvature of each unit body 1, the tension of the drive wire, and the external forces. When an external force is applied to a specific position of the continuum robot, an additional torque from that external force is introduced and acts on the unit body 1 from the robot root to the application position. Therefore, the total torque of a specific unit body 1 is the torque generated by the drive wire tension and the external forces, and also corresponds to the curvature of that unit body 1. This invention uses an FBG sensor on a single-core optical fiber 8 to obtain the drive wire tension applied to a certain unit body 1. The MCF of the robot's central cavity 4 can provide the robot's curvature, and then the robot's shape is calculated. The robot's curvature is used to calculate the total torque of the unit body 1, and the robot's shape is used to determine the three-dimensional spatial relative pose of any two unit bodies 1, to help construct the torque balance equation of any unit body 1. Finally, multiple torque balance equations of unit bodies 1 can be constructed to estimate one or more external forces and their positions. Clearly, the proposed estimation method does not require the establishment of a complex friction model to describe the friction between the drive wire and the drive channel, and extends the force estimation to the entire body of the continuum robot.

[0083] like Figure 7 and Figure 8 As shown, according to the present invention, a multi-contact point force sensing method for a continuum robot based on a fiber optic grating is provided. The sensing method includes the following steps:

[0084] Step S1: Calculate the tension and bending curvature of each drive wire on each unit body 1 based on the measurement values ​​of the FBG sensor on the multi-core optical fiber 7 and the FBG sensor on the single-core optical fiber 8.

[0085] Step S2: Interpolate the curvature calculated in step S1 to reconstruct the three-dimensional shape of the robot and obtain the pose of each unit 1.

[0086] Step S3: Based on the curvature and three-dimensional shape, make a preliminary estimate of the number and location of contact points;

[0087] Step S4: Using the beam theory model, construct the moment balance equation of unit 1 based on the number of stress points, three-dimensional shape, bending curvature of unit 1, and tension of the driving wire of unit 1;

[0088] And / or, using the beam theory model, the moment balance equation of unit 1 is constructed based on the number of force points, three-dimensional shape, bending curvature of unit 1 and tension of the driving wire of unit 1. If the number of external forces is greater than one, the distance between two adjacent forces is determined. If the distance between the two forces is greater than the length of a single unit 1, the moment balance equation is constructed using the unit 1 between the two forces, providing additional constraint equations.

[0089] Step S5: Solve for the location of the contact point and the magnitude of the force based on the equations constructed in step S4.

[0090] Specifically, this includes contact estimation:

[0091] The main principle of the contact estimation method involves decoupling the relationship between the drive wire tension, external force, and bending moment of each unit 1 of the robot. The relationship between the curvature and bending moment of the j-th unit 1 of the robot relative to its own local coordinate system {Sj} along the y and z directions is as follows:

[0092]

[0093] in, y M j(Sj) (j = 1, 2, ..., n) and z M j(Sj) For bending moment, y κ j and z κ j For curvature, y EI j and z EI j Let n be the bending stiffness, and n be the number of unit cells in the continuum robot.

[0094] The robot's base coordinate system {R} and the shape sensor's base coordinate system {S} are located at the root of the robot. The local coordinate system of the j-th unit 1 is located at the root of this unit 1. {S1} represents the local coordinate system of the first unit 1. The relationship between the driving wire tension and bending moment of the j-th unit 1 relative to {Sj} is as follows:

[0095]

[0096] in, and These are the torques of the j-th unit 1 along the y and z directions, respectively, generated by the tension of the driving wire; T ij (i = 1, 2, 3, 4) is the tension of the i-th single-core optical fiber 8 in the j-th unit 1; r is the distance from the robot's central axis to the drive cavity 5;

[0097] If an external force acts on the robot body, the force F and its position P at the contact point can be transformed from {R} to {Sj};

[0098]

[0099]

[0100] Among them, F (Sj) and P (Sj) These represent the force and position of the contact force relative to {Sj}, respectively. and Let {Sj} be the rotation matrix and position vector relative to {S}, respectively. It is the rotation matrix of {S} relative to {R}; the bending moment of the j-th unit 1 relative to {Sj} under the action of external force is:

[0101]

[0102] Therefore, the total torque of the j-th unit 1 along the y-direction {Sj} y M j(Sj) and total torque in the z direction z M j(Sj) for:

[0103]

[0104] in, and yes The values ​​in the y and z directions can be obtained using equations (1), (2), and (6); if If the values ​​in all three directions are 0, it means that no external force is acting on the robot body; if the number of contact points of the continuous robot is greater than 1, then according to equation (6), the torque of all unit bodies 1 between the root of the robot and the first contact point near the end is:

[0105]

[0106] Where q is the number of contact points. and The torques along the y and z directions generated by the j-th element under the action of the k-th external force, respectively;

[0107] If an external force is applied to a certain position of the robot body, the curvature change before and after the force is applied will be different; the number of contact points and their related positions can be estimated in advance, as follows:

[0108] Interpolation techniques are used to calculate the continuous curvature and drive wire tension on the robot body. m points are taken at equal intervals along the robot's length, and a local coordinate system is established for each point. The local coordinate system of the i-th point is {CSi}. Then, the torques at the i-th point along the y and z directions of {CSi} are respectively... and Calculate the torque difference between any two adjacent points. Since torque has a linear relationship with force and distance, therefore and The changes can be used to estimate the amount of external force and its associated location.

[0109] The Bishop algorithm framework is used to estimate the robot's 3D pose. After the robot's shape is reconstructed, three 2D cubic splines are used to fit the robot's shape on the xoy, xoz, and yoz planes, respectively. Since the point of application of the external force on the robot is on the robot body, these three 2D cubic spline functions can be used to constrain the estimated contact point positions, making the optimization process more robust, as shown below:

[0110]

[0111] Where P = [x, y, z] represents the position of the external force, and t ij (i = 1, 2, 3; j = 1, 2, 3, 4) are the fitting parameters of the cubic spline function.

[0112] Including calculation of the driving wire tension:

[0113] If the optical fiber is pulled, the FBG sensor on the fiber will slide along the drive channel, and the wavelength of the FBG sensor will change. Therefore, these wavelength changes can be used to calculate the tension of the drive wire. Since a single optical fiber has multiple FBG sensors, the tension of the drive wire corresponding to each unit 1 of the robot can be accurately calculated without a complex theoretical model. Each unit of the robot is equipped with four single-core fiber 8 FBG sensors and a set of multi-core fiber 7 FBG sensors. The four single-core FBG sensors are located on four single-core fibers 8. When the continuous robot is in a straight line, the FBG sensor corresponding to each unit of the robot can be obtained in advance. If the robot undergoes bending motion, the FBG sensor on the multi-core fiber 7 has no relative movement with respect to the unit 1, but the FBG on the single-core fiber 8 will slide within the drive cavity 5. The FBG corresponding to each unit of the robot is constantly changing, and this positional relationship needs to be calculated in real time. The ends of all optical fibers are fixed to the ends of the continuous robot. In order to obtain the FBG sensor corresponding to each unit in real time, the end of the robot can be used as a reference to gradually deduce the value towards the root.

[0114]

[0115] N ij =m-round(L ij / d f (10)

[0116] Among them, L ij is the length of the i-th single-core optical fiber 8 from the robot's end effector to the j-th unit body 1, p is the number of discrete points between two adjacent unit bodies 1, m is the number of FBG sensors in each single-core optical fiber 8, and d f N is the distance between two adjacent FBG sensors in a single-core optical fiber 8. ijThis is the serial number of the FBG sensor in the i-th single-core fiber 8. Finally, the serial number N... ij The FBG sensor is assigned to the j-th unit 1 to calculate the tension of the drive filament of that unit; when an optical fiber is pulled, the tension T of the i-th single-core optical fiber 8 in the j-th unit 1 is... ij for:

[0117] T ij =ε ij E f A f (11)

[0118] Where, ε ij It is the Nth ij Strain of one FBG sensor; E f and A f These are the Young's modulus and cross-sectional area of ​​the optical fiber, respectively; if the temperature change is negligible, then T ij It can be calculated as follows:

[0119]

[0120] Wherein, λ(N) ij ) and λ0(N ij ) are the Nth fiber. ij The wavelength and initial wavelength of each FBG sensor, S ε is the fiber strain sensitivity coefficient.

[0121] Including calibration methods: Since there is a certain gap between the ideal parameters and actual values ​​of the robot, it is necessary to calibrate the relevant parameters of the robot. Calibration is accomplished by applying a known three-dimensional force to the continuum robot. The stiffness of the robot can be calibrated using equations (1) and (6), as follows:

[0122]

[0123] in, and Based on wavelength readings from four single-core optical fibers 8, y κ j and z κ j Wavelength readings depend on MCF. and Based on the known three-dimensional force, the parameter calibration process of single-core optical fiber 8 mainly involves fixing weights of different masses at the end of the fiber and converting the wavelength change of the fiber optic FBG sensor into tension. If the temperature change can be ignored during the calibration process, the parameters of the optical fiber are:

[0124]

[0125] Where T is the gravity of the weight, and Δλ and λ0 are the wavelength change and initial wavelength of the FBG sensor.

[0126] Preferably, it includes external force calculation: Since each external force can be represented by six parameters (position and force), at least 6q equations are needed to solve for q external forces. According to equation (7), 3q unit cells 1 can establish 6q equations. The unit cell 1 between every two adjacent force points is used to construct the solution equation. According to the curvature of the robot, the contact position of the external forces can be estimated. If the distance between two adjacent external forces along the continuous robot body is greater than ζ, then the unit cell 1 between these two adjacent forces is used to construct equation (7). If the continuous robot is subjected to q external forces, the optimization objective function for force solution can be written as:

[0127]

[0128] in,

[0129]

[0130]

[0131] 1 g(k) = t 11 x k 3 +t 12 x k 2 +t 13 x k +t 14 -y k

[0132] 2 g(k) = t 21 x k 3 +t 22 x k 2 +t 23 x k +t 24 -z k

[0133] 3 g(k) = t 31 y k 3 +t 32 y k 2 +t 33 y k +t 34 -z k

[0134]

[0135]

[0136] h is the total number of unit cells 1 from the root of the robot to the nearest first contact point. If h is greater than 3q, then h is set to 3q. If the distance between any two adjacent contact points is greater than ζ, then ψ includes all unit cells 1 between the two adjacent contact points. u represents the u-th external force from the root to the end of the continuum robot. P k =[x k ,y k ,z k ] T Let {k} be the position of the k-th external force relative to {R}. F k It is the kth external force relative to {R}; η1, η2, η3 are weighting coefficients; and Let P be the torque along the y and z directions generated by the k-th external force acting on the i-th unit 1; the constraints of the optimization process are the minimum and maximum values ​​of the external force and the contact position, denoted as P. min <P k <P max F min <F k <F max ;P min and P max These represent the minimum and maximum values ​​at the contact positions, respectively; F min and F max These represent the minimum and maximum values ​​of the external force, respectively. Finally, an optimization algorithm can be used to solve the above optimization problem and calculate... and In the optimization process, the contact position calculated using curvature will be used as... The initial value.

[0137] This invention proposes a multi-contact force sensing method and device for continuum robots based on fiber Bragg gratings (FBGs). First, single-core and multi-core optical fibers 7, each equipped with multiple FBG sensors, are used as the drive wire and shape sensor for the continuum robot, respectively. Then, the tension distribution along the entire drive wire is reconstructed in real time based on the wavelength changes of the FBG sensors on the single-core fiber 8. Finally, a beam-based mechanical model considering segment differences, multiple drive wires, and the interaction of external forces is established, simultaneously sensing the position and magnitude of multiple contact forces on the continuum robot. Reconstructing the tension along the entire drive wire using FBG sensors eliminates the need for explicit modeling of the friction between the drive wire and the drive channel, overcoming the errors and excessive computational costs associated with modeling. This method can simultaneously estimate the magnitude of multiple contact points and contact forces on the continuum robot body, providing significant assurance for the safe intervention of surgical robots. In summary, this invention achieves multi-contact force sensing for a continuum robot with spatial active bending capabilities while maintaining a small robot scale, simplifying the force estimation process and further increasing the accuracy of the estimation.

[0138] In the description of this application, it should be understood that the terms "upper", "lower", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are only for the convenience of describing this application and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on this application.

[0139] Specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Unless otherwise specified, the embodiments and features described in this application can be arbitrarily combined with each other.

Claims

1. A multi-contact force sensing device for a continuum robot based on fiber Bragg gratings, characterized in that, It includes multiple unit bodies (1) connected in sequence, each of which includes a rigid segment (2) and a flexible segment (3); The rigid section (2) of the unit body (1) is equipped with a positioning block (6), a central cavity (4) is formed in the middle of the positioning block (6), a driving cavity (5) is formed on the periphery of the positioning block (6), and multiple driving cavities (5) are formed at intervals along the periphery of the positioning block (6). The central cavity (4) is equipped with a multi-core optical fiber (7) with multiple FBG sensors, and each of the driving cavities (5) is equipped with a single-core optical fiber (8) with multiple FBG sensors. The positioning block (6) is in the shape of a circular plate. The positioning block (6) is coaxially installed in the rigid section (2) of the unit body (1). The driving cavity (5) is formed by the positioning block (6) and the inner wall of the rigid section (2) of the unit body (1). The driving cavity (5) is formed in four equal intervals along the axial direction of the positioning block (6). The tension and curvature of each drive wire on each unit body (1) are calculated based on the measurements of the FBG sensor on the multi-core fiber (7) and the FBG sensor on the single-core fiber (8).

2. The multi-contact force sensing device for a continuum robot based on fiber optic gratings as described in claim 1, characterized in that, The unit body (1) is a nickel-titanium tube with a symmetrical groove structure. The unit body (1) includes two rigid sections (2) and two flexible sections (3). The two flexible sections (3) are arranged between the two rigid sections (2) and are arranged opposite to each other.

3. The multi-contact force sensing device for a continuum robot based on fiber optic gratings as described in claim 1, characterized in that, In the cross section of the bendable segment (3) of the unit (1), the angle between the beam of the bendable segment (3) and the adjacent drive cavity (5) is 45°.

4. The multi-contact force sensing device for a continuum robot based on fiber optic gratings as described in claim 1, characterized in that, The end of the multi-core optical fiber (7) is fixed to the end of the robot, and the end of any one of the single-core optical fibers (8) is fixed to the end of the robot.

5. A multi-contact point force sensing method for a continuum robot based on fiber Bragg gratings, characterized in that, The multi-contact-point force sensing device for a continuum robot based on fiber optic gratings as described in any one of claims 1-4 includes the following sensing method: Step S1: Calculate the tension and bending curvature of each drive wire on each unit body (1) based on the measurement values ​​of the FBG sensor on the multi-core optical fiber (7) and the FBG sensor on the single-core optical fiber (8). Step S2: Interpolate the bending curvature calculated in step S1 to reconstruct the three-dimensional shape of the robot and obtain the pose of each unit (1). Step S3: Based on the curvature and three-dimensional shape, make a preliminary estimate of the number and location of contact points; Step S4: Using the beam theory model, construct the moment balance equation of unit (1) based on the number of stress points, three-dimensional shape, bending curvature of unit (1) and tension of driving wire of unit (1); And / or, using the beam theory model, the moment balance equation of the unit (1) is constructed based on the number of force points, three-dimensional shape, bending curvature of the unit (1) and the tension of the driving wire of the unit (1). If the number of external forces is greater than one, the distance between two adjacent forces is determined. If the distance between the two forces is greater than the length of a single unit (1), the moment balance equation is constructed using the unit (1) between the two forces, providing additional constraint equations. Step S5: Solve for the location of the contact point and the magnitude of the force based on the equations constructed in step S4.

6. The multi-contact point force sensing method for a continuum robot based on fiber optic gratings as described in claim 5, characterized in that, Including contact estimation: Robot j Each unit cell (1) is relative to its own local coordinate system {Sj} along... y and z The relationship between the curvature and bending moment in the direction is as follows: (1) in, and For bending moment, and For curvature, and Let n be the bending stiffness, and n be the number of unit cells (1) of the continuum robot; The robot's base coordinate system {R} and the shape sensor's base coordinate system {S} are located at the root of the robot. j The local coordinate system of the first unit cell (1) is located at the root of the unit cell (1), and {S1} represents the local coordinate system of the first unit cell (1). j The relationship between the driving wire tension and bending moment of each unit (1) relative to {Sj} is as follows: (2) in, and They are the first j Each unit (1) along y and z The directional torque is generated by the tension of the driving wire; T ij ( i =1, 2, 3, 4) is the first... j In the unit (1) the first i The tension of a single-core optical fiber (8); r It is the distance from the robot's central axis to the drive cavity (5); If an external force acts on the robot body, the force at the point of contact... F and its location P It can be transformed from {R} to {Sj}; (3) (4) in, and These represent the force and position of the contact force relative to {Sj}, respectively. and Let {Sj} be the rotation matrix and position vector relative to {S}, respectively. It is the rotation matrix of {S} relative to {R}; the first... j The bending moment relative to {Sj} generated by the unit (1) under the action of external force is: (5) Therefore, the first j Unit (1) along {Sj} y Total moment in direction and z Total moment in direction for: (6) in, and yes exist y and z The values ​​in the direction can be obtained using equations (1), (2), and (6); if If the values ​​in all three directions are 0, it means that no external force is acting on the robot body; if the number of contact points of the continuous robot is greater than 1, then according to equation (6), the torque of all unit bodies (1) between the root of the robot and the first contact point near the proximal end is: (7) in, q It is the number of contact points. and The respective j The unit in the first k The along the action of an external force y and z Torque in direction; If an external force is applied to a certain position of the robot body, the curvature change before and after the force is applied will be different; the number of contact points and their related positions can be estimated in advance, as follows: Interpolation techniques are used to calculate the continuous curvature and drive wire tension along the robot body, taking values ​​along the robot's body length. m There are n equidistant points, and a local coordinate system is set for each point. i If the local coordinate system of the nth point is {CSi}, then the nth point... i Points along {CSi} y and z The torques in the directions are respectively and ; Calculate the torque difference between every two adjacent points , Since torque has a linear relationship with force and distance, therefore and The changes can be used to estimate the quantity of external forces and their associated locations; The Bishop algorithm framework is used to estimate the robot's 3D pose. After the robot's shape is reconstructed, three 2D cubic splines are used to fit the robot's position in the model. xoy , xoz and yoz Since the external force acts on the robot at its point of application on the robot body, these three two-dimensional cubic spline functions can be used to constrain the estimated contact point positions, making the optimization process more robust, as shown below: (8) in, P =[ x , y , z [This indicates the location of the external force.] These are the fitting parameters for the cubic spline function.

7. The multi-contact point force sensing method for a continuum robot based on fiber optic gratings as described in claim 5, characterized in that, Including calculation of the driving wire tension: Using the robot's end effector as a reference, the result is derived gradually towards the root: (9) (10) in, It is the first i The single-core optical fiber (8) runs from the robot's end effector to the first... j The length of each unit body (1), p It is the number of discrete points between two adjacent unit cells (1). m It is the number of FBG sensors in each single-core optical fiber (8). It is the distance between two adjacent FBG sensors in a single-core optical fiber (8). It is the first i The serial number of the FBG sensor of the single-core optical fiber (8) is finally determined. The FBG sensor is assigned to the first j A unit cell (1) is used to calculate the tension of the unit drive wire; when an optical fiber is pulled, the first unit cell (1) is used to calculate the tension of the drive wire. j The first unit (1) i Tensile force of a single-core optical fiber (8) for: (11) in, It is the first Strain of an FBG sensor; and These are the Young's modulus and cross-sectional area of ​​the optical fiber, respectively; if the temperature change is negligible, then... It can be calculated as follows: (12) in, and These are the first and second optical fibers. The wavelength and initial wavelength of each FBG sensor. is the fiber strain sensitivity coefficient.

8. The multi-contact point force sensing method for a continuum robot based on fiber optic gratings as described in claim 6, characterized in that, The calibration method includes applying a known three-dimensional force to the continuum robot. The robot's stiffness can be calibrated using equations (1) and (6), as follows: (13) in, and Based on wavelength readings from four single-core optical fibers (8), and Wavelength readings depend on MCF. and Based on the known three-dimensional force, the parameter calibration process of a single-core optical fiber (8) mainly involves fixing weights of different masses at the end of the fiber and converting the wavelength change of the fiber FBG sensor into tension. If the temperature change can be ignored during the calibration process, the parameters of the optical fiber are: (14) in, T It is the weight of the weights. and The wavelength change and initial wavelength of the FBG sensor are given.

9. The multi-contact point force sensing method for a continuum robot based on fiber optic gratings as described in claim 8, characterized in that, Including external force calculation: according to equation (7), 3 q One unit (1) can be used to build 6 q The equations are constructed using the unit body (1) between every two adjacent force points. Based on the robot's curvature, the contact position of the external forces can be estimated. If the distance between two adjacent external forces along the continuum of the robot body is greater than 1, the contact position of the external forces can be estimated. Then the unit body (1) between these two adjacent forces is used to construct equation (7), if the continuum robot is subjected to q Given an external force, the objective function for solving this problem can be written as: (15) in, h The total number of unit cells (1) from the root of the robot to the nearest first contact point, if h Greater than 3 q ,but h Set to 3 q If the distance between any two adjacent contact points is greater than ,but Includes all unit bodies (1) between two adjacent contact points; u This represents the first... (The sentence is incomplete and requires more context to translate accurately.) u external force; , For the first k An external force is relative to the position of {R}; , It is relative to the first {R} k An external force; These are the weighting coefficients; and For the first i The unit (1) is subject to the first unit (1) k The effect of an external force along y and z The torque in the direction; the constraints of the optimization process are the minimum and maximum values ​​of the external force and the contact position, denoted as... , ; and These represent the minimum and maximum values ​​at the contact position, respectively. and These represent the minimum and maximum values ​​of the external force, respectively. Finally, an optimization algorithm can be used to solve the above optimization problem and calculate... and In the optimization process, the contact position calculated using curvature will be used as... The initial value.

Images