"長手方向に互いに所定のセンサ間寸法lだけ離れて複数のセンサユニットSiが配置される挿入部11を備える内視鏡10と、前記センサユニットSiでの計測データに基づいて、それぞれの前記センサユニットSiの姿勢を検出する姿勢検出部30と、前記姿勢検出部30で検出されたそれぞれの前記センサユニットSiの姿勢に基づいて、それぞれの前記センサユニットSiの間の形状を寸法が前記センサ間寸法lに等しい直線状のリンクTjと仮定して、前記内視鏡10の前記挿入部11の検出線形形状71を検出する線形形状検出部40と、パーティクルフィルタを用いてそれぞれの前記センサユニットSiの少なくとも位置を補正し、前記線形形状検出部40により検出された前記検出線形形状71を修正した修正形状75,175を検出する形状修正部50,150と、を具備する内視鏡形状検出装置1。前記形状修正部50,150は、最も基端側又は最も先端側の前記センサユニットS1,SNの中心を原点とするグローバル座標系Cにおいて、それぞれの前記リンクTjごとに前記グローバル座標系Cの前記原点から近い側の前記リンクTjから順に形状修正を行い、前記修正形状75,175のそれぞれの前記センサユニットSiの間の形状である修正センサ間要素T ́j,Ljを形成する形状修正順次実施部51,151と、すべての前記リンクTjについて前記形状修正を行う状態に前記形状修正順次実施部51,151を制御する形状修正制御部52,152と、を備える請求項1の内視鏡形状検出装置1。前記形状修正部50,150は、前記形状修正順次実施部51,151により1つの前記リンクTjの前記形状修正が行われるたびに、前記形状修正が行われていない前記リンクTjである未修正リンクから構成される修正未完了部79,179を前記形状修正により形成される前記修正センサ間要素から構成される修正完了部77,177と境界が連続する状態に平行移動し、前記修正未完了部79,179の位置を補正する未修正リンク位置補正部53,153を備える請求項2の内視鏡形状検出装置1。前記形状修正順次実施部51,151は、前記形状修正順次実施部51,151により前記形状修正を行う対象の前記リンクTjである修正対象リンクTkにおいて、前記グローバル座標系Cの前記原点から遠い側の前記センサユニットSiである状態補正対象センサSkの少なくとも位置の推定を、前記パーティクルフィルタを用いて、前記形状修正順次実施部51,151により前記修正対象リンクTkの形状修正が行われる前の前記状態補正対象センサSkの初期状態又は前回の推定の推定結果に基づいて行うセンサ状態推定部61,161と、前記状態補正対象センサの前記推定を所定の回数だけ行う状態に前記センサ状態推定部61,161を制御する状態推定制御部62,162と、を備える請求項2の内視鏡形状検出装置1。前記形状修正順次実施部51,151は、前記センサ状態推定部61,161での推定結果に基づいて、前記状態補正対象センサSkの少なくとも位置を補正するセンサ状態補正部55,155と、前記センサ状態補正部55,155により補正された前記状態補正対象センサSkの位置に基づいて前記修正センサ間要素T ́j,Ljを形成する修正センサ間要素形成部57,157と、を備える請求項4の内視鏡形状検出装置1。前記センサ状態推定部61,161は、前記状態補正対象センサSkの前記初期状態又は前回の前記推定の推定結果に基づいて、所定の数Aのパーティクル72,172を撒くパーティクル撒布部63,163と、前記内視鏡10の前記挿入部11の状態及びそれぞれの前記センサユニットSiでの計測データから移動モデルを検出し、前記移動モデルに基づいて、撒かれたそれぞれの前記パーティクル72,172を移動前パーティクル位置から移動後パーティクル位置へ移動させるパーティクル移動部65,165と、前記状態補正対象センサSkがそれぞれの前記パーティクル72,172の前記移動後パーティクル位置に位置する尤もらしさを示す尤度を算出し、前記尤度からそれぞれの前記パーティクル72,172の重みを算出する重み算出部67,167と、前記重み算出部67,167で算出されたそれぞれの前記パーティクル72,172の前記重みに基づいて、前記状態補正対象センサSkの少なくとも位置を観測するセンサ状態観測部69,169と、を備える請求項4の内視鏡形状検出装置1。前記パーティクル移動部165は、前記内視鏡10の前記挿入部11の曲がり状態から前記移動モデルを検出し、前記移動モデルに基づいて、前記修正対象リンクTkの前記グローバル座標系Cの前記原点から近い側の前記センサユニットSk-1とそれぞれの前記パーティクル172との間の曲線補間を行い、それぞれの前記パーティクル172を移動させる曲線補間部165である請求項6の内視鏡形状検出装置1。前記パーティクル移動部65は、前記内視鏡10の前記挿入部11が平行移動している動状態で検出されたそれぞれの前記センサユニットSiの前記グローバル座標系Cでの加速度ベクトル、速度ベクトル、変位から前記移動モデルを検出し、前記移動モデルに基づいてそれぞれの前記パーティクル72を移動する動状態移動モデル実施部65である請求項6の内視鏡形状検出装置1。内視鏡10の挿入部11に、長手方向に互いに所定のセンサ間寸法lだけ離れて配置される複数のセンサユニットSiで計測を行うことと、前記センサユニットSiでの計測データに基づいて、それぞれの前記センサユニットSiの姿勢を検出することと、検出されたそれぞれの前記センサユニットSiの姿勢に基づいて、それぞれの前記センサユニットSiの間の形状を寸法が前記センサ間寸法lに等しい直線状のリンクTjと仮定して、前記内視鏡10の前記挿入部11の検出線形形状71を検出することと、パーティクルフィルタを用いてそれぞれの前記センサユニットSiの少なくとも位置を補正し、検出された前記検出線形形状71を修正した修正形状75,175を検出することと、を具備する内視鏡10の挿入部11の形状検出方法。それぞれの前記センサユニットSiの少なくとも位置を補正し、前記修正形状75,175を検出することは、最も基端側又は最も先端側の前記センサユニットS1,SNの中心を原点とするグローバル座標系Cにおいて、それぞれの前記リンクTjごとに前記グローバル座標系Cの前記原点から近い側の前記リンクTjから順に形状修正を行い、前記修正形状75,175のそれぞれの前記センサユニットSiの間の形状である修正センサ間要素T ́j,Ljを形成することを、すべての前記リンクTjについて行うことを備える請求項9の内視鏡10の挿入部11の形状検出方法。それぞれの前記センサユニットSiの少なくとも位置を補正し、前記修正形状75,175を検出することは、1つの前記リンクTjの前記形状修正が行われるたびに、前記形状修正が行われていない前記リンクTjである未修正リンクから構成される修正未完了部79,179を前記形状修正により形成される前記修正センサ間要素T ́j,Ljから構成される修正完了部77,177と境界が連続する状態に平行移動し、前記修正未完了部79,179の位置を補正することを備える請求項10の内視鏡10の挿入部11の形状検出方法。前記修正センサ間要素T ́j,Ljを形成することは、前記形状修正を行う対象の前記リンクTjである修正対象リンクTkにおいて、前記グローバル座標系Cの前記原点から遠い側の前記センサユニットSiである状態補正対象センサSkの少なくとも位置の推定を、前記パーティクルフィルタを用いて、前記修正対象リンクの前記形状修正が行われる前の前記状態補正対象センサSkの初期状態又は前回の推定の推定結果に基づいて行うことを所定の回数だけ行うことを備える請求項10の内視鏡10の挿入部11の形状検出方法。前記修正センサ間要素T ́j,Ljを形成することは、前記状態補正対象センサSkの推定結果に基づいて、前記状態補正対象センサSkの少なくとも位置を補正することと、補正された前記状態補正対象センサSkの位置に基づいて前記修正センサ間要素T ́j,Ljを形成することと、を備える請求項12の内視鏡10の挿入部11の形状検出方法。前記状態補正対象センサSkの少なくとも位置の推定を行うことは、前記状態補正対象センサSkの前記初期状態又は前回の前記推定の推定結果に基づいて、所定の数Aのパーティクル72,172を撒くことと、前記内視鏡10の前記挿入部11の状態及びそれぞれの前記センサユニットSiでの計測データから移動モデルを検出し、前記移動モデルに基づいて、撒かれたそれぞれの前記パーティクル72,172を移動前パーティクル位置から移動後パーティクル位置へ移動させることと、前記状態補正対象センサSkがそれぞれの前記パーティクル72,172の前記移動後パーティクル位置に位置する尤もらしさを示す尤度を算出し、前記尤度からそれぞれの前記パーティクル72,172の重みを算出することと、算出されたそれぞれの前記パーティクル72,172の前記重みに基づいて、前記状態補正対象センサSkの少なくとも位置を観測することと、を備える請求項12の内視鏡10の挿入部11の形状検出方法。それぞれの前記パーティクル172を移動させることは、前記内視鏡10の前記挿入部11の曲がり状態から前記移動モデルを検出し、前記移動モデルに基づいて、前記修正対象リンクTkの前記グローバル座標系Cの前記原点から近い側の前記センサユニットSk-1とそれぞれの前記パーティクル172との間の曲線補間を行い、それぞれの前記パーティクル172を移動させることである請求項14の内視鏡10の挿入部11の形状検出方法。それぞれの前記パーティクル72を移動させることは、前記内視鏡10の前記挿入部11が平行移動している動状態で検出されたそれぞれの前記センサユニットSiの前記グローバル座標系Cでの加速度ベクトル、速度ベクトル、変位から前記移動モデルを検出し、前記移動モデルに基づいてそれぞれの前記パーティクル72を移動することである請求項14の内視鏡10の挿入部11の形状検出方法。上記特許文献1の内視鏡形状検出装置では、体腔内のソースコイルの位置を体外のセンスコイルで検出するため、検出装置が大型化するとともに、装置の構成が複雑化する。上記特許文献2では、体腔内のセンサのみを用いて湾曲部の湾曲角度、湾曲方向が検出される。しかし、実際の内視鏡の挿入部は可撓性を有し、体腔内に挿入された際の挿入部の形状は、曲線状である。すなわち、この内視鏡装置では、湾曲部の湾曲角度、湾曲方向が検出されるのみで、体腔内への挿入時の挿入部の形状までは高い精度で検出されない。上記特許文献3では、複数のジャイロを用いて内視鏡の挿入部の形状が検出される。しかし、ジャイロは挿入部が移動する際に発生する慣性力又はコリオリ力に基づいて挿入部の所定の部位での姿勢を検出する。このため、挿入部が移動していない静状態では、形状を検出できず、低速度で挿入部が移動する際の検出精度は低下してしまう。本発明は上記課題に着目してなされたものであり、その目的とするところは、装置の構成を大型化、複雑化することなく、高い精度で挿入部の形状を検出可能な内視鏡形状検出装置及び内視鏡の挿入部の形状検出方法を提供することにある。上記目的を達成するため、本発明のある態様では、長手方向に互いに所定のセンサ間寸法だけ離れて複数のセンサユニットが配置される挿入部を備える内視鏡と、前記センサユニットでの計測データに基づいて、それぞれの前記センサユニットの姿勢を検出する姿勢検出部と、前記姿勢検出部で検出されたそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間の形状を寸法が前記センサ間寸法に等しい直線状のリンクと仮定して、前記内視鏡の前記挿入部の検出線形形状を検出する線形形状検出部と、パーティクルフィルタを用いてそれぞれの前記センサユニットの少なくとも位置を補正し、前記線形形状検出部により検出された前記検出線形形状を修正した修正形状を検出する形状修正部と、を備える内視鏡装置を提供する。また、本発明の別のある態様では、内視鏡の挿入部に、長手方向に互いに所定のセンサ間寸法だけ離れて配置される複数のセンサユニットで計測を行うことと、前記センサユニットでの計測データに基づいて、それぞれの前記センサユニットの姿勢を検出することと、検出されたそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間の形状を寸法が前記センサ間寸法に等しい直線状のリンクと仮定して、前記内視鏡の前記挿入部の検出線形形状を検出することと、パーティクルフィルタを用いてそれぞれの前記センサユニットの少なくとも位置を補正し、前記線形形状検出部により検出された前記検出線形形状を修正した修正形状を検出することと、を備える内視鏡の挿入部の形状検出方法を提供する。本発明によれば、装置の構成を大型化、複雑化することなく、高い精度で挿入部の形状を検出可能な内視鏡形状検出装置及び内視鏡の挿入部の形状検出方法を提供することができる。第1の実施形態本発明の第1の実施形態について図1乃至図15を参照して説明する。図1は、本実施形態の内視鏡形状検出装置1を示す図である。図1に示すように、内視鏡形状検出装置1の内視鏡10は、体腔内に挿入される挿入部11と、挿入部11の基端側に設けられる操作部12とを備える。挿入部11は、最先端に設けられる先端硬性部14と、先端硬性部14の基端側に設けられる湾曲部16と、湾曲部16の基端側に設けられる細長い可撓管部18とを備える。先端硬性部14の内部には、被写体の撮像を行うCCD等の撮像素子20が設けられている。撮像素子20には、撮像用信号線21の一端が接続されている。撮像用信号線21は、挿入部11の内部を通って操作部12から内視鏡10の外部に延出され、他端が画像処理ユニットであるビデオプロセッサ3に接続されている。また、挿入部11の内部には、先端硬性部14の照明窓図示しないに被写体を照射する照明光を導光するライトガイド23が、長手方向に延設されている。ライトガイド23は、操作部12から内視鏡10の外部に延出され、光源ユニット4に接続されている。また、挿入部11の湾曲部16の先端部には、湾曲操作伝達部材である4本の湾曲操作ワイヤ図示しないの一端が接続されている。湾曲操作ワイヤは、可撓管部18の内部を通って、操作部12に設けられる湾曲操作部である湾曲操作ノブ図示しないに他端が接続されている。湾曲操作ノブでの操作により、湾曲操作ワイヤが長手方向に移動する。湾曲操作ワイヤの移動により、湾曲部16が内視鏡10の上下方向及び左右方向に湾曲動作を行う。挿入部2には、複数本実施形態ではN1個のセンサユニットS0~SNが設けられている。それぞれのセンサユニットSii=0,1,2,...,Nは、互いに長手方向に一定の間隔l=50mmだけ離れて配置されている。すなわち、それぞれのセンサユニットSiは、互いに長手方向に所定のセンサ間寸法lだけ離れて配置されている。ここで、例えば最も基端側のセンサユニットS0が可撓管部18の基端部に配置され、最も先端側のセンサユニットSNが湾曲部16の先端部に配置されている。それぞれのセンサユニットSiは、加速度を計測する加速度センサAiと、地磁気を計測する地磁気センサBiとを備える。図2は、内視鏡10の挿入部11を示す図である。図2に示すように、それぞれのセンサユニットSiは、センサユニットSiの中心を原点とし、Xi軸、Yi軸、Zi軸を有するローカル座標系Ci図2で点線で示すを有する。ここで、Xi軸方向は、センサユニットSiの中心での内視鏡10の左右方向と一致し、基端側から視た際の内視鏡10の右方向を正とする。Yi軸方向は、センサユニットSiの中心での長手方向と一致し、先端側方向を正とする。Zi軸方向は、センサユニットSiの中心での内視鏡10の上下方向と一致し、内視鏡10の上方向を正とする。加速度センサAiは、ローカル座標系Ciの原点での加速度のXi軸方向成分、Yi軸方向成分、Zi軸方向成分を計測する。地磁気センサBiは、ローカル座標系Ciの原点での地磁気のXi軸方向成分、Yi軸方向成分、Zi軸方向成分を計測する。また、内視鏡形状検出装置1では、最も基端側のセンサユニットS0の中心を原点とし、X軸、Y軸、Z軸を有するグローバル座標系C図2で実線で示すが定義されている。ここで、グローバル座標系Cは、最も基端側のセンサユニットS0の中心を原点とする右手系の直交デカルト座標系である。X軸方向は、重力が作用する鉛直方向に垂直な所定の方向本実施形態では、図2の矢印D1,D2に平行な方向と一致し、図2の矢印D1の方向を正とする。Y軸方向は、鉛直方向に垂直で、かつ、X軸方向に垂直な方向本実施形態では、図2の矢印E1,E2に平行な方向と一致し、図2の矢印E1の方向を正とする。Z軸方向は、鉛直方向と一致し、鉛直方向の上方向紙面の裏から表への方向を正とする。なお、ここでは説明の都合上、グローバル座標系のX軸方向を磁北方向とする。それぞれのローカル座標系Ciは、グローバル座標系CをX軸回りにαi、Y軸回りにβi、Z軸回りにγiそれぞれ回転し、最も基端側のセンサユニットS0の中心からセンサユニットSiの中心まで原点を平行移動した座標系である。ここで、αiをピッチ角、βiをロール角、γiをヨー角と称し、ピッチ角αi、ロール角βi、ヨー角γiの3つをまとめて姿勢角と称する。姿勢角αi,βi,γiは、それぞれX軸、Y軸、Z軸の負の方向から見て時計回りを正とする。姿勢角αi,βi,γiの値を算出することにより、センサユニットSiの姿勢が検出される。図1に示すように、ぞれぞれのセンサユニットSiの加速度センサAi及び地磁気センサBiには、I2C等のシリアルバス5が接続されている。シリアルバス5は、挿入部11の内部を通って操作部12から内視鏡10の外部に延出され、基端がシリアルコンバータ6に接続されている。シリアルコンバータ6は、それぞれのセンサユニットSiからシリアルバス5を介して入力される計測データのシリアル信号を、USB信号に変換する。シリアルコンバータ6には、USBケーブル7の一端が接続されている。USBケーブル7の他端は、パソコン8に接続されている。パソコン8には、それぞれのセンサユニットSiでの計測データのUSB信号がシリアルコンバータ6から入力される。図3は、パソコン8の構成を示す図である。図3に示すように、パソコン8は、USBケーブル7を介してシリアルコンバータ6に接続される通信部26を備える。通信部26は、それぞれのセンサユニットSiでの計測データを受信する。通信部26には、物理量換算部28が接続されている。物理量換算部28は、通信部26で受信したそれぞれのセンサユニットSiでの計測データを、オフセット、ゲイン等を用いて物理量に換算する。物理量換算部28には、姿勢検出部30が接続されている。姿勢検出部30は、センサユニットSiでの計測データに基づいて、それぞれのセンサユニットSiの姿勢を検出する。姿勢検出部30は、それぞれのセンサユニットSiの加速度センサAi及び地磁気センサBiでの計測データに基づいて、それぞれのセンサユニットSiのローカル座標系Ciのグローバル座標系CからのX軸回り、Y軸回り、Z軸回りの回転角である3つの姿勢角αi,βi,γiを算出する姿勢角算出部32を備える。姿勢角算出部32は、それぞれのセンサユニットSiの加速度センサAiでの加速度データに基づいて、それぞれのセンサユニットSiのローカル座標系Ciのグローバル座標系CからのX軸回りの回転角であるピッチ角αi、及び、それぞれのセンサユニットSiのローカル座標系Ciのグローバル座標系CからのY軸回りの回転角であるロール角βiを算出する第1の角度算出部34を備える。また、姿勢角算出部32は、それぞれのセンサユニットSiの地磁気センサBiでの地磁気データに基づいて、それぞれのセンサユニットSiのローカル座標系Ciのグローバル座標系CからのZ軸回りの回転角であるヨー角γiを算出する第2の角度算出部36を備える。ここで、姿勢検出部30でそれぞれのセンサユニットSiの姿勢を検出する方法について説明する。図4は、内視鏡10の挿入部11が停止している静状態での挿入部11の形状検出方法を示すフローチャートである。図4に示すように、挿入部11の形状検出の際には、まず、それぞれのセンサユニットSiでの計測を行いステップS101、姿勢検出部30がそれぞれのセンサユニットSiでの計測データを取得する。そして、姿勢角算出部32が、それぞれのセンサユニットSiのローカル座標系Ciの3つの姿勢角αi,βi,γiを算出する。姿勢角αi,βi,γiを算出する際には、まず、第1の角度算出部34が、それぞれのセンサユニットSiの加速度センサAiでの計測データに基づいて、それぞれのセンサユニットSiのローカル座標系Ciのピッチ角αi及びロール角βiを算出するステップS102。ここで、姿勢角αi,βi,γiはヨー角γi、ピッチ角αi、ロール角βiの順で回転するZ,X,Y型とする。したがって、グローバル座標系Cからローカル座標系Ciへの回転行列は、となる。挿入部11が停止している静状態では、重力加速度のみが鉛直方向の下方向に作用している。すなわち、グローバル座標系Cにおいても、ローカル座標系Ciにおいても、鉛直方向の下方向に重力加速度のみが作用している。したがって、この際、加速度ベクトルのグローバル座標系CのX軸方向成分、Y軸方向成分、Z軸方向成分は、となる。また、加速度センサAiで計測される加速度ベクトルのローカル座標系CiのXi軸方向成分、Yi軸方向成分、Zi軸方向成分を、とする。ここで、ローカル座標系Ciはグローバル座標系Cをヨー角γi、ピッチ角αi、ロール角βiの順で回転した座標系である。したがって、式1~式3より、ローカル座標系Ciで観測される加速度成分は、となる。ここで、式4.1の2乗と式4.3の2乗を加算すると、となり、となる。そして、式4.2を式6で割ることにより、となり、ローカル座標系Ciのピッチ角αiが求まる。また、式4.1を式4.3で割ることにより、となり、ローカル座標系Ciのロール角βiが求まる。以上のようにして、それぞれの加速度センサAiでの計測データに基づいて、それぞれのローカル座標系Ciのピッチ角αi及びロール角βiが算出される。そして、第2の角度算出部36が、それぞれのセンサユニットSiの地磁気センサBiでの計測データに基づいて、それぞれのセンサユニットSiのローカル座標系Ciのヨー角γiを算出するステップS103。ここで、ステップS102で算出したピッチ角αi及びロール角βiを用いて、それぞれのローカル座標系Ciのグローバル座標系CからのX軸回りの回転及びY軸回りの回転を補正した補正座標系C ́iを定義する。図5は、グローバル座標系C図5において実線で示す及び補正座標系C ́i図5で点線で示すを示す図である。なお、実際はグローバル座標系Cと補正座標系C ́iとでは原点の位置が異なるが、図5では両者の比較のため原点が同一の位置にある状態で示す。図5に示すように、X軸回りの回転及びY軸回りの回転を補正した補正座標系C ́iはグローバル座標系CをZ軸回りにヨー角γiだけ回転した座標系であり、X ́i軸、Y ́i軸、Z ́i軸を有する。X ́i軸方向、Y ́i軸方向は、それぞれグローバル座標系CのX軸方向、Y軸方向からZ軸回りにヨー角γiだけ回転した方向と一致する。Z ́i軸方向は、鉛直方向、すなわちグローバル座標系CのZ軸方向と一致する。ここでは、グローバル座標系CのX軸方向が磁北方向と一致するため、X ́i軸方向が、磁北方向からZ軸回りにヨー角γiだけ回転した方向となる。地磁気センサBiで計測される地磁気ベクトルのローカル座標系CiのXi軸方向成分、Yi軸方向成分、Zi軸方向成分を、とする。補正座標系C ́iは、ローカル座標系Ciのグローバル座標系CからのX軸回りの回転及びY軸回りの回転を補正した座標系である。したがって、式9及び式1のRxi,Ryiを用いて、地磁気センサBiで計測される地磁気ベクトルの補正座標系C ́iのX ́i軸方向成分、Y ́i軸方向成分、Z ́i軸方向成分は、となる。式10.1,式10.2から、となる。鉛直方向に垂直な水平面補正座標系C ́iのX ́i-Y ́i平面での地磁気成分は、磁北方向を向いている。したがって、式11.1,式11.2より、X ́i軸から磁北方向までの角度θiは、地磁気ベクトルの補正座標系C ́iのX ́i軸成分、Y ́i軸成分を用いて求めることができる。すなわち、となる。角度θiはZ ́i軸Z軸を負の方向から見て時計回りを正とする。ここで、補正座標系C ́iはグローバル座標系CをZ軸回りにヨー角γiだけ回転した座標系である。したがって、式12で求まる角度θiが、グローバル座標系Cを基準としたローカル座標系Ciのヨー角γiとなる。なお、グローバル座標系CのX軸方向が磁北方向と一致しない場合も、磁北を基準としてヨー角γiを求めることもできる。地磁気ベクトルのグローバル座標系CのX軸方向成分、Y軸方向成分、Z軸方向成分を、とする。地磁気ベクトルのグローバル座標系CのX軸方向成分、Y軸方向成分、Z軸方向成分は、地磁気センサBiと同一のタイプの地磁気センサを用いて、グローバル座標系CのX軸方向、Y軸方向、Z軸方向と軸の方向が一致する状態で計測を行うことにより、求められる。そして、式13より、X軸から磁北方向までの角度θを、地磁気ベクトルのグローバル座標系CのX軸成分、Y軸成分を用いて求める。すなわち、となる。ここで、角度θはZ軸を負の方向から見て時計回りを正とする。補正座標系C ́iはグローバル座標系CをZ軸回りにヨー角γiだけ回転した座標系である。したがって、式12、式14より、となり、グローバル座標系Cを基準としたローカル座標系Ciのヨー角γiが求まる。以上のようにして、それぞれの地磁気センサBiでの計測データに基づいて、それぞれのローカル座標系Ciのヨー角γiが算出される。算出された姿勢角αi,βi,γiの値に基づいて、姿勢検出部30はそれぞれのセンサユニットSiの姿勢を検出する。図3に示すように、姿勢検出部30には、線形形状検出部40が接続されている。図6は、線形形状検出部40により検出される内視鏡10の挿入部11の検出線形形状71をグローバル座標CのZ軸の正の方向から見た図である。図6に示すように、線形形状検出部40は、姿勢検出部30で検出されたそれぞれのセンサユニットSiの姿勢に基づいて、それぞれのセンサユニットSiの間の形状を寸法がセンサ間寸法lに等しい直線状のリンクTjj=1,2,...Nと仮定して、挿入部11の検出線形形状71を検出する。ここで、基端側からk番目のリンクTkは、基端側からk番目のセンサユニットSk-1と基端側からk1番目のセンサユニットSkとの間のリンクである。線形形状検出部40は、それぞれのリンクTjを形成するリンク形成部41と、リンク形成部41により形成されたそれぞれのリンクTjを平行移動して、リンクTjの位置を補正するリンク位置補正部42とを備える。リンク位置補正部42によりそれぞれのリンクTjは、隣接するリンクTj-1,Tj1とのリンク境界が連続する状態に、平行移動される。線形形状検出部40には、描画部45が接続されている。描画部45には、表示部47が接続されている。線形形状検出部40により検出されたグローバル座標系Cでの挿入部11の検出線形形状71は、描画部45により描画される。術者は、描画部45により描画された検出線形形状71を表示部47で確認可能となっている。ここで、線形形状検出部40で挿入部11の検出線形形状71を検出する方法について説明する。図4に示すように、挿入部11の検出線形形状71を検出する際、まず、ステップS102,S103で算出した姿勢角αi,βi,γiの値に基づいて、リンク形成部41が、直線形状のそれぞれのリンクTjを形成するステップS104。ここでは、基端側からk番目のセンサユニットSk-1と基端側からk1番目のセンサユニットSkとの間の基端側からk番目のリンクTkの形成について説明する。式7、式8、式12又は式15に示すように、ステップS102,S103でローカル座標系Ck-1つまりリンクの姿勢角αk-1,βk-1,γk-1が算出される。この姿勢αk-1,βk-1,γk-1及びそれぞれのセンサユニットSiの間の長手方向についての間隔であるセンサ間寸法lを用いて、センサユニットSk-1がグローバル座標系Cの原点に位置する場合の、センサユニットSkの座標P ́klxk,lyk,lzkが求まる。ここで、座標P ́kは、となる。式16.1,式16.2のeyk-1は、ローカル座標系Ck-1の原点での長手方向であるYk-1軸方向の単位ベクトルである。単位ベクトルeyk-1と式1で算出される回転行列を乗算することにより、単位ベクトルeyk-1のグローバル座標系CのX軸方向成分、Y軸方向成分、Z軸方向成分がそれぞれ算出される。すなわち、lxk,lyk,lzkは、ローカル座標系Ck-1のYk-1軸方向への大きさlのベクトルを、それぞれグローバル座標系CのX軸方向、Y軸方向、Z軸方向に分解した成分である。グローバル座標系Cの原点と式16.1,式16.2で算出される座標P ́klxk,lyk,lzkとを直線状に結ぶことにより、リンクTkが形成される。なお、リンクTk以外のリンクTjについても、リンク形成部41により同様に形成される。すなわち、式16.1,式16.2を用いて、リンクTjの基端側グローバル座標系Cの原点から近い側のセンサユニットSj-1がグローバル座標系Cの原点に位置する場合の、リンクTjの先端側グローバル座標系Cの原点から遠い側のセンサユニットSjの座標P ́jlxj,lyj,lzjを求める。そして、グローバル座標系Cの原点と座標P ́jlxj,lyj,lzjとを直線状に結ぶことにより、リンクTjが形成される。すなわち、リンク形成部41は、リンクTjが基端側グローバル座標系Cの原点から近い側のセンサユニットSj-1の中心から先端側グローバル座標系Cの原点から遠い側のセンサユニットSjの中心まで基端側のセンサユニットSj-1の中心での長手方向に延設されると仮定して、リンクTjを形成する。また、センサ間寸法lは50mm程度であることが好ましい。センサ間寸法lを長くすることにより、センサユニットSiの数が減少し、コストが削減される。また、センサ間寸法lが50mm程度より小さい範囲であれば、それぞれのセンサユニットSiの間を寸法がセンサ間寸法lに等しい直線状のリンクTjと仮定した場合でも、挿入部11の形状検出の際の誤差を小さくすることが可能である。そして、リンク位置補正部42が、リンク形成部41により形成されたそれぞれのリンクTjを隣接するリンクTj-1,Tj1とのリンク境界が連続する状態に平行移動して、リンクTjの位置を補正するステップS105。図7は、リンク位置補正部42での処理を説明する図である。ここでは、基端側からk番目のセンサユニットSk-1と基端側からk1番目のセンサユニットSkとの間の基端側からk番目のリンクTkの位置補正について説明する。図7に示すように、リンク位置補正部42により、リンクTkの位置補正が行われる前の状態では、リンクTkの基端側に隣接するリンクTk-1まで位置補正が完了し、リンク位置補正完了部73が形成されている。リンクTkの位置補正を行う際は、原点からリンク位置補正完了部73の先端までの移動量だけ、リンク位置補正部42はリンクTkを平行移動する。すなわち、図7の点線で示す位置から図7の実線で示す位置にリンクTkを平行移動する。これにより、リンクTk-1とリンクTkとのリンク境界が連続する状態となり、リンクTkの位置が補正される。なお、リンクTk以外のリンクTjについても、リンク位置補正部42により同様に位置が補正される。すなわち、リンクTjの位置補正を行う際は、原点からリンク位置補正完了部73の先端グローバル座標系Cの原点から遠い側の端までの移動量だけ、リンク位置補正部42はリンクTjを平行移動する。これにより、リンクTjとリンクTjの基端側グローバル座標系Cの原点から近い側に隣接するリンクTj-1とのリンク境界が連続する状態となり、リンクTjの位置が補正される。ただし、リンクT1については、リンクT1の基端がグローバル座標系Cの原点であるため、位置の補正は行われない。図3に示すように、線形形状検出部40は、形状修正部50に接続されている。形状修正部50は、パーティクルフィルタによりそれぞれのセンサユニットSiの位置状態を補正し、線形形状検出部40により検出された検出線形形状71を修正する。図8は、形状修正部50により修正された内視鏡10の挿入部11の修正形状75をグローバル座標CのZ軸の正の方向から見た図である。図8に示すように、形状修正部50により、図8の点線で示す検出線形形状71が修正形状75に修正される。それぞれのセンサユニットSiの位置を補正することにより、それぞれのリンクTjの形状が修正され修正リンク修正センサ間要素T ́jが決定される。修正形状75は、それぞれの修正リンクT ́jから構成されている。形状修正部50は、それぞれのリンクTjごとに基端側グローバル座標系Cの原点から近い側のリンクTjから順に形状修正を行い、修正リンクT ́jを形成する形状修正順次実施部51と、すべてのリンクTjについて形状修正を行う状態に形状修正順次実施部51を制御する形状修正制御部52とを備える。また、形状修正部50は、形状修正順次実施部51により1つのリンクTjの形状修正が行われるたびに、形状修正が行われていないリンク未修正リンクTjから構成される修正未完了部79を平行移動し、位置を補正する未修正リンク位置補正部53を備える。未修正リンク位置補正部53により、修正未完了部79が、形状修正により形成される修正リンクT ́jから構成される修正完了部77と境界が連続する状態に平行移動される。図3に示すように、形状修正順次実施部51は、修正対象のリンク修正対象リンクTjの先端側グローバル座標系Cの原点から遠い側のセンサユニット状態補正対象センサSjの位置状態を補正するセンサ位置補正部センサ状態補正部55と、センサ位置補正部55により補正されたセンサユニットSjの位置に基づいて修正リンクT ́jを形成する修正リンク形成部修正センサ間要素形成部57とを備える。また、形状修正順次実施部51は、パーティクルフィルタを用いて、修正対象のリンクTjの先端側のセンサユニットSjの位置の推定を行うセンサ位置推定部センサ状態推定部61を備える。センサ位置推定部61は、リンクTjの先端に位置する初期位置初期状態に基づいて、1回目のセンサユニットSjの位置推定を行う。また、センサ位置推定部61は、1回目の推定結果に基づいて、2回目のセンサユニットSjの位置推定を行う。すなわち、センサ位置推定部61は、パーティクルフィルタを用いて、初期位置又は前回の推定の推定結果に基づいてセンサユニットSjの位置推定を行う。さらに、形状修正順次実施部51は、センサユニットSjの位置推定を所定の回数だけ行う状態にセンサ位置推定部61を制御する位置推定制御部状態推定制御部62を備える。センサ位置推定部61は、パーティクル撒布部63と、パーティクル移動部65と、重み算出部67と、センサ位置観測部センサ状態観測部69とを備える。パーティクル撒布部63、パーティクル移動部65、重み算出部67、センサ位置観測部69の詳細については、後述する。形状修正部50には、描画部45が接続されている。形状修正部50により修正されたグローバル座標系Cでの挿入部11の修正形状75は、描画部45により描画される。術者は、描画部45により描画された修正形状75を表示部47で確認可能となっている。ここで、形状修正部50で、線形形状検出部40により検出された検出線形形状71を修正する方法について説明する。図4に示すように、形状修正部50は、パーティクルフィルタによりそれぞれのセンサユニットSiの位置を補正し、線形形状検出部40により検出された検出線形形状71を修正するステップS106。前述のように、センサ間寸法lが50mm程度より小さい範囲であれば、それぞれのセンサユニットSiの間を寸法がセンサ間寸法lに等しい直線状のリンクTjと仮定した場合でも、挿入部11の形状検出の際の誤差は小さくなる。しかし実際は、それぞれのセンサユニットSiの計測データはノイズ等により誤差を有し、また、体腔内に挿入された際の内視鏡10の挿入部11は曲線形状を呈している。このため、挿入部11の形状は基端側のセンサユニットSj-1の中心から先端側のセンサユニットSjの中心まで基端側のセンサユニットSj-1の中心での長手方向に延設される直線状のリンク形状とは限らない。したがって、検出線形形状71の形状修正を行うことが重要となる。それぞれのセンサユニットSiの位置を補正し検出線形形状71を修正する際、形状修正順次実施部51は、それぞれのリンクTjごとに基端側グローバル座標系Cの原点から近い側のリンクTjから順に形状修正を行い、修正リンクT ́jを形成する。ここで、形状修正順次実施部51により、リンクTjの形状修正を行う方法について説明する。ここでは、基端側からk番目のセンサユニットSk-1と基端側からk1番目のセンサユニットSkとの間の基端側からk番目のリンクTkの形状修正について説明する。図9は、形状修正部50により検出線形形状71の形状を修正する方法を示すフローチャートである。図10乃至図15は、形状修正順次実施部51での処理を説明する図である。図10に示すように、形状修正順次実施部51により、リンクTkの形状修正が行われる前の状態では、リンクTk-1まで形状修正が完了し、修正完了部77が形成されている。修正完了部77は、修正リンクT ́1~T ́k-1から構成されている。また、リンクTk~TNから構成される修正未完了部79が形成されている。この際、修正完了部77と修正未完了部79との境界は連続し、センサユニットSkはグローバル座標系Cで点Qk_0に位置している。この状態で、形状修正順次実施部51により、修正未完了部79の中で最も基端側に位置するリンク修正対象リンクTkの形状修正が開始される。すなわち、形状修正制御部52により、形状修正順次実施部51がリンクTkの形状修正を行う状態に制御される。形状修正順次実施部51によりリンクTkの形状修正を行う際にはまず、センサ位置推定部61によりリンクTkの先端側のセンサユニット状態補正対象センサSkの位置を、パーティクルフィルタを用いて、リンクTkの先端に位置する初期位置Qk_0に基づいて推定する。図9及び図10に示すように、センサユニットSkの位置を初期位置に基づいて推定する際は、まず、パーティクル撒布部63によりリンクTkの先端側のセンサユニットSkの初期位置Qk_0の周辺に所定の数本実施形態ではA個とするのパーティクル72を撒くステップS112。ここで、センサユニットSkの初期位置すなわち、パーティクルフィルタにおける状態ベクトルをQk_0qxk_0,qyk_0,qzk_0とし、初期位置Qk_0の誤差の分散をδx,δy,δzとする。δx,δy,δzを求める際は、ノイズ等により生じるそれぞれのセンサユニットSiの計測データの誤差の分散から、式1~式15を用いてセンサユニットSk-1の姿勢角αk-1,βk-1,γk-1の誤差の分散を求める。そして、姿勢角αk-1,βk-1,γk-1の誤差の分散から式16を用いて、センサユニットSkの初期位置Qk_0の誤差の分散δx,δy,δz求める。パーティクル72は、センサユニットSkの初期位置Qk_0に誤差の分散δx,δy,δzを加えた範囲に一様に撒かれる。すなわち、それぞれのパーティクル72のグローバル座標系Cでの位置をQk_1aqxk_1a,qyk_1a,qzk_1aa=1,2,...,Aとすると、の範囲でパーティクル72が一様に撒かれる。すなわち、システムの雑音をホワイトノイズとして仮定している。なお、パーティクルの撒布範囲は経験的に決定されてもよいが、撒布範囲が大きすぎる場合は、推定精度が劣化する。それぞれのパーティクル72が最初に撒かれる際には、それぞれのパーティクル72に初期の重みwk_0aがあらかじめ与える。重みwk_0aは、例えばそれぞれのパーティクル72で一定に1/Aで与えられる。ここで、パーティクル72は、状態ベクトルの確率密度関数を状態空間のA個の離散的な仮説で表現したものであり、それぞれのパーティクル72が確率に比例する重みwk_0aを有している。すなわち、パーティクルフィルタでは、複数本実施形態ではA個のパーティクル72を用いて、状態ベクトルの確率密度関数を推定する。そして、パーティクル移動部65が挿入部11の状態移動状態、曲がり状態等及びセンサユニットSkでの計測データに基づいてそれぞれのパーティクル72の移動モデルを検出する。そして、パーティクル移動部65は、重みwk_0aに比例する確率からパーティクル72を選択し、検出された移動モデルに基づいてそれぞれのパーティクル72を移動させるステップS113。パーティクル移動部65により、それぞれのパーティクル72は移動前パーティクル位置から移動後パーティクル位置へ移動する。ただし、本実施形態では、挿入部11が移動しない静状態であり、かつ、それぞれのセンサユニットSiの間の曲線補間は行わないため、パーティクル72の移動は行われない。そして、重み算出部67によりそれぞれのパーティクル72の重みwk_1aを算出するステップS114。すなわち、それぞれのパーティクル72の重みが、wk_0aからwk_1aに更新される。重み算出部67は、まず、それぞれのパーティクル72の尤もらしさを評価する尤度vk_1aを算出する。尤度により、センサユニットSkがそれぞれのパーティクル72のパーティクル移動部65により移動された移動後パーティクル位置に位置する尤もらしさが示される。尤度は、となる。bk_1aはグローバル座標系Cにおいて、位置補正後のセンサユニットSk-1とそれぞれのパーティクル72との間の線分の寸法である。例えば図11に示すように、センサユニットSk-1とパーティクル72Aグローバル座標系Cでの位置がQk_1sのパーティクルとの間の線分の寸法は、bk_1sとなる。ここで関数hbk_1aは例えば、とする。b0はそれぞれのセンサユニットSiのセンサ間寸法lとする。式19より、位置補正後のセンサユニットSk-1とパーティクル72との間の線分の寸法と、センサ間寸法lとの差が小さいほど関数hbk_1aは大きくなり、尤度が大きくなる。逆に、位置補正後のセンサユニットSk-1とパーティクル72との間の線分の寸法と、センサ間寸法lとの差が大きいほど関数hbk_1aは小さくなり、尤度が小さくなる。φk_1aは、位置補正後のセンサユニットSk-1からそれぞれのパーティクル72までのベクトルV1と、それぞれのパーティクル72から位置補正前のセンサユニットSk1までのベクトルV2との成す角度である。例えば図11に示すように、パーティクル72Aグローバル座標系Cでの位置がQk_1sのパーティクルでは、ベクトルV1とベクトルV2との成す角度は、φk_1sとなる。ここで関数gφk_1aは例えば、とする。ここで、cは定数であり、例えば4である。式20よりベクトルV1とV2の成す角度が0度~±90度の範囲であれば、関数gφk_1aは大きくなり、尤度が大きくなる。逆に、それ以外の角度であれば、関数gφk_1aは小さくなり、尤度が小さくなる。それぞれのセンサユニットSiのセンサ間寸法lを50mmとした場合、実際に角度φk_1aは0度~略±90度の範囲を取り得る。このことは、内視鏡10の挿入部11と径が略同一の径が10mmの樹脂製チューブを曲げた状態から、容易に想到される。角度φk_1aの範囲は、製品、挿入部の位置によって異なるため、製品および/または挿入部位置に対応して調整されることが望ましい。そして、それぞれのパーティクル72の尤度から、重みwk_1aを算出する。それぞれのパーティクル72の重みは、となる。すなわち、重みはそれぞれのパーティクル72の尤度を、尤度の合計を1として規格化した値である。以上のようにして、それぞれのパーティクル72の重みが算出される。そして、センサ位置観測部69が、重み算出部67で算出されたそれぞれのパーティクル72の重みに基づいて、センサユニットSkの位置を観測するステップS115。この際、センサ位置観測部69は、図12に示すように、それぞれのパーティクル72のパーティクル移動部65により移動される前の移動前パーティクル位置の重み付き平均を算出し、算出された位置を移動前センサ位置Q ́k_1とする。そして、パーティクル移動部65がそれぞれのパーティクル72を移動させる際に用いた移動モデルにより、移動前センサ位置Q ́k_1から移動後センサ位置Qk_1への移動を行い、移動後センサ位置Qk_1をセンサユニットSkの位置として推定する。なお、本実施形態では、パーティクル移動部65によりパーティクル72の移動は行われないため、移動前センサ位置Q ́k_1と移動後センサ位置Qk_1との位置は同一である。そして、位置推定制御部62により、所定の回数本実施形態ではM回とするだけセンサユニットSkの位置推定が行われたかを判断するステップS116。現状態では、センサユニットSkの位置推定が1回しか行われていないためステップS116-No、センサ位置推定部61は位置推定制御部62により、センサユニットSkの位置推定をもう1回行う状態に制御される。この際、センサ位置推定部61は、1回目の推定結果に基づいて、センサユニットSkの位置推定を行う。同様に、例えばt回目の位置推定では、t-1回目の推定結果に基づいて、センサユニットSkの位置推定を行う。すなわち、2回目以降の位置推定では、センサ位置推定部61は、前回の位置推定での推定結果に基づいて、センサユニットSkの位置推定を行う。そして、ステップS112に戻り、パーティクル撒布部63により所定の数本実施形態ではA個とするのパーティクル72を撒くステップS112。2回目の位置推定では、1回目の位置推定の際に重み算出部67で算出されたそれぞれのパーティクル72の重みに基づいて、パーティクル72が撒かれる。同様に例えばt回目の位置推定では、t-1回目の位置推定の際に重み算出部67で算出されたそれぞれのパーティクル72の重みに基づいてパーティクル72が撒かれる。すなわち、2回目以降の位置推定では、パーティクル撒布部63は、前回の位置推定の際に重み算出部67で算出されたそれぞれのパーティクル72の重みに基づいてパーティクル72を撒く。すなわち、パーティクル撒布部63は、前回の位置推定の際のそれぞれのパーティクル72の重みに基づいてパーティクル72を撒く重み対応撒布部となっている。結果的に、図13A及び図13Bに示すように、例えば、前回の位置推定の際に算出された重みが大きいパーティクル72の移動前パーティクル位置の周辺図13A,図13B中のG1で示す範囲では、撒くパーティクル72の数を多くする。一方、前回の位置推定の際に算出された重みが小さいパーティクル72の移動前パーティクル位置の周辺図13A,図13B中のG2で示す範囲では、撒くパーティクル72の数を少なくする。なお、図13Aは前回の位置推定の際に撒かれたパーティクル72のパーティクル移動部65により移動される前の移動前パーティクル位置を示し、図13Bは現在のパーティクルの移動前パーティクル位置を示している。そして、2回目以降の位置推定でも同様に、パーティクル移動部65が、1回目の推定で用いた移動モデルに基づいて、それぞれのパーティクル72を移動させるステップS113。これにより、それぞれのパーティクル72は、移動前パーティクル位置から移動後パーティクル位置へ移動する。そして、重み算出部67により、式18~式20と同様の式を用いて、それぞれのパーティクル72の尤もらしさを評価する尤度を算出する。尤度により、センサユニットSkがそれぞれのパーティクル72のパーティクル移動部65により移動された移動後パーティクル位置に位置する尤もらしさが示される。そして、式21と同様の式を用いて、尤度からそれぞれのパーティクル72の重みが算出されるステップS114。ここで、例えばt回目の位置推定では、式18~式21の尤度vk_1a,重みwk_1a,bk_1a,φk_1aの代わりに、それぞれ尤度vk_ta,重みwk_ta,bk_ta,φk_taを用いて計算する。そして、センサ位置観測部69が、重み算出部67で算出されたそれぞれのパーティクル72の重みに基づいて、センサユニットSkの位置を推定するステップS115。この際、センサ位置観測部69は、それぞれのパーティクル72のパーティクル移動部65により移動される前の移動前パーティクル位置の重み付き平均を算出し、算出された位置を移動前センサ位置とする。そして、パーティクル移動部65がそれぞれのパーティクル72を移動させる際に用いた移動モデルにより、移動前センサ位置から移動後センサ位置への移動を行い、移動後センサ位置をセンサユニットSkの位置として推定する。例えば、t回目の位置推定では、それぞれのパーティクル72の重み付平均から移動前センサ位置Q ́k_tが算出され、移動モデルでの移動により移動前センサ位置Q ́k_tから移動後センサ位置Qk_tへ移動する。そして、位置推定制御部62により、所定の回数本実施形態ではM回とするだけセンサユニットSkの位置推定が行われたかを判断するステップS116。センサユニットSkの位置推定が所定の回数だけ行われていない場合はステップS116-No、センサ位置推定部61は位置推定制御部62により、センサユニットSkの位置推定をもう1回行う状態に制御され、ステップS112~ステップS115をもう1回行う。センサユニットSkの位置推定が所定の回数だけ行われている場合はステップS116-Yes、次のステップに進む。図14に示すように、センサユニットSkの位置推定を所定の回数だけ行った後、センサ位置補正部55は、センサ位置推定部61での推定結果に基づいて、修正対象のリンク修正対象リンクTkの先端側のセンサユニット状態補正対象センサSkの位置を補正するステップS117。センサ位置補正部55は、センサユニットSkの位置を、リンクTkの先端に位置する初期位置Qk_0からセンサ位置観測部61の最終回本実施形態ではM回目の位置推定の際に推定されたセンサユニットSkの移動後センサ位置Qk_Mへと、補正する。また、修正リンク形成部57が、センサ位置補正部55により補正されたセンサユニットSkの位置に基づいて修正リンクT ́kを形成するステップS117。ここでは、位置補正後のセンサユニットSk-1とセンサユニットSkの移動後センサ位置Qk_Mとを結ぶ線分が、修正リンクT ́kとなる。センサユニットSkの位置の移動により、形状修正順次実施部51によりリンクTkの形状修正が行われる前の状態では連続していた修正完了部77と修正未完了部79との境界が、不連続な状態となる。すなわち、修正リンクT ́kとリンクTk1が不連続な状態となる。なお、リンクTk以外のリンクTjについても、形状修正順次実施部51により同様に形状修正が行われる。すなわち、リンク修正対象リンクTjの形状修正が開始されるとステップS111、センサ位置推定部61が、パーティクルフィルタを用いて、修正対象のリンクTjの先端側グローバル座標Cの原点から遠い側のセンサユニット状態補正対象センサSjの位置の推定を行う。センサユニットSjの位置の推定を行う際は、まず、パーティクル撒布部63がセンサユニットSjの初期位置Qj_0、又は、前回の位置推定の際に重み算出部67で算出されたそれぞれのパーティクル72の重みに基づいて所定の数のパーティクル72を撒くステップS112。そして、パーティクル移動部65が、上述した移動モデルを検出し、移動モデルに基づいて、それぞれのパーティクル72を移動させるステップS113。これにより、それぞれのパーティクル72は、移動前パーティクル位置から移動後パーティクル位置へ移動する。そして、重み算出部67により、式18~式20と同様の式を用いて、それぞれのパーティクル72の尤もらしさを評価する尤度を算出する。尤度により、センサユニットSjがそれぞれのパーティクル72のパーティクル移動部65により移動された移動後パーティクル位置に位置する尤もらしさが検出される。そして、式21と同様の式を用いて、尤度からそれぞれのパーティクル72の重みが算出されるステップS114。ここで、リンクTjの形状修正が行われているので、例えばt回目の位置推定では、式18~式21の尤度vk_1a,重みwk_1a,bk_1a,φk_1aの代わりに、それぞれ尤度vj_ta,重みwj_ta,bj_ta,φj_taを用いて計算する。そして、センサ位置観測部69が、重み算出部67で算出されたそれぞれのパーティクル72の重みに基づいて、センサユニットSjの位置を観測するステップS115。例えば、t回目の位置推定では、それぞれのパーティクル72の重み付平均から移動前センサ位置Q ́j_tが算出され、移動モデルでの移動により移動前センサ位置Q ́j_tから移動後センサ位置Qj_tへの移動が行われる。そして、移動後センサ位置Qj_tが、センサユニットSjの位置として推定される。そして、位置推定制御部62により、所定の回数本実施形態ではM回とするだけセンサユニットSjの位置推定が行われたかを判断するステップS116。センサユニットSjの位置推定が所定の回数だけ行われていない場合はステップS116-No、センサ位置推定部61は位置推定制御部62により、センサユニットSjの位置推定を所定の回数だけ行う状態に制御され、ステップS112~ステップS115をもう1回行う。センサユニットSjの位置推定が所定の回数だけ行われている場合はステップS116-Yes、次のステップに進む。センサ位置推定部61によりセンサユニットSjの位置推定が所定の回数だけ行った後、センサ位置補正部55は、センサ位置推定部61の推定結果に基づいて、修正対象のリンク修正対象リンクTjの先端側グローバル座標系Cの原点から遠い側のセンサユニット状態補正対象センサSjの位置を補正するステップS117。センサ位置補正部55は、センサユニットSjの位置を、リンクTjの先端グローバル座標系の遠い側の端に位置する初期位置Qj_0からセンサ位置観測部61の最終回本実施形態ではM回目の位置推定の際に推定されたセンサユニットSjの移動後センサ位置Qj_Mへと、補正する。また、修正リンク形成部57が、センサ位置補正部55により補正されたセンサユニットSjの位置に基づいて修正リンクT ́jを形成するステップS117。ここでは、位置補正後のセンサユニットSj-1とセンサユニットSjの移動後センサ位置Qj_Mとを結ぶ線分が、修正リンクT ́jとなる。図15は、未修正リンク位置補正部53での処理を説明する図である。図15に示すように、形状修正順次実施部51により、リンクTkの形状修正が行われ、修正リンクT ́kが形成された後の状態では、形状修正が完了した修正完了部77と、形状修正が完了していない修正未完了部79とが形成されている。修正完了部77は修正リンクT ́1~T ́kから構成され、修正未完了部79はリンクTk1~TNから構成されている。この際、前述のようにリンクTkの形状修正によりセンサユニットSkの位置が点Qk_0から点Qk_Mに移動するため、修正完了部77と修正未完了部79との境界は、不連続となっている。この状態で、未修正リンク位置補正部53が、修正未完了部79を修正完了部77と境界が連続する状態に平行移動して、修正未完了部79の位置を補正するステップS118。すなわち、図15の点線で示す位置から、実線で示す位置に修正未完了部79が平行移動される。なお、リンクTk以外のリンクTjについても、同様に未修正リンク位置補正部53により修正未完了部79の位置が補正される。すなわち、形状修正順次実施部51によりリンクTjの形状修正が行われた後、修正完了部77と修正未完了部79との境界は不連続となっている。この状態で、未修正リンク位置補正部53が、修正未完了部79を修正完了部77と連続する状態に平行移動して、修正未完了部79の位置を補正するステップS118。未修正リンク位置補正部53は、形状修正順次実施部51により1つのリンクTjの形状修正が行われるたびに、修正未完了部79の位置を補正する。そして、図9に示すように、形状修正制御部52が、すべてのリンクTjについて形状修正が完了したか確認するステップS119。すべてのリンクTjについて形状修正が完了している場合は、挿入部11の修正形状75が形成され、次のステップへ進むステップS119-Yes。すべてのリンクTjについて形状修正が完了していない場合は、ステップS111に戻りステップS119-No、形状修正順次実施部51により、修正未完了部79の中で最も基端側グローバル座標系Cの原点から近い側に位置するリンク修正対象リンクTjの形状修正が行われる。すなわち、ステップS111~S118が、すべてのリンクTjについて形状修正が完了するまで繰り返し行われる。そこで、上記構成の内視鏡形状検出装置1及び内視鏡形状検出装置1を用いた内視鏡10の挿入部11の形状検出方法では、以下の効果を奏する。すなわち、内視鏡形状検出装置1では、それぞれのセンサユニットSiの計測データから姿勢検出部30がセンサユニットSiの姿勢を検出し、線形形状検出部40がそれぞれのセンサユニットSiの姿勢から内視鏡10の挿入部11の検出線形形状71を検出する。そして、形状修正部50が、パーティクルフィルタによりそれぞれのセンサユニットSiの位置を補正し、線形形状検出部40により検出された検出線形形状71を修正する。そして、挿入部11の修正形状75が形成される。以上のように、観察時に体腔内に挿入される挿入部11に配置されるセンサユニットSiの計測データから、挿入部11の修正形状75が検出されるため、体外にセンスコイル等を設ける必要がない。このため、内視鏡形状検出装置1の小型化、単純化を実現することができる。また、内視鏡形状検出装置1では、形状修正部50により、パーティクルフィルタを用いてそれぞれのセンサユニットSiの位置を補正し、検出線形形状71を修正している。形状修正部50では、形状修正順次実施部51のセンサ位置推定部61によりそれぞれのセンサユニットSiの推定を所定の回数だけ行う。センサユニットSiの推定の際、重み算出部67によりそれぞれのパーティクル72の重みを算出する。そして、センサ位置観測部69が、重み算出部67で算出されたそれぞれのパーティクル72の重みに基づいて、センサユニットSiの位置を観測する。算出されるパーティクル72の重みが大きいほど、パーティクル移動部65により移動されたそれぞれのパーティクル72の移動後パーティクル位置が、体腔内に挿入される挿入部11でのセンサユニットSiの尤もらしい位置位置する確率が高い位置となる。このため、センサ位置観測部69により、高い精度でセンサユニットSiの位置が観測される。センサ位置補正部55は、センサ位置推定部61での最終回の推定でセンサ位置観測部69により観測された位置である移動後センサ位置に、それぞれのセンサユニットSiの位置を補正する。このため、位置補正後のセンサユニットSiの位置は、体腔内に挿入される挿入部11でのセンサユニットSiの尤もらしい位置位置する確率が高い位置となる。形状修正部50は、位置補正後のセンサユニットSiの位置に基づいて、検出線形形状71の修正を行うため、実際の挿入部11の形状と誤差の少ない修正形状75を検出することができる。これにより、高い精度で挿入部11の修正形状75を検出することができる。さらに、内視鏡形状検出装置1では、挿入部11が移動していない静状態において、加速度センサAiが重力加速度を計測し、地磁気センサBiが地磁気を計測する。そして、計測された重力加速度、地磁気から姿勢検出部30がそれぞれのセンサユニットSiの姿勢を検出する。静状態では、重力加速度及び地磁気は、常に一定の方向に一定の大きさを有する。重力加速度、地磁気からそれぞれのセンサユニットSiの姿勢を検出するため、静状態においても高い精度でセンサユニットSiの姿勢を検出することができる。これにより、高い精度で挿入部11の修正形状75を検出することができる。第2の実施形態次に、本発明の第2の実施形態について図16乃至図21を参照して説明する。なお、第1の実施形態と同一の部分及び同一の機能を有する部分については同一の符号を付して、その説明は省略する。図16は、本実施形態の形状修正部150の構成を示す図である。形状修正部150は、パーティクルフィルタによりそれぞれのセンサユニットSiの位置状態を補正し、線形形状検出部40により検出された検出線形形状71を修正する。図17は、形状修正部50により修正された内視鏡10の挿入部11の修正形状175をグローバル座標CのZ軸の正の方向から見た図である。図17に示すように、形状修正部150により、図17の点線で示す検出線形形状71が修正形状175に修正される。それぞれのセンサユニットSiの位置を補正することにより、それぞれのリンクTjの形状が修正され円弧修正センサ間要素Ljが決定される。修正形状175は、それぞれの円弧Ljから構成されている。実際に、体腔内に挿入された際の内視鏡10の挿入部11の形状は曲線形状である。ここで、内視鏡10の挿入部11は、製品により差はあるが、適度の弾性を有する。このため、挿入部11の曲線形状の曲率が大きく変化することは、稀である。したがって、センサユニットSiの間の形状を所定の半径曲率の円弧Ljと仮定して形状修正を行うことにより、実際の挿入部11の曲線形状と誤差の少ない修正形状175が形成される。図16に示すように、形状修正部150は、形状修正順次実施部151と、形状修正制御部152と、未修正リンク位置補正部153を備える。形状修正順次実施部151は、それぞれのリンクTjごとに基端側グローバル座標系Cの原点から近い側のリンクTjから順に形状修正を行い、円弧修正センサ間要素Ljを形成する。すなわち、第1の実施形態の形状修正順次実施部51が修正リンクT ́jを形成するのに対し、本実施形態の形状修正順次実施部151は円弧Ljを形成する。形状修正制御部152と、未修正リンク位置補正部153については、第1の実施形態の、形状修正制御部52と、未修正リンク位置補正部53と同一の構成及び機能であるため、その説明を省略する。形状修正順次実施部151は、修正対象のリンク修正対象リンクTjの先端側グローバル座標系Cの原点から遠い側のセンサユニット状態補正対象センサSjの位置を補正するセンサ位置補正部センサ状態補正部155と、センサ位置補正部155により補正されたセンサユニットSjの位置に基づいて円弧Ljを形成する円弧形成部修正センサ間要素形成部157とを備える。すなわち、第1の実施形態の修正リンク形成部57が位置補正後のセンサユニットSjの位置に基づいて修正リンクT ́jを形成するのに対し、本実施形態の円弧形成部157は円弧Ljを形成する。形状修正順次実施部151は、パーティクルフィルタを用いて、修正対象のリンクTjの先端側のセンサユニットSjの位置の推定を行うセンサ位置推定部センサ状態推定部161を備える。センサ位置推定部161は、リンクTjの先端に位置する初期位置に基づいて、1回目のセンサユニットSjの位置推定を行う。また、センサ位置推定部161は、1回目の推定結果に基づいて、2回目のセンサユニットSjの位置推定観測を行う。すなわち、センサ位置推定部161は、パーティクルフィルタを用いて、初期位置又は前回の推定の推定結果に基づいてセンサユニットSjの位置推定を行う。また、形状修正順次実施部151は、センサユニットSjの位置推定を所定の回数だけ行う状態にセンサ位置推定部161を制御する位置推定制御部状態推定制御部162を備える。センサ位置推定部161は、パーティクル撒布部163と、パーティクル移動部である曲線補間部165と、重み算出部167と、センサ位置観測部センサ状態観測部169とを備える。パーティクル撒布部163、曲線補間部165、重み算出部167、センサ位置観測部169の詳細については、後述する。ここで、形状修正部150で、線形形状検出部40により検出された検出線形形状71を修正する方法について説明する。図4に示すように、形状修正部150は、パーティクルフィルタによりそれぞれのセンサユニットSiの位置を補正し、線形形状検出部40により検出された検出線形形状71を修正するステップS106。それぞれのセンサユニットSiの位置を補正し検出線形形状71を修正する際、形状修正順次実施部151は、それぞれのリンクTjごとに基端側グローバル座標系Cの原点から近い側のリンクTjから順に形状修正を行い、円弧Ljを形成する。ここで、形状修正順次実施部51により、リンクTjの形状修正を行う方法について説明する。ここでは、基端側からk番目のセンサユニットSk-1と基端側からk1番目のセンサユニットSkとの間の基端側からk番目のリンクTkの形状修正について説明する。図18は、形状修正部150により検出線形形状71の形状を修正する方法を示すフローチャートである。図19乃至図21は、形状修正順次実施部151での処理を説明する図である。図19に示すように、形状修正順次実施部151により、リンクTkの形状修正が行われる前の状態では、第1の実施形態と同様に、リンクTk-1まで形状修正が完了し、修正完了部177が形成されている。また、リンクTk~TNから構成される修正未完了部179が形成されている。この際、修正完了部177と修正未完了部179との境界は連続し、センサユニットSkはグローバル座標系Cで点Uk_0に位置している。図18及び図19に示すように、この状態で、形状修正順次実施部151により、修正未完了部179の中で最も基端側に位置するリンク修正対象リンクTkの形状修正が開始される。すなわち、形状修正制御部152により、形状修正順次実施部151がリンクTkの形状修正を行う状態に制御される。形状修正順次実施部51によりリンクTkの形状修正を行う際には、まず、センサ位置推定部161によりリンクTkの先端側のセンサユニット状態補正対象センサSkの位置を、パーティクルフィルタを用いて、リンクTkの先端に位置する初期位置Uk_0に基づいて推定する。図18及び図19に示すように、センサユニットSkの位置を初期位置に基づいて推定する際は、まず、パーティクル撒布部163によりリンクTkの先端側のセンサユニットSkの初期位置Uk_0の周辺に所定の数本実施形態ではA個とするのパーティクル172を撒くステップS192。ここで、それぞれのパーティクル172は、第1の実施形態と同様に式17の範囲で一様に撒かれる。そして、曲線補間部165により、センサユニットSk-1とパーティクル撒布部163により撒かれたそれぞれのパーティクル172との間の曲線補間を行うステップS193。曲線補間部165は、挿入部11の曲がり状態に基づいた移動モデルを用いて曲線補間を行い、それぞれのパーティクル172の位置を移動させる。すなわち、曲線補間部165は、挿入部11の曲がり状態に基づいた移動モデルを用いてそれぞれのパーティクル172を移動させるパーティクル移動部である。曲線補間部165により、それぞれのパーティクル172は補間前パーティクル位置移動前パーティクル位置から補間後パーティクル位置移動後パーティクル位置へ移動する。ここでは、センサユニットSk-1とパーティクル172の中の1つであるパーティクル172Aとの間の曲線補間について説明する。図19に示すように、センサユニットSk-1とパーティクル172Aとの間の曲線補間リンクTkの曲線補間を行う前の状態では、パーティクル172Aは補間前パーティクル位置F1に位置している。ここで、センサユニットSk-1とパーティクル172Aの補間前パーティクル位置F1とを結ぶ線分を第1の線形形状171Aとする。第1の線形形状171Aの寸法は、b ́である。また、パーティクル172Aの補間前パーティクル位置F1とリンクTkの先端に位置するセンサユニットSk1と結ぶ線分を第2の線形形状171Bとする。そして、第1の線形形状171Aの基端を通り、第1の線形形状171Aに垂直な第1の補間法線174Aと、第1の線形形状171Aの先端を通り、第2の線形形状171Bに垂直な第2の補間法線174Bとを形成する。そして、第1の補間法線174Aと第2の補間法線174Bとの交点Oを算出する。第1の補間法線174Aと第2の補間法線174Bとが交わらない場合は、第1の補間法線174Aと第2の補間法線174Bとの距離が最小となる2点の中間点Oを算出する。そして、交点O又は中間点Oを中心Oとして円弧176を形成する。円弧176は、中心Oと第1の線形形状171Aの基端との間の距離と同一の半径Rを有する。すなわち、円弧176は曲率1/Rを有する。また、円弧176は、第1の線形形状171Aの基端を始点とする弧の長さが第1の線形形状171Aの寸法b ́に等しい円弧176である。円弧176の先端F2が、パーティクル172Aの補間後パーティクル位置F2となる。すなわち、曲線補間部165での曲線補間により、パーティクル172Aの位置が補間前パーティクル位置F1から補間後パーティクル位置F2に移動する。その他のパーティクルについても、同様にして補間前パーティクル位置から補間後パーティクル位置へ移動される。そして、第1の実施形態と同様に、重み算出部167によりそれぞれのパーティクル172の重みwk_1aを算出するステップS194。すなわち、それぞれのパーティクル72の重みが、初期のwk_0aからwk_1aに更新される。重み算出部167は、まず、それぞれのパーティクル172の補間後パーティクル位置移動後パーティクル位置の尤もらしさを評価する尤度vk_1aを算出する。尤度は、式18~式20を用いて算出される。ただし、本実施形態では、bk_1aはグローバル座標系Cにおいて、位置補正後のセンサユニットSk-1とそれぞれのパーティクル172の補間後パーティクル位置との間の円弧の弧長である。例えば図20に示すように、センサユニットSk-1とパーティクル172Aこのパーティクルでは、a=sとするとの間では、前述のように形成された円弧176の弧長b ́が、bk_1sとなる。また、式19でb0は、それぞれのセンサユニットSiのセンサ間寸法lとする。式19より、位置補正後のセンサユニットSk-1とパーティクル172との間の円弧の弧長と、センサ間寸法lとの差が小さいほど関数hbk_1aは大きくなり、尤度が大きくなる。逆に、位置補正後のセンサユニットSk-1とパーティクル172との間の円弧の弧長と、センサ間寸法lとの差が大きいほど関数hbk_1aは小さくなり、尤度が小さくなる。ただし、前述のように位置補正後のセンサユニットSk-1とそれぞれのパーティクル172の補間後パーティクル位置との間の円弧の弧長は、位置補正後のセンサユニットSk-1とそれぞれのパーティクル172の補間前パーティクル位置との間の線分の寸法と同一であるため、bk_1aをセンサユニットSk-1とそれぞれのパーティクル172の補間前パーティクル位置との間の線分の寸法とした場合も、同様の結果が得られる。また、本実施形態では、φk_1aは、位置補正後のセンサユニットSk-1からそれぞれのパーティクル172の補間後パーティクル位置までのベクトルV ́1と、それぞれのパーティクル172の補間後パーティクル位置から位置補正前のセンサユニットSk1までのベクトルV ́2とが成す角度である。例えば図20に示すように、パーティクル172Aこのパーティクルでは、a=sとするでは、センサユニットSk-1からパーティクル172Aの補間後パーティクル位置F2までのベクトルV ́1と、パーティクル172Aの補間後パーティクル位置F2からセンサユニットSk1までのベクトルV ́2との成す角度が、φk_1sとなる。式20よりベクトルV ́1とV ́2の成す角度が0度~90度の範囲であれば、関数gφk_1aは大きくなり、尤度が大きくなる。逆に、それ以外の角度であれば、関数gφk_1aは小さくなり、尤度が小さくなる。そして、それぞれのパーティクル172の尤度から式21を用いて、重みwk_1aを算出する。そして、センサ位置観測部169が、重み算出部67で算出されたそれぞれのパーティクル172の重みに基づいて、センサユニットSkの位置を観測するステップS195。この際、センサ位置観測部169は、図21に示すように、それぞれのパーティクル172の曲線補間部165により補間される前の補間前パーティクル位置の重み付き平均を算出し、算出された位置を移動前センサ位置U ́k_1とする。そして、パーティクル移動部65が曲線補間を行う際に用いた移動モデルにより、移動前センサ位置U ́k_1から移動後センサ位置Uk_1への移動を行い、移動後センサ位置Uk_1をセンサユニットSkの位置として推定する。そして、位置推定制御部162により、所定の回数本実施形態ではM回とするだけセンサユニットSkの位置推定が行われたかを判断するステップS196。現状態では、センサユニットSkの位置推定が1回しか行われていないためステップS196-No、センサ位置推定部161は位置推定制御部162により、センサユニットSkの位置推定をもう1回行う状態に制御される。2回目以降の位置推定では、センサ位置推定部161は、前回の位置推定での推定結果に基づいて、センサユニットSkの位置推定を行う。そして、ステップS192に戻り、パーティクル撒布部163により所定の数本実施形態ではA個とするのパーティクル172を撒くステップS192。2回目以降の位置推定では、パーティクル撒布部163は、前回の位置推定の際に重み算出部167で算出されたそれぞれのパーティクル172の重みに基づいてパーティクル172を撒く。結果として、例えば、前回の位置推定の際に算出された重みが大きいパーティクル172の補間前パーティクル位置の周辺では、撒くパーティクル172の数を多くする。一方、前回の位置推定の際に算出された重みが小さいパーティクル172の補間前パーティクル位置の周辺では、撒くパーティクル72の数を少なくする。そして、2回目以降の位置推定でも同様に、曲線補間部165が、1回目の推定で用いた移動モデルに基づいて、センサユニットSk-1とそれぞれのパーティクル172との間の曲線補間を行うステップS193。これにより、それぞれのパーティクル172は、補間前パーティクル位置から補間後パーティクル位置へ移動する。そして、重み算出部167により、式18~式20と同様の式を用いて、センサユニットSkがそれぞれのパーティクル172の補間後パーティクル位置に位置する尤もらしさを評価する尤度を算出する。尤度により、それぞれのパーティクル172の補間後パーティクル位置の尤もらしさが検出される。そして、式21と同様の式を用いて、尤度からそれぞれのパーティクル72の重みが算出されるステップS194。ここで、例えばt回目の位置推定では、式18~式21の尤度vk_1a,重みwk_1a,bk_1a,φk_1aの代わりに、それぞれ尤度vk_ta,重みwk_ta,bk_ta,φk_taを用いて計算する。そして、センサ位置観測部169が、重み算出部167で算出されたそれぞれのパーティクル172の重みに基づいて、センサユニットSkの位置を観測するステップS195。例えば、t回目の位置推定では、それぞれのパーティクル172の重み付平均から補間前センサ位置U ́k_tが算出され、上述した移動モデルで曲線補間を行う。曲線補間により、補間前センサ位置U ́k_tから補間後センサ位置Uk_tへの移動が行われる。そして、位置推定制御部162により、所定の回数本実施形態ではM回とするだけセンサユニットSkの位置推定が行われたかを判断するステップS196。センサユニットSkの位置推定が所定の回数だけ行われていない場合はステップS196-No、センサ位置推定部161は位置推定制御部162により、センサユニットSkの位置推定をもう1回行う状態に制御され、ステップS192~ステップS195をもう1回行う。センサユニットSkの位置推定が所定の回数だけ行われている場合はステップS196-Yes、次のステップに進む。図17に示すように、センサユニットSkの位置推定が所定の回数例えば2回だけ行った後、センサ位置補正部155は、センサ位置推定部161の推定結果に基づいて、修正対象のリンク修正対象リンクTkの先端側のセンサユニット状態補正対象センサSkの位置を補正するステップS197。センサ位置補正部155は、センサユニットSkの位置を、リンクTkの先端に位置する初期位置Uk_0からセンサ位置推定部161の最終回本実施形態ではM回目の位置推定の際に推定されたセンサユニットSkの補間後センサ位置Uk_Mへと、補正する。また、円弧形成部157が、センサ位置補正部155により補正されたセンサユニットSkの位置に基づいて円弧Lkを形成するステップS197。ここでは、位置補正後のセンサユニットSk-1とセンサユニットSkの移動後センサ位置Uk_Mとを結ぶ円弧が、円弧Lkとなる。なお、位置補正後のセンサユニットSk-1とセンサユニットSkの補間後センサ位置Uk_Mとを結ぶ円弧は、センサユニットSk-1とセンサユニットSkの補間前センサ位置U ́k_Mとの間を上述した移動モデルで曲線補間を行うことにより形成される。センサユニットSkの位置の移動により、形状修正順次実施部151によりリンクTkの形状修正が行われる前の状態では連続していた修正完了部177と修正未完了部179との境界が、不連続な状態となる。すなわち、円弧LkとリンクTk1が不連続な状態となる。そして、第1の実施形態と同様に、この状態で、未修正リンク位置補正部153が、修正未完了部179を修正完了部177と境界が連続する状態に平行移動して、修正未完了部179の位置を補正するステップS198。なお、リンクTk以外のリンクTjについても、形状修正順次実施部151により同様に形状修正が行われ、円弧Ljが形成される。また、未修正リンク位置補正部153は、形状修正順次実施部151により1つのリンクTjの形状修正が行われるたびに、修正未完了部179の位置を補正する。そして、図17に示すように、形状修正制御部152が、すべてのリンクTjについて形状修正が完了したか確認するステップS199。すべてのリンクTjについて形状修正が完了している場合は、挿入部11の修正形状175が形成され、次のステップへ進むステップS199-Yes。すべてのリンクTjについて形状修正が完了していない場合は、ステップS191に戻りステップS199-No、形状修正順次実施部151により、修正未完了部179の中で最も基端側グローバル座標系Cの原点から近い側に位置するリンク修正対象リンクTjの形状修正が行われる。すなわち、ステップS191~S198が、すべてのリンクTjについて形状修正が完了するまで繰り返し行われる。そこで、上記構成の内視鏡形状検出装置1及び内視鏡形状検出装置1を用いた内視鏡10の挿入部11の形状検出方法では、以下の効果を奏する。なわち、内視鏡形状検出装置1では、それぞれのセンサユニットSiの計測データから姿勢検出部30がセンサユニットSiの姿勢を検出し、線形形状検出部40がそれぞれのセンサユニットSiの姿勢から内視鏡10の挿入部11の検出線形形状71を検出する。そして、形状修正部150が、パーティクルフィルタによりそれぞれのセンサユニットSiの位置を補正し、線形形状検出部40により検出された検出線形形状71を修正する。そして、挿入部11の修正形状175が形成される。以上のように、観察時に体腔内に挿入される挿入部11に配置されるセンサユニットSiの計測データから、挿入部11の修正形状175が検出されるため、体外にセンスコイル等を設ける必要がない。このため、内視鏡形状検出装置1の小型化、単純化を実現することができる。また、内視鏡形状検出装置1では、形状修正部150により、パーティクルフィルタを用いてそれぞれのセンサユニットSiの位置を補正し、検出線形形状71を修正している。形状修正部150では、形状修正順次実施部151のセンサ位置推定部161によりそれぞれのセンサユニットSiの推定を所定の回数だけ行う。センサユニットSiの推定の際、重み算出部167によりそれぞれのパーティクル172の重みを算出する。そして、センサ位置観測部169が、重み算出部167で算出されたそれぞれのパーティクル172の重みに基づいて、センサユニットSiの位置を観測する。算出されるパーティクル172の重みが大きいほど、曲線補間部165での曲線補間により補間されたそれぞれのパーティクル172の補間後パーティクル位置移動後パーティクル位置が、体腔内に挿入される挿入部11でのセンサユニットSiの尤もらしい位置位置する確率が高い位置となる。このため、センサ位置観測部169により、高い精度でセンサユニットSiの位置が観測される。センサ位置補正部155は、センサ位置推定部161での最終回の推定でセンサ位置観測部169により観測された位置である補間後センサ位置移動後センサ位置に、それぞれのセンサユニットSiの位置を補正する。このため、位置補正後のセンサユニットSiの位置は、体腔内に挿入される挿入部11でのセンサユニットSiの尤もらしい位置位置する確率が高い位置となる。形状修正部150は、位置補正後のセンサユニットSiの位置に基づいて、検出線形形状71の修正を行うため、実際の挿入部11の形状と誤差の少ない修正形状175を検出することができる。これにより、高い精度で挿入部11の修正形状175を検出することができる。さらに、内視鏡形状検出装置1では、形状修正順次実施部151によりそれぞれのリンクTjが円弧修正センサ間要素Ljに形状修正される。実際に、体腔内に挿入された際の内視鏡10の挿入部11の形状は曲線形状である。内視鏡10の挿入部11は、製品により差はあるが、適度の弾性を有する。このため、挿入部11の曲線形状の曲率が大きく変化することは、稀である。したがって、センサユニットSiの間の形状を所定の半径曲率の円弧Ljに形状修正を行うことにより、実際の挿入部11の曲線形状と誤差の少ない修正形状175を検出することができる。第1の実施形態の変形例次に、上述の第1の実施形態の変形例について説明する。なお、第1の実施形態と同一の部分及び同一の機能を有する部分については同一の符号を付して、その説明は省略する。本変形例では、内視鏡10の挿入部11が平行移動している動状態での挿入部11の修正形状75を検出可能である。すなわち、センサ位置推定部61のパーティクル移動部65が、それぞれのセンサユニットSiの加速度センサAiにより計測された動状態での加速度データに基づいて、それぞれのパーティクル72の移動を行う。動状態での加速度データに基づいてそれぞれのパーティクルの移動を行う際は、まず、パソコン8に設けられる加速度検出部図示しないが、それぞれのセンサユニットSiの加速度センサAiにより計測された動状態での加速度データを取得する。そして、加速度検出部は、動状態での加速度データに基づいて、それぞれのセンサユニットSiの中心で計測される加速度ベクトルを、グローバル座標系CのX軸方向成分、Y軸方向成分、Z軸方向成分に分解する。そして、それぞれのセンサユニットSiの中心で計測される加速度ベクトルの、グローバル座標系CのX軸方向成分、Y軸方向成分、Z軸方向成分を検出する。動状態では、静状態で鉛直方向に発生する重力加速度に、挿入部11の移動による成分が加わるため、それぞれのセンサユニットSiの中心で計測される加速度ベクトルのグローバル座標系CのX軸方向成分、Y軸方向成分、Z軸方向成分を、とする。また、それぞれのセンサユニットSiの加速度センサAiにより計測された加速度ベクトルのローカル座標系CiのXi軸方向成分、Yi軸方向成分、Zi軸方向成分を、とする。また、本変形例では、内視鏡10の挿入部11が平行移動している場合を考えるため、近接して配置される複数のセンサユニットSiにおいて、グローバル座標系Cでの加速度ベクトルは、同一であると仮定する。実際の内視鏡10では、それぞれのセンサユニットSiの間のセンサ間寸法lは非常に小さいため、このような仮定は可能である。このように仮定することにより、例えばセンサユニットS1で計測される加速度ベクトルとセンサユニットS2で計測される加速度ベクトルとの間で、となる。そして、式24.1、式24.2からそれぞれ、の関係が成り立つ。ここで、センサユニットS1では、グローバル座標系Cとローカル座標系C1の関係から、式1の回転行列を用いて、となる。同様に、センサユニットS2では、グローバル座標系Cとローカル座標系C2の関係から、式1の回転行列を用いて、となる。式25.1の関係を利用して、式26,27を解くことにより、センサユニットS1,S2で計測された重力加速度以外の加速度ベクトルの、グローバル座標系CのX軸方向成分ath1_xath2_X、Y軸方向成分ath1_Yath2_Y、Z軸方向成分ath1_Zath2_Zが求まる。式13で示す地磁気センサBiにより計測される地磁気ベクトルのグローバル座標系Cのそれぞれの軸方向成分と、式9で示す地磁気ベクトルのローカル座標系Ciのそれぞれの軸方向成分との関係は、式1の回転行列を用いて、となる。式28においてi=1を代入した地磁気の関係式を含めて式25.12627を解く場合、方程式12個に対して、未知数が姿勢角6個に加え加速度が6個で合計12個である。その結果、センサユニットS1,S2で計測された重力加速度以外の加速度ベクトルの、グローバル座標系CのX軸方向成分ath1_Xath2_X、Y軸方向成分ath1_Yath2_Y、Z軸方向成分ath1_Zath2_Zを数値演算的に解くことが可能である。その他のセンサユニットSiについても同様にして、それぞれのセンサユニットSiで計測された重力加速度以外の加速度ベクトルの、グローバル座標系CのX軸方向成分athi_X、Y軸方向成分athi_Y、Z軸方向成分athi_Zが求まる。そして、パソコン8に設けられる変位算出部図示しないが、加速度検出部で検出された重力加速度以外の加速度ベクトルに基づいて、前回の静状態からのそれぞれのセンサユニットSiの変位を算出する。前回の静状態からのそれぞれのセンサユニットSiの変位のグローバル座標系CのX軸方向成分、Y軸方向成分、Z軸方向成分は、それぞれのセンサユニットSiで計測された重力加速度以外の加速度ベクトルの、グローバル座標系CのX軸方向成分athi_X、Y軸方向成分athi_Y、Z軸方向成分athi_Zをそれぞれ2重積分することにより求まる。また、変位算出部では、それぞれのセンサユニットSiの速度ベクトルのグローバル座標系CのX軸方向成分、Y軸方向成分、Z軸方向成分も算出される。それぞれのセンサユニットSiの速度ベクトルのグローバル座標系CのX軸方向成分、Y軸方向成分、Z軸方向成分は、それぞれのセンサユニットSiで計測された重力加速度以外の加速度ベクトルの、グローバル座標系CのX軸方向成分athi_X、Y軸方向成分athi_Y、Z軸方向成分athi_Zをそれぞれ1回だけ積分することにより求まる。パーティクル移動部65は、加速度検出部及び変位算出部で検出されたそれぞれのセンサユニットSiの加速度ベクトル、速度ベクトル、変位のグローバル座標系CのX軸方向成分、Y軸方向成分、Z軸方向成分に基づいて、それぞれのパーティクル72の移動モデルを検出する。そして、移動モデルに基づいてそれぞれのパーティクル72を、移動前パーティクル位置から移動後パーティクル位置へと移動させる図9のステップS113。すなわち、パーティクル移動部65は、動状態で検出されたそれぞれのセンサユニットSiの加速度ベクトル、速度ベクトル、変位から移動モデルを検出し、移動モデルに基づいてそれぞれのパーティクル72を移動する動状態移動モデル実施部となる。そして、上述のように、移動後パーティクル位置の尤もらしさであるそれぞれのパーティクル72の尤度から、重みを算出するステップS114。そして、それぞれのパーティクル72の重みからセンサユニットSiの位置を観測するステップS115。以上のようなセンサユニットSiの位置推定を所定の回数行い、最終回の位置推定により推定された移動後センサ位置にセンサユニットSiの位置を補正するステップS117。そして、位置補正後のセンサユニットSiの位置に基づいて、それぞれのセンサユニットSiの間の形状修正後の形状である修正センサ間要素を形成する。ステップS117。以上のようにして、動状態においても、検出線形形状71を形状修正した修正形状75が検出可能となっている。その他の変形例なお、上述の実施形態では、センサ位置補正部55,155により、修正対象リンクTjの先端側のセンサユニットSjの位置補正を行っているが、例えば位置補正に加えて、センサユニットSjの姿勢の補正を行ってもよい。すなわち、センサ位置補正部55,155は、修正対象リンクTjの先端側のセンサユニットSjの少なくとも位置を補正するセンサ状態補正部であればよい。この場合、センサ位置推定部61,161により、パーティクルフィルタを用いてセンサユニットSjの姿勢の推定も行われる。すなわち、センサ位置推定部61,161は、パーティクルフィルタを用いてセンサユニットSjの少なくとも位置の推定を行うセンサ状態推定部となっている。また、センサ位置観測部69,169はセンサユニットSjの少なくとも位置を観測するセンサ状態観測部となる。例えば、センサユニットSkの姿勢推定を行う場合には、パーティクル撒布部63により撒かれるそれぞれのパーティクル72は、式17.1~式17.3に示すグローバル座標系での位置に加え、姿勢を有する。ここで、式7,式8,式12又は式15により求められるセンサユニットSkのローカル座標系Ckの姿勢角をαk,βk,γkがセンサユニットSkの初期姿勢初期状態であり、それぞれの姿勢角の誤差の分散をδα,δβ,δγとする。1回目の推定ではパーティクル72は、式7,式8,式12又は式15により求められるセンサユニットSkの初期姿勢姿勢角αk,βk,γkに誤差の分散δα,δβ,δγを加えた範囲に一様に撒かれる。すなわち、センサユニットSkの1回目の推定でのそれぞれのパーティクル72の姿勢角をαk_1a,βk_1a,γk_1aa=1,2,...,Aとすると、それぞれのパーティクル72は、の範囲で一様に撒かれる。誤差の分散δα,δβ,δγは、ノイズ等により生じるそれぞれのセンサユニットSiの計測データの誤差の分散から、式1~式15を用いて分散を求める。2回目以降の推定では、前回の推定でのパーティクル72の重みに基づいてそれぞれのパーティクル72の姿勢が定められる。また、上述の実施形態では、それぞれのローカル座標系Ciは、Yi軸方向がセンサスニットSiの中心での長手方向と一致している座標系である。しかし、本発明では、それぞれのローカル座標系Ciは、センサユニットSiの中心を原点とし、Xi軸、Yi軸、Zi軸のいずれか1つの軸がセンサユニットSiの中心での長手方向と軸方向が一致する長手方向軸である座標系であればよい。ただし、Xi軸が長手方向軸である場合は、式16.1,式16.2でeyk-1代わりに、を用いる。同様に、Zi軸が長手方向軸である場合は、式16.1,式16.2でeyk-1代わりに、を用いる。ここで、exk-1はローカル座標系Ck-1の原点での長手方向であるXk-1軸方向の単位ベクトルであり、ezk-1はローカル座標系Ck-1の原点での長手方向であるZk-1軸方向の単位ベクトルである。また、上述の実施形態では、グローバル座標系Cは、最も基端側のセンサユニットS0の中心を原点とし、Z軸が鉛直方向と一致し、X軸及びY軸が水平面上に配置される座標系である。しかし、本発明では、X軸、Y軸、Z軸のいずれか1つの軸が鉛直方向と軸方向が一致する鉛直方向軸であり、鉛直方向軸以外の2つの軸が水平面上に配置される水平方向軸である座標系であればよい。これにより、加速度センサAiにより計測される重力加速度及び地磁気センサBiにより計測される地磁気に基づいて、姿勢検出部30はそれぞれのセンサユニットSiの姿勢を検出することが可能である。ただし、X軸が鉛直方向軸の場合は、式2で示される重力加速度ベクトルのグローバル座標系CのX軸方向成分、Y軸方向成分、Z軸方向成分は、となる。また、グローバル座標系CのX軸方向には、地磁気は作用しない。この場合、第1の実施形態では、姿勢角αi,βi,γiはヨー角γi、ピッチ角αi、ロール角βiの順で回転するZ,X,Y型としたが、姿勢角αi,βi,γiの回転する順序を変えて、式1の回転行列とは別の回転行列を用いる。これにより、加速度センサAiで計測された加速度データに基づいて、第1の角度算出部34で水平方向軸であるY軸,Z軸回りの姿勢角βi,γiが算出される。また、地磁気センサBiで計測された地磁気データに基づいて、第2の角度算出部36で鉛直方向軸であるX軸回りの姿勢角αiが算出される。Y軸が鉛直方向軸の場合についても同様であり、加速度センサAiで計測された加速度データに基づいて、第1の角度算出部34で水平方向軸であるX軸,Z軸回りの姿勢角αi,γiが算出される。また、地磁気センサBiで計測された地磁気データに基づいて、第2の角度算出部36で鉛直方向軸であるY軸回りの姿勢角βiが算出される。さらに、上述の実施形態では、グローバル座標系Cは、最も基端側のセンサユニットS0の中心を原点としているが、最も先端側のセンサユニットSNの中心を原点としてもよい。この場合、リンク形成部41は、式16.1,式16.2を用いて、リンクTjの先端側グローバル座標系Cの原点から近い側のセンサユニットSjがグローバル座標系Cの原点に位置する場合の、リンクTjの基端側グローバル座標系Cの原点から遠い側のセンサユニットSj-1の座標P ́jlxj,lyj,lzjを求める。そして、グローバル座標系Cの原点と座標P ́jlxj,lyj,lzjとを直線状に結ぶことにより、リンクTjが形成される。また、形状修正部50の形状修正順次実施部51によりリンクTjの形状修正を行う際は、先端側グローバル座標系Cの原点から近い側のリンクTjから順に形状修正を行が行われる。この際、センサ位置補正部55は、修正対象のリンク修正対象リンクTjの基端側グローバル座標系Cの原点から遠い側のセンサユニット状態補正対象センサSjの位置を補正する。したがって、センサ位置観測部61により、修正対象のリンク修正対象リンクTjの基端側グローバル座標系Cの原点から遠い側のセンサユニット状態補正対象センサSjの観測が所定の回数だけ行われる。以上、本発明の実施形態について説明したが、本発明は上記の実施形態に限定されるものではなく、本発明の要旨を逸脱しない範囲で種々の変形ができることは勿論である。"