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

Abstract

内窥镜形状检测装置(1)具备：姿态检测部(30)，其基于传感器单元(S_i)中的测量数据来检测各个所述传感器单元(S_i)的姿态；以及线形形状检测部(40)，其基于由所述姿态检测部(30)检测到的各个所述传感器单元(S_i)的姿态，将各个所述传感器单元(S_i)之间的形状假定为尺寸与所述传感器间尺寸(I)相等的直线状的链(T_j)，来检测所述内窥镜(10)的所述插入部(11)的检测线形形状(71)。并且，内窥镜形状检测装置(1)具备：形状修正部(50、150)，其使用粒子滤波器对各个所述传感器单元(S_i)的至少位置进行校正，检测对由所述线形形状检测部(40)检测到的所述检测线形形状(71)进行修正后的修正形状(75、175)。

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是示出在利用第一实施方式的形状修正部的传感器位置观测部的第1次的观测时，用粒子散布部散布的各个粒子的散布状态的示意图。

图11是说明第一实施方式的形状修正部的传感器位置观测部的权重计算部中的处理的示意图。

图12是说明第一实施方式的形状修正部的传感器位置观测部的传感器位置估计部中的处理的示意图。

图13A是示出在利用第一实施方式的形状修正部的传感器位置观测部的第2次及以后的观测中，在当前观测的前1次观测时散布的粒子的散布状态的示意图。

图13B是示出在利用第一实施方式的形状修正部的传感器位置观测部的第2次及以后的观测中，在当前观测时散布的粒子的散布状态的示意图。

图14是说明第一实施方式的形状修正部的传感器位置校正部和修正链形成部中的处理的示意图。

图15是说明第一实施方式的形状修正部的未修正链位置校正部中的处理的示意图。

图16是示出本发明的第二实施方式的内窥镜形状检测装置的形状修正部的结构的框图。

图17是示出利用第二实施方式的内窥镜形状检测装置的形状修正部进行形状修正后的修正形状的示意图。

图18是示出第二实施方式的利用形状修正部修正检测线形形状的形状的方法的流程图。

图19是说明第二实施方式的形状修正部的传感器位置观测部的粒子散布部和曲线插补部中的处理的示意图。

图20是说明第二实施方式的形状修正部的传感器位置观测部的权重计算部中的处理的示意图。

图21是说明第二实施方式的形状修正部的传感器位置观测部的传感器位置估计部中的处理的示意图。

具体实施方式

(第一实施方式)

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

图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]$

                                                              (1)。

在插入部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\begin{array}{cc}\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}\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轴方向成为从磁北方向绕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的检测线形形状71时的图。如图6所示，线形形状检测部40基于由姿态检测部30检测到的各个传感器单元S_i的姿态，将各个传感器单元S_i之间的形状假定为尺寸与传感器间尺寸I相等的直线状的链T_j(j＝1，2，...N)，来检测插入部11的检测线形形状71。这里，从基端侧起第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的检测线形形状71通过描绘部45进行描绘。手术操作者能够在显示部47上确认由描绘部45描绘出的检测线形形状71。

这里，对利用线形形状检测部40检测插入部11的检测线形形状71的方法进行说明。如图4所示，在检测插入部11的检测线形形状71时，首先，基于在步骤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】

$i_k={\left[\begin{array}{ccc}{i}_{\mathrm{xk}}& i_{\mathrm{yk}}& i_{\mathrm{zk}}\end{array}\right]}^T-i{C}_{\mathrm{Bk}-1}^G{e}_{\mathrm{yk}-1}=1\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为止完成位置校正，形成链位置校正完成部73。在进行链T_k的位置校正时，链位置校正部42使链T_k平行移动从原点到链位置校正完成部73的前端的移动量。即，使临时链T_k从图7的虚线所示的位置平行移动到图7的实线所示的位置。由此，成为链T_k-1与链T_k的链边界连续的状态，链T_k的位置被校正。

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

如图3所示，线形形状检测部40与形状修正部50连接。形状修正部50通过粒子滤波器对各个传感器单元S_i的位置(状态)进行校正，对由线形形状检测部40检测到的检测线形形状71进行修正。图8是从全局坐标系C的Z轴的正方向观察利用形状修正部50修正后的内窥镜10的插入部11的修正形状75时的图。如图8所示，通过形状修正部50将图8中虚线所示的检测线形形状71修正为修正形状75。通过校正各个传感器单元S_i的位置，修正各个链T_j的形状来确定修正链(修正传感器间要素)T′_j。修正形状75由各个修正链T′_j构成。

形状修正部50具备：形状修正顺序实施部51，其针对各个链T_j从基端侧(离全局坐标系C的原点较近的一侧)的链T_j起依次进行形状修正，形成修正链T′_j；以及形状修正控制部52，其将形状修正顺序实施部51控制为对所有链T_j进行形状修正的状态。并且，形状修正部50具备未修正链位置校正部53，该未修正链位置校正部53每当形状修正顺序实施部51进行一个链T_j的形状修正时，使由未进行形状修正的链(未修正链)T_j构成的修正未完成部79平行移动，对位置进行校正。利用未修正链位置校正部53，修正未完成部79平行移动成与由通过形状修正而形成的修正链T′_j构成的修正完成部77边界连续的状态。

如图3所示，形状修正顺序实施部51具备：传感器位置校正部(传感器状态校正部)55，其对修正对象的链(修正对象链)T_j的前端侧(离全局坐标系C的原点较远的一侧)的传感器单元(状态校正对象传感器)S_j的位置(状态)进行校正；以及修正链形成部(修正传感器间要素形成部)57，其基于由传感器位置校正部55进行了校正的传感器单元S_j的位置形成修正链T′_j。

此外，形状修正顺序实施部51具备使用粒子滤波器进行修正对象的链T_j的前端侧的传感器单元S_j的位置估计的传感器位置估计部(传感器状态估计部)61。传感器位置估计部61基于位于链T_j的前端的初始位置(初始状态)，进行第1次的传感器单元S_j的位置估计。此外，传感器位置估计部61基于第1次的估计结果，进行第2次的传感器单元S_j的位置估计。即，传感器位置估计部61使用粒子滤波器，基于初始位置或上次估计的估计结果进行传感器单元S_j的位置估计。并且，形状修正顺序实施部51具备将传感器位置估计部61控制成进行预定次数的传感器单元S_j的位置估计的状态的位置估计控制部(状态估计控制部)62。

传感器位置估计部61具备粒子散布部63、粒子移动部65、权重计算部67和传感器位置观测部(传感器状态观测部)69。粒子散布部63、粒子移动部65、权重计算部67和传感器位置观测部69的详细情况将在后面叙述。

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

这里，说明利用形状修正部50对由线形形状检测部40检测到的检测线形形状71进行修正的方法。如图4所示，形状修正部50通过粒子滤波器对各个传感器单元S_i的位置进行校正，对由线形形状检测部40检测到的检测线形形状71进行修正(步骤S106)。如前所述，若传感器间尺寸I处于小于50mm左右的范围，则在将各个传感器单元S_i之间假定为尺寸与传感器间尺寸I相等的直线状的链T_j的情况下，插入部11的形状检测时的误差也变小。但是实际上，各个传感器单元S_i的测量数据由于噪声等具有误差，并且插入到体腔内时的内窥镜10的插入部11呈现出曲线形状。因此，插入部11的形状不限于沿着基端侧的传感器单元S_j-1的中心处的长度方向，从基端侧的传感器单元S_j-1的中心延伸设置到前端侧的传感器单元S_j的中心的直线状的链形状。因此，进行检测线形形状71的形状修正较为重要。

