CN104783798B - 用于感知医用软体机械臂形状的系统及方法 - Google Patents

Abstract

本发明提供了一种用于感知医用软体机械臂形状的系统及方法，系统包括激光器、光谱仪、软体机械臂、传感器、计算机；其中，所述激光器发射激光信号作为入射信号进入安装于软体机械臂的传感器形成反射信号，光谱仪将接收到的所述反射信号传递给计算机，计算机根据所述反射信号处理得到软体机械臂的形状信息。所述方法包括根据软体机械臂的形变量计算各个光栅的布拉格波长；根据各个光栅的布拉格波长，计算出各个光栅的曲率和挠率；根据各个光栅的曲率和挠率，计算软体机械臂的实时形状。本发明对比度高、实时性佳，同时解决了传统内部传感器体积过大以及内部电路对患者的安全隐患问题。

Description

用于感知医用软体机械臂形状的系统及方法

技术领域

本发明涉及光纤传感与机器人领域，具体地，涉及一种用于感知医用软体机械臂形状的系统及方法，尤其涉及一种能在密闭环境中，感知软体机械臂形状的系统和方法。

背景技术

软体机器人区别于传统的刚性机器人，它由具有柔性的材料(如硅胶、镍钛合金)等制成，有着良好的灵活性与安全性。因而，软体机器人非常适合用于微创手术、营救等领域。在微创手术、营救等应用中，软体机器人往往要穿越狭小而密闭的环境才能到达目标位置。这样的环境是复杂、非结构化的。软体机器人的构型会随着环境的改变而改变，同时，密闭的环境使操作者(如外科医生)无法直接观察到软体机器人的形状。在缺乏形状反馈的情况下，操作者很难进行下一步的操作。

对软体机器人形状的感知依赖于各种传感器。对于应用于微创手术的软体机器人来说，现有的医学图像手段，如超声、核磁共振等手段，可以检测出体内软体机器人的形状。但是这些方法存在图像对比度过低，而且实时性太差。同时，受工作环境的限制，软体机械臂尺寸的外径一般都比较小，因而安装在软体机械臂上的内部传感器的体积不能太大，而且不能带有电路从而限制了可用的传感器种类。

光纤光栅传感器体积小，同时其电磁惰性能保证不会引起电子干扰，而且传感器内部没有电路元件，这些特性使得光纤光栅传感器广泛应用于建筑、医学等领域。在内窥镜形状的测量方面，经检索发现，专利申请号为200510025854.6，授权公告号为：CN1692871A，专利名称为软性内窥镜三维曲线形状检测装置和方法；专利申请号为200710043767.2，授权公告号为：CN101099657A，专利名称为细长柔性杆的空间形状检测装置和方法；这两项专利中分别提出了一种内窥镜三维形状检测的装置及方法。由于结构的限制，内窥镜无法轴向扭转，所以现有的方法里都没有挠率的检测装置，因而无法准确感知弯曲加扭转等复杂的形状。此外现有技术中还提出了一种可以测量曲率和挠率的方法，但是该方法只能适用于微小扭转的情形，不适用于软体手术机械臂。对于软体机械臂来说，轴向的扭转能为末端带来更高的自由度，简化手术操作、提高手术效率。本发明在传统的形状检测装置中引入了大挠率测量装置，并设计了相应的算法。仿真结果证明，该种方法能准确的检测出复杂的空间形状。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种用于感知医用软体机械臂形状的系统及方法。

根据本发明提供的用于感知医用软体机械臂形状的系统及方法，包括激光器、光谱仪、软体机械臂、传感器、计算机；其中，所述激光器发射激光信号作为入射信号进入安装于软体机械臂的传感器形成反射信号，光谱仪将接收到的所述反射信号传递给计算机，计算机根据所述反射信号处理得到软体机械臂的形状信息。

优选地，所述传感器包括多个光纤光栅传感器，其中所述多个光纤光栅传感器都沿软体机械臂的轴向均匀地埋在所述软体机械臂的内部；且所述多个光纤光栅传感器的横截面呈正方形分布；光纤光栅传感器包括光纤、光栅，其中，每一光纤中分布有多个光栅；入射信号进入光纤后，经光栅反射形成反射信号。

优选地，所述计算机包括如下装置：

第一检测装置：用于通过光谱仪检测出软体机械臂未发生形变时各光栅的布拉格波长；

第二检测装置：用于通过光谱仪检测出软体机械臂发生形变时各光栅的布拉格波长。

优选地，软体机械臂沿轴向具有多个节点，软体机械臂在相邻节点之间沿一段平面圆弧延伸，软体机械臂的扭转全部集中在节点处；所述计算机还包括如下装置：

第一计算装置：用于对各个光栅，计算波长漂移△λ_B，计算式如下：

△λ_B＝λ_B-λ_FBG；

其中，λ_B表示软体机械臂未形变时各光栅的布拉格波长，λ_FBG表示软体机械臂形变时各光栅的布拉格波长；

第二计算装置：用于根据波长漂移△λ_B、软体机械臂未发生形变时各光栅的布拉格波长λ_B和光栅的有效光弹性系数p_e，计算出各个光栅处的应变ε_x，计算公式如下：

Δλ_B＝λ_B(1-p_e)ε_x；

其中，所述应变ε_x的物理意义为光纤伸缩量与光纤原长度的比值；

第三计算装置：用于计算位于第i个节点的光栅处的曲率κ_i挠率τ_i以及和其中ε₀为测量误差；计算公式如下：

