CN103228195B - 插入部形状估计装置 - Google Patents

Abstract

插入部形状显示装置（100）具有：插入部（210），其可变形；以及姿态获取部（110），其用于获取插入部（210）的多个测量点的加速度和与姿态有关的姿态信息。插入部形状显示装置（100）包括形状计算部（136）、移动判定部（133）、移动期间确定部（134）、姿态估计部（143）和图像生成部（146）。形状计算部（136）根据姿态信息计算各测量点的姿态和相对于其它测量点的相对位置。移动期间确定部（134）根据移动判定部（133）的判定结果来确定测量点移动的移动期间。姿态估计部（143）估计各测量点在移动期间的姿态。移动矢量计算部（144）计算各测量点在移动期间的移动矢量。图像生成部（146）生成表示相对位置和移动矢量的显示图像。

Description

插入部形状估计装置

技术领域

本发明涉及插入部形状显示装置。

背景技术

一般情况下，已知采用惯性传感器等来检测物体的姿态及移动的方法。根据这样的方法，通过在变形的物体上配置多个传感器，能够估计在静止状态下该物体的形状。

例如，在日本特开2007-151862号公报中公开了配置有加速度传感器的内窥镜的技术。在该内窥镜中，根据加速度传感器的输出来检测配置有加速度传感器的部分的姿态。此外，通过对加速度传感器的输出进行积分，检测配置有加速度传感器的部分的位移量。

此外，在日本特开2011-059020号公报中公开了三维姿态估计装置的技术。该三维姿态估计装置具有加速度传感器和用于估计三维姿态角度的姿态角度估计部。在日本特开2011-059020号公报中公开了如下技术：根据姿态角度估计部估计的姿态角度而从加速度传感器检测出的加速度中除去重力加速度成分，从而高精度地测量移动的加速度。

发明内容

即使如日本特开2011-059020号公报所述那样采用根据姿态而除去了重力加速度成分的加速度，当如专利文献1那样对加速度进行积分而计算出配置有加速度传感器的部分的位置和姿态时，加速度传感器的输出中包括的误差也会累积。因此，关于求得的物体的形状，误差变大。因此，当显示如上述那样求出的物体的形状时，所显示的形状当然误差大。

因此，本发明的目的在于，提供将累积误差的影响降低的插入部形状显示装置，其检测变形的插入部的形状并对其进行显示。

为了实现上述目的，本发明的插入部形状显示装置的一个方式的特征在于，具备：变形的插入部；多个姿态获取部，其分别配置在固定于所述插入部的多个测量点，用于获取施加给各个该测量点的加速度和与各个该测量点在静止状态下的姿态有关的姿态信息；形状计算部，其根据所述插入部的各个所述测量点的位置和各个所述姿态信息来计算各个该测量点的姿态和相对于其它所述测量点的相对位置；移动判定部，其根据所述加速度来进行所述测量点是否正在移动的判定；移动期间确定部，其根据所述判定来确定所述测量点移动的移动期间；姿态估计部，其根据各个所述测量点在所述移动期间的前后的静止状态下的所述姿态来估计各个该测量点在该移动期间的姿态即移动期间姿态；移动矢量计算部，其根据按各个所述移动期间姿态而校正的各个所述加速度来计算各个所述测量点在所述移动期间的移动矢量；图像生成部，其生成表示各个所述相对位置和各个所述移动矢量的显示图像；以及显示部，其显示所述显示图像。

根据本发明，能够提供将累积误差的影响降低的插入部形状显示装置，其检测变形的插入部的形状并对其进行显示。

附图说明

图1是示出了本发明的一个实施方式的插入部形状显示装置的结构例的框图。

图2是用于说明一个实施方式的各种坐标的图。

图3是示出一个实施方式的插入部形状显示装置的处理例的流程图。

图4是示出相对于时间的传感器单元的加速度的一个示例的图。

图5是示出一个实施方式的插入部形状显示装置的静止形状计算处理的流程图。

图6是用于说明插入部的形状的计算的示意图。

图7是示出一个实施方式的插入部形状显示装置的移动状态估计处理的流程图。

图8是用于说明一个实施方式的插入部形状显示装置在显示部上显示的图像例的图。

具体实施方式

参照附图对本发明的一个实施方式进行说明。图1中示出了本实施方式的插入部形状显示装置的结构的概要。在本实施方式中，作为示例示出了为了显示内窥镜的插入部的形状而应用插入部形状显示装置的情况。如图1所示，采用通常的内窥镜200而构成了插入部形状显示装置100。

内窥镜200具备：呈细长形状的插入部210，其用于插入到体腔内；以及操作部220，其设置于插入部210的基端侧。插入部210具备：设置于最前端的前端硬性部212；设置于前端硬性部212的基端侧的弯曲部214；以及设置于弯曲部214的基端侧的细长的挠性管部216。

在前端硬性部212中设置有用于对被摄体进行摄像的CCD等摄像元件232。摄像用信号线234的一端连接于摄像元件232。摄像用信号线234穿过插入部210的内部，其另一端从操作部220延伸到内窥镜200的外部。该摄像用信号线234的另一端与作为图像处理单元的视频处理器236连接。

此外，在前端硬性部212中设置有用于向被摄体照射照明光的照射窗242。光导244的一端连接于照射窗242。光导244穿过插入部210的内部，其另一端从操作部220延伸到内窥镜200的外部。该光导244的另一端与光源单元246连接。

