(57)【要約】  挿入部形状表示装置（１００）は、変形する挿入部（２１０）と、挿入部（２１０）の複数の計測点の加速度と姿勢に関する姿勢情報とを取得する姿勢取得部（１１０）とを有する。挿入部形状表示装置（１００）は、形状算出部（１３６）と、移動判定部（１３３）と、移動期間特定部（１３４）と、姿勢推定部（１４３）と、画像生成部（１４６）とを含む。形状算出部（１３６）は、姿勢情報に基づいて、各計測点の姿勢と他の計測点に対する相対的位置とを算出する。移動期間特定部（１３４）は、移動判定部（１３３）の判定結果に基づいて計測点が移動している移動期間を特定する。姿勢推定部（１４３）は、移動期間における各計測点の姿勢を推定する。移動ベクトル算出部（１４４）は、移動期間における計測点の移動ベクトルを算出する。画像生成部（１４６）は、相対的位置と移動ベクトルとを表す表示画像を生成する。
2013018404.tif 000002
【特許請求の範囲】
【請求項１】
変形する挿入部（２１０）と、
前記挿入部（２１０）に固定された複数の計測点の各々に配置され、各々の該計測点に加わる加速度と静止状態における各々の該計測点の姿勢に関する姿勢情報とを取得する複数の姿勢取得部（１１０）と、
前記挿入部（２１０）における各々の前記計測点の位置と各々の前記姿勢情報とに基づいて、各々の該計測点の姿勢と他の前記計測点に対する相対的位置とを算出する形状算出部（１３６）と、
前記加速度に基づいて前記計測点が移動しているか否かの判定を行う移動判定部（１３３）と、
前記判定に基づいて前記計測点が移動している移動期間を特定する移動期間特定部（１３４）と、
前記移動期間の前後の静止状態における各々の前記計測点の前記姿勢に基づいて、該移動期間における各々の該計測点の姿勢である移動期間姿勢を推定する姿勢推定部（１４３）と、
各々の前記移動期間姿勢によって補正した各々の前記加速度に基づいて、前記移動期間における各々の前記計測点の移動ベクトルを算出する移動ベクトル算出部（１４４）と、
各々の前記相対的位置と各々の前記移動ベクトルとを表す表示画像を生成する画像生成部（１４６）と、
前記表示画像を表示する表示部（１５０）と、
を具備することを特徴とする挿入部形状表示装置（１００）。
【請求項２】
前記画像生成部（１４６）は、前記移動期間の終了後の前記相対的位置を表す図に前記移動ベクトルを表す記号を重畳した前記表示画像を生成することを特徴とする請求項１に記載の挿入部形状表示装置（１００）。
【請求項３】
前記姿勢情報は、前記加速度を含むことを特徴とする請求項１又は２に記載の挿入部形状表示装置（１００）。
【請求項４】
前記挿入部（２１０）は細長形状をしており、
前記計測点は、前記挿入部（２１０）の長軸に沿って複数配置されている、
ことを特徴とする請求項１又は２に記載の挿入部形状表示装置（１００）。
【請求項５】
前記移動ベクトル算出部（１４４）は、
前記加速度に含まれる重力加速度を前記移動期間姿勢に基づいて補正して、グローバル座標系における前記計測点の移動に伴う加速度であるグローバル座標系加速度を算出し、
前記グローバル座標系加速度を２回積分することで前記移動ベクトルを算出する、
ことを特徴とする請求項１又は２に記載の挿入部形状表示装置（１００）。
【請求項６】
前記画像生成部（１４６）は、前記表示画像における各々の前記計測点に該当する位置に各々の前記移動ベクトルを表す前記記号を重畳した前記表示画像を生成することを特徴する請求項１又は２に記載の挿入部形状表示装置（１００）。
【請求項７】
前記姿勢取得部（１１０）は、
直交する３軸方向の加速度を計測する加速度センサ（１１２）と、
地磁気を計測する地磁気センサ（１１４）と、
からなることを特徴する請求項１又は２に記載の挿入部形状表示装置（１００）。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、挿入部形状表示装置に関する。
【背景技術】
【０００２】
一般に、慣性センサ等を用いて物体の姿勢や移動を検出する方法が知られている。このような方法によれば、変形する物体にセンサを複数配置することにより、静止状態でその物体の形状を推定することが可能である。【０００３】
例えば、日本国特開２００７−１５１８６２号公報には、加速度センサが配置された内視鏡に係る技術が開示されている。この内視鏡では、加速度センサの出力に基づいて、加速度センサが配置された部分の姿勢が検出される。また、加速度センサの出力を積分することにより、加速度センサが配置された部分の変位量が検出される。【０００４】
また、日本国特開２０１１−０５９０２０号公報には、三次元姿勢推定装置に係る技術が開示されている。この三次元姿勢推定装置は、加速度センサと、三次元姿勢角度を推定する姿勢角度推定部とを有している。日本国特開２０１１−０５９０２０号公報には、姿勢角度推定部によって推定された姿勢角度に基づいて、加速度センサが検出した加速度から重力加速度成分を除去することで、移動による加速度を精度よく計測する技術が開示されている。
【発明の概要】
【０００５】
日本国特開２０１１−０５９０２０号公報のように、姿勢に応じて重力加速度成分を除去した加速度を用いても、特許文献１のように加速度を積分して、加速度センサが配置された部分の位置及び姿勢を算出すると、加速度センサの出力に含まれる誤差が累積する。このため、求まる物体の形状について誤差が大きくなる。したがって、上記のようにして求まった物体の形状を表示すると、表示される形状は、当然誤差が大きなものとなる。
【０００６】
そこで本発明は、累積誤差の影響を低減した、変形する挿入部の形状を検出してそれを表示する挿入部形状表示装置を提供することを目的とする。
【０００７】
前記目的を果たすため、本発明の挿入部形状表示装置の一態様は、変形する挿入部と、前記挿入部に固定された複数の計測点の各々に配置され、各々の該計測点に加わる加速度と静止状態における各々の該計測点の姿勢に関する姿勢情報とを取得する複数の姿勢取得部と、前記挿入部における各々の前記計測点の位置と各々の前記姿勢情報とに基づいて、各々の該計測点の姿勢と他の前記計測点に対する相対的位置とを算出する形状算出部と、前記加速度に基づいて前記計測点が移動しているか否かの判定を行う移動判定部と、前記判定に基づいて前記計測点が移動している移動期間を特定する移動期間特定部と、前記移動期間の前後の静止状態における各々の前記計測点の前記姿勢に基づいて、該移動期間における各々の該計測点の姿勢である移動期間姿勢を推定する姿勢推定部と、各々の前記移動期間姿勢によって補正した各々の前記加速度に基づいて、前記移動期間における各々の前記計測点の移動ベクトルを算出する移動ベクトル算出部と、各々の前記相対的位置と各々の前記移動ベクトルとを表す表示画像を生成する画像生成部と、前記表示画像を表示する表示部と、を具備することを特徴とする。
【０００８】
本発明によれば、累積誤差の影響を低減した、変形する物体の形状を検出してそれを表示する挿入部形状表示装置を提供できる。
【図面の簡単な説明】
【０００９】
【図１】図１は、本発明の一実施形態に係る挿入部形状表示装置の構成例を示すブロック図である。
【図２】図２は、一実施形態に係る各種座標を説明するための図である。
【図３】図３は、一実施形態に係る挿入部形状表示装置の処理例を示すフローチャートである。
【図４】図４は、時間に対するセンサユニットの加速度の一例を示す図である。
【図５】図５は、一実施形態に係る挿入部形状表示装置の静止形状算出処理を示すフローチャートである。
【図６】図６は、挿入部の形状の算出を説明するための模式図である。
【図７】図７は、一実施形態に係る挿入部形状表示装置の移動状態推定処理を示すフローチャートである。
【図８】図８は、一実施形態に係る挿入部形状表示装置が表示部に表示する画像例を説明するための図である。
【発明を実施するための形態】
【００１０】
本発明の一実施形態について図面を参照して説明する。本実施形態に係る挿入部形状表示装置の構成の概要を図１に示す。本実施形態では、挿入部形状表示装置を内視鏡の挿入部の形状を表示させるために適用する場合を例として示す。図１に示すように、挿入部形状表示装置１００は、一般的な内視鏡２００を用いて構成されている。
【００１１】
内視鏡２００は、体腔内に挿入される細長形状をした挿入部２１０と、挿入部２１０の基端側に設けられる操作部２２０とを備える。挿入部２１０は、最先端に設けられる先端硬性部２１２と、先端硬性部２１２の基端側に設けられる湾曲部２１４と、湾曲部２１４の基端側に設けられる細長い可撓管部２１６とを備える。
【００１２】
先端硬性部２１２には、被写体の撮像を行うためのＣＣＤ等の撮像素子２３２が設けられている。撮像素子２３２には、撮像用信号線２３４の一端が接続されている。撮像用信号線２３４は、挿入部２１０の内部を通って、その他端は操作部２２０から内視鏡２００の外部に延出されている。この撮像用信号線２３４の他端は、画像処理ユニットであるビデオプロセッサ２３６に接続されている。
【００１３】
また、先端硬性部２１２には、被写体に照明を照射するための照射窓２４２が設けられている。照射窓２４２には、ライトガイド２４４の一端が接続されている。ライトガイド２４４は、挿入部２１０の内部を通って、その他端は操作部２２０から内視鏡２００の外部に延出されている。このライトガイド２４４の他端は、光源ユニット２４６に接続されている。
【００１４】
また、可撓管部２１６の内部には、図示しない４本の湾曲操作ワイヤが通っている。この４本の湾曲操作ワイヤの一端は、挿入部２１０の湾曲部２１４の先端部に接続されている。湾曲操作ワイヤの他端は、操作部２２０に設けられた湾曲操作部である図示しない湾曲操作ノブに接続されている。湾曲操作ノブの操作により、湾曲操作ワイヤが長手方向に移動する。湾曲操作ワイヤの移動により、湾曲部２１４が上下左右の４方向に湾曲する。
【００１５】
挿入部２１０には、その長軸に沿って、複数のセンサユニット１１０が設けられている。本実施形態では、センサユニット１１０の数は、Ｎ＋１個であるものとする。最も挿入部２１０の基端側に設けられた０番目から最先端のＮ番目まで、順にセンサユニット１１０−０乃至センサユニット１１０−Ｎと称することにする。
【００１６】
各センサユニット１１０−ｉ（ｉ＝０，１，２，…，Ｎ）は、互いに長手方向に一定の間隔ｌだけ離れた計測点に配置されている。間隔ｌは、例えば５０ｍｍである。本実施形態では、各センサユニット１１０−ｉは一定の間隔ｌで並んでいるものとするが、これに限らず、予め設定した互いに異なる間隔を有して並んでいてもよい。ここで、例えば最も基端側のセンサユニット１１０−０は可撓管部２１６の基端部に配置され、最も先端側のセンサユニット１１０−Ｎは湾曲部２１４の先端部に配置されている。
【００１７】
それぞれのセンサユニット１１０は、加速度を計測する加速度センサ１１２と、地磁気を計測する地磁気センサ１１４とを備える。ここで、センサユニット１１０−ｉが備えるセンサを、加速度センサ１１２−ｉ及び地磁気センサ１１４−ｉと称することにする。
【００１８】
各センサユニット１１０−ｉの加速度センサ１１２−ｉ及び地磁気センサ１１４−ｉには、Ｉ２Ｃ等のシリアルバス１２２が接続されている。シリアルバス１２２は、挿入部２１０の内部を通って操作部２２０から内視鏡２００の外部に延出されている。シリアルバス１２２の基端は、シリアルコンバータ１２４に接続されている。シリアルコンバータ１２４は、各センサユニット１１０−ｉからシリアルバス１２２を介して入力される計測データのシリアル信号を、例えばＵＳＢ信号に変換する。
【００１９】
シリアルコンバータ１２４には、ＵＳＢケーブル１２６の一端が接続されている。ＵＳＢケーブル１２６の他端は、制御部１３０に接続されている。制御部１３０には、各センサユニット１１０−ｉで計測されたデータのＵＳＢ信号がシリアルコンバータ１２４から入力される。制御部１３０は、例えばパーソナルコンピュータであってもよいし、専用の計算機であってもよい。また、制御部１３０には、例えばディスプレイである表示部１５０が接続されている。
【００２０】
制御部１３０は、コントロール部１３２と、移動判定部１３３と、移動期間特定部１３４と、形状算出部１３６と、移動状態推定部１４２と、画像生成部１４６と、メモリ１４８とを有する。コントロール部１３２は、制御部１３０の各部の動作を制御する。移動判定部１３３は、各センサユニット１１０−ｉの加速度センサ１１２−ｉから入力された加速度に基づいて、センサユニット１１０が移動状態にあるか否かを判定する。移動判定部１３３によってセンサユニット１１０が移動状態にあると判定されたら、移動期間特定部１３４は、センサユニット１１０が移動状態にある移動期間を特定する。
【００２１】
形状算出部１３６は、静止状態における挿入部２１０の形状を算出するため、静止形状算出処理を実行する。形状算出部１３６は、静止姿勢算出部１３７と、座標算出部１３８とを有する。静止姿勢算出部１３７は、加速度センサ１１２及び地磁気センサ１１４の出力に基づいて、各センサユニット１１０の姿勢を算出する。座標算出部１３８は、静止姿勢算出部１３７が算出した各センサユニット１１０の姿勢に基づいて、各センサユニット１１０が位置する座標、すなわち、各センサユニット１１０の他の各センサユニット１１０に対する相対的位置を算出する。
【００２２】
移動状態推定部１４２は、移動状態における挿入部２１０の状態を算出するため、移動状態算出処理を実行する。移動状態推定部１４２は、姿勢推定部１４３と、移動ベクトル算出部１４４とを有する。姿勢推定部１４３は、静止状態の各センサユニットの姿勢に基づいて、移動期間特定部１３４が特定した移動期間の各時刻における各センサユニット１１０の姿勢を算出する。移動ベクトル算出部１４４は、姿勢推定部１４３が算出した各センサユニット１１０の姿勢に基づいて、各加速度センサ１１２の出力である加速度を補正する。移動ベクトル算出部１４４は、この補正した加速度に基づいて、移動状態における各センサユニット１１０の移動ベクトルを算出する。
【００２３】
画像生成部１４６は、形状算出部１３６が算出した各センサユニット１１０の位置座標と、移動状態推定部１４２が算出した移動状態における各センサユニット１１０の移動ベクトルとに基づいて、それらを表現する画像を作成する。画像生成部１４６は、作成した画像を表示部１５０に表示させる。メモリ１４８は、各センサユニット１１０の出力や、各部の計算結果を記憶する。また、メモリ１４８には、処理に必要な各種プログラム等が記憶されている。表示部１５０は、画像生成部１４６の指示に基づいて、画像を表示する。
【００２４】
このように、例えば挿入部２１０は、変形する挿入部として機能する。例えばセンサユニット１１０は、計測点に加わる加速度と静止状態における各々の計測点の姿勢に関する姿勢情報とを取得する姿勢取得部として機能する。例えば形状算出部１３６は、挿入部における各々の計測点の位置と各々の姿勢情報とに基づいて、各々の計測点の姿勢と他の計測点に対する相対的位置とを算出する形状算出部として機能する。例えば移動判定部１３３は、加速度に基づいて、計測点が移動しているか否かを判定する移動判定部として機能する。例えば移動期間特定部１３４は、前記判定に基づいて、計測点が移動している移動期間を特定する移動期間特定部として機能する。例えば姿勢推定部１４３は、移動期間の前後の静止状態における各々の計測点の姿勢に基づいて、移動期間における各々の計測点の姿勢である移動期間姿勢を推定する姿勢推定部として機能する。例えば移動ベクトル算出部１４４は、各々の移動期間姿勢によって補正した各々の加速度に基づいて、移動期間における各々の計測点の移動ベクトルを算出する移動ベクトル算出部として機能する。例えば画像生成部１４６は、各々の相対的位置と各々の移動ベクトルとを表す表示画像を生成する画像生成部として機能する。例えば表示部１５０は、表示画像を表示する表示部として機能する。
【００２５】
ここで、本実施形態で用いる座標系について、図２を参照して説明する。図２には、内視鏡２００の挿入部２１０が示されている。本実施形態では、最も基端側のセンサユニット１１０−０の中心を原点とし、ＸＧ軸、ＹＧ軸、ＺＧ軸を有する右手系の直交デカルト座標系であるグローバル座標系ＣＧが定義されている。グローバル座標系ＣＧの各座標軸は、図３において実線で示されている。ここで、ＸＧ軸は、重力が作用する鉛直方向に垂直な所定の方向である。本実施形態では、ＸＧ軸を図２中の矢印Ｄ１，Ｄ２に平行とし、矢印Ｄ１の方向を正とする。これに限定されないが、本実施形態ではグローバル座標系ＣＧのＸＧ軸正方向を磁北方向とする。ＹＧ軸は、鉛直方向に垂直、かつ、ＸＧ軸方向に垂直とする。本実施形態では、ＹＧ軸は図２中の矢印Ｅ１，Ｅ２に平行とし、矢印Ｅ１の方向を正とする。ＺＧ軸は、鉛直方向とし、鉛直上方向を正とする。このグローバル座標系ＣＧは、鉛直方向及び磁北方向に対して回転はしないが、挿入部２１０の移動に伴って平行移動する。
【００２６】
また、図２に示すように、各センサユニット１１０−ｉには、センサユニット１１０−ｉの中心を原点とし、ＸＢｉ軸、ＹＢｉ軸、ＺＢｉ軸を有する右手系の直交デカルト座標系であるローカル座標系ＣＢｉが定義されている。各ローカル座標系ＣＢｉの各座標軸は、図２において破線で示されている。ここで、ＸＢｉ軸は、内視鏡２００に定義された左右方向と一致し、基端側から先端側をみた際の内視鏡２００の右方向を正とする軸である。ＹＢｉ軸は、内視鏡２００の長手方向と一致し、先端方向を正とする軸である。ＺＢｉ軸は、内視鏡２００に定義された上下方向と一致し、内視鏡２００の上方向を正とする軸である。
【００２７】
各ローカル座標系ＣＢｉは、グローバル座標系ＣＧに対して原点が平行移動している。さらに、各ローカル座標系ＣＢｉは、グローバル座標系ＣＧに対して、ＸＧ軸回りにαｉ、ＹＧ軸回りにβｉ、ＺＧ軸回りにγｉそれぞれ回転している。ここで、αｉをピッチ角、βｉをロール角、γｉをヨー角と称する。また、ピッチ角αｉ、ロール角βｉ、ヨー角γｉの３つをまとめて姿勢角と称する。姿勢角αｉ，βｉ，γｉは、それぞれＸＧ軸、ＹＧ軸、ＺＧ軸の負の方向から見て時計回りを正とする。
【００２８】
各加速度センサ１１２−ｉは、ローカル座標系ＣＢｉの原点における加速度のＸＢｉ軸方向成分、ＹＢｉ軸方向成分、及びＺＢｉ軸方向成分を計測する。また、各地磁気センサ１１４−ｉは、ローカル座標系ＣＢｉの原点における地磁気のＸＢｉ軸方向成分、ＹＢｉ軸方向成分、ＺＢｉ軸方向成分を計測する。各加速度センサ１１２−ｉ及び地磁気センサ１１４−ｉに基づいて姿勢角αｉ，βｉ，γｉの値を算出することで、センサユニット１１０−ｉの姿勢は検出される。
【００２９】
次に本実施形態に係る挿入部形状表示装置１００の動作を説明する。制御部１３０による処理のフローチャートを図３に示す。ステップＳ１０１において、制御部１３０のコントロール部１３２は、各センサユニット１１０−ｉからデータを取得する。このデータには、各加速度センサ１１２−ｉの出力と、地磁気センサ１１４−ｉの出力とが含まれる。図３に示す本処理は、挿入部形状表示装置１００の動作が終了するまで繰り返されるループ処理である。したがって、コントロール部１３２は、定期的に各センサユニット１１０−ｉからデータを取得する。取得されたデータは、メモリ１４８に記憶される。
【００３０】
ステップＳ１０２において、制御部１３０の移動判定部１３３は、何れかの加速度センサ１１２の出力について、前回取得した値と今回取得した値との差が、所定の閾値よりも大きいか否かを判定する。全ての加速度センサ１１２の出力に係る差が所定の閾値以下であるとき、挿入部２１０は静止していると考えられる。この状態を静止状態と称することにする。また、何れかの加速度センサ１１２の出力に係る差が所定の閾値より大きいとき、挿入部２１０は移動していると考えられる。この状態を移動状態と称することにする。この判定において、全ての加速度センサ１１２の出力に係る差が所定の閾値以下であるとき、すなわち静止状態のとき、処理はステップＳ１０３に移る。
【００３１】
本実施形態の説明では、例として図４に示すような状況を考える。この図は、加速度センサ１１２の出力の代表値を模式的に示す。この図に示すように、時刻ｔ１までの期間において、加速度センサ１１２の出力が所定の閾値以下、すなわち、静止状態であるとする。時刻ｔ１から時刻ｔ４までの期間において、加速度センサ１１２の出力が所定の閾値より大きい移動状態であるとする。時刻ｔ４以降において、加速度センサ１１２の出力が所定の閾値以下、すなわち、静止状態であるとする。例えば時刻ｔ＜ｔ１のときや時刻ｔ＞ｔ４のとき、処理はステップＳ１０３に移る。
【００３２】
ステップＳ１０３において、制御部１３０の形状算出部１３６は、静止形状算出処理を実行する。静止形状算出処理を図５に示すフローチャートを参照して説明する。ステップＳ２０１において形状算出部１３６内の静止姿勢算出部１３７は、各加速度センサ１１２−ｉの出力に基づいて、ピッチ角αｉ及びロール角βｉを算出する。ピッチ角αｉ及びロール角βｉの算出方法を説明する。
【００３３】
姿勢角αｉ，βｉ，γｉはヨー角γｉ、ピッチ角αｉ、ロール角βｉの順で回転する（Ｚ，Ｘ，Ｙ）型とする。したがって、グローバル座標系ＣＧからローカル座標系ＣＢｉへの回転行列は、下記式（１）で表される。
【数１】
2013018404.tif 000003
【００３４】
挿入部２１０が静止している静止状態では、加速度は重力加速度のみが鉛直方向の下方向に作用している。したがって、静止状態では、グローバル座標系ＣＧにおける加速度ベクトルａＧｔｈのＸＧ軸方向成分、ＹＧ軸方向成分、ＺＧ軸方向成分は、次式（２）で表すことができる。
【数２】
2013018404.tif 000004
【００３５】
時刻ｔにおける各加速度センサ１１２−ｉで計測される加速度ベクトルａｔｏｂｓｉ、すなわち、加速度のローカル座標系ＣＢｉにおけるＸＢｉ軸方向成分、ＹＢｉ軸方向成分、ＺＢｉ軸方向成分を、次式（３）で表すことにする。
【数３】
2013018404.tif 000005
【００３６】
このとき、ローカル座標系ＣＢｉはグローバル座標系ＣＧをヨー角γｉ、ピッチ角αｉ、ロール角βｉの順に回転させた座標系であるので、ローカル座標系ＣＢｉで観測される加速度ベクトルａｔｏｂｓｉは、次式（４）で表される。
【数４】
2013018404.tif 000006
【００３７】
ここで、式（４）の第１式の２乗と式（４）の第３式の２乗との和をとると、次式（５）が得られる。
【数５】
2013018404.tif 000007
【００３８】
式（４）の第２式を式（５）で割ることにより、次式（６）が得られる。
【数６】
2013018404.tif 000008
【００３９】
このようにして各ローカル座標系ＣＢｉの各時刻ｔにおけるピッチ角αｔｉが求まる。
【００４０】
また、式（４）の第１式を式（４）の第３式で割ることにより、次式（７）が得られる。
【数７】
2013018404.tif 000009
【００４１】
このようにして各ローカル座標系ＣＢｉの各時刻ｔにおけるロール角βｔｉが求まる。
【００４２】
以上のようにして、各時刻ｔについて各加速度センサ１１２−ｉによる計測データに基づいて、各センサユニット１１０−ｉの、すなわち各ローカル座標系ＣＢｉの、ピッチ角αｔｉ及びロール角βｔｉが算出される。
【００４３】
図５に戻って説明を続ける。ステップＳ２０２において、静止姿勢算出部１３７は、地磁気センサ１１４の出力に基づいてヨー角γｔｉを算出する。ヨー角γｔｉの算出方法について説明する。地磁気センサ１１４−ｉで計測される時刻ｔにおける地磁気ベクトルｍｔｏｂｓｉ、すなわち、各地磁気のローカル座標系ＣＢｉにおけるＸＢｉ軸方向成分、ＹＢｉ軸方向成分、ＺＢｉ軸方向成分を、次式（８）で表すことにする。
【数８】
2013018404.tif 000010
【００４４】
ここで、式（８）で表される各地磁気センサ１１４−ｉの出力について、式（６）及び（７）のように求まった各ピッチ角αｔｉ及び各ロール角βｔｉを用いて、グローバル座標系ＣＧのＸＧ軸回り及びＹＧ軸回りに回転させて補正した地磁気ｍｔｔｈを次式（９）によって算出する。
【数９】
2013018404.tif 000011
【００４５】
このような上記式（９）で表される補正後の地磁気を次式（１０）のように表すことにする。
【数１０】
2013018404.tif 000012
【００４６】
上記式（９）及び（１０）に示す地磁気を表す式において、地磁気センサ１１４−ｉで計測される地磁気ベクトルのＸＢｉ軸方向成分、ＹＢｉ軸方向成分、ＺＢｉ軸方向成分は、ピッチ角αｔｉとロール角βｔｉとが補正されているので、ヨー角γｔｉのみがグローバル座標系ＣＧに対して回転していることになる。したがって、磁北に対してセンサユニット１１０−ｉがなす角θｔｂａｓｅ＿ｉ、すなわちヨー角γｔｉは、式（１０）を用いて、次式（１１）で表すことができる。
【数１１】
2013018404.tif 000013
【００４７】
ここで、式（９）及び（１０）より、次式（１２．１）及び（１２．２）が成り立つ。
【数１２】
2013018404.tif 000014
【００４８】
なお、本実施形態では、グローバル座標系ＣＧのＸＧ軸方向が磁北方向と一致するように設定しているので磁北に対してセンサユニット１１０−ｉがなす角θｔｂａｓｅ＿ｉがヨー角γｔｉと一致するが、グローバル座標系ＣＧのＸＧ軸方向が磁北方向と一致しない場合は、そのずれを補正する必要がある。この補正はなす角θｔｂａｓｅ＿ｉにそのずれを加減することで容易に行える。
【００４９】
以上のようにして、形状算出部１３６の静止姿勢算出部１３７は、各加速度センサ１１２−ｉ及び各地磁気センサ１１４−ｉの出力に基づいて、各時刻ｔにおける各センサユニット１１０−ｉのピッチ角αｔｉ、ロール角βｔｉ、ヨー角γｔｉを求めることができる。
【００５０】
図５に戻って説明を続ける。ステップＳ２０３において、形状算出部１３６内の座標算出部１３８は、各センサユニット１１０−ｉの座標を算出する。各センサユニット１１０−ｉの座標の算出方法について説明する。内視鏡２００の挿入部２１０の形状の模式図を図６に示す。この図に示すように、挿入部２１０は、複数のリンク３１０が連なって形成されており、各リンクの接続部分にセンサユニット１１０が配置されていると考える。ここで、リンク３１０−ｉの基端側にはセンサユニット１１０−（ｉ−１）が配置され、リンク３１０−ｉの先端側にはセンサユニット１１０−ｉが配置されているものとする。また、各リンク３１０の長さは、センサユニット１１０間の間隔ｌとする。
【００５１】
このとき、センサユニット１１０−（ｉ−１）が配置されたリンク３１０−ｉの基端がグローバル座標系ＣＧの原点に位置すると仮定したとき、センサユニット１１０−ｉが配置されたリンク３１０−ｉの先端の座標ｌｔｉは次式（１３）を用いて、次式（１４）で表される。
【数１３】
2013018404.tif 000015
【数１４】
2013018404.tif 000016
【００５２】
グローバル座標系ＣＧにおける各センサユニット１１０−ｉの座標Ｓｔｉを次式（１５）と表すものとする。
【数１５】
2013018404.tif 000017
【００５３】
このとき、式（１４）を用いて、各センサユニット１１０−ｉの座標Ｓｔｉは、次式（１６）で得られる。
【数１６】
2013018404.tif 000018
【００５４】
すなわち、グローバル座標系ＣＧの原点である基端側から順に、各センサユニット１１０−ｉの座標Ｓｔｉを決定することができる。
【００５５】
以上のようにして、形状算出部１３６は、各加速度センサ１１２−ｉ及び各地磁気センサ１１４−ｉの出力に基づいて、グローバル座標系ＣＧに対する各センサユニット１１０−ｉの位置及び姿勢を求めることができる。すなわち、形状算出部１３６は、内視鏡２００の挿入部２１０の形状を求めることができる。その後、形状算出部１３６は、静止形状算出処理を終了し、各センサユニット１１０−ｉの位置及び姿勢を戻り値として処理はステップＳ１０３に戻る。算出された各センサユニット１１０−ｉの位置及び姿勢は、コントロール部１３２を介してメモリ１４８に記憶される。ステップＳ１０３の静止形状算出処理を終えたら、処理はステップＳ１１０に移る。
【００５６】
ステップＳ１０２の判定において、加速度の差分が閾値より大きいとき、すなわち移動状態にあるとき、処理はステップＳ１０４に移る。ステップＳ１０４において、移動判定部１３３は、加速度の差分が閾値より大きいか否かを判定する。加速度の差分が閾値より大きい間、すなわち挿入部２１０が移動状態にある間、移動判定部１３３は、ステップＳ１０４の判定を繰り返す。ステップＳ１０４の判定で、加速度の差分が閾値以下となったら、すなわち挿入部２１０が静止状態になったら、処理はステップＳ１０５に移る。ステップＳ１０５において、移動期間特定部１３４は、移動状態にある期間を移動期間として特定する。
【００５７】
ステップＳ１０６において、形状算出部１３６は、移動期間特定部１３４によって特定された移動期間直前及び直後の２つの静止状態について、静止形状算出処理を実行する。すなわち、移動期間直前及び直後の２つの静止状態のそれぞれについて、式（１）乃至（１６）を参照して説明した静止形状算出処理によって、グローバル座標系ＣＧにおける各センサユニット１１０−ｉの位置及び姿勢を算出する。ここで、各時刻における各センサユニット１１０−ｉから取得されたデータは、メモリ１４８に記憶されているので、これを読み出して用いることができる。また、移動期間直前の各センサユニット１１０−ｉの位置及び姿勢は、既にステップＳ１０３で算出され、メモリ１４８に記憶されているので、これを用いてもよい。また、静止形状算出処理によって算出された各センサユニット１１０−ｉの位置及び姿勢は、メモリ１４８に記憶される。
【００５８】
ステップＳ１０７において、制御部１３０の移動状態推定部１４２は、移動状態推定処理を実行する。この移動状態推定処理は、移動状態の各時刻ｔにおける各センサユニット１１０−ｉの位置及び姿勢を推定し、移動状態における移動方向及び移動量を表す移動ベクトルを算出する処理である。この移動状態推定処理を図７に示すフローチャートを参照して説明する。
【００５９】
ステップＳ３０１において、移動状態推定部１４２内の姿勢推定部１４３は、移動状態の各時刻ｔにおける各センサユニット１１０−ｉの姿勢を推定する。この姿勢の推定は、移動開始時である時刻ｔ１の姿勢と、移動終了時である時刻ｔ４の姿勢とに基づく。例えば、時刻ｔ１と時刻ｔ４との間の各時刻ｔにおける姿勢を、線形補完によって求める。すなわち、移動状態における各センサユニット１１０−ｉの姿勢は次式（１７）で求まる。
【数１７】
2013018404.tif 000019
【００６０】
なお、線形補完は一例であり、補完方法はこれに限定されない。
【００６１】
ステップＳ３０２において、移動状態推定部１４２内の移動ベクトル算出部１４４は、各加速度センサ１１２−ｉの出力を、グローバル座標系ＣＧにおける加速度に変換する。この変換は、式（１７）によって得られた移動状態の各時刻における各センサユニット１１０の姿勢に基づく。すなわち、グローバル座標系ＣＧにおける加速度ａｔＧｉは、次式（１８）により求まる。
【数１８】
2013018404.tif 000020
【００６２】
式（１８）で表されるグローバル座標系ＣＧにおける加速度ａｔＧｉから、重力加速度を減ずることで、各センサユニット１１０−ｉの運動加速度が求まる。すなわち、各センサユニット１１０−ｉの運動加速度ａｔＧｉ＿ｍは、次式（１９）で得られる。
【数１９】
2013018404.tif 000021
【００６３】
ステップＳ３０３において、移動ベクトル算出部１４４は、各センサユニット１１０−ｉの運動加速度ａｔＧｉ＿ｍを２回積分することによって、移動ベクトルを求める。すなわち、移動状態にある各センサユニット１１０−ｉの移動ベクトルｄＧｉは、次式（２０）で得られる。
【数２０】
2013018404.tif 000022
【００６４】
以上のようにして、移動状態推定部１４２は、移動状態にある間の各センサユニット１１０−ｉの移動ベクトルｄＧｉを求めることができる。移動状態推定部１４２は、移動状態推定処理を終了し、各センサユニット１１０−ｉの移動ベクトルｄＧｉを戻り値として処理はステップＳ１０７に戻る。移動状態推定処理によって算出された各センサユニット１１０−ｉの移動ベクトルｄＧｉは、コントロール部１３２を介してメモリ１４８に記憶される。
【００６５】
図３に戻って説明を続ける。ステップＳ１０８において、画像生成部１４６は、表示部１５０に表示する移動ベクトルを表す矢印である表示ベクトルに係る画像上の座標を算出する。まず、表示ベクトルの大きさを、式（２０）で得られる移動ベクトルｄＧｉに適当な正の係数を掛けることで決定する。すなわち、ベクトルｒを次式（２１）で算出する。
【数２１】
2013018404.tif 000023
【００６６】
さらに、画像生成部１４６は、ベクトルｒに基づいて、表示ベクトルの始点ＰＧｉ＿ｓと終点ＰＧｉ＿ｅとを、次式（２２）を用いて算出する。
【数２２】
2013018404.tif 000024
【００６７】
ここで、Ｍは適当な正の整数である。また、ｓｉｇｎは、変数が０以上であれば１．０とし、０より小さければ−１．０とする関数である。また、ｓｑｒｔは、変数の平方根を取る関数である。この平方根を求めるのは、表示ベクトルの大きさを調整するためである。適宜大きさを調整できれば、用いる式は式（２２）に限らない。また、式（２２）におけるＵ＝［Ｕｉ＿Ｘ  Ｕｉ＿Ｙ  Ｕｉ＿Ｚ］も表示ベクトルの大きさを調整するために加算されている。
【００６８】
また、Ｕ＝［Ｕｉ＿Ｘ  Ｕｉ＿Ｙ  Ｕｉ＿Ｚ］を、
ｄGi_X＞ｄGi_Y、かつ、ｄGi_X＞ｄGi_Zの場合は、Ｕ＝［１  ０  ０］Ｔ、
ｄGi_Y＞ｄGi_X、かつ、ｄGi_Y＞ｄGi_Zの場合は、Ｕ＝［０  １  ０］Ｔ、
ｄGi_Z＞ｄGi_X、かつ、ｄGi_Z＞ｄGi_Yの場合は、Ｕ＝［０  ０  １］Ｔ、
とすることで、移動量が一番大きい方向を強調する表示ベクトルとすることもできる。
【００６９】
ステップＳ１０９において、コントロール部１３２は、変数ｐを所定値Ｐに設定する。ここで変数ｐは、表示ベクトルを表示する期間を特定するためのカウンタとして機能し、所定値Ｐは、表示ベクトルを表示する期間に比例する値である。その後、処理はステップＳ１１０に移る。
【００７０】
ステップＳ１１０において、画像生成部１４６は、静止状態の挿入部２１０の形状をメモリ１４８から読み出して、それを表示部１５０に表示させる。この説明の例では、画像生成部１４６は、時刻ｔ＜ｔ１では、ステップＳ１０３で算出された挿入部２１０の形状を表示部１５０に表示させる。画像生成部１４６は、時刻ｔ４の直後には、ステップＳ１０６で算出された移動期間直後の挿入部２１０の形状を表示させ、その後、時刻ｔ＞ｔ４では、ステップＳ１０３で算出された挿入部２１０の形状を表示部１５０に表示させる。例えば図８に示すように、表示領域４１０に座標系４２０を設定し描画する。次に、この座標系４２０に各センサユニット１１０−ｉの位置をセンサ表示符号４３２−ｉで示し、それらをリンク表示符号４３４−ｉで結んで、挿入部形状４３０を描画する。
【００７１】
ステップＳ１１１において、コントロール部１３２は、変数ｐが０よりも大きいか否か判定する。ｐが０以下のとき、処理はステップＳ１１４に移る。一方、ｐが０よりも大きいとき、処理はステップＳ１１２に移る。ステップＳ１１２において、画像生成部１４６は、ステップＳ１０８で式（２２）によって算出した始点ＰＧｉ＿ｓ及び終点ＰＧｉ＿ｅを有する表示ベクトル（例えば矢印の記号）を、挿入部形状４３０の各センサユニット１１０−ｉの位置に表示ベクトル４４０−ｉとして、例えば図８に示すように表示する。ステップＳ１１３においてコントロール部１３２は、変数ｐを（ｐ−１）に設定する。その後、処理はステップＳ１１４に移る。
【００７２】
ステップＳ１１４において、コントロール部１３２は、挿入部形状表示装置１００の動作が終了したか否かを判定する。動作が終了していないとき、処理はステップＳ１０１に戻る。一方、動作が終了しているとき、処理は終了する。
【００７３】
以上の処理により、表示部１５０には、座標系４２０上に、挿入部２１０が静止状態にあるときは、ステップＳ１１０によって挿入部２１０の形状が挿入部形状４３０として表示される。一方、移動状態から静止状態に移った後の所定の時間、すなわち、カウンタとしての変数ｐがＰから０になるまでは、同様に座標系４２０上に挿入部２１０の形状が挿入部形状４３０として表示されるとともに、ステップＳ１１２によって移動状態における各表示ベクトル４４０−ｉが重畳表示される。
【００７４】
本実施形態によれば、挿入部２１０の形状とともに、その移動状態が矢印で表示部１５０に表示される。したがって、ユーザは、この表示を見ることで挿入部２１０の形状並びに移動方向及び移動量を画像で確認することができる。このような表示は、ユーザによる挿入部２１０の操作を支援する。
【００７５】
本実施形態によれば、静止状態では各加速度センサ１１２−ｉ及び各地磁気センサ１１４−ｉに基づいてその都度、挿入部２１０の形状が表示される。また、移動状態では、その都度当該移動状態における移動ベクトルが、重力加速度を補正した運動加速度に基づいて算出される。そして、一連の移動状態ごとに、その移動状態における移動ベクトルが表示される。したがって、加速度の積分を続けることによって挿入部２１０の形状と位置と移動量とが算出される場合と比較して、積分を続けることによる誤差の累積が低減され得る。
【００７６】
もちろん図８に示す図は、挿入部２１０の形状とその移動ベクトルとを示す画像の概要を表す一例であり、挿入部２１０の形状とその移動ベクトルとを表示する画像であれば、どのような画像が用いられてもよい。例えば挿入部２１０の形状は内視鏡の形状を模した表示としてもよい。また、例えば表示ベクトル４４０−ｉの代わりにアニメーションを用いて挿入部２１０を表す図が移動するようにして、移動ベクトルが表現されてもよい。この場合、ステップＳ１０８の処理を適宜変更し、必要な情報を算出するようにすればよい。
【００７７】
また、本実施形態では、各センサユニット１１０−ｉの間は直線のリンク３１０−ｉで結ばれていると仮定して、挿入部２１０の形状の算出が行われているが、各センサユニット１１０−ｉ間を曲線とするような補正が適宜行われてもよい。また、本実施形態では、挿入部形状表示装置が内視鏡に適応される例を示したが、内視鏡内に通して用いられるカテーテルやその他処置具など、変形して挿入される挿入部を有する種々の装置に適用され得る。
【００７８】
なお、移動状態における運動加速度の推定において、加速度センサの出力にローパスフィルタをかけることで、運動加速度の影響を低減して重力加速度を推定することもできる。その場合、線形補完による姿勢の推定を行わずに移動ベクトルを算出することもできる。
【００７９】
なお、本発明は上記実施形態そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。また、上記実施形態に開示されている複数の構成要素の適宜な組み合わせにより、種々の発明を形成できる。例えば、実施形態に示される全構成要素から幾つかの構成要素を削除しても、発明が解決しようとする課題の欄で述べられた課題が解決でき、かつ、発明の効果が得られる場合には、この構成要素が削除された構成も発明として抽出され得る。
【図１】
2013018404.tif 000025
【図２】
2013018404.tif 000026
【図３】
2013018404.tif 000027
【図４】
2013018404.tif 000028
【図５】
2013018404.tif 000029
【図６】
2013018404.tif 000030
【図７】
2013018404.tif 000031
【図８】
2013018404.tif 000032
【手続補正書】
【提出日】平成25年1月25日(2013.1.25)
【手続補正１】
【補正対象書類名】特許請求の範囲
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【特許請求の範囲】
【請求項１】
変形する挿入部と、
前記挿入部に設定された複数の計測点の各々における加速度を算出する加速度算出部と、
前記加速度算出部により算出された前記加速度に基づき前記計測点が移動しているか否かを判定する判定部と、
前記判定部において前記計測点が移動していると判断された移動期間より前の静止状態における各々の前記計測点の第１の姿勢情報と、前記移動期間より後の静止状態における各々の前記計測点の第２の姿勢情報とを算出する姿勢情報算出部と、
前記姿勢情報算出部において算出された前記第１の姿勢情報と前記第２の姿勢情報とに基づき前記移動期間における各々の前記計測点の姿勢である移動期間姿勢を推定する姿勢推定部と、
前記姿勢推定部において推定された前記計測点の前記移動期間姿勢に基づき、前記加速度算出部によって算出された前記移動期間における前記加速度を補正し、補正された前記加速度に基づき前記移動期間における各々の前記計測点の移動方向及び移動量を表す移動ベクトルを算出する移動ベクトル算出部と、
前記姿勢情報算出部により算出された前記計測点の姿勢情報に基づき各々の前記計測点の位置を算出する位置算出部と、
前記位置算出部により算出された各々の前記計測点の位置と前記移動ベクトル算出部により算出された各々の前記移動ベクトルとを表す表示画像を生成し出力する画像生成部と、
を有する挿入部形状推定装置。
【請求項２】
前記姿勢情報算出部は、前記移動期間より前の静止状態において前記加速度算出部により算出された前記加速度に基づき前記第１の姿勢情報を算出し、前記移動期間より後の静止状態において前記加速度算出部により算出された前記加速度に基づき前記第２の姿勢情報を算出する
請求項１に記載の挿入部形状推定装置。
【請求項３】
前記挿入部は細長形状をしており
前記計測点は、前記挿入部の長軸に沿って複数配置されている
請求項１に記載の挿入部形状推定装置。
【請求項４】
前記移動ベクトル算出部は、前記移動期間姿勢と前記移動期間に前記加速度算出部において算出された前記加速度とに基づき前記計測点における運動加速度を算出する
請求項１に記載の挿入部形状推定装置。
【請求項５】
前記移動ベクトル算出部は、前記移動期間姿勢に基づき前記移動期間に前記加速度算出部において算出された前記加速度をグローバル座標系における加速度に変換し、前記グローバル座標系における加速度から重力加速度を減ずることにより前記運動加速度を算出する
請求項４に記載の挿入部形状推定装置。
【請求項６】
前記移動ベクトル算出部は、前記運動加速度を二回積分することで前記移動ベクトルを算出する請求項４に記載の挿入部形状推定装置。
【請求項７】
前記画像生成部は、前記表示画像における各々の前記計測点に該当する位置に各々の前記移動ベクトルを表す記号を重畳した前記表示画像を生成する請求項３に記載の挿入部形状推定装置。
【請求項８】
さらに、前記計測点における地磁気を計測する地磁気センサを有し、
前記姿勢情報算出部は、前記計測点が移動していると判断された移動期間より前の静止状態において計測された前記計測点の前記加速度と前記地磁気とから前記計測点の第１の姿勢情報を算出し、前記移動期間より後の静止状態において計測された前記計測点の前記加速度と前記地磁気とから前記計測点の第２の姿勢情報を算出する
請求項１に記載の挿入部形状推定装置。
【手続補正３】
【補正対象書類名】明細書
【補正対象項目名】０００６
【補正方法】変更
【補正の内容】
【０００６】
そこで本発明は、累積誤差の影響を低減した、変形する挿入部の形状を推定する挿入部形状推定装置を提供することを目的とする。
【手続補正４】
【補正対象書類名】明細書
【補正対象項目名】０００７
【補正方法】変更
【補正の内容】
【０００７】
前記目的を果たすため、本発明の一態様によれば挿入部形状推定装置は、変形する挿入部と、前記挿入部に設定された複数の計測点の各々における加速度を算出する加速度算出部と、前記加速度算出部により算出された前記加速度に基づき前記計測点が移動しているか否かを判定する判定部と、前記判定部において前記計測点が移動していると判断された移動期間より前の静止状態における各々の前記計測点の第１の姿勢情報と、前記移動期間より後の静止状態における各々の前記計測点の第２の姿勢情報とを算出する姿勢情報算出部と、前記姿勢情報算出部において算出された前記第１の姿勢情報と前記第２の姿勢情報とに基づき前記移動期間における各々の前記計測点の姿勢である移動期間姿勢を推定する姿勢推定部と、前記姿勢推定部において推定された前記計測点の前記移動期間姿勢に基づき、前記加速度算出部によって算出された前記移動期間における前記加速度を補正し、補正された前記加速度に基づき前記移動期間における各々の前記計測点の移動方向及び移動量を表す移動ベクトルを算出する移動ベクトル算出部と、前記姿勢情報算出部により算出された前記計測点の姿勢情報に基づき各々の前記計測点の位置を算出する位置算出部と、前記位置算出部により算出された各々の前記計測点の位置と前記移動ベクトル算出部により算出された各々の前記移動ベクトルとを表す表示画像を生成し出力する画像生成部と、を有する。
【手続補正５】
【補正対象書類名】明細書
【補正対象項目名】０００８
【補正方法】変更
【補正の内容】
【０００８】
本発明によれば、累積誤差の影響を低減した、変形する挿入部の形状を推定する挿入部形状推定装置を提供できる。
【国際調査報告】
2013018404.tif 000033
2013018404.tif 000034