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

Abstract

内視鏡形状検出装置（１）は、センサユニット（Ｓｉ）での計測データに基づいて、それぞれの前記センサユニット（Ｓｉ）の姿勢を検出する姿勢検出部（３０）と、前記姿勢検出部（３０）で検出されたそれぞれの前記センサユニット（Ｓｉ）の姿勢に基づいて、それぞれの前記センサユニット（Ｓｉ）の間の形状を寸法が前記センサ間寸法（ｌ）に等しい直線状のリンク（Ｔｊ）と仮定して、前記内視鏡（１０）の前記挿入部（１１）の検出線形形状（７１）を検出する線形形状検出部（４０）と、を備える。また、内視鏡形状検出装置（１）は、パーティクルフィルタを用いてそれぞれの前記センサユニット（Ｓｉ）の少なくとも位置を補正し、前記線形形状検出部（４０）により検出された前記検出線形形状（７１）を修正した修正形状（７５，１７５）を検出する形状修正部（５０，１５０）を備える。

Description

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

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

また、特許文献２には、内視鏡の挿入部に２つのセンサが取付けられた内視鏡装置が開示されている。この内視鏡装置では、基端側のセンサを基準として、先端側のセンサの位置、姿勢が検出される。基端側のセンサは湾曲部の基端の近傍に、先端側のセンサは先端硬性部に配置されている。基端側のセンサに対する先端側のセンサの位置、姿勢を検出することにより、湾曲部の湾曲角度、湾曲方向が算出される。

また、特許文献３には、内視鏡の挿入部に複数のジャイロが取付けられた内視鏡形状検出装置が開示されている。この内視鏡形状検出装置では、ジャイロにより内視鏡の挿入部の所定の部位（ジャイロが取り付けられた部位）の姿勢が検出される。そして、検出された所定の部位での姿勢に基づいて、挿入部の形状が検出される。

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

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

上記特許文献２では、体腔内のセンサのみを用いて湾曲部の湾曲角度、湾曲方向が検出される。しかし、実際の内視鏡の挿入部は可撓性を有し、体腔内に挿入された際の挿入部の形状は、曲線状である。すなわち、この内視鏡装置では、湾曲部の湾曲角度、湾曲方向が検出されるのみで、体腔内への挿入時の挿入部の形状までは高い精度で検出されない。

上記特許文献３では、複数のジャイロを用いて内視鏡の挿入部の形状が検出される。しかし、ジャイロは挿入部が移動する際に発生する慣性力又はコリオリ力に基づいて挿入部の所定の部位での姿勢を検出する。このため、挿入部が移動していない静状態では、形状を検出できず、低速度で挿入部が移動する際の検出精度は低下してしまう。

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

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

また、本発明の別のある態様では、内視鏡の挿入部に、長手方向に互いに所定のセンサ間寸法だけ離れて配置される複数のセンサユニットで計測を行うことと、前記センサユニットでの計測データに基づいて、それぞれの前記センサユニットの姿勢を検出することと、検出されたそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間の形状を寸法が前記センサ間寸法に等しい直線状のリンクと仮定して、前記内視鏡の前記挿入部の検出線形形状を検出することと、パーティクルフィルタを用いてそれぞれの前記センサユニットの少なくとも位置を補正し、前記線形形状検出部により検出された前記検出線形形状を修正した修正形状を検出することと、を備える内視鏡の挿入部の形状検出方法を提供する。

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

本発明の第１の実施形態に係る内視鏡形状検出装置の構成を示すブロック図。 第１の実施形態に係る内視鏡の挿入部の構成を示す概略図。 第１の実施形態に係る内視鏡形状検出装置のパソコンの構成を示すブロック図。 第１の実施形態に係る内視鏡の挿入部の静状態での形状を検出する方法を示すフローチャート。 第１の実施形態に係る内視鏡形状検出装置のグローバル座標系と補正座標系とを比較して示す概略図。 第１の実施形態に係る内視鏡形状検出装置の線形形状検出部で検出される検出線形形状を示す概略図。 第１の実施形態に係る線形形状検出部のリンク位置補正部での処理を説明する概略図。 第１の実施形態に係る内視鏡形状検出装置の形状修正部で形状修正された修正形状を示す概略図。 第１の実施形態に係る形状修正部により検出線形形状の形状を修正する方法を示すフローチャート。 第１の実施形態に係る形状修正部のセンサ位置観測部での１回目の観測の際に、パーティクル撒布部で撒かれるそれぞれのパーティクルの撒布状態を示す概略図。 第１の実施形態に係る形状修正部のセンサ位置観測部の重み算出部での処理を説明する概略図。 第１の実施形態に係る形状修正部のセンサ位置観測部のセンサ位置推定部での処理を説明する概略図。 第１の実施形態に係る形状修正部のセンサ位置観測部での２回目以降の観測において、現在の観測の１回だけ前の観測の際に撒かれたパーティクルの撒布状態を示す概略図。 第１の実施形態に係る形状修正部のセンサ位置観測部での２回目以降の観測において、現在の観測の際に撒かれたパーティクルの撒布状態を示す概略図。 第１の実施形態に係る形状修正部のセンサ位置補正部及び修正リンク形成部での処理を説明する概略図。 第１の実施形態に係る形状修正部の未修正リンク位置補正部での処理を説明する概略図。 本発明の第２の実施形態に係る内視鏡形状検出装置の形状修正部の構成を示すブロック図。 第２の実施形態に係る内視鏡形状検出装置の形状修正部で形状修正された修正形状を示す概略図。 第２の実施形態に係る形状修正部により検出線形形状の形状を修正する方法を示すフローチャート。 第２の実施形態に係る形状修正部のセンサ位置観測部のパーティクル撒布部及び曲線補間部での処理を説明する概略図。 第２の実施形態に係る形状修正部のセンサ位置観測部の重み算出部での処理を説明する概略図。 第２の実施形態に係る形状修正部のセンサ位置観測部のセンサ位置推定部での処理を説明する概略図。

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

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

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

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

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

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

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

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

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

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

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

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

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

となる。

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

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

とする。

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

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

となり、

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

式（７）、式（８）、式（１２）（又は式（１５））に示すように、ステップＳ１０２，Ｓ１０３でローカル座標系Ｃ_ｋ−１（つまりリンク）の姿勢角α_ｋ−１，β_ｋ−１，γ_ｋ−１が算出される。この姿勢α_ｋ−１，β_ｋ−１，γ_ｋ−１及びそれぞれのセンサユニットＳ_ｉの間の長手方向についての間隔であるセンサ間寸法ｌを用いて、センサユニットＳ_ｋ−１がグローバル座標系Ｃの原点に位置する場合の、センサユニットＳ_ｋの座標Ｐ´_ｋ（ｌ_ｘｋ，ｌ_ｙｋ，ｌ_ｚｋ）が求まる。ここで、座標Ｐ´_ｋは、

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

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

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

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

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

なお、リンクＴ_ｋ以外のリンクＴ_ｊについても、リンク位置補正部４２により同様に位置が補正される。すなわち、リンクＴ_ｊの位置補正を行う際は、原点からリンク位置補正完了部７３の先端（グローバル座標系Ｃの原点から遠い側の端）までの移動量だけ、リンク位置補正部４２はリンクＴ_ｊを平行移動する。これにより、リンクＴ_ｊとリンクＴ_ｊの基端側（グローバル座標系Ｃの原点から近い側）に隣接するリンクＴ_ｊ−１とのリンク境界が連続する状態となり、リンクＴ_ｊの位置が補正される。ただし、リンクＴ_１については、リンクＴ_１の基端がグローバル座標系Ｃの原点であるため、位置の補正は行われない。

図３に示すように、線形形状検出部４０は、形状修正部５０に接続されている。形状修正部５０は、パーティクルフィルタによりそれぞれのセンサユニットＳ_ｉの位置（状態）を補正し、線形形状検出部４０により検出された検出線形形状７１を修正する。図８は、形状修正部５０により修正された内視鏡１０の挿入部１１の修正形状７５をグローバル座標ＣのＺ軸の正の方向から見た図である。図８に示すように、形状修正部５０により、図８の点線で示す検出線形形状７１が修正形状７５に修正される。それぞれのセンサユニットＳ_ｉの位置を補正することにより、それぞれのリンクＴ_ｊの形状が修正され修正リンク（修正センサ間要素）Ｔ´_ｊが決定される。修正形状７５は、それぞれの修正リンクＴ´_ｊから構成されている。

形状修正部５０は、それぞれのリンクＴ_ｊごとに基端側（グローバル座標系Ｃの原点から近い側）のリンクＴ_ｊから順に形状修正を行い、修正リンクＴ´_ｊを形成する形状修正順次実施部５１と、すべてのリンクＴ_ｊについて形状修正を行う状態に形状修正順次実施部５１を制御する形状修正制御部５２とを備える。また、形状修正部５０は、形状修正順次実施部５１により１つのリンクＴ_ｊの形状修正が行われるたびに、形状修正が行われていないリンク（未修正リンク）Ｔ_ｊから構成される修正未完了部７９を平行移動し、位置を補正する未修正リンク位置補正部５３を備える。未修正リンク位置補正部５３により、修正未完了部７９が、形状修正により形成される修正リンクＴ´_ｊから構成される修正完了部７７と境界が連続する状態に平行移動される。

図３に示すように、形状修正順次実施部５１は、修正対象のリンク（修正対象リンク）Ｔ_ｊの先端側（グローバル座標系Ｃの原点から遠い側）のセンサユニット（状態補正対象センサ）Ｓ_ｊの位置（状態）を補正するセンサ位置補正部（センサ状態補正部）５５と、センサ位置補正部５５により補正されたセンサユニットＳ_ｊの位置に基づいて修正リンクＴ´_ｊを形成する修正リンク形成部（修正センサ間要素形成部）５７とを備える。

また、形状修正順次実施部５１は、パーティクルフィルタを用いて、修正対象のリンクＴ_ｊの先端側のセンサユニットＳ_ｊの位置の推定を行うセンサ位置推定部（センサ状態推定部）６１を備える。センサ位置推定部６１は、リンクＴ_ｊの先端に位置する初期位置（初期状態）に基づいて、１回目のセンサユニットＳ_ｊの位置推定を行う。また、センサ位置推定部６１は、１回目の推定結果に基づいて、２回目のセンサユニットＳ_ｊの位置推定を行う。すなわち、センサ位置推定部６１は、パーティクルフィルタを用いて、初期位置又は前回の推定の推定結果に基づいてセンサユニットＳ_ｊの位置推定を行う。さらに、形状修正順次実施部５１は、センサユニットＳ_ｊの位置推定を所定の回数だけ行う状態にセンサ位置推定部６１を制御する位置推定制御部（状態推定制御部）６２を備える。

センサ位置推定部６１は、パーティクル撒布部６３と、パーティクル移動部６５と、重み算出部６７と、センサ位置観測部（センサ状態観測部）６９とを備える。パーティクル撒布部６３、パーティクル移動部６５、重み算出部６７、センサ位置観測部６９の詳細については、後述する。

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

