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

Abstract

内視鏡形状検出装置は、それぞれのセンサユニットの姿勢に基づいて、それぞれの前記センサユニットの間を寸法がセンサ間寸法に等しい直線状の仮リンクと仮定して、それぞれの前記センサユニットの仮位置を検出するセンサ仮位置検出部と、それぞれの前記センサユニットの前記仮位置に基づいて、それぞれの前記センサユニットの前記仮位置の間を仮円弧で曲線補間を行い、前記挿入部の仮曲線形状を検出する仮曲線形状検出部と、を備える。内視鏡形状検出装置は、それぞれの前記仮円弧の弧長と前記センサ間寸法との差の絶対値に基づいて、それぞれの前記センサユニットの位置を前記仮位置から最終位置まで修正するセンサ位置修正部と、それぞれの前記センサユニットの前記最終位置の間を最終円弧で曲線補間し、前記挿入部の最終曲線形状を検出する最終曲線形状検出部と、を備える。

Description

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

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

また、特許文献２には、内視鏡の挿入部に配置されたそれぞれのコイルの位置を交流磁場から検出し、検出されたそれぞれのコイルの位置の間を曲線補間する内視鏡形状検出装置が開示されている。この内視鏡形状検出装置では、検出されたそれぞれのコイルの位置をベジェ曲線又はスプライン曲線で結ぶことにより、曲線補間を行っている。曲線補間を行うことにより、内視鏡の挿入部の曲線形状が検出される。

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

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

上記特許文献２の内視鏡形状検出装置では、検出されたそれぞれのコイルの位置をベジェ曲線又はスプライン曲線で結ぶことにより、曲線補間を行っている。しかし、この内視鏡形状検出装置では、それぞれのコイルの間の長手方向の寸法であるコイル間寸法を考慮して曲線補間を行っていない。このため、体腔内への挿入時の挿入部の形状は、高い精度で検出されない。

例えば検出されたそれぞれのコイルの位置をスプライン曲線で結んで、挿入部の仮形状を検出した後に、コイル間寸法に基づいて、検出されたそれぞれのコイルの位置を順次に修正することも考えられる。この場合、１つのコイルの位置の修正が行われるたびに、位置修正後のコイルの位置をスプライン曲線で結び、コイル位置修正前の挿入部の検出形状を修正したコイル位置修正後の挿入部の修正形状が検出される。すなわち、１つのコイルの位置修正が行われるたびに、挿入部の曲線形状全体が修正される。ここで、コイル間寸法は、コイルとコイルの間の長手方向の寸法であり、挿入部の局所的なパラメータである。すなわち、コイル間寸法は、挿入部の形状に局所的に影響を与えるパラメータであり、挿入部全体の形状に影響を与えるパラメータではない。このため、１つのコイルの位置の修正により、位置修正されたコイルの近傍で局所的にセンサ位置修正前の挿入部の検出形状が修正されるのではなく、１つのコイルの位置の修正により、挿入部の曲線形状全体が修正される構成では、高い精度で挿入部の曲線形状の修正が行われない。したがって、最終的に挿入部の曲線形状が、高い精度で検出されない。また、スプライン曲線等の種類によっては１つのコイルの位置の修正により、挿入部の全体形状が修正されるため、処理が複雑になる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

となる。

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

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

とする。

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

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

となり、

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

式（７）、式（８）、式（１２）（又は式（１５））のｉをｋ−１で置換した式を用いて、ステップＳ１０２，Ｓ１０３でローカル座標系Ｃ_ｋ−１（つまり仮リンクＴ_０，ｋ）の姿勢角α_ｋ−１，β_ｋ−１，γ_ｋ−１が算出される。この姿勢角α_ｋ−１，β_ｋ−１，γ_ｋ−１及びそれぞれのセンサユニットＳ_ｉの間の長手方向についての間隔であるセンサ間寸法ｌを用いて、センサユニットＳ_ｋ−１からセンサユニットＳ_ｋへ向かうベクトルが求まる。ここで、センサユニットＳ_ｋ−１からセンサユニットＳ_ｋへ向かうベクトルは、

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

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

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

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

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

なお、仮リンクＴ_０，ｋ以外の仮リンクＴ_０，ｊについても、仮リンク移動部４２により同様に移動される。すなわち、仮リンクＴ_０，ｊの移動を行う際は、原点から仮リンク移動完了部７３の先端（グローバル座標系Ｃの原点から遠い側の端）までの移動量だけ、仮リンク移動部４２は仮リンクＴ_０，ｊを平行移動する。これにより、仮リンクＴ_０，ｊと仮リンクＴ_０，ｊの基端側（グローバル座標系Ｃの原点から近い側）に隣接する仮リンクＴ_{０，ｊ−１}との仮リンク境界が連続する状態となる。ただし、仮リンクＴ_０，１については、仮リンクＴ_０，１の基端がグローバル座標系Ｃの原点であるため、移動は行われない。すべての仮リンクＴ_０，ｊの移動が完了することにより、図６に示すように、それぞれのセンサユニットＳ_ｉの間を直線状の仮リンクＴ_０，ｊと仮定した仮線形形状７１が形成される。仮線形形状７１でのそれぞれのセンサユニットＳ_ｉの位置を、それぞれのセンサユニットＳ_ｉの仮位置Ｐ_０，ｉとして検出する（ステップＳ１０４）。

図３に示すように、センサ仮位置検出部４０は、仮曲線形状検出部５０に接続されている。仮曲線形状検出部５０は、センサ仮位置検出部４０により検出されたそれぞれのセンサユニットＳ_ｉの仮位置Ｐ_０，ｉの間を仮円弧Ｌ_０，ｊとして曲線補間を行い、仮曲線形状７５を検出する。図８は、仮曲線形状検出部５０により検出された内視鏡１０の挿入部１１の仮曲線形状７５をグローバル座標ＣのＺ軸の負から正の方向に向かって視た図である。図８に示すように、仮曲線形状検出部５０により、図８の点線で示す仮線形形状７１のそれぞれのセンサユニットＳ_ｉの仮位置Ｐ_０，ｉの間が曲線補間される。これにより、それぞれの仮円弧Ｌ_０，ｊが形成され、図８の実線で示す仮曲線形状７５が検出される。

仮曲線形状検出部５０は、それぞれのセンサユニットＳ_ｉの仮位置Ｐ_０，ｉでの単位接ベクトルを算出する単位接ベクトル算出部５１と、単位接ベクトル算出部５１により算出された単位接ベクトルに基づいて、それぞれのセンサユニットＳ_ｉの仮位置Ｐ_０，ｉの間での単位接ベクトルの変化率を算出する変化率算出部５２と、単位接ベクトル算出部５１により算出された単位接ベクトル及び変化率算出部５２で算出された変化率に基づいて、それぞれのセンサユニットＳ_ｉの仮位置Ｐ_０，ｉの間の仮円弧Ｌ_０，ｊを形成する仮円弧形成部５３と、を備える。すべてのセンサユニットＳ_ｉの仮位置Ｐ_０，ｉの間で曲線補間が行われ、すべての仮円弧Ｌ_０，ｊが形成されることにより、仮曲線形状７５が検出される。

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

仮曲線形状７５を検出する際、仮曲線形状検出部５０は、それぞれのセンサユニットＳ_ｉの仮位置Ｐ_０，ｉの間ごとに順次に曲線補間を行い、仮円弧Ｌ_０，ｊを形成する。ここで、仮曲線形状検出部５０により、それぞれのセンサユニットＳ_ｉの仮位置Ｐ_０，ｉの間の曲線補間を行う方法について説明する。ここでは、基端側からｋ番目のセンサユニットＳ_ｋ−１と基端側から（ｋ＋１）番目のセンサユニットＳ_ｋとの間の曲線補間について説明する。すなわち、仮位置Ｐ_{０，ｋ−１}と仮位置Ｐ_０，ｋとの間が、補間対象である補間対象仮位置間となる。

図９は、仮曲線形状検出部５０により仮曲線形状７５を検出する方法を示すフローチャートである。図１０は、仮曲線形状検出部５０での処理を説明する図である。図９及び図１０に示すように、仮曲線形状検出部５０によりセンサユニットＳ_ｋ−１の仮位置Ｐ_{０，ｋ−１}とセンサユニットＳ_ｋの仮位置Ｐ_０，ｋとの間である補間対象のセンサ間の曲線補間を行う際は、まず、単位接ベクトル算出部５１が、センサユニットＳ_ｋ−１の仮位置Ｐ_{０，ｋ−１}での単位接ベクトル及びセンサユニットＳ_ｋの仮位置Ｐ_０，ｋでの単位接ベクトルを算出する（ステップＳ１１１）。センサユニットＳ_ｋ−１の仮位置Ｐ_{０，ｋ−１}での単位接ベクトルは、

となる。すなわち、仮位置Ｐ_{０，ｋ−１}での単位接ベクトルは、仮位置Ｐ_{０，ｋ−１}から仮位置Ｐ_０，ｋまでのベクトルの単位ベクトルである。センサユニットＳ_ｋの仮位置Ｐ_０，ｋでの単位接ベクトルは、式（１７．１）、式（１７．２）、式（１７．３）のｋをｋ＋１で置換した式を用いて求められる。すなわち、仮位置Ｐ_０，ｋでの単位接ベクトルは、仮位置Ｐ_０，ｋから仮位置Ｐ_{０，ｋ＋１}までのベクトルの単位ベクトルである。

そして、単位接ベクトル算出部５１により算出された単位接ベクトルに基づいて、変化率算出部５２がセンサユニットＳ_ｋ−１の仮位置Ｐ_{０，ｋ−１}とセンサユニットＳ_ｋの仮位置Ｐ_０，ｋとの間での単位接ベクトルの変化率を算出する（ステップＳ１１２）。まず、仮位置Ｐ_{０，ｋ−１}と仮位置Ｐ_０，ｋとの間での単位接ベクトルの変化率ベクトルが、

で求められる。そして、式（１８．１）、式（１８．２）で求められた変化率ベクトルの大きさが仮位置Ｐ_{０，ｋ−１}と仮位置Ｐ_０，ｋとの間での単位接ベクトルの変化率となる。

そして、単位接ベクトル算出部５１により算出された単位接ベクトル及び変化率算出部５２で算出された変化率に基づいて、仮円弧形成部５３が、センサユニットＳ_ｋ−１の仮位置Ｐ_{０，ｋ−１}とセンサユニットＳ_ｋの仮位置Ｐ_０，ｋとの間の仮円弧Ｌ_０，ｋを形成する（ステップＳ１１３）。ここで、ステップＳ１１２で算出した仮位置Ｐ_{０，ｋ−１}と仮位置Ｐ_０，ｋとの間での単位接ベクトルの変化率は、仮円弧Ｌ_０，ｋの曲率１／Ｒ_０，ｋとなる。仮円弧Ｌ_０，ｋの半径Ｒ_０，ｋは曲率１／Ｒ_０，ｋの逆数であるため、仮円弧Ｌ_０，ｋの半径Ｒ_０，ｋは、

