WO2013018404A1

WO2013018404A1 - 挿入部形状表示装置

Info

Publication number: WO2013018404A1
Application number: PCT/JP2012/060368
Authority: WO
Inventors: 克己平川
Original assignee: オリンパスメディカルシステムズ株式会社
Priority date: 2011-08-01
Filing date: 2012-04-17
Publication date: 2013-02-07
Also published as: US8698886B2; CN103228195B; JPWO2013018404A1; EP2620094A1; EP2620094A4; US20130201311A1; JP5242865B1; EP2620094B1; CN103228195A

Abstract

　挿入部形状表示装置（１００）は、変形する挿入部（２１０）と、挿入部（２１０）の複数の計測点の加速度と姿勢に関する姿勢情報とを取得する姿勢取得部（１１０）とを有する。挿入部形状表示装置（１００）は、形状算出部（１３６）と、移動判定部（１３３）と、移動期間特定部（１３４）と、姿勢推定部（１４３）と、画像生成部（１４６）とを含む。形状算出部（１３６）は、姿勢情報に基づいて、各計測点の姿勢と他の計測点に対する相対的位置とを算出する。移動期間特定部（１３４）は、移動判定部（１３３）の判定結果に基づいて計測点が移動している移動期間を特定する。姿勢推定部（１４３）は、移動期間における各計測点の姿勢を推定する。移動ベクトル算出部（１４４）は、移動期間における計測点の移動ベクトルを算出する。画像生成部（１４６）は、相対的位置と移動ベクトルとを表す表示画像を生成する。

Description

挿入部形状表示装置

　本発明は、挿入部形状表示装置に関する。

　一般に、慣性センサ等を用いて物体の姿勢や移動を検出する方法が知られている。このような方法によれば、変形する物体にセンサを複数配置することにより、静止状態でその物体の形状を推定することが可能である。

　例えば、日本国特開２００７－１５１８６２号公報には、加速度センサが配置された内視鏡に係る技術が開示されている。この内視鏡では、加速度センサの出力に基づいて、加速度センサが配置された部分の姿勢が検出される。また、加速度センサの出力を積分することにより、加速度センサが配置された部分の変位量が検出される。

　また、日本国特開２０１１－０５９０２０号公報には、三次元姿勢推定装置に係る技術が開示されている。この三次元姿勢推定装置は、加速度センサと、三次元姿勢角度を推定する姿勢角度推定部とを有している。日本国特開２０１１－０５９０２０号公報には、姿勢角度推定部によって推定された姿勢角度に基づいて、加速度センサが検出した加速度から重力加速度成分を除去することで、移動による加速度を精度よく計測する技術が開示されている。

　日本国特開２０１１－０５９０２０号公報のように、姿勢に応じて重力加速度成分を除去した加速度を用いても、特許文献１のように加速度を積分して、加速度センサが配置された部分の位置及び姿勢を算出すると、加速度センサの出力に含まれる誤差が累積する。このため、求まる物体の形状について誤差が大きくなる。したがって、上記のようにして求まった物体の形状を表示すると、表示される形状は、当然誤差が大きなものとなる。

　そこで本発明は、累積誤差の影響を低減した、変形する挿入部の形状を検出してそれを表示する挿入部形状表示装置を提供することを目的とする。

　前記目的を果たすため、本発明の挿入部形状表示装置の一態様は、変形する挿入部と、前記挿入部に固定された複数の計測点の各々に配置され、各々の該計測点に加わる加速度と静止状態における各々の該計測点の姿勢に関する姿勢情報とを取得する複数の姿勢取得部と、前記挿入部における各々の前記計測点の位置と各々の前記姿勢情報とに基づいて、各々の該計測点の姿勢と他の前記計測点に対する相対的位置とを算出する形状算出部と、前記加速度に基づいて前記計測点が移動しているか否かの判定を行う移動判定部と、前記判定に基づいて前記計測点が移動している移動期間を特定する移動期間特定部と、前記移動期間の前後の静止状態における各々の前記計測点の前記姿勢に基づいて、該移動期間における各々の該計測点の姿勢である移動期間姿勢を推定する姿勢推定部と、各々の前記移動期間姿勢によって補正した各々の前記加速度に基づいて、前記移動期間における各々の前記計測点の移動ベクトルを算出する移動ベクトル算出部と、各々の前記相対的位置と各々の前記移動ベクトルとを表す表示画像を生成する画像生成部と、前記表示画像を表示する表示部と、を具備することを特徴とする。

　本発明によれば、累積誤差の影響を低減した、変形する物体の形状を検出してそれを表示する挿入部形状表示装置を提供できる。

図１は、本発明の一実施形態に係る挿入部形状表示装置の構成例を示すブロック図である。図２は、一実施形態に係る各種座標を説明するための図である。図３は、一実施形態に係る挿入部形状表示装置の処理例を示すフローチャートである。図４は、時間に対するセンサユニットの加速度の一例を示す図である。図５は、一実施形態に係る挿入部形状表示装置の静止形状算出処理を示すフローチャートである。図６は、挿入部の形状の算出を説明するための模式図である。図７は、一実施形態に係る挿入部形状表示装置の移動状態推定処理を示すフローチャートである。図８は、一実施形態に係る挿入部形状表示装置が表示部に表示する画像例を説明するための図である。

