JP4897121B2 - 内視鏡形状検出装置及び内視鏡の挿入部の形状検出方法 - Google Patents

Description

本発明は、体腔内に挿入される内視鏡を備える内視鏡形状検出装置及びその内視鏡形状検出装置の内視鏡の挿入部の形状検出方法に関する。

近年、内視鏡の挿入部の形状を検出可能な内視鏡形状検出装置が実用化されている。特許文献１には、体腔内に挿入される内視鏡の挿入部に複数のソースコイルを取付けた内視鏡挿入形状検出装置が開示されている。この内視鏡挿入形状検出装置では、体外に設けられるセンスコイルにより、それぞれのソースコイルの位置が検出される。そして、検出されたソースコイルの位置に基づいて、内視鏡の挿入部の形状が検出される。

また、特許文献２には、内視鏡の挿入部に２つのセンサが取付けられた内視鏡装置が開示されている。この内視鏡装置では、基端側のセンサを基準として、先端側のセンサの位置、姿勢が検出される。基端側のセンサは湾曲部の基端の近傍に、先端側のセンサは先端硬性部に配置されている。基端側のセンサに対する先端側のセンサの位置、姿勢を検出することにより、湾曲部の湾曲角度、湾曲方向が算出される。

また、特許文献３には、内視鏡の挿入部に複数のジャイロを取付けられた内視鏡形状検出装置が開示されている。この内視鏡形状検出装置では、ジャイロにより内視鏡の挿入部の所定の部位（ジャイロが取り付けられた部位）の姿勢が検出される。そして、検出された所定の部位での姿勢に基づいて、挿入部の形状が検出される。

特開２０００−１７５８６２号公報 特開２００７−３１９６２２号公報 特開平１１−１９０２７号公報

上記特許文献１の内視鏡形状検出装置では、体腔内のソースコイルの位置を体外のセンスコイルで検出するため、検出装置が大型化するとともに、装置の構成が複雑化する。

上記特許文献２では、体腔内のセンサのみを用いて湾曲部の湾曲角度、湾曲方向が検出される。しかし、実際の内視鏡の挿入部は可撓性を有し、体腔内に挿入された際の挿入部の形状は、曲線状である。すなわち、この内視鏡装置では、湾曲部の湾曲角度、湾曲方向が検出されるのみで、体腔内への挿入時の挿入部の形状までは高い精度で検出されない。

上記特許文献３では、複数のジャイロを用いて内視鏡の挿入部の形状が検出される。しかし、ジャイロは挿入部が移動する際に発生する慣性力又はコリオリ力に基づいて挿入部の所定の部位での姿勢を検出する。このため、挿入部が移動していない静状態では、形状を検出できず、低速度で挿入部が移動する際の検出精度は低下してしまう。

本発明は上記課題に着目してなされたものであり、その目的とするところは、装置の構成を大型化、複雑化することなく、高い精度で挿入部の形状を検出可能な内視鏡形状検出装置及び内視鏡の挿入部の形状検出方法を提供することにある。

上記目的を達成するため、本発明のある態様では、長手方向に互いに所定のセンサ間寸法だけ離れて複数のセンサユニットが配置される挿入部を備える内視鏡と、前記センサユニットでの計測データに基づいて、それぞれの前記センサユニットの姿勢を検出する姿勢検出部と、前記姿勢検出部で検出されたそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間の形状を寸法が前記センサ間寸法に等しい直線状のリンクと仮定して、前記内視鏡の前記挿入部の検出線形形状を検出する線形形状検出部と、それぞれの前記センサユニットの間の形状を弧長が前記センサ間寸法に等しい円弧と仮定して、前記線形形状検出部で検出された前記検出線形形状を曲線補間し、検出曲線形状を検出する曲線形状検出部と、を備える内視鏡形状検出装置を提供する。

また、本発明の別のある態様では、内視鏡の挿入部に長手方向に互いに所定のセンサ間寸法だけ離れて配置される複数のセンサユニットが計測を行うことと、前記センサユニットでの計測データに基づいて、それぞれの前記センサユニットの姿勢を姿勢検出部が検出することと、前記姿勢検出部が検出したそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間の形状を寸法が前記センサ間寸法に等しい直線状のリンクと仮定して、前記内視鏡の前記挿入部の検出線形形状を線形形状検出部が検出することと、それぞれの前記センサユニットの間の形状を弧長が前記センサ間寸法に等しい円弧と仮定して、曲線形状検出部が、前記線形形状検出部が検出した前記検出線形形状を曲線補間し、検出曲線形状を形成することと、を備える内視鏡の挿入部の形状検出方法を提供する。

本発明によれば、装置の構成を大型化、複雑化することなく、高い精度で挿入部の形状を検出可能な内視鏡形状検出装置及び内視鏡の挿入部の形状検出方法を提供することができる。

本発明の第１の実施形態に係る内視鏡形状検出装置の構成を示すブロック図。 第１の実施形態に係る内視鏡の挿入部の構成を示す概略図。 第１の実施形態に係る内視鏡形状検出装置のパソコンの構成を示すブロック図。 第１の実施形態に係る内視鏡の挿入部の静状態での形状を検出する方法を示すフローチャート。 第１の実施形態に係る内視鏡形状検出装置のグローバル座標系と補正座標系とを比較して示す概略図。 第１の実施形態に係る内視鏡形状検出装置の線形形状検出部で検出される検出線形形状を示す概略図。 第１の実施形態に係る線形形状検出部のリンク位置補正部での処理を説明する概略図。 第１の実施形態に係る内視鏡形状検出装置の曲線形状検出部で検出される検出曲線形状を示す概略図。 第１の実施形態に係る曲線形状検出部により検出曲線形状を検出する方法を示すフローチャート。 第１の実施形態に係る曲線形状検出部の曲線補間部での処理を説明する概略図。 第１の実施形態に係る曲線形状検出部の円弧位置補正部での処理を説明する概略図。 本発明の第２の実施形態に係る内視鏡形状検出装置のパソコンの構成を示すブロック図。 第２の実施形態に係る内視鏡形状検出装置の曲線形状検出部により検出曲線形状を検出する方法を示すフローチャート。 第２の実施形態に係る曲線形状検出部の補間順次実施部での処理を説明する概略図。 第２の実施形態に係る曲線形状検出部の未補間リンク位置補正部での処理を説明する概略図。 本発明の第１の変形例に係る内視鏡形状検出装置のパソコンの構成を示すブロック図。 第１の変形例に係る内視鏡の挿入部の動状態での形状を検出する方法を示すフローチャート。

（第１の実施形態）
本発明の第１の実施形態について図１乃至図１１を参照して説明する。

図１は、本実施形態の内視鏡形状検出装置１を示す図である。図１に示すように、内視鏡形状検出装置１の内視鏡１０は、体腔内に挿入される挿入部１１と、挿入部１１の基端側に設けられる操作部１２とを備える。挿入部１１は、最先端に設けられる先端硬性部１４と、先端硬性部１４の基端側に設けられる湾曲部１６と、湾曲部１６の基端側に設けられる細長い可撓管部１８とを備える。

先端硬性部１４の内部には、被写体の撮像を行うＣＣＤ等の撮像素子２０が設けられている。撮像素子２０には、撮像用信号線２１の一端が接続されている。撮像用信号線２１は、挿入部１１の内部を通って操作部１２から内視鏡１０の外部に延出され、他端が画像処理ユニットであるビデオプロセッサ３に接続されている。また、挿入部１１の内部には、先端硬性部１４の照明窓（図示しない）に被写体を照射する照明光を導光するライトガイド２３が、長手方向に延設されている。ライトガイド２３は、操作部１２から内視鏡１０の外部に延出され、光源ユニット４に接続されている。

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

挿入部２には、複数（本実施形態ではＮ＋１個）のセンサユニットＳ_０〜Ｓ_Ｎが設けられている。それぞれのセンサユニットＳ_ｉ（ｉ＝０，１，２，…，Ｎ）は、互いに長手方向に一定の間隔ｌ（＝５０ｍｍ）だけ離れて配置されている。すなわち、それぞれのセンサユニットＳ_ｉは、互いに長手方向に所定のセンサ間寸法ｌだけ離れて配置されている。ここで、例えば最も基端側のセンサユニットＳ_０が可撓管部１８の基端部に配置され、最も先端側のセンサユニットＳ_Ｎが湾曲部１６の先端部に配置されている。センサユニットＳ_ｉは、加速度を計測する加速度センサＡ_ｉと、地磁気を計測する地磁気センサＢ_ｉとを備える。

図２は、内視鏡１０の挿入部１１を示す図である。図２に示すように、それぞれのセンサユニットＳ_ｉは、センサユニットＳ_ｉの中心を原点とし、Ｘ_ｉ軸、Ｙ_ｉ軸、Ｚ_ｉ軸を有するローカル座標系Ｃ_ｉ（図２で点線で示す）を備える。ここで、Ｘ_ｉ軸方向は、センサユニットＳ_ｉの中心での内視鏡１０の左右方向と一致し、基端側から視た際の内視鏡１０の右方向を正とする。Ｙ_ｉ軸方向は、センサユニットＳ_ｉの中心での長手方向と一致し、先端側方向を正とする。Ｚ_ｉ軸方向は、センサユニットＳ_ｉの中心での内視鏡１０の上下方向と一致し、内視鏡１０の上方向を正とする。加速度センサＡ_ｉは、ローカル座標系Ｃ_ｉの原点での加速度のＸ_ｉ軸方向成分、Ｙ_ｉ軸方向成分、Ｚ_ｉ軸方向成分を計測する。地磁気センサＢ_ｉは、ローカル座標系Ｃ_ｉの原点での地磁気のＸ_ｉ軸方向成分、Ｙ_ｉ軸方向成分、Ｚ_ｉ軸方向成分を計測する。

また、内視鏡形状検出装置１では、最も基端側のセンサユニットＳ_０の中心を原点とし、Ｘ軸、Ｙ軸、Ｚ軸を有するグローバル座標系Ｃ（図２で実線で示す）が定義されている。ここで、グローバル座標系Ｃは、最も基端側のセンサユニットＳ_０の中心を原点とする右手系の直交デカルト座標系である。Ｘ軸方向は、重力が作用する鉛直方向に垂直な所定の方向（本実施形態では、図２の矢印Ｄ１，Ｄ２に平行な方向）と一致し、図２の矢印Ｄ１の方向を正とする。Ｙ軸方向は、鉛直方向に垂直で、かつ、Ｘ軸方向に垂直な方向（本実施形態では、図２の矢印Ｅ１，Ｅ２に平行な方向）と一致し、図２の矢印Ｅ１の方向を正とする。Ｚ軸方向は、鉛直方向と一致し、鉛直方向の上方向（紙面の裏から表への方向）を正とする。なお、ここでは説明の都合上、グローバル座標系のＸ軸方向を磁北方向とする。