此外，未图示的四根弯曲操作线穿过挠性管部216的内部。该四根弯曲操作线的一端与插入部210的弯曲部214的前端部连接。弯曲操作线的另一端与设置于操作部220的作为弯曲操作部的未图示的弯曲操作旋钮连接。通过操作弯曲操作旋钮，弯曲操作线沿长度方向移动。通过弯曲操作线的移动，弯曲部214向上下左右四个方向弯曲。

在插入部210中，沿着其长轴而设置有多个传感器单元110。在本实施方式中，传感器单元110的数量为N+1个。从设置于插入部210的最基端侧的第0个起至最前端的第N个，按顺序地称为传感器单元110-0至传感器单元110-N。

各传感器单元110-i(i＝0、1、2、……、N)配置于沿着长度方向相互分开一定间隔l的测量点。间隔l例如是50mm。在本实施方式中，各传感器单元110-i按一定间隔l排列，但不限于此，也可以按预先设定的相互不同的间隔来排列。这里，例如最基端侧的传感器单元110-0配置于挠性管部216的基端部，最前端侧的传感器单元110-N配置于弯曲部214的前端部。

各个传感器单元110具备用于测量加速度的加速度传感器112和用于测量地磁的地磁传感器114。这里，将传感器单元110-i所具备的传感器称为加速度传感器112-i和地磁传感器114-i。

在各传感器单元110-i的加速度传感器112-i和地磁传感器114-i上连接有I2C等串行总线122。串行总线122穿过插入部210的内部而从操作部220延伸到内窥镜200的外部。串行总线122的基端与串行转换器124连接。串行转换器124将从各传感器单元110-i经串行总线122而输入的测量数据的串行信号转换成例如USB信号。

USB线缆126的一端与串行转换器124连接。USB线缆126的另一端与控制部130连接。通过各传感器单元110-i测量出的数据的USB信号从串行转换器124输入到控制部130。控制部130既可以是例如个人电脑，也可以是专用的计算机。此外，例如作为显示器的显示部150与控制部130连接。

控制部130具有操控部132、移动判定部133、移动期间确定部134、形状计算部136、移动状态估计部142、图像生成部146和存储器148。操控部132对控制部130的各部的动作进行控制。移动判定部133根据从各传感器单元110-i的加速度传感器112-i输入的加速度来判定传感器单元110是否处于移动状态。若移动判定部133判定为传感器单元110处于移动状态，则移动期间确定部134确定传感器单元110处于移动状态的移动期间。

形状计算部136为了计算静止状态的插入部210的形状而执行静止形状计算处理。形状计算部136具有静止姿态计算部137和坐标计算部138。静止姿态计算部137根据加速度传感器112和地磁传感器114的输出计算各传感器单元110的姿态。坐标计算部138根据静止姿态计算部137计算出的各传感器单元110的姿态而计算各传感器单元110所在的坐标、即各传感器单元110相对于其它各传感器单元110的相对位置。

移动状态估计部142为了计算移动状态的插入部210的状态而执行移动状态计算处理。移动状态估计部142具有姿态估计部143和移动矢量计算部144。姿态估计部143根据静止状态的各传感器单元的姿态而计算移动期间确定部143确定的移动期间的各时刻的各传感器单元110的姿态。移动矢量计算部144根据姿态估计部143计算出的各传感器单元110的姿态而校正作为各加速度传感器112的输出的加速度。移动矢量计算部144根据该校正的加速度而计算移动状态的各传感器单元110的移动矢量。

图像生成部146根据形状计算部136计算出的各传感器单元110的位置坐标和移动状态估计部142计算出的移动状态的各传感器单元110的移动矢量而生成将它们表现出来的图像。图像生成部146使制作出的图像显示于显示部150。存储器148存储各传感器单元110的输出及各部的计算结果。此外，在存储器148中存储有处理所需的各种程序等。显示部150根据图像生成部146的指示而显示图像。

这样，例如插入部210作为变形的插入部而发挥功能。例如传感器单元110作为姿态获取部而发挥功能，其获取施加在测量点的加速度和与各个测量点在静止状态下的姿态相关的姿态信息。例如形状计算部136作为形状计算部而发挥功能，其根据插入部的各个测量点的位置和各个姿态信息来计算各个测量点的姿态和相对于其它测量点的相对位置。例如移动判定部133作为移动判定部而发挥功能，其根据加速度来判定测量点是否正在移动。例如移动期间确定部134作为移动期间确定部而发挥功能，其根据所述判定来确定测量点移动的移动期间。例如姿态估计部143作为姿态估计部而发挥功能，其根据各个测量点在移动期间的前后的静止状态下的姿态来估计各个测量点在移动期间的姿态即移动期间姿态。例如移动矢量计算部144作为移动矢量计算部而发挥功能，其根据按各个移动期间姿态而校正的各个加速度来计算各个测量点在移动期间的移动矢量。例如图像生成部146作为图像生成部而发挥功能，其生成表示各个相对位置和各个移动矢量的显示图像。例如显示部150作为用于对显示图像进行显示的显示部而发挥功能。

这里，参照图2对本实施方式中采用的坐标系进行说明。在图2中示出了内窥镜200的插入部210。在本实施方式中，定义了全局坐标系C_G，该全局坐标系C_G是以最基端侧的传感器单元110-0的中心作为原点、且具有X_G轴、Y_G轴和Z_G轴的右手系统的正交笛卡尔坐标系。在图2中用实线示出了全局坐标系C_G的各坐标轴。这里，X_G轴是与重力所作用的铅垂方向垂直的预定方向。在本实施方式中，使X_G轴与图2中的箭头D1、D2平行，以箭头D1的方向为正。不限定于此，但在本实施方式中以全局坐标系C_G的X_G轴正方向为磁北方向。Y_G轴与铅垂方向垂直、并且与X_G轴向垂直。在本实施方式中，Y_G轴与图2中的箭头E1、E2平行，以箭头E1的方向为正。Z_G轴为铅垂方向，以铅垂上方向为正。该全局坐标系C_G相对于铅垂方向和磁北方向不旋转，随着插入部210的移动而平行移动。

