JP2018176406A - 連続体ロボットの制御システム及びその制御方法、並びに、プログラム - Google Patents

Abstract

【課題】連続体ロボットの湾曲部を駆動させる際に、当該湾曲部の湾曲形状の基準面となる部材と当該湾曲部内の最も基準面に近い部材との接触を防止する仕組みを提供する。【解決手段】切替判定部１２２は、運動学演算部１２１で算出された、湾曲部の湾曲形状の基準面となる部材と当該湾曲部内の最も基準面に近い部材との距離を表す距離情報である座標値ｚe1（θe）に応じて、当該座標値とともに運動学演算部１２１で算出された当該湾曲部を駆動させるためのワイヤの駆動変位量に基づくワイヤの駆動を行うか否かを判定する。【選択図】図１

Description

本発明は、連続体ロボットの制御システム及びその制御方法、並びに、当該制御システムとしてコンピュータを機能させるためのプログラムに関するものである。

連続体ロボットは、コンティニュウムロボットとも呼ばれ、柔軟性のある構造を持つ湾曲部を有し、その湾曲部を変形させることにより形状が制御される。この連続体ロボットは、剛体リンクにより構成されるロボットに対して、主に２つの優位性を持つ。１つ目は、連続体ロボットは、剛体リンクロボットがはまり込んでしまうような狭い空間や散乱物のある環境の中で、曲線に沿って移動可能である。２つ目は、連続体ロボットは、本質的な柔らかさを有するため、脆弱な対象物に損傷を与えることなく操作することができる。そこでは、剛体リンクロボットで必要とされる外力の検出などは必ずしも必要とされない。

この特徴を生かし、連続体ロボットは、内視鏡のシースやカテーテルなどの医療の分野や、レスキューロボットなどの極限作業ロボットへの応用が期待されている。この連続体ロボットの駆動方法としては、腱駆動によるもの、押引き可能なワイヤによるもの、空気アクチュエータによるものなどがある。非特許文献１では、３本のワイヤと、それをガイドするベースディスク、スペーサーディスク、エンドディスクにより連続体ロボットを構成し、運動学を導出することでワイヤの押し引き駆動量を求め、形状の制御を行っている。

K. Xu, M. Fu, and J. Zhao, "An Experimental Kinestatic Comparison between Continuum Manipulators with Structural Variations," in IEEE International Conference on Robotics and Automation (ICRA), Hong Kong, China, 2014, pp. 3258-3264.

非特許文献１では、それぞれのディスクの円周の近くにワイヤをガイドする孔が設けられている。また、非特許文献１では、運動学の導出においては、連続体ロボットの中心を通る仮想的なワイヤを考え、その長さが常に一定となるように実際のワイヤの駆動量を求めている。そのため、非特許文献１では、或る湾曲部の湾曲形状の基準面となる部材と当該湾曲部内の最も基準面に近い部材とが接触をしてしまうおそれがあった。

本発明は、このような問題点に鑑みてなされたものであり、連続体ロボットの湾曲部を駆動させる際に、当該湾曲部の湾曲形状の基準面となる部材と当該湾曲部内の最も基準面に近い部材との接触を防止する仕組みを提供することを目的とする。

本発明の連続体ロボットの制御システムは、基準面を介して延伸する複数のワイヤと、前記複数のワイヤが異なる位置に固定され、前記複数のワイヤを案内する第１のワイヤガイドと、前記基準面と前記第１のワイヤガイドとの間に設けられ、前記複数のワイヤのうちの一部のワイヤが固定された、前記複数のワイヤを案内する第２のワイヤガイドとを含み形成され、前記複数のワイヤのうちの少なくとも一部の所定ワイヤを駆動することにより湾曲可能な湾曲部を、有する連続体ロボットの制御システムであって、前記湾曲部の湾曲角度の目標値である目標湾曲角度および当該湾曲部の旋回角度の目標値である目標旋回角度の入力に応じて、前記所定ワイヤの駆動変位量と、前記第２のワイヤガイドにおける前記基準面に近い側の端部である近位端と当該基準面との距離を表す距離情報とを算出する算出手段と、前記所定ワイヤの駆動を制御する制御手段と、を有し、前記制御手段は、前記距離情報とともに前記算出手段で算出された前記駆動変位量に基づいて前記所定ワイヤの駆動を行うか否かを、前記距離情報に応じて判定する判定手段を有する。
また、本発明は、上述した連続体ロボットの制御方法、及び、上述した連続体ロボットの制御システムとしてコンピュータを機能させるためのプログラムを含む。

本発明によれば、連続体ロボットの湾曲部を駆動させる際に、当該湾曲部の湾曲形状の基準面となる部材と当該湾曲部内の最も基準面に近い部材との接触を防止することができる。

本発明の第１の実施形態に係る連続体ロボットの制御システムの構成例を示す模式図である。 図１に示す連続体ロボットの構成例を示す模式図である。 図２に示すワイヤの配置をｘｙ面において定義するための模式図である。 図２に示す続体ロボットの湾曲部をｗｚ面へ投影した場合の一例を示す模式図である。 本発明の第１の実施形態に係る連続体ロボットの制御システムによる制御方法の処理手順の一例を示すフローチャートである。 本発明の第１の実施形態を示し、各旋回角度と最大湾曲角度との関係をシミュレーションした結果を示す図である。 本発明の第２の実施形態に係る連続体ロボットの制御システムの構成例を示す模式図である。 図７に示す連続体ロボットの構成例を示す模式図である。 図７に示す連続体ロボットの湾曲形状の一例を示す図である。 本発明の第２の実施形態を示し、各旋回角度と最大湾曲角度との関係をシミュレーションした結果を示す図である。 本発明の第３の実施形態に係る連続体ロボットの制御システムの構成例を示す模式図である。

以下に、本発明の各実施形態について、図面を参照して詳細に説明する。本発明の実施形態では、連続体ロボット（連続体マニピュレータともいう）の制御システムが可撓性内視鏡に適用される例を示す。近年、例えば医療分野においては、患者の負担を低減し治療・検査後のＱＯＬを向上させるための低侵襲医療が注目を集めており、低侵襲医療の例として、可撓性内視鏡を用いた手術・検査が挙げられる。可撓性内視鏡は、挿入部が湾曲可能な部材で構成されているため、内視鏡先端の湾曲部の湾曲角度を操作することで広い範囲をくまなく観察できる。なお、本発明の実施形態に係る連続体ロボットの制御システムの例として適用する可撓性内視鏡は、医療分野に限定されるものではなく、湾曲部を挿入・抜去させる経路（以下、「挿抜経路」を称する）の内部を観察するものであれば他の分野（例えば、配管等の内部を観察するための工業用内視鏡）にも適用可能である。

（第１の実施形態）
まず、本発明の第１の実施形態について説明する。

＜連続体ロボットの制御システムの構成例＞
図１は、本発明の第１の実施形態に係る連続体ロボットの制御システム１００の構成例を示す模式図である。連続体ロボットの制御システム１００は、図１に示すように、入力装置１１０と、制御装置１２０と、連続体ロボット１３０とを有して構成されている。

≪連続体ロボットの構成例≫
まず、図１に示す連続体ロボット１３０の構成例について説明する。
図２は、図１に示す連続体ロボット１３０の構成例を示す模式図である。連続体ロボット１３０は、図２に示すように、基台部１３１０及び湾曲部１３２０を含み形成されている。また、図２に示す連続体ロボット１３０は、湾曲部１３２０の長軸方向に移動可能に構成されている。

湾曲部１３２０は、当該湾曲部１３２０の基準面に相当する基台部１３１０の上面１３１４を介して延伸する複数のワイヤ１３０１〜１３０３と、複数のワイヤ１３０１〜１３０３が異なる位置に固定され、複数のワイヤ１３０１〜１３０３を案内する第１のワイヤガイド１３２１と、基台部１３１０の上面１３１４と第１のワイヤガイド１３２１との間に設けられ、複数のワイヤ１３０１〜１３０３のうちの一部のワイヤ１３０１が所定の位置に固定された、複数のワイヤ１３０１〜１３０３を案内する第２のワイヤガイド１３２２〜１３２５と、を含み形成されている。具体的に、第１のワイヤガイド１３２１は、位置１３２１１にワイヤ１３０１が固定され、位置１３２１２にワイヤ１３０２が固定され、位置１３２１３にワイヤ１３０３が固定されている。また、第２のワイヤガイド１３２２は位置１３２２１にワイヤ１３０１が固定され、第２のワイヤガイド１３２３は位置１３２３１にワイヤ１３０１が固定され、第２のワイヤガイド１３２４は位置１３２４１にワイヤ１３０１が固定され、第２のワイヤガイド１３２５は位置１３２５１にワイヤ１３０１が固定されている。なお、図２に示すように、湾曲部１３２０の基準面に相当する基台部１３１０の上面１３１４には、複数のワイヤ１３０１〜１３０３は固定されていない。

そして、図２に示す連続体ロボット１３０では、複数のワイヤ１３０１〜１３０３のうちの一部の所定ワイヤであるワイヤ１３０２及び１３０３を駆動することにより、湾曲部１３２０を所望の湾曲形状（例えば、円弧状）に湾曲可能に構成されている。

ここで、以下の説明においては、湾曲部の各ワイヤガイドにおいて、当該湾曲部の長軸方向について当該湾曲部の基準面から遠い側の端部を「遠位端」と称し、当該湾曲部の基準面から近い側の端部を「近位端」と称する。具体的に、湾曲部１３２０は、基台部１３１０の上面１３１４（厳密に言えば、上面１３１４自体は含まない）から第１のワイヤガイド１３２１の遠位端１３２１４に至るまでとなっている。ここで、図２に示す例では、第１のワイヤガイド１３２１の遠位端１３２１４の異なる位置１３２１１〜１３２１３にそれぞれワイヤ１３０１〜１３０３が固定されているものとする。また、例えば、ワイヤ１３０１が固定される位置１３２２１〜１３２５１は、それぞれ、第２のワイヤガイド１３２２〜１３２５の近位端に設けられている。

また、図２に示す連続体ロボット１３０は、第１のワイヤガイド１３２１よりも上面１３１４から遠い方にカメラ（不図示）が設けられており、湾曲部１３２０を挿抜経路４００に沿って挿入することにより、挿抜経路４００の内部を観察可能となっている。なお、図２では、離散的に配置された複数のワイヤガイドを配置する例を示したが、蛇腹状やメッシュ状等の連続体のワイヤガイドを配置するようにしてもよい。

基台部１３１０の内部には、ワイヤ１３０２を押し引き駆動させるためのアクチュエータ１３１１と、ワイヤ１３０３を押し引き駆動させるためのアクチュエータ１３１２と、ワイヤ１３０１の一端を固定する固定部１３１３が設けられている。すなわち、図２に示す連続体ロボット１３０では、アクチュエータ１３１１及びアクチュエータ１３１２を制御してワイヤ１３０２及び１３０３を駆動することにより、湾曲部１３２０を所望の湾曲形状（例えば、円弧状）に湾曲可能に構成されている。

