CN116999177B - 一种经自然通道内窥镜操作的接触力自主保护算法 - Google Patents

Abstract

一种经自然通道内窥镜操作的接触力自主保护算法，包括直观主从映射和接触力自主保护，其中接触力自主保护包括以下5个步骤：①坐标变换；②阻断运动增量；③柔顺运动增量；④柔顺不动点；⑤坐标反变换。从第4步得到的取值，从第5步得到、、的取值。通过不断地周期迭代，动态调整、、以及，并将这些取值作为输入变量实时地应用到直观主从映射算法中，从而实现了在操作过程中机械臂接触力的自主保护，将接触力和力矩自主地限制在一个可控范围内。

Description

一种经自然通道内窥镜操作的接触力自主保护算法

技术领域

本发明属于自动化控制技术领域，具体涉及一种经自然通道内窥镜操作的接触力自主保护算法。

背景技术

在传统方法使用主从式经自然通道内窥镜手术机器人进行操作时，因为机械臂和内窥镜上力传感器无法媲美人类手部的力感知能力，获取不到那么丰富的接触力信息，加之主端控制平台与从端手术平台的异构性，导致主端控制平台难以完美复现内窥镜在操作过程中的接触力。如果强行复现接触力，将会影响操作者的操作动作，使操作变得更加困难。通过让操作者感知到操作过程中的接触力，从而形成接触力保护的方法，虽然思路直观，但并不具备可行性。

发明内容

本发明的实施例提供一种经自然通道内窥镜操作的接触力自主保护算法，所述经自然通道内窥镜操作的接触力自主保护算法包括直观主从映射和接触力自主保护，其中接触力自主保护包括以下5个步骤：

①坐标变换；

②阻断运动增量；

③柔顺运动增量；

④柔顺不动点；

⑤坐标反变换。

根据本发明的一种实施方式，例如，所述直观主从映射包括：

将主端操作手柄的x、y、z、a坐标在配准的基础上，映射到从端机械臂末端工具尖端的-z、y、x、-α坐标上；所述配准为：在配准时刻获取手柄配准点位姿和机械臂配准位姿，以便进行后续的直观主从映射解算；为了保证操作的安全性，从端机械臂需要带动内窥镜做rcm运动，即在运动过程中确保内窥镜镜鞘轴线通过空间中一个固定点，rcm运动自带2自由度的约束条件，所以直观主从映射算法只映射4个自由度的坐标；

直观主从映射的算法如下：

β＝atan((z_r-z_reg)/(x_reg-x_r+L_str))

α＝a_reg-a_r

其中，x_r、y_r、z_r、a_r为当前的手柄真实传递位姿；x_reg、y_reg、z_reg、a_reg为手柄配准点位姿；L_str为配准时刻内窥镜镜鞘在不动点前面的长度；β、γ、α、d为中间变量；为配准时刻从端机械臂工具中心点坐标系到不动点坐标系的齐次变换矩阵，/>为/>的逆变换，/>

I₃为三阶单位矩阵，p为一个三阶列向量；

为不动点位置柔顺量，当不启用不动点位置柔顺时，它是一个三阶零向量；

T_f为从端机械臂位姿增量齐次变换矩阵；为机械臂配准位姿，它是配准时刻从端机械臂基座坐标系到工具中心点坐标系的齐次变换矩阵；T_dest为当前的机械臂目标位姿，它是从端机械臂基座坐标系到工具中心点坐标系的齐次变换矩阵。

根据本发明的一种实施方式，例如，所述坐标变换包括：

dx_r＝x_r-x_rh

dy_r＝y_r-y_rh

dz_r＝z_r-z_rh

_tcpregR^tcpdesth＝_tcpregT^tcpdesth(1:3,1:3)

其中，(x_r,y_r,z_r)为手柄当前周期的真实传递位置，(x_rh,y_rh,z_rh)为手柄上一周期的真实传递位置；为配准时刻机械臂基座坐标系到工具中心点坐标系的齐次变换矩阵；T_desth为上一周期的机械臂目标位姿，即T_dest在上一周期的取值；(1:3,1:3)表示一个矩阵的前三行、前三列构成的3×3子矩阵；/>为配准时刻工具中心点坐标系到主端操作手柄坐标系的旋转变换，

dp_v表示当前的工具中心点坐标系下的虚拟位移，用于后续接触力自主保护算法的计算过程。