此外，如图2所示，各传感器单元110-i中定义了局部坐标系C_Bi，该局部坐标系C_Bi是以传感器单元110-i的中心作为原点、且具有X_Bi轴、Y_Bi轴和Z_Bi轴的右手系统的正交笛卡尔坐标系。在图2中用虚线示出了各局部坐标系C_Bi的各坐标轴。这里，X_Bi轴与内窥镜200中定义的左右方向一致，是以从基端侧观察前端侧时的内窥镜200的右方向为正的轴。Y_Bi轴与内窥镜200的长度方向一致，是以前端方向为正的轴。Z_Bi轴与内窥镜200中定义的上下方向一致，是以内窥镜200的上方向为正的轴。

相对于全局坐标系C_G，各局部坐标系C_Bi的原点进行平行移动。并且，相对于全局坐标系C_G，各局部坐标系C_Bi分别绕X_G轴旋转α_i、绕Y_G轴旋转β_i、绕Z_G轴旋转γ_i。这里，将α_i称为俯仰角，将β_i称为横滚角，将γ_i称为偏转角。此外，将俯仰角、横滚角、偏转角这三个角总称为姿态角。分别从X_G轴、Y_G轴、Z_G轴的负的方向观察，姿态角α_i、β_i、γ_i以顺时针方向为正。

各加速度传感器112-i测量局部坐标系C_Bi的原点处的加速度的X_Bi轴向成分、Y_Bi轴向成分和Z_Bi轴向成分。此外，各地磁传感器114-i测量局部坐标系C_Bi的原点处的地磁的X_Bi轴向成分、Y_Bi轴向成分和Z_Bi轴向成分。通过根据各加速度传感器120-i和地磁传感器114-i来计算姿态角α_i、β_i、γ_i的值，从而检测传感器单元110-i的姿态。

下面，对本实施方式的插入部形状显示装置100的动作进行说明。图3示出了控制部130的处理的流程图。在步骤S101中，控制部130的操控部132从各传感器单元110-i获取数据。该数据中包括各加速度传感器112-i的输出和地磁传感器114-i的输出。图3所示的本处理是进行重复直至插入部形状显示装置100的动作完毕为止的循环处理。因此，操控部132定期地从各传感器单元110-i获取数据。获取的数据被存储在存储器148中。

在步骤S102中，控制部130的移动判定部133对任一加速度传感器112的输出判定前次获取的值与此次获取的值之差是否大于预定阈值。在所有的加速度传感器112的输出的差在预定阈值以下时，认为插入部210静止。将该状态称为静止状态。此外，在任一加速度传感器112的输出的差大于预定阈值时，认为插入部210正在移动。将该状态称为移动状态。在该判定中，在所有的加速度传感器112的输出的差在预定阈值以下时、即静止状态时，处理移至步骤S103。

在本实施方式的说明中，作为示例而考虑了图4所示的状况。该图示意性地示出了加速度传感器112的输出的代表值。如该图所示，在直至时刻t1为止的期间，加速度传感器112的输出在预定阈值以下，即为静止状态。在从时刻t1至时刻t4为止的期间，为加速度传感器112的输出大于预定阈值的移动状态。在时刻t4以后，加速度传感器112的输出在预定阈值以下，即为静止状态。在例如时刻t＜t1时及时刻t＞t4时，处理移至步骤S103。

在步骤S103中，控制部130的形状计算部136执行静止形状计算处理。参照图5所示的流程图对静止形状计算处理进行说明。在步骤S201中形状计算部136内的静止姿态计算部137根据各加速度传感器112-i的输出来计算俯仰角α_i和横滚角β_i。对俯仰角α_i和横滚角β_i的计算方法进行说明。

姿态角α_i、β_i、γ_i为按偏转角γ_i、俯仰角α_i、横滚角β_i的顺序旋转的(Z、X、Y)类型。因此，用下述算式(1)表示从全局坐标系C_G向局部坐标系C_Bi的旋转矩阵。

[算式1]

$\begin{array}{c}{C}_{\mathrm{Bi}}^G=R_{\mathrm{Zi}}{R}_{\mathrm{Xi}}{R}_{\mathrm{Yi}}\\ =\begin{array}{ccc}\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]& & \end{array}\\ =\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]\end{array}---\left(1\right)$

在插入部210处于静止的静止状态下，加速度中仅重力加速度向铅垂方向的下方向作用。因此，在静止状态下，能够用下述算式(2)表示全局坐标系C_G的加速度矢量a_Gth的X_G轴向成分、Y_G轴向成分和Z_G轴向成分。

[算式2]

a_Gth＝[00-g]^T(2)

用下述算式(3)表示在时刻t通过各加速度传感器112-i测量出的加速度矢量a^t _obsi、即加速度的局部坐标系C_Bi中的X_Bi轴向成分、Y_Bi轴向成分和Z_Bi轴向成分。

[算式3]

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

此时，由于局部坐标系C_Bi是按偏转角γ_i、俯仰角α_i、横滚角β_i的顺序使全局坐标系C_G旋转的坐标系，因此用下述算式(4)表示通过局部坐标系C_Bi观测到的加速度矢量a^t _obsi。

[算式4]