それぞれのローカル座標系Ｃ_ｉは、グローバル座標系ＣをＸ軸回りにα_ｉ、Ｙ軸回りにβ_ｉ、Ｚ軸回りにγ_ｉそれぞれ回転し、最も基端側のセンサユニットＳ_０の中心からセンサユニットＳ_ｉの中心まで原点を平行移動した座標系である。ここで、α_ｉをピッチ角、β_ｉをロール角、γ_ｉをヨー角と称し、ピッチ角α_ｉ、ロール角β_ｉ、ヨー角γ_ｉの３つをまとめて姿勢角と称する。姿勢角α_ｉ，β_ｉ，γ_ｉは、それぞれＸ軸、Ｙ軸、Ｚ軸の負の方向から見て時計回りを正とする。姿勢角α_ｉ，β_ｉ，γ_ｉの値を算出することにより、センサユニットＳ_ｉの姿勢が検出される。

図１に示すように、ぞれぞれのセンサユニットＳ_ｉの加速度センサＡ_ｉ及び地磁気センサＢ_ｉには、Ｉ２Ｃ等のシリアルバス５が接続されている。シリアルバス５は、挿入部１１の内部を通って操作部１２から内視鏡１０の外部に延出され、基端がシリアルコンバータ６に接続されている。シリアルコンバータ６は、それぞれのセンサユニットＳ_ｉからシリアルバス５を介して入力される計測データのシリアル信号を、ＵＳＢ信号に変換する。シリアルコンバータ６には、ＵＳＢケーブル７の一端が接続されている。ＵＳＢケーブル７の他端は、パソコン８に接続されている。パソコン８には、それぞれのセンサユニットＳ_ｉでの計測データのＵＳＢ信号がシリアルコンバータ６から入力される。

図３は、パソコン８の構成を示す図である。図３に示すように、パソコン８は、ＵＳＢケーブル７を介してシリアルコンバータ６に接続される通信部２６を備える。通信部２６は、それぞれのセンサユニットＳ_ｉでの計測データを受信する。通信部２６には、物理量換算部２８が接続されている。物理量換算部２８は、通信部２６で受信したそれぞれのセンサユニットＳ_ｉでの計測データを、オフセット、ゲイン等を用いて物理量に換算する。

物理量換算部２８には、姿勢検出部３０が接続されている。姿勢検出部３０は、センサユニットＳ_ｉでの計測データに基づいて、それぞれのセンサユニットＳ_ｉの姿勢を検出する。姿勢検出部３０は、それぞれのセンサユニットＳ_ｉの加速度センサＡ_ｉ及び地磁気センサＢ_ｉでの計測データに基づいて、それぞれのセンサユニットＳ_ｉのローカル座標系Ｃ_ｉのグローバル座標系ＣからのＸ軸回り、Ｙ軸回り、Ｚ軸回りの回転角である３つの姿勢角α_ｉ，β_ｉ，γ_ｉを算出する姿勢角算出部３２を備える。姿勢角算出部３２は、それぞれのセンサユニットＳ_ｉの加速度センサＡ_ｉでの加速度データに基づいて、それぞれのセンサユニットＳ_ｉのローカル座標系Ｃ_ｉのグローバル座標系ＣからのＸ軸回りの回転角であるピッチ角α_ｉ、及び、それぞれのセンサユニットＳ_ｉのローカル座標系Ｃ_ｉのグローバル座標系ＣからのＹ軸回りの回転角であるロール角β_ｉを算出する第１の角度算出部３４を備える。また、姿勢角算出部３２は、それぞれのセンサユニットＳ_ｉの地磁気センサＢ_ｉでの地磁気データに基づいて、それぞれのセンサユニットＳ_ｉのローカル座標系Ｃ_ｉのグローバル座標系ＣからのＺ軸回りの回転角であるヨー角γ_ｉを算出する第２の角度算出部３６を備える。

ここで、姿勢検出部３０でそれぞれのセンサユニットＳ_ｉの姿勢を検出する方法について説明する。図４は、内視鏡１０の挿入部１１が停止している静状態での挿入部１１の形状検出方法を示すフローチャートである。図４に示すように、挿入部１１の形状検出の際には、まず、それぞれのセンサユニットＳ_ｉでの計測を行い（ステップＳ１０１）、姿勢検出部３０がそれぞれのセンサユニットＳ_ｉでの計測データを取得する。そして、姿勢角算出部３２が、それぞれのセンサユニットＳ_ｉのローカル座標系Ｃ_ｉの３つの姿勢角α_ｉ，β_ｉ，γ_ｉを算出する。

姿勢角α_ｉ，β_ｉ，γ_ｉを算出する際には、まず、第１の角度算出部３４が、それぞれのセンサユニットＳ_ｉの加速度センサＡ_ｉでの計測データに基づいて、それぞれのセンサユニットＳ_ｉのローカル座標系Ｃ_ｉのピッチ角α_ｉ及びロール角β_ｉを算出する（ステップＳ１０２）。ここで、姿勢角α_ｉ，β_ｉ，γ_ｉはヨー角γ_ｉ、ピッチ角α_ｉ、ロール角β_ｉの順で回転する（Ｚ，Ｘ，Ｙ）型とする。したがって、グローバル座標系Ｃからローカル座標系Ｃ_ｉへの回転行列は、

となる。

挿入部１１が停止している静状態では、重力加速度のみが鉛直方向の下方向に作用している。すなわち、グローバル座標系Ｃにおいても、ローカル座標系Ｃ_ｉにおいても、鉛直方向の下方向に重力加速度のみが作用している。したがって、この際、加速度ベクトルのグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分は、

となる。また、加速度センサＡ_ｉで計測される加速度ベクトルのローカル座標系Ｃ_ｉのＸ_ｉ軸方向成分、Ｙ_ｉ軸方向成分、Ｚ_ｉ軸方向成分を、

とする。

ここで、ローカル座標系Ｃ_ｉはグローバル座標系Ｃをヨー角γ_ｉ、ピッチ角α_ｉ、ロール角β_ｉの順で回転した座標系である。したがって、式（１）〜式（３）より、ローカル座標系Ｃ_ｉで観測される加速度成分は、

となる。ここで、式（４．１）の２乗と式（４．３）の２乗を加算すると、

となり、

となる。そして、式（４．２）を式（６）で割ることにより、

となり、ローカル座標系Ｃ_ｉのピッチ角α_ｉが求まる。また、式（４．１）を式（４．３）で割ることにより、

となり、ローカル座標系Ｃ_ｉのロール角β_ｉが求まる。以上のようにして、それぞれの加速度センサＡ_ｉでの計測データに基づいて、それぞれのローカル座標系Ｃ_ｉのピッチ角α_ｉ及びロール角β_ｉが算出される。

そして、第２の角度算出部３６が、それぞれのセンサユニットＳ_ｉの地磁気センサＢ_ｉでの計測データに基づいて、それぞれのセンサユニットＳ_ｉのローカル座標系Ｃ_ｉのヨー角γ_ｉを算出する（ステップＳ１０３）。ここで、ステップＳ１０２で算出したピッチ角α_ｉ及びロール角β_ｉを用いて、それぞれのローカル座標系Ｃ_ｉのグローバル座標系ＣからのＸ軸回りの回転及びＹ軸回りの回転を補正した補正座標系Ｃ´_ｉを定義する。図５は、グローバル座標系Ｃ（図５において実線で示す）及び補正座標系Ｃ´_ｉ（図５で点線で示す）を示す図である。なお、実際はグローバル座標系Ｃと補正座標系Ｃ´_ｉとでは原点の位置が異なるが、図５では両者の比較のため原点が同一の位置にある状態で示す。図５に示すように、Ｘ軸回りの回転及びＹ軸回りの回転を補正した補正座標系Ｃ´_ｉは、結果的にグローバル座標系ＣをＺ軸回りにヨー角γ_ｉだけ回転した座標系であり、Ｘ´_ｉ軸、Ｙ´_ｉ軸、Ｚ´_ｉ軸を有する。Ｘ´_ｉ軸方向、Ｙ´_ｉ軸方向は、それぞれグローバル座標系ＣのＸ軸方向、Ｙ軸方向からＺ軸回りにヨー角γ_ｉだけ回転した方向と一致する。Ｚ´_ｉ軸方向は、鉛直方向、すなわちグローバル座標系ＣのＺ軸方向と一致する。ここでは、グローバル座標系ＣのＸ軸方向が磁北方向と一致するため、Ｘ´_ｉ軸方向が、グローバル座標系ＣのＸ軸方向からＺ軸回りにヨー角γ_ｉだけ回転した方向となる。

地磁気センサＢ_ｉで計測される地磁気ベクトルのローカル座標系Ｃ_ｉのＸ_ｉ軸方向成分、Ｙ_ｉ軸方向成分、Ｚ_ｉ軸方向成分を、

とする。補正座標系Ｃ´_ｉは、ローカル座標系Ｃ_ｉのグローバル座標系ＣからのＸ軸回りの回転及びＹ軸回りの回転を補正した座標系である。したがって、式（９）及び式（１）のＲ_ｘｉ，Ｒ_ｙｉを用いて、地磁気センサＢ_ｉで計測される地磁気ベクトルの補正座標系Ｃ´_ｉのＸ´_ｉ軸方向成分、Ｙ´_ｉ軸方向成分、Ｚ´_ｉ軸方向成分は、

となる。式（１０．１），式（１０．２）から、

となる。鉛直方向に垂直な水平面（補正座標系Ｃ´_ｉのＸ´_ｉ−Ｙ´_ｉ平面）の地磁気成分は、磁北方向を向いている。したがって、式（１１．１），式（１１．２）より、Ｘ´_ｉ軸から磁北方向までの角度θ_ｉは地磁気ベクトルの補正座標系Ｃ´_ｉのＸ´_ｉ軸成分、Ｙ´_ｉ軸成分を用いて求めることができる。すなわち、

となる。角度θ_ｉはＺ´_ｉ軸（Ｚ軸）を負の方向から見て時計回りを正とする。補正座標系Ｃ´_ｉはグローバル座標系ＣをＺ軸回りにヨー角γ_ｉだけ回転した座標系である。したがって、式（１２）で求まる角度θ_ｉが、グローバル座標系Ｃを基準としたローカル座標系Ｃ_ｉのヨー角γ_ｉとなる。

なお、グローバル座標系ＣのＸ軸方向が磁北方向と一致しない場合も、磁北を基準としてヨー角γ_ｉを求めることもできる。地磁気ベクトルのグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分を、

とする。地磁気ベクトルのグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分は、地磁気センサＢ_ｉと同一のタイプの地磁気センサを用いて、グローバル座標系ＣのＸ軸方向、Ｙ軸方向、Ｚ軸方向と軸の方向が一致する状態で計測を行うことにより、求められる。そして、式（１３）より、Ｘ軸から磁北方向までの角度θを、地磁気ベクトルのグローバル座標系ＣのＸ軸成分、Ｙ軸成分を用いて求める。すなわち、

となる。ここで、角度θはＺ軸を負の方向から見て時計回りを正とする。補正座標系Ｃ´_ｉはグローバル座標系ＣをＺ軸回りにヨー角γ_ｉだけ回転した座標系である。したがって、式（１２）、式（１４）より、