根据本发明的一种实施方式，例如，阻断运动增量包括：

采用双重阈值力保护，F_lim,M、F_lim,m分别表示外力大阈值、外力小阈值，F_lim,M＞F_lim,m＞0；T_lim,M、T_lim,m分别表示外力矩大阈值、外力矩小阈值，T_lim,M＞T_lim,m＞0；|T_xy|表示向量T_xy的二范数；

当F_z负向超过外力大阈值，并且|T_xy|超过外力矩大阈值，则执行运动阻断流程1；当只有F_z负向超过外力大阈值，则执行运动阻断流程2；当只有|T_xy|超过外力矩大阈值，则执行运动阻断流程3；当外力大阈值、外力矩大阈值都没有触发，则进入小阈值分支：如果F_z负向超过外力小阈值，就执行运动阻断流程4；如果|T_xy|超过外力矩小阈值，就执行运动阻断流程5；如果以上两个条件仍然都没有触发，那么本周期将不会执行任何运动阻断流程，即不改变虚拟位移dp_v；

其中，运动阻断流程1：

dp_v＝P₁e_Fz+P₂e_Txy

其中，e_Fz表示向量的单位化向量，e_Txy表示向量/>的单位化向量；

运动阻断流程1的作用是对于虚拟位移dp_v，只保留其在F_z正方向以及正方向上的分量，阻断其余所有分量；

运动阻断流程2：

dp_v＝P₁e_Fz

运动阻断流程2的作用是对于虚拟位移dp_v，只保留其在F_z正方向上的分量，阻断其余所有分量；

运动阻断流程3：

dp_v＝P₂e_Txy

运动阻断流程3的作用是对于虚拟位移dp_v，只保留其在正方向上的分量，阻断其余所有分量；

运动阻断流程4：

dp_v＝dp_v-P₃(-e_Fz)

运动阻断流程4的作用是对于虚拟位移dp_v，去掉其在F_z负方向上的分量，保留剩余的分量；

运动阻断流程5：

dp_v＝dp_v-P₄(-e_Txy)

运动阻断流程5的作用是对于虚拟位移dp_v，去掉其在负方向上的分量，保留剩余的分量；

阻断运动增量的过程实际上就是在根据机械臂末端法兰坐标系下的外力反馈，来改变当前的工具中心点坐标系下的虚拟位移dp_v，使其只保留部分分量、去掉部分分量或者保持不变。

根据本发明的一种实施方式，例如，柔顺运动增量包括：

dp_v＝dp_v+P₅e_Fz

dp_v＝dp_v+P₆e_Txy

其中，|F_z|表示向量F_z的二范数，|T_xy|表示向量T_xy的二范数；K_F为与外力相关的刚度系数，K_T为与外力矩相关的刚度系数，K_F＞0，K_T＞0；

柔顺运动增量的过程实际上就是根据机械臂末端法兰坐标系下的外力反馈，以及控制系统的刚度系数，在当前的工具中心点坐标系下的虚拟位移dp_v上增加计算出的柔顺量，用来形成一种使操作的接触力自发减小的机制。

根据本发明的一种实施方式，例如，柔顺不动点包括：

Δrcm＝Δrcm_h+P₇e_Fxy

其中，|F_xy|表示向量F_xy的二范数，e_Fxy表示向量F_xy的单位化向量；Δrcm_h为上一周期的不动点位置柔顺量，Δrcm为当前周期的不动点位置柔顺量，初始周期的不动点位置柔顺量为一个三阶零向量；

柔顺不动点的过程是为Δrcm设计了一种有利于减小操作的接触力的取值策略，使直观主从映射中的不动点位置柔顺环节得以启用，并能够取得正向效果。

根据本发明的一种实施方式，例如，坐标反变换包括：

x_r＝dp_r,x+x_rh

y_r＝dp_r,y+y_rh

z_r＝dp_r,z+z_rh

其中，dp_r表示手柄当前周期真实传递位置经过接触力保护算法之后得到的位置向量，dp_r,x、dp_r,y、dp_r,z分别表示dp_r的x、y、z分量。

根据本发明的一种实施方式，例如，从第4步柔顺不动点得到Δrcm的取值，从第5步坐标反变换得到x_r、y_r、z_r的取值；通过不断地周期迭代，动态调整x_r、y_r、z_r以及Δrcm，并将这些取值作为输入变量实时地应用到直观主从映射中。

附图说明

