CN102481089A - 内窥镜形状检测装置及内窥镜的插入部的形状检测方法 - Google Patents

Abstract

内窥镜形状检测装置(1)具备：姿态检测部(30)，其基于所述传感器单元(S_i)中的测量数据来检测各个所述传感器单元(S_i)的姿态；以及线形形状检测部(40)，其基于由所述姿态检测部(30)检测到的各个所述传感器单元(S_i)的姿态，将各个所述传感器单元(S_i)之间的形状假定为尺寸与所述传感器间尺寸(I)相等的直线状的链(T_j)，来检测所述内窥镜(10)的所述插入部(11)的检测线形形状(61)。并且，内窥镜形状检测装置(1)具备曲线形状检测部(50、70)，该曲线形状检测部(50、70)将各个所述传感器单元(S_i)之间的形状假定为弧长与所述传感器间尺寸(I)相等的圆弧(L_j)，来对由所述线形形状检测部(40)检测到的所述检测线形形状(61)进行曲线插补，并对检测曲线形状(65、81)进行检测。

Description

内窥镜形状检测装置及内窥镜的插入部的形状检测方法

技术领域

本发明涉及具备用于插入体腔内的内窥镜的内窥镜形状检测装置及该内窥镜形状检测装置的内窥镜的插入部的形状检测方法。

背景技术

近年来，能够对内窥镜的插入部的形状进行检测的内窥镜形状检测装置已经实用化。在专利文献1中公开了在用于插入体腔内的内窥镜的插入部中安装有多个源线圈(source coil)的内窥镜插入形状检测装置。在该内窥镜插入形状检测装置中，利用设于体外的读出线圈(sense coil)检测各源线圈的位置。进而，基于检测到的源线圈的位置检测内窥镜的插入部的形状。

并且，在专利文献2中公开了在内窥镜的插入部安装有两个传感器的内窥镜装置。在该内窥镜装置中，以基端侧的传感器为基准，检测前端侧的传感器的位置、姿态。基端侧的传感器配置于弯曲部的基端附近，前端侧的传感器配置于前端硬性部上。通过检测前端侧的传感器相对于基端侧的传感器的位置、姿态，计算弯曲部的弯曲角度、弯曲方向。

并且，在专利文献3中公开了在内窥镜的插入部中安装有多个陀螺仪的内窥镜形状检测装置。在该内窥镜形状检测装置中，利用陀螺仪检测内窥镜的插入部的预定部位(安装有陀螺仪的部位)的姿态。进而，基于检测到的预定部位处的姿态，检测插入部的形状。

现有技术文献

专利文献

专利文献1：日本特开2000-175862号公报

专利文献2：日本特开2007-319622号公报

专利文献3：日本特开平11-19027号公报

发明内容

发明所要解决的课题

在上述专利文献1的内窥镜形状检测装置中，利用体外的读出线圈检测体腔内的源线圈的位置，因此，检测装置大型化，并且装置的结构复杂化。

在上述专利文献2中，仅使用体腔内的传感器检测弯曲部的弯曲角度、弯曲方向。但是，实际的内窥镜的插入部具有挠性，插入部插入到体腔内时的形状为曲线状。即，在该内窥镜装置中，仅通过检测弯曲部的弯曲角度、弯曲方向，是无法以高精度检测出插入部插入体腔内时的形状的。

在上述专利文献3中，使用多个陀螺仪检测内窥镜的插入部的形状。但是，陀螺仪基于插入部移动时产生的惯性力或科里奥利力来检测插入部的预定部位处的姿态。因此，在插入部未移动的静止状态下，无法检测形状，插入部低速移动时的检测精度降低。

本发明就是着眼于上述课题而完成的，其目的在于提供内窥镜形状检测装置及内窥镜的插入部的形状检测方法，能够以高精度检测插入部的形状，而不会使装置的结构大型化、复杂化。

用于解决课题的手段

为了达成上述目的，在本发明的一个方式中，提供一种内窥镜形状检测装置，该内窥镜形状检测装置具备：内窥镜，其具有插入部，该插入部以沿长度方向彼此离开预定的传感器间尺寸的方式配置有多个传感器单元；姿态检测部，其基于所述传感器单元中的测量数据来检测各个所述传感器单元的姿态；线形形状检测部，其基于由所述姿态检测部检测到的各个所述传感器单元的姿态，将各个所述传感器单元之间的形状假定为尺寸与传感器间尺寸相等的直线状的链，来检测所述内窥镜的所述插入部的检测线形形状；以及曲线形状检测部，其将各个所述传感器单元之间的形状假定为弧长与所述传感器间尺寸相等的圆弧，来对由所述线形形状检测部检测到的所述检测线形形状进行曲线插补，并对检测曲线形状进行检测。

并且，在本发明的另一方式中，提供一种内窥镜的插入部的形状检测方法，该方法包括如下步骤：利用在内窥镜的插入部中以沿长度方向彼此离开预定的传感器间尺寸的方式配置的多个传感器单元进行测量；基于所述传感器单元中的测量数据来检测各个所述传感器单元的姿态；基于检测到的各个所述传感器单元的姿态，将各个所述传感器单元之间的形状假定为尺寸与所述传感器间尺寸相等的直线状的链，来检测所述内窥镜的所述插入部的检测线形形状；以及将各个所述传感器单元之间的形状假定为弧长与所述传感器间尺寸相等的圆弧，来对检测到的所述检测线形形状进行曲线插补，形成检测曲线形状。

发明效果

根据本发明，能够提供内窥镜形状检测装置及内窥镜的插入部的形状检测方法，能够以高精度检测插入部的形状，而不会使装置的结构大型化、复杂化。

附图说明

图1是示出本发明的第一实施方式的内窥镜形状检测装置的结构的框图。

图2是示出第一实施方式的内窥镜的插入部的结构的示意图。

图3是示出第一实施方式的内窥镜形状检测装置的计算机的结构的框图。

图4是示出第一实施方式的检测内窥镜的插入部在静止状态下的形状的方法的流程图。

图5是对第一实施方式的内窥镜形状检测装置的全局坐标系和校正坐标系进行比较示出的示意图。

图6是示出利用第一实施方式的内窥镜形状检测装置的线形形状检测部检测的检测线形形状的示意图。

图7是说明第一实施方式的线形形状检测部的链位置校正部中的处理的示意图。

图8是示出利用第一实施方式的内窥镜形状检测装置的曲线形状检测部检测的检测曲线形状的示意图。

图9是示出利用第一实施方式的曲线形状检测部对检测曲线形状进行检测的方法的流程图。

图10是说明第一实施方式的曲线形状检测部的曲线插补部中的处理的示意图。

图11是说明第一实施方式的曲线形状检测部的圆弧位置校正部中的处理的示意图。

图12是示出本发明的第二实施方式的内窥镜形状检测装置的计算机的结构的框图。

图13是示出利用第二实施方式的内窥镜形状检测装置的曲线形状检测部对检测曲线形状进行检测的方法的流程图。

图14是说明第二实施方式的曲线形状检测部的插补顺序实施部中的处理的示意图。

图15是说明第二实施方式的曲线形状检测部的未插补链位置校正部中的处理的示意图。

图16是本发明的第一变形例的内窥镜形状检测装置的计算机的结构的框图。

图17是示出第一变形例的检测内窥镜的插入部在运动状态下的形状的方法的流程图。

具体实施方式

(第一实施方式)

参照图1至图11说明本发明的第一实施方式。

图1是示出本实施方式的内窥镜形状检测装置1的图。如图1所示，内窥镜形状检测装置1的内窥镜10具备用于插入体腔内的插入部11以及设于插入部11的基端侧的操作部12。插入部11具备：设于最前端的前端硬性部14；设于前端硬性部14的基端侧的弯曲部16；以及设于弯曲部16的基端侧的细长的挠性管部18。

在前端硬性部14的内部设有进行被摄体的摄像的CCD等摄像元件20。摄像元件20连接着摄像用信号线21的一端。摄像用信号线21穿过插入部11的内部从操作部12延伸至内窥镜10的外部，摄像用信号线21的另一端连接于作为图像处理单元的视频处理器3。此外，在插入部11的内部，沿长度方向延伸设置有光导23，该光导23将照射被摄体的照明光引导至前端硬性部14的照明窗(未图示)。光导23从操作部12延伸至内窥镜10的外部，并连接于光源单元4。

并且，在插入部11的弯曲部16的前端部，连接着作为弯曲操作传递部件的四根弯曲操作线(未图示)的一端。弯曲操作线穿过挠性管部18的内部，另一端与设于操作部12的作为弯曲操作部的弯曲操作旋钮(未图示)连接。通过弯曲操作旋钮的操作，弯曲操作线沿长度方向移动。通过弯曲操作的移动，弯曲部16在内窥镜10的上下方向及左右方向进行弯曲操作。

插入部2中设有多个(在本实施方式中为N+1个)传感器单元S₀～S_N。各个传感器单元S_i(i＝0，1，2，...，N)彼此在长度方向上离开恒定的间隔I(＝50mm)配置。即，各个传感器单元S_i彼此在长度方向上离开预定的传感器间尺寸I进行配置。这里，例如最靠基端侧的传感器单元S₀配置于挠性管部18的基端部，最靠前端侧的传感器单元S_N配置于弯曲部16的前端部。传感器单元S_i具备测量加速度的加速度传感器A_i和测量地磁的地磁传感器B_i。

图2是示出内窥镜10的插入部11的图。如图2所示，各个传感器单元S_i具备以传感器单元S_i的中心为原点并具有X_i轴、Y_i轴、Z_i轴的局部坐标系C_i(图2中虚线所示)。这里，X_i轴方向与传感器单元S_i的中心处的内窥镜10的左右方向一致，以从基端侧观察时的内窥镜10的右方向为正。Y_i轴方向与传感器单元S_i的中心处的长度方向一致，以前端侧方向为正。Z_i轴方向与传感器单元S_i的中心处的内窥镜10的上下方向一致，以内窥镜10的上方向为正。加速度传感器A_i测量在局部坐标系C_i的原点处的加速度的X_i轴方向分量、Y_i轴方向分量、Z_i轴方向分量。地磁传感器B_i测量在局部坐标系C_i的原点处的地磁的X_i轴方向分量、Y_i轴方向分量、Z_i轴方向分量。

并且，在内窥镜形状检测装置1中，定义了以最靠基端侧的传感器单元S₀的中心为原点并具有X轴、Y轴、Z轴的全局坐标系C(图2中实线所示)。这里，全局坐标系C是以最靠基端侧的传感器单元S₀的中心为原点的右手系的正交笛卡尔坐标系。X轴方向与垂直于重力作用的铅垂方向的预定方向(本实施方式中为与图2的箭头D1、D2平行的方向)一致，以图2中的箭头D1的方向为正。Y轴方向与垂直于铅垂方向且垂直于X轴方向的方向(本实施方式中为与图2中的箭头E1、E2平行的方向)一致，以图2中的箭头E1的方向为正。Z轴方向与铅垂方向一致，以铅垂方向的上方向(从纸面的里侧朝向外侧的方向)为正。另外，这里，为了便于说明，以全局坐标系的X轴方向为磁北方向。