となる。また、仮円弧Ｌ_０，ｋの中心Ｏ_０，ｋのグローバル座標Ｃでの位置が、

により求められる。さらに、仮位置Ｐ_{０，ｋ−１}での単位接ベクトル及び仮位置Ｐ_０，ｋでの単位接ベクトルを用いて、仮円弧Ｌ_０，ｋの中心角φ_０，ｋが、

により求められる。これらのパラメータを用いて、仮位置Ｐ_{０，ｋ−１}と仮位置Ｐ_０，ｋとの間の仮円弧Ｌ_０，ｋが形成される。

なお、仮円弧Ｌ_０，ｋ以外の仮円弧Ｌ_０，ｊについても、仮位置Ｐ_{０，ｊ−１}と仮位置Ｐ_０，ｊとの間を同様に曲線補間することにより、形成される。すなわち、仮円弧Ｌ_０，ｊを形成する際は、まず、単位接ベクトル算出部５１が、補間対象仮位置間の基端側（グローバル座標系Ｃの原点に近い側）に位置するセンサユニットＳ_ｊ−１の仮位置Ｐ_{０，ｊ−１}での単位接ベクトル及び補間対象仮位置間の先端側（グローバル座標系Ｃの原点から遠い側）に位置するセンサユニットＳ_ｊの仮位置Ｐ_０，ｊでの単位接ベクトルを算出する。センサユニットＳ_ｊ−１の仮位置Ｐ_{０，ｊ−１}での単位接ベクトルは、式（１７．１）、式（１７．２）、式（１７．３）のｋをｊで置換した式を用いて求められ、仮位置Ｐ_{０，ｊ−１}から仮位置Ｐ_０，ｊまでのベクトルの単位ベクトルである。また、センサユニットＳ_ｊの仮位置Ｐ_０，ｊでの単位接ベクトルは、式（１７．１）、式（１７．２）、式（１７．３）のｋをｊ＋１で置換した式を用いて求められ、仮位置Ｐ_０，ｊから仮位置Ｐ_{０，ｊ＋１}までのベクトルの単位ベクトルである。これにより、それぞれのセンサユニットＳ_ｉの仮位置Ｐ_０，ｉでの単位接ベクトルが算出される（ステップＳ１１１）。

そして、単位接ベクトル算出部５１により算出された単位接ベクトルに基づいて、変化率算出部５２がセンサユニットＳ_ｊ−１の仮位置Ｐ_{０，ｊ−１}とセンサユニットＳ_ｊの仮位置Ｐ_０，ｊとの間である補間対象仮位置間での単位接ベクトルの変化率を算出する。仮位置Ｐ_{０，ｊ−１}と仮位置Ｐ_０，ｊとの間での単位接ベクトルの変化率は、式（１８．１）、式（１８．２）のｋをｊで置換した式を用いて求められる。これにより、それぞれのセンサユニットＳ_ｉの仮位置Ｐ_０，ｉの間での単位接ベクトルの変化率が算出される（ステップＳ１１２）。

そして、単位接ベクトル算出部５１により算出された単位接ベクトル及び変化率算出部５２で算出された変化率に基づいて、仮円弧形成部５３が、センサユニットＳ_ｊ−１の仮位置Ｐ_{０，ｊ−１}とセンサユニットＳ_ｊの仮位置Ｐ_０，ｊとの間である補間対象仮位置間の仮円弧Ｌ_０，ｊを形成する。ここで、式（１９）、式（２０）、式（２１）のｋをｊで置換した式を用いて、仮円弧Ｌ_０，ｊの半径Ｒ_０，ｊ、中心Ｏ_０，ｊ、中心角φ_０，ｊが求められる。これにより、それぞれのセンサユニットＳ_ｉの仮位置Ｐ_０，ｉの間の仮円弧Ｌ_０，ｊが形成される（ステップＳ１１３）。すべてのセンサユニットＳ_ｉの仮位置Ｐ_０，ｉの間で曲線補間が行われ、すべての仮円弧Ｌ_０，ｊが形成されることにより、仮曲線形状７５が検出される。

図３に示すように、仮曲線形状検出部５０は、それぞれのセンサユニットＳ_ｉの位置を修正するセンサ位置修正部５５に接続されている。センサ位置修正部５５は、仮曲線形状検出部５０により検出された仮曲線形状７５のそれぞれの仮円弧Ｌ_０，ｊの弧長Ｒ_０，ｊφ_０，ｊとセンサ間寸法ｌとの差の絶対値に基づいて、それぞれのセンサユニットＳ_ｉの位置を修正する。ここで、仮円弧Ｌ_０，ｊの弧長Ｒ_０，ｊφ_０，ｊとセンサ間寸法ｌとの差の絶対値をエネルギーＥ_０，ｊとする。図１１は、センサ位置修正部５５により修正されたそれぞれのセンサユニットＳ_ｉの位置をグローバル座標ＣのＺ軸の負から正の方向に向かって視た図である。図１１に示すように、センサ位置修正部５５により、それぞれのセンサユニットＳ_ｉは、仮位置Ｐ_０，ｉから最終位置Ｐ_Ｆ，ｉまで修正される。

センサ位置修正部５５は、基端側（グローバル座標系Ｃの原点に近い側）のセンサユニットＳ_ｉから順次に位置補正を行うセンサ位置順次補正部５７を備える。センサ位置順次補正部５７によりそれぞれのセンサユニットＳ_ｉの位置補正を行う前の状態では、それぞれのセンサユニットＳ_ｉは、仮位置Ｐ_０，ｉから補正前位置Ｑ_０，ｉに移動されている（詳細については、後述する。）。センサ位置順次補正部５７は、それぞれのセンサユニットＳ_ｉを補正前位置Ｑ_０，ｉから最終位置Ｐ_Ｆ，ｉへ位置補正する。また、センサ位置修正部５５は、センサ位置順次補正部５７により１つのセンサユニットＳ_ｉの位置補正が行われるたびに、位置補正が完了していないセンサユニットＳ_ｉである未補正センサ群７７を平行移動させる未補正センサ群移動部５９を備える。センサ位置順次補正部５７により、すべてのセンサユニットＳ_ｉの補正前位置Ｑ_０，ｉから最終位置Ｐ_Ｆ，ｉへの位置補正が行われる。ここで、補正前位置Ｑ_０，ｉは、センサ位置順次補正部５７により位置補正が行われる直前のそれぞれのセンサユニットＳ_ｉの位置である。未補正センサ群移動部５９は、センサ位置順次補正部５７による直前の位置補正で補正対象だったセンサユニットＳ_ｉである直前補正対象センサ７９の補正前位置Ｑ_０，ｉから最終位置Ｐ_Ｆ，ｉへの補正量だけ、未補正センサ群７７を平行移動させる。これにより、未補正センサ群７７は、平行移動前位置Ｕ_{ａ−１，ｉ}（ａ＝１，２，…）から平行移動後位置Ｕ_ａ，ｉまで平行移動する。ここで、ａは、未補正センサ群移動部５９によりそれぞれのセンサユニットＳ_ｉが平行移動した回数を示している。それぞれのセンサユニットＳ_ｉの未補正センサ群移動部５９による１回目の平行移動の平行移動前位置Ｕ_０，ｉは、仮位置Ｐ_０，ｉと一致する。それぞれのセンサユニットＳ_ｉは、未補正センサ群移動部５９により（ｉ−１）回だけ平行移動する。それぞれのセンサユニットＳ_ｉの未補正センサ群移動部５９による（ｉ−１）回目の平行移動の平行移動後位置Ｕ_{ｉ−１，ｉ}は、補正前位置Ｑ_０，ｉと一致する。以上のように、センサ位置順次補正部５７での位置補正及び未補正センサ群移動部５９での平行移動により、それぞれのセンサユニットＳ_ｉが、仮位置Ｐ_０，ｉから最終位置Ｐ_Ｆ，ｉへ位置修正される。

センサ位置順次補正部５７は、位置補正の対象である補正対象センサ８１を位置補正前の補正前位置Ｑ_０，ｉから１回以上移動させるセンサ移動部６１を備える。センサ移動部６１での１回の移動により、補正対象センサ８１は移動前位置Ｑ_{ｔ−１，ｉ}（ｔ＝１，２，…）から移動後位置Ｑ_ｔ，ｉまで移動する。ここで、ｔはセンサ移動部６１によりぞれぞれのセンサユニットＳ_ｉが移動した回数を示している。また、センサ位置順次補正部５７は、センサ移動部６１により補正対象センサ８１の移動が１回行われるたびに、補正対象センサ８１に対して基端側（グローバル座標系Ｃの原点に近い側）に隣設されるセンサユニットＳ_ｉである近位側隣設センサ８２の最終位置Ｐ_Ｆ，ｉと補正対象センサ８１の移動後位置Ｑ_ｔ，ｉとの間を曲線補間する移動後円弧形成部６２とを備える。移動後円弧形成部６２で近位側隣設センサ８２の最終位置Ｐ_Ｆ，ｉと補正対象センサ８１の移動後位置Ｑ_ｔ，ｉとの間を曲線補間することにより、移動後円弧Ｌ_ｔ，ｊが形成される。

センサ位置順次補正部５７は、移動後円弧形成部６２により形成される移動後円弧Ｌ_ｔ，ｊの弧長Ｒ_ｔ，ｊφ_ｔ，ｊとセンサ間寸法ｌとの差の絶対値が所定の閾値以下になるまで、補正対象センサ８１の移動前位置Ｑ_{ｔ−１，ｉ}から移動後位置Ｑ_ｔ，ｉへの移動を繰り返し行う状態にセンサ移動部６１を制御する移動制御部６３を備える。ここで、移動後円弧Ｌ_ｔ，ｊの弧長Ｒ_ｔ，ｊφ_ｔ，ｊとセンサ間寸法ｌとの差の絶対値をエネルギーＥ_ｔ，ｊとする。また、センサ位置順次補正部５７は、センサ移動部６１による最終回（例えばＦ回目）の移動での移動後位置Ｑ_Ｆ，ｉを、補正対象センサ８１の最終位置Ｐ_Ｆ，ｉとして決定する最終位置決定部６４を備える。センサ移動部６１、移動後円弧形成部６２、移動制御部６３、最終位置決定部６４での詳細な処理については、後述する。

ここで、センサ位置修正部５５で、それぞれのセンサユニットＳ_ｉの位置を修正する方法について説明する。図４に示すように、センサ位置修正部５５は、センサ位置修正部５５は、仮曲線形状検出部５０により検出された仮曲線形状７５のそれぞれの仮円弧Ｌ_０，ｊの弧長Ｒ_０，ｊφ_０，ｊとセンサ間寸法ｌとの差の絶対値に基づいて、それぞれのセンサユニットＳ_ｉの位置を修正する（ステップＳ１０６）。すなわち、