また、図２には、連続体ロボット１３０（より詳細に、図２に示す例では湾曲部１３２０）の中心軸１３０４を破線で示している。また、図２では、湾曲部１３２０の基準面に相当する基台部１３１０の上面１３１４について、その中心を原点Ｏとするとともに、その面内で原点Ｏを通り相互に直交するｘ軸及びｙ軸を図示している。また、図２では、基台部１３１０の上面１３１４と直交するｚ軸を図示している。また、図２では、アクチュエータ１３１１によるワイヤ１３０２の駆動変位量ｌ_p1b、及び、アクチュエータ１３１２によるワイヤ１３０３の駆動変位量ｌ_p1cを図示している。また、図２では、第１のワイヤガイド１３２１の遠位端１３２１４において、中心軸１３０４が通る位置１３２１５を図示している。また、図２では、位置１３２１５において、中心軸１３０４とｚ軸と平行な線分とのなす角度を湾曲部１３２０の湾曲角度θ₁として図示している。また、図２では、中心軸１３０４をｘｙ面に投影したｗ軸を図示し、また、位置１３２１５をｘｙ面に投影した位置１３０５を図示している。また、図２では、ｘ軸とｗ軸とのなす角度を湾曲部１３２０の旋回角度ζ₁として図示している。また、図２では、中心軸１３０４に係る湾曲部１３２０の曲率半径ρ₁を図示している。また、図２では、第１のワイヤガイド１３２１の遠位端１３２１４から第２のワイヤガイド１３２５の近位端１３２５２までのワイヤ１３０１の長さｌ_aw1を図示している。

ここで、再び、図１の説明に戻る。
入力装置１１０は、湾曲部１３２０の湾曲形状パラメータである、湾曲部１３２０の湾曲角度θ₁の目標値である目標湾曲角度θ_ref1、及び、湾曲部１３２０の旋回角度ζ₁の目標値である目標旋回角度ζ_ref1を、制御装置１２０に対して入力する。また、入力装置１１０は、目標湾曲角度θ_ref1及び目標旋回角度ζ_ref1からなる湾曲形状パラメータを時系列で（例えば、所定の離散時間ステップごとに）順次入力することが可能に構成されている。この際、入力装置１１０は、異なる値の目標湾曲角度θ_ref1及び目標旋回角度ζ_ref1からなる湾曲形状パラメータを時系列で順次入力することも可能である。

制御装置１２０は、入力装置１１０から入力された湾曲形状パラメータに応じて、連続体ロボット１３０の駆動制御を行う。この制御装置１２０は、図１に示すように、運動学演算部１２１と、切替判定部１２２と、第１の切替部１２３と、記憶部１２４と、第２の切替部１２５を有して構成されている。具体的に、制御装置１２０は、３本のワイヤ１３０１〜１３０３のうちの２本のワイヤ１３０２及び１３０３を駆動することにより立体的に形状の制御が可能な連続体ロボット１３０の運動学に基づくモデルを導出する。なお、本明細書において、運動学に基づくモデルを運動学とも称する。そして、制御装置１２０は、湾曲部１３２０の基準面に相当する基台部１３１０の上面１３１４と、基台部１３１０の上面１３１４に最も近い第２のワイヤガイド１３２５の近位端１３２５２との接触を回避するための制御を行う。

運動学演算部１２１は、入力装置１１０からの目標湾曲角度θ_ref1及び目標旋回角度ζ_ref1の入力に応じて、ワイヤ１３０２の駆動変位量ｌ_p1b及びワイヤ１３０３の駆動変位量ｌ_p1cを算出する。さらに、運動学演算部１２１は、目標湾曲角度θ_ref1及び目標旋回角度ζ_ref1の入力に応じて、第２のワイヤガイド１３２５の近位端１３２５２と基台部１３１０の上面１３１４との距離を表す距離情報である座標値ｚ_e1（θ_e）を算出する。ここで、座標値ｚ_e1（θ_e）は、基準面の原点Ｏを０の値としたｚ軸方向の値を示しているものとする。また、座標値ｚ_e1（θ_e）は、図２に示す第２のワイヤガイド１３２５の近位端１３２５２における外周上の全ての点のｚ軸方向の値を示しているものとする。このワイヤ１３０２の駆動変位量ｌ_p1b及びワイヤ１３０３の駆動変位量ｌ_p1cと、距離情報である座標値ｚ_e1（θ_e）を算出する運動学演算部１２１は、本発明の「算出手段」に相当する構成の一例である。

切替判定部１２２は、当該座標値ｚ_e1（θ_e）とともに運動学演算部１２１で算出された駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cのそれぞれに基づいてワイヤ１３０２及び１３０３の駆動を行うか否かを、運動学演算部１２１で算出された座標値ｚ_e1（θ_e）に応じて判定する。具体的に、切替判定部１２２は、座標値ｚ_e1（θ_e）が閾値以下であるか否かを判断し、当該判断の結果に基づいて、上述した駆動制御を行う。ここで、閾値として、例えば０を設定すれば、座標値ｚ_e1（θ_e）が閾値である０以下の場合には、基台部１３１０の上面１３１４と第２のワイヤガイド１３２５の近位端１３２５２とが接触することが想定される。この基台部１３１０の上面１３１４と第２のワイヤガイド１３２５の近位端１３２５２との接触の観点からは、閾値として０を設定することが好適である。しかしながら、本発明においてはこの態様に限定されるものではなく、接触に係る安全率を考慮して、閾値として所定の正の値を設定する態様も、本発明に適用可能である。

座標値ｚ_e1（θ_e）が閾値以下でない場合、切替判定部１２２は、基台部１３１０の上面１３１４と第２のワイヤガイド１３２５の近位端１３２５２とが接触するおそれが無いと判定し、第１の切替部１２３及び第２の切替部１２５を図１に示すように切り替える。具体的に、この場合、切替判定部１２２は、第２の切替部１２５を図１に示すように切り替えて、当該座標値ｚ_e1（θ_e）とともに運動学演算部１２１で算出された駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cを連続体ロボット１３０に出力する制御を行う。これにより、連続体ロボット１３０では、当該座標値ｚ_e1（θ_e）とともに運動学演算部１２１で算出された駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cのそれぞれに基づくワイヤ１３０２及び１３０３の駆動が行われることになる。また、この場合、切替判定部１２２は、第１の切替部１２３を図１に示すように切り替えて、当該座標値ｚ_e1（θ_e）とともに運動学演算部１２１で算出された駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cを記憶部１２４に記憶する制御を行う。このように、記憶部１２４には、座標値ｚ_e1（θ_e）が閾値以下でなかった場合（すなわち、接触するおそれが無いと判定された場合）に当該座標値ｚ_e1（θ_e）とともに運動学演算部１２１で算出された駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cが記憶される。

一方、座標値ｚ_e1（θ_e）が閾値以下である場合、切替判定部１２２は、基台部１３１０の上面１３１４と第２のワイヤガイド１３２５の近位端１３２５２とが接触するおそれが有ると判定し、第１の切替部１２３及び第２の切替部１２５を図１に示すものと逆方向に切り替える。すなわち、この場合、切替判定部１２２は、当該座標値ｚ_e1（θ_e）とともに運動学演算部１２１で算出された駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cのそれぞれに基づくワイヤ１３０２及び１３０３の駆動を行わないように制御する。具体的に、この場合、まず、切替判定部１２２は、第１の切替部１２３を図１に示すものと逆方向に切り替えて、当該座標値ｚ_e1（θ_e）とともに運動学演算部１２１で算出された駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cを記憶部１２４に記憶しない制御を行う。これにより、記憶部１２４には、座標値ｚ_e1（θ_e）が閾値以下でなかった場合（すなわち、接触するおそれが無いと判定された場合）に当該座標値ｚ_e1（θ_e）とともに運動学演算部１２１で算出された駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cのみが記憶される。また、この場合、切替判定部１２２は、第２の切替部１２５を図１に示すものと逆方向に切り替えて、当該座標値ｚ_e1（θ_e）とともに運動学演算部１２１で算出された駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cを連続体ロボット１３０に出力しない制御を行う。これにより、連続体ロボット１３０では、当該座標値ｚ_e1（θ_e）とともに運動学演算部１２１で算出された駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cのそれぞれに基づくワイヤ１３０２及び１３０３の駆動が行われず、上述した接触を回避することができる。より詳細に、この場合、切替判定部１２２は、第２の切替部１２５を図１に示すものと逆方向に切り替えて、予め記憶部１２４に記憶されている、以前に運動学演算部１２１で算出され座標値ｚ_e1（θ_e）が閾値以下でなかった場合（すなわち、接触するおそれが無いと判定された場合）に当該座標値ｚ_e1（θ_e）とともに算出された駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cを連続体ロボット１３０に出力する制御を行う。これにより、連続体ロボット１３０では、上述した接触を回避しつつ、湾曲部１３２０の駆動が行われることになる。

なお、本実施形態においては、記憶部１２４に、運動学演算部１２１で算出された所定の駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cを記憶するようにしているが、本発明においてはこの形態に限定されるものではない。例えば、運動学演算部１２１で算出されたものではなく、上述した接触を回避できることが検証された駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cを予め記憶しておく形態も、本発明に適用可能である。また、切替判定部１２２は、本発明の「判定手段」に相当する構成の一例である。

図１に示す制御装置１２０の運動学演算部１２１、切替判定部１２２、第１の切替部１２３、記憶部１２４及び第２の切替部１２５は、ハードウェアで構成されていても、ソフトウェアで構成されていてもよい。ここで、図１に示す制御装置１２０の各構成部１２１〜１２５がソフトウェアで構成されている場合には、例えばコンピュータのＣＰＵがＲＯＭ等の記憶媒体に記憶されているプログラムを実行することにより、各構成部１２１〜１２５の機能が実現される。

次いで、制御装置１２０の具体的な処理例について説明する。
以下の説明では、原点Ｏから位置１３２１５までの中心軸１３０４の長さを湾曲部１３２０の中心軸の長さｌ_1dとする。この湾曲部１３２０の中心軸の長さｌ_1dは、当該湾曲部１３２０の湾曲形状に応じて変化する。また、第１のワイヤガイド１３２１の遠位端１３２１４の座標を（ｘ_t1，ｙ_t1，ｚ_t1）とする。また、ワイヤ１３０１の長さをｌ_1aとし、また、ｘｙ面内において反時計回りに順次配置されているワイヤ１３０２及び１３０３の長さをそれぞれｌ_1b及びｌ_1cとする。

さらに、本実施形態においては、ｚ軸及びｙ軸回りの回転行列を、それぞれ、以下に示すように、Ｒ_z（θ）及び、Ｒ_y（θ）と表す。

また、制御装置１２０の具体的な処理例では、以下に示す仮定に基づき、連続体ロボットの運動学を導出する。
［１］湾曲部（湾曲部が複数ある場合には、各湾曲部）において、ワイヤは、曲率一定に変形する。
［２］ワイヤのねじり変形は考慮しない。
［３］ワイヤは、長手方向に変形しない。
［４］ワイヤガイドとワイヤとの間の摩擦は考慮しない。

本実施形態では、まず、湾曲部１３２０の湾曲角度θ₁と第１のワイヤガイド１３２１の遠位端１３２１４の座標（ｘ_t1，ｙ_t1，ｚ_t1）との関係を導出する。