在对各个传感器单元S_i的位置进行校正来修正检测线形形状71时，形状修正顺序实施部51针对各个链T_j从基端侧(离全局坐标系C的原点较近的一侧)的链T_j起依次进行形状修正，形成修正链T′_j。这里，对利用形状修正顺序实施部51进行链T_j的形状修正的方法进行说明。这里，对从基端侧起第k个传感器单元S_k-1与从基端侧起第(k+1)个传感器单元S_k之间的、从基端侧起第k个链T_k的形状修正进行说明。

图9是示出利用形状修正部50修正检测线形形状71的形状的方法的流程图。图10至图15是说明形状修正顺序实施部51中的处理的图。如图10所示，在利用形状修正顺序实施部51进行链T_k的形状修正之前的状态下，到链T_k-1为止完成形状修正，形成修正完成部77。修正完成部77由修正链T′₁～T′_k-1构成。此外，形成由链T_k～T_N构成的修正未完成部79。此时，修正完成部77与修正未完成部79的边界连续，传感器单元S_k在全局坐标系C中位于点Q_{k_0}。

在该状态下，通过形状修正顺序实施部51开始进行在修正未完成部79中位于最基端侧的链(修正对象链)T_k的形状修正(步骤S111)。即，通过形状修正控制部52将形状修正顺序实施部51控制为进行链T_k的形状修正的状态。

在利用形状修正顺序实施部51进行链T_k的形状修正时，首先，通过传感器位置估计部61使用粒子滤波器，基于位于链T_k的前端的初始位置(Q_{k_0})，估计链T_k的前端侧的传感器单元(状态校正对象传感器)S_k的位置。如图9和图10所示，在基于初始位置估计传感器单元S_k的位置时，首先通过粒子散布部63向链T_k的前端侧的传感器单元S_k的初始位置Q_{k_0}的周边散布预定数量(在本实施方式中设为A个)的粒子72(步骤S112)。这里，将传感器单元S_k的初始位置(即粒子滤波器中的状态向量)设为Q_{k_0}(q_{xk_0}，q_{yk_0}，q_{zk_0})，将初始位置Q_{k_0}的误差的方差设为(δ_x，δ_y，δ_z)。在求出δ_x、δ_y、δ_z时，根据由于噪声等产生的各个传感器单元S_i的测量数据的误差的方差，使用式(1)～式(15)求出传感器单元S_k-1的姿态角α_k-1、β_k-1、γ_k-1的误差的方差。并且，根据姿态角α_k-1、β_k-1、γ_k-1的误差的方差，使用式(16)求出传感器单元S_k的初始位置Q_{k_0}的误差的方差δ_x、δ_y、δ_z。在对传感器单元S_k的初始位置Q_{k_0}加上了误差的方差δ_x、δ_y、δ_z后的范围内同样散布粒子72。即，在将各个粒子72在全局坐标系C中的位置设为Q_{k_1} ^a(q_{xk_1} ^a，q_{yk_1} ^a，q_{zk_1} ^a)(a＝1，2，...，A)时，在下式的范围内同样散布粒子72：

【数学式17】

q_{xk_0}-δ_x≤q_{xk_1} ^a≤q_{xk_0}+δ_x    (17.1)

q_{yk_0}-δ_y≤q_{yk_1} ^a≤q_{yk_0}+δ_y    (17.2)

q_{zk_0}-δ_z≤q_{zk_1} ^a≤q_{zk_0}+δ_z    (17.3)。

即，将系统的噪声假定为白噪声。另外，也可以按经验确定粒子的散布范围，但是在散布范围过大的情况下，估计精度劣化。在最初散布各个粒子72时，预先对各个粒子72赋予初始的权重w_{k_0} ^a。权重w_{k_0} ^a例如在各个粒子72中恒定地以1/A给出。这里，粒子72用状态空间的A个离散的假设来表现状态向量的概率密度函数，各个粒子72具有与概率成比例的权重w_{k_0} ^a。即，在粒子滤波器中，使用多个(在本实施方式中为A个)粒子72估计状态向量的概率密度函数。

并且，粒子移动部65基于插入部11的状态(移动状态、弯曲状态等)和传感器单元S_k中的测量数据检测各个粒子72的移动模型。并且，粒子移动部65根据与权重w_{k_0} ^a成比例的概率选择粒子72，基于检测到的移动模型使各个粒子72移动(步骤S113)。通过粒子移动部65将各个粒子72从移动前粒子位置移动到移动后粒子位置。其中，在本实施方式中，是插入部11不移动的静止状态，并且不进行各个传感器单元S_i之间的曲线插补，因此不进行粒子72的移动。

进而，通过权重计算部67计算各个粒子72的权重w_{k_1} ^a(步骤S114)。即，将各个粒子72的权重从w_{k_0} ^a更新为w_{k_1} ^a。权重计算部67首先计算对各个粒子72的似然性进行评价的似然度v_{k_1} ^a。通过似然度表示传感器单元S_k位于各个粒子72的由粒子移动部65移动后的移动后粒子位置处的似然性。似然度为

【数学式18】

v_{k_1} ^a＝h(b_{k_1} ^a)+g(φ_{k_1} ^a)            (18)。

b_{k_1} ^a是在全局坐标系C中，位置校正后的传感器单元S_k-1与各个粒子72之间的线段的尺寸。例如图11所示，传感器单元S_k-1与粒子72A(全局坐标系C中的位置是Q_{k_1} ^s的粒子)之间的线段的尺寸为b_{k_1} ^s。这里，函数h(b_{k_1} ^a)例如为

【数学式19】

h(b_{k_1} ^a)＝exp(-|b_{k_1} ^a-b₀|)           (19)。

将b₀设为各个传感器单元S_i的传感器间尺寸I。根据式(19)，位置校正后的传感器单元S_k-1与粒子72之间的线段的尺寸与传感器间尺寸I的差越小，函数h(b_{k_1} ^a)越大，似然度越大。反之，位置校正后的传感器单元S_k-1与粒子72之间的线段的尺寸与传感器间尺寸I的差越大，函数h(b_{k_1} ^a)越小，似然度越小。

是从位置校正后的传感器单元S_k-1到各个粒子72的向量V1、与从各个粒子72到位置校正前的传感器单元S_k+1的向量V2所成的角度。例如图11所示，在粒子72A(全局坐标系C中的位置是Q_{k_1} ^s的粒子)处，向量V1与向量V2所成的角度为

这里，函数g例如为

【数学式20】

这里，c是常数，例如为4。根据式(20)，如果向量V1与V2所成的角度处于0度～±90度的范围，则

变大，似然度变大。反之，如果是除此以外的角度，则

变小，似然度变小。在将各个传感器单元S_i的传感器间尺寸I设为50mm的情况下，实际上角度

可以取0度～大致±90度的范围。该情况可以从将直径与内窥镜10的插入部11大致相同的直径为10mm的树脂制管进行弯曲后的状态容易想到。角度

的范围根据产品、插入部的位置而不同，因此期望与产品和/或插入部位置对应地进行调整。

并且，根据各个粒子72的似然度计算权重w_{k_1} ^a。各个粒子72的权重为

【数学式21】

${w_{k\_1}}^a=\frac{{v_{k\_1}}^a}{\underset{a=1}{\overset{A}{\mathrm{\Σ}}}{v_{k\_1}}^a}---\left(21\right).$

即，权重是将似然度的合计设为1来对各个粒子72的似然度进行了标准化的值。如上所述那样计算各个粒子72的权重。