で求められるエネルギーＥ_０，ｊ基づいて、それぞれのセンサユニットＳ_ｉの位置を修正する。これにより、それぞれのセンサユニットＳ_ｉは、仮位置Ｐ_０，ｉから最終位置Ｐ_Ｆ，ｉまで修正される。

前述のように、仮曲線形状検出部５０により、実際の挿入部１１の曲線形状と誤差の少ない仮曲線形状７５が形成される。しかし、仮曲線形状検出部５０では、センサ間寸法ｌを考慮して、それぞれのセンサユニットＳ_ｉの間の曲線補間を行っていない。また、それぞれのセンサユニットＳ_ｉの計測データはノイズ等により誤差を有する。したがって、センサ間寸法ｌを考慮して、それぞれの仮円弧Ｌ_０，ｊの弧長Ｒ_０，ｊφ_０，ｊとセンサ間寸法ｌとの差の絶対値に基づいて、それぞれのセンサユニットＳ_ｉの位置を修正することが重要となる。

図１２は、センサ位置修正部５５によりそれぞれのセンサユニットＳ_ｉの位置を修正する方法を示すフローチャートである。図１３は、センサ位置順次補正部５７による処理を説明する図である。図１２に示すように、それぞれのセンサユニットＳ_ｉの位置を修正する際は、センサ位置順次補正部５７により基端側（グローバル座標系Ｃの原点に近い側）のセンサユニットＳ_ｉから順次に、それぞれのセンサユニットＳ_ｉの位置補正前の補正前位置Ｑ_０，ｉから最終位置Ｐ_Ｆ，ｉへの位置補正が行われる。ここで、補正前位置Ｑ_０，ｉは、センサ位置順次補正部５７により位置補正が行われる直前のそれぞれのセンサユニットＳ_ｉの位置であり、それぞれのセンサユニットＳ_ｉが仮位置Ｐ_０，ｉから未補正センサ群移動部５９により（ｉ−１）回だけ平行移動した位置である。センサ位置順次補正部５７は、未補正センサ群７７の中で最も基端側のセンサユニットＳ_ｉの位置補正を開始する（ステップＳ１２１）。すなわち、図１３に示すように、センサユニットＳ_ｋ−１まで補正前位置Ｑ_０，ｉから最終位置Ｐ_Ｆ，ｉへの位置補正が完了している場合には、センサ位置順次補正部５７により、未補正センサ群７７の中で最も基端側のセンサユニットＳ_ｋの最終位置Ｐ_Ｆ，ｋへの位置補正が開始される。すなわち、センサユニットＳ_ｋが位置補正の対象である補正対象センサ８１となる。なお、以下の説明では、基端側から（ｋ＋１）番目のセンサユニットＳ_ｋの位置補正について説明する。

図１３に示すように、補正対象センサ８１であるセンサユニットＳ_ｋの位置補正を開始する際、センサユニットＳ_ｋ−１まで補正前位置Ｑ_０，ｉから最終位置Ｐ_Ｆ，ｉへの位置補正が完了している。この際、センサユニットＳ_ｋは、補正前位置Ｑ_０，ｋに位置している。この状態で、センサ移動部６１により、センサユニットＳ_ｋが位置Ｑ_０，ｋから位置Ｑ_１，ｋに移動し（ステップＳ１２２）、センサユニットＳ_ｋの１回目の移動が行われる。この際、位置Ｑ_０，ｋが移動前位置、位置Ｑ_１，ｋが移動後位置となる。センサ移動部６１によるセンサユニットＳ_ｋの移動は、１回以上行われる。図１４は、センサ移動部６１及び移動後円弧形成部６２での処理を説明する図である。図１４に示すように、ｓ回目の移動では、補正対象センサ８１であるセンサユニットＳ_ｋは移動前位置Ｑ_{ｓ−１，ｋ}から移動後位置Ｑ_ｓ，ｋまで移動する。すなわち、センサ移動部６１による１回の移動により、センサユニットＳ_ｋは移動前位置Ｑ_{ｔ−１，ｋ}から移動後位置Ｑ_ｔ，ｋまで移動する（ステップＳ１２２）。

そして、センサ移動部６１によりセンサユニットＳ_ｋの移動が１回行われるたびに、移動後円弧形成部６２により、センサユニットＳ_ｋに対して基端側に隣設されるセンサユニットＳ_ｋ−１の最終位置Ｐ_{Ｆ，ｋ−１}とセンサユニットＳ_ｋの移動後位置Ｑ_ｔ，ｋとの間が曲線補間され、移動後円弧Ｌ_ｔ，ｋが形成される（ステップＳ１２３）。図１４に示すように、ｓ回目の移動後には、センサユニットＳ_ｋ−１の最終位置Ｐ_{Ｆ，ｋ−１}とセンサユニットＳ_ｋの移動後位置Ｑ_ｓ，ｋとの間が曲線補間され、移動後円弧Ｌ_ｓ，ｋが形成される。ここで、センサユニットＳ_ｋ−１は、補正対象センサ８１であるセンサユニットＳ_ｋに対してグローバル座標系Ｃの原点に近い側に隣設される近位側隣設センサ８２である。

センサユニットＳ_ｋのｓ回目に移動後に移動後円弧Ｌ_ｓ，ｋを形成する際は、センサユニットＳ_ｋ−１の最終位置Ｐ_{Ｆ，ｋ−１}での単位接ベクトル及びセンサユニットＳ_ｋの移動後位置Ｑ_ｓ，ｋでの単位接ベクトルを算出する。センサユニットＳ_ｋ−１の最終位置Ｐ_{Ｆ，ｋ−１}での単位接ベクトルは、

となる。すなわち、最終位置Ｐ_{Ｆ，ｋ−１}での単位接ベクトルは、最終位置Ｐ_{Ｆ，ｋ−１}から移動後位置Ｑ_ｓ，ｋまでのベクトルの単位ベクトルである。また、移動後位置Ｑ_ｓ，ｋでの単位接ベクトルを算出する際は、補正対象センサ８１であるセンサユニットＳ_ｋに対してグローバル座標系Ｃの原点から遠い側に隣設される遠位側隣設センサ８３であるセンサユニットＳ_ｋ＋１が、位置Ｑ´_{ｓ，ｋ＋１}に配置されていると仮定する。ここで、移動後位置Ｑ_ｓ，ｋから位置Ｑ´_{ｓ，ｋ＋１}までのベクトルは、センサユニットＳ_ｋの仮位置Ｐ_０，ｋからセンサユニットＳ_ｋ＋１の仮位置Ｐ_{０，ｋ＋１}までのベクトルと向き及び大きさが同一である。センサユニットＳ_ｋの移動後位置Ｑ_ｓ，ｋでの単位ベクトルは、

となる。すなわち、移動後位置Ｑ_ｓ，ｋでの単位接ベクトルは、移動後位置Ｑ_ｓ，ｋからセンサユニットＳ_ｋ＋１の位置Ｑ´_{ｓ，ｋ＋１}までのベクトルの単位ベクトルである。したがって、移動後位置Ｑ_ｓ，ｋでの単位接ベクトルは、式（１７．１）〜式（１７．３）でｋをｋ＋１に置換して得られる単位接ベクトルと向きが同一である。

そして、式（２３．１）〜式（２３．３）、式（２４．１）〜式（２４．３）により算出された単位接ベクトルに基づいて、センサユニットＳ_ｋ−１の最終位置Ｐ_{Ｆ，ｋ−１}とセンサユニットＳ_ｋの移動後位置Ｑ_ｓ，ｋとの間での単位接ベクトルの変化率を算出する。まず、最終位置Ｐ_{Ｆ，ｋ−１}と移動後位置Ｑ_ｓ，ｋとの間での単位接ベクトルの変化率ベクトルが、

で求められる。そして、式（２５．１）、式（２５．２）で求められた変化率ベクトルの大きさが最終位置Ｐ_{Ｆ，ｋ−１}と移動後位置Ｑ_ｓ，ｋとの間での単位接ベクトルの変化率となる。

そして、式（２３．１）〜式（２３．３）、式（２４．１）〜式（２４．３）により算出された単位接ベクトル及び式（２５．１）、式（２５．２）により算出された変化率に基づいて、センサユニットＳ_ｋ−１の最終位置Ｐ_{Ｆ，ｋ−１}とセンサユニットＳ_ｋの移動後位置Ｑ_ｓ，ｋとの間の移動後円弧Ｌ_ｓ，ｋを形成する。ここで、式（２５．１）、式（２５．２）で算出した最終位置Ｐ_{Ｆ，ｋ−１}と移動後位置Ｑ_ｓ，ｋとの間での単位接ベクトルの変化率は、移動後円弧Ｌ_ｓ，ｋの曲率１／Ｒ_ｓ，ｋとなる。移動後円弧Ｌ_ｓ，ｋの半径Ｒ_ｓ，ｋは曲率１／Ｒ_ｓ，ｋの逆数であるため、移動後円弧Ｌ_ｓ，ｋの半径Ｒ_ｓ，ｋは、

となる。また、移動後円弧Ｌ_ｓ，ｋの中心Ｏ_ｓ，ｋのグローバル座標Ｃでの位置が、

により求められる。さらに、最終位置Ｐ_{Ｆ，ｋ−１}での単位接ベクトル及び移動後位置Ｑ_ｓ，ｋでの単位接ベクトルを用いて、移動後円弧Ｌ_ｓ，ｋの中心角φ_ｓ，ｋが、

により求められる。これらのパラメータを用いて、最終位置Ｐ_{Ｆ，ｋ−１}と移動後位置Ｑ_ｓ，ｋとの間の移動後円弧Ｌ_ｓ，ｋが形成される。

なお、センサユニットＳ_ｋのｓ回目の移動後に移動後円弧Ｌ_ｓ，ｋを形成する処理について説明したが、センサユニットＳ_ｋのｓ回目以外の移動後についても、式（２３．１）〜式（２８）で、ｓをｔで置換した式を用いて同様に、センサユニットＳ_ｋ−１の最終位置Ｐ_{Ｆ，ｋ−１}とセンサユニットＳ_ｋの移動後位置Ｑ_ｔ，ｋとの間の移動後円弧Ｌ_ｔ，ｋが形成される。

そして、移動後円弧Ｌ_ｔ，ｋが形成された後、移動制御部６３により移動後円弧Ｌ_ｔ，ｋの弧長Ｒ_ｔ，ｋφ_ｔ，ｋとセンサ間寸法ｌとの差の絶対値が算出される（ステップＳ１２４）。ここで、移動後円弧Ｌ_ｔ，ｋの弧長Ｒ_ｔ，ｋφ_ｔ，ｋとセンサ間寸法ｌとの差の絶対値をエネルギーＥ_ｔ，ｋとし、