となり、グローバル座標系Ｃを基準としたローカル座標系Ｃ_ｉのヨー角γ_ｉが求まる。

以上のようにして、それぞれの地磁気センサＢ_ｉでの計測データに基づいて、それぞれのローカル座標系Ｃ_ｉのヨー角γ_ｉが算出される。算出された姿勢角α_ｉ，β_ｉ，γ_ｉの値に基づいて、姿勢検出部３０はそれぞれのセンサユニットＳ_ｉの姿勢を検出する。

図３に示すように、姿勢検出部３０には、線形形状検出部４０が接続されている。図６は、線形形状検出部４０により検出される内視鏡１０の挿入部１１の検出線形形状６１をグローバル座標ＣのＺ軸の正の方向から見た図である。図６に示すように、線形形状検出部４０は、姿勢検出部３０で検出されたそれぞれのセンサユニットＳ_ｉの姿勢に基づいて、それぞれのセンサユニットＳ_ｉの間の形状を寸法がセンサ間寸法ｌに等しい直線状のリンクＴ_ｊ（ｊ＝１，２，…Ｎ）と仮定して、挿入部１１の検出線形形状６１を検出する。ここで、基端側からｋ番目のリンクＴ_ｋは、基端側からｋ番目のセンサユニットＳ_ｋ−１と基端側から（ｋ＋１）番目のセンサユニットＳ_ｋとの間のリンクである。線形形状検出部４０は、それぞれのリンクＴ_ｊを形成するリンク形成部４１と、リンク形成部４１により形成されたそれぞれのリンクＴ_ｊを平行移動して、リンクＴ_ｊの位置を補正するリンク位置補正部４２とを備える。リンク位置補正部４２によりそれぞれのリンクＴ_ｊは、隣接するリンクＴ_ｊ−１，Ｔ_ｊ＋１とのリンク境界が連続する状態に、平行移動される。

線形形状検出部４０には、描画部４５が接続されている。描画部４５には、表示部４７が接続されている。線形形状検出部４０により検出されたグローバル座標系Ｃでの挿入部１１の検出線形形状６１は、描画部４５により描画される。術者は、描画部４５により描画された検出線形形状６１を表示部４７で確認可能となっている。

ここで、線形形状検出部４０で挿入部１１の検出線形形状６１を検出する方法について説明する。図４に示すように、挿入部１１の検出線形形状６１を検出する際、まず、ステップＳ１０２，Ｓ１０３で算出した姿勢角α_ｉ，β_ｉ，γ_ｉの値に基づいて、リンク形成部４１が、直線形状のそれぞれのリンクＴ_ｊを形成する（ステップＳ１０４）。ここでは、基端側からｋ番目のセンサユニットＳ_ｋ−１と基端側から（ｋ＋１）番目のセンサユニットＳ_ｋとの間の基端側からｋ番目のリンクＴ_ｋの形成について説明する。

式（７）、式（８）、式（１２）（又は式（１５））に示すように、ステップＳ１０２，Ｓ１０３でローカル座標系Ｃ_ｋ−１（つまりリンク）の姿勢角α_ｋ−１，β_ｋ−１，γ_ｋ−１が算出される。この姿勢角α_ｋ−１，β_ｋ−１，γ_ｋ−１及びそれぞれのセンサユニットＳ_ｉの間の長手方向についての間隔であるセンサ間寸法ｌを用いて、センサユニットＳ_ｋ−１がグローバル座標系Ｃの原点に位置する場合の、センサユニットＳ_ｋの座標Ｐ´_ｋ（ｌ_ｘｋ，ｌ_ｙｋ，ｌ_ｚｋ）が求まる。ここで、座標Ｐ´_ｋは、

となる。式（１６．１），式（１６．２）のｅ_ｙｋ−１は、ローカル座標系Ｃ_ｋ−１の原点での長手方向であるＹ_ｋ−１軸方向の単位ベクトルである。単位ベクトルｅ_ｙｋ−１と式（１）で算出される回転行列を乗算することにより、単位ベクトルｅ_ｙｋ−１のグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分がそれぞれ算出される。すなわち、ｌ_ｘｋ，ｌ_ｙｋ，ｌ_ｚｋは、ローカル座標系Ｃ_ｋ−１のＹ_ｋ−１軸方向への大きさｌのベクトルを、それぞれグローバル座標系ＣのＸ軸方向、Ｙ軸方向、Ｚ軸方向に分解した成分である。グローバル座標系Ｃの原点と式（１６．１），式（１６．２）で算出される座標Ｐ´_ｋ（ｌ_ｘｋ，ｌ_ｙｋ，ｌ_ｚｋ）とを直線状に結ぶことにより、リンクＴ_ｋが形成される。

なお、リンクＴ_ｋ以外のリンクＴ_ｊについても、リンク形成部４１により同様に形成される。すなわち、式（１６．１），式（１６．２）を用いて、リンクＴ_ｊの基端側（グローバル座標系Ｃの原点から近い側）のセンサユニットＳ_ｊ−１がグローバル座標系Ｃの原点に位置する場合の、リンクＴ_ｊの先端側（グローバル座標系Ｃの原点から遠い側）のセンサユニットＳ_ｊの座標Ｐ´_ｊ（ｌ_ｘｊ，ｌ_ｙｊ，ｌ_ｚｊ）を求める。そして、グローバル座標系Ｃの原点と座標Ｐ´_ｊ（ｌ_ｘｊ，ｌ_ｙｊ，ｌ_ｚｊ）とを直線状に結ぶことにより、リンクＴ_ｊが形成される。すなわち、リンク形成部４１は、リンクＴ_ｊが基端側（グローバル座標系Ｃの原点から近い側）のセンサユニットＳ_ｊ−１の中心から先端側（グローバル座標系Ｃの原点から遠い側）のセンサユニットＳ_ｊの中心まで基端側のセンサユニットＳ_ｊ−１の中心での長手方向に延設されると仮定して、リンクＴ_ｊを形成する。

また、センサ間寸法ｌは５０ｍｍ程度であることが好ましい。センサ間寸法ｌを長くすることにより、センサユニットＳ_ｉの数が減少し、コストを削減される。また、センサ間寸法ｌが５０ｍｍ程度より小さい範囲であれば、それぞれのセンサユニットＳ_ｉの間を寸法がセンサ間寸法ｌに等しい直線状のリンクＴ_ｊと仮定した場合でも、挿入部１１の形状検出の際の誤差を小さくすることが可能である。

そして、リンク位置補正部４２が、リンク形成部４１により形成されたそれぞれのリンクＴ_ｊを隣接するリンクＴ_ｊ−１，Ｔ_ｊ＋１とのリンク境界が連続する状態に平行移動して、リンクＴ_ｊの位置を補正する（ステップＳ１０５）。図７は、リンク位置補正部４２での処理を説明する図である。ここでは、基端側からｋ番目のセンサユニットＳ_ｋ−１と基端側から（ｋ＋１）番目のセンサユニットＳ_ｋとの間の基端側からｋ番目のリンクＴ_ｋの位置補正について説明する。

図７に示すように、リンク位置補正部４２により、リンクＴ_ｋの位置補正が行われる前の状態では、リンクＴ_ｋの基端側に隣接するリンクＴ_ｋ−１まで位置補正が完了し、リンク位置補正完了部６３が形成されている。リンクＴ_ｋの位置補正を行う際は、原点からリンク位置補正完了部６３の先端までの移動量だけ、リンク位置補正部４２はリンクＴ_ｋを平行移動する。すなわち、図７の点線で示す位置から図７の実線で示す位置にリンクＴ_ｋを平行移動する。これにより、リンクＴ_ｋ−１とリンクＴ_ｋとのリンク境界が連続する状態となり、リンクＴ_ｋの位置が補正される。

なお、リンクＴ_ｋ以外のリンクＴ_ｊについても、リンク位置補正部４２により同様に位置が補正される。すなわち、リンクＴ_ｊの位置補正を行う際は、原点からリンク位置補正完了部６３の先端（グローバル座標系Ｃの原点から遠い側の端）までの移動量だけ、リンク位置補正部４２はリンクＴ_ｊを平行移動する。これにより、リンクＴ_ｊとリンクＴ_ｊの基端側（グローバル座標系Ｃの原点から近い側）に隣接するリンクＴ_ｊ−１とのリンク境界が連続する状態となり、リンクＴ_ｊの位置が補正される。ただし、リンクＴ_１については、リンクＴ_１の基端がグローバル座標系Ｃの原点であるため、位置の補正は行われない。

図３に示すように、線形形状検出部４０は、曲線形状検出部５０に接続されている。図８は、曲線形状検出部５０により検出された内視鏡１０の挿入部１１の検出曲線形状６５をグローバル座標ＣのＺ軸の正の方向から見た図である。図８に示すように、曲線形状検出部５０は、それぞれのセンサユニットＳ_ｉの間の形状を弧長がセンサ間寸法ｌに等しい円弧Ｌ_ｊ（ｊ＝１，２，…Ｎ）と仮定して、検出線形形状６１を曲線補間する。曲線補間を行うことにより、検出曲線形状６５が形成される。ここで、基端側からｋ番目の円弧Ｌ_ｋは、基端側からｋ番目のセンサユニットＳ_ｋ−１と基端側から（ｋ＋１）番目のセンサユニットＳ_ｋとの間の円弧である。

曲線形状検出部５０は、それぞれのリンクＴ_ｊごとに曲線補間を行い、円弧Ｌ_ｊを形成する曲線補間部５１と、すべてのリンクＴ_ｊについて曲線補間を行う状態に曲線補間部５１を制御する補間制御部５２と、曲線補間部５１により形成されたそれぞれの円弧Ｌ_ｊを平行移動して、円弧Ｌ_ｊの位置を補正する円弧位置補正部５３とを備える。円弧位置補正部５３によりそれぞれの円弧Ｌ_ｊは、隣接する円弧Ｌ_ｊ−１，Ｌ_ｊ＋１との円弧境界が連続する状態に、平行移動される。

図３に示すように、曲線補間部５１は、法線形成部５５と、中心決定部５７と、円弧形成部５９とを備える。法線形成部５５、中心決定部５７及び円弧形成部５９の詳細については、後述する。

曲線形状検出部５０には、描画部４５が接続されている。曲線形状検出部５０により形成されたグローバル座標系Ｃでの挿入部１１の検出曲線形状６５は、描画部４５により描画される。術者は、描画部４５により描画された検出曲線形状６５を表示部４７で確認可能となっている。