各个局部坐标系C_i是如下这样得到的坐标系：使全局坐标系C分别绕X轴旋转α_i、绕Y轴旋转β_i、绕Z轴旋转γ_i，并使原点从最靠基端侧的传感器单元S₀的中心平行移动到传感器单元S_i的中心。这里，将α_i称为俯仰角，将β_i称为滚转角，将γ_i称为偏摆角，将俯仰角α_i、滚转角β_i、偏摆角γ_i这三个角统称为姿态角。姿态角α_i、β_i、γ_i分别以从X轴、Y轴、Z轴的负方向观察的顺时针方向为正。通过计算姿态角α_i、β_i、γ_i的值，检测传感器单元S_i的姿态。

如图1所示，在各个传感器单元S_i的加速度传感器A_i和地磁传感器B_i中，连接着I2C等串行总线5。串行总线5穿过插入部11的内部从操作部12延伸至内窥镜10的外部，基端连接于串行转换器6。串行转换器6将从各个传感器单元S_i经由串行总线5输入的测量数据的串行信号转换为USB信号。在串行转换器6上连接着USB线缆7的一端。USB线缆7的另一端连接于计算机8。各个传感器单元S_i中的测量数据的USB信号从串行转换器6输入至计算机8。

图3是示出计算机8的结构的图。如图3所示，计算机8具备经由USB线缆7而与串行转换器6连接的通信部26。通信部26接收各个传感器单元S_i中的测量数据。在通信部26上连接着物理量换算部28。物理量换算部28使用偏移量、增益等将由通信部26接收到的各个传感器单元S_i中的测量数据换算为物理量。

在物理量换算部28上连接着姿态检测部30。姿态检测部30基于传感器单元S_i中的测量数据来检测各个传感器单元S_i的姿态。姿态检测部30具备姿态角计算部32，该姿态角计算部32基于各个传感器单元S_i的加速度传感器A_i和地磁传感器B_i中的测量数据，计算各个传感器单元S_i的局部坐标系C_i相对于全局坐标系C绕X轴、Y轴、Z轴的旋转角即三个姿态角α_i、β_i、γ_i。姿态角计算部32具备第一角度计算部34，该第一角度计算部34基于各个传感器单元S_i的加速度传感器A_i中的加速度数据，计算各个传感器单元S_i的局部坐标系C_i相对于全局坐标系C绕X轴的旋转角即俯仰角α_i、以及各个传感器单元S_i的局部坐标系C_i相对于全局坐标系C绕Y轴的旋转角即滚转角β_i。并且，姿态角计算部32具备第二角度计算部36，该第二角度计算部36基于各个传感器单元S_i的地磁传感器B_i中的地磁数据，计算各个传感器单元S_i的局部坐标系C_i相对于全局坐标系C绕Z轴的旋转角即偏摆角γ_i。

这里，对利用姿态检测部30检测各个传感器单元S_i的姿态的方法进行说明。图4是示出内窥镜10的插入部11停止的静止状态下的插入部11的形状检测方法的流程图。如图4所示，在检测插入部11的形状时，首先进行各个传感器单元S_i的测量(步骤S101)，姿态检测部30取得各个传感器单元S_i中的测量数据。进而，姿态角计算部32计算各个传感器单元S_i的局部坐标系C_i的三个姿态角α_i、β_i、γ_i。

在计算姿态角α_i、β_i、γ_i时，首先，第一角度计算部34基于各个传感器单元S_i的加速度传感器A_i中的测量数据，计算各个传感器单元S_i的局部坐标系C_i的俯仰角α_i以及滚转角β_i(步骤S102)。这里，姿态角α_i、β_i、γ_i是按照偏摆角γ_i、俯仰角α_i、滚转角β_i的顺序旋转的(Z，X，Y)型。因此，从全局坐标系C向局部坐标系C_i的旋转矩阵为

【数学式1】

$C_{\mathrm{Bi}}^G=R_{\mathrm{Zi}}{R}_{\mathrm{Xi}}{R}_{\mathrm{Yi}}=\left[\begin{array}{ccc}\cos {\mathrm{\γ}}_i& -\sin {\mathrm{\γ}}_i& 0\\ \sin {\mathrm{\γ}}_i& \cos {\mathrm{\γ}}_i& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\α}}_i& -\sin {\mathrm{\α}}_i\\ 0& \sin {\mathrm{\α}}_i& \cos {\mathrm{\α}}_i\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\β}}_i& 0& \sin {\mathrm{\β}}_i\\ 0& 1& 0\\ -\sin {\mathrm{\β}}_i& 0& \cos {\mathrm{\β}}_i\end{array}\right]$

$=\left[\begin{array}{ccc}-{\sin \mathrm{\γ}}_i\·\sin {\mathrm{\α}}_i\·\sin {\mathrm{\β}}_i+\cos {\mathrm{\β}}_i\·\cos {\mathrm{\γ}}_i& -\sin {\mathrm{\γ}}_i\·\cos {\mathrm{\α}}_i& \sin {\mathrm{\γ}}_i\·\sin {\mathrm{\α}}_i\·\cos {\mathrm{\β}}_i+\sin {\mathrm{\β}}_i\·\cos {\mathrm{\γ}}_i\\ \cos {\mathrm{\γ}}_i\·\sin {\mathrm{\α}}_i\·\sin {\mathrm{\β}}_i+\cos {\mathrm{\β}}_i\·\sin {\mathrm{\γ}}_i& \cos {\mathrm{\γ}}_i\·\cos {\mathrm{\α}}_i& -\cos {\mathrm{\γ}}_i\·\sin {\mathrm{\α}}_i\·\cos {\mathrm{\β}}_i+\sin {\mathrm{\β}}_i\·\sin {\mathrm{\γ}}_i\\ -\cos {\mathrm{\α}}_i\·\sin {\mathrm{\β}}_i& \sin {\mathrm{\α}}_i& \cos {\mathrm{\α}}_i\·\cos {\mathrm{\β}}_i\end{array}\right]---\left(1\right).$

在插入部11停止的静止状态下，只有重力加速度作用于铅垂方向的下方向。即，在全局坐标系C中和局部坐标系C_i中，都是只有重力加速度作用于铅垂方向的下方向。因此，此时，加速度矢量的全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量为

【数学式2】

${\stackrel{\·}{a}}_{\mathrm{th}}={\left[\begin{array}{ccc}0& 0& -g\end{array}\right]}^T---\left(2\right).$

此外，加速度传感器A_i所测量的加速度矢量的局部坐标系C_i的X_i轴方向分量、Y_i轴方向分量、Z_i轴方向分量为

【数学式3】

${\stackrel{\·}{a}}_{\mathrm{obsi}}={\left[\begin{array}{ccc}{a}_{\mathrm{Bi}\_X}& a_{\mathrm{Bi}\_Y}& a_{\mathrm{Bi}\_Z}\end{array}\right]}^T---\left(3\right).$

这里，局部坐标系C_i是使全局坐标系C依次旋转偏摆角γ_i、俯仰角α_i、滚转角β_i而得到的坐标系。因此，根据式(1)～式(3)，局部坐标系C_i中观测到的加速度分量为

【数学式4】

${\stackrel{\·}{a}}_{\mathrm{obsi}}={\left(C_{\mathrm{Bi}}^G\right)}^T{\stackrel{\·}{a}}_{\mathrm{th}}=-g\left[\begin{array}{c}-\cos {\mathrm{\α}}_i\·\sin {\mathrm{\β}}_i\\ \sin {\mathrm{\α}}_i\\ \cos {\mathrm{\α}}_i\·\cos {\mathrm{\β}}_i\end{array}\right]---\begin{array}{c}\left(4.1\right)\\ \left(4.2\right)\\ \left(4.3\right)\end{array}.$

这里，若对式(4.1)的平方和式(4.3)的平方进行相加，则为

【数学式5】

a_{Bi_X} ²+a_{Bi_Z} ²＝g²cos²α_i(sin²β_i+cos²β_i)    (5)，

得到

【数学式6】

$g\cos {\mathrm{\α}}_i=\sqrt{{a_{\mathrm{Bi}\_X}}^2+{a_{\mathrm{Bi}\_Z}}^2}---\left(6\right).$

进而，通过式(4.2)除以式(6)，成为

【数学式7】

${\mathrm{\α}}_i={\tan }^{-1}\left(\frac{-a_{\mathrm{Bi}\_Y}}{\sqrt{{a_{\mathrm{Bi}\_X}}^2+{a_{\mathrm{Bi}\_Z}}^2}}\right)---\left(7\right),$

求出局部坐标系C_i的俯仰角α_i。并且，通过式(4.1)除以式(4.3)，成为

【数学式8】

${\mathrm{\β}}_i={\tan }^{-1}\left(\frac{-a_{\mathrm{Bi}\_X}}{a_{\mathrm{Bi}\_Z}}\right)---\left(8\right),$

求出局部坐标系C_i的滚转角β_i。如上所述，基于各个加速度传感器A_i中的测量数据，计算各个局部坐标系C_i的俯仰角α_i及滚转角β_i。

进而，第二角度计算部36基于各个传感器单元S_i的地磁传感器B_i中的测量数据，计算各个传感器单元S_i的局部坐标系C_i的偏摆角γ_i(步骤S103)。这里，使用在步骤S102中计算出的俯仰角α_i和滚转角β_i，定义对各个局部坐标系C_i相对于全局坐标系C绕X轴的旋转及绕Y轴的旋转进行校正得到的校正坐标系C′_i。图5是示出全局坐标系C(在图5中用实线所示)以及校正坐标系C′_i(图5中用虚线所示)的图。另外，实际上在全局坐标系C和校正坐标系C′_i中，原点的位置不同，但在图5中，为了比较两者，以原点处于同一位置的状态进行表示。如图5所示，对绕X轴的旋转及绕Y轴的旋转进行校正得到的校正坐标系C′_i结果是使全局坐标系C绕Z轴旋转偏摆角γ_i得到的坐标系，具有X′_i轴、Y′_i轴、Z′_i轴。X′_i轴方向、Y′_i轴方向分别与从全局坐标系C的X轴方向、Y轴方向绕Z轴方向旋转偏摆角γ_i后的方向一致。Z′_i轴方向与铅垂方向、即全局坐标系C的Z轴方向一致。这里，由于全局坐标系C的X轴方向与磁北方向一致，所以X′_i轴方向成为从全局坐标系C的X轴方向绕Z轴旋转偏摆角γ_i后的方向。

地磁传感器B_i所测量的地磁矢量的局部坐标系C_i的X_i轴方向分量、Y_i轴方向分量、Z_i轴方向分量为

【数学式9】

${\stackrel{\·}{m}}_{\mathrm{obsi}}={\left[\begin{array}{ccc}{M}_{\mathrm{Xi}}& M_{\mathrm{Yi}}& M_{\mathrm{Zi}}\end{array}\right]}^T---\left(9\right).$

校正坐标系C′_i是对局部坐标系C_i相对于全局坐标系C绕X轴的旋转及绕Y轴的旋转进行校正得到的坐标系。因此，使用式(9)及式(1)的R_xi、R_yi，地磁传感器B_i所测量的地磁矢量的校正坐标系C′_i的X′_i轴方向分量、Y′_i轴方向分量、Z′_i轴方向分量为

【数学式10】

${{\stackrel{\·}{m}}_{\mathrm{obsi}}}^{\′}=R_{\mathrm{Xi}}{R}_{\mathrm{Yi}}{\stackrel{\·}{m}}_{\mathrm{obsi}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\α}}_i& -\sin {\mathrm{\α}}_i\\ 0& \sin {\mathrm{\α}}_i& \cos {\mathrm{\α}}_i\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\β}}_i& 0& \sin {\mathrm{\β}}_i\\ 0& 1& 0\\ -\sin {\mathrm{\β}}_i& 0& \cos {\mathrm{\β}}_i\end{array}\right]\left[\begin{array}{c}{M}_{\mathrm{Xi}}\\ M_{\mathrm{Yi}}\\ M_{\mathrm{Zi}}\end{array}\right]$

$=\left[\begin{array}{ccc}\cos {\mathrm{\β}}_i& 0& \sin {\mathrm{\β}}_i\\ \sin {\mathrm{\α}}_i\sin {\mathrm{\β}}_i& \cos {\mathrm{\α}}_i& -\sin {\mathrm{\α}}_i\cos {\mathrm{\β}}_i\\ -\cos {\mathrm{\α}}_i\sin {\mathrm{\β}}_i& \sin {\mathrm{\α}}_i& \cos {\mathrm{\α}}_i\cos {\mathrm{\β}}_i\end{array}\right]\left[\begin{array}{c}{M}_{\mathrm{Xi}}\\ M_{\mathrm{Yi}}\\ M_{\mathrm{Zi}}\end{array}\right]---\left(10.1\right)$

${{\stackrel{\·}{m}}_{\mathrm{obsi}}}^{\′}={\left[\begin{array}{ccc}{M_{\mathrm{Xi}}}^{\′}& {M_{\mathrm{Yi}}}^{\′}& {M_{\mathrm{Zi}}}^{\′}\end{array}\right]}^T---\left(10.2\right).$

从式(10.1)、式(10.2)得到

【数学式11】

M_Xi′＝M_Xicosβ_i+M_Zisinβ_i                     (11.1)

M_Yi′＝M_Yicosα_i+sinα_i(M_Xisinβ_i-M_Zicosβ_i)   (11.2)。

与铅垂方向垂直的水平面(校正坐标系C′_i的X′_i-Y′_i平面)的地磁分量面向磁北方向。因此，根据式(11.1)、式(11.2)，使用地磁矢量的校正坐标系C′_i的X′_i轴分量、Y′_i轴分量能够求得从X′_i轴到磁北方向的角度θ_i。即，成为

【数学式12】

θ_i＝tan^-1(M_Yi′/M_Xi′)                         (12)。

角度θ_i以从负方向观察Z′_i轴(Z轴)时的顺时针为正。校正坐标系C′_i是使全局坐标系C绕Z轴旋转偏摆角γ_i后的坐标系。因此，利用式(12)求得的角度θ_i成为以全局坐标系C为基准的局部坐标系C_i的偏摆角γ_i。

另外，在全局坐标系C的X轴方向与磁北方向不一致的情况下，也可以以磁北为基准求出偏摆角γ_i。地磁矢量的全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量为

【数学式13】

${\stackrel{\·}{m}}_{\mathrm{th}}={\left[\begin{array}{ccc}{E}_X& E_Y& E_Z\end{array}\right]}^T---\left(13\right).$

地磁矢量的全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量是通过使用与地磁传感器B_i为同一类型的地磁传感器，在轴的方向与全局坐标系C的X轴方向、Y轴方向、Z轴方向一致的状态下进行测量而求得的。进而，根据式(13)，使用地磁矢量的全局坐标系C的X轴分量、Y轴分量求出从X轴到磁北方向的角度θ。即，成为

【数学式14】

θ＝tan^-1(E_Y/E_X)                              (14)。

这里，角度θ以从负方向观察Z轴时的顺时针为正。校正坐标系C′_i是使全局坐标系C绕Z轴旋转偏摆角γ_i后的坐标系。因此，根据式(12)、式(14)，成为

【数学式15】

γ_i＝θ-θ_i                                   (15)，

求出以全局坐标系C为基准的局部坐标系C_i的偏摆角γ_i。

如以上所述，基于各个地磁传感器B_i中的测量数据，计算各个局部坐标系C_i的偏摆角γ_i。姿态检测部30基于计算出的姿态角α_i、β_i、γ_i的值，对各个传感器单元S_i的姿态进行检测。

如图3所示，在姿态检测部30上连接着线形形状检测部40。图6是从全局坐标系C的Z轴的正方向观察利用线形形状检测部40检测的内窥镜10的插入部11的检测线形形状61时的图。如图6所示，线形形状检测部40基于由姿态检测部30检测到的各个传感器单元S_i的姿态，将各个传感器单元S_i之间的形状假定为尺寸与传感器间尺寸I相等的直线状的链T_j(j＝1，2，...N)，来检测插入部11的检测线形形状61。这里，从基端侧起第k个链T_k是从基端侧起第k个传感器单元S_k-1与从基端侧起第(k+1)个传感器单元S_k之间的链。线形形状检测部40具备：形成各个链T_j的链形成部41；以及链位置校正部42，其使链形成部41所形成的各个链T_j平行移动，以校正链T_j的位置。通过链位置校正部42，各个链T_j平行移动成与邻接的链T_j-1、T_j+1的链边界连续的状态。

在线形形状检测部40上连接着描绘部45。在描绘部45上连接着显示部47。由线形形状检测部40检测到的在全局坐标系C中的插入部11的检测线形形状61通过描绘部45进行描绘。手术操作者能够在显示部47上确认由描绘部45描绘出的检测线形形状61。

这里，对利用线形形状检测部40检测插入部11的检测线形形状61的方法进行说明。如图4所示，在检测插入部11的检测线形形状61时，首先，基于在步骤S102、S103中计算出的姿态角α_i、β_i、γ_i的值，链形成部41形成直线形状的各个链T_j(步骤S104)。这里，对从基端侧起第k个传感器单元S_k-1与从基端侧起第(k+1)个传感器单元S_k之间的、从基端侧起第k个链T_k的形成进行说明。

如式(7)、式(8)、式(12)(或式(15))所示，在步骤S102、S103中计算局部坐标系C_k-1(即链)的姿态角α_k-1、β_k-1、γ_k-1。使用该姿态角α_k-1、β_k-1、γ_k-1及各个传感器单元S_i之间的在长度方向上的间隔即传感器间尺寸I，求出传感器单元S_k-1位于全局坐标系C的原点时的、传感器单元S_k的坐标P′_k(I_xk，I_yk，I_zk)。这里，坐标P′_k成为

【数学式16】

${{\stackrel{.}{l}}_k=\left[\begin{array}{ccc}{l}_{\mathrm{xk}}& l_{\mathrm{yk}}& l_{\mathrm{zk}}\end{array}\right]}^T={\mathrm{lC}}_{\mathrm{Bk}-1}^G{e}_{\mathrm{yk}-1}=l\left[\begin{array}{c}-{\sin \mathrm{\γ}}_{k-1}\·\cos {\mathrm{\α}}_{k-1}\\ \cos {\mathrm{\γ}}_{k-1}\·\cos {\mathrm{\α}}_{k-1}\\ \sin {\mathrm{\α}}_{k-1}\end{array}\right]---\left(16.1\right)$

e_yk-1＝[0 1 0]^T                                 (16.2)。

式(16.1)、式(16.2)的e_yk-1是在局部坐标系C_k-1的原点处的长度方向即Y_k-1轴方向的单位矢量。通过将单位矢量e_yk-1与用式(1)计算出的旋转矩阵相乘，分别计算出单位矢量e_yk-1的全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量。即，I_xk、I_yk、I_zk是将局部坐标系C_k-1中的朝向Y_k-1轴方向的大小为I的矢量分别分解到全局坐标系C的X轴方向、Y轴方向、Z轴方向的分量。通过将全局坐标系C的原点与利用式(16.1)、式(16.2)计算出的坐标P′_k(I_xk，I_yk，I_zk)呈直线状连结，形成链T_k。

另外，对于链T_k以外的链T_j，也利用链形成部41同样形成。即，使用式(16.1)、式(16.2)，求出链T_j的基端侧(离全局坐标系C的原点较近的一侧)的传感器单元S_j-1位于全局坐标系C的原点时的、链T_j的前端侧(离全局坐标系C的原点较远的一侧)的传感器单元S_j的坐标P′_j(I_xj，I_yj，I_zj)。进而，通过将全局坐标系C的原点与坐标P′_j(I_xj，I_yj，I_zj)呈直线状连结，形成链T_j。即，链形成部41将链T_j假定为，沿着基端侧的传感器单元S_j-1的中心处的长度方向，从基端侧(离全局坐标系C的原点较近的一侧)的传感器单元S_j-1的中心延伸设置到前端侧(离全局坐标系C的原点较远的一侧)的传感器单元S_j的中心，来形成链T_j。

此外，传感器间尺寸I优选为50mm左右。通过加长传感器间尺寸I，传感器单元S_i的数量减少，削减成本。并且，若传感器间尺寸I处于小于50mm左右的范围，则在将各个传感器单元S_i之间假定为尺寸与传感器间尺寸I相等的直线状的链T_j的情况下，也能够减小插入部11的形状检测时的误差。

进而，链位置校正部42使通过链形成部41形成的各个链T_j平行移动成与邻接的链T_j-1、T_j+1的链边界连续的状态，来校正链T_j的位置(步骤S105)。图7是说明链位置校正部42中的处理的图。这里，对从基端侧起第k个传感器单元S_k-1与从基端侧起第(k+1)个传感器单元S_k之间的、从基端侧起第k个链T_k的位置校正进行说明。

如图7所示，在利用链位置校正部42进行链T_k的位置校正之前的状态下，到与链T_k的基端侧邻接的链T_k-1为止完成位置校正，形成链位置校正完成部63。在进行链T_k的位置校正时，链位置校正部42使链T_k平行移动从原点到链位置校正完成部63的前端的移动量。即，使链T_k从图7的虚线所示的位置平行移动到图7的实线所示的位置。由此，成为链T_k-1与链T_k的链边界连续的状态，链T_k的位置被校正。

另外，对于链T_k以外的链T_j，也利用链位置校正部42同样校正位置。即，在进行链T_j的位置校正时，链位置校正部42使链T_j平行移动从原点到链位置校正完成部63的前端(离全局坐标系C的原点较远的一侧的端部)的移动量。由此，成为链T_j和与链T_j的基端侧(离全局坐标系C的原点较近的一侧)邻接的链T_j-1的链边界连续的状态，链T_j的位置被校正。其中，关于链T₁，由于链T₁的基端为全局坐标系C的原点，所以不进行位置的校正。