で与えられる。エネルギーＥ_ｔ，ｋが所定の閾値より大きい場合は（ステップＳ１２４−Ｎｏ）、ステップＳ１２２に戻り、センサ移動部６１がセンサユニットＳ_ｋを位置Ｑ_ｔ，ｋからさらに移動させる。センサユニットＳ_ｋのｓ回目の移動後にエネルギーＥ_ｓ，ｋが所定の閾値より大きい場合は、センサ移動部６１により（ｓ＋１）回目の移動が行われる。（ｓ＋１）回目の移動により、センサユニットＳ_ｋは移動前位置Ｑ_ｓ，ｋから移動後位置Ｑ_{ｓ＋１，ｋ}に移動する。以上のように、センサ移動部６１は、エネルギーＥ_ｔ，ｋが所定の閾値以下になるまで、補正対象センサ８１であるセンサユニットＳ_ｋの移動前位置Ｑ_{ｔ−１，ｋ}から移動後位置Ｑ_ｔ，ｋへの移動を繰り返し行う状態に、移動制御部６３により制御されている。

エネルギーＥ_ｔ，ｋが所定の閾値以下の場合は（ステップＳ１２４−Ｙｅｓ）、次のステップに進む。そして、最終位置決定部６４が、センサ移動部６１によるセンサユニットＳ_ｋの最終回（例えばＦ回目）の移動での移動後位置Ｑ_Ｆ，ｋを、補正対象センサ８１であるセンサユニットＳ_ｋの最終位置Ｐ_Ｆ，ｋとして決定する（ステップＳ１２５）。以上のようにして、センサユニットＳ_ｋの補正前位置Ｑ_０，ｋから最終位置Ｐ_Ｆ，ｋへの位置補正が完了する。

なお、センサユニットＳ_ｋ以外のセンサユニットＳ_ｉについても、センサ位置順次補正部５７によりセンサユニットＳ_ｋと同様に最終位置Ｐ_ｉ，ｋへの位置補正が行われる。すなわち、センサ移動部６１により、センサユニットＳ_ｉは移動前位置Ｑ_{ｔ−１，ｉ}から移動後位置Ｑ_ｔ，ｉまで移動する（ステップＳ１２２）。そして、センサ移動部６１によりセンサユニットＳ_ｉの移動が１回行われるたびに、移動後円弧形成部６２により、センサユニットＳ_ｉに対して基端側に隣設される近位側隣設センサ８２であるセンサユニットＳ_ｉ−１の最終位置Ｐ_{Ｆ，ｉ−１}とセンサユニットＳ_ｉの移動後位置Ｑ_ｔ，ｉとの間が曲線補間され、移動後円弧Ｌ_ｔ，ｊが形成される（ステップＳ１２３）。

そして、移動後円弧Ｌ_ｔ，ｊが形成された後、移動制御部６３により移動後円弧Ｌ_ｔ，ｊの弧長Ｒ_ｔ，ｊφ_ｔ，ｊとセンサ間寸法ｌとの差の絶対値であるエネルギーＥ_ｔ，ｊが算出される（ステップＳ１２４）。エネルギーＥ_ｔ，ｊが所定の閾値より大きい場合は（ステップＳ１２４−Ｎｏ）、ステップＳ１２２に戻り、センサ移動部６１がセンサユニットＳ_ｉを位置Ｑ_ｔ，ｉからさらに移動させる。エネルギーＥ_ｔ，ｊが所定の閾値以下の場合は（ステップＳ１２４−Ｙｅｓ）、次のステップに進む。そして、最終位置決定部６４が、センサ移動部６１によるセンサユニットＳ_ｉの最終回の移動での移動後位置Ｑ_Ｆ，ｉを、補正対象センサ８１であるセンサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉとして決定する（ステップＳ１２５）。以上のようにして、センサユニットＳ_ｉの補正前位置Ｑ_０，ｉから最終位置Ｐ_Ｆ，ｉへの位置補正が完了する。

そして、センサ位置順次補正部５７により１つのセンサユニットＳ_ｉの位置補正が行われるたびに、未補正センサ群移動部５９により、位置補正が完了していないセンサユニットＳ_ｉである未補正センサ群７７が平行移動される（ステップＳ１２６）。図１５は、未補正センサ群移動部５９での処理を説明する図である。図１５に示すように、センサ位置順次補正部５７によりセンサユニットＳ_ｋの補正前位置Ｑ_０，ｋから最終位置Ｐ_Ｆ，ｋへの位置修正が完了した状態では、センサユニットＳ_ｋ＋１〜Ｓ_Ｎが、位置補正が完了していない未補正センサ群７７となる。また、センサユニットＳ_ｋが、センサ位置順次補正部５７による直前の位置補正で補正対象だった直前補正対象センサ７９となる。

この状態で、未補正センサ群移動部５９は、直前補正対象センサ７９であるセンサユニットＳ_ｋの補正前位置Ｑ_０，ｋから最終位置Ｐ_Ｆ，ｋへの補正量だけ、未補正センサ群７７を平行移動させる。これにより、未補正センサ群７７のそれぞれのセンサユニットＳ_ｉが平行移動前位置Ｕ_{ｋ−１，ｉ}から平行移動後位置Ｕ_ｋ，ｉへ移動する。ここで、未補正センサ群７７のそれぞれのセンサユニットＳ_ｉが平行移動前位置Ｕ_{ｋ−１，ｉ}は、それぞれのセンサユニットＳ_ｉが仮位置Ｐ_０，ｉから未補正センサ群移動部５９により（ｋ−１）回だけ移動した位置であり、今回の平行移動で、未補正センサ群７７のそれぞれのセンサユニットＳ_ｉは未補正センサ群移動部５９によりｋ回だけ平行移動したこととなる。また、センサユニットＳ_ｋ＋１の平行移動後位置Ｕ_{ｋ，ｋ＋１}は、補正前位置Ｑ_{０，ｋ＋１}と一致し、センサユニットＳ_ｋの最終位置Ｐ_Ｆ，ｋからセンサユニットＳ_ｋ＋１の平行移動後位置Ｕ_{ｋ，ｋ＋１}へのベクトルは、センサユニットＳ_ｋの仮位置Ｐ_０，ｋからセンサユニットＳ_ｋ＋１の仮位置Ｐ_{０，ｋ＋１}までのベクトルと向き及び大きさが同一である。

なお、センサユニットＳ_ｋ以外のセンサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉへの位置補正が完了した状態でも、センサユニットＳ_ｋと同様に、未補正センサ群７７が平行移動される。すなわち、未補正センサ群移動部５９は、直前補正対象センサ７９がセンサ位置順次補正部５７により補正前位置Ｑ_０，ｉから最終位置Ｐ_Ｆ，ｉへ補正された補正量だけ、未補正センサ群７７を平行移動させる（ステップＳ１２６）。これにより、未補正センサ群７７は、平行移動前位置Ｕ_{ａ−１，ｉ}から平行移動後位置Ｕ_ａ，ｉまで平行移動する。

そして、図１２に示すように、すべてのセンサユニットＳ_ｉについて位置補正が完了したか確認する（ステップＳ１２７）。すべてのセンサユニットＳ_ｉについて位置補正が完了している場合は（ステップＳ１２７−Ｙｅｓ）、次のステップへ進む。すべてのセンサユニットＳ_ｉについて位置補正が完了していない場合は（ステップＳ１２７−Ｎｏ）、ステップＳ１２１に戻り、センサ位置順次補正部５７により、未補正センサ群７７の中で最も基端側のセンサユニットＳ_ｉの位置補正が行われる。すなわち、ステップＳ１２１〜Ｓ１２６が、すべてのセンサユニットＳ_ｉについて位置補正が完了するまで、繰り返し行われる。以上のように、センサ位置順次補正部５７での補正前位置Ｑ_０，ｉから最終位置Ｐ_Ｆ，ｉ位置への位置補正及び未補正センサ群移動部５９での平行移動により、それぞれのセンサユニットＳ_ｉが、仮位置Ｐ_０，ｉから最終位置Ｐ_Ｆ，ｉへ位置修正される。

図３に示すように、センサ位置修正部５５は、最終曲線形状検出部６５に接続されている。最終曲線形状検出部６５は、それぞれのセンサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉの間を最終円弧Ｌ_Ｆ，ｊで曲線補間し、挿入部１１の最終曲線形状８５を検出する。図１６は、最終曲線形状検出部６５により検出された内視鏡１０の挿入部１１の最終曲線形状８５をグローバル座標ＣのＺ軸の負から正の方向に向かって視た図である。図１６に示すように、最終曲線形状検出部６５により、それぞれのセンサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉの間が曲線補間される。これにより、それぞれの最終円弧Ｌ_Ｆ，ｊが形成され、最終曲線形状８５が検出される。

最終曲線形状検出部６５は、それぞれのセンサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉに基づいて、それぞれの最終円弧Ｌ_Ｆ，ｊのパラメータを算出するパラメータ算出部６７と、パラメータ算出部６７により算出されたパラメータに基づいて、最終円弧Ｐ_Ｆ，ｉを形成する最終円弧形成部６９とを備える。最終円弧形成部６９は、クォータニオン（四元数）及び媒介変数ｔを用いた補間関数Ｌ_Ｆ，ｊ（ｔ）により、媒介変数ｔの変化に対して等角速度でそれぞれのセンサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉの間を曲線補間し、最終円弧Ｌ_Ｆ，ｊを形成する。

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

ここで、最終曲線形状検出部６５で、センサ位置修正部５５により位置修正されたそれぞれのセンサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉの間を曲線補間し、最終曲線形状８５を検出する方法について説明する。図４に示すように、最終曲線形状検出部６５は、ステップＳ１０６で位置修正されたそれぞれのセンサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉの間を最終円弧Ｌ_Ｆ，ｊで曲線補間を行い、最終曲線形状８５を検出する（ステップＳ１０７）。これにより、センサ間寸法ｌを考慮してそれぞれのセンサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉの間の曲線補間が行われ、検出精度の高い最終曲線形状８５が検出される。

最終曲線形状８５を検出する際、最終曲線形状検出部６５は、それぞれのセンサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉの間ごとに順次に曲線補間を行い、最終円弧Ｌ_Ｆ，ｊを形成する。ここで、最終曲線形状検出部６５により、それぞれのセンサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉの間の曲線補間を行う方法について説明する。ここでは、基端側からｋ番目のセンサユニットＳ_ｋ−１と基端側から（ｋ＋１）番目のセンサユニットＳ_ｋとの間の曲線補間について説明する。すなわち、最終位置Ｐ_{Ｆ，ｋ−１}と最終位置Ｐ_Ｆ，ｋとの間が、補間対象である補間対象最終位置間となる。

