CN114905549B - Method and system for sensing three-dimensional force at tail end of rope-driven flexible robot - Google Patents
Method and system for sensing three-dimensional force at tail end of rope-driven flexible robot Download PDFInfo
- Publication number
- CN114905549B CN114905549B CN202210389927.3A CN202210389927A CN114905549B CN 114905549 B CN114905549 B CN 114905549B CN 202210389927 A CN202210389927 A CN 202210389927A CN 114905549 B CN114905549 B CN 114905549B
- Authority
- CN
- China
- Prior art keywords
- robot
- tail end
- base
- fbg
- tip
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 239000013307 optical fiber Substances 0.000 claims abstract description 85
- 230000008859 change Effects 0.000 claims description 38
- 238000013178 mathematical model Methods 0.000 abstract description 3
- 238000005452 bending Methods 0.000 description 7
- 239000000835 fiber Substances 0.000 description 7
- 230000008569 process Effects 0.000 description 5
- 230000035945 sensitivity Effects 0.000 description 5
- 239000011159 matrix material Substances 0.000 description 3
- 230000008447 perception Effects 0.000 description 3
- 241000251468 Actinopterygii Species 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 206010069747 Burkholderia mallei infection Diseases 0.000 description 1
- 235000016299 Canarium odontophyllum Nutrition 0.000 description 1
- 244000001582 Canarium odontophyllum Species 0.000 description 1
- 241001465754 Metazoa Species 0.000 description 1
- 241000238413 Octopus Species 0.000 description 1
- 238000010521 absorption reaction Methods 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000003278 mimic effect Effects 0.000 description 1
- 230000008520 organization Effects 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/02—Sensing devices
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
- B25J9/104—Programme-controlled manipulators characterised by positioning means for manipulator elements with cables, chains or ribbons
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01L—MEASURING FORCE, STRESS, TORQUE, WORK, MECHANICAL POWER, MECHANICAL EFFICIENCY, OR FLUID PRESSURE
- G01L1/00—Measuring force or stress, in general
- G01L1/24—Measuring force or stress, in general by measuring variations of optical properties of material when it is stressed, e.g. by photoelastic stress analysis using infrared, visible light, ultraviolet
- G01L1/242—Measuring force or stress, in general by measuring variations of optical properties of material when it is stressed, e.g. by photoelastic stress analysis using infrared, visible light, ultraviolet the material being an optical fibre
- G01L1/246—Measuring force or stress, in general by measuring variations of optical properties of material when it is stressed, e.g. by photoelastic stress analysis using infrared, visible light, ultraviolet the material being an optical fibre using integrated gratings, e.g. Bragg gratings
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Manipulator (AREA)
- Force Measurement Appropriate To Specific Purposes (AREA)
Abstract
The invention provides a three-dimensional force sensing method and a three-dimensional force sensing system for a tail end of a rope-driven flexible robot, wherein the method comprises the steps of designing a base part and the tail end of the robot into four beam structures; fixing four optical fibers on four beams of a base, wherein each optical fiber is provided with an FBG sensor; four optical fibers are assembled in a driving cavity of the robot to serve as driving wires, and each optical fiber is provided with two FBG sensors; dividing all FBG sensors into I, II and III; solving the moment of the tail end of the robot by using the group I, and solving the forces in the x and y directions of the tail end; II, solving the tension of the driving wire; III, solving the moment of the base part of the robot; the force in the z direction of the tip is decoupled by the force in the x and y directions of the tip, the tension of the drive wire, the moment of the base and the pose of the robot tip. The configuration mode of the FBG sensor ensures the compact structure and large inner cavity of the flexible robot; the method does not need to build a complex mathematical model, does not need to keep elastic characteristics of the flexible robot, and can be also suitable for the articulated continuous robot.
Description
Technical Field
The invention relates to the technical field of flexible robots, in particular to a three-dimensional force sensing method and a three-dimensional force sensing system for the tail end of a rope-driven flexible robot, and particularly relates to a three-dimensional force sensing method for the tail end of the rope-driven flexible robot.
Background
Many living things in nature have their own flexibility and flexibility, and flexible robots in fact mimic some animal shapes. The flexible robot can extend and move to various corners like a vine; or like a octopus, the whole body does not have any hard structural organization, like the Dabai in the super land warfare team; there are also underwater robots of fish organisms, soft "fins" that move flexibly in water like real fish. The flexible robot is composed of flexible materials, so the flexible robot has three characteristics necessarily: high flexibility: the robot can move flexibly in a complex space environment; deformability of: the robot can complete various tasks, so that the cost is reduced; the energy absorption characteristic can reduce acting force generated by collision and improve safety when the man-machine is cooperated.
Disclosed in patent document with publication number CN114211503a is a method and system for controlling track of rope-driven flexible robot based on visual feedback, the method comprising: modeling by kinematic modeling of drive space, rope space, joint space, and end cartesian space, including drive space to rope space, rope space to joint space, joint space to end cartesian space; constructing a dynamic mechanical model of the rope-driven flexible robot, and constructing a control model based on a new dynamic track by utilizing the kinematic relationship between the rope and the joint; constructing a track control frame of the rope-driven flexible robot, which consists of a vision measurement module, a dynamics solving module and a track tracking control module; when the tail end of the rope-driven flexible robot executes the task, the track of the Cartesian space of the rope-driven flexible robot is controlled through the track control frame based on visual feedback.
In view of the above related art, the inventors consider that it is a significant challenge to impart force sensing capability to the tip of a continuum robot and to secure a large lumen, and thus, a new solution is required to improve the above technical problems.
Disclosure of Invention
Aiming at the defects in the prior art, the invention aims to provide a three-dimensional force sensing method and system for the tail end of a rope-driven flexible robot.
The invention provides a three-dimensional force sensing method for a tail end of a rope-driven flexible robot, which comprises the following steps of:
step S1: the base and the tail end of the robot are respectively designed into four beam structures;
step S2: four optical fibers are assembled on the base of the robot, the tail end of each optical fiber is provided with an optical Fiber Bragg Grating (FBG) sensor, and the four FBG sensors of the four optical fibers are respectively fixed on four beams of the base;
step S3: four optical fibers are assembled in four driving cavity channels of the robot to serve as driving wires, each driving wire is provided with two FBG sensors, one of the two FBG sensors is located at the tail end of the driving wire, the four tail end FBG sensors of the four driving wires are respectively fixed on four beams at the tail end of the robot, and the other four FBGs slide in the driving cavity channels along with the driving wires;
step S4: dividing all FBG sensors into three groups I, II and III, wherein the four FBG sensors at the tail end of the robot are in the group I, the four sliding FBG sensors are in the group II, and the four FBG sensors at the base of the robot are in the group III;
step S5: the moment of the tail end of the robot is obtained by utilizing the change of the I group of wavelengths, and the forces in the x and y directions of the tail end are obtained;
step S6: solving the tension of the driving wire by utilizing the change of II groups of wavelengths;
step S7: solving the moment of the robot base by utilizing the change of the III group wavelength;
step S8: two electromagnetic EM sensors with five degrees of freedom and one electromagnetic EM sensor with six degrees of freedom are respectively assembled at the tail end and the base part of the robot, and the pose information of the three EM sensors is utilized to obtain the pose of the tail end of the robot;
step S9: the force in the z direction of the tip is decoupled by the force in the x and y directions of the tip, the tension of the drive wire, the moment of the base and the pose of the robot tip.
Preferably, the four optical fibers in the step S2 are F5-F8, and each optical fiber has one FBG sensor at its end.
Preferably, the four optical fibers in the step S3 are F1-F4, and each optical fiber has two FBG sensors, one located at the end of the optical fiber, and the other located at a distance from the end that is the length of the robot.
The invention also provides a three-dimensional force sensing system for the tail end of the rope-driven flexible robot, which comprises the following modules:
module M1: the base and the tail end of the robot are respectively designed into four beam structures;
module M2: four optical fibers are assembled on the base of the robot, the tail end of each optical fiber is provided with an optical Fiber Bragg Grating (FBG) sensor, and the four FBG sensors of the four optical fibers are respectively fixed on four beams of the base;
module M3: four optical fibers are assembled in four driving cavity channels of the robot to serve as driving wires, each driving wire is provided with two FBG sensors, one of the two FBG sensors is located at the tail end of the driving wire, the four tail end FBG sensors of the four driving wires are respectively fixed on four beams at the tail end of the robot, and the other four FBGs slide in the driving cavity channels along with the driving wires;
module M4: dividing all FBG sensors into three groups I, II and III, wherein the four FBG sensors at the tail end of the robot are in the group I, the four sliding FBG sensors are in the group II, and the four FBG sensors at the base of the robot are in the group III;
module M5: the moment of the tail end of the robot is obtained by utilizing the change of the I group of wavelengths, and the forces in the x and y directions of the tail end are obtained;
module M6: solving the tension of the driving wire by utilizing the change of II groups of wavelengths;
module M7: solving the moment of the robot base by utilizing the change of the III group wavelength;
module M8: two electromagnetic EM sensors with five degrees of freedom and one electromagnetic EM sensor with six degrees of freedom are respectively assembled at the tail end and the base part of the robot, and the pose information of the three EM sensors is utilized to obtain the pose of the tail end of the robot;
module M9: the force in the z direction of the tip is decoupled by the force in the x and y directions of the tip, the tension of the drive wire, the moment of the base and the pose of the robot tip.
Preferably, the four optical fibers in the module M2 are F5-F8, and each optical fiber has one FBG sensor at its end.
Preferably, the four optical fibers in the module M3 are F1-F4, and each optical fiber has two FBG sensors, one at the end of the optical fiber and the other at a distance from the end that is the length of the robot.
Compared with the prior art, the invention has the following beneficial effects:
1. the configuration mode of the FBG sensor ensures that the robot has a compact structure, ensures that the robot also has a larger inner cavity under the condition of the smallest dimension, and overcomes the difficulty that the traditional force sensing and the large inner cavity can not be combined;
2. the method has the advantages that a complex flexible robot mathematical model is not required to be established in the solving process, the calculating process is simple, the complexity is low, the real-time performance of force sensing is ensured, and the decoupling between the force sensing and the flexible robot model is realized;
3. the sensing method provided by the invention does not depend on the elasticity of the flexible robot, not only improves the force sensing precision, but also is suitable for wide scenes such as rope-driven flexible robots, articulated flexible robots and the like.
Drawings
Other features, objects and advantages of the present invention will become more apparent upon reading of the detailed description of non-limiting embodiments, given with reference to the accompanying drawings in which:
FIG. 1 is a schematic view of a flexible robot and a driving wire structure according to the present invention;
FIG. 2 is a flow chart of sensing triaxial forces at the end of the flexible robot according to the present invention;
FIG. 3 is a diagram of the end coordinate system of the flexible robot of the present invention;
FIG. 4 is a diagram of a base coordinate system of the flexible robot of the present invention;
FIG. 5 is a graph of the geometry of the bending angle and the end pose of the flexible robot according to the invention;
FIG. 6 is a graph of the human perceived resolution of the flexible robot versus the bending angle of the robot of the present invention.
Detailed Description
The present invention will be described in detail with reference to specific examples. The following examples will assist those skilled in the art in further understanding the present invention, but are not intended to limit the invention in any way. It should be noted that variations and modifications could be made by those skilled in the art without departing from the inventive concept. These are all within the scope of the present invention.
Example 1:
the invention provides a three-dimensional force sensing method for a tail end of a rope-driven flexible robot, which comprises the following steps of:
a method for sensing three-dimensional force at the tail end of a rope-driven flexible robot, comprising the following steps:
step S1: the base and the tail end of the robot are respectively designed into four beam structures.
Step S2: four optical fibers are assembled on the base of the robot, the tail end of each optical fiber is provided with an optical Fiber Bragg Grating (FBG) sensor, and the four FBG sensors of the four optical fibers are respectively fixed on four beams of the base; the four optical fibers are F5-F8, and each optical fiber is provided with an FBG sensor at the tail end.
Step S3: four optical fibers are assembled in four driving cavity channels of the robot to serve as driving wires, each driving wire is provided with two FBG sensors, one of the two FBG sensors is located at the tail end of the driving wire, the four tail end FBG sensors of the four driving wires are respectively fixed on four beams at the tail end of the robot, and the other four FBGs slide in the driving cavity channels along with the driving wires; the four optical fibers are F1-F4, each optical fiber is provided with two FBG sensors, one optical fiber is positioned at the tail end of the optical fiber, and the distance between the other optical fiber and the tail end is the length of the robot.
Step S4: all FBG sensors are divided into three groups I, II and III, four FBG sensors at the tail end of the robot are in the group I, four sliding FBG sensors are in the group II, and four FBG sensors at the base of the robot are in the group III.
Step S5: and (3) calculating the moment of the tail end of the robot by utilizing the change of the I group of wavelengths, and calculating the forces in the x and y directions of the tail end.
Step S6: and (5) solving the tension of the driving wire by utilizing the change of the II group wavelength.
Step S7: and (3) utilizing the change of the III group wavelength to calculate the moment of the robot base.
Step S8: two electromagnetic EM sensors with five degrees of freedom and one electromagnetic EM sensor with six degrees of freedom are respectively assembled at the tail end and the base part of the robot, and the pose information of the three EM sensors is used for obtaining the pose of the tail end of the robot.
Step S9: the force in the z direction of the tip is decoupled by the force in the x and y directions of the tip, the tension of the drive wire, the moment of the base and the pose of the robot tip.
Performing force sensing by using an FBG sensor, converting wavelength readings into strain and then into moment; the relationship between delta epsilon strain change and wavelength change delta lambda is expressed as:
wherein lambda is 0 Is the initial Bragg wavelength of the grating, S ε Is the strain sensitivity coefficient.
The FBG sensors are fixed at the base and the tail end of the robot, and the strain change of the FBG sensors reflects the strain of the robot; the strain change of the robot base or tip is written as:
wherein Deltalambda' and lambda 0 ' the Bragg wavelength variation and the initial Bragg wavelength, respectively; grating lambda 0 ' and lambda 0 Symmetrically distributed along the central axis of the robot.
Example 2:
example 2 is a preferable example of example 1 to more specifically explain the present invention.
The invention also provides a three-dimensional force sensing system for the tail end of the rope-driven flexible robot, which comprises the following modules:
module M1: the base and the tail end of the robot are respectively designed into four beam structures.
Module M2: four optical fibers are assembled on the base of the robot, the tail end of each optical fiber is provided with an optical Fiber Bragg Grating (FBG) sensor, and the four FBG sensors of the four optical fibers are respectively fixed on four beams of the base; the four optical fibers are F5-F8, and each optical fiber is provided with an FBG sensor at the tail end.
Module M3: four optical fibers are assembled in four driving cavity channels of the robot to serve as driving wires, each driving wire is provided with two FBG sensors, one of the two FBG sensors is located at the tail end of the driving wire, the four tail end FBG sensors of the four driving wires are respectively fixed on four beams at the tail end of the robot, and the other four FBGs slide in the driving cavity channels along with the driving wires; the four optical fibers are F1-F4, each optical fiber is provided with two FBG sensors, one optical fiber is positioned at the tail end of the optical fiber, and the distance between the other optical fiber and the tail end is the length of the robot.
Module M4: all FBG sensors are divided into three groups I, II and III, four FBG sensors at the tail end of the robot are in the group I, four sliding FBG sensors are in the group II, and four FBG sensors at the base of the robot are in the group III.
Module M5: and (3) calculating the moment of the tail end of the robot by utilizing the change of the I group of wavelengths, and calculating the forces in the x and y directions of the tail end.
Module M6: and (5) solving the tension of the driving wire by utilizing the change of the II group wavelength.
Module M7: and (3) utilizing the change of the III group wavelength to calculate the moment of the robot base.
Module M8: two electromagnetic EM sensors with five degrees of freedom and one electromagnetic EM sensor with six degrees of freedom are respectively assembled at the tail end and the base part of the robot, and the pose information of the three EM sensors is used for obtaining the pose of the tail end of the robot.
Module M9: the force in the z direction of the tip is decoupled by the force in the x and y directions of the tip, the tension of the drive wire, the moment of the base and the pose of the robot tip.
Performing force sensing by using an FBG sensor, converting wavelength readings into strain and then into moment; the relationship between delta epsilon strain change and wavelength change delta lambda is expressed as:
wherein lambda is 0 Is the initial Bragg wavelength of the grating, S ε Is the strain sensitivity coefficient.
The FBG sensors are fixed at the base and the tail end of the robot, and the strain change of the FBG sensors reflects the strain of the robot; the strain change of the robot base or tip is written as:
wherein Δλ 'and λ' 0 The bragg wavelength variation and the initial bragg wavelength, respectively; grating lambda' 0 And lambda is 0 Symmetrically distributed along the central axis of the robot.
Example 3:
example 3 is a preferable example of example 1 to more specifically explain the present invention.
The invention provides a three-dimensional force sensing method for a tail end of a rope-driven flexible robot, which is realized based on fixed and sliding Fiber Bragg Grating (FBG) sensors. Four optical fibers each having one FBG sensor at the end are symmetrically assembled to the base of the flexible robot, and the sensors at the ends of the optical fibers are fixed to the base. Four optical fibers with two FBG sensors are used as driving wires of the flexible robot, and one FBG sensor is arranged at the tail end of each driving wire and is fixed with the tail end of the robot. Each FBG sensor which is not fixed with the robot on each driving wire slides in the driving cavity along with the driving wire and is used as a sensor of the pulling force of the driving wire. Two five-degree-of-freedom (5-DOF) and one (6-DOF) Electromagnetic (EM) sensor are assembled on the tip and the base of the robot, respectively, for obtaining the pose of the tip relative to the base. The stress of the flexible robot tail end in three directions can be decoupled through the change of the FBG wavelength on all the optical fibers and the information of the EM sensor. The configuration of the FBG sensor ensures the compact structure and large inner cavity of the flexible robot. The method does not need to establish a complex mathematical model, and has low calculation complexity. Furthermore, the proposed sensing method does not rely on the elasticity of the flexible robot, which makes it suitable for a wider range of scenarios for rope driven flexible robots, articulated flexible robots, etc.
The body of the rope driven flexible robot contemplated in the present invention is shown in fig. 1, the robot having a plurality of sections, each section comprising a rigid section and a bendable section. Wherein each bendable portion is comprised of two beams. To improve the sensitivity of force sensing, the base and the tip of the robot are designed as four beams, respectively, as shown in the cross-sectional views A-A and B-B in fig. 1. Four optical fibers F5-F8, each with one FBG sensor, are assembled symmetrically in the base of the robot. Four FBG sensors of four optical fibers of the base form a III group, and the III group is adhered to four cross beams of the base. Four additional optical fibers F1-F4, each with two FBG sensors, are used as the drive wires for the flexible robot. The tail end of each driving wire is provided with an FBG sensor, the four tail end sensors of the four driving wires form a group I, and the group I is stuck to the four beams at the tail end. The length of the other FBG sensor on each driving wire from the tail end is the length of the robot, the FBG sensor can slide in the driving cavity along with the movement of the driving wire, and four sliding FBG sensors on four optical fibers form a group II. Two 5-DOF EM sensors are mounted at the robot tip, mounted to the aperture 1 and the aperture 2, respectively, for establishing the relationship of the electromagnetic sensor coordinate system and the robot tip coordinate system, as shown in the E view in fig. 1. Furthermore, a base of a 6-DOF EM sensor stationary robot is fitted to the hole 3, as shown in the F view in fig. 1. The pose of the robot tip relative to the base can be solved from pose information of the three EM sensors.
Since groups I and III are fixed to the end and base of the robot, respectively, external forces applied to the end of the robot will change the wavelengths of groups I and III. The moment of the tail end can be calculated through the change of the group I wavelength, so that the external forces in the x and y directions of the tail end are calculated. Because II groups slide along with the driving wires, the pulling force of each driving wire can be obtained through the wavelength change of II groups. The moment of the base can be calculated by the wavelength of the group iii variation. Furthermore, three EM sensors at the tip and base may be used to calculate the pose of the tip. Finally, all of the above information can be used to decouple forces in the z-direction at the robot tip. Therefore, the stress in the three directions of the tail end can be obtained.
The working flow is as follows:
first, in order to improve the sensitivity of sensing the force of the robot tip, the robot base and the tip are respectively designed into four beam structures.
Four optical fibers, F5-F8, each having one FBG sensor at each end, are then secured to the base of the robot, each of the FBG sensors on the optical fibers being secured to one of the beams of the base.
Next, four optical fibers, F1-F4, each having two FBG sensors, one located at the end of the optical fiber, and the FBG sensor at the end of each optical fiber is fixed to one beam at the end of the robot, are fixed to the end of the robot.
Next, the FBG sensors are grouped, the base four FBGs are group iii, the four slidable FBG sensors are group ii, and the terminal four FBG sensors are group i.
And secondly, the moment of the tail end of the robot is obtained by utilizing the change of the I group wavelength, so that the forces in the x and y directions of the tail end are obtained.
Secondly, the tension of the driving wire is calculated by utilizing the change of II groups of wavelengths.
Next, the moment of the robot base is determined by using the change in the group iii wavelength.
Secondly, the pose of the tail end of the robot is obtained by using pose information of two end EMs and one base EM.
Finally, the force in the direction of the tail end z is solved through the force in the directions of the tail ends x and y, the pulling force of the driving wire, the moment of the base part and the pose of the tail end of the robot. The main solution process for tip force perception is shown in fig. 2.
Imparting force sensing capability to the continuum robot tip and ensuring a larger lumen is a significant challenge. Accordingly, considerable effort is required to solve this problem in a novel structure and method.
(1) Tip lateral force sensing
The main principle of force sensing using FBG sensors is to convert wavelength readings into strain and then into moment. The relationship between the strain change Δε and the wavelength change Δλ can be expressed as:
where λ is the current wavelength reading of the grating, λ 0 Is the initial Bragg wavelength of the grating, S ε Is the strain sensitivity coefficient. Since the FBG sensor is fixed to the machineThe base and the tip of the person, whose strain changes reflect the strain of the robot. The strain change of the robot base or tip can be written as:
wherein Δλ 'and λ' 0 The amount of bragg wavelength variation and the initial bragg wavelength, respectively. FBG sensor lambda 0 ' and lambda 0 Symmetrically disposed on the circumference of the base or tip, (2) capable of compensating for wavelength reading errors caused by ambient temperature variations and axial strain of the robot.
There is a well-known linear relationship between strain epsilon and curvature kappa of a robot:
ε=κr (3)
where r is the distance of the robot center axis to the FBG sensor. Thus, the curvature of the base or tip is κ=ε/r. The relationship between the moment M and the curvature k of the robot tip or base can be expressed as:
M=κEI (4)
where EI is the stiffness of the robot tip or base.
As shown in FIG. 3, a reference system { T } is established over the cross-section of group I. In FIG. 3, E1 and E2 represent two 5-DOF EM sensors, respectively; FBG (fiber Bragg Grating) Ⅰ1 -FBG Ⅰ4 Group I is shown on fibers F1-F4. If the mass of the robot is negligible, in { T } the forces in the x and y directions of the robot tip can be calculated as:
wherein F is Tx And F Ty Values of the tip force in the x and y directions of { T }, respectively; m is M Tx And M Ty Moment of the end in x and y directions of { T }, respectively; l (L) 1 Is the distance of the robot tip from the group i cross-section as shown in fig. 1.
(2) Tip axial force sensing
If a certain tensile force is loaded on the driving wire, the Bragg wavelength of the sliding FBG sensor can be changed, and the tensile force T of the driving wire can be calculated as:
T=ε f E f A f (6)
wherein ε is f Is the strain of the driving wire, which can be calculated according to (1); e (E) f Is the Young's modulus of the driving wire; a is that f Representing the drive wire cross-sectional area.
Another reference frame B is established over the cross-section of group iii of the base, as shown in fig. 4. In FIG. 4, FBG Ⅲ1 -FBG Ⅲ4 Group III, located on fibers F5-F8; { E3} is the coordinate system of the 6-DOF EM sensor. The pulling force of the drive wire will cause the base of the robot to deform, thereby changing the bragg wavelength of group iii. The change in base moment caused by these tensile forces can be expressed as:
wherein the method comprises the steps ofAnd->The moments of the base in the x and y directions under { B }, respectively, which are generated by the tension of the drive wire; t (T) 1 ,T 2 ,T 3 And T 4 Is the tension of four driving wires (F1-F4); d, d 1 Is the distance from the central shaft of the robot to the driving wire; beta=pi/4 is the angle between the x-axes of { B } and { T } when the robot is in a straight state, and is also the angle between F1 and F6 in the group iii cross-section. If the mass of the robot is negligible, the base moment generated by the tip force is:
wherein M is Bx And M By The total moment of the base in the x and y directions of { B } respectively, can be obtained from groups III according to (1) to (4);and->The x and y moments generated by the tip force on the base, respectively.
The reference frame { E } in FIG. 3 is determined by the two 5-DOF EM sensors at the end. The x-y plane of { E } is parallel to the x-y plane of { T } and the x-direction is defined by the two EM sensors, and the z-direction is the same as the z-direction of the two EM sensors. Finally, one can go from the known x-direction R Ex And z direction R Ez Obtaining the y direction R Ey :
Wherein P is E1 Is the position of E1, P E2 Is the position of E2, z E1 Is the z-direction of E1, cross represents cross. The pose of { T } is:
wherein R is T And P T Is a rotation matrix and translation vector of { T }; p (P) T =(P E1 +P E2 )/2+R T [0,0,-l 1 +l E1 /2] T α=pi/4 is the angle between { E } and { T } x axes; l (L) E1 Is the length of the 5-DOF EM sensor, rotz is the rotation matrix after rotation about the Z-axis, 0 T Is a transpose of the vector where each element is 0. As shown in FIG. 4, the x-y plane of { E3} is parallel to the x-y plane of { B }. Since the continuous robot base has only one EM sensor, the transformation relationship between { E3} and { B } cannot be determined. When the robot is in a straight line state, the initial pose T of { B } can be obtained B 0 :
Wherein P= [0, -L-L 2 +l 1 ] T ;l 1 L and L 2 Is defined as shown in figure 1. The transformation relationship between { B } and { E3} is a constant value
Wherein T is E3 0 Is the pose of { E3} when the robot is in a straight state. The position and the posture of the robot base can be obtained in real timeThus, the pose of the robot tip with respect to { B }>The method comprises the following steps:
the base moment generated by the tip force is:
wherein the method comprises the steps ofAnd->Are respectively->Is a translation vector and rotation matrix of (a); />Is the terminal force in { B }; l (L) T =[0,0,l 1 ] T . Substituting (5), (7), (8) and (13) into (14) can calculate the force F in the terminal z direction Tz . The forces in the three directions at the end can be obtained.
(3) Robot parameter calibration
Many factors affect the accuracy of the intrinsic parameters of the continuum robot, such as manufacturing errors and the impact of the manufacturing process on material properties. All parameter errors affect the accuracy of the force sensing, which means that the intrinsic parameters need to be calibrated. The calibration method mainly involves applying a known three-dimensional force at the end of the continuum robot.
The robot end parameters can be calibrated by integrating (3) - (5):
wherein ε is Tx Is the strain of the four beams at the end in the x direction, E T I T Is the stiffness of the four beams.
The main process of fiber parameter calibration is to fix weights of different masses at the fiber ends and convert strain readings into tensile forces. The parameters of the optical fiber can be calibrated by integrating (1) and (6) assuming that the ambient temperature does not change during the calibration process:
where T is the weight force of the weight. The robot base parameters can be calibrated by integrating (3), (4), (7), (8) and (14):
wherein ε is Bx Is the strain of the four beams of the robot base in the x direction;the method is defined in (7); in addition, in the case of the optical fiber,
during calibration, all Bragg wavelengths and end pose of the fiber need to be recorded. After sufficient data is collected, the parameters of the robot and fiber are calibrated using a least squares algorithm.
(4) Force sensing resolution analysis
Resolution is an important characteristic of force sensors. To analyze the resolution of the tip force perception, we assume that all conditions and parameters are in an ideal state. According to (15), there is a linear relationship between the force in the x and y directions of the tip and the tip strain. Thus, the resolution of the tip force perception is a constant value in the x and y directions. However, the resolution in the z direction is complex, depending on the bending angle of the continuum robot. In (17), we assume thatAnd F Bx Zero, then it is possible to:
when the continuum robot is curved only in the x-z plane of the base coordinate system, the geometric relationship between the angle of curvature and the end pose is shown in fig. 5. P (P) Bx Can be calculated from the bending angle:
θ is the robot bending angle. The relationship between the resolution in the z direction and the robot bending angle is obtained by substituting (19) into (18). FIG. 6 shows resolution versus bend angle for tip force sensing in the x, y and z directions. As can be seen from fig. 6, the resolution in the z direction is greater as the bending angle of the robot is closer to 90 degrees.
The present embodiment will be understood by those skilled in the art as a more specific description of embodiment 1 and embodiment 2.
Those skilled in the art will appreciate that the invention provides a system and its individual devices, modules, units, etc. that can be implemented entirely by logic programming of method steps, in addition to being implemented as pure computer readable program code, in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers, etc. Therefore, the system and various devices, modules and units thereof provided by the invention can be regarded as a hardware component, and the devices, modules and units for realizing various functions included in the system can also be regarded as structures in the hardware component; means, modules, and units for implementing the various functions may also be considered as either software modules for implementing the methods or structures within hardware components.
The foregoing describes specific embodiments of the present invention. It is to be understood that the invention is not limited to the particular embodiments described above, and that various changes or modifications may be made by those skilled in the art within the scope of the appended claims without affecting the spirit of the invention. The embodiments of the present application and features in the embodiments may be combined with each other arbitrarily without conflict.
Claims (6)
1. A method for sensing three-dimensional force at the tail end of a rope-driven flexible robot, which is characterized by comprising the following steps:
step S1: the base and the tail end of the robot are respectively designed into four beam structures;
step S2: four optical fibers are assembled on the base of the robot, the tail end of each optical fiber is provided with an optical Fiber Bragg Grating (FBG) sensor, and the four FBG sensors of the four optical fibers are respectively fixed on four beams of the base;
step S3: four optical fibers are assembled in four driving cavity channels of the robot to serve as driving wires, each driving wire is provided with two FBG sensors, one of the two FBG sensors is located at the tail end of the driving wire, the four tail end FBG sensors of the four driving wires are respectively fixed on four beams at the tail end of the robot, and the other four FBGs slide in the driving cavity channels along with the driving wires;
step S4: dividing all FBG sensors into three groups I, II and III, wherein the four FBG sensors at the tail end of the robot are in the group I, the four sliding FBG sensors are in the group II, and the four FBG sensors at the base of the robot are in the group III;
step S5: the moment of the tail end of the robot is obtained by utilizing the change of the I group of wavelengths, and the forces in the x and y directions of the tail end are obtained;
step S6: solving the tension of the driving wire by utilizing the change of II groups of wavelengths;
step S7: solving the moment of the robot base by utilizing the change of the III group wavelength;
step S8: two electromagnetic EM sensors with five degrees of freedom and one electromagnetic EM sensor with six degrees of freedom are respectively assembled at the tail end and the base part of the robot, and the pose information of the three EM sensors is utilized to obtain the pose of the tail end of the robot;
step S9: the force in the z direction of the tip is decoupled by the force in the x and y directions of the tip, the tension of the drive wire, the moment of the base and the pose of the robot tip.
2. The method for sensing three-dimensional force at the tail end of a rope-driven flexible robot according to claim 1, wherein the four optical fibers in the step S2 are F5-F8, and each optical fiber has an FBG sensor at the tail end.
3. The method for sensing three-dimensional force at the tail end of a rope-driven flexible robot according to claim 1, wherein the four optical fibers in the step S3 are F1-F4, each optical fiber is provided with two FBG sensors, one optical fiber is positioned at the tail end of the optical fiber, and the distance from the other optical fiber to the tail end is the length of the robot.
4. A three-dimensional force sensing system for a rope-driven flexible robot end, the system comprising:
module M1: the base and the tail end of the robot are respectively designed into four beam structures;
module M2: four optical fibers are assembled on the base of the robot, the tail end of each optical fiber is provided with an optical Fiber Bragg Grating (FBG) sensor, and the four FBG sensors of the four optical fibers are respectively fixed on four beams of the base;
module M3: four optical fibers are assembled in four driving cavity channels of the robot to serve as driving wires, each driving wire is provided with two FBG sensors, one of the two FBG sensors is located at the tail end of the driving wire, the four tail end FBG sensors of the four driving wires are respectively fixed on four beams at the tail end of the robot, and the other four FBGs slide in the driving cavity channels along with the driving wires;
module M4: dividing all FBG sensors into three groups I, II and III, wherein the four FBG sensors at the tail end of the robot are in the group I, the four sliding FBG sensors are in the group II, and the four FBG sensors at the base of the robot are in the group III;
module M5: the moment of the tail end of the robot is obtained by utilizing the change of the I group of wavelengths, and the forces in the x and y directions of the tail end are obtained;
module M6: solving the tension of the driving wire by utilizing the change of II groups of wavelengths;
module M7: solving the moment of the robot base by utilizing the change of the III group wavelength;
module M8: two electromagnetic EM sensors with five degrees of freedom and one electromagnetic EM sensor with six degrees of freedom are respectively assembled at the tail end and the base part of the robot, and the pose information of the three EM sensors is utilized to obtain the pose of the tail end of the robot;
module M9: the force in the z direction of the tip is decoupled by the force in the x and y directions of the tip, the tension of the drive wire, the moment of the base and the pose of the robot tip.
5. The three-dimensional force sensing system for a rope-driven flexible robot tip according to claim 4, wherein four optical fibers in the module M2 are F5-F8, and each optical fiber has one FBG sensor at its tip.
6. The three-dimensional force sensing system for a rope-driven flexible robot tip according to claim 4, wherein four optical fibers in the module M3 are F1-F4, and each optical fiber has two FBG sensors, one at the tip of the optical fiber and the other at a distance from the tip of the robot length.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210389927.3A CN114905549B (en) | 2022-04-14 | 2022-04-14 | Method and system for sensing three-dimensional force at tail end of rope-driven flexible robot |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210389927.3A CN114905549B (en) | 2022-04-14 | 2022-04-14 | Method and system for sensing three-dimensional force at tail end of rope-driven flexible robot |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114905549A CN114905549A (en) | 2022-08-16 |
CN114905549B true CN114905549B (en) | 2024-01-16 |
Family
ID=82765454
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210389927.3A Active CN114905549B (en) | 2022-04-14 | 2022-04-14 | Method and system for sensing three-dimensional force at tail end of rope-driven flexible robot |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114905549B (en) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116608983B (en) * | 2023-07-21 | 2023-11-10 | 武汉理工大学 | Three-dimensional force optical fiber self-decoupling sensing and fault tolerance measuring method and integrated method thereof |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR20130015321A (en) * | 2011-08-03 | 2013-02-14 | 한국과학기술원 | Surgical robot and system for minimally invasive surgery including fiber bragg grating force sensor and method for measuring force using the system |
US8622935B1 (en) * | 2007-05-25 | 2014-01-07 | Endosense Sa | Elongated surgical manipulator with body position and distal force sensing |
CN108593161A (en) * | 2018-04-20 | 2018-09-28 | 南开大学 | A kind of minimally invasive surgical operation robot three-dimensional force sensor based on fiber grating |
CN109813473A (en) * | 2019-03-18 | 2019-05-28 | 南开大学 | A kind of minimally invasive surgical operation robot four-dimension force snesor based on fiber grating |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8182433B2 (en) * | 2005-03-04 | 2012-05-22 | Endosense Sa | Medical apparatus system having optical fiber load sensing capability |
EP3028645B1 (en) * | 2005-08-01 | 2019-09-18 | St. Jude Medical International Holding S.à r.l. | Medical apparatus system having optical fiber load sensing capability |
US10663324B2 (en) * | 2016-03-01 | 2020-05-26 | Cmiws Co., Ltd. | Optical fiber sensor |
-
2022
- 2022-04-14 CN CN202210389927.3A patent/CN114905549B/en active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8622935B1 (en) * | 2007-05-25 | 2014-01-07 | Endosense Sa | Elongated surgical manipulator with body position and distal force sensing |
KR20130015321A (en) * | 2011-08-03 | 2013-02-14 | 한국과학기술원 | Surgical robot and system for minimally invasive surgery including fiber bragg grating force sensor and method for measuring force using the system |
CN108593161A (en) * | 2018-04-20 | 2018-09-28 | 南开大学 | A kind of minimally invasive surgical operation robot three-dimensional force sensor based on fiber grating |
CN109813473A (en) * | 2019-03-18 | 2019-05-28 | 南开大学 | A kind of minimally invasive surgical operation robot four-dimension force snesor based on fiber grating |
Non-Patent Citations (1)
Title |
---|
基于SSA-ELM 的双层十字梁结构 光纤布拉格光栅传感器三维力解耦;孙世政,于竞童,韩宇等;光学精密工程;第30卷(第3期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN114905549A (en) | 2022-08-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Rao et al. | How to model tendon-driven continuum robots and benchmark modelling performance | |
US20110113852A1 (en) | Optical fiber shape sensor calibration | |
CN114905549B (en) | Method and system for sensing three-dimensional force at tail end of rope-driven flexible robot | |
CN105682865A (en) | Method and system for determination of at least one property of a manipulator | |
Olson et al. | An Euler–Bernoulli beam model for soft robot arms bent through self-stress and external loads | |
JP2010529910A (en) | Method for calibrating the position of an articulated system such as a robot | |
CN104535027A (en) | Robot precision compensation method for variable-parameter error recognition | |
Schmidt | Modeling techniques and reliable real-time implementation of kinematics for cable-driven parallel robots using polymer fiber cables | |
US12012868B2 (en) | Discrete macroscopic metamaterial systems | |
Maloisel et al. | Automated routing of muscle fibers for soft robots | |
Chen et al. | Design and development of a soft robotic manipulator | |
Sun et al. | Enhancing torsional stiffness of continuum robots using 3-D topology optimized flexure joints | |
Bartholdt et al. | A parameter identification method for static cosserat rod models: Application to soft material actuators with exteroceptive sensors | |
CN105868475A (en) | Quasi real-time assistant control method and system of telescope reflecting surface | |
Zhang et al. | Shape detection and reconstruction of soft robotic arm based on fiber Bragg grating sensor array | |
Wang et al. | Design and experimental characterization of a push-pull flexible rod-driven soft-bodied robot | |
Yang et al. | Self-calibration of three-legged modular reconfigurable parallel robots based on leg-end distance errors | |
Wen et al. | Modeling and analysis of tendon-driven parallel continuum robots under constant curvature and pseudo-rigid-body assumptions | |
Lezcano et al. | Trajectory generation of FBG-sensorized needles for insertions into multi-layer tissue | |
Qi et al. | Shape sensing and feedback control of the catheter robot for interventional surgery | |
Wang et al. | Shape Sensing for Continuum Robots Using FBG Sensors Array Considering Bending and Twisting | |
US20220402130A1 (en) | System and method for controlling continuum robot, and continuum robot | |
CN112220558B (en) | Surgical mechanical arm and shape measuring method thereof | |
Rouse et al. | Development of Single Piece Designed Compliant Locomotive Mechanism | |
Tebyani et al. | A geometric kinematic model for flexible voxel-based robots |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |