JP2015154814A

JP2015154814A - マニピュレータシステムとその制御方法

Info

Publication number: JP2015154814A
Application number: JP2014030516A
Authority: JP
Inventors: 直也畠山; Naoya HATAKEYAMA; 雅敏飯田; Masatoshi Iida; 浩志若井; Hiroshi Wakai
Original assignee: Olympus Corp
Current assignee: Olympus Corp
Priority date: 2014-02-20
Filing date: 2014-02-20
Publication date: 2015-08-27
Also published as: EP3108797A1; EP3108797A4; US20160338784A1; CN105979850A; WO2015125796A1

Abstract

【課題】挿入部が複雑に湾曲した場合においても先端のマニピュレータを精度よく制御する。
【解決手段】軟性の挿入部１０と、挿入部１０の先端において駆動される関節部を有するマニピュレータ６と、挿入部１０の基端側においてマニピュレータ６の関節部を駆動する駆動部７と、挿入部１０の形状を推定する形状推定部と、形状推定部により推定された挿入部１０の形状に基づいて、駆動部７を制御する制御部とを備え、形状推定部が、挿入部１０にその長手方向に間隔をあけて複数配置された被検出体１４と、被検出体１４の３次元位置を検出する位置検出部１５と、位置検出部１５により検出された被検出体１４の３次元位置に基づいて、挿入部１０の形状をその長手方向に並ぶ複数のセクションＳＳに分けて算出する形状算出部とを備えるマニピュレータシステムを提供する。
【選択図】図２

Description

本発明は、マニピュレータシステムとその制御方法に関するものである。

従来、ＵＰＤ装置を用いて内視鏡の湾曲部の形状検出を行い、検出された形状を利用して湾曲部を制御する内視鏡システムが知られている（例えば、特許文献１参照。）。

特開２００９−１３１４０６号公報

しかしながら、軟性内視鏡においては、湾曲部のみならずその基端側に接続する挿入部も挿入経路に応じて複雑に形状を変化させるので、湾曲部の形状を検出して利用する特許文献１の内視鏡システムでは、湾曲部を精度よく制御することが困難であるという不都合がある。

本発明は、上述した事情に鑑みてなされたものであって、挿入部が複雑に湾曲した場合においても先端のマニピュレータを精度よく制御することができるマニピュレータシステムとその制御方法を提供することを目的としている。

上記目的を達成するために、本発明は以下の手段を提供する。
本発明の一態様は、軟性の挿入部と、該挿入部の先端において駆動される関節部を有するマニピュレータと、前記挿入部の基端側において前記マニピュレータの前記関節部を駆動する駆動部と、前記挿入部の形状を推定する形状推定部と、該形状推定部により推定された前記挿入部の形状に基づいて、前記駆動部を制御する制御部とを備え、前記形状推定部が、前記挿入部にその長手方向に間隔をあけて複数配置された被検出体と、各該被検出体の３次元位置を検出する位置検出部と、該位置検出部により検出された各前記被検出体の３次元位置に基づいて、前記挿入部の形状をその長手方向に並ぶ複数のセクションに分けて算出する形状算出部とを備えるマニピュレータシステムを提供する。

本態様によれば、挿入部をその先端側から体腔内に挿入していくと、挿入部は体腔の形状に倣ってその形状を変化させる。このとき、挿入部に配置された被検出体が、挿入部の長手方向に間隔をあけて点在するように配置されるので、位置検出部によって各被検出体の３次元位置を検出することにより、形状推定部によって挿入部の長手方向に沿う形状を推定することができる。

この場合に、形状算出部は、挿入部の形状を長手方向に並ぶ複数のセクションに分けて算出するので、各セクションにおいては単純な形状によって推定しても、全体として複雑な形状に変形された挿入部の形状を精度よく算出することができる。その結果、挿入部の基端側に配置された駆動部による挿入部先端におけるマニピュレータの駆動を、推定された挿入部の形状に基づいて、制御部により精度よく制御することができる。

上記態様においては、前記形状算出部が、隣接する２つの前記被検出体の間に各前記セクションを設定してもよい。
このようにすることで、最大数のセクションを設定することができ、各セクションにおいて挿入部の形状を単純な形状で推定しても、挿入部全体として複雑な形状を推定することができる。

また、上記態様においては、前記形状算出部が、隣接する３つの前記被検出体を含むように各前記セクションを設定してもよい。
このようにすることで、３つの被検出体を通過する曲線によって、挿入部の形状をより精度よく推定することができる。

また、上記態様においては、前記形状算出部が、前記セクション毎に、隣接する２つの前記被検出体の間を単一の円弧で近似してもよい。
このようにすることで、挿入部の形状を、２つの被検出体毎に設定されたセクション毎に、２つの被検出体を通過する円弧によって簡易かつ効果的に推定することができる。

また、上記態様においては、前記形状算出部が、前記セクション毎に、隣接する３つの前記被検出体の間を単一の円弧で近似してもよい。
このようにすることで、挿入部の形状を、３つの被検出体毎に設定されたセクション毎に、３つの被検出体を通過する円弧によって簡易かつ効果的に推定することができる。

また、上記態様においては、前記形状算出部が、隣接する３つ以上の前記被検出体を含むように各前記セクションを設定し、各前記セクション内の前記挿入部の形状を表す曲線の多項式を各前記被検出体の３次元位置からカーブフィッティングにより求めてもよい。

また、本発明の他の態様は、軟性の挿入部の長手方向に並ぶ複数のセクションに分けて該挿入部の形状を算出し、算出された前記挿入部の形状に基づいて、該挿入部の基端側に配置される駆動部による前記挿入部の先端に配置されるマニピュレータの駆動を制御するマニピュレータの制御方法を提供する。
本態様によれば、各セクションにおいては単純な形状によって推定しても、全体として複雑な形状に変形された挿入部の形状を精度よく算出することができる。そして、挿入部の基端側に配置された駆動部による挿入部先端におけるマニピュレータの駆動を、推定された挿入部の形状に基づいて、精度よく制御することができる。

本発明によれば、挿入部が複雑に湾曲した場合においても先端のマニピュレータを精度よく制御することができるという効果を奏する。

本発明の一実施形態に係るマニピュレータシステムを示す全体構成図である。図１のマニピュレータシステムのマニピュレータを示す斜視図である。図２のマニピュレータの制御を説明するブロック図である。図２のマニピュレータの軟性部に取り付けられた磁気コイルの配置を説明する図である。図４の軟性部が湾曲した状態を示す図である。図５の軟性部の１セクションにおける形状を単一の円弧により推定した場合を説明する図である。図６において形状を推定したセクションを模式化した図である。図１のマニピュレータシステムにおける軟性部の（ａ）１セクションをモデル化するための各値を示す図、（ｂ）軟性部のワイヤモデルを示す図である。図８の変形例を示す軟性部のワイヤモデルを示す図である。図１のマニピュレータシステムの変形例であって、３個の磁気コイルを含むセクションによる推定を説明する図である。図１のマニピュレータシステムの変形例であって、重複するセクションによる推定を説明する図である。軟性部の形状推定の他の方法を説明する図である。軟性部の形状推定のさらに他の方法を説明する図である。

本発明の一実施形態に係るマニピュレータシステム１とその制御方法について、図面を参照して以下に説明する。
本実施形態に係るマニピュレータシステム１は、図１に示されるように、医師Ａにより操作されるマスタ装置２と、該マスタ装置２を介した入力により駆動されるスレーブ装置３と、マスタ装置２への入力に基づいてスレーブ装置３を制御するコントローラ４と、モニタ５とを備えている。

スレーブ装置３は、図２に示されるように、患者Ｐの体腔内に挿入される本実施形態に係るマニピュレータ６と、該マニピュレータ６を駆動する駆動部７と、マニピュレータ６の形状を検出するためのセンサシステム８とを備えている。
マニピュレータ６は、湾曲可能な軟性の細長い挿入部９を有する軟性内視鏡であって、細長い軟性部１０と、先端に配置される先端部１１と、これら先端部１１と軟性部１０との間に配置される湾曲部１２とを備えている。

駆動部７は、モータ７ａによって駆動される直動機構１３ａと、該直動機構１３ａに着脱可能に取り付けられて、先端部１１との間を接続するワイヤを内部に収容する接続部１３ｂと、複数の接続部１３ｂを集合させる中継部１３ｃとを備えている。先端部１１は、例えば複数関節を有するアームであり、各関節がワイヤの牽引力によって駆動されるようになっている。
駆動部７のモータ７ａは、図３に示されるように、コントローラ４から送られてくる指令信号に基づいて駆動されるとともに、モータ７ａに備えられた駆動量を検出するエンコーダの出力をフィードバックすることにより指令信号通りの駆動量が達成されるように制御されるようになっている。