$a_{\mathrm{obsi}}^t=\left[\begin{array}{c}{a}_{\mathrm{Bi}\_X}^t\\ a_{\mathrm{Bi}\_Y}^t\\ a_{\mathrm{Bi}\_Z}^t\end{array}\right]={\left(C_{\mathrm{Bi}}^G\right)}^T{a}_{\mathrm{Gth}}=-g\left[\begin{array}{c}-{\cos \mathrm{\α}}_i^t\·{\sin \mathrm{\β}}_i^t\\ \sin {\mathrm{\α}}_i^t\\ {\cos \mathrm{\α}}_i^t\·{\cos \mathrm{\β}}_i^t\end{array}\right]---\left(4\right)$

这里，若取算式(4)的第一式的平方与算式(4)的第三式的平方的和，则可得到下述算式(5)。

[算式5]

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

通过用算式(4)的第二式除以算式(5)而得到下述算式(6)。

[算式6]

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

这样而求得各局部坐标系C_Bi的各时刻t的俯仰角α^t _i。

此外，通过用算式(4)的第一式除以算式(4)的第三式而得到下述算式(7)。

[算式7]

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

这样而求得各局部坐标系C_Bi的各时刻t的横滚角β^t _i。

如上述那样在各时刻t根据各加速度传感器112-i的测量数据来计算各传感器单元110-i的(即各局部坐标系C_Bi的)俯仰角α^t _i和横滚角β^t _i。

返回到图5继续进行说明。在步骤S202中，静止姿态计算部137根据地磁传感器114的输出来计算偏转角γ^t _i。对偏转角γ^t _i的计算方法进行说明。用下述算式(8)表示通过地磁传感器114-i测量出的时刻t的地磁矢量m^t _obsi、即各地磁的局部坐标系C_Bi中的X_Bi轴向成分、Y_Bi轴向成分和Z_Bi轴向成分。

[算式8]

$m_{\mathrm{obsi}}^t={\left[\begin{array}{ccc}{M}_{\mathrm{Bi}\_X}^t& M_{\mathrm{Bi}\_Y}^t& M_{\mathrm{Bi}\_Z}^t\end{array}\right]}^T---\left(8\right)$

这里，关于用算式(8)表示的各地磁传感器114-i的输出，采用如算式(6)和(7)那样求得的各俯仰角α^t _i和各横滚角β^t _i，通过下述算式(9)计算绕全局坐标系C_G的X_G轴和Y_G轴旋转而校正的地磁m^t _th。

[算式9]

$\begin{array}{c}{m}_{\mathrm{th}}^t=R_{\mathrm{Xi}}{R}_{\mathrm{Yi}}{m}_{\mathrm{obsi}}^t\\ =\left[\begin{array}{ccc}1& 0& 0\\ 0& {\cos \mathrm{\α}}_i^t& -\sin {\mathrm{\α}}_i^t\\ 0& \sin {\mathrm{\α}}_i^t& \cos {\mathrm{\α}}_i^t\end{array}\right]\left[\begin{array}{ccc}{\cos \mathrm{\β}}_i^t& 0& {\sin \mathrm{\β}}_i^t\\ 0& 1& 0\\ -\sin {\mathrm{\β}}_i^t& 0& {\cos \mathrm{\β}}_i^t\end{array}\right]\left[\begin{array}{c}{M}_{\mathrm{Bi}\_X}^t\\ M_{\mathrm{Bi}\_Y}^t\\ M_{\mathrm{Bi}\_Z}^t\end{array}\right]\\ =\left[\begin{array}{ccc}{\cos \mathrm{\β}}_i^t& 0& {\sin \mathrm{\β}}_i^t\\ {\sin \mathrm{\α}}_i^t{\mathrm{si\; n\β}}_i^t& {\cos \mathrm{\α}}_i^t& -{\sin \mathrm{\α}}_i^t{\cos \mathrm{\β}}_i^t\\ -\cos {\mathrm{\α}}_i^t\sin {\mathrm{\β}}_i^t& {\sin \mathrm{\α}}_i^t& {\cos \mathrm{\α}}_i^t{\cos \mathrm{\β}}_i^t\end{array}\right]\left[\begin{array}{c}{M}_{\mathrm{Bi}\_X}^t\\ M_{\mathrm{Bi}\_Y}^t\\ M_{\mathrm{Bi}\_Z}^t\end{array}\right]\end{array}---\left(9\right)$

如下述算式(10)那样表示用这样的上述算式(9)表示的校正后的地磁。

[算式10]

$m_{\mathrm{th}}^t={\left[\begin{array}{ccc}{E}_{i\_X}^t& E_{i\_Y}^t& E_{i\_Z}^t\end{array}\right]}^T---\left(10\right)$

在上述算式(9)和(10)所示的表示地磁的算式中，关于通过地磁传感器114-i测量出的地磁矢量的X_Bi轴向成分、Y_Bi轴向成分和Z_Bi轴向成分，由于俯仰角α^t _i和横滚角β^t _i被校正，因此仅偏转角γ^t _i相对于全局坐标系C_G进行旋转。因此，能够采用算式(10)通过下述算式(11)表示传感器单元110-i相对于磁北所成的角θ^t _{base_i}、即偏转角γ^t _i。

[算式11]

${\mathrm{\γ}}_i^t={\mathrm{\θ}}_{\mathrm{base}\_i}^t={\tan }^{-1}(E_{i\_Y}^t/E_{i\_X}^t)---\left(11\right)$

这里，根据算式(9)和(10)，下述算式(12.1)和(12.2)成立。

[算式12]

$E_{i\_X}^t=M_{\mathrm{Bi}\_X}^t\cos {\mathrm{\β}}_i^t+M_{\mathrm{Bi}\_Z}^t\sin {\mathrm{\β}}_i^t---\left(12.1\right)$