式中：标号A、B、C、D分别表示埋入软体机械臂的第一光纤、第二光纤、第三光纤、第四光纤，d_J,i表示下标J指代的标号所表示光纤的第i个光栅到软体机械臂中心线的距离；β_J,i表示下标J指代的标号所表示光纤的第i个光栅与第一光纤的第i个光栅之间在所在软体机械臂的横截面内沿顺时针方向的夹角，β_A,i＝0；

α_i表示软体机械臂在第i个节点与第i+1个节点之间部分的偏转平面角；l_i表示光纤在第i个节点与第i+1个节点之间的原始长度，a_i表示软体机械臂中心线参数方程即圆柱螺旋线参数方程中的参数，是计算κ_i,τ_i的中间变量，b_i表示表示软体机械臂中心线参数方程即圆柱螺旋线参数方程中的参数，a_i，b_i由等式算出，其中由实际测量得出。κ_i表示位于第i个节点的光栅处的曲率，τ_i表示位于第i个节点的光栅处的挠率；表示下标J指代的标号所表示光纤在第i个节点与第i+1个节点之间的计算长度；表示下标J指代的标号所表示光纤在第i个节点与第i+1个节点之间的测量长度；ε_J,i表示下标J指代的标号所表示光纤的第i个光栅处的应变ε_x；i为自然数；

第四计算装置：用于对得到的各光栅处曲率κ_i、挠率τ_i信息进行三次B样条插值，插值所得到曲线中的每条曲线段的表达式如下式所示，这些曲线段组成了曲率曲线κ(s)、挠率曲线τ(s)：

其中，p(t)表示插值得到的曲线方程，t表示归一化的弧长参数，p₀表示第一个控制点，即第i个节点所在软体机械臂横截面处的曲率或挠率；p₁表示第二个控制点，即第i+1个节点所在软体机械臂横截面处的曲率或挠率；p₂表示第三个控制点，即第i+2个节点所在软体机械臂横截面处的曲率(或挠率)；p₃表示第四个控制点，即第i+3个节点所在软体机械臂横截面处的曲率或挠率。

优选地，所述计算机还包括如下装置：

-第五计算装置：用于根据计算出的曲率κ_i、挠率τ_i，计算出各个节点的世界坐标，计算公式如下：

其中，R_i表示第i个节点所在横截面在世界坐标系下的姿态，R_i-1表示第i-1个节点所在横截面在世界坐标系下的姿态，表示第i个节点所在横截面在局部坐标系下的姿态，p_i表示第i个节点所在横截面的圆心在世界坐标系下的位置，p_i-1表示第i-1个节点所在横截面的圆心在世界坐标系下的位置，表示第i个节点所在横截面的圆心在局部坐标系下的位置；

式中：cα_i表示cos(α_i)，sα_i表示sin(α_i)，cθ_i表示cos(θ_i)，sθ_i表示sin(θ_i)，cΦ_i表示cos(Φ_i)，sΦ_i表示sin(Φ_i)，θ_i表示第i个节点与第i+1个节点之间片段的圆心角，Φ_i表示中间变量，L_i表示软体机械臂的中心线在第i个节点与第i+1个节点之间片段的原始长度；其中，所述局部坐标系建立在各个节点所在横截面上，所述世界坐标系能够在所述软体机械臂分布的空间内移动。

根据本发明提供的一种利用所述的用于感知医用软体机械臂形状的系统的感知医用软体机械臂形状的方法，包括如下步骤：

步骤1：针对软体机械臂的不同形变情况检测各个光栅的布拉格波长；

步骤2：根据各个光栅的布拉格波长，计算出各个光栅的曲率和挠率；

步骤3：根据各个光栅的曲率和挠率，计算软体机械臂的实时形状。

优选地，所述步骤1包括如下步骤：

步骤1.1：通过光谱仪检测出软体机械臂未发生形变时各光栅的布拉格波长；

步骤1.2：通过光谱仪检测出软体机械臂发生形变时各光栅的布拉格波长。

优选地，所述步骤2包括如下步骤：

步骤2.1：对各个光栅，计算波长漂移△λ_B，计算式如下：

△λ_B＝λ_B-λ_FBG；

其中，λ_B表示软体机械臂未形变时各光栅的布拉格波长，λ_FBG表示软体机械臂形变时各光栅的布拉格波长；

步骤2.2：根据波长漂移△λ_B、软体机械臂未发生形变时各光栅的布拉格波长λ_B和光栅的有效光弹性系数p_e，计算出各个光栅处的应变ε_x，计算公式如下：

Δλ_B＝λ_B(1-p_e)ε_x；

其中，所述应变ε_x的物理意义为光纤伸缩量与光纤原长度的比值；

步骤2.3：软体机械臂沿轴向具有多个节点，软体机械臂在相邻节点之间沿一段平面圆弧延伸，软体机械臂的扭转全部集中在节点处；

计算位于第i个节点的光栅处的曲率κ_i挠率τ_i以及和其中ε₀为测量误差；计算公式如下：

式中：标号A、B、C、D分别表示埋入软体机械臂的第一光纤、第二光纤、第三光纤、第四光纤，d_J,i表示下标J指代的标号所表示光纤的第i个光栅到软体机械臂中心线的距离；β_J,i表示下标J指代的标号所表示光纤的第i个光栅与第一光纤的第i个光栅之间在所在软体机械臂的横截面内沿顺时针方向的夹角，β_A,i＝0；

