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