进而，传感器位置观测部69基于由权重计算部67计算出的各个粒子72的权重，观测传感器单元S_k的位置(步骤S115)。此时，如图12所示，传感器位置观测部69计算各个粒子72的由粒子移动部65移动前的移动前粒子位置的加权平均，并将计算出的位置设为移动前传感器位置Q′_{k_1}。并且，根据在粒子移动部65移动各个粒子72时使用的移动模型，进行从移动前传感器位置Q′_{k_1}向移动后传感器位置Q_{k_1}的移动，并将移动后传感器位置Q_{k_1}估计为传感器单元S_k的位置。另外，在本实施方式中，由于不通过粒子移动部65进行粒子72的移动，因此移动前传感器位置Q′_{k_1}与移动后传感器位置Q_{k_1}的位置相同。

进而，通过位置估计控制部62判断是否进行了预定次数(在本实施方式中设为M次)的传感器单元S_k的位置估计(步骤S116)。在当前状态下，由于只进行一次传感器单元S_k的位置估计(步骤S116-否)，因此传感器位置估计部61被位置估计控制部62控制成再进行一次传感器单元S_k的位置估计的状态。此时，传感器位置估计部61基于第1次的估计结果，进行传感器单元S_k的位置估计。同样，在例如第t次的位置估计中，基于第(t-1)次的估计结果，进行传感器单元S_k的位置估计。即，在第2次及以后的位置估计中，传感器位置估计部61基于通过上一次位置估计的估计结果，进行传感器单元S_k的位置估计。

进而，返回步骤S112，通过粒子散布部63散布预定数量(在本实施方式中设为A个)的粒子72(步骤S112)。在第2次的位置估计中，基于在第1次的位置估计时由权重计算部67计算出的各个粒子72的权重来散布粒子72。同样例如在第t次的位置估计中，基于在第(t-1)次的位置估计时由权重计算部67计算出的各个粒子72的权重来散布粒子72。即，在第2次及以后的位置估计中，粒子散布部63基于在上一次的位置估计时由权重计算部67计算出的各个粒子72的权重来散布粒子72。即，粒子散布部63成为基于上一次的位置估计时的各个粒子72的权重来散布粒子72的权重对应散布部。结果，如图13A和图13B所示，例如在上一次的位置估计时计算出的权重较大的粒子72的移动前粒子位置的周边(图13A、图13B中的G1所示的范围)，所散布的粒子72的数量增多。另一方面，在上一次的位置估计时计算出的权重较小的粒子72的移动前粒子位置的周边(图13A、图13B中的G2所示的范围)，所散布的粒子72的数量减少。另外，图13A示出了在上一次的位置估计时散布的粒子72的由粒子移动部65移动前的移动前粒子位置，图13B示出了当前的粒子的移动前粒子位置。

并且，在第2次及以后的位置估计中也同样，粒子移动部65基于在第1次的估计中使用的移动模型，移动各个粒子72(步骤S113)。由此，将各个粒子72从移动前粒子位置移动到移动后粒子位置。

并且，通过权重计算部67，使用与式(18)～式(20)同样的式子，计算对各个粒子72的似然性进行评价的似然度。通过似然度表示传感器单元S_k位于各个粒子72的由粒子移动部65移动后的移动后粒子位置处的似然性。并且，使用与式(21)同样的式子，根据似然度计算各个粒子72的权重(步骤S114)。这里，在例如第t次的位置估计中，替代式(18)～式(21)的似然度v_{k_1} ^a、权重w_{k_1} ^a、b_{k_1} ^a、

而分别使用似然度v_{k_t} ^a、权重w_{k_t} ^a、b_{k_t} ^a、

进行计算。

进而，传感器位置观测部69基于由权重计算部67计算出的各个粒子72的权重，估计传感器单元S_k的位置(步骤S115)。此时，传感器位置观测部69计算各个粒子72的由粒子移动部65移动前的移动前粒子位置的加权平均，并将计算出的位置设为移动前传感器位置。并且，根据在粒子移动部65移动各个粒子72时使用的移动模型，进行从移动前传感器位置向移动后传感器位置的移动，并将移动后传感器位置估计为传感器单元S_k的位置。例如，在第t次的位置估计中，根据各个粒子72的加权平均计算移动前传感器位置Q′_{k_t}，并通过根据移动模型的移动从移动前传感器位置Q′_{k_t}移动到移动后传感器位置Q_{k_t}。

进而，通过位置估计控制部62判断是否进行了预定次数(在本实施方式中设为M次)的传感器单元S_k的位置估计(步骤S116)。在没有进行预定次数的传感器单元S_k的位置估计的情况下(步骤S116-否)，传感器位置估计部61被位置估计控制部62控制成再进行一次传感器单元S_k的位置估计的状态，再进行一次步骤S112～步骤S115。在进行了预定次数的传感器单元S_k的位置估计的情况下(步骤S116-是)，进行到下一步骤。

如图14所示，在进行了预定次数的传感器单元S_k的位置估计后，传感器位置校正部55根据利用传感器位置估计部61的估计结果，对修正对象的链(修正对象链)T_k的前端侧的传感器单元(状态校正对象传感器)S_k的位置进行校正(步骤S117)。传感器位置校正部55将传感器单元S_k的位置从位于链T_k的前端的初始位置(Q_{k_0})校正到在传感器位置观测部61的最终一次(在本实施方式中为第M次)的位置估计时估计出的传感器单元S_k的移动后传感器位置Q_{k_M}。并且，修正链形成部57基于由传感器位置校正部55校正后的传感器单元S_k的位置形成修正链T′_k(步骤S117)。这里，连结位置校正后的传感器单元S_k-1与传感器单元S_k的移动后传感器位置Q_{k_M}而成的线段成为修正链T′_k。由于传感器单元S_k的位置移动，在通过形状修正顺序实施部51进行链T_k的形状修正之前的状态下连续的修正完成部77与修正未完成部79的边界变为不连续的状态。即，修正链T′_k与链T_k+1成为不连续的状态。

另外，对于链T_k以外的链T_j，也利用形状修正顺序实施部51同样进行形状修正。即，在开始链(修正对象链)T_j的形状修正时(步骤S111)，传感器位置估计部61使用粒子滤波器，进行修正对象的链T_j的前端侧(离全局坐标系C的原点较远的一侧)的传感器单元(状态校正对象传感器)S_j的位置估计。在进行传感器单元S_j的位置估计时，首先粒子散布部63基于传感器单元S_j的初始位置Q_{j_0}、或在上一次的位置估计时由权重计算部67计算出的各个粒子72的权重来散布预定数量的粒子72(步骤S112)。进而，粒子移动部65检测上述移动模型，并基于移动模型移动各个粒子72(步骤S113)。由此，将各个粒子72从移动前粒子位置移动到移动后粒子位置。

并且，通过权重计算部67，使用与式(18)～式(20)同样的式子，计算对各个粒子72的似然性进行评价的似然度。通过似然度检测传感器单元S_j位于各个粒子72的由粒子移动部65移动后的移动后粒子位置处的似然性。并且，使用与式(21)同样的式子，根据似然度计算各个粒子72的权重(步骤S114)。这里，由于进行了链T_j的形状修正，因此在例如第t次的位置估计中，替代式(18)～式(21)的似然度v_{k_1} ^a、权重w_{k_1} ^a、b_{k_1} ^a、而分别使用似然度v_{j_t} ^a、权重w_{j_t} ^a、b_{j_t} ^a、