图1为主从式经自然通道内窥镜手术机器人结构示意图。

图2为主端操作手柄和从端机械臂的坐标系方向示意图。

图3为阻断运动增量的逻辑流程图。

图4为本发明实施例的实验过程中采集的机械臂末端法兰坐标系下的外力图。

图5为本发明实施例的实验过程中采集的机械臂末端法兰坐标系下的外力矩图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。但本领域技术人员知晓，本发明并不局限于附图和以下实施例。

图1所示为主从式经自然通道内窥镜手术机器人结构示意图，主从式经自然通道内窥镜手术机器人包括主端操作手柄1，从端手术平台2，机械臂3，内窥镜4。在使用这种主从异构式经自然通道内窥镜手术机器人进行操作时，要求能够通过主端操作手柄1，稳定、直观地控制从端机械臂2的运动，并且满足安全性和精准性，从而有效地进行切割、凝血操作。但已有的主从异构式经自然通道内窥镜手术机器人并不能完全满足实际操作的安全性和精确性要求。

本发明的实施例提供了一种经自然通道内窥镜操作的接触力自主保护算法。利用该算法，从端手术平台根据机械臂的力感知数据，不需要经过人的判断，自主地做出接触力保护决策。从端手术平台负责剔除操作动作中将会引起接触力过大的危险分量，并适度加入柔顺动作，以减小接触力中已经偏大的分量。

本发明实施例提供的经自然通道内窥镜操作的接触力自主保护算法，是在直观主从映射的基础上，根据机械臂感知到的法兰六维力，自主改变主从映射的输入量，从而改变机械臂的运动，达到接触力保护的目的。所述经自然通道内窥镜操作的接触力自主保护算法包括直观主从映射和接触力自主保护，其中接触力自主保护包括以下5个步骤：

①坐标变换；

②阻断运动增量；

③柔顺运动增量；

④柔顺不动点；

⑤坐标反变换。

下面分别阐述。

先阐述直观主从映射。图2为主端操作手柄和从端机械臂的坐标系方向示意图。直观主从映射算法将主端操作手柄的x、y、z、a坐标在配准的基础上，映射到从端机械臂末端工具尖端的-z、y、x、-α坐标上。所述配准为：在配准时刻获取手柄配准点位姿和机械臂配准位姿，以便进行后续的直观主从映射解算。直观主从映射坐标的顺序、符号不同，是因为在符合主端操作手柄和从端机械臂各自的坐标系定义基础上，需要实现最为直观的操作体验。为了保证操作的安全性，从端机械臂需要带动内窥镜做rcm运动，即在运动过程中确保内窥镜镜鞘轴线通过空间中一个固定点(不动点)，rcm运动自带2自由度的约束条件，所以直观主从映射算法只映射4个自由度的坐标。

直观主从映射的算法如下：

β＝atan((z_r-z_reg)/(x_reg-x_r+L_str))

α＝a_reg-a_r

其中，x_r、y_r、z_r、a_r为当前的手柄真实传递位姿；x_reg、y_reg、z_reg、a_reg为手柄配准点位姿；L_str为配准时刻内窥镜镜鞘在不动点前面的长度；β、γ、α、d为中间变量；为配准时刻从端机械臂工具中心点坐标系到不动点坐标系的齐次变换矩阵，/>为/>的逆变换，/>

I₃为三阶单位矩阵，p为一个三阶列向量；

为不动点位置柔顺量，当不启用不动点位置柔顺时，它是一个三阶零向量；

T_f为从端机械臂位姿增量齐次变换矩阵；为机械臂配准位姿，它是配准时刻从端机械臂基座坐标系到工具中心点坐标系的齐次变换矩阵；T_dest为当前的机械臂目标位姿，它是从端机械臂基座坐标系到工具中心点坐标系的齐次变换矩阵。

在上述直观主从映射基础上，开展接触力自主保护。本发明实施例提供的接触力自主保护，其本质是通过在操作过程中实时测量的机械臂末端法兰坐标系下的外力F_x、F_y、F_z，外力矩T_x、T_y(本算法中未使用T_z)，动态调整直观主从映射算法的输入变量x_r、y_r、z_r以及Δrcm(本算法中没有调整a_r)，从而将操作过程中的接触力及力矩自主地限制在一个可控范围内。

1.坐标变换包括：

dx_r＝x_r-x_rh

dy_r＝y_r-y_rh