湾曲部１３２０の湾曲角度θ₁は、図２に示すｗｚ面内の第１のワイヤガイド１３２１の遠位端１３２１４における湾曲角度に相当する。ここで、ｗｚ面内における湾曲部１３２０の遠位端１３２１４の座標をｗ_t1,ｚ_t1とすると、座標ｗ_t1及びｚ_t1は、上述した湾曲部１３２０の湾曲角度θ₁及び湾曲部１３２０の曲率半径ρ₁を用いて、それぞれ、以下の（１）式及び（２）式で表すことができる。

また、上述した湾曲部１３２０の中心軸の長さｌ_1dは、以下の（３）式で表すことができる。

そして、この（３）式を、上述した（１）式及び（２）式に代入すると、それぞれ、以下の（４）式及び（５）式が導かれる。

ここで、本実施形態においては、３本のワイヤ１３０１〜１３０３のうちの２本のワイヤ１３０２及び１３０３を駆動させることを考えると、旋回角度ζ₁の変動により湾曲部１３２０の中心軸の長さｌ_1dが変動してしまい、（４）式及び（５）式では遠位端１３２１４の座標が求まらない。そこで、本実施形態では、図３に示すようにｘｙ面を用いてワイヤ１３０１〜１３０３の配置を定義し、湾曲部１３２０の中心軸の長さｌ_1dを求める。

図３は、図２に示すワイヤ１３０１〜１３０３の配置をｘｙ面において定義するための模式図である。図３では、各ワイヤ１３０１〜１３０３は、一片の長さがｒ_sの正三角形の頂点に配置されており、位相角ξ_n（本実施形態では、位相角ξ₁とする）は各ワイヤ１３０１〜１３０３の配置を決定する角度である。本実施形態では、ξ₁＝０とする。

ここで、連続体ロボット１３０の湾曲部１３２０をｗｚ面へ投影することを考える。
図４は、図２に示す連続体ロボット１３０の湾曲部１３２０をｗｚ面へ投影した場合の一例を示す模式図である。この図４において、図２に示す構成と同様の構成については同じ符号を付している。この図４に示す、ワイヤ１３０１のｗｚ面における原点０からの距離ｒ_1a、ワイヤ１３０２のｗｚ面における原点０からの距離ｒ_1b、及び、ワイヤ１３０３のｗｚ面における原点０からの距離ｒ_1cは、それぞれ、以下の（６−１）式、（６−２）式及び（６−３）式のように表せる。

ここで、固定ワイヤであるワイヤ１３０１の長さは不変であるため、ワイヤ１３０１の長さをｌ_1aは、図４と（６−１）式により、以下の（７）式のように表せる。

これより、ｗｚ面における湾曲部１３２０の中心軸の長さｌ_1dは、以下の（８）式のように表せる。

この（８）式を、上述した（４）式及び（５）式に代入すると、それぞれ、以下の（９）式及び（１０）式が導かれる。

これより、ｘｙｚ空間における第１のワイヤガイド１３２１の遠位端１３２１４の座標（ｘ_t1，ｙ_t1，ｚ_t1）は、それぞれ、以下の（１１）式〜（１３）となる。

なお、ワイヤ１３０２の長さｌ_1b、及び、ワイヤ１３０３の長さｌ_1cは、それぞれ、以下の（１４）式及び（１５）となる。

本実施形態では、第１のワイヤガイド１３２１の遠位端１３２１４の中心座標に加えて、遠位端１３２１４から長さｌ_aw1においてワイヤ１３０１に固定されている第２のワイヤガイド１３２５の近位端１３２５２の外周上の座標（ｘ_e1，ｙ_e1，z_e1）を求める。

この際、上述したワイヤ１３０１の長さｌ_1a及び長さｌ_aw1を用いて長さｌ_w1と、更には、湾曲部１３２０の湾曲角度θ₁を用いて角度θ_w1とを、以下の（１６）式のように定義する。

そして、この（１６）式の関係を用いて、第２のワイヤガイド１３２５の近位端１３２５２における中心座標（ｘ_e10，ｙ_e10，ｚ_e10）は、以下の（１７）式〜（１９）式となる。

このことから、第２のワイヤガイド１３２５の近位端１３２５２における外周上の座標は、以下の（２０）式のようになる。なお、（２０）式において、角度θ_eは、（２１）式に示す範囲を採る。

次いで、湾曲部１３２０におけるワイヤ１３０２の駆動変位量ｌ_p1b及びワイヤ１３０３の駆動変位量ｌ_p1cと湾曲部１３２０の湾曲角度θ₁との関係を導出する。ｗｚ面内において、湾曲部１３２０のみを考えると、ワイヤ１３０２の駆動変位量ｌ_p1bと湾曲部１３２０の湾曲角度θ₁との関係、及び、ワイヤ１３０３の駆動変位量ｌ_p1cと湾曲部１３２０の湾曲角度θ₁との関係は、それぞれ、以下の（２２）式及び（２３）式となる。

運動学演算部１２１は、入力装置１１０から目標湾曲角度θ_ref1及び目標旋回角度ζ_ref1が入力されると、これらをそれぞれ、（２２）式及び（２３）式のθ₁及びζ₁に代入して、駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cを算出する。さらに、運動学演算部１２１は、（１６）式に示す長さｌ_aw1を設定して、（１６）式〜（２０）式を用いることにより、（２０）式に示す座標値ｚ_e1（θ_e）を算出する。

＜連続体ロボットの制御方法の処理手順例＞
次に、本実施形態に係る連続体ロボットの制御システム１００による制御方法の処理手順の一例について説明する。
図５は、本発明の第１の実施形態に係る連続体ロボットの制御システム１００による制御方法の処理手順の一例を示すフローチャートである。

まず、ステップＳ１０１において、運動学演算部１２１は、入力装置１１０から目標湾曲角度θ_ref1及び目標旋回角度ζ_ref1の入力を受け付ける。そして、入力装置１１０から目標湾曲角度θ_ref1及び目標旋回角度ζ_ref1が入力されると、ステップＳ１０２に進む。

ステップＳ１０２に進むと、運動学演算部１２１は、ステップＳ１０１で入力された目標湾曲角度θ_ref1及び目標旋回角度ζ_ref1に応じて、ワイヤ１３０２の駆動変位量ｌ_p1b及びワイヤ１３０３の駆動変位量ｌ_p1cを算出する。

続いて、ステップＳ１０３において、運動学演算部１２１は、ステップＳ１０１で入力された目標湾曲角度θ_ref1及び目標旋回角度ζ_ref1に応じて、第２のワイヤガイド１３２５の近位端１３２５２と基台部１３１０の上面１３１４との距離を表す距離情報である座標値ｚ_e1（θ_e）を算出する。

続いて、ステップＳ１０４において、切替判定部１２２は、ステップＳ１０３で算出された座標値ｚ_e1（θ_e）が閾値以下であるか否かを判断する。

ステップＳ１０４の判断の結果、ステップＳ１０３で算出された座標値ｚ_e1（θ_e）が閾値以下でない場合には（Ｓ１０４／Ｎｏ）、ステップＳ１０５に進む。このステップＳ１０５に進む場合とは、切替判定部１２２によって、基台部１３１０の上面１３１４と第２のワイヤガイド１３２５の近位端１３２５２とが接触するおそれが無いと判定された場合である。

ステップＳ１０５に進むと、切替判定部１２２は、第１の切替部１２３を図１に示すように切り替えて、ステップＳ１０２で算出された駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cを第１の切替部１２３を介して記憶部１２４に記憶する制御を行う。同時に、切替判定部１２２は、第２の切替部１２５を図１に示すように切り替えて、ステップＳ１０２で算出された駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cを第２の切替部１２５を介して連続体ロボット１３０に出力する制御を行う。また、記憶部１２４には、上述した接触のおそれが無い駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cが記憶される。

一方、ステップＳ１０４の判断の結果、ステップＳ１０３で算出された座標値ｚ_e1（θ_e）が閾値以下である場合には（Ｓ１０４／Ｙｅｓ）、ステップＳ１０６に進む。このステップＳ１０６に進む場合とは、切替判定部１２２によって、基台部１３１０の上面１３１４と第２のワイヤガイド１３２５の近位端１３２５２とが接触するおそれが有ると判定された場合である。

ステップＳ１０６に進むと、切替判定部１２２は、第２の切替部１２５を図１に示すものと逆方向に切り替えて、ステップＳ１０２で算出された駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cを第２の切替部１２５を介して連続体ロボット１３０に出力しないように制御する。同時に、切替判定部１２２は、第１の切替部１２３を図１に示すものと逆方向に切り替えて、ステップＳ１０２で算出された駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cを記憶部１２４に記憶しないように制御するとともに、予め記憶部１２４に記憶されている、上述した接触のおそれが無い駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cを、第２の切替部１２５を介して連続体ロボット１３０に出力する制御を行う。

ステップＳ１０５の処理が終了した場合、或いは、ステップＳ１０６の処理が終了した場合には、ステップＳ１０７に進む。
ステップＳ１０７に進むと、制御装置１２０は、ステップＳ１０５或いはステップＳ１０６において第２の切替部１２５から出力した（上述した接触のおそれが無い）駆動変位量ｌ_p1b及び駆動変位量ｌ_p1cのそれぞれに基づきワイヤ１３０２及び１３０３の駆動を制御し、連続体ロボット１３０の駆動制御を行う。

ステップＳ１０７の処理が終了すると、ステップＳ１０１に戻り、入力装置１１０から次の目標湾曲角度θ_ref1及び目標旋回角度ζ_ref1が入力されるまで待機する。

＜湾曲角度θ₁の最大値＞
次いで、運動学演算部１２１による運動学を用いて、湾曲角度θ₁の最大値を算出する場合について説明する。
湾曲角度θ₁の最大値を算出する場合、湾曲角度θ₁の範囲は、第１のワイヤガイド１３２１の遠位端１３２１４における基台部１３１０の上面１３１４との接触と、第２のワイヤガイド１３２５の近位端１３２５２における基台部１３１０の上面１３１４との接触により定まる。ここでは、前者による湾曲角度θ₁の制約を２７０度に設定し、本実施形態では、後者が原因となる湾曲角度θ₁の最大値を算出する。この場合、例えば、旋回角度ζ₁を０度から１度刻みで３５９度まで変化させ、各旋回角度ζ₁において湾曲角度θ₁を０度から1度刻みで２７０度まで変化させる。

上述した（２０）式により座標値ｚ_e1（θ_e）を算出し、それが閾値（ここでは０とする）以上となる範囲の最大湾曲角度をθ_1maxとして、シミュレーションした結果を図６に示す。すなわち、図６は、本発明の第１の実施形態を示し、各旋回角度ζ₁と最大湾曲角度θ_1maxとの関係をシミュレーションした結果を示す図である。この図６により、旋回角度ζ₁が１８０度において、最大湾曲角度θ_1maxが最小となることがわかる。これは、旋回方向が原点Ｏからみたワイヤ１３０１の方向と逆方向になる旋回角度ζ₁において、近位端１３２５２と基台部１３１０の上面１３１４とが接触する湾曲角度が最も小さくなることを示している。