　本発明の一実施形態について図面を参照して説明する。本実施形態に係る挿入部形状表示装置の構成の概要を図１に示す。本実施形態では、挿入部形状表示装置を内視鏡の挿入部の形状を表示させるために適用する場合を例として示す。図１に示すように、挿入部形状表示装置１００は、一般的な内視鏡２００を用いて構成されている。

　内視鏡２００は、体腔内に挿入される細長形状をした挿入部２１０と、挿入部２１０の基端側に設けられる操作部２２０とを備える。挿入部２１０は、最先端に設けられる先端硬性部２１２と、先端硬性部２１２の基端側に設けられる湾曲部２１４と、湾曲部２１４の基端側に設けられる細長い可撓管部２１６とを備える。

　先端硬性部２１２には、被写体の撮像を行うためのＣＣＤ等の撮像素子２３２が設けられている。撮像素子２３２には、撮像用信号線２３４の一端が接続されている。撮像用信号線２３４は、挿入部２１０の内部を通って、その他端は操作部２２０から内視鏡２００の外部に延出されている。この撮像用信号線２３４の他端は、画像処理ユニットであるビデオプロセッサ２３６に接続されている。

　また、先端硬性部２１２には、被写体に照明を照射するための照射窓２４２が設けられている。照射窓２４２には、ライトガイド２４４の一端が接続されている。ライトガイド２４４は、挿入部２１０の内部を通って、その他端は操作部２２０から内視鏡２００の外部に延出されている。このライトガイド２４４の他端は、光源ユニット２４６に接続されている。

　また、可撓管部２１６の内部には、図示しない４本の湾曲操作ワイヤが通っている。この４本の湾曲操作ワイヤの一端は、挿入部２１０の湾曲部２１４の先端部に接続されている。湾曲操作ワイヤの他端は、操作部２２０に設けられた湾曲操作部である図示しない湾曲操作ノブに接続されている。湾曲操作ノブの操作により、湾曲操作ワイヤが長手方向に移動する。湾曲操作ワイヤの移動により、湾曲部２１４が上下左右の４方向に湾曲する。

　挿入部２１０には、その長軸に沿って、複数のセンサユニット１１０が設けられている。本実施形態では、センサユニット１１０の数は、Ｎ＋１個であるものとする。最も挿入部２１０の基端側に設けられた０番目から最先端のＮ番目まで、順にセンサユニット１１０－０乃至センサユニット１１０－Ｎと称することにする。

　各センサユニット１１０－ｉ（ｉ＝０，１，２，…，Ｎ）は、互いに長手方向に一定の間隔ｌだけ離れた計測点に配置されている。間隔ｌは、例えば５０ｍｍである。本実施形態では、各センサユニット１１０－ｉは一定の間隔ｌで並んでいるものとするが、これに限らず、予め設定した互いに異なる間隔を有して並んでいてもよい。ここで、例えば最も基端側のセンサユニット１１０－０は可撓管部２１６の基端部に配置され、最も先端側のセンサユニット１１０－Ｎは湾曲部２１４の先端部に配置されている。

　それぞれのセンサユニット１１０は、加速度を計測する加速度センサ１１２と、地磁気を計測する地磁気センサ１１４とを備える。ここで、センサユニット１１０－ｉが備えるセンサを、加速度センサ１１２－ｉ及び地磁気センサ１１４－ｉと称することにする。

　各センサユニット１１０－ｉの加速度センサ１１２－ｉ及び地磁気センサ１１４－ｉには、Ｉ２Ｃ等のシリアルバス１２２が接続されている。シリアルバス１２２は、挿入部２１０の内部を通って操作部２２０から内視鏡２００の外部に延出されている。シリアルバス１２２の基端は、シリアルコンバータ１２４に接続されている。シリアルコンバータ１２４は、各センサユニット１１０－ｉからシリアルバス１２２を介して入力される計測データのシリアル信号を、例えばＵＳＢ信号に変換する。

　シリアルコンバータ１２４には、ＵＳＢケーブル１２６の一端が接続されている。ＵＳＢケーブル１２６の他端は、制御部１３０に接続されている。制御部１３０には、各センサユニット１１０－ｉで計測されたデータのＵＳＢ信号がシリアルコンバータ１２４から入力される。制御部１３０は、例えばパーソナルコンピュータであってもよいし、専用の計算機であってもよい。また、制御部１３０には、例えばディスプレイである表示部１５０が接続されている。

　制御部１３０は、コントロール部１３２と、移動判定部１３３と、移動期間特定部１３４と、形状算出部１３６と、移動状態推定部１４２と、画像生成部１４６と、メモリ１４８とを有する。コントロール部１３２は、制御部１３０の各部の動作を制御する。移動判定部１３３は、各センサユニット１１０－ｉの加速度センサ１１２－ｉから入力された加速度に基づいて、センサユニット１１０が移動状態にあるか否かを判定する。移動判定部１３３によってセンサユニット１１０が移動状態にあると判定されたら、移動期間特定部１３４は、センサユニット１１０が移動状態にある移動期間を特定する。

　形状算出部１３６は、静止状態における挿入部２１０の形状を算出するため、静止形状算出処理を実行する。形状算出部１３６は、静止姿勢算出部１３７と、座標算出部１３８とを有する。静止姿勢算出部１３７は、加速度センサ１１２及び地磁気センサ１１４の出力に基づいて、各センサユニット１１０の姿勢を算出する。座標算出部１３８は、静止姿勢算出部１３７が算出した各センサユニット１１０の姿勢に基づいて、各センサユニット１１０が位置する座標、すなわち、各センサユニット１１０の他の各センサユニット１１０に対する相対的位置を算出する。

