CN116999178A - 一种经自然通道内窥镜操作的双频滤波直观主从映射方法 - Google Patents

Abstract

一种经自然通道内窥镜操作的双频滤波直观主从映射方法，包括：操作者通过主端操作手柄产生手柄原始位姿，通过双频滤波算法滤除原始位姿中的高频噪声以及手部不受控的颤抖和漂移，生成手柄网络位姿；手柄网络位姿通过网络通信传给从端机械臂控制器，通过限速、限接触力、限伸缩量算法对手柄网络位姿做修正，通过直观主从映射算法得到机械臂目标位姿；对机械臂目标位姿实现比例运算，得到机械臂待执行位姿；对机械臂待执行位姿判定是否符合限位条件，不符合条件则拒绝执行，符合条件则最终输出为机械臂执行位姿。

Description

一种经自然通道内窥镜操作的双频滤波直观主从映射方法

技术领域

本发明属于自动化控制技术领域，具体涉及经自然通道内窥镜手术机器人的自动化控制，更具体涉及一种经自然通道内窥镜操作的双频滤波直观主从映射方法。

背景技术

已有的主从异构式经自然通道内窥镜手术机器人一般包括两个部分：（1）主端操作手柄，（2）从端机械臂。在使用主从异构式经自然通道内窥镜手术机器人进行手术操作时，必须能够通过主端操作手柄，稳定、直观地控制从端机械臂的运动，并且满足安全性和精准性，最终保证有效地进行切割、凝血操作。但当前已有的控制技术不足以满足外科手术操作的安全性和精确性。

发明内容

本发明的实施例提供一种经自然通道内窥镜操作的双频滤波直观主从映射方法，包括：

操作者通过主端操作手柄产生手柄原始位姿，通过双频滤波算法滤除原始位姿中的高频噪声以及手部不受控的颤抖和漂移，生成手柄网络位姿；手柄网络位姿通过网络通信传给从端机械臂控制器，通过限速、限接触力、限伸缩量算法对手柄网络位姿做修正，通过直观主从映射算法得到机械臂目标位姿；对机械臂目标位姿实现比例运算，得到机械臂待执行位姿；对机械臂待执行位姿判定是否符合限位条件，不符合条件则拒绝执行，符合条件则最终输出为机械臂执行位姿。

根据本发明的一种实施方式，例如，所述双频滤波算法包括高频降噪滤波和低频防抖滤波；

所述高频降噪滤波包括：对主端操作手柄的原始高频采样数据做卡尔曼滤波，滤除数据中的高频噪声；对于主端操作手柄的每一个自由度，卡尔曼滤波算法如下：

其中，表示主端操作手柄真实的高频采样数据；/>表示手柄数据去噪声后的最大似然估计状态值，即卡尔曼滤波器的输出值；/>表示估计值的均方误差的最大似然估计；表示状态噪声协方差的估计值，为一个非负常数；/>表示采样噪声协方差的估计值，为一个正常数；/>、/>、/>所携带的下角标整数表示对应数据的周期序号；

经过卡尔曼滤波之后，对输出的数据进行低频采样，采样频率为从端机械臂的运动控制频率。

根据本发明的一种实施方式，例如，低频防抖滤波包括：

对低频采样得到的数据进行基于意图判别的防抖滤波，滤除操作者手部的颤抖和漂移；对于主端操作手柄的每一个自由度，基于意图判别的防抖滤波算法如下：

其中，表示低频采样得到的手柄数据；/>表示比例预处理后得到的数据，/>表示比例预处理的比例系数，/>；比例预处理使得输出/>比输入/>产生小幅的滞后，这样做可以使信号中的尖峰变得平滑；/>表示基于意图判别的防抖滤波器的输出值；/>表示意图判别函数，用来判定操作者的意图是想要稳定位置，还是想要良好的跟踪性能，其中/>为判别窗口宽度，/>为判别阶数；/>、/>、/>所携带的下角标整数表示对应数据的周期序号；

意图判别函数用公式表示如下：

。

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

修正后得到手柄真实传递位姿；在踩下运动使能踏板后的第一个周期进行配准，记录下此时的手柄真实传递位姿作为手柄配准点位姿，记录下此时的机械臂当前位姿作为机械臂配准位姿；在之后的每一个周期，手柄真实传递位姿相对于手柄配准点位姿的增量为手柄位姿增量，手柄位姿增量在运动映射规则下变换为机械臂位姿增量；在机械臂配准位姿基础上，做机械臂位姿增量的变换，得到该周期的机械臂目标位姿。

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