$E_{i\_Y}^t=M_{\mathrm{Bi}\_Y}^t\cos {\mathrm{\α}}_i^t+\sin {\mathrm{\α}}_i^t(M_{\mathrm{Bi}\_X}^t\sin {\mathrm{\β}}_i^t-M_{\mathrm{Bi}\_Z}^t\cos {\mathrm{\β}}_i^t)---\left(12.2\right)$

另外，在本实施方式中，由于全局坐标系C_G的X_G轴向设定成与磁北方向一致，因此传感器单元110-i相对于磁北所成的角θ^t _{base_i}与偏转角γ^t _i一致，但在全局坐标系C_G的X_G轴向与磁北方向不一致的情况下，需要对该偏差进行校正。通过在所成的角θ^t _{base_i}上加减该偏差而能够容易地进行该校正。

如上述那样，形状计算部136的静止姿态计算部137能够根据各加速度传感器112-i和各地磁传感器114-i的输出来求得各时刻t的各传感器单元110-i的俯仰角α^t _i、横滚角β^t _i和偏转角γ^t _i。

返回到图5继续进行说明。在步骤S203中，形状计算部136内的坐标计算部138计算各传感器单元110-i的坐标。对各传感器单元110-i的坐标的计算方法进行说明。图6示出了内窥镜200的插入部210的形状的示意图。如该图所示，多个连杆310相连而形成插入部210，考虑在各连杆的连接部分配置有传感器单元110。这里，在连杆310-i的基端侧配置有传感器单元110-(i-1)，在连杆310-i的前端侧配置有传感器单元110-i。此外，各连杆310的长度为传感器单元110之间的间隔l。

此时，假定配置有传感器单元110-(i-1)的连杆310-i的基端位于全局坐标系C_G的原点时，采用下述算式(13)通过下述算式(14)表示配置有传感器单元110-i的连杆310-i的前端的坐标I^t _i。

[算式13]

e_y＝[010]^T(13)

[算式14]

$I_i^t=I\·C_{\mathrm{Bi}}^G\·e_y=I\left[\begin{array}{c}-\sin {\mathrm{\γ}}_i^t\·\cos {\mathrm{\α}}_i^t\\ \cos {\mathrm{\γ}}_i^t\·\cos {\mathrm{\α}}_i^t\\ \sin {\mathrm{\α}}_i^t\end{array}\right]---\left(14\right)$

将全局坐标系C_G中的各传感器单元110-i的坐标S^t _i表示为下述算式(15)。

[算式15]

$S_i^t={\left[\begin{array}{ccc}{L}_{\mathrm{Gi}\_X}^t& L_{\mathrm{Gi}\_Y}^t& L_{\mathrm{Gi}\_Z}^t\end{array}\right]}^T---\left(15\right)$

此时，采用算式(14)通过下述算式(16)可得到各传感器单元110-i的坐标S^t _i。

[算式16]

$S_i^t=S_{i-1}^t+I_i^t---\left(16\right)$

即，能够从全局坐标系C_G的原点即基端侧起按顺序决定各传感器单元110-i的坐标S^t _i。

如上述那样，形状计算部136能够根据各加速度传感器112-i和各地磁传感器114-i的输出来求出各传感器单元110-i相对于全局坐标系C_G的位置和姿态。即，形状计算部136能够求出内窥镜200的插入部210的形状。然后，形状计算部136完成静止形状计算处理，以各传感器单元110-i的位置和姿态作为返回值，处理返回到步骤S103。计算出的各传感器单元110-i的位置和姿态经操控部132而被存储于存储器148中。完成步骤S103的静止形状计算处理后，处理移至步骤S110。

在步骤S102的判定中，在加速度的差分大于阈值时、即处于移动状态时，处理移至步骤S104。在步骤S104中，移动判定部133判定加速度的差分是否大于阈值。在加速度的差分大于阈值的期间、即插入部210处于移动状态的期间，移动判定部133重复步骤S104中的判定。根据步骤S104的判定，若加速度的差分为阈值以下、即插入部210成为静止状态，则处理移至步骤S105。在步骤S105中，移动期间确定部134确定处于移动状态的期间作为移动期间。

在步骤S106中，形状计算部136对紧挨着移动期间确定部134确定的移动期间之前和之后的两个静止状态执行静止形状计算处理。即，关于紧挨着移动期间之前和之后的两个静止状态，分别通过参照算式(1)至(16)而说明的静止形状计算处理来计算全局坐标系C_G中的各传感器单元110-i的位置和姿态。这里，由于各时刻的从各传感器单元110-i获取的数据存储在存储器148中，因此能够读出并采用该数据。此外，由于紧挨着移动期间之前的各传感器单元110-i的位置和姿态已经在步骤S103中被计算出并存储在存储器148中，因此也可以对其进行使用。此外，通过静止形状计算处理而计算出的各传感器单元110-i的位置和姿态被存储在存储器148中。

在步骤S107中，控制部130的移动状态估计部142执行移动状态估计处理。该移动状态估计处理是估计各传感器单元110-i在移动状态的各时刻t的位置和姿态并计算表示移动状态的移动方向和移动量的移动矢量的处理。参照图7所示的流程图来说明该移动状态估计处理。

在步骤S301中，移动状态估计部142内的姿态估计部143估计各传感器单元110-i在移动状态的各时刻t的姿态。该姿态的估计基于作为移动开始时的时刻t1的姿态和作为移动完毕时的时刻t4的姿态。通过线性插值来求出例如时刻t1与时刻t4之间的各时刻t的姿态。即，用下述算式(17)求得移动状态的各传感器单元110-i的姿态。