dz_r＝z_r-z_rh

_tcpregR^tcpdesth＝_tcpregT^tcpdesth(1:3,1:3)

其中，(x_r,y_r,z_r)为手柄当前周期的真实传递位置，(x_rh,y_rh,z_rh)为手柄上一周期的真实传递位置；为配准时刻机械臂基座坐标系到工具中心点坐标系的齐次变换矩阵；T_desth为上一周期的机械臂目标位姿，即T_dest在上一周期的取值；(1:3,1:3)表示一个矩阵的前三行、前三列构成的3×3子矩阵；/>为配准时刻工具中心点坐标系到主端操作手柄坐标系的旋转变换，

dp_v表示当前的工具中心点坐标系下的虚拟位移，用于后续接触力自主保护算法的计算过程。

2.阻断运动增量：

阻断运动增量的逻辑流程图如图3所示。

采用双重阈值力保护，F_lim,M、F_lim,m分别表示外力大阈值、外力小阈值，F_lim,M＞F_lim,m＞0；T_lim,M、T_lim,m分别表示外力矩大阈值、外力矩小阈值，T_lim,M＞T_lim,m＞0。|T_xy|表示向量T_xy的二范数。

当F_z负向超过外力大阈值，并且|T_xy|超过外力矩大阈值，将执行运动阻断流程1；当只有F_z负向超过外力大阈值，将执行运动阻断流程2；当只有|T_xy|超过外力矩大阈值，将执行运动阻断流程3；当外力大阈值、外力矩大阈值都没有触发，将会进入小阈值分支：如果F_z负向超过外力小阈值，就执行运动阻断流程4；如果|T_xy|超过外力矩小阈值，就执行运动阻断流程5；如果以上两个条件仍然都没有触发，那么本周期将不会执行任何运动阻断流程，即不会改变虚拟位移dp_v。

运动阻断流程1：

dp_v＝P₁e_Fz+P₂e_Txy

其中，e_Fz表示向量的单位化向量，e_Txy表示向量/>的单位化向量。

运动阻断流程1的作用是对于虚拟位移dp_v，只保留其在F_z正方向以及正方向上的分量，阻断其余所有分量。

运动阻断流程2：

dp_v＝P₁e_Fz

运动阻断流程2的作用是对于虚拟位移dp_v，只保留其在F_z正方向上的分量，阻断其余所有分量。

运动阻断流程3：

dp_v＝P₂e_Txy

运动阻断流程3的作用是对于虚拟位移dp_v，只保留其在正方向上的分量，阻断其余所有分量。

运动阻断流程4：

dp_v＝dp_v-P₃(-e_Fz)

运动阻断流程4的作用是对于虚拟位移dp_v，去掉其在F_z负方向上的分量，保留剩余的分量。

运动阻断流程5：

dp_v＝dp_v-P₄(-e_Txy)

运动阻断流程5的作用是对于虚拟位移dp_v，去掉其在负方向上的分量，保留剩余的分量。

阻断运动增量的过程实际上就是在根据机械臂末端法兰坐标系下的外力反馈，来改变当前的工具中心点坐标系下的虚拟位移dp_v，使其只保留部分分量、去掉部分分量或者保持不变。

3.柔顺运动增量：

dp_v＝dp_v+P₅e_Fz

dp_v＝dp_v+P₆e_Txy

其中，|F_z|表示向量F_z的二范数，|T_xy|表示向量T_xy的二范数；K_F为与外力相关的刚度系数，K_T为与外力矩相关的刚度系数，K_F＞0，K_T＞0。

柔顺运动增量的过程实际上就是根据机械臂末端法兰坐标系下的外力反馈，以及控制系统的刚度系数，在当前的工具中心点坐标系下的虚拟位移dp_v上增加计算出的柔顺量，用来形成一种使操作的接触力自发减小的机制。

4.柔顺不动点：

Δrcm＝Δrcm_h+P₇e_Fxy

其中，|F_xy|表示向量F_xy的二范数，e_Fxy表示向量F_xy的单位化向量；Δrcm_h为上一周期的不动点位置柔顺量，Δrcm为当前周期的不动点位置柔顺量，初始周期的不动点位置柔顺量为一个三阶零向量。

柔顺不动点的过程是为Δrcm设计了一种有利于减小操作的接触力的取值策略，使直观主从映射中的不动点位置柔顺环节得以启用，并能够取得正向效果。

5.坐标反变换：