ここで、形状修正部５０で、線形形状検出部４０により検出された検出線形形状７１を修正する方法について説明する。図４に示すように、形状修正部５０は、パーティクルフィルタによりそれぞれのセンサユニットＳ_ｉの位置を補正し、線形形状検出部４０により検出された検出線形形状７１を修正する（ステップＳ１０６）。前述のように、センサ間寸法ｌが５０ｍｍ程度より小さい範囲であれば、それぞれのセンサユニットＳ_ｉの間を寸法がセンサ間寸法ｌに等しい直線状のリンクＴ_ｊと仮定した場合でも、挿入部１１の形状検出の際の誤差は小さくなる。しかし実際は、それぞれのセンサユニットＳ_ｉの計測データはノイズ等により誤差を有し、また、体腔内に挿入された際の内視鏡１０の挿入部１１は曲線形状を呈している。このため、挿入部１１の形状は基端側のセンサユニットＳ_ｊ−１の中心から先端側のセンサユニットＳ_ｊの中心まで基端側のセンサユニットＳ_ｊ−１の中心での長手方向に延設される直線状のリンク形状とは限らない。したがって、検出線形形状７１の形状修正を行うことが重要となる。

それぞれのセンサユニットＳ_ｉの位置を補正し検出線形形状７１を修正する際、形状修正順次実施部５１は、それぞれのリンクＴ_ｊごとに基端側（グローバル座標系Ｃの原点から近い側）のリンクＴ_ｊから順に形状修正を行い、修正リンクＴ´_ｊを形成する。ここで、形状修正順次実施部５１により、リンクＴ_ｊの形状修正を行う方法について説明する。ここでは、基端側からｋ番目のセンサユニットＳ_ｋ−１と基端側から（ｋ＋１）番目のセンサユニットＳ_ｋとの間の基端側からｋ番目のリンクＴ_ｋの形状修正について説明する。

図９は、形状修正部５０により検出線形形状７１の形状を修正する方法を示すフローチャートである。図１０乃至図１５は、形状修正順次実施部５１での処理を説明する図である。図１０に示すように、形状修正順次実施部５１により、リンクＴ_ｋの形状修正が行われる前の状態では、リンクＴ_ｋ−１まで形状修正が完了し、修正完了部７７が形成されている。修正完了部７７は、修正リンクＴ´_１〜Ｔ´_ｋ−１から構成されている。また、リンクＴ_ｋ〜Ｔ_Ｎから構成される修正未完了部７９が形成されている。この際、修正完了部７７と修正未完了部７９との境界は連続し、センサユニットＳ_ｋはグローバル座標系Ｃで点Ｑ_ｋ＿０に位置している。

この状態で、形状修正順次実施部５１により、修正未完了部７９の中で最も基端側に位置するリンク（修正対象リンク）Ｔ_ｋの形状修正が開始される(ステップＳ１１１)。すなわち、形状修正制御部５２により、形状修正順次実施部５１がリンクＴ_ｋの形状修正を行う状態に制御される。

形状修正順次実施部５１によりリンクＴ_ｋの形状修正を行う際にはまず、センサ位置推定部６１によりリンクＴ_ｋの先端側のセンサユニット（状態補正対象センサ）Ｓ_ｋの位置を、パーティクルフィルタを用いて、リンクＴ_ｋの先端に位置する初期位置（Ｑ_ｋ＿０）に基づいて推定する。図９及び図１０に示すように、センサユニットＳ_ｋの位置を初期位置に基づいて推定する際は、まず、パーティクル撒布部６３によりリンクＴ_ｋの先端側のセンサユニットＳ_ｋの初期位置Ｑ_ｋ＿０の周辺に所定の数（本実施形態ではＡ個とする）のパーティクル７２を撒く（ステップＳ１１２）。ここで、センサユニットＳ_ｋの初期位置（すなわち、パーティクルフィルタにおける状態ベクトル）をＱ_ｋ＿０（ｑ_ｘｋ＿０，ｑ_ｙｋ＿０，ｑ_ｚｋ＿０）とし、初期位置Ｑ_ｋ＿０の誤差の分散を（δ_ｘ，δ_ｙ，δ_ｚ）とする。δ_ｘ，δ_ｙ，δ_ｚを求める際は、ノイズ等により生じるそれぞれのセンサユニットＳ_ｉの計測データの誤差の分散から、式（１）〜式（１５）を用いてセンサユニットＳ_ｋ−１の姿勢角α_ｋ−１，β_ｋ−１，γ_ｋ−１の誤差の分散を求める。そして、姿勢角α_ｋ−１，β_ｋ−１，γ_ｋ−１の誤差の分散から式（１６）を用いて、センサユニットＳ_ｋの初期位置Ｑ_ｋ＿０の誤差の分散δ_ｘ，δ_ｙ，δ_ｚ求める。パーティクル７２は、センサユニットＳ_ｋの初期位置Ｑ_ｋ＿０に誤差の分散δ_ｘ，δ_ｙ，δ_ｚを加えた範囲に一様に撒かれる。すなわち、それぞれのパーティクル７２のグローバル座標系Ｃでの位置をＱ_ｋ＿１ ^ａ（ｑ_ｘｋ＿１ ^ａ，ｑ_ｙｋ＿１ ^ａ，ｑ_ｚｋ＿１ ^ａ）（ａ＝１，２，…，Ａ）とすると、

の範囲でパーティクル７２が一様に撒かれる。すなわち、システムの雑音をホワイトノイズとして仮定している。なお、パーティクルの撒布範囲は経験的に決定されてもよいが、撒布範囲が大きすぎる場合は、推定精度が劣化する。それぞれのパーティクル７２が最初に撒かれる際には、それぞれのパーティクル７２に初期の重みｗ_ｋ＿０ ^ａがあらかじめ与える。重みｗ_ｋ＿０ ^ａは、例えばそれぞれのパーティクル７２で一定に１／Ａで与えられる。ここで、パーティクル７２は、状態ベクトルの確率密度関数を状態空間のＡ個の離散的な仮説で表現したものであり、それぞれのパーティクル７２が確率に比例する重みｗ_ｋ＿０ ^ａを有している。すなわち、パーティクルフィルタでは、複数（本実施形態ではＡ個）のパーティクル７２を用いて、状態ベクトルの確率密度関数を推定する。

そして、パーティクル移動部６５が挿入部１１の状態（移動状態、曲がり状態等）及びセンサユニットＳ_ｋでの計測データに基づいてそれぞれのパーティクル７２の移動モデルを検出する。そして、パーティクル移動部６５は、重みｗ_ｋ＿０ ^ａに比例する確率からパーティクル７２を選択し、検出された移動モデルに基づいてそれぞれのパーティクル７２を移動させる（ステップＳ１１３）。パーティクル移動部６５により、それぞれのパーティクル７２は移動前パーティクル位置から移動後パーティクル位置へ移動する。ただし、本実施形態では、挿入部１１が移動しない静状態であり、かつ、それぞれのセンサユニットＳ_ｉの間の曲線補間は行わないため、パーティクル７２の移動は行われない。

そして、重み算出部６７によりそれぞれのパーティクル７２の重みｗ_ｋ＿１ ^ａを算出する（ステップＳ１１４）。すなわち、それぞれのパーティクル７２の重みが、ｗ_ｋ＿０ ^ａからｗ_ｋ＿１ ^ａに更新される。重み算出部６７は、まず、それぞれのパーティクル７２の尤もらしさを評価する尤度ｖ_ｋ＿１ ^ａを算出する。尤度により、センサユニットＳ_ｋがそれぞれのパーティクル７２のパーティクル移動部６５により移動された移動後パーティクル位置に位置する尤もらしさが示される。尤度は、

となる。ｂ_ｋ＿１ ^ａはグローバル座標系Ｃにおいて、位置補正後のセンサユニットＳ_ｋ−１とそれぞれのパーティクル７２との間の線分の寸法である。例えば図１１に示すように、センサユニットＳ_ｋ−１とパーティクル７２Ａ（グローバル座標系Ｃでの位置がＱ_ｋ＿１ ^ｓのパーティクル）との間の線分の寸法は、ｂ_ｋ＿１ ^ｓとなる。ここで関数ｈ（ｂ_ｋ＿１ ^ａ）は例えば、

とする。ｂ_０はそれぞれのセンサユニットＳ_ｉのセンサ間寸法ｌとする。式（１９）より、位置補正後のセンサユニットＳ_ｋ−１とパーティクル７２との間の線分の寸法と、センサ間寸法ｌとの差が小さいほど関数ｈ（ｂ_ｋ＿１ ^ａ）は大きくなり、尤度が大きくなる。逆に、位置補正後のセンサユニットＳ_ｋ−１とパーティクル７２との間の線分の寸法と、センサ間寸法ｌとの差が大きいほど関数ｈ（ｂ_ｋ＿１ ^ａ）は小さくなり、尤度が小さくなる。φ_ｋ＿１ ^ａは、位置補正後のセンサユニットＳ_ｋ−１からそれぞれのパーティクル７２までのベクトルＶ１と、それぞれのパーティクル７２から位置補正前のセンサユニットＳ_ｋ＋１までのベクトルＶ２との成す角度である。例えば図１１に示すように、パーティクル７２Ａ（グローバル座標系Ｃでの位置がＱ_ｋ＿１ ^ｓのパーティクル）では、ベクトルＶ１とベクトルＶ２との成す角度は、φ_ｋ＿１ ^ｓとなる。ここで関数ｇ（φ_ｋ＿１ ^ａ）は例えば、

とする。ここで、ｃは定数であり、例えば４である。式（２０）よりベクトルＶ１とＶ２の成す角度が０度〜±９０度の範囲であれば、関数ｇ（φ_ｋ＿１ ^ａ）は大きくなり、尤度が大きくなる。逆に、それ以外の角度であれば、関数ｇ（φ_ｋ＿１ ^ａ）は小さくなり、尤度が小さくなる。それぞれのセンサユニットＳ_ｉのセンサ間寸法ｌを５０ｍｍとした場合、実際に角度φ_ｋ＿１ ^ａは０度〜略±９０度の範囲を取り得る。このことは、内視鏡１０の挿入部１１と径が略同一の径が１０ｍｍの樹脂製チューブを曲げた状態から、容易に想到される。角度φ_ｋ＿１ ^ａの範囲は、製品、挿入部の位置によって異なるため、製品および／または挿入部位置に対応して調整されることが望ましい。

そして、それぞれのパーティクル７２の尤度から、重みｗ_ｋ＿１ ^ａを算出する。それぞれのパーティクル７２の重みは、

となる。すなわち、重みはそれぞれのパーティクル７２の尤度を、尤度の合計を１として規格化した値である。以上のようにして、それぞれのパーティクル７２の重みが算出される。

そして、センサ位置観測部６９が、重み算出部６７で算出されたそれぞれのパーティクル７２の重みに基づいて、センサユニットＳ_ｋの位置を観測する（ステップＳ１１５）。この際、センサ位置観測部６９は、図１２に示すように、それぞれのパーティクル７２のパーティクル移動部６５により移動される前の移動前パーティクル位置の重み付き平均を算出し、算出された位置を移動前センサ位置Ｑ´_ｋ＿１とする。そして、パーティクル移動部６５がそれぞれのパーティクル７２を移動させる際に用いた移動モデルにより、移動前センサ位置Ｑ´_ｋ＿１から移動後センサ位置Ｑ_ｋ＿１への移動を行い、移動後センサ位置Ｑ_ｋ＿１をセンサユニットＳ_ｋの位置として推定する。なお、本実施形態では、パーティクル移動部６５によりパーティクル７２の移動は行われないため、移動前センサ位置Ｑ´_ｋ＿１と移動後センサ位置Ｑ_ｋ＿１との位置は同一である。

