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