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

Abstract

内窥镜形状检测装置具备：传感器临时位置检测部，其基于各个传感器单元的姿态，将各个所述传感器单元之间假定为尺寸与传感器间尺寸相等的直线状的临时链，来检测各个所述传感器单元的临时位置；以及临时曲线形状检测部，其基于各个所述传感器单元的所述临时位置，用临时圆弧对各个所述传感器单元的所述临时位置之间进行曲线插补，并对所述插入部的临时曲线形状进行检测。内窥镜形状检测装置具备：传感器位置修正部，其基于各个所述临时圆弧的弧长与所述传感器间尺寸之间的差的绝对值，将各个所述传感器单元的位置从所述临时位置修正到最终位置；以及最终曲线形状检测部，其用最终圆弧对各个所述传感器单元的所述最终位置之间进行曲线插补，并对所述插入部的最终曲线形状进行检测。

Description

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

技术领域

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

背景技术

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

并且，在专利文献2中公开了如下的内窥镜形状检测装置：根据交流磁场检测配置在内窥镜的插入部中的各个线圈的位置，对检测到的各个线圈的位置之间进行曲线插补。在该内窥镜形状检测装置中，通过用贝塞尔曲线或样条曲线连结检测到的各个线圈的位置来进行曲线插补。通过进行曲线插补检测内窥镜的插入部的曲线形状。

现有技术文献

专利文献

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

【专利文献2】日本特开2007-130175号公报

发明内容

发明所要解决的课题

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

在上述专利文献2的内窥镜形状检测装置中，通过用贝塞尔曲线或样条曲线连结检测到的各个线圈的位置来进行曲线插补。但是，在该内窥镜形状检测装置中，没有考虑各个线圈之间的长度方向的尺寸即线圈间尺寸进行曲线插补。因此，不能以高精度检测插入到体腔内的插入部的形状。

例如还考虑以下情况：在用样条曲线对检测到的各个线圈的位置进行连结，并检测插入部的临时形状后，根据线圈间尺寸，依次修正检测到的各个线圈的位置。此时，每当进行一个线圈的位置修正时，用样条曲线连结位置修正后的线圈的位置，检测对线圈位置修正前的插入部的检测形状进行了修正的线圈位置修正后的插入部的修正形状。即，每当进行一个线圈的位置修正时，修正插入部的曲线形状整体。此处，线圈间尺寸是线圈与线圈之间的长度方向的尺寸，是插入部的局部参数。即，线圈间尺寸是对插入部的形状产生局部影响的参数，不是对插入部整体的形状产生影响的参数。因此，在通过一个线圈的位置修正对插入部的曲线形状整体进行修正，而不是通过一个线圈的位置修正在位置修正后的线圈附近对传感器位置修正前的插入部的检测形状进行局部修正的结构中，不能以高精度进行插入部的曲线形状的修正。因此，最终不能以高精度检测插入部的曲线形状。此外，由于通过一个线圈的位置修正对插入部的整体形状进行修正，因此根据样条曲线等的种类而使处理变复杂。

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

用于解决课题的手段

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

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

发明效果

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

附图说明

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

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

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

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

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

图6是示出利用第一实施方式的内窥镜形状检测装置的传感器临时位置检测部检测的各个传感器单元的临时位置的示意图。

图7是说明第一实施方式的传感器临时位置检测部的临时链移动部中的处理的示意图。

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

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

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

图11是示出利用第一实施方式的内窥镜形状检测装置的传感器位置修正部修正后的各个传感器单元的最终位置的示意图。

图12是示出第一实施方式的利用传感器位置修正部修正各个传感器单元的位置的方法的流程图。

图13是说明第一实施方式的传感器位置修正部的传感器位置顺序校正部的处理的示意图。

图14是说明第一实施方式的传感器位置顺序校正部的传感器移动部以及移动后圆弧形成部中的处理的示意图。

图15是说明第一实施方式的传感器位置修正部的未校正传感器组移动部中的处理的示意图。

图16是示出利用第一实施方式的内窥镜形状检测装置的最终曲线形状检测部检测到的内窥镜的插入部的最终曲线形状的示意图。

图17是示出第一实施方式的利用最终曲线形状检测部检测最终曲线形状的方法的流程图。

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

图19是示出本发明的第二实施方式的利用内窥镜形状检测装置的传感器位置修正部修正各个传感器单元的位置的方法的流程图。

具体实施方式

(第1实施方式)

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

图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的上下方向及左右方向进行弯曲操作。