α_i表示软体机械臂在第i个节点与第i+1个节点之间部分的偏转平面角；l_i表示光纤在第i个节点与第i+1个节点之间的原始长度，a_i表示软体机械臂中心线参数方程即圆柱螺旋线参数方程中的参数，是计算κ_i,τ_i的中间变量，b_i表示表示软体机械臂中心线参数方程即圆柱螺旋线参数方程中的参数，a_i，b_i由等式算出，其中由实际测量得出。κ_i表示位于第i个节点的光栅处的曲率，τ_i表示位于第i个节点的光栅处的挠率；表示下标J指代的标号所表示光纤在第i个节点与第i+1个节点之间的计算长度；表示下标J指代的标号所表示光纤在第i个节点与第i+1个节点之间的测量长度；ε_J,i表示下标J指代的标号所表示光纤的第i个光栅处的应变ε_x；i为自然数；

步骤2.4：对得到的各光栅处曲率κ_i、挠率τ_i信息进行三次B样条插值，插值所得到曲线中的每条曲线段的表达式如下式所示，这些曲线段组成了曲率曲线κ(s)、挠率曲线τ(s)：

其中，p(t)表示插值得到的曲线方程，t表示归一化的弧长参数，p₀表示第一个控制点，即第i个节点所在横截面处的曲率或挠率；p₁表示第二个控制点，即第i+1个节点所在横截面处的曲率或挠率；p₂表示第三个控制点，即第i+2个节点所在横截面处的曲率(或挠率)；p₃表示第四个控制点，即第i+3个节点所在横截面处的曲率或挠率。

优选地，所述步骤3包括如下步骤：

-根据计算出的曲率κ_i、挠率τ_i，计算出各个节点的世界坐标，计算公式如下：

其中，R_i表示第i个节点所在横截面在世界坐标系下的姿态，R_i-1表示第i-1个节点所在横截面在世界坐标系下的姿态，表示第i个节点所在横截面在局部坐标系下的姿态，p_i表示第i个节点所在横截面的圆心在世界坐标系下的位置，p_i-1表示第i-1个节点所在横截面的圆心在世界坐标系下的位置，表示第i个节点所在横截面的圆心在局部坐标系下的位置；

式中：cα_i表示cos(α_i)，sα_i表示sin(α_i)，cθ_i表示cos(θ_i)，sθ_i表示sin(θ_i)，cΦ_i表示cos(Φ_i)，sΦ_i表示sin(Φ_i)，θ_i表示第i个节点与第i+1个节点之间片段的圆心角，Φ_i表示中间变量，L_i表示软体机械臂的中心线在第i个节点与第i+1个节点之间片段的原始长度；其中，所述局部坐标系建立在各个节点所在横截面上，所述世界坐标系能够在所述软体机械臂分布的空间内移动。

与现有技术相比，本发明具有如下的有益效果：

1、本发明采用两套光栅光纤传感阵列，分别测量软体机械臂的曲率与挠率，当软体机械臂发生复杂形变时，仍然能测量到相对完备的形变信息，为计算机提供完整准确的数据。

2、本发明对比度高、实时性佳，同时解决了传统内部传感器体积过大以及内部电路对患者的安全隐患问题。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明提供的完整系统示意图；

图2为本发明提供的光纤光栅传感器阵列结构示意图。

图中：

1-激光器；

2-光谱仪；

3-软体机械臂；

4-计算机；

5-硅胶；

6-驱动线；

7-光纤；

8-光栅。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。

如图1所示，根据本发明提供的用于感知医用软体机械臂形状的系统，包括激光器、光谱仪、软体机械臂、传感器、计算机；其中，所述激光器发射激光信号作为入射信号进入安装于软体机械臂的传感器形成反射信号，光谱仪将接收到的所述反射信号传递给计算机，计算机根据所述反射信号处理得到软体机械臂的形状信息。

优选地，所述传感器包括多个光纤光栅传感器，其中所述多个光纤光栅传感器都沿软体机械臂的轴向均匀地埋在所述软体机械臂的内部；且所述多个光纤光栅传感器的横截面呈正方形分布；光纤光栅传感器包括光纤、光栅，其中，每一光纤中分布有多个光栅；入射信号进入光纤后，经光栅反射形成反射信号。

如图2所示，优选地，所述传感器包括多个光纤光栅传感器，其中所述多个光纤光栅传感器都沿软体机械臂的轴向均匀地埋在所述软体机械臂的内部；且所述多个光纤光栅传感器的横截面呈正方形分布，具体是指：所述多个光纤光栅传感器的横截面的中心构成一矩形的四个顶角。光纤光栅传感器包括光纤、光栅，其中，每一光纤中分布有多个光栅；入射信号进入光纤后，经光栅反射形成反射信号。

具体地，所述光纤光栅传感器为曲率-挠率传感器，共有4套光纤。光纤中的光栅按一定的间距分布。4套光纤沿轴向均匀地埋在软体机械臂内部，其横截面呈正方形分布。

优选地，所述计算机包括如下装置：

第一检测装置：用于通过光谱仪检测出软体机械臂未发生形变时各光栅的布拉格波长；

第二检测装置：用于通过光谱仪检测出软体机械臂发生形变时各光栅的布拉格波长。

优选地，软体机械臂沿轴向具有多个节点，软体机械臂在相邻节点之间沿一段平面圆弧延伸，软体机械臂的扭转全部集中在节点处；所述计算机还包括如下装置：

第一计算装置：用于对各个光栅，计算波长漂移△λ_B，计算式如下：

△λ_B＝λ_B-λ_FBG；