将主端操作手柄的、/>、/>、/>坐标在配准的基础上，映射到从端机械臂末端工具尖端的/>、/>、/>、/>坐标上；所述配准为：在配准时刻获取手柄配准点位姿和机械臂配准位姿，以进行后续的直观主从映射解算；直观主从映射坐标的顺序、符号不同；从端机械臂带动内窥镜做rcm运动，即在运动过程中确保内窥镜镜鞘轴线通过空间中一个固定点（不动点），rcm运动自带2自由度的约束条件，所以直观主从映射算法只映射4个自由度的坐标。

根据本发明的一种实施方式，例如，直观主从映射算法如下：

其中，、/>、/>、/>为当前的手柄真实传递位姿；/>、/>、/>、/>为手柄配准点位姿；/>为配准时刻内窥镜镜鞘在不动点前面的长度；/>、/>、/>、/>为中间变量；/>为配准时刻从端机械臂末端工具中心点坐标系到不动点坐标系的齐次变换矩阵，/>为的逆变换，/>；

；

；

，/>为三阶单位矩阵，/>为一个三阶列向量；

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

；

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

附图说明

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

图2为本发明实施例提出的一种经自然通道内窥镜操作的双频滤波直观主从映射方法流程示意图。

图3为本发明实施例中意图判别函数的示例性图示。

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

图5是直观主从映射算法的运动控制效果示意图；其中箭头左侧图为主端手柄控制动作，箭头右侧图为从端机械臂操作动作。

具体实施方式

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

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

基于上述需求，本发明的实施例提出一种经自然通道内窥镜操作的双频滤波直观主从映射方法，所述方法的流程框图如图2所示。

所述方法包括：操作者通过主端操作手柄产生手柄原始位姿，通过双频滤波算法（高频降噪滤波和低频防抖滤波）滤除原始位姿中的高频噪声以及手部不受控的颤抖和漂移，生成手柄网络位姿；手柄网络位姿通过网络通信传给从端机械臂控制器，通过限速、限接触力、限伸缩量（内窥镜镜鞘***人体内的长度）算法对手柄网络位姿做必要的修正，大幅提高机器人手术主从操作的安全性，通过直观主从映射算法得到机械臂目标位姿（修正后得到手柄真实传递位姿；在踩下运动使能踏板后的第一个周期进行配准，记录下此时的手柄真实传递位姿作为手柄配准点位姿，记录下此时的机械臂当前位姿作为机械臂配准位姿；在之后的每一个周期，手柄真实传递位姿相对于手柄配准点位姿的增量为手柄位姿增量，手柄位姿增量在运动映射规则下变换为机械臂位姿增量；在机械臂配准位姿基础上，做机械臂位姿增量的变换，得到该周期的机械臂目标位姿）；对机械臂目标位姿实现比例运算，得到机械臂待执行位姿；对机械臂待执行位姿判定是否符合限位条件，不符合条件则拒绝执行，符合条件则最终输出为机械臂执行位姿。

在使用经自然通道内窥镜手术机器人进行手术操作时，要解决的两个关键问题分别是：（1）如何让手术操作更加平稳，以及（2）如何让手术操作更加直观。为此，本发明的实施例分别研发了两项关键技术：双频滤波算法、直观主从映射算法。

1.双频滤波算法

本发明实施例的双频滤波算法同时采用高频降噪滤波和低频防抖滤波。高频降噪滤波用来过滤主端操作手柄原始数据中的高频噪声，否则手柄基于自身位置的力反馈（用来实现阻尼力和回弹力）将会把未过滤的高频噪声反馈出来，影响手术操作；低频防抖滤波用来过滤操作者手部的无意识抖动和漂移，但是不能影响正常操作动作的跟踪性能，采用了意图判别技术，实现了过滤和放行的自动调节。双频滤波算法实现了手术操作手感优秀、定位稳定、动作灵活的统一。

1）高频降噪滤波

对主端操作手柄的原始高频采样数据做卡尔曼滤波，滤除数据中的高频噪声。对于主端操作手柄的每一个自由度，卡尔曼滤波算法如下：

其中，表示主端操作手柄真实的高频采样数据；/>表示手柄数据去噪声后的最大似然估计状态值，即卡尔曼滤波器的输出值；/>表示估计值的均方误差的最大似然估计；表示状态噪声协方差的估计值，为一个非负常数；/>表示采样噪声协方差的估计值，为一个正常数；/>、/>、/>所携带的下角标整数表示对应数据的周期序号。

上述算法实现了一种迭代过程，由上一周期的数据可以快速计算出当前周期的数据。经过卡尔曼滤波之后，对输出的数据进行低频采样，采样频率为从端机械臂的运动控制频率。