如图3所示，线形形状检测部40与曲线形状检测部50连接。图8是从全局坐标系C的Z轴的正方向观察利用曲线形状检测部50检测到的内窥镜10的插入部11的检测曲线形状65时的图。如图8所示，曲线形状检测部50将各个传感器单元S_i之间的形状假定为弧长与传感器间尺寸I相等的圆弧L_j(j＝1，2，...N)，对检测线形形状61进行曲线插补。通过进行曲线插补，来形成检测曲线形状65。这里，从基端侧起第k个圆弧L_k是从基端侧起第k个传感器单元S_k-1与从基端侧起第(k+1)个传感器单元S_k之间的圆弧。

曲线形状检测部50具备：针对各个链T_j进行曲线插补而形成圆弧L_j的曲线插补部51；将曲线插补部51控制为对所有链T_j进行曲线插补的状态的插补控制部52；以及圆弧位置校正部53，其使通过曲线插补部51形成的各个圆弧L_j平行移动，以对圆弧L_j的位置进行校正。通过圆弧位置校正部53，各个圆弧L_j平行移动为与邻接的圆弧L_j-1、圆弧L_j+1的圆弧边界连续的状态。

如图3所示，曲线插补部51具备法线形成部55、中心确定部57以及圆弧形成部59。关于法线形成部55、中心确定部57以及圆弧形成部59的详细情况在后面进行描述。

在曲线形状检测部50上连接着描绘部45。由曲线形状检测部50形成的在全局坐标系C中的插入部11的检测曲线形状65通过描绘部45进行描绘。手术操作者能够在显示部47上确认由描绘部45描绘出的检测曲线形状65。

这里，说明利用曲线形状检测部50对由线形形状检测部40检测到的检测线形形状61进行曲线插补并对检测曲线形状65进行检测的方法。如图4所示，曲线形状检测部50将各个传感器单元S_i之间的形状假定为弧长与传感器间尺寸I相等的圆弧L_j，来进行检测线形形状61的曲线插补，并估计检测曲线形状65(步骤S106)。如前所述，若传感器间尺寸I处于小于50mm左右的范围，则在将各个传感器单元S_i之间假定为尺寸与传感器间尺寸I相等的直线状的链T_j的情况下，也能够减小插入部11的形状检测时的误差。但是，插入到体腔内时的内窥镜10的插入部11的形状为曲线形状。因此，进行检测线形形状61的曲线插补较为重要。这里，内窥镜10的插入部11因产品不同而有差异，但具有适度的弹性。因此，插入部11的曲线形状的曲率大幅变化的情况很少见。因此，通过将传感器单元S_i之间的形状假定为半径为R_j(曲率1/R_j)的圆弧L_j来进行曲线插补，形成与实际的插入部11的曲线形状的误差较小的检测曲线形状65。

在进行检测线形形状61的曲线插补并对检测曲线形状65进行检测时，曲线插补部51对各个链T_j进行曲线插补，形成圆弧L_j。这里，对利用曲线插补部51进行链T_j的曲线插补的方法进行说明。这里，对从基端侧起第k个传感器单元S_k-1与从基端侧起第(k+1)个传感器单元S_k之间的、从基端侧起第k个链T_k的曲线插补进行说明。

图9是示出利用曲线形状检测部50从检测线形形状61对检测曲线形状65进行检测的方法的流程图。图10是说明曲线插补部51中的处理的图。如图10所示，在利用曲线插补部51进行插补对象即链(插补对象链)T_k的曲线插补之前的状态下，传感器单元S_k位于点Q′_k。如图9和图10所示，利用曲线插补部51进行链(插补对称链)T_k的曲线插补时，首先，法线形成部55形成：第一插补法线N_k-1，其通过链T_k的基端(第一端点)并与链T_k垂直；以及第二插补法线N_k，其通过链T_k的前端(第二端点)并垂直于与链T_k的前端侧邻接的链(插补对象邻接链)T_k+1(步骤S111)。即，能够利用与链T_k的方向矢量垂直这样的条件以及通过传感器单元(第一端点)S_k-1这样的条件来求出第一插补法线N_k-1。同样，能够利用与链T_k+1的方向矢量垂直这样的条件以及通过传感器单元(第二端点)S_k这样的条件来求出第二插补法线N_k。

进而，中心确定部57计算第一插补法线N_k-1与第二插补法线N_k的交点O_k，并将该交点O_k确定为圆弧L_k的中心(步骤S112)。在第一插补法线N_k-1与第二插补法线N_k不相交的情况下，计算第一插补法线N_k-1与第二插补法线N_k的距离最小的两点的中间点O_k。计算中间点O_k时，首先求出与第一插补法线N_k-1和第二插补法线N_k双方垂直的直线(未图示)的方程式。进而，求出该直线与第一插补法线N_k-1的交点以及该直线与第二插补法线N_k的交点，两个交点的中间点成为中间点O_k。进而，将中间点O_k确定为圆弧L_k的中心O_k(步骤S112)。

进而，圆弧形成部59形成圆弧L_k(步骤S113)。圆弧L_k具有与中心确定部57所确定的中心O_k和链(插补对象链)T_k的基端(第一端点)之间的距离相同的半径R_k。即，圆弧L_k具有曲率1/R_k。此外，圆弧L_k是以链T_k的基端(第一端点)为起点的弧长与传感器间尺寸I相等的圆弧L_k。圆弧L_k的前端Q_k位于曲线插补后的传感器单元S_k的位置。即，利用链T_k的曲线插补，传感器单元S_k的位置从点Q′_k移动到点Q_k。如上所述，进行链T_k的曲线插补。

另外，对于链T_k以外的链T_j，也利用曲线插补部51同样地进行曲线插补。即，在进行链(插补对象链)T_j的曲线插补时，法线形成部55形成：第一插补法线N_j-1，其通过链T_j的基端(离全局坐标系C的原点较近的一侧的端部)即第一端点，并与链T_j垂直；以及第二插补法线N_j，其通过链T_j的前端(离全局坐标系C的原点较远的一侧的端部)即第二端点，并垂直于与链T_j的前端侧(离全局坐标系C的原点较远的一侧)邻接的链(插补对象邻接链)T_j+1(步骤S111)。进而，中心确定部57计算第一插补法线N_j-1与第二插补法线N_j的交点O_j、或者计算第一插补法线N_j-1与第二插补法线N_j的距离最小的两点的中间点O_j，并将该交点O_j或中间点O_j确定为圆弧L_j的中心O_j(步骤S112)。进而，圆弧形成部59形成圆弧L_j(步骤S113)，该圆弧L_j的半径R_j与中心O_j和链(插补对象链)T_j的基端之间的距离相同，且以链T_j的基端为起点的弧长与传感器间尺寸I相等。

进而，如图9所示，插补控制部52确认对所有的链T_j的曲线插补是否完成(步骤S114)。在所有的链T_j的曲线插补完成的情况下，进入下一步骤(步骤S114-是)。在所有的链T_j的曲线插补并未完成的情况下，返回步骤S111(步骤S114-否)，利用曲线插补部51，对未完成曲线插补的链T_j进行曲线插补。即，曲线插补部51被控制为进行步骤S111～S113的状态，直到对所有的链T_j(本实施方式中为N根)完成曲线插补为止。

当曲线插补部51对所有的链T_j进行曲线插补时，圆弧位置校正部53使通过曲线插补部51形成的各个圆弧L_j平行移动成与邻接的圆弧L_j-1、L_j+1的圆弧边界连续的状态，来校正圆弧L_j的位置(步骤S115)。图11是说明圆弧位置校正部53中的处理的图。这里，对从基端侧起第k个传感器单元S_k-1与从基端侧起第(k+1)个传感器单元S_k之间的、从基端侧起第k个圆弧L_k的位置校正进行说明。

如图11所示，在利用圆弧位置校正部53进行圆弧L_k的位置校正之前的状态下，到与圆弧L_k的基端侧邻接的圆弧L_k-1为止完成位置校正，形成圆弧位置校正完成部67。在进行圆弧L_k的位置校正时，圆弧位置校正部53使圆弧L_k平行移动从位置校正前的圆弧L_k的基端到圆弧位置校正完成部67的前端的移动量。即，使圆弧L_k从图11中的虚线所示的位置平行移动到图11中的实线所示的位置。由此，成为圆弧L_k-1与圆弧L_k的圆弧边界连续的状态，圆弧L_k的位置被校正。

另外，对于圆弧L_k以外的圆弧L_j，也利用圆弧位置校正部53同样校正位置。即，在进行圆弧L_j的位置校正时，圆弧位置校正部53使圆弧L_j平行移动从位置校正前的圆弧L_j的基端到圆弧位置校正完成部67的前端(离全局坐标系C的原点较远的一侧的端部)的移动量。由此，成为圆弧L_j和与圆弧L_j的基端侧(离全局坐标系C的原点较近的一侧)邻接的链圆弧L_j-1的圆弧边界连续的状态，圆弧L_j的位置被校正。其中，关于圆弧L₁，由于圆弧L₁的基端为全局坐标系C的原点，所以不进行位置的校正。

如图4所示，当曲线形状检测部50的检测线形形状61的曲线插补完成、形成检测曲线形状65时，确认内窥镜形状装置1的检查是否完成(步骤S107)。在检查未完成的情况下(步骤S107-否)，返回步骤S101，对下一个静止状态下的内窥镜10的插入部11的形状进行检测。在检查完成的情况下(步骤S107-是)，内窥镜10的插入部11的形状检测结束。

因此，在上述结构的内窥镜形状检测装置1及使用内窥镜形状检测装置1的内窥镜10的插入部11的形状检测方法中，发挥以下的效果。即，在内窥镜形状检测装置1中，姿态检测部30根据各个传感器单元S_i的测量数据来检测传感器单元S_i的姿态，线形形状检测部40根据各个传感器单元S_i的姿态来检测内窥镜10的插入部11的检测线形形状61。进而，曲线形状检测部50进行检测线形形状61的曲线插补，并对检测曲线形状65进行检测。如上所述，根据观察时插入体腔内的插入部11所配置的传感器单元S_i的测量数据来检测插入部11的检测曲线形状65，因此，不需要在体外设置读出线圈等。因此，能够实现内窥镜形状检测装置1的小型化、单纯化。

并且，在内窥镜形状检测装置1中，利用曲线形状检测部50将各个传感器单元S_i之间的形状假定为弧长与传感器间尺寸I相等的圆弧L_j，对检测线形形状61进行曲线插补，并对检测曲线形状65进行检测。实际上，内窥镜10的插入部11因产品不同而有差异，但具有适度的弹性。因此，插入部11的曲线形状的曲率大幅变化的情况很少见。因此，通过将传感器单元S_i之间的形状假定为半径为R_j(曲率1/R_j)的圆弧L_j来进行曲线插补，能够检测与实际的插入部11的曲线形状的误差较小的检测曲线形状65。由此，能够以高精度对插入部11的检测曲线形状65进行检测。