センサシステム８は、例えば、軟性部１０に長手方向に間隔をあけて複数配置された磁気コイル（被検出体）１４と、該磁気コイル１４から発生される磁気を受信するアンテナ（形状検出部）１５とを備えている。磁気コイル１４から発せられる磁気をアンテナ１５によって受信することによって、各磁気コイル１４の３次元的な位置情報を取得することができるようになっている。センサシステム８により取得された各磁気コイル１４の３次元的な位置情報は、コントローラ４に送られるようになっている。

コントローラ４は、送られてきた各磁気コイル１４の３次元的な位置情報に基づいて軟性部１０の形状を算出する形状算出部１６と、該形状算出部１６により推定された軟性部１０の形状に基づいて駆動部７を制御する制御信号発生部１７とを備えている。
センサシステム８および形状算出部１６により形状推定部が構成されている。
制御信号発生部１７は、ワイヤの長手方向に沿う変位量を指令信号として駆動部７に出力するようになっている。

以下に、本実施形態に係るマニピュレータシステム１のコントローラ４に設けられた形状算出部１６の動作について説明する。
形状算出部１６は、図４に示されるように、軟性部１０に備えられた磁気コイル１４の軟性部１０に沿う間隔寸法Ｓを予め記憶している。そして、形状算出部１６は、隣接する２つの磁気コイル１４毎にセクションＳＳを設定し、セクションＳＳ毎に軟性部１０の形状を算出するようになっている。

センサシステム８によって取得された各磁気コイル１４の３次元的な位置情報を用いて、図５および図６に示されるように、全ての隣接する２つの磁気コイル１４間の弦の長さLを下式により算出する。
Ｌ＝√（（ｘ_２−ｘ_１）^２＋（ｙ_２−ｙ_１）^２＋（ｚ_２−ｚ_１）^２）
ここで、図５および図６には先端の２つの磁気コイル１４のみ示し、（ｘ_１，ｙ_１，ｚ_１）、（ｘ_２，ｙ_２，ｚ_２）はそれぞれ隣接する２つの磁気コイル１４の座標位置である。

図５のように湾曲させられた隣接する２つの磁気コイル１４間の軟性部１０が、図６および図７に示されるように一様な曲率半径Ｒで湾曲しているものと仮定すると、曲率半径Ｒは下式（１）が成立するように繰り返し計算を行うことにより算出することができる。
Ｌ−２Ｒｓｉｎ（Ｓ／２Ｒ）＝０（１）

これにより、全ての隣接する２つの磁気コイル１４間の軟性部１０が、曲率半径Ｒで湾曲する円弧として推定される。
コントローラ４は駆動部７を制御するため、図８に示されるワイヤモデルに従って、数１に示されるように記述される運動方程式を備えている。ワイヤモデルは、図８（ｂ）に示す例では、各セクションＳＳの質量ｍを有する質点１８を所定の剛性を有するバネ１９で連結したものを仮定して、数１の運動方程式が成立することとしている。ここでは、セクションＳＳの数が３つの場合を例示している。

ここで、
ｍ_１，ｍ_２，ｍ_３は、各セクションＳＳ（１〜３）におけるワイヤの質量、
ｃ_１，ｃ_２，ｃ_３は、各セクションＳＳ（１〜３）におけるワイヤの長手方向に沿う粘性摩擦係数、
ｚ_１′，ｚ_２′，ｚ_３′は、各セクションＳＳ（１〜３）におけるワイヤの長手方向に沿う移動速度、
ｚ_１″，ｚ_２″，ｚ_３″は、各セクションＳＳ（１〜３）におけるワイヤの長手方向に沿う加速度、
Ｔ₀，Ｔ_１，Ｔ_２，Ｔ_３は、各セクションＳＳ（１〜３）におけるワイヤにかかる張力、
ｆ_１，ｆ_２，ｆ_３は、各セクションＳＳ（１〜３）における摩擦力
である。

この運動方程式において、摩擦力ｆ（各摩擦力ｆ_１，ｆ_２，ｆ_３）は下式によって表される。

ここで、
ｓｇｎ（ｚ′）は速度ｚ′の符号を示す関数、
μは動摩擦係数、
ΔＳは弧の長さ、
Ｒは曲率半径,
Ｔは張力
である。