　移動状態推定部１４２は、移動状態における挿入部２１０の状態を算出するため、移動状態算出処理を実行する。移動状態推定部１４２は、姿勢推定部１４３と、移動ベクトル算出部１４４とを有する。姿勢推定部１４３は、静止状態の各センサユニットの姿勢に基づいて、移動期間特定部１３４が特定した移動期間の各時刻における各センサユニット１１０の姿勢を算出する。移動ベクトル算出部１４４は、姿勢推定部１４３が算出した各センサユニット１１０の姿勢に基づいて、各加速度センサ１１２の出力である加速度を補正する。移動ベクトル算出部１４４は、この補正した加速度に基づいて、移動状態における各センサユニット１１０の移動ベクトルを算出する。

　画像生成部１４６は、形状算出部１３６が算出した各センサユニット１１０の位置座標と、移動状態推定部１４２が算出した移動状態における各センサユニット１１０の移動ベクトルとに基づいて、それらを表現する画像を作成する。画像生成部１４６は、作成した画像を表示部１５０に表示させる。メモリ１４８は、各センサユニット１１０の出力や、各部の計算結果を記憶する。また、メモリ１４８には、処理に必要な各種プログラム等が記憶されている。表示部１５０は、画像生成部１４６の指示に基づいて、画像を表示する。

　このように、例えば挿入部２１０は、変形する挿入部として機能する。例えばセンサユニット１１０は、計測点に加わる加速度と静止状態における各々の計測点の姿勢に関する姿勢情報とを取得する姿勢取得部として機能する。例えば形状算出部１３６は、挿入部における各々の計測点の位置と各々の姿勢情報とに基づいて、各々の計測点の姿勢と他の計測点に対する相対的位置とを算出する形状算出部として機能する。例えば移動判定部１３３は、加速度に基づいて、計測点が移動しているか否かを判定する移動判定部として機能する。例えば移動期間特定部１３４は、前記判定に基づいて、計測点が移動している移動期間を特定する移動期間特定部として機能する。例えば姿勢推定部１４３は、移動期間の前後の静止状態における各々の計測点の姿勢に基づいて、移動期間における各々の計測点の姿勢である移動期間姿勢を推定する姿勢推定部として機能する。例えば移動ベクトル算出部１４４は、各々の移動期間姿勢によって補正した各々の加速度に基づいて、移動期間における各々の計測点の移動ベクトルを算出する移動ベクトル算出部として機能する。例えば画像生成部１４６は、各々の相対的位置と各々の移動ベクトルとを表す表示画像を生成する画像生成部として機能する。例えば表示部１５０は、表示画像を表示する表示部として機能する。

　ここで、本実施形態で用いる座標系について、図２を参照して説明する。図２には、内視鏡２００の挿入部２１０が示されている。本実施形態では、最も基端側のセンサユニット１１０－０の中心を原点とし、Ｘ_Ｇ軸、Ｙ_Ｇ軸、Ｚ_Ｇ軸を有する右手系の直交デカルト座標系であるグローバル座標系Ｃ_Ｇが定義されている。グローバル座標系Ｃ_Ｇの各座標軸は、図３において実線で示されている。ここで、Ｘ_Ｇ軸は、重力が作用する鉛直方向に垂直な所定の方向である。本実施形態では、Ｘ_Ｇ軸を図２中の矢印Ｄ１，Ｄ２に平行とし、矢印Ｄ１の方向を正とする。これに限定されないが、本実施形態ではグローバル座標系Ｃ_ＧのＸ_Ｇ軸正方向を磁北方向とする。Ｙ_Ｇ軸は、鉛直方向に垂直、かつ、Ｘ_Ｇ軸方向に垂直とする。本実施形態では、Ｙ_Ｇ軸は図２中の矢印Ｅ１，Ｅ２に平行とし、矢印Ｅ１の方向を正とする。Ｚ_Ｇ軸は、鉛直方向とし、鉛直上方向を正とする。このグローバル座標系Ｃ_Ｇは、鉛直方向及び磁北方向に対して回転はしないが、挿入部２１０の移動に伴って平行移動する。

　また、図２に示すように、各センサユニット１１０－ｉには、センサユニット１１０－ｉの中心を原点とし、Ｘ_Ｂｉ軸、Ｙ_Ｂｉ軸、Ｚ_Ｂｉ軸を有する右手系の直交デカルト座標系であるローカル座標系Ｃ_Ｂｉが定義されている。各ローカル座標系Ｃ_Ｂｉの各座標軸は、図２において破線で示されている。ここで、Ｘ_Ｂｉ軸は、内視鏡２００に定義された左右方向と一致し、基端側から先端側をみた際の内視鏡２００の右方向を正とする軸である。Ｙ_Ｂｉ軸は、内視鏡２００の長手方向と一致し、先端方向を正とする軸である。Ｚ_Ｂｉ軸は、内視鏡２００に定義された上下方向と一致し、内視鏡２００の上方向を正とする軸である。