以上説明したように、第１の実施形態では、切替判定部１２２は、運動学演算部１２１で算出された座標値ｚ_e1（θ_e）に応じて、当該座標値とともに運動学演算部１２１で算出されたワイヤ１３０２及び１３０３の駆動変位量のそれぞれに基づくワイヤ１３０２及び１３０３の駆動を行うか否かを制御（駆動制御）するようにしている。
かかる構成によれば、例えば挿抜経路４００に沿って連続体ロボット１３０の湾曲部１３２０を駆動させる際に（より詳細に、例えば湾曲部１３２０を挿抜経路４００に接触しないように駆動させる際に）、当該湾曲部の湾曲形状の基準面となる部材（基台部１３１０）と当該湾曲部内の最も基準面に近い部材（第２のワイヤガイド１３２５）との接触を防止することができる。

（第２の実施形態）
次に、本発明の第２の実施形態について説明する。なお、以下の第２の実施形態における説明では、第１の実施形態と共通する部分については必要に応じてその説明を省略し、第１の実施形態と異なる部分を中心に説明を行う。

＜連続体ロボットの制御システムの構成例＞
図７は、本発明の第２の実施形態に係る連続体ロボットの制御システム２００の構成例を示す模式図である。連続体ロボットの制御システム２００は、図７に示すように、入力装置２１０と、制御装置２２０と、連続体ロボット２３０とを有して構成されている。

≪連続体ロボットの構成例≫
まず、図７に示す連続体ロボット２３０の構成例について説明する。
図８は、図７に示す連続体ロボット２３０の構成例を示す模式図である。この図８において、図２に示す構成と同様の機能を有する構成については同じ符号を付しており、その詳細な説明は省略する。また、図８においては、図２に示す基台部１３１０の上面１３１４のみを図示し、当該上面１３１４よりも下方に位置する基台部１３１０の内部構成は省略している。連続体ロボット２３０は、図８に示すように、基台部１３１０（図８では、基台部１３１０の上面１３１４のみ図示）と、第１湾曲部１３２０と、第２湾曲部１３３０と、第３湾曲部１３４０を有して構成されている。ここで、図８においては、図２に示す湾曲部１３２０に相当する構成を「第１湾曲部１３２０」としている。

図８に示す連続体ロボット２３０は、長軸方向に直列に複数の湾曲部１３２０〜１３４０を有し、複数の湾曲部１３２０〜１３４０の長軸方向に移動可能に構成されている。なお、図８に示す例では、３つの湾曲部を備える連続体ロボットを示しているが、本実施形態においてはこの態様に限定されるものではない。例えば２つの湾曲部を備える連続体ロボットや４つ以上の湾曲部を備える連続体ロボットも、本実施形態に適用可能である。

第１湾曲部１３２０は、当該第１湾曲部１３２０の基準面に相当する基台部１３１０の上面１３１４を介して延伸する複数のワイヤ１３０１−１〜１３０３−１と、複数のワイヤ１３０１−１〜１３０３−１が異なる位置に固定され、複数のワイヤ１３０１−１〜１３０３−１を案内する第１のワイヤガイド１３２１と、基台部１３１０の上面１３１４と第１のワイヤガイド１３２１との間に複数のワイヤ１３０１−１〜１３０３−１のうちの一部のワイヤ１３０１−１が所定の位置に固定されて設けられ、複数のワイヤ１３０１−１〜１３０３−１を案内する第２のワイヤガイド１３２２〜１３２５と、を含み形成されている。この第１湾曲部１３２０においては、当該第１湾曲部１３２０の基準面に相当する基台部１３１０の上面１３１４を基準として、図２に示すｘ軸に相当する「ｘ₁軸」、図２に示すｙ軸に相当する「ｙ₁軸」、図２に示すｚ軸に相当する「ｚ₁軸」、図２に示すｗ軸に相当する「ｗ₁軸」が定義される。また、この第１湾曲部１３２０において、第１のワイヤガイド１３２１の遠位端１３２１４、及び、第２のワイヤガイド１３２５の近位端１３２５２を示している。この場合、第１湾曲部１３２０は、基台部１３１０の上面１３１４（厳密に言えば、上面１３１４自体は含まない）から第１のワイヤガイド１３２１の遠位端１３２１４に至るまでとなっている。その他、第１湾曲部１３２０に関する構成等については、上述した第１の実施形態における湾曲部１３２０と同様である。

第２湾曲部１３３０は、当該第２湾曲部１３３０の基準面に相当する第１湾曲部１３２０の第１のワイヤガイド１３２１の遠位端１３２１４の面を介して延伸する複数のワイヤ１３０１−２〜１３０３−２と、複数のワイヤ１３０１−２〜１３０３−２が異なる位置に固定され、複数のワイヤ１３０１−２〜１３０３−２を案内する第１のワイヤガイド１３３１と、第１湾曲部１３２０の第１のワイヤガイド１３２１の遠位端１３２１４の面と第１のワイヤガイド１３３１との間に複数のワイヤ１３０１−２〜１３０３−２のうちの一部のワイヤ１３０１−２が所定の位置に固定されて設けられ、複数のワイヤ１３０１−２〜１３０３−２を案内する第２のワイヤガイド１３３２〜１３３５と、を含み形成されている。この第２湾曲部１３３０においては、当該第２湾曲部１３３０の基準面に相当する第１湾曲部１３２０の第１のワイヤガイド１３２１の遠位端１３２１４の面を基準として、図２に示すｘ軸に相当する「ｘ₂軸」、図２に示すｙ軸に相当する「ｙ₂軸」、図２に示すｚ軸に相当する「ｚ₂軸」、図２に示すｗ軸に相当する「ｗ₂軸」が定義される。また、この第２湾曲部１３３０において、第１のワイヤガイド１３３１の遠位端１３３１４、及び、第２のワイヤガイド１３３５の近位端１３３５１を示している。この場合、第２湾曲部１３３０は、第１湾曲部１３２０の第１のワイヤガイド１３２１の遠位端１３２１４の面（厳密に言えば、遠位端１３２１４の面自体は含まない）から第１のワイヤガイド１３３１の遠位端１３３１１に至るまでとなっている。また、本実施形態においては、基台部１３１０の内部には、第１の実施形態における構成に加えて、ワイヤ１３０２−２を押し引き駆動させるためのアクチュエータ（図２のアクチュエータ１３１１に相当するアクチュエータ）と、ワイヤ１３０３−２を押し引き駆動させるためのアクチュエータ（図２のアクチュエータ１３１２に相当するアクチュエータ）と、ワイヤ１３０１−２の一端を固定する固定部（図２の固定部１３１３に相当する固定部）も設けられている。すなわち、図８に示す連続体ロボット２３０では、これらのアクチュエータを制御してワイヤ１３０２−２及び１３０３−２を駆動することにより、第２湾曲部１３３０を所望の湾曲形状（例えば、円弧状）に湾曲可能に構成されている。

第３湾曲部１３４０は、当該第３湾曲部１３４０の基準面に相当する第２湾曲部１３３０の第１のワイヤガイド１３３１の遠位端１３３１１の面を介して延伸する複数のワイヤ１３０１−３〜１３０３−３と、複数のワイヤ１３０１−３〜１３０３−３が異なる位置に固定され、複数のワイヤ１３０１−３〜１３０３−３を案内する第１のワイヤガイド１３４１と、第２湾曲部１３２０の第１のワイヤガイド１３２１の遠位端１３３１１の面と第１のワイヤガイド１３４１との間に複数のワイヤ１３０１−３〜１３０３−３のうちの一部のワイヤ１３０１−３が所定の位置に固定されて設けられ、複数のワイヤ１３０１−３〜１３０３−３を案内する第２のワイヤガイド１３４２〜１３４５と、を含み形成されている。この第３湾曲部１３４０においては、当該第３湾曲部１３３０の基準面に相当する第２湾曲部１３３０の第１のワイヤガイド１３３１の遠位端１３３１１の面を基準として、図２に示すｘ軸に相当する「ｘ₃軸」、図２に示すｙ軸に相当する「ｙ₃軸」、図２に示すｚ軸に相当する「ｚ₃軸」、図２に示すｗ軸に相当する「ｗ₃軸」が定義される。また、この第２湾曲部１３４０において、第１のワイヤガイド１３４１の遠位端１３４１１、及び、第２のワイヤガイド１３４５の近位端１３４５１を示している。この場合、第３湾曲部１３４０は、第２湾曲部１３３０の第１のワイヤガイド１３３１の遠位端１３３１１の面（厳密に言えば、遠位端１３３１１の面自体は含まない）から第１のワイヤガイド１３４１の遠位端１３４１１に至るまでとなっている。また、本実施形態においては、基台部１３１０の内部には、更に、ワイヤ１３０２−３を押し引き駆動させるためのアクチュエータ（図２のアクチュエータ１３１１に相当するアクチュエータ）と、ワイヤ１３０３−３を押し引き駆動させるためのアクチュエータ（図２のアクチュエータ１３１２に相当するアクチュエータ）と、ワイヤ１３０１−３の一端を固定する固定部（図２の固定部１３１３に相当する固定部）も設けられている。すなわち、図８に示す連続体ロボット２３０では、これらのアクチュエータを制御してワイヤ１３０２−３及び１３０３−３を駆動することにより、第３湾曲部１３４０を所望の湾曲形状（例えば、円弧状）に湾曲可能に構成されている。

また、図８に示す連続体ロボット２３０は、第１のワイヤガイド１３４１よりも上面１３１４から遠い方にカメラ（不図示）が設けられている。これにより、湾曲部１３２０〜１３４０を挿抜経路４００に沿って挿入することにより、挿抜経路４００の内部を観察可能となっている。

なお、本実施形態においては、各湾曲部１３２０〜１３４０によって基準面が異なることから、図８では、各湾曲部の湾曲角度θ₁，θ₂，θ₃と、各湾曲部の旋回角度ζ₁，ζ₂，ζ₃に、後述する相対座標系であることを示す符号（〜）を上に付している。

ここで、再び、図７の説明に戻る。なお、図７に示す例では、連続体ロボット２３０にｅ個の湾曲部が設けられている場合を示している。この際、図８に示す連続体ロボット２３０の制御を行う場合には、ｅ＝３となる。

入力装置２１０は、ｅ個の湾曲部における各湾曲部の湾曲形状パラメータである、当該各湾曲部の湾曲角度θの目標値である目標湾曲角度θ_ref、及び、当該各湾曲部の旋回角度ζの目標値である目標旋回角度ζ_refを、制御装置１２０に対して入力する。ここで、入力される目標湾曲角度θ_ref及び目標旋回角度ζ_refは、後述する相対座標系のものであるため、相対座標系であることを示す符号（〜）を上に付している。また、入力装置２１０は、目標湾曲角度θ_ref及び目標旋回角度ζ_refからなる湾曲形状パラメータを時系列で（例えば、所定の離散時間ステップごとに）順次入力することが可能に構成されている。この際、入力装置２１０は、異なる値の目標湾曲角度θ_ref及び目標旋回角度ζ_refからなる湾曲形状パラメータを時系列で順次入力することも可能である。

制御装置２２０は、入力装置２１０から入力された各湾曲部の湾曲形状パラメータに応じて、連続体ロボット２３０の駆動制御を行う。この制御装置２２０は、図７に示すように、運動学演算部２２１と、切替判定部２２２と、第１の切替部２２３と、記憶部２２４と、第２の切替部２２５−１〜２２５−ｅを有して構成されている。具体的に、制御装置２２０は、各湾曲部ごとに、３本のワイヤ１３０１〜１３０３のうちの２本のワイヤ１３０２及び１３０３を駆動することにより立体的に形状の制御が可能な連続体ロボット２３０の運動学を導出する。そして、制御装置２２０は、各湾曲部ごとに、当該湾曲部の基準面と、当該基準面に最も近い第２のワイヤガイドの近位端との接触を回避するための制御を行う。