そして、位置推定制御部６２により、所定の回数（本実施形態ではＭ回とする）だけセンサユニットＳ_ｋの位置推定が行われたかを判断する（ステップＳ１１６）。現状態では、センサユニットＳ_ｋの位置推定が１回しか行われていないため（ステップＳ１１６−Ｎｏ）、センサ位置推定部６１は位置推定制御部６２により、センサユニットＳ_ｋの位置推定をもう１回行う状態に制御される。この際、センサ位置推定部６１は、１回目の推定結果に基づいて、センサユニットＳ_ｋの位置推定を行う。同様に、例えばｔ回目の位置推定では、（ｔ−１）回目の推定結果に基づいて、センサユニットＳ_ｋの位置推定を行う。すなわち、２回目以降の位置推定では、センサ位置推定部６１は、前回の位置推定での推定結果に基づいて、センサユニットＳ_ｋの位置推定を行う。

そして、ステップＳ１１２に戻り、パーティクル撒布部６３により所定の数（本実施形態ではＡ個とする）のパーティクル７２を撒く（ステップＳ１１２）。２回目の位置推定では、１回目の位置推定の際に重み算出部６７で算出されたそれぞれのパーティクル７２の重みに基づいて、パーティクル７２が撒かれる。同様に例えばｔ回目の位置推定では、（ｔ−１）回目の位置推定の際に重み算出部６７で算出されたそれぞれのパーティクル７２の重みに基づいてパーティクル７２が撒かれる。すなわち、２回目以降の位置推定では、パーティクル撒布部６３は、前回の位置推定の際に重み算出部６７で算出されたそれぞれのパーティクル７２の重みに基づいてパーティクル７２を撒く。すなわち、パーティクル撒布部６３は、前回の位置推定の際のそれぞれのパーティクル７２の重みに基づいてパーティクル７２を撒く重み対応撒布部となっている。結果的に、図１３Ａ及び図１３Ｂに示すように、例えば、前回の位置推定の際に算出された重みが大きいパーティクル７２の移動前パーティクル位置の周辺（図１３Ａ，図１３Ｂ中のＧ１で示す範囲）では、撒くパーティクル７２の数を多くする。一方、前回の位置推定の際に算出された重みが小さいパーティクル７２の移動前パーティクル位置の周辺（図１３Ａ，図１３Ｂ中のＧ２で示す範囲）では、撒くパーティクル７２の数を少なくする。なお、図１３Ａは前回の位置推定の際に撒かれたパーティクル７２のパーティクル移動部６５により移動される前の移動前パーティクル位置を示し、図１３Ｂは現在のパーティクルの移動前パーティクル位置を示している。

そして、２回目以降の位置推定でも同様に、パーティクル移動部６５が、１回目の推定で用いた移動モデルに基づいて、それぞれのパーティクル７２を移動させる（ステップＳ１１３）。これにより、それぞれのパーティクル７２は、移動前パーティクル位置から移動後パーティクル位置へ移動する。

そして、重み算出部６７により、式（１８）〜式（２０）と同様の式を用いて、それぞれのパーティクル７２の尤もらしさを評価する尤度を算出する。尤度により、センサユニットＳ_ｋがそれぞれのパーティクル７２のパーティクル移動部６５により移動された移動後パーティクル位置に位置する尤もらしさが示される。そして、式（２１）と同様の式を用いて、尤度からそれぞれのパーティクル７２の重みが算出される（ステップＳ１１４）。ここで、例えばｔ回目の位置推定では、式（１８）〜式（２１）の尤度ｖ_ｋ＿１ ^ａ，重みｗ_ｋ＿１ ^ａ，ｂ_ｋ＿１ ^ａ，φ_ｋ＿１ ^ａの代わりに、それぞれ尤度ｖ_ｋ＿ｔ ^ａ，重みｗ_ｋ＿ｔ ^ａ，ｂ_ｋ＿ｔ ^ａ，φ_ｋ＿ｔ ^ａを用いて計算する。

そして、センサ位置観測部６９が、重み算出部６７で算出されたそれぞれのパーティクル７２の重みに基づいて、センサユニットＳ_ｋの位置を推定する（ステップＳ１１５）。この際、センサ位置観測部６９は、それぞれのパーティクル７２のパーティクル移動部６５により移動される前の移動前パーティクル位置の重み付き平均を算出し、算出された位置を移動前センサ位置とする。そして、パーティクル移動部６５がそれぞれのパーティクル７２を移動させる際に用いた移動モデルにより、移動前センサ位置から移動後センサ位置への移動を行い、移動後センサ位置をセンサユニットＳ_ｋの位置として推定する。例えば、ｔ回目の位置推定では、それぞれのパーティクル７２の重み付平均から移動前センサ位置Ｑ´_ｋ＿ｔが算出され、移動モデルでの移動により移動前センサ位置Ｑ´_ｋ＿ｔから移動後センサ位置Ｑ_ｋ＿ｔへ移動する。

そして、位置推定制御部６２により、所定の回数（本実施形態ではＭ回とする）だけセンサユニットＳ_ｋの位置推定が行われたかを判断する（ステップＳ１１６）。センサユニットＳ_ｋの位置推定が所定の回数だけ行われていない場合は（ステップＳ１１６−Ｎｏ）、センサ位置推定部６１は位置推定制御部６２により、センサユニットＳ_ｋの位置推定をもう１回行う状態に制御され、ステップＳ１１２〜ステップＳ１１５をもう１回行う。センサユニットＳ_ｋの位置推定が所定の回数だけ行われている場合は（ステップＳ１１６−Ｙｅｓ）、次のステップに進む。

図１４に示すように、センサユニットＳ_ｋの位置推定を所定の回数だけ行った後、センサ位置補正部５５は、センサ位置推定部６１での推定結果に基づいて、修正対象のリンク（修正対象リンク）Ｔ_ｋの先端側のセンサユニット（状態補正対象センサ）Ｓ_ｋの位置を補正する（ステップＳ１１７）。センサ位置補正部５５は、センサユニットＳ_ｋの位置を、リンクＴ_ｋの先端に位置する初期位置（Ｑ_ｋ＿０）からセンサ位置観測部６１の最終回（本実施形態ではＭ回目）の位置推定の際に推定されたセンサユニットＳ_ｋの移動後センサ位置Ｑ_ｋ＿Ｍへと、補正する。また、修正リンク形成部５７が、センサ位置補正部５５により補正されたセンサユニットＳ_ｋの位置に基づいて修正リンクＴ´_ｋを形成する（ステップＳ１１７）。ここでは、位置補正後のセンサユニットＳ_ｋ−１とセンサユニットＳ_ｋの移動後センサ位置Ｑ_ｋ＿Ｍとを結ぶ線分が、修正リンクＴ´_ｋとなる。センサユニットＳ_ｋの位置の移動により、形状修正順次実施部５１によりリンクＴ_ｋの形状修正が行われる前の状態では連続していた修正完了部７７と修正未完了部７９との境界が、不連続な状態となる。すなわち、修正リンクＴ´_ｋとリンクＴ_ｋ＋１が不連続な状態となる。

なお、リンクＴ_ｋ以外のリンクＴ_ｊについても、形状修正順次実施部５１により同様に形状修正が行われる。すなわち、リンク（修正対象リンク）Ｔ_ｊの形状修正が開始されると（ステップＳ１１１）、センサ位置推定部６１が、パーティクルフィルタを用いて、修正対象のリンクＴ_ｊの先端側（グローバル座標Ｃの原点から遠い側）のセンサユニット（状態補正対象センサ）Ｓ_ｊの位置の推定を行う。センサユニットＳ_ｊの位置の推定を行う際は、まず、パーティクル撒布部６３がセンサユニットＳ_ｊの初期位置Ｑ_ｊ＿０、又は、前回の位置推定の際に重み算出部６７で算出されたそれぞれのパーティクル７２の重みに基づいて所定の数のパーティクル７２を撒く（ステップＳ１１２）。そして、パーティクル移動部６５が、上述した移動モデルを検出し、移動モデルに基づいて、それぞれのパーティクル７２を移動させる（ステップＳ１１３）。これにより、それぞれのパーティクル７２は、移動前パーティクル位置から移動後パーティクル位置へ移動する。

そして、重み算出部６７により、式（１８）〜式（２０）と同様の式を用いて、それぞれのパーティクル７２の尤もらしさを評価する尤度を算出する。尤度により、センサユニットＳ_ｊがそれぞれのパーティクル７２のパーティクル移動部６５により移動された移動後パーティクル位置に位置する尤もらしさが検出される。そして、式（２１）と同様の式を用いて、尤度からそれぞれのパーティクル７２の重みが算出される（ステップＳ１１４）。ここで、リンクＴ_ｊの形状修正が行われているので、例えばｔ回目の位置推定では、式（１８）〜式（２１）の尤度ｖ_ｋ＿１ ^ａ，重みｗ_ｋ＿１ ^ａ，ｂ_ｋ＿１ ^ａ，φ_ｋ＿１ ^ａの代わりに、それぞれ尤度ｖ_ｊ＿ｔ ^ａ，重みｗ_ｊ＿ｔ ^ａ，ｂ_ｊ＿ｔ ^ａ，φ_ｊ＿ｔ ^ａを用いて計算する。そして、センサ位置観測部６９が、重み算出部６７で算出されたそれぞれのパーティクル７２の重みに基づいて、センサユニットＳ_ｊの位置を観測する（ステップＳ１１５）。例えば、ｔ回目の位置推定では、それぞれのパーティクル７２の重み付平均から移動前センサ位置Ｑ´_ｊ＿ｔが算出され、移動モデルでの移動により移動前センサ位置Ｑ´_ｊ＿ｔから移動後センサ位置Ｑ_ｊ＿ｔへの移動が行われる。そして、移動後センサ位置Ｑ_ｊ＿ｔが、センサユニットＳ_ｊの位置として推定される。

そして、位置推定制御部６２により、所定の回数（本実施形態ではＭ回とする）だけセンサユニットＳ_ｊの位置推定が行われたかを判断する（ステップＳ１１６）。センサユニットＳ_ｊの位置推定が所定の回数だけ行われていない場合は（ステップＳ１１６−Ｎｏ）、センサ位置推定部６１は位置推定制御部６２により、センサユニットＳ_ｊの位置推定を所定の回数だけ行う状態に制御され、ステップＳ１１２〜ステップＳ１１５をもう１回行う。センサユニットＳ_ｊの位置推定が所定の回数だけ行われている場合は（ステップＳ１１６−Ｙｅｓ）、次のステップに進む。

センサ位置推定部６１によりセンサユニットＳ_ｊの位置推定が所定の回数だけ行った後、センサ位置補正部５５は、センサ位置推定部６１の推定結果に基づいて、修正対象のリンク（修正対象リンク）Ｔ_ｊの先端側（グローバル座標系Ｃの原点から遠い側）のセンサユニット（状態補正対象センサ）Ｓ_ｊの位置を補正する（ステップＳ１１７）。センサ位置補正部５５は、センサユニットＳ_ｊの位置を、リンクＴ_ｊの先端（グローバル座標系の遠い側の端）に位置する初期位置（Ｑ_ｊ＿０）からセンサ位置観測部６１の最終回（本実施形態ではＭ回目）の位置推定の際に推定されたセンサユニットＳ_ｊの移動後センサ位置Ｑ_ｊ＿Ｍへと、補正する。また、修正リンク形成部５７が、センサ位置補正部５５により補正されたセンサユニットＳ_ｊの位置に基づいて修正リンクＴ´_ｊを形成する（ステップＳ１１７）。ここでは、位置補正後のセンサユニットＳ_ｊ−１とセンサユニットＳ_ｊの移動後センサ位置Ｑ_ｊ＿Ｍとを結ぶ線分が、修正リンクＴ´_ｊとなる。