　各ローカル座標系Ｃ_Ｂｉは、グローバル座標系Ｃ_Ｇに対して原点が平行移動している。さらに、各ローカル座標系Ｃ_Ｂｉは、グローバル座標系Ｃ_Ｇに対して、Ｘ_Ｇ軸回りにα_ｉ、Ｙ_Ｇ軸回りにβ_ｉ、Ｚ_Ｇ軸回りにγ_ｉそれぞれ回転している。ここで、α_ｉをピッチ角、β_ｉをロール角、γ_ｉをヨー角と称する。また、ピッチ角α_ｉ、ロール角β_ｉ、ヨー角γ_ｉの３つをまとめて姿勢角と称する。姿勢角α_ｉ，β_ｉ，γ_ｉは、それぞれＸ_Ｇ軸、Ｙ_Ｇ軸、Ｚ_Ｇ軸の負の方向から見て時計回りを正とする。

　各加速度センサ１１２－ｉは、ローカル座標系Ｃ_Ｂｉの原点における加速度のＸ_Ｂｉ軸方向成分、Ｙ_Ｂｉ軸方向成分、及びＺ_Ｂｉ軸方向成分を計測する。また、各地磁気センサ１１４－ｉは、ローカル座標系Ｃ_Ｂｉの原点における地磁気のＸ_Ｂｉ軸方向成分、Ｙ_Ｂｉ軸方向成分、Ｚ_Ｂｉ軸方向成分を計測する。各加速度センサ１１２－ｉ及び地磁気センサ１１４－ｉに基づいて姿勢角α_ｉ，β_ｉ，γ_ｉの値を算出することで、センサユニット１１０－ｉの姿勢は検出される。

　次に本実施形態に係る挿入部形状表示装置１００の動作を説明する。制御部１３０による処理のフローチャートを図３に示す。ステップＳ１０１において、制御部１３０のコントロール部１３２は、各センサユニット１１０－ｉからデータを取得する。このデータには、各加速度センサ１１２－ｉの出力と、地磁気センサ１１４－ｉの出力とが含まれる。図３に示す本処理は、挿入部形状表示装置１００の動作が終了するまで繰り返されるループ処理である。したがって、コントロール部１３２は、定期的に各センサユニット１１０－ｉからデータを取得する。取得されたデータは、メモリ１４８に記憶される。

　ステップＳ１０２において、制御部１３０の移動判定部１３３は、何れかの加速度センサ１１２の出力について、前回取得した値と今回取得した値との差が、所定の閾値よりも大きいか否かを判定する。全ての加速度センサ１１２の出力に係る差が所定の閾値以下であるとき、挿入部２１０は静止していると考えられる。この状態を静止状態と称することにする。また、何れかの加速度センサ１１２の出力に係る差が所定の閾値より大きいとき、挿入部２１０は移動していると考えられる。この状態を移動状態と称することにする。この判定において、全ての加速度センサ１１２の出力に係る差が所定の閾値以下であるとき、すなわち静止状態のとき、処理はステップＳ１０３に移る。

　本実施形態の説明では、例として図４に示すような状況を考える。この図は、加速度センサ１１２の出力の代表値を模式的に示す。この図に示すように、時刻ｔ１までの期間において、加速度センサ１１２の出力が所定の閾値以下、すなわち、静止状態であるとする。時刻ｔ１から時刻ｔ４までの期間において、加速度センサ１１２の出力が所定の閾値より大きい移動状態であるとする。時刻ｔ４以降において、加速度センサ１１２の出力が所定の閾値以下、すなわち、静止状態であるとする。例えば時刻ｔ＜ｔ１のときや時刻ｔ＞ｔ４のとき、処理はステップＳ１０３に移る。

　ステップＳ１０３において、制御部１３０の形状算出部１３６は、静止形状算出処理を実行する。静止形状算出処理を図５に示すフローチャートを参照して説明する。ステップＳ２０１において形状算出部１３６内の静止姿勢算出部１３７は、各加速度センサ１１２－ｉの出力に基づいて、ピッチ角α_ｉ及びロール角β_ｉを算出する。ピッチ角α_ｉ及びロール角β_ｉの算出方法を説明する。

　姿勢角α_ｉ，β_ｉ，γ_ｉはヨー角γ_ｉ、ピッチ角α_ｉ、ロール角β_ｉの順で回転する（Ｚ，Ｘ，Ｙ）型とする。したがって、グローバル座標系Ｃ_Ｇからローカル座標系Ｃ_Ｂｉへの回転行列は、下記式（１）で表される。

　挿入部２１０が静止している静止状態では、加速度は重力加速度のみが鉛直方向の下方向に作用している。したがって、静止状態では、グローバル座標系Ｃ_Ｇにおける加速度ベクトルａ_ＧｔｈのＸ_Ｇ軸方向成分、Ｙ_Ｇ軸方向成分、Ｚ_Ｇ軸方向成分は、次式（２）で表すことができる。

　時刻ｔにおける各加速度センサ１１２－ｉで計測される加速度ベクトルａ^ｔ _ｏｂｓｉ、すなわち、加速度のローカル座標系Ｃ_ＢｉにおけるＸ_Ｂｉ軸方向成分、Ｙ_Ｂｉ軸方向成分、Ｚ_Ｂｉ軸方向成分を、次式（３）で表すことにする。