[算式17]

${\mathrm{\α}}_i^t={\mathrm{\α}}_i^{t1}+\frac{t-t1}{t4-t1}({\mathrm{\α}}_i^{t4}-{\mathrm{\α}}_i^{t1})$

${\mathrm{\β}}_i^t={\mathrm{\β}}_i^{t1}+\frac{t-t1}{t4-t1}({\mathrm{\β}}_i^{t4}-{\mathrm{\β}}_i^{t1})---\left(17\right)$

${\mathrm{\γ}}_i^t={\mathrm{\γ}}_i^{t1}+\frac{t-t1}{t4-t1}({\mathrm{\γ}}_i^{t4}-{\mathrm{\γ}}_i^{t1})$

另外，线性插值是一个示例，插值方法不限于此。

在步骤S302中，移动状态估计部142内的移动矢量计算部144将各加速度传感器112-i的输出转换成全局坐标系C_G中的加速度。该转换基于根据算式(17)得到的移动状态的各时刻的各传感器单元110的姿态。即，根据下述算式(18)求得全局坐标系C_G中的加速度a^t _Gi。

[算式18]

$\begin{array}{c}{a}_{\mathrm{Gi}}^t=C_B^G{a}_{\mathrm{obsi}}^t\\ =\left[\begin{array}{ccc}-\sin {\mathrm{\γ}}_i^t\·{\sin \mathrm{\α}}_i^t\·{\sin \mathrm{\β}}_i^t+{\cos \mathrm{\β}}_i^t\·{\cos \mathrm{\γ}}_i^t& -\sin {\mathrm{\γ}}_i^t\·{\cos \mathrm{\α}}_i^t& \sin {\mathrm{\γ}}_i^t\·{\sin \mathrm{\α}}_i^t\·\cos {\mathrm{\β}}_i^t+{\sin \mathrm{\β}}_i^t\·{\cos \mathrm{\γ}}_i^t\\ \cos {\mathrm{\γ}}_i^t\·\sin {\mathrm{\α}}_i^t\·\sin {\mathrm{\β}}_i^t+\cos {\mathrm{\β}}_i^t\·\sin {\mathrm{\γ}}_i^t& {\cos \mathrm{\γ}}_i^t\·\cos {\mathrm{\α}}_i^t& -\cos {\mathrm{\γ}}_i^t\·\sin {\mathrm{\α}}_i^t\·{\cos \mathrm{\β}}_i^t+\sin {\mathrm{\β}}_i^t\·\sin {\mathrm{\γ}}_i^t\\ -\cos {\mathrm{\α}}_i^t\·\sin {\mathrm{\β}}_i^t& \sin {\mathrm{\α}}_i^t& \cos {\mathrm{\α}}_i^t\·\cos {\mathrm{\β}}_i^t\end{array}\right]\·\left[\begin{array}{c}{a}_{\mathrm{Bi}\_X}^t\\ a_{\mathrm{Bi}\_Y}^t\\ a_{\mathrm{Bi}\_Z}^t\end{array}\right]\\ =\left[\begin{array}{c}{a}_{\mathrm{Gi}\_X}^t\\ a_{\mathrm{Gi}\_Y}^t\\ a_{\mathrm{Gi}\_Z}^t\end{array}\right]\end{array}---\left(18\right)$

通过从算式(18)所表示的全局坐标系C_G的加速度a^t _Gi中减去重力加速度，求得各传感器单元110-i的运动加速度。即，通过下述算式(19)求得各传感器单元110-i的运动加速度a^t _{Gi_m}。

[算式19]

$\begin{array}{c}{a}_{\mathrm{Gi}\_m}^t=a_{\mathrm{Gi}}^t-{\left[\begin{array}{ccc}0& 0& -g\end{array}\right]}^T\\ =\left[\begin{array}{c}{a}_{\mathrm{Gi}\_X}^t\\ a_{\mathrm{Gi}\_Y}^t\\ a_{\mathrm{Gi}\_Z}^t-g\end{array}\right]\end{array}---\left(19\right)$

在步骤S303中，移动矢量计算部144对各传感器单元110-i的运动加速度a^t _{Gi_m}进行两次积分，从而求出移动矢量。即，通过下述算式(20)得到处于移动状态的各传感器单元110-i的移动矢量d_Gi。

[算式20]

$d_{\mathrm{Gi}}=\underset{t1}{\overset{t4}{\mathrm{\Σ}}}\underset{t1}{\overset{t4}{\mathrm{\Σ}}}{a}_{\mathrm{Gi}\_m}^t=\left[\begin{array}{c}{d}_{\mathrm{Gi}\_X}\\ d_{\mathrm{Gi}\_X}\\ d_{\mathrm{Gi}\_Z}\end{array}\right]---\left(20\right)$

如上述那样，移动状态估计部142能够求出处于移动状态的期间的各传感器单元110-i的移动矢量d_Gi。移动状态估计部142完成移动状态估计处理，以各传感器单元110-i的移动矢量d_Gi作为返回值，处理返回到步骤S107。通过移动状态估计处理而计算出的各传感器单元110-i的移动矢量d_Gi经操控部132而被存储于存储器148中。

返回到图3继续进行说明。在步骤S108中，图像生成部146计算在显示部150上显示的表示移动矢量的箭头即显示矢量的图像上的坐标。首先，通过对在算式(20)中得到的移动矢量d_Gi乘以适当的正系数，确定显示矢量的大小。即，通过下述算式(21)计算矢量r。

[算式21]