x_r＝dp_r,x+x_rh

y_r＝dp_r,y+y_rh

z_r＝dp_r,z+z_rh

其中，dp_r表示手柄当前周期真实传递位置经过接触力保护算法之后得到的位置向量，dp_r,x、dp_r,y、dp_r,z分别表示dp_r的x、y、z分量。

本发明实施例描述的一种经自然通道内窥镜操作的接触力自主保护算法，从其流程的第4步得到Δrcm的取值，从其流程的第5步得到x_r、y_r、z_r的取值。通过不断地周期迭代，动态调整x_r、y_r、z_r以及Δrcm，并将这些取值作为输入变量实时地应用到直观主从映射算法中，从而实现了在操作过程中机械臂接触力的自主保护，将接触力和力矩自主地限制在一个可控范围内。

如果在经自然通道内窥镜机器人操作过程中，没有接触力保护机制，那么内窥镜与被操作者组织之间的接触力和力矩将是不受控的，容易造成被操作者受伤和内窥镜损坏，造成难以挽回的损失。

图4和图5分别是某次实验过程中采集的机械臂末端法兰坐标系下的外力和外力矩。此次实验中采用了本发明提出的接触力自主保护算法，外力阈值F_lim,M、F_lim,m分别取25牛、15牛，外力矩阈值T_lim,M、T_lim,m分别取10牛米、6牛米。从图4、图5的数据统计得出，在实验全过程中，接触外力和外力矩被限制在大阈值和小阈值的平均值之内的概率分别为97.897％和97.923％，接触外力和外力矩被限制在大阈值之内的概率分别为99.658％和99.527％。

Claims (2)

1.一种经自然通道内窥镜操作的接触力自主保护算法，其特征在于，所述经自然通道内窥镜操作的接触力自主保护算法包括直观主从映射和接触力自主保护，其中接触力自主保护包括以下5个步骤：

①坐标变换；

②阻断运动增量；

③柔顺运动增量；

④柔顺不动点；

⑤坐标反变换；

所述直观主从映射包括：

将主端操作手柄的x、y、z、a坐标在配准的基础上，映射到从端机械臂末端工具尖端的-z、y、x、-α坐标上；所述配准为：在配准时刻获取手柄配准点位姿和机械臂配准位姿，以便进行后续的直观主从映射解算；为了保证操作的安全性，从端机械臂需要带动内窥镜做rcm运动，即在运动过程中确保内窥镜镜鞘轴线通过空间中一个固定点，rcm运动自带2自由度的约束条件，所以直观主从映射算法只映射4个自由度的坐标；

直观主从映射的算法如下：

β＝atan((z_r-z_reg)/(x_reg-x_r+L_str))

α＝a_reg-a_r

其中，x_r、y_r、z_r、a_r为当前的手柄真实传递位姿；x_reg、y_reg、z_reg、a_reg为手柄配准点位姿；L_str为配准时刻内窥镜镜鞘在不动点前面的长度；β、γ、α、d为中间变量；为配准时刻从端机械臂工具中心点坐标系到不动点坐标系的齐次变换矩阵，/>为/>的逆变换，

I₃为三阶单位矩阵，p为一个三阶列向量；

为不动点位置柔顺量，当不启用不动点位置柔顺时，它是一个三阶零向量；

T_f为从端机械臂位姿增量齐次变换矩阵；为机械臂配准位姿，它是配准时刻从端机械臂基座坐标系到工具中心点坐标系的齐次变换矩阵；T_dest为当前的机械臂目标位姿，它是从端机械臂基座坐标系到工具中心点坐标系的齐次变换矩阵；

所述坐标变换包括：

dx_r＝x_r-x_rh

dy_r＝y_r-y_rh

dz_r＝z_r-z_rh

_tcpregR^tcpdesth＝_tcpregT^tcpdesth(1:3,1:3)

其中，(x_r,y_r,z_r)为手柄当前周期的真实传递位置，(x_rh,y_rh,z_rh)为手柄上一周期的真实传递位置；为配准时刻机械臂基座坐标系到工具中心点坐标系的齐次变换矩阵；T_desth为上一周期的机械臂目标位姿，即T_dest在上一周期的取值；(1:3,1:3)表示一个矩阵的前三行、前三列构成的3×3子矩阵；/>为配准时刻工具中心点坐标系到主端操作手柄坐标系的旋转变换，