　このとき、ローカル座標系Ｃ_Ｂｉはグローバル座標系Ｃ_Ｇをヨー角γ_ｉ、ピッチ角α_ｉ、ロール角β_ｉの順に回転させた座標系であるので、ローカル座標系Ｃ_Ｂｉで観測される加速度ベクトルａ^ｔ _ｏｂｓｉは、次式（４）で表される。

　ここで、式（４）の第１式の２乗と式（４）の第３式の２乗との和をとると、次式（５）が得られる。

式（４）の第２式を式（５）で割ることにより、次式（６）が得られる。

このようにして各ローカル座標系Ｃ_Ｂｉの各時刻ｔにおけるピッチ角α^ｔ _ｉが求まる。

　また、式（４）の第１式を式（４）の第３式で割ることにより、次式（７）が得られる。

このようにして各ローカル座標系Ｃ_Ｂｉの各時刻ｔにおけるロール角β^ｔ _ｉが求まる。

　以上のようにして、各時刻ｔについて各加速度センサ１１２－ｉによる計測データに基づいて、各センサユニット１１０－ｉの、すなわち各ローカル座標系Ｃ_Ｂｉの、ピッチ角α^ｔ _ｉ及びロール角β^ｔ _ｉが算出される。

　図５に戻って説明を続ける。ステップＳ２０２において、静止姿勢算出部１３７は、地磁気センサ１１４の出力に基づいてヨー角γ^ｔ _ｉを算出する。ヨー角γ^ｔ _ｉの算出方法について説明する。地磁気センサ１１４－ｉで計測される時刻ｔにおける地磁気ベクトルｍ^ｔ _ｏｂｓｉ、すなわち、各地磁気のローカル座標系Ｃ_ＢｉにおけるＸ_Ｂｉ軸方向成分、Ｙ_Ｂｉ軸方向成分、Ｚ_Ｂｉ軸方向成分を、次式（８）で表すことにする。

　ここで、式（８）で表される各地磁気センサ１１４－ｉの出力について、式（６）及び（７）のように求まった各ピッチ角α^ｔ _ｉ及び各ロール角β^ｔ _ｉを用いて、グローバル座標系Ｃ_ＧのＸ_Ｇ軸回り及びＹ_Ｇ軸回りに回転させて補正した地磁気ｍ^ｔ _ｔｈを次式（９）によって算出する。

このような上記式（９）で表される補正後の地磁気を次式（１０）のように表すことにする。

　上記式（９）及び（１０）に示す地磁気を表す式において、地磁気センサ１１４－ｉで計測される地磁気ベクトルのＸ_Ｂｉ軸方向成分、Ｙ_Ｂｉ軸方向成分、Ｚ_Ｂｉ軸方向成分は、ピッチ角α^ｔ _ｉとロール角β^ｔ _ｉとが補正されているので、ヨー角γ^ｔ _ｉのみがグローバル座標系Ｃ_Ｇに対して回転していることになる。したがって、磁北に対してセンサユニット１１０－ｉがなす角θ^ｔ _{ｂａｓｅ＿ｉ}、すなわちヨー角γ^ｔ _ｉは、式（１０）を用いて、次式（１１）で表すことができる。

ここで、式（９）及び（１０）より、次式（１２．１）及び（１２．２）が成り立つ。

　なお、本実施形態では、グローバル座標系Ｃ_ＧのＸ_Ｇ軸方向が磁北方向と一致するように設定しているので磁北に対してセンサユニット１１０－ｉがなす角θ^ｔ _{ｂａｓｅ＿ｉ}がヨー角γ^ｔ _ｉと一致するが、グローバル座標系Ｃ_ＧのＸ_Ｇ軸方向が磁北方向と一致しない場合は、そのずれを補正する必要がある。この補正はなす角θ^ｔ _{ｂａｓｅ＿ｉ}にそのずれを加減することで容易に行える。

　以上のようにして、形状算出部１３６の静止姿勢算出部１３７は、各加速度センサ１１２－ｉ及び各地磁気センサ１１４－ｉの出力に基づいて、各時刻ｔにおける各センサユニット１１０－ｉのピッチ角α^ｔ _ｉ、ロール角β^ｔ _ｉ、ヨー角γ^ｔ _ｉを求めることができる。

　図５に戻って説明を続ける。ステップＳ２０３において、形状算出部１３６内の座標算出部１３８は、各センサユニット１１０－ｉの座標を算出する。各センサユニット１１０－ｉの座標の算出方法について説明する。内視鏡２００の挿入部２１０の形状の模式図を図６に示す。この図に示すように、挿入部２１０は、複数のリンク３１０が連なって形成されており、各リンクの接続部分にセンサユニット１１０が配置されていると考える。ここで、リンク３１０－ｉの基端側にはセンサユニット１１０－（ｉ－１）が配置され、リンク３１０－ｉの先端側にはセンサユニット１１０－ｉが配置されているものとする。また、各リンク３１０の長さは、センサユニット１１０間の間隔ｌとする。

　このとき、センサユニット１１０－（ｉ－１）が配置されたリンク３１０－ｉの基端がグローバル座標系Ｃ_Ｇの原点に位置すると仮定したとき、センサユニット１１０－ｉが配置されたリンク３１０－ｉの先端の座標ｌ^ｔ _ｉは次式（１３）を用いて、次式（１４）で表される。

　グローバル座標系Ｃ_Ｇにおける各センサユニット１１０－ｉの座標Ｓ^ｔ _ｉを次式（１５）と表すものとする。