ここで、曲線形状検出部５０で、線形形状検出部４０により検出された検出線形形状６１を曲線補間し、検出曲線形状６５を検出する方法について説明する。図４に示すように、曲線形状検出部５０は、それぞれのセンサユニットＳ_ｉの間の形状を弧長がセンサ間寸法ｌに等しい円弧Ｌ_ｊと仮定して、検出線形形状６１の曲線補間を行い、検出曲線形状６５を推定する（ステップＳ１０６）。前述のように、センサ間寸法ｌが５０ｍｍ程度より小さい範囲であれば、それぞれのセンサユニットＳ_ｉの間を寸法がセンサ間寸法ｌに等しい直線状のリンクＴ_ｊと仮定した場合でも、挿入部１１の形状検出の際の誤差は小さくなる。しかし、体腔内に挿入された際の内視鏡１０の挿入部１１の形状は曲線形状である。したがって、検出線形形状６１の曲線補間を行うことが重要となる。ここで、内視鏡１０の挿入部１１は、製品により差はあるが、適度の弾性を有する。このため、挿入部１１の曲線形状の曲率が大きく変化することは、稀である。したがって、センサユニットＳ_ｉの間の形状を半径Ｒ_ｊ（曲率１／Ｒ_ｊ）の円弧Ｌ_ｊと仮定して曲線補間を行うことにより、実際の挿入部１１の曲線形状と誤差の少ない検出曲線形状６５が形成される。

検出線形形状６１の曲線補間を行い、検出曲線形状６５を検出する際、曲線補間部５１は、それぞれのリンクＴ_ｊごとに曲線補間を行い、円弧Ｌ_ｊを形成する。ここで、曲線補間部５１により、リンクＴ_ｊの曲線補間を行う方法について説明する。ここでは、基端側からｋ番目のセンサユニットＳ_ｋ−１と基端側から（ｋ＋１）番目のセンサユニットＳ_ｋとの間の基端側からｋ番目のリンクＴ_ｋの曲線補間について説明する。

図９は、曲線形状検出部５０により検出線形形状６１から検出曲線形状６５を検出する方法を示すフローチャートである。図１０は、曲線補間部５１での処理を説明する図である。図１０に示すように、曲線補間部５１により補間対象であるリンク（補間対象リンク）Ｔ_ｋの曲線補間を行う前の状態では、センサユニットＳ_ｋは点Ｑ´_ｋに位置している。図９及び図１０に示すように、曲線補間部５１によりリンク（補間対称リンク）Ｔ_ｋの曲線補間を行う際は、まず、法線形成部５５が、リンクＴ_ｋの基端（第１の端点）を通り、リンクＴ_ｋに垂直な第１の補間法線Ｎ_ｋ−１と、リンクＴ_ｋの先端（第２の端点）を通り、リンクＴ_ｋの先端側に隣接するリンク（補間対象隣接リンク）Ｔ_ｋ＋１に垂直な第２の補間法線Ｎ_ｋとを形成する（ステップＳ１１１）。すなわち、第１の補間法線Ｎ_ｋ−１は、リンクＴ_ｋの方向ベクトルに垂直という条件、及び、センサユニット（第１の端点）Ｓ_ｋ−１を通過するという条件から求めることができる。同様に、第２の補間法線Ｎ_ｋは、リンクＴ_ｋ＋１の方向ベクトルに垂直という条件、及び、センサユニット（第２の端点）Ｓ_ｋを通過するという条件から求めることができる。

そして、中心決定部５７が、第１の補間法線Ｎ_ｋ−１と第２の補間法線Ｎ_ｋとの交点Ｏ_ｋを算出し、交点Ｏ_ｋを円弧Ｌ_ｋの中心として決定する（ステップＳ１１２）。第１の補間法線Ｎ_ｋ−１と第２の補間法線Ｎ_ｋとが交わらない場合は、第１の補間法線Ｎ_ｋ−１と第２の補間法線Ｎ_ｋとの距離が最小となる２点の中間点Ｏ_ｋを算出する。中間点Ｏ_ｋを算出する際は、まず、第１の補間法線Ｎ_ｋ−１及び第２の補間法線Ｎ_ｋの両方に垂直な直線（図示しない）の方程式を求める。そして、この直線と第１の補間法線Ｎ_ｋ−１との交点、及び、この直線と第２の補間法線Ｎ_ｋとの交点を求め、２つ交点の中間点が中間点Ｏ_ｋとなる。そして、中間点Ｏ_ｋを円弧Ｌ_ｋの中心Ｏ_ｋとして決定する（ステップＳ１１２）。

そして、円弧形成部５９が、円弧Ｌ_ｋを形成する（ステップＳ１１３）。円弧Ｌ_ｋは、中心決定部５７により決定された中心Ｏ_ｋとリンク（補間対象リンク）Ｔ_ｋの基端（第１の端点）との間の距離と同一の半径Ｒ_ｋを有する。すなわち、円弧Ｌ_ｋは曲率１／Ｒ_ｋを有する。また、円弧Ｌ_ｋは、リンクＴ_ｋの基端（第１の端点）を始点とする弧の長さがセンサ間寸法ｌに等しい円弧Ｌ_ｋである。円弧Ｌ_ｋの先端Ｑ_ｋが、曲線補間後のセンサユニットＳ_ｋの位置となる。すなわち、リンクＴ_ｋの曲線補間により、センサユニットＳ_ｋの位置が点Ｑ´_ｋから点Ｑ_ｋに移動する。以上のようにして、リンクＴ_ｋの曲線補間が行われる。

なお、リンクＴ_ｋ以外のリンクＴ_ｊについても、曲線補間部５１により同様に曲線補間が行われる。すなわち、リンク（補間対象リンク）Ｔ_ｊの曲線補間を行う際は、法線形成部５５が、リンクＴ_ｊの基端（グローバル座標Ｃの原点から近い側の端）である第１の端点を通り、リンクＴ_ｊに垂直な第１の補間法線Ｎ_ｊ−１と、リンクＴ_ｊの先端（グローバル座標Ｃの原点から遠い側の端）である第２の端点を通り、リンクＴ_ｊの先端側（グローバル座標Ｃの原点から遠い側）に隣接するリンク（補間対象隣接リンク）Ｔ_ｊ＋１に垂直な第２の補間法線Ｎ_ｊとを形成する（ステップＳ１１１）。そして、中心決定部５７が、第１の補間法線Ｎ_ｊ−１と第２の補間法線Ｎ_ｊとの交点Ｏ_ｊ、又は、第１の補間法線Ｎ_ｊ−１と第２の補間法線Ｎ_ｊとの距離が最小となる２点の中間点Ｏ_ｊを算出し、交点Ｏ_ｊ又は中間点Ｏ_ｊを円弧Ｌ_ｊの中心Ｏ_ｊとして決定する（ステップＳ１１２）。そして、円弧形成部５９が、半径Ｒ_ｊが中心Ｏ_ｊとリンク（補間対象リンク）Ｔ_ｊの基端との間の距離と同一で、かつ、リンクＴ_ｊの基端を始点とする弧の長さがセンサ間寸法ｌに等しい円弧Ｌ_ｊ形成する（ステップＳ１１３）。

そして、図９に示すように、補間制御部５２が、すべてのリンクＴ_ｊについて曲線補間が完了したか確認する（ステップＳ１１４）。すべてのリンクＴ_ｊについて曲線補間が完了している場合は、次のステップに進む（ステップＳ１１４−Ｙｅｓ）。すべてのリンクＴ_ｊについて曲線補間が完了していない場合は、ステップＳ１１１に戻り（ステップＳ１１４−Ｎｏ）、曲線補間部５１により、曲線補間が完了していないリンクＴ_ｊについて曲線補間が行われる。すなわち、すべてのリンクＴ_ｊ（本実施形態ではＮ本）について曲線補間が完了するまでステップＳ１１１〜Ｓ１１３を行う状態に曲線補間部５１が制御される。

曲線補間部５１によりすべてのリンクＴ_ｊについて曲線補間が行われると、円弧位置補正部５３が、曲線補間部５１により形成されたそれぞれの円弧Ｌ_ｊを隣接する円弧Ｌ_ｊ−１，Ｌ_ｊ＋１との円弧境界が連続する状態に平行移動して、円弧Ｌ_ｊの位置を補正する（ステップＳ１１５）。図１１は、円弧位置補正部５３での処理を説明する図である。ここでは、基端側からｋ番目のセンサユニットＳ_ｋ−１と基端側から（ｋ＋１）番目のセンサユニットＳ_ｋとの間の基端側からｋ番目の円弧Ｌ_ｋの位置補正について説明する。

図１１に示すように、円弧位置補正部５３により、円弧Ｌ_ｋの位置補正が行われる前の状態では、円弧Ｌ_ｋの基端側に隣接する円弧Ｌ_ｋ−１まで位置補正が完了し、円弧位置補正完了部６７が形成されている。円弧Ｌ_ｋの位置補正を行う際は、位置補正前の円弧Ｌ_ｋの基端から円弧位置補正完了部６７の先端までの移動量だけ、円弧位置補正部５３は円弧Ｌ_ｋを平行移動する。すなわち、図１１の点線で示す位置から図１１の実線で示す位置に円弧Ｌ_ｋを平行移動する。これにより、円弧Ｌ_ｋ−１と円弧Ｌ_ｋとの円弧境界が連続する状態となり、円弧Ｌ_ｋの位置が補正される。

なお、円弧Ｌ_ｋ以外の円弧Ｌ_ｊについても、円弧位置補正部５３により同様に位置が補正される。すなわち、円弧Ｌ_ｊの位置補正を行う際は、位置補正前の円弧Ｌ_ｊの基端から円弧位置補正完了部６７の先端（グローバル座標系Ｃの原点から遠い側の端）までの移動量だけ、円弧位置補正部５３は円弧Ｌ_ｊを平行移動する。これにより、円弧Ｌ_ｊと円弧Ｌ_ｊの基端側（グローバル座標系Ｃの原点から近い側）に隣接する円弧Ｌ_ｊ−１との円弧境界が連続する状態となり、円弧Ｌ_ｊの位置が補正される。ただし、円弧Ｌ_１については、円弧Ｌ_１の基端がグローバル座標系Ｃの原点であるため、位置の補正は行われない。

図４に示すように、曲線形状検出部５０による検出線形形状６１の曲線補間が完了し、検出曲線形状６５が形成されると、内視鏡形状検出装置１による検査が完了したかを確認する（ステップＳ１０７）。検査が完了していない場合は（ステップＳ１０７−Ｎｏ）、ステップＳ１０１に戻り、次の静状態での内視鏡１０の挿入部１１の形状検出を行う。検査が完了している場合は（ステップＳ１０７−Ｙｅｓ）、内視鏡１０の挿入部１１の形状検出は終了する。

そこで、上記構成の内視鏡形状検出装置１及び内視鏡形状検出装置１を用いた内視鏡１０の挿入部１１の形状検出方法では、以下の効果を奏する。すなわち、内視鏡形状検出装置１では、それぞれのセンサユニットＳ_ｉの計測データから姿勢検出部３０がセンサユニットＳ_ｉの姿勢を検出し、線形形状検出部４０がそれぞれのセンサユニットＳ_ｉの姿勢から内視鏡１０の挿入部１１の検出線形形状６１を検出する。そして、曲線形状検出部５０が、検出線形形状６１の曲線補間を行い、検出曲線形状６５を検出する。以上のように、観察時に体腔内に挿入される挿入部１１に配置されるセンサユニットＳ_ｉの計測データから、挿入部１１の検出曲線形状６５が検出されるため、体外にセンスコイル等を設ける必要がない。このため、内視鏡形状検出装置１の小型化、単純化を実現することができる。