$r=k\left[\begin{array}{c}{d}_{\mathrm{Gi}\_X}\\ d_{\mathrm{Gi}\_Y}\\ d_{\mathrm{Gi}\_Z}\end{array}\right]---\left(21\right)$

并且，图像生成部146根据矢量r，采用下述算式(22)来计算显示矢量的起点P_{Gi_s}和终点P_{Gi_e}。

[算式22]

$P_{\mathrm{Gi}\_s}=\left[\begin{array}{c}{L}_{\mathrm{Gi}\_X}\\ L_{\mathrm{Gi}\_Y}\\ L_{\mathrm{Gi}\_Z}\end{array}\right]-\{\mathrm{sign}\left[\begin{array}{c}{d}_{\mathrm{Gi}\_X}\\ d_{\mathrm{Gi}\_Y}\\ d_{\mathrm{Gi}\_Z}\end{array}\right]\·\mathrm{sqrt}\left(\right|\left[\begin{array}{c}{d}_{\mathrm{Gi}\_X}\\ d_{\mathrm{Gi}\_Y}\\ d_{\mathrm{Gi}\_Z}\end{array}\right]\left|\right)+\left[\begin{array}{c}{U}_{i-X}\\ U_{i-Y}\\ U_{i-Z}\end{array}\right]\}/M$

$P_{\mathrm{Gi}\_e}=\left[\begin{array}{c}{L}_{\mathrm{Gi}\_X}\\ L_{\mathrm{Gi}\_Y}\\ L_{\mathrm{Gi}\_Z}\end{array}\right]+\{\mathrm{sign}\left[\begin{array}{c}{d}_{\mathrm{Gi}\_X}\\ d_{\mathrm{Gi}\_Y}\\ d_{\mathrm{Gi}\_Z}\end{array}\right]\·\mathrm{sqrt}\left(\right|\left[\begin{array}{c}{d}_{\mathrm{Gi}\_X}\\ d_{\mathrm{Gi}\_Y}\\ d_{\mathrm{Gi}\_Z}\end{array}\right]\left|\right)+\left[\begin{array}{c}{U}_{i-X}\\ U_{i-Y}\\ U_{i-Z}\end{array}\right]\}/M---\left(22\right)$

这里，M是适当的正整数。此外，sign是这样的函数：若变量为0以上则为1.0，若变量小于0则为－1.0。此外，sqrt是取变量的平方根的函数。求出该平方根是为了调整显示矢量的大小。只要能够适当地调整大小，则采用的算式不限于算式(22)。此外，为了调整显示矢量的大小，还加上了算式(22)中的U＝[U_{i_X}U_{i_Y}U_{i_Z}]。

此外，通过在d_{Gi_X}＞d_{Gi_Y}、并且d_{Gi_X}＞d_{Gi_Z}的情况下，使U＝[U_{i_X}U_{i_Y}U_{i_Z}]为U＝[100]^T、在d_{Gi_Y}＞d_{Gi_X}、并且d_{Gi_Y}＞d_{Gi_Z}的情况下，使U＝[U_{i_X}U_{i_Y}U_{i_Z}]为U＝[010]^T、在dG_{i_Z}＞d_{Gi_X}、并且d_{Gi_Z}＞d_{Gi_Y}的情况下，使U＝[U_{i_X}U_{i_Y}U_{i_Z}]为U＝[001]^T，也能够成为强调移动量为最大的方向的显示矢量。

在步骤S109中，操控部132将变量p设定成预定值P。这里，变量p作为计数器而发挥功能，其用于确定对显示矢量进行显示的期间，预定值P是与对显示矢量进行显示的期间成比例的值。然后，处理移至步骤S110。

在步骤S110中，图像生成部146从存储器148中读出静止状态的插入部210的形状并使其显示于显示部150。在该说明的示例中，在时刻t＜t1时，图像生成部146使在步骤S103中计算出的插入部210的形状显示于显示部150。在时刻t4之后，图像生成部146立即显示在步骤S106中计算出的紧接着移动期间之后的插入部210的形状，然后，在时刻t＞t4时，图像生成部146使在步骤S103中计算出的插入部210的形状显示于显示部150。例如，如图8所示，在显示区域410中设定坐标系420并进行描绘。然后，在该坐标系420中用传感器显示标号432-i表示各传感器单元110-i的位置，用连结显示标号434-i将它们连接起来而描绘出插入部形状430。

在步骤S111中，操控部132判定变量p是否大于0。当p在0以下时，处理移至步骤S114。另一方面，当p大于0时，处理移至步骤S112。在步骤S112中，图像生成部146使在步骤S108中通过算式(22)计算出的具有起点P_{Gi_s}和终点P_{Gi_e}的显示矢量(例如箭头的标记)作为显示矢量440-i而例如图8所示那样显示于插入部形状430的各传感器单元110-i的位置。在步骤S113中，操控部132将变量p设定成(p－1)。然后，处理移至步骤S114。

在步骤S114中，操控部132判定插入部形状显示装置100的动作是否完毕。在动作未完毕时，处理返回到步骤S101。另一方面，在动作完毕时，处理完毕。

通过以上的处理，在显示部150中，在插入部210处于静止状态时，通过步骤S110而使插入部210的形状作为插入部形状430而显示于坐标系420上。另一方面，同样地插入部210的形状作为插入部形状430而显示于坐标系420上直至从移动状态移至静止状态后的预定时间、即作为计数器的变量p从P变成0为止，并且通过步骤S112重叠地显示出移动状态的各显示矢量440-i。