図１７は、最終曲線形状検出部６５により最終曲線形状８５を検出する方法を示すフローチャートである。図１８は、最終曲線形状検出部６５での処理を説明する図である。図１７及び図１８に示すように、最終曲線形状検出部６５によりセンサユニットＳ_ｋ−１の最終位置Ｐ_{Ｆ，ｋ−１}とセンサユニットＳ_ｋの最終位置Ｐ_Ｆ，ｋとの間である補間対象の最終位置間の曲線補間を行う際は、まず、パラメータ算出部６７により、最終位置Ｐ_{Ｆ，ｋ−１}及び最終位置Ｐ_Ｆ，ｋに基づいて、最終円弧Ｌ_Ｆ，ｋのパラメータを算出する（ステップＳ１３１）。

この際、センサユニットＳ_ｋ−１の最終位置Ｐ_{Ｆ，ｋ−１}での単位接ベクトル及びセンサユニットＳ_ｋの最終位置Ｐ_Ｆ，ｋでの単位接ベクトルを算出する。センサユニットＳ_ｋ−１の最終位置Ｐ_{Ｆ，ｋ−１}での単位接ベクトルは、

となる。すなわち、最終位置Ｐ_{Ｆ，ｋ−１}での単位接ベクトルは、最終位置Ｐ_{Ｆ，ｋ−１}から最終位置Ｐ_Ｆ，ｋまでのベクトルの単位ベクトルである。センサユニットＳ_ｋの最終位置Ｐ_Ｆ，ｋでの単位接ベクトルは、式（３０．１）、式（３０．２）、式（３０．３）のｋをｋ＋１で置換した式を用いて求められる。すなわち、最終位置Ｐ_Ｆ，ｋでの単位接ベクトルは、最終位置Ｐ_Ｆ，ｋから最終位置Ｐ_{Ｆ，ｋ＋１}までのベクトルの単位ベクトルである。

そして、最終位置Ｐ_{Ｆ，ｋ−１}及びＰ_Ｆ，ｋでの単位接ベクトルに基づいて、センサユニットＳ_ｋ−１の最終位置Ｐ_{Ｆ，ｋ−１}とセンサユニットＳ_ｋの最終位置Ｐ_Ｆ，ｋとの間での単位接ベクトルの変化率を算出する。まず、最終位置Ｐ_{Ｆ，ｋ−１}と最終位置Ｐ_Ｆ，ｋとの間での単位接ベクトルの変化率ベクトルが、

で求められる。そして、式（３１．１）、式（３１．２）で求められた変化率ベクトルの大きさが最終位置Ｐ_{Ｆ，ｋ−１}と最終位置Ｐ_Ｆ，ｋとの間での単位接ベクトルの変化率となる。

そして、式（３０．３）〜式（３１．２）により算出された単位接ベクトル及び変化率に基づいて、センサユニットＳ_ｋ−１の最終位置Ｐ_{Ｆ，ｋ−１}とセンサユニットＳ_ｋの最終位置Ｐ_Ｆ，ｋとの間の最終円弧Ｌ_Ｆ，ｋのパラメータが算出される。ここで、最終円弧Ｌ_Ｆ，ｋの半径Ｒ_Ｆ，ｋは、式（３１．１）、式（３１．２）を用いて算出された変化率_ｋの逆数であるため、

となる。また、最終円弧Ｌ_Ｆ，ｋの中心Ｏ_Ｆ，ｋのグローバル座標Ｃでの位置が、

により求められる。さらに、最終円弧Ｌ_Ｆ，ｋの中心角φ_Ｆ，ｋが、

により求められる。

なお、最終円弧Ｌ_Ｆ，ｋ以外の最終円弧Ｌ_Ｆ，ｊについても、式（３０．１）〜式（３４）のｋをｊで置換した式を用いて、最終円弧Ｌ_Ｆ，ｋと同様に、パラメータ算出部６７により、最終円弧Ｌ_Ｆ，ｊのパラメータが算出される。すなわち、それぞれのセンサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉに基づいて、それぞれの最終円弧Ｌ_Ｆ，ｊのパラメータを算出する（ステップＳ１３１）。

そして、パラメータ算出部６７により算出された最終円弧Ｌ_Ｆ，ｊのパラメータに基づいて、最終円弧形成部６９が、それぞれのセンサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉの間を曲線補間し、最終円弧Ｌ_Ｆ，ｊを形成する（ステップＳ１３２）。最終円弧Ｌ_Ｆ，ｊの形成は、クォータニオン（四元数）及び媒介変数を用いた補間関数により行われる。

ここで、クォータニオンは、複素数を拡張した数であり、

で現される。式（３５）のｉ，ｊ，ｋは虚数単位を示し、

の関係を満たす。

図１８に示すように、センサユニットＳ_ｋ−１の最終位置Ｐ_{Ｆ，ｋ−１}とセンサユニットＳ_ｋの最終位置Ｐ_Ｆ，ｋとの間の最終円弧Ｌ_Ｆ，ｋを形成する場合には、グローバル座標系Ｃにおいて、最終円弧Ｌ_Ｆ，ｋの中心Ｏ_Ｆ，ｋからセンサユニットＳ_ｋ−１の最終位置Ｐ_{Ｆ，ｋ−１}へのベクトルを算出する。中心Ｏ_Ｆ，ｋから最終位置Ｐ_{Ｆ，ｋ−１}へのベクトルは、

となる。また、グローバル座標系Ｃにおいて、最終円弧Ｌ_Ｆ，ｋの中心Ｏ_Ｆ，ｋからセンサユニットＳ_ｋの最終位置Ｐ_Ｆ，ｋへのベクトルを算出する。中心Ｏ_Ｆ，ｋから最終位置Ｐ_Ｆ，ｋへのベクトルは、

となる。

式（３７）で算出されたベクトルは、クォータニオンにより、

と現される。同様に、式（３８）で算出されたベクトルは、クォータニオンにより、

と現される。すなわち、算出されたベクトルのグローバル座標系ＣのＸ軸方向成分が、クォータニオンの虚数ｉの成分となる。同様に、ベクトルのグローバル座標系ＣのＹ軸方向成分がクォータニオンの虚数ｊの成分となり、ベクトルのグローバル座標系ＣのＺ軸方向成分がクォータニオンの虚数ｋの成分となる。

そして、式（３９）、式（４０）のクォータ二オンを用いて、以下の関数Ｌ´_Ｆ，ｋ（ｔ）を定義する。関数Ｌ´_Ｆ，ｋ（ｔ）は、

となる。ここで、ｔは媒介変数である。正規化のため、式（４１）の両辺をｓｉｎφ_Ｆ，ｋで割ると、

となり、補間関数Ｌ_Ｆ，ｋ（ｔ）が算出される。

そして、補間関数Ｌ_Ｆ，ｋ（ｔ）でｔを０から１に変化させることにより、センサユニットＳ_ｋ−１の最終位置Ｐ_{Ｆ，ｋ−１}とセンサユニットＳ_ｋの最終位置Ｐ_Ｆ，ｋとの間が曲線補間され、最終円弧Ｌ_Ｆ，ｋが形成される。この際、最終位置Ｐ_{Ｆ，ｋ−１}と最終位置Ｐ_Ｆ，ｋとの間は、媒介変数ｔの変化に対して等角速度で曲線補間される。例えば、ｔが０から０．１に変化する間に補間される距離ｄ１と、ｔが０．５から０．６に変化する間に補間される距離ｄ２は同一である（図１８参照）。したがって、最終位置Ｐ_{Ｆ，ｋ−１}と最終位置Ｐ_Ｆ，ｋとの間を補間する際に、角速度の変化を考慮する必要がないため、最終円弧Ｌ_Ｆ，ｋを形成する処理が単純化される。

なお、最終円弧Ｌ_Ｆ，ｋ以外の最終円弧Ｌ_Ｆ，ｊについても、補間関数Ｌ_Ｆ，ｊ（ｔ）を用いて形成される。補間関数Ｌ_Ｆ，ｊ（ｔ）は、式（３７）〜式（４２）のｋをｊで置換した式を用いて、算出される。この場合も、それぞれのセンサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉの間は、補間関数Ｌ_Ｆ，ｊ（ｔ）の媒介変数ｔの変化に対して等角速度で曲線補間される。すべての最終円弧Ｌ_Ｆ，ｊが形成されることにより、内視鏡１０の挿入部１１の最終曲線形状８５が検出される。

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

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

また、センサ位置順次補正部５７によりそれぞれのセンサユニットＳ_ｉの位置を補正する際には、センサ移動部６１がセンサユニットＳ_ｉを移動前位置Ｑ_{ｔ−１，ｉ}から移動後位置Ｑ_ｔ，ｉまで移動する。そして、センサ移動部６１によりセンサユニットＳ_ｉの移動が１回行われるたびに、移動後円弧形成部６２により、センサユニットＳ_ｉに対して基端側に隣設されるセンサユニットＳ_ｉ−１の最終位置Ｐ_{Ｆ，ｉ−１}とセンサユニットＳ_ｉの移動後位置Ｑ_ｔ，ｉとの間が曲線補間され、移動後円弧Ｌ_ｔ，ｊが形成される。そして、移動後円弧Ｌ_ｔ，ｊが形成された後、移動制御部６３により移動後円弧Ｌ_ｔ，ｊの弧長Ｒ_ｔ，ｊφ_ｔ，ｊとセンサ間寸法ｌとの差の絶対値であるエネルギーＥ_ｔ，ｊが算出される。エネルギーＥ_ｔ，ｊが所定の閾値より大きい場合は、センサ移動部６１がセンサユニットＳ_ｉを位置Ｑ_ｔ，ｉからさらに移動させる。エネルギーＥ_ｔ，ｊが所定の閾値以下の場合は、最終位置決定部６４が、センサ移動部６１によるセンサユニットＳ_ｉの最終回の移動での移動後位置Ｑ_Ｆ，ｉを、補正対象センサ８１であるセンサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉとして決定する。すなわち、エネルギーＥ_ｔ，ｊが所定の閾値以下になるまで、センサ移動部６１によるセンサユニットＳ_ｉの移動が繰り返される。以上のように、それぞれのセンサユニットＳ_ｉの間の実際の寸法であるセンサ間寸法ｌに基づいて、センサ位置順次補正部５７によるそれぞれのセンサユニットＳ_ｉの位置補正が行われるため、実際の挿入部１１の形状と誤差の少ない最終曲線形状８５を検出することができる。これにより、高い精度で挿入部１１の最終曲線形状８５を検出することができる。