另外，在内窥镜形状检测装置1中，在插入部11未移动的静止状态下，加速度传感器A_i测量重力加速度，地磁传感器B_i测量地磁。进而，姿态检测部30根据测量到的重力加速度、地磁来对各个传感器单元S_i的姿态进行检测。在静止状态下，重力加速度和地磁始终在恒定的方向上具有恒定的大小。由于根据重力加速度、地磁来对各个传感器单元S_i的姿态进行检测，所以在静止状态下，也能够以高精度检测传感器单元S_i的姿态。由此，能够以高精度对插入部11的检测曲线形状65进行检测。

(第二实施方式)

接着，参照图12至图15说明本发明的第二实施方式。另外，对与第一实施方式相同的部分及具有相同功能的部分标注同一标号并省略其说明。

图12是示出本实施方式的计算机8的结构的图。如图12所示，计算机8与第一实施方式同样，具备通信部26、物理量换算部28、姿态检测部30、线形形状检测部40、描绘部45以及显示部47。在线形形状检测部40和描绘部45上连接着曲线形状检测部70。

曲线形状检测部70与第一实施方式的曲线形状检测部50同样，将各个传感器单元S_i之间的形状假定为弧长与传感器间尺寸I相等的圆弧L_j(j＝1，2，...N)，来对检测线形形状61进行曲线插补，并对检测曲线形状81进行检测。其中，对检测曲线形状81进行检测的方法与曲线形状检测部50不同。

曲线形状检测部70具备：作为曲线插补部的插补顺序实施部71，其针对各个链T_j从基端侧(离全局坐标系C的原点较近的一侧)的链T_j起依次进行曲线插补，形成圆弧L_j；以及插补控制部72，其将插补顺序实施部71控制为对所有链T_j进行曲线插补的状态。并且，曲线形状检测部70具备未插补链位置校正部73，该未插补链位置校正部73每当插补顺序实施部71进行一个链T_j的曲线插补时，就使由未进行曲线插补的链(未插补链)T_j构成的插补未完成部85平行移动，对位置进行校正。利用未插补链位置校正部73，插补未完成部85平行移动成与由利用曲线插补形成的圆弧L_j构成的插补完成部83的边界连续的状态。

如图12所示，插补顺序实施部71具有法线形成部75、中心确定部77以及圆弧形成部79。关于法线形成部75、中心确定部77以及圆弧形成部79的详细情况在后面进行描述。

这里，说明利用曲线形状检测部70对由线形形状检测部40检测到的检测线形形状61进行曲线插补并对检测曲线形状81进行检测的方法。曲线形状检测部70与第一实施方式的曲线形状检测部50同样，将各个传感器单元S_i之间的形状假定为弧长与传感器间尺寸I相等的圆弧L_j，来对检测线形形状61进行曲线插补(图4的步骤S106)。由此，如前面的第一实施方式所述，形成与实际的插入部11的曲线形状的误差较小的检测曲线形状81。

在对检测线形形状61进行曲线插补时，插补顺序实施部71针对各个链T_j从基端侧(离全局坐标系C的原点较近的一侧)的链T_j起依次进行曲线插补，形成圆弧L_j。这里，说明利用插补顺序实施部71进行链T_j的曲线插补的方法。这里，对从基端侧起第k个传感器单元S_k-1与从基端侧起第(k+1)个传感器单元S_k之间的、从基端侧起第k个链T_k的曲线插补进行说明。

图13是示出通过曲线形状检测部70利用检测线形形状61对检测曲线形状81进行检测的方法的流程图。图14是说明插补顺序实施部71中的处理的图。如图14所示，在利用插补顺序实施部71进行链T_k的曲线插补之前的状态下，直至链T_k-1为止，完成曲线插补，形成插补完成部83。插补完成部83由圆弧L₁～L_k-1构成。并且，形成由未进行插补的链(插补未完成链)T_k～T_N构成的插补未完成部85。此时，插补完成部83与插补未完成部85的边界连续，传感器单元S_k位于点U′_k。

在该状态下，利用插补顺序实施部71对插补未完成部85中位于最靠基端侧的链(插补对象链)T_k进行曲线插补。如图13和图14所示，在进行链T_k的曲线插补时，首先，法线形成部75形成：第一插补法线N_k-1，其通过链T_k的基端(第一端点)并与链T_k垂直；以及第二插补法线N_k，其通过链T_k的前端(第二端点)并垂直于与链T_k的前端侧邻接的链(插补对象邻接链)T_k+1(步骤S121)。

进而，中心确定部77计算第一插补法线N_k-1与第二插补法线N_k的交点O_k，并将该交点O_k确定为圆弧L_k的中心(步骤S122)。在第一插补法线N_k-1与第二插补法线N_k不相交的情况下，计算第一插补法线N_k-1与第二插补法线N_k的距离最小的两点的中间点O_k。中间点O_k通过与第一实施方式同样的方法计算。进而，将中间点O_k确定为圆弧L_k的中心O_k(步骤S122)。

进而，圆弧形成部79形成圆弧L_k(步骤S123)。圆弧L_k具有与中心确定部77所确定的中心O_k和链(插补对象链)T_k的基端之间的距离相同的半径R_k。即，圆弧L_k具有曲率1/R_k。此外，圆弧L_k是以链T_k的基端为起点的弧长与传感器间尺寸I相等的圆弧L_k。圆弧L_k的前端U_k位于曲线插补后的传感器单元S_k的位置。即，利用链T_k的曲线插补，传感器单元S_k的位置从点U′_k移动到点U_k。通过传感器单元S_k的位置的移动，在利用插补顺序实施部71进行链T_k的曲线插补前的状态下连续的插补完成部83与插补未完成部85的边界成为不连续的状态。即，圆弧L_k与链T_k+1成为不连续的状态。

另外，对于链T_k以外的链T_j，也利用插补顺序实施部71同样进行曲线插补。即，在进行链(插补对象链)T_j的曲线插补时，法线形成部75形成：第一插补法线N_j-1，其通过链T_j的基端(离全局坐标系C的原点较近的一侧的端部)即第一端点，并与链T_j垂直；以及第二插补法线N_j，其通过链T_j的前端(离全局坐标系C的原点较远的一侧的端部)即第二端点，并垂直于与链T_j的前端侧(离全局坐标系C的原点较远的一侧)邻接的链(插补对象邻接链)T_j+1(步骤S121)。进而，中心确定部77计算第一插补法线N_j-1与第二插补法线N_j的交点O_j、或者计算第一插补法线N_j-1与第二插补法线N_j的距离最小的两点的中间点O_j，并将该交点O_j或中间点O_j确定为圆弧L_j的中心O_j(步骤S122)。进而，圆弧形成部79形成圆弧L_j(步骤S123)，该圆弧L_j的半径R_j与中心O_j和链(插补对象链)T_j的基端之间的距离相同，且以链T_j的基端为起点的弧长与传感器间尺寸I相等。

图15是说明未插补链位置校正部73中的处理的图。如图15所示，在利用插补顺序实施部71进行链T_k的曲线插补、并形成了圆弧L_k后的状态下，形成有完成了曲线插补的插补完成部83和未完成曲线插补的插补未完成部85。插补完成部83由圆弧L₁～L_k构成，插补未完成部85由链T_k+1～T_N构成。此时，如前所述，通过链T_k的曲线插补，传感器单元S_k的位置从点U′_k移动到点U_k，因此，插补完成部83与插补未完成部85的边界不连续。在该状态下，未插补链位置校正部73使插补未完成部85平行移动到与插补完成部83的边界连续的状态，从而对插补未完成部85的位置进行校正(步骤S124)。即，插补未完成部85从图15的虚线所示的位置平行移动到实线所示的位置。

另外，对于链T_k以外的链T_j，也同样利用未插补链位置校正部73校正插补未完成部85的位置。即，在利用插补顺序实施部71进行了链T_j的曲线插补后，插补完成部83与插补未完成部85的边界不连续。在该状态下，未插补链位置校正部73使插补未完成部85平行移动到与插补完成部83连续的状态，从而对插补未完成部85的位置进行校正(步骤S124)。未插补链位置校正部73每当插补顺序实施部71进行一个链T_j的曲线插补时，就对插补未完成部85的位置进行校正。

进而，如图13所示，插补控制部72确认对所有的链T_j的曲线插补是否完成(步骤S125)。在所有的链T_j的曲线插补完成的情况下，形成插入部11的检测曲线形状81，进入下一步骤(步骤S125-是)。在所有的链T_j的曲线插补并未完成的情况下，返回步骤S121(步骤S125-否)，利用插补顺序实施部71，对插补未完成部85中位于最靠基端侧(离全局坐标系C的原点较近的一侧)的链(插补对象链)T_j进行曲线插补。即，插补顺序实施部71被控制为进行步骤S121～S123的状态，直到对所有的链T_j完成曲线插补为止。进而，当利用插补顺序实施部71进行下一链T_j的曲线插补时(步骤S121～S123)，利用未插补链位置校正部73进行插补未完成部85的位置的校正(步骤S124)。

因此，在上述结构的内窥镜形状检测装置1及使用内窥镜形状检测装置1的内窥镜10的插入部11的形状检测方法中，发挥以下的效果。即，在内窥镜形状检测装置1中，姿态检测部30根据各个传感器单元S_i的测量数据来检测传感器单元S_i的姿态，线形形状检测部40根据各个传感器单元S_i的姿态来检测内窥镜10的插入部11的检测线形形状61。进而，曲线形状检测部70进行检测线形形状61的曲线插补，并对检测曲线形状81进行检测。如上所述，根据观察时插入体腔内的插入部11所配置的传感器单元S_i的测量数据来检测插入部11的检测曲线形状81，因此，不需要在体外设置读出线圈等。因此，能够实现内窥镜形状检测装置1的小型化、单纯化。

并且，在内窥镜形状检测装置1中，利用曲线形状检测部70将各个传感器单元S_i之间的形状假定为弧长与传感器间尺寸I相等的圆弧L_j，来对检测线形形状61进行曲线插补，并对检测曲线形状81进行检测。实际上，内窥镜10的插入部11因产品不同而有差异，但具有适度的弹性。因此，插入部11的曲线形状的曲率大幅变化的情况很少见。因此，通过将传感器单元S_i之间的形状假定为半径为R_j(曲率1/R_j)的圆弧L_j来进行曲线插补，能够检测与实际的插入部11的曲线形状的误差较小的检测曲线形状81。由此，能够以高精度对插入部11的检测曲线形状81进行检测。

另外，在内窥镜形状检测装置1中，在插入部11未移动的静止状态下，加速度传感器A_i测量重力加速度，地磁传感器B_i测量地磁。进而，姿态检测部30根据测量到的重力加速度、地磁来对各个传感器单元S_i的姿态进行检测。在静止状态下，重力加速度和地磁始终在恒定的方向上具有恒定的大小。由于根据重力加速度、地磁来对各个传感器单元S_i的姿态进行检测，所以在静止状态下，也能够以高精度检测传感器单元S_i的姿态。由此，能够以高精度对插入部11的检测曲线形状81进行检测。

(第一变形例)

接着，参照图16和图17说明上述实施方式的第一变形例。另外，对与第一实施方式和第二实施方式相同的部分及具有相同功能的部分标注同一标号并省略其说明。