进行计算。进而，传感器位置观测部69基于由权重计算部67计算出的各个粒子72的权重，观测传感器单元S_j的位置(步骤S115)。例如，在第t次的位置估计中，根据各个粒子72的加权平均计算移动前传感器位置Q′_{j_t}，并通过根据移动模型的移动进行从移动前传感器位置Q′_{j_t}向移动后传感器位置Q_{j_t}的移动。进而，将移动后传感器位置Q_{j_t}估计为传感器单元S_j的位置。

进而，通过位置估计控制部62判断是否进行了预定次数(在本实施方式中设为M次)的传感器单元S_j的位置估计(步骤S116)。在没有进行预定次数的传感器单元S_j的位置估计的情况下(步骤S116-否)，传感器位置估计部61被位置估计控制部62控制成进行预定次数的传感器单元S_j的位置估计的状态，再进行一次步骤S112～步骤S115。在进行了预定次数的传感器单元S_j的位置估计的情况下(步骤S116-是)，进入下一步骤。

在通过传感器位置估计部61进行了预定次数的传感器单元S_j的位置估计后，传感器位置校正部55根据传感器位置估计部61的估计结果，对修正对象的链(修正对象链)T_j的前端侧(离全局坐标系C的原点较远的一侧)的传感器单元(状态校正对象传感器)S_j的位置进行校正(步骤S117)。传感器位置校正部55将传感器单元S_j的位置从位于链T_j的前端(全局坐标系C的较远一侧的端部)的初始位置(Q_{j_0})校正到在传感器位置观测部61的最终一次(在本实施方式中为第M次)的位置估计时估计出的传感器单元S_j的移动后传感器位置Q_{j_M}。并且，修正链形成部57基于由传感器位置校正部55校正后的传感器单元S_j的位置形成修正链T′_j(步骤S117)。这里，连结位置校正后的传感器单元S_j-1与传感器单元S_j的移动后传感器位置Q_{j_M}而成的线段成为修正链T′_j。

图15是说明未修正链位置校正部53中的处理的图。如图15所示，在利用形状修正顺序实施部51进行链T_k的形状修正、并形成了修正链T′_k后的状态下，形成有完成了形状修正的修正完成部77和未完成形状修正的修正未完成部79。修正完成部77由修正链T′₁～T′_k构成，修正未完成部79由链T_k+1～T_N构成。此时，如前所述，通过链T_k的形状修正，传感器单元S_k的位置从点Q_{k_0}移动到点Q_{k_M}，因此修正完成部77与修正未完成部79的边界不连续。在该状态下，未修正链位置校正部53使修正未完成部79平行移动到与修正完成部77边界连续的状态，从而对修正未完成部79的位置进行校正(步骤S118)。即，修正未完成部79从图15的虚线所示的位置平行移动到实线所示的位置。

另外，对于链T_k以外的链T_j，也同样利用未修正链位置校正部53校正修正未完成部79的位置。即，在利用形状修正顺序实施部51进行了链T_j的形状修正后，修正完成部77与修正未完成部79的边界不连续。在该状态下，未修正链位置校正部53使修正未完成部79平行移动到与修正完成部77连续的状态，从而对修正未完成部79的位置进行校正(步骤S118)。未修正链位置校正部53每当形状修正顺序实施部51进行一个链T_j的形状修正时，对修正未完成部79的位置进行校正。

进而，如图9所示，形状修正控制部52确认是否针对所有的链T_j完成了形状修正(步骤S119)。在针对所有的链T_j完成了形状修正的情况下，形成插入部11的修正形状75，进入下一步骤(步骤S119-是)。在没有针对所有的链T_j完成形状修正的情况下，返回步骤S111(步骤S119-否)，利用形状修正顺序实施部51，进行修正未完成部79中位于最靠基端侧(离全局坐标系C的原点较近的一侧)的链(修正对象链)T_j的形状修正。即，在针对所有的链T_j完成形状修正之前，重复进行步骤S111～S118。

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

此外，在内窥镜形状检测装置1中，通过形状修正部50，使用粒子滤波器对各个传感器单元S_i的位置进行校正，从而对检测线形形状71进行了修正。在形状修正部50中，利用形状修正顺序实施部51的传感器位置估计部61进行预定次数的各个传感器单元S_i的估计。在传感器单元S_i的估计时，通过权重计算部67计算各个粒子72的权重。进而，传感器位置观测部69基于由权重计算部67计算出的各个粒子72的权重，观测传感器单元S_i的位置。计算出的粒子72的权重越大，通过粒子移动部65移动后的各个粒子72的移动后粒子位置越位于插入到体腔内的插入部11中的传感器单元S_i的似然的位置(位于此处的概率较高的位置)。因此，能够通过传感器位置观测部69以高精度观测传感器单元S_i的位置。传感器位置校正部55将各个传感器单元S_i的位置校正到在通过传感器位置估计部61的最终一次的估计中由传感器位置观测部69观测到的位置即移动后传感器位置。因此，位置校正后的传感器单元S_i的位置成为插入到体腔内的插入部11中的传感器单元S_i的似然的位置(位于此处的概率较高的位置)。形状修正部50基于位置校正后的传感器单元S_i的位置进行检测线形形状71的修正，因此能够检测与实际的插入部11的形状的误差较小的修正形状75。由此，能够以高精度对插入部11的修正形状75进行检测。

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

(第二实施方式)

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

图16是示出本实施方式的形状修正部150的结构的图。形状修正部150通过粒子滤波器对各个传感器单元S_i的位置(状态)进行校正，对由线形形状检测部40检测到的检测线形形状71进行修正。图17是从全局坐标系C的Z轴的正方向观察利用形状修正部50修正后的内窥镜10的插入部11的修正形状175时的图。如图17所示，通过形状修正部150将用图17的虚线所示的检测线形形状71修正为修正形状175。通过校正各个传感器单元S_i的位置，修正各个链T_j的形状来确定圆弧(修正传感器间要素)L_j。修正形状175由各个圆弧L_j构成。实际上，插入到体腔内时的内窥镜10的插入部11的形状为曲线形状。这里，内窥镜10的插入部11因产品不同而有差异，但具有适度的弹性。因此，插入部11的曲线形状的曲率大幅变化的情况很少见。因此，通过将传感器单元S_i之间的形状假定为预定半径(曲率)的圆弧L_j来进行形状修正，形成与实际的插入部11的曲线形状的误差较小的修正形状75。

如图16所示，形状修正部150具备形状修正顺序实施部151、形状修正控制部152和未修正链位置校正部153。形状修正顺序实施部151针对各个链T_j从基端侧(离全局坐标系C的原点较近的一侧)的链T_j起依次进行形状修正，形成圆弧(修正传感器间要素)L_j。即，相对于第一实施方式的形状修正顺序实施部51形成修正链T′_j，本实施方式的形状修正顺序实施部151形成圆弧L_j。关于形状修正控制部152和未修正链位置校正部153，与第一实施方式的形状修正控制部52和未修正链位置校正部53为相同的结构和功能，因此省略其说明。

形状修正顺序实施部151具备：传感器位置校正部(传感器状态校正部)155，其对修正对象的链(修正对象链)T_j的前端侧(离全局坐标系C的原点较远的一侧)的传感器单元(状态校正对象传感器)S_j的位置进行校正；以及圆弧形成部(修正传感器间要素形成部)157，其基于由传感器位置校正部155进行了校正的传感器单元S_j的位置形成圆弧L_j。即，相对于第一实施方式的修正链形成部57基于位置校正后的传感器单元S_j的位置形成修正链T′_j，本实施方式的圆弧形成部157形成圆弧L_j。