其中，λ_B表示软体机械臂未形变时各光栅的布拉格波长，λ_FBG表示软体机械臂形变时各光栅的布拉格波长；

第二计算装置：用于根据波长漂移△λ_B、软体机械臂未发生形变时各光栅的布拉格波长λ_B和光栅的有效光弹性系数p_e，计算出各个光栅处的应变ε_x，计算公式如下：

Δλ_B＝λ_B(1-p_e)ε_x；

其中，所述应变ε_x的物理意义为光纤伸缩量与光纤原长度的比值；

第三计算装置：用于计算位于第i个节点的光栅处的曲率κ_i挠率τ_i以及和

其中ε₀为测量误差；计算公式如下：

式中：标号A、B、C、D分别表示埋入软体机械臂的第一光纤、第二光纤、第三光纤、第四光纤，d_J,i表示下标J指代的标号所表示光纤的第i个光栅到软体机械臂中心线的距离；β_J,i表示下标J指代的标号所表示光纤的第i个光栅与第一光纤的第i个光栅之间在所在软体机械臂的横截面内沿顺时针方向的夹角，β_A,i＝0；

α_i表示软体机械臂在第i个节点与第i+1个节点之间部分的偏转平面角；l_i表示光纤在第i个节点与第i+1个节点之间的原始长度，a_i表示软体机械臂中心线参数方程即圆柱螺旋线参数方程中的参数，是计算κ_i,τ_i的中间变量，b_i表示表示软体机械臂中心线参数方程即圆柱螺旋线参数方程中的参数，a_i，b_i由等式算出，其中由实际测量得出。κ_i表示位于第i个节点的光栅处的曲率，τ_i表示位于第i个节点的光栅处的挠率；表示下标J指代的标号所表示光纤在第i个节点与第i+1个节点之间的计算长度；表示下标J指代的标号所表示光纤在第i个节点与第i+1个节点之间的测量长度；ε_J,i表示下标J指代的标号所表示光纤的第i个光栅处的应变ε_x；i为自然数；

第四计算装置：用于对得到的各光栅处曲率κ_i、挠率τ_i信息进行三次B样条插值，插值所得到曲线中的每条曲线段的表达式如下式所示，这些曲线段组成了曲率曲线κ(s)、挠率曲线τ(s)：

其中，p(t)表示插值得到的曲线方程，t表示归一化的弧长参数，p₀表示第一个控制点，即第i个节点所在横截面处的曲率或挠率；p₁表示第二个控制点，即第i+1个节点所在横截面处的曲率或挠率；p₂表示第三个控制点，即第i+2个节点所在横截面处的曲率(或挠率)；p₃表示第四个控制点，即第i+3个节点所在横截面处的曲率或挠率。

优选地，所述计算机还包括如下装置：

-第五计算装置：用于根据计算出的曲率κ_i、挠率τ_i，计算出各个节点的世界坐标，计算公式如下：

其中，R_i表示第i个节点所在横截面在世界坐标系下的姿态，R_i-1表示第i-1个节点所在横截面在世界坐标系下的姿态，表示第i个节点所在横截面在局部坐标系下的姿态，p_i表示第i个节点所在横截面的圆心在世界坐标系下的位置，p_i-1表示第i-1个节点所在横截面的圆心在世界坐标系下的位置，表示第i个节点所在横截面的圆心在局部坐标系下的位置；

式中：cα_i表示cos(α_i)，sα_i表示sin(α_i)，cθ_i表示cos(θ_i)，sθ_i表示sin(θ_i)，cΦ_i表示cos(Φ_i)，sΦ_i表示sin(Φ_i)，θ_i表示第i个节点与第i+1个节点之间片段的圆心角，Φ_i表示中间变量，L_i表示软体机械臂的中心线在第i个节点与第i+1个节点之间片段的原始长度；其中，所述局部坐标系建立在各个节点所在横截面上，所述世界坐标系能够在所述软体机械臂分布的空间内移动。

根据本发明提供的一种利用所述的用于感知医用软体机械臂形状的系统的感知医用软体机械臂形状的方法，包括如下步骤：

步骤1：针对软体机械臂的不同形变情况检测各个光栅的布拉格波长；

步骤2：根据各个光栅的布拉格波长，计算出各个光栅的曲率和挠率；

步骤3：根据各个光栅的曲率和挠率，计算软体机械臂的实时形状。

优选地，所述步骤1包括如下步骤：

步骤1.1：通过光谱仪检测出软体机械臂未发生形变时各光栅的布拉格波长；

步骤1.2：通过光谱仪检测出软体机械臂发生形变时各光栅的布拉格波长。

具体地，假设每段软体机械臂片段的中心线在受力变形时，由直线应变为常曲率、常挠率的空间曲线(即圆柱螺旋线)，光纤7位于圆柱的母线上，当中心线发生形变时，母线即光纤7的长度会随着伸长或缩短，其长度由中心线的曲率、挠率及弯曲方向决定。

优选地，所述步骤2包括如下步骤：

步骤2.1：对各个光栅，计算波长漂移△λ_B，计算式如下：

△λ_B＝λ_B-λ_FBG；

其中，λ_B表示软体机械臂未形变时各光栅的布拉格波长，λ_FBG表示软体机械臂形变时各光栅的布拉格波长；

步骤2.2：根据波长漂移△λ_B、软体机械臂未发生形变时各光栅的布拉格波长λ_B和光栅的有效光弹性系数p_e，计算出各个光栅处的应变ε_x，计算公式如下：

Δλ_B＝λ_B(1-p_e)ε_x；