図１５は、未修正リンク位置補正部５３での処理を説明する図である。図１５に示すように、形状修正順次実施部５１により、リンクＴ_ｋの形状修正が行われ、修正リンクＴ´_ｋが形成された後の状態では、形状修正が完了した修正完了部７７と、形状修正が完了していない修正未完了部７９とが形成されている。修正完了部７７は修正リンクＴ´_１〜Ｔ´_ｋから構成され、修正未完了部７９はリンクＴ_ｋ＋１〜Ｔ_Ｎから構成されている。この際、前述のようにリンクＴ_ｋの形状修正によりセンサユニットＳ_ｋの位置が点Ｑ_ｋ＿０から点Ｑ_ｋ＿Ｍに移動するため、修正完了部７７と修正未完了部７９との境界は、不連続となっている。この状態で、未修正リンク位置補正部５３が、修正未完了部７９を修正完了部７７と境界が連続する状態に平行移動して、修正未完了部７９の位置を補正する（ステップＳ１１８）。すなわち、図１５の点線で示す位置から、実線で示す位置に修正未完了部７９が平行移動される。

なお、リンクＴ_ｋ以外のリンクＴ_ｊについても、同様に未修正リンク位置補正部５３により修正未完了部７９の位置が補正される。すなわち、形状修正順次実施部５１によりリンクＴ_ｊの形状修正が行われた後、修正完了部７７と修正未完了部７９との境界は不連続となっている。この状態で、未修正リンク位置補正部５３が、修正未完了部７９を修正完了部７７と連続する状態に平行移動して、修正未完了部７９の位置を補正する（ステップＳ１１８）。未修正リンク位置補正部５３は、形状修正順次実施部５１により１つのリンクＴ_ｊの形状修正が行われるたびに、修正未完了部７９の位置を補正する。

そして、図９に示すように、形状修正制御部５２が、すべてのリンクＴ_ｊについて形状修正が完了したか確認する（ステップＳ１１９）。すべてのリンクＴ_ｊについて形状修正が完了している場合は、挿入部１１の修正形状７５が形成され、次のステップへ進む（ステップＳ１１９−Ｙｅｓ）。すべてのリンクＴ_ｊについて形状修正が完了していない場合は、ステップＳ１１１に戻り（ステップＳ１１９−Ｎｏ）、形状修正順次実施部５１により、修正未完了部７９の中で最も基端側（グローバル座標系Ｃの原点から近い側）に位置するリンク（修正対象リンク）Ｔ_ｊの形状修正が行われる。すなわち、ステップＳ１１１〜Ｓ１１８が、すべてのリンクＴ_ｊについて形状修正が完了するまで繰り返し行われる。

そこで、上記構成の内視鏡形状検出装置１及び内視鏡形状検出装置１を用いた内視鏡１０の挿入部１１の形状検出方法では、以下の効果を奏する。すなわち、内視鏡形状検出装置１では、それぞれのセンサユニットＳ_ｉの計測データから姿勢検出部３０がセンサユニットＳ_ｉの姿勢を検出し、線形形状検出部４０がそれぞれのセンサユニットＳ_ｉの姿勢から内視鏡１０の挿入部１１の検出線形形状７１を検出する。そして、形状修正部５０が、パーティクルフィルタによりそれぞれのセンサユニットＳ_ｉの位置を補正し、線形形状検出部４０により検出された検出線形形状７１を修正する。そして、挿入部１１の修正形状７５が形成される。以上のように、観察時に体腔内に挿入される挿入部１１に配置されるセンサユニットＳ_ｉの計測データから、挿入部１１の修正形状７５が検出されるため、体外にセンスコイル等を設ける必要がない。このため、内視鏡形状検出装置１の小型化、単純化を実現することができる。

また、内視鏡形状検出装置１では、形状修正部５０により、パーティクルフィルタを用いてそれぞれのセンサユニットＳ_ｉの位置を補正し、検出線形形状７１を修正している。形状修正部５０では、形状修正順次実施部５１のセンサ位置推定部６１によりそれぞれのセンサユニットＳ_ｉの推定を所定の回数だけ行う。センサユニットＳ_ｉの推定の際、重み算出部６７によりそれぞれのパーティクル７２の重みを算出する。そして、センサ位置観測部６９が、重み算出部６７で算出されたそれぞれのパーティクル７２の重みに基づいて、センサユニットＳ_ｉの位置を観測する。算出されるパーティクル７２の重みが大きいほど、パーティクル移動部６５により移動されたそれぞれのパーティクル７２の移動後パーティクル位置が、体腔内に挿入される挿入部１１でのセンサユニットＳ_ｉの尤もらしい位置（位置する確率が高い位置）となる。このため、センサ位置観測部６９により、高い精度でセンサユニットＳ_ｉの位置が観測される。センサ位置補正部５５は、センサ位置推定部６１での最終回の推定でセンサ位置観測部６９により観測された位置である移動後センサ位置に、それぞれのセンサユニットＳ_ｉの位置を補正する。このため、位置補正後のセンサユニットＳ_ｉの位置は、体腔内に挿入される挿入部１１でのセンサユニットＳ_ｉの尤もらしい位置（位置する確率が高い位置）となる。形状修正部５０は、位置補正後のセンサユニットＳ_ｉの位置に基づいて、検出線形形状７１の修正を行うため、実際の挿入部１１の形状と誤差の少ない修正形状７５を検出することができる。これにより、高い精度で挿入部１１の修正形状７５を検出することができる。

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

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

図１６は、本実施形態の形状修正部１５０の構成を示す図である。形状修正部１５０は、パーティクルフィルタによりそれぞれのセンサユニットＳ_ｉの位置（状態）を補正し、線形形状検出部４０により検出された検出線形形状７１を修正する。図１７は、形状修正部５０により修正された内視鏡１０の挿入部１１の修正形状１７５をグローバル座標ＣのＺ軸の正の方向から見た図である。図１７に示すように、形状修正部１５０により、図１７の点線で示す検出線形形状７１が修正形状１７５に修正される。それぞれのセンサユニットＳ_ｉの位置を補正することにより、それぞれのリンクＴ_ｊの形状が修正され円弧（修正センサ間要素）Ｌ_ｊが決定される。修正形状１７５は、それぞれの円弧Ｌ_ｊから構成されている。実際に、体腔内に挿入された際の内視鏡１０の挿入部１１の形状は曲線形状である。ここで、内視鏡１０の挿入部１１は、製品により差はあるが、適度の弾性を有する。このため、挿入部１１の曲線形状の曲率が大きく変化することは、稀である。したがって、センサユニットＳ_ｉの間の形状を所定の半径（曲率）の円弧Ｌ_ｊと仮定して形状修正を行うことにより、実際の挿入部１１の曲線形状と誤差の少ない修正形状１７５が形成される。

図１６に示すように、形状修正部１５０は、形状修正順次実施部１５１と、形状修正制御部１５２と、未修正リンク位置補正部１５３を備える。形状修正順次実施部１５１は、それぞれのリンクＴ_ｊごとに基端側（グローバル座標系Ｃの原点から近い側）のリンクＴ_ｊから順に形状修正を行い、円弧（修正センサ間要素）Ｌ_ｊを形成する。すなわち、第１の実施形態の形状修正順次実施部５１が修正リンクＴ´_ｊを形成するのに対し、本実施形態の形状修正順次実施部１５１は円弧Ｌ_ｊを形成する。形状修正制御部１５２と、未修正リンク位置補正部１５３については、第１の実施形態の、形状修正制御部５２と、未修正リンク位置補正部５３と同一の構成及び機能であるため、その説明を省略する。

形状修正順次実施部１５１は、修正対象のリンク（修正対象リンク）Ｔ_ｊの先端側（グローバル座標系Ｃの原点から遠い側）のセンサユニット（状態補正対象センサ）Ｓ_ｊの位置を補正するセンサ位置補正部（センサ状態補正部）１５５と、センサ位置補正部１５５により補正されたセンサユニットＳ_ｊの位置に基づいて円弧Ｌ_ｊを形成する円弧形成部（修正センサ間要素形成部）１５７とを備える。すなわち、第１の実施形態の修正リンク形成部５７が位置補正後のセンサユニットＳ_ｊの位置に基づいて修正リンクＴ´_ｊを形成するのに対し、本実施形態の円弧形成部１５７は円弧Ｌ_ｊを形成する。

形状修正順次実施部１５１は、パーティクルフィルタを用いて、修正対象のリンクＴ_ｊの先端側のセンサユニットＳ_ｊの位置の推定を行うセンサ位置推定部（センサ状態推定部）１６１を備える。センサ位置推定部１６１は、リンクＴ_ｊの先端に位置する初期位置に基づいて、１回目のセンサユニットＳ_ｊの位置推定を行う。また、センサ位置推定部１６１は、１回目の推定結果に基づいて、２回目のセンサユニットＳ_ｊの位置推定観測を行う。すなわち、センサ位置推定部１６１は、パーティクルフィルタを用いて、初期位置又は前回の推定の推定結果に基づいてセンサユニットＳ_ｊの位置推定を行う。また、形状修正順次実施部１５１は、センサユニットＳ_ｊの位置推定を所定の回数だけ行う状態にセンサ位置推定部１６１を制御する位置推定制御部（状態推定制御部）１６２を備える。

センサ位置推定部１６１は、パーティクル撒布部１６３と、パーティクル移動部である曲線補間部１６５と、重み算出部１６７と、センサ位置観測部（センサ状態観測部）１６９とを備える。パーティクル撒布部１６３、曲線補間部１６５、重み算出部１６７、センサ位置観測部１６９の詳細については、後述する。

ここで、形状修正部１５０で、線形形状検出部４０により検出された検出線形形状７１を修正する方法について説明する。図４に示すように、形状修正部１５０は、パーティクルフィルタによりそれぞれのセンサユニットＳ_ｉの位置を補正し、線形形状検出部４０により検出された検出線形形状７１を修正する（ステップＳ１０６）。

それぞれのセンサユニットＳ_ｉの位置を補正し検出線形形状７１を修正する際、形状修正順次実施部１５１は、それぞれのリンクＴ_ｊごとに基端側（グローバル座標系Ｃの原点から近い側）のリンクＴ_ｊから順に形状修正を行い、円弧Ｌ_ｊを形成する。ここで、形状修正順次実施部５１により、リンクＴ_ｊの形状修正を行う方法について説明する。ここでは、基端側からｋ番目のセンサユニットＳ_ｋ−１と基端側から（ｋ＋１）番目のセンサユニットＳ_ｋとの間の基端側からｋ番目のリンクＴ_ｋの形状修正について説明する。

図１８は、形状修正部１５０により検出線形形状７１の形状を修正する方法を示すフローチャートである。図１９乃至図２１は、形状修正順次実施部１５１での処理を説明する図である。図１９に示すように、形状修正順次実施部１５１により、リンクＴ_ｋの形状修正が行われる前の状態では、第１の実施形態と同様に、リンクＴ_ｋ−１まで形状修正が完了し、修正完了部１７７が形成されている。また、リンクＴ_ｋ〜Ｔ_Ｎから構成される修正未完了部１７９が形成されている。この際、修正完了部１７７と修正未完了部１７９との境界は連続し、センサユニットＳ_ｋはグローバル座標系Ｃで点Ｕ_ｋ＿０に位置している。