形状修正顺序实施部151具备使用粒子滤波器进行修正对象的链T_j的前端侧的传感器单元S_j的位置估计的传感器位置估计部(传感器状态估计部)161。传感器位置估计部161基于位于链T_j的前端的初始位置，进行第1次的传感器单元S_j的位置估计。此外，传感器位置估计部161基于第1次的估计结果，进行第2次的传感器单元S_j的位置估计观测。即，传感器位置估计部161使用粒子滤波器，基于初始位置或上次估计的估计结果进行传感器单元S_j的位置估计。此外，形状修正顺序实施部151具备将传感器位置估计部161控制成进行预定次数的传感器单元S_j的位置估计的状态的位置估计控制部(状态估计控制部)162。

传感器位置估计部161具备粒子散布部163、作为粒子移动部的曲线插补部165、权重计算部167和传感器位置观测部(传感器状态观测部)169。粒子散布部163、曲线插补部165、权重计算部167和传感器位置观测部169的详细情况将在后面叙述。

这里，说明利用形状修正部150对由线形形状检测部40检测到的检测线形形状71进行修正的方法。如图4所示，形状修正部150通过粒子滤波器对各个传感器单元S_i的位置进行校正，对由线形形状检测部40检测到的检测线形形状71进行修正(步骤S106)。

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

图18是示出利用形状修正部150修正检测线形形状71的形状的方法的流程图。图19至图21是说明形状修正顺序实施部151中的处理的图。如图19所示，在利用形状修正顺序实施部151进行链T_k的形状修正之前的状态下，与第一实施方式同样，到链T_k-1为止完成形状修正，形成修正完成部177。此外，形成由链T_k～T_N构成的修正未完成部179。此时，修正完成部177与修正未完成部179的边界连续，传感器单元S_k在全局坐标系C中位于点U_{k_0}。

如图18和图19所示，在该状态下，通过形状修正顺序实施部151开始进行在修正未完成部179中位于最靠基端侧的链(修正对象链)T_k的形状修正(步骤S191)。即，通过形状修正控制部152将形状修正顺序实施部151控制为进行链T_k的形状修正的状态。

在利用形状修正顺序实施部51进行链T_k的形状修正时，首先，通过传感器位置估计部161使用粒子滤波器，基于位于链T_k的前端的初始位置(U_{k_0})，估计链T_k的前端侧的传感器单元(状态校正对象传感器)S_k的位置。如图18和图19所示，在基于初始位置估计传感器单元S_k的位置时，首先通过粒子散布部163向链T_k的前端侧的传感器单元S_k的初始位置U_{k_0}的周边散布预定数量(在本实施方式中设为A个)的粒子172(步骤S192)。这里，与第一实施方式同样地在式(17)的范围内同样散布各个粒子172。

进而，通过曲线插补部165进行传感器单元S_k-1与由粒子散布部163散布的各个粒子172之间的曲线插补(步骤S193)。曲线插补部165使用基于插入部11的弯曲状态的移动模型进行曲线插补，移动各个粒子172的位置。即，曲线插补部165是使用基于插入部11的弯曲状态的移动模型来移动各个粒子172的粒子移动部。通过曲线插补部165将各个粒子172从插补前粒子位置(移动前粒子位置)移动到插补后粒子位置(移动后粒子位置)。

这里，对传感器单元S_k-1与作为粒子172中的一个的粒子172A之间的曲线插补进行说明。如图19所示，在进行传感器单元S_k-1与粒子172A之间的曲线插补链T_k的曲线插补之前的状态下，粒子172A位于插补前粒子位置F1。这里，将连结传感器单元S_k-1与粒子172A的插补前粒子位置F1而成的线段设为第一线形形状171A。第一线形形状171A的尺寸为b′。此外，将连结粒子172A的插补前粒子位置F1与位于链T_k的前端的传感器单元S_k+1而成的线段设为第二线形形状171B。并且，形成第一插补法线174A和第二插补法线174B，该第一插补法线174A通过第一线形形状171A的基端，并与第一线形形状171A垂直，所述第二插补法线174B通过第一线形形状171A的前端，并与第二线形形状171B垂直。进而，计算第一插补法线174A与第二插补法线174B的交点O。在第一插补法线174A与第二插补法线174B不相交的情况下，计算第一插补法线174A与第二插补法线174B的距离最小的两点的中间点O。进而，将交点O或中间点O设为中心O来形成圆弧176。圆弧176具有与中心O和第一线形形状171A的基端之间的距离相同的半径R。即，圆弧176具有曲率1/R。此外，圆弧176是以第一线形形状171A的基端为起点的弧长与第一线形形状171A的尺寸b′相等的圆弧176。圆弧176的前端F2成为粒子172A的插补后粒子位置F2。即，通过曲线插补部165的曲线插补，粒子172A的位置从插补前粒子位置F1移动到插补后粒子位置F2。关于其他粒子，也同样从插补前粒子位置移动到插补后粒子位置。

进而，与第一实施方式同样地，通过权重计算部167计算各个粒子172的权重w_{k_1} ^a(步骤S194)。即，将各个粒子72的权重从初始的w_{k_0} ^a更新为w_{k_1} ^a。权重计算部167首先计算对各个粒子172的插补后粒子位置(移动后粒子位置)的似然性进行评价的似然度v_{k_1} ^a。使用式(18)～式(20)计算似然度。其中，在本实施方式中，b_{k_1} ^a在全局坐标系C中，是位置校正后的传感器单元S_k-1与各个粒子172的插补后粒子位置之间的圆弧的弧长。例如图20所示，在传感器单元S_k-1与粒子172A之间(在该粒子处，设为a＝s)之间，如前所述那样形成的圆弧176的弧长b′成为b_{k_1} ^s。此外，在式(19)中将b₀设为各个传感器单元S_i的传感器间尺寸I。根据式(19)，位置校正后的传感器单元S_k-1与粒子172之间的圆弧的弧长与传感器间尺寸I的差越小，函数h(b_{k_1} ^a)越大，似然度越大。反之，位置校正后的传感器单元S_k-1与粒子172之间的圆弧的弧长与传感器间尺寸I的差越大，函数h(b_{k_1} ^a)越小，似然度越小。其中，如前所述那样，位置校正后的传感器单元S_k-1与各个粒子172的插补后粒子位置之间的圆弧的弧长与位置校正后的传感器单元S_k-1与各个粒子172的插补前粒子位置之间的线段的尺寸相同，因此即使在将b_{k_1} ^a设为传感器单元S_k-1与各个粒子172的插补前粒子位置之间的线段的尺寸的情况下，也能够得到相同的结果。此外，在本实施方式中，

是从位置校正后的传感器单元S_k-1到各个粒子172的插补后粒子位置的向量V′1、与从各个粒子172的插补后粒子位置到位置校正前的传感器单元S_k+1的向量V′2所成的角度。例如图20所示，在粒子172A(在该粒子处，设为a＝s)处，从传感器单元S_k-1到粒子172A的插补后粒子位置F2的向量V′1、与从粒子172A的插补后粒子位置F2到传感器单元S_k+1的向量V′2所成的角度为

根据式(20)，如果向量V′1与V′2所成的角度处于0度～90度的范围，则

变大，似然度变大。反之，如果是除此以外的角度，则变小，似然度变小。并且，使用式(21)根据各个粒子172的似然度计算权重w_{k_1} ^a。