また、センサ位置修正部５５によりセンサユニットＳ_ｉの位置を修正すると、センサユニットＳ_ｉに対して基端側に隣設されるセンサユニットＳ_ｉ−１とセンサユニットＳ_ｉとの間の形状のみが影響を受け、仮円弧Ｌ_０，ｊが最終円弧Ｌ_Ｆ，ｊに修正される。すなわち、１つのセンサユニットＳ_ｉの位置修正が行われるたびに、挿入部１１の形状全体が修正される構成でない。ここで、センサ間寸法ｌは、それぞれのセンサユニットＳ_ｉの間の長手方向の寸法であり、挿入部１１の局所的なパラメータである。すなわち、センサ間寸法ｌは、挿入部１１全体の形状に影響を与えるパラメータではなく、挿入部１１の形状に局所的に影響を与えるパラメータである。したがって、１つのセンサユニットＳ_ｉの位置の修正により、位置修正されたセンサユニットＳ_ｉの近傍で局所的に挿入部１１の形状が修正されるため、高い精度で仮円弧Ｌ_０，ｊから最終円弧Ｌ_Ｆ，ｊへの修正が行われる。これにより、挿入部１１の最終曲線形状８５を、高い精度で検出することができる。また、１つのセンサユニットＳ_ｉの位置の修正により、位置修正されたセンサユニットＳ_ｉの近傍でのみ局所的に挿入部１１の形状が修正されるため、挿入部１１の形状を修正する処理を単純化することができる。

また、最終曲線形状検出部６５では、補間関数Ｌ_Ｆ，ｊ（ｔ）でｔを０から１に変化させることにより、それぞれのセンサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉの間が曲線補間され、それぞれの最終円弧Ｌ_Ｆ，ｊが形成される。この際、それぞれのセンサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉの間は、媒介変数ｔの変化に対して等角速度で曲線補間される。したがって、それぞれの最終位置Ｐ_Ｆ，ｉの間を補間する際に、角速度の変化を考慮する必要がないため、それぞれの最終円弧Ｌ_Ｆ，ｊを形成する処理を単純化することができる。

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

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

本実施形態のセンサ位置修正部５５は、第１の実施形態と同様に、基端側（グローバル座標系Ｃの原点に近い側）のセンサユニットＳ_ｉから順次に、それぞれのセンサユニットＳ_ｉの補正前位置Ｑ_０，ｉから最終位置Ｐ_Ｆ，ｉへの位置補正を行うセンサ位置順次補正部５７と、センサ位置順次補正部５７により１つのセンサユニットＳ_ｉの位置補正が行われるたびに、位置補正が完了していないセンサユニットＳ_ｉである未補正センサ群７７を平行移動させる未補正センサ群移動部５９と、を備える。

センサ位置順次補正部５７は、第１の実施形態と同様に、位置補正の対象である補正対象センサ８１を補正前位置Ｑ_０，ｉから１回以上移動させるセンサ移動部６１を備える。センサ移動部６１での１回の移動により、補正対象センサ８１は移動前位置Ｑ_{ｔ−１，ｉ}から移動後位置Ｑ_ｔ，ｉまで移動する。また、センサ位置順次補正部５７は、センサ移動部６１により補正対象センサ８１の移動が１回行われるたびに、補正対象センサ８１に対して基端側（グローバル座標系Ｃの原点に近い側）に隣設されるセンサユニットＳ_ｉである近位側隣設センサ８２の最終位置Ｐ_{Ｆ，ｉ−１}と補正対象センサ８１の移動後位置Ｑ_ｔ，ｉとの間を曲線補間する移動後円弧形成部６２とを備える。移動後円弧形成部６２で近位側隣設センサ８２の最終位置Ｐ_{Ｆ，ｉ−１}と補正対象センサ８１の移動後位置Ｑ_ｔ，ｉとの間を曲線補間することにより、移動後円弧Ｌ_ｔ，ｊが形成される。

センサ位置順次補正部５７は、近位側隣設センサ８２の最終位置Ｐ_{Ｆ，ｉ−１}と補正対象センサ８１の移動前の移動前位置Ｑ_{ｔ−１，ｉ}との間を曲線補間して形成される移動前円弧Ｌ_{ｔ−１，ｊ}の弧長Ｒ_{ｔ−１，ｊ}φ_{ｔ−１，ｊ}とセンサ間寸法ｌとの差の絶対値に対して、移動後円弧Ｌ_ｔ，ｊの弧長Ｒ_ｔ，ｊφ_ｔ，ｊとセンサ間寸法ｌとの差の絶対値が小さくなる補正対象センサ８１の移動を所定の回数だけ繰り返す状態にセンサ移動部６１を制御する移動制御部６３を備える。ここで、移動前円弧Ｌ_{ｔ−１，ｊ}の弧長Ｒ_{ｔ−１，ｊ}φ_{ｔ−１，ｊ}とセンサ間寸法ｌとの差の絶対値をエネルギーＥ_{ｔ−１，ｊ}とする。同様に、移動後円弧Ｌ_ｔ，ｊの弧長Ｒ_ｔ，ｊφ_ｔ，ｊとセンサ間寸法ｌとの差の絶対値をエネルギーＥ_ｔ，ｊとする。また、センサ位置順次補正部５７は、センサ移動部６１による最終回の移動での移動後位置Ｑ_Ｆ，ｉを、補正対象センサ８１の最終位置Ｐ_Ｆ，ｉとして決定する最終位置決定部６４を備える。

ここで、センサ位置修正部５５で、それぞれのセンサユニットＳ_ｉの位置を修正する方法について説明する。センサ位置修正部５５は、第１の実施形態と同様に、仮曲線形状検出部５０により検出された仮曲線形状７５のそれぞれの仮円弧Ｌ_０，ｊの弧長Ｒ_０，ｊφ_０，ｊとセンサ間寸法ｌとの差の絶対値に基づいて、それぞれのセンサユニットＳ_ｉの位置を修正する（図４のステップＳ１０６）。すなわち、式（２２）で求められるエネルギーＥ_０，ｊ基づいて、それぞれのセンサユニットＳ_ｉの位置を修正する。これにより、それぞれのセンサユニットＳ_ｉは、仮位置Ｐ_０，ｉから最終位置Ｐ_Ｆ，ｉまで修正される。

図１９は、センサ位置修正部５５によりそれぞれのセンサユニットＳ_ｉの位置を修正する方法を示すフローチャートである。図１９に示すように、それぞれのセンサユニットＳ_ｉの位置を修正する際は、第１の実施形態と同様に、センサ位置順次補正部５７により基端側（グローバル座標系Ｃの原点に近い側）のセンサユニットＳ_ｉから順次に、それぞれのセンサユニットＳ_ｉの補正前位置Ｑ_０，ｉから最終位置Ｐ_Ｆ，ｉへの位置補正が行われる。センサ位置順次補正部５７は、未補正センサ群７７の中で最も基端側のセンサユニットＳ_ｉの位置補正を開始する（ステップＳ１４１）。すなわち、センサユニットＳ_ｋ−１まで補正前位置Ｑ_０，ｉから最終位置Ｐ_Ｆ，ｉへの位置補正が完了している場合には、センサ位置順次補正部５７により、未補正センサ群７７の中で最も基端側のセンサユニットＳ_ｋの最終位置Ｐ_Ｆ，ｋへの位置補正が開始される。この場合、センサユニットＳ_ｋが位置補正の対象である補正対象センサ８１となる。なお、以下の説明では、基端側から（ｋ＋１）番目のセンサユニットＳ_ｋの位置補正について説明する。

補正対象センサ８１であるセンサユニットＳ_ｋの位置補正を開始する際、センサユニットＳ_ｋ−１まで最終位置Ｐ_Ｆ，ｉへの位置補正が完了している。この際、センサユニットＳ_ｋは、補正前位置Ｑ_０，ｋに位置している。この状態で、センサ移動部６１により、センサユニットＳ_ｋが位置Ｑ_０，ｋから位置Ｑ_１，ｋに移動し（ステップＳ１４２）、センサユニットＳ_ｋの１回目の移動が行われる。センサ移動部６１によるセンサユニットＳ_ｋの移動は、１回以上行われる。そして、ｓ回目の移動では、補正対象センサ８１であるセンサユニットＳ_ｋは移動前位置Ｑ_{ｓ−１，ｋ}から移動後位置Ｑ_ｓ，ｋまで移動する。すなわち、センサ移動部６１による１回の移動により、センサユニットＳ_ｋは移動前位置Ｑ_{ｔ−１，ｋ}から移動後位置Ｑ_ｔ，ｋまで移動する（ステップＳ１４２）。

そして、センサ移動部６１によりセンサユニットＳ_ｋの移動が１回行われるたびに、移動後円弧形成部６２により、センサユニットＳ_ｋに対して基端側に隣設されるセンサユニットＳ_ｋ−１の最終位置Ｐ_{Ｆ，ｋ−１}とセンサユニットＳ_ｋの移動後位置Ｑ_ｔ，ｋとの間が曲線補間され、移動後円弧Ｌ_ｔ，ｋが形成される（ステップＳ１４３）。ｓ回目の移動後には、センサユニットＳ_ｋ−１の最終位置Ｐ_{Ｆ，ｋ−１}とセンサユニットＳ_ｋの移動後位置Ｑ_ｓ，ｋとの間が曲線補間され、移動後円弧Ｌ_ｓ，ｋが形成される。ここで、センサユニットＳ_ｋ−１は、補正対象センサ８１であるセンサユニットＳ_ｋに対してグローバル座標系Ｃの原点に近い側に隣設される近位側隣設センサ８２である。

センサユニットＳ_ｋのｓ回目に移動後に移動後円弧Ｌ_ｓ，ｋを形成する際は、第１の実施形態と同様に式（２３．１）〜式（２８）を用いて、移動後円弧Ｌ_ｓ，ｋのパラメータが算出される。そして算出されたパラメータを用いて、最終位置Ｐ_{Ｆ，ｋ−１}と移動後位置Ｑ_ｓ，ｋとの間の移動後円弧Ｌ_ｓ，ｋが形成される。

なお、センサユニットＳ_ｋのｓ回目に移動後に移動後円弧Ｌ_ｓ，ｋを形成する処理について説明したが、センサユニットＳ_ｋのｓ回目以外の移動後についても、式（２３．１）〜式（２８）で、ｓをｔで置換した式を用いて同様に、センサユニットＳ_ｋ−１の最終位置Ｐ_{Ｆ，ｋ−１}とセンサユニットＳ_ｋの移動後位置Ｑ_ｔ，ｋとの間の移動後円弧Ｌ_ｔ，ｋが形成される。

