CN116999177B - Contact force autonomous protection algorithm operated by natural channel endoscope - Google Patents
Contact force autonomous protection algorithm operated by natural channel endoscope Download PDFInfo
- Publication number
- CN116999177B CN116999177B CN202311280041.6A CN202311280041A CN116999177B CN 116999177 B CN116999177 B CN 116999177B CN 202311280041 A CN202311280041 A CN 202311280041A CN 116999177 B CN116999177 B CN 116999177B
- Authority
- CN
- China
- Prior art keywords
- motion
- coordinate system
- contact force
- mechanical arm
- blocking
- 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
- 230000033001 locomotion Effects 0.000 claims abstract description 81
- 238000000034 method Methods 0.000 claims abstract description 72
- 230000000903 blocking effect Effects 0.000 claims abstract description 58
- 238000013507 mapping Methods 0.000 claims abstract description 31
- 230000009466 transformation Effects 0.000 claims abstract description 30
- 230000008569 process Effects 0.000 claims abstract description 29
- 230000000007 visual effect Effects 0.000 claims abstract description 12
- 230000001131 transforming effect Effects 0.000 claims abstract description 4
- 239000013598 vector Substances 0.000 claims description 38
- 239000011159 matrix material Substances 0.000 claims description 24
- 230000005540 biological transmission Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 6
- 230000001960 triggered effect Effects 0.000 claims description 6
- 230000008859 change Effects 0.000 claims description 4
- 230000007246 mechanism Effects 0.000 claims description 4
- 230000009977 dual effect Effects 0.000 claims description 3
- 230000000694 effects Effects 0.000 claims description 3
- 230000002349 favourable effect Effects 0.000 claims description 3
- 230000000737 periodic effect Effects 0.000 abstract 1
- 238000010586 diagram Methods 0.000 description 6
- 230000009471 action Effects 0.000 description 3
- 238000002674 endoscopic surgery Methods 0.000 description 3
- 230000006378 damage Effects 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 230000002269 spontaneous effect Effects 0.000 description 2
- 208000027418 Wounds and injury Diseases 0.000 description 1
- FFBHFFJDDLITSX-UHFFFAOYSA-N benzyl N-[2-hydroxy-4-(3-oxomorpholin-4-yl)phenyl]carbamate Chemical compound OC1=C(NC(=O)OCC2=CC=CC=C2)C=CC(=C1)N1CCOCC1=O FFBHFFJDDLITSX-UHFFFAOYSA-N 0.000 description 1
- 230000015271 coagulation Effects 0.000 description 1
- 238000005345 coagulation Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000001839 endoscopy Methods 0.000 description 1
- 208000014674 injury Diseases 0.000 description 1
- 230000037361 pathway Effects 0.000 description 1
Classifications
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
- A61B34/30—Surgical robots
- A61B34/37—Master-slave robots
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
- A61B34/70—Manipulators specially adapted for use in surgery
- A61B34/75—Manipulators having means for prevention or compensation of hand tremors
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
- A61B34/70—Manipulators specially adapted for use in surgery
- A61B34/76—Manipulators having means for providing feel, e.g. force or tactile feedback
Abstract
Endoscope operation through natural channelThe contact force autonomous protection algorithm comprises visual master-slave mapping and contact force autonomous protection, wherein the contact force autonomous protection comprises the following 5 steps: (1) transforming coordinates; (2) blocking motion delta; (3) compliant motion delta; (4) compliant immobilized spots; (5) and (5) carrying out inverse coordinate transformation. From step 4Is obtained from step 5、、Is a value of (a). Dynamic adjustment by constantly periodic iteration、、AndAnd the values are used as input variables to be applied to an intuitive master-slave mapping algorithm in real time, so that the automatic protection of the contact force of the mechanical arm in the operation process is realized, and the contact force and the moment are automatically limited in a controllable range.
Description
Technical Field
The invention belongs to the technical field of automatic control, and particularly relates to a contact force autonomous protection algorithm operated by a natural channel endoscope.
Background
When the traditional method uses a master-slave type endoscopic surgery robot to operate through a natural channel, because the force sensing capability of human hands cannot be compared with that of force sensors on a mechanical arm and an endoscope, not so abundant contact force information is acquired, and the contact force of the endoscope in the operation process is difficult to perfectly reappear due to the isomerism of a master-end control platform and a slave-end operation platform. If the contact force is forcibly reproduced, the operation action of the operator is affected, making the operation more difficult. The method for protecting the contact force is intuitive though not feasible by enabling an operator to sense the contact force during operation.
Disclosure of Invention
The embodiment of the invention provides a contact force autonomous protection algorithm operated by a natural channel endoscope, which comprises visual master-slave mapping and contact force autonomous protection, wherein the contact force autonomous protection comprises the following 5 steps:
(1) transforming coordinates;
(2) blocking motion delta;
(3) compliant motion delta;
(4) compliant immobilized spots;
(5) and (5) carrying out inverse coordinate transformation.
According to one embodiment of the invention, for example, the intuitive master-slave mapping includes:
mapping x, y, z, a coordinates of the master end operating handle to-z, y, x and alpha coordinates of the tool tip at the tail end of the slave end mechanical arm on the basis of registration; the registration is: acquiring a handle registration point pose and a mechanical arm registration pose at registration time so as to perform subsequent visual master-slave mapping calculation; in order to ensure the safety of operation, the slave mechanical arm needs to drive the endoscope to perform rcm motion, namely, the axis of the sheath of the endoscope passes through a fixed point in space in the motion process, and the rcm motion is provided with a constraint condition of 2 degrees of freedom, so that the intuitive master-slave mapping algorithm only maps coordinates of 4 degrees of freedom;
the algorithm of the intuitive master-slave mapping is as follows:
β＝atan((z r -z reg )/(x reg -x r +L str ))
α＝a reg -a r
wherein x is r 、y r 、z r 、a r The pose is truly transferred for the current handle; x is x reg 、y reg 、z reg 、a reg Registering the point pose for the handle; l (L) str The length of the endoscope sheath in front of the fixed point at the registration moment; beta, gamma, alpha, d are intermediate variables;for the homogeneous transformation matrix from the center point coordinate system of the end mechanical arm tool to the stationary point coordinate system at the registration moment,/for the registration moment>Is->Is the inverse of->
I 3 The three-order identity matrix is formed, and p is a three-order column vector;
the soft quantity of the position of the fixed point is a third-order zero vector when the soft quantity of the position of the fixed point is not started;
T f the position and posture increment homogeneous transformation matrix of the slave manipulator;the method is used for registering the pose of the mechanical arm, and is a homogeneous transformation matrix from a base coordinate system of the mechanical arm at the end to a center point coordinate system of the tool at the registration moment; t (T) dest The method is a current mechanical arm target pose, and is a homogeneous transformation matrix from an end mechanical arm base coordinate system to a tool center point coordinate system.
According to one embodiment of the invention, for example, the coordinate transformation comprises:
dx r ＝x r -x rh
dy r ＝y r -y rh
dz r ＝z r -z rh
tcpreg R tcpdesth ＝ tcpreg T tcpdesth (1:3,1:3)
wherein, (x) r ,y r ,z r ) For the true delivery position of the current cycle of the handle, (x) rh ,y rh ,z rh ) The actual transmission position of the cycle on the handle;the method comprises the steps of registering a homogeneous transformation matrix from a mechanical arm base coordinate system to a tool center point coordinate system at the moment; t (T) desth For the last period of the target pose of the mechanical arm, i.e. T dest The value of the last period; (1:3 ) represents a 3 x 3 sub-matrix of the first three rows and the first three columns of a matrix; />For registering the rotational transformation of the tool center point coordinate system to the master end operating handle coordinate system,
dp v and representing the virtual displacement of the current tool center point coordinate system, and using the virtual displacement in the calculation process of the subsequent contact force autonomous protection algorithm.
According to one embodiment of the invention, for example, blocking motion delta comprises:
with dual threshold force protection, F lim,M 、F lim,m Respectively represents a large external force threshold and a small external force threshold, F lim,M ＞F lim,m ＞0；T lim,M 、T lim,m Respectively represents a large external moment threshold and a small external moment threshold, T lim,M ＞T lim,m ＞0；|T xy I represents vector T xy Is a binary norm of (2);
when F z Negative going beyond the external force big threshold and |T xy Executing a motion blocking flow 1 if the I exceeds the external moment big threshold; when only F z Executing a motion blocking flow 2 if the negative direction exceeds the external force big threshold value;when only |T xy Executing a motion blocking flow 3 if the I exceeds the external moment big threshold; when the external force large threshold value and the external force moment large threshold value are not triggered, entering a small threshold value branch: if F z Executing a motion blocking process 4 when the negative direction exceeds the small external force threshold; if |T xy Executing a motion blocking process 5 when the I exceeds the external moment small threshold; if neither of the above conditions is still triggered, then the present cycle will not perform any motion blocking procedure, i.e. the virtual displacement dp is not changed v ；
Wherein, motion blocking procedure 1:
dp v ＝P 1 e Fz +P 2 e Txy
wherein e Fz Representing vectorsIs a unitized vector of e Txy Representation vector->Is a unitized vector of (a);
the role of motion occlusion procedure 1 is for virtual displacement dp v Only remain in F z Forward direction and directionBlocking all other components by components in the positive direction;
motion blocking procedure 2:
dp v ＝P 1 e Fz
the motion blocking procedure 2 functions for the virtual displacement dp v Only remain in F z In the positive directionBlocking all the remaining components;
motion blocking procedure 3:
dp v ＝P 2 e Txy
the motion blocking procedure 3 functions for the virtual displacement dp v Only remain inBlocking all other components by components in the positive direction;
motion blocking procedure 4:
dp v ＝dp v -P 3 (-e Fz )
the motion blocking procedure 4 functions for the virtual displacement dp v Remove it at F z Components in the negative direction, remaining components;
motion blocking procedure 5:
dp v ＝dp v -P 4 (-e Txy )
the motion blocking procedure 5 functions for the virtual displacement dp v Remove it atComponents in the negative direction, remaining components;
the process of blocking the motion increment is to change the virtual displacement dp of the current tool center point coordinate system according to the external force feedback of the flange coordinate system at the tail end of the mechanical arm v Leaving only a portion of the components, removing a portion of the components, or leaving unchanged.
According to one embodiment of the invention, for example, compliant motion delta comprises:
dp v ＝dp v +P 5 e Fz
dp v ＝dp v +P 6 e Txy
wherein, |F z I represents vector F z Is |T xy I represents vector T xy Is a binary norm of (2); k (K) F K is the rigidity coefficient related to external force T K is the rigidity coefficient related to external moment F ＞0，K T ＞0；
The process of the compliant motion increment is actually based on the external force feedback under the flange coordinate system of the tail end of the mechanical arm and the rigidity coefficient of the control system, and the virtual displacement dp under the current tool center point coordinate system v The calculated compliance is increased to create a mechanism for the spontaneous reduction of the operating contact force.
According to one embodiment of the invention, for example, the compliant stationary point comprises:
Δrcm＝Δrcm h +P 7 e Fxy
wherein,|F xy i represents vector F xy Is the second norm of e Fxy Representing vector F xy Is a unitized vector of (a); Δrcm h For the position compliance of the fixed point in the previous period, deltrcm is the position compliance of the fixed point in the current period, and the position compliance of the fixed point in the initial period is a third-order zero directionAn amount of;
the process of softening the fixed point is to design a value strategy which is favorable for reducing the contact force of operation for Deltarcm, so that the position softening link of the fixed point in the visual master-slave mapping is started, and the forward effect can be obtained.
According to one embodiment of the invention, for example, the inverse coordinate transformation includes:
x r ＝dp r,x +x rh
y r ＝dp r,y +y rh
z r ＝dp r,z +z rh
wherein dp r Representing a position vector, dp, obtained after the actual transmission position of the current period of the handle passes through the contact force protection algorithm r,x 、dp r,y 、dp r,z Respectively represent dp r X, y, z components of (c).
According to one embodiment of the invention, for example, the value of Δrcm is obtained from the 4 th compliant stationary point and x is obtained from the 5 th coordinate inverse transformation r 、y r 、z r Is a value of (2); dynamically adjusting x by constantly periodically iterating r 、y r 、z r And Δrcm, and apply these values as input variables in real time to the intuitive master-slave mapping.
Drawings
FIG. 1 is a schematic diagram of a master-slave type endoscopic surgical robot via a natural channel.
Fig. 2 is a schematic view of the coordinate system directions of the master end operating handle and the slave end mechanical arm.
FIG. 3 is a logic flow diagram of blocking motion delta.
Fig. 4 is an external force diagram of the end flange coordinate system of the mechanical arm collected during the experimental process according to the embodiment of the invention.
Fig. 5 is a diagram of external force moment in the flange coordinate system of the end of the mechanical arm collected during the experimental process according to the embodiment of the invention.
Detailed Description
The present invention will be further described in detail below with reference to specific embodiments and with reference to the accompanying drawings, in order to make the objects, technical solutions and advantages of the present invention more apparent. Those skilled in the art will recognize that the present invention is not limited to the drawings and the following examples.
Fig. 1 is a schematic structural diagram of a master-slave type endoscopic surgery robot through a natural channel, wherein the master-slave type endoscopic surgery robot through the natural channel comprises a master-end operation handle 1, a slave-end operation platform 2, a mechanical arm 3 and an endoscope 4. When such a master-slave heterogeneous type endoscopic surgical robot is used for operation through a natural channel, it is required to be able to control the movement of the slave manipulator 2 stably and intuitively by the master-end operation handle 1 and to satisfy safety and accuracy, thereby effectively performing cutting and coagulation operations. However, the existing master-slave heterogeneous endoscopic surgical robots through natural channels cannot completely meet the safety and accuracy requirements of actual operations.
Embodiments of the present invention provide a contact force autonomous protection algorithm operated via natural channel endoscopy. By utilizing the algorithm, the slave operation platform autonomously makes contact force protection decisions according to force sensing data of the mechanical arm without judgment of people. The slave surgical platform is responsible for rejecting dangerous components that would cause excessive contact force in the operative action, and moderately adding compliant action to reduce components that have been too large in the contact force.
The contact force autonomous protection algorithm operated by the natural channel endoscope provided by the embodiment of the invention is based on visual master-slave mapping, and autonomously changes the input quantity of the master-slave mapping according to the flange six-dimensional force perceived by the mechanical arm, so that the movement of the mechanical arm is changed, and the purpose of contact force protection is achieved. The contact force autonomous protection algorithm operated by the natural channel endoscope comprises an intuitive master-slave mapping and contact force autonomous protection, wherein the contact force autonomous protection comprises the following 5 steps:
(1) transforming coordinates;
(2) blocking motion delta;
(3) compliant motion delta;
(4) compliant immobilized spots;
(5) and (5) carrying out inverse coordinate transformation.
As will be described below.
Visual master-slave mapping is first described. Fig. 2 is a schematic view of the coordinate system directions of the master end operating handle and the slave end mechanical arm. The direct-viewing master-slave mapping algorithm maps x, y, z, a coordinates of the master end operating handle to-z, y, x and alpha coordinates of the tool tip at the tail end of the slave end mechanical arm on the basis of registration. The registration is: and acquiring the registration point pose of the handle and the registration pose of the mechanical arm at the registration moment so as to perform subsequent visual master-slave mapping calculation. The sequence and the sign of the intuitive master-slave mapping coordinates are different, and the most intuitive operation experience needs to be realized on the basis of conforming to the definition of the respective coordinate systems of the master-end operation handle and the slave-end mechanical arm. In order to ensure the safety of operation, the slave mechanical arm needs to drive the endoscope to perform rcm motion, namely, in the motion process, the axis of the sheath of the endoscope passes through a fixed point (fixed point) in space, and the rcm motion is provided with constraint conditions of 2 degrees of freedom, so that the intuitive master-slave mapping algorithm only maps coordinates of 4 degrees of freedom.
The algorithm of the intuitive master-slave mapping is as follows:
β＝atan((z r -z reg )/(x reg -x r +L str ))
α＝a reg -a r
wherein x is r 、y r 、z r 、a r The pose is truly transferred for the current handle; x is x reg 、y reg 、z reg 、a reg Registering the point pose for the handle; l (L) str The length of the endoscope sheath in front of the fixed point at the registration moment; beta, gamma, alpha, d are intermediate variables;for the homogeneous transformation matrix from the center point coordinate system of the end mechanical arm tool to the stationary point coordinate system at the registration moment,/for the registration moment>Is->Is the inverse of->
I 3 The three-order identity matrix is formed, and p is a three-order column vector;
the soft quantity of the position of the fixed point is a third-order zero vector when the soft quantity of the position of the fixed point is not started;
T f the position and posture increment homogeneous transformation matrix of the slave manipulator;the method is used for registering the pose of the mechanical arm, and is a homogeneous transformation matrix from a base coordinate system of the mechanical arm at the end to a center point coordinate system of the tool at the registration moment; t (T) dest The method is a current mechanical arm target pose, and is a homogeneous transformation matrix from an end mechanical arm base coordinate system to a tool center point coordinate system.
And on the basis of the visual master-slave mapping, the contact force autonomous protection is developed. The contact force autonomous protection provided by the embodiment of the invention is essentially realized by the external force F under the flange coordinate system of the tail end of the mechanical arm measured in real time in the operation process x 、F y 、F z External moment T x 、T y (T is not used in the present algorithm) z ) Dynamically adjusting input variable x of visual master-slave mapping algorithm r 、y r 、z r Δrcm (no adjustment of a in the present algorithm) r ) Thereby autonomously limiting the contact force and moment during operation to a controllable range.
1. The coordinate transformation includes:
dx r ＝x r -x rh
dy r ＝y r -y rh
dz r ＝z r -z rh
tcpreg R tcpdesth ＝ tcpreg T tcpdesth (1:3,1:3)
wherein, (x) r ,y r ,z r ) For the true delivery position of the current cycle of the handle, (x) rh ,y rh ,z rh ) The actual transmission position of the cycle on the handle;the method comprises the steps of registering a homogeneous transformation matrix from a mechanical arm base coordinate system to a tool center point coordinate system at the moment; t (T) desth For the last period of the target pose of the mechanical arm, i.e. T dest The value of the last period; (1:3 ) represents a 3 x 3 sub-matrix of the first three rows and the first three columns of a matrix; />For registering the rotational transformation of the tool center point coordinate system to the master end operating handle coordinate system,
dp v and representing the virtual displacement of the current tool center point coordinate system, and using the virtual displacement in the calculation process of the subsequent contact force autonomous protection algorithm.
2. Blocking motion delta:
a logic flow diagram for blocking motion delta is shown in fig. 3.
With dual threshold force protection, F lim,M 、F lim,m Respectively represents a large external force threshold and a small external force threshold, F lim,M ＞F lim,m ＞0；T lim,M 、T lim,m Respectively represents a large external moment threshold and a small external moment threshold, T lim,M ＞T lim,m ＞0。|T xy I represents vector T xy Two of (2)Norms.
When F z Negative going beyond the external force big threshold and |T xy The motion blocking process 1 is executed when the I exceeds the external moment big threshold; when only F z The negative direction exceeds the external force big threshold value, and the motion blocking flow 2 is executed; when only |T xy The motion blocking process 3 is executed when the I exceeds the external moment big threshold; when the external force large threshold value and the external force moment large threshold value are not triggered, a small threshold value branch is entered: if F z Executing a motion blocking process 4 when the negative direction exceeds the small external force threshold; if |T xy Executing a motion blocking process 5 when the I exceeds the external moment small threshold; if neither of the above conditions is still triggered, then the present cycle will not perform any motion blocking procedure, i.e. will not change the virtual displacement dp v 。
Motion blocking scheme 1:
dp v ＝P 1 e Fz +P 2 e Txy
wherein e Fz Representing vectorsIs a unitized vector of e Txy Representation vector->Is a unit vector of (a).
The role of motion occlusion procedure 1 is for virtual displacement dp v Only remain in F z Forward direction and directionComponents in the positive direction block all the remaining components.
Motion blocking procedure 2:
dp v ＝P 1 e Fz
the motion blocking procedure 2 functions for the virtual displacement dp v Only remain in F z Components in the positive direction block all the remaining components.
Motion blocking procedure 3:
dp v ＝P 2 e Txy
the motion blocking procedure 3 functions for the virtual displacement dp v Only remain inComponents in the positive direction block all the remaining components.
Motion blocking procedure 4:
dp v ＝dp v -P 3 (-e Fz )
the motion blocking procedure 4 functions for the virtual displacement dp v Remove it at F z The components in the negative direction remain the remaining components.
Motion blocking procedure 5:
dp v ＝dp v -P 4 (-e Txy )
the motion blocking procedure 5 functions for the virtual displacement dp v Remove it atThe components in the negative direction remain the remaining components.
The process of blocking the motion increment is to change the current according to the external force feedback of the flange coordinate system of the tail end of the mechanical armVirtual displacement dp in tool center point coordinate system v Leaving only a portion of the components, removing a portion of the components, or leaving unchanged.
3. Compliant motion delta:
dp v ＝dp v +P 5 e Fz
dp v ＝dp v +P 6 e Txy
wherein, |F z I represents vector F z Is |T xy I represents vector T xy Is a binary norm of (2); k (K) F K is the rigidity coefficient related to external force T K is the rigidity coefficient related to external moment F ＞0，K T ＞0。
The process of the compliant motion increment is actually based on the external force feedback under the flange coordinate system of the tail end of the mechanical arm and the rigidity coefficient of the control system, and the virtual displacement dp under the current tool center point coordinate system v The calculated compliance is increased to create a mechanism for the spontaneous reduction of the operating contact force.
4. Compliant immobilization point:
Δrcm＝Δrcm h +P 7 e Fxy
wherein,|F xy i represents vector F xy Is the second norm of e Fxy Representing vector F xy Is a unitized vector of (a); Δrcm h To get up toThe motionless point position compliance quantity of one period is deltarcm, the motionless point position compliance quantity of the current period is deltarcm, and the motionless point position compliance quantity of the initial period is a third-order zero vector.
The process of softening the fixed point is to design a value strategy which is favorable for reducing the contact force of operation for Deltarcm, so that the position softening link of the fixed point in the visual master-slave mapping is started, and the forward effect can be obtained.
5. Inverse coordinate transformation:
x r ＝dp r,x +x rh
y r ＝dp r,y +y rh
z r ＝dp r,z +z rh
wherein dp r Representing a position vector, dp, obtained after the actual transmission position of the current period of the handle passes through the contact force protection algorithm r,x 、dp r,y 、dp r,z Respectively represent dp r X, y, z components of (c).
The embodiment of the invention describes a contact force autonomous protection algorithm operated by a natural channel endoscope, wherein the value of Deltarcm is obtained from the 4 th step of the flow, and x is obtained from the 5 th step of the flow r 、y r 、z r Is a value of (a). Dynamically adjusting x by constantly periodically iterating r 、y r 、z r And Deltrcm, and applying the values as input variables to an intuitive master-slave mapping algorithm in real time, thereby realizing the autonomous protection of the contact force of the mechanical arm in the operation process and autonomously limiting the contact force and moment in a controllable range.
If there is no contact force protection mechanism during operation of the endoscope robot via the natural pathway, the contact force and moment between the endoscope and the tissue of the operator will be uncontrolled, easily resulting in injury to the operator and damage to the endoscope, resulting in irreparable losses.
Fig. 4 and fig. 5 are respectively an external force and an external moment under a flange coordinate system of the tail end of the mechanical arm, which are collected in a certain experimental process. The experiment adopts the contact force autonomous protection algorithm provided by the invention, and the external force threshold F lim,M 、F lim,m Respectively taking 25N and 15N, and the threshold value T of the external moment lim,M 、T lim,m Respectively taking 10 nm and 6 nm. From the data statistics of fig. 4 and 5, the probabilities that the external contact force and the external force moment are limited within the average values of the large threshold value and the small threshold value are 97.897% and 97.923%, respectively, and the probabilities that the external contact force and the external force moment are limited within the large threshold value are 99.658% and 99.527%, respectively, during the whole experiment.
Claims (2)
1. A natural channel endoscope operated contact force autonomous protection algorithm, characterized in that the natural channel endoscope operated contact force autonomous protection algorithm comprises an intuitive master-slave mapping and contact force autonomous protection, wherein the contact force autonomous protection comprises the following 5 steps:
(1) transforming coordinates;
(2) blocking motion delta;
(3) compliant motion delta;
(4) compliant immobilized spots;
(5) inverse coordinate transformation;
the intuitive master-slave mapping includes:
mapping x, y, z, a coordinates of the master end operating handle to-z, y, x and alpha coordinates of the tool tip at the tail end of the slave end mechanical arm on the basis of registration; the registration is: acquiring a handle registration point pose and a mechanical arm registration pose at registration time so as to perform subsequent visual master-slave mapping calculation; in order to ensure the safety of operation, the slave mechanical arm needs to drive the endoscope to perform rcm motion, namely, the axis of the sheath of the endoscope passes through a fixed point in space in the motion process, and the rcm motion is provided with a constraint condition of 2 degrees of freedom, so that the intuitive master-slave mapping algorithm only maps coordinates of 4 degrees of freedom;
the algorithm of the intuitive master-slave mapping is as follows:
β＝atan((z r -z reg )/(x reg -x r +L str ))
α＝a reg -a r
wherein x is r 、y r 、z r 、a r The pose is truly transferred for the current handle; x is x reg 、y reg 、z reg 、a reg Registering the point pose for the handle; l (L) str The length of the endoscope sheath in front of the fixed point at the registration moment; beta, gamma, alpha, d are intermediate variables;for the homogeneous transformation matrix from the center point coordinate system of the end mechanical arm tool to the stationary point coordinate system at the registration moment,/for the registration moment>Is->Is a function of the inverse transform of (a),
I 3 the three-order identity matrix is formed, and p is a three-order column vector;
the soft quantity of the position of the fixed point is a third-order zero vector when the soft quantity of the position of the fixed point is not started;
T f the position and posture increment homogeneous transformation matrix of the slave manipulator;the method is used for registering the pose of the mechanical arm, and is a homogeneous transformation matrix from a base coordinate system of the mechanical arm at the end to a center point coordinate system of the tool at the registration moment; t (T) dest The method is a current mechanical arm target pose, and is a homogeneous transformation matrix from an end mechanical arm base coordinate system to a tool center point coordinate system;
the coordinate transformation includes:
dx r ＝x r -x rh
dy r ＝y r -y rh
dz r ＝z r -z rh
tcpreg R tcpdesth ＝ tcpreg T tcpdesth (1:3,1:3)
wherein, (x) r ,y r ,z r ) For the true delivery position of the current cycle of the handle, (x) rh ,y rh ,z rh ) The actual transmission position of the cycle on the handle;the method comprises the steps of registering a homogeneous transformation matrix from a mechanical arm base coordinate system to a tool center point coordinate system at the moment; t (T) desth For the last period of the target pose of the mechanical arm, i.e. T dest The value of the last period; (1:3 ) represents a 3 x 3 sub-matrix of the first three rows and the first three columns of a matrix; />For registering the rotational transformation of the tool center point coordinate system to the master end operating handle coordinate system,
dp v representing the virtual displacement of the current tool center point coordinate system for the calculation process of the subsequent contact force autonomous protection algorithm;
blocking motion delta includes:
with dual threshold force protection, F lim,M 、F lim,m Respectively represents the large threshold value of the external forceSmall threshold of external force, F lim,M ＞F lim,m ＞0；T lim,M 、T lim,m Respectively represents a large external moment threshold and a small external moment threshold, T lim,M ＞T lim,m ＞0；|T xy I represents vector T xy Is a binary norm of (2);
when F z Negative going beyond the external force big threshold and |T xy Executing a motion blocking flow 1 if the I exceeds the external moment big threshold; when only F z Executing a motion blocking flow 2 if the negative direction exceeds the external force big threshold value; when only |T xy Executing a motion blocking flow 3 if the I exceeds the external moment big threshold; when the external force large threshold value and the external force moment large threshold value are not triggered, entering a small threshold value branch: if F z Executing a motion blocking process 4 when the negative direction exceeds the small external force threshold; if |T xy Executing a motion blocking process 5 when the I exceeds the external moment small threshold; if neither of the above conditions is still triggered, then the present cycle will not perform any motion blocking procedure, i.e. the virtual displacement dp is not changed v ；
Wherein, motion blocking procedure 1:
dp v ＝P 1 e Fz +P 2 e Txy
wherein e Fz Representing vectorsIs a unitized vector of e Txy Representation vector->Is a unitized vector of (a);
the role of motion occlusion procedure 1 is for virtual displacement dp v Only remain in F z Forward direction and directionBlocking all other components by components in the positive direction;
motion blocking procedure 2:
dp v ＝P 1 e Fz
the motion blocking procedure 2 functions for the virtual displacement dp v Only remain in F z Blocking all other components by components in the positive direction;
motion blocking procedure 3:
dp v ＝P 2 e Txy
the motion blocking procedure 3 functions for the virtual displacement dp v Only remain inBlocking all other components by components in the positive direction;
motion blocking procedure 4:
dp v ＝dp v -P 3 (-e Fz )
the motion blocking procedure 4 functions for the virtual displacement dp v Remove it at F z Components in the negative direction, remaining components;
motion blocking procedure 5:
dp v ＝dp v -P 4 (-e Txy )
the motion blocking procedure 5 functions for the virtual displacement dp v Remove it atComponents in the negative direction, remaining components;
the process of blocking the motion increment is to change the virtual displacement dp of the current tool center point coordinate system according to the external force feedback of the flange coordinate system at the tail end of the mechanical arm v So that it retains only a part of the components, removes a part of the components or remains unchanged;
compliant motion delta includes:
dp v ＝dp v +P 5 e Fz
dp v ＝dp v +P 6 e Txy
wherein, |F z I represents vector F z Is |T xy I represents vector T xy Is a binary norm of (2); k (K) F K is the rigidity coefficient related to external force T K is the rigidity coefficient related to external moment F ＞0，K T ＞0；
The process of the compliant motion increment is actually based on the external force feedback under the flange coordinate system of the tail end of the mechanical arm and the rigidity coefficient of the control system, and the virtual displacement dp under the current tool center point coordinate system v Increasing the calculated compliance to form a mechanism for spontaneously reducing the contact force of the operation;
compliant dead spots include:
Δrcm＝Δrcm h +P 7 e Fxy
wherein,|F xy i represents vector F xy Is the second norm of e Fxy Representing vector F xy Is a unitized vector of (a); Δrcm h For the motionless point position compliance of the previous period, Δrcm is the motionless point position compliance of the current period, and the motionless point position compliance of the initial period is a third-order zero vector;
the process of softening the fixed point is to design a value strategy which is favorable for reducing the contact force of operation for Deltarcm, so that the position softening link of the fixed point in the visual master-slave mapping is started, and the forward effect can be obtained;
the inverse transformation of coordinates includes:
x r ＝dp r,x +x rh
y r ＝dp r,y +y rh
z r ＝dp r,z +z rh
wherein dp r Representing a position vector, dp, obtained after the actual transmission position of the current period of the handle passes through the contact force protection algorithm r,x 、dp r,y 、dp r,z Respectively represent dp r X, y, z components of (c).
2. The natural channel endoscopically operated contact force autonomous protection algorithm of claim 1, wherein the value of Δrcm is obtained from the 4 th compliant dead point and x is obtained from the 5 th coordinate inverse transformation r 、y r 、z r Is a value of (2); by passing throughContinuously and periodically iterating, and dynamically adjusting x r 、y r 、z r And Δrcm, and apply these values as input variables in real time to the intuitive master-slave mapping.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311280041.6A CN116999177B (en) | 2023-10-07 | 2023-10-07 | Contact force autonomous protection algorithm operated by natural channel endoscope |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311280041.6A CN116999177B (en) | 2023-10-07 | 2023-10-07 | Contact force autonomous protection algorithm operated by natural channel endoscope |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116999177A CN116999177A (en) | 2023-11-07 |
| CN116999177B true CN116999177B (en) | 2024-01-12 |
Family
ID=88569459
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311280041.6A Active CN116999177B (en) | 2023-10-07 | 2023-10-07 | Contact force autonomous protection algorithm operated by natural channel endoscope |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116999177B (en) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH0976185A (en) * | 1995-07-12 | 1997-03-25 | Fanuc Ltd | Movement control method of robot |
| WO2006124390A2 (en) * | 2005-05-19 | 2006-11-23 | Intuitive Surgical, Inc | Software center and highly configurable robotic systems for surgery and other uses |
| JP2008200763A (en) * | 2007-02-16 | 2008-09-04 | Hitachi Ltd | Control device for manipulator for working |
| CN105911863A (en) * | 2016-05-19 | 2016-08-31 | 杭州新松机器人自动化有限公司 | Multi-robot cooperative clamping system neural network trajectory tracking control method |
| CN112311192A (en) * | 2020-10-20 | 2021-02-02 | 江苏科技大学 | Logistics device based on magnetic suspension planar motor and control method thereof |
| CN114795495A (en) * | 2022-04-25 | 2022-07-29 | 北京肿瘤医院(北京大学肿瘤医院) | Master-slave operation minimally invasive surgery robot system |
| CN115317130A (en) * | 2021-05-10 | 2022-11-11 | 上海微创医疗机器人（集团）股份有限公司 | Surgical robot system, adjustment system, and storage medium |
-
2023
- 2023-10-07 CN CN202311280041.6A patent/CN116999177B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH0976185A (en) * | 1995-07-12 | 1997-03-25 | Fanuc Ltd | Movement control method of robot |
| WO2006124390A2 (en) * | 2005-05-19 | 2006-11-23 | Intuitive Surgical, Inc | Software center and highly configurable robotic systems for surgery and other uses |
| JP2008200763A (en) * | 2007-02-16 | 2008-09-04 | Hitachi Ltd | Control device for manipulator for working |
| CN105911863A (en) * | 2016-05-19 | 2016-08-31 | 杭州新松机器人自动化有限公司 | Multi-robot cooperative clamping system neural network trajectory tracking control method |
| CN112311192A (en) * | 2020-10-20 | 2021-02-02 | 江苏科技大学 | Logistics device based on magnetic suspension planar motor and control method thereof |
| CN115317130A (en) * | 2021-05-10 | 2022-11-11 | 上海微创医疗机器人（集团）股份有限公司 | Surgical robot system, adjustment system, and storage medium |
| CN114795495A (en) * | 2022-04-25 | 2022-07-29 | 北京肿瘤医院(北京大学肿瘤医院) | Master-slave operation minimally invasive surgery robot system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116999177A (en) | 2023-11-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106945043B (en) | Multi-arm cooperative control system of master-slave teleoperation surgical robot | |
| Si et al. | A review on manipulation skill acquisition through teleoperation‐based learning from demonstration | |
| JP6106594B2 (en) | Human-machine linkage robot system | |
| JP6382203B2 (en) | Position / force control device, position / force control method and program | |
| Liang et al. | Using dVRK teleoperation to facilitate deep learning of automation tasks for an industrial robot | |
| CN116999177B (en) | Contact force autonomous protection algorithm operated by natural channel endoscope | |
| JP2006212741A (en) | Task skill generating device | |
| CN114571469A (en) | Zero-space real-time obstacle avoidance control method and system for mechanical arm | |
| Gras et al. | Implicit gaze-assisted adaptive motion scaling for highly articulated instrument manipulation | |
| Xia et al. | A constrained optimization approach to virtual fixtures for multi-robot collaborative teleoperation | |
| Sabetian et al. | Self-collision detection and avoidance for dual-arm concentric tube robots | |
| CN116572258B (en) | Dynamic obstacle avoidance control method for welding robot and computer readable storage medium | |
| CN114145848B (en) | Constrained mechanical arm zero-force control system and control method thereof | |
| Chen et al. | A muscle teleoperation system of a robotic rollator based on bilateral shared control | |
| CN114176790B (en) | Clutch control method of master-slave robot | |
| JP2015107138A (en) | Operation control device for insertion instrument | |
| Lee et al. | Toward vision-based high sampling interaction force estimation with master position and orientation for teleoperation | |
| Bian et al. | Interface design of a human-robot interaction system for dual-manipulators teleoperation based on virtual reality | |
| Yang et al. | Optimization-based inverse kinematic analysis of an experimental minimally invasive robotic surgery system | |
| Straižys et al. | Learning robotic cutting from demonstration: Non-holonomic DMPs using the Udwadia-Kalaba method | |
| Seung et al. | Tele-operation master-slave system for minimal invasive brain surgery | |
| CN114274145A (en) | Real-time obstacle avoidance method for multiple mechanical arms in laparoscopic surgery | |
| CN117731404A (en) | Incremental master-slave mapping method of master-slave heterogeneous teleoperation system | |
| Dharmarajan et al. | A trimodal framework for robot-assisted vascular shunt insertion when a supervising surgeon is local, remote, or unavailable | |
| Zhu et al. | The three-dimensional shape control for a soft robot |
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 |