插入部11中设有多个(在本实施方式中为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轴方向与铅垂方向一致，以铅垂方向的上方向(从纸面的里侧朝向外侧的方向)为正。另外，这里，为了便于说明，以全局坐标系C的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_i向全局坐标系C的旋转矩阵为

【数学式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轴方向成为从磁北方向绕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(M_Y/M_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检测的各个传感器单元S_i的临时位置P_0，i时的图。如图6所示，传感器临时位置检测部40基于由姿态检测部30检测到的各个传感器单元S_i的姿态，将各个传感器单元S_i之间的形状假定为尺寸与传感器间尺寸I相等的直线状的临时链T_0，j(j＝1，2，...N)，来检测各个传感器单元S_i的临时位置P_0，i。这里，从基端侧起第k个临时链T_0，k是从基端侧起第k个传感器单元S_k-1与从基端侧起第(k+1)个传感器单元S_k之间的临时链T_0，k。传感器临时位置检测部40具备：形成各个临时链T_0，j的临时链形成部41；以及临时链移动部42，其使临时链形成部41所形成的各个临时链T_0，j平行移动。通过临时链移动部42，各个临时链T_0，j平行移动成与邻接的临时链T_0，j-1、T_0，j+1的临时链边界连续的状态。由此，形成将各个传感器单元S_i之间假定为直线状的临时链T_0，j的临时线形形状71。

这里，对利用传感器临时位置检测部40检测各个传感器单元S_i的临时位置P_0，i的方法进行说明。在检测各个传感器单元S_i的临时位置P_0，i(步骤S104)时，首先，基于在步骤S102、S103中计算出的姿态角α_i、β_i、γ_i的值，临时链形成部41形成直线形状的各个临时链T_0，j。这里，对从基端侧起第k个传感器单元S_k-1与从基端侧起第(k+1)个传感器单元S_k之间的、从基端侧起第k个临时链T_0，k的形成进行说明。

使用以k-1置换了式(7)、式(8)、式(12)(或式(15))中的i的式子，在步骤S102、S103中计算局部坐标系C_k-1(即临时链T_0，k)的姿态角α_k-1、β_k-1、γ_k-1。使用该姿态角α_k-1、β_k-1、γ_k-1及各个传感器单元S_i之间的在长度方向上的间隔即传感器间尺寸I，求出从传感器单元S_k-1朝向传感器单元S_k的向量。这里，从传感器单元S_k-1朝向传感器单元S_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{\stackrel{\·}{f}}_{\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)$

${\stackrel{\·}{f}}_{\mathrm{yk}-1}={\left[\begin{array}{ccc}0& 1& 0\end{array}\right]}^T---\left(16.2\right).$

在式(16.1)中，通过将局部坐标系C_k-1的原点处的长度方向即Y_k-1轴方向的单位向量与用式(1)计算出的旋转矩阵相乘，计算出将局部坐标系C_k-1的原点处的Y_k-1轴方向的单位向量分别分解到全局坐标系C的X轴方向、Y轴方向、Z轴方向后的分量。即，I_xk、I_yk、I_zk是将局部坐标系C_k-1中的朝向Y_k-1轴方向的大小为I的向量分别分解到全局坐标系C的X轴方向、Y轴方向、Z轴方向的分量。利用在式(16.1)中计算出的向量形成临时链T_0，k。

另外，对于临时链T_0，k以外的临时链T_0，j，也利用临时链形成部41同样形成。即，使用以j置换了式(16.1)、式(16.2)中的k的式子，求出从临时链T_0，j的基端侧(离全局坐标系C的原点较近的一侧)的传感器单元S_j-1向临时链T_0，j的前端侧(离全局坐标系C的原点较远的一侧)的传感器单元S_j的向量。进而，利用从传感器单元S_j-1向传感器单元S_j的向量形成临时链T_0，j。即，临时链形成部41将临时链T_0，j假定为，沿着基端侧的传感器单元S_j-1的中心处的长度方向，从基端侧(离全局坐标系C的原点较近的一侧)的传感器单元S_j-1的中心延伸设置到前端侧(离全局坐标系C的原点较远的一侧)的传感器单元S_j的中心，来形成临时链T_0，j。

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

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

如图7所示，在利用临时链移动部42进行临时链T_0，k的移动之前的状态下，到与临时链T_0，k的基端侧邻接的临时链T_0，k-1为止完成移动，形成临时链移动完成部73。在进行临时链T_0，k的移动时，临时链移动部42使临时链T_0，k平行移动从原点到临时链移动完成部73的前端(即传感器单元S_k-1的临时位置P_0，k-1)的移动量。即，使临时链T_0，k从图7的虚线所示的位置平行移动到图7的实线所示的位置。由此，成为临时链T_0，k-1与临时链T_0，k的链边界连续的状态。

另外，对于临时链T_0，k以外的临时链T_0，j，也利用临时链移动部42同样进行移动。即，在进行临时链T_0，j的移动时，临时链移动部42使临时链T_0，j平行移动从原点到临时链移动完成部73的前端(离全局坐标系C的原点较远的一侧的端部)的移动量。由此，成为临时链T_0，j和与临时链T_0，j的基端侧(离全局坐标系C的原点较近的一侧)邻接的临时链T_0，j-1的临时链边界连续的状态。其中，关于临时链T_0，1，由于临时链T_0，1的基端为全局坐标系C的原点，所以不进行移动。所有临时链T_0，j的移动完成，由此如图6所示，形成将各个传感器单元S_i之间假定为直线状的临时链T_0，j的临时线形形状71。将临时线形形状71中的各个传感器单元S_i的位置检测为各个传感器单元S_i的临时位置P_0，i(步骤S104)。

如图3所示，传感器临时位置检测部40与临时曲线形状检测部50连接。临时曲线形状检测部50将由传感器临时位置检测部40检测到的各个传感器单元S_i的临时位置P_0，i之间利用临时圆弧L_0，j进行曲线插补，并对临时曲线形状75进行检测。图8是从全局坐标系C的Z轴的负方向朝向正方向观察利用临时曲线形状检测部50检测到的内窥镜10的插入部11的临时曲线形状75时的图。如图8所示，通过临时曲线形状检测部50对图8的虚线所示的临时线形形状71的各个传感器单元S_i的临时位置P_0，i之间进行曲线插补。由此，形成各个临时圆弧L_0，j，检测到图8的实线所示的临时曲线形状75。

临时曲线形状检测部50具备：单位切向量计算部51，其计算各个传感器单元S_i的临时位置P_0，i处的单位切向量；变化率计算部52，其基于由单位切向量计算部51计算出的单位切向量，计算各个传感器单元S_i的临时位置P_0，i之间的单位切向量的变化率；以及临时圆弧形成部53，其基于由单位切向量计算部51计算出的单位切向量和由变化率计算部52计算出的变化率，形成各个传感器单元S_i的临时位置P_0，i之间的临时圆弧L_0，j。在所有的传感器单元S_i的临时位置P_0，i之间进行曲线插补，形成所有的临时圆弧L_0，j，从而检测到临时曲线形状75。

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

在对临时曲线形状75进行检测时，临时曲线形状检测部50按照各个传感器单元S_i的各个临时位置P_0，i之间依次进行曲线插补，形成临时圆弧L_0，j。这里，对利用临时曲线形状检测部50进行各个传感器单元S_i的临时位置P_0，i之间的曲线插补的方法进行说明。这里，对从基端侧起第k个传感器单元S_k-1与从基端侧起第(k+1)个传感器单元S_k之间的曲线插补进行说明。即，临时位置P_0，k-1与临时位置P_0，k之间成为作为插补对象的插补对象临时位置间。

图9是示出利用临时曲线形状检测部50检测临时曲线形状75的方法的流程图。图10是说明临时曲线形状检测部50中的处理的图。如图9和图10所示，在利用临时曲线形状检测部50进行传感器单元S_k-1的临时位置P_0，k-1与传感器单元S_k的临时位置P_0，k之间即插补对象的传感器间的曲线插补时，首先，单位切向量计算部51计算传感器单元S_k-1的临时位置P_0，k-1处的单位切向量和传感器单元S_k的临时位置P_0，k处的单位切向量(步骤S111)。传感器单元S_k-1的临时位置P_0，k-1处的单位切向量为

【数学式17】

$\mathrm{\Δ}{\stackrel{\·}{P}}_{0,k}={\stackrel{\·}{P}}_{0,k}-{\stackrel{\·}{P}}_{0,k-1}---\left(17.1\right)$

$\mathrm{\Δ}{L}_{0,k}=\left|\mathrm{\Δ}{\stackrel{\·}{P}}_{0,k}\right|---\left(17.2\right)$

${\stackrel{\·}{e}}_{0,k}=\frac{d\stackrel{\·}{P}}{\mathrm{dL}}|0,k=\frac{\mathrm{\Δ}{\stackrel{\·}{P}}_{0,k}}{\mathrm{\Δ}{L}_{0,k}}---\left(17.3\right).$

即，临时位置P_0，k-1处的单位切向量是从临时位置P_0，k-1到临时位置P_0，k的向量的单位向量。使用以k+1置换了式(17.1)、式(17.2)、式(17.3)中的k的式子求出传感器单元S_k的临时位置P_0，k处的单位切向量。即，临时位置P_0，k处的单位切向量是从临时位置P_0，k到临时位置P_0，k+1的向量的单位向量。

并且，变化率计算部52基于由单位切向量计算部51计算出的单位切向量，计算传感器单元S_k-1的临时位置P_0，k-1与传感器单元S_k的临时位置P_0，k之间的单位切向量的变化率(步骤S112)。首先，临时位置P_0，k-1与临时位置P_0，k之间的单位切向量的变化率向量用下式求出：

【数学式18】

${\stackrel{\·}{\mathrm{\λ}}}_{0,k}=\frac{d^2\stackrel{\·}{P}}{{\mathrm{dL}}^2}|0,k=\frac{d\stackrel{\·}{e}}{\mathrm{dL}}|0,k=\frac{{\stackrel{\·}{e}}_{0,k+1}-{\stackrel{\·}{e}}_{0,k}}{\mathrm{\Δ}{\stackrel{\‾}{L}}_{0,k}}---\left(18.1\right)$

$\mathrm{\Δ}{\stackrel{\‾}{L}}_{0,k}=\frac{\mathrm{\Δ}{L}_{0,k+1}-\mathrm{\Δ}{L}_{0,k}}{2}---\left(18.2\right).$

并且，用式(18.1)、式(18.2)求出的变化率向量的大小为临时位置P_0，k-1与临时位置P_0，k之间的单位切向量的变化率。

并且，临时圆弧形成部53基于由单位切向量计算部51计算出的单位切向量和由变化率计算部52计算出的变化率，形成传感器单元S_k-1的临时位置P_0，k-1与传感器单元S_k的临时位置P_0，k之间的临时圆弧L_0，k(步骤S113)。这里，在步骤S112中计算出的临时位置P_0，k-1与临时位置P_0，k之间的单位切向量的变化率为临时圆弧L_0，k的曲率1/R_0，k。临时圆弧L_0，k的半径R_0，k是曲率1/R_0，k的倒数，因此临时圆弧L_0，k的半径R_0，k为

【数学式19】

$R_{0,k}=\frac{1}{\left|{\stackrel{\·}{\mathrm{\λ}}}_{0,k}\right|}---\left(19\right).$

此外，临时圆弧L_0，k的中心O_0，k在全局坐标C中的位置通过下式求出：

【数学式20】

$O_{0,k}={\stackrel{\·}{P}}_{0,k-1}+{R_{0,k}}^2{\stackrel{\·}{\mathrm{\λ}}}_{0,k}---\left(20\right).$

并且，使用临时位置P_0，k-1处的单位切向量和临时位置P_0，k处的单位切向量，通过下式求出临时圆弧L_0，k的中心角

【数学式21】

${\mathrm{\φ}}_{0,k}={\cos }^{-1}\frac{\mathrm{\Δ}{\stackrel{\·}{P}}_{0,k}\·\mathrm{\Δ}{\stackrel{\·}{P}}_{0,k+1}}{\left|\mathrm{\Δ}{\stackrel{\·}{P}}_{0,k}\right|\left|\mathrm{\Δ}{\stackrel{\·}{P}}_{0,k+1}\right|}={\cos }^{-1}({\stackrel{\·}{e}}_{0,k}\·{\stackrel{\·}{e}}_{0,k+1})---\left(21\right).$

使用这些参数，形成临时位置P_0，k-1与临时位置P_0，k之间的临时圆弧L_0，k。

另外，关于临时圆弧L_0，k以外的临时圆弧L_0，j，也同样通过对临时位置P_0，j-1与临时位置P_0，j之间进行曲线插补来形成。即，在形成临时圆弧L_0，j时，首先，单位切向量计算部51计算位于插补对象临时位置间的基端侧(离全局坐标系C的原点较近的一侧)的传感器单元S_j-1的临时位置P_0，j-1处的单位切向量以及位于插补对象临时位置间的前端侧(离全局坐标系C的原点较远的一侧)的传感器单元S_j的临时位置P_0，j处的单位切向量。传感器单元S_j-1的临时位置P_0，j-1处的单位切向量使用以j置换了式(17.1)、式(17.2)、式(17.3)中的k的式子求出，是从临时位置P_0，j-1到临时位置P_0，j的向量的单位向量。此外，传感器单元S_j的临时位置P_0，j处的单位切向量使用以j+1置换了式(17.1)、式(17.2)、式(17.3)中的k的式子求出，是从临时位置P_0，j到临时位置P_0，j+1的向量的单位向量。由此，计算各个传感器单元S_i的临时位置P_0，i处的单位切向量(步骤S111)。

并且，变化率计算部52基于由单位切向量计算部51计算出的单位切向量，计算传感器单元S_j-1的临时位置P_0，j-1与传感器单元S_j的临时位置P_0，j之间即插补对象临时位置间的单位切向量的变化率。临时位置P_0，j-1与临时位置P_0，j之间的单位切向量的变化率用以j置换了式(18.1)、式(18.2)中的k的式子求出。由此，计算各个传感器单元S_i的临时位置P_0，i之间的单位切向量的变化率(步骤S112)。

并且，临时圆弧形成部53基于由单位切向量计算部51计算出的单位切向量和由变化率计算部52计算出的变化率，形成传感器单元S_j-1的临时位置P_0，j-1与传感器单元S_j的临时位置P_0，j之间即插补对象临时位置间的临时圆弧L_0，j。这里，使用以j置换了式(19)、式(20)、式(21)中的k的式子，求出临时圆弧L_0，j的半径R_0，j、中心O_0，j和中心角

由此，形成各个传感器单元S_i的临时位置P_0，i之间的临时圆弧L_0，j(步骤S113)。在所有的传感器单元S_i的临时位置P_0，i之间进行曲线插补，形成所有的临时圆弧L_0，j，从而检测到临时曲线形状75。

如图3所示，临时曲线形状检测部50与对各个传感器单元S_i的位置进行修正的传感器位置修正部55连接。传感器位置修正部55基于由临时曲线形状检测部50检测到的临时曲线形状75的各个临时圆弧L_0，j的弧长

与传感器间尺寸I之间的差的绝对值，对各个传感器单元S_i的位置进行修正。这里，将临时圆弧L_0，j的弧长

与传感器间尺寸I之间的差的绝对值设为能量E_0，j。图11是从全局坐标C的Z轴的负方向朝向正方向观察被传感器位置修正部55修正后的各个传感器单元S_i的位置的图。如图11所示，利用传感器位置修正部55将各个传感器单元S_i从临时位置P_0，i修正到最终位置P_F，i。

传感器位置修正部55具有从基端侧(离全局坐标系C的原点较近的一侧)的传感器单元S_i依次进行位置校正的传感器位置顺序校正部57。在利用传感器位置顺序校正部57进行各个传感器单元S_i的位置校正前的状态下，将各个传感器单元S_i从临时位置P_0，i移动到校正前位置Q_0，i(具体将后述。)。传感器位置顺序校正部57将各个传感器单元S_i的位置从校正前位置Q_0，i校正到最终位置P_F，i。此外，传感器位置修正部55具有未校正传感器组移动部59，每当由传感器位置顺序校正部57进行一个传感器单元S_i的位置校正时，该未校正传感器组移动部59使未完成位置校正的传感器单元S_i即未校正传感器组77平行移动。通过传感器位置顺序校正部57进行所有传感器单元S_i的从校正前位置Q_0，i到最终位置P_F，i的位置校正。这里，校正前位置Q_0，i是通过传感器位置顺序校正部57进行位置校正之前的各个传感器单元S_i的位置。未校正传感器组移动部59使未校正传感器组77平行移动在传感器位置顺序校正部57刚刚进行的位置校正中作为校正对象的传感器单元S_i即刚刚校正对象传感器79从校正前位置Q_0，i到最终位置P_F，i的校正量。由此，未校正传感器组77从平行移动前位置U_a-1，i(a＝1，2...)平行移动到平行移动后位置U_a，i。这里，a表示由未校正传感器组移动部59平行移动各个传感器单元S_i的次数。各个传感器单元S_i的利用未校正传感器组移动部59的第1次平行移动的平行移动前位置U_0，i与临时位置P_0，i一致。各个传感器单元S_i通过未校正传感器组移动部59平行移动(i-1)次。各个传感器单元S_i的利用未校正传感器组移动部59的第(i-1)次平行移动的平行移动后位置U_i-1，i与校正前位置Q_0，i一致。如上所述，通过利用传感器位置顺序校正部57的位置校正和利用未校正传感器组移动部59的平行移动，将各个传感器单元S_i的位置从临时位置P_0，i修正到最终位置P_F，i。

传感器位置顺序校正部57具有使作为位置校正的对象的校正对象传感器81从位置校正前的校正前位置Q_0，i移动一次以上的传感器移动部61。通过利用传感器移动部61的一次移动，校正对象传感器81从移动前位置Q_t-1，i(t＝1，2...)移动到移动后位置Q_t，i。这里，t表示由传感器移动部61移动各个传感器单元S_i的次数。此外，传感器位置顺序校正部57具有移动后圆弧形成部62，每当通过传感器移动部61进行一次校正对象传感器81的移动时，该移动后圆弧形成部62对相对于校正对象传感器81而相邻设置于基端侧(离全局坐标系C的原点较近的一侧)的传感器单元S_i即近位侧相邻设置传感器82的最终位置P_F，i与校正对象传感器81的移动后位置Q_t，i之间进行曲线插补。利用移动后圆弧形成部62对近位侧相邻设置传感器82的最终位置P_F，i与校正对象传感器81的移动后位置Q_t，i之间进行曲线插补，由此形成移动后圆弧L_t，j。

传感器位置顺序校正部57具有移动控制部63，该移动控制部63将传感器移动部61控制成重复进行校正对象传感器81的从移动前位置Q_t-1，i向移动后位置Q_t，i的移动的状态，直到通过移动后圆弧形成部62形成的移动后圆弧L_t，j的弧长

与传感器间尺寸I之间的差的绝对值为预定阈值以下为止。这里，将移动后圆弧L_t，j的弧长

与传感器间尺寸I之间的差的绝对值设为能量E_t，j。此外，传感器位置顺序校正部57具有将通过传感器移动部61的最终一次(例如第F次)移动的移动后位置Q_F，i确定为校正对象传感器81的最终位置P_F，i的最终位置确定部64。传感器移动部61、移动后圆弧形成部62、移动控制部63、最终位置确定部64中的详细处理将在后面叙述。

这里，对利用传感器位置修正部55修正各个传感器单元S_i的位置的方法进行说明。如图4所示，传感器位置修正部55基于由临时曲线形状检测部50检测到的临时曲线形状75的各个临时圆弧L_0，j的弧长

与传感器间尺寸I之间的差的绝对值，对各个传感器单元S_i的位置进行修正(步骤S106)。即，基于用下式求出的能量E_0，j，修正各个传感器单元S_i的位置：

【数学式22】

E_0，j＝|R_0，jφ_0，j ^-1|    (22)。

由此，将各个传感器单元S_i从临时位置P_0，i修正到最终位置P_F，i。

如前所述，通过临时曲线形状检测部50形成与实际的插入部11的曲线形状的误差较少的临时曲线形状75。但是，在临时曲线形状检测部50中，没有考虑传感器间尺寸I而进行各个传感器单元S_i之间的曲线插补。此外，各个传感器单元S_i的测量数据由于噪声等具有误差。因此，考虑传感器间尺寸I，基于各个临时圆弧L_0，j的弧长

与传感器间尺寸I之间的差的绝对值，对各个传感器单元S_i的位置进行修正较为重要。

图12是示出利用传感器位置修正部55修正各个传感器单元S_i的位置的方法的流程图。图13是说明传感器位置顺序校正部57的处理的图。如图12所示，在修正各个传感器单元S_i的位置时，通过传感器位置顺序校正部57从基端侧(离全局坐标系C的原点较近的一侧)的传感器单元S_i依次进行各个传感器单元S_i的从位置校正前的校正前位置Q_0，i向最终位置P_F，i的位置校正。这里，校正前位置Q_0，i是通过传感器位置顺序校正部57进行位置校正之前的各个传感器单元S_i的位置，是各个传感器单元S_i从临时位置P_0，i通过未校正传感器组移动部59平行移动(i-1)次后的位置。传感器位置顺序校正部57开始进行未校正传感器组77中的最靠基端侧的传感器单元S_i的位置校正(步骤S121)。即，如图13所示，在到传感器单元S_k-1为止已经完成了从校正前位置Q_0，i到最终位置P_F，i的位置校正的情况下，通过传感器位置顺序校正部57开始进行未校正传感器组77中的最靠基端侧的传感器单元S_k的向最终位置P_F，k的位置校正。即，传感器单元S_k成为作为位置校正的对象的校正对象传感器81。另外，在以下的说明中，对从基端侧起第(k+1)个传感器单元S_k的位置校正进行说明。

如图13所示，在开始进行作为校正对象传感器81的传感器单元S_k的位置校正时，到传感器单元S_k-1为止已经完成了从校正前位置Q_0，i到最终位置P_F，i的位置校正。此时，传感器单元S_k位于校正前位置Q_0，k。在该状态下，通过传感器移动部61将传感器单元S_k从位置Q_0，k移动到位置Q_1，k(步骤S122)，并进行传感器单元S_k的第一次移动。此时，位置Q_0，k是移动前位置，位置Q_1，k是移动后位置。进行1次以上的传感器移动部61对传感器单元S_k的移动。图14是说明传感器移动部61和移动后圆弧形成部62中的处理的图。如图14所示，在第s次的移动中，将作为校正对象传感器81的传感器单元S_k从移动前位置Q_s-1，k移动到移动后位置Q_s，k。即，通过传感器移动部61的一次移动，将传感器单元S_k从移动前位置Q_t-1，k移动到移动后位置Q_t，k(步骤S122)。

并且，每当通过传感器移动部61进行一次传感器单元S_k的移动时，移动后圆弧形成部62对相对于传感器单元S_k相邻设置于基端侧的传感器单元S_k-1的最终位置P_F，k-1与传感器单元S_k的移动后位置Q_t，k之间进行曲线插补，由此形成移动后圆弧L_t，k(步骤S123)。如图14所示，在第s次的移动后，对传感器单元S_k-1的最终位置P_F，k-1与传感器单元S_k的移动后位置Q_s，k之间进行曲线插补，由此形成移动后圆弧L_s，k。此处，传感器单元S_k-1是相对于作为校正对象传感器81的传感器单元S_k相邻设置于离全局坐标系C的原点较近一侧的近位侧相邻设置传感器82。

在传感器单元S_k的第s次移动后形成移动后圆弧L_s，k时，计算传感器单元S_k-1的最终位置P_F，k-1处的单位切向量和传感器单元S_k的移动后位置Q_s，k处的单位切向量。传感器单元S_k-1的最终位置P_F，k-1处的单位切向量为

【数学式23】

$\mathrm{\Δ}{\stackrel{\·}{P}}_{s,k}={\stackrel{\·}{Q}}_{s,k}-{\stackrel{\·}{P}}_{F,k-1}---\left(23.1\right)$

$\mathrm{\Δ}{L}_{s,k}=\left|\mathrm{\Δ}{\stackrel{\·}{P}}_{s,k}\right|---\left(23.2\right)$

${\stackrel{\·}{e}}_{s,k}=\frac{d\stackrel{\·}{P}}{\mathrm{dL}}|s,k=\frac{\mathrm{\Δ}{\stackrel{\·}{P}}_{s,k}}{\mathrm{\Δ}{L}_{s,k}}---\left(23.3\right).$

即，最终位置P_F，k-1处的单位切向量是从最终位置P_F，k-1到移动后位置Q_s，k的向量的单位向量。此外，在计算移动后位置Q_s，k处的单位切向量时，假定将相对于作为校正对象传感器81的传感器单元S_k相邻设置于离全局坐标系C的原点较远一侧的远位侧相邻设置传感器83即传感器单元S_k+1配置于位置Q′_s，k+1。这里，从移动后位置Q_s，k到位置Q′_s，k+1的向量的方向和大小与从传感器单元S_k的临时位置P_0，k到传感器单元S_k+1的临时位置P_0，k+1的向量相同。传感器单元S_k的移动后位置Q_s，k处的单位向量为

【数学式24】

$\mathrm{\Δ}{\stackrel{\·}{P}}_{s,k+1}^{\′}={\stackrel{\·}{Q}}_{s,k+1}^{\′}-{\stackrel{\·}{Q}}_{s,k}---\left(24.1\right)$

$\mathrm{\Δ}{L}_{s,k+1}^{\′}=\left|\mathrm{\Δ}{\stackrel{\·}{P}}_{s,k+1}^{\′}\right|---\left(24.2\right)$

${\stackrel{\·}{e}}_{s,k+1}^{\′}=\frac{d{\stackrel{\·}{P}}^{\′}}{{\mathrm{dL}}^{\′}}|s,k+1=\frac{\mathrm{\Δ}{\stackrel{\·}{P}}_{s,k+1}^{\′}}{\mathrm{\Δ}{L}_{s,k+1}^{\′}}---\left(24.3\right).$

即，移动后位置Q_s，k处的单位切向量为从移动后位置Q_s，k到传感器单元S_k+1的位置Q′_s，k+1的向量的单位向量。因此，移动后位置Q_s，k处的单位切向量的方向与在式(17.1)～式(17.3)中将k置换成k+1而得到的单位切向量相同。

并且，基于利用式(23.1)～式(23.3)、式(24.1)～式(24.3)计算出的单位切向量，计算传感器单元S_k-1的最终位置P_F，k-1与传感器单元S_k的移动后位置Q_s，k之间的单位切向量的变化率。首先，最终位置P_F，k-1与移动后位置Q_s，k之间的单位切向量的变化率向量用下式求出：

【数学式25】

${\stackrel{\·}{\mathrm{\λ}}}_{s,k}=\frac{d^2\stackrel{\·}{P}}{{\mathrm{dL}}^2}|s,k=\frac{d\stackrel{\·}{e}}{\mathrm{dL}}|s,k=\frac{{\stackrel{\·}{e}}_{s,k+1}^{\′}-{\stackrel{\·}{e}}_{s,k}}{\mathrm{\Δ}{\stackrel{\‾}{L}}_{s,k}}---\left(25.1\right)$

$\mathrm{\Δ}{\stackrel{\‾}{L}}_{s,k}=\frac{\mathrm{\Δ}{L}_{s,k+1}^{\′}-\mathrm{\Δ}{L}_{s,k}}{2}---\left(25.2\right).$

并且，用式(25.1)、式(25.2)求出的变化率向量的大小为最终位置P_F，k-1与移动后位置Q_s，k之间的单位切向量的变化率。

并且，基于利用式(23.1)～式(23.3)、式(24.1)～式(24.3)计算出的单位切向量以及利用式(25.1)、式(25.2)计算出的变化率，形成传感器单元S_k-1的最终位置P_F，k-1与传感器单元S_k的移动后位置Q_s，k之间的移动后圆弧L_s，k。这里，用式(25.1)、式(25.2)计算出的最终位置P_F，k-1与移动后位置Q_s，k之间的单位切向量的变化率为移动后圆弧L_s，k的曲率1/R_s，k。移动后圆弧L_s，k的半径R_s，k是曲率1/R_s，k的倒数，因此移动后圆弧L_s，k的半径R_s，k为

【数学式26】

$R_{s,k}=\frac{1}{\left|{\stackrel{\·}{\mathrm{\λ}}}_{s,k}\right|}---\left(26\right).$

此外，移动后圆弧L_s，k的中心O_s，k在全局坐标C中的位置通过下式求出：

【数学式27】

$O_{s,k}={\stackrel{\·}{P}}_{F,k-1}+{R_{s,k}}^2{\stackrel{\·}{\mathrm{\λ}}}_{s,k}---\left(27\right).$

并且，使用最终位置P_F，k-1处的单位切向量和移动后位置Q_s，k处的单位切向量，通过下式求出移动后圆弧L_s，k的中心角

【数学式28】

${\mathrm{\φ}}_{s,k}={\cos }^{-1}\frac{\mathrm{\Δ}{\stackrel{\·}{P}}_{s,k}\·\mathrm{\Δ}{\stackrel{\·}{P}}_{s,k+1}^{\′}}{\left|\mathrm{\Δ}{\stackrel{\·}{P}}_{s,k}\right|\left|\mathrm{\Δ}{\stackrel{\·}{P}}_{s,k+1}^{\′}\right|}={\cos }^{-1}({\stackrel{\·}{e}}_{s,k}\·{\stackrel{\·}{e}}_{s,k}^{\′})---\left(28\right).$

使用这些参数，形成最终位置P_F，k-1与移动后位置Q_s，k之间的移动后圆弧L_s，k。

另外，对在传感器单元S_k的第s次移动后形成移动后圆弧L_s，k的处理进行了说明，但是关于传感器单元S_k的第s次以外的移动后，使用在式(23.1)～式(28)中用t置换了s后的式子，也同样地形成传感器单元S_k-1的最终位置P_F，k-1与传感器单元S_k的移动后位置Q_t，k之间的移动后圆弧L_t，k。

并且，在形成移动后圆弧L_t，k后，通过移动控制部63计算移动后圆弧L_t，k的弧长

与传感器间尺寸I之间的差的绝对值(步骤S124)。这里，将移动后圆弧L_t，k的弧长

与传感器间尺寸I之间的差的绝对值设为能量E_t，k，并用下式给出：

【数学式29】

E_t，k＝|R_t，kφ_t，k ^-1|    (29)。

在能量E_t，k大于预定阈值的情况下(步骤S124-否)，返回步骤S122，传感器移动部61使传感器单元S_k从位置Q_t，k进一步移动。在传感器单元S_k的第s次移动后能量E_s，k大于预定阈值的情况下，通过传感器移动部61进行第(s+1)次的移动。通过第(s+1)次的移动，传感器单元S_k从移动前位置Q_s，k移动到移动后位置Q_s+1，k。如上所述，传感器移动部61被移动控制部63控制成重复进行作为校正对象传感器81的传感器单元S_k的从移动前位置Q_t-1，k向移动后位置Q_t，k的移动的状态，直到能量E_t，k为预定阈值以下为止。

在能量E_t，k为预定阈值以下的情况下(步骤S124-是)，进入下一步骤。并且，最终位置确定部64将通过传感器移动部61对传感器单元S_k的最终一次(例如第F次)移动的移动后位置Q_F，k确定为作为校正对象传感器81的传感器单元S_k的最终位置P_F，k(步骤S125)。如上所述，传感器单元S_k的从校正前位置Q_0，k到最终位置P_F，k的位置校正完成。

另外，关于传感器单元S_k以外的传感器单元S_i，也通过传感器位置顺序校正部57与传感器单元S_k同样地进行向最终位置P_i，k的位置校正。即，通过传感器移动部61将传感器单元S_i从移动前位置Q_t-1，i移动到移动后位置Q_t，i(步骤S122)。并且，每当通过传感器移动部61进行一次传感器单元S_i的移动时，通过移动后圆弧形成部62对相对于传感器单元S_i相邻设置于基端侧的近位侧相邻设置传感器82即传感器单元S_i-1的最终位置P_F，i-1与传感器单元S_i的移动后位置Q_t，i之间进行曲线插补，形成移动后圆弧L_t，j(步骤S123)。

并且，在形成移动后圆弧L_t，j后，通过移动控制部63计算移动后圆弧L_t，j的弧长

与传感器间尺寸I之间的差的绝对值即能量E_t，j(步骤S124)。在能量E_t，j大于预定阈值的情况下(步骤S124-否)，返回步骤S122，传感器移动部61使传感器单元S_i从位置Q_t，i进一步移动。在能量E_t，j为预定阈值以下的情况下(步骤S124-是)，进入下一步骤。并且，最终位置确定部64将通过传感器移动部61对传感器单元S_i的最终一次移动的移动后位置Q_F，i确定为作为校正对象传感器81的传感器单元S_i的最终位置P_F，i(步骤S125)。如上所述，传感器单元S_i的从校正前位置Q_0，i到最终位置P_F，i的位置校正完成。

并且，每当由传感器位置顺序校正部57进行一个传感器单元S_i的位置校正时，通过未校正传感器组移动部59使未完成位置校正的传感器单元S_i即未校正传感器组77平行移动(步骤S126)。图15是说明未校正传感器组移动部59中的处理的图。如图15所示，在通过传感器位置顺序校正部57完成了传感器单元S_k的从校正前位置Q_0，k到最终位置P_F，k的位置校正的状态下，传感器单元S_k+1～S_N成为未完成位置校正的未校正传感器组77。此外，传感器单元S_k成为在传感器位置顺序校正部57刚刚进行的位置校正中作为校正对象的刚刚校正对象传感器79。

在该状态下，未校正传感器组移动部59使未校正传感器组77平行移动作为刚刚校正对象传感器79的传感器单元S_k的从校正前位置Q_0，k到最终位置P_F，k的校正量。由此，未校正传感器组77的各个传感器单元S_i从平行移动前位置U_k-1，i移动到平行移动后位置U_k，i。这里，未校正传感器组77的各个传感器单元S_i的平行移动前位置U_k-1，i是通过未校正传感器组移动部59将各个传感器单元S_i从临时位置P_0，i移动(k-1)后的位置，在此次的平行移动中，未校正传感器组77的各个传感器单元S_i由未校正传感器组移动部59平行移动了k次。此外，传感器单元S_k+1的平行移动后位置U_k，k+1与校正前位置Q_0，k+1一致，从传感器单元S_k的最终位置P_F，k到传感器单元S_k+1的平行移动后位置U_k，k+1的向量的方向和大小与从传感器单元S_k的临时位置P_0，k到传感器单元S_k+1的临时位置P_0，k+1的向量相同。

另外，即使在传感器单元S_k以外的传感器单元S_i的向最终位置P_F，k的位置校正已完成的状态下，也与传感器单元S_k同样，平行移动未校正传感器组77。即，未校正传感器组移动部59使未校正传感器组77平行移动通过传感器位置顺序校正部57将刚刚校正对象传感器79从校正前位置Q_0，i校正到最终位置P_F，i的校正量(步骤S126)。由此，未校正传感器组77从平行移动前位置U_a-1，i平行移动到平行移动后位置U_a，i。

并且，如图12所示，确认是否对所有的传感器单元S_i完成了位置校正(步骤S127)。在对所有的传感器单元S_i完成了位置校正的情况下(步骤S127-是)，进入下一步骤。在没有对所有的传感器单元S_i完成位置校正的情况下(步骤S127-否)，返回步骤S121，通过传感器位置顺序校正部57进行未校正传感器组77中的最靠基端侧的传感器单元S_i的位置校正。即，在对所有的传感器单元S_i完成位置校正之前，重复进行步骤S121～S126。如上所述，通过利用传感器位置顺序校正部57的从校正前位置Q_0，i到最终位置P_F，i位置的位置校正和利用未校正传感器组移动部59的平行移动，将各个传感器单元S_i的位置从临时位置P_0，i修正到最终位置P_F，i。

如图3所示，传感器位置修正部55与最终曲线形状检测部65连接。最终曲线形状检测部65用最终圆弧L_F，j对各个传感器单元S_i的最终位置P_F，i之间进行曲线插补，并对插入部11的最终曲线形状85进行检测。图16是从全局坐标系C的Z轴的负方向朝向正方向观察利用最终曲线形状检测部65检测到的内窥镜10的插入部11的最终曲线形状85时的图。如图16所示，通过最终曲线形状检测部65对各个传感器单元S_i的最终位置P_F，i之间进行曲线插补。由此，形成各个最终圆弧L_F，j，检测到最终曲线形状85。

最终曲线形状检测部65具有基于各个传感器单元S_i的最终位置P_F，i计算各个最终圆弧L_F，j的参数的参数计算部67、和基于由参数计算部67计算出的参数形成最终圆弧P_F，i的最终圆弧形成部69。最终圆弧形成部69通过使用了四元数(quaternion)和参变量t的插补函数L_F，j(t)，相对于参变量t的变化以等角速度对各个传感器单元S_i的最终位置P_F，i之间进行曲线插补，形成最终圆弧L_F，j。

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

这里，说明利用最终曲线形状检测部65对由传感器位置修正部55进行了位置修正后的各个传感器单元S_i的最终位置P_F，i之间进行曲线插补并对最终曲线形状85进行检测的方法。如图4所示，最终曲线形状检测部65用最终圆弧L_F，j对在步骤S106中进行了位置修正后的各个传感器单元S_i的最终位置P_F，i之间进行曲线插补，并对最终曲线形状85进行检测(步骤S107)。由此，考虑传感器间尺寸I进行各个传感器单元S_i的最终位置P_F，i之间的曲线插补，从而检测到检测精度高的最终曲线形状85。

在对最终曲线形状85进行检测时，最终曲线形状检测部65按照各个传感器单元S_i的各个最终位置P_F，i之间依次进行曲线插补，形成最终圆弧L_F，j。这里，对利用最终曲线形状检测部65进行各个传感器单元S_i的最终位置P_F，i之间的曲线插补的方法进行说明。这里，对从基端侧起第k个传感器单元S_k-1与从基端侧起第(k+1)个传感器单元S_k之间的曲线插补进行说明。即，最终位置P_F，k-1与最终位置P_F，k之间成为作为插补对象的插补对象最终位置间。

图17是示出利用最终曲线形状检测部65检测最终曲线形状85的方法的流程图。图18是说明最终曲线形状检测部65中的处理的图。如图17和图18所示，在通过最终曲线形状检测部65进行传感器单元S_k-1的最终位置P_F，k-1与传感器单元S_k的最终位置P_F，k之间即插补对象的最终位置间的曲线插补时，首先，通过参数计算部67基于最终位置P_F，k-1和最终位置P_F，k，计算最终圆弧L_F，k的参数(步骤S131)。

此时，计算传感器单元S_k-1的最终位置P_F，k-1处的单位切向量和传感器单元S_k的最终位置P_F，k处的单位切向量。传感器单元S_k-1的最终位置P_F，k-1处的单位切向量为

【数学式30】

$\mathrm{\Δ}{\stackrel{\·}{P}}_{F,k}={\stackrel{\·}{P}}_{F,k}-{\stackrel{\·}{P}}_{F,k-1}---\left(30.1\right)$

$\mathrm{\Δ}{L}_{F,k}=\left|\mathrm{\Δ}{\stackrel{\·}{P}}_{F,k}\right|---\left(30.2\right)$

${\stackrel{\·}{e}}_{F,k}=\frac{d\stackrel{\·}{P}}{\mathrm{dL}}|F,k=\frac{\mathrm{\Δ}{\stackrel{\·}{P}}_{F,k}}{\mathrm{\Δ}{L}_{F,k}}---\left(30.3\right).$

即，最终位置P_F，k-1处的单位切向量是从最终位置P_F，k-1到最终位置P_F，k的向量的单位向量。使用以k+1置换了式(30.1)、式(30.2)、式(30.3)中的k的式子求出传感器单元S_k的最终位置P_F，k处的单位切向量。即，最终位置P_F，k处的单位切向量是从最终位置P_F，k到最终位置P_F，k+1的向量的单位向量。

并且，基于最终位置P_F，k-1和P_F，k处的单位切向量，计算传感器单元S_k-1的最终位置P_F，k-1与传感器单元S_k的最终位置P_F，k之间的单位切向量的变化率。首先，最终位置P_F，k-1与最终位置P_F，k之间的单位切向量的变化率向量用下式求出：

【数学式31】

${\stackrel{\·}{\mathrm{\λ}}}_{F,k}=\frac{d^2\stackrel{\·}{P}}{{\mathrm{dL}}^2}|F,k=\frac{d\stackrel{\·}{e}}{\mathrm{dL}}|F,k=\frac{{\stackrel{\·}{e}}_{F,k+1}-{\stackrel{\·}{e}}_{F,k}}{\mathrm{\Δ}{\stackrel{\‾}{L}}_{F,k}}---\left(31.1\right)$

$\mathrm{\Δ}{\stackrel{\‾}{L}}_{F,k}=\frac{\mathrm{\Δ}{L}_{F,k+1}-\mathrm{\Δ}{L}_{F,k}}{2}---\left(31.2\right).$

并且，用式(31.1)、式(31.2)求出的变化率向量的大小为最终位置P_F，k-1与最终位置P_F，k之间的单位切向量的变化率。

并且，基于利用式(30.3)～式(31.2)计算出的单位切向量和变化率，计算传感器单元S_k-1的最终位置P_F，k-1与传感器单元S_k的最终位置P_F，k之间的最终圆弧L_F，k的参数。这里，最终圆弧L_F，k的半径R_F，k是使用式(31.1)、式(31.2)计算出的变化率k的倒数，因此成为

【数学式32】

$R_{F,k}=\frac{1}{\left|{\stackrel{\·}{\mathrm{\λ}}}_{F,k}\right|}---\left(32\right).$

此外，最终圆弧L_F，k的中心O_F，k在全局坐标C中的位置通过下式求出：

【数学式33】

$O_{F,k}={\stackrel{\·}{P}}_{F,k-1}+{R_{F,k}}^2{\stackrel{\·}{\mathrm{\λ}}}_{F,k}---\left(33\right).$

并且，最终圆弧L_F，k的中心角

通过下式求出：

【数学式34】

${\mathrm{\φ}}_{F,k}={\cos }^{-1}\frac{\mathrm{\Δ}{\stackrel{\·}{P}}_{F,k}\·\mathrm{\Δ}{\stackrel{\·}{P}}_{F,k+1}}{\left|\mathrm{\Δ}{\stackrel{\·}{P}}_{F,k}\right|\left|\mathrm{\Δ}{\stackrel{\·}{P}}_{F,k+1}\right|}={\cos }^{-1}({\stackrel{\·}{e}}_{F,k}\·{\stackrel{\·}{e}}_{F,k+1})---\left(34\right).$

另外，关于最终圆弧L_F，k以外的最终圆弧L_F，j，使用以j置换了式(30.1)～式(34)中的k后的式子，与最终圆弧L_F，k同样地，也通过参数计算部67计算最终圆弧L_F，j的参数。即，基于各个传感器单元S_i的最终位置P_F，i，计算各个最终圆弧L_F，j的参数(步骤S131)。

并且，最终圆弧形成部69基于由参数计算部67计算出的最终圆弧L_F，j的参数，对各个传感器单元S_i的最终位置P_F，i之间进行曲线插补，形成最终圆弧L_F，j(步骤S132)。通过使用了四元数(quaternion)和参变量的插补函数进行最终圆弧L_F，j的形成。

这里，四元数是对复数进行扩展后的数，用下式表现：

【数学式35】

q＝w+xi+yj+zk    (35)。

式(35)的i，j，k表示虚数单位，满足下式的关系：

【数学式36】

i²＝j²＝k²＝-1    (36.1)

ij＝k             (36.2)

ji＝-k            (36.3)

jk＝i             (36.4)

kj＝-i            (36.5)

ki＝j             (36.6)

ik＝-j            (36.7)。

如图18所示，在形成传感器单元S_k-1的最终位置P_F，k-1与传感器单元S_k的最终位置P_F，k之间的最终圆弧L_F，k的情况下，在全局坐标系C中，计算从最终圆弧L_F，k的中心O_F，k到传感器单元S_k-1的最终位置P_F，k-1的向量。从中心O_F，k到最终位置P_F，k-1的向量为

【数学式37】

${\stackrel{\·}{q}}_{1,k}=(x_{1,k},y_{1,k},z_{1,k})---\left(37\right).$

此外，在全局坐标系C中，计算从最终圆弧L_F，k的中心O_F，k到传感器单元S_k-1的最终位置P_F，k的向量。从中心O_F，k到最终位置P_F，k的向量为

【数学式38】

${\stackrel{\·}{q}}_{2,k}=(x_{2,k},y_{2,k},z_{2,k})---\left(38\right).$

用式(37)计算出的向量通过四元数表现为

【数学式39】

q_1，k＝x_1，ki+y_1，kj+z_1，kk    (39)。

同样，用式(38)计算出的向量通过四元数表现为

【数学式40】

q_2，k＝x_2，ki+y_2，kj+z_2，kk    (40)。

即，计算出的向量的全局坐标系C的X轴方向分量为四元数的虚数i的分量。同样，向量的全局坐标系C的Y轴方向分量为四元数的虚数i的分量，向量的全局坐标系C的Z轴方向分量为四元数的虚数k的分量。

并且，使用式(39)、式(40)的四元数，定义以下函数L′_F，k(t)。函数L′_F，k(t)为

【数学式41】

L′_F，k(t)＝q_1，ksin(1-t)φ_F，k+q_2，ksintφ_F，k(0≤t≤1)    (41)。

这里，t是参变量。为了归一化而在式(41)的两边除以

时，成为

【数学式42】

$L_{F,k}\left(t\right)=q_{1,k}=\frac{\sin (1-t){\mathrm{\φ}}_{F,k}}{\sin {\mathrm{\φ}}_{F,k}}+q_{2,k}\frac{\sin t{\mathrm{\φ}}_{F,k}}{{\sin \mathrm{\φ}}_{F,k}},(0\≤t\≤1)---\left(42\right),$

从而计算出插补函数L_F，k(t)。

并且，通过在插补函数L_F，k(t)中使t从0变化到1，对传感器单元S_k-1的最终位置P_F，k-1与传感器单元S_k的最终位置P_F，k之间进行曲线插补，形成最终圆弧L_F，k。此时，相对于参变量t的变化以等角速度对最终位置P_F，k-1与最终位置P_F，k之间进行曲线插补。例如，在t从0变化到0.1之间被插补的距离d1与t从0.5变化到0.6之间被插补的距离d2相同(参照图18)。因此，在对最终位置P_F，k-1与最终位置P_F，k之间进行插补时，不需要考虑角速度的变化，因此形成最终圆弧L_F，k的处理单纯化。

另外，关于最终圆弧L_F，k以外的最终圆弧L_F，j，也使用插补函数L_F，j(t)形成。使用以j置换了式(37)～式(42)中的k的式子计算插补函数L_F，j(t)。此时，也相对于插补函数L_F，j(t)的参变量t的变化以等角速度对各个传感器单元S_i的最终位置P_F，i之间进行曲线插补。通过形成所有的最终圆弧L_F，j，检测到内窥镜10的插入部11的最终曲线形状85。

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

因此，在上述结构的内窥镜形状检测装置1及使用内窥镜形状检测装置1的内窥镜10的插入部11的形状检测方法中，发挥以下的效果。即，在内窥镜形状检测装置1中，姿态检测部30根据各个传感器单元S_i的测量数据来检测传感器单元S_i的姿态，由传感器临时位置检测部40根据各个传感器单元S_i的姿态来检测各个传感器单元S_i的临时位置P_0，i。并且，临时曲线形状检测部50用临时圆弧L_0，j对各个传感器单元S_i的临时位置P_0，i之间进行曲线插补，对临时曲线形状75进行检测。并且，传感器位置修正部55基于临时曲线形状75的各个临时圆弧L_0，j的弧长与传感器间尺寸I之间的差的绝对值，对各个传感器单元S_i的位置进行修正。此外，最终曲线形状检测部65用最终圆弧L_F，j对进行了位置修正的各个传感器单元S_i的最终位置P_F，i之间进行曲线插补，并对最终曲线形状85进行检测。如上所述，根据观察时插入体腔内的插入部11所配置的传感器单元S_i的测量数据来检测插入部11的最终曲线形状85，因此不需要在体外设置读出线圈等。因此，能够实现内窥镜形状检测装置1的小型化、单纯化。

此外，在通过传感器位置顺序校正部57对各个传感器单元S_i的位置进行校正时，传感器移动部61将传感器单元S_i从移动前位置Q_t-1，i移动到移动后位置Q_t，i。并且，每当通过传感器移动部61进行一次传感器单元S_i的移动时，通过移动后圆弧形成部62对相对于传感器单元S_i相邻设置于基端侧的传感器单元S_i-1的最终位置P_F，i-1与传感器单元S_i的移动后位置Q_t，i之间进行曲线插补，形成移动后圆弧L_t，j。并且，在形成移动后圆弧L_t，j后，通过移动控制部63计算移动后圆弧L_t，j的弧长

与传感器间尺寸I之间的差的绝对值即能量E_t，j。在能量E_t，j大于预定阈值的情况下，传感器移动部61使传感器单元S_i从位置Q_t，i进一步移动。在能量E_t，j在预定阈值以下的情况下，最终位置确定部64将通过传感器移动部61对传感器单元S_i的最终一次移动的移动后位置Q_F，i确定为作为校正对象传感器81的传感器单元S_i的最终位置P_F，i。即，重复传感器移动部61对传感器单元S_i的移动，直到能量E_t，j为预定阈值以下为止。如上所述，由于基于各个传感器单元S_i之间的实际尺寸即传感器间尺寸I，进行传感器位置顺序校正部57对各个传感器单元S_i的位置校正，因此能够检测与实际的插入部11的形状的误差较小的最终曲线形状85。由此，能够以高精度对插入部11的最终曲线形状85进行检测。

此外，在通过传感器位置修正部55对传感器单元S_i的位置进行修正时，仅相对于传感器单元S_i相邻设置于基端侧的传感器单元S_i-1与传感器单元S_i之间的形状受影响，将临时圆弧L_0，j修正为最终圆弧L_F，j。即，不是每当进行一个传感器单元S_i的位置修正时，就修正插入部11的形状整体的结构。这里，传感器间尺寸I是各个传感器单元S_i在长度方向的尺寸，是插入部11的局部参数。即，传感器间尺寸I不是对插入部11整体的形状产生影响的参数，而是对插入部11的形状产生局部影响的参数。因此，通过一个传感器单元S_i的位置修正，在位置修正后的传感器单元S_i附近对插入部11的形状进行局部修正，因此能够以高精度进行从临时圆弧L_0，j到最终圆弧L_F，j的修正。由此，能够以高精度对插入部11的最终曲线形状85进行检测。此外，由于通过一个传感器单元S_i的位置修正，仅在位置修正后的传感器单元S_i附近对插入部11的形状进行局部修正，因此能够使修正插入部11的形状的处理单纯化。

此外，在最终曲线形状检测部65中，通过在插补函数L_F，j(t)中使t从0变化到1，对各个传感器单元S_i的最终位置P_F，i之间进行曲线插补，形成各个最终圆弧L_F，j。此时，相对于参变量t的变化以等角速度对各个传感器单元S_i的最终位置P_F，i之间进行曲线插补。因此，在对各个最终位置P_F，i之间进行插补时，不需要考虑角速度的变化，因此能够使形成各个最终圆弧L_F，j的处理单纯化。

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

(第2实施方式)

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

与第一实施方式同样，本实施方式的传感器位置修正部55具有：传感器位置顺序校正部57，其从基端侧(离全局坐标系C的原点较近的一侧)的传感器单元S_i依次进行各个传感器单元S_i的从校正前位置Q_0，i向最终位置P_F，i的位置校正；以及未校正传感器组移动部59，其每当传感器位置顺序校正部57进行一个传感器单元S_i的位置校正时，使未完成位置校正的传感器单元S_i即未校正传感器组77平行移动。

与第一实施方式同样，传感器位置顺序校正部57具有使作为位置校正的对象的校正对象传感器81从校正前位置Q_0，i移动一次以上的传感器移动部61。通过利用传感器移动部61的一次移动，校正对象传感器81从移动前位置Q_t-1，i移动到移动后位置Q_t，i。此外，传感器位置顺序校正部57具有移动后圆弧形成部62，每当通过传感器移动部61进行一次校正对象传感器81的移动时，移动后圆弧形成部62对相对于校正对象传感器81相邻设置于基端侧(离全局坐标系C的原点较近的一侧)的传感器单元S_i即近位侧相邻设置传感器82的最终位置P_F，i-1与校正对象传感器81的移动后位置Q_t，i之间进行曲线插补。在移动后圆弧形成部62中对近位侧相邻设置传感器82的最终位置P_F，i-1与校正对象传感器81的移动后位置Q_t，i之间进行曲线插补，由此形成移动后圆弧L_t，j。

传感器位置顺序校正部57具有移动控制部63，该移动控制部63将传感器移动部61控制成重复进行预定次数的校正对象传感器81的移动的状态，该移动使得移动后圆弧L_t，j的弧长

与传感器间尺寸I之间的差的绝对值比对近位侧相邻设置传感器82的最终位置P_F，i-1与校正对象传感器81的移动前的移动前位置Q_t-1，i之间进行曲线插补而形成的移动前圆弧L_t-1，j的弧长

与传感器间尺寸I之间的差的绝对值小。这里，将移动前圆弧L_t-1，j的弧长与传感器间尺寸I之间的差的绝对值设为能量E_t-1，j。同样，将移动后圆弧L_t，j的弧长

与传感器间尺寸I之间的差的绝对值设为能量E_t，j。此外，传感器位置顺序校正部57具有将通过传感器移动部61的最终一次移动的移动后位置Q_F，i确定为校正对象传感器81的最终位置P_F，i的最终位置确定部64。

这里，对利用传感器位置修正部55修正各个传感器单元S_i的位置的方法进行说明。与第一实施方式同样，传感器位置修正部55基于由临时曲线形状检测部50检测到的临时曲线形状75的各个临时圆弧L_0，j的弧长

与传感器间尺寸I之间的差的绝对值，对各个传感器单元S_i的位置进行修正(图4的步骤S106)。即，基于用式(22)求出的能量E_0，j，对各个传感器单元S_i的位置进行修正。由此，将各个传感器单元S_i从临时位置P_0，i修正到最终位置P_F，i。

图19是示出利用传感器位置修正部55修正各个传感器单元S_i的位置的方法的流程图。如图19所示，与第一实施方式同样，在修正各个传感器单元S_i的位置时，通过传感器位置顺序校正部57从基端侧(离全局坐标系C的原点较近的一侧)的传感器单元S_i依次进行各个传感器单元S_i的从校正前位置Q_0，i向最终位置P_F，i的位置校正。传感器位置顺序校正部57开始进行未校正传感器组77中的最靠基端侧的传感器单元S_i的位置校正(步骤S141)。即，在到传感器单元S_k-1为止已经完成了从校正前位置Q_0，i到最终位置P_F，i的位置校正的情况下，通过传感器位置顺序校正部57开始未校正传感器组77中的最靠基端侧的传感器单元S_k的向最终位置P_F，k的位置校正。此时，传感器单元S_k成为作为位置校正的对象的校正对象传感器81。另外，在以下的说明中，对从基端侧起第(k+1)个传感器单元S_k的位置校正进行说明。

在开始进行作为校正对象传感器81的传感器单元S_k的位置校正时，到传感器单元S_k-1为止已经完成了向最终位置P_F，i的位置校正。此时，传感器单元S_k位于校正前位置Q_0，k。在该状态下，通过传感器移动部61将传感器单元S_k从位置Q_0，k移动到位置Q_1，k(步骤S142)，并进行传感器单元S_k的第一次移动。进行1次以上的传感器移动部61对传感器单元S_k的移动。并且，在第s次的移动中，将作为校正对象传感器81的传感器单元S_k从移动前位置Q_s-1，k移动到移动后位置Q_s，k。即，通过传感器移动部61的一次移动，将传感器单元S_k从移动前位置Q_t-1，k移动到移动后位置Q_t，k(步骤S142)。

并且，每当通过传感器移动部61进行一次传感器单元S_k的移动时，移动后圆弧形成部62对相对于传感器单元S_k相邻设置于基端侧的传感器单元S_k-1的最终位置P_{F， k-1}与传感器单元S_k的移动后位置Q_t，k之间进行曲线插补，形成移动后圆弧L_t，k(步骤S143)。在第s次的移动后，对传感器单元S_k-1的最终位置P_F，k-1与传感器单元S_k的移动后位置Q_s，k之间进行曲线插补，形成移动后圆弧L_s，k。此处，传感器单元S_k-1是相对于作为校正对象传感器81的传感器单元S_k相邻设置于离全局坐标系C的原点较近一侧的近位侧相邻设置传感器82。

在传感器单元S_k的第s次的移动后形成移动后圆弧L_s，k时，与第一实施方式同样地使用式(23.1)～式(28)计算移动后圆弧L_s，k的参数。并且使用计算出的参数，形成最终位置P_F，k-1与移动后位置Q_s，k之间的移动后圆弧L_s，k。

另外，对在传感器单元S_k的第s次移动后形成移动后圆弧L_s，k的处理进行了说明，但是关于传感器单元S_k的第s次以外的移动后，使用在式(23.1)～式(28)中用t置换了s后的式子，也同样地形成传感器单元S_k-1的最终位置P_F，k-1与传感器单元S_k的移动后位置Q_t，k之间的移动后圆弧L_t，k。

传感器移动部61被移动控制部63控制成进行校正对象传感器81的移动的状态，该移动使得移动后圆弧L_t，k的弧长与传感器间尺寸I之间的差的绝对值比对近位侧相邻设置传感器82的最终位置P_F，k-1与校正对象传感器81的移动前的移动前位置Q_t-1，k之间进行曲线插补而形成的移动前圆弧L_t-1，k的弧长与传感器间尺寸I之间的差的绝对值小。这里，将移动后圆弧L_t，k的弧长

与传感器间尺寸I之间的差的绝对值设为能量E_t，k，并与第一实施方式同样地用式(29)给出。此外，将移动前圆弧L_t-1，k的弧长与传感器间尺寸I之间的差的绝对值设为能量E_t-1，k，并用以t-1置换了式(29)中的t的式子给出。即，传感器移动部61进行作为校正对象传感器81的传感器单元S_k的移动以变为移动后的能量E_t，k比移动前的能量E_t-1，k小的状态(步骤S142)。另外，例如传感器单元S_k的通过第s次移动的移动前圆弧L_s-1，k与传感器单元S_k的通过第(s-1)次移动的移动后圆弧L_s-1，k是相同的圆弧。使用在式(23.1)～式(28)中以s-1置换了s后的式子，形成传感器单元S_k的通过第s次移动的移动前圆弧L_s-1，k。

并且，移动控制部63判断是否进行了预定次数的作为校正对象传感器81的传感器单元S_k的移动(步骤S144)。在没有进行预定次数的传感器单元S_k的移动的情况下(步骤S144-否)，返回步骤S122，传感器移动部61使传感器单元S_k从位置Q_{t， k}进一步移动。此时，在传感器单元S_k的第s次的移动后，通过传感器移动部61进行第(s+1)次的移动。通过第(s+1)次的移动，传感器单元S_k从移动前位置Q_s，k移动到移动后位置Q_s-1，k。如上所述，传感器移动部61被移动控制部63控制成重复进行预定次数的作为校正对象传感器81的传感器单元S_k的从移动前位置Q_t-1，k向移动后位置Q_t，k的移动的状态。

在进行了预定次数的传感器单元S_k的移动的情况下(步骤S144-是)，进入下一步骤。并且，最终位置确定部64将通过传感器移动部61对传感器单元S_k的最终一次(例如第F次)移动的移动后位置Q_F，k确定为作为校正对象传感器81的传感器单元S_k的最终位置P_F，k(步骤S145)。如上所述，传感器单元S_k的向最终位置P_F，k的位置修正完成。

另外，关于传感器单元S_k以外的传感器单元S_i，也通过传感器位置顺序校正部57与传感器单元S_k同样地进行向最终位置P_i，k的位置校正。即，通过传感器移动部61将传感器单元S_i从移动前位置Q_t-1，i移动到移动后位置Q_t，i(步骤S142)。并且，每当通过传感器移动部61进行一次传感器单元S_i的移动时，通过移动后圆弧形成部62对相对于传感器单元S_i相邻设置于基端侧的传感器单元S_i-1的最终位置P_F，i-1与传感器单元S_i的移动后位置Q_t，i之间进行曲线插补，形成移动后圆弧L_t，j(步骤S143)。传感器移动部61进行传感器单元S_i的移动，以变为移动后圆弧L_t，j的弧长

与传感器间尺寸I之间的差的绝对值比移动前圆弧L_t-1，j的弧长

与传感器间尺寸I之间的差的绝对值小的状态(步骤S142)。即，进行作为校正对象传感器81的传感器单元S_i的移动以变为移动后的能量E_t，j比移动前的能量E_t-1，j小的状态。

并且，在形成了移动后圆弧L_t，j后，判断是否通过移动控制部63进行了预定次数的传感器单元S_i的移动(步骤S144)。在没有进行预定次数的传感器单元S_i的移动的情况下(步骤S144-否)，返回步骤S142，传感器移动部61使传感器单元S_i从位置Q_t，i进一步移动。在进行了预定次数的传感器单元S_i的移动的情况下(步骤S144-是)，进入下一步骤。并且，最终位置确定部64将通过传感器移动部61对传感器单元S_i的最终一次移动的移动后位置Q_F，i确定为作为校正对象传感器81的传感器单元S_i的最终位置P_F，i(步骤S145)。如上所述，传感器单元S_i的向最终位置P_F，i的位置校正完成。

并且，与第一实施方式同样，每当由传感器位置顺序校正部57进行一个传感器单元S_i的位置校正时，通过未校正传感器组移动部59使没有进行位置校正的传感器单元S_i即未校正传感器组77平行移动(步骤S146)。未校正传感器组移动部59中的处理与第一实施方式相同，因此省略详细说明。

并且，确认是否对所有的传感器单元S_i完成了位置校正(步骤S147)。在对所有的传感器单元S_i完成了位置校正的情况下(步骤S147-是)，进入下一步骤。在没有对所有的传感器单元S_i完成位置校正的情况下(步骤S147-否)，返回步骤S141，通过传感器位置顺序校正部57进行未校正传感器组77中的最靠基端侧的传感器单元S_i的位置校正。即，在对所有的传感器单元S_i完成位置校正之前，重复进行步骤S141～S146。

因此，在上述结构的内窥镜形状检测装置1及使用了内窥镜形状检测装置1的内窥镜10的插入部11的形状检测方法中，除了与第一实施方式相同的效果以外，还发挥以下的效果。即，在内窥镜形状检测装置1中，通过传感器位置顺序校正部57对各个传感器单元S_i的位置进行校正时，传感器移动部61将传感器单元S_i从移动前位置Q_t-1，i移动到移动后位置Q_t，i。并且，每当通过传感器移动部61进行一次传感器单元S_i的移动时，通过移动后圆弧形成部62对相对于传感器单元S_i相邻设置于基端侧的传感器单元S_i-1的最终位置P_F，i-1与传感器单元S_i的移动后位置Q_t，i之间进行曲线插补，形成移动后圆弧L_t，j。传感器移动部61进行作为校正对象传感器81的传感器单元S_i的移动，以变为移动后圆弧L_t，j的弧长

与传感器间尺寸I之间的差的绝对值比移动前圆弧L_t-1，j的弧长

与传感器间尺寸I之间的差的绝对值小的状态。并且，在形成了移动后圆弧L_t，j后，判断是否通过移动控制部63进行了预定次数的传感器单元S_i的移动。在没有进行预定次数的传感器单元S_i的移动的情况下，传感器移动部61使传感器单元S_i从位置Q_t，i进一步移动。在进行了预定次数的传感器单元S_i的移动的情况下，最终位置确定部64将通过传感器移动部61对传感器单元S_i的最终一次移动的移动后位置Q_F，i确定为作为校正对象传感器81的传感器单元S_i的最终位置P_F，i。即，重复传感器移动部61对传感器单元S_i的移动，直到进行预定次数的传感器单元S_i的移动为止。如上所述，基于各个传感器单元S_i之间的实际尺寸即传感器间尺寸I，进行传感器位置顺序校正部57对各个传感器单元S_i的位置校正，因此能够检测与实际的插入部11的形状的误差较小的最终曲线形状85。由此，能够以高精度对插入部11的最终曲线形状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)，使用

【数学式43】

${\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{\stackrel{\·}{f}}_{\mathrm{xk}-1}=1\left[\begin{array}{c}-{\sin \mathrm{\γ}}_{k-1}\·{\sin \mathrm{\α}}_{k-1}\·s{\mathrm{in\β}}_{k-1}+\cos {\mathrm{\β}}_{k-1}\·{\cos \mathrm{\γ}}_{k-1}\\ \cos {\mathrm{\γ}}_{k-1}\·{\sin \mathrm{\α}}_{k-1}\·{\sin \mathrm{\β}}_{k-1}+{\cos \mathrm{\β}}_{k-1}\·{\sin \mathrm{\γ}}_{k-1}\\ -\cos {\mathrm{\α}}_{k-1}\·\sin {\mathrm{\β}}_{k-1}\end{array}\right]---\left(43.1\right)$

${\stackrel{\·}{f}}_{\mathrm{xk}-1}={\left[\begin{array}{ccc}1& 0& 0\end{array}\right]}^T---\left(43.2\right).$

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

【数学式44】

${\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{\stackrel{\·}{f}}_{\mathrm{zk}-1}=1\left[\begin{array}{c}{\sin \mathrm{\γ}}_{k-1}\·{\cos \mathrm{\α}}_{k-1}\·\cos {\mathrm{\β}}_{k-1}+\sin {\mathrm{\β}}_{k-1}\·{\cos \mathrm{\γ}}_{k-1}\\ -\cos {\mathrm{\γ}}_{k-1}\·{\sin \mathrm{\α}}_{k-1}\·{\cos \mathrm{\β}}_{k-1}+{\sin \mathrm{\β}}_{k-1}\·{\sin \mathrm{\γ}}_{k-1}\\ \cos {\mathrm{\α}}_{k-1}\·\cos {\mathrm{\β}}_{k-1}\end{array}\right]---\left(44.1\right)$

${\stackrel{\·}{f}}_{\mathrm{zk}-1}={\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T---\left(44.2\right).$

这里，式(43.2)的向量是局部坐标系C_k-1的原点处的长度方向即X_k-1轴方向的单位向量，式(44.2)的向量是局部坐标系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轴方向分量为

【数学式45】

${\stackrel{\·}{a}}_{\mathrm{th}}={\left[\begin{array}{ccc}-g& 0& 0\end{array}\right]}^T---\left(45\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使用以j+1置换了式(16.1)、式(16.2)中的k的式子，求出从临时链T_0，j的前端侧(离全局坐标系C的原点较近的一侧)的传感器单元S_j向临时链T_0，j的基端侧(离全局坐标系C的原点较远的一侧)的传感器单元S_j-1的向量。进而，利用从传感器单元S_j向传感器单元S_j-1的向量形成临时链T_0，j。即，临时链形成部41将临时链T_0，j假定为，沿着前端侧的传感器单元S_j的中心处的长度方向，从前端侧(离全局坐标系C的原点较近的一侧)的传感器单元S_j的中心延伸设置到基端侧(离全局坐标系C的原点较远的一侧)的传感器单元S_j-1的中心，来形成临时链T_0，j。并且，在利用传感器位置修正部55的传感器位置顺序校正部57进行各个传感器单元S_i的位置校正时，从前端侧(离全局坐标系C的原点较近的一侧)的传感器单元S_i依次进行位置校正。此时，与校正对象传感器81的前端侧(离全局坐标系C的原点较近的一侧)相邻设置的传感器单元S_i成为近位侧相邻设置传感器82。

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

Claims (14)

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

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

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

传感器临时位置检测部，其基于由所述姿态检测部检测到的各个所述传感器单元的姿态，将各个所述传感器单元之间假定为尺寸与所述传感器间尺寸相等的直线状的临时链，来检测各个所述传感器单元的临时位置；

临时曲线形状检测部，其基于由所述传感器临时位置检测部检测到的各个所述传感器单元的所述临时位置，用临时圆弧对各个所述传感器单元的所述临时位置之间进行曲线插补，并对所述插入部的临时曲线形状进行检测；

传感器位置修正部，其基于由所述临时曲线形状检测部检测到的所述临时曲线形状的各个所述临时圆弧的弧长与所述传感器间尺寸之间的差的绝对值，将各个所述传感器单元的位置从所述临时位置修正到最终位置；以及

最终曲线形状检测部，其用最终圆弧对各个所述传感器单元的所述最终位置之间进行曲线插补，并对所述插入部的最终曲线形状进行检测。

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

所述临时曲线形状检测部具备：

单位切向量计算部，其计算各个所述传感器单元的所述临时位置处的单位切向量；

变化率计算部，其基于由所述单位切向量计算部计算出的各个所述单位切向量，计算各个所述传感器单元的所述临时位置之间的所述单位切向量的变化率；以及

临时圆弧形成部，其基于由所述单位切向量计算部计算出的各个所述单位切向量和由所述变化率计算部计算出的各个所述变化率，形成各个所述传感器单元的所述临时位置之间的所述临时圆弧。

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

所述传感器位置修正部具备：

传感器位置顺序校正部，其在以最靠基端侧或最靠前端侧的所述传感器单元的中心为原点的全局坐标系中，从离所述全局坐标系的所述原点较近一侧的所述传感器单元起依次进行位置校正，将各个所述传感器单元的位置从位置校正前的校正前位置校正到所述最终位置；以及

未校正传感器组移动部，其每当所述传感器位置顺序校正部进行一个所述传感器单元的位置校正时，使未完成位置校正的所述传感器单元即未校正传感器组平行移动在刚刚的位置校正中作为校正对象的所述传感器单元即刚刚校正对象传感器的从所述校正前位置到所述最终位置的校正量。

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

所述传感器位置顺序校正部具备：

传感器移动部，其使作为位置校正的对象的校正对象传感器从所述校正前位置移动一次以上，利用一次移动使所述校正对象传感器从移动前位置移动到移动后位置；以及

移动后圆弧形成部，其每当所述传感器移动部进行一次所述校正对象传感器的移动时，对相对于所述校正对象传感器相邻设置于离所述全局坐标系的所述原点较近一侧的所述传感器单元即近位侧相邻设置传感器的所述最终位置与所述校正对象传感器的所述移动后位置之间进行曲线插补，形成移动后圆弧。

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

所述传感器位置顺序校正部具备：

移动控制部，其将所述传感器移动部控制成重复进行所述校正对象传感器的从所述移动前位置向所述移动后位置的移动的状态，直到通过所述移动后圆弧形成部形成的所述移动后圆弧的弧长与所述传感器间尺寸之间的差的绝对值为预定阈值以下为止；以及

最终位置确定部，其将所述传感器移动部的最终一次移动的所述移动后位置确定为所述校正对象传感器的所述最终位置。

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

所述传感器位置顺序校正部具备：

移动控制部，其将所述传感器移动部控制成重复进行预定次数的所述校正对象传感器的移动的状态，该移动使得所述移动后圆弧的弧长与所述传感器间尺寸之间的差的绝对值比对所述近位侧相邻设置传感器的所述最终位置与所述校正对象传感器的所述移动前位置之间进行曲线插补而形成的移动前圆弧的弧长与所述传感器间尺寸之间的差的绝对值小；以及

最终位置确定部，其将所述传感器移动部的最终一次移动的所述移动后位置确定为所述校正对象传感器的所述最终位置。

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

所述最终曲线形状检测部具备：

参数计算部，其基于各个所述传感器单元的所述最终位置，计算各个所述最终圆弧的参数；以及

最终圆弧形成部，其基于由所述参数计算部计算出的所述参数，通过使用了四元数和参变量的插补函数，相对于所述参变量的变化以等角速度对各个所述传感器单元的所述最终位置之间进行曲线插补，形成所述最终圆弧。

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

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

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

基于检测到的各个所述传感器单元的姿态，将各个所述传感器单元之间假定为尺寸与所述传感器间尺寸相等的直线状的临时链，来检测各个所述传感器单元的临时位置；

基于检测到的各个所述传感器单元的所述临时位置，用临时圆弧对各个所述传感器单元的所述临时位置之间进行曲线插补，并对所述插入部的临时曲线形状进行检测；

基于检测到的所述临时曲线形状的各个所述临时圆弧的弧长与所述传感器间尺寸之间的差的绝对值，将各个所述传感器单元的位置从所述临时位置修正到最终位置；以及

用最终圆弧对各个所述传感器单元的所述最终位置之间进行曲线插补，并对所述插入部的最终曲线形状进行检测。

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

检测所述临时曲线形状的步骤包括如下步骤：

计算各个所述传感器单元的所述临时位置处的单位切向量；

基于计算出的各个所述单位切向量，计算各个所述传感器单元的所述临时位置之间的所述单位切向量的变化率；以及

基于计算出的各个所述单位切向量和各个所述变化率，形成各个所述传感器单元的所述临时位置之间的所述临时圆弧。

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

将各个所述传感器单元的位置从所述临时位置修正到所述最终位置的步骤包括如下步骤：

在以最靠基端侧或最靠前端侧的所述传感器单元的中心为原点的全局坐标系中，从离所述全局坐标系的所述原点较近一侧的所述传感器单元起依次进行位置校正，将各个所述传感器单元的位置从位置校正前的校正前位置校正到所述最终位置；以及

每当进行一个所述传感器单元的位置校正时，使未完成位置校正的所述传感器单元即未校正传感器组平行移动在刚刚的位置校正中作为校正对象的所述传感器单元即刚刚校正对象传感器的从所述校正前位置到所述最终位置的校正量。

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

从离所述全局坐标系的所述原点较近一侧的所述传感器单元起依次进行各个所述传感器单元的位置校正的步骤包括如下步骤：

使作为位置校正的对象的校正对象传感器从所述校正前位置移动一次以上，利用一次移动使所述校正对象传感器从移动前位置移动到移动后位置；以及

每当进行一次所述校正对象传感器的移动时，对相对于所述校正对象传感器相邻设置于离所述全局坐标系的所述原点较近一侧的所述传感器单元即近位侧相邻设置传感器的所述最终位置与所述校正对象传感器的所述移动后位置之间进行曲线插补，形成移动后圆弧。

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

从离所述全局坐标系的所述原点较近一侧的所述传感器单元起依次进行各个所述传感器单元的位置校正的步骤包括如下步骤：

重复进行所述校正对象传感器的从所述移动前位置向所述移动后位置的移动，直到所形成的所述移动后圆弧的弧长与所述传感器间尺寸之间的差的绝对值为预定阈值以下为止；以及

将最终一次移动的所述移动后位置确定为所述校正对象传感器的所述最终位置。

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

从离所述全局坐标系的所述原点较近一侧的所述传感器单元起依次进行各个所述传感器单元的位置校正的步骤包括如下步骤：

重复进行预定次数的所述校正对象传感器的移动，该移动使得所述移动后圆弧的弧长与所述传感器间尺寸之间的差的绝对值比对所述近位侧相邻设置传感器的所述最终位置与所述校正对象传感器的所述移动前位置之间进行曲线插补而形成的移动前圆弧的弧长与所述传感器间尺寸之间的差的绝对值小；以及

将最终一次移动的所述移动后位置确定为所述校正对象传感器的所述最终位置。

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

检测所述插入部的最终曲线形状的步骤包括如下步骤：

基于各个所述传感器单元的所述最终位置，计算各个所述最终圆弧的参数；以及

基于计算出的所述参数，通过使用了四元数和参变量的插补函数，相对于所述参变量的变化以等角速度对各个所述传感器单元的所述最终位置之间进行曲线插补，形成所述最终圆弧。

Images