また、内視鏡形状検出装置１では、曲線形状検出部５０により、それぞれのセンサユニットＳ_ｉの間の形状を弧長がセンサ間寸法ｌに等しい円弧Ｌ_ｊと仮定して、検出線形形状６１の曲線補間を行い、検出曲線形状６５を検出している。実際に、内視鏡１０の挿入部１１は、製品により差はあるが、適度の弾性を有する。このため、挿入部１１の曲線形状の曲率が大きく変化することは、稀である。したがって、センサユニットＳ_ｉの間の形状を半径Ｒ_ｊ（曲率１／Ｒ_ｊ）の円弧Ｌ_ｊと仮定して曲線補間を行うことにより、実際の挿入部１１の曲線形状と誤差の少ない検出曲線形状６５を検出することができる。これにより、高い精度で挿入部１１の検出曲線形状６５を検出することができる。

さらに、内視鏡形状検出装置１では、挿入部１１が移動していない静状態において、加速度センサＡ_ｉが重力加速度を計測し、地磁気センサＢ_ｉが地磁気を計測する。そして、計測された重力加速度、地磁気から姿勢検出部３０がそれぞれのセンサユニットＳ_ｉの姿勢を検出する。静状態では、重力加速度及び地磁気は、常に一定の方向に一定の大きさを有する。重力加速度、地磁気からそれぞれのセンサユニットＳ_ｉの姿勢を検出するため、静状態においても高い精度でセンサユニットＳ_ｉの姿勢を検出することができる。これにより、高い精度で挿入部１１の検出曲線形状６５を検出することができる。

（第２の実施形態）
次に、本発明の第２の実施形態について図１２乃至図１５を参照して説明する。なお、第１の実施形態と同一の部分及び同一の機能を有する部分については同一の符号を付して、その説明は省略する。

図１２は、本実施形態のパソコン８の構成を示す図である。図１２に示すように、パソコン８は、第１の実施形態と同様に、通信部２６、物理量換算部２８、姿勢検出部３０、線形形状検出部４０、描画部４５及び表示部４７とを備える。線形形状検出部４０及び描画部４５には、曲線形状検出部７０が接続されている。

曲線形状検出部７０は、第１の実施形態の曲線形状検出部５０と同様に、それぞれのセンサユニットＳ_ｉの間の形状を弧長がセンサ間寸法ｌに等しい円弧Ｌ_ｊ（ｊ＝１，２，…Ｎ）と仮定して、検出線形形状６１を曲線補間し、検出曲線形状８１を検出する。ただし、検出曲線形状８１を検出する手法が曲線形状検出部５０とは異なる。

曲線形状検出部７０は、それぞれのリンクＴ_ｊごとに基端側（グローバル座標系Ｃの原点から近い側）のリンクＴ_ｊから順に曲線補間を行い、円弧Ｌ_ｊを形成する曲線補間部である補間順次実施部７１と、すべてのリンクＴ_ｊについて曲線補間を行う状態に補間順次実施部７１を制御する補間制御部７２とを備える。また、曲線形状検出部７０は、補間順次実施部７１により１つのリンクＴ_ｊの曲線補間が行われるたびに、曲線補間が行われていないリンク（未補間リンク）Ｔ_ｊから構成される補間未完了部８５を平行移動し、位置を補正する未補間リンク位置補正部７３を備える。未補間リンク位置補正部７３により、補間未完了部８５が、曲線補間により形成される円弧Ｌ_ｊから構成される補間完了部８３と境界が連続する状態に平行移動される。

図１２に示すように、補間順次実施部７１は、法線形成部７５と、中心決定部７７と、円弧形成部７９とを備える。法線形成部７５、中心決定部７７及び円弧形成部７９の詳細については、後述する。

ここで、曲線形状検出部７０で、線形形状検出部４０により検出された検出線形形状６１の曲線補間を行い、検出曲線形状８１を検出する方法について説明する。曲線形状検出部７０は、第１の実施形態の曲線形状検出部５０と同様に、それぞれのセンサユニットＳ_ｉの間の形状を弧長がセンサ間寸法ｌに等しい円弧Ｌ_ｊと仮定して、検出線形形状６１を曲線補間する（図４のステップＳ１０６）。これにより、第１の実施形態で前述したように、実際の挿入部１１の曲線形状と誤差の少ない検出曲線形状８１が形成される。

検出線形形状６１の曲線補間を行う際、補間順次実施部７１は、それぞれのリンクＴ_ｊごとに基端側（グローバル座標系Ｃの原点から近い側）のリンクＴ_ｊから順に曲線補間を行い、円弧Ｌ_ｊを形成する。ここで、補間順次実施部７１により、リンクＴ_ｊの曲線補間を行う方法について説明する。ここでは、基端側からｋ番目のセンサユニットＳ_ｋ−１と基端側から（ｋ＋１）番目のセンサユニットＳ_ｋとの間の基端側からｋ番目のリンクＴ_ｋの曲線補間について説明する。

図１３は、曲線形状検出部７０により検出線形形状６１から検出曲線形状８１を検出する方法を示すフローチャートである。図１４は、補間順次実施部７１での処理を説明する図である。図１４に示すように、補間順次実施部７１により、リンクＴ_ｋの曲線補間が行われる前の状態では、リンクＴ_ｋ−１まで曲線補間が完了し、補間完了部８３が形成されている。補間完了部８３は、円弧Ｌ_１〜Ｌ_ｋ−１から構成されている。また、補間が行われていないリンク（補間未完了リンク）Ｔ_ｋ〜Ｔ_Ｎから構成される補間未完了部８５が形成されている。この際、補間完了部８３と補間未完了部８５との境界は連続し、センサユニットＳ_ｋは点Ｕ´_ｋに位置している。

この状態で、補間順次実施部７１により、補間未完了部８５の中で最も基端側に位置するリンク（補間対象リンク）Ｔ_ｋの曲線補間が行われる。図１３及び図１４に示すように、リンクＴ_ｋの曲線補間を行う際は、まず、法線形成部７５が、リンクＴ_ｋの基端（第１の端点）を通り、リンクＴ_ｋに垂直な第１の補間法線Ｎ_ｋ−１と、リンクＴ_ｋの先端（第２の端点）を通り、リンクＴ_ｋの先端側に隣接するリンク（補間対象隣接リンク）Ｔ_ｋ＋１に垂直な第２の補間法線Ｎ_ｋとを形成する（ステップＳ１２１）。

そして、中心決定部７７が、第１の補間法線Ｎ_ｋ−１と第２の補間法線Ｎ_ｋとの交点Ｏ_ｋを算出し、交点Ｏ_ｋを円弧Ｌ_ｋの中心として決定する（ステップＳ１２２）。第１の補間法線Ｎ_ｋ−１と第２の補間法線Ｎ_ｋとが交わらない場合は、第１の補間法線Ｎ_ｋ−１と第２の補間法線Ｎ_ｋとの距離が最小となる２点の中間点Ｏ_ｋを算出する。中間点Ｏ_ｋは、第１の実施形態と同様の手法により算出される。そして、中間点Ｏ_ｋを円弧Ｌ_ｋの中心Ｏ_ｋとして決定する（ステップＳ１２２）。

そして、円弧形成部７９が、円弧Ｌ_ｋを形成する（ステップＳ１２３）。円弧Ｌ_ｋは、中心決定部７７により決定された中心Ｏ_ｋとリンク（補間対象リンク）Ｔ_ｋの基端との間の距離と同一の半径Ｒ_ｋを有する。すなわち、円弧Ｌ_ｋは曲率１／Ｒ_ｋを有する。また、円弧Ｌ_ｋは、リンクＴ_ｋの基端を始点とする弧の長さがセンサ間寸法ｌに等しい円弧Ｌ_ｋである。円弧Ｌ_ｋの先端Ｕ_ｋが、曲線補間後のセンサユニットＳ_ｋの位置となる。すなわち、リンクＴ_ｋの曲線補間により、センサユニットＳ_ｋの位置が点Ｕ´_ｋから点Ｕ_ｋに移動する。センサユニットＳ_ｋの位置の移動により、補間順次実施部７１によりリンクＴ_ｋの曲線補間が行われる前の状態では連続していた補間完了部８３と補間未完了部８５との境界が、不連続な状態となる。すなわち、円弧Ｌ_ｋとリンクＴ_ｋ＋１が不連続な状態となる。

なお、リンクＴ_ｋ以外のリンクＴ_ｊについても、補間順次実施部７１により同様に曲線補間が行われる。すなわち、リンク（補間対象リンク）Ｔ_ｊの曲線補間を行う際は、法線形成部７５が、リンクＴ_ｊの基端（グローバル座標Ｃの原点から近い側の端）である第１の端点を通り、リンクＴ_ｊに垂直な第１の補間法線Ｎ_ｊ−１と、リンクＴ_ｊの先端（グローバル座標Ｃの原点から遠い側の端）である第２の端点を通り、リンクＴ_ｊの先端側（グローバル座標Ｃの原点から遠い側）に隣接するリンク（補間対象隣接リンク）Ｔ_ｊ＋１に垂直な第２の補間法線Ｎ_ｊとを形成する（ステップＳ１２１）。そして、中心決定部７７が、第１の補間法線Ｎ_ｊ−１と第２の補間法線Ｎ_ｊとの交点Ｏ_ｊ、又は、第１の補間法線Ｎ_ｊ−１と第２の補間法線Ｎ_ｊとの距離が最小となる２点の中間点Ｏ_ｊを算出し、交点Ｏ_ｊ又は中間点Ｏ_ｊを円弧Ｌ_ｊの中心Ｏ_ｊとして決定する（ステップＳ１２２）。そして、円弧形成部７９が、半径Ｒ_ｊが中心Ｏ_ｊとリンク（補間対象リンク）Ｔ_ｊの基端との間の距離と同一で、かつ、リンクＴ_ｊの基端を始点とする弧の長さがセンサ間寸法ｌに等しい円弧Ｌ_ｊ形成する（ステップＳ１２３）。

図１５は、未補間リンク位置補正部７３での処理を説明する図である。図１５に示すように、補間順次実施部７１により、リンクＴ_ｋの曲線補間が行われ、円弧Ｌ_ｋが形成された後の状態では、曲線補間が完了した補間完了部８３と、曲線補間が完了していない補間未完了部８５とが形成されている。補間完了部８３は円弧Ｌ_１〜Ｌ_ｋから構成され、補間未完了部８５はリンクＴ_ｋ＋１〜Ｔ_Ｎから構成されている。この際、前述のようにリンクＴ_ｋの曲線補間によりセンサユニットＳ_ｋの位置が点Ｕ´_ｋから点Ｕ_ｋに移動するため、補間完了部８３と補間未完了部８５との境界は、不連続となっている。この状態で、未補間リンク位置補正部７３が、補間未完了部８５を補間完了部８３と境界が連続する状態に平行移動して、補間未完了部８５の位置を補正する（ステップＳ１２４）。すなわち、図１５の点線で示す位置から、実線で示す位置に補間未完了部８５が平行移動される。

