(57)【要約】  内視鏡形状検出装置は、それぞれのセンサユニットの姿勢に基づいて、それぞれの前記センサユニットの間を寸法がセンサ間寸法に等しい直線状の仮リンクと仮定して、それぞれの前記センサユニットの仮位置を検出するセンサ仮位置検出部と、それぞれの前記センサユニットの前記仮位置に基づいて、それぞれの前記センサユニットの前記仮位置の間を仮円弧で曲線補間を行い、前記挿入部の仮曲線形状を検出する仮曲線形状検出部と、を備える。内視鏡形状検出装置は、それぞれの前記仮円弧の弧長と前記センサ間寸法との差の絶対値に基づいて、それぞれの前記センサユニットの位置を前記仮位置から最終位置まで修正するセンサ位置修正部と、それぞれの前記センサユニットの前記最終位置の間を最終円弧で曲線補間し、前記挿入部の最終曲線形状を検出する最終曲線形状検出部と、を備える。
2012026184.tif 000002
【特許請求の範囲】
【請求項１】
長手方向に互いに所定のセンサ間寸法だけ離れて複数のセンサユニットが配置される挿入部を備える内視鏡と、
前記センサユニットの計測データに基づいて、それぞれの前記センサユニットの姿勢を検出する姿勢検出部と、
前記姿勢検出部により検出されたそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間を寸法が前記センサ間寸法に等しい直線状の仮リンクと仮定して、それぞれの前記センサユニットの仮位置を検出するセンサ仮位置検出部と、
前記センサ仮位置検出部により検出されたそれぞれの前記センサユニットの前記仮位置に基づいて、それぞれの前記センサユニットの前記仮位置の間を仮円弧で曲線補間を行い、前記挿入部の仮曲線形状を検出する仮曲線形状検出部と、
前記仮曲線形状検出部により検出された前記仮曲線形状のそれぞれの前記仮円弧の弧長と前記センサ間寸法との差の絶対値に基づいて、それぞれの前記センサユニットの位置を前記仮位置から最終位置まで修正するセンサ位置修正部と、
それぞれの前記センサユニットの前記最終位置の間を最終円弧で曲線補間し、前記挿入部の最終曲線形状を検出する最終曲線形状検出部と、
を具備する内視鏡形状検出装置。
【請求項２】
前記仮曲線形状検出部は、
それぞれの前記センサユニットの前記仮位置での単位接ベクトルを算出する単位接ベクトル算出部と、
前記単位接ベクトル算出部で算出されたそれぞれの前記単位接ベクトルに基づいて、それぞれの前記センサユニットの前記仮位置の間での前記単位接ベクトルの変化率を算出する変化率算出部と、
前記単位接ベクトル算出部で算出されたそれぞれの前記単位接ベクトル、及び、前記変化率算出部で算出されたそれぞれの前記変化率に基づいて、それぞれの前記センサユニットの前記仮位置の間の前記仮円弧を形成する仮円弧形成部と、
を備える請求項１の内視鏡形状検出装置。
【請求項３】
前記センサ位置修正部は、
最も基端側又は最も先端側の前記センサユニットの中心を原点とするグローバル座標系において、前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に位置補正を行い、それぞれの前記センサユニットを位置補正前の補正前位置から前記最終位置へ位置補正するセンサ位置順次補正部と、
前記センサ位置順次補正部により１つの前記センサユニットの位置補正が行われるたびに、直前の位置補正で補正対象だった前記センサユニットである直前補正対象センサの前記補正前位置から前記最終位置への補正量だけ、位置補正が完了していない前記センサユニットである未補正センサ群を平行移動させる未補正センサ群移動部と、
を備える請求項１の内視鏡形状検出装置。
【請求項４】
前記センサ位置順次補正部は、
位置補正の対象である補正対象センサを前記補正前位置から１回以上移動させ、１回の移動により前記補正対象センサを移動前位置から移動後位置まで移動させるセンサ移動部と、
前記センサ移動部により前記補正対象センサの移動が１回行われるたびに、前記補正対象センサに対して前記グローバル座標系の前記原点に近い側に隣設される前記センサユニットでるある近位側隣設センサの前記最終位置と前記補正対象センサの前記移動後位置との間を曲線補間し、移動後円弧を形成する移動後円弧形成部と、
を備える請求項３の内視鏡形状検出装置。
【請求項５】
前記センサ位置順次補正部は、
前記移動後円弧形成部により形成される前記移動後円弧の弧長と前記センサ間寸法との差の絶対値が所定の閾値以下になるまで、前記補正対象センサの前記移動前位置から前記移動後位置への移動を繰り返し行う状態に前記センサ移動部を制御する移動制御部と、
前記センサ移動部による最終回の移動での前記移動後位置を、前記補正対象センサの前記最終位置として決定する最終位置決定部と、
を備える請求項４の内視鏡形状検出装置。
【請求項６】
前記センサ位置順次補正部は、
前記近位側隣設センサの前記最終位置と前記補正対象センサの前記移動前位置との間を曲線補間して形成される移動前円弧の弧長と前記センサ間寸法との差の絶対値に対して、前記移動後円弧の弧長と前記センサ間寸法との差の絶対値が小さくなる前記補正対象センサの移動を所定の回数だけ繰り返す状態に、前記センサ移動部を制御する移動制御部と、
前記センサ移動部による最終回の移動での前記移動後位置を、前記補正対象センサの前記最終位置として決定する最終位置決定部と、
を備える請求項４の内視鏡形状検出装置。
【請求項７】
前記最終曲線形状検出部は、
それぞれの前記センサユニットの前記最終位置に基づいて、それぞれの前記最終円弧のパラメータを算出するパラメータ算出部と、
前記パラメータ算出部により算出された前記パラメータに基づいて、クォータニオン及び媒介変数を用いた補間関数により、前記媒介変数の変化に対して等角速度でそれぞれの前記センサユニットの前記最終位置の間を曲線補間し、前記最終円弧を形成する最終円弧形成部と、
を備える請求項１の内視鏡形状検出装置。
【請求項８】
内視鏡の挿入部に、長手方向に互いに所定のセンサ間寸法だけ離れて配置される複数のセンサユニットで計測を行うことと、
前記センサユニットでの計測データに基づいて、それぞれの前記センサユニットの姿勢を検出することと、
検出されたそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間を寸法が前記センサ間寸法に等しい直線状の仮リンクと仮定して、それぞれの前記センサユニットの仮位置を検出することと、
検出されたそれぞれの前記センサユニットの前記仮位置に基づいて、それぞれの前記センサユニットの前記仮位置の間を仮円弧で曲線補間を行い、前記挿入部の仮曲線形状を検出することと、
検出された前記仮曲線形状のそれぞれの前記仮円弧の弧長と前記センサ間寸法との差の絶対値に基づいて、それぞれの前記センサユニットの位置を前記仮位置から最終位置まで修正することと、
それぞれの前記センサユニットの前記最終位置の間を最終円弧で曲線補間し、前記挿入部の最終曲線形状を検出することと、
を具備する内視鏡の挿入部の形状検出方法。
【請求項９】
前記仮曲線形状を検出することは、
それぞれの前記センサユニットの前記仮位置での単位接ベクトルを算出することと、
算出されたそれぞれの前記単位接ベクトルに基づいて、それぞれの前記センサユニットの前記仮位置の間での前記単位接ベクトルの変化率を算出することと、
算出されたそれぞれの前記単位接ベクトル及びそれぞれの前記変化率に基づいて、それぞれの前記センサユニットの前記仮位置の間の前記仮円弧を形成することと、
を備える請求項８の内視鏡の挿入部の形状検出方法。
【請求項１０】
それぞれの前記センサユニットの位置を前記仮位置から前記最終位置まで修正することは、
最も基端側又は最も先端側の前記センサユニットの中心を原点とするグローバル座標系において、前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に位置補正を行い、それぞれの前記センサユニットを位置補正前の補正前位置から前記最終位置へ位置補正することと、
１つの前記センサユニットの位置補正が行われるたびに、直前の位置補正で補正対象だった前記センサユニットである直前補正対象センサの前記補正前位置から前記最終位置への補正量だけ、位置補正が完了していない前記センサユニットである未補正センサ群を平行移動させることと、
を備える請求項８の内視鏡の挿入部の形状検出方法。
【請求項１１】
前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に、それぞれの前記センサユニットの位置補正を行うことは、
位置補正の対象である補正対象センサを前記補正前位置から１回以上移動させ、１回の移動により前記補正対象センサを移動前位置から移動後位置まで移動させることと、
前記補正対象センサの移動が１回行われるたびに、前記補正対象センサに対して前記グローバル座標系の前記原点に近い側に隣設される前記センサユニットでるある近位側隣設センサの前記最終位置と前記補正対象センサの前記移動後位置との間を曲線補間し、移動後円弧を形成することと、
を備える請求項１０の内視鏡の挿入部の形状検出方法。
【請求項１２】
前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に、それぞれの前記センサユニットの位置補正を行うことは、
形成される前記移動後円弧の弧長と前記センサ間寸法との差の絶対値が所定の閾値以下になるまで、前記補正対象センサの前記移動前位置から前記移動後位置への移動を繰り返し行うことと、
最終回の移動での前記移動後位置を、前記補正対象センサの前記最終位置として決定することと、
を備える請求項１１の内視鏡の挿入部の形状検出方法。
【請求項１３】
前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に、それぞれの前記センサユニットの位置補正を行うことは、
前記近位側隣設センサの前記最終位置と前記補正対象センサの前記移動前位置との間を曲線補間して形成される移動前円弧の弧長と前記センサ間寸法との差の絶対値に対して、前記移動後円弧の弧長と前記センサ間寸法との差の絶対値が小さくなる前記補正対象センサの移動を所定の回数だけ繰り返すことと、
最終回の移動での前記移動後位置を、前記補正対象センサの前記最終位置として決定することと、
を備える請求項１１の内視鏡の挿入部の形状検出方法。
【請求項１４】
前記挿入部の最終曲線形状を検出することは、
それぞれの前記センサユニットの前記最終位置に基づいて、それぞれの前記最終円弧のパラメータを算出することと、
算出された前記パラメータに基づいて、クォータニオン及び媒介変数を用いた補間関数により、前記媒介変数の変化に対して等角速度でそれぞれの前記センサユニットの前記最終位置の間を曲線補間し、前記最終円弧を形成することと、
を備える請求項８の内視鏡の挿入部の形状検出方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、体腔内に挿入される内視鏡を備える内視鏡形状検出装置及びその内視鏡形状検出装置の内視鏡の挿入部の形状検出方法に関する。
【背景技術】
【０００２】
近年、内視鏡の挿入部の形状を検出可能な内視鏡形状検出装置が実用化されている。特許文献１には、体腔内に挿入される内視鏡の挿入部に複数のソースコイルを取付けた内視鏡形状検出装置が開示されている。この内視鏡形状検出装置では、体外に設けられるセンスコイルにより、それぞれのソースコイルの位置が検出される。そして、検出されたソースコイルの位置に基づいて、内視鏡の挿入部の形状が検出される。【０００３】
また、特許文献２には、内視鏡の挿入部に配置されたそれぞれのコイルの位置を交流磁場から検出し、検出されたそれぞれのコイルの位置の間を曲線補間する内視鏡形状検出装置が開示されている。この内視鏡形状検出装置では、検出されたそれぞれのコイルの位置をベジェ曲線又はスプライン曲線で結ぶことにより、曲線補間を行っている。曲線補間を行うことにより、内視鏡の挿入部の曲線形状が検出される。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０００−１７５８６２号公報
【特許文献２】特開２００７−１３０１７５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
上記特許文献１の内視鏡形状検出装置では、体腔内のソースコイルの位置を体外のセンスコイルで検出するため、検出装置が大型化するとともに、装置の構成が複雑化する。【０００６】
上記特許文献２の内視鏡形状検出装置では、検出されたそれぞれのコイルの位置をベジェ曲線又はスプライン曲線で結ぶことにより、曲線補間を行っている。しかし、この内視鏡形状検出装置では、それぞれのコイルの間の長手方向の寸法であるコイル間寸法を考慮して曲線補間を行っていない。このため、体腔内への挿入時の挿入部の形状は、高い精度で検出されない。【０００７】
例えば検出されたそれぞれのコイルの位置をスプライン曲線で結んで、挿入部の仮形状を検出した後に、コイル間寸法に基づいて、検出されたそれぞれのコイルの位置を順次に修正することも考えられる。この場合、１つのコイルの位置の修正が行われるたびに、位置修正後のコイルの位置をスプライン曲線で結び、コイル位置修正前の挿入部の検出形状を修正したコイル位置修正後の挿入部の修正形状が検出される。すなわち、１つのコイルの位置修正が行われるたびに、挿入部の曲線形状全体が修正される。ここで、コイル間寸法は、コイルとコイルの間の長手方向の寸法であり、挿入部の局所的なパラメータである。すなわち、コイル間寸法は、挿入部の形状に局所的に影響を与えるパラメータであり、挿入部全体の形状に影響を与えるパラメータではない。このため、１つのコイルの位置の修正により、位置修正されたコイルの近傍で局所的にセンサ位置修正前の挿入部の検出形状が修正されるのではなく、１つのコイルの位置の修正により、挿入部の曲線形状全体が修正される構成では、高い精度で挿入部の曲線形状の修正が行われない。したがって、最終的に挿入部の曲線形状が、高い精度で検出されない。また、スプライン曲線等の種類によっては１つのコイルの位置の修正により、挿入部の全体形状が修正されるため、処理が複雑になる。【０００８】
本発明は上記課題に着目してなされたものであり、その目的とするところは、装置の構成が大型化、複雑化することなく、かつ、処理が複雑化することなく、高い精度で挿入部の形状を検出可能な内視鏡形状検出装置及び内視鏡の挿入部の形状検出方法を提供することにある。
【課題を解決するための手段】
【０００９】
上記目的を達成するため、本発明のある態様では、長手方向に互いに所定のセンサ間寸法だけ離れて複数のセンサユニットが配置される挿入部を備える内視鏡と、前記センサユニットの計測データに基づいて、それぞれの前記センサユニットの姿勢を検出する姿勢検出部と、前記姿勢検出部により検出されたそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間を寸法が前記センサ間寸法に等しい直線状の仮リンクと仮定して、それぞれの前記センサユニットの仮位置を検出するセンサ仮位置検出部と、前記センサ仮位置検出部により検出されたそれぞれの前記センサユニットの前記仮位置に基づいて、それぞれの前記センサユニットの前記仮位置の間を仮円弧で曲線補間を行い、前記挿入部の仮曲線形状を検出する仮曲線形状検出部と、前記仮曲線形状検出部により検出された前記仮曲線形状のそれぞれの前記仮円弧の弧長と前記センサ間寸法との差の絶対値に基づいて、それぞれの前記センサユニットの位置を前記仮位置から最終位置まで修正するセンサ位置修正部と、それぞれの前記センサユニットの前記最終位置の間を最終円弧で曲線補間し、前記挿入部の最終曲線形状を検出する最終曲線形状検出部と、を備える内視鏡形状検出装置を提供する。【００１０】
また、本発明の別のある態様では、内視鏡の挿入部に、長手方向に互いに所定のセンサ間寸法だけ離れて配置される複数のセンサユニットで計測を行うことと、前記センサユニットでの計測データに基づいて、それぞれの前記センサユニットの姿勢を検出することと、  検出されたそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間を寸法が前記センサ間寸法に等しい直線状の仮リンクと仮定して、それぞれの前記センサユニットの仮位置を検出することと、検出されたそれぞれの前記センサユニットの前記仮位置に基づいて、それぞれの前記センサユニットの前記仮位置の間を仮円弧で曲線補間を行い、前記挿入部の仮曲線形状を検出することと、検出された前記仮曲線形状のそれぞれの前記仮円弧の弧長と前記センサ間寸法との差の絶対値に基づいて、それぞれの前記センサユニットの位置を前記仮位置から最終位置まで修正することと、それぞれの前記センサユニットの前記最終位置の間を最終円弧で曲線補間し、前記挿入部の最終曲線形状を検出することと、を備える内視鏡の挿入部の形状検出方法を提供する。
【発明の効果】
【００１１】
本発明によれば、装置の構成を大型化、複雑化することなく、かつ、処理が複雑化することなく、高い精度で挿入部の形状を検出可能な内視鏡形状検出装置及び内視鏡の挿入部の形状検出方法を提供することができる。
【図面の簡単な説明】
【００１２】
【図１】本発明の第１の実施形態に係る内視鏡形状検出装置の構成を示すブロック図。
【図２】第１の実施形態に係る内視鏡の挿入部の構成を示す概略図。
【図３】第１の実施形態に係る内視鏡形状検出装置のパソコンの構成を示すブロック図。
【図４】第１の実施形態に係る内視鏡の挿入部の静状態での形状を検出する方法を示すフローチャート。
【図５】第１の実施形態に係る内視鏡形状検出装置のグローバル座標系と補正座標系とを比較して示す概略図。
【図６】第１の実施形態に係る内視鏡形状検出装置のセンサ仮位置検出部により検出されるそれぞれのセンサユニットの仮位置を示す概略図。
【図７】第１の実施形態に係るセンサ仮位置検出部の仮リンク移動部での処理を説明する概略図。
【図８】第１の実施形態に係る内視鏡形状検出装置の仮曲線形状検出部により検出された内視鏡の挿入部の仮曲線形状を示す概略図。
【図９】第１の実施形態に係る仮曲線形状検出部により仮曲線形状を検出する方法を示すフローチャート。
【図１０】第１の実施形態に係る仮曲線形状検出部での処理を説明する概略図。
【図１１】第１の実施形態に係る内視鏡形状検出装置のセンサ位置修正部により修正されたそれぞれのセンサユニットの最終位置を示す概略図。
【図１２】第１の実施形態に係るセンサ位置修正部によりそれぞれのセンサユニットの位置を修正する方法を示すフローチャート。
【図１３】第１の実施形態に係るセンサ位置修正部のセンサ位置順次補正部による処理を説明する概略図。
【図１４】第１の実施形態に係るセンサ位置順次補正部のセンサ移動部及び移動後円弧形成部での処理を説明する概略図。
【図１５】第１の実施形態に係るセンサ位置修正部の未補正センサ群移動部での処理を説明する概略図。
【図１６】第１の実施形態に係る内視鏡形状検出装置の最終曲線形状検出部により検出された内視鏡の挿入部の最終曲線形状を示す概略図。
【図１７】第１の実施形態に係る最終曲線形状検出部により最終曲線形状を検出する方法を示すフローチャート。
【図１８】第１の実施形態に係る最終曲線形状検出部での処理を説明する概略図。
【図１９】本発明の第２の実施形態に係る内視鏡形状検出装置のセンサ位置修正部によりそれぞれのセンサユニットの位置を修正する方法を示すフローチャート。
【発明を実施するための形態】
【００１３】
（第１の実施形態）  
本発明の第１の実施形態について図１乃至図１８を参照して説明する。
【００１４】
図１は、本実施形態の内視鏡形状検出装置１を示す図である。図１に示すように、内視鏡形状検出装置１の内視鏡１０は、体腔内に挿入される挿入部１１と、挿入部１１の基端側に設けられる操作部１２とを備える。挿入部１１は、最先端に設けられる先端硬性部１４と、先端硬性部１４の基端側に設けられる湾曲部１６と、湾曲部１６の基端側に設けられる細長い可撓管部１８とを備える。
【００１５】
先端硬性部１４の内部には、被写体の撮像を行うＣＣＤ等の撮像素子２０が設けられている。撮像素子２０には、撮像用信号線２１の一端が接続されている。撮像用信号線２１は、挿入部１１の内部を通って操作部１２から内視鏡１０の外部に延出され、他端が画像処理ユニットであるビデオプロセッサ３に接続されている。また、挿入部１１の内部には、先端硬性部１４の照明窓（図示しない）に被写体を照射する照明光を導光するライトガイド２３が、長手方向に延設されている。ライトガイド２３は、操作部１２から内視鏡１０の外部に延出され、光源ユニット４に接続されている。
【００１６】
また、挿入部１１の湾曲部１６の先端部には、湾曲操作伝達部材である４本の湾曲操作ワイヤ（図示しない）の一端が接続されている。湾曲操作ワイヤは、可撓管部１８の内部を通って、操作部１２に設けられる湾曲操作部である湾曲操作ノブ（図示しない）に他端が接続されている。湾曲操作ノブでの操作により、湾曲操作ワイヤが長手方向に移動する。湾曲操作の移動により、湾曲部１６が内視鏡１０の上下方向及び左右方向に湾曲動作を行う。
【００１７】
挿入部１１には、複数（本実施形態ではＮ＋１個）のセンサユニットＳ０〜ＳＮが設けられている。それぞれのセンサユニットＳｉ（ｉ＝０，１，２，…，Ｎ）は、互いに長手方向に一定の間隔ｌ（＝５０ｍｍ）だけ離れて配置されている。すなわち、それぞれのセンサユニットＳｉは、互いに長手方向に所定のセンサ間寸法ｌだけ離れて配置されている。ここで、例えば最も基端側のセンサユニットＳ０が可撓管部１８の基端部に配置され、最も先端側のセンサユニットＳＮが湾曲部１６の先端部に配置されている。センサユニットＳｉは、加速度を計測する加速度センサＡｉと、地磁気を計測する地磁気センサＢｉとを備える。
【００１８】
図２は、内視鏡１０の挿入部１１を示す図である。図２に示すように、それぞれのセンサユニットＳｉは、センサユニットＳｉの中心を原点とし、Ｘｉ軸、Ｙｉ軸、Ｚｉ軸を有するローカル座標系Ｃｉ（図２で点線で示す）を有する。ここで、Ｘｉ軸方向は、センサユニットＳｉの中心での内視鏡１０の左右方向と一致し、基端側から視た際の内視鏡１０の右方向を正とする。Ｙｉ軸方向は、センサユニットＳｉの中心での長手方向と一致し、先端方向を正とする。Ｚｉ軸方向は、センサユニットＳｉの中心での内視鏡１０の上下方向と一致し、内視鏡１０の上方向を正とする。加速度センサＡｉは、ローカル座標系Ｃｉの原点での加速度のＸｉ軸方向成分、Ｙｉ軸方向成分、Ｚｉ軸方向成分を計測する。地磁気センサＢｉは、ローカル座標系Ｃｉの原点での地磁気のＸｉ軸方向成分、Ｙｉ軸方向成分、Ｚｉ軸方向成分を計測する。
【００１９】
また、内視鏡形状検出装置１では、最も基端側のセンサユニットＳ０の中心を原点とし、Ｘ軸、Ｙ軸、Ｚ軸を有するグローバル座標系Ｃ（図２で実線で示す）が定義されている。ここで、グローバル座標系Ｃは、最も基端側のセンサユニットＳ０の中心を原点とする右手系の直交デカルト座標系である。Ｘ軸方向は、重力が作用する鉛直方向に垂直な所定の方向（本実施形態では、図２の矢印Ｄ１，Ｄ２に平行な方向）と一致し、図２の矢印Ｄ１の方向を正とする。Ｙ軸方向は、鉛直方向に垂直で、かつ、Ｘ軸方向に垂直な方向（本実施形態では、図２の矢印Ｅ１，Ｅ２に平行な方向）と一致し、図２の矢印Ｅ１の方向を正とする。Ｚ軸方向は、鉛直方向と一致し、鉛直方向の上方向（紙面の裏から表への方向）を正とする。なお、ここでは説明の都合上、グローバル座標系ＣのＸ軸方向を磁北方向とする。
【００２０】
それぞれのローカル座標系Ｃｉは、グローバル座標系ＣをＸ軸回りにαｉ、Ｙ軸回りにβｉ、Ｚ軸回りにγｉそれぞれ回転し、最も基端側のセンサユニットＳ０の中心からセンサユニットＳｉの中心まで原点を平行移動した座標系である。ここで、αｉをピッチ角、βｉをロール角、γｉをヨー角と称し、ピッチ角αｉ、ロール角βｉ、ヨー角γｉの３つをまとめて姿勢角と称する。姿勢角αｉ，βｉ，γｉは、それぞれＸ軸、Ｙ軸、Ｚ軸の負の方向から見て時計回りを正とする。姿勢角αｉ，βｉ，γｉの値を算出することにより、センサユニットＳｉの姿勢が検出される。
【００２１】
図１に示すように、ぞれぞれのセンサユニットＳｉの加速度センサＡｉ及び地磁気センサＢｉは、Ｉ２Ｃ等のシリアルバス５に接続されている。シリアルバス５は、挿入部１１の内部を通って操作部１２から内視鏡１０の外部に延出され、基端がシリアルコンバータ６に接続されている。シリアルコンバータ６は、それぞれのセンサユニットＳｉからシリアルバス５を介して入力される計測データのシリアル信号を、ＵＳＢ信号に変換する。シリアルコンバータ６には、ＵＳＢケーブル７の一端が接続されている。ＵＳＢケーブル７の他端は、パソコン８に接続されている。パソコン８には、それぞれのセンサユニットＳｉでの計測データのＵＳＢ信号がシリアルコンバータ６から入力される。
【００２２】
図３は、パソコン８の構成を示す図である。図３に示すように、パソコン８は、ＵＳＢケーブル７を介してシリアルコンバータ６に接続される通信部２６を備える。通信部２６は、それぞれのセンサユニットＳｉでの計測データを受信する。通信部２６には、物理量換算部２８が接続されている。物理量換算部２８は、通信部２６で受信したそれぞれのセンサユニットＳｉでの計測データを、オフセット、ゲイン等を用いて物理量に換算する。
【００２３】
物理量換算部２８には、姿勢検出部３０が接続されている。姿勢検出部３０は、センサユニットＳｉでの計測データに基づいて、それぞれのセンサユニットＳｉの姿勢を検出する。姿勢検出部３０は、それぞれのセンサユニットＳｉの加速度センサＡｉ及び地磁気センサＢｉでの計測データに基づいて、それぞれのセンサユニットＳｉのローカル座標系Ｃｉのグローバル座標系ＣからのＸ軸回り、Ｙ軸回り、Ｚ軸回りの回転角である３つの姿勢角αｉ，βｉ，γｉを算出する姿勢角算出部３２を備える。姿勢角算出部３２は、それぞれのセンサユニットＳｉの加速度センサＡｉでの加速度データに基づいて、それぞれのセンサユニットＳｉのローカル座標系Ｃｉのグローバル座標系ＣからのＸ軸回りの回転角であるピッチ角αｉ、及び、それぞれのセンサユニットＳｉのローカル座標系Ｃｉのグローバル座標系ＣからのＹ軸回りの回転角であるロール角βｉを算出する第１の角度算出部３４を備える。また、姿勢角算出部３２は、それぞれのセンサユニットＳｉの地磁気センサＢｉでの地磁気データに基づいて、それぞれのセンサユニットＳｉのローカル座標系Ｃｉのグローバル座標系ＣからのＺ軸回りの回転角であるヨー角γｉを算出する第２の角度算出部３６を備える。
【００２４】
ここで、姿勢検出部３０でそれぞれのセンサユニットＳｉの姿勢を検出する方法について説明する。図４は、内視鏡１０の挿入部１１が停止している静状態での挿入部１１の形状検出方法を示すフローチャートである。図４に示すように、挿入部１１の形状検出の際には、まず、それぞれのセンサユニットＳｉでの計測を行い（ステップＳ１０１）、姿勢検出部３０がそれぞれのセンサユニットＳｉでの計測データを取得する。そして、姿勢角算出部３２が、それぞれのセンサユニットＳｉのローカル座標系Ｃｉの３つの姿勢角αｉ，βｉ，γｉを算出する。
【００２５】
姿勢角αｉ，βｉ，γｉを算出する際には、まず、第１の角度算出部３４が、それぞれのセンサユニットＳｉの加速度センサＡｉでの計測データに基づいて、それぞれのセンサユニットＳｉのローカル座標系Ｃｉのピッチ角αｉ及びロール角βｉを算出する（ステップＳ１０２）。ここで、姿勢角αｉ，βｉ，γｉはヨー角γｉ、ピッチ角αｉ、ロール角βｉの順で回転する（Ｚ，Ｘ，Ｙ）型とする。したがって、ローカル座標系Ｃｉからグローバル座標系Ｃへの回転行列は、  
【数１】
2012026184.tif 000003
【００２６】
となる。
【００２７】
挿入部１１が停止している静状態では、重力加速度のみが鉛直方向の下方向に作用している。すなわち、グローバル座標系Ｃにおいても、ローカル座標系Ｃｉにおいても、鉛直方向の下方向に重力加速度のみが作用している。したがって、この際、加速度ベクトルのグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分は、  
【数２】
2012026184.tif 000004
【００２８】
となる。また、加速度センサＡｉで計測される加速度ベクトルのローカル座標系ＣｉのＸｉ軸方向成分、Ｙｉ軸方向成分、Ｚｉ軸方向成分を、  
【数３】
2012026184.tif 000005
【００２９】
とする。
【００３０】
ここで、ローカル座標系Ｃｉはグローバル座標系Ｃをヨー角γｉ、ピッチ角αｉ、ロール角βｉの順で回転した座標系である。したがって、式（１）〜式（３）より、ローカル座標系Ｃｉで観測される加速度成分は、  
【数４】
2012026184.tif 000006
【００３１】
となる。ここで、式（４．１）の２乗と式（４．３）の２乗を加算すると、  
【数５】
2012026184.tif 000007
【００３２】
となり、  
【数６】
2012026184.tif 000008
【００３３】
となる。そして、式（４．２）を式（６）で割ることにより、  
【数７】
2012026184.tif 000009
【００３４】
となり、ローカル座標系Ｃｉのピッチ角αｉが求まる。また、式（４．１）を式（４．３）で割ることにより、  
【数８】
2012026184.tif 000010
【００３５】
となり、ローカル座標系Ｃｉのロール角βｉが求まる。以上のようにして、それぞれの加速度センサＡｉでの計測データに基づいて、それぞれのローカル座標系Ｃｉのピッチ角αｉ及びロール角βｉが算出される。
【００３６】
そして、第２の角度算出部３６が、それぞれのセンサユニットＳｉの地磁気センサＢｉでの計測データに基づいて、それぞれのセンサユニットＳｉのローカル座標系Ｃｉのヨー角γｉを算出する（ステップＳ１０３）。ここで、ステップＳ１０２で算出したピッチ角αｉ及びロール角βｉを用いて、それぞれのローカル座標系Ｃｉのグローバル座標系ＣからのＸ軸回りの回転及びＹ軸回りの回転を補正した補正座標系Ｃ´ｉを定義する。図５は、グローバル座標系Ｃ（図５において実線で示す）及び補正座標系Ｃ´ｉ（図５で点線で示す）を示す図である。なお、実際はグローバル座標系Ｃと補正座標系Ｃ´ｉとでは原点の位置が異なるが、図５では両者の比較のため原点が同一の位置にある状態で示す。図５に示すように、Ｘ軸及びＹ軸補正した補正座標系Ｃ´ｉはグローバル座標系ＣをＺ軸回りにヨー角γｉだけ回転した座標系であり、Ｘ´ｉ軸、Ｙ´ｉ軸、Ｚ´ｉ軸を有する。Ｘ´ｉ軸方向、Ｙ´ｉ軸方向は、それぞれグローバル座標系ＣのＸ軸方向、Ｙ軸方向からＺ軸回りにヨー角γｉだけ回転した方向と一致する。Ｚ´ｉ軸方向は、鉛直方向、すなわちグローバル座標系ＣのＺ軸方向と一致する。本実施形態では、グローバル座標系ＣのＸ軸方向が磁北方向と一致している。このため、Ｘ´ｉ軸方向が、磁北方向からＺ軸回りにヨー角γｉだけ回転した方向となる。
【００３７】
地磁気センサＢｉで計測される地磁気ベクトルのローカル座標系ＣｉのＸｉ軸方向成分、Ｙｉ軸方向成分、Ｚｉ軸方向成分を、  
【数９】
2012026184.tif 000011
【００３８】
とする。補正座標系Ｃ´ｉは、ローカル座標系Ｃｉのグローバル座標系ＣからのＸ軸回りの回転及びＹ軸回りの回転を補正した座標系である。したがって、式（９）及び式（１）のＲｘｉ，Ｒｙｉを用いて、地磁気センサＢｉで計測される地磁気ベクトルの補正座標系Ｃ´ｉのＸ´ｉ軸方向成分、Ｙ´ｉ軸方向成分、Ｚ´ｉ軸方向成分は、  
【数１０】
2012026184.tif 000012
【００３９】
となる。式（１０．１），式（１０．２）から、  
【数１１】
2012026184.tif 000013
【００４０】
となる。鉛直方向に垂直な水平面（補正座標系Ｃ´ｉのＸ´ｉ−Ｙ´ｉ平面）の地磁気成分は、磁北方向を向いている。したがって、式（１１．１），式（１１．２）より、Ｘ´ｉ軸から磁北方向までの角度θｉは、地磁気ベクトルの補正座標系Ｃ´ｉのＸ´ｉ軸成分、Ｙ´ｉ軸成分を用いて求めることができる。すなわち、  
【数１２】
2012026184.tif 000014
【００４１】
となる。角度θｉはＺ´ｉ軸（Ｚ軸）を負の方向から見て時計回りを正とする。ここで、補正座標系Ｃ´ｉはグローバル座標系ＣをＺ軸回りにヨー角γｉだけ回転した座標系である。したがって、式（１２）で求まる角度θｉが、グローバル座標系Ｃを基準としたローカル座標系Ｃｉのヨー角γｉとなる。
【００４２】
なお、グローバル座標系ＣのＸ軸方向が磁北方向と一致しない場合も、磁北を基準としてヨー角γｉを求めることができる。地磁気ベクトルのグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分を、  
【数１３】
2012026184.tif 000015
【００４３】
とする。地磁気ベクトルのグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分は、地磁気センサＢｉと同一のタイプの地磁気センサを用いて、グローバル座標系ＣのＸ軸方向、Ｙ軸方向、Ｚ軸方向と軸方向が一致する状態で計測を行うことにより、求められる。そして、式（１３）より、Ｘ軸から磁北方向までの角度θを、地磁気ベクトルのグローバル座標系ＣのＸ軸成分、Ｙ軸成分を用いて求める。すなわち、  
【数１４】
2012026184.tif 000016
【００４４】
となる。ここで、角度θはＺ軸を負の方向から見て時計回りを正とする。補正座標系Ｃ´ｉはグローバル座標系ＣをＺ軸回りにヨー角γｉだけ回転した座標系である。したがって、式（１２）、式（１４）より、
【数１５】
2012026184.tif 000017
【００４５】
となり、グローバル座標系Ｃを基準としたローカル座標系Ｃｉのヨー角γｉが求まる。
【００４６】
以上のようにして、それぞれの地磁気センサＢｉでの計測データに基づいて、それぞれのローカル座標系Ｃｉのヨー角γｉが算出される。算出された姿勢角αｉ，βｉ，γｉの値に基づいて、姿勢検出部３０はそれぞれのセンサユニットＳｉの姿勢を検出する。
【００４７】
図３に示すように、姿勢検出部３０には、センサ仮位置検出部４０が接続されている。図６は、センサ仮位置検出部４０により検出されるそれぞれのセンサユニットＳｉの仮位置Ｐ０，ｉを、グローバル座標系ＣのＺ軸の負から正の方向に向かって視た図である。図６に示すように、センサ仮位置検出部４０は、姿勢検出部３０で検出されたそれぞれのセンサユニットＳｉの姿勢に基づいて、それぞれのセンサユニットＳｉの間の形状を寸法がセンサ間寸法ｌに等しい直線状の仮リンクＴ０，ｊ（ｊ＝１，２，…Ｎ）と仮定して、それぞれのセンサユニットＳｉの仮位置Ｐ０，ｉを検出する。ここで、基端側からｋ番目の仮リンクＴ０，ｋは、基端側からｋ番目のセンサユニットＳｋ−１と基端側から（ｋ＋１）番目のセンサユニットＳｋとの間の仮リンクＴ０，ｋである。センサ仮位置検出部４０は、それぞれの仮リンクＴ０，ｊを形成する仮リンク形成部４１と、仮リンク形成部４１により形成されたそれぞれの仮リンクＴ０，ｊを平行移動する仮リンク移動部４２とを備える。仮リンク移動部４２により、それぞれの仮リンクＴ０，ｊは、隣接する仮リンクＴ０，ｊ−１，Ｔ０，ｊ＋１との仮リンク境界が連続する状態に、平行移動される。これにより、それぞれのセンサユニットＳｉの間を直線状の仮リンクＴ０，ｊと仮定した仮線形形状７１が形成される。
【００４８】
ここで、センサ仮位置検出部４０でそれぞれのセンサユニットＳｉの仮位置Ｐ０，ｉを検出する方法について説明する。それぞれのセンサユニットＳｉの仮位置Ｐ０，ｉを検出する（ステップＳ１０４）際、まず、ステップＳ１０２，Ｓ１０３で算出した姿勢角αｉ，βｉ，γｉの値に基づいて、仮リンク形成部４１が、直線形状のそれぞれの仮リンクＴ０，ｊを形成する。ここでは、基端側からｋ番目のセンサユニットＳｋ−１と基端側から（ｋ＋１）番目のセンサユニットＳｋとの間の基端側からｋ番目の仮リンクＴ０，ｋの形成について説明する。
【００４９】
式（７）、式（８）、式（１２）（又は式（１５））のｉをｋ−１で置換した式を用いて、ステップＳ１０２，Ｓ１０３でローカル座標系Ｃｋ−１（つまり仮リンクＴ０，ｋ）の姿勢角αｋ−１，βｋ−１，γｋ−１が算出される。この姿勢角αｋ−１，βｋ−１，γｋ−１及びそれぞれのセンサユニットＳｉの間の長手方向についての間隔であるセンサ間寸法ｌを用いて、センサユニットＳｋ−１からセンサユニットＳｋへ向かうベクトルが求まる。ここで、センサユニットＳｋ−１からセンサユニットＳｋへ向かうベクトルは、  
【数１６】
2012026184.tif 000018
【００５０】
となる。式（１６．１）では、ローカル座標系Ｃｋ−１の原点での長手方向であるＹｋ−１軸方向の単位ベクトルと式（１）で算出される回転行列を乗算することにより、ローカル座標系Ｃｋ−１の原点でのＹｋ−１軸方向の単位ベクトルを、グローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向にそれぞれ分解した成分が算出される。すなわち、ｌｘｋ，ｌｙｋ，ｌｚｋは、ローカル座標系Ｃｋ−１のＹｋ−１軸方向への大きさｌのベクトルを、それぞれグローバル座標系ＣのＸ軸方向、Ｙ軸方向、Ｚ軸方向に分解した成分である。式（１６．１）で算出されるベクトルにより、仮リンクＴ０，ｋが形成される。
【００５１】
なお、仮リンクＴ０，ｋ以外の仮リンクＴ０，ｊについても、仮リンク形成部４１により同様に形成される。すなわち、式（１６．１），式（１６．２）のｋをｊで置換した式を用いて、仮リンクＴ０，ｊの基端側（グローバル座標系Ｃの原点から近い側）のセンサユニットＳｊ−１から仮リンクＴ０，ｊの先端側（グローバル座標系Ｃの原点から遠い側）のセンサユニットＳｊへのベクトルを求める。そして、センサユニットＳｊ−１からセンサユニットＳｊへのベクトルにより、仮リンクＴ０，ｊが形成される。すなわち、仮リンク形成部４１は、仮リンクＴ０，ｊが基端側（グローバル座標系Ｃの原点から近い側）のセンサユニットＳｊ−１の中心から先端側（グローバル座標系Ｃの原点から遠い側）のセンサユニットＳｊの中心まで基端側のセンサユニットＳｊ−１の中心での長手方向に延設されると仮定して、仮リンクＴ０，ｊを形成する。
【００５２】
また、センサ間寸法ｌは５０ｍｍ程度であることが好ましい。センサ間寸法ｌを長くすることにより、センサユニットＳｉの数が減少し、コストが削減される。また、センサ間寸法ｌが５０ｍｍ程度より小さい範囲であれば、それぞれのセンサユニットＳｉの間を寸法がセンサ間寸法ｌに等しい直線状の仮リンクＴ０，ｊと仮定した場合でも、挿入部１１の形状検出の際の誤差を小さくすることが可能である。
【００５３】
そして、仮リンク移動部４２が、仮リンク形成部４１により形成されたそれぞれの仮リンクＴ０，ｊを隣接する仮リンクＴ０，ｊ−１，Ｔ０，ｊ＋１との仮リンク境界が連続する状態に平行移動する。図７は、仮リンク移動部４２での処理を説明する図である。ここでは、基端側からｋ番目のセンサユニットＳｋ−１と基端側から（ｋ＋１）番目のセンサユニットＳｋとの間の基端側からｋ番目の仮リンクＴ０，ｋの移動について説明する。
【００５４】
図７に示すように、仮リンク移動部４２により、仮リンクＴ０，ｋの移動が行われる前の状態では、仮リンクＴ０，ｋの基端側に隣接する仮リンクＴ０，ｋ−１まで移動が完了し、仮リンク移動完了部７３が形成されている。仮リンクＴ０，ｋの移動を行う際は、原点から仮リンク移動完了部７３の先端（すなわちセンサユニットＳｋ−１の仮位置Ｐ０，ｋ−１）までの移動量だけ、仮リンク移動部４２は仮リンクＴ０，ｋを平行移動する。すなわち、図７の点線で示す位置から図７の実線で示す位置に仮リンクＴ０，ｋを平行移動する。これにより、仮リンクＴ０，ｋ−１と仮リンクＴ０，ｋとのリンク境界が連続する状態となる。
【００５５】
なお、仮リンクＴ０，ｋ以外の仮リンクＴ０，ｊについても、仮リンク移動部４２により同様に移動される。すなわち、仮リンクＴ０，ｊの移動を行う際は、原点から仮リンク移動完了部７３の先端（グローバル座標系Ｃの原点から遠い側の端）までの移動量だけ、仮リンク移動部４２は仮リンクＴ０，ｊを平行移動する。これにより、仮リンクＴ０，ｊと仮リンクＴ０，ｊの基端側（グローバル座標系Ｃの原点から近い側）に隣接する仮リンクＴ０，ｊ−１との仮リンク境界が連続する状態となる。ただし、仮リンクＴ０，１については、仮リンクＴ０，１の基端がグローバル座標系Ｃの原点であるため、移動は行われない。すべての仮リンクＴ０，ｊの移動が完了することにより、図６に示すように、それぞれのセンサユニットＳｉの間を直線状の仮リンクＴ０，ｊと仮定した仮線形形状７１が形成される。仮線形形状７１でのそれぞれのセンサユニットＳｉの位置を、それぞれのセンサユニットＳｉの仮位置Ｐ０，ｉとして検出する（ステップＳ１０４）。
【００５６】
図３に示すように、センサ仮位置検出部４０は、仮曲線形状検出部５０に接続されている。仮曲線形状検出部５０は、センサ仮位置検出部４０により検出されたそれぞれのセンサユニットＳｉの仮位置Ｐ０，ｉの間を仮円弧Ｌ０，ｊとして曲線補間を行い、仮曲線形状７５を検出する。図８は、仮曲線形状検出部５０により検出された内視鏡１０の挿入部１１の仮曲線形状７５をグローバル座標ＣのＺ軸の負から正の方向に向かって視た図である。図８に示すように、仮曲線形状検出部５０により、図８の点線で示す仮線形形状７１のそれぞれのセンサユニットＳｉの仮位置Ｐ０，ｉの間が曲線補間される。これにより、それぞれの仮円弧Ｌ０，ｊが形成され、図８の実線で示す仮曲線形状７５が検出される。
【００５７】
仮曲線形状検出部５０は、それぞれのセンサユニットＳｉの仮位置Ｐ０，ｉでの単位接ベクトルを算出する単位接ベクトル算出部５１と、単位接ベクトル算出部５１により算出された単位接ベクトルに基づいて、それぞれのセンサユニットＳｉの仮位置Ｐ０，ｉの間での単位接ベクトルの変化率を算出する変化率算出部５２と、単位接ベクトル算出部５１により算出された単位接ベクトル及び変化率算出部５２で算出された変化率に基づいて、それぞれのセンサユニットＳｉの仮位置Ｐ０，ｉの間の仮円弧Ｌ０，ｊを形成する仮円弧形成部５３と、を備える。すべてのセンサユニットＳｉの仮位置Ｐ０，ｉの間で曲線補間が行われ、すべての仮円弧Ｌ０，ｊが形成されることにより、仮曲線形状７５が検出される。
【００５８】
ここで、仮曲線形状検出部５０で、センサ仮位置検出部４０により検出されたそれぞれのセンサユニットＳｉの仮位置Ｐ０，ｉの間を曲線補間し、仮曲線形状７５を検出する方法について説明する。図４に示すように、仮曲線形状検出部５０は、ステップＳ１０４で検出されたそれぞれのセンサユニットＳｉの仮位置Ｐ０，ｉの間を仮円弧Ｌ０，ｊで曲線補間を行い、仮曲線形状７５を検出する（ステップＳ１０５）。前述のように、センサ間寸法ｌが５０ｍｍ程度より小さい範囲であれば、それぞれのセンサユニットＳｉの間を寸法がセンサ間寸法ｌに等しい直線状の仮リンクＴ０，ｊと仮定した場合でも、挿入部１１の形状検出の際の誤差は小さくなる。しかし、体腔内に挿入された際の内視鏡１０の挿入部１１の形状は曲線形状である。したがって、それぞれのセンサユニットＳｉの仮位置Ｐ０，ｉの間の曲線補間を行うことが重要となる。ここで、内視鏡１０の挿入部１１は、製品により差はあるが、適度の弾性を有する。このため、挿入部１１の曲線形状の曲率が大きく変化することは、稀である。したがって、それぞれのセンサユニットＳｉの間の形状を半径Ｒ０，ｊ（曲率１／Ｒ０，ｊ）の仮円弧Ｌ０，ｊと仮定して曲線補間を行うことにより、実際の挿入部１１の曲線形状と誤差の少ない仮曲線形状７５が形成される。
【００５９】
仮曲線形状７５を検出する際、仮曲線形状検出部５０は、それぞれのセンサユニットＳｉの仮位置Ｐ０，ｉの間ごとに順次に曲線補間を行い、仮円弧Ｌ０，ｊを形成する。ここで、仮曲線形状検出部５０により、それぞれのセンサユニットＳｉの仮位置Ｐ０，ｉの間の曲線補間を行う方法について説明する。ここでは、基端側からｋ番目のセンサユニットＳｋ−１と基端側から（ｋ＋１）番目のセンサユニットＳｋとの間の曲線補間について説明する。すなわち、仮位置Ｐ０，ｋ−１と仮位置Ｐ０，ｋとの間が、補間対象である補間対象仮位置間となる。
【００６０】
図９は、仮曲線形状検出部５０により仮曲線形状７５を検出する方法を示すフローチャートである。図１０は、仮曲線形状検出部５０での処理を説明する図である。図９及び図１０に示すように、仮曲線形状検出部５０によりセンサユニットＳｋ−１の仮位置Ｐ０，ｋ−１とセンサユニットＳｋの仮位置Ｐ０，ｋとの間である補間対象のセンサ間の曲線補間を行う際は、まず、単位接ベクトル算出部５１が、センサユニットＳｋ−１の仮位置Ｐ０，ｋ−１での単位接ベクトル及びセンサユニットＳｋの仮位置Ｐ０，ｋでの単位接ベクトルを算出する（ステップＳ１１１）。センサユニットＳｋ−１の仮位置Ｐ０，ｋ−１での単位接ベクトルは、  
【数１７】
2012026184.tif 000019
【００６１】
となる。すなわち、仮位置Ｐ０，ｋ−１での単位接ベクトルは、仮位置Ｐ０，ｋ−１から仮位置Ｐ０，ｋまでのベクトルの単位ベクトルである。センサユニットＳｋの仮位置Ｐ０，ｋでの単位接ベクトルは、式（１７．１）、式（１７．２）、式（１７．３）のｋをｋ＋１で置換した式を用いて求められる。すなわち、仮位置Ｐ０，ｋでの単位接ベクトルは、仮位置Ｐ０，ｋから仮位置Ｐ０，ｋ＋１までのベクトルの単位ベクトルである。
【００６２】
そして、単位接ベクトル算出部５１により算出された単位接ベクトルに基づいて、変化率算出部５２がセンサユニットＳｋ−１の仮位置Ｐ０，ｋ−１とセンサユニットＳｋの仮位置Ｐ０，ｋとの間での単位接ベクトルの変化率を算出する（ステップＳ１１２）。まず、仮位置Ｐ０，ｋ−１と仮位置Ｐ０，ｋとの間での単位接ベクトルの変化率ベクトルが、  
【数１８】
2012026184.tif 000020
【００６３】
で求められる。そして、式（１８．１）、式（１８．２）で求められた変化率ベクトルの大きさが仮位置Ｐ０，ｋ−１と仮位置Ｐ０，ｋとの間での単位接ベクトルの変化率となる。
【００６４】
そして、単位接ベクトル算出部５１により算出された単位接ベクトル及び変化率算出部５２で算出された変化率に基づいて、仮円弧形成部５３が、センサユニットＳｋ−１の仮位置Ｐ０，ｋ−１とセンサユニットＳｋの仮位置Ｐ０，ｋとの間の仮円弧Ｌ０，ｋを形成する（ステップＳ１１３）。ここで、ステップＳ１１２で算出した仮位置Ｐ０，ｋ−１と仮位置Ｐ０，ｋとの間での単位接ベクトルの変化率は、仮円弧Ｌ０，ｋの曲率１／Ｒ０，ｋとなる。仮円弧Ｌ０，ｋの半径Ｒ０，ｋは曲率１／Ｒ０，ｋの逆数であるため、仮円弧Ｌ０，ｋの半径Ｒ０，ｋは、  
【数１９】
2012026184.tif 000021
【００６５】
となる。また、仮円弧Ｌ０，ｋの中心Ｏ０，ｋのグローバル座標Ｃでの位置が、  
【数２０】
2012026184.tif 000022
【００６６】
により求められる。さらに、仮位置Ｐ０，ｋ−１での単位接ベクトル及び仮位置Ｐ０，ｋでの単位接ベクトルを用いて、仮円弧Ｌ０，ｋの中心角φ０，ｋが、  
【数２１】
2012026184.tif 000023
【００６７】
により求められる。これらのパラメータを用いて、仮位置Ｐ０，ｋ−１と仮位置Ｐ０，ｋとの間の仮円弧Ｌ０，ｋが形成される。
【００６８】
なお、仮円弧Ｌ０，ｋ以外の仮円弧Ｌ０，ｊについても、仮位置Ｐ０，ｊ−１と仮位置Ｐ０，ｊとの間を同様に曲線補間することにより、形成される。すなわち、仮円弧Ｌ０，ｊを形成する際は、まず、単位接ベクトル算出部５１が、補間対象仮位置間の基端側（グローバル座標系Ｃの原点に近い側）に位置するセンサユニットＳｊ−１の仮位置Ｐ０，ｊ−１での単位接ベクトル及び補間対象仮位置間の先端側（グローバル座標系Ｃの原点から遠い側）に位置するセンサユニットＳｊの仮位置Ｐ０，ｊでの単位接ベクトルを算出する。センサユニットＳｊ−１の仮位置Ｐ０，ｊ−１での単位接ベクトルは、式（１７．１）、式（１７．２）、式（１７．３）のｋをｊで置換した式を用いて求められ、仮位置Ｐ０，ｊ−１から仮位置Ｐ０，ｊまでのベクトルの単位ベクトルである。また、センサユニットＳｊの仮位置Ｐ０，ｊでの単位接ベクトルは、式（１７．１）、式（１７．２）、式（１７．３）のｋをｊ＋１で置換した式を用いて求められ、仮位置Ｐ０，ｊから仮位置Ｐ０，ｊ＋１までのベクトルの単位ベクトルである。これにより、それぞれのセンサユニットＳｉの仮位置Ｐ０，ｉでの単位接ベクトルが算出される（ステップＳ１１１）。
【００６９】
そして、単位接ベクトル算出部５１により算出された単位接ベクトルに基づいて、変化率算出部５２がセンサユニットＳｊ−１の仮位置Ｐ０，ｊ−１とセンサユニットＳｊの仮位置Ｐ０，ｊとの間である補間対象仮位置間での単位接ベクトルの変化率を算出する。仮位置Ｐ０，ｊ−１と仮位置Ｐ０，ｊとの間での単位接ベクトルの変化率は、式（１８．１）、式（１８．２）のｋをｊで置換した式を用いて求められる。これにより、それぞれのセンサユニットＳｉの仮位置Ｐ０，ｉの間での単位接ベクトルの変化率が算出される（ステップＳ１１２）。
【００７０】
そして、単位接ベクトル算出部５１により算出された単位接ベクトル及び変化率算出部５２で算出された変化率に基づいて、仮円弧形成部５３が、センサユニットＳｊ−１の仮位置Ｐ０，ｊ−１とセンサユニットＳｊの仮位置Ｐ０，ｊとの間である補間対象仮位置間の仮円弧Ｌ０，ｊを形成する。ここで、式（１９）、式（２０）、式（２１）のｋをｊで置換した式を用いて、仮円弧Ｌ０，ｊの半径Ｒ０，ｊ、中心Ｏ０，ｊ、中心角φ０，ｊが求められる。これにより、それぞれのセンサユニットＳｉの仮位置Ｐ０，ｉの間の仮円弧Ｌ０，ｊが形成される（ステップＳ１１３）。すべてのセンサユニットＳｉの仮位置Ｐ０，ｉの間で曲線補間が行われ、すべての仮円弧Ｌ０，ｊが形成されることにより、仮曲線形状７５が検出される。
【００７１】
図３に示すように、仮曲線形状検出部５０は、それぞれのセンサユニットＳｉの位置を修正するセンサ位置修正部５５に接続されている。センサ位置修正部５５は、仮曲線形状検出部５０により検出された仮曲線形状７５のそれぞれの仮円弧Ｌ０，ｊの弧長Ｒ０，ｊφ０，ｊとセンサ間寸法ｌとの差の絶対値に基づいて、それぞれのセンサユニットＳｉの位置を修正する。ここで、仮円弧Ｌ０，ｊの弧長Ｒ０，ｊφ０，ｊとセンサ間寸法ｌとの差の絶対値をエネルギーＥ０，ｊとする。図１１は、センサ位置修正部５５により修正されたそれぞれのセンサユニットＳｉの位置をグローバル座標ＣのＺ軸の負から正の方向に向かって視た図である。図１１に示すように、センサ位置修正部５５により、それぞれのセンサユニットＳｉは、仮位置Ｐ０，ｉから最終位置ＰＦ，ｉまで修正される。
【００７２】
センサ位置修正部５５は、基端側（グローバル座標系Ｃの原点に近い側）のセンサユニットＳｉから順次に位置補正を行うセンサ位置順次補正部５７を備える。センサ位置順次補正部５７によりそれぞれのセンサユニットＳｉの位置補正を行う前の状態では、それぞれのセンサユニットＳｉは、仮位置Ｐ０，ｉから補正前位置Ｑ０，ｉに移動されている（詳細については、後述する。）。センサ位置順次補正部５７は、それぞれのセンサユニットＳｉを補正前位置Ｑ０，ｉから最終位置ＰＦ，ｉへ位置補正する。また、センサ位置修正部５５は、センサ位置順次補正部５７により１つのセンサユニットＳｉの位置補正が行われるたびに、位置補正が完了していないセンサユニットＳｉである未補正センサ群７７を平行移動させる未補正センサ群移動部５９を備える。センサ位置順次補正部５７により、すべてのセンサユニットＳｉの補正前位置Ｑ０，ｉから最終位置ＰＦ，ｉへの位置補正が行われる。ここで、補正前位置Ｑ０，ｉは、センサ位置順次補正部５７により位置補正が行われる直前のそれぞれのセンサユニットＳｉの位置である。未補正センサ群移動部５９は、センサ位置順次補正部５７による直前の位置補正で補正対象だったセンサユニットＳｉである直前補正対象センサ７９の補正前位置Ｑ０，ｉから最終位置ＰＦ，ｉへの補正量だけ、未補正センサ群７７を平行移動させる。これにより、未補正センサ群７７は、平行移動前位置Ｕａ−１，ｉ（ａ＝１，２，…）から平行移動後位置Ｕａ，ｉまで平行移動する。ここで、ａは、未補正センサ群移動部５９によりそれぞれのセンサユニットＳｉが平行移動した回数を示している。それぞれのセンサユニットＳｉの未補正センサ群移動部５９による１回目の平行移動の平行移動前位置Ｕ０，ｉは、仮位置Ｐ０，ｉと一致する。それぞれのセンサユニットＳｉは、未補正センサ群移動部５９により（ｉ−１）回だけ平行移動する。それぞれのセンサユニットＳｉの未補正センサ群移動部５９による（ｉ−１）回目の平行移動の平行移動後位置Ｕｉ−１，ｉは、補正前位置Ｑ０，ｉと一致する。以上のように、センサ位置順次補正部５７での位置補正及び未補正センサ群移動部５９での平行移動により、それぞれのセンサユニットＳｉが、仮位置Ｐ０，ｉから最終位置ＰＦ，ｉへ位置修正される。
【００７３】
センサ位置順次補正部５７は、位置補正の対象である補正対象センサ８１を位置補正前の補正前位置Ｑ０，ｉから１回以上移動させるセンサ移動部６１を備える。センサ移動部６１での１回の移動により、補正対象センサ８１は移動前位置Ｑｔ−１，ｉ（ｔ＝１，２，…）から移動後位置Ｑｔ，ｉまで移動する。ここで、ｔはセンサ移動部６１によりぞれぞれのセンサユニットＳｉが移動した回数を示している。また、センサ位置順次補正部５７は、センサ移動部６１により補正対象センサ８１の移動が１回行われるたびに、補正対象センサ８１に対して基端側（グローバル座標系Ｃの原点に近い側）に隣設されるセンサユニットＳｉである近位側隣設センサ８２の最終位置ＰＦ，ｉと補正対象センサ８１の移動後位置Ｑｔ，ｉとの間を曲線補間する移動後円弧形成部６２とを備える。移動後円弧形成部６２で近位側隣設センサ８２の最終位置ＰＦ，ｉと補正対象センサ８１の移動後位置Ｑｔ，ｉとの間を曲線補間することにより、移動後円弧Ｌｔ，ｊが形成される。
【００７４】
センサ位置順次補正部５７は、移動後円弧形成部６２により形成される移動後円弧Ｌｔ，ｊの弧長Ｒｔ，ｊφｔ，ｊとセンサ間寸法ｌとの差の絶対値が所定の閾値以下になるまで、補正対象センサ８１の移動前位置Ｑｔ−１，ｉから移動後位置Ｑｔ，ｉへの移動を繰り返し行う状態にセンサ移動部６１を制御する移動制御部６３を備える。ここで、移動後円弧Ｌｔ，ｊの弧長Ｒｔ，ｊφｔ，ｊとセンサ間寸法ｌとの差の絶対値をエネルギーＥｔ，ｊとする。また、センサ位置順次補正部５７は、センサ移動部６１による最終回（例えばＦ回目）の移動での移動後位置ＱＦ，ｉを、補正対象センサ８１の最終位置ＰＦ，ｉとして決定する最終位置決定部６４を備える。センサ移動部６１、移動後円弧形成部６２、移動制御部６３、最終位置決定部６４での詳細な処理については、後述する。
【００７５】
ここで、センサ位置修正部５５で、それぞれのセンサユニットＳｉの位置を修正する方法について説明する。図４に示すように、センサ位置修正部５５は、センサ位置修正部５５は、仮曲線形状検出部５０により検出された仮曲線形状７５のそれぞれの仮円弧Ｌ０，ｊの弧長Ｒ０，ｊφ０，ｊとセンサ間寸法ｌとの差の絶対値に基づいて、それぞれのセンサユニットＳｉの位置を修正する（ステップＳ１０６）。すなわち、  
【数２２】
2012026184.tif 000024
【００７６】
で求められるエネルギーＥ０，ｊ基づいて、それぞれのセンサユニットＳｉの位置を修正する。これにより、それぞれのセンサユニットＳｉは、仮位置Ｐ０，ｉから最終位置ＰＦ，ｉまで修正される。
【００７７】
前述のように、仮曲線形状検出部５０により、実際の挿入部１１の曲線形状と誤差の少ない仮曲線形状７５が形成される。しかし、仮曲線形状検出部５０では、センサ間寸法ｌを考慮して、それぞれのセンサユニットＳｉの間の曲線補間を行っていない。また、それぞれのセンサユニットＳｉの計測データはノイズ等により誤差を有する。したがって、センサ間寸法ｌを考慮して、それぞれの仮円弧Ｌ０，ｊの弧長Ｒ０，ｊφ０，ｊとセンサ間寸法ｌとの差の絶対値に基づいて、それぞれのセンサユニットＳｉの位置を修正することが重要となる。
【００７８】
図１２は、センサ位置修正部５５によりそれぞれのセンサユニットＳｉの位置を修正する方法を示すフローチャートである。図１３は、センサ位置順次補正部５７による処理を説明する図である。図１２に示すように、それぞれのセンサユニットＳｉの位置を修正する際は、センサ位置順次補正部５７により基端側（グローバル座標系Ｃの原点に近い側）のセンサユニットＳｉから順次に、それぞれのセンサユニットＳｉの位置補正前の補正前位置Ｑ０，ｉから最終位置ＰＦ，ｉへの位置補正が行われる。ここで、補正前位置Ｑ０，ｉは、センサ位置順次補正部５７により位置補正が行われる直前のそれぞれのセンサユニットＳｉの位置であり、それぞれのセンサユニットＳｉが仮位置Ｐ０，ｉから未補正センサ群移動部５９により（ｉ−１）回だけ平行移動した位置である。センサ位置順次補正部５７は、未補正センサ群７７の中で最も基端側のセンサユニットＳｉの位置補正を開始する（ステップＳ１２１）。すなわち、図１３に示すように、センサユニットＳｋ−１まで補正前位置Ｑ０，ｉから最終位置ＰＦ，ｉへの位置補正が完了している場合には、センサ位置順次補正部５７により、未補正センサ群７７の中で最も基端側のセンサユニットＳｋの最終位置ＰＦ，ｋへの位置補正が開始される。すなわち、センサユニットＳｋが位置補正の対象である補正対象センサ８１となる。なお、以下の説明では、基端側から（ｋ＋１）番目のセンサユニットＳｋの位置補正について説明する。
【００７９】
図１３に示すように、補正対象センサ８１であるセンサユニットＳｋの位置補正を開始する際、センサユニットＳｋ−１まで補正前位置Ｑ０，ｉから最終位置ＰＦ，ｉへの位置補正が完了している。この際、センサユニットＳｋは、補正前位置Ｑ０，ｋに位置している。この状態で、センサ移動部６１により、センサユニットＳｋが位置Ｑ０，ｋから位置Ｑ１，ｋに移動し（ステップＳ１２２）、センサユニットＳｋの１回目の移動が行われる。この際、位置Ｑ０，ｋが移動前位置、位置Ｑ１，ｋが移動後位置となる。センサ移動部６１によるセンサユニットＳｋの移動は、１回以上行われる。図１４は、センサ移動部６１及び移動後円弧形成部６２での処理を説明する図である。図１４に示すように、ｓ回目の移動では、補正対象センサ８１であるセンサユニットＳｋは移動前位置Ｑｓ−１，ｋから移動後位置Ｑｓ，ｋまで移動する。すなわち、センサ移動部６１による１回の移動により、センサユニットＳｋは移動前位置Ｑｔ−１，ｋから移動後位置Ｑｔ，ｋまで移動する（ステップＳ１２２）。
【００８０】
そして、センサ移動部６１によりセンサユニットＳｋの移動が１回行われるたびに、移動後円弧形成部６２により、センサユニットＳｋに対して基端側に隣設されるセンサユニットＳｋ−１の最終位置ＰＦ，ｋ−１とセンサユニットＳｋの移動後位置Ｑｔ，ｋとの間が曲線補間され、移動後円弧Ｌｔ，ｋが形成される（ステップＳ１２３）。図１４に示すように、ｓ回目の移動後には、センサユニットＳｋ−１の最終位置ＰＦ，ｋ−１とセンサユニットＳｋの移動後位置Ｑｓ，ｋとの間が曲線補間され、移動後円弧Ｌｓ，ｋが形成される。ここで、センサユニットＳｋ−１は、補正対象センサ８１であるセンサユニットＳｋに対してグローバル座標系Ｃの原点に近い側に隣設される近位側隣設センサ８２である。
【００８１】
センサユニットＳｋのｓ回目に移動後に移動後円弧Ｌｓ，ｋを形成する際は、センサユニットＳｋ−１の最終位置ＰＦ，ｋ−１での単位接ベクトル及びセンサユニットＳｋの移動後位置Ｑｓ，ｋでの単位接ベクトルを算出する。センサユニットＳｋ−１の最終位置ＰＦ，ｋ−１での単位接ベクトルは、  
【数２３】
2012026184.tif 000025
【００８２】
となる。すなわち、最終位置ＰＦ，ｋ−１での単位接ベクトルは、最終位置ＰＦ，ｋ−１から移動後位置Ｑｓ，ｋまでのベクトルの単位ベクトルである。また、移動後位置Ｑｓ，ｋでの単位接ベクトルを算出する際は、補正対象センサ８１であるセンサユニットＳｋに対してグローバル座標系Ｃの原点から遠い側に隣設される遠位側隣設センサ８３であるセンサユニットＳｋ＋１が、位置Ｑ´ｓ，ｋ＋１に配置されていると仮定する。ここで、移動後位置Ｑｓ，ｋから位置Ｑ´ｓ，ｋ＋１までのベクトルは、センサユニットＳｋの仮位置Ｐ０，ｋからセンサユニットＳｋ＋１の仮位置Ｐ０，ｋ＋１までのベクトルと向き及び大きさが同一である。センサユニットＳｋの移動後位置Ｑｓ，ｋでの単位ベクトルは、  
【数２４】
2012026184.tif 000026
【００８３】
となる。すなわち、移動後位置Ｑｓ，ｋでの単位接ベクトルは、移動後位置Ｑｓ，ｋからセンサユニットＳｋ＋１の位置Ｑ´ｓ，ｋ＋１までのベクトルの単位ベクトルである。したがって、移動後位置Ｑｓ，ｋでの単位接ベクトルは、式（１７．１）〜式（１７．３）でｋをｋ＋１に置換して得られる単位接ベクトルと向きが同一である。
【００８４】
そして、式（２３．１）〜式（２３．３）、式（２４．１）〜式（２４．３）により算出された単位接ベクトルに基づいて、センサユニットＳｋ−１の最終位置ＰＦ，ｋ−１とセンサユニットＳｋの移動後位置Ｑｓ，ｋとの間での単位接ベクトルの変化率を算出する。まず、最終位置ＰＦ，ｋ−１と移動後位置Ｑｓ，ｋとの間での単位接ベクトルの変化率ベクトルが、  
【数２５】
2012026184.tif 000027
【００８５】
で求められる。そして、式（２５．１）、式（２５．２）で求められた変化率ベクトルの大きさが最終位置ＰＦ，ｋ−１と移動後位置Ｑｓ，ｋとの間での単位接ベクトルの変化率となる。
【００８６】
そして、式（２３．１）〜式（２３．３）、式（２４．１）〜式（２４．３）により算出された単位接ベクトル及び式（２５．１）、式（２５．２）により算出された変化率に基づいて、センサユニットＳｋ−１の最終位置ＰＦ，ｋ−１とセンサユニットＳｋの移動後位置Ｑｓ，ｋとの間の移動後円弧Ｌｓ，ｋを形成する。ここで、式（２５．１）、式（２５．２）で算出した最終位置ＰＦ，ｋ−１と移動後位置Ｑｓ，ｋとの間での単位接ベクトルの変化率は、移動後円弧Ｌｓ，ｋの曲率１／Ｒｓ，ｋとなる。移動後円弧Ｌｓ，ｋの半径Ｒｓ，ｋは曲率１／Ｒｓ，ｋの逆数であるため、移動後円弧Ｌｓ，ｋの半径Ｒｓ，ｋは、  
【数２６】
2012026184.tif 000028
【００８７】
となる。また、移動後円弧Ｌｓ，ｋの中心Ｏｓ，ｋのグローバル座標Ｃでの位置が、  
【数２７】
2012026184.tif 000029
【００８８】
により求められる。さらに、最終位置ＰＦ，ｋ−１での単位接ベクトル及び移動後位置Ｑｓ，ｋでの単位接ベクトルを用いて、移動後円弧Ｌｓ，ｋの中心角φｓ，ｋが、  
【数２８】
2012026184.tif 000030
【００８９】
により求められる。これらのパラメータを用いて、最終位置ＰＦ，ｋ−１と移動後位置Ｑｓ，ｋとの間の移動後円弧Ｌｓ，ｋが形成される。
【００９０】
なお、センサユニットＳｋのｓ回目の移動後に移動後円弧Ｌｓ，ｋを形成する処理について説明したが、センサユニットＳｋのｓ回目以外の移動後についても、式（２３．１）〜式（２８）で、ｓをｔで置換した式を用いて同様に、センサユニットＳｋ−１の最終位置ＰＦ，ｋ−１とセンサユニットＳｋの移動後位置Ｑｔ，ｋとの間の移動後円弧Ｌｔ，ｋが形成される。
【００９１】
そして、移動後円弧Ｌｔ，ｋが形成された後、移動制御部６３により移動後円弧Ｌｔ，ｋの弧長Ｒｔ，ｋφｔ，ｋとセンサ間寸法ｌとの差の絶対値が算出される（ステップＳ１２４）。ここで、移動後円弧Ｌｔ，ｋの弧長Ｒｔ，ｋφｔ，ｋとセンサ間寸法ｌとの差の絶対値をエネルギーＥｔ，ｋとし、  
【数２９】
2012026184.tif 000031
【００９２】
で与えられる。エネルギーＥｔ，ｋが所定の閾値より大きい場合は（ステップＳ１２４−Ｎｏ）、ステップＳ１２２に戻り、センサ移動部６１がセンサユニットＳｋを位置Ｑｔ，ｋからさらに移動させる。センサユニットＳｋのｓ回目の移動後にエネルギーＥｓ，ｋが所定の閾値より大きい場合は、センサ移動部６１により（ｓ＋１）回目の移動が行われる。（ｓ＋１）回目の移動により、センサユニットＳｋは移動前位置Ｑｓ，ｋから移動後位置Ｑｓ＋１，ｋに移動する。以上のように、センサ移動部６１は、エネルギーＥｔ，ｋが所定の閾値以下になるまで、補正対象センサ８１であるセンサユニットＳｋの移動前位置Ｑｔ−１，ｋから移動後位置Ｑｔ，ｋへの移動を繰り返し行う状態に、移動制御部６３により制御されている。
【００９３】
エネルギーＥｔ，ｋが所定の閾値以下の場合は（ステップＳ１２４−Ｙｅｓ）、次のステップに進む。そして、最終位置決定部６４が、センサ移動部６１によるセンサユニットＳｋの最終回（例えばＦ回目）の移動での移動後位置ＱＦ，ｋを、補正対象センサ８１であるセンサユニットＳｋの最終位置ＰＦ，ｋとして決定する（ステップＳ１２５）。以上のようにして、センサユニットＳｋの補正前位置Ｑ０，ｋから最終位置ＰＦ，ｋへの位置補正が完了する。
【００９４】
なお、センサユニットＳｋ以外のセンサユニットＳｉについても、センサ位置順次補正部５７によりセンサユニットＳｋと同様に最終位置Ｐｉ，ｋへの位置補正が行われる。すなわち、センサ移動部６１により、センサユニットＳｉは移動前位置Ｑｔ−１，ｉから移動後位置Ｑｔ，ｉまで移動する（ステップＳ１２２）。そして、センサ移動部６１によりセンサユニットＳｉの移動が１回行われるたびに、移動後円弧形成部６２により、センサユニットＳｉに対して基端側に隣設される近位側隣設センサ８２であるセンサユニットＳｉ−１の最終位置ＰＦ，ｉ−１とセンサユニットＳｉの移動後位置Ｑｔ，ｉとの間が曲線補間され、移動後円弧Ｌｔ，ｊが形成される（ステップＳ１２３）。
【００９５】
そして、移動後円弧Ｌｔ，ｊが形成された後、移動制御部６３により移動後円弧Ｌｔ，ｊの弧長Ｒｔ，ｊφｔ，ｊとセンサ間寸法ｌとの差の絶対値であるエネルギーＥｔ，ｊが算出される（ステップＳ１２４）。エネルギーＥｔ，ｊが所定の閾値より大きい場合は（ステップＳ１２４−Ｎｏ）、ステップＳ１２２に戻り、センサ移動部６１がセンサユニットＳｉを位置Ｑｔ，ｉからさらに移動させる。エネルギーＥｔ，ｊが所定の閾値以下の場合は（ステップＳ１２４−Ｙｅｓ）、次のステップに進む。そして、最終位置決定部６４が、センサ移動部６１によるセンサユニットＳｉの最終回の移動での移動後位置ＱＦ，ｉを、補正対象センサ８１であるセンサユニットＳｉの最終位置ＰＦ，ｉとして決定する（ステップＳ１２５）。以上のようにして、センサユニットＳｉの補正前位置Ｑ０，ｉから最終位置ＰＦ，ｉへの位置補正が完了する。
【００９６】
そして、センサ位置順次補正部５７により１つのセンサユニットＳｉの位置補正が行われるたびに、未補正センサ群移動部５９により、位置補正が完了していないセンサユニットＳｉである未補正センサ群７７が平行移動される（ステップＳ１２６）。図１５は、未補正センサ群移動部５９での処理を説明する図である。図１５に示すように、センサ位置順次補正部５７によりセンサユニットＳｋの補正前位置Ｑ０，ｋから最終位置ＰＦ，ｋへの位置修正が完了した状態では、センサユニットＳｋ＋１〜ＳＮが、位置補正が完了していない未補正センサ群７７となる。また、センサユニットＳｋが、センサ位置順次補正部５７による直前の位置補正で補正対象だった直前補正対象センサ７９となる。
【００９７】
この状態で、未補正センサ群移動部５９は、直前補正対象センサ７９であるセンサユニットＳｋの補正前位置Ｑ０，ｋから最終位置ＰＦ，ｋへの補正量だけ、未補正センサ群７７を平行移動させる。これにより、未補正センサ群７７のそれぞれのセンサユニットＳｉが平行移動前位置Ｕｋ−１，ｉから平行移動後位置Ｕｋ，ｉへ移動する。ここで、未補正センサ群７７のそれぞれのセンサユニットＳｉが平行移動前位置Ｕｋ−１，ｉは、それぞれのセンサユニットＳｉが仮位置Ｐ０，ｉから未補正センサ群移動部５９により（ｋ−１）回だけ移動した位置であり、今回の平行移動で、未補正センサ群７７のそれぞれのセンサユニットＳｉは未補正センサ群移動部５９によりｋ回だけ平行移動したこととなる。また、センサユニットＳｋ＋１の平行移動後位置Ｕｋ，ｋ＋１は、補正前位置Ｑ０，ｋ＋１と一致し、センサユニットＳｋの最終位置ＰＦ，ｋからセンサユニットＳｋ＋１の平行移動後位置Ｕｋ，ｋ＋１へのベクトルは、センサユニットＳｋの仮位置Ｐ０，ｋからセンサユニットＳｋ＋１の仮位置Ｐ０，ｋ＋１までのベクトルと向き及び大きさが同一である。
【００９８】
なお、センサユニットＳｋ以外のセンサユニットＳｉの最終位置ＰＦ，ｉへの位置補正が完了した状態でも、センサユニットＳｋと同様に、未補正センサ群７７が平行移動される。すなわち、未補正センサ群移動部５９は、直前補正対象センサ７９がセンサ位置順次補正部５７により補正前位置Ｑ０，ｉから最終位置ＰＦ，ｉへ補正された補正量だけ、未補正センサ群７７を平行移動させる（ステップＳ１２６）。これにより、未補正センサ群７７は、平行移動前位置Ｕａ−１，ｉから平行移動後位置Ｕａ，ｉまで平行移動する。
【００９９】
そして、図１２に示すように、すべてのセンサユニットＳｉについて位置補正が完了したか確認する（ステップＳ１２７）。すべてのセンサユニットＳｉについて位置補正が完了している場合は（ステップＳ１２７−Ｙｅｓ）、次のステップへ進む。すべてのセンサユニットＳｉについて位置補正が完了していない場合は（ステップＳ１２７−Ｎｏ）、ステップＳ１２１に戻り、センサ位置順次補正部５７により、未補正センサ群７７の中で最も基端側のセンサユニットＳｉの位置補正が行われる。すなわち、ステップＳ１２１〜Ｓ１２６が、すべてのセンサユニットＳｉについて位置補正が完了するまで、繰り返し行われる。以上のように、センサ位置順次補正部５７での補正前位置Ｑ０，ｉから最終位置ＰＦ，ｉ位置への位置補正及び未補正センサ群移動部５９での平行移動により、それぞれのセンサユニットＳｉが、仮位置Ｐ０，ｉから最終位置ＰＦ，ｉへ位置修正される。
【０１００】
図３に示すように、センサ位置修正部５５は、最終曲線形状検出部６５に接続されている。最終曲線形状検出部６５は、それぞれのセンサユニットＳｉの最終位置ＰＦ，ｉの間を最終円弧ＬＦ，ｊで曲線補間し、挿入部１１の最終曲線形状８５を検出する。図１６は、最終曲線形状検出部６５により検出された内視鏡１０の挿入部１１の最終曲線形状８５をグローバル座標ＣのＺ軸の負から正の方向に向かって視た図である。図１６に示すように、最終曲線形状検出部６５により、それぞれのセンサユニットＳｉの最終位置ＰＦ，ｉの間が曲線補間される。これにより、それぞれの最終円弧ＬＦ，ｊが形成され、最終曲線形状８５が検出される。
【０１０１】
最終曲線形状検出部６５は、それぞれのセンサユニットＳｉの最終位置ＰＦ，ｉに基づいて、それぞれの最終円弧ＬＦ，ｊのパラメータを算出するパラメータ算出部６７と、パラメータ算出部６７により算出されたパラメータに基づいて、最終円弧ＰＦ，ｉを形成する最終円弧形成部６９とを備える。最終円弧形成部６９は、クォータニオン（四元数）及び媒介変数ｔを用いた補間関数ＬＦ，ｊ（ｔ）により、媒介変数ｔの変化に対して等角速度でそれぞれのセンサユニットＳｉの最終位置ＰＦ，ｉの間を曲線補間し、最終円弧ＬＦ，ｊを形成する。
【０１０２】
最終曲線形状検出部６５には、描画部４５が接続されている。最終曲線形状検出部６５により検出されたグローバル座標系Ｃでの挿入部１１の最終曲線形状８５は、描画部４５により描画される。術者は、描画部４５により描画された最終曲線形状８５を表示部４７で確認可能となっている。
【０１０３】
ここで、最終曲線形状検出部６５で、センサ位置修正部５５により位置修正されたそれぞれのセンサユニットＳｉの最終位置ＰＦ，ｉの間を曲線補間し、最終曲線形状８５を検出する方法について説明する。図４に示すように、最終曲線形状検出部６５は、ステップＳ１０６で位置修正されたそれぞれのセンサユニットＳｉの最終位置ＰＦ，ｉの間を最終円弧ＬＦ，ｊで曲線補間を行い、最終曲線形状８５を検出する（ステップＳ１０７）。これにより、センサ間寸法ｌを考慮してそれぞれのセンサユニットＳｉの最終位置ＰＦ，ｉの間の曲線補間が行われ、検出精度の高い最終曲線形状８５が検出される。
【０１０４】
最終曲線形状８５を検出する際、最終曲線形状検出部６５は、それぞれのセンサユニットＳｉの最終位置ＰＦ，ｉの間ごとに順次に曲線補間を行い、最終円弧ＬＦ，ｊを形成する。ここで、最終曲線形状検出部６５により、それぞれのセンサユニットＳｉの最終位置ＰＦ，ｉの間の曲線補間を行う方法について説明する。ここでは、基端側からｋ番目のセンサユニットＳｋ−１と基端側から（ｋ＋１）番目のセンサユニットＳｋとの間の曲線補間について説明する。すなわち、最終位置ＰＦ，ｋ−１と最終位置ＰＦ，ｋとの間が、補間対象である補間対象最終位置間となる。
【０１０５】
図１７は、最終曲線形状検出部６５により最終曲線形状８５を検出する方法を示すフローチャートである。図１８は、最終曲線形状検出部６５での処理を説明する図である。図１７及び図１８に示すように、最終曲線形状検出部６５によりセンサユニットＳｋ−１の最終位置ＰＦ，ｋ−１とセンサユニットＳｋの最終位置ＰＦ，ｋとの間である補間対象の最終位置間の曲線補間を行う際は、まず、パラメータ算出部６７により、最終位置ＰＦ，ｋ−１及び最終位置ＰＦ，ｋに基づいて、最終円弧ＬＦ，ｋのパラメータを算出する（ステップＳ１３１）。
【０１０６】
この際、センサユニットＳｋ−１の最終位置ＰＦ，ｋ−１での単位接ベクトル及びセンサユニットＳｋの最終位置ＰＦ，ｋでの単位接ベクトルを算出する。センサユニットＳｋ−１の最終位置ＰＦ，ｋ−１での単位接ベクトルは、  
【数３０】
2012026184.tif 000032
【０１０７】
となる。すなわち、最終位置ＰＦ，ｋ−１での単位接ベクトルは、最終位置ＰＦ，ｋ−１から最終位置ＰＦ，ｋまでのベクトルの単位ベクトルである。センサユニットＳｋの最終位置ＰＦ，ｋでの単位接ベクトルは、式（３０．１）、式（３０．２）、式（３０．３）のｋをｋ＋１で置換した式を用いて求められる。すなわち、最終位置ＰＦ，ｋでの単位接ベクトルは、最終位置ＰＦ，ｋから最終位置ＰＦ，ｋ＋１までのベクトルの単位ベクトルである。
【０１０８】
そして、最終位置ＰＦ，ｋ−１及びＰＦ，ｋでの単位接ベクトルに基づいて、センサユニットＳｋ−１の最終位置ＰＦ，ｋ−１とセンサユニットＳｋの最終位置ＰＦ，ｋとの間での単位接ベクトルの変化率を算出する。まず、最終位置ＰＦ，ｋ−１と最終位置ＰＦ，ｋとの間での単位接ベクトルの変化率ベクトルが、  
【数３１】
2012026184.tif 000033
【０１０９】
で求められる。そして、式（３１．１）、式（３１．２）で求められた変化率ベクトルの大きさが最終位置ＰＦ，ｋ−１と最終位置ＰＦ，ｋとの間での単位接ベクトルの変化率となる。
【０１１０】
そして、式（３０．３）〜式（３１．２）により算出された単位接ベクトル及び変化率に基づいて、センサユニットＳｋ−１の最終位置ＰＦ，ｋ−１とセンサユニットＳｋの最終位置ＰＦ，ｋとの間の最終円弧ＬＦ，ｋのパラメータが算出される。ここで、最終円弧ＬＦ，ｋの半径ＲＦ，ｋは、式（３１．１）、式（３１．２）を用いて算出された変化率ｋの逆数であるため、  
【数３２】
2012026184.tif 000034
【０１１１】
となる。また、最終円弧ＬＦ，ｋの中心ＯＦ，ｋのグローバル座標Ｃでの位置が、  
【数３３】
2012026184.tif 000035
【０１１２】
により求められる。さらに、最終円弧ＬＦ，ｋの中心角φＦ，ｋが、  
【数３４】
2012026184.tif 000036
【０１１３】
により求められる。
【０１１４】
なお、最終円弧ＬＦ，ｋ以外の最終円弧ＬＦ，ｊについても、式（３０．１）〜式（３４）のｋをｊで置換した式を用いて、最終円弧ＬＦ，ｋと同様に、パラメータ算出部６７により、最終円弧ＬＦ，ｊのパラメータが算出される。すなわち、それぞれのセンサユニットＳｉの最終位置ＰＦ，ｉに基づいて、それぞれの最終円弧ＬＦ，ｊのパラメータを算出する（ステップＳ１３１）。
【０１１５】
そして、パラメータ算出部６７により算出された最終円弧ＬＦ，ｊのパラメータに基づいて、最終円弧形成部６９が、それぞれのセンサユニットＳｉの最終位置ＰＦ，ｉの間を曲線補間し、最終円弧ＬＦ，ｊを形成する（ステップＳ１３２）。最終円弧ＬＦ，ｊの形成は、クォータニオン（四元数）及び媒介変数を用いた補間関数により行われる。
【０１１６】
ここで、クォータニオンは、複素数を拡張した数であり、  
【数３５】
2012026184.tif 000037
【０１１７】
で現される。式（３５）のｉ，ｊ，ｋは虚数単位を示し、  
【数３６】
2012026184.tif 000038
【０１１８】
の関係を満たす。
【０１１９】
図１８に示すように、センサユニットＳｋ−１の最終位置ＰＦ，ｋ−１とセンサユニットＳｋの最終位置ＰＦ，ｋとの間の最終円弧ＬＦ，ｋを形成する場合には、グローバル座標系Ｃにおいて、最終円弧ＬＦ，ｋの中心ＯＦ，ｋからセンサユニットＳｋ−１の最終位置ＰＦ，ｋ−１へのベクトルを算出する。中心ＯＦ，ｋから最終位置ＰＦ，ｋ−１へのベクトルは、  
【数３７】
2012026184.tif 000039
【０１２０】
となる。また、グローバル座標系Ｃにおいて、最終円弧ＬＦ，ｋの中心ＯＦ，ｋからセンサユニットＳｋの最終位置ＰＦ，ｋへのベクトルを算出する。中心ＯＦ，ｋから最終位置ＰＦ，ｋへのベクトルは、  
【数３８】
2012026184.tif 000040
【０１２１】
となる。
【０１２２】
式（３７）で算出されたベクトルは、クォータニオンにより、  
【数３９】
2012026184.tif 000041
【０１２３】
と現される。同様に、式（３８）で算出されたベクトルは、クォータニオンにより、  
【数４０】
2012026184.tif 000042
【０１２４】
と現される。すなわち、算出されたベクトルのグローバル座標系ＣのＸ軸方向成分が、クォータニオンの虚数ｉの成分となる。同様に、ベクトルのグローバル座標系ＣのＹ軸方向成分がクォータニオンの虚数ｊの成分となり、ベクトルのグローバル座標系ＣのＺ軸方向成分がクォータニオンの虚数ｋの成分となる。
【０１２５】
そして、式（３９）、式（４０）のクォータ二オンを用いて、以下の関数Ｌ´Ｆ，ｋ（ｔ）を定義する。関数Ｌ´Ｆ，ｋ（ｔ）は、  
【数４１】
2012026184.tif 000043
【０１２６】
となる。ここで、ｔは媒介変数である。正規化のため、式（４１）の両辺をｓｉｎφＦ，ｋで割ると、  
【数４２】
2012026184.tif 000044
【０１２７】
となり、補間関数ＬＦ，ｋ（ｔ）が算出される。
【０１２８】
そして、補間関数ＬＦ，ｋ（ｔ）でｔを０から１に変化させることにより、センサユニットＳｋ−１の最終位置ＰＦ，ｋ−１とセンサユニットＳｋの最終位置ＰＦ，ｋとの間が曲線補間され、最終円弧ＬＦ，ｋが形成される。この際、最終位置ＰＦ，ｋ−１と最終位置ＰＦ，ｋとの間は、媒介変数ｔの変化に対して等角速度で曲線補間される。例えば、ｔが０から０．１に変化する間に補間される距離ｄ１と、ｔが０．５から０．６に変化する間に補間される距離ｄ２は同一である（図１８参照）。したがって、最終位置ＰＦ，ｋ−１と最終位置ＰＦ，ｋとの間を補間する際に、角速度の変化を考慮する必要がないため、最終円弧ＬＦ，ｋを形成する処理が単純化される。
【０１２９】
なお、最終円弧ＬＦ，ｋ以外の最終円弧ＬＦ，ｊについても、補間関数ＬＦ，ｊ（ｔ）を用いて形成される。補間関数ＬＦ，ｊ（ｔ）は、式（３７）〜式（４２）のｋをｊで置換した式を用いて、算出される。この場合も、それぞれのセンサユニットＳｉの最終位置ＰＦ，ｉの間は、補間関数ＬＦ，ｊ（ｔ）の媒介変数ｔの変化に対して等角速度で曲線補間される。すべての最終円弧ＬＦ，ｊが形成されることにより、内視鏡１０の挿入部１１の最終曲線形状８５が検出される。
【０１３０】
図４に示すように、最終曲線形状検出部６５により最終曲線形状８５が検出されると、内視鏡形状検出装置１による検査が完了したかを確認する（ステップＳ１０８）。検査が完了していない場合は（ステップＳ１０８−Ｎｏ）、ステップＳ１０１に戻り、次の静状態での内視鏡１０の挿入部１１の形状検出を行う。検査が完了している場合は（ステップＳ１０８−Ｙｅｓ）、内視鏡１０の挿入部１１の形状検出は終了する。
【０１３１】
そこで、上記構成の内視鏡形状検出装置１及び内視鏡形状検出装置１を用いた内視鏡１０の挿入部１１の形状検出方法では、以下の効果を奏する。すなわち、内視鏡形状検出装置１では、それぞれのセンサユニットＳｉの計測データから姿勢検出部３０がセンサユニットＳｉの姿勢を検出し、それぞれのセンサユニットＳｉの姿勢からセンサ仮位置検出部４０でそれぞれのセンサユニットＳｉの仮位置Ｐ０，ｉを検出する。そして、仮曲線形状検出部５０が、それぞれのセンサユニットＳｉの仮位置Ｐ０，ｉの間を仮円弧Ｌ０，ｊで曲線補間を行い、仮曲線形状７５を検出する。そして、センサ位置修正部５５が、仮曲線形状７５のそれぞれの仮円弧Ｌ０，ｊの弧長Ｒ０，ｊφ０，ｊとセンサ間寸法ｌとの差の絶対値に基づいて、それぞれのセンサユニットＳｉの位置を修正する。そして、最終曲線形状検出部６５が位置修正されたそれぞれのセンサユニットＳｉの最終位置ＰＦ，ｉの間を最終円弧ＬＦ，ｊで曲線補間を行い、最終曲線形状８５を検出する。以上のように、観察時に体腔内に挿入される挿入部１１に配置されるセンサユニットＳｉの計測データから、挿入部１１の最終曲線形状８５が検出されるため、体外にセンスコイル等を設ける必要がない。このため、内視鏡形状検出装置１の小型化、単純化を実現することができる。
【０１３２】
また、センサ位置順次補正部５７によりそれぞれのセンサユニットＳｉの位置を補正する際には、センサ移動部６１がセンサユニットＳｉを移動前位置Ｑｔ−１，ｉから移動後位置Ｑｔ，ｉまで移動する。そして、センサ移動部６１によりセンサユニットＳｉの移動が１回行われるたびに、移動後円弧形成部６２により、センサユニットＳｉに対して基端側に隣設されるセンサユニットＳｉ−１の最終位置ＰＦ，ｉ−１とセンサユニットＳｉの移動後位置Ｑｔ，ｉとの間が曲線補間され、移動後円弧Ｌｔ，ｊが形成される。そして、移動後円弧Ｌｔ，ｊが形成された後、移動制御部６３により移動後円弧Ｌｔ，ｊの弧長Ｒｔ，ｊφｔ，ｊとセンサ間寸法ｌとの差の絶対値であるエネルギーＥｔ，ｊが算出される。エネルギーＥｔ，ｊが所定の閾値より大きい場合は、センサ移動部６１がセンサユニットＳｉを位置Ｑｔ，ｉからさらに移動させる。エネルギーＥｔ，ｊが所定の閾値以下の場合は、最終位置決定部６４が、センサ移動部６１によるセンサユニットＳｉの最終回の移動での移動後位置ＱＦ，ｉを、補正対象センサ８１であるセンサユニットＳｉの最終位置ＰＦ，ｉとして決定する。すなわち、エネルギーＥｔ，ｊが所定の閾値以下になるまで、センサ移動部６１によるセンサユニットＳｉの移動が繰り返される。以上のように、それぞれのセンサユニットＳｉの間の実際の寸法であるセンサ間寸法ｌに基づいて、センサ位置順次補正部５７によるそれぞれのセンサユニットＳｉの位置補正が行われるため、実際の挿入部１１の形状と誤差の少ない最終曲線形状８５を検出することができる。これにより、高い精度で挿入部１１の最終曲線形状８５を検出することができる。
【０１３３】
また、センサ位置修正部５５によりセンサユニットＳｉの位置を修正すると、センサユニットＳｉに対して基端側に隣設されるセンサユニットＳｉ−１とセンサユニットＳｉとの間の形状のみが影響を受け、仮円弧Ｌ０，ｊが最終円弧ＬＦ，ｊに修正される。すなわち、１つのセンサユニットＳｉの位置修正が行われるたびに、挿入部１１の形状全体が修正される構成でない。ここで、センサ間寸法ｌは、それぞれのセンサユニットＳｉの間の長手方向の寸法であり、挿入部１１の局所的なパラメータである。すなわち、センサ間寸法ｌは、挿入部１１全体の形状に影響を与えるパラメータではなく、挿入部１１の形状に局所的に影響を与えるパラメータである。したがって、１つのセンサユニットＳｉの位置の修正により、位置修正されたセンサユニットＳｉの近傍で局所的に挿入部１１の形状が修正されるため、高い精度で仮円弧Ｌ０，ｊから最終円弧ＬＦ，ｊへの修正が行われる。これにより、挿入部１１の最終曲線形状８５を、高い精度で検出することができる。また、１つのセンサユニットＳｉの位置の修正により、位置修正されたセンサユニットＳｉの近傍でのみ局所的に挿入部１１の形状が修正されるため、挿入部１１の形状を修正する処理を単純化することができる。
【０１３４】
また、最終曲線形状検出部６５では、補間関数ＬＦ，ｊ（ｔ）でｔを０から１に変化させることにより、それぞれのセンサユニットＳｉの最終位置ＰＦ，ｉの間が曲線補間され、それぞれの最終円弧ＬＦ，ｊが形成される。この際、それぞれのセンサユニットＳｉの最終位置ＰＦ，ｉの間は、媒介変数ｔの変化に対して等角速度で曲線補間される。したがって、それぞれの最終位置ＰＦ，ｉの間を補間する際に、角速度の変化を考慮する必要がないため、それぞれの最終円弧ＬＦ，ｊを形成する処理を単純化することができる。
【０１３５】
さらに、内視鏡形状検出装置１では、挿入部１１が移動していない静状態において、加速度センサＡｉが重力加速度を計測し、地磁気センサＢｉが地磁気を計測する。そして、計測された重力加速度、地磁気から姿勢検出部３０がそれぞれのセンサユニットＳｉの姿勢を検出する。静状態では、重力加速度及び地磁気は、常に一定の方向に一定の大きさを有する。重力加速度、地磁気からそれぞれのセンサユニットＳｉの姿勢を検出するため、静状態においても高い精度でセンサユニットＳｉの姿勢を検出することができる。これにより、高い精度で挿入部１１の最終曲線形状８５を検出することができる。
【０１３６】
（第２の実施形態）  
次に、本発明の第２の実施形態について図１９を参照して説明する。なお、第１の実施形態と同一の部分及び同一の機能を有する部分については同一の符号を付して、その説明は省略する。
【０１３７】
本実施形態のセンサ位置修正部５５は、第１の実施形態と同様に、基端側（グローバル座標系Ｃの原点に近い側）のセンサユニットＳｉから順次に、それぞれのセンサユニットＳｉの補正前位置Ｑ０，ｉから最終位置ＰＦ，ｉへの位置補正を行うセンサ位置順次補正部５７と、センサ位置順次補正部５７により１つのセンサユニットＳｉの位置補正が行われるたびに、位置補正が完了していないセンサユニットＳｉである未補正センサ群７７を平行移動させる未補正センサ群移動部５９と、を備える。
【０１３８】
センサ位置順次補正部５７は、第１の実施形態と同様に、位置補正の対象である補正対象センサ８１を補正前位置Ｑ０，ｉから１回以上移動させるセンサ移動部６１を備える。センサ移動部６１での１回の移動により、補正対象センサ８１は移動前位置Ｑｔ−１，ｉから移動後位置Ｑｔ，ｉまで移動する。また、センサ位置順次補正部５７は、センサ移動部６１により補正対象センサ８１の移動が１回行われるたびに、補正対象センサ８１に対して基端側（グローバル座標系Ｃの原点に近い側）に隣設されるセンサユニットＳｉである近位側隣設センサ８２の最終位置ＰＦ，ｉ−１と補正対象センサ８１の移動後位置Ｑｔ，ｉとの間を曲線補間する移動後円弧形成部６２とを備える。移動後円弧形成部６２で近位側隣設センサ８２の最終位置ＰＦ，ｉ−１と補正対象センサ８１の移動後位置Ｑｔ，ｉとの間を曲線補間することにより、移動後円弧Ｌｔ，ｊが形成される。
【０１３９】
センサ位置順次補正部５７は、近位側隣設センサ８２の最終位置ＰＦ，ｉ−１と補正対象センサ８１の移動前の移動前位置Ｑｔ−１，ｉとの間を曲線補間して形成される移動前円弧Ｌｔ−１，ｊの弧長Ｒｔ−１，ｊφｔ−１，ｊとセンサ間寸法ｌとの差の絶対値に対して、移動後円弧Ｌｔ，ｊの弧長Ｒｔ，ｊφｔ，ｊとセンサ間寸法ｌとの差の絶対値が小さくなる補正対象センサ８１の移動を所定の回数だけ繰り返す状態にセンサ移動部６１を制御する移動制御部６３を備える。ここで、移動前円弧Ｌｔ−１，ｊの弧長Ｒｔ−１，ｊφｔ−１，ｊとセンサ間寸法ｌとの差の絶対値をエネルギーＥｔ−１，ｊとする。同様に、移動後円弧Ｌｔ，ｊの弧長Ｒｔ，ｊφｔ，ｊとセンサ間寸法ｌとの差の絶対値をエネルギーＥｔ，ｊとする。また、センサ位置順次補正部５７は、センサ移動部６１による最終回の移動での移動後位置ＱＦ，ｉを、補正対象センサ８１の最終位置ＰＦ，ｉとして決定する最終位置決定部６４を備える。
【０１４０】
ここで、センサ位置修正部５５で、それぞれのセンサユニットＳｉの位置を修正する方法について説明する。センサ位置修正部５５は、第１の実施形態と同様に、仮曲線形状検出部５０により検出された仮曲線形状７５のそれぞれの仮円弧Ｌ０，ｊの弧長Ｒ０，ｊφ０，ｊとセンサ間寸法ｌとの差の絶対値に基づいて、それぞれのセンサユニットＳｉの位置を修正する（図４のステップＳ１０６）。すなわち、式（２２）で求められるエネルギーＥ０，ｊ基づいて、それぞれのセンサユニットＳｉの位置を修正する。これにより、それぞれのセンサユニットＳｉは、仮位置Ｐ０，ｉから最終位置ＰＦ，ｉまで修正される。
【０１４１】
図１９は、センサ位置修正部５５によりそれぞれのセンサユニットＳｉの位置を修正する方法を示すフローチャートである。図１９に示すように、それぞれのセンサユニットＳｉの位置を修正する際は、第１の実施形態と同様に、センサ位置順次補正部５７により基端側（グローバル座標系Ｃの原点に近い側）のセンサユニットＳｉから順次に、それぞれのセンサユニットＳｉの補正前位置Ｑ０，ｉから最終位置ＰＦ，ｉへの位置補正が行われる。センサ位置順次補正部５７は、未補正センサ群７７の中で最も基端側のセンサユニットＳｉの位置補正を開始する（ステップＳ１４１）。すなわち、センサユニットＳｋ−１まで補正前位置Ｑ０，ｉから最終位置ＰＦ，ｉへの位置補正が完了している場合には、センサ位置順次補正部５７により、未補正センサ群７７の中で最も基端側のセンサユニットＳｋの最終位置ＰＦ，ｋへの位置補正が開始される。この場合、センサユニットＳｋが位置補正の対象である補正対象センサ８１となる。なお、以下の説明では、基端側から（ｋ＋１）番目のセンサユニットＳｋの位置補正について説明する。
【０１４２】
補正対象センサ８１であるセンサユニットＳｋの位置補正を開始する際、センサユニットＳｋ−１まで最終位置ＰＦ，ｉへの位置補正が完了している。この際、センサユニットＳｋは、補正前位置Ｑ０，ｋに位置している。この状態で、センサ移動部６１により、センサユニットＳｋが位置Ｑ０，ｋから位置Ｑ１，ｋに移動し（ステップＳ１４２）、センサユニットＳｋの１回目の移動が行われる。センサ移動部６１によるセンサユニットＳｋの移動は、１回以上行われる。そして、ｓ回目の移動では、補正対象センサ８１であるセンサユニットＳｋは移動前位置Ｑｓ−１，ｋから移動後位置Ｑｓ，ｋまで移動する。すなわち、センサ移動部６１による１回の移動により、センサユニットＳｋは移動前位置Ｑｔ−１，ｋから移動後位置Ｑｔ，ｋまで移動する（ステップＳ１４２）。
【０１４３】
そして、センサ移動部６１によりセンサユニットＳｋの移動が１回行われるたびに、移動後円弧形成部６２により、センサユニットＳｋに対して基端側に隣設されるセンサユニットＳｋ−１の最終位置ＰＦ，ｋ−１とセンサユニットＳｋの移動後位置Ｑｔ，ｋとの間が曲線補間され、移動後円弧Ｌｔ，ｋが形成される（ステップＳ１４３）。ｓ回目の移動後には、センサユニットＳｋ−１の最終位置ＰＦ，ｋ−１とセンサユニットＳｋの移動後位置Ｑｓ，ｋとの間が曲線補間され、移動後円弧Ｌｓ，ｋが形成される。ここで、センサユニットＳｋ−１は、補正対象センサ８１であるセンサユニットＳｋに対してグローバル座標系Ｃの原点に近い側に隣設される近位側隣設センサ８２である。
【０１４４】
センサユニットＳｋのｓ回目に移動後に移動後円弧Ｌｓ，ｋを形成する際は、第１の実施形態と同様に式（２３．１）〜式（２８）を用いて、移動後円弧Ｌｓ，ｋのパラメータが算出される。そして算出されたパラメータを用いて、最終位置ＰＦ，ｋ−１と移動後位置Ｑｓ，ｋとの間の移動後円弧Ｌｓ，ｋが形成される。
【０１４５】
なお、センサユニットＳｋのｓ回目に移動後に移動後円弧Ｌｓ，ｋを形成する処理について説明したが、センサユニットＳｋのｓ回目以外の移動後についても、式（２３．１）〜式（２８）で、ｓをｔで置換した式を用いて同様に、センサユニットＳｋ−１の最終位置ＰＦ，ｋ−１とセンサユニットＳｋの移動後位置Ｑｔ，ｋとの間の移動後円弧Ｌｔ，ｋが形成される。
【０１４６】
センサ移動部６１は、移動制御部６３により、近位側隣設センサ８２の最終位置ＰＦ，ｋ−１と補正対象センサ８１の移動前の移動前位置Ｑｔ−１，ｋとの間を曲線補間して形成される移動前円弧Ｌｔ−１，ｋの弧長Ｒｔ−１，ｋφｔ−１，ｋとセンサ間寸法ｌとの差の絶対値に対して、移動後円弧Ｌｔ，ｋの弧長Ｒｔ，ｋφｔ，ｋとセンサ間寸法ｌとの差の絶対値が小さくなる補正対象センサ８１の移動を行う状態に制御されている。ここで、移動後円弧Ｌｔ，ｋの弧長Ｒｔ，ｋφｔ，ｋとセンサ間寸法ｌとの差の絶対値をエネルギーＥｔ，ｋとし、第１の実施形態と同様に式（２９）で与えられる。また、移動前円弧Ｌｔ−１，ｋの弧長Ｒｔ−１，ｋφｔ−１，ｋとセンサ間寸法ｌとの差の絶対値をエネルギーＥｔ−１，ｋとし、式（２９）のｔをｔ−１で置換した式で与えられる。すなわち、センサ移動部６１は、移動後のエネルギーＥｔ，ｋが移動前のエネルギーＥｔ−１，ｋより小さくなる状態に補正対象センサ８１であるセンサユニットＳｋの移動を行っている（ステップＳ１４２）。なお、例えばセンサユニットＳｋのｓ回目の移動での移動前円弧Ｌｓ−１，ｋは、センサユニットＳｋの（ｓ−１）回目の移動での移動後円弧Ｌｓ−１，ｋと同一の円弧である。センサユニットＳｋのｓ回目の移動での移動前円弧Ｌｓ−１，ｋは、式（２３．１）〜式（２８）でｓをｓ−１で置換した式を用いて、形成される。
【０１４７】
そして、移動制御部６３が、補正対象センサ８１であるセンサユニットＳｋの移動を所定の回数だけ行ったかを判断する（ステップＳ１４４）。センサユニットＳｋの移動を所定の回数だけ行っていない場合は（ステップＳ１４４−Ｎｏ）、ステップＳ１２２に戻り、センサ移動部６１がセンサユニットＳｋを位置Ｑｔ，ｋからさらに移動させる。この場合、センサユニットＳｋのｓ回目の移動後に、センサ移動部６１により（ｓ＋１）回目の移動が行われる。（ｓ＋１）回目の移動により、センサユニットＳｋは移動前位置Ｑｓ，ｋから移動後位置Ｑｓ＋１，ｋに移動する。以上のように、センサ移動部６１は、補正対象センサ８１であるセンサユニットＳｋの移動前位置Ｑｔ−１，ｋから移動後位置Ｑｔ，ｋへの移動を所定の回数だけ繰り返し行う状態に、移動制御部６３により制御されている。
【０１４８】
センサユニットＳｋの移動が所定の回数だけ行われた場合は（ステップＳ１４４−Ｙｅｓ）、次のステップに進む。そして、最終位置決定部６４が、センサ移動部６１によるセンサユニットＳｋの最終回（例えばＦ回目）の移動での移動後位置ＱＦ，ｋを、補正対象センサ８１であるセンサユニットＳｋの最終位置ＰＦ，ｋとして決定する（ステップＳ１４５）。以上のようにして、センサユニットＳｋの最終位置ＰＦ，ｋへの位置修正が完了する。
【０１４９】
なお、センサユニットＳｋ以外のセンサユニットＳｉについても、センサ位置順次補正部５７によりセンサユニットＳｋと同様に最終位置Ｐｉ，ｋへの位置補正が行われる。すなわち、センサ移動部６１により、センサユニットＳｉは移動前位置Ｑｔ−１，ｉから移動後位置Ｑｔ，ｉまで移動する（ステップＳ１４２）。そして、センサ移動部６１によりセンサユニットＳｉの移動が１回行われるたびに、移動後円弧形成部６２により、センサユニットＳｉに対して基端側に隣設されるセンサユニットＳｉ−１の最終位置ＰＦ，ｉ−１とセンサユニットＳｉの移動後位置Ｑｔ，ｉとの間が曲線補間され、移動後円弧Ｌｔ，ｊが形成される（ステップＳ１４３）。センサ移動部６１は、移動前円弧Ｌｔ−１，ｊの弧長Ｒｔ−１，ｊφｔ−１，ｊとセンサ間寸法ｌとの差の絶対値に対して、移動後円弧Ｌｔ，ｊの弧長Ｒｔ，ｊφｔ，ｊとセンサ間寸法ｌとの差の絶対値が小さくなる状態に、センサユニットＳｉの移動を行っている（ステップＳ１４２）。すなわち、移動後のエネルギーＥｔ，ｊが移動前のエネルギーＥｔ−１，ｊより小さくなる状態に、補正対象センサ８１であるセンサユニットＳｉの移動を行っている。
【０１５０】
そして、移動後円弧Ｌｔ，ｊが形成された後、移動制御部６３によりセンサユニットＳｉの移動が所定の回数だけ行われたか判断される（ステップＳ１４４）。センサユニットＳｉの移動が所定の回数だけ行われていない場合は（ステップＳ１４４−Ｎｏ）、ステップＳ１４２に戻り、センサ移動部６１がセンサユニットＳｉを位置Ｑｔ，ｉからさらに移動させる。センサユニットＳｉの移動が所定の回数だけ行われた場合は（ステップＳ１４４−Ｙｅｓ）、次のステップに進む。そして、最終位置決定部６４が、センサ移動部６１によるセンサユニットＳｉの最終回の移動での移動後位置ＱＦ，ｉを、補正対象センサ８１であるセンサユニットＳｉの最終位置ＰＦ，ｉとして決定する（ステップＳ１４５）。以上のようにして、センサユニットＳｉの最終位置ＰＦ，ｉへの位置補正が完了する。
【０１５１】
そして、第１の実施形態と同様に、センサ位置順次補正部５７により１つのセンサユニットＳｉの位置補正が行われるたびに、未補正センサ群移動部５９により、位置補正が行われていないセンサユニットＳｉである未補正センサ群７７が平行移動される（ステップＳ１４６）。未補正センサ群移動部５９での処理は、第１の実施形態と同様であるため、詳細な説明は省略する。
【０１５２】
そして、すべてのセンサユニットＳｉについて位置補正が完了したか確認する（ステップＳ１４７）。すべてのセンサユニットＳｉについて位置補正が完了している場合は（ステップＳ１４７−Ｙｅｓ）、次のステップへ進む。すべてのセンサユニットＳｉについて位置補正が完了していない場合は（ステップＳ１４７−Ｎｏ）、ステップＳ１４１に戻り、センサ位置順次補正部５７により、未補正センサ群７７の中で最も基端側のセンサユニットＳｉの位置補正が行われる。すなわち、ステップＳ１４１〜Ｓ１４６が、すべてのセンサユニットＳｉについて位置補正が完了するまで、繰り返し行われる。
【０１５３】
そこで、上記構成の内視鏡形状検出装置１及び内視鏡形状検出装置１を用いた内視鏡１０の挿入部１１の形状検出方法では、第１の実施形態と同様の効果に加えて、以下の効果を奏する。すなわち、内視鏡形状検出装置１では、センサ位置順次補正部５７によりそれぞれのセンサユニットＳｉの位置を補正する際に、センサ移動部６１がセンサユニットＳｉを移動前位置Ｑｔ−１，ｉから移動後位置Ｑｔ，ｉまで移動する。そして、センサ移動部６１によりセンサユニットＳｉの移動が１回行われるたびに、移動後円弧形成部６２により、センサユニットＳｉに対して基端側に隣設されるセンサユニットＳｉ−１の最終位置ＰＦ，ｉ−１とセンサユニットＳｉの移動後位置Ｑｔ，ｉとの間が曲線補間され、移動後円弧Ｌｔ，ｊが形成される。センサ移動部６１は、移動前円弧Ｌｔ−１，ｊの弧長Ｒｔ−１，ｊφｔ−１，ｊとセンサ間寸法ｌとの差の絶対値に対して、移動後円弧Ｌｔ，ｊの弧長Ｒｔ，ｊφｔ，ｊとセンサ間寸法ｌとの差の絶対値が小さくなる状態に、補正対象センサ８１であるセンサユニットＳｉの移動を行っている。そして、移動後円弧Ｌｔ，ｊが形成された後、移動制御部６３によりセンサユニットＳｉの移動が所定の回数だけ行われたか判断される。センサユニットＳｉの移動が所定の回数だけ行われていない場合は、センサ移動部６１がセンサユニットＳｉを位置Ｑｔ，ｉからさらに移動させる。センサユニットＳｉの移動が所定の回数だけ行われた場合は、最終位置決定部６４が、センサ移動部６１によるセンサユニットＳｉの最終回の移動での移動後位置ＱＦ，ｉを、補正対象センサ８１であるセンサユニットＳｉの最終位置ＰＦ，ｉとして決定する。すなわち、センサユニットＳｉの移動が所定の回数だけ行われるまで、センサ移動部６１によるセンサユニットＳｉの移動が繰り返される。以上のように、それぞれのセンサユニットＳｉの間の実際の寸法であるセンサ間寸法ｌに基づいて、センサ位置順次補正部５７によるそれぞれのセンサユニットＳｉの位置補正が行われるため、実際の挿入部１１の形状と誤差の少ない最終曲線形状８５を検出することができる。これにより、高い精度で挿入部１１の最終曲線形状８５を検出することができる。
【０１５４】
（変形例）  
なお、上述の実施形態では、それぞれのローカル座標系Ｃｉは、Ｙｉ軸方向がセンサユニットＳｉの中心での長手方向と一致している座標系である。しかし、それぞれのローカル座標系Ｃｉは、センサユニットＳｉの中心を原点とし、Ｘｉ軸、Ｙｉ軸、Ｚｉ軸のいずれか１つの軸がセンサユニットＳｉの中心での長手方向と軸方向が一致する長手方向軸である座標系であればよい。ただし、Ｘｉ軸が長手方向軸である場合は、式（１６．１），式（１６．２）の代わりに、  
【数４３】
2012026184.tif 000045
【０１５５】
を用いる。同様に、Ｚｉ軸が長手方向軸である場合は、式（１６．１），式（１６．２）の代わりに、  
【数４４】
2012026184.tif 000046
【０１５６】
を用いる。ここで、式（４３．２）のベクトルはローカル座標系Ｃｋ−１の原点での長手方向であるＸｋ−１軸方向の単位ベクトルであり、式（４４．２）のベクトルはローカル座標系Ｃｋ−１の原点での長手方向であるＺｋ−１軸方向の単位ベクトルである。
【０１５７】
また、上述の実施形態では、グローバル座標系Ｃは、最も基端側のセンサユニットＳ０の中心を原点とし、Ｚ軸が鉛直方向と一致し、Ｘ軸及びＹ軸が水平面上に配置される座標系である。しかし、Ｘ軸、Ｙ軸、Ｚ軸のいずれか１つの軸が鉛直方向と軸方向が一致する鉛直方向軸であり、鉛直方向軸以外の２つの軸が水平面上に配置される水平方向軸である座標系であればよい。これにより、加速度センサＡｉにより計測される重力加速度及び地磁気センサＢｉにより計測される地磁気に基づいて、姿勢検出部３０はそれぞれのセンサユニットＳｉの姿勢を検出することが可能である。ただし、Ｘ軸が鉛直方向軸の場合は、式（２）で示される重力加速度ベクトルのグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分は、  
【数４５】
2012026184.tif 000047
【０１５８】
となる。この場合、第１の実施形態では、姿勢角αｉ，βｉ，γｉはヨー角γｉ、ピッチ角αｉ、ロール角βｉの順で回転する（Ｚ，Ｘ，Ｙ）型としたが、姿勢角αｉ，βｉ，γｉの回転する順序を変えて、式（１）の回転行列とは別の回転行列を用いる。これにより、加速度センサＡｉで計測された加速度データに基づいて、第１の角度算出部３４で水平方向軸であるＹ軸，Ｚ軸回りの姿勢角βｉ，γｉが算出される。また、地磁気センサＢｉで計測された地磁気データに基づいて、第２の角度算出部３６で鉛直方向軸であるＸ軸回りの姿勢角αｉが算出される。Ｙ軸が鉛直方向軸の場合についても同様であり、加速度センサＡｉで計測された加速度データに基づいて、第１の角度算出部３４で水平方向軸であるＸ軸，Ｚ軸回りの姿勢角αｉ，γｉが算出される。また、地磁気センサＢｉで計測された地磁気データに基づいて、第２の角度算出部３６で鉛直方向軸であるＹ軸回りの姿勢角βｉが算出される。
【０１５９】
さらに、上述の実施形態では、グローバル座標系Ｃは、最も基端側のセンサユニットＳ０の中心を原点としているが、最も先端側のセンサユニットＳＮの中心を原点としてもよい。この場合、仮リンク形成部４１は、式（１６．１），式（１６．２）のｋをｊ＋１で置換した式を用いて、仮リンクＴ０，ｊの先端側（グローバル座標系Ｃの原点から近い側）のセンサユニットＳｊから仮リンクＴ０，ｊの基端側（グローバル座標系Ｃの原点から遠い側）のセンサユニットＳｊ−１へのベクトルを求める。そして、センサユニットＳｊからセンサユニットＳｊ−１へのベクトルにより、仮リンクＴ０，ｊが形成される。すなわち、仮リンク形成部４１は、仮リンクＴ０，ｊが先端側（グローバル座標系Ｃの原点から近い側）のセンサユニットＳｊの中心から基端側（グローバル座標系Ｃの原点から遠い側）のセンサユニットＳｊ−１の中心まで先端側のセンサユニットＳｊの中心での長手方向に延設されると仮定して、仮リンクＴ０，ｊを形成する。また、センサ位置修正部５５のセンサ位置順次補正部５７によりそれぞれのセンサユニットＳｉの位置補正を行う際は、先端側（グローバル座標系Ｃの原点から近い側）のセンサユニットＳｉから順次に位置補正が行われる。この場合、補正対象センサ８１の先端側（グローバル座標系Ｃの原点に近い側）に隣設されるセンサユニットＳｉが近位側隣設センサ８２となる。
【０１６０】
以上、本発明の実施形態について説明したが、本発明は上記の実施形態に限定されるものではなく、本発明の要旨を逸脱しない範囲で種々の変形ができることは勿論である。
【図１】
2012026184.tif 000048
【図２】
2012026184.tif 000049
【図３】
2012026184.tif 000050
【図４】
2012026184.tif 000051
【図５】
2012026184.tif 000052
【図６】
2012026184.tif 000053
【図７】
2012026184.tif 000054
【図８】
2012026184.tif 000055
【図９】
2012026184.tif 000056
【図１０】
2012026184.tif 000057
【図１１】
2012026184.tif 000058
【図１２】
2012026184.tif 000059
【図１３】
2012026184.tif 000060
【図１４】
2012026184.tif 000061
【図１５】
2012026184.tif 000062
【図１６】
2012026184.tif 000063
【図１７】
2012026184.tif 000064
【図１８】
2012026184.tif 000065
【図１９】
2012026184.tif 000066
【手続補正書】
【提出日】平成23年9月1日(2011.9.1)
【手続補正１】
【補正対象書類名】特許請求の範囲
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【特許請求の範囲】
【請求項１】
長手方向に互いに所定のセンサ間寸法だけ離れて複数のセンサユニットが配置される挿入部を備える内視鏡と、
前記センサユニットの計測データに基づいて、それぞれの前記センサユニットの姿勢を検出する姿勢検出部と、
前記姿勢検出部により検出されたそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間を寸法が前記センサ間寸法に等しい直線状の仮リンクと仮定して、それぞれの前記センサユニットの仮位置を検出するセンサ仮位置検出部と、
前記センサ仮位置検出部により検出されたそれぞれの前記センサユニットの前記仮位置に基づいて、それぞれの前記センサユニットの前記仮位置の間を仮円弧で曲線補間を行い、前記挿入部の仮曲線形状を検出する仮曲線形状検出部と、
前記仮曲線形状検出部により検出された前記仮曲線形状のそれぞれの前記仮円弧の弧長と前記センサ間寸法との差の絶対値に基づいて、それぞれの前記センサユニットの位置を前記仮位置から最終位置まで修正するセンサ位置修正部と、
それぞれの前記センサユニットの前記最終位置の間を最終円弧で曲線補間し、前記挿入部の最終曲線形状を検出する最終曲線形状検出部と、
を具備する内視鏡形状検出装置。
【請求項２】
前記仮曲線形状検出部は、
それぞれの前記センサユニットの前記仮位置での単位接ベクトルを算出する単位接ベクトル算出部と、
前記単位接ベクトル算出部で算出されたそれぞれの前記単位接ベクトルに基づいて、それぞれの前記センサユニットの前記仮位置の間での前記単位接ベクトルの変化率を算出する変化率算出部と、
前記単位接ベクトル算出部で算出されたそれぞれの前記単位接ベクトル、及び、前記変化率算出部で算出されたそれぞれの前記変化率に基づいて、それぞれの前記センサユニットの前記仮位置の間の前記仮円弧を形成する仮円弧形成部と、
を備える請求項１の内視鏡形状検出装置。
【請求項３】
前記センサ位置修正部は、
最も基端側又は最も先端側の前記センサユニットの中心を原点とするグローバル座標系において、前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に位置補正を行い、それぞれの前記センサユニットを位置補正前の補正前位置から前記最終位置へ位置補正するセンサ位置順次補正部と、
前記センサ位置順次補正部により１つの前記センサユニットの位置補正が行われるたびに、直前の位置補正で補正対象だった前記センサユニットである直前補正対象センサの前記補正前位置から前記最終位置への補正量だけ、位置補正が完了していない前記センサユニットである未補正センサ群を平行移動させる未補正センサ群移動部と、
を備える請求項１の内視鏡形状検出装置。
【請求項４】
前記センサ位置順次補正部は、
位置補正の対象である補正対象センサを前記補正前位置から１回以上移動させ、１回の移動により前記補正対象センサを移動前位置から移動後位置まで移動させるセンサ移動部と、
前記センサ移動部により前記補正対象センサの移動が１回行われるたびに、前記補正対象センサに対して前記グローバル座標系の前記原点に近い側に隣設される前記センサユニットでるある近位側隣設センサの前記最終位置と前記補正対象センサの前記移動後位置との間を曲線補間し、移動後円弧を形成する移動後円弧形成部と、
を備える請求項３の内視鏡形状検出装置。
【請求項５】
前記センサ位置順次補正部は、
前記移動後円弧形成部により形成される前記移動後円弧の弧長と前記センサ間寸法との差の絶対値が所定の閾値以下になるまで、前記補正対象センサの前記移動前位置から前記移動後位置への移動を繰り返し行う状態に前記センサ移動部を制御する移動制御部と、
前記センサ移動部による最終回の移動での前記移動後位置を、前記補正対象センサの前記最終位置として決定する最終位置決定部と、
を備える請求項４の内視鏡形状検出装置。
【請求項６】
前記センサ位置順次補正部は、
前記近位側隣設センサの前記最終位置と前記補正対象センサの前記移動前位置との間を曲線補間して形成される移動前円弧の弧長と前記センサ間寸法との差の絶対値に対して、前記移動後円弧の弧長と前記センサ間寸法との差の絶対値が小さくなる前記補正対象センサの移動を所定の回数だけ繰り返す状態に、前記センサ移動部を制御する移動制御部と、
前記センサ移動部による最終回の移動での前記移動後位置を、前記補正対象センサの前記最終位置として決定する最終位置決定部と、
を備える請求項４の内視鏡形状検出装置。
【請求項７】
前記最終曲線形状検出部は、
それぞれの前記センサユニットの前記最終位置に基づいて、それぞれの前記最終円弧のパラメータを算出するパラメータ算出部と、
前記パラメータ算出部により算出された前記パラメータに基づいて、クォータニオン及び媒介変数を用いた補間関数により、前記媒介変数の変化に対して等角速度でそれぞれの前記センサユニットの前記最終位置の間を曲線補間し、前記最終円弧を形成する最終円弧形成部と、
を備える請求項１の内視鏡形状検出装置。
【請求項８】
内視鏡の挿入部に、長手方向に互いに所定のセンサ間寸法だけ離れて配置される複数のセンサユニットにより計測を行うことと、
前記センサユニットでの計測データに基づいて、それぞれの前記センサユニットの姿勢を姿勢検出部により検出することと、
前記姿勢検出部により検出されたそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間を寸法が前記センサ間寸法に等しい直線状の仮リンクと仮定して、それぞれの前記センサユニットの仮位置をセンサ仮位置検出部により検出することと、
前記センサ仮位置検出部により検出されたそれぞれの前記センサユニットの前記仮位置に基づいて、それぞれの前記センサユニットの前記仮位置の間を仮円弧で曲線補間を行い、前記挿入部の仮曲線形状を仮曲線形状検出部により検出することと、
前記仮曲線形状検出部により検出された前記仮曲線形状のそれぞれの前記仮円弧の弧長と前記センサ間寸法との差の絶対値に基づいて、それぞれの前記センサユニットの位置を前記仮位置から最終位置までセンサ位置修正部により修正することと、
それぞれの前記センサユニットの前記最終位置の間を最終円弧で曲線補間し、前記挿入部の最終曲線形状を最終曲線形状検出部により検出することと、
を具備する内視鏡の挿入部の形状検出方法。
【請求項９】
前記仮曲線形状検出部により前記仮曲線形状を検出することは、
それぞれの前記センサユニットの前記仮位置での単位接ベクトルを前記仮曲線形状検出部の単位接ベクトル算出部により算出することと、
前記単位接ベクトル算出部により算出されたそれぞれの前記単位接ベクトルに基づいて、それぞれの前記センサユニットの前記仮位置の間での前記単位接ベクトルの変化率を前記仮曲線形状検出部の変化率算出部により算出することと、
前記単位接ベクトル算出部により算出されたそれぞれの前記単位接ベクトル、及び、前記変化率算出部により算出されたそれぞれの前記変化率に基づいて、それぞれの前記センサユニットの前記仮位置の間の前記仮円弧を前記仮曲線形状検出部の仮円弧形成部により形成することと、
を備える請求項８の内視鏡の挿入部の形状検出方法。
【請求項１０】
前記センサ位置修正部によりそれぞれの前記センサユニットの位置を前記仮位置から前記最終位置まで修正することは、
最も基端側又は最も先端側の前記センサユニットの中心を原点とするグローバル座標系において、前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に位置補正を行い、それぞれの前記センサユニットを位置補正前の補正前位置から前記最終位置へ前記センサ位置修正部のセンサ位置順次補正部により位置補正することと、
前記センサ位置順次補正部により１つの前記センサユニットの位置補正が行われるたびに、直前の位置補正で補正対象だった前記センサユニットである直前補正対象センサの前記補正前位置から前記最終位置への補正量だけ、位置補正が完了していない前記センサユニットである未補正センサ群を前記センサ位置修正部の未補正センサ群移動部により平行移動させることと、
を備える請求項８の内視鏡の挿入部の形状検出方法。
【請求項１１】
前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に、それぞれの前記センサユニットの位置補正を前記センサ位置順次補正部により行うことは、
位置補正の対象である補正対象センサを前記補正前位置から１回以上移動させ、１回の移動により前記補正対象センサを移動前位置から移動後位置まで前記センサ位置順次補正部のセンサ移動部により移動させることと、
前記センサ移動部により前記補正対象センサの移動が１回行われるたびに、前記補正対象センサに対して前記グローバル座標系の前記原点に近い側に隣設される前記センサユニットでるある近位側隣設センサの前記最終位置と前記補正対象センサの前記移動後位置との間を曲線補間し、移動後円弧を前記センサ位置順次補正部の移動後円弧形成部により形成することと、
を備える請求項１０の内視鏡の挿入部の形状検出方法。
【請求項１２】
前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に、それぞれの前記センサユニットの位置補正を前記センサ位置順次補正部により行うことは、
前記移動後円弧形成部により形成される前記移動後円弧の弧長と前記センサ間寸法との差の絶対値が所定の閾値以下になるまで、前記センサ移動部により前記補正対象センサの前記移動前位置から前記移動後位置への移動が繰り返し行われる状態に前記センサ位置順次補正部の移動制御部により制御することと、
前記センサ移動部による最終回の移動での前記移動後位置を、前記補正対象センサの前記最終位置として前記センサ位置順次補正部の最終位置決定部により決定することと、
を備える請求項１１の内視鏡の挿入部の形状検出方法。
【請求項１３】
前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に、それぞれの前記センサユニットの位置補正を前記センサ位置順次補正部により行うことは、
前記近位側隣設センサの前記最終位置と前記補正対象センサの前記移動前位置との間を曲線補間して形成される移動前円弧の弧長と前記センサ間寸法との差の絶対値に対して、前記移動後円弧の弧長と前記センサ間寸法との差の絶対値が小さくなる前記補正対象センサの移動を前記センサ移動部により所定の回数だけ繰り返す状態に前記センサ位置順次補正部の移動制御部により制御することと、
前記センサ移動部による最終回の移動での前記移動後位置を、前記補正対象センサの前記最終位置として前記センサ位置順次補正部の最終位置決定部により決定することと、
を備える請求項１１の内視鏡の挿入部の形状検出方法。
【請求項１４】
前記最終曲線形状検出部により前記挿入部の最終曲線形状を検出することは、
それぞれの前記センサユニットの前記最終位置に基づいて、それぞれの前記最終円弧のパラメータを前記最終曲線形状検出部のパラメータ算出部により算出することと、
前記パラメータ算出部により算出された前記パラメータに基づいて、クォータニオン及び媒介変数を用いた補間関数により、前記媒介変数の変化に対して等角速度でそれぞれの前記センサユニットの前記最終位置の間を曲線補間し、前記最終円弧を前記最終曲線形状算出部の最終円弧形成部により形成することと、
を備える請求項８の内視鏡の挿入部の形状検出方法。
【手続補正２】
【補正対象書類名】明細書
【補正対象項目名】００１０
【補正方法】変更
【補正の内容】
【００１０】  また、本発明の別のある態様では、内視鏡の挿入部に、長手方向に互いに所定のセンサ間寸法だけ離れて配置される複数のセンサユニットにより計測を行うことと、  前記センサユニットでの計測データに基づいて、それぞれの前記センサユニットの姿勢を姿勢検出部により検出することと、前記姿勢検出部により検出されたそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間を寸法が前記センサ間寸法に等しい直線状の仮リンクと仮定して、それぞれの前記センサユニットの仮位置をセンサ仮位置検出部により検出することと、前記センサ仮位置検出部により検出されたそれぞれの前記センサユニットの前記仮位置に基づいて、それぞれの前記センサユニットの前記仮位置の間を仮円弧で曲線補間を行い、前記挿入部の仮曲線形状を仮曲線形状検出部により検出することと、前記仮曲線形状検出部により検出された前記仮曲線形状のそれぞれの前記仮円弧の弧長と前記センサ間寸法との差の絶対値に基づいて、それぞれの前記センサユニットの位置を前記仮位置から最終位置までセンサ位置修正部により修正することと、  それぞれの前記センサユニットの前記最終位置の間を最終円弧で曲線補間し、前記挿入部の最終曲線形状を最終曲線形状検出部により検出することと、を備える内視鏡の挿入部の形状検出方法を提供する。
【発明の効果】
【手続補正書】
【提出日】平成23年11月16日(2011.11.16)
【手続補正１】
【補正対象書類名】特許請求の範囲
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【特許請求の範囲】
【請求項１】
長手方向に互いに所定のセンサ間寸法だけ離れて複数のセンサユニットが配置される挿入部を備える内視鏡と、
前記センサユニットの計測データに基づいて、それぞれの前記センサユニットの姿勢を検出する姿勢検出部と、
前記姿勢検出部により検出されたそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間を寸法が前記センサ間寸法に等しい直線状の仮リンクと仮定して、それぞれの前記センサユニットの仮位置を検出するセンサ仮位置検出部と、
前記センサ仮位置検出部により検出されたそれぞれの前記センサユニットの前記仮位置に基づいて、それぞれの前記センサユニットの前記仮位置の間を仮円弧で曲線補間を行い、前記挿入部の仮曲線形状を検出する仮曲線形状検出部と、
前記仮曲線形状検出部により検出された前記仮曲線形状のそれぞれの前記仮円弧の弧長と前記センサ間寸法との差の絶対値に基づいて、それぞれの前記センサユニットの位置を前記仮位置から最終位置まで修正するセンサ位置修正部と、
それぞれの前記センサユニットの前記最終位置の間を最終円弧で曲線補間し、前記挿入部の最終曲線形状を検出する最終曲線形状検出部と、
を具備する内視鏡形状検出装置。
【請求項２】
前記仮曲線形状検出部は、
それぞれの前記センサユニットの前記仮位置での単位接ベクトルを算出する単位接ベクトル算出部と、
前記単位接ベクトル算出部で算出されたそれぞれの前記単位接ベクトルに基づいて、それぞれの前記センサユニットの前記仮位置の間での前記単位接ベクトルの変化率を算出する変化率算出部と、
前記単位接ベクトル算出部で算出されたそれぞれの前記単位接ベクトル、及び、前記変化率算出部で算出されたそれぞれの前記変化率に基づいて、それぞれの前記センサユニットの前記仮位置の間の前記仮円弧を形成する仮円弧形成部と、
を備える請求項１の内視鏡形状検出装置。
【請求項３】
前記センサ位置修正部は、
最も基端側又は最も先端側の前記センサユニットの中心を原点とするグローバル座標系において、前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に位置補正を行い、それぞれの前記センサユニットを位置補正前の補正前位置から前記最終位置へ位置補正するセンサ位置順次補正部と、
前記センサ位置順次補正部により１つの前記センサユニットの位置補正が行われるたびに、直前の位置補正で補正対象だった前記センサユニットである直前補正対象センサの前記補正前位置から前記最終位置への補正量だけ、位置補正が完了していない前記センサユニットである未補正センサ群を平行移動させる未補正センサ群移動部と、
を備える請求項１の内視鏡形状検出装置。
【請求項４】
前記センサ位置順次補正部は、
位置補正の対象である補正対象センサを前記補正前位置から１回以上移動させ、１回の移動により前記補正対象センサを移動前位置から移動後位置まで移動させるセンサ移動部と、
前記センサ移動部により前記補正対象センサの移動が１回行われるたびに、前記補正対象センサに対して前記グローバル座標系の前記原点に近い側に隣設される前記センサユニットでるある近位側隣設センサの前記最終位置と前記補正対象センサの前記移動後位置との間を曲線補間し、移動後円弧を形成する移動後円弧形成部と、
を備える請求項３の内視鏡形状検出装置。
【請求項５】
前記センサ位置順次補正部は、
前記移動後円弧形成部により形成される前記移動後円弧の弧長と前記センサ間寸法との差の絶対値が所定の閾値以下になるまで、前記補正対象センサの前記移動前位置から前記移動後位置への移動を繰り返し行う状態に前記センサ移動部を制御する移動制御部と、
前記センサ移動部による最終回の移動での前記移動後位置を、前記補正対象センサの前記最終位置として決定する最終位置決定部と、
を備える請求項４の内視鏡形状検出装置。
【請求項６】
前記センサ位置順次補正部は、
前記近位側隣設センサの前記最終位置と前記補正対象センサの前記移動前位置との間を曲線補間して形成される移動前円弧の弧長と前記センサ間寸法との差の絶対値に対して、前記移動後円弧の弧長と前記センサ間寸法との差の絶対値が小さくなる前記補正対象センサの移動を所定の回数だけ繰り返す状態に、前記センサ移動部を制御する移動制御部と、
前記センサ移動部による最終回の移動での前記移動後位置を、前記補正対象センサの前記最終位置として決定する最終位置決定部と、
を備える請求項４の内視鏡形状検出装置。
【請求項７】
前記最終曲線形状検出部は、
それぞれの前記センサユニットの前記最終位置に基づいて、それぞれの前記最終円弧のパラメータを算出するパラメータ算出部と、
前記パラメータ算出部により算出された前記パラメータに基づいて、クォータニオン及び媒介変数を用いた補間関数により、前記媒介変数の変化に対して等角速度でそれぞれの前記センサユニットの前記最終位置の間を曲線補間し、前記最終円弧を形成する最終円弧形成部と、
を備える請求項１の内視鏡形状検出装置。
【請求項８】
内視鏡の挿入部に、長手方向に互いに所定のセンサ間寸法だけ離れて配置される複数のセンサユニットが計測を行うことと、
前記センサユニットでの計測データに基づいて、それぞれの前記センサユニットの姿勢を姿勢検出部が検出することと、
前記姿勢検出部が検出したそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間を寸法が前記センサ間寸法に等しい直線状の仮リンクと仮定して、それぞれの前記センサユニットの仮位置をセンサ仮位置検出部が検出することと、
仮曲線形状検出部が、前記センサ仮位置検出部が検出したそれぞれの前記センサユニットの前記仮位置に基づいて、それぞれの前記センサユニットの前記仮位置の間を仮円弧で曲線補間を行い、前記挿入部の仮曲線形状を検出することと、
前記仮曲線形状検出部が検出した前記仮曲線形状のそれぞれの前記仮円弧の弧長と前記センサ間寸法との差の絶対値に基づいて、それぞれの前記センサユニットの位置を前記仮位置から最終位置までセンサ位置修正部が修正することと、
最終曲線形状検出部が、それぞれの前記センサユニットの前記最終位置の間を最終円弧で曲線補間し、前記挿入部の最終曲線形状を検出することと、
を具備する内視鏡の挿入部の形状検出方法。
【請求項９】
前記仮曲線形状検出部が前記仮曲線形状を検出することは、
それぞれの前記センサユニットの前記仮位置での単位接ベクトルを前記仮曲線形状検出部の単位接ベクトル算出部が算出することと、
前記単位接ベクトル算出部が算出したそれぞれの前記単位接ベクトルに基づいて、それぞれの前記センサユニットの前記仮位置の間での前記単位接ベクトルの変化率を前記仮曲線形状検出部の変化率算出部が算出することと、
前記単位接ベクトル算出部が算出したそれぞれの前記単位接ベクトル、及び、前記変化率算出部が算出したそれぞれの前記変化率に基づいて、それぞれの前記センサユニットの前記仮位置の間の前記仮円弧を前記仮曲線形状検出部の仮円弧形成部が形成することと、
を備える請求項８の内視鏡の挿入部の形状検出方法。
【請求項１０】
前記センサ位置修正部がそれぞれの前記センサユニットの位置を前記仮位置から前記最終位置まで修正することは、
最も基端側又は最も先端側の前記センサユニットの中心を原点とするグローバル座標系において、前記センサ位置修正部のセンサ位置順次補正部が、前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に位置補正を行い、それぞれの前記センサユニットを位置補正前の補正前位置から前記最終位置へ位置補正することと、
前記センサ位置順次補正部が１つの前記センサユニットの位置補正を行うたびに、直前の位置補正で補正対象だった前記センサユニットである直前補正対象センサの前記補正前位置から前記最終位置への補正量だけ、位置補正が完了していない前記センサユニットである未補正センサ群を前記センサ位置修正部の未補正センサ群移動部が平行移動させることと、
を備える請求項８の内視鏡の挿入部の形状検出方法。
【請求項１１】
前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に、それぞれの前記センサユニットの位置補正を前記センサ位置順次補正部が行うことは、
前記センサ位置順次補正部のセンサ移動部が、位置補正の対象である補正対象センサを前記補正前位置から１回以上移動させ、１回の移動により前記補正対象センサを移動前位置から移動後位置まで移動させることと、
前記センサ位置順次補正部の移動後円弧形成部が、前記センサ移動部が前記補正対象センサの移動を１回行うたびに、前記補正対象センサに対して前記グローバル座標系の前記原点に近い側に隣設される前記センサユニットでるある近位側隣設センサの前記最終位置と前記補正対象センサの前記移動後位置との間を曲線補間し、移動後円弧を形成することと、
を備える請求項１０の内視鏡の挿入部の形状検出方法。
【請求項１２】
前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に、それぞれの前記センサユニットの位置補正を前記センサ位置順次補正部が行うことは、
前記移動後円弧形成部が形成する前記移動後円弧の弧長と前記センサ間寸法との差の絶対値が所定の閾値以下になるまで、前記センサ移動部が前記補正対象センサの前記移動前位置から前記移動後位置への移動を繰り返し行う状態に、前記センサ位置順次補正部の移動制御部が制御することと、
前記センサ移動部による最終回の移動での前記移動後位置を、前記補正対象センサの前記最終位置として前記センサ位置順次補正部の最終位置決定部が決定することと、
を備える請求項１１の内視鏡の挿入部の形状検出方法。
【請求項１３】
前記グローバル座標系の前記原点に近い側の前記センサユニットから順次に、それぞれの前記センサユニットの位置補正を前記センサ位置順次補正部が行うことは、
前記近位側隣設センサの前記最終位置と前記補正対象センサの前記移動前位置との間を曲線補間して形成される移動前円弧の弧長と前記センサ間寸法との差の絶対値に対して、前記移動後円弧の弧長と前記センサ間寸法との差の絶対値が小さくなる前記補正対象センサの移動を前記センサ移動部が所定の回数だけ繰り返す状態に、前記センサ位置順次補正部の移動制御部が制御することと、
前記センサ移動部による最終回の移動での前記移動後位置を、前記補正対象センサの前記最終位置として前記センサ位置順次補正部の最終位置決定部が決定することと、
を備える請求項１１の内視鏡の挿入部の形状検出方法。
【請求項１４】
前記最終曲線形状検出部が前記挿入部の最終曲線形状を検出することは、
それぞれの前記センサユニットの前記最終位置に基づいて、それぞれの前記最終円弧のパラメータを前記最終曲線形状検出部のパラメータ算出部が算出することと、
前記最終曲線形状算出部の最終円弧形成部が、前記パラメータ算出部が算出した前記パラメータに基づいて、クォータニオン及び媒介変数を用いた補間関数により、前記媒介変数の変化に対して等角速度でそれぞれの前記センサユニットの前記最終位置の間を曲線補間し、前記最終円弧を形成することと、
を備える請求項８の内視鏡の挿入部の形状検出方法。
【手続補正２】
【補正対象書類名】明細書
【補正対象項目名】００１０
【補正方法】変更
【補正の内容】
【００１０】
また、本発明の別のある態様では、内視鏡の挿入部に、長手方向に互いに所定のセンサ間寸法だけ離れて配置される複数のセンサユニットが計測を行うことと、前記センサユニットでの計測データに基づいて、それぞれの前記センサユニットの姿勢を姿勢検出部が検出することと、前記姿勢検出部が検出したそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間を寸法が前記センサ間寸法に等しい直線状の仮リンクと仮定して、それぞれの前記センサユニットの仮位置をセンサ仮位置検出部が検出することと、仮曲線形状検出部が、前記センサ仮位置検出部が検出したそれぞれの前記センサユニットの前記仮位置に基づいて、それぞれの前記センサユニットの前記仮位置の間を仮円弧で曲線補間を行い、前記挿入部の仮曲線形状を検出することと、前記仮曲線形状検出部が検出した前記仮曲線形状のそれぞれの前記仮円弧の弧長と前記センサ間寸法との差の絶対値に基づいて、それぞれの前記センサユニットの位置を前記仮位置から最終位置までセンサ位置修正部が修正することと、最終曲線形状検出部が、それぞれの前記センサユニットの前記最終位置の間を最終円弧で曲線補間し、前記挿入部の最終曲線形状を検出することと、を備える内視鏡の挿入部の形状検出方法を提供する。
【国際調査報告】
2012026184.tif 000067
2012026184.tif 000068
2012026184.tif 000069
2012026184.tif 000070
2012026184.tif 000071