数２において、曲率半径Ｒには、式（１）によって推定された値が用いられる。
この数２によれば、摩擦力ｆは各セクションＳＳを構成している軟性部１０の曲げに応じて変動するものとなっている。したがって、各セクションＳＳを構成している軟性部１０の曲げが変化した場合には、この摩擦力ｆの変化として、コントローラ４に記憶されている運動方程式が修正されるようになっている。

このように構成された本実施形態に係るマニピュレータシステム１の作用について、以下に説明する。
本実施形態に係るマニピュレータシステム１を用いて、患者Ｐの体腔内に配置されている患部の処置を行うには、マニピュレータ６を先端部１１から体腔内に導入していき、先端部１１を患部に対向させる。

このようにするとマニピュレータ６の挿入部を構成している軟性部１０が体腔の形状に倣って複雑に湾曲する。この状態で、センサシステム８の作動により軟性部１０に装着されている磁気コイル１４の３次元的な位置がアンテナ１５によって検出される。

アンテナ１５によって検出された各磁気コイル１４の位置情報はコントローラ４に送られることにより、コントローラ４に備えられている形状算出部１６により軟性部１０の形状が算出される。
この場合において、本実施形態に係るマニピュレータシステム１においては、形状算出部１６が、軟性部１０をその長手方向に並ぶ複数のセクションＳＳ毎に算出する。

本実施形態に示す例では、形状算出部１６は、全ての隣接する２つの磁気センサ１４毎にセクションＳＳを設定し、各セクションＳＳの軟性部１０を単一の円弧によって推定しているので、簡易な推定により複雑な軟性部１０の形状を精度よく算出することができる。そして、セクションＳＳ毎に推定された軟性部１０の曲率半径Ｒに基づいて、数２により摩擦力ｆが算出され、セクションＳＳ毎に算出された摩擦力ｆ_１，ｆ_２，ｆ_３によって数１の運動方程式が修正される。

制御信号発生部１７は、この数１の運動方程式を解くことにより、指令信号としてのワイヤの変位量Ａを算出するとともに、この変位量Ａからこの変位量Aを達成するモータの駆動量に換算した上で、駆動部７に対して出力する。これにより、モータ７ａが指令された駆動量だけ駆動し、所望の変位量だけワイヤが変位させられ、先端部１１を構成しているアームの各関節が精度よく駆動される。

このように、本実施形態に係るマニピュレータシステム１とその制御方法によれば、軟性部１０の先端に配置されているアームを軟性部１０の基端側に配置されているモータ７ａによってワイヤを牽引して駆動する場合に、軟性部１０が複雑な形状に湾曲されている場合であっても、その形状を精度よく推定して、アームの関節を駆動するための指令信号を精度よく算出することができるという利点がある。そして、形状の推定に際して軟性部１０を複数のセクションＳＳに分割し、セクションＳＳ毎に単一の円弧によって簡易に形状を推定しているので、単純な計算により計算に要する時間を抑えて、リアルタイムに制御することができる。

なお、本実施形態においては、セクションＳＳを３つに分けた場合について例示したが、図９および数３に示されるように、セクションＳＳの分割数はいくつでもよい。

また、本実施形態においては、セクションＳＳ毎に単一の円弧を仮定してその曲率半径Ｒを推定することとしたが、これに代えて、曲げ角度θを推定することにしてもよい。
また、曲率半径Ｒあるいは曲げ角度θの推定には演算に時間がかかるので、各セクションＳＳを構成する軟性部１０が直線である場合や同一円弧のような既知形状である場合に、演算を省略してもよい。

例えば、各セクションＳＳにおいて２つの磁気コイル１４間の直線距離によって計算される軟性部１０の弦の長さＬが軟性部１０に沿う方向の磁気コイル１４間の距離に等しい場合にはそのセクションＳＳは直線であると見なすことができ、曲率半径Ｒ等の推定のための繰り返し演算を省略することができる。また、いずれかのセクションＳＳで曲率半径Ｒ等が推定された場合に、その推定された円弧上に隣接するセクションＳＳの磁気コイル１４が配置されている場合には、曲率半径Ｒ等を改めて推定することなく、既に推定された曲率半径Ｒ等を用いることにすればよい。

また、磁気コイル１４の間隔は等間隔であってもよいし、等間隔でなくてもよい。
また、隣接する２つの磁気コイル１４の間にセクションＳＳを設け、２つの磁気コイル１４間の距離を弦の長さＬとして円弧の曲率半径Ｒを推定したが、これに代えて、図１０に示されるように、各セクションＳＳが３以上の磁気コイル１４を含むものとし、これらの磁気コイル１４の位置を通過する円弧によって曲率半径Ｒを推定することにしてもよい。なお、セクションＳＳは、図１０のように独立していてもよいし、図１１のように重複していてもよい。