进而，传感器位置观测部169基于由权重计算部67计算出的各个粒子172的权重，观测传感器单元S_k的位置(步骤S195)。此时，如图21所示，传感器位置观测部169计算各个粒子172的由曲线插补部165插补前的插补前粒子位置的加权平均，并将计算出的位置设为移动前传感器位置U′_{k_1}。并且，根据在粒子移动部65进行曲线插补时使用的移动模型，进行从移动前传感器位置U′_{k_1}向移动后传感器位置U_{k_1}的移动，并将移动后传感器位置U_{k_1}估计为传感器单元S_k的位置。

进而，通过位置估计控制部162判断是否进行了预定次数(在本实施方式中设为M次)的传感器单元S_k的位置估计(步骤S196)。在当前状态下，只进行一次传感器单元S_k的位置估计(步骤S196-否)，因此传感器位置估计部161被位置估计控制部162控制成再进行一次传感器单元S_k的位置估计的状态。在第2次及以后的位置估计中，传感器位置估计部161基于通过上一次位置估计的估计结果，进行传感器单元S_k的位置估计。

进而，返回步骤S192，通过粒子散布部163散布预定数量(在本实施方式中设为A个)的粒子172(步骤S192)。在第2次及以后的位置估计中，粒子散布部163基于在上一次的位置估计时由权重计算部167计算出的各个粒子172的权重来散布粒子172。结果例如在上一次的位置估计时计算出的权重较大的粒子172的插补前粒子位置的周边，所散布的粒子172的数量增多。另一方面，在上一次的位置估计时计算出的权重较小的粒子172的插补前粒子位置的周边，所散布的粒子72的数量减少。

并且，在第2次及以后的位置估计中，也同样由曲线插补部165基于在第1次的估计中使用的移动模型，进行传感器单元S_k-1与各个粒子172之间的曲线插补(步骤S193)。由此，将各个粒子172从插补前粒子位置移动到插补后粒子位置。

并且，通过权重计算部167，使用与式(18)～式(20)同样的式子，计算对传感器单元S_k位于各个粒子172的插补后粒子位置处的似然性进行评价的似然度。通过似然度检测各个粒子172的插补后粒子位置的似然性。并且，使用与式(21)同样的式子，根据似然度计算各个粒子72的权重(步骤S194)。这里，在例如第t次的位置估计中，替代式(18)～式(21)的似然度v_{k_1} ^a、权重w_{k_1} ^a、b_{k_1} ^a、

而分别使用似然度v_{k_t} ^a、权重w_{k_t} ^a、b_{k_t} ^a、

进行计算。

进而，传感器位置观测部169基于由权重计算部167计算出的各个粒子172的权重，观测传感器单元S_k的位置(步骤S195)。例如，在第t次的位置估计中，根据各个粒子172的加权平均计算插补前传感器位置U′_{k_t}，利用上述移动模型进行曲线插补。通过曲线插补进行从插补前传感器位置U′_{k_t}向插补后传感器位置U_{k_t}的移动。

进而，通过位置估计控制部162判断是否进行了预定次数(在本实施方式中设为M次)的传感器单元S_k的位置估计(步骤S196)。在没有进行预定次数的传感器单元S_k的位置估计的情况下(步骤S196-否)，传感器位置估计部161被位置估计控制部162控制成再进行一次传感器单元S_k的位置估计的状态，再进行一次步骤S192～步骤S195。在进行了预定次数的传感器单元S_k的位置估计的情况下(步骤S196-是)，进入下一步骤。

如图17所示，在进行了预定次数(例如2次)的传感器单元S_k的位置估计后，传感器位置校正部155根据传感器位置估计部161的估计结果，对修正对象的链(修正对象链)T_k的前端侧的传感器单元(状态校正对象传感器)S_k的位置进行校正(步骤S197)。传感器位置校正部155将传感器单元S_k的位置从位于链T_k的前端的初始位置(U_{k_0})校正到在传感器位置估计部161的最终一次(在本实施方式中为第M次)的位置估计时估计出的传感器单元S_k的插补后传感器位置U_{k_M}。并且，圆弧形成部157基于由传感器位置校正部155校正后的传感器单元S_k的位置形成圆弧L_k(步骤S197)。这里，连结位置校正后的传感器单元S_k-1与传感器单元S_k的移动后传感器位置U_{k_M}而成的圆弧成为圆弧L_k。另外，通过用上述移动模型对传感器单元S_k-1与传感器单元S_k的插补前传感器位置U′_{k_M}之间进行曲线插补，形成连结位置校正后的传感器单元S_k-1与传感器单元S_k的插补后传感器位置U_{k_M}而成的圆弧。由于传感器单元S_k的位置移动，在通过形状修正顺序实施部151进行链T_k的形状修正之前的状态下连续的修正完成部177与修正未完成部179的边界变为不连续的状态。即，圆弧L_k与链T_k+1成为不连续的状态。

并且，与第一实施方式同样，在该状态下，未修正链位置校正部153使修正未完成部179平行移动到与修正完成部177的边界连续的状态，从而对修正未完成部179的位置进行校正(步骤S198)。

另外，对于链T_k以外的链T_j，也利用形状修正顺序实施部151同样进行形状修正，从而形成圆弧L_j。此外，未修正链位置校正部153每当形状修正顺序实施部151进行一个链T_j的形状修正时，对修正未完成部179的位置进行校正。

进而，如图17所示，形状修正控制部152确认是否针对所有的链T_j完成了形状修正(步骤S199)。在针对所有的链T_j完成了形状修正的情况下，形成插入部11的修正形状175，进入下一步骤(步骤S199-是)。在没有针对所有的链T_j完成形状修正的情况下，返回步骤S191(步骤S199-否)，利用形状修正顺序实施部151，进行修正未完成部179中位于最靠基端侧(离全局坐标系C的原点较近的一侧)的链(修正对象链)T_j的形状修正。即，在针对所有的链T_j完成形状修正之前，重复进行步骤S191～S198。

因此，在上述结构的内窥镜形状检测装置1及使用了内窥镜形状检测装置1的内窥镜10的插入部11的形状检测方法中，发挥以下的效果。即，在内窥镜形状检测装置1中，姿态检测部30根据各个传感器单元S_i的测量数据来检测传感器单元S_i的姿态，线形形状检测部40根据各个传感器单元S_i的姿态来检测内窥镜10的插入部11的检测线形形状71。并且，形状修正部150通过粒子滤波器对各个传感器单元S_i的位置进行校正，对由线形形状检测部40检测到的检测线形形状71进行修正。进而，形成插入部11的修正形状175。如上所述，根据观察时插入体腔内的插入部11所配置的传感器单元S_i的测量数据来检测插入部11的修正形状175，因此不需要在体外设置读出线圈等。因此，能够实现内窥镜形状检测装置1的小型化、单纯化。

此外，在内窥镜形状检测装置1中，通过形状修正部150，使用粒子滤波器对各个传感器单元S_i的位置进行校正，从而对检测线形形状71进行了修正。在形状修正部150中，利用形状修正顺序实施部151的传感器位置估计部161进行预定次数的各个传感器单元S_i的估计。在传感器单元S_i的估计时，通过权重计算部167计算各个粒子172的权重。进而，传感器位置观测部169基于由权重计算部167计算出的各个粒子172的权重，观测传感器单元S_i的位置。计算出的粒子172的权重越大，通过曲线插补部65的曲线插补而进行插补后的各个粒子172的插补后粒子位置(移动后粒子位置)越位于插入到体腔内的插入部11中的传感器单元S_i的似然的位置(位于此处的概率较高的位置)。因此，能够通过传感器位置观测部169以高精度观测传感器单元S_i的位置。传感器位置校正部155将各个传感器单元S_i的位置校正到在通过传感器位置估计部161的最终一次的估计中由传感器位置观测部169观测到的位置即插补后传感器位置(移动后传感器位置)。因此，位置校正后的传感器单元S_i的位置成为插入到体腔内的插入部11中的传感器单元S_i的似然的位置(位于此处的概率较高的位置)。形状修正部150基于位置校正后的传感器单元S_i的位置进行检测线形形状71的修正，因此能够检测与实际的插入部11的形状的误差较小的修正形状175。由此，能够以高精度对插入部11的修正形状175进行检测。

