(57)【要約】【課題】  手術ロボット及びその制御方法を提供すること。【解決手段】  手術ロボットが手術作業を行う間に、腹腔内環境に対する映像情報を獲得し、映像情報及び手術ロボットに装着された内視鏡及びツールをなす各リンクの機構学情報及び多様なセンサ情報（内視鏡映像情報、慣性測定情報など）に基づいて内視鏡及びツールの位置を認識することで、位置認識の性能（位置認識の正確性及び収斂性）を向上させることができる。【選択図】      図１０
2015213753.tif 000002
【特許請求の範囲】
【請求項１】
内視鏡及びツールが装着された手術ロボットの制御方法であって、
前記手術ロボットが手術作業を行う間に腹腔内環境に対する映像情報を獲得するステップと、
前記映像情報及び前記内視鏡及びツールをなす各リンクの機構学情報に基づいて、前記内視鏡及びツールの位置を認識するステップと、を含む、
ことを特徴とする方法。
【請求項２】
前記内視鏡及びツールの位置の認識の結果に基づいて、前記腹腔内環境に対する地図を作成するステップと、をさらに含む、
ことを特徴とする請求項１に記載の方法。
【請求項３】
前記内視鏡及びツールの位置を認識するステップは、
現在獲得した映像情報及び前記機構学情報に基づいて、前記内視鏡及びツールの位置及び姿勢と、特徴点の位置とを予測するステップと、
既存の標識と前記現在獲得した映像情報から抽出された特徴点との間の同一性を判断するステップと、
前記既存の標識の位置と前記現在獲得した映像情報から抽出されて前記既存の標識とマッチングされた特徴点の位置情報を用いて、前記予測された内視鏡及びツールの位置及び姿勢と、標識に登録された特徴点の位置とを更新するステップと、を含む、
ことを特徴とする請求項１または２に記載の方法。
【請求項４】
前記内視鏡及びツールの位置及び姿勢と、特徴点の位置とを予測するステップの後、前記現在獲得した映像情報を複数の関心領域に分離するステップと、をさらに含む、
ことを特徴とする請求項３に記載の方法。
【請求項５】
前記現在獲得した映像情報を複数の関心領域に分離するステップは、
前記予測された内視鏡及びツールの位置及び姿勢情報を用いて、前記内視鏡に対するツールの相対的位置及び姿勢情報を算出するステップと、
前記現在獲得した映像情報にツールモデルを投映するステップと、
前記現在獲得した映像情報をツール映像関心領域と腹腔内映像関心領域とに分離するステップと、を含む、
ことを特徴とする請求項４に記載の方法。
【請求項６】
内視鏡及びツールが装着された手術ロボットの制御方法であって、
前記手術ロボットが手術作業を行う間に腹腔内環境に対する映像情報及び前記手術ロボットの慣性測定情報を獲得するステップと、
前記映像情報及び前記獲得した慣性測定情報に基づいて、前記内視鏡及びツールの位置を認識するステップと、を含む、
ことを特徴とする方法。
【請求項７】
前記内視鏡及びツールの位置認識の結果に基づいて、前記腹腔内環境に対する地図を作成するステップと、をさらに含む、
ことを特徴とする請求項６に記載の方法。
【請求項８】
前記内視鏡及びツールの位置を認識するステップは、
現在獲得した映像情報及び前記慣性測定情報に基づいて、前記内視鏡及びツールの位置及び姿勢と、特徴点の位置とを予測するステップと、
既存の標識と前記現在獲得した映像情報から抽出された特徴点との間の同一性を判断するステップと、
前記既存の標識の位置と前記現在獲得した映像情報から抽出されて前記既存の標識とマッチングされた特徴点の位置情報を用いて、前記予測された内視鏡及びツールの位置及び姿勢と、標識に登録された特徴点の位置とを更新するステップと、を含む、
ことを特徴とする請求項６または７に記載の方法。
【請求項９】
前記内視鏡及びツールの位置及び姿勢と、特徴点の位置とを予測するステップの後、前記現在獲得した映像情報を複数の関心領域に分離するステップ、をさらに含む、
ことを特徴とする請求項８に記載の方法。
【請求項１０】
前記現在獲得した映像情報を複数の関心領域に分離するステップは、
前記予測された内視鏡及びツールの位置及び姿勢情報を用いて、前記内視鏡に対するツールの相対的位置及び姿勢情報を算出するステップと、
前記現在獲得した映像情報にツールモデルを投映するステップと、
前記現在獲得した映像情報をツール映像関心領域と腹腔内映像関心領域とに分離するステップと、を含む、
ことを特徴とする請求項９に記載の方法。
【請求項１１】
手術ロボットが手術作業を行う間に腹腔内環境に対する映像情報を獲得する映像情報獲得部と、
前記映像情報及び前記手術ロボットに装着された内視鏡及びツールをなす各リンクの機構学情報に基づいて、前記内視鏡及びツールの位置を認識する制御部と、を含む、
ことを特徴とする手術ロボット。
【請求項１２】
前記制御部は、前記内視鏡及びツールの位置認識の結果に基づいて、前記腹腔内環境に対する地図を作成する、
ことを特徴とする請求項１１に記載の手術ロボット。
【請求項１３】
前記制御部は、現在獲得した映像情報及び前記機構学情報に基づいて、前記内視鏡及びツールの位置及び姿勢と、特徴点の位置とを予測し、
既存の標識と前記現在獲得した映像情報から抽出された特徴点との間の同一性を判断し、
前記既存の標識の位置と前記現在獲得した映像情報から抽出されて前記既存の標識とマッチングされた特徴点の位置情報を用いて、前記予測された内視鏡及びツールの位置及び姿勢と、標識に登録された特徴点の位置とを更新して前記内視鏡及びツールの位置を認識する、
ことを特徴とする請求項１１または１２に記載の手術ロボット。
【請求項１４】
前記制御部は、前記内視鏡及びツールの位置及び姿勢と、特徴点の位置とを予測した後、前記現在獲得した映像情報を複数の関心領域に分離する、
ことを特徴とする請求項１３に記載の手術ロボット。
【請求項１５】
前記制御部は、前記予測された内視鏡及びツールの位置及び姿勢情報を用いて、前記内視鏡に対するツールの相対的位置及び姿勢情報を算出し、
前記現在獲得した映像情報にツールモデルを投映し、
前記現在獲得した映像情報をツール映像関心領域と腹腔内映像関心領域とに分離して前記現在獲得した映像情報を複数の関心領域に分離する、
ことを特徴とする請求項１４に記載の手術ロボット。
【請求項１６】
手術ロボットが手術作業を行う間に腹腔内環境に対する映像情報を獲得する映像情報獲得部と
前記手術ロボットの慣性測定情報を獲得する慣性測定部と、
前記映像情報及び前記慣性測定情報に基づいて、前記内視鏡及びツールの位置を認識する制御部と、を含む、
ことを特徴とする手術ロボット。
【請求項１７】
前記制御部は、前記内視鏡及びツールの位置認識の結果に基づいて、前記腹腔内環境に対する地図を作成する、
ことを特徴とする請求項１６に記載の手術ロボット。
【請求項１８】
前記制御部は、現在獲得した映像情報及び前記慣性測定情報に基づいて、前記内視鏡及びツールの位置及び姿勢と、特徴点の位置とを予測し、
既存の標識と前記現在獲得した映像情報から抽出された特徴点との間の同一性を判断し、
前記既存の標識の位置と前記現在獲得した映像情報から抽出されて前記既存の標識とマッチングされた特徴点の位置情報を用いて、前記予測された内視鏡及びツールの位置及び姿勢と、標識に登録された特徴点の位置とを更新して前記内視鏡及びツールの位置を認識する、
ことを特徴とする請求項１６に記載の手術ロボット。
【請求項１９】
前記制御部は、前記内視鏡及びツールの位置及び姿勢と、特徴点の位置とを予測した後、前記現在獲得した映像情報を複数の関心領域に分離する、
ことを特徴とする請求項１８に記載の手術ロボット。
【請求項２０】
前記制御部は、前記予測された内視鏡及びツールの位置及び姿勢情報を用いて、前記内視鏡に対するツールの相対的位置及び姿勢情報を算出し、
前記現在獲得した映像情報にツールモデルを投映し、
前記現在獲得した映像情報をツール映像関心領域と腹腔内映像関心領域とに分離して前記現在獲得した映像情報を複数の関心領域に分離する、
ことを特徴とする請求項１９に記載の手術ロボット。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット手術過程において内視鏡映像情報に基づいて内視鏡及びツールの位置をリアルタイムで推定する方法に関する。
【背景技術】
【０００２】
低侵襲手術（Ｍｉｎｉｍａｌ  Ｉｎｖａｓｉｖｅ  Ｓｕｒｇｅｒｙ）とは、患部の大きさを最小化する手術を総称するもので、腹腔鏡手術（Ｌａｐａｒｏｓｃｏｐｉｃ  Ｓｕｒｇｅｒｙ）や、手術ロボット（ｓｕｒｇｉｃａｌ  ｒｏｂｏｔ）を用いた手術が低侵襲手術の代表的な例である。低侵襲手術は、人体の一部（例：腹部）に大きな切開窓を開いて施行する開腹手術と異なって、腹部に０．５ｃｍ〜１．５ｃｍ大きさの小さな穴（切開孔または侵襲口）を１つまたは複数を形成し、切開孔を介してビデオカメラ（内視鏡）と各種器具を入れた後、映像を見ながら施行する手術方法である。
【０００３】
このような低侵襲手術は開腹手術と違って手術後に痛みが少なく、腸運動の早期回復及び食べ物の早期摂取が可能で入院期間が短く、正常状態への復帰が早く、切開範囲が狭くて美容効果が優れるという特徴を有する。このような特徴により低侵襲手術は胆嚢切除術、前立腺癌手術、脱膓矯正術などに使用されていて、その分野は一層広がる趨勢である。
【０００４】
手術ロボットは、操作者（主に医師）の操作によって必要な信号を生成して伝送するマスタ装置とマスタ装置から信号を受けて直接患者に対して手術に必要な操作を行うスレーブロボットからなるが、通常、スレーブロボットは手術室に、マスタ装置は操作室にそれぞれ設置され、マスタ装置とスレーブロボットとを有線又は無線で連結して遠隔で手術を行う。
【０００５】
このような手術ロボットを用いて腹腔内においてロボット手術を行う場合、操作者は内視鏡を介して腹腔内環境に対する情報をモニタリングすることができる。しかしながら、狭い視野の範囲を有する内視鏡では、手術を行っている間に操作者に対して腹腔内での内視鏡及びツールがどの位置あるかについての判断ができなくなる。これは、内視鏡とツールとの間の干渉または衝突を起こしたり、内視鏡及びツールの不必要な動きによる臓器／組職損傷を起こす主な原因のうちの１つとなる。
【発明の概要】
【発明が解決しようとする課題】
【０００６】
内視鏡の位置及び姿勢だけでなく、ツールの位置及び姿勢まで位置認識フィルタの状態変数として利用して内視鏡の位置認識とツールの位置認識を同時に行うことで、位置認識の性能（位置認識の正確性及び収斂性）を向上させることができる手術ロボット及びその制御方法を提案する。
【０００７】
また、内視鏡及びツールの位置認識過程において機構学情報及び多様なセンサ情報（内視鏡映像情報、慣性測定情報など）を融合することで、位置認識の性能（位置認識の正確性及び収斂性）を向上させることができる手術ロボット及びその制御方法を提案する。
【０００８】
また、位置認識フィルタの結果物である内視鏡の位置／姿勢情報及び腹腔内特徴点の位置情報に基づいて腹腔内環境をモデリングすることで、モデリングされた腹腔内環境と内視鏡／ツールの位置及び姿勢との相対的な関係をリアルタイムに把握することができる手術ロボット及びその制御方法を提案する。
【課題を解決するための手段】
【０００９】
手術ロボットの制御方法は、内視鏡及びツールが装着された手術ロボットの制御方法において、手術ロボットが手術作業を行う間に腹腔内環境に対する映像情報を獲得し、映像情報及び内視鏡及びツールをなす各リンクの機構学情報に基づいて内視鏡及びツールの位置を認識する。
【００１０】
また、内視鏡及びツールの位置認識の結果に基づいて腹腔内環境に対する地図を作成することをさらに含む。
【００１１】
また、内視鏡及びツールの位置を認識することは、現在獲得した映像情報及び機構学情報に基づいて内視鏡及びツールの位置及び姿勢と、特徴点の位置とを予測し、既存の標識と現在獲得した映像情報から抽出された特徴点との間の同一性を判断し、既存の標識の位置と現在獲得した映像情報から抽出されて既存の標識とマッチングした特徴点との位置情報を用いて予測された内視鏡及びツールの位置及び姿勢と、標識に登録された特徴点の位置を更新することを含む。
【００１２】
また、内視鏡及びツールの位置及び姿勢と、特徴点の位置を予測した後、現在獲得した映像情報を複数の関心領域に分離することをさらに含む。
【００１３】
また、現在獲得した映像情報を複数の関心領域に分離することは、予測された内視鏡及びツールの位置及び姿勢情報を用いて内視鏡に対するツールの相対的位置及び姿勢情報を算出し、現在獲得した映像情報にツールモデルを投映し、現在獲得した映像情報をツール映像関心領域と腹腔内映像関心領域とに分離することを含む。
【００１４】
手術ロボットの制御方法は、内視鏡及びツールが装着された手術ロボットの制御方法において、手術ロボットが手術作業を行う間に腹腔内環境に対する映像情報及び手術ロボットの慣性測定情報を獲得し、映像情報及び獲得した慣性測定情報に基づいて内視鏡及びツールの位置を認識する。
【００１５】
また、内視鏡及びツールの位置認識の結果に基づいて腹腔内環境に対する地図を作成することをさらに含む。
【００１６】
また、内視鏡及びツールの位置を認識することは、現在獲得した映像情報及び慣性測定情報に基づいて内視鏡及びツールの位置及び姿勢と、特徴点の位置とを予測し、既存の標識と現在獲得した映像情報から抽出された特徴点との間の同一性を判断し、既存の標識の位置と現在獲得した映像情報から抽出されて既存の標識とマッチングされた特徴点との位置情報を用いて予測された内視鏡及びツールの位置及び姿勢と、標識に登録された特徴点との位置を更新することを含む。
【００１７】
また、内視鏡及びツールの位置及び姿勢と、特徴点の位置とを予測した後、現在獲得した映像情報を複数の関心領域に分離することをさらに含む。
【００１８】
また、現在獲得した映像情報を複数の関心領域に分離することは、予測された内視鏡及びツールの位置及び姿勢情報を用いて内視鏡に対するツールの相対的位置及び姿勢情報を算出し、現在獲得した映像情報にツールモデルを投映し、現在獲得した映像情報をツール映像関心領域と腹腔内映像関心領域とに分離することを含む。
【００１９】
手術ロボットは、手術ロボットが手術作業を行う間に腹腔内環境に対する映像情報を獲得する映像情報獲得部と、映像情報及び手術ロボットに装着された内視鏡及びツールをなす各リンクの機構学情報に基づいて内視鏡及びツールの位置を認識する制御部とを含む。
【００２０】
また、制御部は、内視鏡及びツールの位置認識の結果に基づいて腹腔内環境に対する地図を作成する。
【００２１】
また、制御部は、現在獲得した映像情報及び機構学情報に基づいて内視鏡及びツールの位置及び姿勢と、特徴点の位置とを予測し、既存の標識と現在獲得した映像情報から抽出された特徴点との間の同一性を判断し、既存の標識の位置と現在獲得した映像情報から抽出されて既存の標識とマッチングされた特徴点の位置情報を用いて予測された内視鏡及びツールの位置及び姿勢と、標識に登録された特徴点の位置とを更新して内視鏡及びツールの位置を認識する。
【００２２】
また、制御部は、内視鏡及びツールの位置及び姿勢と、特徴点の位置を予測した後、現在獲得した映像情報を複数の関心領域に分離する。
【００２３】
また、制御部は、予測された内視鏡及びツールの位置及び姿勢情報を用いて内視鏡に対するツールの相対的位置及び姿勢情報を算出し、現在獲得した映像情報にツールモデルを投映し、現在獲得した映像情報をツール映像関心領域と腹腔内映像関心領域とに分離して現在獲得した映像情報を複数の関心領域に分離する。
【００２４】
手術ロボットは、手術ロボットが手術作業を行う間に腹腔内環境に対する映像情報を獲得する映像情報獲得部と、手術ロボットの慣性測定情報を獲得する慣性測定部と、映像情報及び慣性測定情報に基づいて内視鏡及びツールの位置を認識する制御部とを含む。
【００２５】
また、制御部は、内視鏡及びツールの位置認識の結果に基づいて腹腔内環境に対する地図を作成する。
【００２６】
また、制御部は、現在獲得した映像情報及び慣性測定情報に基づいて内視鏡及びツールの位置及び姿勢と、特徴点の位置とを予測し、既存の標識と現在獲得した映像情報から抽出された特徴点との間の同一性を判断し、既存の標識の位置と現在獲得した映像情報から抽出されて既存の標識とマッチングされた特徴点の位置情報を用いて予測された内視鏡及びツールの位置及び姿勢と、標識に登録された特徴点の位置とを更新して内視鏡及びツールの位置を認識する。
【００２７】
また、制御部は、内視鏡及びツールの位置及び姿勢と、特徴点の位置とを予測した後、現在獲得した映像情報を複数の関心領域に分離する。
【００２８】
また、制御部は、予測された内視鏡及びツールの位置及び姿勢情報を用いて内視鏡に対するツールの相対的位置及び姿勢情報を算出し、現在獲得した映像情報にツールモデルを投映し、現在獲得した映像情報をツール映像関心領域と腹腔内映像関心領域とに分離して現在獲得した映像情報を複数の関心領域に分離する。
【発明の効果】
【００２９】
本発明の手術ロボット及びその制御方法によれば、内視鏡の位置及び姿勢だけでなく、ツールの位置及び姿勢まで位置認識フィルタの状態変数として利用して内視鏡の位置認識とツールの位置認識を同時に行うことで、位置認識の性能（位置認識の正確性及び収斂性）を向上させることができる。
【００３０】
また、本発明の手術ロボット及びその制御方法によれば、内視鏡及びツールの位置認識過程で機構学情報及び多様なセンサ情報（内視鏡映像情報、慣性測定情報など）を融合することで、位置認識の性能（位置認識の正確性及び収斂性）を向上させることができる。
【００３１】
また、本発明の手術ロボット及びその制御方法によれば、位置認識フィルタの結果物である内視鏡の位置／姿勢情報及び腹腔内の特徴点の位置情報に基づいて腹腔内環境をモデリングすることで、モデリングされた腹腔内環境と内視鏡／ツールの位置及び姿勢の相対的関係をリアルタイムに把握することができる。
【図面の簡単な説明】
【００３２】
【図１】手術ロボットの全体構造の一例を示す斜視図である。
【図２】図１に示す「Ａ」部分の内部状態を示す図である。
【図３】手術ロボット制御の一例を示すブロック図である。
【図４】手術ロボット制御の他の例を示すブロック図である。
【図５】位置認識フィルタの概念を説明するための図である。
【図６】カメラ（内視鏡）座標系に対するツールの相対的位置情報を算出する概念を説明するための図である。
【図７Ａ】内視鏡映像にツールモデルを投映して獲得した結果の映像図である。
【図７Ｂ】ツール映像と腹腔内映像とを分離した結果の映像図である。
【図８Ａ】内視鏡及びツールの同時位置認識結果及び腹腔内環境情報と相対的距離を表示した結果の映像を例示した図である。
【図８Ｂ】内視鏡及びツールの同時位置認識結果及び腹腔内環境情報と相対的距離を表示した結果の映像を例示した図である。
【図９】腹腔モデリング情報（内視鏡映像）と事前モデル情報（診断映像）とを整合して獲得した結果の映像図である。
【図１０】手術ロボットの制御方法の一例を示すフローチャートである。
【図１１】図１０に示す内視鏡映像の分離及び特徴点の抽出過程を詳細に示すフローチャートである。
【発明を実施するための形態】
【００３３】
以下に、添付図面を参照しながら実施形態を詳細に説明する。
【００３４】
図１は、手術ロボットの全体構造の一例を示す斜視図であり、図２は、図１に示す「Ａ」部分の内部状態を示す図である。特に、図１は、末端部に手術器具（ｓｕｒｇｉｃａｌ  ｔｏｏｌ）が付着された複数の手術機構（ｓｕｒｇｉｃａｌ  ｉｎｓｔｒｕｍｅｎｔ）を１つの切開孔（ｓｉｎｇｌｅ−ｐｏｒｔ）を介して患者の体内に進入させて、人体内の多様な場所で手術を可能にさせたシングルポート手術ロボット（Ｓｉｎｇｌｅ−Ｐｏｒｔ  Ｓｕｒｇｉｃａｌ  Ｒｏｂｏｔ）を示す。シングルポート手術ロボットを利用する手術では、以下に説明する多様な要件が必要である。１番目に、複数の手術器具が１つの切開孔を介して体内に進入した後に手術作業のために任意の位置に移動可能でなければならないので、手術器具の作業領域（ｗｏｒｋ  ｓｐａｃｅ）が広くなければならない。２番目に、手術器具が高い自由度を有して多様な作業が可能でありながら、腹腔壁などの人体組職との衝突を最小化すべきである。３番目に、広い作業領域と高自由度を有するスレーブロボットを利用して手術することにおいて、視覚的案内（Ｖｉｓｕａｌ  Ｇｕｉｄａｎｃｅ）機能を介して操作者に有用な情報を提供して安全かつ精密な手術が行われるように支援しなければならない。
【００３５】
図１に示すように、手術ロボットは、手術台に横たわった患者に手術を行うスレーブロボット２００と、操作者（主に医師）がスレーブロボット２００を遠隔操作のために用いるマスタ装置１００とを含んでなる。マスタ装置１００は操作者（主に医師）の操作による制御信号を生成してスレーブロボット２００に伝送する。一方、スレーブロボット２００は、マスタ装置１００から制御信号を受信し、受信された制御信号によって動いて手術に必要な操作を患者に加える。ここで、マスタ装置１００とスレーブロボット２００とは必ず物理的に独立した別途の装置として分離しなければならないのではなく、１つに統合されて一体型として構成されてもよい。
【００３６】
図１及び図２に示すように、スレーブロボット２００は、マウンティングアーム２０２及び円筒状のケーシング２０４を含むことができる。
【００３７】
スレーブロボット２００のマウンティングアーム２０２は、多自由度を有して駆動されるように実現することができる。マウンティングアーム２０２は複数のリンク及び複数の関節からなり、その上部には円筒状のケーシング２０４が連結される。ケーシング２０４には、内部に複数のツール２１２ａ、２１２ｂ及び内視鏡２１４を含むガイドチューブ２１０と、複数のツール２１２ａ、２１２ｂ及び内視鏡２１４とガイドチューブ２１０の駆動のための駆動部（図３の２６０Ａ及び図４の２６０Ｂ参照）が内蔵される。ガイドチューブ２１０はマウンティングアーム２０２に連結設置される。スレーブロボット２００が手術を行わないときには、ガイドチューブ２１０がケーシング２０４内に内蔵されていて、スレーブロボット２００が手術を行うときには図１及び図２に示すようにケーシング２０４内に内蔵していたガイドチューブ２１０を外に出し患者の体内に挿入される。
【００３８】
ガイドチューブ２１０が患者の体内に挿入されて手術作業を行う状態（図１に示す「Ａ」部分の内部状態）をより詳細に参照すると、図２に示す通りである。ガイドチューブ２１０が患者の肌に形成された切開孔Ｉを介して体内に進入した後、手術しようとする部位（手術部位）に接近するとガイドチューブ２１０から複数のツール２１２ａ、２１２ｂと内視鏡２１４が分岐されて手術作業を行う。ここで、ガイドチューブ２１０、複数のツール２１２ａ、２１２ｂ、及び内視鏡２１４もマウンティングアーム２０２と同様に、複数のリンク及び複数の関節からなって多自由度を有し、駆動されるように実現することができる。複数のツール２１２ａ、２１２ｂのそれぞれの末端部（ｄｉｓｔａｌ  ｅｎｄ）には、腹腔内で臓器と接触し、切断したり縫い込むなど直接的な手術作業を行う鉗子（ｆｏｒｃｅｐｓ）、ジョー（ｊａｗ）、グラスパー（ｇｒａｓｐｅｒ）、シザーズ（ｓｃｉｓｓｏｒｓ）、ステープラー（ｓｔａｐｌｅｒ）、焼灼器（ｃａｕｔｅｒｙ）、ニードル（ｎｅｅｄｌｅ）などの手術器具（ｓｕｒｇｉｃａｌ  ｔｏｏｌ）であるエンドエフェクタ２１６ａ、２１６ｂが装着される。また、内視鏡２１４の末端部には、腹腔内観察対象（例：臓器、組職、病変）などに対する映像情報を獲得する内視鏡カメラ２１８が装着される。内視鏡２１４では、ロボット手術に、主に使われる腹腔鏡（ｌａｐａｒｏｓｃｏｐｅ）以外に、胸腔鏡（ｔｈｏｒａｃｏｓｃｏｐｙ）、関節鏡（ａｒｔｈｒｏｓｃｏｐｅ）、及び鼻鏡（ｒｈｉｎｏｓｃｏｐｅ）など多様な手術用内視鏡が使用される。
【００３９】
一方、マスタ装置１００は、マスタ操作器１１２Ｌ、１１２Ｒ、ペダルセンサ１１４Ｌ、１１４Ｒ及び表示部１１６を含むことができる。
【００４０】
マスタ装置１００は、操作者が両手にそれぞれ把持して操作するマスタ操作器１１２Ｌ、１１２Ｒを備える。操作者は、マスタ操作器１１２Ｌ、１１２Ｒを介してスレーブロボット２００のマウンティングアーム２０２、ガイドチューブ２１０、複数のツール２１２ａ、２１２ｂ、及び内視鏡２１４の位置及び機能を操作する。マスタ操作器１１２Ｌ、１１２Ｒは、３次元空間上でマウンティングアーム２０２などのｘ軸、ｙ軸、ｚ軸の併進運動（ｔｒａｎｓｌａｔｉｏｎａｌ  ｍｏｔｉｏｎ）及びロール（ｒｏｌｌ）方向、ピッチ（ｐｉｔｃｈ）方向、ヨー（ｙａｗ）方向の回転運動（ｒｏｔａｔｉｏｎａｌ  ｍｏｔｉｏｎ）を制御するために６自由度を有するように実現される。マスタ操作器１１２Ｌ、１１２Ｒは、図１に例示したように、２つのハンドルに実現され、操作者のハンドル操作による制御信号がスレーブロボット２００に伝送されてマウンティングアーム２０２などを含むスレーブロボット２００の動作が制御される。操作者のハンドル操作によりマウンティングアーム２０２、ガイドチューブ２１０、複数のツール２１２ａ、２１２ｂ、及び内視鏡２１４の併進運動及び回転運動、このような動きを利用して実質的な手術作業（例：縫合、配管挿入など）が行われる。
【００４１】
マスタ装置１００は、マスタ操作器１１２Ｌ、１１２Ｒの操作機能を拡張するために、操作者が両足でそれぞれ踏んだり押したりして操作できるように、２つのペダルセンサ１１４Ｌ、１１４Ｒを含む。図１に示す２つのハンドルから構成されるマスタ操作器１１２Ｌ、１１２Ｒと２つのペダルセンサ１１４Ｌ、１１４Ｒを利用してマウンティングアーム２０２などの動作を制御する方式を詳細な例として説明すると、まず、左側ハンドル１１２Ｌを利用してマウンティングアーム２０２の位置及び動作を制御し、右側ハンドル１１２Ｒを利用してガイドチューブ２１０の位置及び動作を制御することができる。また、マスタ装置１００に用意されているモードスイッチ（図示せず）またはボタン（図示せず）が操作された状態では、左側ハンドル１１２Ｌを利用して第１ツール（左側ツール、２１２ａ）の位置及び動作を制御し、右側ハンドル１１２Ｒを利用して第２ツール（右側ツール、２１２ｂ）の位置及び動作を制御することができる。これに加えて（モードスイッチまたはボタンが操作された後）、左側ペダルセンサ１１４Ｌが操作された状態では、左側ハンドル１１２Ｌを利用して内視鏡２１４の位置及び動作を制御することができる。また、モードスイッチまたはボタンが操作された後、右側ペダルセンサ１１４Ｒが操作された状態で右側ハンドル１１２Ｒを利用して内視鏡２１４の位置及び動作を制御できるように実現することも可能である。
【００４２】
図１では、マスタ装置１００に２つのマスタ操作器ハンドルが装着された場合を例として説明したが、ハンドルをさらに追加して同時に複数の手術装備（例：ガイドチューブ、複数のツール）をリアルタイムで操作できるように実現することも可能である。ハンドル１１２Ｌ、１１２Ｒは、その操作方式によって多様な機構的構成を有することができ、例えば、ジョイスティック（ｊｏｙｓｔｉｃｋ）など３次元的な動きを有することができながらスレーブロボット２００のマウンティングアーム２０２、ガイドチューブ２１０及び複数のツール２１２ａ、２１２ｂなどを作動させるための多様な入力手段が利用される。マスタ操作器１１２Ｌ、１１２Ｒには、複数のリンク及び複数の関節（リンクとリンクとの連結部位）が連結される。マスタ操作器１１２Ｌ、１１２Ｒに連結される複数の関節のそれぞれには、マスタ操作器１１２Ｌ、１１２Ｒに連結される各関節の回転角度を検出する回転角センサ（例：エンコーダ）が装着される。
【００４３】
マスタ装置１００の表示部１１６には、内視鏡２１４により入力された映像が画像イメージとして表示される。表示部１１６は、１つ以上のモニタで構成されることができ、各モニタに手術時に必要な情報が個別的に表示されるようにすることができる。図１には、表示部１１６が３つのモニタを含む場合が例示されたが、モニタの数は表示を要する情報の類型や種類などによって多様に決定される。
【００４４】
マスタ装置１００とスレーブロボット２００は、有線通信網または無線通信網を介して相互結合されて制御信号、内視鏡２１４を介して入力された内視鏡映像及び各種センサ（例：慣性センサ）を介して入力された検出情報などが相手（スレーブロボットまたはマスタ装置）に伝送される。万一、マスタ装置１００に用意された２つのマスタ操作器ハンドルによる２つの制御信号が伝送される必要がある場合に、２つの制御信号は相互独立的に伝送することができる。例えば、ガイドチューブ２１０から分岐した第１ツール２１２ａの位置調整のための制御信号とガイドチューブ２１０から分岐した第２ツール２１２ｂの位置調整のための制御信号とが同時にまたは近接する時点で伝送される必要がある場合、各制御信号は相互独立的にスレーブロボット２００に伝送される。
【００４５】
独立的に伝送される制御信号は、互いに干渉せず、いずれか１つの制御信号が他の１つの制御信号に影響を与えない。このように、複数の制御信号が互いに独立的に伝送されるためには、各制御信号の生成段階において各制御信号に対するヘッダ情報を付加して伝送するか、各制御信号がその生成手順によって伝送されるようにするか、または各制御信号の伝送手順に関して予め優先順位を決めておいて、それにより伝送されるようにするなど多様な方式が利用される。この場合、各制御信号が伝送される伝送経路が独立的に備えられることで各制御信号との間に干渉が根本的に防止できるようにすることができる。
【００４６】
図３は、手術ロボット制御の一例を示すブロック図である。
【００４７】
図３に示すように、手術ロボットは、マスタ装置１００Ａ及びスレーブロボット２００Ａを含んでなる。
【００４８】
マスタ装置１００Ａは、入力部１２０Ａ、保存部１３０Ａ、マスタ制御部１４０Ａ、通信部１５０Ａ、及び表示部１１６Ａを含むことができる。
【００４９】
入力部１２０Ａは、使用者がスレーブロボット２００Ａの動作命令（例：手術開始命令、手術作業命令など）を入力するためのものであって、上述のマスタ操作器１１２Ｌ、１１２Ｒ及びペダルセンサ１１４Ｌ、１１４Ｒ、または使用者インターフェースＵＩなどで実現することができる。
【００５０】
保存部１３０Ａは、マスタ制御部１４０Ａが内視鏡２１４及び複数のツール２１２ａ、２１２ｂに対する位置認識を行うために必要とする事前情報及びアルゴリズムと、位置認識の実行結果を保存するためのメモリであって、保存部１３０Ａには、複数のツール２１２ａ、２１２ｂのそれぞれに対する３次元モデル情報（ＣＡＤモデル情報）、内視鏡２１４及び複数のツール２１２ａ、２１２ｂのそれぞれをなす各リンク（関節と関節との間を連結する器具構造）の機構学的情報（長さ情報）、スレーブロボット２００Ａの手術作業進行過程で視覚センサ基盤ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ  Ｌｏｃａｌｉｚａｔｉｏｎ  Ａｎｄ  Ｍａｐｐｉｎｇ）アルゴリズムを用いて算出される内視鏡２１４及び複数のツール２１２ａ、２１２ｂに対する位置認識結果（内視鏡の位置／姿勢情報、複数のツールの位置／姿勢情報及び標識の位置情報）及び位置認識結果に基づいて作成される腹腔内環境に対する３次元地図などが保存される。保存部１３０Ａは、さらに手術以前に撮影したエックス−レイ（Ｘ−ｒａｙ）映像、超音波映像、コンピュータ断層撮映（ＣＴ）映像及び磁気共鳴映像（ＭＲＩ）などの多様な診断映像などを保存することができる。
【００５１】
マスタ制御部１４０Ａは、手術ロボットの全般的な動作を制御するためのプロセッサであって、マスタ制御部１４０Ａは、さらに位置推定部１４２Ａ、地図作成部１４４Ａ、及び映像処理部１４６Ａを含む。
【００５２】
位置推定部１４２Ａは、スレーブロボット２００Ａ側の映像情報獲得部２２０Ａを介して獲得された映像情報及び上述の内視鏡２１４及び複数のツール２１２ａ、２１２ｂのそれぞれをなす各リンクの機構学的情報にＳＬＡＭアルゴリズムを適用するか、または映像情報獲得部２２０Ａを介して獲得された映像情報及び慣性測定部２２５Ａを介して獲得された慣性測定情報（加速度情報または角速度情報）にＳＬＡＭアルゴリズムを適用して内視鏡２１４に対する位置／姿勢及び複数のツール２１２ａ、２１２ｂに対する位置／姿勢を推定する。ＳＬＡＭアルゴリズムは、映像内の特徴点（ｆｅａｔｕｒｅ）の位置と内視鏡２１４及び複数のツール２１２ａ、２１２ｂの位置／姿勢情報を１つの状態変数（ｓｔａｔｅ  ｖａｒｉａｂｌｅ）として設定して確率的なフィルタ技法を介して状態変数をなす要素を同時に推定することになり、その過程は予測（Ｐｒｅｄｉｃｔｉｏｎ）段階、データ統合（Ｄａｔａ  Ａｓｓｏｃｉａｔｉｏｎ）段階、及び更新（Ｕｐｄａｔｅ）段階からなって繰り返し行われる。このとき、確率フィルタでは、拡張カルマンフィルタ（Ｅｘｔｅｎｄｅｄ  Ｋａｌｍａｎ  Ｆｉｌｔｅｒ）、パーティクルフィルタ（Ｐａｒｔｉｃｌｅ  Ｆｉｌｔｅｒ）などが使用される。さらに位置推定部１４２Ａは視覚センサ基盤オドメトリ技法を利用して内視鏡２１４に対する位置／姿勢を推定することができる。
【００５３】
地図作成部１４４Ａは、位置推定部１４２Ａを介して行われた位置認識結果である内視鏡２１４の位置情報及び姿勢情報と腹腔内映像の特徴点の位置情報から腹腔内環境に対する３次元地図を作成する。
【００５４】
映像処理部１４６Ａは、スレーブロボット２００Ａの映像情報獲得部２２０Ａ、例えば内視鏡２１４の末端部に装着された内視鏡カメラ２１８から入力された映像を画像イメージに出力するために入力された映像に対する処理を行う。ここで、映像処理の例では、撮影された映像の拡大、縮小、回転、移動、編集及びフィルタリングなどがあげられる。
【００５５】
通信部１５０Ａは、有線通信網または無線通信網を介してマスタ制御部１４０Ａ及びスレーブロボット２００Ａの通信部２５０Ａに連結されてデータを送受信する通信回路であって、マスタ制御部１４０Ａを介して生成されたトルク制御信号（各関節の目標回転角度を追従するための関節トルクに相応するトルク制御信号）をスレーブロボット２００Ａに伝送するか、または映像情報獲得部２２０Ａを介して獲得された映像情報（内視鏡映像情報）及び慣性測定部２２５Ａを介して獲得された慣性測定情報などをスレーブロボット２００Ａから受信することができる。
【００５６】
表示部１１６Ａは、位置推定部１４２Ａを介して行われた位置認識の結果及び地図作成部１４４Ａを介して作成された腹腔内環境に対する地図に基づいてモデリングされた腹腔内環境と内視鏡２１４及びツール２１２ａ、２１２ｂの位置／姿勢の相対的関係を表示する。
【００５７】
また、表示部１１６Ａは、スレーブロボット２００Ａの映像情報獲得部２２０Ａ、例えば内視鏡カメラ２１８から伝達された内視鏡映像に相応する画像イメージ及び／または保存部１３０Ａに保存されている手術以前に撮影されたエックス−レイ（Ｘ−ｒａｙ）映像、超音波映像、コンピュータ断層撮映（ＣＴ）映像及び磁気共鳴映像（ＭＲＩ）などの多様な診断映像などを視覚的情報に出力する。
【００５８】
スレーブロボット２００Ａは、マスタ装置１００Ａから受信された制御信号によりマウンティングアーム２０２、ガイドチューブ２１０、複数のツール２１２ａ、２１２ｂ、及び内視鏡２１４を動作させて直接患者に対して手術に必要な操作を加える。このようなスレーブロボット２００Ａは、図３に示すように、映像情報獲得部２２０Ａ、慣性測定部２２５Ａ、保存部２３０Ａ、スレーブ制御部２４０Ａ、通信部２５０Ａ、及び駆動部２６０Ａを含むことができる。
【００５９】
映像情報獲得部２２０Ａは、患者体内に挿入されて移動しながら内臓臓器や体腔内部を撮影して手術部位の映像情報を獲得する。映像情報獲得部２２０Ａは内視鏡２１４で実現することができる。映像情報獲得部２２０Ａを介して獲得された映像情報はスレーブ制御部２４０Ａ内の映像処理部２４６Ａに伝達されて映像処理過程を経るか、映像処理過程なしに通信部２５０Ａを介してマスタ装置１００Ａに伝送され得る。
【００６０】
慣性測定部２２５Ａは、スレーブロボット２００Ａの加速度、速度、方向（角度）など多様な航法関連情報を測定するためのものであって、スレーブロボット２００Ａの複数のツール２１２ａ、２１２ｂ及び／または内視鏡２１４に設けられて姿勢情報（角度情報）を検出する。慣性測定部２２５Ａは、重力方向と慣性系に対してマウンティングアーム２０２の相対的角度を検出してロール方向、ピッチ方向、ヨー方向の姿勢情報（角度情報）を発生させる。慣性測定部２２５Ａは、角度を測定する勾配センサと、角速度を測定する角速度センサを含んで構成される。勾配センサとしては加速度計（ａｃｃｅｌｅｒｏｍｅｔｅｒ）が用いられ、角速度センサとしてはレートジャイロスコープ（ｒａｔｅ−ｇｙｒｏｓｃｏｐｅ）が用いられる。
【００６１】
保存部２３０Ａはスレーブロボット２００Ａの動作を制御するのに必要な情報及びアルゴリズム、スレーブロボット２００Ａ側で獲得された情報などを保存する。例えば、保存部２３０Ａ内には映像情報獲得部２２０Ａを介して獲得された手術部位に対する映像情報及び慣性測定部２２５Ａを介して獲得された慣性測定情報（加速度情報または角速度情報）が保存される。また、保存部２３０Ａは、手術以前に撮影したエックス−レイ（Ｘ−ｒａｙ）映像、超音波映像、コンピュータ断層撮映（ＣＴ）映像及び磁気共鳴映像（ＭＲＩ）などの多様な診断映像などを保存することができる。
【００６２】
スレーブ制御部２４０Ａは、スレーブロボット２００Ａをなす各種構成要素を連結してスレーブロボット２００Ａの動作を制御するためのプロセッサであって、映像情報獲得部２２０Ａで獲得された手術部位の映像情報を、通信部２５０Ａを介してマスタ装置１００Ａに伝送するか、またはマスタ制御部１４０Ａで生成したトルク制御信号を、通信部２５０Ａを介して受信して駆動部２６０Ａに伝達する役割をする。
【００６３】
またスレーブ制御部２４０Ａは、映像情報獲得部２２０Ａを介して獲得された手術部位の映像に対する処理を行う映像処理部２４６Ａを含むことができる。ここで、映像処理の例では、撮影された映像の拡大、縮小、回転、移動、編集及びフィルタリングなどがあげられる。スレーブ制御部２４０Ａ内で行われる映像処理過程は場合によって省略することができる。
【００６４】
通信部２５０Ａは、有線通信網または無線通信網を介してスレーブ制御部２４０Ａ及びマスタ装置１００Ａの通信部１５０Ａに連結されてデータを送受信する通信回路であって、マスタ装置１００Ａからトルク制御信号を受信するか、または映像情報獲得部２２０Ａを介して獲得された映像情報（内視鏡映像情報）及び慣性測定部２２５Ａを介して獲得された慣性測定情報（加速度情報または角速度情報）などをマスタ装置１００Ａに伝送することができる。
【００６５】
駆動部２６０Ａは、マウンティングアーム２０２、ガイドチューブ２１０、それぞれのツール２１２ａ、２１２ｂ、及び内視鏡２１４をなす複数の関節それぞれに電気または油圧による動力を伝達するためのモータなどのアクチュエータであって、マスタ制御部１４０Ａから伝達されたトルク制御信号によりマウンティングアーム２０２、ガイドチューブ２１０、それぞれのツール２１２ａ、２１２ｂ、及び内視鏡２１４をなす各関節を回転駆動する。
【００６６】
図４は、手術ロボット制御の他の例を示すブロック図である。
【００６７】
以上では、図３を参照して一実施形態に係る手術ロボットのマスタ装置１００Ａ及びスレーブロボット２００Ａの制御構成を説明した。図３は、マスタ装置１００Ａのマスタ制御部１４０Ａが内視鏡２１４の位置及び姿勢と、複数のツール２１２ａ、２１２ｂの位置及び姿勢を推定する位置推定部１４２Ａ及び位置推定部１４２Ａの位置認識結果に基づいて腹腔内環境をモデリングする地図作成部１４４Ａを含む場合を示す。これとは異なって、図４に示す実施形態では、位置推定部２４２Ｂ及び地図作成部２４４Ｂがスレーブロボット２００Ｂのスレーブ制御部２４０Ｂ内に含まれる。
【００６８】
図４に示す実施形態の場合、スレーブ制御部２４０Ｂが内視鏡２１４及び複数のツール２１２ａ、２１２ｂに対する位置推定過程を行うので、スレーブロボット２００Ｂ側の保存部２３０Ｂ内に内視鏡２１４及び複数のツール２１２ａ、２１２ｂの位置／姿勢を推定するのに必要な情報が保存される。例えば、複数のツールのそれぞれに対する３次元モデル情報、内視鏡２１４及び複数のツール２１２ａ、２１２ｂのそれぞれをなす各リンクの機構学的情報が保存部２３０Ｂに保存される。また保存部２３０Ｂには、位置推定部２４２Ｂで行われた内視鏡２１４及び複数のツール２１２ａ、２１２ｂに対する位置認識結果（内視鏡の位置／姿勢情報、複数のツールの位置／姿勢情報及び標識の位置情報）及び位置認識結果に基づいて作成される腹腔内環境に対する３次元地図が保存される。このとき、スレーブ制御部２４０Ｂ内の位置推定部２４２Ｂで行われた位置認識結果及び位置認識結果に基づいて作成された腹腔内環境に対する地図は、通信部２５０Ｂ、１５０Ｂを介してマスタ制御部１４０Ｂに伝達され、マスタ制御部１４０Ｂは受信された位置認識結果及び腹腔内環境に対する地図に基づいてモデリングされた腹腔内環境と内視鏡及びツールの位置／姿勢との相対的関係を表示部１１６Ｂに表示する。
【００６９】
図４に示す手術ロボットの制御構成は、図３に示す手術ロボットの制御構成と比較した場合、位置推定部２４２Ｂ及び地図作成部２４４Ｂがマスタ制御部１４０Ｂでないスレーブ制御部２４０Ｂ内に設けられるという点だけに差があって、その以外の他の構成要素は図３に示す手術ロボットの構成要素と等しいので、ここでは詳しい説明を省略する。
【００７０】
以下では、図５ないし図１０を参照して手術ロボットに装着された内視鏡及びツールの位置／姿勢を推定認識する方法と認識結果に基づいて腹腔内環境に対する地図を作成する方法について詳細に説明する。
【００７１】
図５は、位置認識フィルタの概念を説明するための図である。
【００７２】
図５に示すように、位置認識フィルタは、ロボット手術過程において内視鏡２１４の位置及び姿勢ｒＥＮ、ｑＥＮ、複数のツール２１２ａ、２１２ｂの位置及び姿勢ｒＬＴ、ｑＬＴ、ｒＲＴ、ｑＲＴ、腹腔内映像の特徴点ｙｌ、ツール映像の特徴点ｙＬＴ、ｉ、ｙＲＴ、ｊを全域座標系（Ｗｏｒｌｄ  Ｃｏｏｒｄｉｎａｔｅ）Ｗに対して同時にリアルタイムで推定することができる。参照までに、図５において、符号Ｏは臓器（器官、ｏｒｇａｎ）を示し、符号Ｌは病変（ｌｅｓｉｏｎ）を示す。
【００７３】
位置認識フィルタは、基本的に全域座標系Ｗに対する内視鏡２１４及びツール２１２ａ、２１２ｂの３次元位置（ｐｏｓｉｔｉｏｎ）ｒと３次元姿勢（ｏｒｉｅｎｔａｔｉｏｎ）ｑ、内視鏡映像に表示される特徴点（ｆｅａｔｕｒｅ）の３次元位置ｙを１つの状態変数（ｓｔａｔｅｖａｒｉａｂｌｅ）ｘ（ｋ｜ｋ）で構成し、確率基盤フィルタアルゴリズム（例えば、Ｋａｌｍａｎ  Ｆｉｌｔｅｒ、Ｐａｒｔｉｃｌｅ  Ｆｉｌｔｅｒなど）を介して位置推定を行う。ここで、内視鏡２１４及びツール２１２ａ、２１２ｂの３次元姿勢は、四元数（ｑｕａｔｅｒｎｉｏｎ）で表示される。
【００７４】
図５に示すように、スレーブロボット２００に１つの内視鏡２１４と２つのツール２１２ａ、２１２ｂが装着されている場合、位置認識フィルタは内視鏡２１４の位置及び姿勢ｒＥＮ、ｑＥＮ、２つのツール２１２ａ、２１２ｂの位置及び姿勢ｒＬＴ、ｑＬＴ、ｒＲＴ、ｑＲＴ、腹腔内映像から抽出した複数個の特徴点の位置ｙ、ツール映像から抽出した複数個の特徴点の位置ｙＬＴ、ｉ、ｙＲＴ、ｊを状態変数として有することができる。状態変数ｘ（ｋ｜ｋ）は、下記数式（１）のように示される。
【００７５】
【数１】
2015213753.tif 000003
【００７６】
内視鏡２１４及びツール２１２ａ、２１２ｂの位置／姿勢、腹腔内映像から抽出した複数個の特徴点の位置及びツール映像から抽出した複数個の特徴点の位置をリアルタイムで推定するための位置認識フィルタは大きく、予測（Ｐｒｅｄｉｃｔｉｏｎ）段階と更新（Ｕｐｄａｔｅ）段階からなっていて、各段階を繰り返し行う。
【００７７】
以下では、まず状態変数をなす構成要素のうちの内視鏡２１４とツール２１２ａ、２１２ｂの位置及び姿勢ｒＥＮ、ｑＥＮ、ｒＬＴ、ｑＬＴ、ｒＲＴ、ｑＲＴを予測する過程について説明する。
【００７８】
位置認識フィルタで内視鏡２１４とツール２１２ａ、２１２ｂの位置及び姿勢ｒＥＮ、ｑＥＮ、ｒＬＴ、ｑＬＴ、ｒＲＴ、ｑＲＴを予測するということは、内視鏡２１４とツール２１２ａ、２１２ｂの位置及び姿勢ｒＥＮ、ｑＥＮ、ｒＬＴ、ｑＬＴ、ｒＲＴ、ｑＲＴを更新段階においてセンサ情報と融合（ｆｕｓｉｏｎ）する前に、現在段階の状態変数を動きモデルを介して推定することを意味する。このとき、内視鏡２１４とツール２１２ａ、２１２ｂは、共通的に内視鏡２１４及び複数のツール２１２ａ、２１２ｂのそれぞれをなす各リンクの機構学情報（長さ情報）を利用するか、慣性測定部２２５Ａ、２２５Ｂを介して測定された加速度情報または角速度情報などを利用し、内視鏡２１４とツール２１２ａ、２１２ｂが以前段階ｋ−１で現在段階ｋに移動した程度を相対的位置及び姿勢変化情報δｒ、δｑ形態の測定値として算出することができる。
【００７９】
一方、映像情報を利用することができる内視鏡２１４の場合には、視覚センサ基盤オドメトリ技法を活用して、現在段階ｋの映像と以前段階ｋ−１の映像の入力を受けて各映像から視覚特徴点（ｆｅａｔｕｒｅ）を抽出して各映像から抽出された特徴点をマッチングして得られた相関関係から内視鏡２１４の相対的位置及び姿勢変化情報δｒ、δｑを測定値として算出することができる。
【００８０】
相対的位置及び姿勢変化情報から予測状態変数を求める過程は、動き予測モデルｆ（・）を利用して構成される。この過程は、下記数式（２）のように表現することができる。
【００８１】
【数２】
2015213753.tif 000004
【００８２】
上記の数式（２）は、位置認識フィルタをなす予測段階で相対的位置及び姿勢変化情報を用いて予測状態変数を算出するための包括的な数式である。ここで、ｘ（ｋ｜ｋ−１）は予測状態変数を示し、ｆ（・）は動き予測モデルを示し、ｘ（ｋ−１｜ｋ−１）は以前段階ｋ−１で更新された状態変数を示し、δｘ（ｋ）は相対的位置及び姿勢変化情報を示す。
【００８３】
数式（２）は、以前段階で更新された状態変数ｘ（ｋ−１｜ｋ−１）と以前段階ｋ−１で現在段階ｋに移動した程度を示す相対的位置及び姿勢変化情報δｘ（ｋ）に対して動き予測モデルｆ（・）を適用して現在段階ｋでの予測状態変数ｘ（ｋ｜ｋ−１）を算出する過程を意味するものとする。数式（２）に記載された状態変数ｘは、内視鏡２１４及び複数のツール２１２ａ、２１２ｂの位置情報及び姿勢情報をすべて含む。言い換えれば、δｘ（ｋ）は相対的位置変化情報であるδｒ及び相対的姿勢変化情報であるδｑをすべて含む概念である。
【００８４】
内視鏡２１４及び複数のツール２１２ａ、２１２ｂの予測位置情報ｒ（ｋ｜ｋ−１）は下記数式（３）を介して算出することができ、内視鏡２１４及び複数のツール２１２ａ、２１２ｂの予測姿勢情報ｑ（ｋ｜ｋ−１）を算出することができる。
【００８５】
【数３】
2015213753.tif 000005
【００８６】
ここで、Ｒは、回転変換行列（Ｒｏｔａｔｉｏｎ  Ｍａｔｒｉｘ）を示す。
【００８７】
【数４】
2015213753.tif 000006
【００８８】
また、予測される状態変数の誤差の程度を推定する不確実性（ｕｎｃｅｒｔａｉｎｔｙ）は、確率基盤フィルタアルゴリズムによって上記の数式（２）のモデルに基づいて共分散行列（ｃｏｖａｒｉａｎｃｅ  ｍａｔｒｉｘ）またはパーティクル（ｐａｒｔｉｃｌｅ）の分布などに予測される。
【００８９】
確率基盤フィルタとして、カルマンフィルタ（Ｋａｌｍａｎ  ｆｉｌｔｅｒ）を利用する場合、下記数式（５）に示すように、動き予測モデルｆ（・）のヤコビ行列（Ｊａｃｏｂｉａｎ  ｍａｔｒｉｘ）Ｆと相対的位置及び姿勢変化情報δｒ、δｑの雑音（ｎｏｉｓｅ）Ｑ（ｋ）から共分散行列を算出することができる。
【００９０】
【数５】
2015213753.tif 000007
【００９１】
ここで、Ｐ（ｋ｜ｋ−１）は予測共分散行列であり、Ｐ（ｋ−１｜ｋ−１）は以前段階ｋ−１で更新された共分散行列であり、ＦＴはヤコビ行列の転置行列（ｔｒａｎｓｐｏｓｅｄ  ｍａｔｒｉｘ）を示す。一方、確率基盤フィルタとしてパーティクルフィルタ（Ｐａｒｉｃｌｅ  ｆｉｌｔｅｒ）を利用する場合には、予測モデルｆ（・）に基づいてパーティクルをサンプリング（ｓａｍｐｌｉｎｇ）する方式で、新しいパーティクルの分布を構成することができる。
【００９２】
状態変数をなす構成要素のうち内視鏡２１４とツール２１２ａ、２１２ｂの位置及び姿勢ｒＥＮ、ｑＥＮ、ｒＬＴ、ｑＬＴ、ｒＲＴ、ｑＲＴを除去した残りの状態変数腹腔内映像の特徴点及びツール映像の特徴点の位置を予測する方法は次のようになる。
【００９３】
腹腔内映像から抽出した特徴点の位置ｙｌは、静的な環境の場合（例：腹腔内臓器の動きがない場合）コンスタントポジションモデル（ｃｏｎｓｔａｎｔ  ｐｏｓｉｔｉｏｎ  ｍｏｄｅｌ）のように相対的位置変化がないように予測する。これは下記数式（６）のように示すことができる。
【００９４】
【数６】
2015213753.tif 000008
【００９５】
腹腔内映像から抽出した特徴点の位置ｙｌは、動的な環境の場合（例：腹腔内臓器の動きがある場合）有限要素法（Ｆｉｎｉｔｅ  Ｅｌｅｍｅｎｔ  Ｍｅｔｈｏｄ；ＦＥＭ）のような腹腔物性モデルを用いて予測するか、または任意のブラウンモーションノイズ（Ｂｒｏｗｎｉａｎ  ｍｏｔｉｏｎ  ｎｏｉｓｅ）を反映して予測することができ、その形態は、下記数式（７）のように示すことができる。
【００９６】
【数７】
2015213753.tif 000009
【００９７】
上で算出した腹腔内映像から抽出した特徴点の位置変化情報△ｙｌから予測位置情報ｙｌ（ｋ｜ｋ−１）を算出する過程は、下記数式（８）の過程を介して適用することができる。
【００９８】
【数８】
2015213753.tif 000010
【００９９】
一方、ツール映像から抽出した特徴点の位置ｙＬＴ、ｉ、ｙＲＴ、ｊは、図６に示すように、ツールモデル情報（モデル内での特徴点の位置）と予測された内視鏡２１４の位置及び姿勢ｒＥＮ、ｑＥＮとツール２１２ａ、２１２ｂの位置及び姿勢ｒＬＴ、ｒＲＴ、ｑＬＴ、ｑＲＴから３次元座標変換を介して予測することができる。まず、図６に示すように、予測段階で予測した情報を用いてカメラ座標系Ｃに対するツール２１２ａ、２１２ｂの相対的位置情報(ＥＮｒＬＴ、ＥＮｒＲＴ、図６から点線矢印で図示)を求める。次に、カメラモデル（Ｉｎｔｒｉｎｓｉｃ  ｐａｒａｍｅｔｅｒ（ｃｕ、ｃｖ、ｆｕ、ｆｖ）、ｄｉｓｔｏｒｔｉｏｎ  ｐａｒａｍｅｔｅｒ（ｋ１、ｋ２））を利用して下記数式（９）及び数式（１０）を介してツール２１２ａ、２１２ｂの相対的位置情報ＥＮｒＬＴ、ＥＮｒＲＴをカメラ座標系Ｃに投映してツール映像から抽出した特徴点の位置ｙＬＴ、ｉ、ｙＲＴ、ｊを予測する。
【０１００】
【数９】
2015213753.tif 000011
ここで、
【０１０１】
【数１０】
2015213753.tif 000012
である。
【０１０２】
状態変数をなす構成要素である内視鏡２１４及びツール２１２ａ、２１２ｂの位置／姿勢、腹腔内映像から抽出した複数個の特徴点の位置及びツール映像から抽出した複数個の特徴点の位置に対する予測段階を行った以後には、内視鏡映像分離及び特徴点抽出段階を行う。内視鏡映像分離及び特徴点抽出段階では、追後に進行する更新段階で必要とする繰り返し特徴点を抽出し、観測値と予測観測値を正しくマッチングする過程を行う。内視鏡映像分離及び特徴点抽出段階は、位置認識フィルタでの予測段階と更新段階との間の過程に該当する。
【０１０３】
上述のツール映像から抽出した特徴点の位置ｙＬＴ、ｉ、ｙＲＴ、ｊを予測する方法と同様に、内視鏡２１４に対するツール２１２ａ、２１２ｂの相対的位置情報ＥＮｒＬＴ、ＥＮｒＲＴを算出して上述の数式（９）及び数式（１０）を介して、図７Ａに示すように、現在の内視鏡映像にツール２１２ａ、２１２ｂのモデル（図７Ａで、白色の太い実線で表示した領域）を投映することができる。
【０１０４】
図７Ｂに示すように、投影されたツールモデルが位置する領域はツール映像関心領域Ａとして決めることができ、残り領域は腹腔内映像関心領域Ｂとして指定することができる。それぞれの関心領域ＲＯＩから抽出された特徴点の映像座標は、以後進行される更新段階で観測値（ｍｅａｓｕｒｅｍｅｎｔ）として順次に利用され、位置認識誤差を減らすのに用いられる。
【０１０５】
内視鏡映像でツール２１２ａ、２１２ｂが占める領域であるツール映像と腹腔内映像（内視鏡映像でツール映像を除いた部分）に分離することによって、ツール２１２ａ、２１２ｂの動きから発生しうる誤った特徴点マッチングを防止することができて位置認識フィルタの強靭性を向上させることができる。
【０１０６】
位置認識フィルタを構成する更新段階は、内視鏡映像のセンサ処理を介して得られた観測値（ｍｅａｓｕｒｅｍｅｎｔ）ｚ（ｋ）と観測ノイズＲ（ｋ）、そして予測段階で得られた状態変数ｘ（ｋ｜ｋ−１）を利用して得られた予測観測値（ｐｒｅｄｉｃｔｅｄ  ｍｅａｓｕｒｅｍｅｎｔ）ｈ（ｋ）と、を利用して行われる。このとき、予測観測値は、上記の数式（９）及び数式（１０）と同一方法で特徴点の３次元位置を内視鏡（カメラ）座標系を基準に変換して求める。
【０１０７】
ツール映像から抽出される特徴点は、道具に付着されたマーカー（ｍａｒｋｅｒ）のような人工標識（ａｒｔｉｆｉｃｉａｌ  ｌａｎｄｍａｒｋ）またはツール２１２ａ、２１２ｂに含まれた角（ｅｄｇｅ）やボルト穴などのような自然標識（ｎａｔｕｒａｌ  ｌａｎｄｍａｒｋ）が利用される。一方、腹腔内映像から抽出される特徴点は、コーナー（ｃｏｒｎｅｒ）やブロブ（ｂｌｏｂ）のような映像特徴点や血管、神経、臓器のような人体モデル情報が利用される。
【０１０８】
位置認識フィルタの更新段階は、確率基盤フィルタアルゴリズム（Ｋａｌｍａｎ  Ｆｉｌｔｅｒ、Ｐａｒｔｉｃｌｅ  Ｆｉｌｔｅｒなど）によって異なるように適用される。
【０１０９】
確率基盤フィルタとしてカルマンフィルタ（Ｋａｌｍａｎ  ｆｉｌｔｅｒ）を利用する場合には、下記数式（１１）のように、予測観測値ｈ（ｋ）のヤコビ行列Ｈ（ｋ）、観測ノイズＲ（ｋ）からＫａｌｍａｎ  Ｇａｉｎ  Ｋを算出し、下記の数式（１２）及び数式（１３）を利用して新しい推定値と共分散行列を算出する。
【０１１０】
【数１１】
2015213753.tif 000013
【０１１１】
一方、確率基盤フィルタとしてパーティクルフィルタ（Ｐａｒｔｉｃｌｅ  ｆｉｌｔｅｒ）を利用する場合には、予測観測値と実際観測値から計算された加重値（ｗｅｉｇｈｔ）を利用して予測段階で得られたパーティクルをリサンプリング（ｒｅ−ｓａｍｐｌｉｎｇ）して更新されたパーティクル分布を求めることができる。
【０１１２】
図８Ａ及び図８Ｂは、内視鏡及びツールの同時位置認識結果及び腹腔内環境情報と相対的距離を表示した結果映像を例示した図である。
【０１１３】
上述の予測段階、内視鏡映像分離及び特徴点抽出、及び更新段階を経て位置認識過程を行った後には、位置認識フィルタの結果物である内視鏡の位置／姿勢情報と腹腔内特徴点の位置情報から腹腔内環境をモデリングすることができる。
【０１１４】
位置認識フィルタは、腹腔内特徴点の位置ｙｌをリアルタイムで推定することができるので、３次元空間上に腹腔内特徴点の位置ｙｌを表示することが可能である。図８Ａに示すように、３次元空間上の点を利用して三角形メッシュ（Ｔｒｉａｎｇｕｌａｒ  Ｍｅｓｈ）を構成すると３次元情報の腹腔モデリングが可能である。
【０１１５】
追加的に、ステレオ内視鏡や３次元距離情報を算出することができる内視鏡を使用する場合、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ  Ｃｌｏｓｅｓｔ  Ｐｏｉｎｔ）アルゴリズムを利用して３次元点群（Ｐｏｉｎｔ  Ｃｌｏｕｄ）データを整合するか、または確率に基づいた格子地図（Ｇｒｉｄ  Ｍａｐ）を構成して図８Ｂに示すように、腹腔内環境をモデリングすることができる。
【０１１６】
また、図８Ａ及び図８Ｂに示すように、内視鏡映像だけを利用して腹腔内環境をモデリングすることもできるが、図９に示すように、腹腔モデリング情報内視鏡映像と事前モデル情報（診断映像）を整合して新しい結果の映像を生成することも可能である。言い換えれば、図９に示すように、手術または検査対象領域に対する３次元内視鏡映像ａ、超音波映像ｂ、ＭＲ映像ｃ、及びＣＴ映像ｄを整合して新しい結果の映像ｅを生成することで、腹腔内環境に対するより詳細かつ正確なモデリングが可能となる。
【０１１７】
３次元点群データとの間の整合のために利用するＩＣＰアルゴリズムはｐ、ｑで定義したステレオ内視鏡または３次元距離情報を算出することができる内視鏡を介して獲得された２つの３次元点群データとの間で最も近い点を一対一（１：１）に対応させてこれらの間の距離の合計が最小となる変換を捜し、変換させた状態で、さらに対応関係を捜す過程を繰り返し行う方式の最適化技法を使用する。このとき、下記数式（１４）を用いて、３次元点群データｑとに計算された３次元点群データｐとの間の距離の合計Ｇ（Ｒ、ｔ）を最適化過程の基準（尺度）とする。最終的に、下記数式（１５）を用いて、数式（１４）を最小とする剛体変換の関係式であるＲ’、ｔ’が整合に用いられる。
【０１１８】
【数１２】
2015213753.tif 000014
【０１１９】
ここで、Ｒは、ＩＣＰアルゴリズムの実行の結果、算出された回転変換行列であって、ｔは、ＩＣＰアルゴリズムの実行の結果、算出された並進変換行列である。
【０１２０】
図１０は、手術ロボットの制御方法の一例を示すフローチャートであり、図１１は、図１０に示す内視鏡映像の分離及び特徴点抽出過程を詳細に示すフローチャートである。
【０１２１】
動作説明のための初期条件として、保存部１３０Ａには、内視鏡２１４及び複数のツール２１２ａ、２１２ｂに対する位置認識を行うために必要とする事前情報であって、複数のツール２１２ａ、２１２ｂのそれぞれに対する３次元モデル情報（ＣＡＤモデル情報）、内視鏡２１４及び複数のツール２１２ａ、２１２ｂのそれぞれをなす各リンク（関節と関節の間を連結する器具構造）の機構学的情報（長さ情報）が予め保存されていることを前提とする。
【０１２２】
また説明の便宜上、図３に示すマスタ装置１００Ａ及びスレーブロボット２００Ａに基づいて、手術ロボットの制御方法の一例を説明する。
【０１２３】
まず、入力部１２０Ａを介して操作者から手術ロボットに対する手術作業命令が入力されると手術ロボットの手術が開始される。
【０１２４】
手術ロボットの手術が開始されると、マスタ制御部１４０Ａはスレーブロボット側の映像情報獲得部２２０Ａから腹腔内環境に対する映像情報を、慣性測定部２２５Ａから慣性測定情報（加速度情報及び角速度情報）の入力を周期的に受けながら手術動作を行う（３００）。
【０１２５】
手術ロボットの手術動作が進行される間に、マスタ制御部１４０Ａ内の位置推定部１４２Ａはビジョンセンサ基盤ＳＬＡＭアルゴリズムに基づき、これに追加的に保存部１３０Ａに既に保存されている内視鏡２１４をなす各リンクの機構学的情報または慣性測定部２２５Ａを介して獲得した慣性測定情報（加速度情報及び角速度情報）を用いて内視鏡２１４の位置及び姿勢を予測する（３１０）。また、位置推定部１４２Ａは、ビジョンセンサ基盤オドメトリ技法を利用して内視鏡２１４の位置及び姿勢を予測することができる。
【０１２６】
次に、位置推定部１４２Ａは、ビジョンセンサ基盤ＳＬＡＭアルゴリズムに基づき、これに追加的に保存部１３０Ａに既に保存されている複数のツール２１２ａ、２１２ｂのそれぞれをなす各リンクの機構学的情報または慣性測定部２２５Ａを介して獲得した慣性測定情報（加速度情報及び角速度情報）を用いて複数のツール２１２ａ、２１２ｂの位置及び姿勢を予測する（３２０）。
【０１２７】
その後、位置推定部１４２Ａは、内視鏡映像で獲得した特徴点の位置を予測する（３３０）。動作３２０及び３３０は、ＳＬＡＭアルゴリズムの予測（Ｐｒｅｄｉｃｔｉｏｎ）段階に該当する。
【０１２８】
次に、位置推定部１４２Ａは、映像情報獲得部２２０Ａを介して獲得した内視鏡映像をツール映像と腹腔内映像とに分離し、ツール映像及び腹腔内映像からそれぞれの特徴点を抽出する（３４０）。
【０１２９】
動作３４０に記載した内視鏡映像の分離及び特徴点抽出過程をより詳細に参照すると、図１１に示すように、位置推定部１４２Ａは内視鏡２１４に対するツール２１２ａ、２１２ｂの相対的位置及び姿勢情報を算出する（３４２）。
【０１３０】
その後、位置推定部１４２Ａは、図７Ａに示すように、現在内視鏡映像にツールモデルを投映する（３４４）。ここで、内視鏡映像にツールモデルを投映するときには、複数のツール２１２ａ、２１２ｂのそれぞれに対する３次元モデル情報（ＣＡＤモデル情報）及び内視鏡２１４の固有パラメータ（ｉｎｔｒｉｎｓｉｃ  ｐａｒａｍｅｔｅｒ）及び歪曲パラメータ（ｄｉｓｔｏｒｔｉｏｎ  ｐａｒａｍｅｔｅｒ）を利用する。
【０１３１】
次に、位置推定部１４２Ａは、図７Ｂに示すように、ツール映像と腹腔内映像とを分離する（３４６）。
【０１３２】
その後、位置推定部１４２Ａは、ツール映像及び腹腔内映像からそれぞれ特徴点を抽出する（３４８）。
【０１３３】
また、図１０に対する説明に戻って、位置推定部１４２Ａは、保存部１３０Ａに既に保存されている既存の標識と現在抽出された特徴点との間の同一性を判断する（３５０）。言い換えれば、位置推定部１４２Ａは、特徴点の追跡（ｔｒａｃｋｉｎｇ）及びマッチング（ｍａｔｃｈｉｎｇ）過程を介して現在獲得した映像情報から抽出された特徴点が、既に標識として使用されていたものか、それとも新しい標識として登録すべきかの可否を判断する。この過程は、ＳＬＡＭアルゴリズムのデータ統合（Ｄａｔａ  Ａｓｓｏｃｉａｔｉｏｎ）段階に該当する。
【０１３４】
次に、位置推定部１４２Ａは、既に標識として登録された特徴点の位置情報と現在獲得された映像情報から抽出されて既存の標識とマッチングされた特徴点の位置情報を用いて予測段階で予測した内視鏡２１４の位置及び姿勢、ツール２１２ａ、２１２ｂの位置及び姿勢、そして標識に登録された特徴点（ｆｅａｔｕｒｅ）の位置を更新する（３６０）。
【０１３５】
その後、マスタ制御部１４０Ａ内の地図作成部１４４Ａは、位置推定部１４２Ａを介して行われた位置認識結果である内視鏡２１４の位置情報及び姿勢情報、腹腔内映像の特徴点の位置情報から腹腔内環境に対する地図を作成する（３７０）。
【０１３６】
次に、マスタ制御部１４０Ａは、表示部１１６Ａに制御信号を送って地図作成部１４４Ａを介してモデリングされた腹腔内環境と内視鏡／ツールの位置及び姿勢との相対的関係を表示する（３８０）。
【０１３７】
その後、マスタ制御部１４０Ａは、手術ロボットの手術が完了したか否かを判断する（３９０）。マスタ制御部１４０Ａは、入力部１２０Ａを介して操作者から手術ロボットの手術停止命令が入力されるか、またはマスタ操作器１１２Ｌ、１１２Ｒに装着された回転角センサからこれ以上の回転角度情報が入力されなければ手術ロボットの手術が完了されたものと判断する。
【０１３８】
手術ロボットの手術が完了されないと（３９０での「いいえ」）マスタ制御部１４０Ａは動作３００にリターンして映像情報及び慣性測定情報を入力受けて、更新段階で更新された情報を用いて内視鏡及びツールの位置及び姿勢を予測する（３１０、３２０）。一方、手術ロボットの手術が完了したら（３９０での「はい」）マスタ制御部１４０Ａは腹腔内環境に対して最終的に作成された地図を保存部１３０Ａに保存し、手術ロボットに装着された内視鏡及びツールの位置認識過程を終了する。
【符号の説明】
【０１３９】
１００  マスタ装置
１１２  マスタ操作部
１１４  ペダルセンサ
１１６  表示部
２００  スレーブロボット
２０２  マウンティングアーム
２０４  ケーシング
２１０  ガイドチューブ
２１２  ツール
２１４  内視鏡
２１６  エンドエフェクタ
２１８  内視鏡カメラ
【図１】
2015213753.tif 000015
【図２】
2015213753.tif 000016
【図３】
2015213753.tif 000017
【図４】
2015213753.tif 000018
【図５】
2015213753.tif 000019
【図６】
2015213753.tif 000020
【図１０】
2015213753.tif 000021
【図１１】
2015213753.tif 000022
【図７Ａ】
2015213753.tif 000023
【図７Ｂ】
2015213753.tif 000024
【図８Ａ】
2015213753.tif 000025
【図８Ｂ】
2015213753.tif 000026
【図９】
2015213753.tif 000027