在本变形例中，能够对内窥镜10的插入部11平行移动的运动状态下的插入部11的形状进行检测。运动状态下的插入部11的形状的检测在图4的步骤S106和步骤S107之间进行。图16是示出本变形例的计算机8的结构的图。图17是示出对运动状态下的插入部11的形状进行检测的方法的流程图。

如图16所示，计算机8与第一实施方式同样，具备通信部26、物理量换算部28、姿态检测部30、线形形状检测部40、描绘部45、显示部47以及曲线形状检测部50。

在物理量换算部28上连接着加速度检测部90。如图17所示，在对运动状态下的插入部11的形状进行检测时，加速度检测部90取得由各个传感器单元S_i的加速度传感器A_i测量到的运动状态下的加速度数据(步骤S131)。进而，加速度检测部90基于运动状态下的加速度数据，将在各个传感器单元S_i的中心测量到的加速度矢量分解为全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量。进而，对在各个传感器单元S_i的中心测量到的加速度矢量的全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量进行检测(步骤S132)。

这里，说明利用加速度检测部90对在各个传感器单元S_i的中心测量到的加速度矢量的全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量的方法。在运动状态下，对在静止状态下产生于铅垂方向的重力加速度施加由插入部11的移动产生的分量，因此，在各个传感器单元S_i的中心测量到的加速度矢量的全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量为

【数学式17】

${\stackrel{\·}{a}}_{\mathrm{thi}}={\left[\begin{array}{ccc}{a}_{\mathrm{thi}\_X}& a_{\mathrm{thi}\_Y}& -g+a_{\mathrm{thi}\_Z}\end{array}\right]}^T---\left(17\right).$

并且，由各个传感器单元S_i的加速度传感器A_i测量到的加速度矢量的局部坐标系C_i的X_i轴方向分量、Y_i轴方向分量、Z_i轴方向分量为

【数学式18】

${{\stackrel{\·}{a}}_{\mathrm{obsi}}}^{\′}={\left[\begin{array}{ccc}{a_{\mathrm{Bi}\_X}}^{\′}& {a_{\mathrm{Bi}\_Y}}^{\′}& {a_{\mathrm{Bi}\_Z}}^{\′}\end{array}\right]}^T---\left(18\right).$

并且，在本变形例中，由于考虑内窥镜10的插入部11平行移动的情况，因此，在接近配置的多个传感器单元S_i中，局部坐标系C_i中的加速度矢量假定为相同。在实际的内窥镜10中，各个传感器单元S_i之间的传感器间尺寸I非常小，因此该假定是可能的。通过这样假定，例如在传感器单元S₁测量的加速度矢量与传感器单元S₂测量的加速度矢量之间，成为

【数学式19】

${\stackrel{\·}{a}}_{\mathrm{th}1}={\stackrel{\·}{a}}_{\mathrm{th}2}---\left(19.1\right)$

$\left|\right|{{\stackrel{\·}{a}}_{\mathrm{obs}1}}^{\′}\left|\right|=\left|\right|{{\stackrel{\·}{a}}_{\mathrm{obs}2}}^{\′}\left|\right|---\left(19.2\right).$

进而，根据式(19.1)、式(19.2)，如下关系分别成立：

【数学式20】

[a_{th1_X} a_{th1_Y} a_{th1_Z}]^T＝[a_{th2_X} a_{th2_Y} a_{th2_Z}]^T

                                                (20.1)

${\left[\begin{array}{ccc}{a_{B1\_X}}^{\′}& {a_{B1\_Y}}^{\′}& {a_{B1\_Z}}^{\′}\end{array}\right]}^T={\left[\begin{array}{ccc}{a_{B2\_X}}^{\′}& {a_{B2\_Y}}^{\′}& \sqrt{{\left|\right|{{\stackrel{\·}{a}}_{\mathrm{obs}1}}^{\′}\left|\right|}^2-({a_{B2\_X}}^{\′2}+{a_{B2\_Y}}^{\′2})}\end{array}\right]}^T---\left(20.2\right).$

这里，在传感器单元S₁中，根据全局坐标系C与局部坐标系C₁的关系，使用式(1)的旋转矩阵，成为

【数学式21】

${{\stackrel{\·}{a}}_{\mathrm{obs}1}}^{\′}{\left(C_{B1}^G\right)}^T{\stackrel{\·}{a}}_{\mathrm{th}1}$

$=\left[\begin{array}{ccc}-\sin {\mathrm{\γ}}_1\·\sin {\mathrm{\α}}_1\·\sin {\mathrm{\β}}_1+\cos {\mathrm{\β}}_1\·\cos {\mathrm{\γ}}_1& -\sin {\mathrm{\γ}}_1\·\cos {\text{\α}}_1& \sin {\mathrm{\γ}}_1\·\sin {\mathrm{\α}}_1\·\cos {\mathrm{\β}}_1+\sin {\mathrm{\β}}_1\·\cos {\mathrm{\γ}}_1\\ \cos {\mathrm{\γ}}_1\·\sin {\mathrm{\α}}_1\·\sin {\mathrm{\β}}_1+\cos {\mathrm{\β}}_1\·\sin {\mathrm{\γ}}_1& \cos {\mathrm{\γ}}_1\·\cos {\mathrm{\α}}_1& -\cos {\mathrm{\γ}}_1\·\sin {\mathrm{\α}}_1\·\cos {\mathrm{\β}}_1+\sin {\mathrm{\β}}_1\·\sin {\mathrm{\γ}}_1\\ -\cos {\mathrm{\α}}_1\·\sin {\mathrm{\β}}_1& \sin {\mathrm{\α}}_1& \cos {\mathrm{\α}}_1\·\cos {\mathrm{\β}}_1\end{array}\right]$

$\left[\begin{array}{c}{a}_{\mathrm{th}1\_X}\\ a_{\mathrm{th}1\_Y}\\ -g+a_{\mathrm{th}1\_Z}\end{array}\right]---\left(21\right).$

同样，在传感器单元S₂中，根据全局坐标系C与局部坐标系C₂的关系，使用式(1)的旋转矩阵，成为

【数学式22】

${{\stackrel{\·}{a}}_{\mathrm{obs}2}}^{\′}={\left(C_{B2}^G\right)}^T{\stackrel{\·}{a}}_{\mathrm{th}2}$

$=\left[\begin{array}{ccc}-\sin {\mathrm{\γ}}_2\·\sin {\mathrm{\α}}_2\·\sin {\mathrm{\β}}_2+\cos {\mathrm{\β}}_2\·\cos {\mathrm{\γ}}_2& -\sin {\mathrm{\γ}}_2\·\cos {\mathrm{\α}}_2& \sin {\mathrm{\γ}}_2\·\sin {\mathrm{\α}}_2\·\cos {\mathrm{\β}}_2+\sin {\mathrm{\β}}_2\·\cos {\mathrm{\γ}}_2\\ \cos {\mathrm{\γ}}_2\·\sin {\mathrm{\α}}_2\·\sin {\mathrm{\β}}_2+\cos {\mathrm{\β}}_2\·\sin {\mathrm{\γ}}_2& \cos {\mathrm{\γ}}_2\·\cos {\mathrm{\α}}_2& -\cos {\mathrm{\γ}}_2\·\sin {\mathrm{\α}}_2\·\cos {\mathrm{\β}}_2+\sin {\mathrm{\β}}_2\·\sin {\mathrm{\γ}}_2\\ -\cos {\mathrm{\α}}_2\·\sin {\mathrm{\β}}_2& \sin {\mathrm{\α}}_2& \cos {\mathrm{\α}}_2\·\cos {\mathrm{\β}}_2\end{array}\right]$

$\left[\begin{array}{c}{a}_{\mathrm{th}2\_X}\\ a_{\mathrm{th}2\_Y}\\ -g+a_{\mathrm{th}2\_Z}\end{array}\right]---\left(22\right).$

利用式(20.1)的关系，解出式(21)、(22)，由此，求出传感器单元S₁、S₂测量到的重力加速度以外的加速度矢量的全局坐标系C的X轴方向分量a_{th1_x}(a_{th2_X})、Y轴方向分量a_{th1_Y}(a_{th2_Y})、Z轴方向分量a_{th1_Z}(a_{th2_Z})。使用式(1)的旋转矩阵，由式(13)所示的地磁传感器B_i测量的地磁矢量的全局坐标系C的各个轴方向分量与由式(9)所示的地磁矢量的局部坐标系C_i的各个轴方向分量的关系成为

【数学式23】

${\stackrel{\·}{M}}_{\mathrm{obsi}}={\left(C_{\mathrm{Bi}}^G\right)}^T{\stackrel{\·}{M}}_{\mathrm{th}}$

$=\left[\begin{array}{ccc}-\sin {\mathrm{\γ}}_i\·\sin {\mathrm{\α}}_i\·\sin {\mathrm{\β}}_i+\cos {\mathrm{\β}}_i\·\cos {\mathrm{\γ}}_i& -\sin {\mathrm{\γ}}_i\·\cos {\mathrm{\α}}_i& \sin {\mathrm{\γ}}_i\·\sin {\mathrm{\α}}_i\·\cos {\mathrm{\β}}_i+\sin {\mathrm{\β}}_i\·\cos {\mathrm{\γ}}_i\\ \cos {\mathrm{\γ}}_i\·\sin {\mathrm{\α}}_i\·\sin {\mathrm{\β}}_i+{\cos \mathrm{\β}}_i\·\sin {\mathrm{\γ}}_i& \cos {\mathrm{\γ}}_i\·\cos {\mathrm{\α}}_i& -\cos {\mathrm{\γ}}_i\·\sin {\mathrm{\α}}_i\·\cos {\mathrm{\β}}_i+\sin {\mathrm{\β}}_i\·\sin {\mathrm{\γ}}_i\\ -\cos {\mathrm{\α}}_i\·\sin {\mathrm{\β}}_i& \sin {\mathrm{\α}}_i& \cos {\mathrm{\α}}_i\·\cos {\mathrm{\β}}_i\end{array}\right]$

$\left[\begin{array}{c}{E}_X\\ E_Y\\ E_Z\end{array}\right]---\left(23\right).$

在式(23)中，在包括代入了i＝1(或2)的地磁的关系式在内求解式(20.1)(21)(22)的情况下，相对于12个方程式，未知数除了6个姿态角之外还有6个加速度，合计为12个。其结果，能够通过数值运算解出传感器单元S₁、S₂测量到的重力加速度以外的加速度矢量的全局坐标系C的X轴方向分量a_{th1_x}(a_{th2_X})、Y轴方向分量a_{th1_Y}(a_{th2_Y})、Z轴方向分量a_{th1_Z}(a_{th2_z})。对于其它传感器单元S_i也同样求出各个传感器单元S_i测量到的重力加速度以外的加速度矢量的全局坐标系C的X轴方向分量a_{thi_x}、Y轴方向分量a_{thi_Y}、Z轴方向分量a_{thi_Z}。