　このとき、式（１４）を用いて、各センサユニット１１０－ｉの座標Ｓ^ｔ _ｉは、次式（１６）で得られる。

すなわち、グローバル座標系Ｃ_Ｇの原点である基端側から順に、各センサユニット１１０－ｉの座標Ｓ^ｔ _ｉを決定することができる。

　以上のようにして、形状算出部１３６は、各加速度センサ１１２－ｉ及び各地磁気センサ１１４－ｉの出力に基づいて、グローバル座標系Ｃ_Ｇに対する各センサユニット１１０－ｉの位置及び姿勢を求めることができる。すなわち、形状算出部１３６は、内視鏡２００の挿入部２１０の形状を求めることができる。その後、形状算出部１３６は、静止形状算出処理を終了し、各センサユニット１１０－ｉの位置及び姿勢を戻り値として処理はステップＳ１０３に戻る。算出された各センサユニット１１０－ｉの位置及び姿勢は、コントロール部１３２を介してメモリ１４８に記憶される。ステップＳ１０３の静止形状算出処理を終えたら、処理はステップＳ１１０に移る。

　ステップＳ１０２の判定において、加速度の差分が閾値より大きいとき、すなわち移動状態にあるとき、処理はステップＳ１０４に移る。ステップＳ１０４において、移動判定部１３３は、加速度の差分が閾値より大きいか否かを判定する。加速度の差分が閾値より大きい間、すなわち挿入部２１０が移動状態にある間、移動判定部１３３は、ステップＳ１０４の判定を繰り返す。ステップＳ１０４の判定で、加速度の差分が閾値以下となったら、すなわち挿入部２１０が静止状態になったら、処理はステップＳ１０５に移る。ステップＳ１０５において、移動期間特定部１３４は、移動状態にある期間を移動期間として特定する。

　ステップＳ１０６において、形状算出部１３６は、移動期間特定部１３４によって特定された移動期間直前及び直後の２つの静止状態について、静止形状算出処理を実行する。すなわち、移動期間直前及び直後の２つの静止状態のそれぞれについて、式（１）乃至（１６）を参照して説明した静止形状算出処理によって、グローバル座標系Ｃ_Ｇにおける各センサユニット１１０－ｉの位置及び姿勢を算出する。ここで、各時刻における各センサユニット１１０－ｉから取得されたデータは、メモリ１４８に記憶されているので、これを読み出して用いることができる。また、移動期間直前の各センサユニット１１０－ｉの位置及び姿勢は、既にステップＳ１０３で算出され、メモリ１４８に記憶されているので、これを用いてもよい。また、静止形状算出処理によって算出された各センサユニット１１０－ｉの位置及び姿勢は、メモリ１４８に記憶される。

　ステップＳ１０７において、制御部１３０の移動状態推定部１４２は、移動状態推定処理を実行する。この移動状態推定処理は、移動状態の各時刻ｔにおける各センサユニット１１０－ｉの位置及び姿勢を推定し、移動状態における移動方向及び移動量を表す移動ベクトルを算出する処理である。この移動状態推定処理を図７に示すフローチャートを参照して説明する。

　ステップＳ３０１において、移動状態推定部１４２内の姿勢推定部１４３は、移動状態の各時刻ｔにおける各センサユニット１１０－ｉの姿勢を推定する。この姿勢の推定は、移動開始時である時刻ｔ１の姿勢と、移動終了時である時刻ｔ４の姿勢とに基づく。例えば、時刻ｔ１と時刻ｔ４との間の各時刻ｔにおける姿勢を、線形補完によって求める。すなわち、移動状態における各センサユニット１１０－ｉの姿勢は次式（１７）で求まる。

なお、線形補完は一例であり、補完方法はこれに限定されない。

　ステップＳ３０２において、移動状態推定部１４２内の移動ベクトル算出部１４４は、各加速度センサ１１２－ｉの出力を、グローバル座標系Ｃ_Ｇにおける加速度に変換する。この変換は、式（１７）によって得られた移動状態の各時刻における各センサユニット１１０の姿勢に基づく。すなわち、グローバル座標系Ｃ_Ｇにおける加速度ａ^ｔ _Ｇｉは、次式（１８）により求まる。

　式（１８）で表されるグローバル座標系Ｃ_Ｇにおける加速度ａ^ｔ _Ｇｉから、重力加速度を減ずることで、各センサユニット１１０－ｉの運動加速度が求まる。すなわち、各センサユニット１１０－ｉの運動加速度ａ^ｔ _Ｇｉ＿ｍは、次式（１９）で得られる。

　ステップＳ３０３において、移動ベクトル算出部１４４は、各センサユニット１１０－ｉの運動加速度ａ^ｔ _Ｇｉ＿ｍを２回積分することによって、移動ベクトルを求める。すなわち、移動状態にある各センサユニット１１０－ｉの移動ベクトルｄ_Ｇｉは、次式（２０）で得られる。

　以上のようにして、移動状態推定部１４２は、移動状態にある間の各センサユニット１１０－ｉの移動ベクトルｄ_Ｇｉを求めることができる。移動状態推定部１４２は、移動状態推定処理を終了し、各センサユニット１１０－ｉの移動ベクトルｄ_Ｇｉを戻り値として処理はステップＳ１０７に戻る。移動状態推定処理によって算出された各センサユニット１１０－ｉの移動ベクトルｄ_Ｇｉは、コントロール部１３２を介してメモリ１４８に記憶される。