運動学演算部２２１は、入力装置２１０からの各湾曲部の目標湾曲角度θ_ref及び目標旋回角度ζ_refの入力に応じて、ワイヤ１３０２の駆動変位量ｌ_p1b，ｌ_p2b，…，ｌ_peb、及び、ワイヤ１３０３の駆動変位量ｌ_p1c，ｌ_p2c，…，ｌ_pecを算出する。さらに、運動学演算部２２１は、入力装置２１０からの各湾曲部の目標湾曲角度θ_ref及び目標旋回角度ζ_refの入力に応じて、基準面と当該基準面に最も近い第２のワイヤガイドの近位端との距離を表す距離情報である座標値ｚ_e1（θ_e），…，ｚ_ee（θ_e）を算出する。ここで算出される座標値ｚ_e1（θ_e），…，ｚ_ee（θ_e）は、後述する相対座標系のものであるため、相対座標系であることを示す符号（〜）を上に付している。この駆動変位量ｌ_p1b，ｌ_p2b，…，ｌ_peb、駆動変位量ｌ_p1c，ｌ_p2c，…，ｌ_pec、及び、座標値ｚ_e1（θ_e），…，ｚ_ee（θ_e）を算出する運動学演算部２２１は、本発明の「算出手段」に相当する構成の一例である。

切替判定部２２２は、運動学演算部２２１で算出された座標値ｚ_e1（θ_e），…，ｚ_ee（θ_e）に応じて、各湾曲部ごとに、当該座標値とともに運動学演算部２２１で算出された駆動変位量ｌ_p1b，ｌ_p2b，…，ｌ_peb及び駆動変位量ｌ_p1c，ｌ_p2c，…，ｌ_pecのそれぞれに基づくワイヤ１３０２及び１３０３の駆動を行うか否かを制御（駆動制御）する。具体的に、切替判定部２２２は、各湾曲部ごとに、座標値ｚ_e1（θ_e），…，ｚ_ee（θ_e）が閾値以下であるか否かを判断し、当該判断の結果に基づいて、上述した駆動制御を行う。ここで、閾値として、例えば０を設定すれば、上述した座標値が閾値である０以下の場合には、各湾曲部において、当該湾曲部の基準面と当該基準面に最も近い第２のワイヤガイドの近位端とが接触することが想定される。この基準面と当該基準面に最も近い第２のワイヤガイドの近位端との接触の観点からは、閾値として０を設定することが好適である。しかしながら、本発明においてはこの態様に限定されるものではなく、接触に係る安全率を考慮して、閾値として所定の正の値を設定する態様も、本発明に適用可能である。

各湾曲部ごとに算出された座標値ｚ_e1（θ_e），…，ｚ_ee（θ_e）のそれぞれについて当該座標値が閾値以下でない場合、切替判定部２２２は、当該湾曲部の基準面と当該基準面に最も近い第２のワイヤガイドの近位端とが接触するおそれが無いと判定する。この場合、切替判定部２２２は、各湾曲部ごとに、対応する第１の切替部２２３及び第２の切替部２２５（２２５−１〜２２５−ｅ）を図７に示すように切り替える。具体的に、切替判定部２２２は、各湾曲部ごとに、対応する第２の切替部２２５を図７に示すように切り替えて、当該座標値とともに運動学演算部２２１で算出されたワイヤ１３０２及び１３０３の駆動変位量を連続体ロボット２３０に出力する制御を行う。これにより、連続体ロボット２３０では、当該座標値とともに運動学演算部２２１で算出されたワイヤ１３０２及び１３０３の駆動変位量のそれぞれに基づくワイヤ１３０２及び１３０３の駆動が行われることになる。また、この場合、切替判定部２２２は、各湾曲部ごとに、対応する第１の切替部２２３を図７に示すように切り替えて、当該座標値とともに運動学演算部２２１で算出されたワイヤ１３０２及び１３０３の駆動変位量を記憶部２２４に記憶する制御を行う。このように、記憶部２２４には、上述した座標値が閾値以下でなかった場合（すなわち、接触するおそれが無いと判定された場合）に当該座標値とともに運動学演算部２２１で算出されたワイヤ１３０２及び１３０３の駆動変位量が記憶される。

一方、各湾曲部ごとに算出された座標値ｚ_e1（θ_e），…，ｚ_ee（θ_e）のそれぞれについて当該座標値が閾値以下である場合、切替判定部２２２は、当該湾曲部の基準面と当該基準面に最も近い第２のワイヤガイドの近位端とが接触するおそれが有ると判定する。この場合、切替判定部２２２は、各湾曲部ごとに、対応する第１の切替部２２３及び第２の切替部２２５（２２５−１〜２２５−ｅ）を図７に示すものと逆方向に切り替える。すなわち、この場合、切替判定部２２２は、当該座標値とともに運動学演算部２２１で算出されたワイヤ１３０２及び１３０３の駆動変位量のそれぞれに基づくワイヤ１３０２及び１３０３の駆動を行わないように制御する。具体的に、まず、切替判定部２２２は、各湾曲部ごとに、対応する第１の切替部２２３を図７に示すものと逆方向に切り替えて、当該座標値とともに運動学演算部２２１で算出されたワイヤ１３０２及び１３０３の駆動変位量を記憶部２２４に記憶しない制御を行う。これにより、記憶部２２４には、各湾曲部ごとに、上述した座標値が閾値以下でなかった場合（すなわち、接触するおそれが無いと判定された場合）に当該座標値とともに運動学演算部２２１で算出されたワイヤ１３０２及び１３０３の駆動変位量のみが記憶される。また、切替判定部２２２は、各湾曲部ごとに、対応する第２の切替部２２５を図７に示すものと逆方向に切り替えて、当該座標値とともに運動学演算部２２１で算出されたワイヤ１３０２及び１３０３の駆動変位量を連続体ロボット２３０に出力しない制御を行う。これにより、連続体ロボット２３０では、当該座標値とともに運動学演算部２２１で算出されたワイヤ１３０２及び１３０３の駆動変位量のそれぞれに基づくワイヤ１３０２及び１３０３の駆動が行われず、上述した接触を回避することができる。より詳細に、この場合、切替判定部２２２は、各湾曲部ごとに、対応する第２の切替部２２５を図７に示すものと逆方向に切り替えて、予め記憶部２２４に記憶されている、以前に運動学演算部２２１で当該湾曲部について算出され上述した座標値が閾値以下でなかった場合（すなわち、接触するおそれが無いと判定された場合）に当該座標値とともに算出されたワイヤ１３０２及び１３０３の駆動変位量を連続体ロボット２３０に出力する制御を行う。これにより、連続体ロボット２３０では、各湾曲部ごとに、上述した接触を回避しつつ、当該各湾曲部の駆動が行われることになる。

なお、本実施形態においては、記憶部２２４に、各湾曲部ごとに、運動学演算部２２１で算出された所定の駆動変位量を記憶するようにしているが、本発明においてはこの形態に限定されるものではない。例えば、運動学演算部２２１で算出されたものではなく、各湾曲部ごとに、上述した接触を回避できることが検証されたワイヤ１３０２及び１３０３の駆動変位量を予め記憶しておく形態も、本発明に適用可能である。また、切替判定部２２２は、本発明の「判定手段」に相当する構成の一例である。

図７に示す制御装置２２０の運動学演算部２２１、切替判定部２２２、第１の切替部２２３、記憶部２２４及び第２の切替部２２５−１〜２２５−ｅは、ハードウェアで構成されていても、ソフトウェアで構成されていてもよい。ここで、図７に示す制御装置２２０の各構成部２２１〜２２５がソフトウェアで構成されている場合には、例えばコンピュータのＣＰＵがＲＯＭ等の記憶媒体に記憶されているプログラムを実行することにより、各構成部２２１〜２２５の機能が実現される。

次いで、制御装置２２０の具体的な処理例について説明する。
なお、以下の説明では、図８に示す連続体ロボット２３０にｎ個の湾曲部が設けられている場合について説明する。

この場合、第ｎ湾曲部の相対座標系としては、第ｎ−１湾曲部における第１のワイヤガイドの遠位端の面が基準面となる。第ｎ−１湾曲部における第１のワイヤガイドの遠位端の面の中心軸１３０４の延長線上にｚ_n軸をとり、ｚ_n-1ｗ_n-1平面上のｚ_n軸と直交する方向にｘ_n軸をとる。そして、ｚ_n軸及びｘ_n軸と直交する右手系の座標軸としてｙ_n軸をとる。

本実施形態では、まず、第ｎ湾曲部の湾曲角度とその第１のワイヤガイドの遠位端の座標との関係を導出する。ただし、ｎは２以上とする。第１の実施形態で説明したように、第ｎ湾曲部の中心軸の長さ（ｌ_nd）はその湾曲形状により変動する。複数の湾曲部を考えるとき、形状の変化に対して不変なワイヤ長は、各湾曲部の固定ワイヤであるワイヤ１３０１の、当該湾曲部の第１のワイヤガイドの遠位端から基台部１３１０までの全長である。そこで、第ｎ湾曲部におけるワイヤ１３０１の長さを求めるために、当該第ｎ湾曲部よりも基台部１３１０に近い全ての湾曲部において、第ｎ湾曲部のワイヤ１３０１が通過している長さを求める必要がある。

ここで、第ｍ湾曲部の第１のワイヤガイドの遠位端に接続されているワイヤ１３０１の第ｎ湾曲部のおける長さをｌ_manとする。また、全ての湾曲角度及び旋回角度を０とする直立姿勢を取るときの、各湾曲部の中心軸の長さをｌとする。これより、第ｎ湾曲部の固定ワイヤであるワイヤ１３０１の全長はｎｌとなる。

まず、第１湾曲部における第ｍ湾曲部のワイヤの長さを求める。図３に示す位相角ξ_nを位相角ξ_mと置き換えて、第ｍ湾曲部のワイヤが配置されているとき、第ｍ湾曲部の第１湾曲部に対する相対的な位相角を以下のようにして、（２４）式と定義する。

これにより、第ｍ湾曲部の第１のワイヤガイドの遠位端に接続されているワイヤ１３０１の第１湾曲部のおける長さｌ_ma1、この場合のワイヤ１３０２の長さｌ_mb1及びワイヤ１３０３の長さｌ_mc1は、それぞれ、以下の（２５）式〜（２７）式となる。

続いて、第２湾曲部を考えると、区間内のワイヤ１３０１の長さｌ_2aは、第２湾曲部のワイヤ１３０１の全長（２ｌ）から第１湾曲部の内部を通過する第２湾曲部のワイヤ１３０１の長さ（ｌ_2a1）を差し引けばよいから、以下の（２８）式により求まる。

ここで、第２湾曲部まで中心軸回りの旋回角度の総和から位相角ξ₂を差し引いた角度ν₂を、以下の（２９）式により定義する。

そうすると、第２湾曲部における中心軸の長さｌ_2dと、その場合のワイヤ１３０２の長さｌ_2b及びワイヤ１３０３の長さｌ_2cは、それぞれ、以下の（３０）式〜（３２）式となる。