根据本实施方式，与插入部210的形状一同地用箭头将其移动状态显示于显示部150。因此，使用者通过观看该显示能够通过图像来确认插入部210的形状以及移动方向和移动量。这样的显示支援了使用者对插入部210的操作。

根据本实施方式，在静止状态下根据各加速度传感器112-i和各地磁传感器114-i每次都显示插入部210的形状。此外，在移动状态下，根据对重力加速度进行了校正的运动加速度每次都计算该移动状态的移动矢量。并且，按每个一连串的移动状态显示该移动状态的移动矢量。因此，与通过持续对加速度进行积分而计算插入部210的形状、位置和移动量的情况相比，持续积分而产生的误差的累积得以降低。

当然，图8所示的图是表示将插入部210的形状及其移动矢量示出的图像的概要的一个示例，只要是显示插入部210的形状及其移动矢量的图像，则采用怎样的图像都可以。例如，插入部210的形状也可以为模仿内窥镜的形状的显示。此外，也可以例如代替显示矢量440-i而采用动画使表示插入部210的图移动来表现移动矢量。在该情况下，适当地改变步骤S108的处理并计算所需的信息即可。

此外，在本实施方式中，假设用直线的连杆310-i将各传感器单元110-i之间连接起来而进行插入部210的形状的计算，但也可以适当地进行使各传感器单元110-i之间为曲线这样的校正。此外，在本实施方式中，示出了将插入部形状显示装置应用于内窥镜的示例，但也能够应用于贯穿于内窥镜内而使用的导管及其它处置器具等具有变形插入的插入部的各种装置。

另外，在移动状态的运动加速度的估计中，还能够通过在加速度传感器的输出处设置低通滤波器而降低运动加速度的影响来估计重力加速度。在该情况下，也可以不进行基于线性插值的姿态估计而计算移动矢量。

另外，本发明不直接被上述实施方式限定，在实施阶段能够在不脱离其主旨的范围内将技术特征变形而具体化。此外，能够利用在上述实施方式中公开的多个技术特征的适当的组合来形成各种发明。例如，即使从实施方式所示的全部技术特征中删除几个技术特征也能够解决在发明要解决的课题的部分中所述的课题并且能够得到发明效果的情况下，也能够提取被删除了该技术特征的结构作为发明。

Claims (8)

1.一种插入部形状估计装置，其特征在于，

所述插入部形状估计装置具备：

变形的插入部；

加速度计算部，其沿所述插入部的长轴设置在所述插入部中，用于计算在所述插入部上设定的多个测量点各自的加速度；

判定部，其根据由所述加速度计算部计算出的所述加速度来判定所述测量点是否正在移动；

姿态信息计算部，其计算在所述判定部中判断为所述测量点正在移动的移动期间之前的静止状态下的各个所述测量点的第一姿态信息、和所述移动期间之后的静止状态下的各个所述测量点的第二姿态信息；

姿态估计部，其根据在所述姿态信息计算部中计算出的所述第一姿态信息和所述第二姿态信息，估计各个所述测量点在所述移动期间内的姿态即移动期间姿态；

移动矢量计算部，其根据在所述姿态估计部中估计出的所述测量点的所述移动期间姿态，对所述加速度计算部计算出的所述移动期间内的所述加速度进行校正，根据校正后的所述加速度来计算表示各个所述测量点在所述移动期间内的移动方向和移动量的移动矢量；

位置计算部，其根据由所述姿态信息计算部计算出的所述测量点的姿态信息来计算各个所述测量点的位置；以及

图像生成部，其生成并输出表示由所述位置计算部计算出的各个所述测量点的位置和由所述移动矢量计算部计算出的各个所述移动矢量的显示图像。

2.根据权利要求1所述的插入部形状估计装置，其特征在于，

所述姿态信息计算部根据在所述移动期间之前的静止状态下由所述加速度计算部计算出的所述加速度来计算所述第一姿态信息，根据在所述移动期间之后的静止状态下由所述加速度计算部计算出的所述加速度来计算所述第二姿态信息。

3.根据权利要求1所述的插入部形状估计装置，其特征在于，

所述插入部呈细长形状，

所述测量点沿着所述插入部的长轴配置有多个。

4.根据权利要求1所述的插入部形状估计装置，其特征在于，

所述移动矢量计算部根据所述移动期间姿态和在所述移动期间内由所述加速度计算部计算出的所述加速度来计算所述测量点处的运动加速度。

5.根据权利要求4所述的插入部形状估计装置，其特征在于，

所述移动矢量计算部根据所述移动期间姿态，将在所述移动期间内由所述加速度计算部计算出的所述加速度转换为全局坐标系中的加速度，通过从所述全局坐标系中的加速度中减去重力加速度来计算所述运动加速度。

6.根据权利要求4所述的插入部形状估计装置，其特征在于，

所述移动矢量计算部通过对所述运动加速度进行二次积分来计算所述移动矢量。

7.根据权利要求3所述的插入部形状估计装置，其特征在于，

所述图像生成部生成将表示各个所述移动矢量的符号重叠在所述显示图像的与各个所述测量点相应的位置上而形成的所述显示图像。

8.根据权利要求1所述的插入部形状估计装置，其特征在于，

所述插入部形状估计装置还具有地磁传感器，该地磁传感器沿所述插入部的长轴设置在所述插入部中，用于对所述测量点处的地磁进行测量，

所述姿态信息计算部根据在判断为所述测量点正在移动的移动期间之前的静止状态下测量到的所述测量点的所述加速度和所述地磁来计算所述测量点的第一姿态信息，根据在所述移动期间之后的静止状态下测量到的所述测量点的所述加速度和所述地磁来计算所述测量点的第二姿态信息。