　図３に戻って説明を続ける。ステップＳ１０８において、画像生成部１４６は、表示部１５０に表示する移動ベクトルを表す矢印である表示ベクトルに係る画像上の座標を算出する。まず、表示ベクトルの大きさを、式（２０）で得られる移動ベクトルｄ_Ｇｉに適当な正の係数を掛けることで決定する。すなわち、ベクトルｒを次式（２１）で算出する。

　さらに、画像生成部１４６は、ベクトルｒに基づいて、表示ベクトルの始点Ｐ_Ｇｉ＿ｓと終点Ｐ_Ｇｉ＿ｅとを、次式（２２）を用いて算出する。

ここで、Ｍは適当な正の整数である。また、ｓｉｇｎは、変数が０以上であれば１．０とし、０より小さければ－１．０とする関数である。また、ｓｑｒｔは、変数の平方根を取る関数である。この平方根を求めるのは、表示ベクトルの大きさを調整するためである。適宜大きさを調整できれば、用いる式は式（２２）に限らない。また、式（２２）におけるＵ＝［Ｕ_ｉ＿Ｘ　Ｕ_ｉ＿Ｙ　Ｕ_ｉ＿Ｚ］も表示ベクトルの大きさを調整するために加算されている。

　また、Ｕ＝［Ｕ_ｉ＿Ｘ　Ｕ_ｉ＿Ｙ　Ｕ_ｉ＿Ｚ］を、
　ｄ_{Gi_X}＞ｄ_{Gi_Y}、かつ、ｄ_{Gi_X}＞ｄ_{Gi_Z}の場合は、Ｕ＝［１　０　０］^Ｔ、
　ｄ_{Gi_Y}＞ｄ_{Gi_X}、かつ、ｄ_{Gi_Y}＞ｄ_{Gi_Z}の場合は、Ｕ＝［０　１　０］^Ｔ、
　ｄ_{Gi_Z}＞ｄ_{Gi_X}、かつ、ｄ_{Gi_Z}＞ｄ_{Gi_Y}の場合は、Ｕ＝［０　０　１］^Ｔ、
とすることで、移動量が一番大きい方向を強調する表示ベクトルとすることもできる。

　ステップＳ１０９において、コントロール部１３２は、変数ｐを所定値Ｐに設定する。ここで変数ｐは、表示ベクトルを表示する期間を特定するためのカウンタとして機能し、所定値Ｐは、表示ベクトルを表示する期間に比例する値である。その後、処理はステップＳ１１０に移る。

　ステップＳ１１０において、画像生成部１４６は、静止状態の挿入部２１０の形状をメモリ１４８から読み出して、それを表示部１５０に表示させる。この説明の例では、画像生成部１４６は、時刻ｔ＜ｔ１では、ステップＳ１０３で算出された挿入部２１０の形状を表示部１５０に表示させる。画像生成部１４６は、時刻ｔ４の直後には、ステップＳ１０６で算出された移動期間直後の挿入部２１０の形状を表示させ、その後、時刻ｔ＞ｔ４では、ステップＳ１０３で算出された挿入部２１０の形状を表示部１５０に表示させる。例えば図８に示すように、表示領域４１０に座標系４２０を設定し描画する。次に、この座標系４２０に各センサユニット１１０－ｉの位置をセンサ表示符号４３２－ｉで示し、それらをリンク表示符号４３４－ｉで結んで、挿入部形状４３０を描画する。

　ステップＳ１１１において、コントロール部１３２は、変数ｐが０よりも大きいか否か判定する。ｐが０以下のとき、処理はステップＳ１１４に移る。一方、ｐが０よりも大きいとき、処理はステップＳ１１２に移る。ステップＳ１１２において、画像生成部１４６は、ステップＳ１０８で式（２２）によって算出した始点Ｐ_Ｇｉ＿ｓ及び終点Ｐ_Ｇｉ＿ｅを有する表示ベクトル（例えば矢印の記号）を、挿入部形状４３０の各センサユニット１１０－ｉの位置に表示ベクトル４４０－ｉとして、例えば図８に示すように表示する。ステップＳ１１３においてコントロール部１３２は、変数ｐを（ｐ－１）に設定する。その後、処理はステップＳ１１４に移る。

　ステップＳ１１４において、コントロール部１３２は、挿入部形状表示装置１００の動作が終了したか否かを判定する。動作が終了していないとき、処理はステップＳ１０１に戻る。一方、動作が終了しているとき、処理は終了する。

　以上の処理により、表示部１５０には、座標系４２０上に、挿入部２１０が静止状態にあるときは、ステップＳ１１０によって挿入部２１０の形状が挿入部形状４３０として表示される。一方、移動状態から静止状態に移った後の所定の時間、すなわち、カウンタとしての変数ｐがＰから０になるまでは、同様に座標系４２０上に挿入部２１０の形状が挿入部形状４３０として表示されるとともに、ステップＳ１１２によって移動状態における各表示ベクトル４４０－ｉが重畳表示される。

　本実施形態によれば、挿入部２１０の形状とともに、その移動状態が矢印で表示部１５０に表示される。したがって、ユーザは、この表示を見ることで挿入部２１０の形状並びに移動方向及び移動量を画像で確認することができる。このような表示は、ユーザによる挿入部２１０の操作を支援する。