其中，所述应变ε_x的物理意义为光纤伸缩量与光纤原长度的比值；

步骤2.3：软体机械臂沿轴向具有多个节点，软体机械臂在相邻节点之间沿一段平面圆弧延伸，软体机械臂的扭转全部集中在节点处；

计算位于第i个节点的光栅处的曲率κ_i挠率τ_i以及和其中ε₀为测量误差；计算公式如下：

式中：标号A、B、C、D分别表示埋入软体机械臂的第一光纤、第二光纤、第三光纤、第四光纤，d_J,i表示下标J指代的标号所表示光纤的第i个光栅到软体机械臂中心线的距离；β_J,i表示下标J指代的标号所表示光纤的第i个光栅与第一光纤的第i个光栅之间在所在软体机械臂的横截面内沿顺时针方向的夹角，β_A,i＝0；

α_i表示软体机械臂在第i个节点与第i+1个节点之间部分的偏转平面角；l_i表示光纤在第i个节点与第i+1个节点之间的原始长度，a_i表示软体机械臂中心线参数方程即圆柱螺旋线参数方程中的参数，是计算κ_i,τ_i的中间变量，b_i表示表示软体机械臂中心线参数方程即圆柱螺旋线参数方程中的参数，a_i，b_i由等式算出，其中由实际测量得出。κ_i表示位于第i个节点的光栅处的曲率，τ_i表示位于第i个节点的光栅处的挠率；表示下标J指代的标号所表示光纤在第i个节点与第i+1个节点之间的计算长度；表示下标J指代的标号所表示光纤在第i个节点与第i+1个节点之间的测量长度；ε_J,i表示下标J指代的标号所表示光纤的第i个光栅处的应变ε_x；i为自然数。

具体地，如图2所示：g_A,i表示光纤A上第i个光栅，所述光栅位于第i段节点中，g_B,i表示光纤B上第i个光栅，所述光栅位于第i段节点中，g_C,i表示光纤C上第i个光栅，所述光栅位于第i段节点中，g_D,i表示光纤D上第i个光栅，所述光栅位于第i段节点中；d_A,i表示光栅g_A,i到中心线的距离，所述中心线为第i段节点的中心线，d_B,i表示光栅g_Bi到中心线的距离，d_C,i表示光栅g_C,i到中心线的距离，所述中心线为第i段节点的中心线，d_D,i表示光栅g_D,i到中心线的距离，所述中心线为第i段节点的中心线。

步骤2.4：对得到的各光栅处曲率κ_i、挠率τ_i信息进行三次B样条插值，插值所得到曲线中的每条曲线段的表达式如下式所示，这些曲线段组成了曲率曲线κ(s)、挠率曲线τ(s)：

其中，p(t)表示插值得到的曲线方程，t表示归一化的弧长参数，p₀表示第一个控制点，即第i个节点所在横截面处的曲率或挠率；p₁表示第二个控制点，即第i+1个节点所在横截面处的曲率或挠率；p₂表示第三个控制点，即第i+2个节点所在横截面处的曲率(或挠率)；p₃表示第四个控制点，即第i+3个节点所在横截面处的曲率或挠率。

具体地，所述步骤2.4是基于微分几何与分段常曲率假设，利用测量到的节点曲率、挠率信息，插值出信息较为完整的曲率曲线、挠率曲线。然后在两条曲线上取足够的样本点，计算出整个机械臂的形状。

优选地，所述步骤3包括如下步骤：

-根据计算出的曲率κ_i、挠率τ_i，计算出各个节点的世界坐标，计算公式如下：

其中，R_i表示第i个节点所在横截面在世界坐标系下的姿态，R_i-1表示第i-1个节点所在横截面在世界坐标系下的姿态，表示第i个节点所在横截面在局部坐标系下的姿态，p_i表示第i个节点所在横截面的圆心在世界坐标系下的位置，p_i-1表示第i-1个节点所在横截面的圆心在世界坐标系下的位置，表示第i个节点所在横截面的圆心在局部坐标系下的位置；

式中：cα_i表示cos(α_i)，sα_i表示sin(α_i)，cθ_i表示cos(θ_i)，sθ_i表示sin(θ_i)，cΦ_i表示cos(Φ_i)，sΦ_i表示sin(Φ_i)，θ_i表示第i个节点与第i+1个节点之间片段的圆心角，Φ_i表示中间变量，L_i表示软体机械臂的中心线在第i个节点与第i+1个节点之间片段的原始长度；其中，所述局部坐标系建立在各个节点所在横截面上，所述世界坐标系能够在所述软体机械臂分布的空间内移动。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本发明的实质内容。

Claims (4)

1.一种用于感知医用软体机械臂形状的系统，其特征在于，包括激光器、光谱仪、软体机械臂、传感器、计算机；其中，所述激光器发射的激光信号作为入射信号进入安装于软体机械臂的传感器形成反射信号，光谱仪将接收到的所述反射信号传递给计算机，计算机根据所述反射信号处理得到软体机械臂的形状信息；

所述传感器包括多个光纤光栅传感器，其中所述多个光纤光栅传感器沿软体机械臂的轴向均匀地埋在所述软体机械臂的内部；且所述多个光纤光栅传感器的横截面呈正方形分布；光纤光栅传感器包括光纤、光栅，其中，每一光纤中分布有多个光栅；入射信号进入光纤后，经光栅反射形成反射信号；

所述计算机包括如下装置：

第一检测装置：用于通过光谱仪检测出软体机械臂未发生形变时各光栅的布拉格波长；