図１８及び図１９に示すように、この状態で、形状修正順次実施部１５１により、修正未完了部１７９の中で最も基端側に位置するリンク（修正対象リンク）Ｔ_ｋの形状修正が開始される(ステップＳ１９１)。すなわち、形状修正制御部１５２により、形状修正順次実施部１５１がリンクＴ_ｋの形状修正を行う状態に制御される。

形状修正順次実施部５１によりリンクＴ_ｋの形状修正を行う際には、まず、センサ位置推定部１６１によりリンクＴ_ｋの先端側のセンサユニット（状態補正対象センサ）Ｓ_ｋの位置を、パーティクルフィルタを用いて、リンクＴ_ｋの先端に位置する初期位置（Ｕ_ｋ＿０）に基づいて推定する。図１８及び図１９に示すように、センサユニットＳ_ｋの位置を初期位置に基づいて推定する際は、まず、パーティクル撒布部１６３によりリンクＴ_ｋの先端側のセンサユニットＳ_ｋの初期位置Ｕ_ｋ＿０の周辺に所定の数（本実施形態ではＡ個とする）のパーティクル１７２を撒く（ステップＳ１９２）。ここで、それぞれのパーティクル１７２は、第１の実施形態と同様に式（１７）の範囲で一様に撒かれる。

そして、曲線補間部１６５により、センサユニットＳ_ｋ−１とパーティクル撒布部１６３により撒かれたそれぞれのパーティクル１７２との間の曲線補間を行う（ステップＳ１９３）。曲線補間部１６５は、挿入部１１の曲がり状態に基づいた移動モデルを用いて曲線補間を行い、それぞれのパーティクル１７２の位置を移動させる。すなわち、曲線補間部１６５は、挿入部１１の曲がり状態に基づいた移動モデルを用いてそれぞれのパーティクル１７２を移動させるパーティクル移動部である。曲線補間部１６５により、それぞれのパーティクル１７２は補間前パーティクル位置（移動前パーティクル位置）から補間後パーティクル位置（移動後パーティクル位置）へ移動する。

ここでは、センサユニットＳ_ｋ−１とパーティクル１７２の中の１つであるパーティクル１７２Ａとの間の曲線補間について説明する。図１９に示すように、センサユニットＳ_ｋ−１とパーティクル１７２Ａとの間の曲線補間リンクＴ_ｋの曲線補間を行う前の状態では、パーティクル１７２Ａは補間前パーティクル位置Ｆ１に位置している。ここで、センサユニットＳ_ｋ−１とパーティクル１７２Ａの補間前パーティクル位置Ｆ１とを結ぶ線分を第１の線形形状１７１Ａとする。第１の線形形状１７１Ａの寸法は、ｂ´である。また、パーティクル１７２Ａの補間前パーティクル位置Ｆ１とリンクＴ_ｋの先端に位置するセンサユニットＳ_ｋ＋１と結ぶ線分を第２の線形形状１７１Ｂとする。そして、第１の線形形状１７１Ａの基端を通り、第１の線形形状１７１Ａに垂直な第１の補間法線１７４Ａと、第１の線形形状１７１Ａの先端を通り、第２の線形形状１７１Ｂに垂直な第２の補間法線１７４Ｂとを形成する。そして、第１の補間法線１７４Ａと第２の補間法線１７４Ｂとの交点Ｏを算出する。第１の補間法線１７４Ａと第２の補間法線１７４Ｂとが交わらない場合は、第１の補間法線１７４Ａと第２の補間法線１７４Ｂとの距離が最小となる２点の中間点Ｏを算出する。そして、交点Ｏ又は中間点Ｏを中心Ｏとして円弧１７６を形成する。円弧１７６は、中心Ｏと第１の線形形状１７１Ａの基端との間の距離と同一の半径Ｒを有する。すなわち、円弧１７６は曲率１／Ｒを有する。また、円弧１７６は、第１の線形形状１７１Ａの基端を始点とする弧の長さが第１の線形形状１７１Ａの寸法ｂ´に等しい円弧１７６である。円弧１７６の先端Ｆ２が、パーティクル１７２Ａの補間後パーティクル位置Ｆ２となる。すなわち、曲線補間部１６５での曲線補間により、パーティクル１７２Ａの位置が補間前パーティクル位置Ｆ１から補間後パーティクル位置Ｆ２に移動する。その他のパーティクルについても、同様にして補間前パーティクル位置から補間後パーティクル位置へ移動される。

そして、第１の実施形態と同様に、重み算出部１６７によりそれぞれのパーティクル１７２の重みｗ_ｋ＿１ ^ａを算出する（ステップＳ１９４）。すなわち、それぞれのパーティクル７２の重みが、初期のｗ_ｋ＿０ ^ａからｗ_ｋ＿１ ^ａに更新される。重み算出部1６７は、まず、それぞれのパーティクル１７２の補間後パーティクル位置（移動後パーティクル位置）の尤もらしさを評価する尤度ｖ_ｋ＿１ ^ａを算出する。尤度は、式（１８）〜式（２０）を用いて算出される。ただし、本実施形態では、ｂ_ｋ＿１ ^ａはグローバル座標系Ｃにおいて、位置補正後のセンサユニットＳ_ｋ−１とそれぞれのパーティクル１７２の補間後パーティクル位置との間の円弧の弧長である。例えば図２０に示すように、センサユニットＳ_ｋ−１とパーティクル１７２Ａ（このパーティクルでは、ａ＝ｓとする）との間では、前述のように形成された円弧１７６の弧長ｂ´が、ｂ_ｋ＿１ ^ｓとなる。また、式（１９）でｂ_０は、それぞれのセンサユニットＳ_ｉのセンサ間寸法ｌとする。式（１９）より、位置補正後のセンサユニットＳ_ｋ−１とパーティクル１７２との間の円弧の弧長と、センサ間寸法ｌとの差が小さいほど関数ｈ（ｂ_ｋ＿１ ^ａ）は大きくなり、尤度が大きくなる。逆に、位置補正後のセンサユニットＳ_ｋ−１とパーティクル１７２との間の円弧の弧長と、センサ間寸法ｌとの差が大きいほど関数ｈ（ｂ_ｋ＿１ ^ａ）は小さくなり、尤度が小さくなる。ただし、前述のように位置補正後のセンサユニットＳ_ｋ−１とそれぞれのパーティクル１７２の補間後パーティクル位置との間の円弧の弧長は、位置補正後のセンサユニットＳ_ｋ−１とそれぞれのパーティクル１７２の補間前パーティクル位置との間の線分の寸法と同一であるため、ｂ_ｋ＿１ ^ａをセンサユニットＳ_ｋ−１とそれぞれのパーティクル１７２の補間前パーティクル位置との間の線分の寸法とした場合も、同様の結果が得られる。また、本実施形態では、φ_ｋ＿１ ^ａは、位置補正後のセンサユニットＳ_ｋ−１からそれぞれのパーティクル１７２の補間後パーティクル位置までのベクトルＶ´１と、それぞれのパーティクル１７２の補間後パーティクル位置から位置補正前のセンサユニットＳ_ｋ＋１までのベクトルＶ´２とが成す角度である。例えば図２０に示すように、パーティクル１７２Ａ（このパーティクルでは、ａ＝ｓとする）では、センサユニットＳ_ｋ−１からパーティクル１７２Ａの補間後パーティクル位置Ｆ２までのベクトルＶ´１と、パーティクル１７２Ａの補間後パーティクル位置Ｆ２からセンサユニットＳ_ｋ＋１までのベクトルＶ´２との成す角度が、φ_ｋ＿１ ^ｓとなる。式（２０）よりベクトルＶ´１とＶ´２の成す角度が０度〜９０度の範囲であれば、関数ｇ（φ_ｋ＿１ ^ａ）は大きくなり、尤度が大きくなる。逆に、それ以外の角度であれば、関数ｇ（φ_ｋ＿１ ^ａ）は小さくなり、尤度が小さくなる。そして、それぞれのパーティクル１７２の尤度から式（２１）を用いて、重みｗ_ｋ＿１ ^ａを算出する。

そして、センサ位置観測部１６９が、重み算出部６７で算出されたそれぞれのパーティクル１７２の重みに基づいて、センサユニットＳ_ｋの位置を観測する（ステップＳ１９５）。この際、センサ位置観測部１６９は、図２１に示すように、それぞれのパーティクル１７２の曲線補間部１６５により補間される前の補間前パーティクル位置の重み付き平均を算出し、算出された位置を移動前センサ位置Ｕ´_ｋ＿１とする。そして、パーティクル移動部６５が曲線補間を行う際に用いた移動モデルにより、移動前センサ位置Ｕ´_ｋ＿１から移動後センサ位置Ｕ_ｋ＿１への移動を行い、移動後センサ位置Ｕ_ｋ＿１をセンサユニットＳ_ｋの位置として推定する。

そして、位置推定制御部１６２により、所定の回数（本実施形態ではＭ回とする）だけセンサユニットＳ_ｋの位置推定が行われたかを判断する（ステップＳ１９６）。現状態では、センサユニットＳ_ｋの位置推定が１回しか行われていないため（ステップＳ１９６−Ｎｏ）、センサ位置推定部１６１は位置推定制御部１６２により、センサユニットＳ_ｋの位置推定をもう１回行う状態に制御される。２回目以降の位置推定では、センサ位置推定部１６１は、前回の位置推定での推定結果に基づいて、センサユニットＳ_ｋの位置推定を行う。

そして、ステップＳ１９２に戻り、パーティクル撒布部１６３により所定の数（本実施形態ではＡ個とする）のパーティクル１７２を撒く（ステップＳ１９２）。２回目以降の位置推定では、パーティクル撒布部１６３は、前回の位置推定の際に重み算出部１６７で算出されたそれぞれのパーティクル１７２の重みに基づいてパーティクル１７２を撒く。結果として、例えば、前回の位置推定の際に算出された重みが大きいパーティクル１７２の補間前パーティクル位置の周辺では、撒くパーティクル１７２の数を多くする。一方、前回の位置推定の際に算出された重みが小さいパーティクル１７２の補間前パーティクル位置の周辺では、撒くパーティクル７２の数を少なくする。

そして、２回目以降の位置推定でも同様に、曲線補間部１６５が、１回目の推定で用いた移動モデルに基づいて、センサユニットＳ_ｋ−１とそれぞれのパーティクル１７２との間の曲線補間を行う（ステップＳ１９３）。これにより、それぞれのパーティクル１７２は、補間前パーティクル位置から補間後パーティクル位置へ移動する。

そして、重み算出部１６７により、式（１８）〜式（２０）と同様の式を用いて、センサユニットＳ_ｋがそれぞれのパーティクル１７２の補間後パーティクル位置に位置する尤もらしさを評価する尤度を算出する。尤度により、それぞれのパーティクル１７２の補間後パーティクル位置の尤もらしさが検出される。そして、式（２１）と同様の式を用いて、尤度からそれぞれのパーティクル７２の重みが算出される（ステップＳ１９４）。ここで、例えばｔ回目の位置推定では、式（１８）〜式（２１）の尤度ｖ_ｋ＿１ ^ａ，重みｗ_ｋ＿１ ^ａ，ｂ_ｋ＿１ ^ａ，φ_ｋ＿１ ^ａの代わりに、それぞれ尤度ｖ_ｋ＿ｔ ^ａ，重みｗ_ｋ＿ｔ ^ａ，ｂ_ｋ＿ｔ ^ａ，φ_ｋ＿ｔ ^ａを用いて計算する。