センサ移動部６１は、移動制御部６３により、近位側隣設センサ８２の最終位置Ｐ_{Ｆ，ｋ−１}と補正対象センサ８１の移動前の移動前位置Ｑ_{ｔ−１，ｋ}との間を曲線補間して形成される移動前円弧Ｌ_{ｔ−１，ｋ}の弧長Ｒ_{ｔ−１，ｋ}φ_{ｔ−１，ｋ}とセンサ間寸法ｌとの差の絶対値に対して、移動後円弧Ｌ_ｔ，ｋの弧長Ｒ_ｔ，ｋφ_ｔ，ｋとセンサ間寸法ｌとの差の絶対値が小さくなる補正対象センサ８１の移動を行う状態に制御されている。ここで、移動後円弧Ｌ_ｔ，ｋの弧長Ｒ_ｔ，ｋφ_ｔ，ｋとセンサ間寸法ｌとの差の絶対値をエネルギーＥ_ｔ，ｋとし、第１の実施形態と同様に式（２９）で与えられる。また、移動前円弧Ｌ_{ｔ−１，ｋ}の弧長Ｒ_{ｔ−１，ｋ}φ_{ｔ−１，ｋ}とセンサ間寸法ｌとの差の絶対値をエネルギーＥ_{ｔ−１，ｋ}とし、式（２９）のｔをｔ−１で置換した式で与えられる。すなわち、センサ移動部６１は、移動後のエネルギーＥ_ｔ，ｋが移動前のエネルギーＥ_{ｔ−１，ｋ}より小さくなる状態に補正対象センサ８１であるセンサユニットＳ_ｋの移動を行っている（ステップＳ１４２）。なお、例えばセンサユニットＳ_ｋのｓ回目の移動での移動前円弧Ｌ_{ｓ−１，ｋ}は、センサユニットＳ_ｋの（ｓ−１）回目の移動での移動後円弧Ｌ_{ｓ−１，ｋ}と同一の円弧である。センサユニットＳ_ｋのｓ回目の移動での移動前円弧Ｌ_{ｓ−１，ｋ}は、式（２３．１）〜式（２８）でｓをｓ−１で置換した式を用いて、形成される。

そして、移動制御部６３が、補正対象センサ８１であるセンサユニットＳ_ｋの移動を所定の回数だけ行ったかを判断する（ステップＳ１４４）。センサユニットＳ_ｋの移動を所定の回数だけ行っていない場合は（ステップＳ１４４−Ｎｏ）、ステップＳ１２２に戻り、センサ移動部６１がセンサユニットＳ_ｋを位置Ｑ_ｔ，ｋからさらに移動させる。この場合、センサユニットＳ_ｋのｓ回目の移動後に、センサ移動部６１により（ｓ＋１）回目の移動が行われる。（ｓ＋１）回目の移動により、センサユニットＳ_ｋは移動前位置Ｑ_ｓ，ｋから移動後位置Ｑ_{ｓ＋１，ｋ}に移動する。以上のように、センサ移動部６１は、補正対象センサ８１であるセンサユニットＳ_ｋの移動前位置Ｑ_{ｔ−１，ｋ}から移動後位置Ｑ_ｔ，ｋへの移動を所定の回数だけ繰り返し行う状態に、移動制御部６３により制御されている。

センサユニットＳ_ｋの移動が所定の回数だけ行われた場合は（ステップＳ１４４−Ｙｅｓ）、次のステップに進む。そして、最終位置決定部６４が、センサ移動部６１によるセンサユニットＳ_ｋの最終回（例えばＦ回目）の移動での移動後位置Ｑ_Ｆ，ｋを、補正対象センサ８１であるセンサユニットＳ_ｋの最終位置Ｐ_Ｆ，ｋとして決定する（ステップＳ１４５）。以上のようにして、センサユニットＳ_ｋの最終位置Ｐ_Ｆ，ｋへの位置修正が完了する。

なお、センサユニットＳ_ｋ以外のセンサユニットＳ_ｉについても、センサ位置順次補正部５７によりセンサユニットＳ_ｋと同様に最終位置Ｐ_ｉ，ｋへの位置補正が行われる。すなわち、センサ移動部６１により、センサユニットＳ_ｉは移動前位置Ｑ_{ｔ−１，ｉ}から移動後位置Ｑ_ｔ，ｉまで移動する（ステップＳ１４２）。そして、センサ移動部６１によりセンサユニットＳ_ｉの移動が１回行われるたびに、移動後円弧形成部６２により、センサユニットＳ_ｉに対して基端側に隣設されるセンサユニットＳ_ｉ−１の最終位置Ｐ_{Ｆ，ｉ−１}とセンサユニットＳ_ｉの移動後位置Ｑ_ｔ，ｉとの間が曲線補間され、移動後円弧Ｌ_ｔ，ｊが形成される（ステップＳ１４３）。センサ移動部６１は、移動前円弧Ｌ_{ｔ−１，ｊ}の弧長Ｒ_{ｔ−１，ｊ}φ_{ｔ−１，ｊ}とセンサ間寸法ｌとの差の絶対値に対して、移動後円弧Ｌ_ｔ，ｊの弧長Ｒ_ｔ，ｊφ_ｔ，ｊとセンサ間寸法ｌとの差の絶対値が小さくなる状態に、センサユニットＳ_ｉの移動を行っている（ステップＳ１４２）。すなわち、移動後のエネルギーＥ_ｔ，ｊが移動前のエネルギーＥ_{ｔ−１，ｊ}より小さくなる状態に、補正対象センサ８１であるセンサユニットＳ_ｉの移動を行っている。

そして、移動後円弧Ｌ_ｔ，ｊが形成された後、移動制御部６３によりセンサユニットＳ_ｉの移動が所定の回数だけ行われたか判断される（ステップＳ１４４）。センサユニットＳ_ｉの移動が所定の回数だけ行われていない場合は（ステップＳ１４４−Ｎｏ）、ステップＳ１４２に戻り、センサ移動部６１がセンサユニットＳ_ｉを位置Ｑ_ｔ，ｉからさらに移動させる。センサユニットＳ_ｉの移動が所定の回数だけ行われた場合は（ステップＳ１４４−Ｙｅｓ）、次のステップに進む。そして、最終位置決定部６４が、センサ移動部６１によるセンサユニットＳ_ｉの最終回の移動での移動後位置Ｑ_Ｆ，ｉを、補正対象センサ８１であるセンサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉとして決定する（ステップＳ１４５）。以上のようにして、センサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉへの位置補正が完了する。

そして、第１の実施形態と同様に、センサ位置順次補正部５７により１つのセンサユニットＳ_ｉの位置補正が行われるたびに、未補正センサ群移動部５９により、位置補正が行われていないセンサユニットＳ_ｉである未補正センサ群７７が平行移動される（ステップＳ１４６）。未補正センサ群移動部５９での処理は、第１の実施形態と同様であるため、詳細な説明は省略する。

そして、すべてのセンサユニットＳ_ｉについて位置補正が完了したか確認する（ステップＳ１４７）。すべてのセンサユニットＳ_ｉについて位置補正が完了している場合は（ステップＳ１４７−Ｙｅｓ）、次のステップへ進む。すべてのセンサユニットＳ_ｉについて位置補正が完了していない場合は（ステップＳ１４７−Ｎｏ）、ステップＳ１４１に戻り、センサ位置順次補正部５７により、未補正センサ群７７の中で最も基端側のセンサユニットＳ_ｉの位置補正が行われる。すなわち、ステップＳ１４１〜Ｓ１４６が、すべてのセンサユニットＳ_ｉについて位置補正が完了するまで、繰り返し行われる。

そこで、上記構成の内視鏡形状検出装置１及び内視鏡形状検出装置１を用いた内視鏡１０の挿入部１１の形状検出方法では、第１の実施形態と同様の効果に加えて、以下の効果を奏する。すなわち、内視鏡形状検出装置１では、センサ位置順次補正部５７によりそれぞれのセンサユニットＳ_ｉの位置を補正する際に、センサ移動部６１がセンサユニットＳ_ｉを移動前位置Ｑ_{ｔ−１，ｉ}から移動後位置Ｑ_ｔ，ｉまで移動する。そして、センサ移動部６１によりセンサユニットＳ_ｉの移動が１回行われるたびに、移動後円弧形成部６２により、センサユニットＳ_ｉに対して基端側に隣設されるセンサユニットＳ_ｉ−１の最終位置Ｐ_{Ｆ，ｉ−１}とセンサユニットＳ_ｉの移動後位置Ｑ_ｔ，ｉとの間が曲線補間され、移動後円弧Ｌ_ｔ，ｊが形成される。センサ移動部６１は、移動前円弧Ｌ_{ｔ−１，ｊ}の弧長Ｒ_{ｔ−１，ｊ}φ_{ｔ−１，ｊ}とセンサ間寸法ｌとの差の絶対値に対して、移動後円弧Ｌ_ｔ，ｊの弧長Ｒ_ｔ，ｊφ_ｔ，ｊとセンサ間寸法ｌとの差の絶対値が小さくなる状態に、補正対象センサ８１であるセンサユニットＳ_ｉの移動を行っている。そして、移動後円弧Ｌ_ｔ，ｊが形成された後、移動制御部６３によりセンサユニットＳ_ｉの移動が所定の回数だけ行われたか判断される。センサユニットＳ_ｉの移動が所定の回数だけ行われていない場合は、センサ移動部６１がセンサユニットＳ_ｉを位置Ｑ_ｔ，ｉからさらに移動させる。センサユニットＳ_ｉの移動が所定の回数だけ行われた場合は、最終位置決定部６４が、センサ移動部６１によるセンサユニットＳ_ｉの最終回の移動での移動後位置Ｑ_Ｆ，ｉを、補正対象センサ８１であるセンサユニットＳ_ｉの最終位置Ｐ_Ｆ，ｉとして決定する。すなわち、センサユニットＳ_ｉの移動が所定の回数だけ行われるまで、センサ移動部６１によるセンサユニットＳ_ｉの移動が繰り返される。以上のように、それぞれのセンサユニットＳ_ｉの間の実際の寸法であるセンサ間寸法ｌに基づいて、センサ位置順次補正部５７によるそれぞれのセンサユニットＳ_ｉの位置補正が行われるため、実際の挿入部１１の形状と誤差の少ない最終曲線形状８５を検出することができる。これにより、高い精度で挿入部１１の最終曲線形状８５を検出することができる。

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

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

を用いる。ここで、式（４３．２）のベクトルはローカル座標系Ｃ_ｋ−１の原点での長手方向であるＸ_ｋ−１軸方向の単位ベクトルであり、式（４４．２）のベクトルはローカル座標系Ｃ_ｋ−１の原点での長手方向であるＺ_ｋ−１軸方向の単位ベクトルである。

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

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