2）低频防抖滤波

对低频采样得到的数据进行基于意图判别的防抖滤波，滤除操作者手部的颤抖和漂移。对于主端操作手柄的每一个自由度，基于意图判别的防抖滤波算法如下：

其中，表示低频采样得到的手柄数据；/>表示比例预处理后得到的数据，/>表示比例预处理的比例系数，/>；比例预处理使得输出/>比输入/>产生小幅的滞后，这样做可以使信号中的尖峰变得平滑；/>表示基于意图判别的防抖滤波器的输出值；/>表示意图判别函数，用来判定操作者的意图是想要稳定位置，还是想要良好的跟踪性能，其中/>为判别窗口宽度，/>为判别阶数；/>、/>、/>所携带的下角标整数表示对应数据的周期序号。

上述算法实现了一种迭代过程，由上一周期的数据可以快速计算出当前周期的数据。

意图判别函数是防抖滤波器的核心算法，用公式表示如下：

比如，的图像如图3所示。

从意图判别函数的示例性图像可以直观地看出，当输入值的绝对值比较小时，输出值的绝对值会被进一步大幅减小；当输入值的绝对值比较大时，输出值等于输入值；输入值的绝对值的“小”与“大”之间有平滑的过渡带。当定义输入值为主端操作手柄某自由度的数据增量时，输入值的绝对值小即对应着该自由度上几乎没有操作者主观故意的运动，以无意识的抖动和漂移为主；输入值的绝对值大即对应着该自由度上以操作者主观故意的运动为主，抖动和漂移只占可以忽略的很小一部分。意图判别函数将会自动在抖动和漂移占主导时抑制抖动和漂移，在操作者主观故意的运动占主导时全透明地放行运动通过。

2.直观主从映射算法

在主从异构式经自然通道内窥镜机器人手术操作过程中，因为主端操作手柄和从端机械臂在机械结构上是异构的，所以需要设计一种映射方法，将主端手柄控制动作和从端机械臂操作动作联系起来，并且要求操作者可以非常直观地做出决策和控制动作，使手术操作过程尽可能简单。

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

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

其中，、/>、/>、/>为当前的手柄真实传递位姿；/>、/>、/>、/>为手柄配准点位姿；/>为配准时刻内窥镜镜鞘在不动点前面的长度；/>、/>、/>、/>为中间变量；/>为配准时刻从端机械臂末端工具中心点坐标系到不动点坐标系的齐次变换矩阵，/>为的逆变换，/>；

；

；

，/>为三阶单位矩阵，/>为一个三阶列向量；

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

；

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

直观主从映射算法的运动控制效果如图5所示。

本发明主要解决主从异构式经自然通道内窥镜手术机器人在手术操作过程中的平稳性和直观性问题。通过双频滤波算法实现平稳操作，高频降噪滤波可以滤除主端操作手柄原始采样数据中的高频噪声，防止对操作者造成干扰；低频防抖滤波滤除操作者手部无意识的抖动和漂移，这是此方法提升操作平稳性的关键因素，操作者手部抖动和漂移是无法避免的，因为主从操作的对象是内窥镜，即术中视频反馈的来源，如不能滤除手部抖动和漂移，将造成视频反馈的抖动和漂移，而术中定位完全依靠视频反馈，这将造成更加难以术中定位的情况，形成不利因素的叠加，利用低频防抖滤波对主端操作手柄数据进行滤波后，即可实现极低速动作被阻断、正常动作透明输出的效果，也就是过滤了手部无意识的抖动和漂移。

本发明通过直观主从映射算法实现主从操作的直观性。如图5所示，直观主从映射算法使得主端操作手柄运动的前移、后移、上移、下移、左移、右移、左转、右转8个方向分别与机械臂抓持的内窥镜的镜鞘末端运动的前移、后移、上移、下移、左移、右移、左转、右转一一对应，并且各方向运动之间没有任何耦合。需要指出的是，主端操作手柄运动对应的是内窥镜的镜鞘末端运动，内窥镜在运动的过程中还需要满足不动点约束，即内窥镜做rcm运动，所以内窥镜整体在运动过程中看起来与主端操作手柄的运动并不是对应的。因为视频反馈是从内窥镜镜鞘中的镜头返回的，即视频反馈看到的是内窥镜镜鞘末端开口所对应的场景，所以视频反馈中看到的场景运动与镜鞘末端运动是严格对应的，视屏反馈中的场景运动与主端操作手柄的运动也是严格对应的。手术操作的过程是通过观看视频反馈中的场景运动，控制主端操作手柄的运动，从而实现手术动作；因为两者的严格对应性，从而实现了主从手术操作的直观性。