そして、センサ位置観測部１６９が、重み算出部１６７で算出されたそれぞれのパーティクル１７２の重みに基づいて、センサユニットＳ_ｋの位置を観測する（ステップＳ１９５）。例えば、ｔ回目の位置推定では、それぞれのパーティクル１７２の重み付平均から補間前センサ位置Ｕ´_ｋ＿ｔが算出され、上述した移動モデルで曲線補間を行う。曲線補間により、補間前センサ位置Ｕ´_ｋ＿ｔから補間後センサ位置Ｕ_ｋ＿ｔへの移動が行われる。

そして、位置推定制御部１６２により、所定の回数（本実施形態ではＭ回とする）だけセンサユニットＳ_ｋの位置推定が行われたかを判断する（ステップＳ１９６）。センサユニットＳ_ｋの位置推定が所定の回数だけ行われていない場合は（ステップＳ１９６−Ｎｏ）、センサ位置推定部１６１は位置推定制御部１６２により、センサユニットＳ_ｋの位置推定をもう１回行う状態に制御され、ステップＳ１９２〜ステップＳ１９５をもう１回行う。センサユニットＳ_ｋの位置推定が所定の回数だけ行われている場合は（ステップＳ１９６−Ｙｅｓ）、次のステップに進む。

図１７に示すように、センサユニットＳ_ｋの位置推定が所定の回数（例えば２回）だけ行った後、センサ位置補正部１５５は、センサ位置推定部１６１の推定結果に基づいて、修正対象のリンク（修正対象リンク）Ｔ_ｋの先端側のセンサユニット（状態補正対象センサ）Ｓ_ｋの位置を補正する（ステップＳ１９７）。センサ位置補正部１５５は、センサユニットＳ_ｋの位置を、リンクＴ_ｋの先端に位置する初期位置（Ｕ_ｋ＿０）からセンサ位置推定部１６１の最終回（本実施形態ではＭ回目）の位置推定の際に推定されたセンサユニットＳ_ｋの補間後センサ位置Ｕ_ｋ＿Ｍへと、補正する。また、円弧形成部１５７が、センサ位置補正部１５５により補正されたセンサユニットＳ_ｋの位置に基づいて円弧Ｌ_ｋを形成する（ステップＳ１９７）。ここでは、位置補正後のセンサユニットＳ_ｋ−１とセンサユニットＳ_ｋの移動後センサ位置Ｕ_ｋ＿Ｍとを結ぶ円弧が、円弧Ｌ_ｋとなる。なお、位置補正後のセンサユニットＳ_ｋ−１とセンサユニットＳ_ｋの補間後センサ位置Ｕ_ｋ＿Ｍとを結ぶ円弧は、センサユニットＳ_ｋ−１とセンサユニットＳ_ｋの補間前センサ位置Ｕ´_ｋ＿Ｍとの間を上述した移動モデルで曲線補間を行うことにより形成される。センサユニットＳ_ｋの位置の移動により、形状修正順次実施部１５１によりリンクＴ_ｋの形状修正が行われる前の状態では連続していた修正完了部１７７と修正未完了部１７９との境界が、不連続な状態となる。すなわち、円弧Ｌ_ｋとリンクＴ_ｋ＋１が不連続な状態となる。

そして、第１の実施形態と同様に、この状態で、未修正リンク位置補正部１５３が、修正未完了部１７９を修正完了部１７７と境界が連続する状態に平行移動して、修正未完了部１７９の位置を補正する（ステップＳ１９８）。

なお、リンクＴ_ｋ以外のリンクＴ_ｊについても、形状修正順次実施部１５１により同様に形状修正が行われ、円弧Ｌ_ｊが形成される。また、未修正リンク位置補正部１５３は、形状修正順次実施部１５１により１つのリンクＴ_ｊの形状修正が行われるたびに、修正未完了部１７９の位置を補正する。

そして、図１７に示すように、形状修正制御部１５２が、すべてのリンクＴ_ｊについて形状修正が完了したか確認する（ステップＳ１９９）。すべてのリンクＴ_ｊについて形状修正が完了している場合は、挿入部１１の修正形状１７５が形成され、次のステップへ進む（ステップＳ１９９−Ｙｅｓ）。すべてのリンクＴ_ｊについて形状修正が完了していない場合は、ステップＳ１９１に戻り（ステップＳ１９９−Ｎｏ）、形状修正順次実施部１５１により、修正未完了部１７９の中で最も基端側（グローバル座標系Ｃの原点から近い側）に位置するリンク（修正対象リンク）Ｔ_ｊの形状修正が行われる。すなわち、ステップＳ１９１〜Ｓ１９８が、すべてのリンクＴ_ｊについて形状修正が完了するまで繰り返し行われる。

そこで、上記構成の内視鏡形状検出装置１及び内視鏡形状検出装置１を用いた内視鏡１０の挿入部１１の形状検出方法では、以下の効果を奏する。なわち、内視鏡形状検出装置１では、それぞれのセンサユニットＳ_ｉの計測データから姿勢検出部３０がセンサユニットＳ_ｉの姿勢を検出し、線形形状検出部４０がそれぞれのセンサユニットＳ_ｉの姿勢から内視鏡１０の挿入部１１の検出線形形状７１を検出する。そして、形状修正部１５０が、パーティクルフィルタによりそれぞれのセンサユニットＳ_ｉの位置を補正し、線形形状検出部４０により検出された検出線形形状７１を修正する。そして、挿入部１１の修正形状１７５が形成される。以上のように、観察時に体腔内に挿入される挿入部１１に配置されるセンサユニットＳ_ｉの計測データから、挿入部１１の修正形状１７５が検出されるため、体外にセンスコイル等を設ける必要がない。このため、内視鏡形状検出装置１の小型化、単純化を実現することができる。

また、内視鏡形状検出装置１では、形状修正部１５０により、パーティクルフィルタを用いてそれぞれのセンサユニットＳ_ｉの位置を補正し、検出線形形状７１を修正している。形状修正部１５０では、形状修正順次実施部１５１のセンサ位置推定部１６１によりそれぞれのセンサユニットＳ_ｉの推定を所定の回数だけ行う。センサユニットＳ_ｉの推定の際、重み算出部１６７によりそれぞれのパーティクル１７２の重みを算出する。そして、センサ位置観測部１６９が、重み算出部１６７で算出されたそれぞれのパーティクル１７２の重みに基づいて、センサユニットＳ_ｉの位置を観測する。算出されるパーティクル１７２の重みが大きいほど、曲線補間部１６５での曲線補間により補間されたそれぞれのパーティクル１７２の補間後パーティクル位置（移動後パーティクル位置）が、体腔内に挿入される挿入部１１でのセンサユニットＳ_ｉの尤もらしい位置（位置する確率が高い位置）となる。このため、センサ位置観測部１６９により、高い精度でセンサユニットＳ_ｉの位置が観測される。センサ位置補正部１５５は、センサ位置推定部１６１での最終回の推定でセンサ位置観測部１６９により観測された位置である補間後センサ位置（移動後センサ位置）に、それぞれのセンサユニットＳ_ｉの位置を補正する。このため、位置補正後のセンサユニットＳ_ｉの位置は、体腔内に挿入される挿入部１１でのセンサユニットＳ_ｉの尤もらしい位置（位置する確率が高い位置）となる。形状修正部１５０は、位置補正後のセンサユニットＳ_ｉの位置に基づいて、検出線形形状７１の修正を行うため、実際の挿入部１１の形状と誤差の少ない修正形状１７５を検出することができる。これにより、高い精度で挿入部１１の修正形状１７５を検出することができる。

さらに、内視鏡形状検出装置１では、形状修正順次実施部１５１によりそれぞれのリンクＴ_ｊが円弧（修正センサ間要素）Ｌ_ｊに形状修正される。実際に、体腔内に挿入された際の内視鏡１０の挿入部１１の形状は曲線形状である。内視鏡１０の挿入部１１は、製品により差はあるが、適度の弾性を有する。このため、挿入部１１の曲線形状の曲率が大きく変化することは、稀である。したがって、センサユニットＳ_ｉの間の形状を所定の半径（曲率）の円弧Ｌ_ｊに形状修正を行うことにより、実際の挿入部１１の曲線形状と誤差の少ない修正形状１７５を検出することができる。

（第１の実施形態の変形例）
次に、上述の第１の実施形態の変形例について説明する。なお、第１の実施形態と同一の部分及び同一の機能を有する部分については同一の符号を付して、その説明は省略する。

本変形例では、内視鏡１０の挿入部１１が平行移動している動状態での挿入部１１の修正形状７５を検出可能である。すなわち、センサ位置推定部６１のパーティクル移動部６５が、それぞれのセンサユニットＳ_ｉの加速度センサＡ_ｉにより計測された動状態での加速度データに基づいて、それぞれのパーティクル７２の移動を行う。

動状態での加速度データに基づいてそれぞれのパーティクルの移動を行う際は、まず、パソコン８に設けられる加速度検出部（図示しない）が、それぞれのセンサユニットＳ_ｉの加速度センサＡ_ｉにより計測された動状態での加速度データを取得する。

そして、加速度検出部は、動状態での加速度データに基づいて、それぞれのセンサユニットＳ_ｉの中心で計測される加速度ベクトルを、グローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分に分解する。そして、それぞれのセンサユニットＳ_ｉの中心で計測される加速度ベクトルの、グローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分を検出する。動状態では、静状態で鉛直方向に発生する重力加速度に、挿入部１１の移動による成分が加わるため、それぞれのセンサユニットＳ_ｉの中心で計測される加速度ベクトルのグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分を、

とする。また、それぞれのセンサユニットＳ_ｉの加速度センサＡ_ｉにより計測された加速度ベクトルのローカル座標系Ｃ_ｉのＸ_ｉ軸方向成分、Ｙ_ｉ軸方向成分、Ｚ_ｉ軸方向成分を、

とする。

また、本変形例では、内視鏡１０の挿入部１１が平行移動している場合を考えるため、近接して配置される複数のセンサユニットＳ_ｉにおいて、グローバル座標系Ｃでの加速度ベクトルは、同一であると仮定する。実際の内視鏡１０では、それぞれのセンサユニットＳ_ｉの間のセンサ間寸法ｌは非常に小さいため、このような仮定は可能である。このように仮定することにより、例えばセンサユニットＳ_１で計測される加速度ベクトルとセンサユニットＳ_２で計測される加速度ベクトルとの間で、

となる。そして、式（２４．１）、式（２４．２）からそれぞれ、

の関係が成り立つ。

ここで、センサユニットＳ_１では、グローバル座標系Ｃとローカル座標系Ｃ_１の関係から、式（１）の回転行列を用いて、

となる。同様に、センサユニットＳ_２では、グローバル座標系Ｃとローカル座標系Ｃ_２の関係から、式（１）の回転行列を用いて、

となる。式（２５．１）の関係を利用して、式（２６），（２７）を解くことにより、センサユニットＳ_１，Ｓ_２で計測された重力加速度以外の加速度ベクトルの、グローバル座標系ＣのＸ軸方向成分ａ_{ｔｈ１＿ｘ}（ａ_{ｔｈ２＿Ｘ}）、Ｙ軸方向成分ａ_{ｔｈ１＿Ｙ}（ａ_{ｔｈ２＿Ｙ}）、Ｚ軸方向成分ａ_{ｔｈ１＿Ｚ}（ａ_{ｔｈ２＿Ｚ}）が求まる。式（１３）で示す地磁気センサＢ_ｉにより計測される地磁気ベクトルのグローバル座標系Ｃのそれぞれの軸方向成分と、式（９）で示す地磁気ベクトルのローカル座標系Ｃ_ｉのそれぞれの軸方向成分との関係は、式（１）の回転行列を用いて、