并且，在内窥镜形状检测装置1中，通过形状修正顺序实施部151将各个链T_j的形状修正为圆弧(修正传感器间要素)L_j。实际上，插入到体腔内时的内窥镜10的插入部11的形状为曲线形状。内窥镜10的插入部11因产品不同而有差异，但具有适度的弹性。因此，插入部11的曲线形状的曲率大幅变化的情况很少见。因此，通过将传感器单元S_i之间的形状修正为预定半径(曲率)的圆弧L_j，能够检测与实际的插入部11的曲线形状的误差较小的修正形状175。

(第一实施方式的变形例)

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

在本变形例中，能够对内窥镜10的插入部11平行移动的运动状态下的插入部11的修正形状75进行检测。即，传感器位置估计部61的粒子移动部65基于由各个传感器单元S_i的加速度传感器A_i测量到的运动状态下的加速度数据，进行各个粒子72的移动。

在基于运动状态下的加速度数据进行各个粒子的移动时，首先，设置在计算机8中的加速度检测部(未图示)取得由各个传感器单元S_i的加速度传感器A_i测量到的运动状态下的加速度数据。

进而，加速度检测部基于运动状态下的加速度数据，将在各个传感器单元S_i的中心测量到的加速度向量分解为全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量。进而，对在各个传感器单元S_i的中心测量到的加速度向量的全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量进行检测。在运动状态下，对在静止状态下产生于铅垂方向的重力加速度施加由插入部11的移动产生的分量，因此，在各个传感器单元S_i的中心测量到的加速度向量的全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量为

【数学式22】