第二检测装置：用于通过光谱仪检测出软体机械臂发生形变时各光栅的布拉格波长；

软体机械臂沿轴向具有多个节点，软体机械臂在相邻节点之间沿一段平面圆弧延伸，软体机械臂的扭转全部集中在节点处；所述计算机还包括如下装置：

第一计算装置：用于对各个光栅，计算波长漂移Δλ_B，计算式如下：

Δλ_B＝λ_B-λ_FBG；

其中，λ_B表示软体机械臂未形变时各光栅的布拉格波长，λ_FBG表示软体机械臂形变时各光栅的布拉格波长；

第二计算装置：用于根据波长漂移Δλ_B、软体机械臂未发生形变时各光栅的布拉格波长λ_B和光栅的有效光弹性系数p_e，计算出各个光栅处的应变ε_x，计算公式如下：

Δλ_B＝λ_B(1-p_e)ε_x；

其中，所述应变ε_x的物理意义为光纤伸缩量与光纤原长度的比值；

第三计算装置：用于计算位于第i个节点的光栅处的曲率κ_i、挠率τ_i、以及其中ε₀为测量误差；计算公式如下：

$l_{J,i}^{cal}=\frac{l_i\[a_i-d_{J,i}cos({\mathrm{\α}}_i+{\mathrm{\β}}_{J,i})\]\sqrt{1+\frac{b_i^2}{a_i^2}}}{\sqrt{a_i^2+b_i^2}},J=A,B,C,D$

$l_{J,i}^{meas}=(1+{\mathrm{\ϵ}}_{J,i}+{\mathrm{\ϵ}}_0)l_i,J=A,B,C,D$

$l_{J,i}^{meas}=l_{J,i}^{cal}$

${\mathrm{\κ}}_i=\frac{a_i}{a_i^2+b_i^2}$

${\mathrm{\τ}}_i=\frac{b_i}{a_i^2+b_i^2}$

式中：标号A、B、C、D分别表示埋入软体机械臂的第一光纤、第二光纤、第三光纤、第四光纤，d_J,i表示下标J指代的标号所表示光纤的第i个光栅到软体机械臂中心线的距离；β_J,i表示下标J指代的标号所表示光纤的第i个光栅与第一光纤的第i个光栅之间在所在软体机械臂的横截面内沿顺时针方向的夹角，β_A,i＝0；

α_i表示软体机械臂在第i个节点与第i+1个节点之间部分的偏转平面角；l_i表示光纤在第i个节点与第i+1个节点之间的原始长度，a_i表示软体机械臂中心线参数方程即圆柱螺旋线参数方程中的参数，是计算κ_i、τ_i的中间变量，b_i表示表示软体机械臂中心线参数方程即圆柱螺旋线参数方程中的参数，a_i、b_i由等式算出，其中由实际测量得出；κ_i表示位于第i个节点的光栅处的曲率，τ_i表示位于第i个节点的光栅处的挠率；表示下标J指代的标号所表示光纤在第i个节点与第i+1个节点之间的计算长度；表示下标J指代的标号所表示光纤在第i个节点与第i+1个节点之间的测量长度；ε_J,i表示下标J指代的标号所表示光纤的第i个光栅处的应变ε_x；i为自然数；

第四计算装置：用于对得到的各光栅处曲率κ_i、挠率τ_i信息进行三次B样条插值，插值所得到曲线中的每条曲线段的表达式如下式所示，这些曲线段组成了曲率曲线κ(s)、挠率曲线τ(s)：

$p\left(t\right)=\frac{1}{6}\left[\begin{array}{cccc}1& t& t^2& t^3\end{array}\right]\left[\begin{array}{cccc}1& 4& 1& 0\\ -3& 0& 3& 0\\ 3& -6& 3& 0\\ -1& 3& -3& 1\end{array}\right]\left[\begin{array}{c}{p}_0\\ p_1\\ p_2\\ p_3\end{array}\right]$

其中，p(t)表示插值得到的曲线方程，t表示归一化的弧长参数，p₀表示第一个控制点，即第i个节点所在横截面处的曲率或挠率；p₁表示第二个控制点，即第i+1个节点所在横截面处的曲率或挠率；p₂表示第三个控制点，即第i+2个节点所在横截面处的曲率或挠率；p₃表示第四个控制点，即第i+3个节点所在横截面处的曲率或挠率。

2.根据权利要求1所述的用于感知医用软体机械臂形状的系统，其特征在于，所述计算机还包括如下装置：

-第五计算装置：用于根据计算出的曲率κ_i、挠率τ_i，计算出各个节点的世界坐标，计算公式如下：