なお、リンクＴ_ｋ以外のリンクＴ_ｊについても、同様に未補間リンク位置補正部７３により補間未完了部８５の位置が補正される。すなわち、補間順次実施部７１によりリンクＴ_ｊの曲線補間が行われた後、補間完了部８３と補間未完了部８５との境界は不連続となっている。この状態で、未補間リンク位置補正部７３が、補間未完了部８５を補間完了部８３と連続する状態に平行移動して、補間未完了部８５の位置を補正する（ステップＳ１２４）。未補間リンク位置補正部７３は、補間順次実施部７１により１つのリンクＴ_ｊの曲線補間が行われるたびに、補間未完了部８５の位置を補正する。

そして、図１３に示すように、補間制御部７２が、すべてのリンクＴ_ｊについて曲線補間が完了したか確認する（ステップＳ１２５）。すべてのリンクＴ_ｊについて曲線補間が完了している場合は、挿入部１１の検出曲線形状８１が形成され、次のステップに進む（ステップＳ１２５−Ｙｅｓ）。すべてのリンクＴ_ｊについて曲線補間が完了していない場合は、ステップＳ１２１に戻り（ステップＳ１２５−Ｎｏ）、補間順次実施部７１により、補間未完了部８５の中で最も基端側（グローバル座標系Ｃの原点から近い側）に位置するリンク（補間対象リンク）Ｔ_ｊの曲線補間が行われる。すなわち、すべてのリンクＴ_ｊについて曲線補間が完了するまでステップＳ１２１〜Ｓ１２３を行う状態に、補間順次実施部７１が制御される。そして、補間順次実施部７１により次のリンクＴ_ｊの曲線補間が行われると（ステップＳ１２１〜Ｓ１２３）、未補間リンク位置補正部７３により補間未完了部８５の位置の補正が行われる（ステップＳ１２４）。

そこで、上記構成の内視鏡形状検出装置１及び内視鏡形状検出装置１を用いた内視鏡１０の挿入部１１の形状検出方法では、以下の効果を奏する。すなわち、内視鏡形状検出装置１では、それぞれのセンサユニットＳ_ｉの計測データから姿勢検出部３０がセンサユニットＳ_ｉの姿勢を検出し、線形形状検出部４０がそれぞれのセンサユニットＳ_ｉの姿勢から内視鏡１０の挿入部１１の検出線形形状６１を検出する。そして、曲線形状検出部７０が、検出線形形状６１の曲線補間を行い、検出曲線形状８１を検出する。以上のように、観察時に体腔内に挿入される挿入部１１に配置されるセンサユニットＳ_ｉの計測データから、挿入部１１の検出曲線形状８１が検出されるため、体外にセンスコイル等を設ける必要がない。このため、内視鏡形状検出装置１の小型化、単純化を実現することができる。

また、内視鏡形状検出装置１では、曲線形状検出部７０により、それぞれのセンサユニットＳ_ｉの間の形状を弧長がセンサ間寸法ｌに等しい円弧Ｌ_ｊと仮定して、検出線形形状６１の曲線補間を行い、検出曲線形状８１を検出している。実際に、内視鏡１０の挿入部１１は、製品により差はあるが、適度の弾性を有する。このため、挿入部１１の曲線形状の曲率が大きく変化することは、稀である。したがって、センサユニットＳ_ｉの間の形状を半径Ｒ_ｊ（曲率１／Ｒ_ｊ）の円弧Ｌ_ｊと仮定して曲線補間を行うことにより、実際の挿入部１１の曲線形状と誤差の少ない検出曲線形状８１を検出することができる。これにより、高い精度で挿入部１１の検出曲線形状８１を検出することができる。

さらに、内視鏡形状検出装置１では、挿入部１１が移動していない静状態において、加速度センサＡ_ｉが重力加速度を計測し、地磁気センサＢ_ｉが地磁気を計測する。そして、計測された重力加速度、地磁気から姿勢検出部３０がそれぞれのセンサユニットＳ_ｉの姿勢を検出する。静状態では、重力加速度及び地磁気は、常に一定の方向に一定の大きさを有する。重力加速度、地磁気からそれぞれのセンサユニットＳ_ｉの姿勢を検出するため、静状態においても高い精度でセンサユニットＳ_ｉの姿勢を検出することができる。これにより、高い精度で挿入部１１の検出曲線形状８１を検出することができる。

（第１の変形例）
次に、上述の実施形態の第１の変形例について図１６及び図１７を参照して説明する。なお、第１の実施形態及び第２の実施形態と同一の部分及び同一の機能を有する部分については同一の符号を付して、その説明は省略する。

本変形例では、内視鏡１０の挿入部１１が平行移動している動状態での挿入部１１の形状を検出可能である。動状態での挿入部１１の形状を検出は、図４のステップＳ１０６とステップＳ１０７との間に行われる。図１６は、本変形例のパソコン８の構成を示す図である。図１７は、動状態での挿入部１１の形状を検出する手法を示すフローチャートである。

図１６に示すように、パソコン８は、第１の実施形態と同様に、通信部２６、物理量換算部２８、姿勢検出部３０、線形形状検出部４０、描画部４５、表示部４７及び曲線形状検出部５０とを備える。

物理量換算部２８には、加速度検出部９０が接続されている。図１７に示すように、動状態での挿入部１１の形状を検出する際、加速度検出部９０は、それぞれのセンサユニットＳ_ｉの加速度センサＡ_ｉにより計測された動状態での加速度データを取得する（ステップＳ１３１）。そして、加速度検出部９０は、動状態での加速度データに基づいて、それぞれのセンサユニットＳ_ｉの中心で計測される加速度ベクトルを、グローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分に分解する。そして、それぞれのセンサユニットＳ_ｉの中心で計測される加速度ベクトルの、グローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分を検出する（ステップＳ１３２）。

ここで、加速度検出部９０で、それぞれのセンサユニットＳ_ｉの中心で計測される加速度ベクトルの、グローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分を検出する手法について説明する。動状態では、静状態で鉛直方向に発生する重力加速度に、挿入部１１の移動による成分が加わるため、それぞれのセンサユニットＳ_ｉの中心で計測される加速度ベクトルのグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分を、

とする。また、それぞれのセンサユニットＳ_ｉの加速度センサＡ_ｉにより計測された加速度ベクトルのローカル座標系Ｃ_ｉのＸ_ｉ軸方向成分、Ｙ_ｉ軸方向成分、Ｚ_ｉ軸方向成分を、

とする。

また、本変形例では、内視鏡１０の挿入部１１が平行移動している場合を考えるため、近接して配置される複数のセンサユニットＳ_ｉにおいて、グローバル座標系Ｃでの加速度ベクトルは、同一であると仮定する。実際の内視鏡１０では、それぞれのセンサユニットＳ_ｉの間のセンサ間寸法ｌは非常に小さいため、このような仮定は可能である。このように仮定することにより、例えばセンサユニットＳ_１で計測される加速度ベクトルとセンサユニットＳ_２で計測される加速度ベクトルとの間で、

となる。そして、式（１９．１）、式（１９．２）からそれぞれ、

の関係が成り立つ。

ここで、センサユニットＳ_１では、グローバル座標系Ｃとローカル座標系Ｃ_１の関係から、式（１）の回転行列を用いて、

となる。同様に、センサユニットＳ_２では、グローバル座標系Ｃとローカル座標系Ｃ_２の関係から、式（１）の回転行列を用いて、

となる。式（２０．１）の関係を利用して、式（２１），（２２）を解くことにより、センサユニットＳ_１，Ｓ_２で計測された重力加速度以外の加速度ベクトルの、グローバル座標系ＣのＸ軸方向成分ａ_{ｔｈ１＿ｘ}（ａ_{ｔｈ２＿Ｘ}）、Ｙ軸方向成分ａ_{ｔｈ１＿Ｙ}（ａ_{ｔｈ２＿Ｙ}）、Ｚ軸方向成分ａ_{ｔｈ１＿Ｚ}（ａ_{ｔｈ２＿Ｚ}）が求まる。式（１３）で示す地磁気センサＢ_ｉにより計測される地磁気ベクトルのグローバル座標系Ｃのそれぞれの軸方向成分と、式（９）で示す地磁気ベクトルのローカル座標系Ｃ_ｉのそれぞれの軸方向成分との関係は、式（１）の回転行列を用いて、

となる。式（２３）においてｉ＝１(又は２)を代入した地磁気の関係式を含めて式（２０．１）（２１）（２２）を解く場合、方程式１２個に対して、未知数が姿勢角６個に加え加速度が６個で合計１２個である。その結果、センサユニットＳ_１，Ｓ_２で計測された重力加速度以外の加速度ベクトルの、グローバル座標系ＣのＸ軸方向成分ａ_{ｔｈ１＿ｘ}（ａ_{ｔｈ２＿Ｘ}）、Ｙ軸方向成分ａ_{ｔｈ１＿Ｙ}（ａ_{ｔｈ２＿Ｙ}）、Ｚ軸方向成分ａ_{ｔｈ１＿Ｚ}（ａ_{ｔｈ２＿Ｚ}）を数値演算的に解くことが可能である。その他のセンサユニットＳ_ｉについても同様にして、それぞれのセンサユニットＳ_ｉで計測された重力加速度以外の加速度ベクトルの、グローバル座標系ＣのＸ軸方向成分ａ_{ｔｈｉ＿ｘ}、Ｙ軸方向成分ａ_{ｔｈｉ＿Ｙ}、Ｚ軸方向成分ａ_{ｔｈｉ＿Ｚ}が求まる。

加速度検出部９０には、変位算出部９２が接続されている。変位算出部９２は、加速度検出部９０で検出された重力加速度以外の加速度ベクトルに基づいて、前回の静状態からのそれぞれのセンサユニットＳ_ｉの変位を算出する（ステップＳ１３３）。前回の静状態からのそれぞれのセンサユニットＳ_ｉの変位のグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分は、それぞれのセンサユニットＳ_ｉで計測された重力加速度以外の加速度ベクトルの、グローバル座標系ＣのＸ軸方向成分ａ_{ｔｈｉ＿ｘ}、Ｙ軸方向成分ａ_{ｔｈｉ＿Ｙ}、Ｚ軸方向成分ａ_{ｔｈｉ＿Ｚ}をそれぞれ２重積分することにより求まる。また、変位算出部９２では、それぞれのセンサユニットＳ_ｉの速度ベクトルのグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分も算出される。それぞれのセンサユニットＳ_ｉの速度ベクトルのグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分は、それぞれのセンサユニットＳ_ｉで計測された重力加速度以外の加速度ベクトルの、グローバル座標系ＣのＸ軸方向成分ａ_{ｔｈｉ＿ｘ}、Ｙ軸方向成分ａ_{ｔｈｉ＿Ｙ}、Ｚ軸方向成分ａ_{ｔｈｉ＿Ｚ}をそれぞれ１回だけ積分することにより求まる。