在加速度检测部90上连接着位移计算部92。位移计算部92基于由加速度检测部90检测到的重力加速度以外的加速度矢量，计算各个传感器单元S_i自上次的静止状态开始的位移(步骤S133)。通过对各个传感器单元S_i测量到的重力加速度以外的加速度矢量的全局坐标系C的X轴方向分量a_{thi_x}、Y轴方向分量a_{thi_Y}、Z轴方向分量a_{thi_Y}进行双重积分，求出各个传感器单元S_i自上次的静止状态开始的位移的全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量。并且，在位移计算部92中，还计算各个传感器单元S_i的速度矢量的全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量。通过分别对各个传感器单元S_i测量到的重力加速度以外的加速度矢量的全局坐标系C的X轴方向分量a_{thi_x}、Y轴方向分量a_{thi_Y}、Z轴方向分量a_{thi_Z}进行1次积分，求出各个传感器单元S_i的速度矢量的全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量。

位移计算部92与移动状态检测部94连接。移动状态检测部94连接在曲线形状检测部50上。如图17所示，在对运动状态下的插入部11的形状进行检测时，通过前述的方法(图4的步骤S101～S106)对上次的静止状态下的插入部11的检测曲线形状65进行检测。上次的静止状态下的插入部11的检测曲线形状65从曲线形状检测部50输入到移动状态检测部94。并且，各个传感器单元S_i自上次的静止状态开始的位移的全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量的计算结果从位移计算部92输入至移动状态检测部94。移动状态检测部94基于上次的静止状态下的插入部11的检测曲线形状65以及各个传感器单元S_i自上次的静止状态开始的位移，来检测插入部11自上次的静止状态开始的移动状态(步骤S134)。由此，检测运动状态下的内窥镜10的插入部11的形状。

在移动状态检测部94上连接着描绘部45。由移动状态检测部94检测到的在全局坐标系C中的插入部11的运动状态下的检测形状通过描绘部45进行描绘。手术操作者能够在显示部47上确认由描绘部45描绘出的运动状态下的检测形状。

并且，在位移计算部92上连接着滞留状态检测部96。在滞留状态检测部96上连接着警告部98。在滞留状态检测部96中，基于各个传感器单元S_i的加速度矢量、速度矢量、位移的全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量，来检测插入部11的滞留状态(步骤S135)。即，例如在利用内窥镜10观察S状结肠时，检测插入部11在S-top等处滞留的状态。由此，手术操作者能够确认操作部12的操作是否传递至插入部11。在插入部11滞留的情况下(步骤S135-是)，由警告部98进行警告显示(步骤S136)。在插入部11未滞留的情况下(步骤S135-否)，不进行警告显示，进入步骤S107(参照图4)。

如上所述，在本变形例的内窥镜形状检测装置1中，利用加速度检测部90对在各个传感器单元S_i的中心测量的加速度矢量的全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量进行检测。进而，在位移计算部92中，还计算各个传感器单元S_i的位移以及速度矢量的全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量。移动状态检测部94根据计算出的加速度矢量、速度矢量、位移以及上次的静止状态下的检测曲线形状65，来检测运动状态下的插入部11的形状。并且，滞留状态检测部96根据计算出的加速度矢量、速度矢量、位移，来检测插入部11的滞留状态。进而，在检测到滞留状态的情况下，由警告部98进行警告显示。通过这样构成，手术操作者能够确认操作部12的操作是否传递至插入部11。由此，手术操作者在操作内窥镜10时，能够进行更恰当的判断。

(其它变形例)

另外，在第一实施方式中，首先，曲线插补部51对所有链T_j进行曲线插补，形成圆弧L_j。进而，圆弧位置校正部53使各个圆弧L_j平行移动为与邻接的圆弧L_j-1、圆弧L_j+1的边界连续的状态，以对圆弧L_j的位置进行校正。另一方面，在第二实施方式中，利用插补顺序实施部71，针对各个链T_j从基端侧的链T_j起依次进行曲线插补。进而，每当对一个链T_j进行曲线插补时，就使插补未完成部85平行移动为与插补完成部83连续的状态，以对插补未完成部85的位置进行校正。即，由第一实施方式的曲线形状检测部50检测到的检测曲线形状65和由第二实施方式的曲线形状检测部70检测到的检测曲线形状81为同一曲线形状。根据以上所说明的，虽然电路结构等复杂，但本发明可以具备组合第一实施方式和第二实施方式而得到的曲线形状检测部。在该曲线形状检测部中，例如，首先，与第一实施方式同样进行曲线插补直至链T_k为止，形成圆弧L₁～L_k，然后，进行圆弧L₁～L_k的位置校正。进而，从链T_k+1起，与第二实施方式同样地每当对一个链T_j进行曲线插补时，就进行插补未完成部85的位置校正。

并且，在上述实施方式中，各个局部坐标系C_i是Y_i轴方向与各个传感器单元S_i的中心处的长度方向一致的坐标系。但是，在本发明中，各个局部坐标系C_i是以传感器单元S_i的中心为原点并且X_i轴、Y_i轴、Z_i轴中的任意一个轴是轴方向与传感器单元S_i的中心处的长度方向一致的长度方向轴的坐标系即可。其中，当X_i轴是长度方向轴时，以式(16.1)、式(16.2)代替e_yk-1，使用

【数学式24】

e_xk-1＝[1 0 0]^T                             (24)。

同样，当Z_i轴是长度方向轴时，以式(16.1)、式(16.2)代替e_yk-1，使用

【数学式25】

e_zk-1＝[0 0 1]^T                             (25)。

这里，e_yk-1是局部坐标系C_k-1的原点处的长度方向即X_k-1轴方向的单位矢量，e_zk-1是局部坐标系C_k-1的原点处的长度方向即Z_k-1轴方向的单位矢量。

并且，在上述实施方式中，全局坐标系C是这样的坐标系：以最靠基端侧的传感器单元S₀的中心为原点，Z轴与铅垂方向一致，X轴和Y轴配置于水平面上。但是，在本发明中，是这样的坐标系即可：X轴、Y轴、Z轴中的任意一个轴是轴方向与铅垂方向一致的铅垂方向轴，铅垂方向轴以外的两个轴是配置于水平面上的水平方向轴。由此，姿态检测部30能够基于由加速度传感器A_i测量的重力加速度和由地磁传感器B_i测量的地磁，来检测各个传感器单元S_i的姿态。其中，在X轴为铅垂方向轴的情况下，式(2)所示的重力加速度矢量的全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量为

【数学式26】

${\stackrel{\·}{a}}_{\mathrm{th}}={\left[\begin{array}{ccc}-g& 0& 0\end{array}\right]}^T---\left(26\right).$

在第一实施方式中，姿态角α_i、β_i、γ_i是按照偏摆角γ_i、俯仰角α_i、滚转角β_i的顺序旋转的(Z，X，Y)型，但在该情况下，改变姿态角α_i、β_i、γ_i的旋转顺序，而使用与式(1)的旋转矩阵不同的旋转矩阵。由此，基于由加速度传感器A_i测量的加速度数据，利用第一角度计算部34计算绕水平方向轴即Y轴、Z轴的姿态角β_i、γ_i。并且，基于由地磁传感器B_i测量的地磁数据，利用第二角度计算部36计算绕铅垂方向轴即X轴的姿态角α_i。对于Y轴为铅垂方向轴的情况也同样，基于由加速度传感器A_i测量的加速度数据，利用第一角度计算部34计算绕水平方向轴即X轴、Z轴的姿态角α_i、γ_i。并且，基于由地磁传感器B_i测量的地磁数据，利用第二角度计算部36计算绕铅垂方向轴即Y轴的姿态角β_i。

另外，在上述实施方式中，全局坐标系C以最靠基端侧的传感器单元S₀的中心为原点，但也可以以最靠前端侧的传感器单元S_N的中心为原点。在该情况下，链形成部41使用式(16.1)、式(16.2)，求出链T_j的前端侧(离全局坐标系C的原点较近的一侧)的传感器单元S_j位于全局坐标系C的原点时的链T_j的基端侧(离全局坐标系C的原点较远的一侧)的传感器单元S_j-1的坐标P′_j(I_xj，I_yj，I_zj)。进而，通过将全局坐标系C的原点与坐标P′_j(I_xj，I_yj，I_zj)呈直线状连结，形成链T_j。

并且，在该情况下，在利用曲线形状检测部50的曲线插补部51进行链(插补对象链)T_j的曲线插补时，法线形成部55形成：第一插补法线N_j，其通过链T_j的前端(离全局坐标系C的原点较近的一侧的端部)即第一端点，并与链T_j垂直；以及第二插补法线N_j-1，其通过链T_j的基端(离全局坐标系C的原点较远的一侧的端部)即第二端点，并垂直于与链T_j的基端侧(离全局坐标系C的原点较远的一侧)邻接的链(插补对象邻接链)T_j-1。进而，中心确定部57计算第一插补法线N_j与第二插补法线N_j-1的交点O_j、或者计算第一插补法线N_j与第二插补法线N_j-1的距离最小的两点的中间点O_j，并将该交点O_j或中间点O_j确定为圆弧L_j的中心O_j。进而，圆弧形成部59形成圆弧L_j，该圆弧L_j的半径R_j与中心O_j和链(插补对象链)T_j的前端之间的距离相同，且以链T_j的前端为起点的弧长与传感器间尺寸I相等。

另外，在该情况下，曲线形状检测部70的插补顺序实施部71针对各个链T_j从前端侧(离全局坐标系C的原点较近的一侧)的链T_j起依次进行曲线插补，形成圆弧L_j。

以上，说明了本发明的实施方式，但本发明不限于上述实施方式，在不脱离本发明主旨的范围内当然能够进行各种变形。

Claims (16)

1.一种内窥镜形状检测装置(1)，其具备：

内窥镜(10)，其具有插入部(11)，该插入部(11)以沿长度方向彼此离开预定的传感器间尺寸(I)的方式配置有多个传感器单元(S_i)；

姿态检测部(30)，其基于所述传感器单元(S_i)中的测量数据来检测各个所述传感器单元(S_i)的姿态；

线形形状检测部(40)，其基于由所述姿态检测部(30)检测到的各个所述传感器单元(S_i)的姿态，将各个所述传感器单元(S_i)之间的形状假定为尺寸与所述传感器间尺寸(I)相等的直线状的链(T_j)，来检测所述内窥镜(10)的所述插入部(11)的检测线形形状(61)；以及

曲线形状检测部(50、70)，其将各个所述传感器单元(S_i)之间的形状假定为弧长与所述传感器间尺寸(I)相等的圆弧(L_j)，来对由所述线形形状检测部(40)检测到的所述检测线形形状(61)进行曲线插补，并对检测曲线形状(65、81)进行检测。

2.根据权利要求1所述的内窥镜形状检测装置(1)，其中，

所述曲线形状检测部(50、70)具备：

曲线插补部(51、71)，其针对各个所述链(T_j)进行所述曲线插补，形成各个所述传感器单元(S_i)之间的所述圆弧(L_j)；以及

插补控制部(52、72)，其将所述曲线插补部(51、71)控制为对所有的所述链(T_j)进行所述曲线插补的状态。

3.根据权利要求2所述的内窥镜形状检测装置(1)，其中，

所述曲线插补部(51、71)具备：