$\begin{array}{c}{R}_i=\left\{\begin{array}{c}{R}_i^{i-1},i=1\\ R_{i-1}{R}_i^{i-1},i>1\end{array}\right.\\ p_i=\left\{\begin{array}{c}{p}_i^{i-1},i=1\\ p_{i-1}+R_{i-1}{p}_i^{i-1},i>1\end{array}\right.\end{array};$

其中，R_i表示第i个节点所在横截面在世界坐标系下的姿态，R_i-1表示第i-1个节点所在横截面在世界坐标系下的姿态，表示第i个节点所在横截面在局部坐标系下的姿态，p_i表示第i个节点所在横截面的圆心在世界坐标系下的位置，p_i-1表示第i-1个节点所在横截面的圆心在世界坐标系下的位置，表示第i个节点所在横截面的圆心在局部坐标系下的位置；

$\begin{array}{c}{R}_i^{i-1}=\left[\begin{array}{ccc}{\mathrm{c\α}}_i{\mathrm{c\θ}}_i{\mathrm{c\Φ}}_i-{\mathrm{s\α}}_i{\mathrm{s\Φ}}_i& -{\mathrm{c\α}}_i{\mathrm{c\θ}}_i{\mathrm{s\Φ}}_i-{\mathrm{s\α}}_i{\mathrm{c\Φ}}_i& -{\mathrm{s\α}}_i{\mathrm{s\θ}}_i\\ {\mathrm{s\α}}_i{\mathrm{c\θ}}_i{\mathrm{c\Φ}}_i+{\mathrm{c\α}}_{i}s\mathrm{\Φ}& -{\mathrm{s\α}}_i{\mathrm{c\θ}}_i{\mathrm{s\Φ}}_i+{\mathrm{c\α}}_{i}c\mathrm{\Φ}& -{\mathrm{s\α}}_i{\mathrm{s\θ}}_i\\ {\mathrm{s\θ}}_i{\mathrm{c\Φ}}_i& -{\mathrm{s\θ}}_i{\mathrm{s\Φ}}_i& {\mathrm{c\θ}}_i\end{array}\right]\\ p_i^{i-1}=\left[\begin{array}{ccc}\frac{{\mathrm{c\α}}_i(1-{\mathrm{c\θ}}_i)}{{\mathrm{\κ}}_i}& \frac{{\mathrm{s\α}}_i(1-{\mathrm{c\θ}}_i)}{{\mathrm{\κ}}_i}& \frac{{\mathrm{s\θ}}_i}{{\mathrm{\κ}}_i}\end{array}\right]\\ {\mathrm{\θ}}_i={\mathrm{\κ}}_i{L}_i\\ {\mathrm{\Φ}}_i={\mathrm{\τ}}_i-{\mathrm{\α}}_i\end{array};$

式中：cα_i表示cos(α_i)，sα_i表示sin(α_i)，cθ_i表示cos(θ_i)，sθ_i表示sin(θ_i)，cΦ_i表示cos(Φ_i)，sΦ_i表示sin(Φ_i)，θ_i表示第i个节点与第i+1个节点之间片段的圆心角，Φ_i表示中间变量，L_i表示软体机械臂的中心线在第i个节点与第i+1个节点之间片段的原始长度；其中，所述局部坐标系建立在各个节点所在横截面上，所述世界坐标系能够在所述软体机械臂分布的空间内移动。

3.一种利用权利要求1所述的用于感知医用软体机械臂形状的系统的用于感知医用软体机械臂形状的方法，其特征在于，包括如下步骤：

步骤1：针对软体机械臂的不同形变情况检测各个光栅的布拉格波长；

步骤2：根据各个光栅的布拉格波长，计算出各个光栅的曲率和挠率；

步骤3：根据各个光栅的曲率和挠率，计算软体机械臂的实时形状；

所述步骤1包括如下步骤：

步骤1.1：通过光谱仪检测出软体机械臂未发生形变时各光栅的布拉格波长；

步骤1.2：通过光谱仪检测出软体机械臂发生形变时各光栅的布拉格波长；

所述步骤2包括如下步骤：

步骤2.1：对各个光栅，计算波长漂移Δλ_B，计算式如下：

Δλ_B＝λ_B-λ_FBG；

其中，λ_B表示软体机械臂未形变时各光栅的布拉格波长，λ_FBG表示软体机械臂形变时各光栅的布拉格波长；

步骤2.2：根据波长漂移Δλ_B、软体机械臂未发生形变时各光栅的布拉格波长λ_B和光栅的有效光弹性系数p_e，计算出各个光栅处的应变ε_x，计算公式如下：

Δλ_B＝λ_B(1-p_e)ε_x；

其中，所述应变ε_x的物理意义为光纤伸缩量与光纤原长度的比值；

步骤2.3：软体机械臂沿轴向具有多个节点，软体机械臂在相邻节点之间沿一段平面圆弧延伸，软体机械臂的扭转全部集中在节点处；

计算位于第i个节点的光栅处的曲率κ_i、挠率τ_i、以及其中ε₀为测量误差；计算公式如下：

$\begin{array}{c}{l}_{J,i}^{cal}=\frac{l_i\[a_i-d_{J,i}cos({\mathrm{\α}}_i+{\mathrm{\β}}_{J,i})\]\sqrt{1+\frac{b_i^2}{a_i^2}}}{\sqrt{a_i^2+b_i^2}},J=A,B,C,D\\ l_{J,i}^{meas}=(1+{\mathrm{\ϵ}}_{J,i}+{\mathrm{\ϵ}}_0)l_i,J=A,B,C,D\\ l_{J,i}^{cal}=l_{J,i}^{meas}\\ {\mathrm{\κ}}_i=\frac{a_i}{a_i^2+b_i^2}\\ {\mathrm{\τ}}_i=\frac{b_i}{a_i^2+b_i^2}\end{array};$

式中：标号A、B、C、D分别表示埋入软体机械臂的第一光纤、第二光纤、第三光纤、第四光纤，d_J,i表示下标J指代的标号所表示光纤的第i个光栅到软体机械臂中心线的距离；β_J,i表示下标J指代的标号所表示光纤的第i个光栅与第一光纤的第i个光栅之间在所在软体机械臂的横截面内沿顺时针方向的夹角，β_A,i＝0；

α_i表示软体机械臂在第i个节点与第i+1个节点之间部分的偏转平面角；l_i表示光纤在第i个节点与第i+1个节点之间的原始长度，a_i表示软体机械臂中心线参数方程即圆柱螺旋线参数方程中的参数，是计算κ_i、τ_i的中间变量，b_i表示表示软体机械臂中心线参数方程即圆柱螺旋线参数方程中的参数，a_i、b_i由等式算出，其中由实际测量得出；κ_i表示位于第i个节点的光栅处的曲率，τ_i表示位于第i个节点的光栅处的挠率；表示下标J指代的标号所表示光纤在第i个节点与第i+1个节点之间的计算长度；表示下标J指代的标号所表示光纤在第i个节点与第i+1个节点之间的测量长度；ε_J,i表示下标J指代的标号所表示光纤的第i个光栅处的应变ε_x；i为自然数；

步骤2.4：对得到的各光栅处曲率κ_i、挠率τ_i信息进行三次B样条插值，插值所得到曲线中的每条曲线段的表达式如下式所示，这些曲线段组成了曲率曲线κ(s)、挠率曲线τ(s)：

$p\left(t\right)=\frac{1}{6}\left[\begin{array}{cccc}1& t& t^2& t^3\end{array}\right]\left[\begin{array}{cccc}1& 4& 1& 0\\ -3& 0& 3& 0\\ 3& -6& 3& 0\\ -1& 3& -3& 1\end{array}\right]\left[\begin{array}{c}{p}_0\\ p_1\\ p_2\\ p_3\end{array}\right]$

其中，p(t)表示插值得到的曲线方程，t表示归一化的弧长参数，p₀表示第一个控制点，即第i个节点所在横截面处的曲率或挠率；p₁表示第二个控制点，即第i+1个节点所在横截面处的曲率或挠率；p₂表示第三个控制点，即第i+2个节点所在横截面处的曲率或挠率；p₃表示第四个控制点，即第i+3个节点所在横截面处的曲率或挠率。

4.根据权利要求3所述的用于感知医用软体机械臂形状的方法，其特征在于，所述步骤3包括如下步骤：

-根据计算出的曲率κ_i、挠率τ_i，计算出各个节点的世界坐标，计算公式如下：

$\begin{array}{c}{R}_i=\left\{\begin{array}{c}{R}_i^{i-1},i=1\\ R_{i-1}{R}_i^{i-1},i>1\end{array}\right.\\ p_i=\left\{\begin{array}{c}{p}_i^{i-1},i=1\\ p_{i-1}+R_{i-1}{p}_i^{i-1},i>1\end{array}\right.\end{array};$

其中，R_i表示第i个节点所在横截面在世界坐标系下的姿态，R_i-1表示第i-1个节点所在横截面在世界坐标系下的姿态，表示第i个节点所在横截面在局部坐标系下的姿态，p_i表示第i个节点所在横截面的圆心在世界坐标系下的位置，p_i-1表示第i-1个节点所在横截面的圆心在世界坐标系下的位置，表示第i个节点所在横截面的圆心在局部坐标系下的位置；

$\begin{array}{c}{R}_i^{i-1}=\left[\begin{array}{ccc}{\mathrm{c\α}}_i{\mathrm{c\θ}}_i{\mathrm{c\Φ}}_i-{\mathrm{s\α}}_i{\mathrm{s\Φ}}_i& -{\mathrm{c\α}}_i{\mathrm{c\θ}}_i{\mathrm{s\Φ}}_i-{\mathrm{s\α}}_i{\mathrm{c\Φ}}_i& -{\mathrm{s\α}}_i{\mathrm{s\θ}}_i\\ {\mathrm{s\α}}_i{\mathrm{c\θ}}_i{\mathrm{c\Φ}}_i+{\mathrm{c\α}}_{i}s\mathrm{\Φ}& -{\mathrm{s\α}}_i{\mathrm{c\θ}}_i{\mathrm{s\Φ}}_i+{\mathrm{c\α}}_{i}c\mathrm{\Φ}& -{\mathrm{s\α}}_i{\mathrm{s\θ}}_i\\ {\mathrm{s\θ}}_i{\mathrm{c\Φ}}_i& -{\mathrm{s\θ}}_i{\mathrm{s\Φ}}_i& {\mathrm{c\θ}}_i\end{array}\right]\\ p_i^{i-1}=\left[\begin{array}{ccc}\frac{{\mathrm{c\α}}_i(1-{\mathrm{c\θ}}_i)}{{\mathrm{\κ}}_i}& \frac{{\mathrm{s\α}}_i(1-{\mathrm{c\θ}}_i)}{{\mathrm{\κ}}_i}& \frac{{\mathrm{s\θ}}_i}{{\mathrm{\κ}}_i}\end{array}\right]\\ {\mathrm{\θ}}_i={\mathrm{\κ}}_i{L}_i\\ {\mathrm{\Φ}}_i={\mathrm{\τ}}_i-{\mathrm{\α}}_i\end{array};$

式中：cα_i表示cos(α_i)，sα_i表示sin(α_i)，cθ_i表示cos(θ_i)，sθ_i表示sin(θ_i)，cΦ_i表示cos(Φ_i)，sΦ_i表示sin(Φ_i)，θ_i表示第i个节点与第i+1个节点之间片段的圆心角，Φ_i表示中间变量，L_i表示软体机械臂的中心线在第i个节点与第i+1个节点之间片段的原始长度；其中，所述局部坐标系建立在各个节点所在横截面上，所述世界坐标系能够在所述软体机械臂分布的空间内移动。