　本実施形態によれば、静止状態では各加速度センサ１１２－ｉ及び各地磁気センサ１１４－ｉに基づいてその都度、挿入部２１０の形状が表示される。また、移動状態では、その都度当該移動状態における移動ベクトルが、重力加速度を補正した運動加速度に基づいて算出される。そして、一連の移動状態ごとに、その移動状態における移動ベクトルが表示される。したがって、加速度の積分を続けることによって挿入部２１０の形状と位置と移動量とが算出される場合と比較して、積分を続けることによる誤差の累積が低減され得る。

　もちろん図８に示す図は、挿入部２１０の形状とその移動ベクトルとを示す画像の概要を表す一例であり、挿入部２１０の形状とその移動ベクトルとを表示する画像であれば、どのような画像が用いられてもよい。例えば挿入部２１０の形状は内視鏡の形状を模した表示としてもよい。また、例えば表示ベクトル４４０－ｉの代わりにアニメーションを用いて挿入部２１０を表す図が移動するようにして、移動ベクトルが表現されてもよい。この場合、ステップＳ１０８の処理を適宜変更し、必要な情報を算出するようにすればよい。

　また、本実施形態では、各センサユニット１１０－ｉの間は直線のリンク３１０－ｉで結ばれていると仮定して、挿入部２１０の形状の算出が行われているが、各センサユニット１１０－ｉ間を曲線とするような補正が適宜行われてもよい。また、本実施形態では、挿入部形状表示装置が内視鏡に適応される例を示したが、内視鏡内に通して用いられるカテーテルやその他処置具など、変形して挿入される挿入部を有する種々の装置に適用され得る。

　なお、移動状態における運動加速度の推定において、加速度センサの出力にローパスフィルタをかけることで、運動加速度の影響を低減して重力加速度を推定することもできる。その場合、線形補完による姿勢の推定を行わずに移動ベクトルを算出することもできる。

　なお、本発明は上記実施形態そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。また、上記実施形態に開示されている複数の構成要素の適宜な組み合わせにより、種々の発明を形成できる。例えば、実施形態に示される全構成要素から幾つかの構成要素を削除しても、発明が解決しようとする課題の欄で述べられた課題が解決でき、かつ、発明の効果が得られる場合には、この構成要素が削除された構成も発明として抽出され得る。

Claims

　変形する挿入部（２１０）と、
　前記挿入部（２１０）に固定された複数の計測点の各々に配置され、各々の該計測点に加わる加速度と静止状態における各々の該計測点の姿勢に関する姿勢情報とを取得する複数の姿勢取得部（１１０）と、
　前記挿入部（２１０）における各々の前記計測点の位置と各々の前記姿勢情報とに基づいて、各々の該計測点の姿勢と他の前記計測点に対する相対的位置とを算出する形状算出部（１３６）と、
　前記加速度に基づいて前記計測点が移動しているか否かの判定を行う移動判定部（１３３）と、
　前記判定に基づいて前記計測点が移動している移動期間を特定する移動期間特定部（１３４）と、
　前記移動期間の前後の静止状態における各々の前記計測点の前記姿勢に基づいて、該移動期間における各々の該計測点の姿勢である移動期間姿勢を推定する姿勢推定部（１４３）と、
　各々の前記移動期間姿勢によって補正した各々の前記加速度に基づいて、前記移動期間における各々の前記計測点の移動ベクトルを算出する移動ベクトル算出部（１４４）と、
　各々の前記相対的位置と各々の前記移動ベクトルとを表す表示画像を生成する画像生成部（１４６）と、
　前記表示画像を表示する表示部（１５０）と、
　を具備することを特徴とする挿入部形状表示装置（１００）。
　前記画像生成部（１４６）は、前記移動期間の終了後の前記相対的位置を表す図に前記移動ベクトルを表す記号を重畳した前記表示画像を生成することを特徴とする請求項１に記載の挿入部形状表示装置（１００）。
　前記姿勢情報は、前記加速度を含むことを特徴とする請求項１又は２に記載の挿入部形状表示装置（１００）。
　前記挿入部（２１０）は細長形状をしており、
　前記計測点は、前記挿入部（２１０）の長軸に沿って複数配置されている、
　ことを特徴とする請求項１又は２に記載の挿入部形状表示装置（１００）。
　前記移動ベクトル算出部（１４４）は、
　　前記加速度に含まれる重力加速度を前記移動期間姿勢に基づいて補正して、グローバル座標系における前記計測点の移動に伴う加速度であるグローバル座標系加速度を算出し、
　　前記グローバル座標系加速度を２回積分することで前記移動ベクトルを算出する、
　ことを特徴とする請求項１又は２に記載の挿入部形状表示装置（１００）。
　前記画像生成部（１４６）は、前記表示画像における各々の前記計測点に該当する位置に各々の前記移動ベクトルを表す前記記号を重畳した前記表示画像を生成することを特徴する請求項１又は２に記載の挿入部形状表示装置（１００）。
　前記姿勢取得部（１１０）は、
　　直交する３軸方向の加速度を計測する加速度センサ（１１２）と、
　　地磁気を計測する地磁気センサ（１１４）と、
　からなることを特徴する請求項１又は２に記載の挿入部形状表示装置（１００）。