となる。式（２８）においてｉ＝１(又は２)を代入した地磁気の関係式を含めて式（２５．１）（２６）（２７）を解く場合、方程式１２個に対して、未知数が姿勢角６個に加え加速度が６個で合計１２個である。その結果、センサユニットＳ_１，Ｓ_２で計測された重力加速度以外の加速度ベクトルの、グローバル座標系ＣのＸ軸方向成分ａ_{ｔｈ１＿Ｘ}（ａ_{ｔｈ２＿Ｘ}）、Ｙ軸方向成分ａ_{ｔｈ１＿Ｙ}（ａ_{ｔｈ２＿Ｙ}）、Ｚ軸方向成分ａ_{ｔｈ１＿Ｚ}（ａ_{ｔｈ２＿Ｚ}）を数値演算的に解くことが可能である。その他のセンサユニットＳ_ｉについても同様にして、それぞれのセンサユニットＳ_ｉで計測された重力加速度以外の加速度ベクトルの、グローバル座標系ＣのＸ軸方向成分ａ_{ｔｈｉ＿Ｘ}、Ｙ軸方向成分ａ_{ｔｈｉ＿Ｙ}、Ｚ軸方向成分ａ_{ｔｈｉ＿Ｚ}が求まる。

そして、パソコン８に設けられる変位算出部（図示しない）が、加速度検出部で検出された重力加速度以外の加速度ベクトルに基づいて、前回の静状態からのそれぞれのセンサユニットＳ_ｉの変位を算出する。前回の静状態からのそれぞれのセンサユニットＳ_ｉの変位のグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分は、それぞれのセンサユニットＳ_ｉで計測された重力加速度以外の加速度ベクトルの、グローバル座標系ＣのＸ軸方向成分ａ_{ｔｈｉ＿Ｘ}、Ｙ軸方向成分ａ_{ｔｈｉ＿Ｙ}、Ｚ軸方向成分ａ_{ｔｈｉ＿Ｚ}をそれぞれ２重積分することにより求まる。また、変位算出部では、それぞれのセンサユニットＳ_ｉの速度ベクトルのグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分も算出される。それぞれのセンサユニットＳ_ｉの速度ベクトルのグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分は、それぞれのセンサユニットＳ_ｉで計測された重力加速度以外の加速度ベクトルの、グローバル座標系ＣのＸ軸方向成分ａ_{ｔｈｉ＿Ｘ}、Ｙ軸方向成分ａ_{ｔｈｉ＿Ｙ}、Ｚ軸方向成分ａ_{ｔｈｉ＿Ｚ}をそれぞれ１回だけ積分することにより求まる。

パーティクル移動部６５は、加速度検出部及び変位算出部で検出されたそれぞれのセンサユニットＳ_ｉの加速度ベクトル、速度ベクトル、変位のグローバル座標系ＣのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分に基づいて、それぞれのパーティクル７２の移動モデルを検出する。そして、移動モデルに基づいてそれぞれのパーティクル７２を、移動前パーティクル位置から移動後パーティクル位置へと移動させる（図９のステップＳ１１３）。すなわち、パーティクル移動部６５は、動状態で検出されたそれぞれのセンサユニットＳ_ｉの加速度ベクトル、速度ベクトル、変位から移動モデルを検出し、移動モデルに基づいてそれぞれのパーティクル７２を移動する動状態移動モデル実施部となる。

そして、上述のように、移動後パーティクル位置の尤もらしさであるそれぞれのパーティクル７２の尤度から、重みを算出する（ステップＳ１１４）。そして、それぞれのパーティクル７２の重みからセンサユニットＳ_ｉの位置を観測する（ステップＳ１１５）。以上のようなセンサユニットＳ_ｉの位置推定を所定の回数行い、最終回の位置推定により推定された移動後センサ位置にセンサユニットＳ_ｉの位置を補正する（ステップＳ１１７）。そして、位置補正後のセンサユニットＳ_ｉの位置に基づいて、それぞれのセンサユニットＳ_ｉの間の形状修正後の形状である修正センサ間要素を形成する。（ステップＳ１１７）。以上のようにして、動状態においても、検出線形形状７１を形状修正した修正形状７５が検出可能となっている。

（その他の変形例）
なお、上述の実施形態では、センサ位置補正部５５，１５５により、修正対象リンクＴ_ｊの先端側のセンサユニットＳ_ｊの位置補正を行っているが、例えば位置補正に加えて、センサユニットＳ_ｊの姿勢の補正を行ってもよい。すなわち、センサ位置補正部５５，１５５は、修正対象リンクＴ_ｊの先端側のセンサユニットＳ_ｊの少なくとも位置を補正するセンサ状態補正部であればよい。この場合、センサ位置推定部６１，１６１により、パーティクルフィルタを用いてセンサユニットＳ_ｊの姿勢の推定も行われる。すなわち、センサ位置推定部６１，１６１は、パーティクルフィルタを用いてセンサユニットＳ_ｊの少なくとも位置の推定を行うセンサ状態推定部となっている。また、センサ位置観測部６９，１６９はセンサユニットＳ_ｊの少なくとも位置を観測するセンサ状態観測部となる。例えば、センサユニットＳ_ｋの姿勢推定を行う場合には、パーティクル撒布部６３により撒かれるそれぞれのパーティクル７２は、式（１７．１）〜式（１７．３）に示すグローバル座標系での位置に加え、姿勢を有する。ここで、式（７），式（８），式（１２）（又は式（１５））により求められるセンサユニットＳ_ｋのローカル座標系Ｃ_ｋの姿勢角をα_ｋ，β_ｋ，γ_ｋがセンサユニットＳ_ｋの初期姿勢（初期状態）であり、それぞれの姿勢角の誤差の分散をδ_α，δ_β，δ_γとする。１回目の推定ではパーティクル７２は、式（７），式（８），式（１２）（又は式（１５））により求められるセンサユニットＳ_ｋの初期姿勢（姿勢角α_ｋ，β_ｋ，γ_ｋ）に誤差の分散δ_α，δ_β，δ_γを加えた範囲に一様に撒かれる。すなわち、センサユニットＳ_ｋの１回目の推定でのそれぞれのパーティクル７２の姿勢角をα_ｋ＿１ ^ａ，β_ｋ＿１ ^ａ，γ_ｋ＿１ ^ａ）（ａ＝１，２，…，Ａ）とすると、それぞれのパーティクル７２は、

の範囲で一様に撒かれる。誤差の分散δ_α，δ_β，δ_γは、ノイズ等により生じるそれぞれのセンサユニットＳ_ｉの計測データの誤差の分散から、式（１）〜式（１５）を用いて分散を求める。２回目以降の推定では、前回の推定でのパーティクル７２の重みに基づいてそれぞれのパーティクル７２の姿勢が定められる。

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

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

を用いる。ここで、ｅ_ｘｋ−１はローカル座標系Ｃ_ｋ−１の原点での長手方向であるＸ_ｋ−１軸方向の単位ベクトルであり、ｅ_ｚｋ−１はローカル座標系Ｃ_ｋ−１の原点での長手方向であるＺ_ｋ−１軸方向の単位ベクトルである。

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

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

さらに、上述の実施形態では、グローバル座標系Ｃは、最も基端側のセンサユニットＳ_０の中心を原点としているが、最も先端側のセンサユニットＳ_Ｎの中心を原点としてもよい。この場合、リンク形成部４１は、式（１６．１），式（１６．２）を用いて、リンクＴ_ｊの先端側（グローバル座標系Ｃの原点から近い側）のセンサユニットＳ_ｊがグローバル座標系Ｃの原点に位置する場合の、リンクＴ_ｊの基端側（グローバル座標系Ｃの原点から遠い側）のセンサユニットＳ_ｊ−１の座標Ｐ´_ｊ（ｌ_ｘｊ，ｌ_ｙｊ，ｌ_ｚｊ）を求める。そして、グローバル座標系Ｃの原点と座標Ｐ´_ｊ（ｌ_ｘｊ，ｌ_ｙｊ，ｌ_ｚｊ）とを直線状に結ぶことにより、リンクＴ_ｊが形成される。また、形状修正部５０の形状修正順次実施部５１によりリンクＴ_ｊの形状修正を行う際は、先端側（グローバル座標系Ｃの原点から近い側）のリンクＴ_ｊから順に形状修正を行が行われる。この際、センサ位置補正部５５は、修正対象のリンク（修正対象リンク）Ｔ_ｊの基端側（グローバル座標系Ｃの原点から遠い側）のセンサユニット（状態補正対象センサ）Ｓ_ｊの位置を補正する。したがって、センサ位置観測部６１により、修正対象のリンク（修正対象リンク）Ｔ_ｊの基端側（グローバル座標系Ｃの原点から遠い側）のセンサユニット（状態補正対象センサ）Ｓ_ｊの観測が所定の回数だけ行われる。

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

また、本発明の別のある態様では、内視鏡の挿入部に長手方向に互いに所定のセンサ間寸法だけ離れて配置される複数のセンサユニットにより計測を行うことと、前記センサユニットでの計測データに基づいて、それぞれの前記センサユニットの姿勢を姿勢検出部により検出することと、前記姿勢検出部により検出されたそれぞれの前記センサユニットの姿勢に基づいて、それぞれの前記センサユニットの間の形状を寸法が前記センサ間寸法に等しい直線状のリンクと仮定して、前記内視鏡の前記挿入部の検出線形形状を線形形状検出部により検出することと、パーティクルフィルタを用いてそれぞれの前記センサユニットの少なくとも位置を補正し、前記線形形状検出部により検出された前記検出線形形状を修正した修正形状を形状修正部により検出することと、を備える内視鏡の挿入部の形状検出方法を提供する。

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

Claims (16)

1. 長手方向に互いに所定のセンサ間寸法（ｌ）だけ離れて複数のセンサユニット（Ｓ_ｉ）が配置される挿入部（１１）を備える内視鏡（１０）と、
   前記センサユニット（Ｓ_ｉ）での計測データに基づいて、それぞれの前記センサユニット（Ｓ_ｉ）の姿勢を検出する姿勢検出部（３０）と、
   前記姿勢検出部（３０）で検出されたそれぞれの前記センサユニット（Ｓ_ｉ）の姿勢に基づいて、それぞれの前記センサユニット（Ｓ_ｉ）の間の形状を寸法が前記センサ間寸法（ｌ）に等しい直線状のリンク（Ｔ_ｊ）と仮定して、前記内視鏡（１０）の前記挿入部（１１）の検出線形形状（７１）を検出する線形形状検出部（４０）と、
   パーティクルフィルタを用いてそれぞれの前記センサユニット（Ｓ_ｉ）の少なくとも位置を補正し、前記線形形状検出部（４０）により検出された前記検出線形形状（７１）を修正した修正形状（７５，１７５）を検出する形状修正部（５０，１５０）と、
   を具備する内視鏡形状検出装置（１）。