変位算出部９２は、移動状態検出部９４が接続されている。移動状態検出部９４は、曲線形状検出部５０に接続されている。図１７に示すように、動状態での挿入部１１の形状を検出する際には、前述した手法（図４のステップＳ１０１〜Ｓ１０６）により前回の静状態での挿入部１１の検出曲線形状６５が検出されている。移動状態検出部９４には、前回の静状態での挿入部１１の検出曲線形状６５が、曲線形状検出部５０から入力される。また、移動状態検出部９４には、変位算出部９２から前回の静状態からのそれぞれのセンサユニットＳ_ｉの変位のグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分の算出結果が入力される。移動状態検出部９４は、前回の静状態での挿入部１１の検出曲線形状６５、及び、前回の静状態からのそれぞれのセンサユニットＳ_ｉの変位に基づいて、前回の静状態からの挿入部１１の移動状態を検出する（ステップＳ１３４）。これにより、動状態での内視鏡１０の挿入部１１の形状が検出される。

移動状態検出部９４には、描画部４５が接続されている。移動状態検出部９４により検出されたグローバル座標系Ｃにおける挿入部１１の動状態での検出形状は、描画部４５により描画される。術者は、描画部４５により描画された動状態での検出形状を表示部４７で確認可能となっている。

また、変位算出部９２には、滞留状態検出部９６が接続されている。滞留状態検出部９６には、警告部９８が接続されている。滞留状態検出部９６では、それぞれのセンサユニットＳ_ｉの加速度ベクトル、速度ベクトル、変位のグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分に基づいて、挿入部１１の滞留状態を検出する（ステップＳ１３５）。すなわち、例えば内視鏡１０によりＳ状結腸の観察を行う際に、挿入部１１がＳトップ等で滞留している状態を検出する。これにより、操作部１２での操作が挿入部１１に伝達されているか、術者は確認可能となる。挿入部１１が滞留している場合は（ステップＳ１３５−Ｙｅｓ）、警告部９８により警告表示される（ステップＳ１３６）。挿入部１１が滞留していない場合は（ステップＳ１３５−Ｎｏ）、警告表示は行われずステップＳ１０７（図４参照）に進む。

以上のように、本変形例の内視鏡形状検出装置１では、加速度検出部９０でそれぞれのセンサユニットＳ_ｉの中心で計測される加速度ベクトルの、グローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分を検出される。そして、変位算出部９２では、それぞれのセンサユニットＳ_ｉの変位及び速度ベクトルの、グローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分も算出される。算出された加速度ベクトル、速度ベクトル、変位及び前回の静状態での検出曲線形状６５から、移動状態検出部９４が動状態での挿入部１１の形状を検出する。また、算出された加速度ベクトル、速度ベクトル、変位に基づいて、滞留状態検出部９６が挿入部１１の滞留状態を検出する。そして、滞留状態が検出された場合は、警告部９８により警告表示が行われる。このような構成にすることにより、操作部１２での操作が挿入部１１に伝達されているか、術者は確認可能となる。これにより、術者は内視鏡１０を操作する際、より適切な判断を行うことができる。

（その他の変形例）
なお、第１の実施形態では、まず、曲線補間部５１がすべてのリンクＴ_ｊについて曲線補間を行い、円弧Ｌ_ｊを形成している。そして、円弧位置補正部５３がそれぞれの円弧Ｌ_ｊを隣接する円弧Ｌ_ｊ−１，Ｌ_ｊ＋１と境界が連続する状態に平行移動し、円弧Ｌ_ｊの位置を補正している。一方、第２の実施形態では、補間順次実施部７１により、それぞれのリンクＴ_ｊごとに基端側のリンクＴ_ｊから順に曲線補間が行われる。そして、１つのリンクＴ_ｊについて曲線補間が行われるたびに、補間未完了部８５を補間完了部８３と連続する状態に平行移動して、補間未完了部８５の位置を補正する。すなわち、第１の実施形態の曲線形状検出部５０で検出される検出曲線形状６５と、第２の実施形態の曲線形状検出部７０で検出される検出曲線形状８１とは同一の曲線形状である。以上のことから、本発明は、回路構成等が複雑になるが、第１の実施形態と第２の実施形態とを組み合わせた曲線形状検出部を備えてもよい。この曲線形状検出部では、例えば、まず、リンクＴ_ｋまでは第１の実施形態と同様に曲線補間を行い、円弧Ｌ_１〜Ｌ_ｋ形成された後に円弧Ｌ_１〜Ｌ_ｋの位置補正を行う。そして、リンクＴ_ｋ＋１からは、第２の実施形態と同様に１つのリンクＴ_ｊについて曲線補間が行われるたびに、補間未完了部８５の位置補正を行う。

また、上述の実施形態では、それぞれのローカル座標系Ｃ_ｉは、Ｙ_ｉ軸方向がセンサユニットＳ_ｉの中心での長手方向と一致している座標系である。しかし、本発明では、それぞれのローカル座標系Ｃ_ｉは、センサユニットＳ_ｉの中心を原点とし、Ｘ_ｉ軸、Ｙ_ｉ軸、Ｚ_ｉ軸のいずれか１つの軸がセンサユニットＳ_ｉの中心での長手方向と軸方向が一致する長手方向軸である座標系であればよい。ただし、Ｘ_ｉ軸が長手方向軸である場合は、式（１６．１），式（１６．２）でｅ_ｙｋ−１代わりに、

を用いる。同様に、Ｚ_ｉ軸が長手方向軸である場合は、式（１６．１），式（１６．２）でｅ_ｙｋ−１代わりに、

を用いる。ここで、ｅ_ｘｋ−１はローカル座標系Ｃ_ｋ−１の原点での長手方向であるＸ_ｋ−１軸方向の単位ベクトルであり、ｅ_ｚｋ−１はローカル座標系Ｃ_ｋ−１の原点での長手方向であるＺ_ｋ−１軸方向の単位ベクトルである。

また、上述の実施形態では、グローバル座標系Ｃは、最も基端側のセンサユニットＳ_０の中心を原点とし、Ｚ軸が鉛直方向と一致し、Ｘ軸及びＹ軸が水平面上に配置される座標系である。しかし、本発明では、Ｘ軸、Ｙ軸、Ｚ軸のいずれか１つの軸が鉛直方向と軸方向が一致する鉛直方向軸であり、鉛直方向軸以外の２つの軸が水平面上に配置される水平方向軸である座標系であればよい。これにより、加速度センサＡ_ｉにより計測される重力加速度及び地磁気センサＢ_ｉにより計測される地磁気に基づいて、姿勢検出部３０はそれぞれのセンサユニットＳ_ｉの姿勢を検出することが可能である。ただし、Ｘ軸が鉛直方向軸の場合は、式（２）で示される重力加速度ベクトルのグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分は、

となる。この場合、第１の実施形態では、姿勢角α_ｉ，β_ｉ，γ_ｉはヨー角γ_ｉ、ピッチ角α_ｉ、ロール角β_ｉの順で回転する（Ｚ，Ｘ，Ｙ）型としたが、姿勢角α_ｉ，β_ｉ，γ_ｉの回転する順序を変えて、式（１）の回転行列とは別の回転行列を用いる。これにより、加速度センサＡ_ｉで計測された加速度データに基づいて、第１の角度算出部３４で水平方向軸であるＹ軸，Ｚ軸回りの姿勢角β_ｉ，γ_ｉが算出される。また、地磁気センサＢ_ｉで計測された地磁気データに基づいて、第２の角度算出部３６で鉛直方向軸であるＸ軸回りの姿勢角α_ｉが算出される。Ｙ軸が鉛直方向軸の場合についても同様であり、加速度センサＡ_ｉで計測された加速度データに基づいて、第１の角度算出部３４で水平方向軸であるＸ軸，Ｚ軸回りの姿勢角α_ｉ，γ_ｉが算出される。また、地磁気センサＢ_ｉで計測された地磁気データに基づいて、第２の角度算出部３６で鉛直方向軸であるＹ軸回りの姿勢角β_ｉが算出される。

さらに、上述の実施形態では、グローバル座標系Ｃは、最も基端側のセンサユニットＳ_０の中心を原点としているが、最も先端側のセンサユニットＳ_Ｎの中心を原点としてもよい。この場合、リンク形成部４１は、式（１６．１），式（１６．２）を用いて、リンクＴ_ｊの先端側（グローバル座標系Ｃの原点から近い側）のセンサユニットＳ_ｊがグローバル座標系Ｃの原点に位置する場合の、リンクＴ_ｊの基端側（グローバル座標系Ｃの原点から遠い側）のセンサユニットＳ_ｊ−１の座標Ｐ´_ｊ（ｌ_ｘｊ，ｌ_ｙｊ，ｌ_ｚｊ）を求める。そして、グローバル座標系Ｃの原点と座標Ｐ´_ｊ（ｌ_ｘｊ，ｌ_ｙｊ，ｌ_ｚｊ）とを直線状に結ぶことにより、リンクＴ_ｊが形成される。

また、この場合、曲線形状検出部５０の曲線補間部５１によりリンク（補間対象リンク）Ｔ_ｊの曲線補間を行う際は、法線形成部５５が、リンクＴ_ｊの先端（グローバル座標Ｃの原点から近い側の端）である第１の端点を通り、リンクＴ_ｊに垂直な第１の補間法線Ｎ_ｊと、リンクＴ_ｊの基端（グローバル座標Ｃの原点から遠い側の端）である第２の端点を通り、リンクＴ_ｊの基端側（グローバル座標Ｃの原点から遠い側）に隣接するリンク（補間対象隣接リンク）Ｔ_ｊー１に垂直な第２の補間法線Ｎ_ｊ−１とを形成する。そして、中心決定部５７が、第１の補間法線Ｎ_ｊと第２の補間法線Ｎ_ｊ−１との交点Ｏ_ｊ、又は、第１の補間法線Ｎ_ｊと第２の補間法線Ｎ_ｊ−１との距離が最小となる２点の中間点Ｏ_ｊを算出し、交点Ｏ_ｊ又は中間点Ｏ_ｊを円弧Ｌ_ｊの中心Ｏ_ｊとして決定する。そして、円弧形成部５９が、半径Ｒ_ｊが中心Ｏ_ｊとリンク（補間対象リンク）Ｔ_ｊの先端との間の距離と同一で、かつ、リンクＴ_ｊの先端を始点とする弧の長さがセンサ間寸法ｌに等しい円弧Ｌ_ｊ形成する。

さらに、この場合、曲線形状検出部７０の補間順次実施部７１は、それぞれのリンクＴ_ｊごとに先端側（グローバル座標系Ｃの原点から近い側）のリンクＴ_ｊから順に曲線補間を行い、円弧Ｌ_ｊを形成する。

以上、本発明の実施形態について説明したが、本発明は上記の実施形態に限定されるものではなく、本発明の要旨を逸脱しない範囲で種々の変形ができることは勿論である。

Claims (16)