さらに、上述の実施形態では、グローバル座標系Ｃは、最も基端側のセンサユニットＳ_０の中心を原点としているが、最も先端側のセンサユニットＳ_Ｎの中心を原点としてもよい。この場合、仮リンク形成部４１は、式（１６．１），式（１６．２）のｋをｊ＋１で置換した式を用いて、仮リンクＴ_０，ｊの先端側（グローバル座標系Ｃの原点から近い側）のセンサユニットＳ_ｊから仮リンクＴ_０，ｊの基端側（グローバル座標系Ｃの原点から遠い側）のセンサユニットＳ_ｊ−１へのベクトルを求める。そして、センサユニットＳ_ｊからセンサユニットＳ_ｊ−１へのベクトルにより、仮リンクＴ_０，ｊが形成される。すなわち、仮リンク形成部４１は、仮リンクＴ_０，ｊが先端側（グローバル座標系Ｃの原点から近い側）のセンサユニットＳ_ｊの中心から基端側（グローバル座標系Ｃの原点から遠い側）のセンサユニットＳ_ｊ−１の中心まで先端側のセンサユニットＳ_ｊの中心での長手方向に延設されると仮定して、仮リンクＴ_０，ｊを形成する。また、センサ位置修正部５５のセンサ位置順次補正部５７によりそれぞれのセンサユニットＳ_ｉの位置補正を行う際は、先端側（グローバル座標系Ｃの原点から近い側）のセンサユニットＳ_ｉから順次に位置補正が行われる。この場合、補正対象センサ８１の先端側（グローバル座標系Ｃの原点に近い側）に隣設されるセンサユニットＳ_ｉが近位側隣設センサ８２となる。

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

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

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

Claims (14)

1. 長手方向に互いに所定のセンサ間寸法だけ離れて複数のセンサユニットが配置される挿入部を備える内視鏡と、
   前記センサユニットの計測データに基づいて、それぞれの前記センサユニットの姿勢を検出する姿勢検出部と、
   前記姿勢検出部により検出されたそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間を寸法が前記センサ間寸法に等しい直線状の仮リンクと仮定して、それぞれの前記センサユニットの仮位置を検出するセンサ仮位置検出部と、
   前記センサ仮位置検出部により検出されたそれぞれの前記センサユニットの前記仮位置に基づいて、それぞれの前記センサユニットの前記仮位置の間を仮円弧で曲線補間を行い、前記挿入部の仮曲線形状を検出する仮曲線形状検出部と、
   前記仮曲線形状検出部により検出された前記仮曲線形状のそれぞれの前記仮円弧の弧長と前記センサ間寸法との差の絶対値に基づいて、それぞれの前記センサユニットの位置を前記仮位置から最終位置まで修正するセンサ位置修正部と、
   それぞれの前記センサユニットの前記最終位置の間を最終円弧で曲線補間し、前記挿入部の最終曲線形状を検出する最終曲線形状検出部と、
   を具備する内視鏡形状検出装置。
2. 前記仮曲線形状検出部は、
   それぞれの前記センサユニットの前記仮位置での単位接ベクトルを算出する単位接ベクトル算出部と、
   前記単位接ベクトル算出部で算出されたそれぞれの前記単位接ベクトルに基づいて、それぞれの前記センサユニットの前記仮位置の間での前記単位接ベクトルの変化率を算出する変化率算出部と、
   前記単位接ベクトル算出部で算出されたそれぞれの前記単位接ベクトル、及び、前記変化率算出部で算出されたそれぞれの前記変化率に基づいて、それぞれの前記センサユニットの前記仮位置の間の前記仮円弧を形成する仮円弧形成部と、
   を備える請求項１の内視鏡形状検出装置。
3. 前記センサ位置修正部は、
   最も基端側又は最も先端側の前記センサユニットの中心を原点とするグローバル座標系において、前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に位置補正を行い、それぞれの前記センサユニットを位置補正前の補正前位置から前記最終位置へ位置補正するセンサ位置順次補正部と、
   前記センサ位置順次補正部により１つの前記センサユニットの位置補正が行われるたびに、直前の位置補正で補正対象だった前記センサユニットである直前補正対象センサの前記補正前位置から前記最終位置への補正量だけ、位置補正が完了していない前記センサユニットである未補正センサ群を平行移動させる未補正センサ群移動部と、
   を備える請求項１の内視鏡形状検出装置。
4. 前記センサ位置順次補正部は、
   位置補正の対象である補正対象センサを前記補正前位置から１回以上移動させ、１回の移動により前記補正対象センサを移動前位置から移動後位置まで移動させるセンサ移動部と、
   前記センサ移動部により前記補正対象センサの移動が１回行われるたびに、前記補正対象センサに対して前記グローバル座標系の前記原点に近い側に隣設される前記センサユニットでるある近位側隣設センサの前記最終位置と前記補正対象センサの前記移動後位置との間を曲線補間し、移動後円弧を形成する移動後円弧形成部と、
   を備える請求項３の内視鏡形状検出装置。
5. 前記センサ位置順次補正部は、
   前記移動後円弧形成部により形成される前記移動後円弧の弧長と前記センサ間寸法との差の絶対値が所定の閾値以下になるまで、前記補正対象センサの前記移動前位置から前記移動後位置への移動を繰り返し行う状態に前記センサ移動部を制御する移動制御部と、
   前記センサ移動部による最終回の移動での前記移動後位置を、前記補正対象センサの前記最終位置として決定する最終位置決定部と、
   を備える請求項４の内視鏡形状検出装置。
6. 前記センサ位置順次補正部は、
   前記近位側隣設センサの前記最終位置と前記補正対象センサの前記移動前位置との間を曲線補間して形成される移動前円弧の弧長と前記センサ間寸法との差の絶対値に対して、前記移動後円弧の弧長と前記センサ間寸法との差の絶対値が小さくなる前記補正対象センサの移動を所定の回数だけ繰り返す状態に、前記センサ移動部を制御する移動制御部と、
   前記センサ移動部による最終回の移動での前記移動後位置を、前記補正対象センサの前記最終位置として決定する最終位置決定部と、
   を備える請求項４の内視鏡形状検出装置。
7. 前記最終曲線形状検出部は、
   それぞれの前記センサユニットの前記最終位置に基づいて、それぞれの前記最終円弧のパラメータを算出するパラメータ算出部と、
   前記パラメータ算出部により算出された前記パラメータに基づいて、クォータニオン及び媒介変数を用いた補間関数により、前記媒介変数の変化に対して等角速度でそれぞれの前記センサユニットの前記最終位置の間を曲線補間し、前記最終円弧を形成する最終円弧形成部と、
   を備える請求項１の内視鏡形状検出装置。
8. 内視鏡の挿入部に、長手方向に互いに所定のセンサ間寸法だけ離れて配置される複数のセンサユニットで計測を行うことと、
   前記センサユニットでの計測データに基づいて、それぞれの前記センサユニットの姿勢を検出することと、
   検出されたそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間を寸法が前記センサ間寸法に等しい直線状の仮リンクと仮定して、それぞれの前記センサユニットの仮位置を検出することと、
   検出されたそれぞれの前記センサユニットの前記仮位置に基づいて、それぞれの前記センサユニットの前記仮位置の間を仮円弧で曲線補間を行い、前記挿入部の仮曲線形状を検出することと、
   検出された前記仮曲線形状のそれぞれの前記仮円弧の弧長と前記センサ間寸法との差の絶対値に基づいて、それぞれの前記センサユニットの位置を前記仮位置から最終位置まで修正することと、
   それぞれの前記センサユニットの前記最終位置の間を最終円弧で曲線補間し、前記挿入部の最終曲線形状を検出することと、
   を具備する内視鏡の挿入部の形状検出方法。
9. 前記仮曲線形状を検出することは、
   それぞれの前記センサユニットの前記仮位置での単位接ベクトルを算出することと、
   算出されたそれぞれの前記単位接ベクトルに基づいて、それぞれの前記センサユニットの前記仮位置の間での前記単位接ベクトルの変化率を算出することと、
   算出されたそれぞれの前記単位接ベクトル及びそれぞれの前記変化率に基づいて、それぞれの前記センサユニットの前記仮位置の間の前記仮円弧を形成することと、
   を備える請求項８の内視鏡の挿入部の形状検出方法。
10. それぞれの前記センサユニットの位置を前記仮位置から前記最終位置まで修正することは、
    最も基端側又は最も先端側の前記センサユニットの中心を原点とするグローバル座標系において、前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に位置補正を行い、それぞれの前記センサユニットを位置補正前の補正前位置から前記最終位置へ位置補正することと、
    １つの前記センサユニットの位置補正が行われるたびに、直前の位置補正で補正対象だった前記センサユニットである直前補正対象センサの前記補正前位置から前記最終位置への補正量だけ、位置補正が完了していない前記センサユニットである未補正センサ群を平行移動させることと、
    を備える請求項８の内視鏡の挿入部の形状検出方法。
11. 前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に、それぞれの前記センサユニットの位置補正を行うことは、
    位置補正の対象である補正対象センサを前記補正前位置から１回以上移動させ、１回の移動により前記補正対象センサを移動前位置から移動後位置まで移動させることと、
    前記補正対象センサの移動が１回行われるたびに、前記補正対象センサに対して前記グローバル座標系の前記原点に近い側に隣設される前記センサユニットでるある近位側隣設センサの前記最終位置と前記補正対象センサの前記移動後位置との間を曲線補間し、移動後円弧を形成することと、
    を備える請求項１０の内視鏡の挿入部の形状検出方法。
12. 前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に、それぞれの前記センサユニットの位置補正を行うことは、
    形成される前記移動後円弧の弧長と前記センサ間寸法との差の絶対値が所定の閾値以下になるまで、前記補正対象センサの前記移動前位置から前記移動後位置への移動を繰り返し行うことと、
    最終回の移動での前記移動後位置を、前記補正対象センサの前記最終位置として決定することと、
    を備える請求項１１の内視鏡の挿入部の形状検出方法。
13. 前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に、それぞれの前記センサユニットの位置補正を行うことは、
    前記近位側隣設センサの前記最終位置と前記補正対象センサの前記移動前位置との間を曲線補間して形成される移動前円弧の弧長と前記センサ間寸法との差の絶対値に対して、前記移動後円弧の弧長と前記センサ間寸法との差の絶対値が小さくなる前記補正対象センサの移動を所定の回数だけ繰り返すことと、
    最終回の移動での前記移動後位置を、前記補正対象センサの前記最終位置として決定することと、
    を備える請求項１１の内視鏡の挿入部の形状検出方法。
14. 前記挿入部の最終曲線形状を検出することは、
    それぞれの前記センサユニットの前記最終位置に基づいて、それぞれの前記最終円弧のパラメータを算出することと、
    算出された前記パラメータに基づいて、クォータニオン及び媒介変数を用いた補間関数により、前記媒介変数の変化に対して等角速度でそれぞれの前記センサユニットの前記最終位置の間を曲線補間し、前記最終円弧を形成することと、
    を備える請求項８の内視鏡の挿入部の形状検出方法。

Images