dp_v表示当前的工具中心点坐标系下的虚拟位移，用于后续接触力自主保护算法的计算过程；

阻断运动增量包括：

采用双重阈值力保护，F_lim,M、F_lim,m分别表示外力大阈值、外力小阈值，F_lim,M＞F_lim,m＞0；T_lim,M、T_lim,m分别表示外力矩大阈值、外力矩小阈值，T_lim,M＞T_lim,m＞0；|T_xy|表示向量T_xy的二范数；

当F_z负向超过外力大阈值，并且|T_xy|超过外力矩大阈值，则执行运动阻断流程1；当只有F_z负向超过外力大阈值，则执行运动阻断流程2；当只有|T_xy|超过外力矩大阈值，则执行运动阻断流程3；当外力大阈值、外力矩大阈值都没有触发，则进入小阈值分支：如果F_z负向超过外力小阈值，就执行运动阻断流程4；如果|T_xy|超过外力矩小阈值，就执行运动阻断流程5；如果以上两个条件仍然都没有触发，那么本周期将不会执行任何运动阻断流程，即不改变虚拟位移dp_v；

其中，运动阻断流程1：

dp_v＝P₁e_Fz+P₂e_Txy

其中，e_Fz表示向量的单位化向量，e_Txy表示向量/>的单位化向量；

运动阻断流程1的作用是对于虚拟位移dp_v，只保留其在F_z正方向以及正方向上的分量，阻断其余所有分量；

运动阻断流程2：

dp_v＝P₁e_Fz

运动阻断流程2的作用是对于虚拟位移dp_v，只保留其在F_z正方向上的分量，阻断其余所有分量；

运动阻断流程3：

dp_v＝P₂e_Txy

运动阻断流程3的作用是对于虚拟位移dp_v，只保留其在正方向上的分量，阻断其余所有分量；

运动阻断流程4：

dp_v＝dp_v-P₃(-e_Fz)

运动阻断流程4的作用是对于虚拟位移dp_v，去掉其在F_z负方向上的分量，保留剩余的分量；

运动阻断流程5：

dp_v＝dp_v-P₄(-e_Txy)

运动阻断流程5的作用是对于虚拟位移dp_v，去掉其在负方向上的分量，保留剩余的分量；

阻断运动增量的过程实际上就是在根据机械臂末端法兰坐标系下的外力反馈，来改变当前的工具中心点坐标系下的虚拟位移dp_v，使其只保留部分分量、去掉部分分量或者保持不变；

柔顺运动增量包括：

dp_v＝dp_v+P₅e_Fz

dp_v＝dp_v+P₆e_Txy

其中，|F_z|表示向量F_z的二范数，|T_xy|表示向量T_xy的二范数；K_F为与外力相关的刚度系数，K_T为与外力矩相关的刚度系数，K_F＞0，K_T＞0；

柔顺运动增量的过程实际上就是根据机械臂末端法兰坐标系下的外力反馈，以及控制系统的刚度系数，在当前的工具中心点坐标系下的虚拟位移dp_v上增加计算出的柔顺量，用来形成一种使操作的接触力自发减小的机制；

柔顺不动点包括：

Δrcm＝Δrcm_h+P₇e_Fxy

其中，|F_xy|表示向量F_xy的二范数，e_Fxy表示向量F_xy的单位化向量；Δrcm_h为上一周期的不动点位置柔顺量，Δrcm为当前周期的不动点位置柔顺量，初始周期的不动点位置柔顺量为一个三阶零向量；

柔顺不动点的过程是为Δrcm设计了一种有利于减小操作的接触力的取值策略，使直观主从映射中的不动点位置柔顺环节得以启用，并能够取得正向效果；

坐标反变换包括：

x_r＝dp_r,x+x_rh

y_r＝dp_r,y+y_rh

z_r＝dp_r,z+z_rh

其中，dp_r表示手柄当前周期真实传递位置经过接触力保护算法之后得到的位置向量，dp_r,x、dp_r,y、dp_r,z分别表示dp_r的x、y、z分量。

2.根据权利要求1所述的经自然通道内窥镜操作的接触力自主保护算法，其特征在于，从第4步柔顺不动点得到Δrcm的取值，从第5步坐标反变换得到x_r、y_r、z_r的取值；通过不断地周期迭代，动态调整x_r、y_r、z_r以及Δrcm，并将这些取值作为输入变量实时地应用到直观主从映射中。