Claims (6)

1.一种经自然通道内窥镜操作的双频滤波直观主从映射方法，其特征在于，所述方法包括：

操作者通过主端操作手柄产生手柄原始位姿，通过双频滤波算法滤除原始位姿中的高频噪声以及手部不受控的颤抖和漂移，生成手柄网络位姿；手柄网络位姿通过网络通信传给从端机械臂控制器，通过限速、限接触力、限伸缩量算法对手柄网络位姿做修正，通过直观主从映射算法得到机械臂目标位姿；对机械臂目标位姿实现比例运算，得到机械臂待执行位姿；对机械臂待执行位姿判定是否符合限位条件，不符合条件则拒绝执行，符合条件则最终输出为机械臂执行位姿。

2.根据权利要求1所述的经自然通道内窥镜操作的双频滤波直观主从映射方法，其特征在于，所述双频滤波算法包括高频降噪滤波和低频防抖滤波；

所述高频降噪滤波包括：对主端操作手柄的原始高频采样数据做卡尔曼滤波，滤除数据中的高频噪声；对于主端操作手柄的每一个自由度，卡尔曼滤波算法如下：

其中，表示主端操作手柄真实的高频采样数据；/>表示手柄数据去噪声后的最大似然估计状态值，即卡尔曼滤波器的输出值；/>表示估计值的均方误差的最大似然估计；/>表示状态噪声协方差的估计值，为一个非负常数；/>表示采样噪声协方差的估计值，为一个正常数；/>、/>、/>所携带的下角标整数表示对应数据的周期序号；

经过卡尔曼滤波之后，对输出的数据进行低频采样，采样频率为从端机械臂的运动控制频率。

3.根据权利要求2所述的经自然通道内窥镜操作的双频滤波直观主从映射方法，其特征在于，低频防抖滤波包括：

对低频采样得到的数据进行基于意图判别的防抖滤波，滤除操作者手部的颤抖和漂移；对于主端操作手柄的每一个自由度，基于意图判别的防抖滤波算法如下：

其中，表示低频采样得到的手柄数据；/>表示比例预处理后得到的数据，/>表示比例预处理的比例系数，/>；比例预处理使得输出/>比输入/>产生小幅的滞后，这样做可以使信号中的尖峰变得平滑；/>表示基于意图判别的防抖滤波器的输出值；/>表示意图判别函数，用来判定操作者的意图是想要稳定位置，还是想要良好的跟踪性能，其中/>为判别窗口宽度，/>为判别阶数；/>、/>、/>所携带的下角标整数表示对应数据的周期序号；

意图判别函数用公式表示如下：

。

4.根据权利要求1所述的经自然通道内窥镜操作的双频滤波直观主从映射方法，其特征在于，所述直观主从映射算法包括：

修正后得到手柄真实传递位姿；在踩下运动使能踏板后的第一个周期进行配准，记录下此时的手柄真实传递位姿作为手柄配准点位姿，记录下此时的机械臂当前位姿作为机械臂配准位姿；在之后的每一个周期，手柄真实传递位姿相对于手柄配准点位姿的增量为手柄位姿增量，手柄位姿增量在运动映射规则下变换为机械臂位姿增量；在机械臂配准位姿基础上，做机械臂位姿增量的变换，得到该周期的机械臂目标位姿。

5.根据权利要求4所述的经自然通道内窥镜操作的双频滤波直观主从映射方法，其特征在于，直观主从映射算法包括：

将主端操作手柄的、/>、/>、/>坐标在配准的基础上，映射到从端机械臂末端工具尖端的/>、/>、/>、/>坐标上；所述配准为：在配准时刻获取手柄配准点位姿和机械臂配准位姿，以进行后续的直观主从映射解算；直观主从映射坐标的顺序、符号不同；从端机械臂带动内窥镜做rcm运动，即在运动过程中确保内窥镜镜鞘轴线通过空间中一个固定点，rcm运动自带2自由度的约束条件，所以直观主从映射算法只映射4个自由度的坐标。

6.根据权利要求5所述的经自然通道内窥镜操作的双频滤波直观主从映射方法，其特征在于，直观主从映射算法如下：

其中，、/>、/>、/>为当前的手柄真实传递位姿；/>、/>、/>、/>为手柄配准点位姿；/>为配准时刻内窥镜镜鞘在不动点前面的长度；/>、/>、/>、/>为中间变量；/>为配准时刻从端机械臂末端工具中心点坐标系到不动点坐标系的齐次变换矩阵，/>为/>的逆变换，/>；

；

；

，/>为三阶单位矩阵，/>为一个三阶列向量；

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

；

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