このように、第２湾曲部における中心軸の長さｌ_2dは、（２８）式に示すワイヤ１３０１の長さｌ_2a1を用いて求めることができる。さらに、第２湾曲部以降の湾曲部の中心軸の長さを求めるために、第２湾曲部を通過する第ｍ湾曲部のワイヤの長さを求める。第ｍ湾曲部の第２湾曲部に対する相対的な位相角を以下のようにして、以下の（３３）式と定義する。

これにより、第ｍ湾曲部の第１のワイヤガイドの遠位端に接続されているワイヤ１３０１の第２湾曲部のおける長さｌ_ma2、この場合のワイヤ１３０２の長さｌ_mb2及びワイヤ１３０３の長さｌ_mc2は、それぞれ、以下の（３４）式〜（３６）式となる。

このように、各湾曲部の中心軸の長さは、基台部１３１０に近い湾曲部から順に求めることができる。一般化し、第ｎ湾曲部を考えると、区間内のワイヤ１３０１の長さｌ_naは、以下の（３７）式で求まる。

ここで、（２９）式に示す角度を一般化した角度ν_nを、以下の（３８）式により定義する。

そうすると、第ｎ湾曲部における中心軸の長さｌ_ndと、その場合のワイヤ１３０２の長さｌ_nb及びワイヤ１３０３の長さｌ_ncは、それぞれ、以下の（３９）式〜（４１）式となる。

さらに、（３３）式を一般化して、以下の（４２）式と定義する。

これにより、第ｎ湾曲部を通過する第ｍ湾曲部のワイヤ１３０１の長さｌ_man、この場合のワイヤ１３０２の長さｌ_mbn及びワイヤ１３０３の長さｌ_mcnは、それぞれ、以下の（４３）式〜（４５）式となる。

以上の手順で求められる第ｎ湾曲部の中心軸の長さｌ_ndを用いると、相対座標系（ｘ_n，ｙ_n，ｚ_n）における第ｎ湾曲部の第１のワイヤガイドの遠位端の座標は、以下の（４６）式〜（４８）式となる。

これより、絶対座標系における第ｎ湾曲部の第１のワイヤガイドの遠位端の座標（ｘ_tn，ｙ_tn，ｚ_tn）は、回転変換行列を用いて、以下の（４９）式となる。

第ｎ湾曲部の第１のワイヤガイドの遠位端から長さｌ_awnにある第２のワイヤガイドの近位端の外周の座標については、以下に示す相対座標系における座標を求める。また、当該第２のワイヤガイドの中心座標を以下に示す座標として記載する。

そして、以下の（５０）式を定義する。

この場合、上述した第２のワイヤガイドの中心座標は、以下の（５１）式〜（５３）式となる。

このことから、上述した第ｎ湾曲部の第１のワイヤガイドの遠位端から長さｌ_awnにある第２のワイヤガイドの近位端の外周の相対座標系における座標は、以下の（５４）式となる。

次いで、第ｎ湾曲部におけるワイヤ１３０２の駆動変位量ｌ_pnb及びワイヤ１３０３の駆動変位量ｌ_pncと、第ｎ湾曲部の湾曲角度θ_n及び旋回角度ζ_nとの関係を導出する。ただし、ｎは２以上とする。相対座標系（ｘ_n，ｙ_n，ｚ_n）における各ワイヤの駆動変位量と第1湾曲部の湾曲角度との関係は、以下の（５５）式〜（５６）式のようになる。

第ｎ湾曲部におけるワイヤ１３０２及び１３０３の駆動変位量ｌ_pnb，ｌ_pncは、第１湾曲部から第ｎ湾曲部までの相対座標系におけるワイヤの駆動変位量の総和となり、以下の（５７）式となる。

ここでは、図７に示す入力装置２１０から入力する湾曲形状パラメータを、以下の（５８）式で表す。

この場合、運動学演算部２２１は、入力装置１１０から、上述の（５８）式に示す湾曲形状パラメータが入力されると、各湾曲部ごとに、以下に示す、基準面と当該基準面に最も近い第２のワイヤガイドの近位端との距離を表す距離情報である座標値を算出する。

そして、切替判定部２２２は、各湾曲部ごとに、上述した座標値が閾値以下であるか否かを判断し、当該判断の結果に基づいて、連続体ロボット２３０の駆動制御を行う。この具体的な連続体ロボット２３０の駆動制御は、図７を用いて上述した通りである。

＜湾曲角度の最大値＞
次いで、運動学演算部２２１による運動学を用いて、図８に示す第３湾曲部１３４０の湾曲角度θ₃の最大値を算出する場合について説明する。
ここでは、直立姿勢時の各湾曲部１３２０〜１３４０の中心軸１３０４の長さｌを０．０１０ｍとする。また、直立姿勢時の各湾曲部１３２０〜１３４０における長さｌ_aw1〜ｌ_aw3は、いずれも０．００９２ｍとする。また、第１実施形態と同様に、第１のワイヤガイドの遠位端における湾曲角度の制約は２７０度と設定し、本実施形態では、基準面に最も近い第２のワイヤガイドの近位端の接触が原因となる湾曲角度θ₃の最大値を算出する。第３湾曲部１３４０の湾曲角度θ₃の最大値は、それよりも基台部１３１０に近い第１湾曲部１３２０及び第２湾曲部１３３０の角度により変動する。そこで、ここでは、第１湾曲部１３２０及び第２湾曲部１３３０の湾曲形状パラメータを、以下のａ）〜ｄ）に示す４通りでシミュレーションを行う。

この際、第１の実施形態と同様に、旋回角度ζ₃を０度から１度刻みで３５９度まで変化させ、各旋回角度ζ₃において湾曲角度θ₃を０度から１度刻みで２７０度まで変化させる。

上述した（２０）式を用いて座標値ｚ_e3（θ_e）を算出し、それが閾値（ここでは０とする）以上となる範囲の最大湾曲角度をθ_3maxとする。第３湾曲部１３４０を湾曲させない状態での、上述したａ）〜ｄ）に示す湾曲形状パラメータに基づく連続体ロボット２３０の湾曲形状を、図９（ａ）〜図９（ｄ）にそれぞれ示す。また、第３湾曲部１３４０を湾曲させない状態での、上述したａ）〜ｄ）に示す湾曲形状パラメータに対応するシミュレーション応答を、図１０にそれぞれ実線、破線、一点鎖線、点線で示す。

図１０において実線及び破線で示す、上述したａ）及びｂ）に示す湾曲形状パラメータでは、第３湾曲部１３４０の最大湾曲角度θ_3max及びそれが最小となる旋回角度ζ₃は、第２湾曲部１３３０が湾曲することにより変動することがわかる。さらに、図１０において一点鎖線で示す、上述したｃ）に示す湾曲形状パラメータでは、第２湾曲部１３３０に加えて第１湾曲部１３２０を駆動しているが、図９（ｃ）からわかるように、第３湾曲部１３４０の中心軸の長さが長くなる姿勢であるため、第３湾曲部１３４０の最大湾曲角度θ_3maxの最小値は大きくなる。一方、図１０において点線で示す、上述したｄ）に示す湾曲形状パラメータでは、第１湾曲部１３２０を逆方向に湾曲駆動しているため、第３湾曲部１３４０の中心軸の長さが短くなり、第３湾曲部１３４０の最大湾曲角度θ_3maxの最小値は小さくなる。このように、最大湾曲角度（θ_3max）は、全ての湾曲部の形状によって非線形に変動するため、一定の値で制限することが難しく、本実施形態で用いる運動学による接触回避制御は有効であることがわかる。なお、ここでのシミュレーションでは効果を示すため、直立姿勢における第ｎ湾曲部の第１のワイヤガイドの遠位端と当該遠位端に最も近い第ｎ＋１湾曲部の第２のワイヤガイドの近位端との隙間を０．０００８ｍとしている。この際、この隙間の値を大きくとれば、湾曲姿勢の制約は軽減される。

以上説明したように、第２の実施形態では、切替判定部２２２は、運動学演算部２２１で算出された座標値ｚ_e1（θ_e），…，ｚ_ee（θ_e）に応じて、各湾曲部ごとに、当該座標値とともに運動学演算部２２１で算出されたワイヤ１３０２及び１３０３の駆動変位量のそれぞれに基づくワイヤ１３０２及び１３０３の駆動を行うか否かを制御（駆動制御）するようにしている。
かかる構成によれば、例えば挿抜経路４００に沿って連続体ロボット２３０の各湾曲部を駆動させる際に（より詳細に、例えば各湾曲部を挿抜経路４００に接触しないように駆動させる際に）、当該湾曲部の湾曲形状の基準面となる部材（基台部１３１０或いは第１のワイヤガイド）と当該湾曲部内の最も基準面に近い部材（第２のワイヤガイド）との接触を防止することができる。

（第３の実施形態）
次に、本発明の第３の実施形態について説明する。なお、以下の第３の実施形態における説明では、第１の実施形態或いは第２の実施形態と共通する部分については必要に応じてその説明を省略し、第１の実施形態或いは第２の実施形態と異なる部分を中心に説明を行う。

＜連続体ロボットの制御システムの構成例＞
図１１は、本発明の第３の実施形態に係る連続体ロボットの制御システム３００の構成例を示す模式図である。連続体ロボットの制御システム３００は、図１１に示すように、入力装置３１０と、制御装置３２０と、連続体ロボット３３０とを有して構成されている。

まず、図１１に示す連続体ロボット３３０の構成例について説明する。
第２の実施形態（第１の実施形態も同様）では、各湾曲部において、３本のワイヤ１３０１〜１３０３のうちの一部のワイヤ１３０２及び１３０３を所定ワイヤとして駆動させ、当該所定ワイヤを除く他のワイヤ１３０１を固定するものであった。第３の実施形態では、図２や図８に示すワイヤ１３０１も、駆動させる形態である。すなわち、第３の実施形態では、各湾曲部において、３本のワイヤ１３０１〜１３０３のうちの全部のワイヤ１３０１〜１３０３を所定ワイヤとして駆動させる形態である。本実施形態の場合、図２や図８に示すワイヤ１３０１における基台部１３１０内部の一端に、図２に示すアクチュエータ１３１１及びアクチュエータ１３１２と同様の機能を有するアクチュエータを接続し、当該アクチュエータも制御する形態を採る。本実施形態では、湾曲部の中心軸１３０４の長さを湾曲形状によらずに一定とするように駆動することができる。しかしながら、各湾曲部の中心軸１３０４の長さをそれぞれ変動させるときには、上述した第１及び第２の実施形態で説明した運動学を用いて接触を回避する駆動制御が有効となる。

ここで、図１１に示す連続体ロボットの制御システム３００では、図８に示す第２の実施形態の連続体ロボット２３０のワイヤ１３０１も駆動させる場合を示しているため、以下、この例について説明する。

入力装置３１０は、ｅ個の湾曲部における各湾曲部の湾曲形状パラメータである、当該各湾曲部の湾曲角度θの目標値である目標湾曲角度θ_ref、及び、当該各湾曲部の旋回角度ζの目標値である目標旋回角度ζ_refを、制御装置１２０に対して入力する。ここで、入力される目標湾曲角度θ_ref及び目標旋回角度ζ_refは、第２の実施形態で説明した相対座標系のものであるため、相対座標系であることを示す符号（〜）を上に付している。また、入力装置３１０は、目標湾曲角度θ_ref及び目標旋回角度ζ_refからなる湾曲形状パラメータを時系列で（例えば、所定の離散時間ステップごとに）順次入力することが可能に構成されている。この際、入力装置３１０は、異なる値の目標湾曲角度θ_ref及び目標旋回角度ζ_refからなる湾曲形状パラメータを時系列で順次入力することも可能である。