1. 長手方向に互いに所定のセンサ間寸法だけ離れて複数のセンサユニットが配置される挿入部を備える内視鏡と、
   前記センサユニットでの計測データに基づいて、それぞれの前記センサユニットの姿勢を検出する姿勢検出部と、
   前記姿勢検出部で検出されたそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間の形状を寸法が前記センサ間寸法に等しい直線状のリンクと仮定して、前記内視鏡の前記挿入部の検出線形形状を検出する線形形状検出部と、
   それぞれの前記センサユニットの間の形状を弧長が前記センサ間寸法に等しい円弧と仮定して、前記線形形状検出部で検出された前記検出線形形状を曲線補間し、検出曲線形状検出する曲線形状検出部と、
   を具備する内視鏡形状検出装置。
2. 前記曲線形状検出部は、
   それぞれの前記リンクごとに前記曲線補間を行い、ぞれぞれの前記センサユニットの間の前記円弧を形成する曲線補間部と、
   すべての前記リンクについて前記曲線補間を行う状態に前記曲線補間部を制御する補間制御部と、
   を備える請求項１の内視鏡形状検出装置。
3. 前記曲線補間部は、
   最も基端側又は最も先端側の前記センサユニットの中心を原点とするグローバル座標系において、前記曲線補間部により前記曲線補間を行う対象の前記リンクである補間対象リンクの前記グローバル座標の前記原点から近い側の端である第１の端点を通り、前記補間対象リンクに垂直な第１の補間法線と、前記補間対象リンクの前記グローバル座標の前記原点から遠い側の端である第２の端点を通り、前記補間対象リンクの前記グローバル座標の前記原点から遠い側に隣接する前記リンクである補間対象隣接リンクに垂直な第２の補間法線と、を形成する法線形成部と、
   前記法線形成部で形成された前記第１の補間法線と前記第２の補間法線との交点、又は、前記第１の補間法線と前記第２の補間法線との距離が最小となる２点の中間点を算出し、前記交点又は前記中間点を前記円弧の中心として決定する中心決定部と、
   半径が前記中心と前記補間対象リンクの前記第１の端点との間の距離と同一で、かつ、前記補間対象リンクの前記第１の端点を始点として前記円弧を形成する円弧形成部と、
   を備える請求項２の内視鏡形状検出装置。
4. 前記曲線形状検出部は、前記曲線補間部により形成されたそれぞれの前記円弧を、隣接する前記円弧との円弧境界が連続する状態に平行移動し、前記円弧の位置を補正する円弧位置補正部を備える請求項２の内視鏡形状検出装置。
5. 前記曲線補間部は、最も基端側又は最も先端側の前記センサユニットの中心を原点とするグローバル座標系において、前記グローバル座標系の前記原点から近い側の前記リンクから順に前記曲線補間を行い、前記円弧を形成する補間順次実施部であり、
   前記曲線形状検出部は、前記補間順次実施部により１つの前記リンクの前記曲線補間が行われるたびに、曲線補間が行われていない前記リンクである未補間リンクから構成される補間未完了部を前記曲線補間により形成される前記円弧から構成される補間完了部と境界が連続する状態に平行移動し、前記補間未完了部の位置を補正する未補間リンク位置補正部を備える請求項２の内視鏡形状検出装置。
6. それぞれの前記センサユニットは、
   前記センサユニットの中心を原点とし、Ｘ軸、Ｙ軸、Ｚ軸のいずれか１つの軸が前記センサユニットの前記中心での前記内視鏡の長手方向と軸方向が一致する長手方向軸であるローカル座標系において、前記ローカル座標系の前記原点での加速度の３つの軸方向成分を計測する加速度センサと、
   前記ローカル座標系において、前記ローカル座標系の前記原点での地磁気の３つの軸方向成分を計測する地磁気センサと、
   を備える請求項１の内視鏡形状検出装置。
7. 前記姿勢検出部は、最も基端側又は最も先端側の前記センサユニットの中心を原点とし、Ｘ軸、Ｙ軸、Ｚ軸のいずれか１つの軸が鉛直方向と軸方向が一致する鉛直方向軸であり、前記鉛直方向軸以外の２つの軸が水平面上に配置される水平方向軸であるグローバル座標系を基準として、それぞれの前記センサユニットの前記ローカル座標系の前記鉛直方向軸回り及びそれぞれの前記水平方向軸回りの回転角である３つの姿勢角を算出する姿勢角算出部を備える請求項６の内視鏡形状検出装置。
8. 前記姿勢角算出部は、
   前記加速度センサで計測された加速度データに基づいて、それぞれの前記センサユニットの前記ローカル座標系の前記グローバル座標系からのそれぞれの前記水平方向軸回りの２つの前記姿勢角を算出する第１の角度算出部と、
   前記地磁気センサで計測された地磁気データに基づいて、それぞれの前記センサユニットの前記ローカル座標系の前記グローバル座標系からの前記鉛直方向軸回りの前記姿勢角を算出する第２の角度算出部と、
   を備える請求項７の内視鏡形状検出装置。
9. 内視鏡の挿入部に長手方向に互いに所定のセンサ間寸法だけ離れて配置される複数のセンサユニットが計測を行うことと、
   前記センサユニットでの計測データに基づいて、それぞれの前記センサユニットの姿勢を姿勢検出部が検出することと、
   前記姿勢検出部が検出したそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間の形状を寸法が前記センサ間寸法に等しい直線状のリンクと仮定して、前記内視鏡の前記挿入部の検出線形形状を線形形状検出部が検出することと、
   それぞれの前記センサユニットの間の形状を弧長が前記センサ間寸法に等しい円弧と仮定して、曲線形状検出部が、前記線形形状検出部が検出した前記検出線形形状を曲線補間し、検出曲線形状を形成することと、
   を具備する内視鏡の挿入部の形状検出方法。
10. 前記検出線形形状を曲線補間し、前記曲線形状検出部が検出曲線形状を形成することは、
    前記曲線形状検出部の曲線補間部が、それぞれの前記リンクごとに前記曲線補間を行い、ぞれぞれの前記センサユニットの間の前記円弧を形成することと、
    前記曲線補間部がすべての前記リンクについて前記曲線補間を行う状態に、前記曲線形状検出部の補間制御部が制御することと、
    を備える請求項９の内視鏡の挿入部の形状検出方法。
11. 前記曲線補間部がそれぞれの前記リンクごとに前記曲線補間を行うことは、
    最も基端側又は最も先端側の前記センサユニットの中心を原点とするグローバル座標系において、前記曲線補間部により前記曲線補間を行う対象の前記リンクである補間対象リンクの前記グローバル座標の前記原点から近い側の端である第１の端点を通り、前記補間対象リンクに垂直な第１の補間法線と、前記補間対象リンクの前記グローバル座標の前記原点から遠い側の端である第２の端点を通り、前記補間対象リンクの前記グローバル座標の前記原点から遠い側に隣接する前記リンクである補間対象隣接リンクに垂直な第２の補間法線と、を前記曲線補間部の法線形成部が形成することと、
    前記曲線補間部の中心決定部が、前記法線形成部が形成した前記第１の補間法線と前記第２の補間法線との交点、又は、前記第１の補間法線と前記第２の補間法線との距離が最小となる２点の中間点を算出し、前記交点又は前記中間点を前記円弧の中心として決定することと、
    半径が前記中心と前記補間対象リンクの前記第１の端点との間の距離と同一で、かつ、前記補間対象リンクの第１の端点を始点として前記曲線補間部の円弧形成部が前記円弧を形成することと、
    を備える請求項１０の内視鏡の挿入部の形状検出方法。
12. 前記曲線形状検出部が前記検出線形形状を曲線補間し、検出曲線形状を形成することは、前記曲線形状検出部の円弧位置補正部が、前記曲線補間部がそれぞれの前記リンクごとに前記曲線補間を行うことにより形成したそれぞれの前記円弧を、隣接する前記円弧との円弧境界が連続する状態に平行移動し、前記円弧の位置を補正することを備える請求項１０の内視鏡の挿入部の形状検出方法。
13. 前記曲線補間部がそれぞれの前記リンクごとに前記曲線補間を行うことは、最も基端側又は最も先端側の前記センサユニットの中心を原点とするグローバル座標系において、前記曲線補間部となる補間順次実施部が、前記グローバル座標系の前記原点から近い側の前記リンクから順に前記曲線補間を行い、前記円弧を形成することを備え、
    前記曲線形状検出部が前記検出線形形状を曲線補間し、検出曲線形状を形成することは、前記曲線形状検出部の未補間リンク位置補正部が、前記補間順次実施部が１つの前記リンクの前記曲線補間が行うたびに、前記曲線補間が行われていない前記リンクである未補間リンクから構成される補間未完了部を前記曲線補間により形成される前記円弧から構成される補間完了部と境界が連続する状態に平行移動し、前記補間未完了部の位置を補正することを備える請求項１０の内視鏡の挿入部の形状検出方法。
14. それぞれの前記センサユニットが計測を行うことは、
    それぞれの前記センサユニットが備える加速度センサが、前記センサユニットの中心を原点とし、Ｘ軸、Ｙ軸、Ｚ軸のいずれか１つの軸が前記センサユニットの前記中心での前記内視鏡の長手方向と軸方向が一致する長手方向軸であるローカル座標系において、前記原点での加速度の３つの軸方向成分を計測することと、
    それぞれの前記センサユニットが備える地磁気センサが、前記ローカル座標系において、前記原点での地磁気の３つの軸方向成分を計測することと、
    を備える請求項９の内視鏡の挿入部の形状検出方法。
15. 前記姿勢検出部がそれぞれの前記センサユニットの姿勢を検出することは、最も基端側又は最も先端側の前記センサユニットの中心を原点とし、Ｘ軸、Ｙ軸、Ｚ軸のいずれか１つの軸が鉛直方向と軸方向が一致する鉛直方向軸であり、前記鉛直方向軸以外の２つの軸が水平面上に配置される水平方向軸であるグローバル座標系を基準として、それぞれの前記センサユニットの前記ローカル座標系の、前記鉛直方向軸回り及びそれぞれの前記水平方向軸回りの回転角である３つの姿勢角を前記姿勢検出部の姿勢角算出部が算出することを備える請求項１４の内視鏡の挿入部の形状検出方法。
16. 前記姿勢角算出部が前記鉛直方向軸回り及びそれぞれの前記水平方向軸回りの３つの姿勢角を算出することは、
    前記加速度センサが計測した加速度データに基づいて、それぞれの前記センサユニットの前記ローカル座標系の前記グローバル座標系からのそれぞれの前記水平方向軸回りの２つの前記姿勢角を前記姿勢角算出部の第１の角度算出部が算出することと、
    前記地磁気センサが計測した地磁気データに基づいて、それぞれの前記センサユニットの前記ローカル座標系の前記グローバル座標系からの前記鉛直方向軸回りの前記姿勢角を前記姿勢角算出部の第２の角度算出部が算出することと、
    を備える請求項１５の内視鏡の挿入部の形状検出方法。

Images