また、軟性部１０の形状の推定は、スレーブ装置３の作動中、常時リアルタイムに行うことにしてもよいし、スレーブ装置３を作動させるための準備ができたとき、あるいは、定期的に、あるいは必要に応じて手動で行うことにしてもよい。

また、本実施形態においては、図１２に示されるように、３以上の磁気コイル１４の位置情報から曲線Ｃの多項式をカーブフィッティングによって求め、各磁気コイル１４の位置における曲線Ｃの接線を求め、隣接する接線の傾斜角度α、α＋Δαの差分Δαを曲げ角度として演算してもよい。この場合、磁気コイル１４間の弧長ΔＳが既知であるとすると、曲率半径Ｒは、
Ｒ＝ΔＳ／Δα
として求めることができる。

また、図１３に示されるように、セクションＳＳ毎に軟性部１０を表す曲線Ｃを弧長Ｓで表現し、その接線ベクトルｔ、ｔ＋ｄｔの差分ｄｔを弧長ｄｓで微分することにより下式のように曲率κを求めることにしてもよい。
κ＝｜ｄｔ／ｄｓ｜

また、本実施形態においては、センサシステム８として、軟性部１０に設けた磁気コイル１４とアンテナ１５とからなる内視鏡挿入形状観測装置（ＵＰＤ；ＥｎｄｏｓｃｏｐｅＰｏｓｉｔｉｏｎＤｅｔｅｃｔｉｎｇＵｎｉｔ）を例示したが、これに代えて、ＣＴやＭＲＩ等の術前の臓器形状の情報から、挿入後の軟性部１０の形状を予測してもよい。また、軟性部１０にひずみセンサや光ファイバセンサ等の曲げを検出するセンサを取り付けて形状を検出してもよい。
また、挿入部９の先端における関節部として、アームを有する先端部１１を例示したが、先端部１１が設けられていない軟性内視鏡であってもよい。この場合、湾曲部１２が挿入部の先端における関節部を成す。
また、マニピュレータ６として、軟性内視鏡を例示したが、軟性処置具であっても軟性のオーバーチューブであってもよい。

ＳＳセクション
１マニピュレータシステム
６マニピュレータ
７駆動部
１０軟性部（挿入部）
１４磁気コイル（被検出体）
１５アンテナ（位置検出部、形状推定部）
１６形状算出部（形状推定部）
１７制御信号発生部（制御部）

Claims

軟性の挿入部と、
該挿入部の先端において駆動される関節部を有するマニピュレータと、
該マニピュレータの前記関節部を駆動する駆動部と、
前記挿入部の形状を推定する形状推定部と、
該形状推定部により推定された前記挿入部の形状に基づいて、前記駆動部を制御する制御部とを備え、
前記形状推定部が、前記挿入部にその長手方向に間隔をあけて複数配置された被検出体と、各該被検出体の３次元位置を検出する位置検出部と、該位置検出部により検出された各前記被検出体の３次元位置に基づいて、前記挿入部の形状をその長手方向に並ぶ複数のセクションに分けて算出する形状算出部とを備えるマニピュレータシステム。
前記形状算出部が、隣接する２つの前記被検出体の間に各前記セクションを設定する請求項１に記載のマニピュレータシステム。
前記形状算出部が、隣接する３つの前記被検出体を含むように各前記セクションを設定する請求項１に記載のマニピュレータシステム。
前記形状算出部が、前記セクション毎に、隣接する２つの前記被検出体の間を単一の円弧で近似する請求項２に記載のマニピュレータシステム。
前記形状算出部が、前記セクション毎に、隣接する３つの前記被検出体の間を単一の円弧で近似する請求項３に記載のマニピュレータシステム。
前記形状算出部が、隣接する３つ以上の前記被検出体を含むように各前記セクションを設定し、各前記セクション内の前記挿入部の形状を表す曲線の多項式を各前記被検出体の３次元位置からカーブフィッティングにより求める請求項１に記載のマニピュレータシステム。
軟性の挿入部の長手方向に並ぶ複数のセクションに分けて該挿入部の形状を算出し、
算出された前記挿入部の形状に基づいて、該挿入部の基端側に配置される駆動部による前記挿入部の先端に配置されるマニピュレータの駆動を制御するマニピュレータの制御方法。