制御装置３２０は、入力装置３１０から入力された各湾曲部の湾曲形状パラメータに応じて、連続体ロボット３３０の駆動制御を行う。この制御装置３２０は、図１１に示すように、運動学演算部３２１と、切替判定部３２２と、第３の切替部２２３と、記憶部３２４と、第２の切替部３２５−１〜３２５−ｅを有して構成されている。具体的に、制御装置３２０は、各湾曲部ごとに、３本のワイヤ１３０１〜１３０３を駆動することにより立体的に形状の制御が可能な連続体ロボット３３０の運動学を導出する。そして、制御装置３２０は、各湾曲部ごとに、当該湾曲部の基準面と、当該基準面に最も近い第２のワイヤガイドの近位端との接触を回避するための制御を行う。

運動学演算部３２１は、入力装置３１０からの各湾曲部の目標湾曲角度θ_ref及び目標旋回角度ζ_refの入力に応じて、ワイヤ１３０１の駆動変位量ｌ_p1a，ｌ_p2a，…，ｌ_pea、ワイヤ１３０２の駆動変位量ｌ_p1b，ｌ_p2b，…，ｌ_peb、及び、ワイヤ１３０３の駆動変位量ｌ_p1c，ｌ_p2c，…，ｌ_pecを算出する。さらに、運動学演算部３２１は、入力装置３１０からの各湾曲部の目標湾曲角度θ_ref及び目標旋回角度ζ_refの入力に応じて、基準面と当該基準面に最も近い第２のワイヤガイドの近位端との距離を表す距離情報である座標値ｚ_e1（θ_e），…，ｚ_ee（θ_e）を算出する。ここで算出される座標値ｚ_e1（θ_e），…，ｚ_ee（θ_e）は、第２の実施形態で説明した相対座標系のものであるため、相対座標系であることを示す符号（〜）を上に付している。この駆動変位量ｌ_p1a，ｌ_p2a，…，ｌ_pea、駆動変位量ｌ_p1b，ｌ_p2b，…，ｌ_peb、駆動変位量ｌ_p1c，ｌ_p2c，…，ｌ_pec、及び、座標値ｚ_e1（θ_e），…，ｚ_ee（θ_e）を算出する運動学演算部３２１は、本発明の「算出手段」に相当する構成の一例である。

切替判定部３２２は、運動学演算部３２１で算出された座標値ｚ_e1（θ_e），…，ｚ_ee（θ_e）に応じて、各湾曲部ごとに、当該座標値とともに運動学演算部３２１で算出された駆動変位量ｌ_p1a，ｌ_p2a，…，ｌ_pea、駆動変位量ｌ_p1b，ｌ_p2b，…，ｌ_peb及び駆動変位量ｌ_p1c，ｌ_p2c，…，ｌ_pecのそれぞれに基づくワイヤ１３０１、ワイヤ１３０２及び１３０３の駆動を行うか否かを制御（駆動制御）する。具体的に、切替判定部３２２は、各湾曲部ごとに、座標値ｚ_e1（θ_e），…，ｚ_ee（θ_e）が閾値以下であるか否かを判断し、当該判断の結果に基づいて、上述した駆動制御を行う。ここで、閾値として、例えば０を設定すれば、上述した座標値が閾値である０以下の場合には、各湾曲部において、当該湾曲部の基準面と当該基準面に最も近い第２のワイヤガイドの近位端とが接触することが想定される。この基準面と当該基準面に最も近い第２のワイヤガイドの近位端との接触の観点からは、閾値として０を設定することが好適である。しかしながら、本発明においてはこの態様に限定されるものではなく、接触に係る安全率を考慮して、閾値として所定の正の値を設定する態様も、本発明に適用可能である。

各湾曲部ごとに算出された座標値ｚ_e1（θ_e），…，ｚ_ee（θ_e）のそれぞれについて当該座標値が閾値以下でない場合、切替判定部３２２は、当該湾曲部の基準面と当該基準面に最も近い第２のワイヤガイドの近位端とが接触するおそれが無いと判定する。この場合、切替判定部３２２は、各湾曲部ごとに、対応する第１の切替部３２３及び第２の切替部３２５（３２５−１〜３２５−ｅ）を図１１に示すように切り替える。具体的に、切替判定部３２２は、各湾曲部ごとに、対応する第２の切替部３２５を図１１に示すように切り替えて、当該座標値とともに運動学演算部３２１で算出されたワイヤ１３０１〜１３０３の駆動変位量を連続体ロボット３３０に出力する制御を行う。これにより、連続体ロボット３３０では、当該座標値とともに運動学演算部３２１で算出されたワイヤ１３０１〜１３０３の駆動変位量のそれぞれに基づくワイヤ１３０１〜１３０３の駆動が行われることになる。また、この場合、切替判定部３２２は、各湾曲部ごとに、対応する第１の切替部３２３を図１１に示すように切り替えて、当該座標値とともに運動学演算部３２１で算出されたワイヤ１３０１〜１３０３の駆動変位量を記憶部３２４に記憶する制御を行う。このように、記憶部３２４には、上述した座標値が閾値以下でなかった場合（すなわち、接触するおそれが無いと判定された場合）に当該座標値とともに運動学演算部３２１で算出されたワイヤ１３０１〜１３０３の駆動変位量が記憶される。

一方、各湾曲部ごとに算出された座標値ｚ_e1（θ_e），…，ｚ_ee（θ_e）のそれぞれについて当該座標値が閾値以下である場合、切替判定部３２２は、当該湾曲部の基準面と当該基準面に最も近い第２のワイヤガイドの近位端とが接触するおそれが有ると判定する。この場合、切替判定部３２２は、各湾曲部ごとに、対応する第１の切替部３２３及び第２の切替部３２５（３２５−１〜３２５−ｅ）を図１１に示すものと逆方向に切り替える。すなわち、この場合、切替判定部３２２は、当該座標値とともに運動学演算部３２１で算出されたワイヤ１３０１〜１３０３の駆動変位量のそれぞれに基づくワイヤ１３０１〜１３０３の駆動を行わないように制御する。具体的に、まず、切替判定部３２２は、各湾曲部ごとに、対応する第１の切替部３２３を図１１に示すものと逆方向に切り替えて、当該座標値とともに運動学演算部３２１で算出されたワイヤ１３０１〜１３０３の駆動変位量を記憶部３２４に記憶しない制御を行う。これにより、記憶部３２４には、各湾曲部ごとに、上述した座標値が閾値以下でなかった場合（すなわち、接触するおそれが無いと判定された場合）に当該座標値とともに運動学演算部３２１で算出されたワイヤ１３０１〜１３０３の駆動変位量のみが記憶される。また、切替判定部３２２は、各湾曲部ごとに、対応する第２の切替部３２５を図１１に示すものと逆方向に切り替えて、当該座標値とともに運動学演算部３２１で算出されたワイヤ１３０１〜１３０３の駆動変位量を連続体ロボット３３０に出力しない制御を行う。これにより、連続体ロボット３３０では、当該座標値とともに運動学演算部３２１で算出されたワイヤ１３０１〜１３０３の駆動変位量のそれぞれに基づくワイヤ１３０１〜１３０３の駆動が行われず、上述した接触を回避することができる。より詳細に、この場合、切替判定部３２２は、各湾曲部ごとに、対応する第２の切替部３２５を図１１に示すものと逆方向に切り替えて、予め記憶部３２４に記憶されている、以前に運動学演算部３２１で当該湾曲部について算出され上述した座標値が閾値以下でなかった場合（すなわち、接触するおそれが無いと判定された場合）に当該座標値とともに算出されたワイヤ１３０１〜１３０３の駆動変位量を連続体ロボット３３０に出力する制御を行う。これにより、連続体ロボット３３０では、各湾曲部ごとに、上述した接触を回避しつつ、当該各湾曲部の駆動が行われることになる。

なお、本実施形態においては、記憶部３２４に、各湾曲部ごとに、運動学演算部３２１で算出された所定の駆動変位量を記憶するようにしているが、本発明においてはこの形態に限定されるものではない。例えば、運動学演算部３２１で算出されたものではなく、各湾曲部ごとに、上述した接触を回避できることが検証されたワイヤ１３０１〜１３０３の駆動変位量を予め記憶しておく形態も、本発明に適用可能である。また、切替判定部３２２は、本発明の「判定手段」に相当する構成の一例である。

図１１に示す制御装置３２０の運動学演算部３２１、切替判定部３２２、第１の切替部３２３、記憶部３２４及び第２の切替部３２５−１〜３２５−ｅは、ハードウェアで構成されていても、ソフトウェアで構成されていてもよい。ここで、図１１に示す制御装置３２０の各構成部３２１〜３２５がソフトウェアで構成されている場合には、例えばコンピュータのＣＰＵがＲＯＭ等の記憶媒体に記憶されているプログラムを実行することにより、各構成部３２１〜３２５の機能が実現される。

次いで、制御装置３２０の具体的な処理例について説明する。
なお、以下の説明では、連続体ロボット３３０にｎ個の湾曲部が設けられている場合について説明する。

本実施形態では、まず、第ｎ湾曲部の湾曲角度とその第１のワイヤガイドの遠位端の座標との関係を導出する。相対座標系（ｘ_n，ｙ_n，ｚ_n）における第ｎ湾曲部の第１のワイヤガイドの遠位端の座標は、第ｎ湾曲部の中心軸の長さをｌ_ndとすると、以下の（５９）式〜（６１）式となる。

これにより、絶対座標系における第ｎ湾曲部の第１のワイヤガイドの遠位端の座標（ｘ_tn，ｙ_tn，ｚ_tn）は、回転変換行列を用いて、以下の（６２）式となる。

第ｎ湾曲部の第１のワイヤガイドの遠位端の中心から長さｌ_enにある第２のワイヤガイドの近位端の外周の変位については、以下に示す相対座標系における座標を求める。また、当該第２のワイヤガイドの中心座標を以下に示す座標として記載する。

そして、以下の（６３）式を定義する。

この場合、上述した第２のワイヤガイドの中心座標は、以下の（６４）式〜（６６）式となる。

このことから、上述した第ｎ湾曲部の第１のワイヤガイドの遠位端の中心から長さｌ_enにある第２のワイヤガイドの近位端の外周の相対座標系における座標は、以下の（６７）式となる。

次いで、第ｎ湾曲部のワイヤ１３０１〜１３０３の駆動変位量ｌ_pna，ｌ_pnb，ｌ_pncと、第ｎ湾曲部の湾曲角度θ_n及び旋回角度ζ_nとの関係を導出する。ただし、ｎは２以上とする。相対座標系（ｘ_n，ｙ_n，ｚ_n）における各ワイヤの駆動変位量と第1湾曲部の湾曲角度との関係は、以下の（６８）式〜（７０）式のようになる。