2. 前記形状修正部（５０，１５０）は、
   最も基端側又は最も先端側の前記センサユニット（Ｓ_１，Ｓ_Ｎ）の中心を原点とするグローバル座標系（Ｃ）において、それぞれの前記リンク（Ｔ_ｊ）ごとに前記グローバル座標系（Ｃ）の前記原点から近い側の前記リンク（Ｔ_ｊ）から順に形状修正を行い、前記修正形状（７５，１７５）のそれぞれの前記センサユニット（Ｓ_ｉ）の間の形状である修正センサ間要素（Ｔ´_ｊ，Ｌ_ｊ）を形成する形状修正順次実施部（５１，１５１）と、
   すべての前記リンク（Ｔ_ｊ）について前記形状修正を行う状態に前記形状修正順次実施部（５１，１５１）を制御する形状修正制御部（５２，１５２）と、
   を備える請求項１の内視鏡形状検出装置（１）。
3. 前記形状修正部（５０，１５０）は、前記形状修正順次実施部（５１，１５１）により１つの前記リンク（Ｔ_ｊ）の前記形状修正が行われるたびに、前記形状修正が行われていない前記リンク（Ｔ_ｊ）である未修正リンクから構成される修正未完了部（７９，１７９）を前記形状修正により形成される前記修正センサ間要素から構成される修正完了部（７７，１７７）と境界が連続する状態に平行移動し、前記修正未完了部（７９，１７９）の位置を補正する未修正リンク位置補正部（５３，１５３）を備える請求項２の内視鏡形状検出装置（１）。
4. 前記形状修正順次実施部（５１，１５１）は、
   前記形状修正順次実施部（５１，１５１）により前記形状修正を行う対象の前記リンク（Ｔ_ｊ）である修正対象リンク（Ｔ_ｋ）において、前記グローバル座標系（Ｃ）の前記原点から遠い側の前記センサユニット（Ｓ_ｉ）である状態補正対象センサ（Ｓ_ｋ）の少なくとも位置の推定を、前記パーティクルフィルタを用いて、前記形状修正順次実施部（５１，１５１）により前記修正対象リンク（Ｔ_ｋ）の形状修正が行われる前の前記状態補正対象センサ（Ｓ_ｋ）の初期状態又は前回の推定の推定結果に基づいて行うセンサ状態推定部（６１，１６１）と、
   前記状態補正対象センサの前記推定を所定の回数だけ行う状態に前記センサ状態推定部（６１，１６１）を制御する状態推定制御部（６２，１６２）と、
   を備える請求項２の内視鏡形状検出装置（１）。
5. 前記形状修正順次実施部（５１，１５１）は、
   前記センサ状態推定部（６１，１６１）での推定結果に基づいて、前記状態補正対象センサ（Ｓ_ｋ）の少なくとも位置を補正するセンサ状態補正部（５５，１５５）と、
   前記センサ状態補正部（５５，１５５）により補正された前記状態補正対象センサ（Ｓ_ｋ）の位置に基づいて前記修正センサ間要素（Ｔ´_ｊ，Ｌ_ｊ）を形成する修正センサ間要素形成部（５７，１５７）と、
   を備える請求項４の内視鏡形状検出装置（１）。
6. 前記センサ状態推定部（６１，１６１）は、
   前記状態補正対象センサ（Ｓ_ｋ）の前記初期状態又は前回の前記推定の推定結果に基づいて、所定の数（Ａ）のパーティクル（７２，１７２）を撒くパーティクル撒布部（６３，１６３）と、
   前記内視鏡（１０）の前記挿入部（１１）の状態及びそれぞれの前記センサユニット（Ｓ_ｉ）での計測データから移動モデルを検出し、前記移動モデルに基づいて、撒かれたそれぞれの前記パーティクル（７２，１７２）を移動前パーティクル位置から移動後パーティクル位置へ移動させるパーティクル移動部（６５，１６５）と、
   前記状態補正対象センサ（Ｓ_ｋ）がそれぞれの前記パーティクル（７２，１７２）の前記移動後パーティクル位置に位置する尤もらしさを示す尤度を算出し、前記尤度からそれぞれの前記パーティクル（７２，１７２）の重みを算出する重み算出部（６７，１６７）と、
   前記重み算出部（６７，１６７）で算出されたそれぞれの前記パーティクル（７２，１７２）の前記重みに基づいて、前記状態補正対象センサ（Ｓ_ｋ）の少なくとも位置を観測するセンサ状態観測部（６９，１６９）と、
   を備える請求項４の内視鏡形状検出装置（１）。
7. 前記パーティクル移動部（１６５）は、前記内視鏡（１０）の前記挿入部（１１）の曲がり状態から前記移動モデルを検出し、前記移動モデルに基づいて、前記修正対象リンク（Ｔ_ｋ）の前記グローバル座標系（Ｃ）の前記原点から近い側の前記センサユニット（Ｓ_ｋ−１）とそれぞれの前記パーティクル（１７２）との間の曲線補間を行い、それぞれの前記パーティクル（１７２）を移動させる曲線補間部（１６５）である請求項６の内視鏡形状検出装置（１）。
8. 前記パーティクル移動部（６５）は、前記内視鏡（１０）の前記挿入部（１１）が平行移動している動状態で検出されたそれぞれの前記センサユニット（Ｓ_ｉ）の前記グローバル座標系（Ｃ）での加速度ベクトル、速度ベクトル、変位から前記移動モデルを検出し、前記移動モデルに基づいてそれぞれの前記パーティクル（７２）を移動する動状態移動モデル実施部（６５）である請求項６の内視鏡形状検出装置（１）。
9. 内視鏡（１０）の挿入部（１１）に、長手方向に互いに所定のセンサ間寸法（ｌ）だけ離れて配置される複数のセンサユニット（Ｓ_ｉ）で計測を行うことと、
   前記センサユニット（Ｓ_ｉ）での計測データに基づいて、それぞれの前記センサユニット（Ｓ_ｉ）の姿勢を検出することと、
   検出されたそれぞれの前記センサユニット（Ｓ_ｉ）の姿勢に基づいて、それぞれの前記センサユニット（Ｓ_ｉ）の間の形状を寸法が前記センサ間寸法（ｌ）に等しい直線状のリンク（Ｔ_ｊ）と仮定して、前記内視鏡（１０）の前記挿入部（１１）の検出線形形状（７１）を検出することと、
   パーティクルフィルタを用いてそれぞれの前記センサユニット（Ｓ_ｉ）の少なくとも位置を補正し、検出された前記検出線形形状（７１）を修正した修正形状（７５，１７５）を検出することと、
   を具備する内視鏡（１０）の挿入部（１１）の形状検出方法。
10. それぞれの前記センサユニット（Ｓ_ｉ）の少なくとも位置を補正し、前記修正形状（７５，１７５）を検出することは、最も基端側又は最も先端側の前記センサユニット（Ｓ_１，Ｓ_Ｎ）の中心を原点とするグローバル座標系（Ｃ）において、それぞれの前記リンク（Ｔ_ｊ）ごとに前記グローバル座標系（Ｃ）の前記原点から近い側の前記リンク（Ｔ_ｊ）から順に形状修正を行い、前記修正形状（７５，１７５）のそれぞれの前記センサユニット（Ｓ_ｉ）の間の形状である修正センサ間要素（Ｔ´_ｊ，Ｌ_ｊ）を形成することを、すべての前記リンク（Ｔ_ｊ）について行うことを備える請求項９の内視鏡（１０）の挿入部（１１）の形状検出方法。
11. それぞれの前記センサユニット（Ｓ_ｉ）の少なくとも位置を補正し、前記修正形状（７５，１７５）を検出することは、１つの前記リンク（Ｔ_ｊ）の前記形状修正が行われるたびに、前記形状修正が行われていない前記リンク（Ｔ_ｊ）である未修正リンクから構成される修正未完了部（７９，１７９）を前記形状修正により形成される前記修正センサ間要素（Ｔ´_ｊ，Ｌ_ｊ）から構成される修正完了部（７７，１７７）と境界が連続する状態に平行移動し、前記修正未完了部（７９，１７９）の位置を補正することを備える請求項１０の内視鏡（１０）の挿入部（１１）の形状検出方法。
12. 前記修正センサ間要素（Ｔ´_ｊ，Ｌ_ｊ）を形成することは、前記形状修正を行う対象の前記リンク（Ｔ_ｊ）である修正対象リンク（Ｔ_ｋ）において、前記グローバル座標系（Ｃ）の前記原点から遠い側の前記センサユニット（Ｓ_ｉ）である状態補正対象センサ（Ｓ_ｋ）の少なくとも位置の推定を、前記パーティクルフィルタを用いて、前記修正対象リンクの前記形状修正が行われる前の前記状態補正対象センサ（Ｓ_ｋ）の初期状態又は前回の推定の推定結果に基づいて行うことを所定の回数だけ行うことを備える請求項１０の内視鏡（１０）の挿入部（１１）の形状検出方法。
13. 前記修正センサ間要素（Ｔ´_ｊ，Ｌ_ｊ）を形成することは、
    前記状態補正対象センサ（Ｓ_ｋ）の推定結果に基づいて、前記状態補正対象センサ（Ｓ_ｋ）の少なくとも位置を補正することと、
    補正された前記状態補正対象センサ（Ｓ_ｋ）の位置に基づいて前記修正センサ間要素（Ｔ´_ｊ，Ｌ_ｊ）を形成することと、
    を備える請求項１２の内視鏡（１０）の挿入部（１１）の形状検出方法。
14. 前記状態補正対象センサ（Ｓ_ｋ）の少なくとも位置の推定を行うことは、
    前記状態補正対象センサ（Ｓ_ｋ）の前記初期状態又は前回の前記推定の推定結果に基づいて、所定の数（Ａ）のパーティクル（７２，１７２）を撒くことと、
    前記内視鏡（１０）の前記挿入部（１１）の状態及びそれぞれの前記センサユニット（Ｓ_ｉ）での計測データから移動モデルを検出し、前記移動モデルに基づいて、撒かれたそれぞれの前記パーティクル（７２，１７２）を移動前パーティクル位置から移動後パーティクル位置へ移動させることと、
    前記状態補正対象センサ（Ｓ_ｋ）がそれぞれの前記パーティクル（７２，１７２）の前記移動後パーティクル位置に位置する尤もらしさを示す尤度を算出し、前記尤度からそれぞれの前記パーティクル（７２，１７２）の重みを算出することと、
    算出されたそれぞれの前記パーティクル（７２，１７２）の前記重みに基づいて、前記状態補正対象センサ（Ｓ_ｋ）の少なくとも位置を観測することと、
    を備える請求項１２の内視鏡（１０）の挿入部（１１）の形状検出方法。
15. それぞれの前記パーティクル（１７２）を移動させることは、前記内視鏡（１０）の前記挿入部（１１）の曲がり状態から前記移動モデルを検出し、前記移動モデルに基づいて、前記修正対象リンク（Ｔ_ｋ）の前記グローバル座標系（Ｃ）の前記原点から近い側の前記センサユニット（Ｓ_ｋ−１）とそれぞれの前記パーティクル（１７２）との間の曲線補間を行い、それぞれの前記パーティクル（１７２）を移動させることである請求項１４の内視鏡（１０）の挿入部（１１）の形状検出方法。
16. それぞれの前記パーティクル（７２）を移動させることは、前記内視鏡（１０）の前記挿入部（１１）が平行移動している動状態で検出されたそれぞれの前記センサユニット（Ｓ_ｉ）の前記グローバル座標系（Ｃ）での加速度ベクトル、速度ベクトル、変位から前記移動モデルを検出し、前記移動モデルに基づいてそれぞれの前記パーティクル（７２）を移動することである請求項１４の内視鏡（１０）の挿入部（１１）の形状検出方法。

Images