${\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(22\right).$

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

【数学式23】

${{\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(23\right).$

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

【数学式24】

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

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

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

【数学式25】

[a_{th1_X} a_{th1_Y} a_{th1_Z}]^T＝[a_{th2_X} a_{th2_Y} a_{th2_Z}]^T      (25.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$

                                                      (25.2)。

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

【数学式26】

${{\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 \mathrm{\α}}_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(26\right).$

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

【数学式27】

${{\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(27\right).$

利用式(25.1)的关系，解出式(26)、(27)，由此，求出传感器单元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的各个轴方向分量的关系成为

【数学式28】

${\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(28\right).$

在式(28)中，在包括代入了i＝1(或2)的地磁的关系式在内求解式(25.1)(26)(27)的情况下，相对于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}。

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

粒子移动部65基于由加速度检测部和位移计算部检测到的各个传感器单元S_i的加速度向量、速度向量、位移的全局坐标系C的X轴方向分量、Y轴方向分量、Z轴方向分量，检测各个粒子72的移动模型。进而，基于移动模型将各个粒子172从移动前粒子位置移动到移动后粒子位置(图9的步骤S113)。即，粒子移动部65是如下的运动状态移动模型实施部：根据在运动状态下检测到的各个传感器单元S_i的加速度向量、速度向量、位移来检测移动模型，基于移动模型来移动各个粒子72。

并且，如上所述，根据移动后粒子位置的似然性即各个粒子72的似然度，计算权重(步骤S114)。进而，根据各个粒子72的权重观测传感器单元S_i的位置(步骤S115)。进行预定次数的以上那样的传感器单元S_i的位置估计，并将传感器单元S_i的位置校正为通过最终一次的位置估计而估计出的移动后传感器位置(步骤S117)。并且，根据位置校正后的传感器单元S_i的位置，形成各个传感器单元S_i之间的形状修正后的形状即修正传感器间要素(步骤S117)。如上所述，即使在运动状态下，也能够检测对检测线形形状71进行形状修正后的修正形状75。

(其他变形例)

另外，在上述实施方式中，通过传感器位置校正部55、155进行了修正对象链T_j的前端侧的传感器单元S_j的位置校正，但例如除了位置校正以外，还可以进行传感器单元S_j的姿态校正。即，传感器位置校正部55、155只要是对修正对象链T_j的前端侧的传感器单元S_j的至少位置进行校正的传感器状态校正部即可。此时，还通过传感器位置估计部61、161，使用粒子滤波器进行传感器单元S_j的姿态估计。即，传感器位置估计部61、161是使用粒子滤波器对传感器单元S_j的至少位置进行估计的传感器状态估计部。此外，传感器位置观测部69、169是观测传感器单元S_j的至少位置的传感器状态观测部。例如，在进行传感器单元S_k的姿态估计的情况下，除了式(17.1)～式(17.3)所示的全局坐标系中的位置以外，通过粒子散布部63散布的各个粒子72具有姿态。这里，将通过式(7)、式(8)、式(12)(或式(15))求出的传感器单元S_k的局部坐标系C_k的姿态角设为α_k、β_k、γ_k，是传感器单元S_k的初始姿态(初始状态)，将各个姿态角的误差的方差设为δ_α、δ_β、δ_γ。在第1次的估计中，在对通过式(7)、式(8)、式(12)(或式(15))求出的传感器单元S_k的初始姿态(姿态角α_k、β_k、γ_k)加上了误差的方差δ_α、δ_β、δ_γ后的范围内同样散布粒子72。即，在将传感器单元S_k的第1次估计中的各个粒子72的姿态角设为α_{k_1} ^a、β_{k_1} ^a、γ_{k_1} ^a(a＝1，2，...，A)时，在下式的范围内同样散布各个粒子72：

【数学式29】

α_k-δ_α≤α_{k_1} ^a≤α_k+δ_α        (29.1)

β_k-δ_β≤β_{k_1} ^a≤β_k+δ_β        (29.2)

γ_k-δ_γ≤γ_{k_1} ^a≤γ_k+δ_γ        (29.3)。

根据由于噪声等产生的各个传感器单元S_i的测量数据的误差的方差，使用式(1)～式(15)求出误差的方差δ_α、δ_β、δ_γ。在第2次及以后的估计中，基于上一次估计时的粒子72的权重来确定各个粒子72的姿态。

并且，在上述实施方式中，各个局部坐标系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，使用

【数学式30】

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

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

【数学式31】

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

这里，e_xk-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轴方向分量为

【数学式32】

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

此外，在全局坐标系C的X轴方向上没有作用地磁。在第一实施方式中，姿态角α_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的形状修正时，从前端侧(离全局坐标系C的原点较近的一侧)的链T_j起依次进行形状修正。此时，传感器位置校正部55对修正对象的链(修正对象链)T_j的基端侧(离全局坐标系C的原点较远的一侧)的传感器单元(状态校正对象传感器)S_j的位置进行校正。因此，通过传感器位置观测部61进行预定次数的修正对象的链(修正对象链)T_j的基端侧(离全局坐标系C的原点较远的一侧)的传感器单元(状态校正对象传感器)S_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)的检测线形形状(71)；以及

形状修正部(50、150)，其使用粒子滤波器对各个所述传感器单元(S_i)的至少位置进行校正，检测对所述线形形状检测部(40)检测到的所述检测线形形状(71)进行修正后的修正形状(75、175)。

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

所述形状修正部(50、150)具备：

形状修正顺序实施部(51、151)，其在以最靠基端侧或最靠前端侧的所述传感器单元(S₁、S_N)的中心为原点的全局坐标系(C)中，针对各个所述链(T_j)从离所述全局坐标系(C)的所述原点较近一侧的所述链(T_j)起依次进行形状修正，形成所述修正形状(75、175)的各个所述传感器单元(S_i)之间的形状即修正传感器间要素(T′_j、L_j)；以及

形状修正控制部(52、152)，其将所述形状修正顺序实施部(51、151)控制为对所有所述链(T_j)进行所述形状修正的状态。

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

所述形状修正部(50、150)具备未修正链位置校正部(53、153)，该未修正链位置校正部(53、153)每当所述形状修正顺序实施部(51、151)进行一个所述链(T_j)的所述形状修正时，使由未进行所述形状修正的所述链(T_j)即未修正链构成的修正未完成部(79、179)平行移动为与由通过所述形状修正而形成的所述修正传感器间要素构成的修正完成部(77、177)边界连续的状态，对所述修正未完成部(79、179)的位置进行校正。

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

所述形状修正顺序实施部(51、151)具备：

传感器状态估计部(61、161)，其在通过所述形状修正顺序实施部(51、151)进行所述形状修正的对象的所述链(T_j)即修正对象链(T_k)中，使用所述粒子滤波器，基于通过所述形状修正顺序实施部(51、151)进行所述修正对象链(T_k)的形状修正之前的所述状态校正对象传感器(S_k)的初始状态或上一次的估计的估计结果，对离所述全局坐标系(C)的所述原点较远一侧的所述传感器单元(S_i)即状态校正对象传感器(S_k)的至少位置进行估计；以及

状态估计控制部(62、162)，其将所述传感器状态估计部(61、161)控制成进行预定次数的所述状态校正对象传感器的所述估计的状态。

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

所述形状修正顺序实施部(51、151)具备：

传感器状态校正部(55、155)，其基于所述传感器状态估计部(61、161)的估计结果，校正所述状态校正对象传感器(S_k)的至少位置；以及

修正传感器间要素形成部(57、157)，其基于由所述传感器状态校正部(55、155)进行了校正的所述状态校正对象传感器(S_k)的位置来形成所述修正传感器间要素(T′_j、L_j)。

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

所述传感器状态估计部(61、161)具备：

粒子散布部(63、163)，其基于所述状态校正对象传感器(S_k)的所述初始状态或上一次的所述估计的估计结果，散布预定数量(A)的粒子(72、172)；

粒子移动部(65、165)，其根据所述内窥镜(10)的所述插入部(11)的状态和各个所述传感器单元(S_i)中的测量数据来检测移动模型，并基于所述移动模型，使所散布的各个所述粒子(72、172)从移动前粒子位置移动到移动后粒子位置；

权重计算部(67、167)，其对表示所述状态校正对象传感器(S_k)位于各个所述粒子(72、172)的所述移动后粒子位置处的似然性的似然度进行计算，并根据所述似然度计算各个所述粒子(72、172)的权重；以及

传感器状态观测部(69、169)，其基于由所述权重计算部(67、167)计算出的各个所述粒子(72、172)的所述权重，对所述状态校正对象传感器(S_k)的至少位置进行观测。

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

所述粒子移动部(165)是如下的曲线插补部(165)：根据所述内窥镜(10)的所述插入部(11)的弯曲状态检测所述移动模型，并基于所述移动模型，进行所述修正对象链(T_k)的离所述全局坐标系(C)的所述原点较近一侧的所述传感器单元(S_k-1)与各个所述粒子(172)之间的曲线插补，使各个所述粒子(172)移动。

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

所述粒子移动部(65)是如下的运动状态移动模型实施部(65)：根据在所述内窥镜(10)的所述插入部(11)平行移动的运动状态下检测到的各个所述传感器单元(S_i)在所述全局坐标系(C)中的加速度向量、速度向量、位移来检测所述移动模型，基于所述移动模型使各个所述粒子(72)移动。

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

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

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

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

使用粒子滤波器对各个所述传感器单元(S_i)的至少位置进行校正，检测对检测到的所述检测线形形状(71)进行修正后的修正形状(75、175)。

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

对各个所述传感器单元(S_i)的至少位置进行校正，并对所述修正形状(75、175)进行检测的步骤包括如下步骤：

对所有所述链(T_j)进行如下处理：在以最靠基端侧或最靠前端侧的所述传感器单元(S₁、S_N)的中心为原点的全局坐标系(C)中，针对各个所述链(T_j)从离所述全局坐标系(C)的所述原点较近一侧的所述链(T_j)起依次进行形状修正，形成所述修正形状(75、175)的各个所述传感器单元(S_i)之间的形状即修正传感器间要素(T′_j、L_j)。

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

对各个所述传感器单元(S_i)的至少位置进行校正，并对所述修正形状(75、175)进行检测的步骤包括如下步骤：

每当进行一个所述链(T_j)的所述形状修正时，使由未进行所述形状修正的所述链(T_j)即未修正链构成的修正未完成部(79、179)平行移动为与由通过所述形状修正而形成的所述修正传感器间要素(T′_j、L_j)构成的修正完成部(77、177)边界连续的状态，对所述修正未完成部(79、179)的位置进行校正。

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

形成所述修正传感器间要素(T′_j、L_j)的步骤包括如下步骤：

在进行所述形状修正的对象的所述链(T_j)即修正对象链(T_k)中，使用所述粒子滤波器，基于进行所述修正对象链的所述形状修正之前的所述状态校正对象传感器(S_k)的初始状态或上一次的估计的估计结果，对离所述全局坐标系(C)的所述原点较远一侧的所述传感器单元(S_i)即状态校正对象传感器(S_k)的至少位置进行估计。

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

形成所述修正传感器间要素(T′_j、L_j)的步骤包括如下步骤：

基于所述状态校正对象传感器(S_k)的估计结果，校正所述状态校正对象传感器(S_k)的至少位置；以及

基于校正后的所述状态校正对象传感器(S_k)的位置，形成所述修正传感器间要素(T′_j、L_j)。

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

对所述状态校正对象传感器(S_k)的至少位置进行估计的步骤包括如下步骤：

基于所述状态校正对象传感器(S_k)的所述初始状态或上一次的所述估计的估计结果，散布预定数量(A)的粒子(72、172)；

根据所述内窥镜(10)的所述插入部(11)的状态和各个所述传感器单元(S_i)中的测量数据来检测移动模型，并基于所述移动模型，使所散布的各个所述粒子(72、172)从移动前粒子位置移动到移动后粒子位置；

对表示所述状态校正对象传感器(S_k)位于各个所述粒子(72、172)的所述移动后粒子位置处的似然性的似然度进行计算，并根据所述似然度计算各个所述粒子(72、172)的权重；以及

根据计算出的各个所述粒子(72、172)的所述权重，对所述状态校正对象传感器(S_k)的至少位置进行观测。

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

使各个所述粒子(172)移动的步骤包括如下步骤：

根据所述内窥镜(10)的所述插入部(11)的弯曲状态检测所述移动模型，并基于所述移动模型，进行所述修正对象链(T_k)的离所述全局坐标系(C)的所述原点较近一侧的所述传感器单元(S_k-1)与各个所述粒子(172)之间的曲线插补，使各个所述粒子(172)移动。

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

使各个所述粒子(72)移动的步骤包括如下步骤：

根据在所述内窥镜(10)的所述插入部(11)平行移动的运动状态下检测到的各个所述传感器单元(S_i)在所述全局坐标系(C)中的加速度向量、速度向量、位移来检测所述移动模型，基于所述移动模型使各个所述粒子(72)移动。

Images