第ｎ湾曲部におけるワイヤ１３０１〜１３０３の駆動変位量ｌ_pna，ｌ_pnb，ｌ_pncは、第１湾曲部から第ｎ−１湾曲部までの相対座標系における第ｎ湾曲部を駆動するためのワイヤの変位の総和となり、以下の（７１）式となる。

なお、図１１に示す第３の実施形態に係る連続体ロボットの制御システム３００では、図８に示す第２の実施形態の連続体ロボット２３０のワイヤ１３０１も駆動させる例を示したが、本発明においてはこの形態に限定されるものではない。例えば、第３の実施形態に係る連続体ロボットの制御システム３００として、図２に示す第１の実施形態の連続体ロボット１３０のワイヤ１３０１も駆動させる形態も、本発明に適用可能である。

以上説明したように、第３の実施形態では、切替判定部３２２は、運動学演算部３２１で算出された座標値ｚ_e1（θ_e），…，ｚ_ee（θ_e）に応じて、各湾曲部ごとに、当該座標値とともに運動学演算部２２１で算出されたワイヤ１３０１〜１３０３の駆動変位量のそれぞれに基づくワイヤ１３０１〜１３０３の駆動を行うか否かを制御（駆動制御）するようにしている。
かかる構成によれば、例えば挿抜経路４００に沿って連続体ロボット３３０の各湾曲部を駆動させる際に（より詳細に、例えば各湾曲部を挿抜経路４００に接触しないように駆動させる際に）、当該湾曲部の湾曲形状の基準面となる部材（基台部１３１０或いは第１のワイヤガイド）と当該湾曲部内の最も基準面に近い部材（第２のワイヤガイド）との接触を防止することができる。

（第４の実施形態）
次に、本発明の第４の実施形態について説明する。

上述の第２及び第３の実施形態においては、複数の湾曲部を有する連続体ロボットに対して湾曲部ごとに相対座標系を導入し、運動学の導出と制御系の設計を行う場合を示した。本実施形態では、絶対座標系による第２のワイヤガイドの接触回避を伴う制御を対象とする。

まず、相対角度から絶対座標系における第ｎ湾曲部の湾曲角度θ_n及び旋回角度ζ_n を求める手順を示す。

第ｎ湾曲部の相対座標系において、各湾曲形状の延長線上に遠位端から長さ１の位置（ｘ_ttn，ｙ_ttn，ｚ_ttn）を取ると、その座標は、以下の（７２）式となる。ただし、以下の座標では、相対座標系であることを示す符号（〜）を上に付している。

そして、絶対座標系における位置（ｘ_ttn，ｙ_ttn，ｚ_ttn）は、回転変換行列を用いると、以下の（７３）式となる。

これを用いて、以下の（７４）式に示す単位ベクトルｅ_znを定義する。

単位ベクトルｅ_znは、第ｎ湾曲部の遠位端の絶対座標系における方向を表し、ｚ軸からの角度が湾曲角度θ_nとなり、単位ベクトルｅ_znのｘｙ平面への射影のｘ軸からの角度が旋回角度ζ_nとなる。絶対角度から相対角度を求めるには、単位ベクトルｅ_znの方向に第ｎ＋１湾曲部に対する相対座標系のｚ_n+1軸をとり、第ｎ湾曲部の相対座標系のｗ_nｚ_n平面上においてｚ_n+1軸と直交する方向にｘ_n+1軸をとる。右手系より第ｎ＋１湾曲部の相対座標系が定義されるので、座標軸と単位ベクトルｅ_zn+1との関係から相対角度が求められる。第１湾曲部は相対角度と絶対角度が等しいので、これを近位端から繰り返すことで、全ての湾曲部の相対角度を求めることができる。

絶対座標系を用いると、第ｎ湾曲部のワイヤ１３０２の駆動変位量ｌ_pnb及びワイヤ１３０３の駆動変位量ｌ_pncは、それぞれ、以下の（７５）式及び（７６）式となる。

これは、第２の実施形態のような回転変換を必要としないため、演算量が少ない。また、絶対座標系で設定される経路に最遠位湾曲部の角度を追従させる制御では、絶対座標系による制御が簡便となる。しかし、第２のワイヤガイドの接触を回避するためには、上記の手順により絶対角度から相対角度を求め、第２の実施形態に示した接触判断を行う必要があり、演算量が増えてしまう。そこで、絶対座標系において簡易的に第２のワイヤガイドの接触を回避する手法を以下に示す。

隣接する２つの湾曲部である第ｎ−１湾曲部と第ｎ湾曲部に対し、接触を回避するために第ｎ湾曲部の湾曲角度θ_nを、その相対座標系における湾曲角度θ^~ _lim に制約することを考える。例えば旋回角度ζ_n-1と旋回角度ζ_nが等しい場合、湾曲角度の差分（θ_n−θ_n-1）に以下の（７７）式に示す制約を設ければよい。

一方、例えば旋回角度ζ_n-1と旋回角度ζ_nとが９０度の差をもつ場合は、以下の（７８）式に示す制約を設ければよい。

そこで、旋回角度の差分（ζ_n−ζ_n-1）を引数とする関数αを導入して以下の（７９）式で表し、湾曲角度θ_nを以下の（８０）式に示すように制約すればよい。

このようにすることで、絶対座標系においても隣接する湾曲部のワイヤガイド同士が接触することを回避することが可能となる。

（その他の実施形態）
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
このプログラム及び当該プログラムを記憶したコンピュータ読み取り可能な記憶媒体は、本発明に含まれる。

なお、上述した本発明の実施形態は、いずれも本発明を実施するにあたっての具体化の例を示したものに過ぎず、これらによって本発明の技術的範囲が限定的に解釈されてはならないものである。すなわち、本発明はその技術思想、又はその主要な特徴から逸脱することなく、様々な形で実施することができる。

１００：連続体ロボットの制御システム、１１０：入力装置、１２０：制御装置、１２１：運動学演算部、１２２：切替判定部、１２３：第１の切替部、１２４：記憶部、１２５：第２の切替部、１３０：連続体ロボット

Claims (13)

1. 基準面を介して延伸する複数のワイヤと、
   前記複数のワイヤが異なる位置に固定され、前記複数のワイヤを案内する第１のワイヤガイドと、
   前記基準面と前記第１のワイヤガイドとの間に設けられ、前記複数のワイヤのうちの一部のワイヤが固定された、前記複数のワイヤを案内する第２のワイヤガイドとを含み形成され、
   前記複数のワイヤのうちの少なくとも一部の所定ワイヤを駆動することにより湾曲可能な湾曲部を、有する連続体ロボットの制御システムであって、
   前記湾曲部の湾曲角度の目標値である目標湾曲角度および当該湾曲部の旋回角度の目標値である目標旋回角度の入力に応じて、前記所定ワイヤの駆動変位量と、前記第２のワイヤガイドにおける前記基準面に近い側の端部である近位端と当該基準面との距離を表す距離情報とを算出する算出手段と、
   前記所定ワイヤの駆動を制御する制御手段と、を有し、
   前記制御手段は、前記距離情報とともに前記算出手段で算出された前記駆動変位量に基づいて前記所定ワイヤの駆動を行うか否かを、前記距離情報に応じて判定する判定手段を有することを特徴とする連続体ロボットの制御システム。
2. 前記制御手段は、前記距離情報が閾値以下であった場合、当該距離情報とともに前記算出手段で算出された前記駆動変位量に基づく前記所定ワイヤの駆動を行わないように制御することを特徴とする請求項１に記載の連続体ロボットの制御システム。
3. 前記制御手段は、前記距離情報が閾値以下であった場合、予め記憶部に記憶されている前記所定ワイヤの駆動変位量に基づく前記所定ワイヤの駆動を行う制御をすることを特徴とする請求項１に記載の連続体ロボットの制御システム。
4. 前記制御手段は、前記距離情報が前記閾値以下であった場合、予め記憶部に記憶されている前記所定ワイヤの駆動変位量に基づく前記所定ワイヤの駆動を行う制御をすることを特徴とする請求項２に記載の連続体ロボットの制御システム。
5. 前記記憶部に記憶されている前記所定ワイヤの駆動変位量は、前記距離情報が前記閾値以下でなかった場合に当該距離情報とともに前記算出手段により算出された前記駆動変位量であることを特徴とする請求項３または４に記載の連続体ロボットの制御システム。
6. 前記湾曲部は、複数の前記第２のワイヤガイドを含んで構成され、
   前記算出手段は、前記距離情報として、前記複数の第２のワイヤガイドのうち前記基準面に最も近い第２のワイヤガイドにおける前記近位端と当該基準面との距離を表す距離情報を算出することを特徴とする請求項１乃至５のいずれか１項に記載の連続体ロボットの制御システム。
7. 前記少なくとも一部の所定ワイヤは複数の所定ワイヤを有し、
   前記算出手段は、当該複数の所定ワイヤを構成するワイヤごとに前記駆動変位量を算出することを特徴とする請求項１乃至６のいずれか１項に記載の連続体ロボットの制御システム。
8. 前記湾曲部の長さは、当該湾曲部の湾曲形状に応じて変化することを特徴とする請求項１乃至７のいずれか１項に記載の連続体ロボットの制御システム。
9. 前記連続体ロボットの長軸方向に直列に複数の前記湾曲部が構成され、
   前記算出手段は、前記複数の湾曲部の湾曲部ごとに、前記所定ワイヤの駆動変位量と前記距離情報とを算出し、
   前記制御手段は、前記複数の湾曲部の湾曲部ごとに、前記制御を行うことを特徴とする請求項１乃至８のいずれか１項に記載の連続体ロボットの制御システム。
10. 前記第２のワイヤガイドは、前記複数のワイヤのうちの前記所定ワイヤを除く他のワイヤが所定の位置に固定されていることを特徴とする請求項１乃至９のいずれか１項に記載の連続体ロボットの制御システム。
11. 前記湾曲部は、前記複数のワイヤのうちの全部のワイヤを前記所定ワイヤとして駆動することにより湾曲可能に構成されていることを特徴とする請求項１乃至９のいずれか１項に記載の連続体ロボットの制御システム。
12. 基準面を介して延伸する複数のワイヤと、前記複数のワイヤが異なる位置に固定され、前記複数のワイヤを案内する第１のワイヤガイドと、前記基準面と前記第１のワイヤガイドとの間に設けられ、前記複数のワイヤのうちの一部のワイヤが固定された、前記複数のワイヤを案内する第２のワイヤガイドとを含み形成され、前記複数のワイヤのうちの少なくとも一部の所定ワイヤを駆動することにより湾曲可能な湾曲部を、有する連続体ロボットの制御方法であって、
    前記湾曲部の湾曲角度の目標値である目標湾曲角度および当該湾曲部の旋回角度の目標値である目標旋回角度の入力に応じて、前記所定ワイヤの駆動変位量と、前記第２のワイヤガイドにおける前記基準面に近い側の端部である近位端と当該基準面との距離を表す距離情報とを算出する算出ステップと、
    前記距離情報とともに前記算出ステップで算出された前記駆動変位量に基づいて前記所定ワイヤの駆動を行うか否かを、前記距離情報に応じて判定する判定ステップと、
    を有することを特徴とする連続体ロボットの制御方法。
13. 請求項１乃至１１のいずれか１項に記載の連続体ロボットの制御システムにおける各手段としてコンピュータを機能させるためのプログラム。