法线形成部(55、75)，其在以最靠基端侧或最靠前端侧的所述传感器单元(S₁、S_N)的中心为原点的全局坐标系(C)中，形成第一插补法线(N_k-1)和第二插补法线(N_k)，所述第一插补法线(N_k-1)通过由所述曲线插补部(51、71)进行所述曲线插补的对象的所述链(T_j)即插补对象链(T_k)的离所述全局坐标(C)的所述原点较近一侧的端部即第一端点，并与所述插补对象链(T_k)垂直，所述第二插补法线(N_k)通过所述插补对象链(T_k)的离所述全局坐标(C)的所述原点较远一侧的端部即第二端点，并垂直于与所述插补对象链(T_k)的离所述全局坐标(C)的所述原点较远一侧邻接的所述链(T_j)即插补对象邻接链(T_k+1)；

中心确定部(57、77)，其计算由所述法线形成部(55、75)形成的所述第一插补法线(N_k-1)与所述第二插补法线(N_k)的交点，或者计算所述第一插补法线(N_k-1)与所述第二插补法线(N_k)之间的距离最小的两点的中间点，并将所述交点或所述中间点确定为所述圆弧(L_k)的中心(O_k)；以及

圆弧形成部(59、79)，其形成所述圆弧(L_k)，该圆弧(L_k)的半径(R_k)与所述中心(O_k)和所述插补对象链(T_k)的所述第一端点之间的距离相同，并且该圆弧以所述插补对象链(T_k)的所述第一端点为起点。

4.根据权利要求2所述的内窥镜形状检测装置(1)，其中，

所述曲线形状检测部(50)具备圆弧位置校正部(53)，该圆弧位置校正部(53)使通过所述曲线插补部(51)形成的各个所述圆弧(L_j)平行移动为与邻接的所述圆弧(L_j-1、L_j+1)的圆弧边界连续的状态，对所述圆弧(L_j)的位置进行校正。

5.根据权利要求2所述的内窥镜形状检测装置(1)，其中，

所述曲线插补部(71)是插补顺序实施部(71)，该插补顺序实施部(71)在以最靠基端侧或最靠前端侧的所述传感器单元(S₁、S_N)的中心为原点的全局坐标系(C)中，从离所述全局坐标系(C)的所述原点较近一侧的所述链(T_j)起依次进行所述曲线插补，形成所述圆弧(L_j)，

所述曲线形状检测部(70)具备未插补链位置校正部(73)，该未插补链位置校正部(73)每当所述插补顺序实施部(71)进行一个所述链(T_j)的所述曲线插补时，使由未进行曲线插补的所述链(T_j)即未插补链构成的插补未完成部(85)平行移动为与由通过所述曲线插补形成的所述圆弧(L_j)构成的插补完成部(83)边界连续的状态，对所述插补未完成部(85)的位置进行校正。

6.根据权利要求1所述的内窥镜形状检测装置(1)，其中，

各个所述传感器单元(S_i)具备：

加速度传感器(A_i)，其在以所述传感器单元(S_i)的中心为原点并且X轴、Y轴、Z轴中的任意一个轴是轴方向与所述传感器单元(S_i)的所述中心处的所述内窥镜(10)的长度方向一致的长度方向轴的局部坐标系(C_i)中，测量所述局部坐标系(C_i)的所述原点处的加速度的三个轴方向分量；以及

地磁传感器(B_i)，其在所述局部坐标系(C_i)中，测量所述局部坐标系(C_i)的所述原点处的地磁的三个轴方向分量。

7.根据权利要求6所述的内窥镜形状检测装置(1)，其中，

所述姿态检测部(30)具备姿态角计算部(32)，该姿态角计算部(32)以全局坐标系(C)为基准，计算各个所述传感器单元(S_i)的所述局部坐标系(C_i)的绕铅垂方向轴及绕各水平方向轴的旋转角即三个姿态角(α_i、β_i、γ_i)，所述全局坐标系(C)以最靠基端侧或最靠前端侧的所述传感器单元(S_i)的中心为原点，X轴、Y轴、Z轴中的任意一个轴是轴方向与铅垂方向一致的所述铅垂方向轴，所述铅垂方向轴以外的两个轴是配置于水平面上的所述水平方向轴。

8.根据权利要求7所述的内窥镜形状检测装置(1)，其中，

所述姿态角计算部(32)具备：

第一角度计算部(34)，其基于由所述加速度传感器(A_i)测量到的加速度数据，计算各个所述传感器单元(S_i)的所述局部坐标系(C_i)相对于所述全局坐标系(C)的绕各个所述水平方向轴的两个所述姿态角(α_i、β_i)；以及

第二角度计算部(36)，其基于由所述地磁传感器(B_i)测量到的地磁数据，计算各个所述传感器单元(S_i)的所述局部坐标系(C_i)相对于所述全局坐标系(C)的绕所述铅垂方向轴的所述姿态角(γ_i)。

9.一种内窥镜(10)的插入部(11)的形状检测方法，该方法包括如下步骤：

利用在内窥镜(10)的插入部(11)中以沿长度方向彼此离开预定的传感器间尺寸(I)的方式配置的多个传感器单元(S_i)进行测量；

基于所述传感器单元(S_i)中的测量数据来检测各个所述传感器单元(S_i)的姿态；

基于检测到的各个所述传感器单元(S_i)的姿态，将各个所述传感器单元(S_i)之间的形状假定为尺寸与所述传感器间尺寸(I)相等的直线状的链(T_j)，来检测所述内窥镜(10)的所述插入部(11)的检测线形形状(61)；以及

将各个所述传感器单元(S_i)之间的形状假定为弧长与所述传感器间尺寸(I)相等的圆弧(L_j)，来对检测到的所述检测线形形状(61)进行曲线插补，形成检测曲线形状(65、81)。

10.根据权利要求9所述的内窥镜(10)的插入部(11)的形状检测方法，其中，

对所述检测线形形状(61)进行曲线插补，形成检测曲线形状的步骤(65、81)包括如下步骤：

对所有的所述链(T_j)进行如下处理：针对各个所述链(T_j)进行所述曲线插补，形成各个所述传感器单元(S_i)之间的所述圆弧(L_j)。

11.根据权利要求10所述的内窥镜(10)的插入部(11)的形状检测方法，其中，

针对各个所述链(T_j)进行所述曲线插补的步骤包括如下步骤：

在以最靠基端侧或最靠前端侧的所述传感器单元(S₁、S_N)的中心为原点的全局坐标系(C)中，形成第一插补法线(N_k-1)和第二插补法线(N_k)，所述第一插补法线(N_k-1)通过进行所述曲线插补的对象的所述链(T_j)即插补对象链(T_k)的离所述全局坐标(C)的所述原点较近一侧的端部即第一端点，并与所述插补对象链(T_k)垂直，所述第二插补法线(N_k)通过所述插补对象链的离所述全局坐标的所述原点较远一侧的端部即第二端点，并垂直于与所述插补对象链(T_k)的离所述全局坐标(C)的所述原点较远一侧邻接的所述链(T_j)即插补对象邻接链(T_k+1)；

计算所形成的所述第一插补法线(N_k-1)与所述第二插补法线(N_k)的交点，或者计算所述第一插补法线(N_k-1)与所述第二插补法线(N_k)之间的距离最小的两点的中间点，并将所述交点或所述中间点确定为所述圆弧(L_k)的中心(O_k)；以及

形成所述圆弧(L_k)，该圆弧(L_k)的半径(R_k)与所述中心(O_k)和所述插补对象链(T_k)的所述第一端点之间的距离相同，并且该圆弧以所述插补对象链(T_k)的第一端点为起点。

12.根据权利要求10所述的内窥镜(10)的插入部(11)的形状检测方法，其中，

对所述检测线形形状(61)进行曲线插补，形成检测曲线形状(65)的步骤包括如下步骤：

使针对各个所述链(T_j)进行所述曲线插补而形成的各个所述圆弧(L_j)平行移动为与邻接的所述圆弧(L_j-1、L_j+1)的圆弧边界连续的状态，对所述圆弧(L_j)的位置进行校正。

13.根据权利要求10所述的内窥镜(10)的插入部(11)的形状检测方法，其中，

针对各个所述链(T_j)进行所述曲线插补的步骤包括如下步骤：在以最靠基端侧或最靠前端侧的所述传感器单元(S₁、S_N)的中心为原点的全局坐标系(C)中，从离所述全局坐标系(C)的所述原点较近一侧的所述链(T_j)起依次进行所述曲线插补，形成所述圆弧(L_j)；

对所述检测线形形状(61)进行曲线插补，形成检测曲线形状(81)的步骤包括如下步骤：每当进行一个所述链(T_j)的所述曲线插补时，使由未进行所述曲线插补的所述链(T_j)即未插补链构成的插补未完成部(85)平行移动为与由通过所述曲线插补形成的所述圆弧(L_j)构成的插补完成部(83)边界连续的状态，对所述插补未完成部(85)的位置进行校正。

14.根据权利要求9所述的内窥镜(10)的插入部(11)的形状检测方法，其中，

利用各个所述传感器单元(S_i)进行测量的步骤包括如下步骤：

利用各个所述传感器单元(S_i)所具备的加速度传感器(A_i)，在以所述传感器单元(S_i)的中心为原点并且X轴、Y轴、Z轴中的任意一个轴是轴方向与所述传感器单元(S_i)的所述中心处的所述内窥镜(10)的长度方向一致的长度方向轴的局部坐标系(C_i)中，测量所述原点处的加速度的三个轴方向分量；以及

利用各个所述传感器单元(S_i)所具备的地磁传感器(B_i)，在所述局部坐标系(C_i)中，测量所述原点处的地磁的三个轴方向分量。

15.根据权利要求14所述的内窥镜(10)的插入部(11)的形状检测方法，其中，

对各个所述传感器单元(S_i)的姿态进行检测的步骤包括如下步骤：

以全局坐标系(C)为基准，计算各个所述传感器单元(S_i)的所述局部坐标系(C_i)的绕铅垂方向轴及绕各水平方向轴的旋转角即三个姿态角(α_i、β_i、γ_i)，所述全局坐标系(C)以最靠基端侧或最靠前端侧的所述传感器单元(S₁、S_N)的中心为原点，X轴、Y轴、Z轴中的任意一个轴是轴方向与铅垂方向一致的所述铅垂方向轴，所述铅垂方向轴以外的两个轴是配置于水平面上的所述水平方向轴。

16.根据权利要求15所述的内窥镜(10)的插入部(11)的形状检测方法，其中，

计算绕所述铅垂方向轴及绕各所述水平方向轴的三个姿态角(α_i、β_i、γ_i)的步骤包括如下步骤：

基于由所述加速度传感器(A_i)测量到的加速度数据，计算各个所述传感器单元(S_i)的所述局部坐标系(C_i)相对于所述全局坐标系(C)的绕各所述水平方向轴的两个所述姿态角(α_i、β_i)；以及

基于由所述地磁传感器(B_i)测量到的地磁数据，计算各个所述传感器单元(S_i)的所述局部坐标系(C_i)相对于所述全局坐标系(C)的绕所述铅垂方向轴的所述姿态角(γ_i)。

Images